1825 files changed, 179170 insertions, 57659 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 9bfb71ff3a6a..177c7d156933 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -117,4 +117,6 @@ source "drivers/staging/Kconfig"
 source "drivers/platform/Kconfig"
 
 source "drivers/clk/Kconfig"
+
+source "drivers/hwspinlock/Kconfig"
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index b423bb16c3a8..a125e0bba4fd 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -117,3 +117,6 @@ obj-y				+= platform/
 obj-y				+= ieee802154/
 #common clk code
 obj-y				+= clk/
+
+obj-$(CONFIG_HWSPINLOCK)	+= hwspinlock/
+obj-y				+= vbus/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 2aa042a5da6d..3a17ca5fff6f 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -7,7 +7,6 @@ menuconfig ACPI
 	depends on !IA64_HP_SIM
 	depends on IA64 || X86
 	depends on PCI
-	depends on PM
 	select PNP
 	default y
 	help
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index 54784bb42cec..edc25867ad9d 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -416,10 +416,15 @@ struct acpi_gpe_handler_info {
 	u8 originally_enabled;  /* True if GPE was originally enabled */
 };
 
+struct acpi_gpe_notify_object {
+	struct acpi_namespace_node *node;
+	struct acpi_gpe_notify_object *next;
+};
+
 union acpi_gpe_dispatch_info {
 	struct acpi_namespace_node *method_node;	/* Method node for this GPE level */
 	struct acpi_gpe_handler_info *handler;  /* Installed GPE handler */
-	struct acpi_namespace_node *device_node;        /* Parent _PRW device for implicit notify */
+	struct acpi_gpe_notify_object device;   /* List of _PRW devices for implicit notify */
 };
 
 /*
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index 14988a86066f..f4725212eb48 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -457,6 +457,7 @@ static void ACPI_SYSTEM_XFACE acpi_ev_asynch_execute_gpe_method(void *context)
 	acpi_status status;
 	struct acpi_gpe_event_info *local_gpe_event_info;
 	struct acpi_evaluate_info *info;
+	struct acpi_gpe_notify_object *notify_object;
 
 	ACPI_FUNCTION_TRACE(ev_asynch_execute_gpe_method);
 
@@ -508,10 +509,18 @@ static void ACPI_SYSTEM_XFACE acpi_ev_asynch_execute_gpe_method(void *context)
 		 * from this thread -- because handlers may in turn run other
 		 * control methods.
 		 */
-		status =
-		    acpi_ev_queue_notify_request(local_gpe_event_info->dispatch.
-						 device_node,
-						 ACPI_NOTIFY_DEVICE_WAKE);
+		status = acpi_ev_queue_notify_request(
+				local_gpe_event_info->dispatch.device.node,
+				ACPI_NOTIFY_DEVICE_WAKE);
+
+		notify_object = local_gpe_event_info->dispatch.device.next;
+		while (ACPI_SUCCESS(status) && notify_object) {
+			status = acpi_ev_queue_notify_request(
+					notify_object->node,
+					ACPI_NOTIFY_DEVICE_WAKE);
+			notify_object = notify_object->next;
+		}
+
 		break;
 
 	case ACPI_GPE_DISPATCH_METHOD:
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 3b20a3401b64..52aaff3df562 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -198,7 +198,9 @@ acpi_setup_gpe_for_wake(acpi_handle wake_device,
 	acpi_status status = AE_BAD_PARAMETER;
 	struct acpi_gpe_event_info *gpe_event_info;
 	struct acpi_namespace_node *device_node;
+	struct acpi_gpe_notify_object *notify_object;
 	acpi_cpu_flags flags;
+	u8 gpe_dispatch_mask;
 
 	ACPI_FUNCTION_TRACE(acpi_setup_gpe_for_wake);
 
@@ -221,27 +223,49 @@ acpi_setup_gpe_for_wake(acpi_handle wake_device,
 		goto unlock_and_exit;
 	}
 
+	if (wake_device == ACPI_ROOT_OBJECT) {
+		goto out;
+	}
+
 	/*
 	 * If there is no method or handler for this GPE, then the
 	 * wake_device will be notified whenever this GPE fires (aka
 	 * "implicit notify") Note: The GPE is assumed to be
 	 * level-triggered (for windows compatibility).
 	 */
-	if (((gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK) ==
-	      ACPI_GPE_DISPATCH_NONE) && (wake_device != ACPI_ROOT_OBJECT)) {
+	gpe_dispatch_mask = gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK;
+	if (gpe_dispatch_mask != ACPI_GPE_DISPATCH_NONE
+	    && gpe_dispatch_mask != ACPI_GPE_DISPATCH_NOTIFY) {
+		goto out;
+	}
 
-		/* Validate wake_device is of type Device */
+	/* Validate wake_device is of type Device */
 
-		device_node = ACPI_CAST_PTR(struct acpi_namespace_node,
-					    wake_device);
-		if (device_node->type != ACPI_TYPE_DEVICE) {
-			goto unlock_and_exit;
-		}
+	device_node = ACPI_CAST_PTR(struct acpi_namespace_node, wake_device);
+	if (device_node->type != ACPI_TYPE_DEVICE) {
+		goto unlock_and_exit;
+	}
+
+	if (gpe_dispatch_mask == ACPI_GPE_DISPATCH_NONE) {
 		gpe_event_info->flags = (ACPI_GPE_DISPATCH_NOTIFY |
 					 ACPI_GPE_LEVEL_TRIGGERED);
-		gpe_event_info->dispatch.device_node = device_node;
+		gpe_event_info->dispatch.device.node = device_node;
+		gpe_event_info->dispatch.device.next = NULL;
+	} else {
+		/* There are multiple devices to notify implicitly. */
+
+		notify_object = ACPI_ALLOCATE_ZEROED(sizeof(*notify_object));
+		if (!notify_object) {
+			status = AE_NO_MEMORY;
+			goto unlock_and_exit;
+		}
+
+		notify_object->node = device_node;
+		notify_object->next = gpe_event_info->dispatch.device.next;
+		gpe_event_info->dispatch.device.next = notify_object;
 	}
 
+ out:
 	gpe_event_info->flags |= ACPI_GPE_CAN_WAKE;
 	status = AE_OK;
 
diff --git a/drivers/acpi/apei/Kconfig b/drivers/acpi/apei/Kconfig
index fca34ccfd294..a188b868f29a 100644
--- a/drivers/acpi/apei/Kconfig
+++ b/drivers/acpi/apei/Kconfig
@@ -1,5 +1,6 @@
 config ACPI_APEI
 	bool "ACPI Platform Error Interface (APEI)"
+	select PSTORE
 	depends on X86
 	help
 	  APEI allows to report errors (for example from the chipset)
@@ -11,6 +12,8 @@ config ACPI_APEI_GHES
 	tristate "APEI Generic Hardware Error Source"
 	depends on ACPI_APEI && X86
 	select ACPI_HED
+	select LLIST
+	select GENERIC_ALLOCATOR
 	help
 	  Generic Hardware Error Source provides a way to report
 	  platform hardware errors (such as that from chipset). It
@@ -21,6 +24,13 @@ config ACPI_APEI_GHES
 	  by firmware to produce more valuable hardware error
 	  information for Linux.
 
+config ACPI_APEI_PCIEAER
+	bool "APEI PCIe AER logging/recovering support"
+	depends on ACPI_APEI && PCIEAER
+	help
+	  PCIe AER errors may be reported via APEI firmware first mode.
+	  Turn on this option to enable the corresponding support.
+
 config ACPI_APEI_EINJ
 	tristate "APEI Error INJection (EINJ)"
 	depends on ACPI_APEI && DEBUG_FS
diff --git a/drivers/acpi/apei/cper.c b/drivers/acpi/apei/cper.c
index 31464a006d76..5d4189464d63 100644
--- a/drivers/acpi/apei/cper.c
+++ b/drivers/acpi/apei/cper.c
@@ -29,6 +29,7 @@
 #include <linux/time.h>
 #include <linux/cper.h>
 #include <linux/acpi.h>
+#include <linux/aer.h>
 
 /*
  * CPER record ID need to be unique even after reboot, because record
@@ -70,8 +71,8 @@ static const char *cper_severity_str(unsigned int severity)
  * If the output length is longer than 80, multiple line will be
  * printed, with @pfx is printed at the beginning of each line.
  */
-static void cper_print_bits(const char *pfx, unsigned int bits,
-			    const char *strs[], unsigned int strs_size)
+void cper_print_bits(const char *pfx, unsigned int bits,
+		     const char *strs[], unsigned int strs_size)
 {
 	int i, len = 0;
 	const char *str;
@@ -81,6 +82,8 @@ static void cper_print_bits(const char *pfx, unsigned int bits,
 		if (!(bits & (1U << i)))
 			continue;
 		str = strs[i];
+		if (!str)
+			continue;
 		if (len && len + strlen(str) + 2 > 80) {
 			printk("%s\n", buf);
 			len = 0;
@@ -243,7 +246,8 @@ static const char *cper_pcie_port_type_strs[] = {
 	"root complex event collector",
 };
 
-static void cper_print_pcie(const char *pfx, const struct cper_sec_pcie *pcie)
+static void cper_print_pcie(const char *pfx, const struct cper_sec_pcie *pcie,
+			    const struct acpi_hest_generic_data *gdata)
 {
 	if (pcie->validation_bits & CPER_PCIE_VALID_PORT_TYPE)
 		printk("%s""port_type: %d, %s\n", pfx, pcie->port_type,
@@ -276,6 +280,12 @@ static void cper_print_pcie(const char *pfx, const struct cper_sec_pcie *pcie)
 		printk(
 	"%s""bridge: secondary_status: 0x%04x, control: 0x%04x\n",
 	pfx, pcie->bridge.secondary_status, pcie->bridge.control);
+#ifdef CONFIG_ACPI_APEI_PCIEAER
+	if (pcie->validation_bits & CPER_PCIE_VALID_AER_INFO) {
+		struct aer_capability_regs *aer_regs = (void *)pcie->aer_info;
+		cper_print_aer(pfx, gdata->error_severity, aer_regs);
+	}
+#endif
 }
 
 static const char *apei_estatus_section_flag_strs[] = {
@@ -322,7 +332,7 @@ static void apei_estatus_print_section(
 		struct cper_sec_pcie *pcie = (void *)(gdata + 1);
 		printk("%s""section_type: PCIe error\n", pfx);
 		if (gdata->error_data_length >= sizeof(*pcie))
-			cper_print_pcie(pfx, pcie);
+			cper_print_pcie(pfx, pcie, gdata);
 		else
 			goto err_section_too_small;
 	} else
diff --git a/drivers/acpi/apei/erst-dbg.c b/drivers/acpi/apei/erst-dbg.c
index de73caf3cebc..a4cfb64c86a1 100644
--- a/drivers/acpi/apei/erst-dbg.c
+++ b/drivers/acpi/apei/erst-dbg.c
@@ -43,12 +43,27 @@ static DEFINE_MUTEX(erst_dbg_mutex);
 
 static int erst_dbg_open(struct inode *inode, struct file *file)
 {
+	int rc, *pos;
+
 	if (erst_disable)
 		return -ENODEV;
 
+	pos = (int *)&file->private_data;
+
+	rc = erst_get_record_id_begin(pos);
+	if (rc)
+		return rc;
+
 	return nonseekable_open(inode, file);
 }
 
+static int erst_dbg_release(struct inode *inode, struct file *file)
+{
+	erst_get_record_id_end();
+
+	return 0;
+}
+
 static long erst_dbg_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
 {
 	int rc;
@@ -79,18 +94,20 @@ static long erst_dbg_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
 static ssize_t erst_dbg_read(struct file *filp, char __user *ubuf,
 			     size_t usize, loff_t *off)
 {
-	int rc;
+	int rc, *pos;
 	ssize_t len = 0;
 	u64 id;
 
-	if (*off != 0)
+	if (*off)
 		return -EINVAL;
 
 	if (mutex_lock_interruptible(&erst_dbg_mutex) != 0)
 		return -EINTR;
 
+	pos = (int *)&filp->private_data;
+
 retry_next:
-	rc = erst_get_next_record_id(&id);
+	rc = erst_get_record_id_next(pos, &id);
 	if (rc)
 		goto out;
 	/* no more record */
@@ -181,6 +198,7 @@ out:
 static const struct file_operations erst_dbg_ops = {
 	.owner		= THIS_MODULE,
 	.open		= erst_dbg_open,
+	.release	= erst_dbg_release,
 	.read		= erst_dbg_read,
 	.write		= erst_dbg_write,
 	.unlocked_ioctl	= erst_dbg_ioctl,
diff --git a/drivers/acpi/apei/erst.c b/drivers/acpi/apei/erst.c
index cf6db6b7662a..d6cb0ff6988e 100644
--- a/drivers/acpi/apei/erst.c
+++ b/drivers/acpi/apei/erst.c
@@ -34,6 +34,7 @@
 #include <linux/cper.h>
 #include <linux/nmi.h>
 #include <linux/hardirq.h>
+#include <linux/pstore.h>
 #include <acpi/apei.h>
 
 #include "apei-internal.h"
@@ -429,6 +430,22 @@ ssize_t erst_get_record_count(void)
 }
 EXPORT_SYMBOL_GPL(erst_get_record_count);
 
+#define ERST_RECORD_ID_CACHE_SIZE_MIN	16
+#define ERST_RECORD_ID_CACHE_SIZE_MAX	1024
+
+struct erst_record_id_cache {
+	struct mutex lock;
+	u64 *entries;
+	int len;
+	int size;
+	int refcount;
+};
+
+static struct erst_record_id_cache erst_record_id_cache = {
+	.lock = __MUTEX_INITIALIZER(erst_record_id_cache.lock),
+	.refcount = 0,
+};
+
 static int __erst_get_next_record_id(u64 *record_id)
 {
 	struct apei_exec_context ctx;
@@ -443,26 +460,179 @@ static int __erst_get_next_record_id(u64 *record_id)
 	return 0;
 }
 
+int erst_get_record_id_begin(int *pos)
+{
+	int rc;
+
+	if (erst_disable)
+		return -ENODEV;
+
+	rc = mutex_lock_interruptible(&erst_record_id_cache.lock);
+	if (rc)
+		return rc;
+	erst_record_id_cache.refcount++;
+	mutex_unlock(&erst_record_id_cache.lock);
+
+	*pos = 0;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(erst_get_record_id_begin);
+
+/* erst_record_id_cache.lock must be held by caller */
+static int __erst_record_id_cache_add_one(void)
+{
+	u64 id, prev_id, first_id;
+	int i, rc;
+	u64 *entries;
+	unsigned long flags;
+
+	id = prev_id = first_id = APEI_ERST_INVALID_RECORD_ID;
+retry:
+	raw_spin_lock_irqsave(&erst_lock, flags);
+	rc = __erst_get_next_record_id(&id);
+	raw_spin_unlock_irqrestore(&erst_lock, flags);
+	if (rc == -ENOENT)
+		return 0;
+	if (rc)
+		return rc;
+	if (id == APEI_ERST_INVALID_RECORD_ID)
+		return 0;
+	/* can not skip current ID, or loop back to first ID */
+	if (id == prev_id || id == first_id)
+		return 0;
+	if (first_id == APEI_ERST_INVALID_RECORD_ID)
+		first_id = id;
+	prev_id = id;
+
+	entries = erst_record_id_cache.entries;
+	for (i = 0; i < erst_record_id_cache.len; i++) {
+		if (entries[i] == id)
+			break;
+	}
+	/* record id already in cache, try next */
+	if (i < erst_record_id_cache.len)
+		goto retry;
+	if (erst_record_id_cache.len >= erst_record_id_cache.size) {
+		int new_size, alloc_size;
+		u64 *new_entries;
+
+		new_size = erst_record_id_cache.size * 2;
+		new_size = clamp_val(new_size, ERST_RECORD_ID_CACHE_SIZE_MIN,
+				     ERST_RECORD_ID_CACHE_SIZE_MAX);
+		if (new_size <= erst_record_id_cache.size) {
+			if (printk_ratelimit())
+				pr_warning(FW_WARN ERST_PFX
+					   "too many record ID!\n");
+			return 0;
+		}
+		alloc_size = new_size * sizeof(entries[0]);
+		if (alloc_size < PAGE_SIZE)
+			new_entries = kmalloc(alloc_size, GFP_KERNEL);
+		else
+			new_entries = vmalloc(alloc_size);
+		if (!new_entries)
+			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);
+		erst_record_id_cache.entries = entries = new_entries;
+		erst_record_id_cache.size = new_size;
+	}
+	entries[i] = id;
+	erst_record_id_cache.len++;
+
+	return 1;
+}
+
 /*
  * Get the record ID of an existing error record on the persistent
  * storage. If there is no error record on the persistent storage, the
  * returned record_id is APEI_ERST_INVALID_RECORD_ID.
  */
-int erst_get_next_record_id(u64 *record_id)
+int erst_get_record_id_next(int *pos, u64 *record_id)
 {
-	int rc;
-	unsigned long flags;
+	int rc = 0;
+	u64 *entries;
 
 	if (erst_disable)
 		return -ENODEV;
 
-	raw_spin_lock_irqsave(&erst_lock, flags);
-	rc = __erst_get_next_record_id(record_id);
-	raw_spin_unlock_irqrestore(&erst_lock, flags);
+	/* must be enclosed by erst_get_record_id_begin/end */
+	BUG_ON(!erst_record_id_cache.refcount);
+	BUG_ON(*pos < 0 || *pos > erst_record_id_cache.len);
+
+	mutex_lock(&erst_record_id_cache.lock);
+	entries = erst_record_id_cache.entries;
+	for (; *pos < erst_record_id_cache.len; (*pos)++)
+		if (entries[*pos] != APEI_ERST_INVALID_RECORD_ID)
+			break;
+	/* found next record id in cache */
+	if (*pos < erst_record_id_cache.len) {
+		*record_id = entries[*pos];
+		(*pos)++;
+		goto out_unlock;
+	}
+
+	/* Try to add one more record ID to cache */
+	rc = __erst_record_id_cache_add_one();
+	if (rc < 0)
+		goto out_unlock;
+	/* successfully add one new ID */
+	if (rc == 1) {
+		*record_id = erst_record_id_cache.entries[*pos];
+		(*pos)++;
+		rc = 0;
+	} else {
+		*pos = -1;
+		*record_id = APEI_ERST_INVALID_RECORD_ID;
+	}
+out_unlock:
+	mutex_unlock(&erst_record_id_cache.lock);
 
 	return rc;
 }
-EXPORT_SYMBOL_GPL(erst_get_next_record_id);
+EXPORT_SYMBOL_GPL(erst_get_record_id_next);
+
+/* erst_record_id_cache.lock must be held by caller */
+static void __erst_record_id_cache_compact(void)
+{
+	int i, wpos = 0;
+	u64 *entries;
+
+	if (erst_record_id_cache.refcount)
+		return;
+
+	entries = erst_record_id_cache.entries;
+	for (i = 0; i < erst_record_id_cache.len; i++) {
+		if (entries[i] == APEI_ERST_INVALID_RECORD_ID)
+			continue;
+		if (wpos != i)
+			memcpy(&entries[wpos], &entries[i], sizeof(entries[i]));
+		wpos++;
+	}
+	erst_record_id_cache.len = wpos;
+}
+
+void erst_get_record_id_end(void)
+{
+	/*
+	 * erst_disable != 0 should be detected by invoker via the
+	 * return value of erst_get_record_id_begin/next, so this
+	 * function should not be called for erst_disable != 0.
+	 */
+	BUG_ON(erst_disable);
+
+	mutex_lock(&erst_record_id_cache.lock);
+	erst_record_id_cache.refcount--;
+	BUG_ON(erst_record_id_cache.refcount < 0);
+	__erst_record_id_cache_compact();
+	mutex_unlock(&erst_record_id_cache.lock);
+}
+EXPORT_SYMBOL_GPL(erst_get_record_id_end);
 
 static int __erst_write_to_storage(u64 offset)
 {
@@ -703,56 +873,34 @@ ssize_t erst_read(u64 record_id, struct cper_record_header *record,
 }
 EXPORT_SYMBOL_GPL(erst_read);
 
-/*
- * If return value > buflen, the buffer size is not big enough,
- * else if return value = 0, there is no more record to read,
- * else if return value < 0, something goes wrong,
- * else everything is OK, and return value is record length
- */
-ssize_t erst_read_next(struct cper_record_header *record, size_t buflen)
-{
-	int rc;
-	ssize_t len;
-	unsigned long flags;
-	u64 record_id;
-
-	if (erst_disable)
-		return -ENODEV;
-
-	raw_spin_lock_irqsave(&erst_lock, flags);
-	rc = __erst_get_next_record_id(&record_id);
-	if (rc) {
-		raw_spin_unlock_irqrestore(&erst_lock, flags);
-		return rc;
-	}
-	/* no more record */
-	if (record_id == APEI_ERST_INVALID_RECORD_ID) {
-		raw_spin_unlock_irqrestore(&erst_lock, flags);
-		return 0;
-	}
-
-	len = __erst_read(record_id, record, buflen);
-	raw_spin_unlock_irqrestore(&erst_lock, flags);
-
-	return len;
-}
-EXPORT_SYMBOL_GPL(erst_read_next);
-
 int erst_clear(u64 record_id)
 {
-	int rc;
+	int rc, i;
 	unsigned long flags;
+	u64 *entries;
 
 	if (erst_disable)
 		return -ENODEV;
 
+	rc = mutex_lock_interruptible(&erst_record_id_cache.lock);
+	if (rc)
+		return rc;
 	raw_spin_lock_irqsave(&erst_lock, flags);
 	if (erst_erange.attr & ERST_RANGE_NVRAM)
 		rc = __erst_clear_from_nvram(record_id);
 	else
 		rc = __erst_clear_from_storage(record_id);
 	raw_spin_unlock_irqrestore(&erst_lock, flags);
-
+	if (rc)
+		goto out;
+	entries = erst_record_id_cache.entries;
+	for (i = 0; i < erst_record_id_cache.len; i++) {
+		if (entries[i] == record_id)
+			entries[i] = APEI_ERST_INVALID_RECORD_ID;
+	}
+	__erst_record_id_cache_compact();
+out:
+	mutex_unlock(&erst_record_id_cache.lock);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(erst_clear);
@@ -781,6 +929,128 @@ static int erst_check_table(struct acpi_table_erst *erst_tab)
 	return 0;
 }
 
+static size_t erst_reader(u64 *id, enum pstore_type_id *type,
+		       struct timespec *time);
+static u64 erst_writer(enum pstore_type_id type, size_t size);
+
+static struct pstore_info erst_info = {
+	.owner		= THIS_MODULE,
+	.name		= "erst",
+	.read		= erst_reader,
+	.write		= erst_writer,
+	.erase		= erst_clear
+};
+
+#define CPER_CREATOR_PSTORE						\
+	UUID_LE(0x75a574e3, 0x5052, 0x4b29, 0x8a, 0x8e, 0xbe, 0x2c,	\
+		0x64, 0x90, 0xb8, 0x9d)
+#define CPER_SECTION_TYPE_DMESG						\
+	UUID_LE(0xc197e04e, 0xd545, 0x4a70, 0x9c, 0x17, 0xa5, 0x54,	\
+		0x94, 0x19, 0xeb, 0x12)
+#define CPER_SECTION_TYPE_MCE						\
+	UUID_LE(0xfe08ffbe, 0x95e4, 0x4be7, 0xbc, 0x73, 0x40, 0x96,	\
+		0x04, 0x4a, 0x38, 0xfc)
+
+struct cper_pstore_record {
+	struct cper_record_header hdr;
+	struct cper_section_descriptor sec_hdr;
+	char data[];
+} __packed;
+
+static size_t erst_reader(u64 *id, enum pstore_type_id *type,
+		       struct timespec *time)
+{
+	int rc;
+	ssize_t len;
+	unsigned long flags;
+	u64 record_id;
+	struct cper_pstore_record *rcd = (struct cper_pstore_record *)
+					(erst_info.buf - sizeof(*rcd));
+
+	if (erst_disable)
+		return -ENODEV;
+
+	raw_spin_lock_irqsave(&erst_lock, flags);
+skip:
+	rc = __erst_get_next_record_id(&record_id);
+	if (rc) {
+		raw_spin_unlock_irqrestore(&erst_lock, flags);
+		return rc;
+	}
+	/* no more record */
+	if (record_id == APEI_ERST_INVALID_RECORD_ID) {
+		raw_spin_unlock_irqrestore(&erst_lock, flags);
+		return 0;
+	}
+
+	len = __erst_read(record_id, &rcd->hdr, sizeof(*rcd) +
+			  erst_erange.size);
+	if (uuid_le_cmp(rcd->hdr.creator_id, CPER_CREATOR_PSTORE) != 0)
+		goto skip;
+	raw_spin_unlock_irqrestore(&erst_lock, flags);
+
+	*id = record_id;
+	if (uuid_le_cmp(rcd->sec_hdr.section_type,
+			CPER_SECTION_TYPE_DMESG) == 0)
+		*type = PSTORE_TYPE_DMESG;
+	else if (uuid_le_cmp(rcd->sec_hdr.section_type,
+			     CPER_SECTION_TYPE_MCE) == 0)
+		*type = PSTORE_TYPE_MCE;
+	else
+		*type = PSTORE_TYPE_UNKNOWN;
+
+	if (rcd->hdr.validation_bits & CPER_VALID_TIMESTAMP)
+		time->tv_sec = rcd->hdr.timestamp;
+	else
+		time->tv_sec = 0;
+	time->tv_nsec = 0;
+
+	return len - sizeof(*rcd);
+}
+
+static u64 erst_writer(enum pstore_type_id type, size_t size)
+{
+	struct cper_pstore_record *rcd = (struct cper_pstore_record *)
+					(erst_info.buf - sizeof(*rcd));
+
+	memset(rcd, 0, sizeof(*rcd));
+	memcpy(rcd->hdr.signature, CPER_SIG_RECORD, CPER_SIG_SIZE);
+	rcd->hdr.revision = CPER_RECORD_REV;
+	rcd->hdr.signature_end = CPER_SIG_END;
+	rcd->hdr.section_count = 1;
+	rcd->hdr.error_severity = CPER_SEV_FATAL;
+	/* timestamp valid. platform_id, partition_id are invalid */
+	rcd->hdr.validation_bits = CPER_VALID_TIMESTAMP;
+	rcd->hdr.timestamp = get_seconds();
+	rcd->hdr.record_length = sizeof(*rcd) + size;
+	rcd->hdr.creator_id = CPER_CREATOR_PSTORE;
+	rcd->hdr.notification_type = CPER_NOTIFY_MCE;
+	rcd->hdr.record_id = cper_next_record_id();
+	rcd->hdr.flags = CPER_HW_ERROR_FLAGS_PREVERR;
+
+	rcd->sec_hdr.section_offset = sizeof(*rcd);
+	rcd->sec_hdr.section_length = size;
+	rcd->sec_hdr.revision = CPER_SEC_REV;
+	/* fru_id and fru_text is invalid */
+	rcd->sec_hdr.validation_bits = 0;
+	rcd->sec_hdr.flags = CPER_SEC_PRIMARY;
+	switch (type) {
+	case PSTORE_TYPE_DMESG:
+		rcd->sec_hdr.section_type = CPER_SECTION_TYPE_DMESG;
+		break;
+	case PSTORE_TYPE_MCE:
+		rcd->sec_hdr.section_type = CPER_SECTION_TYPE_MCE;
+		break;
+	default:
+		return -EINVAL;
+	}
+	rcd->sec_hdr.section_severity = CPER_SEV_FATAL;
+
+	erst_write(&rcd->hdr);
+
+	return rcd->hdr.record_id;
+}
+
 static int __init erst_init(void)
 {
 	int rc = 0;
@@ -788,6 +1058,7 @@ static int __init erst_init(void)
 	struct apei_exec_context ctx;
 	struct apei_resources erst_resources;
 	struct resource *r;
+	char *buf;
 
 	if (acpi_disabled)
 		goto err;
@@ -854,6 +1125,18 @@ static int __init erst_init(void)
 	if (!erst_erange.vaddr)
 		goto err_release_erange;
 
+	buf = kmalloc(erst_erange.size, GFP_KERNEL);
+	mutex_init(&erst_info.buf_mutex);
+	if (buf) {
+		erst_info.buf = buf + sizeof(struct cper_pstore_record);
+		erst_info.bufsize = erst_erange.size -
+				    sizeof(struct cper_pstore_record);
+		if (pstore_register(&erst_info)) {
+			pr_info(ERST_PFX "Could not register with persistent store\n");
+			kfree(buf);
+		}
+	}
+
 	pr_info(ERST_PFX
 	"Error Record Serialization Table (ERST) support is initialized.\n");
 
diff --git a/drivers/acpi/apei/ghes.c b/drivers/acpi/apei/ghes.c
index d1d484d4a06a..1cc28d04da2a 100644
--- a/drivers/acpi/apei/ghes.c
+++ b/drivers/acpi/apei/ghes.c
@@ -42,6 +42,9 @@
 #include <linux/mutex.h>
 #include <linux/ratelimit.h>
 #include <linux/vmalloc.h>
+#include <linux/irq_work.h>
+#include <linux/llist.h>
+#include <linux/genalloc.h>
 #include <acpi/apei.h>
 #include <acpi/atomicio.h>
 #include <acpi/hed.h>
@@ -53,6 +56,15 @@
 #define GHES_PFX	"GHES: "
 
 #define GHES_ESTATUS_MAX_SIZE		65536
+#define GHES_ESOURCE_PREALLOC_MAX_SIZE	65536
+
+#define GHES_ESTATUS_POOL_MIN_ALLOC_ORDER	3
+
+#define GHES_ESTATUS_NODE_LEN(estatus_len)			\
+	(sizeof(struct ghes_estatus_node) + (estatus_len))
+#define GHES_ESTATUS_FROM_NODE(estatus_node)				\
+	((struct acpi_hest_generic_status *)				\
+	 ((struct ghes_estatus_node *)(estatus_node) + 1))
 
 /*
  * One struct ghes is created for each generic hardware error source.
@@ -77,6 +89,11 @@ struct ghes {
 	};
 };
 
+struct ghes_estatus_node {
+	struct llist_node llnode;
+	struct acpi_hest_generic *generic;
+};
+
 static int ghes_panic_timeout	__read_mostly = 30;
 
 /*
@@ -121,6 +138,19 @@ static struct vm_struct *ghes_ioremap_area;
 static DEFINE_RAW_SPINLOCK(ghes_ioremap_lock_nmi);
 static DEFINE_SPINLOCK(ghes_ioremap_lock_irq);
 
+/*
+ * printk is not safe in NMI context.  So in NMI handler, we allocate
+ * required memory from lock-less memory allocator
+ * (ghes_estatus_pool), save estatus into it, put them into lock-less
+ * list (ghes_estatus_llist), then delay printk into IRQ context via
+ * irq_work (ghes_proc_irq_work).  ghes_estatus_size_request record
+ * required pool size by all NMI error source.
+ */
+static struct gen_pool *ghes_estatus_pool;
+static unsigned long ghes_estatus_pool_size_request;
+static struct llist_head ghes_estatus_llist;
+static struct irq_work ghes_proc_irq_work;
+
 static int ghes_ioremap_init(void)
 {
 	ghes_ioremap_area = __get_vm_area(PAGE_SIZE * GHES_IOREMAP_PAGES,
@@ -180,6 +210,50 @@ static void ghes_iounmap_irq(void __iomem *vaddr_ptr)
 	__flush_tlb_one(vaddr);
 }
 
+static int ghes_estatus_pool_init(void)
+{
+	ghes_estatus_pool = gen_pool_create(GHES_ESTATUS_POOL_MIN_ALLOC_ORDER, -1);
+	if (!ghes_estatus_pool)
+		return -ENOMEM;
+	return 0;
+}
+
+static void ghes_estatus_pool_exit(void)
+{
+	struct gen_pool_chunk *chunk;
+
+	gen_pool_for_each_chunk(chunk, ghes_estatus_pool)
+		free_page(chunk->start_addr);
+	gen_pool_destroy(ghes_estatus_pool);
+}
+
+static int ghes_estatus_pool_expand(unsigned long len)
+{
+	unsigned long i, pages, size, addr;
+	int ret;
+
+	ghes_estatus_pool_size_request += PAGE_ALIGN(len);
+	size = gen_pool_size(ghes_estatus_pool);
+	if (size >= ghes_estatus_pool_size_request)
+		return 0;
+	pages = (ghes_estatus_pool_size_request - size) / PAGE_SIZE;
+	for (i = 0; i < pages; i++) {
+		addr = __get_free_page(GFP_KERNEL);
+		if (!addr)
+			return -ENOMEM;
+		ret = gen_pool_add(ghes_estatus_pool, addr, PAGE_SIZE, -1);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void ghes_estatus_pool_shrink(unsigned long len)
+{
+	ghes_estatus_pool_size_request -= PAGE_ALIGN(len);
+}
+
 static struct ghes *ghes_new(struct acpi_hest_generic *generic)
 {
 	struct ghes *ghes;
@@ -341,13 +415,13 @@ static void ghes_clear_estatus(struct ghes *ghes)
 	ghes->flags &= ~GHES_TO_CLEAR;
 }
 
-static void ghes_do_proc(struct ghes *ghes)
+static void ghes_do_proc(const struct acpi_hest_generic_status *estatus)
 {
 	int sev, processed = 0;
 	struct acpi_hest_generic_data *gdata;
 
-	sev = ghes_severity(ghes->estatus->error_severity);
-	apei_estatus_for_each_section(ghes->estatus, gdata) {
+	sev = ghes_severity(estatus->error_severity);
+	apei_estatus_for_each_section(estatus, gdata) {
 #ifdef CONFIG_X86_MCE
 		if (!uuid_le_cmp(*(uuid_le *)gdata->section_type,
 				 CPER_SEC_PLATFORM_MEM)) {
@@ -360,13 +434,15 @@ static void ghes_do_proc(struct ghes *ghes)
 	}
 }
 
-static void ghes_print_estatus(const char *pfx, struct ghes *ghes)
+static void ghes_print_estatus(const char *pfx,
+			       const struct acpi_hest_generic *generic,
+			       const struct acpi_hest_generic_status *estatus)
 {
 	/* Not more than 2 messages every 5 seconds */
 	static DEFINE_RATELIMIT_STATE(ratelimit, 5*HZ, 2);
 
 	if (pfx == NULL) {
-		if (ghes_severity(ghes->estatus->error_severity) <=
+		if (ghes_severity(estatus->error_severity) <=
 		    GHES_SEV_CORRECTED)
 			pfx = KERN_WARNING HW_ERR;
 		else
@@ -375,8 +451,8 @@ static void ghes_print_estatus(const char *pfx, struct ghes *ghes)
 	if (__ratelimit(&ratelimit)) {
 		printk(
 	"%s""Hardware error from APEI Generic Hardware Error Source: %d\n",
-	pfx, ghes->generic->header.source_id);
-		apei_estatus_print(pfx, ghes->estatus);
+	pfx, generic->header.source_id);
+		apei_estatus_print(pfx, estatus);
 	}
 }
 
@@ -387,8 +463,8 @@ static int ghes_proc(struct ghes *ghes)
 	rc = ghes_read_estatus(ghes, 0);
 	if (rc)
 		goto out;
-	ghes_print_estatus(NULL, ghes);
-	ghes_do_proc(ghes);
+	ghes_print_estatus(NULL, ghes->generic, ghes->estatus);
+	ghes_do_proc(ghes->estatus);
 
 out:
 	ghes_clear_estatus(ghes);
@@ -447,6 +523,40 @@ static int ghes_notify_sci(struct notifier_block *this,
 	return ret;
 }
 
+static void ghes_proc_in_irq(struct irq_work *irq_work)
+{
+	struct llist_node *llnode, *next, *tail = NULL;
+	struct ghes_estatus_node *estatus_node;
+	struct acpi_hest_generic_status *estatus;
+	u32 len, node_len;
+
+	/*
+	 * Because the time order of estatus in list is reversed,
+	 * revert it back to proper order.
+	 */
+	llnode = llist_del_all(&ghes_estatus_llist);
+	while (llnode) {
+		next = llnode->next;
+		llnode->next = tail;
+		tail = llnode;
+		llnode = next;
+	}
+	llnode = tail;
+	while (llnode) {
+		next = llnode->next;
+		estatus_node = llist_entry(llnode, struct ghes_estatus_node,
+					   llnode);
+		estatus = GHES_ESTATUS_FROM_NODE(estatus_node);
+		len = apei_estatus_len(estatus);
+		node_len = GHES_ESTATUS_NODE_LEN(len);
+		ghes_do_proc(estatus);
+		ghes_print_estatus(NULL, estatus_node->generic, estatus);
+		gen_pool_free(ghes_estatus_pool, (unsigned long)estatus_node,
+			      node_len);
+		llnode = next;
+	}
+}
+
 static int ghes_notify_nmi(struct notifier_block *this,
 				  unsigned long cmd, void *data)
 {
@@ -476,7 +586,8 @@ static int ghes_notify_nmi(struct notifier_block *this,
 
 	if (sev_global >= GHES_SEV_PANIC) {
 		oops_begin();
-		ghes_print_estatus(KERN_EMERG HW_ERR, ghes_global);
+		ghes_print_estatus(KERN_EMERG HW_ERR, ghes_global->generic,
+				   ghes_global->estatus);
 		/* reboot to log the error! */
 		if (panic_timeout == 0)
 			panic_timeout = ghes_panic_timeout;
@@ -484,12 +595,31 @@ static int ghes_notify_nmi(struct notifier_block *this,
 	}
 
 	list_for_each_entry_rcu(ghes, &ghes_nmi, list) {
+#ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
+		u32 len, node_len;
+		struct ghes_estatus_node *estatus_node;
+		struct acpi_hest_generic_status *estatus;
+#endif
 		if (!(ghes->flags & GHES_TO_CLEAR))
 			continue;
-		/* Do not print estatus because printk is not NMI safe */
-		ghes_do_proc(ghes);
+#ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
+		/* Save estatus for further processing in IRQ context */
+		len = apei_estatus_len(ghes->estatus);
+		node_len = GHES_ESTATUS_NODE_LEN(len);
+		estatus_node = (void *)gen_pool_alloc(ghes_estatus_pool,
+						      node_len);
+		if (estatus_node) {
+			estatus_node->generic = ghes->generic;
+			estatus = GHES_ESTATUS_FROM_NODE(estatus_node);
+			memcpy(estatus, ghes->estatus, len);
+			llist_add(&estatus_node->llnode, &ghes_estatus_llist);
+		}
+#endif
 		ghes_clear_estatus(ghes);
 	}
+#ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
+	irq_work_queue(&ghes_proc_irq_work);
+#endif
 
 out:
 	raw_spin_unlock(&ghes_nmi_lock);
@@ -504,10 +634,26 @@ static struct notifier_block ghes_notifier_nmi = {
 	.notifier_call = ghes_notify_nmi,
 };
 
+static unsigned long ghes_esource_prealloc_size(
+	const struct acpi_hest_generic *generic)
+{
+	unsigned long block_length, prealloc_records, prealloc_size;
+
+	block_length = min_t(unsigned long, generic->error_block_length,
+			     GHES_ESTATUS_MAX_SIZE);
+	prealloc_records = max_t(unsigned long,
+				 generic->records_to_preallocate, 1);
+	prealloc_size = min_t(unsigned long, block_length * prealloc_records,
+			      GHES_ESOURCE_PREALLOC_MAX_SIZE);
+
+	return prealloc_size;
+}
+
 static int __devinit ghes_probe(struct platform_device *ghes_dev)
 {
 	struct acpi_hest_generic *generic;
 	struct ghes *ghes = NULL;
+	unsigned long len;
 	int rc = -EINVAL;
 
 	generic = *(struct acpi_hest_generic **)ghes_dev->dev.platform_data;
@@ -573,6 +719,8 @@ static int __devinit ghes_probe(struct platform_device *ghes_dev)
 		mutex_unlock(&ghes_list_mutex);
 		break;
 	case ACPI_HEST_NOTIFY_NMI:
+		len = ghes_esource_prealloc_size(generic);
+		ghes_estatus_pool_expand(len);
 		mutex_lock(&ghes_list_mutex);
 		if (list_empty(&ghes_nmi))
 			register_die_notifier(&ghes_notifier_nmi);
@@ -597,6 +745,7 @@ static int __devexit ghes_remove(struct platform_device *ghes_dev)
 {
 	struct ghes *ghes;
 	struct acpi_hest_generic *generic;
+	unsigned long len;
 
 	ghes = platform_get_drvdata(ghes_dev);
 	generic = ghes->generic;
@@ -627,6 +776,8 @@ static int __devexit ghes_remove(struct platform_device *ghes_dev)
 		 * freed after NMI handler finishes.
 		 */
 		synchronize_rcu();
+		len = ghes_esource_prealloc_size(generic);
+		ghes_estatus_pool_shrink(len);
 		break;
 	default:
 		BUG();
@@ -662,15 +813,23 @@ static int __init ghes_init(void)
 		return -EINVAL;
 	}
 
+	init_irq_work(&ghes_proc_irq_work, ghes_proc_in_irq);
+
 	rc = ghes_ioremap_init();
 	if (rc)
 		goto err;
 
-	rc = platform_driver_register(&ghes_platform_driver);
+	rc = ghes_estatus_pool_init();
 	if (rc)
 		goto err_ioremap_exit;
 
+	rc = platform_driver_register(&ghes_platform_driver);
+	if (rc)
+		goto err_pool_exit;
+
 	return 0;
+err_pool_exit:
+	ghes_estatus_pool_exit();
 err_ioremap_exit:
 	ghes_ioremap_exit();
 err:
@@ -680,6 +839,7 @@ err:
 static void __exit ghes_exit(void)
 {
 	platform_driver_unregister(&ghes_platform_driver);
+	ghes_estatus_pool_exit();
 	ghes_ioremap_exit();
 }
 
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 7ced61f39492..9749980ca6ca 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -40,6 +40,7 @@
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
 #include <linux/dmi.h>
+#include <linux/suspend.h>
 
 #include "internal.h"
 
@@ -1006,8 +1007,7 @@ struct kobject *acpi_kobj;
 
 static int __init acpi_init(void)
 {
-	int result = 0;
-
+	int result;
 
 	if (acpi_disabled) {
 		printk(KERN_INFO PREFIX "Interpreter disabled.\n");
@@ -1022,29 +1022,18 @@ static int __init acpi_init(void)
 
 	init_acpi_device_notify();
 	result = acpi_bus_init();
-
-	if (!result) {
-		pci_mmcfg_late_init();
-		if (!(pm_flags & PM_APM))
-			pm_flags |= PM_ACPI;
-		else {
-			printk(KERN_INFO PREFIX
-			       "APM is already active, exiting\n");
-			disable_acpi();
-			result = -ENODEV;
-		}
-	} else
+	if (result) {
 		disable_acpi();
-
-	if (acpi_disabled)
 		return result;
+	}
 
+	pci_mmcfg_late_init();
 	acpi_scan_init();
 	acpi_ec_init();
 	acpi_debugfs_init();
 	acpi_sleep_proc_init();
 	acpi_wakeup_device_init();
-	return result;
+	return 0;
 }
 
 subsys_initcall(acpi_init);
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index 76bbb78a5ad9..12c28f4adb67 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -98,6 +98,7 @@ struct acpi_button {
 	struct input_dev *input;
 	char phys[32];			/* for input device */
 	unsigned long pushed;
+	bool wakeup_enabled;
 };
 
 static const struct file_operations acpi_button_info_fops = {
@@ -430,8 +431,10 @@ static int acpi_button_add(struct acpi_device *device)
 		/* Button's GPE is run-wake GPE */
 		acpi_enable_gpe(device->wakeup.gpe_device,
 				device->wakeup.gpe_number);
-		device->wakeup.run_wake_count++;
-		device_set_wakeup_enable(&device->dev, true);
+		if (!device_may_wakeup(&device->dev)) {
+			device_set_wakeup_enable(&device->dev, true);
+			button->wakeup_enabled = true;
+		}
 	}
 
 	printk(KERN_INFO PREFIX "%s [%s]\n", name, acpi_device_bid(device));
@@ -453,8 +456,8 @@ static int acpi_button_remove(struct acpi_device *device, int type)
 	if (device->wakeup.flags.valid) {
 		acpi_disable_gpe(device->wakeup.gpe_device,
 				device->wakeup.gpe_number);
-		device->wakeup.run_wake_count--;
-		device_set_wakeup_enable(&device->dev, false);
+		if (button->wakeup_enabled)
+			device_set_wakeup_enable(&device->dev, false);
 	}
 
 	acpi_button_remove_fs(device);
diff --git a/drivers/acpi/debugfs.c b/drivers/acpi/debugfs.c
index 5df67f1d6c61..384f7abcff77 100644
--- a/drivers/acpi/debugfs.c
+++ b/drivers/acpi/debugfs.c
@@ -26,7 +26,9 @@ static ssize_t cm_write(struct file *file, const char __user * user_buf,
 			size_t count, loff_t *ppos)
 {
 	static char *buf;
-	static int uncopied_bytes;
+	static u32 max_size;
+	static u32 uncopied_bytes;
+
 	struct acpi_table_header table;
 	acpi_status status;
 
@@ -37,19 +39,24 @@ static ssize_t cm_write(struct file *file, const char __user * user_buf,
 		if (copy_from_user(&table, user_buf,
 				   sizeof(struct acpi_table_header)))
 			return -EFAULT;
-		uncopied_bytes = table.length;
-		buf = kzalloc(uncopied_bytes, GFP_KERNEL);
+		uncopied_bytes = max_size = table.length;
+		buf = kzalloc(max_size, GFP_KERNEL);
 		if (!buf)
 			return -ENOMEM;
 	}
 
-	if (uncopied_bytes < count) {
-		kfree(buf);
+	if (buf == NULL)
+		return -EINVAL;
+
+	if ((*ppos > max_size) ||
+	    (*ppos + count > max_size) ||
+	    (*ppos + count < count) ||
+	    (count > uncopied_bytes))
 		return -EINVAL;
-	}
 
 	if (copy_from_user(buf + (*ppos), user_buf, count)) {
 		kfree(buf);
+		buf = NULL;
 		return -EFAULT;
 	}
 
@@ -59,6 +66,7 @@ static ssize_t cm_write(struct file *file, const char __user * user_buf,
 	if (!uncopied_bytes) {
 		status = acpi_install_method(buf);
 		kfree(buf);
+		buf = NULL;
 		if (ACPI_FAILURE(status))
 			return -EINVAL;
 		add_taint(TAINT_OVERRIDDEN_ACPI_TABLE);
diff --git a/drivers/acpi/numa.c b/drivers/acpi/numa.c
index 5eb25eb3ea48..3b5c3189fd99 100644
--- a/drivers/acpi/numa.c
+++ b/drivers/acpi/numa.c
@@ -274,7 +274,7 @@ acpi_table_parse_srat(enum acpi_srat_type id,
 
 int __init acpi_numa_init(void)
 {
-	int ret = 0;
+	int cnt = 0;
 
 	/*
 	 * Should not limit number with cpu num that is from NR_CPUS or nr_cpus=
@@ -288,7 +288,7 @@ int __init acpi_numa_init(void)
 				     acpi_parse_x2apic_affinity, 0);
 		acpi_table_parse_srat(ACPI_SRAT_TYPE_CPU_AFFINITY,
 				     acpi_parse_processor_affinity, 0);
-		ret = acpi_table_parse_srat(ACPI_SRAT_TYPE_MEMORY_AFFINITY,
+		cnt = acpi_table_parse_srat(ACPI_SRAT_TYPE_MEMORY_AFFINITY,
 					    acpi_parse_memory_affinity,
 					    NR_NODE_MEMBLKS);
 	}
@@ -297,7 +297,10 @@ int __init acpi_numa_init(void)
 	acpi_table_parse(ACPI_SIG_SLIT, acpi_parse_slit);
 
 	acpi_numa_arch_fixup();
-	return ret;
+
+	if (cnt <= 0)
+		return cnt ?: -ENOENT;
+	return 0;
 }
 
 int acpi_get_pxm(acpi_handle h)
diff --git a/drivers/acpi/nvs.c b/drivers/acpi/nvs.c
index fa5a1df42b79..096787b43c96 100644
--- a/drivers/acpi/nvs.c
+++ b/drivers/acpi/nvs.c
@@ -26,6 +26,7 @@ struct nvs_page {
 	unsigned int size;
 	void *kaddr;
 	void *data;
+	bool unmap;
 	struct list_head node;
 };
 
@@ -44,6 +45,9 @@ int suspend_nvs_register(unsigned long start, unsigned long size)
 {
 	struct nvs_page *entry, *next;
 
+	pr_info("PM: Registering ACPI NVS region at %lx (%ld bytes)\n",
+		start, size);
+
 	while (size > 0) {
 		unsigned int nr_bytes;
 
@@ -81,7 +85,13 @@ void suspend_nvs_free(void)
 			free_page((unsigned long)entry->data);
 			entry->data = NULL;
 			if (entry->kaddr) {
-				iounmap(entry->kaddr);
+				if (entry->unmap) {
+					iounmap(entry->kaddr);
+					entry->unmap = false;
+				} else {
+					acpi_os_unmap_memory(entry->kaddr,
+							     entry->size);
+				}
 				entry->kaddr = NULL;
 			}
 		}
@@ -115,8 +125,14 @@ int suspend_nvs_save(void)
 
 	list_for_each_entry(entry, &nvs_list, node)
 		if (entry->data) {
-			entry->kaddr = acpi_os_ioremap(entry->phys_start,
-						    entry->size);
+			unsigned long phys = entry->phys_start;
+			unsigned int size = entry->size;
+
+			entry->kaddr = acpi_os_get_iomem(phys, size);
+			if (!entry->kaddr) {
+				entry->kaddr = acpi_os_ioremap(phys, size);
+				entry->unmap = !!entry->kaddr;
+			}
 			if (!entry->kaddr) {
 				suspend_nvs_free();
 				return -ENOMEM;
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index c90c76aa7f8b..45ad4ffef533 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -76,7 +76,6 @@ EXPORT_SYMBOL(acpi_in_debugger);
 extern char line_buf[80];
 #endif				/*ENABLE_DEBUGGER */
 
-static unsigned int acpi_irq_irq;
 static acpi_osd_handler acpi_irq_handler;
 static void *acpi_irq_context;
 static struct workqueue_struct *kacpid_wq;
@@ -105,11 +104,11 @@ struct acpi_ioremap {
 	void __iomem *virt;
 	acpi_physical_address phys;
 	acpi_size size;
-	struct kref ref;
+	unsigned long refcount;
 };
 
 static LIST_HEAD(acpi_ioremaps);
-static DEFINE_SPINLOCK(acpi_ioremap_lock);
+static DEFINE_MUTEX(acpi_ioremap_lock);
 
 static void __init acpi_osi_setup_late(void);
 
@@ -285,6 +284,22 @@ acpi_map_vaddr_lookup(acpi_physical_address phys, unsigned int size)
 	return NULL;
 }
 
+void __iomem *acpi_os_get_iomem(acpi_physical_address phys, unsigned int size)
+{
+	struct acpi_ioremap *map;
+	void __iomem *virt = NULL;
+
+	mutex_lock(&acpi_ioremap_lock);
+	map = acpi_map_lookup(phys, size);
+	if (map) {
+		virt = map->virt + (phys - map->phys);
+		map->refcount++;
+	}
+	mutex_unlock(&acpi_ioremap_lock);
+	return virt;
+}
+EXPORT_SYMBOL_GPL(acpi_os_get_iomem);
+
 /* Must be called with 'acpi_ioremap_lock' or RCU read lock held. */
 static struct acpi_ioremap *
 acpi_map_lookup_virt(void __iomem *virt, acpi_size size)
@@ -302,8 +317,7 @@ acpi_map_lookup_virt(void __iomem *virt, acpi_size size)
 void __iomem *__init_refok
 acpi_os_map_memory(acpi_physical_address phys, acpi_size size)
 {
-	struct acpi_ioremap *map, *tmp_map;
-	unsigned long flags;
+	struct acpi_ioremap *map;
 	void __iomem *virt;
 	acpi_physical_address pg_off;
 	acpi_size pg_sz;
@@ -316,14 +330,25 @@ acpi_os_map_memory(acpi_physical_address phys, acpi_size size)
 	if (!acpi_gbl_permanent_mmap)
 		return __acpi_map_table((unsigned long)phys, size);
 
+	mutex_lock(&acpi_ioremap_lock);
+	/* Check if there's a suitable mapping already. */
+	map = acpi_map_lookup(phys, size);
+	if (map) {
+		map->refcount++;
+		goto out;
+	}
+
 	map = kzalloc(sizeof(*map), GFP_KERNEL);
-	if (!map)
+	if (!map) {
+		mutex_unlock(&acpi_ioremap_lock);
 		return NULL;
+	}
 
 	pg_off = round_down(phys, PAGE_SIZE);
 	pg_sz = round_up(phys + size, PAGE_SIZE) - pg_off;
 	virt = acpi_os_ioremap(pg_off, pg_sz);
 	if (!virt) {
+		mutex_unlock(&acpi_ioremap_lock);
 		kfree(map);
 		return NULL;
 	}
@@ -332,62 +357,51 @@ acpi_os_map_memory(acpi_physical_address phys, acpi_size size)
 	map->virt = virt;
 	map->phys = pg_off;
 	map->size = pg_sz;
-	kref_init(&map->ref);
-
-	spin_lock_irqsave(&acpi_ioremap_lock, flags);
-	/* Check if page has already been mapped. */
-	tmp_map = acpi_map_lookup(phys, size);
-	if (tmp_map) {
-		kref_get(&tmp_map->ref);
-		spin_unlock_irqrestore(&acpi_ioremap_lock, flags);
-		iounmap(map->virt);
-		kfree(map);
-		return tmp_map->virt + (phys - tmp_map->phys);
-	}
+	map->refcount = 1;
+
 	list_add_tail_rcu(&map->list, &acpi_ioremaps);
-	spin_unlock_irqrestore(&acpi_ioremap_lock, flags);
 
+ out:
+	mutex_unlock(&acpi_ioremap_lock);
 	return map->virt + (phys - map->phys);
 }
 EXPORT_SYMBOL_GPL(acpi_os_map_memory);
 
-static void acpi_kref_del_iomap(struct kref *ref)
+static void acpi_os_drop_map_ref(struct acpi_ioremap *map)
 {
-	struct acpi_ioremap *map;
+	if (!--map->refcount)
+		list_del_rcu(&map->list);
+}
 
-	map = container_of(ref, struct acpi_ioremap, ref);
-	list_del_rcu(&map->list);
+static void acpi_os_map_cleanup(struct acpi_ioremap *map)
+{
+	if (!map->refcount) {
+		synchronize_rcu();
+		iounmap(map->virt);
+		kfree(map);
+	}
 }
 
 void __ref acpi_os_unmap_memory(void __iomem *virt, acpi_size size)
 {
 	struct acpi_ioremap *map;
-	unsigned long flags;
-	int del;
 
 	if (!acpi_gbl_permanent_mmap) {
 		__acpi_unmap_table(virt, size);
 		return;
 	}
 
-	spin_lock_irqsave(&acpi_ioremap_lock, flags);
+	mutex_lock(&acpi_ioremap_lock);
 	map = acpi_map_lookup_virt(virt, size);
 	if (!map) {
-		spin_unlock_irqrestore(&acpi_ioremap_lock, flags);
-		printk(KERN_ERR PREFIX "%s: bad address %p\n", __func__, virt);
-		dump_stack();
+		mutex_unlock(&acpi_ioremap_lock);
+		WARN(true, PREFIX "%s: bad address %p\n", __func__, virt);
 		return;
 	}
+	acpi_os_drop_map_ref(map);
+	mutex_unlock(&acpi_ioremap_lock);
 
-	del = kref_put(&map->ref, acpi_kref_del_iomap);
-	spin_unlock_irqrestore(&acpi_ioremap_lock, flags);
-
-	if (!del)
-		return;
-
-	synchronize_rcu();
-	iounmap(map->virt);
-	kfree(map);
+	acpi_os_map_cleanup(map);
 }
 EXPORT_SYMBOL_GPL(acpi_os_unmap_memory);
 
@@ -397,7 +411,7 @@ void __init early_acpi_os_unmap_memory(void __iomem *virt, acpi_size size)
 		__acpi_unmap_table(virt, size);
 }
 
-int acpi_os_map_generic_address(struct acpi_generic_address *addr)
+static int acpi_os_map_generic_address(struct acpi_generic_address *addr)
 {
 	void __iomem *virt;
 
@@ -413,13 +427,10 @@ int acpi_os_map_generic_address(struct acpi_generic_address *addr)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(acpi_os_map_generic_address);
 
-void acpi_os_unmap_generic_address(struct acpi_generic_address *addr)
+static void acpi_os_unmap_generic_address(struct acpi_generic_address *addr)
 {
-	void __iomem *virt;
-	unsigned long flags;
-	acpi_size size = addr->bit_width / 8;
+	struct acpi_ioremap *map;
 
 	if (addr->space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY)
 		return;
@@ -427,13 +438,17 @@ void acpi_os_unmap_generic_address(struct acpi_generic_address *addr)
 	if (!addr->address || !addr->bit_width)
 		return;
 
-	spin_lock_irqsave(&acpi_ioremap_lock, flags);
-	virt = acpi_map_vaddr_lookup(addr->address, size);
-	spin_unlock_irqrestore(&acpi_ioremap_lock, flags);
+	mutex_lock(&acpi_ioremap_lock);
+	map = acpi_map_lookup(addr->address, addr->bit_width / 8);
+	if (!map) {
+		mutex_unlock(&acpi_ioremap_lock);
+		return;
+	}
+	acpi_os_drop_map_ref(map);
+	mutex_unlock(&acpi_ioremap_lock);
 
-	acpi_os_unmap_memory(virt, size);
+	acpi_os_map_cleanup(map);
 }
-EXPORT_SYMBOL_GPL(acpi_os_unmap_generic_address);
 
 #ifdef ACPI_FUTURE_USAGE
 acpi_status
@@ -516,11 +531,15 @@ acpi_os_install_interrupt_handler(u32 gsi, acpi_osd_handler handler,
 	acpi_irq_stats_init();
 
 	/*
-	 * Ignore the GSI from the core, and use the value in our copy of the
-	 * FADT. It may not be the same if an interrupt source override exists
-	 * for the SCI.
+	 * ACPI interrupts different from the SCI in our copy of the FADT are
+	 * not supported.
 	 */
-	gsi = acpi_gbl_FADT.sci_interrupt;
+	if (gsi != acpi_gbl_FADT.sci_interrupt)
+		return AE_BAD_PARAMETER;
+
+	if (acpi_irq_handler)
+		return AE_ALREADY_ACQUIRED;
+
 	if (acpi_gsi_to_irq(gsi, &irq) < 0) {
 		printk(KERN_ERR PREFIX "SCI (ACPI GSI %d) not registered\n",
 		       gsi);
@@ -531,20 +550,20 @@ acpi_os_install_interrupt_handler(u32 gsi, acpi_osd_handler handler,
 	acpi_irq_context = context;
 	if (request_irq(irq, acpi_irq, IRQF_SHARED, "acpi", acpi_irq)) {
 		printk(KERN_ERR PREFIX "SCI (IRQ%d) allocation failed\n", irq);
+		acpi_irq_handler = NULL;
 		return AE_NOT_ACQUIRED;
 	}
-	acpi_irq_irq = irq;
 
 	return AE_OK;
 }
 
 acpi_status acpi_os_remove_interrupt_handler(u32 irq, acpi_osd_handler handler)
 {
-	if (irq) {
-		free_irq(irq, acpi_irq);
-		acpi_irq_handler = NULL;
-		acpi_irq_irq = 0;
-	}
+	if (irq != acpi_gbl_FADT.sci_interrupt)
+		return AE_BAD_PARAMETER;
+
+	free_irq(irq, acpi_irq);
+	acpi_irq_handler = NULL;
 
 	return AE_OK;
 }
@@ -1589,9 +1608,9 @@ acpi_status __init acpi_os_initialize(void)
 
 acpi_status __init acpi_os_initialize1(void)
 {
-	kacpid_wq = create_workqueue("kacpid");
-	kacpi_notify_wq = create_workqueue("kacpi_notify");
-	kacpi_hotplug_wq = create_workqueue("kacpi_hotplug");
+	kacpid_wq = alloc_workqueue("kacpid", 0, 1);
+	kacpi_notify_wq = alloc_workqueue("kacpi_notify", 0, 1);
+	kacpi_hotplug_wq = alloc_workqueue("kacpi_hotplug", 0, 1);
 	BUG_ON(!kacpid_wq);
 	BUG_ON(!kacpi_notify_wq);
 	BUG_ON(!kacpi_hotplug_wq);
@@ -1603,7 +1622,7 @@ acpi_status __init acpi_os_initialize1(void)
 acpi_status acpi_os_terminate(void)
 {
 	if (acpi_irq_handler) {
-		acpi_os_remove_interrupt_handler(acpi_irq_irq,
+		acpi_os_remove_interrupt_handler(acpi_gbl_FADT.sci_interrupt,
 						 acpi_irq_handler);
 	}
 
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index b99e62494607..b136c9c1e531 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -797,7 +797,6 @@ static void acpi_bus_set_run_wake_flags(struct acpi_device *device)
 	acpi_status status;
 	acpi_event_status event_status;
 
-	device->wakeup.run_wake_count = 0;
 	device->wakeup.flags.notifier_present = 0;
 
 	/* Power button, Lid switch always enable wakeup */
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index d6a8cd14de2e..6c949602cbd1 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -16,6 +16,7 @@
 #include <linux/device.h>
 #include <linux/suspend.h>
 #include <linux/reboot.h>
+#include <linux/acpi.h>
 
 #include <asm/io.h>
 
@@ -199,8 +200,6 @@ static void acpi_pm_end(void)
 #endif /* CONFIG_ACPI_SLEEP */
 
 #ifdef CONFIG_SUSPEND
-extern void do_suspend_lowlevel(void);
-
 static u32 acpi_suspend_states[] = {
 	[PM_SUSPEND_ON] = ACPI_STATE_S0,
 	[PM_SUSPEND_STANDBY] = ACPI_STATE_S1,
@@ -243,20 +242,11 @@ static int acpi_suspend_begin(suspend_state_t pm_state)
 static int acpi_suspend_enter(suspend_state_t pm_state)
 {
 	acpi_status status = AE_OK;
-	unsigned long flags = 0;
 	u32 acpi_state = acpi_target_sleep_state;
+	int error;
 
 	ACPI_FLUSH_CPU_CACHE();
 
-	/* Do arch specific saving of state. */
-	if (acpi_state == ACPI_STATE_S3) {
-		int error = acpi_save_state_mem();
-
-		if (error)
-			return error;
-	}
-
-	local_irq_save(flags);
 	switch (acpi_state) {
 	case ACPI_STATE_S1:
 		barrier();
@@ -264,7 +254,10 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
 		break;
 
 	case ACPI_STATE_S3:
-		do_suspend_lowlevel();
+		error = acpi_suspend_lowlevel();
+		if (error)
+			return error;
+		pr_info(PREFIX "Low-level resume complete\n");
 		break;
 	}
 
@@ -290,13 +283,6 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
 	/* Allow EC transactions to happen. */
 	acpi_ec_unblock_transactions_early();
 
-	local_irq_restore(flags);
-	printk(KERN_DEBUG "Back to C!\n");
-
-	/* restore processor state */
-	if (acpi_state == ACPI_STATE_S3)
-		acpi_restore_state_mem();
-
 	suspend_nvs_restore();
 
 	return ACPI_SUCCESS(status) ? 0 : -EFAULT;
@@ -472,16 +458,13 @@ static int acpi_hibernation_begin(void)
 static int acpi_hibernation_enter(void)
 {
 	acpi_status status = AE_OK;
-	unsigned long flags = 0;
 
 	ACPI_FLUSH_CPU_CACHE();
 
-	local_irq_save(flags);
 	/* This shouldn't return.  If it returns, we have a problem */
 	status = acpi_enter_sleep_state(ACPI_STATE_S4);
 	/* Reprogram control registers and execute _BFS */
 	acpi_leave_sleep_state_prep(ACPI_STATE_S4);
-	local_irq_restore(flags);
 
 	return ACPI_SUCCESS(status) ? 0 : -EFAULT;
 }
@@ -585,7 +568,7 @@ int acpi_suspend(u32 acpi_state)
 	return -EINVAL;
 }
 
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 /**
  *	acpi_pm_device_sleep_state - return preferred power state of ACPI device
  *		in the system sleep state given by %acpi_target_sleep_state
@@ -671,7 +654,7 @@ int acpi_pm_device_sleep_state(struct device *dev, int *d_min_p)
 		*d_min_p = d_min;
 	return d_max;
 }
-#endif /* CONFIG_PM_OPS */
+#endif /* CONFIG_PM */
 
 #ifdef CONFIG_PM_SLEEP
 /**
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index e7df019d29d4..6d2bb2524b6e 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -13,16 +13,17 @@
 #include <linux/string.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
 #include <linux/amba/bus.h>
 
 #include <asm/irq.h>
 #include <asm/sizes.h>
 
-#define to_amba_device(d)	container_of(d, struct amba_device, dev)
 #define to_amba_driver(d)	container_of(d, struct amba_driver, drv)
 
-static struct amba_id *
-amba_lookup(struct amba_id *table, struct amba_device *dev)
+static const struct amba_id *
+amba_lookup(const struct amba_id *table, struct amba_device *dev)
 {
 	int ret = 0;
 
@@ -57,26 +58,6 @@ static int amba_uevent(struct device *dev, struct kobj_uevent_env *env)
 #define amba_uevent NULL
 #endif
 
-static int amba_suspend(struct device *dev, pm_message_t state)
-{
-	struct amba_driver *drv = to_amba_driver(dev->driver);
-	int ret = 0;
-
-	if (dev->driver && drv->suspend)
-		ret = drv->suspend(to_amba_device(dev), state);
-	return ret;
-}
-
-static int amba_resume(struct device *dev)
-{
-	struct amba_driver *drv = to_amba_driver(dev->driver);
-	int ret = 0;
-
-	if (dev->driver && drv->resume)
-		ret = drv->resume(to_amba_device(dev));
-	return ret;
-}
-
 #define amba_attr_func(name,fmt,arg...)					\
 static ssize_t name##_show(struct device *_dev,				\
 			   struct device_attribute *attr, char *buf)	\
@@ -102,17 +83,330 @@ static struct device_attribute amba_dev_attrs[] = {
 	__ATTR_NULL,
 };
 
+#ifdef CONFIG_PM_SLEEP
+
+static int amba_legacy_suspend(struct device *dev, pm_message_t mesg)
+{
+	struct amba_driver *adrv = to_amba_driver(dev->driver);
+	struct amba_device *adev = to_amba_device(dev);
+	int ret = 0;
+
+	if (dev->driver && adrv->suspend)
+		ret = adrv->suspend(adev, mesg);
+
+	return ret;
+}
+
+static int amba_legacy_resume(struct device *dev)
+{
+	struct amba_driver *adrv = to_amba_driver(dev->driver);
+	struct amba_device *adev = to_amba_device(dev);
+	int ret = 0;
+
+	if (dev->driver && adrv->resume)
+		ret = adrv->resume(adev);
+
+	return ret;
+}
+
+static int amba_pm_prepare(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (drv && drv->pm && drv->pm->prepare)
+		ret = drv->pm->prepare(dev);
+
+	return ret;
+}
+
+static void amba_pm_complete(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+
+	if (drv && drv->pm && drv->pm->complete)
+		drv->pm->complete(dev);
+}
+
+#else /* !CONFIG_PM_SLEEP */
+
+#define amba_pm_prepare		NULL
+#define amba_pm_complete		NULL
+
+#endif /* !CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_SUSPEND
+
+static int amba_pm_suspend(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->suspend)
+			ret = drv->pm->suspend(dev);
+	} else {
+		ret = amba_legacy_suspend(dev, PMSG_SUSPEND);
+	}
+
+	return ret;
+}
+
+static int amba_pm_suspend_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->suspend_noirq)
+			ret = drv->pm->suspend_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_resume(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->resume)
+			ret = drv->pm->resume(dev);
+	} else {
+		ret = amba_legacy_resume(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_resume_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->resume_noirq)
+			ret = drv->pm->resume_noirq(dev);
+	}
+
+	return ret;
+}
+
+#else /* !CONFIG_SUSPEND */
+
+#define amba_pm_suspend		NULL
+#define amba_pm_resume		NULL
+#define amba_pm_suspend_noirq	NULL
+#define amba_pm_resume_noirq	NULL
+
+#endif /* !CONFIG_SUSPEND */
+
+#ifdef CONFIG_HIBERNATION
+
+static int amba_pm_freeze(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->freeze)
+			ret = drv->pm->freeze(dev);
+	} else {
+		ret = amba_legacy_suspend(dev, PMSG_FREEZE);
+	}
+
+	return ret;
+}
+
+static int amba_pm_freeze_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->freeze_noirq)
+			ret = drv->pm->freeze_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_thaw(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->thaw)
+			ret = drv->pm->thaw(dev);
+	} else {
+		ret = amba_legacy_resume(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_thaw_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->thaw_noirq)
+			ret = drv->pm->thaw_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_poweroff(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->poweroff)
+			ret = drv->pm->poweroff(dev);
+	} else {
+		ret = amba_legacy_suspend(dev, PMSG_HIBERNATE);
+	}
+
+	return ret;
+}
+
+static int amba_pm_poweroff_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->poweroff_noirq)
+			ret = drv->pm->poweroff_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_restore(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->restore)
+			ret = drv->pm->restore(dev);
+	} else {
+		ret = amba_legacy_resume(dev);
+	}
+
+	return ret;
+}
+
+static int amba_pm_restore_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->restore_noirq)
+			ret = drv->pm->restore_noirq(dev);
+	}
+
+	return ret;
+}
+
+#else /* !CONFIG_HIBERNATION */
+
+#define amba_pm_freeze		NULL
+#define amba_pm_thaw		NULL
+#define amba_pm_poweroff		NULL
+#define amba_pm_restore		NULL
+#define amba_pm_freeze_noirq	NULL
+#define amba_pm_thaw_noirq		NULL
+#define amba_pm_poweroff_noirq	NULL
+#define amba_pm_restore_noirq	NULL
+
+#endif /* !CONFIG_HIBERNATION */
+
+#ifdef CONFIG_PM
+
+static const struct dev_pm_ops amba_pm = {
+	.prepare	= amba_pm_prepare,
+	.complete	= amba_pm_complete,
+	.suspend	= amba_pm_suspend,
+	.resume		= amba_pm_resume,
+	.freeze		= amba_pm_freeze,
+	.thaw		= amba_pm_thaw,
+	.poweroff	= amba_pm_poweroff,
+	.restore	= amba_pm_restore,
+	.suspend_noirq	= amba_pm_suspend_noirq,
+	.resume_noirq	= amba_pm_resume_noirq,
+	.freeze_noirq	= amba_pm_freeze_noirq,
+	.thaw_noirq	= amba_pm_thaw_noirq,
+	.poweroff_noirq	= amba_pm_poweroff_noirq,
+	.restore_noirq	= amba_pm_restore_noirq,
+	SET_RUNTIME_PM_OPS(
+		pm_generic_runtime_suspend,
+		pm_generic_runtime_resume,
+		pm_generic_runtime_idle
+	)
+};
+
+#define AMBA_PM (&amba_pm)
+
+#else /* !CONFIG_PM */
+
+#define AMBA_PM	NULL
+
+#endif /* !CONFIG_PM */
+
 /*
  * Primecells are part of the Advanced Microcontroller Bus Architecture,
  * so we call the bus "amba".
  */
-static struct bus_type amba_bustype = {
+struct bus_type amba_bustype = {
 	.name		= "amba",
 	.dev_attrs	= amba_dev_attrs,
 	.match		= amba_match,
 	.uevent		= amba_uevent,
-	.suspend	= amba_suspend,
-	.resume		= amba_resume,
+	.pm		= AMBA_PM,
 };
 
 static int __init amba_init(void)
@@ -188,7 +482,7 @@ static int amba_probe(struct device *dev)
 {
 	struct amba_device *pcdev = to_amba_device(dev);
 	struct amba_driver *pcdrv = to_amba_driver(dev->driver);
-	struct amba_id *id = amba_lookup(pcdrv->id_table, pcdev);
+	const struct amba_id *id = amba_lookup(pcdrv->id_table, pcdev);
 	int ret;
 
 	do {
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index c2328aed0836..8a1fc396f055 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -202,6 +202,18 @@ config SATA_DWC
 
 	  If unsure, say N.
 
+config SATA_DWC_DEBUG
+	bool "Debugging driver version"
+	depends on SATA_DWC
+	help
+	  This option enables debugging output in the driver.
+
+config SATA_DWC_VDEBUG
+	bool "Verbose debug output"
+	depends on SATA_DWC_DEBUG
+	help
+	  This option enables the taskfile dumping and NCQ debugging.
+
 config SATA_MV
 	tristate "Marvell SATA support"
 	help
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index b8d96ce37fc9..256d2b72cd1c 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -175,8 +175,7 @@ static const struct ata_port_info ahci_port_info[] = {
 	{
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_NCQ | AHCI_HFLAG_NO_MSI |
 				 AHCI_HFLAG_MV_PATA | AHCI_HFLAG_NO_PMP),
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA,
+		.flags		= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
@@ -383,6 +382,8 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	  .class = PCI_CLASS_STORAGE_SATA_AHCI,
 	  .class_mask = 0xffffff,
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9128 */
+	{ PCI_DEVICE(0x1b4b, 0x9125),
+	  .driver_data = board_ahci_yes_fbs },			/* 88se9125 */
 
 	/* Promise */
 	{ PCI_VDEVICE(PROMISE, 0x3f20), board_ahci },	/* PDC42819 */
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 3e606c34f57b..ccaf08122058 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -213,10 +213,8 @@ enum {
 
 	/* ap->flags bits */
 
-	AHCI_FLAG_COMMON		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-					  ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA |
-					  ATA_FLAG_ACPI_SATA | ATA_FLAG_AN |
-					  ATA_FLAG_LPM,
+	AHCI_FLAG_COMMON		= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA |
+					  ATA_FLAG_ACPI_SATA | ATA_FLAG_AN,
 
 	ICH_MAP				= 0x90, /* ICH MAP register */
 
diff --git a/drivers/ata/ata_generic.c b/drivers/ata/ata_generic.c
index 6981f7680a00..721d38bfa339 100644
--- a/drivers/ata/ata_generic.c
+++ b/drivers/ata/ata_generic.c
@@ -237,7 +237,7 @@ static struct pci_device_id ata_generic[] = {
 #endif
 	/* Intel, IDE class device */
 	{ PCI_VENDOR_ID_INTEL, PCI_ANY_ID, PCI_ANY_ID, PCI_ANY_ID,
-	  PCI_CLASS_STORAGE_IDE << 8, 0xFFFFFF00UL, 
+	  PCI_CLASS_STORAGE_IDE << 8, 0xFFFFFF00UL,
 	  .driver_data = ATA_GEN_INTEL_IDER },
 	/* Must come last. If you add entries adjust this table appropriately */
 	{ PCI_DEVICE_CLASS(PCI_CLASS_STORAGE_IDE << 8, 0xFFFFFF00UL),
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index 6cb14ca8ee85..cdec4ab3b159 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -230,7 +230,7 @@ static const struct pci_device_id piix_pci_tbl[] = {
 	{ 0x8086, 0x2850, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich_pata_100 },
 
 	/* SATA ports */
-	
+
 	/* 82801EB (ICH5) */
 	{ 0x8086, 0x24d1, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich5_sata },
 	/* 82801EB (ICH5) */
diff --git a/drivers/ata/libata-acpi.c b/drivers/ata/libata-acpi.c
index 8b5ea399a4f4..a791b8ce6294 100644
--- a/drivers/ata/libata-acpi.c
+++ b/drivers/ata/libata-acpi.c
@@ -660,8 +660,7 @@ static int ata_acpi_filter_tf(struct ata_device *dev,
  * @dev: target ATA device
  * @gtf: raw ATA taskfile register set (0x1f1 - 0x1f7)
  *
- * Outputs ATA taskfile to standard ATA host controller using MMIO
- * or PIO as indicated by the ATA_FLAG_MMIO flag.
+ * Outputs ATA taskfile to standard ATA host controller.
  * Writes the control, feature, nsect, lbal, lbam, and lbah registers.
  * Optionally (ATA_TFLAG_LBA48) writes hob_feature, hob_nsect,
  * hob_lbal, hob_lbam, and hob_lbah.
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index d4e52e214859..b91e19cab102 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -4210,7 +4210,7 @@ static int glob_match (const char *text, const char *pattern)
 		return 0;  /* End of both strings: match */
 	return 1;  /* No match */
 }
- 
+
 static unsigned long ata_dev_blacklisted(const struct ata_device *dev)
 {
 	unsigned char model_num[ATA_ID_PROD_LEN + 1];
@@ -5479,7 +5479,7 @@ struct ata_port *ata_port_alloc(struct ata_host *host)
 	ap = kzalloc(sizeof(*ap), GFP_KERNEL);
 	if (!ap)
 		return NULL;
-	
+
 	ap->pflags |= ATA_PFLAG_INITIALIZING;
 	ap->lock = &host->lock;
 	ap->print_id = -1;
@@ -5887,21 +5887,9 @@ void ata_host_init(struct ata_host *host, struct device *dev,
 	host->ops = ops;
 }
 
-
-static void async_port_probe(void *data, async_cookie_t cookie)
+int ata_port_probe(struct ata_port *ap)
 {
-	int rc;
-	struct ata_port *ap = data;
-
-	/*
-	 * If we're not allowed to scan this host in parallel,
-	 * we need to wait until all previous scans have completed
-	 * before going further.
-	 * Jeff Garzik says this is only within a controller, so we
-	 * don't need to wait for port 0, only for later ports.
-	 */
-	if (!(ap->host->flags & ATA_HOST_PARALLEL_SCAN) && ap->port_no != 0)
-		async_synchronize_cookie(cookie);
+	int rc = 0;
 
 	/* probe */
 	if (ap->ops->error_handler) {
@@ -5927,23 +5915,33 @@ static void async_port_probe(void *data, async_cookie_t cookie)
 		DPRINTK("ata%u: bus probe begin\n", ap->print_id);
 		rc = ata_bus_probe(ap);
 		DPRINTK("ata%u: bus probe end\n", ap->print_id);
-
-		if (rc) {
-			/* FIXME: do something useful here?
-			 * Current libata behavior will
-			 * tear down everything when
-			 * the module is removed
-			 * or the h/w is unplugged.
-			 */
-		}
 	}
+	return rc;
+}
+
+
+static void async_port_probe(void *data, async_cookie_t cookie)
+{
+	struct ata_port *ap = data;
+
+	/*
+	 * If we're not allowed to scan this host in parallel,
+	 * we need to wait until all previous scans have completed
+	 * before going further.
+	 * Jeff Garzik says this is only within a controller, so we
+	 * don't need to wait for port 0, only for later ports.
+	 */
+	if (!(ap->host->flags & ATA_HOST_PARALLEL_SCAN) && ap->port_no != 0)
+		async_synchronize_cookie(cookie);
+
+	(void)ata_port_probe(ap);
 
 	/* in order to keep device order, we need to synchronize at this point */
 	async_synchronize_cookie(cookie);
 
 	ata_scsi_scan_host(ap, 1);
-
 }
+
 /**
  *	ata_host_register - register initialized ATA host
  *	@host: ATA host to register
@@ -5983,7 +5981,7 @@ int ata_host_register(struct ata_host *host, struct scsi_host_template *sht)
 	for (i = 0; i < host->n_ports; i++)
 		host->ports[i]->print_id = ata_print_id++;
 
-	
+
 	/* Create associated sysfs transport objects  */
 	for (i = 0; i < host->n_ports; i++) {
 		rc = ata_tport_add(host->dev,host->ports[i]);
@@ -6471,7 +6469,7 @@ static int __init ata_init(void)
 		ata_sff_exit();
 		rc = -ENOMEM;
 		goto err_out;
-	}		
+	}
 
 	printk(KERN_DEBUG "libata version " DRV_VERSION " loaded.\n");
 	return 0;
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index 17a637877d03..df3f3140c9c7 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -587,11 +587,43 @@ static void ata_eh_unload(struct ata_port *ap)
 void ata_scsi_error(struct Scsi_Host *host)
 {
 	struct ata_port *ap = ata_shost_to_port(host);
-	int i;
 	unsigned long flags;
+	LIST_HEAD(eh_work_q);
 
 	DPRINTK("ENTER\n");
 
+	spin_lock_irqsave(host->host_lock, flags);
+	list_splice_init(&host->eh_cmd_q, &eh_work_q);
+	spin_unlock_irqrestore(host->host_lock, flags);
+
+	ata_scsi_cmd_error_handler(host, ap, &eh_work_q);
+
+	/* If we timed raced normal completion and there is nothing to
+	   recover nr_timedout == 0 why exactly are we doing error recovery ? */
+	ata_scsi_port_error_handler(host, ap);
+
+	/* finish or retry handled scmd's and clean up */
+	WARN_ON(host->host_failed || !list_empty(&eh_work_q));
+
+	DPRINTK("EXIT\n");
+}
+
+/**
+ * ata_scsi_cmd_error_handler - error callback for a list of commands
+ * @host:	scsi host containing the port
+ * @ap:		ATA port within the host
+ * @eh_work_q:	list of commands to process
+ *
+ * process the given list of commands and return those finished to the
+ * ap->eh_done_q.  This function is the first part of the libata error
+ * handler which processes a given list of failed commands.
+ */
+void ata_scsi_cmd_error_handler(struct Scsi_Host *host, struct ata_port *ap,
+				struct list_head *eh_work_q)
+{
+	int i;
+	unsigned long flags;
+
 	/* make sure sff pio task is not running */
 	ata_sff_flush_pio_task(ap);
 
@@ -627,7 +659,7 @@ void ata_scsi_error(struct Scsi_Host *host)
 		if (ap->ops->lost_interrupt)
 			ap->ops->lost_interrupt(ap);
 
-		list_for_each_entry_safe(scmd, tmp, &host->eh_cmd_q, eh_entry) {
+		list_for_each_entry_safe(scmd, tmp, eh_work_q, eh_entry) {
 			struct ata_queued_cmd *qc;
 
 			for (i = 0; i < ATA_MAX_QUEUE; i++) {
@@ -671,8 +703,20 @@ void ata_scsi_error(struct Scsi_Host *host)
 	} else
 		spin_unlock_wait(ap->lock);
 
-	/* If we timed raced normal completion and there is nothing to
-	   recover nr_timedout == 0 why exactly are we doing error recovery ? */
+}
+EXPORT_SYMBOL(ata_scsi_cmd_error_handler);
+
+/**
+ * ata_scsi_port_error_handler - recover the port after the commands
+ * @host:	SCSI host containing the port
+ * @ap:		the ATA port
+ *
+ * Handle the recovery of the port @ap after all the commands
+ * have been recovered.
+ */
+void ata_scsi_port_error_handler(struct Scsi_Host *host, struct ata_port *ap)
+{
+	unsigned long flags;
 
 	/* invoke error handler */
 	if (ap->ops->error_handler) {
@@ -761,9 +805,6 @@ void ata_scsi_error(struct Scsi_Host *host)
 		ap->ops->eng_timeout(ap);
 	}
 
-	/* finish or retry handled scmd's and clean up */
-	WARN_ON(host->host_failed || !list_empty(&host->eh_cmd_q));
-
 	scsi_eh_flush_done_q(&ap->eh_done_q);
 
 	/* clean up */
@@ -784,9 +825,8 @@ void ata_scsi_error(struct Scsi_Host *host)
 	wake_up_all(&ap->eh_wait_q);
 
 	spin_unlock_irqrestore(ap->lock, flags);
-
-	DPRINTK("EXIT\n");
 }
+EXPORT_SYMBOL_GPL(ata_scsi_port_error_handler);
 
 /**
  *	ata_port_wait_eh - Wait for the currently pending EH to complete
@@ -1618,7 +1658,7 @@ static void ata_eh_analyze_serror(struct ata_link *link)
 	 * host links.  For disabled PMP links, only N bit is
 	 * considered as X bit is left at 1 for link plugging.
 	 */
-	if (link->lpm_policy != ATA_LPM_MAX_POWER)
+	if (link->lpm_policy > ATA_LPM_MAX_POWER)
 		hotplug_mask = 0;	/* hotplug doesn't work w/ LPM */
 	else if (!(link->flags & ATA_LFLAG_DISABLED) || ata_is_host_link(link))
 		hotplug_mask = SERR_PHYRDY_CHG | SERR_DEV_XCHG;
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 600f6353ecf8..c11675f34b93 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -3759,7 +3759,7 @@ struct ata_port *ata_sas_port_alloc(struct ata_host *host,
 		return NULL;
 
 	ap->port_no = 0;
-	ap->lock = shost->host_lock;
+	ap->lock = &host->lock;
 	ap->pio_mask = port_info->pio_mask;
 	ap->mwdma_mask = port_info->mwdma_mask;
 	ap->udma_mask = port_info->udma_mask;
@@ -3821,7 +3821,7 @@ int ata_sas_port_init(struct ata_port *ap)
 
 	if (!rc) {
 		ap->print_id = ata_print_id++;
-		rc = ata_bus_probe(ap);
+		rc = ata_port_probe(ap);
 	}
 
 	return rc;
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index af6141bb1ba3..e75a02d61a8f 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -1336,7 +1336,7 @@ static void ata_sff_pio_task(struct work_struct *work)
 	u8 status;
 	int poll_next;
 
-	BUG_ON(ap->sff_pio_task_link == NULL); 
+	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) {
diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h
index a9be110dbf51..773de97988a2 100644
--- a/drivers/ata/libata.h
+++ b/drivers/ata/libata.h
@@ -103,6 +103,7 @@ extern int ata_task_ioctl(struct scsi_device *scsidev, void __user *arg);
 extern int ata_cmd_ioctl(struct scsi_device *scsidev, void __user *arg);
 extern struct ata_port *ata_port_alloc(struct ata_host *host);
 extern const char *sata_spd_string(unsigned int spd);
+extern int ata_port_probe(struct ata_port *ap);
 
 /* libata-acpi.c */
 #ifdef CONFIG_ATA_ACPI
diff --git a/drivers/ata/pata_acpi.c b/drivers/ata/pata_acpi.c
index c8d47034d5e9..91949d997555 100644
--- a/drivers/ata/pata_acpi.c
+++ b/drivers/ata/pata_acpi.c
@@ -245,7 +245,7 @@ static struct ata_port_operations pacpi_ops = {
 static int pacpi_init_one (struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	static const struct ata_port_info info = {
-		.flags		= ATA_FLAG_SLAVE_POSS | ATA_FLAG_SRST,
+		.flags		= ATA_FLAG_SLAVE_POSS,
 
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
diff --git a/drivers/ata/pata_at32.c b/drivers/ata/pata_at32.c
index 66ce6a526f27..36f189c7ee8c 100644
--- a/drivers/ata/pata_at32.c
+++ b/drivers/ata/pata_at32.c
@@ -194,7 +194,7 @@ static int __init pata_at32_init_one(struct device *dev,
 	/* Setup ATA bindings */
 	ap->ops	     = &at32_port_ops;
 	ap->pio_mask = PIO_MASK;
-	ap->flags   |= ATA_FLAG_MMIO | ATA_FLAG_SLAVE_POSS;
+	ap->flags   |= ATA_FLAG_SLAVE_POSS;
 
 	/*
 	 * Since all 8-bit taskfile transfers has to go on the lower
diff --git a/drivers/ata/pata_bf54x.c b/drivers/ata/pata_bf54x.c
index 7aed5c792597..e0b58b8dfe6f 100644
--- a/drivers/ata/pata_bf54x.c
+++ b/drivers/ata/pata_bf54x.c
@@ -1454,9 +1454,7 @@ static struct ata_port_operations bfin_pata_ops = {
 
 static struct ata_port_info bfin_port_info[] = {
 	{
-		.flags		= ATA_FLAG_SLAVE_POSS
-				| ATA_FLAG_MMIO
-				| ATA_FLAG_NO_LEGACY,
+		.flags		= ATA_FLAG_SLAVE_POSS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= 0,
 		.udma_mask	= 0,
diff --git a/drivers/ata/pata_hpt3x3.c b/drivers/ata/pata_hpt3x3.c
index b63d5e2d4628..24d7df81546b 100644
--- a/drivers/ata/pata_hpt3x3.c
+++ b/drivers/ata/pata_hpt3x3.c
@@ -151,7 +151,7 @@ static struct ata_port_operations hpt3x3_port_ops = {
 	.check_atapi_dma= hpt3x3_atapi_dma,
 	.freeze		= hpt3x3_freeze,
 #endif
-	
+
 };
 
 /**
diff --git a/drivers/ata/pata_it821x.c b/drivers/ata/pata_it821x.c
index aa0e0c51cc08..2d15f2548a10 100644
--- a/drivers/ata/pata_it821x.c
+++ b/drivers/ata/pata_it821x.c
@@ -616,7 +616,7 @@ static void it821x_display_disk(int n, u8 *buf)
 	if (buf[52] > 4)	/* No Disk */
 		return;
 
-	ata_id_c_string((u16 *)buf, id, 0, 41); 
+	ata_id_c_string((u16 *)buf, id, 0, 41);
 
 	if (buf[51]) {
 		mode = ffs(buf[51]);
@@ -910,7 +910,7 @@ static int it821x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	rc = pcim_enable_device(pdev);
 	if (rc)
 		return rc;
-		
+
 	if (pdev->vendor == PCI_VENDOR_ID_RDC) {
 		/* Deal with Vortex86SX */
 		if (pdev->revision == 0x11)
diff --git a/drivers/ata/pata_ixp4xx_cf.c b/drivers/ata/pata_ixp4xx_cf.c
index ba54b089f98c..5253b271b3fe 100644
--- a/drivers/ata/pata_ixp4xx_cf.c
+++ b/drivers/ata/pata_ixp4xx_cf.c
@@ -177,7 +177,7 @@ static __devinit int ixp4xx_pata_probe(struct platform_device *pdev)
 
 	ap->ops	= &ixp4xx_port_ops;
 	ap->pio_mask = ATA_PIO4;
-	ap->flags |= ATA_FLAG_MMIO | ATA_FLAG_NO_LEGACY | ATA_FLAG_NO_ATAPI;
+	ap->flags |= ATA_FLAG_NO_ATAPI;
 
 	ixp4xx_setup_port(ap, data, cs0->start, cs1->start);
 
diff --git a/drivers/ata/pata_macio.c b/drivers/ata/pata_macio.c
index 75b49d01780b..46f589edccdb 100644
--- a/drivers/ata/pata_macio.c
+++ b/drivers/ata/pata_macio.c
@@ -1053,8 +1053,7 @@ static int __devinit pata_macio_common_init(struct pata_macio_priv	*priv,
 	/* Allocate libata host for 1 port */
 	memset(&pinfo, 0, sizeof(struct ata_port_info));
 	pmac_macio_calc_timing_masks(priv, &pinfo);
-	pinfo.flags		= ATA_FLAG_SLAVE_POSS | ATA_FLAG_MMIO |
-				  ATA_FLAG_NO_LEGACY;
+	pinfo.flags		= ATA_FLAG_SLAVE_POSS;
 	pinfo.port_ops		= &pata_macio_ops;
 	pinfo.private_data	= priv;
 
diff --git a/drivers/ata/pata_marvell.c b/drivers/ata/pata_marvell.c
index dd38083dcbeb..75a6a0c0094f 100644
--- a/drivers/ata/pata_marvell.c
+++ b/drivers/ata/pata_marvell.c
@@ -38,7 +38,7 @@ static int marvell_pata_active(struct pci_dev *pdev)
 
 	/* We don't yet know how to do this for other devices */
 	if (pdev->device != 0x6145)
-		return 1;	
+		return 1;
 
 	barp = pci_iomap(pdev, 5, 0x10);
 	if (barp == NULL)
diff --git a/drivers/ata/pata_mpc52xx.c b/drivers/ata/pata_mpc52xx.c
index d7d8026cde99..2fcac511d39c 100644
--- a/drivers/ata/pata_mpc52xx.c
+++ b/drivers/ata/pata_mpc52xx.c
@@ -680,7 +680,7 @@ mpc52xx_ata_remove_one(struct device *dev)
 /* ======================================================================== */
 
 static int __devinit
-mpc52xx_ata_probe(struct platform_device *op, const struct of_device_id *match)
+mpc52xx_ata_probe(struct platform_device *op)
 {
 	unsigned int ipb_freq;
 	struct resource res_mem;
@@ -883,7 +883,7 @@ static struct of_device_id mpc52xx_ata_of_match[] = {
 };
 
 
-static struct of_platform_driver mpc52xx_ata_of_platform_driver = {
+static struct platform_driver mpc52xx_ata_of_platform_driver = {
 	.probe		= mpc52xx_ata_probe,
 	.remove		= mpc52xx_ata_remove,
 #ifdef CONFIG_PM
@@ -906,13 +906,13 @@ static int __init
 mpc52xx_ata_init(void)
 {
 	printk(KERN_INFO "ata: MPC52xx IDE/ATA libata driver\n");
-	return of_register_platform_driver(&mpc52xx_ata_of_platform_driver);
+	return platform_driver_register(&mpc52xx_ata_of_platform_driver);
 }
 
 static void __exit
 mpc52xx_ata_exit(void)
 {
-	of_unregister_platform_driver(&mpc52xx_ata_of_platform_driver);
+	platform_driver_unregister(&mpc52xx_ata_of_platform_driver);
 }
 
 module_init(mpc52xx_ata_init);
diff --git a/drivers/ata/pata_ninja32.c b/drivers/ata/pata_ninja32.c
index cc50bd09aa26..e277a142138c 100644
--- a/drivers/ata/pata_ninja32.c
+++ b/drivers/ata/pata_ninja32.c
@@ -165,7 +165,7 @@ static int ninja32_reinit_one(struct pci_dev *pdev)
 		return rc;
 	ninja32_program(host->iomap[0]);
 	ata_host_resume(host);
-	return 0;			
+	return 0;
 }
 #endif
 
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index fa1b95a9a7ff..220ddc90608f 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -848,8 +848,7 @@ static int __devinit octeon_cf_probe(struct platform_device *pdev)
 	cf_port->ap = ap;
 	ap->ops = &octeon_cf_ops;
 	ap->pio_mask = ATA_PIO6;
-	ap->flags |= ATA_FLAG_MMIO | ATA_FLAG_NO_LEGACY
-		  | ATA_FLAG_NO_ATAPI | ATA_FLAG_PIO_POLLING;
+	ap->flags |= ATA_FLAG_NO_ATAPI | ATA_FLAG_PIO_POLLING;
 
 	base = cs0 + ocd->base_region_bias;
 	if (!ocd->is16bit) {
diff --git a/drivers/ata/pata_of_platform.c b/drivers/ata/pata_of_platform.c
index 480e043ce6b8..f3054009bd25 100644
--- a/drivers/ata/pata_of_platform.c
+++ b/drivers/ata/pata_of_platform.c
@@ -14,8 +14,7 @@
 #include <linux/of_platform.h>
 #include <linux/ata_platform.h>
 
-static int __devinit pata_of_platform_probe(struct platform_device *ofdev,
-					    const struct of_device_id *match)
+static int __devinit pata_of_platform_probe(struct platform_device *ofdev)
 {
 	int ret;
 	struct device_node *dn = ofdev->dev.of_node;
@@ -90,7 +89,7 @@ static struct of_device_id pata_of_platform_match[] = {
 };
 MODULE_DEVICE_TABLE(of, pata_of_platform_match);
 
-static struct of_platform_driver pata_of_platform_driver = {
+static struct platform_driver pata_of_platform_driver = {
 	.driver = {
 		.name = "pata_of_platform",
 		.owner = THIS_MODULE,
@@ -102,13 +101,13 @@ static struct of_platform_driver pata_of_platform_driver = {
 
 static int __init pata_of_platform_init(void)
 {
-	return of_register_platform_driver(&pata_of_platform_driver);
+	return platform_driver_register(&pata_of_platform_driver);
 }
 module_init(pata_of_platform_init);
 
 static void __exit pata_of_platform_exit(void)
 {
-	of_unregister_platform_driver(&pata_of_platform_driver);
+	platform_driver_unregister(&pata_of_platform_driver);
 }
 module_exit(pata_of_platform_exit);
 
diff --git a/drivers/ata/pata_palmld.c b/drivers/ata/pata_palmld.c
index 11fb4ccc74b4..a2a73d953840 100644
--- a/drivers/ata/pata_palmld.c
+++ b/drivers/ata/pata_palmld.c
@@ -85,7 +85,7 @@ static __devinit int palmld_pata_probe(struct platform_device *pdev)
 	ap = host->ports[0];
 	ap->ops	= &palmld_port_ops;
 	ap->pio_mask = ATA_PIO4;
-	ap->flags |= ATA_FLAG_MMIO | ATA_FLAG_NO_LEGACY | ATA_FLAG_PIO_POLLING;
+	ap->flags |= ATA_FLAG_PIO_POLLING;
 
 	/* memory mapping voodoo */
 	ap->ioaddr.cmd_addr = mem + 0x10;
diff --git a/drivers/ata/pata_pcmcia.c b/drivers/ata/pata_pcmcia.c
index 806292160b3f..29af660d968b 100644
--- a/drivers/ata/pata_pcmcia.c
+++ b/drivers/ata/pata_pcmcia.c
@@ -124,7 +124,7 @@ static unsigned int ata_data_xfer_8bit(struct ata_device *dev,
  *	reset will recover the device.
  *
  */
- 
+
 static void pcmcia_8bit_drain_fifo(struct ata_queued_cmd *qc)
 {
 	int count;
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index b18351122525..9765ace16921 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -150,8 +150,7 @@ static struct ata_port_operations pdc2027x_pata133_ops = {
 static struct ata_port_info pdc2027x_port_info[] = {
 	/* PDC_UDMA_100 */
 	{
-		.flags		= ATA_FLAG_NO_LEGACY | ATA_FLAG_SLAVE_POSS |
-		                  ATA_FLAG_MMIO,
+		.flags		= ATA_FLAG_SLAVE_POSS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA5,
@@ -159,8 +158,7 @@ static struct ata_port_info pdc2027x_port_info[] = {
 	},
 	/* PDC_UDMA_133 */
 	{
-		.flags		= ATA_FLAG_NO_LEGACY | ATA_FLAG_SLAVE_POSS |
-                        	  ATA_FLAG_MMIO,
+		.flags		= ATA_FLAG_SLAVE_POSS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/pata_pxa.c b/drivers/ata/pata_pxa.c
index 1898c6ed4b4e..b4ede40f8ae1 100644
--- a/drivers/ata/pata_pxa.c
+++ b/drivers/ata/pata_pxa.c
@@ -292,7 +292,6 @@ static int __devinit pxa_ata_probe(struct platform_device *pdev)
 	ap->ops		= &pxa_ata_port_ops;
 	ap->pio_mask	= ATA_PIO4;
 	ap->mwdma_mask	= ATA_MWDMA2;
-	ap->flags	= ATA_FLAG_MMIO;
 
 	ap->ioaddr.cmd_addr	= devm_ioremap(&pdev->dev, cmd_res->start,
 						resource_size(cmd_res));
diff --git a/drivers/ata/pata_rb532_cf.c b/drivers/ata/pata_rb532_cf.c
index 0ffd631000b7..baeaf938d55b 100644
--- a/drivers/ata/pata_rb532_cf.c
+++ b/drivers/ata/pata_rb532_cf.c
@@ -91,7 +91,6 @@ static void rb532_pata_setup_ports(struct ata_host *ah)
 
 	ap->ops		= &rb532_pata_port_ops;
 	ap->pio_mask	= ATA_PIO4;
-	ap->flags	= ATA_FLAG_NO_LEGACY | ATA_FLAG_MMIO;
 
 	ap->ioaddr.cmd_addr	= info->iobase + RB500_CF_REG_BASE;
 	ap->ioaddr.ctl_addr	= info->iobase + RB500_CF_REG_CTRL;
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index 8a51d673e5b2..c446ae6055a3 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -531,7 +531,6 @@ static int __init pata_s3c_probe(struct platform_device *pdev)
 	}
 
 	ap = host->ports[0];
-	ap->flags |= ATA_FLAG_MMIO;
 	ap->pio_mask = ATA_PIO4;
 
 	if (cpu_type == TYPE_S3C64XX) {
diff --git a/drivers/ata/pata_scc.c b/drivers/ata/pata_scc.c
index 093715c3273a..88ea9b677b47 100644
--- a/drivers/ata/pata_scc.c
+++ b/drivers/ata/pata_scc.c
@@ -959,7 +959,7 @@ static struct ata_port_operations scc_pata_ops = {
 
 static struct ata_port_info scc_port_info[] = {
 	{
-		.flags		= ATA_FLAG_SLAVE_POSS | ATA_FLAG_MMIO | ATA_FLAG_NO_LEGACY,
+		.flags		= ATA_FLAG_SLAVE_POSS,
 		.pio_mask	= ATA_PIO4,
 		/* No MWDMA */
 		.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/pata_sis.c b/drivers/ata/pata_sis.c
index 60cea13cccce..c04abc393fc5 100644
--- a/drivers/ata/pata_sis.c
+++ b/drivers/ata/pata_sis.c
@@ -593,7 +593,7 @@ static const struct ata_port_info sis_info133 = {
 	.port_ops	= &sis_133_ops,
 };
 const struct ata_port_info sis_info133_for_sata = {
-	.flags		= ATA_FLAG_SLAVE_POSS | ATA_FLAG_SRST,
+	.flags		= ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
 	/* No MWDMA */
 	.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/pdc_adma.c b/drivers/ata/pdc_adma.c
index adbe0426c8f0..1111712b3d7d 100644
--- a/drivers/ata/pdc_adma.c
+++ b/drivers/ata/pdc_adma.c
@@ -166,9 +166,7 @@ static struct ata_port_operations adma_ata_ops = {
 static struct ata_port_info adma_port_info[] = {
 	/* board_1841_idx */
 	{
-		.flags		= ATA_FLAG_SLAVE_POSS |
-				  ATA_FLAG_NO_LEGACY | ATA_FLAG_MMIO |
-				  ATA_FLAG_PIO_POLLING,
+		.flags		= ATA_FLAG_SLAVE_POSS | ATA_FLAG_PIO_POLLING,
 		.pio_mask	= ATA_PIO4_ONLY,
 		.udma_mask	= ATA_UDMA4,
 		.port_ops	= &adma_ata_ops,
diff --git a/drivers/ata/sata_dwc_460ex.c b/drivers/ata/sata_dwc_460ex.c
index 6cf57c5c2b5f..1c4b3aa4c7c4 100644
--- a/drivers/ata/sata_dwc_460ex.c
+++ b/drivers/ata/sata_dwc_460ex.c
@@ -40,8 +40,11 @@
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
 
+/* These two are defined in "libata.h" */
+#undef	DRV_NAME
+#undef	DRV_VERSION
 #define DRV_NAME        "sata-dwc"
-#define DRV_VERSION     "1.0"
+#define DRV_VERSION     "1.3"
 
 /* SATA DMA driver Globals */
 #define DMA_NUM_CHANS		1
@@ -333,11 +336,47 @@ static int dma_dwc_xfer_setup(struct scatterlist *sg, int num_elems,
 			      void __iomem *addr, int dir);
 static void dma_dwc_xfer_start(int dma_ch);
 
+static const char *get_prot_descript(u8 protocol)
+{
+	switch ((enum ata_tf_protocols)protocol) {
+	case ATA_PROT_NODATA:
+		return "ATA no data";
+	case ATA_PROT_PIO:
+		return "ATA PIO";
+	case ATA_PROT_DMA:
+		return "ATA DMA";
+	case ATA_PROT_NCQ:
+		return "ATA NCQ";
+	case ATAPI_PROT_NODATA:
+		return "ATAPI no data";
+	case ATAPI_PROT_PIO:
+		return "ATAPI PIO";
+	case ATAPI_PROT_DMA:
+		return "ATAPI DMA";
+	default:
+		return "unknown";
+	}
+}
+
+static const char *get_dma_dir_descript(int dma_dir)
+{
+	switch ((enum dma_data_direction)dma_dir) {
+	case DMA_BIDIRECTIONAL:
+		return "bidirectional";
+	case DMA_TO_DEVICE:
+		return "to device";
+	case DMA_FROM_DEVICE:
+		return "from device";
+	default:
+		return "none";
+	}
+}
+
 static void sata_dwc_tf_dump(struct ata_taskfile *tf)
 {
 	dev_vdbg(host_pvt.dwc_dev, "taskfile cmd: 0x%02x protocol: %s flags:"
-		"0x%lx device: %x\n", tf->command, ata_get_cmd_descript\
-		(tf->protocol), tf->flags, tf->device);
+		"0x%lx device: %x\n", tf->command,
+		get_prot_descript(tf->protocol), tf->flags, tf->device);
 	dev_vdbg(host_pvt.dwc_dev, "feature: 0x%02x nsect: 0x%x lbal: 0x%x "
 		"lbam: 0x%x lbah: 0x%x\n", tf->feature, tf->nsect, tf->lbal,
 		 tf->lbam, tf->lbah);
@@ -715,7 +754,7 @@ static int dma_dwc_xfer_setup(struct scatterlist *sg, int num_elems,
 	/* Program the CTL register with src enable / dst enable */
 	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].ctl.low),
 		 DMA_CTL_LLP_SRCEN | DMA_CTL_LLP_DSTEN);
-	return 0;
+	return dma_ch;
 }
 
 /*
@@ -967,7 +1006,7 @@ static irqreturn_t sata_dwc_isr(int irq, void *dev_instance)
 		}
 
 		dev_dbg(ap->dev, "%s non-NCQ cmd interrupt, protocol: %s\n",
-			__func__, ata_get_cmd_descript(qc->tf.protocol));
+			__func__, get_prot_descript(qc->tf.protocol));
 DRVSTILLBUSY:
 		if (ata_is_dma(qc->tf.protocol)) {
 			/*
@@ -1057,7 +1096,7 @@ DRVSTILLBUSY:
 
 		/* Process completed command */
 		dev_dbg(ap->dev, "%s NCQ command, protocol: %s\n", __func__,
-			ata_get_cmd_descript(qc->tf.protocol));
+			get_prot_descript(qc->tf.protocol));
 		if (ata_is_dma(qc->tf.protocol)) {
 			host_pvt.dma_interrupt_count++;
 			if (hsdevp->dma_pending[tag] == \
@@ -1142,8 +1181,8 @@ static void sata_dwc_dma_xfer_complete(struct ata_port *ap, u32 check_status)
 	if (tag > 0) {
 		dev_info(ap->dev, "%s tag=%u cmd=0x%02x dma dir=%s proto=%s "
 			 "dmacr=0x%08x\n", __func__, qc->tag, qc->tf.command,
-			 ata_get_cmd_descript(qc->dma_dir),
-			 ata_get_cmd_descript(qc->tf.protocol),
+			 get_dma_dir_descript(qc->dma_dir),
+			 get_prot_descript(qc->tf.protocol),
 			 in_le32(&(hsdev->sata_dwc_regs->dmacr)));
 	}
 #endif
@@ -1354,7 +1393,7 @@ static void sata_dwc_exec_command_by_tag(struct ata_port *ap,
 	struct sata_dwc_device_port *hsdevp = HSDEVP_FROM_AP(ap);
 
 	dev_dbg(ap->dev, "%s cmd(0x%02x): %s tag=%d\n", __func__, tf->command,
-		ata_get_cmd_descript(tf), tag);
+		ata_get_cmd_descript(tf->command), tag);
 
 	spin_lock_irqsave(&ap->host->lock, flags);
 	hsdevp->cmd_issued[tag] = cmd_issued;
@@ -1413,7 +1452,7 @@ static void sata_dwc_bmdma_start_by_tag(struct ata_queued_cmd *qc, u8 tag)
 
 	dev_dbg(ap->dev, "%s qc=%p tag: %x cmd: 0x%02x dma_dir: %s "
 		"start_dma? %x\n", __func__, qc, tag, qc->tf.command,
-		ata_get_cmd_descript(qc->dma_dir), start_dma);
+		get_dma_dir_descript(qc->dma_dir), start_dma);
 	sata_dwc_tf_dump(&(qc->tf));
 
 	if (start_dma) {
@@ -1462,10 +1501,9 @@ static void sata_dwc_qc_prep_by_tag(struct ata_queued_cmd *qc, u8 tag)
 	int dma_chan;
 	struct sata_dwc_device *hsdev = HSDEV_FROM_AP(ap);
 	struct sata_dwc_device_port *hsdevp = HSDEVP_FROM_AP(ap);
-	int err;
 
 	dev_dbg(ap->dev, "%s: port=%d dma dir=%s n_elem=%d\n",
-		__func__, ap->port_no, ata_get_cmd_descript(qc->dma_dir),
+		__func__, ap->port_no, get_dma_dir_descript(qc->dma_dir),
 		 qc->n_elem);
 
 	dma_chan = dma_dwc_xfer_setup(sg, qc->n_elem, hsdevp->llit[tag],
@@ -1474,7 +1512,7 @@ static void sata_dwc_qc_prep_by_tag(struct ata_queued_cmd *qc, u8 tag)
 				      dmadr), qc->dma_dir);
 	if (dma_chan < 0) {
 		dev_err(ap->dev, "%s: dma_dwc_xfer_setup returns err %d\n",
-			__func__, err);
+			__func__, dma_chan);
 		return;
 	}
 	hsdevp->dma_chan[tag] = dma_chan;
@@ -1491,8 +1529,8 @@ static unsigned int sata_dwc_qc_issue(struct ata_queued_cmd *qc)
 		dev_info(ap->dev, "%s ap id=%d cmd(0x%02x)=%s qc tag=%d "
 			 "prot=%s ap active_tag=0x%08x ap sactive=0x%08x\n",
 			 __func__, ap->print_id, qc->tf.command,
-			 ata_get_cmd_descript(&qc->tf),
-			 qc->tag, ata_get_cmd_descript(qc->tf.protocol),
+			 ata_get_cmd_descript(qc->tf.command),
+			 qc->tag, get_prot_descript(qc->tf.protocol),
 			 ap->link.active_tag, ap->link.sactive);
 #endif
 
@@ -1533,7 +1571,7 @@ static void sata_dwc_qc_prep(struct ata_queued_cmd *qc)
 #ifdef DEBUG_NCQ
 	if (qc->tag > 0)
 		dev_info(qc->ap->dev, "%s: qc->tag=%d ap->active_tag=0x%08x\n",
-			 __func__, tag, qc->ap->link.active_tag);
+			 __func__, qc->tag, qc->ap->link.active_tag);
 
 	return ;
 #endif
@@ -1580,16 +1618,14 @@ static struct ata_port_operations sata_dwc_ops = {
 
 static const struct ata_port_info sata_dwc_port_info[] = {
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | ATA_FLAG_NCQ,
-		.pio_mask	= 0x1f,	/* pio 0-4 */
+		.flags		= ATA_FLAG_SATA | ATA_FLAG_NCQ,
+		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &sata_dwc_ops,
 	},
 };
 
-static int sata_dwc_probe(struct platform_device *ofdev,
-			const struct of_device_id *match)
+static int sata_dwc_probe(struct platform_device *ofdev)
 {
 	struct sata_dwc_device *hsdev;
 	u32 idr, versionr;
@@ -1727,7 +1763,7 @@ static const struct of_device_id sata_dwc_match[] = {
 };
 MODULE_DEVICE_TABLE(of, sata_dwc_match);
 
-static struct of_platform_driver sata_dwc_driver = {
+static struct platform_driver sata_dwc_driver = {
 	.driver = {
 		.name = DRV_NAME,
 		.owner = THIS_MODULE,
@@ -1739,12 +1775,12 @@ static struct of_platform_driver sata_dwc_driver = {
 
 static int __init sata_dwc_init(void)
 {
-	return	of_register_platform_driver(&sata_dwc_driver);
+	return platform_driver_register(&sata_dwc_driver);
 }
 
 static void __exit sata_dwc_exit(void)
 {
-	of_unregister_platform_driver(&sata_dwc_driver);
+	platform_driver_unregister(&sata_dwc_driver);
 }
 
 module_init(sata_dwc_init);
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index b0214d00d50b..883c967b11b9 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -33,8 +33,7 @@ enum {
 	SATA_FSL_MAX_PRD_USABLE	= SATA_FSL_MAX_PRD - 1,
 	SATA_FSL_MAX_PRD_DIRECT	= 16,	/* Direct PRDT entries */
 
-	SATA_FSL_HOST_FLAGS	= (ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA |
+	SATA_FSL_HOST_FLAGS	= (ATA_FLAG_SATA | ATA_FLAG_PIO_DMA |
 				ATA_FLAG_PMP | ATA_FLAG_NCQ | ATA_FLAG_AN),
 
 	SATA_FSL_MAX_CMDS	= SATA_FSL_QUEUE_DEPTH,
@@ -1293,8 +1292,7 @@ static const struct ata_port_info sata_fsl_port_info[] = {
 	 },
 };
 
-static int sata_fsl_probe(struct platform_device *ofdev,
-			const struct of_device_id *match)
+static int sata_fsl_probe(struct platform_device *ofdev)
 {
 	int retval = -ENXIO;
 	void __iomem *hcr_base = NULL;
@@ -1423,7 +1421,7 @@ static struct of_device_id fsl_sata_match[] = {
 
 MODULE_DEVICE_TABLE(of, fsl_sata_match);
 
-static struct of_platform_driver fsl_sata_driver = {
+static struct platform_driver fsl_sata_driver = {
 	.driver = {
 		.name = "fsl-sata",
 		.owner = THIS_MODULE,
@@ -1439,13 +1437,13 @@ static struct of_platform_driver fsl_sata_driver = {
 
 static int __init sata_fsl_init(void)
 {
-	of_register_platform_driver(&fsl_sata_driver);
+	platform_driver_register(&fsl_sata_driver);
 	return 0;
 }
 
 static void __exit sata_fsl_exit(void)
 {
-	of_unregister_platform_driver(&fsl_sata_driver);
+	platform_driver_unregister(&fsl_sata_driver);
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index bf74a36d3cc3..cd40651e9b72 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -160,8 +160,7 @@ enum {
 	/* Host Flags */
 	MV_FLAG_DUAL_HC		= (1 << 30),  /* two SATA Host Controllers */
 
-	MV_COMMON_FLAGS		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | ATA_FLAG_PIO_POLLING,
+	MV_COMMON_FLAGS		= ATA_FLAG_SATA | ATA_FLAG_PIO_POLLING,
 
 	MV_GEN_I_FLAGS		= MV_COMMON_FLAGS | ATA_FLAG_NO_ATAPI,
 
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index 7254e255fd78..42344e3c686d 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -539,7 +539,7 @@ struct nv_pi_priv {
 static const struct ata_port_info nv_port_info[] = {
 	/* generic */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+		.flags		= ATA_FLAG_SATA,
 		.pio_mask	= NV_PIO_MASK,
 		.mwdma_mask	= NV_MWDMA_MASK,
 		.udma_mask	= NV_UDMA_MASK,
@@ -548,7 +548,7 @@ static const struct ata_port_info nv_port_info[] = {
 	},
 	/* nforce2/3 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+		.flags		= ATA_FLAG_SATA,
 		.pio_mask	= NV_PIO_MASK,
 		.mwdma_mask	= NV_MWDMA_MASK,
 		.udma_mask	= NV_UDMA_MASK,
@@ -557,7 +557,7 @@ static const struct ata_port_info nv_port_info[] = {
 	},
 	/* ck804 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+		.flags		= ATA_FLAG_SATA,
 		.pio_mask	= NV_PIO_MASK,
 		.mwdma_mask	= NV_MWDMA_MASK,
 		.udma_mask	= NV_UDMA_MASK,
@@ -566,8 +566,7 @@ static const struct ata_port_info nv_port_info[] = {
 	},
 	/* ADMA */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | ATA_FLAG_NCQ,
+		.flags		= ATA_FLAG_SATA | ATA_FLAG_NCQ,
 		.pio_mask	= NV_PIO_MASK,
 		.mwdma_mask	= NV_MWDMA_MASK,
 		.udma_mask	= NV_UDMA_MASK,
@@ -576,7 +575,7 @@ static const struct ata_port_info nv_port_info[] = {
 	},
 	/* MCP5x */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+		.flags		= ATA_FLAG_SATA,
 		.pio_mask	= NV_PIO_MASK,
 		.mwdma_mask	= NV_MWDMA_MASK,
 		.udma_mask	= NV_UDMA_MASK,
@@ -585,8 +584,7 @@ static const struct ata_port_info nv_port_info[] = {
 	},
 	/* SWNCQ */
 	{
-		.flags	        = ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_NCQ,
+		.flags	        = ATA_FLAG_SATA | ATA_FLAG_NCQ,
 		.pio_mask	= NV_PIO_MASK,
 		.mwdma_mask	= NV_MWDMA_MASK,
 		.udma_mask	= NV_UDMA_MASK,
diff --git a/drivers/ata/sata_promise.c b/drivers/ata/sata_promise.c
index f03ad48273ff..a004b1e0ea6d 100644
--- a/drivers/ata/sata_promise.c
+++ b/drivers/ata/sata_promise.c
@@ -134,9 +134,7 @@ enum {
 	PDC_IRQ_DISABLE		= (1 << 10),
 	PDC_RESET		= (1 << 11), /* HDMA reset */
 
-	PDC_COMMON_FLAGS	= ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO |
-				  ATA_FLAG_PIO_POLLING,
+	PDC_COMMON_FLAGS	= ATA_FLAG_PIO_POLLING,
 
 	/* ap->flags bits */
 	PDC_FLAG_GEN_II		= (1 << 24),
diff --git a/drivers/ata/sata_qstor.c b/drivers/ata/sata_qstor.c
index daeebf19a6a9..c5603265fa58 100644
--- a/drivers/ata/sata_qstor.c
+++ b/drivers/ata/sata_qstor.c
@@ -155,8 +155,7 @@ static struct ata_port_operations qs_ata_ops = {
 static const struct ata_port_info qs_port_info[] = {
 	/* board_2068_idx */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | ATA_FLAG_PIO_POLLING,
+		.flags		= ATA_FLAG_SATA | ATA_FLAG_PIO_POLLING,
 		.pio_mask	= ATA_PIO4_ONLY,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &qs_ata_ops,
diff --git a/drivers/ata/sata_sil.c b/drivers/ata/sata_sil.c
index 3a4f84219719..b42edaaf3a53 100644
--- a/drivers/ata/sata_sil.c
+++ b/drivers/ata/sata_sil.c
@@ -61,8 +61,7 @@ enum {
 	SIL_FLAG_RERR_ON_DMA_ACT = (1 << 29),
 	SIL_FLAG_MOD15WRITE	= (1 << 30),
 
-	SIL_DFL_PORT_FLAGS	= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO,
+	SIL_DFL_PORT_FLAGS	= ATA_FLAG_SATA,
 
 	/*
 	 * Controller IDs
diff --git a/drivers/ata/sata_sil24.c b/drivers/ata/sata_sil24.c
index af41c6fd1254..06c564e55051 100644
--- a/drivers/ata/sata_sil24.c
+++ b/drivers/ata/sata_sil24.c
@@ -244,8 +244,7 @@ enum {
 	BID_SIL3131		= 2,
 
 	/* host flags */
-	SIL24_COMMON_FLAGS	= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA |
+	SIL24_COMMON_FLAGS	= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA |
 				  ATA_FLAG_NCQ | ATA_FLAG_ACPI_SATA |
 				  ATA_FLAG_AN | ATA_FLAG_PMP,
 	SIL24_FLAG_PCIX_IRQ_WOC	= (1 << 24), /* IRQ loss errata on PCI-X */
diff --git a/drivers/ata/sata_sis.c b/drivers/ata/sata_sis.c
index 2bfe3ae03976..cdcc13e9cf51 100644
--- a/drivers/ata/sata_sis.c
+++ b/drivers/ata/sata_sis.c
@@ -96,7 +96,7 @@ static struct ata_port_operations sis_ops = {
 };
 
 static const struct ata_port_info sis_port_info = {
-	.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+	.flags		= ATA_FLAG_SATA,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
 	.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/sata_svw.c b/drivers/ata/sata_svw.c
index 7d9db4aaf07e..35eabcf34568 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -359,8 +359,7 @@ static struct ata_port_operations k2_sata_ops = {
 static const struct ata_port_info k2_port_info[] = {
 	/* chip_svw4 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | K2_FLAG_NO_ATAPI_DMA,
+		.flags		= ATA_FLAG_SATA | K2_FLAG_NO_ATAPI_DMA,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA6,
@@ -368,8 +367,7 @@ static const struct ata_port_info k2_port_info[] = {
 	},
 	/* chip_svw8 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | K2_FLAG_NO_ATAPI_DMA |
+		.flags		= ATA_FLAG_SATA | K2_FLAG_NO_ATAPI_DMA |
 				  K2_FLAG_SATA_8_PORTS,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
@@ -378,8 +376,7 @@ static const struct ata_port_info k2_port_info[] = {
 	},
 	/* chip_svw42 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO | K2_FLAG_BAR_POS_3,
+		.flags		= ATA_FLAG_SATA | K2_FLAG_BAR_POS_3,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA6,
@@ -387,8 +384,7 @@ static const struct ata_port_info k2_port_info[] = {
 	},
 	/* chip_svw43 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO,
+		.flags		= ATA_FLAG_SATA,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/sata_sx4.c b/drivers/ata/sata_sx4.c
index bedd5188e5b0..8fd3b7252bda 100644
--- a/drivers/ata/sata_sx4.c
+++ b/drivers/ata/sata_sx4.c
@@ -273,9 +273,8 @@ static struct ata_port_operations pdc_20621_ops = {
 static const struct ata_port_info pdc_port_info[] = {
 	/* board_20621 */
 	{
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_SRST | ATA_FLAG_MMIO |
-				  ATA_FLAG_NO_ATAPI | ATA_FLAG_PIO_POLLING,
+		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_ATAPI |
+				  ATA_FLAG_PIO_POLLING,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/sata_uli.c b/drivers/ata/sata_uli.c
index b8578c32d344..235be717a713 100644
--- a/drivers/ata/sata_uli.c
+++ b/drivers/ata/sata_uli.c
@@ -88,8 +88,7 @@ static struct ata_port_operations uli_ops = {
 };
 
 static const struct ata_port_info uli_port_info = {
-	.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-			  ATA_FLAG_IGN_SIMPLEX,
+	.flags		= ATA_FLAG_SATA | ATA_FLAG_IGN_SIMPLEX,
 	.pio_mask       = ATA_PIO4,
 	.udma_mask      = ATA_UDMA6,
 	.port_ops       = &uli_ops,
diff --git a/drivers/ata/sata_via.c b/drivers/ata/sata_via.c
index 8b677bbf2d37..21242c5709a0 100644
--- a/drivers/ata/sata_via.c
+++ b/drivers/ata/sata_via.c
@@ -148,7 +148,7 @@ static struct ata_port_operations vt8251_ops = {
 };
 
 static const struct ata_port_info vt6420_port_info = {
-	.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+	.flags		= ATA_FLAG_SATA,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
 	.udma_mask	= ATA_UDMA6,
@@ -156,7 +156,7 @@ static const struct ata_port_info vt6420_port_info = {
 };
 
 static struct ata_port_info vt6421_sport_info = {
-	.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY,
+	.flags		= ATA_FLAG_SATA,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
 	.udma_mask	= ATA_UDMA6,
@@ -164,7 +164,7 @@ static struct ata_port_info vt6421_sport_info = {
 };
 
 static struct ata_port_info vt6421_pport_info = {
-	.flags		= ATA_FLAG_SLAVE_POSS | ATA_FLAG_NO_LEGACY,
+	.flags		= ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
 	/* No MWDMA */
 	.udma_mask	= ATA_UDMA6,
@@ -172,8 +172,7 @@ static struct ata_port_info vt6421_pport_info = {
 };
 
 static struct ata_port_info vt8251_port_info = {
-	.flags		= ATA_FLAG_SATA | ATA_FLAG_SLAVE_POSS |
-			  ATA_FLAG_NO_LEGACY,
+	.flags		= ATA_FLAG_SATA | ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
 	.udma_mask	= ATA_UDMA6,
diff --git a/drivers/ata/sata_vsc.c b/drivers/ata/sata_vsc.c
index e079cf29ed5d..7c987371136e 100644
--- a/drivers/ata/sata_vsc.c
+++ b/drivers/ata/sata_vsc.c
@@ -340,8 +340,7 @@ static int __devinit vsc_sata_init_one(struct pci_dev *pdev,
 				       const struct pci_device_id *ent)
 {
 	static const struct ata_port_info pi = {
-		.flags		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-				  ATA_FLAG_MMIO,
+		.flags		= ATA_FLAG_SATA,
 		.pio_mask	= ATA_PIO4,
 		.mwdma_mask	= ATA_MWDMA2,
 		.udma_mask	= ATA_UDMA6,
diff --git a/drivers/atm/firestream.c b/drivers/atm/firestream.c
index 7d912baf01d4..049650d42c88 100644
--- a/drivers/atm/firestream.c
+++ b/drivers/atm/firestream.c
@@ -1031,7 +1031,7 @@ static int fs_open(struct atm_vcc *atm_vcc)
 		/* We now use the "submit_command" function to submit commands to
 		   the firestream. There is a define up near the definition of
 		   that routine that switches this routine between immediate write
-		   to the immediate comamnd registers and queuing the commands in
+		   to the immediate command registers and queuing the commands in
 		   the HPTXQ for execution. This last technique might be more
 		   efficient if we know we're going to submit a whole lot of
 		   commands in one go, but this driver is not setup to be able to
diff --git a/drivers/atm/fore200e.c b/drivers/atm/fore200e.c
index 44f778507770..bdd2719f3f68 100644
--- a/drivers/atm/fore200e.c
+++ b/drivers/atm/fore200e.c
@@ -2643,14 +2643,17 @@ fore200e_init(struct fore200e* fore200e, struct device *parent)
 }
 
 #ifdef CONFIG_SBUS
-static int __devinit fore200e_sba_probe(struct platform_device *op,
-					const struct of_device_id *match)
+static int __devinit fore200e_sba_probe(struct platform_device *op)
 {
-	const struct fore200e_bus *bus = match->data;
+	const struct fore200e_bus *bus;
 	struct fore200e *fore200e;
 	static int index = 0;
 	int err;
 
+	if (!op->dev.of_match)
+		return -EINVAL;
+	bus = op->dev.of_match->data;
+
 	fore200e = kzalloc(sizeof(struct fore200e), GFP_KERNEL);
 	if (!fore200e)
 		return -ENOMEM;
@@ -2694,7 +2697,7 @@ static const struct of_device_id fore200e_sba_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fore200e_sba_match);
 
-static struct of_platform_driver fore200e_sba_driver = {
+static struct platform_driver fore200e_sba_driver = {
 	.driver = {
 		.name = "fore_200e",
 		.owner = THIS_MODULE,
@@ -2795,7 +2798,7 @@ static int __init fore200e_module_init(void)
 	printk(FORE200E "FORE Systems 200E-series ATM driver - version " FORE200E_VERSION "\n");
 
 #ifdef CONFIG_SBUS
-	err = of_register_platform_driver(&fore200e_sba_driver);
+	err = platform_driver_register(&fore200e_sba_driver);
 	if (err)
 		return err;
 #endif
@@ -2806,7 +2809,7 @@ static int __init fore200e_module_init(void)
 
 #ifdef CONFIG_SBUS
 	if (err)
-		of_unregister_platform_driver(&fore200e_sba_driver);
+		platform_driver_unregister(&fore200e_sba_driver);
 #endif
 
 	return err;
@@ -2818,7 +2821,7 @@ static void __exit fore200e_module_cleanup(void)
 	pci_unregister_driver(&fore200e_pca_driver);
 #endif
 #ifdef CONFIG_SBUS
-	of_unregister_platform_driver(&fore200e_sba_driver);
+	platform_driver_unregister(&fore200e_sba_driver);
 #endif
 }
 
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 080e9ca11017..81b78ede37c4 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -1320,7 +1320,10 @@ struct root_device
 	struct module *owner;
 };
 
-#define to_root_device(dev) container_of(dev, struct root_device, dev)
+inline struct root_device *to_root_device(struct device *d)
+{
+	return container_of(d, struct root_device, dev);
+}
 
 static void root_device_release(struct device *dev)
 {
@@ -1551,7 +1554,34 @@ EXPORT_SYMBOL_GPL(device_destroy);
  * on the same device to ensure that new_name is valid and
  * won't conflict with other devices.
  *
- * "Never use this function, bad things will happen" - gregkh
+ * Note: Don't call this function.  Currently, the networking layer calls this
+ * function, but that will change.  The following text from Kay Sievers offers
+ * some insight:
+ *
+ * Renaming devices is racy at many levels, symlinks and other stuff are not
+ * replaced atomically, and you get a "move" uevent, but it's not easy to
+ * connect the event to the old and new device. Device nodes are not renamed at
+ * all, there isn't even support for that in the kernel now.
+ *
+ * In the meantime, during renaming, your target name might be taken by another
+ * driver, creating conflicts. Or the old name is taken directly after you
+ * renamed it -- then you get events for the same DEVPATH, before you even see
+ * the "move" event. It's just a mess, and nothing new should ever rely on
+ * kernel device renaming. Besides that, it's not even implemented now for
+ * other things than (driver-core wise very simple) network devices.
+ *
+ * We are currently about to change network renaming in udev to completely
+ * disallow renaming of devices in the same namespace as the kernel uses,
+ * because we can't solve the problems properly, that arise with swapping names
+ * of multiple interfaces without races. Means, renaming of eth[0-9]* will only
+ * be allowed to some other name than eth[0-9]*, for the aforementioned
+ * reasons.
+ *
+ * Make up a "real" name in the driver before you register anything, or add
+ * some other attributes for userspace to find the device, or use udev to add
+ * symlinks -- but never rename kernel devices later, it's a complete mess. We
+ * don't even want to get into that and try to implement the missing pieces in
+ * the core. We really have other pieces to fix in the driver core mess. :)
  */
 int device_rename(struct device *dev, const char *new_name)
 {
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 40af43ebd92d..8c798ef7f13f 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -593,8 +593,7 @@ int
 request_firmware(const struct firmware **firmware_p, const char *name,
                  struct device *device)
 {
-        int uevent = 1;
-        return _request_firmware(firmware_p, name, device, uevent, false);
+        return _request_firmware(firmware_p, name, device, true, false);
 }
 
 /**
@@ -618,7 +617,7 @@ struct firmware_work {
 	struct device *device;
 	void *context;
 	void (*cont)(const struct firmware *fw, void *context);
-	int uevent;
+	bool uevent;
 };
 
 static int request_firmware_work_func(void *arg)
@@ -661,7 +660,7 @@ static int request_firmware_work_func(void *arg)
  **/
 int
 request_firmware_nowait(
-	struct module *module, int uevent,
+	struct module *module, bool uevent,
 	const char *name, struct device *device, gfp_t gfp, void *context,
 	void (*cont)(const struct firmware *fw, void *context))
 {
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index cafeaaf0428f..3da6a43b7756 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -30,6 +30,14 @@
 static DEFINE_MUTEX(mem_sysfs_mutex);
 
 #define MEMORY_CLASS_NAME	"memory"
+#define MIN_MEMORY_BLOCK_SIZE	(1 << SECTION_SIZE_BITS)
+
+static int sections_per_block;
+
+static inline int base_memory_block_id(int section_nr)
+{
+	return section_nr / sections_per_block;
+}
 
 static struct sysdev_class memory_sysdev_class = {
 	.name = MEMORY_CLASS_NAME,
@@ -84,39 +92,72 @@ EXPORT_SYMBOL(unregister_memory_isolate_notifier);
  * register_memory - Setup a sysfs device for a memory block
  */
 static
-int register_memory(struct memory_block *memory, struct mem_section *section)
+int register_memory(struct memory_block *memory)
 {
 	int error;
 
 	memory->sysdev.cls = &memory_sysdev_class;
-	memory->sysdev.id = __section_nr(section);
+	memory->sysdev.id = memory->start_section_nr / sections_per_block;
 
 	error = sysdev_register(&memory->sysdev);
 	return error;
 }
 
 static void
-unregister_memory(struct memory_block *memory, struct mem_section *section)
+unregister_memory(struct memory_block *memory)
 {
 	BUG_ON(memory->sysdev.cls != &memory_sysdev_class);
-	BUG_ON(memory->sysdev.id != __section_nr(section));
 
 	/* drop the ref. we got in remove_memory_block() */
 	kobject_put(&memory->sysdev.kobj);
 	sysdev_unregister(&memory->sysdev);
 }
 
+unsigned long __weak memory_block_size_bytes(void)
+{
+	return MIN_MEMORY_BLOCK_SIZE;
+}
+
+static unsigned long get_memory_block_size(void)
+{
+	unsigned long block_sz;
+
+	block_sz = memory_block_size_bytes();
+
+	/* Validate blk_sz is a power of 2 and not less than section size */
+	if ((block_sz & (block_sz - 1)) || (block_sz < MIN_MEMORY_BLOCK_SIZE)) {
+		WARN_ON(1);
+		block_sz = MIN_MEMORY_BLOCK_SIZE;
+	}
+
+	return block_sz;
+}
+
 /*
  * use this as the physical section index that this memsection
  * uses.
  */
 
-static ssize_t show_mem_phys_index(struct sys_device *dev,
+static ssize_t show_mem_start_phys_index(struct sys_device *dev,
 			struct sysdev_attribute *attr, char *buf)
 {
 	struct memory_block *mem =
 		container_of(dev, struct memory_block, sysdev);
-	return sprintf(buf, "%08lx\n", mem->phys_index);
+	unsigned long phys_index;
+
+	phys_index = mem->start_section_nr / sections_per_block;
+	return sprintf(buf, "%08lx\n", phys_index);
+}
+
+static ssize_t show_mem_end_phys_index(struct sys_device *dev,
+			struct sysdev_attribute *attr, char *buf)
+{
+	struct memory_block *mem =
+		container_of(dev, struct memory_block, sysdev);
+	unsigned long phys_index;
+
+	phys_index = mem->end_section_nr / sections_per_block;
+	return sprintf(buf, "%08lx\n", phys_index);
 }
 
 /*
@@ -125,13 +166,16 @@ static ssize_t show_mem_phys_index(struct sys_device *dev,
 static ssize_t show_mem_removable(struct sys_device *dev,
 			struct sysdev_attribute *attr, char *buf)
 {
-	unsigned long start_pfn;
-	int ret;
+	unsigned long i, pfn;
+	int ret = 1;
 	struct memory_block *mem =
 		container_of(dev, struct memory_block, sysdev);
 
-	start_pfn = section_nr_to_pfn(mem->phys_index);
-	ret = is_mem_section_removable(start_pfn, PAGES_PER_SECTION);
+	for (i = 0; i < sections_per_block; i++) {
+		pfn = section_nr_to_pfn(mem->start_section_nr + i);
+		ret &= is_mem_section_removable(pfn, PAGES_PER_SECTION);
+	}
+
 	return sprintf(buf, "%d\n", ret);
 }
 
@@ -184,17 +228,14 @@ int memory_isolate_notify(unsigned long val, void *v)
  * OK to have direct references to sparsemem variables in here.
  */
 static int
-memory_block_action(struct memory_block *mem, unsigned long action)
+memory_section_action(unsigned long phys_index, unsigned long action)
 {
 	int i;
-	unsigned long psection;
 	unsigned long start_pfn, start_paddr;
 	struct page *first_page;
 	int ret;
-	int old_state = mem->state;
 
-	psection = mem->phys_index;
-	first_page = pfn_to_page(psection << PFN_SECTION_SHIFT);
+	first_page = pfn_to_page(phys_index << PFN_SECTION_SHIFT);
 
 	/*
 	 * The probe routines leave the pages reserved, just
@@ -207,8 +248,8 @@ memory_block_action(struct memory_block *mem, unsigned long action)
 				continue;
 
 			printk(KERN_WARNING "section number %ld page number %d "
-				"not reserved, was it already online? \n",
-				psection, i);
+				"not reserved, was it already online?\n",
+				phys_index, i);
 			return -EBUSY;
 		}
 	}
@@ -219,18 +260,13 @@ memory_block_action(struct memory_block *mem, unsigned long action)
 			ret = online_pages(start_pfn, PAGES_PER_SECTION);
 			break;
 		case MEM_OFFLINE:
-			mem->state = MEM_GOING_OFFLINE;
 			start_paddr = page_to_pfn(first_page) << PAGE_SHIFT;
 			ret = remove_memory(start_paddr,
 					    PAGES_PER_SECTION << PAGE_SHIFT);
-			if (ret) {
-				mem->state = old_state;
-				break;
-			}
 			break;
 		default:
-			WARN(1, KERN_WARNING "%s(%p, %ld) unknown action: %ld\n",
-					__func__, mem, action, action);
+			WARN(1, KERN_WARNING "%s(%ld, %ld) unknown action: "
+			     "%ld\n", __func__, phys_index, action, action);
 			ret = -EINVAL;
 	}
 
@@ -240,7 +276,8 @@ memory_block_action(struct memory_block *mem, unsigned long action)
 static int memory_block_change_state(struct memory_block *mem,
 		unsigned long to_state, unsigned long from_state_req)
 {
-	int ret = 0;
+	int i, ret = 0;
+
 	mutex_lock(&mem->state_mutex);
 
 	if (mem->state != from_state_req) {
@@ -248,8 +285,23 @@ static int memory_block_change_state(struct memory_block *mem,
 		goto out;
 	}
 
-	ret = memory_block_action(mem, to_state);
-	if (!ret)
+	if (to_state == MEM_OFFLINE)
+		mem->state = MEM_GOING_OFFLINE;
+
+	for (i = 0; i < sections_per_block; i++) {
+		ret = memory_section_action(mem->start_section_nr + i,
+					    to_state);
+		if (ret)
+			break;
+	}
+
+	if (ret) {
+		for (i = 0; i < sections_per_block; i++)
+			memory_section_action(mem->start_section_nr + i,
+					      from_state_req);
+
+		mem->state = from_state_req;
+	} else
 		mem->state = to_state;
 
 out:
@@ -262,20 +314,15 @@ store_mem_state(struct sys_device *dev,
 		struct sysdev_attribute *attr, const char *buf, size_t count)
 {
 	struct memory_block *mem;
-	unsigned int phys_section_nr;
 	int ret = -EINVAL;
 
 	mem = container_of(dev, struct memory_block, sysdev);
-	phys_section_nr = mem->phys_index;
-
-	if (!present_section_nr(phys_section_nr))
-		goto out;
 
 	if (!strncmp(buf, "online", min((int)count, 6)))
 		ret = memory_block_change_state(mem, MEM_ONLINE, MEM_OFFLINE);
 	else if(!strncmp(buf, "offline", min((int)count, 7)))
 		ret = memory_block_change_state(mem, MEM_OFFLINE, MEM_ONLINE);
-out:
+
 	if (ret)
 		return ret;
 	return count;
@@ -298,7 +345,8 @@ static ssize_t show_phys_device(struct sys_device *dev,
 	return sprintf(buf, "%d\n", mem->phys_device);
 }
 
-static SYSDEV_ATTR(phys_index, 0444, show_mem_phys_index, NULL);
+static SYSDEV_ATTR(phys_index, 0444, show_mem_start_phys_index, NULL);
+static SYSDEV_ATTR(end_phys_index, 0444, show_mem_end_phys_index, NULL);
 static SYSDEV_ATTR(state, 0644, show_mem_state, store_mem_state);
 static SYSDEV_ATTR(phys_device, 0444, show_phys_device, NULL);
 static SYSDEV_ATTR(removable, 0444, show_mem_removable, NULL);
@@ -315,7 +363,7 @@ static ssize_t
 print_block_size(struct sysdev_class *class, struct sysdev_class_attribute *attr,
 		 char *buf)
 {
-	return sprintf(buf, "%lx\n", (unsigned long)PAGES_PER_SECTION * PAGE_SIZE);
+	return sprintf(buf, "%lx\n", get_memory_block_size());
 }
 
 static SYSDEV_CLASS_ATTR(block_size_bytes, 0444, print_block_size, NULL);
@@ -339,12 +387,19 @@ memory_probe_store(struct class *class, struct class_attribute *attr,
 {
 	u64 phys_addr;
 	int nid;
-	int ret;
+	int i, ret;
 
 	phys_addr = simple_strtoull(buf, NULL, 0);
 
-	nid = memory_add_physaddr_to_nid(phys_addr);
-	ret = add_memory(nid, phys_addr, PAGES_PER_SECTION << PAGE_SHIFT);
+	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)
+			break;
+
+		phys_addr += MIN_MEMORY_BLOCK_SIZE;
+	}
 
 	if (ret)
 		count = ret;
@@ -444,6 +499,7 @@ struct memory_block *find_memory_block_hinted(struct mem_section *section,
 	struct sys_device *sysdev;
 	struct memory_block *mem;
 	char name[sizeof(MEMORY_CLASS_NAME) + 9 + 1];
+	int block_id = base_memory_block_id(__section_nr(section));
 
 	kobj = hint ? &hint->sysdev.kobj : NULL;
 
@@ -451,7 +507,7 @@ struct memory_block *find_memory_block_hinted(struct mem_section *section,
 	 * This only works because we know that section == sysdev->id
 	 * slightly redundant with sysdev_register()
 	 */
-	sprintf(&name[0], "%s%d", MEMORY_CLASS_NAME, __section_nr(section));
+	sprintf(&name[0], "%s%d", MEMORY_CLASS_NAME, block_id);
 
 	kobj = kset_find_obj_hinted(&memory_sysdev_class.kset, name, kobj);
 	if (!kobj)
@@ -476,36 +532,62 @@ struct memory_block *find_memory_block(struct mem_section *section)
 	return find_memory_block_hinted(section, NULL);
 }
 
-static int add_memory_block(int nid, struct mem_section *section,
-			unsigned long state, enum mem_add_context context)
+static int init_memory_block(struct memory_block **memory,
+			     struct mem_section *section, unsigned long state)
 {
-	struct memory_block *mem = kzalloc(sizeof(*mem), GFP_KERNEL);
+	struct memory_block *mem;
 	unsigned long start_pfn;
+	int scn_nr;
 	int ret = 0;
 
+	mem = kzalloc(sizeof(*mem), GFP_KERNEL);
 	if (!mem)
 		return -ENOMEM;
 
-	mutex_lock(&mem_sysfs_mutex);
-
-	mem->phys_index = __section_nr(section);
+	scn_nr = __section_nr(section);
+	mem->start_section_nr =
+			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++;
 	mutex_init(&mem->state_mutex);
-	start_pfn = section_nr_to_pfn(mem->phys_index);
+	start_pfn = section_nr_to_pfn(mem->start_section_nr);
 	mem->phys_device = arch_get_memory_phys_device(start_pfn);
 
-	ret = register_memory(mem, section);
+	ret = register_memory(mem);
 	if (!ret)
 		ret = mem_create_simple_file(mem, phys_index);
 	if (!ret)
+		ret = mem_create_simple_file(mem, end_phys_index);
+	if (!ret)
 		ret = mem_create_simple_file(mem, state);
 	if (!ret)
 		ret = mem_create_simple_file(mem, phys_device);
 	if (!ret)
 		ret = mem_create_simple_file(mem, removable);
+
+	*memory = mem;
+	return ret;
+}
+
+static int add_memory_section(int nid, struct mem_section *section,
+			unsigned long state, enum mem_add_context context)
+{
+	struct memory_block *mem;
+	int ret = 0;
+
+	mutex_lock(&mem_sysfs_mutex);
+
+	mem = find_memory_block(section);
+	if (mem) {
+		mem->section_count++;
+		kobject_put(&mem->sysdev.kobj);
+	} else
+		ret = init_memory_block(&mem, section, state);
+
 	if (!ret) {
-		if (context == HOTPLUG)
+		if (context == HOTPLUG &&
+		    mem->section_count == sections_per_block)
 			ret = register_mem_sect_under_node(mem, nid);
 	}
 
@@ -520,16 +602,19 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
 
 	mutex_lock(&mem_sysfs_mutex);
 	mem = find_memory_block(section);
+	unregister_mem_sect_under_nodes(mem, __section_nr(section));
 
 	mem->section_count--;
 	if (mem->section_count == 0) {
-		unregister_mem_sect_under_nodes(mem);
 		mem_remove_simple_file(mem, phys_index);
+		mem_remove_simple_file(mem, end_phys_index);
 		mem_remove_simple_file(mem, state);
 		mem_remove_simple_file(mem, phys_device);
 		mem_remove_simple_file(mem, removable);
-		unregister_memory(mem, section);
-	}
+		unregister_memory(mem);
+		kfree(mem);
+	} else
+		kobject_put(&mem->sysdev.kobj);
 
 	mutex_unlock(&mem_sysfs_mutex);
 	return 0;
@@ -541,7 +626,7 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
  */
 int register_new_memory(int nid, struct mem_section *section)
 {
-	return add_memory_block(nid, section, MEM_OFFLINE, HOTPLUG);
+	return add_memory_section(nid, section, MEM_OFFLINE, HOTPLUG);
 }
 
 int unregister_memory_section(struct mem_section *section)
@@ -560,12 +645,16 @@ int __init memory_dev_init(void)
 	unsigned int i;
 	int ret;
 	int err;
+	unsigned long block_sz;
 
 	memory_sysdev_class.kset.uevent_ops = &memory_uevent_ops;
 	ret = sysdev_class_register(&memory_sysdev_class);
 	if (ret)
 		goto out;
 
+	block_sz = get_memory_block_size();
+	sections_per_block = block_sz / MIN_MEMORY_BLOCK_SIZE;
+
 	/*
 	 * Create entries for memory sections that were found
 	 * during boot and have been initialized
@@ -573,8 +662,8 @@ int __init memory_dev_init(void)
 	for (i = 0; i < NR_MEM_SECTIONS; i++) {
 		if (!present_section_nr(i))
 			continue;
-		err = add_memory_block(0, __nr_to_section(i), MEM_ONLINE,
-				       BOOT);
+		err = add_memory_section(0, __nr_to_section(i), MEM_ONLINE,
+					 BOOT);
 		if (!ret)
 			ret = err;
 	}
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 36b43052001d..b3b72d64e805 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -375,8 +375,10 @@ int register_mem_sect_under_node(struct memory_block *mem_blk, int nid)
 		return -EFAULT;
 	if (!node_online(nid))
 		return 0;
-	sect_start_pfn = section_nr_to_pfn(mem_blk->phys_index);
-	sect_end_pfn = sect_start_pfn + PAGES_PER_SECTION - 1;
+
+	sect_start_pfn = section_nr_to_pfn(mem_blk->start_section_nr);
+	sect_end_pfn = section_nr_to_pfn(mem_blk->end_section_nr);
+	sect_end_pfn += PAGES_PER_SECTION - 1;
 	for (pfn = sect_start_pfn; pfn <= sect_end_pfn; pfn++) {
 		int page_nid;
 
@@ -400,7 +402,8 @@ int register_mem_sect_under_node(struct memory_block *mem_blk, int nid)
 }
 
 /* unregister memory section under all nodes that it spans */
-int unregister_mem_sect_under_nodes(struct memory_block *mem_blk)
+int unregister_mem_sect_under_nodes(struct memory_block *mem_blk,
+				    unsigned long phys_index)
 {
 	NODEMASK_ALLOC(nodemask_t, unlinked_nodes, GFP_KERNEL);
 	unsigned long pfn, sect_start_pfn, sect_end_pfn;
@@ -412,7 +415,8 @@ int unregister_mem_sect_under_nodes(struct memory_block *mem_blk)
 	if (!unlinked_nodes)
 		return -ENOMEM;
 	nodes_clear(*unlinked_nodes);
-	sect_start_pfn = section_nr_to_pfn(mem_blk->phys_index);
+
+	sect_start_pfn = section_nr_to_pfn(phys_index);
 	sect_end_pfn = sect_start_pfn + PAGES_PER_SECTION - 1;
 	for (pfn = sect_start_pfn; pfn <= sect_end_pfn; pfn++) {
 		int nid;
diff --git a/drivers/base/power/Makefile b/drivers/base/power/Makefile
index abe46edfe5b4..118c1b92a511 100644
--- a/drivers/base/power/Makefile
+++ b/drivers/base/power/Makefile
@@ -1,7 +1,6 @@
-obj-$(CONFIG_PM)	+= sysfs.o
+obj-$(CONFIG_PM)	+= sysfs.o generic_ops.o
 obj-$(CONFIG_PM_SLEEP)	+= main.o wakeup.o
 obj-$(CONFIG_PM_RUNTIME)	+= runtime.o
-obj-$(CONFIG_PM_OPS)	+= generic_ops.o
 obj-$(CONFIG_PM_TRACE_RTC)	+= trace.o
 obj-$(CONFIG_PM_OPP)	+= opp.o
 
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 83404973f97a..052dc53eef38 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -423,26 +423,22 @@ static int device_resume_noirq(struct device *dev, pm_message_t state)
 	TRACE_DEVICE(dev);
 	TRACE_RESUME(0);
 
-	if (dev->bus && dev->bus->pm) {
-		pm_dev_dbg(dev, state, "EARLY ");
-		error = pm_noirq_op(dev, dev->bus->pm, state);
-		if (error)
-			goto End;
+	if (dev->pwr_domain) {
+		pm_dev_dbg(dev, state, "EARLY power domain ");
+		pm_noirq_op(dev, &dev->pwr_domain->ops, state);
 	}
 
 	if (dev->type && dev->type->pm) {
 		pm_dev_dbg(dev, state, "EARLY type ");
 		error = pm_noirq_op(dev, dev->type->pm, state);
-		if (error)
-			goto End;
-	}
-
-	if (dev->class && dev->class->pm) {
+	} else if (dev->class && dev->class->pm) {
 		pm_dev_dbg(dev, state, "EARLY class ");
 		error = pm_noirq_op(dev, dev->class->pm, state);
+	} else if (dev->bus && dev->bus->pm) {
+		pm_dev_dbg(dev, state, "EARLY ");
+		error = pm_noirq_op(dev, dev->bus->pm, state);
 	}
 
-End:
 	TRACE_RESUME(error);
 	return error;
 }
@@ -518,36 +514,39 @@ static int device_resume(struct device *dev, pm_message_t state, bool async)
 
 	dev->power.in_suspend = false;
 
-	if (dev->bus) {
-		if (dev->bus->pm) {
-			pm_dev_dbg(dev, state, "");
-			error = pm_op(dev, dev->bus->pm, state);
-		} else if (dev->bus->resume) {
-			pm_dev_dbg(dev, state, "legacy ");
-			error = legacy_resume(dev, dev->bus->resume);
-		}
-		if (error)
-			goto End;
+	if (dev->pwr_domain) {
+		pm_dev_dbg(dev, state, "power domain ");
+		pm_op(dev, &dev->pwr_domain->ops, state);
 	}
 
-	if (dev->type) {
-		if (dev->type->pm) {
-			pm_dev_dbg(dev, state, "type ");
-			error = pm_op(dev, dev->type->pm, state);
-		}
-		if (error)
-			goto End;
+	if (dev->type && dev->type->pm) {
+		pm_dev_dbg(dev, state, "type ");
+		error = pm_op(dev, dev->type->pm, state);
+		goto End;
 	}
 
 	if (dev->class) {
 		if (dev->class->pm) {
 			pm_dev_dbg(dev, state, "class ");
 			error = pm_op(dev, dev->class->pm, state);
+			goto End;
 		} else if (dev->class->resume) {
 			pm_dev_dbg(dev, state, "legacy class ");
 			error = legacy_resume(dev, dev->class->resume);
+			goto End;
 		}
 	}
+
+	if (dev->bus) {
+		if (dev->bus->pm) {
+			pm_dev_dbg(dev, state, "");
+			error = pm_op(dev, dev->bus->pm, state);
+		} else if (dev->bus->resume) {
+			pm_dev_dbg(dev, state, "legacy ");
+			error = legacy_resume(dev, dev->bus->resume);
+		}
+	}
+
  End:
 	device_unlock(dev);
 	complete_all(&dev->power.completion);
@@ -629,19 +628,23 @@ static void device_complete(struct device *dev, pm_message_t state)
 {
 	device_lock(dev);
 
-	if (dev->class && dev->class->pm && dev->class->pm->complete) {
-		pm_dev_dbg(dev, state, "completing class ");
-		dev->class->pm->complete(dev);
+	if (dev->pwr_domain && dev->pwr_domain->ops.complete) {
+		pm_dev_dbg(dev, state, "completing power domain ");
+		dev->pwr_domain->ops.complete(dev);
 	}
 
-	if (dev->type && dev->type->pm && dev->type->pm->complete) {
+	if (dev->type && dev->type->pm) {
 		pm_dev_dbg(dev, state, "completing type ");
-		dev->type->pm->complete(dev);
-	}
-
-	if (dev->bus && dev->bus->pm && dev->bus->pm->complete) {
+		if (dev->type->pm->complete)
+			dev->type->pm->complete(dev);
+	} else if (dev->class && dev->class->pm) {
+		pm_dev_dbg(dev, state, "completing class ");
+		if (dev->class->pm->complete)
+			dev->class->pm->complete(dev);
+	} else if (dev->bus && dev->bus->pm) {
 		pm_dev_dbg(dev, state, "completing ");
-		dev->bus->pm->complete(dev);
+		if (dev->bus->pm->complete)
+			dev->bus->pm->complete(dev);
 	}
 
 	device_unlock(dev);
@@ -669,7 +672,6 @@ static void dpm_complete(pm_message_t state)
 		mutex_unlock(&dpm_list_mtx);
 
 		device_complete(dev, state);
-		pm_runtime_put_sync(dev);
 
 		mutex_lock(&dpm_list_mtx);
 		put_device(dev);
@@ -727,29 +729,31 @@ static pm_message_t resume_event(pm_message_t sleep_state)
  */
 static int device_suspend_noirq(struct device *dev, pm_message_t state)
 {
-	int error = 0;
-
-	if (dev->class && dev->class->pm) {
-		pm_dev_dbg(dev, state, "LATE class ");
-		error = pm_noirq_op(dev, dev->class->pm, state);
-		if (error)
-			goto End;
-	}
+	int error;
 
 	if (dev->type && dev->type->pm) {
 		pm_dev_dbg(dev, state, "LATE type ");
 		error = pm_noirq_op(dev, dev->type->pm, state);
 		if (error)
-			goto End;
-	}
-
-	if (dev->bus && dev->bus->pm) {
+			return error;
+	} else if (dev->class && dev->class->pm) {
+		pm_dev_dbg(dev, state, "LATE class ");
+		error = pm_noirq_op(dev, dev->class->pm, state);
+		if (error)
+			return error;
+	} else if (dev->bus && dev->bus->pm) {
 		pm_dev_dbg(dev, state, "LATE ");
 		error = pm_noirq_op(dev, dev->bus->pm, state);
+		if (error)
+			return error;
 	}
 
-End:
-	return error;
+	if (dev->pwr_domain) {
+		pm_dev_dbg(dev, state, "LATE power domain ");
+		pm_noirq_op(dev, &dev->pwr_domain->ops, state);
+	}
+
+	return 0;
 }
 
 /**
@@ -836,25 +840,22 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 		goto End;
 	}
 
+	if (dev->type && dev->type->pm) {
+		pm_dev_dbg(dev, state, "type ");
+		error = pm_op(dev, dev->type->pm, state);
+		goto Domain;
+	}
+
 	if (dev->class) {
 		if (dev->class->pm) {
 			pm_dev_dbg(dev, state, "class ");
 			error = pm_op(dev, dev->class->pm, state);
+			goto Domain;
 		} else if (dev->class->suspend) {
 			pm_dev_dbg(dev, state, "legacy class ");
 			error = legacy_suspend(dev, state, dev->class->suspend);
+			goto Domain;
 		}
-		if (error)
-			goto End;
-	}
-
-	if (dev->type) {
-		if (dev->type->pm) {
-			pm_dev_dbg(dev, state, "type ");
-			error = pm_op(dev, dev->type->pm, state);
-		}
-		if (error)
-			goto End;
 	}
 
 	if (dev->bus) {
@@ -867,6 +868,12 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 		}
 	}
 
+ Domain:
+	if (!error && dev->pwr_domain) {
+		pm_dev_dbg(dev, state, "power domain ");
+		pm_op(dev, &dev->pwr_domain->ops, state);
+	}
+
  End:
 	device_unlock(dev);
 	complete_all(&dev->power.completion);
@@ -957,27 +964,34 @@ static int device_prepare(struct device *dev, pm_message_t state)
 
 	device_lock(dev);
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->prepare) {
+	if (dev->type && dev->type->pm) {
+		pm_dev_dbg(dev, state, "preparing type ");
+		if (dev->type->pm->prepare)
+			error = dev->type->pm->prepare(dev);
+		suspend_report_result(dev->type->pm->prepare, error);
+		if (error)
+			goto End;
+	} else if (dev->class && dev->class->pm) {
+		pm_dev_dbg(dev, state, "preparing class ");
+		if (dev->class->pm->prepare)
+			error = dev->class->pm->prepare(dev);
+		suspend_report_result(dev->class->pm->prepare, error);
+		if (error)
+			goto End;
+	} else if (dev->bus && dev->bus->pm) {
 		pm_dev_dbg(dev, state, "preparing ");
-		error = dev->bus->pm->prepare(dev);
+		if (dev->bus->pm->prepare)
+			error = dev->bus->pm->prepare(dev);
 		suspend_report_result(dev->bus->pm->prepare, error);
 		if (error)
 			goto End;
 	}
 
-	if (dev->type && dev->type->pm && dev->type->pm->prepare) {
-		pm_dev_dbg(dev, state, "preparing type ");
-		error = dev->type->pm->prepare(dev);
-		suspend_report_result(dev->type->pm->prepare, error);
-		if (error)
-			goto End;
+	if (dev->pwr_domain && dev->pwr_domain->ops.prepare) {
+		pm_dev_dbg(dev, state, "preparing power domain ");
+		dev->pwr_domain->ops.prepare(dev);
 	}
 
-	if (dev->class && dev->class->pm && dev->class->pm->prepare) {
-		pm_dev_dbg(dev, state, "preparing class ");
-		error = dev->class->pm->prepare(dev);
-		suspend_report_result(dev->class->pm->prepare, error);
-	}
  End:
 	device_unlock(dev);
 
@@ -1005,12 +1019,9 @@ static int dpm_prepare(pm_message_t state)
 		if (pm_runtime_barrier(dev) && device_may_wakeup(dev))
 			pm_wakeup_event(dev, 0);
 
-		if (pm_wakeup_pending()) {
-			pm_runtime_put_sync(dev);
-			error = -EBUSY;
-		} else {
-			error = device_prepare(dev, state);
-		}
+		pm_runtime_put_sync(dev);
+		error = pm_wakeup_pending() ?
+				-EBUSY : device_prepare(dev, state);
 
 		mutex_lock(&dpm_list_mtx);
 		if (error) {
diff --git a/drivers/base/power/opp.c b/drivers/base/power/opp.c
index 2bb9b4cf59d7..56a6899f5e9e 100644
--- a/drivers/base/power/opp.c
+++ b/drivers/base/power/opp.c
@@ -222,7 +222,7 @@ int opp_get_opp_count(struct device *dev)
  * opp_find_freq_exact() - search for an exact frequency
  * @dev:		device for which we do this operation
  * @freq:		frequency to search for
- * @is_available:	true/false - match for available opp
+ * @available:		true/false - match for available opp
  *
  * Searches for exact match in the opp list and returns pointer to the matching
  * opp if found, else returns ERR_PTR in case of error and should be handled
diff --git a/drivers/base/power/power.h b/drivers/base/power/power.h
index 698dde742587..f2a25f18fde7 100644
--- a/drivers/base/power/power.h
+++ b/drivers/base/power/power.h
@@ -58,19 +58,18 @@ static inline void device_pm_move_last(struct device *dev) {}
  * sysfs.c
  */
 
-extern int dpm_sysfs_add(struct device *);
-extern void dpm_sysfs_remove(struct device *);
-extern void rpm_sysfs_remove(struct device *);
+extern int dpm_sysfs_add(struct device *dev);
+extern void dpm_sysfs_remove(struct device *dev);
+extern void rpm_sysfs_remove(struct device *dev);
+extern int wakeup_sysfs_add(struct device *dev);
+extern void wakeup_sysfs_remove(struct device *dev);
 
 #else /* CONFIG_PM */
 
-static inline int dpm_sysfs_add(struct device *dev)
-{
-	return 0;
-}
-
-static inline void dpm_sysfs_remove(struct device *dev)
-{
-}
+static inline int dpm_sysfs_add(struct device *dev) { return 0; }
+static inline void dpm_sysfs_remove(struct device *dev) {}
+static inline void rpm_sysfs_remove(struct device *dev) {}
+static inline int wakeup_sysfs_add(struct device *dev) { return 0; }
+static inline void wakeup_sysfs_remove(struct device *dev) {}
 
 #endif
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index 42615b419dfb..54597c859ecb 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -168,6 +168,7 @@ static int rpm_check_suspend_allowed(struct device *dev)
 static int rpm_idle(struct device *dev, int rpmflags)
 {
 	int (*callback)(struct device *);
+	int (*domain_callback)(struct device *);
 	int retval;
 
 	retval = rpm_check_suspend_allowed(dev);
@@ -213,19 +214,28 @@ static int rpm_idle(struct device *dev, int rpmflags)
 
 	dev->power.idle_notification = true;
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->runtime_idle)
-		callback = dev->bus->pm->runtime_idle;
-	else if (dev->type && dev->type->pm && dev->type->pm->runtime_idle)
+	if (dev->type && dev->type->pm)
 		callback = dev->type->pm->runtime_idle;
 	else if (dev->class && dev->class->pm)
 		callback = dev->class->pm->runtime_idle;
+	else if (dev->bus && dev->bus->pm)
+		callback = dev->bus->pm->runtime_idle;
 	else
 		callback = NULL;
 
-	if (callback) {
+	if (dev->pwr_domain)
+		domain_callback = dev->pwr_domain->ops.runtime_idle;
+	else
+		domain_callback = NULL;
+
+	if (callback || domain_callback) {
 		spin_unlock_irq(&dev->power.lock);
 
-		callback(dev);
+		if (domain_callback)
+			retval = domain_callback(dev);
+
+		if (!retval && callback)
+			callback(dev);
 
 		spin_lock_irq(&dev->power.lock);
 	}
@@ -372,12 +382,12 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 
 	__update_runtime_status(dev, RPM_SUSPENDING);
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->runtime_suspend)
-		callback = dev->bus->pm->runtime_suspend;
-	else if (dev->type && dev->type->pm && dev->type->pm->runtime_suspend)
+	if (dev->type && dev->type->pm)
 		callback = dev->type->pm->runtime_suspend;
 	else if (dev->class && dev->class->pm)
 		callback = dev->class->pm->runtime_suspend;
+	else if (dev->bus && dev->bus->pm)
+		callback = dev->bus->pm->runtime_suspend;
 	else
 		callback = NULL;
 
@@ -390,6 +400,8 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 		else
 			pm_runtime_cancel_pending(dev);
 	} else {
+		if (dev->pwr_domain)
+			rpm_callback(dev->pwr_domain->ops.runtime_suspend, dev);
  no_callback:
 		__update_runtime_status(dev, RPM_SUSPENDED);
 		pm_runtime_deactivate_timer(dev);
@@ -569,12 +581,15 @@ static int rpm_resume(struct device *dev, int rpmflags)
 
 	__update_runtime_status(dev, RPM_RESUMING);
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->runtime_resume)
-		callback = dev->bus->pm->runtime_resume;
-	else if (dev->type && dev->type->pm && dev->type->pm->runtime_resume)
+	if (dev->pwr_domain)
+		rpm_callback(dev->pwr_domain->ops.runtime_resume, dev);
+
+	if (dev->type && dev->type->pm)
 		callback = dev->type->pm->runtime_resume;
 	else if (dev->class && dev->class->pm)
 		callback = dev->class->pm->runtime_resume;
+	else if (dev->bus && dev->bus->pm)
+		callback = dev->bus->pm->runtime_resume;
 	else
 		callback = NULL;
 
diff --git a/drivers/base/power/sysfs.c b/drivers/base/power/sysfs.c
index 0b1e46bf3e56..fff49bee781d 100644
--- a/drivers/base/power/sysfs.c
+++ b/drivers/base/power/sysfs.c
@@ -431,26 +431,18 @@ static ssize_t async_store(struct device *dev, struct device_attribute *attr,
 static DEVICE_ATTR(async, 0644, async_show, async_store);
 #endif /* CONFIG_PM_ADVANCED_DEBUG */
 
-static struct attribute * power_attrs[] = {
-	&dev_attr_wakeup.attr,
-#ifdef CONFIG_PM_SLEEP
-	&dev_attr_wakeup_count.attr,
-	&dev_attr_wakeup_active_count.attr,
-	&dev_attr_wakeup_hit_count.attr,
-	&dev_attr_wakeup_active.attr,
-	&dev_attr_wakeup_total_time_ms.attr,
-	&dev_attr_wakeup_max_time_ms.attr,
-	&dev_attr_wakeup_last_time_ms.attr,
-#endif
+static struct attribute *power_attrs[] = {
 #ifdef CONFIG_PM_ADVANCED_DEBUG
+#ifdef CONFIG_PM_SLEEP
 	&dev_attr_async.attr,
+#endif
 #ifdef CONFIG_PM_RUNTIME
 	&dev_attr_runtime_status.attr,
 	&dev_attr_runtime_usage.attr,
 	&dev_attr_runtime_active_kids.attr,
 	&dev_attr_runtime_enabled.attr,
 #endif
-#endif
+#endif /* CONFIG_PM_ADVANCED_DEBUG */
 	NULL,
 };
 static struct attribute_group pm_attr_group = {
@@ -458,9 +450,26 @@ static struct attribute_group pm_attr_group = {
 	.attrs	= power_attrs,
 };
 
-#ifdef CONFIG_PM_RUNTIME
+static struct attribute *wakeup_attrs[] = {
+#ifdef CONFIG_PM_SLEEP
+	&dev_attr_wakeup.attr,
+	&dev_attr_wakeup_count.attr,
+	&dev_attr_wakeup_active_count.attr,
+	&dev_attr_wakeup_hit_count.attr,
+	&dev_attr_wakeup_active.attr,
+	&dev_attr_wakeup_total_time_ms.attr,
+	&dev_attr_wakeup_max_time_ms.attr,
+	&dev_attr_wakeup_last_time_ms.attr,
+#endif
+	NULL,
+};
+static struct attribute_group pm_wakeup_attr_group = {
+	.name	= power_group_name,
+	.attrs	= wakeup_attrs,
+};
 
 static struct attribute *runtime_attrs[] = {
+#ifdef CONFIG_PM_RUNTIME
 #ifndef CONFIG_PM_ADVANCED_DEBUG
 	&dev_attr_runtime_status.attr,
 #endif
@@ -468,6 +477,7 @@ static struct attribute *runtime_attrs[] = {
 	&dev_attr_runtime_suspended_time.attr,
 	&dev_attr_runtime_active_time.attr,
 	&dev_attr_autosuspend_delay_ms.attr,
+#endif /* CONFIG_PM_RUNTIME */
 	NULL,
 };
 static struct attribute_group pm_runtime_attr_group = {
@@ -480,35 +490,49 @@ int dpm_sysfs_add(struct device *dev)
 	int rc;
 
 	rc = sysfs_create_group(&dev->kobj, &pm_attr_group);
-	if (rc == 0 && !dev->power.no_callbacks) {
+	if (rc)
+		return rc;
+
+	if (pm_runtime_callbacks_present(dev)) {
 		rc = sysfs_merge_group(&dev->kobj, &pm_runtime_attr_group);
 		if (rc)
-			sysfs_remove_group(&dev->kobj, &pm_attr_group);
+			goto err_out;
+	}
+
+	if (device_can_wakeup(dev)) {
+		rc = sysfs_merge_group(&dev->kobj, &pm_wakeup_attr_group);
+		if (rc) {
+			if (pm_runtime_callbacks_present(dev))
+				sysfs_unmerge_group(&dev->kobj,
+						    &pm_runtime_attr_group);
+			goto err_out;
+		}
 	}
+	return 0;
+
+ err_out:
+	sysfs_remove_group(&dev->kobj, &pm_attr_group);
 	return rc;
 }
 
-void rpm_sysfs_remove(struct device *dev)
+int wakeup_sysfs_add(struct device *dev)
 {
-	sysfs_unmerge_group(&dev->kobj, &pm_runtime_attr_group);
+	return sysfs_merge_group(&dev->kobj, &pm_wakeup_attr_group);
 }
 
-void dpm_sysfs_remove(struct device *dev)
+void wakeup_sysfs_remove(struct device *dev)
 {
-	rpm_sysfs_remove(dev);
-	sysfs_remove_group(&dev->kobj, &pm_attr_group);
+	sysfs_unmerge_group(&dev->kobj, &pm_wakeup_attr_group);
 }
 
-#else /* CONFIG_PM_RUNTIME */
-
-int dpm_sysfs_add(struct device * dev)
+void rpm_sysfs_remove(struct device *dev)
 {
-	return sysfs_create_group(&dev->kobj, &pm_attr_group);
+	sysfs_unmerge_group(&dev->kobj, &pm_runtime_attr_group);
 }
 
-void dpm_sysfs_remove(struct device * dev)
+void dpm_sysfs_remove(struct device *dev)
 {
+	rpm_sysfs_remove(dev);
+	sysfs_unmerge_group(&dev->kobj, &pm_wakeup_attr_group);
 	sysfs_remove_group(&dev->kobj, &pm_attr_group);
 }
-
-#endif
diff --git a/drivers/base/power/trace.c b/drivers/base/power/trace.c
index 9f4258df4cfd..c80e138b62fe 100644
--- a/drivers/base/power/trace.c
+++ b/drivers/base/power/trace.c
@@ -112,7 +112,7 @@ static unsigned int read_magic_time(void)
 	unsigned int val;
 
 	get_rtc_time(&time);
-	printk("Time: %2d:%02d:%02d  Date: %02d/%02d/%02d\n",
+	pr_info("Time: %2d:%02d:%02d  Date: %02d/%02d/%02d\n",
 		time.tm_hour, time.tm_min, time.tm_sec,
 		time.tm_mon + 1, time.tm_mday, time.tm_year % 100);
 	val = time.tm_year;				/* 100 years */
@@ -179,7 +179,7 @@ static int show_file_hash(unsigned int value)
 		unsigned int hash = hash_string(lineno, file, FILEHASH);
 		if (hash != value)
 			continue;
-		printk("  hash matches %s:%u\n", file, lineno);
+		pr_info("  hash matches %s:%u\n", file, lineno);
 		match++;
 	}
 	return match;
@@ -255,7 +255,7 @@ static int late_resume_init(void)
 	val = val / FILEHASH;
 	dev = val /* % DEVHASH */;
 
-	printk("  Magic number: %d:%d:%d\n", user, file, dev);
+	pr_info("  Magic number: %d:%d:%d\n", user, file, dev);
 	show_file_hash(file);
 	show_dev_hash(dev);
 	return 0;
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index 8ec406d8f548..4573c83df6dd 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -24,12 +24,26 @@
  */
 bool events_check_enabled;
 
-/* The counter of registered wakeup events. */
-static atomic_t event_count = ATOMIC_INIT(0);
-/* A preserved old value of event_count. */
+/*
+ * Combined counters of registered wakeup events and wakeup events in progress.
+ * They need to be modified together atomically, so it's better to use one
+ * atomic variable to hold them both.
+ */
+static atomic_t combined_event_count = ATOMIC_INIT(0);
+
+#define IN_PROGRESS_BITS	(sizeof(int) * 4)
+#define MAX_IN_PROGRESS		((1 << IN_PROGRESS_BITS) - 1)
+
+static void split_counters(unsigned int *cnt, unsigned int *inpr)
+{
+	unsigned int comb = atomic_read(&combined_event_count);
+
+	*cnt = (comb >> IN_PROGRESS_BITS);
+	*inpr = comb & MAX_IN_PROGRESS;
+}
+
+/* A preserved old value of the events counter. */
 static unsigned int saved_count;
-/* The counter of wakeup events being processed. */
-static atomic_t events_in_progress = ATOMIC_INIT(0);
 
 static DEFINE_SPINLOCK(events_lock);
 
@@ -228,6 +242,35 @@ int device_wakeup_disable(struct device *dev)
 EXPORT_SYMBOL_GPL(device_wakeup_disable);
 
 /**
+ * device_set_wakeup_capable - Set/reset device wakeup capability flag.
+ * @dev: Device to handle.
+ * @capable: Whether or not @dev is capable of waking up the system from sleep.
+ *
+ * If @capable is set, set the @dev's power.can_wakeup flag and add its
+ * wakeup-related attributes to sysfs.  Otherwise, unset the @dev's
+ * power.can_wakeup flag and remove its wakeup-related attributes from sysfs.
+ *
+ * This function may sleep and it can't be called from any context where
+ * sleeping is not allowed.
+ */
+void device_set_wakeup_capable(struct device *dev, bool capable)
+{
+	if (!!dev->power.can_wakeup == !!capable)
+		return;
+
+	if (device_is_registered(dev)) {
+		if (capable) {
+			if (wakeup_sysfs_add(dev))
+				return;
+		} else {
+			wakeup_sysfs_remove(dev);
+		}
+	}
+	dev->power.can_wakeup = capable;
+}
+EXPORT_SYMBOL_GPL(device_set_wakeup_capable);
+
+/**
  * device_init_wakeup - Device wakeup initialization.
  * @dev: Device to handle.
  * @enable: Whether or not to enable @dev as a wakeup device.
@@ -307,7 +350,8 @@ static void wakeup_source_activate(struct wakeup_source *ws)
 	ws->timer_expires = jiffies;
 	ws->last_time = ktime_get();
 
-	atomic_inc(&events_in_progress);
+	/* Increment the counter of events in progress. */
+	atomic_inc(&combined_event_count);
 }
 
 /**
@@ -394,14 +438,10 @@ static void wakeup_source_deactivate(struct wakeup_source *ws)
 	del_timer(&ws->timer);
 
 	/*
-	 * event_count has to be incremented before events_in_progress is
-	 * modified, so that the callers of pm_check_wakeup_events() and
-	 * pm_save_wakeup_count() don't see the old value of event_count and
-	 * events_in_progress equal to zero at the same time.
+	 * Increment the counter of registered wakeup events and decrement the
+	 * couter of wakeup events in progress simultaneously.
 	 */
-	atomic_inc(&event_count);
-	smp_mb__before_atomic_dec();
-	atomic_dec(&events_in_progress);
+	atomic_add(MAX_IN_PROGRESS, &combined_event_count);
 }
 
 /**
@@ -556,8 +596,10 @@ bool pm_wakeup_pending(void)
 
 	spin_lock_irqsave(&events_lock, flags);
 	if (events_check_enabled) {
-		ret = ((unsigned int)atomic_read(&event_count) != saved_count)
-			|| atomic_read(&events_in_progress);
+		unsigned int cnt, inpr;
+
+		split_counters(&cnt, &inpr);
+		ret = (cnt != saved_count || inpr > 0);
 		events_check_enabled = !ret;
 	}
 	spin_unlock_irqrestore(&events_lock, flags);
@@ -573,25 +615,25 @@ bool pm_wakeup_pending(void)
  * Store the number of registered wakeup events at the address in @count.  Block
  * if the current number of wakeup events being processed is nonzero.
  *
- * Return false if the wait for the number of wakeup events being processed to
+ * Return 'false' if the wait for the number of wakeup events being processed to
  * drop down to zero has been interrupted by a signal (and the current number
- * of wakeup events being processed is still nonzero).  Otherwise return true.
+ * of wakeup events being processed is still nonzero).  Otherwise return 'true'.
  */
 bool pm_get_wakeup_count(unsigned int *count)
 {
-	bool ret;
-
-	if (capable(CAP_SYS_ADMIN))
-		events_check_enabled = false;
+	unsigned int cnt, inpr;
 
-	while (atomic_read(&events_in_progress) && !signal_pending(current)) {
+	for (;;) {
+		split_counters(&cnt, &inpr);
+		if (inpr == 0 || signal_pending(current))
+			break;
 		pm_wakeup_update_hit_counts();
 		schedule_timeout_interruptible(msecs_to_jiffies(TIMEOUT));
 	}
 
-	ret = !atomic_read(&events_in_progress);
-	*count = atomic_read(&event_count);
-	return ret;
+	split_counters(&cnt, &inpr);
+	*count = cnt;
+	return !inpr;
 }
 
 /**
@@ -600,24 +642,25 @@ bool pm_get_wakeup_count(unsigned int *count)
  *
  * If @count is equal to the current number of registered wakeup events and the
  * current number of wakeup events being processed is zero, store @count as the
- * old number of registered wakeup events to be used by pm_check_wakeup_events()
- * and return true.  Otherwise return false.
+ * old number of registered wakeup events for pm_check_wakeup_events(), enable
+ * wakeup events detection and return 'true'.  Otherwise disable wakeup events
+ * detection and return 'false'.
  */
 bool pm_save_wakeup_count(unsigned int count)
 {
-	bool ret = false;
+	unsigned int cnt, inpr;
 
+	events_check_enabled = false;
 	spin_lock_irq(&events_lock);
-	if (count == (unsigned int)atomic_read(&event_count)
-	    && !atomic_read(&events_in_progress)) {
+	split_counters(&cnt, &inpr);
+	if (cnt == count && inpr == 0) {
 		saved_count = count;
 		events_check_enabled = true;
-		ret = true;
 	}
 	spin_unlock_irq(&events_lock);
-	if (!ret)
+	if (!events_check_enabled)
 		pm_wakeup_update_hit_counts();
-	return ret;
+	return events_check_enabled;
 }
 
 static struct dentry *wakeup_sources_stats_dentry;
diff --git a/drivers/base/sys.c b/drivers/base/sys.c
index 1667aaf4fde6..f6fb54741602 100644
--- a/drivers/base/sys.c
+++ b/drivers/base/sys.c
@@ -166,6 +166,36 @@ EXPORT_SYMBOL_GPL(sysdev_class_unregister);
 
 static DEFINE_MUTEX(sysdev_drivers_lock);
 
+/*
+ * @dev != NULL means that we're unwinding because some drv->add()
+ * failed for some reason. You need to grab sysdev_drivers_lock before
+ * calling this.
+ */
+static void __sysdev_driver_remove(struct sysdev_class *cls,
+				   struct sysdev_driver *drv,
+				   struct sys_device *from_dev)
+{
+	struct sys_device *dev = from_dev;
+
+	list_del_init(&drv->entry);
+	if (!cls)
+		return;
+
+	if (!drv->remove)
+		goto kset_put;
+
+	if (dev)
+		list_for_each_entry_continue_reverse(dev, &cls->kset.list,
+						     kobj.entry)
+			drv->remove(dev);
+	else
+		list_for_each_entry(dev, &cls->kset.list, kobj.entry)
+			drv->remove(dev);
+
+kset_put:
+	kset_put(&cls->kset);
+}
+
 /**
  *	sysdev_driver_register - Register auxillary driver
  *	@cls:	Device class driver belongs to.
@@ -175,14 +205,14 @@ static DEFINE_MUTEX(sysdev_drivers_lock);
  *	called on each operation on devices of that class. The refcount
  *	of @cls is incremented.
  */
-
 int sysdev_driver_register(struct sysdev_class *cls, struct sysdev_driver *drv)
 {
+	struct sys_device *dev = NULL;
 	int err = 0;
 
 	if (!cls) {
-		WARN(1, KERN_WARNING "sysdev: invalid class passed to "
-			"sysdev_driver_register!\n");
+		WARN(1, KERN_WARNING "sysdev: invalid class passed to %s!\n",
+			__func__);
 		return -EINVAL;
 	}
 
@@ -198,19 +228,27 @@ int sysdev_driver_register(struct sysdev_class *cls, struct sysdev_driver *drv)
 
 		/* If devices of this class already exist, tell the driver */
 		if (drv->add) {
-			struct sys_device *dev;
-			list_for_each_entry(dev, &cls->kset.list, kobj.entry)
-				drv->add(dev);
+			list_for_each_entry(dev, &cls->kset.list, kobj.entry) {
+				err = drv->add(dev);
+				if (err)
+					goto unwind;
+			}
 		}
 	} else {
 		err = -EINVAL;
 		WARN(1, KERN_ERR "%s: invalid device class\n", __func__);
 	}
+
+	goto unlock;
+
+unwind:
+	__sysdev_driver_remove(cls, drv, dev);
+
+unlock:
 	mutex_unlock(&sysdev_drivers_lock);
 	return err;
 }
 
-
 /**
  *	sysdev_driver_unregister - Remove an auxillary driver.
  *	@cls:	Class driver belongs to.
@@ -220,23 +258,12 @@ void sysdev_driver_unregister(struct sysdev_class *cls,
 			      struct sysdev_driver *drv)
 {
 	mutex_lock(&sysdev_drivers_lock);
-	list_del_init(&drv->entry);
-	if (cls) {
-		if (drv->remove) {
-			struct sys_device *dev;
-			list_for_each_entry(dev, &cls->kset.list, kobj.entry)
-				drv->remove(dev);
-		}
-		kset_put(&cls->kset);
-	}
+	__sysdev_driver_remove(cls, drv, NULL);
 	mutex_unlock(&sysdev_drivers_lock);
 }
-
 EXPORT_SYMBOL_GPL(sysdev_driver_register);
 EXPORT_SYMBOL_GPL(sysdev_driver_unregister);
 
-
-
 /**
  *	sysdev_register - add a system device to the tree
  *	@sysdev:	device in question
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 8cbfaa687d72..fe81c851ca88 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -2177,7 +2177,7 @@ static void drbd_connector_callback(struct cn_msg *req, struct netlink_skb_parms
 		return;
 	}
 
-	if (!cap_raised(nsp->eff_cap, CAP_SYS_ADMIN)) {
+	if (!cap_raised(current_cap(), CAP_SYS_ADMIN)) {
 		retcode = ERR_PERM;
 		goto fail;
 	}
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index b9ba04fc2b34..77fc76f8aea9 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -3281,7 +3281,7 @@ static int set_geometry(unsigned int cmd, struct floppy_struct *g,
 			struct block_device *bdev = opened_bdev[cnt];
 			if (!bdev || ITYPE(drive_state[cnt].fd_device) != type)
 				continue;
-			__invalidate_device(bdev);
+			__invalidate_device(bdev, true);
 		}
 		mutex_unlock(&open_lock);
 	} else {
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index 49e6a545eb63..79c3079a6203 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -78,7 +78,6 @@
 
 #include <asm/uaccess.h>
 
-static DEFINE_MUTEX(loop_mutex);
 static LIST_HEAD(loop_devices);
 static DEFINE_MUTEX(loop_devices_mutex);
 
@@ -1501,11 +1500,9 @@ static int lo_open(struct block_device *bdev, fmode_t mode)
 {
 	struct loop_device *lo = bdev->bd_disk->private_data;
 
-	mutex_lock(&loop_mutex);
 	mutex_lock(&lo->lo_ctl_mutex);
 	lo->lo_refcnt++;
 	mutex_unlock(&lo->lo_ctl_mutex);
-	mutex_unlock(&loop_mutex);
 
 	return 0;
 }
@@ -1515,7 +1512,6 @@ static int lo_release(struct gendisk *disk, fmode_t mode)
 	struct loop_device *lo = disk->private_data;
 	int err;
 
-	mutex_lock(&loop_mutex);
 	mutex_lock(&lo->lo_ctl_mutex);
 
 	if (--lo->lo_refcnt)
@@ -1540,7 +1536,6 @@ static int lo_release(struct gendisk *disk, fmode_t mode)
 out:
 	mutex_unlock(&lo->lo_ctl_mutex);
 out_unlocked:
-	mutex_unlock(&loop_mutex);
 	return 0;
 }
 
@@ -1641,9 +1636,6 @@ out:
 
 static void loop_free(struct loop_device *lo)
 {
-	if (!lo->lo_queue->queue_lock)
-		lo->lo_queue->queue_lock = &lo->lo_queue->__queue_lock;
-
 	blk_cleanup_queue(lo->lo_queue);
 	put_disk(lo->lo_disk);
 	list_del(&lo->lo_list);
diff --git a/drivers/block/smart1,2.h b/drivers/block/smart1,2.h
index a0b403a6b4ed..e5565fbaeb30 100644
--- a/drivers/block/smart1,2.h
+++ b/drivers/block/smart1,2.h
@@ -95,7 +95,7 @@ static unsigned long smart4_completed(ctlr_info_t *h)
  /*
  *  This hardware returns interrupt pending at a different place and 
  *  it does not tell us if the fifo is empty, we will have check  
- *  that by getting a 0 back from the comamnd_completed call. 
+ *  that by getting a 0 back from the command_completed call. 
  */
 static unsigned long smart4_intr_pending(ctlr_info_t *h)
 {
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 6ecf89cdf006..33a48a80c7e8 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -6,10 +6,12 @@
 #include <linux/virtio.h>
 #include <linux/virtio_blk.h>
 #include <linux/scatterlist.h>
+#include <linux/string_helpers.h>
 
 #define PART_BITS 4
 
 static int major, index;
+struct workqueue_struct *virtblk_wq;
 
 struct virtio_blk
 {
@@ -26,6 +28,9 @@ struct virtio_blk
 
 	mempool_t *pool;
 
+	/* Process context for config space updates */
+	struct work_struct config_work;
+
 	/* What host tells us, plus 2 for header & tailer. */
 	unsigned int sg_elems;
 
@@ -291,6 +296,46 @@ static ssize_t virtblk_serial_show(struct device *dev,
 }
 DEVICE_ATTR(serial, S_IRUGO, virtblk_serial_show, NULL);
 
+static void virtblk_config_changed_work(struct work_struct *work)
+{
+	struct virtio_blk *vblk =
+		container_of(work, struct virtio_blk, config_work);
+	struct virtio_device *vdev = vblk->vdev;
+	struct request_queue *q = vblk->disk->queue;
+	char cap_str_2[10], cap_str_10[10];
+	u64 capacity, size;
+
+	/* Host must always specify the capacity. */
+	vdev->config->get(vdev, offsetof(struct virtio_blk_config, capacity),
+			  &capacity, sizeof(capacity));
+
+	/* If capacity is too big, truncate with warning. */
+	if ((sector_t)capacity != capacity) {
+		dev_warn(&vdev->dev, "Capacity %llu too large: truncating\n",
+			 (unsigned long long)capacity);
+		capacity = (sector_t)-1;
+	}
+
+	size = capacity * queue_logical_block_size(q);
+	string_get_size(size, STRING_UNITS_2, cap_str_2, sizeof(cap_str_2));
+	string_get_size(size, STRING_UNITS_10, cap_str_10, sizeof(cap_str_10));
+
+	dev_notice(&vdev->dev,
+		  "new size: %llu %d-byte logical blocks (%s/%s)\n",
+		  (unsigned long long)capacity,
+		  queue_logical_block_size(q),
+		  cap_str_10, cap_str_2);
+
+	set_capacity(vblk->disk, capacity);
+}
+
+static void virtblk_config_changed(struct virtio_device *vdev)
+{
+	struct virtio_blk *vblk = vdev->priv;
+
+	queue_work(virtblk_wq, &vblk->config_work);
+}
+
 static int __devinit virtblk_probe(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk;
@@ -327,6 +372,7 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
 	vblk->vdev = vdev;
 	vblk->sg_elems = sg_elems;
 	sg_init_table(vblk->sg, vblk->sg_elems);
+	INIT_WORK(&vblk->config_work, virtblk_config_changed_work);
 
 	/* We expect one virtqueue, for output. */
 	vblk->vq = virtio_find_single_vq(vdev, blk_done, "requests");
@@ -477,6 +523,8 @@ static void __devexit virtblk_remove(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk = vdev->priv;
 
+	flush_work(&vblk->config_work);
+
 	/* Nothing should be pending. */
 	BUG_ON(!list_empty(&vblk->reqs));
 
@@ -508,27 +556,47 @@ static unsigned int features[] = {
  * Use __refdata to avoid this warning.
  */
 static struct virtio_driver __refdata virtio_blk = {
-	.feature_table = features,
-	.feature_table_size = ARRAY_SIZE(features),
-	.driver.name =	KBUILD_MODNAME,
-	.driver.owner =	THIS_MODULE,
-	.id_table =	id_table,
-	.probe =	virtblk_probe,
-	.remove =	__devexit_p(virtblk_remove),
+	.feature_table		= features,
+	.feature_table_size	= ARRAY_SIZE(features),
+	.driver.name		= KBUILD_MODNAME,
+	.driver.owner		= THIS_MODULE,
+	.id_table		= id_table,
+	.probe			= virtblk_probe,
+	.remove			= __devexit_p(virtblk_remove),
+	.config_changed		= virtblk_config_changed,
 };
 
 static int __init init(void)
 {
+	int error;
+
+	virtblk_wq = alloc_workqueue("virtio-blk", 0, 0);
+	if (!virtblk_wq)
+		return -ENOMEM;
+
 	major = register_blkdev(0, "virtblk");
-	if (major < 0)
-		return major;
-	return register_virtio_driver(&virtio_blk);
+	if (major < 0) {
+		error = major;
+		goto out_destroy_workqueue;
+	}
+
+	error = register_virtio_driver(&virtio_blk);
+	if (error)
+		goto out_unregister_blkdev;
+	return 0;
+
+out_unregister_blkdev:
+	unregister_blkdev(major, "virtblk");
+out_destroy_workqueue:
+	destroy_workqueue(virtblk_wq);
+	return error;
 }
 
 static void __exit fini(void)
 {
 	unregister_blkdev(major, "virtblk");
 	unregister_virtio_driver(&virtio_blk);
+	destroy_workqueue(virtblk_wq);
 }
 module_init(init);
 module_exit(fini);
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index d7aa39e349a6..9cb8668ff5f4 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -120,6 +120,10 @@ static DEFINE_SPINLOCK(minor_lock);
 #define EXTENDED (1<<EXT_SHIFT)
 #define VDEV_IS_EXTENDED(dev) ((dev)&(EXTENDED))
 #define BLKIF_MINOR_EXT(dev) ((dev)&(~EXTENDED))
+#define EMULATED_HD_DISK_MINOR_OFFSET (0)
+#define EMULATED_HD_DISK_NAME_OFFSET (EMULATED_HD_DISK_MINOR_OFFSET / 256)
+#define EMULATED_SD_DISK_MINOR_OFFSET (EMULATED_HD_DISK_MINOR_OFFSET + (4 * 16))
+#define EMULATED_SD_DISK_NAME_OFFSET (EMULATED_HD_DISK_NAME_OFFSET + 4)
 
 #define DEV_NAME	"xvd"	/* name in /dev */
 
@@ -281,7 +285,7 @@ static int blkif_queue_request(struct request *req)
 	info->shadow[id].request = req;
 
 	ring_req->id = id;
-	ring_req->sector_number = (blkif_sector_t)blk_rq_pos(req);
+	ring_req->u.rw.sector_number = (blkif_sector_t)blk_rq_pos(req);
 	ring_req->handle = info->handle;
 
 	ring_req->operation = rq_data_dir(req) ?
@@ -317,7 +321,7 @@ static int blkif_queue_request(struct request *req)
 				rq_data_dir(req) );
 
 		info->shadow[id].frame[i] = mfn_to_pfn(buffer_mfn);
-		ring_req->seg[i] =
+		ring_req->u.rw.seg[i] =
 				(struct blkif_request_segment) {
 					.gref       = ref,
 					.first_sect = fsect,
@@ -434,6 +438,65 @@ static void xlvbd_flush(struct blkfront_info *info)
 	       info->feature_flush ? "enabled" : "disabled");
 }
 
+static int xen_translate_vdev(int vdevice, int *minor, unsigned int *offset)
+{
+	int major;
+	major = BLKIF_MAJOR(vdevice);
+	*minor = BLKIF_MINOR(vdevice);
+	switch (major) {
+		case XEN_IDE0_MAJOR:
+			*offset = (*minor / 64) + EMULATED_HD_DISK_NAME_OFFSET;
+			*minor = ((*minor / 64) * PARTS_PER_DISK) +
+				EMULATED_HD_DISK_MINOR_OFFSET;
+			break;
+		case XEN_IDE1_MAJOR:
+			*offset = (*minor / 64) + 2 + EMULATED_HD_DISK_NAME_OFFSET;
+			*minor = (((*minor / 64) + 2) * PARTS_PER_DISK) +
+				EMULATED_HD_DISK_MINOR_OFFSET;
+			break;
+		case XEN_SCSI_DISK0_MAJOR:
+			*offset = (*minor / PARTS_PER_DISK) + EMULATED_SD_DISK_NAME_OFFSET;
+			*minor = *minor + EMULATED_SD_DISK_MINOR_OFFSET;
+			break;
+		case XEN_SCSI_DISK1_MAJOR:
+		case XEN_SCSI_DISK2_MAJOR:
+		case XEN_SCSI_DISK3_MAJOR:
+		case XEN_SCSI_DISK4_MAJOR:
+		case XEN_SCSI_DISK5_MAJOR:
+		case XEN_SCSI_DISK6_MAJOR:
+		case XEN_SCSI_DISK7_MAJOR:
+			*offset = (*minor / PARTS_PER_DISK) + 
+				((major - XEN_SCSI_DISK1_MAJOR + 1) * 16) +
+				EMULATED_SD_DISK_NAME_OFFSET;
+			*minor = *minor +
+				((major - XEN_SCSI_DISK1_MAJOR + 1) * 16 * PARTS_PER_DISK) +
+				EMULATED_SD_DISK_MINOR_OFFSET;
+			break;
+		case XEN_SCSI_DISK8_MAJOR:
+		case XEN_SCSI_DISK9_MAJOR:
+		case XEN_SCSI_DISK10_MAJOR:
+		case XEN_SCSI_DISK11_MAJOR:
+		case XEN_SCSI_DISK12_MAJOR:
+		case XEN_SCSI_DISK13_MAJOR:
+		case XEN_SCSI_DISK14_MAJOR:
+		case XEN_SCSI_DISK15_MAJOR:
+			*offset = (*minor / PARTS_PER_DISK) + 
+				((major - XEN_SCSI_DISK8_MAJOR + 8) * 16) +
+				EMULATED_SD_DISK_NAME_OFFSET;
+			*minor = *minor +
+				((major - XEN_SCSI_DISK8_MAJOR + 8) * 16 * PARTS_PER_DISK) +
+				EMULATED_SD_DISK_MINOR_OFFSET;
+			break;
+		case XENVBD_MAJOR:
+			*offset = *minor / PARTS_PER_DISK;
+			break;
+		default:
+			printk(KERN_WARNING "blkfront: your disk configuration is "
+					"incorrect, please use an xvd device instead\n");
+			return -ENODEV;
+	}
+	return 0;
+}
 
 static int xlvbd_alloc_gendisk(blkif_sector_t capacity,
 			       struct blkfront_info *info,
@@ -441,7 +504,7 @@ static int xlvbd_alloc_gendisk(blkif_sector_t capacity,
 {
 	struct gendisk *gd;
 	int nr_minors = 1;
-	int err = -ENODEV;
+	int err;
 	unsigned int offset;
 	int minor;
 	int nr_parts;
@@ -456,12 +519,20 @@ static int xlvbd_alloc_gendisk(blkif_sector_t capacity,
 	}
 
 	if (!VDEV_IS_EXTENDED(info->vdevice)) {
-		minor = BLKIF_MINOR(info->vdevice);
-		nr_parts = PARTS_PER_DISK;
+		err = xen_translate_vdev(info->vdevice, &minor, &offset);
+		if (err)
+			return err;		
+ 		nr_parts = PARTS_PER_DISK;
 	} else {
 		minor = BLKIF_MINOR_EXT(info->vdevice);
 		nr_parts = PARTS_PER_EXT_DISK;
+		offset = minor / nr_parts;
+		if (xen_hvm_domain() && offset <= EMULATED_HD_DISK_NAME_OFFSET + 4)
+			printk(KERN_WARNING "blkfront: vdevice 0x%x might conflict with "
+					"emulated IDE disks,\n\t choose an xvd device name"
+					"from xvde on\n", info->vdevice);
 	}
+	err = -ENODEV;
 
 	if ((minor % nr_parts) == 0)
 		nr_minors = nr_parts;
@@ -475,8 +546,6 @@ static int xlvbd_alloc_gendisk(blkif_sector_t capacity,
 	if (gd == NULL)
 		goto release;
 
-	offset = minor / nr_parts;
-
 	if (nr_minors > 1) {
 		if (offset < 26)
 			sprintf(gd->disk_name, "%s%c", DEV_NAME, 'a' + offset);
@@ -615,7 +684,7 @@ static void blkif_completion(struct blk_shadow *s)
 {
 	int i;
 	for (i = 0; i < s->req.nr_segments; i++)
-		gnttab_end_foreign_access(s->req.seg[i].gref, 0, 0UL);
+		gnttab_end_foreign_access(s->req.u.rw.seg[i].gref, 0, 0UL);
 }
 
 static irqreturn_t blkif_interrupt(int irq, void *dev_id)
@@ -932,7 +1001,7 @@ static int blkif_recover(struct blkfront_info *info)
 		/* Rewrite any grant references invalidated by susp/resume. */
 		for (j = 0; j < req->nr_segments; j++)
 			gnttab_grant_foreign_access_ref(
-				req->seg[j].gref,
+				req->u.rw.seg[j].gref,
 				info->xbdev->otherend_id,
 				pfn_to_mfn(info->shadow[req->id].frame[j]),
 				rq_data_dir(info->shadow[req->id].request));
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index 829161edae53..2c590a796aa1 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -1195,16 +1195,13 @@ static struct platform_driver ace_platform_driver = {
  */
 
 #if defined(CONFIG_OF)
-static int __devinit
-ace_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit ace_of_probe(struct platform_device *op)
 {
 	struct resource res;
 	resource_size_t physaddr;
 	const u32 *id;
 	int irq, bus_width, rc;
 
-	dev_dbg(&op->dev, "ace_of_probe(%p, %p)\n", op, match);
-
 	/* device id */
 	id = of_get_property(op->dev.of_node, "port-number", NULL);
 
@@ -1245,7 +1242,7 @@ static const struct of_device_id ace_of_match[] __devinitconst = {
 };
 MODULE_DEVICE_TABLE(of, ace_of_match);
 
-static struct of_platform_driver ace_of_driver = {
+static struct platform_driver ace_of_driver = {
 	.probe = ace_of_probe,
 	.remove = __devexit_p(ace_of_remove),
 	.driver = {
@@ -1259,12 +1256,12 @@ static struct of_platform_driver ace_of_driver = {
 static inline int __init ace_of_register(void)
 {
 	pr_debug("xsysace: registering OF binding\n");
-	return of_register_platform_driver(&ace_of_driver);
+	return platform_driver_register(&ace_of_driver);
 }
 
 static inline void __exit ace_of_unregister(void)
 {
-	of_unregister_platform_driver(&ace_of_driver);
+	platform_driver_unregister(&ace_of_driver);
 }
 #else /* CONFIG_OF */
 /* CONFIG_OF not enabled; do nothing helpers */
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 333c21289d97..5577ed656e2f 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -31,6 +31,30 @@
 
 #define VERSION "1.0"
 
+#define ATH3K_DNLOAD				0x01
+#define ATH3K_GETSTATE				0x05
+#define ATH3K_SET_NORMAL_MODE			0x07
+#define ATH3K_GETVERSION			0x09
+#define USB_REG_SWITCH_VID_PID			0x0a
+
+#define ATH3K_MODE_MASK				0x3F
+#define ATH3K_NORMAL_MODE			0x0E
+
+#define ATH3K_PATCH_UPDATE			0x80
+#define ATH3K_SYSCFG_UPDATE			0x40
+
+#define ATH3K_XTAL_FREQ_26M			0x00
+#define ATH3K_XTAL_FREQ_40M			0x01
+#define ATH3K_XTAL_FREQ_19P2			0x02
+#define ATH3K_NAME_LEN				0xFF
+
+struct ath3k_version {
+	unsigned int	rom_version;
+	unsigned int	build_version;
+	unsigned int	ram_version;
+	unsigned char	ref_clock;
+	unsigned char	reserved[0x07];
+};
 
 static struct usb_device_id ath3k_table[] = {
 	/* Atheros AR3011 */
@@ -41,13 +65,32 @@ static struct usb_device_id ath3k_table[] = {
 
 	/* Atheros AR9285 Malbec with sflash firmware */
 	{ USB_DEVICE(0x03F0, 0x311D) },
+
+	/* Atheros AR3012 with sflash firmware*/
+	{ USB_DEVICE(0x0CF3, 0x3004) },
+
+	/* Atheros AR5BBU12 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xE02C) },
+
 	{ }	/* Terminating entry */
 };
 
 MODULE_DEVICE_TABLE(usb, ath3k_table);
 
+#define BTUSB_ATH3012		0x80
+/* This table is to load patch and sysconfig files
+ * for AR3012 */
+static struct usb_device_id ath3k_blist_tbl[] = {
+
+	/* Atheros AR3012 with sflash firmware*/
+	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
+
+	{ }	/* Terminating entry */
+};
+
 #define USB_REQ_DFU_DNLOAD	1
 #define BULK_SIZE		4096
+#define FW_HDR_SIZE		20
 
 static int ath3k_load_firmware(struct usb_device *udev,
 				const struct firmware *firmware)
@@ -103,28 +146,265 @@ error:
 	return err;
 }
 
+static int ath3k_get_state(struct usb_device *udev, unsigned char *state)
+{
+	int pipe = 0;
+
+	pipe = usb_rcvctrlpipe(udev, 0);
+	return usb_control_msg(udev, pipe, ATH3K_GETSTATE,
+			USB_TYPE_VENDOR | USB_DIR_IN, 0, 0,
+			state, 0x01, USB_CTRL_SET_TIMEOUT);
+}
+
+static int ath3k_get_version(struct usb_device *udev,
+			struct ath3k_version *version)
+{
+	int pipe = 0;
+
+	pipe = usb_rcvctrlpipe(udev, 0);
+	return usb_control_msg(udev, pipe, ATH3K_GETVERSION,
+			USB_TYPE_VENDOR | USB_DIR_IN, 0, 0, version,
+			sizeof(struct ath3k_version),
+			USB_CTRL_SET_TIMEOUT);
+}
+
+static int ath3k_load_fwfile(struct usb_device *udev,
+		const struct firmware *firmware)
+{
+	u8 *send_buf;
+	int err, pipe, len, size, count, sent = 0;
+	int ret;
+
+	count = firmware->size;
+
+	send_buf = kmalloc(BULK_SIZE, GFP_ATOMIC);
+	if (!send_buf) {
+		BT_ERR("Can't allocate memory chunk for firmware");
+		return -ENOMEM;
+	}
+
+	size = min_t(uint, count, FW_HDR_SIZE);
+	memcpy(send_buf, firmware->data, size);
+
+	pipe = usb_sndctrlpipe(udev, 0);
+	ret = usb_control_msg(udev, pipe, ATH3K_DNLOAD,
+			USB_TYPE_VENDOR, 0, 0, send_buf,
+			size, USB_CTRL_SET_TIMEOUT);
+	if (ret < 0) {
+		BT_ERR("Can't change to loading configuration err");
+		kfree(send_buf);
+		return ret;
+	}
+
+	sent += size;
+	count -= size;
+
+	while (count) {
+		size = min_t(uint, count, BULK_SIZE);
+		pipe = usb_sndbulkpipe(udev, 0x02);
+
+		memcpy(send_buf, firmware->data + sent, size);
+
+		err = usb_bulk_msg(udev, pipe, send_buf, size,
+					&len, 3000);
+		if (err || (len != size)) {
+			BT_ERR("Error in firmware loading err = %d,"
+				"len = %d, size = %d", err, len, size);
+			kfree(send_buf);
+			return err;
+		}
+		sent  += size;
+		count -= size;
+	}
+
+	kfree(send_buf);
+	return 0;
+}
+
+static int ath3k_switch_pid(struct usb_device *udev)
+{
+	int pipe = 0;
+
+	pipe = usb_sndctrlpipe(udev, 0);
+	return usb_control_msg(udev, pipe, USB_REG_SWITCH_VID_PID,
+			USB_TYPE_VENDOR, 0, 0,
+			NULL, 0, USB_CTRL_SET_TIMEOUT);
+}
+
+static int ath3k_set_normal_mode(struct usb_device *udev)
+{
+	unsigned char fw_state;
+	int pipe = 0, ret;
+
+	ret = ath3k_get_state(udev, &fw_state);
+	if (ret < 0) {
+		BT_ERR("Can't get state to change to normal mode err");
+		return ret;
+	}
+
+	if ((fw_state & ATH3K_MODE_MASK) == ATH3K_NORMAL_MODE) {
+		BT_DBG("firmware was already in normal mode");
+		return 0;
+	}
+
+	pipe = usb_sndctrlpipe(udev, 0);
+	return usb_control_msg(udev, pipe, ATH3K_SET_NORMAL_MODE,
+			USB_TYPE_VENDOR, 0, 0,
+			NULL, 0, USB_CTRL_SET_TIMEOUT);
+}
+
+static int ath3k_load_patch(struct usb_device *udev)
+{
+	unsigned char fw_state;
+	char filename[ATH3K_NAME_LEN] = {0};
+	const struct firmware *firmware;
+	struct ath3k_version fw_version, pt_version;
+	int ret;
+
+	ret = ath3k_get_state(udev, &fw_state);
+	if (ret < 0) {
+		BT_ERR("Can't get state to change to load ram patch err");
+		return ret;
+	}
+
+	if (fw_state & ATH3K_PATCH_UPDATE) {
+		BT_DBG("Patch was already downloaded");
+		return 0;
+	}
+
+	ret = ath3k_get_version(udev, &fw_version);
+	if (ret < 0) {
+		BT_ERR("Can't get version to change to load ram patch err");
+		return ret;
+	}
+
+	snprintf(filename, ATH3K_NAME_LEN, "ar3k/AthrBT_0x%08x.dfu",
+		fw_version.rom_version);
+
+	ret = request_firmware(&firmware, filename, &udev->dev);
+	if (ret < 0) {
+		BT_ERR("Patch file not found %s", filename);
+		return ret;
+	}
+
+	pt_version.rom_version = *(int *)(firmware->data + firmware->size - 8);
+	pt_version.build_version = *(int *)
+		(firmware->data + firmware->size - 4);
+
+	if ((pt_version.rom_version != fw_version.rom_version) ||
+		(pt_version.build_version <= fw_version.build_version)) {
+		BT_ERR("Patch file version did not match with firmware");
+		release_firmware(firmware);
+		return -EINVAL;
+	}
+
+	ret = ath3k_load_fwfile(udev, firmware);
+	release_firmware(firmware);
+
+	return ret;
+}
+
+static int ath3k_load_syscfg(struct usb_device *udev)
+{
+	unsigned char fw_state;
+	char filename[ATH3K_NAME_LEN] = {0};
+	const struct firmware *firmware;
+	struct ath3k_version fw_version;
+	int clk_value, ret;
+
+	ret = ath3k_get_state(udev, &fw_state);
+	if (ret < 0) {
+		BT_ERR("Can't get state to change to load configration err");
+		return -EBUSY;
+	}
+
+	ret = ath3k_get_version(udev, &fw_version);
+	if (ret < 0) {
+		BT_ERR("Can't get version to change to load ram patch err");
+		return ret;
+	}
+
+	switch (fw_version.ref_clock) {
+
+	case ATH3K_XTAL_FREQ_26M:
+		clk_value = 26;
+		break;
+	case ATH3K_XTAL_FREQ_40M:
+		clk_value = 40;
+		break;
+	case ATH3K_XTAL_FREQ_19P2:
+		clk_value = 19;
+		break;
+	default:
+		clk_value = 0;
+		break;
+	}
+
+	snprintf(filename, ATH3K_NAME_LEN, "ar3k/ramps_0x%08x_%d%s",
+		fw_version.rom_version, clk_value, ".dfu");
+
+	ret = request_firmware(&firmware, filename, &udev->dev);
+	if (ret < 0) {
+		BT_ERR("Configuration file not found %s", filename);
+		return ret;
+	}
+
+	ret = ath3k_load_fwfile(udev, firmware);
+	release_firmware(firmware);
+
+	return ret;
+}
+
 static int ath3k_probe(struct usb_interface *intf,
 			const struct usb_device_id *id)
 {
 	const struct firmware *firmware;
 	struct usb_device *udev = interface_to_usbdev(intf);
+	int ret;
 
 	BT_DBG("intf %p id %p", intf, id);
 
 	if (intf->cur_altsetting->desc.bInterfaceNumber != 0)
 		return -ENODEV;
 
-	if (request_firmware(&firmware, "ath3k-1.fw", &udev->dev) < 0) {
-		return -EIO;
+	/* match device ID in ath3k blacklist table */
+	if (!id->driver_info) {
+		const struct usb_device_id *match;
+		match = usb_match_id(intf, ath3k_blist_tbl);
+		if (match)
+			id = match;
 	}
 
-	if (ath3k_load_firmware(udev, firmware)) {
-		release_firmware(firmware);
+	/* load patch and sysconfig files for AR3012 */
+	if (id->driver_info & BTUSB_ATH3012) {
+		ret = ath3k_load_patch(udev);
+		if (ret < 0) {
+			BT_ERR("Loading patch file failed");
+			return ret;
+		}
+		ret = ath3k_load_syscfg(udev);
+		if (ret < 0) {
+			BT_ERR("Loading sysconfig file failed");
+			return ret;
+		}
+		ret = ath3k_set_normal_mode(udev);
+		if (ret < 0) {
+			BT_ERR("Set normal mode failed");
+			return ret;
+		}
+		ath3k_switch_pid(udev);
+		return 0;
+	}
+
+	if (request_firmware(&firmware, "ath3k-1.fw", &udev->dev) < 0) {
+		BT_ERR("Error loading firmware");
 		return -EIO;
 	}
+
+	ret = ath3k_load_firmware(udev, firmware);
 	release_firmware(firmware);
 
-	return 0;
+	return ret;
 }
 
 static void ath3k_disconnect(struct usb_interface *intf)
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 4cefa91e6c34..866811428e20 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -105,6 +105,12 @@ static struct usb_device_id blacklist_table[] = {
 	/* Atheros AR9285 Malbec with sflash firmware */
 	{ USB_DEVICE(0x03f0, 0x311d), .driver_info = BTUSB_IGNORE },
 
+	/* Atheros 3012 with sflash firmware */
+	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_IGNORE },
+
+	/* Atheros AR5BBU12 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xe02c), .driver_info = BTUSB_IGNORE },
+
 	/* Broadcom BCM2035 */
 	{ USB_DEVICE(0x0a5c, 0x2035), .driver_info = BTUSB_WRONG_SCO_MTU },
 	{ USB_DEVICE(0x0a5c, 0x200a), .driver_info = BTUSB_WRONG_SCO_MTU },
@@ -427,7 +433,7 @@ static void btusb_isoc_complete(struct urb *urb)
 	}
 }
 
-static void inline __fill_isoc_descriptor(struct urb *urb, int len, int mtu)
+static inline void __fill_isoc_descriptor(struct urb *urb, int len, int mtu)
 {
 	int i, offset = 0;
 
@@ -711,15 +717,11 @@ static int btusb_send_frame(struct sk_buff *skb)
 		pipe = usb_sndisocpipe(data->udev,
 					data->isoc_tx_ep->bEndpointAddress);
 
-		urb->dev      = data->udev;
-		urb->pipe     = pipe;
-		urb->context  = skb;
-		urb->complete = btusb_isoc_tx_complete;
-		urb->interval = data->isoc_tx_ep->bInterval;
+		usb_fill_int_urb(urb, data->udev, pipe,
+				skb->data, skb->len, btusb_isoc_tx_complete,
+				skb, data->isoc_tx_ep->bInterval);
 
 		urb->transfer_flags  = URB_ISO_ASAP;
-		urb->transfer_buffer = skb->data;
-		urb->transfer_buffer_length = skb->len;
 
 		__fill_isoc_descriptor(urb, skb->len,
 				le16_to_cpu(data->isoc_tx_ep->wMaxPacketSize));
@@ -778,7 +780,7 @@ static void btusb_notify(struct hci_dev *hdev, unsigned int evt)
 	}
 }
 
-static int inline __set_isoc_interface(struct hci_dev *hdev, int altsetting)
+static inline int __set_isoc_interface(struct hci_dev *hdev, int altsetting)
 {
 	struct btusb_data *data = hdev->driver_data;
 	struct usb_interface *intf = data->isoc;
@@ -829,7 +831,7 @@ static void btusb_work(struct work_struct *work)
 
 	if (hdev->conn_hash.sco_num > 0) {
 		if (!test_bit(BTUSB_DID_ISO_RESUME, &data->flags)) {
-			err = usb_autopm_get_interface(data->isoc);
+			err = usb_autopm_get_interface(data->isoc ? data->isoc : data->intf);
 			if (err < 0) {
 				clear_bit(BTUSB_ISOC_RUNNING, &data->flags);
 				usb_kill_anchored_urbs(&data->isoc_anchor);
@@ -858,7 +860,7 @@ static void btusb_work(struct work_struct *work)
 
 		__set_isoc_interface(hdev, 0);
 		if (test_and_clear_bit(BTUSB_DID_ISO_RESUME, &data->flags))
-			usb_autopm_put_interface(data->isoc);
+			usb_autopm_put_interface(data->isoc ? data->isoc : data->intf);
 	}
 }
 
@@ -1041,8 +1043,6 @@ static int btusb_probe(struct usb_interface *intf,
 
 	usb_set_intfdata(intf, data);
 
-	usb_enable_autosuspend(interface_to_usbdev(intf));
-
 	return 0;
 }
 
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 3c6cabcb7d84..48ad2a7ab080 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -398,6 +398,7 @@ static int hci_uart_register_dev(struct hci_uart *hu)
 	hdev->flush = hci_uart_flush;
 	hdev->send  = hci_uart_send_frame;
 	hdev->destruct = hci_uart_destruct;
+	hdev->parent = hu->tty->dev;
 
 	hdev->owner = THIS_MODULE;
 
diff --git a/drivers/char/agp/amd64-agp.c b/drivers/char/agp/amd64-agp.c
index 9252e85706ef..780498d76581 100644
--- a/drivers/char/agp/amd64-agp.c
+++ b/drivers/char/agp/amd64-agp.c
@@ -773,18 +773,23 @@ int __init agp_amd64_init(void)
 #else
 			printk(KERN_INFO PFX "You can boot with agp=try_unsupported\n");
 #endif
+			pci_unregister_driver(&agp_amd64_pci_driver);
 			return -ENODEV;
 		}
 
 		/* First check that we have at least one AMD64 NB */
-		if (!pci_dev_present(amd_nb_misc_ids))
+		if (!pci_dev_present(amd_nb_misc_ids)) {
+			pci_unregister_driver(&agp_amd64_pci_driver);
 			return -ENODEV;
+		}
 
 		/* Look for any AGP bridge */
 		agp_amd64_pci_driver.id_table = agp_amd64_pci_promisc_table;
 		err = driver_attach(&agp_amd64_pci_driver.driver);
-		if (err == 0 && agp_bridges_found == 0)
+		if (err == 0 && agp_bridges_found == 0) {
+			pci_unregister_driver(&agp_amd64_pci_driver);
 			err = -ENODEV;
+		}
 	}
 	return err;
 }
diff --git a/drivers/char/agp/intel-agp.h b/drivers/char/agp/intel-agp.h
index c195bfeade11..5feebe2800e9 100644
--- a/drivers/char/agp/intel-agp.h
+++ b/drivers/char/agp/intel-agp.h
@@ -130,6 +130,7 @@
 #define INTEL_GMCH_GMS_STOLEN_352M	(0xd << 4)
 
 #define I915_IFPADDR    0x60
+#define I830_HIC        0x70
 
 /* Intel 965G registers */
 #define I965_MSAC 0x62
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index fab3d3265adb..0d09b537bb9a 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -21,6 +21,7 @@
 #include <linux/kernel.h>
 #include <linux/pagemap.h>
 #include <linux/agp_backend.h>
+#include <linux/delay.h>
 #include <asm/smp.h>
 #include "agp.h"
 #include "intel-agp.h"
@@ -70,12 +71,8 @@ static struct _intel_private {
 	u32 __iomem *gtt;		/* I915G */
 	bool clear_fake_agp; /* on first access via agp, fill with scratch */
 	int num_dcache_entries;
-	union {
-		void __iomem *i9xx_flush_page;
-		void *i8xx_flush_page;
-	};
+	void __iomem *i9xx_flush_page;
 	char *i81x_gtt_table;
-	struct page *i8xx_page;
 	struct resource ifp_resource;
 	int resource_valid;
 	struct page *scratch_page;
@@ -722,28 +719,6 @@ static int intel_fake_agp_fetch_size(void)
 
 static void i830_cleanup(void)
 {
-	if (intel_private.i8xx_flush_page) {
-		kunmap(intel_private.i8xx_flush_page);
-		intel_private.i8xx_flush_page = NULL;
-	}
-
-	__free_page(intel_private.i8xx_page);
-	intel_private.i8xx_page = NULL;
-}
-
-static void intel_i830_setup_flush(void)
-{
-	/* return if we've already set the flush mechanism up */
-	if (intel_private.i8xx_page)
-		return;
-
-	intel_private.i8xx_page = alloc_page(GFP_KERNEL);
-	if (!intel_private.i8xx_page)
-		return;
-
-	intel_private.i8xx_flush_page = kmap(intel_private.i8xx_page);
-	if (!intel_private.i8xx_flush_page)
-		i830_cleanup();
 }
 
 /* The chipset_flush interface needs to get data that has already been
@@ -758,14 +733,27 @@ static void intel_i830_setup_flush(void)
  */
 static void i830_chipset_flush(void)
 {
-	unsigned int *pg = intel_private.i8xx_flush_page;
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	/* Forcibly evict everything from the CPU write buffers.
+	 * clflush appears to be insufficient.
+	 */
+	wbinvd_on_all_cpus();
+
+	/* Now we've only seen documents for this magic bit on 855GM,
+	 * we hope it exists for the other gen2 chipsets...
+	 *
+	 * Also works as advertised on my 845G.
+	 */
+	writel(readl(intel_private.registers+I830_HIC) | (1<<31),
+	       intel_private.registers+I830_HIC);
 
-	memset(pg, 0, 1024);
+	while (readl(intel_private.registers+I830_HIC) & (1<<31)) {
+		if (time_after(jiffies, timeout))
+			break;
 
-	if (cpu_has_clflush)
-		clflush_cache_range(pg, 1024);
-	else if (wbinvd_on_all_cpus() != 0)
-		printk(KERN_ERR "Timed out waiting for cache flush.\n");
+		udelay(50);
+	}
 }
 
 static void i830_write_entry(dma_addr_t addr, unsigned int entry,
@@ -849,8 +837,6 @@ static int i830_setup(void)
 
 	intel_private.gtt_bus_addr = reg_addr + I810_PTE_BASE;
 
-	intel_i830_setup_flush();
-
 	return 0;
 }
 
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index d31483c54883..beecd1cf9b99 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -198,3 +198,15 @@ config HW_RANDOM_NOMADIK
 	  module will be called nomadik-rng.
 
 	  If unsure, say Y.
+
+config HW_RANDOM_PICOXCELL
+	tristate "Picochip picoXcell true random number generator support"
+	depends on HW_RANDOM && ARCH_PICOXCELL && PICOXCELL_PC3X3
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on Picochip PC3x3 and later devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called picoxcell-rng.
+
+	  If unsure, say Y.
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index 4273308aa1e3..3db4eb8b19c0 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -19,3 +19,4 @@ obj-$(CONFIG_HW_RANDOM_TX4939) += tx4939-rng.o
 obj-$(CONFIG_HW_RANDOM_MXC_RNGA) += mxc-rnga.o
 obj-$(CONFIG_HW_RANDOM_OCTEON) += octeon-rng.o
 obj-$(CONFIG_HW_RANDOM_NOMADIK) += nomadik-rng.o
+obj-$(CONFIG_HW_RANDOM_PICOXCELL) += picoxcell-rng.o
diff --git a/drivers/char/hw_random/n2-drv.c b/drivers/char/hw_random/n2-drv.c
index a3f5e381e746..43ac61978d8b 100644
--- a/drivers/char/hw_random/n2-drv.c
+++ b/drivers/char/hw_random/n2-drv.c
@@ -619,15 +619,17 @@ static void __devinit n2rng_driver_version(void)
 		pr_info("%s", version);
 }
 
-static int __devinit n2rng_probe(struct platform_device *op,
-				 const struct of_device_id *match)
+static int __devinit n2rng_probe(struct platform_device *op)
 {
-	int victoria_falls = (match->data != NULL);
+	int victoria_falls;
 	int err = -ENOMEM;
 	struct n2rng *np;
 
-	n2rng_driver_version();
+	if (!op->dev.of_match)
+		return -EINVAL;
+	victoria_falls = (op->dev.of_match->data != NULL);
 
+	n2rng_driver_version();
 	np = kzalloc(sizeof(*np), GFP_KERNEL);
 	if (!np)
 		goto out;
@@ -750,7 +752,7 @@ static const struct of_device_id n2rng_match[] = {
 };
 MODULE_DEVICE_TABLE(of, n2rng_match);
 
-static struct of_platform_driver n2rng_driver = {
+static struct platform_driver n2rng_driver = {
 	.driver = {
 		.name = "n2rng",
 		.owner = THIS_MODULE,
@@ -762,12 +764,12 @@ static struct of_platform_driver n2rng_driver = {
 
 static int __init n2rng_init(void)
 {
-	return of_register_platform_driver(&n2rng_driver);
+	return platform_driver_register(&n2rng_driver);
 }
 
 static void __exit n2rng_exit(void)
 {
-	of_unregister_platform_driver(&n2rng_driver);
+	platform_driver_unregister(&n2rng_driver);
 }
 
 module_init(n2rng_init);
diff --git a/drivers/char/hw_random/nomadik-rng.c b/drivers/char/hw_random/nomadik-rng.c
index a348c7e9aa0b..dd1d143eb8ea 100644
--- a/drivers/char/hw_random/nomadik-rng.c
+++ b/drivers/char/hw_random/nomadik-rng.c
@@ -39,7 +39,7 @@ static struct hwrng nmk_rng = {
 	.read		= nmk_rng_read,
 };
 
-static int nmk_rng_probe(struct amba_device *dev, struct amba_id *id)
+static int nmk_rng_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	void __iomem *base;
 	int ret;
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index 06aad0831c73..2cc755a64302 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -91,7 +91,7 @@ static struct hwrng omap_rng_ops = {
 
 static int __devinit omap_rng_probe(struct platform_device *pdev)
 {
-	struct resource *res, *mem;
+	struct resource *res;
 	int ret;
 
 	/*
@@ -116,14 +116,12 @@ static int __devinit omap_rng_probe(struct platform_device *pdev)
 	if (!res)
 		return -ENOENT;
 
-	mem = request_mem_region(res->start, resource_size(res),
-				 pdev->name);
-	if (mem == NULL) {
+	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
 		ret = -EBUSY;
 		goto err_region;
 	}
 
-	dev_set_drvdata(&pdev->dev, mem);
+	dev_set_drvdata(&pdev->dev, res);
 	rng_base = ioremap(res->start, resource_size(res));
 	if (!rng_base) {
 		ret = -ENOMEM;
@@ -146,7 +144,7 @@ err_register:
 	iounmap(rng_base);
 	rng_base = NULL;
 err_ioremap:
-	release_resource(mem);
+	release_mem_region(res->start, resource_size(res));
 err_region:
 	if (cpu_is_omap24xx()) {
 		clk_disable(rng_ick);
@@ -157,7 +155,7 @@ err_region:
 
 static int __exit omap_rng_remove(struct platform_device *pdev)
 {
-	struct resource *mem = dev_get_drvdata(&pdev->dev);
+	struct resource *res = dev_get_drvdata(&pdev->dev);
 
 	hwrng_unregister(&omap_rng_ops);
 
@@ -170,7 +168,7 @@ static int __exit omap_rng_remove(struct platform_device *pdev)
 		clk_put(rng_ick);
 	}
 
-	release_resource(mem);
+	release_mem_region(res->start, resource_size(res));
 	rng_base = NULL;
 
 	return 0;
diff --git a/drivers/char/hw_random/pasemi-rng.c b/drivers/char/hw_random/pasemi-rng.c
index a31c830ca8cd..1d504815e6db 100644
--- a/drivers/char/hw_random/pasemi-rng.c
+++ b/drivers/char/hw_random/pasemi-rng.c
@@ -94,8 +94,7 @@ static struct hwrng pasemi_rng = {
 	.data_read	= pasemi_rng_data_read,
 };
 
-static int __devinit rng_probe(struct platform_device *ofdev,
-			       const struct of_device_id *match)
+static int __devinit rng_probe(struct platform_device *ofdev)
 {
 	void __iomem *rng_regs;
 	struct device_node *rng_np = ofdev->dev.of_node;
@@ -139,7 +138,7 @@ static struct of_device_id rng_match[] = {
 	{ },
 };
 
-static struct of_platform_driver rng_driver = {
+static struct platform_driver rng_driver = {
 	.driver = {
 		.name = "pasemi-rng",
 		.owner = THIS_MODULE,
@@ -151,13 +150,13 @@ static struct of_platform_driver rng_driver = {
 
 static int __init rng_init(void)
 {
-	return of_register_platform_driver(&rng_driver);
+	return platform_driver_register(&rng_driver);
 }
 module_init(rng_init);
 
 static void __exit rng_exit(void)
 {
-	of_unregister_platform_driver(&rng_driver);
+	platform_driver_unregister(&rng_driver);
 }
 module_exit(rng_exit);
 
diff --git a/drivers/char/hw_random/picoxcell-rng.c b/drivers/char/hw_random/picoxcell-rng.c
new file mode 100644
index 000000000000..990d55a5e3e8
--- /dev/null
+++ b/drivers/char/hw_random/picoxcell-rng.c
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) 2010-2011 Picochip Ltd., Jamie Iles
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * All enquiries to support@picochip.com
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/hw_random.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#define DATA_REG_OFFSET		0x0200
+#define CSR_REG_OFFSET		0x0278
+#define CSR_OUT_EMPTY_MASK	(1 << 24)
+#define CSR_FAULT_MASK		(1 << 1)
+#define TRNG_BLOCK_RESET_MASK	(1 << 0)
+#define TAI_REG_OFFSET		0x0380
+
+/*
+ * The maximum amount of time in microseconds to spend waiting for data if the
+ * core wants us to wait.  The TRNG should generate 32 bits every 320ns so a
+ * timeout of 20us seems reasonable.  The TRNG does builtin tests of the data
+ * for randomness so we can't always assume there is data present.
+ */
+#define PICO_TRNG_TIMEOUT		20
+
+static void __iomem *rng_base;
+static struct clk *rng_clk;
+struct device *rng_dev;
+
+static inline u32 picoxcell_trng_read_csr(void)
+{
+	return __raw_readl(rng_base + CSR_REG_OFFSET);
+}
+
+static inline bool picoxcell_trng_is_empty(void)
+{
+	return picoxcell_trng_read_csr() & CSR_OUT_EMPTY_MASK;
+}
+
+/*
+ * Take the random number generator out of reset and make sure the interrupts
+ * are masked. We shouldn't need to get large amounts of random bytes so just
+ * poll the status register. The hardware generates 32 bits every 320ns so we
+ * shouldn't have to wait long enough to warrant waiting for an IRQ.
+ */
+static void picoxcell_trng_start(void)
+{
+	__raw_writel(0, rng_base + TAI_REG_OFFSET);
+	__raw_writel(0, rng_base + CSR_REG_OFFSET);
+}
+
+static void picoxcell_trng_reset(void)
+{
+	__raw_writel(TRNG_BLOCK_RESET_MASK, rng_base + CSR_REG_OFFSET);
+	__raw_writel(TRNG_BLOCK_RESET_MASK, rng_base + TAI_REG_OFFSET);
+	picoxcell_trng_start();
+}
+
+/*
+ * Get some random data from the random number generator. The hw_random core
+ * layer provides us with locking.
+ */
+static int picoxcell_trng_read(struct hwrng *rng, void *buf, size_t max,
+			       bool wait)
+{
+	int i;
+
+	/* Wait for some data to become available. */
+	for (i = 0; i < PICO_TRNG_TIMEOUT && picoxcell_trng_is_empty(); ++i) {
+		if (!wait)
+			return 0;
+
+		udelay(1);
+	}
+
+	if (picoxcell_trng_read_csr() & CSR_FAULT_MASK) {
+		dev_err(rng_dev, "fault detected, resetting TRNG\n");
+		picoxcell_trng_reset();
+		return -EIO;
+	}
+
+	if (i == PICO_TRNG_TIMEOUT)
+		return 0;
+
+	*(u32 *)buf = __raw_readl(rng_base + DATA_REG_OFFSET);
+	return sizeof(u32);
+}
+
+static struct hwrng picoxcell_trng = {
+	.name		= "picoxcell",
+	.read		= picoxcell_trng_read,
+};
+
+static int picoxcell_trng_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct resource *mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!mem) {
+		dev_warn(&pdev->dev, "no memory resource\n");
+		return -ENOMEM;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, mem->start, resource_size(mem),
+				     "picoxcell_trng")) {
+		dev_warn(&pdev->dev, "unable to request io mem\n");
+		return -EBUSY;
+	}
+
+	rng_base = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
+	if (!rng_base) {
+		dev_warn(&pdev->dev, "unable to remap io mem\n");
+		return -ENOMEM;
+	}
+
+	rng_clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(rng_clk)) {
+		dev_warn(&pdev->dev, "no clk\n");
+		return PTR_ERR(rng_clk);
+	}
+
+	ret = clk_enable(rng_clk);
+	if (ret) {
+		dev_warn(&pdev->dev, "unable to enable clk\n");
+		goto err_enable;
+	}
+
+	picoxcell_trng_start();
+	ret = hwrng_register(&picoxcell_trng);
+	if (ret)
+		goto err_register;
+
+	rng_dev = &pdev->dev;
+	dev_info(&pdev->dev, "pixoxcell random number generator active\n");
+
+	return 0;
+
+err_register:
+	clk_disable(rng_clk);
+err_enable:
+	clk_put(rng_clk);
+
+	return ret;
+}
+
+static int __devexit picoxcell_trng_remove(struct platform_device *pdev)
+{
+	hwrng_unregister(&picoxcell_trng);
+	clk_disable(rng_clk);
+	clk_put(rng_clk);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int picoxcell_trng_suspend(struct device *dev)
+{
+	clk_disable(rng_clk);
+
+	return 0;
+}
+
+static int picoxcell_trng_resume(struct device *dev)
+{
+	return clk_enable(rng_clk);
+}
+
+static const struct dev_pm_ops picoxcell_trng_pm_ops = {
+	.suspend	= picoxcell_trng_suspend,
+	.resume		= picoxcell_trng_resume,
+};
+#endif /* CONFIG_PM */
+
+static struct platform_driver picoxcell_trng_driver = {
+	.probe		= picoxcell_trng_probe,
+	.remove		= __devexit_p(picoxcell_trng_remove),
+	.driver		= {
+		.name	= "picoxcell-trng",
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &picoxcell_trng_pm_ops,
+#endif /* CONFIG_PM */
+	},
+};
+
+static int __init picoxcell_trng_init(void)
+{
+	return platform_driver_register(&picoxcell_trng_driver);
+}
+module_init(picoxcell_trng_init);
+
+static void __exit picoxcell_trng_exit(void)
+{
+	platform_driver_unregister(&picoxcell_trng_driver);
+}
+module_exit(picoxcell_trng_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jamie Iles");
+MODULE_DESCRIPTION("Picochip picoXcell TRNG driver");
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 7855f9f45b8e..dcfc39ca753e 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -66,13 +66,10 @@
 #include <linux/string.h>
 #include <linux/ctype.h>
 #include <linux/pnp.h>
-
-#ifdef CONFIG_PPC_OF
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
-#endif
 
 #define PFX "ipmi_si: "
 
@@ -116,13 +113,7 @@ static char *ipmi_addr_src_to_str[] = { NULL, "hotmod", "hardcoded", "SPMI",
 
 #define DEVICE_NAME "ipmi_si"
 
-static struct platform_driver ipmi_driver = {
-	.driver = {
-		.name = DEVICE_NAME,
-		.bus = &platform_bus_type
-	}
-};
-
+static struct platform_driver ipmi_driver;
 
 /*
  * Indexes into stats[] in smi_info below.
@@ -308,9 +299,6 @@ static int pci_registered;
 #ifdef CONFIG_ACPI
 static int pnp_registered;
 #endif
-#ifdef CONFIG_PPC_OF
-static int of_registered;
-#endif
 
 static unsigned int kipmid_max_busy_us[SI_MAX_PARMS];
 static int num_max_busy_us;
@@ -1860,8 +1848,9 @@ static int hotmod_handler(const char *val, struct kernel_param *kp)
 	return rv;
 }
 
-static void __devinit hardcode_find_bmc(void)
+static int __devinit hardcode_find_bmc(void)
 {
+	int ret = -ENODEV;
 	int             i;
 	struct smi_info *info;
 
@@ -1871,7 +1860,7 @@ static void __devinit hardcode_find_bmc(void)
 
 		info = smi_info_alloc();
 		if (!info)
-			return;
+			return -ENOMEM;
 
 		info->addr_source = SI_HARDCODED;
 		printk(KERN_INFO PFX "probing via hardcoded address\n");
@@ -1924,10 +1913,12 @@ static void __devinit hardcode_find_bmc(void)
 		if (!add_smi(info)) {
 			if (try_smi_init(info))
 				cleanup_one_si(info);
+			ret = 0;
 		} else {
 			kfree(info);
 		}
 	}
+	return ret;
 }
 
 #ifdef CONFIG_ACPI
@@ -2555,11 +2546,9 @@ static struct pci_driver ipmi_pci_driver = {
 };
 #endif /* CONFIG_PCI */
 
-
-#ifdef CONFIG_PPC_OF
-static int __devinit ipmi_of_probe(struct platform_device *dev,
-			 const struct of_device_id *match)
+static int __devinit ipmi_probe(struct platform_device *dev)
 {
+#ifdef CONFIG_OF
 	struct smi_info *info;
 	struct resource resource;
 	const __be32 *regsize, *regspacing, *regshift;
@@ -2569,6 +2558,9 @@ static int __devinit ipmi_of_probe(struct platform_device *dev,
 
 	dev_info(&dev->dev, "probing via device tree\n");
 
+	if (!dev->dev.of_match)
+		return -EINVAL;
+
 	ret = of_address_to_resource(np, 0, &resource);
 	if (ret) {
 		dev_warn(&dev->dev, PFX "invalid address from OF\n");
@@ -2601,7 +2593,7 @@ static int __devinit ipmi_of_probe(struct platform_device *dev,
 		return -ENOMEM;
 	}
 
-	info->si_type		= (enum si_type) match->data;
+	info->si_type		= (enum si_type) dev->dev.of_match->data;
 	info->addr_source	= SI_DEVICETREE;
 	info->irq_setup		= std_irq_setup;
 
@@ -2632,13 +2624,15 @@ static int __devinit ipmi_of_probe(struct platform_device *dev,
 		kfree(info);
 		return -EBUSY;
 	}
-
+#endif
 	return 0;
 }
 
-static int __devexit ipmi_of_remove(struct platform_device *dev)
+static int __devexit ipmi_remove(struct platform_device *dev)
 {
+#ifdef CONFIG_OF
 	cleanup_one_si(dev_get_drvdata(&dev->dev));
+#endif
 	return 0;
 }
 
@@ -2653,16 +2647,15 @@ static struct of_device_id ipmi_match[] =
 	{},
 };
 
-static struct of_platform_driver ipmi_of_platform_driver = {
+static struct platform_driver ipmi_driver = {
 	.driver = {
-		.name = "ipmi",
+		.name = DEVICE_NAME,
 		.owner = THIS_MODULE,
 		.of_match_table = ipmi_match,
 	},
-	.probe		= ipmi_of_probe,
-	.remove		= __devexit_p(ipmi_of_remove),
+	.probe		= ipmi_probe,
+	.remove		= __devexit_p(ipmi_remove),
 };
-#endif /* CONFIG_PPC_OF */
 
 static int wait_for_msg_done(struct smi_info *smi_info)
 {
@@ -3340,8 +3333,7 @@ static int __devinit init_ipmi_si(void)
 		return 0;
 	initialized = 1;
 
-	/* Register the device drivers. */
-	rv = driver_register(&ipmi_driver.driver);
+	rv = platform_driver_register(&ipmi_driver);
 	if (rv) {
 		printk(KERN_ERR PFX "Unable to register driver: %d\n", rv);
 		return rv;
@@ -3365,15 +3357,9 @@ static int __devinit init_ipmi_si(void)
 
 	printk(KERN_INFO "IPMI System Interface driver.\n");
 
-	hardcode_find_bmc();
-
 	/* If the user gave us a device, they presumably want us to use it */
-	mutex_lock(&smi_infos_lock);
-	if (!list_empty(&smi_infos)) {
-		mutex_unlock(&smi_infos_lock);
+	if (!hardcode_find_bmc())
 		return 0;
-	}
-	mutex_unlock(&smi_infos_lock);
 
 #ifdef CONFIG_PCI
 	rv = pci_register_driver(&ipmi_pci_driver);
@@ -3396,11 +3382,6 @@ static int __devinit init_ipmi_si(void)
 	spmi_find_bmc();
 #endif
 
-#ifdef CONFIG_PPC_OF
-	of_register_platform_driver(&ipmi_of_platform_driver);
-	of_registered = 1;
-#endif
-
 	/* We prefer devices with interrupts, but in the case of a machine
 	   with multiple BMCs we assume that there will be several instances
 	   of a given type so if we succeed in registering a type then also
@@ -3548,17 +3529,12 @@ static void __exit cleanup_ipmi_si(void)
 		pnp_unregister_driver(&ipmi_pnp_driver);
 #endif
 
-#ifdef CONFIG_PPC_OF
-	if (of_registered)
-		of_unregister_platform_driver(&ipmi_of_platform_driver);
-#endif
+	platform_driver_unregister(&ipmi_driver);
 
 	mutex_lock(&smi_infos_lock);
 	list_for_each_entry_safe(e, tmp_e, &smi_infos, link)
 		cleanup_one_si(e);
 	mutex_unlock(&smi_infos_lock);
-
-	driver_unregister(&ipmi_driver.driver);
 }
 module_exit(cleanup_ipmi_si);
 
diff --git a/drivers/char/mmtimer.c b/drivers/char/mmtimer.c
index e6d75627c6c8..33dc2298af73 100644
--- a/drivers/char/mmtimer.c
+++ b/drivers/char/mmtimer.c
@@ -53,6 +53,8 @@ MODULE_LICENSE("GPL");
 
 #define RTC_BITS 55 /* 55 bits for this implementation */
 
+static struct k_clock sgi_clock;
+
 extern unsigned long sn_rtc_cycles_per_second;
 
 #define RTC_COUNTER_ADDR        ((long *)LOCAL_MMR_ADDR(SH_RTC))
@@ -487,7 +489,7 @@ static int sgi_clock_get(clockid_t clockid, struct timespec *tp)
 	return 0;
 };
 
-static int sgi_clock_set(clockid_t clockid, struct timespec *tp)
+static int sgi_clock_set(const clockid_t clockid, const struct timespec *tp)
 {
 
 	u64 nsec;
@@ -763,15 +765,21 @@ static int sgi_timer_set(struct k_itimer *timr, int flags,
 	return err;
 }
 
+static int sgi_clock_getres(const clockid_t which_clock, struct timespec *tp)
+{
+	tp->tv_sec = 0;
+	tp->tv_nsec = sgi_clock_period;
+	return 0;
+}
+
 static struct k_clock sgi_clock = {
-	.res = 0,
-	.clock_set = sgi_clock_set,
-	.clock_get = sgi_clock_get,
-	.timer_create = sgi_timer_create,
-	.nsleep = do_posix_clock_nonanosleep,
-	.timer_set = sgi_timer_set,
-	.timer_del = sgi_timer_del,
-	.timer_get = sgi_timer_get
+	.clock_set	= sgi_clock_set,
+	.clock_get	= sgi_clock_get,
+	.clock_getres	= sgi_clock_getres,
+	.timer_create	= sgi_timer_create,
+	.timer_set	= sgi_timer_set,
+	.timer_del	= sgi_timer_del,
+	.timer_get	= sgi_timer_get
 };
 
 /**
@@ -831,8 +839,8 @@ static int __init mmtimer_init(void)
 			(unsigned long) node);
 	}
 
-	sgi_clock_period = sgi_clock.res = NSEC_PER_SEC / sn_rtc_cycles_per_second;
-	register_posix_clock(CLOCK_SGI_CYCLE, &sgi_clock);
+	sgi_clock_period = NSEC_PER_SEC / sn_rtc_cycles_per_second;
+	posix_timers_register_clock(CLOCK_SGI_CYCLE, &sgi_clock);
 
 	printk(KERN_INFO "%s: v%s, %ld MHz\n", MMTIMER_DESC, MMTIMER_VERSION,
 	       sn_rtc_cycles_per_second/(unsigned long)1E6);
diff --git a/drivers/char/pcmcia/cm4000_cs.c b/drivers/char/pcmcia/cm4000_cs.c
index 777181a2e603..bcbbc71febb7 100644
--- a/drivers/char/pcmcia/cm4000_cs.c
+++ b/drivers/char/pcmcia/cm4000_cs.c
@@ -830,8 +830,7 @@ static void monitor_card(unsigned long p)
 			    test_bit(IS_ANY_T1, &dev->flags))) {
 				DEBUGP(4, dev, "Perform AUTOPPS\n");
 				set_bit(IS_AUTOPPS_ACT, &dev->flags);
-				ptsreq.protocol = ptsreq.protocol =
-				    (0x01 << dev->proto);
+				ptsreq.protocol = (0x01 << dev->proto);
 				ptsreq.flags = 0x01;
 				ptsreq.pts1 = 0x00;
 				ptsreq.pts2 = 0x00;
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 72a4fcb17745..5e29e8031bbc 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -128,6 +128,7 @@
  * 	void add_input_randomness(unsigned int type, unsigned int code,
  *                                unsigned int value);
  * 	void add_interrupt_randomness(int irq);
+ * 	void add_disk_randomness(struct gendisk *disk);
  *
  * add_input_randomness() uses the input layer interrupt timing, as well as
  * the event type information from the hardware.
@@ -136,9 +137,15 @@
  * inputs to the entropy pool.  Note that not all interrupts are good
  * sources of randomness!  For example, the timer interrupts is not a
  * good choice, because the periodicity of the interrupts is too
- * regular, and hence predictable to an attacker.  Disk interrupts are
- * a better measure, since the timing of the disk interrupts are more
- * unpredictable.
+ * regular, and hence predictable to an attacker.  Network Interface
+ * Controller interrupts are a better measure, since the timing of the
+ * NIC interrupts are more unpredictable.
+ *
+ * add_disk_randomness() uses what amounts to the seek time of block
+ * layer request events, on a per-disk_devt basis, as input to the
+ * entropy pool. Note that high-speed solid state drives with very low
+ * seek times do not make for good sources of entropy, as their seek
+ * times are usually fairly consistent.
  *
  * All of these routines try to estimate how many bits of randomness a
  * particular randomness source.  They do this by keeping track of the
diff --git a/drivers/char/tpm/tpm.c b/drivers/char/tpm/tpm.c
index 36e0fa161c2b..1f46f1cd9225 100644
--- a/drivers/char/tpm/tpm.c
+++ b/drivers/char/tpm/tpm.c
@@ -364,14 +364,12 @@ unsigned long tpm_calc_ordinal_duration(struct tpm_chip *chip,
 		    tpm_protected_ordinal_duration[ordinal &
 						   TPM_PROTECTED_ORDINAL_MASK];
 
-	if (duration_idx != TPM_UNDEFINED) {
+	if (duration_idx != TPM_UNDEFINED)
 		duration = chip->vendor.duration[duration_idx];
-		/* if duration is 0, it's because chip->vendor.duration wasn't */
-		/* filled yet, so we set the lowest timeout just to give enough */
-		/* time for tpm_get_timeouts() to succeed */
-		return (duration <= 0 ? HZ : duration);
-	} else
+	if (duration <= 0)
 		return 2 * 60 * HZ;
+	else
+		return duration;
 }
 EXPORT_SYMBOL_GPL(tpm_calc_ordinal_duration);
 
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index 490393186338..84b164d1eb2b 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -388,6 +388,10 @@ static void discard_port_data(struct port *port)
 	unsigned int len;
 	int ret;
 
+	if (!port->portdev) {
+		/* Device has been unplugged.  vqs are already gone. */
+		return;
+	}
 	vq = port->in_vq;
 	if (port->inbuf)
 		buf = port->inbuf;
@@ -470,6 +474,10 @@ static void reclaim_consumed_buffers(struct port *port)
 	void *buf;
 	unsigned int len;
 
+	if (!port->portdev) {
+		/* Device has been unplugged.  vqs are already gone. */
+		return;
+	}
 	while ((buf = virtqueue_get_buf(port->out_vq, &len))) {
 		kfree(buf);
 		port->outvq_full = false;
diff --git a/drivers/char/xilinx_hwicap/xilinx_hwicap.c b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
index 9f2272e6de1c..d3c9d755ed98 100644
--- a/drivers/char/xilinx_hwicap/xilinx_hwicap.c
+++ b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
@@ -714,20 +714,29 @@ static int __devexit hwicap_remove(struct device *dev)
 	return 0;		/* success */
 }
 
-static int __devinit hwicap_drv_probe(struct platform_device *pdev)
+#ifdef CONFIG_OF
+static int __devinit hwicap_of_probe(struct platform_device *op)
 {
-	struct resource *res;
-	const struct config_registers *regs;
+	struct resource res;
+	const unsigned int *id;
 	const char *family;
+	int rc;
+	const struct hwicap_driver_config *config = op->dev.of_match->data;
+	const struct config_registers *regs;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
+
+	rc = of_address_to_resource(op->dev.of_node, 0, &res);
+	if (rc) {
+		dev_err(&op->dev, "invalid address\n");
+		return rc;
+	}
+
+	id = of_get_property(op->dev.of_node, "port-number", NULL);
 
 	/* It's most likely that we're using V4, if the family is not
 	   specified */
 	regs = &v4_config_registers;
-	family = pdev->dev.platform_data;
+	family = of_get_property(op->dev.of_node, "xlnx,family", NULL);
 
 	if (family) {
 		if (!strcmp(family, "virtex2p")) {
@@ -738,54 +747,33 @@ static int __devinit hwicap_drv_probe(struct platform_device *pdev)
 			regs = &v5_config_registers;
 		}
 	}
-
-	return hwicap_setup(&pdev->dev, pdev->id, res,
-			&buffer_icap_config, regs);
+	return hwicap_setup(&op->dev, id ? *id : -1, &res, config,
+			regs);
 }
-
-static int __devexit hwicap_drv_remove(struct platform_device *pdev)
+#else
+static inline int hwicap_of_probe(struct platform_device *op)
 {
-	return hwicap_remove(&pdev->dev);
+	return -EINVAL;
 }
+#endif /* CONFIG_OF */
 
-static struct platform_driver hwicap_platform_driver = {
-	.probe = hwicap_drv_probe,
-	.remove = hwicap_drv_remove,
-	.driver = {
-		.owner = THIS_MODULE,
-		.name = DRIVER_NAME,
-	},
-};
-
-/* ---------------------------------------------------------------------
- * OF bus binding
- */
-
-#if defined(CONFIG_OF)
-static int __devinit
-hwicap_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit hwicap_drv_probe(struct platform_device *pdev)
 {
-	struct resource res;
-	const unsigned int *id;
-	const char *family;
-	int rc;
-	const struct hwicap_driver_config *config = match->data;
+	struct resource *res;
 	const struct config_registers *regs;
+	const char *family;
 
-	dev_dbg(&op->dev, "hwicap_of_probe(%p, %p)\n", op, match);
-
-	rc = of_address_to_resource(op->dev.of_node, 0, &res);
-	if (rc) {
-		dev_err(&op->dev, "invalid address\n");
-		return rc;
-	}
+	if (pdev->dev.of_match)
+		return hwicap_of_probe(pdev);
 
-	id = of_get_property(op->dev.of_node, "port-number", NULL);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
 
 	/* It's most likely that we're using V4, if the family is not
 	   specified */
 	regs = &v4_config_registers;
-	family = of_get_property(op->dev.of_node, "xlnx,family", NULL);
+	family = pdev->dev.platform_data;
 
 	if (family) {
 		if (!strcmp(family, "virtex2p")) {
@@ -796,50 +784,38 @@ hwicap_of_probe(struct platform_device *op, const struct of_device_id *match)
 			regs = &v5_config_registers;
 		}
 	}
-	return hwicap_setup(&op->dev, id ? *id : -1, &res, config,
-			regs);
+
+	return hwicap_setup(&pdev->dev, pdev->id, res,
+			&buffer_icap_config, regs);
 }
 
-static int __devexit hwicap_of_remove(struct platform_device *op)
+static int __devexit hwicap_drv_remove(struct platform_device *pdev)
 {
-	return hwicap_remove(&op->dev);
+	return hwicap_remove(&pdev->dev);
 }
 
-/* Match table for of_platform binding */
+#ifdef CONFIG_OF
+/* Match table for device tree binding */
 static const struct of_device_id __devinitconst hwicap_of_match[] = {
 	{ .compatible = "xlnx,opb-hwicap-1.00.b", .data = &buffer_icap_config},
 	{ .compatible = "xlnx,xps-hwicap-1.00.a", .data = &fifo_icap_config},
 	{},
 };
 MODULE_DEVICE_TABLE(of, hwicap_of_match);
+#else
+#define hwicap_of_match NULL
+#endif
 
-static struct of_platform_driver hwicap_of_driver = {
-	.probe = hwicap_of_probe,
-	.remove = __devexit_p(hwicap_of_remove),
+static struct platform_driver hwicap_platform_driver = {
+	.probe = hwicap_drv_probe,
+	.remove = hwicap_drv_remove,
 	.driver = {
-		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
+		.name = DRIVER_NAME,
 		.of_match_table = hwicap_of_match,
 	},
 };
 
-/* Registration helpers to keep the number of #ifdefs to a minimum */
-static inline int __init hwicap_of_register(void)
-{
-	pr_debug("hwicap: calling of_register_platform_driver()\n");
-	return of_register_platform_driver(&hwicap_of_driver);
-}
-
-static inline void __exit hwicap_of_unregister(void)
-{
-	of_unregister_platform_driver(&hwicap_of_driver);
-}
-#else /* CONFIG_OF */
-/* CONFIG_OF not enabled; do nothing helpers */
-static inline int __init hwicap_of_register(void) { return 0; }
-static inline void __exit hwicap_of_unregister(void) { }
-#endif /* CONFIG_OF */
-
 static int __init hwicap_module_init(void)
 {
 	dev_t devt;
@@ -856,21 +832,12 @@ static int __init hwicap_module_init(void)
 		return retval;
 
 	retval = platform_driver_register(&hwicap_platform_driver);
-
-	if (retval)
-		goto failed1;
-
-	retval = hwicap_of_register();
-
 	if (retval)
-		goto failed2;
+		goto failed;
 
 	return retval;
 
- failed2:
-	platform_driver_unregister(&hwicap_platform_driver);
-
- failed1:
+ failed:
 	unregister_chrdev_region(devt, HWICAP_DEVICES);
 
 	return retval;
@@ -884,8 +851,6 @@ static void __exit hwicap_module_cleanup(void)
 
 	platform_driver_unregister(&hwicap_platform_driver);
 
-	hwicap_of_unregister();
-
 	unregister_chrdev_region(devt, HWICAP_DEVICES);
 }
 
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 1109f6848a43..5cb4d09919d6 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -1919,8 +1919,10 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 
 	ret = sysdev_driver_register(&cpu_sysdev_class,
 					&cpufreq_sysdev_driver);
+	if (ret)
+		goto err_null_driver;
 
-	if ((!ret) && !(cpufreq_driver->flags & CPUFREQ_STICKY)) {
+	if (!(cpufreq_driver->flags & CPUFREQ_STICKY)) {
 		int i;
 		ret = -ENODEV;
 
@@ -1935,21 +1937,22 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 		if (ret) {
 			dprintk("no CPU initialized for driver %s\n",
 							driver_data->name);
-			sysdev_driver_unregister(&cpu_sysdev_class,
-						&cpufreq_sysdev_driver);
-
-			spin_lock_irqsave(&cpufreq_driver_lock, flags);
-			cpufreq_driver = NULL;
-			spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+			goto err_sysdev_unreg;
 		}
 	}
 
-	if (!ret) {
-		register_hotcpu_notifier(&cpufreq_cpu_notifier);
-		dprintk("driver %s up and running\n", driver_data->name);
-		cpufreq_debug_enable_ratelimit();
-	}
+	register_hotcpu_notifier(&cpufreq_cpu_notifier);
+	dprintk("driver %s up and running\n", driver_data->name);
+	cpufreq_debug_enable_ratelimit();
 
+	return 0;
+err_sysdev_unreg:
+	sysdev_driver_unregister(&cpu_sysdev_class,
+			&cpufreq_sysdev_driver);
+err_null_driver:
+	spin_lock_irqsave(&cpufreq_driver_lock, flags);
+	cpufreq_driver = NULL;
+	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cpufreq_register_driver);
diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 526bfbf69611..33b56e5c5c14 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -76,13 +76,10 @@ static DEFINE_PER_CPU(struct cpu_dbs_info_s, cs_cpu_dbs_info);
 static unsigned int dbs_enable;	/* number of CPUs using this policy */
 
 /*
- * dbs_mutex protects data in dbs_tuners_ins from concurrent changes on
- * different CPUs. It protects dbs_enable in governor start/stop.
+ * dbs_mutex protects dbs_enable in governor start/stop.
  */
 static DEFINE_MUTEX(dbs_mutex);
 
-static struct workqueue_struct	*kconservative_wq;
-
 static struct dbs_tuners {
 	unsigned int sampling_rate;
 	unsigned int sampling_down_factor;
@@ -118,7 +115,7 @@ static inline cputime64_t get_cpu_idle_time_jiffy(unsigned int cpu,
 	if (wall)
 		*wall = (cputime64_t)jiffies_to_usecs(cur_wall_time);
 
-	return (cputime64_t)jiffies_to_usecs(idle_time);;
+	return (cputime64_t)jiffies_to_usecs(idle_time);
 }
 
 static inline cputime64_t get_cpu_idle_time(unsigned int cpu, cputime64_t *wall)
@@ -164,21 +161,12 @@ static struct notifier_block dbs_cpufreq_notifier_block = {
 };
 
 /************************** sysfs interface ************************/
-static ssize_t show_sampling_rate_max(struct kobject *kobj,
-				      struct attribute *attr, char *buf)
-{
-	printk_once(KERN_INFO "CPUFREQ: conservative sampling_rate_max "
-		    "sysfs file is deprecated - used by: %s\n", current->comm);
-	return sprintf(buf, "%u\n", -1U);
-}
-
 static ssize_t show_sampling_rate_min(struct kobject *kobj,
 				      struct attribute *attr, char *buf)
 {
 	return sprintf(buf, "%u\n", min_sampling_rate);
 }
 
-define_one_global_ro(sampling_rate_max);
 define_one_global_ro(sampling_rate_min);
 
 /* cpufreq_conservative Governor Tunables */
@@ -195,33 +183,6 @@ show_one(down_threshold, down_threshold);
 show_one(ignore_nice_load, ignore_nice);
 show_one(freq_step, freq_step);
 
-/*** delete after deprecation time ***/
-#define DEPRECATION_MSG(file_name)					\
-	printk_once(KERN_INFO "CPUFREQ: Per core conservative sysfs "	\
-		"interface is deprecated - " #file_name "\n");
-
-#define show_one_old(file_name)						\
-static ssize_t show_##file_name##_old					\
-(struct cpufreq_policy *unused, char *buf)				\
-{									\
-	printk_once(KERN_INFO "CPUFREQ: Per core conservative sysfs "	\
-		"interface is deprecated - " #file_name "\n");		\
-	return show_##file_name(NULL, NULL, buf);			\
-}
-show_one_old(sampling_rate);
-show_one_old(sampling_down_factor);
-show_one_old(up_threshold);
-show_one_old(down_threshold);
-show_one_old(ignore_nice_load);
-show_one_old(freq_step);
-show_one_old(sampling_rate_min);
-show_one_old(sampling_rate_max);
-
-cpufreq_freq_attr_ro_old(sampling_rate_min);
-cpufreq_freq_attr_ro_old(sampling_rate_max);
-
-/*** delete after deprecation time ***/
-
 static ssize_t store_sampling_down_factor(struct kobject *a,
 					  struct attribute *b,
 					  const char *buf, size_t count)
@@ -233,10 +194,7 @@ static ssize_t store_sampling_down_factor(struct kobject *a,
 	if (ret != 1 || input > MAX_SAMPLING_DOWN_FACTOR || input < 1)
 		return -EINVAL;
 
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.sampling_down_factor = input;
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -250,10 +208,7 @@ static ssize_t store_sampling_rate(struct kobject *a, struct attribute *b,
 	if (ret != 1)
 		return -EINVAL;
 
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.sampling_rate = max(input, min_sampling_rate);
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -264,16 +219,11 @@ static ssize_t store_up_threshold(struct kobject *a, struct attribute *b,
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 
-	mutex_lock(&dbs_mutex);
 	if (ret != 1 || input > 100 ||
-			input <= dbs_tuners_ins.down_threshold) {
-		mutex_unlock(&dbs_mutex);
+			input <= dbs_tuners_ins.down_threshold)
 		return -EINVAL;
-	}
 
 	dbs_tuners_ins.up_threshold = input;
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -284,17 +234,12 @@ static ssize_t store_down_threshold(struct kobject *a, struct attribute *b,
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 
-	mutex_lock(&dbs_mutex);
 	/* cannot be lower than 11 otherwise freq will not fall */
 	if (ret != 1 || input < 11 || input > 100 ||
-			input >= dbs_tuners_ins.up_threshold) {
-		mutex_unlock(&dbs_mutex);
+			input >= dbs_tuners_ins.up_threshold)
 		return -EINVAL;
-	}
 
 	dbs_tuners_ins.down_threshold = input;
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -313,11 +258,9 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 	if (input > 1)
 		input = 1;
 
-	mutex_lock(&dbs_mutex);
-	if (input == dbs_tuners_ins.ignore_nice) { /* nothing to do */
-		mutex_unlock(&dbs_mutex);
+	if (input == dbs_tuners_ins.ignore_nice) /* nothing to do */
 		return count;
-	}
+
 	dbs_tuners_ins.ignore_nice = input;
 
 	/* we need to re-evaluate prev_cpu_idle */
@@ -329,8 +272,6 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 		if (dbs_tuners_ins.ignore_nice)
 			dbs_info->prev_cpu_nice = kstat_cpu(j).cpustat.nice;
 	}
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -349,10 +290,7 @@ static ssize_t store_freq_step(struct kobject *a, struct attribute *b,
 
 	/* no need to test here if freq_step is zero as the user might actually
 	 * want this, they would be crazy though :) */
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.freq_step = input;
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -364,7 +302,6 @@ define_one_global_rw(ignore_nice_load);
 define_one_global_rw(freq_step);
 
 static struct attribute *dbs_attributes[] = {
-	&sampling_rate_max.attr,
 	&sampling_rate_min.attr,
 	&sampling_rate.attr,
 	&sampling_down_factor.attr,
@@ -380,49 +317,6 @@ static struct attribute_group dbs_attr_group = {
 	.name = "conservative",
 };
 
-/*** delete after deprecation time ***/
-
-#define write_one_old(file_name)					\
-static ssize_t store_##file_name##_old					\
-(struct cpufreq_policy *unused, const char *buf, size_t count)		\
-{									\
-	printk_once(KERN_INFO "CPUFREQ: Per core conservative sysfs "	\
-		"interface is deprecated - " #file_name "\n");	\
-	return store_##file_name(NULL, NULL, buf, count);		\
-}
-write_one_old(sampling_rate);
-write_one_old(sampling_down_factor);
-write_one_old(up_threshold);
-write_one_old(down_threshold);
-write_one_old(ignore_nice_load);
-write_one_old(freq_step);
-
-cpufreq_freq_attr_rw_old(sampling_rate);
-cpufreq_freq_attr_rw_old(sampling_down_factor);
-cpufreq_freq_attr_rw_old(up_threshold);
-cpufreq_freq_attr_rw_old(down_threshold);
-cpufreq_freq_attr_rw_old(ignore_nice_load);
-cpufreq_freq_attr_rw_old(freq_step);
-
-static struct attribute *dbs_attributes_old[] = {
-	&sampling_rate_max_old.attr,
-	&sampling_rate_min_old.attr,
-	&sampling_rate_old.attr,
-	&sampling_down_factor_old.attr,
-	&up_threshold_old.attr,
-	&down_threshold_old.attr,
-	&ignore_nice_load_old.attr,
-	&freq_step_old.attr,
-	NULL
-};
-
-static struct attribute_group dbs_attr_group_old = {
-	.attrs = dbs_attributes_old,
-	.name = "conservative",
-};
-
-/*** delete after deprecation time ***/
-
 /************************** sysfs end ************************/
 
 static void dbs_check_cpu(struct cpu_dbs_info_s *this_dbs_info)
@@ -560,7 +454,7 @@ static void do_dbs_timer(struct work_struct *work)
 
 	dbs_check_cpu(dbs_info);
 
-	queue_delayed_work_on(cpu, kconservative_wq, &dbs_info->work, delay);
+	schedule_delayed_work_on(cpu, &dbs_info->work, delay);
 	mutex_unlock(&dbs_info->timer_mutex);
 }
 
@@ -572,8 +466,7 @@ static inline void dbs_timer_init(struct cpu_dbs_info_s *dbs_info)
 
 	dbs_info->enable = 1;
 	INIT_DELAYED_WORK_DEFERRABLE(&dbs_info->work, do_dbs_timer);
-	queue_delayed_work_on(dbs_info->cpu, kconservative_wq, &dbs_info->work,
-				delay);
+	schedule_delayed_work_on(dbs_info->cpu, &dbs_info->work, delay);
 }
 
 static inline void dbs_timer_exit(struct cpu_dbs_info_s *dbs_info)
@@ -599,12 +492,6 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 
 		mutex_lock(&dbs_mutex);
 
-		rc = sysfs_create_group(&policy->kobj, &dbs_attr_group_old);
-		if (rc) {
-			mutex_unlock(&dbs_mutex);
-			return rc;
-		}
-
 		for_each_cpu(j, policy->cpus) {
 			struct cpu_dbs_info_s *j_dbs_info;
 			j_dbs_info = &per_cpu(cs_cpu_dbs_info, j);
@@ -667,7 +554,6 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		dbs_timer_exit(this_dbs_info);
 
 		mutex_lock(&dbs_mutex);
-		sysfs_remove_group(&policy->kobj, &dbs_attr_group_old);
 		dbs_enable--;
 		mutex_destroy(&this_dbs_info->timer_mutex);
 
@@ -716,25 +602,12 @@ struct cpufreq_governor cpufreq_gov_conservative = {
 
 static int __init cpufreq_gov_dbs_init(void)
 {
-	int err;
-
-	kconservative_wq = create_workqueue("kconservative");
-	if (!kconservative_wq) {
-		printk(KERN_ERR "Creation of kconservative failed\n");
-		return -EFAULT;
-	}
-
-	err = cpufreq_register_governor(&cpufreq_gov_conservative);
-	if (err)
-		destroy_workqueue(kconservative_wq);
-
-	return err;
+	return cpufreq_register_governor(&cpufreq_gov_conservative);
 }
 
 static void __exit cpufreq_gov_dbs_exit(void)
 {
 	cpufreq_unregister_governor(&cpufreq_gov_conservative);
-	destroy_workqueue(kconservative_wq);
 }
 
 
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index c631f27a3dcc..891360edecdd 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -99,13 +99,10 @@ static DEFINE_PER_CPU(struct cpu_dbs_info_s, od_cpu_dbs_info);
 static unsigned int dbs_enable;	/* number of CPUs using this policy */
 
 /*
- * dbs_mutex protects data in dbs_tuners_ins from concurrent changes on
- * different CPUs. It protects dbs_enable in governor start/stop.
+ * dbs_mutex protects dbs_enable in governor start/stop.
  */
 static DEFINE_MUTEX(dbs_mutex);
 
-static struct workqueue_struct	*kondemand_wq;
-
 static struct dbs_tuners {
 	unsigned int sampling_rate;
 	unsigned int up_threshold;
@@ -237,21 +234,12 @@ static void ondemand_powersave_bias_init(void)
 
 /************************** sysfs interface ************************/
 
-static ssize_t show_sampling_rate_max(struct kobject *kobj,
-				      struct attribute *attr, char *buf)
-{
-	printk_once(KERN_INFO "CPUFREQ: ondemand sampling_rate_max "
-	       "sysfs file is deprecated - used by: %s\n", current->comm);
-	return sprintf(buf, "%u\n", -1U);
-}
-
 static ssize_t show_sampling_rate_min(struct kobject *kobj,
 				      struct attribute *attr, char *buf)
 {
 	return sprintf(buf, "%u\n", min_sampling_rate);
 }
 
-define_one_global_ro(sampling_rate_max);
 define_one_global_ro(sampling_rate_min);
 
 /* cpufreq_ondemand Governor Tunables */
@@ -268,32 +256,6 @@ show_one(sampling_down_factor, sampling_down_factor);
 show_one(ignore_nice_load, ignore_nice);
 show_one(powersave_bias, powersave_bias);
 
-/*** delete after deprecation time ***/
-
-#define DEPRECATION_MSG(file_name)					\
-	printk_once(KERN_INFO "CPUFREQ: Per core ondemand sysfs "	\
-		    "interface is deprecated - " #file_name "\n");
-
-#define show_one_old(file_name)						\
-static ssize_t show_##file_name##_old					\
-(struct cpufreq_policy *unused, char *buf)				\
-{									\
-	printk_once(KERN_INFO "CPUFREQ: Per core ondemand sysfs "	\
-		    "interface is deprecated - " #file_name "\n");	\
-	return show_##file_name(NULL, NULL, buf);			\
-}
-show_one_old(sampling_rate);
-show_one_old(up_threshold);
-show_one_old(ignore_nice_load);
-show_one_old(powersave_bias);
-show_one_old(sampling_rate_min);
-show_one_old(sampling_rate_max);
-
-cpufreq_freq_attr_ro_old(sampling_rate_min);
-cpufreq_freq_attr_ro_old(sampling_rate_max);
-
-/*** delete after deprecation time ***/
-
 static ssize_t store_sampling_rate(struct kobject *a, struct attribute *b,
 				   const char *buf, size_t count)
 {
@@ -302,11 +264,7 @@ static ssize_t store_sampling_rate(struct kobject *a, struct attribute *b,
 	ret = sscanf(buf, "%u", &input);
 	if (ret != 1)
 		return -EINVAL;
-
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.sampling_rate = max(input, min_sampling_rate);
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -319,11 +277,7 @@ static ssize_t store_io_is_busy(struct kobject *a, struct attribute *b,
 	ret = sscanf(buf, "%u", &input);
 	if (ret != 1)
 		return -EINVAL;
-
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.io_is_busy = !!input;
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -338,11 +292,7 @@ static ssize_t store_up_threshold(struct kobject *a, struct attribute *b,
 			input < MIN_FREQUENCY_UP_THRESHOLD) {
 		return -EINVAL;
 	}
-
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.up_threshold = input;
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -355,7 +305,6 @@ static ssize_t store_sampling_down_factor(struct kobject *a,
 
 	if (ret != 1 || input > MAX_SAMPLING_DOWN_FACTOR || input < 1)
 		return -EINVAL;
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.sampling_down_factor = input;
 
 	/* Reset down sampling multiplier in case it was active */
@@ -364,8 +313,6 @@ static ssize_t store_sampling_down_factor(struct kobject *a,
 		dbs_info = &per_cpu(od_cpu_dbs_info, j);
 		dbs_info->rate_mult = 1;
 	}
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -384,9 +331,7 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 	if (input > 1)
 		input = 1;
 
-	mutex_lock(&dbs_mutex);
 	if (input == dbs_tuners_ins.ignore_nice) { /* nothing to do */
-		mutex_unlock(&dbs_mutex);
 		return count;
 	}
 	dbs_tuners_ins.ignore_nice = input;
@@ -401,8 +346,6 @@ static ssize_t store_ignore_nice_load(struct kobject *a, struct attribute *b,
 			dbs_info->prev_cpu_nice = kstat_cpu(j).cpustat.nice;
 
 	}
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -419,11 +362,8 @@ static ssize_t store_powersave_bias(struct kobject *a, struct attribute *b,
 	if (input > 1000)
 		input = 1000;
 
-	mutex_lock(&dbs_mutex);
 	dbs_tuners_ins.powersave_bias = input;
 	ondemand_powersave_bias_init();
-	mutex_unlock(&dbs_mutex);
-
 	return count;
 }
 
@@ -435,7 +375,6 @@ define_one_global_rw(ignore_nice_load);
 define_one_global_rw(powersave_bias);
 
 static struct attribute *dbs_attributes[] = {
-	&sampling_rate_max.attr,
 	&sampling_rate_min.attr,
 	&sampling_rate.attr,
 	&up_threshold.attr,
@@ -451,43 +390,6 @@ static struct attribute_group dbs_attr_group = {
 	.name = "ondemand",
 };
 
-/*** delete after deprecation time ***/
-
-#define write_one_old(file_name)					\
-static ssize_t store_##file_name##_old					\
-(struct cpufreq_policy *unused, const char *buf, size_t count)		\
-{									\
-       printk_once(KERN_INFO "CPUFREQ: Per core ondemand sysfs "	\
-		   "interface is deprecated - " #file_name "\n");	\
-       return store_##file_name(NULL, NULL, buf, count);		\
-}
-write_one_old(sampling_rate);
-write_one_old(up_threshold);
-write_one_old(ignore_nice_load);
-write_one_old(powersave_bias);
-
-cpufreq_freq_attr_rw_old(sampling_rate);
-cpufreq_freq_attr_rw_old(up_threshold);
-cpufreq_freq_attr_rw_old(ignore_nice_load);
-cpufreq_freq_attr_rw_old(powersave_bias);
-
-static struct attribute *dbs_attributes_old[] = {
-       &sampling_rate_max_old.attr,
-       &sampling_rate_min_old.attr,
-       &sampling_rate_old.attr,
-       &up_threshold_old.attr,
-       &ignore_nice_load_old.attr,
-       &powersave_bias_old.attr,
-       NULL
-};
-
-static struct attribute_group dbs_attr_group_old = {
-       .attrs = dbs_attributes_old,
-       .name = "ondemand",
-};
-
-/*** delete after deprecation time ***/
-
 /************************** sysfs end ************************/
 
 static void dbs_freq_increase(struct cpufreq_policy *p, unsigned int freq)
@@ -644,12 +546,7 @@ static void do_dbs_timer(struct work_struct *work)
 	unsigned int cpu = dbs_info->cpu;
 	int sample_type = dbs_info->sample_type;
 
-	/* We want all CPUs to do sampling nearly on same jiffy */
-	int delay = usecs_to_jiffies(dbs_tuners_ins.sampling_rate
-		* dbs_info->rate_mult);
-
-	if (num_online_cpus() > 1)
-		delay -= jiffies % delay;
+	int delay;
 
 	mutex_lock(&dbs_info->timer_mutex);
 
@@ -662,12 +559,22 @@ static void do_dbs_timer(struct work_struct *work)
 			/* Setup timer for SUB_SAMPLE */
 			dbs_info->sample_type = DBS_SUB_SAMPLE;
 			delay = dbs_info->freq_hi_jiffies;
+		} else {
+			/* We want all CPUs to do sampling nearly on
+			 * same jiffy
+			 */
+			delay = usecs_to_jiffies(dbs_tuners_ins.sampling_rate
+				* dbs_info->rate_mult);
+
+			if (num_online_cpus() > 1)
+				delay -= jiffies % delay;
 		}
 	} else {
 		__cpufreq_driver_target(dbs_info->cur_policy,
 			dbs_info->freq_lo, CPUFREQ_RELATION_H);
+		delay = dbs_info->freq_lo_jiffies;
 	}
-	queue_delayed_work_on(cpu, kondemand_wq, &dbs_info->work, delay);
+	schedule_delayed_work_on(cpu, &dbs_info->work, delay);
 	mutex_unlock(&dbs_info->timer_mutex);
 }
 
@@ -681,8 +588,7 @@ static inline void dbs_timer_init(struct cpu_dbs_info_s *dbs_info)
 
 	dbs_info->sample_type = DBS_NORMAL_SAMPLE;
 	INIT_DELAYED_WORK_DEFERRABLE(&dbs_info->work, do_dbs_timer);
-	queue_delayed_work_on(dbs_info->cpu, kondemand_wq, &dbs_info->work,
-		delay);
+	schedule_delayed_work_on(dbs_info->cpu, &dbs_info->work, delay);
 }
 
 static inline void dbs_timer_exit(struct cpu_dbs_info_s *dbs_info)
@@ -730,12 +636,6 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 
 		mutex_lock(&dbs_mutex);
 
-		rc = sysfs_create_group(&policy->kobj, &dbs_attr_group_old);
-		if (rc) {
-			mutex_unlock(&dbs_mutex);
-			return rc;
-		}
-
 		dbs_enable++;
 		for_each_cpu(j, policy->cpus) {
 			struct cpu_dbs_info_s *j_dbs_info;
@@ -788,7 +688,6 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		dbs_timer_exit(this_dbs_info);
 
 		mutex_lock(&dbs_mutex);
-		sysfs_remove_group(&policy->kobj, &dbs_attr_group_old);
 		mutex_destroy(&this_dbs_info->timer_mutex);
 		dbs_enable--;
 		mutex_unlock(&dbs_mutex);
@@ -814,7 +713,6 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 
 static int __init cpufreq_gov_dbs_init(void)
 {
-	int err;
 	cputime64_t wall;
 	u64 idle_time;
 	int cpu = get_cpu();
@@ -838,22 +736,12 @@ static int __init cpufreq_gov_dbs_init(void)
 			MIN_SAMPLING_RATE_RATIO * jiffies_to_usecs(10);
 	}
 
-	kondemand_wq = create_workqueue("kondemand");
-	if (!kondemand_wq) {
-		printk(KERN_ERR "Creation of kondemand failed\n");
-		return -EFAULT;
-	}
-	err = cpufreq_register_governor(&cpufreq_gov_ondemand);
-	if (err)
-		destroy_workqueue(kondemand_wq);
-
-	return err;
+	return cpufreq_register_governor(&cpufreq_gov_ondemand);
 }
 
 static void __exit cpufreq_gov_dbs_exit(void)
 {
 	cpufreq_unregister_governor(&cpufreq_gov_ondemand);
-	destroy_workqueue(kondemand_wq);
 }
 
 
diff --git a/drivers/cpuidle/sysfs.c b/drivers/cpuidle/sysfs.c
index 0310ffaec9df..be7917ec40c9 100644
--- a/drivers/cpuidle/sysfs.c
+++ b/drivers/cpuidle/sysfs.c
@@ -300,7 +300,7 @@ static struct kobj_type ktype_state_cpuidle = {
 	.release = cpuidle_state_sysfs_release,
 };
 
-static void inline cpuidle_free_state_kobj(struct cpuidle_device *device, int i)
+static inline void cpuidle_free_state_kobj(struct cpuidle_device *device, int i)
 {
 	kobject_put(&device->kobjs[i]->kobj);
 	wait_for_completion(&device->kobjs[i]->kobj_unregister);
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index eab2cf7a0269..e54185223c8c 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -252,4 +252,21 @@ config CRYPTO_DEV_OMAP_AES
 	  OMAP processors have AES module accelerator. Select this if you
 	  want to use the OMAP module for AES algorithms.
 
+config CRYPTO_DEV_PICOXCELL
+	tristate "Support for picoXcell IPSEC and Layer2 crypto engines"
+	depends on ARCH_PICOXCELL
+	select CRYPTO_AES
+	select CRYPTO_AUTHENC
+	select CRYPTO_ALGAPI
+	select CRYPTO_DES
+	select CRYPTO_CBC
+	select CRYPTO_ECB
+	select CRYPTO_SEQIV
+	help
+	  This option enables support for the hardware offload engines in the
+	  Picochip picoXcell SoC devices. Select this for IPSEC ESP offload
+	  and for 3gpp Layer 2 ciphering support.
+
+	  Saying m here will build a module named pipcoxcell_crypto.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 256697330a41..5203e34248d7 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_CRYPTO_DEV_IXP4XX) += ixp4xx_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_PPC4XX) += amcc/
 obj-$(CONFIG_CRYPTO_DEV_OMAP_SHAM) += omap-sham.o
 obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
-
+obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index 2b1baee525bc..18912521a7a5 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -1150,8 +1150,7 @@ struct crypto4xx_alg_common crypto4xx_alg[] = {
 /**
  * Module Initialization Routine
  */
-static int __init crypto4xx_probe(struct platform_device *ofdev,
-				  const struct of_device_id *match)
+static int __init crypto4xx_probe(struct platform_device *ofdev)
 {
 	int rc;
 	struct resource res;
@@ -1280,7 +1279,7 @@ static const struct of_device_id crypto4xx_match[] = {
 	{ },
 };
 
-static struct of_platform_driver crypto4xx_driver = {
+static struct platform_driver crypto4xx_driver = {
 	.driver = {
 		.name = "crypto4xx",
 		.owner = THIS_MODULE,
@@ -1292,12 +1291,12 @@ static struct of_platform_driver crypto4xx_driver = {
 
 static int __init crypto4xx_init(void)
 {
-	return of_register_platform_driver(&crypto4xx_driver);
+	return platform_driver_register(&crypto4xx_driver);
 }
 
 static void __exit crypto4xx_exit(void)
 {
-	of_unregister_platform_driver(&crypto4xx_driver);
+	platform_driver_unregister(&crypto4xx_driver);
 }
 
 module_init(crypto4xx_init);
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 80dc094e78c6..2e5b2044c96f 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -2004,8 +2004,7 @@ static void __devinit n2_spu_driver_version(void)
 		pr_info("%s", version);
 }
 
-static int __devinit n2_crypto_probe(struct platform_device *dev,
-				     const struct of_device_id *match)
+static int __devinit n2_crypto_probe(struct platform_device *dev)
 {
 	struct mdesc_handle *mdesc;
 	const char *full_name;
@@ -2116,8 +2115,7 @@ static void free_ncp(struct n2_mau *mp)
 	kfree(mp);
 }
 
-static int __devinit n2_mau_probe(struct platform_device *dev,
-				     const struct of_device_id *match)
+static int __devinit n2_mau_probe(struct platform_device *dev)
 {
 	struct mdesc_handle *mdesc;
 	const char *full_name;
@@ -2211,7 +2209,7 @@ static struct of_device_id n2_crypto_match[] = {
 
 MODULE_DEVICE_TABLE(of, n2_crypto_match);
 
-static struct of_platform_driver n2_crypto_driver = {
+static struct platform_driver n2_crypto_driver = {
 	.driver = {
 		.name		=	"n2cp",
 		.owner		=	THIS_MODULE,
@@ -2235,7 +2233,7 @@ static struct of_device_id n2_mau_match[] = {
 
 MODULE_DEVICE_TABLE(of, n2_mau_match);
 
-static struct of_platform_driver n2_mau_driver = {
+static struct platform_driver n2_mau_driver = {
 	.driver = {
 		.name		=	"ncp",
 		.owner		=	THIS_MODULE,
@@ -2247,20 +2245,20 @@ static struct of_platform_driver n2_mau_driver = {
 
 static int __init n2_init(void)
 {
-	int err = of_register_platform_driver(&n2_crypto_driver);
+	int err = platform_driver_register(&n2_crypto_driver);
 
 	if (!err) {
-		err = of_register_platform_driver(&n2_mau_driver);
+		err = platform_driver_register(&n2_mau_driver);
 		if (err)
-			of_unregister_platform_driver(&n2_crypto_driver);
+			platform_driver_unregister(&n2_crypto_driver);
 	}
 	return err;
 }
 
 static void __exit n2_exit(void)
 {
-	of_unregister_platform_driver(&n2_mau_driver);
-	of_unregister_platform_driver(&n2_crypto_driver);
+	platform_driver_unregister(&n2_mau_driver);
+	platform_driver_unregister(&n2_crypto_driver);
 }
 
 module_init(n2_init);
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index add2a1a72ba4..5b970d9e9956 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -839,9 +839,9 @@ static int omap_aes_probe(struct platform_device *pdev)
 
 	/* Initializing the clock */
 	dd->iclk = clk_get(dev, "ick");
-	if (!dd->iclk) {
+	if (IS_ERR(dd->iclk)) {
 		dev_err(dev, "clock intialization failed.\n");
-		err = -ENODEV;
+		err = PTR_ERR(dd->iclk);
 		goto err_res;
 	}
 
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 2e71123516e0..465cde3e4f60 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -1206,9 +1206,9 @@ static int __devinit omap_sham_probe(struct platform_device *pdev)
 
 	/* Initializing the clock */
 	dd->iclk = clk_get(dev, "ick");
-	if (!dd->iclk) {
+	if (IS_ERR(dd->iclk)) {
 		dev_err(dev, "clock intialization failed.\n");
-		err = -ENODEV;
+		err = PTR_ERR(dd->iclk);
 		goto clk_err;
 	}
 
diff --git a/drivers/crypto/picoxcell_crypto.c b/drivers/crypto/picoxcell_crypto.c
new file mode 100644
index 000000000000..b092d0a65837
--- /dev/null
+++ b/drivers/crypto/picoxcell_crypto.c
@@ -0,0 +1,1867 @@
+/*
+ * Copyright (c) 2010-2011 Picochip Ltd., Jamie Iles
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU 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 <crypto/aead.h>
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/authenc.h>
+#include <crypto/des.h>
+#include <crypto/md5.h>
+#include <crypto/sha.h>
+#include <crypto/internal/skcipher.h>
+#include <linux/clk.h>
+#include <linux/crypto.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/rtnetlink.h>
+#include <linux/scatterlist.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+
+#include "picoxcell_crypto_regs.h"
+
+/*
+ * The threshold for the number of entries in the CMD FIFO available before
+ * the CMD0_CNT interrupt is raised. Increasing this value will reduce the
+ * number of interrupts raised to the CPU.
+ */
+#define CMD0_IRQ_THRESHOLD   1
+
+/*
+ * The timeout period (in jiffies) for a PDU. When the the number of PDUs in
+ * flight is greater than the STAT_IRQ_THRESHOLD or 0 the timer is disabled.
+ * When there are packets in flight but lower than the threshold, we enable
+ * the timer and at expiry, attempt to remove any processed packets from the
+ * queue and if there are still packets left, schedule the timer again.
+ */
+#define PACKET_TIMEOUT	    1
+
+/* The priority to register each algorithm with. */
+#define SPACC_CRYPTO_ALG_PRIORITY	10000
+
+#define SPACC_CRYPTO_KASUMI_F8_KEY_LEN	16
+#define SPACC_CRYPTO_IPSEC_CIPHER_PG_SZ 64
+#define SPACC_CRYPTO_IPSEC_HASH_PG_SZ	64
+#define SPACC_CRYPTO_IPSEC_MAX_CTXS	32
+#define SPACC_CRYPTO_IPSEC_FIFO_SZ	32
+#define SPACC_CRYPTO_L2_CIPHER_PG_SZ	64
+#define SPACC_CRYPTO_L2_HASH_PG_SZ	64
+#define SPACC_CRYPTO_L2_MAX_CTXS	128
+#define SPACC_CRYPTO_L2_FIFO_SZ		128
+
+#define MAX_DDT_LEN			16
+
+/* DDT format. This must match the hardware DDT format exactly. */
+struct spacc_ddt {
+	dma_addr_t	p;
+	u32		len;
+};
+
+/*
+ * Asynchronous crypto request structure.
+ *
+ * This structure defines a request that is either queued for processing or
+ * being processed.
+ */
+struct spacc_req {
+	struct list_head		list;
+	struct spacc_engine		*engine;
+	struct crypto_async_request	*req;
+	int				result;
+	bool				is_encrypt;
+	unsigned			ctx_id;
+	dma_addr_t			src_addr, dst_addr;
+	struct spacc_ddt		*src_ddt, *dst_ddt;
+	void				(*complete)(struct spacc_req *req);
+
+	/* AEAD specific bits. */
+	u8				*giv;
+	size_t				giv_len;
+	dma_addr_t			giv_pa;
+};
+
+struct spacc_engine {
+	void __iomem			*regs;
+	struct list_head		pending;
+	int				next_ctx;
+	spinlock_t			hw_lock;
+	int				in_flight;
+	struct list_head		completed;
+	struct list_head		in_progress;
+	struct tasklet_struct		complete;
+	unsigned long			fifo_sz;
+	void __iomem			*cipher_ctx_base;
+	void __iomem			*hash_key_base;
+	struct spacc_alg		*algs;
+	unsigned			num_algs;
+	struct list_head		registered_algs;
+	size_t				cipher_pg_sz;
+	size_t				hash_pg_sz;
+	const char			*name;
+	struct clk			*clk;
+	struct device			*dev;
+	unsigned			max_ctxs;
+	struct timer_list		packet_timeout;
+	unsigned			stat_irq_thresh;
+	struct dma_pool			*req_pool;
+};
+
+/* Algorithm type mask. */
+#define SPACC_CRYPTO_ALG_MASK		0x7
+
+/* SPACC definition of a crypto algorithm. */
+struct spacc_alg {
+	unsigned long			ctrl_default;
+	unsigned long			type;
+	struct crypto_alg		alg;
+	struct spacc_engine		*engine;
+	struct list_head		entry;
+	int				key_offs;
+	int				iv_offs;
+};
+
+/* Generic context structure for any algorithm type. */
+struct spacc_generic_ctx {
+	struct spacc_engine		*engine;
+	int				flags;
+	int				key_offs;
+	int				iv_offs;
+};
+
+/* Block cipher context. */
+struct spacc_ablk_ctx {
+	struct spacc_generic_ctx	generic;
+	u8				key[AES_MAX_KEY_SIZE];
+	u8				key_len;
+	/*
+	 * The fallback cipher. If the operation can't be done in hardware,
+	 * fallback to a software version.
+	 */
+	struct crypto_ablkcipher	*sw_cipher;
+};
+
+/* AEAD cipher context. */
+struct spacc_aead_ctx {
+	struct spacc_generic_ctx	generic;
+	u8				cipher_key[AES_MAX_KEY_SIZE];
+	u8				hash_ctx[SPACC_CRYPTO_IPSEC_HASH_PG_SZ];
+	u8				cipher_key_len;
+	u8				hash_key_len;
+	struct crypto_aead		*sw_cipher;
+	size_t				auth_size;
+	u8				salt[AES_BLOCK_SIZE];
+};
+
+static inline struct spacc_alg *to_spacc_alg(struct crypto_alg *alg)
+{
+	return alg ? container_of(alg, struct spacc_alg, alg) : NULL;
+}
+
+static inline int spacc_fifo_cmd_full(struct spacc_engine *engine)
+{
+	u32 fifo_stat = readl(engine->regs + SPA_FIFO_STAT_REG_OFFSET);
+
+	return fifo_stat & SPA_FIFO_CMD_FULL;
+}
+
+/*
+ * Given a cipher context, and a context number, get the base address of the
+ * context page.
+ *
+ * Returns the address of the context page where the key/context may
+ * be written.
+ */
+static inline void __iomem *spacc_ctx_page_addr(struct spacc_generic_ctx *ctx,
+						unsigned indx,
+						bool is_cipher_ctx)
+{
+	return is_cipher_ctx ? ctx->engine->cipher_ctx_base +
+			(indx * ctx->engine->cipher_pg_sz) :
+		ctx->engine->hash_key_base + (indx * ctx->engine->hash_pg_sz);
+}
+
+/* The context pages can only be written with 32-bit accesses. */
+static inline void memcpy_toio32(u32 __iomem *dst, const void *src,
+				 unsigned count)
+{
+	const u32 *src32 = (const u32 *) src;
+
+	while (count--)
+		writel(*src32++, dst++);
+}
+
+static void spacc_cipher_write_ctx(struct spacc_generic_ctx *ctx,
+				   void __iomem *page_addr, const u8 *key,
+				   size_t key_len, const u8 *iv, size_t iv_len)
+{
+	void __iomem *key_ptr = page_addr + ctx->key_offs;
+	void __iomem *iv_ptr = page_addr + ctx->iv_offs;
+
+	memcpy_toio32(key_ptr, key, key_len / 4);
+	memcpy_toio32(iv_ptr, iv, iv_len / 4);
+}
+
+/*
+ * Load a context into the engines context memory.
+ *
+ * Returns the index of the context page where the context was loaded.
+ */
+static unsigned spacc_load_ctx(struct spacc_generic_ctx *ctx,
+			       const u8 *ciph_key, size_t ciph_len,
+			       const u8 *iv, size_t ivlen, const u8 *hash_key,
+			       size_t hash_len)
+{
+	unsigned indx = ctx->engine->next_ctx++;
+	void __iomem *ciph_page_addr, *hash_page_addr;
+
+	ciph_page_addr = spacc_ctx_page_addr(ctx, indx, 1);
+	hash_page_addr = spacc_ctx_page_addr(ctx, indx, 0);
+
+	ctx->engine->next_ctx &= ctx->engine->fifo_sz - 1;
+	spacc_cipher_write_ctx(ctx, ciph_page_addr, ciph_key, ciph_len, iv,
+			       ivlen);
+	writel(ciph_len | (indx << SPA_KEY_SZ_CTX_INDEX_OFFSET) |
+	       (1 << SPA_KEY_SZ_CIPHER_OFFSET),
+	       ctx->engine->regs + SPA_KEY_SZ_REG_OFFSET);
+
+	if (hash_key) {
+		memcpy_toio32(hash_page_addr, hash_key, hash_len / 4);
+		writel(hash_len | (indx << SPA_KEY_SZ_CTX_INDEX_OFFSET),
+		       ctx->engine->regs + SPA_KEY_SZ_REG_OFFSET);
+	}
+
+	return indx;
+}
+
+/* Count the number of scatterlist entries in a scatterlist. */
+static int sg_count(struct scatterlist *sg_list, int nbytes)
+{
+	struct scatterlist *sg = sg_list;
+	int sg_nents = 0;
+
+	while (nbytes > 0) {
+		++sg_nents;
+		nbytes -= sg->length;
+		sg = sg_next(sg);
+	}
+
+	return sg_nents;
+}
+
+static inline void ddt_set(struct spacc_ddt *ddt, dma_addr_t phys, size_t len)
+{
+	ddt->p = phys;
+	ddt->len = len;
+}
+
+/*
+ * Take a crypto request and scatterlists for the data and turn them into DDTs
+ * for passing to the crypto engines. This also DMA maps the data so that the
+ * crypto engines can DMA to/from them.
+ */
+static struct spacc_ddt *spacc_sg_to_ddt(struct spacc_engine *engine,
+					 struct scatterlist *payload,
+					 unsigned nbytes,
+					 enum dma_data_direction dir,
+					 dma_addr_t *ddt_phys)
+{
+	unsigned nents, mapped_ents;
+	struct scatterlist *cur;
+	struct spacc_ddt *ddt;
+	int i;
+
+	nents = sg_count(payload, nbytes);
+	mapped_ents = dma_map_sg(engine->dev, payload, nents, dir);
+
+	if (mapped_ents + 1 > MAX_DDT_LEN)
+		goto out;
+
+	ddt = dma_pool_alloc(engine->req_pool, GFP_ATOMIC, ddt_phys);
+	if (!ddt)
+		goto out;
+
+	for_each_sg(payload, cur, mapped_ents, i)
+		ddt_set(&ddt[i], sg_dma_address(cur), sg_dma_len(cur));
+	ddt_set(&ddt[mapped_ents], 0, 0);
+
+	return ddt;
+
+out:
+	dma_unmap_sg(engine->dev, payload, nents, dir);
+	return NULL;
+}
+
+static int spacc_aead_make_ddts(struct spacc_req *req, u8 *giv)
+{
+	struct aead_request *areq = container_of(req->req, struct aead_request,
+						 base);
+	struct spacc_engine *engine = req->engine;
+	struct spacc_ddt *src_ddt, *dst_ddt;
+	unsigned ivsize = crypto_aead_ivsize(crypto_aead_reqtfm(areq));
+	unsigned nents = sg_count(areq->src, areq->cryptlen);
+	dma_addr_t iv_addr;
+	struct scatterlist *cur;
+	int i, dst_ents, src_ents, assoc_ents;
+	u8 *iv = giv ? giv : areq->iv;
+
+	src_ddt = dma_pool_alloc(engine->req_pool, GFP_ATOMIC, &req->src_addr);
+	if (!src_ddt)
+		return -ENOMEM;
+
+	dst_ddt = dma_pool_alloc(engine->req_pool, GFP_ATOMIC, &req->dst_addr);
+	if (!dst_ddt) {
+		dma_pool_free(engine->req_pool, src_ddt, req->src_addr);
+		return -ENOMEM;
+	}
+
+	req->src_ddt = src_ddt;
+	req->dst_ddt = dst_ddt;
+
+	assoc_ents = dma_map_sg(engine->dev, areq->assoc,
+		sg_count(areq->assoc, areq->assoclen), DMA_TO_DEVICE);
+	if (areq->src != areq->dst) {
+		src_ents = dma_map_sg(engine->dev, areq->src, nents,
+				      DMA_TO_DEVICE);
+		dst_ents = dma_map_sg(engine->dev, areq->dst, nents,
+				      DMA_FROM_DEVICE);
+	} else {
+		src_ents = dma_map_sg(engine->dev, areq->src, nents,
+				      DMA_BIDIRECTIONAL);
+		dst_ents = 0;
+	}
+
+	/*
+	 * Map the IV/GIV. For the GIV it needs to be bidirectional as it is
+	 * formed by the crypto block and sent as the ESP IV for IPSEC.
+	 */
+	iv_addr = dma_map_single(engine->dev, iv, ivsize,
+				 giv ? DMA_BIDIRECTIONAL : DMA_TO_DEVICE);
+	req->giv_pa = iv_addr;
+
+	/*
+	 * Map the associated data. For decryption we don't copy the
+	 * associated data.
+	 */
+	for_each_sg(areq->assoc, cur, assoc_ents, i) {
+		ddt_set(src_ddt++, sg_dma_address(cur), sg_dma_len(cur));
+		if (req->is_encrypt)
+			ddt_set(dst_ddt++, sg_dma_address(cur),
+				sg_dma_len(cur));
+	}
+	ddt_set(src_ddt++, iv_addr, ivsize);
+
+	if (giv || req->is_encrypt)
+		ddt_set(dst_ddt++, iv_addr, ivsize);
+
+	/*
+	 * Now map in the payload for the source and destination and terminate
+	 * with the NULL pointers.
+	 */
+	for_each_sg(areq->src, cur, src_ents, i) {
+		ddt_set(src_ddt++, sg_dma_address(cur), sg_dma_len(cur));
+		if (areq->src == areq->dst)
+			ddt_set(dst_ddt++, sg_dma_address(cur),
+				sg_dma_len(cur));
+	}
+
+	for_each_sg(areq->dst, cur, dst_ents, i)
+		ddt_set(dst_ddt++, sg_dma_address(cur),
+			sg_dma_len(cur));
+
+	ddt_set(src_ddt, 0, 0);
+	ddt_set(dst_ddt, 0, 0);
+
+	return 0;
+}
+
+static void spacc_aead_free_ddts(struct spacc_req *req)
+{
+	struct aead_request *areq = container_of(req->req, struct aead_request,
+						 base);
+	struct spacc_alg *alg = to_spacc_alg(req->req->tfm->__crt_alg);
+	struct spacc_ablk_ctx *aead_ctx = crypto_tfm_ctx(req->req->tfm);
+	struct spacc_engine *engine = aead_ctx->generic.engine;
+	unsigned ivsize = alg->alg.cra_aead.ivsize;
+	unsigned nents = sg_count(areq->src, areq->cryptlen);
+
+	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, areq->cryptlen),
+			     DMA_FROM_DEVICE);
+	} else
+		dma_unmap_sg(engine->dev, areq->src, nents, DMA_BIDIRECTIONAL);
+
+	dma_unmap_sg(engine->dev, areq->assoc,
+		     sg_count(areq->assoc, areq->assoclen), DMA_TO_DEVICE);
+
+	dma_unmap_single(engine->dev, req->giv_pa, ivsize, DMA_BIDIRECTIONAL);
+
+	dma_pool_free(engine->req_pool, req->src_ddt, req->src_addr);
+	dma_pool_free(engine->req_pool, req->dst_ddt, req->dst_addr);
+}
+
+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);
+
+	dma_unmap_sg(req->engine->dev, payload, nents, dir);
+	dma_pool_free(req->engine->req_pool, ddt, ddt_addr);
+}
+
+/*
+ * Set key for a DES operation in an AEAD cipher. This also performs weak key
+ * checking if required.
+ */
+static int spacc_aead_des_setkey(struct crypto_aead *aead, const u8 *key,
+				 unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_aead_tfm(aead);
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 tmp[DES_EXPKEY_WORDS];
+
+	if (unlikely(!des_ekey(tmp, key)) &&
+	    (crypto_aead_get_flags(aead)) & CRYPTO_TFM_REQ_WEAK_KEY) {
+		tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->cipher_key, key, len);
+	ctx->cipher_key_len = len;
+
+	return 0;
+}
+
+/* Set the key for the AES block cipher component of the AEAD transform. */
+static int spacc_aead_aes_setkey(struct crypto_aead *aead, const u8 *key,
+				 unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_aead_tfm(aead);
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	/*
+	 * IPSec engine only supports 128 and 256 bit AES keys. If we get a
+	 * request for any other size (192 bits) then we need to do a software
+	 * fallback.
+	 */
+	if (len != AES_KEYSIZE_128 && len != AES_KEYSIZE_256) {
+		/*
+		 * Set the fallback transform to use the same request flags as
+		 * the hardware transform.
+		 */
+		ctx->sw_cipher->base.crt_flags &= ~CRYPTO_TFM_REQ_MASK;
+		ctx->sw_cipher->base.crt_flags |=
+			tfm->crt_flags & CRYPTO_TFM_REQ_MASK;
+		return crypto_aead_setkey(ctx->sw_cipher, key, len);
+	}
+
+	memcpy(ctx->cipher_key, key, len);
+	ctx->cipher_key_len = len;
+
+	return 0;
+}
+
+static int spacc_aead_setkey(struct crypto_aead *tfm, const u8 *key,
+			     unsigned int keylen)
+{
+	struct spacc_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct spacc_alg *alg = to_spacc_alg(tfm->base.__crt_alg);
+	struct rtattr *rta = (void *)key;
+	struct crypto_authenc_key_param *param;
+	unsigned int authkeylen, enckeylen;
+	int err = -EINVAL;
+
+	if (!RTA_OK(rta, keylen))
+		goto badkey;
+
+	if (rta->rta_type != CRYPTO_AUTHENC_KEYA_PARAM)
+		goto badkey;
+
+	if (RTA_PAYLOAD(rta) < sizeof(*param))
+		goto badkey;
+
+	param = RTA_DATA(rta);
+	enckeylen = be32_to_cpu(param->enckeylen);
+
+	key += RTA_ALIGN(rta->rta_len);
+	keylen -= RTA_ALIGN(rta->rta_len);
+
+	if (keylen < enckeylen)
+		goto badkey;
+
+	authkeylen = keylen - enckeylen;
+
+	if (enckeylen > AES_MAX_KEY_SIZE)
+		goto badkey;
+
+	if ((alg->ctrl_default & SPACC_CRYPTO_ALG_MASK) ==
+	    SPA_CTRL_CIPH_ALG_AES)
+		err = spacc_aead_aes_setkey(tfm, key + authkeylen, enckeylen);
+	else
+		err = spacc_aead_des_setkey(tfm, key + authkeylen, enckeylen);
+
+	if (err)
+		goto badkey;
+
+	memcpy(ctx->hash_ctx, key, authkeylen);
+	ctx->hash_key_len = authkeylen;
+
+	return 0;
+
+badkey:
+	crypto_aead_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	return -EINVAL;
+}
+
+static int spacc_aead_setauthsize(struct crypto_aead *tfm,
+				  unsigned int authsize)
+{
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(crypto_aead_tfm(tfm));
+
+	ctx->auth_size = authsize;
+
+	return 0;
+}
+
+/*
+ * Check if an AEAD request requires a fallback operation. Some requests can't
+ * be completed in hardware because the hardware may not support certain key
+ * sizes. In these cases we need to complete the request in software.
+ */
+static int spacc_aead_need_fallback(struct spacc_req *req)
+{
+	struct aead_request *aead_req;
+	struct crypto_tfm *tfm = req->req->tfm;
+	struct crypto_alg *alg = req->req->tfm->__crt_alg;
+	struct spacc_alg *spacc_alg = to_spacc_alg(alg);
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	aead_req = container_of(req->req, struct aead_request, base);
+	/*
+	 * If we have a non-supported key-length, then we need to do a
+	 * software fallback.
+	 */
+	if ((spacc_alg->ctrl_default & SPACC_CRYPTO_ALG_MASK) ==
+	    SPA_CTRL_CIPH_ALG_AES &&
+	    ctx->cipher_key_len != AES_KEYSIZE_128 &&
+	    ctx->cipher_key_len != AES_KEYSIZE_256)
+		return 1;
+
+	return 0;
+}
+
+static int spacc_aead_do_fallback(struct aead_request *req, unsigned alg_type,
+				  bool is_encrypt)
+{
+	struct crypto_tfm *old_tfm = crypto_aead_tfm(crypto_aead_reqtfm(req));
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(old_tfm);
+	int err;
+
+	if (ctx->sw_cipher) {
+		/*
+		 * Change the request to use the software fallback transform,
+		 * and once the ciphering has completed, put the old transform
+		 * back into the request.
+		 */
+		aead_request_set_tfm(req, ctx->sw_cipher);
+		err = is_encrypt ? crypto_aead_encrypt(req) :
+		    crypto_aead_decrypt(req);
+		aead_request_set_tfm(req, __crypto_aead_cast(old_tfm));
+	} else
+		err = -EINVAL;
+
+	return err;
+}
+
+static void spacc_aead_complete(struct spacc_req *req)
+{
+	spacc_aead_free_ddts(req);
+	req->req->complete(req->req, req->result);
+}
+
+static int spacc_aead_submit(struct spacc_req *req)
+{
+	struct crypto_tfm *tfm = req->req->tfm;
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = req->req->tfm->__crt_alg;
+	struct spacc_alg *spacc_alg = to_spacc_alg(alg);
+	struct spacc_engine *engine = ctx->generic.engine;
+	u32 ctrl, proc_len, assoc_len;
+	struct aead_request *aead_req =
+		container_of(req->req, struct aead_request, base);
+
+	req->result = -EINPROGRESS;
+	req->ctx_id = spacc_load_ctx(&ctx->generic, ctx->cipher_key,
+		ctx->cipher_key_len, aead_req->iv, alg->cra_aead.ivsize,
+		ctx->hash_ctx, ctx->hash_key_len);
+
+	/* Set the source and destination DDT pointers. */
+	writel(req->src_addr, engine->regs + SPA_SRC_PTR_REG_OFFSET);
+	writel(req->dst_addr, engine->regs + SPA_DST_PTR_REG_OFFSET);
+	writel(0, engine->regs + SPA_OFFSET_REG_OFFSET);
+
+	assoc_len = aead_req->assoclen;
+	proc_len = aead_req->cryptlen + assoc_len;
+
+	/*
+	 * If we aren't generating an IV, then we need to include the IV in the
+	 * associated data so that it is included in the hash.
+	 */
+	if (!req->giv) {
+		assoc_len += crypto_aead_ivsize(crypto_aead_reqtfm(aead_req));
+		proc_len += crypto_aead_ivsize(crypto_aead_reqtfm(aead_req));
+	} else
+		proc_len += req->giv_len;
+
+	/*
+	 * If we are decrypting, we need to take the length of the ICV out of
+	 * the processing length.
+	 */
+	if (!req->is_encrypt)
+		proc_len -= ctx->auth_size;
+
+	writel(proc_len, engine->regs + SPA_PROC_LEN_REG_OFFSET);
+	writel(assoc_len, engine->regs + SPA_AAD_LEN_REG_OFFSET);
+	writel(ctx->auth_size, engine->regs + SPA_ICV_LEN_REG_OFFSET);
+	writel(0, engine->regs + SPA_ICV_OFFSET_REG_OFFSET);
+	writel(0, engine->regs + SPA_AUX_INFO_REG_OFFSET);
+
+	ctrl = spacc_alg->ctrl_default | (req->ctx_id << SPA_CTRL_CTX_IDX) |
+		(1 << SPA_CTRL_ICV_APPEND);
+	if (req->is_encrypt)
+		ctrl |= (1 << SPA_CTRL_ENCRYPT_IDX) | (1 << SPA_CTRL_AAD_COPY);
+	else
+		ctrl |= (1 << SPA_CTRL_KEY_EXP);
+
+	mod_timer(&engine->packet_timeout, jiffies + PACKET_TIMEOUT);
+
+	writel(ctrl, engine->regs + SPA_CTRL_REG_OFFSET);
+
+	return -EINPROGRESS;
+}
+
+/*
+ * Setup an AEAD request for processing. This will configure the engine, load
+ * the context and then start the packet processing.
+ *
+ * @giv Pointer to destination address for a generated IV. If the
+ *	request does not need to generate an IV then this should be set to NULL.
+ */
+static int spacc_aead_setup(struct aead_request *req, u8 *giv,
+			    unsigned alg_type, bool is_encrypt)
+{
+	struct crypto_alg *alg = req->base.tfm->__crt_alg;
+	struct spacc_engine *engine = to_spacc_alg(alg)->engine;
+	struct spacc_req *dev_req = aead_request_ctx(req);
+	int err = -EINPROGRESS;
+	unsigned long flags;
+	unsigned ivsize = crypto_aead_ivsize(crypto_aead_reqtfm(req));
+
+	dev_req->giv		= giv;
+	dev_req->giv_len	= ivsize;
+	dev_req->req		= &req->base;
+	dev_req->is_encrypt	= is_encrypt;
+	dev_req->result		= -EBUSY;
+	dev_req->engine		= engine;
+	dev_req->complete	= spacc_aead_complete;
+
+	if (unlikely(spacc_aead_need_fallback(dev_req)))
+		return spacc_aead_do_fallback(req, alg_type, is_encrypt);
+
+	spacc_aead_make_ddts(dev_req, dev_req->giv);
+
+	err = -EINPROGRESS;
+	spin_lock_irqsave(&engine->hw_lock, flags);
+	if (unlikely(spacc_fifo_cmd_full(engine))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG)) {
+			err = -EBUSY;
+			spin_unlock_irqrestore(&engine->hw_lock, flags);
+			goto out_free_ddts;
+		}
+		list_add_tail(&dev_req->list, &engine->pending);
+	} else {
+		++engine->in_flight;
+		list_add_tail(&dev_req->list, &engine->in_progress);
+		spacc_aead_submit(dev_req);
+	}
+	spin_unlock_irqrestore(&engine->hw_lock, flags);
+
+	goto out;
+
+out_free_ddts:
+	spacc_aead_free_ddts(dev_req);
+out:
+	return err;
+}
+
+static int spacc_aead_encrypt(struct aead_request *req)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct crypto_tfm *tfm = crypto_aead_tfm(aead);
+	struct spacc_alg *alg = to_spacc_alg(tfm->__crt_alg);
+
+	return spacc_aead_setup(req, NULL, alg->type, 1);
+}
+
+static int spacc_aead_givencrypt(struct aead_givcrypt_request *req)
+{
+	struct crypto_aead *tfm = aead_givcrypt_reqtfm(req);
+	struct spacc_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	size_t ivsize = crypto_aead_ivsize(tfm);
+	struct spacc_alg *alg = to_spacc_alg(tfm->base.__crt_alg);
+	unsigned len;
+	__be64 seq;
+
+	memcpy(req->areq.iv, ctx->salt, ivsize);
+	len = ivsize;
+	if (ivsize > sizeof(u64)) {
+		memset(req->giv, 0, ivsize - sizeof(u64));
+		len = sizeof(u64);
+	}
+	seq = cpu_to_be64(req->seq);
+	memcpy(req->giv + ivsize - len, &seq, len);
+
+	return spacc_aead_setup(&req->areq, req->giv, alg->type, 1);
+}
+
+static int spacc_aead_decrypt(struct aead_request *req)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct crypto_tfm *tfm = crypto_aead_tfm(aead);
+	struct spacc_alg *alg = to_spacc_alg(tfm->__crt_alg);
+
+	return spacc_aead_setup(req, NULL, alg->type, 0);
+}
+
+/*
+ * Initialise a new AEAD context. This is responsible for allocating the
+ * fallback cipher and initialising the context.
+ */
+static int spacc_aead_cra_init(struct crypto_tfm *tfm)
+{
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct spacc_alg *spacc_alg = to_spacc_alg(alg);
+	struct spacc_engine *engine = spacc_alg->engine;
+
+	ctx->generic.flags = spacc_alg->type;
+	ctx->generic.engine = engine;
+	ctx->sw_cipher = crypto_alloc_aead(alg->cra_name, 0,
+					   CRYPTO_ALG_ASYNC |
+					   CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(ctx->sw_cipher)) {
+		dev_warn(engine->dev, "failed to allocate fallback for %s\n",
+			 alg->cra_name);
+		ctx->sw_cipher = NULL;
+	}
+	ctx->generic.key_offs = spacc_alg->key_offs;
+	ctx->generic.iv_offs = spacc_alg->iv_offs;
+
+	get_random_bytes(ctx->salt, sizeof(ctx->salt));
+
+	tfm->crt_aead.reqsize = sizeof(struct spacc_req);
+
+	return 0;
+}
+
+/*
+ * Destructor for an AEAD context. This is called when the transform is freed
+ * and must free the fallback cipher.
+ */
+static void spacc_aead_cra_exit(struct crypto_tfm *tfm)
+{
+	struct spacc_aead_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (ctx->sw_cipher)
+		crypto_free_aead(ctx->sw_cipher);
+	ctx->sw_cipher = NULL;
+}
+
+/*
+ * Set the DES key for a block cipher transform. This also performs weak key
+ * checking if the transform has requested it.
+ */
+static int spacc_des_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			    unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 tmp[DES_EXPKEY_WORDS];
+
+	if (len > DES3_EDE_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	if (unlikely(!des_ekey(tmp, key)) &&
+	    (crypto_ablkcipher_get_flags(cipher) & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, len);
+	ctx->key_len = len;
+
+	return 0;
+}
+
+/*
+ * Set the key for an AES block cipher. Some key lengths are not supported in
+ * hardware so this must also check whether a fallback is needed.
+ */
+static int spacc_aes_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			    unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(tfm);
+	int err = 0;
+
+	if (len > AES_MAX_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	/*
+	 * IPSec engine only supports 128 and 256 bit AES keys. If we get a
+	 * request for any other size (192 bits) then we need to do a software
+	 * fallback.
+	 */
+	if ((len != AES_KEYSIZE_128 || len != AES_KEYSIZE_256) &&
+	    ctx->sw_cipher) {
+		/*
+		 * Set the fallback transform to use the same request flags as
+		 * the hardware transform.
+		 */
+		ctx->sw_cipher->base.crt_flags &= ~CRYPTO_TFM_REQ_MASK;
+		ctx->sw_cipher->base.crt_flags |=
+			cipher->base.crt_flags & CRYPTO_TFM_REQ_MASK;
+
+		err = crypto_ablkcipher_setkey(ctx->sw_cipher, key, len);
+		if (err)
+			goto sw_setkey_failed;
+	} else if ((len != AES_KEYSIZE_128 || len != AES_KEYSIZE_256) &&
+		   !ctx->sw_cipher)
+		err = -EINVAL;
+
+	memcpy(ctx->key, key, len);
+	ctx->key_len = len;
+
+sw_setkey_failed:
+	if (err && ctx->sw_cipher) {
+		tfm->crt_flags &= ~CRYPTO_TFM_RES_MASK;
+		tfm->crt_flags |=
+			ctx->sw_cipher->base.crt_flags & CRYPTO_TFM_RES_MASK;
+	}
+
+	return err;
+}
+
+static int spacc_kasumi_f8_setkey(struct crypto_ablkcipher *cipher,
+				  const u8 *key, unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(tfm);
+	int err = 0;
+
+	if (len > AES_MAX_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		err = -EINVAL;
+		goto out;
+	}
+
+	memcpy(ctx->key, key, len);
+	ctx->key_len = len;
+
+out:
+	return err;
+}
+
+static int spacc_ablk_need_fallback(struct spacc_req *req)
+{
+	struct spacc_ablk_ctx *ctx;
+	struct crypto_tfm *tfm = req->req->tfm;
+	struct crypto_alg *alg = req->req->tfm->__crt_alg;
+	struct spacc_alg *spacc_alg = to_spacc_alg(alg);
+
+	ctx = crypto_tfm_ctx(tfm);
+
+	return (spacc_alg->ctrl_default & SPACC_CRYPTO_ALG_MASK) ==
+			SPA_CTRL_CIPH_ALG_AES &&
+			ctx->key_len != AES_KEYSIZE_128 &&
+			ctx->key_len != AES_KEYSIZE_256;
+}
+
+static void spacc_ablk_complete(struct spacc_req *req)
+{
+	struct ablkcipher_request *ablk_req =
+		container_of(req->req, struct ablkcipher_request, base);
+
+	if (ablk_req->src != ablk_req->dst) {
+		spacc_free_ddt(req, req->src_ddt, req->src_addr, ablk_req->src,
+			       ablk_req->nbytes, DMA_TO_DEVICE);
+		spacc_free_ddt(req, req->dst_ddt, req->dst_addr, ablk_req->dst,
+			       ablk_req->nbytes, DMA_FROM_DEVICE);
+	} else
+		spacc_free_ddt(req, req->dst_ddt, req->dst_addr, ablk_req->dst,
+			       ablk_req->nbytes, DMA_BIDIRECTIONAL);
+
+	req->req->complete(req->req, req->result);
+}
+
+static int spacc_ablk_submit(struct spacc_req *req)
+{
+	struct crypto_tfm *tfm = req->req->tfm;
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct ablkcipher_request *ablk_req = ablkcipher_request_cast(req->req);
+	struct crypto_alg *alg = req->req->tfm->__crt_alg;
+	struct spacc_alg *spacc_alg = to_spacc_alg(alg);
+	struct spacc_engine *engine = ctx->generic.engine;
+	u32 ctrl;
+
+	req->ctx_id = spacc_load_ctx(&ctx->generic, ctx->key,
+		ctx->key_len, ablk_req->info, alg->cra_ablkcipher.ivsize,
+		NULL, 0);
+
+	writel(req->src_addr, engine->regs + SPA_SRC_PTR_REG_OFFSET);
+	writel(req->dst_addr, engine->regs + SPA_DST_PTR_REG_OFFSET);
+	writel(0, engine->regs + SPA_OFFSET_REG_OFFSET);
+
+	writel(ablk_req->nbytes, engine->regs + SPA_PROC_LEN_REG_OFFSET);
+	writel(0, engine->regs + SPA_ICV_OFFSET_REG_OFFSET);
+	writel(0, engine->regs + SPA_AUX_INFO_REG_OFFSET);
+	writel(0, engine->regs + SPA_AAD_LEN_REG_OFFSET);
+
+	ctrl = spacc_alg->ctrl_default | (req->ctx_id << SPA_CTRL_CTX_IDX) |
+		(req->is_encrypt ? (1 << SPA_CTRL_ENCRYPT_IDX) :
+		 (1 << SPA_CTRL_KEY_EXP));
+
+	mod_timer(&engine->packet_timeout, jiffies + PACKET_TIMEOUT);
+
+	writel(ctrl, engine->regs + SPA_CTRL_REG_OFFSET);
+
+	return -EINPROGRESS;
+}
+
+static int spacc_ablk_do_fallback(struct ablkcipher_request *req,
+				  unsigned alg_type, bool is_encrypt)
+{
+	struct crypto_tfm *old_tfm =
+	    crypto_ablkcipher_tfm(crypto_ablkcipher_reqtfm(req));
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(old_tfm);
+	int err;
+
+	if (!ctx->sw_cipher)
+		return -EINVAL;
+
+	/*
+	 * Change the request to use the software fallback transform, and once
+	 * the ciphering has completed, put the old transform back into the
+	 * request.
+	 */
+	ablkcipher_request_set_tfm(req, ctx->sw_cipher);
+	err = is_encrypt ? crypto_ablkcipher_encrypt(req) :
+		crypto_ablkcipher_decrypt(req);
+	ablkcipher_request_set_tfm(req, __crypto_ablkcipher_cast(old_tfm));
+
+	return err;
+}
+
+static int spacc_ablk_setup(struct ablkcipher_request *req, unsigned alg_type,
+			    bool is_encrypt)
+{
+	struct crypto_alg *alg = req->base.tfm->__crt_alg;
+	struct spacc_engine *engine = to_spacc_alg(alg)->engine;
+	struct spacc_req *dev_req = ablkcipher_request_ctx(req);
+	unsigned long flags;
+	int err = -ENOMEM;
+
+	dev_req->req		= &req->base;
+	dev_req->is_encrypt	= is_encrypt;
+	dev_req->engine		= engine;
+	dev_req->complete	= spacc_ablk_complete;
+	dev_req->result		= -EINPROGRESS;
+
+	if (unlikely(spacc_ablk_need_fallback(dev_req)))
+		return spacc_ablk_do_fallback(req, alg_type, is_encrypt);
+
+	/*
+	 * Create the DDT's for the engine. If we share the same source and
+	 * destination then we can optimize by reusing the DDT's.
+	 */
+	if (req->src != req->dst) {
+		dev_req->src_ddt = spacc_sg_to_ddt(engine, req->src,
+			req->nbytes, DMA_TO_DEVICE, &dev_req->src_addr);
+		if (!dev_req->src_ddt)
+			goto out;
+
+		dev_req->dst_ddt = spacc_sg_to_ddt(engine, req->dst,
+			req->nbytes, DMA_FROM_DEVICE, &dev_req->dst_addr);
+		if (!dev_req->dst_ddt)
+			goto out_free_src;
+	} else {
+		dev_req->dst_ddt = spacc_sg_to_ddt(engine, req->dst,
+			req->nbytes, DMA_BIDIRECTIONAL, &dev_req->dst_addr);
+		if (!dev_req->dst_ddt)
+			goto out;
+
+		dev_req->src_ddt = NULL;
+		dev_req->src_addr = dev_req->dst_addr;
+	}
+
+	err = -EINPROGRESS;
+	spin_lock_irqsave(&engine->hw_lock, flags);
+	/*
+	 * Check if the engine will accept the operation now. If it won't then
+	 * we either stick it on the end of a pending list if we can backlog,
+	 * or bailout with an error if not.
+	 */
+	if (unlikely(spacc_fifo_cmd_full(engine))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG)) {
+			err = -EBUSY;
+			spin_unlock_irqrestore(&engine->hw_lock, flags);
+			goto out_free_ddts;
+		}
+		list_add_tail(&dev_req->list, &engine->pending);
+	} else {
+		++engine->in_flight;
+		list_add_tail(&dev_req->list, &engine->in_progress);
+		spacc_ablk_submit(dev_req);
+	}
+	spin_unlock_irqrestore(&engine->hw_lock, flags);
+
+	goto out;
+
+out_free_ddts:
+	spacc_free_ddt(dev_req, dev_req->dst_ddt, dev_req->dst_addr, req->dst,
+		       req->nbytes, req->src == req->dst ?
+		       DMA_BIDIRECTIONAL : DMA_FROM_DEVICE);
+out_free_src:
+	if (req->src != req->dst)
+		spacc_free_ddt(dev_req, dev_req->src_ddt, dev_req->src_addr,
+			       req->src, req->nbytes, DMA_TO_DEVICE);
+out:
+	return err;
+}
+
+static int spacc_ablk_cra_init(struct crypto_tfm *tfm)
+{
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct spacc_alg *spacc_alg = to_spacc_alg(alg);
+	struct spacc_engine *engine = spacc_alg->engine;
+
+	ctx->generic.flags = spacc_alg->type;
+	ctx->generic.engine = engine;
+	if (alg->cra_flags & CRYPTO_ALG_NEED_FALLBACK) {
+		ctx->sw_cipher = crypto_alloc_ablkcipher(alg->cra_name, 0,
+				CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+		if (IS_ERR(ctx->sw_cipher)) {
+			dev_warn(engine->dev, "failed to allocate fallback for %s\n",
+				 alg->cra_name);
+			ctx->sw_cipher = NULL;
+		}
+	}
+	ctx->generic.key_offs = spacc_alg->key_offs;
+	ctx->generic.iv_offs = spacc_alg->iv_offs;
+
+	tfm->crt_ablkcipher.reqsize = sizeof(struct spacc_req);
+
+	return 0;
+}
+
+static void spacc_ablk_cra_exit(struct crypto_tfm *tfm)
+{
+	struct spacc_ablk_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (ctx->sw_cipher)
+		crypto_free_ablkcipher(ctx->sw_cipher);
+	ctx->sw_cipher = NULL;
+}
+
+static int spacc_ablk_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(req);
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct spacc_alg *alg = to_spacc_alg(tfm->__crt_alg);
+
+	return spacc_ablk_setup(req, alg->type, 1);
+}
+
+static int spacc_ablk_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(req);
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct spacc_alg *alg = to_spacc_alg(tfm->__crt_alg);
+
+	return spacc_ablk_setup(req, alg->type, 0);
+}
+
+static inline int spacc_fifo_stat_empty(struct spacc_engine *engine)
+{
+	return readl(engine->regs + SPA_FIFO_STAT_REG_OFFSET) &
+		SPA_FIFO_STAT_EMPTY;
+}
+
+static void spacc_process_done(struct spacc_engine *engine)
+{
+	struct spacc_req *req;
+	unsigned long flags;
+
+	spin_lock_irqsave(&engine->hw_lock, flags);
+
+	while (!spacc_fifo_stat_empty(engine)) {
+		req = list_first_entry(&engine->in_progress, struct spacc_req,
+				       list);
+		list_move_tail(&req->list, &engine->completed);
+
+		/* POP the status register. */
+		writel(~0, engine->regs + SPA_STAT_POP_REG_OFFSET);
+		req->result = (readl(engine->regs + SPA_STATUS_REG_OFFSET) &
+		     SPA_STATUS_RES_CODE_MASK) >> SPA_STATUS_RES_CODE_OFFSET;
+
+		/*
+		 * Convert the SPAcc error status into the standard POSIX error
+		 * codes.
+		 */
+		if (unlikely(req->result)) {
+			switch (req->result) {
+			case SPA_STATUS_ICV_FAIL:
+				req->result = -EBADMSG;
+				break;
+
+			case SPA_STATUS_MEMORY_ERROR:
+				dev_warn(engine->dev,
+					 "memory error triggered\n");
+				req->result = -EFAULT;
+				break;
+
+			case SPA_STATUS_BLOCK_ERROR:
+				dev_warn(engine->dev,
+					 "block error triggered\n");
+				req->result = -EIO;
+				break;
+			}
+		}
+	}
+
+	tasklet_schedule(&engine->complete);
+
+	spin_unlock_irqrestore(&engine->hw_lock, flags);
+}
+
+static irqreturn_t spacc_spacc_irq(int irq, void *dev)
+{
+	struct spacc_engine *engine = (struct spacc_engine *)dev;
+	u32 spacc_irq_stat = readl(engine->regs + SPA_IRQ_STAT_REG_OFFSET);
+
+	writel(spacc_irq_stat, engine->regs + SPA_IRQ_STAT_REG_OFFSET);
+	spacc_process_done(engine);
+
+	return IRQ_HANDLED;
+}
+
+static void spacc_packet_timeout(unsigned long data)
+{
+	struct spacc_engine *engine = (struct spacc_engine *)data;
+
+	spacc_process_done(engine);
+}
+
+static int spacc_req_submit(struct spacc_req *req)
+{
+	struct crypto_alg *alg = req->req->tfm->__crt_alg;
+
+	if (CRYPTO_ALG_TYPE_AEAD == (CRYPTO_ALG_TYPE_MASK & alg->cra_flags))
+		return spacc_aead_submit(req);
+	else
+		return spacc_ablk_submit(req);
+}
+
+static void spacc_spacc_complete(unsigned long data)
+{
+	struct spacc_engine *engine = (struct spacc_engine *)data;
+	struct spacc_req *req, *tmp;
+	unsigned long flags;
+	int num_removed = 0;
+	LIST_HEAD(completed);
+
+	spin_lock_irqsave(&engine->hw_lock, flags);
+	list_splice_init(&engine->completed, &completed);
+	spin_unlock_irqrestore(&engine->hw_lock, flags);
+
+	list_for_each_entry_safe(req, tmp, &completed, list) {
+		++num_removed;
+		req->complete(req);
+	}
+
+	/* Try and fill the engine back up again. */
+	spin_lock_irqsave(&engine->hw_lock, flags);
+
+	engine->in_flight -= num_removed;
+
+	list_for_each_entry_safe(req, tmp, &engine->pending, list) {
+		if (spacc_fifo_cmd_full(engine))
+			break;
+
+		list_move_tail(&req->list, &engine->in_progress);
+		++engine->in_flight;
+		req->result = spacc_req_submit(req);
+	}
+
+	if (engine->in_flight)
+		mod_timer(&engine->packet_timeout, jiffies + PACKET_TIMEOUT);
+
+	spin_unlock_irqrestore(&engine->hw_lock, flags);
+}
+
+#ifdef CONFIG_PM
+static int spacc_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct spacc_engine *engine = platform_get_drvdata(pdev);
+
+	/*
+	 * We only support standby mode. All we have to do is gate the clock to
+	 * the spacc. The hardware will preserve state until we turn it back
+	 * on again.
+	 */
+	clk_disable(engine->clk);
+
+	return 0;
+}
+
+static int spacc_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct spacc_engine *engine = platform_get_drvdata(pdev);
+
+	return clk_enable(engine->clk);
+}
+
+static const struct dev_pm_ops spacc_pm_ops = {
+	.suspend	= spacc_suspend,
+	.resume		= spacc_resume,
+};
+#endif /* CONFIG_PM */
+
+static inline struct spacc_engine *spacc_dev_to_engine(struct device *dev)
+{
+	return dev ? platform_get_drvdata(to_platform_device(dev)) : NULL;
+}
+
+static ssize_t spacc_stat_irq_thresh_show(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct spacc_engine *engine = spacc_dev_to_engine(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", engine->stat_irq_thresh);
+}
+
+static ssize_t spacc_stat_irq_thresh_store(struct device *dev,
+					   struct device_attribute *attr,
+					   const char *buf, size_t len)
+{
+	struct spacc_engine *engine = spacc_dev_to_engine(dev);
+	unsigned long thresh;
+
+	if (strict_strtoul(buf, 0, &thresh))
+		return -EINVAL;
+
+	thresh = clamp(thresh, 1UL, engine->fifo_sz - 1);
+
+	engine->stat_irq_thresh = thresh;
+	writel(engine->stat_irq_thresh << SPA_IRQ_CTRL_STAT_CNT_OFFSET,
+	       engine->regs + SPA_IRQ_CTRL_REG_OFFSET);
+
+	return len;
+}
+static DEVICE_ATTR(stat_irq_thresh, 0644, spacc_stat_irq_thresh_show,
+		   spacc_stat_irq_thresh_store);
+
+static struct spacc_alg ipsec_engine_algs[] = {
+	{
+		.ctrl_default = SPA_CTRL_CIPH_ALG_AES | SPA_CTRL_CIPH_MODE_CBC,
+		.key_offs = 0,
+		.iv_offs = AES_MAX_KEY_SIZE,
+		.alg = {
+			.cra_name = "cbc(aes)",
+			.cra_driver_name = "cbc-aes-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				     CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_aes_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = AES_MIN_KEY_SIZE,
+				.max_keysize = AES_MAX_KEY_SIZE,
+				.ivsize = AES_BLOCK_SIZE,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+	{
+		.key_offs = 0,
+		.iv_offs = AES_MAX_KEY_SIZE,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_AES | SPA_CTRL_CIPH_MODE_ECB,
+		.alg = {
+			.cra_name = "ecb(aes)",
+			.cra_driver_name = "ecb-aes-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_aes_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = AES_MIN_KEY_SIZE,
+				.max_keysize = AES_MAX_KEY_SIZE,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_DES | SPA_CTRL_CIPH_MODE_CBC,
+		.alg = {
+			.cra_name = "cbc(des)",
+			.cra_driver_name = "cbc-des-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_des_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = DES_KEY_SIZE,
+				.max_keysize = DES_KEY_SIZE,
+				.ivsize = DES_BLOCK_SIZE,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_DES | SPA_CTRL_CIPH_MODE_ECB,
+		.alg = {
+			.cra_name = "ecb(des)",
+			.cra_driver_name = "ecb-des-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_des_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = DES_KEY_SIZE,
+				.max_keysize = DES_KEY_SIZE,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_DES | SPA_CTRL_CIPH_MODE_CBC,
+		.alg = {
+			.cra_name = "cbc(des3_ede)",
+			.cra_driver_name = "cbc-des3-ede-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_des_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = DES3_EDE_KEY_SIZE,
+				.max_keysize = DES3_EDE_KEY_SIZE,
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_DES | SPA_CTRL_CIPH_MODE_ECB,
+		.alg = {
+			.cra_name = "ecb(des3_ede)",
+			.cra_driver_name = "ecb-des3-ede-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_des_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = DES3_EDE_KEY_SIZE,
+				.max_keysize = DES3_EDE_KEY_SIZE,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+	{
+		.ctrl_default = SPA_CTRL_CIPH_ALG_AES | SPA_CTRL_CIPH_MODE_CBC |
+				SPA_CTRL_HASH_ALG_SHA | SPA_CTRL_HASH_MODE_HMAC,
+		.key_offs = 0,
+		.iv_offs = AES_MAX_KEY_SIZE,
+		.alg = {
+			.cra_name = "authenc(hmac(sha1),cbc(aes))",
+			.cra_driver_name = "authenc-hmac-sha1-cbc-aes-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_aead_ctx),
+			.cra_type = &crypto_aead_type,
+			.cra_module = THIS_MODULE,
+			.cra_aead = {
+				.setkey = spacc_aead_setkey,
+				.setauthsize = spacc_aead_setauthsize,
+				.encrypt = spacc_aead_encrypt,
+				.decrypt = spacc_aead_decrypt,
+				.givencrypt = spacc_aead_givencrypt,
+				.ivsize = AES_BLOCK_SIZE,
+				.maxauthsize = SHA1_DIGEST_SIZE,
+			},
+			.cra_init = spacc_aead_cra_init,
+			.cra_exit = spacc_aead_cra_exit,
+		},
+	},
+	{
+		.ctrl_default = SPA_CTRL_CIPH_ALG_AES | SPA_CTRL_CIPH_MODE_CBC |
+				SPA_CTRL_HASH_ALG_SHA256 |
+				SPA_CTRL_HASH_MODE_HMAC,
+		.key_offs = 0,
+		.iv_offs = AES_MAX_KEY_SIZE,
+		.alg = {
+			.cra_name = "authenc(hmac(sha256),cbc(aes))",
+			.cra_driver_name = "authenc-hmac-sha256-cbc-aes-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_aead_ctx),
+			.cra_type = &crypto_aead_type,
+			.cra_module = THIS_MODULE,
+			.cra_aead = {
+				.setkey = spacc_aead_setkey,
+				.setauthsize = spacc_aead_setauthsize,
+				.encrypt = spacc_aead_encrypt,
+				.decrypt = spacc_aead_decrypt,
+				.givencrypt = spacc_aead_givencrypt,
+				.ivsize = AES_BLOCK_SIZE,
+				.maxauthsize = SHA256_DIGEST_SIZE,
+			},
+			.cra_init = spacc_aead_cra_init,
+			.cra_exit = spacc_aead_cra_exit,
+		},
+	},
+	{
+		.key_offs = 0,
+		.iv_offs = AES_MAX_KEY_SIZE,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_AES | SPA_CTRL_CIPH_MODE_CBC |
+				SPA_CTRL_HASH_ALG_MD5 | SPA_CTRL_HASH_MODE_HMAC,
+		.alg = {
+			.cra_name = "authenc(hmac(md5),cbc(aes))",
+			.cra_driver_name = "authenc-hmac-md5-cbc-aes-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_aead_ctx),
+			.cra_type = &crypto_aead_type,
+			.cra_module = THIS_MODULE,
+			.cra_aead = {
+				.setkey = spacc_aead_setkey,
+				.setauthsize = spacc_aead_setauthsize,
+				.encrypt = spacc_aead_encrypt,
+				.decrypt = spacc_aead_decrypt,
+				.givencrypt = spacc_aead_givencrypt,
+				.ivsize = AES_BLOCK_SIZE,
+				.maxauthsize = MD5_DIGEST_SIZE,
+			},
+			.cra_init = spacc_aead_cra_init,
+			.cra_exit = spacc_aead_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_DES | SPA_CTRL_CIPH_MODE_CBC |
+				SPA_CTRL_HASH_ALG_SHA | SPA_CTRL_HASH_MODE_HMAC,
+		.alg = {
+			.cra_name = "authenc(hmac(sha1),cbc(des3_ede))",
+			.cra_driver_name = "authenc-hmac-sha1-cbc-3des-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_aead_ctx),
+			.cra_type = &crypto_aead_type,
+			.cra_module = THIS_MODULE,
+			.cra_aead = {
+				.setkey = spacc_aead_setkey,
+				.setauthsize = spacc_aead_setauthsize,
+				.encrypt = spacc_aead_encrypt,
+				.decrypt = spacc_aead_decrypt,
+				.givencrypt = spacc_aead_givencrypt,
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+				.maxauthsize = SHA1_DIGEST_SIZE,
+			},
+			.cra_init = spacc_aead_cra_init,
+			.cra_exit = spacc_aead_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_AES | SPA_CTRL_CIPH_MODE_CBC |
+				SPA_CTRL_HASH_ALG_SHA256 |
+				SPA_CTRL_HASH_MODE_HMAC,
+		.alg = {
+			.cra_name = "authenc(hmac(sha256),cbc(des3_ede))",
+			.cra_driver_name = "authenc-hmac-sha256-cbc-3des-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_aead_ctx),
+			.cra_type = &crypto_aead_type,
+			.cra_module = THIS_MODULE,
+			.cra_aead = {
+				.setkey = spacc_aead_setkey,
+				.setauthsize = spacc_aead_setauthsize,
+				.encrypt = spacc_aead_encrypt,
+				.decrypt = spacc_aead_decrypt,
+				.givencrypt = spacc_aead_givencrypt,
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+				.maxauthsize = SHA256_DIGEST_SIZE,
+			},
+			.cra_init = spacc_aead_cra_init,
+			.cra_exit = spacc_aead_cra_exit,
+		},
+	},
+	{
+		.key_offs = DES_BLOCK_SIZE,
+		.iv_offs = 0,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_DES | SPA_CTRL_CIPH_MODE_CBC |
+				SPA_CTRL_HASH_ALG_MD5 | SPA_CTRL_HASH_MODE_HMAC,
+		.alg = {
+			.cra_name = "authenc(hmac(md5),cbc(des3_ede))",
+			.cra_driver_name = "authenc-hmac-md5-cbc-3des-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct spacc_aead_ctx),
+			.cra_type = &crypto_aead_type,
+			.cra_module = THIS_MODULE,
+			.cra_aead = {
+				.setkey = spacc_aead_setkey,
+				.setauthsize = spacc_aead_setauthsize,
+				.encrypt = spacc_aead_encrypt,
+				.decrypt = spacc_aead_decrypt,
+				.givencrypt = spacc_aead_givencrypt,
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+				.maxauthsize = MD5_DIGEST_SIZE,
+			},
+			.cra_init = spacc_aead_cra_init,
+			.cra_exit = spacc_aead_cra_exit,
+		},
+	},
+};
+
+static struct spacc_alg l2_engine_algs[] = {
+	{
+		.key_offs = 0,
+		.iv_offs = SPACC_CRYPTO_KASUMI_F8_KEY_LEN,
+		.ctrl_default = SPA_CTRL_CIPH_ALG_KASUMI |
+				SPA_CTRL_CIPH_MODE_F8,
+		.alg = {
+			.cra_name = "f8(kasumi)",
+			.cra_driver_name = "f8-kasumi-picoxcell",
+			.cra_priority = SPACC_CRYPTO_ALG_PRIORITY,
+			.cra_flags = CRYPTO_ALG_TYPE_GIVCIPHER | CRYPTO_ALG_ASYNC,
+			.cra_blocksize = 8,
+			.cra_ctxsize = sizeof(struct spacc_ablk_ctx),
+			.cra_type = &crypto_ablkcipher_type,
+			.cra_module = THIS_MODULE,
+			.cra_ablkcipher = {
+				.setkey = spacc_kasumi_f8_setkey,
+				.encrypt = spacc_ablk_encrypt,
+				.decrypt = spacc_ablk_decrypt,
+				.min_keysize = 16,
+				.max_keysize = 16,
+				.ivsize = 8,
+			},
+			.cra_init = spacc_ablk_cra_init,
+			.cra_exit = spacc_ablk_cra_exit,
+		},
+	},
+};
+
+static int __devinit spacc_probe(struct platform_device *pdev,
+				 unsigned max_ctxs, size_t cipher_pg_sz,
+				 size_t hash_pg_sz, size_t fifo_sz,
+				 struct spacc_alg *algs, size_t num_algs)
+{
+	int i, err, ret = -EINVAL;
+	struct resource *mem, *irq;
+	struct spacc_engine *engine = devm_kzalloc(&pdev->dev, sizeof(*engine),
+						   GFP_KERNEL);
+	if (!engine)
+		return -ENOMEM;
+
+	engine->max_ctxs	= max_ctxs;
+	engine->cipher_pg_sz	= cipher_pg_sz;
+	engine->hash_pg_sz	= hash_pg_sz;
+	engine->fifo_sz		= fifo_sz;
+	engine->algs		= algs;
+	engine->num_algs	= num_algs;
+	engine->name		= dev_name(&pdev->dev);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!mem || !irq) {
+		dev_err(&pdev->dev, "no memory/irq resource for engine\n");
+		return -ENXIO;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, mem->start, resource_size(mem),
+				     engine->name))
+		return -ENOMEM;
+
+	engine->regs = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
+	if (!engine->regs) {
+		dev_err(&pdev->dev, "memory map failed\n");
+		return -ENOMEM;
+	}
+
+	if (devm_request_irq(&pdev->dev, irq->start, spacc_spacc_irq, 0,
+			     engine->name, engine)) {
+		dev_err(engine->dev, "failed to request IRQ\n");
+		return -EBUSY;
+	}
+
+	engine->dev		= &pdev->dev;
+	engine->cipher_ctx_base = engine->regs + SPA_CIPH_KEY_BASE_REG_OFFSET;
+	engine->hash_key_base	= engine->regs + SPA_HASH_KEY_BASE_REG_OFFSET;
+
+	engine->req_pool = dmam_pool_create(engine->name, engine->dev,
+		MAX_DDT_LEN * sizeof(struct spacc_ddt), 8, SZ_64K);
+	if (!engine->req_pool)
+		return -ENOMEM;
+
+	spin_lock_init(&engine->hw_lock);
+
+	engine->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(engine->clk)) {
+		dev_info(&pdev->dev, "clk unavailable\n");
+		device_remove_file(&pdev->dev, &dev_attr_stat_irq_thresh);
+		return PTR_ERR(engine->clk);
+	}
+
+	if (clk_enable(engine->clk)) {
+		dev_info(&pdev->dev, "unable to enable clk\n");
+		clk_put(engine->clk);
+		return -EIO;
+	}
+
+	err = device_create_file(&pdev->dev, &dev_attr_stat_irq_thresh);
+	if (err) {
+		clk_disable(engine->clk);
+		clk_put(engine->clk);
+		return err;
+	}
+
+
+	/*
+	 * Use an IRQ threshold of 50% as a default. This seems to be a
+	 * reasonable trade off of latency against throughput but can be
+	 * changed at runtime.
+	 */
+	engine->stat_irq_thresh = (engine->fifo_sz / 2);
+
+	/*
+	 * Configure the interrupts. We only use the STAT_CNT interrupt as we
+	 * only submit a new packet for processing when we complete another in
+	 * the queue. This minimizes time spent in the interrupt handler.
+	 */
+	writel(engine->stat_irq_thresh << SPA_IRQ_CTRL_STAT_CNT_OFFSET,
+	       engine->regs + SPA_IRQ_CTRL_REG_OFFSET);
+	writel(SPA_IRQ_EN_STAT_EN | SPA_IRQ_EN_GLBL_EN,
+	       engine->regs + SPA_IRQ_EN_REG_OFFSET);
+
+	setup_timer(&engine->packet_timeout, spacc_packet_timeout,
+		    (unsigned long)engine);
+
+	INIT_LIST_HEAD(&engine->pending);
+	INIT_LIST_HEAD(&engine->completed);
+	INIT_LIST_HEAD(&engine->in_progress);
+	engine->in_flight = 0;
+	tasklet_init(&engine->complete, spacc_spacc_complete,
+		     (unsigned long)engine);
+
+	platform_set_drvdata(pdev, engine);
+
+	INIT_LIST_HEAD(&engine->registered_algs);
+	for (i = 0; i < engine->num_algs; ++i) {
+		engine->algs[i].engine = engine;
+		err = crypto_register_alg(&engine->algs[i].alg);
+		if (!err) {
+			list_add_tail(&engine->algs[i].entry,
+				      &engine->registered_algs);
+			ret = 0;
+		}
+		if (err)
+			dev_err(engine->dev, "failed to register alg \"%s\"\n",
+				engine->algs[i].alg.cra_name);
+		else
+			dev_dbg(engine->dev, "registered alg \"%s\"\n",
+				engine->algs[i].alg.cra_name);
+	}
+
+	return ret;
+}
+
+static int __devexit spacc_remove(struct platform_device *pdev)
+{
+	struct spacc_alg *alg, *next;
+	struct spacc_engine *engine = platform_get_drvdata(pdev);
+
+	del_timer_sync(&engine->packet_timeout);
+	device_remove_file(&pdev->dev, &dev_attr_stat_irq_thresh);
+
+	list_for_each_entry_safe(alg, next, &engine->registered_algs, entry) {
+		list_del(&alg->entry);
+		crypto_unregister_alg(&alg->alg);
+	}
+
+	clk_disable(engine->clk);
+	clk_put(engine->clk);
+
+	return 0;
+}
+
+static int __devinit ipsec_probe(struct platform_device *pdev)
+{
+	return spacc_probe(pdev, SPACC_CRYPTO_IPSEC_MAX_CTXS,
+			   SPACC_CRYPTO_IPSEC_CIPHER_PG_SZ,
+			   SPACC_CRYPTO_IPSEC_HASH_PG_SZ,
+			   SPACC_CRYPTO_IPSEC_FIFO_SZ, ipsec_engine_algs,
+			   ARRAY_SIZE(ipsec_engine_algs));
+}
+
+static struct platform_driver ipsec_driver = {
+	.probe		= ipsec_probe,
+	.remove		= __devexit_p(spacc_remove),
+	.driver		= {
+		.name	= "picoxcell-ipsec",
+#ifdef CONFIG_PM
+		.pm	= &spacc_pm_ops,
+#endif /* CONFIG_PM */
+	},
+};
+
+static int __devinit l2_probe(struct platform_device *pdev)
+{
+	return spacc_probe(pdev, SPACC_CRYPTO_L2_MAX_CTXS,
+			   SPACC_CRYPTO_L2_CIPHER_PG_SZ,
+			   SPACC_CRYPTO_L2_HASH_PG_SZ, SPACC_CRYPTO_L2_FIFO_SZ,
+			   l2_engine_algs, ARRAY_SIZE(l2_engine_algs));
+}
+
+static struct platform_driver l2_driver = {
+	.probe		= l2_probe,
+	.remove		= __devexit_p(spacc_remove),
+	.driver		= {
+		.name	= "picoxcell-l2",
+#ifdef CONFIG_PM
+		.pm	= &spacc_pm_ops,
+#endif /* CONFIG_PM */
+	},
+};
+
+static int __init spacc_init(void)
+{
+	int ret = platform_driver_register(&ipsec_driver);
+	if (ret) {
+		pr_err("failed to register ipsec spacc driver");
+		goto out;
+	}
+
+	ret = platform_driver_register(&l2_driver);
+	if (ret) {
+		pr_err("failed to register l2 spacc driver");
+		goto l2_failed;
+	}
+
+	return 0;
+
+l2_failed:
+	platform_driver_unregister(&ipsec_driver);
+out:
+	return ret;
+}
+module_init(spacc_init);
+
+static void __exit spacc_exit(void)
+{
+	platform_driver_unregister(&ipsec_driver);
+	platform_driver_unregister(&l2_driver);
+}
+module_exit(spacc_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jamie Iles");
diff --git a/drivers/crypto/picoxcell_crypto_regs.h b/drivers/crypto/picoxcell_crypto_regs.h
new file mode 100644
index 000000000000..af93442564c9
--- /dev/null
+++ b/drivers/crypto/picoxcell_crypto_regs.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2010 Picochip Ltd., Jamie Iles
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef __PICOXCELL_CRYPTO_REGS_H__
+#define __PICOXCELL_CRYPTO_REGS_H__
+
+#define SPA_STATUS_OK			0
+#define SPA_STATUS_ICV_FAIL		1
+#define SPA_STATUS_MEMORY_ERROR		2
+#define SPA_STATUS_BLOCK_ERROR		3
+
+#define SPA_IRQ_CTRL_STAT_CNT_OFFSET	16
+#define SPA_IRQ_STAT_STAT_MASK		(1 << 4)
+#define SPA_FIFO_STAT_STAT_OFFSET	16
+#define SPA_FIFO_STAT_STAT_CNT_MASK	(0x3F << SPA_FIFO_STAT_STAT_OFFSET)
+#define SPA_STATUS_RES_CODE_OFFSET	24
+#define SPA_STATUS_RES_CODE_MASK	(0x3 << SPA_STATUS_RES_CODE_OFFSET)
+#define SPA_KEY_SZ_CTX_INDEX_OFFSET	8
+#define SPA_KEY_SZ_CIPHER_OFFSET	31
+
+#define SPA_IRQ_EN_REG_OFFSET		0x00000000
+#define SPA_IRQ_STAT_REG_OFFSET		0x00000004
+#define SPA_IRQ_CTRL_REG_OFFSET		0x00000008
+#define SPA_FIFO_STAT_REG_OFFSET	0x0000000C
+#define SPA_SDMA_BRST_SZ_REG_OFFSET	0x00000010
+#define SPA_SRC_PTR_REG_OFFSET		0x00000020
+#define SPA_DST_PTR_REG_OFFSET		0x00000024
+#define SPA_OFFSET_REG_OFFSET		0x00000028
+#define SPA_AAD_LEN_REG_OFFSET		0x0000002C
+#define SPA_PROC_LEN_REG_OFFSET		0x00000030
+#define SPA_ICV_LEN_REG_OFFSET		0x00000034
+#define SPA_ICV_OFFSET_REG_OFFSET	0x00000038
+#define SPA_SW_CTRL_REG_OFFSET		0x0000003C
+#define SPA_CTRL_REG_OFFSET		0x00000040
+#define SPA_AUX_INFO_REG_OFFSET		0x0000004C
+#define SPA_STAT_POP_REG_OFFSET		0x00000050
+#define SPA_STATUS_REG_OFFSET		0x00000054
+#define SPA_KEY_SZ_REG_OFFSET		0x00000100
+#define SPA_CIPH_KEY_BASE_REG_OFFSET	0x00004000
+#define SPA_HASH_KEY_BASE_REG_OFFSET	0x00008000
+#define SPA_RC4_CTX_BASE_REG_OFFSET	0x00020000
+
+#define SPA_IRQ_EN_REG_RESET		0x00000000
+#define SPA_IRQ_CTRL_REG_RESET		0x00000000
+#define SPA_FIFO_STAT_REG_RESET		0x00000000
+#define SPA_SDMA_BRST_SZ_REG_RESET	0x00000000
+#define SPA_SRC_PTR_REG_RESET		0x00000000
+#define SPA_DST_PTR_REG_RESET		0x00000000
+#define SPA_OFFSET_REG_RESET		0x00000000
+#define SPA_AAD_LEN_REG_RESET		0x00000000
+#define SPA_PROC_LEN_REG_RESET		0x00000000
+#define SPA_ICV_LEN_REG_RESET		0x00000000
+#define SPA_ICV_OFFSET_REG_RESET	0x00000000
+#define SPA_SW_CTRL_REG_RESET		0x00000000
+#define SPA_CTRL_REG_RESET		0x00000000
+#define SPA_AUX_INFO_REG_RESET		0x00000000
+#define SPA_STAT_POP_REG_RESET		0x00000000
+#define SPA_STATUS_REG_RESET		0x00000000
+#define SPA_KEY_SZ_REG_RESET		0x00000000
+
+#define SPA_CTRL_HASH_ALG_IDX		4
+#define SPA_CTRL_CIPH_MODE_IDX		8
+#define SPA_CTRL_HASH_MODE_IDX		12
+#define SPA_CTRL_CTX_IDX		16
+#define SPA_CTRL_ENCRYPT_IDX		24
+#define SPA_CTRL_AAD_COPY		25
+#define SPA_CTRL_ICV_PT			26
+#define SPA_CTRL_ICV_ENC		27
+#define SPA_CTRL_ICV_APPEND		28
+#define SPA_CTRL_KEY_EXP		29
+
+#define SPA_KEY_SZ_CXT_IDX		8
+#define SPA_KEY_SZ_CIPHER_IDX		31
+
+#define SPA_IRQ_EN_CMD0_EN		(1 << 0)
+#define SPA_IRQ_EN_STAT_EN		(1 << 4)
+#define SPA_IRQ_EN_GLBL_EN		(1 << 31)
+
+#define SPA_CTRL_CIPH_ALG_NULL		0x00
+#define SPA_CTRL_CIPH_ALG_DES		0x01
+#define SPA_CTRL_CIPH_ALG_AES		0x02
+#define SPA_CTRL_CIPH_ALG_RC4		0x03
+#define SPA_CTRL_CIPH_ALG_MULTI2	0x04
+#define SPA_CTRL_CIPH_ALG_KASUMI	0x05
+
+#define SPA_CTRL_HASH_ALG_NULL		(0x00 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_MD5		(0x01 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_SHA		(0x02 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_SHA224	(0x03 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_SHA256	(0x04 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_SHA384	(0x05 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_SHA512	(0x06 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_AESMAC	(0x07 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_AESCMAC	(0x08 << SPA_CTRL_HASH_ALG_IDX)
+#define SPA_CTRL_HASH_ALG_KASF9		(0x09 << SPA_CTRL_HASH_ALG_IDX)
+
+#define SPA_CTRL_CIPH_MODE_NULL		(0x00 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_ECB		(0x00 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_CBC		(0x01 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_CTR		(0x02 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_CCM		(0x03 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_GCM		(0x05 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_OFB		(0x07 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_CFB		(0x08 << SPA_CTRL_CIPH_MODE_IDX)
+#define SPA_CTRL_CIPH_MODE_F8		(0x09 << SPA_CTRL_CIPH_MODE_IDX)
+
+#define SPA_CTRL_HASH_MODE_RAW		(0x00 << SPA_CTRL_HASH_MODE_IDX)
+#define SPA_CTRL_HASH_MODE_SSLMAC	(0x01 << SPA_CTRL_HASH_MODE_IDX)
+#define SPA_CTRL_HASH_MODE_HMAC		(0x02 << SPA_CTRL_HASH_MODE_IDX)
+
+#define SPA_FIFO_STAT_EMPTY		(1 << 31)
+#define SPA_FIFO_CMD_FULL		(1 << 7)
+
+#endif /* __PICOXCELL_CRYPTO_REGS_H__ */
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index b879c3f5d7c0..854e2632f9a6 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -2402,8 +2402,7 @@ static struct talitos_crypto_alg *talitos_alg_alloc(struct device *dev,
 	return t_alg;
 }
 
-static int talitos_probe(struct platform_device *ofdev,
-			 const struct of_device_id *match)
+static int talitos_probe(struct platform_device *ofdev)
 {
 	struct device *dev = &ofdev->dev;
 	struct device_node *np = ofdev->dev.of_node;
@@ -2580,7 +2579,7 @@ static const struct of_device_id talitos_match[] = {
 };
 MODULE_DEVICE_TABLE(of, talitos_match);
 
-static struct of_platform_driver talitos_driver = {
+static struct platform_driver talitos_driver = {
 	.driver = {
 		.name = "talitos",
 		.owner = THIS_MODULE,
@@ -2592,13 +2591,13 @@ static struct of_platform_driver talitos_driver = {
 
 static int __init talitos_init(void)
 {
-	return of_register_platform_driver(&talitos_driver);
+	return platform_driver_register(&talitos_driver);
 }
 module_init(talitos_init);
 
 static void __exit talitos_exit(void)
 {
-	of_unregister_platform_driver(&talitos_driver);
+	platform_driver_unregister(&talitos_driver);
 }
 module_exit(talitos_exit);
 
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 07bca4970e50..e6d7228b1479 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -1845,7 +1845,7 @@ static inline void init_pl08x_debugfs(struct pl08x_driver_data *pl08x)
 }
 #endif
 
-static int pl08x_probe(struct amba_device *adev, struct amba_id *id)
+static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	struct pl08x_driver_data *pl08x;
 	const struct vendor_data *vd = id->data;
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index a6656834f0ff..00deabd9a04b 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -849,7 +849,7 @@ static irqreturn_t dma_irq_handler(int irq, void *dev_id)
 
 				/* Must clear TC interrupt before calling
 				 * dma_tc_handle
-				 * in case tc_handle initate a new dma job
+				 * in case tc_handle initiate a new dma job
 				 */
 				__set_bit(i, virtbase + COH901318_TC_INT_CLEAR1);
 
@@ -894,7 +894,7 @@ static irqreturn_t dma_irq_handler(int irq, void *dev_id)
 				}
 				/* Must clear TC interrupt before calling
 				 * dma_tc_handle
-				 * in case tc_handle initate a new dma job
+				 * in case tc_handle initiate a new dma job
 				 */
 				__set_bit(i, virtbase + COH901318_TC_INT_CLEAR2);
 
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index a3991ab0d67e..08dab3badad2 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -32,15 +32,18 @@
  * which does not support descriptor writeback.
  */
 
-/* NOTE:  DMS+SMS is system-specific. We should get this information
- * from the platform code somehow.
- */
-#define DWC_DEFAULT_CTLLO	(DWC_CTLL_DST_MSIZE(0)		\
-				| DWC_CTLL_SRC_MSIZE(0)		\
-				| DWC_CTLL_DMS(0)		\
-				| DWC_CTLL_SMS(1)		\
-				| DWC_CTLL_LLP_D_EN		\
-				| DWC_CTLL_LLP_S_EN)
+#define DWC_DEFAULT_CTLLO(private) ({				\
+		struct dw_dma_slave *__slave = (private);	\
+		int dms = __slave ? __slave->dst_master : 0;	\
+		int sms = __slave ? __slave->src_master : 1;	\
+								\
+		(DWC_CTLL_DST_MSIZE(0)				\
+		 | DWC_CTLL_SRC_MSIZE(0)			\
+		 | DWC_CTLL_LLP_D_EN				\
+		 | DWC_CTLL_LLP_S_EN				\
+		 | DWC_CTLL_DMS(dms)				\
+		 | DWC_CTLL_SMS(sms));				\
+	})
 
 /*
  * This is configuration-dependent and usually a funny size like 4095.
@@ -291,6 +294,9 @@ static void dwc_scan_descriptors(struct dw_dma *dw, struct dw_dma_chan *dwc)
 		return;
 	}
 
+	if (list_empty(&dwc->active_list))
+		return;
+
 	dev_vdbg(chan2dev(&dwc->chan), "scan_descriptors: llp=0x%x\n", llp);
 
 	list_for_each_entry_safe(desc, _desc, &dwc->active_list, desc_node) {
@@ -588,7 +594,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	else
 		src_width = dst_width = 0;
 
-	ctllo = DWC_DEFAULT_CTLLO
+	ctllo = DWC_DEFAULT_CTLLO(chan->private)
 			| DWC_CTLL_DST_WIDTH(dst_width)
 			| DWC_CTLL_SRC_WIDTH(src_width)
 			| DWC_CTLL_DST_INC
@@ -669,7 +675,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 
 	switch (direction) {
 	case DMA_TO_DEVICE:
-		ctllo = (DWC_DEFAULT_CTLLO
+		ctllo = (DWC_DEFAULT_CTLLO(chan->private)
 				| DWC_CTLL_DST_WIDTH(reg_width)
 				| DWC_CTLL_DST_FIX
 				| DWC_CTLL_SRC_INC
@@ -714,7 +720,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		}
 		break;
 	case DMA_FROM_DEVICE:
-		ctllo = (DWC_DEFAULT_CTLLO
+		ctllo = (DWC_DEFAULT_CTLLO(chan->private)
 				| DWC_CTLL_SRC_WIDTH(reg_width)
 				| DWC_CTLL_DST_INC
 				| DWC_CTLL_SRC_FIX
@@ -1126,7 +1132,7 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 		case DMA_TO_DEVICE:
 			desc->lli.dar = dws->tx_reg;
 			desc->lli.sar = buf_addr + (period_len * i);
-			desc->lli.ctllo = (DWC_DEFAULT_CTLLO
+			desc->lli.ctllo = (DWC_DEFAULT_CTLLO(chan->private)
 					| DWC_CTLL_DST_WIDTH(reg_width)
 					| DWC_CTLL_SRC_WIDTH(reg_width)
 					| DWC_CTLL_DST_FIX
@@ -1137,7 +1143,7 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 		case DMA_FROM_DEVICE:
 			desc->lli.dar = buf_addr + (period_len * i);
 			desc->lli.sar = dws->rx_reg;
-			desc->lli.ctllo = (DWC_DEFAULT_CTLLO
+			desc->lli.ctllo = (DWC_DEFAULT_CTLLO(chan->private)
 					| DWC_CTLL_SRC_WIDTH(reg_width)
 					| DWC_CTLL_DST_WIDTH(reg_width)
 					| DWC_CTLL_DST_INC
@@ -1335,6 +1341,8 @@ static int __init dw_probe(struct platform_device *pdev)
 
 	dma_cap_set(DMA_MEMCPY, dw->dma.cap_mask);
 	dma_cap_set(DMA_SLAVE, dw->dma.cap_mask);
+	if (pdata->is_private)
+		dma_cap_set(DMA_PRIVATE, dw->dma.cap_mask);
 	dw->dma.dev = &pdev->dev;
 	dw->dma.device_alloc_chan_resources = dwc_alloc_chan_resources;
 	dw->dma.device_free_chan_resources = dwc_free_chan_resources;
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index 4de947a450fc..e3854a8f0de0 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -1281,8 +1281,7 @@ static void fsl_dma_chan_remove(struct fsldma_chan *chan)
 	kfree(chan);
 }
 
-static int __devinit fsldma_of_probe(struct platform_device *op,
-			const struct of_device_id *match)
+static int __devinit fsldma_of_probe(struct platform_device *op)
 {
 	struct fsldma_device *fdev;
 	struct device_node *child;
@@ -1414,20 +1413,13 @@ static struct of_platform_driver fsldma_of_driver = {
 
 static __init int fsldma_init(void)
 {
-	int ret;
-
 	pr_info("Freescale Elo / Elo Plus DMA driver\n");
-
-	ret = of_register_platform_driver(&fsldma_of_driver);
-	if (ret)
-		pr_err("fsldma: failed to register platform driver\n");
-
-	return ret;
+	return platform_driver_register(&fsldma_of_driver);
 }
 
 static void __exit fsldma_exit(void)
 {
-	of_unregister_platform_driver(&fsldma_of_driver);
+	platform_driver_unregister(&fsldma_of_driver);
 }
 
 subsys_initcall(fsldma_init);
diff --git a/drivers/dma/mpc512x_dma.c b/drivers/dma/mpc512x_dma.c
index 59c270192ccc..4f95d31f5a20 100644
--- a/drivers/dma/mpc512x_dma.c
+++ b/drivers/dma/mpc512x_dma.c
@@ -649,8 +649,7 @@ mpc_dma_prep_memcpy(struct dma_chan *chan, dma_addr_t dst, dma_addr_t src,
 	return &mdesc->desc;
 }
 
-static int __devinit mpc_dma_probe(struct platform_device *op,
-					const struct of_device_id *match)
+static int __devinit mpc_dma_probe(struct platform_device *op)
 {
 	struct device_node *dn = op->dev.of_node;
 	struct device *dev = &op->dev;
@@ -827,7 +826,7 @@ static struct of_device_id mpc_dma_match[] = {
 	{},
 };
 
-static struct of_platform_driver mpc_dma_driver = {
+static struct platform_driver mpc_dma_driver = {
 	.probe		= mpc_dma_probe,
 	.remove		= __devexit_p(mpc_dma_remove),
 	.driver = {
@@ -839,13 +838,13 @@ static struct of_platform_driver mpc_dma_driver = {
 
 static int __init mpc_dma_init(void)
 {
-	return of_register_platform_driver(&mpc_dma_driver);
+	return platform_driver_register(&mpc_dma_driver);
 }
 module_init(mpc_dma_init);
 
 static void __exit mpc_dma_exit(void)
 {
-	of_unregister_platform_driver(&mpc_dma_driver);
+	platform_driver_unregister(&mpc_dma_driver);
 }
 module_exit(mpc_dma_exit);
 
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 7c50f6dfd3f4..6abe1ec1f2ce 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -657,7 +657,7 @@ static irqreturn_t pl330_irq_handler(int irq, void *data)
 }
 
 static int __devinit
-pl330_probe(struct amba_device *adev, struct amba_id *id)
+pl330_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	struct dma_pl330_platdata *pdat;
 	struct dma_pl330_dmac *pdmac;
diff --git a/drivers/dma/ppc4xx/adma.c b/drivers/dma/ppc4xx/adma.c
index cef584533ee8..3b0247e74cc4 100644
--- a/drivers/dma/ppc4xx/adma.c
+++ b/drivers/dma/ppc4xx/adma.c
@@ -4393,8 +4393,7 @@ static void ppc440spe_adma_release_irqs(struct ppc440spe_adma_device *adev,
 /**
  * ppc440spe_adma_probe - probe the asynch device
  */
-static int __devinit ppc440spe_adma_probe(struct platform_device *ofdev,
-					  const struct of_device_id *match)
+static int __devinit ppc440spe_adma_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct resource res;
@@ -4944,7 +4943,7 @@ static const struct of_device_id ppc440spe_adma_of_match[] __devinitconst = {
 };
 MODULE_DEVICE_TABLE(of, ppc440spe_adma_of_match);
 
-static struct of_platform_driver ppc440spe_adma_driver = {
+static struct platform_driver ppc440spe_adma_driver = {
 	.probe = ppc440spe_adma_probe,
 	.remove = __devexit_p(ppc440spe_adma_remove),
 	.driver = {
@@ -4962,7 +4961,7 @@ static __init int ppc440spe_adma_init(void)
 	if (ret)
 		return ret;
 
-	ret = of_register_platform_driver(&ppc440spe_adma_driver);
+	ret = platform_driver_register(&ppc440spe_adma_driver);
 	if (ret) {
 		pr_err("%s: failed to register platform driver\n",
 			__func__);
@@ -4996,7 +4995,7 @@ out_dev:
 	/* User will not be able to enable h/w RAID-6 */
 	pr_err("%s: failed to create RAID-6 driver interface\n",
 		__func__);
-	of_unregister_platform_driver(&ppc440spe_adma_driver);
+	platform_driver_unregister(&ppc440spe_adma_driver);
 out_reg:
 	dcr_unmap(ppc440spe_mq_dcr_host, ppc440spe_mq_dcr_len);
 	kfree(ppc440spe_dma_fifo_buf);
@@ -5011,7 +5010,7 @@ static void __exit ppc440spe_adma_exit(void)
 			   &driver_attr_enable);
 	driver_remove_file(&ppc440spe_adma_driver.driver,
 			   &driver_attr_devices);
-	of_unregister_platform_driver(&ppc440spe_adma_driver);
+	platform_driver_unregister(&ppc440spe_adma_driver);
 	dcr_unmap(ppc440spe_mq_dcr_host, ppc440spe_mq_dcr_len);
 	kfree(ppc440spe_dma_fifo_buf);
 }
diff --git a/drivers/dma/shdma.c b/drivers/dma/shdma.c
index 28720d3103c4..6451b581a70b 100644
--- a/drivers/dma/shdma.c
+++ b/drivers/dma/shdma.c
@@ -750,7 +750,7 @@ static void sh_chan_xfer_ld_queue(struct sh_dmae_chan *sh_chan)
 		return;
 	}
 
-	/* Find the first not transferred desciptor */
+	/* Find the first not transferred descriptor */
 	list_for_each_entry(desc, &sh_chan->ld_queue, node)
 		if (desc->mark == DESC_SUBMITTED) {
 			dev_dbg(sh_chan->dev, "Queue #%d to %d: %u@%x -> %x\n",
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 6e1d46a65d0e..af955de035f4 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -68,6 +68,7 @@ enum d40_command {
  * @base: Pointer to memory area when the pre_alloc_lli's are not large
  * enough, IE bigger than the most common case, 1 dst and 1 src. NULL if
  * pre_alloc_lli is used.
+ * @dma_addr: DMA address, if mapped
  * @size: The size in bytes of the memory at base or the size of pre_alloc_lli.
  * @pre_alloc_lli: Pre allocated area for the most common case of transfers,
  * one buffer to one buffer.
@@ -75,6 +76,7 @@ enum d40_command {
 struct d40_lli_pool {
 	void	*base;
 	int	 size;
+	dma_addr_t	dma_addr;
 	/* Space for dst and src, plus an extra for padding */
 	u8	 pre_alloc_lli[3 * sizeof(struct d40_phy_lli)];
 };
@@ -94,7 +96,6 @@ struct d40_lli_pool {
  * during a transfer.
  * @node: List entry.
  * @is_in_client_list: true if the client owns this descriptor.
- * @is_hw_linked: true if this job will automatically be continued for
  * the previous one.
  *
  * This descriptor is used for both logical and physical transfers.
@@ -114,7 +115,7 @@ struct d40_desc {
 	struct list_head		 node;
 
 	bool				 is_in_client_list;
-	bool				 is_hw_linked;
+	bool				 cyclic;
 };
 
 /**
@@ -130,6 +131,7 @@ struct d40_desc {
  */
 struct d40_lcla_pool {
 	void		*base;
+	dma_addr_t	dma_addr;
 	void		*base_unaligned;
 	int		 pages;
 	spinlock_t	 lock;
@@ -303,9 +305,37 @@ struct d40_reg_val {
 	unsigned int val;
 };
 
-static int d40_pool_lli_alloc(struct d40_desc *d40d,
-			      int lli_len, bool is_log)
+static struct device *chan2dev(struct d40_chan *d40c)
 {
+	return &d40c->chan.dev->device;
+}
+
+static bool chan_is_physical(struct d40_chan *chan)
+{
+	return chan->log_num == D40_PHY_CHAN;
+}
+
+static bool chan_is_logical(struct d40_chan *chan)
+{
+	return !chan_is_physical(chan);
+}
+
+static void __iomem *chan_base(struct d40_chan *chan)
+{
+	return chan->base->virtbase + D40_DREG_PCBASE +
+	       chan->phy_chan->num * D40_DREG_PCDELTA;
+}
+
+#define d40_err(dev, format, arg...)		\
+	dev_err(dev, "[%s] " format, __func__, ## arg)
+
+#define chan_err(d40c, format, arg...)		\
+	d40_err(chan2dev(d40c), format, ## arg)
+
+static int d40_pool_lli_alloc(struct d40_chan *d40c, struct d40_desc *d40d,
+			      int lli_len)
+{
+	bool is_log = chan_is_logical(d40c);
 	u32 align;
 	void *base;
 
@@ -319,7 +349,7 @@ static int d40_pool_lli_alloc(struct d40_desc *d40d,
 		d40d->lli_pool.size = sizeof(d40d->lli_pool.pre_alloc_lli);
 		d40d->lli_pool.base = NULL;
 	} else {
-		d40d->lli_pool.size = ALIGN(lli_len * 2 * align, align);
+		d40d->lli_pool.size = lli_len * 2 * align;
 
 		base = kmalloc(d40d->lli_pool.size + align, GFP_NOWAIT);
 		d40d->lli_pool.base = base;
@@ -329,22 +359,37 @@ static int d40_pool_lli_alloc(struct d40_desc *d40d,
 	}
 
 	if (is_log) {
-		d40d->lli_log.src = PTR_ALIGN((struct d40_log_lli *) base,
-					      align);
-		d40d->lli_log.dst = PTR_ALIGN(d40d->lli_log.src + lli_len,
-					      align);
+		d40d->lli_log.src = PTR_ALIGN(base, align);
+		d40d->lli_log.dst = d40d->lli_log.src + lli_len;
+
+		d40d->lli_pool.dma_addr = 0;
 	} else {
-		d40d->lli_phy.src = PTR_ALIGN((struct d40_phy_lli *)base,
-					      align);
-		d40d->lli_phy.dst = PTR_ALIGN(d40d->lli_phy.src + lli_len,
-					      align);
+		d40d->lli_phy.src = PTR_ALIGN(base, align);
+		d40d->lli_phy.dst = d40d->lli_phy.src + lli_len;
+
+		d40d->lli_pool.dma_addr = dma_map_single(d40c->base->dev,
+							 d40d->lli_phy.src,
+							 d40d->lli_pool.size,
+							 DMA_TO_DEVICE);
+
+		if (dma_mapping_error(d40c->base->dev,
+				      d40d->lli_pool.dma_addr)) {
+			kfree(d40d->lli_pool.base);
+			d40d->lli_pool.base = NULL;
+			d40d->lli_pool.dma_addr = 0;
+			return -ENOMEM;
+		}
 	}
 
 	return 0;
 }
 
-static void d40_pool_lli_free(struct d40_desc *d40d)
+static void d40_pool_lli_free(struct d40_chan *d40c, struct d40_desc *d40d)
 {
+	if (d40d->lli_pool.dma_addr)
+		dma_unmap_single(d40c->base->dev, d40d->lli_pool.dma_addr,
+				 d40d->lli_pool.size, DMA_TO_DEVICE);
+
 	kfree(d40d->lli_pool.base);
 	d40d->lli_pool.base = NULL;
 	d40d->lli_pool.size = 0;
@@ -391,7 +436,7 @@ static int d40_lcla_free_all(struct d40_chan *d40c,
 	int i;
 	int ret = -EINVAL;
 
-	if (d40c->log_num == D40_PHY_CHAN)
+	if (chan_is_physical(d40c))
 		return 0;
 
 	spin_lock_irqsave(&d40c->base->lcla_pool.lock, flags);
@@ -430,7 +475,7 @@ static struct d40_desc *d40_desc_get(struct d40_chan *d40c)
 
 		list_for_each_entry_safe(d, _d, &d40c->client, node)
 			if (async_tx_test_ack(&d->txd)) {
-				d40_pool_lli_free(d);
+				d40_pool_lli_free(d40c, d);
 				d40_desc_remove(d);
 				desc = d;
 				memset(desc, 0, sizeof(*desc));
@@ -450,6 +495,7 @@ static struct d40_desc *d40_desc_get(struct d40_chan *d40c)
 static void d40_desc_free(struct d40_chan *d40c, struct d40_desc *d40d)
 {
 
+	d40_pool_lli_free(d40c, d40d);
 	d40_lcla_free_all(d40c, d40d);
 	kmem_cache_free(d40c->base->desc_slab, d40d);
 }
@@ -459,57 +505,128 @@ static void d40_desc_submit(struct d40_chan *d40c, struct d40_desc *desc)
 	list_add_tail(&desc->node, &d40c->active);
 }
 
-static void d40_desc_load(struct d40_chan *d40c, struct d40_desc *d40d)
+static void d40_phy_lli_load(struct d40_chan *chan, struct d40_desc *desc)
 {
-	int curr_lcla = -EINVAL, next_lcla;
+	struct d40_phy_lli *lli_dst = desc->lli_phy.dst;
+	struct d40_phy_lli *lli_src = desc->lli_phy.src;
+	void __iomem *base = chan_base(chan);
+
+	writel(lli_src->reg_cfg, base + D40_CHAN_REG_SSCFG);
+	writel(lli_src->reg_elt, base + D40_CHAN_REG_SSELT);
+	writel(lli_src->reg_ptr, base + D40_CHAN_REG_SSPTR);
+	writel(lli_src->reg_lnk, base + D40_CHAN_REG_SSLNK);
+
+	writel(lli_dst->reg_cfg, base + D40_CHAN_REG_SDCFG);
+	writel(lli_dst->reg_elt, base + D40_CHAN_REG_SDELT);
+	writel(lli_dst->reg_ptr, base + D40_CHAN_REG_SDPTR);
+	writel(lli_dst->reg_lnk, base + D40_CHAN_REG_SDLNK);
+}
 
-	if (d40c->log_num == D40_PHY_CHAN) {
-		d40_phy_lli_write(d40c->base->virtbase,
-				  d40c->phy_chan->num,
-				  d40d->lli_phy.dst,
-				  d40d->lli_phy.src);
-		d40d->lli_current = d40d->lli_len;
-	} else {
+static void d40_log_lli_to_lcxa(struct d40_chan *chan, struct d40_desc *desc)
+{
+	struct d40_lcla_pool *pool = &chan->base->lcla_pool;
+	struct d40_log_lli_bidir *lli = &desc->lli_log;
+	int lli_current = desc->lli_current;
+	int lli_len = desc->lli_len;
+	bool cyclic = desc->cyclic;
+	int curr_lcla = -EINVAL;
+	int first_lcla = 0;
+	bool linkback;
 
-		if ((d40d->lli_len - d40d->lli_current) > 1)
-			curr_lcla = d40_lcla_alloc_one(d40c, d40d);
+	/*
+	 * We may have partially running cyclic transfers, in case we did't get
+	 * enough LCLA entries.
+	 */
+	linkback = cyclic && lli_current == 0;
 
-		d40_log_lli_lcpa_write(d40c->lcpa,
-				       &d40d->lli_log.dst[d40d->lli_current],
-				       &d40d->lli_log.src[d40d->lli_current],
-				       curr_lcla);
+	/*
+	 * For linkback, we need one LCLA even with only one link, because we
+	 * can't link back to the one in LCPA space
+	 */
+	if (linkback || (lli_len - lli_current > 1)) {
+		curr_lcla = d40_lcla_alloc_one(chan, desc);
+		first_lcla = curr_lcla;
+	}
 
-		d40d->lli_current++;
-		for (; d40d->lli_current < d40d->lli_len; d40d->lli_current++) {
-			struct d40_log_lli *lcla;
+	/*
+	 * For linkback, we normally load the LCPA in the loop since we need to
+	 * link it to the second LCLA and not the first.  However, if we
+	 * couldn't even get a first LCLA, then we have to run in LCPA and
+	 * reload manually.
+	 */
+	if (!linkback || curr_lcla == -EINVAL) {
+		unsigned int flags = 0;
 
-			if (d40d->lli_current + 1 < d40d->lli_len)
-				next_lcla = d40_lcla_alloc_one(d40c, d40d);
-			else
-				next_lcla = -EINVAL;
+		if (curr_lcla == -EINVAL)
+			flags |= LLI_TERM_INT;
 
-			lcla = d40c->base->lcla_pool.base +
-				d40c->phy_chan->num * 1024 +
-				8 * curr_lcla * 2;
+		d40_log_lli_lcpa_write(chan->lcpa,
+				       &lli->dst[lli_current],
+				       &lli->src[lli_current],
+				       curr_lcla,
+				       flags);
+		lli_current++;
+	}
 
-			d40_log_lli_lcla_write(lcla,
-					       &d40d->lli_log.dst[d40d->lli_current],
-					       &d40d->lli_log.src[d40d->lli_current],
-					       next_lcla);
+	if (curr_lcla < 0)
+		goto out;
 
-			(void) dma_map_single(d40c->base->dev, lcla,
-					      2 * sizeof(struct d40_log_lli),
-					      DMA_TO_DEVICE);
+	for (; lli_current < lli_len; lli_current++) {
+		unsigned int lcla_offset = chan->phy_chan->num * 1024 +
+					   8 * curr_lcla * 2;
+		struct d40_log_lli *lcla = pool->base + lcla_offset;
+		unsigned int flags = 0;
+		int next_lcla;
 
-			curr_lcla = next_lcla;
+		if (lli_current + 1 < lli_len)
+			next_lcla = d40_lcla_alloc_one(chan, desc);
+		else
+			next_lcla = linkback ? first_lcla : -EINVAL;
 
-			if (curr_lcla == -EINVAL) {
-				d40d->lli_current++;
-				break;
-			}
+		if (cyclic || next_lcla == -EINVAL)
+			flags |= LLI_TERM_INT;
+
+		if (linkback && curr_lcla == first_lcla) {
+			/* First link goes in both LCPA and LCLA */
+			d40_log_lli_lcpa_write(chan->lcpa,
+					       &lli->dst[lli_current],
+					       &lli->src[lli_current],
+					       next_lcla, flags);
+		}
+
+		/*
+		 * One unused LCLA in the cyclic case if the very first
+		 * next_lcla fails...
+		 */
+		d40_log_lli_lcla_write(lcla,
+				       &lli->dst[lli_current],
+				       &lli->src[lli_current],
+				       next_lcla, flags);
+
+		dma_sync_single_range_for_device(chan->base->dev,
+					pool->dma_addr, lcla_offset,
+					2 * sizeof(struct d40_log_lli),
+					DMA_TO_DEVICE);
 
+		curr_lcla = next_lcla;
+
+		if (curr_lcla == -EINVAL || curr_lcla == first_lcla) {
+			lli_current++;
+			break;
 		}
 	}
+
+out:
+	desc->lli_current = lli_current;
+}
+
+static void d40_desc_load(struct d40_chan *d40c, struct d40_desc *d40d)
+{
+	if (chan_is_physical(d40c)) {
+		d40_phy_lli_load(d40c, d40d);
+		d40d->lli_current = d40d->lli_len;
+	} else
+		d40_log_lli_to_lcxa(d40c, d40d);
 }
 
 static struct d40_desc *d40_first_active_get(struct d40_chan *d40c)
@@ -543,18 +660,6 @@ static struct d40_desc *d40_first_queued(struct d40_chan *d40c)
 	return d;
 }
 
-static struct d40_desc *d40_last_queued(struct d40_chan *d40c)
-{
-	struct d40_desc *d;
-
-	if (list_empty(&d40c->queue))
-		return NULL;
-	list_for_each_entry(d, &d40c->queue, node)
-		if (list_is_last(&d->node, &d40c->queue))
-			break;
-	return d;
-}
-
 static int d40_psize_2_burst_size(bool is_log, int psize)
 {
 	if (is_log) {
@@ -666,9 +771,9 @@ static int d40_channel_execute_command(struct d40_chan *d40c,
 		}
 
 		if (i == D40_SUSPEND_MAX_IT) {
-			dev_err(&d40c->chan.dev->device,
-				"[%s]: unable to suspend the chl %d (log: %d) status %x\n",
-				__func__, d40c->phy_chan->num, d40c->log_num,
+			chan_err(d40c,
+				"unable to suspend the chl %d (log: %d) status %x\n",
+				d40c->phy_chan->num, d40c->log_num,
 				status);
 			dump_stack();
 			ret = -EBUSY;
@@ -701,17 +806,45 @@ static void d40_term_all(struct d40_chan *d40c)
 	d40c->busy = false;
 }
 
+static void __d40_config_set_event(struct d40_chan *d40c, bool enable,
+				   u32 event, int reg)
+{
+	void __iomem *addr = chan_base(d40c) + reg;
+	int tries;
+
+	if (!enable) {
+		writel((D40_DEACTIVATE_EVENTLINE << D40_EVENTLINE_POS(event))
+		       | ~D40_EVENTLINE_MASK(event), addr);
+		return;
+	}
+
+	/*
+	 * The hardware sometimes doesn't register the enable when src and dst
+	 * event lines are active on the same logical channel.  Retry to ensure
+	 * it does.  Usually only one retry is sufficient.
+	 */
+	tries = 100;
+	while (--tries) {
+		writel((D40_ACTIVATE_EVENTLINE << D40_EVENTLINE_POS(event))
+		       | ~D40_EVENTLINE_MASK(event), addr);
+
+		if (readl(addr) & D40_EVENTLINE_MASK(event))
+			break;
+	}
+
+	if (tries != 99)
+		dev_dbg(chan2dev(d40c),
+			"[%s] workaround enable S%cLNK (%d tries)\n",
+			__func__, reg == D40_CHAN_REG_SSLNK ? 'S' : 'D',
+			100 - tries);
+
+	WARN_ON(!tries);
+}
+
 static void d40_config_set_event(struct d40_chan *d40c, bool do_enable)
 {
-	u32 val;
 	unsigned long flags;
 
-	/* Notice, that disable requires the physical channel to be stopped */
-	if (do_enable)
-		val = D40_ACTIVATE_EVENTLINE;
-	else
-		val = D40_DEACTIVATE_EVENTLINE;
-
 	spin_lock_irqsave(&d40c->phy_chan->lock, flags);
 
 	/* Enable event line connected to device (or memcpy) */
@@ -719,20 +852,15 @@ static void d40_config_set_event(struct d40_chan *d40c, bool do_enable)
 	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH)) {
 		u32 event = D40_TYPE_TO_EVENT(d40c->dma_cfg.src_dev_type);
 
-		writel((val << D40_EVENTLINE_POS(event)) |
-		       ~D40_EVENTLINE_MASK(event),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SSLNK);
+		__d40_config_set_event(d40c, do_enable, event,
+				       D40_CHAN_REG_SSLNK);
 	}
+
 	if (d40c->dma_cfg.dir !=  STEDMA40_PERIPH_TO_MEM) {
 		u32 event = D40_TYPE_TO_EVENT(d40c->dma_cfg.dst_dev_type);
 
-		writel((val << D40_EVENTLINE_POS(event)) |
-		       ~D40_EVENTLINE_MASK(event),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SDLNK);
+		__d40_config_set_event(d40c, do_enable, event,
+				       D40_CHAN_REG_SDLNK);
 	}
 
 	spin_unlock_irqrestore(&d40c->phy_chan->lock, flags);
@@ -740,15 +868,12 @@ static void d40_config_set_event(struct d40_chan *d40c, bool do_enable)
 
 static u32 d40_chan_has_events(struct d40_chan *d40c)
 {
+	void __iomem *chanbase = chan_base(d40c);
 	u32 val;
 
-	val = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-		    d40c->phy_chan->num * D40_DREG_PCDELTA +
-		    D40_CHAN_REG_SSLNK);
+	val = readl(chanbase + D40_CHAN_REG_SSLNK);
+	val |= readl(chanbase + D40_CHAN_REG_SDLNK);
 
-	val |= readl(d40c->base->virtbase + D40_DREG_PCBASE +
-		     d40c->phy_chan->num * D40_DREG_PCDELTA +
-		     D40_CHAN_REG_SDLNK);
 	return val;
 }
 
@@ -771,7 +896,7 @@ static u32 d40_get_prmo(struct d40_chan *d40c)
 			= D40_DREG_PRMO_LCHAN_SRC_LOG_DST_LOG,
 	};
 
-	if (d40c->log_num == D40_PHY_CHAN)
+	if (chan_is_physical(d40c))
 		return phy_map[d40c->dma_cfg.mode_opt];
 	else
 		return log_map[d40c->dma_cfg.mode_opt];
@@ -785,7 +910,7 @@ static void d40_config_write(struct d40_chan *d40c)
 	/* Odd addresses are even addresses + 4 */
 	addr_base = (d40c->phy_chan->num % 2) * 4;
 	/* Setup channel mode to logical or physical */
-	var = ((u32)(d40c->log_num != D40_PHY_CHAN) + 1) <<
+	var = ((u32)(chan_is_logical(d40c)) + 1) <<
 		D40_CHAN_POS(d40c->phy_chan->num);
 	writel(var, d40c->base->virtbase + D40_DREG_PRMSE + addr_base);
 
@@ -794,30 +919,18 @@ static void d40_config_write(struct d40_chan *d40c)
 
 	writel(var, d40c->base->virtbase + D40_DREG_PRMOE + addr_base);
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	if (chan_is_logical(d40c)) {
+		int lidx = (d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS)
+			   & D40_SREG_ELEM_LOG_LIDX_MASK;
+		void __iomem *chanbase = chan_base(d40c);
+
 		/* Set default config for CFG reg */
-		writel(d40c->src_def_cfg,
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SSCFG);
-		writel(d40c->dst_def_cfg,
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SDCFG);
+		writel(d40c->src_def_cfg, chanbase + D40_CHAN_REG_SSCFG);
+		writel(d40c->dst_def_cfg, chanbase + D40_CHAN_REG_SDCFG);
 
 		/* Set LIDX for lcla */
-		writel((d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS) &
-		       D40_SREG_ELEM_LOG_LIDX_MASK,
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SDELT);
-
-		writel((d40c->phy_chan->num << D40_SREG_ELEM_LOG_LIDX_POS) &
-		       D40_SREG_ELEM_LOG_LIDX_MASK,
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SSELT);
-
+		writel(lidx, chanbase + D40_CHAN_REG_SSELT);
+		writel(lidx, chanbase + D40_CHAN_REG_SDELT);
 	}
 }
 
@@ -825,15 +938,15 @@ static u32 d40_residue(struct d40_chan *d40c)
 {
 	u32 num_elt;
 
-	if (d40c->log_num != D40_PHY_CHAN)
+	if (chan_is_logical(d40c))
 		num_elt = (readl(&d40c->lcpa->lcsp2) & D40_MEM_LCSP2_ECNT_MASK)
 			>> D40_MEM_LCSP2_ECNT_POS;
-	else
-		num_elt = (readl(d40c->base->virtbase + D40_DREG_PCBASE +
-				 d40c->phy_chan->num * D40_DREG_PCDELTA +
-				 D40_CHAN_REG_SDELT) &
-			   D40_SREG_ELEM_PHY_ECNT_MASK) >>
-			D40_SREG_ELEM_PHY_ECNT_POS;
+	else {
+		u32 val = readl(chan_base(d40c) + D40_CHAN_REG_SDELT);
+		num_elt = (val & D40_SREG_ELEM_PHY_ECNT_MASK)
+			  >> D40_SREG_ELEM_PHY_ECNT_POS;
+	}
+
 	return num_elt * (1 << d40c->dma_cfg.dst_info.data_width);
 }
 
@@ -841,20 +954,17 @@ static bool d40_tx_is_linked(struct d40_chan *d40c)
 {
 	bool is_link;
 
-	if (d40c->log_num != D40_PHY_CHAN)
+	if (chan_is_logical(d40c))
 		is_link = readl(&d40c->lcpa->lcsp3) &  D40_MEM_LCSP3_DLOS_MASK;
 	else
-		is_link = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-				d40c->phy_chan->num * D40_DREG_PCDELTA +
-				D40_CHAN_REG_SDLNK) &
-			D40_SREG_LNK_PHYS_LNK_MASK;
+		is_link = readl(chan_base(d40c) + D40_CHAN_REG_SDLNK)
+			  & D40_SREG_LNK_PHYS_LNK_MASK;
+
 	return is_link;
 }
 
-static int d40_pause(struct dma_chan *chan)
+static int d40_pause(struct d40_chan *d40c)
 {
-	struct d40_chan *d40c =
-		container_of(chan, struct d40_chan, chan);
 	int res = 0;
 	unsigned long flags;
 
@@ -865,7 +975,7 @@ static int d40_pause(struct dma_chan *chan)
 
 	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
 	if (res == 0) {
-		if (d40c->log_num != D40_PHY_CHAN) {
+		if (chan_is_logical(d40c)) {
 			d40_config_set_event(d40c, false);
 			/* Resume the other logical channels if any */
 			if (d40_chan_has_events(d40c))
@@ -878,10 +988,8 @@ static int d40_pause(struct dma_chan *chan)
 	return res;
 }
 
-static int d40_resume(struct dma_chan *chan)
+static int d40_resume(struct d40_chan *d40c)
 {
-	struct d40_chan *d40c =
-		container_of(chan, struct d40_chan, chan);
 	int res = 0;
 	unsigned long flags;
 
@@ -891,7 +999,7 @@ static int d40_resume(struct dma_chan *chan)
 	spin_lock_irqsave(&d40c->lock, flags);
 
 	if (d40c->base->rev == 0)
-		if (d40c->log_num != D40_PHY_CHAN) {
+		if (chan_is_logical(d40c)) {
 			res = d40_channel_execute_command(d40c,
 							  D40_DMA_SUSPEND_REQ);
 			goto no_suspend;
@@ -900,7 +1008,7 @@ static int d40_resume(struct dma_chan *chan)
 	/* If bytes left to transfer or linked tx resume job */
 	if (d40_residue(d40c) || d40_tx_is_linked(d40c)) {
 
-		if (d40c->log_num != D40_PHY_CHAN)
+		if (chan_is_logical(d40c))
 			d40_config_set_event(d40c, true);
 
 		res = d40_channel_execute_command(d40c, D40_DMA_RUN);
@@ -911,75 +1019,20 @@ no_suspend:
 	return res;
 }
 
-static void d40_tx_submit_log(struct d40_chan *d40c, struct d40_desc *d40d)
+static int d40_terminate_all(struct d40_chan *chan)
 {
-	/* TODO: Write */
-}
-
-static void d40_tx_submit_phy(struct d40_chan *d40c, struct d40_desc *d40d)
-{
-	struct d40_desc *d40d_prev = NULL;
-	int i;
-	u32 val;
-
-	if (!list_empty(&d40c->queue))
-		d40d_prev = d40_last_queued(d40c);
-	else if (!list_empty(&d40c->active))
-		d40d_prev = d40_first_active_get(d40c);
-
-	if (!d40d_prev)
-		return;
-
-	/* Here we try to join this job with previous jobs */
-	val = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-		    d40c->phy_chan->num * D40_DREG_PCDELTA +
-		    D40_CHAN_REG_SSLNK);
-
-	/* Figure out which link we're currently transmitting */
-	for (i = 0; i < d40d_prev->lli_len; i++)
-		if (val == d40d_prev->lli_phy.src[i].reg_lnk)
-			break;
-
-	val = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-		    d40c->phy_chan->num * D40_DREG_PCDELTA +
-		    D40_CHAN_REG_SSELT) >> D40_SREG_ELEM_LOG_ECNT_POS;
-
-	if (i == (d40d_prev->lli_len - 1) && val > 0) {
-		/* Change the current one */
-		writel(virt_to_phys(d40d->lli_phy.src),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SSLNK);
-		writel(virt_to_phys(d40d->lli_phy.dst),
-		       d40c->base->virtbase + D40_DREG_PCBASE +
-		       d40c->phy_chan->num * D40_DREG_PCDELTA +
-		       D40_CHAN_REG_SDLNK);
-
-		d40d->is_hw_linked = true;
-
-	} else if (i < d40d_prev->lli_len) {
-		(void) dma_unmap_single(d40c->base->dev,
-					virt_to_phys(d40d_prev->lli_phy.src),
-					d40d_prev->lli_pool.size,
-					DMA_TO_DEVICE);
+	unsigned long flags;
+	int ret = 0;
 
-		/* Keep the settings */
-		val = d40d_prev->lli_phy.src[d40d_prev->lli_len - 1].reg_lnk &
-			~D40_SREG_LNK_PHYS_LNK_MASK;
-		d40d_prev->lli_phy.src[d40d_prev->lli_len - 1].reg_lnk =
-			val | virt_to_phys(d40d->lli_phy.src);
+	ret = d40_pause(chan);
+	if (!ret && chan_is_physical(chan))
+		ret = d40_channel_execute_command(chan, D40_DMA_STOP);
 
-		val = d40d_prev->lli_phy.dst[d40d_prev->lli_len - 1].reg_lnk &
-			~D40_SREG_LNK_PHYS_LNK_MASK;
-		d40d_prev->lli_phy.dst[d40d_prev->lli_len - 1].reg_lnk =
-			val | virt_to_phys(d40d->lli_phy.dst);
+	spin_lock_irqsave(&chan->lock, flags);
+	d40_term_all(chan);
+	spin_unlock_irqrestore(&chan->lock, flags);
 
-		(void) dma_map_single(d40c->base->dev,
-				      d40d_prev->lli_phy.src,
-				      d40d_prev->lli_pool.size,
-				      DMA_TO_DEVICE);
-		d40d->is_hw_linked = true;
-	}
+	return ret;
 }
 
 static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
@@ -990,8 +1043,6 @@ static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
 	struct d40_desc *d40d = container_of(tx, struct d40_desc, txd);
 	unsigned long flags;
 
-	(void) d40_pause(&d40c->chan);
-
 	spin_lock_irqsave(&d40c->lock, flags);
 
 	d40c->chan.cookie++;
@@ -1001,17 +1052,10 @@ static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	d40d->txd.cookie = d40c->chan.cookie;
 
-	if (d40c->log_num == D40_PHY_CHAN)
-		d40_tx_submit_phy(d40c, d40d);
-	else
-		d40_tx_submit_log(d40c, d40d);
-
 	d40_desc_queue(d40c, d40d);
 
 	spin_unlock_irqrestore(&d40c->lock, flags);
 
-	(void) d40_resume(&d40c->chan);
-
 	return tx->cookie;
 }
 
@@ -1020,7 +1064,7 @@ static int d40_start(struct d40_chan *d40c)
 	if (d40c->base->rev == 0) {
 		int err;
 
-		if (d40c->log_num != D40_PHY_CHAN) {
+		if (chan_is_logical(d40c)) {
 			err = d40_channel_execute_command(d40c,
 							  D40_DMA_SUSPEND_REQ);
 			if (err)
@@ -1028,7 +1072,7 @@ static int d40_start(struct d40_chan *d40c)
 		}
 	}
 
-	if (d40c->log_num != D40_PHY_CHAN)
+	if (chan_is_logical(d40c))
 		d40_config_set_event(d40c, true);
 
 	return d40_channel_execute_command(d40c, D40_DMA_RUN);
@@ -1051,21 +1095,14 @@ static struct d40_desc *d40_queue_start(struct d40_chan *d40c)
 		/* Add to active queue */
 		d40_desc_submit(d40c, d40d);
 
-		/*
-		 * If this job is already linked in hw,
-		 * do not submit it.
-		 */
-
-		if (!d40d->is_hw_linked) {
-			/* Initiate DMA job */
-			d40_desc_load(d40c, d40d);
+		/* Initiate DMA job */
+		d40_desc_load(d40c, d40d);
 
-			/* Start dma job */
-			err = d40_start(d40c);
+		/* Start dma job */
+		err = d40_start(d40c);
 
-			if (err)
-				return NULL;
-		}
+		if (err)
+			return NULL;
 	}
 
 	return d40d;
@@ -1082,17 +1119,36 @@ static void dma_tc_handle(struct d40_chan *d40c)
 	if (d40d == NULL)
 		return;
 
-	d40_lcla_free_all(d40c, d40d);
+	if (d40d->cyclic) {
+		/*
+		 * If this was a paritially loaded list, we need to reloaded
+		 * it, and only when the list is completed.  We need to check
+		 * for done because the interrupt will hit for every link, and
+		 * not just the last one.
+		 */
+		if (d40d->lli_current < d40d->lli_len
+		    && !d40_tx_is_linked(d40c)
+		    && !d40_residue(d40c)) {
+			d40_lcla_free_all(d40c, d40d);
+			d40_desc_load(d40c, d40d);
+			(void) d40_start(d40c);
 
-	if (d40d->lli_current < d40d->lli_len) {
-		d40_desc_load(d40c, d40d);
-		/* Start dma job */
-		(void) d40_start(d40c);
-		return;
-	}
+			if (d40d->lli_current == d40d->lli_len)
+				d40d->lli_current = 0;
+		}
+	} else {
+		d40_lcla_free_all(d40c, d40d);
 
-	if (d40_queue_start(d40c) == NULL)
-		d40c->busy = false;
+		if (d40d->lli_current < d40d->lli_len) {
+			d40_desc_load(d40c, d40d);
+			/* Start dma job */
+			(void) d40_start(d40c);
+			return;
+		}
+
+		if (d40_queue_start(d40c) == NULL)
+			d40c->busy = false;
+	}
 
 	d40c->pending_tx++;
 	tasklet_schedule(&d40c->tasklet);
@@ -1111,11 +1167,11 @@ static void dma_tasklet(unsigned long data)
 
 	/* Get first active entry from list */
 	d40d = d40_first_active_get(d40c);
-
 	if (d40d == NULL)
 		goto err;
 
-	d40c->completed = d40d->txd.cookie;
+	if (!d40d->cyclic)
+		d40c->completed = d40d->txd.cookie;
 
 	/*
 	 * If terminating a channel pending_tx is set to zero.
@@ -1130,16 +1186,18 @@ static void dma_tasklet(unsigned long data)
 	callback = d40d->txd.callback;
 	callback_param = d40d->txd.callback_param;
 
-	if (async_tx_test_ack(&d40d->txd)) {
-		d40_pool_lli_free(d40d);
-		d40_desc_remove(d40d);
-		d40_desc_free(d40c, d40d);
-	} else {
-		if (!d40d->is_in_client_list) {
+	if (!d40d->cyclic) {
+		if (async_tx_test_ack(&d40d->txd)) {
+			d40_pool_lli_free(d40c, d40d);
 			d40_desc_remove(d40d);
-			d40_lcla_free_all(d40c, d40d);
-			list_add_tail(&d40d->node, &d40c->client);
-			d40d->is_in_client_list = true;
+			d40_desc_free(d40c, d40d);
+		} else {
+			if (!d40d->is_in_client_list) {
+				d40_desc_remove(d40d);
+				d40_lcla_free_all(d40c, d40d);
+				list_add_tail(&d40d->node, &d40c->client);
+				d40d->is_in_client_list = true;
+			}
 		}
 	}
 
@@ -1216,9 +1274,8 @@ static irqreturn_t d40_handle_interrupt(int irq, void *data)
 		if (!il[row].is_error)
 			dma_tc_handle(d40c);
 		else
-			dev_err(base->dev,
-				"[%s] IRQ chan: %ld offset %d idx %d\n",
-				__func__, chan, il[row].offset, idx);
+			d40_err(base->dev, "IRQ chan: %ld offset %d idx %d\n",
+				chan, il[row].offset, idx);
 
 		spin_unlock(&d40c->lock);
 	}
@@ -1237,8 +1294,7 @@ static int d40_validate_conf(struct d40_chan *d40c,
 	bool is_log = conf->mode == STEDMA40_MODE_LOGICAL;
 
 	if (!conf->dir) {
-		dev_err(&d40c->chan.dev->device, "[%s] Invalid direction.\n",
-			__func__);
+		chan_err(d40c, "Invalid direction.\n");
 		res = -EINVAL;
 	}
 
@@ -1246,46 +1302,40 @@ static int d40_validate_conf(struct d40_chan *d40c,
 	    d40c->base->plat_data->dev_tx[conf->dst_dev_type] == 0 &&
 	    d40c->runtime_addr == 0) {
 
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Invalid TX channel address (%d)\n",
-			__func__, conf->dst_dev_type);
+		chan_err(d40c, "Invalid TX channel address (%d)\n",
+			 conf->dst_dev_type);
 		res = -EINVAL;
 	}
 
 	if (conf->src_dev_type != STEDMA40_DEV_SRC_MEMORY &&
 	    d40c->base->plat_data->dev_rx[conf->src_dev_type] == 0 &&
 	    d40c->runtime_addr == 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Invalid RX channel address (%d)\n",
-			__func__, conf->src_dev_type);
+		chan_err(d40c, "Invalid RX channel address (%d)\n",
+			conf->src_dev_type);
 		res = -EINVAL;
 	}
 
 	if (conf->dir == STEDMA40_MEM_TO_PERIPH &&
 	    dst_event_group == STEDMA40_DEV_DST_MEMORY) {
-		dev_err(&d40c->chan.dev->device, "[%s] Invalid dst\n",
-			__func__);
+		chan_err(d40c, "Invalid dst\n");
 		res = -EINVAL;
 	}
 
 	if (conf->dir == STEDMA40_PERIPH_TO_MEM &&
 	    src_event_group == STEDMA40_DEV_SRC_MEMORY) {
-		dev_err(&d40c->chan.dev->device, "[%s] Invalid src\n",
-			__func__);
+		chan_err(d40c, "Invalid src\n");
 		res = -EINVAL;
 	}
 
 	if (src_event_group == STEDMA40_DEV_SRC_MEMORY &&
 	    dst_event_group == STEDMA40_DEV_DST_MEMORY && is_log) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] No event line\n", __func__);
+		chan_err(d40c, "No event line\n");
 		res = -EINVAL;
 	}
 
 	if (conf->dir == STEDMA40_PERIPH_TO_PERIPH &&
 	    (src_event_group != dst_event_group)) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Invalid event group\n", __func__);
+		chan_err(d40c, "Invalid event group\n");
 		res = -EINVAL;
 	}
 
@@ -1294,9 +1344,7 @@ static int d40_validate_conf(struct d40_chan *d40c,
 		 * DMAC HW supports it. Will be added to this driver,
 		 * in case any dma client requires it.
 		 */
-		dev_err(&d40c->chan.dev->device,
-			"[%s] periph to periph not supported\n",
-			__func__);
+		chan_err(d40c, "periph to periph not supported\n");
 		res = -EINVAL;
 	}
 
@@ -1309,9 +1357,7 @@ static int d40_validate_conf(struct d40_chan *d40c,
 		 * src (burst x width) == dst (burst x width)
 		 */
 
-		dev_err(&d40c->chan.dev->device,
-			"[%s] src (burst x width) != dst (burst x width)\n",
-			__func__);
+		chan_err(d40c, "src (burst x width) != dst (burst x width)\n");
 		res = -EINVAL;
 	}
 
@@ -1514,8 +1560,7 @@ static int d40_config_memcpy(struct d40_chan *d40c)
 		   dma_has_cap(DMA_SLAVE, cap)) {
 		d40c->dma_cfg = *d40c->base->plat_data->memcpy_conf_phy;
 	} else {
-		dev_err(&d40c->chan.dev->device, "[%s] No memcpy\n",
-			__func__);
+		chan_err(d40c, "No memcpy\n");
 		return -EINVAL;
 	}
 
@@ -1540,21 +1585,19 @@ static int d40_free_dma(struct d40_chan *d40c)
 	/* Release client owned descriptors */
 	if (!list_empty(&d40c->client))
 		list_for_each_entry_safe(d, _d, &d40c->client, node) {
-			d40_pool_lli_free(d);
+			d40_pool_lli_free(d40c, d);
 			d40_desc_remove(d);
 			d40_desc_free(d40c, d);
 		}
 
 	if (phy == NULL) {
-		dev_err(&d40c->chan.dev->device, "[%s] phy == null\n",
-			__func__);
+		chan_err(d40c, "phy == null\n");
 		return -EINVAL;
 	}
 
 	if (phy->allocated_src == D40_ALLOC_FREE &&
 	    phy->allocated_dst == D40_ALLOC_FREE) {
-		dev_err(&d40c->chan.dev->device, "[%s] channel already free\n",
-			__func__);
+		chan_err(d40c, "channel already free\n");
 		return -EINVAL;
 	}
 
@@ -1566,19 +1609,17 @@ static int d40_free_dma(struct d40_chan *d40c)
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.src_dev_type);
 		is_src = true;
 	} else {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unknown direction\n", __func__);
+		chan_err(d40c, "Unknown direction\n");
 		return -EINVAL;
 	}
 
 	res = d40_channel_execute_command(d40c, D40_DMA_SUSPEND_REQ);
 	if (res) {
-		dev_err(&d40c->chan.dev->device, "[%s] suspend failed\n",
-			__func__);
+		chan_err(d40c, "suspend failed\n");
 		return res;
 	}
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	if (chan_is_logical(d40c)) {
 		/* Release logical channel, deactivate the event line */
 
 		d40_config_set_event(d40c, false);
@@ -1594,9 +1635,8 @@ static int d40_free_dma(struct d40_chan *d40c)
 				res = d40_channel_execute_command(d40c,
 								  D40_DMA_RUN);
 				if (res) {
-					dev_err(&d40c->chan.dev->device,
-						"[%s] Executing RUN command\n",
-						__func__);
+					chan_err(d40c,
+						"Executing RUN command\n");
 					return res;
 				}
 			}
@@ -1609,8 +1649,7 @@ static int d40_free_dma(struct d40_chan *d40c)
 	/* Release physical channel */
 	res = d40_channel_execute_command(d40c, D40_DMA_STOP);
 	if (res) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Failed to stop channel\n", __func__);
+		chan_err(d40c, "Failed to stop channel\n");
 		return res;
 	}
 	d40c->phy_chan = NULL;
@@ -1622,6 +1661,7 @@ static int d40_free_dma(struct d40_chan *d40c)
 
 static bool d40_is_paused(struct d40_chan *d40c)
 {
+	void __iomem *chanbase = chan_base(d40c);
 	bool is_paused = false;
 	unsigned long flags;
 	void __iomem *active_reg;
@@ -1630,7 +1670,7 @@ static bool d40_is_paused(struct d40_chan *d40c)
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	if (d40c->log_num == D40_PHY_CHAN) {
+	if (chan_is_physical(d40c)) {
 		if (d40c->phy_chan->num % 2 == 0)
 			active_reg = d40c->base->virtbase + D40_DREG_ACTIVE;
 		else
@@ -1648,17 +1688,12 @@ static bool d40_is_paused(struct d40_chan *d40c)
 	if (d40c->dma_cfg.dir == STEDMA40_MEM_TO_PERIPH ||
 	    d40c->dma_cfg.dir == STEDMA40_MEM_TO_MEM) {
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.dst_dev_type);
-		status = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-			       d40c->phy_chan->num * D40_DREG_PCDELTA +
-			       D40_CHAN_REG_SDLNK);
+		status = readl(chanbase + D40_CHAN_REG_SDLNK);
 	} else if (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM) {
 		event = D40_TYPE_TO_EVENT(d40c->dma_cfg.src_dev_type);
-		status = readl(d40c->base->virtbase + D40_DREG_PCBASE +
-			       d40c->phy_chan->num * D40_DREG_PCDELTA +
-			       D40_CHAN_REG_SSLNK);
+		status = readl(chanbase + D40_CHAN_REG_SSLNK);
 	} else {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unknown direction\n", __func__);
+		chan_err(d40c, "Unknown direction\n");
 		goto _exit;
 	}
 
@@ -1688,114 +1723,184 @@ static u32 stedma40_residue(struct dma_chan *chan)
 	return bytes_left;
 }
 
-struct dma_async_tx_descriptor *stedma40_memcpy_sg(struct dma_chan *chan,
-						   struct scatterlist *sgl_dst,
-						   struct scatterlist *sgl_src,
-						   unsigned int sgl_len,
-						   unsigned long dma_flags)
+static int
+d40_prep_sg_log(struct d40_chan *chan, struct d40_desc *desc,
+		struct scatterlist *sg_src, struct scatterlist *sg_dst,
+		unsigned int sg_len, dma_addr_t src_dev_addr,
+		dma_addr_t dst_dev_addr)
 {
-	int res;
-	struct d40_desc *d40d;
-	struct d40_chan *d40c = container_of(chan, struct d40_chan,
-					     chan);
-	unsigned long flags;
+	struct stedma40_chan_cfg *cfg = &chan->dma_cfg;
+	struct stedma40_half_channel_info *src_info = &cfg->src_info;
+	struct stedma40_half_channel_info *dst_info = &cfg->dst_info;
+	int ret;
 
-	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unallocated channel.\n", __func__);
-		return ERR_PTR(-EINVAL);
-	}
+	ret = d40_log_sg_to_lli(sg_src, sg_len,
+				src_dev_addr,
+				desc->lli_log.src,
+				chan->log_def.lcsp1,
+				src_info->data_width,
+				dst_info->data_width);
 
-	spin_lock_irqsave(&d40c->lock, flags);
-	d40d = d40_desc_get(d40c);
+	ret = d40_log_sg_to_lli(sg_dst, sg_len,
+				dst_dev_addr,
+				desc->lli_log.dst,
+				chan->log_def.lcsp3,
+				dst_info->data_width,
+				src_info->data_width);
 
-	if (d40d == NULL)
+	return ret < 0 ? ret : 0;
+}
+
+static int
+d40_prep_sg_phy(struct d40_chan *chan, struct d40_desc *desc,
+		struct scatterlist *sg_src, struct scatterlist *sg_dst,
+		unsigned int sg_len, dma_addr_t src_dev_addr,
+		dma_addr_t dst_dev_addr)
+{
+	struct stedma40_chan_cfg *cfg = &chan->dma_cfg;
+	struct stedma40_half_channel_info *src_info = &cfg->src_info;
+	struct stedma40_half_channel_info *dst_info = &cfg->dst_info;
+	unsigned long flags = 0;
+	int ret;
+
+	if (desc->cyclic)
+		flags |= LLI_CYCLIC | LLI_TERM_INT;
+
+	ret = d40_phy_sg_to_lli(sg_src, sg_len, src_dev_addr,
+				desc->lli_phy.src,
+				virt_to_phys(desc->lli_phy.src),
+				chan->src_def_cfg,
+				src_info, dst_info, flags);
+
+	ret = d40_phy_sg_to_lli(sg_dst, sg_len, dst_dev_addr,
+				desc->lli_phy.dst,
+				virt_to_phys(desc->lli_phy.dst),
+				chan->dst_def_cfg,
+				dst_info, src_info, flags);
+
+	dma_sync_single_for_device(chan->base->dev, desc->lli_pool.dma_addr,
+				   desc->lli_pool.size, DMA_TO_DEVICE);
+
+	return ret < 0 ? ret : 0;
+}
+
+
+static struct d40_desc *
+d40_prep_desc(struct d40_chan *chan, struct scatterlist *sg,
+	      unsigned int sg_len, unsigned long dma_flags)
+{
+	struct stedma40_chan_cfg *cfg = &chan->dma_cfg;
+	struct d40_desc *desc;
+	int ret;
+
+	desc = d40_desc_get(chan);
+	if (!desc)
+		return NULL;
+
+	desc->lli_len = d40_sg_2_dmalen(sg, sg_len, cfg->src_info.data_width,
+					cfg->dst_info.data_width);
+	if (desc->lli_len < 0) {
+		chan_err(chan, "Unaligned size\n");
 		goto err;
+	}
 
-	d40d->lli_len = d40_sg_2_dmalen(sgl_dst, sgl_len,
-					d40c->dma_cfg.src_info.data_width,
-					d40c->dma_cfg.dst_info.data_width);
-	if (d40d->lli_len < 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unaligned size\n", __func__);
+	ret = d40_pool_lli_alloc(chan, desc, desc->lli_len);
+	if (ret < 0) {
+		chan_err(chan, "Could not allocate lli\n");
 		goto err;
 	}
 
-	d40d->lli_current = 0;
-	d40d->txd.flags = dma_flags;
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	desc->lli_current = 0;
+	desc->txd.flags = dma_flags;
+	desc->txd.tx_submit = d40_tx_submit;
 
-		if (d40_pool_lli_alloc(d40d, d40d->lli_len, true) < 0) {
-			dev_err(&d40c->chan.dev->device,
-				"[%s] Out of memory\n", __func__);
-			goto err;
-		}
+	dma_async_tx_descriptor_init(&desc->txd, &chan->chan);
 
-		(void) d40_log_sg_to_lli(sgl_src,
-					 sgl_len,
-					 d40d->lli_log.src,
-					 d40c->log_def.lcsp1,
-					 d40c->dma_cfg.src_info.data_width,
-					 d40c->dma_cfg.dst_info.data_width);
-
-		(void) d40_log_sg_to_lli(sgl_dst,
-					 sgl_len,
-					 d40d->lli_log.dst,
-					 d40c->log_def.lcsp3,
-					 d40c->dma_cfg.dst_info.data_width,
-					 d40c->dma_cfg.src_info.data_width);
-	} else {
-		if (d40_pool_lli_alloc(d40d, d40d->lli_len, false) < 0) {
-			dev_err(&d40c->chan.dev->device,
-				"[%s] Out of memory\n", __func__);
-			goto err;
-		}
+	return desc;
+
+err:
+	d40_desc_free(chan, desc);
+	return NULL;
+}
+
+static dma_addr_t
+d40_get_dev_addr(struct d40_chan *chan, enum dma_data_direction direction)
+{
+	struct stedma40_platform_data *plat = chan->base->plat_data;
+	struct stedma40_chan_cfg *cfg = &chan->dma_cfg;
+	dma_addr_t addr;
 
-		res = d40_phy_sg_to_lli(sgl_src,
-					sgl_len,
-					0,
-					d40d->lli_phy.src,
-					virt_to_phys(d40d->lli_phy.src),
-					d40c->src_def_cfg,
-					d40c->dma_cfg.src_info.data_width,
-					d40c->dma_cfg.dst_info.data_width,
-					d40c->dma_cfg.src_info.psize);
+	if (chan->runtime_addr)
+		return chan->runtime_addr;
 
-		if (res < 0)
-			goto err;
+	if (direction == DMA_FROM_DEVICE)
+		addr = plat->dev_rx[cfg->src_dev_type];
+	else if (direction == DMA_TO_DEVICE)
+		addr = plat->dev_tx[cfg->dst_dev_type];
 
-		res = d40_phy_sg_to_lli(sgl_dst,
-					sgl_len,
-					0,
-					d40d->lli_phy.dst,
-					virt_to_phys(d40d->lli_phy.dst),
-					d40c->dst_def_cfg,
-					d40c->dma_cfg.dst_info.data_width,
-					d40c->dma_cfg.src_info.data_width,
-					d40c->dma_cfg.dst_info.psize);
+	return addr;
+}
 
-		if (res < 0)
-			goto err;
+static struct dma_async_tx_descriptor *
+d40_prep_sg(struct dma_chan *dchan, struct scatterlist *sg_src,
+	    struct scatterlist *sg_dst, unsigned int sg_len,
+	    enum dma_data_direction direction, unsigned long dma_flags)
+{
+	struct d40_chan *chan = container_of(dchan, struct d40_chan, chan);
+	dma_addr_t src_dev_addr = 0;
+	dma_addr_t dst_dev_addr = 0;
+	struct d40_desc *desc;
+	unsigned long flags;
+	int ret;
 
-		(void) dma_map_single(d40c->base->dev, d40d->lli_phy.src,
-				      d40d->lli_pool.size, DMA_TO_DEVICE);
+	if (!chan->phy_chan) {
+		chan_err(chan, "Cannot prepare unallocated channel\n");
+		return NULL;
 	}
 
-	dma_async_tx_descriptor_init(&d40d->txd, chan);
 
-	d40d->txd.tx_submit = d40_tx_submit;
+	spin_lock_irqsave(&chan->lock, flags);
 
-	spin_unlock_irqrestore(&d40c->lock, flags);
+	desc = d40_prep_desc(chan, sg_src, sg_len, dma_flags);
+	if (desc == NULL)
+		goto err;
+
+	if (sg_next(&sg_src[sg_len - 1]) == sg_src)
+		desc->cyclic = true;
+
+	if (direction != DMA_NONE) {
+		dma_addr_t dev_addr = d40_get_dev_addr(chan, direction);
+
+		if (direction == DMA_FROM_DEVICE)
+			src_dev_addr = dev_addr;
+		else if (direction == DMA_TO_DEVICE)
+			dst_dev_addr = dev_addr;
+	}
+
+	if (chan_is_logical(chan))
+		ret = d40_prep_sg_log(chan, desc, sg_src, sg_dst,
+				      sg_len, src_dev_addr, dst_dev_addr);
+	else
+		ret = d40_prep_sg_phy(chan, desc, sg_src, sg_dst,
+				      sg_len, src_dev_addr, dst_dev_addr);
+
+	if (ret) {
+		chan_err(chan, "Failed to prepare %s sg job: %d\n",
+			 chan_is_logical(chan) ? "log" : "phy", ret);
+		goto err;
+	}
+
+	spin_unlock_irqrestore(&chan->lock, flags);
+
+	return &desc->txd;
 
-	return &d40d->txd;
 err:
-	if (d40d)
-		d40_desc_free(d40c, d40d);
-	spin_unlock_irqrestore(&d40c->lock, flags);
+	if (desc)
+		d40_desc_free(chan, desc);
+	spin_unlock_irqrestore(&chan->lock, flags);
 	return NULL;
 }
-EXPORT_SYMBOL(stedma40_memcpy_sg);
 
 bool stedma40_filter(struct dma_chan *chan, void *data)
 {
@@ -1818,6 +1923,38 @@ bool stedma40_filter(struct dma_chan *chan, void *data)
 }
 EXPORT_SYMBOL(stedma40_filter);
 
+static void __d40_set_prio_rt(struct d40_chan *d40c, int dev_type, bool src)
+{
+	bool realtime = d40c->dma_cfg.realtime;
+	bool highprio = d40c->dma_cfg.high_priority;
+	u32 prioreg = highprio ? D40_DREG_PSEG1 : D40_DREG_PCEG1;
+	u32 rtreg = realtime ? D40_DREG_RSEG1 : D40_DREG_RCEG1;
+	u32 event = D40_TYPE_TO_EVENT(dev_type);
+	u32 group = D40_TYPE_TO_GROUP(dev_type);
+	u32 bit = 1 << event;
+
+	/* Destination event lines are stored in the upper halfword */
+	if (!src)
+		bit <<= 16;
+
+	writel(bit, d40c->base->virtbase + prioreg + group * 4);
+	writel(bit, d40c->base->virtbase + rtreg + group * 4);
+}
+
+static void d40_set_prio_realtime(struct d40_chan *d40c)
+{
+	if (d40c->base->rev < 3)
+		return;
+
+	if ((d40c->dma_cfg.dir ==  STEDMA40_PERIPH_TO_MEM) ||
+	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH))
+		__d40_set_prio_rt(d40c, d40c->dma_cfg.src_dev_type, true);
+
+	if ((d40c->dma_cfg.dir ==  STEDMA40_MEM_TO_PERIPH) ||
+	    (d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_PERIPH))
+		__d40_set_prio_rt(d40c, d40c->dma_cfg.dst_dev_type, false);
+}
+
 /* DMA ENGINE functions */
 static int d40_alloc_chan_resources(struct dma_chan *chan)
 {
@@ -1834,9 +1971,7 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 	if (!d40c->configured) {
 		err = d40_config_memcpy(d40c);
 		if (err) {
-			dev_err(&d40c->chan.dev->device,
-				"[%s] Failed to configure memcpy channel\n",
-				__func__);
+			chan_err(d40c, "Failed to configure memcpy channel\n");
 			goto fail;
 		}
 	}
@@ -1844,16 +1979,17 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 
 	err = d40_allocate_channel(d40c);
 	if (err) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Failed to allocate channel\n", __func__);
+		chan_err(d40c, "Failed to allocate channel\n");
 		goto fail;
 	}
 
 	/* Fill in basic CFG register values */
 	d40_phy_cfg(&d40c->dma_cfg, &d40c->src_def_cfg,
-		    &d40c->dst_def_cfg, d40c->log_num != D40_PHY_CHAN);
+		    &d40c->dst_def_cfg, chan_is_logical(d40c));
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	d40_set_prio_realtime(d40c);
+
+	if (chan_is_logical(d40c)) {
 		d40_log_cfg(&d40c->dma_cfg,
 			    &d40c->log_def.lcsp1, &d40c->log_def.lcsp3);
 
@@ -1886,8 +2022,7 @@ static void d40_free_chan_resources(struct dma_chan *chan)
 	unsigned long flags;
 
 	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Cannot free unallocated channel\n", __func__);
+		chan_err(d40c, "Cannot free unallocated channel\n");
 		return;
 	}
 
@@ -1897,8 +2032,7 @@ static void d40_free_chan_resources(struct dma_chan *chan)
 	err = d40_free_dma(d40c);
 
 	if (err)
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Failed to free channel\n", __func__);
+		chan_err(d40c, "Failed to free channel\n");
 	spin_unlock_irqrestore(&d40c->lock, flags);
 }
 
@@ -1908,251 +2042,31 @@ static struct dma_async_tx_descriptor *d40_prep_memcpy(struct dma_chan *chan,
 						       size_t size,
 						       unsigned long dma_flags)
 {
-	struct d40_desc *d40d;
-	struct d40_chan *d40c = container_of(chan, struct d40_chan,
-					     chan);
-	unsigned long flags;
-
-	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Channel is not allocated.\n", __func__);
-		return ERR_PTR(-EINVAL);
-	}
-
-	spin_lock_irqsave(&d40c->lock, flags);
-	d40d = d40_desc_get(d40c);
-
-	if (d40d == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Descriptor is NULL\n", __func__);
-		goto err;
-	}
+	struct scatterlist dst_sg;
+	struct scatterlist src_sg;
 
-	d40d->txd.flags = dma_flags;
-	d40d->lli_len = d40_size_2_dmalen(size,
-					  d40c->dma_cfg.src_info.data_width,
-					  d40c->dma_cfg.dst_info.data_width);
-	if (d40d->lli_len < 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unaligned size\n", __func__);
-		goto err;
-	}
+	sg_init_table(&dst_sg, 1);
+	sg_init_table(&src_sg, 1);
 
+	sg_dma_address(&dst_sg) = dst;
+	sg_dma_address(&src_sg) = src;
 
-	dma_async_tx_descriptor_init(&d40d->txd, chan);
+	sg_dma_len(&dst_sg) = size;
+	sg_dma_len(&src_sg) = size;
 
-	d40d->txd.tx_submit = d40_tx_submit;
-
-	if (d40c->log_num != D40_PHY_CHAN) {
-
-		if (d40_pool_lli_alloc(d40d, d40d->lli_len, true) < 0) {
-			dev_err(&d40c->chan.dev->device,
-				"[%s] Out of memory\n", __func__);
-			goto err;
-		}
-		d40d->lli_current = 0;
-
-		if (d40_log_buf_to_lli(d40d->lli_log.src,
-				       src,
-				       size,
-				       d40c->log_def.lcsp1,
-				       d40c->dma_cfg.src_info.data_width,
-				       d40c->dma_cfg.dst_info.data_width,
-				       true) == NULL)
-			goto err;
-
-		if (d40_log_buf_to_lli(d40d->lli_log.dst,
-				       dst,
-				       size,
-				       d40c->log_def.lcsp3,
-				       d40c->dma_cfg.dst_info.data_width,
-				       d40c->dma_cfg.src_info.data_width,
-				       true) == NULL)
-			goto err;
-
-	} else {
-
-		if (d40_pool_lli_alloc(d40d, d40d->lli_len, false) < 0) {
-			dev_err(&d40c->chan.dev->device,
-				"[%s] Out of memory\n", __func__);
-			goto err;
-		}
-
-		if (d40_phy_buf_to_lli(d40d->lli_phy.src,
-				       src,
-				       size,
-				       d40c->dma_cfg.src_info.psize,
-				       0,
-				       d40c->src_def_cfg,
-				       true,
-				       d40c->dma_cfg.src_info.data_width,
-				       d40c->dma_cfg.dst_info.data_width,
-				       false) == NULL)
-			goto err;
-
-		if (d40_phy_buf_to_lli(d40d->lli_phy.dst,
-				       dst,
-				       size,
-				       d40c->dma_cfg.dst_info.psize,
-				       0,
-				       d40c->dst_def_cfg,
-				       true,
-				       d40c->dma_cfg.dst_info.data_width,
-				       d40c->dma_cfg.src_info.data_width,
-				       false) == NULL)
-			goto err;
-
-		(void) dma_map_single(d40c->base->dev, d40d->lli_phy.src,
-				      d40d->lli_pool.size, DMA_TO_DEVICE);
-	}
-
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return &d40d->txd;
-
-err:
-	if (d40d)
-		d40_desc_free(d40c, d40d);
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return NULL;
+	return d40_prep_sg(chan, &src_sg, &dst_sg, 1, DMA_NONE, dma_flags);
 }
 
 static struct dma_async_tx_descriptor *
-d40_prep_sg(struct dma_chan *chan,
-	    struct scatterlist *dst_sg, unsigned int dst_nents,
-	    struct scatterlist *src_sg, unsigned int src_nents,
-	    unsigned long dma_flags)
+d40_prep_memcpy_sg(struct dma_chan *chan,
+		   struct scatterlist *dst_sg, unsigned int dst_nents,
+		   struct scatterlist *src_sg, unsigned int src_nents,
+		   unsigned long dma_flags)
 {
 	if (dst_nents != src_nents)
 		return NULL;
 
-	return stedma40_memcpy_sg(chan, dst_sg, src_sg, dst_nents, dma_flags);
-}
-
-static int d40_prep_slave_sg_log(struct d40_desc *d40d,
-				 struct d40_chan *d40c,
-				 struct scatterlist *sgl,
-				 unsigned int sg_len,
-				 enum dma_data_direction direction,
-				 unsigned long dma_flags)
-{
-	dma_addr_t dev_addr = 0;
-	int total_size;
-
-	d40d->lli_len = d40_sg_2_dmalen(sgl, sg_len,
-					d40c->dma_cfg.src_info.data_width,
-					d40c->dma_cfg.dst_info.data_width);
-	if (d40d->lli_len < 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unaligned size\n", __func__);
-		return -EINVAL;
-	}
-
-	if (d40_pool_lli_alloc(d40d, d40d->lli_len, true) < 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Out of memory\n", __func__);
-		return -ENOMEM;
-	}
-
-	d40d->lli_current = 0;
-
-	if (direction == DMA_FROM_DEVICE)
-		if (d40c->runtime_addr)
-			dev_addr = d40c->runtime_addr;
-		else
-			dev_addr = d40c->base->plat_data->dev_rx[d40c->dma_cfg.src_dev_type];
-	else if (direction == DMA_TO_DEVICE)
-		if (d40c->runtime_addr)
-			dev_addr = d40c->runtime_addr;
-		else
-			dev_addr = d40c->base->plat_data->dev_tx[d40c->dma_cfg.dst_dev_type];
-
-	else
-		return -EINVAL;
-
-	total_size = d40_log_sg_to_dev(sgl, sg_len,
-				       &d40d->lli_log,
-				       &d40c->log_def,
-				       d40c->dma_cfg.src_info.data_width,
-				       d40c->dma_cfg.dst_info.data_width,
-				       direction,
-				       dev_addr);
-
-	if (total_size < 0)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int d40_prep_slave_sg_phy(struct d40_desc *d40d,
-				 struct d40_chan *d40c,
-				 struct scatterlist *sgl,
-				 unsigned int sgl_len,
-				 enum dma_data_direction direction,
-				 unsigned long dma_flags)
-{
-	dma_addr_t src_dev_addr;
-	dma_addr_t dst_dev_addr;
-	int res;
-
-	d40d->lli_len = d40_sg_2_dmalen(sgl, sgl_len,
-					d40c->dma_cfg.src_info.data_width,
-					d40c->dma_cfg.dst_info.data_width);
-	if (d40d->lli_len < 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Unaligned size\n", __func__);
-		return -EINVAL;
-	}
-
-	if (d40_pool_lli_alloc(d40d, d40d->lli_len, false) < 0) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Out of memory\n", __func__);
-		return -ENOMEM;
-	}
-
-	d40d->lli_current = 0;
-
-	if (direction == DMA_FROM_DEVICE) {
-		dst_dev_addr = 0;
-		if (d40c->runtime_addr)
-			src_dev_addr = d40c->runtime_addr;
-		else
-			src_dev_addr = d40c->base->plat_data->dev_rx[d40c->dma_cfg.src_dev_type];
-	} else if (direction == DMA_TO_DEVICE) {
-		if (d40c->runtime_addr)
-			dst_dev_addr = d40c->runtime_addr;
-		else
-			dst_dev_addr = d40c->base->plat_data->dev_tx[d40c->dma_cfg.dst_dev_type];
-		src_dev_addr = 0;
-	} else
-		return -EINVAL;
-
-	res = d40_phy_sg_to_lli(sgl,
-				sgl_len,
-				src_dev_addr,
-				d40d->lli_phy.src,
-				virt_to_phys(d40d->lli_phy.src),
-				d40c->src_def_cfg,
-				d40c->dma_cfg.src_info.data_width,
-				d40c->dma_cfg.dst_info.data_width,
-				d40c->dma_cfg.src_info.psize);
-	if (res < 0)
-		return res;
-
-	res = d40_phy_sg_to_lli(sgl,
-				sgl_len,
-				dst_dev_addr,
-				d40d->lli_phy.dst,
-				virt_to_phys(d40d->lli_phy.dst),
-				d40c->dst_def_cfg,
-				d40c->dma_cfg.dst_info.data_width,
-				d40c->dma_cfg.src_info.data_width,
-				d40c->dma_cfg.dst_info.psize);
-	if (res < 0)
-		return res;
-
-	(void) dma_map_single(d40c->base->dev, d40d->lli_phy.src,
-			      d40d->lli_pool.size, DMA_TO_DEVICE);
-	return 0;
+	return d40_prep_sg(chan, src_sg, dst_sg, src_nents, DMA_NONE, dma_flags);
 }
 
 static struct dma_async_tx_descriptor *d40_prep_slave_sg(struct dma_chan *chan,
@@ -2161,52 +2075,40 @@ static struct dma_async_tx_descriptor *d40_prep_slave_sg(struct dma_chan *chan,
 							 enum dma_data_direction direction,
 							 unsigned long dma_flags)
 {
-	struct d40_desc *d40d;
-	struct d40_chan *d40c = container_of(chan, struct d40_chan,
-					     chan);
-	unsigned long flags;
-	int err;
-
-	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Cannot prepare unallocated channel\n", __func__);
-		return ERR_PTR(-EINVAL);
-	}
+	if (direction != DMA_FROM_DEVICE && direction != DMA_TO_DEVICE)
+		return NULL;
 
-	spin_lock_irqsave(&d40c->lock, flags);
-	d40d = d40_desc_get(d40c);
+	return d40_prep_sg(chan, sgl, sgl, sg_len, direction, dma_flags);
+}
 
-	if (d40d == NULL)
-		goto err;
+static struct dma_async_tx_descriptor *
+dma40_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
+		     size_t buf_len, size_t period_len,
+		     enum dma_data_direction direction)
+{
+	unsigned int periods = buf_len / period_len;
+	struct dma_async_tx_descriptor *txd;
+	struct scatterlist *sg;
+	int i;
 
-	if (d40c->log_num != D40_PHY_CHAN)
-		err = d40_prep_slave_sg_log(d40d, d40c, sgl, sg_len,
-					    direction, dma_flags);
-	else
-		err = d40_prep_slave_sg_phy(d40d, d40c, sgl, sg_len,
-					    direction, dma_flags);
-	if (err) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Failed to prepare %s slave sg job: %d\n",
-			__func__,
-			d40c->log_num != D40_PHY_CHAN ? "log" : "phy", err);
-		goto err;
+	sg = kcalloc(periods + 1, sizeof(struct scatterlist), GFP_KERNEL);
+	for (i = 0; i < periods; i++) {
+		sg_dma_address(&sg[i]) = dma_addr;
+		sg_dma_len(&sg[i]) = period_len;
+		dma_addr += period_len;
 	}
 
-	d40d->txd.flags = dma_flags;
+	sg[periods].offset = 0;
+	sg[periods].length = 0;
+	sg[periods].page_link =
+		((unsigned long)sg | 0x01) & ~0x02;
 
-	dma_async_tx_descriptor_init(&d40d->txd, chan);
+	txd = d40_prep_sg(chan, sg, sg, periods, direction,
+			  DMA_PREP_INTERRUPT);
 
-	d40d->txd.tx_submit = d40_tx_submit;
+	kfree(sg);
 
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return &d40d->txd;
-
-err:
-	if (d40d)
-		d40_desc_free(d40c, d40d);
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return NULL;
+	return txd;
 }
 
 static enum dma_status d40_tx_status(struct dma_chan *chan,
@@ -2219,9 +2121,7 @@ static enum dma_status d40_tx_status(struct dma_chan *chan,
 	int ret;
 
 	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Cannot read status of unallocated channel\n",
-			__func__);
+		chan_err(d40c, "Cannot read status of unallocated channel\n");
 		return -EINVAL;
 	}
 
@@ -2245,8 +2145,7 @@ static void d40_issue_pending(struct dma_chan *chan)
 	unsigned long flags;
 
 	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Channel is not allocated!\n", __func__);
+		chan_err(d40c, "Channel is not allocated!\n");
 		return;
 	}
 
@@ -2339,7 +2238,7 @@ static void d40_set_runtime_config(struct dma_chan *chan,
 		return;
 	}
 
-	if (d40c->log_num != D40_PHY_CHAN) {
+	if (chan_is_logical(d40c)) {
 		if (config_maxburst >= 16)
 			psize = STEDMA40_PSIZE_LOG_16;
 		else if (config_maxburst >= 8)
@@ -2372,7 +2271,7 @@ static void d40_set_runtime_config(struct dma_chan *chan,
 	cfg->dst_info.flow_ctrl = STEDMA40_NO_FLOW_CTRL;
 
 	/* Fill in register values */
-	if (d40c->log_num != D40_PHY_CHAN)
+	if (chan_is_logical(d40c))
 		d40_log_cfg(cfg, &d40c->log_def.lcsp1, &d40c->log_def.lcsp3);
 	else
 		d40_phy_cfg(cfg, &d40c->src_def_cfg,
@@ -2393,25 +2292,20 @@ static void d40_set_runtime_config(struct dma_chan *chan,
 static int d40_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		       unsigned long arg)
 {
-	unsigned long flags;
 	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
 
 	if (d40c->phy_chan == NULL) {
-		dev_err(&d40c->chan.dev->device,
-			"[%s] Channel is not allocated!\n", __func__);
+		chan_err(d40c, "Channel is not allocated!\n");
 		return -EINVAL;
 	}
 
 	switch (cmd) {
 	case DMA_TERMINATE_ALL:
-		spin_lock_irqsave(&d40c->lock, flags);
-		d40_term_all(d40c);
-		spin_unlock_irqrestore(&d40c->lock, flags);
-		return 0;
+		return d40_terminate_all(d40c);
 	case DMA_PAUSE:
-		return d40_pause(chan);
+		return d40_pause(d40c);
 	case DMA_RESUME:
-		return d40_resume(chan);
+		return d40_resume(d40c);
 	case DMA_SLAVE_CONFIG:
 		d40_set_runtime_config(chan,
 			(struct dma_slave_config *) arg);
@@ -2456,6 +2350,35 @@ static void __init d40_chan_init(struct d40_base *base, struct dma_device *dma,
 	}
 }
 
+static void d40_ops_init(struct d40_base *base, struct dma_device *dev)
+{
+	if (dma_has_cap(DMA_SLAVE, dev->cap_mask))
+		dev->device_prep_slave_sg = d40_prep_slave_sg;
+
+	if (dma_has_cap(DMA_MEMCPY, dev->cap_mask)) {
+		dev->device_prep_dma_memcpy = d40_prep_memcpy;
+
+		/*
+		 * This controller can only access address at even
+		 * 32bit boundaries, i.e. 2^2
+		 */
+		dev->copy_align = 2;
+	}
+
+	if (dma_has_cap(DMA_SG, dev->cap_mask))
+		dev->device_prep_dma_sg = d40_prep_memcpy_sg;
+
+	if (dma_has_cap(DMA_CYCLIC, dev->cap_mask))
+		dev->device_prep_dma_cyclic = dma40_prep_dma_cyclic;
+
+	dev->device_alloc_chan_resources = d40_alloc_chan_resources;
+	dev->device_free_chan_resources = d40_free_chan_resources;
+	dev->device_issue_pending = d40_issue_pending;
+	dev->device_tx_status = d40_tx_status;
+	dev->device_control = d40_control;
+	dev->dev = base->dev;
+}
+
 static int __init d40_dmaengine_init(struct d40_base *base,
 				     int num_reserved_chans)
 {
@@ -2466,23 +2389,14 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 
 	dma_cap_zero(base->dma_slave.cap_mask);
 	dma_cap_set(DMA_SLAVE, base->dma_slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, base->dma_slave.cap_mask);
 
-	base->dma_slave.device_alloc_chan_resources = d40_alloc_chan_resources;
-	base->dma_slave.device_free_chan_resources = d40_free_chan_resources;
-	base->dma_slave.device_prep_dma_memcpy = d40_prep_memcpy;
-	base->dma_slave.device_prep_dma_sg = d40_prep_sg;
-	base->dma_slave.device_prep_slave_sg = d40_prep_slave_sg;
-	base->dma_slave.device_tx_status = d40_tx_status;
-	base->dma_slave.device_issue_pending = d40_issue_pending;
-	base->dma_slave.device_control = d40_control;
-	base->dma_slave.dev = base->dev;
+	d40_ops_init(base, &base->dma_slave);
 
 	err = dma_async_device_register(&base->dma_slave);
 
 	if (err) {
-		dev_err(base->dev,
-			"[%s] Failed to register slave channels\n",
-			__func__);
+		d40_err(base->dev, "Failed to register slave channels\n");
 		goto failure1;
 	}
 
@@ -2491,29 +2405,15 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 
 	dma_cap_zero(base->dma_memcpy.cap_mask);
 	dma_cap_set(DMA_MEMCPY, base->dma_memcpy.cap_mask);
-	dma_cap_set(DMA_SG, base->dma_slave.cap_mask);
-
-	base->dma_memcpy.device_alloc_chan_resources = d40_alloc_chan_resources;
-	base->dma_memcpy.device_free_chan_resources = d40_free_chan_resources;
-	base->dma_memcpy.device_prep_dma_memcpy = d40_prep_memcpy;
-	base->dma_slave.device_prep_dma_sg = d40_prep_sg;
-	base->dma_memcpy.device_prep_slave_sg = d40_prep_slave_sg;
-	base->dma_memcpy.device_tx_status = d40_tx_status;
-	base->dma_memcpy.device_issue_pending = d40_issue_pending;
-	base->dma_memcpy.device_control = d40_control;
-	base->dma_memcpy.dev = base->dev;
-	/*
-	 * This controller can only access address at even
-	 * 32bit boundaries, i.e. 2^2
-	 */
-	base->dma_memcpy.copy_align = 2;
+	dma_cap_set(DMA_SG, base->dma_memcpy.cap_mask);
+
+	d40_ops_init(base, &base->dma_memcpy);
 
 	err = dma_async_device_register(&base->dma_memcpy);
 
 	if (err) {
-		dev_err(base->dev,
-			"[%s] Failed to regsiter memcpy only channels\n",
-			__func__);
+		d40_err(base->dev,
+			"Failed to regsiter memcpy only channels\n");
 		goto failure2;
 	}
 
@@ -2523,24 +2423,15 @@ static int __init d40_dmaengine_init(struct d40_base *base,
 	dma_cap_zero(base->dma_both.cap_mask);
 	dma_cap_set(DMA_SLAVE, base->dma_both.cap_mask);
 	dma_cap_set(DMA_MEMCPY, base->dma_both.cap_mask);
-	dma_cap_set(DMA_SG, base->dma_slave.cap_mask);
-
-	base->dma_both.device_alloc_chan_resources = d40_alloc_chan_resources;
-	base->dma_both.device_free_chan_resources = d40_free_chan_resources;
-	base->dma_both.device_prep_dma_memcpy = d40_prep_memcpy;
-	base->dma_slave.device_prep_dma_sg = d40_prep_sg;
-	base->dma_both.device_prep_slave_sg = d40_prep_slave_sg;
-	base->dma_both.device_tx_status = d40_tx_status;
-	base->dma_both.device_issue_pending = d40_issue_pending;
-	base->dma_both.device_control = d40_control;
-	base->dma_both.dev = base->dev;
-	base->dma_both.copy_align = 2;
+	dma_cap_set(DMA_SG, base->dma_both.cap_mask);
+	dma_cap_set(DMA_CYCLIC, base->dma_slave.cap_mask);
+
+	d40_ops_init(base, &base->dma_both);
 	err = dma_async_device_register(&base->dma_both);
 
 	if (err) {
-		dev_err(base->dev,
-			"[%s] Failed to register logical and physical capable channels\n",
-			__func__);
+		d40_err(base->dev,
+			"Failed to register logical and physical capable channels\n");
 		goto failure3;
 	}
 	return 0;
@@ -2616,9 +2507,10 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		{ .reg = D40_DREG_PERIPHID1, .val = 0x0000},
 		/*
 		 * D40_DREG_PERIPHID2 Depends on HW revision:
-		 *  MOP500/HREF ED has 0x0008,
+		 *  DB8500ed has 0x0008,
 		 *  ? has 0x0018,
-		 *  HREF V1 has 0x0028
+		 *  DB8500v1 has 0x0028
+		 *  DB8500v2 has 0x0038
 		 */
 		{ .reg = D40_DREG_PERIPHID3, .val = 0x0000},
 
@@ -2642,8 +2534,7 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	clk = clk_get(&pdev->dev, NULL);
 
 	if (IS_ERR(clk)) {
-		dev_err(&pdev->dev, "[%s] No matching clock found\n",
-			__func__);
+		d40_err(&pdev->dev, "No matching clock found\n");
 		goto failure;
 	}
 
@@ -2666,9 +2557,8 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	for (i = 0; i < ARRAY_SIZE(dma_id_regs); i++) {
 		if (dma_id_regs[i].val !=
 		    readl(virtbase + dma_id_regs[i].reg)) {
-			dev_err(&pdev->dev,
-				"[%s] Unknown hardware! Expected 0x%x at 0x%x but got 0x%x\n",
-				__func__,
+			d40_err(&pdev->dev,
+				"Unknown hardware! Expected 0x%x at 0x%x but got 0x%x\n",
 				dma_id_regs[i].val,
 				dma_id_regs[i].reg,
 				readl(virtbase + dma_id_regs[i].reg));
@@ -2681,9 +2571,8 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 
 	if ((val & D40_DREG_PERIPHID2_DESIGNER_MASK) !=
 	    D40_HW_DESIGNER) {
-		dev_err(&pdev->dev,
-			"[%s] Unknown designer! Got %x wanted %x\n",
-			__func__, val & D40_DREG_PERIPHID2_DESIGNER_MASK,
+		d40_err(&pdev->dev, "Unknown designer! Got %x wanted %x\n",
+			val & D40_DREG_PERIPHID2_DESIGNER_MASK,
 			D40_HW_DESIGNER);
 		goto failure;
 	}
@@ -2713,7 +2602,7 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 		       sizeof(struct d40_chan), GFP_KERNEL);
 
 	if (base == NULL) {
-		dev_err(&pdev->dev, "[%s] Out of memory\n", __func__);
+		d40_err(&pdev->dev, "Out of memory\n");
 		goto failure;
 	}
 
@@ -2860,6 +2749,7 @@ static void __init d40_hw_init(struct d40_base *base)
 
 static int __init d40_lcla_allocate(struct d40_base *base)
 {
+	struct d40_lcla_pool *pool = &base->lcla_pool;
 	unsigned long *page_list;
 	int i, j;
 	int ret = 0;
@@ -2885,9 +2775,8 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 						base->lcla_pool.pages);
 		if (!page_list[i]) {
 
-			dev_err(base->dev,
-				"[%s] Failed to allocate %d pages.\n",
-				__func__, base->lcla_pool.pages);
+			d40_err(base->dev, "Failed to allocate %d pages.\n",
+				base->lcla_pool.pages);
 
 			for (j = 0; j < i; j++)
 				free_pages(page_list[j], base->lcla_pool.pages);
@@ -2925,6 +2814,15 @@ static int __init d40_lcla_allocate(struct d40_base *base)
 						 LCLA_ALIGNMENT);
 	}
 
+	pool->dma_addr = dma_map_single(base->dev, pool->base,
+					SZ_1K * base->num_phy_chans,
+					DMA_TO_DEVICE);
+	if (dma_mapping_error(base->dev, pool->dma_addr)) {
+		pool->dma_addr = 0;
+		ret = -ENOMEM;
+		goto failure;
+	}
+
 	writel(virt_to_phys(base->lcla_pool.base),
 	       base->virtbase + D40_DREG_LCLA);
 failure:
@@ -2957,9 +2855,7 @@ static int __init d40_probe(struct platform_device *pdev)
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "lcpa");
 	if (!res) {
 		ret = -ENOENT;
-		dev_err(&pdev->dev,
-			"[%s] No \"lcpa\" memory resource\n",
-			__func__);
+		d40_err(&pdev->dev, "No \"lcpa\" memory resource\n");
 		goto failure;
 	}
 	base->lcpa_size = resource_size(res);
@@ -2968,9 +2864,9 @@ static int __init d40_probe(struct platform_device *pdev)
 	if (request_mem_region(res->start, resource_size(res),
 			       D40_NAME " I/O lcpa") == NULL) {
 		ret = -EBUSY;
-		dev_err(&pdev->dev,
-			"[%s] Failed to request LCPA region 0x%x-0x%x\n",
-			__func__, res->start, res->end);
+		d40_err(&pdev->dev,
+			"Failed to request LCPA region 0x%x-0x%x\n",
+			res->start, res->end);
 		goto failure;
 	}
 
@@ -2986,16 +2882,13 @@ static int __init d40_probe(struct platform_device *pdev)
 	base->lcpa_base = ioremap(res->start, resource_size(res));
 	if (!base->lcpa_base) {
 		ret = -ENOMEM;
-		dev_err(&pdev->dev,
-			"[%s] Failed to ioremap LCPA region\n",
-			__func__);
+		d40_err(&pdev->dev, "Failed to ioremap LCPA region\n");
 		goto failure;
 	}
 
 	ret = d40_lcla_allocate(base);
 	if (ret) {
-		dev_err(&pdev->dev, "[%s] Failed to allocate LCLA area\n",
-			__func__);
+		d40_err(&pdev->dev, "Failed to allocate LCLA area\n");
 		goto failure;
 	}
 
@@ -3004,9 +2897,8 @@ static int __init d40_probe(struct platform_device *pdev)
 	base->irq = platform_get_irq(pdev, 0);
 
 	ret = request_irq(base->irq, d40_handle_interrupt, 0, D40_NAME, base);
-
 	if (ret) {
-		dev_err(&pdev->dev, "[%s] No IRQ defined\n", __func__);
+		d40_err(&pdev->dev, "No IRQ defined\n");
 		goto failure;
 	}
 
@@ -3025,6 +2917,12 @@ failure:
 			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);
@@ -3049,7 +2947,7 @@ failure:
 		kfree(base);
 	}
 
-	dev_err(&pdev->dev, "[%s] probe failed\n", __func__);
+	d40_err(&pdev->dev, "probe failed\n");
 	return ret;
 }
 
@@ -3060,7 +2958,7 @@ static struct platform_driver d40_driver = {
 	},
 };
 
-int __init stedma40_init(void)
+static int __init stedma40_init(void)
 {
 	return platform_driver_probe(&d40_driver, d40_probe);
 }
diff --git a/drivers/dma/ste_dma40_ll.c b/drivers/dma/ste_dma40_ll.c
index 0b096a38322d..cad9e1daedff 100644
--- a/drivers/dma/ste_dma40_ll.c
+++ b/drivers/dma/ste_dma40_ll.c
@@ -125,13 +125,15 @@ void d40_phy_cfg(struct stedma40_chan_cfg *cfg,
 static int d40_phy_fill_lli(struct d40_phy_lli *lli,
 			    dma_addr_t data,
 			    u32 data_size,
-			    int psize,
 			    dma_addr_t next_lli,
 			    u32 reg_cfg,
-			    bool term_int,
-			    u32 data_width,
-			    bool is_device)
+			    struct stedma40_half_channel_info *info,
+			    unsigned int flags)
 {
+	bool addr_inc = flags & LLI_ADDR_INC;
+	bool term_int = flags & LLI_TERM_INT;
+	unsigned int data_width = info->data_width;
+	int psize = info->psize;
 	int num_elems;
 
 	if (psize == STEDMA40_PSIZE_PHY_1)
@@ -154,7 +156,7 @@ static int d40_phy_fill_lli(struct d40_phy_lli *lli,
 	 * Distance to next element sized entry.
 	 * Usually the size of the element unless you want gaps.
 	 */
-	if (!is_device)
+	if (addr_inc)
 		lli->reg_elt |= (0x1 << data_width) <<
 			D40_SREG_ELEM_PHY_EIDX_POS;
 
@@ -198,47 +200,51 @@ static int d40_seg_size(int size, int data_width1, int data_width2)
 	return seg_max;
 }
 
-struct d40_phy_lli *d40_phy_buf_to_lli(struct d40_phy_lli *lli,
-				       dma_addr_t addr,
-				       u32 size,
-				       int psize,
-				       dma_addr_t lli_phys,
-				       u32 reg_cfg,
-				       bool term_int,
-				       u32 data_width1,
-				       u32 data_width2,
-				       bool is_device)
+static struct d40_phy_lli *
+d40_phy_buf_to_lli(struct d40_phy_lli *lli, dma_addr_t addr, u32 size,
+		   dma_addr_t lli_phys, dma_addr_t first_phys, u32 reg_cfg,
+		   struct stedma40_half_channel_info *info,
+		   struct stedma40_half_channel_info *otherinfo,
+		   unsigned long flags)
 {
+	bool lastlink = flags & LLI_LAST_LINK;
+	bool addr_inc = flags & LLI_ADDR_INC;
+	bool term_int = flags & LLI_TERM_INT;
+	bool cyclic = flags & LLI_CYCLIC;
 	int err;
 	dma_addr_t next = lli_phys;
 	int size_rest = size;
 	int size_seg = 0;
 
+	/*
+	 * This piece may be split up based on d40_seg_size(); we only want the
+	 * term int on the last part.
+	 */
+	if (term_int)
+		flags &= ~LLI_TERM_INT;
+
 	do {
-		size_seg = d40_seg_size(size_rest, data_width1, data_width2);
+		size_seg = d40_seg_size(size_rest, info->data_width,
+					otherinfo->data_width);
 		size_rest -= size_seg;
 
-		if (term_int && size_rest == 0)
-			next = 0;
+		if (size_rest == 0 && term_int)
+			flags |= LLI_TERM_INT;
+
+		if (size_rest == 0 && lastlink)
+			next = cyclic ? first_phys : 0;
 		else
 			next = ALIGN(next + sizeof(struct d40_phy_lli),
 				     D40_LLI_ALIGN);
 
-		err = d40_phy_fill_lli(lli,
-				       addr,
-				       size_seg,
-				       psize,
-				       next,
-				       reg_cfg,
-				       !next,
-				       data_width1,
-				       is_device);
+		err = d40_phy_fill_lli(lli, addr, size_seg, next,
+				       reg_cfg, info, flags);
 
 		if (err)
 			goto err;
 
 		lli++;
-		if (!is_device)
+		if (addr_inc)
 			addr += size_seg;
 	} while (size_rest);
 
@@ -254,39 +260,35 @@ int d40_phy_sg_to_lli(struct scatterlist *sg,
 		      struct d40_phy_lli *lli_sg,
 		      dma_addr_t lli_phys,
 		      u32 reg_cfg,
-		      u32 data_width1,
-		      u32 data_width2,
-		      int psize)
+		      struct stedma40_half_channel_info *info,
+		      struct stedma40_half_channel_info *otherinfo,
+		      unsigned long flags)
 {
 	int total_size = 0;
 	int i;
 	struct scatterlist *current_sg = sg;
-	dma_addr_t dst;
 	struct d40_phy_lli *lli = lli_sg;
 	dma_addr_t l_phys = lli_phys;
 
+	if (!target)
+		flags |= LLI_ADDR_INC;
+
 	for_each_sg(sg, current_sg, sg_len, i) {
+		dma_addr_t sg_addr = sg_dma_address(current_sg);
+		unsigned int len = sg_dma_len(current_sg);
+		dma_addr_t dst = target ?: sg_addr;
 
 		total_size += sg_dma_len(current_sg);
 
-		if (target)
-			dst = target;
-		else
-			dst = sg_phys(current_sg);
+		if (i == sg_len - 1)
+			flags |= LLI_TERM_INT | LLI_LAST_LINK;
 
 		l_phys = ALIGN(lli_phys + (lli - lli_sg) *
 			       sizeof(struct d40_phy_lli), D40_LLI_ALIGN);
 
-		lli = d40_phy_buf_to_lli(lli,
-					 dst,
-					 sg_dma_len(current_sg),
-					 psize,
-					 l_phys,
-					 reg_cfg,
-					 sg_len - 1 == i,
-					 data_width1,
-					 data_width2,
-					 target == dst);
+		lli = d40_phy_buf_to_lli(lli, dst, len, l_phys, lli_phys,
+					 reg_cfg, info, otherinfo, flags);
+
 		if (lli == NULL)
 			return -EINVAL;
 	}
@@ -295,45 +297,22 @@ int d40_phy_sg_to_lli(struct scatterlist *sg,
 }
 
 
-void d40_phy_lli_write(void __iomem *virtbase,
-		       u32 phy_chan_num,
-		       struct d40_phy_lli *lli_dst,
-		       struct d40_phy_lli *lli_src)
-{
-
-	writel(lli_src->reg_cfg, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SSCFG);
-	writel(lli_src->reg_elt, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SSELT);
-	writel(lli_src->reg_ptr, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SSPTR);
-	writel(lli_src->reg_lnk, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SSLNK);
-
-	writel(lli_dst->reg_cfg, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SDCFG);
-	writel(lli_dst->reg_elt, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SDELT);
-	writel(lli_dst->reg_ptr, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SDPTR);
-	writel(lli_dst->reg_lnk, virtbase + D40_DREG_PCBASE +
-	       phy_chan_num * D40_DREG_PCDELTA + D40_CHAN_REG_SDLNK);
-
-}
-
 /* DMA logical lli operations */
 
 static void d40_log_lli_link(struct d40_log_lli *lli_dst,
 			     struct d40_log_lli *lli_src,
-			     int next)
+			     int next, unsigned int flags)
 {
+	bool interrupt = flags & LLI_TERM_INT;
 	u32 slos = 0;
 	u32 dlos = 0;
 
 	if (next != -EINVAL) {
 		slos = next * 2;
 		dlos = next * 2 + 1;
-	} else {
+	}
+
+	if (interrupt) {
 		lli_dst->lcsp13 |= D40_MEM_LCSP1_SCFG_TIM_MASK;
 		lli_dst->lcsp13 |= D40_MEM_LCSP3_DTCP_MASK;
 	}
@@ -348,9 +327,9 @@ static void d40_log_lli_link(struct d40_log_lli *lli_dst,
 void d40_log_lli_lcpa_write(struct d40_log_lli_full *lcpa,
 			   struct d40_log_lli *lli_dst,
 			   struct d40_log_lli *lli_src,
-			   int next)
+			   int next, unsigned int flags)
 {
-	d40_log_lli_link(lli_dst, lli_src, next);
+	d40_log_lli_link(lli_dst, lli_src, next, flags);
 
 	writel(lli_src->lcsp02, &lcpa[0].lcsp0);
 	writel(lli_src->lcsp13, &lcpa[0].lcsp1);
@@ -361,9 +340,9 @@ void d40_log_lli_lcpa_write(struct d40_log_lli_full *lcpa,
 void d40_log_lli_lcla_write(struct d40_log_lli *lcla,
 			   struct d40_log_lli *lli_dst,
 			   struct d40_log_lli *lli_src,
-			   int next)
+			   int next, unsigned int flags)
 {
-	d40_log_lli_link(lli_dst, lli_src, next);
+	d40_log_lli_link(lli_dst, lli_src, next, flags);
 
 	writel(lli_src->lcsp02, &lcla[0].lcsp02);
 	writel(lli_src->lcsp13, &lcla[0].lcsp13);
@@ -375,8 +354,10 @@ static void d40_log_fill_lli(struct d40_log_lli *lli,
 			     dma_addr_t data, u32 data_size,
 			     u32 reg_cfg,
 			     u32 data_width,
-			     bool addr_inc)
+			     unsigned int flags)
 {
+	bool addr_inc = flags & LLI_ADDR_INC;
+
 	lli->lcsp13 = reg_cfg;
 
 	/* The number of elements to transfer */
@@ -395,67 +376,15 @@ static void d40_log_fill_lli(struct d40_log_lli *lli,
 
 }
 
-int d40_log_sg_to_dev(struct scatterlist *sg,
-		      int sg_len,
-		      struct d40_log_lli_bidir *lli,
-		      struct d40_def_lcsp *lcsp,
-		      u32 src_data_width,
-		      u32 dst_data_width,
-		      enum dma_data_direction direction,
-		      dma_addr_t dev_addr)
-{
-	int total_size = 0;
-	struct scatterlist *current_sg = sg;
-	int i;
-	struct d40_log_lli *lli_src = lli->src;
-	struct d40_log_lli *lli_dst = lli->dst;
-
-	for_each_sg(sg, current_sg, sg_len, i) {
-		total_size += sg_dma_len(current_sg);
-
-		if (direction == DMA_TO_DEVICE) {
-			lli_src =
-				d40_log_buf_to_lli(lli_src,
-						   sg_phys(current_sg),
-						   sg_dma_len(current_sg),
-						   lcsp->lcsp1, src_data_width,
-						   dst_data_width,
-						   true);
-			lli_dst =
-				d40_log_buf_to_lli(lli_dst,
-						   dev_addr,
-						   sg_dma_len(current_sg),
-						   lcsp->lcsp3, dst_data_width,
-						   src_data_width,
-						   false);
-		} else {
-			lli_dst =
-				d40_log_buf_to_lli(lli_dst,
-						   sg_phys(current_sg),
-						   sg_dma_len(current_sg),
-						   lcsp->lcsp3, dst_data_width,
-						   src_data_width,
-						   true);
-			lli_src =
-				d40_log_buf_to_lli(lli_src,
-						   dev_addr,
-						   sg_dma_len(current_sg),
-						   lcsp->lcsp1, src_data_width,
-						   dst_data_width,
-						   false);
-		}
-	}
-	return total_size;
-}
-
-struct d40_log_lli *d40_log_buf_to_lli(struct d40_log_lli *lli_sg,
+static struct d40_log_lli *d40_log_buf_to_lli(struct d40_log_lli *lli_sg,
 				       dma_addr_t addr,
 				       int size,
 				       u32 lcsp13, /* src or dst*/
 				       u32 data_width1,
 				       u32 data_width2,
-				       bool addr_inc)
+				       unsigned int flags)
 {
+	bool addr_inc = flags & LLI_ADDR_INC;
 	struct d40_log_lli *lli = lli_sg;
 	int size_rest = size;
 	int size_seg = 0;
@@ -468,7 +397,7 @@ struct d40_log_lli *d40_log_buf_to_lli(struct d40_log_lli *lli_sg,
 				 addr,
 				 size_seg,
 				 lcsp13, data_width1,
-				 addr_inc);
+				 flags);
 		if (addr_inc)
 			addr += size_seg;
 		lli++;
@@ -479,6 +408,7 @@ struct d40_log_lli *d40_log_buf_to_lli(struct d40_log_lli *lli_sg,
 
 int d40_log_sg_to_lli(struct scatterlist *sg,
 		      int sg_len,
+		      dma_addr_t dev_addr,
 		      struct d40_log_lli *lli_sg,
 		      u32 lcsp13, /* src or dst*/
 		      u32 data_width1, u32 data_width2)
@@ -487,14 +417,24 @@ int d40_log_sg_to_lli(struct scatterlist *sg,
 	struct scatterlist *current_sg = sg;
 	int i;
 	struct d40_log_lli *lli = lli_sg;
+	unsigned long flags = 0;
+
+	if (!dev_addr)
+		flags |= LLI_ADDR_INC;
 
 	for_each_sg(sg, current_sg, sg_len, i) {
+		dma_addr_t sg_addr = sg_dma_address(current_sg);
+		unsigned int len = sg_dma_len(current_sg);
+		dma_addr_t addr = dev_addr ?: sg_addr;
+
 		total_size += sg_dma_len(current_sg);
-		lli = d40_log_buf_to_lli(lli,
-					 sg_phys(current_sg),
-					 sg_dma_len(current_sg),
+
+		lli = d40_log_buf_to_lli(lli, addr, len,
 					 lcsp13,
-					 data_width1, data_width2, true);
+					 data_width1,
+					 data_width2,
+					 flags);
 	}
+
 	return total_size;
 }
diff --git a/drivers/dma/ste_dma40_ll.h b/drivers/dma/ste_dma40_ll.h
index 9cc43495bea2..195ee65ee7f3 100644
--- a/drivers/dma/ste_dma40_ll.h
+++ b/drivers/dma/ste_dma40_ll.h
@@ -163,6 +163,22 @@
 #define D40_DREG_LCEIS1		0x0B4
 #define D40_DREG_LCEIS2		0x0B8
 #define D40_DREG_LCEIS3		0x0BC
+#define D40_DREG_PSEG1		0x110
+#define D40_DREG_PSEG2		0x114
+#define D40_DREG_PSEG3		0x118
+#define D40_DREG_PSEG4		0x11C
+#define D40_DREG_PCEG1		0x120
+#define D40_DREG_PCEG2		0x124
+#define D40_DREG_PCEG3		0x128
+#define D40_DREG_PCEG4		0x12C
+#define D40_DREG_RSEG1		0x130
+#define D40_DREG_RSEG2		0x134
+#define D40_DREG_RSEG3		0x138
+#define D40_DREG_RSEG4		0x13C
+#define D40_DREG_RCEG1		0x140
+#define D40_DREG_RCEG2		0x144
+#define D40_DREG_RCEG3		0x148
+#define D40_DREG_RCEG4		0x14C
 #define D40_DREG_STFU		0xFC8
 #define D40_DREG_ICFG		0xFCC
 #define D40_DREG_PERIPHID0	0xFE0
@@ -277,6 +293,13 @@ struct d40_def_lcsp {
 
 /* Physical channels */
 
+enum d40_lli_flags {
+	LLI_ADDR_INC	= 1 << 0,
+	LLI_TERM_INT	= 1 << 1,
+	LLI_CYCLIC	= 1 << 2,
+	LLI_LAST_LINK	= 1 << 3,
+};
+
 void d40_phy_cfg(struct stedma40_chan_cfg *cfg,
 		 u32 *src_cfg,
 		 u32 *dst_cfg,
@@ -292,46 +315,15 @@ int d40_phy_sg_to_lli(struct scatterlist *sg,
 		      struct d40_phy_lli *lli,
 		      dma_addr_t lli_phys,
 		      u32 reg_cfg,
-		      u32 data_width1,
-		      u32 data_width2,
-		      int psize);
-
-struct d40_phy_lli *d40_phy_buf_to_lli(struct d40_phy_lli *lli,
-				       dma_addr_t data,
-				       u32 data_size,
-				       int psize,
-				       dma_addr_t next_lli,
-				       u32 reg_cfg,
-				       bool term_int,
-				       u32 data_width1,
-				       u32 data_width2,
-				       bool is_device);
-
-void d40_phy_lli_write(void __iomem *virtbase,
-		       u32 phy_chan_num,
-		       struct d40_phy_lli *lli_dst,
-		       struct d40_phy_lli *lli_src);
+		      struct stedma40_half_channel_info *info,
+		      struct stedma40_half_channel_info *otherinfo,
+		      unsigned long flags);
 
 /* Logical channels */
 
-struct d40_log_lli *d40_log_buf_to_lli(struct d40_log_lli *lli_sg,
-				       dma_addr_t addr,
-				       int size,
-				       u32 lcsp13, /* src or dst*/
-				       u32 data_width1, u32 data_width2,
-				       bool addr_inc);
-
-int d40_log_sg_to_dev(struct scatterlist *sg,
-		      int sg_len,
-		      struct d40_log_lli_bidir *lli,
-		      struct d40_def_lcsp *lcsp,
-		      u32 src_data_width,
-		      u32 dst_data_width,
-		      enum dma_data_direction direction,
-		      dma_addr_t dev_addr);
-
 int d40_log_sg_to_lli(struct scatterlist *sg,
 		      int sg_len,
+		      dma_addr_t dev_addr,
 		      struct d40_log_lli *lli_sg,
 		      u32 lcsp13, /* src or dst*/
 		      u32 data_width1, u32 data_width2);
@@ -339,11 +331,11 @@ int d40_log_sg_to_lli(struct scatterlist *sg,
 void d40_log_lli_lcpa_write(struct d40_log_lli_full *lcpa,
 			    struct d40_log_lli *lli_dst,
 			    struct d40_log_lli *lli_src,
-			    int next);
+			    int next, unsigned int flags);
 
 void d40_log_lli_lcla_write(struct d40_log_lli *lcla,
 			    struct d40_log_lli *lli_dst,
 			    struct d40_log_lli *lli_src,
-			    int next);
+			    int next, unsigned int flags);
 
 #endif /* STE_DMA40_LLI_H */
diff --git a/drivers/dma/timb_dma.c b/drivers/dma/timb_dma.c
index 3b88a4e7c98a..d2c75feff7df 100644
--- a/drivers/dma/timb_dma.c
+++ b/drivers/dma/timb_dma.c
@@ -27,6 +27,7 @@
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 
 #include <linux/timb_dma.h>
@@ -629,7 +630,7 @@ static int td_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		desc_node)
 		list_move(&td_desc->desc_node, &td_chan->free_list);
 
-	/* now tear down the runnning */
+	/* now tear down the running */
 	__td_finish(td_chan);
 	spin_unlock_bh(&td_chan->lock);
 
@@ -684,7 +685,7 @@ static irqreturn_t td_irq(int irq, void *devid)
 
 static int __devinit td_probe(struct platform_device *pdev)
 {
-	struct timb_dma_platform_data *pdata = pdev->dev.platform_data;
+	struct timb_dma_platform_data *pdata = mfd_get_data(pdev);
 	struct timb_dma *td;
 	struct resource *iomem;
 	int irq;
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 23e03554f0d3..0be30e978c85 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -25,59 +25,12 @@ static struct mem_ctl_info **mcis;
 static struct ecc_settings **ecc_stngs;
 
 /*
- * Address to DRAM bank mapping: see F2x80 for K8 and F2x[1,0]80 for Fam10 and
- * later.
- */
-static int ddr2_dbam_revCG[] = {
-			   [0]		= 32,
-			   [1]		= 64,
-			   [2]		= 128,
-			   [3]		= 256,
-			   [4]		= 512,
-			   [5]		= 1024,
-			   [6]		= 2048,
-};
-
-static int ddr2_dbam_revD[] = {
-			   [0]		= 32,
-			   [1]		= 64,
-			   [2 ... 3]	= 128,
-			   [4]		= 256,
-			   [5]		= 512,
-			   [6]		= 256,
-			   [7]		= 512,
-			   [8 ... 9]	= 1024,
-			   [10]		= 2048,
-};
-
-static int ddr2_dbam[] = { [0]		= 128,
-			   [1]		= 256,
-			   [2 ... 4]	= 512,
-			   [5 ... 6]	= 1024,
-			   [7 ... 8]	= 2048,
-			   [9 ... 10]	= 4096,
-			   [11]		= 8192,
-};
-
-static int ddr3_dbam[] = { [0]		= -1,
-			   [1]		= 256,
-			   [2]		= 512,
-			   [3 ... 4]	= -1,
-			   [5 ... 6]	= 1024,
-			   [7 ... 8]	= 2048,
-			   [9 ... 10]	= 4096,
-			   [11]		= 8192,
-};
-
-/*
  * Valid scrub rates for the K8 hardware memory scrubber. We map the scrubbing
  * bandwidth to a valid bit pattern. The 'set' operation finds the 'matching-
  * or higher value'.
  *
  *FIXME: Produce a better mapping/linearisation.
  */
-
-
 struct scrubrate {
        u32 scrubval;           /* bit pattern for scrub rate */
        u32 bandwidth;          /* bandwidth consumed (bytes/sec) */
@@ -107,6 +60,79 @@ struct scrubrate {
 	{ 0x00, 0UL},        /* scrubbing off */
 };
 
+static int __amd64_read_pci_cfg_dword(struct pci_dev *pdev, int offset,
+				      u32 *val, const char *func)
+{
+	int err = 0;
+
+	err = pci_read_config_dword(pdev, offset, val);
+	if (err)
+		amd64_warn("%s: error reading F%dx%03x.\n",
+			   func, PCI_FUNC(pdev->devfn), offset);
+
+	return err;
+}
+
+int __amd64_write_pci_cfg_dword(struct pci_dev *pdev, int offset,
+				u32 val, const char *func)
+{
+	int err = 0;
+
+	err = pci_write_config_dword(pdev, offset, val);
+	if (err)
+		amd64_warn("%s: error writing to F%dx%03x.\n",
+			   func, PCI_FUNC(pdev->devfn), offset);
+
+	return err;
+}
+
+/*
+ *
+ * Depending on the family, F2 DCT reads need special handling:
+ *
+ * K8: has a single DCT only
+ *
+ * F10h: each DCT has its own set of regs
+ *	DCT0 -> F2x040..
+ *	DCT1 -> F2x140..
+ *
+ * F15h: we select which DCT we access using F1x10C[DctCfgSel]
+ *
+ */
+static int k8_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
+			       const char *func)
+{
+	if (addr >= 0x100)
+		return -EINVAL;
+
+	return __amd64_read_pci_cfg_dword(pvt->F2, addr, val, func);
+}
+
+static int f10_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
+				 const char *func)
+{
+	return __amd64_read_pci_cfg_dword(pvt->F2, addr, val, func);
+}
+
+static int f15_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
+				 const char *func)
+{
+	u32 reg = 0;
+	u8 dct  = 0;
+
+	if (addr >= 0x140 && addr <= 0x1a0) {
+		dct   = 1;
+		addr -= 0x100;
+	}
+
+	amd64_read_pci_cfg(pvt->F1, DCT_CFG_SEL, &reg);
+	reg &= 0xfffffffe;
+	reg |= dct;
+	amd64_write_pci_cfg(pvt->F1, DCT_CFG_SEL, reg);
+
+	return __amd64_read_pci_cfg_dword(pvt->F2, addr, val, func);
+}
+
 /*
  * Memory scrubber control interface. For K8, memory scrubbing is handled by
  * hardware and can involve L2 cache, dcache as well as the main memory. With
@@ -156,7 +182,7 @@ static int __amd64_set_scrub_rate(struct pci_dev *ctl, u32 new_bw, u32 min_rate)
 
 	scrubval = scrubrates[i].scrubval;
 
-	pci_write_bits32(ctl, K8_SCRCTRL, scrubval, 0x001F);
+	pci_write_bits32(ctl, SCRCTRL, scrubval, 0x001F);
 
 	if (scrubval)
 		return scrubrates[i].bandwidth;
@@ -167,8 +193,12 @@ static int __amd64_set_scrub_rate(struct pci_dev *ctl, u32 new_bw, u32 min_rate)
 static int amd64_set_scrub_rate(struct mem_ctl_info *mci, u32 bw)
 {
 	struct amd64_pvt *pvt = mci->pvt_info;
+	u32 min_scrubrate = 0x5;
+
+	if (boot_cpu_data.x86 == 0xf)
+		min_scrubrate = 0x0;
 
-	return __amd64_set_scrub_rate(pvt->F3, bw, pvt->min_scrubrate);
+	return __amd64_set_scrub_rate(pvt->F3, bw, min_scrubrate);
 }
 
 static int amd64_get_scrub_rate(struct mem_ctl_info *mci)
@@ -177,7 +207,7 @@ static int amd64_get_scrub_rate(struct mem_ctl_info *mci)
 	u32 scrubval = 0;
 	int i, retval = -EINVAL;
 
-	amd64_read_pci_cfg(pvt->F3, K8_SCRCTRL, &scrubval);
+	amd64_read_pci_cfg(pvt->F3, SCRCTRL, &scrubval);
 
 	scrubval = scrubval & 0x001F;
 
@@ -192,63 +222,14 @@ static int amd64_get_scrub_rate(struct mem_ctl_info *mci)
 	return retval;
 }
 
-/* Map from a CSROW entry to the mask entry that operates on it */
-static inline u32 amd64_map_to_dcs_mask(struct amd64_pvt *pvt, int csrow)
-{
-	if (boot_cpu_data.x86 == 0xf && pvt->ext_model < K8_REV_F)
-		return csrow;
-	else
-		return csrow >> 1;
-}
-
-/* return the 'base' address the i'th CS entry of the 'dct' DRAM controller */
-static u32 amd64_get_dct_base(struct amd64_pvt *pvt, int dct, int csrow)
-{
-	if (dct == 0)
-		return pvt->dcsb0[csrow];
-	else
-		return pvt->dcsb1[csrow];
-}
-
-/*
- * Return the 'mask' address the i'th CS entry. This function is needed because
- * there number of DCSM registers on Rev E and prior vs Rev F and later is
- * different.
- */
-static u32 amd64_get_dct_mask(struct amd64_pvt *pvt, int dct, int csrow)
-{
-	if (dct == 0)
-		return pvt->dcsm0[amd64_map_to_dcs_mask(pvt, csrow)];
-	else
-		return pvt->dcsm1[amd64_map_to_dcs_mask(pvt, csrow)];
-}
-
-
 /*
- * In *base and *limit, pass back the full 40-bit base and limit physical
- * addresses for the node given by node_id.  This information is obtained from
- * DRAM Base (section 3.4.4.1) and DRAM Limit (section 3.4.4.2) registers. The
- * base and limit addresses are of type SysAddr, as defined at the start of
- * section 3.4.4 (p. 70).  They are the lowest and highest physical addresses
- * in the address range they represent.
+ * returns true if the SysAddr given by sys_addr matches the
+ * DRAM base/limit associated with node_id
  */
-static void amd64_get_base_and_limit(struct amd64_pvt *pvt, int node_id,
-			       u64 *base, u64 *limit)
+static bool amd64_base_limit_match(struct amd64_pvt *pvt, u64 sys_addr,
+				   unsigned nid)
 {
-	*base = pvt->dram_base[node_id];
-	*limit = pvt->dram_limit[node_id];
-}
-
-/*
- * Return 1 if the SysAddr given by sys_addr matches the base/limit associated
- * with node_id
- */
-static int amd64_base_limit_match(struct amd64_pvt *pvt,
-					u64 sys_addr, int node_id)
-{
-	u64 base, limit, addr;
-
-	amd64_get_base_and_limit(pvt, node_id, &base, &limit);
+	u64 addr;
 
 	/* The K8 treats this as a 40-bit value.  However, bits 63-40 will be
 	 * all ones if the most significant implemented address bit is 1.
@@ -258,7 +239,8 @@ static int amd64_base_limit_match(struct amd64_pvt *pvt,
 	 */
 	addr = sys_addr & 0x000000ffffffffffull;
 
-	return (addr >= base) && (addr <= limit);
+	return ((addr >= get_dram_base(pvt, nid)) &&
+		(addr <= get_dram_limit(pvt, nid)));
 }
 
 /*
@@ -271,7 +253,7 @@ static struct mem_ctl_info *find_mc_by_sys_addr(struct mem_ctl_info *mci,
 						u64 sys_addr)
 {
 	struct amd64_pvt *pvt;
-	int node_id;
+	unsigned node_id;
 	u32 intlv_en, bits;
 
 	/*
@@ -285,10 +267,10 @@ static struct mem_ctl_info *find_mc_by_sys_addr(struct mem_ctl_info *mci,
 	 * registers.  Therefore we arbitrarily choose to read it from the
 	 * register for node 0.
 	 */
-	intlv_en = pvt->dram_IntlvEn[0];
+	intlv_en = dram_intlv_en(pvt, 0);
 
 	if (intlv_en == 0) {
-		for (node_id = 0; node_id < DRAM_REG_COUNT; node_id++) {
+		for (node_id = 0; node_id < DRAM_RANGES; node_id++) {
 			if (amd64_base_limit_match(pvt, sys_addr, node_id))
 				goto found;
 		}
@@ -305,10 +287,10 @@ static struct mem_ctl_info *find_mc_by_sys_addr(struct mem_ctl_info *mci,
 	bits = (((u32) sys_addr) >> 12) & intlv_en;
 
 	for (node_id = 0; ; ) {
-		if ((pvt->dram_IntlvSel[node_id] & intlv_en) == bits)
+		if ((dram_intlv_sel(pvt, node_id) & intlv_en) == bits)
 			break;	/* intlv_sel field matches */
 
-		if (++node_id >= DRAM_REG_COUNT)
+		if (++node_id >= DRAM_RANGES)
 			goto err_no_match;
 	}
 
@@ -321,7 +303,7 @@ static struct mem_ctl_info *find_mc_by_sys_addr(struct mem_ctl_info *mci,
 	}
 
 found:
-	return edac_mc_find(node_id);
+	return edac_mc_find((int)node_id);
 
 err_no_match:
 	debugf2("sys_addr 0x%lx doesn't match any node\n",
@@ -331,37 +313,50 @@ err_no_match:
 }
 
 /*
- * Extract the DRAM CS base address from selected csrow register.
+ * compute the CS base address of the @csrow on the DRAM controller @dct.
+ * For details see F2x[5C:40] in the processor's BKDG
  */
-static u64 base_from_dct_base(struct amd64_pvt *pvt, int csrow)
+static void get_cs_base_and_mask(struct amd64_pvt *pvt, int csrow, u8 dct,
+				 u64 *base, u64 *mask)
 {
-	return ((u64) (amd64_get_dct_base(pvt, 0, csrow) & pvt->dcsb_base)) <<
-				pvt->dcs_shift;
-}
+	u64 csbase, csmask, base_bits, mask_bits;
+	u8 addr_shift;
 
-/*
- * Extract the mask from the dcsb0[csrow] entry in a CPU revision-specific way.
- */
-static u64 mask_from_dct_mask(struct amd64_pvt *pvt, int csrow)
-{
-	u64 dcsm_bits, other_bits;
-	u64 mask;
-
-	/* Extract bits from DRAM CS Mask. */
-	dcsm_bits = amd64_get_dct_mask(pvt, 0, csrow) & pvt->dcsm_mask;
+	if (boot_cpu_data.x86 == 0xf && pvt->ext_model < K8_REV_F) {
+		csbase		= pvt->csels[dct].csbases[csrow];
+		csmask		= pvt->csels[dct].csmasks[csrow];
+		base_bits	= GENMASK(21, 31) | GENMASK(9, 15);
+		mask_bits	= GENMASK(21, 29) | GENMASK(9, 15);
+		addr_shift	= 4;
+	} else {
+		csbase		= pvt->csels[dct].csbases[csrow];
+		csmask		= pvt->csels[dct].csmasks[csrow >> 1];
+		addr_shift	= 8;
 
-	other_bits = pvt->dcsm_mask;
-	other_bits = ~(other_bits << pvt->dcs_shift);
+		if (boot_cpu_data.x86 == 0x15)
+			base_bits = mask_bits = GENMASK(19,30) | GENMASK(5,13);
+		else
+			base_bits = mask_bits = GENMASK(19,28) | GENMASK(5,13);
+	}
 
-	/*
-	 * The extracted bits from DCSM belong in the spaces represented by
-	 * the cleared bits in other_bits.
-	 */
-	mask = (dcsm_bits << pvt->dcs_shift) | other_bits;
+	*base  = (csbase & base_bits) << addr_shift;
 
-	return mask;
+	*mask  = ~0ULL;
+	/* poke holes for the csmask */
+	*mask &= ~(mask_bits << addr_shift);
+	/* OR them in */
+	*mask |= (csmask & mask_bits) << addr_shift;
 }
 
+#define for_each_chip_select(i, dct, pvt) \
+	for (i = 0; i < pvt->csels[dct].b_cnt; i++)
+
+#define chip_select_base(i, dct, pvt) \
+	pvt->csels[dct].csbases[i]
+
+#define for_each_chip_select_mask(i, dct, pvt) \
+	for (i = 0; i < pvt->csels[dct].m_cnt; i++)
+
 /*
  * @input_addr is an InputAddr associated with the node given by mci. Return the
  * csrow that input_addr maps to, or -1 on failure (no csrow claims input_addr).
@@ -374,19 +369,13 @@ static int input_addr_to_csrow(struct mem_ctl_info *mci, u64 input_addr)
 
 	pvt = mci->pvt_info;
 
-	/*
-	 * Here we use the DRAM CS Base and DRAM CS Mask registers. For each CS
-	 * base/mask register pair, test the condition shown near the start of
-	 * section 3.5.4 (p. 84, BKDG #26094, K8, revA-E).
-	 */
-	for (csrow = 0; csrow < pvt->cs_count; csrow++) {
-
-		/* This DRAM chip select is disabled on this node */
-		if ((pvt->dcsb0[csrow] & K8_DCSB_CS_ENABLE) == 0)
+	for_each_chip_select(csrow, 0, pvt) {
+		if (!csrow_enabled(csrow, 0, pvt))
 			continue;
 
-		base = base_from_dct_base(pvt, csrow);
-		mask = ~mask_from_dct_mask(pvt, csrow);
+		get_cs_base_and_mask(pvt, csrow, 0, &base, &mask);
+
+		mask = ~mask;
 
 		if ((input_addr & mask) == (base & mask)) {
 			debugf2("InputAddr 0x%lx matches csrow %d (node %d)\n",
@@ -396,7 +385,6 @@ static int input_addr_to_csrow(struct mem_ctl_info *mci, u64 input_addr)
 			return csrow;
 		}
 	}
-
 	debugf2("no matching csrow for InputAddr 0x%lx (MC node %d)\n",
 		(unsigned long)input_addr, pvt->mc_node_id);
 
@@ -404,19 +392,6 @@ static int input_addr_to_csrow(struct mem_ctl_info *mci, u64 input_addr)
 }
 
 /*
- * Return the base value defined by the DRAM Base register for the node
- * represented by mci.  This function returns the full 40-bit value despite the
- * fact that the register only stores bits 39-24 of the value. See section
- * 3.4.4.1 (BKDG #26094, K8, revA-E)
- */
-static inline u64 get_dram_base(struct mem_ctl_info *mci)
-{
-	struct amd64_pvt *pvt = mci->pvt_info;
-
-	return pvt->dram_base[pvt->mc_node_id];
-}
-
-/*
  * Obtain info from the DRAM Hole Address Register (section 3.4.8, pub #26094)
  * for the node represented by mci. Info is passed back in *hole_base,
  * *hole_offset, and *hole_size.  Function returns 0 if info is valid or 1 if
@@ -445,14 +420,13 @@ int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 		return 1;
 	}
 
-	/* only valid for Fam10h */
-	if (boot_cpu_data.x86 == 0x10 &&
-	    (pvt->dhar & F10_DRAM_MEM_HOIST_VALID) == 0) {
+	/* valid for Fam10h and above */
+	if (boot_cpu_data.x86 >= 0x10 && !dhar_mem_hoist_valid(pvt)) {
 		debugf1("  Dram Memory Hoisting is DISABLED on this system\n");
 		return 1;
 	}
 
-	if ((pvt->dhar & DHAR_VALID) == 0) {
+	if (!dhar_valid(pvt)) {
 		debugf1("  Dram Memory Hoisting is DISABLED on this node %d\n",
 			pvt->mc_node_id);
 		return 1;
@@ -476,15 +450,15 @@ int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 	 * addresses in the hole so that they start at 0x100000000.
 	 */
 
-	base = dhar_base(pvt->dhar);
+	base = dhar_base(pvt);
 
 	*hole_base = base;
 	*hole_size = (0x1ull << 32) - base;
 
 	if (boot_cpu_data.x86 > 0xf)
-		*hole_offset = f10_dhar_offset(pvt->dhar);
+		*hole_offset = f10_dhar_offset(pvt);
 	else
-		*hole_offset = k8_dhar_offset(pvt->dhar);
+		*hole_offset = k8_dhar_offset(pvt);
 
 	debugf1("  DHAR info for node %d base 0x%lx offset 0x%lx size 0x%lx\n",
 		pvt->mc_node_id, (unsigned long)*hole_base,
@@ -525,10 +499,11 @@ EXPORT_SYMBOL_GPL(amd64_get_dram_hole_info);
  */
 static u64 sys_addr_to_dram_addr(struct mem_ctl_info *mci, u64 sys_addr)
 {
+	struct amd64_pvt *pvt = mci->pvt_info;
 	u64 dram_base, hole_base, hole_offset, hole_size, dram_addr;
 	int ret = 0;
 
-	dram_base = get_dram_base(mci);
+	dram_base = get_dram_base(pvt, pvt->mc_node_id);
 
 	ret = amd64_get_dram_hole_info(mci, &hole_base, &hole_offset,
 				      &hole_size);
@@ -556,7 +531,7 @@ static u64 sys_addr_to_dram_addr(struct mem_ctl_info *mci, u64 sys_addr)
 	 * section 3.4.2 of AMD publication 24592: AMD x86-64 Architecture
 	 * Programmer's Manual Volume 1 Application Programming.
 	 */
-	dram_addr = (sys_addr & 0xffffffffffull) - dram_base;
+	dram_addr = (sys_addr & GENMASK(0, 39)) - dram_base;
 
 	debugf2("using DRAM Base register to translate SysAddr 0x%lx to "
 		"DramAddr 0x%lx\n", (unsigned long)sys_addr,
@@ -592,9 +567,9 @@ static u64 dram_addr_to_input_addr(struct mem_ctl_info *mci, u64 dram_addr)
 	 * See the start of section 3.4.4 (p. 70, BKDG #26094, K8, revA-E)
 	 * concerning translating a DramAddr to an InputAddr.
 	 */
-	intlv_shift = num_node_interleave_bits(pvt->dram_IntlvEn[0]);
-	input_addr = ((dram_addr >> intlv_shift) & 0xffffff000ull) +
-	    (dram_addr & 0xfff);
+	intlv_shift = num_node_interleave_bits(dram_intlv_en(pvt, 0));
+	input_addr = ((dram_addr >> intlv_shift) & GENMASK(12, 35)) +
+		      (dram_addr & 0xfff);
 
 	debugf2("  Intlv Shift=%d DramAddr=0x%lx maps to InputAddr=0x%lx\n",
 		intlv_shift, (unsigned long)dram_addr,
@@ -628,7 +603,7 @@ static u64 sys_addr_to_input_addr(struct mem_ctl_info *mci, u64 sys_addr)
 static u64 input_addr_to_dram_addr(struct mem_ctl_info *mci, u64 input_addr)
 {
 	struct amd64_pvt *pvt;
-	int node_id, intlv_shift;
+	unsigned node_id, intlv_shift;
 	u64 bits, dram_addr;
 	u32 intlv_sel;
 
@@ -642,10 +617,10 @@ static u64 input_addr_to_dram_addr(struct mem_ctl_info *mci, u64 input_addr)
 	 */
 	pvt = mci->pvt_info;
 	node_id = pvt->mc_node_id;
-	BUG_ON((node_id < 0) || (node_id > 7));
 
-	intlv_shift = num_node_interleave_bits(pvt->dram_IntlvEn[0]);
+	BUG_ON(node_id > 7);
 
+	intlv_shift = num_node_interleave_bits(dram_intlv_en(pvt, 0));
 	if (intlv_shift == 0) {
 		debugf1("    InputAddr 0x%lx translates to DramAddr of "
 			"same value\n",	(unsigned long)input_addr);
@@ -653,10 +628,10 @@ static u64 input_addr_to_dram_addr(struct mem_ctl_info *mci, u64 input_addr)
 		return input_addr;
 	}
 
-	bits = ((input_addr & 0xffffff000ull) << intlv_shift) +
-	    (input_addr & 0xfff);
+	bits = ((input_addr & GENMASK(12, 35)) << intlv_shift) +
+		(input_addr & 0xfff);
 
-	intlv_sel = pvt->dram_IntlvSel[node_id] & ((1 << intlv_shift) - 1);
+	intlv_sel = dram_intlv_sel(pvt, node_id) & ((1 << intlv_shift) - 1);
 	dram_addr = bits + (intlv_sel << 12);
 
 	debugf1("InputAddr 0x%lx translates to DramAddr 0x%lx "
@@ -673,7 +648,7 @@ static u64 input_addr_to_dram_addr(struct mem_ctl_info *mci, u64 input_addr)
 static u64 dram_addr_to_sys_addr(struct mem_ctl_info *mci, u64 dram_addr)
 {
 	struct amd64_pvt *pvt = mci->pvt_info;
-	u64 hole_base, hole_offset, hole_size, base, limit, sys_addr;
+	u64 hole_base, hole_offset, hole_size, base, sys_addr;
 	int ret = 0;
 
 	ret = amd64_get_dram_hole_info(mci, &hole_base, &hole_offset,
@@ -691,7 +666,7 @@ static u64 dram_addr_to_sys_addr(struct mem_ctl_info *mci, u64 dram_addr)
 		}
 	}
 
-	amd64_get_base_and_limit(pvt, pvt->mc_node_id, &base, &limit);
+	base     = get_dram_base(pvt, pvt->mc_node_id);
 	sys_addr = dram_addr + base;
 
 	/*
@@ -736,13 +711,12 @@ static void find_csrow_limits(struct mem_ctl_info *mci, int csrow,
 	u64 base, mask;
 
 	pvt = mci->pvt_info;
-	BUG_ON((csrow < 0) || (csrow >= pvt->cs_count));
+	BUG_ON((csrow < 0) || (csrow >= pvt->csels[0].b_cnt));
 
-	base = base_from_dct_base(pvt, csrow);
-	mask = mask_from_dct_mask(pvt, csrow);
+	get_cs_base_and_mask(pvt, csrow, 0, &base, &mask);
 
 	*input_addr_min = base & ~mask;
-	*input_addr_max = base | mask | pvt->dcs_mask_notused;
+	*input_addr_max = base | mask;
 }
 
 /* Map the Error address to a PAGE and PAGE OFFSET. */
@@ -775,18 +749,13 @@ static int sys_addr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr)
 
 static int get_channel_from_ecc_syndrome(struct mem_ctl_info *, u16);
 
-static u16 extract_syndrome(struct err_regs *err)
-{
-	return ((err->nbsh >> 15) & 0xff) | ((err->nbsl >> 16) & 0xff00);
-}
-
 /*
  * Determine if the DIMMs have ECC enabled. ECC is enabled ONLY if all the DIMMs
  * are ECC capable.
  */
 static enum edac_type amd64_determine_edac_cap(struct amd64_pvt *pvt)
 {
-	int bit;
+	u8 bit;
 	enum dev_type edac_cap = EDAC_FLAG_NONE;
 
 	bit = (boot_cpu_data.x86 > 0xf || pvt->ext_model >= K8_REV_F)
@@ -799,8 +768,7 @@ static enum edac_type amd64_determine_edac_cap(struct amd64_pvt *pvt)
 	return edac_cap;
 }
 
-
-static void amd64_debug_display_dimm_sizes(int ctrl, struct amd64_pvt *pvt);
+static void amd64_debug_display_dimm_sizes(struct amd64_pvt *, u8);
 
 static void amd64_dump_dramcfg_low(u32 dclr, int chan)
 {
@@ -813,8 +781,9 @@ static void amd64_dump_dramcfg_low(u32 dclr, int chan)
 	debugf1("  PAR/ERR parity: %s\n",
 		(dclr & BIT(8)) ?  "enabled" : "disabled");
 
-	debugf1("  DCT 128bit mode width: %s\n",
-		(dclr & BIT(11)) ?  "128b" : "64b");
+	if (boot_cpu_data.x86 == 0x10)
+		debugf1("  DCT 128bit mode width: %s\n",
+			(dclr & BIT(11)) ?  "128b" : "64b");
 
 	debugf1("  x4 logical DIMMs present: L0: %s L1: %s L2: %s L3: %s\n",
 		(dclr & BIT(12)) ?  "yes" : "no",
@@ -824,16 +793,16 @@ static void amd64_dump_dramcfg_low(u32 dclr, int chan)
 }
 
 /* Display and decode various NB registers for debug purposes. */
-static void amd64_dump_misc_regs(struct amd64_pvt *pvt)
+static void dump_misc_regs(struct amd64_pvt *pvt)
 {
 	debugf1("F3xE8 (NB Cap): 0x%08x\n", pvt->nbcap);
 
 	debugf1("  NB two channel DRAM capable: %s\n",
-		(pvt->nbcap & K8_NBCAP_DCT_DUAL) ? "yes" : "no");
+		(pvt->nbcap & NBCAP_DCT_DUAL) ? "yes" : "no");
 
 	debugf1("  ECC capable: %s, ChipKill ECC capable: %s\n",
-		(pvt->nbcap & K8_NBCAP_SECDED) ? "yes" : "no",
-		(pvt->nbcap & K8_NBCAP_CHIPKILL) ? "yes" : "no");
+		(pvt->nbcap & NBCAP_SECDED) ? "yes" : "no",
+		(pvt->nbcap & NBCAP_CHIPKILL) ? "yes" : "no");
 
 	amd64_dump_dramcfg_low(pvt->dclr0, 0);
 
@@ -841,130 +810,84 @@ static void amd64_dump_misc_regs(struct amd64_pvt *pvt)
 
 	debugf1("F1xF0 (DRAM Hole Address): 0x%08x, base: 0x%08x, "
 			"offset: 0x%08x\n",
-			pvt->dhar,
-			dhar_base(pvt->dhar),
-			(boot_cpu_data.x86 == 0xf) ? k8_dhar_offset(pvt->dhar)
-						   : f10_dhar_offset(pvt->dhar));
+			pvt->dhar, dhar_base(pvt),
+			(boot_cpu_data.x86 == 0xf) ? k8_dhar_offset(pvt)
+						   : f10_dhar_offset(pvt));
 
-	debugf1("  DramHoleValid: %s\n",
-		(pvt->dhar & DHAR_VALID) ? "yes" : "no");
+	debugf1("  DramHoleValid: %s\n", dhar_valid(pvt) ? "yes" : "no");
 
-	amd64_debug_display_dimm_sizes(0, pvt);
+	amd64_debug_display_dimm_sizes(pvt, 0);
 
 	/* everything below this point is Fam10h and above */
 	if (boot_cpu_data.x86 == 0xf)
 		return;
 
-	amd64_debug_display_dimm_sizes(1, pvt);
+	amd64_debug_display_dimm_sizes(pvt, 1);
 
-	amd64_info("using %s syndromes.\n", ((pvt->syn_type == 8) ? "x8" : "x4"));
+	amd64_info("using %s syndromes.\n", ((pvt->ecc_sym_sz == 8) ? "x8" : "x4"));
 
 	/* Only if NOT ganged does dclr1 have valid info */
 	if (!dct_ganging_enabled(pvt))
 		amd64_dump_dramcfg_low(pvt->dclr1, 1);
 }
 
-/* Read in both of DBAM registers */
-static void amd64_read_dbam_reg(struct amd64_pvt *pvt)
-{
-	amd64_read_pci_cfg(pvt->F2, DBAM0, &pvt->dbam0);
-
-	if (boot_cpu_data.x86 >= 0x10)
-		amd64_read_pci_cfg(pvt->F2, DBAM1, &pvt->dbam1);
-}
-
 /*
- * NOTE: CPU Revision Dependent code: Rev E and Rev F
- *
- * Set the DCSB and DCSM mask values depending on the CPU revision value. Also
- * set the shift factor for the DCSB and DCSM values.
- *
- * ->dcs_mask_notused, RevE:
- *
- * To find the max InputAddr for the csrow, start with the base address and set
- * all bits that are "don't care" bits in the test at the start of section
- * 3.5.4 (p. 84).
- *
- * The "don't care" bits are all set bits in the mask and all bits in the gaps
- * between bit ranges [35:25] and [19:13]. The value REV_E_DCS_NOTUSED_BITS
- * represents bits [24:20] and [12:0], which are all bits in the above-mentioned
- * gaps.
- *
- * ->dcs_mask_notused, RevF and later:
- *
- * To find the max InputAddr for the csrow, start with the base address and set
- * all bits that are "don't care" bits in the test at the start of NPT section
- * 4.5.4 (p. 87).
- *
- * The "don't care" bits are all set bits in the mask and all bits in the gaps
- * between bit ranges [36:27] and [21:13].
- *
- * The value REV_F_F1Xh_DCS_NOTUSED_BITS represents bits [26:22] and [12:0],
- * which are all bits in the above-mentioned gaps.
+ * see BKDG, F2x[1,0][5C:40], F2[1,0][6C:60]
  */
-static void amd64_set_dct_base_and_mask(struct amd64_pvt *pvt)
+static void prep_chip_selects(struct amd64_pvt *pvt)
 {
-
 	if (boot_cpu_data.x86 == 0xf && pvt->ext_model < K8_REV_F) {
-		pvt->dcsb_base		= REV_E_DCSB_BASE_BITS;
-		pvt->dcsm_mask		= REV_E_DCSM_MASK_BITS;
-		pvt->dcs_mask_notused	= REV_E_DCS_NOTUSED_BITS;
-		pvt->dcs_shift		= REV_E_DCS_SHIFT;
-		pvt->cs_count		= 8;
-		pvt->num_dcsm		= 8;
+		pvt->csels[0].b_cnt = pvt->csels[1].b_cnt = 8;
+		pvt->csels[0].m_cnt = pvt->csels[1].m_cnt = 8;
 	} else {
-		pvt->dcsb_base		= REV_F_F1Xh_DCSB_BASE_BITS;
-		pvt->dcsm_mask		= REV_F_F1Xh_DCSM_MASK_BITS;
-		pvt->dcs_mask_notused	= REV_F_F1Xh_DCS_NOTUSED_BITS;
-		pvt->dcs_shift		= REV_F_F1Xh_DCS_SHIFT;
-		pvt->cs_count		= 8;
-		pvt->num_dcsm		= 4;
+		pvt->csels[0].b_cnt = pvt->csels[1].b_cnt = 8;
+		pvt->csels[0].m_cnt = pvt->csels[1].m_cnt = 4;
 	}
 }
 
 /*
- * Function 2 Offset F10_DCSB0; read in the DCS Base and DCS Mask hw registers
+ * Function 2 Offset F10_DCSB0; read in the DCS Base and DCS Mask registers
  */
-static void amd64_read_dct_base_mask(struct amd64_pvt *pvt)
+static void read_dct_base_mask(struct amd64_pvt *pvt)
 {
-	int cs, reg;
+	int cs;
 
-	amd64_set_dct_base_and_mask(pvt);
+	prep_chip_selects(pvt);
 
-	for (cs = 0; cs < pvt->cs_count; cs++) {
-		reg = K8_DCSB0 + (cs * 4);
-		if (!amd64_read_pci_cfg(pvt->F2, reg, &pvt->dcsb0[cs]))
+	for_each_chip_select(cs, 0, pvt) {
+		int reg0   = DCSB0 + (cs * 4);
+		int reg1   = DCSB1 + (cs * 4);
+		u32 *base0 = &pvt->csels[0].csbases[cs];
+		u32 *base1 = &pvt->csels[1].csbases[cs];
+
+		if (!amd64_read_dct_pci_cfg(pvt, reg0, base0))
 			debugf0("  DCSB0[%d]=0x%08x reg: F2x%x\n",
-				cs, pvt->dcsb0[cs], reg);
-
-		/* If DCT are NOT ganged, then read in DCT1's base */
-		if (boot_cpu_data.x86 >= 0x10 && !dct_ganging_enabled(pvt)) {
-			reg = F10_DCSB1 + (cs * 4);
-			if (!amd64_read_pci_cfg(pvt->F2, reg,
-						&pvt->dcsb1[cs]))
-				debugf0("  DCSB1[%d]=0x%08x reg: F2x%x\n",
-					cs, pvt->dcsb1[cs], reg);
-		} else {
-			pvt->dcsb1[cs] = 0;
-		}
+				cs, *base0, reg0);
+
+		if (boot_cpu_data.x86 == 0xf || dct_ganging_enabled(pvt))
+			continue;
+
+		if (!amd64_read_dct_pci_cfg(pvt, reg1, base1))
+			debugf0("  DCSB1[%d]=0x%08x reg: F2x%x\n",
+				cs, *base1, reg1);
 	}
 
-	for (cs = 0; cs < pvt->num_dcsm; cs++) {
-		reg = K8_DCSM0 + (cs * 4);
-		if (!amd64_read_pci_cfg(pvt->F2, reg, &pvt->dcsm0[cs]))
+	for_each_chip_select_mask(cs, 0, pvt) {
+		int reg0   = DCSM0 + (cs * 4);
+		int reg1   = DCSM1 + (cs * 4);
+		u32 *mask0 = &pvt->csels[0].csmasks[cs];
+		u32 *mask1 = &pvt->csels[1].csmasks[cs];
+
+		if (!amd64_read_dct_pci_cfg(pvt, reg0, mask0))
 			debugf0("    DCSM0[%d]=0x%08x reg: F2x%x\n",
-				cs, pvt->dcsm0[cs], reg);
-
-		/* If DCT are NOT ganged, then read in DCT1's mask */
-		if (boot_cpu_data.x86 >= 0x10 && !dct_ganging_enabled(pvt)) {
-			reg = F10_DCSM1 + (cs * 4);
-			if (!amd64_read_pci_cfg(pvt->F2, reg,
-						&pvt->dcsm1[cs]))
-				debugf0("    DCSM1[%d]=0x%08x reg: F2x%x\n",
-					cs, pvt->dcsm1[cs], reg);
-		} else {
-			pvt->dcsm1[cs] = 0;
-		}
+				cs, *mask0, reg0);
+
+		if (boot_cpu_data.x86 == 0xf || dct_ganging_enabled(pvt))
+			continue;
+
+		if (!amd64_read_dct_pci_cfg(pvt, reg1, mask1))
+			debugf0("    DCSM1[%d]=0x%08x reg: F2x%x\n",
+				cs, *mask1, reg1);
 	}
 }
 
@@ -972,7 +895,10 @@ static enum mem_type amd64_determine_memory_type(struct amd64_pvt *pvt, int cs)
 {
 	enum mem_type type;
 
-	if (boot_cpu_data.x86 >= 0x10 || pvt->ext_model >= K8_REV_F) {
+	/* F15h supports only DDR3 */
+	if (boot_cpu_data.x86 >= 0x15)
+		type = (pvt->dclr0 & BIT(16)) ?	MEM_DDR3 : MEM_RDDR3;
+	else if (boot_cpu_data.x86 == 0x10 || pvt->ext_model >= K8_REV_F) {
 		if (pvt->dchr0 & DDR3_MODE)
 			type = (pvt->dclr0 & BIT(16)) ?	MEM_DDR3 : MEM_RDDR3;
 		else
@@ -986,26 +912,14 @@ static enum mem_type amd64_determine_memory_type(struct amd64_pvt *pvt, int cs)
 	return type;
 }
 
-/*
- * Read the DRAM Configuration Low register. It differs between CG, D & E revs
- * and the later RevF memory controllers (DDR vs DDR2)
- *
- * Return:
- *      number of memory channels in operation
- * Pass back:
- *      contents of the DCL0_LOW register
- */
+/* Get the number of DCT channels the memory controller is using. */
 static int k8_early_channel_count(struct amd64_pvt *pvt)
 {
-	int flag, err = 0;
-
-	err = amd64_read_pci_cfg(pvt->F2, F10_DCLR_0, &pvt->dclr0);
-	if (err)
-		return err;
+	int flag;
 
 	if (pvt->ext_model >= K8_REV_F)
 		/* RevF (NPT) and later */
-		flag = pvt->dclr0 & F10_WIDTH_128;
+		flag = pvt->dclr0 & WIDTH_128;
 	else
 		/* RevE and earlier */
 		flag = pvt->dclr0 & REVE_WIDTH_128;
@@ -1016,55 +930,47 @@ static int k8_early_channel_count(struct amd64_pvt *pvt)
 	return (flag) ? 2 : 1;
 }
 
-/* extract the ERROR ADDRESS for the K8 CPUs */
-static u64 k8_get_error_address(struct mem_ctl_info *mci,
-				struct err_regs *info)
+/* On F10h and later ErrAddr is MC4_ADDR[47:1] */
+static u64 get_error_address(struct mce *m)
 {
-	return (((u64) (info->nbeah & 0xff)) << 32) +
-			(info->nbeal & ~0x03);
+	u8 start_bit = 1;
+	u8 end_bit   = 47;
+
+	if (boot_cpu_data.x86 == 0xf) {
+		start_bit = 3;
+		end_bit   = 39;
+	}
+
+	return m->addr & GENMASK(start_bit, end_bit);
 }
 
-/*
- * Read the Base and Limit registers for K8 based Memory controllers; extract
- * fields from the 'raw' reg into separate data fields
- *
- * Isolates: BASE, LIMIT, IntlvEn, IntlvSel, RW_EN
- */
-static void k8_read_dram_base_limit(struct amd64_pvt *pvt, int dram)
+static void read_dram_base_limit_regs(struct amd64_pvt *pvt, unsigned range)
 {
-	u32 low;
-	u32 off = dram << 3;	/* 8 bytes between DRAM entries */
+	int off = range << 3;
 
-	amd64_read_pci_cfg(pvt->F1, K8_DRAM_BASE_LOW + off, &low);
+	amd64_read_pci_cfg(pvt->F1, DRAM_BASE_LO + off,  &pvt->ranges[range].base.lo);
+	amd64_read_pci_cfg(pvt->F1, DRAM_LIMIT_LO + off, &pvt->ranges[range].lim.lo);
 
-	/* Extract parts into separate data entries */
-	pvt->dram_base[dram] = ((u64) low & 0xFFFF0000) << 8;
-	pvt->dram_IntlvEn[dram] = (low >> 8) & 0x7;
-	pvt->dram_rw_en[dram] = (low & 0x3);
+	if (boot_cpu_data.x86 == 0xf)
+		return;
 
-	amd64_read_pci_cfg(pvt->F1, K8_DRAM_LIMIT_LOW + off, &low);
+	if (!dram_rw(pvt, range))
+		return;
 
-	/*
-	 * Extract parts into separate data entries. Limit is the HIGHEST memory
-	 * location of the region, so lower 24 bits need to be all ones
-	 */
-	pvt->dram_limit[dram] = (((u64) low & 0xFFFF0000) << 8) | 0x00FFFFFF;
-	pvt->dram_IntlvSel[dram] = (low >> 8) & 0x7;
-	pvt->dram_DstNode[dram] = (low & 0x7);
+	amd64_read_pci_cfg(pvt->F1, DRAM_BASE_HI + off,  &pvt->ranges[range].base.hi);
+	amd64_read_pci_cfg(pvt->F1, DRAM_LIMIT_HI + off, &pvt->ranges[range].lim.hi);
 }
 
-static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci,
-				    struct err_regs *err_info, u64 sys_addr)
+static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
+				    u16 syndrome)
 {
 	struct mem_ctl_info *src_mci;
+	struct amd64_pvt *pvt = mci->pvt_info;
 	int channel, csrow;
 	u32 page, offset;
-	u16 syndrome;
-
-	syndrome = extract_syndrome(err_info);
 
 	/* CHIPKILL enabled */
-	if (err_info->nbcfg & K8_NBCFG_CHIPKILL) {
+	if (pvt->nbcfg & NBCFG_CHIPKILL) {
 		channel = get_channel_from_ecc_syndrome(mci, syndrome);
 		if (channel < 0) {
 			/*
@@ -1113,18 +1019,41 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci,
 	}
 }
 
-static int k8_dbam_to_chip_select(struct amd64_pvt *pvt, int cs_mode)
+static int ddr2_cs_size(unsigned i, bool dct_width)
 {
-	int *dbam_map;
+	unsigned shift = 0;
 
-	if (pvt->ext_model >= K8_REV_F)
-		dbam_map = ddr2_dbam;
-	else if (pvt->ext_model >= K8_REV_D)
-		dbam_map = ddr2_dbam_revD;
+	if (i <= 2)
+		shift = i;
+	else if (!(i & 0x1))
+		shift = i >> 1;
 	else
-		dbam_map = ddr2_dbam_revCG;
+		shift = (i + 1) >> 1;
 
-	return dbam_map[cs_mode];
+	return 128 << (shift + !!dct_width);
+}
+
+static int k8_dbam_to_chip_select(struct amd64_pvt *pvt, u8 dct,
+				  unsigned cs_mode)
+{
+	u32 dclr = dct ? pvt->dclr1 : pvt->dclr0;
+
+	if (pvt->ext_model >= K8_REV_F) {
+		WARN_ON(cs_mode > 11);
+		return ddr2_cs_size(cs_mode, dclr & WIDTH_128);
+	}
+	else if (pvt->ext_model >= K8_REV_D) {
+		WARN_ON(cs_mode > 10);
+
+		if (cs_mode == 3 || cs_mode == 8)
+			return 32 << (cs_mode - 1);
+		else
+			return 32 << cs_mode;
+	}
+	else {
+		WARN_ON(cs_mode > 6);
+		return 32 << cs_mode;
+	}
 }
 
 /*
@@ -1135,17 +1064,13 @@ static int k8_dbam_to_chip_select(struct amd64_pvt *pvt, int cs_mode)
  * Pass back:
  *	contents of the DCL0_LOW register
  */
-static int f10_early_channel_count(struct amd64_pvt *pvt)
+static int f1x_early_channel_count(struct amd64_pvt *pvt)
 {
-	int dbams[] = { DBAM0, DBAM1 };
 	int i, j, channels = 0;
-	u32 dbam;
 
-	/* If we are in 128 bit mode, then we are using 2 channels */
-	if (pvt->dclr0 & F10_WIDTH_128) {
-		channels = 2;
-		return channels;
-	}
+	/* On F10h, if we are in 128 bit mode, then we are using 2 channels */
+	if (boot_cpu_data.x86 == 0x10 && (pvt->dclr0 & WIDTH_128))
+		return 2;
 
 	/*
 	 * Need to check if in unganged mode: In such, there are 2 channels,
@@ -1162,9 +1087,8 @@ static int f10_early_channel_count(struct amd64_pvt *pvt)
 	 * is more than just one DIMM present in unganged mode. Need to check
 	 * both controllers since DIMMs can be placed in either one.
 	 */
-	for (i = 0; i < ARRAY_SIZE(dbams); i++) {
-		if (amd64_read_pci_cfg(pvt->F2, dbams[i], &dbam))
-			goto err_reg;
+	for (i = 0; i < 2; i++) {
+		u32 dbam = (i ? pvt->dbam1 : pvt->dbam0);
 
 		for (j = 0; j < 4; j++) {
 			if (DBAM_DIMM(j, dbam) > 0) {
@@ -1180,216 +1104,191 @@ static int f10_early_channel_count(struct amd64_pvt *pvt)
 	amd64_info("MCT channel count: %d\n", channels);
 
 	return channels;
-
-err_reg:
-	return -1;
-
 }
 
-static int f10_dbam_to_chip_select(struct amd64_pvt *pvt, int cs_mode)
+static int ddr3_cs_size(unsigned i, bool dct_width)
 {
-	int *dbam_map;
+	unsigned shift = 0;
+	int cs_size = 0;
 
-	if (pvt->dchr0 & DDR3_MODE || pvt->dchr1 & DDR3_MODE)
-		dbam_map = ddr3_dbam;
+	if (i == 0 || i == 3 || i == 4)
+		cs_size = -1;
+	else if (i <= 2)
+		shift = i;
+	else if (i == 12)
+		shift = 7;
+	else if (!(i & 0x1))
+		shift = i >> 1;
 	else
-		dbam_map = ddr2_dbam;
+		shift = (i + 1) >> 1;
+
+	if (cs_size != -1)
+		cs_size = (128 * (1 << !!dct_width)) << shift;
 
-	return dbam_map[cs_mode];
+	return cs_size;
 }
 
-static u64 f10_get_error_address(struct mem_ctl_info *mci,
-			struct err_regs *info)
+static int f10_dbam_to_chip_select(struct amd64_pvt *pvt, u8 dct,
+				   unsigned cs_mode)
 {
-	return (((u64) (info->nbeah & 0xffff)) << 32) +
-			(info->nbeal & ~0x01);
+	u32 dclr = dct ? pvt->dclr1 : pvt->dclr0;
+
+	WARN_ON(cs_mode > 11);
+
+	if (pvt->dchr0 & DDR3_MODE || pvt->dchr1 & DDR3_MODE)
+		return ddr3_cs_size(cs_mode, dclr & WIDTH_128);
+	else
+		return ddr2_cs_size(cs_mode, dclr & WIDTH_128);
 }
 
 /*
- * Read the Base and Limit registers for F10 based Memory controllers. Extract
- * fields from the 'raw' reg into separate data fields.
- *
- * Isolates: BASE, LIMIT, IntlvEn, IntlvSel, RW_EN.
+ * F15h supports only 64bit DCT interfaces
  */
-static void f10_read_dram_base_limit(struct amd64_pvt *pvt, int dram)
+static int f15_dbam_to_chip_select(struct amd64_pvt *pvt, u8 dct,
+				   unsigned cs_mode)
 {
-	u32 high_offset, low_offset, high_base, low_base, high_limit, low_limit;
-
-	low_offset = K8_DRAM_BASE_LOW + (dram << 3);
-	high_offset = F10_DRAM_BASE_HIGH + (dram << 3);
-
-	/* read the 'raw' DRAM BASE Address register */
-	amd64_read_pci_cfg(pvt->F1, low_offset, &low_base);
-	amd64_read_pci_cfg(pvt->F1, high_offset, &high_base);
-
-	/* Extract parts into separate data entries */
-	pvt->dram_rw_en[dram] = (low_base & 0x3);
-
-	if (pvt->dram_rw_en[dram] == 0)
-		return;
-
-	pvt->dram_IntlvEn[dram] = (low_base >> 8) & 0x7;
-
-	pvt->dram_base[dram] = (((u64)high_base & 0x000000FF) << 40) |
-			       (((u64)low_base  & 0xFFFF0000) << 8);
-
-	low_offset = K8_DRAM_LIMIT_LOW + (dram << 3);
-	high_offset = F10_DRAM_LIMIT_HIGH + (dram << 3);
-
-	/* read the 'raw' LIMIT registers */
-	amd64_read_pci_cfg(pvt->F1, low_offset, &low_limit);
-	amd64_read_pci_cfg(pvt->F1, high_offset, &high_limit);
-
-	pvt->dram_DstNode[dram] = (low_limit & 0x7);
-	pvt->dram_IntlvSel[dram] = (low_limit >> 8) & 0x7;
+	WARN_ON(cs_mode > 12);
 
-	/*
-	 * Extract address values and form a LIMIT address. Limit is the HIGHEST
-	 * memory location of the region, so low 24 bits need to be all ones.
-	 */
-	pvt->dram_limit[dram] = (((u64)high_limit & 0x000000FF) << 40) |
-				(((u64) low_limit & 0xFFFF0000) << 8) |
-				0x00FFFFFF;
+	return ddr3_cs_size(cs_mode, false);
 }
 
-static void f10_read_dram_ctl_register(struct amd64_pvt *pvt)
+static void read_dram_ctl_register(struct amd64_pvt *pvt)
 {
 
-	if (!amd64_read_pci_cfg(pvt->F2, F10_DCTL_SEL_LOW,
-				&pvt->dram_ctl_select_low)) {
-		debugf0("F2x110 (DCTL Sel. Low): 0x%08x, "
-			"High range addresses at: 0x%x\n",
-			pvt->dram_ctl_select_low,
-			dct_sel_baseaddr(pvt));
+	if (boot_cpu_data.x86 == 0xf)
+		return;
+
+	if (!amd64_read_dct_pci_cfg(pvt, DCT_SEL_LO, &pvt->dct_sel_lo)) {
+		debugf0("F2x110 (DCTSelLow): 0x%08x, High range addrs at: 0x%x\n",
+			pvt->dct_sel_lo, dct_sel_baseaddr(pvt));
 
-		debugf0("  DCT mode: %s, All DCTs on: %s\n",
-			(dct_ganging_enabled(pvt) ? "ganged" : "unganged"),
-			(dct_dram_enabled(pvt) ? "yes"   : "no"));
+		debugf0("  DCTs operate in %s mode.\n",
+			(dct_ganging_enabled(pvt) ? "ganged" : "unganged"));
 
 		if (!dct_ganging_enabled(pvt))
 			debugf0("  Address range split per DCT: %s\n",
 				(dct_high_range_enabled(pvt) ? "yes" : "no"));
 
-		debugf0("  DCT data interleave for ECC: %s, "
+		debugf0("  data interleave for ECC: %s, "
 			"DRAM cleared since last warm reset: %s\n",
 			(dct_data_intlv_enabled(pvt) ? "enabled" : "disabled"),
 			(dct_memory_cleared(pvt) ? "yes" : "no"));
 
-		debugf0("  DCT channel interleave: %s, "
-			"DCT interleave bits selector: 0x%x\n",
+		debugf0("  channel interleave: %s, "
+			"interleave bits selector: 0x%x\n",
 			(dct_interleave_enabled(pvt) ? "enabled" : "disabled"),
 			dct_sel_interleave_addr(pvt));
 	}
 
-	amd64_read_pci_cfg(pvt->F2, F10_DCTL_SEL_HIGH,
-			   &pvt->dram_ctl_select_high);
+	amd64_read_dct_pci_cfg(pvt, DCT_SEL_HI, &pvt->dct_sel_hi);
 }
 
 /*
- * determine channel based on the interleaving mode: F10h BKDG, 2.8.9 Memory
+ * Determine channel (DCT) based on the interleaving mode: F10h BKDG, 2.8.9 Memory
  * Interleaving Modes.
  */
-static u32 f10_determine_channel(struct amd64_pvt *pvt, u64 sys_addr,
-				int hi_range_sel, u32 intlv_en)
+static u8 f1x_determine_channel(struct amd64_pvt *pvt, u64 sys_addr,
+				bool hi_range_sel, u8 intlv_en)
 {
-	u32 cs, temp, dct_sel_high = (pvt->dram_ctl_select_low >> 1) & 1;
+	u8 dct_sel_high = (pvt->dct_sel_lo >> 1) & 1;
 
 	if (dct_ganging_enabled(pvt))
-		cs = 0;
-	else if (hi_range_sel)
-		cs = dct_sel_high;
-	else if (dct_interleave_enabled(pvt)) {
-		/*
-		 * see F2x110[DctSelIntLvAddr] - channel interleave mode
-		 */
-		if (dct_sel_interleave_addr(pvt) == 0)
-			cs = sys_addr >> 6 & 1;
-		else if ((dct_sel_interleave_addr(pvt) >> 1) & 1) {
-			temp = hweight_long((u32) ((sys_addr >> 16) & 0x1F)) % 2;
+		return 0;
 
-			if (dct_sel_interleave_addr(pvt) & 1)
-				cs = (sys_addr >> 9 & 1) ^ temp;
-			else
-				cs = (sys_addr >> 6 & 1) ^ temp;
-		} else if (intlv_en & 4)
-			cs = sys_addr >> 15 & 1;
-		else if (intlv_en & 2)
-			cs = sys_addr >> 14 & 1;
-		else if (intlv_en & 1)
-			cs = sys_addr >> 13 & 1;
-		else
-			cs = sys_addr >> 12 & 1;
-	} else if (dct_high_range_enabled(pvt) && !dct_ganging_enabled(pvt))
-		cs = ~dct_sel_high & 1;
-	else
-		cs = 0;
+	if (hi_range_sel)
+		return dct_sel_high;
 
-	return cs;
-}
+	/*
+	 * see F2x110[DctSelIntLvAddr] - channel interleave mode
+	 */
+	if (dct_interleave_enabled(pvt)) {
+		u8 intlv_addr = dct_sel_interleave_addr(pvt);
 
-static inline u32 f10_map_intlv_en_to_shift(u32 intlv_en)
-{
-	if (intlv_en == 1)
-		return 1;
-	else if (intlv_en == 3)
-		return 2;
-	else if (intlv_en == 7)
-		return 3;
+		/* return DCT select function: 0=DCT0, 1=DCT1 */
+		if (!intlv_addr)
+			return sys_addr >> 6 & 1;
+
+		if (intlv_addr & 0x2) {
+			u8 shift = intlv_addr & 0x1 ? 9 : 6;
+			u32 temp = hweight_long((u32) ((sys_addr >> 16) & 0x1F)) % 2;
+
+			return ((sys_addr >> shift) & 1) ^ temp;
+		}
+
+		return (sys_addr >> (12 + hweight8(intlv_en))) & 1;
+	}
+
+	if (dct_high_range_enabled(pvt))
+		return ~dct_sel_high & 1;
 
 	return 0;
 }
 
-/* See F10h BKDG, 2.8.10.2 DctSelBaseOffset Programming */
-static inline u64 f10_get_base_addr_offset(u64 sys_addr, int hi_range_sel,
-						 u32 dct_sel_base_addr,
-						 u64 dct_sel_base_off,
-						 u32 hole_valid, u32 hole_off,
-						 u64 dram_base)
+/* Convert the sys_addr to the normalized DCT address */
+static u64 f1x_get_norm_dct_addr(struct amd64_pvt *pvt, unsigned range,
+				 u64 sys_addr, bool hi_rng,
+				 u32 dct_sel_base_addr)
 {
 	u64 chan_off;
+	u64 dram_base		= get_dram_base(pvt, range);
+	u64 hole_off		= f10_dhar_offset(pvt);
+	u64 dct_sel_base_off	= (pvt->dct_sel_hi & 0xFFFFFC00) << 16;
 
-	if (hi_range_sel) {
-		if (!(dct_sel_base_addr & 0xFFFF0000) &&
-		   hole_valid && (sys_addr >= 0x100000000ULL))
-			chan_off = hole_off << 16;
+	if (hi_rng) {
+		/*
+		 * if
+		 * base address of high range is below 4Gb
+		 * (bits [47:27] at [31:11])
+		 * DRAM address space on this DCT is hoisted above 4Gb	&&
+		 * sys_addr > 4Gb
+		 *
+		 *	remove hole offset from sys_addr
+		 * else
+		 *	remove high range offset from sys_addr
+		 */
+		if ((!(dct_sel_base_addr >> 16) ||
+		     dct_sel_base_addr < dhar_base(pvt)) &&
+		    dhar_valid(pvt) &&
+		    (sys_addr >= BIT_64(32)))
+			chan_off = hole_off;
 		else
 			chan_off = dct_sel_base_off;
 	} else {
-		if (hole_valid && (sys_addr >= 0x100000000ULL))
-			chan_off = hole_off << 16;
+		/*
+		 * if
+		 * we have a valid hole		&&
+		 * sys_addr > 4Gb
+		 *
+		 *	remove hole
+		 * else
+		 *	remove dram base to normalize to DCT address
+		 */
+		if (dhar_valid(pvt) && (sys_addr >= BIT_64(32)))
+			chan_off = hole_off;
 		else
-			chan_off = dram_base & 0xFFFFF8000000ULL;
+			chan_off = dram_base;
 	}
 
-	return (sys_addr & 0x0000FFFFFFFFFFC0ULL) -
-			(chan_off & 0x0000FFFFFF800000ULL);
+	return (sys_addr & GENMASK(6,47)) - (chan_off & GENMASK(23,47));
 }
 
-/* Hack for the time being - Can we get this from BIOS?? */
-#define	CH0SPARE_RANK	0
-#define	CH1SPARE_RANK	1
-
 /*
  * checks if the csrow passed in is marked as SPARED, if so returns the new
  * spare row
  */
-static inline int f10_process_possible_spare(int csrow,
-				u32 cs, struct amd64_pvt *pvt)
-{
-	u32 swap_done;
-	u32 bad_dram_cs;
-
-	/* Depending on channel, isolate respective SPARING info */
-	if (cs) {
-		swap_done = F10_ONLINE_SPARE_SWAPDONE1(pvt->online_spare);
-		bad_dram_cs = F10_ONLINE_SPARE_BADDRAM_CS1(pvt->online_spare);
-		if (swap_done && (csrow == bad_dram_cs))
-			csrow = CH1SPARE_RANK;
-	} else {
-		swap_done = F10_ONLINE_SPARE_SWAPDONE0(pvt->online_spare);
-		bad_dram_cs = F10_ONLINE_SPARE_BADDRAM_CS0(pvt->online_spare);
-		if (swap_done && (csrow == bad_dram_cs))
-			csrow = CH0SPARE_RANK;
+static int f10_process_possible_spare(struct amd64_pvt *pvt, u8 dct, int csrow)
+{
+	int tmp_cs;
+
+	if (online_spare_swap_done(pvt, dct) &&
+	    csrow == online_spare_bad_dramcs(pvt, dct)) {
+
+		for_each_chip_select(tmp_cs, dct, pvt) {
+			if (chip_select_base(tmp_cs, dct, pvt) & 0x2) {
+				csrow = tmp_cs;
+				break;
+			}
+		}
 	}
 	return csrow;
 }
@@ -1402,11 +1301,11 @@ static inline int f10_process_possible_spare(int csrow,
  *	-EINVAL:  NOT FOUND
  *	0..csrow = Chip-Select Row
  */
-static int f10_lookup_addr_in_dct(u32 in_addr, u32 nid, u32 cs)
+static int f1x_lookup_addr_in_dct(u64 in_addr, u32 nid, u8 dct)
 {
 	struct mem_ctl_info *mci;
 	struct amd64_pvt *pvt;
-	u32 cs_base, cs_mask;
+	u64 cs_base, cs_mask;
 	int cs_found = -EINVAL;
 	int csrow;
 
@@ -1416,39 +1315,25 @@ static int f10_lookup_addr_in_dct(u32 in_addr, u32 nid, u32 cs)
 
 	pvt = mci->pvt_info;
 
-	debugf1("InputAddr=0x%x  channelselect=%d\n", in_addr, cs);
-
-	for (csrow = 0; csrow < pvt->cs_count; csrow++) {
+	debugf1("input addr: 0x%llx, DCT: %d\n", in_addr, dct);
 
-		cs_base = amd64_get_dct_base(pvt, cs, csrow);
-		if (!(cs_base & K8_DCSB_CS_ENABLE))
+	for_each_chip_select(csrow, dct, pvt) {
+		if (!csrow_enabled(csrow, dct, pvt))
 			continue;
 
-		/*
-		 * We have an ENABLED CSROW, Isolate just the MASK bits of the
-		 * target: [28:19] and [13:5], which map to [36:27] and [21:13]
-		 * of the actual address.
-		 */
-		cs_base &= REV_F_F1Xh_DCSB_BASE_BITS;
-
-		/*
-		 * Get the DCT Mask, and ENABLE the reserved bits: [18:16] and
-		 * [4:0] to become ON. Then mask off bits [28:0] ([36:8])
-		 */
-		cs_mask = amd64_get_dct_mask(pvt, cs, csrow);
+		get_cs_base_and_mask(pvt, csrow, dct, &cs_base, &cs_mask);
 
-		debugf1("    CSROW=%d CSBase=0x%x RAW CSMask=0x%x\n",
-				csrow, cs_base, cs_mask);
+		debugf1("    CSROW=%d CSBase=0x%llx CSMask=0x%llx\n",
+			csrow, cs_base, cs_mask);
 
-		cs_mask = (cs_mask | 0x0007C01F) & 0x1FFFFFFF;
+		cs_mask = ~cs_mask;
 
-		debugf1("              Final CSMask=0x%x\n", cs_mask);
-		debugf1("    (InputAddr & ~CSMask)=0x%x "
-				"(CSBase & ~CSMask)=0x%x\n",
-				(in_addr & ~cs_mask), (cs_base & ~cs_mask));
+		debugf1("    (InputAddr & ~CSMask)=0x%llx "
+			"(CSBase & ~CSMask)=0x%llx\n",
+			(in_addr & cs_mask), (cs_base & cs_mask));
 
-		if ((in_addr & ~cs_mask) == (cs_base & ~cs_mask)) {
-			cs_found = f10_process_possible_spare(csrow, cs, pvt);
+		if ((in_addr & cs_mask) == (cs_base & cs_mask)) {
+			cs_found = f10_process_possible_spare(pvt, dct, csrow);
 
 			debugf1(" MATCH csrow=%d\n", cs_found);
 			break;
@@ -1457,38 +1342,75 @@ static int f10_lookup_addr_in_dct(u32 in_addr, u32 nid, u32 cs)
 	return cs_found;
 }
 
-/* For a given @dram_range, check if @sys_addr falls within it. */
-static int f10_match_to_this_node(struct amd64_pvt *pvt, int dram_range,
-				  u64 sys_addr, int *nid, int *chan_sel)
+/*
+ * See F2x10C. Non-interleaved graphics framebuffer memory under the 16G is
+ * swapped with a region located at the bottom of memory so that the GPU can use
+ * the interleaved region and thus two channels.
+ */
+static u64 f1x_swap_interleaved_region(struct amd64_pvt *pvt, u64 sys_addr)
 {
-	int node_id, cs_found = -EINVAL, high_range = 0;
-	u32 intlv_en, intlv_sel, intlv_shift, hole_off;
-	u32 hole_valid, tmp, dct_sel_base, channel;
-	u64 dram_base, chan_addr, dct_sel_base_off;
+	u32 swap_reg, swap_base, swap_limit, rgn_size, tmp_addr;
 
-	dram_base = pvt->dram_base[dram_range];
-	intlv_en = pvt->dram_IntlvEn[dram_range];
+	if (boot_cpu_data.x86 == 0x10) {
+		/* only revC3 and revE have that feature */
+		if (boot_cpu_data.x86_model < 4 ||
+		    (boot_cpu_data.x86_model < 0xa &&
+		     boot_cpu_data.x86_mask < 3))
+			return sys_addr;
+	}
 
-	node_id = pvt->dram_DstNode[dram_range];
-	intlv_sel = pvt->dram_IntlvSel[dram_range];
+	amd64_read_dct_pci_cfg(pvt, SWAP_INTLV_REG, &swap_reg);
 
-	debugf1("(dram=%d) Base=0x%llx SystemAddr= 0x%llx Limit=0x%llx\n",
-		dram_range, dram_base, sys_addr, pvt->dram_limit[dram_range]);
+	if (!(swap_reg & 0x1))
+		return sys_addr;
 
-	/*
-	 * This assumes that one node's DHAR is the same as all the other
-	 * nodes' DHAR.
-	 */
-	hole_off = (pvt->dhar & 0x0000FF80);
-	hole_valid = (pvt->dhar & 0x1);
-	dct_sel_base_off = (pvt->dram_ctl_select_high & 0xFFFFFC00) << 16;
+	swap_base	= (swap_reg >> 3) & 0x7f;
+	swap_limit	= (swap_reg >> 11) & 0x7f;
+	rgn_size	= (swap_reg >> 20) & 0x7f;
+	tmp_addr	= sys_addr >> 27;
 
-	debugf1("   HoleOffset=0x%x  HoleValid=0x%x IntlvSel=0x%x\n",
-			hole_off, hole_valid, intlv_sel);
+	if (!(sys_addr >> 34) &&
+	    (((tmp_addr >= swap_base) &&
+	     (tmp_addr <= swap_limit)) ||
+	     (tmp_addr < rgn_size)))
+		return sys_addr ^ (u64)swap_base << 27;
+
+	return sys_addr;
+}
+
+/* For a given @dram_range, check if @sys_addr falls within it. */
+static int f1x_match_to_this_node(struct amd64_pvt *pvt, unsigned range,
+				  u64 sys_addr, int *nid, int *chan_sel)
+{
+	int cs_found = -EINVAL;
+	u64 chan_addr;
+	u32 dct_sel_base;
+	u8 channel;
+	bool high_range = false;
+
+	u8 node_id    = dram_dst_node(pvt, range);
+	u8 intlv_en   = dram_intlv_en(pvt, range);
+	u32 intlv_sel = dram_intlv_sel(pvt, range);
+
+	debugf1("(range %d) SystemAddr= 0x%llx Limit=0x%llx\n",
+		range, sys_addr, get_dram_limit(pvt, range));
+
+	if (dhar_valid(pvt) &&
+	    dhar_base(pvt) <= sys_addr &&
+	    sys_addr < BIT_64(32)) {
+		amd64_warn("Huh? Address is in the MMIO hole: 0x%016llx\n",
+			    sys_addr);
+		return -EINVAL;
+	}
 
 	if (intlv_en &&
-	    (intlv_sel != ((sys_addr >> 12) & intlv_en)))
+	    (intlv_sel != ((sys_addr >> 12) & intlv_en))) {
+		amd64_warn("Botched intlv bits, en: 0x%x, sel: 0x%x\n",
+			   intlv_en, intlv_sel);
 		return -EINVAL;
+	}
+
+	sys_addr = f1x_swap_interleaved_region(pvt, sys_addr);
 
 	dct_sel_base = dct_sel_baseaddr(pvt);
 
@@ -1499,38 +1421,41 @@ static int f10_match_to_this_node(struct amd64_pvt *pvt, int dram_range,
 	if (dct_high_range_enabled(pvt) &&
 	   !dct_ganging_enabled(pvt) &&
 	   ((sys_addr >> 27) >= (dct_sel_base >> 11)))
-		high_range = 1;
-
-	channel = f10_determine_channel(pvt, sys_addr, high_range, intlv_en);
+		high_range = true;
 
-	chan_addr = f10_get_base_addr_offset(sys_addr, high_range, dct_sel_base,
-					     dct_sel_base_off, hole_valid,
-					     hole_off, dram_base);
+	channel = f1x_determine_channel(pvt, sys_addr, high_range, intlv_en);
 
-	intlv_shift = f10_map_intlv_en_to_shift(intlv_en);
+	chan_addr = f1x_get_norm_dct_addr(pvt, range, sys_addr,
+					  high_range, dct_sel_base);
 
-	/* remove Node ID (in case of memory interleaving) */
-	tmp = chan_addr & 0xFC0;
+	/* Remove node interleaving, see F1x120 */
+	if (intlv_en)
+		chan_addr = ((chan_addr >> (12 + hweight8(intlv_en))) << 12) |
+			    (chan_addr & 0xfff);
 
-	chan_addr = ((chan_addr >> intlv_shift) & 0xFFFFFFFFF000ULL) | tmp;
-
-	/* remove channel interleave and hash */
+	/* remove channel interleave */
 	if (dct_interleave_enabled(pvt) &&
 	   !dct_high_range_enabled(pvt) &&
 	   !dct_ganging_enabled(pvt)) {
-		if (dct_sel_interleave_addr(pvt) != 1)
-			chan_addr = (chan_addr >> 1) & 0xFFFFFFFFFFFFFFC0ULL;
-		else {
-			tmp = chan_addr & 0xFC0;
-			chan_addr = ((chan_addr & 0xFFFFFFFFFFFFC000ULL) >> 1)
-					| tmp;
-		}
+
+		if (dct_sel_interleave_addr(pvt) != 1) {
+			if (dct_sel_interleave_addr(pvt) == 0x3)
+				/* hash 9 */
+				chan_addr = ((chan_addr >> 10) << 9) |
+					     (chan_addr & 0x1ff);
+			else
+				/* A[6] or hash 6 */
+				chan_addr = ((chan_addr >> 7) << 6) |
+					     (chan_addr & 0x3f);
+		} else
+			/* A[12] */
+			chan_addr = ((chan_addr >> 13) << 12) |
+				     (chan_addr & 0xfff);
 	}
 
-	debugf1("   (ChannelAddrLong=0x%llx) >> 8 becomes InputAddr=0x%x\n",
-		chan_addr, (u32)(chan_addr >> 8));
+	debugf1("   Normalized DCT addr: 0x%llx\n", chan_addr);
 
-	cs_found = f10_lookup_addr_in_dct(chan_addr >> 8, node_id, channel);
+	cs_found = f1x_lookup_addr_in_dct(chan_addr, node_id, channel);
 
 	if (cs_found >= 0) {
 		*nid = node_id;
@@ -1539,23 +1464,21 @@ static int f10_match_to_this_node(struct amd64_pvt *pvt, int dram_range,
 	return cs_found;
 }
 
-static int f10_translate_sysaddr_to_cs(struct amd64_pvt *pvt, u64 sys_addr,
+static int f1x_translate_sysaddr_to_cs(struct amd64_pvt *pvt, u64 sys_addr,
 				       int *node, int *chan_sel)
 {
-	int dram_range, cs_found = -EINVAL;
-	u64 dram_base, dram_limit;
+	int cs_found = -EINVAL;
+	unsigned range;
 
-	for (dram_range = 0; dram_range < DRAM_REG_COUNT; dram_range++) {
+	for (range = 0; range < DRAM_RANGES; range++) {
 
-		if (!pvt->dram_rw_en[dram_range])
+		if (!dram_rw(pvt, range))
 			continue;
 
-		dram_base = pvt->dram_base[dram_range];
-		dram_limit = pvt->dram_limit[dram_range];
+		if ((get_dram_base(pvt, range)  <= sys_addr) &&
+		    (get_dram_limit(pvt, range) >= sys_addr)) {
 
-		if ((dram_base <= sys_addr) && (sys_addr <= dram_limit)) {
-
-			cs_found = f10_match_to_this_node(pvt, dram_range,
+			cs_found = f1x_match_to_this_node(pvt, range,
 							  sys_addr, node,
 							  chan_sel);
 			if (cs_found >= 0)
@@ -1572,16 +1495,14 @@ static int f10_translate_sysaddr_to_cs(struct amd64_pvt *pvt, u64 sys_addr,
  * The @sys_addr is usually an error address received from the hardware
  * (MCX_ADDR).
  */
-static void f10_map_sysaddr_to_csrow(struct mem_ctl_info *mci,
-				     struct err_regs *err_info,
-				     u64 sys_addr)
+static void f1x_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
+				     u16 syndrome)
 {
 	struct amd64_pvt *pvt = mci->pvt_info;
 	u32 page, offset;
 	int nid, csrow, chan = 0;
-	u16 syndrome;
 
-	csrow = f10_translate_sysaddr_to_cs(pvt, sys_addr, &nid, &chan);
+	csrow = f1x_translate_sysaddr_to_cs(pvt, sys_addr, &nid, &chan);
 
 	if (csrow < 0) {
 		edac_mc_handle_ce_no_info(mci, EDAC_MOD_STR);
@@ -1590,14 +1511,12 @@ static void f10_map_sysaddr_to_csrow(struct mem_ctl_info *mci,
 
 	error_address_to_page_and_offset(sys_addr, &page, &offset);
 
-	syndrome = extract_syndrome(err_info);
-
 	/*
 	 * We need the syndromes for channel detection only when we're
 	 * ganged. Otherwise @chan should already contain the channel at
 	 * this point.
 	 */
-	if (dct_ganging_enabled(pvt) && (pvt->nbcfg & K8_NBCFG_CHIPKILL))
+	if (dct_ganging_enabled(pvt))
 		chan = get_channel_from_ecc_syndrome(mci, syndrome);
 
 	if (chan >= 0)
@@ -1614,16 +1533,16 @@ static void f10_map_sysaddr_to_csrow(struct mem_ctl_info *mci,
 
 /*
  * debug routine to display the memory sizes of all logical DIMMs and its
- * CSROWs as well
+ * CSROWs
  */
-static void amd64_debug_display_dimm_sizes(int ctrl, struct amd64_pvt *pvt)
+static void amd64_debug_display_dimm_sizes(struct amd64_pvt *pvt, u8 ctrl)
 {
 	int dimm, size0, size1, factor = 0;
-	u32 dbam;
-	u32 *dcsb;
+	u32 *dcsb = ctrl ? pvt->csels[1].csbases : pvt->csels[0].csbases;
+	u32 dbam  = ctrl ? pvt->dbam1 : pvt->dbam0;
 
 	if (boot_cpu_data.x86 == 0xf) {
-		if (pvt->dclr0 & F10_WIDTH_128)
+		if (pvt->dclr0 & WIDTH_128)
 			factor = 1;
 
 		/* K8 families < revF not supported yet */
@@ -1634,7 +1553,8 @@ static void amd64_debug_display_dimm_sizes(int ctrl, struct amd64_pvt *pvt)
 	}
 
 	dbam = (ctrl && !dct_ganging_enabled(pvt)) ? pvt->dbam1 : pvt->dbam0;
-	dcsb = (ctrl && !dct_ganging_enabled(pvt)) ? pvt->dcsb1 : pvt->dcsb0;
+	dcsb = (ctrl && !dct_ganging_enabled(pvt)) ? pvt->csels[1].csbases
+						   : pvt->csels[0].csbases;
 
 	debugf1("F2x%d80 (DRAM Bank Address Mapping): 0x%08x\n", ctrl, dbam);
 
@@ -1644,12 +1564,14 @@ static void amd64_debug_display_dimm_sizes(int ctrl, struct amd64_pvt *pvt)
 	for (dimm = 0; dimm < 4; dimm++) {
 
 		size0 = 0;
-		if (dcsb[dimm*2] & K8_DCSB_CS_ENABLE)
-			size0 = pvt->ops->dbam_to_cs(pvt, DBAM_DIMM(dimm, dbam));
+		if (dcsb[dimm*2] & DCSB_CS_ENABLE)
+			size0 = pvt->ops->dbam_to_cs(pvt, ctrl,
+						     DBAM_DIMM(dimm, dbam));
 
 		size1 = 0;
-		if (dcsb[dimm*2 + 1] & K8_DCSB_CS_ENABLE)
-			size1 = pvt->ops->dbam_to_cs(pvt, DBAM_DIMM(dimm, dbam));
+		if (dcsb[dimm*2 + 1] & DCSB_CS_ENABLE)
+			size1 = pvt->ops->dbam_to_cs(pvt, ctrl,
+						     DBAM_DIMM(dimm, dbam));
 
 		amd64_info(EDAC_MC ": %d: %5dMB %d: %5dMB\n",
 				dimm * 2,     size0 << factor,
@@ -1664,10 +1586,9 @@ static struct amd64_family_type amd64_family_types[] = {
 		.f3_id = PCI_DEVICE_ID_AMD_K8_NB_MISC,
 		.ops = {
 			.early_channel_count	= k8_early_channel_count,
-			.get_error_address	= k8_get_error_address,
-			.read_dram_base_limit	= k8_read_dram_base_limit,
 			.map_sysaddr_to_csrow	= k8_map_sysaddr_to_csrow,
 			.dbam_to_cs		= k8_dbam_to_chip_select,
+			.read_dct_pci_cfg	= k8_read_dct_pci_cfg,
 		}
 	},
 	[F10_CPUS] = {
@@ -1675,12 +1596,21 @@ static struct amd64_family_type amd64_family_types[] = {
 		.f1_id = PCI_DEVICE_ID_AMD_10H_NB_MAP,
 		.f3_id = PCI_DEVICE_ID_AMD_10H_NB_MISC,
 		.ops = {
-			.early_channel_count	= f10_early_channel_count,
-			.get_error_address	= f10_get_error_address,
-			.read_dram_base_limit	= f10_read_dram_base_limit,
-			.read_dram_ctl_register	= f10_read_dram_ctl_register,
-			.map_sysaddr_to_csrow	= f10_map_sysaddr_to_csrow,
+			.early_channel_count	= f1x_early_channel_count,
+			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
 			.dbam_to_cs		= f10_dbam_to_chip_select,
+			.read_dct_pci_cfg	= f10_read_dct_pci_cfg,
+		}
+	},
+	[F15_CPUS] = {
+		.ctl_name = "F15h",
+		.f1_id = PCI_DEVICE_ID_AMD_15H_NB_F1,
+		.f3_id = PCI_DEVICE_ID_AMD_15H_NB_F3,
+		.ops = {
+			.early_channel_count	= f1x_early_channel_count,
+			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
+			.dbam_to_cs		= f15_dbam_to_chip_select,
+			.read_dct_pci_cfg	= f15_read_dct_pci_cfg,
 		}
 	},
 };
@@ -1770,15 +1700,15 @@ static u16 x8_vectors[] = {
 	0x0100, 0x0200, 0x0400, 0x0800, 0x1000, 0x2000, 0x4000, 0x8000,
 };
 
-static int decode_syndrome(u16 syndrome, u16 *vectors, int num_vecs,
-			   int v_dim)
+static int decode_syndrome(u16 syndrome, u16 *vectors, unsigned num_vecs,
+			   unsigned v_dim)
 {
 	unsigned int i, err_sym;
 
 	for (err_sym = 0; err_sym < num_vecs / v_dim; err_sym++) {
 		u16 s = syndrome;
-		int v_idx =  err_sym * v_dim;
-		int v_end = (err_sym + 1) * v_dim;
+		unsigned v_idx =  err_sym * v_dim;
+		unsigned v_end = (err_sym + 1) * v_dim;
 
 		/* walk over all 16 bits of the syndrome */
 		for (i = 1; i < (1U << 16); i <<= 1) {
@@ -1850,51 +1780,50 @@ static int get_channel_from_ecc_syndrome(struct mem_ctl_info *mci, u16 syndrome)
 	struct amd64_pvt *pvt = mci->pvt_info;
 	int err_sym = -1;
 
-	if (pvt->syn_type == 8)
+	if (pvt->ecc_sym_sz == 8)
 		err_sym = decode_syndrome(syndrome, x8_vectors,
 					  ARRAY_SIZE(x8_vectors),
-					  pvt->syn_type);
-	else if (pvt->syn_type == 4)
+					  pvt->ecc_sym_sz);
+	else if (pvt->ecc_sym_sz == 4)
 		err_sym = decode_syndrome(syndrome, x4_vectors,
 					  ARRAY_SIZE(x4_vectors),
-					  pvt->syn_type);
+					  pvt->ecc_sym_sz);
 	else {
-		amd64_warn("Illegal syndrome type: %u\n", pvt->syn_type);
+		amd64_warn("Illegal syndrome type: %u\n", pvt->ecc_sym_sz);
 		return err_sym;
 	}
 
-	return map_err_sym_to_channel(err_sym, pvt->syn_type);
+	return map_err_sym_to_channel(err_sym, pvt->ecc_sym_sz);
 }
 
 /*
  * Handle any Correctable Errors (CEs) that have occurred. Check for valid ERROR
  * ADDRESS and process.
  */
-static void amd64_handle_ce(struct mem_ctl_info *mci,
-			    struct err_regs *info)
+static void amd64_handle_ce(struct mem_ctl_info *mci, struct mce *m)
 {
 	struct amd64_pvt *pvt = mci->pvt_info;
 	u64 sys_addr;
+	u16 syndrome;
 
 	/* Ensure that the Error Address is VALID */
-	if (!(info->nbsh & K8_NBSH_VALID_ERROR_ADDR)) {
+	if (!(m->status & MCI_STATUS_ADDRV)) {
 		amd64_mc_err(mci, "HW has no ERROR_ADDRESS available\n");
 		edac_mc_handle_ce_no_info(mci, EDAC_MOD_STR);
 		return;
 	}
 
-	sys_addr = pvt->ops->get_error_address(mci, info);
+	sys_addr = get_error_address(m);
+	syndrome = extract_syndrome(m->status);
 
 	amd64_mc_err(mci, "CE ERROR_ADDRESS= 0x%llx\n", sys_addr);
 
-	pvt->ops->map_sysaddr_to_csrow(mci, info, sys_addr);
+	pvt->ops->map_sysaddr_to_csrow(mci, sys_addr, syndrome);
 }
 
 /* Handle any Un-correctable Errors (UEs) */
-static void amd64_handle_ue(struct mem_ctl_info *mci,
-			    struct err_regs *info)
+static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 {
-	struct amd64_pvt *pvt = mci->pvt_info;
 	struct mem_ctl_info *log_mci, *src_mci = NULL;
 	int csrow;
 	u64 sys_addr;
@@ -1902,13 +1831,13 @@ static void amd64_handle_ue(struct mem_ctl_info *mci,
 
 	log_mci = mci;
 
-	if (!(info->nbsh & K8_NBSH_VALID_ERROR_ADDR)) {
+	if (!(m->status & MCI_STATUS_ADDRV)) {
 		amd64_mc_err(mci, "HW has no ERROR_ADDRESS available\n");
 		edac_mc_handle_ue_no_info(log_mci, EDAC_MOD_STR);
 		return;
 	}
 
-	sys_addr = pvt->ops->get_error_address(mci, info);
+	sys_addr = get_error_address(m);
 
 	/*
 	 * Find out which node the error address belongs to. This may be
@@ -1936,14 +1865,14 @@ static void amd64_handle_ue(struct mem_ctl_info *mci,
 }
 
 static inline void __amd64_decode_bus_error(struct mem_ctl_info *mci,
-					    struct err_regs *info)
+					    struct mce *m)
 {
-	u16 ec = EC(info->nbsl);
-	u8 xec = XEC(info->nbsl, 0x1f);
-	int ecc_type = (info->nbsh >> 13) & 0x3;
+	u16 ec = EC(m->status);
+	u8 xec = XEC(m->status, 0x1f);
+	u8 ecc_type = (m->status >> 45) & 0x3;
 
 	/* Bail early out if this was an 'observed' error */
-	if (PP(ec) == K8_NBSL_PP_OBS)
+	if (PP(ec) == NBSL_PP_OBS)
 		return;
 
 	/* Do only ECC errors */
@@ -1951,34 +1880,16 @@ static inline void __amd64_decode_bus_error(struct mem_ctl_info *mci,
 		return;
 
 	if (ecc_type == 2)
-		amd64_handle_ce(mci, info);
+		amd64_handle_ce(mci, m);
 	else if (ecc_type == 1)
-		amd64_handle_ue(mci, info);
+		amd64_handle_ue(mci, m);
 }
 
 void amd64_decode_bus_error(int node_id, struct mce *m, u32 nbcfg)
 {
 	struct mem_ctl_info *mci = mcis[node_id];
-	struct err_regs regs;
-
-	regs.nbsl  = (u32) m->status;
-	regs.nbsh  = (u32)(m->status >> 32);
-	regs.nbeal = (u32) m->addr;
-	regs.nbeah = (u32)(m->addr >> 32);
-	regs.nbcfg = nbcfg;
-
-	__amd64_decode_bus_error(mci, &regs);
-
-	/*
-	 * Check the UE bit of the NB status high register, if set generate some
-	 * logs. If NOT a GART error, then process the event as a NO-INFO event.
-	 * If it was a GART error, skip that process.
-	 *
-	 * FIXME: this should go somewhere else, if at all.
-	 */
-	if (regs.nbsh & K8_NBSH_UC_ERR && !report_gart_errors)
-		edac_mc_handle_ue_no_info(mci, "UE bit is set");
 
+	__amd64_decode_bus_error(mci, m);
 }
 
 /*
@@ -2027,9 +1938,10 @@ static void free_mc_sibling_devs(struct amd64_pvt *pvt)
  */
 static void read_mc_regs(struct amd64_pvt *pvt)
 {
+	struct cpuinfo_x86 *c = &boot_cpu_data;
 	u64 msr_val;
 	u32 tmp;
-	int dram;
+	unsigned range;
 
 	/*
 	 * Retrieve TOP_MEM and TOP_MEM2; no masking off of reserved bits since
@@ -2046,75 +1958,66 @@ static void read_mc_regs(struct amd64_pvt *pvt)
 	} else
 		debugf0("  TOP_MEM2 disabled.\n");
 
-	amd64_read_pci_cfg(pvt->F3, K8_NBCAP, &pvt->nbcap);
+	amd64_read_pci_cfg(pvt->F3, NBCAP, &pvt->nbcap);
 
-	if (pvt->ops->read_dram_ctl_register)
-		pvt->ops->read_dram_ctl_register(pvt);
+	read_dram_ctl_register(pvt);
 
-	for (dram = 0; dram < DRAM_REG_COUNT; dram++) {
-		/*
-		 * Call CPU specific READ function to get the DRAM Base and
-		 * Limit values from the DCT.
-		 */
-		pvt->ops->read_dram_base_limit(pvt, dram);
+	for (range = 0; range < DRAM_RANGES; range++) {
+		u8 rw;
 
-		/*
-		 * Only print out debug info on rows with both R and W Enabled.
-		 * Normal processing, compiler should optimize this whole 'if'
-		 * debug output block away.
-		 */
-		if (pvt->dram_rw_en[dram] != 0) {
-			debugf1("  DRAM-BASE[%d]: 0x%016llx "
-				"DRAM-LIMIT:  0x%016llx\n",
-				dram,
-				pvt->dram_base[dram],
-				pvt->dram_limit[dram]);
-
-			debugf1("        IntlvEn=%s %s %s "
-				"IntlvSel=%d DstNode=%d\n",
-				pvt->dram_IntlvEn[dram] ?
-					"Enabled" : "Disabled",
-				(pvt->dram_rw_en[dram] & 0x2) ? "W" : "!W",
-				(pvt->dram_rw_en[dram] & 0x1) ? "R" : "!R",
-				pvt->dram_IntlvSel[dram],
-				pvt->dram_DstNode[dram]);
-		}
+		/* read settings for this DRAM range */
+		read_dram_base_limit_regs(pvt, range);
+
+		rw = dram_rw(pvt, range);
+		if (!rw)
+			continue;
+
+		debugf1("  DRAM range[%d], base: 0x%016llx; limit: 0x%016llx\n",
+			range,
+			get_dram_base(pvt, range),
+			get_dram_limit(pvt, range));
+
+		debugf1("   IntlvEn=%s; Range access: %s%s IntlvSel=%d DstNode=%d\n",
+			dram_intlv_en(pvt, range) ? "Enabled" : "Disabled",
+			(rw & 0x1) ? "R" : "-",
+			(rw & 0x2) ? "W" : "-",
+			dram_intlv_sel(pvt, range),
+			dram_dst_node(pvt, range));
 	}
 
-	amd64_read_dct_base_mask(pvt);
+	read_dct_base_mask(pvt);
 
-	amd64_read_pci_cfg(pvt->F1, K8_DHAR, &pvt->dhar);
-	amd64_read_dbam_reg(pvt);
+	amd64_read_pci_cfg(pvt->F1, DHAR, &pvt->dhar);
+	amd64_read_dct_pci_cfg(pvt, DBAM0, &pvt->dbam0);
 
 	amd64_read_pci_cfg(pvt->F3, F10_ONLINE_SPARE, &pvt->online_spare);
 
-	amd64_read_pci_cfg(pvt->F2, F10_DCLR_0, &pvt->dclr0);
-	amd64_read_pci_cfg(pvt->F2, F10_DCHR_0, &pvt->dchr0);
+	amd64_read_dct_pci_cfg(pvt, DCLR0, &pvt->dclr0);
+	amd64_read_dct_pci_cfg(pvt, DCHR0, &pvt->dchr0);
 
-	if (boot_cpu_data.x86 >= 0x10) {
-		if (!dct_ganging_enabled(pvt)) {
-			amd64_read_pci_cfg(pvt->F2, F10_DCLR_1, &pvt->dclr1);
-			amd64_read_pci_cfg(pvt->F2, F10_DCHR_1, &pvt->dchr1);
-		}
-		amd64_read_pci_cfg(pvt->F3, EXT_NB_MCA_CFG, &tmp);
+	if (!dct_ganging_enabled(pvt)) {
+		amd64_read_dct_pci_cfg(pvt, DCLR1, &pvt->dclr1);
+		amd64_read_dct_pci_cfg(pvt, DCHR1, &pvt->dchr1);
 	}
 
-	if (boot_cpu_data.x86 == 0x10 &&
-	    boot_cpu_data.x86_model > 7 &&
-	    /* F3x180[EccSymbolSize]=1 => x8 symbols */
-	    tmp & BIT(25))
-		pvt->syn_type = 8;
-	else
-		pvt->syn_type = 4;
+	pvt->ecc_sym_sz = 4;
 
-	amd64_dump_misc_regs(pvt);
+	if (c->x86 >= 0x10) {
+		amd64_read_pci_cfg(pvt->F3, EXT_NB_MCA_CFG, &tmp);
+		amd64_read_dct_pci_cfg(pvt, DBAM1, &pvt->dbam1);
+
+		/* F10h, revD and later can do x8 ECC too */
+		if ((c->x86 > 0x10 || c->x86_model > 7) && tmp & BIT(25))
+			pvt->ecc_sym_sz = 8;
+	}
+	dump_misc_regs(pvt);
 }
 
 /*
  * NOTE: CPU Revision Dependent code
  *
  * Input:
- *	@csrow_nr ChipSelect Row Number (0..pvt->cs_count-1)
+ *	@csrow_nr ChipSelect Row Number (0..NUM_CHIPSELECTS-1)
  *	k8 private pointer to -->
  *			DRAM Bank Address mapping register
  *			node_id
@@ -2144,7 +2047,7 @@ static void read_mc_regs(struct amd64_pvt *pvt)
  *	encompasses
  *
  */
-static u32 amd64_csrow_nr_pages(int csrow_nr, struct amd64_pvt *pvt)
+static u32 amd64_csrow_nr_pages(struct amd64_pvt *pvt, u8 dct, int csrow_nr)
 {
 	u32 cs_mode, nr_pages;
 
@@ -2157,7 +2060,7 @@ static u32 amd64_csrow_nr_pages(int csrow_nr, struct amd64_pvt *pvt)
 	 */
 	cs_mode = (pvt->dbam0 >> ((csrow_nr / 2) * 4)) & 0xF;
 
-	nr_pages = pvt->ops->dbam_to_cs(pvt, cs_mode) << (20 - PAGE_SHIFT);
+	nr_pages = pvt->ops->dbam_to_cs(pvt, dct, cs_mode) << (20 - PAGE_SHIFT);
 
 	/*
 	 * If dual channel then double the memory size of single channel.
@@ -2180,23 +2083,22 @@ static int init_csrows(struct mem_ctl_info *mci)
 {
 	struct csrow_info *csrow;
 	struct amd64_pvt *pvt = mci->pvt_info;
-	u64 input_addr_min, input_addr_max, sys_addr;
+	u64 input_addr_min, input_addr_max, sys_addr, base, mask;
 	u32 val;
 	int i, empty = 1;
 
-	amd64_read_pci_cfg(pvt->F3, K8_NBCFG, &val);
+	amd64_read_pci_cfg(pvt->F3, NBCFG, &val);
 
 	pvt->nbcfg = val;
-	pvt->ctl_error_info.nbcfg = val;
 
 	debugf0("node %d, NBCFG=0x%08x[ChipKillEccCap: %d|DramEccEn: %d]\n",
 		pvt->mc_node_id, val,
-		!!(val & K8_NBCFG_CHIPKILL), !!(val & K8_NBCFG_ECC_ENABLE));
+		!!(val & NBCFG_CHIPKILL), !!(val & NBCFG_ECC_ENABLE));
 
-	for (i = 0; i < pvt->cs_count; i++) {
+	for_each_chip_select(i, 0, pvt) {
 		csrow = &mci->csrows[i];
 
-		if ((pvt->dcsb0[i] & K8_DCSB_CS_ENABLE) == 0) {
+		if (!csrow_enabled(i, 0, pvt)) {
 			debugf1("----CSROW %d EMPTY for node %d\n", i,
 				pvt->mc_node_id);
 			continue;
@@ -2206,13 +2108,15 @@ static int init_csrows(struct mem_ctl_info *mci)
 			i, pvt->mc_node_id);
 
 		empty = 0;
-		csrow->nr_pages = amd64_csrow_nr_pages(i, pvt);
+		csrow->nr_pages = amd64_csrow_nr_pages(pvt, 0, i);
 		find_csrow_limits(mci, i, &input_addr_min, &input_addr_max);
 		sys_addr = input_addr_to_sys_addr(mci, input_addr_min);
 		csrow->first_page = (u32) (sys_addr >> PAGE_SHIFT);
 		sys_addr = input_addr_to_sys_addr(mci, input_addr_max);
 		csrow->last_page = (u32) (sys_addr >> PAGE_SHIFT);
-		csrow->page_mask = ~mask_from_dct_mask(pvt, i);
+
+		get_cs_base_and_mask(pvt, i, 0, &base, &mask);
+		csrow->page_mask = ~mask;
 		/* 8 bytes of resolution */
 
 		csrow->mtype = amd64_determine_memory_type(pvt, i);
@@ -2231,9 +2135,9 @@ static int init_csrows(struct mem_ctl_info *mci)
 		/*
 		 * determine whether CHIPKILL or JUST ECC or NO ECC is operating
 		 */
-		if (pvt->nbcfg & K8_NBCFG_ECC_ENABLE)
+		if (pvt->nbcfg & NBCFG_ECC_ENABLE)
 			csrow->edac_mode =
-			    (pvt->nbcfg & K8_NBCFG_CHIPKILL) ?
+			    (pvt->nbcfg & NBCFG_CHIPKILL) ?
 			    EDAC_S4ECD4ED : EDAC_SECDED;
 		else
 			csrow->edac_mode = EDAC_NONE;
@@ -2243,7 +2147,7 @@ static int init_csrows(struct mem_ctl_info *mci)
 }
 
 /* get all cores on this DCT */
-static void get_cpus_on_this_dct_cpumask(struct cpumask *mask, int nid)
+static void get_cpus_on_this_dct_cpumask(struct cpumask *mask, unsigned nid)
 {
 	int cpu;
 
@@ -2253,7 +2157,7 @@ static void get_cpus_on_this_dct_cpumask(struct cpumask *mask, int nid)
 }
 
 /* check MCG_CTL on all the cpus on this node */
-static bool amd64_nb_mce_bank_enabled_on_node(int nid)
+static bool amd64_nb_mce_bank_enabled_on_node(unsigned nid)
 {
 	cpumask_var_t mask;
 	int cpu, nbe;
@@ -2270,7 +2174,7 @@ static bool amd64_nb_mce_bank_enabled_on_node(int nid)
 
 	for_each_cpu(cpu, mask) {
 		struct msr *reg = per_cpu_ptr(msrs, cpu);
-		nbe = reg->l & K8_MSR_MCGCTL_NBE;
+		nbe = reg->l & MSR_MCGCTL_NBE;
 
 		debugf0("core: %u, MCG_CTL: 0x%llx, NB MSR is %s\n",
 			cpu, reg->q,
@@ -2305,16 +2209,16 @@ static int toggle_ecc_err_reporting(struct ecc_settings *s, u8 nid, bool on)
 		struct msr *reg = per_cpu_ptr(msrs, cpu);
 
 		if (on) {
-			if (reg->l & K8_MSR_MCGCTL_NBE)
+			if (reg->l & MSR_MCGCTL_NBE)
 				s->flags.nb_mce_enable = 1;
 
-			reg->l |= K8_MSR_MCGCTL_NBE;
+			reg->l |= MSR_MCGCTL_NBE;
 		} else {
 			/*
 			 * Turn off NB MCE reporting only when it was off before
 			 */
 			if (!s->flags.nb_mce_enable)
-				reg->l &= ~K8_MSR_MCGCTL_NBE;
+				reg->l &= ~MSR_MCGCTL_NBE;
 		}
 	}
 	wrmsr_on_cpus(cmask, MSR_IA32_MCG_CTL, msrs);
@@ -2328,40 +2232,38 @@ static bool enable_ecc_error_reporting(struct ecc_settings *s, u8 nid,
 				       struct pci_dev *F3)
 {
 	bool ret = true;
-	u32 value, mask = K8_NBCTL_CECCEn | K8_NBCTL_UECCEn;
+	u32 value, mask = 0x3;		/* UECC/CECC enable */
 
 	if (toggle_ecc_err_reporting(s, nid, ON)) {
 		amd64_warn("Error enabling ECC reporting over MCGCTL!\n");
 		return false;
 	}
 
-	amd64_read_pci_cfg(F3, K8_NBCTL, &value);
+	amd64_read_pci_cfg(F3, NBCTL, &value);
 
-	/* turn on UECCEn and CECCEn bits */
 	s->old_nbctl   = value & mask;
 	s->nbctl_valid = true;
 
 	value |= mask;
-	pci_write_config_dword(F3, K8_NBCTL, value);
+	amd64_write_pci_cfg(F3, NBCTL, value);
 
-	amd64_read_pci_cfg(F3, K8_NBCFG, &value);
+	amd64_read_pci_cfg(F3, NBCFG, &value);
 
-	debugf0("1: node %d, NBCFG=0x%08x[ChipKillEccCap: %d|DramEccEn: %d]\n",
-		nid, value,
-		!!(value & K8_NBCFG_CHIPKILL), !!(value & K8_NBCFG_ECC_ENABLE));
+	debugf0("1: node %d, NBCFG=0x%08x[DramEccEn: %d]\n",
+		nid, value, !!(value & NBCFG_ECC_ENABLE));
 
-	if (!(value & K8_NBCFG_ECC_ENABLE)) {
+	if (!(value & NBCFG_ECC_ENABLE)) {
 		amd64_warn("DRAM ECC disabled on this node, enabling...\n");
 
 		s->flags.nb_ecc_prev = 0;
 
 		/* Attempt to turn on DRAM ECC Enable */
-		value |= K8_NBCFG_ECC_ENABLE;
-		pci_write_config_dword(F3, K8_NBCFG, value);
+		value |= NBCFG_ECC_ENABLE;
+		amd64_write_pci_cfg(F3, NBCFG, value);
 
-		amd64_read_pci_cfg(F3, K8_NBCFG, &value);
+		amd64_read_pci_cfg(F3, NBCFG, &value);
 
-		if (!(value & K8_NBCFG_ECC_ENABLE)) {
+		if (!(value & NBCFG_ECC_ENABLE)) {
 			amd64_warn("Hardware rejected DRAM ECC enable,"
 				   "check memory DIMM configuration.\n");
 			ret = false;
@@ -2372,9 +2274,8 @@ static bool enable_ecc_error_reporting(struct ecc_settings *s, u8 nid,
 		s->flags.nb_ecc_prev = 1;
 	}
 
-	debugf0("2: node %d, NBCFG=0x%08x[ChipKillEccCap: %d|DramEccEn: %d]\n",
-		nid, value,
-		!!(value & K8_NBCFG_CHIPKILL), !!(value & K8_NBCFG_ECC_ENABLE));
+	debugf0("2: node %d, NBCFG=0x%08x[DramEccEn: %d]\n",
+		nid, value, !!(value & NBCFG_ECC_ENABLE));
 
 	return ret;
 }
@@ -2382,22 +2283,23 @@ static bool enable_ecc_error_reporting(struct ecc_settings *s, u8 nid,
 static void restore_ecc_error_reporting(struct ecc_settings *s, u8 nid,
 					struct pci_dev *F3)
 {
-	u32 value, mask = K8_NBCTL_CECCEn | K8_NBCTL_UECCEn;
+	u32 value, mask = 0x3;		/* UECC/CECC enable */
+
 
 	if (!s->nbctl_valid)
 		return;
 
-	amd64_read_pci_cfg(F3, K8_NBCTL, &value);
+	amd64_read_pci_cfg(F3, NBCTL, &value);
 	value &= ~mask;
 	value |= s->old_nbctl;
 
-	pci_write_config_dword(F3, K8_NBCTL, value);
+	amd64_write_pci_cfg(F3, NBCTL, value);
 
 	/* restore previous BIOS DRAM ECC "off" setting we force-enabled */
 	if (!s->flags.nb_ecc_prev) {
-		amd64_read_pci_cfg(F3, K8_NBCFG, &value);
-		value &= ~K8_NBCFG_ECC_ENABLE;
-		pci_write_config_dword(F3, K8_NBCFG, value);
+		amd64_read_pci_cfg(F3, NBCFG, &value);
+		value &= ~NBCFG_ECC_ENABLE;
+		amd64_write_pci_cfg(F3, NBCFG, value);
 	}
 
 	/* restore the NB Enable MCGCTL bit */
@@ -2423,9 +2325,9 @@ static bool ecc_enabled(struct pci_dev *F3, u8 nid)
 	u8 ecc_en = 0;
 	bool nb_mce_en = false;
 
-	amd64_read_pci_cfg(F3, K8_NBCFG, &value);
+	amd64_read_pci_cfg(F3, NBCFG, &value);
 
-	ecc_en = !!(value & K8_NBCFG_ECC_ENABLE);
+	ecc_en = !!(value & NBCFG_ECC_ENABLE);
 	amd64_info("DRAM ECC %s.\n", (ecc_en ? "enabled" : "disabled"));
 
 	nb_mce_en = amd64_nb_mce_bank_enabled_on_node(nid);
@@ -2463,23 +2365,24 @@ static void set_mc_sysfs_attrs(struct mem_ctl_info *mci)
 	mci->mc_driver_sysfs_attributes = sysfs_attrs;
 }
 
-static void setup_mci_misc_attrs(struct mem_ctl_info *mci)
+static void setup_mci_misc_attrs(struct mem_ctl_info *mci,
+				 struct amd64_family_type *fam)
 {
 	struct amd64_pvt *pvt = mci->pvt_info;
 
 	mci->mtype_cap		= MEM_FLAG_DDR2 | MEM_FLAG_RDDR2;
 	mci->edac_ctl_cap	= EDAC_FLAG_NONE;
 
-	if (pvt->nbcap & K8_NBCAP_SECDED)
+	if (pvt->nbcap & NBCAP_SECDED)
 		mci->edac_ctl_cap |= EDAC_FLAG_SECDED;
 
-	if (pvt->nbcap & K8_NBCAP_CHIPKILL)
+	if (pvt->nbcap & NBCAP_CHIPKILL)
 		mci->edac_ctl_cap |= EDAC_FLAG_S4ECD4ED;
 
 	mci->edac_cap		= amd64_determine_edac_cap(pvt);
 	mci->mod_name		= EDAC_MOD_STR;
 	mci->mod_ver		= EDAC_AMD64_VERSION;
-	mci->ctl_name		= pvt->ctl_name;
+	mci->ctl_name		= fam->ctl_name;
 	mci->dev_name		= pci_name(pvt->F2);
 	mci->ctl_page_to_phys	= NULL;
 
@@ -2500,14 +2403,16 @@ static struct amd64_family_type *amd64_per_family_init(struct amd64_pvt *pvt)
 	case 0xf:
 		fam_type		= &amd64_family_types[K8_CPUS];
 		pvt->ops		= &amd64_family_types[K8_CPUS].ops;
-		pvt->ctl_name		= fam_type->ctl_name;
-		pvt->min_scrubrate	= K8_MIN_SCRUB_RATE_BITS;
 		break;
+
 	case 0x10:
 		fam_type		= &amd64_family_types[F10_CPUS];
 		pvt->ops		= &amd64_family_types[F10_CPUS].ops;
-		pvt->ctl_name		= fam_type->ctl_name;
-		pvt->min_scrubrate	= F10_MIN_SCRUB_RATE_BITS;
+		break;
+
+	case 0x15:
+		fam_type		= &amd64_family_types[F15_CPUS];
+		pvt->ops		= &amd64_family_types[F15_CPUS].ops;
 		break;
 
 	default:
@@ -2517,7 +2422,7 @@ static struct amd64_family_type *amd64_per_family_init(struct amd64_pvt *pvt)
 
 	pvt->ext_model = boot_cpu_data.x86_model >> 4;
 
-	amd64_info("%s %sdetected (node %d).\n", pvt->ctl_name,
+	amd64_info("%s %sdetected (node %d).\n", fam_type->ctl_name,
 		     (fam == 0xf ?
 				(pvt->ext_model >= K8_REV_F  ? "revF or later "
 							     : "revE or earlier ")
@@ -2564,14 +2469,14 @@ static int amd64_init_one_instance(struct pci_dev *F2)
 		goto err_siblings;
 
 	ret = -ENOMEM;
-	mci = edac_mc_alloc(0, pvt->cs_count, pvt->channel_count, nid);
+	mci = edac_mc_alloc(0, pvt->csels[0].b_cnt, pvt->channel_count, nid);
 	if (!mci)
 		goto err_siblings;
 
 	mci->pvt_info = pvt;
 	mci->dev = &pvt->F2->dev;
 
-	setup_mci_misc_attrs(mci);
+	setup_mci_misc_attrs(mci, fam_type);
 
 	if (init_csrows(mci))
 		mci->edac_cap = EDAC_FLAG_NONE;
@@ -2714,6 +2619,15 @@ static const struct pci_device_id amd64_pci_table[] __devinitdata = {
 		.class		= 0,
 		.class_mask	= 0,
 	},
+	{
+		.vendor		= PCI_VENDOR_ID_AMD,
+		.device		= PCI_DEVICE_ID_AMD_15H_NB_F2,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.class		= 0,
+		.class_mask	= 0,
+	},
+
 	{0, }
 };
 MODULE_DEVICE_TABLE(pci, amd64_pci_table);
@@ -2754,7 +2668,7 @@ static int __init amd64_edac_init(void)
 {
 	int err = -ENODEV;
 
-	edac_printk(KERN_INFO, EDAC_MOD_STR, EDAC_AMD64_VERSION "\n");
+	printk(KERN_INFO "AMD64 EDAC driver v%s\n", EDAC_AMD64_VERSION);
 
 	opstate_init();
 
diff --git a/drivers/edac/amd64_edac.h b/drivers/edac/amd64_edac.h
index 613ec72b0f65..11be36a311eb 100644
--- a/drivers/edac/amd64_edac.h
+++ b/drivers/edac/amd64_edac.h
@@ -144,7 +144,7 @@
  *         sections 3.5.4 and 3.5.5 for more information.
  */
 
-#define EDAC_AMD64_VERSION		"v3.3.0"
+#define EDAC_AMD64_VERSION		"3.4.0"
 #define EDAC_MOD_STR			"amd64_edac"
 
 /* Extended Model from CPUID, for CPU Revision numbers */
@@ -153,85 +153,64 @@
 #define K8_REV_F			4
 
 /* Hardware limit on ChipSelect rows per MC and processors per system */
-#define MAX_CS_COUNT			8
-#define DRAM_REG_COUNT			8
+#define NUM_CHIPSELECTS			8
+#define DRAM_RANGES			8
 
 #define ON true
 #define OFF false
 
 /*
+ * Create a contiguous bitmask starting at bit position @lo and ending at
+ * position @hi. For example
+ *
+ * GENMASK(21, 39) gives us the 64bit vector 0x000000ffffe00000.
+ */
+#define GENMASK(lo, hi)			(((1ULL << ((hi) - (lo) + 1)) - 1) << (lo))
+
+/*
  * PCI-defined configuration space registers
  */
+#define PCI_DEVICE_ID_AMD_15H_NB_F1	0x1601
+#define PCI_DEVICE_ID_AMD_15H_NB_F2	0x1602
 
 
 /*
  * Function 1 - Address Map
  */
-#define K8_DRAM_BASE_LOW		0x40
-#define K8_DRAM_LIMIT_LOW		0x44
-#define K8_DHAR				0xf0
-
-#define DHAR_VALID			BIT(0)
-#define F10_DRAM_MEM_HOIST_VALID	BIT(1)
+#define DRAM_BASE_LO			0x40
+#define DRAM_LIMIT_LO			0x44
 
-#define DHAR_BASE_MASK			0xff000000
-#define dhar_base(dhar)			(dhar & DHAR_BASE_MASK)
+#define dram_intlv_en(pvt, i)		((u8)((pvt->ranges[i].base.lo >> 8) & 0x7))
+#define dram_rw(pvt, i)			((u8)(pvt->ranges[i].base.lo & 0x3))
+#define dram_intlv_sel(pvt, i)		((u8)((pvt->ranges[i].lim.lo >> 8) & 0x7))
+#define dram_dst_node(pvt, i)		((u8)(pvt->ranges[i].lim.lo & 0x7))
 
-#define K8_DHAR_OFFSET_MASK		0x0000ff00
-#define k8_dhar_offset(dhar)		((dhar & K8_DHAR_OFFSET_MASK) << 16)
+#define DHAR				0xf0
+#define dhar_valid(pvt)			((pvt)->dhar & BIT(0))
+#define dhar_mem_hoist_valid(pvt)	((pvt)->dhar & BIT(1))
+#define dhar_base(pvt)			((pvt)->dhar & 0xff000000)
+#define k8_dhar_offset(pvt)		(((pvt)->dhar & 0x0000ff00) << 16)
 
-#define F10_DHAR_OFFSET_MASK		0x0000ff80
 					/* NOTE: Extra mask bit vs K8 */
-#define f10_dhar_offset(dhar)		((dhar & F10_DHAR_OFFSET_MASK) << 16)
+#define f10_dhar_offset(pvt)		(((pvt)->dhar & 0x0000ff80) << 16)
 
+#define DCT_CFG_SEL			0x10C
 
-/* F10 High BASE/LIMIT registers */
-#define F10_DRAM_BASE_HIGH		0x140
-#define F10_DRAM_LIMIT_HIGH		0x144
+#define DRAM_BASE_HI			0x140
+#define DRAM_LIMIT_HI			0x144
 
 
 /*
  * Function 2 - DRAM controller
  */
-#define K8_DCSB0			0x40
-#define F10_DCSB1			0x140
+#define DCSB0				0x40
+#define DCSB1				0x140
+#define DCSB_CS_ENABLE			BIT(0)
 
-#define K8_DCSB_CS_ENABLE		BIT(0)
-#define K8_DCSB_NPT_SPARE		BIT(1)
-#define K8_DCSB_NPT_TESTFAIL		BIT(2)
+#define DCSM0				0x60
+#define DCSM1				0x160
 
-/*
- * REV E: select [31:21] and [15:9] from DCSB and the shift amount to form
- * the address
- */
-#define REV_E_DCSB_BASE_BITS		(0xFFE0FE00ULL)
-#define REV_E_DCS_SHIFT			4
-
-#define REV_F_F1Xh_DCSB_BASE_BITS	(0x1FF83FE0ULL)
-#define REV_F_F1Xh_DCS_SHIFT		8
-
-/*
- * REV F and later: selects [28:19] and [13:5] from DCSB and the shift amount
- * to form the address
- */
-#define REV_F_DCSB_BASE_BITS		(0x1FF83FE0ULL)
-#define REV_F_DCS_SHIFT			8
-
-/* DRAM CS Mask Registers */
-#define K8_DCSM0			0x60
-#define F10_DCSM1			0x160
-
-/* REV E: select [29:21] and [15:9] from DCSM */
-#define REV_E_DCSM_MASK_BITS		0x3FE0FE00
-
-/* unused bits [24:20] and [12:0] */
-#define REV_E_DCS_NOTUSED_BITS		0x01F01FFF
-
-/* REV F and later: select [28:19] and [13:5] from DCSM */
-#define REV_F_F1Xh_DCSM_MASK_BITS	0x1FF83FE0
-
-/* unused bits [26:22] and [12:0] */
-#define REV_F_F1Xh_DCS_NOTUSED_BITS	0x07C01FFF
+#define csrow_enabled(i, dct, pvt)	((pvt)->csels[(dct)].csbases[(i)] & DCSB_CS_ENABLE)
 
 #define DBAM0				0x80
 #define DBAM1				0x180
@@ -241,148 +220,84 @@
 
 #define DBAM_MAX_VALUE			11
 
-
-#define F10_DCLR_0			0x90
-#define F10_DCLR_1			0x190
+#define DCLR0				0x90
+#define DCLR1				0x190
 #define REVE_WIDTH_128			BIT(16)
-#define F10_WIDTH_128			BIT(11)
+#define WIDTH_128			BIT(11)
 
+#define DCHR0				0x94
+#define DCHR1				0x194
+#define DDR3_MODE			BIT(8)
 
-#define F10_DCHR_0			0x94
-#define F10_DCHR_1			0x194
+#define DCT_SEL_LO			0x110
+#define dct_sel_baseaddr(pvt)		((pvt)->dct_sel_lo & 0xFFFFF800)
+#define dct_sel_interleave_addr(pvt)	(((pvt)->dct_sel_lo >> 6) & 0x3)
+#define dct_high_range_enabled(pvt)	((pvt)->dct_sel_lo & BIT(0))
+#define dct_interleave_enabled(pvt)	((pvt)->dct_sel_lo & BIT(2))
 
-#define F10_DCHR_FOUR_RANK_DIMM		BIT(18)
-#define DDR3_MODE			BIT(8)
-#define F10_DCHR_MblMode		BIT(6)
+#define dct_ganging_enabled(pvt)	((boot_cpu_data.x86 == 0x10) && ((pvt)->dct_sel_lo & BIT(4)))
 
+#define dct_data_intlv_enabled(pvt)	((pvt)->dct_sel_lo & BIT(5))
+#define dct_memory_cleared(pvt)		((pvt)->dct_sel_lo & BIT(10))
 
-#define F10_DCTL_SEL_LOW		0x110
-#define dct_sel_baseaddr(pvt)		((pvt->dram_ctl_select_low) & 0xFFFFF800)
-#define dct_sel_interleave_addr(pvt)	(((pvt->dram_ctl_select_low) >> 6) & 0x3)
-#define dct_high_range_enabled(pvt)	(pvt->dram_ctl_select_low & BIT(0))
-#define dct_interleave_enabled(pvt)	(pvt->dram_ctl_select_low & BIT(2))
-#define dct_ganging_enabled(pvt)	(pvt->dram_ctl_select_low & BIT(4))
-#define dct_data_intlv_enabled(pvt)	(pvt->dram_ctl_select_low & BIT(5))
-#define dct_dram_enabled(pvt)		(pvt->dram_ctl_select_low & BIT(8))
-#define dct_memory_cleared(pvt)		(pvt->dram_ctl_select_low & BIT(10))
+#define SWAP_INTLV_REG			0x10c
 
-#define F10_DCTL_SEL_HIGH		0x114
+#define DCT_SEL_HI			0x114
 
 /*
  * Function 3 - Misc Control
  */
-#define K8_NBCTL			0x40
-
-/* Correctable ECC error reporting enable */
-#define K8_NBCTL_CECCEn			BIT(0)
-
-/* UnCorrectable ECC error reporting enable */
-#define K8_NBCTL_UECCEn			BIT(1)
+#define NBCTL				0x40
 
-#define K8_NBCFG			0x44
-#define K8_NBCFG_CHIPKILL		BIT(23)
-#define K8_NBCFG_ECC_ENABLE		BIT(22)
+#define NBCFG				0x44
+#define NBCFG_CHIPKILL			BIT(23)
+#define NBCFG_ECC_ENABLE		BIT(22)
 
-#define K8_NBSL				0x48
-
-
-/* Family F10h: Normalized Extended Error Codes */
-#define F10_NBSL_EXT_ERR_RES		0x0
+/* F3x48: NBSL */
 #define F10_NBSL_EXT_ERR_ECC		0x8
+#define NBSL_PP_OBS			0x2
 
-/* Next two are overloaded values */
-#define F10_NBSL_EXT_ERR_LINK_PROTO	0xB
-#define F10_NBSL_EXT_ERR_L3_PROTO	0xB
-
-#define F10_NBSL_EXT_ERR_NB_ARRAY	0xC
-#define F10_NBSL_EXT_ERR_DRAM_PARITY	0xD
-#define F10_NBSL_EXT_ERR_LINK_RETRY	0xE
-
-/* Next two are overloaded values */
-#define F10_NBSL_EXT_ERR_GART_WALK	0xF
-#define F10_NBSL_EXT_ERR_DEV_WALK	0xF
-
-/* 0x10 to 0x1B: Reserved */
-#define F10_NBSL_EXT_ERR_L3_DATA	0x1C
-#define F10_NBSL_EXT_ERR_L3_TAG		0x1D
-#define F10_NBSL_EXT_ERR_L3_LRU		0x1E
-
-/* K8: Normalized Extended Error Codes */
-#define K8_NBSL_EXT_ERR_ECC		0x0
-#define K8_NBSL_EXT_ERR_CRC		0x1
-#define K8_NBSL_EXT_ERR_SYNC		0x2
-#define K8_NBSL_EXT_ERR_MST		0x3
-#define K8_NBSL_EXT_ERR_TGT		0x4
-#define K8_NBSL_EXT_ERR_GART		0x5
-#define K8_NBSL_EXT_ERR_RMW		0x6
-#define K8_NBSL_EXT_ERR_WDT		0x7
-#define K8_NBSL_EXT_ERR_CHIPKILL_ECC	0x8
-#define K8_NBSL_EXT_ERR_DRAM_PARITY	0xD
-
-/*
- * The following are for BUS type errors AFTER values have been normalized by
- * shifting right
- */
-#define K8_NBSL_PP_SRC			0x0
-#define K8_NBSL_PP_RES			0x1
-#define K8_NBSL_PP_OBS			0x2
-#define K8_NBSL_PP_GENERIC		0x3
-
-#define EXTRACT_ERR_CPU_MAP(x)		((x) & 0xF)
-
-#define K8_NBEAL			0x50
-#define K8_NBEAH			0x54
-#define K8_SCRCTRL			0x58
-
-#define F10_NB_CFG_LOW			0x88
+#define SCRCTRL				0x58
 
 #define F10_ONLINE_SPARE		0xB0
-#define F10_ONLINE_SPARE_SWAPDONE0(x)	((x) & BIT(1))
-#define F10_ONLINE_SPARE_SWAPDONE1(x)	((x) & BIT(3))
-#define F10_ONLINE_SPARE_BADDRAM_CS0(x) (((x) >> 4) & 0x00000007)
-#define F10_ONLINE_SPARE_BADDRAM_CS1(x) (((x) >> 8) & 0x00000007)
+#define online_spare_swap_done(pvt, c)	(((pvt)->online_spare >> (1 + 2 * (c))) & 0x1)
+#define online_spare_bad_dramcs(pvt, c)	(((pvt)->online_spare >> (4 + 4 * (c))) & 0x7)
 
 #define F10_NB_ARRAY_ADDR		0xB8
-
-#define F10_NB_ARRAY_DRAM_ECC		0x80000000
+#define F10_NB_ARRAY_DRAM_ECC		BIT(31)
 
 /* Bits [2:1] are used to select 16-byte section within a 64-byte cacheline  */
 #define SET_NB_ARRAY_ADDRESS(section)	(((section) & 0x3) << 1)
 
 #define F10_NB_ARRAY_DATA		0xBC
-
 #define SET_NB_DRAM_INJECTION_WRITE(word, bits)  \
 					(BIT(((word) & 0xF) + 20) | \
 					BIT(17) | bits)
-
 #define SET_NB_DRAM_INJECTION_READ(word, bits)  \
 					(BIT(((word) & 0xF) + 20) | \
 					BIT(16) |  bits)
 
-#define K8_NBCAP			0xE8
-#define K8_NBCAP_CORES			(BIT(12)|BIT(13))
-#define K8_NBCAP_CHIPKILL		BIT(4)
-#define K8_NBCAP_SECDED			BIT(3)
-#define K8_NBCAP_DCT_DUAL		BIT(0)
+#define NBCAP				0xE8
+#define NBCAP_CHIPKILL			BIT(4)
+#define NBCAP_SECDED			BIT(3)
+#define NBCAP_DCT_DUAL			BIT(0)
 
 #define EXT_NB_MCA_CFG			0x180
 
 /* MSRs */
-#define K8_MSR_MCGCTL_NBE		BIT(4)
-
-#define K8_MSR_MC4CTL			0x0410
-#define K8_MSR_MC4STAT			0x0411
-#define K8_MSR_MC4ADDR			0x0412
+#define MSR_MCGCTL_NBE			BIT(4)
 
 /* AMD sets the first MC device at device ID 0x18. */
-static inline int get_node_id(struct pci_dev *pdev)
+static inline u8 get_node_id(struct pci_dev *pdev)
 {
 	return PCI_SLOT(pdev->devfn) - 0x18;
 }
 
-enum amd64_chipset_families {
+enum amd_families {
 	K8_CPUS = 0,
 	F10_CPUS,
+	F15_CPUS,
+	NUM_FAMILIES,
 };
 
 /* Error injection control structure */
@@ -392,13 +307,35 @@ struct error_injection {
 	u32	bit_map;
 };
 
+/* low and high part of PCI config space regs */
+struct reg_pair {
+	u32 lo, hi;
+};
+
+/*
+ * See F1x[1, 0][7C:40] DRAM Base/Limit Registers
+ */
+struct dram_range {
+	struct reg_pair base;
+	struct reg_pair lim;
+};
+
+/* A DCT chip selects collection */
+struct chip_select {
+	u32 csbases[NUM_CHIPSELECTS];
+	u8 b_cnt;
+
+	u32 csmasks[NUM_CHIPSELECTS];
+	u8 m_cnt;
+};
+
 struct amd64_pvt {
 	struct low_ops *ops;
 
 	/* pci_device handles which we utilize */
 	struct pci_dev *F1, *F2, *F3;
 
-	int mc_node_id;		/* MC index of this MC node */
+	unsigned mc_node_id;	/* MC index of this MC node */
 	int ext_model;		/* extended model value of this node */
 	int channel_count;
 
@@ -414,60 +351,50 @@ struct amd64_pvt {
 	u32 dbam0;		/* DRAM Base Address Mapping reg for DCT0 */
 	u32 dbam1;		/* DRAM Base Address Mapping reg for DCT1 */
 
-	/* DRAM CS Base Address Registers F2x[1,0][5C:40] */
-	u32 dcsb0[MAX_CS_COUNT];
-	u32 dcsb1[MAX_CS_COUNT];
-
-	/* DRAM CS Mask Registers F2x[1,0][6C:60] */
-	u32 dcsm0[MAX_CS_COUNT];
-	u32 dcsm1[MAX_CS_COUNT];
-
-	/*
-	 * Decoded parts of DRAM BASE and LIMIT Registers
-	 * F1x[78,70,68,60,58,50,48,40]
-	 */
-	u64 dram_base[DRAM_REG_COUNT];
-	u64 dram_limit[DRAM_REG_COUNT];
-	u8  dram_IntlvSel[DRAM_REG_COUNT];
-	u8  dram_IntlvEn[DRAM_REG_COUNT];
-	u8  dram_DstNode[DRAM_REG_COUNT];
-	u8  dram_rw_en[DRAM_REG_COUNT];
-
-	/*
-	 * The following fields are set at (load) run time, after CPU revision
-	 * has been determined, since the dct_base and dct_mask registers vary
-	 * based on revision
-	 */
-	u32 dcsb_base;		/* DCSB base bits */
-	u32 dcsm_mask;		/* DCSM mask bits */
-	u32 cs_count;		/* num chip selects (== num DCSB registers) */
-	u32 num_dcsm;		/* Number of DCSM registers */
-	u32 dcs_mask_notused;	/* DCSM notused mask bits */
-	u32 dcs_shift;		/* DCSB and DCSM shift value */
+	/* one for each DCT */
+	struct chip_select csels[2];
+
+	/* DRAM base and limit pairs F1x[78,70,68,60,58,50,48,40] */
+	struct dram_range ranges[DRAM_RANGES];
 
 	u64 top_mem;		/* top of memory below 4GB */
 	u64 top_mem2;		/* top of memory above 4GB */
 
-	u32 dram_ctl_select_low;	/* DRAM Controller Select Low Reg */
-	u32 dram_ctl_select_high;	/* DRAM Controller Select High Reg */
-	u32 online_spare;               /* On-Line spare Reg */
+	u32 dct_sel_lo;		/* DRAM Controller Select Low */
+	u32 dct_sel_hi;		/* DRAM Controller Select High */
+	u32 online_spare;	/* On-Line spare Reg */
 
 	/* x4 or x8 syndromes in use */
-	u8 syn_type;
-
-	/* temp storage for when input is received from sysfs */
-	struct err_regs ctl_error_info;
+	u8 ecc_sym_sz;
 
 	/* place to store error injection parameters prior to issue */
 	struct error_injection injection;
+};
 
-	/* DCT per-family scrubrate setting */
-	u32 min_scrubrate;
+static inline u64 get_dram_base(struct amd64_pvt *pvt, unsigned i)
+{
+	u64 addr = ((u64)pvt->ranges[i].base.lo & 0xffff0000) << 8;
 
-	/* family name this instance is running on */
-	const char *ctl_name;
+	if (boot_cpu_data.x86 == 0xf)
+		return addr;
 
-};
+	return (((u64)pvt->ranges[i].base.hi & 0x000000ff) << 40) | addr;
+}
+
+static inline u64 get_dram_limit(struct amd64_pvt *pvt, unsigned i)
+{
+	u64 lim = (((u64)pvt->ranges[i].lim.lo & 0xffff0000) << 8) | 0x00ffffff;
+
+	if (boot_cpu_data.x86 == 0xf)
+		return lim;
+
+	return (((u64)pvt->ranges[i].lim.hi & 0x000000ff) << 40) | lim;
+}
+
+static inline u16 extract_syndrome(u64 status)
+{
+	return ((status >> 47) & 0xff) | ((status >> 16) & 0xff00);
+}
 
 /*
  * per-node ECC settings descriptor
@@ -482,14 +409,6 @@ struct ecc_settings {
 	} flags;
 };
 
-extern const char *tt_msgs[4];
-extern const char *ll_msgs[4];
-extern const char *rrrr_msgs[16];
-extern const char *to_msgs[2];
-extern const char *pp_msgs[4];
-extern const char *ii_msgs[4];
-extern const char *htlink_msgs[8];
-
 #ifdef CONFIG_EDAC_DEBUG
 #define NUM_DBG_ATTRS 5
 #else
@@ -511,14 +430,11 @@ extern struct mcidev_sysfs_attribute amd64_dbg_attrs[NUM_DBG_ATTRS],
  */
 struct low_ops {
 	int (*early_channel_count)	(struct amd64_pvt *pvt);
-
-	u64 (*get_error_address)	(struct mem_ctl_info *mci,
-					 struct err_regs *info);
-	void (*read_dram_base_limit)	(struct amd64_pvt *pvt, int dram);
-	void (*read_dram_ctl_register)	(struct amd64_pvt *pvt);
-	void (*map_sysaddr_to_csrow)	(struct mem_ctl_info *mci,
-					 struct err_regs *info, u64 SystemAddr);
-	int (*dbam_to_cs)		(struct amd64_pvt *pvt, int cs_mode);
+	void (*map_sysaddr_to_csrow)	(struct mem_ctl_info *mci, u64 sys_addr,
+					 u16 syndrome);
+	int (*dbam_to_cs)		(struct amd64_pvt *pvt, u8 dct, unsigned cs_mode);
+	int (*read_dct_pci_cfg)		(struct amd64_pvt *pvt, int offset,
+					 u32 *val, const char *func);
 };
 
 struct amd64_family_type {
@@ -527,28 +443,17 @@ struct amd64_family_type {
 	struct low_ops ops;
 };
 
-static inline int amd64_read_pci_cfg_dword(struct pci_dev *pdev, int offset,
-					   u32 *val, const char *func)
-{
-	int err = 0;
-
-	err = pci_read_config_dword(pdev, offset, val);
-	if (err)
-		amd64_warn("%s: error reading F%dx%x.\n",
-			   func, PCI_FUNC(pdev->devfn), offset);
-
-	return err;
-}
+int __amd64_write_pci_cfg_dword(struct pci_dev *pdev, int offset,
+				u32 val, const char *func);
 
 #define amd64_read_pci_cfg(pdev, offset, val)	\
-	amd64_read_pci_cfg_dword(pdev, offset, val, __func__)
+	__amd64_read_pci_cfg_dword(pdev, offset, val, __func__)
 
-/*
- * For future CPU versions, verify the following as new 'slow' rates appear and
- * modify the necessary skip values for the supported CPU.
- */
-#define K8_MIN_SCRUB_RATE_BITS	0x0
-#define F10_MIN_SCRUB_RATE_BITS	0x5
+#define amd64_write_pci_cfg(pdev, offset, val)	\
+	__amd64_write_pci_cfg_dword(pdev, offset, val, __func__)
+
+#define amd64_read_dct_pci_cfg(pvt, offset, val) \
+	pvt->ops->read_dct_pci_cfg(pvt, offset, val, __func__)
 
 int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 			     u64 *hole_offset, u64 *hole_size);
diff --git a/drivers/edac/amd64_edac_inj.c b/drivers/edac/amd64_edac_inj.c
index 688478de1cbd..303f10e03dda 100644
--- a/drivers/edac/amd64_edac_inj.c
+++ b/drivers/edac/amd64_edac_inj.c
@@ -117,13 +117,13 @@ static ssize_t amd64_inject_read_store(struct mem_ctl_info *mci,
 		/* Form value to choose 16-byte section of cacheline */
 		section = F10_NB_ARRAY_DRAM_ECC |
 				SET_NB_ARRAY_ADDRESS(pvt->injection.section);
-		pci_write_config_dword(pvt->F3, F10_NB_ARRAY_ADDR, section);
+		amd64_write_pci_cfg(pvt->F3, F10_NB_ARRAY_ADDR, section);
 
 		word_bits = SET_NB_DRAM_INJECTION_READ(pvt->injection.word,
 						pvt->injection.bit_map);
 
 		/* Issue 'word' and 'bit' along with the READ request */
-		pci_write_config_dword(pvt->F3, F10_NB_ARRAY_DATA, word_bits);
+		amd64_write_pci_cfg(pvt->F3, F10_NB_ARRAY_DATA, word_bits);
 
 		debugf0("section=0x%x word_bits=0x%x\n", section, word_bits);
 
@@ -150,13 +150,13 @@ static ssize_t amd64_inject_write_store(struct mem_ctl_info *mci,
 		/* Form value to choose 16-byte section of cacheline */
 		section = F10_NB_ARRAY_DRAM_ECC |
 				SET_NB_ARRAY_ADDRESS(pvt->injection.section);
-		pci_write_config_dword(pvt->F3, F10_NB_ARRAY_ADDR, section);
+		amd64_write_pci_cfg(pvt->F3, F10_NB_ARRAY_ADDR, section);
 
 		word_bits = SET_NB_DRAM_INJECTION_WRITE(pvt->injection.word,
 						pvt->injection.bit_map);
 
 		/* Issue 'word' and 'bit' along with the READ request */
-		pci_write_config_dword(pvt->F3, F10_NB_ARRAY_DATA, word_bits);
+		amd64_write_pci_cfg(pvt->F3, F10_NB_ARRAY_DATA, word_bits);
 
 		debugf0("section=0x%x word_bits=0x%x\n", section, word_bits);
 
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index 39d97cfdf58c..73196f7b7229 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -785,10 +785,10 @@ static int edac_create_mci_instance_attributes(struct mem_ctl_info *mci,
 {
 	int err;
 
-	debugf1("%s()\n", __func__);
+	debugf4("%s()\n", __func__);
 
 	while (sysfs_attrib) {
-		debugf1("%s() sysfs_attrib = %p\n",__func__, sysfs_attrib);
+		debugf4("%s() sysfs_attrib = %p\n",__func__, sysfs_attrib);
 		if (sysfs_attrib->grp) {
 			struct mcidev_sysfs_group_kobj *grp_kobj;
 
@@ -818,7 +818,7 @@ static int edac_create_mci_instance_attributes(struct mem_ctl_info *mci,
 			if (err < 0)
 				return err;
 		} else if (sysfs_attrib->attr.name) {
-			debugf0("%s() file %s\n", __func__,
+			debugf4("%s() file %s\n", __func__,
 				sysfs_attrib->attr.name);
 
 			err = sysfs_create_file(kobj, &sysfs_attrib->attr);
@@ -853,26 +853,26 @@ static void edac_remove_mci_instance_attributes(struct mem_ctl_info *mci,
 	 * Remove first all the atributes
 	 */
 	while (sysfs_attrib) {
-		debugf1("%s() sysfs_attrib = %p\n",__func__, sysfs_attrib);
+		debugf4("%s() sysfs_attrib = %p\n",__func__, sysfs_attrib);
 		if (sysfs_attrib->grp) {
-			debugf1("%s() seeking for group %s\n",
+			debugf4("%s() seeking for group %s\n",
 				__func__, sysfs_attrib->grp->name);
 			list_for_each_entry(grp_kobj,
 					    &mci->grp_kobj_list, list) {
-				debugf1("%s() grp_kobj->grp = %p\n",__func__, grp_kobj->grp);
+				debugf4("%s() grp_kobj->grp = %p\n",__func__, grp_kobj->grp);
 				if (grp_kobj->grp == sysfs_attrib->grp) {
 					edac_remove_mci_instance_attributes(mci,
 						    grp_kobj->grp->mcidev_attr,
 						    &grp_kobj->kobj, count + 1);
-					debugf0("%s() group %s\n", __func__,
+					debugf4("%s() group %s\n", __func__,
 						sysfs_attrib->grp->name);
 					kobject_put(&grp_kobj->kobj);
 				}
 			}
-			debugf1("%s() end of seeking for group %s\n",
+			debugf4("%s() end of seeking for group %s\n",
 				__func__, sysfs_attrib->grp->name);
 		} else if (sysfs_attrib->attr.name) {
-			debugf0("%s() file %s\n", __func__,
+			debugf4("%s() file %s\n", __func__,
 				sysfs_attrib->attr.name);
 			sysfs_remove_file(kobj, &sysfs_attrib->attr);
 		} else
@@ -979,7 +979,7 @@ void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci)
 	debugf0("%s()\n", __func__);
 
 	/* remove all csrow kobjects */
-	debugf0("%s()  unregister this mci kobj\n", __func__);
+	debugf4("%s()  unregister this mci kobj\n", __func__);
 	for (i = 0; i < mci->nr_csrows; i++) {
 		if (mci->csrows[i].nr_pages > 0) {
 			debugf0("%s()  unreg csrow-%d\n", __func__, i);
@@ -989,18 +989,18 @@ void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci)
 
 	/* remove this mci instance's attribtes */
 	if (mci->mc_driver_sysfs_attributes) {
-		debugf0("%s()  unregister mci private attributes\n", __func__);
+		debugf4("%s()  unregister mci private attributes\n", __func__);
 		edac_remove_mci_instance_attributes(mci,
 						mci->mc_driver_sysfs_attributes,
 						&mci->edac_mci_kobj, 0);
 	}
 
 	/* remove the symlink */
-	debugf0("%s()  remove_link\n", __func__);
+	debugf4("%s()  remove_link\n", __func__);
 	sysfs_remove_link(&mci->edac_mci_kobj, EDAC_DEVICE_SYMLINK);
 
 	/* unregister this instance's kobject */
-	debugf0("%s()  remove_mci_instance\n", __func__);
+	debugf4("%s()  remove_mci_instance\n", __func__);
 	kobject_put(&mci->edac_mci_kobj);
 }
 
diff --git a/drivers/edac/i7300_edac.c b/drivers/edac/i7300_edac.c
index 05523b504271..76d1f576cdc8 100644
--- a/drivers/edac/i7300_edac.c
+++ b/drivers/edac/i7300_edac.c
@@ -162,7 +162,7 @@ static struct edac_pci_ctl_info *i7300_pci;
 #define AMBPRESENT_0	0x64
 #define AMBPRESENT_1	0x66
 
-const static u16 mtr_regs[MAX_SLOTS] = {
+static const u16 mtr_regs[MAX_SLOTS] = {
 	0x80, 0x84, 0x88, 0x8c,
 	0x82, 0x86, 0x8a, 0x8e
 };
diff --git a/drivers/edac/i7core_edac.c b/drivers/edac/i7core_edac.c
index 81154ab296b6..5a92a6d7ec86 100644
--- a/drivers/edac/i7core_edac.c
+++ b/drivers/edac/i7core_edac.c
@@ -1874,7 +1874,8 @@ static void i7core_pci_ctl_create(struct i7core_pvt *pvt)
 						&pvt->i7core_dev->pdev[0]->dev,
 						EDAC_MOD_STR);
 	if (unlikely(!pvt->i7core_pci))
-		pr_warn("Unable to setup PCI error report via EDAC\n");
+		i7core_printk(KERN_WARNING,
+			      "Unable to setup PCI error report via EDAC\n");
 }
 
 static void i7core_pci_ctl_release(struct i7core_pvt *pvt)
@@ -2035,7 +2036,7 @@ fail0:
 static int __devinit i7core_probe(struct pci_dev *pdev,
 				  const struct pci_device_id *id)
 {
-	int rc;
+	int rc, count = 0;
 	struct i7core_dev *i7core_dev;
 
 	/* get the pci devices we want to reserve for our use */
@@ -2055,12 +2056,28 @@ static int __devinit i7core_probe(struct pci_dev *pdev,
 		goto fail0;
 
 	list_for_each_entry(i7core_dev, &i7core_edac_list, list) {
+		count++;
 		rc = i7core_register_mci(i7core_dev);
 		if (unlikely(rc < 0))
 			goto fail1;
 	}
 
-	i7core_printk(KERN_INFO, "Driver loaded.\n");
+	/*
+	 * Nehalem-EX uses a different memory controller. However, as the
+	 * memory controller is not visible on some Nehalem/Nehalem-EP, we
+	 * need to indirectly probe via a X58 PCI device. The same devices
+	 * are found on (some) Nehalem-EX. So, on those machines, the
+	 * probe routine needs to return -ENODEV, as the actual Memory
+	 * Controller registers won't be detected.
+	 */
+	if (!count) {
+		rc = -ENODEV;
+		goto fail1;
+	}
+
+	i7core_printk(KERN_INFO,
+		      "Driver loaded, %d memory controller(s) found.\n",
+		      count);
 
 	mutex_unlock(&i7core_edac_lock);
 	return 0;
diff --git a/drivers/edac/i82975x_edac.c b/drivers/edac/i82975x_edac.c
index 3218819b7286..92e65e7038e9 100644
--- a/drivers/edac/i82975x_edac.c
+++ b/drivers/edac/i82975x_edac.c
@@ -160,8 +160,8 @@ NOTE: Only ONE of the three must be enabled
 					 * 3:2  Rank 1 architecture
 					 * 1:0  Rank 0 architecture
 					 *
-					 * 00 => x16 devices; i.e 4 banks
-					 * 01 => x8  devices; i.e 8 banks
+					 * 00 => 4 banks
+					 * 01 => 8 banks
 					 */
 #define I82975X_C0BNKARC	0x10e
 #define I82975X_C1BNKARC	0x18e
@@ -278,6 +278,7 @@ static int i82975x_process_error_info(struct mem_ctl_info *mci,
 		struct i82975x_error_info *info, int handle_errors)
 {
 	int row, multi_chan, chan;
+	unsigned long offst, page;
 
 	multi_chan = mci->csrows[0].nr_channels - 1;
 
@@ -292,17 +293,19 @@ static int i82975x_process_error_info(struct mem_ctl_info *mci,
 		info->errsts = info->errsts2;
 	}
 
-	chan = info->eap & 1;
-	info->eap >>= 1;
-	if (info->xeap )
-		info->eap |= 0x80000000;
-	info->eap >>= PAGE_SHIFT;
-	row = edac_mc_find_csrow_by_page(mci, info->eap);
+	page = (unsigned long) info->eap;
+	if (info->xeap & 1)
+		page |= 0x100000000ul;
+	chan = page & 1;
+	page >>= 1;
+	offst = page & ((1 << PAGE_SHIFT) - 1);
+	page >>= PAGE_SHIFT;
+	row = edac_mc_find_csrow_by_page(mci, page);
 
 	if (info->errsts & 0x0002)
-		edac_mc_handle_ue(mci, info->eap, 0, row, "i82975x UE");
+		edac_mc_handle_ue(mci, page, offst , row, "i82975x UE");
 	else
-		edac_mc_handle_ce(mci, info->eap, 0, info->derrsyn, row,
+		edac_mc_handle_ce(mci, page, offst, info->derrsyn, row,
 				multi_chan ? chan : 0,
 				"i82975x CE");
 
@@ -344,11 +347,7 @@ static int dual_channel_active(void __iomem *mch_window)
 static enum dev_type i82975x_dram_type(void __iomem *mch_window, int rank)
 {
 	/*
-	 * ASUS P5W DH either does not program this register or programs
-	 * it wrong!
-	 * ECC is possible on i92975x ONLY with DEV_X8 which should mean 'val'
-	 * for each rank should be 01b - the LSB of the word should be 0x55;
-	 * but it reads 0!
+	 * ECC is possible on i92975x ONLY with DEV_X8
 	 */
 	return DEV_X8;
 }
@@ -356,11 +355,15 @@ static enum dev_type i82975x_dram_type(void __iomem *mch_window, int rank)
 static void i82975x_init_csrows(struct mem_ctl_info *mci,
 		struct pci_dev *pdev, void __iomem *mch_window)
 {
+	static const char *labels[4] = {
+							"DIMM A1", "DIMM A2",
+							"DIMM B1", "DIMM B2"
+						};
 	struct csrow_info *csrow;
 	unsigned long last_cumul_size;
 	u8 value;
 	u32 cumul_size;
-	int index;
+	int index, chan;
 
 	last_cumul_size = 0;
 
@@ -369,11 +372,7 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 	 * The dram row boundary (DRB) reg values are boundary address
 	 * for each DRAM row with a granularity of 32 or 64MB (single/dual
 	 * channel operation).  DRB regs are cumulative; therefore DRB7 will
-	 * contain the total memory contained in all eight rows.
-	 *
-	 * FIXME:
-	 *  EDAC currently works for Dual-channel Interleaved configuration.
-	 *  Other configurations, which the chip supports, need fixing/testing.
+	 * contain the total memory contained in all rows.
 	 *
 	 */
 
@@ -384,8 +383,26 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 					((index >= 4) ? 0x80 : 0));
 		cumul_size = value;
 		cumul_size <<= (I82975X_DRB_SHIFT - PAGE_SHIFT);
+		/*
+		 * Adjust cumul_size w.r.t number of channels
+		 *
+		 */
+		if (csrow->nr_channels > 1)
+			cumul_size <<= 1;
 		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
 			cumul_size);
+
+		/*
+		 * Initialise dram labels
+		 * index values:
+		 *   [0-7] for single-channel; i.e. csrow->nr_channels = 1
+		 *   [0-3] for dual-channel; i.e. csrow->nr_channels = 2
+		 */
+		for (chan = 0; chan < csrow->nr_channels; chan++)
+			strncpy(csrow->channels[chan].label,
+					labels[(index >> 1) + (chan * 2)],
+					EDAC_MC_LABEL_LEN);
+
 		if (cumul_size == last_cumul_size)
 			continue;	/* not populated */
 
@@ -393,8 +410,8 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 		csrow->last_page = cumul_size - 1;
 		csrow->nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
-		csrow->grain = 1 << 7;	/* I82975X_EAP has 128B resolution */
-		csrow->mtype = MEM_DDR; /* i82975x supports only DDR2 */
+		csrow->grain = 1 << 6;	/* I82975X_EAP has 64B resolution */
+		csrow->mtype = MEM_DDR2; /* I82975x supports only DDR2 */
 		csrow->dtype = i82975x_dram_type(mch_window, index);
 		csrow->edac_mode = EDAC_SECDED; /* only supported */
 	}
@@ -515,18 +532,20 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 
 	debugf3("%s(): init mci\n", __func__);
 	mci->dev = &pdev->dev;
-	mci->mtype_cap = MEM_FLAG_DDR;
+	mci->mtype_cap = MEM_FLAG_DDR2;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
 	mci->edac_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
 	mci->mod_name = EDAC_MOD_STR;
 	mci->mod_ver = I82975X_REVISION;
 	mci->ctl_name = i82975x_devs[dev_idx].ctl_name;
+	mci->dev_name = pci_name(pdev);
 	mci->edac_check = i82975x_check;
 	mci->ctl_page_to_phys = NULL;
 	debugf3("%s(): init pvt\n", __func__);
 	pvt = (struct i82975x_pvt *) mci->pvt_info;
 	pvt->mch_window = mch_window;
 	i82975x_init_csrows(mci, pdev, mch_window);
+	mci->scrub_mode = SCRUB_HW_SRC;
 	i82975x_get_error_info(mci, &discard);  /* clear counters */
 
 	/* finalize this instance of memory controller with edac core */
@@ -664,7 +683,7 @@ module_init(i82975x_init);
 module_exit(i82975x_exit);
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Arvind R. <arvind@acarlab.com>");
+MODULE_AUTHOR("Arvind R. <arvino55@gmail.com>");
 MODULE_DESCRIPTION("MC support for Intel 82975 memory hub controllers");
 
 module_param(edac_op_state, int, 0444);
diff --git a/drivers/edac/mce_amd.c b/drivers/edac/mce_amd.c
index f6cf73d93359..795cfbc0bf50 100644
--- a/drivers/edac/mce_amd.c
+++ b/drivers/edac/mce_amd.c
@@ -594,6 +594,7 @@ static bool nb_noop_mce(u16 ec, u8 xec)
 
 void amd_decode_nb_mce(int node_id, struct mce *m, u32 nbcfg)
 {
+	struct cpuinfo_x86 *c = &boot_cpu_data;
 	u16 ec   = EC(m->status);
 	u8 xec   = XEC(m->status, 0x1f);
 	u32 nbsh = (u32)(m->status >> 32);
@@ -602,9 +603,8 @@ void amd_decode_nb_mce(int node_id, struct mce *m, u32 nbcfg)
 	pr_emerg(HW_ERR "Northbridge Error (node %d", node_id);
 
 	/* F10h, revD can disable ErrCpu[3:0] through ErrCpuVal */
-	if ((boot_cpu_data.x86 == 0x10) &&
-	    (boot_cpu_data.x86_model > 7)) {
-		if (nbsh & K8_NBSH_ERR_CPU_VAL)
+	if (c->x86 == 0x10 && c->x86_model > 7) {
+		if (nbsh & NBSH_ERR_CPU_VAL)
 			core = nbsh & nb_err_cpumask;
 	} else {
 		u8 assoc_cpus = nbsh & nb_err_cpumask;
@@ -646,7 +646,7 @@ void amd_decode_nb_mce(int node_id, struct mce *m, u32 nbcfg)
 	if (!fam_ops->nb_mce(ec, xec))
 		goto wrong_nb_mce;
 
-	if (boot_cpu_data.x86 == 0xf || boot_cpu_data.x86 == 0x10)
+	if (c->x86 == 0xf || c->x86 == 0x10 || c->x86 == 0x15)
 		if ((xec == 0x8 || xec == 0x0) && nb_bus_decoder)
 			nb_bus_decoder(node_id, m, nbcfg);
 
diff --git a/drivers/edac/mce_amd.h b/drivers/edac/mce_amd.h
index 45dda47173f2..795a3206acf5 100644
--- a/drivers/edac/mce_amd.h
+++ b/drivers/edac/mce_amd.h
@@ -31,19 +31,10 @@
 #define R4(x)				(((x) >> 4) & 0xf)
 #define R4_MSG(x)			((R4(x) < 9) ?  rrrr_msgs[R4(x)] : "Wrong R4!")
 
-#define K8_NBSH				0x4C
-
-#define K8_NBSH_VALID_BIT		BIT(31)
-#define K8_NBSH_OVERFLOW		BIT(30)
-#define K8_NBSH_UC_ERR			BIT(29)
-#define K8_NBSH_ERR_EN			BIT(28)
-#define K8_NBSH_MISCV			BIT(27)
-#define K8_NBSH_VALID_ERROR_ADDR	BIT(26)
-#define K8_NBSH_PCC			BIT(25)
-#define K8_NBSH_ERR_CPU_VAL		BIT(24)
-#define K8_NBSH_CECC			BIT(14)
-#define K8_NBSH_UECC			BIT(13)
-#define K8_NBSH_ERR_SCRUBER		BIT(8)
+/*
+ * F3x4C bits (MCi_STATUS' high half)
+ */
+#define NBSH_ERR_CPU_VAL		BIT(24)
 
 enum tt_ids {
 	TT_INSTR = 0,
@@ -86,17 +77,6 @@ extern const char *to_msgs[];
 extern const char *ii_msgs[];
 
 /*
- * relevant NB regs
- */
-struct err_regs {
-	u32 nbcfg;
-	u32 nbsh;
-	u32 nbsl;
-	u32 nbeah;
-	u32 nbeal;
-};
-
-/*
  * per-family decoder ops
  */
 struct amd_decoder_ops {
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index b123bb308a4a..ffb5ad080bee 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -200,8 +200,7 @@ static irqreturn_t mpc85xx_pci_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int __devinit mpc85xx_pci_err_probe(struct platform_device *op,
-					   const struct of_device_id *match)
+static int __devinit mpc85xx_pci_err_probe(struct platform_device *op)
 {
 	struct edac_pci_ctl_info *pci;
 	struct mpc85xx_pci_pdata *pdata;
@@ -338,7 +337,7 @@ static struct of_device_id mpc85xx_pci_err_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mpc85xx_pci_err_of_match);
 
-static struct of_platform_driver mpc85xx_pci_err_driver = {
+static struct platform_driver mpc85xx_pci_err_driver = {
 	.probe = mpc85xx_pci_err_probe,
 	.remove = __devexit_p(mpc85xx_pci_err_remove),
 	.driver = {
@@ -503,8 +502,7 @@ static irqreturn_t mpc85xx_l2_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int __devinit mpc85xx_l2_err_probe(struct platform_device *op,
-					  const struct of_device_id *match)
+static int __devinit mpc85xx_l2_err_probe(struct platform_device *op)
 {
 	struct edac_device_ctl_info *edac_dev;
 	struct mpc85xx_l2_pdata *pdata;
@@ -656,7 +654,7 @@ static struct of_device_id mpc85xx_l2_err_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mpc85xx_l2_err_of_match);
 
-static struct of_platform_driver mpc85xx_l2_err_driver = {
+static struct platform_driver mpc85xx_l2_err_driver = {
 	.probe = mpc85xx_l2_err_probe,
 	.remove = mpc85xx_l2_err_remove,
 	.driver = {
@@ -956,8 +954,7 @@ static void __devinit mpc85xx_init_csrows(struct mem_ctl_info *mci)
 	}
 }
 
-static int __devinit mpc85xx_mc_err_probe(struct platform_device *op,
-					  const struct of_device_id *match)
+static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 {
 	struct mem_ctl_info *mci;
 	struct mpc85xx_mc_pdata *pdata;
@@ -1136,7 +1133,7 @@ static struct of_device_id mpc85xx_mc_err_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mpc85xx_mc_err_of_match);
 
-static struct of_platform_driver mpc85xx_mc_err_driver = {
+static struct platform_driver mpc85xx_mc_err_driver = {
 	.probe = mpc85xx_mc_err_probe,
 	.remove = mpc85xx_mc_err_remove,
 	.driver = {
@@ -1171,16 +1168,16 @@ static int __init mpc85xx_mc_init(void)
 		break;
 	}
 
-	res = of_register_platform_driver(&mpc85xx_mc_err_driver);
+	res = platform_driver_register(&mpc85xx_mc_err_driver);
 	if (res)
 		printk(KERN_WARNING EDAC_MOD_STR "MC fails to register\n");
 
-	res = of_register_platform_driver(&mpc85xx_l2_err_driver);
+	res = platform_driver_register(&mpc85xx_l2_err_driver);
 	if (res)
 		printk(KERN_WARNING EDAC_MOD_STR "L2 fails to register\n");
 
 #ifdef CONFIG_PCI
-	res = of_register_platform_driver(&mpc85xx_pci_err_driver);
+	res = platform_driver_register(&mpc85xx_pci_err_driver);
 	if (res)
 		printk(KERN_WARNING EDAC_MOD_STR "PCI fails to register\n");
 #endif
@@ -1212,10 +1209,10 @@ static void __exit mpc85xx_mc_exit(void)
 	on_each_cpu(mpc85xx_mc_restore_hid1, NULL, 0);
 #endif
 #ifdef CONFIG_PCI
-	of_unregister_platform_driver(&mpc85xx_pci_err_driver);
+	platform_driver_unregister(&mpc85xx_pci_err_driver);
 #endif
-	of_unregister_platform_driver(&mpc85xx_l2_err_driver);
-	of_unregister_platform_driver(&mpc85xx_mc_err_driver);
+	platform_driver_unregister(&mpc85xx_l2_err_driver);
+	platform_driver_unregister(&mpc85xx_mc_err_driver);
 }
 
 module_exit(mpc85xx_mc_exit);
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index b9f0c20df1aa..c1f0045ceb8e 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -184,8 +184,7 @@ struct ppc4xx_ecc_status {
 
 /* Function Prototypes */
 
-static int ppc4xx_edac_probe(struct platform_device *device,
-			     const struct of_device_id *device_id);
+static int ppc4xx_edac_probe(struct platform_device *device)
 static int ppc4xx_edac_remove(struct platform_device *device);
 
 /* Global Variables */
@@ -201,7 +200,7 @@ static struct of_device_id ppc4xx_edac_match[] = {
 	{ }
 };
 
-static struct of_platform_driver ppc4xx_edac_driver = {
+static struct platform_driver ppc4xx_edac_driver = {
 	.probe			= ppc4xx_edac_probe,
 	.remove			= ppc4xx_edac_remove,
 	.driver = {
@@ -997,9 +996,6 @@ ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
  *       initialized.
  * @op: A pointer to the OpenFirmware device tree node associated
  *      with the controller this EDAC instance is bound to.
- * @match: A pointer to the OpenFirmware device tree match
- *         information associated with the controller this EDAC instance
- *         is bound to.
  * @dcr_host: A pointer to the DCR data containing the DCR mapping
  *            for this controller instance.
  * @mcopt1: The 32-bit Memory Controller Option 1 register value
@@ -1015,7 +1011,6 @@ ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
 static int __devinit
 ppc4xx_edac_mc_init(struct mem_ctl_info *mci,
 		    struct platform_device *op,
-		    const struct of_device_id *match,
 		    const dcr_host_t *dcr_host,
 		    u32 mcopt1)
 {
@@ -1024,7 +1019,7 @@ ppc4xx_edac_mc_init(struct mem_ctl_info *mci,
 	struct ppc4xx_edac_pdata *pdata = NULL;
 	const struct device_node *np = op->dev.of_node;
 
-	if (match == NULL)
+	if (op->dev.of_match == NULL)
 		return -EINVAL;
 
 	/* Initial driver pointers and private data */
@@ -1227,9 +1222,6 @@ ppc4xx_edac_map_dcrs(const struct device_node *np, dcr_host_t *dcr_host)
  * ppc4xx_edac_probe - check controller and bind driver
  * @op: A pointer to the OpenFirmware device tree node associated
  *      with the controller being probed for driver binding.
- * @match: A pointer to the OpenFirmware device tree match
- *         information associated with the controller being probed
- *         for driver binding.
  *
  * This routine probes a specific ibm,sdram-4xx-ddr2 controller
  * instance for binding with the driver.
@@ -1237,8 +1229,7 @@ ppc4xx_edac_map_dcrs(const struct device_node *np, dcr_host_t *dcr_host)
  * Returns 0 if the controller instance was successfully bound to the
  * driver; otherwise, < 0 on error.
  */
-static int __devinit
-ppc4xx_edac_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit ppc4xx_edac_probe(struct platform_device *op)
 {
 	int status = 0;
 	u32 mcopt1, memcheck;
@@ -1304,7 +1295,7 @@ ppc4xx_edac_probe(struct platform_device *op, const struct of_device_id *match)
 		goto done;
 	}
 
-	status = ppc4xx_edac_mc_init(mci, op, match, &dcr_host, mcopt1);
+	status = ppc4xx_edac_mc_init(mci, op, &dcr_host, mcopt1);
 
 	if (status) {
 		ppc4xx_edac_mc_printk(KERN_ERR, mci,
@@ -1421,7 +1412,7 @@ ppc4xx_edac_init(void)
 
 	ppc4xx_edac_opstate_init();
 
-	return of_register_platform_driver(&ppc4xx_edac_driver);
+	return platform_driver_register(&ppc4xx_edac_driver);
 }
 
 /**
@@ -1434,7 +1425,7 @@ ppc4xx_edac_init(void)
 static void __exit
 ppc4xx_edac_exit(void)
 {
-	of_unregister_platform_driver(&ppc4xx_edac_driver);
+	platform_driver_unregister(&ppc4xx_edac_driver);
 }
 
 module_init(ppc4xx_edac_init);
diff --git a/drivers/firewire/Kconfig b/drivers/firewire/Kconfig
index 0c56989cd907..2be6f4520772 100644
--- a/drivers/firewire/Kconfig
+++ b/drivers/firewire/Kconfig
@@ -75,7 +75,8 @@ config FIREWIRE_NOSY
 	  The following cards are known to be based on PCILynx or PCILynx-2:
 	  IOI IOI-1394TT (PCI card), Unibrain Fireboard 400 PCI Lynx-2
 	  (PCI card), Newer Technology FireWire 2 Go (CardBus card),
-	  Apple Power Mac G3 blue & white (onboard controller).
+	  Apple Power Mac G3 blue & white and G4 with PCI graphics
+	  (onboard controller).
 
 	  To compile this driver as a module, say M here:  The module will be
 	  called nosy.  Source code of a userspace interface to nosy, called
diff --git a/drivers/firewire/core-card.c b/drivers/firewire/core-card.c
index 24ff35511e2b..3241dc4e1fc9 100644
--- a/drivers/firewire/core-card.c
+++ b/drivers/firewire/core-card.c
@@ -233,7 +233,7 @@ static void br_work(struct work_struct *work)
 
 	/* Delay for 2s after last reset per IEEE 1394 clause 8.2.1. */
 	if (card->reset_jiffies != 0 &&
-	    time_is_after_jiffies(card->reset_jiffies + 2 * HZ)) {
+	    time_before64(get_jiffies_64(), card->reset_jiffies + 2 * HZ)) {
 		if (!schedule_delayed_work(&card->br_work, 2 * HZ))
 			fw_card_put(card);
 		return;
@@ -316,7 +316,8 @@ static void bm_work(struct work_struct *work)
 	irm_id   = card->irm_node->node_id;
 	local_id = card->local_node->node_id;
 
-	grace = time_after(jiffies, card->reset_jiffies + DIV_ROUND_UP(HZ, 8));
+	grace = time_after64(get_jiffies_64(),
+			     card->reset_jiffies + DIV_ROUND_UP(HZ, 8));
 
 	if ((is_next_generation(generation, card->bm_generation) &&
 	     !card->bm_abdicate) ||
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 48ae712e2101..62ac111af243 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -64,6 +64,7 @@ struct client {
 	struct idr resource_idr;
 	struct list_head event_list;
 	wait_queue_head_t wait;
+	wait_queue_head_t tx_flush_wait;
 	u64 bus_reset_closure;
 
 	struct fw_iso_context *iso_context;
@@ -251,6 +252,7 @@ static int fw_device_op_open(struct inode *inode, struct file *file)
 	idr_init(&client->resource_idr);
 	INIT_LIST_HEAD(&client->event_list);
 	init_waitqueue_head(&client->wait);
+	init_waitqueue_head(&client->tx_flush_wait);
 	INIT_LIST_HEAD(&client->phy_receiver_link);
 	kref_init(&client->kref);
 
@@ -520,10 +522,6 @@ static int release_client_resource(struct client *client, u32 handle,
 static void release_transaction(struct client *client,
 				struct client_resource *resource)
 {
-	struct outbound_transaction_resource *r = container_of(resource,
-			struct outbound_transaction_resource, resource);
-
-	fw_cancel_transaction(client->device->card, &r->transaction);
 }
 
 static void complete_transaction(struct fw_card *card, int rcode,
@@ -540,22 +538,9 @@ static void complete_transaction(struct fw_card *card, int rcode,
 		memcpy(rsp->data, payload, rsp->length);
 
 	spin_lock_irqsave(&client->lock, flags);
-	/*
-	 * 1. If called while in shutdown, the idr tree must be left untouched.
-	 *    The idr handle will be removed and the client reference will be
-	 *    dropped later.
-	 * 2. If the call chain was release_client_resource ->
-	 *    release_transaction -> complete_transaction (instead of a normal
-	 *    conclusion of the transaction), i.e. if this resource was already
-	 *    unregistered from the idr, the client reference will be dropped
-	 *    by release_client_resource and we must not drop it here.
-	 */
-	if (!client->in_shutdown &&
-	    idr_find(&client->resource_idr, e->r.resource.handle)) {
-		idr_remove(&client->resource_idr, e->r.resource.handle);
-		/* Drop the idr's reference */
-		client_put(client);
-	}
+	idr_remove(&client->resource_idr, e->r.resource.handle);
+	if (client->in_shutdown)
+		wake_up(&client->tx_flush_wait);
 	spin_unlock_irqrestore(&client->lock, flags);
 
 	rsp->type = FW_CDEV_EVENT_RESPONSE;
@@ -575,7 +560,7 @@ static void complete_transaction(struct fw_card *card, int rcode,
 		queue_event(client, &e->event, rsp, sizeof(*rsp) + rsp->length,
 			    NULL, 0);
 
-	/* Drop the transaction callback's reference */
+	/* Drop the idr's reference */
 	client_put(client);
 }
 
@@ -614,9 +599,6 @@ static int init_request(struct client *client,
 	if (ret < 0)
 		goto failed;
 
-	/* Get a reference for the transaction callback */
-	client_get(client);
-
 	fw_send_request(client->device->card, &e->r.transaction,
 			request->tcode, destination_id, request->generation,
 			speed, request->offset, e->response.data,
@@ -1223,7 +1205,8 @@ static void iso_resource_work(struct work_struct *work)
 	todo = r->todo;
 	/* Allow 1000ms grace period for other reallocations. */
 	if (todo == ISO_RES_ALLOC &&
-	    time_is_after_jiffies(client->device->card->reset_jiffies + HZ)) {
+	    time_before64(get_jiffies_64(),
+			  client->device->card->reset_jiffies + HZ)) {
 		schedule_iso_resource(r, DIV_ROUND_UP(HZ, 3));
 		skip = true;
 	} else {
@@ -1678,6 +1661,25 @@ static int fw_device_op_mmap(struct file *file, struct vm_area_struct *vma)
 	return ret;
 }
 
+static int is_outbound_transaction_resource(int id, void *p, void *data)
+{
+	struct client_resource *resource = p;
+
+	return resource->release == release_transaction;
+}
+
+static int has_outbound_transactions(struct client *client)
+{
+	int ret;
+
+	spin_lock_irq(&client->lock);
+	ret = idr_for_each(&client->resource_idr,
+			   is_outbound_transaction_resource, NULL);
+	spin_unlock_irq(&client->lock);
+
+	return ret;
+}
+
 static int shutdown_resource(int id, void *p, void *data)
 {
 	struct client_resource *resource = p;
@@ -1713,6 +1715,8 @@ static int fw_device_op_release(struct inode *inode, struct file *file)
 	client->in_shutdown = true;
 	spin_unlock_irq(&client->lock);
 
+	wait_event(client->tx_flush_wait, !has_outbound_transactions(client));
+
 	idr_for_each(&client->resource_idr, shutdown_resource, client);
 	idr_remove_all(&client->resource_idr);
 	idr_destroy(&client->resource_idr);
diff --git a/drivers/firewire/core-device.c b/drivers/firewire/core-device.c
index 6113b896e790..57461923bacf 100644
--- a/drivers/firewire/core-device.c
+++ b/drivers/firewire/core-device.c
@@ -747,7 +747,8 @@ static void fw_device_shutdown(struct work_struct *work)
 		container_of(work, struct fw_device, work.work);
 	int minor = MINOR(device->device.devt);
 
-	if (time_is_after_jiffies(device->card->reset_jiffies + SHUTDOWN_DELAY)
+	if (time_before64(get_jiffies_64(),
+			  device->card->reset_jiffies + SHUTDOWN_DELAY)
 	    && !list_empty(&device->card->link)) {
 		schedule_delayed_work(&device->work, SHUTDOWN_DELAY);
 		return;
diff --git a/drivers/firewire/core-iso.c b/drivers/firewire/core-iso.c
index c003fa4e2db1..95bc01a02a88 100644
--- a/drivers/firewire/core-iso.c
+++ b/drivers/firewire/core-iso.c
@@ -235,45 +235,45 @@ static int manage_bandwidth(struct fw_card *card, int irm_id, int generation,
 static int manage_channel(struct fw_card *card, int irm_id, int generation,
 		u32 channels_mask, u64 offset, bool allocate, __be32 data[2])
 {
-	__be32 c, all, old;
-	int i, ret = -EIO, retry = 5;
+	__be32 bit, all, old;
+	int channel, ret = -EIO, retry = 5;
 
 	old = all = allocate ? cpu_to_be32(~0) : 0;
 
-	for (i = 0; i < 32; i++) {
-		if (!(channels_mask & 1 << i))
+	for (channel = 0; channel < 32; channel++) {
+		if (!(channels_mask & 1 << channel))
 			continue;
 
 		ret = -EBUSY;
 
-		c = cpu_to_be32(1 << (31 - i));
-		if ((old & c) != (all & c))
+		bit = cpu_to_be32(1 << (31 - channel));
+		if ((old & bit) != (all & bit))
 			continue;
 
 		data[0] = old;
-		data[1] = old ^ c;
+		data[1] = old ^ bit;
 		switch (fw_run_transaction(card, TCODE_LOCK_COMPARE_SWAP,
 					   irm_id, generation, SCODE_100,
 					   offset, data, 8)) {
 		case RCODE_GENERATION:
 			/* A generation change frees all channels. */
-			return allocate ? -EAGAIN : i;
+			return allocate ? -EAGAIN : channel;
 
 		case RCODE_COMPLETE:
 			if (data[0] == old)
-				return i;
+				return channel;
 
 			old = data[0];
 
 			/* Is the IRM 1394a-2000 compliant? */
-			if ((data[0] & c) == (data[1] & c))
+			if ((data[0] & bit) == (data[1] & bit))
 				continue;
 
 			/* 1394-1995 IRM, fall through to retry. */
 		default:
 			if (retry) {
 				retry--;
-				i--;
+				channel--;
 			} else {
 				ret = -EIO;
 			}
diff --git a/drivers/firewire/core-topology.c b/drivers/firewire/core-topology.c
index 09be1a635505..193ed9233144 100644
--- a/drivers/firewire/core-topology.c
+++ b/drivers/firewire/core-topology.c
@@ -545,7 +545,7 @@ void fw_core_handle_bus_reset(struct fw_card *card, int node_id, int generation,
 	 */
 	smp_wmb();
 	card->generation = generation;
-	card->reset_jiffies = jiffies;
+	card->reset_jiffies = get_jiffies_64();
 	card->bm_node_id  = 0xffff;
 	card->bm_abdicate = bm_abdicate;
 	fw_schedule_bm_work(card, 0);
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index bd3c61b6dd8d..c0572283b93e 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -208,9 +208,11 @@ struct fw_ohci {
 	struct context at_request_ctx;
 	struct context at_response_ctx;
 
+	u32 it_context_support;
 	u32 it_context_mask;     /* unoccupied IT contexts */
 	struct iso_context *it_context_list;
 	u64 ir_context_channels; /* unoccupied channels */
+	u32 ir_context_support;
 	u32 ir_context_mask;     /* unoccupied IR contexts */
 	struct iso_context *ir_context_list;
 	u64 mc_channels; /* channels in use by the multichannel IR context */
@@ -338,7 +340,7 @@ static void log_irqs(u32 evt)
 	    !(evt & OHCI1394_busReset))
 		return;
 
-	fw_notify("IRQ %08x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", evt,
+	fw_notify("IRQ %08x%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", evt,
 	    evt & OHCI1394_selfIDComplete	? " selfID"		: "",
 	    evt & OHCI1394_RQPkt		? " AR_req"		: "",
 	    evt & OHCI1394_RSPkt		? " AR_resp"		: "",
@@ -351,6 +353,7 @@ static void log_irqs(u32 evt)
 	    evt & OHCI1394_cycle64Seconds	? " cycle64Seconds"	: "",
 	    evt & OHCI1394_cycleInconsistent	? " cycleInconsistent"	: "",
 	    evt & OHCI1394_regAccessFail	? " regAccessFail"	: "",
+	    evt & OHCI1394_unrecoverableError	? " unrecoverableError"	: "",
 	    evt & OHCI1394_busReset		? " busReset"		: "",
 	    evt & ~(OHCI1394_selfIDComplete | OHCI1394_RQPkt |
 		    OHCI1394_RSPkt | OHCI1394_reqTxComplete |
@@ -1590,6 +1593,47 @@ static void at_context_transmit(struct context *ctx, struct fw_packet *packet)
 
 }
 
+static void detect_dead_context(struct fw_ohci *ohci,
+				const char *name, unsigned int regs)
+{
+	u32 ctl;
+
+	ctl = reg_read(ohci, CONTROL_SET(regs));
+	if (ctl & CONTEXT_DEAD) {
+#ifdef CONFIG_FIREWIRE_OHCI_DEBUG
+		fw_error("DMA context %s has stopped, error code: %s\n",
+			 name, evts[ctl & 0x1f]);
+#else
+		fw_error("DMA context %s has stopped, error code: %#x\n",
+			 name, ctl & 0x1f);
+#endif
+	}
+}
+
+static void handle_dead_contexts(struct fw_ohci *ohci)
+{
+	unsigned int i;
+	char name[8];
+
+	detect_dead_context(ohci, "ATReq", OHCI1394_AsReqTrContextBase);
+	detect_dead_context(ohci, "ATRsp", OHCI1394_AsRspTrContextBase);
+	detect_dead_context(ohci, "ARReq", OHCI1394_AsReqRcvContextBase);
+	detect_dead_context(ohci, "ARRsp", OHCI1394_AsRspRcvContextBase);
+	for (i = 0; i < 32; ++i) {
+		if (!(ohci->it_context_support & (1 << i)))
+			continue;
+		sprintf(name, "IT%u", i);
+		detect_dead_context(ohci, name, OHCI1394_IsoXmitContextBase(i));
+	}
+	for (i = 0; i < 32; ++i) {
+		if (!(ohci->ir_context_support & (1 << i)))
+			continue;
+		sprintf(name, "IR%u", i);
+		detect_dead_context(ohci, name, OHCI1394_IsoRcvContextBase(i));
+	}
+	/* TODO: maybe try to flush and restart the dead contexts */
+}
+
 static u32 cycle_timer_ticks(u32 cycle_timer)
 {
 	u32 ticks;
@@ -1904,6 +1948,9 @@ static irqreturn_t irq_handler(int irq, void *data)
 			fw_notify("isochronous cycle inconsistent\n");
 	}
 
+	if (unlikely(event & OHCI1394_unrecoverableError))
+		handle_dead_contexts(ohci);
+
 	if (event & OHCI1394_cycle64Seconds) {
 		spin_lock(&ohci->lock);
 		update_bus_time(ohci);
@@ -2141,7 +2188,9 @@ static int ohci_enable(struct fw_card *card,
 		OHCI1394_selfIDComplete |
 		OHCI1394_regAccessFail |
 		OHCI1394_cycle64Seconds |
-		OHCI1394_cycleInconsistent | OHCI1394_cycleTooLong |
+		OHCI1394_cycleInconsistent |
+		OHCI1394_unrecoverableError |
+		OHCI1394_cycleTooLong |
 		OHCI1394_masterIntEnable;
 	if (param_debug & OHCI_PARAM_DEBUG_BUSRESETS)
 		irqs |= OHCI1394_busReset;
@@ -2657,6 +2706,10 @@ static int ohci_start_iso(struct fw_iso_context *base,
 	u32 control = IR_CONTEXT_ISOCH_HEADER, match;
 	int index;
 
+	/* the controller cannot start without any queued packets */
+	if (ctx->context.last->branch_address == 0)
+		return -ENODATA;
+
 	switch (ctx->base.type) {
 	case FW_ISO_CONTEXT_TRANSMIT:
 		index = ctx - ohci->it_context_list;
@@ -2715,6 +2768,7 @@ static int ohci_stop_iso(struct fw_iso_context *base)
 	}
 	flush_writes(ohci);
 	context_stop(&ctx->context);
+	tasklet_kill(&ctx->context.tasklet);
 
 	return 0;
 }
@@ -3207,15 +3261,17 @@ static int __devinit pci_probe(struct pci_dev *dev,
 
 	reg_write(ohci, OHCI1394_IsoRecvIntMaskSet, ~0);
 	ohci->ir_context_channels = ~0ULL;
-	ohci->ir_context_mask = reg_read(ohci, OHCI1394_IsoRecvIntMaskSet);
+	ohci->ir_context_support = reg_read(ohci, OHCI1394_IsoRecvIntMaskSet);
 	reg_write(ohci, OHCI1394_IsoRecvIntMaskClear, ~0);
+	ohci->ir_context_mask = ohci->ir_context_support;
 	ohci->n_ir = hweight32(ohci->ir_context_mask);
 	size = sizeof(struct iso_context) * ohci->n_ir;
 	ohci->ir_context_list = kzalloc(size, GFP_KERNEL);
 
 	reg_write(ohci, OHCI1394_IsoXmitIntMaskSet, ~0);
-	ohci->it_context_mask = reg_read(ohci, OHCI1394_IsoXmitIntMaskSet);
+	ohci->it_context_support = reg_read(ohci, OHCI1394_IsoXmitIntMaskSet);
 	reg_write(ohci, OHCI1394_IsoXmitIntMaskClear, ~0);
+	ohci->it_context_mask = ohci->it_context_support;
 	ohci->n_it = hweight32(ohci->it_context_mask);
 	size = sizeof(struct iso_context) * ohci->n_it;
 	ohci->it_context_list = kzalloc(size, GFP_KERNEL);
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index e710424b59ea..3c56afc5eb1b 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -113,6 +113,17 @@ config DMIID
 	  information from userspace through /sys/class/dmi/id/ or if you want
 	  DMI-based module auto-loading.
 
+config DMI_SYSFS
+	tristate "DMI table support in sysfs"
+	depends on SYSFS && DMI
+	default n
+	help
+	  Say Y or M here to enable the exporting of the raw DMI table
+	  data via sysfs.  This is useful for consuming the data without
+	  requiring any access to /dev/mem at all.  Tables are found
+	  under /sys/firmware/dmi when this option is enabled and
+	  loaded.
+
 config ISCSI_IBFT_FIND
 	bool "iSCSI Boot Firmware Table Attributes"
 	depends on X86
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index 1c3c17343dbe..20c17fca1232 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -2,6 +2,7 @@
 # Makefile for the linux kernel.
 #
 obj-$(CONFIG_DMI)		+= dmi_scan.o
+obj-$(CONFIG_DMI_SYSFS)		+= dmi-sysfs.o
 obj-$(CONFIG_EDD)		+= edd.o
 obj-$(CONFIG_EFI_VARS)		+= efivars.o
 obj-$(CONFIG_EFI_PCDP)		+= pcdp.o
diff --git a/drivers/firmware/dcdbas.c b/drivers/firmware/dcdbas.c
index 69ad529d92fb..ea5ac2dc1233 100644
--- a/drivers/firmware/dcdbas.c
+++ b/drivers/firmware/dcdbas.c
@@ -268,8 +268,10 @@ int dcdbas_smi_request(struct smi_cmd *smi_cmd)
 	}
 
 	/* generate SMI */
+	/* inb to force posted write through and make SMI happen now */
 	asm volatile (
-		"outb %b0,%w1"
+		"outb %b0,%w1\n"
+		"inb %w1"
 		: /* no output args */
 		: "a" (smi_cmd->command_code),
 		  "d" (smi_cmd->command_address),
diff --git a/drivers/firmware/dmi-sysfs.c b/drivers/firmware/dmi-sysfs.c
new file mode 100644
index 000000000000..eb26d62e5188
--- /dev/null
+++ b/drivers/firmware/dmi-sysfs.c
@@ -0,0 +1,696 @@
+/*
+ * dmi-sysfs.c
+ *
+ * This module exports the DMI tables read-only to userspace through the
+ * sysfs file system.
+ *
+ * Data is currently found below
+ *    /sys/firmware/dmi/...
+ *
+ * DMI attributes are presented in attribute files with names
+ * formatted using %d-%d, so that the first integer indicates the
+ * structure type (0-255), and the second field is the instance of that
+ * entry.
+ *
+ * Copyright 2011 Google, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kobject.h>
+#include <linux/dmi.h>
+#include <linux/capability.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/io.h>
+
+#define MAX_ENTRY_TYPE 255 /* Most of these aren't used, but we consider
+			      the top entry type is only 8 bits */
+
+struct dmi_sysfs_entry {
+	struct dmi_header dh;
+	struct kobject kobj;
+	int instance;
+	int position;
+	struct list_head list;
+	struct kobject *child;
+};
+
+/*
+ * Global list of dmi_sysfs_entry.  Even though this should only be
+ * manipulated at setup and teardown, the lazy nature of the kobject
+ * system means we get lazy removes.
+ */
+static LIST_HEAD(entry_list);
+static DEFINE_SPINLOCK(entry_list_lock);
+
+/* dmi_sysfs_attribute - Top level attribute. used by all entries. */
+struct dmi_sysfs_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct dmi_sysfs_entry *entry, char *buf);
+};
+
+#define DMI_SYSFS_ATTR(_entry, _name) \
+struct dmi_sysfs_attribute dmi_sysfs_attr_##_entry##_##_name = { \
+	.attr = {.name = __stringify(_name), .mode = 0400}, \
+	.show = dmi_sysfs_##_entry##_##_name, \
+}
+
+/*
+ * dmi_sysfs_mapped_attribute - Attribute where we require the entry be
+ * mapped in.  Use in conjunction with dmi_sysfs_specialize_attr_ops.
+ */
+struct dmi_sysfs_mapped_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct dmi_sysfs_entry *entry,
+			const struct dmi_header *dh,
+			char *buf);
+};
+
+#define DMI_SYSFS_MAPPED_ATTR(_entry, _name) \
+struct dmi_sysfs_mapped_attribute dmi_sysfs_attr_##_entry##_##_name = { \
+	.attr = {.name = __stringify(_name), .mode = 0400}, \
+	.show = dmi_sysfs_##_entry##_##_name, \
+}
+
+/*************************************************
+ * Generic DMI entry support.
+ *************************************************/
+static void dmi_entry_free(struct kobject *kobj)
+{
+	kfree(kobj);
+}
+
+static struct dmi_sysfs_entry *to_entry(struct kobject *kobj)
+{
+	return container_of(kobj, struct dmi_sysfs_entry, kobj);
+}
+
+static struct dmi_sysfs_attribute *to_attr(struct attribute *attr)
+{
+	return container_of(attr, struct dmi_sysfs_attribute, attr);
+}
+
+static ssize_t dmi_sysfs_attr_show(struct kobject *kobj,
+				   struct attribute *_attr, char *buf)
+{
+	struct dmi_sysfs_entry *entry = to_entry(kobj);
+	struct dmi_sysfs_attribute *attr = to_attr(_attr);
+
+	/* DMI stuff is only ever admin visible */
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+
+	return attr->show(entry, buf);
+}
+
+static const struct sysfs_ops dmi_sysfs_attr_ops = {
+	.show = dmi_sysfs_attr_show,
+};
+
+typedef ssize_t (*dmi_callback)(struct dmi_sysfs_entry *,
+				const struct dmi_header *dh, void *);
+
+struct find_dmi_data {
+	struct dmi_sysfs_entry	*entry;
+	dmi_callback		callback;
+	void			*private;
+	int			instance_countdown;
+	ssize_t			ret;
+};
+
+static void find_dmi_entry_helper(const struct dmi_header *dh,
+				  void *_data)
+{
+	struct find_dmi_data *data = _data;
+	struct dmi_sysfs_entry *entry = data->entry;
+
+	/* Is this the entry we want? */
+	if (dh->type != entry->dh.type)
+		return;
+
+	if (data->instance_countdown != 0) {
+		/* try the next instance? */
+		data->instance_countdown--;
+		return;
+	}
+
+	/*
+	 * Don't ever revisit the instance.  Short circuit later
+	 * instances by letting the instance_countdown run negative
+	 */
+	data->instance_countdown--;
+
+	/* Found the entry */
+	data->ret = data->callback(entry, dh, data->private);
+}
+
+/* State for passing the read parameters through dmi_find_entry() */
+struct dmi_read_state {
+	char *buf;
+	loff_t pos;
+	size_t count;
+};
+
+static ssize_t find_dmi_entry(struct dmi_sysfs_entry *entry,
+			      dmi_callback callback, void *private)
+{
+	struct find_dmi_data data = {
+		.entry = entry,
+		.callback = callback,
+		.private = private,
+		.instance_countdown = entry->instance,
+		.ret = -EIO,  /* To signal the entry disappeared */
+	};
+	int ret;
+
+	ret = dmi_walk(find_dmi_entry_helper, &data);
+	/* This shouldn't happen, but just in case. */
+	if (ret)
+		return -EINVAL;
+	return data.ret;
+}
+
+/*
+ * Calculate and return the byte length of the dmi entry identified by
+ * dh.  This includes both the formatted portion as well as the
+ * unformatted string space, including the two trailing nul characters.
+ */
+static size_t dmi_entry_length(const struct dmi_header *dh)
+{
+	const char *p = (const char *)dh;
+
+	p += dh->length;
+
+	while (p[0] || p[1])
+		p++;
+
+	return 2 + p - (const char *)dh;
+}
+
+/*************************************************
+ * Support bits for specialized DMI entry support
+ *************************************************/
+struct dmi_entry_attr_show_data {
+	struct attribute *attr;
+	char *buf;
+};
+
+static ssize_t dmi_entry_attr_show_helper(struct dmi_sysfs_entry *entry,
+					  const struct dmi_header *dh,
+					  void *_data)
+{
+	struct dmi_entry_attr_show_data *data = _data;
+	struct dmi_sysfs_mapped_attribute *attr;
+
+	attr = container_of(data->attr,
+			    struct dmi_sysfs_mapped_attribute, attr);
+	return attr->show(entry, dh, data->buf);
+}
+
+static ssize_t dmi_entry_attr_show(struct kobject *kobj,
+				   struct attribute *attr,
+				   char *buf)
+{
+	struct dmi_entry_attr_show_data data = {
+		.attr = attr,
+		.buf  = buf,
+	};
+	/* Find the entry according to our parent and call the
+	 * normalized show method hanging off of the attribute */
+	return find_dmi_entry(to_entry(kobj->parent),
+			      dmi_entry_attr_show_helper, &data);
+}
+
+static const struct sysfs_ops dmi_sysfs_specialize_attr_ops = {
+	.show = dmi_entry_attr_show,
+};
+
+/*************************************************
+ * Specialized DMI entry support.
+ *************************************************/
+
+/*** Type 15 - System Event Table ***/
+
+#define DMI_SEL_ACCESS_METHOD_IO8	0x00
+#define DMI_SEL_ACCESS_METHOD_IO2x8	0x01
+#define DMI_SEL_ACCESS_METHOD_IO16	0x02
+#define DMI_SEL_ACCESS_METHOD_PHYS32	0x03
+#define DMI_SEL_ACCESS_METHOD_GPNV	0x04
+
+struct dmi_system_event_log {
+	struct dmi_header header;
+	u16	area_length;
+	u16	header_start_offset;
+	u16	data_start_offset;
+	u8	access_method;
+	u8	status;
+	u32	change_token;
+	union {
+		struct {
+			u16 index_addr;
+			u16 data_addr;
+		} io;
+		u32	phys_addr32;
+		u16	gpnv_handle;
+		u32	access_method_address;
+	};
+	u8	header_format;
+	u8	type_descriptors_supported_count;
+	u8	per_log_type_descriptor_length;
+	u8	supported_log_type_descriptos[0];
+} __packed;
+
+#define DMI_SYSFS_SEL_FIELD(_field) \
+static ssize_t dmi_sysfs_sel_##_field(struct dmi_sysfs_entry *entry, \
+				      const struct dmi_header *dh, \
+				      char *buf) \
+{ \
+	struct dmi_system_event_log sel; \
+	if (sizeof(sel) > dmi_entry_length(dh)) \
+		return -EIO; \
+	memcpy(&sel, dh, sizeof(sel)); \
+	return sprintf(buf, "%u\n", sel._field); \
+} \
+static DMI_SYSFS_MAPPED_ATTR(sel, _field)
+
+DMI_SYSFS_SEL_FIELD(area_length);
+DMI_SYSFS_SEL_FIELD(header_start_offset);
+DMI_SYSFS_SEL_FIELD(data_start_offset);
+DMI_SYSFS_SEL_FIELD(access_method);
+DMI_SYSFS_SEL_FIELD(status);
+DMI_SYSFS_SEL_FIELD(change_token);
+DMI_SYSFS_SEL_FIELD(access_method_address);
+DMI_SYSFS_SEL_FIELD(header_format);
+DMI_SYSFS_SEL_FIELD(type_descriptors_supported_count);
+DMI_SYSFS_SEL_FIELD(per_log_type_descriptor_length);
+
+static struct attribute *dmi_sysfs_sel_attrs[] = {
+	&dmi_sysfs_attr_sel_area_length.attr,
+	&dmi_sysfs_attr_sel_header_start_offset.attr,
+	&dmi_sysfs_attr_sel_data_start_offset.attr,
+	&dmi_sysfs_attr_sel_access_method.attr,
+	&dmi_sysfs_attr_sel_status.attr,
+	&dmi_sysfs_attr_sel_change_token.attr,
+	&dmi_sysfs_attr_sel_access_method_address.attr,
+	&dmi_sysfs_attr_sel_header_format.attr,
+	&dmi_sysfs_attr_sel_type_descriptors_supported_count.attr,
+	&dmi_sysfs_attr_sel_per_log_type_descriptor_length.attr,
+	NULL,
+};
+
+
+static struct kobj_type dmi_system_event_log_ktype = {
+	.release = dmi_entry_free,
+	.sysfs_ops = &dmi_sysfs_specialize_attr_ops,
+	.default_attrs = dmi_sysfs_sel_attrs,
+};
+
+typedef u8 (*sel_io_reader)(const struct dmi_system_event_log *sel,
+			    loff_t offset);
+
+static DEFINE_MUTEX(io_port_lock);
+
+static u8 read_sel_8bit_indexed_io(const struct dmi_system_event_log *sel,
+				   loff_t offset)
+{
+	u8 ret;
+
+	mutex_lock(&io_port_lock);
+	outb((u8)offset, sel->io.index_addr);
+	ret = inb(sel->io.data_addr);
+	mutex_unlock(&io_port_lock);
+	return ret;
+}
+
+static u8 read_sel_2x8bit_indexed_io(const struct dmi_system_event_log *sel,
+				     loff_t offset)
+{
+	u8 ret;
+
+	mutex_lock(&io_port_lock);
+	outb((u8)offset, sel->io.index_addr);
+	outb((u8)(offset >> 8), sel->io.index_addr + 1);
+	ret = inb(sel->io.data_addr);
+	mutex_unlock(&io_port_lock);
+	return ret;
+}
+
+static u8 read_sel_16bit_indexed_io(const struct dmi_system_event_log *sel,
+				    loff_t offset)
+{
+	u8 ret;
+
+	mutex_lock(&io_port_lock);
+	outw((u16)offset, sel->io.index_addr);
+	ret = inb(sel->io.data_addr);
+	mutex_unlock(&io_port_lock);
+	return ret;
+}
+
+static sel_io_reader sel_io_readers[] = {
+	[DMI_SEL_ACCESS_METHOD_IO8]	= read_sel_8bit_indexed_io,
+	[DMI_SEL_ACCESS_METHOD_IO2x8]	= read_sel_2x8bit_indexed_io,
+	[DMI_SEL_ACCESS_METHOD_IO16]	= read_sel_16bit_indexed_io,
+};
+
+static ssize_t dmi_sel_raw_read_io(struct dmi_sysfs_entry *entry,
+				   const struct dmi_system_event_log *sel,
+				   char *buf, loff_t pos, size_t count)
+{
+	ssize_t wrote = 0;
+
+	sel_io_reader io_reader = sel_io_readers[sel->access_method];
+
+	while (count && pos < sel->area_length) {
+		count--;
+		*(buf++) = io_reader(sel, pos++);
+		wrote++;
+	}
+
+	return wrote;
+}
+
+static ssize_t dmi_sel_raw_read_phys32(struct dmi_sysfs_entry *entry,
+				       const struct dmi_system_event_log *sel,
+				       char *buf, loff_t pos, size_t count)
+{
+	u8 __iomem *mapped;
+	ssize_t wrote = 0;
+
+	mapped = ioremap(sel->access_method_address, sel->area_length);
+	if (!mapped)
+		return -EIO;
+
+	while (count && pos < sel->area_length) {
+		count--;
+		*(buf++) = readb(mapped + pos++);
+		wrote++;
+	}
+
+	iounmap(mapped);
+	return wrote;
+}
+
+static ssize_t dmi_sel_raw_read_helper(struct dmi_sysfs_entry *entry,
+				       const struct dmi_header *dh,
+				       void *_state)
+{
+	struct dmi_read_state *state = _state;
+	struct dmi_system_event_log sel;
+
+	if (sizeof(sel) > dmi_entry_length(dh))
+		return -EIO;
+
+	memcpy(&sel, dh, sizeof(sel));
+
+	switch (sel.access_method) {
+	case DMI_SEL_ACCESS_METHOD_IO8:
+	case DMI_SEL_ACCESS_METHOD_IO2x8:
+	case DMI_SEL_ACCESS_METHOD_IO16:
+		return dmi_sel_raw_read_io(entry, &sel, state->buf,
+					   state->pos, state->count);
+	case DMI_SEL_ACCESS_METHOD_PHYS32:
+		return dmi_sel_raw_read_phys32(entry, &sel, state->buf,
+					       state->pos, state->count);
+	case DMI_SEL_ACCESS_METHOD_GPNV:
+		pr_info("dmi-sysfs: GPNV support missing.\n");
+		return -EIO;
+	default:
+		pr_info("dmi-sysfs: Unknown access method %02x\n",
+			sel.access_method);
+		return -EIO;
+	}
+}
+
+static ssize_t dmi_sel_raw_read(struct file *filp, struct kobject *kobj,
+				struct bin_attribute *bin_attr,
+				char *buf, loff_t pos, size_t count)
+{
+	struct dmi_sysfs_entry *entry = to_entry(kobj->parent);
+	struct dmi_read_state state = {
+		.buf = buf,
+		.pos = pos,
+		.count = count,
+	};
+
+	return find_dmi_entry(entry, dmi_sel_raw_read_helper, &state);
+}
+
+static struct bin_attribute dmi_sel_raw_attr = {
+	.attr = {.name = "raw_event_log", .mode = 0400},
+	.read = dmi_sel_raw_read,
+};
+
+static int dmi_system_event_log(struct dmi_sysfs_entry *entry)
+{
+	int ret;
+
+	entry->child = kzalloc(sizeof(*entry->child), GFP_KERNEL);
+	if (!entry->child)
+		return -ENOMEM;
+	ret = kobject_init_and_add(entry->child,
+				   &dmi_system_event_log_ktype,
+				   &entry->kobj,
+				   "system_event_log");
+	if (ret)
+		goto out_free;
+
+	ret = sysfs_create_bin_file(entry->child, &dmi_sel_raw_attr);
+	if (ret)
+		goto out_del;
+
+	return 0;
+
+out_del:
+	kobject_del(entry->child);
+out_free:
+	kfree(entry->child);
+	return ret;
+}
+
+/*************************************************
+ * Generic DMI entry support.
+ *************************************************/
+
+static ssize_t dmi_sysfs_entry_length(struct dmi_sysfs_entry *entry, char *buf)
+{
+	return sprintf(buf, "%d\n", entry->dh.length);
+}
+
+static ssize_t dmi_sysfs_entry_handle(struct dmi_sysfs_entry *entry, char *buf)
+{
+	return sprintf(buf, "%d\n", entry->dh.handle);
+}
+
+static ssize_t dmi_sysfs_entry_type(struct dmi_sysfs_entry *entry, char *buf)
+{
+	return sprintf(buf, "%d\n", entry->dh.type);
+}
+
+static ssize_t dmi_sysfs_entry_instance(struct dmi_sysfs_entry *entry,
+					char *buf)
+{
+	return sprintf(buf, "%d\n", entry->instance);
+}
+
+static ssize_t dmi_sysfs_entry_position(struct dmi_sysfs_entry *entry,
+					char *buf)
+{
+	return sprintf(buf, "%d\n", entry->position);
+}
+
+static DMI_SYSFS_ATTR(entry, length);
+static DMI_SYSFS_ATTR(entry, handle);
+static DMI_SYSFS_ATTR(entry, type);
+static DMI_SYSFS_ATTR(entry, instance);
+static DMI_SYSFS_ATTR(entry, position);
+
+static struct attribute *dmi_sysfs_entry_attrs[] = {
+	&dmi_sysfs_attr_entry_length.attr,
+	&dmi_sysfs_attr_entry_handle.attr,
+	&dmi_sysfs_attr_entry_type.attr,
+	&dmi_sysfs_attr_entry_instance.attr,
+	&dmi_sysfs_attr_entry_position.attr,
+	NULL,
+};
+
+static ssize_t dmi_entry_raw_read_helper(struct dmi_sysfs_entry *entry,
+					 const struct dmi_header *dh,
+					 void *_state)
+{
+	struct dmi_read_state *state = _state;
+	size_t entry_length;
+
+	entry_length = dmi_entry_length(dh);
+
+	return memory_read_from_buffer(state->buf, state->count,
+				       &state->pos, dh, entry_length);
+}
+
+static ssize_t dmi_entry_raw_read(struct file *filp,
+				  struct kobject *kobj,
+				  struct bin_attribute *bin_attr,
+				  char *buf, loff_t pos, size_t count)
+{
+	struct dmi_sysfs_entry *entry = to_entry(kobj);
+	struct dmi_read_state state = {
+		.buf = buf,
+		.pos = pos,
+		.count = count,
+	};
+
+	return find_dmi_entry(entry, dmi_entry_raw_read_helper, &state);
+}
+
+static const struct bin_attribute dmi_entry_raw_attr = {
+	.attr = {.name = "raw", .mode = 0400},
+	.read = dmi_entry_raw_read,
+};
+
+static void dmi_sysfs_entry_release(struct kobject *kobj)
+{
+	struct dmi_sysfs_entry *entry = to_entry(kobj);
+	sysfs_remove_bin_file(&entry->kobj, &dmi_entry_raw_attr);
+	spin_lock(&entry_list_lock);
+	list_del(&entry->list);
+	spin_unlock(&entry_list_lock);
+	kfree(entry);
+}
+
+static struct kobj_type dmi_sysfs_entry_ktype = {
+	.release = dmi_sysfs_entry_release,
+	.sysfs_ops = &dmi_sysfs_attr_ops,
+	.default_attrs = dmi_sysfs_entry_attrs,
+};
+
+static struct kobject *dmi_kobj;
+static struct kset *dmi_kset;
+
+/* Global count of all instances seen.  Only for setup */
+static int __initdata instance_counts[MAX_ENTRY_TYPE + 1];
+
+/* Global positional count of all entries seen.  Only for setup */
+static int __initdata position_count;
+
+static void __init dmi_sysfs_register_handle(const struct dmi_header *dh,
+					     void *_ret)
+{
+	struct dmi_sysfs_entry *entry;
+	int *ret = _ret;
+
+	/* If a previous entry saw an error, short circuit */
+	if (*ret)
+		return;
+
+	/* Allocate and register a new entry into the entries set */
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry) {
+		*ret = -ENOMEM;
+		return;
+	}
+
+	/* Set the key */
+	memcpy(&entry->dh, dh, sizeof(*dh));
+	entry->instance = instance_counts[dh->type]++;
+	entry->position = position_count++;
+
+	entry->kobj.kset = dmi_kset;
+	*ret = kobject_init_and_add(&entry->kobj, &dmi_sysfs_entry_ktype, NULL,
+				    "%d-%d", dh->type, entry->instance);
+
+	if (*ret) {
+		kfree(entry);
+		return;
+	}
+
+	/* Thread on the global list for cleanup */
+	spin_lock(&entry_list_lock);
+	list_add_tail(&entry->list, &entry_list);
+	spin_unlock(&entry_list_lock);
+
+	/* Handle specializations by type */
+	switch (dh->type) {
+	case DMI_ENTRY_SYSTEM_EVENT_LOG:
+		*ret = dmi_system_event_log(entry);
+		break;
+	default:
+		/* No specialization */
+		break;
+	}
+	if (*ret)
+		goto out_err;
+
+	/* Create the raw binary file to access the entry */
+	*ret = sysfs_create_bin_file(&entry->kobj, &dmi_entry_raw_attr);
+	if (*ret)
+		goto out_err;
+
+	return;
+out_err:
+	kobject_put(entry->child);
+	kobject_put(&entry->kobj);
+	return;
+}
+
+static void cleanup_entry_list(void)
+{
+	struct dmi_sysfs_entry *entry, *next;
+
+	/* No locks, we are on our way out */
+	list_for_each_entry_safe(entry, next, &entry_list, list) {
+		kobject_put(entry->child);
+		kobject_put(&entry->kobj);
+	}
+}
+
+static int __init dmi_sysfs_init(void)
+{
+	int error = -ENOMEM;
+	int val;
+
+	/* Set up our directory */
+	dmi_kobj = kobject_create_and_add("dmi", firmware_kobj);
+	if (!dmi_kobj)
+		goto err;
+
+	dmi_kset = kset_create_and_add("entries", NULL, dmi_kobj);
+	if (!dmi_kset)
+		goto err;
+
+	val = 0;
+	error = dmi_walk(dmi_sysfs_register_handle, &val);
+	if (error)
+		goto err;
+	if (val) {
+		error = val;
+		goto err;
+	}
+
+	pr_debug("dmi-sysfs: loaded.\n");
+
+	return 0;
+err:
+	cleanup_entry_list();
+	kset_unregister(dmi_kset);
+	kobject_put(dmi_kobj);
+	return error;
+}
+
+/* clean up everything. */
+static void __exit dmi_sysfs_exit(void)
+{
+	pr_debug("dmi-sysfs: unloading.\n");
+	cleanup_entry_list();
+	kset_unregister(dmi_kset);
+	kobject_put(dmi_kobj);
+}
+
+module_init(dmi_sysfs_init);
+module_exit(dmi_sysfs_exit);
+
+MODULE_AUTHOR("Mike Waychison <mikew@google.com>");
+MODULE_DESCRIPTION("DMI sysfs support");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/janz-ttl.c b/drivers/gpio/janz-ttl.c
index 813ac077e5d7..2514fb075f4a 100644
--- a/drivers/gpio/janz-ttl.c
+++ b/drivers/gpio/janz-ttl.c
@@ -15,6 +15,7 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -149,7 +150,7 @@ static int __devinit ttl_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret;
 
-	pdata = pdev->dev.platform_data;
+	pdata = mfd_get_data(pdev);
 	if (!pdata) {
 		dev_err(dev, "no platform data\n");
 		ret = -ENXIO;
diff --git a/drivers/gpio/pca953x.c b/drivers/gpio/pca953x.c
index b473429eee75..2fc25dec7cf5 100644
--- a/drivers/gpio/pca953x.c
+++ b/drivers/gpio/pca953x.c
@@ -462,7 +462,8 @@ pca953x_get_alt_pdata(struct i2c_client *client)
 {
 	struct pca953x_platform_data *pdata;
 	struct device_node *node;
-	const uint16_t *val;
+	const __be32 *val;
+	int size;
 
 	node = client->dev.of_node;
 	if (node == NULL)
@@ -475,13 +476,13 @@ pca953x_get_alt_pdata(struct i2c_client *client)
 	}
 
 	pdata->gpio_base = -1;
-	val = of_get_property(node, "linux,gpio-base", NULL);
+	val = of_get_property(node, "linux,gpio-base", &size);
 	if (val) {
-		if (*val < 0)
-			dev_warn(&client->dev,
-				 "invalid gpio-base in device tree\n");
+		if (size != sizeof(*val))
+			dev_warn(&client->dev, "%s: wrong linux,gpio-base\n",
+				 node->full_name);
 		else
-			pdata->gpio_base = *val;
+			pdata->gpio_base = be32_to_cpup(val);
 	}
 
 	val = of_get_property(node, "polarity", NULL);
diff --git a/drivers/gpio/pl061.c b/drivers/gpio/pl061.c
index 2975d22daffe..838ddbdf90cc 100644
--- a/drivers/gpio/pl061.c
+++ b/drivers/gpio/pl061.c
@@ -232,7 +232,7 @@ static void pl061_irq_handler(unsigned irq, struct irq_desc *desc)
 	desc->irq_data.chip->irq_unmask(&desc->irq_data);
 }
 
-static int pl061_probe(struct amba_device *dev, struct amba_id *id)
+static int pl061_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct pl061_platform_data *pdata;
 	struct pl061_gpio *chip;
diff --git a/drivers/gpio/rdc321x-gpio.c b/drivers/gpio/rdc321x-gpio.c
index 897e0577e65e..a9bda881935a 100644
--- a/drivers/gpio/rdc321x-gpio.c
+++ b/drivers/gpio/rdc321x-gpio.c
@@ -27,6 +27,7 @@
 #include <linux/pci.h>
 #include <linux/gpio.h>
 #include <linux/mfd/rdc321x.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 
 struct rdc321x_gpio {
@@ -135,7 +136,7 @@ static int __devinit rdc321x_gpio_probe(struct platform_device *pdev)
 	struct rdc321x_gpio *rdc321x_gpio_dev;
 	struct rdc321x_gpio_pdata *pdata;
 
-	pdata = platform_get_drvdata(pdev);
+	pdata = mfd_get_data(pdev);
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform data supplied\n");
 		return -ENODEV;
diff --git a/drivers/gpio/sx150x.c b/drivers/gpio/sx150x.c
index e60be0015c9b..d2f874c3d3d5 100644
--- a/drivers/gpio/sx150x.c
+++ b/drivers/gpio/sx150x.c
@@ -25,6 +25,8 @@
 #include <linux/workqueue.h>
 #include <linux/i2c/sx150x.h>
 
+#define NO_UPDATE_PENDING	-1
+
 struct sx150x_device_data {
 	u8 reg_pullup;
 	u8 reg_pulldn;
@@ -47,8 +49,11 @@ struct sx150x_chip {
 	const struct sx150x_device_data *dev_cfg;
 	int                              irq_summary;
 	int                              irq_base;
+	int				 irq_update;
 	u32                              irq_sense;
-	unsigned long                    irq_set_type_pending;
+	u32				 irq_masked;
+	u32				 dev_sense;
+	u32				 dev_masked;
 	struct irq_chip                  irq_chip;
 	struct mutex                     lock;
 };
@@ -312,9 +317,8 @@ static void sx150x_irq_mask(struct irq_data *d)
 
 	chip = container_of(ic, struct sx150x_chip, irq_chip);
 	n = d->irq - chip->irq_base;
-
-	sx150x_write_cfg(chip, n, 1, chip->dev_cfg->reg_irq_mask, 1);
-	sx150x_write_cfg(chip, n, 2, chip->dev_cfg->reg_sense, 0);
+	chip->irq_masked |= (1 << n);
+	chip->irq_update = n;
 }
 
 static void sx150x_irq_unmask(struct irq_data *d)
@@ -326,9 +330,8 @@ static void sx150x_irq_unmask(struct irq_data *d)
 	chip = container_of(ic, struct sx150x_chip, irq_chip);
 	n = d->irq - chip->irq_base;
 
-	sx150x_write_cfg(chip, n, 1, chip->dev_cfg->reg_irq_mask, 0);
-	sx150x_write_cfg(chip, n, 2, chip->dev_cfg->reg_sense,
-			 chip->irq_sense >> (n * 2));
+	chip->irq_masked &= ~(1 << n);
+	chip->irq_update = n;
 }
 
 static int sx150x_irq_set_type(struct irq_data *d, unsigned int flow_type)
@@ -350,7 +353,7 @@ static int sx150x_irq_set_type(struct irq_data *d, unsigned int flow_type)
 
 	chip->irq_sense &= ~(3UL << (n * 2));
 	chip->irq_sense |= val << (n * 2);
-	chip->irq_set_type_pending |= BIT(n);
+	chip->irq_update = n;
 	return 0;
 }
 
@@ -404,15 +407,29 @@ static void sx150x_irq_bus_sync_unlock(struct irq_data *d)
 
 	chip = container_of(ic, struct sx150x_chip, irq_chip);
 
-	while (chip->irq_set_type_pending) {
-		n = __ffs(chip->irq_set_type_pending);
-		chip->irq_set_type_pending &= ~BIT(n);
-		if (!(irq_to_desc(n + chip->irq_base)->status & IRQ_MASKED))
-			sx150x_write_cfg(chip, n, 2,
-					chip->dev_cfg->reg_sense,
-					chip->irq_sense >> (n * 2));
-	}
+	if (chip->irq_update == NO_UPDATE_PENDING)
+		goto out;
+
+	n = chip->irq_update;
+	chip->irq_update = NO_UPDATE_PENDING;
 
+	/* Avoid updates if nothing changed */
+	if (chip->dev_sense == chip->irq_sense &&
+	    chip->dev_sense == chip->irq_masked)
+		goto out;
+
+	chip->dev_sense = chip->irq_sense;
+	chip->dev_masked = chip->irq_masked;
+
+	if (chip->irq_masked & (1 << n)) {
+		sx150x_write_cfg(chip, n, 1, chip->dev_cfg->reg_irq_mask, 1);
+		sx150x_write_cfg(chip, n, 2, chip->dev_cfg->reg_sense, 0);
+	} else {
+		sx150x_write_cfg(chip, n, 1, chip->dev_cfg->reg_irq_mask, 0);
+		sx150x_write_cfg(chip, n, 2, chip->dev_cfg->reg_sense,
+				 chip->irq_sense >> (n * 2));
+	}
+out:
 	mutex_unlock(&chip->lock);
 }
 
@@ -445,8 +462,11 @@ static void sx150x_init_chip(struct sx150x_chip *chip,
 	chip->irq_chip.irq_bus_sync_unlock = sx150x_irq_bus_sync_unlock;
 	chip->irq_summary                  = -1;
 	chip->irq_base                     = -1;
+	chip->irq_masked                   = ~0;
 	chip->irq_sense                    = 0;
-	chip->irq_set_type_pending         = 0;
+	chip->dev_masked                   = ~0;
+	chip->dev_sense                    = 0;
+	chip->irq_update		   = NO_UPDATE_PENDING;
 }
 
 static int sx150x_init_io(struct sx150x_chip *chip, u8 base, u16 cfg)
diff --git a/drivers/gpio/timbgpio.c b/drivers/gpio/timbgpio.c
index 58c8f30352dd..ffcd815b8b8b 100644
--- a/drivers/gpio/timbgpio.c
+++ b/drivers/gpio/timbgpio.c
@@ -23,6 +23,7 @@
 #include <linux/module.h>
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/irq.h>
 #include <linux/io.h>
 #include <linux/timb_gpio.h>
@@ -228,7 +229,7 @@ static int __devinit timbgpio_probe(struct platform_device *pdev)
 	struct gpio_chip *gc;
 	struct timbgpio *tgpio;
 	struct resource *iomem;
-	struct timbgpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timbgpio_platform_data *pdata = mfd_get_data(pdev);
 	int irq = platform_get_irq(pdev, 0);
 
 	if (!pdata || pdata->nr_pins > 32) {
@@ -319,14 +320,13 @@ err_mem:
 static int __devexit timbgpio_remove(struct platform_device *pdev)
 {
 	int err;
-	struct timbgpio_platform_data *pdata = pdev->dev.platform_data;
 	struct timbgpio *tgpio = platform_get_drvdata(pdev);
 	struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	int irq = platform_get_irq(pdev, 0);
 
 	if (irq >= 0 && tgpio->irq_base > 0) {
 		int i;
-		for (i = 0; i < pdata->nr_pins; i++) {
+		for (i = 0; i < tgpio->gpio.ngpio; i++) {
 			set_irq_chip(tgpio->irq_base + i, NULL);
 			set_irq_chip_data(tgpio->irq_base + i, NULL);
 		}
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index 0902d4460039..a6feb78c404c 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -73,32 +73,17 @@ source "drivers/gpu/drm/radeon/Kconfig"
 
 config DRM_I810
 	tristate "Intel I810"
-	# BKL usage in order to avoid AB-BA deadlocks, may become BROKEN_ON_SMP
-	depends on DRM && AGP && AGP_INTEL && BKL
+	# !PREEMPT because of missing ioctl locking
+	depends on DRM && AGP && AGP_INTEL && (!PREEMPT || BROKEN)
 	help
 	  Choose this option if you have an Intel I810 graphics card.  If M is
 	  selected, the module will be called i810.  AGP support is required
 	  for this driver to work.
 
-choice
-	prompt "Intel 830M, 845G, 852GM, 855GM, 865G"
-	depends on DRM && AGP && AGP_INTEL
-	optional
-
-config DRM_I830
-	tristate "i830 driver"
-	# BKL usage in order to avoid AB-BA deadlocks, i830 may get removed
-	depends on BKL
-	help
-	  Choose this option if you have a system that has Intel 830M, 845G,
-	  852GM, 855GM or 865G integrated graphics.  If M is selected, the
-	  module will be called i830.  AGP support is required for this driver
-	  to work. This driver is used by the older X releases X.org 6.7 and
-	  XFree86 4.3. If unsure, build this and i915 as modules and the X server
-	  will load the correct one.
-
 config DRM_I915
-	tristate "i915 driver"
+	tristate "Intel 8xx/9xx/G3x/G4x/HD Graphics"
+	depends on DRM
+	depends on AGP
 	depends on AGP_INTEL
 	# we need shmfs for the swappable backing store, and in particular
 	# the shmem_readpage() which depends upon tmpfs
@@ -115,12 +100,20 @@ config DRM_I915
 	select ACPI_VIDEO if ACPI
 	select ACPI_BUTTON if ACPI
 	help
-	  Choose this option if you have a system that has Intel 830M, 845G,
-	  852GM, 855GM 865G or 915G integrated graphics.  If M is selected, the
-	  module will be called i915.  AGP support is required for this driver
-	  to work. This driver is used by the Intel driver in X.org 6.8 and
-	  XFree86 4.4 and above. If unsure, build this and i830 as modules and
-	  the X server will load the correct one.
+	  Choose this option if you have a system that has "Intel Graphics
+	  Media Accelerator" or "HD Graphics" integrated graphics,
+	  including 830M, 845G, 852GM, 855GM, 865G, 915G, 945G, 965G,
+	  G35, G41, G43, G45 chipsets and Celeron, Pentium, Core i3,
+	  Core i5, Core i7 as well as Atom CPUs with integrated graphics.
+	  If M is selected, the module will be called i915.  AGP support
+	  is required for this driver to work. This driver is used by
+	  the Intel driver in X.org 6.8 and XFree86 4.4 and above. It
+	  replaces the older i830 module that supported a subset of the
+	  hardware in older X.org releases.
+
+	  Note that the older i810/i815 chipsets require the use of the
+	  i810 driver instead, and the Atom z5xx series has an entirely
+	  different implementation.
 
 config DRM_I915_KMS
 	bool "Enable modesetting on intel by default"
@@ -132,8 +125,6 @@ config DRM_I915_KMS
 	  the driver to bind to PCI devices, which precludes loading things
 	  like intelfb.
 
-endchoice
-
 config DRM_MGA
 	tristate "Matrox g200/g400"
 	depends on DRM && PCI
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index 997c43d04909..89cf05a72d1c 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -12,7 +12,7 @@ drm-y       :=	drm_auth.o drm_buffer.o drm_bufs.o drm_cache.o \
 		drm_platform.o drm_sysfs.o drm_hashtab.o drm_sman.o drm_mm.o \
 		drm_crtc.o drm_modes.o drm_edid.o \
 		drm_info.o drm_debugfs.o drm_encoder_slave.o \
-		drm_trace_points.o drm_global.o
+		drm_trace_points.o drm_global.o drm_usb.o
 
 drm-$(CONFIG_COMPAT) += drm_ioc32.o
 
@@ -29,7 +29,6 @@ obj-$(CONFIG_DRM_R128)	+= r128/
 obj-$(CONFIG_DRM_RADEON)+= radeon/
 obj-$(CONFIG_DRM_MGA)	+= mga/
 obj-$(CONFIG_DRM_I810)	+= i810/
-obj-$(CONFIG_DRM_I830)	+= i830/
 obj-$(CONFIG_DRM_I915)  += i915/
 obj-$(CONFIG_DRM_SIS)   += sis/
 obj-$(CONFIG_DRM_SAVAGE)+= savage/
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 654faa803dcb..4c95b5fd9df3 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -2694,3 +2694,36 @@ void drm_mode_config_reset(struct drm_device *dev)
 			connector->funcs->reset(connector);
 }
 EXPORT_SYMBOL(drm_mode_config_reset);
+
+int drm_mode_create_dumb_ioctl(struct drm_device *dev,
+			       void *data, struct drm_file *file_priv)
+{
+	struct drm_mode_create_dumb *args = data;
+
+	if (!dev->driver->dumb_create)
+		return -ENOSYS;
+	return dev->driver->dumb_create(file_priv, dev, args);
+}
+
+int drm_mode_mmap_dumb_ioctl(struct drm_device *dev,
+			     void *data, struct drm_file *file_priv)
+{
+	struct drm_mode_map_dumb *args = data;
+
+	/* call driver ioctl to get mmap offset */
+	if (!dev->driver->dumb_map_offset)
+		return -ENOSYS;
+
+	return dev->driver->dumb_map_offset(file_priv, dev, args->handle, &args->offset);
+}
+
+int drm_mode_destroy_dumb_ioctl(struct drm_device *dev,
+				void *data, struct drm_file *file_priv)
+{
+	struct drm_mode_destroy_dumb *args = data;
+
+	if (!dev->driver->dumb_destroy)
+		return -ENOSYS;
+
+	return dev->driver->dumb_destroy(file_priv, dev, args->handle);
+}
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 271835a71570..93a112d45c1a 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -67,6 +67,7 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAP, drm_getmap, 0),
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_CLIENT, drm_getclient, 0),
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_STATS, drm_getstats, 0),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_CAP, drm_getcap, 0),
 	DRM_IOCTL_DEF(DRM_IOCTL_SET_VERSION, drm_setversion, DRM_MASTER),
 
 	DRM_IOCTL_DEF(DRM_IOCTL_SET_UNIQUE, drm_setunique, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
@@ -150,7 +151,10 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB, drm_mode_addfb, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_RMFB, drm_mode_rmfb, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_PAGE_FLIP, drm_mode_page_flip_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED)
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CREATE_DUMB, drm_mode_create_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_MAP_DUMB, drm_mode_mmap_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED)
 };
 
 #define DRM_CORE_IOCTL_COUNT	ARRAY_SIZE( drm_ioctls )
@@ -234,49 +238,6 @@ int drm_lastclose(struct drm_device * dev)
 	return 0;
 }
 
-/**
- * Module initialization. Called via init_module at module load time, or via
- * linux/init/main.c (this is not currently supported).
- *
- * \return zero on success or a negative number on failure.
- *
- * Initializes an array of drm_device structures, and attempts to
- * initialize all available devices, using consecutive minors, registering the
- * stubs and initializing the device.
- *
- * Expands the \c DRIVER_PREINIT and \c DRIVER_POST_INIT macros before and
- * after the initialization for driver customization.
- */
-int drm_init(struct drm_driver *driver)
-{
-	DRM_DEBUG("\n");
-	INIT_LIST_HEAD(&driver->device_list);
-
-	if (driver->driver_features & DRIVER_USE_PLATFORM_DEVICE)
-		return drm_platform_init(driver);
-	else
-		return drm_pci_init(driver);
-}
-
-EXPORT_SYMBOL(drm_init);
-
-void drm_exit(struct drm_driver *driver)
-{
-	struct drm_device *dev, *tmp;
-	DRM_DEBUG("\n");
-
-	if (driver->driver_features & DRIVER_MODESET) {
-		pci_unregister_driver(&driver->pci_driver);
-	} else {
-		list_for_each_entry_safe(dev, tmp, &driver->device_list, driver_item)
-			drm_put_dev(dev);
-	}
-
-	DRM_INFO("Module unloaded\n");
-}
-
-EXPORT_SYMBOL(drm_exit);
-
 /** File operations structure */
 static const struct file_operations drm_stub_fops = {
 	.owner = THIS_MODULE,
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index a245d17165ae..af60d9be9632 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -449,12 +449,11 @@ static void edid_fixup_preferred(struct drm_connector *connector,
 struct drm_display_mode *drm_mode_find_dmt(struct drm_device *dev,
 					   int hsize, int vsize, int fresh)
 {
+	struct drm_display_mode *mode = NULL;
 	int i;
-	struct drm_display_mode *ptr, *mode;
 
-	mode = NULL;
 	for (i = 0; i < drm_num_dmt_modes; i++) {
-		ptr = &drm_dmt_modes[i];
+		const struct drm_display_mode *ptr = &drm_dmt_modes[i];
 		if (hsize == ptr->hdisplay &&
 			vsize == ptr->vdisplay &&
 			fresh == drm_mode_vrefresh(ptr)) {
@@ -885,7 +884,7 @@ static struct drm_display_mode *drm_mode_detailed(struct drm_device *dev,
 }
 
 static bool
-mode_is_rb(struct drm_display_mode *mode)
+mode_is_rb(const struct drm_display_mode *mode)
 {
 	return (mode->htotal - mode->hdisplay == 160) &&
 	       (mode->hsync_end - mode->hdisplay == 80) &&
@@ -894,7 +893,8 @@ mode_is_rb(struct drm_display_mode *mode)
 }
 
 static bool
-mode_in_hsync_range(struct drm_display_mode *mode, struct edid *edid, u8 *t)
+mode_in_hsync_range(const struct drm_display_mode *mode,
+		    struct edid *edid, u8 *t)
 {
 	int hsync, hmin, hmax;
 
@@ -910,7 +910,8 @@ mode_in_hsync_range(struct drm_display_mode *mode, struct edid *edid, u8 *t)
 }
 
 static bool
-mode_in_vsync_range(struct drm_display_mode *mode, struct edid *edid, u8 *t)
+mode_in_vsync_range(const struct drm_display_mode *mode,
+		    struct edid *edid, u8 *t)
 {
 	int vsync, vmin, vmax;
 
@@ -941,7 +942,7 @@ range_pixel_clock(struct edid *edid, u8 *t)
 }
 
 static bool
-mode_in_range(struct drm_display_mode *mode, struct edid *edid,
+mode_in_range(const struct drm_display_mode *mode, struct edid *edid,
 	      struct detailed_timing *timing)
 {
 	u32 max_clock;
@@ -1472,7 +1473,7 @@ int drm_add_modes_noedid(struct drm_connector *connector,
 			int hdisplay, int vdisplay)
 {
 	int i, count, num_modes = 0;
-	struct drm_display_mode *mode, *ptr;
+	struct drm_display_mode *mode;
 	struct drm_device *dev = connector->dev;
 
 	count = sizeof(drm_dmt_modes) / sizeof(struct drm_display_mode);
@@ -1482,7 +1483,7 @@ int drm_add_modes_noedid(struct drm_connector *connector,
 		vdisplay = 0;
 
 	for (i = 0; i < count; i++) {
-		ptr = &drm_dmt_modes[i];
+		const struct drm_display_mode *ptr = &drm_dmt_modes[i];
 		if (hdisplay && vdisplay) {
 			/*
 			 * Only when two are valid, they will be used to check
diff --git a/drivers/gpu/drm/drm_edid_modes.h b/drivers/gpu/drm/drm_edid_modes.h
index 6eb7592e152f..5f2064489fd5 100644
--- a/drivers/gpu/drm/drm_edid_modes.h
+++ b/drivers/gpu/drm/drm_edid_modes.h
@@ -32,7 +32,7 @@
  * This table is copied from xfree86/modes/xf86EdidModes.c.
  * But the mode with Reduced blank feature is deleted.
  */
-static struct drm_display_mode drm_dmt_modes[] = {
+static const struct drm_display_mode drm_dmt_modes[] = {
 	/* 640x350@85Hz */
 	{ DRM_MODE("640x350", DRM_MODE_TYPE_DRIVER, 31500, 640, 672,
 		   736, 832, 0, 350, 382, 385, 445, 0,
@@ -266,7 +266,7 @@ static struct drm_display_mode drm_dmt_modes[] = {
 static const int drm_num_dmt_modes =
 	sizeof(drm_dmt_modes) / sizeof(struct drm_display_mode);
 
-static struct drm_display_mode edid_est_modes[] = {
+static const struct drm_display_mode edid_est_modes[] = {
 	{ DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 40000, 800, 840,
 		   968, 1056, 0, 600, 601, 605, 628, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) }, /* 800x600@60Hz */
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 6977a1ce9d98..950720473967 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -627,6 +627,11 @@ static int setcolreg(struct drm_crtc *crtc, u16 red, u16 green,
 		value = (red << info->var.red.offset) |
 			(green << info->var.green.offset) |
 			(blue << info->var.blue.offset);
+		if (info->var.transp.length > 0) {
+			u32 mask = (1 << info->var.transp.length) - 1;
+			mask <<= info->var.transp.offset;
+			value |= mask;
+		}
 		palette[regno] = value;
 		return 0;
 	}
@@ -672,7 +677,7 @@ int drm_fb_helper_setcmap(struct fb_cmap *cmap, struct fb_info *info)
 	struct drm_crtc_helper_funcs *crtc_funcs;
 	u16 *red, *green, *blue, *transp;
 	struct drm_crtc *crtc;
-	int i, rc = 0;
+	int i, j, rc = 0;
 	int start;
 
 	for (i = 0; i < fb_helper->crtc_count; i++) {
@@ -685,7 +690,7 @@ int drm_fb_helper_setcmap(struct fb_cmap *cmap, struct fb_info *info)
 		transp = cmap->transp;
 		start = cmap->start;
 
-		for (i = 0; i < cmap->len; i++) {
+		for (j = 0; j < cmap->len; j++) {
 			u16 hred, hgreen, hblue, htransp = 0xffff;
 
 			hred = *red++;
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index ea1c4b019ebf..57ce27c9a747 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -101,7 +101,7 @@ drm_gem_init(struct drm_device *dev)
 
 	dev->mm_private = mm;
 
-	if (drm_ht_create(&mm->offset_hash, 19)) {
+	if (drm_ht_create(&mm->offset_hash, 12)) {
 		kfree(mm);
 		return -ENOMEM;
 	}
@@ -181,7 +181,7 @@ EXPORT_SYMBOL(drm_gem_object_alloc);
 /**
  * Removes the mapping from handle to filp for this object.
  */
-static int
+int
 drm_gem_handle_delete(struct drm_file *filp, u32 handle)
 {
 	struct drm_device *dev;
@@ -214,6 +214,7 @@ drm_gem_handle_delete(struct drm_file *filp, u32 handle)
 
 	return 0;
 }
+EXPORT_SYMBOL(drm_gem_handle_delete);
 
 /**
  * Create a handle for this object. This adds a handle reference
diff --git a/drivers/gpu/drm/drm_hashtab.c b/drivers/gpu/drm/drm_hashtab.c
index a93d7b4ddaa6..e3a75688f3cd 100644
--- a/drivers/gpu/drm/drm_hashtab.c
+++ b/drivers/gpu/drm/drm_hashtab.c
@@ -39,27 +39,18 @@
 
 int drm_ht_create(struct drm_open_hash *ht, unsigned int order)
 {
-	unsigned int i;
+	unsigned int size = 1 << order;
 
-	ht->size = 1 << order;
 	ht->order = order;
-	ht->fill = 0;
 	ht->table = NULL;
-	ht->use_vmalloc = ((ht->size * sizeof(*ht->table)) > PAGE_SIZE);
-	if (!ht->use_vmalloc) {
-		ht->table = kcalloc(ht->size, sizeof(*ht->table), GFP_KERNEL);
-	}
-	if (!ht->table) {
-		ht->use_vmalloc = 1;
-		ht->table = vmalloc(ht->size*sizeof(*ht->table));
-	}
+	if (size <= PAGE_SIZE / sizeof(*ht->table))
+		ht->table = kcalloc(size, sizeof(*ht->table), GFP_KERNEL);
+	else
+		ht->table = vzalloc(size*sizeof(*ht->table));
 	if (!ht->table) {
 		DRM_ERROR("Out of memory for hash table\n");
 		return -ENOMEM;
 	}
-	for (i=0; i< ht->size; ++i) {
-		INIT_HLIST_HEAD(&ht->table[i]);
-	}
 	return 0;
 }
 EXPORT_SYMBOL(drm_ht_create);
@@ -180,7 +171,6 @@ int drm_ht_remove_key(struct drm_open_hash *ht, unsigned long key)
 	list = drm_ht_find_key(ht, key);
 	if (list) {
 		hlist_del_init(list);
-		ht->fill--;
 		return 0;
 	}
 	return -EINVAL;
@@ -189,7 +179,6 @@ int drm_ht_remove_key(struct drm_open_hash *ht, unsigned long key)
 int drm_ht_remove_item(struct drm_open_hash *ht, struct drm_hash_item *item)
 {
 	hlist_del_init(&item->head);
-	ht->fill--;
 	return 0;
 }
 EXPORT_SYMBOL(drm_ht_remove_item);
@@ -197,10 +186,10 @@ EXPORT_SYMBOL(drm_ht_remove_item);
 void drm_ht_remove(struct drm_open_hash *ht)
 {
 	if (ht->table) {
-		if (ht->use_vmalloc)
-			vfree(ht->table);
-		else
+		if ((PAGE_SIZE / sizeof(*ht->table)) >> ht->order)
 			kfree(ht->table);
+		else
+			vfree(ht->table);
 		ht->table = NULL;
 	}
 }
diff --git a/drivers/gpu/drm/drm_info.c b/drivers/gpu/drm/drm_info.c
index be9a9c07d152..ab1162da70f8 100644
--- a/drivers/gpu/drm/drm_info.c
+++ b/drivers/gpu/drm/drm_info.c
@@ -47,30 +47,19 @@ int drm_name_info(struct seq_file *m, void *data)
 	struct drm_minor *minor = node->minor;
 	struct drm_device *dev = minor->dev;
 	struct drm_master *master = minor->master;
-
+	const char *bus_name;
 	if (!master)
 		return 0;
 
-	if (drm_core_check_feature(dev, DRIVER_USE_PLATFORM_DEVICE)) {
-		if (master->unique) {
-			seq_printf(m, "%s %s %s\n",
-					dev->driver->platform_device->name,
-					dev_name(dev->dev), master->unique);
-		} else {
-			seq_printf(m, "%s\n",
-				dev->driver->platform_device->name);
-		}
+	bus_name = dev->driver->bus->get_name(dev);
+	if (master->unique) {
+		seq_printf(m, "%s %s %s\n",
+			   bus_name,
+			   dev_name(dev->dev), master->unique);
 	} else {
-		if (master->unique) {
-			seq_printf(m, "%s %s %s\n",
-				dev->driver->pci_driver.name,
-				dev_name(dev->dev), master->unique);
-		} else {
-			seq_printf(m, "%s %s\n", dev->driver->pci_driver.name,
-				dev_name(dev->dev));
-		}
+		seq_printf(m, "%s %s\n",
+			   bus_name, dev_name(dev->dev));
 	}
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/drm_ioctl.c b/drivers/gpu/drm/drm_ioctl.c
index 47db4df37a69..7f6912a16761 100644
--- a/drivers/gpu/drm/drm_ioctl.c
+++ b/drivers/gpu/drm/drm_ioctl.c
@@ -96,7 +96,7 @@ int drm_setunique(struct drm_device *dev, void *data,
 {
 	struct drm_unique *u = data;
 	struct drm_master *master = file_priv->master;
-	int domain, bus, slot, func, ret;
+	int ret;
 
 	if (master->unique_len || master->unique)
 		return -EBUSY;
@@ -104,50 +104,12 @@ int drm_setunique(struct drm_device *dev, void *data,
 	if (!u->unique_len || u->unique_len > 1024)
 		return -EINVAL;
 
-	master->unique_len = u->unique_len;
-	master->unique_size = u->unique_len + 1;
-	master->unique = kmalloc(master->unique_size, GFP_KERNEL);
-	if (!master->unique) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	if (copy_from_user(master->unique, u->unique, master->unique_len)) {
-		ret = -EFAULT;
-		goto err;
-	}
-
-	master->unique[master->unique_len] = '\0';
-
-	dev->devname = kmalloc(strlen(dev->driver->pci_driver.name) +
-			       strlen(master->unique) + 2, GFP_KERNEL);
-	if (!dev->devname) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	sprintf(dev->devname, "%s@%s", dev->driver->pci_driver.name,
-		master->unique);
+	if (!dev->driver->bus->set_unique)
+		return -EINVAL;
 
-	/* Return error if the busid submitted doesn't match the device's actual
-	 * busid.
-	 */
-	ret = sscanf(master->unique, "PCI:%d:%d:%d", &bus, &slot, &func);
-	if (ret != 3) {
-		ret = -EINVAL;
+	ret = dev->driver->bus->set_unique(dev, master, u);
+	if (ret)
 		goto err;
-	}
-
-	domain = bus >> 8;
-	bus &= 0xff;
-
-	if ((domain != drm_get_pci_domain(dev)) ||
-	    (bus != dev->pdev->bus->number) ||
-	    (slot != PCI_SLOT(dev->pdev->devfn)) ||
-	    (func != PCI_FUNC(dev->pdev->devfn))) {
-		ret = -EINVAL;
-		goto err;
-	}
 
 	return 0;
 
@@ -159,74 +121,15 @@ err:
 static int drm_set_busid(struct drm_device *dev, struct drm_file *file_priv)
 {
 	struct drm_master *master = file_priv->master;
-	int len, ret;
+	int ret;
 
 	if (master->unique != NULL)
 		drm_unset_busid(dev, master);
 
-	if (drm_core_check_feature(dev, DRIVER_USE_PLATFORM_DEVICE)) {
-		master->unique_len = 10 + strlen(dev->platformdev->name);
-		master->unique = kmalloc(master->unique_len + 1, GFP_KERNEL);
-
-		if (master->unique == NULL)
-			return -ENOMEM;
-
-		len = snprintf(master->unique, master->unique_len,
-			"platform:%s", dev->platformdev->name);
-
-		if (len > master->unique_len) {
-			DRM_ERROR("Unique buffer overflowed\n");
-			ret = -EINVAL;
-			goto err;
-		}
-
-		dev->devname =
-			kmalloc(strlen(dev->platformdev->name) +
-				master->unique_len + 2, GFP_KERNEL);
-
-		if (dev->devname == NULL) {
-			ret = -ENOMEM;
-			goto err;
-		}
-
-		sprintf(dev->devname, "%s@%s", dev->platformdev->name,
-			master->unique);
-
-	} else {
-		master->unique_len = 40;
-		master->unique_size = master->unique_len;
-		master->unique = kmalloc(master->unique_size, GFP_KERNEL);
-		if (master->unique == NULL)
-			return -ENOMEM;
-
-		len = snprintf(master->unique, master->unique_len,
-			"pci:%04x:%02x:%02x.%d",
-			drm_get_pci_domain(dev),
-			dev->pdev->bus->number,
-			PCI_SLOT(dev->pdev->devfn),
-			PCI_FUNC(dev->pdev->devfn));
-		if (len >= master->unique_len) {
-			DRM_ERROR("buffer overflow");
-			ret = -EINVAL;
-			goto err;
-		} else
-			master->unique_len = len;
-
-		dev->devname =
-			kmalloc(strlen(dev->driver->pci_driver.name) +
-				master->unique_len + 2, GFP_KERNEL);
-
-		if (dev->devname == NULL) {
-			ret = -ENOMEM;
-			goto err;
-		}
-
-		sprintf(dev->devname, "%s@%s", dev->driver->pci_driver.name,
-			master->unique);
-	}
-
+	ret = dev->driver->bus->set_busid(dev, master);
+	if (ret)
+		goto err;
 	return 0;
-
 err:
 	drm_unset_busid(dev, master);
 	return ret;
@@ -365,6 +268,25 @@ int drm_getstats(struct drm_device *dev, void *data,
 }
 
 /**
+ * Get device/driver capabilities
+ */
+int drm_getcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
+{
+	struct drm_get_cap *req = data;
+
+	req->value = 0;
+	switch (req->capability) {
+	case DRM_CAP_DUMB_BUFFER:
+		if (dev->driver->dumb_create)
+			req->value = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
  * Setversion ioctl.
  *
  * \param inode device inode.
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index 3dadfa2a8528..a34ef97d3c81 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -74,23 +74,13 @@ int drm_irq_by_busid(struct drm_device *dev, void *data,
 {
 	struct drm_irq_busid *p = data;
 
-	if (drm_core_check_feature(dev, DRIVER_USE_PLATFORM_DEVICE))
+	if (!dev->driver->bus->irq_by_busid)
 		return -EINVAL;
 
 	if (!drm_core_check_feature(dev, DRIVER_HAVE_IRQ))
 		return -EINVAL;
 
-	if ((p->busnum >> 8) != drm_get_pci_domain(dev) ||
-	    (p->busnum & 0xff) != dev->pdev->bus->number ||
-	    p->devnum != PCI_SLOT(dev->pdev->devfn) || p->funcnum != PCI_FUNC(dev->pdev->devfn))
-		return -EINVAL;
-
-	p->irq = dev->pdev->irq;
-
-	DRM_DEBUG("%d:%d:%d => IRQ %d\n", p->busnum, p->devnum, p->funcnum,
-		  p->irq);
-
-	return 0;
+	return dev->driver->bus->irq_by_busid(dev, p);
 }
 
 /*
@@ -164,8 +154,10 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	 * available. In that case we can't account for this and just
 	 * hope for the best.
 	 */
-	if ((vblrc > 0) && (abs(diff_ns) > 1000000))
+	if ((vblrc > 0) && (abs64(diff_ns) > 1000000)) {
 		atomic_inc(&dev->_vblank_count[crtc]);
+		smp_mb__after_atomic_inc();
+	}
 
 	/* Invalidate all timestamps while vblank irq's are off. */
 	clear_vblank_timestamps(dev, crtc);
@@ -491,6 +483,12 @@ void drm_calc_timestamping_constants(struct drm_crtc *crtc)
 	/* Dot clock in Hz: */
 	dotclock = (u64) crtc->hwmode.clock * 1000;
 
+	/* Fields of interlaced scanout modes are only halve a frame duration.
+	 * Double the dotclock to get halve the frame-/line-/pixelduration.
+	 */
+	if (crtc->hwmode.flags & DRM_MODE_FLAG_INTERLACE)
+		dotclock *= 2;
+
 	/* Valid dotclock? */
 	if (dotclock > 0) {
 		/* Convert scanline length in pixels and video dot clock to
@@ -603,14 +601,6 @@ int drm_calc_vbltimestamp_from_scanoutpos(struct drm_device *dev, int crtc,
 		return -EAGAIN;
 	}
 
-	/* Don't know yet how to handle interlaced or
-	 * double scan modes. Just no-op for now.
-	 */
-	if (mode->flags & (DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLSCAN)) {
-		DRM_DEBUG("crtc %d: Noop due to unsupported mode.\n", crtc);
-		return -ENOTSUPP;
-	}
-
 	/* Get current scanout position with system timestamp.
 	 * Repeat query up to DRM_TIMESTAMP_MAXRETRIES times
 	 * if single query takes longer than max_error nanoseconds.
@@ -858,10 +848,11 @@ static void drm_update_vblank_count(struct drm_device *dev, int crtc)
 	if (rc) {
 		tslot = atomic_read(&dev->_vblank_count[crtc]) + diff;
 		vblanktimestamp(dev, crtc, tslot) = t_vblank;
-		smp_wmb();
 	}
 
+	smp_mb__before_atomic_inc();
 	atomic_add(diff, &dev->_vblank_count[crtc]);
+	smp_mb__after_atomic_inc();
 }
 
 /**
@@ -1011,7 +1002,8 @@ int drm_modeset_ctl(struct drm_device *dev, void *data,
 		    struct drm_file *file_priv)
 {
 	struct drm_modeset_ctl *modeset = data;
-	int crtc, ret = 0;
+	int ret = 0;
+	unsigned int crtc;
 
 	/* If drm_vblank_init() hasn't been called yet, just no-op */
 	if (!dev->num_crtcs)
@@ -1293,15 +1285,16 @@ bool drm_handle_vblank(struct drm_device *dev, int crtc)
 	 * e.g., due to spurious vblank interrupts. We need to
 	 * ignore those for accounting.
 	 */
-	if (abs(diff_ns) > DRM_REDUNDANT_VBLIRQ_THRESH_NS) {
+	if (abs64(diff_ns) > DRM_REDUNDANT_VBLIRQ_THRESH_NS) {
 		/* Store new timestamp in ringbuffer. */
 		vblanktimestamp(dev, crtc, vblcount + 1) = tvblank;
-		smp_wmb();
 
 		/* Increment cooked vblank count. This also atomically commits
 		 * the timestamp computed above.
 		 */
+		smp_mb__before_atomic_inc();
 		atomic_inc(&dev->_vblank_count[crtc]);
+		smp_mb__after_atomic_inc();
 	} else {
 		DRM_DEBUG("crtc %d: Redundant vblirq ignored. diff_ns = %d\n",
 			  crtc, (int) diff_ns);
diff --git a/drivers/gpu/drm/drm_mm.c b/drivers/gpu/drm/drm_mm.c
index c59515ba7e69..add1737dae0d 100644
--- a/drivers/gpu/drm/drm_mm.c
+++ b/drivers/gpu/drm/drm_mm.c
@@ -64,8 +64,8 @@ static struct drm_mm_node *drm_mm_kmalloc(struct drm_mm *mm, int atomic)
 		else {
 			child =
 			    list_entry(mm->unused_nodes.next,
-				       struct drm_mm_node, free_stack);
-			list_del(&child->free_stack);
+				       struct drm_mm_node, node_list);
+			list_del(&child->node_list);
 			--mm->num_unused;
 		}
 		spin_unlock(&mm->unused_lock);
@@ -94,195 +94,242 @@ int drm_mm_pre_get(struct drm_mm *mm)
 			return ret;
 		}
 		++mm->num_unused;
-		list_add_tail(&node->free_stack, &mm->unused_nodes);
+		list_add_tail(&node->node_list, &mm->unused_nodes);
 	}
 	spin_unlock(&mm->unused_lock);
 	return 0;
 }
 EXPORT_SYMBOL(drm_mm_pre_get);
 
-static int drm_mm_create_tail_node(struct drm_mm *mm,
-				   unsigned long start,
-				   unsigned long size, int atomic)
+static inline unsigned long drm_mm_hole_node_start(struct drm_mm_node *hole_node)
 {
-	struct drm_mm_node *child;
-
-	child = drm_mm_kmalloc(mm, atomic);
-	if (unlikely(child == NULL))
-		return -ENOMEM;
-
-	child->free = 1;
-	child->size = size;
-	child->start = start;
-	child->mm = mm;
+	return hole_node->start + hole_node->size;
+}
 
-	list_add_tail(&child->node_list, &mm->node_list);
-	list_add_tail(&child->free_stack, &mm->free_stack);
+static inline unsigned long drm_mm_hole_node_end(struct drm_mm_node *hole_node)
+{
+	struct drm_mm_node *next_node =
+		list_entry(hole_node->node_list.next, struct drm_mm_node,
+			   node_list);
 
-	return 0;
+	return next_node->start;
 }
 
-static struct drm_mm_node *drm_mm_split_at_start(struct drm_mm_node *parent,
-						 unsigned long size,
-						 int atomic)
+static void drm_mm_insert_helper(struct drm_mm_node *hole_node,
+				 struct drm_mm_node *node,
+				 unsigned long size, unsigned alignment)
 {
-	struct drm_mm_node *child;
+	struct drm_mm *mm = hole_node->mm;
+	unsigned long tmp = 0, wasted = 0;
+	unsigned long hole_start = drm_mm_hole_node_start(hole_node);
+	unsigned long hole_end = drm_mm_hole_node_end(hole_node);
 
-	child = drm_mm_kmalloc(parent->mm, atomic);
-	if (unlikely(child == NULL))
-		return NULL;
+	BUG_ON(!hole_node->hole_follows || node->allocated);
 
-	INIT_LIST_HEAD(&child->free_stack);
+	if (alignment)
+		tmp = hole_start % alignment;
 
-	child->size = size;
-	child->start = parent->start;
-	child->mm = parent->mm;
+	if (!tmp) {
+		hole_node->hole_follows = 0;
+		list_del_init(&hole_node->hole_stack);
+	} else
+		wasted = alignment - tmp;
 
-	list_add_tail(&child->node_list, &parent->node_list);
-	INIT_LIST_HEAD(&child->free_stack);
+	node->start = hole_start + wasted;
+	node->size = size;
+	node->mm = mm;
+	node->allocated = 1;
 
-	parent->size -= size;
-	parent->start += size;
-	return child;
-}
+	INIT_LIST_HEAD(&node->hole_stack);
+	list_add(&node->node_list, &hole_node->node_list);
+
+	BUG_ON(node->start + node->size > hole_end);
 
+	if (node->start + node->size < hole_end) {
+		list_add(&node->hole_stack, &mm->hole_stack);
+		node->hole_follows = 1;
+	} else {
+		node->hole_follows = 0;
+	}
+}
 
-struct drm_mm_node *drm_mm_get_block_generic(struct drm_mm_node *node,
+struct drm_mm_node *drm_mm_get_block_generic(struct drm_mm_node *hole_node,
 					     unsigned long size,
 					     unsigned alignment,
 					     int atomic)
 {
+	struct drm_mm_node *node;
 
-	struct drm_mm_node *align_splitoff = NULL;
-	unsigned tmp = 0;
+	node = drm_mm_kmalloc(hole_node->mm, atomic);
+	if (unlikely(node == NULL))
+		return NULL;
 
-	if (alignment)
-		tmp = node->start % alignment;
+	drm_mm_insert_helper(hole_node, node, size, alignment);
 
-	if (tmp) {
-		align_splitoff =
-		    drm_mm_split_at_start(node, alignment - tmp, atomic);
-		if (unlikely(align_splitoff == NULL))
-			return NULL;
-	}
+	return node;
+}
+EXPORT_SYMBOL(drm_mm_get_block_generic);
 
-	if (node->size == size) {
-		list_del_init(&node->free_stack);
-		node->free = 0;
-	} else {
-		node = drm_mm_split_at_start(node, size, atomic);
-	}
+/**
+ * Search for free space and insert a preallocated memory node. Returns
+ * -ENOSPC if no suitable free area is available. The preallocated memory node
+ * must be cleared.
+ */
+int drm_mm_insert_node(struct drm_mm *mm, struct drm_mm_node *node,
+		       unsigned long size, unsigned alignment)
+{
+	struct drm_mm_node *hole_node;
 
-	if (align_splitoff)
-		drm_mm_put_block(align_splitoff);
+	hole_node = drm_mm_search_free(mm, size, alignment, 0);
+	if (!hole_node)
+		return -ENOSPC;
 
-	return node;
+	drm_mm_insert_helper(hole_node, node, size, alignment);
+
+	return 0;
 }
-EXPORT_SYMBOL(drm_mm_get_block_generic);
+EXPORT_SYMBOL(drm_mm_insert_node);
 
-struct drm_mm_node *drm_mm_get_block_range_generic(struct drm_mm_node *node,
-						unsigned long size,
-						unsigned alignment,
-						unsigned long start,
-						unsigned long end,
-						int atomic)
+static void drm_mm_insert_helper_range(struct drm_mm_node *hole_node,
+				       struct drm_mm_node *node,
+				       unsigned long size, unsigned alignment,
+				       unsigned long start, unsigned long end)
 {
-	struct drm_mm_node *align_splitoff = NULL;
-	unsigned tmp = 0;
-	unsigned wasted = 0;
+	struct drm_mm *mm = hole_node->mm;
+	unsigned long tmp = 0, wasted = 0;
+	unsigned long hole_start = drm_mm_hole_node_start(hole_node);
+	unsigned long hole_end = drm_mm_hole_node_end(hole_node);
+
+	BUG_ON(!hole_node->hole_follows || node->allocated);
 
-	if (node->start < start)
-		wasted += start - node->start;
+	if (hole_start < start)
+		wasted += start - hole_start;
 	if (alignment)
-		tmp = ((node->start + wasted) % alignment);
+		tmp = (hole_start + wasted) % alignment;
 
 	if (tmp)
 		wasted += alignment - tmp;
-	if (wasted) {
-		align_splitoff = drm_mm_split_at_start(node, wasted, atomic);
-		if (unlikely(align_splitoff == NULL))
-			return NULL;
+
+	if (!wasted) {
+		hole_node->hole_follows = 0;
+		list_del_init(&hole_node->hole_stack);
 	}
 
-	if (node->size == size) {
-		list_del_init(&node->free_stack);
-		node->free = 0;
+	node->start = hole_start + wasted;
+	node->size = size;
+	node->mm = mm;
+	node->allocated = 1;
+
+	INIT_LIST_HEAD(&node->hole_stack);
+	list_add(&node->node_list, &hole_node->node_list);
+
+	BUG_ON(node->start + node->size > hole_end);
+	BUG_ON(node->start + node->size > end);
+
+	if (node->start + node->size < hole_end) {
+		list_add(&node->hole_stack, &mm->hole_stack);
+		node->hole_follows = 1;
 	} else {
-		node = drm_mm_split_at_start(node, size, atomic);
+		node->hole_follows = 0;
 	}
+}
 
-	if (align_splitoff)
-		drm_mm_put_block(align_splitoff);
+struct drm_mm_node *drm_mm_get_block_range_generic(struct drm_mm_node *hole_node,
+						unsigned long size,
+						unsigned alignment,
+						unsigned long start,
+						unsigned long end,
+						int atomic)
+{
+	struct drm_mm_node *node;
+
+	node = drm_mm_kmalloc(hole_node->mm, atomic);
+	if (unlikely(node == NULL))
+		return NULL;
+
+	drm_mm_insert_helper_range(hole_node, node, size, alignment,
+				   start, end);
 
 	return node;
 }
 EXPORT_SYMBOL(drm_mm_get_block_range_generic);
 
-/*
- * Put a block. Merge with the previous and / or next block if they are free.
- * Otherwise add to the free stack.
+/**
+ * Search for free space and insert a preallocated memory node. Returns
+ * -ENOSPC if no suitable free area is available. This is for range
+ * restricted allocations. The preallocated memory node must be cleared.
  */
-
-void drm_mm_put_block(struct drm_mm_node *cur)
+int drm_mm_insert_node_in_range(struct drm_mm *mm, struct drm_mm_node *node,
+				unsigned long size, unsigned alignment,
+				unsigned long start, unsigned long end)
 {
+	struct drm_mm_node *hole_node;
 
-	struct drm_mm *mm = cur->mm;
-	struct list_head *cur_head = &cur->node_list;
-	struct list_head *root_head = &mm->node_list;
-	struct drm_mm_node *prev_node = NULL;
-	struct drm_mm_node *next_node;
+	hole_node = drm_mm_search_free_in_range(mm, size, alignment,
+						start, end, 0);
+	if (!hole_node)
+		return -ENOSPC;
 
-	int merged = 0;
+	drm_mm_insert_helper_range(hole_node, node, size, alignment,
+				   start, end);
 
-	BUG_ON(cur->scanned_block || cur->scanned_prev_free
-				  || cur->scanned_next_free);
+	return 0;
+}
+EXPORT_SYMBOL(drm_mm_insert_node_in_range);
 
-	if (cur_head->prev != root_head) {
-		prev_node =
-		    list_entry(cur_head->prev, struct drm_mm_node, node_list);
-		if (prev_node->free) {
-			prev_node->size += cur->size;
-			merged = 1;
-		}
-	}
-	if (cur_head->next != root_head) {
-		next_node =
-		    list_entry(cur_head->next, struct drm_mm_node, node_list);
-		if (next_node->free) {
-			if (merged) {
-				prev_node->size += next_node->size;
-				list_del(&next_node->node_list);
-				list_del(&next_node->free_stack);
-				spin_lock(&mm->unused_lock);
-				if (mm->num_unused < MM_UNUSED_TARGET) {
-					list_add(&next_node->free_stack,
-						 &mm->unused_nodes);
-					++mm->num_unused;
-				} else
-					kfree(next_node);
-				spin_unlock(&mm->unused_lock);
-			} else {
-				next_node->size += cur->size;
-				next_node->start = cur->start;
-				merged = 1;
-			}
-		}
-	}
-	if (!merged) {
-		cur->free = 1;
-		list_add(&cur->free_stack, &mm->free_stack);
-	} else {
-		list_del(&cur->node_list);
-		spin_lock(&mm->unused_lock);
-		if (mm->num_unused < MM_UNUSED_TARGET) {
-			list_add(&cur->free_stack, &mm->unused_nodes);
-			++mm->num_unused;
-		} else
-			kfree(cur);
-		spin_unlock(&mm->unused_lock);
-	}
+/**
+ * Remove a memory node from the allocator.
+ */
+void drm_mm_remove_node(struct drm_mm_node *node)
+{
+	struct drm_mm *mm = node->mm;
+	struct drm_mm_node *prev_node;
+
+	BUG_ON(node->scanned_block || node->scanned_prev_free
+				   || node->scanned_next_free);
+
+	prev_node =
+	    list_entry(node->node_list.prev, struct drm_mm_node, node_list);
+
+	if (node->hole_follows) {
+		BUG_ON(drm_mm_hole_node_start(node)
+				== drm_mm_hole_node_end(node));
+		list_del(&node->hole_stack);
+	} else
+		BUG_ON(drm_mm_hole_node_start(node)
+				!= drm_mm_hole_node_end(node));
+
+	if (!prev_node->hole_follows) {
+		prev_node->hole_follows = 1;
+		list_add(&prev_node->hole_stack, &mm->hole_stack);
+	} else
+		list_move(&prev_node->hole_stack, &mm->hole_stack);
+
+	list_del(&node->node_list);
+	node->allocated = 0;
 }
+EXPORT_SYMBOL(drm_mm_remove_node);
+
+/*
+ * Remove a memory node from the allocator and free the allocated struct
+ * drm_mm_node. Only to be used on a struct drm_mm_node obtained by one of the
+ * drm_mm_get_block functions.
+ */
+void drm_mm_put_block(struct drm_mm_node *node)
+{
 
+	struct drm_mm *mm = node->mm;
+
+	drm_mm_remove_node(node);
+
+	spin_lock(&mm->unused_lock);
+	if (mm->num_unused < MM_UNUSED_TARGET) {
+		list_add(&node->node_list, &mm->unused_nodes);
+		++mm->num_unused;
+	} else
+		kfree(node);
+	spin_unlock(&mm->unused_lock);
+}
 EXPORT_SYMBOL(drm_mm_put_block);
 
 static int check_free_hole(unsigned long start, unsigned long end,
@@ -319,8 +366,10 @@ struct drm_mm_node *drm_mm_search_free(const struct drm_mm *mm,
 	best = NULL;
 	best_size = ~0UL;
 
-	list_for_each_entry(entry, &mm->free_stack, free_stack) {
-		if (!check_free_hole(entry->start, entry->start + entry->size,
+	list_for_each_entry(entry, &mm->hole_stack, hole_stack) {
+		BUG_ON(!entry->hole_follows);
+		if (!check_free_hole(drm_mm_hole_node_start(entry),
+				     drm_mm_hole_node_end(entry),
 				     size, alignment))
 			continue;
 
@@ -353,12 +402,13 @@ struct drm_mm_node *drm_mm_search_free_in_range(const struct drm_mm *mm,
 	best = NULL;
 	best_size = ~0UL;
 
-	list_for_each_entry(entry, &mm->free_stack, free_stack) {
-		unsigned long adj_start = entry->start < start ?
-			start : entry->start;
-		unsigned long adj_end = entry->start + entry->size > end ?
-			end : entry->start + entry->size;
+	list_for_each_entry(entry, &mm->hole_stack, hole_stack) {
+		unsigned long adj_start = drm_mm_hole_node_start(entry) < start ?
+			start : drm_mm_hole_node_start(entry);
+		unsigned long adj_end = drm_mm_hole_node_end(entry) > end ?
+			end : drm_mm_hole_node_end(entry);
 
+		BUG_ON(!entry->hole_follows);
 		if (!check_free_hole(adj_start, adj_end, size, alignment))
 			continue;
 
@@ -376,6 +426,23 @@ struct drm_mm_node *drm_mm_search_free_in_range(const struct drm_mm *mm,
 EXPORT_SYMBOL(drm_mm_search_free_in_range);
 
 /**
+ * Moves an allocation. To be used with embedded struct drm_mm_node.
+ */
+void drm_mm_replace_node(struct drm_mm_node *old, struct drm_mm_node *new)
+{
+	list_replace(&old->node_list, &new->node_list);
+	list_replace(&old->node_list, &new->hole_stack);
+	new->hole_follows = old->hole_follows;
+	new->mm = old->mm;
+	new->start = old->start;
+	new->size = old->size;
+
+	old->allocated = 0;
+	new->allocated = 1;
+}
+EXPORT_SYMBOL(drm_mm_replace_node);
+
+/**
  * Initializa lru scanning.
  *
  * This simply sets up the scanning routines with the parameters for the desired
@@ -393,6 +460,7 @@ void drm_mm_init_scan(struct drm_mm *mm, unsigned long size,
 	mm->scan_hit_start = 0;
 	mm->scan_hit_size = 0;
 	mm->scan_check_range = 0;
+	mm->prev_scanned_node = NULL;
 }
 EXPORT_SYMBOL(drm_mm_init_scan);
 
@@ -418,6 +486,7 @@ void drm_mm_init_scan_with_range(struct drm_mm *mm, unsigned long size,
 	mm->scan_start = start;
 	mm->scan_end = end;
 	mm->scan_check_range = 1;
+	mm->prev_scanned_node = NULL;
 }
 EXPORT_SYMBOL(drm_mm_init_scan_with_range);
 
@@ -430,70 +499,42 @@ EXPORT_SYMBOL(drm_mm_init_scan_with_range);
 int drm_mm_scan_add_block(struct drm_mm_node *node)
 {
 	struct drm_mm *mm = node->mm;
-	struct list_head *prev_free, *next_free;
-	struct drm_mm_node *prev_node, *next_node;
+	struct drm_mm_node *prev_node;
+	unsigned long hole_start, hole_end;
 	unsigned long adj_start;
 	unsigned long adj_end;
 
 	mm->scanned_blocks++;
 
-	prev_free = next_free = NULL;
-
-	BUG_ON(node->free);
+	BUG_ON(node->scanned_block);
 	node->scanned_block = 1;
-	node->free = 1;
-
-	if (node->node_list.prev != &mm->node_list) {
-		prev_node = list_entry(node->node_list.prev, struct drm_mm_node,
-				       node_list);
-
-		if (prev_node->free) {
-			list_del(&prev_node->node_list);
 
-			node->start = prev_node->start;
-			node->size += prev_node->size;
+	prev_node = list_entry(node->node_list.prev, struct drm_mm_node,
+			       node_list);
 
-			prev_node->scanned_prev_free = 1;
-
-			prev_free = &prev_node->free_stack;
-		}
-	}
-
-	if (node->node_list.next != &mm->node_list) {
-		next_node = list_entry(node->node_list.next, struct drm_mm_node,
-				       node_list);
-
-		if (next_node->free) {
-			list_del(&next_node->node_list);
-
-			node->size += next_node->size;
-
-			next_node->scanned_next_free = 1;
-
-			next_free = &next_node->free_stack;
-		}
-	}
-
-	/* The free_stack list is not used for allocated objects, so these two
-	 * pointers can be abused (as long as no allocations in this memory
-	 * manager happens). */
-	node->free_stack.prev = prev_free;
-	node->free_stack.next = next_free;
+	node->scanned_preceeds_hole = prev_node->hole_follows;
+	prev_node->hole_follows = 1;
+	list_del(&node->node_list);
+	node->node_list.prev = &prev_node->node_list;
+	node->node_list.next = &mm->prev_scanned_node->node_list;
+	mm->prev_scanned_node = node;
 
+	hole_start = drm_mm_hole_node_start(prev_node);
+	hole_end = drm_mm_hole_node_end(prev_node);
 	if (mm->scan_check_range) {
-		adj_start = node->start < mm->scan_start ?
-			mm->scan_start : node->start;
-		adj_end = node->start + node->size > mm->scan_end ?
-			mm->scan_end : node->start + node->size;
+		adj_start = hole_start < mm->scan_start ?
+			mm->scan_start : hole_start;
+		adj_end = hole_end > mm->scan_end ?
+			mm->scan_end : hole_end;
 	} else {
-		adj_start = node->start;
-		adj_end = node->start + node->size;
+		adj_start = hole_start;
+		adj_end = hole_end;
 	}
 
 	if (check_free_hole(adj_start , adj_end,
 			    mm->scan_size, mm->scan_alignment)) {
-		mm->scan_hit_start = node->start;
-		mm->scan_hit_size = node->size;
+		mm->scan_hit_start = hole_start;
+		mm->scan_hit_size = hole_end;
 
 		return 1;
 	}
@@ -519,39 +560,19 @@ EXPORT_SYMBOL(drm_mm_scan_add_block);
 int drm_mm_scan_remove_block(struct drm_mm_node *node)
 {
 	struct drm_mm *mm = node->mm;
-	struct drm_mm_node *prev_node, *next_node;
+	struct drm_mm_node *prev_node;
 
 	mm->scanned_blocks--;
 
 	BUG_ON(!node->scanned_block);
 	node->scanned_block = 0;
-	node->free = 0;
-
-	prev_node = list_entry(node->free_stack.prev, struct drm_mm_node,
-			       free_stack);
-	next_node = list_entry(node->free_stack.next, struct drm_mm_node,
-			       free_stack);
 
-	if (prev_node) {
-		BUG_ON(!prev_node->scanned_prev_free);
-		prev_node->scanned_prev_free = 0;
-
-		list_add_tail(&prev_node->node_list, &node->node_list);
-
-		node->start = prev_node->start + prev_node->size;
-		node->size -= prev_node->size;
-	}
+	prev_node = list_entry(node->node_list.prev, struct drm_mm_node,
+			       node_list);
 
-	if (next_node) {
-		BUG_ON(!next_node->scanned_next_free);
-		next_node->scanned_next_free = 0;
-
-		list_add(&next_node->node_list, &node->node_list);
-
-		node->size -= next_node->size;
-	}
-
-	INIT_LIST_HEAD(&node->free_stack);
+	prev_node->hole_follows = node->scanned_preceeds_hole;
+	INIT_LIST_HEAD(&node->node_list);
+	list_add(&node->node_list, &prev_node->node_list);
 
 	/* Only need to check for containement because start&size for the
 	 * complete resulting free block (not just the desired part) is
@@ -568,7 +589,7 @@ EXPORT_SYMBOL(drm_mm_scan_remove_block);
 
 int drm_mm_clean(struct drm_mm * mm)
 {
-	struct list_head *head = &mm->node_list;
+	struct list_head *head = &mm->head_node.node_list;
 
 	return (head->next->next == head);
 }
@@ -576,38 +597,40 @@ EXPORT_SYMBOL(drm_mm_clean);
 
 int drm_mm_init(struct drm_mm * mm, unsigned long start, unsigned long size)
 {
-	INIT_LIST_HEAD(&mm->node_list);
-	INIT_LIST_HEAD(&mm->free_stack);
+	INIT_LIST_HEAD(&mm->hole_stack);
 	INIT_LIST_HEAD(&mm->unused_nodes);
 	mm->num_unused = 0;
 	mm->scanned_blocks = 0;
 	spin_lock_init(&mm->unused_lock);
 
-	return drm_mm_create_tail_node(mm, start, size, 0);
+	/* Clever trick to avoid a special case in the free hole tracking. */
+	INIT_LIST_HEAD(&mm->head_node.node_list);
+	INIT_LIST_HEAD(&mm->head_node.hole_stack);
+	mm->head_node.hole_follows = 1;
+	mm->head_node.scanned_block = 0;
+	mm->head_node.scanned_prev_free = 0;
+	mm->head_node.scanned_next_free = 0;
+	mm->head_node.mm = mm;
+	mm->head_node.start = start + size;
+	mm->head_node.size = start - mm->head_node.start;
+	list_add_tail(&mm->head_node.hole_stack, &mm->hole_stack);
+
+	return 0;
 }
 EXPORT_SYMBOL(drm_mm_init);
 
 void drm_mm_takedown(struct drm_mm * mm)
 {
-	struct list_head *bnode = mm->free_stack.next;
-	struct drm_mm_node *entry;
-	struct drm_mm_node *next;
+	struct drm_mm_node *entry, *next;
 
-	entry = list_entry(bnode, struct drm_mm_node, free_stack);
-
-	if (entry->node_list.next != &mm->node_list ||
-	    entry->free_stack.next != &mm->free_stack) {
+	if (!list_empty(&mm->head_node.node_list)) {
 		DRM_ERROR("Memory manager not clean. Delaying takedown\n");
 		return;
 	}
 
-	list_del(&entry->free_stack);
-	list_del(&entry->node_list);
-	kfree(entry);
-
 	spin_lock(&mm->unused_lock);
-	list_for_each_entry_safe(entry, next, &mm->unused_nodes, free_stack) {
-		list_del(&entry->free_stack);
+	list_for_each_entry_safe(entry, next, &mm->unused_nodes, node_list) {
+		list_del(&entry->node_list);
 		kfree(entry);
 		--mm->num_unused;
 	}
@@ -620,19 +643,37 @@ EXPORT_SYMBOL(drm_mm_takedown);
 void drm_mm_debug_table(struct drm_mm *mm, const char *prefix)
 {
 	struct drm_mm_node *entry;
-	int total_used = 0, total_free = 0, total = 0;
-
-	list_for_each_entry(entry, &mm->node_list, node_list) {
-		printk(KERN_DEBUG "%s 0x%08lx-0x%08lx: %8ld: %s\n",
+	unsigned long total_used = 0, total_free = 0, total = 0;
+	unsigned long hole_start, hole_end, hole_size;
+
+	hole_start = drm_mm_hole_node_start(&mm->head_node);
+	hole_end = drm_mm_hole_node_end(&mm->head_node);
+	hole_size = hole_end - hole_start;
+	if (hole_size)
+		printk(KERN_DEBUG "%s 0x%08lx-0x%08lx: %8lu: free\n",
+			prefix, hole_start, hole_end,
+			hole_size);
+	total_free += hole_size;
+
+	drm_mm_for_each_node(entry, mm) {
+		printk(KERN_DEBUG "%s 0x%08lx-0x%08lx: %8lu: used\n",
 			prefix, entry->start, entry->start + entry->size,
-			entry->size, entry->free ? "free" : "used");
-		total += entry->size;
-		if (entry->free)
-			total_free += entry->size;
-		else
-			total_used += entry->size;
+			entry->size);
+		total_used += entry->size;
+
+		if (entry->hole_follows) {
+			hole_start = drm_mm_hole_node_start(entry);
+			hole_end = drm_mm_hole_node_end(entry);
+			hole_size = hole_end - hole_start;
+			printk(KERN_DEBUG "%s 0x%08lx-0x%08lx: %8lu: free\n",
+				prefix, hole_start, hole_end,
+				hole_size);
+			total_free += hole_size;
+		}
 	}
-	printk(KERN_DEBUG "%s total: %d, used %d free %d\n", prefix, total,
+	total = total_free + total_used;
+
+	printk(KERN_DEBUG "%s total: %lu, used %lu free %lu\n", prefix, total,
 		total_used, total_free);
 }
 EXPORT_SYMBOL(drm_mm_debug_table);
@@ -641,17 +682,34 @@ EXPORT_SYMBOL(drm_mm_debug_table);
 int drm_mm_dump_table(struct seq_file *m, struct drm_mm *mm)
 {
 	struct drm_mm_node *entry;
-	int total_used = 0, total_free = 0, total = 0;
-
-	list_for_each_entry(entry, &mm->node_list, node_list) {
-		seq_printf(m, "0x%08lx-0x%08lx: 0x%08lx: %s\n", entry->start, entry->start + entry->size, entry->size, entry->free ? "free" : "used");
-		total += entry->size;
-		if (entry->free)
-			total_free += entry->size;
-		else
-			total_used += entry->size;
+	unsigned long total_used = 0, total_free = 0, total = 0;
+	unsigned long hole_start, hole_end, hole_size;
+
+	hole_start = drm_mm_hole_node_start(&mm->head_node);
+	hole_end = drm_mm_hole_node_end(&mm->head_node);
+	hole_size = hole_end - hole_start;
+	if (hole_size)
+		seq_printf(m, "0x%08lx-0x%08lx: 0x%08lx: free\n",
+				hole_start, hole_end, hole_size);
+	total_free += hole_size;
+
+	drm_mm_for_each_node(entry, mm) {
+		seq_printf(m, "0x%08lx-0x%08lx: 0x%08lx: used\n",
+				entry->start, entry->start + entry->size,
+				entry->size);
+		total_used += entry->size;
+		if (entry->hole_follows) {
+			hole_start = drm_mm_hole_node_start(&mm->head_node);
+			hole_end = drm_mm_hole_node_end(&mm->head_node);
+			hole_size = hole_end - hole_start;
+			seq_printf(m, "0x%08lx-0x%08lx: 0x%08lx: free\n",
+					hole_start, hole_end, hole_size);
+			total_free += hole_size;
+		}
 	}
-	seq_printf(m, "total: %d, used %d free %d\n", total, total_used, total_free);
+	total = total_free + total_used;
+
+	seq_printf(m, "total: %lu, used %lu free %lu\n", total, total_used, total_free);
 	return 0;
 }
 EXPORT_SYMBOL(drm_mm_dump_table);
diff --git a/drivers/gpu/drm/drm_modes.c b/drivers/gpu/drm/drm_modes.c
index 58e65f92c232..25bf87390f53 100644
--- a/drivers/gpu/drm/drm_modes.c
+++ b/drivers/gpu/drm/drm_modes.c
@@ -593,7 +593,7 @@ EXPORT_SYMBOL(drm_mode_height);
  *
  * Return @modes's hsync rate in kHz, rounded to the nearest int.
  */
-int drm_mode_hsync(struct drm_display_mode *mode)
+int drm_mode_hsync(const struct drm_display_mode *mode)
 {
 	unsigned int calc_val;
 
@@ -627,7 +627,7 @@ EXPORT_SYMBOL(drm_mode_hsync);
  * If it is 70.288, it will return 70Hz.
  * If it is 59.6, it will return 60Hz.
  */
-int drm_mode_vrefresh(struct drm_display_mode *mode)
+int drm_mode_vrefresh(const struct drm_display_mode *mode)
 {
 	int refresh = 0;
 	unsigned int calc_val;
@@ -725,7 +725,7 @@ EXPORT_SYMBOL(drm_mode_set_crtcinfo);
  * a pointer to it.  Used to create new instances of established modes.
  */
 struct drm_display_mode *drm_mode_duplicate(struct drm_device *dev,
-					    struct drm_display_mode *mode)
+					    const struct drm_display_mode *mode)
 {
 	struct drm_display_mode *nmode;
 	int new_id;
diff --git a/drivers/gpu/drm/drm_pci.c b/drivers/gpu/drm/drm_pci.c
index f5bd9e590c80..e1aee4f6a7c6 100644
--- a/drivers/gpu/drm/drm_pci.c
+++ b/drivers/gpu/drm/drm_pci.c
@@ -125,6 +125,176 @@ void drm_pci_free(struct drm_device * dev, drm_dma_handle_t * dmah)
 EXPORT_SYMBOL(drm_pci_free);
 
 #ifdef CONFIG_PCI
+
+static int drm_get_pci_domain(struct drm_device *dev)
+{
+#ifndef __alpha__
+	/* For historical reasons, drm_get_pci_domain() is busticated
+	 * on most archs and has to remain so for userspace interface
+	 * < 1.4, except on alpha which was right from the beginning
+	 */
+	if (dev->if_version < 0x10004)
+		return 0;
+#endif /* __alpha__ */
+
+	return pci_domain_nr(dev->pdev->bus);
+}
+
+static int drm_pci_get_irq(struct drm_device *dev)
+{
+	return dev->pdev->irq;
+}
+
+static const char *drm_pci_get_name(struct drm_device *dev)
+{
+	struct pci_driver *pdriver = dev->driver->kdriver.pci;
+	return pdriver->name;
+}
+
+int drm_pci_set_busid(struct drm_device *dev, struct drm_master *master)
+{
+	int len, ret;
+	struct pci_driver *pdriver = dev->driver->kdriver.pci;
+	master->unique_len = 40;
+	master->unique_size = master->unique_len;
+	master->unique = kmalloc(master->unique_size, GFP_KERNEL);
+	if (master->unique == NULL)
+		return -ENOMEM;
+
+
+	len = snprintf(master->unique, master->unique_len,
+		       "pci:%04x:%02x:%02x.%d",
+		       drm_get_pci_domain(dev),
+		       dev->pdev->bus->number,
+		       PCI_SLOT(dev->pdev->devfn),
+		       PCI_FUNC(dev->pdev->devfn));
+
+	if (len >= master->unique_len) {
+		DRM_ERROR("buffer overflow");
+		ret = -EINVAL;
+		goto err;
+	} else
+		master->unique_len = len;
+
+	dev->devname =
+		kmalloc(strlen(pdriver->name) +
+			master->unique_len + 2, GFP_KERNEL);
+
+	if (dev->devname == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	sprintf(dev->devname, "%s@%s", pdriver->name,
+		master->unique);
+
+	return 0;
+err:
+	return ret;
+}
+
+int drm_pci_set_unique(struct drm_device *dev,
+		       struct drm_master *master,
+		       struct drm_unique *u)
+{
+	int domain, bus, slot, func, ret;
+	const char *bus_name;
+
+	master->unique_len = u->unique_len;
+	master->unique_size = u->unique_len + 1;
+	master->unique = kmalloc(master->unique_size, GFP_KERNEL);
+	if (!master->unique) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	if (copy_from_user(master->unique, u->unique, master->unique_len)) {
+		ret = -EFAULT;
+		goto err;
+	}
+
+	master->unique[master->unique_len] = '\0';
+
+	bus_name = dev->driver->bus->get_name(dev);
+	dev->devname = kmalloc(strlen(bus_name) +
+			       strlen(master->unique) + 2, GFP_KERNEL);
+	if (!dev->devname) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	sprintf(dev->devname, "%s@%s", bus_name,
+		master->unique);
+
+	/* Return error if the busid submitted doesn't match the device's actual
+	 * busid.
+	 */
+	ret = sscanf(master->unique, "PCI:%d:%d:%d", &bus, &slot, &func);
+	if (ret != 3) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	domain = bus >> 8;
+	bus &= 0xff;
+
+	if ((domain != drm_get_pci_domain(dev)) ||
+	    (bus != dev->pdev->bus->number) ||
+	    (slot != PCI_SLOT(dev->pdev->devfn)) ||
+	    (func != PCI_FUNC(dev->pdev->devfn))) {
+		ret = -EINVAL;
+		goto err;
+	}
+	return 0;
+err:
+	return ret;
+}
+
+
+int drm_pci_irq_by_busid(struct drm_device *dev, struct drm_irq_busid *p)
+{
+	if ((p->busnum >> 8) != drm_get_pci_domain(dev) ||
+	    (p->busnum & 0xff) != dev->pdev->bus->number ||
+	    p->devnum != PCI_SLOT(dev->pdev->devfn) || p->funcnum != PCI_FUNC(dev->pdev->devfn))
+		return -EINVAL;
+
+	p->irq = dev->pdev->irq;
+
+	DRM_DEBUG("%d:%d:%d => IRQ %d\n", p->busnum, p->devnum, p->funcnum,
+		  p->irq);
+	return 0;
+}
+
+int drm_pci_agp_init(struct drm_device *dev)
+{
+	if (drm_core_has_AGP(dev)) {
+		if (drm_pci_device_is_agp(dev))
+			dev->agp = drm_agp_init(dev);
+		if (drm_core_check_feature(dev, DRIVER_REQUIRE_AGP)
+		    && (dev->agp == NULL)) {
+			DRM_ERROR("Cannot initialize the agpgart module.\n");
+			return -EINVAL;
+		}
+		if (drm_core_has_MTRR(dev)) {
+			if (dev->agp)
+				dev->agp->agp_mtrr =
+					mtrr_add(dev->agp->agp_info.aper_base,
+						 dev->agp->agp_info.aper_size *
+						 1024 * 1024, MTRR_TYPE_WRCOMB, 1);
+		}
+	}
+	return 0;
+}
+
+static struct drm_bus drm_pci_bus = {
+	.bus_type = DRIVER_BUS_PCI,
+	.get_irq = drm_pci_get_irq,
+	.get_name = drm_pci_get_name,
+	.set_busid = drm_pci_set_busid,
+	.set_unique = drm_pci_set_unique,
+	.agp_init = drm_pci_agp_init,
+};
+
 /**
  * Register.
  *
@@ -219,7 +389,7 @@ err_g1:
 EXPORT_SYMBOL(drm_get_pci_dev);
 
 /**
- * PCI device initialization. Called via drm_init at module load time,
+ * PCI device initialization. Called direct from modules at load time.
  *
  * \return zero on success or a negative number on failure.
  *
@@ -229,18 +399,24 @@ EXPORT_SYMBOL(drm_get_pci_dev);
  * Expands the \c DRIVER_PREINIT and \c DRIVER_POST_INIT macros before and
  * after the initialization for driver customization.
  */
-int drm_pci_init(struct drm_driver *driver)
+int drm_pci_init(struct drm_driver *driver, struct pci_driver *pdriver)
 {
 	struct pci_dev *pdev = NULL;
 	const struct pci_device_id *pid;
 	int i;
 
+	DRM_DEBUG("\n");
+
+	INIT_LIST_HEAD(&driver->device_list);
+	driver->kdriver.pci = pdriver;
+	driver->bus = &drm_pci_bus;
+
 	if (driver->driver_features & DRIVER_MODESET)
-		return pci_register_driver(&driver->pci_driver);
+		return pci_register_driver(pdriver);
 
 	/* If not using KMS, fall back to stealth mode manual scanning. */
-	for (i = 0; driver->pci_driver.id_table[i].vendor != 0; i++) {
-		pid = &driver->pci_driver.id_table[i];
+	for (i = 0; pdriver->id_table[i].vendor != 0; i++) {
+		pid = &pdriver->id_table[i];
 
 		/* Loop around setting up a DRM device for each PCI device
 		 * matching our ID and device class.  If we had the internal
@@ -265,10 +441,27 @@ int drm_pci_init(struct drm_driver *driver)
 
 #else
 
-int drm_pci_init(struct drm_driver *driver)
+int drm_pci_init(struct drm_driver *driver, struct pci_driver *pdriver)
 {
 	return -1;
 }
 
 #endif
+
+EXPORT_SYMBOL(drm_pci_init);
+
 /*@}*/
+void drm_pci_exit(struct drm_driver *driver, struct pci_driver *pdriver)
+{
+	struct drm_device *dev, *tmp;
+	DRM_DEBUG("\n");
+
+	if (driver->driver_features & DRIVER_MODESET) {
+		pci_unregister_driver(pdriver);
+	} else {
+		list_for_each_entry_safe(dev, tmp, &driver->device_list, driver_item)
+			drm_put_dev(dev);
+	}
+	DRM_INFO("Module unloaded\n");
+}
+EXPORT_SYMBOL(drm_pci_exit);
diff --git a/drivers/gpu/drm/drm_platform.c b/drivers/gpu/drm/drm_platform.c
index 92d1d0fb7b75..7223f06d8e58 100644
--- a/drivers/gpu/drm/drm_platform.c
+++ b/drivers/gpu/drm/drm_platform.c
@@ -109,8 +109,60 @@ err_g1:
 }
 EXPORT_SYMBOL(drm_get_platform_dev);
 
+static int drm_platform_get_irq(struct drm_device *dev)
+{
+	return platform_get_irq(dev->platformdev, 0);
+}
+
+static const char *drm_platform_get_name(struct drm_device *dev)
+{
+	return dev->platformdev->name;
+}
+
+static int drm_platform_set_busid(struct drm_device *dev, struct drm_master *master)
+{
+	int len, ret;
+
+	master->unique_len = 10 + strlen(dev->platformdev->name);
+	master->unique = kmalloc(master->unique_len + 1, GFP_KERNEL);
+
+	if (master->unique == NULL)
+		return -ENOMEM;
+
+	len = snprintf(master->unique, master->unique_len,
+		       "platform:%s", dev->platformdev->name);
+
+	if (len > master->unique_len) {
+		DRM_ERROR("Unique buffer overflowed\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	dev->devname =
+		kmalloc(strlen(dev->platformdev->name) +
+			master->unique_len + 2, GFP_KERNEL);
+
+	if (dev->devname == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	sprintf(dev->devname, "%s@%s", dev->platformdev->name,
+		master->unique);
+	return 0;
+err:
+	return ret;
+}
+
+static struct drm_bus drm_platform_bus = {
+	.bus_type = DRIVER_BUS_PLATFORM,
+	.get_irq = drm_platform_get_irq,
+	.get_name = drm_platform_get_name,
+	.set_busid = drm_platform_set_busid,
+};
+
 /**
- * Platform device initialization. Called via drm_init at module load time,
+ * Platform device initialization. Called direct from modules.
  *
  * \return zero on success or a negative number on failure.
  *
@@ -121,7 +173,24 @@ EXPORT_SYMBOL(drm_get_platform_dev);
  * after the initialization for driver customization.
  */
 
-int drm_platform_init(struct drm_driver *driver)
+int drm_platform_init(struct drm_driver *driver, struct platform_device *platform_device)
 {
-	return drm_get_platform_dev(driver->platform_device, driver);
+	DRM_DEBUG("\n");
+
+	driver->kdriver.platform_device = platform_device;
+	driver->bus = &drm_platform_bus;
+	INIT_LIST_HEAD(&driver->device_list);
+	return drm_get_platform_dev(platform_device, driver);
+}
+EXPORT_SYMBOL(drm_platform_init);
+
+void drm_platform_exit(struct drm_driver *driver, struct platform_device *platform_device)
+{
+	struct drm_device *dev, *tmp;
+	DRM_DEBUG("\n");
+
+	list_for_each_entry_safe(dev, tmp, &driver->device_list, driver_item)
+		drm_put_dev(dev);
+	DRM_INFO("Module unloaded\n");
 }
+EXPORT_SYMBOL(drm_platform_exit);
diff --git a/drivers/gpu/drm/drm_sman.c b/drivers/gpu/drm/drm_sman.c
index 463aed9403db..34664587a74e 100644
--- a/drivers/gpu/drm/drm_sman.c
+++ b/drivers/gpu/drm/drm_sman.c
@@ -59,9 +59,7 @@ drm_sman_init(struct drm_sman * sman, unsigned int num_managers,
 {
 	int ret = 0;
 
-	sman->mm = (struct drm_sman_mm *) kcalloc(num_managers,
-						  sizeof(*sman->mm),
-						  GFP_KERNEL);
+	sman->mm = kcalloc(num_managers, sizeof(*sman->mm), GFP_KERNEL);
 	if (!sman->mm) {
 		ret = -ENOMEM;
 		goto out;
diff --git a/drivers/gpu/drm/drm_stub.c b/drivers/gpu/drm/drm_stub.c
index d59edc18301f..001273d57f2d 100644
--- a/drivers/gpu/drm/drm_stub.c
+++ b/drivers/gpu/drm/drm_stub.c
@@ -269,25 +269,14 @@ int drm_fill_in_dev(struct drm_device *dev,
 
 	dev->driver = driver;
 
-	if (drm_core_has_AGP(dev)) {
-		if (drm_device_is_agp(dev))
-			dev->agp = drm_agp_init(dev);
-		if (drm_core_check_feature(dev, DRIVER_REQUIRE_AGP)
-		    && (dev->agp == NULL)) {
-			DRM_ERROR("Cannot initialize the agpgart module.\n");
-			retcode = -EINVAL;
+	if (dev->driver->bus->agp_init) {
+		retcode = dev->driver->bus->agp_init(dev);
+		if (retcode)
 			goto error_out_unreg;
-		}
-		if (drm_core_has_MTRR(dev)) {
-			if (dev->agp)
-				dev->agp->agp_mtrr =
-				    mtrr_add(dev->agp->agp_info.aper_base,
-					     dev->agp->agp_info.aper_size *
-					     1024 * 1024, MTRR_TYPE_WRCOMB, 1);
-		}
 	}
 
 
+
 	retcode = drm_ctxbitmap_init(dev);
 	if (retcode) {
 		DRM_ERROR("Cannot allocate memory for context bitmap.\n");
@@ -425,7 +414,6 @@ int drm_put_minor(struct drm_minor **minor_p)
  *
  * Cleans up all DRM device, calling drm_lastclose().
  *
- * \sa drm_init
  */
 void drm_put_dev(struct drm_device *dev)
 {
@@ -475,6 +463,7 @@ void drm_put_dev(struct drm_device *dev)
 
 	drm_put_minor(&dev->primary);
 
+	list_del(&dev->driver_item);
 	if (dev->devname) {
 		kfree(dev->devname);
 		dev->devname = NULL;
diff --git a/drivers/gpu/drm/drm_usb.c b/drivers/gpu/drm/drm_usb.c
new file mode 100644
index 000000000000..206d2300d873
--- /dev/null
+++ b/drivers/gpu/drm/drm_usb.c
@@ -0,0 +1,117 @@
+#include "drmP.h"
+#include <linux/usb.h>
+
+#ifdef CONFIG_USB
+int drm_get_usb_dev(struct usb_interface *interface,
+		    const struct usb_device_id *id,
+		    struct drm_driver *driver)
+{
+	struct drm_device *dev;
+	struct usb_device *usbdev;
+	int ret;
+
+	DRM_DEBUG("\n");
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	usbdev = interface_to_usbdev(interface);
+	dev->usbdev = usbdev;
+	dev->dev = &usbdev->dev;
+
+	mutex_lock(&drm_global_mutex);
+
+	ret = drm_fill_in_dev(dev, NULL, driver);
+	if (ret) {
+		printk(KERN_ERR "DRM: Fill_in_dev failed.\n");
+		goto err_g1;
+	}
+
+	usb_set_intfdata(interface, dev);
+	ret = drm_get_minor(dev, &dev->control, DRM_MINOR_CONTROL);
+	if (ret)
+		goto err_g1;
+
+	ret = drm_get_minor(dev, &dev->primary, DRM_MINOR_LEGACY);
+	if (ret)
+		goto err_g2;
+
+	if (dev->driver->load) {
+		ret = dev->driver->load(dev, 0);
+		if (ret)
+			goto err_g3;
+	}
+
+	/* setup the grouping for the legacy output */
+	ret = drm_mode_group_init_legacy_group(dev,
+					       &dev->primary->mode_group);
+	if (ret)
+		goto err_g3;
+
+	list_add_tail(&dev->driver_item, &driver->device_list);
+
+	mutex_unlock(&drm_global_mutex);
+
+	DRM_INFO("Initialized %s %d.%d.%d %s on minor %d\n",
+		 driver->name, driver->major, driver->minor, driver->patchlevel,
+		 driver->date, dev->primary->index);
+
+	return 0;
+
+err_g3:
+	drm_put_minor(&dev->primary);
+err_g2:
+	drm_put_minor(&dev->control);
+err_g1:
+	kfree(dev);
+	mutex_unlock(&drm_global_mutex);
+	return ret;
+
+}
+EXPORT_SYMBOL(drm_get_usb_dev);
+
+static int drm_usb_get_irq(struct drm_device *dev)
+{
+	return 0;
+}
+
+static const char *drm_usb_get_name(struct drm_device *dev)
+{
+	return "USB";
+}
+
+static int drm_usb_set_busid(struct drm_device *dev,
+			       struct drm_master *master)
+{
+	return 0;
+}
+
+static struct drm_bus drm_usb_bus = {
+	.bus_type = DRIVER_BUS_USB,
+	.get_irq = drm_usb_get_irq,
+	.get_name = drm_usb_get_name,
+	.set_busid = drm_usb_set_busid,
+};
+    
+int drm_usb_init(struct drm_driver *driver, struct usb_driver *udriver)
+{
+	int res;
+	DRM_DEBUG("\n");
+
+	INIT_LIST_HEAD(&driver->device_list);
+	driver->kdriver.usb = udriver;
+	driver->bus = &drm_usb_bus;
+
+	res = usb_register(udriver);
+	return res;
+}
+EXPORT_SYMBOL(drm_usb_init);
+
+void drm_usb_exit(struct drm_driver *driver,
+		  struct usb_driver *udriver)
+{
+	usb_deregister(udriver);
+}
+EXPORT_SYMBOL(drm_usb_exit);
+#endif
diff --git a/drivers/gpu/drm/i810/i810_dma.c b/drivers/gpu/drm/i810/i810_dma.c
index ff33e53bbbf8..8f371e8d630f 100644
--- a/drivers/gpu/drm/i810/i810_dma.c
+++ b/drivers/gpu/drm/i810/i810_dma.c
@@ -37,7 +37,6 @@
 #include <linux/interrupt.h>	/* For task queue support */
 #include <linux/delay.h>
 #include <linux/slab.h>
-#include <linux/smp_lock.h>
 #include <linux/pagemap.h>
 
 #define I810_BUF_FREE		2
@@ -94,7 +93,6 @@ static int i810_mmap_buffers(struct file *filp, struct vm_area_struct *vma)
 	struct drm_buf *buf;
 	drm_i810_buf_priv_t *buf_priv;
 
-	lock_kernel();
 	dev = priv->minor->dev;
 	dev_priv = dev->dev_private;
 	buf = dev_priv->mmap_buffer;
@@ -104,7 +102,6 @@ static int i810_mmap_buffers(struct file *filp, struct vm_area_struct *vma)
 	vma->vm_file = filp;
 
 	buf_priv->currently_mapped = I810_BUF_MAPPED;
-	unlock_kernel();
 
 	if (io_remap_pfn_range(vma, vma->vm_start,
 			       vma->vm_pgoff,
@@ -116,7 +113,7 @@ static int i810_mmap_buffers(struct file *filp, struct vm_area_struct *vma)
 static const struct file_operations i810_buffer_fops = {
 	.open = drm_open,
 	.release = drm_release,
-	.unlocked_ioctl = i810_ioctl,
+	.unlocked_ioctl = drm_ioctl,
 	.mmap = i810_mmap_buffers,
 	.fasync = drm_fasync,
 	.llseek = noop_llseek,
@@ -1242,19 +1239,6 @@ int i810_driver_dma_quiescent(struct drm_device *dev)
 	return 0;
 }
 
-/*
- * call the drm_ioctl under the big kernel lock because
- * to lock against the i810_mmap_buffers function.
- */
-long i810_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	int ret;
-	lock_kernel();
-	ret = drm_ioctl(file, cmd, arg);
-	unlock_kernel();
-	return ret;
-}
-
 struct drm_ioctl_desc i810_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(I810_INIT, i810_dma_init, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I810_VERTEX, i810_dma_vertex, DRM_AUTH|DRM_UNLOCKED),
diff --git a/drivers/gpu/drm/i810/i810_drv.c b/drivers/gpu/drm/i810/i810_drv.c
index 88bcd331e7c5..6f98d059f68a 100644
--- a/drivers/gpu/drm/i810/i810_drv.c
+++ b/drivers/gpu/drm/i810/i810_drv.c
@@ -57,18 +57,13 @@ static struct drm_driver driver = {
 		 .owner = THIS_MODULE,
 		 .open = drm_open,
 		 .release = drm_release,
-		 .unlocked_ioctl = i810_ioctl,
+		 .unlocked_ioctl = drm_ioctl,
 		 .mmap = drm_mmap,
 		 .poll = drm_poll,
 		 .fasync = drm_fasync,
 		 .llseek = noop_llseek,
 	},
 
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-	},
-
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -77,15 +72,24 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver i810_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init i810_init(void)
 {
+	if (num_possible_cpus() > 1) {
+		pr_err("drm/i810 does not support SMP\n");
+		return -EINVAL;
+	}
 	driver.num_ioctls = i810_max_ioctl;
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &i810_pci_driver);
 }
 
 static void __exit i810_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &i810_pci_driver);
 }
 
 module_init(i810_init);
diff --git a/drivers/gpu/drm/i830/Makefile b/drivers/gpu/drm/i830/Makefile
deleted file mode 100644
index c642ee0b238c..000000000000
--- a/drivers/gpu/drm/i830/Makefile
+++ /dev/null
@@ -1,8 +0,0 @@
-#
-# 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
-i830-y := i830_drv.o i830_dma.o i830_irq.o
-
-obj-$(CONFIG_DRM_I830) += i830.o
diff --git a/drivers/gpu/drm/i830/i830_dma.c b/drivers/gpu/drm/i830/i830_dma.c
deleted file mode 100644
index ca6f31ff0eec..000000000000
--- a/drivers/gpu/drm/i830/i830_dma.c
+++ /dev/null
@@ -1,1560 +0,0 @@
-/* i830_dma.c -- DMA support for the I830 -*- linux-c -*-
- * Created: Mon Dec 13 01:50:01 1999 by jhartmann@precisioninsight.com
- *
- * Copyright 1999 Precision Insight, Inc., Cedar Park, Texas.
- * Copyright 2000 VA Linux Systems, Inc., Sunnyvale, 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: Rickard E. (Rik) Faith <faith@valinux.com>
- *	    Jeff Hartmann <jhartmann@valinux.com>
- *	    Keith Whitwell <keith@tungstengraphics.com>
- *	    Abraham vd Merwe <abraham@2d3d.co.za>
- *
- */
-
-#include "drmP.h"
-#include "drm.h"
-#include "i830_drm.h"
-#include "i830_drv.h"
-#include <linux/interrupt.h>	/* For task queue support */
-#include <linux/smp_lock.h>
-#include <linux/pagemap.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <asm/uaccess.h>
-
-#define I830_BUF_FREE		2
-#define I830_BUF_CLIENT		1
-#define I830_BUF_HARDWARE	0
-
-#define I830_BUF_UNMAPPED 0
-#define I830_BUF_MAPPED   1
-
-static struct drm_buf *i830_freelist_get(struct drm_device * dev)
-{
-	struct drm_device_dma *dma = dev->dma;
-	int i;
-	int used;
-
-	/* Linear search might not be the best solution */
-
-	for (i = 0; i < dma->buf_count; i++) {
-		struct drm_buf *buf = dma->buflist[i];
-		drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-		/* In use is already a pointer */
-		used = cmpxchg(buf_priv->in_use, I830_BUF_FREE,
-			       I830_BUF_CLIENT);
-		if (used == I830_BUF_FREE)
-			return buf;
-	}
-	return NULL;
-}
-
-/* This should only be called if the buffer is not sent to the hardware
- * yet, the hardware updates in use for us once its on the ring buffer.
- */
-
-static int i830_freelist_put(struct drm_device *dev, struct drm_buf *buf)
-{
-	drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-	int used;
-
-	/* In use is already a pointer */
-	used = cmpxchg(buf_priv->in_use, I830_BUF_CLIENT, I830_BUF_FREE);
-	if (used != I830_BUF_CLIENT) {
-		DRM_ERROR("Freeing buffer thats not in use : %d\n", buf->idx);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int i830_mmap_buffers(struct file *filp, struct vm_area_struct *vma)
-{
-	struct drm_file *priv = filp->private_data;
-	struct drm_device *dev;
-	drm_i830_private_t *dev_priv;
-	struct drm_buf *buf;
-	drm_i830_buf_priv_t *buf_priv;
-
-	lock_kernel();
-	dev = priv->minor->dev;
-	dev_priv = dev->dev_private;
-	buf = dev_priv->mmap_buffer;
-	buf_priv = buf->dev_private;
-
-	vma->vm_flags |= (VM_IO | VM_DONTCOPY);
-	vma->vm_file = filp;
-
-	buf_priv->currently_mapped = I830_BUF_MAPPED;
-	unlock_kernel();
-
-	if (io_remap_pfn_range(vma, vma->vm_start,
-			       vma->vm_pgoff,
-			       vma->vm_end - vma->vm_start, vma->vm_page_prot))
-		return -EAGAIN;
-	return 0;
-}
-
-static const struct file_operations i830_buffer_fops = {
-	.open = drm_open,
-	.release = drm_release,
-	.unlocked_ioctl = i830_ioctl,
-	.mmap = i830_mmap_buffers,
-	.fasync = drm_fasync,
-	.llseek = noop_llseek,
-};
-
-static int i830_map_buffer(struct drm_buf *buf, struct drm_file *file_priv)
-{
-	struct drm_device *dev = file_priv->minor->dev;
-	drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	const struct file_operations *old_fops;
-	unsigned long virtual;
-	int retcode = 0;
-
-	if (buf_priv->currently_mapped == I830_BUF_MAPPED)
-		return -EINVAL;
-
-	down_write(&current->mm->mmap_sem);
-	old_fops = file_priv->filp->f_op;
-	file_priv->filp->f_op = &i830_buffer_fops;
-	dev_priv->mmap_buffer = buf;
-	virtual = do_mmap(file_priv->filp, 0, buf->total, PROT_READ | PROT_WRITE,
-			  MAP_SHARED, buf->bus_address);
-	dev_priv->mmap_buffer = NULL;
-	file_priv->filp->f_op = old_fops;
-	if (IS_ERR((void *)virtual)) {	/* ugh */
-		/* Real error */
-		DRM_ERROR("mmap error\n");
-		retcode = PTR_ERR((void *)virtual);
-		buf_priv->virtual = NULL;
-	} else {
-		buf_priv->virtual = (void __user *)virtual;
-	}
-	up_write(&current->mm->mmap_sem);
-
-	return retcode;
-}
-
-static int i830_unmap_buffer(struct drm_buf *buf)
-{
-	drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-	int retcode = 0;
-
-	if (buf_priv->currently_mapped != I830_BUF_MAPPED)
-		return -EINVAL;
-
-	down_write(&current->mm->mmap_sem);
-	retcode = do_munmap(current->mm,
-			    (unsigned long)buf_priv->virtual,
-			    (size_t) buf->total);
-	up_write(&current->mm->mmap_sem);
-
-	buf_priv->currently_mapped = I830_BUF_UNMAPPED;
-	buf_priv->virtual = NULL;
-
-	return retcode;
-}
-
-static int i830_dma_get_buffer(struct drm_device *dev, drm_i830_dma_t *d,
-			       struct drm_file *file_priv)
-{
-	struct drm_buf *buf;
-	drm_i830_buf_priv_t *buf_priv;
-	int retcode = 0;
-
-	buf = i830_freelist_get(dev);
-	if (!buf) {
-		retcode = -ENOMEM;
-		DRM_DEBUG("retcode=%d\n", retcode);
-		return retcode;
-	}
-
-	retcode = i830_map_buffer(buf, file_priv);
-	if (retcode) {
-		i830_freelist_put(dev, buf);
-		DRM_ERROR("mapbuf failed, retcode %d\n", retcode);
-		return retcode;
-	}
-	buf->file_priv = file_priv;
-	buf_priv = buf->dev_private;
-	d->granted = 1;
-	d->request_idx = buf->idx;
-	d->request_size = buf->total;
-	d->virtual = buf_priv->virtual;
-
-	return retcode;
-}
-
-static int i830_dma_cleanup(struct drm_device *dev)
-{
-	struct drm_device_dma *dma = dev->dma;
-
-	/* 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 (dev->dev_private) {
-		int i;
-		drm_i830_private_t *dev_priv =
-		    (drm_i830_private_t *) dev->dev_private;
-
-		if (dev_priv->ring.virtual_start)
-			drm_core_ioremapfree(&dev_priv->ring.map, dev);
-		if (dev_priv->hw_status_page) {
-			pci_free_consistent(dev->pdev, PAGE_SIZE,
-					    dev_priv->hw_status_page,
-					    dev_priv->dma_status_page);
-			/* Need to rewrite hardware status page */
-			I830_WRITE(0x02080, 0x1ffff000);
-		}
-
-		kfree(dev->dev_private);
-		dev->dev_private = NULL;
-
-		for (i = 0; i < dma->buf_count; i++) {
-			struct drm_buf *buf = dma->buflist[i];
-			drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-			if (buf_priv->kernel_virtual && buf->total)
-				drm_core_ioremapfree(&buf_priv->map, dev);
-		}
-	}
-	return 0;
-}
-
-int i830_wait_ring(struct drm_device *dev, int n, const char *caller)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_ring_buffer_t *ring = &(dev_priv->ring);
-	int iters = 0;
-	unsigned long end;
-	unsigned int last_head = I830_READ(LP_RING + RING_HEAD) & HEAD_ADDR;
-
-	end = jiffies + (HZ * 3);
-	while (ring->space < n) {
-		ring->head = I830_READ(LP_RING + RING_HEAD) & HEAD_ADDR;
-		ring->space = ring->head - (ring->tail + 8);
-		if (ring->space < 0)
-			ring->space += ring->Size;
-
-		if (ring->head != last_head) {
-			end = jiffies + (HZ * 3);
-			last_head = ring->head;
-		}
-
-		iters++;
-		if (time_before(end, jiffies)) {
-			DRM_ERROR("space: %d wanted %d\n", ring->space, n);
-			DRM_ERROR("lockup\n");
-			goto out_wait_ring;
-		}
-		udelay(1);
-		dev_priv->sarea_priv->perf_boxes |= I830_BOX_WAIT;
-	}
-
-out_wait_ring:
-	return iters;
-}
-
-static void i830_kernel_lost_context(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_ring_buffer_t *ring = &(dev_priv->ring);
-
-	ring->head = I830_READ(LP_RING + RING_HEAD) & HEAD_ADDR;
-	ring->tail = I830_READ(LP_RING + RING_TAIL) & TAIL_ADDR;
-	ring->space = ring->head - (ring->tail + 8);
-	if (ring->space < 0)
-		ring->space += ring->Size;
-
-	if (ring->head == ring->tail)
-		dev_priv->sarea_priv->perf_boxes |= I830_BOX_RING_EMPTY;
-}
-
-static int i830_freelist_init(struct drm_device *dev, drm_i830_private_t *dev_priv)
-{
-	struct drm_device_dma *dma = dev->dma;
-	int my_idx = 36;
-	u32 *hw_status = (u32 *) (dev_priv->hw_status_page + my_idx);
-	int i;
-
-	if (dma->buf_count > 1019) {
-		/* Not enough space in the status page for the freelist */
-		return -EINVAL;
-	}
-
-	for (i = 0; i < dma->buf_count; i++) {
-		struct drm_buf *buf = dma->buflist[i];
-		drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-
-		buf_priv->in_use = hw_status++;
-		buf_priv->my_use_idx = my_idx;
-		my_idx += 4;
-
-		*buf_priv->in_use = I830_BUF_FREE;
-
-		buf_priv->map.offset = buf->bus_address;
-		buf_priv->map.size = buf->total;
-		buf_priv->map.type = _DRM_AGP;
-		buf_priv->map.flags = 0;
-		buf_priv->map.mtrr = 0;
-
-		drm_core_ioremap(&buf_priv->map, dev);
-		buf_priv->kernel_virtual = buf_priv->map.handle;
-	}
-	return 0;
-}
-
-static int i830_dma_initialize(struct drm_device *dev,
-			       drm_i830_private_t *dev_priv,
-			       drm_i830_init_t *init)
-{
-	struct drm_map_list *r_list;
-
-	memset(dev_priv, 0, sizeof(drm_i830_private_t));
-
-	list_for_each_entry(r_list, &dev->maplist, head) {
-		if (r_list->map &&
-		    r_list->map->type == _DRM_SHM &&
-		    r_list->map->flags & _DRM_CONTAINS_LOCK) {
-			dev_priv->sarea_map = r_list->map;
-			break;
-		}
-	}
-
-	if (!dev_priv->sarea_map) {
-		dev->dev_private = (void *)dev_priv;
-		i830_dma_cleanup(dev);
-		DRM_ERROR("can not find sarea!\n");
-		return -EINVAL;
-	}
-	dev_priv->mmio_map = drm_core_findmap(dev, init->mmio_offset);
-	if (!dev_priv->mmio_map) {
-		dev->dev_private = (void *)dev_priv;
-		i830_dma_cleanup(dev);
-		DRM_ERROR("can not find mmio map!\n");
-		return -EINVAL;
-	}
-	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_core_findmap(dev, init->buffers_offset);
-	if (!dev->agp_buffer_map) {
-		dev->dev_private = (void *)dev_priv;
-		i830_dma_cleanup(dev);
-		DRM_ERROR("can not find dma buffer map!\n");
-		return -EINVAL;
-	}
-
-	dev_priv->sarea_priv = (drm_i830_sarea_t *)
-	    ((u8 *) dev_priv->sarea_map->handle + init->sarea_priv_offset);
-
-	dev_priv->ring.Start = init->ring_start;
-	dev_priv->ring.End = init->ring_end;
-	dev_priv->ring.Size = init->ring_size;
-
-	dev_priv->ring.map.offset = dev->agp->base + init->ring_start;
-	dev_priv->ring.map.size = init->ring_size;
-	dev_priv->ring.map.type = _DRM_AGP;
-	dev_priv->ring.map.flags = 0;
-	dev_priv->ring.map.mtrr = 0;
-
-	drm_core_ioremap(&dev_priv->ring.map, dev);
-
-	if (dev_priv->ring.map.handle == NULL) {
-		dev->dev_private = (void *)dev_priv;
-		i830_dma_cleanup(dev);
-		DRM_ERROR("can not ioremap virtual address for"
-			  " ring buffer\n");
-		return -ENOMEM;
-	}
-
-	dev_priv->ring.virtual_start = dev_priv->ring.map.handle;
-
-	dev_priv->ring.tail_mask = dev_priv->ring.Size - 1;
-
-	dev_priv->w = init->w;
-	dev_priv->h = init->h;
-	dev_priv->pitch = init->pitch;
-	dev_priv->back_offset = init->back_offset;
-	dev_priv->depth_offset = init->depth_offset;
-	dev_priv->front_offset = init->front_offset;
-
-	dev_priv->front_di1 = init->front_offset | init->pitch_bits;
-	dev_priv->back_di1 = init->back_offset | init->pitch_bits;
-	dev_priv->zi1 = init->depth_offset | init->pitch_bits;
-
-	DRM_DEBUG("front_di1 %x\n", dev_priv->front_di1);
-	DRM_DEBUG("back_offset %x\n", dev_priv->back_offset);
-	DRM_DEBUG("back_di1 %x\n", dev_priv->back_di1);
-	DRM_DEBUG("pitch_bits %x\n", init->pitch_bits);
-
-	dev_priv->cpp = init->cpp;
-	/* We are using separate values as placeholders for mechanisms for
-	 * private backbuffer/depthbuffer usage.
-	 */
-
-	dev_priv->back_pitch = init->back_pitch;
-	dev_priv->depth_pitch = init->depth_pitch;
-	dev_priv->do_boxes = 0;
-	dev_priv->use_mi_batchbuffer_start = 0;
-
-	/* Program Hardware Status Page */
-	dev_priv->hw_status_page =
-	    pci_alloc_consistent(dev->pdev, PAGE_SIZE,
-				 &dev_priv->dma_status_page);
-	if (!dev_priv->hw_status_page) {
-		dev->dev_private = (void *)dev_priv;
-		i830_dma_cleanup(dev);
-		DRM_ERROR("Can not allocate hardware status page\n");
-		return -ENOMEM;
-	}
-	memset(dev_priv->hw_status_page, 0, PAGE_SIZE);
-	DRM_DEBUG("hw status page @ %p\n", dev_priv->hw_status_page);
-
-	I830_WRITE(0x02080, dev_priv->dma_status_page);
-	DRM_DEBUG("Enabled hardware status page\n");
-
-	/* Now we need to init our freelist */
-	if (i830_freelist_init(dev, dev_priv) != 0) {
-		dev->dev_private = (void *)dev_priv;
-		i830_dma_cleanup(dev);
-		DRM_ERROR("Not enough space in the status page for"
-			  " the freelist\n");
-		return -ENOMEM;
-	}
-	dev->dev_private = (void *)dev_priv;
-
-	return 0;
-}
-
-static int i830_dma_init(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv;
-	drm_i830_init_t *init = data;
-	int retcode = 0;
-
-	switch (init->func) {
-	case I830_INIT_DMA:
-		dev_priv = kmalloc(sizeof(drm_i830_private_t), GFP_KERNEL);
-		if (dev_priv == NULL)
-			return -ENOMEM;
-		retcode = i830_dma_initialize(dev, dev_priv, init);
-		break;
-	case I830_CLEANUP_DMA:
-		retcode = i830_dma_cleanup(dev);
-		break;
-	default:
-		retcode = -EINVAL;
-		break;
-	}
-
-	return retcode;
-}
-
-#define GFX_OP_STIPPLE           ((0x3<<29)|(0x1d<<24)|(0x83<<16))
-#define ST1_ENABLE               (1<<16)
-#define ST1_MASK                 (0xffff)
-
-/* Most efficient way to verify state for the i830 is as it is
- * emitted.  Non-conformant state is silently dropped.
- */
-static void i830EmitContextVerified(struct drm_device *dev, unsigned int *code)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	int i, j = 0;
-	unsigned int tmp;
-	RING_LOCALS;
-
-	BEGIN_LP_RING(I830_CTX_SETUP_SIZE + 4);
-
-	for (i = 0; i < I830_CTXREG_BLENDCOLR0; i++) {
-		tmp = code[i];
-		if ((tmp & (7 << 29)) == CMD_3D &&
-		    (tmp & (0x1f << 24)) < (0x1d << 24)) {
-			OUT_RING(tmp);
-			j++;
-		} else {
-			DRM_ERROR("Skipping %d\n", i);
-		}
-	}
-
-	OUT_RING(STATE3D_CONST_BLEND_COLOR_CMD);
-	OUT_RING(code[I830_CTXREG_BLENDCOLR]);
-	j += 2;
-
-	for (i = I830_CTXREG_VF; i < I830_CTXREG_MCSB0; i++) {
-		tmp = code[i];
-		if ((tmp & (7 << 29)) == CMD_3D &&
-		    (tmp & (0x1f << 24)) < (0x1d << 24)) {
-			OUT_RING(tmp);
-			j++;
-		} else {
-			DRM_ERROR("Skipping %d\n", i);
-		}
-	}
-
-	OUT_RING(STATE3D_MAP_COORD_SETBIND_CMD);
-	OUT_RING(code[I830_CTXREG_MCSB1]);
-	j += 2;
-
-	if (j & 1)
-		OUT_RING(0);
-
-	ADVANCE_LP_RING();
-}
-
-static void i830EmitTexVerified(struct drm_device *dev, unsigned int *code)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	int i, j = 0;
-	unsigned int tmp;
-	RING_LOCALS;
-
-	if (code[I830_TEXREG_MI0] == GFX_OP_MAP_INFO ||
-	    (code[I830_TEXREG_MI0] & ~(0xf * LOAD_TEXTURE_MAP0)) ==
-	    (STATE3D_LOAD_STATE_IMMEDIATE_2 | 4)) {
-
-		BEGIN_LP_RING(I830_TEX_SETUP_SIZE);
-
-		OUT_RING(code[I830_TEXREG_MI0]);	/* TM0LI */
-		OUT_RING(code[I830_TEXREG_MI1]);	/* TM0S0 */
-		OUT_RING(code[I830_TEXREG_MI2]);	/* TM0S1 */
-		OUT_RING(code[I830_TEXREG_MI3]);	/* TM0S2 */
-		OUT_RING(code[I830_TEXREG_MI4]);	/* TM0S3 */
-		OUT_RING(code[I830_TEXREG_MI5]);	/* TM0S4 */
-
-		for (i = 6; i < I830_TEX_SETUP_SIZE; i++) {
-			tmp = code[i];
-			OUT_RING(tmp);
-			j++;
-		}
-
-		if (j & 1)
-			OUT_RING(0);
-
-		ADVANCE_LP_RING();
-	} else
-		printk("rejected packet %x\n", code[0]);
-}
-
-static void i830EmitTexBlendVerified(struct drm_device *dev,
-				     unsigned int *code, unsigned int num)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	int i, j = 0;
-	unsigned int tmp;
-	RING_LOCALS;
-
-	if (!num)
-		return;
-
-	BEGIN_LP_RING(num + 1);
-
-	for (i = 0; i < num; i++) {
-		tmp = code[i];
-		OUT_RING(tmp);
-		j++;
-	}
-
-	if (j & 1)
-		OUT_RING(0);
-
-	ADVANCE_LP_RING();
-}
-
-static void i830EmitTexPalette(struct drm_device *dev,
-			       unsigned int *palette, int number, int is_shared)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	int i;
-	RING_LOCALS;
-
-	return;
-
-	BEGIN_LP_RING(258);
-
-	if (is_shared == 1) {
-		OUT_RING(CMD_OP_MAP_PALETTE_LOAD |
-			 MAP_PALETTE_NUM(0) | MAP_PALETTE_BOTH);
-	} else {
-		OUT_RING(CMD_OP_MAP_PALETTE_LOAD | MAP_PALETTE_NUM(number));
-	}
-	for (i = 0; i < 256; i++)
-		OUT_RING(palette[i]);
-	OUT_RING(0);
-	/* KW:  WHERE IS THE ADVANCE_LP_RING?  This is effectively a noop!
-	 */
-}
-
-/* Need to do some additional checking when setting the dest buffer.
- */
-static void i830EmitDestVerified(struct drm_device *dev, unsigned int *code)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	unsigned int tmp;
-	RING_LOCALS;
-
-	BEGIN_LP_RING(I830_DEST_SETUP_SIZE + 10);
-
-	tmp = code[I830_DESTREG_CBUFADDR];
-	if (tmp == dev_priv->front_di1 || tmp == dev_priv->back_di1) {
-		if (((int)outring) & 8) {
-			OUT_RING(0);
-			OUT_RING(0);
-		}
-
-		OUT_RING(CMD_OP_DESTBUFFER_INFO);
-		OUT_RING(BUF_3D_ID_COLOR_BACK |
-			 BUF_3D_PITCH(dev_priv->back_pitch * dev_priv->cpp) |
-			 BUF_3D_USE_FENCE);
-		OUT_RING(tmp);
-		OUT_RING(0);
-
-		OUT_RING(CMD_OP_DESTBUFFER_INFO);
-		OUT_RING(BUF_3D_ID_DEPTH | BUF_3D_USE_FENCE |
-			 BUF_3D_PITCH(dev_priv->depth_pitch * dev_priv->cpp));
-		OUT_RING(dev_priv->zi1);
-		OUT_RING(0);
-	} else {
-		DRM_ERROR("bad di1 %x (allow %x or %x)\n",
-			  tmp, dev_priv->front_di1, dev_priv->back_di1);
-	}
-
-	/* invarient:
-	 */
-
-	OUT_RING(GFX_OP_DESTBUFFER_VARS);
-	OUT_RING(code[I830_DESTREG_DV1]);
-
-	OUT_RING(GFX_OP_DRAWRECT_INFO);
-	OUT_RING(code[I830_DESTREG_DR1]);
-	OUT_RING(code[I830_DESTREG_DR2]);
-	OUT_RING(code[I830_DESTREG_DR3]);
-	OUT_RING(code[I830_DESTREG_DR4]);
-
-	/* Need to verify this */
-	tmp = code[I830_DESTREG_SENABLE];
-	if ((tmp & ~0x3) == GFX_OP_SCISSOR_ENABLE) {
-		OUT_RING(tmp);
-	} else {
-		DRM_ERROR("bad scissor enable\n");
-		OUT_RING(0);
-	}
-
-	OUT_RING(GFX_OP_SCISSOR_RECT);
-	OUT_RING(code[I830_DESTREG_SR1]);
-	OUT_RING(code[I830_DESTREG_SR2]);
-	OUT_RING(0);
-
-	ADVANCE_LP_RING();
-}
-
-static void i830EmitStippleVerified(struct drm_device *dev, unsigned int *code)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-
-	BEGIN_LP_RING(2);
-	OUT_RING(GFX_OP_STIPPLE);
-	OUT_RING(code[1]);
-	ADVANCE_LP_RING();
-}
-
-static void i830EmitState(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_sarea_t *sarea_priv = dev_priv->sarea_priv;
-	unsigned int dirty = sarea_priv->dirty;
-
-	DRM_DEBUG("%s %x\n", __func__, dirty);
-
-	if (dirty & I830_UPLOAD_BUFFERS) {
-		i830EmitDestVerified(dev, sarea_priv->BufferState);
-		sarea_priv->dirty &= ~I830_UPLOAD_BUFFERS;
-	}
-
-	if (dirty & I830_UPLOAD_CTX) {
-		i830EmitContextVerified(dev, sarea_priv->ContextState);
-		sarea_priv->dirty &= ~I830_UPLOAD_CTX;
-	}
-
-	if (dirty & I830_UPLOAD_TEX0) {
-		i830EmitTexVerified(dev, sarea_priv->TexState[0]);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEX0;
-	}
-
-	if (dirty & I830_UPLOAD_TEX1) {
-		i830EmitTexVerified(dev, sarea_priv->TexState[1]);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEX1;
-	}
-
-	if (dirty & I830_UPLOAD_TEXBLEND0) {
-		i830EmitTexBlendVerified(dev, sarea_priv->TexBlendState[0],
-					 sarea_priv->TexBlendStateWordsUsed[0]);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEXBLEND0;
-	}
-
-	if (dirty & I830_UPLOAD_TEXBLEND1) {
-		i830EmitTexBlendVerified(dev, sarea_priv->TexBlendState[1],
-					 sarea_priv->TexBlendStateWordsUsed[1]);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEXBLEND1;
-	}
-
-	if (dirty & I830_UPLOAD_TEX_PALETTE_SHARED) {
-		i830EmitTexPalette(dev, sarea_priv->Palette[0], 0, 1);
-	} else {
-		if (dirty & I830_UPLOAD_TEX_PALETTE_N(0)) {
-			i830EmitTexPalette(dev, sarea_priv->Palette[0], 0, 0);
-			sarea_priv->dirty &= ~I830_UPLOAD_TEX_PALETTE_N(0);
-		}
-		if (dirty & I830_UPLOAD_TEX_PALETTE_N(1)) {
-			i830EmitTexPalette(dev, sarea_priv->Palette[1], 1, 0);
-			sarea_priv->dirty &= ~I830_UPLOAD_TEX_PALETTE_N(1);
-		}
-
-		/* 1.3:
-		 */
-#if 0
-		if (dirty & I830_UPLOAD_TEX_PALETTE_N(2)) {
-			i830EmitTexPalette(dev, sarea_priv->Palette2[0], 0, 0);
-			sarea_priv->dirty &= ~I830_UPLOAD_TEX_PALETTE_N(2);
-		}
-		if (dirty & I830_UPLOAD_TEX_PALETTE_N(3)) {
-			i830EmitTexPalette(dev, sarea_priv->Palette2[1], 1, 0);
-			sarea_priv->dirty &= ~I830_UPLOAD_TEX_PALETTE_N(2);
-		}
-#endif
-	}
-
-	/* 1.3:
-	 */
-	if (dirty & I830_UPLOAD_STIPPLE) {
-		i830EmitStippleVerified(dev, sarea_priv->StippleState);
-		sarea_priv->dirty &= ~I830_UPLOAD_STIPPLE;
-	}
-
-	if (dirty & I830_UPLOAD_TEX2) {
-		i830EmitTexVerified(dev, sarea_priv->TexState2);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEX2;
-	}
-
-	if (dirty & I830_UPLOAD_TEX3) {
-		i830EmitTexVerified(dev, sarea_priv->TexState3);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEX3;
-	}
-
-	if (dirty & I830_UPLOAD_TEXBLEND2) {
-		i830EmitTexBlendVerified(dev,
-					 sarea_priv->TexBlendState2,
-					 sarea_priv->TexBlendStateWordsUsed2);
-
-		sarea_priv->dirty &= ~I830_UPLOAD_TEXBLEND2;
-	}
-
-	if (dirty & I830_UPLOAD_TEXBLEND3) {
-		i830EmitTexBlendVerified(dev,
-					 sarea_priv->TexBlendState3,
-					 sarea_priv->TexBlendStateWordsUsed3);
-		sarea_priv->dirty &= ~I830_UPLOAD_TEXBLEND3;
-	}
-}
-
-/* ================================================================
- * Performance monitoring functions
- */
-
-static void i830_fill_box(struct drm_device *dev,
-			  int x, int y, int w, int h, int r, int g, int b)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	u32 color;
-	unsigned int BR13, CMD;
-	RING_LOCALS;
-
-	BR13 = (0xF0 << 16) | (dev_priv->pitch * dev_priv->cpp) | (1 << 24);
-	CMD = XY_COLOR_BLT_CMD;
-	x += dev_priv->sarea_priv->boxes[0].x1;
-	y += dev_priv->sarea_priv->boxes[0].y1;
-
-	if (dev_priv->cpp == 4) {
-		BR13 |= (1 << 25);
-		CMD |= (XY_COLOR_BLT_WRITE_ALPHA | XY_COLOR_BLT_WRITE_RGB);
-		color = (((0xff) << 24) | (r << 16) | (g << 8) | b);
-	} else {
-		color = (((r & 0xf8) << 8) |
-			 ((g & 0xfc) << 3) | ((b & 0xf8) >> 3));
-	}
-
-	BEGIN_LP_RING(6);
-	OUT_RING(CMD);
-	OUT_RING(BR13);
-	OUT_RING((y << 16) | x);
-	OUT_RING(((y + h) << 16) | (x + w));
-
-	if (dev_priv->current_page == 1)
-		OUT_RING(dev_priv->front_offset);
-	else
-		OUT_RING(dev_priv->back_offset);
-
-	OUT_RING(color);
-	ADVANCE_LP_RING();
-}
-
-static void i830_cp_performance_boxes(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-
-	/* Purple box for page flipping
-	 */
-	if (dev_priv->sarea_priv->perf_boxes & I830_BOX_FLIP)
-		i830_fill_box(dev, 4, 4, 8, 8, 255, 0, 255);
-
-	/* Red box if we have to wait for idle at any point
-	 */
-	if (dev_priv->sarea_priv->perf_boxes & I830_BOX_WAIT)
-		i830_fill_box(dev, 16, 4, 8, 8, 255, 0, 0);
-
-	/* Blue box: lost context?
-	 */
-	if (dev_priv->sarea_priv->perf_boxes & I830_BOX_LOST_CONTEXT)
-		i830_fill_box(dev, 28, 4, 8, 8, 0, 0, 255);
-
-	/* Yellow box for texture swaps
-	 */
-	if (dev_priv->sarea_priv->perf_boxes & I830_BOX_TEXTURE_LOAD)
-		i830_fill_box(dev, 40, 4, 8, 8, 255, 255, 0);
-
-	/* Green box if hardware never idles (as far as we can tell)
-	 */
-	if (!(dev_priv->sarea_priv->perf_boxes & I830_BOX_RING_EMPTY))
-		i830_fill_box(dev, 64, 4, 8, 8, 0, 255, 0);
-
-	/* Draw bars indicating number of buffers allocated
-	 * (not a great measure, easily confused)
-	 */
-	if (dev_priv->dma_used) {
-		int bar = dev_priv->dma_used / 10240;
-		if (bar > 100)
-			bar = 100;
-		if (bar < 1)
-			bar = 1;
-		i830_fill_box(dev, 4, 16, bar, 4, 196, 128, 128);
-		dev_priv->dma_used = 0;
-	}
-
-	dev_priv->sarea_priv->perf_boxes = 0;
-}
-
-static void i830_dma_dispatch_clear(struct drm_device *dev, int flags,
-				    unsigned int clear_color,
-				    unsigned int clear_zval,
-				    unsigned int clear_depthmask)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_sarea_t *sarea_priv = dev_priv->sarea_priv;
-	int nbox = sarea_priv->nbox;
-	struct drm_clip_rect *pbox = sarea_priv->boxes;
-	int pitch = dev_priv->pitch;
-	int cpp = dev_priv->cpp;
-	int i;
-	unsigned int BR13, CMD, D_CMD;
-	RING_LOCALS;
-
-	if (dev_priv->current_page == 1) {
-		unsigned int tmp = flags;
-
-		flags &= ~(I830_FRONT | I830_BACK);
-		if (tmp & I830_FRONT)
-			flags |= I830_BACK;
-		if (tmp & I830_BACK)
-			flags |= I830_FRONT;
-	}
-
-	i830_kernel_lost_context(dev);
-
-	switch (cpp) {
-	case 2:
-		BR13 = (0xF0 << 16) | (pitch * cpp) | (1 << 24);
-		D_CMD = CMD = XY_COLOR_BLT_CMD;
-		break;
-	case 4:
-		BR13 = (0xF0 << 16) | (pitch * cpp) | (1 << 24) | (1 << 25);
-		CMD = (XY_COLOR_BLT_CMD | XY_COLOR_BLT_WRITE_ALPHA |
-		       XY_COLOR_BLT_WRITE_RGB);
-		D_CMD = XY_COLOR_BLT_CMD;
-		if (clear_depthmask & 0x00ffffff)
-			D_CMD |= XY_COLOR_BLT_WRITE_RGB;
-		if (clear_depthmask & 0xff000000)
-			D_CMD |= XY_COLOR_BLT_WRITE_ALPHA;
-		break;
-	default:
-		BR13 = (0xF0 << 16) | (pitch * cpp) | (1 << 24);
-		D_CMD = CMD = XY_COLOR_BLT_CMD;
-		break;
-	}
-
-	if (nbox > I830_NR_SAREA_CLIPRECTS)
-		nbox = I830_NR_SAREA_CLIPRECTS;
-
-	for (i = 0; i < nbox; i++, pbox++) {
-		if (pbox->x1 > pbox->x2 ||
-		    pbox->y1 > pbox->y2 ||
-		    pbox->x2 > dev_priv->w || pbox->y2 > dev_priv->h)
-			continue;
-
-		if (flags & I830_FRONT) {
-			DRM_DEBUG("clear front\n");
-			BEGIN_LP_RING(6);
-			OUT_RING(CMD);
-			OUT_RING(BR13);
-			OUT_RING((pbox->y1 << 16) | pbox->x1);
-			OUT_RING((pbox->y2 << 16) | pbox->x2);
-			OUT_RING(dev_priv->front_offset);
-			OUT_RING(clear_color);
-			ADVANCE_LP_RING();
-		}
-
-		if (flags & I830_BACK) {
-			DRM_DEBUG("clear back\n");
-			BEGIN_LP_RING(6);
-			OUT_RING(CMD);
-			OUT_RING(BR13);
-			OUT_RING((pbox->y1 << 16) | pbox->x1);
-			OUT_RING((pbox->y2 << 16) | pbox->x2);
-			OUT_RING(dev_priv->back_offset);
-			OUT_RING(clear_color);
-			ADVANCE_LP_RING();
-		}
-
-		if (flags & I830_DEPTH) {
-			DRM_DEBUG("clear depth\n");
-			BEGIN_LP_RING(6);
-			OUT_RING(D_CMD);
-			OUT_RING(BR13);
-			OUT_RING((pbox->y1 << 16) | pbox->x1);
-			OUT_RING((pbox->y2 << 16) | pbox->x2);
-			OUT_RING(dev_priv->depth_offset);
-			OUT_RING(clear_zval);
-			ADVANCE_LP_RING();
-		}
-	}
-}
-
-static void i830_dma_dispatch_swap(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_sarea_t *sarea_priv = dev_priv->sarea_priv;
-	int nbox = sarea_priv->nbox;
-	struct drm_clip_rect *pbox = sarea_priv->boxes;
-	int pitch = dev_priv->pitch;
-	int cpp = dev_priv->cpp;
-	int i;
-	unsigned int CMD, BR13;
-	RING_LOCALS;
-
-	DRM_DEBUG("swapbuffers\n");
-
-	i830_kernel_lost_context(dev);
-
-	if (dev_priv->do_boxes)
-		i830_cp_performance_boxes(dev);
-
-	switch (cpp) {
-	case 2:
-		BR13 = (pitch * cpp) | (0xCC << 16) | (1 << 24);
-		CMD = XY_SRC_COPY_BLT_CMD;
-		break;
-	case 4:
-		BR13 = (pitch * cpp) | (0xCC << 16) | (1 << 24) | (1 << 25);
-		CMD = (XY_SRC_COPY_BLT_CMD | XY_SRC_COPY_BLT_WRITE_ALPHA |
-		       XY_SRC_COPY_BLT_WRITE_RGB);
-		break;
-	default:
-		BR13 = (pitch * cpp) | (0xCC << 16) | (1 << 24);
-		CMD = XY_SRC_COPY_BLT_CMD;
-		break;
-	}
-
-	if (nbox > I830_NR_SAREA_CLIPRECTS)
-		nbox = I830_NR_SAREA_CLIPRECTS;
-
-	for (i = 0; i < nbox; i++, pbox++) {
-		if (pbox->x1 > pbox->x2 ||
-		    pbox->y1 > pbox->y2 ||
-		    pbox->x2 > dev_priv->w || pbox->y2 > dev_priv->h)
-			continue;
-
-		DRM_DEBUG("dispatch swap %d,%d-%d,%d!\n",
-			  pbox->x1, pbox->y1, pbox->x2, pbox->y2);
-
-		BEGIN_LP_RING(8);
-		OUT_RING(CMD);
-		OUT_RING(BR13);
-		OUT_RING((pbox->y1 << 16) | pbox->x1);
-		OUT_RING((pbox->y2 << 16) | pbox->x2);
-
-		if (dev_priv->current_page == 0)
-			OUT_RING(dev_priv->front_offset);
-		else
-			OUT_RING(dev_priv->back_offset);
-
-		OUT_RING((pbox->y1 << 16) | pbox->x1);
-		OUT_RING(BR13 & 0xffff);
-
-		if (dev_priv->current_page == 0)
-			OUT_RING(dev_priv->back_offset);
-		else
-			OUT_RING(dev_priv->front_offset);
-
-		ADVANCE_LP_RING();
-	}
-}
-
-static void i830_dma_dispatch_flip(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-
-	DRM_DEBUG("%s: page=%d pfCurrentPage=%d\n",
-		  __func__,
-		  dev_priv->current_page,
-		  dev_priv->sarea_priv->pf_current_page);
-
-	i830_kernel_lost_context(dev);
-
-	if (dev_priv->do_boxes) {
-		dev_priv->sarea_priv->perf_boxes |= I830_BOX_FLIP;
-		i830_cp_performance_boxes(dev);
-	}
-
-	BEGIN_LP_RING(2);
-	OUT_RING(INST_PARSER_CLIENT | INST_OP_FLUSH | INST_FLUSH_MAP_CACHE);
-	OUT_RING(0);
-	ADVANCE_LP_RING();
-
-	BEGIN_LP_RING(6);
-	OUT_RING(CMD_OP_DISPLAYBUFFER_INFO | ASYNC_FLIP);
-	OUT_RING(0);
-	if (dev_priv->current_page == 0) {
-		OUT_RING(dev_priv->back_offset);
-		dev_priv->current_page = 1;
-	} else {
-		OUT_RING(dev_priv->front_offset);
-		dev_priv->current_page = 0;
-	}
-	OUT_RING(0);
-	ADVANCE_LP_RING();
-
-	BEGIN_LP_RING(2);
-	OUT_RING(MI_WAIT_FOR_EVENT | MI_WAIT_FOR_PLANE_A_FLIP);
-	OUT_RING(0);
-	ADVANCE_LP_RING();
-
-	dev_priv->sarea_priv->pf_current_page = dev_priv->current_page;
-}
-
-static void i830_dma_dispatch_vertex(struct drm_device *dev,
-				     struct drm_buf *buf, int discard, int used)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-	drm_i830_sarea_t *sarea_priv = dev_priv->sarea_priv;
-	struct drm_clip_rect *box = sarea_priv->boxes;
-	int nbox = sarea_priv->nbox;
-	unsigned long address = (unsigned long)buf->bus_address;
-	unsigned long start = address - dev->agp->base;
-	int i = 0, u;
-	RING_LOCALS;
-
-	i830_kernel_lost_context(dev);
-
-	if (nbox > I830_NR_SAREA_CLIPRECTS)
-		nbox = I830_NR_SAREA_CLIPRECTS;
-
-	if (discard) {
-		u = cmpxchg(buf_priv->in_use, I830_BUF_CLIENT,
-			    I830_BUF_HARDWARE);
-		if (u != I830_BUF_CLIENT)
-			DRM_DEBUG("xxxx 2\n");
-	}
-
-	if (used > 4 * 1023)
-		used = 0;
-
-	if (sarea_priv->dirty)
-		i830EmitState(dev);
-
-	DRM_DEBUG("dispatch vertex addr 0x%lx, used 0x%x nbox %d\n",
-		  address, used, nbox);
-
-	dev_priv->counter++;
-	DRM_DEBUG("dispatch counter : %ld\n", dev_priv->counter);
-	DRM_DEBUG("i830_dma_dispatch\n");
-	DRM_DEBUG("start : %lx\n", start);
-	DRM_DEBUG("used : %d\n", used);
-	DRM_DEBUG("start + used - 4 : %ld\n", start + used - 4);
-
-	if (buf_priv->currently_mapped == I830_BUF_MAPPED) {
-		u32 *vp = buf_priv->kernel_virtual;
-
-		vp[0] = (GFX_OP_PRIMITIVE |
-			 sarea_priv->vertex_prim | ((used / 4) - 2));
-
-		if (dev_priv->use_mi_batchbuffer_start) {
-			vp[used / 4] = MI_BATCH_BUFFER_END;
-			used += 4;
-		}
-
-		if (used & 4) {
-			vp[used / 4] = 0;
-			used += 4;
-		}
-
-		i830_unmap_buffer(buf);
-	}
-
-	if (used) {
-		do {
-			if (i < nbox) {
-				BEGIN_LP_RING(6);
-				OUT_RING(GFX_OP_DRAWRECT_INFO);
-				OUT_RING(sarea_priv->
-					 BufferState[I830_DESTREG_DR1]);
-				OUT_RING(box[i].x1 | (box[i].y1 << 16));
-				OUT_RING(box[i].x2 | (box[i].y2 << 16));
-				OUT_RING(sarea_priv->
-					 BufferState[I830_DESTREG_DR4]);
-				OUT_RING(0);
-				ADVANCE_LP_RING();
-			}
-
-			if (dev_priv->use_mi_batchbuffer_start) {
-				BEGIN_LP_RING(2);
-				OUT_RING(MI_BATCH_BUFFER_START | (2 << 6));
-				OUT_RING(start | MI_BATCH_NON_SECURE);
-				ADVANCE_LP_RING();
-			} else {
-				BEGIN_LP_RING(4);
-				OUT_RING(MI_BATCH_BUFFER);
-				OUT_RING(start | MI_BATCH_NON_SECURE);
-				OUT_RING(start + used - 4);
-				OUT_RING(0);
-				ADVANCE_LP_RING();
-			}
-
-		} while (++i < nbox);
-	}
-
-	if (discard) {
-		dev_priv->counter++;
-
-		(void)cmpxchg(buf_priv->in_use, I830_BUF_CLIENT,
-			      I830_BUF_HARDWARE);
-
-		BEGIN_LP_RING(8);
-		OUT_RING(CMD_STORE_DWORD_IDX);
-		OUT_RING(20);
-		OUT_RING(dev_priv->counter);
-		OUT_RING(CMD_STORE_DWORD_IDX);
-		OUT_RING(buf_priv->my_use_idx);
-		OUT_RING(I830_BUF_FREE);
-		OUT_RING(CMD_REPORT_HEAD);
-		OUT_RING(0);
-		ADVANCE_LP_RING();
-	}
-}
-
-static void i830_dma_quiescent(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-
-	i830_kernel_lost_context(dev);
-
-	BEGIN_LP_RING(4);
-	OUT_RING(INST_PARSER_CLIENT | INST_OP_FLUSH | INST_FLUSH_MAP_CACHE);
-	OUT_RING(CMD_REPORT_HEAD);
-	OUT_RING(0);
-	OUT_RING(0);
-	ADVANCE_LP_RING();
-
-	i830_wait_ring(dev, dev_priv->ring.Size - 8, __func__);
-}
-
-static int i830_flush_queue(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	struct drm_device_dma *dma = dev->dma;
-	int i, ret = 0;
-	RING_LOCALS;
-
-	i830_kernel_lost_context(dev);
-
-	BEGIN_LP_RING(2);
-	OUT_RING(CMD_REPORT_HEAD);
-	OUT_RING(0);
-	ADVANCE_LP_RING();
-
-	i830_wait_ring(dev, dev_priv->ring.Size - 8, __func__);
-
-	for (i = 0; i < dma->buf_count; i++) {
-		struct drm_buf *buf = dma->buflist[i];
-		drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-
-		int used = cmpxchg(buf_priv->in_use, I830_BUF_HARDWARE,
-				   I830_BUF_FREE);
-
-		if (used == I830_BUF_HARDWARE)
-			DRM_DEBUG("reclaimed from HARDWARE\n");
-		if (used == I830_BUF_CLIENT)
-			DRM_DEBUG("still on client\n");
-	}
-
-	return ret;
-}
-
-/* Must be called with the lock held */
-static void i830_reclaim_buffers(struct drm_device *dev, struct drm_file *file_priv)
-{
-	struct drm_device_dma *dma = dev->dma;
-	int i;
-
-	if (!dma)
-		return;
-	if (!dev->dev_private)
-		return;
-	if (!dma->buflist)
-		return;
-
-	i830_flush_queue(dev);
-
-	for (i = 0; i < dma->buf_count; i++) {
-		struct drm_buf *buf = dma->buflist[i];
-		drm_i830_buf_priv_t *buf_priv = buf->dev_private;
-
-		if (buf->file_priv == file_priv && buf_priv) {
-			int used = cmpxchg(buf_priv->in_use, I830_BUF_CLIENT,
-					   I830_BUF_FREE);
-
-			if (used == I830_BUF_CLIENT)
-				DRM_DEBUG("reclaimed from client\n");
-			if (buf_priv->currently_mapped == I830_BUF_MAPPED)
-				buf_priv->currently_mapped = I830_BUF_UNMAPPED;
-		}
-	}
-}
-
-static int i830_flush_ioctl(struct drm_device *dev, void *data,
-			    struct drm_file *file_priv)
-{
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	i830_flush_queue(dev);
-	return 0;
-}
-
-static int i830_dma_vertex(struct drm_device *dev, void *data,
-			   struct drm_file *file_priv)
-{
-	struct drm_device_dma *dma = dev->dma;
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-	u32 *hw_status = dev_priv->hw_status_page;
-	drm_i830_sarea_t *sarea_priv = (drm_i830_sarea_t *)
-	    dev_priv->sarea_priv;
-	drm_i830_vertex_t *vertex = data;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	DRM_DEBUG("i830 dma vertex, idx %d used %d discard %d\n",
-		  vertex->idx, vertex->used, vertex->discard);
-
-	if (vertex->idx < 0 || vertex->idx > dma->buf_count)
-		return -EINVAL;
-
-	i830_dma_dispatch_vertex(dev,
-				 dma->buflist[vertex->idx],
-				 vertex->discard, vertex->used);
-
-	sarea_priv->last_enqueue = dev_priv->counter - 1;
-	sarea_priv->last_dispatch = (int)hw_status[5];
-
-	return 0;
-}
-
-static int i830_clear_bufs(struct drm_device *dev, void *data,
-			   struct drm_file *file_priv)
-{
-	drm_i830_clear_t *clear = data;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	/* GH: Someone's doing nasty things... */
-	if (!dev->dev_private)
-		return -EINVAL;
-
-	i830_dma_dispatch_clear(dev, clear->flags,
-				clear->clear_color,
-				clear->clear_depth, clear->clear_depthmask);
-	return 0;
-}
-
-static int i830_swap_bufs(struct drm_device *dev, void *data,
-			  struct drm_file *file_priv)
-{
-	DRM_DEBUG("i830_swap_bufs\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	i830_dma_dispatch_swap(dev);
-	return 0;
-}
-
-/* Not sure why this isn't set all the time:
- */
-static void i830_do_init_pageflip(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-
-	DRM_DEBUG("%s\n", __func__);
-	dev_priv->page_flipping = 1;
-	dev_priv->current_page = 0;
-	dev_priv->sarea_priv->pf_current_page = dev_priv->current_page;
-}
-
-static int i830_do_cleanup_pageflip(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-
-	DRM_DEBUG("%s\n", __func__);
-	if (dev_priv->current_page != 0)
-		i830_dma_dispatch_flip(dev);
-
-	dev_priv->page_flipping = 0;
-	return 0;
-}
-
-static int i830_flip_bufs(struct drm_device *dev, void *data,
-			  struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-
-	DRM_DEBUG("%s\n", __func__);
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (!dev_priv->page_flipping)
-		i830_do_init_pageflip(dev);
-
-	i830_dma_dispatch_flip(dev);
-	return 0;
-}
-
-static int i830_getage(struct drm_device *dev, void *data,
-		       struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-	u32 *hw_status = dev_priv->hw_status_page;
-	drm_i830_sarea_t *sarea_priv = (drm_i830_sarea_t *)
-	    dev_priv->sarea_priv;
-
-	sarea_priv->last_dispatch = (int)hw_status[5];
-	return 0;
-}
-
-static int i830_getbuf(struct drm_device *dev, void *data,
-		       struct drm_file *file_priv)
-{
-	int retcode = 0;
-	drm_i830_dma_t *d = data;
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-	u32 *hw_status = dev_priv->hw_status_page;
-	drm_i830_sarea_t *sarea_priv = (drm_i830_sarea_t *)
-	    dev_priv->sarea_priv;
-
-	DRM_DEBUG("getbuf\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	d->granted = 0;
-
-	retcode = i830_dma_get_buffer(dev, d, file_priv);
-
-	DRM_DEBUG("i830_dma: %d returning %d, granted = %d\n",
-		  task_pid_nr(current), retcode, d->granted);
-
-	sarea_priv->last_dispatch = (int)hw_status[5];
-
-	return retcode;
-}
-
-static int i830_copybuf(struct drm_device *dev, void *data,
-			struct drm_file *file_priv)
-{
-	/* Never copy - 2.4.x doesn't need it */
-	return 0;
-}
-
-static int i830_docopy(struct drm_device *dev, void *data,
-		       struct drm_file *file_priv)
-{
-	return 0;
-}
-
-static int i830_getparam(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_getparam_t *param = data;
-	int value;
-
-	if (!dev_priv) {
-		DRM_ERROR("%s called with no initialization\n", __func__);
-		return -EINVAL;
-	}
-
-	switch (param->param) {
-	case I830_PARAM_IRQ_ACTIVE:
-		value = dev->irq_enabled;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	if (copy_to_user(param->value, &value, sizeof(int))) {
-		DRM_ERROR("copy_to_user\n");
-		return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int i830_setparam(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_setparam_t *param = data;
-
-	if (!dev_priv) {
-		DRM_ERROR("%s called with no initialization\n", __func__);
-		return -EINVAL;
-	}
-
-	switch (param->param) {
-	case I830_SETPARAM_USE_MI_BATCHBUFFER_START:
-		dev_priv->use_mi_batchbuffer_start = param->value;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-int i830_driver_load(struct drm_device *dev, unsigned long flags)
-{
-	/* i830 has 4 more counters */
-	dev->counters += 4;
-	dev->types[6] = _DRM_STAT_IRQ;
-	dev->types[7] = _DRM_STAT_PRIMARY;
-	dev->types[8] = _DRM_STAT_SECONDARY;
-	dev->types[9] = _DRM_STAT_DMA;
-
-	return 0;
-}
-
-void i830_driver_lastclose(struct drm_device *dev)
-{
-	i830_dma_cleanup(dev);
-}
-
-void i830_driver_preclose(struct drm_device *dev, struct drm_file *file_priv)
-{
-	if (dev->dev_private) {
-		drm_i830_private_t *dev_priv = dev->dev_private;
-		if (dev_priv->page_flipping)
-			i830_do_cleanup_pageflip(dev);
-	}
-}
-
-void i830_driver_reclaim_buffers_locked(struct drm_device *dev, struct drm_file *file_priv)
-{
-	i830_reclaim_buffers(dev, file_priv);
-}
-
-int i830_driver_dma_quiescent(struct drm_device *dev)
-{
-	i830_dma_quiescent(dev);
-	return 0;
-}
-
-/*
- * call the drm_ioctl under the big kernel lock because
- * to lock against the i830_mmap_buffers function.
- */
-long i830_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	int ret;
-	lock_kernel();
-	ret = drm_ioctl(file, cmd, arg);
-	unlock_kernel();
-	return ret;
-}
-
-struct drm_ioctl_desc i830_ioctls[] = {
-	DRM_IOCTL_DEF_DRV(I830_INIT, i830_dma_init, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_VERTEX, i830_dma_vertex, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_CLEAR, i830_clear_bufs, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_FLUSH, i830_flush_ioctl, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_GETAGE, i830_getage, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_GETBUF, i830_getbuf, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_SWAP, i830_swap_bufs, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_COPY, i830_copybuf, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_DOCOPY, i830_docopy, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_FLIP, i830_flip_bufs, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_IRQ_EMIT, i830_irq_emit, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_IRQ_WAIT, i830_irq_wait, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_GETPARAM, i830_getparam, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(I830_SETPARAM, i830_setparam, DRM_AUTH|DRM_UNLOCKED),
-};
-
-int i830_max_ioctl = DRM_ARRAY_SIZE(i830_ioctls);
-
-/**
- * Determine if the device really is AGP or not.
- *
- * All Intel graphics chipsets are treated as AGP, even if they are really
- * PCI-e.
- *
- * \param dev   The device to be tested.
- *
- * \returns
- * A value of 1 is always retured to indictate every i8xx is AGP.
- */
-int i830_driver_device_is_agp(struct drm_device *dev)
-{
-	return 1;
-}
diff --git a/drivers/gpu/drm/i830/i830_drv.c b/drivers/gpu/drm/i830/i830_drv.c
deleted file mode 100644
index f655ab7977da..000000000000
--- a/drivers/gpu/drm/i830/i830_drv.c
+++ /dev/null
@@ -1,107 +0,0 @@
-/* i830_drv.c -- I810 driver -*- linux-c -*-
- * Created: Mon Dec 13 01:56:22 1999 by jhartmann@precisioninsight.com
- *
- * Copyright 1999 Precision Insight, Inc., Cedar Park, Texas.
- * Copyright 2000 VA Linux Systems, Inc., Sunnyvale, 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
- * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *    Rickard E. (Rik) Faith <faith@valinux.com>
- *    Jeff Hartmann <jhartmann@valinux.com>
- *    Gareth Hughes <gareth@valinux.com>
- *    Abraham vd Merwe <abraham@2d3d.co.za>
- *    Keith Whitwell <keith@tungstengraphics.com>
- */
-
-#include "drmP.h"
-#include "drm.h"
-#include "i830_drm.h"
-#include "i830_drv.h"
-
-#include "drm_pciids.h"
-
-static struct pci_device_id pciidlist[] = {
-	i830_PCI_IDS
-};
-
-static struct drm_driver driver = {
-	.driver_features =
-	    DRIVER_USE_AGP | DRIVER_REQUIRE_AGP | DRIVER_USE_MTRR |
-	    DRIVER_HAVE_DMA | DRIVER_DMA_QUEUE,
-#if USE_IRQS
-	.driver_features |= DRIVER_HAVE_IRQ | DRIVER_SHARED_IRQ,
-#endif
-	.dev_priv_size = sizeof(drm_i830_buf_priv_t),
-	.load = i830_driver_load,
-	.lastclose = i830_driver_lastclose,
-	.preclose = i830_driver_preclose,
-	.device_is_agp = i830_driver_device_is_agp,
-	.reclaim_buffers_locked = i830_driver_reclaim_buffers_locked,
-	.dma_quiescent = i830_driver_dma_quiescent,
-#if USE_IRQS
-	.irq_preinstall = i830_driver_irq_preinstall,
-	.irq_postinstall = i830_driver_irq_postinstall,
-	.irq_uninstall = i830_driver_irq_uninstall,
-	.irq_handler = i830_driver_irq_handler,
-#endif
-	.ioctls = i830_ioctls,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = i830_ioctl,
-		 .mmap = drm_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .llseek = noop_llseek,
-	},
-
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-	},
-
-	.name = DRIVER_NAME,
-	.desc = DRIVER_DESC,
-	.date = DRIVER_DATE,
-	.major = DRIVER_MAJOR,
-	.minor = DRIVER_MINOR,
-	.patchlevel = DRIVER_PATCHLEVEL,
-};
-
-static int __init i830_init(void)
-{
-	driver.num_ioctls = i830_max_ioctl;
-	return drm_init(&driver);
-}
-
-static void __exit i830_exit(void)
-{
-	drm_exit(&driver);
-}
-
-module_init(i830_init);
-module_exit(i830_exit);
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/gpu/drm/i830/i830_drv.h b/drivers/gpu/drm/i830/i830_drv.h
deleted file mode 100644
index 0df1c720560b..000000000000
--- a/drivers/gpu/drm/i830/i830_drv.h
+++ /dev/null
@@ -1,295 +0,0 @@
-/* i830_drv.h -- Private header for the I830 driver -*- linux-c -*-
- * Created: Mon Dec 13 01:50:01 1999 by jhartmann@precisioninsight.com
- *
- * Copyright 1999 Precision Insight, Inc., Cedar Park, Texas.
- * Copyright 2000 VA Linux Systems, Inc., Sunnyvale, 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: Rickard E. (Rik) Faith <faith@valinux.com>
- *	    Jeff Hartmann <jhartmann@valinux.com>
- *
- */
-
-#ifndef _I830_DRV_H_
-#define _I830_DRV_H_
-
-/* General customization:
- */
-
-#define DRIVER_AUTHOR		"VA Linux Systems Inc."
-
-#define DRIVER_NAME		"i830"
-#define DRIVER_DESC		"Intel 830M"
-#define DRIVER_DATE		"20021108"
-
-/* Interface history:
- *
- * 1.1: Original.
- * 1.2: ?
- * 1.3: New irq emit/wait ioctls.
- *      New pageflip ioctl.
- *      New getparam ioctl.
- *      State for texunits 3&4 in sarea.
- *      New (alternative) layout for texture state.
- */
-#define DRIVER_MAJOR		1
-#define DRIVER_MINOR		3
-#define DRIVER_PATCHLEVEL	2
-
-/* Driver will work either way: IRQ's save cpu time when waiting for
- * the card, but are subject to subtle interactions between bios,
- * hardware and the driver.
- */
-/* XXX: Add vblank support? */
-#define USE_IRQS 0
-
-typedef struct drm_i830_buf_priv {
-	u32 *in_use;
-	int my_use_idx;
-	int currently_mapped;
-	void __user *virtual;
-	void *kernel_virtual;
-	drm_local_map_t map;
-} drm_i830_buf_priv_t;
-
-typedef struct _drm_i830_ring_buffer {
-	int tail_mask;
-	unsigned long Start;
-	unsigned long End;
-	unsigned long Size;
-	u8 *virtual_start;
-	int head;
-	int tail;
-	int space;
-	drm_local_map_t map;
-} drm_i830_ring_buffer_t;
-
-typedef struct drm_i830_private {
-	struct drm_local_map *sarea_map;
-	struct drm_local_map *mmio_map;
-
-	drm_i830_sarea_t *sarea_priv;
-	drm_i830_ring_buffer_t ring;
-
-	void *hw_status_page;
-	unsigned long counter;
-
-	dma_addr_t dma_status_page;
-
-	struct drm_buf *mmap_buffer;
-
-	u32 front_di1, back_di1, zi1;
-
-	int back_offset;
-	int depth_offset;
-	int front_offset;
-	int w, h;
-	int pitch;
-	int back_pitch;
-	int depth_pitch;
-	unsigned int cpp;
-
-	int do_boxes;
-	int dma_used;
-
-	int current_page;
-	int page_flipping;
-
-	wait_queue_head_t irq_queue;
-	atomic_t irq_received;
-	atomic_t irq_emitted;
-
-	int use_mi_batchbuffer_start;
-
-} drm_i830_private_t;
-
-long i830_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
-extern struct drm_ioctl_desc i830_ioctls[];
-extern int i830_max_ioctl;
-
-/* i830_irq.c */
-extern int i830_irq_emit(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv);
-extern int i830_irq_wait(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv);
-
-extern irqreturn_t i830_driver_irq_handler(DRM_IRQ_ARGS);
-extern void i830_driver_irq_preinstall(struct drm_device *dev);
-extern void i830_driver_irq_postinstall(struct drm_device *dev);
-extern void i830_driver_irq_uninstall(struct drm_device *dev);
-extern int i830_driver_load(struct drm_device *, unsigned long flags);
-extern void i830_driver_preclose(struct drm_device *dev,
-				 struct drm_file *file_priv);
-extern void i830_driver_lastclose(struct drm_device *dev);
-extern void i830_driver_reclaim_buffers_locked(struct drm_device *dev,
-					       struct drm_file *file_priv);
-extern int i830_driver_dma_quiescent(struct drm_device *dev);
-extern int i830_driver_device_is_agp(struct drm_device *dev);
-
-#define I830_READ(reg)		DRM_READ32(dev_priv->mmio_map, reg)
-#define I830_WRITE(reg, val)	DRM_WRITE32(dev_priv->mmio_map, reg, val)
-#define I830_READ16(reg)	DRM_READ16(dev_priv->mmio_map, reg)
-#define I830_WRITE16(reg, val)	DRM_WRITE16(dev_priv->mmio_map, reg, val)
-
-#define I830_VERBOSE 0
-
-#define RING_LOCALS	unsigned int outring, ringmask, outcount; \
-			volatile char *virt;
-
-#define BEGIN_LP_RING(n) do {				\
-	if (I830_VERBOSE)				\
-		printk("BEGIN_LP_RING(%d)\n", (n));	\
-	if (dev_priv->ring.space < n*4)			\
-		i830_wait_ring(dev, n*4, __func__);	\
-	outcount = 0;					\
-	outring = dev_priv->ring.tail;			\
-	ringmask = dev_priv->ring.tail_mask;		\
-	virt = dev_priv->ring.virtual_start;		\
-} while (0)
-
-#define OUT_RING(n) do {					\
-	if (I830_VERBOSE)					\
-		printk("   OUT_RING %x\n", (int)(n));		\
-	*(volatile unsigned int *)(virt + outring) = n;		\
-	outcount++;						\
-	outring += 4;						\
-	outring &= ringmask;					\
-} while (0)
-
-#define ADVANCE_LP_RING() do {					\
-	if (I830_VERBOSE)					\
-		printk("ADVANCE_LP_RING %x\n", outring);	\
-	dev_priv->ring.tail = outring;				\
-	dev_priv->ring.space -= outcount * 4;			\
-	I830_WRITE(LP_RING + RING_TAIL, outring);		\
-} while (0)
-
-extern int i830_wait_ring(struct drm_device *dev, int n, const char *caller);
-
-#define GFX_OP_USER_INTERRUPT		((0<<29)|(2<<23))
-#define GFX_OP_BREAKPOINT_INTERRUPT	((0<<29)|(1<<23))
-#define CMD_REPORT_HEAD			(7<<23)
-#define CMD_STORE_DWORD_IDX		((0x21<<23) | 0x1)
-#define CMD_OP_BATCH_BUFFER  ((0x0<<29)|(0x30<<23)|0x1)
-
-#define STATE3D_LOAD_STATE_IMMEDIATE_2      ((0x3<<29)|(0x1d<<24)|(0x03<<16))
-#define LOAD_TEXTURE_MAP0                   (1<<11)
-
-#define INST_PARSER_CLIENT   0x00000000
-#define INST_OP_FLUSH        0x02000000
-#define INST_FLUSH_MAP_CACHE 0x00000001
-
-#define BB1_START_ADDR_MASK   (~0x7)
-#define BB1_PROTECTED         (1<<0)
-#define BB1_UNPROTECTED       (0<<0)
-#define BB2_END_ADDR_MASK     (~0x7)
-
-#define I830REG_HWSTAM		0x02098
-#define I830REG_INT_IDENTITY_R	0x020a4
-#define I830REG_INT_MASK_R	0x020a8
-#define I830REG_INT_ENABLE_R	0x020a0
-
-#define I830_IRQ_RESERVED ((1<<13)|(3<<2))
-
-#define LP_RING			0x2030
-#define HP_RING			0x2040
-#define RING_TAIL		0x00
-#define TAIL_ADDR		0x001FFFF8
-#define RING_HEAD		0x04
-#define HEAD_WRAP_COUNT		0xFFE00000
-#define HEAD_WRAP_ONE		0x00200000
-#define HEAD_ADDR		0x001FFFFC
-#define RING_START		0x08
-#define START_ADDR		0x0xFFFFF000
-#define RING_LEN		0x0C
-#define RING_NR_PAGES		0x001FF000
-#define RING_REPORT_MASK	0x00000006
-#define RING_REPORT_64K		0x00000002
-#define RING_REPORT_128K	0x00000004
-#define RING_NO_REPORT		0x00000000
-#define RING_VALID_MASK		0x00000001
-#define RING_VALID		0x00000001
-#define RING_INVALID		0x00000000
-
-#define GFX_OP_SCISSOR         ((0x3<<29)|(0x1c<<24)|(0x10<<19))
-#define SC_UPDATE_SCISSOR       (0x1<<1)
-#define SC_ENABLE_MASK          (0x1<<0)
-#define SC_ENABLE               (0x1<<0)
-
-#define GFX_OP_SCISSOR_INFO    ((0x3<<29)|(0x1d<<24)|(0x81<<16)|(0x1))
-#define SCI_YMIN_MASK      (0xffff<<16)
-#define SCI_XMIN_MASK      (0xffff<<0)
-#define SCI_YMAX_MASK      (0xffff<<16)
-#define SCI_XMAX_MASK      (0xffff<<0)
-
-#define GFX_OP_SCISSOR_ENABLE	 ((0x3<<29)|(0x1c<<24)|(0x10<<19))
-#define GFX_OP_SCISSOR_RECT	 ((0x3<<29)|(0x1d<<24)|(0x81<<16)|1)
-#define GFX_OP_COLOR_FACTOR      ((0x3<<29)|(0x1d<<24)|(0x1<<16)|0x0)
-#define GFX_OP_STIPPLE           ((0x3<<29)|(0x1d<<24)|(0x83<<16))
-#define GFX_OP_MAP_INFO          ((0x3<<29)|(0x1d<<24)|0x4)
-#define GFX_OP_DESTBUFFER_VARS   ((0x3<<29)|(0x1d<<24)|(0x85<<16)|0x0)
-#define GFX_OP_DRAWRECT_INFO     ((0x3<<29)|(0x1d<<24)|(0x80<<16)|(0x3))
-#define GFX_OP_PRIMITIVE         ((0x3<<29)|(0x1f<<24))
-
-#define CMD_OP_DESTBUFFER_INFO	 ((0x3<<29)|(0x1d<<24)|(0x8e<<16)|1)
-
-#define CMD_OP_DISPLAYBUFFER_INFO ((0x0<<29)|(0x14<<23)|2)
-#define ASYNC_FLIP                (1<<22)
-
-#define CMD_3D                          (0x3<<29)
-#define STATE3D_CONST_BLEND_COLOR_CMD   (CMD_3D|(0x1d<<24)|(0x88<<16))
-#define STATE3D_MAP_COORD_SETBIND_CMD   (CMD_3D|(0x1d<<24)|(0x02<<16))
-
-#define BR00_BITBLT_CLIENT   0x40000000
-#define BR00_OP_COLOR_BLT    0x10000000
-#define BR00_OP_SRC_COPY_BLT 0x10C00000
-#define BR13_SOLID_PATTERN   0x80000000
-
-#define BUF_3D_ID_COLOR_BACK    (0x3<<24)
-#define BUF_3D_ID_DEPTH         (0x7<<24)
-#define BUF_3D_USE_FENCE        (1<<23)
-#define BUF_3D_PITCH(x)         (((x)/4)<<2)
-
-#define CMD_OP_MAP_PALETTE_LOAD	((3<<29)|(0x1d<<24)|(0x82<<16)|255)
-#define MAP_PALETTE_NUM(x)	((x<<8) & (1<<8))
-#define MAP_PALETTE_BOTH	(1<<11)
-
-#define XY_COLOR_BLT_CMD		((2<<29)|(0x50<<22)|0x4)
-#define XY_COLOR_BLT_WRITE_ALPHA	(1<<21)
-#define XY_COLOR_BLT_WRITE_RGB		(1<<20)
-
-#define XY_SRC_COPY_BLT_CMD             ((2<<29)|(0x53<<22)|6)
-#define XY_SRC_COPY_BLT_WRITE_ALPHA     (1<<21)
-#define XY_SRC_COPY_BLT_WRITE_RGB       (1<<20)
-
-#define MI_BATCH_BUFFER		((0x30<<23)|1)
-#define MI_BATCH_BUFFER_START	(0x31<<23)
-#define MI_BATCH_BUFFER_END	(0xA<<23)
-#define MI_BATCH_NON_SECURE	(1)
-
-#define MI_WAIT_FOR_EVENT       ((0x3<<23))
-#define MI_WAIT_FOR_PLANE_A_FLIP      (1<<2)
-#define MI_WAIT_FOR_PLANE_A_SCANLINES (1<<1)
-
-#define MI_LOAD_SCAN_LINES_INCL  ((0x12<<23))
-
-#endif
diff --git a/drivers/gpu/drm/i830/i830_irq.c b/drivers/gpu/drm/i830/i830_irq.c
deleted file mode 100644
index d1a6b95d631d..000000000000
--- a/drivers/gpu/drm/i830/i830_irq.c
+++ /dev/null
@@ -1,186 +0,0 @@
-/* i830_dma.c -- DMA support for the I830 -*- linux-c -*-
- *
- * Copyright 2002 Tungsten Graphics, 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
- * TUNGSTEN GRAPHICS 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>
- *
- */
-
-#include "drmP.h"
-#include "drm.h"
-#include "i830_drm.h"
-#include "i830_drv.h"
-#include <linux/interrupt.h>	/* For task queue support */
-#include <linux/delay.h>
-
-irqreturn_t i830_driver_irq_handler(DRM_IRQ_ARGS)
-{
-	struct drm_device *dev = (struct drm_device *) arg;
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-	u16 temp;
-
-	temp = I830_READ16(I830REG_INT_IDENTITY_R);
-	DRM_DEBUG("%x\n", temp);
-
-	if (!(temp & 2))
-		return IRQ_NONE;
-
-	I830_WRITE16(I830REG_INT_IDENTITY_R, temp);
-
-	atomic_inc(&dev_priv->irq_received);
-	wake_up_interruptible(&dev_priv->irq_queue);
-
-	return IRQ_HANDLED;
-}
-
-static int i830_emit_irq(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-
-	DRM_DEBUG("%s\n", __func__);
-
-	atomic_inc(&dev_priv->irq_emitted);
-
-	BEGIN_LP_RING(2);
-	OUT_RING(0);
-	OUT_RING(GFX_OP_USER_INTERRUPT);
-	ADVANCE_LP_RING();
-
-	return atomic_read(&dev_priv->irq_emitted);
-}
-
-static int i830_wait_irq(struct drm_device *dev, int irq_nr)
-{
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-	DECLARE_WAITQUEUE(entry, current);
-	unsigned long end = jiffies + HZ * 3;
-	int ret = 0;
-
-	DRM_DEBUG("%s\n", __func__);
-
-	if (atomic_read(&dev_priv->irq_received) >= irq_nr)
-		return 0;
-
-	dev_priv->sarea_priv->perf_boxes |= I830_BOX_WAIT;
-
-	add_wait_queue(&dev_priv->irq_queue, &entry);
-
-	for (;;) {
-		__set_current_state(TASK_INTERRUPTIBLE);
-		if (atomic_read(&dev_priv->irq_received) >= irq_nr)
-			break;
-		if ((signed)(end - jiffies) <= 0) {
-			DRM_ERROR("timeout iir %x imr %x ier %x hwstam %x\n",
-				  I830_READ16(I830REG_INT_IDENTITY_R),
-				  I830_READ16(I830REG_INT_MASK_R),
-				  I830_READ16(I830REG_INT_ENABLE_R),
-				  I830_READ16(I830REG_HWSTAM));
-
-			ret = -EBUSY;	/* Lockup?  Missed irq? */
-			break;
-		}
-		schedule_timeout(HZ * 3);
-		if (signal_pending(current)) {
-			ret = -EINTR;
-			break;
-		}
-	}
-
-	__set_current_state(TASK_RUNNING);
-	remove_wait_queue(&dev_priv->irq_queue, &entry);
-	return ret;
-}
-
-/* Needs the lock as it touches the ring.
- */
-int i830_irq_emit(struct drm_device *dev, void *data,
-		  struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_irq_emit_t *emit = data;
-	int result;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (!dev_priv) {
-		DRM_ERROR("%s called with no initialization\n", __func__);
-		return -EINVAL;
-	}
-
-	result = i830_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 i830_irq_wait(struct drm_device *dev, void *data,
-		  struct drm_file *file_priv)
-{
-	drm_i830_private_t *dev_priv = dev->dev_private;
-	drm_i830_irq_wait_t *irqwait = data;
-
-	if (!dev_priv) {
-		DRM_ERROR("%s called with no initialization\n", __func__);
-		return -EINVAL;
-	}
-
-	return i830_wait_irq(dev, irqwait->irq_seq);
-}
-
-/* drm_dma.h hooks
-*/
-void i830_driver_irq_preinstall(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-
-	I830_WRITE16(I830REG_HWSTAM, 0xffff);
-	I830_WRITE16(I830REG_INT_MASK_R, 0x0);
-	I830_WRITE16(I830REG_INT_ENABLE_R, 0x0);
-	atomic_set(&dev_priv->irq_received, 0);
-	atomic_set(&dev_priv->irq_emitted, 0);
-	init_waitqueue_head(&dev_priv->irq_queue);
-}
-
-void i830_driver_irq_postinstall(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-
-	I830_WRITE16(I830REG_INT_ENABLE_R, 0x2);
-}
-
-void i830_driver_irq_uninstall(struct drm_device *dev)
-{
-	drm_i830_private_t *dev_priv = (drm_i830_private_t *) dev->dev_private;
-	if (!dev_priv)
-		return;
-
-	I830_WRITE16(I830REG_INT_MASK_R, 0xffff);
-	I830_WRITE16(I830REG_INT_ENABLE_R, 0x0);
-}
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 3601466c5502..4ff9b6cc973f 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -865,7 +865,7 @@ static int i915_cur_delayinfo(struct seq_file *m, void *unused)
 		int max_freq;
 
 		/* RPSTAT1 is in the GT power well */
-		__gen6_force_wake_get(dev_priv);
+		__gen6_gt_force_wake_get(dev_priv);
 
 		seq_printf(m, "GT_PERF_STATUS: 0x%08x\n", gt_perf_status);
 		seq_printf(m, "RPSTAT1: 0x%08x\n", I915_READ(GEN6_RPSTAT1));
@@ -888,7 +888,7 @@ static int i915_cur_delayinfo(struct seq_file *m, void *unused)
 		seq_printf(m, "Max non-overclocked (RP0) frequency: %dMHz\n",
 			   max_freq * 100);
 
-		__gen6_force_wake_put(dev_priv);
+		__gen6_gt_force_wake_put(dev_priv);
 	} else {
 		seq_printf(m, "no P-state info available\n");
 	}
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 17bd766f2081..e33d9be7df3b 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1895,6 +1895,17 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	if (IS_GEN2(dev))
 		dma_set_coherent_mask(&dev->pdev->dev, DMA_BIT_MASK(30));
 
+	/* 965GM sometimes incorrectly writes to hardware status page (HWS)
+	 * using 32bit addressing, overwriting memory if HWS is located
+	 * above 4GB.
+	 *
+	 * The documentation also mentions an issue with undefined
+	 * behaviour if any general state is accessed within a page above 4GB,
+	 * which also needs to be handled carefully.
+	 */
+	if (IS_BROADWATER(dev) || IS_CRESTLINE(dev))
+		dma_set_coherent_mask(&dev->pdev->dev, DMA_BIT_MASK(32));
+
 	mmio_bar = IS_GEN2(dev) ? 1 : 0;
 	dev_priv->regs = pci_iomap(dev->pdev, mmio_bar, 0);
 	if (!dev_priv->regs) {
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 0ad533f06af9..99b09ce6cf44 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -46,6 +46,9 @@ module_param_named(fbpercrtc, i915_fbpercrtc, int, 0400);
 unsigned int i915_powersave = 1;
 module_param_named(powersave, i915_powersave, int, 0600);
 
+unsigned int i915_semaphores = 0;
+module_param_named(semaphores, i915_semaphores, int, 0600);
+
 unsigned int i915_enable_rc6 = 0;
 module_param_named(i915_enable_rc6, i915_enable_rc6, int, 0600);
 
@@ -254,7 +257,7 @@ void intel_detect_pch (struct drm_device *dev)
 	}
 }
 
-void __gen6_force_wake_get(struct drm_i915_private *dev_priv)
+void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
 {
 	int count;
 
@@ -270,12 +273,22 @@ void __gen6_force_wake_get(struct drm_i915_private *dev_priv)
 		udelay(10);
 }
 
-void __gen6_force_wake_put(struct drm_i915_private *dev_priv)
+void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
 {
 	I915_WRITE_NOTRACE(FORCEWAKE, 0);
 	POSTING_READ(FORCEWAKE);
 }
 
+void __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
+{
+	int loop = 500;
+	u32 fifo = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
+	while (fifo < 20 && loop--) {
+		udelay(10);
+		fifo = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
+	}
+}
+
 static int i915_drm_freeze(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -703,6 +716,9 @@ static struct drm_driver driver = {
 	.gem_init_object = i915_gem_init_object,
 	.gem_free_object = i915_gem_free_object,
 	.gem_vm_ops = &i915_gem_vm_ops,
+	.dumb_create = i915_gem_dumb_create,
+	.dumb_map_offset = i915_gem_mmap_gtt,
+	.dumb_destroy = i915_gem_dumb_destroy,
 	.ioctls = i915_ioctls,
 	.fops = {
 		 .owner = THIS_MODULE,
@@ -719,14 +735,6 @@ static struct drm_driver driver = {
 		 .llseek = noop_llseek,
 	},
 
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-		 .probe = i915_pci_probe,
-		 .remove = i915_pci_remove,
-		 .driver.pm = &i915_pm_ops,
-	},
-
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -735,6 +743,14 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver i915_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = i915_pci_probe,
+	.remove = i915_pci_remove,
+	.driver.pm = &i915_pm_ops,
+};
+
 static int __init i915_init(void)
 {
 	if (!intel_agp_enabled) {
@@ -768,12 +784,12 @@ static int __init i915_init(void)
 	if (!(driver.driver_features & DRIVER_MODESET))
 		driver.get_vblank_timestamp = NULL;
 
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &i915_pci_driver);
 }
 
 static void __exit i915_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &i915_pci_driver);
 }
 
 module_init(i915_init);
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 65dfe81d0035..4305758bd6b5 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -956,6 +956,7 @@ extern struct drm_ioctl_desc i915_ioctls[];
 extern int i915_max_ioctl;
 extern unsigned int i915_fbpercrtc;
 extern unsigned int i915_powersave;
+extern unsigned int i915_semaphores;
 extern unsigned int i915_lvds_downclock;
 extern unsigned int i915_panel_use_ssc;
 extern unsigned int i915_enable_rc6;
@@ -1115,6 +1116,13 @@ void i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 				    struct intel_ring_buffer *ring,
 				    u32 seqno);
 
+int i915_gem_dumb_create(struct drm_file *file_priv,
+			 struct drm_device *dev,
+			 struct drm_mode_create_dumb *args);
+int i915_gem_mmap_gtt(struct drm_file *file_priv, struct drm_device *dev,
+		      uint32_t handle, uint64_t *offset);
+int i915_gem_dumb_destroy(struct drm_file *file_priv, struct drm_device *dev,
+			  uint32_t handle);			  
 /**
  * Returns true if seq1 is later than seq2.
  */
@@ -1177,6 +1185,9 @@ void i915_gem_detach_phys_object(struct drm_device *dev,
 void i915_gem_free_all_phys_object(struct drm_device *dev);
 void i915_gem_release(struct drm_device *dev, struct drm_file *file);
 
+uint32_t
+i915_gem_get_unfenced_gtt_alignment(struct drm_i915_gem_object *obj);
+
 /* i915_gem_gtt.c */
 void i915_gem_restore_gtt_mappings(struct drm_device *dev);
 int __must_check i915_gem_gtt_bind_object(struct drm_i915_gem_object *obj);
@@ -1353,22 +1364,32 @@ __i915_write(64, q)
  * must be set to prevent GT core from power down and stale values being
  * returned.
  */
-void __gen6_force_wake_get(struct drm_i915_private *dev_priv);
-void __gen6_force_wake_put (struct drm_i915_private *dev_priv);
-static inline u32 i915_safe_read(struct drm_i915_private *dev_priv, u32 reg)
+void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv);
+void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv);
+void __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv);
+
+static inline u32 i915_gt_read(struct drm_i915_private *dev_priv, u32 reg)
 {
 	u32 val;
 
 	if (dev_priv->info->gen >= 6) {
-		__gen6_force_wake_get(dev_priv);
+		__gen6_gt_force_wake_get(dev_priv);
 		val = I915_READ(reg);
-		__gen6_force_wake_put(dev_priv);
+		__gen6_gt_force_wake_put(dev_priv);
 	} else
 		val = I915_READ(reg);
 
 	return val;
 }
 
+static inline void i915_gt_write(struct drm_i915_private *dev_priv,
+				u32 reg, u32 val)
+{
+	if (dev_priv->info->gen >= 6)
+		__gen6_gt_wait_for_fifo(dev_priv);
+	I915_WRITE(reg, val);
+}
+
 static inline void
 i915_write(struct drm_i915_private *dev_priv, u32 reg, u64 val, int len)
 {
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index cf4f74c7c6fb..4aaf6cdd9a0b 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -193,22 +193,20 @@ i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
-/**
- * Creates a new mm object and returns a handle to it.
- */
-int
-i915_gem_create_ioctl(struct drm_device *dev, void *data,
-		      struct drm_file *file)
+static int
+i915_gem_create(struct drm_file *file,
+		struct drm_device *dev,
+		uint64_t size,
+		uint32_t *handle_p)
 {
-	struct drm_i915_gem_create *args = data;
 	struct drm_i915_gem_object *obj;
 	int ret;
 	u32 handle;
 
-	args->size = roundup(args->size, PAGE_SIZE);
+	size = roundup(size, PAGE_SIZE);
 
 	/* Allocate the new object */
-	obj = i915_gem_alloc_object(dev, args->size);
+	obj = i915_gem_alloc_object(dev, size);
 	if (obj == NULL)
 		return -ENOMEM;
 
@@ -224,10 +222,41 @@ i915_gem_create_ioctl(struct drm_device *dev, void *data,
 	drm_gem_object_unreference(&obj->base);
 	trace_i915_gem_object_create(obj);
 
-	args->handle = handle;
+	*handle_p = handle;
 	return 0;
 }
 
+int
+i915_gem_dumb_create(struct drm_file *file,
+		     struct drm_device *dev,
+		     struct drm_mode_create_dumb *args)
+{
+	/* have to work out size/pitch and return them */
+	args->pitch = ALIGN(args->width & ((args->bpp + 1) / 8), 64);
+	args->size = args->pitch * args->height;
+	return i915_gem_create(file, dev,
+			       args->size, &args->handle);
+}
+
+int i915_gem_dumb_destroy(struct drm_file *file,
+			  struct drm_device *dev,
+			  uint32_t handle)
+{
+	return drm_gem_handle_delete(file, handle);
+}
+
+/**
+ * Creates a new mm object and returns a handle to it.
+ */
+int
+i915_gem_create_ioctl(struct drm_device *dev, void *data,
+		      struct drm_file *file)
+{
+	struct drm_i915_gem_create *args = data;
+	return i915_gem_create(file, dev,
+			       args->size, &args->handle);
+}
+
 static int i915_gem_object_needs_bit17_swizzle(struct drm_i915_gem_object *obj)
 {
 	drm_i915_private_t *dev_priv = obj->base.dev->dev_private;
@@ -1398,7 +1427,7 @@ i915_gem_get_gtt_alignment(struct drm_i915_gem_object *obj)
  * Return the required GTT alignment for an object, only taking into account
  * unfenced tiled surface requirements.
  */
-static uint32_t
+uint32_t
 i915_gem_get_unfenced_gtt_alignment(struct drm_i915_gem_object *obj)
 {
 	struct drm_device *dev = obj->base.dev;
@@ -1425,27 +1454,13 @@ i915_gem_get_unfenced_gtt_alignment(struct drm_i915_gem_object *obj)
 	return tile_height * obj->stride * 2;
 }
 
-/**
- * i915_gem_mmap_gtt_ioctl - prepare an object for GTT mmap'ing
- * @dev: DRM device
- * @data: GTT mapping ioctl data
- * @file: GEM object info
- *
- * Simply returns the fake offset to userspace so it can mmap it.
- * The mmap call will end up in drm_gem_mmap(), which will set things
- * up so we can get faults in the handler above.
- *
- * The fault handler will take care of binding the object into the GTT
- * (since it may have been evicted to make room for something), allocating
- * a fence register, and mapping the appropriate aperture address into
- * userspace.
- */
 int
-i915_gem_mmap_gtt_ioctl(struct drm_device *dev, void *data,
-			struct drm_file *file)
+i915_gem_mmap_gtt(struct drm_file *file,
+		  struct drm_device *dev,
+		  uint32_t handle,
+		  uint64_t *offset)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_mmap_gtt *args = data;
 	struct drm_i915_gem_object *obj;
 	int ret;
 
@@ -1456,7 +1471,7 @@ i915_gem_mmap_gtt_ioctl(struct drm_device *dev, void *data,
 	if (ret)
 		return ret;
 
-	obj = to_intel_bo(drm_gem_object_lookup(dev, file, args->handle));
+	obj = to_intel_bo(drm_gem_object_lookup(dev, file, handle));
 	if (obj == NULL) {
 		ret = -ENOENT;
 		goto unlock;
@@ -1479,7 +1494,7 @@ i915_gem_mmap_gtt_ioctl(struct drm_device *dev, void *data,
 			goto out;
 	}
 
-	args->offset = (u64)obj->base.map_list.hash.key << PAGE_SHIFT;
+	*offset = (u64)obj->base.map_list.hash.key << PAGE_SHIFT;
 
 out:
 	drm_gem_object_unreference(&obj->base);
@@ -1488,6 +1503,34 @@ unlock:
 	return ret;
 }
 
+/**
+ * i915_gem_mmap_gtt_ioctl - prepare an object for GTT mmap'ing
+ * @dev: DRM device
+ * @data: GTT mapping ioctl data
+ * @file: GEM object info
+ *
+ * Simply returns the fake offset to userspace so it can mmap it.
+ * The mmap call will end up in drm_gem_mmap(), which will set things
+ * up so we can get faults in the handler above.
+ *
+ * The fault handler will take care of binding the object into the GTT
+ * (since it may have been evicted to make room for something), allocating
+ * a fence register, and mapping the appropriate aperture address into
+ * userspace.
+ */
+int
+i915_gem_mmap_gtt_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file)
+{
+	struct drm_i915_gem_mmap_gtt *args = data;
+
+	if (!(dev->driver->driver_features & DRIVER_GEM))
+		return -ENODEV;
+
+	return i915_gem_mmap_gtt(file, dev, args->handle, &args->offset);
+}
+
+
 static int
 i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj,
 			      gfp_t gfpmask)
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index d2f445e825f2..50ab1614571c 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -772,8 +772,8 @@ i915_gem_execbuffer_sync_rings(struct drm_i915_gem_object *obj,
 	if (from == NULL || to == from)
 		return 0;
 
-	/* XXX gpu semaphores are currently causing hard hangs on SNB mobile */
-	if (INTEL_INFO(obj->base.dev)->gen < 6 || IS_MOBILE(obj->base.dev))
+	/* XXX gpu semaphores are implicated in various hard hangs on SNB */
+	if (INTEL_INFO(obj->base.dev)->gen < 6 || !i915_semaphores)
 		return i915_gem_object_wait_rendering(obj, true);
 
 	idx = intel_ring_sync_index(from, to);
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index 22a32b9932c5..d64843e18df2 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -349,14 +349,27 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 			(obj->gtt_offset + obj->base.size <= dev_priv->mm.gtt_mappable_end &&
 			 i915_gem_object_fence_ok(obj, args->tiling_mode));
 
-		obj->tiling_changed = true;
-		obj->tiling_mode = args->tiling_mode;
-		obj->stride = args->stride;
+		/* Rebind if we need a change of alignment */
+		if (!obj->map_and_fenceable) {
+			u32 unfenced_alignment =
+				i915_gem_get_unfenced_gtt_alignment(obj);
+			if (obj->gtt_offset & (unfenced_alignment - 1))
+				ret = i915_gem_object_unbind(obj);
+		}
+
+		if (ret == 0) {
+			obj->tiling_changed = true;
+			obj->tiling_mode = args->tiling_mode;
+			obj->stride = args->stride;
+		}
 	}
+	/* we have to maintain this existing ABI... */
+	args->stride = obj->stride;
+	args->tiling_mode = obj->tiling_mode;
 	drm_gem_object_unreference(&obj->base);
 	mutex_unlock(&dev->struct_mutex);
 
-	return 0;
+	return ret;
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 97f946dcc1aa..8a9e08bf1cf7 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -316,6 +316,8 @@ static void i915_hotplug_work_func(struct work_struct *work)
 	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 
+	DRM_DEBUG_KMS("running encoder hotplug functions\n");
+
 	list_for_each_entry(encoder, &mode_config->encoder_list, base.head)
 		if (encoder->hot_plug)
 			encoder->hot_plug(encoder);
@@ -1649,9 +1651,7 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 	} else {
 		hotplug_mask = SDE_CRT_HOTPLUG | SDE_PORTB_HOTPLUG |
 			       SDE_PORTC_HOTPLUG | SDE_PORTD_HOTPLUG;
-		hotplug_mask |= SDE_AUX_MASK | SDE_FDI_MASK | SDE_TRANS_MASK;
-		I915_WRITE(FDI_RXA_IMR, 0);
-		I915_WRITE(FDI_RXB_IMR, 0);
+		hotplug_mask |= SDE_AUX_MASK;
 	}
 
 	dev_priv->pch_irq_mask = ~hotplug_mask;
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 729d4233b763..3e6f486f4605 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -3261,6 +3261,8 @@
 #define  FORCEWAKE				0xA18C
 #define  FORCEWAKE_ACK				0x130090
 
+#define  GT_FIFO_FREE_ENTRIES			0x120008
+
 #define GEN6_RPNSWREQ				0xA008
 #define   GEN6_TURBO_DISABLE			(1<<31)
 #define   GEN6_FREQUENCY(x)			((x)<<25)
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 3b006536b3d2..49fb54fd9a18 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -1219,7 +1219,7 @@ static void sandybridge_blit_fbc_update(struct drm_device *dev)
 	u32 blt_ecoskpd;
 
 	/* Make sure blitter notifies FBC of writes */
-	__gen6_force_wake_get(dev_priv);
+	__gen6_gt_force_wake_get(dev_priv);
 	blt_ecoskpd = I915_READ(GEN6_BLITTER_ECOSKPD);
 	blt_ecoskpd |= GEN6_BLITTER_FBC_NOTIFY <<
 		GEN6_BLITTER_LOCK_SHIFT;
@@ -1230,7 +1230,7 @@ static void sandybridge_blit_fbc_update(struct drm_device *dev)
 			 GEN6_BLITTER_LOCK_SHIFT);
 	I915_WRITE(GEN6_BLITTER_ECOSKPD, blt_ecoskpd);
 	POSTING_READ(GEN6_BLITTER_ECOSKPD);
-	__gen6_force_wake_put(dev_priv);
+	__gen6_gt_force_wake_put(dev_priv);
 }
 
 static void ironlake_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
@@ -1630,19 +1630,19 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
 		struct drm_i915_gem_object *obj = to_intel_framebuffer(old_fb)->obj;
 
 		wait_event(dev_priv->pending_flip_queue,
+			   atomic_read(&dev_priv->mm.wedged) ||
 			   atomic_read(&obj->pending_flip) == 0);
 
 		/* 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.
+		 *
+		 * This should only fail upon a hung GPU, in which case we
+		 * can safely continue.
 		 */
 		ret = i915_gem_object_flush_gpu(obj, false);
-		if (ret) {
-			i915_gem_object_unpin(to_intel_framebuffer(crtc->fb)->obj);
-			mutex_unlock(&dev->struct_mutex);
-			return ret;
-		}
+		(void) ret;
 	}
 
 	ret = intel_pipe_set_base_atomic(crtc, crtc->fb, x, y,
@@ -2045,6 +2045,31 @@ static void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 		   atomic_read(&obj->pending_flip) == 0);
 }
 
+static bool intel_crtc_driving_pch(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct intel_encoder *encoder;
+
+	/*
+	 * If there's a non-PCH eDP on this crtc, it must be DP_A, and that
+	 * must be driven by its own crtc; no sharing is possible.
+	 */
+	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
+		if (encoder->base.crtc != crtc)
+			continue;
+
+		switch (encoder->type) {
+		case INTEL_OUTPUT_EDP:
+			if (!intel_encoder_is_pch_edp(&encoder->base))
+				return false;
+			continue;
+		}
+	}
+
+	return true;
+}
+
 static void ironlake_crtc_enable(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -2053,6 +2078,7 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	int pipe = intel_crtc->pipe;
 	int plane = intel_crtc->plane;
 	u32 reg, temp;
+	bool is_pch_port = false;
 
 	if (intel_crtc->active)
 		return;
@@ -2066,7 +2092,56 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 			I915_WRITE(PCH_LVDS, temp | LVDS_PORT_EN);
 	}
 
-	ironlake_fdi_enable(crtc);
+	is_pch_port = intel_crtc_driving_pch(crtc);
+
+	if (is_pch_port)
+		ironlake_fdi_enable(crtc);
+	else {
+		/* disable CPU FDI tx and PCH FDI rx */
+		reg = FDI_TX_CTL(pipe);
+		temp = I915_READ(reg);
+		I915_WRITE(reg, temp & ~FDI_TX_ENABLE);
+		POSTING_READ(reg);
+
+		reg = FDI_RX_CTL(pipe);
+		temp = I915_READ(reg);
+		temp &= ~(0x7 << 16);
+		temp |= (I915_READ(PIPECONF(pipe)) & PIPE_BPC_MASK) << 11;
+		I915_WRITE(reg, temp & ~FDI_RX_ENABLE);
+
+		POSTING_READ(reg);
+		udelay(100);
+
+		/* Ironlake workaround, disable clock pointer after downing FDI */
+		if (HAS_PCH_IBX(dev))
+			I915_WRITE(FDI_RX_CHICKEN(pipe),
+				   I915_READ(FDI_RX_CHICKEN(pipe) &
+					     ~FDI_RX_PHASE_SYNC_POINTER_ENABLE));
+
+		/* still set train pattern 1 */
+		reg = FDI_TX_CTL(pipe);
+		temp = I915_READ(reg);
+		temp &= ~FDI_LINK_TRAIN_NONE;
+		temp |= FDI_LINK_TRAIN_PATTERN_1;
+		I915_WRITE(reg, temp);
+
+		reg = FDI_RX_CTL(pipe);
+		temp = I915_READ(reg);
+		if (HAS_PCH_CPT(dev)) {
+			temp &= ~FDI_LINK_TRAIN_PATTERN_MASK_CPT;
+			temp |= FDI_LINK_TRAIN_PATTERN_1_CPT;
+		} else {
+			temp &= ~FDI_LINK_TRAIN_NONE;
+			temp |= FDI_LINK_TRAIN_PATTERN_1;
+		}
+		/* BPC in FDI rx is consistent with that in PIPECONF */
+		temp &= ~(0x07 << 16);
+		temp |= (I915_READ(PIPECONF(pipe)) & PIPE_BPC_MASK) << 11;
+		I915_WRITE(reg, temp);
+
+		POSTING_READ(reg);
+		udelay(100);
+	}
 
 	/* Enable panel fitting for LVDS */
 	if (dev_priv->pch_pf_size &&
@@ -2100,6 +2175,10 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 		intel_flush_display_plane(dev, plane);
 	}
 
+	/* Skip the PCH stuff if possible */
+	if (!is_pch_port)
+		goto done;
+
 	/* For PCH output, training FDI link */
 	if (IS_GEN6(dev))
 		gen6_fdi_link_train(crtc);
@@ -2184,7 +2263,7 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	I915_WRITE(reg, temp | TRANS_ENABLE);
 	if (wait_for(I915_READ(reg) & TRANS_STATE_ENABLE, 100))
 		DRM_ERROR("failed to enable transcoder %d\n", pipe);
-
+done:
 	intel_crtc_load_lut(crtc);
 	intel_update_fbc(dev);
 	intel_crtc_update_cursor(crtc, true);
@@ -6203,7 +6282,7 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	 * userspace...
 	 */
 	I915_WRITE(GEN6_RC_STATE, 0);
-	__gen6_force_wake_get(dev_priv);
+	__gen6_gt_force_wake_get(dev_priv);
 
 	/* disable the counters and set deterministic thresholds */
 	I915_WRITE(GEN6_RC_CONTROL, 0);
@@ -6301,7 +6380,7 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	/* enable all PM interrupts */
 	I915_WRITE(GEN6_PMINTRMSK, 0);
 
-	__gen6_force_wake_put(dev_priv);
+	__gen6_gt_force_wake_put(dev_priv);
 }
 
 void intel_enable_clock_gating(struct drm_device *dev)
@@ -6496,7 +6575,7 @@ static void ironlake_disable_rc6(struct drm_device *dev)
 		POSTING_READ(RSTDBYCTL);
 	}
 
-	ironlake_disable_rc6(dev);
+	ironlake_teardown_rc6(dev);
 }
 
 static int ironlake_setup_rc6(struct drm_device *dev)
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 6d6fde85a636..34306865a5df 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -14,22 +14,23 @@ struct  intel_hw_status_page {
 	struct		drm_i915_gem_object *obj;
 };
 
-#define I915_RING_READ(reg) i915_safe_read(dev_priv, reg)
+#define I915_RING_READ(reg) i915_gt_read(dev_priv, reg)
+#define I915_RING_WRITE(reg, val) i915_gt_write(dev_priv, reg, val)
 
 #define I915_READ_TAIL(ring) I915_RING_READ(RING_TAIL((ring)->mmio_base))
-#define I915_WRITE_TAIL(ring, val) I915_WRITE(RING_TAIL((ring)->mmio_base), val)
+#define I915_WRITE_TAIL(ring, val) I915_RING_WRITE(RING_TAIL((ring)->mmio_base), val)
 
 #define I915_READ_START(ring) I915_RING_READ(RING_START((ring)->mmio_base))
-#define I915_WRITE_START(ring, val) I915_WRITE(RING_START((ring)->mmio_base), val)
+#define I915_WRITE_START(ring, val) I915_RING_WRITE(RING_START((ring)->mmio_base), val)
 
 #define I915_READ_HEAD(ring)  I915_RING_READ(RING_HEAD((ring)->mmio_base))
-#define I915_WRITE_HEAD(ring, val) I915_WRITE(RING_HEAD((ring)->mmio_base), val)
+#define I915_WRITE_HEAD(ring, val) I915_RING_WRITE(RING_HEAD((ring)->mmio_base), val)
 
 #define I915_READ_CTL(ring) I915_RING_READ(RING_CTL((ring)->mmio_base))
-#define I915_WRITE_CTL(ring, val) I915_WRITE(RING_CTL((ring)->mmio_base), val)
+#define I915_WRITE_CTL(ring, val) I915_RING_WRITE(RING_CTL((ring)->mmio_base), val)
 
-#define I915_WRITE_IMR(ring, val) I915_WRITE(RING_IMR((ring)->mmio_base), val)
 #define I915_READ_IMR(ring) I915_RING_READ(RING_IMR((ring)->mmio_base))
+#define I915_WRITE_IMR(ring, val) I915_RING_WRITE(RING_IMR((ring)->mmio_base), val)
 
 #define I915_READ_NOPID(ring) I915_RING_READ(RING_NOPID((ring)->mmio_base))
 #define I915_READ_SYNC_0(ring) I915_RING_READ(RING_SYNC_0((ring)->mmio_base))
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index 7c50cdce84f0..fefc8d60a835 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -1482,7 +1482,7 @@ static void intel_sdvo_get_ddc_modes(struct drm_connector *connector)
  * Note!  This is in reply order (see loop in get_tv_modes).
  * XXX: all 60Hz refresh?
  */
-struct drm_display_mode sdvo_tv_modes[] = {
+static const struct drm_display_mode sdvo_tv_modes[] = {
 	{ DRM_MODE("320x200", DRM_MODE_TYPE_DRIVER, 5815, 320, 321, 384,
 		   416, 0, 200, 201, 232, 233, 0,
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
diff --git a/drivers/gpu/drm/mga/mga_dma.c b/drivers/gpu/drm/mga/mga_dma.c
index 08868ac3048a..1e1eb1d7e971 100644
--- a/drivers/gpu/drm/mga/mga_dma.c
+++ b/drivers/gpu/drm/mga/mga_dma.c
@@ -703,7 +703,7 @@ static int mga_do_pci_dma_bootstrap(struct drm_device *dev,
 static int mga_do_dma_bootstrap(struct drm_device *dev,
 				drm_mga_dma_bootstrap_t *dma_bs)
 {
-	const int is_agp = (dma_bs->agp_mode != 0) && drm_device_is_agp(dev);
+	const int is_agp = (dma_bs->agp_mode != 0) && drm_pci_device_is_agp(dev);
 	int err;
 	drm_mga_private_t *const dev_priv =
 	    (drm_mga_private_t *) dev->dev_private;
diff --git a/drivers/gpu/drm/mga/mga_drv.c b/drivers/gpu/drm/mga/mga_drv.c
index 0aaf5f67a436..42d31874edf2 100644
--- a/drivers/gpu/drm/mga/mga_drv.c
+++ b/drivers/gpu/drm/mga/mga_drv.c
@@ -75,10 +75,6 @@ static struct drm_driver driver = {
 #endif
 		.llseek = noop_llseek,
 	},
-	.pci_driver = {
-		.name = DRIVER_NAME,
-		.id_table = pciidlist,
-	},
 
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
@@ -88,15 +84,20 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver mga_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init mga_init(void)
 {
 	driver.num_ioctls = mga_max_ioctl;
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &mga_pci_driver);
 }
 
 static void __exit mga_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &mga_pci_driver);
 }
 
 module_init(mga_init);
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index 6bdab891c64e..8314a49b6b9a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -282,7 +282,7 @@ static void still_alive(void)
 {
 #if 0
 	sync();
-	msleep(2);
+	mdelay(2);
 #endif
 }
 
@@ -1904,7 +1904,7 @@ init_condition_time(struct nvbios *bios, uint16_t offset,
 			BIOSLOG(bios, "0x%04X: "
 				"Condition not met, sleeping for 20ms\n",
 								offset);
-			msleep(20);
+			mdelay(20);
 		}
 	}
 
@@ -1938,7 +1938,7 @@ init_ltime(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 	BIOSLOG(bios, "0x%04X: Sleeping for 0x%04X milliseconds\n",
 		offset, time);
 
-	msleep(time);
+	mdelay(time);
 
 	return 3;
 }
@@ -2962,7 +2962,7 @@ init_time(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 	if (time < 1000)
 		udelay(time);
 	else
-		msleep((time + 900) / 1000);
+		mdelay((time + 900) / 1000);
 
 	return 3;
 }
@@ -3856,7 +3856,7 @@ static int call_lvds_manufacturer_script(struct drm_device *dev, struct dcb_entr
 
 	if (script == LVDS_PANEL_OFF) {
 		/* off-on delay in ms */
-		msleep(ROM16(bios->data[bios->fp.xlated_entry + 7]));
+		mdelay(ROM16(bios->data[bios->fp.xlated_entry + 7]));
 	}
 #ifdef __powerpc__
 	/* Powerbook specific quirks */
@@ -5950,6 +5950,11 @@ apply_dcb_connector_quirks(struct nvbios *bios, int idx)
 	}
 }
 
+static const u8 hpd_gpio[16] = {
+	0xff, 0x07, 0x08, 0xff, 0xff, 0x51, 0x52, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0x5e, 0x5f, 0x60,
+};
+
 static void
 parse_dcb_connector_table(struct nvbios *bios)
 {
@@ -5986,23 +5991,9 @@ parse_dcb_connector_table(struct nvbios *bios)
 
 		cte->type  = (cte->entry & 0x000000ff) >> 0;
 		cte->index2 = (cte->entry & 0x00000f00) >> 8;
-		switch (cte->entry & 0x00033000) {
-		case 0x00001000:
-			cte->gpio_tag = 0x07;
-			break;
-		case 0x00002000:
-			cte->gpio_tag = 0x08;
-			break;
-		case 0x00010000:
-			cte->gpio_tag = 0x51;
-			break;
-		case 0x00020000:
-			cte->gpio_tag = 0x52;
-			break;
-		default:
-			cte->gpio_tag = 0xff;
-			break;
-		}
+
+		cte->gpio_tag = ffs((cte->entry & 0x07033000) >> 12);
+		cte->gpio_tag = hpd_gpio[cte->gpio_tag];
 
 		if (cte->type == 0xff)
 			continue;
@@ -6702,11 +6693,11 @@ nouveau_bios_run_init_table(struct drm_device *dev, uint16_t table,
 	struct nvbios *bios = &dev_priv->vbios;
 	struct init_exec iexec = { true, false };
 
-	mutex_lock(&bios->lock);
+	spin_lock_bh(&bios->lock);
 	bios->display.output = dcbent;
 	parse_init_table(bios, table, &iexec);
 	bios->display.output = NULL;
-	mutex_unlock(&bios->lock);
+	spin_unlock_bh(&bios->lock);
 }
 
 static bool NVInitVBIOS(struct drm_device *dev)
@@ -6715,7 +6706,7 @@ static bool NVInitVBIOS(struct drm_device *dev)
 	struct nvbios *bios = &dev_priv->vbios;
 
 	memset(bios, 0, sizeof(struct nvbios));
-	mutex_init(&bios->lock);
+	spin_lock_init(&bios->lock);
 	bios->dev = dev;
 
 	if (!NVShadowVBIOS(dev, bios->data))
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.h b/drivers/gpu/drm/nouveau/nouveau_bios.h
index 50a648e01c49..8a54fa7edf5c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.h
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.h
@@ -251,7 +251,7 @@ struct nvbios {
 	uint8_t digital_min_front_porch;
 	bool fp_no_ddc;
 
-	struct mutex lock;
+	spinlock_t lock;
 
 	uint8_t data[NV_PROM_SIZE];
 	unsigned int length;
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index d38a4d9f9b0b..2ad49cbf7c8b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -49,13 +49,16 @@ nouveau_bo_del_ttm(struct ttm_buffer_object *bo)
 		DRM_ERROR("bo %p still attached to GEM object\n", bo);
 
 	nv10_mem_put_tile_region(dev, nvbo->tile, NULL);
-	nouveau_vm_put(&nvbo->vma);
+	if (nvbo->vma.node) {
+		nouveau_vm_unmap(&nvbo->vma);
+		nouveau_vm_put(&nvbo->vma);
+	}
 	kfree(nvbo);
 }
 
 static void
-nouveau_bo_fixup_align(struct nouveau_bo *nvbo, int *align, int *size,
-		       int *page_shift)
+nouveau_bo_fixup_align(struct nouveau_bo *nvbo, u32 flags,
+		       int *align, int *size, int *page_shift)
 {
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(nvbo->bo.bdev);
 
@@ -80,7 +83,7 @@ nouveau_bo_fixup_align(struct nouveau_bo *nvbo, int *align, int *size,
 		}
 	} else {
 		if (likely(dev_priv->chan_vm)) {
-			if (*size > 256 * 1024)
+			if (!(flags & TTM_PL_FLAG_TT) &&  *size > 256 * 1024)
 				*page_shift = dev_priv->chan_vm->lpg_shift;
 			else
 				*page_shift = dev_priv->chan_vm->spg_shift;
@@ -98,8 +101,7 @@ nouveau_bo_fixup_align(struct nouveau_bo *nvbo, int *align, int *size,
 int
 nouveau_bo_new(struct drm_device *dev, struct nouveau_channel *chan,
 	       int size, int align, uint32_t flags, uint32_t tile_mode,
-	       uint32_t tile_flags, bool no_vm, bool mappable,
-	       struct nouveau_bo **pnvbo)
+	       uint32_t tile_flags, struct nouveau_bo **pnvbo)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_bo *nvbo;
@@ -110,16 +112,14 @@ nouveau_bo_new(struct drm_device *dev, struct nouveau_channel *chan,
 		return -ENOMEM;
 	INIT_LIST_HEAD(&nvbo->head);
 	INIT_LIST_HEAD(&nvbo->entry);
-	nvbo->mappable = mappable;
-	nvbo->no_vm = no_vm;
 	nvbo->tile_mode = tile_mode;
 	nvbo->tile_flags = tile_flags;
 	nvbo->bo.bdev = &dev_priv->ttm.bdev;
 
-	nouveau_bo_fixup_align(nvbo, &align, &size, &page_shift);
+	nouveau_bo_fixup_align(nvbo, flags, &align, &size, &page_shift);
 	align >>= PAGE_SHIFT;
 
-	if (!nvbo->no_vm && dev_priv->chan_vm) {
+	if (dev_priv->chan_vm) {
 		ret = nouveau_vm_get(dev_priv->chan_vm, size, page_shift,
 				     NV_MEM_ACCESS_RW, &nvbo->vma);
 		if (ret) {
@@ -141,11 +141,8 @@ nouveau_bo_new(struct drm_device *dev, struct nouveau_channel *chan,
 	}
 	nvbo->channel = NULL;
 
-	if (nvbo->vma.node) {
-		if (nvbo->bo.mem.mem_type == TTM_PL_VRAM)
-			nvbo->bo.offset = nvbo->vma.offset;
-	}
-
+	if (nvbo->vma.node)
+		nvbo->bo.offset = nvbo->vma.offset;
 	*pnvbo = nvbo;
 	return 0;
 }
@@ -315,11 +312,8 @@ nouveau_bo_validate(struct nouveau_bo *nvbo, bool interruptible,
 	if (ret)
 		return ret;
 
-	if (nvbo->vma.node) {
-		if (nvbo->bo.mem.mem_type == TTM_PL_VRAM)
-			nvbo->bo.offset = nvbo->vma.offset;
-	}
-
+	if (nvbo->vma.node)
+		nvbo->bo.offset = nvbo->vma.offset;
 	return 0;
 }
 
@@ -382,7 +376,8 @@ nouveau_bo_create_ttm_backend_entry(struct ttm_bo_device *bdev)
 	case NOUVEAU_GART_AGP:
 		return ttm_agp_backend_init(bdev, dev->agp->bridge);
 #endif
-	case NOUVEAU_GART_SGDMA:
+	case NOUVEAU_GART_PDMA:
+	case NOUVEAU_GART_HW:
 		return nouveau_sgdma_init_ttm(dev);
 	default:
 		NV_ERROR(dev, "Unknown GART type %d\n",
@@ -428,7 +423,10 @@ nouveau_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
 		man->default_caching = TTM_PL_FLAG_WC;
 		break;
 	case TTM_PL_TT:
-		man->func = &ttm_bo_manager_func;
+		if (dev_priv->card_type >= NV_50)
+			man->func = &nouveau_gart_manager;
+		else
+			man->func = &ttm_bo_manager_func;
 		switch (dev_priv->gart_info.type) {
 		case NOUVEAU_GART_AGP:
 			man->flags = TTM_MEMTYPE_FLAG_MAPPABLE;
@@ -436,7 +434,8 @@ nouveau_bo_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
 				TTM_PL_FLAG_WC;
 			man->default_caching = TTM_PL_FLAG_WC;
 			break;
-		case NOUVEAU_GART_SGDMA:
+		case NOUVEAU_GART_PDMA:
+		case NOUVEAU_GART_HW:
 			man->flags = TTM_MEMTYPE_FLAG_MAPPABLE |
 				     TTM_MEMTYPE_FLAG_CMA;
 			man->available_caching = TTM_PL_MASK_CACHING;
@@ -498,45 +497,22 @@ nouveau_bo_move_accel_cleanup(struct nouveau_channel *chan,
 	return ret;
 }
 
-static inline uint32_t
-nouveau_bo_mem_ctxdma(struct ttm_buffer_object *bo,
-		      struct nouveau_channel *chan, struct ttm_mem_reg *mem)
-{
-	struct nouveau_bo *nvbo = nouveau_bo(bo);
-
-	if (nvbo->no_vm) {
-		if (mem->mem_type == TTM_PL_TT)
-			return NvDmaGART;
-		return NvDmaVRAM;
-	}
-
-	if (mem->mem_type == TTM_PL_TT)
-		return chan->gart_handle;
-	return chan->vram_handle;
-}
-
 static int
 nvc0_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
 {
-	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
+	struct nouveau_mem *old_node = old_mem->mm_node;
+	struct nouveau_mem *new_node = new_mem->mm_node;
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
-	u64 src_offset = old_mem->start << PAGE_SHIFT;
-	u64 dst_offset = new_mem->start << PAGE_SHIFT;
 	u32 page_count = new_mem->num_pages;
+	u64 src_offset, dst_offset;
 	int ret;
 
-	if (!nvbo->no_vm) {
-		if (old_mem->mem_type == TTM_PL_VRAM)
-			src_offset  = nvbo->vma.offset;
-		else
-			src_offset += dev_priv->gart_info.aper_base;
-
-		if (new_mem->mem_type == TTM_PL_VRAM)
-			dst_offset  = nvbo->vma.offset;
-		else
-			dst_offset += dev_priv->gart_info.aper_base;
-	}
+	src_offset = old_node->tmp_vma.offset;
+	if (new_node->tmp_vma.node)
+		dst_offset = new_node->tmp_vma.offset;
+	else
+		dst_offset = nvbo->vma.offset;
 
 	page_count = new_mem->num_pages;
 	while (page_count) {
@@ -571,33 +547,18 @@ static int
 nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
 {
-	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
+	struct nouveau_mem *old_node = old_mem->mm_node;
+	struct nouveau_mem *new_node = new_mem->mm_node;
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
 	u64 length = (new_mem->num_pages << PAGE_SHIFT);
 	u64 src_offset, dst_offset;
 	int ret;
 
-	src_offset = old_mem->start << PAGE_SHIFT;
-	dst_offset = new_mem->start << PAGE_SHIFT;
-	if (!nvbo->no_vm) {
-		if (old_mem->mem_type == TTM_PL_VRAM)
-			src_offset  = nvbo->vma.offset;
-		else
-			src_offset += dev_priv->gart_info.aper_base;
-
-		if (new_mem->mem_type == TTM_PL_VRAM)
-			dst_offset  = nvbo->vma.offset;
-		else
-			dst_offset += dev_priv->gart_info.aper_base;
-	}
-
-	ret = RING_SPACE(chan, 3);
-	if (ret)
-		return ret;
-
-	BEGIN_RING(chan, NvSubM2MF, 0x0184, 2);
-	OUT_RING  (chan, nouveau_bo_mem_ctxdma(bo, chan, old_mem));
-	OUT_RING  (chan, nouveau_bo_mem_ctxdma(bo, chan, new_mem));
+	src_offset = old_node->tmp_vma.offset;
+	if (new_node->tmp_vma.node)
+		dst_offset = new_node->tmp_vma.offset;
+	else
+		dst_offset = nvbo->vma.offset;
 
 	while (length) {
 		u32 amount, stride, height;
@@ -678,6 +639,15 @@ nv50_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 	return 0;
 }
 
+static inline uint32_t
+nouveau_bo_mem_ctxdma(struct ttm_buffer_object *bo,
+		      struct nouveau_channel *chan, struct ttm_mem_reg *mem)
+{
+	if (mem->mem_type == TTM_PL_TT)
+		return chan->gart_handle;
+	return chan->vram_handle;
+}
+
 static int
 nv04_bo_move_m2mf(struct nouveau_channel *chan, struct ttm_buffer_object *bo,
 		  struct ttm_mem_reg *old_mem, struct ttm_mem_reg *new_mem)
@@ -731,15 +701,43 @@ nouveau_bo_move_m2mf(struct ttm_buffer_object *bo, int evict, bool intr,
 {
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
+	struct ttm_mem_reg *old_mem = &bo->mem;
 	struct nouveau_channel *chan;
 	int ret;
 
 	chan = nvbo->channel;
-	if (!chan || nvbo->no_vm) {
+	if (!chan) {
 		chan = dev_priv->channel;
 		mutex_lock_nested(&chan->mutex, NOUVEAU_KCHANNEL_MUTEX);
 	}
 
+	/* create temporary vma for old memory, this will get cleaned
+	 * up after ttm destroys the ttm_mem_reg
+	 */
+	if (dev_priv->card_type >= NV_50) {
+		struct nouveau_mem *node = old_mem->mm_node;
+		if (!node->tmp_vma.node) {
+			u32 page_shift = nvbo->vma.node->type;
+			if (old_mem->mem_type == TTM_PL_TT)
+				page_shift = nvbo->vma.vm->spg_shift;
+
+			ret = nouveau_vm_get(chan->vm,
+					     old_mem->num_pages << PAGE_SHIFT,
+					     page_shift, NV_MEM_ACCESS_RO,
+					     &node->tmp_vma);
+			if (ret)
+				goto out;
+		}
+
+		if (old_mem->mem_type == TTM_PL_VRAM)
+			nouveau_vm_map(&node->tmp_vma, node);
+		else {
+			nouveau_vm_map_sg(&node->tmp_vma, 0,
+					  old_mem->num_pages << PAGE_SHIFT,
+					  node, node->pages);
+		}
+	}
+
 	if (dev_priv->card_type < NV_50)
 		ret = nv04_bo_move_m2mf(chan, bo, &bo->mem, new_mem);
 	else
@@ -753,6 +751,7 @@ nouveau_bo_move_m2mf(struct ttm_buffer_object *bo, int evict, bool intr,
 						    no_wait_gpu, new_mem);
 	}
 
+out:
 	if (chan == dev_priv->channel)
 		mutex_unlock(&chan->mutex);
 	return ret;
@@ -763,6 +762,7 @@ nouveau_bo_move_flipd(struct ttm_buffer_object *bo, bool evict, bool intr,
 		      bool no_wait_reserve, bool no_wait_gpu,
 		      struct ttm_mem_reg *new_mem)
 {
+	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
 	u32 placement_memtype = TTM_PL_FLAG_TT | TTM_PL_MASK_CACHING;
 	struct ttm_placement placement;
 	struct ttm_mem_reg tmp_mem;
@@ -782,7 +782,23 @@ nouveau_bo_move_flipd(struct ttm_buffer_object *bo, bool evict, bool intr,
 	if (ret)
 		goto out;
 
+	if (dev_priv->card_type >= NV_50) {
+		struct nouveau_bo *nvbo = nouveau_bo(bo);
+		struct nouveau_mem *node = tmp_mem.mm_node;
+		struct nouveau_vma *vma = &nvbo->vma;
+		if (vma->node->type != vma->vm->spg_shift)
+			vma = &node->tmp_vma;
+		nouveau_vm_map_sg(vma, 0, tmp_mem.num_pages << PAGE_SHIFT,
+				  node, node->pages);
+	}
+
 	ret = nouveau_bo_move_m2mf(bo, true, intr, no_wait_reserve, no_wait_gpu, &tmp_mem);
+
+	if (dev_priv->card_type >= NV_50) {
+		struct nouveau_bo *nvbo = nouveau_bo(bo);
+		nouveau_vm_unmap(&nvbo->vma);
+	}
+
 	if (ret)
 		goto out;
 
@@ -825,6 +841,36 @@ out:
 	return ret;
 }
 
+static void
+nouveau_bo_move_ntfy(struct ttm_buffer_object *bo, struct ttm_mem_reg *new_mem)
+{
+	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
+	struct nouveau_mem *node = new_mem->mm_node;
+	struct nouveau_bo *nvbo = nouveau_bo(bo);
+	struct nouveau_vma *vma = &nvbo->vma;
+	struct nouveau_vm *vm = vma->vm;
+
+	if (dev_priv->card_type < NV_50)
+		return;
+
+	switch (new_mem->mem_type) {
+	case TTM_PL_VRAM:
+		nouveau_vm_map(vma, node);
+		break;
+	case TTM_PL_TT:
+		if (vma->node->type != vm->spg_shift) {
+			nouveau_vm_unmap(vma);
+			vma = &node->tmp_vma;
+		}
+		nouveau_vm_map_sg(vma, 0, new_mem->num_pages << PAGE_SHIFT,
+				  node, node->pages);
+		break;
+	default:
+		nouveau_vm_unmap(&nvbo->vma);
+		break;
+	}
+}
+
 static int
 nouveau_bo_vm_bind(struct ttm_buffer_object *bo, struct ttm_mem_reg *new_mem,
 		   struct nouveau_tile_reg **new_tile)
@@ -832,19 +878,13 @@ nouveau_bo_vm_bind(struct ttm_buffer_object *bo, struct ttm_mem_reg *new_mem,
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
 	struct drm_device *dev = dev_priv->dev;
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
-	uint64_t offset;
+	u64 offset = new_mem->start << PAGE_SHIFT;
 
-	if (nvbo->no_vm || new_mem->mem_type != TTM_PL_VRAM) {
-		/* Nothing to do. */
-		*new_tile = NULL;
+	*new_tile = NULL;
+	if (new_mem->mem_type != TTM_PL_VRAM)
 		return 0;
-	}
-
-	offset = new_mem->start << PAGE_SHIFT;
 
-	if (dev_priv->chan_vm) {
-		nouveau_vm_map(&nvbo->vma, new_mem->mm_node);
-	} else if (dev_priv->card_type >= NV_10) {
+	if (dev_priv->card_type >= NV_10) {
 		*new_tile = nv10_mem_set_tiling(dev, offset, new_mem->size,
 						nvbo->tile_mode,
 						nvbo->tile_flags);
@@ -861,11 +901,8 @@ nouveau_bo_vm_cleanup(struct ttm_buffer_object *bo,
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
 	struct drm_device *dev = dev_priv->dev;
 
-	if (dev_priv->card_type >= NV_10 &&
-	    dev_priv->card_type < NV_50) {
-		nv10_mem_put_tile_region(dev, *old_tile, bo->sync_obj);
-		*old_tile = new_tile;
-	}
+	nv10_mem_put_tile_region(dev, *old_tile, bo->sync_obj);
+	*old_tile = new_tile;
 }
 
 static int
@@ -879,9 +916,11 @@ nouveau_bo_move(struct ttm_buffer_object *bo, bool evict, bool intr,
 	struct nouveau_tile_reg *new_tile = NULL;
 	int ret = 0;
 
-	ret = nouveau_bo_vm_bind(bo, new_mem, &new_tile);
-	if (ret)
-		return ret;
+	if (dev_priv->card_type < NV_50) {
+		ret = nouveau_bo_vm_bind(bo, new_mem, &new_tile);
+		if (ret)
+			return ret;
+	}
 
 	/* Fake bo copy. */
 	if (old_mem->mem_type == TTM_PL_SYSTEM && !bo->ttm) {
@@ -912,10 +951,12 @@ nouveau_bo_move(struct ttm_buffer_object *bo, bool evict, bool intr,
 	ret = ttm_bo_move_memcpy(bo, evict, no_wait_reserve, no_wait_gpu, new_mem);
 
 out:
-	if (ret)
-		nouveau_bo_vm_cleanup(bo, NULL, &new_tile);
-	else
-		nouveau_bo_vm_cleanup(bo, new_tile, &nvbo->tile);
+	if (dev_priv->card_type < NV_50) {
+		if (ret)
+			nouveau_bo_vm_cleanup(bo, NULL, &new_tile);
+		else
+			nouveau_bo_vm_cleanup(bo, new_tile, &nvbo->tile);
+	}
 
 	return ret;
 }
@@ -956,7 +997,7 @@ nouveau_ttm_io_mem_reserve(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
 		break;
 	case TTM_PL_VRAM:
 	{
-		struct nouveau_vram *vram = mem->mm_node;
+		struct nouveau_mem *node = mem->mm_node;
 		u8 page_shift;
 
 		if (!dev_priv->bar1_vm) {
@@ -967,23 +1008,23 @@ nouveau_ttm_io_mem_reserve(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
 		}
 
 		if (dev_priv->card_type == NV_C0)
-			page_shift = vram->page_shift;
+			page_shift = node->page_shift;
 		else
 			page_shift = 12;
 
 		ret = nouveau_vm_get(dev_priv->bar1_vm, mem->bus.size,
 				     page_shift, NV_MEM_ACCESS_RW,
-				     &vram->bar_vma);
+				     &node->bar_vma);
 		if (ret)
 			return ret;
 
-		nouveau_vm_map(&vram->bar_vma, vram);
+		nouveau_vm_map(&node->bar_vma, node);
 		if (ret) {
-			nouveau_vm_put(&vram->bar_vma);
+			nouveau_vm_put(&node->bar_vma);
 			return ret;
 		}
 
-		mem->bus.offset = vram->bar_vma.offset;
+		mem->bus.offset = node->bar_vma.offset;
 		if (dev_priv->card_type == NV_50) /*XXX*/
 			mem->bus.offset -= 0x0020000000ULL;
 		mem->bus.base = pci_resource_start(dev->pdev, 1);
@@ -1000,16 +1041,16 @@ static void
 nouveau_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_reg *mem)
 {
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(bdev);
-	struct nouveau_vram *vram = mem->mm_node;
+	struct nouveau_mem *node = mem->mm_node;
 
 	if (!dev_priv->bar1_vm || mem->mem_type != TTM_PL_VRAM)
 		return;
 
-	if (!vram->bar_vma.node)
+	if (!node->bar_vma.node)
 		return;
 
-	nouveau_vm_unmap(&vram->bar_vma);
-	nouveau_vm_put(&vram->bar_vma);
+	nouveau_vm_unmap(&node->bar_vma);
+	nouveau_vm_put(&node->bar_vma);
 }
 
 static int
@@ -1059,6 +1100,7 @@ struct ttm_bo_driver nouveau_bo_driver = {
 	.invalidate_caches = nouveau_bo_invalidate_caches,
 	.init_mem_type = nouveau_bo_init_mem_type,
 	.evict_flags = nouveau_bo_evict_flags,
+	.move_notify = nouveau_bo_move_ntfy,
 	.move = nouveau_bo_move,
 	.verify_access = nouveau_bo_verify_access,
 	.sync_obj_signaled = __nouveau_fence_signalled,
diff --git a/drivers/gpu/drm/nouveau/nouveau_channel.c b/drivers/gpu/drm/nouveau/nouveau_channel.c
index 3960d66d7aba..3837090d66af 100644
--- a/drivers/gpu/drm/nouveau/nouveau_channel.c
+++ b/drivers/gpu/drm/nouveau/nouveau_channel.c
@@ -35,7 +35,7 @@ nouveau_channel_pushbuf_ctxdma_init(struct nouveau_channel *chan)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_bo *pb = chan->pushbuf_bo;
 	struct nouveau_gpuobj *pushbuf = NULL;
-	int ret;
+	int ret = 0;
 
 	if (dev_priv->card_type >= NV_50) {
 		if (dev_priv->card_type < NV_C0) {
@@ -90,8 +90,7 @@ nouveau_channel_user_pushbuf_alloc(struct drm_device *dev)
 	else
 		location = TTM_PL_FLAG_TT;
 
-	ret = nouveau_bo_new(dev, NULL, 65536, 0, location, 0, 0x0000, false,
-			     true, &pushbuf);
+	ret = nouveau_bo_new(dev, NULL, 65536, 0, location, 0, 0x0000, &pushbuf);
 	if (ret) {
 		NV_ERROR(dev, "error allocating DMA push buffer: %d\n", ret);
 		return NULL;
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 505c6bfb4d75..764c15d537ba 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -32,6 +32,7 @@
 #include "nouveau_hw.h"
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
+#include "nv50_display.h"
 
 static void
 nouveau_user_framebuffer_destroy(struct drm_framebuffer *drm_fb)
@@ -61,18 +62,59 @@ static const struct drm_framebuffer_funcs nouveau_framebuffer_funcs = {
 };
 
 int
-nouveau_framebuffer_init(struct drm_device *dev, struct nouveau_framebuffer *nouveau_fb,
-			 struct drm_mode_fb_cmd *mode_cmd, struct nouveau_bo *nvbo)
+nouveau_framebuffer_init(struct drm_device *dev,
+			 struct nouveau_framebuffer *nv_fb,
+			 struct drm_mode_fb_cmd *mode_cmd,
+			 struct nouveau_bo *nvbo)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct drm_framebuffer *fb = &nv_fb->base;
 	int ret;
 
-	ret = drm_framebuffer_init(dev, &nouveau_fb->base, &nouveau_framebuffer_funcs);
+	ret = drm_framebuffer_init(dev, fb, &nouveau_framebuffer_funcs);
 	if (ret) {
 		return ret;
 	}
 
-	drm_helper_mode_fill_fb_struct(&nouveau_fb->base, mode_cmd);
-	nouveau_fb->nvbo = nvbo;
+	drm_helper_mode_fill_fb_struct(fb, mode_cmd);
+	nv_fb->nvbo = nvbo;
+
+	if (dev_priv->card_type >= NV_50) {
+		u32 tile_flags = nouveau_bo_tile_layout(nvbo);
+		if (tile_flags == 0x7a00 ||
+		    tile_flags == 0xfe00)
+			nv_fb->r_dma = NvEvoFB32;
+		else
+		if (tile_flags == 0x7000)
+			nv_fb->r_dma = NvEvoFB16;
+		else
+			nv_fb->r_dma = NvEvoVRAM_LP;
+
+		switch (fb->depth) {
+		case  8: nv_fb->r_format = NV50_EVO_CRTC_FB_DEPTH_8; break;
+		case 15: nv_fb->r_format = NV50_EVO_CRTC_FB_DEPTH_15; break;
+		case 16: nv_fb->r_format = NV50_EVO_CRTC_FB_DEPTH_16; break;
+		case 24:
+		case 32: nv_fb->r_format = NV50_EVO_CRTC_FB_DEPTH_24; break;
+		case 30: nv_fb->r_format = NV50_EVO_CRTC_FB_DEPTH_30; break;
+		default:
+			 NV_ERROR(dev, "unknown depth %d\n", fb->depth);
+			 return -EINVAL;
+		}
+
+		if (dev_priv->chipset == 0x50)
+			nv_fb->r_format |= (tile_flags << 8);
+
+		if (!tile_flags)
+			nv_fb->r_pitch = 0x00100000 | fb->pitch;
+		else {
+			u32 mode = nvbo->tile_mode;
+			if (dev_priv->card_type >= NV_C0)
+				mode >>= 4;
+			nv_fb->r_pitch = ((fb->pitch / 4) << 4) | mode;
+		}
+	}
+
 	return 0;
 }
 
@@ -182,6 +224,7 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 		       struct nouveau_page_flip_state *s,
 		       struct nouveau_fence **pfence)
 {
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
 	struct drm_device *dev = chan->dev;
 	unsigned long flags;
 	int ret;
@@ -201,9 +244,12 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 	if (ret)
 		goto fail;
 
-	BEGIN_RING(chan, NvSubSw, NV_SW_PAGE_FLIP, 1);
-	OUT_RING(chan, 0);
-	FIRE_RING(chan);
+	if (dev_priv->card_type < NV_C0)
+		BEGIN_RING(chan, NvSubSw, NV_SW_PAGE_FLIP, 1);
+	else
+		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0500, 1);
+	OUT_RING  (chan, 0);
+	FIRE_RING (chan);
 
 	ret = nouveau_fence_new(chan, pfence, true);
 	if (ret)
@@ -244,7 +290,7 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 
 	/* Initialize a page flip struct */
 	*s = (struct nouveau_page_flip_state)
-		{ { }, s->event, nouveau_crtc(crtc)->index,
+		{ { }, event, nouveau_crtc(crtc)->index,
 		  fb->bits_per_pixel, fb->pitch, crtc->x, crtc->y,
 		  new_bo->bo.offset };
 
@@ -255,6 +301,14 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	mutex_lock(&chan->mutex);
 
 	/* Emit a page flip */
+	if (dev_priv->card_type >= NV_50) {
+		ret = nv50_display_flip_next(crtc, fb, chan);
+		if (ret) {
+			nouveau_channel_put(&chan);
+			goto fail_unreserve;
+		}
+	}
+
 	ret = nouveau_page_flip_emit(chan, old_bo, new_bo, s, &fence);
 	nouveau_channel_put(&chan);
 	if (ret)
@@ -305,7 +359,8 @@ nouveau_finish_page_flip(struct nouveau_channel *chan,
 	}
 
 	list_del(&s->head);
-	*ps = *s;
+	if (ps)
+		*ps = *s;
 	kfree(s);
 
 	spin_unlock_irqrestore(&dev->event_lock, flags);
diff --git a/drivers/gpu/drm/nouveau/nouveau_dma.c b/drivers/gpu/drm/nouveau/nouveau_dma.c
index 65699bfaaaea..ce38e97b9428 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dma.c
+++ b/drivers/gpu/drm/nouveau/nouveau_dma.c
@@ -83,7 +83,8 @@ nouveau_dma_init(struct nouveau_channel *chan)
 		return ret;
 
 	/* NV_MEMORY_TO_MEMORY_FORMAT requires a notifier object */
-	ret = nouveau_notifier_alloc(chan, NvNotify0, 32, &chan->m2mf_ntfy);
+	ret = nouveau_notifier_alloc(chan, NvNotify0, 32, 0xfd0, 0x1000,
+				     &chan->m2mf_ntfy);
 	if (ret)
 		return ret;
 
@@ -96,13 +97,15 @@ nouveau_dma_init(struct nouveau_channel *chan)
 		OUT_RING(chan, 0);
 
 	/* Initialise NV_MEMORY_TO_MEMORY_FORMAT */
-	ret = RING_SPACE(chan, 4);
+	ret = RING_SPACE(chan, 6);
 	if (ret)
 		return ret;
 	BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_NAME, 1);
-	OUT_RING(chan, NvM2MF);
-	BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_DMA_NOTIFY, 1);
-	OUT_RING(chan, NvNotify0);
+	OUT_RING  (chan, NvM2MF);
+	BEGIN_RING(chan, NvSubM2MF, NV_MEMORY_TO_MEMORY_FORMAT_DMA_NOTIFY, 3);
+	OUT_RING  (chan, NvNotify0);
+	OUT_RING  (chan, chan->vram_handle);
+	OUT_RING  (chan, chan->gart_handle);
 
 	/* Sit back and pray the channel works.. */
 	FIRE_RING(chan);
diff --git a/drivers/gpu/drm/nouveau/nouveau_dma.h b/drivers/gpu/drm/nouveau/nouveau_dma.h
index c36f1763feaa..23d4edf992b7 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dma.h
+++ b/drivers/gpu/drm/nouveau/nouveau_dma.h
@@ -61,8 +61,6 @@ enum {
 	NvM2MF		= 0x80000001,
 	NvDmaFB		= 0x80000002,
 	NvDmaTT		= 0x80000003,
-	NvDmaVRAM	= 0x80000004,
-	NvDmaGART	= 0x80000005,
 	NvNotify0       = 0x80000006,
 	Nv2D		= 0x80000007,
 	NvCtxSurf2D	= 0x80000008,
@@ -73,12 +71,15 @@ enum {
 	NvImageBlit	= 0x8000000d,
 	NvSw		= 0x8000000e,
 	NvSema		= 0x8000000f,
+	NvEvoSema0	= 0x80000010,
+	NvEvoSema1	= 0x80000011,
 
 	/* G80+ display objects */
 	NvEvoVRAM	= 0x01000000,
 	NvEvoFB16	= 0x01000001,
 	NvEvoFB32	= 0x01000002,
-	NvEvoVRAM_LP	= 0x01000003
+	NvEvoVRAM_LP	= 0x01000003,
+	NvEvoSync	= 0xcafe0000
 };
 
 #define NV_MEMORY_TO_MEMORY_FORMAT                                    0x00000039
diff --git a/drivers/gpu/drm/nouveau/nouveau_dp.c b/drivers/gpu/drm/nouveau/nouveau_dp.c
index 38d599554bce..7beb82a0315d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dp.c
+++ b/drivers/gpu/drm/nouveau/nouveau_dp.c
@@ -175,7 +175,6 @@ nouveau_dp_link_train_adjust(struct drm_encoder *encoder, uint8_t *config)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct drm_device *dev = encoder->dev;
-	struct bit_displayport_encoder_table_entry *dpse;
 	struct bit_displayport_encoder_table *dpe;
 	int ret, i, dpe_headerlen, vs = 0, pre = 0;
 	uint8_t request[2];
@@ -183,7 +182,6 @@ nouveau_dp_link_train_adjust(struct drm_encoder *encoder, uint8_t *config)
 	dpe = nouveau_bios_dp_table(dev, nv_encoder->dcb, &dpe_headerlen);
 	if (!dpe)
 		return false;
-	dpse = (void *)((char *)dpe + dpe_headerlen);
 
 	ret = auxch_rd(encoder, DP_ADJUST_REQUEST_LANE0_1, request, 2);
 	if (ret)
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.c b/drivers/gpu/drm/nouveau/nouveau_drv.c
index f658a04eecf9..155ebdcbf06f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.c
@@ -408,14 +408,6 @@ static struct drm_driver driver = {
 #endif
 		.llseek = noop_llseek,
 	},
-	.pci_driver = {
-		.name = DRIVER_NAME,
-		.id_table = pciidlist,
-		.probe = nouveau_pci_probe,
-		.remove = nouveau_pci_remove,
-		.suspend = nouveau_pci_suspend,
-		.resume = nouveau_pci_resume
-	},
 
 	.gem_init_object = nouveau_gem_object_new,
 	.gem_free_object = nouveau_gem_object_del,
@@ -432,6 +424,15 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver nouveau_pci_driver = {
+		.name = DRIVER_NAME,
+		.id_table = pciidlist,
+		.probe = nouveau_pci_probe,
+		.remove = nouveau_pci_remove,
+		.suspend = nouveau_pci_suspend,
+		.resume = nouveau_pci_resume
+};
+
 static int __init nouveau_init(void)
 {
 	driver.num_ioctls = nouveau_max_ioctl;
@@ -449,7 +450,7 @@ static int __init nouveau_init(void)
 		return 0;
 
 	nouveau_register_dsm_handler();
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &nouveau_pci_driver);
 }
 
 static void __exit nouveau_exit(void)
@@ -457,7 +458,7 @@ static void __exit nouveau_exit(void)
 	if (!nouveau_modeset)
 		return;
 
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &nouveau_pci_driver);
 	nouveau_unregister_dsm_handler();
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.h b/drivers/gpu/drm/nouveau/nouveau_drv.h
index 9821fcacc3d2..b6143266341e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.h
@@ -57,7 +57,7 @@ struct nouveau_fpriv {
 #include "nouveau_util.h"
 
 struct nouveau_grctx;
-struct nouveau_vram;
+struct nouveau_mem;
 #include "nouveau_vm.h"
 
 #define MAX_NUM_DCB_ENTRIES 16
@@ -65,13 +65,16 @@ struct nouveau_vram;
 #define NOUVEAU_MAX_CHANNEL_NR 128
 #define NOUVEAU_MAX_TILE_NR 15
 
-struct nouveau_vram {
+struct nouveau_mem {
 	struct drm_device *dev;
 
 	struct nouveau_vma bar_vma;
+	struct nouveau_vma tmp_vma;
 	u8  page_shift;
 
+	struct drm_mm_node *tag;
 	struct list_head regions;
+	dma_addr_t *pages;
 	u32 memtype;
 	u64 offset;
 	u64 size;
@@ -90,6 +93,7 @@ struct nouveau_tile_reg {
 struct nouveau_bo {
 	struct ttm_buffer_object bo;
 	struct ttm_placement placement;
+	u32 valid_domains;
 	u32 placements[3];
 	u32 busy_placements[3];
 	struct ttm_bo_kmap_obj kmap;
@@ -104,8 +108,6 @@ struct nouveau_bo {
 	struct nouveau_channel *channel;
 
 	struct nouveau_vma vma;
-	bool mappable;
-	bool no_vm;
 
 	uint32_t tile_mode;
 	uint32_t tile_flags;
@@ -387,6 +389,7 @@ struct nouveau_pgraph_engine {
 };
 
 struct nouveau_display_engine {
+	void *priv;
 	int (*early_init)(struct drm_device *);
 	void (*late_takedown)(struct drm_device *);
 	int (*create)(struct drm_device *);
@@ -509,8 +512,8 @@ struct nouveau_crypt_engine {
 struct nouveau_vram_engine {
 	int  (*init)(struct drm_device *);
 	int  (*get)(struct drm_device *, u64, u32 align, u32 size_nc,
-		    u32 type, struct nouveau_vram **);
-	void (*put)(struct drm_device *, struct nouveau_vram **);
+		    u32 type, struct nouveau_mem **);
+	void (*put)(struct drm_device *, struct nouveau_mem **);
 
 	bool (*flags_valid)(struct drm_device *, u32 tile_flags);
 };
@@ -652,8 +655,6 @@ struct drm_nouveau_private {
 	/* interrupt handling */
 	void (*irq_handler[32])(struct drm_device *);
 	bool msi_enabled;
-	struct workqueue_struct *wq;
-	struct work_struct irq_work;
 
 	struct list_head vbl_waiting;
 
@@ -691,15 +692,22 @@ struct drm_nouveau_private {
 	struct {
 		enum {
 			NOUVEAU_GART_NONE = 0,
-			NOUVEAU_GART_AGP,
-			NOUVEAU_GART_SGDMA
+			NOUVEAU_GART_AGP,	/* AGP */
+			NOUVEAU_GART_PDMA,	/* paged dma object */
+			NOUVEAU_GART_HW		/* on-chip gart/vm */
 		} type;
 		uint64_t aper_base;
 		uint64_t aper_size;
 		uint64_t aper_free;
 
+		struct ttm_backend_func *func;
+
+		struct {
+			struct page *page;
+			dma_addr_t   addr;
+		} dummy;
+
 		struct nouveau_gpuobj *sg_ctxdma;
-		struct nouveau_vma vma;
 	} gart_info;
 
 	/* nv10-nv40 tiling regions */
@@ -740,14 +748,6 @@ struct drm_nouveau_private {
 
 	struct backlight_device *backlight;
 
-	struct nouveau_channel *evo;
-	u32 evo_alloc;
-	struct {
-		struct dcb_entry *dcb;
-		u16 script;
-		u32 pclk;
-	} evo_irq;
-
 	struct {
 		struct dentry *channel_root;
 	} debugfs;
@@ -847,12 +847,14 @@ extern void nv10_mem_put_tile_region(struct drm_device *dev,
 				     struct nouveau_tile_reg *tile,
 				     struct nouveau_fence *fence);
 extern const struct ttm_mem_type_manager_func nouveau_vram_manager;
+extern const struct ttm_mem_type_manager_func nouveau_gart_manager;
 
 /* nouveau_notifier.c */
 extern int  nouveau_notifier_init_channel(struct nouveau_channel *);
 extern void nouveau_notifier_takedown_channel(struct nouveau_channel *);
 extern int  nouveau_notifier_alloc(struct nouveau_channel *, uint32_t handle,
-				   int cout, uint32_t *offset);
+				   int cout, uint32_t start, uint32_t end,
+				   uint32_t *offset);
 extern int  nouveau_notifier_offset(struct nouveau_gpuobj *, uint32_t *);
 extern int  nouveau_ioctl_notifier_alloc(struct drm_device *, void *data,
 					 struct drm_file *);
@@ -1294,7 +1296,7 @@ extern struct ttm_bo_driver nouveau_bo_driver;
 extern int nouveau_bo_new(struct drm_device *, struct nouveau_channel *,
 			  int size, int align, uint32_t flags,
 			  uint32_t tile_mode, uint32_t tile_flags,
-			  bool no_vm, bool mappable, struct nouveau_bo **);
+			  struct nouveau_bo **);
 extern int nouveau_bo_pin(struct nouveau_bo *, uint32_t flags);
 extern int nouveau_bo_unpin(struct nouveau_bo *);
 extern int nouveau_bo_map(struct nouveau_bo *);
@@ -1355,9 +1357,9 @@ static inline struct nouveau_fence *nouveau_fence_ref(struct nouveau_fence *obj)
 
 /* nouveau_gem.c */
 extern int nouveau_gem_new(struct drm_device *, struct nouveau_channel *,
-			   int size, int align, uint32_t flags,
+			   int size, int align, uint32_t domain,
 			   uint32_t tile_mode, uint32_t tile_flags,
-			   bool no_vm, bool mappable, struct nouveau_bo **);
+			   struct nouveau_bo **);
 extern int nouveau_gem_object_new(struct drm_gem_object *);
 extern void nouveau_gem_object_del(struct drm_gem_object *);
 extern int nouveau_gem_ioctl_new(struct drm_device *, void *,
diff --git a/drivers/gpu/drm/nouveau/nouveau_fb.h b/drivers/gpu/drm/nouveau/nouveau_fb.h
index d432134b71e0..a3a88ad00f86 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fb.h
+++ b/drivers/gpu/drm/nouveau/nouveau_fb.h
@@ -30,6 +30,9 @@
 struct nouveau_framebuffer {
 	struct drm_framebuffer base;
 	struct nouveau_bo *nvbo;
+	u32 r_dma;
+	u32 r_format;
+	u32 r_pitch;
 };
 
 static inline struct nouveau_framebuffer *
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 60769d2f9a66..889c4454682e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -296,8 +296,8 @@ nouveau_fbcon_create(struct nouveau_fbdev *nfbdev,
 	size = mode_cmd.pitch * mode_cmd.height;
 	size = roundup(size, PAGE_SIZE);
 
-	ret = nouveau_gem_new(dev, dev_priv->channel, size, 0, TTM_PL_FLAG_VRAM,
-			      0, 0x0000, false, true, &nvbo);
+	ret = nouveau_gem_new(dev, dev_priv->channel, size, 0,
+			      NOUVEAU_GEM_DOMAIN_VRAM, 0, 0x0000, &nvbo);
 	if (ret) {
 		NV_ERROR(dev, "failed to allocate framebuffer\n");
 		goto out;
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.c b/drivers/gpu/drm/nouveau/nouveau_fence.c
index 221b8462ea37..a244702bb227 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.c
@@ -32,8 +32,7 @@
 #include "nouveau_dma.h"
 
 #define USE_REFCNT(dev) (nouveau_private(dev)->chipset >= 0x10)
-#define USE_SEMA(dev) (nouveau_private(dev)->chipset >= 0x17 && \
-		       nouveau_private(dev)->card_type < NV_C0)
+#define USE_SEMA(dev) (nouveau_private(dev)->chipset >= 0x17)
 
 struct nouveau_fence {
 	struct nouveau_channel *channel;
@@ -259,11 +258,12 @@ __nouveau_fence_wait(void *sync_obj, void *sync_arg, bool lazy, bool intr)
 }
 
 static struct nouveau_semaphore *
-alloc_semaphore(struct drm_device *dev)
+semaphore_alloc(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_semaphore *sema;
-	int ret;
+	int size = (dev_priv->chipset < 0x84) ? 4 : 16;
+	int ret, i;
 
 	if (!USE_SEMA(dev))
 		return NULL;
@@ -277,9 +277,9 @@ alloc_semaphore(struct drm_device *dev)
 		goto fail;
 
 	spin_lock(&dev_priv->fence.lock);
-	sema->mem = drm_mm_search_free(&dev_priv->fence.heap, 4, 0, 0);
+	sema->mem = drm_mm_search_free(&dev_priv->fence.heap, size, 0, 0);
 	if (sema->mem)
-		sema->mem = drm_mm_get_block_atomic(sema->mem, 4, 0);
+		sema->mem = drm_mm_get_block_atomic(sema->mem, size, 0);
 	spin_unlock(&dev_priv->fence.lock);
 
 	if (!sema->mem)
@@ -287,7 +287,8 @@ alloc_semaphore(struct drm_device *dev)
 
 	kref_init(&sema->ref);
 	sema->dev = dev;
-	nouveau_bo_wr32(dev_priv->fence.bo, sema->mem->start / 4, 0);
+	for (i = sema->mem->start; i < sema->mem->start + size; i += 4)
+		nouveau_bo_wr32(dev_priv->fence.bo, i / 4, 0);
 
 	return sema;
 fail:
@@ -296,7 +297,7 @@ fail:
 }
 
 static void
-free_semaphore(struct kref *ref)
+semaphore_free(struct kref *ref)
 {
 	struct nouveau_semaphore *sema =
 		container_of(ref, struct nouveau_semaphore, ref);
@@ -318,61 +319,107 @@ semaphore_work(void *priv, bool signalled)
 	if (unlikely(!signalled))
 		nouveau_bo_wr32(dev_priv->fence.bo, sema->mem->start / 4, 1);
 
-	kref_put(&sema->ref, free_semaphore);
+	kref_put(&sema->ref, semaphore_free);
 }
 
 static int
-emit_semaphore(struct nouveau_channel *chan, int method,
-	       struct nouveau_semaphore *sema)
+semaphore_acquire(struct nouveau_channel *chan, struct nouveau_semaphore *sema)
 {
-	struct drm_nouveau_private *dev_priv = sema->dev->dev_private;
-	struct nouveau_fence *fence;
-	bool smart = (dev_priv->card_type >= NV_50);
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
+	struct nouveau_fence *fence = NULL;
 	int ret;
 
-	ret = RING_SPACE(chan, smart ? 8 : 4);
+	if (dev_priv->chipset < 0x84) {
+		ret = RING_SPACE(chan, 3);
+		if (ret)
+			return ret;
+
+		BEGIN_RING(chan, NvSubSw, NV_SW_SEMAPHORE_OFFSET, 2);
+		OUT_RING  (chan, sema->mem->start);
+		OUT_RING  (chan, 1);
+	} else
+	if (dev_priv->chipset < 0xc0) {
+		struct nouveau_vma *vma = &dev_priv->fence.bo->vma;
+		u64 offset = vma->offset + sema->mem->start;
+
+		ret = RING_SPACE(chan, 5);
+		if (ret)
+			return ret;
+
+		BEGIN_RING(chan, NvSubSw, 0x0010, 4);
+		OUT_RING  (chan, upper_32_bits(offset));
+		OUT_RING  (chan, lower_32_bits(offset));
+		OUT_RING  (chan, 1);
+		OUT_RING  (chan, 1); /* ACQUIRE_EQ */
+	} else {
+		struct nouveau_vma *vma = &dev_priv->fence.bo->vma;
+		u64 offset = vma->offset + sema->mem->start;
+
+		ret = RING_SPACE(chan, 5);
+		if (ret)
+			return ret;
+
+		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0010, 4);
+		OUT_RING  (chan, upper_32_bits(offset));
+		OUT_RING  (chan, lower_32_bits(offset));
+		OUT_RING  (chan, 1);
+		OUT_RING  (chan, 0x1001); /* ACQUIRE_EQ */
+	}
+
+	/* Delay semaphore destruction until its work is done */
+	ret = nouveau_fence_new(chan, &fence, true);
 	if (ret)
 		return ret;
 
-	if (smart) {
-		BEGIN_RING(chan, NvSubSw, NV_SW_DMA_SEMAPHORE, 1);
-		OUT_RING(chan, NvSema);
-	}
-	BEGIN_RING(chan, NvSubSw, NV_SW_SEMAPHORE_OFFSET, 1);
-	OUT_RING(chan, sema->mem->start);
-
-	if (smart && method == NV_SW_SEMAPHORE_ACQUIRE) {
-		/*
-		 * NV50 tries to be too smart and context-switch
-		 * between semaphores instead of doing a "first come,
-		 * first served" strategy like previous cards
-		 * do.
-		 *
-		 * That's bad because the ACQUIRE latency can get as
-		 * large as the PFIFO context time slice in the
-		 * typical DRI2 case where you have several
-		 * outstanding semaphores at the same moment.
-		 *
-		 * If we're going to ACQUIRE, force the card to
-		 * context switch before, just in case the matching
-		 * RELEASE is already scheduled to be executed in
-		 * another channel.
-		 */
-		BEGIN_RING(chan, NvSubSw, NV_SW_YIELD, 1);
-		OUT_RING(chan, 0);
-	}
+	kref_get(&sema->ref);
+	nouveau_fence_work(fence, semaphore_work, sema);
+	nouveau_fence_unref(&fence);
+	return 0;
+}
+
+static int
+semaphore_release(struct nouveau_channel *chan, struct nouveau_semaphore *sema)
+{
+	struct drm_nouveau_private *dev_priv = chan->dev->dev_private;
+	struct nouveau_fence *fence = NULL;
+	int ret;
+
+	if (dev_priv->chipset < 0x84) {
+		ret = RING_SPACE(chan, 4);
+		if (ret)
+			return ret;
+
+		BEGIN_RING(chan, NvSubSw, NV_SW_SEMAPHORE_OFFSET, 1);
+		OUT_RING  (chan, sema->mem->start);
+		BEGIN_RING(chan, NvSubSw, NV_SW_SEMAPHORE_RELEASE, 1);
+		OUT_RING  (chan, 1);
+	} else
+	if (dev_priv->chipset < 0xc0) {
+		struct nouveau_vma *vma = &dev_priv->fence.bo->vma;
+		u64 offset = vma->offset + sema->mem->start;
 
-	BEGIN_RING(chan, NvSubSw, method, 1);
-	OUT_RING(chan, 1);
-
-	if (smart && method == NV_SW_SEMAPHORE_RELEASE) {
-		/*
-		 * Force the card to context switch, there may be
-		 * another channel waiting for the semaphore we just
-		 * released.
-		 */
-		BEGIN_RING(chan, NvSubSw, NV_SW_YIELD, 1);
-		OUT_RING(chan, 0);
+		ret = RING_SPACE(chan, 5);
+		if (ret)
+			return ret;
+
+		BEGIN_RING(chan, NvSubSw, 0x0010, 4);
+		OUT_RING  (chan, upper_32_bits(offset));
+		OUT_RING  (chan, lower_32_bits(offset));
+		OUT_RING  (chan, 1);
+		OUT_RING  (chan, 2); /* RELEASE */
+	} else {
+		struct nouveau_vma *vma = &dev_priv->fence.bo->vma;
+		u64 offset = vma->offset + sema->mem->start;
+
+		ret = RING_SPACE(chan, 5);
+		if (ret)
+			return ret;
+
+		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0010, 4);
+		OUT_RING  (chan, upper_32_bits(offset));
+		OUT_RING  (chan, lower_32_bits(offset));
+		OUT_RING  (chan, 1);
+		OUT_RING  (chan, 0x1002); /* RELEASE */
 	}
 
 	/* Delay semaphore destruction until its work is done */
@@ -383,7 +430,6 @@ emit_semaphore(struct nouveau_channel *chan, int method,
 	kref_get(&sema->ref);
 	nouveau_fence_work(fence, semaphore_work, sema);
 	nouveau_fence_unref(&fence);
-
 	return 0;
 }
 
@@ -400,7 +446,7 @@ nouveau_fence_sync(struct nouveau_fence *fence,
 		   nouveau_fence_signalled(fence)))
 		goto out;
 
-	sema = alloc_semaphore(dev);
+	sema = semaphore_alloc(dev);
 	if (!sema) {
 		/* Early card or broken userspace, fall back to
 		 * software sync. */
@@ -418,17 +464,17 @@ nouveau_fence_sync(struct nouveau_fence *fence,
 	}
 
 	/* Make wchan wait until it gets signalled */
-	ret = emit_semaphore(wchan, NV_SW_SEMAPHORE_ACQUIRE, sema);
+	ret = semaphore_acquire(wchan, sema);
 	if (ret)
 		goto out_unlock;
 
 	/* Signal the semaphore from chan */
-	ret = emit_semaphore(chan, NV_SW_SEMAPHORE_RELEASE, sema);
+	ret = semaphore_release(chan, sema);
 
 out_unlock:
 	mutex_unlock(&chan->mutex);
 out_unref:
-	kref_put(&sema->ref, free_semaphore);
+	kref_put(&sema->ref, semaphore_free);
 out:
 	if (chan)
 		nouveau_channel_put_unlocked(&chan);
@@ -449,22 +495,23 @@ nouveau_fence_channel_init(struct nouveau_channel *chan)
 	struct nouveau_gpuobj *obj = NULL;
 	int ret;
 
+	if (dev_priv->card_type >= NV_C0)
+		goto out_initialised;
+
 	/* Create an NV_SW object for various sync purposes */
 	ret = nouveau_gpuobj_gr_new(chan, NvSw, NV_SW);
 	if (ret)
 		return ret;
 
 	/* we leave subchannel empty for nvc0 */
-	if (dev_priv->card_type < NV_C0) {
-		ret = RING_SPACE(chan, 2);
-		if (ret)
-			return ret;
-		BEGIN_RING(chan, NvSubSw, 0, 1);
-		OUT_RING(chan, NvSw);
-	}
+	ret = RING_SPACE(chan, 2);
+	if (ret)
+		return ret;
+	BEGIN_RING(chan, NvSubSw, 0, 1);
+	OUT_RING(chan, NvSw);
 
 	/* Create a DMA object for the shared cross-channel sync area. */
-	if (USE_SEMA(dev)) {
+	if (USE_SEMA(dev) && dev_priv->chipset < 0x84) {
 		struct ttm_mem_reg *mem = &dev_priv->fence.bo->bo.mem;
 
 		ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_IN_MEMORY,
@@ -484,14 +531,20 @@ nouveau_fence_channel_init(struct nouveau_channel *chan)
 			return ret;
 		BEGIN_RING(chan, NvSubSw, NV_SW_DMA_SEMAPHORE, 1);
 		OUT_RING(chan, NvSema);
+	} else {
+		ret = RING_SPACE(chan, 2);
+		if (ret)
+			return ret;
+		BEGIN_RING(chan, NvSubSw, NV_SW_DMA_SEMAPHORE, 1);
+		OUT_RING  (chan, chan->vram_handle); /* whole VM */
 	}
 
 	FIRE_RING(chan);
 
+out_initialised:
 	INIT_LIST_HEAD(&chan->fence.pending);
 	spin_lock_init(&chan->fence.lock);
 	atomic_set(&chan->fence.last_sequence_irq, 0);
-
 	return 0;
 }
 
@@ -519,12 +572,13 @@ int
 nouveau_fence_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	int size = (dev_priv->chipset < 0x84) ? 4096 : 16384;
 	int ret;
 
 	/* Create a shared VRAM heap for cross-channel sync. */
 	if (USE_SEMA(dev)) {
-		ret = nouveau_bo_new(dev, NULL, 4096, 0, TTM_PL_FLAG_VRAM,
-				     0, 0, false, true, &dev_priv->fence.bo);
+		ret = nouveau_bo_new(dev, NULL, size, 0, TTM_PL_FLAG_VRAM,
+				     0, 0, &dev_priv->fence.bo);
 		if (ret)
 			return ret;
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index 506c508b7eda..3ce58d2222cb 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -61,19 +61,36 @@ nouveau_gem_object_del(struct drm_gem_object *gem)
 
 int
 nouveau_gem_new(struct drm_device *dev, struct nouveau_channel *chan,
-		int size, int align, uint32_t flags, uint32_t tile_mode,
-		uint32_t tile_flags, bool no_vm, bool mappable,
-		struct nouveau_bo **pnvbo)
+		int size, int align, uint32_t domain, uint32_t tile_mode,
+		uint32_t tile_flags, struct nouveau_bo **pnvbo)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_bo *nvbo;
+	u32 flags = 0;
 	int ret;
 
+	if (domain & NOUVEAU_GEM_DOMAIN_VRAM)
+		flags |= TTM_PL_FLAG_VRAM;
+	if (domain & NOUVEAU_GEM_DOMAIN_GART)
+		flags |= TTM_PL_FLAG_TT;
+	if (!flags || domain & NOUVEAU_GEM_DOMAIN_CPU)
+		flags |= TTM_PL_FLAG_SYSTEM;
+
 	ret = nouveau_bo_new(dev, chan, size, align, flags, tile_mode,
-			     tile_flags, no_vm, mappable, pnvbo);
+			     tile_flags, pnvbo);
 	if (ret)
 		return ret;
 	nvbo = *pnvbo;
 
+	/* we restrict allowed domains on nv50+ to only the types
+	 * that were requested at creation time.  not possibly on
+	 * earlier chips without busting the ABI.
+	 */
+	nvbo->valid_domains = NOUVEAU_GEM_DOMAIN_VRAM |
+			      NOUVEAU_GEM_DOMAIN_GART;
+	if (dev_priv->card_type >= NV_50)
+		nvbo->valid_domains &= domain;
+
 	nvbo->gem = drm_gem_object_alloc(dev, nvbo->bo.mem.size);
 	if (!nvbo->gem) {
 		nouveau_bo_ref(NULL, pnvbo);
@@ -97,7 +114,7 @@ nouveau_gem_info(struct drm_gem_object *gem, struct drm_nouveau_gem_info *rep)
 
 	rep->size = nvbo->bo.mem.num_pages << PAGE_SHIFT;
 	rep->offset = nvbo->bo.offset;
-	rep->map_handle = nvbo->mappable ? nvbo->bo.addr_space_offset : 0;
+	rep->map_handle = nvbo->bo.addr_space_offset;
 	rep->tile_mode = nvbo->tile_mode;
 	rep->tile_flags = nvbo->tile_flags;
 	return 0;
@@ -111,19 +128,11 @@ nouveau_gem_ioctl_new(struct drm_device *dev, void *data,
 	struct drm_nouveau_gem_new *req = data;
 	struct nouveau_bo *nvbo = NULL;
 	struct nouveau_channel *chan = NULL;
-	uint32_t flags = 0;
 	int ret = 0;
 
 	if (unlikely(dev_priv->ttm.bdev.dev_mapping == NULL))
 		dev_priv->ttm.bdev.dev_mapping = dev_priv->dev->dev_mapping;
 
-	if (req->info.domain & NOUVEAU_GEM_DOMAIN_VRAM)
-		flags |= TTM_PL_FLAG_VRAM;
-	if (req->info.domain & NOUVEAU_GEM_DOMAIN_GART)
-		flags |= TTM_PL_FLAG_TT;
-	if (!flags || req->info.domain & NOUVEAU_GEM_DOMAIN_CPU)
-		flags |= TTM_PL_FLAG_SYSTEM;
-
 	if (!dev_priv->engine.vram.flags_valid(dev, req->info.tile_flags)) {
 		NV_ERROR(dev, "bad page flags: 0x%08x\n", req->info.tile_flags);
 		return -EINVAL;
@@ -135,10 +144,9 @@ nouveau_gem_ioctl_new(struct drm_device *dev, void *data,
 			return PTR_ERR(chan);
 	}
 
-	ret = nouveau_gem_new(dev, chan, req->info.size, req->align, flags,
-			      req->info.tile_mode, req->info.tile_flags, false,
-			      (req->info.domain & NOUVEAU_GEM_DOMAIN_MAPPABLE),
-			      &nvbo);
+	ret = nouveau_gem_new(dev, chan, req->info.size, req->align,
+			      req->info.domain, req->info.tile_mode,
+			      req->info.tile_flags, &nvbo);
 	if (chan)
 		nouveau_channel_put(&chan);
 	if (ret)
@@ -161,7 +169,7 @@ nouveau_gem_set_domain(struct drm_gem_object *gem, uint32_t read_domains,
 {
 	struct nouveau_bo *nvbo = gem->driver_private;
 	struct ttm_buffer_object *bo = &nvbo->bo;
-	uint32_t domains = valid_domains &
+	uint32_t domains = valid_domains & nvbo->valid_domains &
 		(write_domains ? write_domains : read_domains);
 	uint32_t pref_flags = 0, valid_flags = 0;
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_mem.c b/drivers/gpu/drm/nouveau/nouveau_mem.c
index 26347b7cd872..e9cd7258b22d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_mem.c
@@ -152,7 +152,6 @@ nouveau_mem_vram_fini(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 
-	nouveau_bo_unpin(dev_priv->vga_ram);
 	nouveau_bo_ref(NULL, &dev_priv->vga_ram);
 
 	ttm_bo_device_release(&dev_priv->ttm.bdev);
@@ -393,11 +392,17 @@ nouveau_mem_vram_init(struct drm_device *dev)
 	struct ttm_bo_device *bdev = &dev_priv->ttm.bdev;
 	int ret, dma_bits;
 
-	if (dev_priv->card_type >= NV_50 &&
-	    pci_dma_supported(dev->pdev, DMA_BIT_MASK(40)))
-		dma_bits = 40;
-	else
-		dma_bits = 32;
+	dma_bits = 32;
+	if (dev_priv->card_type >= NV_50) {
+		if (pci_dma_supported(dev->pdev, DMA_BIT_MASK(40)))
+			dma_bits = 40;
+	} else
+	if (drm_pci_device_is_pcie(dev) &&
+	    dev_priv->chipset != 0x40 &&
+	    dev_priv->chipset != 0x45) {
+		if (pci_dma_supported(dev->pdev, DMA_BIT_MASK(39)))
+			dma_bits = 39;
+	}
 
 	ret = pci_set_dma_mask(dev->pdev, DMA_BIT_MASK(dma_bits));
 	if (ret)
@@ -455,13 +460,17 @@ nouveau_mem_vram_init(struct drm_device *dev)
 		return ret;
 	}
 
-	ret = nouveau_bo_new(dev, NULL, 256*1024, 0, TTM_PL_FLAG_VRAM,
-			     0, 0, true, true, &dev_priv->vga_ram);
-	if (ret == 0)
-		ret = nouveau_bo_pin(dev_priv->vga_ram, TTM_PL_FLAG_VRAM);
-	if (ret) {
-		NV_WARN(dev, "failed to reserve VGA memory\n");
-		nouveau_bo_ref(NULL, &dev_priv->vga_ram);
+	if (dev_priv->card_type < NV_50) {
+		ret = nouveau_bo_new(dev, NULL, 256*1024, 0, TTM_PL_FLAG_VRAM,
+				     0, 0, &dev_priv->vga_ram);
+		if (ret == 0)
+			ret = nouveau_bo_pin(dev_priv->vga_ram,
+					     TTM_PL_FLAG_VRAM);
+
+		if (ret) {
+			NV_WARN(dev, "failed to reserve VGA memory\n");
+			nouveau_bo_ref(NULL, &dev_priv->vga_ram);
+		}
 	}
 
 	dev_priv->fb_mtrr = drm_mtrr_add(pci_resource_start(dev->pdev, 1),
@@ -480,7 +489,7 @@ nouveau_mem_gart_init(struct drm_device *dev)
 	dev_priv->gart_info.type = NOUVEAU_GART_NONE;
 
 #if !defined(__powerpc__) && !defined(__ia64__)
-	if (drm_device_is_agp(dev) && dev->agp && nouveau_agpmode) {
+	if (drm_pci_device_is_agp(dev) && dev->agp && nouveau_agpmode) {
 		ret = nouveau_mem_init_agp(dev);
 		if (ret)
 			NV_ERROR(dev, "Error initialising AGP: %d\n", ret);
@@ -666,13 +675,14 @@ nouveau_vram_manager_init(struct ttm_mem_type_manager *man, unsigned long p_size
 {
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(man->bdev);
 	struct nouveau_mm *mm;
-	u32 b_size;
+	u64 size, block, rsvd;
 	int ret;
 
-	p_size = (p_size << PAGE_SHIFT) >> 12;
-	b_size = dev_priv->vram_rblock_size >> 12;
+	rsvd  = (256 * 1024); /* vga memory */
+	size  = (p_size << PAGE_SHIFT) - rsvd;
+	block = dev_priv->vram_rblock_size;
 
-	ret = nouveau_mm_init(&mm, 0, p_size, b_size);
+	ret = nouveau_mm_init(&mm, rsvd >> 12, size >> 12, block >> 12);
 	if (ret)
 		return ret;
 
@@ -700,9 +710,15 @@ nouveau_vram_manager_del(struct ttm_mem_type_manager *man,
 {
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(man->bdev);
 	struct nouveau_vram_engine *vram = &dev_priv->engine.vram;
+	struct nouveau_mem *node = mem->mm_node;
 	struct drm_device *dev = dev_priv->dev;
 
-	vram->put(dev, (struct nouveau_vram **)&mem->mm_node);
+	if (node->tmp_vma.node) {
+		nouveau_vm_unmap(&node->tmp_vma);
+		nouveau_vm_put(&node->tmp_vma);
+	}
+
+	vram->put(dev, (struct nouveau_mem **)&mem->mm_node);
 }
 
 static int
@@ -715,7 +731,7 @@ nouveau_vram_manager_new(struct ttm_mem_type_manager *man,
 	struct nouveau_vram_engine *vram = &dev_priv->engine.vram;
 	struct drm_device *dev = dev_priv->dev;
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
-	struct nouveau_vram *node;
+	struct nouveau_mem *node;
 	u32 size_nc = 0;
 	int ret;
 
@@ -724,9 +740,11 @@ nouveau_vram_manager_new(struct ttm_mem_type_manager *man,
 
 	ret = vram->get(dev, mem->num_pages << PAGE_SHIFT,
 			mem->page_alignment << PAGE_SHIFT, size_nc,
-			(nvbo->tile_flags >> 8) & 0xff, &node);
-	if (ret)
-		return ret;
+			(nvbo->tile_flags >> 8) & 0x3ff, &node);
+	if (ret) {
+		mem->mm_node = NULL;
+		return (ret == -ENOSPC) ? 0 : ret;
+	}
 
 	node->page_shift = 12;
 	if (nvbo->vma.node)
@@ -769,3 +787,84 @@ const struct ttm_mem_type_manager_func nouveau_vram_manager = {
 	nouveau_vram_manager_del,
 	nouveau_vram_manager_debug
 };
+
+static int
+nouveau_gart_manager_init(struct ttm_mem_type_manager *man, unsigned long psize)
+{
+	return 0;
+}
+
+static int
+nouveau_gart_manager_fini(struct ttm_mem_type_manager *man)
+{
+	return 0;
+}
+
+static void
+nouveau_gart_manager_del(struct ttm_mem_type_manager *man,
+			 struct ttm_mem_reg *mem)
+{
+	struct nouveau_mem *node = mem->mm_node;
+
+	if (node->tmp_vma.node) {
+		nouveau_vm_unmap(&node->tmp_vma);
+		nouveau_vm_put(&node->tmp_vma);
+	}
+	mem->mm_node = NULL;
+}
+
+static int
+nouveau_gart_manager_new(struct ttm_mem_type_manager *man,
+			 struct ttm_buffer_object *bo,
+			 struct ttm_placement *placement,
+			 struct ttm_mem_reg *mem)
+{
+	struct drm_nouveau_private *dev_priv = nouveau_bdev(bo->bdev);
+	struct nouveau_bo *nvbo = nouveau_bo(bo);
+	struct nouveau_vma *vma = &nvbo->vma;
+	struct nouveau_vm *vm = vma->vm;
+	struct nouveau_mem *node;
+	int ret;
+
+	if (unlikely((mem->num_pages << PAGE_SHIFT) >=
+		     dev_priv->gart_info.aper_size))
+		return -ENOMEM;
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return -ENOMEM;
+
+	/* This node must be for evicting large-paged VRAM
+	 * to system memory.  Due to a nv50 limitation of
+	 * not being able to mix large/small pages within
+	 * the same PDE, we need to create a temporary
+	 * small-paged VMA for the eviction.
+	 */
+	if (vma->node->type != vm->spg_shift) {
+		ret = nouveau_vm_get(vm, (u64)vma->node->length << 12,
+				     vm->spg_shift, NV_MEM_ACCESS_RW,
+				     &node->tmp_vma);
+		if (ret) {
+			kfree(node);
+			return ret;
+		}
+	}
+
+	node->page_shift = nvbo->vma.node->type;
+	mem->mm_node = node;
+	mem->start   = 0;
+	return 0;
+}
+
+void
+nouveau_gart_manager_debug(struct ttm_mem_type_manager *man, const char *prefix)
+{
+}
+
+const struct ttm_mem_type_manager_func nouveau_gart_manager = {
+	nouveau_gart_manager_init,
+	nouveau_gart_manager_fini,
+	nouveau_gart_manager_new,
+	nouveau_gart_manager_del,
+	nouveau_gart_manager_debug
+};
diff --git a/drivers/gpu/drm/nouveau/nouveau_mm.c b/drivers/gpu/drm/nouveau/nouveau_mm.c
index 8844b50c3e54..7609756b6faf 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_mm.c
@@ -123,7 +123,7 @@ nouveau_mm_get(struct nouveau_mm *rmm, int type, u32 size, u32 size_nc,
 		return 0;
 	}
 
-	return -ENOMEM;
+	return -ENOSPC;
 }
 
 int
diff --git a/drivers/gpu/drm/nouveau/nouveau_mm.h b/drivers/gpu/drm/nouveau/nouveau_mm.h
index 798eaf39691c..1f7483aae9a4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_mm.h
@@ -53,13 +53,13 @@ void nouveau_mm_put(struct nouveau_mm *, struct nouveau_mm_node *);
 
 int  nv50_vram_init(struct drm_device *);
 int  nv50_vram_new(struct drm_device *, u64 size, u32 align, u32 size_nc,
-		    u32 memtype, struct nouveau_vram **);
-void nv50_vram_del(struct drm_device *, struct nouveau_vram **);
+		    u32 memtype, struct nouveau_mem **);
+void nv50_vram_del(struct drm_device *, struct nouveau_mem **);
 bool nv50_vram_flags_valid(struct drm_device *, u32 tile_flags);
 
 int  nvc0_vram_init(struct drm_device *);
 int  nvc0_vram_new(struct drm_device *, u64 size, u32 align, u32 ncmin,
-		    u32 memtype, struct nouveau_vram **);
+		    u32 memtype, struct nouveau_mem **);
 bool nvc0_vram_flags_valid(struct drm_device *, u32 tile_flags);
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_notifier.c b/drivers/gpu/drm/nouveau/nouveau_notifier.c
index fe29d604b820..7ba3fc0b30c1 100644
--- a/drivers/gpu/drm/nouveau/nouveau_notifier.c
+++ b/drivers/gpu/drm/nouveau/nouveau_notifier.c
@@ -39,12 +39,11 @@ nouveau_notifier_init_channel(struct nouveau_channel *chan)
 	int ret;
 
 	if (nouveau_vram_notify)
-		flags = TTM_PL_FLAG_VRAM;
+		flags = NOUVEAU_GEM_DOMAIN_VRAM;
 	else
-		flags = TTM_PL_FLAG_TT;
+		flags = NOUVEAU_GEM_DOMAIN_GART;
 
-	ret = nouveau_gem_new(dev, NULL, PAGE_SIZE, 0, flags,
-			      0, 0x0000, false, true, &ntfy);
+	ret = nouveau_gem_new(dev, NULL, PAGE_SIZE, 0, flags, 0, 0, &ntfy);
 	if (ret)
 		return ret;
 
@@ -96,27 +95,35 @@ nouveau_notifier_gpuobj_dtor(struct drm_device *dev,
 
 int
 nouveau_notifier_alloc(struct nouveau_channel *chan, uint32_t handle,
-		       int size, uint32_t *b_offset)
+		       int size, uint32_t start, uint32_t end,
+		       uint32_t *b_offset)
 {
 	struct drm_device *dev = chan->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *nobj = NULL;
 	struct drm_mm_node *mem;
 	uint32_t offset;
 	int target, ret;
 
-	mem = drm_mm_search_free(&chan->notifier_heap, size, 0, 0);
+	mem = drm_mm_search_free_in_range(&chan->notifier_heap, size, 0,
+					  start, end, 0);
 	if (mem)
-		mem = drm_mm_get_block(mem, size, 0);
+		mem = drm_mm_get_block_range(mem, size, 0, start, end);
 	if (!mem) {
 		NV_ERROR(dev, "Channel %d notifier block full\n", chan->id);
 		return -ENOMEM;
 	}
 
-	if (chan->notifier_bo->bo.mem.mem_type == TTM_PL_VRAM)
-		target = NV_MEM_TARGET_VRAM;
-	else
-		target = NV_MEM_TARGET_GART;
-	offset  = chan->notifier_bo->bo.mem.start << PAGE_SHIFT;
+	if (dev_priv->card_type < NV_50) {
+		if (chan->notifier_bo->bo.mem.mem_type == TTM_PL_VRAM)
+			target = NV_MEM_TARGET_VRAM;
+		else
+			target = NV_MEM_TARGET_GART;
+		offset  = chan->notifier_bo->bo.mem.start << PAGE_SHIFT;
+	} else {
+		target = NV_MEM_TARGET_VM;
+		offset = chan->notifier_bo->vma.offset;
+	}
 	offset += mem->start;
 
 	ret = nouveau_gpuobj_dma_new(chan, NV_CLASS_DMA_IN_MEMORY, offset,
@@ -177,7 +184,8 @@ nouveau_ioctl_notifier_alloc(struct drm_device *dev, void *data,
 	if (IS_ERR(chan))
 		return PTR_ERR(chan);
 
-	ret = nouveau_notifier_alloc(chan, na->handle, na->size, &na->offset);
+	ret = nouveau_notifier_alloc(chan, na->handle, na->size, 0, 0x1000,
+				     &na->offset);
 	nouveau_channel_put(&chan);
 	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_object.c b/drivers/gpu/drm/nouveau/nouveau_object.c
index 30b6544467ca..4f00c87ed86e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_object.c
+++ b/drivers/gpu/drm/nouveau/nouveau_object.c
@@ -36,6 +36,7 @@
 #include "nouveau_drm.h"
 #include "nouveau_ramht.h"
 #include "nouveau_vm.h"
+#include "nv50_display.h"
 
 struct nouveau_gpuobj_method {
 	struct list_head head;
@@ -490,16 +491,22 @@ nouveau_gpuobj_dma_new(struct nouveau_channel *chan, int class, u64 base,
 	}
 
 	if (target == NV_MEM_TARGET_GART) {
-		if (dev_priv->gart_info.type == NOUVEAU_GART_AGP) {
-			target = NV_MEM_TARGET_PCI_NOSNOOP;
-			base  += dev_priv->gart_info.aper_base;
-		} else
-		if (base != 0) {
-			base = nouveau_sgdma_get_physical(dev, base);
+		struct nouveau_gpuobj *gart = dev_priv->gart_info.sg_ctxdma;
+
+		if (dev_priv->gart_info.type == NOUVEAU_GART_PDMA) {
+			if (base == 0) {
+				nouveau_gpuobj_ref(gart, pobj);
+				return 0;
+			}
+
+			base   = nouveau_sgdma_get_physical(dev, base);
 			target = NV_MEM_TARGET_PCI;
 		} else {
-			nouveau_gpuobj_ref(dev_priv->gart_info.sg_ctxdma, pobj);
-			return 0;
+			base += dev_priv->gart_info.aper_base;
+			if (dev_priv->gart_info.type == NOUVEAU_GART_AGP)
+				target = NV_MEM_TARGET_PCI_NOSNOOP;
+			else
+				target = NV_MEM_TARGET_PCI;
 		}
 	}
 
@@ -776,7 +783,7 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *vram = NULL, *tt = NULL;
-	int ret;
+	int ret, i;
 
 	NV_DEBUG(dev, "ch%d vram=0x%08x tt=0x%08x\n", chan->id, vram_h, tt_h);
 
@@ -841,6 +848,25 @@ nouveau_gpuobj_channel_init(struct nouveau_channel *chan,
 		nouveau_gpuobj_ref(NULL, &ramht);
 		if (ret)
 			return ret;
+
+		/* dma objects for display sync channel semaphore blocks */
+		for (i = 0; i < 2; i++) {
+			struct nouveau_gpuobj *sem = NULL;
+			struct nv50_display_crtc *dispc =
+				&nv50_display(dev)->crtc[i];
+			u64 offset = dispc->sem.bo->bo.mem.start << PAGE_SHIFT;
+
+			ret = nouveau_gpuobj_dma_new(chan, 0x3d, offset, 0xfff,
+						     NV_MEM_ACCESS_RW,
+						     NV_MEM_TARGET_VRAM, &sem);
+			if (ret)
+				return ret;
+
+			ret = nouveau_ramht_insert(chan, NvEvoSema0 + i, sem);
+			nouveau_gpuobj_ref(NULL, &sem);
+			if (ret)
+				return ret;
+		}
 	}
 
 	/* VRAM ctxdma */
@@ -909,7 +935,7 @@ nouveau_gpuobj_channel_takedown(struct nouveau_channel *chan)
 	nouveau_vm_ref(NULL, &chan->vm, chan->vm_pd);
 	nouveau_gpuobj_ref(NULL, &chan->vm_pd);
 
-	if (chan->ramin_heap.free_stack.next)
+	if (drm_mm_initialized(&chan->ramin_heap))
 		drm_mm_takedown(&chan->ramin_heap);
 	nouveau_gpuobj_ref(NULL, &chan->ramin);
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_ramht.c b/drivers/gpu/drm/nouveau/nouveau_ramht.c
index bef3e6910418..a24a81f5a89e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_ramht.c
+++ b/drivers/gpu/drm/nouveau/nouveau_ramht.c
@@ -114,7 +114,9 @@ nouveau_ramht_insert(struct nouveau_channel *chan, u32 handle,
 		      (gpuobj->engine << NV40_RAMHT_CONTEXT_ENGINE_SHIFT);
 	} else {
 		if (gpuobj->engine == NVOBJ_ENGINE_DISPLAY) {
-			ctx = (gpuobj->cinst << 10) | chan->id;
+			ctx = (gpuobj->cinst << 10) |
+			      (chan->id << 28) |
+			      chan->id; /* HASH_TAG */
 		} else {
 			ctx = (gpuobj->cinst >> 4) |
 			      ((gpuobj->engine <<
diff --git a/drivers/gpu/drm/nouveau/nouveau_sgdma.c b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
index 9a250eb53098..1205f0f345b9 100644
--- a/drivers/gpu/drm/nouveau/nouveau_sgdma.c
+++ b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
@@ -12,6 +12,7 @@ struct nouveau_sgdma_be {
 	struct drm_device *dev;
 
 	dma_addr_t *pages;
+	bool *ttm_alloced;
 	unsigned nr_pages;
 
 	u64 offset;
@@ -20,7 +21,8 @@ struct nouveau_sgdma_be {
 
 static int
 nouveau_sgdma_populate(struct ttm_backend *be, unsigned long num_pages,
-		       struct page **pages, struct page *dummy_read_page)
+		       struct page **pages, struct page *dummy_read_page,
+		       dma_addr_t *dma_addrs)
 {
 	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
 	struct drm_device *dev = nvbe->dev;
@@ -34,15 +36,25 @@ nouveau_sgdma_populate(struct ttm_backend *be, unsigned long num_pages,
 	if (!nvbe->pages)
 		return -ENOMEM;
 
+	nvbe->ttm_alloced = kmalloc(sizeof(bool) * num_pages, GFP_KERNEL);
+	if (!nvbe->ttm_alloced)
+		return -ENOMEM;
+
 	nvbe->nr_pages = 0;
 	while (num_pages--) {
-		nvbe->pages[nvbe->nr_pages] =
-			pci_map_page(dev->pdev, pages[nvbe->nr_pages], 0,
+		if (dma_addrs[nvbe->nr_pages] != DMA_ERROR_CODE) {
+			nvbe->pages[nvbe->nr_pages] =
+					dma_addrs[nvbe->nr_pages];
+		 	nvbe->ttm_alloced[nvbe->nr_pages] = true;
+		} else {
+			nvbe->pages[nvbe->nr_pages] =
+				pci_map_page(dev->pdev, pages[nvbe->nr_pages], 0,
 				     PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-		if (pci_dma_mapping_error(dev->pdev,
-					  nvbe->pages[nvbe->nr_pages])) {
-			be->func->clear(be);
-			return -EFAULT;
+			if (pci_dma_mapping_error(dev->pdev,
+						  nvbe->pages[nvbe->nr_pages])) {
+				be->func->clear(be);
+				return -EFAULT;
+			}
 		}
 
 		nvbe->nr_pages++;
@@ -65,17 +77,36 @@ nouveau_sgdma_clear(struct ttm_backend *be)
 			be->func->unbind(be);
 
 		while (nvbe->nr_pages--) {
-			pci_unmap_page(dev->pdev, nvbe->pages[nvbe->nr_pages],
+			if (!nvbe->ttm_alloced[nvbe->nr_pages])
+				pci_unmap_page(dev->pdev, nvbe->pages[nvbe->nr_pages],
 				       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
 		}
 		kfree(nvbe->pages);
+		kfree(nvbe->ttm_alloced);
 		nvbe->pages = NULL;
+		nvbe->ttm_alloced = NULL;
 		nvbe->nr_pages = 0;
 	}
 }
 
+static void
+nouveau_sgdma_destroy(struct ttm_backend *be)
+{
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+
+	if (be) {
+		NV_DEBUG(nvbe->dev, "\n");
+
+		if (nvbe) {
+			if (nvbe->pages)
+				be->func->clear(be);
+			kfree(nvbe);
+		}
+	}
+}
+
 static int
-nouveau_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+nv04_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 {
 	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
 	struct drm_device *dev = nvbe->dev;
@@ -102,7 +133,7 @@ nouveau_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 }
 
 static int
-nouveau_sgdma_unbind(struct ttm_backend *be)
+nv04_sgdma_unbind(struct ttm_backend *be)
 {
 	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
 	struct drm_device *dev = nvbe->dev;
@@ -125,59 +156,245 @@ nouveau_sgdma_unbind(struct ttm_backend *be)
 	return 0;
 }
 
+static struct ttm_backend_func nv04_sgdma_backend = {
+	.populate		= nouveau_sgdma_populate,
+	.clear			= nouveau_sgdma_clear,
+	.bind			= nv04_sgdma_bind,
+	.unbind			= nv04_sgdma_unbind,
+	.destroy		= nouveau_sgdma_destroy
+};
+
 static void
-nouveau_sgdma_destroy(struct ttm_backend *be)
+nv41_sgdma_flush(struct nouveau_sgdma_be *nvbe)
+{
+	struct drm_device *dev = nvbe->dev;
+
+	nv_wr32(dev, 0x100810, 0x00000022);
+	if (!nv_wait(dev, 0x100810, 0x00000100, 0x00000100))
+		NV_ERROR(dev, "vm flush timeout: 0x%08x\n",
+			 nv_rd32(dev, 0x100810));
+	nv_wr32(dev, 0x100810, 0x00000000);
+}
+
+static int
+nv41_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 {
 	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
+	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
+	dma_addr_t *list = nvbe->pages;
+	u32 pte = mem->start << 2;
+	u32 cnt = nvbe->nr_pages;
 
-	if (be) {
-		NV_DEBUG(nvbe->dev, "\n");
+	nvbe->offset = mem->start << PAGE_SHIFT;
 
-		if (nvbe) {
-			if (nvbe->pages)
-				be->func->clear(be);
-			kfree(nvbe);
+	while (cnt--) {
+		nv_wo32(pgt, pte, (*list++ >> 7) | 1);
+		pte += 4;
+	}
+
+	nv41_sgdma_flush(nvbe);
+	nvbe->bound = true;
+	return 0;
+}
+
+static int
+nv41_sgdma_unbind(struct ttm_backend *be)
+{
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
+	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
+	u32 pte = (nvbe->offset >> 12) << 2;
+	u32 cnt = nvbe->nr_pages;
+
+	while (cnt--) {
+		nv_wo32(pgt, pte, 0x00000000);
+		pte += 4;
+	}
+
+	nv41_sgdma_flush(nvbe);
+	nvbe->bound = false;
+	return 0;
+}
+
+static struct ttm_backend_func nv41_sgdma_backend = {
+	.populate		= nouveau_sgdma_populate,
+	.clear			= nouveau_sgdma_clear,
+	.bind			= nv41_sgdma_bind,
+	.unbind			= nv41_sgdma_unbind,
+	.destroy		= nouveau_sgdma_destroy
+};
+
+static void
+nv44_sgdma_flush(struct nouveau_sgdma_be *nvbe)
+{
+	struct drm_device *dev = nvbe->dev;
+
+	nv_wr32(dev, 0x100814, (nvbe->nr_pages - 1) << 12);
+	nv_wr32(dev, 0x100808, nvbe->offset | 0x20);
+	if (!nv_wait(dev, 0x100808, 0x00000001, 0x00000001))
+		NV_ERROR(dev, "gart flush timeout: 0x%08x\n",
+			 nv_rd32(dev, 0x100808));
+	nv_wr32(dev, 0x100808, 0x00000000);
+}
+
+static void
+nv44_sgdma_fill(struct nouveau_gpuobj *pgt, dma_addr_t *list, u32 base, u32 cnt)
+{
+	struct drm_nouveau_private *dev_priv = pgt->dev->dev_private;
+	dma_addr_t dummy = dev_priv->gart_info.dummy.addr;
+	u32 pte, tmp[4];
+
+	pte   = base >> 2;
+	base &= ~0x0000000f;
+
+	tmp[0] = nv_ro32(pgt, base + 0x0);
+	tmp[1] = nv_ro32(pgt, base + 0x4);
+	tmp[2] = nv_ro32(pgt, base + 0x8);
+	tmp[3] = nv_ro32(pgt, base + 0xc);
+	while (cnt--) {
+		u32 addr = list ? (*list++ >> 12) : (dummy >> 12);
+		switch (pte++ & 0x3) {
+		case 0:
+			tmp[0] &= ~0x07ffffff;
+			tmp[0] |= addr;
+			break;
+		case 1:
+			tmp[0] &= ~0xf8000000;
+			tmp[0] |= addr << 27;
+			tmp[1] &= ~0x003fffff;
+			tmp[1] |= addr >> 5;
+			break;
+		case 2:
+			tmp[1] &= ~0xffc00000;
+			tmp[1] |= addr << 22;
+			tmp[2] &= ~0x0001ffff;
+			tmp[2] |= addr >> 10;
+			break;
+		case 3:
+			tmp[2] &= ~0xfffe0000;
+			tmp[2] |= addr << 17;
+			tmp[3] &= ~0x00000fff;
+			tmp[3] |= addr >> 15;
+			break;
 		}
 	}
+
+	tmp[3] |= 0x40000000;
+
+	nv_wo32(pgt, base + 0x0, tmp[0]);
+	nv_wo32(pgt, base + 0x4, tmp[1]);
+	nv_wo32(pgt, base + 0x8, tmp[2]);
+	nv_wo32(pgt, base + 0xc, tmp[3]);
 }
 
 static int
-nv50_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+nv44_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 {
 	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
 	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
+	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
+	dma_addr_t *list = nvbe->pages;
+	u32 pte = mem->start << 2, tmp[4];
+	u32 cnt = nvbe->nr_pages;
+	int i;
 
 	nvbe->offset = mem->start << PAGE_SHIFT;
 
-	nouveau_vm_map_sg(&dev_priv->gart_info.vma, nvbe->offset,
-			  nvbe->nr_pages << PAGE_SHIFT, nvbe->pages);
+	if (pte & 0x0000000c) {
+		u32  max = 4 - ((pte >> 2) & 0x3);
+		u32 part = (cnt > max) ? max : cnt;
+		nv44_sgdma_fill(pgt, list, pte, part);
+		pte  += (part << 2);
+		list += part;
+		cnt  -= part;
+	}
+
+	while (cnt >= 4) {
+		for (i = 0; i < 4; i++)
+			tmp[i] = *list++ >> 12;
+		nv_wo32(pgt, pte + 0x0, tmp[0] >>  0 | tmp[1] << 27);
+		nv_wo32(pgt, pte + 0x4, tmp[1] >>  5 | tmp[2] << 22);
+		nv_wo32(pgt, pte + 0x8, tmp[2] >> 10 | tmp[3] << 17);
+		nv_wo32(pgt, pte + 0xc, tmp[3] >> 15 | 0x40000000);
+		pte  += 0x10;
+		cnt  -= 4;
+	}
+
+	if (cnt)
+		nv44_sgdma_fill(pgt, list, pte, cnt);
+
+	nv44_sgdma_flush(nvbe);
 	nvbe->bound = true;
 	return 0;
 }
 
 static int
-nv50_sgdma_unbind(struct ttm_backend *be)
+nv44_sgdma_unbind(struct ttm_backend *be)
 {
 	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
 	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
+	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
+	u32 pte = (nvbe->offset >> 12) << 2;
+	u32 cnt = nvbe->nr_pages;
+
+	if (pte & 0x0000000c) {
+		u32  max = 4 - ((pte >> 2) & 0x3);
+		u32 part = (cnt > max) ? max : cnt;
+		nv44_sgdma_fill(pgt, NULL, pte, part);
+		pte  += (part << 2);
+		cnt  -= part;
+	}
 
-	if (!nvbe->bound)
-		return 0;
+	while (cnt >= 4) {
+		nv_wo32(pgt, pte + 0x0, 0x00000000);
+		nv_wo32(pgt, pte + 0x4, 0x00000000);
+		nv_wo32(pgt, pte + 0x8, 0x00000000);
+		nv_wo32(pgt, pte + 0xc, 0x00000000);
+		pte  += 0x10;
+		cnt  -= 4;
+	}
+
+	if (cnt)
+		nv44_sgdma_fill(pgt, NULL, pte, cnt);
 
-	nouveau_vm_unmap_at(&dev_priv->gart_info.vma, nvbe->offset,
-			    nvbe->nr_pages << PAGE_SHIFT);
+	nv44_sgdma_flush(nvbe);
 	nvbe->bound = false;
 	return 0;
 }
 
-static struct ttm_backend_func nouveau_sgdma_backend = {
+static struct ttm_backend_func nv44_sgdma_backend = {
 	.populate		= nouveau_sgdma_populate,
 	.clear			= nouveau_sgdma_clear,
-	.bind			= nouveau_sgdma_bind,
-	.unbind			= nouveau_sgdma_unbind,
+	.bind			= nv44_sgdma_bind,
+	.unbind			= nv44_sgdma_unbind,
 	.destroy		= nouveau_sgdma_destroy
 };
 
+static int
+nv50_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+{
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_mem *node = mem->mm_node;
+	/* noop: bound in move_notify() */
+	node->pages = nvbe->pages;
+	nvbe->pages = (dma_addr_t *)node;
+	nvbe->bound = true;
+	return 0;
+}
+
+static int
+nv50_sgdma_unbind(struct ttm_backend *be)
+{
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_mem *node = (struct nouveau_mem *)nvbe->pages;
+	/* noop: unbound in move_notify() */
+	nvbe->pages = node->pages;
+	node->pages = NULL;
+	nvbe->bound = false;
+	return 0;
+}
+
 static struct ttm_backend_func nv50_sgdma_backend = {
 	.populate		= nouveau_sgdma_populate,
 	.clear			= nouveau_sgdma_clear,
@@ -198,10 +415,7 @@ nouveau_sgdma_init_ttm(struct drm_device *dev)
 
 	nvbe->dev = dev;
 
-	if (dev_priv->card_type < NV_50)
-		nvbe->backend.func = &nouveau_sgdma_backend;
-	else
-		nvbe->backend.func = &nv50_sgdma_backend;
+	nvbe->backend.func = dev_priv->gart_info.func;
 	return &nvbe->backend;
 }
 
@@ -210,21 +424,65 @@ nouveau_sgdma_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *gpuobj = NULL;
-	uint32_t aper_size, obj_size;
-	int i, ret;
+	u32 aper_size, align;
+	int ret;
 
-	if (dev_priv->card_type < NV_50) {
-		if(dev_priv->ramin_rsvd_vram < 2 * 1024 * 1024)
-			aper_size = 64 * 1024 * 1024;
-		else
-			aper_size = 512 * 1024 * 1024;
+	if (dev_priv->card_type >= NV_50 ||
+	    dev_priv->ramin_rsvd_vram >= 2 * 1024 * 1024)
+		aper_size = 512 * 1024 * 1024;
+	else
+		aper_size = 64 * 1024 * 1024;
+
+	/* Dear NVIDIA, NV44+ would like proper present bits in PTEs for
+	 * christmas.  The cards before it have them, the cards after
+	 * it have them, why is NV44 so unloved?
+	 */
+	dev_priv->gart_info.dummy.page = alloc_page(GFP_DMA32 | GFP_KERNEL);
+	if (!dev_priv->gart_info.dummy.page)
+		return -ENOMEM;
+
+	dev_priv->gart_info.dummy.addr =
+		pci_map_page(dev->pdev, dev_priv->gart_info.dummy.page,
+			     0, PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+	if (pci_dma_mapping_error(dev->pdev, dev_priv->gart_info.dummy.addr)) {
+		NV_ERROR(dev, "error mapping dummy page\n");
+		__free_page(dev_priv->gart_info.dummy.page);
+		dev_priv->gart_info.dummy.page = NULL;
+		return -ENOMEM;
+	}
+
+	if (dev_priv->card_type >= NV_50) {
+		dev_priv->gart_info.aper_base = 0;
+		dev_priv->gart_info.aper_size = aper_size;
+		dev_priv->gart_info.type = NOUVEAU_GART_HW;
+		dev_priv->gart_info.func = &nv50_sgdma_backend;
+	} else
+	if (drm_pci_device_is_pcie(dev) &&
+	    dev_priv->chipset != 0x40 && dev_priv->chipset != 0x45) {
+		if (nv44_graph_class(dev)) {
+			dev_priv->gart_info.func = &nv44_sgdma_backend;
+			align = 512 * 1024;
+		} else {
+			dev_priv->gart_info.func = &nv41_sgdma_backend;
+			align = 16;
+		}
 
-		obj_size  = (aper_size >> NV_CTXDMA_PAGE_SHIFT) * 4;
-		obj_size += 8; /* ctxdma header */
+		ret = nouveau_gpuobj_new(dev, NULL, aper_size / 1024, align,
+					 NVOBJ_FLAG_ZERO_ALLOC |
+					 NVOBJ_FLAG_ZERO_FREE, &gpuobj);
+		if (ret) {
+			NV_ERROR(dev, "Error creating sgdma object: %d\n", ret);
+			return ret;
+		}
 
-		ret = nouveau_gpuobj_new(dev, NULL, obj_size, 16,
-					      NVOBJ_FLAG_ZERO_ALLOC |
-					      NVOBJ_FLAG_ZERO_FREE, &gpuobj);
+		dev_priv->gart_info.sg_ctxdma = gpuobj;
+		dev_priv->gart_info.aper_base = 0;
+		dev_priv->gart_info.aper_size = aper_size;
+		dev_priv->gart_info.type = NOUVEAU_GART_HW;
+	} else {
+		ret = nouveau_gpuobj_new(dev, NULL, (aper_size / 1024) + 8, 16,
+					 NVOBJ_FLAG_ZERO_ALLOC |
+					 NVOBJ_FLAG_ZERO_FREE, &gpuobj);
 		if (ret) {
 			NV_ERROR(dev, "Error creating sgdma object: %d\n", ret);
 			return ret;
@@ -236,25 +494,14 @@ nouveau_sgdma_init(struct drm_device *dev)
 				   (0 << 14) /* RW */ |
 				   (2 << 16) /* PCI */);
 		nv_wo32(gpuobj, 4, aper_size - 1);
-		for (i = 2; i < 2 + (aper_size >> 12); i++)
-			nv_wo32(gpuobj, i * 4, 0x00000000);
 
 		dev_priv->gart_info.sg_ctxdma = gpuobj;
 		dev_priv->gart_info.aper_base = 0;
 		dev_priv->gart_info.aper_size = aper_size;
-	} else
-	if (dev_priv->chan_vm) {
-		ret = nouveau_vm_get(dev_priv->chan_vm, 512 * 1024 * 1024,
-				     12, NV_MEM_ACCESS_RW,
-				     &dev_priv->gart_info.vma);
-		if (ret)
-			return ret;
-
-		dev_priv->gart_info.aper_base = dev_priv->gart_info.vma.offset;
-		dev_priv->gart_info.aper_size = 512 * 1024 * 1024;
+		dev_priv->gart_info.type = NOUVEAU_GART_PDMA;
+		dev_priv->gart_info.func = &nv04_sgdma_backend;
 	}
 
-	dev_priv->gart_info.type      = NOUVEAU_GART_SGDMA;
 	return 0;
 }
 
@@ -264,7 +511,13 @@ nouveau_sgdma_takedown(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 
 	nouveau_gpuobj_ref(NULL, &dev_priv->gart_info.sg_ctxdma);
-	nouveau_vm_put(&dev_priv->gart_info.vma);
+
+	if (dev_priv->gart_info.dummy.page) {
+		pci_unmap_page(dev->pdev, dev_priv->gart_info.dummy.addr,
+			       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+		__free_page(dev_priv->gart_info.dummy.page);
+		dev_priv->gart_info.dummy.page = NULL;
+	}
 }
 
 uint32_t
diff --git a/drivers/gpu/drm/nouveau/nouveau_state.c b/drivers/gpu/drm/nouveau/nouveau_state.c
index a54fc431fe98..05294910e135 100644
--- a/drivers/gpu/drm/nouveau/nouveau_state.c
+++ b/drivers/gpu/drm/nouveau/nouveau_state.c
@@ -544,7 +544,6 @@ static int
 nouveau_card_init_channel(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpuobj *gpuobj = NULL;
 	int ret;
 
 	ret = nouveau_channel_alloc(dev, &dev_priv->channel,
@@ -552,41 +551,8 @@ nouveau_card_init_channel(struct drm_device *dev)
 	if (ret)
 		return ret;
 
-	/* no dma objects on fermi... */
-	if (dev_priv->card_type >= NV_C0)
-		goto out_done;
-
-	ret = nouveau_gpuobj_dma_new(dev_priv->channel, NV_CLASS_DMA_IN_MEMORY,
-				     0, dev_priv->vram_size,
-				     NV_MEM_ACCESS_RW, NV_MEM_TARGET_VRAM,
-				     &gpuobj);
-	if (ret)
-		goto out_err;
-
-	ret = nouveau_ramht_insert(dev_priv->channel, NvDmaVRAM, gpuobj);
-	nouveau_gpuobj_ref(NULL, &gpuobj);
-	if (ret)
-		goto out_err;
-
-	ret = nouveau_gpuobj_dma_new(dev_priv->channel, NV_CLASS_DMA_IN_MEMORY,
-				     0, dev_priv->gart_info.aper_size,
-				     NV_MEM_ACCESS_RW, NV_MEM_TARGET_GART,
-				     &gpuobj);
-	if (ret)
-		goto out_err;
-
-	ret = nouveau_ramht_insert(dev_priv->channel, NvDmaGART, gpuobj);
-	nouveau_gpuobj_ref(NULL, &gpuobj);
-	if (ret)
-		goto out_err;
-
-out_done:
 	mutex_unlock(&dev_priv->channel->mutex);
 	return 0;
-
-out_err:
-	nouveau_channel_put(&dev_priv->channel);
-	return ret;
 }
 
 static void nouveau_switcheroo_set_state(struct pci_dev *pdev,
@@ -929,12 +895,6 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 	NV_DEBUG(dev, "vendor: 0x%X device: 0x%X class: 0x%X\n",
 		 dev->pci_vendor, dev->pci_device, dev->pdev->class);
 
-	dev_priv->wq = create_workqueue("nouveau");
-	if (!dev_priv->wq) {
-		ret = -EINVAL;
-		goto err_priv;
-	}
-
 	/* resource 0 is mmio regs */
 	/* resource 1 is linear FB */
 	/* resource 2 is RAMIN (mmio regs + 0x1000000) */
@@ -947,7 +907,7 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 		NV_ERROR(dev, "Unable to initialize the mmio mapping. "
 			 "Please report your setup to " DRIVER_EMAIL "\n");
 		ret = -EINVAL;
-		goto err_wq;
+		goto err_priv;
 	}
 	NV_DEBUG(dev, "regs mapped ok at 0x%llx\n",
 					(unsigned long long)mmio_start_offs);
@@ -1054,8 +1014,6 @@ err_ramin:
 	iounmap(dev_priv->ramin);
 err_mmio:
 	iounmap(dev_priv->mmio);
-err_wq:
-	destroy_workqueue(dev_priv->wq);
 err_priv:
 	kfree(dev_priv);
 	dev->dev_private = NULL;
@@ -1103,9 +1061,9 @@ int nouveau_ioctl_getparam(struct drm_device *dev, void *data,
 		getparam->value = dev->pci_device;
 		break;
 	case NOUVEAU_GETPARAM_BUS_TYPE:
-		if (drm_device_is_agp(dev))
+		if (drm_pci_device_is_agp(dev))
 			getparam->value = NV_AGP;
-		else if (drm_device_is_pcie(dev))
+		else if (drm_pci_device_is_pcie(dev))
 			getparam->value = NV_PCIE;
 		else
 			getparam->value = NV_PCI;
@@ -1126,7 +1084,7 @@ int nouveau_ioctl_getparam(struct drm_device *dev, void *data,
 		getparam->value = 1;
 		break;
 	case NOUVEAU_GETPARAM_HAS_PAGEFLIP:
-		getparam->value = (dev_priv->card_type < NV_50);
+		getparam->value = 1;
 		break;
 	case NOUVEAU_GETPARAM_GRAPH_UNITS:
 		/* NV40 and NV50 versions are quite different, but register
diff --git a/drivers/gpu/drm/nouveau/nouveau_temp.c b/drivers/gpu/drm/nouveau/nouveau_temp.c
index 8d9968e1cba8..649b0413b09f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_temp.c
+++ b/drivers/gpu/drm/nouveau/nouveau_temp.c
@@ -239,11 +239,9 @@ static bool
 probe_monitoring_device(struct nouveau_i2c_chan *i2c,
 			struct i2c_board_info *info)
 {
-	char modalias[16] = "i2c:";
 	struct i2c_client *client;
 
-	strlcat(modalias, info->type, sizeof(modalias));
-	request_module(modalias);
+	request_module("%s%s", I2C_MODULE_PREFIX, info->type);
 
 	client = i2c_new_device(&i2c->adapter, info);
 	if (!client)
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.c b/drivers/gpu/drm/nouveau/nouveau_vm.c
index 97d82aedf86b..62824c80bcb8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.c
@@ -28,7 +28,7 @@
 #include "nouveau_vm.h"
 
 void
-nouveau_vm_map_at(struct nouveau_vma *vma, u64 delta, struct nouveau_vram *vram)
+nouveau_vm_map_at(struct nouveau_vma *vma, u64 delta, struct nouveau_mem *node)
 {
 	struct nouveau_vm *vm = vma->vm;
 	struct nouveau_mm_node *r;
@@ -40,7 +40,8 @@ nouveau_vm_map_at(struct nouveau_vma *vma, u64 delta, struct nouveau_vram *vram)
 	u32 max  = 1 << (vm->pgt_bits - bits);
 	u32 end, len;
 
-	list_for_each_entry(r, &vram->regions, rl_entry) {
+	delta = 0;
+	list_for_each_entry(r, &node->regions, rl_entry) {
 		u64 phys = (u64)r->offset << 12;
 		u32 num  = r->length >> bits;
 
@@ -52,7 +53,7 @@ nouveau_vm_map_at(struct nouveau_vma *vma, u64 delta, struct nouveau_vram *vram)
 				end = max;
 			len = end - pte;
 
-			vm->map(vma, pgt, vram, pte, len, phys);
+			vm->map(vma, pgt, node, pte, len, phys, delta);
 
 			num -= len;
 			pte += len;
@@ -60,6 +61,8 @@ nouveau_vm_map_at(struct nouveau_vma *vma, u64 delta, struct nouveau_vram *vram)
 				pde++;
 				pte = 0;
 			}
+
+			delta += (u64)len << vma->node->type;
 		}
 	}
 
@@ -67,14 +70,14 @@ nouveau_vm_map_at(struct nouveau_vma *vma, u64 delta, struct nouveau_vram *vram)
 }
 
 void
-nouveau_vm_map(struct nouveau_vma *vma, struct nouveau_vram *vram)
+nouveau_vm_map(struct nouveau_vma *vma, struct nouveau_mem *node)
 {
-	nouveau_vm_map_at(vma, 0, vram);
+	nouveau_vm_map_at(vma, 0, node);
 }
 
 void
 nouveau_vm_map_sg(struct nouveau_vma *vma, u64 delta, u64 length,
-		  dma_addr_t *list)
+		  struct nouveau_mem *mem, dma_addr_t *list)
 {
 	struct nouveau_vm *vm = vma->vm;
 	int big = vma->node->type != vm->spg_shift;
@@ -94,7 +97,7 @@ nouveau_vm_map_sg(struct nouveau_vma *vma, u64 delta, u64 length,
 			end = max;
 		len = end - pte;
 
-		vm->map_sg(vma, pgt, pte, list, len);
+		vm->map_sg(vma, pgt, mem, pte, len, list);
 
 		num  -= len;
 		pte  += len;
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.h b/drivers/gpu/drm/nouveau/nouveau_vm.h
index e1193515771b..2e06b55cfdc1 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.h
@@ -67,9 +67,10 @@ struct nouveau_vm {
 	void (*map_pgt)(struct nouveau_gpuobj *pgd, u32 pde,
 			struct nouveau_gpuobj *pgt[2]);
 	void (*map)(struct nouveau_vma *, struct nouveau_gpuobj *,
-		    struct nouveau_vram *, u32 pte, u32 cnt, u64 phys);
+		    struct nouveau_mem *, u32 pte, u32 cnt,
+		    u64 phys, u64 delta);
 	void (*map_sg)(struct nouveau_vma *, struct nouveau_gpuobj *,
-		       u32 pte, dma_addr_t *, u32 cnt);
+		       struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
 	void (*unmap)(struct nouveau_gpuobj *pgt, u32 pte, u32 cnt);
 	void (*flush)(struct nouveau_vm *);
 };
@@ -82,20 +83,20 @@ int  nouveau_vm_ref(struct nouveau_vm *, struct nouveau_vm **,
 int  nouveau_vm_get(struct nouveau_vm *, u64 size, u32 page_shift,
 		    u32 access, struct nouveau_vma *);
 void nouveau_vm_put(struct nouveau_vma *);
-void nouveau_vm_map(struct nouveau_vma *, struct nouveau_vram *);
-void nouveau_vm_map_at(struct nouveau_vma *, u64 offset, struct nouveau_vram *);
+void nouveau_vm_map(struct nouveau_vma *, struct nouveau_mem *);
+void nouveau_vm_map_at(struct nouveau_vma *, u64 offset, struct nouveau_mem *);
 void nouveau_vm_unmap(struct nouveau_vma *);
 void nouveau_vm_unmap_at(struct nouveau_vma *, u64 offset, u64 length);
 void nouveau_vm_map_sg(struct nouveau_vma *, u64 offset, u64 length,
-		       dma_addr_t *);
+		       struct nouveau_mem *, dma_addr_t *);
 
 /* nv50_vm.c */
 void nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 		     struct nouveau_gpuobj *pgt[2]);
 void nv50_vm_map(struct nouveau_vma *, struct nouveau_gpuobj *,
-		 struct nouveau_vram *, u32 pte, u32 cnt, u64 phys);
+		 struct nouveau_mem *, u32 pte, u32 cnt, u64 phys, u64 delta);
 void nv50_vm_map_sg(struct nouveau_vma *, struct nouveau_gpuobj *,
-		    u32 pte, dma_addr_t *, u32 cnt);
+		    struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
 void nv50_vm_unmap(struct nouveau_gpuobj *, u32 pte, u32 cnt);
 void nv50_vm_flush(struct nouveau_vm *);
 void nv50_vm_flush_engine(struct drm_device *, int engine);
@@ -104,9 +105,9 @@ void nv50_vm_flush_engine(struct drm_device *, int engine);
 void nvc0_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 		     struct nouveau_gpuobj *pgt[2]);
 void nvc0_vm_map(struct nouveau_vma *, struct nouveau_gpuobj *,
-		 struct nouveau_vram *, u32 pte, u32 cnt, u64 phys);
+		 struct nouveau_mem *, u32 pte, u32 cnt, u64 phys, u64 delta);
 void nvc0_vm_map_sg(struct nouveau_vma *, struct nouveau_gpuobj *,
-		    u32 pte, dma_addr_t *, u32 cnt);
+		    struct nouveau_mem *, u32 pte, u32 cnt, dma_addr_t *);
 void nvc0_vm_unmap(struct nouveau_gpuobj *, u32 pte, u32 cnt);
 void nvc0_vm_flush(struct nouveau_vm *);
 
diff --git a/drivers/gpu/drm/nouveau/nv04_crtc.c b/drivers/gpu/drm/nouveau/nv04_crtc.c
index 297505eb98d5..a260fbbe3d9b 100644
--- a/drivers/gpu/drm/nouveau/nv04_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv04_crtc.c
@@ -1031,7 +1031,7 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, NULL, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, false, true, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nv04_fifo.c b/drivers/gpu/drm/nouveau/nv04_fifo.c
index f89d104698df..dfa600c46186 100644
--- a/drivers/gpu/drm/nouveau/nv04_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv04_fifo.c
@@ -379,6 +379,15 @@ out:
 	return handled;
 }
 
+static const char *nv_dma_state_err(u32 state)
+{
+	static const char * const desc[] = {
+		"NONE", "CALL_SUBR_ACTIVE", "INVALID_MTHD", "RET_SUBR_INACTIVE",
+		"INVALID_CMD", "IB_EMPTY"/* NV50+ */, "MEM_FAULT", "UNK"
+	};
+	return desc[(state >> 29) & 0x7];
+}
+
 void
 nv04_fifo_isr(struct drm_device *dev)
 {
@@ -460,9 +469,10 @@ nv04_fifo_isr(struct drm_device *dev)
 				if (nouveau_ratelimit())
 					NV_INFO(dev, "PFIFO_DMA_PUSHER - Ch %d Get 0x%02x%08x "
 					     "Put 0x%02x%08x IbGet 0x%08x IbPut 0x%08x "
-					     "State 0x%08x Push 0x%08x\n",
+					     "State 0x%08x (err: %s) Push 0x%08x\n",
 						chid, ho_get, dma_get, ho_put,
 						dma_put, ib_get, ib_put, state,
+						nv_dma_state_err(state),
 						push);
 
 				/* METHOD_COUNT, in DMA_STATE on earlier chipsets */
@@ -476,8 +486,9 @@ nv04_fifo_isr(struct drm_device *dev)
 				}
 			} else {
 				NV_INFO(dev, "PFIFO_DMA_PUSHER - Ch %d Get 0x%08x "
-					     "Put 0x%08x State 0x%08x Push 0x%08x\n",
-					chid, dma_get, dma_put, state, push);
+					     "Put 0x%08x State 0x%08x (err: %s) Push 0x%08x\n",
+					chid, dma_get, dma_put, state,
+					nv_dma_state_err(state), push);
 
 				if (dma_get != dma_put)
 					nv_wr32(dev, 0x003244, dma_put);
diff --git a/drivers/gpu/drm/nouveau/nv17_tv.c b/drivers/gpu/drm/nouveau/nv17_tv.c
index 28119fd19d03..3900cebba560 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv.c
+++ b/drivers/gpu/drm/nouveau/nv17_tv.c
@@ -197,10 +197,12 @@ static int nv17_tv_get_ld_modes(struct drm_encoder *encoder,
 				struct drm_connector *connector)
 {
 	struct nv17_tv_norm_params *tv_norm = get_tv_norm(encoder);
-	struct drm_display_mode *mode, *tv_mode;
+	const struct drm_display_mode *tv_mode;
 	int n = 0;
 
 	for (tv_mode = nv17_tv_modes; tv_mode->hdisplay; tv_mode++) {
+		struct drm_display_mode *mode;
+
 		mode = drm_mode_duplicate(encoder->dev, tv_mode);
 
 		mode->clock = tv_norm->tv_enc_mode.vrefresh *
diff --git a/drivers/gpu/drm/nouveau/nv17_tv.h b/drivers/gpu/drm/nouveau/nv17_tv.h
index 6bf03840f9eb..622e72221682 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv.h
+++ b/drivers/gpu/drm/nouveau/nv17_tv.h
@@ -112,7 +112,7 @@ extern struct nv17_tv_norm_params {
 } nv17_tv_norms[NUM_TV_NORMS];
 #define get_tv_norm(enc) (&nv17_tv_norms[to_tv_enc(enc)->tv_norm])
 
-extern struct drm_display_mode nv17_tv_modes[];
+extern const struct drm_display_mode nv17_tv_modes[];
 
 static inline int interpolate(int y0, int y1, int y2, int x)
 {
diff --git a/drivers/gpu/drm/nouveau/nv17_tv_modes.c b/drivers/gpu/drm/nouveau/nv17_tv_modes.c
index 9d3893c50a41..4d1d29f60307 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv_modes.c
+++ b/drivers/gpu/drm/nouveau/nv17_tv_modes.c
@@ -438,7 +438,7 @@ void nv17_tv_state_load(struct drm_device *dev, struct nv17_tv_state *state)
 
 /* Timings similar to the ones the blob sets */
 
-struct drm_display_mode nv17_tv_modes[] = {
+const struct drm_display_mode nv17_tv_modes[] = {
 	{ DRM_MODE("320x200", DRM_MODE_TYPE_DRIVER, 0,
 		   320, 344, 392, 560, 0, 200, 200, 202, 220, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC
diff --git a/drivers/gpu/drm/nouveau/nv40_fb.c b/drivers/gpu/drm/nouveau/nv40_fb.c
index f3d9c0505f7b..f0ac2a768c67 100644
--- a/drivers/gpu/drm/nouveau/nv40_fb.c
+++ b/drivers/gpu/drm/nouveau/nv40_fb.c
@@ -24,6 +24,53 @@ nv40_fb_set_tile_region(struct drm_device *dev, int i)
 	}
 }
 
+static void
+nv40_fb_init_gart(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpuobj *gart = dev_priv->gart_info.sg_ctxdma;
+
+	if (dev_priv->gart_info.type != NOUVEAU_GART_HW) {
+		nv_wr32(dev, 0x100800, 0x00000001);
+		return;
+	}
+
+	nv_wr32(dev, 0x100800, gart->pinst | 0x00000002);
+	nv_mask(dev, 0x10008c, 0x00000100, 0x00000100);
+	nv_wr32(dev, 0x100820, 0x00000000);
+}
+
+static void
+nv44_fb_init_gart(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpuobj *gart = dev_priv->gart_info.sg_ctxdma;
+	u32 vinst;
+
+	if (dev_priv->gart_info.type != NOUVEAU_GART_HW) {
+		nv_wr32(dev, 0x100850, 0x80000000);
+		nv_wr32(dev, 0x100800, 0x00000001);
+		return;
+	}
+
+	/* calculate vram address of this PRAMIN block, object
+	 * must be allocated on 512KiB alignment, and not exceed
+	 * a total size of 512KiB for this to work correctly
+	 */
+	vinst  = nv_rd32(dev, 0x10020c);
+	vinst -= ((gart->pinst >> 19) + 1) << 19;
+
+	nv_wr32(dev, 0x100850, 0x80000000);
+	nv_wr32(dev, 0x100818, dev_priv->gart_info.dummy.addr);
+
+	nv_wr32(dev, 0x100804, dev_priv->gart_info.aper_size);
+	nv_wr32(dev, 0x100850, 0x00008000);
+	nv_mask(dev, 0x10008c, 0x00000200, 0x00000200);
+	nv_wr32(dev, 0x100820, 0x00000000);
+	nv_wr32(dev, 0x10082c, 0x00000001);
+	nv_wr32(dev, 0x100800, vinst | 0x00000010);
+}
+
 int
 nv40_fb_init(struct drm_device *dev)
 {
@@ -32,12 +79,12 @@ nv40_fb_init(struct drm_device *dev)
 	uint32_t tmp;
 	int i;
 
-	/* This is strictly a NV4x register (don't know about NV5x). */
-	/* The blob sets these to all kinds of values, and they mess up our setup. */
-	/* I got value 0x52802 instead. For some cards the blob even sets it back to 0x1. */
-	/* Note: the blob doesn't read this value, so i'm pretty sure this is safe for all cards. */
-	/* Any idea what this is? */
-	nv_wr32(dev, NV40_PFB_UNK_800, 0x1);
+	if (dev_priv->chipset != 0x40 && dev_priv->chipset != 0x45) {
+		if (nv44_graph_class(dev))
+			nv44_fb_init_gart(dev);
+		else
+			nv40_fb_init_gart(dev);
+	}
 
 	switch (dev_priv->chipset) {
 	case 0x40:
diff --git a/drivers/gpu/drm/nouveau/nv50_crtc.c b/drivers/gpu/drm/nouveau/nv50_crtc.c
index 9023c4dbb449..2b9984027f41 100644
--- a/drivers/gpu/drm/nouveau/nv50_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv50_crtc.c
@@ -65,7 +65,7 @@ nv50_crtc_blank(struct nouveau_crtc *nv_crtc, bool blanked)
 {
 	struct drm_device *dev = nv_crtc->base.dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	int index = nv_crtc->index, ret;
 
 	NV_DEBUG_KMS(dev, "index %d\n", nv_crtc->index);
@@ -135,8 +135,7 @@ static int
 nv50_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool on, bool update)
 {
 	struct drm_device *dev = nv_crtc->base.dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	int ret;
 
 	NV_DEBUG_KMS(dev, "\n");
@@ -186,8 +185,7 @@ nv50_crtc_set_scale(struct nouveau_crtc *nv_crtc, int scaling_mode, bool update)
 	struct nouveau_connector *nv_connector =
 		nouveau_crtc_connector_get(nv_crtc);
 	struct drm_device *dev = nv_crtc->base.dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	struct drm_display_mode *native_mode = NULL;
 	struct drm_display_mode *mode = &nv_crtc->base.mode;
 	uint32_t outX, outY, horiz, vert;
@@ -445,6 +443,42 @@ nv50_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 }
 
+static int
+nv50_crtc_wait_complete(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_timer_engine *ptimer = &dev_priv->engine.timer;
+	struct nv50_display *disp = nv50_display(dev);
+	struct nouveau_channel *evo = disp->master;
+	u64 start;
+	int ret;
+
+	ret = RING_SPACE(evo, 6);
+	if (ret)
+		return ret;
+	BEGIN_RING(evo, 0, 0x0084, 1);
+	OUT_RING  (evo, 0x80000000);
+	BEGIN_RING(evo, 0, 0x0080, 1);
+	OUT_RING  (evo, 0);
+	BEGIN_RING(evo, 0, 0x0084, 1);
+	OUT_RING  (evo, 0x00000000);
+
+	nv_wo32(disp->ntfy, 0x000, 0x00000000);
+	FIRE_RING (evo);
+
+	start = ptimer->read(dev);
+	do {
+		nv_wr32(dev, 0x61002c, 0x370);
+		nv_wr32(dev, 0x000140, 1);
+
+		if (nv_ro32(disp->ntfy, 0x000))
+			return 0;
+	} while (ptimer->read(dev) - start < 2000000000ULL);
+
+	return -EBUSY;
+}
+
 static void
 nv50_crtc_prepare(struct drm_crtc *crtc)
 {
@@ -453,6 +487,7 @@ nv50_crtc_prepare(struct drm_crtc *crtc)
 
 	NV_DEBUG_KMS(dev, "index %d\n", nv_crtc->index);
 
+	nv50_display_flip_stop(crtc);
 	drm_vblank_pre_modeset(dev, nv_crtc->index);
 	nv50_crtc_blank(nv_crtc, true);
 }
@@ -461,24 +496,14 @@ static void
 nv50_crtc_commit(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
-	int ret;
 
 	NV_DEBUG_KMS(dev, "index %d\n", nv_crtc->index);
 
 	nv50_crtc_blank(nv_crtc, false);
 	drm_vblank_post_modeset(dev, nv_crtc->index);
-
-	ret = RING_SPACE(evo, 2);
-	if (ret) {
-		NV_ERROR(dev, "no space while committing crtc\n");
-		return;
-	}
-	BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
-	OUT_RING  (evo, 0);
-	FIRE_RING (evo);
+	nv50_crtc_wait_complete(crtc);
+	nv50_display_flip_next(crtc, crtc->fb, NULL);
 }
 
 static bool
@@ -491,15 +516,15 @@ nv50_crtc_mode_fixup(struct drm_crtc *crtc, struct drm_display_mode *mode,
 static int
 nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 			   struct drm_framebuffer *passed_fb,
-			   int x, int y, bool update, bool atomic)
+			   int x, int y, bool atomic)
 {
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	struct drm_device *dev = nv_crtc->base.dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	struct drm_framebuffer *drm_fb = nv_crtc->base.fb;
 	struct nouveau_framebuffer *fb = nouveau_framebuffer(drm_fb);
-	int ret, format;
+	int ret;
 
 	NV_DEBUG_KMS(dev, "index %d\n", nv_crtc->index);
 
@@ -525,28 +550,6 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 		}
 	}
 
-	switch (drm_fb->depth) {
-	case  8:
-		format = NV50_EVO_CRTC_FB_DEPTH_8;
-		break;
-	case 15:
-		format = NV50_EVO_CRTC_FB_DEPTH_15;
-		break;
-	case 16:
-		format = NV50_EVO_CRTC_FB_DEPTH_16;
-		break;
-	case 24:
-	case 32:
-		format = NV50_EVO_CRTC_FB_DEPTH_24;
-		break;
-	case 30:
-		format = NV50_EVO_CRTC_FB_DEPTH_30;
-		break;
-	default:
-		 NV_ERROR(dev, "unknown depth %d\n", drm_fb->depth);
-		 return -EINVAL;
-	}
-
 	nv_crtc->fb.offset = fb->nvbo->bo.mem.start << PAGE_SHIFT;
 	nv_crtc->fb.tile_flags = nouveau_bo_tile_layout(fb->nvbo);
 	nv_crtc->fb.cpp = drm_fb->bits_per_pixel / 8;
@@ -556,14 +559,7 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 			return ret;
 
 		BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_DMA), 1);
-		if (nv_crtc->fb.tile_flags == 0x7a00 ||
-		    nv_crtc->fb.tile_flags == 0xfe00)
-			OUT_RING(evo, NvEvoFB32);
-		else
-		if (nv_crtc->fb.tile_flags == 0x7000)
-			OUT_RING(evo, NvEvoFB16);
-		else
-			OUT_RING(evo, NvEvoVRAM_LP);
+		OUT_RING  (evo, fb->r_dma);
 	}
 
 	ret = RING_SPACE(evo, 12);
@@ -571,45 +567,26 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 		return ret;
 
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_OFFSET), 5);
-	OUT_RING(evo, nv_crtc->fb.offset >> 8);
-	OUT_RING(evo, 0);
-	OUT_RING(evo, (drm_fb->height << 16) | drm_fb->width);
-	if (!nv_crtc->fb.tile_flags) {
-		OUT_RING(evo, drm_fb->pitch | (1 << 20));
-	} else {
-		u32 tile_mode = fb->nvbo->tile_mode;
-		if (dev_priv->card_type >= NV_C0)
-			tile_mode >>= 4;
-		OUT_RING(evo, ((drm_fb->pitch / 4) << 4) | tile_mode);
-	}
-	if (dev_priv->chipset == 0x50)
-		OUT_RING(evo, (nv_crtc->fb.tile_flags << 8) | format);
-	else
-		OUT_RING(evo, format);
+	OUT_RING  (evo, nv_crtc->fb.offset >> 8);
+	OUT_RING  (evo, 0);
+	OUT_RING  (evo, (drm_fb->height << 16) | drm_fb->width);
+	OUT_RING  (evo, fb->r_pitch);
+	OUT_RING  (evo, fb->r_format);
 
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CLUT_MODE), 1);
-	OUT_RING(evo, fb->base.depth == 8 ?
-		 NV50_EVO_CRTC_CLUT_MODE_OFF : NV50_EVO_CRTC_CLUT_MODE_ON);
+	OUT_RING  (evo, fb->base.depth == 8 ?
+		   NV50_EVO_CRTC_CLUT_MODE_OFF : NV50_EVO_CRTC_CLUT_MODE_ON);
 
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, COLOR_CTRL), 1);
-	OUT_RING(evo, NV50_EVO_CRTC_COLOR_CTRL_COLOR);
+	OUT_RING  (evo, NV50_EVO_CRTC_COLOR_CTRL_COLOR);
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, FB_POS), 1);
-	OUT_RING(evo, (y << 16) | x);
+	OUT_RING  (evo, (y << 16) | x);
 
 	if (nv_crtc->lut.depth != fb->base.depth) {
 		nv_crtc->lut.depth = fb->base.depth;
 		nv50_crtc_lut_load(crtc);
 	}
 
-	if (update) {
-		ret = RING_SPACE(evo, 2);
-		if (ret)
-			return ret;
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
-		OUT_RING(evo, 0);
-		FIRE_RING(evo);
-	}
-
 	return 0;
 }
 
@@ -619,8 +596,7 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 		   struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	struct nouveau_connector *nv_connector = NULL;
 	uint32_t hsync_dur,  vsync_dur, hsync_start_to_end, vsync_start_to_end;
@@ -700,14 +676,25 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 	nv_crtc->set_dither(nv_crtc, nv_connector->use_dithering, false);
 	nv_crtc->set_scale(nv_crtc, nv_connector->scaling_mode, false);
 
-	return nv50_crtc_do_mode_set_base(crtc, old_fb, x, y, false, false);
+	return nv50_crtc_do_mode_set_base(crtc, old_fb, x, y, false);
 }
 
 static int
 nv50_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 			struct drm_framebuffer *old_fb)
 {
-	return nv50_crtc_do_mode_set_base(crtc, old_fb, x, y, true, false);
+	int ret;
+
+	nv50_display_flip_stop(crtc);
+	ret = nv50_crtc_do_mode_set_base(crtc, old_fb, x, y, false);
+	if (ret)
+		return ret;
+
+	ret = nv50_crtc_wait_complete(crtc);
+	if (ret)
+		return ret;
+
+	return nv50_display_flip_next(crtc, crtc->fb, NULL);
 }
 
 static int
@@ -715,7 +702,14 @@ nv50_crtc_mode_set_base_atomic(struct drm_crtc *crtc,
 			       struct drm_framebuffer *fb,
 			       int x, int y, enum mode_set_atomic state)
 {
-	return nv50_crtc_do_mode_set_base(crtc, fb, x, y, true, true);
+	int ret;
+
+	nv50_display_flip_stop(crtc);
+	ret = nv50_crtc_do_mode_set_base(crtc, fb, x, y, true);
+	if (ret)
+		return ret;
+
+	return nv50_crtc_wait_complete(crtc);
 }
 
 static const struct drm_crtc_helper_funcs nv50_crtc_helper_funcs = {
@@ -758,7 +752,7 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	nv_crtc->lut.depth = 0;
 
 	ret = nouveau_bo_new(dev, NULL, 4096, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, false, true, &nv_crtc->lut.nvbo);
+			     0, 0x0000, &nv_crtc->lut.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->lut.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
@@ -784,7 +778,7 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, NULL, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, false, true, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret)
diff --git a/drivers/gpu/drm/nouveau/nv50_cursor.c b/drivers/gpu/drm/nouveau/nv50_cursor.c
index 1b9ce3021aa3..9752c35bb84b 100644
--- a/drivers/gpu/drm/nouveau/nv50_cursor.c
+++ b/drivers/gpu/drm/nouveau/nv50_cursor.c
@@ -36,9 +36,9 @@
 static void
 nv50_cursor_show(struct nouveau_crtc *nv_crtc, bool update)
 {
-	struct drm_nouveau_private *dev_priv = nv_crtc->base.dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
 	struct drm_device *dev = nv_crtc->base.dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	int ret;
 
 	NV_DEBUG_KMS(dev, "\n");
@@ -71,9 +71,9 @@ nv50_cursor_show(struct nouveau_crtc *nv_crtc, bool update)
 static void
 nv50_cursor_hide(struct nouveau_crtc *nv_crtc, bool update)
 {
-	struct drm_nouveau_private *dev_priv = nv_crtc->base.dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
 	struct drm_device *dev = nv_crtc->base.dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	int ret;
 
 	NV_DEBUG_KMS(dev, "\n");
diff --git a/drivers/gpu/drm/nouveau/nv50_dac.c b/drivers/gpu/drm/nouveau/nv50_dac.c
index 875414b09ade..808f3ec8f827 100644
--- a/drivers/gpu/drm/nouveau/nv50_dac.c
+++ b/drivers/gpu/drm/nouveau/nv50_dac.c
@@ -41,8 +41,7 @@ nv50_dac_disconnect(struct drm_encoder *encoder)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct drm_device *dev = encoder->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	int ret;
 
 	if (!nv_encoder->crtc)
@@ -216,8 +215,7 @@ nv50_dac_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct drm_device *dev = encoder->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	struct nouveau_crtc *crtc = nouveau_crtc(encoder->crtc);
 	uint32_t mode_ctl = 0, mode_ctl2 = 0;
 	int ret;
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 7cc94ed9ed95..75a376cc342a 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -24,6 +24,7 @@
  *
  */
 
+#define NOUVEAU_DMA_DEBUG (nouveau_reg_debug & NOUVEAU_REG_DEBUG_EVO)
 #include "nv50_display.h"
 #include "nouveau_crtc.h"
 #include "nouveau_encoder.h"
@@ -34,6 +35,7 @@
 #include "drm_crtc_helper.h"
 
 static void nv50_display_isr(struct drm_device *);
+static void nv50_display_bh(unsigned long);
 
 static inline int
 nv50_sor_nr(struct drm_device *dev)
@@ -172,16 +174,16 @@ nv50_display_init(struct drm_device *dev)
 	ret = nv50_evo_init(dev);
 	if (ret)
 		return ret;
-	evo = dev_priv->evo;
+	evo = nv50_display(dev)->master;
 
 	nv_wr32(dev, NV50_PDISPLAY_OBJECTS, (evo->ramin->vinst >> 8) | 9);
 
-	ret = RING_SPACE(evo, 11);
+	ret = RING_SPACE(evo, 15);
 	if (ret)
 		return ret;
 	BEGIN_RING(evo, 0, NV50_EVO_UNK84, 2);
 	OUT_RING(evo, NV50_EVO_UNK84_NOTIFY_DISABLED);
-	OUT_RING(evo, NV50_EVO_DMA_NOTIFY_HANDLE_NONE);
+	OUT_RING(evo, NvEvoSync);
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, FB_DMA), 1);
 	OUT_RING(evo, NV50_EVO_CRTC_FB_DMA_HANDLE_NONE);
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, UNK0800), 1);
@@ -190,6 +192,11 @@ nv50_display_init(struct drm_device *dev)
 	OUT_RING(evo, 0);
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, UNK082C), 1);
 	OUT_RING(evo, 0);
+	/* required to make display sync channels not hate life */
+	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, UNK900), 1);
+	OUT_RING  (evo, 0x00000311);
+	BEGIN_RING(evo, 0, NV50_EVO_CRTC(1, UNK900), 1);
+	OUT_RING  (evo, 0x00000311);
 	FIRE_RING(evo);
 	if (!nv_wait(dev, 0x640004, 0xffffffff, evo->dma.put << 2))
 		NV_ERROR(dev, "evo pushbuf stalled\n");
@@ -201,6 +208,8 @@ nv50_display_init(struct drm_device *dev)
 static int nv50_display_disable(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_display *disp = nv50_display(dev);
+	struct nouveau_channel *evo = disp->master;
 	struct drm_crtc *drm_crtc;
 	int ret, i;
 
@@ -212,12 +221,12 @@ static int nv50_display_disable(struct drm_device *dev)
 		nv50_crtc_blank(crtc, true);
 	}
 
-	ret = RING_SPACE(dev_priv->evo, 2);
+	ret = RING_SPACE(evo, 2);
 	if (ret == 0) {
-		BEGIN_RING(dev_priv->evo, 0, NV50_EVO_UPDATE, 1);
-		OUT_RING(dev_priv->evo, 0);
+		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+		OUT_RING(evo, 0);
 	}
-	FIRE_RING(dev_priv->evo);
+	FIRE_RING(evo);
 
 	/* Almost like ack'ing a vblank interrupt, maybe in the spirit of
 	 * cleaning up?
@@ -267,10 +276,16 @@ int nv50_display_create(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct dcb_table *dcb = &dev_priv->vbios.dcb;
 	struct drm_connector *connector, *ct;
+	struct nv50_display *priv;
 	int ret, i;
 
 	NV_DEBUG_KMS(dev, "\n");
 
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	dev_priv->engine.display.priv = priv;
+
 	/* init basic kernel modesetting */
 	drm_mode_config_init(dev);
 
@@ -330,7 +345,7 @@ int nv50_display_create(struct drm_device *dev)
 		}
 	}
 
-	INIT_WORK(&dev_priv->irq_work, nv50_display_irq_handler_bh);
+	tasklet_init(&priv->tasklet, nv50_display_bh, (unsigned long)dev);
 	nouveau_irq_register(dev, 26, nv50_display_isr);
 
 	ret = nv50_display_init(dev);
@@ -345,12 +360,131 @@ int nv50_display_create(struct drm_device *dev)
 void
 nv50_display_destroy(struct drm_device *dev)
 {
+	struct nv50_display *disp = nv50_display(dev);
+
 	NV_DEBUG_KMS(dev, "\n");
 
 	drm_mode_config_cleanup(dev);
 
 	nv50_display_disable(dev);
 	nouveau_irq_unregister(dev, 26);
+	kfree(disp);
+}
+
+void
+nv50_display_flip_stop(struct drm_crtc *crtc)
+{
+	struct nv50_display *disp = nv50_display(crtc->dev);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+	struct nv50_display_crtc *dispc = &disp->crtc[nv_crtc->index];
+	struct nouveau_channel *evo = dispc->sync;
+	int ret;
+
+	ret = RING_SPACE(evo, 8);
+	if (ret) {
+		WARN_ON(1);
+		return;
+	}
+
+	BEGIN_RING(evo, 0, 0x0084, 1);
+	OUT_RING  (evo, 0x00000000);
+	BEGIN_RING(evo, 0, 0x0094, 1);
+	OUT_RING  (evo, 0x00000000);
+	BEGIN_RING(evo, 0, 0x00c0, 1);
+	OUT_RING  (evo, 0x00000000);
+	BEGIN_RING(evo, 0, 0x0080, 1);
+	OUT_RING  (evo, 0x00000000);
+	FIRE_RING (evo);
+}
+
+int
+nv50_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
+		       struct nouveau_channel *chan)
+{
+	struct drm_nouveau_private *dev_priv = crtc->dev->dev_private;
+	struct nouveau_framebuffer *nv_fb = nouveau_framebuffer(fb);
+	struct nv50_display *disp = nv50_display(crtc->dev);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+	struct nv50_display_crtc *dispc = &disp->crtc[nv_crtc->index];
+	struct nouveau_channel *evo = dispc->sync;
+	int ret;
+
+	ret = RING_SPACE(evo, 24);
+	if (unlikely(ret))
+		return ret;
+
+	/* synchronise with the rendering channel, if necessary */
+	if (likely(chan)) {
+		u64 offset = dispc->sem.bo->vma.offset + dispc->sem.offset;
+
+		ret = RING_SPACE(chan, 10);
+		if (ret) {
+			WIND_RING(evo);
+			return ret;
+		}
+
+		if (dev_priv->chipset < 0xc0) {
+			BEGIN_RING(chan, NvSubSw, 0x0060, 2);
+			OUT_RING  (chan, NvEvoSema0 + nv_crtc->index);
+			OUT_RING  (chan, dispc->sem.offset);
+			BEGIN_RING(chan, NvSubSw, 0x006c, 1);
+			OUT_RING  (chan, 0xf00d0000 | dispc->sem.value);
+			BEGIN_RING(chan, NvSubSw, 0x0064, 2);
+			OUT_RING  (chan, dispc->sem.offset ^ 0x10);
+			OUT_RING  (chan, 0x74b1e000);
+			BEGIN_RING(chan, NvSubSw, 0x0060, 1);
+			if (dev_priv->chipset < 0x84)
+				OUT_RING  (chan, NvSema);
+			else
+				OUT_RING  (chan, chan->vram_handle);
+		} else {
+			BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0010, 4);
+			OUT_RING  (chan, upper_32_bits(offset));
+			OUT_RING  (chan, lower_32_bits(offset));
+			OUT_RING  (chan, 0xf00d0000 | dispc->sem.value);
+			OUT_RING  (chan, 0x1002);
+			BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0010, 4);
+			OUT_RING  (chan, upper_32_bits(offset));
+			OUT_RING  (chan, lower_32_bits(offset ^ 0x10));
+			OUT_RING  (chan, 0x74b1e000);
+			OUT_RING  (chan, 0x1001);
+		}
+		FIRE_RING (chan);
+	} else {
+		nouveau_bo_wr32(dispc->sem.bo, dispc->sem.offset / 4,
+				0xf00d0000 | dispc->sem.value);
+	}
+
+	/* queue the flip on the crtc's "display sync" channel */
+	BEGIN_RING(evo, 0, 0x0100, 1);
+	OUT_RING  (evo, 0xfffe0000);
+	BEGIN_RING(evo, 0, 0x0084, 5);
+	OUT_RING  (evo, chan ? 0x00000100 : 0x00000010);
+	OUT_RING  (evo, dispc->sem.offset);
+	OUT_RING  (evo, 0xf00d0000 | dispc->sem.value);
+	OUT_RING  (evo, 0x74b1e000);
+	OUT_RING  (evo, NvEvoSync);
+	BEGIN_RING(evo, 0, 0x00a0, 2);
+	OUT_RING  (evo, 0x00000000);
+	OUT_RING  (evo, 0x00000000);
+	BEGIN_RING(evo, 0, 0x00c0, 1);
+	OUT_RING  (evo, nv_fb->r_dma);
+	BEGIN_RING(evo, 0, 0x0110, 2);
+	OUT_RING  (evo, 0x00000000);
+	OUT_RING  (evo, 0x00000000);
+	BEGIN_RING(evo, 0, 0x0800, 5);
+	OUT_RING  (evo, (nv_fb->nvbo->bo.mem.start << PAGE_SHIFT) >> 8);
+	OUT_RING  (evo, 0);
+	OUT_RING  (evo, (fb->height << 16) | fb->width);
+	OUT_RING  (evo, nv_fb->r_pitch);
+	OUT_RING  (evo, nv_fb->r_format);
+	BEGIN_RING(evo, 0, 0x0080, 1);
+	OUT_RING  (evo, 0x00000000);
+	FIRE_RING (evo);
+
+	dispc->sem.offset ^= 0x10;
+	dispc->sem.value++;
+	return 0;
 }
 
 static u16
@@ -466,11 +600,12 @@ static void
 nv50_display_unk10_handler(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_display *disp = nv50_display(dev);
 	u32 unk30 = nv_rd32(dev, 0x610030), mc;
 	int i, crtc, or, type = OUTPUT_ANY;
 
 	NV_DEBUG_KMS(dev, "0x610030: 0x%08x\n", unk30);
-	dev_priv->evo_irq.dcb = NULL;
+	disp->irq.dcb = NULL;
 
 	nv_wr32(dev, 0x619494, nv_rd32(dev, 0x619494) & ~8);
 
@@ -541,7 +676,7 @@ nv50_display_unk10_handler(struct drm_device *dev)
 
 		if (dcb->type == type && (dcb->or & (1 << or))) {
 			nouveau_bios_run_display_table(dev, dcb, 0, -1);
-			dev_priv->evo_irq.dcb = dcb;
+			disp->irq.dcb = dcb;
 			goto ack;
 		}
 	}
@@ -587,15 +722,16 @@ static void
 nv50_display_unk20_handler(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	u32 unk30 = nv_rd32(dev, 0x610030), tmp, pclk, script, mc;
+	struct nv50_display *disp = nv50_display(dev);
+	u32 unk30 = nv_rd32(dev, 0x610030), tmp, pclk, script, mc = 0;
 	struct dcb_entry *dcb;
 	int i, crtc, or, type = OUTPUT_ANY;
 
 	NV_DEBUG_KMS(dev, "0x610030: 0x%08x\n", unk30);
-	dcb = dev_priv->evo_irq.dcb;
+	dcb = disp->irq.dcb;
 	if (dcb) {
 		nouveau_bios_run_display_table(dev, dcb, 0, -2);
-		dev_priv->evo_irq.dcb = NULL;
+		disp->irq.dcb = NULL;
 	}
 
 	/* CRTC clock change requested? */
@@ -692,9 +828,9 @@ nv50_display_unk20_handler(struct drm_device *dev)
 		nv_wr32(dev, NV50_PDISPLAY_DAC_CLK_CTRL2(or), 0);
 	}
 
-	dev_priv->evo_irq.dcb = dcb;
-	dev_priv->evo_irq.pclk = pclk;
-	dev_priv->evo_irq.script = script;
+	disp->irq.dcb = dcb;
+	disp->irq.pclk = pclk;
+	disp->irq.script = script;
 
 ack:
 	nv_wr32(dev, NV50_PDISPLAY_INTR_1, NV50_PDISPLAY_INTR_1_CLK_UNK20);
@@ -735,13 +871,13 @@ nv50_display_unk40_dp_set_tmds(struct drm_device *dev, struct dcb_entry *dcb)
 static void
 nv50_display_unk40_handler(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct dcb_entry *dcb = dev_priv->evo_irq.dcb;
-	u16 script = dev_priv->evo_irq.script;
-	u32 unk30 = nv_rd32(dev, 0x610030), pclk = dev_priv->evo_irq.pclk;
+	struct nv50_display *disp = nv50_display(dev);
+	struct dcb_entry *dcb = disp->irq.dcb;
+	u16 script = disp->irq.script;
+	u32 unk30 = nv_rd32(dev, 0x610030), pclk = disp->irq.pclk;
 
 	NV_DEBUG_KMS(dev, "0x610030: 0x%08x\n", unk30);
-	dev_priv->evo_irq.dcb = NULL;
+	disp->irq.dcb = NULL;
 	if (!dcb)
 		goto ack;
 
@@ -754,12 +890,10 @@ ack:
 	nv_wr32(dev, 0x619494, nv_rd32(dev, 0x619494) | 8);
 }
 
-void
-nv50_display_irq_handler_bh(struct work_struct *work)
+static void
+nv50_display_bh(unsigned long data)
 {
-	struct drm_nouveau_private *dev_priv =
-		container_of(work, struct drm_nouveau_private, irq_work);
-	struct drm_device *dev = dev_priv->dev;
+	struct drm_device *dev = (struct drm_device *)data;
 
 	for (;;) {
 		uint32_t intr0 = nv_rd32(dev, NV50_PDISPLAY_INTR_0);
@@ -807,7 +941,7 @@ nv50_display_error_handler(struct drm_device *dev)
 static void
 nv50_display_isr(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_display *disp = nv50_display(dev);
 	uint32_t delayed = 0;
 
 	while (nv_rd32(dev, NV50_PMC_INTR_0) & NV50_PMC_INTR_0_DISPLAY) {
@@ -835,8 +969,7 @@ nv50_display_isr(struct drm_device *dev)
 				  NV50_PDISPLAY_INTR_1_CLK_UNK40));
 		if (clock) {
 			nv_wr32(dev, NV03_PMC_INTR_EN_0, 0);
-			if (!work_pending(&dev_priv->irq_work))
-				queue_work(dev_priv->wq, &dev_priv->irq_work);
+			tasklet_schedule(&disp->tasklet);
 			delayed |= clock;
 			intr1 &= ~clock;
 		}
diff --git a/drivers/gpu/drm/nouveau/nv50_display.h b/drivers/gpu/drm/nouveau/nv50_display.h
index f0e30b78ef6b..c2da503a22aa 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.h
+++ b/drivers/gpu/drm/nouveau/nv50_display.h
@@ -35,7 +35,36 @@
 #include "nouveau_crtc.h"
 #include "nv50_evo.h"
 
-void nv50_display_irq_handler_bh(struct work_struct *work);
+struct nv50_display_crtc {
+	struct nouveau_channel *sync;
+	struct {
+		struct nouveau_bo *bo;
+		u32 offset;
+		u16 value;
+	} sem;
+};
+
+struct nv50_display {
+	struct nouveau_channel *master;
+	struct nouveau_gpuobj *ntfy;
+
+	struct nv50_display_crtc crtc[2];
+
+	struct tasklet_struct tasklet;
+	struct {
+		struct dcb_entry *dcb;
+		u16 script;
+		u32 pclk;
+	} irq;
+};
+
+static inline struct nv50_display *
+nv50_display(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	return dev_priv->engine.display.priv;
+}
+
 int nv50_display_early_init(struct drm_device *dev);
 void nv50_display_late_takedown(struct drm_device *dev);
 int nv50_display_create(struct drm_device *dev);
@@ -44,4 +73,15 @@ void nv50_display_destroy(struct drm_device *dev);
 int nv50_crtc_blank(struct nouveau_crtc *, bool blank);
 int nv50_crtc_set_clock(struct drm_device *, int head, int pclk);
 
+int  nv50_display_flip_next(struct drm_crtc *, struct drm_framebuffer *,
+			    struct nouveau_channel *chan);
+void nv50_display_flip_stop(struct drm_crtc *);
+
+int  nv50_evo_init(struct drm_device *dev);
+void nv50_evo_fini(struct drm_device *dev);
+void nv50_evo_dmaobj_init(struct nouveau_gpuobj *, u32 memtype, u64 base,
+			  u64 size);
+int  nv50_evo_dmaobj_new(struct nouveau_channel *, u32 handle, u32 memtype,
+			 u64 base, u64 size, struct nouveau_gpuobj **);
+
 #endif /* __NV50_DISPLAY_H__ */
diff --git a/drivers/gpu/drm/nouveau/nv50_evo.c b/drivers/gpu/drm/nouveau/nv50_evo.c
index 0ea090f4244a..a2cfaa691e9b 100644
--- a/drivers/gpu/drm/nouveau/nv50_evo.c
+++ b/drivers/gpu/drm/nouveau/nv50_evo.c
@@ -27,20 +27,17 @@
 #include "nouveau_drv.h"
 #include "nouveau_dma.h"
 #include "nouveau_ramht.h"
+#include "nv50_display.h"
 
 static void
 nv50_evo_channel_del(struct nouveau_channel **pevo)
 {
-	struct drm_nouveau_private *dev_priv;
 	struct nouveau_channel *evo = *pevo;
 
 	if (!evo)
 		return;
 	*pevo = NULL;
 
-	dev_priv = evo->dev->dev_private;
-	dev_priv->evo_alloc &= ~(1 << evo->id);
-
 	nouveau_gpuobj_channel_takedown(evo);
 	nouveau_bo_unmap(evo->pushbuf_bo);
 	nouveau_bo_ref(NULL, &evo->pushbuf_bo);
@@ -51,42 +48,61 @@ nv50_evo_channel_del(struct nouveau_channel **pevo)
 	kfree(evo);
 }
 
+void
+nv50_evo_dmaobj_init(struct nouveau_gpuobj *obj, u32 memtype, u64 base, u64 size)
+{
+	struct drm_nouveau_private *dev_priv = obj->dev->dev_private;
+	u32 flags5;
+
+	if (dev_priv->chipset < 0xc0) {
+		/* not supported on 0x50, specified in format mthd */
+		if (dev_priv->chipset == 0x50)
+			memtype = 0;
+		flags5 = 0x00010000;
+	} else {
+		if (memtype & 0x80000000)
+			flags5 = 0x00000000; /* large pages */
+		else
+			flags5 = 0x00020000;
+	}
+
+	nv50_gpuobj_dma_init(obj, 0, 0x3d, base, size, NV_MEM_TARGET_VRAM,
+			     NV_MEM_ACCESS_RW, (memtype >> 8) & 0xff, 0);
+	nv_wo32(obj, 0x14, flags5);
+	dev_priv->engine.instmem.flush(obj->dev);
+}
+
 int
-nv50_evo_dmaobj_new(struct nouveau_channel *evo, u32 class, u32 name,
-		    u32 tile_flags, u32 magic_flags, u32 offset, u32 limit,
-		    u32 flags5)
+nv50_evo_dmaobj_new(struct nouveau_channel *evo, u32 handle, u32 memtype,
+		    u64 base, u64 size, struct nouveau_gpuobj **pobj)
 {
-	struct drm_nouveau_private *dev_priv = evo->dev->dev_private;
-	struct drm_device *dev = evo->dev;
+	struct nv50_display *disp = nv50_display(evo->dev);
 	struct nouveau_gpuobj *obj = NULL;
 	int ret;
 
-	ret = nouveau_gpuobj_new(dev, dev_priv->evo, 6*4, 32, 0, &obj);
+	ret = nouveau_gpuobj_new(evo->dev, disp->master, 6*4, 32, 0, &obj);
 	if (ret)
 		return ret;
 	obj->engine = NVOBJ_ENGINE_DISPLAY;
 
-	nv_wo32(obj,  0, (tile_flags << 22) | (magic_flags << 16) | class);
-	nv_wo32(obj,  4, limit);
-	nv_wo32(obj,  8, offset);
-	nv_wo32(obj, 12, 0x00000000);
-	nv_wo32(obj, 16, 0x00000000);
-	nv_wo32(obj, 20, flags5);
-	dev_priv->engine.instmem.flush(dev);
+	nv50_evo_dmaobj_init(obj, memtype, base, size);
 
-	ret = nouveau_ramht_insert(evo, name, obj);
-	nouveau_gpuobj_ref(NULL, &obj);
-	if (ret) {
-		return ret;
-	}
+	ret = nouveau_ramht_insert(evo, handle, obj);
+	if (ret)
+		goto out;
 
-	return 0;
+	if (pobj)
+		nouveau_gpuobj_ref(obj, pobj);
+out:
+	nouveau_gpuobj_ref(NULL, &obj);
+	return ret;
 }
 
 static int
-nv50_evo_channel_new(struct drm_device *dev, struct nouveau_channel **pevo)
+nv50_evo_channel_new(struct drm_device *dev, int chid,
+		     struct nouveau_channel **pevo)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_display *disp = nv50_display(dev);
 	struct nouveau_channel *evo;
 	int ret;
 
@@ -95,25 +111,13 @@ nv50_evo_channel_new(struct drm_device *dev, struct nouveau_channel **pevo)
 		return -ENOMEM;
 	*pevo = evo;
 
-	for (evo->id = 0; evo->id < 5; evo->id++) {
-		if (dev_priv->evo_alloc & (1 << evo->id))
-			continue;
-
-		dev_priv->evo_alloc |= (1 << evo->id);
-		break;
-	}
-
-	if (evo->id == 5) {
-		kfree(evo);
-		return -ENODEV;
-	}
-
+	evo->id = chid;
 	evo->dev = dev;
 	evo->user_get = 4;
 	evo->user_put = 0;
 
 	ret = nouveau_bo_new(dev, NULL, 4096, 0, TTM_PL_FLAG_VRAM, 0, 0,
-			     false, true, &evo->pushbuf_bo);
+			     &evo->pushbuf_bo);
 	if (ret == 0)
 		ret = nouveau_bo_pin(evo->pushbuf_bo, TTM_PL_FLAG_VRAM);
 	if (ret) {
@@ -138,8 +142,8 @@ nv50_evo_channel_new(struct drm_device *dev, struct nouveau_channel **pevo)
 	}
 
 	/* bind primary evo channel's ramht to the channel */
-	if (dev_priv->evo && evo != dev_priv->evo)
-		nouveau_ramht_ref(dev_priv->evo->ramht, &evo->ramht, NULL);
+	if (disp->master && evo != disp->master)
+		nouveau_ramht_ref(disp->master->ramht, &evo->ramht, NULL);
 
 	return 0;
 }
@@ -212,21 +216,39 @@ nv50_evo_channel_fini(struct nouveau_channel *evo)
 	}
 }
 
+static void
+nv50_evo_destroy(struct drm_device *dev)
+{
+	struct nv50_display *disp = nv50_display(dev);
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (disp->crtc[i].sem.bo) {
+			nouveau_bo_unmap(disp->crtc[i].sem.bo);
+			nouveau_bo_ref(NULL, &disp->crtc[i].sem.bo);
+		}
+		nv50_evo_channel_del(&disp->crtc[i].sync);
+	}
+	nouveau_gpuobj_ref(NULL, &disp->ntfy);
+	nv50_evo_channel_del(&disp->master);
+}
+
 static int
 nv50_evo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_display *disp = nv50_display(dev);
 	struct nouveau_gpuobj *ramht = NULL;
 	struct nouveau_channel *evo;
-	int ret;
+	int ret, i, j;
 
 	/* create primary evo channel, the one we use for modesetting
 	 * purporses
 	 */
-	ret = nv50_evo_channel_new(dev, &dev_priv->evo);
+	ret = nv50_evo_channel_new(dev, 0, &disp->master);
 	if (ret)
 		return ret;
-	evo = dev_priv->evo;
+	evo = disp->master;
 
 	/* setup object management on it, any other evo channel will
 	 * use this also as there's no per-channel support on the
@@ -236,109 +258,167 @@ nv50_evo_create(struct drm_device *dev)
 				 NVOBJ_FLAG_ZERO_ALLOC, &evo->ramin);
 	if (ret) {
 		NV_ERROR(dev, "Error allocating EVO channel memory: %d\n", ret);
-		nv50_evo_channel_del(&dev_priv->evo);
-		return ret;
+		goto err;
 	}
 
 	ret = drm_mm_init(&evo->ramin_heap, 0, 32768);
 	if (ret) {
 		NV_ERROR(dev, "Error initialising EVO PRAMIN heap: %d\n", ret);
-		nv50_evo_channel_del(&dev_priv->evo);
-		return ret;
+		goto err;
 	}
 
 	ret = nouveau_gpuobj_new(dev, evo, 4096, 16, 0, &ramht);
 	if (ret) {
 		NV_ERROR(dev, "Unable to allocate EVO RAMHT: %d\n", ret);
-		nv50_evo_channel_del(&dev_priv->evo);
-		return ret;
+		goto err;
 	}
 
 	ret = nouveau_ramht_new(dev, ramht, &evo->ramht);
 	nouveau_gpuobj_ref(NULL, &ramht);
-	if (ret) {
-		nv50_evo_channel_del(&dev_priv->evo);
-		return ret;
-	}
+	if (ret)
+		goto err;
+
+	/* not sure exactly what this is..
+	 *
+	 * the first dword of the structure is used by nvidia to wait on
+	 * full completion of an EVO "update" command.
+	 *
+	 * method 0x8c on the master evo channel will fill a lot more of
+	 * this structure with some undefined info
+	 */
+	ret = nouveau_gpuobj_new(dev, disp->master, 0x1000, 0,
+				 NVOBJ_FLAG_ZERO_ALLOC, &disp->ntfy);
+	if (ret)
+		goto err;
+
+	ret = nv50_evo_dmaobj_new(disp->master, NvEvoSync, 0x0000,
+				  disp->ntfy->vinst, disp->ntfy->size, NULL);
+	if (ret)
+		goto err;
 
 	/* create some default objects for the scanout memtypes we support */
-	if (dev_priv->card_type >= NV_C0) {
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoFB32, 0xfe, 0x19,
-					  0, 0xffffffff, 0x00000000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
-		}
+	ret = nv50_evo_dmaobj_new(disp->master, NvEvoVRAM, 0x0000,
+				  0, dev_priv->vram_size, NULL);
+	if (ret)
+		goto err;
 
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoVRAM, 0, 0x19,
-					  0, dev_priv->vram_size, 0x00020000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
-		}
+	ret = nv50_evo_dmaobj_new(disp->master, NvEvoVRAM_LP, 0x80000000,
+				  0, dev_priv->vram_size, NULL);
+	if (ret)
+		goto err;
 
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoVRAM_LP, 0, 0x19,
-					  0, dev_priv->vram_size, 0x00000000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
-		}
-	} else {
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoFB16, 0x70, 0x19,
-					  0, 0xffffffff, 0x00010000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
-		}
+	ret = nv50_evo_dmaobj_new(disp->master, NvEvoFB32, 0x80000000 |
+				  (dev_priv->chipset < 0xc0 ? 0x7a00 : 0xfe00),
+				  0, dev_priv->vram_size, NULL);
+	if (ret)
+		goto err;
 
+	ret = nv50_evo_dmaobj_new(disp->master, NvEvoFB16, 0x80000000 |
+				  (dev_priv->chipset < 0xc0 ? 0x7000 : 0xfe00),
+				  0, dev_priv->vram_size, NULL);
+	if (ret)
+		goto err;
 
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoFB32, 0x7a, 0x19,
-					  0, 0xffffffff, 0x00010000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
-		}
+	/* create "display sync" channels and other structures we need
+	 * to implement page flipping
+	 */
+	for (i = 0; i < 2; i++) {
+		struct nv50_display_crtc *dispc = &disp->crtc[i];
+		u64 offset;
 
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoVRAM, 0, 0x19,
-					  0, dev_priv->vram_size, 0x00010000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
+		ret = nv50_evo_channel_new(dev, 1 + i, &dispc->sync);
+		if (ret)
+			goto err;
+
+		ret = nouveau_bo_new(dev, NULL, 4096, 0x1000, TTM_PL_FLAG_VRAM,
+				     0, 0x0000, &dispc->sem.bo);
+		if (!ret) {
+			offset = dispc->sem.bo->bo.mem.start << PAGE_SHIFT;
+
+			ret = nouveau_bo_pin(dispc->sem.bo, TTM_PL_FLAG_VRAM);
+			if (!ret)
+				ret = nouveau_bo_map(dispc->sem.bo);
+			if (ret)
+				nouveau_bo_ref(NULL, &dispc->sem.bo);
 		}
 
-		ret = nv50_evo_dmaobj_new(evo, 0x3d, NvEvoVRAM_LP, 0, 0x19,
-					  0, dev_priv->vram_size, 0x00010000);
-		if (ret) {
-			nv50_evo_channel_del(&dev_priv->evo);
-			return ret;
-		}
+		if (ret)
+			goto err;
+
+		ret = nv50_evo_dmaobj_new(dispc->sync, NvEvoSync, 0x0000,
+					  offset, 4096, NULL);
+		if (ret)
+			goto err;
+
+		ret = nv50_evo_dmaobj_new(dispc->sync, NvEvoVRAM_LP, 0x80000000,
+					  0, dev_priv->vram_size, NULL);
+		if (ret)
+			goto err;
+
+		ret = nv50_evo_dmaobj_new(dispc->sync, NvEvoFB32, 0x80000000 |
+					  (dev_priv->chipset < 0xc0 ?
+					  0x7a00 : 0xfe00),
+					  0, dev_priv->vram_size, NULL);
+		if (ret)
+			goto err;
+
+		ret = nv50_evo_dmaobj_new(dispc->sync, NvEvoFB16, 0x80000000 |
+					  (dev_priv->chipset < 0xc0 ?
+					  0x7000 : 0xfe00),
+					  0, dev_priv->vram_size, NULL);
+		if (ret)
+			goto err;
+
+		for (j = 0; j < 4096; j += 4)
+			nouveau_bo_wr32(dispc->sem.bo, j / 4, 0x74b1e000);
+		dispc->sem.offset = 0;
 	}
 
 	return 0;
+
+err:
+	nv50_evo_destroy(dev);
+	return ret;
 }
 
 int
 nv50_evo_init(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	int ret;
+	struct nv50_display *disp = nv50_display(dev);
+	int ret, i;
 
-	if (!dev_priv->evo) {
+	if (!disp->master) {
 		ret = nv50_evo_create(dev);
 		if (ret)
 			return ret;
 	}
 
-	return nv50_evo_channel_init(dev_priv->evo);
+	ret = nv50_evo_channel_init(disp->master);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < 2; i++) {
+		ret = nv50_evo_channel_init(disp->crtc[i].sync);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
 }
 
 void
 nv50_evo_fini(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_display *disp = nv50_display(dev);
+	int i;
 
-	if (dev_priv->evo) {
-		nv50_evo_channel_fini(dev_priv->evo);
-		nv50_evo_channel_del(&dev_priv->evo);
+	for (i = 0; i < 2; i++) {
+		if (disp->crtc[i].sync)
+			nv50_evo_channel_fini(disp->crtc[i].sync);
 	}
+
+	if (disp->master)
+		nv50_evo_channel_fini(disp->master);
+
+	nv50_evo_destroy(dev);
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_evo.h b/drivers/gpu/drm/nouveau/nv50_evo.h
index aa4f0d3cea8e..3860ca62cb19 100644
--- a/drivers/gpu/drm/nouveau/nv50_evo.h
+++ b/drivers/gpu/drm/nouveau/nv50_evo.h
@@ -27,12 +27,6 @@
 #ifndef __NV50_EVO_H__
 #define __NV50_EVO_H__
 
-int  nv50_evo_init(struct drm_device *dev);
-void nv50_evo_fini(struct drm_device *dev);
-int  nv50_evo_dmaobj_new(struct nouveau_channel *, u32 class, u32 name,
-			 u32 tile_flags, u32 magic_flags,
-			 u32 offset, u32 limit);
-
 #define NV50_EVO_UPDATE                                              0x00000080
 #define NV50_EVO_UNK84                                               0x00000084
 #define NV50_EVO_UNK84_NOTIFY                                        0x40000000
@@ -119,5 +113,7 @@ int  nv50_evo_dmaobj_new(struct nouveau_channel *, u32 class, u32 name,
 /* Both of these are needed, otherwise nothing happens. */
 #define NV50_EVO_CRTC_SCALE_RES1                                     0x000008d8
 #define NV50_EVO_CRTC_SCALE_RES2                                     0x000008dc
+#define NV50_EVO_CRTC_UNK900                                         0x00000900
+#define NV50_EVO_CRTC_UNK904                                         0x00000904
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/nv50_fb.c b/drivers/gpu/drm/nouveau/nv50_fb.c
index 50290dea0ac4..ed411d88451d 100644
--- a/drivers/gpu/drm/nouveau/nv50_fb.c
+++ b/drivers/gpu/drm/nouveau/nv50_fb.c
@@ -8,31 +8,61 @@ struct nv50_fb_priv {
 	dma_addr_t r100c08;
 };
 
+static void
+nv50_fb_destroy(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
+	struct nv50_fb_priv *priv = pfb->priv;
+
+	if (drm_mm_initialized(&pfb->tag_heap))
+		drm_mm_takedown(&pfb->tag_heap);
+
+	if (priv->r100c08_page) {
+		pci_unmap_page(dev->pdev, priv->r100c08, PAGE_SIZE,
+			       PCI_DMA_BIDIRECTIONAL);
+		__free_page(priv->r100c08_page);
+	}
+
+	kfree(priv);
+	pfb->priv = NULL;
+}
+
 static int
 nv50_fb_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
 	struct nv50_fb_priv *priv;
+	u32 tagmem;
+	int ret;
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
+	pfb->priv = priv;
 
 	priv->r100c08_page = alloc_page(GFP_KERNEL | __GFP_ZERO);
 	if (!priv->r100c08_page) {
-		kfree(priv);
+		nv50_fb_destroy(dev);
 		return -ENOMEM;
 	}
 
 	priv->r100c08 = pci_map_page(dev->pdev, priv->r100c08_page, 0,
 				     PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
 	if (pci_dma_mapping_error(dev->pdev, priv->r100c08)) {
-		__free_page(priv->r100c08_page);
-		kfree(priv);
+		nv50_fb_destroy(dev);
 		return -EFAULT;
 	}
 
-	dev_priv->engine.fb.priv = priv;
+	tagmem = nv_rd32(dev, 0x100320);
+	NV_DEBUG(dev, "%d tags available\n", tagmem);
+	ret = drm_mm_init(&pfb->tag_heap, 0, tagmem);
+	if (ret) {
+		nv50_fb_destroy(dev);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -81,18 +111,7 @@ nv50_fb_init(struct drm_device *dev)
 void
 nv50_fb_takedown(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nv50_fb_priv *priv;
-
-	priv = dev_priv->engine.fb.priv;
-	if (!priv)
-		return;
-	dev_priv->engine.fb.priv = NULL;
-
-	pci_unmap_page(dev->pdev, priv->r100c08, PAGE_SIZE,
-		       PCI_DMA_BIDIRECTIONAL);
-	__free_page(priv->r100c08_page);
-	kfree(priv);
+	nv50_fb_destroy(dev);
 }
 
 void
diff --git a/drivers/gpu/drm/nouveau/nv50_fifo.c b/drivers/gpu/drm/nouveau/nv50_fifo.c
index 8dd04c5dac67..c34a074f7ea1 100644
--- a/drivers/gpu/drm/nouveau/nv50_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv50_fifo.c
@@ -149,6 +149,7 @@ nv50_fifo_init_regs(struct drm_device *dev)
 	nv_wr32(dev, 0x3204, 0);
 	nv_wr32(dev, 0x3210, 0);
 	nv_wr32(dev, 0x3270, 0);
+	nv_wr32(dev, 0x2044, 0x01003fff);
 
 	/* Enable dummy channels setup by nv50_instmem.c */
 	nv50_fifo_channel_enable(dev, 0);
@@ -273,7 +274,7 @@ nv50_fifo_create_context(struct nouveau_channel *chan)
 	nv_wo32(ramfc, 0x80, ((chan->ramht->bits - 9) << 27) |
 			     (4 << 24) /* SEARCH_FULL */ |
 			     (chan->ramht->gpuobj->cinst >> 4));
-	nv_wo32(ramfc, 0x44, 0x2101ffff);
+	nv_wo32(ramfc, 0x44, 0x01003fff);
 	nv_wo32(ramfc, 0x60, 0x7fffffff);
 	nv_wo32(ramfc, 0x40, 0x00000000);
 	nv_wo32(ramfc, 0x7c, 0x30000001);
diff --git a/drivers/gpu/drm/nouveau/nv50_gpio.c b/drivers/gpu/drm/nouveau/nv50_gpio.c
index 6b149c0cc06d..d4f4206dad7e 100644
--- a/drivers/gpu/drm/nouveau/nv50_gpio.c
+++ b/drivers/gpu/drm/nouveau/nv50_gpio.c
@@ -137,6 +137,7 @@ nv50_gpio_irq_unregister(struct drm_device *dev, enum dcb_gpio_tag tag,
 	struct nv50_gpio_priv *priv = pgpio->priv;
 	struct nv50_gpio_handler *gpioh, *tmp;
 	struct dcb_gpio_entry *gpio;
+	LIST_HEAD(tofree);
 	unsigned long flags;
 
 	gpio = nouveau_bios_gpio_entry(dev, tag);
@@ -149,10 +150,14 @@ nv50_gpio_irq_unregister(struct drm_device *dev, enum dcb_gpio_tag tag,
 		    gpioh->handler != handler ||
 		    gpioh->data != data)
 			continue;
-		list_del(&gpioh->head);
-		kfree(gpioh);
+		list_move(&gpioh->head, &tofree);
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
+
+	list_for_each_entry_safe(gpioh, tmp, &tofree, head) {
+		flush_work_sync(&gpioh->work);
+		kfree(gpioh);
+	}
 }
 
 bool
@@ -205,7 +210,6 @@ nv50_gpio_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct nv50_gpio_priv *priv;
 	int ret;
 
 	if (!pgpio->priv) {
@@ -213,7 +217,6 @@ nv50_gpio_init(struct drm_device *dev)
 		if (ret)
 			return ret;
 	}
-	priv = pgpio->priv;
 
 	/* disable, and ack any pending gpio interrupts */
 	nv_wr32(dev, 0xe050, 0x00000000);
@@ -293,7 +296,7 @@ nv50_gpio_isr(struct drm_device *dev)
 			continue;
 		gpioh->inhibit = true;
 
-		queue_work(dev_priv->wq, &gpioh->work);
+		schedule_work(&gpioh->work);
 	}
 	spin_unlock(&priv->lock);
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_graph.c b/drivers/gpu/drm/nouveau/nv50_graph.c
index 37e21d2be95b..e1267a1f6d10 100644
--- a/drivers/gpu/drm/nouveau/nv50_graph.c
+++ b/drivers/gpu/drm/nouveau/nv50_graph.c
@@ -95,13 +95,41 @@ nv50_graph_init_regs__nv(struct drm_device *dev)
 }
 
 static void
-nv50_graph_init_regs(struct drm_device *dev)
+nv50_graph_init_zcull(struct drm_device *dev)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	int i;
+
 	NV_DEBUG(dev, "\n");
 
-	nv_wr32(dev, NV04_PGRAPH_DEBUG_3,
-				(1 << 2) /* HW_CONTEXT_SWITCH_ENABLED */);
-	nv_wr32(dev, 0x402ca8, 0x800);
+	switch (dev_priv->chipset & 0xf0) {
+	case 0x50:
+	case 0x80:
+	case 0x90:
+		nv_wr32(dev, 0x402ca8, 0x00000800);
+		break;
+	case 0xa0:
+	default:
+		nv_wr32(dev, 0x402cc0, 0x00000000);
+		if (dev_priv->chipset == 0xa0 ||
+		    dev_priv->chipset == 0xaa ||
+		    dev_priv->chipset == 0xac) {
+			nv_wr32(dev, 0x402ca8, 0x00000802);
+		} else {
+			nv_wr32(dev, 0x402cc0, 0x00000000);
+			nv_wr32(dev, 0x402ca8, 0x00000002);
+		}
+
+		break;
+	}
+
+	/* zero out zcull regions */
+	for (i = 0; i < 8; i++) {
+		nv_wr32(dev, 0x402c20 + (i * 8), 0x00000000);
+		nv_wr32(dev, 0x402c24 + (i * 8), 0x00000000);
+		nv_wr32(dev, 0x402c28 + (i * 8), 0x00000000);
+		nv_wr32(dev, 0x402c2c + (i * 8), 0x00000000);
+	}
 }
 
 static int
@@ -136,6 +164,7 @@ nv50_graph_init_ctxctl(struct drm_device *dev)
 	}
 	kfree(cp);
 
+	nv_wr32(dev, 0x40008c, 0x00000004); /* HW_CTX_SWITCH_ENABLED */
 	nv_wr32(dev, 0x400320, 4);
 	nv_wr32(dev, NV40_PGRAPH_CTXCTL_CUR, 0);
 	nv_wr32(dev, NV20_PGRAPH_CHANNEL_CTX_POINTER, 0);
@@ -151,7 +180,7 @@ nv50_graph_init(struct drm_device *dev)
 
 	nv50_graph_init_reset(dev);
 	nv50_graph_init_regs__nv(dev);
-	nv50_graph_init_regs(dev);
+	nv50_graph_init_zcull(dev);
 
 	ret = nv50_graph_init_ctxctl(dev);
 	if (ret)
@@ -409,12 +438,7 @@ static int
 nv50_graph_nvsw_mthd_page_flip(struct nouveau_channel *chan,
 			       u32 class, u32 mthd, u32 data)
 {
-	struct nouveau_page_flip_state s;
-
-	if (!nouveau_finish_page_flip(chan, &s)) {
-		/* XXX - Do something here */
-	}
-
+	nouveau_finish_page_flip(chan, NULL);
 	return 0;
 }
 
@@ -912,10 +936,10 @@ nv50_pgraph_trap_handler(struct drm_device *dev, u32 display, u64 inst, u32 chid
 			printk("\n");
 			NV_INFO(dev, "PGRAPH - TRAP_CCACHE %08x %08x %08x %08x"
 				     " %08x %08x %08x\n",
-				nv_rd32(dev, 0x405800), nv_rd32(dev, 0x405804),
-				nv_rd32(dev, 0x405808), nv_rd32(dev, 0x40580c),
-				nv_rd32(dev, 0x405810), nv_rd32(dev, 0x405814),
-				nv_rd32(dev, 0x40581c));
+				nv_rd32(dev, 0x405000), nv_rd32(dev, 0x405004),
+				nv_rd32(dev, 0x405008), nv_rd32(dev, 0x40500c),
+				nv_rd32(dev, 0x405010), nv_rd32(dev, 0x405014),
+				nv_rd32(dev, 0x40501c));
 
 		}
 
diff --git a/drivers/gpu/drm/nouveau/nv50_instmem.c b/drivers/gpu/drm/nouveau/nv50_instmem.c
index ea0041810ae3..a6f8aa651fc6 100644
--- a/drivers/gpu/drm/nouveau/nv50_instmem.c
+++ b/drivers/gpu/drm/nouveau/nv50_instmem.c
@@ -56,7 +56,7 @@ nv50_channel_del(struct nouveau_channel **pchan)
 	nouveau_gpuobj_ref(NULL, &chan->ramfc);
 	nouveau_vm_ref(NULL, &chan->vm, chan->vm_pd);
 	nouveau_gpuobj_ref(NULL, &chan->vm_pd);
-	if (chan->ramin_heap.free_stack.next)
+	if (drm_mm_initialized(&chan->ramin_heap))
 		drm_mm_takedown(&chan->ramin_heap);
 	nouveau_gpuobj_ref(NULL, &chan->ramin);
 	kfree(chan);
@@ -259,7 +259,7 @@ nv50_instmem_takedown(struct drm_device *dev)
 	nouveau_gpuobj_ref(NULL, &dev_priv->bar3_vm->pgt[0].obj[0]);
 	nouveau_vm_ref(NULL, &dev_priv->bar3_vm, NULL);
 
-	if (dev_priv->ramin_heap.free_stack.next)
+	if (drm_mm_initialized(&dev_priv->ramin_heap))
 		drm_mm_takedown(&dev_priv->ramin_heap);
 
 	dev_priv->engine.instmem.priv = NULL;
@@ -300,7 +300,7 @@ nv50_instmem_resume(struct drm_device *dev)
 }
 
 struct nv50_gpuobj_node {
-	struct nouveau_vram *vram;
+	struct nouveau_mem *vram;
 	struct nouveau_vma chan_vma;
 	u32 align;
 };
@@ -403,16 +403,24 @@ nv50_instmem_unmap(struct nouveau_gpuobj *gpuobj)
 void
 nv50_instmem_flush(struct drm_device *dev)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	spin_lock(&dev_priv->ramin_lock);
 	nv_wr32(dev, 0x00330c, 0x00000001);
 	if (!nv_wait(dev, 0x00330c, 0x00000002, 0x00000000))
 		NV_ERROR(dev, "PRAMIN flush timeout\n");
+	spin_unlock(&dev_priv->ramin_lock);
 }
 
 void
 nv84_instmem_flush(struct drm_device *dev)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	spin_lock(&dev_priv->ramin_lock);
 	nv_wr32(dev, 0x070000, 0x00000001);
 	if (!nv_wait(dev, 0x070000, 0x00000002, 0x00000000))
 		NV_ERROR(dev, "PRAMIN flush timeout\n");
+	spin_unlock(&dev_priv->ramin_lock);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv50_sor.c b/drivers/gpu/drm/nouveau/nv50_sor.c
index b4a5ecb199f9..c25c59386420 100644
--- a/drivers/gpu/drm/nouveau/nv50_sor.c
+++ b/drivers/gpu/drm/nouveau/nv50_sor.c
@@ -41,8 +41,7 @@ nv50_sor_disconnect(struct drm_encoder *encoder)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct drm_device *dev = encoder->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(dev)->master;
 	int ret;
 
 	if (!nv_encoder->crtc)
@@ -184,8 +183,7 @@ static void
 nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		  struct drm_display_mode *adjusted_mode)
 {
-	struct drm_nouveau_private *dev_priv = encoder->dev->dev_private;
-	struct nouveau_channel *evo = dev_priv->evo;
+	struct nouveau_channel *evo = nv50_display(encoder->dev)->master;
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct drm_device *dev = encoder->dev;
 	struct nouveau_crtc *crtc = nouveau_crtc(encoder->crtc);
diff --git a/drivers/gpu/drm/nouveau/nv50_vm.c b/drivers/gpu/drm/nouveau/nv50_vm.c
index 459ff08241e5..4fd3432b5b8d 100644
--- a/drivers/gpu/drm/nouveau/nv50_vm.c
+++ b/drivers/gpu/drm/nouveau/nv50_vm.c
@@ -31,7 +31,6 @@ void
 nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 		struct nouveau_gpuobj *pgt[2])
 {
-	struct drm_nouveau_private *dev_priv = pgd->dev->dev_private;
 	u64 phys = 0xdeadcafe00000000ULL;
 	u32 coverage = 0;
 
@@ -58,10 +57,9 @@ nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 }
 
 static inline u64
-nv50_vm_addr(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
-	     u64 phys, u32 memtype, u32 target)
+nv50_vm_addr(struct nouveau_vma *vma, u64 phys, u32 memtype, u32 target)
 {
-	struct drm_nouveau_private *dev_priv = pgt->dev->dev_private;
+	struct drm_nouveau_private *dev_priv = vma->vm->dev->dev_private;
 
 	phys |= 1; /* present */
 	phys |= (u64)memtype << 40;
@@ -85,12 +83,13 @@ nv50_vm_addr(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
 
 void
 nv50_vm_map(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
-	    struct nouveau_vram *mem, u32 pte, u32 cnt, u64 phys)
+	    struct nouveau_mem *mem, u32 pte, u32 cnt, u64 phys, u64 delta)
 {
+	u32 comp = (mem->memtype & 0x180) >> 7;
 	u32 block;
 	int i;
 
-	phys  = nv50_vm_addr(vma, pgt, phys, mem->memtype, 0);
+	phys  = nv50_vm_addr(vma, phys, mem->memtype, 0);
 	pte <<= 3;
 	cnt <<= 3;
 
@@ -107,6 +106,11 @@ nv50_vm_map(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
 
 		phys += block << (vma->node->type - 3);
 		cnt  -= block;
+		if (comp) {
+			u32 tag = mem->tag->start + ((delta >> 16) * comp);
+			offset_h |= (tag << 17);
+			delta    += block << (vma->node->type - 3);
+		}
 
 		while (block) {
 			nv_wo32(pgt, pte + 0, offset_l);
@@ -119,11 +123,11 @@ nv50_vm_map(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
 
 void
 nv50_vm_map_sg(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
-	       u32 pte, dma_addr_t *list, u32 cnt)
+	       struct nouveau_mem *mem, u32 pte, u32 cnt, dma_addr_t *list)
 {
 	pte <<= 3;
 	while (cnt--) {
-		u64 phys = nv50_vm_addr(vma, pgt, (u64)*list++, 0, 2);
+		u64 phys = nv50_vm_addr(vma, (u64)*list++, mem->memtype, 2);
 		nv_wo32(pgt, pte + 0, lower_32_bits(phys));
 		nv_wo32(pgt, pte + 4, upper_32_bits(phys));
 		pte += 8;
@@ -169,7 +173,11 @@ nv50_vm_flush(struct nouveau_vm *vm)
 void
 nv50_vm_flush_engine(struct drm_device *dev, int engine)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	spin_lock(&dev_priv->ramin_lock);
 	nv_wr32(dev, 0x100c80, (engine << 16) | 1);
 	if (!nv_wait(dev, 0x100c80, 0x00000001, 0x00000000))
 		NV_ERROR(dev, "vm flush timeout: engine %d\n", engine);
+	spin_unlock(&dev_priv->ramin_lock);
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_vram.c b/drivers/gpu/drm/nouveau/nv50_vram.c
index 58e98ad36347..ffbc3d8cf5be 100644
--- a/drivers/gpu/drm/nouveau/nv50_vram.c
+++ b/drivers/gpu/drm/nouveau/nv50_vram.c
@@ -48,42 +48,49 @@ nv50_vram_flags_valid(struct drm_device *dev, u32 tile_flags)
 }
 
 void
-nv50_vram_del(struct drm_device *dev, struct nouveau_vram **pvram)
+nv50_vram_del(struct drm_device *dev, struct nouveau_mem **pmem)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct ttm_bo_device *bdev = &dev_priv->ttm.bdev;
 	struct ttm_mem_type_manager *man = &bdev->man[TTM_PL_VRAM];
 	struct nouveau_mm *mm = man->priv;
 	struct nouveau_mm_node *this;
-	struct nouveau_vram *vram;
+	struct nouveau_mem *mem;
 
-	vram = *pvram;
-	*pvram = NULL;
-	if (unlikely(vram == NULL))
+	mem = *pmem;
+	*pmem = NULL;
+	if (unlikely(mem == NULL))
 		return;
 
 	mutex_lock(&mm->mutex);
-	while (!list_empty(&vram->regions)) {
-		this = list_first_entry(&vram->regions, struct nouveau_mm_node, rl_entry);
+	while (!list_empty(&mem->regions)) {
+		this = list_first_entry(&mem->regions, struct nouveau_mm_node, rl_entry);
 
 		list_del(&this->rl_entry);
 		nouveau_mm_put(mm, this);
 	}
+
+	if (mem->tag) {
+		drm_mm_put_block(mem->tag);
+		mem->tag = NULL;
+	}
 	mutex_unlock(&mm->mutex);
 
-	kfree(vram);
+	kfree(mem);
 }
 
 int
 nv50_vram_new(struct drm_device *dev, u64 size, u32 align, u32 size_nc,
-	      u32 type, struct nouveau_vram **pvram)
+	      u32 memtype, struct nouveau_mem **pmem)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct ttm_bo_device *bdev = &dev_priv->ttm.bdev;
 	struct ttm_mem_type_manager *man = &bdev->man[TTM_PL_VRAM];
 	struct nouveau_mm *mm = man->priv;
 	struct nouveau_mm_node *r;
-	struct nouveau_vram *vram;
+	struct nouveau_mem *mem;
+	int comp = (memtype & 0x300) >> 8;
+	int type = (memtype & 0x07f);
 	int ret;
 
 	if (!types[type])
@@ -92,32 +99,46 @@ nv50_vram_new(struct drm_device *dev, u64 size, u32 align, u32 size_nc,
 	align >>= 12;
 	size_nc >>= 12;
 
-	vram = kzalloc(sizeof(*vram), GFP_KERNEL);
-	if (!vram)
+	mem = kzalloc(sizeof(*mem), GFP_KERNEL);
+	if (!mem)
 		return -ENOMEM;
 
-	INIT_LIST_HEAD(&vram->regions);
-	vram->dev = dev_priv->dev;
-	vram->memtype = type;
-	vram->size = size;
-
 	mutex_lock(&mm->mutex);
+	if (comp) {
+		if (align == 16) {
+			struct nouveau_fb_engine *pfb = &dev_priv->engine.fb;
+			int n = (size >> 4) * comp;
+
+			mem->tag = drm_mm_search_free(&pfb->tag_heap, n, 0, 0);
+			if (mem->tag)
+				mem->tag = drm_mm_get_block(mem->tag, n, 0);
+		}
+
+		if (unlikely(!mem->tag))
+			comp = 0;
+	}
+
+	INIT_LIST_HEAD(&mem->regions);
+	mem->dev = dev_priv->dev;
+	mem->memtype = (comp << 7) | type;
+	mem->size = size;
+
 	do {
 		ret = nouveau_mm_get(mm, types[type], size, size_nc, align, &r);
 		if (ret) {
 			mutex_unlock(&mm->mutex);
-			nv50_vram_del(dev, &vram);
+			nv50_vram_del(dev, &mem);
 			return ret;
 		}
 
-		list_add_tail(&r->rl_entry, &vram->regions);
+		list_add_tail(&r->rl_entry, &mem->regions);
 		size -= r->length;
 	} while (size);
 	mutex_unlock(&mm->mutex);
 
-	r = list_first_entry(&vram->regions, struct nouveau_mm_node, rl_entry);
-	vram->offset = (u64)r->offset << 12;
-	*pvram = vram;
+	r = list_first_entry(&mem->regions, struct nouveau_mm_node, rl_entry);
+	mem->offset = (u64)r->offset << 12;
+	*pmem = mem;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nvc0_fifo.c b/drivers/gpu/drm/nouveau/nvc0_fifo.c
index e6f92c541dba..2886f2726a9e 100644
--- a/drivers/gpu/drm/nouveau/nvc0_fifo.c
+++ b/drivers/gpu/drm/nouveau/nvc0_fifo.c
@@ -116,7 +116,7 @@ nvc0_fifo_create_context(struct nouveau_channel *chan)
 
 	/* allocate vram for control regs, map into polling area */
 	ret = nouveau_bo_new(dev, NULL, 0x1000, 0, TTM_PL_FLAG_VRAM,
-			     0, 0, true, true, &fifoch->user);
+			     0, 0, &fifoch->user);
 	if (ret)
 		goto error;
 
@@ -418,6 +418,12 @@ nvc0_fifo_isr(struct drm_device *dev)
 {
 	u32 stat = nv_rd32(dev, 0x002100);
 
+	if (stat & 0x00000100) {
+		NV_INFO(dev, "PFIFO: unknown status 0x00000100\n");
+		nv_wr32(dev, 0x002100, 0x00000100);
+		stat &= ~0x00000100;
+	}
+
 	if (stat & 0x10000000) {
 		u32 units = nv_rd32(dev, 0x00259c);
 		u32 u = units;
@@ -446,10 +452,15 @@ nvc0_fifo_isr(struct drm_device *dev)
 		stat &= ~0x20000000;
 	}
 
+	if (stat & 0x40000000) {
+		NV_INFO(dev, "PFIFO: unknown status 0x40000000\n");
+		nv_mask(dev, 0x002a00, 0x00000000, 0x00000000);
+		stat &= ~0x40000000;
+	}
+
 	if (stat) {
 		NV_INFO(dev, "PFIFO: unhandled status 0x%08x\n", stat);
 		nv_wr32(dev, 0x002100, stat);
+		nv_wr32(dev, 0x002140, 0);
 	}
-
-	nv_wr32(dev, 0x2140, 0);
 }
diff --git a/drivers/gpu/drm/nouveau/nvc0_graph.c b/drivers/gpu/drm/nouveau/nvc0_graph.c
index eb18a7e89f5b..3de9b721d8db 100644
--- a/drivers/gpu/drm/nouveau/nvc0_graph.c
+++ b/drivers/gpu/drm/nouveau/nvc0_graph.c
@@ -299,6 +299,14 @@ nvc0_graph_takedown(struct drm_device *dev)
 }
 
 static int
+nvc0_graph_mthd_page_flip(struct nouveau_channel *chan,
+			  u32 class, u32 mthd, u32 data)
+{
+	nouveau_finish_page_flip(chan, NULL);
+	return 0;
+}
+
+static int
 nvc0_graph_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
@@ -395,6 +403,7 @@ nvc0_graph_create(struct drm_device *dev)
 	nouveau_irq_register(dev, 25, nvc0_runk140_isr);
 	NVOBJ_CLASS(dev, 0x902d, GR); /* 2D */
 	NVOBJ_CLASS(dev, 0x9039, GR); /* M2MF */
+	NVOBJ_MTHD (dev, 0x9039, 0x0500, nvc0_graph_mthd_page_flip);
 	NVOBJ_CLASS(dev, 0x9097, GR); /* 3D */
 	NVOBJ_CLASS(dev, 0x90c0, GR); /* COMPUTE */
 	return 0;
@@ -640,7 +649,6 @@ nvc0_graph_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_pgraph_engine *pgraph = &dev_priv->engine.graph;
-	struct nvc0_graph_priv *priv;
 	int ret;
 
 	dev_priv->engine.graph.accel_blocked = true;
@@ -665,7 +673,6 @@ nvc0_graph_init(struct drm_device *dev)
 		if (ret)
 			return ret;
 	}
-	priv = pgraph->priv;
 
 	nvc0_graph_init_obj418880(dev);
 	nvc0_graph_init_regs(dev);
@@ -730,9 +737,12 @@ nvc0_graph_isr(struct drm_device *dev)
 	u32 class = nv_rd32(dev, 0x404200 + (subc * 4));
 
 	if (stat & 0x00000010) {
-		NV_INFO(dev, "PGRAPH: ILLEGAL_MTHD ch %d [0x%010llx] subc %d "
-			     "class 0x%04x mthd 0x%04x data 0x%08x\n",
-			chid, inst, subc, class, mthd, data);
+		if (nouveau_gpuobj_mthd_call2(dev, chid, class, mthd, data)) {
+			NV_INFO(dev, "PGRAPH: ILLEGAL_MTHD ch %d [0x%010llx] "
+				     "subc %d class 0x%04x mthd 0x%04x "
+				     "data 0x%08x\n",
+				chid, inst, subc, class, mthd, data);
+		}
 		nv_wr32(dev, 0x400100, 0x00000010);
 		stat &= ~0x00000010;
 	}
diff --git a/drivers/gpu/drm/nouveau/nvc0_instmem.c b/drivers/gpu/drm/nouveau/nvc0_instmem.c
index c09091749054..82357d2df1f4 100644
--- a/drivers/gpu/drm/nouveau/nvc0_instmem.c
+++ b/drivers/gpu/drm/nouveau/nvc0_instmem.c
@@ -67,7 +67,7 @@ nvc0_channel_del(struct nouveau_channel **pchan)
 		return;
 
 	nouveau_vm_ref(NULL, &chan->vm, NULL);
-	if (chan->ramin_heap.free_stack.next)
+	if (drm_mm_initialized(&chan->ramin_heap))
 		drm_mm_takedown(&chan->ramin_heap);
 	nouveau_gpuobj_ref(NULL, &chan->ramin);
 	kfree(chan);
diff --git a/drivers/gpu/drm/nouveau/nvc0_vm.c b/drivers/gpu/drm/nouveau/nvc0_vm.c
index e4e83c2caf5b..69af0ba7edd3 100644
--- a/drivers/gpu/drm/nouveau/nvc0_vm.c
+++ b/drivers/gpu/drm/nouveau/nvc0_vm.c
@@ -59,7 +59,7 @@ nvc0_vm_addr(struct nouveau_vma *vma, u64 phys, u32 memtype, u32 target)
 
 void
 nvc0_vm_map(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
-	    struct nouveau_vram *mem, u32 pte, u32 cnt, u64 phys)
+	    struct nouveau_mem *mem, u32 pte, u32 cnt, u64 phys, u64 delta)
 {
 	u32 next = 1 << (vma->node->type - 8);
 
@@ -75,11 +75,11 @@ nvc0_vm_map(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
 
 void
 nvc0_vm_map_sg(struct nouveau_vma *vma, struct nouveau_gpuobj *pgt,
-	       u32 pte, dma_addr_t *list, u32 cnt)
+	       struct nouveau_mem *mem, u32 pte, u32 cnt, dma_addr_t *list)
 {
 	pte <<= 3;
 	while (cnt--) {
-		u64 phys = nvc0_vm_addr(vma, *list++, 0, 5);
+		u64 phys = nvc0_vm_addr(vma, *list++, mem->memtype, 5);
 		nv_wo32(pgt, pte + 0, lower_32_bits(phys));
 		nv_wo32(pgt, pte + 4, upper_32_bits(phys));
 		pte += 8;
diff --git a/drivers/gpu/drm/nouveau/nvc0_vram.c b/drivers/gpu/drm/nouveau/nvc0_vram.c
index 858eda5dedd1..67c6ec6f34ea 100644
--- a/drivers/gpu/drm/nouveau/nvc0_vram.c
+++ b/drivers/gpu/drm/nouveau/nvc0_vram.c
@@ -26,64 +26,78 @@
 #include "nouveau_drv.h"
 #include "nouveau_mm.h"
 
+/* 0 = unsupported
+ * 1 = non-compressed
+ * 3 = compressed
+ */
+static const u8 types[256] = {
+	1, 1, 3, 3, 3, 3, 0, 3, 3, 3, 3, 0, 0, 0, 0, 0,
+	0, 1, 0, 0, 0, 0, 0, 3, 3, 3, 3, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3,
+	3, 3, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 0, 1, 1, 1, 1, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 3, 3, 3, 3, 1, 1, 1, 1, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3,
+	3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3,
+	3, 3, 0, 0, 0, 0, 0, 0, 3, 0, 0, 3, 0, 3, 0, 3,
+	3, 0, 3, 3, 3, 3, 3, 0, 0, 3, 0, 3, 0, 3, 3, 0,
+	3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 0, 1, 1, 0
+};
+
 bool
 nvc0_vram_flags_valid(struct drm_device *dev, u32 tile_flags)
 {
-	switch (tile_flags & NOUVEAU_GEM_TILE_LAYOUT_MASK) {
-	case 0x0000:
-	case 0xfe00:
-	case 0xdb00:
-	case 0x1100:
-		return true;
-	default:
-		break;
-	}
-
-	return false;
+	u8 memtype = (tile_flags & NOUVEAU_GEM_TILE_LAYOUT_MASK) >> 8;
+	return likely((types[memtype] == 1));
 }
 
 int
 nvc0_vram_new(struct drm_device *dev, u64 size, u32 align, u32 ncmin,
-	      u32 type, struct nouveau_vram **pvram)
+	      u32 type, struct nouveau_mem **pmem)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct ttm_bo_device *bdev = &dev_priv->ttm.bdev;
 	struct ttm_mem_type_manager *man = &bdev->man[TTM_PL_VRAM];
 	struct nouveau_mm *mm = man->priv;
 	struct nouveau_mm_node *r;
-	struct nouveau_vram *vram;
+	struct nouveau_mem *mem;
 	int ret;
 
 	size  >>= 12;
 	align >>= 12;
 	ncmin >>= 12;
 
-	vram = kzalloc(sizeof(*vram), GFP_KERNEL);
-	if (!vram)
+	mem = kzalloc(sizeof(*mem), GFP_KERNEL);
+	if (!mem)
 		return -ENOMEM;
 
-	INIT_LIST_HEAD(&vram->regions);
-	vram->dev = dev_priv->dev;
-	vram->memtype = type;
-	vram->size = size;
+	INIT_LIST_HEAD(&mem->regions);
+	mem->dev = dev_priv->dev;
+	mem->memtype = (type & 0xff);
+	mem->size = size;
 
 	mutex_lock(&mm->mutex);
 	do {
 		ret = nouveau_mm_get(mm, 1, size, ncmin, align, &r);
 		if (ret) {
 			mutex_unlock(&mm->mutex);
-			nv50_vram_del(dev, &vram);
+			nv50_vram_del(dev, &mem);
 			return ret;
 		}
 
-		list_add_tail(&r->rl_entry, &vram->regions);
+		list_add_tail(&r->rl_entry, &mem->regions);
 		size -= r->length;
 	} while (size);
 	mutex_unlock(&mm->mutex);
 
-	r = list_first_entry(&vram->regions, struct nouveau_mm_node, rl_entry);
-	vram->offset = (u64)r->offset << 12;
-	*pvram = vram;
+	r = list_first_entry(&mem->regions, struct nouveau_mm_node, rl_entry);
+	mem->offset = (u64)r->offset << 12;
+	*pmem = mem;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/r128/r128_drv.c b/drivers/gpu/drm/r128/r128_drv.c
index 18c3c71e41b1..b9e8efd2b754 100644
--- a/drivers/gpu/drm/r128/r128_drv.c
+++ b/drivers/gpu/drm/r128/r128_drv.c
@@ -71,10 +71,7 @@ static struct drm_driver driver = {
 #endif
 		.llseek = noop_llseek,
 	},
-	.pci_driver = {
-		.name = DRIVER_NAME,
-		.id_table = pciidlist,
-	},
+
 
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
@@ -89,16 +86,21 @@ int r128_driver_load(struct drm_device *dev, unsigned long flags)
 	return drm_vblank_init(dev, 1);
 }
 
+static struct pci_driver r128_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init r128_init(void)
 {
 	driver.num_ioctls = r128_max_ioctl;
 
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &r128_pci_driver);
 }
 
 static void __exit r128_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &r128_pci_driver);
 }
 
 module_init(r128_init);
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index e47eecfc2df4..3896ef811102 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -36,6 +36,9 @@ $(obj)/r600_reg_safe.h: $(src)/reg_srcs/r600 $(obj)/mkregtable
 $(obj)/evergreen_reg_safe.h: $(src)/reg_srcs/evergreen $(obj)/mkregtable
 	$(call if_changed,mkregtable)
 
+$(obj)/cayman_reg_safe.h: $(src)/reg_srcs/cayman $(obj)/mkregtable
+	$(call if_changed,mkregtable)
+
 $(obj)/r100.o: $(obj)/r100_reg_safe.h $(obj)/rn50_reg_safe.h
 
 $(obj)/r200.o: $(obj)/r200_reg_safe.h
@@ -50,7 +53,7 @@ $(obj)/rs600.o: $(obj)/rs600_reg_safe.h
 
 $(obj)/r600_cs.o: $(obj)/r600_reg_safe.h
 
-$(obj)/evergreen_cs.o: $(obj)/evergreen_reg_safe.h
+$(obj)/evergreen_cs.o: $(obj)/evergreen_reg_safe.h $(obj)/cayman_reg_safe.h
 
 radeon-y := radeon_drv.o radeon_cp.o radeon_state.o radeon_mem.o \
 	radeon_irq.o r300_cmdbuf.o r600_cp.o
@@ -66,7 +69,7 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	r200.o radeon_legacy_tv.o r600_cs.o r600_blit.o r600_blit_shaders.o \
 	r600_blit_kms.o radeon_pm.o atombios_dp.o r600_audio.o r600_hdmi.o \
 	evergreen.o evergreen_cs.o evergreen_blit_shaders.o evergreen_blit_kms.o \
-	radeon_trace_points.o ni.o
+	radeon_trace_points.o ni.o cayman_blit_shaders.o
 
 radeon-$(CONFIG_COMPAT) += radeon_ioc32.o
 radeon-$(CONFIG_VGA_SWITCHEROO) += radeon_atpx_handler.o
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index a4e5e53e0a62..a2199fe9fa9b 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -1026,7 +1026,7 @@ static int dce4_crtc_do_set_base(struct drm_crtc *crtc,
 	 * just update base pointers
 	 */
 	obj = radeon_fb->obj;
-	rbo = obj->driver_private;
+	rbo = gem_to_radeon_bo(obj);
 	r = radeon_bo_reserve(rbo, false);
 	if (unlikely(r != 0))
 		return r;
@@ -1135,7 +1135,7 @@ static int dce4_crtc_do_set_base(struct drm_crtc *crtc,
 
 	if (!atomic && fb && fb != crtc->fb) {
 		radeon_fb = to_radeon_framebuffer(fb);
-		rbo = radeon_fb->obj->driver_private;
+		rbo = gem_to_radeon_bo(radeon_fb->obj);
 		r = radeon_bo_reserve(rbo, false);
 		if (unlikely(r != 0))
 			return r;
@@ -1181,7 +1181,7 @@ static int avivo_crtc_do_set_base(struct drm_crtc *crtc,
 	}
 
 	obj = radeon_fb->obj;
-	rbo = obj->driver_private;
+	rbo = gem_to_radeon_bo(obj);
 	r = radeon_bo_reserve(rbo, false);
 	if (unlikely(r != 0))
 		return r;
@@ -1292,7 +1292,7 @@ static int avivo_crtc_do_set_base(struct drm_crtc *crtc,
 
 	if (!atomic && fb && fb != crtc->fb) {
 		radeon_fb = to_radeon_framebuffer(fb);
-		rbo = radeon_fb->obj->driver_private;
+		rbo = gem_to_radeon_bo(radeon_fb->obj);
 		r = radeon_bo_reserve(rbo, false);
 		if (unlikely(r != 0))
 			return r;
diff --git a/drivers/gpu/drm/radeon/cayman_blit_shaders.c b/drivers/gpu/drm/radeon/cayman_blit_shaders.c
new file mode 100644
index 000000000000..e148ab04b80b
--- /dev/null
+++ b/drivers/gpu/drm/radeon/cayman_blit_shaders.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright 2010 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>
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+
+/*
+ * evergreen cards need to use the 3D engine to blit data which requires
+ * quite a bit of hw state setup.  Rather than pull the whole 3D driver
+ * (which normally generates the 3D state) into the DRM, we opt to use
+ * statically generated state tables.  The regsiter state and shaders
+ * were hand generated to support blitting functionality.  See the 3D
+ * driver or documentation for descriptions of the registers and
+ * shader instructions.
+ */
+
+const u32 cayman_default_state[] =
+{
+	/* XXX fill in additional blit state */
+
+	0xc0026900,
+	0x00000316,
+	0x0000000e, /* VGT_VERTEX_REUSE_BLOCK_CNTL */
+	0x00000010, /*  */
+
+	0xc0026900,
+	0x000000d9,
+	0x00000000, /* CP_RINGID */
+	0x00000000, /* CP_VMID */
+};
+
+const u32 cayman_default_size = ARRAY_SIZE(cayman_default_state);
diff --git a/drivers/gpu/drm/radeon/cayman_blit_shaders.h b/drivers/gpu/drm/radeon/cayman_blit_shaders.h
new file mode 100644
index 000000000000..33b75e5d0fa4
--- /dev/null
+++ b/drivers/gpu/drm/radeon/cayman_blit_shaders.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright 2010 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.
+ *
+ */
+
+#ifndef CAYMAN_BLIT_SHADERS_H
+#define CAYMAN_BLIT_SHADERS_H
+
+extern const u32 cayman_default_state[];
+
+extern const u32 cayman_default_size;
+
+#endif
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index d270b3ff896b..fcfa621d1274 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -804,7 +804,7 @@ void evergreen_bandwidth_update(struct radeon_device *rdev)
 	}
 }
 
-static int evergreen_mc_wait_for_idle(struct radeon_device *rdev)
+int evergreen_mc_wait_for_idle(struct radeon_device *rdev)
 {
 	unsigned i;
 	u32 tmp;
@@ -957,7 +957,7 @@ void evergreen_agp_enable(struct radeon_device *rdev)
 	WREG32(VM_CONTEXT1_CNTL, 0);
 }
 
-static void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_save *save)
+void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_save *save)
 {
 	save->vga_control[0] = RREG32(D1VGA_CONTROL);
 	save->vga_control[1] = RREG32(D2VGA_CONTROL);
@@ -1011,7 +1011,7 @@ static void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_sa
 	WREG32(EVERGREEN_D6VGA_CONTROL, 0);
 }
 
-static void evergreen_mc_resume(struct radeon_device *rdev, struct evergreen_mc_save *save)
+void evergreen_mc_resume(struct radeon_device *rdev, struct evergreen_mc_save *save)
 {
 	WREG32(EVERGREEN_GRPH_PRIMARY_SURFACE_ADDRESS_HIGH + EVERGREEN_CRTC0_REGISTER_OFFSET,
 	       upper_32_bits(rdev->mc.vram_start));
@@ -1108,7 +1108,7 @@ static void evergreen_mc_resume(struct radeon_device *rdev, struct evergreen_mc_
 	WREG32(VGA_RENDER_CONTROL, save->vga_render_control);
 }
 
-static void evergreen_mc_program(struct radeon_device *rdev)
+void evergreen_mc_program(struct radeon_device *rdev)
 {
 	struct evergreen_mc_save save;
 	u32 tmp;
@@ -2577,7 +2577,7 @@ void evergreen_irq_disable(struct radeon_device *rdev)
 	evergreen_disable_interrupt_state(rdev);
 }
 
-static void evergreen_irq_suspend(struct radeon_device *rdev)
+void evergreen_irq_suspend(struct radeon_device *rdev)
 {
 	evergreen_irq_disable(rdev);
 	r600_rlc_stop(rdev);
@@ -2900,7 +2900,7 @@ static int evergreen_startup(struct radeon_device *rdev)
 				return r;
 			}
 		}
-		r = btc_mc_load_microcode(rdev);
+		r = ni_mc_load_microcode(rdev);
 		if (r) {
 			DRM_ERROR("Failed to load MC firmware!\n");
 			return r;
@@ -2982,13 +2982,13 @@ int evergreen_resume(struct radeon_device *rdev)
 
 	r = evergreen_startup(rdev);
 	if (r) {
-		DRM_ERROR("r600 startup failed on resume\n");
+		DRM_ERROR("evergreen startup failed on resume\n");
 		return r;
 	}
 
 	r = r600_ib_test(rdev);
 	if (r) {
-		DRM_ERROR("radeon: failled testing IB (%d).\n", r);
+		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
 		return r;
 	}
 
@@ -3062,7 +3062,7 @@ int evergreen_init(struct radeon_device *rdev)
 	}
 	/* Must be an ATOMBIOS */
 	if (!rdev->is_atom_bios) {
-		dev_err(rdev->dev, "Expecting atombios for R600 GPU\n");
+		dev_err(rdev->dev, "Expecting atombios for evergreen GPU\n");
 		return -EINVAL;
 	}
 	r = radeon_atombios_init(rdev);
diff --git a/drivers/gpu/drm/radeon/evergreen_blit_kms.c b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
index 2adfb03f479b..3218287f4c51 100644
--- a/drivers/gpu/drm/radeon/evergreen_blit_kms.c
+++ b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
@@ -579,7 +579,7 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	obj_size += evergreen_ps_size * 4;
 	obj_size = ALIGN(obj_size, 256);
 
-	r = radeon_bo_create(rdev, NULL, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
+	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
 				&rdev->r600_blit.shader_obj);
 	if (r) {
 		DRM_ERROR("evergreen failed to allocate shader\n");
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
index 345a75a03c96..5e4f9f876d4f 100644
--- a/drivers/gpu/drm/radeon/evergreen_cs.c
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -29,6 +29,7 @@
 #include "radeon.h"
 #include "evergreend.h"
 #include "evergreen_reg_safe.h"
+#include "cayman_reg_safe.h"
 
 static int evergreen_cs_packet_next_reloc(struct radeon_cs_parser *p,
 					  struct radeon_cs_reloc **cs_reloc);
@@ -425,18 +426,28 @@ static inline int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u3
 {
 	struct evergreen_cs_track *track = (struct evergreen_cs_track *)p->track;
 	struct radeon_cs_reloc *reloc;
-	u32 last_reg = ARRAY_SIZE(evergreen_reg_safe_bm);
+	u32 last_reg;
 	u32 m, i, tmp, *ib;
 	int r;
 
+	if (p->rdev->family >= CHIP_CAYMAN)
+		last_reg = ARRAY_SIZE(cayman_reg_safe_bm);
+	else
+		last_reg = ARRAY_SIZE(evergreen_reg_safe_bm);
+
 	i = (reg >> 7);
 	if (i > last_reg) {
 		dev_warn(p->dev, "forbidden register 0x%08x at %d\n", reg, idx);
 		return -EINVAL;
 	}
 	m = 1 << ((reg >> 2) & 31);
-	if (!(evergreen_reg_safe_bm[i] & m))
-		return 0;
+	if (p->rdev->family >= CHIP_CAYMAN) {
+		if (!(cayman_reg_safe_bm[i] & m))
+			return 0;
+	} else {
+		if (!(evergreen_reg_safe_bm[i] & m))
+			return 0;
+	}
 	ib = p->ib->ptr;
 	switch (reg) {
 	/* force following reg to 0 in an attemp to disable out buffer
@@ -468,12 +479,42 @@ static inline int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u3
 	case SQ_VSTMP_RING_ITEMSIZE:
 	case VGT_TF_RING_SIZE:
 		/* get value to populate the IB don't remove */
-		tmp =radeon_get_ib_value(p, idx);
-		ib[idx] = 0;
+		/*tmp =radeon_get_ib_value(p, idx);
+		  ib[idx] = 0;*/
+		break;
+	case SQ_ESGS_RING_BASE:
+	case SQ_GSVS_RING_BASE:
+	case SQ_ESTMP_RING_BASE:
+	case SQ_GSTMP_RING_BASE:
+	case SQ_HSTMP_RING_BASE:
+	case SQ_LSTMP_RING_BASE:
+	case SQ_PSTMP_RING_BASE:
+	case SQ_VSTMP_RING_BASE:
+		r = evergreen_cs_packet_next_reloc(p, &reloc);
+		if (r) {
+			dev_warn(p->dev, "bad SET_CONTEXT_REG "
+					"0x%04X\n", reg);
+			return -EINVAL;
+		}
+		ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
 		break;
 	case DB_DEPTH_CONTROL:
 		track->db_depth_control = radeon_get_ib_value(p, idx);
 		break;
+	case CAYMAN_DB_EQAA:
+		if (p->rdev->family < CHIP_CAYMAN) {
+			dev_warn(p->dev, "bad SET_CONTEXT_REG "
+				 "0x%04X\n", reg);
+			return -EINVAL;
+		}
+		break;
+	case CAYMAN_DB_DEPTH_INFO:
+		if (p->rdev->family < CHIP_CAYMAN) {
+			dev_warn(p->dev, "bad SET_CONTEXT_REG "
+				 "0x%04X\n", reg);
+			return -EINVAL;
+		}
+		break;
 	case DB_Z_INFO:
 		r = evergreen_cs_packet_next_reloc(p, &reloc);
 		if (r) {
@@ -559,9 +600,23 @@ static inline int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u3
 		track->cb_shader_mask = radeon_get_ib_value(p, idx);
 		break;
 	case PA_SC_AA_CONFIG:
+		if (p->rdev->family >= CHIP_CAYMAN) {
+			dev_warn(p->dev, "bad SET_CONTEXT_REG "
+				 "0x%04X\n", reg);
+			return -EINVAL;
+		}
 		tmp = radeon_get_ib_value(p, idx) & MSAA_NUM_SAMPLES_MASK;
 		track->nsamples = 1 << tmp;
 		break;
+	case CAYMAN_PA_SC_AA_CONFIG:
+		if (p->rdev->family < CHIP_CAYMAN) {
+			dev_warn(p->dev, "bad SET_CONTEXT_REG "
+				 "0x%04X\n", reg);
+			return -EINVAL;
+		}
+		tmp = radeon_get_ib_value(p, idx) & CAYMAN_MSAA_NUM_SAMPLES_MASK;
+		track->nsamples = 1 << tmp;
+		break;
 	case CB_COLOR0_VIEW:
 	case CB_COLOR1_VIEW:
 	case CB_COLOR2_VIEW:
@@ -942,6 +997,37 @@ static int evergreen_packet3_check(struct radeon_cs_parser *p,
 	idx_value = radeon_get_ib_value(p, idx);
 
 	switch (pkt->opcode) {
+	case PACKET3_SET_PREDICATION:
+	{
+		int pred_op;
+		int tmp;
+		if (pkt->count != 1) {
+			DRM_ERROR("bad SET PREDICATION\n");
+			return -EINVAL;
+		}
+
+		tmp = radeon_get_ib_value(p, idx + 1);
+		pred_op = (tmp >> 16) & 0x7;
+
+		/* for the clear predicate operation */
+		if (pred_op == 0)
+			return 0;
+
+		if (pred_op > 2) {
+			DRM_ERROR("bad SET PREDICATION operation %d\n", pred_op);
+			return -EINVAL;
+		}
+
+		r = evergreen_cs_packet_next_reloc(p, &reloc);
+		if (r) {
+			DRM_ERROR("bad SET PREDICATION\n");
+			return -EINVAL;
+		}
+
+		ib[idx + 0] = idx_value + (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+		ib[idx + 1] = tmp + (upper_32_bits(reloc->lobj.gpu_offset) & 0xff);
+	}
+	break;
 	case PACKET3_CONTEXT_CONTROL:
 		if (pkt->count != 1) {
 			DRM_ERROR("bad CONTEXT_CONTROL\n");
@@ -956,6 +1042,16 @@ static int evergreen_packet3_check(struct radeon_cs_parser *p,
 			return -EINVAL;
 		}
 		break;
+	case CAYMAN_PACKET3_DEALLOC_STATE:
+		if (p->rdev->family < CHIP_CAYMAN) {
+			DRM_ERROR("bad PACKET3_DEALLOC_STATE\n");
+			return -EINVAL;
+		}
+		if (pkt->count) {
+			DRM_ERROR("bad INDEX_TYPE/NUM_INSTANCES/CLEAR_STATE\n");
+			return -EINVAL;
+		}
+		break;
 	case PACKET3_INDEX_BASE:
 		if (pkt->count != 1) {
 			DRM_ERROR("bad INDEX_BASE\n");
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index eb4acf4528ff..9aaa3f0c9372 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -755,13 +755,21 @@
 
 #define SQ_CONST_MEM_BASE				0x8df8
 
+#define SQ_ESGS_RING_BASE				0x8c40
 #define SQ_ESGS_RING_SIZE				0x8c44
+#define SQ_GSVS_RING_BASE				0x8c48
 #define SQ_GSVS_RING_SIZE				0x8c4c
+#define SQ_ESTMP_RING_BASE				0x8c50
 #define SQ_ESTMP_RING_SIZE				0x8c54
+#define SQ_GSTMP_RING_BASE				0x8c58
 #define SQ_GSTMP_RING_SIZE				0x8c5c
+#define SQ_VSTMP_RING_BASE				0x8c60
 #define SQ_VSTMP_RING_SIZE				0x8c64
+#define SQ_PSTMP_RING_BASE				0x8c68
 #define SQ_PSTMP_RING_SIZE				0x8c6c
+#define SQ_LSTMP_RING_BASE				0x8e10
 #define SQ_LSTMP_RING_SIZE				0x8e14
+#define SQ_HSTMP_RING_BASE				0x8e18
 #define SQ_HSTMP_RING_SIZE				0x8e1c
 #define VGT_TF_RING_SIZE				0x8988
 
@@ -1093,5 +1101,14 @@
 #define SQ_TEX_RESOURCE_WORD6_0                         0x30018
 #define SQ_TEX_RESOURCE_WORD7_0                         0x3001c
 
+/* cayman 3D regs */
+#define CAYMAN_VGT_OFFCHIP_LDS_BASE			0x89B0
+#define CAYMAN_DB_EQAA					0x28804
+#define CAYMAN_DB_DEPTH_INFO				0x2803C
+#define CAYMAN_PA_SC_AA_CONFIG				0x28BE0
+#define         CAYMAN_MSAA_NUM_SAMPLES_SHIFT           0
+#define         CAYMAN_MSAA_NUM_SAMPLES_MASK            0x7
+/* cayman packet3 addition */
+#define	CAYMAN_PACKET3_DEALLOC_STATE			0x14
 
 #endif
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 5e0bef80ad7f..8c199c49731b 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -31,12 +31,25 @@
 #include "nid.h"
 #include "atom.h"
 #include "ni_reg.h"
+#include "cayman_blit_shaders.h"
+
+extern void evergreen_mc_stop(struct radeon_device *rdev, struct evergreen_mc_save *save);
+extern void evergreen_mc_resume(struct radeon_device *rdev, struct evergreen_mc_save *save);
+extern int evergreen_mc_wait_for_idle(struct radeon_device *rdev);
+extern void evergreen_mc_program(struct radeon_device *rdev);
+extern void evergreen_irq_suspend(struct radeon_device *rdev);
+extern int evergreen_mc_init(struct radeon_device *rdev);
 
 #define EVERGREEN_PFP_UCODE_SIZE 1120
 #define EVERGREEN_PM4_UCODE_SIZE 1376
 #define EVERGREEN_RLC_UCODE_SIZE 768
 #define BTC_MC_UCODE_SIZE 6024
 
+#define CAYMAN_PFP_UCODE_SIZE 2176
+#define CAYMAN_PM4_UCODE_SIZE 2176
+#define CAYMAN_RLC_UCODE_SIZE 1024
+#define CAYMAN_MC_UCODE_SIZE 6037
+
 /* Firmware Names */
 MODULE_FIRMWARE("radeon/BARTS_pfp.bin");
 MODULE_FIRMWARE("radeon/BARTS_me.bin");
@@ -48,6 +61,10 @@ MODULE_FIRMWARE("radeon/TURKS_mc.bin");
 MODULE_FIRMWARE("radeon/CAICOS_pfp.bin");
 MODULE_FIRMWARE("radeon/CAICOS_me.bin");
 MODULE_FIRMWARE("radeon/CAICOS_mc.bin");
+MODULE_FIRMWARE("radeon/CAYMAN_pfp.bin");
+MODULE_FIRMWARE("radeon/CAYMAN_me.bin");
+MODULE_FIRMWARE("radeon/CAYMAN_mc.bin");
+MODULE_FIRMWARE("radeon/CAYMAN_rlc.bin");
 
 #define BTC_IO_MC_REGS_SIZE 29
 
@@ -147,12 +164,44 @@ static const u32 caicos_io_mc_regs[BTC_IO_MC_REGS_SIZE][2] = {
 	{0x0000009f, 0x00916a00}
 };
 
-int btc_mc_load_microcode(struct radeon_device *rdev)
+static const u32 cayman_io_mc_regs[BTC_IO_MC_REGS_SIZE][2] = {
+	{0x00000077, 0xff010100},
+	{0x00000078, 0x00000000},
+	{0x00000079, 0x00001434},
+	{0x0000007a, 0xcc08ec08},
+	{0x0000007b, 0x00040000},
+	{0x0000007c, 0x000080c0},
+	{0x0000007d, 0x09000000},
+	{0x0000007e, 0x00210404},
+	{0x00000081, 0x08a8e800},
+	{0x00000082, 0x00030444},
+	{0x00000083, 0x00000000},
+	{0x00000085, 0x00000001},
+	{0x00000086, 0x00000002},
+	{0x00000087, 0x48490000},
+	{0x00000088, 0x20244647},
+	{0x00000089, 0x00000005},
+	{0x0000008b, 0x66030000},
+	{0x0000008c, 0x00006603},
+	{0x0000008d, 0x00000100},
+	{0x0000008f, 0x00001c0a},
+	{0x00000090, 0xff000001},
+	{0x00000094, 0x00101101},
+	{0x00000095, 0x00000fff},
+	{0x00000096, 0x00116fff},
+	{0x00000097, 0x60010000},
+	{0x00000098, 0x10010000},
+	{0x00000099, 0x00006000},
+	{0x0000009a, 0x00001000},
+	{0x0000009f, 0x00976b00}
+};
+
+int ni_mc_load_microcode(struct radeon_device *rdev)
 {
 	const __be32 *fw_data;
 	u32 mem_type, running, blackout = 0;
 	u32 *io_mc_regs;
-	int i;
+	int i, ucode_size, regs_size;
 
 	if (!rdev->mc_fw)
 		return -EINVAL;
@@ -160,13 +209,24 @@ int btc_mc_load_microcode(struct radeon_device *rdev)
 	switch (rdev->family) {
 	case CHIP_BARTS:
 		io_mc_regs = (u32 *)&barts_io_mc_regs;
+		ucode_size = BTC_MC_UCODE_SIZE;
+		regs_size = BTC_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_TURKS:
 		io_mc_regs = (u32 *)&turks_io_mc_regs;
+		ucode_size = BTC_MC_UCODE_SIZE;
+		regs_size = BTC_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_CAICOS:
 	default:
 		io_mc_regs = (u32 *)&caicos_io_mc_regs;
+		ucode_size = BTC_MC_UCODE_SIZE;
+		regs_size = BTC_IO_MC_REGS_SIZE;
+		break;
+	case CHIP_CAYMAN:
+		io_mc_regs = (u32 *)&cayman_io_mc_regs;
+		ucode_size = CAYMAN_MC_UCODE_SIZE;
+		regs_size = BTC_IO_MC_REGS_SIZE;
 		break;
 	}
 
@@ -184,13 +244,13 @@ int btc_mc_load_microcode(struct radeon_device *rdev)
 		WREG32(MC_SEQ_SUP_CNTL, 0x00000010);
 
 		/* load mc io regs */
-		for (i = 0; i < BTC_IO_MC_REGS_SIZE; i++) {
+		for (i = 0; i < regs_size; i++) {
 			WREG32(MC_SEQ_IO_DEBUG_INDEX, io_mc_regs[(i << 1)]);
 			WREG32(MC_SEQ_IO_DEBUG_DATA, io_mc_regs[(i << 1) + 1]);
 		}
 		/* load the MC ucode */
 		fw_data = (const __be32 *)rdev->mc_fw->data;
-		for (i = 0; i < BTC_MC_UCODE_SIZE; i++)
+		for (i = 0; i < ucode_size; i++)
 			WREG32(MC_SEQ_SUP_PGM, be32_to_cpup(fw_data++));
 
 		/* put the engine back into the active state */
@@ -231,23 +291,38 @@ int ni_init_microcode(struct radeon_device *rdev)
 	case CHIP_BARTS:
 		chip_name = "BARTS";
 		rlc_chip_name = "BTC";
+		pfp_req_size = EVERGREEN_PFP_UCODE_SIZE * 4;
+		me_req_size = EVERGREEN_PM4_UCODE_SIZE * 4;
+		rlc_req_size = EVERGREEN_RLC_UCODE_SIZE * 4;
+		mc_req_size = BTC_MC_UCODE_SIZE * 4;
 		break;
 	case CHIP_TURKS:
 		chip_name = "TURKS";
 		rlc_chip_name = "BTC";
+		pfp_req_size = EVERGREEN_PFP_UCODE_SIZE * 4;
+		me_req_size = EVERGREEN_PM4_UCODE_SIZE * 4;
+		rlc_req_size = EVERGREEN_RLC_UCODE_SIZE * 4;
+		mc_req_size = BTC_MC_UCODE_SIZE * 4;
 		break;
 	case CHIP_CAICOS:
 		chip_name = "CAICOS";
 		rlc_chip_name = "BTC";
+		pfp_req_size = EVERGREEN_PFP_UCODE_SIZE * 4;
+		me_req_size = EVERGREEN_PM4_UCODE_SIZE * 4;
+		rlc_req_size = EVERGREEN_RLC_UCODE_SIZE * 4;
+		mc_req_size = BTC_MC_UCODE_SIZE * 4;
+		break;
+	case CHIP_CAYMAN:
+		chip_name = "CAYMAN";
+		rlc_chip_name = "CAYMAN";
+		pfp_req_size = CAYMAN_PFP_UCODE_SIZE * 4;
+		me_req_size = CAYMAN_PM4_UCODE_SIZE * 4;
+		rlc_req_size = CAYMAN_RLC_UCODE_SIZE * 4;
+		mc_req_size = CAYMAN_MC_UCODE_SIZE * 4;
 		break;
 	default: BUG();
 	}
 
-	pfp_req_size = EVERGREEN_PFP_UCODE_SIZE * 4;
-	me_req_size = EVERGREEN_PM4_UCODE_SIZE * 4;
-	rlc_req_size = EVERGREEN_RLC_UCODE_SIZE * 4;
-	mc_req_size = BTC_MC_UCODE_SIZE * 4;
-
 	DRM_INFO("Loading %s Microcode\n", chip_name);
 
 	snprintf(fw_name, sizeof(fw_name), "radeon/%s_pfp.bin", chip_name);
@@ -314,3 +389,1204 @@ out:
 	return err;
 }
 
+/*
+ * Core functions
+ */
+static u32 cayman_get_tile_pipe_to_backend_map(struct radeon_device *rdev,
+					       u32 num_tile_pipes,
+					       u32 num_backends_per_asic,
+					       u32 *backend_disable_mask_per_asic,
+					       u32 num_shader_engines)
+{
+	u32 backend_map = 0;
+	u32 enabled_backends_mask = 0;
+	u32 enabled_backends_count = 0;
+	u32 num_backends_per_se;
+	u32 cur_pipe;
+	u32 swizzle_pipe[CAYMAN_MAX_PIPES];
+	u32 cur_backend = 0;
+	u32 i;
+	bool force_no_swizzle;
+
+	/* force legal values */
+	if (num_tile_pipes < 1)
+		num_tile_pipes = 1;
+	if (num_tile_pipes > rdev->config.cayman.max_tile_pipes)
+		num_tile_pipes = rdev->config.cayman.max_tile_pipes;
+	if (num_shader_engines < 1)
+		num_shader_engines = 1;
+	if (num_shader_engines > rdev->config.cayman.max_shader_engines)
+		num_shader_engines = rdev->config.cayman.max_shader_engines;
+	if (num_backends_per_asic > num_shader_engines)
+		num_backends_per_asic = num_shader_engines;
+	if (num_backends_per_asic > (rdev->config.cayman.max_backends_per_se * num_shader_engines))
+		num_backends_per_asic = rdev->config.cayman.max_backends_per_se * num_shader_engines;
+
+	/* make sure we have the same number of backends per se */
+	num_backends_per_asic = ALIGN(num_backends_per_asic, num_shader_engines);
+	/* set up the number of backends per se */
+	num_backends_per_se = num_backends_per_asic / num_shader_engines;
+	if (num_backends_per_se > rdev->config.cayman.max_backends_per_se) {
+		num_backends_per_se = rdev->config.cayman.max_backends_per_se;
+		num_backends_per_asic = num_backends_per_se * num_shader_engines;
+	}
+
+	/* create enable mask and count for enabled backends */
+	for (i = 0; i < CAYMAN_MAX_BACKENDS; ++i) {
+		if (((*backend_disable_mask_per_asic >> i) & 1) == 0) {
+			enabled_backends_mask |= (1 << i);
+			++enabled_backends_count;
+		}
+		if (enabled_backends_count == num_backends_per_asic)
+			break;
+	}
+
+	/* force the backends mask to match the current number of backends */
+	if (enabled_backends_count != num_backends_per_asic) {
+		u32 this_backend_enabled;
+		u32 shader_engine;
+		u32 backend_per_se;
+
+		enabled_backends_mask = 0;
+		enabled_backends_count = 0;
+		*backend_disable_mask_per_asic = CAYMAN_MAX_BACKENDS_MASK;
+		for (i = 0; i < CAYMAN_MAX_BACKENDS; ++i) {
+			/* calc the current se */
+			shader_engine = i / rdev->config.cayman.max_backends_per_se;
+			/* calc the backend per se */
+			backend_per_se = i % rdev->config.cayman.max_backends_per_se;
+			/* default to not enabled */
+			this_backend_enabled = 0;
+			if ((shader_engine < num_shader_engines) &&
+			    (backend_per_se < num_backends_per_se))
+				this_backend_enabled = 1;
+			if (this_backend_enabled) {
+				enabled_backends_mask |= (1 << i);
+				*backend_disable_mask_per_asic &= ~(1 << i);
+				++enabled_backends_count;
+			}
+		}
+	}
+
+
+	memset((uint8_t *)&swizzle_pipe[0], 0, sizeof(u32) * CAYMAN_MAX_PIPES);
+	switch (rdev->family) {
+	case CHIP_CAYMAN:
+		force_no_swizzle = true;
+		break;
+	default:
+		force_no_swizzle = false;
+		break;
+	}
+	if (force_no_swizzle) {
+		bool last_backend_enabled = false;
+
+		force_no_swizzle = false;
+		for (i = 0; i < CAYMAN_MAX_BACKENDS; ++i) {
+			if (((enabled_backends_mask >> i) & 1) == 1) {
+				if (last_backend_enabled)
+					force_no_swizzle = true;
+				last_backend_enabled = true;
+			} else
+				last_backend_enabled = false;
+		}
+	}
+
+	switch (num_tile_pipes) {
+	case 1:
+	case 3:
+	case 5:
+	case 7:
+		DRM_ERROR("odd number of pipes!\n");
+		break;
+	case 2:
+		swizzle_pipe[0] = 0;
+		swizzle_pipe[1] = 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] = 1;
+			swizzle_pipe[3] = 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] = 1;
+			swizzle_pipe[4] = 3;
+			swizzle_pipe[5] = 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] = 1;
+			swizzle_pipe[5] = 3;
+			swizzle_pipe[6] = 5;
+			swizzle_pipe[7] = 7;
+		}
+		break;
+	}
+
+	for (cur_pipe = 0; cur_pipe < num_tile_pipes; ++cur_pipe) {
+		while (((1 << cur_backend) & enabled_backends_mask) == 0)
+			cur_backend = (cur_backend + 1) % CAYMAN_MAX_BACKENDS;
+
+		backend_map |= (((cur_backend & 0xf) << (swizzle_pipe[cur_pipe] * 4)));
+
+		cur_backend = (cur_backend + 1) % CAYMAN_MAX_BACKENDS;
+	}
+
+	return backend_map;
+}
+
+static void cayman_program_channel_remap(struct radeon_device *rdev)
+{
+	u32 tcp_chan_steer_lo, tcp_chan_steer_hi, mc_shared_chremap, tmp;
+
+	tmp = RREG32(MC_SHARED_CHMAP);
+	switch ((tmp & NOOFCHAN_MASK) >> NOOFCHAN_SHIFT) {
+	case 0:
+	case 1:
+	case 2:
+	case 3:
+	default:
+		/* default mapping */
+		mc_shared_chremap = 0x00fac688;
+		break;
+	}
+
+	switch (rdev->family) {
+	case CHIP_CAYMAN:
+	default:
+		//tcp_chan_steer_lo = 0x54763210
+		tcp_chan_steer_lo = 0x76543210;
+		tcp_chan_steer_hi = 0x0000ba98;
+		break;
+	}
+
+	WREG32(TCP_CHAN_STEER_LO, tcp_chan_steer_lo);
+	WREG32(TCP_CHAN_STEER_HI, tcp_chan_steer_hi);
+	WREG32(MC_SHARED_CHREMAP, mc_shared_chremap);
+}
+
+static u32 cayman_get_disable_mask_per_asic(struct radeon_device *rdev,
+					    u32 disable_mask_per_se,
+					    u32 max_disable_mask_per_se,
+					    u32 num_shader_engines)
+{
+	u32 disable_field_width_per_se = r600_count_pipe_bits(disable_mask_per_se);
+	u32 disable_mask_per_asic = disable_mask_per_se & max_disable_mask_per_se;
+
+	if (num_shader_engines == 1)
+		return disable_mask_per_asic;
+	else if (num_shader_engines == 2)
+		return disable_mask_per_asic | (disable_mask_per_asic << disable_field_width_per_se);
+	else
+		return 0xffffffff;
+}
+
+static void cayman_gpu_init(struct radeon_device *rdev)
+{
+	u32 cc_rb_backend_disable = 0;
+	u32 cc_gc_shader_pipe_config;
+	u32 gb_addr_config = 0;
+	u32 mc_shared_chmap, mc_arb_ramcfg;
+	u32 gb_backend_map;
+	u32 cgts_tcc_disable;
+	u32 sx_debug_1;
+	u32 smx_dc_ctl0;
+	u32 gc_user_shader_pipe_config;
+	u32 gc_user_rb_backend_disable;
+	u32 cgts_user_tcc_disable;
+	u32 cgts_sm_ctrl_reg;
+	u32 hdp_host_path_cntl;
+	u32 tmp;
+	int i, j;
+
+	switch (rdev->family) {
+	case CHIP_CAYMAN:
+	default:
+		rdev->config.cayman.max_shader_engines = 2;
+		rdev->config.cayman.max_pipes_per_simd = 4;
+		rdev->config.cayman.max_tile_pipes = 8;
+		rdev->config.cayman.max_simds_per_se = 12;
+		rdev->config.cayman.max_backends_per_se = 4;
+		rdev->config.cayman.max_texture_channel_caches = 8;
+		rdev->config.cayman.max_gprs = 256;
+		rdev->config.cayman.max_threads = 256;
+		rdev->config.cayman.max_gs_threads = 32;
+		rdev->config.cayman.max_stack_entries = 512;
+		rdev->config.cayman.sx_num_of_sets = 8;
+		rdev->config.cayman.sx_max_export_size = 256;
+		rdev->config.cayman.sx_max_export_pos_size = 64;
+		rdev->config.cayman.sx_max_export_smx_size = 192;
+		rdev->config.cayman.max_hw_contexts = 8;
+		rdev->config.cayman.sq_num_cf_insts = 2;
+
+		rdev->config.cayman.sc_prim_fifo_size = 0x100;
+		rdev->config.cayman.sc_hiz_tile_fifo_size = 0x30;
+		rdev->config.cayman.sc_earlyz_tile_fifo_size = 0x130;
+		break;
+	}
+
+	/* Initialize HDP */
+	for (i = 0, j = 0; i < 32; i++, j += 0x18) {
+		WREG32((0x2c14 + j), 0x00000000);
+		WREG32((0x2c18 + j), 0x00000000);
+		WREG32((0x2c1c + j), 0x00000000);
+		WREG32((0x2c20 + j), 0x00000000);
+		WREG32((0x2c24 + j), 0x00000000);
+	}
+
+	WREG32(GRBM_CNTL, GRBM_READ_TIMEOUT(0xff));
+
+	mc_shared_chmap = RREG32(MC_SHARED_CHMAP);
+	mc_arb_ramcfg = RREG32(MC_ARB_RAMCFG);
+
+	cc_rb_backend_disable = RREG32(CC_RB_BACKEND_DISABLE);
+	cc_gc_shader_pipe_config = RREG32(CC_GC_SHADER_PIPE_CONFIG);
+	cgts_tcc_disable = RREG32(CGTS_TCC_DISABLE);
+	gc_user_rb_backend_disable = RREG32(GC_USER_RB_BACKEND_DISABLE);
+	gc_user_shader_pipe_config = RREG32(GC_USER_SHADER_PIPE_CONFIG);
+	cgts_user_tcc_disable = RREG32(CGTS_USER_TCC_DISABLE);
+
+	rdev->config.cayman.num_shader_engines = rdev->config.cayman.max_shader_engines;
+	tmp = ((~gc_user_shader_pipe_config) & INACTIVE_QD_PIPES_MASK) >> INACTIVE_QD_PIPES_SHIFT;
+	rdev->config.cayman.num_shader_pipes_per_simd = r600_count_pipe_bits(tmp);
+	rdev->config.cayman.num_tile_pipes = rdev->config.cayman.max_tile_pipes;
+	tmp = ((~gc_user_shader_pipe_config) & INACTIVE_SIMDS_MASK) >> INACTIVE_SIMDS_SHIFT;
+	rdev->config.cayman.num_simds_per_se = r600_count_pipe_bits(tmp);
+	tmp = ((~gc_user_rb_backend_disable) & BACKEND_DISABLE_MASK) >> BACKEND_DISABLE_SHIFT;
+	rdev->config.cayman.num_backends_per_se = r600_count_pipe_bits(tmp);
+	tmp = (gc_user_rb_backend_disable & BACKEND_DISABLE_MASK) >> BACKEND_DISABLE_SHIFT;
+	rdev->config.cayman.backend_disable_mask_per_asic =
+		cayman_get_disable_mask_per_asic(rdev, tmp, CAYMAN_MAX_BACKENDS_PER_SE_MASK,
+						 rdev->config.cayman.num_shader_engines);
+	rdev->config.cayman.backend_map =
+		cayman_get_tile_pipe_to_backend_map(rdev, rdev->config.cayman.num_tile_pipes,
+						    rdev->config.cayman.num_backends_per_se *
+						    rdev->config.cayman.num_shader_engines,
+						    &rdev->config.cayman.backend_disable_mask_per_asic,
+						    rdev->config.cayman.num_shader_engines);
+	tmp = ((~cgts_user_tcc_disable) & TCC_DISABLE_MASK) >> TCC_DISABLE_SHIFT;
+	rdev->config.cayman.num_texture_channel_caches = r600_count_pipe_bits(tmp);
+	tmp = (mc_arb_ramcfg & BURSTLENGTH_MASK) >> BURSTLENGTH_SHIFT;
+	rdev->config.cayman.mem_max_burst_length_bytes = (tmp + 1) * 256;
+	if (rdev->config.cayman.mem_max_burst_length_bytes > 512)
+		rdev->config.cayman.mem_max_burst_length_bytes = 512;
+	tmp = (mc_arb_ramcfg & NOOFCOLS_MASK) >> NOOFCOLS_SHIFT;
+	rdev->config.cayman.mem_row_size_in_kb = (4 * (1 << (8 + tmp))) / 1024;
+	if (rdev->config.cayman.mem_row_size_in_kb > 4)
+		rdev->config.cayman.mem_row_size_in_kb = 4;
+	/* XXX use MC settings? */
+	rdev->config.cayman.shader_engine_tile_size = 32;
+	rdev->config.cayman.num_gpus = 1;
+	rdev->config.cayman.multi_gpu_tile_size = 64;
+
+	//gb_addr_config = 0x02011003
+#if 0
+	gb_addr_config = RREG32(GB_ADDR_CONFIG);
+#else
+	gb_addr_config = 0;
+	switch (rdev->config.cayman.num_tile_pipes) {
+	case 1:
+	default:
+		gb_addr_config |= NUM_PIPES(0);
+		break;
+	case 2:
+		gb_addr_config |= NUM_PIPES(1);
+		break;
+	case 4:
+		gb_addr_config |= NUM_PIPES(2);
+		break;
+	case 8:
+		gb_addr_config |= NUM_PIPES(3);
+		break;
+	}
+
+	tmp = (rdev->config.cayman.mem_max_burst_length_bytes / 256) - 1;
+	gb_addr_config |= PIPE_INTERLEAVE_SIZE(tmp);
+	gb_addr_config |= NUM_SHADER_ENGINES(rdev->config.cayman.num_shader_engines - 1);
+	tmp = (rdev->config.cayman.shader_engine_tile_size / 16) - 1;
+	gb_addr_config |= SHADER_ENGINE_TILE_SIZE(tmp);
+	switch (rdev->config.cayman.num_gpus) {
+	case 1:
+	default:
+		gb_addr_config |= NUM_GPUS(0);
+		break;
+	case 2:
+		gb_addr_config |= NUM_GPUS(1);
+		break;
+	case 4:
+		gb_addr_config |= NUM_GPUS(2);
+		break;
+	}
+	switch (rdev->config.cayman.multi_gpu_tile_size) {
+	case 16:
+		gb_addr_config |= MULTI_GPU_TILE_SIZE(0);
+		break;
+	case 32:
+	default:
+		gb_addr_config |= MULTI_GPU_TILE_SIZE(1);
+		break;
+	case 64:
+		gb_addr_config |= MULTI_GPU_TILE_SIZE(2);
+		break;
+	case 128:
+		gb_addr_config |= MULTI_GPU_TILE_SIZE(3);
+		break;
+	}
+	switch (rdev->config.cayman.mem_row_size_in_kb) {
+	case 1:
+	default:
+		gb_addr_config |= ROW_SIZE(0);
+		break;
+	case 2:
+		gb_addr_config |= ROW_SIZE(1);
+		break;
+	case 4:
+		gb_addr_config |= ROW_SIZE(2);
+		break;
+	}
+#endif
+
+	tmp = (gb_addr_config & NUM_PIPES_MASK) >> NUM_PIPES_SHIFT;
+	rdev->config.cayman.num_tile_pipes = (1 << tmp);
+	tmp = (gb_addr_config & PIPE_INTERLEAVE_SIZE_MASK) >> PIPE_INTERLEAVE_SIZE_SHIFT;
+	rdev->config.cayman.mem_max_burst_length_bytes = (tmp + 1) * 256;
+	tmp = (gb_addr_config & NUM_SHADER_ENGINES_MASK) >> NUM_SHADER_ENGINES_SHIFT;
+	rdev->config.cayman.num_shader_engines = tmp + 1;
+	tmp = (gb_addr_config & NUM_GPUS_MASK) >> NUM_GPUS_SHIFT;
+	rdev->config.cayman.num_gpus = tmp + 1;
+	tmp = (gb_addr_config & MULTI_GPU_TILE_SIZE_MASK) >> MULTI_GPU_TILE_SIZE_SHIFT;
+	rdev->config.cayman.multi_gpu_tile_size = 1 << tmp;
+	tmp = (gb_addr_config & ROW_SIZE_MASK) >> ROW_SIZE_SHIFT;
+	rdev->config.cayman.mem_row_size_in_kb = 1 << tmp;
+
+	//gb_backend_map = 0x76541032;
+#if 0
+	gb_backend_map = RREG32(GB_BACKEND_MAP);
+#else
+	gb_backend_map =
+		cayman_get_tile_pipe_to_backend_map(rdev, rdev->config.cayman.num_tile_pipes,
+						    rdev->config.cayman.num_backends_per_se *
+						    rdev->config.cayman.num_shader_engines,
+						    &rdev->config.cayman.backend_disable_mask_per_asic,
+						    rdev->config.cayman.num_shader_engines);
+#endif
+	/* setup tiling info dword.  gb_addr_config is not adequate since it does
+	 * not have bank info, so create a custom tiling dword.
+	 * bits 3:0   num_pipes
+	 * bits 7:4   num_banks
+	 * bits 11:8  group_size
+	 * bits 15:12 row_size
+	 */
+	rdev->config.cayman.tile_config = 0;
+	switch (rdev->config.cayman.num_tile_pipes) {
+	case 1:
+	default:
+		rdev->config.cayman.tile_config |= (0 << 0);
+		break;
+	case 2:
+		rdev->config.cayman.tile_config |= (1 << 0);
+		break;
+	case 4:
+		rdev->config.cayman.tile_config |= (2 << 0);
+		break;
+	case 8:
+		rdev->config.cayman.tile_config |= (3 << 0);
+		break;
+	}
+	rdev->config.cayman.tile_config |=
+		((mc_arb_ramcfg & NOOFBANK_MASK) >> NOOFBANK_SHIFT) << 4;
+	rdev->config.cayman.tile_config |=
+		(gb_addr_config & PIPE_INTERLEAVE_SIZE_MASK) >> PIPE_INTERLEAVE_SIZE_SHIFT;
+	rdev->config.cayman.tile_config |=
+		((gb_addr_config & ROW_SIZE_MASK) >> ROW_SIZE_SHIFT) << 12;
+
+	WREG32(GB_BACKEND_MAP, gb_backend_map);
+	WREG32(GB_ADDR_CONFIG, gb_addr_config);
+	WREG32(DMIF_ADDR_CONFIG, gb_addr_config);
+	WREG32(HDP_ADDR_CONFIG, gb_addr_config);
+
+	cayman_program_channel_remap(rdev);
+
+	/* primary versions */
+	WREG32(CC_RB_BACKEND_DISABLE, cc_rb_backend_disable);
+	WREG32(CC_SYS_RB_BACKEND_DISABLE, cc_rb_backend_disable);
+	WREG32(CC_GC_SHADER_PIPE_CONFIG, cc_gc_shader_pipe_config);
+
+	WREG32(CGTS_TCC_DISABLE, cgts_tcc_disable);
+	WREG32(CGTS_SYS_TCC_DISABLE, cgts_tcc_disable);
+
+	/* user versions */
+	WREG32(GC_USER_RB_BACKEND_DISABLE, cc_rb_backend_disable);
+	WREG32(GC_USER_SYS_RB_BACKEND_DISABLE, cc_rb_backend_disable);
+	WREG32(GC_USER_SHADER_PIPE_CONFIG, cc_gc_shader_pipe_config);
+
+	WREG32(CGTS_USER_SYS_TCC_DISABLE, cgts_tcc_disable);
+	WREG32(CGTS_USER_TCC_DISABLE, cgts_tcc_disable);
+
+	/* reprogram the shader complex */
+	cgts_sm_ctrl_reg = RREG32(CGTS_SM_CTRL_REG);
+	for (i = 0; i < 16; i++)
+		WREG32(CGTS_SM_CTRL_REG, OVERRIDE);
+	WREG32(CGTS_SM_CTRL_REG, cgts_sm_ctrl_reg);
+
+	/* set HW defaults for 3D engine */
+	WREG32(CP_MEQ_THRESHOLDS, MEQ1_START(0x30) | MEQ2_START(0x60));
+
+	sx_debug_1 = RREG32(SX_DEBUG_1);
+	sx_debug_1 |= ENABLE_NEW_SMX_ADDRESS;
+	WREG32(SX_DEBUG_1, sx_debug_1);
+
+	smx_dc_ctl0 = RREG32(SMX_DC_CTL0);
+	smx_dc_ctl0 &= ~NUMBER_OF_SETS(0x1ff);
+	smx_dc_ctl0 |= NUMBER_OF_SETS(rdev->config.evergreen.sx_num_of_sets);
+	WREG32(SMX_DC_CTL0, smx_dc_ctl0);
+
+	WREG32(SPI_CONFIG_CNTL_1, VTX_DONE_DELAY(4) | CRC_SIMD_ID_WADDR_DISABLE);
+
+	/* need to be explicitly zero-ed */
+	WREG32(VGT_OFFCHIP_LDS_BASE, 0);
+	WREG32(SQ_LSTMP_RING_BASE, 0);
+	WREG32(SQ_HSTMP_RING_BASE, 0);
+	WREG32(SQ_ESTMP_RING_BASE, 0);
+	WREG32(SQ_GSTMP_RING_BASE, 0);
+	WREG32(SQ_VSTMP_RING_BASE, 0);
+	WREG32(SQ_PSTMP_RING_BASE, 0);
+
+	WREG32(TA_CNTL_AUX, DISABLE_CUBE_ANISO);
+
+	WREG32(SX_EXPORT_BUFFER_SIZES, (COLOR_BUFFER_SIZE((rdev->config.evergreen.sx_max_export_size / 4) - 1) |
+					POSITION_BUFFER_SIZE((rdev->config.evergreen.sx_max_export_pos_size / 4) - 1) |
+					SMX_BUFFER_SIZE((rdev->config.evergreen.sx_max_export_smx_size / 4) - 1)));
+
+	WREG32(PA_SC_FIFO_SIZE, (SC_PRIM_FIFO_SIZE(rdev->config.evergreen.sc_prim_fifo_size) |
+				 SC_HIZ_TILE_FIFO_SIZE(rdev->config.evergreen.sc_hiz_tile_fifo_size) |
+				 SC_EARLYZ_TILE_FIFO_SIZE(rdev->config.evergreen.sc_earlyz_tile_fifo_size)));
+
+
+	WREG32(VGT_NUM_INSTANCES, 1);
+
+	WREG32(CP_PERFMON_CNTL, 0);
+
+	WREG32(SQ_MS_FIFO_SIZES, (CACHE_FIFO_SIZE(16 * rdev->config.evergreen.sq_num_cf_insts) |
+				  FETCH_FIFO_HIWATER(0x4) |
+				  DONE_FIFO_HIWATER(0xe0) |
+				  ALU_UPDATE_FIFO_HIWATER(0x8)));
+
+	WREG32(SQ_GPR_RESOURCE_MGMT_1, NUM_CLAUSE_TEMP_GPRS(4));
+	WREG32(SQ_CONFIG, (VC_ENABLE |
+			   EXPORT_SRC_C |
+			   GFX_PRIO(0) |
+			   CS1_PRIO(0) |
+			   CS2_PRIO(1)));
+	WREG32(SQ_DYN_GPR_CNTL_PS_FLUSH_REQ, DYN_GPR_ENABLE);
+
+	WREG32(PA_SC_FORCE_EOV_MAX_CNTS, (FORCE_EOV_MAX_CLK_CNT(4095) |
+					  FORCE_EOV_MAX_REZ_CNT(255)));
+
+	WREG32(VGT_CACHE_INVALIDATION, CACHE_INVALIDATION(VC_AND_TC) |
+	       AUTO_INVLD_EN(ES_AND_GS_AUTO));
+
+	WREG32(VGT_GS_VERTEX_REUSE, 16);
+	WREG32(PA_SC_LINE_STIPPLE_STATE, 0);
+
+	WREG32(CB_PERF_CTR0_SEL_0, 0);
+	WREG32(CB_PERF_CTR0_SEL_1, 0);
+	WREG32(CB_PERF_CTR1_SEL_0, 0);
+	WREG32(CB_PERF_CTR1_SEL_1, 0);
+	WREG32(CB_PERF_CTR2_SEL_0, 0);
+	WREG32(CB_PERF_CTR2_SEL_1, 0);
+	WREG32(CB_PERF_CTR3_SEL_0, 0);
+	WREG32(CB_PERF_CTR3_SEL_1, 0);
+
+	hdp_host_path_cntl = RREG32(HDP_HOST_PATH_CNTL);
+	WREG32(HDP_HOST_PATH_CNTL, hdp_host_path_cntl);
+
+	WREG32(PA_CL_ENHANCE, CLIP_VTX_REORDER_ENA | NUM_CLIP_SEQ(3));
+
+	udelay(50);
+}
+
+/*
+ * GART
+ */
+void cayman_pcie_gart_tlb_flush(struct radeon_device *rdev)
+{
+	/* flush hdp cache */
+	WREG32(HDP_MEM_COHERENCY_FLUSH_CNTL, 0x1);
+
+	/* bits 0-7 are the VM contexts0-7 */
+	WREG32(VM_INVALIDATE_REQUEST, 1);
+}
+
+int cayman_pcie_gart_enable(struct radeon_device *rdev)
+{
+	int r;
+
+	if (rdev->gart.table.vram.robj == NULL) {
+		dev_err(rdev->dev, "No VRAM object for PCIE GART.\n");
+		return -EINVAL;
+	}
+	r = radeon_gart_table_vram_pin(rdev);
+	if (r)
+		return r;
+	radeon_gart_restore(rdev);
+	/* Setup TLB control */
+	WREG32(MC_VM_MX_L1_TLB_CNTL, ENABLE_L1_TLB |
+	       ENABLE_L1_FRAGMENT_PROCESSING |
+	       SYSTEM_ACCESS_MODE_NOT_IN_SYS |
+	       SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU);
+	/* Setup L2 cache */
+	WREG32(VM_L2_CNTL, ENABLE_L2_CACHE |
+	       ENABLE_L2_PTE_CACHE_LRU_UPDATE_BY_WRITE |
+	       ENABLE_L2_PDE0_CACHE_LRU_UPDATE_BY_WRITE |
+	       EFFECTIVE_L2_QUEUE_SIZE(7) |
+	       CONTEXT1_IDENTITY_ACCESS_MODE(1));
+	WREG32(VM_L2_CNTL2, INVALIDATE_ALL_L1_TLBS | INVALIDATE_L2_CACHE);
+	WREG32(VM_L2_CNTL3, L2_CACHE_BIGK_ASSOCIATIVITY |
+	       L2_CACHE_BIGK_FRAGMENT_SIZE(6));
+	/* setup context0 */
+	WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR, rdev->mc.gtt_start >> 12);
+	WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR, rdev->mc.gtt_end >> 12);
+	WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR, rdev->gart.table_addr >> 12);
+	WREG32(VM_CONTEXT0_PROTECTION_FAULT_DEFAULT_ADDR,
+			(u32)(rdev->dummy_page.addr >> 12));
+	WREG32(VM_CONTEXT0_CNTL2, 0);
+	WREG32(VM_CONTEXT0_CNTL, ENABLE_CONTEXT | PAGE_TABLE_DEPTH(0) |
+				RANGE_PROTECTION_FAULT_ENABLE_DEFAULT);
+	/* disable context1-7 */
+	WREG32(VM_CONTEXT1_CNTL2, 0);
+	WREG32(VM_CONTEXT1_CNTL, 0);
+
+	cayman_pcie_gart_tlb_flush(rdev);
+	rdev->gart.ready = true;
+	return 0;
+}
+
+void cayman_pcie_gart_disable(struct radeon_device *rdev)
+{
+	int r;
+
+	/* Disable all tables */
+	WREG32(VM_CONTEXT0_CNTL, 0);
+	WREG32(VM_CONTEXT1_CNTL, 0);
+	/* Setup TLB control */
+	WREG32(MC_VM_MX_L1_TLB_CNTL, ENABLE_L1_FRAGMENT_PROCESSING |
+	       SYSTEM_ACCESS_MODE_NOT_IN_SYS |
+	       SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU);
+	/* Setup L2 cache */
+	WREG32(VM_L2_CNTL, ENABLE_L2_PTE_CACHE_LRU_UPDATE_BY_WRITE |
+	       ENABLE_L2_PDE0_CACHE_LRU_UPDATE_BY_WRITE |
+	       EFFECTIVE_L2_QUEUE_SIZE(7) |
+	       CONTEXT1_IDENTITY_ACCESS_MODE(1));
+	WREG32(VM_L2_CNTL2, 0);
+	WREG32(VM_L2_CNTL3, L2_CACHE_BIGK_ASSOCIATIVITY |
+	       L2_CACHE_BIGK_FRAGMENT_SIZE(6));
+	if (rdev->gart.table.vram.robj) {
+		r = radeon_bo_reserve(rdev->gart.table.vram.robj, false);
+		if (likely(r == 0)) {
+			radeon_bo_kunmap(rdev->gart.table.vram.robj);
+			radeon_bo_unpin(rdev->gart.table.vram.robj);
+			radeon_bo_unreserve(rdev->gart.table.vram.robj);
+		}
+	}
+}
+
+void cayman_pcie_gart_fini(struct radeon_device *rdev)
+{
+	cayman_pcie_gart_disable(rdev);
+	radeon_gart_table_vram_free(rdev);
+	radeon_gart_fini(rdev);
+}
+
+/*
+ * CP.
+ */
+static void cayman_cp_enable(struct radeon_device *rdev, bool enable)
+{
+	if (enable)
+		WREG32(CP_ME_CNTL, 0);
+	else {
+		rdev->mc.active_vram_size = rdev->mc.visible_vram_size;
+		WREG32(CP_ME_CNTL, (CP_ME_HALT | CP_PFP_HALT));
+		WREG32(SCRATCH_UMSK, 0);
+	}
+}
+
+static int cayman_cp_load_microcode(struct radeon_device *rdev)
+{
+	const __be32 *fw_data;
+	int i;
+
+	if (!rdev->me_fw || !rdev->pfp_fw)
+		return -EINVAL;
+
+	cayman_cp_enable(rdev, false);
+
+	fw_data = (const __be32 *)rdev->pfp_fw->data;
+	WREG32(CP_PFP_UCODE_ADDR, 0);
+	for (i = 0; i < CAYMAN_PFP_UCODE_SIZE; i++)
+		WREG32(CP_PFP_UCODE_DATA, be32_to_cpup(fw_data++));
+	WREG32(CP_PFP_UCODE_ADDR, 0);
+
+	fw_data = (const __be32 *)rdev->me_fw->data;
+	WREG32(CP_ME_RAM_WADDR, 0);
+	for (i = 0; i < CAYMAN_PM4_UCODE_SIZE; i++)
+		WREG32(CP_ME_RAM_DATA, be32_to_cpup(fw_data++));
+
+	WREG32(CP_PFP_UCODE_ADDR, 0);
+	WREG32(CP_ME_RAM_WADDR, 0);
+	WREG32(CP_ME_RAM_RADDR, 0);
+	return 0;
+}
+
+static int cayman_cp_start(struct radeon_device *rdev)
+{
+	int r, i;
+
+	r = radeon_ring_lock(rdev, 7);
+	if (r) {
+		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
+		return r;
+	}
+	radeon_ring_write(rdev, PACKET3(PACKET3_ME_INITIALIZE, 5));
+	radeon_ring_write(rdev, 0x1);
+	radeon_ring_write(rdev, 0x0);
+	radeon_ring_write(rdev, rdev->config.cayman.max_hw_contexts - 1);
+	radeon_ring_write(rdev, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
+	radeon_ring_write(rdev, 0);
+	radeon_ring_write(rdev, 0);
+	radeon_ring_unlock_commit(rdev);
+
+	cayman_cp_enable(rdev, true);
+
+	r = radeon_ring_lock(rdev, cayman_default_size + 19);
+	if (r) {
+		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
+		return r;
+	}
+
+	/* setup clear context state */
+	radeon_ring_write(rdev, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
+	radeon_ring_write(rdev, PACKET3_PREAMBLE_BEGIN_CLEAR_STATE);
+
+	for (i = 0; i < cayman_default_size; i++)
+		radeon_ring_write(rdev, cayman_default_state[i]);
+
+	radeon_ring_write(rdev, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
+	radeon_ring_write(rdev, PACKET3_PREAMBLE_END_CLEAR_STATE);
+
+	/* set clear context state */
+	radeon_ring_write(rdev, PACKET3(PACKET3_CLEAR_STATE, 0));
+	radeon_ring_write(rdev, 0);
+
+	/* SQ_VTX_BASE_VTX_LOC */
+	radeon_ring_write(rdev, 0xc0026f00);
+	radeon_ring_write(rdev, 0x00000000);
+	radeon_ring_write(rdev, 0x00000000);
+	radeon_ring_write(rdev, 0x00000000);
+
+	/* Clear consts */
+	radeon_ring_write(rdev, 0xc0036f00);
+	radeon_ring_write(rdev, 0x00000bc4);
+	radeon_ring_write(rdev, 0xffffffff);
+	radeon_ring_write(rdev, 0xffffffff);
+	radeon_ring_write(rdev, 0xffffffff);
+
+	radeon_ring_write(rdev, 0xc0026900);
+	radeon_ring_write(rdev, 0x00000316);
+	radeon_ring_write(rdev, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
+	radeon_ring_write(rdev, 0x00000010); /*  */
+
+	radeon_ring_unlock_commit(rdev);
+
+	/* XXX init other rings */
+
+	return 0;
+}
+
+static void cayman_cp_fini(struct radeon_device *rdev)
+{
+	cayman_cp_enable(rdev, false);
+	radeon_ring_fini(rdev);
+}
+
+int cayman_cp_resume(struct radeon_device *rdev)
+{
+	u32 tmp;
+	u32 rb_bufsz;
+	int r;
+
+	/* Reset cp; if cp is reset, then PA, SH, VGT also need to be reset */
+	WREG32(GRBM_SOFT_RESET, (SOFT_RESET_CP |
+				 SOFT_RESET_PA |
+				 SOFT_RESET_SH |
+				 SOFT_RESET_VGT |
+				 SOFT_RESET_SX));
+	RREG32(GRBM_SOFT_RESET);
+	mdelay(15);
+	WREG32(GRBM_SOFT_RESET, 0);
+	RREG32(GRBM_SOFT_RESET);
+
+	WREG32(CP_SEM_WAIT_TIMER, 0x4);
+
+	/* Set the write pointer delay */
+	WREG32(CP_RB_WPTR_DELAY, 0);
+
+	WREG32(CP_DEBUG, (1 << 27));
+
+	/* ring 0 - compute and gfx */
+	/* Set ring buffer size */
+	rb_bufsz = drm_order(rdev->cp.ring_size / 8);
+	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
+#ifdef __BIG_ENDIAN
+	tmp |= BUF_SWAP_32BIT;
+#endif
+	WREG32(CP_RB0_CNTL, tmp);
+
+	/* Initialize the ring buffer's read and write pointers */
+	WREG32(CP_RB0_CNTL, tmp | RB_RPTR_WR_ENA);
+	WREG32(CP_RB0_WPTR, 0);
+
+	/* set the wb address wether it's enabled or not */
+	WREG32(CP_RB0_RPTR_ADDR, (rdev->wb.gpu_addr + RADEON_WB_CP_RPTR_OFFSET) & 0xFFFFFFFC);
+	WREG32(CP_RB0_RPTR_ADDR_HI, upper_32_bits(rdev->wb.gpu_addr + RADEON_WB_CP_RPTR_OFFSET) & 0xFF);
+	WREG32(SCRATCH_ADDR, ((rdev->wb.gpu_addr + RADEON_WB_SCRATCH_OFFSET) >> 8) & 0xFFFFFFFF);
+
+	if (rdev->wb.enabled)
+		WREG32(SCRATCH_UMSK, 0xff);
+	else {
+		tmp |= RB_NO_UPDATE;
+		WREG32(SCRATCH_UMSK, 0);
+	}
+
+	mdelay(1);
+	WREG32(CP_RB0_CNTL, tmp);
+
+	WREG32(CP_RB0_BASE, rdev->cp.gpu_addr >> 8);
+
+	rdev->cp.rptr = RREG32(CP_RB0_RPTR);
+	rdev->cp.wptr = RREG32(CP_RB0_WPTR);
+
+	/* ring1  - compute only */
+	/* Set ring buffer size */
+	rb_bufsz = drm_order(rdev->cp1.ring_size / 8);
+	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
+#ifdef __BIG_ENDIAN
+	tmp |= BUF_SWAP_32BIT;
+#endif
+	WREG32(CP_RB1_CNTL, tmp);
+
+	/* Initialize the ring buffer's read and write pointers */
+	WREG32(CP_RB1_CNTL, tmp | RB_RPTR_WR_ENA);
+	WREG32(CP_RB1_WPTR, 0);
+
+	/* set the wb address wether it's enabled or not */
+	WREG32(CP_RB1_RPTR_ADDR, (rdev->wb.gpu_addr + RADEON_WB_CP1_RPTR_OFFSET) & 0xFFFFFFFC);
+	WREG32(CP_RB1_RPTR_ADDR_HI, upper_32_bits(rdev->wb.gpu_addr + RADEON_WB_CP1_RPTR_OFFSET) & 0xFF);
+
+	mdelay(1);
+	WREG32(CP_RB1_CNTL, tmp);
+
+	WREG32(CP_RB1_BASE, rdev->cp1.gpu_addr >> 8);
+
+	rdev->cp1.rptr = RREG32(CP_RB1_RPTR);
+	rdev->cp1.wptr = RREG32(CP_RB1_WPTR);
+
+	/* ring2 - compute only */
+	/* Set ring buffer size */
+	rb_bufsz = drm_order(rdev->cp2.ring_size / 8);
+	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
+#ifdef __BIG_ENDIAN
+	tmp |= BUF_SWAP_32BIT;
+#endif
+	WREG32(CP_RB2_CNTL, tmp);
+
+	/* Initialize the ring buffer's read and write pointers */
+	WREG32(CP_RB2_CNTL, tmp | RB_RPTR_WR_ENA);
+	WREG32(CP_RB2_WPTR, 0);
+
+	/* set the wb address wether it's enabled or not */
+	WREG32(CP_RB2_RPTR_ADDR, (rdev->wb.gpu_addr + RADEON_WB_CP2_RPTR_OFFSET) & 0xFFFFFFFC);
+	WREG32(CP_RB2_RPTR_ADDR_HI, upper_32_bits(rdev->wb.gpu_addr + RADEON_WB_CP2_RPTR_OFFSET) & 0xFF);
+
+	mdelay(1);
+	WREG32(CP_RB2_CNTL, tmp);
+
+	WREG32(CP_RB2_BASE, rdev->cp2.gpu_addr >> 8);
+
+	rdev->cp2.rptr = RREG32(CP_RB2_RPTR);
+	rdev->cp2.wptr = RREG32(CP_RB2_WPTR);
+
+	/* start the rings */
+	cayman_cp_start(rdev);
+	rdev->cp.ready = true;
+	rdev->cp1.ready = true;
+	rdev->cp2.ready = true;
+	/* this only test cp0 */
+	r = radeon_ring_test(rdev);
+	if (r) {
+		rdev->cp.ready = false;
+		rdev->cp1.ready = false;
+		rdev->cp2.ready = false;
+		return r;
+	}
+
+	return 0;
+}
+
+bool cayman_gpu_is_lockup(struct radeon_device *rdev)
+{
+	u32 srbm_status;
+	u32 grbm_status;
+	u32 grbm_status_se0, grbm_status_se1;
+	struct r100_gpu_lockup *lockup = &rdev->config.cayman.lockup;
+	int r;
+
+	srbm_status = RREG32(SRBM_STATUS);
+	grbm_status = RREG32(GRBM_STATUS);
+	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
+	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
+	if (!(grbm_status & GUI_ACTIVE)) {
+		r100_gpu_lockup_update(lockup, &rdev->cp);
+		return false;
+	}
+	/* force CP activities */
+	r = radeon_ring_lock(rdev, 2);
+	if (!r) {
+		/* PACKET2 NOP */
+		radeon_ring_write(rdev, 0x80000000);
+		radeon_ring_write(rdev, 0x80000000);
+		radeon_ring_unlock_commit(rdev);
+	}
+	/* XXX deal with CP0,1,2 */
+	rdev->cp.rptr = RREG32(CP_RB0_RPTR);
+	return r100_gpu_cp_is_lockup(rdev, lockup, &rdev->cp);
+}
+
+static int cayman_gpu_soft_reset(struct radeon_device *rdev)
+{
+	struct evergreen_mc_save save;
+	u32 grbm_reset = 0;
+
+	if (!(RREG32(GRBM_STATUS) & GUI_ACTIVE))
+		return 0;
+
+	dev_info(rdev->dev, "GPU softreset \n");
+	dev_info(rdev->dev, "  GRBM_STATUS=0x%08X\n",
+		RREG32(GRBM_STATUS));
+	dev_info(rdev->dev, "  GRBM_STATUS_SE0=0x%08X\n",
+		RREG32(GRBM_STATUS_SE0));
+	dev_info(rdev->dev, "  GRBM_STATUS_SE1=0x%08X\n",
+		RREG32(GRBM_STATUS_SE1));
+	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
+		RREG32(SRBM_STATUS));
+	evergreen_mc_stop(rdev, &save);
+	if (evergreen_mc_wait_for_idle(rdev)) {
+		dev_warn(rdev->dev, "Wait for MC idle timedout !\n");
+	}
+	/* Disable CP parsing/prefetching */
+	WREG32(CP_ME_CNTL, CP_ME_HALT | CP_PFP_HALT);
+
+	/* reset all the gfx blocks */
+	grbm_reset = (SOFT_RESET_CP |
+		      SOFT_RESET_CB |
+		      SOFT_RESET_DB |
+		      SOFT_RESET_GDS |
+		      SOFT_RESET_PA |
+		      SOFT_RESET_SC |
+		      SOFT_RESET_SPI |
+		      SOFT_RESET_SH |
+		      SOFT_RESET_SX |
+		      SOFT_RESET_TC |
+		      SOFT_RESET_TA |
+		      SOFT_RESET_VGT |
+		      SOFT_RESET_IA);
+
+	dev_info(rdev->dev, "  GRBM_SOFT_RESET=0x%08X\n", grbm_reset);
+	WREG32(GRBM_SOFT_RESET, grbm_reset);
+	(void)RREG32(GRBM_SOFT_RESET);
+	udelay(50);
+	WREG32(GRBM_SOFT_RESET, 0);
+	(void)RREG32(GRBM_SOFT_RESET);
+	/* Wait a little for things to settle down */
+	udelay(50);
+	dev_info(rdev->dev, "  GRBM_STATUS=0x%08X\n",
+		RREG32(GRBM_STATUS));
+	dev_info(rdev->dev, "  GRBM_STATUS_SE0=0x%08X\n",
+		RREG32(GRBM_STATUS_SE0));
+	dev_info(rdev->dev, "  GRBM_STATUS_SE1=0x%08X\n",
+		RREG32(GRBM_STATUS_SE1));
+	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
+		RREG32(SRBM_STATUS));
+	evergreen_mc_resume(rdev, &save);
+	return 0;
+}
+
+int cayman_asic_reset(struct radeon_device *rdev)
+{
+	return cayman_gpu_soft_reset(rdev);
+}
+
+static int cayman_startup(struct radeon_device *rdev)
+{
+	int r;
+
+	if (!rdev->me_fw || !rdev->pfp_fw || !rdev->rlc_fw || !rdev->mc_fw) {
+		r = ni_init_microcode(rdev);
+		if (r) {
+			DRM_ERROR("Failed to load firmware!\n");
+			return r;
+		}
+	}
+	r = ni_mc_load_microcode(rdev);
+	if (r) {
+		DRM_ERROR("Failed to load MC firmware!\n");
+		return r;
+	}
+
+	evergreen_mc_program(rdev);
+	r = cayman_pcie_gart_enable(rdev);
+	if (r)
+		return r;
+	cayman_gpu_init(rdev);
+
+#if 0
+	r = cayman_blit_init(rdev);
+	if (r) {
+		cayman_blit_fini(rdev);
+		rdev->asic->copy = NULL;
+		dev_warn(rdev->dev, "failed blitter (%d) falling back to memcpy\n", r);
+	}
+#endif
+
+	/* allocate wb buffer */
+	r = radeon_wb_init(rdev);
+	if (r)
+		return r;
+
+	/* Enable IRQ */
+	r = r600_irq_init(rdev);
+	if (r) {
+		DRM_ERROR("radeon: IH init failed (%d).\n", r);
+		radeon_irq_kms_fini(rdev);
+		return r;
+	}
+	evergreen_irq_set(rdev);
+
+	r = radeon_ring_init(rdev, rdev->cp.ring_size);
+	if (r)
+		return r;
+	r = cayman_cp_load_microcode(rdev);
+	if (r)
+		return r;
+	r = cayman_cp_resume(rdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+int cayman_resume(struct radeon_device *rdev)
+{
+	int r;
+
+	/* Do not reset GPU before posting, on rv770 hw unlike on r500 hw,
+	 * posting will perform necessary task to bring back GPU into good
+	 * shape.
+	 */
+	/* post card */
+	atom_asic_init(rdev->mode_info.atom_context);
+
+	r = cayman_startup(rdev);
+	if (r) {
+		DRM_ERROR("cayman startup failed on resume\n");
+		return r;
+	}
+
+	r = r600_ib_test(rdev);
+	if (r) {
+		DRM_ERROR("radeon: failled testing IB (%d).\n", r);
+		return r;
+	}
+
+	return r;
+
+}
+
+int cayman_suspend(struct radeon_device *rdev)
+{
+	/* int r; */
+
+	/* FIXME: we should wait for ring to be empty */
+	cayman_cp_enable(rdev, false);
+	rdev->cp.ready = false;
+	evergreen_irq_suspend(rdev);
+	radeon_wb_disable(rdev);
+	cayman_pcie_gart_disable(rdev);
+
+#if 0
+	/* unpin shaders bo */
+	r = radeon_bo_reserve(rdev->r600_blit.shader_obj, false);
+	if (likely(r == 0)) {
+		radeon_bo_unpin(rdev->r600_blit.shader_obj);
+		radeon_bo_unreserve(rdev->r600_blit.shader_obj);
+	}
+#endif
+	return 0;
+}
+
+/* Plan is to move initialization in that function and use
+ * helper function so that radeon_device_init pretty much
+ * do nothing more than calling asic specific function. This
+ * should also allow to remove a bunch of callback function
+ * like vram_info.
+ */
+int cayman_init(struct radeon_device *rdev)
+{
+	int r;
+
+	/* This don't do much */
+	r = radeon_gem_init(rdev);
+	if (r)
+		return r;
+	/* Read BIOS */
+	if (!radeon_get_bios(rdev)) {
+		if (ASIC_IS_AVIVO(rdev))
+			return -EINVAL;
+	}
+	/* Must be an ATOMBIOS */
+	if (!rdev->is_atom_bios) {
+		dev_err(rdev->dev, "Expecting atombios for cayman GPU\n");
+		return -EINVAL;
+	}
+	r = radeon_atombios_init(rdev);
+	if (r)
+		return r;
+
+	/* Post card if necessary */
+	if (!radeon_card_posted(rdev)) {
+		if (!rdev->bios) {
+			dev_err(rdev->dev, "Card not posted and no BIOS - ignoring\n");
+			return -EINVAL;
+		}
+		DRM_INFO("GPU not posted. posting now...\n");
+		atom_asic_init(rdev->mode_info.atom_context);
+	}
+	/* Initialize scratch registers */
+	r600_scratch_init(rdev);
+	/* Initialize surface registers */
+	radeon_surface_init(rdev);
+	/* Initialize clocks */
+	radeon_get_clock_info(rdev->ddev);
+	/* Fence driver */
+	r = radeon_fence_driver_init(rdev);
+	if (r)
+		return r;
+	/* initialize memory controller */
+	r = evergreen_mc_init(rdev);
+	if (r)
+		return r;
+	/* Memory manager */
+	r = radeon_bo_init(rdev);
+	if (r)
+		return r;
+
+	r = radeon_irq_kms_init(rdev);
+	if (r)
+		return r;
+
+	rdev->cp.ring_obj = NULL;
+	r600_ring_init(rdev, 1024 * 1024);
+
+	rdev->ih.ring_obj = NULL;
+	r600_ih_ring_init(rdev, 64 * 1024);
+
+	r = r600_pcie_gart_init(rdev);
+	if (r)
+		return r;
+
+	rdev->accel_working = true;
+	r = cayman_startup(rdev);
+	if (r) {
+		dev_err(rdev->dev, "disabling GPU acceleration\n");
+		cayman_cp_fini(rdev);
+		r600_irq_fini(rdev);
+		radeon_wb_fini(rdev);
+		radeon_irq_kms_fini(rdev);
+		cayman_pcie_gart_fini(rdev);
+		rdev->accel_working = false;
+	}
+	if (rdev->accel_working) {
+		r = radeon_ib_pool_init(rdev);
+		if (r) {
+			DRM_ERROR("radeon: failed initializing IB pool (%d).\n", r);
+			rdev->accel_working = false;
+		}
+		r = r600_ib_test(rdev);
+		if (r) {
+			DRM_ERROR("radeon: failed testing IB (%d).\n", r);
+			rdev->accel_working = false;
+		}
+	}
+
+	/* Don't start up if the MC ucode is missing.
+	 * The default clocks and voltages before the MC ucode
+	 * is loaded are not suffient for advanced operations.
+	 */
+	if (!rdev->mc_fw) {
+		DRM_ERROR("radeon: MC ucode required for NI+.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void cayman_fini(struct radeon_device *rdev)
+{
+	/* cayman_blit_fini(rdev); */
+	cayman_cp_fini(rdev);
+	r600_irq_fini(rdev);
+	radeon_wb_fini(rdev);
+	radeon_irq_kms_fini(rdev);
+	cayman_pcie_gart_fini(rdev);
+	radeon_gem_fini(rdev);
+	radeon_fence_driver_fini(rdev);
+	radeon_bo_fini(rdev);
+	radeon_atombios_fini(rdev);
+	kfree(rdev->bios);
+	rdev->bios = NULL;
+}
+
diff --git a/drivers/gpu/drm/radeon/nid.h b/drivers/gpu/drm/radeon/nid.h
index f7b445390e02..0f9a08b53fbd 100644
--- a/drivers/gpu/drm/radeon/nid.h
+++ b/drivers/gpu/drm/radeon/nid.h
@@ -24,7 +24,101 @@
 #ifndef NI_H
 #define NI_H
 
+#define CAYMAN_MAX_SH_GPRS           256
+#define CAYMAN_MAX_TEMP_GPRS         16
+#define CAYMAN_MAX_SH_THREADS        256
+#define CAYMAN_MAX_SH_STACK_ENTRIES  4096
+#define CAYMAN_MAX_FRC_EOV_CNT       16384
+#define CAYMAN_MAX_BACKENDS          8
+#define CAYMAN_MAX_BACKENDS_MASK     0xFF
+#define CAYMAN_MAX_BACKENDS_PER_SE_MASK 0xF
+#define CAYMAN_MAX_SIMDS             16
+#define CAYMAN_MAX_SIMDS_MASK        0xFFFF
+#define CAYMAN_MAX_SIMDS_PER_SE_MASK 0xFFF
+#define CAYMAN_MAX_PIPES             8
+#define CAYMAN_MAX_PIPES_MASK        0xFF
+#define CAYMAN_MAX_LDS_NUM           0xFFFF
+#define CAYMAN_MAX_TCC               16
+#define CAYMAN_MAX_TCC_MASK          0xFF
+
+#define DMIF_ADDR_CONFIG  				0xBD4
+#define	SRBM_STATUS				        0x0E50
+
+#define VM_CONTEXT0_REQUEST_RESPONSE			0x1470
+#define		REQUEST_TYPE(x)					(((x) & 0xf) << 0)
+#define		RESPONSE_TYPE_MASK				0x000000F0
+#define		RESPONSE_TYPE_SHIFT				4
+#define VM_L2_CNTL					0x1400
+#define		ENABLE_L2_CACHE					(1 << 0)
+#define		ENABLE_L2_FRAGMENT_PROCESSING			(1 << 1)
+#define		ENABLE_L2_PTE_CACHE_LRU_UPDATE_BY_WRITE		(1 << 9)
+#define		ENABLE_L2_PDE0_CACHE_LRU_UPDATE_BY_WRITE	(1 << 10)
+#define		EFFECTIVE_L2_QUEUE_SIZE(x)			(((x) & 7) << 14)
+#define		CONTEXT1_IDENTITY_ACCESS_MODE(x)		(((x) & 3) << 18)
+/* CONTEXT1_IDENTITY_ACCESS_MODE
+ * 0 physical = logical
+ * 1 logical via context1 page table
+ * 2 inside identity aperture use translation, outside physical = logical
+ * 3 inside identity aperture physical = logical, outside use translation
+ */
+#define VM_L2_CNTL2					0x1404
+#define		INVALIDATE_ALL_L1_TLBS				(1 << 0)
+#define		INVALIDATE_L2_CACHE				(1 << 1)
+#define VM_L2_CNTL3					0x1408
+#define		BANK_SELECT(x)					((x) << 0)
+#define		CACHE_UPDATE_MODE(x)				((x) << 6)
+#define		L2_CACHE_BIGK_ASSOCIATIVITY			(1 << 20)
+#define		L2_CACHE_BIGK_FRAGMENT_SIZE(x)			((x) << 15)
+#define	VM_L2_STATUS					0x140C
+#define		L2_BUSY						(1 << 0)
+#define VM_CONTEXT0_CNTL				0x1410
+#define		ENABLE_CONTEXT					(1 << 0)
+#define		PAGE_TABLE_DEPTH(x)				(((x) & 3) << 1)
+#define		RANGE_PROTECTION_FAULT_ENABLE_DEFAULT		(1 << 4)
+#define VM_CONTEXT1_CNTL				0x1414
+#define VM_CONTEXT0_CNTL2				0x1430
+#define VM_CONTEXT1_CNTL2				0x1434
+#define VM_INVALIDATE_REQUEST				0x1478
+#define VM_INVALIDATE_RESPONSE				0x147c
+#define VM_CONTEXT0_PROTECTION_FAULT_DEFAULT_ADDR	0x1518
+#define VM_CONTEXT1_PROTECTION_FAULT_DEFAULT_ADDR	0x151c
+#define	VM_CONTEXT0_PAGE_TABLE_BASE_ADDR		0x153C
+#define	VM_CONTEXT0_PAGE_TABLE_START_ADDR		0x155C
+#define	VM_CONTEXT0_PAGE_TABLE_END_ADDR			0x157C
+
+#define MC_SHARED_CHMAP						0x2004
+#define		NOOFCHAN_SHIFT					12
+#define		NOOFCHAN_MASK					0x00003000
+#define MC_SHARED_CHREMAP					0x2008
+
+#define	MC_VM_SYSTEM_APERTURE_LOW_ADDR			0x2034
+#define	MC_VM_SYSTEM_APERTURE_HIGH_ADDR			0x2038
+#define	MC_VM_SYSTEM_APERTURE_DEFAULT_ADDR		0x203C
+#define	MC_VM_MX_L1_TLB_CNTL				0x2064
+#define		ENABLE_L1_TLB					(1 << 0)
+#define		ENABLE_L1_FRAGMENT_PROCESSING			(1 << 1)
+#define		SYSTEM_ACCESS_MODE_PA_ONLY			(0 << 3)
+#define		SYSTEM_ACCESS_MODE_USE_SYS_MAP			(1 << 3)
+#define		SYSTEM_ACCESS_MODE_IN_SYS			(2 << 3)
+#define		SYSTEM_ACCESS_MODE_NOT_IN_SYS			(3 << 3)
+#define		SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU	(0 << 5)
+#define		ENABLE_ADVANCED_DRIVER_MODEL			(1 << 6)
+
 #define MC_SHARED_BLACKOUT_CNTL           		0x20ac
+#define	MC_ARB_RAMCFG					0x2760
+#define		NOOFBANK_SHIFT					0
+#define		NOOFBANK_MASK					0x00000003
+#define		NOOFRANK_SHIFT					2
+#define		NOOFRANK_MASK					0x00000004
+#define		NOOFROWS_SHIFT					3
+#define		NOOFROWS_MASK					0x00000038
+#define		NOOFCOLS_SHIFT					6
+#define		NOOFCOLS_MASK					0x000000C0
+#define		CHANSIZE_SHIFT					8
+#define		CHANSIZE_MASK					0x00000100
+#define		BURSTLENGTH_SHIFT				9
+#define		BURSTLENGTH_MASK				0x00000200
+#define		CHANSIZE_OVERRIDE				(1 << 11)
 #define MC_SEQ_SUP_CNTL           			0x28c8
 #define		RUN_MASK      				(1 << 0)
 #define MC_SEQ_SUP_PGM           			0x28cc
@@ -37,5 +131,406 @@
 #define MC_SEQ_IO_DEBUG_INDEX           		0x2a44
 #define MC_SEQ_IO_DEBUG_DATA           			0x2a48
 
+#define	HDP_HOST_PATH_CNTL				0x2C00
+#define	HDP_NONSURFACE_BASE				0x2C04
+#define	HDP_NONSURFACE_INFO				0x2C08
+#define	HDP_NONSURFACE_SIZE				0x2C0C
+#define HDP_ADDR_CONFIG  				0x2F48
+
+#define	CC_SYS_RB_BACKEND_DISABLE			0x3F88
+#define	GC_USER_SYS_RB_BACKEND_DISABLE			0x3F8C
+#define	CGTS_SYS_TCC_DISABLE				0x3F90
+#define	CGTS_USER_SYS_TCC_DISABLE			0x3F94
+
+#define	CONFIG_MEMSIZE					0x5428
+
+#define HDP_MEM_COHERENCY_FLUSH_CNTL			0x5480
+#define HDP_REG_COHERENCY_FLUSH_CNTL			0x54A0
+
+#define	GRBM_CNTL					0x8000
+#define		GRBM_READ_TIMEOUT(x)				((x) << 0)
+#define	GRBM_STATUS					0x8010
+#define		CMDFIFO_AVAIL_MASK				0x0000000F
+#define		RING2_RQ_PENDING				(1 << 4)
+#define		SRBM_RQ_PENDING					(1 << 5)
+#define		RING1_RQ_PENDING				(1 << 6)
+#define		CF_RQ_PENDING					(1 << 7)
+#define		PF_RQ_PENDING					(1 << 8)
+#define		GDS_DMA_RQ_PENDING				(1 << 9)
+#define		GRBM_EE_BUSY					(1 << 10)
+#define		SX_CLEAN					(1 << 11)
+#define		DB_CLEAN					(1 << 12)
+#define		CB_CLEAN					(1 << 13)
+#define		TA_BUSY 					(1 << 14)
+#define		GDS_BUSY 					(1 << 15)
+#define		VGT_BUSY_NO_DMA					(1 << 16)
+#define		VGT_BUSY					(1 << 17)
+#define		IA_BUSY_NO_DMA					(1 << 18)
+#define		IA_BUSY						(1 << 19)
+#define		SX_BUSY 					(1 << 20)
+#define		SH_BUSY 					(1 << 21)
+#define		SPI_BUSY					(1 << 22)
+#define		SC_BUSY 					(1 << 24)
+#define		PA_BUSY 					(1 << 25)
+#define		DB_BUSY 					(1 << 26)
+#define		CP_COHERENCY_BUSY      				(1 << 28)
+#define		CP_BUSY 					(1 << 29)
+#define		CB_BUSY 					(1 << 30)
+#define		GUI_ACTIVE					(1 << 31)
+#define	GRBM_STATUS_SE0					0x8014
+#define	GRBM_STATUS_SE1					0x8018
+#define		SE_SX_CLEAN					(1 << 0)
+#define		SE_DB_CLEAN					(1 << 1)
+#define		SE_CB_CLEAN					(1 << 2)
+#define		SE_VGT_BUSY					(1 << 23)
+#define		SE_PA_BUSY					(1 << 24)
+#define		SE_TA_BUSY					(1 << 25)
+#define		SE_SX_BUSY					(1 << 26)
+#define		SE_SPI_BUSY					(1 << 27)
+#define		SE_SH_BUSY					(1 << 28)
+#define		SE_SC_BUSY					(1 << 29)
+#define		SE_DB_BUSY					(1 << 30)
+#define		SE_CB_BUSY					(1 << 31)
+#define	GRBM_SOFT_RESET					0x8020
+#define		SOFT_RESET_CP					(1 << 0)
+#define		SOFT_RESET_CB					(1 << 1)
+#define		SOFT_RESET_DB					(1 << 3)
+#define		SOFT_RESET_GDS					(1 << 4)
+#define		SOFT_RESET_PA					(1 << 5)
+#define		SOFT_RESET_SC					(1 << 6)
+#define		SOFT_RESET_SPI					(1 << 8)
+#define		SOFT_RESET_SH					(1 << 9)
+#define		SOFT_RESET_SX					(1 << 10)
+#define		SOFT_RESET_TC					(1 << 11)
+#define		SOFT_RESET_TA					(1 << 12)
+#define		SOFT_RESET_VGT					(1 << 14)
+#define		SOFT_RESET_IA					(1 << 15)
+
+#define	SCRATCH_REG0					0x8500
+#define	SCRATCH_REG1					0x8504
+#define	SCRATCH_REG2					0x8508
+#define	SCRATCH_REG3					0x850C
+#define	SCRATCH_REG4					0x8510
+#define	SCRATCH_REG5					0x8514
+#define	SCRATCH_REG6					0x8518
+#define	SCRATCH_REG7					0x851C
+#define	SCRATCH_UMSK					0x8540
+#define	SCRATCH_ADDR					0x8544
+#define	CP_SEM_WAIT_TIMER				0x85BC
+#define CP_ME_CNTL					0x86D8
+#define		CP_ME_HALT					(1 << 28)
+#define		CP_PFP_HALT					(1 << 26)
+#define	CP_RB2_RPTR					0x86f8
+#define	CP_RB1_RPTR					0x86fc
+#define	CP_RB0_RPTR					0x8700
+#define	CP_RB_WPTR_DELAY				0x8704
+#define CP_MEQ_THRESHOLDS				0x8764
+#define		MEQ1_START(x)				((x) << 0)
+#define		MEQ2_START(x)				((x) << 8)
+#define	CP_PERFMON_CNTL					0x87FC
+
+#define	VGT_CACHE_INVALIDATION				0x88C4
+#define		CACHE_INVALIDATION(x)				((x) << 0)
+#define			VC_ONLY						0
+#define			TC_ONLY						1
+#define			VC_AND_TC					2
+#define		AUTO_INVLD_EN(x)				((x) << 6)
+#define			NO_AUTO						0
+#define			ES_AUTO						1
+#define			GS_AUTO						2
+#define			ES_AND_GS_AUTO					3
+#define	VGT_GS_VERTEX_REUSE				0x88D4
+
+#define CC_GC_SHADER_PIPE_CONFIG			0x8950
+#define	GC_USER_SHADER_PIPE_CONFIG			0x8954
+#define		INACTIVE_QD_PIPES(x)				((x) << 8)
+#define		INACTIVE_QD_PIPES_MASK				0x0000FF00
+#define		INACTIVE_QD_PIPES_SHIFT				8
+#define		INACTIVE_SIMDS(x)				((x) << 16)
+#define		INACTIVE_SIMDS_MASK				0xFFFF0000
+#define		INACTIVE_SIMDS_SHIFT				16
+
+#define VGT_PRIMITIVE_TYPE                              0x8958
+#define	VGT_NUM_INSTANCES				0x8974
+#define VGT_TF_RING_SIZE				0x8988
+#define VGT_OFFCHIP_LDS_BASE				0x89b4
+
+#define	PA_SC_LINE_STIPPLE_STATE			0x8B10
+#define	PA_CL_ENHANCE					0x8A14
+#define		CLIP_VTX_REORDER_ENA				(1 << 0)
+#define		NUM_CLIP_SEQ(x)					((x) << 1)
+#define	PA_SC_FIFO_SIZE					0x8BCC
+#define		SC_PRIM_FIFO_SIZE(x)				((x) << 0)
+#define		SC_HIZ_TILE_FIFO_SIZE(x)			((x) << 12)
+#define		SC_EARLYZ_TILE_FIFO_SIZE(x)			((x) << 20)
+#define	PA_SC_FORCE_EOV_MAX_CNTS			0x8B24
+#define		FORCE_EOV_MAX_CLK_CNT(x)			((x) << 0)
+#define		FORCE_EOV_MAX_REZ_CNT(x)			((x) << 16)
+
+#define	SQ_CONFIG					0x8C00
+#define		VC_ENABLE					(1 << 0)
+#define		EXPORT_SRC_C					(1 << 1)
+#define		GFX_PRIO(x)					((x) << 2)
+#define		CS1_PRIO(x)					((x) << 4)
+#define		CS2_PRIO(x)					((x) << 6)
+#define	SQ_GPR_RESOURCE_MGMT_1				0x8C04
+#define		NUM_PS_GPRS(x)					((x) << 0)
+#define		NUM_VS_GPRS(x)					((x) << 16)
+#define		NUM_CLAUSE_TEMP_GPRS(x)				((x) << 28)
+#define SQ_ESGS_RING_SIZE				0x8c44
+#define SQ_GSVS_RING_SIZE				0x8c4c
+#define SQ_ESTMP_RING_BASE				0x8c50
+#define SQ_ESTMP_RING_SIZE				0x8c54
+#define SQ_GSTMP_RING_BASE				0x8c58
+#define SQ_GSTMP_RING_SIZE				0x8c5c
+#define SQ_VSTMP_RING_BASE				0x8c60
+#define SQ_VSTMP_RING_SIZE				0x8c64
+#define SQ_PSTMP_RING_BASE				0x8c68
+#define SQ_PSTMP_RING_SIZE				0x8c6c
+#define	SQ_MS_FIFO_SIZES				0x8CF0
+#define		CACHE_FIFO_SIZE(x)				((x) << 0)
+#define		FETCH_FIFO_HIWATER(x)				((x) << 8)
+#define		DONE_FIFO_HIWATER(x)				((x) << 16)
+#define		ALU_UPDATE_FIFO_HIWATER(x)			((x) << 24)
+#define SQ_LSTMP_RING_BASE				0x8e10
+#define SQ_LSTMP_RING_SIZE				0x8e14
+#define SQ_HSTMP_RING_BASE				0x8e18
+#define SQ_HSTMP_RING_SIZE				0x8e1c
+#define	SQ_DYN_GPR_CNTL_PS_FLUSH_REQ    		0x8D8C
+#define		DYN_GPR_ENABLE					(1 << 8)
+#define SQ_CONST_MEM_BASE				0x8df8
+
+#define	SX_EXPORT_BUFFER_SIZES				0x900C
+#define		COLOR_BUFFER_SIZE(x)				((x) << 0)
+#define		POSITION_BUFFER_SIZE(x)				((x) << 8)
+#define		SMX_BUFFER_SIZE(x)				((x) << 16)
+#define	SX_DEBUG_1					0x9058
+#define		ENABLE_NEW_SMX_ADDRESS				(1 << 16)
+
+#define	SPI_CONFIG_CNTL					0x9100
+#define		GPR_WRITE_PRIORITY(x)				((x) << 0)
+#define	SPI_CONFIG_CNTL_1				0x913C
+#define		VTX_DONE_DELAY(x)				((x) << 0)
+#define		INTERP_ONE_PRIM_PER_ROW				(1 << 4)
+#define		CRC_SIMD_ID_WADDR_DISABLE			(1 << 8)
+
+#define	CGTS_TCC_DISABLE				0x9148
+#define	CGTS_USER_TCC_DISABLE				0x914C
+#define		TCC_DISABLE_MASK				0xFFFF0000
+#define		TCC_DISABLE_SHIFT				16
+#define	CGTS_SM_CTRL_REG				0x915C
+#define		OVERRIDE				(1 << 21)
+
+#define	TA_CNTL_AUX					0x9508
+#define		DISABLE_CUBE_WRAP				(1 << 0)
+#define		DISABLE_CUBE_ANISO				(1 << 1)
+
+#define	TCP_CHAN_STEER_LO				0x960c
+#define	TCP_CHAN_STEER_HI				0x9610
+
+#define CC_RB_BACKEND_DISABLE				0x98F4
+#define		BACKEND_DISABLE(x)     			((x) << 16)
+#define GB_ADDR_CONFIG  				0x98F8
+#define		NUM_PIPES(x)				((x) << 0)
+#define		NUM_PIPES_MASK				0x00000007
+#define		NUM_PIPES_SHIFT				0
+#define		PIPE_INTERLEAVE_SIZE(x)			((x) << 4)
+#define		PIPE_INTERLEAVE_SIZE_MASK		0x00000070
+#define		PIPE_INTERLEAVE_SIZE_SHIFT		4
+#define		BANK_INTERLEAVE_SIZE(x)			((x) << 8)
+#define		NUM_SHADER_ENGINES(x)			((x) << 12)
+#define		NUM_SHADER_ENGINES_MASK			0x00003000
+#define		NUM_SHADER_ENGINES_SHIFT		12
+#define		SHADER_ENGINE_TILE_SIZE(x)     		((x) << 16)
+#define		SHADER_ENGINE_TILE_SIZE_MASK		0x00070000
+#define		SHADER_ENGINE_TILE_SIZE_SHIFT		16
+#define		NUM_GPUS(x)     			((x) << 20)
+#define		NUM_GPUS_MASK				0x00700000
+#define		NUM_GPUS_SHIFT				20
+#define		MULTI_GPU_TILE_SIZE(x)     		((x) << 24)
+#define		MULTI_GPU_TILE_SIZE_MASK		0x03000000
+#define		MULTI_GPU_TILE_SIZE_SHIFT		24
+#define		ROW_SIZE(x)             		((x) << 28)
+#define		ROW_SIZE_MASK				0x30000007
+#define		ROW_SIZE_SHIFT				28
+#define		NUM_LOWER_PIPES(x)			((x) << 30)
+#define		NUM_LOWER_PIPES_MASK			0x40000000
+#define		NUM_LOWER_PIPES_SHIFT			30
+#define GB_BACKEND_MAP  				0x98FC
+
+#define CB_PERF_CTR0_SEL_0				0x9A20
+#define CB_PERF_CTR0_SEL_1				0x9A24
+#define CB_PERF_CTR1_SEL_0				0x9A28
+#define CB_PERF_CTR1_SEL_1				0x9A2C
+#define CB_PERF_CTR2_SEL_0				0x9A30
+#define CB_PERF_CTR2_SEL_1				0x9A34
+#define CB_PERF_CTR3_SEL_0				0x9A38
+#define CB_PERF_CTR3_SEL_1				0x9A3C
+
+#define	GC_USER_RB_BACKEND_DISABLE			0x9B7C
+#define		BACKEND_DISABLE_MASK			0x00FF0000
+#define		BACKEND_DISABLE_SHIFT			16
+
+#define	SMX_DC_CTL0					0xA020
+#define		USE_HASH_FUNCTION				(1 << 0)
+#define		NUMBER_OF_SETS(x)				((x) << 1)
+#define		FLUSH_ALL_ON_EVENT				(1 << 10)
+#define		STALL_ON_EVENT					(1 << 11)
+#define	SMX_EVENT_CTL					0xA02C
+#define		ES_FLUSH_CTL(x)					((x) << 0)
+#define		GS_FLUSH_CTL(x)					((x) << 3)
+#define		ACK_FLUSH_CTL(x)				((x) << 6)
+#define		SYNC_FLUSH_CTL					(1 << 8)
+
+#define	CP_RB0_BASE					0xC100
+#define	CP_RB0_CNTL					0xC104
+#define		RB_BUFSZ(x)					((x) << 0)
+#define		RB_BLKSZ(x)					((x) << 8)
+#define		RB_NO_UPDATE					(1 << 27)
+#define		RB_RPTR_WR_ENA					(1 << 31)
+#define		BUF_SWAP_32BIT					(2 << 16)
+#define	CP_RB0_RPTR_ADDR				0xC10C
+#define	CP_RB0_RPTR_ADDR_HI				0xC110
+#define	CP_RB0_WPTR					0xC114
+#define	CP_RB1_BASE					0xC180
+#define	CP_RB1_CNTL					0xC184
+#define	CP_RB1_RPTR_ADDR				0xC188
+#define	CP_RB1_RPTR_ADDR_HI				0xC18C
+#define	CP_RB1_WPTR					0xC190
+#define	CP_RB2_BASE					0xC194
+#define	CP_RB2_CNTL					0xC198
+#define	CP_RB2_RPTR_ADDR				0xC19C
+#define	CP_RB2_RPTR_ADDR_HI				0xC1A0
+#define	CP_RB2_WPTR					0xC1A4
+#define	CP_PFP_UCODE_ADDR				0xC150
+#define	CP_PFP_UCODE_DATA				0xC154
+#define	CP_ME_RAM_RADDR					0xC158
+#define	CP_ME_RAM_WADDR					0xC15C
+#define	CP_ME_RAM_DATA					0xC160
+#define	CP_DEBUG					0xC1FC
+
+/*
+ * PM4
+ */
+#define	PACKET_TYPE0	0
+#define	PACKET_TYPE1	1
+#define	PACKET_TYPE2	2
+#define	PACKET_TYPE3	3
+
+#define CP_PACKET_GET_TYPE(h) (((h) >> 30) & 3)
+#define CP_PACKET_GET_COUNT(h) (((h) >> 16) & 0x3FFF)
+#define CP_PACKET0_GET_REG(h) (((h) & 0xFFFF) << 2)
+#define CP_PACKET3_GET_OPCODE(h) (((h) >> 8) & 0xFF)
+#define PACKET0(reg, n)	((PACKET_TYPE0 << 30) |				\
+			 (((reg) >> 2) & 0xFFFF) |			\
+			 ((n) & 0x3FFF) << 16)
+#define CP_PACKET2			0x80000000
+#define		PACKET2_PAD_SHIFT		0
+#define		PACKET2_PAD_MASK		(0x3fffffff << 0)
+
+#define PACKET2(v)	(CP_PACKET2 | REG_SET(PACKET2_PAD, (v)))
+
+#define PACKET3(op, n)	((PACKET_TYPE3 << 30) |				\
+			 (((op) & 0xFF) << 8) |				\
+			 ((n) & 0x3FFF) << 16)
+
+/* Packet 3 types */
+#define	PACKET3_NOP					0x10
+#define	PACKET3_SET_BASE				0x11
+#define	PACKET3_CLEAR_STATE				0x12
+#define	PACKET3_INDEX_BUFFER_SIZE			0x13
+#define	PACKET3_DEALLOC_STATE				0x14
+#define	PACKET3_DISPATCH_DIRECT				0x15
+#define	PACKET3_DISPATCH_INDIRECT			0x16
+#define	PACKET3_INDIRECT_BUFFER_END			0x17
+#define	PACKET3_SET_PREDICATION				0x20
+#define	PACKET3_REG_RMW					0x21
+#define	PACKET3_COND_EXEC				0x22
+#define	PACKET3_PRED_EXEC				0x23
+#define	PACKET3_DRAW_INDIRECT				0x24
+#define	PACKET3_DRAW_INDEX_INDIRECT			0x25
+#define	PACKET3_INDEX_BASE				0x26
+#define	PACKET3_DRAW_INDEX_2				0x27
+#define	PACKET3_CONTEXT_CONTROL				0x28
+#define	PACKET3_DRAW_INDEX_OFFSET			0x29
+#define	PACKET3_INDEX_TYPE				0x2A
+#define	PACKET3_DRAW_INDEX				0x2B
+#define	PACKET3_DRAW_INDEX_AUTO				0x2D
+#define	PACKET3_DRAW_INDEX_IMMD				0x2E
+#define	PACKET3_NUM_INSTANCES				0x2F
+#define	PACKET3_DRAW_INDEX_MULTI_AUTO			0x30
+#define	PACKET3_INDIRECT_BUFFER				0x32
+#define	PACKET3_STRMOUT_BUFFER_UPDATE			0x34
+#define	PACKET3_DRAW_INDEX_OFFSET_2			0x35
+#define	PACKET3_DRAW_INDEX_MULTI_ELEMENT		0x36
+#define	PACKET3_WRITE_DATA				0x37
+#define	PACKET3_MEM_SEMAPHORE				0x39
+#define	PACKET3_MPEG_INDEX				0x3A
+#define	PACKET3_WAIT_REG_MEM				0x3C
+#define	PACKET3_MEM_WRITE				0x3D
+#define	PACKET3_SURFACE_SYNC				0x43
+#              define PACKET3_CB0_DEST_BASE_ENA    (1 << 6)
+#              define PACKET3_CB1_DEST_BASE_ENA    (1 << 7)
+#              define PACKET3_CB2_DEST_BASE_ENA    (1 << 8)
+#              define PACKET3_CB3_DEST_BASE_ENA    (1 << 9)
+#              define PACKET3_CB4_DEST_BASE_ENA    (1 << 10)
+#              define PACKET3_CB5_DEST_BASE_ENA    (1 << 11)
+#              define PACKET3_CB6_DEST_BASE_ENA    (1 << 12)
+#              define PACKET3_CB7_DEST_BASE_ENA    (1 << 13)
+#              define PACKET3_DB_DEST_BASE_ENA     (1 << 14)
+#              define PACKET3_CB8_DEST_BASE_ENA    (1 << 15)
+#              define PACKET3_CB9_DEST_BASE_ENA    (1 << 16)
+#              define PACKET3_CB10_DEST_BASE_ENA   (1 << 17)
+#              define PACKET3_CB11_DEST_BASE_ENA   (1 << 18)
+#              define PACKET3_FULL_CACHE_ENA       (1 << 20)
+#              define PACKET3_TC_ACTION_ENA        (1 << 23)
+#              define PACKET3_CB_ACTION_ENA        (1 << 25)
+#              define PACKET3_DB_ACTION_ENA        (1 << 26)
+#              define PACKET3_SH_ACTION_ENA        (1 << 27)
+#              define PACKET3_SX_ACTION_ENA        (1 << 28)
+#define	PACKET3_ME_INITIALIZE				0x44
+#define		PACKET3_ME_INITIALIZE_DEVICE_ID(x) ((x) << 16)
+#define	PACKET3_COND_WRITE				0x45
+#define	PACKET3_EVENT_WRITE				0x46
+#define	PACKET3_EVENT_WRITE_EOP				0x47
+#define	PACKET3_EVENT_WRITE_EOS				0x48
+#define	PACKET3_PREAMBLE_CNTL				0x4A
+#              define PACKET3_PREAMBLE_BEGIN_CLEAR_STATE     (2 << 28)
+#              define PACKET3_PREAMBLE_END_CLEAR_STATE       (3 << 28)
+#define	PACKET3_ALU_PS_CONST_BUFFER_COPY		0x4C
+#define	PACKET3_ALU_VS_CONST_BUFFER_COPY		0x4D
+#define	PACKET3_ALU_PS_CONST_UPDATE		        0x4E
+#define	PACKET3_ALU_VS_CONST_UPDATE		        0x4F
+#define	PACKET3_ONE_REG_WRITE				0x57
+#define	PACKET3_SET_CONFIG_REG				0x68
+#define		PACKET3_SET_CONFIG_REG_START			0x00008000
+#define		PACKET3_SET_CONFIG_REG_END			0x0000ac00
+#define	PACKET3_SET_CONTEXT_REG				0x69
+#define		PACKET3_SET_CONTEXT_REG_START			0x00028000
+#define		PACKET3_SET_CONTEXT_REG_END			0x00029000
+#define	PACKET3_SET_ALU_CONST				0x6A
+/* alu const buffers only; no reg file */
+#define	PACKET3_SET_BOOL_CONST				0x6B
+#define		PACKET3_SET_BOOL_CONST_START			0x0003a500
+#define		PACKET3_SET_BOOL_CONST_END			0x0003a518
+#define	PACKET3_SET_LOOP_CONST				0x6C
+#define		PACKET3_SET_LOOP_CONST_START			0x0003a200
+#define		PACKET3_SET_LOOP_CONST_END			0x0003a500
+#define	PACKET3_SET_RESOURCE				0x6D
+#define		PACKET3_SET_RESOURCE_START			0x00030000
+#define		PACKET3_SET_RESOURCE_END			0x00038000
+#define	PACKET3_SET_SAMPLER				0x6E
+#define		PACKET3_SET_SAMPLER_START			0x0003c000
+#define		PACKET3_SET_SAMPLER_END				0x0003c600
+#define	PACKET3_SET_CTL_CONST				0x6F
+#define		PACKET3_SET_CTL_CONST_START			0x0003cff0
+#define		PACKET3_SET_CTL_CONST_END			0x0003ff0c
+#define	PACKET3_SET_RESOURCE_OFFSET			0x70
+#define	PACKET3_SET_ALU_CONST_VS			0x71
+#define	PACKET3_SET_ALU_CONST_DI			0x72
+#define	PACKET3_SET_CONTEXT_REG_INDIRECT		0x73
+#define	PACKET3_SET_RESOURCE_INDIRECT			0x74
+#define	PACKET3_SET_APPEND_CNT			        0x75
+
 #endif
 
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 56deae5bf02e..37b95f097420 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -3490,7 +3490,7 @@ void r100_cs_track_clear(struct radeon_device *rdev, struct r100_cs_track *track
 		track->num_texture = 16;
 		track->maxy = 4096;
 		track->separate_cube = 0;
-		track->aaresolve = true;
+		track->aaresolve = false;
 		track->aa.robj = NULL;
 	}
 
@@ -3639,7 +3639,7 @@ int r100_ib_test(struct radeon_device *rdev)
 	if (i < rdev->usec_timeout) {
 		DRM_INFO("ib test succeeded in %u usecs\n", i);
 	} else {
-		DRM_ERROR("radeon: ib test failed (sracth(0x%04X)=0x%08X)\n",
+		DRM_ERROR("radeon: ib test failed (scratch(0x%04X)=0x%08X)\n",
 			  scratch, tmp);
 		r = -EINVAL;
 	}
@@ -3659,13 +3659,13 @@ int r100_ib_init(struct radeon_device *rdev)
 
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB pool (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB pool (%d).\n", r);
 		r100_ib_fini(rdev);
 		return r;
 	}
 	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled testing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
 		r100_ib_fini(rdev);
 		return r;
 	}
@@ -3801,8 +3801,6 @@ static int r100_startup(struct radeon_device *rdev)
 	r100_mc_program(rdev);
 	/* Resume clock */
 	r100_clock_startup(rdev);
-	/* Initialize GPU configuration (# pipes, ...) */
-//	r100_gpu_init(rdev);
 	/* Initialize GART (initialize after TTM so we can allocate
 	 * memory through TTM but finalize after TTM) */
 	r100_enable_bm(rdev);
@@ -3823,12 +3821,12 @@ static int r100_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index 069efa8c8ecf..8713731fa014 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -1401,12 +1401,12 @@ static int r300_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index 0b59ed7c7d2c..417fab81812f 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -260,13 +260,13 @@ static int r420_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r420_cp_errata_init(rdev);
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/radeon/r520.c b/drivers/gpu/drm/radeon/r520.c
index 2ce80d976568..3081d07f8de5 100644
--- a/drivers/gpu/drm/radeon/r520.c
+++ b/drivers/gpu/drm/radeon/r520.c
@@ -193,12 +193,12 @@ static int r520_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index de88624d5f87..91a85c7c8f57 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -47,6 +47,7 @@
 #define EVERGREEN_PFP_UCODE_SIZE 1120
 #define EVERGREEN_PM4_UCODE_SIZE 1376
 #define EVERGREEN_RLC_UCODE_SIZE 768
+#define CAYMAN_RLC_UCODE_SIZE 1024
 
 /* Firmware Names */
 MODULE_FIRMWARE("radeon/R600_pfp.bin");
@@ -2464,7 +2465,7 @@ int r600_resume(struct radeon_device *rdev)
 
 	r = r600_ib_test(rdev);
 	if (r) {
-		DRM_ERROR("radeon: failled testing IB (%d).\n", r);
+		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
 		return r;
 	}
 
@@ -2740,7 +2741,7 @@ static int r600_ih_ring_alloc(struct radeon_device *rdev)
 
 	/* Allocate ring buffer */
 	if (rdev->ih.ring_obj == NULL) {
-		r = radeon_bo_create(rdev, NULL, rdev->ih.ring_size,
+		r = radeon_bo_create(rdev, rdev->ih.ring_size,
 				     PAGE_SIZE, true,
 				     RADEON_GEM_DOMAIN_GTT,
 				     &rdev->ih.ring_obj);
@@ -2821,13 +2822,20 @@ static int r600_rlc_init(struct radeon_device *rdev)
 	WREG32(RLC_HB_CNTL, 0);
 	WREG32(RLC_HB_RPTR, 0);
 	WREG32(RLC_HB_WPTR, 0);
-	WREG32(RLC_HB_WPTR_LSB_ADDR, 0);
-	WREG32(RLC_HB_WPTR_MSB_ADDR, 0);
+	if (rdev->family <= CHIP_CAICOS) {
+		WREG32(RLC_HB_WPTR_LSB_ADDR, 0);
+		WREG32(RLC_HB_WPTR_MSB_ADDR, 0);
+	}
 	WREG32(RLC_MC_CNTL, 0);
 	WREG32(RLC_UCODE_CNTL, 0);
 
 	fw_data = (const __be32 *)rdev->rlc_fw->data;
-	if (rdev->family >= CHIP_CEDAR) {
+	if (rdev->family >= CHIP_CAYMAN) {
+		for (i = 0; i < CAYMAN_RLC_UCODE_SIZE; i++) {
+			WREG32(RLC_UCODE_ADDR, i);
+			WREG32(RLC_UCODE_DATA, be32_to_cpup(fw_data++));
+		}
+	} else if (rdev->family >= CHIP_CEDAR) {
 		for (i = 0; i < EVERGREEN_RLC_UCODE_SIZE; i++) {
 			WREG32(RLC_UCODE_ADDR, i);
 			WREG32(RLC_UCODE_DATA, be32_to_cpup(fw_data++));
diff --git a/drivers/gpu/drm/radeon/r600_audio.c b/drivers/gpu/drm/radeon/r600_audio.c
index b5443fe1c1d1..846fae576399 100644
--- a/drivers/gpu/drm/radeon/r600_audio.c
+++ b/drivers/gpu/drm/radeon/r600_audio.c
@@ -26,6 +26,7 @@
 #include "drmP.h"
 #include "radeon.h"
 #include "radeon_reg.h"
+#include "radeon_asic.h"
 #include "atom.h"
 
 #define AUDIO_TIMER_INTERVALL 100 /* 1/10 sekund should be enough */
diff --git a/drivers/gpu/drm/radeon/r600_blit_kms.c b/drivers/gpu/drm/radeon/r600_blit_kms.c
index 41f7aafc97c4..2fed91750126 100644
--- a/drivers/gpu/drm/radeon/r600_blit_kms.c
+++ b/drivers/gpu/drm/radeon/r600_blit_kms.c
@@ -512,7 +512,7 @@ int r600_blit_init(struct radeon_device *rdev)
 	obj_size += r6xx_ps_size * 4;
 	obj_size = ALIGN(obj_size, 256);
 
-	r = radeon_bo_create(rdev, NULL, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
+	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
 				&rdev->r600_blit.shader_obj);
 	if (r) {
 		DRM_ERROR("r600 failed to allocate shader\n");
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index 153095fba62f..0a0848f0346d 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -71,75 +71,167 @@ struct r600_cs_track {
 	u64			db_bo_mc;
 };
 
+#define FMT_8_BIT(fmt, vc) [fmt] = { 1, 1, 1, vc }
+#define FMT_16_BIT(fmt, vc) [fmt] = { 1, 1, 2, vc }
+#define FMT_24_BIT(fmt) [fmt] = { 1, 1, 3, 0 }
+#define FMT_32_BIT(fmt, vc) [fmt] = { 1, 1, 4, vc }
+#define FMT_48_BIT(fmt) [fmt] = { 1, 1, 6, 0 }
+#define FMT_64_BIT(fmt, vc) [fmt] = { 1, 1, 8, vc }
+#define FMT_96_BIT(fmt) [fmt] = { 1, 1, 12, 0 }
+#define FMT_128_BIT(fmt, vc) [fmt] = { 1, 1, 16, vc }
+
+struct gpu_formats {
+	unsigned blockwidth;
+	unsigned blockheight;
+	unsigned blocksize;
+	unsigned valid_color;
+};
+
+static const struct gpu_formats color_formats_table[] = {
+	/* 8 bit */
+	FMT_8_BIT(V_038004_COLOR_8, 1),
+	FMT_8_BIT(V_038004_COLOR_4_4, 1),
+	FMT_8_BIT(V_038004_COLOR_3_3_2, 1),
+	FMT_8_BIT(V_038004_FMT_1, 0),
+
+	/* 16-bit */
+	FMT_16_BIT(V_038004_COLOR_16, 1),
+	FMT_16_BIT(V_038004_COLOR_16_FLOAT, 1),
+	FMT_16_BIT(V_038004_COLOR_8_8, 1),
+	FMT_16_BIT(V_038004_COLOR_5_6_5, 1),
+	FMT_16_BIT(V_038004_COLOR_6_5_5, 1),
+	FMT_16_BIT(V_038004_COLOR_1_5_5_5, 1),
+	FMT_16_BIT(V_038004_COLOR_4_4_4_4, 1),
+	FMT_16_BIT(V_038004_COLOR_5_5_5_1, 1),
+
+	/* 24-bit */
+	FMT_24_BIT(V_038004_FMT_8_8_8),
+					       
+	/* 32-bit */
+	FMT_32_BIT(V_038004_COLOR_32, 1),
+	FMT_32_BIT(V_038004_COLOR_32_FLOAT, 1),
+	FMT_32_BIT(V_038004_COLOR_16_16, 1),
+	FMT_32_BIT(V_038004_COLOR_16_16_FLOAT, 1),
+	FMT_32_BIT(V_038004_COLOR_8_24, 1),
+	FMT_32_BIT(V_038004_COLOR_8_24_FLOAT, 1),
+	FMT_32_BIT(V_038004_COLOR_24_8, 1),
+	FMT_32_BIT(V_038004_COLOR_24_8_FLOAT, 1),
+	FMT_32_BIT(V_038004_COLOR_10_11_11, 1),
+	FMT_32_BIT(V_038004_COLOR_10_11_11_FLOAT, 1),
+	FMT_32_BIT(V_038004_COLOR_11_11_10, 1),
+	FMT_32_BIT(V_038004_COLOR_11_11_10_FLOAT, 1),
+	FMT_32_BIT(V_038004_COLOR_2_10_10_10, 1),
+	FMT_32_BIT(V_038004_COLOR_8_8_8_8, 1),
+	FMT_32_BIT(V_038004_COLOR_10_10_10_2, 1),
+	FMT_32_BIT(V_038004_FMT_5_9_9_9_SHAREDEXP, 0),
+	FMT_32_BIT(V_038004_FMT_32_AS_8, 0),
+	FMT_32_BIT(V_038004_FMT_32_AS_8_8, 0),
+
+	/* 48-bit */
+	FMT_48_BIT(V_038004_FMT_16_16_16),
+	FMT_48_BIT(V_038004_FMT_16_16_16_FLOAT),
+
+	/* 64-bit */
+	FMT_64_BIT(V_038004_COLOR_X24_8_32_FLOAT, 1),
+	FMT_64_BIT(V_038004_COLOR_32_32, 1),
+	FMT_64_BIT(V_038004_COLOR_32_32_FLOAT, 1),
+	FMT_64_BIT(V_038004_COLOR_16_16_16_16, 1),
+	FMT_64_BIT(V_038004_COLOR_16_16_16_16_FLOAT, 1),
+
+	FMT_96_BIT(V_038004_FMT_32_32_32),
+	FMT_96_BIT(V_038004_FMT_32_32_32_FLOAT),
+
+	/* 128-bit */
+	FMT_128_BIT(V_038004_COLOR_32_32_32_32, 1),
+	FMT_128_BIT(V_038004_COLOR_32_32_32_32_FLOAT, 1),
+
+	[V_038004_FMT_GB_GR] = { 2, 1, 4, 0 },
+	[V_038004_FMT_BG_RG] = { 2, 1, 4, 0 },
+
+	/* block compressed formats */
+	[V_038004_FMT_BC1] = { 4, 4, 8, 0 },
+	[V_038004_FMT_BC2] = { 4, 4, 16, 0 },
+	[V_038004_FMT_BC3] = { 4, 4, 16, 0 },
+	[V_038004_FMT_BC4] = { 4, 4, 8, 0 },
+	[V_038004_FMT_BC5] = { 4, 4, 16, 0},
+
+};
+
+static inline bool fmt_is_valid_color(u32 format)
+{
+	if (format >= ARRAY_SIZE(color_formats_table))
+		return false;
+	
+	if (color_formats_table[format].valid_color)
+		return true;
+
+	return false;
+}
+
+static inline bool fmt_is_valid_texture(u32 format)
+{
+	if (format >= ARRAY_SIZE(color_formats_table))
+		return false;
+	
+	if (color_formats_table[format].blockwidth > 0)
+		return true;
+
+	return false;
+}
+
+static inline int fmt_get_blocksize(u32 format)
+{
+	if (format >= ARRAY_SIZE(color_formats_table))
+		return 0;
+
+	return color_formats_table[format].blocksize;
+}
+
+static inline int fmt_get_nblocksx(u32 format, u32 w)
+{
+	unsigned bw;
+
+	if (format >= ARRAY_SIZE(color_formats_table))
+		return 0;
+
+	bw = color_formats_table[format].blockwidth;
+	if (bw == 0)
+		return 0;
+
+	return (w + bw - 1) / bw;
+}
+
+static inline int fmt_get_nblocksy(u32 format, u32 h)
+{
+	unsigned bh;
+
+	if (format >= ARRAY_SIZE(color_formats_table))
+		return 0;
+
+	bh = color_formats_table[format].blockheight;
+	if (bh == 0)
+		return 0;
+
+	return (h + bh - 1) / bh;
+}
+
 static inline int r600_bpe_from_format(u32 *bpe, u32 format)
 {
-	switch (format) {
-	case V_038004_COLOR_8:
-	case V_038004_COLOR_4_4:
-	case V_038004_COLOR_3_3_2:
-	case V_038004_FMT_1:
-		*bpe = 1;
-		break;
-	case V_038004_COLOR_16:
-	case V_038004_COLOR_16_FLOAT:
-	case V_038004_COLOR_8_8:
-	case V_038004_COLOR_5_6_5:
-	case V_038004_COLOR_6_5_5:
-	case V_038004_COLOR_1_5_5_5:
-	case V_038004_COLOR_4_4_4_4:
-	case V_038004_COLOR_5_5_5_1:
-		*bpe = 2;
-		break;
-	case V_038004_FMT_8_8_8:
-		*bpe = 3;
-		break;
-	case V_038004_COLOR_32:
-	case V_038004_COLOR_32_FLOAT:
-	case V_038004_COLOR_16_16:
-	case V_038004_COLOR_16_16_FLOAT:
-	case V_038004_COLOR_8_24:
-	case V_038004_COLOR_8_24_FLOAT:
-	case V_038004_COLOR_24_8:
-	case V_038004_COLOR_24_8_FLOAT:
-	case V_038004_COLOR_10_11_11:
-	case V_038004_COLOR_10_11_11_FLOAT:
-	case V_038004_COLOR_11_11_10:
-	case V_038004_COLOR_11_11_10_FLOAT:
-	case V_038004_COLOR_2_10_10_10:
-	case V_038004_COLOR_8_8_8_8:
-	case V_038004_COLOR_10_10_10_2:
-	case V_038004_FMT_5_9_9_9_SHAREDEXP:
-	case V_038004_FMT_32_AS_8:
-	case V_038004_FMT_32_AS_8_8:
-		*bpe = 4;
-		break;
-	case V_038004_COLOR_X24_8_32_FLOAT:
-	case V_038004_COLOR_32_32:
-	case V_038004_COLOR_32_32_FLOAT:
-	case V_038004_COLOR_16_16_16_16:
-	case V_038004_COLOR_16_16_16_16_FLOAT:
-		*bpe = 8;
-		break;
-	case V_038004_FMT_16_16_16:
-	case V_038004_FMT_16_16_16_FLOAT:
-		*bpe = 6;
-		break;
-	case V_038004_FMT_32_32_32:
-	case V_038004_FMT_32_32_32_FLOAT:
-		*bpe = 12;
-		break;
-	case V_038004_COLOR_32_32_32_32:
-	case V_038004_COLOR_32_32_32_32_FLOAT:
-		*bpe = 16;
-		break;
-	case V_038004_FMT_GB_GR:
-	case V_038004_FMT_BG_RG:
-	case V_038004_COLOR_INVALID:
-	default:
-		*bpe = 16;
-		return -EINVAL;
-	}
+ 	unsigned res;
+
+	if (format >= ARRAY_SIZE(color_formats_table))
+		goto fail;
+
+	res = color_formats_table[format].blocksize;
+	if (res == 0)
+		goto fail;
+
+	*bpe = res;
 	return 0;
+
+fail:
+	*bpe = 16;
+	return -EINVAL;
 }
 
 struct array_mode_checker {
@@ -148,7 +240,7 @@ struct array_mode_checker {
 	u32 nbanks;
 	u32 npipes;
 	u32 nsamples;
-	u32 bpe;
+	u32 blocksize;
 };
 
 /* returns alignment in pixels for pitch/height/depth and bytes for base */
@@ -162,7 +254,7 @@ static inline int r600_get_array_mode_alignment(struct array_mode_checker *value
 	u32 tile_height = 8;
 	u32 macro_tile_width = values->nbanks;
 	u32 macro_tile_height = values->npipes;
-	u32 tile_bytes = tile_width * tile_height * values->bpe * values->nsamples;
+	u32 tile_bytes = tile_width * tile_height * values->blocksize * values->nsamples;
 	u32 macro_tile_bytes = macro_tile_width * macro_tile_height * tile_bytes;
 
 	switch (values->array_mode) {
@@ -174,7 +266,7 @@ static inline int r600_get_array_mode_alignment(struct array_mode_checker *value
 		*base_align = 1;
 		break;
 	case ARRAY_LINEAR_ALIGNED:
-		*pitch_align = max((u32)64, (u32)(values->group_size / values->bpe));
+		*pitch_align = max((u32)64, (u32)(values->group_size / values->blocksize));
 		*height_align = tile_height;
 		*depth_align = 1;
 		*base_align = values->group_size;
@@ -182,7 +274,7 @@ static inline int r600_get_array_mode_alignment(struct array_mode_checker *value
 	case ARRAY_1D_TILED_THIN1:
 		*pitch_align = max((u32)tile_width,
 				   (u32)(values->group_size /
-					 (tile_height * values->bpe * values->nsamples)));
+					 (tile_height * values->blocksize * values->nsamples)));
 		*height_align = tile_height;
 		*depth_align = 1;
 		*base_align = values->group_size;
@@ -190,12 +282,12 @@ static inline int r600_get_array_mode_alignment(struct array_mode_checker *value
 	case ARRAY_2D_TILED_THIN1:
 		*pitch_align = max((u32)macro_tile_width,
 				  (u32)(((values->group_size / tile_height) /
-					 (values->bpe * values->nsamples)) *
+					 (values->blocksize * values->nsamples)) *
 					values->nbanks)) * tile_width;
 		*height_align = macro_tile_height * tile_height;
 		*depth_align = 1;
 		*base_align = max(macro_tile_bytes,
-				  (*pitch_align) * values->bpe * (*height_align) * values->nsamples);
+				  (*pitch_align) * values->blocksize * (*height_align) * values->nsamples);
 		break;
 	default:
 		return -EINVAL;
@@ -234,21 +326,22 @@ static void r600_cs_track_init(struct r600_cs_track *track)
 static inline int r600_cs_track_validate_cb(struct radeon_cs_parser *p, int i)
 {
 	struct r600_cs_track *track = p->track;
-	u32 bpe = 0, slice_tile_max, size, tmp;
+	u32 slice_tile_max, size, tmp;
 	u32 height, height_align, pitch, pitch_align, depth_align;
 	u64 base_offset, base_align;
 	struct array_mode_checker array_check;
 	volatile u32 *ib = p->ib->ptr;
 	unsigned array_mode;
-
+	u32 format;
 	if (G_0280A0_TILE_MODE(track->cb_color_info[i])) {
 		dev_warn(p->dev, "FMASK or CMASK buffer are not supported by this kernel\n");
 		return -EINVAL;
 	}
 	size = radeon_bo_size(track->cb_color_bo[i]) - track->cb_color_bo_offset[i];
-	if (r600_bpe_from_format(&bpe, G_0280A0_FORMAT(track->cb_color_info[i]))) {
+	format = G_0280A0_FORMAT(track->cb_color_info[i]);
+	if (!fmt_is_valid_color(format)) {
 		dev_warn(p->dev, "%s:%d cb invalid format %d for %d (0x%08X)\n",
-			 __func__, __LINE__, G_0280A0_FORMAT(track->cb_color_info[i]),
+			 __func__, __LINE__, format,
 			i, track->cb_color_info[i]);
 		return -EINVAL;
 	}
@@ -267,7 +360,7 @@ static inline int r600_cs_track_validate_cb(struct radeon_cs_parser *p, int i)
 	array_check.nbanks = track->nbanks;
 	array_check.npipes = track->npipes;
 	array_check.nsamples = track->nsamples;
-	array_check.bpe = bpe;
+	array_check.blocksize = fmt_get_blocksize(format);
 	if (r600_get_array_mode_alignment(&array_check,
 					  &pitch_align, &height_align, &depth_align, &base_align)) {
 		dev_warn(p->dev, "%s invalid tiling %d for %d (0x%08X)\n", __func__,
@@ -311,7 +404,7 @@ static inline int r600_cs_track_validate_cb(struct radeon_cs_parser *p, int i)
 	}
 
 	/* check offset */
-	tmp = height * pitch * bpe;
+	tmp = fmt_get_nblocksy(format, height) * fmt_get_nblocksx(format, pitch) * fmt_get_blocksize(format);
 	if ((tmp + track->cb_color_bo_offset[i]) > radeon_bo_size(track->cb_color_bo[i])) {
 		if (array_mode == V_0280A0_ARRAY_LINEAR_GENERAL) {
 			/* the initial DDX does bad things with the CB size occasionally */
@@ -436,7 +529,7 @@ static int r600_cs_track_check(struct radeon_cs_parser *p)
 			array_check.nbanks = track->nbanks;
 			array_check.npipes = track->npipes;
 			array_check.nsamples = track->nsamples;
-			array_check.bpe = bpe;
+			array_check.blocksize = bpe;
 			if (r600_get_array_mode_alignment(&array_check,
 							  &pitch_align, &height_align, &depth_align, &base_align)) {
 				dev_warn(p->dev, "%s invalid tiling %d (0x%08X)\n", __func__,
@@ -1113,39 +1206,61 @@ static inline int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx
 	return 0;
 }
 
-static inline unsigned minify(unsigned size, unsigned levels)
+static inline unsigned mip_minify(unsigned size, unsigned level)
 {
-	size = size >> levels;
-	if (size < 1)
-		size = 1;
-	return size;
+	unsigned val;
+
+	val = max(1U, size >> level);
+	if (level > 0)
+		val = roundup_pow_of_two(val);
+	return val;
 }
 
-static void r600_texture_size(unsigned nfaces, unsigned blevel, unsigned nlevels,
-			      unsigned w0, unsigned h0, unsigned d0, unsigned bpe,
-			      unsigned pitch_align,
+static void r600_texture_size(unsigned nfaces, unsigned blevel, unsigned llevel,
+			      unsigned w0, unsigned h0, unsigned d0, unsigned format,
+			      unsigned block_align, unsigned height_align, unsigned base_align,
 			      unsigned *l0_size, unsigned *mipmap_size)
 {
-	unsigned offset, i, level, face;
-	unsigned width, height, depth, rowstride, size;
-
-	w0 = minify(w0, 0);
-	h0 = minify(h0, 0);
-	d0 = minify(d0, 0);
+	unsigned offset, i, level;
+	unsigned width, height, depth, size;
+	unsigned blocksize;
+	unsigned nbx, nby;
+	unsigned nlevels = llevel - blevel + 1;
+
+	*l0_size = -1;
+	blocksize = fmt_get_blocksize(format);
+
+	w0 = mip_minify(w0, 0);
+	h0 = mip_minify(h0, 0);
+	d0 = mip_minify(d0, 0);
 	for(i = 0, offset = 0, level = blevel; i < nlevels; i++, level++) {
-		width = minify(w0, i);
-		height = minify(h0, i);
-		depth = minify(d0, i);
-		for(face = 0; face < nfaces; face++) {
-			rowstride = ALIGN((width * bpe), pitch_align);
-			size = height * rowstride * depth;
-			offset += size;
-			offset = (offset + 0x1f) & ~0x1f;
-		}
+		width = mip_minify(w0, i);
+		nbx = fmt_get_nblocksx(format, width);
+
+		nbx = round_up(nbx, block_align);
+
+		height = mip_minify(h0, i);
+		nby = fmt_get_nblocksy(format, height);
+		nby = round_up(nby, height_align);
+
+		depth = mip_minify(d0, i);
+
+		size = nbx * nby * blocksize;
+		if (nfaces)
+			size *= nfaces;
+		else
+			size *= depth;
+
+		if (i == 0)
+			*l0_size = size;
+
+		if (i == 0 || i == 1)
+			offset = round_up(offset, base_align);
+
+		offset += size;
 	}
-	*l0_size = ALIGN((w0 * bpe), pitch_align) * h0 * d0;
 	*mipmap_size = offset;
-	if (!nlevels)
+	if (llevel == 0)
 		*mipmap_size = *l0_size;
 	if (!blevel)
 		*mipmap_size -= *l0_size;
@@ -1169,11 +1284,13 @@ static inline int r600_check_texture_resource(struct radeon_cs_parser *p,  u32 i
 					      u32 tiling_flags)
 {
 	struct r600_cs_track *track = p->track;
-	u32 nfaces, nlevels, blevel, w0, h0, d0, bpe = 0;
-	u32 word0, word1, l0_size, mipmap_size;
+	u32 nfaces, llevel, blevel, w0, h0, d0;
+	u32 word0, word1, l0_size, mipmap_size, word2, word3;
 	u32 height_align, pitch, pitch_align, depth_align;
+	u32 array, barray, larray;
 	u64 base_align;
 	struct array_mode_checker array_check;
+	u32 format;
 
 	/* on legacy kernel we don't perform advanced check */
 	if (p->rdev == NULL)
@@ -1199,19 +1316,25 @@ static inline int r600_check_texture_resource(struct radeon_cs_parser *p,  u32 i
 	case V_038000_SQ_TEX_DIM_3D:
 		break;
 	case V_038000_SQ_TEX_DIM_CUBEMAP:
-		nfaces = 6;
+		if (p->family >= CHIP_RV770)
+			nfaces = 8;
+		else
+			nfaces = 6;
 		break;
 	case V_038000_SQ_TEX_DIM_1D_ARRAY:
 	case V_038000_SQ_TEX_DIM_2D_ARRAY:
+		array = 1;
+		break;
 	case V_038000_SQ_TEX_DIM_2D_MSAA:
 	case V_038000_SQ_TEX_DIM_2D_ARRAY_MSAA:
 	default:
 		dev_warn(p->dev, "this kernel doesn't support %d texture dim\n", G_038000_DIM(word0));
 		return -EINVAL;
 	}
-	if (r600_bpe_from_format(&bpe,  G_038004_DATA_FORMAT(word1))) {
+	format = G_038004_DATA_FORMAT(word1);
+	if (!fmt_is_valid_texture(format)) {
 		dev_warn(p->dev, "%s:%d texture invalid format %d\n",
-			 __func__, __LINE__, G_038004_DATA_FORMAT(word1));
+			 __func__, __LINE__, format);
 		return -EINVAL;
 	}
 
@@ -1222,7 +1345,7 @@ static inline int r600_check_texture_resource(struct radeon_cs_parser *p,  u32 i
 	array_check.nbanks = track->nbanks;
 	array_check.npipes = track->npipes;
 	array_check.nsamples = 1;
-	array_check.bpe = bpe;
+	array_check.blocksize = fmt_get_blocksize(format);
 	if (r600_get_array_mode_alignment(&array_check,
 					  &pitch_align, &height_align, &depth_align, &base_align)) {
 		dev_warn(p->dev, "%s:%d tex array mode (%d) invalid\n",
@@ -1248,25 +1371,34 @@ static inline int r600_check_texture_resource(struct radeon_cs_parser *p,  u32 i
 		return -EINVAL;
 	}
 
+	word2 = radeon_get_ib_value(p, idx + 2) << 8;
+	word3 = radeon_get_ib_value(p, idx + 3) << 8;
+
 	word0 = radeon_get_ib_value(p, idx + 4);
 	word1 = radeon_get_ib_value(p, idx + 5);
 	blevel = G_038010_BASE_LEVEL(word0);
-	nlevels = G_038014_LAST_LEVEL(word1);
-	r600_texture_size(nfaces, blevel, nlevels, w0, h0, d0, bpe,
-			  (pitch_align * bpe),
+	llevel = G_038014_LAST_LEVEL(word1);
+	if (array == 1) {
+		barray = G_038014_BASE_ARRAY(word1);
+		larray = G_038014_LAST_ARRAY(word1);
+
+		nfaces = larray - barray + 1;
+	}
+	r600_texture_size(nfaces, blevel, llevel, w0, h0, d0, format,
+			  pitch_align, height_align, base_align,
 			  &l0_size, &mipmap_size);
 	/* using get ib will give us the offset into the texture bo */
-	word0 = radeon_get_ib_value(p, idx + 2) << 8;
-	if ((l0_size + word0) > radeon_bo_size(texture)) {
+	if ((l0_size + word2) > radeon_bo_size(texture)) {
 		dev_warn(p->dev, "texture bo too small (%d %d %d %d -> %d have %ld)\n",
-			w0, h0, bpe, word0, l0_size, radeon_bo_size(texture));
+			w0, h0, format, word2, l0_size, radeon_bo_size(texture));
+		dev_warn(p->dev, "alignments %d %d %d %lld\n", pitch, pitch_align, height_align, base_align);
 		return -EINVAL;
 	}
 	/* using get ib will give us the offset into the mipmap bo */
-	word0 = radeon_get_ib_value(p, idx + 3) << 8;
-	if ((mipmap_size + word0) > radeon_bo_size(mipmap)) {
+	word3 = radeon_get_ib_value(p, idx + 3) << 8;
+	if ((mipmap_size + word3) > radeon_bo_size(mipmap)) {
 		/*dev_warn(p->dev, "mipmap bo too small (%d %d %d %d %d %d -> %d have %ld)\n",
-		  w0, h0, bpe, blevel, nlevels, word0, mipmap_size, radeon_bo_size(texture));*/
+		  w0, h0, format, blevel, nlevels, word3, mipmap_size, radeon_bo_size(texture));*/
 	}
 	return 0;
 }
@@ -1289,6 +1421,38 @@ static int r600_packet3_check(struct radeon_cs_parser *p,
 	idx_value = radeon_get_ib_value(p, idx);
 
 	switch (pkt->opcode) {
+	case PACKET3_SET_PREDICATION:
+	{
+		int pred_op;
+		int tmp;
+		if (pkt->count != 1) {
+			DRM_ERROR("bad SET PREDICATION\n");
+			return -EINVAL;
+		}
+
+		tmp = radeon_get_ib_value(p, idx + 1);
+		pred_op = (tmp >> 16) & 0x7;
+
+		/* for the clear predicate operation */
+		if (pred_op == 0)
+			return 0;
+
+		if (pred_op > 2) {
+			DRM_ERROR("bad SET PREDICATION operation %d\n", pred_op);
+			return -EINVAL;
+		}
+
+		r = r600_cs_packet_next_reloc(p, &reloc);
+		if (r) {
+			DRM_ERROR("bad SET PREDICATION\n");
+			return -EINVAL;
+		}
+
+		ib[idx + 0] = idx_value + (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+		ib[idx + 1] = tmp + (upper_32_bits(reloc->lobj.gpu_offset) & 0xff);
+	}
+	break;
+
 	case PACKET3_START_3D_CMDBUF:
 		if (p->family >= CHIP_RV770 || pkt->count) {
 			DRM_ERROR("bad START_3D\n");
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index e6a58ed48dcf..50db6d62eec2 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -26,6 +26,7 @@
 #include "drmP.h"
 #include "radeon_drm.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "atom.h"
 
 /*
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index 04bac0bbd3ec..b2b944bcd05a 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -1304,6 +1304,11 @@
 #define     V_038004_FMT_16_16_16_FLOAT                0x0000002E
 #define     V_038004_FMT_32_32_32                      0x0000002F
 #define     V_038004_FMT_32_32_32_FLOAT                0x00000030
+#define     V_038004_FMT_BC1                           0x00000031
+#define     V_038004_FMT_BC2                           0x00000032
+#define     V_038004_FMT_BC3                           0x00000033
+#define     V_038004_FMT_BC4                           0x00000034
+#define     V_038004_FMT_BC5                           0x00000035
 #define R_038010_SQ_TEX_RESOURCE_WORD4_0             0x038010
 #define   S_038010_FORMAT_COMP_X(x)                    (((x) & 0x3) << 0)
 #define   G_038010_FORMAT_COMP_X(x)                    (((x) >> 0) & 0x3)
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 56c48b67ef3d..6989e3422e87 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -258,8 +258,9 @@ struct radeon_bo {
 	int				surface_reg;
 	/* Constant after initialization */
 	struct radeon_device		*rdev;
-	struct drm_gem_object		*gobj;
+	struct drm_gem_object		gem_base;
 };
+#define gem_to_radeon_bo(gobj) container_of((gobj), struct radeon_bo, gem_base)
 
 struct radeon_bo_list {
 	struct ttm_validate_buffer tv;
@@ -288,6 +289,15 @@ int radeon_gem_object_pin(struct drm_gem_object *obj, uint32_t pin_domain,
 			  uint64_t *gpu_addr);
 void radeon_gem_object_unpin(struct drm_gem_object *obj);
 
+int radeon_mode_dumb_create(struct drm_file *file_priv,
+			    struct drm_device *dev,
+			    struct drm_mode_create_dumb *args);
+int radeon_mode_dumb_mmap(struct drm_file *filp,
+			  struct drm_device *dev,
+			  uint32_t handle, uint64_t *offset_p);
+int radeon_mode_dumb_destroy(struct drm_file *file_priv,
+			     struct drm_device *dev,
+			     uint32_t handle);
 
 /*
  * GART structures, functions & helpers
@@ -319,6 +329,7 @@ struct radeon_gart {
 	union radeon_gart_table		table;
 	struct page			**pages;
 	dma_addr_t			*pages_addr;
+	bool				*ttm_alloced;
 	bool				ready;
 };
 
@@ -331,7 +342,8 @@ void radeon_gart_fini(struct radeon_device *rdev);
 void radeon_gart_unbind(struct radeon_device *rdev, unsigned offset,
 			int pages);
 int radeon_gart_bind(struct radeon_device *rdev, unsigned offset,
-		     int pages, struct page **pagelist);
+		     int pages, struct page **pagelist,
+		     dma_addr_t *dma_addr);
 
 
 /*
@@ -652,6 +664,8 @@ struct radeon_wb {
 
 #define RADEON_WB_SCRATCH_OFFSET 0
 #define RADEON_WB_CP_RPTR_OFFSET 1024
+#define RADEON_WB_CP1_RPTR_OFFSET 1280
+#define RADEON_WB_CP2_RPTR_OFFSET 1536
 #define R600_WB_IH_WPTR_OFFSET   2048
 #define R600_WB_EVENT_OFFSET     3072
 
@@ -1038,12 +1052,52 @@ struct evergreen_asic {
 	struct r100_gpu_lockup	lockup;
 };
 
+struct cayman_asic {
+	unsigned max_shader_engines;
+	unsigned max_pipes_per_simd;
+	unsigned max_tile_pipes;
+	unsigned max_simds_per_se;
+	unsigned max_backends_per_se;
+	unsigned max_texture_channel_caches;
+	unsigned max_gprs;
+	unsigned max_threads;
+	unsigned max_gs_threads;
+	unsigned max_stack_entries;
+	unsigned sx_num_of_sets;
+	unsigned sx_max_export_size;
+	unsigned sx_max_export_pos_size;
+	unsigned sx_max_export_smx_size;
+	unsigned max_hw_contexts;
+	unsigned sq_num_cf_insts;
+	unsigned sc_prim_fifo_size;
+	unsigned sc_hiz_tile_fifo_size;
+	unsigned sc_earlyz_tile_fifo_size;
+
+	unsigned num_shader_engines;
+	unsigned num_shader_pipes_per_simd;
+	unsigned num_tile_pipes;
+	unsigned num_simds_per_se;
+	unsigned num_backends_per_se;
+	unsigned backend_disable_mask_per_asic;
+	unsigned backend_map;
+	unsigned num_texture_channel_caches;
+	unsigned mem_max_burst_length_bytes;
+	unsigned mem_row_size_in_kb;
+	unsigned shader_engine_tile_size;
+	unsigned num_gpus;
+	unsigned multi_gpu_tile_size;
+
+	unsigned tile_config;
+	struct r100_gpu_lockup	lockup;
+};
+
 union radeon_asic_config {
 	struct r300_asic	r300;
 	struct r100_asic	r100;
 	struct r600_asic	r600;
 	struct rv770_asic	rv770;
 	struct evergreen_asic	evergreen;
+	struct cayman_asic	cayman;
 };
 
 /*
@@ -1134,6 +1188,9 @@ struct radeon_device {
 	struct radeon_mman		mman;
 	struct radeon_fence_driver	fence_drv;
 	struct radeon_cp		cp;
+	/* cayman compute rings */
+	struct radeon_cp		cp1;
+	struct radeon_cp		cp2;
 	struct radeon_ib_pool		ib_pool;
 	struct radeon_irq		irq;
 	struct radeon_asic		*asic;
@@ -1186,19 +1243,6 @@ int radeon_device_init(struct radeon_device *rdev,
 void radeon_device_fini(struct radeon_device *rdev);
 int radeon_gpu_wait_for_idle(struct radeon_device *rdev);
 
-/* r600 blit */
-int r600_blit_prepare_copy(struct radeon_device *rdev, int size_bytes);
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence);
-void r600_kms_blit_copy(struct radeon_device *rdev,
-			u64 src_gpu_addr, u64 dst_gpu_addr,
-			int size_bytes);
-/* evergreen blit */
-int evergreen_blit_prepare_copy(struct radeon_device *rdev, int size_bytes);
-void evergreen_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence);
-void evergreen_kms_blit_copy(struct radeon_device *rdev,
-			     u64 src_gpu_addr, u64 dst_gpu_addr,
-			     int size_bytes);
-
 static inline uint32_t r100_mm_rreg(struct radeon_device *rdev, uint32_t reg)
 {
 	if (reg < rdev->rmmio_size)
@@ -1449,62 +1493,15 @@ extern void radeon_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc
 extern int radeon_resume_kms(struct drm_device *dev);
 extern int radeon_suspend_kms(struct drm_device *dev, pm_message_t state);
 
-/* r600, rv610, rv630, rv620, rv635, rv670, rs780, rs880 */
-extern bool r600_card_posted(struct radeon_device *rdev);
-extern void r600_cp_stop(struct radeon_device *rdev);
-extern int r600_cp_start(struct radeon_device *rdev);
-extern void r600_ring_init(struct radeon_device *rdev, unsigned ring_size);
-extern int r600_cp_resume(struct radeon_device *rdev);
-extern void r600_cp_fini(struct radeon_device *rdev);
-extern int r600_count_pipe_bits(uint32_t val);
-extern int r600_mc_wait_for_idle(struct radeon_device *rdev);
-extern int r600_pcie_gart_init(struct radeon_device *rdev);
-extern void r600_pcie_gart_tlb_flush(struct radeon_device *rdev);
-extern int r600_ib_test(struct radeon_device *rdev);
-extern int r600_ring_test(struct radeon_device *rdev);
-extern void r600_scratch_init(struct radeon_device *rdev);
-extern int r600_blit_init(struct radeon_device *rdev);
-extern void r600_blit_fini(struct radeon_device *rdev);
-extern int r600_init_microcode(struct radeon_device *rdev);
-extern int r600_asic_reset(struct radeon_device *rdev);
-/* r600 irq */
-extern int r600_irq_init(struct radeon_device *rdev);
-extern void r600_irq_fini(struct radeon_device *rdev);
-extern void r600_ih_ring_init(struct radeon_device *rdev, unsigned ring_size);
-extern int r600_irq_set(struct radeon_device *rdev);
-extern void r600_irq_suspend(struct radeon_device *rdev);
-extern void r600_disable_interrupts(struct radeon_device *rdev);
-extern void r600_rlc_stop(struct radeon_device *rdev);
-/* r600 audio */
-extern int r600_audio_init(struct radeon_device *rdev);
-extern int r600_audio_tmds_index(struct drm_encoder *encoder);
-extern void r600_audio_set_clock(struct drm_encoder *encoder, int clock);
-extern int r600_audio_channels(struct radeon_device *rdev);
-extern int r600_audio_bits_per_sample(struct radeon_device *rdev);
-extern int r600_audio_rate(struct radeon_device *rdev);
-extern uint8_t r600_audio_status_bits(struct radeon_device *rdev);
-extern uint8_t r600_audio_category_code(struct radeon_device *rdev);
-extern void r600_audio_schedule_polling(struct radeon_device *rdev);
-extern void r600_audio_enable_polling(struct drm_encoder *encoder);
-extern void r600_audio_disable_polling(struct drm_encoder *encoder);
-extern void r600_audio_fini(struct radeon_device *rdev);
-extern void r600_hdmi_init(struct drm_encoder *encoder);
+/*
+ * r600 functions used by radeon_encoder.c
+ */
 extern void r600_hdmi_enable(struct drm_encoder *encoder);
 extern void r600_hdmi_disable(struct drm_encoder *encoder);
 extern void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
-extern int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder);
-extern void r600_hdmi_update_audio_settings(struct drm_encoder *encoder);
-
-extern void r700_vram_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc);
-extern void r700_cp_stop(struct radeon_device *rdev);
-extern void r700_cp_fini(struct radeon_device *rdev);
-extern void evergreen_disable_interrupt_state(struct radeon_device *rdev);
-extern int evergreen_irq_set(struct radeon_device *rdev);
-extern int evergreen_blit_init(struct radeon_device *rdev);
-extern void evergreen_blit_fini(struct radeon_device *rdev);
 
 extern int ni_init_microcode(struct radeon_device *rdev);
-extern int btc_mc_load_microcode(struct radeon_device *rdev);
+extern int ni_mc_load_microcode(struct radeon_device *rdev);
 
 /* radeon_acpi.c */ 
 #if defined(CONFIG_ACPI) 
@@ -1513,14 +1510,6 @@ extern int radeon_acpi_init(struct radeon_device *rdev);
 static inline int radeon_acpi_init(struct radeon_device *rdev) { return 0; } 
 #endif 
 
-/* evergreen */
-struct evergreen_mc_save {
-	u32 vga_control[6];
-	u32 vga_render_control;
-	u32 vga_hdp_control;
-	u32 crtc_control[6];
-};
-
 #include "radeon_object.h"
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index e75d63b8e21d..3c5d140d2efb 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -882,6 +882,52 @@ static struct radeon_asic btc_asic = {
 	.post_page_flip = &evergreen_post_page_flip,
 };
 
+static struct radeon_asic cayman_asic = {
+	.init = &cayman_init,
+	.fini = &cayman_fini,
+	.suspend = &cayman_suspend,
+	.resume = &cayman_resume,
+	.cp_commit = &r600_cp_commit,
+	.gpu_is_lockup = &cayman_gpu_is_lockup,
+	.asic_reset = &cayman_asic_reset,
+	.vga_set_state = &r600_vga_set_state,
+	.gart_tlb_flush = &cayman_pcie_gart_tlb_flush,
+	.gart_set_page = &rs600_gart_set_page,
+	.ring_test = &r600_ring_test,
+	.ring_ib_execute = &evergreen_ring_ib_execute,
+	.irq_set = &evergreen_irq_set,
+	.irq_process = &evergreen_irq_process,
+	.get_vblank_counter = &evergreen_get_vblank_counter,
+	.fence_ring_emit = &r600_fence_ring_emit,
+	.cs_parse = &evergreen_cs_parse,
+	.copy_blit = NULL,
+	.copy_dma = NULL,
+	.copy = NULL,
+	.get_engine_clock = &radeon_atom_get_engine_clock,
+	.set_engine_clock = &radeon_atom_set_engine_clock,
+	.get_memory_clock = &radeon_atom_get_memory_clock,
+	.set_memory_clock = &radeon_atom_set_memory_clock,
+	.get_pcie_lanes = NULL,
+	.set_pcie_lanes = NULL,
+	.set_clock_gating = NULL,
+	.set_surface_reg = r600_set_surface_reg,
+	.clear_surface_reg = r600_clear_surface_reg,
+	.bandwidth_update = &evergreen_bandwidth_update,
+	.hpd_init = &evergreen_hpd_init,
+	.hpd_fini = &evergreen_hpd_fini,
+	.hpd_sense = &evergreen_hpd_sense,
+	.hpd_set_polarity = &evergreen_hpd_set_polarity,
+	.gui_idle = &r600_gui_idle,
+	.pm_misc = &evergreen_pm_misc,
+	.pm_prepare = &evergreen_pm_prepare,
+	.pm_finish = &evergreen_pm_finish,
+	.pm_init_profile = &r600_pm_init_profile,
+	.pm_get_dynpm_state = &r600_pm_get_dynpm_state,
+	.pre_page_flip = &evergreen_pre_page_flip,
+	.page_flip = &evergreen_page_flip,
+	.post_page_flip = &evergreen_post_page_flip,
+};
+
 int radeon_asic_init(struct radeon_device *rdev)
 {
 	radeon_register_accessor_init(rdev);
@@ -974,6 +1020,9 @@ int radeon_asic_init(struct radeon_device *rdev)
 	case CHIP_CAICOS:
 		rdev->asic = &btc_asic;
 		break;
+	case CHIP_CAYMAN:
+		rdev->asic = &cayman_asic;
+		break;
 	default:
 		/* FIXME: not supported yet */
 		return -EINVAL;
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index c59bd98a2029..3d7a0d7c6a9a 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -57,8 +57,6 @@ int r100_init(struct radeon_device *rdev);
 void r100_fini(struct radeon_device *rdev);
 int r100_suspend(struct radeon_device *rdev);
 int r100_resume(struct radeon_device *rdev);
-uint32_t r100_mm_rreg(struct radeon_device *rdev, uint32_t reg);
-void r100_mm_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 void r100_vga_set_state(struct radeon_device *rdev, bool state);
 bool r100_gpu_is_lockup(struct radeon_device *rdev);
 int r100_asic_reset(struct radeon_device *rdev);
@@ -164,8 +162,6 @@ extern void r300_fence_ring_emit(struct radeon_device *rdev,
 extern int r300_cs_parse(struct radeon_cs_parser *p);
 extern void rv370_pcie_gart_tlb_flush(struct radeon_device *rdev);
 extern int rv370_pcie_gart_set_page(struct radeon_device *rdev, int i, uint64_t addr);
-extern uint32_t rv370_pcie_rreg(struct radeon_device *rdev, uint32_t reg);
-extern void rv370_pcie_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 extern void rv370_set_pcie_lanes(struct radeon_device *rdev, int lanes);
 extern int rv370_get_pcie_lanes(struct radeon_device *rdev);
 extern void r300_set_reg_safe(struct radeon_device *rdev);
@@ -208,7 +204,6 @@ void rs400_gart_adjust_size(struct radeon_device *rdev);
 void rs400_gart_disable(struct radeon_device *rdev);
 void rs400_gart_fini(struct radeon_device *rdev);
 
-
 /*
  * rs600.
  */
@@ -270,8 +265,6 @@ void rv515_fini(struct radeon_device *rdev);
 uint32_t rv515_mc_rreg(struct radeon_device *rdev, uint32_t reg);
 void rv515_mc_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 void rv515_ring_start(struct radeon_device *rdev);
-uint32_t rv515_pcie_rreg(struct radeon_device *rdev, uint32_t reg);
-void rv515_pcie_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 void rv515_bandwidth_update(struct radeon_device *rdev);
 int rv515_resume(struct radeon_device *rdev);
 int rv515_suspend(struct radeon_device *rdev);
@@ -307,14 +300,13 @@ void r600_pciep_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 int r600_cs_parse(struct radeon_cs_parser *p);
 void r600_fence_ring_emit(struct radeon_device *rdev,
 			  struct radeon_fence *fence);
-int r600_irq_process(struct radeon_device *rdev);
-int r600_irq_set(struct radeon_device *rdev);
 bool r600_gpu_is_lockup(struct radeon_device *rdev);
 int r600_asic_reset(struct radeon_device *rdev);
 int r600_set_surface_reg(struct radeon_device *rdev, int reg,
 			 uint32_t tiling_flags, uint32_t pitch,
 			 uint32_t offset, uint32_t obj_size);
 void r600_clear_surface_reg(struct radeon_device *rdev, int reg);
+int r600_ib_test(struct radeon_device *rdev);
 void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
 int r600_ring_test(struct radeon_device *rdev);
 int r600_copy_blit(struct radeon_device *rdev,
@@ -333,6 +325,50 @@ extern void rs780_pm_init_profile(struct radeon_device *rdev);
 extern void r600_pm_get_dynpm_state(struct radeon_device *rdev);
 extern void r600_set_pcie_lanes(struct radeon_device *rdev, int lanes);
 extern int r600_get_pcie_lanes(struct radeon_device *rdev);
+bool r600_card_posted(struct radeon_device *rdev);
+void r600_cp_stop(struct radeon_device *rdev);
+int r600_cp_start(struct radeon_device *rdev);
+void r600_ring_init(struct radeon_device *rdev, unsigned ring_size);
+int r600_cp_resume(struct radeon_device *rdev);
+void r600_cp_fini(struct radeon_device *rdev);
+int r600_count_pipe_bits(uint32_t val);
+int r600_mc_wait_for_idle(struct radeon_device *rdev);
+int r600_pcie_gart_init(struct radeon_device *rdev);
+void r600_scratch_init(struct radeon_device *rdev);
+int r600_blit_init(struct radeon_device *rdev);
+void r600_blit_fini(struct radeon_device *rdev);
+int r600_init_microcode(struct radeon_device *rdev);
+/* r600 irq */
+int r600_irq_process(struct radeon_device *rdev);
+int r600_irq_init(struct radeon_device *rdev);
+void r600_irq_fini(struct radeon_device *rdev);
+void r600_ih_ring_init(struct radeon_device *rdev, unsigned ring_size);
+int r600_irq_set(struct radeon_device *rdev);
+void r600_irq_suspend(struct radeon_device *rdev);
+void r600_disable_interrupts(struct radeon_device *rdev);
+void r600_rlc_stop(struct radeon_device *rdev);
+/* r600 audio */
+int r600_audio_init(struct radeon_device *rdev);
+int r600_audio_tmds_index(struct drm_encoder *encoder);
+void r600_audio_set_clock(struct drm_encoder *encoder, int clock);
+int r600_audio_channels(struct radeon_device *rdev);
+int r600_audio_bits_per_sample(struct radeon_device *rdev);
+int r600_audio_rate(struct radeon_device *rdev);
+uint8_t r600_audio_status_bits(struct radeon_device *rdev);
+uint8_t r600_audio_category_code(struct radeon_device *rdev);
+void r600_audio_schedule_polling(struct radeon_device *rdev);
+void r600_audio_enable_polling(struct drm_encoder *encoder);
+void r600_audio_disable_polling(struct drm_encoder *encoder);
+void r600_audio_fini(struct radeon_device *rdev);
+void r600_hdmi_init(struct drm_encoder *encoder);
+int r600_hdmi_buffer_status_changed(struct drm_encoder *encoder);
+void r600_hdmi_update_audio_settings(struct drm_encoder *encoder);
+/* r600 blit */
+int r600_blit_prepare_copy(struct radeon_device *rdev, int size_bytes);
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence);
+void r600_kms_blit_copy(struct radeon_device *rdev,
+			u64 src_gpu_addr, u64 dst_gpu_addr,
+			int size_bytes);
 
 /*
  * rv770,rv730,rv710,rv740
@@ -341,12 +377,21 @@ int rv770_init(struct radeon_device *rdev);
 void rv770_fini(struct radeon_device *rdev);
 int rv770_suspend(struct radeon_device *rdev);
 int rv770_resume(struct radeon_device *rdev);
-extern void rv770_pm_misc(struct radeon_device *rdev);
-extern u32 rv770_page_flip(struct radeon_device *rdev, int crtc, u64 crtc_base);
+void rv770_pm_misc(struct radeon_device *rdev);
+u32 rv770_page_flip(struct radeon_device *rdev, int crtc, u64 crtc_base);
+void r700_vram_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc);
+void r700_cp_stop(struct radeon_device *rdev);
+void r700_cp_fini(struct radeon_device *rdev);
 
 /*
  * evergreen
  */
+struct evergreen_mc_save {
+	u32 vga_control[6];
+	u32 vga_render_control;
+	u32 vga_hdp_control;
+	u32 crtc_control[6];
+};
 void evergreen_pcie_gart_tlb_flush(struct radeon_device *rdev);
 int evergreen_init(struct radeon_device *rdev);
 void evergreen_fini(struct radeon_device *rdev);
@@ -374,5 +419,25 @@ extern void evergreen_pm_finish(struct radeon_device *rdev);
 extern void evergreen_pre_page_flip(struct radeon_device *rdev, int crtc);
 extern u32 evergreen_page_flip(struct radeon_device *rdev, int crtc, u64 crtc_base);
 extern void evergreen_post_page_flip(struct radeon_device *rdev, int crtc);
+void evergreen_disable_interrupt_state(struct radeon_device *rdev);
+int evergreen_blit_init(struct radeon_device *rdev);
+void evergreen_blit_fini(struct radeon_device *rdev);
+/* evergreen blit */
+int evergreen_blit_prepare_copy(struct radeon_device *rdev, int size_bytes);
+void evergreen_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence);
+void evergreen_kms_blit_copy(struct radeon_device *rdev,
+			     u64 src_gpu_addr, u64 dst_gpu_addr,
+			     int size_bytes);
+
+/*
+ * cayman
+ */
+void cayman_pcie_gart_tlb_flush(struct radeon_device *rdev);
+int cayman_init(struct radeon_device *rdev);
+void cayman_fini(struct radeon_device *rdev);
+int cayman_suspend(struct radeon_device *rdev);
+int cayman_resume(struct radeon_device *rdev);
+bool cayman_gpu_is_lockup(struct radeon_device *rdev);
+int cayman_asic_reset(struct radeon_device *rdev);
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_benchmark.c b/drivers/gpu/drm/radeon/radeon_benchmark.c
index c558685cc637..10191d9372d8 100644
--- a/drivers/gpu/drm/radeon/radeon_benchmark.c
+++ b/drivers/gpu/drm/radeon/radeon_benchmark.c
@@ -41,7 +41,7 @@ void radeon_benchmark_move(struct radeon_device *rdev, unsigned bsize,
 
 	size = bsize;
 	n = 1024;
-	r = radeon_bo_create(rdev, NULL, size, PAGE_SIZE, true, sdomain, &sobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, &sobj);
 	if (r) {
 		goto out_cleanup;
 	}
@@ -53,7 +53,7 @@ void radeon_benchmark_move(struct radeon_device *rdev, unsigned bsize,
 	if (r) {
 		goto out_cleanup;
 	}
-	r = radeon_bo_create(rdev, NULL, size, PAGE_SIZE, true, ddomain, &dobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, &dobj);
 	if (r) {
 		goto out_cleanup;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 22b7e3dc0eca..3f3c9aac46cc 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -972,7 +972,16 @@ static int radeon_dvi_mode_valid(struct drm_connector *connector,
 		    (radeon_connector->connector_object_id == CONNECTOR_OBJECT_ID_DUAL_LINK_DVI_D) ||
 		    (radeon_connector->connector_object_id == CONNECTOR_OBJECT_ID_HDMI_TYPE_B))
 			return MODE_OK;
-		else
+		else if (radeon_connector->connector_object_id == CONNECTOR_OBJECT_ID_HDMI_TYPE_A) {
+			if (ASIC_IS_DCE3(rdev)) {
+				/* HDMI 1.3+ supports max clock of 340 Mhz */
+				if (mode->clock > 340000)
+					return MODE_CLOCK_HIGH;
+				else
+					return MODE_OK;
+			} else
+				return MODE_CLOCK_HIGH;
+		} else
 			return MODE_CLOCK_HIGH;
 	}
 	return MODE_OK;
diff --git a/drivers/gpu/drm/radeon/radeon_cp.c b/drivers/gpu/drm/radeon/radeon_cp.c
index eb6b9eed7349..3d599e33b9cc 100644
--- a/drivers/gpu/drm/radeon/radeon_cp.c
+++ b/drivers/gpu/drm/radeon/radeon_cp.c
@@ -2113,9 +2113,9 @@ int radeon_driver_load(struct drm_device *dev, unsigned long flags)
 		break;
 	}
 
-	if (drm_device_is_agp(dev))
+	if (drm_pci_device_is_agp(dev))
 		dev_priv->flags |= RADEON_IS_AGP;
-	else if (drm_device_is_pcie(dev))
+	else if (drm_pci_device_is_pcie(dev))
 		dev_priv->flags |= RADEON_IS_PCIE;
 	else
 		dev_priv->flags |= RADEON_IS_PCI;
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 35b5eb8fbe2a..8c1916941871 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -75,7 +75,7 @@ int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 				return -ENOENT;
 			}
 			p->relocs_ptr[i] = &p->relocs[i];
-			p->relocs[i].robj = p->relocs[i].gobj->driver_private;
+			p->relocs[i].robj = gem_to_radeon_bo(p->relocs[i].gobj);
 			p->relocs[i].lobj.bo = p->relocs[i].robj;
 			p->relocs[i].lobj.wdomain = r->write_domain;
 			p->relocs[i].lobj.rdomain = r->read_domains;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 4954e2d6ffa2..f0209be7a34b 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -85,6 +85,7 @@ static const char radeon_family_name[][16] = {
 	"BARTS",
 	"TURKS",
 	"CAICOS",
+	"CAYMAN",
 	"LAST",
 };
 
@@ -184,7 +185,7 @@ int radeon_wb_init(struct radeon_device *rdev)
 	int r;
 
 	if (rdev->wb.wb_obj == NULL) {
-		r = radeon_bo_create(rdev, NULL, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
+		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
 				RADEON_GEM_DOMAIN_GTT, &rdev->wb.wb_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create WB bo failed\n", r);
@@ -860,7 +861,7 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 		if (rfb == NULL || rfb->obj == NULL) {
 			continue;
 		}
-		robj = rfb->obj->driver_private;
+		robj = gem_to_radeon_bo(rfb->obj);
 		/* don't unpin kernel fb objects */
 		if (!radeon_fbdev_robj_is_fb(rdev, robj)) {
 			r = radeon_bo_reserve(robj, false);
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 0e657095de7c..4be58793dc17 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -371,7 +371,7 @@ static int radeon_crtc_page_flip(struct drm_crtc *crtc,
 	new_radeon_fb = to_radeon_framebuffer(fb);
 	/* schedule unpin of the old buffer */
 	obj = old_radeon_fb->obj;
-	rbo = obj->driver_private;
+	rbo = gem_to_radeon_bo(obj);
 	work->old_rbo = rbo;
 	INIT_WORK(&work->work, radeon_unpin_work_func);
 
@@ -391,7 +391,7 @@ static int radeon_crtc_page_flip(struct drm_crtc *crtc,
 
 	/* pin the new buffer */
 	obj = new_radeon_fb->obj;
-	rbo = obj->driver_private;
+	rbo = gem_to_radeon_bo(obj);
 
 	DRM_DEBUG_DRIVER("flip-ioctl() cur_fbo = %p, cur_bbo = %p\n",
 			 work->old_rbo, rbo);
@@ -971,7 +971,7 @@ void radeon_compute_pll_legacy(struct radeon_pll *pll,
 		max_fractional_feed_div = pll->max_frac_feedback_div;
 	}
 
-	for (post_div = min_post_div; post_div <= max_post_div; ++post_div) {
+	for (post_div = max_post_div; post_div >= min_post_div; --post_div) {
 		uint32_t ref_div;
 
 		if ((pll->flags & RADEON_PLL_NO_ODD_POST_DIV) && (post_div & 1))
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 275b26a708d6..63d2de8771dc 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -49,9 +49,10 @@
  * - 2.6.0 - add tiling config query (r6xx+), add initial HiZ support (r300->r500)
  *   2.7.0 - fixups for r600 2D tiling support. (no external ABI change), add eg dyn gpr regs
  *   2.8.0 - pageflip support, r500 US_FORMAT regs. r500 ARGB2101010 colorbuf, r300->r500 CMASK, clock crystal query
+ *   2.9.0 - r600 tiling (s3tc,rgtc) working, SET_PREDICATION packet 3 on r600 + eg, backend query
  */
 #define KMS_DRIVER_MAJOR	2
-#define KMS_DRIVER_MINOR	8
+#define KMS_DRIVER_MINOR	9
 #define KMS_DRIVER_PATCHLEVEL	0
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags);
 int radeon_driver_unload_kms(struct drm_device *dev);
@@ -84,6 +85,16 @@ extern int radeon_get_crtc_scanoutpos(struct drm_device *dev, int crtc,
 extern struct drm_ioctl_desc radeon_ioctls_kms[];
 extern int radeon_max_kms_ioctl;
 int radeon_mmap(struct file *filp, struct vm_area_struct *vma);
+int radeon_mode_dumb_mmap(struct drm_file *filp,
+			  struct drm_device *dev,
+			  uint32_t handle, uint64_t *offset_p);
+int radeon_mode_dumb_create(struct drm_file *file_priv,
+			    struct drm_device *dev,
+			    struct drm_mode_create_dumb *args);
+int radeon_mode_dumb_destroy(struct drm_file *file_priv,
+			     struct drm_device *dev,
+			     uint32_t handle);
+
 #if defined(CONFIG_DEBUG_FS)
 int radeon_debugfs_init(struct drm_minor *minor);
 void radeon_debugfs_cleanup(struct drm_minor *minor);
@@ -228,11 +239,6 @@ static struct drm_driver driver_old = {
 		 .llseek = noop_llseek,
 	},
 
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-	},
-
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -322,6 +328,9 @@ static struct drm_driver kms_driver = {
 	.gem_init_object = radeon_gem_object_init,
 	.gem_free_object = radeon_gem_object_free,
 	.dma_ioctl = radeon_dma_ioctl_kms,
+	.dumb_create = radeon_mode_dumb_create,
+	.dumb_map_offset = radeon_mode_dumb_mmap,
+	.dumb_destroy = radeon_mode_dumb_destroy,
 	.fops = {
 		 .owner = THIS_MODULE,
 		 .open = drm_open,
@@ -336,15 +345,6 @@ static struct drm_driver kms_driver = {
 #endif
 	},
 
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-		 .probe = radeon_pci_probe,
-		 .remove = radeon_pci_remove,
-		 .suspend = radeon_pci_suspend,
-		 .resume = radeon_pci_resume,
-	},
-
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -354,15 +354,32 @@ static struct drm_driver kms_driver = {
 };
 
 static struct drm_driver *driver;
+static struct pci_driver *pdriver;
+
+static struct pci_driver radeon_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
+static struct pci_driver radeon_kms_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = radeon_pci_probe,
+	.remove = radeon_pci_remove,
+	.suspend = radeon_pci_suspend,
+	.resume = radeon_pci_resume,
+};
 
 static int __init radeon_init(void)
 {
 	driver = &driver_old;
+	pdriver = &radeon_pci_driver;
 	driver->num_ioctls = radeon_max_ioctl;
 #ifdef CONFIG_VGA_CONSOLE
 	if (vgacon_text_force() && radeon_modeset == -1) {
 		DRM_INFO("VGACON disable radeon kernel modesetting.\n");
 		driver = &driver_old;
+		pdriver = &radeon_pci_driver;
 		driver->driver_features &= ~DRIVER_MODESET;
 		radeon_modeset = 0;
 	}
@@ -380,18 +397,19 @@ static int __init radeon_init(void)
 	if (radeon_modeset == 1) {
 		DRM_INFO("radeon kernel modesetting enabled.\n");
 		driver = &kms_driver;
+		pdriver = &radeon_kms_pci_driver;
 		driver->driver_features |= DRIVER_MODESET;
 		driver->num_ioctls = radeon_max_kms_ioctl;
 		radeon_register_atpx_handler();
 	}
 	/* if the vga console setting is enabled still
 	 * let modprobe override it */
-	return drm_init(driver);
+	return drm_pci_init(driver, pdriver);
 }
 
 static void __exit radeon_exit(void)
 {
-	drm_exit(driver);
+	drm_pci_exit(driver, pdriver);
 	radeon_unregister_atpx_handler();
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_family.h b/drivers/gpu/drm/radeon/radeon_family.h
index 1ca55eb09ad3..6f1d9e563e77 100644
--- a/drivers/gpu/drm/radeon/radeon_family.h
+++ b/drivers/gpu/drm/radeon/radeon_family.h
@@ -84,6 +84,7 @@ enum radeon_family {
 	CHIP_BARTS,
 	CHIP_TURKS,
 	CHIP_CAICOS,
+	CHIP_CAYMAN,
 	CHIP_LAST,
 };
 
diff --git a/drivers/gpu/drm/radeon/radeon_fb.c b/drivers/gpu/drm/radeon/radeon_fb.c
index 66324b5bb5ba..0b7b486c97e8 100644
--- a/drivers/gpu/drm/radeon/radeon_fb.c
+++ b/drivers/gpu/drm/radeon/radeon_fb.c
@@ -64,7 +64,7 @@ static struct fb_ops radeonfb_ops = {
 };
 
 
-static int radeon_align_pitch(struct radeon_device *rdev, int width, int bpp, bool tiled)
+int radeon_align_pitch(struct radeon_device *rdev, int width, int bpp, bool tiled)
 {
 	int aligned = width;
 	int align_large = (ASIC_IS_AVIVO(rdev)) || tiled;
@@ -90,7 +90,7 @@ static int radeon_align_pitch(struct radeon_device *rdev, int width, int bpp, bo
 
 static void radeonfb_destroy_pinned_object(struct drm_gem_object *gobj)
 {
-	struct radeon_bo *rbo = gobj->driver_private;
+	struct radeon_bo *rbo = gem_to_radeon_bo(gobj);
 	int ret;
 
 	ret = radeon_bo_reserve(rbo, false);
@@ -113,11 +113,14 @@ static int radeonfb_create_pinned_object(struct radeon_fbdev *rfbdev,
 	u32 tiling_flags = 0;
 	int ret;
 	int aligned_size, size;
+	int height = mode_cmd->height;
 
 	/* need to align pitch with crtc limits */
 	mode_cmd->pitch = radeon_align_pitch(rdev, mode_cmd->width, mode_cmd->bpp, fb_tiled) * ((mode_cmd->bpp + 1) / 8);
 
-	size = mode_cmd->pitch * mode_cmd->height;
+	if (rdev->family >= CHIP_R600)
+		height = ALIGN(mode_cmd->height, 8);
+	size = mode_cmd->pitch * height;
 	aligned_size = ALIGN(size, PAGE_SIZE);
 	ret = radeon_gem_object_create(rdev, aligned_size, 0,
 				       RADEON_GEM_DOMAIN_VRAM,
@@ -128,7 +131,7 @@ static int radeonfb_create_pinned_object(struct radeon_fbdev *rfbdev,
 		       aligned_size);
 		return -ENOMEM;
 	}
-	rbo = gobj->driver_private;
+	rbo = gem_to_radeon_bo(gobj);
 
 	if (fb_tiled)
 		tiling_flags = RADEON_TILING_MACRO;
@@ -202,7 +205,7 @@ static int radeonfb_create(struct radeon_fbdev *rfbdev,
 	mode_cmd.depth = sizes->surface_depth;
 
 	ret = radeonfb_create_pinned_object(rfbdev, &mode_cmd, &gobj);
-	rbo = gobj->driver_private;
+	rbo = gem_to_radeon_bo(gobj);
 
 	/* okay we have an object now allocate the framebuffer */
 	info = framebuffer_alloc(0, device);
@@ -403,14 +406,14 @@ int radeon_fbdev_total_size(struct radeon_device *rdev)
 	struct radeon_bo *robj;
 	int size = 0;
 
-	robj = rdev->mode_info.rfbdev->rfb.obj->driver_private;
+	robj = gem_to_radeon_bo(rdev->mode_info.rfbdev->rfb.obj);
 	size += radeon_bo_size(robj);
 	return size;
 }
 
 bool radeon_fbdev_robj_is_fb(struct radeon_device *rdev, struct radeon_bo *robj)
 {
-	if (robj == rdev->mode_info.rfbdev->rfb.obj->driver_private)
+	if (robj == gem_to_radeon_bo(rdev->mode_info.rfbdev->rfb.obj))
 		return true;
 	return false;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index 65016117d95f..f0534ef2f331 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -78,7 +78,7 @@ int radeon_gart_table_vram_alloc(struct radeon_device *rdev)
 	int r;
 
 	if (rdev->gart.table.vram.robj == NULL) {
-		r = radeon_bo_create(rdev, NULL, rdev->gart.table_size,
+		r = radeon_bo_create(rdev, rdev->gart.table_size,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
 				     &rdev->gart.table.vram.robj);
 		if (r) {
@@ -149,8 +149,9 @@ void radeon_gart_unbind(struct radeon_device *rdev, unsigned offset,
 	p = t / (PAGE_SIZE / RADEON_GPU_PAGE_SIZE);
 	for (i = 0; i < pages; i++, p++) {
 		if (rdev->gart.pages[p]) {
-			pci_unmap_page(rdev->pdev, rdev->gart.pages_addr[p],
-				       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+			if (!rdev->gart.ttm_alloced[p])
+				pci_unmap_page(rdev->pdev, rdev->gart.pages_addr[p],
+				       		PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
 			rdev->gart.pages[p] = NULL;
 			rdev->gart.pages_addr[p] = rdev->dummy_page.addr;
 			page_base = rdev->gart.pages_addr[p];
@@ -165,7 +166,7 @@ void radeon_gart_unbind(struct radeon_device *rdev, unsigned offset,
 }
 
 int radeon_gart_bind(struct radeon_device *rdev, unsigned offset,
-		     int pages, struct page **pagelist)
+		     int pages, struct page **pagelist, dma_addr_t *dma_addr)
 {
 	unsigned t;
 	unsigned p;
@@ -180,15 +181,22 @@ int radeon_gart_bind(struct radeon_device *rdev, unsigned offset,
 	p = t / (PAGE_SIZE / RADEON_GPU_PAGE_SIZE);
 
 	for (i = 0; i < pages; i++, p++) {
-		/* we need to support large memory configurations */
-		/* assume that unbind have already been call on the range */
-		rdev->gart.pages_addr[p] = pci_map_page(rdev->pdev, pagelist[i],
+		/* On TTM path, we only use the DMA API if TTM_PAGE_FLAG_DMA32
+		 * is requested. */
+		if (dma_addr[i] != DMA_ERROR_CODE) {
+			rdev->gart.ttm_alloced[p] = true;
+			rdev->gart.pages_addr[p] = dma_addr[i];
+		} else {
+			/* we need to support large memory configurations */
+			/* assume that unbind have already been call on the range */
+			rdev->gart.pages_addr[p] = pci_map_page(rdev->pdev, pagelist[i],
 							0, PAGE_SIZE,
 							PCI_DMA_BIDIRECTIONAL);
-		if (pci_dma_mapping_error(rdev->pdev, rdev->gart.pages_addr[p])) {
-			/* FIXME: failed to map page (return -ENOMEM?) */
-			radeon_gart_unbind(rdev, offset, pages);
-			return -ENOMEM;
+			if (pci_dma_mapping_error(rdev->pdev, rdev->gart.pages_addr[p])) {
+				/* FIXME: failed to map page (return -ENOMEM?) */
+				radeon_gart_unbind(rdev, offset, pages);
+				return -ENOMEM;
+			}
 		}
 		rdev->gart.pages[p] = pagelist[i];
 		page_base = rdev->gart.pages_addr[p];
@@ -251,6 +259,12 @@ int radeon_gart_init(struct radeon_device *rdev)
 		radeon_gart_fini(rdev);
 		return -ENOMEM;
 	}
+	rdev->gart.ttm_alloced = kzalloc(sizeof(bool) *
+					 rdev->gart.num_cpu_pages, GFP_KERNEL);
+	if (rdev->gart.ttm_alloced == NULL) {
+		radeon_gart_fini(rdev);
+		return -ENOMEM;
+	}
 	/* set GART entry to point to the dummy page by default */
 	for (i = 0; i < rdev->gart.num_cpu_pages; i++) {
 		rdev->gart.pages_addr[i] = rdev->dummy_page.addr;
@@ -267,6 +281,8 @@ void radeon_gart_fini(struct radeon_device *rdev)
 	rdev->gart.ready = false;
 	kfree(rdev->gart.pages);
 	kfree(rdev->gart.pages_addr);
+	kfree(rdev->gart.ttm_alloced);
 	rdev->gart.pages = NULL;
 	rdev->gart.pages_addr = NULL;
+	rdev->gart.ttm_alloced = NULL;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index df95eb83dac6..a419b67d8401 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -32,21 +32,18 @@
 
 int radeon_gem_object_init(struct drm_gem_object *obj)
 {
-	/* we do nothings here */
+	BUG();
+
 	return 0;
 }
 
 void radeon_gem_object_free(struct drm_gem_object *gobj)
 {
-	struct radeon_bo *robj = gobj->driver_private;
+	struct radeon_bo *robj = gem_to_radeon_bo(gobj);
 
-	gobj->driver_private = NULL;
 	if (robj) {
 		radeon_bo_unref(&robj);
 	}
-
-	drm_gem_object_release(gobj);
-	kfree(gobj);
 }
 
 int radeon_gem_object_create(struct radeon_device *rdev, int size,
@@ -54,36 +51,34 @@ int radeon_gem_object_create(struct radeon_device *rdev, int size,
 				bool discardable, bool kernel,
 				struct drm_gem_object **obj)
 {
-	struct drm_gem_object *gobj;
 	struct radeon_bo *robj;
 	int r;
 
 	*obj = NULL;
-	gobj = drm_gem_object_alloc(rdev->ddev, size);
-	if (!gobj) {
-		return -ENOMEM;
-	}
 	/* At least align on page size */
 	if (alignment < PAGE_SIZE) {
 		alignment = PAGE_SIZE;
 	}
-	r = radeon_bo_create(rdev, gobj, size, alignment, kernel, initial_domain, &robj);
+	r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain, &robj);
 	if (r) {
 		if (r != -ERESTARTSYS)
 			DRM_ERROR("Failed to allocate GEM object (%d, %d, %u, %d)\n",
 				  size, initial_domain, alignment, r);
-		drm_gem_object_unreference_unlocked(gobj);
 		return r;
 	}
-	gobj->driver_private = robj;
-	*obj = gobj;
+	*obj = &robj->gem_base;
+
+	mutex_lock(&rdev->gem.mutex);
+	list_add_tail(&robj->list, &rdev->gem.objects);
+	mutex_unlock(&rdev->gem.mutex);
+
 	return 0;
 }
 
 int radeon_gem_object_pin(struct drm_gem_object *obj, uint32_t pin_domain,
 			  uint64_t *gpu_addr)
 {
-	struct radeon_bo *robj = obj->driver_private;
+	struct radeon_bo *robj = gem_to_radeon_bo(obj);
 	int r;
 
 	r = radeon_bo_reserve(robj, false);
@@ -96,7 +91,7 @@ int radeon_gem_object_pin(struct drm_gem_object *obj, uint32_t pin_domain,
 
 void radeon_gem_object_unpin(struct drm_gem_object *obj)
 {
-	struct radeon_bo *robj = obj->driver_private;
+	struct radeon_bo *robj = gem_to_radeon_bo(obj);
 	int r;
 
 	r = radeon_bo_reserve(robj, false);
@@ -114,7 +109,7 @@ int radeon_gem_set_domain(struct drm_gem_object *gobj,
 	int r;
 
 	/* FIXME: reeimplement */
-	robj = gobj->driver_private;
+	robj = gem_to_radeon_bo(gobj);
 	/* work out where to validate the buffer to */
 	domain = wdomain;
 	if (!domain) {
@@ -228,7 +223,7 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	if (gobj == NULL) {
 		return -ENOENT;
 	}
-	robj = gobj->driver_private;
+	robj = gem_to_radeon_bo(gobj);
 
 	r = radeon_gem_set_domain(gobj, args->read_domains, args->write_domain);
 
@@ -236,23 +231,31 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	return r;
 }
 
-int radeon_gem_mmap_ioctl(struct drm_device *dev, void *data,
-			  struct drm_file *filp)
+int radeon_mode_dumb_mmap(struct drm_file *filp,
+			  struct drm_device *dev,
+			  uint32_t handle, uint64_t *offset_p)
 {
-	struct drm_radeon_gem_mmap *args = data;
 	struct drm_gem_object *gobj;
 	struct radeon_bo *robj;
 
-	gobj = drm_gem_object_lookup(dev, filp, args->handle);
+	gobj = drm_gem_object_lookup(dev, filp, handle);
 	if (gobj == NULL) {
 		return -ENOENT;
 	}
-	robj = gobj->driver_private;
-	args->addr_ptr = radeon_bo_mmap_offset(robj);
+	robj = gem_to_radeon_bo(gobj);
+	*offset_p = radeon_bo_mmap_offset(robj);
 	drm_gem_object_unreference_unlocked(gobj);
 	return 0;
 }
 
+int radeon_gem_mmap_ioctl(struct drm_device *dev, void *data,
+			  struct drm_file *filp)
+{
+	struct drm_radeon_gem_mmap *args = data;
+
+	return radeon_mode_dumb_mmap(filp, dev, args->handle, &args->addr_ptr);
+}
+
 int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 			  struct drm_file *filp)
 {
@@ -266,7 +269,7 @@ int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 	if (gobj == NULL) {
 		return -ENOENT;
 	}
-	robj = gobj->driver_private;
+	robj = gem_to_radeon_bo(gobj);
 	r = radeon_bo_wait(robj, &cur_placement, true);
 	switch (cur_placement) {
 	case TTM_PL_VRAM:
@@ -296,7 +299,7 @@ int radeon_gem_wait_idle_ioctl(struct drm_device *dev, void *data,
 	if (gobj == NULL) {
 		return -ENOENT;
 	}
-	robj = gobj->driver_private;
+	robj = gem_to_radeon_bo(gobj);
 	r = radeon_bo_wait(robj, NULL, false);
 	/* callback hw specific functions if any */
 	if (robj->rdev->asic->ioctl_wait_idle)
@@ -317,7 +320,7 @@ int radeon_gem_set_tiling_ioctl(struct drm_device *dev, void *data,
 	gobj = drm_gem_object_lookup(dev, filp, args->handle);
 	if (gobj == NULL)
 		return -ENOENT;
-	robj = gobj->driver_private;
+	robj = gem_to_radeon_bo(gobj);
 	r = radeon_bo_set_tiling_flags(robj, args->tiling_flags, args->pitch);
 	drm_gem_object_unreference_unlocked(gobj);
 	return r;
@@ -335,7 +338,7 @@ int radeon_gem_get_tiling_ioctl(struct drm_device *dev, void *data,
 	gobj = drm_gem_object_lookup(dev, filp, args->handle);
 	if (gobj == NULL)
 		return -ENOENT;
-	rbo = gobj->driver_private;
+	rbo = gem_to_radeon_bo(gobj);
 	r = radeon_bo_reserve(rbo, false);
 	if (unlikely(r != 0))
 		goto out;
@@ -345,3 +348,38 @@ out:
 	drm_gem_object_unreference_unlocked(gobj);
 	return r;
 }
+
+int radeon_mode_dumb_create(struct drm_file *file_priv,
+			    struct drm_device *dev,
+			    struct drm_mode_create_dumb *args)
+{
+	struct radeon_device *rdev = dev->dev_private;
+	struct drm_gem_object *gobj;
+	int r;
+
+	args->pitch = radeon_align_pitch(rdev, args->width, args->bpp, 0) * ((args->bpp + 1) / 8);
+	args->size = args->pitch * args->height;
+	args->size = ALIGN(args->size, PAGE_SIZE);
+
+	r = radeon_gem_object_create(rdev, args->size, 0,
+				     RADEON_GEM_DOMAIN_VRAM,
+				     false, ttm_bo_type_device,
+				     &gobj);
+	if (r)
+		return -ENOMEM;
+
+	r = drm_gem_handle_create(file_priv, gobj, &args->handle);
+	if (r) {
+		drm_gem_object_unreference_unlocked(gobj);
+		return r;
+	}
+	drm_gem_object_handle_unreference_unlocked(gobj);
+	return 0;
+}
+
+int radeon_mode_dumb_destroy(struct drm_file *file_priv,
+			     struct drm_device *dev,
+			     uint32_t handle)
+{
+	return drm_gem_handle_delete(file_priv, handle);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 8387d32caaa7..bf7d4c061451 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -58,9 +58,9 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 	dev->dev_private = (void *)rdev;
 
 	/* update BUS flag */
-	if (drm_device_is_agp(dev)) {
+	if (drm_pci_device_is_agp(dev)) {
 		flags |= RADEON_IS_AGP;
-	} else if (drm_device_is_pcie(dev)) {
+	} else if (drm_pci_device_is_pcie(dev)) {
 		flags |= RADEON_IS_PCIE;
 	} else {
 		flags |= RADEON_IS_PCI;
@@ -169,7 +169,9 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		value = rdev->accel_working;
 		break;
 	case RADEON_INFO_TILING_CONFIG:
-		if (rdev->family >= CHIP_CEDAR)
+		if (rdev->family >= CHIP_CAYMAN)
+			value = rdev->config.cayman.tile_config;
+		else if (rdev->family >= CHIP_CEDAR)
 			value = rdev->config.evergreen.tile_config;
 		else if (rdev->family >= CHIP_RV770)
 			value = rdev->config.rv770.tile_config;
@@ -205,6 +207,20 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		/* return clock value in KHz */
 		value = rdev->clock.spll.reference_freq * 10;
 		break;
+	case RADEON_INFO_NUM_BACKENDS:
+		if (rdev->family >= CHIP_CAYMAN)
+			value = rdev->config.cayman.max_backends_per_se *
+				rdev->config.cayman.max_shader_engines;
+		else if (rdev->family >= CHIP_CEDAR)
+			value = rdev->config.evergreen.max_backends;
+		else if (rdev->family >= CHIP_RV770)
+			value = rdev->config.rv770.max_backends;
+		else if (rdev->family >= CHIP_R600)
+			value = rdev->config.r600.max_backends;
+		else {
+			return -EINVAL;
+		}
+		break;
 	default:
 		DRM_DEBUG_KMS("Invalid request %d\n", info->request);
 		return -EINVAL;
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index cf0638c3b7c7..9ae599eb2e6d 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -415,7 +415,7 @@ int radeon_crtc_do_set_base(struct drm_crtc *crtc,
 
 	/* Pin framebuffer & get tilling informations */
 	obj = radeon_fb->obj;
-	rbo = obj->driver_private;
+	rbo = gem_to_radeon_bo(obj);
 	r = radeon_bo_reserve(rbo, false);
 	if (unlikely(r != 0))
 		return r;
@@ -520,7 +520,7 @@ int radeon_crtc_do_set_base(struct drm_crtc *crtc,
 
 	if (!atomic && fb && fb != crtc->fb) {
 		radeon_fb = to_radeon_framebuffer(fb);
-		rbo = radeon_fb->obj->driver_private;
+		rbo = gem_to_radeon_bo(radeon_fb->obj);
 		r = radeon_bo_reserve(rbo, false);
 		if (unlikely(r != 0))
 			return r;
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index a670caaee29e..5067d18d0009 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -676,4 +676,5 @@ void radeon_fb_output_poll_changed(struct radeon_device *rdev);
 
 void radeon_crtc_handle_flip(struct radeon_device *rdev, int crtc_id);
 
+int radeon_align_pitch(struct radeon_device *rdev, int width, int bpp, bool tiled);
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 7d6b8e88f746..976c3b1b1b6e 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -55,6 +55,7 @@ static void radeon_ttm_bo_destroy(struct ttm_buffer_object *tbo)
 	list_del_init(&bo->list);
 	mutex_unlock(&bo->rdev->gem.mutex);
 	radeon_bo_clear_surface_reg(bo);
+	drm_gem_object_release(&bo->gem_base);
 	kfree(bo);
 }
 
@@ -86,7 +87,7 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 	rbo->placement.num_busy_placement = c;
 }
 
-int radeon_bo_create(struct radeon_device *rdev, struct drm_gem_object *gobj,
+int radeon_bo_create(struct radeon_device *rdev,
 		     unsigned long size, int byte_align, bool kernel, u32 domain,
 		     struct radeon_bo **bo_ptr)
 {
@@ -96,6 +97,8 @@ int radeon_bo_create(struct radeon_device *rdev, struct drm_gem_object *gobj,
 	unsigned long max_size = 0;
 	int r;
 
+	size = ALIGN(size, PAGE_SIZE);
+
 	if (unlikely(rdev->mman.bdev.dev_mapping == NULL)) {
 		rdev->mman.bdev.dev_mapping = rdev->ddev->dev_mapping;
 	}
@@ -118,8 +121,13 @@ retry:
 	bo = kzalloc(sizeof(struct radeon_bo), GFP_KERNEL);
 	if (bo == NULL)
 		return -ENOMEM;
+	r = drm_gem_object_init(rdev->ddev, &bo->gem_base, size);
+	if (unlikely(r)) {
+		kfree(bo);
+		return r;
+	}
 	bo->rdev = rdev;
-	bo->gobj = gobj;
+	bo->gem_base.driver_private = NULL;
 	bo->surface_reg = -1;
 	INIT_LIST_HEAD(&bo->list);
 	radeon_ttm_placement_from_domain(bo, domain);
@@ -142,12 +150,9 @@ retry:
 		return r;
 	}
 	*bo_ptr = bo;
-	if (gobj) {
-		mutex_lock(&bo->rdev->gem.mutex);
-		list_add_tail(&bo->list, &rdev->gem.objects);
-		mutex_unlock(&bo->rdev->gem.mutex);
-	}
+
 	trace_radeon_bo_create(bo);
+
 	return 0;
 }
 
@@ -260,7 +265,6 @@ int radeon_bo_evict_vram(struct radeon_device *rdev)
 void radeon_bo_force_delete(struct radeon_device *rdev)
 {
 	struct radeon_bo *bo, *n;
-	struct drm_gem_object *gobj;
 
 	if (list_empty(&rdev->gem.objects)) {
 		return;
@@ -268,16 +272,14 @@ void radeon_bo_force_delete(struct radeon_device *rdev)
 	dev_err(rdev->dev, "Userspace still has active objects !\n");
 	list_for_each_entry_safe(bo, n, &rdev->gem.objects, list) {
 		mutex_lock(&rdev->ddev->struct_mutex);
-		gobj = bo->gobj;
 		dev_err(rdev->dev, "%p %p %lu %lu force free\n",
-			gobj, bo, (unsigned long)gobj->size,
-			*((unsigned long *)&gobj->refcount));
+			&bo->gem_base, bo, (unsigned long)bo->gem_base.size,
+			*((unsigned long *)&bo->gem_base.refcount));
 		mutex_lock(&bo->rdev->gem.mutex);
 		list_del_init(&bo->list);
 		mutex_unlock(&bo->rdev->gem.mutex);
-		radeon_bo_unref(&bo);
-		gobj->driver_private = NULL;
-		drm_gem_object_unreference(gobj);
+		/* this should unref the ttm bo */
+		drm_gem_object_unreference(&bo->gem_base);
 		mutex_unlock(&rdev->ddev->struct_mutex);
 	}
 }
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index 22d4c237dea5..7f8e778dba46 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -137,10 +137,9 @@ static inline int radeon_bo_wait(struct radeon_bo *bo, u32 *mem_type,
 }
 
 extern int radeon_bo_create(struct radeon_device *rdev,
-			    struct drm_gem_object *gobj, unsigned long size,
-			    int byte_align,
-			    bool kernel, u32 domain,
-			    struct radeon_bo **bo_ptr);
+				unsigned long size, int byte_align,
+				bool kernel, u32 domain,
+				struct radeon_bo **bo_ptr);
 extern int radeon_bo_kmap(struct radeon_bo *bo, void **ptr);
 extern void radeon_bo_kunmap(struct radeon_bo *bo);
 extern void radeon_bo_unref(struct radeon_bo **bo);
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index 06e79822a2bf..bbc9cd823334 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -151,7 +151,7 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
 	/* 64 dwords should be enough for fence too */
 	r = radeon_ring_lock(rdev, 64);
 	if (r) {
-		DRM_ERROR("radeon: scheduling IB failled (%d).\n", r);
+		DRM_ERROR("radeon: scheduling IB failed (%d).\n", r);
 		return r;
 	}
 	radeon_ring_ib_execute(rdev, ib);
@@ -175,7 +175,7 @@ int radeon_ib_pool_init(struct radeon_device *rdev)
 		return 0;
 	INIT_LIST_HEAD(&rdev->ib_pool.bogus_ib);
 	/* Allocate 1M object buffer */
-	r = radeon_bo_create(rdev, NULL,  RADEON_IB_POOL_SIZE*64*1024,
+	r = radeon_bo_create(rdev, RADEON_IB_POOL_SIZE*64*1024,
 			     PAGE_SIZE, true, RADEON_GEM_DOMAIN_GTT,
 			     &rdev->ib_pool.robj);
 	if (r) {
@@ -332,7 +332,7 @@ int radeon_ring_init(struct radeon_device *rdev, unsigned ring_size)
 	rdev->cp.ring_size = ring_size;
 	/* Allocate ring buffer */
 	if (rdev->cp.ring_obj == NULL) {
-		r = radeon_bo_create(rdev, NULL, rdev->cp.ring_size, PAGE_SIZE, true,
+		r = radeon_bo_create(rdev, rdev->cp.ring_size, PAGE_SIZE, true,
 					RADEON_GEM_DOMAIN_GTT,
 					&rdev->cp.ring_obj);
 		if (r) {
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index 5b44f652145c..dee4a0c1b4b2 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -52,7 +52,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 		goto out_cleanup;
 	}
 
-	r = radeon_bo_create(rdev, NULL, size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
 				&vram_obj);
 	if (r) {
 		DRM_ERROR("Failed to create VRAM object\n");
@@ -71,7 +71,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 		void **gtt_start, **gtt_end;
 		void **vram_start, **vram_end;
 
-		r = radeon_bo_create(rdev, NULL, size, PAGE_SIZE, true,
+		r = radeon_bo_create(rdev, size, PAGE_SIZE, true,
 					 RADEON_GEM_DOMAIN_GTT, gtt_obj + i);
 		if (r) {
 			DRM_ERROR("Failed to create GTT object %d\n", i);
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index e5b2cf10cbf4..e446979e0e0a 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -529,7 +529,7 @@ int radeon_ttm_init(struct radeon_device *rdev)
 		DRM_ERROR("Failed initializing VRAM heap.\n");
 		return r;
 	}
-	r = radeon_bo_create(rdev, NULL, 256 * 1024, PAGE_SIZE, true,
+	r = radeon_bo_create(rdev, 256 * 1024, PAGE_SIZE, true,
 				RADEON_GEM_DOMAIN_VRAM,
 				&rdev->stollen_vga_memory);
 	if (r) {
@@ -647,6 +647,7 @@ struct radeon_ttm_backend {
 	unsigned long			num_pages;
 	struct page			**pages;
 	struct page			*dummy_read_page;
+	dma_addr_t			*dma_addrs;
 	bool				populated;
 	bool				bound;
 	unsigned			offset;
@@ -655,12 +656,14 @@ struct radeon_ttm_backend {
 static int radeon_ttm_backend_populate(struct ttm_backend *backend,
 				       unsigned long num_pages,
 				       struct page **pages,
-				       struct page *dummy_read_page)
+				       struct page *dummy_read_page,
+				       dma_addr_t *dma_addrs)
 {
 	struct radeon_ttm_backend *gtt;
 
 	gtt = container_of(backend, struct radeon_ttm_backend, backend);
 	gtt->pages = pages;
+	gtt->dma_addrs = dma_addrs;
 	gtt->num_pages = num_pages;
 	gtt->dummy_read_page = dummy_read_page;
 	gtt->populated = true;
@@ -673,6 +676,7 @@ static void radeon_ttm_backend_clear(struct ttm_backend *backend)
 
 	gtt = container_of(backend, struct radeon_ttm_backend, backend);
 	gtt->pages = NULL;
+	gtt->dma_addrs = NULL;
 	gtt->num_pages = 0;
 	gtt->dummy_read_page = NULL;
 	gtt->populated = false;
@@ -693,7 +697,7 @@ static int radeon_ttm_backend_bind(struct ttm_backend *backend,
 		     gtt->num_pages, bo_mem, backend);
 	}
 	r = radeon_gart_bind(gtt->rdev, gtt->offset,
-			     gtt->num_pages, gtt->pages);
+			     gtt->num_pages, gtt->pages, gtt->dma_addrs);
 	if (r) {
 		DRM_ERROR("failed to bind %lu pages at 0x%08X\n",
 			  gtt->num_pages, gtt->offset);
diff --git a/drivers/gpu/drm/radeon/reg_srcs/cayman b/drivers/gpu/drm/radeon/reg_srcs/cayman
new file mode 100644
index 000000000000..6334f8ac1209
--- /dev/null
+++ b/drivers/gpu/drm/radeon/reg_srcs/cayman
@@ -0,0 +1,619 @@
+cayman 0x9400
+0x0000802C GRBM_GFX_INDEX
+0x000088B0 VGT_VTX_VECT_EJECT_REG
+0x000088C4 VGT_CACHE_INVALIDATION
+0x000088D4 VGT_GS_VERTEX_REUSE
+0x00008958 VGT_PRIMITIVE_TYPE
+0x0000895C VGT_INDEX_TYPE
+0x00008970 VGT_NUM_INDICES
+0x00008974 VGT_NUM_INSTANCES
+0x00008990 VGT_COMPUTE_DIM_X
+0x00008994 VGT_COMPUTE_DIM_Y
+0x00008998 VGT_COMPUTE_DIM_Z
+0x0000899C VGT_COMPUTE_START_X
+0x000089A0 VGT_COMPUTE_START_Y
+0x000089A4 VGT_COMPUTE_START_Z
+0x000089A8 VGT_COMPUTE_INDEX
+0x000089AC VGT_COMPUTE_THREAD_GOURP_SIZE
+0x000089B0 VGT_HS_OFFCHIP_PARAM
+0x00008A14 PA_CL_ENHANCE
+0x00008A60 PA_SC_LINE_STIPPLE_VALUE
+0x00008B10 PA_SC_LINE_STIPPLE_STATE
+0x00008BF0 PA_SC_ENHANCE
+0x00008D8C SQ_DYN_GPR_CNTL_PS_FLUSH_REQ
+0x00008D94 SQ_DYN_GPR_SIMD_LOCK_EN
+0x00008C00 SQ_CONFIG
+0x00008C04 SQ_GPR_RESOURCE_MGMT_1
+0x00008C10 SQ_GLOBAL_GPR_RESOURCE_MGMT_1
+0x00008C14 SQ_GLOBAL_GPR_RESOURCE_MGMT_2
+0x00008DF8 SQ_CONST_MEM_BASE
+0x00008E20 SQ_STATIC_THREAD_MGMT_1
+0x00008E24 SQ_STATIC_THREAD_MGMT_2
+0x00008E28 SQ_STATIC_THREAD_MGMT_3
+0x00008E48 SQ_EX_ALLOC_TABLE_SLOTS
+0x00009100 SPI_CONFIG_CNTL
+0x0000913C SPI_CONFIG_CNTL_1
+0x00009830 DB_DEBUG
+0x00009834 DB_DEBUG2
+0x00009838 DB_DEBUG3
+0x0000983C DB_DEBUG4
+0x00009854 DB_WATERMARKS
+0x0000A400 TD_PS_BORDER_COLOR_INDEX
+0x0000A404 TD_PS_BORDER_COLOR_RED
+0x0000A408 TD_PS_BORDER_COLOR_GREEN
+0x0000A40C TD_PS_BORDER_COLOR_BLUE
+0x0000A410 TD_PS_BORDER_COLOR_ALPHA
+0x0000A414 TD_VS_BORDER_COLOR_INDEX
+0x0000A418 TD_VS_BORDER_COLOR_RED
+0x0000A41C TD_VS_BORDER_COLOR_GREEN
+0x0000A420 TD_VS_BORDER_COLOR_BLUE
+0x0000A424 TD_VS_BORDER_COLOR_ALPHA
+0x0000A428 TD_GS_BORDER_COLOR_INDEX
+0x0000A42C TD_GS_BORDER_COLOR_RED
+0x0000A430 TD_GS_BORDER_COLOR_GREEN
+0x0000A434 TD_GS_BORDER_COLOR_BLUE
+0x0000A438 TD_GS_BORDER_COLOR_ALPHA
+0x0000A43C TD_HS_BORDER_COLOR_INDEX
+0x0000A440 TD_HS_BORDER_COLOR_RED
+0x0000A444 TD_HS_BORDER_COLOR_GREEN
+0x0000A448 TD_HS_BORDER_COLOR_BLUE
+0x0000A44C TD_HS_BORDER_COLOR_ALPHA
+0x0000A450 TD_LS_BORDER_COLOR_INDEX
+0x0000A454 TD_LS_BORDER_COLOR_RED
+0x0000A458 TD_LS_BORDER_COLOR_GREEN
+0x0000A45C TD_LS_BORDER_COLOR_BLUE
+0x0000A460 TD_LS_BORDER_COLOR_ALPHA
+0x0000A464 TD_CS_BORDER_COLOR_INDEX
+0x0000A468 TD_CS_BORDER_COLOR_RED
+0x0000A46C TD_CS_BORDER_COLOR_GREEN
+0x0000A470 TD_CS_BORDER_COLOR_BLUE
+0x0000A474 TD_CS_BORDER_COLOR_ALPHA
+0x00028000 DB_RENDER_CONTROL
+0x00028004 DB_COUNT_CONTROL
+0x0002800C DB_RENDER_OVERRIDE
+0x00028010 DB_RENDER_OVERRIDE2
+0x00028028 DB_STENCIL_CLEAR
+0x0002802C DB_DEPTH_CLEAR
+0x00028030 PA_SC_SCREEN_SCISSOR_TL
+0x00028034 PA_SC_SCREEN_SCISSOR_BR
+0x0002805C DB_DEPTH_SLICE
+0x00028140 SQ_ALU_CONST_BUFFER_SIZE_PS_0
+0x00028144 SQ_ALU_CONST_BUFFER_SIZE_PS_1
+0x00028148 SQ_ALU_CONST_BUFFER_SIZE_PS_2
+0x0002814C SQ_ALU_CONST_BUFFER_SIZE_PS_3
+0x00028150 SQ_ALU_CONST_BUFFER_SIZE_PS_4
+0x00028154 SQ_ALU_CONST_BUFFER_SIZE_PS_5
+0x00028158 SQ_ALU_CONST_BUFFER_SIZE_PS_6
+0x0002815C SQ_ALU_CONST_BUFFER_SIZE_PS_7
+0x00028160 SQ_ALU_CONST_BUFFER_SIZE_PS_8
+0x00028164 SQ_ALU_CONST_BUFFER_SIZE_PS_9
+0x00028168 SQ_ALU_CONST_BUFFER_SIZE_PS_10
+0x0002816C SQ_ALU_CONST_BUFFER_SIZE_PS_11
+0x00028170 SQ_ALU_CONST_BUFFER_SIZE_PS_12
+0x00028174 SQ_ALU_CONST_BUFFER_SIZE_PS_13
+0x00028178 SQ_ALU_CONST_BUFFER_SIZE_PS_14
+0x0002817C SQ_ALU_CONST_BUFFER_SIZE_PS_15
+0x00028180 SQ_ALU_CONST_BUFFER_SIZE_VS_0
+0x00028184 SQ_ALU_CONST_BUFFER_SIZE_VS_1
+0x00028188 SQ_ALU_CONST_BUFFER_SIZE_VS_2
+0x0002818C SQ_ALU_CONST_BUFFER_SIZE_VS_3
+0x00028190 SQ_ALU_CONST_BUFFER_SIZE_VS_4
+0x00028194 SQ_ALU_CONST_BUFFER_SIZE_VS_5
+0x00028198 SQ_ALU_CONST_BUFFER_SIZE_VS_6
+0x0002819C SQ_ALU_CONST_BUFFER_SIZE_VS_7
+0x000281A0 SQ_ALU_CONST_BUFFER_SIZE_VS_8
+0x000281A4 SQ_ALU_CONST_BUFFER_SIZE_VS_9
+0x000281A8 SQ_ALU_CONST_BUFFER_SIZE_VS_10
+0x000281AC SQ_ALU_CONST_BUFFER_SIZE_VS_11
+0x000281B0 SQ_ALU_CONST_BUFFER_SIZE_VS_12
+0x000281B4 SQ_ALU_CONST_BUFFER_SIZE_VS_13
+0x000281B8 SQ_ALU_CONST_BUFFER_SIZE_VS_14
+0x000281BC SQ_ALU_CONST_BUFFER_SIZE_VS_15
+0x000281C0 SQ_ALU_CONST_BUFFER_SIZE_GS_0
+0x000281C4 SQ_ALU_CONST_BUFFER_SIZE_GS_1
+0x000281C8 SQ_ALU_CONST_BUFFER_SIZE_GS_2
+0x000281CC SQ_ALU_CONST_BUFFER_SIZE_GS_3
+0x000281D0 SQ_ALU_CONST_BUFFER_SIZE_GS_4
+0x000281D4 SQ_ALU_CONST_BUFFER_SIZE_GS_5
+0x000281D8 SQ_ALU_CONST_BUFFER_SIZE_GS_6
+0x000281DC SQ_ALU_CONST_BUFFER_SIZE_GS_7
+0x000281E0 SQ_ALU_CONST_BUFFER_SIZE_GS_8
+0x000281E4 SQ_ALU_CONST_BUFFER_SIZE_GS_9
+0x000281E8 SQ_ALU_CONST_BUFFER_SIZE_GS_10
+0x000281EC SQ_ALU_CONST_BUFFER_SIZE_GS_11
+0x000281F0 SQ_ALU_CONST_BUFFER_SIZE_GS_12
+0x000281F4 SQ_ALU_CONST_BUFFER_SIZE_GS_13
+0x000281F8 SQ_ALU_CONST_BUFFER_SIZE_GS_14
+0x000281FC SQ_ALU_CONST_BUFFER_SIZE_GS_15
+0x00028200 PA_SC_WINDOW_OFFSET
+0x00028204 PA_SC_WINDOW_SCISSOR_TL
+0x00028208 PA_SC_WINDOW_SCISSOR_BR
+0x0002820C PA_SC_CLIPRECT_RULE
+0x00028210 PA_SC_CLIPRECT_0_TL
+0x00028214 PA_SC_CLIPRECT_0_BR
+0x00028218 PA_SC_CLIPRECT_1_TL
+0x0002821C PA_SC_CLIPRECT_1_BR
+0x00028220 PA_SC_CLIPRECT_2_TL
+0x00028224 PA_SC_CLIPRECT_2_BR
+0x00028228 PA_SC_CLIPRECT_3_TL
+0x0002822C PA_SC_CLIPRECT_3_BR
+0x00028230 PA_SC_EDGERULE
+0x00028234 PA_SU_HARDWARE_SCREEN_OFFSET
+0x00028240 PA_SC_GENERIC_SCISSOR_TL
+0x00028244 PA_SC_GENERIC_SCISSOR_BR
+0x00028250 PA_SC_VPORT_SCISSOR_0_TL
+0x00028254 PA_SC_VPORT_SCISSOR_0_BR
+0x00028258 PA_SC_VPORT_SCISSOR_1_TL
+0x0002825C PA_SC_VPORT_SCISSOR_1_BR
+0x00028260 PA_SC_VPORT_SCISSOR_2_TL
+0x00028264 PA_SC_VPORT_SCISSOR_2_BR
+0x00028268 PA_SC_VPORT_SCISSOR_3_TL
+0x0002826C PA_SC_VPORT_SCISSOR_3_BR
+0x00028270 PA_SC_VPORT_SCISSOR_4_TL
+0x00028274 PA_SC_VPORT_SCISSOR_4_BR
+0x00028278 PA_SC_VPORT_SCISSOR_5_TL
+0x0002827C PA_SC_VPORT_SCISSOR_5_BR
+0x00028280 PA_SC_VPORT_SCISSOR_6_TL
+0x00028284 PA_SC_VPORT_SCISSOR_6_BR
+0x00028288 PA_SC_VPORT_SCISSOR_7_TL
+0x0002828C PA_SC_VPORT_SCISSOR_7_BR
+0x00028290 PA_SC_VPORT_SCISSOR_8_TL
+0x00028294 PA_SC_VPORT_SCISSOR_8_BR
+0x00028298 PA_SC_VPORT_SCISSOR_9_TL
+0x0002829C PA_SC_VPORT_SCISSOR_9_BR
+0x000282A0 PA_SC_VPORT_SCISSOR_10_TL
+0x000282A4 PA_SC_VPORT_SCISSOR_10_BR
+0x000282A8 PA_SC_VPORT_SCISSOR_11_TL
+0x000282AC PA_SC_VPORT_SCISSOR_11_BR
+0x000282B0 PA_SC_VPORT_SCISSOR_12_TL
+0x000282B4 PA_SC_VPORT_SCISSOR_12_BR
+0x000282B8 PA_SC_VPORT_SCISSOR_13_TL
+0x000282BC PA_SC_VPORT_SCISSOR_13_BR
+0x000282C0 PA_SC_VPORT_SCISSOR_14_TL
+0x000282C4 PA_SC_VPORT_SCISSOR_14_BR
+0x000282C8 PA_SC_VPORT_SCISSOR_15_TL
+0x000282CC PA_SC_VPORT_SCISSOR_15_BR
+0x000282D0 PA_SC_VPORT_ZMIN_0
+0x000282D4 PA_SC_VPORT_ZMAX_0
+0x000282D8 PA_SC_VPORT_ZMIN_1
+0x000282DC PA_SC_VPORT_ZMAX_1
+0x000282E0 PA_SC_VPORT_ZMIN_2
+0x000282E4 PA_SC_VPORT_ZMAX_2
+0x000282E8 PA_SC_VPORT_ZMIN_3
+0x000282EC PA_SC_VPORT_ZMAX_3
+0x000282F0 PA_SC_VPORT_ZMIN_4
+0x000282F4 PA_SC_VPORT_ZMAX_4
+0x000282F8 PA_SC_VPORT_ZMIN_5
+0x000282FC PA_SC_VPORT_ZMAX_5
+0x00028300 PA_SC_VPORT_ZMIN_6
+0x00028304 PA_SC_VPORT_ZMAX_6
+0x00028308 PA_SC_VPORT_ZMIN_7
+0x0002830C PA_SC_VPORT_ZMAX_7
+0x00028310 PA_SC_VPORT_ZMIN_8
+0x00028314 PA_SC_VPORT_ZMAX_8
+0x00028318 PA_SC_VPORT_ZMIN_9
+0x0002831C PA_SC_VPORT_ZMAX_9
+0x00028320 PA_SC_VPORT_ZMIN_10
+0x00028324 PA_SC_VPORT_ZMAX_10
+0x00028328 PA_SC_VPORT_ZMIN_11
+0x0002832C PA_SC_VPORT_ZMAX_11
+0x00028330 PA_SC_VPORT_ZMIN_12
+0x00028334 PA_SC_VPORT_ZMAX_12
+0x00028338 PA_SC_VPORT_ZMIN_13
+0x0002833C PA_SC_VPORT_ZMAX_13
+0x00028340 PA_SC_VPORT_ZMIN_14
+0x00028344 PA_SC_VPORT_ZMAX_14
+0x00028348 PA_SC_VPORT_ZMIN_15
+0x0002834C PA_SC_VPORT_ZMAX_15
+0x00028350 SX_MISC
+0x00028354 SX_SURFACE_SYNC
+0x00028380 SQ_VTX_SEMANTIC_0
+0x00028384 SQ_VTX_SEMANTIC_1
+0x00028388 SQ_VTX_SEMANTIC_2
+0x0002838C SQ_VTX_SEMANTIC_3
+0x00028390 SQ_VTX_SEMANTIC_4
+0x00028394 SQ_VTX_SEMANTIC_5
+0x00028398 SQ_VTX_SEMANTIC_6
+0x0002839C SQ_VTX_SEMANTIC_7
+0x000283A0 SQ_VTX_SEMANTIC_8
+0x000283A4 SQ_VTX_SEMANTIC_9
+0x000283A8 SQ_VTX_SEMANTIC_10
+0x000283AC SQ_VTX_SEMANTIC_11
+0x000283B0 SQ_VTX_SEMANTIC_12
+0x000283B4 SQ_VTX_SEMANTIC_13
+0x000283B8 SQ_VTX_SEMANTIC_14
+0x000283BC SQ_VTX_SEMANTIC_15
+0x000283C0 SQ_VTX_SEMANTIC_16
+0x000283C4 SQ_VTX_SEMANTIC_17
+0x000283C8 SQ_VTX_SEMANTIC_18
+0x000283CC SQ_VTX_SEMANTIC_19
+0x000283D0 SQ_VTX_SEMANTIC_20
+0x000283D4 SQ_VTX_SEMANTIC_21
+0x000283D8 SQ_VTX_SEMANTIC_22
+0x000283DC SQ_VTX_SEMANTIC_23
+0x000283E0 SQ_VTX_SEMANTIC_24
+0x000283E4 SQ_VTX_SEMANTIC_25
+0x000283E8 SQ_VTX_SEMANTIC_26
+0x000283EC SQ_VTX_SEMANTIC_27
+0x000283F0 SQ_VTX_SEMANTIC_28
+0x000283F4 SQ_VTX_SEMANTIC_29
+0x000283F8 SQ_VTX_SEMANTIC_30
+0x000283FC SQ_VTX_SEMANTIC_31
+0x00028400 VGT_MAX_VTX_INDX
+0x00028404 VGT_MIN_VTX_INDX
+0x00028408 VGT_INDX_OFFSET
+0x0002840C VGT_MULTI_PRIM_IB_RESET_INDX
+0x00028410 SX_ALPHA_TEST_CONTROL
+0x00028414 CB_BLEND_RED
+0x00028418 CB_BLEND_GREEN
+0x0002841C CB_BLEND_BLUE
+0x00028420 CB_BLEND_ALPHA
+0x00028430 DB_STENCILREFMASK
+0x00028434 DB_STENCILREFMASK_BF
+0x00028438 SX_ALPHA_REF
+0x0002843C PA_CL_VPORT_XSCALE_0
+0x00028440 PA_CL_VPORT_XOFFSET_0
+0x00028444 PA_CL_VPORT_YSCALE_0
+0x00028448 PA_CL_VPORT_YOFFSET_0
+0x0002844C PA_CL_VPORT_ZSCALE_0
+0x00028450 PA_CL_VPORT_ZOFFSET_0
+0x00028454 PA_CL_VPORT_XSCALE_1
+0x00028458 PA_CL_VPORT_XOFFSET_1
+0x0002845C PA_CL_VPORT_YSCALE_1
+0x00028460 PA_CL_VPORT_YOFFSET_1
+0x00028464 PA_CL_VPORT_ZSCALE_1
+0x00028468 PA_CL_VPORT_ZOFFSET_1
+0x0002846C PA_CL_VPORT_XSCALE_2
+0x00028470 PA_CL_VPORT_XOFFSET_2
+0x00028474 PA_CL_VPORT_YSCALE_2
+0x00028478 PA_CL_VPORT_YOFFSET_2
+0x0002847C PA_CL_VPORT_ZSCALE_2
+0x00028480 PA_CL_VPORT_ZOFFSET_2
+0x00028484 PA_CL_VPORT_XSCALE_3
+0x00028488 PA_CL_VPORT_XOFFSET_3
+0x0002848C PA_CL_VPORT_YSCALE_3
+0x00028490 PA_CL_VPORT_YOFFSET_3
+0x00028494 PA_CL_VPORT_ZSCALE_3
+0x00028498 PA_CL_VPORT_ZOFFSET_3
+0x0002849C PA_CL_VPORT_XSCALE_4
+0x000284A0 PA_CL_VPORT_XOFFSET_4
+0x000284A4 PA_CL_VPORT_YSCALE_4
+0x000284A8 PA_CL_VPORT_YOFFSET_4
+0x000284AC PA_CL_VPORT_ZSCALE_4
+0x000284B0 PA_CL_VPORT_ZOFFSET_4
+0x000284B4 PA_CL_VPORT_XSCALE_5
+0x000284B8 PA_CL_VPORT_XOFFSET_5
+0x000284BC PA_CL_VPORT_YSCALE_5
+0x000284C0 PA_CL_VPORT_YOFFSET_5
+0x000284C4 PA_CL_VPORT_ZSCALE_5
+0x000284C8 PA_CL_VPORT_ZOFFSET_5
+0x000284CC PA_CL_VPORT_XSCALE_6
+0x000284D0 PA_CL_VPORT_XOFFSET_6
+0x000284D4 PA_CL_VPORT_YSCALE_6
+0x000284D8 PA_CL_VPORT_YOFFSET_6
+0x000284DC PA_CL_VPORT_ZSCALE_6
+0x000284E0 PA_CL_VPORT_ZOFFSET_6
+0x000284E4 PA_CL_VPORT_XSCALE_7
+0x000284E8 PA_CL_VPORT_XOFFSET_7
+0x000284EC PA_CL_VPORT_YSCALE_7
+0x000284F0 PA_CL_VPORT_YOFFSET_7
+0x000284F4 PA_CL_VPORT_ZSCALE_7
+0x000284F8 PA_CL_VPORT_ZOFFSET_7
+0x000284FC PA_CL_VPORT_XSCALE_8
+0x00028500 PA_CL_VPORT_XOFFSET_8
+0x00028504 PA_CL_VPORT_YSCALE_8
+0x00028508 PA_CL_VPORT_YOFFSET_8
+0x0002850C PA_CL_VPORT_ZSCALE_8
+0x00028510 PA_CL_VPORT_ZOFFSET_8
+0x00028514 PA_CL_VPORT_XSCALE_9
+0x00028518 PA_CL_VPORT_XOFFSET_9
+0x0002851C PA_CL_VPORT_YSCALE_9
+0x00028520 PA_CL_VPORT_YOFFSET_9
+0x00028524 PA_CL_VPORT_ZSCALE_9
+0x00028528 PA_CL_VPORT_ZOFFSET_9
+0x0002852C PA_CL_VPORT_XSCALE_10
+0x00028530 PA_CL_VPORT_XOFFSET_10
+0x00028534 PA_CL_VPORT_YSCALE_10
+0x00028538 PA_CL_VPORT_YOFFSET_10
+0x0002853C PA_CL_VPORT_ZSCALE_10
+0x00028540 PA_CL_VPORT_ZOFFSET_10
+0x00028544 PA_CL_VPORT_XSCALE_11
+0x00028548 PA_CL_VPORT_XOFFSET_11
+0x0002854C PA_CL_VPORT_YSCALE_11
+0x00028550 PA_CL_VPORT_YOFFSET_11
+0x00028554 PA_CL_VPORT_ZSCALE_11
+0x00028558 PA_CL_VPORT_ZOFFSET_11
+0x0002855C PA_CL_VPORT_XSCALE_12
+0x00028560 PA_CL_VPORT_XOFFSET_12
+0x00028564 PA_CL_VPORT_YSCALE_12
+0x00028568 PA_CL_VPORT_YOFFSET_12
+0x0002856C PA_CL_VPORT_ZSCALE_12
+0x00028570 PA_CL_VPORT_ZOFFSET_12
+0x00028574 PA_CL_VPORT_XSCALE_13
+0x00028578 PA_CL_VPORT_XOFFSET_13
+0x0002857C PA_CL_VPORT_YSCALE_13
+0x00028580 PA_CL_VPORT_YOFFSET_13
+0x00028584 PA_CL_VPORT_ZSCALE_13
+0x00028588 PA_CL_VPORT_ZOFFSET_13
+0x0002858C PA_CL_VPORT_XSCALE_14
+0x00028590 PA_CL_VPORT_XOFFSET_14
+0x00028594 PA_CL_VPORT_YSCALE_14
+0x00028598 PA_CL_VPORT_YOFFSET_14
+0x0002859C PA_CL_VPORT_ZSCALE_14
+0x000285A0 PA_CL_VPORT_ZOFFSET_14
+0x000285A4 PA_CL_VPORT_XSCALE_15
+0x000285A8 PA_CL_VPORT_XOFFSET_15
+0x000285AC PA_CL_VPORT_YSCALE_15
+0x000285B0 PA_CL_VPORT_YOFFSET_15
+0x000285B4 PA_CL_VPORT_ZSCALE_15
+0x000285B8 PA_CL_VPORT_ZOFFSET_15
+0x000285BC PA_CL_UCP_0_X
+0x000285C0 PA_CL_UCP_0_Y
+0x000285C4 PA_CL_UCP_0_Z
+0x000285C8 PA_CL_UCP_0_W
+0x000285CC PA_CL_UCP_1_X
+0x000285D0 PA_CL_UCP_1_Y
+0x000285D4 PA_CL_UCP_1_Z
+0x000285D8 PA_CL_UCP_1_W
+0x000285DC PA_CL_UCP_2_X
+0x000285E0 PA_CL_UCP_2_Y
+0x000285E4 PA_CL_UCP_2_Z
+0x000285E8 PA_CL_UCP_2_W
+0x000285EC PA_CL_UCP_3_X
+0x000285F0 PA_CL_UCP_3_Y
+0x000285F4 PA_CL_UCP_3_Z
+0x000285F8 PA_CL_UCP_3_W
+0x000285FC PA_CL_UCP_4_X
+0x00028600 PA_CL_UCP_4_Y
+0x00028604 PA_CL_UCP_4_Z
+0x00028608 PA_CL_UCP_4_W
+0x0002860C PA_CL_UCP_5_X
+0x00028610 PA_CL_UCP_5_Y
+0x00028614 PA_CL_UCP_5_Z
+0x00028618 PA_CL_UCP_5_W
+0x0002861C SPI_VS_OUT_ID_0
+0x00028620 SPI_VS_OUT_ID_1
+0x00028624 SPI_VS_OUT_ID_2
+0x00028628 SPI_VS_OUT_ID_3
+0x0002862C SPI_VS_OUT_ID_4
+0x00028630 SPI_VS_OUT_ID_5
+0x00028634 SPI_VS_OUT_ID_6
+0x00028638 SPI_VS_OUT_ID_7
+0x0002863C SPI_VS_OUT_ID_8
+0x00028640 SPI_VS_OUT_ID_9
+0x00028644 SPI_PS_INPUT_CNTL_0
+0x00028648 SPI_PS_INPUT_CNTL_1
+0x0002864C SPI_PS_INPUT_CNTL_2
+0x00028650 SPI_PS_INPUT_CNTL_3
+0x00028654 SPI_PS_INPUT_CNTL_4
+0x00028658 SPI_PS_INPUT_CNTL_5
+0x0002865C SPI_PS_INPUT_CNTL_6
+0x00028660 SPI_PS_INPUT_CNTL_7
+0x00028664 SPI_PS_INPUT_CNTL_8
+0x00028668 SPI_PS_INPUT_CNTL_9
+0x0002866C SPI_PS_INPUT_CNTL_10
+0x00028670 SPI_PS_INPUT_CNTL_11
+0x00028674 SPI_PS_INPUT_CNTL_12
+0x00028678 SPI_PS_INPUT_CNTL_13
+0x0002867C SPI_PS_INPUT_CNTL_14
+0x00028680 SPI_PS_INPUT_CNTL_15
+0x00028684 SPI_PS_INPUT_CNTL_16
+0x00028688 SPI_PS_INPUT_CNTL_17
+0x0002868C SPI_PS_INPUT_CNTL_18
+0x00028690 SPI_PS_INPUT_CNTL_19
+0x00028694 SPI_PS_INPUT_CNTL_20
+0x00028698 SPI_PS_INPUT_CNTL_21
+0x0002869C SPI_PS_INPUT_CNTL_22
+0x000286A0 SPI_PS_INPUT_CNTL_23
+0x000286A4 SPI_PS_INPUT_CNTL_24
+0x000286A8 SPI_PS_INPUT_CNTL_25
+0x000286AC SPI_PS_INPUT_CNTL_26
+0x000286B0 SPI_PS_INPUT_CNTL_27
+0x000286B4 SPI_PS_INPUT_CNTL_28
+0x000286B8 SPI_PS_INPUT_CNTL_29
+0x000286BC SPI_PS_INPUT_CNTL_30
+0x000286C0 SPI_PS_INPUT_CNTL_31
+0x000286C4 SPI_VS_OUT_CONFIG
+0x000286C8 SPI_THREAD_GROUPING
+0x000286CC SPI_PS_IN_CONTROL_0
+0x000286D0 SPI_PS_IN_CONTROL_1
+0x000286D4 SPI_INTERP_CONTROL_0
+0x000286D8 SPI_INPUT_Z
+0x000286DC SPI_FOG_CNTL
+0x000286E0 SPI_BARYC_CNTL
+0x000286E4 SPI_PS_IN_CONTROL_2
+0x000286E8 SPI_COMPUTE_INPUT_CNTL
+0x000286EC SPI_COMPUTE_NUM_THREAD_X
+0x000286F0 SPI_COMPUTE_NUM_THREAD_Y
+0x000286F4 SPI_COMPUTE_NUM_THREAD_Z
+0x000286F8 SPI_GPR_MGMT
+0x000286FC SPI_LDS_MGMT
+0x00028700 SPI_STACK_MGMT
+0x00028704 SPI_WAVE_MGMT_1
+0x00028708 SPI_WAVE_MGMT_2
+0x00028724 GDS_ADDR_SIZE
+0x00028780 CB_BLEND0_CONTROL
+0x00028784 CB_BLEND1_CONTROL
+0x00028788 CB_BLEND2_CONTROL
+0x0002878C CB_BLEND3_CONTROL
+0x00028790 CB_BLEND4_CONTROL
+0x00028794 CB_BLEND5_CONTROL
+0x00028798 CB_BLEND6_CONTROL
+0x0002879C CB_BLEND7_CONTROL
+0x000287CC CS_COPY_STATE
+0x000287D0 GFX_COPY_STATE
+0x000287D4 PA_CL_POINT_X_RAD
+0x000287D8 PA_CL_POINT_Y_RAD
+0x000287DC PA_CL_POINT_SIZE
+0x000287E0 PA_CL_POINT_CULL_RAD
+0x00028808 CB_COLOR_CONTROL
+0x0002880C DB_SHADER_CONTROL
+0x00028810 PA_CL_CLIP_CNTL
+0x00028814 PA_SU_SC_MODE_CNTL
+0x00028818 PA_CL_VTE_CNTL
+0x0002881C PA_CL_VS_OUT_CNTL
+0x00028820 PA_CL_NANINF_CNTL
+0x00028824 PA_SU_LINE_STIPPLE_CNTL
+0x00028828 PA_SU_LINE_STIPPLE_SCALE
+0x0002882C PA_SU_PRIM_FILTER_CNTL
+0x00028844 SQ_PGM_RESOURCES_PS
+0x00028848 SQ_PGM_RESOURCES_2_PS
+0x0002884C SQ_PGM_EXPORTS_PS
+0x00028860 SQ_PGM_RESOURCES_VS
+0x00028864 SQ_PGM_RESOURCES_2_VS
+0x00028878 SQ_PGM_RESOURCES_GS
+0x0002887C SQ_PGM_RESOURCES_2_GS
+0x00028890 SQ_PGM_RESOURCES_ES
+0x00028894 SQ_PGM_RESOURCES_2_ES
+0x000288A8 SQ_PGM_RESOURCES_FS
+0x000288BC SQ_PGM_RESOURCES_HS
+0x000288C0 SQ_PGM_RESOURCES_2_HS
+0x000288D4 SQ_PGM_RESOURCES_LS
+0x000288D8 SQ_PGM_RESOURCES_2_LS
+0x000288E8 SQ_LDS_ALLOC
+0x000288EC SQ_LDS_ALLOC_PS
+0x000288F0 SQ_VTX_SEMANTIC_CLEAR
+0x00028A00 PA_SU_POINT_SIZE
+0x00028A04 PA_SU_POINT_MINMAX
+0x00028A08 PA_SU_LINE_CNTL
+0x00028A0C PA_SC_LINE_STIPPLE
+0x00028A10 VGT_OUTPUT_PATH_CNTL
+0x00028A14 VGT_HOS_CNTL
+0x00028A18 VGT_HOS_MAX_TESS_LEVEL
+0x00028A1C VGT_HOS_MIN_TESS_LEVEL
+0x00028A20 VGT_HOS_REUSE_DEPTH
+0x00028A24 VGT_GROUP_PRIM_TYPE
+0x00028A28 VGT_GROUP_FIRST_DECR
+0x00028A2C VGT_GROUP_DECR
+0x00028A30 VGT_GROUP_VECT_0_CNTL
+0x00028A34 VGT_GROUP_VECT_1_CNTL
+0x00028A38 VGT_GROUP_VECT_0_FMT_CNTL
+0x00028A3C VGT_GROUP_VECT_1_FMT_CNTL
+0x00028A40 VGT_GS_MODE
+0x00028A48 PA_SC_MODE_CNTL_0
+0x00028A4C PA_SC_MODE_CNTL_1
+0x00028A50 VGT_ENHANCE
+0x00028A54 VGT_GS_PER_ES
+0x00028A58 VGT_ES_PER_GS
+0x00028A5C VGT_GS_PER_VS
+0x00028A6C VGT_GS_OUT_PRIM_TYPE
+0x00028A70 IA_ENHANCE
+0x00028A84 VGT_PRIMITIVEID_EN
+0x00028A94 VGT_MULTI_PRIM_IB_RESET_EN
+0x00028AA0 VGT_INSTANCE_STEP_RATE_0
+0x00028AA4 VGT_INSTANCE_STEP_RATE_1
+0x00028AA8 IA_MULTI_VGT_PARAM
+0x00028AB4 VGT_REUSE_OFF
+0x00028AB8 VGT_VTX_CNT_EN
+0x00028ABC DB_HTILE_SURFACE
+0x00028AC0 DB_SRESULTS_COMPARE_STATE0
+0x00028AC4 DB_SRESULTS_COMPARE_STATE1
+0x00028AC8 DB_PRELOAD_CONTROL
+0x00028B38 VGT_GS_MAX_VERT_OUT
+0x00028B54 VGT_SHADER_STAGES_EN
+0x00028B58 VGT_LS_HS_CONFIG
+0x00028B6C VGT_TF_PARAM
+0x00028B70 DB_ALPHA_TO_MASK
+0x00028B74 VGT_DISPATCH_INITIATOR
+0x00028B78 PA_SU_POLY_OFFSET_DB_FMT_CNTL
+0x00028B7C PA_SU_POLY_OFFSET_CLAMP
+0x00028B80 PA_SU_POLY_OFFSET_FRONT_SCALE
+0x00028B84 PA_SU_POLY_OFFSET_FRONT_OFFSET
+0x00028B88 PA_SU_POLY_OFFSET_BACK_SCALE
+0x00028B8C PA_SU_POLY_OFFSET_BACK_OFFSET
+0x00028B74 VGT_GS_INSTANCE_CNT
+0x00028BD4 PA_SC_CENTROID_PRIORITY_0
+0x00028BD8 PA_SC_CENTROID_PRIORITY_1
+0x00028BDC PA_SC_LINE_CNTL
+0x00028BE4 PA_SU_VTX_CNTL
+0x00028BE8 PA_CL_GB_VERT_CLIP_ADJ
+0x00028BEC PA_CL_GB_VERT_DISC_ADJ
+0x00028BF0 PA_CL_GB_HORZ_CLIP_ADJ
+0x00028BF4 PA_CL_GB_HORZ_DISC_ADJ
+0x00028BF8 PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y0_0
+0x00028BFC PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y0_1
+0x00028C00 PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y0_2
+0x00028C04 PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y0_3
+0x00028C08 PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y0_0
+0x00028C0C PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y0_1
+0x00028C10 PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y0_2
+0x00028C14 PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y0_3
+0x00028C18 PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y1_0
+0x00028C1C PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y1_1
+0x00028C20 PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y1_2
+0x00028C24 PA_SC_AA_SAMPLE_LOCS_PIXEL_X0_Y1_3
+0x00028C28 PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y1_0
+0x00028C2C PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y1_1
+0x00028C30 PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y1_2
+0x00028C34 PA_SC_AA_SAMPLE_LOCS_PIXEL_X1_Y1_3
+0x00028C38 PA_SC_AA_MASK_X0_Y0_X1_Y0
+0x00028C3C PA_SC_AA_MASK_X0_Y1_X1_Y1
+0x00028C8C CB_COLOR0_CLEAR_WORD0
+0x00028C90 CB_COLOR0_CLEAR_WORD1
+0x00028C94 CB_COLOR0_CLEAR_WORD2
+0x00028C98 CB_COLOR0_CLEAR_WORD3
+0x00028CC8 CB_COLOR1_CLEAR_WORD0
+0x00028CCC CB_COLOR1_CLEAR_WORD1
+0x00028CD0 CB_COLOR1_CLEAR_WORD2
+0x00028CD4 CB_COLOR1_CLEAR_WORD3
+0x00028D04 CB_COLOR2_CLEAR_WORD0
+0x00028D08 CB_COLOR2_CLEAR_WORD1
+0x00028D0C CB_COLOR2_CLEAR_WORD2
+0x00028D10 CB_COLOR2_CLEAR_WORD3
+0x00028D40 CB_COLOR3_CLEAR_WORD0
+0x00028D44 CB_COLOR3_CLEAR_WORD1
+0x00028D48 CB_COLOR3_CLEAR_WORD2
+0x00028D4C CB_COLOR3_CLEAR_WORD3
+0x00028D7C CB_COLOR4_CLEAR_WORD0
+0x00028D80 CB_COLOR4_CLEAR_WORD1
+0x00028D84 CB_COLOR4_CLEAR_WORD2
+0x00028D88 CB_COLOR4_CLEAR_WORD3
+0x00028DB8 CB_COLOR5_CLEAR_WORD0
+0x00028DBC CB_COLOR5_CLEAR_WORD1
+0x00028DC0 CB_COLOR5_CLEAR_WORD2
+0x00028DC4 CB_COLOR5_CLEAR_WORD3
+0x00028DF4 CB_COLOR6_CLEAR_WORD0
+0x00028DF8 CB_COLOR6_CLEAR_WORD1
+0x00028DFC CB_COLOR6_CLEAR_WORD2
+0x00028E00 CB_COLOR6_CLEAR_WORD3
+0x00028E30 CB_COLOR7_CLEAR_WORD0
+0x00028E34 CB_COLOR7_CLEAR_WORD1
+0x00028E38 CB_COLOR7_CLEAR_WORD2
+0x00028E3C CB_COLOR7_CLEAR_WORD3
+0x00028F80 SQ_ALU_CONST_BUFFER_SIZE_HS_0
+0x00028F84 SQ_ALU_CONST_BUFFER_SIZE_HS_1
+0x00028F88 SQ_ALU_CONST_BUFFER_SIZE_HS_2
+0x00028F8C SQ_ALU_CONST_BUFFER_SIZE_HS_3
+0x00028F90 SQ_ALU_CONST_BUFFER_SIZE_HS_4
+0x00028F94 SQ_ALU_CONST_BUFFER_SIZE_HS_5
+0x00028F98 SQ_ALU_CONST_BUFFER_SIZE_HS_6
+0x00028F9C SQ_ALU_CONST_BUFFER_SIZE_HS_7
+0x00028FA0 SQ_ALU_CONST_BUFFER_SIZE_HS_8
+0x00028FA4 SQ_ALU_CONST_BUFFER_SIZE_HS_9
+0x00028FA8 SQ_ALU_CONST_BUFFER_SIZE_HS_10
+0x00028FAC SQ_ALU_CONST_BUFFER_SIZE_HS_11
+0x00028FB0 SQ_ALU_CONST_BUFFER_SIZE_HS_12
+0x00028FB4 SQ_ALU_CONST_BUFFER_SIZE_HS_13
+0x00028FB8 SQ_ALU_CONST_BUFFER_SIZE_HS_14
+0x00028FBC SQ_ALU_CONST_BUFFER_SIZE_HS_15
+0x00028FC0 SQ_ALU_CONST_BUFFER_SIZE_LS_0
+0x00028FC4 SQ_ALU_CONST_BUFFER_SIZE_LS_1
+0x00028FC8 SQ_ALU_CONST_BUFFER_SIZE_LS_2
+0x00028FCC SQ_ALU_CONST_BUFFER_SIZE_LS_3
+0x00028FD0 SQ_ALU_CONST_BUFFER_SIZE_LS_4
+0x00028FD4 SQ_ALU_CONST_BUFFER_SIZE_LS_5
+0x00028FD8 SQ_ALU_CONST_BUFFER_SIZE_LS_6
+0x00028FDC SQ_ALU_CONST_BUFFER_SIZE_LS_7
+0x00028FE0 SQ_ALU_CONST_BUFFER_SIZE_LS_8
+0x00028FE4 SQ_ALU_CONST_BUFFER_SIZE_LS_9
+0x00028FE8 SQ_ALU_CONST_BUFFER_SIZE_LS_10
+0x00028FEC SQ_ALU_CONST_BUFFER_SIZE_LS_11
+0x00028FF0 SQ_ALU_CONST_BUFFER_SIZE_LS_12
+0x00028FF4 SQ_ALU_CONST_BUFFER_SIZE_LS_13
+0x00028FF8 SQ_ALU_CONST_BUFFER_SIZE_LS_14
+0x00028FFC SQ_ALU_CONST_BUFFER_SIZE_LS_15
+0x0003CFF0 SQ_VTX_BASE_VTX_LOC
+0x0003CFF4 SQ_VTX_START_INST_LOC
+0x0003FF00 SQ_TEX_SAMPLER_CLEAR
+0x0003FF04 SQ_TEX_RESOURCE_CLEAR
+0x0003FF08 SQ_LOOP_BOOL_CLEAR
diff --git a/drivers/gpu/drm/radeon/reg_srcs/evergreen b/drivers/gpu/drm/radeon/reg_srcs/evergreen
index 9177f9191837..7e1637176e08 100644
--- a/drivers/gpu/drm/radeon/reg_srcs/evergreen
+++ b/drivers/gpu/drm/radeon/reg_srcs/evergreen
@@ -1,4 +1,5 @@
 evergreen 0x9400
+0x0000802C GRBM_GFX_INDEX
 0x00008040 WAIT_UNTIL
 0x00008044 WAIT_UNTIL_POLL_CNTL
 0x00008048 WAIT_UNTIL_POLL_MASK
@@ -220,6 +221,7 @@ evergreen 0x9400
 0x00028348 PA_SC_VPORT_ZMIN_15
 0x0002834C PA_SC_VPORT_ZMAX_15
 0x00028350 SX_MISC
+0x00028354 SX_SURFACE_SYNC
 0x00028380 SQ_VTX_SEMANTIC_0
 0x00028384 SQ_VTX_SEMANTIC_1
 0x00028388 SQ_VTX_SEMANTIC_2
diff --git a/drivers/gpu/drm/radeon/rs400.c b/drivers/gpu/drm/radeon/rs400.c
index c76283d9eb3d..aa6a66eeb4ec 100644
--- a/drivers/gpu/drm/radeon/rs400.c
+++ b/drivers/gpu/drm/radeon/rs400.c
@@ -412,12 +412,12 @@ static int rs400_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index 5afe294ed51f..26807bf87473 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -866,12 +866,12 @@ static int rs600_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/radeon/rs690.c b/drivers/gpu/drm/radeon/rs690.c
index 6638c8e4c81b..9c2dc966c3ff 100644
--- a/drivers/gpu/drm/radeon/rs690.c
+++ b/drivers/gpu/drm/radeon/rs690.c
@@ -628,12 +628,12 @@ static int rs690_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index 64b57af93714..6613ee9ecca3 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -398,12 +398,12 @@ static int rv515_startup(struct radeon_device *rdev)
 	/* 1M ring buffer */
 	r = r100_cp_init(rdev, 1024 * 1024);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing CP (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
 	r = r100_ib_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failled initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
 		return r;
 	}
 	return 0;
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index d8ba67690656..5007b54f914f 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -1003,7 +1003,7 @@ static int rv770_vram_scratch_init(struct radeon_device *rdev)
 	u64 gpu_addr;
 
 	if (rdev->vram_scratch.robj == NULL) {
-		r = radeon_bo_create(rdev, NULL, RADEON_GPU_PAGE_SIZE,
+		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
 				     &rdev->vram_scratch.robj);
 		if (r) {
@@ -1210,7 +1210,7 @@ int rv770_resume(struct radeon_device *rdev)
 
 	r = r600_ib_test(rdev);
 	if (r) {
-		DRM_ERROR("radeon: failled testing IB (%d).\n", r);
+		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/savage/savage_drv.c b/drivers/gpu/drm/savage/savage_drv.c
index fa64d25d4248..6464490b240b 100644
--- a/drivers/gpu/drm/savage/savage_drv.c
+++ b/drivers/gpu/drm/savage/savage_drv.c
@@ -55,11 +55,6 @@ static struct drm_driver driver = {
 		 .llseek = noop_llseek,
 	},
 
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-	},
-
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -68,15 +63,20 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver savage_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init savage_init(void)
 {
 	driver.num_ioctls = savage_max_ioctl;
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &savage_pci_driver);
 }
 
 static void __exit savage_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &savage_pci_driver);
 }
 
 module_init(savage_init);
diff --git a/drivers/gpu/drm/sis/sis_drv.c b/drivers/gpu/drm/sis/sis_drv.c
index 4caf5d01cfd3..46d5be6e97e5 100644
--- a/drivers/gpu/drm/sis/sis_drv.c
+++ b/drivers/gpu/drm/sis/sis_drv.c
@@ -82,10 +82,6 @@ static struct drm_driver driver = {
 		 .fasync = drm_fasync,
 		 .llseek = noop_llseek,
 	},
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-	},
 
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
@@ -95,15 +91,20 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver sis_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init sis_init(void)
 {
 	driver.num_ioctls = sis_max_ioctl;
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &sis_pci_driver);
 }
 
 static void __exit sis_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &sis_pci_driver);
 }
 
 module_init(sis_init);
diff --git a/drivers/gpu/drm/tdfx/tdfx_drv.c b/drivers/gpu/drm/tdfx/tdfx_drv.c
index b70fa91d761a..8bf98810a8d6 100644
--- a/drivers/gpu/drm/tdfx/tdfx_drv.c
+++ b/drivers/gpu/drm/tdfx/tdfx_drv.c
@@ -52,10 +52,6 @@ static struct drm_driver driver = {
 		 .fasync = drm_fasync,
 		 .llseek = noop_llseek,
 	},
-	.pci_driver = {
-		 .name = DRIVER_NAME,
-		 .id_table = pciidlist,
-	},
 
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
@@ -65,14 +61,19 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver tdfx_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init tdfx_init(void)
 {
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &tdfx_pci_driver);
 }
 
 static void __exit tdfx_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &tdfx_pci_driver);
 }
 
 module_init(tdfx_init);
diff --git a/drivers/gpu/drm/ttm/ttm_agp_backend.c b/drivers/gpu/drm/ttm/ttm_agp_backend.c
index f999e36f30b4..1c4a72f681c1 100644
--- a/drivers/gpu/drm/ttm/ttm_agp_backend.c
+++ b/drivers/gpu/drm/ttm/ttm_agp_backend.c
@@ -47,7 +47,8 @@ struct ttm_agp_backend {
 
 static int ttm_agp_populate(struct ttm_backend *backend,
 			    unsigned long num_pages, struct page **pages,
-			    struct page *dummy_read_page)
+			    struct page *dummy_read_page,
+			    dma_addr_t *dma_addrs)
 {
 	struct ttm_agp_backend *agp_be =
 	    container_of(backend, struct ttm_agp_backend, backend);
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index af61fc29e843..0b6a55ac2f87 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -406,11 +406,12 @@ static int ttm_bo_handle_move_mem(struct ttm_buffer_object *bo,
 		}
 
 		if (bo->mem.mem_type == TTM_PL_SYSTEM) {
+			if (bdev->driver->move_notify)
+				bdev->driver->move_notify(bo, mem);
 			bo->mem = *mem;
 			mem->mm_node = NULL;
 			goto moved;
 		}
-
 	}
 
 	if (bdev->driver->move_notify)
diff --git a/drivers/gpu/drm/ttm/ttm_page_alloc.c b/drivers/gpu/drm/ttm/ttm_page_alloc.c
index b1e02fffd3cc..737a2a2e46a5 100644
--- a/drivers/gpu/drm/ttm/ttm_page_alloc.c
+++ b/drivers/gpu/drm/ttm/ttm_page_alloc.c
@@ -38,6 +38,7 @@
 #include <linux/mm.h>
 #include <linux/seq_file.h> /* for seq_printf */
 #include <linux/slab.h>
+#include <linux/dma-mapping.h>
 
 #include <asm/atomic.h>
 
@@ -662,7 +663,8 @@ out:
  * cached pages.
  */
 int ttm_get_pages(struct list_head *pages, int flags,
-		enum ttm_caching_state cstate, unsigned count)
+		  enum ttm_caching_state cstate, unsigned count,
+		  dma_addr_t *dma_address)
 {
 	struct ttm_page_pool *pool = ttm_get_pool(flags, cstate);
 	struct page *p = NULL;
@@ -681,14 +683,22 @@ int ttm_get_pages(struct list_head *pages, int flags,
 			gfp_flags |= GFP_HIGHUSER;
 
 		for (r = 0; r < count; ++r) {
-			p = alloc_page(gfp_flags);
+			if ((flags & TTM_PAGE_FLAG_DMA32) && dma_address) {
+				void *addr;
+				addr = dma_alloc_coherent(NULL, PAGE_SIZE,
+							  &dma_address[r],
+							  gfp_flags);
+				if (addr == NULL)
+					return -ENOMEM;
+				p = virt_to_page(addr);
+			} else
+				p = alloc_page(gfp_flags);
 			if (!p) {
 
 				printk(KERN_ERR TTM_PFX
 				       "Unable to allocate page.");
 				return -ENOMEM;
 			}
-
 			list_add(&p->lru, pages);
 		}
 		return 0;
@@ -720,7 +730,7 @@ int ttm_get_pages(struct list_head *pages, int flags,
 			printk(KERN_ERR TTM_PFX
 			       "Failed to allocate extra pages "
 			       "for large request.");
-			ttm_put_pages(pages, 0, flags, cstate);
+			ttm_put_pages(pages, 0, flags, cstate, NULL);
 			return r;
 		}
 	}
@@ -731,17 +741,29 @@ int ttm_get_pages(struct list_head *pages, int flags,
 
 /* Put all pages in pages list to correct pool to wait for reuse */
 void ttm_put_pages(struct list_head *pages, unsigned page_count, int flags,
-		enum ttm_caching_state cstate)
+		   enum ttm_caching_state cstate, dma_addr_t *dma_address)
 {
 	unsigned long irq_flags;
 	struct ttm_page_pool *pool = ttm_get_pool(flags, cstate);
 	struct page *p, *tmp;
+	unsigned r;
 
 	if (pool == NULL) {
 		/* No pool for this memory type so free the pages */
 
+		r = page_count-1;
 		list_for_each_entry_safe(p, tmp, pages, lru) {
-			__free_page(p);
+			if ((flags & TTM_PAGE_FLAG_DMA32) && dma_address) {
+				void *addr = page_address(p);
+				WARN_ON(!addr || !dma_address[r]);
+				if (addr)
+					dma_free_coherent(NULL, PAGE_SIZE,
+							  addr,
+							  dma_address[r]);
+				dma_address[r] = 0;
+			} else
+				__free_page(p);
+			r--;
 		}
 		/* Make the pages list empty */
 		INIT_LIST_HEAD(pages);
diff --git a/drivers/gpu/drm/ttm/ttm_tt.c b/drivers/gpu/drm/ttm/ttm_tt.c
index af789dc869b9..86d5b1745a45 100644
--- a/drivers/gpu/drm/ttm/ttm_tt.c
+++ b/drivers/gpu/drm/ttm/ttm_tt.c
@@ -49,12 +49,16 @@ static int ttm_tt_swapin(struct ttm_tt *ttm);
 static void ttm_tt_alloc_page_directory(struct ttm_tt *ttm)
 {
 	ttm->pages = drm_calloc_large(ttm->num_pages, sizeof(*ttm->pages));
+	ttm->dma_address = drm_calloc_large(ttm->num_pages,
+					    sizeof(*ttm->dma_address));
 }
 
 static void ttm_tt_free_page_directory(struct ttm_tt *ttm)
 {
 	drm_free_large(ttm->pages);
 	ttm->pages = NULL;
+	drm_free_large(ttm->dma_address);
+	ttm->dma_address = NULL;
 }
 
 static void ttm_tt_free_user_pages(struct ttm_tt *ttm)
@@ -105,7 +109,8 @@ static struct page *__ttm_tt_get_page(struct ttm_tt *ttm, int index)
 
 		INIT_LIST_HEAD(&h);
 
-		ret = ttm_get_pages(&h, ttm->page_flags, ttm->caching_state, 1);
+		ret = ttm_get_pages(&h, ttm->page_flags, ttm->caching_state, 1,
+				    &ttm->dma_address[index]);
 
 		if (ret != 0)
 			return NULL;
@@ -164,7 +169,7 @@ int ttm_tt_populate(struct ttm_tt *ttm)
 	}
 
 	be->func->populate(be, ttm->num_pages, ttm->pages,
-			   ttm->dummy_read_page);
+			   ttm->dummy_read_page, ttm->dma_address);
 	ttm->state = tt_unbound;
 	return 0;
 }
@@ -298,7 +303,8 @@ static void ttm_tt_free_alloced_pages(struct ttm_tt *ttm)
 			count++;
 		}
 	}
-	ttm_put_pages(&h, count, ttm->page_flags, ttm->caching_state);
+	ttm_put_pages(&h, count, ttm->page_flags, ttm->caching_state,
+		      ttm->dma_address);
 	ttm->state = tt_unpopulated;
 	ttm->first_himem_page = ttm->num_pages;
 	ttm->last_lomem_page = -1;
diff --git a/drivers/gpu/drm/via/via_drv.c b/drivers/gpu/drm/via/via_drv.c
index e1ff4e7a6eb0..920a55214bcf 100644
--- a/drivers/gpu/drm/via/via_drv.c
+++ b/drivers/gpu/drm/via/via_drv.c
@@ -62,10 +62,6 @@ static struct drm_driver driver = {
 		.fasync = drm_fasync,
 		.llseek = noop_llseek,
 		},
-	.pci_driver = {
-		.name = DRIVER_NAME,
-		.id_table = pciidlist,
-	},
 
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
@@ -75,16 +71,21 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static struct pci_driver via_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+};
+
 static int __init via_init(void)
 {
 	driver.num_ioctls = via_max_ioctl;
 	via_init_command_verifier();
-	return drm_init(&driver);
+	return drm_pci_init(&driver, &via_pci_driver);
 }
 
 static void __exit via_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &via_pci_driver);
 }
 
 module_init(via_init);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c b/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
index 80bc37b274e7..87e43e0733bf 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
@@ -102,7 +102,8 @@ struct vmw_ttm_backend {
 
 static int vmw_ttm_populate(struct ttm_backend *backend,
 			    unsigned long num_pages, struct page **pages,
-			    struct page *dummy_read_page)
+			    struct page *dummy_read_page,
+			    dma_addr_t *dma_addrs)
 {
 	struct vmw_ttm_backend *vmw_be =
 	    container_of(backend, struct vmw_ttm_backend, backend);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index 10ca97ee0206..96949b93d920 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -909,15 +909,6 @@ static struct drm_driver driver = {
 #endif
 		 .llseek = noop_llseek,
 	},
-	.pci_driver = {
-		 .name = VMWGFX_DRIVER_NAME,
-		 .id_table = vmw_pci_id_list,
-		 .probe = vmw_probe,
-		 .remove = vmw_remove,
-		 .driver = {
-			 .pm = &vmw_pm_ops
-		 }
-	 },
 	.name = VMWGFX_DRIVER_NAME,
 	.desc = VMWGFX_DRIVER_DESC,
 	.date = VMWGFX_DRIVER_DATE,
@@ -926,6 +917,16 @@ static struct drm_driver driver = {
 	.patchlevel = VMWGFX_DRIVER_PATCHLEVEL
 };
 
+static struct pci_driver vmw_pci_driver = {
+	.name = VMWGFX_DRIVER_NAME,
+	.id_table = vmw_pci_id_list,
+	.probe = vmw_probe,
+	.remove = vmw_remove,
+	.driver = {
+		.pm = &vmw_pm_ops
+	}
+};
+
 static int vmw_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	return drm_get_pci_dev(pdev, ent, &driver);
@@ -934,7 +935,7 @@ static int vmw_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 static int __init vmwgfx_init(void)
 {
 	int ret;
-	ret = drm_init(&driver);
+	ret = drm_pci_init(&driver, &vmw_pci_driver);
 	if (ret)
 		DRM_ERROR("Failed initializing DRM.\n");
 	return ret;
@@ -942,7 +943,7 @@ static int __init vmwgfx_init(void)
 
 static void __exit vmwgfx_exit(void)
 {
-	drm_exit(&driver);
+	drm_pci_exit(&driver, &vmw_pci_driver);
 }
 
 module_init(vmwgfx_init);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
index 29113c9b26a8..b3a2cd5118d7 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
@@ -345,7 +345,7 @@ static enum drm_connector_status
 	return connector_status_disconnected;
 }
 
-static struct drm_display_mode vmw_ldu_connector_builtin[] = {
+static const struct drm_display_mode vmw_ldu_connector_builtin[] = {
 	/* 640x480@60Hz */
 	{ DRM_MODE("640x480", DRM_MODE_TYPE_DRIVER, 25175, 640, 656,
 		   752, 800, 0, 480, 489, 492, 525, 0,
@@ -429,7 +429,6 @@ static int vmw_ldu_connector_fill_modes(struct drm_connector *connector,
 	struct drm_device *dev = connector->dev;
 	struct vmw_private *dev_priv = vmw_priv(dev);
 	struct drm_display_mode *mode = NULL;
-	struct drm_display_mode *bmode;
 	struct drm_display_mode prefmode = { DRM_MODE("preferred",
 		DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED,
 		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
@@ -459,6 +458,8 @@ static int vmw_ldu_connector_fill_modes(struct drm_connector *connector,
 	}
 
 	for (i = 0; vmw_ldu_connector_builtin[i].type != 0; i++) {
+		const struct drm_display_mode *bmode;
+
 		bmode = &vmw_ldu_connector_builtin[i];
 		if (bmode->hdisplay > max_width ||
 		    bmode->vdisplay > max_height)
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 2560f01c1a63..4b88bbe9ed07 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -180,6 +180,14 @@ config HID_EZKEY
 	---help---
 	Support for Ezkey BTC 8193 keyboard.
 
+config HID_KEYTOUCH
+	tristate "Keyoutch HID devices"
+	depends on USB_HID
+	---help---
+	Support for Keytouch HID devices not fully compliant with
+	the specification. Currently supported:
+		- Keytouch IEC 60945
+
 config HID_KYE
 	tristate "Kye/Genius Ergo Mouse" if EXPERT
 	depends on USB_HID
@@ -218,6 +226,12 @@ config HID_KENSINGTON
 	---help---
 	Support for Kensington Slimblade Trackball.
 
+config HID_LCPOWER
+	tristate "LC-Power"
+	depends on USB_HID
+	---help---
+	Support for LC-Power RC1000MCE RF remote control.
+
 config HID_LOGITECH
 	tristate "Logitech devices" if EXPERT
 	depends on USB_HID
@@ -304,6 +318,7 @@ config HID_MULTITOUCH
 	  Say Y here if you have one of the following devices:
 	  - Cypress TrueTouch panels
 	  - Hanvon dual touch panels
+	  - IrTouch Infrared USB panels
 	  - Pixcir dual touch panels
 	  - 'Sensing Win7-TwoFinger' panel by GeneralTouch
 
@@ -417,10 +432,22 @@ config HID_ROCCAT
 	Say Y here if you have a Roccat mouse or keyboard and want OSD or
 	macro execution support.
 
+config HID_ROCCAT_COMMON
+	tristate
+
+config HID_ROCCAT_ARVO
+	tristate "Roccat Arvo keyboard support"
+	depends on USB_HID
+	select HID_ROCCAT
+	select HID_ROCCAT_COMMON
+	---help---
+	Support for Roccat Arvo keyboard.
+
 config HID_ROCCAT_KONE
 	tristate "Roccat Kone Mouse support"
 	depends on USB_HID
 	select HID_ROCCAT
+	select HID_ROCCAT_COMMON
 	---help---
 	Support for Roccat Kone mouse.
 
@@ -428,13 +455,23 @@ config HID_ROCCAT_KONEPLUS
 	tristate "Roccat Kone[+] mouse support"
 	depends on USB_HID
 	select HID_ROCCAT
+	select HID_ROCCAT_COMMON
 	---help---
 	Support for Roccat Kone[+] mouse.
 
+config HID_ROCCAT_KOVAPLUS
+	tristate "Roccat Kova[+] mouse support"
+	depends on USB_HID
+	select HID_ROCCAT
+	select HID_ROCCAT_COMMON
+	---help---
+	Support for Roccat Kova[+] mouse.
+
 config HID_ROCCAT_PYRA
 	tristate "Roccat Pyra mouse support"
 	depends on USB_HID
 	select HID_ROCCAT
+	select HID_ROCCAT_COMMON
 	---help---
 	Support for Roccat Pyra mouse.
 
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 6efc2a0370ad..04496abc7b76 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -41,7 +41,9 @@ obj-$(CONFIG_HID_ELECOM)	+= hid-elecom.o
 obj-$(CONFIG_HID_EZKEY)		+= hid-ezkey.o
 obj-$(CONFIG_HID_GYRATION)	+= hid-gyration.o
 obj-$(CONFIG_HID_KENSINGTON)	+= hid-kensington.o
+obj-$(CONFIG_HID_KEYTOUCH)	+= hid-keytouch.o
 obj-$(CONFIG_HID_KYE)		+= hid-kye.o
+obj-$(CONFIG_HID_LCPOWER)       += hid-lcpower.o
 obj-$(CONFIG_HID_LOGITECH)	+= hid-logitech.o
 obj-$(CONFIG_HID_MAGICMOUSE)    += hid-magicmouse.o
 obj-$(CONFIG_HID_MICROSOFT)	+= hid-microsoft.o
@@ -56,8 +58,11 @@ obj-$(CONFIG_HID_PANTHERLORD)	+= hid-pl.o
 obj-$(CONFIG_HID_PETALYNX)	+= hid-petalynx.o
 obj-$(CONFIG_HID_PICOLCD)	+= hid-picolcd.o
 obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o
+obj-$(CONFIG_HID_ROCCAT_COMMON)	+= hid-roccat-common.o
+obj-$(CONFIG_HID_ROCCAT_ARVO)	+= hid-roccat-arvo.o
 obj-$(CONFIG_HID_ROCCAT_KONE)	+= hid-roccat-kone.o
 obj-$(CONFIG_HID_ROCCAT_KONEPLUS)	+= hid-roccat-koneplus.o
+obj-$(CONFIG_HID_ROCCAT_KOVAPLUS)	+= hid-roccat-kovaplus.o
 obj-$(CONFIG_HID_ROCCAT_PYRA)	+= hid-roccat-pyra.o
 obj-$(CONFIG_HID_SAMSUNG)	+= hid-samsung.o
 obj-$(CONFIG_HID_SMARTJOYPLUS)	+= hid-sjoy.o
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index d678cf3d33d5..7d7034af4ca0 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -1159,6 +1159,32 @@ static bool hid_hiddev(struct hid_device *hdev)
 	return !!hid_match_id(hdev, hid_hiddev_list);
 }
 
+
+static ssize_t
+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);
+
+	if (off >= hdev->rsize)
+		return 0;
+
+	if (off + count > hdev->rsize)
+		count = hdev->rsize - off;
+
+	memcpy(buf, hdev->rdesc + off, count);
+
+	return count;
+}
+
+static struct bin_attribute dev_bin_attr_report_desc = {
+	.attr = { .name = "report_descriptor", .mode = 0444 },
+	.read = read_report_descriptor,
+	.size = HID_MAX_DESCRIPTOR_SIZE,
+};
+
 int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 {
 	static const char *types[] = { "Device", "Pointer", "Mouse", "Device",
@@ -1169,6 +1195,7 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 	char buf[64];
 	unsigned int i;
 	int len;
+	int ret;
 
 	if (hdev->quirks & HID_QUIRK_HIDDEV_FORCE)
 		connect_mask |= (HID_CONNECT_HIDDEV_FORCE | HID_CONNECT_HIDDEV);
@@ -1230,6 +1257,11 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 		bus = "<UNKNOWN>";
 	}
 
+	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);
@@ -1240,6 +1272,7 @@ EXPORT_SYMBOL_GPL(hid_connect);
 
 void hid_disconnect(struct hid_device *hdev)
 {
+	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)
@@ -1345,9 +1378,12 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HANVON, USB_DEVICE_ID_HANVON_MULTITOUCH) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_IRTOUCHSYSTEMS, USB_DEVICE_ID_IRTOUCH_INFRARED_USB) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KENSINGTON, USB_DEVICE_ID_KS_SLIMBLADE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_KEYTOUCH, USB_DEVICE_ID_KEYTOUCH_IEC) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_ERGO_525V) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LCPOWER, USB_DEVICE_ID_LCPOWER_LC1000 ) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_MX3000_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER_2) },
@@ -1405,7 +1441,9 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONEPLUS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KOVAPLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_PYRA_WIRED) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_IR_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAMSUNG, USB_DEVICE_ID_SAMSUNG_WIRELESS_KBD_MOUSE) },
diff --git a/drivers/hid/hid-gyration.c b/drivers/hid/hid-gyration.c
index 3975e039c3dd..e88b951cd10d 100644
--- a/drivers/hid/hid-gyration.c
+++ b/drivers/hid/hid-gyration.c
@@ -43,6 +43,11 @@ static int gyration_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	case 0x048: gy_map_key_clear(KEY_MEDIA);	break;
 	case 0x049: gy_map_key_clear(KEY_CAMERA);	break;
 	case 0x04a: gy_map_key_clear(KEY_VIDEO);	break;
+	case 0x05a: gy_map_key_clear(KEY_TEXT);		break;
+	case 0x05b: gy_map_key_clear(KEY_RED);		break;
+	case 0x05c: gy_map_key_clear(KEY_GREEN);	break;
+	case 0x05d: gy_map_key_clear(KEY_YELLOW);	break;
+	case 0x05e: gy_map_key_clear(KEY_BLUE);		break;
 
 	default:
 		return 0;
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 92a0d61a7379..f43525218fa2 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -333,6 +333,9 @@
 #define USB_VENDOR_ID_IMATION		0x0718
 #define USB_DEVICE_ID_DISC_STAKKA	0xd000
 
+#define USB_VENDOR_ID_IRTOUCHSYSTEMS	0x6615
+#define USB_DEVICE_ID_IRTOUCH_INFRARED_USB	0x0070
+
 #define USB_VENDOR_ID_JESS		0x0c45
 #define USB_DEVICE_ID_JESS_YUREX	0x1010
 
@@ -345,6 +348,9 @@
 #define USB_VENDOR_ID_KWORLD		0x1b80
 #define USB_DEVICE_ID_KWORLD_RADIO_FM700	0xd700
 
+#define USB_VENDOR_ID_KEYTOUCH		0x0926
+#define USB_DEVICE_ID_KEYTOUCH_IEC	0x3333
+
 #define USB_VENDOR_ID_KYE		0x0458
 #define USB_DEVICE_ID_KYE_ERGO_525V	0x0087
 #define USB_DEVICE_ID_KYE_GPEN_560	0x5003
@@ -352,6 +358,9 @@
 #define USB_VENDOR_ID_LABTEC		0x1020
 #define USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD	0x0006
 
+#define USB_VENDOR_ID_LCPOWER		0x1241
+#define USB_DEVICE_ID_LCPOWER_LC1000	0xf767
+
 #define USB_VENDOR_ID_LD		0x0f11
 #define USB_DEVICE_ID_LD_CASSY		0x1000
 #define USB_DEVICE_ID_LD_POCKETCASSY	0x1010
@@ -496,8 +505,10 @@
 #define USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN	0x3001
 
 #define USB_VENDOR_ID_ROCCAT		0x1e7d
+#define USB_DEVICE_ID_ROCCAT_ARVO	0x30d4
 #define USB_DEVICE_ID_ROCCAT_KONE	0x2ced
 #define USB_DEVICE_ID_ROCCAT_KONEPLUS	0x2d51
+#define USB_DEVICE_ID_ROCCAT_KOVAPLUS	0x2d50
 #define USB_DEVICE_ID_ROCCAT_PYRA_WIRED	0x2c24
 #define USB_DEVICE_ID_ROCCAT_PYRA_WIRELESS	0x2cf6
 
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 7f552bfad32c..33dde8724e02 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -290,14 +290,6 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		goto ignore;
 	}
 
-	if (field->report_type == HID_FEATURE_REPORT) {
-		if (device->driver->feature_mapping) {
-			device->driver->feature_mapping(device, hidinput, field,
-				usage);
-		}
-		goto ignore;
-	}
-
 	if (device->driver->input_mapping) {
 		int ret = device->driver->input_mapping(device, hidinput, field,
 				usage, &bit, &max);
@@ -835,6 +827,24 @@ static void hidinput_close(struct input_dev *dev)
 	hid_hw_close(hid);
 }
 
+static void report_features(struct hid_device *hid)
+{
+	struct hid_driver *drv = hid->driver;
+	struct hid_report_enum *rep_enum;
+	struct hid_report *rep;
+	int i, j;
+
+	if (!drv->feature_mapping)
+		return;
+
+	rep_enum = &hid->report_enum[HID_FEATURE_REPORT];
+	list_for_each_entry(rep, &rep_enum->report_list, list)
+		for (i = 0; i < rep->maxfield; i++)
+			for (j = 0; j < rep->field[i]->maxusage; j++)
+				drv->feature_mapping(hid, rep->field[i],
+						     rep->field[i]->usage + j);
+}
+
 /*
  * Register the input device; print a message.
  * Configure the input layer interface
@@ -863,7 +873,9 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 			return -1;
 	}
 
-	for (k = HID_INPUT_REPORT; k <= HID_FEATURE_REPORT; k++) {
+	report_features(hid);
+
+	for (k = HID_INPUT_REPORT; k <= HID_OUTPUT_REPORT; k++) {
 		if (k == HID_OUTPUT_REPORT &&
 			hid->quirks & HID_QUIRK_SKIP_OUTPUT_REPORTS)
 			continue;
@@ -888,8 +900,8 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 					hid->ll_driver->hidinput_input_event;
 				input_dev->open = hidinput_open;
 				input_dev->close = hidinput_close;
-				input_dev->setkeycode_new = hidinput_setkeycode;
-				input_dev->getkeycode_new = hidinput_getkeycode;
+				input_dev->setkeycode = hidinput_setkeycode;
+				input_dev->getkeycode = hidinput_getkeycode;
 
 				input_dev->name = hid->name;
 				input_dev->phys = hid->phys;
@@ -928,6 +940,7 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 	return 0;
 
 out_cleanup:
+	list_del(&hidinput->list);
 	input_free_device(hidinput->input);
 	kfree(hidinput);
 out_unwind:
diff --git a/drivers/hid/hid-keytouch.c b/drivers/hid/hid-keytouch.c
new file mode 100644
index 000000000000..07cd825f6f01
--- /dev/null
+++ b/drivers/hid/hid-keytouch.c
@@ -0,0 +1,66 @@
+/*
+ *  HID driver for Keytouch devices not fully compliant with HID standard
+ *
+ *  Copyright (c) 2011 Jiri Kosina
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+
+#include "hid-ids.h"
+
+/* Replace the broken report descriptor of this device with rather
+ * a default one */
+static __u8 keytouch_fixed_rdesc[] = {
+0x05, 0x01, 0x09, 0x06, 0xa1, 0x01, 0x05, 0x07, 0x19, 0xe0, 0x29, 0xe7, 0x15,
+0x00, 0x25, 0x01, 0x75, 0x01, 0x95, 0x08, 0x81, 0x02, 0x95, 0x01, 0x75, 0x08,
+0x81, 0x01, 0x95, 0x03, 0x75, 0x01, 0x05, 0x08, 0x19, 0x01, 0x29, 0x03, 0x91,
+0x02, 0x95, 0x05, 0x75, 0x01, 0x91, 0x01, 0x95, 0x06, 0x75, 0x08, 0x15, 0x00,
+0x26, 0xff, 0x00, 0x05, 0x07, 0x19, 0x00, 0x2a, 0xff, 0x00, 0x81, 0x00, 0xc0
+};
+
+static __u8 *keytouch_report_fixup(struct hid_device *hdev, __u8 *rdesc,
+		unsigned int *rsize)
+{
+	hid_info(hdev, "fixing up Keytouch IEC report descriptor\n");
+
+	rdesc = keytouch_fixed_rdesc;
+	*rsize = sizeof(keytouch_fixed_rdesc);
+
+	return rdesc;
+}
+
+static const struct hid_device_id keytouch_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_KEYTOUCH, USB_DEVICE_ID_KEYTOUCH_IEC) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, keytouch_devices);
+
+static struct hid_driver keytouch_driver = {
+	.name = "keytouch",
+	.id_table = keytouch_devices,
+	.report_fixup = keytouch_report_fixup,
+};
+
+static int __init keytouch_init(void)
+{
+	return hid_register_driver(&keytouch_driver);
+}
+
+static void __exit keytouch_exit(void)
+{
+	hid_unregister_driver(&keytouch_driver);
+}
+
+module_init(keytouch_init);
+module_exit(keytouch_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jiri Kosina");
diff --git a/drivers/hid/hid-lcpower.c b/drivers/hid/hid-lcpower.c
new file mode 100644
index 000000000000..c4fe9bd095b7
--- /dev/null
+++ b/drivers/hid/hid-lcpower.c
@@ -0,0 +1,70 @@
+/*
+ *  HID driver for LC Power Model RC1000MCE
+ *
+ *  Copyright (c) 2011 Chris Schlund 
+ *  based on hid-topseed module
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+
+#include "hid-ids.h"
+
+#define ts_map_key_clear(c)	hid_map_usage_clear(hi, usage, bit, max, \
+					EV_KEY, (c))
+static int ts_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	if ((usage->hid & HID_USAGE_PAGE) != 0x0ffbc0000)
+		return 0;
+
+	switch (usage->hid & HID_USAGE) {
+        case 0x046: ts_map_key_clear(KEY_YELLOW);         break;
+        case 0x047: ts_map_key_clear(KEY_GREEN);          break;
+        case 0x049: ts_map_key_clear(KEY_BLUE);           break;
+        case 0x04a: ts_map_key_clear(KEY_RED);		  break;
+        case 0x00d: ts_map_key_clear(KEY_HOME);           break;
+        case 0x025: ts_map_key_clear(KEY_TV);             break;
+        case 0x048: ts_map_key_clear(KEY_VCR);            break;
+        case 0x024: ts_map_key_clear(KEY_MENU);           break;
+        default:
+        return 0;
+	}
+
+	return 1;
+}
+
+static const struct hid_device_id ts_devices[] = {
+	{ HID_USB_DEVICE( USB_VENDOR_ID_LCPOWER, USB_DEVICE_ID_LCPOWER_LC1000) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, ts_devices);
+
+static struct hid_driver ts_driver = {
+	.name = "LC RC1000MCE",
+	.id_table = ts_devices,
+	.input_mapping = ts_input_mapping,
+};
+
+static int __init ts_init(void)
+{
+	return hid_register_driver(&ts_driver);
+}
+
+static void __exit ts_exit(void)
+{
+	hid_unregister_driver(&ts_driver);
+}
+
+module_init(ts_init);
+module_exit(ts_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 07d3183fdde5..65e7f20c4b22 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -24,6 +24,7 @@
 
 
 MODULE_AUTHOR("Stephane Chatty <chatty@enac.fr>");
+MODULE_AUTHOR("Benjamin Tissoires <benjamin.tissoires@gmail.com>");
 MODULE_DESCRIPTION("HID multitouch panels");
 MODULE_LICENSE("GPL");
 
@@ -65,10 +66,10 @@ struct mt_class {
 };
 
 /* classes of device behavior */
-#define MT_CLS_DEFAULT	1
-#define MT_CLS_DUAL1	2
-#define MT_CLS_DUAL2	3
-#define MT_CLS_CYPRESS	4
+#define MT_CLS_DEFAULT				1
+#define MT_CLS_DUAL_INRANGE_CONTACTID		2
+#define MT_CLS_DUAL_INRANGE_CONTACTNUMBER	3
+#define MT_CLS_CYPRESS				4
 
 /*
  * these device-dependent functions determine what slot corresponds
@@ -104,13 +105,13 @@ static int find_slot_from_contactid(struct mt_device *td)
 
 struct mt_class mt_classes[] = {
 	{ .name = MT_CLS_DEFAULT,
-		.quirks = MT_QUIRK_VALID_IS_INRANGE,
+		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP,
 		.maxcontacts = 10 },
-	{ .name = MT_CLS_DUAL1,
+	{ .name = MT_CLS_DUAL_INRANGE_CONTACTID,
 		.quirks = MT_QUIRK_VALID_IS_INRANGE |
 			MT_QUIRK_SLOT_IS_CONTACTID,
 		.maxcontacts = 2 },
-	{ .name = MT_CLS_DUAL2,
+	{ .name = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
 		.quirks = MT_QUIRK_VALID_IS_INRANGE |
 			MT_QUIRK_SLOT_IS_CONTACTNUMBER,
 		.maxcontacts = 2 },
@@ -122,7 +123,7 @@ struct mt_class mt_classes[] = {
 	{ }
 };
 
-static void mt_feature_mapping(struct hid_device *hdev, struct hid_input *hi,
+static void mt_feature_mapping(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage)
 {
 	if (usage->hid == HID_DG_INPUTMODE) {
@@ -466,15 +467,20 @@ static const struct hid_device_id mt_devices[] = {
 			USB_DEVICE_ID_CYPRESS_TRUETOUCH) },
 
 	/* GeneralTouch panel */
-	{ .driver_data = MT_CLS_DUAL2,
+	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
 		HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH,
 			USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS) },
 
+	/* IRTOUCH panels */
+	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
+		HID_USB_DEVICE(USB_VENDOR_ID_IRTOUCHSYSTEMS,
+			USB_DEVICE_ID_IRTOUCH_INFRARED_USB) },
+
 	/* PixCir-based panels */
-	{ .driver_data = MT_CLS_DUAL1,
+	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
 		HID_USB_DEVICE(USB_VENDOR_ID_HANVON,
 			USB_DEVICE_ID_HANVON_MULTITOUCH) },
-	{ .driver_data = MT_CLS_DUAL1,
+	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
 		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_PIXCIR_MULTI_TOUCH) },
 
diff --git a/drivers/hid/hid-ntrig.c b/drivers/hid/hid-ntrig.c
index beb403421e72..a93e58ca81ed 100644
--- a/drivers/hid/hid-ntrig.c
+++ b/drivers/hid/hid-ntrig.c
@@ -539,8 +539,19 @@ static int ntrig_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 static int ntrig_event (struct hid_device *hid, struct hid_field *field,
 			struct hid_usage *usage, __s32 value)
 {
-	struct input_dev *input = field->hidinput->input;
 	struct ntrig_data *nd = hid_get_drvdata(hid);
+	struct input_dev *input;
+
+	/* Skip processing if not a claimed input */
+	if (!(hid->claimed & HID_CLAIMED_INPUT))
+		goto not_claimed_input;
+
+	/* This function is being called before the structures are fully
+	 * initialized */
+	if(!(field->hidinput && field->hidinput->input))
+		return -EINVAL;
+
+	input = field->hidinput->input;
 
 	/* No special handling needed for the pen */
 	if (field->application == HID_DG_PEN)
@@ -810,6 +821,8 @@ static int ntrig_event (struct hid_device *hid, struct hid_field *field,
 		}
 	}
 
+not_claimed_input:
+
 	/* we have handled the hidinput part, now remains hiddev */
 	if ((hid->claimed & HID_CLAIMED_HIDDEV) && hid->hiddev_hid_event)
 		hid->hiddev_hid_event(hid, field, usage, value);
diff --git a/drivers/hid/hid-roccat-arvo.c b/drivers/hid/hid-roccat-arvo.c
new file mode 100644
index 000000000000..2307471d96dc
--- /dev/null
+++ b/drivers/hid/hid-roccat-arvo.c
@@ -0,0 +1,450 @@
+/*
+ * Roccat Arvo driver for Linux
+ *
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+/*
+ * Roccat Arvo is a gamer keyboard with 5 macro keys that can be configured in
+ * 5 profiles.
+ */
+
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/hid-roccat.h>
+#include "hid-ids.h"
+#include "hid-roccat-common.h"
+#include "hid-roccat-arvo.h"
+
+static struct class *arvo_class;
+
+static ssize_t arvo_sysfs_show_mode_key(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct arvo_device *arvo =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	struct usb_device *usb_dev =
+			interface_to_usbdev(to_usb_interface(dev->parent->parent));
+	struct arvo_mode_key temp_buf;
+	int retval;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_receive(usb_dev, ARVO_USB_COMMAND_MODE_KEY,
+			&temp_buf, sizeof(struct arvo_mode_key));
+	mutex_unlock(&arvo->arvo_lock);
+	if (retval)
+		return retval;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", temp_buf.state);
+}
+
+static ssize_t arvo_sysfs_set_mode_key(struct device *dev,
+		struct device_attribute *attr, char const *buf, size_t size)
+{
+	struct arvo_device *arvo =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	struct usb_device *usb_dev =
+			interface_to_usbdev(to_usb_interface(dev->parent->parent));
+	struct arvo_mode_key temp_buf;
+	unsigned long state;
+	int retval;
+
+	retval = strict_strtoul(buf, 10, &state);
+	if (retval)
+		return retval;
+
+	temp_buf.command = ARVO_COMMAND_MODE_KEY;
+	temp_buf.state = state;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_send(usb_dev, ARVO_USB_COMMAND_MODE_KEY,
+			&temp_buf, sizeof(struct arvo_mode_key));
+	mutex_unlock(&arvo->arvo_lock);
+	if (retval)
+		return retval;
+
+	return size;
+}
+
+static ssize_t arvo_sysfs_show_key_mask(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct arvo_device *arvo =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	struct usb_device *usb_dev =
+			interface_to_usbdev(to_usb_interface(dev->parent->parent));
+	struct arvo_key_mask temp_buf;
+	int retval;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_receive(usb_dev, ARVO_USB_COMMAND_KEY_MASK,
+			&temp_buf, sizeof(struct arvo_key_mask));
+	mutex_unlock(&arvo->arvo_lock);
+	if (retval)
+		return retval;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", temp_buf.key_mask);
+}
+
+static ssize_t arvo_sysfs_set_key_mask(struct device *dev,
+		struct device_attribute *attr, char const *buf, size_t size)
+{
+	struct arvo_device *arvo =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	struct usb_device *usb_dev =
+			interface_to_usbdev(to_usb_interface(dev->parent->parent));
+	struct arvo_key_mask temp_buf;
+	unsigned long key_mask;
+	int retval;
+
+	retval = strict_strtoul(buf, 10, &key_mask);
+	if (retval)
+		return retval;
+
+	temp_buf.command = ARVO_COMMAND_KEY_MASK;
+	temp_buf.key_mask = key_mask;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_send(usb_dev, ARVO_USB_COMMAND_KEY_MASK,
+			&temp_buf, sizeof(struct arvo_key_mask));
+	mutex_unlock(&arvo->arvo_lock);
+	if (retval)
+		return retval;
+
+	return size;
+}
+
+/* retval is 1-5 on success, < 0 on error */
+static int arvo_get_actual_profile(struct usb_device *usb_dev)
+{
+	struct arvo_actual_profile temp_buf;
+	int retval;
+
+	retval = roccat_common_receive(usb_dev, ARVO_USB_COMMAND_ACTUAL_PROFILE,
+			&temp_buf, sizeof(struct arvo_actual_profile));
+
+	if (retval)
+		return retval;
+
+	return temp_buf.actual_profile;
+}
+
+static ssize_t arvo_sysfs_show_actual_profile(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct arvo_device *arvo =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", arvo->actual_profile);
+}
+
+static ssize_t arvo_sysfs_set_actual_profile(struct device *dev,
+		struct device_attribute *attr, char const *buf, size_t size)
+{
+	struct arvo_device *arvo =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	struct usb_device *usb_dev =
+			interface_to_usbdev(to_usb_interface(dev->parent->parent));
+	struct arvo_actual_profile temp_buf;
+	unsigned long profile;
+	int retval;
+
+	retval = strict_strtoul(buf, 10, &profile);
+	if (retval)
+		return retval;
+
+	temp_buf.command = ARVO_COMMAND_ACTUAL_PROFILE;
+	temp_buf.actual_profile = profile;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_send(usb_dev, ARVO_USB_COMMAND_ACTUAL_PROFILE,
+			&temp_buf, sizeof(struct arvo_actual_profile));
+	if (!retval) {
+		arvo->actual_profile = profile;
+		retval = size;
+	}
+	mutex_unlock(&arvo->arvo_lock);
+	return retval;
+}
+
+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 arvo_device *arvo = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval;
+
+	if (off != 0 || count != real_size)
+		return -EINVAL;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_send(usb_dev, command, buf, real_size);
+	mutex_unlock(&arvo->arvo_lock);
+
+	return (retval ? retval : real_size);
+}
+
+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 arvo_device *arvo = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval;
+
+	if (off >= real_size)
+		return 0;
+
+	if (off != 0 || count != real_size)
+		return -EINVAL;
+
+	mutex_lock(&arvo->arvo_lock);
+	retval = roccat_common_receive(usb_dev, command, buf, real_size);
+	mutex_unlock(&arvo->arvo_lock);
+
+	return (retval ? retval : real_size);
+}
+
+static ssize_t arvo_sysfs_write_button(struct file *fp,
+		struct kobject *kobj, struct bin_attribute *attr, char *buf,
+		loff_t off, size_t count)
+{
+	return arvo_sysfs_write(fp, kobj, buf, off, count,
+			sizeof(struct arvo_button), ARVO_USB_COMMAND_BUTTON);
+}
+
+static ssize_t arvo_sysfs_read_info(struct file *fp,
+		struct kobject *kobj, struct bin_attribute *attr, char *buf,
+		loff_t off, size_t count)
+{
+	return arvo_sysfs_read(fp, kobj, buf, off, count,
+			sizeof(struct arvo_info), ARVO_USB_COMMAND_INFO);
+}
+
+
+static struct device_attribute arvo_attributes[] = {
+	__ATTR(mode_key, 0660,
+			arvo_sysfs_show_mode_key, arvo_sysfs_set_mode_key),
+	__ATTR(key_mask, 0660,
+			arvo_sysfs_show_key_mask, arvo_sysfs_set_key_mask),
+	__ATTR(actual_profile, 0660,
+			arvo_sysfs_show_actual_profile,
+			arvo_sysfs_set_actual_profile),
+	__ATTR_NULL
+};
+
+static struct bin_attribute arvo_bin_attributes[] = {
+	{
+		.attr = { .name = "button", .mode = 0220 },
+		.size = sizeof(struct arvo_button),
+		.write = arvo_sysfs_write_button
+	},
+	{
+		.attr = { .name = "info", .mode = 0440 },
+		.size = sizeof(struct arvo_info),
+		.read = arvo_sysfs_read_info
+	},
+	__ATTR_NULL
+};
+
+static int arvo_init_arvo_device_struct(struct usb_device *usb_dev,
+		struct arvo_device *arvo)
+{
+	int retval;
+
+	mutex_init(&arvo->arvo_lock);
+
+	retval = arvo_get_actual_profile(usb_dev);
+	if (retval < 0)
+		return retval;
+	arvo->actual_profile = retval;
+
+	return 0;
+}
+
+static int arvo_init_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct usb_device *usb_dev = interface_to_usbdev(intf);
+	struct arvo_device *arvo;
+	int retval;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			== USB_INTERFACE_PROTOCOL_KEYBOARD) {
+		hid_set_drvdata(hdev, NULL);
+		return 0;
+	}
+
+	arvo = kzalloc(sizeof(*arvo), GFP_KERNEL);
+	if (!arvo) {
+		hid_err(hdev, "can't alloc device descriptor\n");
+		return -ENOMEM;
+	}
+	hid_set_drvdata(hdev, arvo);
+
+	retval = arvo_init_arvo_device_struct(usb_dev, arvo);
+	if (retval) {
+		hid_err(hdev, "couldn't init struct arvo_device\n");
+		goto exit_free;
+	}
+
+	retval = roccat_connect(arvo_class, hdev,
+			sizeof(struct arvo_roccat_report));
+	if (retval < 0) {
+		hid_err(hdev, "couldn't init char dev\n");
+	} else {
+		arvo->chrdev_minor = retval;
+		arvo->roccat_claimed = 1;
+	}
+
+	return 0;
+exit_free:
+	kfree(arvo);
+	return retval;
+}
+
+static void arvo_remove_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct arvo_device *arvo;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			== USB_INTERFACE_PROTOCOL_KEYBOARD)
+		return;
+
+	arvo = hid_get_drvdata(hdev);
+	if (arvo->roccat_claimed)
+		roccat_disconnect(arvo->chrdev_minor);
+	kfree(arvo);
+}
+
+static int arvo_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int retval;
+
+	retval = hid_parse(hdev);
+	if (retval) {
+		hid_err(hdev, "parse failed\n");
+		goto exit;
+	}
+
+	retval = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (retval) {
+		hid_err(hdev, "hw start failed\n");
+		goto exit;
+	}
+
+	retval = arvo_init_specials(hdev);
+	if (retval) {
+		hid_err(hdev, "couldn't install keyboard\n");
+		goto exit_stop;
+	}
+
+	return 0;
+
+exit_stop:
+	hid_hw_stop(hdev);
+exit:
+	return retval;
+}
+
+static void arvo_remove(struct hid_device *hdev)
+{
+	arvo_remove_specials(hdev);
+	hid_hw_stop(hdev);
+}
+
+static void arvo_report_to_chrdev(struct arvo_device const *arvo,
+		u8 const *data)
+{
+	struct arvo_special_report const *special_report;
+	struct arvo_roccat_report roccat_report;
+
+	special_report = (struct arvo_special_report const *)data;
+
+	roccat_report.profile = arvo->actual_profile;
+	roccat_report.button = special_report->event &
+			ARVO_SPECIAL_REPORT_EVENT_MASK_BUTTON;
+	if ((special_report->event & ARVO_SPECIAL_REPORT_EVENT_MASK_ACTION) ==
+			ARVO_SPECIAL_REPORT_EVENT_ACTION_PRESS)
+		roccat_report.action = ARVO_ROCCAT_REPORT_ACTION_PRESS;
+	else
+		roccat_report.action = ARVO_ROCCAT_REPORT_ACTION_RELEASE;
+
+	roccat_report_event(arvo->chrdev_minor,
+			(uint8_t const *)&roccat_report);
+}
+
+static int arvo_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *data, int size)
+{
+	struct arvo_device *arvo = hid_get_drvdata(hdev);
+
+	if (size != 3)
+		return 0;
+
+	if (arvo->roccat_claimed)
+		arvo_report_to_chrdev(arvo, data);
+
+	return 0;
+}
+
+static const struct hid_device_id arvo_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, arvo_devices);
+
+static struct hid_driver arvo_driver = {
+	.name = "arvo",
+	.id_table = arvo_devices,
+	.probe = arvo_probe,
+	.remove = arvo_remove,
+	.raw_event = arvo_raw_event
+};
+
+static int __init arvo_init(void)
+{
+	int retval;
+
+	arvo_class = class_create(THIS_MODULE, "arvo");
+	if (IS_ERR(arvo_class))
+		return PTR_ERR(arvo_class);
+	arvo_class->dev_attrs = arvo_attributes;
+	arvo_class->dev_bin_attrs = arvo_bin_attributes;
+
+	retval = hid_register_driver(&arvo_driver);
+	if (retval)
+		class_destroy(arvo_class);
+	return retval;
+}
+
+static void __exit arvo_exit(void)
+{
+	hid_unregister_driver(&arvo_driver);
+	class_destroy(arvo_class);
+}
+
+module_init(arvo_init);
+module_exit(arvo_exit);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat Arvo driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-arvo.h b/drivers/hid/hid-roccat-arvo.h
new file mode 100644
index 000000000000..d284a781c99e
--- /dev/null
+++ b/drivers/hid/hid-roccat-arvo.h
@@ -0,0 +1,98 @@
+#ifndef __HID_ROCCAT_ARVO_H
+#define __HID_ROCCAT_ARVO_H
+
+/*
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/types.h>
+
+struct arvo_mode_key { /* 2 bytes */
+	uint8_t command; /* ARVO_COMMAND_MODE_KEY */
+	uint8_t state;
+} __packed;
+
+struct arvo_button {
+	uint8_t unknown[24];
+} __packed;
+
+struct arvo_info {
+	uint8_t unknown[8];
+} __packed;
+
+struct arvo_key_mask { /* 2 bytes */
+	uint8_t command; /* ARVO_COMMAND_KEY_MASK */
+	uint8_t key_mask;
+} __packed;
+
+/* selected profile is persistent */
+struct arvo_actual_profile { /* 2 bytes */
+	uint8_t command; /* ARVO_COMMAND_ACTUAL_PROFILE */
+	uint8_t actual_profile;
+} __packed;
+
+enum arvo_commands {
+	ARVO_COMMAND_MODE_KEY = 0x3,
+	ARVO_COMMAND_BUTTON = 0x4,
+	ARVO_COMMAND_INFO = 0x5,
+	ARVO_COMMAND_KEY_MASK = 0x6,
+	ARVO_COMMAND_ACTUAL_PROFILE = 0x7,
+};
+
+enum arvo_usb_commands {
+	ARVO_USB_COMMAND_MODE_KEY = 0x303,
+	/*
+	 * read/write
+	 * Read uses both index bytes as profile/key indexes
+	 * Write has index 0, profile/key is determined by payload
+	 */
+	ARVO_USB_COMMAND_BUTTON = 0x304,
+	ARVO_USB_COMMAND_INFO = 0x305,
+	ARVO_USB_COMMAND_KEY_MASK = 0x306,
+	ARVO_USB_COMMAND_ACTUAL_PROFILE = 0x307,
+};
+
+struct arvo_special_report {
+	uint8_t unknown1; /* always 0x01 */
+	uint8_t event;
+	uint8_t unknown2; /* always 0x70 */
+} __packed;
+
+enum arvo_special_report_events {
+	ARVO_SPECIAL_REPORT_EVENT_ACTION_PRESS = 0x10,
+	ARVO_SPECIAL_REPORT_EVENT_ACTION_RELEASE = 0x0,
+};
+
+enum arvo_special_report_event_masks {
+	ARVO_SPECIAL_REPORT_EVENT_MASK_ACTION = 0xf0,
+	ARVO_SPECIAL_REPORT_EVENT_MASK_BUTTON = 0x0f,
+};
+
+struct arvo_roccat_report {
+	uint8_t profile;
+	uint8_t button;
+	uint8_t action;
+} __packed;
+
+enum arvo_roccat_report_action {
+	ARVO_ROCCAT_REPORT_ACTION_RELEASE = 0,
+	ARVO_ROCCAT_REPORT_ACTION_PRESS = 1,
+};
+
+struct arvo_device {
+	int roccat_claimed;
+	int chrdev_minor;
+
+	struct mutex arvo_lock;
+
+	int actual_profile;
+};
+
+#endif
diff --git a/drivers/hid/hid-roccat-common.c b/drivers/hid/hid-roccat-common.c
new file mode 100644
index 000000000000..13b1eb0c8c65
--- /dev/null
+++ b/drivers/hid/hid-roccat-common.c
@@ -0,0 +1,62 @@
+/*
+ * Roccat common functions for device specific drivers
+ *
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/slab.h>
+#include "hid-roccat-common.h"
+
+int roccat_common_receive(struct usb_device *usb_dev, uint usb_command,
+		void *data, uint size)
+{
+	char *buf;
+	int len;
+
+	buf = kmalloc(size, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
+	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
+			USB_REQ_CLEAR_FEATURE,
+			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
+			usb_command, 0, buf, size, USB_CTRL_SET_TIMEOUT);
+
+	memcpy(data, buf, size);
+	kfree(buf);
+	return ((len < 0) ? len : ((len != size) ? -EIO : 0));
+}
+EXPORT_SYMBOL_GPL(roccat_common_receive);
+
+int roccat_common_send(struct usb_device *usb_dev, uint usb_command,
+		void const *data, uint size)
+{
+	char *buf;
+	int len;
+
+	buf = kmalloc(size, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
+	memcpy(buf, data, size);
+
+	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
+			USB_REQ_SET_CONFIGURATION,
+			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
+			usb_command, 0, buf, size, USB_CTRL_SET_TIMEOUT);
+
+	kfree(buf);
+	return ((len < 0) ? len : ((len != size) ? -EIO : 0));
+}
+EXPORT_SYMBOL_GPL(roccat_common_send);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat common driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-common.h b/drivers/hid/hid-roccat-common.h
new file mode 100644
index 000000000000..fe45fae05bb9
--- /dev/null
+++ b/drivers/hid/hid-roccat-common.h
@@ -0,0 +1,23 @@
+#ifndef __HID_ROCCAT_COMMON_H
+#define __HID_ROCCAT_COMMON_H
+
+/*
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/usb.h>
+#include <linux/types.h>
+
+int roccat_common_receive(struct usb_device *usb_dev, uint usb_command,
+		void *data, uint size);
+int roccat_common_send(struct usb_device *usb_dev, uint usb_command,
+		void const *data, uint size);
+
+#endif
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index cbd8cc42e75a..a57838d15267 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -28,11 +28,11 @@
 #include <linux/device.h>
 #include <linux/input.h>
 #include <linux/hid.h>
-#include <linux/usb.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/hid-roccat.h>
 #include "hid-ids.h"
-#include "hid-roccat.h"
+#include "hid-roccat-common.h"
 #include "hid-roccat-kone.h"
 
 static uint profile_numbers[5] = {0, 1, 2, 3, 4};
@@ -58,12 +58,8 @@ static void kone_set_settings_checksum(struct kone_settings *settings)
  */
 static int kone_check_write(struct usb_device *usb_dev)
 {
-	int len;
-	unsigned char *data;
-
-	data = kmalloc(1, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
+	int retval;
+	uint8_t data;
 
 	do {
 		/*
@@ -72,56 +68,36 @@ static int kone_check_write(struct usb_device *usb_dev)
 		 */
 		msleep(80);
 
-		len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-				USB_REQ_CLEAR_FEATURE,
-				USB_TYPE_CLASS | USB_RECIP_INTERFACE |
-				USB_DIR_IN,
-				kone_command_confirm_write, 0, data, 1,
-				USB_CTRL_SET_TIMEOUT);
-
-		if (len != 1) {
-			kfree(data);
-			return -EIO;
-		}
+		retval = roccat_common_receive(usb_dev,
+				kone_command_confirm_write, &data, 1);
+		if (retval)
+			return retval;
 
 		/*
 		 * value of 3 seems to mean something like
 		 * "not finished yet, but it looks good"
 		 * So check again after a moment.
 		 */
-	} while (*data == 3);
+	} while (data == 3);
 
-	if (*data == 1) { /* everything alright */
-		kfree(data);
+	if (data == 1) /* everything alright */
 		return 0;
-	} else { /* unknown answer */
-		hid_err(usb_dev, "got retval %d when checking write\n", *data);
-		kfree(data);
-		return -EIO;
-	}
+
+	/* unknown answer */
+	hid_err(usb_dev, "got retval %d when checking write\n", data);
+	return -EIO;
 }
 
 /*
  * Reads settings from mouse and stores it in @buf
- * @buf has to be alloced with GFP_KERNEL
  * On success returns 0
  * On failure returns errno
  */
 static int kone_get_settings(struct usb_device *usb_dev,
 		struct kone_settings *buf)
 {
-	int len;
-
-	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			kone_command_settings, 0, buf,
-			sizeof(struct kone_settings), USB_CTRL_SET_TIMEOUT);
-
-	if (len != sizeof(struct kone_settings))
-		return -EIO;
-
-	return 0;
+	return roccat_common_receive(usb_dev, kone_command_settings, buf,
+			sizeof(struct kone_settings));
 }
 
 /*
@@ -132,22 +108,12 @@ static int kone_get_settings(struct usb_device *usb_dev,
 static int kone_set_settings(struct usb_device *usb_dev,
 		struct kone_settings const *settings)
 {
-	int len;
-
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			kone_command_settings, 0, (char *)settings,
-			sizeof(struct kone_settings),
-			USB_CTRL_SET_TIMEOUT);
-
-	if (len != sizeof(struct kone_settings))
-		return -EIO;
-
-	if (kone_check_write(usb_dev))
-		return -EIO;
-
-	return 0;
+	int retval;
+	retval = roccat_common_send(usb_dev, kone_command_settings,
+			settings, sizeof(struct kone_settings));
+	if (retval)
+		return retval;
+	return kone_check_write(usb_dev);
 }
 
 /*
@@ -193,7 +159,7 @@ static int kone_set_profile(struct usb_device *usb_dev,
 	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
 			USB_REQ_SET_CONFIGURATION,
 			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			kone_command_profile, number, (char *)profile,
+			kone_command_profile, number, (void *)profile,
 			sizeof(struct kone_profile),
 			USB_CTRL_SET_TIMEOUT);
 
@@ -213,24 +179,15 @@ static int kone_set_profile(struct usb_device *usb_dev,
  */
 static int kone_get_weight(struct usb_device *usb_dev, int *result)
 {
-	int len;
-	uint8_t *data;
+	int retval;
+	uint8_t data;
 
-	data = kmalloc(1, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
+	retval = roccat_common_receive(usb_dev, kone_command_weight, &data, 1);
 
-	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			kone_command_weight, 0, data, 1, USB_CTRL_SET_TIMEOUT);
+	if (retval)
+		return retval;
 
-	if (len != 1) {
-		kfree(data);
-		return -EIO;
-	}
-	*result = (int)*data;
-	kfree(data);
+	*result = (int)data;
 	return 0;
 }
 
@@ -241,25 +198,15 @@ static int kone_get_weight(struct usb_device *usb_dev, int *result)
  */
 static int kone_get_firmware_version(struct usb_device *usb_dev, int *result)
 {
-	int len;
-	unsigned char *data;
-
-	data = kmalloc(2, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
+	int retval;
+	uint16_t data;
 
-	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			kone_command_firmware_version, 0, data, 2,
-			USB_CTRL_SET_TIMEOUT);
+	retval = roccat_common_receive(usb_dev, kone_command_firmware_version,
+			&data, 2);
+	if (retval)
+		return retval;
 
-	if (len != 2) {
-		kfree(data);
-		return -EIO;
-	}
-	*result = le16_to_cpu(*data);
-	kfree(data);
+	*result = le16_to_cpu(data);
 	return 0;
 }
 
@@ -435,23 +382,9 @@ static ssize_t kone_sysfs_show_tcu(struct device *dev,
 
 static int kone_tcu_command(struct usb_device *usb_dev, int number)
 {
-	int len;
-	char *value;
-
-	value = kmalloc(1, GFP_KERNEL);
-	if (!value)
-		return -ENOMEM;
-
-	*value = number;
-
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			kone_command_calibrate, 0, value, 1,
-			USB_CTRL_SET_TIMEOUT);
-
-	kfree(value);
-	return ((len != 1) ? -EIO : 0);
+	unsigned char value;
+	value = number;
+	return roccat_common_send(usb_dev, kone_command_calibrate, &value, 1);
 }
 
 /*
@@ -727,7 +660,8 @@ static int kone_init_specials(struct hid_device *hdev)
 			goto exit_free;
 		}
 
-		retval = roccat_connect(kone_class, hdev);
+		retval = roccat_connect(kone_class, hdev,
+				sizeof(struct kone_roccat_report));
 		if (retval < 0) {
 			hid_err(hdev, "couldn't init char dev\n");
 			/* be tolerant about not getting chrdev */
@@ -827,8 +761,7 @@ static void kone_report_to_chrdev(struct kone_device const *kone,
 		roccat_report.value = event->value;
 		roccat_report.key = 0;
 		roccat_report_event(kone->chrdev_minor,
-				(uint8_t *)&roccat_report,
-				sizeof(struct kone_roccat_report));
+				(uint8_t *)&roccat_report);
 		break;
 	case kone_mouse_event_call_overlong_macro:
 		if (event->value == kone_keystroke_action_press) {
@@ -836,8 +769,7 @@ static void kone_report_to_chrdev(struct kone_device const *kone,
 			roccat_report.value = kone->actual_profile;
 			roccat_report.key = event->macro_key;
 			roccat_report_event(kone->chrdev_minor,
-					(uint8_t *)&roccat_report,
-					sizeof(struct kone_roccat_report));
+					(uint8_t *)&roccat_report);
 		}
 		break;
 	}
@@ -912,8 +844,8 @@ static int __init kone_init(void)
 
 static void __exit kone_exit(void)
 {
-	class_destroy(kone_class);
 	hid_unregister_driver(&kone_driver);
+	class_destroy(kone_class);
 }
 
 module_init(kone_init);
diff --git a/drivers/hid/hid-roccat-koneplus.c b/drivers/hid/hid-roccat-koneplus.c
index 1608c8d1efd6..a8e2117756ac 100644
--- a/drivers/hid/hid-roccat-koneplus.c
+++ b/drivers/hid/hid-roccat-koneplus.c
@@ -19,11 +19,11 @@
 #include <linux/device.h>
 #include <linux/input.h>
 #include <linux/hid.h>
-#include <linux/usb.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/hid-roccat.h>
 #include "hid-ids.h"
-#include "hid-roccat.h"
+#include "hid-roccat-common.h"
 #include "hid-roccat-koneplus.h"
 
 static uint profile_numbers[5] = {0, 1, 2, 3, 4};
@@ -39,110 +39,63 @@ static void koneplus_profile_activated(struct koneplus_device *koneplus,
 static int koneplus_send_control(struct usb_device *usb_dev, uint value,
 		enum koneplus_control_requests request)
 {
-	int len;
-	struct koneplus_control *control;
+	struct koneplus_control control;
 
 	if ((request == KONEPLUS_CONTROL_REQUEST_PROFILE_SETTINGS ||
 			request == KONEPLUS_CONTROL_REQUEST_PROFILE_BUTTONS) &&
 			value > 4)
 		return -EINVAL;
 
-	control = kmalloc(sizeof(struct koneplus_control), GFP_KERNEL);
-	if (!control)
-		return -ENOMEM;
+	control.command = KONEPLUS_COMMAND_CONTROL;
+	control.value = value;
+	control.request = request;
 
-	control->command = KONEPLUS_COMMAND_CONTROL;
-	control->value = value;
-	control->request = request;
-
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			KONEPLUS_USB_COMMAND_CONTROL, 0, control,
-			sizeof(struct koneplus_control),
-			USB_CTRL_SET_TIMEOUT);
-
-	kfree(control);
-
-	if (len != sizeof(struct koneplus_control))
-		return len;
-
-	return 0;
-}
-
-static int koneplus_receive(struct usb_device *usb_dev, uint usb_command,
-		void *buf, uint size) {
-	int len;
-
-	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			usb_command, 0, buf, size, USB_CTRL_SET_TIMEOUT);
-
-	return (len != size) ? -EIO : 0;
+	return roccat_common_send(usb_dev, KONEPLUS_USB_COMMAND_CONTROL,
+			&control, sizeof(struct koneplus_control));
 }
 
 static int koneplus_receive_control_status(struct usb_device *usb_dev)
 {
 	int retval;
-	struct koneplus_control *control;
-
-	control = kmalloc(sizeof(struct koneplus_control), GFP_KERNEL);
-	if (!control)
-		return -ENOMEM;
+	struct koneplus_control control;
 
 	do {
-		retval = koneplus_receive(usb_dev, KONEPLUS_USB_COMMAND_CONTROL,
-				control, sizeof(struct koneplus_control));
+		retval = roccat_common_receive(usb_dev, KONEPLUS_USB_COMMAND_CONTROL,
+				&control, sizeof(struct koneplus_control));
 
 		/* check if we get a completely wrong answer */
 		if (retval)
-			goto out;
+			return retval;
 
-		if (control->value == KONEPLUS_CONTROL_REQUEST_STATUS_OK) {
-			retval = 0;
-			goto out;
-		}
+		if (control.value == KONEPLUS_CONTROL_REQUEST_STATUS_OK)
+			return 0;
 
 		/* indicates that hardware needs some more time to complete action */
-		if (control->value == KONEPLUS_CONTROL_REQUEST_STATUS_WAIT) {
+		if (control.value == KONEPLUS_CONTROL_REQUEST_STATUS_WAIT) {
 			msleep(500); /* windows driver uses 1000 */
 			continue;
 		}
 
 		/* seems to be critical - replug necessary */
-		if (control->value == KONEPLUS_CONTROL_REQUEST_STATUS_OVERLOAD) {
-			retval = -EINVAL;
-			goto out;
-		}
-
-		dev_err(&usb_dev->dev, "koneplus_receive_control_status: "
-				"unknown response value 0x%x\n", control->value);
-		retval = -EINVAL;
-		goto out;
+		if (control.value == KONEPLUS_CONTROL_REQUEST_STATUS_OVERLOAD)
+			return -EINVAL;
 
+		hid_err(usb_dev, "koneplus_receive_control_status: "
+				"unknown response value 0x%x\n", control.value);
+		return -EINVAL;
 	} while (1);
-out:
-	kfree(control);
-	return retval;
 }
 
 static int koneplus_send(struct usb_device *usb_dev, uint command,
-		void *buf, uint size) {
-	int len;
-
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			command, 0, buf, size, USB_CTRL_SET_TIMEOUT);
-
-	if (len != size)
-		return -EIO;
+		void const *buf, uint size)
+{
+	int retval;
 
-	if (koneplus_receive_control_status(usb_dev))
-		return -EIO;
+	retval = roccat_common_send(usb_dev, command, buf, size);
+	if (retval)
+		return retval;
 
-	return 0;
+	return koneplus_receive_control_status(usb_dev);
 }
 
 static int koneplus_select_profile(struct usb_device *usb_dev, uint number,
@@ -167,7 +120,7 @@ static int koneplus_select_profile(struct usb_device *usb_dev, uint number,
 static int koneplus_get_info(struct usb_device *usb_dev,
 		struct koneplus_info *buf)
 {
-	return koneplus_receive(usb_dev, KONEPLUS_USB_COMMAND_INFO,
+	return roccat_common_receive(usb_dev, KONEPLUS_USB_COMMAND_INFO,
 			buf, sizeof(struct koneplus_info));
 }
 
@@ -181,7 +134,7 @@ static int koneplus_get_profile_settings(struct usb_device *usb_dev,
 	if (retval)
 		return retval;
 
-	return koneplus_receive(usb_dev, KONEPLUS_USB_COMMAND_PROFILE_SETTINGS,
+	return roccat_common_receive(usb_dev, KONEPLUS_USB_COMMAND_PROFILE_SETTINGS,
 			buf, sizeof(struct koneplus_profile_settings));
 }
 
@@ -189,7 +142,7 @@ static int koneplus_set_profile_settings(struct usb_device *usb_dev,
 		struct koneplus_profile_settings const *settings)
 {
 	return koneplus_send(usb_dev, KONEPLUS_USB_COMMAND_PROFILE_SETTINGS,
-			(void *)settings, sizeof(struct koneplus_profile_settings));
+			settings, sizeof(struct koneplus_profile_settings));
 }
 
 static int koneplus_get_profile_buttons(struct usb_device *usb_dev,
@@ -202,7 +155,7 @@ static int koneplus_get_profile_buttons(struct usb_device *usb_dev,
 	if (retval)
 		return retval;
 
-	return koneplus_receive(usb_dev, KONEPLUS_USB_COMMAND_PROFILE_BUTTONS,
+	return roccat_common_receive(usb_dev, KONEPLUS_USB_COMMAND_PROFILE_BUTTONS,
 			buf, sizeof(struct koneplus_profile_buttons));
 }
 
@@ -210,27 +163,19 @@ static int koneplus_set_profile_buttons(struct usb_device *usb_dev,
 		struct koneplus_profile_buttons const *buttons)
 {
 	return koneplus_send(usb_dev, KONEPLUS_USB_COMMAND_PROFILE_BUTTONS,
-			(void *)buttons, sizeof(struct koneplus_profile_buttons));
+			buttons, sizeof(struct koneplus_profile_buttons));
 }
 
 /* retval is 0-4 on success, < 0 on error */
 static int koneplus_get_startup_profile(struct usb_device *usb_dev)
 {
-	struct koneplus_startup_profile *buf;
+	struct koneplus_startup_profile buf;
 	int retval;
 
-	buf = kmalloc(sizeof(struct koneplus_startup_profile), GFP_KERNEL);
+	retval = roccat_common_receive(usb_dev, KONEPLUS_USB_COMMAND_STARTUP_PROFILE,
+			&buf, sizeof(struct koneplus_startup_profile));
 
-	retval = koneplus_receive(usb_dev, KONEPLUS_USB_COMMAND_STARTUP_PROFILE,
-			buf, sizeof(struct koneplus_startup_profile));
-
-	if (retval)
-		goto out;
-
-	retval = buf->startup_profile;
-out:
-	kfree(buf);
-	return retval;
+	return retval ? retval : buf.startup_profile;
 }
 
 static int koneplus_set_startup_profile(struct usb_device *usb_dev,
@@ -243,7 +188,7 @@ static int koneplus_set_startup_profile(struct usb_device *usb_dev,
 	buf.startup_profile = startup_profile;
 
 	return koneplus_send(usb_dev, KONEPLUS_USB_COMMAND_STARTUP_PROFILE,
-			(char *)&buf, sizeof(struct koneplus_profile_buttons));
+			&buf, sizeof(struct koneplus_profile_buttons));
 }
 
 static ssize_t koneplus_sysfs_read(struct file *fp, struct kobject *kobj,
@@ -256,11 +201,14 @@ static ssize_t koneplus_sysfs_read(struct file *fp, struct kobject *kobj,
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
 
+	if (off >= real_size)
+		return 0;
+
 	if (off != 0 || count != real_size)
 		return -EINVAL;
 
 	mutex_lock(&koneplus->koneplus_lock);
-	retval = koneplus_receive(usb_dev, command, buf, real_size);
+	retval = roccat_common_receive(usb_dev, command, buf, real_size);
 	mutex_unlock(&koneplus->koneplus_lock);
 
 	if (retval)
@@ -283,7 +231,7 @@ static ssize_t koneplus_sysfs_write(struct file *fp, struct kobject *kobj,
 		return -EINVAL;
 
 	mutex_lock(&koneplus->koneplus_lock);
-	retval = koneplus_send(usb_dev, command, (void *)buf, real_size);
+	retval = koneplus_send(usb_dev, command, buf, real_size);
 	mutex_unlock(&koneplus->koneplus_lock);
 
 	if (retval)
@@ -347,7 +295,7 @@ static ssize_t koneplus_sysfs_read_profilex_settings(struct file *fp,
 		count = sizeof(struct koneplus_profile_settings) - off;
 
 	mutex_lock(&koneplus->koneplus_lock);
-	memcpy(buf, ((void const *)&koneplus->profile_settings[*(uint *)(attr->private)]) + off,
+	memcpy(buf, ((char const *)&koneplus->profile_settings[*(uint *)(attr->private)]) + off,
 			count);
 	mutex_unlock(&koneplus->koneplus_lock);
 
@@ -406,7 +354,7 @@ static ssize_t koneplus_sysfs_read_profilex_buttons(struct file *fp,
 		count = sizeof(struct koneplus_profile_buttons) - off;
 
 	mutex_lock(&koneplus->koneplus_lock);
-	memcpy(buf, ((void const *)&koneplus->profile_buttons[*(uint *)(attr->private)]) + off,
+	memcpy(buf, ((char const *)&koneplus->profile_buttons[*(uint *)(attr->private)]) + off,
 			count);
 	mutex_unlock(&koneplus->koneplus_lock);
 
@@ -609,11 +557,13 @@ static int koneplus_init_koneplus_device_struct(struct usb_device *usb_dev,
 		struct koneplus_device *koneplus)
 {
 	int retval, i;
-	static uint wait = 70; /* device will freeze with just 60 */
+	static uint wait = 100; /* device will freeze with just 60 */
 
 	mutex_init(&koneplus->koneplus_lock);
 
 	koneplus->startup_profile = koneplus_get_startup_profile(usb_dev);
+	if (koneplus->startup_profile < 0)
+		return koneplus->startup_profile;
 
 	msleep(wait);
 	retval = koneplus_get_info(usb_dev, &koneplus->info);
@@ -651,21 +601,21 @@ static int koneplus_init_specials(struct hid_device *hdev)
 
 		koneplus = kzalloc(sizeof(*koneplus), GFP_KERNEL);
 		if (!koneplus) {
-			dev_err(&hdev->dev, "can't alloc device descriptor\n");
+			hid_err(hdev, "can't alloc device descriptor\n");
 			return -ENOMEM;
 		}
 		hid_set_drvdata(hdev, koneplus);
 
 		retval = koneplus_init_koneplus_device_struct(usb_dev, koneplus);
 		if (retval) {
-			dev_err(&hdev->dev,
-					"couldn't init struct koneplus_device\n");
+			hid_err(hdev, "couldn't init struct koneplus_device\n");
 			goto exit_free;
 		}
 
-		retval = roccat_connect(koneplus_class, hdev);
+		retval = roccat_connect(koneplus_class, hdev,
+				sizeof(struct koneplus_roccat_report));
 		if (retval < 0) {
-			dev_err(&hdev->dev, "couldn't init char dev\n");
+			hid_err(hdev, "couldn't init char dev\n");
 		} else {
 			koneplus->chrdev_minor = retval;
 			koneplus->roccat_claimed = 1;
@@ -701,19 +651,19 @@ static int koneplus_probe(struct hid_device *hdev,
 
 	retval = hid_parse(hdev);
 	if (retval) {
-		dev_err(&hdev->dev, "parse failed\n");
+		hid_err(hdev, "parse failed\n");
 		goto exit;
 	}
 
 	retval = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
 	if (retval) {
-		dev_err(&hdev->dev, "hw start failed\n");
+		hid_err(hdev, "hw start failed\n");
 		goto exit;
 	}
 
 	retval = koneplus_init_specials(hdev);
 	if (retval) {
-		dev_err(&hdev->dev, "couldn't install mouse\n");
+		hid_err(hdev, "couldn't install mouse\n");
 		goto exit_stop;
 	}
 
@@ -769,8 +719,7 @@ static void koneplus_report_to_chrdev(struct koneplus_device const *koneplus,
 	roccat_report.data2 = button_report->data2;
 	roccat_report.profile = koneplus->actual_profile + 1;
 	roccat_report_event(koneplus->chrdev_minor,
-			(uint8_t const *)&roccat_report,
-			sizeof(struct koneplus_roccat_report));
+			(uint8_t const *)&roccat_report);
 }
 
 static int koneplus_raw_event(struct hid_device *hdev,
@@ -825,8 +774,8 @@ static int __init koneplus_init(void)
 
 static void __exit koneplus_exit(void)
 {
-	class_destroy(koneplus_class);
 	hid_unregister_driver(&koneplus_driver);
+	class_destroy(koneplus_class);
 }
 
 module_init(koneplus_init);
diff --git a/drivers/hid/hid-roccat-kovaplus.c b/drivers/hid/hid-roccat-kovaplus.c
new file mode 100644
index 000000000000..984be2f8967e
--- /dev/null
+++ b/drivers/hid/hid-roccat-kovaplus.c
@@ -0,0 +1,715 @@
+/*
+ * Roccat Kova[+] driver for Linux
+ *
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+/*
+ * Roccat Kova[+] is a bigger version of the Pyra with two more side buttons.
+ */
+
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/hid-roccat.h>
+#include "hid-ids.h"
+#include "hid-roccat-common.h"
+#include "hid-roccat-kovaplus.h"
+
+static uint profile_numbers[5] = {0, 1, 2, 3, 4};
+
+static struct class *kovaplus_class;
+
+static uint kovaplus_convert_event_cpi(uint value)
+{
+	return (value == 7 ? 4 : (value == 4 ? 3 : value));
+}
+
+static void kovaplus_profile_activated(struct kovaplus_device *kovaplus,
+		uint new_profile_index)
+{
+	kovaplus->actual_profile = new_profile_index;
+	kovaplus->actual_cpi = kovaplus->profile_settings[new_profile_index].cpi_startup_level;
+	kovaplus->actual_x_sensitivity = kovaplus->profile_settings[new_profile_index].sensitivity_x;
+	kovaplus->actual_y_sensitivity = kovaplus->profile_settings[new_profile_index].sensitivity_y;
+}
+
+static int kovaplus_send_control(struct usb_device *usb_dev, uint value,
+		enum kovaplus_control_requests request)
+{
+	int retval;
+	struct kovaplus_control control;
+
+	if ((request == KOVAPLUS_CONTROL_REQUEST_PROFILE_SETTINGS ||
+			request == KOVAPLUS_CONTROL_REQUEST_PROFILE_BUTTONS) &&
+			value > 4)
+		return -EINVAL;
+
+	control.command = KOVAPLUS_COMMAND_CONTROL;
+	control.value = value;
+	control.request = request;
+
+	retval = roccat_common_send(usb_dev, KOVAPLUS_USB_COMMAND_CONTROL,
+			&control, sizeof(struct kovaplus_control));
+
+	return retval;
+}
+
+static int kovaplus_receive_control_status(struct usb_device *usb_dev)
+{
+	int retval;
+	struct kovaplus_control control;
+
+	do {
+		retval = roccat_common_receive(usb_dev, KOVAPLUS_USB_COMMAND_CONTROL,
+				&control, sizeof(struct kovaplus_control));
+
+		/* check if we get a completely wrong answer */
+		if (retval)
+			return retval;
+
+		if (control.value == KOVAPLUS_CONTROL_REQUEST_STATUS_OK)
+			return 0;
+
+		/* indicates that hardware needs some more time to complete action */
+		if (control.value == KOVAPLUS_CONTROL_REQUEST_STATUS_WAIT) {
+			msleep(500); /* windows driver uses 1000 */
+			continue;
+		}
+
+		/* seems to be critical - replug necessary */
+		if (control.value == KOVAPLUS_CONTROL_REQUEST_STATUS_OVERLOAD)
+			return -EINVAL;
+
+		hid_err(usb_dev, "kovaplus_receive_control_status: "
+				"unknown response value 0x%x\n", control.value);
+		return -EINVAL;
+	} while (1);
+}
+
+static int kovaplus_send(struct usb_device *usb_dev, uint command,
+		void const *buf, uint size)
+{
+	int retval;
+
+	retval = roccat_common_send(usb_dev, command, buf, size);
+	if (retval)
+		return retval;
+
+	msleep(100);
+
+	return kovaplus_receive_control_status(usb_dev);
+}
+
+static int kovaplus_select_profile(struct usb_device *usb_dev, uint number,
+		enum kovaplus_control_requests request)
+{
+	return kovaplus_send_control(usb_dev, number, request);
+}
+
+static int kovaplus_get_info(struct usb_device *usb_dev,
+		struct kovaplus_info *buf)
+{
+	return roccat_common_receive(usb_dev, KOVAPLUS_USB_COMMAND_INFO,
+			buf, sizeof(struct kovaplus_info));
+}
+
+static int kovaplus_get_profile_settings(struct usb_device *usb_dev,
+		struct kovaplus_profile_settings *buf, uint number)
+{
+	int retval;
+
+	retval = kovaplus_select_profile(usb_dev, number,
+			KOVAPLUS_CONTROL_REQUEST_PROFILE_SETTINGS);
+	if (retval)
+		return retval;
+
+	return roccat_common_receive(usb_dev, KOVAPLUS_USB_COMMAND_PROFILE_SETTINGS,
+			buf, sizeof(struct kovaplus_profile_settings));
+}
+
+static int kovaplus_set_profile_settings(struct usb_device *usb_dev,
+		struct kovaplus_profile_settings const *settings)
+{
+	return kovaplus_send(usb_dev, KOVAPLUS_USB_COMMAND_PROFILE_SETTINGS,
+			settings, sizeof(struct kovaplus_profile_settings));
+}
+
+static int kovaplus_get_profile_buttons(struct usb_device *usb_dev,
+		struct kovaplus_profile_buttons *buf, int number)
+{
+	int retval;
+
+	retval = kovaplus_select_profile(usb_dev, number,
+			KOVAPLUS_CONTROL_REQUEST_PROFILE_BUTTONS);
+	if (retval)
+		return retval;
+
+	return roccat_common_receive(usb_dev, KOVAPLUS_USB_COMMAND_PROFILE_BUTTONS,
+			buf, sizeof(struct kovaplus_profile_buttons));
+}
+
+static int kovaplus_set_profile_buttons(struct usb_device *usb_dev,
+		struct kovaplus_profile_buttons const *buttons)
+{
+	return kovaplus_send(usb_dev, KOVAPLUS_USB_COMMAND_PROFILE_BUTTONS,
+			buttons, sizeof(struct kovaplus_profile_buttons));
+}
+
+/* retval is 0-4 on success, < 0 on error */
+static int kovaplus_get_actual_profile(struct usb_device *usb_dev)
+{
+	struct kovaplus_actual_profile buf;
+	int retval;
+
+	retval = roccat_common_receive(usb_dev, KOVAPLUS_USB_COMMAND_ACTUAL_PROFILE,
+			&buf, sizeof(struct kovaplus_actual_profile));
+
+	return retval ? retval : buf.actual_profile;
+}
+
+static int kovaplus_set_actual_profile(struct usb_device *usb_dev,
+		int new_profile)
+{
+	struct kovaplus_actual_profile buf;
+
+	buf.command = KOVAPLUS_COMMAND_ACTUAL_PROFILE;
+	buf.size = sizeof(struct kovaplus_actual_profile);
+	buf.actual_profile = new_profile;
+
+	return kovaplus_send(usb_dev, KOVAPLUS_USB_COMMAND_ACTUAL_PROFILE,
+			&buf, sizeof(struct kovaplus_actual_profile));
+}
+
+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 kovaplus_device *kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
+
+	if (off >= sizeof(struct kovaplus_profile_settings))
+		return 0;
+
+	if (off + count > sizeof(struct kovaplus_profile_settings))
+		count = sizeof(struct kovaplus_profile_settings) - off;
+
+	mutex_lock(&kovaplus->kovaplus_lock);
+	memcpy(buf, ((char const *)&kovaplus->profile_settings[*(uint *)(attr->private)]) + off,
+			count);
+	mutex_unlock(&kovaplus->kovaplus_lock);
+
+	return count;
+}
+
+static ssize_t kovaplus_sysfs_write_profile_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 kovaplus_device *kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval = 0;
+	int difference;
+	int profile_index;
+	struct kovaplus_profile_settings *profile_settings;
+
+	if (off != 0 || count != sizeof(struct kovaplus_profile_settings))
+		return -EINVAL;
+
+	profile_index = ((struct kovaplus_profile_settings const *)buf)->profile_index;
+	profile_settings = &kovaplus->profile_settings[profile_index];
+
+	mutex_lock(&kovaplus->kovaplus_lock);
+	difference = memcmp(buf, profile_settings,
+			sizeof(struct kovaplus_profile_settings));
+	if (difference) {
+		retval = kovaplus_set_profile_settings(usb_dev,
+				(struct kovaplus_profile_settings const *)buf);
+		if (!retval)
+			memcpy(profile_settings, buf,
+					sizeof(struct kovaplus_profile_settings));
+	}
+	mutex_unlock(&kovaplus->kovaplus_lock);
+
+	if (retval)
+		return retval;
+
+	return sizeof(struct kovaplus_profile_settings);
+}
+
+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 kovaplus_device *kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
+
+	if (off >= sizeof(struct kovaplus_profile_buttons))
+		return 0;
+
+	if (off + count > sizeof(struct kovaplus_profile_buttons))
+		count = sizeof(struct kovaplus_profile_buttons) - off;
+
+	mutex_lock(&kovaplus->kovaplus_lock);
+	memcpy(buf, ((char const *)&kovaplus->profile_buttons[*(uint *)(attr->private)]) + off,
+			count);
+	mutex_unlock(&kovaplus->kovaplus_lock);
+
+	return count;
+}
+
+static ssize_t kovaplus_sysfs_write_profile_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 kovaplus_device *kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval = 0;
+	int difference;
+	uint profile_index;
+	struct kovaplus_profile_buttons *profile_buttons;
+
+	if (off != 0 || count != sizeof(struct kovaplus_profile_buttons))
+		return -EINVAL;
+
+	profile_index = ((struct kovaplus_profile_buttons const *)buf)->profile_index;
+	profile_buttons = &kovaplus->profile_buttons[profile_index];
+
+	mutex_lock(&kovaplus->kovaplus_lock);
+	difference = memcmp(buf, profile_buttons,
+			sizeof(struct kovaplus_profile_buttons));
+	if (difference) {
+		retval = kovaplus_set_profile_buttons(usb_dev,
+				(struct kovaplus_profile_buttons const *)buf);
+		if (!retval)
+			memcpy(profile_buttons, buf,
+					sizeof(struct kovaplus_profile_buttons));
+	}
+	mutex_unlock(&kovaplus->kovaplus_lock);
+
+	if (retval)
+		return retval;
+
+	return sizeof(struct kovaplus_profile_buttons);
+}
+
+static ssize_t kovaplus_sysfs_show_actual_profile(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kovaplus_device *kovaplus =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	return snprintf(buf, PAGE_SIZE, "%d\n", kovaplus->actual_profile);
+}
+
+static ssize_t kovaplus_sysfs_set_actual_profile(struct device *dev,
+		struct device_attribute *attr, char const *buf, size_t size)
+{
+	struct kovaplus_device *kovaplus;
+	struct usb_device *usb_dev;
+	unsigned long profile;
+	int retval;
+
+	dev = dev->parent->parent;
+	kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
+	usb_dev = interface_to_usbdev(to_usb_interface(dev));
+
+	retval = strict_strtoul(buf, 10, &profile);
+	if (retval)
+		return retval;
+
+	if (profile >= 5)
+		return -EINVAL;
+
+	mutex_lock(&kovaplus->kovaplus_lock);
+	retval = kovaplus_set_actual_profile(usb_dev, profile);
+	kovaplus->actual_profile = profile;
+	mutex_unlock(&kovaplus->kovaplus_lock);
+	if (retval)
+		return retval;
+
+	return size;
+}
+
+static ssize_t kovaplus_sysfs_show_actual_cpi(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kovaplus_device *kovaplus =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	return snprintf(buf, PAGE_SIZE, "%d\n", kovaplus->actual_cpi);
+}
+
+static ssize_t kovaplus_sysfs_show_actual_sensitivity_x(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kovaplus_device *kovaplus =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	return snprintf(buf, PAGE_SIZE, "%d\n", kovaplus->actual_x_sensitivity);
+}
+
+static ssize_t kovaplus_sysfs_show_actual_sensitivity_y(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kovaplus_device *kovaplus =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	return snprintf(buf, PAGE_SIZE, "%d\n", kovaplus->actual_y_sensitivity);
+}
+
+static ssize_t kovaplus_sysfs_show_firmware_version(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kovaplus_device *kovaplus =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	return snprintf(buf, PAGE_SIZE, "%d\n", kovaplus->info.firmware_version);
+}
+
+static struct device_attribute kovaplus_attributes[] = {
+	__ATTR(actual_cpi, 0440,
+		kovaplus_sysfs_show_actual_cpi, NULL),
+	__ATTR(firmware_version, 0440,
+		kovaplus_sysfs_show_firmware_version, NULL),
+	__ATTR(actual_profile, 0660,
+		kovaplus_sysfs_show_actual_profile,
+		kovaplus_sysfs_set_actual_profile),
+	__ATTR(actual_sensitivity_x, 0440,
+		kovaplus_sysfs_show_actual_sensitivity_x, NULL),
+	__ATTR(actual_sensitivity_y, 0440,
+		kovaplus_sysfs_show_actual_sensitivity_y, NULL),
+	__ATTR_NULL
+};
+
+static struct bin_attribute kovaplus_bin_attributes[] = {
+	{
+		.attr = { .name = "profile_settings", .mode = 0220 },
+		.size = sizeof(struct kovaplus_profile_settings),
+		.write = kovaplus_sysfs_write_profile_settings
+	},
+	{
+		.attr = { .name = "profile1_settings", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_settings),
+		.read = kovaplus_sysfs_read_profilex_settings,
+		.private = &profile_numbers[0]
+	},
+	{
+		.attr = { .name = "profile2_settings", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_settings),
+		.read = kovaplus_sysfs_read_profilex_settings,
+		.private = &profile_numbers[1]
+	},
+	{
+		.attr = { .name = "profile3_settings", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_settings),
+		.read = kovaplus_sysfs_read_profilex_settings,
+		.private = &profile_numbers[2]
+	},
+	{
+		.attr = { .name = "profile4_settings", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_settings),
+		.read = kovaplus_sysfs_read_profilex_settings,
+		.private = &profile_numbers[3]
+	},
+	{
+		.attr = { .name = "profile5_settings", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_settings),
+		.read = kovaplus_sysfs_read_profilex_settings,
+		.private = &profile_numbers[4]
+	},
+	{
+		.attr = { .name = "profile_buttons", .mode = 0220 },
+		.size = sizeof(struct kovaplus_profile_buttons),
+		.write = kovaplus_sysfs_write_profile_buttons
+	},
+	{
+		.attr = { .name = "profile1_buttons", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_buttons),
+		.read = kovaplus_sysfs_read_profilex_buttons,
+		.private = &profile_numbers[0]
+	},
+	{
+		.attr = { .name = "profile2_buttons", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_buttons),
+		.read = kovaplus_sysfs_read_profilex_buttons,
+		.private = &profile_numbers[1]
+	},
+	{
+		.attr = { .name = "profile3_buttons", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_buttons),
+		.read = kovaplus_sysfs_read_profilex_buttons,
+		.private = &profile_numbers[2]
+	},
+	{
+		.attr = { .name = "profile4_buttons", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_buttons),
+		.read = kovaplus_sysfs_read_profilex_buttons,
+		.private = &profile_numbers[3]
+	},
+	{
+		.attr = { .name = "profile5_buttons", .mode = 0440 },
+		.size = sizeof(struct kovaplus_profile_buttons),
+		.read = kovaplus_sysfs_read_profilex_buttons,
+		.private = &profile_numbers[4]
+	},
+	__ATTR_NULL
+};
+
+static int kovaplus_init_kovaplus_device_struct(struct usb_device *usb_dev,
+		struct kovaplus_device *kovaplus)
+{
+	int retval, i;
+	static uint wait = 70; /* device will freeze with just 60 */
+
+	mutex_init(&kovaplus->kovaplus_lock);
+
+	retval = kovaplus_get_info(usb_dev, &kovaplus->info);
+	if (retval)
+		return retval;
+
+	for (i = 0; i < 5; ++i) {
+		msleep(wait);
+		retval = kovaplus_get_profile_settings(usb_dev,
+				&kovaplus->profile_settings[i], i);
+		if (retval)
+			return retval;
+
+		msleep(wait);
+		retval = kovaplus_get_profile_buttons(usb_dev,
+				&kovaplus->profile_buttons[i], i);
+		if (retval)
+			return retval;
+	}
+
+	msleep(wait);
+	retval = kovaplus_get_actual_profile(usb_dev);
+	if (retval < 0)
+		return retval;
+	kovaplus_profile_activated(kovaplus, retval);
+
+	return 0;
+}
+
+static int kovaplus_init_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct usb_device *usb_dev = interface_to_usbdev(intf);
+	struct kovaplus_device *kovaplus;
+	int retval;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			== USB_INTERFACE_PROTOCOL_MOUSE) {
+
+		kovaplus = kzalloc(sizeof(*kovaplus), GFP_KERNEL);
+		if (!kovaplus) {
+			hid_err(hdev, "can't alloc device descriptor\n");
+			return -ENOMEM;
+		}
+		hid_set_drvdata(hdev, kovaplus);
+
+		retval = kovaplus_init_kovaplus_device_struct(usb_dev, kovaplus);
+		if (retval) {
+			hid_err(hdev, "couldn't init struct kovaplus_device\n");
+			goto exit_free;
+		}
+
+		retval = roccat_connect(kovaplus_class, hdev,
+				sizeof(struct kovaplus_roccat_report));
+		if (retval < 0) {
+			hid_err(hdev, "couldn't init char dev\n");
+		} else {
+			kovaplus->chrdev_minor = retval;
+			kovaplus->roccat_claimed = 1;
+		}
+
+	} else {
+		hid_set_drvdata(hdev, NULL);
+	}
+
+	return 0;
+exit_free:
+	kfree(kovaplus);
+	return retval;
+}
+
+static void kovaplus_remove_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct kovaplus_device *kovaplus;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			== USB_INTERFACE_PROTOCOL_MOUSE) {
+		kovaplus = hid_get_drvdata(hdev);
+		if (kovaplus->roccat_claimed)
+			roccat_disconnect(kovaplus->chrdev_minor);
+		kfree(kovaplus);
+	}
+}
+
+static int kovaplus_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int retval;
+
+	retval = hid_parse(hdev);
+	if (retval) {
+		hid_err(hdev, "parse failed\n");
+		goto exit;
+	}
+
+	retval = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (retval) {
+		hid_err(hdev, "hw start failed\n");
+		goto exit;
+	}
+
+	retval = kovaplus_init_specials(hdev);
+	if (retval) {
+		hid_err(hdev, "couldn't install mouse\n");
+		goto exit_stop;
+	}
+
+	return 0;
+
+exit_stop:
+	hid_hw_stop(hdev);
+exit:
+	return retval;
+}
+
+static void kovaplus_remove(struct hid_device *hdev)
+{
+	kovaplus_remove_specials(hdev);
+	hid_hw_stop(hdev);
+}
+
+static void kovaplus_keep_values_up_to_date(struct kovaplus_device *kovaplus,
+		u8 const *data)
+{
+	struct kovaplus_mouse_report_button const *button_report;
+
+	if (data[0] != KOVAPLUS_MOUSE_REPORT_NUMBER_BUTTON)
+		return;
+
+	button_report = (struct kovaplus_mouse_report_button const *)data;
+
+	switch (button_report->type) {
+	case KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_PROFILE_1:
+		kovaplus_profile_activated(kovaplus, button_report->data1 - 1);
+		break;
+	case KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_CPI:
+		kovaplus->actual_cpi = kovaplus_convert_event_cpi(button_report->data1);
+	case KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_SENSITIVITY:
+		kovaplus->actual_x_sensitivity = button_report->data1;
+		kovaplus->actual_y_sensitivity = button_report->data2;
+	}
+}
+
+static void kovaplus_report_to_chrdev(struct kovaplus_device const *kovaplus,
+		u8 const *data)
+{
+	struct kovaplus_roccat_report roccat_report;
+	struct kovaplus_mouse_report_button const *button_report;
+
+	if (data[0] != KOVAPLUS_MOUSE_REPORT_NUMBER_BUTTON)
+		return;
+
+	button_report = (struct kovaplus_mouse_report_button const *)data;
+
+	if (button_report->type == KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_PROFILE_2)
+		return;
+
+	roccat_report.type = button_report->type;
+	roccat_report.profile = kovaplus->actual_profile + 1;
+
+	if (roccat_report.type == KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_MACRO ||
+			roccat_report.type == KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_SHORTCUT ||
+			roccat_report.type == KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_QUICKLAUNCH ||
+			roccat_report.type == KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_TIMER)
+		roccat_report.button = button_report->data1;
+	else
+		roccat_report.button = 0;
+
+	if (roccat_report.type == KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_CPI)
+		roccat_report.data1 = kovaplus_convert_event_cpi(button_report->data1);
+	else
+		roccat_report.data1 = button_report->data1;
+
+	roccat_report.data2 = button_report->data2;
+
+	roccat_report_event(kovaplus->chrdev_minor,
+			(uint8_t const *)&roccat_report);
+}
+
+static int kovaplus_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *data, int size)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct kovaplus_device *kovaplus = hid_get_drvdata(hdev);
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE)
+		return 0;
+
+	kovaplus_keep_values_up_to_date(kovaplus, data);
+
+	if (kovaplus->roccat_claimed)
+		kovaplus_report_to_chrdev(kovaplus, data);
+
+	return 0;
+}
+
+static const struct hid_device_id kovaplus_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KOVAPLUS) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, kovaplus_devices);
+
+static struct hid_driver kovaplus_driver = {
+		.name = "kovaplus",
+		.id_table = kovaplus_devices,
+		.probe = kovaplus_probe,
+		.remove = kovaplus_remove,
+		.raw_event = kovaplus_raw_event
+};
+
+static int __init kovaplus_init(void)
+{
+	int retval;
+
+	kovaplus_class = class_create(THIS_MODULE, "kovaplus");
+	if (IS_ERR(kovaplus_class))
+		return PTR_ERR(kovaplus_class);
+	kovaplus_class->dev_attrs = kovaplus_attributes;
+	kovaplus_class->dev_bin_attrs = kovaplus_bin_attributes;
+
+	retval = hid_register_driver(&kovaplus_driver);
+	if (retval)
+		class_destroy(kovaplus_class);
+	return retval;
+}
+
+static void __exit kovaplus_exit(void)
+{
+	hid_unregister_driver(&kovaplus_driver);
+	class_destroy(kovaplus_class);
+}
+
+module_init(kovaplus_init);
+module_exit(kovaplus_exit);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat Kova[+] driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-kovaplus.h b/drivers/hid/hid-roccat-kovaplus.h
new file mode 100644
index 000000000000..ce40607d21c7
--- /dev/null
+++ b/drivers/hid/hid-roccat-kovaplus.h
@@ -0,0 +1,157 @@
+#ifndef __HID_ROCCAT_KOVAPLUS_H
+#define __HID_ROCCAT_KOVAPLUS_H
+
+/*
+ * Copyright (c) 2010 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/types.h>
+
+struct kovaplus_control {
+	uint8_t command; /* KOVAPLUS_COMMAND_CONTROL */
+	uint8_t value;
+	uint8_t request;
+} __packed;
+
+enum kovaplus_control_requests {
+	/* read after write; value = 1 */
+	KOVAPLUS_CONTROL_REQUEST_STATUS = 0x0,
+	/* write; value = profile number range 0-4 */
+	KOVAPLUS_CONTROL_REQUEST_PROFILE_SETTINGS = 0x10,
+	/* write; value = profile number range 0-4 */
+	KOVAPLUS_CONTROL_REQUEST_PROFILE_BUTTONS = 0x20,
+};
+
+enum kovaplus_control_values {
+	KOVAPLUS_CONTROL_REQUEST_STATUS_OVERLOAD = 0, /* supposed */
+	KOVAPLUS_CONTROL_REQUEST_STATUS_OK = 1,
+	KOVAPLUS_CONTROL_REQUEST_STATUS_WAIT = 3, /* supposed */
+};
+
+struct kovaplus_actual_profile {
+	uint8_t command; /* KOVAPLUS_COMMAND_ACTUAL_PROFILE */
+	uint8_t size; /* always 3 */
+	uint8_t actual_profile; /* Range 0-4! */
+} __packed;
+
+struct kovaplus_profile_settings {
+	uint8_t command; /* KOVAPLUS_COMMAND_PROFILE_SETTINGS */
+	uint8_t size; /* 16 */
+	uint8_t profile_index; /* range 0-4 */
+	uint8_t unknown1;
+	uint8_t sensitivity_x; /* range 1-10 */
+	uint8_t sensitivity_y; /* range 1-10 */
+	uint8_t cpi_levels_enabled;
+	uint8_t cpi_startup_level; /* range 1-4 */
+	uint8_t data[8];
+} __packed;
+
+struct kovaplus_profile_buttons {
+	uint8_t command; /* KOVAPLUS_COMMAND_PROFILE_BUTTONS */
+	uint8_t size; /* 23 */
+	uint8_t profile_index; /* range 0-4 */
+	uint8_t data[20];
+} __packed;
+
+struct kovaplus_info {
+	uint8_t command; /* KOVAPLUS_COMMAND_INFO */
+	uint8_t size; /* 6 */
+	uint8_t firmware_version;
+	uint8_t unknown[3];
+} __packed;
+
+/* writes 1 on plugin */
+struct kovaplus_a {
+	uint8_t command; /* KOVAPLUS_COMMAND_A */
+	uint8_t size; /* 3 */
+	uint8_t unknown;
+} __packed;
+
+enum kovaplus_commands {
+	KOVAPLUS_COMMAND_CONTROL = 0x4,
+	KOVAPLUS_COMMAND_ACTUAL_PROFILE = 0x5,
+	KOVAPLUS_COMMAND_PROFILE_SETTINGS = 0x6,
+	KOVAPLUS_COMMAND_PROFILE_BUTTONS = 0x7,
+	KOVAPLUS_COMMAND_INFO = 0x9,
+	KOVAPLUS_COMMAND_A = 0xa,
+};
+
+enum kovaplus_usb_commands {
+	KOVAPLUS_USB_COMMAND_CONTROL = 0x304,
+	KOVAPLUS_USB_COMMAND_ACTUAL_PROFILE = 0x305,
+	KOVAPLUS_USB_COMMAND_PROFILE_SETTINGS = 0x306,
+	KOVAPLUS_USB_COMMAND_PROFILE_BUTTONS = 0x307,
+	KOVAPLUS_USB_COMMAND_INFO = 0x309,
+	KOVAPLUS_USB_COMMAND_A = 0x30a,
+};
+
+enum kovaplus_mouse_report_numbers {
+	KOVAPLUS_MOUSE_REPORT_NUMBER_MOUSE = 1,
+	KOVAPLUS_MOUSE_REPORT_NUMBER_AUDIO = 2,
+	KOVAPLUS_MOUSE_REPORT_NUMBER_BUTTON = 3,
+	KOVAPLUS_MOUSE_REPORT_NUMBER_KBD = 4,
+};
+
+struct kovaplus_mouse_report_button {
+	uint8_t report_number; /* KOVAPLUS_MOUSE_REPORT_NUMBER_BUTTON */
+	uint8_t unknown1;
+	uint8_t type;
+	uint8_t data1;
+	uint8_t data2;
+} __packed;
+
+enum kovaplus_mouse_report_button_types {
+	/* data1 = profile_number range 1-5; no release event */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_PROFILE_1 = 0x20,
+	/* data1 = profile_number range 1-5; no release event */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_PROFILE_2 = 0x30,
+	/* data1 = button_number range 1-18; data2 = action */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_MACRO = 0x40,
+	/* data1 = button_number range 1-18; data2 = action */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_SHORTCUT = 0x50,
+	/* data1 = button_number range 1-18; data2 = action */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_QUICKLAUNCH = 0x60,
+	/* data1 = button_number range 1-18; data2 = action */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_TIMER = 0x80,
+	/* data1 = 1 = 400, 2 = 800, 4 = 1600, 7 = 3200; no release event */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_CPI = 0xb0,
+	/* data1 + data2 = sense range 1-10; no release event */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_SENSITIVITY = 0xc0,
+	/* data1 = type as in profile_buttons; data2 = action */
+	KOVAPLUS_MOUSE_REPORT_BUTTON_TYPE_MULTIMEDIA = 0xf0,
+};
+
+enum kovaplus_mouse_report_button_actions {
+	KOVAPLUS_MOUSE_REPORT_BUTTON_ACTION_PRESS = 0,
+	KOVAPLUS_MOUSE_REPORT_BUTTON_ACTION_RELEASE = 1,
+};
+
+struct kovaplus_roccat_report {
+	uint8_t type;
+	uint8_t profile;
+	uint8_t button;
+	uint8_t data1;
+	uint8_t data2;
+} __packed;
+
+struct kovaplus_device {
+	int actual_profile;
+	int actual_cpi;
+	int actual_x_sensitivity;
+	int actual_y_sensitivity;
+	int roccat_claimed;
+	int chrdev_minor;
+	struct mutex kovaplus_lock;
+	struct kovaplus_info info;
+	struct kovaplus_profile_settings profile_settings[5];
+	struct kovaplus_profile_buttons profile_buttons[5];
+};
+
+#endif
diff --git a/drivers/hid/hid-roccat-pyra.c b/drivers/hid/hid-roccat-pyra.c
index 02c58e015bee..160f481344f6 100644
--- a/drivers/hid/hid-roccat-pyra.c
+++ b/drivers/hid/hid-roccat-pyra.c
@@ -20,11 +20,11 @@
 #include <linux/device.h>
 #include <linux/input.h>
 #include <linux/hid.h>
-#include <linux/usb.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/hid-roccat.h>
 #include "hid-ids.h"
-#include "hid-roccat.h"
+#include "hid-roccat-common.h"
 #include "hid-roccat-pyra.h"
 
 static uint profile_numbers[5] = {0, 1, 2, 3, 4};
@@ -42,7 +42,6 @@ static void profile_activated(struct pyra_device *pyra,
 static int pyra_send_control(struct usb_device *usb_dev, int value,
 		enum pyra_control_requests request)
 {
-	int len;
 	struct pyra_control control;
 
 	if ((request == PYRA_CONTROL_REQUEST_PROFILE_SETTINGS ||
@@ -54,47 +53,31 @@ static int pyra_send_control(struct usb_device *usb_dev, int value,
 	control.value = value;
 	control.request = request;
 
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			PYRA_USB_COMMAND_CONTROL, 0, (char *)&control,
-			sizeof(struct pyra_control),
-			USB_CTRL_SET_TIMEOUT);
-
-	if (len != sizeof(struct pyra_control))
-		return len;
-
-	return 0;
+	return roccat_common_send(usb_dev, PYRA_USB_COMMAND_CONTROL,
+			&control, sizeof(struct pyra_control));
 }
 
 static int pyra_receive_control_status(struct usb_device *usb_dev)
 {
-	int len;
+	int retval;
 	struct pyra_control control;
 
 	do {
 		msleep(10);
-
-		len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-				USB_REQ_CLEAR_FEATURE,
-				USB_TYPE_CLASS | USB_RECIP_INTERFACE |
-				USB_DIR_IN,
-				PYRA_USB_COMMAND_CONTROL, 0, (char *)&control,
-				sizeof(struct pyra_control),
-				USB_CTRL_SET_TIMEOUT);
+		retval = roccat_common_receive(usb_dev, PYRA_USB_COMMAND_CONTROL,
+				&control, sizeof(struct pyra_control));
 
 		/* requested too early, try again */
-	} while (len == -EPROTO);
+	} while (retval == -EPROTO);
 
-	if (len == sizeof(struct pyra_control) &&
-			control.command == PYRA_COMMAND_CONTROL &&
+	if (!retval && control.command == PYRA_COMMAND_CONTROL &&
 			control.request == PYRA_CONTROL_REQUEST_STATUS &&
 			control.value == 1)
-			return 0;
+		return 0;
 	else {
 		hid_err(usb_dev, "receive control status: unknown response 0x%x 0x%x\n",
 			control.request, control.value);
-		return -EINVAL;
+		return retval ? retval : -EINVAL;
 	}
 }
 
@@ -102,125 +85,72 @@ static int pyra_get_profile_settings(struct usb_device *usb_dev,
 		struct pyra_profile_settings *buf, int number)
 {
 	int retval;
-
 	retval = pyra_send_control(usb_dev, number,
 			PYRA_CONTROL_REQUEST_PROFILE_SETTINGS);
-
 	if (retval)
 		return retval;
-
-	retval = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			PYRA_USB_COMMAND_PROFILE_SETTINGS, 0, (char *)buf,
-			sizeof(struct pyra_profile_settings),
-			USB_CTRL_SET_TIMEOUT);
-
-	if (retval != sizeof(struct pyra_profile_settings))
-		return retval;
-
-	return 0;
+	return roccat_common_receive(usb_dev, PYRA_USB_COMMAND_PROFILE_SETTINGS,
+			buf, sizeof(struct pyra_profile_settings));
 }
 
 static int pyra_get_profile_buttons(struct usb_device *usb_dev,
 		struct pyra_profile_buttons *buf, int number)
 {
 	int retval;
-
 	retval = pyra_send_control(usb_dev, number,
 			PYRA_CONTROL_REQUEST_PROFILE_BUTTONS);
-
 	if (retval)
 		return retval;
-
-	retval = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			PYRA_USB_COMMAND_PROFILE_BUTTONS, 0, (char *)buf,
-			sizeof(struct pyra_profile_buttons),
-			USB_CTRL_SET_TIMEOUT);
-
-	if (retval != sizeof(struct pyra_profile_buttons))
-		return retval;
-
-	return 0;
+	return roccat_common_receive(usb_dev, PYRA_USB_COMMAND_PROFILE_BUTTONS,
+			buf, sizeof(struct pyra_profile_buttons));
 }
 
 static int pyra_get_settings(struct usb_device *usb_dev,
 		struct pyra_settings *buf)
 {
-	int len;
-	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			PYRA_USB_COMMAND_SETTINGS, 0, buf,
-			sizeof(struct pyra_settings), USB_CTRL_SET_TIMEOUT);
-	if (len != sizeof(struct pyra_settings))
-		return -EIO;
-	return 0;
+	return roccat_common_receive(usb_dev, PYRA_USB_COMMAND_SETTINGS,
+			buf, sizeof(struct pyra_settings));
 }
 
 static int pyra_get_info(struct usb_device *usb_dev, struct pyra_info *buf)
 {
-	int len;
-	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
-			USB_REQ_CLEAR_FEATURE,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			PYRA_USB_COMMAND_INFO, 0, buf,
-			sizeof(struct pyra_info), USB_CTRL_SET_TIMEOUT);
-	if (len != sizeof(struct pyra_info))
-		return -EIO;
-	return 0;
+	return roccat_common_receive(usb_dev, PYRA_USB_COMMAND_INFO,
+			buf, sizeof(struct pyra_info));
+}
+
+static int pyra_send(struct usb_device *usb_dev, uint command,
+		void const *buf, uint size)
+{
+	int retval;
+	retval = roccat_common_send(usb_dev, command, buf, size);
+	if (retval)
+		return retval;
+	return pyra_receive_control_status(usb_dev);
 }
 
 static int pyra_set_profile_settings(struct usb_device *usb_dev,
 		struct pyra_profile_settings const *settings)
 {
-	int len;
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			PYRA_USB_COMMAND_PROFILE_SETTINGS, 0, (char *)settings,
-			sizeof(struct pyra_profile_settings),
-			USB_CTRL_SET_TIMEOUT);
-	if (len != sizeof(struct pyra_profile_settings))
-		return -EIO;
-	if (pyra_receive_control_status(usb_dev))
-		return -EIO;
-	return 0;
+	return pyra_send(usb_dev, PYRA_USB_COMMAND_PROFILE_SETTINGS, settings,
+			sizeof(struct pyra_profile_settings));
 }
 
 static int pyra_set_profile_buttons(struct usb_device *usb_dev,
 		struct pyra_profile_buttons const *buttons)
 {
-	int len;
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			PYRA_USB_COMMAND_PROFILE_BUTTONS, 0, (char *)buttons,
-			sizeof(struct pyra_profile_buttons),
-			USB_CTRL_SET_TIMEOUT);
-	if (len != sizeof(struct pyra_profile_buttons))
-		return -EIO;
-	if (pyra_receive_control_status(usb_dev))
-		return -EIO;
-	return 0;
+	return pyra_send(usb_dev, PYRA_USB_COMMAND_PROFILE_BUTTONS, buttons,
+			sizeof(struct pyra_profile_buttons));
 }
 
 static int pyra_set_settings(struct usb_device *usb_dev,
 		struct pyra_settings const *settings)
 {
-	int len;
-	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
-			USB_REQ_SET_CONFIGURATION,
-			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			PYRA_USB_COMMAND_SETTINGS, 0, (char *)settings,
-			sizeof(struct pyra_settings), USB_CTRL_SET_TIMEOUT);
-	if (len != sizeof(struct pyra_settings))
-		return -EIO;
-	if (pyra_receive_control_status(usb_dev))
-		return -EIO;
-	return 0;
+	int retval;
+	retval = roccat_common_send(usb_dev, PYRA_USB_COMMAND_SETTINGS, settings,
+			sizeof(struct pyra_settings));
+	if (retval)
+		return retval;
+	return pyra_receive_control_status(usb_dev);
 }
 
 static ssize_t pyra_sysfs_read_profilex_settings(struct file *fp,
@@ -521,21 +451,16 @@ static struct bin_attribute pyra_bin_attributes[] = {
 static int pyra_init_pyra_device_struct(struct usb_device *usb_dev,
 		struct pyra_device *pyra)
 {
-	struct pyra_info *info;
+	struct pyra_info info;
 	int retval, i;
 
 	mutex_init(&pyra->pyra_lock);
 
-	info = kmalloc(sizeof(struct pyra_info), GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-	retval = pyra_get_info(usb_dev, info);
-	if (retval) {
-		kfree(info);
+	retval = pyra_get_info(usb_dev, &info);
+	if (retval)
 		return retval;
-	}
-	pyra->firmware_version = info->firmware_version;
-	kfree(info);
+
+	pyra->firmware_version = info.firmware_version;
 
 	retval = pyra_get_settings(usb_dev, &pyra->settings);
 	if (retval)
@@ -581,7 +506,8 @@ static int pyra_init_specials(struct hid_device *hdev)
 			goto exit_free;
 		}
 
-		retval = roccat_connect(pyra_class, hdev);
+		retval = roccat_connect(pyra_class, hdev,
+				sizeof(struct pyra_roccat_report));
 		if (retval < 0) {
 			hid_err(hdev, "couldn't init char dev\n");
 		} else {
@@ -685,8 +611,7 @@ static void pyra_report_to_chrdev(struct pyra_device const *pyra,
 		roccat_report.value = button_event->data1;
 		roccat_report.key = 0;
 		roccat_report_event(pyra->chrdev_minor,
-				(uint8_t const *)&roccat_report,
-				sizeof(struct pyra_roccat_report));
+				(uint8_t const *)&roccat_report);
 		break;
 	case PYRA_MOUSE_EVENT_BUTTON_TYPE_MACRO:
 	case PYRA_MOUSE_EVENT_BUTTON_TYPE_SHORTCUT:
@@ -700,8 +625,7 @@ static void pyra_report_to_chrdev(struct pyra_device const *pyra,
 			 */
 			roccat_report.value = pyra->actual_profile + 1;
 			roccat_report_event(pyra->chrdev_minor,
-					(uint8_t const *)&roccat_report,
-					sizeof(struct pyra_roccat_report));
+					(uint8_t const *)&roccat_report);
 		}
 		break;
 	}
@@ -761,8 +685,8 @@ static int __init pyra_init(void)
 
 static void __exit pyra_exit(void)
 {
-	class_destroy(pyra_class);
 	hid_unregister_driver(&pyra_driver);
+	class_destroy(pyra_class);
 }
 
 module_init(pyra_init);
diff --git a/drivers/hid/hid-roccat.c b/drivers/hid/hid-roccat.c
index a14c579ea781..5666e7587b18 100644
--- a/drivers/hid/hid-roccat.c
+++ b/drivers/hid/hid-roccat.c
@@ -26,8 +26,7 @@
 #include <linux/cdev.h>
 #include <linux/poll.h>
 #include <linux/sched.h>
-
-#include "hid-roccat.h"
+#include <linux/hid-roccat.h>
 
 #define ROCCAT_FIRST_MINOR 0
 #define ROCCAT_MAX_DEVICES 8
@@ -37,11 +36,11 @@
 
 struct roccat_report {
 	uint8_t *value;
-	int len;
 };
 
 struct roccat_device {
 	unsigned int minor;
+	int report_size;
 	int open;
 	int exist;
 	wait_queue_head_t wait;
@@ -123,7 +122,7 @@ static ssize_t roccat_read(struct file *file, char __user *buffer,
 	 * If report is larger than requested amount of data, rest of report
 	 * is lost!
 	 */
-	len = report->len > count ? count : report->len;
+	len = device->report_size > count ? count : device->report_size;
 
 	if (copy_to_user(buffer, report->value, len)) {
 		retval = -EFAULT;
@@ -248,26 +247,25 @@ static int roccat_release(struct inode *inode, struct file *file)
  *
  * This is called from interrupt handler.
  */
-int roccat_report_event(int minor, u8 const *data, int len)
+int roccat_report_event(int minor, u8 const *data)
 {
 	struct roccat_device *device;
 	struct roccat_reader *reader;
 	struct roccat_report *report;
 	uint8_t *new_value;
 
-	new_value = kmemdup(data, len, GFP_ATOMIC);
+	device = devices[minor];
+
+	new_value = kmemdup(data, device->report_size, GFP_ATOMIC);
 	if (!new_value)
 		return -ENOMEM;
 
-	device = devices[minor];
-
 	report = &device->cbuf[device->cbuf_end];
 
 	/* passing NULL is safe */
 	kfree(report->value);
 
 	report->value = new_value;
-	report->len = len;
 	device->cbuf_end = (device->cbuf_end + 1) % ROCCAT_CBUF_SIZE;
 
 	list_for_each_entry(reader, &device->readers, node) {
@@ -295,7 +293,7 @@ EXPORT_SYMBOL_GPL(roccat_report_event);
  * Return value is minor device number in Range [0, ROCCAT_MAX_DEVICES] on
  * success, a negative error code on failure.
  */
-int roccat_connect(struct class *klass, struct hid_device *hid)
+int roccat_connect(struct class *klass, struct hid_device *hid, int report_size)
 {
 	unsigned int minor;
 	struct roccat_device *device;
@@ -343,6 +341,7 @@ int roccat_connect(struct class *klass, struct hid_device *hid)
 	device->hid = hid;
 	device->exist = 1;
 	device->cbuf_end = 0;
+	device->report_size = report_size;
 
 	return minor;
 }
@@ -357,13 +356,16 @@ void roccat_disconnect(int minor)
 
 	mutex_lock(&devices_lock);
 	device = devices[minor];
-	devices[minor] = NULL;
 	mutex_unlock(&devices_lock);
 
 	device->exist = 0; /* TODO exist maybe not needed */
 
 	device_destroy(device->dev->class, MKDEV(roccat_major, minor));
 
+	mutex_lock(&devices_lock);
+	devices[minor] = NULL;
+	mutex_unlock(&devices_lock);
+	
 	if (device->open) {
 		hid_hw_close(device->hid);
 		wake_up_interruptible(&device->wait);
@@ -373,6 +375,34 @@ void roccat_disconnect(int minor)
 }
 EXPORT_SYMBOL_GPL(roccat_disconnect);
 
+static long roccat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct inode *inode = file->f_path.dentry->d_inode;
+	struct roccat_device *device;
+	unsigned int minor = iminor(inode);
+	long retval = 0;
+
+	mutex_lock(&devices_lock);
+
+	device = devices[minor];
+	if (!device) {
+		retval = -ENODEV;
+		goto out;
+	}
+
+	switch (cmd) {
+	case ROCCATIOCGREPSIZE:
+		if (put_user(device->report_size, (int __user *)arg))
+			retval = -EFAULT;
+		break;
+	default:
+		retval = -ENOTTY;
+	}
+out:
+	mutex_unlock(&devices_lock);
+	return retval;
+}
+
 static const struct file_operations roccat_ops = {
 	.owner = THIS_MODULE,
 	.read = roccat_read,
@@ -380,6 +410,7 @@ static const struct file_operations roccat_ops = {
 	.open = roccat_open,
 	.release = roccat_release,
 	.llseek = noop_llseek,
+	.unlocked_ioctl = roccat_ioctl,
 };
 
 static int __init roccat_init(void)
diff --git a/drivers/hid/hid-roccat.h b/drivers/hid/hid-roccat.h
deleted file mode 100644
index 5784281d613f..000000000000
--- a/drivers/hid/hid-roccat.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef __HID_ROCCAT_H
-#define __HID_ROCCAT_H
-
-/*
- * Copyright (c) 2010 Stefan Achatz <erazor_de@users.sourceforge.net>
- */
-
-/*
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the Free
- * Software Foundation; either version 2 of the License, or (at your option)
- * any later version.
- */
-
-#include <linux/hid.h>
-#include <linux/types.h>
-
-#if defined(CONFIG_HID_ROCCAT) || defined(CONFIG_HID_ROCCAT_MODULE)
-int roccat_connect(struct class *klass, struct hid_device *hid);
-void roccat_disconnect(int minor);
-int roccat_report_event(int minor, u8 const *data, int len);
-#else
-static inline int roccat_connect(struct class *klass,
-		struct hid_device *hid) { return -1; }
-static inline void roccat_disconnect(int minor) {}
-static inline int roccat_report_event(int minor, u8 const *data, int len)
-{
-	return 0;
-}
-#endif
-
-#endif
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 68d7b36e31e4..93819a08121a 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -46,6 +46,16 @@ static __u8 *sony_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 	return rdesc;
 }
 
+/*
+ * The Sony Sixaxis does not handle HID Output Reports on the Interrupt EP
+ * like it should according to usbhid/hid-core.c::usbhid_output_raw_report()
+ * so we need to override that forcing HID Output Reports on the Control EP.
+ *
+ * There is also another issue about HID Output Reports via USB, the Sixaxis
+ * does not want the report_id as part of the data packet, so we have to
+ * discard buf[0] when sending the actual control message, even for numbered
+ * reports, humpf!
+ */
 static int sixaxis_usb_output_raw_report(struct hid_device *hid, __u8 *buf,
 		size_t count, unsigned char report_type)
 {
@@ -55,6 +65,12 @@ static int sixaxis_usb_output_raw_report(struct hid_device *hid, __u8 *buf,
 	int report_id = buf[0];
 	int ret;
 
+	if (report_type == HID_OUTPUT_REPORT) {
+		/* Don't send the Report ID */
+		buf++;
+		count--;
+	}
+
 	ret = usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
 		HID_REQ_SET_REPORT,
 		USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
@@ -62,6 +78,10 @@ static int sixaxis_usb_output_raw_report(struct hid_device *hid, __u8 *buf,
 		interface->desc.bInterfaceNumber, buf, count,
 		USB_CTRL_SET_TIMEOUT);
 
+	/* Count also the Report ID, in case of an Output report. */
+	if (ret > 0 && report_type == HID_OUTPUT_REPORT)
+		ret++;
+
 	return ret;
 }
 
diff --git a/drivers/hid/hidraw.c b/drivers/hid/hidraw.c
index 468e87b53ed2..54409cba018c 100644
--- a/drivers/hid/hidraw.c
+++ b/drivers/hid/hidraw.c
@@ -91,7 +91,7 @@ static ssize_t hidraw_read(struct file *file, char __user *buffer, size_t count,
 			ret = -EFAULT;
 			goto out;
 		}
-		ret += len;
+		ret = len;
 
 		kfree(list->buffer[list->tail].value);
 		list->tail = (list->tail + 1) & (HIDRAW_BUFFER_SIZE - 1);
@@ -102,15 +102,14 @@ out:
 }
 
 /* the first byte is expected to be a report number */
-static ssize_t hidraw_write(struct file *file, const char __user *buffer, size_t count, loff_t *ppos)
+/* This function is to be called with the minors_lock mutex held */
+static ssize_t hidraw_send_report(struct file *file, const char __user *buffer, size_t count, unsigned char report_type)
 {
 	unsigned int minor = iminor(file->f_path.dentry->d_inode);
 	struct hid_device *dev;
 	__u8 *buf;
 	int ret = 0;
 
-	mutex_lock(&minors_lock);
-
 	if (!hidraw_table[minor]) {
 		ret = -ENODEV;
 		goto out;
@@ -148,14 +147,92 @@ static ssize_t hidraw_write(struct file *file, const char __user *buffer, size_t
 		goto out_free;
 	}
 
-	ret = dev->hid_output_raw_report(dev, buf, count, HID_OUTPUT_REPORT);
+	ret = dev->hid_output_raw_report(dev, buf, count, report_type);
 out_free:
 	kfree(buf);
 out:
+	return ret;
+}
+
+/* the first byte is expected to be a report number */
+static ssize_t hidraw_write(struct file *file, const char __user *buffer, size_t count, loff_t *ppos)
+{
+	ssize_t ret;
+	mutex_lock(&minors_lock);
+	ret = hidraw_send_report(file, buffer, count, HID_OUTPUT_REPORT);
 	mutex_unlock(&minors_lock);
 	return ret;
 }
 
+
+/* This function performs a Get_Report transfer over the control endpoint
+   per section 7.2.1 of the HID specification, version 1.1.  The first byte
+   of buffer is the report number to request, or 0x0 if the defice does not
+   use numbered reports. The report_type parameter can be HID_FEATURE_REPORT
+   or HID_INPUT_REPORT.  This function is to be called with the minors_lock
+   mutex held.  */
+static ssize_t hidraw_get_report(struct file *file, char __user *buffer, size_t count, unsigned char report_type)
+{
+	unsigned int minor = iminor(file->f_path.dentry->d_inode);
+	struct hid_device *dev;
+	__u8 *buf;
+	int ret = 0, len;
+	unsigned char report_number;
+
+	dev = hidraw_table[minor]->hid;
+
+	if (!dev->hid_get_raw_report) {
+		ret = -ENODEV;
+		goto out;
+	}
+
+	if (count > HID_MAX_BUFFER_SIZE) {
+		printk(KERN_WARNING "hidraw: pid %d passed too large report\n",
+				task_pid_nr(current));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (count < 2) {
+		printk(KERN_WARNING "hidraw: pid %d passed too short report\n",
+				task_pid_nr(current));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	buf = kmalloc(count * sizeof(__u8), GFP_KERNEL);
+	if (!buf) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Read the first byte from the user. This is the report number,
+	   which is passed to dev->hid_get_raw_report(). */
+	if (copy_from_user(&report_number, buffer, 1)) {
+		ret = -EFAULT;
+		goto out_free;
+	}
+
+	ret = dev->hid_get_raw_report(dev, report_number, buf, count, report_type);
+
+	if (ret < 0)
+		goto out_free;
+
+	len = (ret < count) ? ret : count;
+
+	if (copy_to_user(buffer, buf, len)) {
+		ret = -EFAULT;
+		goto out_free;
+	}
+
+	ret = len;
+
+out_free:
+	kfree(buf);
+out:
+	return ret;
+}
+
 static unsigned int hidraw_poll(struct file *file, poll_table *wait)
 {
 	struct hidraw_list *list = file->private_data;
@@ -295,7 +372,24 @@ static long hidraw_ioctl(struct file *file, unsigned int cmd,
 		default:
 			{
 				struct hid_device *hid = dev->hid;
-				if (_IOC_TYPE(cmd) != 'H' || _IOC_DIR(cmd) != _IOC_READ) {
+				if (_IOC_TYPE(cmd) != 'H') {
+					ret = -EINVAL;
+					break;
+				}
+
+				if (_IOC_NR(cmd) == _IOC_NR(HIDIOCSFEATURE(0))) {
+					int len = _IOC_SIZE(cmd);
+					ret = hidraw_send_report(file, user_arg, len, HID_FEATURE_REPORT);
+					break;
+				}
+				if (_IOC_NR(cmd) == _IOC_NR(HIDIOCGFEATURE(0))) {
+					int len = _IOC_SIZE(cmd);
+					ret = hidraw_get_report(file, user_arg, len, HID_FEATURE_REPORT);
+					break;
+				}
+
+				/* Begin Read-only ioctls. */
+				if (_IOC_DIR(cmd) != _IOC_READ) {
 					ret = -EINVAL;
 					break;
 				}
@@ -327,7 +421,7 @@ static long hidraw_ioctl(struct file *file, unsigned int cmd,
 						-EFAULT : len;
 					break;
 				}
-		}
+			}
 
 		ret = -ENOTTY;
 	}
@@ -428,12 +522,12 @@ void hidraw_disconnect(struct hid_device *hid)
 
 	hidraw->exist = 0;
 
+	device_destroy(hidraw_class, MKDEV(hidraw_major, hidraw->minor));
+
 	mutex_lock(&minors_lock);
 	hidraw_table[hidraw->minor] = NULL;
 	mutex_unlock(&minors_lock);
 
-	device_destroy(hidraw_class, MKDEV(hidraw_major, hidraw->minor));
-
 	if (hidraw->open) {
 		hid_hw_close(hid);
 		wake_up_interruptible(&hidraw->wait);
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index b336dd84036f..38c261a40c74 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -799,6 +799,40 @@ static int hid_alloc_buffers(struct usb_device *dev, struct hid_device *hid)
 	return 0;
 }
 
+static int usbhid_get_raw_report(struct hid_device *hid,
+		unsigned char report_number, __u8 *buf, size_t count,
+		unsigned char report_type)
+{
+	struct usbhid_device *usbhid = hid->driver_data;
+	struct usb_device *dev = hid_to_usb_dev(hid);
+	struct usb_interface *intf = usbhid->intf;
+	struct usb_host_interface *interface = intf->cur_altsetting;
+	int skipped_report_id = 0;
+	int ret;
+
+	/* Byte 0 is the report number. Report data starts at byte 1.*/
+	buf[0] = report_number;
+	if (report_number == 0x0) {
+		/* Offset the return buffer by 1, so that the report ID
+		   will remain in byte 0. */
+		buf++;
+		count--;
+		skipped_report_id = 1;
+	}
+	ret = usb_control_msg(dev, usb_rcvctrlpipe(dev, 0),
+		HID_REQ_GET_REPORT,
+		USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+		((report_type + 1) << 8) | report_number,
+		interface->desc.bInterfaceNumber, buf, count,
+		USB_CTRL_SET_TIMEOUT);
+
+	/* count also the report id */
+	if (ret > 0 && skipped_report_id)
+		ret++;
+
+	return ret;
+}
+
 static int usbhid_output_raw_report(struct hid_device *hid, __u8 *buf, size_t count,
 		unsigned char report_type)
 {
@@ -1139,6 +1173,7 @@ static int usbhid_probe(struct usb_interface *intf, const struct usb_device_id *
 
 	usb_set_intfdata(intf, hid);
 	hid->ll_driver = &usb_hid_driver;
+	hid->hid_get_raw_report = usbhid_get_raw_report;
 	hid->hid_output_raw_report = usbhid_output_raw_report;
 	hid->ff_init = hid_pidff_init;
 #ifdef CONFIG_USB_HIDDEV
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 297bc9a7d6e6..898059086996 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -467,6 +467,17 @@ config SENSORS_JC42
 	  This driver can also be built as a module.  If so, the module
 	  will be called jc42.
 
+config SENSORS_LINEAGE
+	tristate "Lineage Compact Power Line Power Entry Module"
+	depends on I2C && EXPERIMENTAL
+	help
+	  If you say yes here you get support for the Lineage Compact Power Line
+	  series of DC/DC and AC/DC converters such as CP1800, CP2000AC,
+	  CP2000DC, CP2725, and others.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called lineage-pem.
+
 config SENSORS_LM63
 	tristate "National Semiconductor LM63 and LM64"
 	depends on I2C
@@ -510,7 +521,7 @@ config SENSORS_LM75
 		- Dallas Semiconductor DS75 and DS1775
 		- Maxim MAX6625 and MAX6626
 		- Microchip MCP980x
-		- National Semiconductor LM75
+		- National Semiconductor LM75, LM75A
 		- NXP's LM75A
 		- ST Microelectronics STDS75
 		- TelCom (now Microchip) TCN75
@@ -625,6 +636,17 @@ config SENSORS_LM93
 	  This driver can also be built as a module.  If so, the module
 	  will be called lm93.
 
+config SENSORS_LTC4151
+	tristate "Linear Technology LTC4151"
+	depends on I2C
+	default n
+	help
+	  If you say yes here you get support for Linear Technology LTC4151
+	  High Voltage I2C Current and Voltage Monitor interface.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called ltc4151.
+
 config SENSORS_LTC4215
 	tristate "Linear Technology LTC4215"
 	depends on I2C && EXPERIMENTAL
@@ -685,6 +707,16 @@ config SENSORS_MAX1619
 	  This driver can also be built as a module.  If so, the module
 	  will be called max1619.
 
+config SENSORS_MAX6639
+	tristate "Maxim MAX6639 sensor chip"
+	depends on I2C && EXPERIMENTAL
+	help
+	  If you say yes here you get support for the MAX6639
+	  sensor chips.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called max6639.
+
 config SENSORS_MAX6650
 	tristate "Maxim MAX6650 sensor chip"
 	depends on I2C && EXPERIMENTAL
@@ -735,6 +767,61 @@ config SENSORS_PCF8591
 	  These devices are hard to detect and rarely found on mainstream
 	  hardware.  If unsure, say N.
 
+config PMBUS
+	tristate "PMBus support"
+	depends on I2C && EXPERIMENTAL
+	default n
+	help
+	  Say yes here if you want to enable PMBus support.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called pmbus_core.
+
+if PMBUS
+
+config SENSORS_PMBUS
+	tristate "Generic PMBus devices"
+	default n
+	help
+	  If you say yes here you get hardware monitoring support for generic
+	  PMBus devices, including but not limited to BMR450, BMR451, BMR453,
+	  BMR454, and LTC2978.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called pmbus.
+
+config SENSORS_MAX16064
+	tristate "Maxim MAX16064"
+	default n
+	help
+	  If you say yes here you get hardware monitoring support for Maxim
+	  MAX16064.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called max16064.
+
+config SENSORS_MAX34440
+	tristate "Maxim MAX34440/MAX34441"
+	default n
+	help
+	  If you say yes here you get hardware monitoring support for Maxim
+	  MAX34440 and MAX34441.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called max34440.
+
+config SENSORS_MAX8688
+	tristate "Maxim MAX8688"
+	default n
+	help
+	  If you say yes here you get hardware monitoring support for Maxim
+	  MAX8688.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called max8688.
+
+endif # PMBUS
+
 config SENSORS_SHT15
 	tristate "Sensiron humidity and temperature sensors. SHT15 and compat."
 	depends on GENERIC_GPIO
@@ -872,6 +959,16 @@ config SENSORS_SMSC47B397
 	  This driver can also be built as a module.  If so, the module
 	  will be called smsc47b397.
 
+config SENSORS_ADS1015
+	tristate "Texas Instruments ADS1015"
+	depends on I2C
+	help
+	  If you say yes here you get support for Texas Instruments ADS1015
+	  12-bit 4-input ADC device.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called ads1015.
+
 config SENSORS_ADS7828
 	tristate "Texas Instruments ADS7828"
 	depends on I2C
@@ -941,6 +1038,16 @@ config SENSORS_TMP421
 	  This driver can also be built as a module.  If so, the module
 	  will be called tmp421.
 
+config SENSORS_TWL4030_MADC
+	tristate "Texas Instruments TWL4030 MADC Hwmon"
+	depends on TWL4030_MADC
+	help
+	If you say yes here you get hwmon support for triton
+	TWL4030-MADC.
+
+	This driver can also be built as a module. If so it will be called
+	twl4030-madc-hwmon.
+
 config SENSORS_VIA_CPUTEMP
 	tristate "VIA CPU temperature sensor"
 	depends on X86
@@ -1083,7 +1190,7 @@ config SENSORS_W83627HF
 	  will be called w83627hf.
 
 config SENSORS_W83627EHF
-	tristate "Winbond W83627EHF/EHG/DHG, W83667HG"
+	tristate "Winbond W83627EHF/EHG/DHG, W83667HG, NCT6775F, NCT6776F"
 	select HWMON_VID
 	help
 	  If you say yes here you get support for the hardware
@@ -1094,7 +1201,8 @@ config SENSORS_W83627EHF
 	  chip suited for specific Intel processors that use PECI such as
 	  the Core 2 Duo.
 
-	  This driver also supports the W83667HG chip.
+	  This driver also supports Nuvoton W83667HG, W83667HG-B, NCT6775F
+	  (also known as W83667HG-I), and NCT6776F.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called w83627ehf.
@@ -1127,40 +1235,6 @@ config SENSORS_ULTRA45
 	  This driver provides support for the Ultra45 workstation environmental
 	  sensors.
 
-config SENSORS_LIS3_SPI
-	tristate "STMicroeletronics LIS3LV02Dx three-axis digital accelerometer (SPI)"
-	depends on !ACPI && SPI_MASTER && INPUT
-	select INPUT_POLLDEV
-	default n
-	help
-	  This driver provides support for the LIS3LV02Dx accelerometer connected
-	  via SPI. The accelerometer data is readable via
-	  /sys/devices/platform/lis3lv02d.
-
-	  This driver also provides an absolute input class device, allowing
-	  the laptop to act as a pinball machine-esque joystick.
-
-	  This driver can also be built as modules.  If so, the core module
-	  will be called lis3lv02d and a specific module for the SPI transport
-	  is called lis3lv02d_spi.
-
-config SENSORS_LIS3_I2C
-	tristate "STMicroeletronics LIS3LV02Dx three-axis digital accelerometer (I2C)"
-	depends on I2C && INPUT
-	select INPUT_POLLDEV
-	default n
-	help
-	  This driver provides support for the LIS3LV02Dx accelerometer connected
-	  via I2C. The accelerometer data is readable via
-	  /sys/devices/platform/lis3lv02d.
-
-	  This driver also provides an absolute input class device, allowing
-	  the device to act as a pinball machine-esque joystick.
-
-	  This driver can also be built as modules.  If so, the core module
-	  will be called lis3lv02d and a specific module for the I2C transport
-	  is called lis3lv02d_i2c.
-
 config SENSORS_APPLESMC
 	tristate "Apple SMC (Motion sensor, light sensor, keyboard backlight)"
 	depends on INPUT && X86
@@ -1208,36 +1282,6 @@ config SENSORS_ATK0110
 	  This driver can also be built as a module. If so, the module
 	  will be called asus_atk0110.
 
-config SENSORS_LIS3LV02D
-	tristate "STMicroeletronics LIS3* three-axis digital accelerometer"
-	depends on INPUT
-	select INPUT_POLLDEV
-	select NEW_LEDS
-	select LEDS_CLASS
-	default n
-	help
-	  This driver provides support for the LIS3* accelerometers, such as the
-	  LIS3LV02DL or the LIS331DL. In particular, it can be found in a number
-	  of HP laptops, which have the "Mobile Data Protection System 3D" or
-	  "3D DriveGuard" feature. On such systems the driver should load
-	  automatically (via ACPI alias). The accelerometer might also be found
-	  in other systems, connected via SPI or I2C. The accelerometer data is
-	  readable via /sys/devices/platform/lis3lv02d.
-
-	  This driver also provides an absolute input class device, allowing
-	  a laptop to act as a pinball machine-esque joystick. It provides also
-	  a misc device which can be used to detect free-fall. On HP laptops,
-	  if the led infrastructure is activated, support for a led indicating
-	  disk protection will be provided as hp::hddprotect. For more
-	  information on the feature, refer to Documentation/hwmon/lis3lv02d.
-
-	  This driver can also be built as modules.  If so, the core module
-	  will be called lis3lv02d and a specific module for HP laptops will be
-	  called hp_accel.
-
-	  Say Y here if you have an applicable laptop and want to experience
-	  the awesome power of lis3lv02d.
-
 endif # ACPI
 
 endif # HWMON
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index dde02d99c238..01faebd87585 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_SENSORS_ADM1026)	+= adm1026.o
 obj-$(CONFIG_SENSORS_ADM1029)	+= adm1029.o
 obj-$(CONFIG_SENSORS_ADM1031)	+= adm1031.o
 obj-$(CONFIG_SENSORS_ADM9240)	+= adm9240.o
+obj-$(CONFIG_SENSORS_ADS1015)	+= ads1015.o
 obj-$(CONFIG_SENSORS_ADS7828)	+= ads7828.o
 obj-$(CONFIG_SENSORS_ADS7871)	+= ads7871.o
 obj-$(CONFIG_SENSORS_ADT7411)	+= adt7411.o
@@ -62,9 +63,7 @@ obj-$(CONFIG_SENSORS_JC42)	+= jc42.o
 obj-$(CONFIG_SENSORS_JZ4740)	+= jz4740-hwmon.o
 obj-$(CONFIG_SENSORS_K8TEMP)	+= k8temp.o
 obj-$(CONFIG_SENSORS_K10TEMP)	+= k10temp.o
-obj-$(CONFIG_SENSORS_LIS3LV02D) += lis3lv02d.o hp_accel.o
-obj-$(CONFIG_SENSORS_LIS3_SPI)	+= lis3lv02d.o lis3lv02d_spi.o
-obj-$(CONFIG_SENSORS_LIS3_I2C)	+= lis3lv02d.o lis3lv02d_i2c.o
+obj-$(CONFIG_SENSORS_LINEAGE)	+= lineage-pem.o
 obj-$(CONFIG_SENSORS_LM63)	+= lm63.o
 obj-$(CONFIG_SENSORS_LM70)	+= lm70.o
 obj-$(CONFIG_SENSORS_LM73)	+= lm73.o
@@ -79,11 +78,13 @@ obj-$(CONFIG_SENSORS_LM90)	+= lm90.o
 obj-$(CONFIG_SENSORS_LM92)	+= lm92.o
 obj-$(CONFIG_SENSORS_LM93)	+= lm93.o
 obj-$(CONFIG_SENSORS_LM95241)	+= lm95241.o
+obj-$(CONFIG_SENSORS_LTC4151)	+= ltc4151.o
 obj-$(CONFIG_SENSORS_LTC4215)	+= ltc4215.o
 obj-$(CONFIG_SENSORS_LTC4245)	+= ltc4245.o
 obj-$(CONFIG_SENSORS_LTC4261)	+= ltc4261.o
 obj-$(CONFIG_SENSORS_MAX1111)	+= max1111.o
 obj-$(CONFIG_SENSORS_MAX1619)	+= max1619.o
+obj-$(CONFIG_SENSORS_MAX6639)	+= max6639.o
 obj-$(CONFIG_SENSORS_MAX6650)	+= max6650.o
 obj-$(CONFIG_SENSORS_MC13783_ADC)+= mc13783-adc.o
 obj-$(CONFIG_SENSORS_PC87360)	+= pc87360.o
@@ -102,6 +103,7 @@ obj-$(CONFIG_SENSORS_THMC50)	+= thmc50.o
 obj-$(CONFIG_SENSORS_TMP102)	+= tmp102.o
 obj-$(CONFIG_SENSORS_TMP401)	+= tmp401.o
 obj-$(CONFIG_SENSORS_TMP421)	+= tmp421.o
+obj-$(CONFIG_SENSORS_TWL4030_MADC)+= twl4030-madc-hwmon.o
 obj-$(CONFIG_SENSORS_VIA_CPUTEMP)+= via-cputemp.o
 obj-$(CONFIG_SENSORS_VIA686A)	+= via686a.o
 obj-$(CONFIG_SENSORS_VT1211)	+= vt1211.o
@@ -112,6 +114,13 @@ obj-$(CONFIG_SENSORS_W83L786NG)	+= w83l786ng.o
 obj-$(CONFIG_SENSORS_WM831X)	+= wm831x-hwmon.o
 obj-$(CONFIG_SENSORS_WM8350)	+= wm8350-hwmon.o
 
+# PMBus drivers
+obj-$(CONFIG_PMBUS)		+= pmbus_core.o
+obj-$(CONFIG_SENSORS_PMBUS)	+= pmbus.o
+obj-$(CONFIG_SENSORS_MAX16064)	+= max16064.o
+obj-$(CONFIG_SENSORS_MAX34440)	+= max34440.o
+obj-$(CONFIG_SENSORS_MAX8688)	+= max8688.o
+
 ifeq ($(CONFIG_HWMON_DEBUG_CHIP),y)
 EXTRA_CFLAGS += -DDEBUG
 endif
diff --git a/drivers/hwmon/ad7414.c b/drivers/hwmon/ad7414.c
index 86d822aa9bbf..d46c0c758ddf 100644
--- a/drivers/hwmon/ad7414.c
+++ b/drivers/hwmon/ad7414.c
@@ -242,6 +242,7 @@ static const struct i2c_device_id ad7414_id[] = {
 	{ "ad7414", 0 },
 	{}
 };
+MODULE_DEVICE_TABLE(i2c, ad7414_id);
 
 static struct i2c_driver ad7414_driver = {
 	.driver = {
diff --git a/drivers/hwmon/ads1015.c b/drivers/hwmon/ads1015.c
new file mode 100644
index 000000000000..8ef2b608fc66
--- /dev/null
+++ b/drivers/hwmon/ads1015.c
@@ -0,0 +1,268 @@
+/*
+ * ads1015.c - lm_sensors driver for ads1015 12-bit 4-input ADC
+ * (C) Copyright 2010
+ * Dirk Eibach, Guntermann & Drunck GmbH <eibach@gdsys.de>
+ *
+ * Based on the ads7828 driver by Steve Hardy.
+ *
+ * Datasheet available at: http://focus.ti.com/lit/ds/symlink/ads1015.pdf
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+
+#include <linux/i2c/ads1015.h>
+
+/* ADS1015 registers */
+enum {
+	ADS1015_CONVERSION = 0,
+	ADS1015_CONFIG = 1,
+};
+
+/* PGA fullscale voltages in mV */
+static const unsigned int fullscale_table[8] = {
+	6144, 4096, 2048, 1024, 512, 256, 256, 256 };
+
+#define ADS1015_CONFIG_CHANNELS 8
+#define ADS1015_DEFAULT_CHANNELS 0xff
+
+struct ads1015_data {
+	struct device *hwmon_dev;
+	struct mutex update_lock; /* mutex protect updates */
+};
+
+static s32 ads1015_read_reg(struct i2c_client *client, unsigned int reg)
+{
+	s32 data = i2c_smbus_read_word_data(client, reg);
+
+	return (data < 0) ? data : swab16(data);
+}
+
+static s32 ads1015_write_reg(struct i2c_client *client, unsigned int reg,
+			     u16 val)
+{
+	return i2c_smbus_write_word_data(client, reg, swab16(val));
+}
+
+static int ads1015_read_value(struct i2c_client *client, unsigned int channel,
+			      int *value)
+{
+	u16 config;
+	s16 conversion;
+	unsigned int pga;
+	int fullscale;
+	unsigned int k;
+	struct ads1015_data *data = i2c_get_clientdata(client);
+	int res;
+
+	mutex_lock(&data->update_lock);
+
+	/* get fullscale voltage */
+	res = ads1015_read_reg(client, ADS1015_CONFIG);
+	if (res < 0)
+		goto err_unlock;
+	config = res;
+	pga = (config >> 9) & 0x0007;
+	fullscale = fullscale_table[pga];
+
+	/* set channel and start single conversion */
+	config &= ~(0x0007 << 12);
+	config |= (1 << 15) | (1 << 8) | (channel & 0x0007) << 12;
+
+	/* wait until conversion finished */
+	res = ads1015_write_reg(client, ADS1015_CONFIG, config);
+	if (res < 0)
+		goto err_unlock;
+	for (k = 0; k < 5; ++k) {
+		schedule_timeout(msecs_to_jiffies(1));
+		res = ads1015_read_reg(client, ADS1015_CONFIG);
+		if (res < 0)
+			goto err_unlock;
+		config = res;
+		if (config & (1 << 15))
+			break;
+	}
+	if (k == 5) {
+		res = -EIO;
+		goto err_unlock;
+	}
+
+	res = ads1015_read_reg(client, ADS1015_CONVERSION);
+	if (res < 0)
+		goto err_unlock;
+	conversion = res;
+
+	mutex_unlock(&data->update_lock);
+
+	*value = DIV_ROUND_CLOSEST(conversion * fullscale, 0x7ff0);
+
+	return 0;
+
+err_unlock:
+	mutex_unlock(&data->update_lock);
+	return res;
+}
+
+/* sysfs callback function */
+static ssize_t show_in(struct device *dev, struct device_attribute *da,
+	char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct i2c_client *client = to_i2c_client(dev);
+	int in;
+	int res;
+
+	res = ads1015_read_value(client, attr->index, &in);
+
+	return (res < 0) ? res : sprintf(buf, "%d\n", in);
+}
+
+static const struct sensor_device_attribute ads1015_in[] = {
+	SENSOR_ATTR(in0_input, S_IRUGO, show_in, NULL, 0),
+	SENSOR_ATTR(in1_input, S_IRUGO, show_in, NULL, 1),
+	SENSOR_ATTR(in2_input, S_IRUGO, show_in, NULL, 2),
+	SENSOR_ATTR(in3_input, S_IRUGO, show_in, NULL, 3),
+	SENSOR_ATTR(in4_input, S_IRUGO, show_in, NULL, 4),
+	SENSOR_ATTR(in5_input, S_IRUGO, show_in, NULL, 5),
+	SENSOR_ATTR(in6_input, S_IRUGO, show_in, NULL, 6),
+	SENSOR_ATTR(in7_input, S_IRUGO, show_in, NULL, 7),
+};
+
+/*
+ * Driver interface
+ */
+
+static int ads1015_remove(struct i2c_client *client)
+{
+	struct ads1015_data *data = i2c_get_clientdata(client);
+	int k;
+
+	hwmon_device_unregister(data->hwmon_dev);
+	for (k = 0; k < ADS1015_CONFIG_CHANNELS; ++k)
+		device_remove_file(&client->dev, &ads1015_in[k].dev_attr);
+	kfree(data);
+	return 0;
+}
+
+static unsigned int ads1015_get_exported_channels(struct i2c_client *client)
+{
+	struct ads1015_platform_data *pdata = dev_get_platdata(&client->dev);
+#ifdef CONFIG_OF
+	struct device_node *np = client->dev.of_node;
+	const __be32 *of_channels;
+	int of_channels_size;
+#endif
+
+	/* prefer platform data */
+	if (pdata)
+		return pdata->exported_channels;
+
+#ifdef CONFIG_OF
+	/* fallback on OF */
+	of_channels = of_get_property(np, "exported-channels",
+				      &of_channels_size);
+	if (of_channels && (of_channels_size == sizeof(*of_channels)))
+		return be32_to_cpup(of_channels);
+#endif
+
+	/* fallback on default configuration */
+	return ADS1015_DEFAULT_CHANNELS;
+}
+
+static int ads1015_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct ads1015_data *data;
+	int err;
+	unsigned int exported_channels;
+	unsigned int k;
+
+	data = kzalloc(sizeof(struct ads1015_data), GFP_KERNEL);
+	if (!data) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/* build sysfs attribute group */
+	exported_channels = ads1015_get_exported_channels(client);
+	for (k = 0; k < ADS1015_CONFIG_CHANNELS; ++k) {
+		if (!(exported_channels & (1<<k)))
+			continue;
+		err = device_create_file(&client->dev, &ads1015_in[k].dev_attr);
+		if (err)
+			goto exit_free;
+	}
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove;
+	}
+
+	return 0;
+
+exit_remove:
+	for (k = 0; k < ADS1015_CONFIG_CHANNELS; ++k)
+		device_remove_file(&client->dev, &ads1015_in[k].dev_attr);
+exit_free:
+	kfree(data);
+exit:
+	return err;
+}
+
+static const struct i2c_device_id ads1015_id[] = {
+	{ "ads1015", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ads1015_id);
+
+static struct i2c_driver ads1015_driver = {
+	.driver = {
+		.name = "ads1015",
+	},
+	.probe = ads1015_probe,
+	.remove = ads1015_remove,
+	.id_table = ads1015_id,
+};
+
+static int __init sensors_ads1015_init(void)
+{
+	return i2c_add_driver(&ads1015_driver);
+}
+
+static void __exit sensors_ads1015_exit(void)
+{
+	i2c_del_driver(&ads1015_driver);
+}
+
+MODULE_AUTHOR("Dirk Eibach <eibach@gdsys.de>");
+MODULE_DESCRIPTION("ADS1015 driver");
+MODULE_LICENSE("GPL");
+
+module_init(sensors_ads1015_init);
+module_exit(sensors_ads1015_exit);
diff --git a/drivers/hwmon/adt7411.c b/drivers/hwmon/adt7411.c
index f13c843a2964..5cc3e3784b42 100644
--- a/drivers/hwmon/adt7411.c
+++ b/drivers/hwmon/adt7411.c
@@ -334,6 +334,7 @@ static const struct i2c_device_id adt7411_id[] = {
 	{ "adt7411", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, adt7411_id);
 
 static struct i2c_driver adt7411_driver = {
 	.driver		= {
diff --git a/drivers/hwmon/jz4740-hwmon.c b/drivers/hwmon/jz4740-hwmon.c
index 1c8b3d9e2051..fea292d43407 100644
--- a/drivers/hwmon/jz4740-hwmon.c
+++ b/drivers/hwmon/jz4740-hwmon.c
@@ -32,7 +32,7 @@ struct jz4740_hwmon {
 
 	int irq;
 
-	struct mfd_cell *cell;
+	const struct mfd_cell *cell;
 	struct device *hwmon;
 
 	struct completion read_completion;
@@ -112,7 +112,7 @@ static int __devinit jz4740_hwmon_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	hwmon->cell = pdev->dev.platform_data;
+	hwmon->cell = mfd_get_cell(pdev);
 
 	hwmon->irq = platform_get_irq(pdev, 0);
 	if (hwmon->irq < 0) {
diff --git a/drivers/hwmon/lineage-pem.c b/drivers/hwmon/lineage-pem.c
new file mode 100644
index 000000000000..d39ee24e52f6
--- /dev/null
+++ b/drivers/hwmon/lineage-pem.c
@@ -0,0 +1,589 @@
+/*
+ * Driver for Lineage Compact Power Line series of power entry modules.
+ *
+ * Copyright (C) 2010, 2011 Ericsson AB.
+ *
+ * Documentation:
+ *  http://www.lineagepower.com/oem/pdf/CPLI2C.pdf
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+
+/*
+ * This driver supports various Lineage Compact Power Line DC/DC and AC/DC
+ * converters such as CP1800, CP2000AC, CP2000DC, CP2100DC, and others.
+ *
+ * The devices are nominally PMBus compliant. However, most standard PMBus
+ * commands are not supported. Specifically, all hardware monitoring and
+ * status reporting commands are non-standard. For this reason, a standard
+ * PMBus driver can not be used.
+ *
+ * All Lineage CPL devices have a built-in I2C bus master selector (PCA9541).
+ * To ensure device access, this driver should only be used as client driver
+ * to the pca9541 I2C master selector driver.
+ */
+
+/* Command codes */
+#define PEM_OPERATION		0x01
+#define PEM_CLEAR_INFO_FLAGS	0x03
+#define PEM_VOUT_COMMAND	0x21
+#define PEM_VOUT_OV_FAULT_LIMIT	0x40
+#define PEM_READ_DATA_STRING	0xd0
+#define PEM_READ_INPUT_STRING	0xdc
+#define PEM_READ_FIRMWARE_REV	0xdd
+#define PEM_READ_RUN_TIMER	0xde
+#define PEM_FAN_HI_SPEED	0xdf
+#define PEM_FAN_NORMAL_SPEED	0xe0
+#define PEM_READ_FAN_SPEED	0xe1
+
+/* offsets in data string */
+#define PEM_DATA_STATUS_2	0
+#define PEM_DATA_STATUS_1	1
+#define PEM_DATA_ALARM_2	2
+#define PEM_DATA_ALARM_1	3
+#define PEM_DATA_VOUT_LSB	4
+#define PEM_DATA_VOUT_MSB	5
+#define PEM_DATA_CURRENT	6
+#define PEM_DATA_TEMP		7
+
+/* Virtual entries, to report constants */
+#define PEM_DATA_TEMP_MAX	10
+#define PEM_DATA_TEMP_CRIT	11
+
+/* offsets in input string */
+#define PEM_INPUT_VOLTAGE	0
+#define PEM_INPUT_POWER_LSB	1
+#define PEM_INPUT_POWER_MSB	2
+
+/* offsets in fan data */
+#define PEM_FAN_ADJUSTMENT	0
+#define PEM_FAN_FAN1		1
+#define PEM_FAN_FAN2		2
+#define PEM_FAN_FAN3		3
+
+/* Status register bits */
+#define STS1_OUTPUT_ON		(1 << 0)
+#define STS1_LEDS_FLASHING	(1 << 1)
+#define STS1_EXT_FAULT		(1 << 2)
+#define STS1_SERVICE_LED_ON	(1 << 3)
+#define STS1_SHUTDOWN_OCCURRED	(1 << 4)
+#define STS1_INT_FAULT		(1 << 5)
+#define STS1_ISOLATION_TEST_OK	(1 << 6)
+
+#define STS2_ENABLE_PIN_HI	(1 << 0)
+#define STS2_DATA_OUT_RANGE	(1 << 1)
+#define STS2_RESTARTED_OK	(1 << 1)
+#define STS2_ISOLATION_TEST_FAIL (1 << 3)
+#define STS2_HIGH_POWER_CAP	(1 << 4)
+#define STS2_INVALID_INSTR	(1 << 5)
+#define STS2_WILL_RESTART	(1 << 6)
+#define STS2_PEC_ERR		(1 << 7)
+
+/* Alarm register bits */
+#define ALRM1_VIN_OUT_LIMIT	(1 << 0)
+#define ALRM1_VOUT_OUT_LIMIT	(1 << 1)
+#define ALRM1_OV_VOLT_SHUTDOWN	(1 << 2)
+#define ALRM1_VIN_OVERCURRENT	(1 << 3)
+#define ALRM1_TEMP_WARNING	(1 << 4)
+#define ALRM1_TEMP_SHUTDOWN	(1 << 5)
+#define ALRM1_PRIMARY_FAULT	(1 << 6)
+#define ALRM1_POWER_LIMIT	(1 << 7)
+
+#define ALRM2_5V_OUT_LIMIT	(1 << 1)
+#define ALRM2_TEMP_FAULT	(1 << 2)
+#define ALRM2_OV_LOW		(1 << 3)
+#define ALRM2_DCDC_TEMP_HIGH	(1 << 4)
+#define ALRM2_PRI_TEMP_HIGH	(1 << 5)
+#define ALRM2_NO_PRIMARY	(1 << 6)
+#define ALRM2_FAN_FAULT		(1 << 7)
+
+#define FIRMWARE_REV_LEN	4
+#define DATA_STRING_LEN		9
+#define INPUT_STRING_LEN	5	/* 4 for most devices	*/
+#define FAN_SPEED_LEN		5
+
+struct pem_data {
+	struct device *hwmon_dev;
+
+	struct mutex update_lock;
+	bool valid;
+	bool fans_supported;
+	int input_length;
+	unsigned long last_updated;	/* in jiffies */
+
+	u8 firmware_rev[FIRMWARE_REV_LEN];
+	u8 data_string[DATA_STRING_LEN];
+	u8 input_string[INPUT_STRING_LEN];
+	u8 fan_speed[FAN_SPEED_LEN];
+};
+
+static int pem_read_block(struct i2c_client *client, u8 command, u8 *data,
+			  int data_len)
+{
+	u8 block_buffer[I2C_SMBUS_BLOCK_MAX];
+	int result;
+
+	result = i2c_smbus_read_block_data(client, command, block_buffer);
+	if (unlikely(result < 0))
+		goto abort;
+	if (unlikely(result == 0xff || result != data_len)) {
+		result = -EIO;
+		goto abort;
+	}
+	memcpy(data, block_buffer, data_len);
+	result = 0;
+abort:
+	return result;
+}
+
+static struct pem_data *pem_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct pem_data *data = i2c_get_clientdata(client);
+	struct pem_data *ret = data;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+		int result;
+
+		/* Read data string */
+		result = pem_read_block(client, PEM_READ_DATA_STRING,
+					data->data_string,
+					sizeof(data->data_string));
+		if (unlikely(result < 0)) {
+			ret = ERR_PTR(result);
+			goto abort;
+		}
+
+		/* Read input string */
+		if (data->input_length) {
+			result = pem_read_block(client, PEM_READ_INPUT_STRING,
+						data->input_string,
+						data->input_length);
+			if (unlikely(result < 0)) {
+				ret = ERR_PTR(result);
+				goto abort;
+			}
+		}
+
+		/* Read fan speeds */
+		if (data->fans_supported) {
+			result = pem_read_block(client, PEM_READ_FAN_SPEED,
+						data->fan_speed,
+						sizeof(data->fan_speed));
+			if (unlikely(result < 0)) {
+				ret = ERR_PTR(result);
+				goto abort;
+			}
+		}
+
+		i2c_smbus_write_byte(client, PEM_CLEAR_INFO_FLAGS);
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+abort:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static long pem_get_data(u8 *data, int len, int index)
+{
+	long val;
+
+	switch (index) {
+	case PEM_DATA_VOUT_LSB:
+		val = (data[index] + (data[index+1] << 8)) * 5 / 2;
+		break;
+	case PEM_DATA_CURRENT:
+		val = data[index] * 200;
+		break;
+	case PEM_DATA_TEMP:
+		val = data[index] * 1000;
+		break;
+	case PEM_DATA_TEMP_MAX:
+		val = 97 * 1000;	/* 97 degrees C per datasheet */
+		break;
+	case PEM_DATA_TEMP_CRIT:
+		val = 107 * 1000;	/* 107 degrees C per datasheet */
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		val = 0;
+	}
+	return val;
+}
+
+static long pem_get_input(u8 *data, int len, int index)
+{
+	long val;
+
+	switch (index) {
+	case PEM_INPUT_VOLTAGE:
+		if (len == INPUT_STRING_LEN)
+			val = (data[index] + (data[index+1] << 8) - 75) * 1000;
+		else
+			val = (data[index] - 75) * 1000;
+		break;
+	case PEM_INPUT_POWER_LSB:
+		if (len == INPUT_STRING_LEN)
+			index++;
+		val = (data[index] + (data[index+1] << 8)) * 1000000L;
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		val = 0;
+	}
+	return val;
+}
+
+static long pem_get_fan(u8 *data, int len, int index)
+{
+	long val;
+
+	switch (index) {
+	case PEM_FAN_FAN1:
+	case PEM_FAN_FAN2:
+	case PEM_FAN_FAN3:
+		val = data[index] * 100;
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		val = 0;
+	}
+	return val;
+}
+
+/*
+ * Show boolean, either a fault or an alarm.
+ * .nr points to the register, .index is the bit mask to check
+ */
+static ssize_t pem_show_bool(struct device *dev,
+			     struct device_attribute *da, char *buf)
+{
+	struct sensor_device_attribute_2 *attr = to_sensor_dev_attr_2(da);
+	struct pem_data *data = pem_update_device(dev);
+	u8 status;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	status = data->data_string[attr->nr] & attr->index;
+	return snprintf(buf, PAGE_SIZE, "%d\n", !!status);
+}
+
+static ssize_t pem_show_data(struct device *dev, struct device_attribute *da,
+			     char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct pem_data *data = pem_update_device(dev);
+	long value;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	value = pem_get_data(data->data_string, sizeof(data->data_string),
+			     attr->index);
+
+	return snprintf(buf, PAGE_SIZE, "%ld\n", value);
+}
+
+static ssize_t pem_show_input(struct device *dev, struct device_attribute *da,
+			      char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct pem_data *data = pem_update_device(dev);
+	long value;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	value = pem_get_input(data->input_string, sizeof(data->input_string),
+			      attr->index);
+
+	return snprintf(buf, PAGE_SIZE, "%ld\n", value);
+}
+
+static ssize_t pem_show_fan(struct device *dev, struct device_attribute *da,
+			    char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct pem_data *data = pem_update_device(dev);
+	long value;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	value = pem_get_fan(data->fan_speed, sizeof(data->fan_speed),
+			    attr->index);
+
+	return snprintf(buf, PAGE_SIZE, "%ld\n", value);
+}
+
+/* Voltages */
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, pem_show_data, NULL,
+			  PEM_DATA_VOUT_LSB);
+static SENSOR_DEVICE_ATTR_2(in1_min_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_2, ALRM2_OV_LOW);
+static SENSOR_DEVICE_ATTR_2(in1_max_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1, ALRM1_VOUT_OUT_LIMIT);
+static SENSOR_DEVICE_ATTR_2(in1_crit_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1, ALRM1_OV_VOLT_SHUTDOWN);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, pem_show_input, NULL,
+			  PEM_INPUT_VOLTAGE);
+static SENSOR_DEVICE_ATTR_2(in2_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1,
+			    ALRM1_VIN_OUT_LIMIT | ALRM1_PRIMARY_FAULT);
+
+/* Currents */
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, pem_show_data, NULL,
+			  PEM_DATA_CURRENT);
+static SENSOR_DEVICE_ATTR_2(curr1_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1, ALRM1_VIN_OVERCURRENT);
+
+/* Power */
+static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, pem_show_input, NULL,
+			  PEM_INPUT_POWER_LSB);
+static SENSOR_DEVICE_ATTR_2(power1_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1, ALRM1_POWER_LIMIT);
+
+/* Fans */
+static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, pem_show_fan, NULL,
+			  PEM_FAN_FAN1);
+static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, pem_show_fan, NULL,
+			  PEM_FAN_FAN2);
+static SENSOR_DEVICE_ATTR(fan3_input, S_IRUGO, pem_show_fan, NULL,
+			  PEM_FAN_FAN3);
+static SENSOR_DEVICE_ATTR_2(fan1_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_2, ALRM2_FAN_FAULT);
+
+/* Temperatures */
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, pem_show_data, NULL,
+			  PEM_DATA_TEMP);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, pem_show_data, NULL,
+			  PEM_DATA_TEMP_MAX);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, pem_show_data, NULL,
+			  PEM_DATA_TEMP_CRIT);
+static SENSOR_DEVICE_ATTR_2(temp1_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1, ALRM1_TEMP_WARNING);
+static SENSOR_DEVICE_ATTR_2(temp1_crit_alarm, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_1, ALRM1_TEMP_SHUTDOWN);
+static SENSOR_DEVICE_ATTR_2(temp1_fault, S_IRUGO, pem_show_bool, NULL,
+			    PEM_DATA_ALARM_2, ALRM2_TEMP_FAULT);
+
+static struct attribute *pem_attributes[] = {
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_in1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_in1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_in1_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_in2_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_curr1_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_power1_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_fan1_alarm.dev_attr.attr,
+
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_fault.dev_attr.attr,
+
+	NULL,
+};
+
+static const struct attribute_group pem_group = {
+	.attrs = pem_attributes,
+};
+
+static struct attribute *pem_input_attributes[] = {
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+	&sensor_dev_attr_curr1_input.dev_attr.attr,
+	&sensor_dev_attr_power1_input.dev_attr.attr,
+};
+
+static const struct attribute_group pem_input_group = {
+	.attrs = pem_input_attributes,
+};
+
+static struct attribute *pem_fan_attributes[] = {
+	&sensor_dev_attr_fan1_input.dev_attr.attr,
+	&sensor_dev_attr_fan2_input.dev_attr.attr,
+	&sensor_dev_attr_fan3_input.dev_attr.attr,
+};
+
+static const struct attribute_group pem_fan_group = {
+	.attrs = pem_fan_attributes,
+};
+
+static int pem_probe(struct i2c_client *client,
+		     const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct pem_data *data;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BLOCK_DATA
+				     | I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -ENODEV;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/*
+	 * We use the next two commands to determine if the device is really
+	 * there.
+	 */
+	ret = pem_read_block(client, PEM_READ_FIRMWARE_REV,
+			     data->firmware_rev, sizeof(data->firmware_rev));
+	if (ret < 0)
+		goto out_kfree;
+
+	ret = i2c_smbus_write_byte(client, PEM_CLEAR_INFO_FLAGS);
+	if (ret < 0)
+		goto out_kfree;
+
+	dev_info(&client->dev, "Firmware revision %d.%d.%d\n",
+		 data->firmware_rev[0], data->firmware_rev[1],
+		 data->firmware_rev[2]);
+
+	/* Register sysfs hooks */
+	ret = sysfs_create_group(&client->dev.kobj, &pem_group);
+	if (ret)
+		goto out_kfree;
+
+	/*
+	 * Check if input readings are supported.
+	 * This is the case if we can read input data,
+	 * and if the returned data is not all zeros.
+	 * Note that input alarms are always supported.
+	 */
+	ret = pem_read_block(client, PEM_READ_INPUT_STRING,
+			     data->input_string,
+			     sizeof(data->input_string) - 1);
+	if (!ret && (data->input_string[0] || data->input_string[1] ||
+		     data->input_string[2]))
+		data->input_length = sizeof(data->input_string) - 1;
+	else if (ret < 0) {
+		/* Input string is one byte longer for some devices */
+		ret = pem_read_block(client, PEM_READ_INPUT_STRING,
+				    data->input_string,
+				    sizeof(data->input_string));
+		if (!ret && (data->input_string[0] || data->input_string[1] ||
+			    data->input_string[2] || data->input_string[3]))
+			data->input_length = sizeof(data->input_string);
+	}
+	ret = 0;
+	if (data->input_length) {
+		ret = sysfs_create_group(&client->dev.kobj, &pem_input_group);
+		if (ret)
+			goto out_remove_groups;
+	}
+
+	/*
+	 * Check if fan speed readings are supported.
+	 * This is the case if we can read fan speed data,
+	 * and if the returned data is not all zeros.
+	 * Note that the fan alarm is always supported.
+	 */
+	ret = pem_read_block(client, PEM_READ_FAN_SPEED,
+			     data->fan_speed,
+			     sizeof(data->fan_speed));
+	if (!ret && (data->fan_speed[0] || data->fan_speed[1] ||
+		     data->fan_speed[2] || data->fan_speed[3])) {
+		data->fans_supported = true;
+		ret = sysfs_create_group(&client->dev.kobj, &pem_fan_group);
+		if (ret)
+			goto out_remove_groups;
+	}
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		ret = PTR_ERR(data->hwmon_dev);
+		goto out_remove_groups;
+	}
+
+	return 0;
+
+out_remove_groups:
+	sysfs_remove_group(&client->dev.kobj, &pem_input_group);
+	sysfs_remove_group(&client->dev.kobj, &pem_fan_group);
+	sysfs_remove_group(&client->dev.kobj, &pem_group);
+out_kfree:
+	kfree(data);
+	return ret;
+}
+
+static int pem_remove(struct i2c_client *client)
+{
+	struct pem_data *data = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(data->hwmon_dev);
+
+	sysfs_remove_group(&client->dev.kobj, &pem_input_group);
+	sysfs_remove_group(&client->dev.kobj, &pem_fan_group);
+	sysfs_remove_group(&client->dev.kobj, &pem_group);
+
+	kfree(data);
+	return 0;
+}
+
+static const struct i2c_device_id pem_id[] = {
+	{"lineage_pem", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, pem_id);
+
+static struct i2c_driver pem_driver = {
+	.driver = {
+		   .name = "lineage_pem",
+		   },
+	.probe = pem_probe,
+	.remove = pem_remove,
+	.id_table = pem_id,
+};
+
+static int __init pem_init(void)
+{
+	return i2c_add_driver(&pem_driver);
+}
+
+static void __exit pem_exit(void)
+{
+	i2c_del_driver(&pem_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck <guenter.roeck@ericsson.com>");
+MODULE_DESCRIPTION("Lineage CPL PEM hardware monitoring driver");
+MODULE_LICENSE("GPL");
+
+module_init(pem_init);
+module_exit(pem_exit);
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index f36eb80d227f..ef902d5d06ab 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -232,13 +232,16 @@ static const struct i2c_device_id lm75_ids[] = {
 };
 MODULE_DEVICE_TABLE(i2c, lm75_ids);
 
+#define LM75A_ID 0xA1
+
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm75_detect(struct i2c_client *new_client,
 		       struct i2c_board_info *info)
 {
 	struct i2c_adapter *adapter = new_client->adapter;
 	int i;
-	int cur, conf, hyst, os;
+	int conf, hyst, os;
+	bool is_lm75a = 0;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA |
 				     I2C_FUNC_SMBUS_WORD_DATA))
@@ -250,37 +253,58 @@ static int lm75_detect(struct i2c_client *new_client,
 	   addresses 0x04-0x07 returning the last read value.
 	   The cycling+unused addresses combination is not tested,
 	   since it would significantly slow the detection down and would
-	   hardly add any value. */
+	   hardly add any value.
 
-	/* Unused addresses */
-	cur = i2c_smbus_read_word_data(new_client, 0);
-	conf = i2c_smbus_read_byte_data(new_client, 1);
-	hyst = i2c_smbus_read_word_data(new_client, 2);
-	if (i2c_smbus_read_word_data(new_client, 4) != hyst
-	 || i2c_smbus_read_word_data(new_client, 5) != hyst
-	 || i2c_smbus_read_word_data(new_client, 6) != hyst
-	 || i2c_smbus_read_word_data(new_client, 7) != hyst)
-		return -ENODEV;
-	os = i2c_smbus_read_word_data(new_client, 3);
-	if (i2c_smbus_read_word_data(new_client, 4) != os
-	 || i2c_smbus_read_word_data(new_client, 5) != os
-	 || i2c_smbus_read_word_data(new_client, 6) != os
-	 || i2c_smbus_read_word_data(new_client, 7) != os)
-		return -ENODEV;
+	   The National Semiconductor LM75A is different than earlier
+	   LM75s.  It has an ID byte of 0xaX (where X is the chip
+	   revision, with 1 being the only revision in existence) in
+	   register 7, and unused registers return 0xff rather than the
+	   last read value. */
 
 	/* Unused bits */
+	conf = i2c_smbus_read_byte_data(new_client, 1);
 	if (conf & 0xe0)
 		return -ENODEV;
 
+	/* First check for LM75A */
+	if (i2c_smbus_read_byte_data(new_client, 7) == LM75A_ID) {
+		/* LM75A returns 0xff on unused registers so
+		   just to be sure we check for that too. */
+		if (i2c_smbus_read_byte_data(new_client, 4) != 0xff
+		 || i2c_smbus_read_byte_data(new_client, 5) != 0xff
+		 || i2c_smbus_read_byte_data(new_client, 6) != 0xff)
+			return -ENODEV;
+		is_lm75a = 1;
+		hyst = i2c_smbus_read_byte_data(new_client, 2);
+		os = i2c_smbus_read_byte_data(new_client, 3);
+	} else { /* Traditional style LM75 detection */
+		/* Unused addresses */
+		hyst = i2c_smbus_read_byte_data(new_client, 2);
+		if (i2c_smbus_read_byte_data(new_client, 4) != hyst
+		 || i2c_smbus_read_byte_data(new_client, 5) != hyst
+		 || i2c_smbus_read_byte_data(new_client, 6) != hyst
+		 || i2c_smbus_read_byte_data(new_client, 7) != hyst)
+			return -ENODEV;
+		os = i2c_smbus_read_byte_data(new_client, 3);
+		if (i2c_smbus_read_byte_data(new_client, 4) != os
+		 || i2c_smbus_read_byte_data(new_client, 5) != os
+		 || i2c_smbus_read_byte_data(new_client, 6) != os
+		 || i2c_smbus_read_byte_data(new_client, 7) != os)
+			return -ENODEV;
+	}
+
 	/* Addresses cycling */
-	for (i = 8; i < 0xff; i += 8) {
+	for (i = 8; i <= 248; i += 40) {
 		if (i2c_smbus_read_byte_data(new_client, i + 1) != conf
-		 || i2c_smbus_read_word_data(new_client, i + 2) != hyst
-		 || i2c_smbus_read_word_data(new_client, i + 3) != os)
+		 || i2c_smbus_read_byte_data(new_client, i + 2) != hyst
+		 || i2c_smbus_read_byte_data(new_client, i + 3) != os)
+			return -ENODEV;
+		if (is_lm75a && i2c_smbus_read_byte_data(new_client, i + 7)
+				!= LM75A_ID)
 			return -ENODEV;
 	}
 
-	strlcpy(info->type, "lm75", I2C_NAME_SIZE);
+	strlcpy(info->type, is_lm75a ? "lm75a" : "lm75", I2C_NAME_SIZE);
 
 	return 0;
 }
diff --git a/drivers/hwmon/lm85.c b/drivers/hwmon/lm85.c
index d2cc28660816..cf47e6e476ed 100644
--- a/drivers/hwmon/lm85.c
+++ b/drivers/hwmon/lm85.c
@@ -41,7 +41,7 @@ static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, I2C_CLIENT_END };
 enum chips {
 	any_chip, lm85b, lm85c,
 	adm1027, adt7463, adt7468,
-	emc6d100, emc6d102, emc6d103
+	emc6d100, emc6d102, emc6d103, emc6d103s
 };
 
 /* The LM85 registers */
@@ -283,10 +283,6 @@ struct lm85_zone {
 	u8 hyst;	/* Low limit hysteresis. (0-15) */
 	u8 range;	/* Temp range, encoded */
 	s8 critical;	/* "All fans ON" temp limit */
-	u8 off_desired; /* Actual "off" temperature specified.  Preserved
-			 * to prevent "drift" as other autofan control
-			 * values change.
-			 */
 	u8 max_desired; /* Actual "max" temperature specified.  Preserved
 			 * to prevent "drift" as other autofan control
 			 * values change.
@@ -306,6 +302,8 @@ struct lm85_data {
 	const int *freq_map;
 	enum chips type;
 
+	bool has_vid5;	/* true if VID5 is configured for ADT7463 or ADT7468 */
+
 	struct mutex update_lock;
 	int valid;		/* !=0 if following fields are valid */
 	unsigned long last_reading;	/* In jiffies */
@@ -352,6 +350,7 @@ static const struct i2c_device_id lm85_id[] = {
 	{ "emc6d101", emc6d100 },
 	{ "emc6d102", emc6d102 },
 	{ "emc6d103", emc6d103 },
+	{ "emc6d103s", emc6d103s },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, lm85_id);
@@ -420,8 +419,7 @@ static ssize_t show_vid_reg(struct device *dev, struct device_attribute *attr,
 	struct lm85_data *data = lm85_update_device(dev);
 	int vid;
 
-	if ((data->type == adt7463 || data->type == adt7468) &&
-	    (data->vid & 0x80)) {
+	if (data->has_vid5) {
 		/* 6-pin VID (VRM 10) */
 		vid = vid_from_reg(data->vid & 0x3f, data->vrm);
 	} else {
@@ -891,7 +889,6 @@ static ssize_t set_temp_auto_temp_off(struct device *dev,
 
 	mutex_lock(&data->update_lock);
 	min = TEMP_FROM_REG(data->zone[nr].limit);
-	data->zone[nr].off_desired = TEMP_TO_REG(val);
 	data->zone[nr].hyst = HYST_TO_REG(min - val);
 	if (nr == 0 || nr == 1) {
 		lm85_write_value(client, LM85_REG_AFAN_HYST1,
@@ -934,18 +931,6 @@ static ssize_t set_temp_auto_temp_min(struct device *dev,
 		((data->zone[nr].range & 0x0f) << 4)
 		| (data->pwm_freq[nr] & 0x07));
 
-/* Update temp_auto_hyst and temp_auto_off */
-	data->zone[nr].hyst = HYST_TO_REG(TEMP_FROM_REG(
-		data->zone[nr].limit) - TEMP_FROM_REG(
-		data->zone[nr].off_desired));
-	if (nr == 0 || nr == 1) {
-		lm85_write_value(client, LM85_REG_AFAN_HYST1,
-			(data->zone[0].hyst << 4)
-			| data->zone[1].hyst);
-	} else {
-		lm85_write_value(client, LM85_REG_AFAN_HYST2,
-			(data->zone[2].hyst << 4));
-	}
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -1084,13 +1069,7 @@ static struct attribute *lm85_attributes[] = {
 	&sensor_dev_attr_pwm1_auto_pwm_min.dev_attr.attr,
 	&sensor_dev_attr_pwm2_auto_pwm_min.dev_attr.attr,
 	&sensor_dev_attr_pwm3_auto_pwm_min.dev_attr.attr,
-	&sensor_dev_attr_pwm1_auto_pwm_minctl.dev_attr.attr,
-	&sensor_dev_attr_pwm2_auto_pwm_minctl.dev_attr.attr,
-	&sensor_dev_attr_pwm3_auto_pwm_minctl.dev_attr.attr,
 
-	&sensor_dev_attr_temp1_auto_temp_off.dev_attr.attr,
-	&sensor_dev_attr_temp2_auto_temp_off.dev_attr.attr,
-	&sensor_dev_attr_temp3_auto_temp_off.dev_attr.attr,
 	&sensor_dev_attr_temp1_auto_temp_min.dev_attr.attr,
 	&sensor_dev_attr_temp2_auto_temp_min.dev_attr.attr,
 	&sensor_dev_attr_temp3_auto_temp_min.dev_attr.attr,
@@ -1111,6 +1090,26 @@ static const struct attribute_group lm85_group = {
 	.attrs = lm85_attributes,
 };
 
+static struct attribute *lm85_attributes_minctl[] = {
+	&sensor_dev_attr_pwm1_auto_pwm_minctl.dev_attr.attr,
+	&sensor_dev_attr_pwm2_auto_pwm_minctl.dev_attr.attr,
+	&sensor_dev_attr_pwm3_auto_pwm_minctl.dev_attr.attr,
+};
+
+static const struct attribute_group lm85_group_minctl = {
+	.attrs = lm85_attributes_minctl,
+};
+
+static struct attribute *lm85_attributes_temp_off[] = {
+	&sensor_dev_attr_temp1_auto_temp_off.dev_attr.attr,
+	&sensor_dev_attr_temp2_auto_temp_off.dev_attr.attr,
+	&sensor_dev_attr_temp3_auto_temp_off.dev_attr.attr,
+};
+
+static const struct attribute_group lm85_group_temp_off = {
+	.attrs = lm85_attributes_temp_off,
+};
+
 static struct attribute *lm85_attributes_in4[] = {
 	&sensor_dev_attr_in4_input.dev_attr.attr,
 	&sensor_dev_attr_in4_min.dev_attr.attr,
@@ -1258,16 +1257,9 @@ static int lm85_detect(struct i2c_client *client, struct i2c_board_info *info)
 		case LM85_VERSTEP_EMC6D103_A1:
 			type_name = "emc6d103";
 			break;
-		/*
-		 * Registers apparently missing in EMC6D103S/EMC6D103:A2
-		 * compared to EMC6D103:A0, EMC6D103:A1, and EMC6D102
-		 * (according to the data sheets), but used unconditionally
-		 * in the driver: 62[5:7], 6D[0:7], and 6E[0:7].
-		 * So skip EMC6D103S for now.
 		case LM85_VERSTEP_EMC6D103S:
 			type_name = "emc6d103s";
 			break;
-		 */
 		}
 	} else {
 		dev_dbg(&adapter->dev,
@@ -1280,6 +1272,19 @@ static int lm85_detect(struct i2c_client *client, struct i2c_board_info *info)
 	return 0;
 }
 
+static void lm85_remove_files(struct i2c_client *client, struct lm85_data *data)
+{
+	sysfs_remove_group(&client->dev.kobj, &lm85_group);
+	if (data->type != emc6d103s) {
+		sysfs_remove_group(&client->dev.kobj, &lm85_group_minctl);
+		sysfs_remove_group(&client->dev.kobj, &lm85_group_temp_off);
+	}
+	if (!data->has_vid5)
+		sysfs_remove_group(&client->dev.kobj, &lm85_group_in4);
+	if (data->type == emc6d100)
+		sysfs_remove_group(&client->dev.kobj, &lm85_group_in567);
+}
+
 static int lm85_probe(struct i2c_client *client,
 		      const struct i2c_device_id *id)
 {
@@ -1302,6 +1307,7 @@ static int lm85_probe(struct i2c_client *client,
 	case emc6d100:
 	case emc6d102:
 	case emc6d103:
+	case emc6d103s:
 		data->freq_map = adm1027_freq_map;
 		break;
 	default:
@@ -1319,11 +1325,26 @@ static int lm85_probe(struct i2c_client *client,
 	if (err)
 		goto err_kfree;
 
+	/* minctl and temp_off exist on all chips except emc6d103s */
+	if (data->type != emc6d103s) {
+		err = sysfs_create_group(&client->dev.kobj, &lm85_group_minctl);
+		if (err)
+			goto err_kfree;
+		err = sysfs_create_group(&client->dev.kobj,
+					 &lm85_group_temp_off);
+		if (err)
+			goto err_kfree;
+	}
+
 	/* The ADT7463/68 have an optional VRM 10 mode where pin 21 is used
 	   as a sixth digital VID input rather than an analog input. */
-	data->vid = lm85_read_value(client, LM85_REG_VID);
-	if (!((data->type == adt7463 || data->type == adt7468) &&
-	    (data->vid & 0x80)))
+	if (data->type == adt7463 || data->type == adt7468) {
+		u8 vid = lm85_read_value(client, LM85_REG_VID);
+		if (vid & 0x80)
+			data->has_vid5 = true;
+	}
+
+	if (!data->has_vid5)
 		if ((err = sysfs_create_group(&client->dev.kobj,
 					&lm85_group_in4)))
 			goto err_remove_files;
@@ -1344,10 +1365,7 @@ static int lm85_probe(struct i2c_client *client,
 
 	/* Error out and cleanup code */
  err_remove_files:
-	sysfs_remove_group(&client->dev.kobj, &lm85_group);
-	sysfs_remove_group(&client->dev.kobj, &lm85_group_in4);
-	if (data->type == emc6d100)
-		sysfs_remove_group(&client->dev.kobj, &lm85_group_in567);
+	lm85_remove_files(client, data);
  err_kfree:
 	kfree(data);
 	return err;
@@ -1357,10 +1375,7 @@ static int lm85_remove(struct i2c_client *client)
 {
 	struct lm85_data *data = i2c_get_clientdata(client);
 	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm85_group);
-	sysfs_remove_group(&client->dev.kobj, &lm85_group_in4);
-	if (data->type == emc6d100)
-		sysfs_remove_group(&client->dev.kobj, &lm85_group_in567);
+	lm85_remove_files(client, data);
 	kfree(data);
 	return 0;
 }
@@ -1457,11 +1472,8 @@ static struct lm85_data *lm85_update_device(struct device *dev)
 			    lm85_read_value(client, LM85_REG_FAN(i));
 		}
 
-		if (!((data->type == adt7463 || data->type == adt7468) &&
-		    (data->vid & 0x80))) {
-			data->in[4] = lm85_read_value(client,
-				      LM85_REG_IN(4));
-		}
+		if (!data->has_vid5)
+			data->in[4] = lm85_read_value(client, LM85_REG_IN(4));
 
 		if (data->type == adt7468)
 			data->cfg5 = lm85_read_value(client, ADT7468_REG_CFG5);
@@ -1487,7 +1499,8 @@ static struct lm85_data *lm85_update_device(struct device *dev)
 			/* More alarm bits */
 			data->alarms |= lm85_read_value(client,
 						EMC6D100_REG_ALARM3) << 16;
-		} else if (data->type == emc6d102 || data->type == emc6d103) {
+		} else if (data->type == emc6d102 || data->type == emc6d103 ||
+			   data->type == emc6d103s) {
 			/* Have to read LSB bits after the MSB ones because
 			   the reading of the MSB bits has frozen the
 			   LSBs (backward from the ADM1027).
@@ -1528,8 +1541,7 @@ static struct lm85_data *lm85_update_device(struct device *dev)
 			    lm85_read_value(client, LM85_REG_FAN_MIN(i));
 		}
 
-		if (!((data->type == adt7463 || data->type == adt7468) &&
-		    (data->vid & 0x80))) {
+		if (!data->has_vid5)  {
 			data->in_min[4] = lm85_read_value(client,
 					  LM85_REG_IN_MIN(4));
 			data->in_max[4] = lm85_read_value(client,
@@ -1573,17 +1585,19 @@ static struct lm85_data *lm85_update_device(struct device *dev)
 			}
 		}
 
-		i = lm85_read_value(client, LM85_REG_AFAN_SPIKE1);
-		data->autofan[0].min_off = (i & 0x20) != 0;
-		data->autofan[1].min_off = (i & 0x40) != 0;
-		data->autofan[2].min_off = (i & 0x80) != 0;
+		if (data->type != emc6d103s) {
+			i = lm85_read_value(client, LM85_REG_AFAN_SPIKE1);
+			data->autofan[0].min_off = (i & 0x20) != 0;
+			data->autofan[1].min_off = (i & 0x40) != 0;
+			data->autofan[2].min_off = (i & 0x80) != 0;
 
-		i = lm85_read_value(client, LM85_REG_AFAN_HYST1);
-		data->zone[0].hyst = i >> 4;
-		data->zone[1].hyst = i & 0x0f;
+			i = lm85_read_value(client, LM85_REG_AFAN_HYST1);
+			data->zone[0].hyst = i >> 4;
+			data->zone[1].hyst = i & 0x0f;
 
-		i = lm85_read_value(client, LM85_REG_AFAN_HYST2);
-		data->zone[2].hyst = i >> 4;
+			i = lm85_read_value(client, LM85_REG_AFAN_HYST2);
+			data->zone[2].hyst = i >> 4;
+		}
 
 		data->last_config = jiffies;
 	}  /* last_config */
diff --git a/drivers/hwmon/ltc4151.c b/drivers/hwmon/ltc4151.c
new file mode 100644
index 000000000000..4ac06b75aa60
--- /dev/null
+++ b/drivers/hwmon/ltc4151.c
@@ -0,0 +1,256 @@
+/*
+ * Driver for Linear Technology LTC4151 High Voltage I2C Current
+ * and Voltage Monitor
+ *
+ * Copyright (C) 2011 AppearTV AS
+ *
+ * Derived from:
+ *
+ *  Driver for Linear Technology LTC4261 I2C Negative Voltage Hot
+ *  Swap Controller
+ *  Copyright (C) 2010 Ericsson AB.
+ *
+ * Datasheet: http://www.linear.com/docs/Datasheet/4151fc.pdf
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+
+/* chip registers */
+#define LTC4151_SENSE_H	0x00
+#define LTC4151_SENSE_L	0x01
+#define LTC4151_VIN_H	0x02
+#define LTC4151_VIN_L	0x03
+#define LTC4151_ADIN_H	0x04
+#define LTC4151_ADIN_L	0x05
+
+struct ltc4151_data {
+	struct device *hwmon_dev;
+
+	struct mutex update_lock;
+	bool valid;
+	unsigned long last_updated; /* in jiffies */
+
+	/* Registers */
+	u8 regs[6];
+};
+
+static struct ltc4151_data *ltc4151_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ltc4151_data *data = i2c_get_clientdata(client);
+	struct ltc4151_data *ret = data;
+
+	mutex_lock(&data->update_lock);
+
+	/*
+	 * The chip's A/D updates 6 times per second
+	 * (Conversion Rate 6 - 9 Hz)
+	 */
+	if (time_after(jiffies, data->last_updated + HZ / 6) || !data->valid) {
+		int i;
+
+		dev_dbg(&client->dev, "Starting ltc4151 update\n");
+
+		/* Read all registers */
+		for (i = 0; i < ARRAY_SIZE(data->regs); i++) {
+			int val;
+
+			val = i2c_smbus_read_byte_data(client, i);
+			if (unlikely(val < 0)) {
+				dev_dbg(dev,
+					"Failed to read ADC value: error %d\n",
+					val);
+				ret = ERR_PTR(val);
+				goto abort;
+			}
+			data->regs[i] = val;
+		}
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+abort:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+/* Return the voltage from the given register in mV */
+static int ltc4151_get_value(struct ltc4151_data *data, u8 reg)
+{
+	u32 val;
+
+	val = (data->regs[reg] << 4) + (data->regs[reg + 1] >> 4);
+
+	switch (reg) {
+	case LTC4151_ADIN_H:
+		/* 500uV resolution. Convert to mV. */
+		val = val * 500 / 1000;
+		break;
+	case LTC4151_SENSE_H:
+		/*
+		 * 20uV resolution. Convert to current as measured with
+		 * an 1 mOhm sense resistor, in mA.
+		 */
+		val = val * 20;
+		break;
+	case LTC4151_VIN_H:
+		/* 25 mV per increment */
+		val = val * 25;
+		break;
+	default:
+		/* If we get here, the developer messed up */
+		WARN_ON_ONCE(1);
+		val = 0;
+		break;
+	}
+
+	return val;
+}
+
+static ssize_t ltc4151_show_value(struct device *dev,
+				  struct device_attribute *da, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct ltc4151_data *data = ltc4151_update_device(dev);
+	int value;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	value = ltc4151_get_value(data, attr->index);
+	return snprintf(buf, PAGE_SIZE, "%d\n", value);
+}
+
+/*
+ * Input voltages.
+ */
+static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, \
+	ltc4151_show_value, NULL, LTC4151_VIN_H);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, \
+	ltc4151_show_value, NULL, LTC4151_ADIN_H);
+
+/* Currents (via sense resistor) */
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, \
+	ltc4151_show_value, NULL, LTC4151_SENSE_H);
+
+/* Finally, construct an array of pointers to members of the above objects,
+ * as required for sysfs_create_group()
+ */
+static struct attribute *ltc4151_attributes[] = {
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+
+	&sensor_dev_attr_curr1_input.dev_attr.attr,
+
+	NULL,
+};
+
+static const struct attribute_group ltc4151_group = {
+	.attrs = ltc4151_attributes,
+};
+
+static int ltc4151_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	struct ltc4151_data *data;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto out_kzalloc;
+	}
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/* Register sysfs hooks */
+	ret = sysfs_create_group(&client->dev.kobj, &ltc4151_group);
+	if (ret)
+		goto out_sysfs_create_group;
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		ret = PTR_ERR(data->hwmon_dev);
+		goto out_hwmon_device_register;
+	}
+
+	return 0;
+
+out_hwmon_device_register:
+	sysfs_remove_group(&client->dev.kobj, &ltc4151_group);
+out_sysfs_create_group:
+	kfree(data);
+out_kzalloc:
+	return ret;
+}
+
+static int ltc4151_remove(struct i2c_client *client)
+{
+	struct ltc4151_data *data = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &ltc4151_group);
+
+	kfree(data);
+
+	return 0;
+}
+
+static const struct i2c_device_id ltc4151_id[] = {
+	{ "ltc4151", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc4151_id);
+
+/* This is the driver that will be inserted */
+static struct i2c_driver ltc4151_driver = {
+	.driver = {
+		.name	= "ltc4151",
+	},
+	.probe		= ltc4151_probe,
+	.remove		= ltc4151_remove,
+	.id_table	= ltc4151_id,
+};
+
+static int __init ltc4151_init(void)
+{
+	return i2c_add_driver(&ltc4151_driver);
+}
+
+static void __exit ltc4151_exit(void)
+{
+	i2c_del_driver(&ltc4151_driver);
+}
+
+MODULE_AUTHOR("Per Dalen <per.dalen@appeartv.com>");
+MODULE_DESCRIPTION("LTC4151 driver");
+MODULE_LICENSE("GPL");
+
+module_init(ltc4151_init);
+module_exit(ltc4151_exit);
diff --git a/drivers/hwmon/max16064.c b/drivers/hwmon/max16064.c
new file mode 100644
index 000000000000..1d6d717060d3
--- /dev/null
+++ b/drivers/hwmon/max16064.c
@@ -0,0 +1,91 @@
+/*
+ * Hardware monitoring driver for Maxim MAX16064
+ *
+ * Copyright (c) 2011 Ericsson AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include "pmbus.h"
+
+static struct pmbus_driver_info max16064_info = {
+	.pages = 4,
+	.direct[PSC_VOLTAGE_IN] = true,
+	.direct[PSC_VOLTAGE_OUT] = true,
+	.direct[PSC_TEMPERATURE] = true,
+	.m[PSC_VOLTAGE_IN] = 19995,
+	.b[PSC_VOLTAGE_IN] = 0,
+	.R[PSC_VOLTAGE_IN] = -1,
+	.m[PSC_VOLTAGE_OUT] = 19995,
+	.b[PSC_VOLTAGE_OUT] = 0,
+	.R[PSC_VOLTAGE_OUT] = -1,
+	.m[PSC_TEMPERATURE] = -7612,
+	.b[PSC_TEMPERATURE] = 335,
+	.R[PSC_TEMPERATURE] = -3,
+	.func[0] = PMBUS_HAVE_VOUT | PMBUS_HAVE_TEMP
+		| PMBUS_HAVE_STATUS_VOUT | PMBUS_HAVE_STATUS_TEMP,
+	.func[1] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT,
+	.func[2] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT,
+	.func[3] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT,
+};
+
+static int max16064_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	return pmbus_do_probe(client, id, &max16064_info);
+}
+
+static int max16064_remove(struct i2c_client *client)
+{
+	return pmbus_do_remove(client);
+}
+
+static const struct i2c_device_id max16064_id[] = {
+	{"max16064", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, max16064_id);
+
+/* This is the driver that will be inserted */
+static struct i2c_driver max16064_driver = {
+	.driver = {
+		   .name = "max16064",
+		   },
+	.probe = max16064_probe,
+	.remove = max16064_remove,
+	.id_table = max16064_id,
+};
+
+static int __init max16064_init(void)
+{
+	return i2c_add_driver(&max16064_driver);
+}
+
+static void __exit max16064_exit(void)
+{
+	i2c_del_driver(&max16064_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck");
+MODULE_DESCRIPTION("PMBus driver for Maxim MAX16064");
+MODULE_LICENSE("GPL");
+module_init(max16064_init);
+module_exit(max16064_exit);
diff --git a/drivers/hwmon/max34440.c b/drivers/hwmon/max34440.c
new file mode 100644
index 000000000000..992b701b4c5e
--- /dev/null
+++ b/drivers/hwmon/max34440.c
@@ -0,0 +1,199 @@
+/*
+ * Hardware monitoring driver for Maxim MAX34440/MAX34441
+ *
+ * Copyright (c) 2011 Ericsson AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include "pmbus.h"
+
+enum chips { max34440, max34441 };
+
+#define MAX34440_STATUS_OC_WARN		(1 << 0)
+#define MAX34440_STATUS_OC_FAULT	(1 << 1)
+#define MAX34440_STATUS_OT_FAULT	(1 << 5)
+#define MAX34440_STATUS_OT_WARN		(1 << 6)
+
+static int max34440_get_status(struct i2c_client *client, int page, int reg)
+{
+	int ret;
+	int mfg_status;
+
+	ret = pmbus_set_page(client, page);
+	if (ret < 0)
+		return ret;
+
+	switch (reg) {
+	case PMBUS_STATUS_IOUT:
+		mfg_status = pmbus_read_word_data(client, 0,
+						  PMBUS_STATUS_MFR_SPECIFIC);
+		if (mfg_status < 0)
+			return mfg_status;
+		if (mfg_status & MAX34440_STATUS_OC_WARN)
+			ret |= PB_IOUT_OC_WARNING;
+		if (mfg_status & MAX34440_STATUS_OC_FAULT)
+			ret |= PB_IOUT_OC_FAULT;
+		break;
+	case PMBUS_STATUS_TEMPERATURE:
+		mfg_status = pmbus_read_word_data(client, 0,
+						  PMBUS_STATUS_MFR_SPECIFIC);
+		if (mfg_status < 0)
+			return mfg_status;
+		if (mfg_status & MAX34440_STATUS_OT_WARN)
+			ret |= PB_TEMP_OT_WARNING;
+		if (mfg_status & MAX34440_STATUS_OT_FAULT)
+			ret |= PB_TEMP_OT_FAULT;
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+	return ret;
+}
+
+static struct pmbus_driver_info max34440_info[] = {
+	[max34440] = {
+		.pages = 14,
+		.direct[PSC_VOLTAGE_IN] = true,
+		.direct[PSC_VOLTAGE_OUT] = true,
+		.direct[PSC_TEMPERATURE] = true,
+		.direct[PSC_CURRENT_OUT] = true,
+		.m[PSC_VOLTAGE_IN] = 1,
+		.b[PSC_VOLTAGE_IN] = 0,
+		.R[PSC_VOLTAGE_IN] = 3,	    /* R = 0 in datasheet reflects mV */
+		.m[PSC_VOLTAGE_OUT] = 1,
+		.b[PSC_VOLTAGE_OUT] = 0,
+		.R[PSC_VOLTAGE_OUT] = 3,    /* R = 0 in datasheet reflects mV */
+		.m[PSC_CURRENT_OUT] = 1,
+		.b[PSC_CURRENT_OUT] = 0,
+		.R[PSC_CURRENT_OUT] = 3,    /* R = 0 in datasheet reflects mA */
+		.m[PSC_TEMPERATURE] = 1,
+		.b[PSC_TEMPERATURE] = 0,
+		.R[PSC_TEMPERATURE] = 2,
+		.func[0] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[1] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[2] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[3] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[4] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[5] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[6] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[7] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[8] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[9] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[10] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[11] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[12] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[13] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.get_status = max34440_get_status,
+	},
+	[max34441] = {
+		.pages = 12,
+		.direct[PSC_VOLTAGE_IN] = true,
+		.direct[PSC_VOLTAGE_OUT] = true,
+		.direct[PSC_TEMPERATURE] = true,
+		.direct[PSC_CURRENT_OUT] = true,
+		.direct[PSC_FAN] = true,
+		.m[PSC_VOLTAGE_IN] = 1,
+		.b[PSC_VOLTAGE_IN] = 0,
+		.R[PSC_VOLTAGE_IN] = 3,
+		.m[PSC_VOLTAGE_OUT] = 1,
+		.b[PSC_VOLTAGE_OUT] = 0,
+		.R[PSC_VOLTAGE_OUT] = 3,
+		.m[PSC_CURRENT_OUT] = 1,
+		.b[PSC_CURRENT_OUT] = 0,
+		.R[PSC_CURRENT_OUT] = 3,
+		.m[PSC_TEMPERATURE] = 1,
+		.b[PSC_TEMPERATURE] = 0,
+		.R[PSC_TEMPERATURE] = 2,
+		.m[PSC_FAN] = 1,
+		.b[PSC_FAN] = 0,
+		.R[PSC_FAN] = 0,
+		.func[0] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[1] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[2] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[3] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[4] = PMBUS_HAVE_VOUT | PMBUS_HAVE_STATUS_VOUT
+		  | PMBUS_HAVE_IOUT | PMBUS_HAVE_STATUS_IOUT,
+		.func[5] = PMBUS_HAVE_FAN12 | PMBUS_HAVE_STATUS_FAN12,
+		.func[6] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[7] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[8] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[9] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[10] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.func[11] = PMBUS_HAVE_TEMP | PMBUS_HAVE_STATUS_TEMP,
+		.get_status = max34440_get_status,
+	},
+};
+
+static int max34440_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	return pmbus_do_probe(client, id, &max34440_info[id->driver_data]);
+}
+
+static int max34440_remove(struct i2c_client *client)
+{
+	return pmbus_do_remove(client);
+}
+
+static const struct i2c_device_id max34440_id[] = {
+	{"max34440", max34440},
+	{"max34441", max34441},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, max34440_id);
+
+/* This is the driver that will be inserted */
+static struct i2c_driver max34440_driver = {
+	.driver = {
+		   .name = "max34440",
+		   },
+	.probe = max34440_probe,
+	.remove = max34440_remove,
+	.id_table = max34440_id,
+};
+
+static int __init max34440_init(void)
+{
+	return i2c_add_driver(&max34440_driver);
+}
+
+static void __exit max34440_exit(void)
+{
+	i2c_del_driver(&max34440_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck");
+MODULE_DESCRIPTION("PMBus driver for Maxim MAX34440/MAX34441");
+MODULE_LICENSE("GPL");
+module_init(max34440_init);
+module_exit(max34440_exit);
diff --git a/drivers/hwmon/max6639.c b/drivers/hwmon/max6639.c
new file mode 100644
index 000000000000..f20d9978ee78
--- /dev/null
+++ b/drivers/hwmon/max6639.c
@@ -0,0 +1,653 @@
+/*
+ * max6639.c - Support for Maxim MAX6639
+ *
+ * 2-Channel Temperature Monitor with Dual PWM Fan-Speed Controller
+ *
+ * Copyright (C) 2010, 2011 Roland Stigge <stigge@antcom.de>
+ *
+ * based on the initial MAX6639 support from semptian.net
+ * by He Changqing <hechangqing@semptian.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/i2c/max6639.h>
+
+/* Addresses to scan */
+static unsigned short normal_i2c[] = { 0x2c, 0x2e, 0x2f, I2C_CLIENT_END };
+
+/* The MAX6639 registers, valid channel numbers: 0, 1 */
+#define MAX6639_REG_TEMP(ch)			(0x00 + (ch))
+#define MAX6639_REG_STATUS			0x02
+#define MAX6639_REG_OUTPUT_MASK			0x03
+#define MAX6639_REG_GCONFIG			0x04
+#define MAX6639_REG_TEMP_EXT(ch)		(0x05 + (ch))
+#define MAX6639_REG_ALERT_LIMIT(ch)		(0x08 + (ch))
+#define MAX6639_REG_OT_LIMIT(ch)		(0x0A + (ch))
+#define MAX6639_REG_THERM_LIMIT(ch)		(0x0C + (ch))
+#define MAX6639_REG_FAN_CONFIG1(ch)		(0x10 + (ch) * 4)
+#define MAX6639_REG_FAN_CONFIG2a(ch)		(0x11 + (ch) * 4)
+#define MAX6639_REG_FAN_CONFIG2b(ch)		(0x12 + (ch) * 4)
+#define MAX6639_REG_FAN_CONFIG3(ch)		(0x13 + (ch) * 4)
+#define MAX6639_REG_FAN_CNT(ch)			(0x20 + (ch))
+#define MAX6639_REG_TARGET_CNT(ch)		(0x22 + (ch))
+#define MAX6639_REG_FAN_PPR(ch)			(0x24 + (ch))
+#define MAX6639_REG_TARGTDUTY(ch)		(0x26 + (ch))
+#define MAX6639_REG_FAN_START_TEMP(ch)		(0x28 + (ch))
+#define MAX6639_REG_DEVID			0x3D
+#define MAX6639_REG_MANUID			0x3E
+#define MAX6639_REG_DEVREV			0x3F
+
+/* Register bits */
+#define MAX6639_GCONFIG_STANDBY			0x80
+#define MAX6639_GCONFIG_POR			0x40
+#define MAX6639_GCONFIG_DISABLE_TIMEOUT		0x20
+#define MAX6639_GCONFIG_CH2_LOCAL		0x10
+#define MAX6639_GCONFIG_PWM_FREQ_HI		0x08
+
+#define MAX6639_FAN_CONFIG1_PWM			0x80
+
+#define MAX6639_FAN_CONFIG3_THERM_FULL_SPEED	0x40
+
+static const int rpm_ranges[] = { 2000, 4000, 8000, 16000 };
+
+#define FAN_FROM_REG(val, div, rpm_range)	((val) == 0 ? -1 : \
+	(val) == 255 ? 0 : (rpm_ranges[rpm_range] * 30) / ((div + 1) * (val)))
+#define TEMP_LIMIT_TO_REG(val)	SENSORS_LIMIT((val) / 1000, 0, 255)
+
+/*
+ * Client data (each client gets its own)
+ */
+struct max6639_data {
+	struct device *hwmon_dev;
+	struct mutex update_lock;
+	char valid;		/* !=0 if following fields are valid */
+	unsigned long last_updated;	/* In jiffies */
+
+	/* Register values sampled regularly */
+	u16 temp[2];		/* Temperature, in 1/8 C, 0..255 C */
+	bool temp_fault[2];	/* Detected temperature diode failure */
+	u8 fan[2];		/* Register value: TACH count for fans >=30 */
+	u8 status;		/* Detected channel alarms and fan failures */
+
+	/* Register values only written to */
+	u8 pwm[2];		/* Register value: Duty cycle 0..120 */
+	u8 temp_therm[2];	/* THERM Temperature, 0..255 C (->_max) */
+	u8 temp_alert[2];	/* ALERT Temperature, 0..255 C (->_crit) */
+	u8 temp_ot[2];		/* OT Temperature, 0..255 C (->_emergency) */
+
+	/* Register values initialized only once */
+	u8 ppr;			/* Pulses per rotation 0..3 for 1..4 ppr */
+	u8 rpm_range;		/* Index in above rpm_ranges table */
+};
+
+static struct max6639_data *max6639_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct max6639_data *ret = data;
+	int i;
+	int status_reg;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + 2 * HZ) || !data->valid) {
+		int res;
+
+		dev_dbg(&client->dev, "Starting max6639 update\n");
+
+		status_reg = i2c_smbus_read_byte_data(client,
+						      MAX6639_REG_STATUS);
+		if (status_reg < 0) {
+			ret = ERR_PTR(status_reg);
+			goto abort;
+		}
+
+		data->status = status_reg;
+
+		for (i = 0; i < 2; i++) {
+			res = i2c_smbus_read_byte_data(client,
+					MAX6639_REG_FAN_CNT(i));
+			if (res < 0) {
+				ret = ERR_PTR(res);
+				goto abort;
+			}
+			data->fan[i] = res;
+
+			res = i2c_smbus_read_byte_data(client,
+					MAX6639_REG_TEMP_EXT(i));
+			if (res < 0) {
+				ret = ERR_PTR(res);
+				goto abort;
+			}
+			data->temp[i] = res >> 5;
+			data->temp_fault[i] = res & 0x01;
+
+			res = i2c_smbus_read_byte_data(client,
+					MAX6639_REG_TEMP(i));
+			if (res < 0) {
+				ret = ERR_PTR(res);
+				goto abort;
+			}
+			data->temp[i] |= res << 3;
+		}
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+abort:
+	mutex_unlock(&data->update_lock);
+
+	return ret;
+}
+
+static ssize_t show_temp_input(struct device *dev,
+			       struct device_attribute *dev_attr, char *buf)
+{
+	long temp;
+	struct max6639_data *data = max6639_update_device(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	temp = data->temp[attr->index] * 125;
+	return sprintf(buf, "%ld\n", temp);
+}
+
+static ssize_t show_temp_fault(struct device *dev,
+			       struct device_attribute *dev_attr, char *buf)
+{
+	struct max6639_data *data = max6639_update_device(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	return sprintf(buf, "%d\n", data->temp_fault[attr->index]);
+}
+
+static ssize_t show_temp_max(struct device *dev,
+			     struct device_attribute *dev_attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	return sprintf(buf, "%d\n", (data->temp_therm[attr->index] * 1000));
+}
+
+static ssize_t set_temp_max(struct device *dev,
+			    struct device_attribute *dev_attr,
+			    const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+	unsigned long val;
+	int res;
+
+	res = strict_strtoul(buf, 10, &val);
+	if (res)
+		return res;
+
+	mutex_lock(&data->update_lock);
+	data->temp_therm[attr->index] = TEMP_LIMIT_TO_REG(val);
+	i2c_smbus_write_byte_data(client,
+				  MAX6639_REG_THERM_LIMIT(attr->index),
+				  data->temp_therm[attr->index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_temp_crit(struct device *dev,
+			      struct device_attribute *dev_attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	return sprintf(buf, "%d\n", (data->temp_alert[attr->index] * 1000));
+}
+
+static ssize_t set_temp_crit(struct device *dev,
+			     struct device_attribute *dev_attr,
+			     const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+	unsigned long val;
+	int res;
+
+	res = strict_strtoul(buf, 10, &val);
+	if (res)
+		return res;
+
+	mutex_lock(&data->update_lock);
+	data->temp_alert[attr->index] = TEMP_LIMIT_TO_REG(val);
+	i2c_smbus_write_byte_data(client,
+				  MAX6639_REG_ALERT_LIMIT(attr->index),
+				  data->temp_alert[attr->index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_temp_emergency(struct device *dev,
+				   struct device_attribute *dev_attr,
+				   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	return sprintf(buf, "%d\n", (data->temp_ot[attr->index] * 1000));
+}
+
+static ssize_t set_temp_emergency(struct device *dev,
+				  struct device_attribute *dev_attr,
+				  const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+	unsigned long val;
+	int res;
+
+	res = strict_strtoul(buf, 10, &val);
+	if (res)
+		return res;
+
+	mutex_lock(&data->update_lock);
+	data->temp_ot[attr->index] = TEMP_LIMIT_TO_REG(val);
+	i2c_smbus_write_byte_data(client,
+				  MAX6639_REG_OT_LIMIT(attr->index),
+				  data->temp_ot[attr->index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_pwm(struct device *dev,
+			struct device_attribute *dev_attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	return sprintf(buf, "%d\n", data->pwm[attr->index] * 255 / 120);
+}
+
+static ssize_t set_pwm(struct device *dev,
+		       struct device_attribute *dev_attr,
+		       const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+	unsigned long val;
+	int res;
+
+	res = strict_strtoul(buf, 10, &val);
+	if (res)
+		return res;
+
+	val = SENSORS_LIMIT(val, 0, 255);
+
+	mutex_lock(&data->update_lock);
+	data->pwm[attr->index] = (u8)(val * 120 / 255);
+	i2c_smbus_write_byte_data(client,
+				  MAX6639_REG_TARGTDUTY(attr->index),
+				  data->pwm[attr->index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_fan_input(struct device *dev,
+			      struct device_attribute *dev_attr, char *buf)
+{
+	struct max6639_data *data = max6639_update_device(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	return sprintf(buf, "%d\n", FAN_FROM_REG(data->fan[attr->index],
+		       data->ppr, data->rpm_range));
+}
+
+static ssize_t show_alarm(struct device *dev,
+			  struct device_attribute *dev_attr, char *buf)
+{
+	struct max6639_data *data = max6639_update_device(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(dev_attr);
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	return sprintf(buf, "%d\n", !!(data->status & (1 << attr->index)));
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_input, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp_input, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_fault, S_IRUGO, show_temp_fault, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_temp_fault, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_max,
+		set_temp_max, 0);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp_max,
+		set_temp_max, 1);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp_crit,
+		set_temp_crit, 0);
+static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp_crit,
+		set_temp_crit, 1);
+static SENSOR_DEVICE_ATTR(temp1_emergency, S_IWUSR | S_IRUGO,
+		show_temp_emergency, set_temp_emergency, 0);
+static SENSOR_DEVICE_ATTR(temp2_emergency, S_IWUSR | S_IRUGO,
+		show_temp_emergency, set_temp_emergency, 1);
+static SENSOR_DEVICE_ATTR(pwm1, S_IWUSR | S_IRUGO, show_pwm, set_pwm, 0);
+static SENSOR_DEVICE_ATTR(pwm2, S_IWUSR | S_IRUGO, show_pwm, set_pwm, 1);
+static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, show_fan_input, NULL, 0);
+static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, show_fan_input, NULL, 1);
+static SENSOR_DEVICE_ATTR(fan1_fault, S_IRUGO, show_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(fan2_fault, S_IRUGO, show_alarm, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 7);
+static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL, 6);
+static SENSOR_DEVICE_ATTR(temp1_emergency_alarm, S_IRUGO, show_alarm, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp2_emergency_alarm, S_IRUGO, show_alarm, NULL, 4);
+
+
+static struct attribute *max6639_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_fault.dev_attr.attr,
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_emergency.dev_attr.attr,
+	&sensor_dev_attr_temp2_emergency.dev_attr.attr,
+	&sensor_dev_attr_pwm1.dev_attr.attr,
+	&sensor_dev_attr_pwm2.dev_attr.attr,
+	&sensor_dev_attr_fan1_input.dev_attr.attr,
+	&sensor_dev_attr_fan2_input.dev_attr.attr,
+	&sensor_dev_attr_fan1_fault.dev_attr.attr,
+	&sensor_dev_attr_fan2_fault.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_emergency_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_emergency_alarm.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group max6639_group = {
+	.attrs = max6639_attributes,
+};
+
+/*
+ *  returns respective index in rpm_ranges table
+ *  1 by default on invalid range
+ */
+static int rpm_range_to_reg(int range)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(rpm_ranges); i++) {
+		if (rpm_ranges[i] == range)
+			return i;
+	}
+
+	return 1; /* default: 4000 RPM */
+}
+
+static int max6639_init_client(struct i2c_client *client)
+{
+	struct max6639_data *data = i2c_get_clientdata(client);
+	struct max6639_platform_data *max6639_info =
+		client->dev.platform_data;
+	int i = 0;
+	int rpm_range = 1; /* default: 4000 RPM */
+	int err = 0;
+
+	/* Reset chip to default values, see below for GCONFIG setup */
+	err = i2c_smbus_write_byte_data(client, MAX6639_REG_GCONFIG,
+				  MAX6639_GCONFIG_POR);
+	if (err)
+		goto exit;
+
+	/* Fans pulse per revolution is 2 by default */
+	if (max6639_info && max6639_info->ppr > 0 &&
+			max6639_info->ppr < 5)
+		data->ppr = max6639_info->ppr;
+	else
+		data->ppr = 2;
+	data->ppr -= 1;
+	err = i2c_smbus_write_byte_data(client,
+			MAX6639_REG_FAN_PPR(i),
+			data->ppr << 5);
+	if (err)
+		goto exit;
+
+	if (max6639_info)
+		rpm_range = rpm_range_to_reg(max6639_info->rpm_range);
+	data->rpm_range = rpm_range;
+
+	for (i = 0; i < 2; i++) {
+
+		/* Fans config PWM, RPM */
+		err = i2c_smbus_write_byte_data(client,
+			MAX6639_REG_FAN_CONFIG1(i),
+			MAX6639_FAN_CONFIG1_PWM | rpm_range);
+		if (err)
+			goto exit;
+
+		/* Fans PWM polarity high by default */
+		if (max6639_info && max6639_info->pwm_polarity == 0)
+			err = i2c_smbus_write_byte_data(client,
+				MAX6639_REG_FAN_CONFIG2a(i), 0x00);
+		else
+			err = i2c_smbus_write_byte_data(client,
+				MAX6639_REG_FAN_CONFIG2a(i), 0x02);
+		if (err)
+			goto exit;
+
+		/*
+		 * /THERM full speed enable,
+		 * PWM frequency 25kHz, see also GCONFIG below
+		 */
+		err = i2c_smbus_write_byte_data(client,
+			MAX6639_REG_FAN_CONFIG3(i),
+			MAX6639_FAN_CONFIG3_THERM_FULL_SPEED | 0x03);
+		if (err)
+			goto exit;
+
+		/* Max. temp. 80C/90C/100C */
+		data->temp_therm[i] = 80;
+		data->temp_alert[i] = 90;
+		data->temp_ot[i] = 100;
+		err = i2c_smbus_write_byte_data(client,
+				MAX6639_REG_THERM_LIMIT(i),
+				data->temp_therm[i]);
+		if (err)
+			goto exit;
+		err = i2c_smbus_write_byte_data(client,
+				MAX6639_REG_ALERT_LIMIT(i),
+				data->temp_alert[i]);
+		if (err)
+			goto exit;
+		err = i2c_smbus_write_byte_data(client,
+				MAX6639_REG_OT_LIMIT(i), data->temp_ot[i]);
+		if (err)
+			goto exit;
+
+		/* PWM 120/120 (i.e. 100%) */
+		data->pwm[i] = 120;
+		err = i2c_smbus_write_byte_data(client,
+				MAX6639_REG_TARGTDUTY(i), data->pwm[i]);
+		if (err)
+			goto exit;
+	}
+	/* Start monitoring */
+	err = i2c_smbus_write_byte_data(client, MAX6639_REG_GCONFIG,
+		MAX6639_GCONFIG_DISABLE_TIMEOUT | MAX6639_GCONFIG_CH2_LOCAL |
+		MAX6639_GCONFIG_PWM_FREQ_HI);
+exit:
+	return err;
+}
+
+/* Return 0 if detection is successful, -ENODEV otherwise */
+static int max6639_detect(struct i2c_client *client,
+			  struct i2c_board_info *info)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int dev_id, manu_id;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	/* Actual detection via device and manufacturer ID */
+	dev_id = i2c_smbus_read_byte_data(client, MAX6639_REG_DEVID);
+	manu_id = i2c_smbus_read_byte_data(client, MAX6639_REG_MANUID);
+	if (dev_id != 0x58 || manu_id != 0x4D)
+		return -ENODEV;
+
+	strlcpy(info->type, "max6639", I2C_NAME_SIZE);
+
+	return 0;
+}
+
+static int max6639_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct max6639_data *data;
+	int err;
+
+	data = kzalloc(sizeof(struct max6639_data), GFP_KERNEL);
+	if (!data) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/* Initialize the max6639 chip */
+	err = max6639_init_client(client);
+	if (err < 0)
+		goto error_free;
+
+	/* Register sysfs hooks */
+	err = sysfs_create_group(&client->dev.kobj, &max6639_group);
+	if (err)
+		goto error_free;
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto error_remove;
+	}
+
+	dev_info(&client->dev, "temperature sensor and fan control found\n");
+
+	return 0;
+
+error_remove:
+	sysfs_remove_group(&client->dev.kobj, &max6639_group);
+error_free:
+	kfree(data);
+exit:
+	return err;
+}
+
+static int max6639_remove(struct i2c_client *client)
+{
+	struct max6639_data *data = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &max6639_group);
+
+	kfree(data);
+	return 0;
+}
+
+static int max6639_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	int data = i2c_smbus_read_byte_data(client, MAX6639_REG_GCONFIG);
+	if (data < 0)
+		return data;
+
+	return i2c_smbus_write_byte_data(client,
+			MAX6639_REG_GCONFIG, data | MAX6639_GCONFIG_STANDBY);
+}
+
+static int max6639_resume(struct i2c_client *client)
+{
+	int data = i2c_smbus_read_byte_data(client, MAX6639_REG_GCONFIG);
+	if (data < 0)
+		return data;
+
+	return i2c_smbus_write_byte_data(client,
+			MAX6639_REG_GCONFIG, data & ~MAX6639_GCONFIG_STANDBY);
+}
+
+static const struct i2c_device_id max6639_id[] = {
+	{"max6639", 0},
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, max6639_id);
+
+static struct i2c_driver max6639_driver = {
+	.class = I2C_CLASS_HWMON,
+	.driver = {
+		   .name = "max6639",
+		   },
+	.probe = max6639_probe,
+	.remove = max6639_remove,
+	.suspend = max6639_suspend,
+	.resume = max6639_resume,
+	.id_table = max6639_id,
+	.detect = max6639_detect,
+	.address_list = normal_i2c,
+};
+
+static int __init max6639_init(void)
+{
+	return i2c_add_driver(&max6639_driver);
+}
+
+static void __exit max6639_exit(void)
+{
+	i2c_del_driver(&max6639_driver);
+}
+
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("max6639 driver");
+MODULE_LICENSE("GPL");
+
+module_init(max6639_init);
+module_exit(max6639_exit);
diff --git a/drivers/hwmon/max8688.c b/drivers/hwmon/max8688.c
new file mode 100644
index 000000000000..8ebfef2ecf26
--- /dev/null
+++ b/drivers/hwmon/max8688.c
@@ -0,0 +1,158 @@
+/*
+ * Hardware monitoring driver for Maxim MAX8688
+ *
+ * Copyright (c) 2011 Ericsson AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include "pmbus.h"
+
+#define MAX8688_MFG_STATUS		0xd8
+
+#define MAX8688_STATUS_OC_FAULT		(1 << 4)
+#define MAX8688_STATUS_OV_FAULT		(1 << 5)
+#define MAX8688_STATUS_OV_WARNING	(1 << 8)
+#define MAX8688_STATUS_UV_FAULT		(1 << 9)
+#define MAX8688_STATUS_UV_WARNING	(1 << 10)
+#define MAX8688_STATUS_UC_FAULT		(1 << 11)
+#define MAX8688_STATUS_OC_WARNING	(1 << 12)
+#define MAX8688_STATUS_OT_FAULT		(1 << 13)
+#define MAX8688_STATUS_OT_WARNING	(1 << 14)
+
+static int max8688_get_status(struct i2c_client *client, int page, int reg)
+{
+	int ret = 0;
+	int mfg_status;
+
+	if (page)
+		return -EINVAL;
+
+	switch (reg) {
+	case PMBUS_STATUS_VOUT:
+		mfg_status = pmbus_read_word_data(client, 0,
+						  MAX8688_MFG_STATUS);
+		if (mfg_status < 0)
+			return mfg_status;
+		if (mfg_status & MAX8688_STATUS_UV_WARNING)
+			ret |= PB_VOLTAGE_UV_WARNING;
+		if (mfg_status & MAX8688_STATUS_UV_FAULT)
+			ret |= PB_VOLTAGE_UV_FAULT;
+		if (mfg_status & MAX8688_STATUS_OV_WARNING)
+			ret |= PB_VOLTAGE_OV_WARNING;
+		if (mfg_status & MAX8688_STATUS_OV_FAULT)
+			ret |= PB_VOLTAGE_OV_FAULT;
+		break;
+	case PMBUS_STATUS_IOUT:
+		mfg_status = pmbus_read_word_data(client, 0,
+						  MAX8688_MFG_STATUS);
+		if (mfg_status < 0)
+			return mfg_status;
+		if (mfg_status & MAX8688_STATUS_UC_FAULT)
+			ret |= PB_IOUT_UC_FAULT;
+		if (mfg_status & MAX8688_STATUS_OC_WARNING)
+			ret |= PB_IOUT_OC_WARNING;
+		if (mfg_status & MAX8688_STATUS_OC_FAULT)
+			ret |= PB_IOUT_OC_FAULT;
+		break;
+	case PMBUS_STATUS_TEMPERATURE:
+		mfg_status = pmbus_read_word_data(client, 0,
+						  MAX8688_MFG_STATUS);
+		if (mfg_status < 0)
+			return mfg_status;
+		if (mfg_status & MAX8688_STATUS_OT_WARNING)
+			ret |= PB_TEMP_OT_WARNING;
+		if (mfg_status & MAX8688_STATUS_OT_FAULT)
+			ret |= PB_TEMP_OT_FAULT;
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+	return ret;
+}
+
+static struct pmbus_driver_info max8688_info = {
+	.pages = 1,
+	.direct[PSC_VOLTAGE_IN] = true,
+	.direct[PSC_VOLTAGE_OUT] = true,
+	.direct[PSC_TEMPERATURE] = true,
+	.direct[PSC_CURRENT_OUT] = true,
+	.m[PSC_VOLTAGE_IN] = 19995,
+	.b[PSC_VOLTAGE_IN] = 0,
+	.R[PSC_VOLTAGE_IN] = -1,
+	.m[PSC_VOLTAGE_OUT] = 19995,
+	.b[PSC_VOLTAGE_OUT] = 0,
+	.R[PSC_VOLTAGE_OUT] = -1,
+	.m[PSC_CURRENT_OUT] = 23109,
+	.b[PSC_CURRENT_OUT] = 0,
+	.R[PSC_CURRENT_OUT] = -2,
+	.m[PSC_TEMPERATURE] = -7612,
+	.b[PSC_TEMPERATURE] = 335,
+	.R[PSC_TEMPERATURE] = -3,
+	.func[0] = PMBUS_HAVE_VOUT | PMBUS_HAVE_IOUT | PMBUS_HAVE_TEMP
+		| PMBUS_HAVE_STATUS_VOUT | PMBUS_HAVE_STATUS_IOUT
+		| PMBUS_HAVE_STATUS_TEMP,
+	.get_status = max8688_get_status,
+};
+
+static int max8688_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	return pmbus_do_probe(client, id, &max8688_info);
+}
+
+static int max8688_remove(struct i2c_client *client)
+{
+	return pmbus_do_remove(client);
+}
+
+static const struct i2c_device_id max8688_id[] = {
+	{"max8688", 0},
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, max8688_id);
+
+/* This is the driver that will be inserted */
+static struct i2c_driver max8688_driver = {
+	.driver = {
+		   .name = "max8688",
+		   },
+	.probe = max8688_probe,
+	.remove = max8688_remove,
+	.id_table = max8688_id,
+};
+
+static int __init max8688_init(void)
+{
+	return i2c_add_driver(&max8688_driver);
+}
+
+static void __exit max8688_exit(void)
+{
+	i2c_del_driver(&max8688_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck");
+MODULE_DESCRIPTION("PMBus driver for Maxim MAX8688");
+MODULE_LICENSE("GPL");
+module_init(max8688_init);
+module_exit(max8688_exit);
diff --git a/drivers/hwmon/pmbus.c b/drivers/hwmon/pmbus.c
new file mode 100644
index 000000000000..98e2e28899e2
--- /dev/null
+++ b/drivers/hwmon/pmbus.c
@@ -0,0 +1,203 @@
+/*
+ * Hardware monitoring driver for PMBus devices
+ *
+ * Copyright (c) 2010, 2011 Ericsson AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/i2c.h>
+#include "pmbus.h"
+
+/*
+ * Find sensor groups and status registers on each page.
+ */
+static void pmbus_find_sensor_groups(struct i2c_client *client,
+				     struct pmbus_driver_info *info)
+{
+	int page;
+
+	/* Sensors detected on page 0 only */
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_VIN))
+		info->func[0] |= PMBUS_HAVE_VIN;
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_VCAP))
+		info->func[0] |= PMBUS_HAVE_VCAP;
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_IIN))
+		info->func[0] |= PMBUS_HAVE_IIN;
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_PIN))
+		info->func[0] |= PMBUS_HAVE_PIN;
+	if (info->func[0]
+	    && pmbus_check_byte_register(client, 0, PMBUS_STATUS_INPUT))
+		info->func[0] |= PMBUS_HAVE_STATUS_INPUT;
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_FAN_SPEED_1)) {
+		info->func[0] |= PMBUS_HAVE_FAN12;
+		if (pmbus_check_byte_register(client, 0, PMBUS_STATUS_FAN_12))
+			info->func[0] |= PMBUS_HAVE_STATUS_FAN12;
+	}
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_FAN_SPEED_3)) {
+		info->func[0] |= PMBUS_HAVE_FAN34;
+		if (pmbus_check_byte_register(client, 0, PMBUS_STATUS_FAN_34))
+			info->func[0] |= PMBUS_HAVE_STATUS_FAN34;
+	}
+	if (pmbus_check_word_register(client, 0, PMBUS_READ_TEMPERATURE_1)) {
+		info->func[0] |= PMBUS_HAVE_TEMP;
+		if (pmbus_check_byte_register(client, 0,
+					      PMBUS_STATUS_TEMPERATURE))
+			info->func[0] |= PMBUS_HAVE_STATUS_TEMP;
+	}
+
+	/* Sensors detected on all pages */
+	for (page = 0; page < info->pages; page++) {
+		if (pmbus_check_word_register(client, page, PMBUS_READ_VOUT)) {
+			info->func[page] |= PMBUS_HAVE_VOUT;
+			if (pmbus_check_byte_register(client, page,
+						      PMBUS_STATUS_VOUT))
+				info->func[page] |= PMBUS_HAVE_STATUS_VOUT;
+		}
+		if (pmbus_check_word_register(client, page, PMBUS_READ_IOUT)) {
+			info->func[page] |= PMBUS_HAVE_IOUT;
+			if (pmbus_check_byte_register(client, 0,
+						      PMBUS_STATUS_IOUT))
+				info->func[page] |= PMBUS_HAVE_STATUS_IOUT;
+		}
+		if (pmbus_check_word_register(client, page, PMBUS_READ_POUT))
+			info->func[page] |= PMBUS_HAVE_POUT;
+	}
+}
+
+/*
+ * Identify chip parameters.
+ */
+static int pmbus_identify(struct i2c_client *client,
+			  struct pmbus_driver_info *info)
+{
+	if (!info->pages) {
+		/*
+		 * Check if the PAGE command is supported. If it is,
+		 * keep setting the page number until it fails or until the
+		 * maximum number of pages has been reached. Assume that
+		 * this is the number of pages supported by the chip.
+		 */
+		if (pmbus_check_byte_register(client, 0, PMBUS_PAGE)) {
+			int page;
+
+			for (page = 1; page < PMBUS_PAGES; page++) {
+				if (pmbus_set_page(client, page) < 0)
+					break;
+			}
+			pmbus_set_page(client, 0);
+			info->pages = page;
+		} else {
+			info->pages = 1;
+		}
+	}
+
+	/*
+	 * We should check if the COEFFICIENTS register is supported.
+	 * If it is, and the chip is configured for direct mode, we can read
+	 * the coefficients from the chip, one set per group of sensor
+	 * registers.
+	 *
+	 * To do this, we will need access to a chip which actually supports the
+	 * COEFFICIENTS command, since the command is too complex to implement
+	 * without testing it.
+	 */
+
+	/* Try to find sensor groups  */
+	pmbus_find_sensor_groups(client, info);
+
+	return 0;
+}
+
+static int pmbus_probe(struct i2c_client *client,
+		       const struct i2c_device_id *id)
+{
+	struct pmbus_driver_info *info;
+	int ret;
+
+	info = kzalloc(sizeof(struct pmbus_driver_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->pages = id->driver_data;
+	info->identify = pmbus_identify;
+
+	ret = pmbus_do_probe(client, id, info);
+	if (ret < 0)
+		goto out;
+	return 0;
+
+out:
+	kfree(info);
+	return ret;
+}
+
+static int pmbus_remove(struct i2c_client *client)
+{
+	int ret;
+	const struct pmbus_driver_info *info;
+
+	info = pmbus_get_driver_info(client);
+	ret = pmbus_do_remove(client);
+	kfree(info);
+	return ret;
+}
+
+/*
+ * Use driver_data to set the number of pages supported by the chip.
+ */
+static const struct i2c_device_id pmbus_id[] = {
+	{"bmr450", 1},
+	{"bmr451", 1},
+	{"bmr453", 1},
+	{"bmr454", 1},
+	{"ltc2978", 8},
+	{"pmbus", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, pmbus_id);
+
+/* This is the driver that will be inserted */
+static struct i2c_driver pmbus_driver = {
+	.driver = {
+		   .name = "pmbus",
+		   },
+	.probe = pmbus_probe,
+	.remove = pmbus_remove,
+	.id_table = pmbus_id,
+};
+
+static int __init pmbus_init(void)
+{
+	return i2c_add_driver(&pmbus_driver);
+}
+
+static void __exit pmbus_exit(void)
+{
+	i2c_del_driver(&pmbus_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck");
+MODULE_DESCRIPTION("Generic PMBus driver");
+MODULE_LICENSE("GPL");
+module_init(pmbus_init);
+module_exit(pmbus_exit);
diff --git a/drivers/hwmon/pmbus.h b/drivers/hwmon/pmbus.h
new file mode 100644
index 000000000000..a81f7f228762
--- /dev/null
+++ b/drivers/hwmon/pmbus.h
@@ -0,0 +1,313 @@
+/*
+ * pmbus.h - Common defines and structures for PMBus devices
+ *
+ * Copyright (c) 2010, 2011 Ericsson AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef PMBUS_H
+#define PMBUS_H
+
+/*
+ * Registers
+ */
+#define PMBUS_PAGE			0x00
+#define PMBUS_OPERATION			0x01
+#define PMBUS_ON_OFF_CONFIG		0x02
+#define PMBUS_CLEAR_FAULTS		0x03
+#define PMBUS_PHASE			0x04
+
+#define PMBUS_CAPABILITY		0x19
+#define PMBUS_QUERY			0x1A
+
+#define PMBUS_VOUT_MODE			0x20
+#define PMBUS_VOUT_COMMAND		0x21
+#define PMBUS_VOUT_TRIM			0x22
+#define PMBUS_VOUT_CAL_OFFSET		0x23
+#define PMBUS_VOUT_MAX			0x24
+#define PMBUS_VOUT_MARGIN_HIGH		0x25
+#define PMBUS_VOUT_MARGIN_LOW		0x26
+#define PMBUS_VOUT_TRANSITION_RATE	0x27
+#define PMBUS_VOUT_DROOP		0x28
+#define PMBUS_VOUT_SCALE_LOOP		0x29
+#define PMBUS_VOUT_SCALE_MONITOR	0x2A
+
+#define PMBUS_COEFFICIENTS		0x30
+#define PMBUS_POUT_MAX			0x31
+
+#define PMBUS_FAN_CONFIG_12		0x3A
+#define PMBUS_FAN_COMMAND_1		0x3B
+#define PMBUS_FAN_COMMAND_2		0x3C
+#define PMBUS_FAN_CONFIG_34		0x3D
+#define PMBUS_FAN_COMMAND_3		0x3E
+#define PMBUS_FAN_COMMAND_4		0x3F
+
+#define PMBUS_VOUT_OV_FAULT_LIMIT	0x40
+#define PMBUS_VOUT_OV_FAULT_RESPONSE	0x41
+#define PMBUS_VOUT_OV_WARN_LIMIT	0x42
+#define PMBUS_VOUT_UV_WARN_LIMIT	0x43
+#define PMBUS_VOUT_UV_FAULT_LIMIT	0x44
+#define PMBUS_VOUT_UV_FAULT_RESPONSE	0x45
+#define PMBUS_IOUT_OC_FAULT_LIMIT	0x46
+#define PMBUS_IOUT_OC_FAULT_RESPONSE	0x47
+#define PMBUS_IOUT_OC_LV_FAULT_LIMIT	0x48
+#define PMBUS_IOUT_OC_LV_FAULT_RESPONSE	0x49
+#define PMBUS_IOUT_OC_WARN_LIMIT	0x4A
+#define PMBUS_IOUT_UC_FAULT_LIMIT	0x4B
+#define PMBUS_IOUT_UC_FAULT_RESPONSE	0x4C
+
+#define PMBUS_OT_FAULT_LIMIT		0x4F
+#define PMBUS_OT_FAULT_RESPONSE		0x50
+#define PMBUS_OT_WARN_LIMIT		0x51
+#define PMBUS_UT_WARN_LIMIT		0x52
+#define PMBUS_UT_FAULT_LIMIT		0x53
+#define PMBUS_UT_FAULT_RESPONSE		0x54
+#define PMBUS_VIN_OV_FAULT_LIMIT	0x55
+#define PMBUS_VIN_OV_FAULT_RESPONSE	0x56
+#define PMBUS_VIN_OV_WARN_LIMIT		0x57
+#define PMBUS_VIN_UV_WARN_LIMIT		0x58
+#define PMBUS_VIN_UV_FAULT_LIMIT	0x59
+
+#define PMBUS_IIN_OC_FAULT_LIMIT	0x5B
+#define PMBUS_IIN_OC_WARN_LIMIT		0x5D
+
+#define PMBUS_POUT_OP_FAULT_LIMIT	0x68
+#define PMBUS_POUT_OP_WARN_LIMIT	0x6A
+#define PMBUS_PIN_OP_WARN_LIMIT		0x6B
+
+#define PMBUS_STATUS_BYTE		0x78
+#define PMBUS_STATUS_WORD		0x79
+#define PMBUS_STATUS_VOUT		0x7A
+#define PMBUS_STATUS_IOUT		0x7B
+#define PMBUS_STATUS_INPUT		0x7C
+#define PMBUS_STATUS_TEMPERATURE	0x7D
+#define PMBUS_STATUS_CML		0x7E
+#define PMBUS_STATUS_OTHER		0x7F
+#define PMBUS_STATUS_MFR_SPECIFIC	0x80
+#define PMBUS_STATUS_FAN_12		0x81
+#define PMBUS_STATUS_FAN_34		0x82
+
+#define PMBUS_READ_VIN			0x88
+#define PMBUS_READ_IIN			0x89
+#define PMBUS_READ_VCAP			0x8A
+#define PMBUS_READ_VOUT			0x8B
+#define PMBUS_READ_IOUT			0x8C
+#define PMBUS_READ_TEMPERATURE_1	0x8D
+#define PMBUS_READ_TEMPERATURE_2	0x8E
+#define PMBUS_READ_TEMPERATURE_3	0x8F
+#define PMBUS_READ_FAN_SPEED_1		0x90
+#define PMBUS_READ_FAN_SPEED_2		0x91
+#define PMBUS_READ_FAN_SPEED_3		0x92
+#define PMBUS_READ_FAN_SPEED_4		0x93
+#define PMBUS_READ_DUTY_CYCLE		0x94
+#define PMBUS_READ_FREQUENCY		0x95
+#define PMBUS_READ_POUT			0x96
+#define PMBUS_READ_PIN			0x97
+
+#define PMBUS_REVISION			0x98
+#define PMBUS_MFR_ID			0x99
+#define PMBUS_MFR_MODEL			0x9A
+#define PMBUS_MFR_REVISION		0x9B
+#define PMBUS_MFR_LOCATION		0x9C
+#define PMBUS_MFR_DATE			0x9D
+#define PMBUS_MFR_SERIAL		0x9E
+
+/*
+ * CAPABILITY
+ */
+#define PB_CAPABILITY_SMBALERT		(1<<4)
+#define PB_CAPABILITY_ERROR_CHECK	(1<<7)
+
+/*
+ * VOUT_MODE
+ */
+#define PB_VOUT_MODE_MODE_MASK		0xe0
+#define PB_VOUT_MODE_PARAM_MASK		0x1f
+
+#define PB_VOUT_MODE_LINEAR		0x00
+#define PB_VOUT_MODE_VID		0x20
+#define PB_VOUT_MODE_DIRECT		0x40
+
+/*
+ * Fan configuration
+ */
+#define PB_FAN_2_PULSE_MASK		((1 << 0) | (1 << 1))
+#define PB_FAN_2_RPM			(1 << 2)
+#define PB_FAN_2_INSTALLED		(1 << 3)
+#define PB_FAN_1_PULSE_MASK		((1 << 4) | (1 << 5))
+#define PB_FAN_1_RPM			(1 << 6)
+#define PB_FAN_1_INSTALLED		(1 << 7)
+
+/*
+ * STATUS_BYTE, STATUS_WORD (lower)
+ */
+#define PB_STATUS_NONE_ABOVE		(1<<0)
+#define PB_STATUS_CML			(1<<1)
+#define PB_STATUS_TEMPERATURE		(1<<2)
+#define PB_STATUS_VIN_UV		(1<<3)
+#define PB_STATUS_IOUT_OC		(1<<4)
+#define PB_STATUS_VOUT_OV		(1<<5)
+#define PB_STATUS_OFF			(1<<6)
+#define PB_STATUS_BUSY			(1<<7)
+
+/*
+ * STATUS_WORD (upper)
+ */
+#define PB_STATUS_UNKNOWN		(1<<8)
+#define PB_STATUS_OTHER			(1<<9)
+#define PB_STATUS_FANS			(1<<10)
+#define PB_STATUS_POWER_GOOD_N		(1<<11)
+#define PB_STATUS_WORD_MFR		(1<<12)
+#define PB_STATUS_INPUT			(1<<13)
+#define PB_STATUS_IOUT_POUT		(1<<14)
+#define PB_STATUS_VOUT			(1<<15)
+
+/*
+ * STATUS_IOUT
+ */
+#define PB_POUT_OP_WARNING		(1<<0)
+#define PB_POUT_OP_FAULT		(1<<1)
+#define PB_POWER_LIMITING		(1<<2)
+#define PB_CURRENT_SHARE_FAULT		(1<<3)
+#define PB_IOUT_UC_FAULT		(1<<4)
+#define PB_IOUT_OC_WARNING		(1<<5)
+#define PB_IOUT_OC_LV_FAULT		(1<<6)
+#define PB_IOUT_OC_FAULT		(1<<7)
+
+/*
+ * STATUS_VOUT, STATUS_INPUT
+ */
+#define PB_VOLTAGE_UV_FAULT		(1<<4)
+#define PB_VOLTAGE_UV_WARNING		(1<<5)
+#define PB_VOLTAGE_OV_WARNING		(1<<6)
+#define PB_VOLTAGE_OV_FAULT		(1<<7)
+
+/*
+ * STATUS_INPUT
+ */
+#define PB_PIN_OP_WARNING		(1<<0)
+#define PB_IIN_OC_WARNING		(1<<1)
+#define PB_IIN_OC_FAULT			(1<<2)
+
+/*
+ * STATUS_TEMPERATURE
+ */
+#define PB_TEMP_UT_FAULT		(1<<4)
+#define PB_TEMP_UT_WARNING		(1<<5)
+#define PB_TEMP_OT_WARNING		(1<<6)
+#define PB_TEMP_OT_FAULT		(1<<7)
+
+/*
+ * STATUS_FAN
+ */
+#define PB_FAN_AIRFLOW_WARNING		(1<<0)
+#define PB_FAN_AIRFLOW_FAULT		(1<<1)
+#define PB_FAN_FAN2_SPEED_OVERRIDE	(1<<2)
+#define PB_FAN_FAN1_SPEED_OVERRIDE	(1<<3)
+#define PB_FAN_FAN2_WARNING		(1<<4)
+#define PB_FAN_FAN1_WARNING		(1<<5)
+#define PB_FAN_FAN2_FAULT		(1<<6)
+#define PB_FAN_FAN1_FAULT		(1<<7)
+
+/*
+ * CML_FAULT_STATUS
+ */
+#define PB_CML_FAULT_OTHER_MEM_LOGIC	(1<<0)
+#define PB_CML_FAULT_OTHER_COMM		(1<<1)
+#define PB_CML_FAULT_PROCESSOR		(1<<3)
+#define PB_CML_FAULT_MEMORY		(1<<4)
+#define PB_CML_FAULT_PACKET_ERROR	(1<<5)
+#define PB_CML_FAULT_INVALID_DATA	(1<<6)
+#define PB_CML_FAULT_INVALID_COMMAND	(1<<7)
+
+enum pmbus_sensor_classes {
+	PSC_VOLTAGE_IN = 0,
+	PSC_VOLTAGE_OUT,
+	PSC_CURRENT_IN,
+	PSC_CURRENT_OUT,
+	PSC_POWER,
+	PSC_TEMPERATURE,
+	PSC_FAN,
+	PSC_NUM_CLASSES		/* Number of power sensor classes */
+};
+
+#define PMBUS_PAGES	32	/* Per PMBus specification */
+
+/* Functionality bit mask */
+#define PMBUS_HAVE_VIN		(1 << 0)
+#define PMBUS_HAVE_VCAP		(1 << 1)
+#define PMBUS_HAVE_VOUT		(1 << 2)
+#define PMBUS_HAVE_IIN		(1 << 3)
+#define PMBUS_HAVE_IOUT		(1 << 4)
+#define PMBUS_HAVE_PIN		(1 << 5)
+#define PMBUS_HAVE_POUT		(1 << 6)
+#define PMBUS_HAVE_FAN12	(1 << 7)
+#define PMBUS_HAVE_FAN34	(1 << 8)
+#define PMBUS_HAVE_TEMP		(1 << 9)
+#define PMBUS_HAVE_TEMP2	(1 << 10)
+#define PMBUS_HAVE_TEMP3	(1 << 11)
+#define PMBUS_HAVE_STATUS_VOUT	(1 << 12)
+#define PMBUS_HAVE_STATUS_IOUT	(1 << 13)
+#define PMBUS_HAVE_STATUS_INPUT	(1 << 14)
+#define PMBUS_HAVE_STATUS_TEMP	(1 << 15)
+#define PMBUS_HAVE_STATUS_FAN12	(1 << 16)
+#define PMBUS_HAVE_STATUS_FAN34	(1 << 17)
+
+struct pmbus_driver_info {
+	int pages;		/* Total number of pages */
+	bool direct[PSC_NUM_CLASSES];
+				/* true if device uses direct data format
+				   for the given sensor class */
+	/*
+	 * Support one set of coefficients for each sensor type
+	 * Used for chips providing data in direct mode.
+	 */
+	int m[PSC_NUM_CLASSES];	/* mantissa for direct data format */
+	int b[PSC_NUM_CLASSES];	/* offset */
+	int R[PSC_NUM_CLASSES];	/* exponent */
+
+	u32 func[PMBUS_PAGES];	/* Functionality, per page */
+	/*
+	 * The get_status function maps manufacturing specific status values
+	 * into PMBus standard status values.
+	 * This function is optional and only necessary if chip specific status
+	 * register values have to be mapped into standard PMBus status register
+	 * values.
+	 */
+	int (*get_status)(struct i2c_client *client, int page, int reg);
+	/*
+	 * The identify function determines supported PMBus functionality.
+	 * This function is only necessary if a chip driver supports multiple
+	 * chips, and the chip functionality is not pre-determined.
+	 */
+	int (*identify)(struct i2c_client *client,
+			struct pmbus_driver_info *info);
+};
+
+/* Function declarations */
+
+int pmbus_set_page(struct i2c_client *client, u8 page);
+int pmbus_read_word_data(struct i2c_client *client, u8 page, u8 reg);
+void pmbus_clear_faults(struct i2c_client *client);
+bool pmbus_check_byte_register(struct i2c_client *client, int page, int reg);
+bool pmbus_check_word_register(struct i2c_client *client, int page, int reg);
+int pmbus_do_probe(struct i2c_client *client, const struct i2c_device_id *id,
+		   struct pmbus_driver_info *info);
+int pmbus_do_remove(struct i2c_client *client);
+const struct pmbus_driver_info *pmbus_get_driver_info(struct i2c_client
+						      *client);
+
+#endif /* PMBUS_H */
diff --git a/drivers/hwmon/pmbus_core.c b/drivers/hwmon/pmbus_core.c
new file mode 100644
index 000000000000..6474512f49b0
--- /dev/null
+++ b/drivers/hwmon/pmbus_core.c
@@ -0,0 +1,1658 @@
+/*
+ * Hardware monitoring driver for PMBus devices
+ *
+ * Copyright (c) 2010, 2011 Ericsson AB.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/delay.h>
+#include <linux/i2c/pmbus.h>
+#include "pmbus.h"
+
+/*
+ * Constants needed to determine number of sensors, booleans, and labels.
+ */
+#define PMBUS_MAX_INPUT_SENSORS		11	/* 6*volt, 3*curr, 2*power */
+#define PMBUS_VOUT_SENSORS_PER_PAGE	5	/* input, min, max, lcrit,
+						   crit */
+#define PMBUS_IOUT_SENSORS_PER_PAGE	4	/* input, min, max, crit */
+#define PMBUS_POUT_SENSORS_PER_PAGE	4	/* input, cap, max, crit */
+#define PMBUS_MAX_SENSORS_PER_FAN	1	/* input */
+#define PMBUS_MAX_SENSORS_PER_TEMP	5	/* input, min, max, lcrit,
+						   crit */
+
+#define PMBUS_MAX_INPUT_BOOLEANS	7	/* v: min_alarm, max_alarm,
+						   lcrit_alarm, crit_alarm;
+						   c: alarm, crit_alarm;
+						   p: crit_alarm */
+#define PMBUS_VOUT_BOOLEANS_PER_PAGE	4	/* min_alarm, max_alarm,
+						   lcrit_alarm, crit_alarm */
+#define PMBUS_IOUT_BOOLEANS_PER_PAGE	3	/* alarm, lcrit_alarm,
+						   crit_alarm */
+#define PMBUS_POUT_BOOLEANS_PER_PAGE	2	/* alarm, crit_alarm */
+#define PMBUS_MAX_BOOLEANS_PER_FAN	2	/* alarm, fault */
+#define PMBUS_MAX_BOOLEANS_PER_TEMP	4	/* min_alarm, max_alarm,
+						   lcrit_alarm, crit_alarm */
+
+#define PMBUS_MAX_INPUT_LABELS		4	/* vin, vcap, iin, pin */
+
+/*
+ * status, status_vout, status_iout, status_fans, status_fan34, and status_temp
+ * are paged. status_input is unpaged.
+ */
+#define PB_NUM_STATUS_REG	(PMBUS_PAGES * 6 + 1)
+
+/*
+ * Index into status register array, per status register group
+ */
+#define PB_STATUS_BASE		0
+#define PB_STATUS_VOUT_BASE	(PB_STATUS_BASE + PMBUS_PAGES)
+#define PB_STATUS_IOUT_BASE	(PB_STATUS_VOUT_BASE + PMBUS_PAGES)
+#define PB_STATUS_FAN_BASE	(PB_STATUS_IOUT_BASE + PMBUS_PAGES)
+#define PB_STATUS_FAN34_BASE	(PB_STATUS_FAN_BASE + PMBUS_PAGES)
+#define PB_STATUS_INPUT_BASE	(PB_STATUS_FAN34_BASE + PMBUS_PAGES)
+#define PB_STATUS_TEMP_BASE	(PB_STATUS_INPUT_BASE + 1)
+
+struct pmbus_sensor {
+	char name[I2C_NAME_SIZE];	/* sysfs sensor name */
+	struct sensor_device_attribute attribute;
+	u8 page;		/* page number */
+	u8 reg;			/* register */
+	enum pmbus_sensor_classes class;	/* sensor class */
+	bool update;		/* runtime sensor update needed */
+	int data;		/* Sensor data.
+				   Negative if there was a read error */
+};
+
+struct pmbus_boolean {
+	char name[I2C_NAME_SIZE];	/* sysfs boolean name */
+	struct sensor_device_attribute attribute;
+};
+
+struct pmbus_label {
+	char name[I2C_NAME_SIZE];	/* sysfs label name */
+	struct sensor_device_attribute attribute;
+	char label[I2C_NAME_SIZE];	/* label */
+};
+
+struct pmbus_data {
+	struct device *hwmon_dev;
+
+	u32 flags;		/* from platform data */
+
+	int exponent;		/* linear mode: exponent for output voltages */
+
+	const struct pmbus_driver_info *info;
+
+	int max_attributes;
+	int num_attributes;
+	struct attribute **attributes;
+	struct attribute_group group;
+
+	/*
+	 * Sensors cover both sensor and limit registers.
+	 */
+	int max_sensors;
+	int num_sensors;
+	struct pmbus_sensor *sensors;
+	/*
+	 * Booleans are used for alarms.
+	 * Values are determined from status registers.
+	 */
+	int max_booleans;
+	int num_booleans;
+	struct pmbus_boolean *booleans;
+	/*
+	 * Labels are used to map generic names (e.g., "in1")
+	 * to PMBus specific names (e.g., "vin" or "vout1").
+	 */
+	int max_labels;
+	int num_labels;
+	struct pmbus_label *labels;
+
+	struct mutex update_lock;
+	bool valid;
+	unsigned long last_updated;	/* in jiffies */
+
+	/*
+	 * A single status register covers multiple attributes,
+	 * so we keep them all together.
+	 */
+	u8 status_bits;
+	u8 status[PB_NUM_STATUS_REG];
+
+	u8 currpage;
+};
+
+int pmbus_set_page(struct i2c_client *client, u8 page)
+{
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	int rv = 0;
+	int newpage;
+
+	if (page != data->currpage) {
+		rv = i2c_smbus_write_byte_data(client, PMBUS_PAGE, page);
+		newpage = i2c_smbus_read_byte_data(client, PMBUS_PAGE);
+		if (newpage != page)
+			rv = -EINVAL;
+		else
+			data->currpage = page;
+	}
+	return rv;
+}
+EXPORT_SYMBOL_GPL(pmbus_set_page);
+
+static int pmbus_write_byte(struct i2c_client *client, u8 page, u8 value)
+{
+	int rv;
+
+	rv = pmbus_set_page(client, page);
+	if (rv < 0)
+		return rv;
+
+	return i2c_smbus_write_byte(client, value);
+}
+
+static int pmbus_write_word_data(struct i2c_client *client, u8 page, u8 reg,
+				 u16 word)
+{
+	int rv;
+
+	rv = pmbus_set_page(client, page);
+	if (rv < 0)
+		return rv;
+
+	return i2c_smbus_write_word_data(client, reg, word);
+}
+
+int pmbus_read_word_data(struct i2c_client *client, u8 page, u8 reg)
+{
+	int rv;
+
+	rv = pmbus_set_page(client, page);
+	if (rv < 0)
+		return rv;
+
+	return i2c_smbus_read_word_data(client, reg);
+}
+EXPORT_SYMBOL_GPL(pmbus_read_word_data);
+
+static int pmbus_read_byte_data(struct i2c_client *client, u8 page, u8 reg)
+{
+	int rv;
+
+	rv = pmbus_set_page(client, page);
+	if (rv < 0)
+		return rv;
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+static void pmbus_clear_fault_page(struct i2c_client *client, int page)
+{
+	pmbus_write_byte(client, page, PMBUS_CLEAR_FAULTS);
+}
+
+void pmbus_clear_faults(struct i2c_client *client)
+{
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	int i;
+
+	for (i = 0; i < data->info->pages; i++)
+		pmbus_clear_fault_page(client, i);
+}
+EXPORT_SYMBOL_GPL(pmbus_clear_faults);
+
+static int pmbus_check_status_cml(struct i2c_client *client, int page)
+{
+	int status, status2;
+
+	status = pmbus_read_byte_data(client, page, PMBUS_STATUS_BYTE);
+	if (status < 0 || (status & PB_STATUS_CML)) {
+		status2 = pmbus_read_byte_data(client, page, PMBUS_STATUS_CML);
+		if (status2 < 0 || (status2 & PB_CML_FAULT_INVALID_COMMAND))
+			return -EINVAL;
+	}
+	return 0;
+}
+
+bool pmbus_check_byte_register(struct i2c_client *client, int page, int reg)
+{
+	int rv;
+	struct pmbus_data *data = i2c_get_clientdata(client);
+
+	rv = pmbus_read_byte_data(client, page, reg);
+	if (rv >= 0 && !(data->flags & PMBUS_SKIP_STATUS_CHECK))
+		rv = pmbus_check_status_cml(client, page);
+	pmbus_clear_fault_page(client, page);
+	return rv >= 0;
+}
+EXPORT_SYMBOL_GPL(pmbus_check_byte_register);
+
+bool pmbus_check_word_register(struct i2c_client *client, int page, int reg)
+{
+	int rv;
+	struct pmbus_data *data = i2c_get_clientdata(client);
+
+	rv = pmbus_read_word_data(client, page, reg);
+	if (rv >= 0 && !(data->flags & PMBUS_SKIP_STATUS_CHECK))
+		rv = pmbus_check_status_cml(client, page);
+	pmbus_clear_fault_page(client, page);
+	return rv >= 0;
+}
+EXPORT_SYMBOL_GPL(pmbus_check_word_register);
+
+const struct pmbus_driver_info *pmbus_get_driver_info(struct i2c_client *client)
+{
+	struct pmbus_data *data = i2c_get_clientdata(client);
+
+	return data->info;
+}
+EXPORT_SYMBOL_GPL(pmbus_get_driver_info);
+
+static int pmbus_get_status(struct i2c_client *client, int page, int reg)
+{
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	const struct pmbus_driver_info *info = data->info;
+	int status;
+
+	if (info->get_status) {
+		status = info->get_status(client, page, reg);
+		if (status != -ENODATA)
+			return status;
+	}
+	return  pmbus_read_byte_data(client, page, reg);
+}
+
+static struct pmbus_data *pmbus_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	const struct pmbus_driver_info *info = data->info;
+
+	mutex_lock(&data->update_lock);
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+		int i;
+
+		for (i = 0; i < info->pages; i++)
+			data->status[PB_STATUS_BASE + i]
+			    = pmbus_read_byte_data(client, i,
+						   PMBUS_STATUS_BYTE);
+		for (i = 0; i < info->pages; i++) {
+			if (!(info->func[i] & PMBUS_HAVE_STATUS_VOUT))
+				continue;
+			data->status[PB_STATUS_VOUT_BASE + i]
+			  = pmbus_get_status(client, i, PMBUS_STATUS_VOUT);
+		}
+		for (i = 0; i < info->pages; i++) {
+			if (!(info->func[i] & PMBUS_HAVE_STATUS_IOUT))
+				continue;
+			data->status[PB_STATUS_IOUT_BASE + i]
+			  = pmbus_get_status(client, i, PMBUS_STATUS_IOUT);
+		}
+		for (i = 0; i < info->pages; i++) {
+			if (!(info->func[i] & PMBUS_HAVE_STATUS_TEMP))
+				continue;
+			data->status[PB_STATUS_TEMP_BASE + i]
+			  = pmbus_get_status(client, i,
+					     PMBUS_STATUS_TEMPERATURE);
+		}
+		for (i = 0; i < info->pages; i++) {
+			if (!(info->func[i] & PMBUS_HAVE_STATUS_FAN12))
+				continue;
+			data->status[PB_STATUS_FAN_BASE + i]
+			  = pmbus_get_status(client, i, PMBUS_STATUS_FAN_12);
+		}
+
+		for (i = 0; i < info->pages; i++) {
+			if (!(info->func[i] & PMBUS_HAVE_STATUS_FAN34))
+				continue;
+			data->status[PB_STATUS_FAN34_BASE + i]
+			  = pmbus_get_status(client, i, PMBUS_STATUS_FAN_34);
+		}
+
+		if (info->func[0] & PMBUS_HAVE_STATUS_INPUT)
+			data->status[PB_STATUS_INPUT_BASE]
+			  = pmbus_get_status(client, 0, PMBUS_STATUS_INPUT);
+
+		for (i = 0; i < data->num_sensors; i++) {
+			struct pmbus_sensor *sensor = &data->sensors[i];
+
+			if (!data->valid || sensor->update)
+				sensor->data
+				    = pmbus_read_word_data(client, sensor->page,
+							   sensor->reg);
+		}
+		pmbus_clear_faults(client);
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+	mutex_unlock(&data->update_lock);
+	return data;
+}
+
+/*
+ * Convert linear sensor values to milli- or micro-units
+ * depending on sensor type.
+ */
+static int pmbus_reg2data_linear(struct pmbus_data *data,
+				 struct pmbus_sensor *sensor)
+{
+	s16 exponent;
+	s32 mantissa;
+	long val;
+
+	if (sensor->class == PSC_VOLTAGE_OUT) {	/* LINEAR16 */
+		exponent = data->exponent;
+		mantissa = (u16) sensor->data;
+	} else {				/* LINEAR11 */
+		exponent = (sensor->data >> 11) & 0x001f;
+		mantissa = sensor->data & 0x07ff;
+
+		if (exponent > 0x0f)
+			exponent |= 0xffe0;	/* sign extend exponent */
+		if (mantissa > 0x03ff)
+			mantissa |= 0xfffff800;	/* sign extend mantissa */
+	}
+
+	val = mantissa;
+
+	/* scale result to milli-units for all sensors except fans */
+	if (sensor->class != PSC_FAN)
+		val = val * 1000L;
+
+	/* scale result to micro-units for power sensors */
+	if (sensor->class == PSC_POWER)
+		val = val * 1000L;
+
+	if (exponent >= 0)
+		val <<= exponent;
+	else
+		val >>= -exponent;
+
+	return (int)val;
+}
+
+/*
+ * Convert direct sensor values to milli- or micro-units
+ * depending on sensor type.
+ */
+static int pmbus_reg2data_direct(struct pmbus_data *data,
+				 struct pmbus_sensor *sensor)
+{
+	long val = (s16) sensor->data;
+	long m, b, R;
+
+	m = data->info->m[sensor->class];
+	b = data->info->b[sensor->class];
+	R = data->info->R[sensor->class];
+
+	if (m == 0)
+		return 0;
+
+	/* X = 1/m * (Y * 10^-R - b) */
+	R = -R;
+	/* scale result to milli-units for everything but fans */
+	if (sensor->class != PSC_FAN) {
+		R += 3;
+		b *= 1000;
+	}
+
+	/* scale result to micro-units for power sensors */
+	if (sensor->class == PSC_POWER) {
+		R += 3;
+		b *= 1000;
+	}
+
+	while (R > 0) {
+		val *= 10;
+		R--;
+	}
+	while (R < 0) {
+		val = DIV_ROUND_CLOSEST(val, 10);
+		R++;
+	}
+
+	return (int)((val - b) / m);
+}
+
+static int pmbus_reg2data(struct pmbus_data *data, struct pmbus_sensor *sensor)
+{
+	int val;
+
+	if (data->info->direct[sensor->class])
+		val = pmbus_reg2data_direct(data, sensor);
+	else
+		val = pmbus_reg2data_linear(data, sensor);
+
+	return val;
+}
+
+#define MAX_MANTISSA	(1023 * 1000)
+#define MIN_MANTISSA	(511 * 1000)
+
+static u16 pmbus_data2reg_linear(struct pmbus_data *data,
+				 enum pmbus_sensor_classes class, long val)
+{
+	s16 exponent = 0, mantissa;
+	bool negative = false;
+
+	/* simple case */
+	if (val == 0)
+		return 0;
+
+	if (class == PSC_VOLTAGE_OUT) {
+		/* LINEAR16 does not support negative voltages */
+		if (val < 0)
+			return 0;
+
+		/*
+		 * For a static exponents, we don't have a choice
+		 * but to adjust the value to it.
+		 */
+		if (data->exponent < 0)
+			val <<= -data->exponent;
+		else
+			val >>= data->exponent;
+		val = DIV_ROUND_CLOSEST(val, 1000);
+		return val & 0xffff;
+	}
+
+	if (val < 0) {
+		negative = true;
+		val = -val;
+	}
+
+	/* Power is in uW. Convert to mW before converting. */
+	if (class == PSC_POWER)
+		val = DIV_ROUND_CLOSEST(val, 1000L);
+
+	/*
+	 * For simplicity, convert fan data to milli-units
+	 * before calculating the exponent.
+	 */
+	if (class == PSC_FAN)
+		val = val * 1000;
+
+	/* Reduce large mantissa until it fits into 10 bit */
+	while (val >= MAX_MANTISSA && exponent < 15) {
+		exponent++;
+		val >>= 1;
+	}
+	/* Increase small mantissa to improve precision */
+	while (val < MIN_MANTISSA && exponent > -15) {
+		exponent--;
+		val <<= 1;
+	}
+
+	/* Convert mantissa from milli-units to units */
+	mantissa = DIV_ROUND_CLOSEST(val, 1000);
+
+	/* Ensure that resulting number is within range */
+	if (mantissa > 0x3ff)
+		mantissa = 0x3ff;
+
+	/* restore sign */
+	if (negative)
+		mantissa = -mantissa;
+
+	/* Convert to 5 bit exponent, 11 bit mantissa */
+	return (mantissa & 0x7ff) | ((exponent << 11) & 0xf800);
+}
+
+static u16 pmbus_data2reg_direct(struct pmbus_data *data,
+				 enum pmbus_sensor_classes class, long val)
+{
+	long m, b, R;
+
+	m = data->info->m[class];
+	b = data->info->b[class];
+	R = data->info->R[class];
+
+	/* Power is in uW. Adjust R and b. */
+	if (class == PSC_POWER) {
+		R -= 3;
+		b *= 1000;
+	}
+
+	/* Calculate Y = (m * X + b) * 10^R */
+	if (class != PSC_FAN) {
+		R -= 3;		/* Adjust R and b for data in milli-units */
+		b *= 1000;
+	}
+	val = val * m + b;
+
+	while (R > 0) {
+		val *= 10;
+		R--;
+	}
+	while (R < 0) {
+		val = DIV_ROUND_CLOSEST(val, 10);
+		R++;
+	}
+
+	return val;
+}
+
+static u16 pmbus_data2reg(struct pmbus_data *data,
+			  enum pmbus_sensor_classes class, long val)
+{
+	u16 regval;
+
+	if (data->info->direct[class])
+		regval = pmbus_data2reg_direct(data, class, val);
+	else
+		regval = pmbus_data2reg_linear(data, class, val);
+
+	return regval;
+}
+
+/*
+ * Return boolean calculated from converted data.
+ * <index> defines a status register index and mask, and optionally
+ * two sensor indexes.
+ * The upper half-word references the two sensors,
+ * two sensor indices.
+ * The upper half-word references the two optional sensors,
+ * the lower half word references status register and mask.
+ * The function returns true if (status[reg] & mask) is true and,
+ * if specified, if v1 >= v2.
+ * To determine if an object exceeds upper limits, specify <v, limit>.
+ * To determine if an object exceeds lower limits, specify <limit, v>.
+ *
+ * For booleans created with pmbus_add_boolean_reg(), only the lower 16 bits of
+ * index are set. s1 and s2 (the sensor index values) are zero in this case.
+ * The function returns true if (status[reg] & mask) is true.
+ *
+ * If the boolean was created with pmbus_add_boolean_cmp(), a comparison against
+ * a specified limit has to be performed to determine the boolean result.
+ * In this case, the function returns true if v1 >= v2 (where v1 and v2 are
+ * sensor values referenced by sensor indices s1 and s2).
+ *
+ * To determine if an object exceeds upper limits, specify <s1,s2> = <v,limit>.
+ * To determine if an object exceeds lower limits, specify <s1,s2> = <limit,v>.
+ *
+ * If a negative value is stored in any of the referenced registers, this value
+ * reflects an error code which will be returned.
+ */
+static int pmbus_get_boolean(struct pmbus_data *data, int index, int *val)
+{
+	u8 s1 = (index >> 24) & 0xff;
+	u8 s2 = (index >> 16) & 0xff;
+	u8 reg = (index >> 8) & 0xff;
+	u8 mask = index & 0xff;
+	int status;
+	u8 regval;
+
+	status = data->status[reg];
+	if (status < 0)
+		return status;
+
+	regval = status & mask;
+	if (!s1 && !s2)
+		*val = !!regval;
+	else {
+		int v1, v2;
+		struct pmbus_sensor *sensor1, *sensor2;
+
+		sensor1 = &data->sensors[s1];
+		if (sensor1->data < 0)
+			return sensor1->data;
+		sensor2 = &data->sensors[s2];
+		if (sensor2->data < 0)
+			return sensor2->data;
+
+		v1 = pmbus_reg2data(data, sensor1);
+		v2 = pmbus_reg2data(data, sensor2);
+		*val = !!(regval && v1 >= v2);
+	}
+	return 0;
+}
+
+static ssize_t pmbus_show_boolean(struct device *dev,
+				  struct device_attribute *da, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct pmbus_data *data = pmbus_update_device(dev);
+	int val;
+	int err;
+
+	err = pmbus_get_boolean(data, attr->index, &val);
+	if (err)
+		return err;
+	return snprintf(buf, PAGE_SIZE, "%d\n", val);
+}
+
+static ssize_t pmbus_show_sensor(struct device *dev,
+				 struct device_attribute *da, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	struct pmbus_data *data = pmbus_update_device(dev);
+	struct pmbus_sensor *sensor;
+
+	sensor = &data->sensors[attr->index];
+	if (sensor->data < 0)
+		return sensor->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", pmbus_reg2data(data, sensor));
+}
+
+static ssize_t pmbus_set_sensor(struct device *dev,
+				struct device_attribute *devattr,
+				const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	struct pmbus_sensor *sensor = &data->sensors[attr->index];
+	ssize_t rv = count;
+	long val = 0;
+	int ret;
+	u16 regval;
+
+	if (strict_strtol(buf, 10, &val) < 0)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	regval = pmbus_data2reg(data, sensor->class, val);
+	ret = pmbus_write_word_data(client, sensor->page, sensor->reg, regval);
+	if (ret < 0)
+		rv = ret;
+	else
+		data->sensors[attr->index].data = regval;
+	mutex_unlock(&data->update_lock);
+	return rv;
+}
+
+static ssize_t pmbus_show_label(struct device *dev,
+				struct device_attribute *da, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			data->labels[attr->index].label);
+}
+
+#define PMBUS_ADD_ATTR(data, _name, _idx, _mode, _type, _show, _set)	\
+do {									\
+	struct sensor_device_attribute *a				\
+	    = &data->_type##s[data->num_##_type##s].attribute;		\
+	BUG_ON(data->num_attributes >= data->max_attributes);		\
+	a->dev_attr.attr.name = _name;					\
+	a->dev_attr.attr.mode = _mode;					\
+	a->dev_attr.show = _show;					\
+	a->dev_attr.store = _set;					\
+	a->index = _idx;						\
+	data->attributes[data->num_attributes] = &a->dev_attr.attr;	\
+	data->num_attributes++;						\
+} while (0)
+
+#define PMBUS_ADD_GET_ATTR(data, _name, _type, _idx)			\
+	PMBUS_ADD_ATTR(data, _name, _idx, S_IRUGO, _type,		\
+		       pmbus_show_##_type,  NULL)
+
+#define PMBUS_ADD_SET_ATTR(data, _name, _type, _idx)			\
+	PMBUS_ADD_ATTR(data, _name, _idx, S_IWUSR | S_IRUGO, _type,	\
+		       pmbus_show_##_type, pmbus_set_##_type)
+
+static void pmbus_add_boolean(struct pmbus_data *data,
+			      const char *name, const char *type, int seq,
+			      int idx)
+{
+	struct pmbus_boolean *boolean;
+
+	BUG_ON(data->num_booleans >= data->max_booleans);
+
+	boolean = &data->booleans[data->num_booleans];
+
+	snprintf(boolean->name, sizeof(boolean->name), "%s%d_%s",
+		 name, seq, type);
+	PMBUS_ADD_GET_ATTR(data, boolean->name, boolean, idx);
+	data->num_booleans++;
+}
+
+static void pmbus_add_boolean_reg(struct pmbus_data *data,
+				  const char *name, const char *type,
+				  int seq, int reg, int bit)
+{
+	pmbus_add_boolean(data, name, type, seq, (reg << 8) | bit);
+}
+
+static void pmbus_add_boolean_cmp(struct pmbus_data *data,
+				  const char *name, const char *type,
+				  int seq, int i1, int i2, int reg, int mask)
+{
+	pmbus_add_boolean(data, name, type, seq,
+			  (i1 << 24) | (i2 << 16) | (reg << 8) | mask);
+}
+
+static void pmbus_add_sensor(struct pmbus_data *data,
+			     const char *name, const char *type, int seq,
+			     int page, int reg, enum pmbus_sensor_classes class,
+			     bool update)
+{
+	struct pmbus_sensor *sensor;
+
+	BUG_ON(data->num_sensors >= data->max_sensors);
+
+	sensor = &data->sensors[data->num_sensors];
+	snprintf(sensor->name, sizeof(sensor->name), "%s%d_%s",
+		 name, seq, type);
+	sensor->page = page;
+	sensor->reg = reg;
+	sensor->class = class;
+	sensor->update = update;
+	if (update)
+		PMBUS_ADD_GET_ATTR(data, sensor->name, sensor,
+				   data->num_sensors);
+	else
+		PMBUS_ADD_SET_ATTR(data, sensor->name, sensor,
+				   data->num_sensors);
+	data->num_sensors++;
+}
+
+static void pmbus_add_label(struct pmbus_data *data,
+			    const char *name, int seq,
+			    const char *lstring, int index)
+{
+	struct pmbus_label *label;
+
+	BUG_ON(data->num_labels >= data->max_labels);
+
+	label = &data->labels[data->num_labels];
+	snprintf(label->name, sizeof(label->name), "%s%d_label", name, seq);
+	if (!index)
+		strncpy(label->label, lstring, sizeof(label->label) - 1);
+	else
+		snprintf(label->label, sizeof(label->label), "%s%d", lstring,
+			 index);
+
+	PMBUS_ADD_GET_ATTR(data, label->name, label, data->num_labels);
+	data->num_labels++;
+}
+
+static const int pmbus_temp_registers[] = {
+	PMBUS_READ_TEMPERATURE_1,
+	PMBUS_READ_TEMPERATURE_2,
+	PMBUS_READ_TEMPERATURE_3
+};
+
+static const int pmbus_temp_flags[] = {
+	PMBUS_HAVE_TEMP,
+	PMBUS_HAVE_TEMP2,
+	PMBUS_HAVE_TEMP3
+};
+
+static const int pmbus_fan_registers[] = {
+	PMBUS_READ_FAN_SPEED_1,
+	PMBUS_READ_FAN_SPEED_2,
+	PMBUS_READ_FAN_SPEED_3,
+	PMBUS_READ_FAN_SPEED_4
+};
+
+static const int pmbus_fan_config_registers[] = {
+	PMBUS_FAN_CONFIG_12,
+	PMBUS_FAN_CONFIG_12,
+	PMBUS_FAN_CONFIG_34,
+	PMBUS_FAN_CONFIG_34
+};
+
+static const int pmbus_fan_status_registers[] = {
+	PMBUS_STATUS_FAN_12,
+	PMBUS_STATUS_FAN_12,
+	PMBUS_STATUS_FAN_34,
+	PMBUS_STATUS_FAN_34
+};
+
+static const u32 pmbus_fan_flags[] = {
+	PMBUS_HAVE_FAN12,
+	PMBUS_HAVE_FAN12,
+	PMBUS_HAVE_FAN34,
+	PMBUS_HAVE_FAN34
+};
+
+static const u32 pmbus_fan_status_flags[] = {
+	PMBUS_HAVE_STATUS_FAN12,
+	PMBUS_HAVE_STATUS_FAN12,
+	PMBUS_HAVE_STATUS_FAN34,
+	PMBUS_HAVE_STATUS_FAN34
+};
+
+/*
+ * Determine maximum number of sensors, booleans, and labels.
+ * To keep things simple, only make a rough high estimate.
+ */
+static void pmbus_find_max_attr(struct i2c_client *client,
+				struct pmbus_data *data)
+{
+	const struct pmbus_driver_info *info = data->info;
+	int page, max_sensors, max_booleans, max_labels;
+
+	max_sensors = PMBUS_MAX_INPUT_SENSORS;
+	max_booleans = PMBUS_MAX_INPUT_BOOLEANS;
+	max_labels = PMBUS_MAX_INPUT_LABELS;
+
+	for (page = 0; page < info->pages; page++) {
+		if (info->func[page] & PMBUS_HAVE_VOUT) {
+			max_sensors += PMBUS_VOUT_SENSORS_PER_PAGE;
+			max_booleans += PMBUS_VOUT_BOOLEANS_PER_PAGE;
+			max_labels++;
+		}
+		if (info->func[page] & PMBUS_HAVE_IOUT) {
+			max_sensors += PMBUS_IOUT_SENSORS_PER_PAGE;
+			max_booleans += PMBUS_IOUT_BOOLEANS_PER_PAGE;
+			max_labels++;
+		}
+		if (info->func[page] & PMBUS_HAVE_POUT) {
+			max_sensors += PMBUS_POUT_SENSORS_PER_PAGE;
+			max_booleans += PMBUS_POUT_BOOLEANS_PER_PAGE;
+			max_labels++;
+		}
+		if (info->func[page] & PMBUS_HAVE_FAN12) {
+			max_sensors += 2 * PMBUS_MAX_SENSORS_PER_FAN;
+			max_booleans += 2 * PMBUS_MAX_BOOLEANS_PER_FAN;
+		}
+		if (info->func[page] & PMBUS_HAVE_FAN34) {
+			max_sensors += 2 * PMBUS_MAX_SENSORS_PER_FAN;
+			max_booleans += 2 * PMBUS_MAX_BOOLEANS_PER_FAN;
+		}
+		if (info->func[page] & PMBUS_HAVE_TEMP) {
+			max_sensors += PMBUS_MAX_SENSORS_PER_TEMP;
+			max_booleans += PMBUS_MAX_BOOLEANS_PER_TEMP;
+		}
+		if (info->func[page] & PMBUS_HAVE_TEMP2) {
+			max_sensors += PMBUS_MAX_SENSORS_PER_TEMP;
+			max_booleans += PMBUS_MAX_BOOLEANS_PER_TEMP;
+		}
+		if (info->func[page] & PMBUS_HAVE_TEMP3) {
+			max_sensors += PMBUS_MAX_SENSORS_PER_TEMP;
+			max_booleans += PMBUS_MAX_BOOLEANS_PER_TEMP;
+		}
+	}
+	data->max_sensors = max_sensors;
+	data->max_booleans = max_booleans;
+	data->max_labels = max_labels;
+	data->max_attributes = max_sensors + max_booleans + max_labels;
+}
+
+/*
+ * Search for attributes. Allocate sensors, booleans, and labels as needed.
+ */
+static void pmbus_find_attributes(struct i2c_client *client,
+				  struct pmbus_data *data)
+{
+	const struct pmbus_driver_info *info = data->info;
+	int page, i0, i1, in_index;
+
+	/*
+	 * Input voltage sensors
+	 */
+	in_index = 1;
+	if (info->func[0] & PMBUS_HAVE_VIN) {
+		bool have_alarm = false;
+
+		i0 = data->num_sensors;
+		pmbus_add_label(data, "in", in_index, "vin", 0);
+		pmbus_add_sensor(data, "in", "input", in_index,
+				 0, PMBUS_READ_VIN, PSC_VOLTAGE_IN, true);
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_VIN_UV_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "min", in_index,
+					 0, PMBUS_VIN_UV_WARN_LIMIT,
+					 PSC_VOLTAGE_IN, false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT) {
+				pmbus_add_boolean_reg(data, "in", "min_alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_VOLTAGE_UV_WARNING);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_VIN_UV_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "lcrit", in_index,
+					 0, PMBUS_VIN_UV_FAULT_LIMIT,
+					 PSC_VOLTAGE_IN, false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT) {
+				pmbus_add_boolean_reg(data, "in", "lcrit_alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_VOLTAGE_UV_FAULT);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_VIN_OV_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "max", in_index,
+					 0, PMBUS_VIN_OV_WARN_LIMIT,
+					 PSC_VOLTAGE_IN, false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT) {
+				pmbus_add_boolean_reg(data, "in", "max_alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_VOLTAGE_OV_WARNING);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_VIN_OV_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "crit", in_index,
+					 0, PMBUS_VIN_OV_FAULT_LIMIT,
+					 PSC_VOLTAGE_IN, false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT) {
+				pmbus_add_boolean_reg(data, "in", "crit_alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_VOLTAGE_OV_FAULT);
+				have_alarm = true;
+			}
+		}
+		/*
+		 * Add generic alarm attribute only if there are no individual
+		 * attributes.
+		 */
+		if (!have_alarm)
+			pmbus_add_boolean_reg(data, "in", "alarm",
+					      in_index,
+					      PB_STATUS_BASE,
+					      PB_STATUS_VIN_UV);
+		in_index++;
+	}
+	if (info->func[0] & PMBUS_HAVE_VCAP) {
+		pmbus_add_label(data, "in", in_index, "vcap", 0);
+		pmbus_add_sensor(data, "in", "input", in_index, 0,
+				 PMBUS_READ_VCAP, PSC_VOLTAGE_IN, true);
+		in_index++;
+	}
+
+	/*
+	 * Output voltage sensors
+	 */
+	for (page = 0; page < info->pages; page++) {
+		bool have_alarm = false;
+
+		if (!(info->func[page] & PMBUS_HAVE_VOUT))
+			continue;
+
+		i0 = data->num_sensors;
+		pmbus_add_label(data, "in", in_index, "vout", page + 1);
+		pmbus_add_sensor(data, "in", "input", in_index, page,
+				 PMBUS_READ_VOUT, PSC_VOLTAGE_OUT, true);
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_VOUT_UV_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "min", in_index, page,
+					 PMBUS_VOUT_UV_WARN_LIMIT,
+					 PSC_VOLTAGE_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_VOUT) {
+				pmbus_add_boolean_reg(data, "in", "min_alarm",
+						      in_index,
+						      PB_STATUS_VOUT_BASE +
+						      page,
+						      PB_VOLTAGE_UV_WARNING);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_VOUT_UV_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "lcrit", in_index, page,
+					 PMBUS_VOUT_UV_FAULT_LIMIT,
+					 PSC_VOLTAGE_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_VOUT) {
+				pmbus_add_boolean_reg(data, "in", "lcrit_alarm",
+						      in_index,
+						      PB_STATUS_VOUT_BASE +
+						      page,
+						      PB_VOLTAGE_UV_FAULT);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_VOUT_OV_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "max", in_index, page,
+					 PMBUS_VOUT_OV_WARN_LIMIT,
+					 PSC_VOLTAGE_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_VOUT) {
+				pmbus_add_boolean_reg(data, "in", "max_alarm",
+						      in_index,
+						      PB_STATUS_VOUT_BASE +
+						      page,
+						      PB_VOLTAGE_OV_WARNING);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_VOUT_OV_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "in", "crit", in_index, page,
+					 PMBUS_VOUT_OV_FAULT_LIMIT,
+					 PSC_VOLTAGE_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_VOUT) {
+				pmbus_add_boolean_reg(data, "in", "crit_alarm",
+						      in_index,
+						      PB_STATUS_VOUT_BASE +
+						      page,
+						      PB_VOLTAGE_OV_FAULT);
+				have_alarm = true;
+			}
+		}
+		/*
+		 * Add generic alarm attribute only if there are no individual
+		 * attributes.
+		 */
+		if (!have_alarm)
+			pmbus_add_boolean_reg(data, "in", "alarm",
+					      in_index,
+					      PB_STATUS_BASE + page,
+					      PB_STATUS_VOUT_OV);
+		in_index++;
+	}
+
+	/*
+	 * Current sensors
+	 */
+
+	/*
+	 * Input current sensors
+	 */
+	in_index = 1;
+	if (info->func[0] & PMBUS_HAVE_IIN) {
+		i0 = data->num_sensors;
+		pmbus_add_label(data, "curr", in_index, "iin", 0);
+		pmbus_add_sensor(data, "curr", "input", in_index,
+				 0, PMBUS_READ_IIN, PSC_CURRENT_IN, true);
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_IIN_OC_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "curr", "max", in_index,
+					 0, PMBUS_IIN_OC_WARN_LIMIT,
+					 PSC_CURRENT_IN, false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT) {
+				pmbus_add_boolean_reg(data, "curr", "max_alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_IIN_OC_WARNING);
+			}
+		}
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_IIN_OC_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "curr", "crit", in_index,
+					 0, PMBUS_IIN_OC_FAULT_LIMIT,
+					 PSC_CURRENT_IN, false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT)
+				pmbus_add_boolean_reg(data, "curr",
+						      "crit_alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_IIN_OC_FAULT);
+		}
+		in_index++;
+	}
+
+	/*
+	 * Output current sensors
+	 */
+	for (page = 0; page < info->pages; page++) {
+		bool have_alarm = false;
+
+		if (!(info->func[page] & PMBUS_HAVE_IOUT))
+			continue;
+
+		i0 = data->num_sensors;
+		pmbus_add_label(data, "curr", in_index, "iout", page + 1);
+		pmbus_add_sensor(data, "curr", "input", in_index, page,
+				 PMBUS_READ_IOUT, PSC_CURRENT_OUT, true);
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_IOUT_OC_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "curr", "max", in_index, page,
+					 PMBUS_IOUT_OC_WARN_LIMIT,
+					 PSC_CURRENT_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_IOUT) {
+				pmbus_add_boolean_reg(data, "curr", "max_alarm",
+						      in_index,
+						      PB_STATUS_IOUT_BASE +
+						      page, PB_IOUT_OC_WARNING);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_IOUT_UC_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "curr", "lcrit", in_index, page,
+					 PMBUS_IOUT_UC_FAULT_LIMIT,
+					 PSC_CURRENT_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_IOUT) {
+				pmbus_add_boolean_reg(data, "curr",
+						      "lcrit_alarm",
+						      in_index,
+						      PB_STATUS_IOUT_BASE +
+						      page, PB_IOUT_UC_FAULT);
+				have_alarm = true;
+			}
+		}
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_IOUT_OC_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "curr", "crit", in_index, page,
+					 PMBUS_IOUT_OC_FAULT_LIMIT,
+					 PSC_CURRENT_OUT, false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_IOUT) {
+				pmbus_add_boolean_reg(data, "curr",
+						      "crit_alarm",
+						      in_index,
+						      PB_STATUS_IOUT_BASE +
+						      page, PB_IOUT_OC_FAULT);
+				have_alarm = true;
+			}
+		}
+		/*
+		 * Add generic alarm attribute only if there are no individual
+		 * attributes.
+		 */
+		if (!have_alarm)
+			pmbus_add_boolean_reg(data, "curr", "alarm",
+					      in_index,
+					      PB_STATUS_BASE + page,
+					      PB_STATUS_IOUT_OC);
+		in_index++;
+	}
+
+	/*
+	 * Power sensors
+	 */
+	/*
+	 * Input Power sensors
+	 */
+	in_index = 1;
+	if (info->func[0] & PMBUS_HAVE_PIN) {
+		i0 = data->num_sensors;
+		pmbus_add_label(data, "power", in_index, "pin", 0);
+		pmbus_add_sensor(data, "power", "input", in_index,
+				 0, PMBUS_READ_PIN, PSC_POWER, true);
+		if (pmbus_check_word_register(client, 0,
+					      PMBUS_PIN_OP_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "power", "max", in_index,
+					 0, PMBUS_PIN_OP_WARN_LIMIT, PSC_POWER,
+					 false);
+			if (info->func[0] & PMBUS_HAVE_STATUS_INPUT)
+				pmbus_add_boolean_reg(data, "power",
+						      "alarm",
+						      in_index,
+						      PB_STATUS_INPUT_BASE,
+						      PB_PIN_OP_WARNING);
+		}
+		in_index++;
+	}
+
+	/*
+	 * Output Power sensors
+	 */
+	for (page = 0; page < info->pages; page++) {
+		bool need_alarm = false;
+
+		if (!(info->func[page] & PMBUS_HAVE_POUT))
+			continue;
+
+		i0 = data->num_sensors;
+		pmbus_add_label(data, "power", in_index, "pout", page + 1);
+		pmbus_add_sensor(data, "power", "input", in_index, page,
+				 PMBUS_READ_POUT, PSC_POWER, true);
+		/*
+		 * Per hwmon sysfs API, power_cap is to be used to limit output
+		 * power.
+		 * We have two registers related to maximum output power,
+		 * PMBUS_POUT_MAX and PMBUS_POUT_OP_WARN_LIMIT.
+		 * PMBUS_POUT_MAX matches the powerX_cap attribute definition.
+		 * There is no attribute in the API to match
+		 * PMBUS_POUT_OP_WARN_LIMIT. We use powerX_max for now.
+		 */
+		if (pmbus_check_word_register(client, page, PMBUS_POUT_MAX)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "power", "cap", in_index, page,
+					 PMBUS_POUT_MAX, PSC_POWER, false);
+			need_alarm = true;
+		}
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_POUT_OP_WARN_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "power", "max", in_index, page,
+					 PMBUS_POUT_OP_WARN_LIMIT, PSC_POWER,
+					 false);
+			need_alarm = true;
+		}
+		if (need_alarm && (info->func[page] & PMBUS_HAVE_STATUS_IOUT))
+			pmbus_add_boolean_reg(data, "power", "alarm",
+					      in_index,
+					      PB_STATUS_IOUT_BASE + page,
+					      PB_POUT_OP_WARNING
+					      | PB_POWER_LIMITING);
+
+		if (pmbus_check_word_register(client, page,
+					      PMBUS_POUT_OP_FAULT_LIMIT)) {
+			i1 = data->num_sensors;
+			pmbus_add_sensor(data, "power", "crit", in_index, page,
+					 PMBUS_POUT_OP_FAULT_LIMIT, PSC_POWER,
+					 false);
+			if (info->func[page] & PMBUS_HAVE_STATUS_IOUT)
+				pmbus_add_boolean_reg(data, "power",
+						      "crit_alarm",
+						      in_index,
+						      PB_STATUS_IOUT_BASE
+						      + page,
+						      PB_POUT_OP_FAULT);
+		}
+		in_index++;
+	}
+
+	/*
+	 * Temperature sensors
+	 */
+	in_index = 1;
+	for (page = 0; page < info->pages; page++) {
+		int t;
+
+		for (t = 0; t < ARRAY_SIZE(pmbus_temp_registers); t++) {
+			bool have_alarm = false;
+
+			/*
+			 * A PMBus chip may support any combination of
+			 * temperature registers on any page. So we can not
+			 * abort after a failure to detect a register, but have
+			 * to continue checking for all registers on all pages.
+			 */
+			if (!(info->func[page] & pmbus_temp_flags[t]))
+				continue;
+
+			if (!pmbus_check_word_register
+			    (client, page, pmbus_temp_registers[t]))
+				continue;
+
+			i0 = data->num_sensors;
+			pmbus_add_sensor(data, "temp", "input", in_index, page,
+					 pmbus_temp_registers[t],
+					 PSC_TEMPERATURE, true);
+
+			/*
+			 * PMBus provides only one status register for TEMP1-3.
+			 * Thus, we can not use the status register to determine
+			 * which of the three sensors actually caused an alarm.
+			 * Always compare current temperature against the limit
+			 * registers to determine alarm conditions for a
+			 * specific sensor.
+			 *
+			 * Since there is only one set of limit registers for
+			 * up to three temperature sensors, we need to update
+			 * all limit registers after the limit was changed for
+			 * one of the sensors. This ensures that correct limits
+			 * are reported for all temperature sensors.
+			 */
+			if (pmbus_check_word_register
+			    (client, page, PMBUS_UT_WARN_LIMIT)) {
+				i1 = data->num_sensors;
+				pmbus_add_sensor(data, "temp", "min", in_index,
+						 page, PMBUS_UT_WARN_LIMIT,
+						 PSC_TEMPERATURE, true);
+				if (info->func[page] & PMBUS_HAVE_STATUS_TEMP) {
+					pmbus_add_boolean_cmp(data, "temp",
+						"min_alarm", in_index, i1, i0,
+						PB_STATUS_TEMP_BASE + page,
+						PB_TEMP_UT_WARNING);
+					have_alarm = true;
+				}
+			}
+			if (pmbus_check_word_register(client, page,
+						      PMBUS_UT_FAULT_LIMIT)) {
+				i1 = data->num_sensors;
+				pmbus_add_sensor(data, "temp", "lcrit",
+						 in_index, page,
+						 PMBUS_UT_FAULT_LIMIT,
+						 PSC_TEMPERATURE, true);
+				if (info->func[page] & PMBUS_HAVE_STATUS_TEMP) {
+					pmbus_add_boolean_cmp(data, "temp",
+						"lcrit_alarm", in_index, i1, i0,
+						PB_STATUS_TEMP_BASE + page,
+						PB_TEMP_UT_FAULT);
+					have_alarm = true;
+				}
+			}
+			if (pmbus_check_word_register
+			    (client, page, PMBUS_OT_WARN_LIMIT)) {
+				i1 = data->num_sensors;
+				pmbus_add_sensor(data, "temp", "max", in_index,
+						 page, PMBUS_OT_WARN_LIMIT,
+						 PSC_TEMPERATURE, true);
+				if (info->func[page] & PMBUS_HAVE_STATUS_TEMP) {
+					pmbus_add_boolean_cmp(data, "temp",
+						"max_alarm", in_index, i0, i1,
+						PB_STATUS_TEMP_BASE + page,
+						PB_TEMP_OT_WARNING);
+					have_alarm = true;
+				}
+			}
+			if (pmbus_check_word_register(client, page,
+						      PMBUS_OT_FAULT_LIMIT)) {
+				i1 = data->num_sensors;
+				pmbus_add_sensor(data, "temp", "crit", in_index,
+						 page, PMBUS_OT_FAULT_LIMIT,
+						 PSC_TEMPERATURE, true);
+				if (info->func[page] & PMBUS_HAVE_STATUS_TEMP) {
+					pmbus_add_boolean_cmp(data, "temp",
+						"crit_alarm", in_index, i0, i1,
+						PB_STATUS_TEMP_BASE + page,
+						PB_TEMP_OT_FAULT);
+					have_alarm = true;
+				}
+			}
+			/*
+			 * Last resort - we were not able to create any alarm
+			 * registers. Report alarm for all sensors using the
+			 * status register temperature alarm bit.
+			 */
+			if (!have_alarm)
+				pmbus_add_boolean_reg(data, "temp", "alarm",
+						      in_index,
+						      PB_STATUS_BASE + page,
+						      PB_STATUS_TEMPERATURE);
+			in_index++;
+		}
+	}
+
+	/*
+	 * Fans
+	 */
+	in_index = 1;
+	for (page = 0; page < info->pages; page++) {
+		int f;
+
+		for (f = 0; f < ARRAY_SIZE(pmbus_fan_registers); f++) {
+			int regval;
+
+			if (!(info->func[page] & pmbus_fan_flags[f]))
+				break;
+
+			if (!pmbus_check_word_register(client, page,
+						       pmbus_fan_registers[f])
+			    || !pmbus_check_byte_register(client, page,
+						pmbus_fan_config_registers[f]))
+				break;
+
+			/*
+			 * Skip fan if not installed.
+			 * Each fan configuration register covers multiple fans,
+			 * so we have to do some magic.
+			 */
+			regval = pmbus_read_byte_data(client, page,
+				pmbus_fan_config_registers[f]);
+			if (regval < 0 ||
+			    (!(regval & (PB_FAN_1_INSTALLED >> ((f & 1) * 4)))))
+				continue;
+
+			i0 = data->num_sensors;
+			pmbus_add_sensor(data, "fan", "input", in_index, page,
+					 pmbus_fan_registers[f], PSC_FAN, true);
+
+			/*
+			 * Each fan status register covers multiple fans,
+			 * so we have to do some magic.
+			 */
+			if ((info->func[page] & pmbus_fan_status_flags[f]) &&
+			    pmbus_check_byte_register(client,
+					page, pmbus_fan_status_registers[f])) {
+				int base;
+
+				if (f > 1)	/* fan 3, 4 */
+					base = PB_STATUS_FAN34_BASE + page;
+				else
+					base = PB_STATUS_FAN_BASE + page;
+				pmbus_add_boolean_reg(data, "fan", "alarm",
+					in_index, base,
+					PB_FAN_FAN1_WARNING >> (f & 1));
+				pmbus_add_boolean_reg(data, "fan", "fault",
+					in_index, base,
+					PB_FAN_FAN1_FAULT >> (f & 1));
+			}
+			in_index++;
+		}
+	}
+}
+
+/*
+ * Identify chip parameters.
+ * This function is called for all chips.
+ */
+static int pmbus_identify_common(struct i2c_client *client,
+				 struct pmbus_data *data)
+{
+	int vout_mode = -1, exponent;
+
+	if (pmbus_check_byte_register(client, 0, PMBUS_VOUT_MODE))
+		vout_mode = pmbus_read_byte_data(client, 0, PMBUS_VOUT_MODE);
+	if (vout_mode >= 0 && vout_mode != 0xff) {
+		/*
+		 * Not all chips support the VOUT_MODE command,
+		 * so a failure to read it is not an error.
+		 */
+		switch (vout_mode >> 5) {
+		case 0:	/* linear mode      */
+			if (data->info->direct[PSC_VOLTAGE_OUT])
+				return -ENODEV;
+
+			exponent = vout_mode & 0x1f;
+			/* and sign-extend it */
+			if (exponent & 0x10)
+				exponent |= ~0x1f;
+			data->exponent = exponent;
+			break;
+		case 2:	/* direct mode      */
+			if (!data->info->direct[PSC_VOLTAGE_OUT])
+				return -ENODEV;
+			break;
+		default:
+			return -ENODEV;
+		}
+	}
+
+	/* Determine maximum number of sensors, booleans, and labels */
+	pmbus_find_max_attr(client, data);
+	pmbus_clear_fault_page(client, 0);
+	return 0;
+}
+
+int pmbus_do_probe(struct i2c_client *client, const struct i2c_device_id *id,
+		   struct pmbus_driver_info *info)
+{
+	const struct pmbus_platform_data *pdata = client->dev.platform_data;
+	struct pmbus_data *data;
+	int ret;
+
+	if (!info) {
+		dev_err(&client->dev, "Missing chip information");
+		return -ENODEV;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WRITE_BYTE
+				     | I2C_FUNC_SMBUS_BYTE_DATA
+				     | I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&client->dev, "No memory to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/*
+	 * Bail out if status register or PMBus revision register
+	 * does not exist.
+	 */
+	if (i2c_smbus_read_byte_data(client, PMBUS_STATUS_BYTE) < 0
+	    || i2c_smbus_read_byte_data(client, PMBUS_REVISION) < 0) {
+		dev_err(&client->dev,
+			"Status or revision register not found\n");
+		ret = -ENODEV;
+		goto out_data;
+	}
+
+	if (pdata)
+		data->flags = pdata->flags;
+	data->info = info;
+
+	pmbus_clear_faults(client);
+
+	if (info->identify) {
+		ret = (*info->identify)(client, info);
+		if (ret < 0) {
+			dev_err(&client->dev, "Chip identification failed\n");
+			goto out_data;
+		}
+	}
+
+	if (info->pages <= 0 || info->pages > PMBUS_PAGES) {
+		dev_err(&client->dev, "Bad number of PMBus pages: %d\n",
+			info->pages);
+		ret = -EINVAL;
+		goto out_data;
+	}
+	/*
+	 * Bail out if more than one page was configured, but we can not
+	 * select the highest page. This is an indication that the wrong
+	 * chip type was selected. Better bail out now than keep
+	 * returning errors later on.
+	 */
+	if (info->pages > 1 && pmbus_set_page(client, info->pages - 1) < 0) {
+		dev_err(&client->dev, "Failed to select page %d\n",
+			info->pages - 1);
+		ret = -EINVAL;
+		goto out_data;
+	}
+
+	ret = pmbus_identify_common(client, data);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to identify chip capabilities\n");
+		goto out_data;
+	}
+
+	ret = -ENOMEM;
+	data->sensors = kzalloc(sizeof(struct pmbus_sensor) * data->max_sensors,
+				GFP_KERNEL);
+	if (!data->sensors) {
+		dev_err(&client->dev, "No memory to allocate sensor data\n");
+		goto out_data;
+	}
+
+	data->booleans = kzalloc(sizeof(struct pmbus_boolean)
+				 * data->max_booleans, GFP_KERNEL);
+	if (!data->booleans) {
+		dev_err(&client->dev, "No memory to allocate boolean data\n");
+		goto out_sensors;
+	}
+
+	data->labels = kzalloc(sizeof(struct pmbus_label) * data->max_labels,
+			       GFP_KERNEL);
+	if (!data->labels) {
+		dev_err(&client->dev, "No memory to allocate label data\n");
+		goto out_booleans;
+	}
+
+	data->attributes = kzalloc(sizeof(struct attribute *)
+				   * data->max_attributes, GFP_KERNEL);
+	if (!data->attributes) {
+		dev_err(&client->dev, "No memory to allocate attribute data\n");
+		goto out_labels;
+	}
+
+	pmbus_find_attributes(client, data);
+
+	/*
+	 * If there are no attributes, something is wrong.
+	 * Bail out instead of trying to register nothing.
+	 */
+	if (!data->num_attributes) {
+		dev_err(&client->dev, "No attributes found\n");
+		ret = -ENODEV;
+		goto out_attributes;
+	}
+
+	/* Register sysfs hooks */
+	data->group.attrs = data->attributes;
+	ret = sysfs_create_group(&client->dev.kobj, &data->group);
+	if (ret) {
+		dev_err(&client->dev, "Failed to create sysfs entries\n");
+		goto out_attributes;
+	}
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		ret = PTR_ERR(data->hwmon_dev);
+		dev_err(&client->dev, "Failed to register hwmon device\n");
+		goto out_hwmon_device_register;
+	}
+	return 0;
+
+out_hwmon_device_register:
+	sysfs_remove_group(&client->dev.kobj, &data->group);
+out_attributes:
+	kfree(data->attributes);
+out_labels:
+	kfree(data->labels);
+out_booleans:
+	kfree(data->booleans);
+out_sensors:
+	kfree(data->sensors);
+out_data:
+	kfree(data);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(pmbus_do_probe);
+
+int pmbus_do_remove(struct i2c_client *client)
+{
+	struct pmbus_data *data = i2c_get_clientdata(client);
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &data->group);
+	kfree(data->attributes);
+	kfree(data->labels);
+	kfree(data->booleans);
+	kfree(data->sensors);
+	kfree(data);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(pmbus_do_remove);
+
+MODULE_AUTHOR("Guenter Roeck");
+MODULE_DESCRIPTION("PMBus core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/sht15.c b/drivers/hwmon/sht15.c
index a610e7880fb3..3ba7dd82589a 100644
--- a/drivers/hwmon/sht15.c
+++ b/drivers/hwmon/sht15.c
@@ -610,7 +610,7 @@ static int __devexit sht15_remove(struct platform_device *pdev)
 	struct sht15_data *data = platform_get_drvdata(pdev);
 
 	/* Make sure any reads from the device are done and
-	 * prevent new ones beginnning */
+	 * prevent new ones from beginning */
 	mutex_lock(&data->read_lock);
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&pdev->dev.kobj, &sht15_attr_group);
diff --git a/drivers/hwmon/twl4030-madc-hwmon.c b/drivers/hwmon/twl4030-madc-hwmon.c
new file mode 100644
index 000000000000..97e22bef85ab
--- /dev/null
+++ b/drivers/hwmon/twl4030-madc-hwmon.c
@@ -0,0 +1,157 @@
+/*
+ *
+ * TWL4030 MADC Hwmon driver-This driver monitors the real time
+ * conversion of analog signals like battery temperature,
+ * battery type, battery level etc. User can ask for the conversion on a
+ * particular channel using the sysfs nodes.
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * J Keerthy <j-keerthy@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c/twl.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/i2c/twl4030-madc.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/stddef.h>
+#include <linux/sysfs.h>
+#include <linux/err.h>
+#include <linux/types.h>
+
+/*
+ * sysfs hook function
+ */
+static ssize_t madc_read(struct device *dev,
+			 struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct twl4030_madc_request req;
+	long val;
+
+	req.channels = (1 << attr->index);
+	req.method = TWL4030_MADC_SW2;
+	req.func_cb = NULL;
+	val = twl4030_madc_conversion(&req);
+	if (val < 0)
+		return val;
+
+	return sprintf(buf, "%d\n", req.rbuf[attr->index]);
+}
+
+/* sysfs nodes to read individual channels from user side */
+static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, madc_read, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, madc_read, NULL, 1);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, madc_read, NULL, 2);
+static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, madc_read, NULL, 3);
+static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, madc_read, NULL, 4);
+static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, madc_read, NULL, 5);
+static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, madc_read, NULL, 6);
+static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, madc_read, NULL, 7);
+static SENSOR_DEVICE_ATTR(in8_input, S_IRUGO, madc_read, NULL, 8);
+static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, madc_read, NULL, 9);
+static SENSOR_DEVICE_ATTR(curr10_input, S_IRUGO, madc_read, NULL, 10);
+static SENSOR_DEVICE_ATTR(in11_input, S_IRUGO, madc_read, NULL, 11);
+static SENSOR_DEVICE_ATTR(in12_input, S_IRUGO, madc_read, NULL, 12);
+static SENSOR_DEVICE_ATTR(in15_input, S_IRUGO, madc_read, NULL, 15);
+
+static struct attribute *twl4030_madc_attributes[] = {
+	&sensor_dev_attr_in0_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+	&sensor_dev_attr_in3_input.dev_attr.attr,
+	&sensor_dev_attr_in4_input.dev_attr.attr,
+	&sensor_dev_attr_in5_input.dev_attr.attr,
+	&sensor_dev_attr_in6_input.dev_attr.attr,
+	&sensor_dev_attr_in7_input.dev_attr.attr,
+	&sensor_dev_attr_in8_input.dev_attr.attr,
+	&sensor_dev_attr_in9_input.dev_attr.attr,
+	&sensor_dev_attr_curr10_input.dev_attr.attr,
+	&sensor_dev_attr_in11_input.dev_attr.attr,
+	&sensor_dev_attr_in12_input.dev_attr.attr,
+	&sensor_dev_attr_in15_input.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group twl4030_madc_group = {
+	.attrs = twl4030_madc_attributes,
+};
+
+static int __devinit twl4030_madc_hwmon_probe(struct platform_device *pdev)
+{
+	int ret;
+	int status;
+	struct device *hwmon;
+
+	ret = sysfs_create_group(&pdev->dev.kobj, &twl4030_madc_group);
+	if (ret)
+		goto err_sysfs;
+	hwmon = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(hwmon)) {
+		dev_err(&pdev->dev, "hwmon_device_register failed.\n");
+		status = PTR_ERR(hwmon);
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	sysfs_remove_group(&pdev->dev.kobj, &twl4030_madc_group);
+err_sysfs:
+
+	return ret;
+}
+
+static int __devexit twl4030_madc_hwmon_remove(struct platform_device *pdev)
+{
+	hwmon_device_unregister(&pdev->dev);
+	sysfs_remove_group(&pdev->dev.kobj, &twl4030_madc_group);
+
+	return 0;
+}
+
+static struct platform_driver twl4030_madc_hwmon_driver = {
+	.probe = twl4030_madc_hwmon_probe,
+	.remove = __exit_p(twl4030_madc_hwmon_remove),
+	.driver = {
+		   .name = "twl4030_madc_hwmon",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init twl4030_madc_hwmon_init(void)
+{
+	return platform_driver_register(&twl4030_madc_hwmon_driver);
+}
+
+module_init(twl4030_madc_hwmon_init);
+
+static void __exit twl4030_madc_hwmon_exit(void)
+{
+	platform_driver_unregister(&twl4030_madc_hwmon_driver);
+}
+
+module_exit(twl4030_madc_hwmon_exit);
+
+MODULE_DESCRIPTION("TWL4030 ADC Hwmon driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("J Keerthy");
+MODULE_ALIAS("twl4030_madc_hwmon");
diff --git a/drivers/hwmon/ultra45_env.c b/drivers/hwmon/ultra45_env.c
index d863e13a50b8..1f36c635d933 100644
--- a/drivers/hwmon/ultra45_env.c
+++ b/drivers/hwmon/ultra45_env.c
@@ -234,8 +234,7 @@ static const struct attribute_group env_group = {
 	.attrs = env_attributes,
 };
 
-static int __devinit env_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit env_probe(struct platform_device *op)
 {
 	struct env *p = kzalloc(sizeof(*p), GFP_KERNEL);
 	int err = -ENOMEM;
@@ -299,7 +298,7 @@ static const struct of_device_id env_match[] = {
 };
 MODULE_DEVICE_TABLE(of, env_match);
 
-static struct of_platform_driver env_driver = {
+static struct platform_driver env_driver = {
 	.driver = {
 		.name = "ultra45_env",
 		.owner = THIS_MODULE,
@@ -311,12 +310,12 @@ static struct of_platform_driver env_driver = {
 
 static int __init env_init(void)
 {
-	return of_register_platform_driver(&env_driver);
+	return platform_driver_register(&env_driver);
 }
 
 static void __exit env_exit(void)
 {
-	of_unregister_platform_driver(&env_driver);
+	platform_driver_unregister(&env_driver);
 }
 
 module_init(env_init);
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index 073eabedc432..d18031944960 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -1,11 +1,12 @@
 /*
     w83627ehf - Driver for the hardware monitoring functionality of
-                the Winbond W83627EHF Super-I/O chip
+		the Winbond W83627EHF Super-I/O chip
     Copyright (C) 2005  Jean Delvare <khali@linux-fr.org>
     Copyright (C) 2006  Yuan Mu (Winbond),
-                        Rudolf Marek <r.marek@assembler.cz>
-                        David Hubbard <david.c.hubbard@gmail.com>
+			Rudolf Marek <r.marek@assembler.cz>
+			David Hubbard <david.c.hubbard@gmail.com>
 			Daniel J Blueman <daniel.blueman@gmail.com>
+    Copyright (C) 2010  Sheng-Yuan Huang (Nuvoton) (PS00)
 
     Shamelessly ripped from the w83627hf driver
     Copyright (C) 2003  Mark Studebaker
@@ -35,11 +36,13 @@
 
     Chip        #vin    #fan    #pwm    #temp  chip IDs       man ID
     w83627ehf   10      5       4       3      0x8850 0x88    0x5ca3
-                                               0x8860 0xa1
+					       0x8860 0xa1
     w83627dhg    9      5       4       3      0xa020 0xc1    0x5ca3
     w83627dhg-p  9      5       4       3      0xb070 0xc1    0x5ca3
     w83667hg     9      5       3       3      0xa510 0xc1    0x5ca3
-    w83667hg-b   9      5       3       3      0xb350 0xc1    0x5ca3
+    w83667hg-b   9      5       3       4      0xb350 0xc1    0x5ca3
+    nct6775f     9      4       3       9      0xb470 0xc1    0x5ca3
+    nct6776f     9      5       3       9      0xC330 0xc1    0x5ca3
 */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -58,21 +61,28 @@
 #include <linux/io.h>
 #include "lm75.h"
 
-enum kinds { w83627ehf, w83627dhg, w83627dhg_p, w83667hg, w83667hg_b };
+enum kinds { w83627ehf, w83627dhg, w83627dhg_p, w83667hg, w83667hg_b, nct6775,
+	nct6776 };
 
 /* used to set data->name = w83627ehf_device_names[data->sio_kind] */
-static const char * w83627ehf_device_names[] = {
+static const char * const w83627ehf_device_names[] = {
 	"w83627ehf",
 	"w83627dhg",
 	"w83627dhg",
 	"w83667hg",
 	"w83667hg",
+	"nct6775",
+	"nct6776",
 };
 
 static unsigned short force_id;
 module_param(force_id, ushort, 0);
 MODULE_PARM_DESC(force_id, "Override the detected device ID");
 
+static unsigned short fan_debounce;
+module_param(fan_debounce, ushort, 0);
+MODULE_PARM_DESC(fan_debounce, "Enable debouncing for fan RPM signal");
+
 #define DRVNAME "w83627ehf"
 
 /*
@@ -80,7 +90,7 @@ MODULE_PARM_DESC(force_id, "Override the detected device ID");
  */
 
 #define W83627EHF_LD_HWM	0x0b
-#define W83667HG_LD_VID 	0x0d
+#define W83667HG_LD_VID		0x0d
 
 #define SIO_REG_LDSEL		0x07	/* Logical device select */
 #define SIO_REG_DEVID		0x20	/* Device ID (2 bytes) */
@@ -94,8 +104,10 @@ MODULE_PARM_DESC(force_id, "Override the detected device ID");
 #define SIO_W83627EHG_ID	0x8860
 #define SIO_W83627DHG_ID	0xa020
 #define SIO_W83627DHG_P_ID	0xb070
-#define SIO_W83667HG_ID 	0xa510
+#define SIO_W83667HG_ID		0xa510
 #define SIO_W83667HG_B_ID	0xb350
+#define SIO_NCT6775_ID		0xb470
+#define SIO_NCT6776_ID		0xc330
 #define SIO_ID_MASK		0xFFF0
 
 static inline void
@@ -138,7 +150,7 @@ superio_exit(int ioreg)
  * ISA constants
  */
 
-#define IOREGION_ALIGNMENT	~7
+#define IOREGION_ALIGNMENT	(~7)
 #define IOREGION_OFFSET		5
 #define IOREGION_LENGTH		2
 #define ADDR_REG_OFFSET		0
@@ -164,13 +176,10 @@ static const u16 W83627EHF_REG_FAN_MIN[] = { 0x3b, 0x3c, 0x3d, 0x3e, 0x55c };
 #define W83627EHF_REG_IN(nr)		((nr < 7) ? (0x20 + (nr)) : \
 					 (0x550 + (nr) - 7))
 
-#define W83627EHF_REG_TEMP1		0x27
-#define W83627EHF_REG_TEMP1_HYST	0x3a
-#define W83627EHF_REG_TEMP1_OVER	0x39
-static const u16 W83627EHF_REG_TEMP[] = { 0x150, 0x250 };
-static const u16 W83627EHF_REG_TEMP_HYST[] = { 0x153, 0x253 };
-static const u16 W83627EHF_REG_TEMP_OVER[] = { 0x155, 0x255 };
-static const u16 W83627EHF_REG_TEMP_CONFIG[] = { 0x152, 0x252 };
+static const u16 W83627EHF_REG_TEMP[] = { 0x27, 0x150, 0x250, 0x7e };
+static const u16 W83627EHF_REG_TEMP_HYST[] = { 0x3a, 0x153, 0x253, 0 };
+static const u16 W83627EHF_REG_TEMP_OVER[] = { 0x39, 0x155, 0x255, 0 };
+static const u16 W83627EHF_REG_TEMP_CONFIG[] = { 0, 0x152, 0x252, 0 };
 
 /* Fan clock dividers are spread over the following five registers */
 #define W83627EHF_REG_FANDIV1		0x47
@@ -179,6 +188,11 @@ static const u16 W83627EHF_REG_TEMP_CONFIG[] = { 0x152, 0x252 };
 #define W83627EHF_REG_DIODE		0x59
 #define W83627EHF_REG_SMI_OVT		0x4C
 
+/* NCT6775F has its own fan divider registers */
+#define NCT6775_REG_FANDIV1		0x506
+#define NCT6775_REG_FANDIV2		0x507
+#define NCT6775_REG_FAN_DEBOUNCE	0xf0
+
 #define W83627EHF_REG_ALARM1		0x459
 #define W83627EHF_REG_ALARM2		0x45A
 #define W83627EHF_REG_ALARM3		0x45B
@@ -199,22 +213,123 @@ static const u8 W83627EHF_PWM_MODE_SHIFT[] = { 0, 1, 0, 6 };
 static const u8 W83627EHF_PWM_ENABLE_SHIFT[] = { 2, 4, 1, 4 };
 
 /* FAN Duty Cycle, be used to control */
-static const u8 W83627EHF_REG_PWM[] = { 0x01, 0x03, 0x11, 0x61 };
-static const u8 W83627EHF_REG_TARGET[] = { 0x05, 0x06, 0x13, 0x63 };
+static const u16 W83627EHF_REG_PWM[] = { 0x01, 0x03, 0x11, 0x61 };
+static const u16 W83627EHF_REG_TARGET[] = { 0x05, 0x06, 0x13, 0x63 };
 static const u8 W83627EHF_REG_TOLERANCE[] = { 0x07, 0x07, 0x14, 0x62 };
 
 /* Advanced Fan control, some values are common for all fans */
-static const u8 W83627EHF_REG_FAN_START_OUTPUT[] = { 0x0a, 0x0b, 0x16, 0x65 };
-static const u8 W83627EHF_REG_FAN_STOP_OUTPUT[] = { 0x08, 0x09, 0x15, 0x64 };
-static const u8 W83627EHF_REG_FAN_STOP_TIME[] = { 0x0c, 0x0d, 0x17, 0x66 };
+static const u16 W83627EHF_REG_FAN_START_OUTPUT[] = { 0x0a, 0x0b, 0x16, 0x65 };
+static const u16 W83627EHF_REG_FAN_STOP_OUTPUT[] = { 0x08, 0x09, 0x15, 0x64 };
+static const u16 W83627EHF_REG_FAN_STOP_TIME[] = { 0x0c, 0x0d, 0x17, 0x66 };
 
-static const u8 W83627EHF_REG_FAN_MAX_OUTPUT_COMMON[]
+static const u16 W83627EHF_REG_FAN_MAX_OUTPUT_COMMON[]
 						= { 0xff, 0x67, 0xff, 0x69 };
-static const u8 W83627EHF_REG_FAN_STEP_OUTPUT_COMMON[]
+static const u16 W83627EHF_REG_FAN_STEP_OUTPUT_COMMON[]
 						= { 0xff, 0x68, 0xff, 0x6a };
 
-static const u8 W83627EHF_REG_FAN_MAX_OUTPUT_W83667_B[] = { 0x67, 0x69, 0x6b };
-static const u8 W83627EHF_REG_FAN_STEP_OUTPUT_W83667_B[] = { 0x68, 0x6a, 0x6c };
+static const u16 W83627EHF_REG_FAN_MAX_OUTPUT_W83667_B[] = { 0x67, 0x69, 0x6b };
+static const u16 W83627EHF_REG_FAN_STEP_OUTPUT_W83667_B[]
+						= { 0x68, 0x6a, 0x6c };
+
+static const u16 NCT6775_REG_TARGET[] = { 0x101, 0x201, 0x301 };
+static const u16 NCT6775_REG_FAN_MODE[] = { 0x102, 0x202, 0x302 };
+static const u16 NCT6775_REG_FAN_STOP_OUTPUT[] = { 0x105, 0x205, 0x305 };
+static const u16 NCT6775_REG_FAN_START_OUTPUT[] = { 0x106, 0x206, 0x306 };
+static const u16 NCT6775_REG_FAN_STOP_TIME[] = { 0x107, 0x207, 0x307 };
+static const u16 NCT6775_REG_PWM[] = { 0x109, 0x209, 0x309 };
+static const u16 NCT6775_REG_FAN_MAX_OUTPUT[] = { 0x10a, 0x20a, 0x30a };
+static const u16 NCT6775_REG_FAN_STEP_OUTPUT[] = { 0x10b, 0x20b, 0x30b };
+static const u16 NCT6775_REG_FAN[] = { 0x630, 0x632, 0x634, 0x636, 0x638 };
+static const u16 NCT6776_REG_FAN_MIN[] = { 0x63a, 0x63c, 0x63e, 0x640, 0x642};
+
+static const u16 NCT6775_REG_TEMP[]
+	= { 0x27, 0x150, 0x250, 0x73, 0x75, 0x77, 0x62b, 0x62c, 0x62d };
+static const u16 NCT6775_REG_TEMP_CONFIG[]
+	= { 0, 0x152, 0x252, 0, 0, 0, 0x628, 0x629, 0x62A };
+static const u16 NCT6775_REG_TEMP_HYST[]
+	= { 0x3a, 0x153, 0x253, 0, 0, 0, 0x673, 0x678, 0x67D };
+static const u16 NCT6775_REG_TEMP_OVER[]
+	= { 0x39, 0x155, 0x255, 0, 0, 0, 0x672, 0x677, 0x67C };
+static const u16 NCT6775_REG_TEMP_SOURCE[]
+	= { 0x621, 0x622, 0x623, 0x100, 0x200, 0x300, 0x624, 0x625, 0x626 };
+
+static const char *const w83667hg_b_temp_label[] = {
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN",
+	"AMDTSI",
+	"PECI Agent 1",
+	"PECI Agent 2",
+	"PECI Agent 3",
+	"PECI Agent 4"
+};
+
+static const char *const nct6775_temp_label[] = {
+	"",
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN",
+	"AMD SB-TSI",
+	"PECI Agent 0",
+	"PECI Agent 1",
+	"PECI Agent 2",
+	"PECI Agent 3",
+	"PECI Agent 4",
+	"PECI Agent 5",
+	"PECI Agent 6",
+	"PECI Agent 7",
+	"PCH_CHIP_CPU_MAX_TEMP",
+	"PCH_CHIP_TEMP",
+	"PCH_CPU_TEMP",
+	"PCH_MCH_TEMP",
+	"PCH_DIM0_TEMP",
+	"PCH_DIM1_TEMP",
+	"PCH_DIM2_TEMP",
+	"PCH_DIM3_TEMP"
+};
+
+static const char *const nct6776_temp_label[] = {
+	"",
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN",
+	"SMBUSMASTER 0",
+	"SMBUSMASTER 1",
+	"SMBUSMASTER 2",
+	"SMBUSMASTER 3",
+	"SMBUSMASTER 4",
+	"SMBUSMASTER 5",
+	"SMBUSMASTER 6",
+	"SMBUSMASTER 7",
+	"PECI Agent 0",
+	"PECI Agent 1",
+	"PCH_CHIP_CPU_MAX_TEMP",
+	"PCH_CHIP_TEMP",
+	"PCH_CPU_TEMP",
+	"PCH_MCH_TEMP",
+	"PCH_DIM0_TEMP",
+	"PCH_DIM1_TEMP",
+	"PCH_DIM2_TEMP",
+	"PCH_DIM3_TEMP",
+	"BYTE_TEMP"
+};
+
+#define NUM_REG_TEMP	ARRAY_SIZE(NCT6775_REG_TEMP)
+
+static inline int is_word_sized(u16 reg)
+{
+	return ((((reg & 0xff00) == 0x100
+	      || (reg & 0xff00) == 0x200)
+	     && ((reg & 0x00ff) == 0x50
+	      || (reg & 0x00ff) == 0x53
+	      || (reg & 0x00ff) == 0x55))
+	     || (reg & 0xfff0) == 0x630
+	     || reg == 0x640 || reg == 0x642
+	     || ((reg & 0xfff0) == 0x650
+		 && (reg & 0x000f) >= 0x06)
+	     || reg == 0x73 || reg == 0x75 || reg == 0x77
+		);
+}
 
 /*
  * Conversions
@@ -232,12 +347,36 @@ static inline u8 step_time_to_reg(unsigned int msec, u8 mode)
 						(msec + 200) / 400), 1, 255);
 }
 
-static inline unsigned int
-fan_from_reg(u8 reg, unsigned int div)
+static unsigned int fan_from_reg8(u16 reg, unsigned int divreg)
 {
 	if (reg == 0 || reg == 255)
 		return 0;
-	return 1350000U / (reg * div);
+	return 1350000U / (reg << divreg);
+}
+
+static unsigned int fan_from_reg13(u16 reg, unsigned int divreg)
+{
+	if ((reg & 0xff1f) == 0xff1f)
+		return 0;
+
+	reg = (reg & 0x1f) | ((reg & 0xff00) >> 3);
+
+	if (reg == 0)
+		return 0;
+
+	return 1350000U / reg;
+}
+
+static unsigned int fan_from_reg16(u16 reg, unsigned int divreg)
+{
+	if (reg == 0 || reg == 0xffff)
+		return 0;
+
+	/*
+	 * Even though the registers are 16 bit wide, the fan divisor
+	 * still applies.
+	 */
+	return 1350000U / (reg << divreg);
 }
 
 static inline unsigned int
@@ -247,21 +386,19 @@ div_from_reg(u8 reg)
 }
 
 static inline int
-temp1_from_reg(s8 reg)
+temp_from_reg(u16 reg, s16 regval)
 {
-	return reg * 1000;
+	if (is_word_sized(reg))
+		return LM75_TEMP_FROM_REG(regval);
+	return regval * 1000;
 }
 
-static inline s8
-temp1_to_reg(long temp, int min, int max)
+static inline u16
+temp_to_reg(u16 reg, long temp)
 {
-	if (temp <= min)
-		return min / 1000;
-	if (temp >= max)
-		return max / 1000;
-	if (temp < 0)
-		return (temp - 500) / 1000;
-	return (temp + 500) / 1000;
+	if (is_word_sized(reg))
+		return LM75_TEMP_TO_REG(temp);
+	return DIV_ROUND_CLOSEST(SENSORS_LIMIT(temp, -127000, 128000), 1000);
 }
 
 /* Some of analog inputs have internal scaling (2x), 8mV is ADC LSB */
@@ -275,7 +412,8 @@ static inline long in_from_reg(u8 reg, u8 nr)
 
 static inline u8 in_to_reg(u32 val, u8 nr)
 {
-	return SENSORS_LIMIT(((val + (scale_in[nr] / 2)) / scale_in[nr]), 0, 255);
+	return SENSORS_LIMIT(((val + (scale_in[nr] / 2)) / scale_in[nr]), 0,
+			     255);
 }
 
 /*
@@ -289,38 +427,57 @@ struct w83627ehf_data {
 	struct device *hwmon_dev;
 	struct mutex lock;
 
-	const u8 *REG_FAN_START_OUTPUT;
-	const u8 *REG_FAN_STOP_OUTPUT;
-	const u8 *REG_FAN_MAX_OUTPUT;
-	const u8 *REG_FAN_STEP_OUTPUT;
+	u16 reg_temp[NUM_REG_TEMP];
+	u16 reg_temp_over[NUM_REG_TEMP];
+	u16 reg_temp_hyst[NUM_REG_TEMP];
+	u16 reg_temp_config[NUM_REG_TEMP];
+	u8 temp_src[NUM_REG_TEMP];
+	const char * const *temp_label;
+
+	const u16 *REG_PWM;
+	const u16 *REG_TARGET;
+	const u16 *REG_FAN;
+	const u16 *REG_FAN_MIN;
+	const u16 *REG_FAN_START_OUTPUT;
+	const u16 *REG_FAN_STOP_OUTPUT;
+	const u16 *REG_FAN_STOP_TIME;
+	const u16 *REG_FAN_MAX_OUTPUT;
+	const u16 *REG_FAN_STEP_OUTPUT;
+
+	unsigned int (*fan_from_reg)(u16 reg, unsigned int divreg);
+	unsigned int (*fan_from_reg_min)(u16 reg, unsigned int divreg);
 
 	struct mutex update_lock;
 	char valid;		/* !=0 if following fields are valid */
 	unsigned long last_updated;	/* In jiffies */
 
 	/* Register values */
+	u8 bank;		/* current register bank */
 	u8 in_num;		/* number of in inputs we have */
 	u8 in[10];		/* Register value */
 	u8 in_max[10];		/* Register value */
 	u8 in_min[10];		/* Register value */
-	u8 fan[5];
-	u8 fan_min[5];
+	unsigned int rpm[5];
+	u16 fan_min[5];
 	u8 fan_div[5];
 	u8 has_fan;		/* some fan inputs can be disabled */
+	u8 has_fan_min;		/* some fans don't have min register */
+	bool has_fan_div;
 	u8 temp_type[3];
-	s8 temp1;
-	s8 temp1_max;
-	s8 temp1_max_hyst;
-	s16 temp[2];
-	s16 temp_max[2];
-	s16 temp_max_hyst[2];
+	s16 temp[9];
+	s16 temp_max[9];
+	s16 temp_max_hyst[9];
 	u32 alarms;
 
 	u8 pwm_mode[4]; /* 0->DC variable voltage, 1->PWM variable duty cycle */
 	u8 pwm_enable[4]; /* 1->manual
 			     2->thermal cruise mode (also called SmartFan I)
 			     3->fan speed cruise mode
-			     4->variable thermal cruise (also called SmartFan III) */
+			     4->variable thermal cruise (also called
+				SmartFan III)
+			     5->enhanced variable thermal cruise (also called
+				SmartFan IV) */
+	u8 pwm_enable_orig[4];	/* original value of pwm_enable */
 	u8 pwm_num;		/* number of pwm */
 	u8 pwm[4];
 	u8 target_temp[4];
@@ -335,7 +492,7 @@ struct w83627ehf_data {
 	u8 vid;
 	u8 vrm;
 
-	u8 temp3_disable;
+	u16 have_temp;
 	u8 in6_skip;
 };
 
@@ -344,30 +501,19 @@ struct w83627ehf_sio_data {
 	enum kinds kind;
 };
 
-static inline int is_word_sized(u16 reg)
-{
-	return (((reg & 0xff00) == 0x100
-	      || (reg & 0xff00) == 0x200)
-	     && ((reg & 0x00ff) == 0x50
-	      || (reg & 0x00ff) == 0x53
-	      || (reg & 0x00ff) == 0x55));
-}
-
-/* Registers 0x50-0x5f are banked */
+/*
+ * On older chips, only registers 0x50-0x5f are banked.
+ * On more recent chips, all registers are banked.
+ * Assume that is the case and set the bank number for each access.
+ * Cache the bank number so it only needs to be set if it changes.
+ */
 static inline void w83627ehf_set_bank(struct w83627ehf_data *data, u16 reg)
 {
-	if ((reg & 0x00f0) == 0x50) {
+	u8 bank = reg >> 8;
+	if (data->bank != bank) {
 		outb_p(W83627EHF_REG_BANK, data->addr + ADDR_REG_OFFSET);
-		outb_p(reg >> 8, data->addr + DATA_REG_OFFSET);
-	}
-}
-
-/* Not strictly necessary, but play it safe for now */
-static inline void w83627ehf_reset_bank(struct w83627ehf_data *data, u16 reg)
-{
-	if (reg & 0xff00) {
-		outb_p(W83627EHF_REG_BANK, data->addr + ADDR_REG_OFFSET);
-		outb_p(0, data->addr + DATA_REG_OFFSET);
+		outb_p(bank, data->addr + DATA_REG_OFFSET);
+		data->bank = bank;
 	}
 }
 
@@ -385,14 +531,13 @@ static u16 w83627ehf_read_value(struct w83627ehf_data *data, u16 reg)
 		       data->addr + ADDR_REG_OFFSET);
 		res = (res << 8) + inb_p(data->addr + DATA_REG_OFFSET);
 	}
-	w83627ehf_reset_bank(data, reg);
 
 	mutex_unlock(&data->lock);
-
 	return res;
 }
 
-static int w83627ehf_write_value(struct w83627ehf_data *data, u16 reg, u16 value)
+static int w83627ehf_write_value(struct w83627ehf_data *data, u16 reg,
+				 u16 value)
 {
 	int word_sized = is_word_sized(reg);
 
@@ -406,13 +551,40 @@ static int w83627ehf_write_value(struct w83627ehf_data *data, u16 reg, u16 value
 		       data->addr + ADDR_REG_OFFSET);
 	}
 	outb_p(value & 0xff, data->addr + DATA_REG_OFFSET);
-	w83627ehf_reset_bank(data, reg);
 
 	mutex_unlock(&data->lock);
 	return 0;
 }
 
 /* This function assumes that the caller holds data->update_lock */
+static void nct6775_write_fan_div(struct w83627ehf_data *data, int nr)
+{
+	u8 reg;
+
+	switch (nr) {
+	case 0:
+		reg = (w83627ehf_read_value(data, NCT6775_REG_FANDIV1) & 0x70)
+		    | (data->fan_div[0] & 0x7);
+		w83627ehf_write_value(data, NCT6775_REG_FANDIV1, reg);
+		break;
+	case 1:
+		reg = (w83627ehf_read_value(data, NCT6775_REG_FANDIV1) & 0x7)
+		    | ((data->fan_div[1] << 4) & 0x70);
+		w83627ehf_write_value(data, NCT6775_REG_FANDIV1, reg);
+	case 2:
+		reg = (w83627ehf_read_value(data, NCT6775_REG_FANDIV2) & 0x70)
+		    | (data->fan_div[2] & 0x7);
+		w83627ehf_write_value(data, NCT6775_REG_FANDIV2, reg);
+		break;
+	case 3:
+		reg = (w83627ehf_read_value(data, NCT6775_REG_FANDIV2) & 0x7)
+		    | ((data->fan_div[3] << 4) & 0x70);
+		w83627ehf_write_value(data, NCT6775_REG_FANDIV2, reg);
+		break;
+	}
+}
+
+/* This function assumes that the caller holds data->update_lock */
 static void w83627ehf_write_fan_div(struct w83627ehf_data *data, int nr)
 {
 	u8 reg;
@@ -463,6 +635,32 @@ static void w83627ehf_write_fan_div(struct w83627ehf_data *data, int nr)
 	}
 }
 
+static void w83627ehf_write_fan_div_common(struct device *dev,
+					   struct w83627ehf_data *data, int nr)
+{
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
+
+	if (sio_data->kind == nct6776)
+		; /* no dividers, do nothing */
+	else if (sio_data->kind == nct6775)
+		nct6775_write_fan_div(data, nr);
+	else
+		w83627ehf_write_fan_div(data, nr);
+}
+
+static void nct6775_update_fan_div(struct w83627ehf_data *data)
+{
+	u8 i;
+
+	i = w83627ehf_read_value(data, NCT6775_REG_FANDIV1);
+	data->fan_div[0] = i & 0x7;
+	data->fan_div[1] = (i & 0x70) >> 4;
+	i = w83627ehf_read_value(data, NCT6775_REG_FANDIV2);
+	data->fan_div[2] = i & 0x7;
+	if (data->has_fan & (1<<3))
+		data->fan_div[3] = (i & 0x70) >> 4;
+}
+
 static void w83627ehf_update_fan_div(struct w83627ehf_data *data)
 {
 	int i;
@@ -488,10 +686,79 @@ static void w83627ehf_update_fan_div(struct w83627ehf_data *data)
 	}
 }
 
+static void w83627ehf_update_fan_div_common(struct device *dev,
+					    struct w83627ehf_data *data)
+{
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
+
+	if (sio_data->kind == nct6776)
+		; /* no dividers, do nothing */
+	else if (sio_data->kind == nct6775)
+		nct6775_update_fan_div(data);
+	else
+		w83627ehf_update_fan_div(data);
+}
+
+static void nct6775_update_pwm(struct w83627ehf_data *data)
+{
+	int i;
+	int pwmcfg, fanmodecfg;
+
+	for (i = 0; i < data->pwm_num; i++) {
+		pwmcfg = w83627ehf_read_value(data,
+					      W83627EHF_REG_PWM_ENABLE[i]);
+		fanmodecfg = w83627ehf_read_value(data,
+						  NCT6775_REG_FAN_MODE[i]);
+		data->pwm_mode[i] =
+		  ((pwmcfg >> W83627EHF_PWM_MODE_SHIFT[i]) & 1) ? 0 : 1;
+		data->pwm_enable[i] = ((fanmodecfg >> 4) & 7) + 1;
+		data->tolerance[i] = fanmodecfg & 0x0f;
+		data->pwm[i] = w83627ehf_read_value(data, data->REG_PWM[i]);
+	}
+}
+
+static void w83627ehf_update_pwm(struct w83627ehf_data *data)
+{
+	int i;
+	int pwmcfg = 0, tolerance = 0; /* shut up the compiler */
+
+	for (i = 0; i < data->pwm_num; i++) {
+		if (!(data->has_fan & (1 << i)))
+			continue;
+
+		/* pwmcfg, tolerance mapped for i=0, i=1 to same reg */
+		if (i != 1) {
+			pwmcfg = w83627ehf_read_value(data,
+					W83627EHF_REG_PWM_ENABLE[i]);
+			tolerance = w83627ehf_read_value(data,
+					W83627EHF_REG_TOLERANCE[i]);
+		}
+		data->pwm_mode[i] =
+			((pwmcfg >> W83627EHF_PWM_MODE_SHIFT[i]) & 1) ? 0 : 1;
+		data->pwm_enable[i] = ((pwmcfg >> W83627EHF_PWM_ENABLE_SHIFT[i])
+				       & 3) + 1;
+		data->pwm[i] = w83627ehf_read_value(data, data->REG_PWM[i]);
+
+		data->tolerance[i] = (tolerance >> (i == 1 ? 4 : 0)) & 0x0f;
+	}
+}
+
+static void w83627ehf_update_pwm_common(struct device *dev,
+					struct w83627ehf_data *data)
+{
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
+
+	if (sio_data->kind == nct6775 || sio_data->kind == nct6776)
+		nct6775_update_pwm(data);
+	else
+		w83627ehf_update_pwm(data);
+}
+
 static struct w83627ehf_data *w83627ehf_update_device(struct device *dev)
 {
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
-	int pwmcfg = 0, tolerance = 0; /* shut up the compiler */
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
+
 	int i;
 
 	mutex_lock(&data->update_lock);
@@ -499,7 +766,7 @@ static struct w83627ehf_data *w83627ehf_update_device(struct device *dev)
 	if (time_after(jiffies, data->last_updated + HZ + HZ/2)
 	 || !data->valid) {
 		/* Fan clock dividers */
-		w83627ehf_update_fan_div(data);
+		w83627ehf_update_fan_div_common(dev, data);
 
 		/* Measured voltages and limits */
 		for (i = 0; i < data->in_num; i++) {
@@ -513,92 +780,90 @@ static struct w83627ehf_data *w83627ehf_update_device(struct device *dev)
 
 		/* Measured fan speeds and limits */
 		for (i = 0; i < 5; i++) {
+			u16 reg;
+
 			if (!(data->has_fan & (1 << i)))
 				continue;
 
-			data->fan[i] = w83627ehf_read_value(data,
-				       W83627EHF_REG_FAN[i]);
-			data->fan_min[i] = w83627ehf_read_value(data,
-					   W83627EHF_REG_FAN_MIN[i]);
+			reg = w83627ehf_read_value(data, data->REG_FAN[i]);
+			data->rpm[i] = data->fan_from_reg(reg,
+							  data->fan_div[i]);
+
+			if (data->has_fan_min & (1 << i))
+				data->fan_min[i] = w83627ehf_read_value(data,
+					   data->REG_FAN_MIN[i]);
 
 			/* If we failed to measure the fan speed and clock
 			   divider can be increased, let's try that for next
 			   time */
-			if (data->fan[i] == 0xff
-			 && data->fan_div[i] < 0x07) {
-			 	dev_dbg(dev, "Increasing fan%d "
+			if (data->has_fan_div
+			    && (reg >= 0xff || (sio_data->kind == nct6775
+						&& reg == 0x00))
+			    && data->fan_div[i] < 0x07) {
+				dev_dbg(dev, "Increasing fan%d "
 					"clock divider from %u to %u\n",
 					i + 1, div_from_reg(data->fan_div[i]),
 					div_from_reg(data->fan_div[i] + 1));
 				data->fan_div[i]++;
-				w83627ehf_write_fan_div(data, i);
+				w83627ehf_write_fan_div_common(dev, data, i);
 				/* Preserve min limit if possible */
-				if (data->fan_min[i] >= 2
+				if ((data->has_fan_min & (1 << i))
+				 && data->fan_min[i] >= 2
 				 && data->fan_min[i] != 255)
 					w83627ehf_write_value(data,
-						W83627EHF_REG_FAN_MIN[i],
+						data->REG_FAN_MIN[i],
 						(data->fan_min[i] /= 2));
 			}
 		}
 
+		w83627ehf_update_pwm_common(dev, data);
+
 		for (i = 0; i < data->pwm_num; i++) {
 			if (!(data->has_fan & (1 << i)))
 				continue;
 
-			/* pwmcfg, tolerance mapped for i=0, i=1 to same reg */
-			if (i != 1) {
-				pwmcfg = w83627ehf_read_value(data,
-						W83627EHF_REG_PWM_ENABLE[i]);
-				tolerance = w83627ehf_read_value(data,
-						W83627EHF_REG_TOLERANCE[i]);
-			}
-			data->pwm_mode[i] =
-				((pwmcfg >> W83627EHF_PWM_MODE_SHIFT[i]) & 1)
-				? 0 : 1;
-			data->pwm_enable[i] =
-					((pwmcfg >> W83627EHF_PWM_ENABLE_SHIFT[i])
-						& 3) + 1;
-			data->pwm[i] = w83627ehf_read_value(data,
-						W83627EHF_REG_PWM[i]);
-			data->fan_start_output[i] = w83627ehf_read_value(data,
-						W83627EHF_REG_FAN_START_OUTPUT[i]);
-			data->fan_stop_output[i] = w83627ehf_read_value(data,
-						W83627EHF_REG_FAN_STOP_OUTPUT[i]);
-			data->fan_stop_time[i] = w83627ehf_read_value(data,
-						W83627EHF_REG_FAN_STOP_TIME[i]);
-
-			if (data->REG_FAN_MAX_OUTPUT[i] != 0xff)
+			data->fan_start_output[i] =
+			  w83627ehf_read_value(data,
+					       data->REG_FAN_START_OUTPUT[i]);
+			data->fan_stop_output[i] =
+			  w83627ehf_read_value(data,
+					       data->REG_FAN_STOP_OUTPUT[i]);
+			data->fan_stop_time[i] =
+			  w83627ehf_read_value(data,
+					       data->REG_FAN_STOP_TIME[i]);
+
+			if (data->REG_FAN_MAX_OUTPUT &&
+			    data->REG_FAN_MAX_OUTPUT[i] != 0xff)
 				data->fan_max_output[i] =
 				  w83627ehf_read_value(data,
-					       data->REG_FAN_MAX_OUTPUT[i]);
+						data->REG_FAN_MAX_OUTPUT[i]);
 
-			if (data->REG_FAN_STEP_OUTPUT[i] != 0xff)
+			if (data->REG_FAN_STEP_OUTPUT &&
+			    data->REG_FAN_STEP_OUTPUT[i] != 0xff)
 				data->fan_step_output[i] =
 				  w83627ehf_read_value(data,
-					       data->REG_FAN_STEP_OUTPUT[i]);
+						data->REG_FAN_STEP_OUTPUT[i]);
 
 			data->target_temp[i] =
 				w83627ehf_read_value(data,
-					W83627EHF_REG_TARGET[i]) &
+					data->REG_TARGET[i]) &
 					(data->pwm_mode[i] == 1 ? 0x7f : 0xff);
-			data->tolerance[i] = (tolerance >> (i == 1 ? 4 : 0))
-									& 0x0f;
 		}
 
 		/* Measured temperatures and limits */
-		data->temp1 = w83627ehf_read_value(data,
-			      W83627EHF_REG_TEMP1);
-		data->temp1_max = w83627ehf_read_value(data,
-				  W83627EHF_REG_TEMP1_OVER);
-		data->temp1_max_hyst = w83627ehf_read_value(data,
-				       W83627EHF_REG_TEMP1_HYST);
-		for (i = 0; i < 2; i++) {
+		for (i = 0; i < NUM_REG_TEMP; i++) {
+			if (!(data->have_temp & (1 << i)))
+				continue;
 			data->temp[i] = w83627ehf_read_value(data,
-					W83627EHF_REG_TEMP[i]);
-			data->temp_max[i] = w83627ehf_read_value(data,
-					    W83627EHF_REG_TEMP_OVER[i]);
-			data->temp_max_hyst[i] = w83627ehf_read_value(data,
-						 W83627EHF_REG_TEMP_HYST[i]);
+						data->reg_temp[i]);
+			if (data->reg_temp_over[i])
+				data->temp_max[i]
+				  = w83627ehf_read_value(data,
+						data->reg_temp_over[i]);
+			if (data->reg_temp_hyst[i])
+				data->temp_max_hyst[i]
+				  = w83627ehf_read_value(data,
+						data->reg_temp_hyst[i]);
 		}
 
 		data->alarms = w83627ehf_read_value(data,
@@ -625,7 +890,8 @@ show_##reg(struct device *dev, struct device_attribute *attr, \
 	   char *buf) \
 { \
 	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
 	return sprintf(buf, "%ld\n", in_from_reg(data->reg[nr], nr)); \
 }
@@ -635,14 +901,18 @@ show_in_reg(in_max)
 
 #define store_in_reg(REG, reg) \
 static ssize_t \
-store_in_##reg (struct device *dev, struct device_attribute *attr, \
-			const char *buf, size_t count) \
+store_in_##reg(struct device *dev, struct device_attribute *attr, \
+	       const char *buf, size_t count) \
 { \
 	struct w83627ehf_data *data = dev_get_drvdata(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
-	u32 val = simple_strtoul(buf, NULL, 10); \
- \
+	unsigned long val; \
+	int err; \
+	err = strict_strtoul(buf, 10, &val); \
+	if (err < 0) \
+		return err; \
 	mutex_lock(&data->update_lock); \
 	data->in_##reg[nr] = in_to_reg(val, nr); \
 	w83627ehf_write_value(data, W83627EHF_REG_IN_##REG(nr), \
@@ -654,7 +924,8 @@ store_in_##reg (struct device *dev, struct device_attribute *attr, \
 store_in_reg(MIN, min)
 store_in_reg(MAX, max)
 
-static ssize_t show_alarm(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
+			  char *buf)
 {
 	struct w83627ehf_data *data = w83627ehf_update_device(dev);
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
@@ -689,45 +960,50 @@ static struct sensor_device_attribute sda_in_alarm[] = {
 };
 
 static struct sensor_device_attribute sda_in_min[] = {
-       SENSOR_ATTR(in0_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 0),
-       SENSOR_ATTR(in1_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 1),
-       SENSOR_ATTR(in2_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 2),
-       SENSOR_ATTR(in3_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 3),
-       SENSOR_ATTR(in4_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 4),
-       SENSOR_ATTR(in5_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 5),
-       SENSOR_ATTR(in6_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 6),
-       SENSOR_ATTR(in7_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 7),
-       SENSOR_ATTR(in8_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 8),
-       SENSOR_ATTR(in9_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 9),
+	SENSOR_ATTR(in0_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 0),
+	SENSOR_ATTR(in1_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 1),
+	SENSOR_ATTR(in2_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 2),
+	SENSOR_ATTR(in3_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 3),
+	SENSOR_ATTR(in4_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 4),
+	SENSOR_ATTR(in5_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 5),
+	SENSOR_ATTR(in6_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 6),
+	SENSOR_ATTR(in7_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 7),
+	SENSOR_ATTR(in8_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 8),
+	SENSOR_ATTR(in9_min, S_IWUSR | S_IRUGO, show_in_min, store_in_min, 9),
 };
 
 static struct sensor_device_attribute sda_in_max[] = {
-       SENSOR_ATTR(in0_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 0),
-       SENSOR_ATTR(in1_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 1),
-       SENSOR_ATTR(in2_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 2),
-       SENSOR_ATTR(in3_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 3),
-       SENSOR_ATTR(in4_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 4),
-       SENSOR_ATTR(in5_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 5),
-       SENSOR_ATTR(in6_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 6),
-       SENSOR_ATTR(in7_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 7),
-       SENSOR_ATTR(in8_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 8),
-       SENSOR_ATTR(in9_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 9),
+	SENSOR_ATTR(in0_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 0),
+	SENSOR_ATTR(in1_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 1),
+	SENSOR_ATTR(in2_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 2),
+	SENSOR_ATTR(in3_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 3),
+	SENSOR_ATTR(in4_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 4),
+	SENSOR_ATTR(in5_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 5),
+	SENSOR_ATTR(in6_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 6),
+	SENSOR_ATTR(in7_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 7),
+	SENSOR_ATTR(in8_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 8),
+	SENSOR_ATTR(in9_max, S_IWUSR | S_IRUGO, show_in_max, store_in_max, 9),
 };
 
-#define show_fan_reg(reg) \
-static ssize_t \
-show_##reg(struct device *dev, struct device_attribute *attr, \
-	   char *buf) \
-{ \
-	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
-	int nr = sensor_attr->index; \
-	return sprintf(buf, "%d\n", \
-		       fan_from_reg(data->reg[nr], \
-				    div_from_reg(data->fan_div[nr]))); \
+static ssize_t
+show_fan(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct w83627ehf_data *data = w83627ehf_update_device(dev);
+	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
+	int nr = sensor_attr->index;
+	return sprintf(buf, "%d\n", data->rpm[nr]);
+}
+
+static ssize_t
+show_fan_min(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct w83627ehf_data *data = w83627ehf_update_device(dev);
+	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
+	int nr = sensor_attr->index;
+	return sprintf(buf, "%d\n",
+		       data->fan_from_reg_min(data->fan_min[nr],
+					      data->fan_div[nr]));
 }
-show_fan_reg(fan);
-show_fan_reg(fan_min);
 
 static ssize_t
 show_fan_div(struct device *dev, struct device_attribute *attr,
@@ -746,11 +1022,32 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int nr = sensor_attr->index;
-	unsigned int val = simple_strtoul(buf, NULL, 10);
+	unsigned long val;
+	int err;
 	unsigned int reg;
 	u8 new_div;
 
+	err = strict_strtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
 	mutex_lock(&data->update_lock);
+	if (!data->has_fan_div) {
+		/*
+		 * Only NCT6776F for now, so we know that this is a 13 bit
+		 * register
+		 */
+		if (!val) {
+			val = 0xff1f;
+		} else {
+			if (val > 1350000U)
+				val = 135000U;
+			val = 1350000U / val;
+			val = (val & 0x1f) | ((val << 3) & 0xff00);
+		}
+		data->fan_min[nr] = val;
+		goto done;	/* Leave fan divider alone */
+	}
 	if (!val) {
 		/* No min limit, alarm disabled */
 		data->fan_min[nr] = 255;
@@ -761,15 +1058,17 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 		   even with the highest divider (128) */
 		data->fan_min[nr] = 254;
 		new_div = 7; /* 128 == (1 << 7) */
-		dev_warn(dev, "fan%u low limit %u below minimum %u, set to "
-			 "minimum\n", nr + 1, val, fan_from_reg(254, 128));
+		dev_warn(dev, "fan%u low limit %lu below minimum %u, set to "
+			 "minimum\n", nr + 1, val,
+			 data->fan_from_reg_min(254, 7));
 	} else if (!reg) {
 		/* Speed above this value cannot possibly be represented,
 		   even with the lowest divider (1) */
 		data->fan_min[nr] = 1;
 		new_div = 0; /* 1 == (1 << 0) */
-		dev_warn(dev, "fan%u low limit %u above maximum %u, set to "
-			 "maximum\n", nr + 1, val, fan_from_reg(1, 1));
+		dev_warn(dev, "fan%u low limit %lu above maximum %u, set to "
+			 "maximum\n", nr + 1, val,
+			 data->fan_from_reg_min(1, 0));
 	} else {
 		/* Automatically pick the best divider, i.e. the one such
 		   that the min limit will correspond to a register value
@@ -785,25 +1084,16 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 	/* Write both the fan clock divider (if it changed) and the new
 	   fan min (unconditionally) */
 	if (new_div != data->fan_div[nr]) {
-		/* Preserve the fan speed reading */
-		if (data->fan[nr] != 0xff) {
-			if (new_div > data->fan_div[nr])
-				data->fan[nr] >>= new_div - data->fan_div[nr];
-			else if (data->fan[nr] & 0x80)
-				data->fan[nr] = 0xff;
-			else
-				data->fan[nr] <<= data->fan_div[nr] - new_div;
-		}
-
 		dev_dbg(dev, "fan%u clock divider changed from %u to %u\n",
 			nr + 1, div_from_reg(data->fan_div[nr]),
 			div_from_reg(new_div));
 		data->fan_div[nr] = new_div;
-		w83627ehf_write_fan_div(data, nr);
+		w83627ehf_write_fan_div_common(dev, data, nr);
 		/* Give the chip time to sample a new speed value */
 		data->last_updated = jiffies;
 	}
-	w83627ehf_write_value(data, W83627EHF_REG_FAN_MIN[nr],
+done:
+	w83627ehf_write_value(data, data->REG_FAN_MIN[nr],
 			      data->fan_min[nr]);
 	mutex_unlock(&data->update_lock);
 
@@ -847,70 +1137,54 @@ static struct sensor_device_attribute sda_fan_div[] = {
 	SENSOR_ATTR(fan5_div, S_IRUGO, show_fan_div, NULL, 4),
 };
 
-#define show_temp1_reg(reg) \
-static ssize_t \
-show_##reg(struct device *dev, struct device_attribute *attr, \
-	   char *buf) \
-{ \
-	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	return sprintf(buf, "%d\n", temp1_from_reg(data->reg)); \
-}
-show_temp1_reg(temp1);
-show_temp1_reg(temp1_max);
-show_temp1_reg(temp1_max_hyst);
-
-#define store_temp1_reg(REG, reg) \
-static ssize_t \
-store_temp1_##reg(struct device *dev, struct device_attribute *attr, \
-		  const char *buf, size_t count) \
-{ \
-	struct w83627ehf_data *data = dev_get_drvdata(dev); \
-	long val = simple_strtol(buf, NULL, 10); \
- \
-	mutex_lock(&data->update_lock); \
-	data->temp1_##reg = temp1_to_reg(val, -128000, 127000); \
-	w83627ehf_write_value(data, W83627EHF_REG_TEMP1_##REG, \
-			      data->temp1_##reg); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t
+show_temp_label(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct w83627ehf_data *data = w83627ehf_update_device(dev);
+	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
+	int nr = sensor_attr->index;
+	return sprintf(buf, "%s\n", data->temp_label[data->temp_src[nr]]);
 }
-store_temp1_reg(OVER, max);
-store_temp1_reg(HYST, max_hyst);
 
-#define show_temp_reg(reg) \
+#define show_temp_reg(addr, reg) \
 static ssize_t \
 show_##reg(struct device *dev, struct device_attribute *attr, \
 	   char *buf) \
 { \
 	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
 	return sprintf(buf, "%d\n", \
-		       LM75_TEMP_FROM_REG(data->reg[nr])); \
+		       temp_from_reg(data->addr[nr], data->reg[nr])); \
 }
-show_temp_reg(temp);
-show_temp_reg(temp_max);
-show_temp_reg(temp_max_hyst);
+show_temp_reg(reg_temp, temp);
+show_temp_reg(reg_temp_over, temp_max);
+show_temp_reg(reg_temp_hyst, temp_max_hyst);
 
-#define store_temp_reg(REG, reg) \
+#define store_temp_reg(addr, reg) \
 static ssize_t \
 store_##reg(struct device *dev, struct device_attribute *attr, \
 	    const char *buf, size_t count) \
 { \
 	struct w83627ehf_data *data = dev_get_drvdata(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
-	long val = simple_strtol(buf, NULL, 10); \
- \
+	int err; \
+	long val; \
+	err = strict_strtol(buf, 10, &val); \
+	if (err < 0) \
+		return err; \
 	mutex_lock(&data->update_lock); \
-	data->reg[nr] = LM75_TEMP_TO_REG(val); \
-	w83627ehf_write_value(data, W83627EHF_REG_TEMP_##REG[nr], \
+	data->reg[nr] = temp_to_reg(data->addr[nr], val); \
+	w83627ehf_write_value(data, data->addr[nr], \
 			      data->reg[nr]); \
 	mutex_unlock(&data->update_lock); \
 	return count; \
 }
-store_temp_reg(OVER, temp_max);
-store_temp_reg(HYST, temp_max_hyst);
+store_temp_reg(reg_temp_over, temp_max);
+store_temp_reg(reg_temp_hyst, temp_max_hyst);
 
 static ssize_t
 show_temp_type(struct device *dev, struct device_attribute *attr, char *buf)
@@ -922,27 +1196,69 @@ show_temp_type(struct device *dev, struct device_attribute *attr, char *buf)
 }
 
 static struct sensor_device_attribute sda_temp_input[] = {
-	SENSOR_ATTR(temp1_input, S_IRUGO, show_temp1, NULL, 0),
-	SENSOR_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 0),
-	SENSOR_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 1),
+	SENSOR_ATTR(temp1_input, S_IRUGO, show_temp, NULL, 0),
+	SENSOR_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 1),
+	SENSOR_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 2),
+	SENSOR_ATTR(temp4_input, S_IRUGO, show_temp, NULL, 3),
+	SENSOR_ATTR(temp5_input, S_IRUGO, show_temp, NULL, 4),
+	SENSOR_ATTR(temp6_input, S_IRUGO, show_temp, NULL, 5),
+	SENSOR_ATTR(temp7_input, S_IRUGO, show_temp, NULL, 6),
+	SENSOR_ATTR(temp8_input, S_IRUGO, show_temp, NULL, 7),
+	SENSOR_ATTR(temp9_input, S_IRUGO, show_temp, NULL, 8),
+};
+
+static struct sensor_device_attribute sda_temp_label[] = {
+	SENSOR_ATTR(temp1_label, S_IRUGO, show_temp_label, NULL, 0),
+	SENSOR_ATTR(temp2_label, S_IRUGO, show_temp_label, NULL, 1),
+	SENSOR_ATTR(temp3_label, S_IRUGO, show_temp_label, NULL, 2),
+	SENSOR_ATTR(temp4_label, S_IRUGO, show_temp_label, NULL, 3),
+	SENSOR_ATTR(temp5_label, S_IRUGO, show_temp_label, NULL, 4),
+	SENSOR_ATTR(temp6_label, S_IRUGO, show_temp_label, NULL, 5),
+	SENSOR_ATTR(temp7_label, S_IRUGO, show_temp_label, NULL, 6),
+	SENSOR_ATTR(temp8_label, S_IRUGO, show_temp_label, NULL, 7),
+	SENSOR_ATTR(temp9_label, S_IRUGO, show_temp_label, NULL, 8),
 };
 
 static struct sensor_device_attribute sda_temp_max[] = {
-	SENSOR_ATTR(temp1_max, S_IRUGO | S_IWUSR, show_temp1_max,
-		    store_temp1_max, 0),
-	SENSOR_ATTR(temp2_max, S_IRUGO | S_IWUSR, show_temp_max,
+	SENSOR_ATTR(temp1_max, S_IRUGO | S_IWUSR, show_temp_max,
 		    store_temp_max, 0),
-	SENSOR_ATTR(temp3_max, S_IRUGO | S_IWUSR, show_temp_max,
+	SENSOR_ATTR(temp2_max, S_IRUGO | S_IWUSR, show_temp_max,
 		    store_temp_max, 1),
+	SENSOR_ATTR(temp3_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 2),
+	SENSOR_ATTR(temp4_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 3),
+	SENSOR_ATTR(temp5_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 4),
+	SENSOR_ATTR(temp6_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 5),
+	SENSOR_ATTR(temp7_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 6),
+	SENSOR_ATTR(temp8_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 7),
+	SENSOR_ATTR(temp9_max, S_IRUGO | S_IWUSR, show_temp_max,
+		    store_temp_max, 8),
 };
 
 static struct sensor_device_attribute sda_temp_max_hyst[] = {
-	SENSOR_ATTR(temp1_max_hyst, S_IRUGO | S_IWUSR, show_temp1_max_hyst,
-		    store_temp1_max_hyst, 0),
-	SENSOR_ATTR(temp2_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+	SENSOR_ATTR(temp1_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
 		    store_temp_max_hyst, 0),
-	SENSOR_ATTR(temp3_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+	SENSOR_ATTR(temp2_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
 		    store_temp_max_hyst, 1),
+	SENSOR_ATTR(temp3_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 2),
+	SENSOR_ATTR(temp4_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 3),
+	SENSOR_ATTR(temp5_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 4),
+	SENSOR_ATTR(temp6_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 5),
+	SENSOR_ATTR(temp7_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 6),
+	SENSOR_ATTR(temp8_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 7),
+	SENSOR_ATTR(temp9_max_hyst, S_IRUGO | S_IWUSR, show_temp_max_hyst,
+		    store_temp_max_hyst, 8),
 };
 
 static struct sensor_device_attribute sda_temp_alarm[] = {
@@ -958,11 +1274,12 @@ static struct sensor_device_attribute sda_temp_type[] = {
 };
 
 #define show_pwm_reg(reg) \
-static ssize_t show_##reg (struct device *dev, struct device_attribute *attr, \
-				char *buf) \
+static ssize_t show_##reg(struct device *dev, struct device_attribute *attr, \
+			  char *buf) \
 { \
 	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
 	return sprintf(buf, "%d\n", data->reg[nr]); \
 }
@@ -978,9 +1295,14 @@ store_pwm_mode(struct device *dev, struct device_attribute *attr,
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int nr = sensor_attr->index;
-	u32 val = simple_strtoul(buf, NULL, 10);
+	unsigned long val;
+	int err;
 	u16 reg;
 
+	err = strict_strtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
 	if (val > 1)
 		return -EINVAL;
 	mutex_lock(&data->update_lock);
@@ -1001,11 +1323,18 @@ store_pwm(struct device *dev, struct device_attribute *attr,
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int nr = sensor_attr->index;
-	u32 val = SENSORS_LIMIT(simple_strtoul(buf, NULL, 10), 0, 255);
+	unsigned long val;
+	int err;
+
+	err = strict_strtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = SENSORS_LIMIT(val, 0, 255);
 
 	mutex_lock(&data->update_lock);
 	data->pwm[nr] = val;
-	w83627ehf_write_value(data, W83627EHF_REG_PWM[nr], val);
+	w83627ehf_write_value(data, data->REG_PWM[nr], val);
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -1015,19 +1344,38 @@ store_pwm_enable(struct device *dev, struct device_attribute *attr,
 			const char *buf, size_t count)
 {
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int nr = sensor_attr->index;
-	u32 val = simple_strtoul(buf, NULL, 10);
+	unsigned long val;
+	int err;
 	u16 reg;
 
-	if (!val || (val > 4))
+	err = strict_strtoul(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	if (!val || (val > 4 && val != data->pwm_enable_orig[nr]))
 		return -EINVAL;
+	/* SmartFan III mode is not supported on NCT6776F */
+	if (sio_data->kind == nct6776 && val == 4)
+		return -EINVAL;
+
 	mutex_lock(&data->update_lock);
-	reg = w83627ehf_read_value(data, W83627EHF_REG_PWM_ENABLE[nr]);
 	data->pwm_enable[nr] = val;
-	reg &= ~(0x03 << W83627EHF_PWM_ENABLE_SHIFT[nr]);
-	reg |= (val - 1) << W83627EHF_PWM_ENABLE_SHIFT[nr];
-	w83627ehf_write_value(data, W83627EHF_REG_PWM_ENABLE[nr], reg);
+	if (sio_data->kind == nct6775 || sio_data->kind == nct6776) {
+		reg = w83627ehf_read_value(data,
+					   NCT6775_REG_FAN_MODE[nr]);
+		reg &= 0x0f;
+		reg |= (val - 1) << 4;
+		w83627ehf_write_value(data,
+				      NCT6775_REG_FAN_MODE[nr], reg);
+	} else {
+		reg = w83627ehf_read_value(data, W83627EHF_REG_PWM_ENABLE[nr]);
+		reg &= ~(0x03 << W83627EHF_PWM_ENABLE_SHIFT[nr]);
+		reg |= (val - 1) << W83627EHF_PWM_ENABLE_SHIFT[nr];
+		w83627ehf_write_value(data, W83627EHF_REG_PWM_ENABLE[nr], reg);
+	}
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -1038,9 +1386,10 @@ static ssize_t show_##reg(struct device *dev, struct device_attribute *attr, \
 				char *buf) \
 { \
 	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
-	return sprintf(buf, "%d\n", temp1_from_reg(data->reg[nr])); \
+	return sprintf(buf, "%d\n", data->reg[nr] * 1000); \
 }
 
 show_tol_temp(tolerance)
@@ -1053,11 +1402,18 @@ store_target_temp(struct device *dev, struct device_attribute *attr,
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int nr = sensor_attr->index;
-	u8 val = temp1_to_reg(simple_strtoul(buf, NULL, 10), 0, 127000);
+	long val;
+	int err;
+
+	err = strict_strtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	val = SENSORS_LIMIT(DIV_ROUND_CLOSEST(val, 1000), 0, 127);
 
 	mutex_lock(&data->update_lock);
 	data->target_temp[nr] = val;
-	w83627ehf_write_value(data, W83627EHF_REG_TARGET[nr], val);
+	w83627ehf_write_value(data, data->REG_TARGET[nr], val);
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -1067,20 +1423,37 @@ store_tolerance(struct device *dev, struct device_attribute *attr,
 			const char *buf, size_t count)
 {
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int nr = sensor_attr->index;
 	u16 reg;
+	long val;
+	int err;
+
+	err = strict_strtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
 	/* Limit the temp to 0C - 15C */
-	u8 val = temp1_to_reg(simple_strtoul(buf, NULL, 10), 0, 15000);
+	val = SENSORS_LIMIT(DIV_ROUND_CLOSEST(val, 1000), 0, 15);
 
 	mutex_lock(&data->update_lock);
-	reg = w83627ehf_read_value(data, W83627EHF_REG_TOLERANCE[nr]);
-	data->tolerance[nr] = val;
-	if (nr == 1)
-		reg = (reg & 0x0f) | (val << 4);
-	else
+	if (sio_data->kind == nct6775 || sio_data->kind == nct6776) {
+		/* Limit tolerance further for NCT6776F */
+		if (sio_data->kind == nct6776 && val > 7)
+			val = 7;
+		reg = w83627ehf_read_value(data, NCT6775_REG_FAN_MODE[nr]);
 		reg = (reg & 0xf0) | val;
-	w83627ehf_write_value(data, W83627EHF_REG_TOLERANCE[nr], reg);
+		w83627ehf_write_value(data, NCT6775_REG_FAN_MODE[nr], reg);
+	} else {
+		reg = w83627ehf_read_value(data, W83627EHF_REG_TOLERANCE[nr]);
+		if (nr == 1)
+			reg = (reg & 0x0f) | (val << 4);
+		else
+			reg = (reg & 0xf0) | val;
+		w83627ehf_write_value(data, W83627EHF_REG_TOLERANCE[nr], reg);
+	}
+	data->tolerance[nr] = val;
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -1143,18 +1516,25 @@ static ssize_t show_##reg(struct device *dev, struct device_attribute *attr, \
 		       char *buf) \
 { \
 	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
 	return sprintf(buf, "%d\n", data->reg[nr]); \
-}\
+} \
 static ssize_t \
 store_##reg(struct device *dev, struct device_attribute *attr, \
 			    const char *buf, size_t count) \
-{\
+{ \
 	struct w83627ehf_data *data = dev_get_drvdata(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
-	u32 val = SENSORS_LIMIT(simple_strtoul(buf, NULL, 10), 1, 255); \
+	unsigned long val; \
+	int err; \
+	err = strict_strtoul(buf, 10, &val); \
+	if (err < 0) \
+		return err; \
+	val = SENSORS_LIMIT(val, 1, 255); \
 	mutex_lock(&data->update_lock); \
 	data->reg[nr] = val; \
 	w83627ehf_write_value(data, data->REG_##REG[nr], val); \
@@ -1172,10 +1552,12 @@ static ssize_t show_##reg(struct device *dev, struct device_attribute *attr, \
 				char *buf) \
 { \
 	struct w83627ehf_data *data = w83627ehf_update_device(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
 	return sprintf(buf, "%d\n", \
-			step_time_from_reg(data->reg[nr], data->pwm_mode[nr])); \
+			step_time_from_reg(data->reg[nr], \
+					   data->pwm_mode[nr])); \
 } \
 \
 static ssize_t \
@@ -1183,10 +1565,15 @@ store_##reg(struct device *dev, struct device_attribute *attr, \
 			const char *buf, size_t count) \
 { \
 	struct w83627ehf_data *data = dev_get_drvdata(dev); \
-	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr); \
+	struct sensor_device_attribute *sensor_attr = \
+		to_sensor_dev_attr(attr); \
 	int nr = sensor_attr->index; \
-	u8 val = step_time_to_reg(simple_strtoul(buf, NULL, 10), \
-					data->pwm_mode[nr]); \
+	unsigned long val; \
+	int err; \
+	err = strict_strtoul(buf, 10, &val); \
+	if (err < 0) \
+		return err; \
+	val = step_time_to_reg(val, data->pwm_mode[nr]); \
 	mutex_lock(&data->update_lock); \
 	data->reg[nr] = val; \
 	w83627ehf_write_value(data, W83627EHF_REG_##REG[nr], val); \
@@ -1283,7 +1670,8 @@ static void w83627ehf_device_remove_files(struct device *dev)
 	for (i = 0; i < ARRAY_SIZE(sda_sf3_max_step_arrays); i++) {
 		struct sensor_device_attribute *attr =
 		  &sda_sf3_max_step_arrays[i];
-		if (data->REG_FAN_STEP_OUTPUT[attr->index] != 0xff)
+		if (data->REG_FAN_STEP_OUTPUT &&
+		    data->REG_FAN_STEP_OUTPUT[attr->index] != 0xff)
 			device_remove_file(dev, &attr->dev_attr);
 	}
 	for (i = 0; i < ARRAY_SIZE(sda_sf3_arrays_fan4); i++)
@@ -1309,12 +1697,15 @@ static void w83627ehf_device_remove_files(struct device *dev)
 		device_remove_file(dev, &sda_target_temp[i].dev_attr);
 		device_remove_file(dev, &sda_tolerance[i].dev_attr);
 	}
-	for (i = 0; i < 3; i++) {
-		if ((i == 2) && data->temp3_disable)
+	for (i = 0; i < NUM_REG_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
 			continue;
 		device_remove_file(dev, &sda_temp_input[i].dev_attr);
+		device_remove_file(dev, &sda_temp_label[i].dev_attr);
 		device_remove_file(dev, &sda_temp_max[i].dev_attr);
 		device_remove_file(dev, &sda_temp_max_hyst[i].dev_attr);
+		if (i > 2)
+			continue;
 		device_remove_file(dev, &sda_temp_alarm[i].dev_attr);
 		device_remove_file(dev, &sda_temp_type[i].dev_attr);
 	}
@@ -1335,15 +1726,17 @@ static inline void __devinit w83627ehf_init_device(struct w83627ehf_data *data)
 		w83627ehf_write_value(data, W83627EHF_REG_CONFIG,
 				      tmp | 0x01);
 
-	/* Enable temp2 and temp3 if needed */
-	for (i = 0; i < 2; i++) {
-		tmp = w83627ehf_read_value(data,
-					   W83627EHF_REG_TEMP_CONFIG[i]);
-		if ((i == 1) && data->temp3_disable)
+	/* Enable temperature sensors if needed */
+	for (i = 0; i < NUM_REG_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+		if (!data->reg_temp_config[i])
 			continue;
+		tmp = w83627ehf_read_value(data,
+					   data->reg_temp_config[i]);
 		if (tmp & 0x01)
 			w83627ehf_write_value(data,
-					      W83627EHF_REG_TEMP_CONFIG[i],
+					      data->reg_temp_config[i],
 					      tmp & 0xfe);
 	}
 
@@ -1362,13 +1755,39 @@ static inline void __devinit w83627ehf_init_device(struct w83627ehf_data *data)
 	}
 }
 
+static void w82627ehf_swap_tempreg(struct w83627ehf_data *data,
+				   int r1, int r2)
+{
+	u16 tmp;
+
+	tmp = data->temp_src[r1];
+	data->temp_src[r1] = data->temp_src[r2];
+	data->temp_src[r2] = tmp;
+
+	tmp = data->reg_temp[r1];
+	data->reg_temp[r1] = data->reg_temp[r2];
+	data->reg_temp[r2] = tmp;
+
+	tmp = data->reg_temp_over[r1];
+	data->reg_temp_over[r1] = data->reg_temp_over[r2];
+	data->reg_temp_over[r2] = tmp;
+
+	tmp = data->reg_temp_hyst[r1];
+	data->reg_temp_hyst[r1] = data->reg_temp_hyst[r2];
+	data->reg_temp_hyst[r2] = tmp;
+
+	tmp = data->reg_temp_config[r1];
+	data->reg_temp_config[r1] = data->reg_temp_config[r2];
+	data->reg_temp_config[r2] = tmp;
+}
+
 static int __devinit w83627ehf_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct w83627ehf_sio_data *sio_data = dev->platform_data;
 	struct w83627ehf_data *data;
 	struct resource *res;
-	u8 fan4pin, fan5pin, en_vrm10;
+	u8 fan3pin, fan4pin, fan4min, fan5pin, en_vrm10;
 	int i, err = 0;
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
@@ -1380,7 +1799,8 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 		goto exit;
 	}
 
-	if (!(data = kzalloc(sizeof(struct w83627ehf_data), GFP_KERNEL))) {
+	data = kzalloc(sizeof(struct w83627ehf_data), GFP_KERNEL);
+	if (!data) {
 		err = -ENOMEM;
 		goto exit_release;
 	}
@@ -1393,25 +1813,201 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 
 	/* 627EHG and 627EHF have 10 voltage inputs; 627DHG and 667HG have 9 */
 	data->in_num = (sio_data->kind == w83627ehf) ? 10 : 9;
-	/* 667HG has 3 pwms */
+	/* 667HG, NCT6775F, and NCT6776F have 3 pwms */
 	data->pwm_num = (sio_data->kind == w83667hg
-			 || sio_data->kind == w83667hg_b) ? 3 : 4;
+			 || sio_data->kind == w83667hg_b
+			 || sio_data->kind == nct6775
+			 || sio_data->kind == nct6776) ? 3 : 4;
 
+	data->have_temp = 0x07;
 	/* Check temp3 configuration bit for 667HG */
-	if (sio_data->kind == w83667hg || sio_data->kind == w83667hg_b) {
-		data->temp3_disable = w83627ehf_read_value(data,
-					W83627EHF_REG_TEMP_CONFIG[1]) & 0x01;
-		data->in6_skip = !data->temp3_disable;
+	if (sio_data->kind == w83667hg) {
+		u8 reg;
+
+		reg = w83627ehf_read_value(data, W83627EHF_REG_TEMP_CONFIG[2]);
+		if (reg & 0x01)
+			data->have_temp &= ~(1 << 2);
+		else
+			data->in6_skip = 1;	/* either temp3 or in6 */
+	}
+
+	/* Deal with temperature register setup first. */
+	if (sio_data->kind == nct6775 || sio_data->kind == nct6776) {
+		int mask = 0;
+
+		/*
+		 * Display temperature sensor output only if it monitors
+		 * a source other than one already reported. Always display
+		 * first three temperature registers, though.
+		 */
+		for (i = 0; i < NUM_REG_TEMP; i++) {
+			u8 src;
+
+			data->reg_temp[i] = NCT6775_REG_TEMP[i];
+			data->reg_temp_over[i] = NCT6775_REG_TEMP_OVER[i];
+			data->reg_temp_hyst[i] = NCT6775_REG_TEMP_HYST[i];
+			data->reg_temp_config[i] = NCT6775_REG_TEMP_CONFIG[i];
+
+			src = w83627ehf_read_value(data,
+						   NCT6775_REG_TEMP_SOURCE[i]);
+			src &= 0x1f;
+			if (src && !(mask & (1 << src))) {
+				data->have_temp |= 1 << i;
+				mask |= 1 << src;
+			}
+
+			data->temp_src[i] = src;
+
+			/*
+			 * Now do some register swapping if index 0..2 don't
+			 * point to SYSTIN(1), CPUIN(2), and AUXIN(3).
+			 * Idea is to have the first three attributes
+			 * report SYSTIN, CPUIN, and AUXIN if possible
+			 * without overriding the basic system configuration.
+			 */
+			if (i > 0 && data->temp_src[0] != 1
+			    && data->temp_src[i] == 1)
+				w82627ehf_swap_tempreg(data, 0, i);
+			if (i > 1 && data->temp_src[1] != 2
+			    && data->temp_src[i] == 2)
+				w82627ehf_swap_tempreg(data, 1, i);
+			if (i > 2 && data->temp_src[2] != 3
+			    && data->temp_src[i] == 3)
+				w82627ehf_swap_tempreg(data, 2, i);
+		}
+		if (sio_data->kind == nct6776) {
+			/*
+			 * On NCT6776, AUXTIN and VIN3 pins are shared.
+			 * Only way to detect it is to check if AUXTIN is used
+			 * as a temperature source, and if that source is
+			 * enabled.
+			 *
+			 * If that is the case, disable in6, which reports VIN3.
+			 * Otherwise disable temp3.
+			 */
+			if (data->temp_src[2] == 3) {
+				u8 reg;
+
+				if (data->reg_temp_config[2])
+					reg = w83627ehf_read_value(data,
+						data->reg_temp_config[2]);
+				else
+					reg = 0; /* Assume AUXTIN is used */
+
+				if (reg & 0x01)
+					data->have_temp &= ~(1 << 2);
+				else
+					data->in6_skip = 1;
+			}
+		}
+
+		data->temp_label = nct6776_temp_label;
+	} else if (sio_data->kind == w83667hg_b) {
+		u8 reg;
+
+		/*
+		 * Temperature sources are selected with bank 0, registers 0x49
+		 * and 0x4a.
+		 */
+		for (i = 0; i < ARRAY_SIZE(W83627EHF_REG_TEMP); i++) {
+			data->reg_temp[i] = W83627EHF_REG_TEMP[i];
+			data->reg_temp_over[i] = W83627EHF_REG_TEMP_OVER[i];
+			data->reg_temp_hyst[i] = W83627EHF_REG_TEMP_HYST[i];
+			data->reg_temp_config[i] = W83627EHF_REG_TEMP_CONFIG[i];
+		}
+		reg = w83627ehf_read_value(data, 0x4a);
+		data->temp_src[0] = reg >> 5;
+		reg = w83627ehf_read_value(data, 0x49);
+		data->temp_src[1] = reg & 0x07;
+		data->temp_src[2] = (reg >> 4) & 0x07;
+
+		/*
+		 * W83667HG-B has another temperature register at 0x7e.
+		 * The temperature source is selected with register 0x7d.
+		 * Support it if the source differs from already reported
+		 * sources.
+		 */
+		reg = w83627ehf_read_value(data, 0x7d);
+		reg &= 0x07;
+		if (reg != data->temp_src[0] && reg != data->temp_src[1]
+		    && reg != data->temp_src[2]) {
+			data->temp_src[3] = reg;
+			data->have_temp |= 1 << 3;
+		}
+
+		/*
+		 * Chip supports either AUXTIN or VIN3. Try to find out which
+		 * one.
+		 */
+		reg = w83627ehf_read_value(data, W83627EHF_REG_TEMP_CONFIG[2]);
+		if (data->temp_src[2] == 2 && (reg & 0x01))
+			data->have_temp &= ~(1 << 2);
+
+		if ((data->temp_src[2] == 2 && (data->have_temp & (1 << 2)))
+		    || (data->temp_src[3] == 2 && (data->have_temp & (1 << 3))))
+			data->in6_skip = 1;
+
+		data->temp_label = w83667hg_b_temp_label;
+	} else {
+		/* Temperature sources are fixed */
+		for (i = 0; i < 3; i++) {
+			data->reg_temp[i] = W83627EHF_REG_TEMP[i];
+			data->reg_temp_over[i] = W83627EHF_REG_TEMP_OVER[i];
+			data->reg_temp_hyst[i] = W83627EHF_REG_TEMP_HYST[i];
+			data->reg_temp_config[i] = W83627EHF_REG_TEMP_CONFIG[i];
+		}
 	}
 
-	data->REG_FAN_START_OUTPUT = W83627EHF_REG_FAN_START_OUTPUT;
-	data->REG_FAN_STOP_OUTPUT = W83627EHF_REG_FAN_STOP_OUTPUT;
-	if (sio_data->kind == w83667hg_b) {
+	if (sio_data->kind == nct6775) {
+		data->has_fan_div = true;
+		data->fan_from_reg = fan_from_reg16;
+		data->fan_from_reg_min = fan_from_reg8;
+		data->REG_PWM = NCT6775_REG_PWM;
+		data->REG_TARGET = NCT6775_REG_TARGET;
+		data->REG_FAN = NCT6775_REG_FAN;
+		data->REG_FAN_MIN = W83627EHF_REG_FAN_MIN;
+		data->REG_FAN_START_OUTPUT = NCT6775_REG_FAN_START_OUTPUT;
+		data->REG_FAN_STOP_OUTPUT = NCT6775_REG_FAN_STOP_OUTPUT;
+		data->REG_FAN_STOP_TIME = NCT6775_REG_FAN_STOP_TIME;
+		data->REG_FAN_MAX_OUTPUT = NCT6775_REG_FAN_MAX_OUTPUT;
+		data->REG_FAN_STEP_OUTPUT = NCT6775_REG_FAN_STEP_OUTPUT;
+	} else if (sio_data->kind == nct6776) {
+		data->has_fan_div = false;
+		data->fan_from_reg = fan_from_reg13;
+		data->fan_from_reg_min = fan_from_reg13;
+		data->REG_PWM = NCT6775_REG_PWM;
+		data->REG_TARGET = NCT6775_REG_TARGET;
+		data->REG_FAN = NCT6775_REG_FAN;
+		data->REG_FAN_MIN = NCT6776_REG_FAN_MIN;
+		data->REG_FAN_START_OUTPUT = NCT6775_REG_FAN_START_OUTPUT;
+		data->REG_FAN_STOP_OUTPUT = NCT6775_REG_FAN_STOP_OUTPUT;
+		data->REG_FAN_STOP_TIME = NCT6775_REG_FAN_STOP_TIME;
+	} else if (sio_data->kind == w83667hg_b) {
+		data->has_fan_div = true;
+		data->fan_from_reg = fan_from_reg8;
+		data->fan_from_reg_min = fan_from_reg8;
+		data->REG_PWM = W83627EHF_REG_PWM;
+		data->REG_TARGET = W83627EHF_REG_TARGET;
+		data->REG_FAN = W83627EHF_REG_FAN;
+		data->REG_FAN_MIN = W83627EHF_REG_FAN_MIN;
+		data->REG_FAN_START_OUTPUT = W83627EHF_REG_FAN_START_OUTPUT;
+		data->REG_FAN_STOP_OUTPUT = W83627EHF_REG_FAN_STOP_OUTPUT;
+		data->REG_FAN_STOP_TIME = W83627EHF_REG_FAN_STOP_TIME;
 		data->REG_FAN_MAX_OUTPUT =
 		  W83627EHF_REG_FAN_MAX_OUTPUT_W83667_B;
 		data->REG_FAN_STEP_OUTPUT =
 		  W83627EHF_REG_FAN_STEP_OUTPUT_W83667_B;
 	} else {
+		data->has_fan_div = true;
+		data->fan_from_reg = fan_from_reg8;
+		data->fan_from_reg_min = fan_from_reg8;
+		data->REG_PWM = W83627EHF_REG_PWM;
+		data->REG_TARGET = W83627EHF_REG_TARGET;
+		data->REG_FAN = W83627EHF_REG_FAN;
+		data->REG_FAN_MIN = W83627EHF_REG_FAN_MIN;
+		data->REG_FAN_START_OUTPUT = W83627EHF_REG_FAN_START_OUTPUT;
+		data->REG_FAN_STOP_OUTPUT = W83627EHF_REG_FAN_STOP_OUTPUT;
+		data->REG_FAN_STOP_TIME = W83627EHF_REG_FAN_STOP_TIME;
 		data->REG_FAN_MAX_OUTPUT =
 		  W83627EHF_REG_FAN_MAX_OUTPUT_COMMON;
 		data->REG_FAN_STEP_OUTPUT =
@@ -1424,7 +2020,8 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 	data->vrm = vid_which_vrm();
 	superio_enter(sio_data->sioreg);
 	/* Read VID value */
-	if (sio_data->kind == w83667hg || sio_data->kind == w83667hg_b) {
+	if (sio_data->kind == w83667hg || sio_data->kind == w83667hg_b ||
+	    sio_data->kind == nct6775 || sio_data->kind == nct6776) {
 		/* W83667HG has different pins for VID input and output, so
 		we can get the VID input values directly at logical device D
 		0xe3. */
@@ -1475,13 +2072,44 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 	}
 
 	/* fan4 and fan5 share some pins with the GPIO and serial flash */
-	if (sio_data->kind == w83667hg || sio_data->kind == w83667hg_b) {
-		fan5pin = superio_inb(sio_data->sioreg, 0x27) & 0x20;
+	if (sio_data->kind == nct6775) {
+		/* On NCT6775, fan4 shares pins with the fdc interface */
+		fan3pin = 1;
+		fan4pin = !(superio_inb(sio_data->sioreg, 0x2A) & 0x80);
+		fan4min = 0;
+		fan5pin = 0;
+	} else if (sio_data->kind == nct6776) {
+		fan3pin = !(superio_inb(sio_data->sioreg, 0x24) & 0x40);
+		fan4pin = !!(superio_inb(sio_data->sioreg, 0x1C) & 0x01);
+		fan5pin = !!(superio_inb(sio_data->sioreg, 0x1C) & 0x02);
+		fan4min = fan4pin;
+	} else if (sio_data->kind == w83667hg || sio_data->kind == w83667hg_b) {
+		fan3pin = 1;
 		fan4pin = superio_inb(sio_data->sioreg, 0x27) & 0x40;
+		fan5pin = superio_inb(sio_data->sioreg, 0x27) & 0x20;
+		fan4min = fan4pin;
 	} else {
-		fan5pin = !(superio_inb(sio_data->sioreg, 0x24) & 0x02);
+		fan3pin = 1;
 		fan4pin = !(superio_inb(sio_data->sioreg, 0x29) & 0x06);
+		fan5pin = !(superio_inb(sio_data->sioreg, 0x24) & 0x02);
+		fan4min = fan4pin;
 	}
+
+	if (fan_debounce &&
+	    (sio_data->kind == nct6775 || sio_data->kind == nct6776)) {
+		u8 tmp;
+
+		superio_select(sio_data->sioreg, W83627EHF_LD_HWM);
+		tmp = superio_inb(sio_data->sioreg, NCT6775_REG_FAN_DEBOUNCE);
+		if (sio_data->kind == nct6776)
+			superio_outb(sio_data->sioreg, NCT6775_REG_FAN_DEBOUNCE,
+				     0x3e | tmp);
+		else
+			superio_outb(sio_data->sioreg, NCT6775_REG_FAN_DEBOUNCE,
+				     0x1e | tmp);
+		pr_info("Enabled fan debounce for chip %s\n", data->name);
+	}
+
 	superio_exit(sio_data->sioreg);
 
 	/* It looks like fan4 and fan5 pins can be alternatively used
@@ -1490,26 +2118,54 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 	   connected fan5 as input unless they are emitting log 1, which
 	   is not the default. */
 
-	data->has_fan = 0x07; /* fan1, fan2 and fan3 */
-	i = w83627ehf_read_value(data, W83627EHF_REG_FANDIV1);
-	if ((i & (1 << 2)) && fan4pin)
-		data->has_fan |= (1 << 3);
-	if (!(i & (1 << 1)) && fan5pin)
-		data->has_fan |= (1 << 4);
+	data->has_fan = data->has_fan_min = 0x03; /* fan1 and fan2 */
+
+	data->has_fan |= (fan3pin << 2);
+	data->has_fan_min |= (fan3pin << 2);
+
+	/*
+	 * NCT6775F and NCT6776F don't have the W83627EHF_REG_FANDIV1 register
+	 */
+	if (sio_data->kind == nct6775 || sio_data->kind == nct6776) {
+		data->has_fan |= (fan4pin << 3) | (fan5pin << 4);
+		data->has_fan_min |= (fan4min << 3) | (fan5pin << 4);
+	} else {
+		i = w83627ehf_read_value(data, W83627EHF_REG_FANDIV1);
+		if ((i & (1 << 2)) && fan4pin) {
+			data->has_fan |= (1 << 3);
+			data->has_fan_min |= (1 << 3);
+		}
+		if (!(i & (1 << 1)) && fan5pin) {
+			data->has_fan |= (1 << 4);
+			data->has_fan_min |= (1 << 4);
+		}
+	}
 
 	/* Read fan clock dividers immediately */
-	w83627ehf_update_fan_div(data);
+	w83627ehf_update_fan_div_common(dev, data);
+
+	/* Read pwm data to save original values */
+	w83627ehf_update_pwm_common(dev, data);
+	for (i = 0; i < data->pwm_num; i++)
+		data->pwm_enable_orig[i] = data->pwm_enable[i];
+
+	/* Read pwm data to save original values */
+	w83627ehf_update_pwm_common(dev, data);
+	for (i = 0; i < data->pwm_num; i++)
+		data->pwm_enable_orig[i] = data->pwm_enable[i];
 
 	/* Register sysfs hooks */
-  	for (i = 0; i < ARRAY_SIZE(sda_sf3_arrays); i++)
-		if ((err = device_create_file(dev,
-			&sda_sf3_arrays[i].dev_attr)))
+	for (i = 0; i < ARRAY_SIZE(sda_sf3_arrays); i++) {
+		err = device_create_file(dev, &sda_sf3_arrays[i].dev_attr);
+		if (err)
 			goto exit_remove;
+	}
 
 	for (i = 0; i < ARRAY_SIZE(sda_sf3_max_step_arrays); i++) {
 		struct sensor_device_attribute *attr =
 		  &sda_sf3_max_step_arrays[i];
-		if (data->REG_FAN_STEP_OUTPUT[attr->index] != 0xff) {
+		if (data->REG_FAN_STEP_OUTPUT &&
+		    data->REG_FAN_STEP_OUTPUT[attr->index] != 0xff) {
 			err = device_create_file(dev, &attr->dev_attr);
 			if (err)
 				goto exit_remove;
@@ -1518,8 +2174,9 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 	/* if fan4 is enabled create the sf3 files for it */
 	if ((data->has_fan & (1 << 3)) && data->pwm_num >= 4)
 		for (i = 0; i < ARRAY_SIZE(sda_sf3_arrays_fan4); i++) {
-			if ((err = device_create_file(dev,
-				&sda_sf3_arrays_fan4[i].dev_attr)))
+			err = device_create_file(dev,
+					&sda_sf3_arrays_fan4[i].dev_attr);
+			if (err)
 				goto exit_remove;
 		}
 
@@ -1541,12 +2198,20 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 			if ((err = device_create_file(dev,
 					&sda_fan_input[i].dev_attr))
 				|| (err = device_create_file(dev,
-					&sda_fan_alarm[i].dev_attr))
-				|| (err = device_create_file(dev,
-					&sda_fan_div[i].dev_attr))
-				|| (err = device_create_file(dev,
-					&sda_fan_min[i].dev_attr)))
+					&sda_fan_alarm[i].dev_attr)))
 				goto exit_remove;
+			if (sio_data->kind != nct6776) {
+				err = device_create_file(dev,
+						&sda_fan_div[i].dev_attr);
+				if (err)
+					goto exit_remove;
+			}
+			if (data->has_fan_min & (1 << i)) {
+				err = device_create_file(dev,
+						&sda_fan_min[i].dev_attr);
+				if (err)
+					goto exit_remove;
+			}
 			if (i < data->pwm_num &&
 				((err = device_create_file(dev,
 					&sda_pwm[i].dev_attr))
@@ -1562,16 +2227,33 @@ static int __devinit w83627ehf_probe(struct platform_device *pdev)
 		}
 	}
 
-	for (i = 0; i < 3; i++) {
-		if ((i == 2) && data->temp3_disable)
+	for (i = 0; i < NUM_REG_TEMP; i++) {
+		if (!(data->have_temp & (1 << i)))
+			continue;
+		err = device_create_file(dev, &sda_temp_input[i].dev_attr);
+		if (err)
+			goto exit_remove;
+		if (data->temp_label) {
+			err = device_create_file(dev,
+						 &sda_temp_label[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->reg_temp_over[i]) {
+			err = device_create_file(dev,
+				&sda_temp_max[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (data->reg_temp_hyst[i]) {
+			err = device_create_file(dev,
+				&sda_temp_max_hyst[i].dev_attr);
+			if (err)
+				goto exit_remove;
+		}
+		if (i > 2)
 			continue;
 		if ((err = device_create_file(dev,
-				&sda_temp_input[i].dev_attr))
-			|| (err = device_create_file(dev,
-				&sda_temp_max[i].dev_attr))
-			|| (err = device_create_file(dev,
-				&sda_temp_max_hyst[i].dev_attr))
-			|| (err = device_create_file(dev,
 				&sda_temp_alarm[i].dev_attr))
 			|| (err = device_create_file(dev,
 				&sda_temp_type[i].dev_attr)))
@@ -1632,6 +2314,8 @@ static int __init w83627ehf_find(int sioaddr, unsigned short *addr,
 	static const char __initdata sio_name_W83627DHG_P[] = "W83627DHG-P";
 	static const char __initdata sio_name_W83667HG[] = "W83667HG";
 	static const char __initdata sio_name_W83667HG_B[] = "W83667HG-B";
+	static const char __initdata sio_name_NCT6775[] = "NCT6775F";
+	static const char __initdata sio_name_NCT6776[] = "NCT6776F";
 
 	u16 val;
 	const char *sio_name;
@@ -1668,6 +2352,14 @@ static int __init w83627ehf_find(int sioaddr, unsigned short *addr,
 		sio_data->kind = w83667hg_b;
 		sio_name = sio_name_W83667HG_B;
 		break;
+	case SIO_NCT6775_ID:
+		sio_data->kind = nct6775;
+		sio_name = sio_name_NCT6775;
+		break;
+	case SIO_NCT6776_ID:
+		sio_data->kind = nct6776;
+		sio_name = sio_name_NCT6776;
+		break;
 	default:
 		if (val != 0xffff)
 			pr_debug("unsupported chip ID: 0x%04x\n", val);
@@ -1689,7 +2381,8 @@ static int __init w83627ehf_find(int sioaddr, unsigned short *addr,
 	/* Activate logical device if needed */
 	val = superio_inb(sioaddr, SIO_REG_ENABLE);
 	if (!(val & 0x01)) {
-		pr_warn("Forcibly enabling Super-I/O. Sensor is probably unusable.\n");
+		pr_warn("Forcibly enabling Super-I/O. "
+			"Sensor is probably unusable.\n");
 		superio_outb(sioaddr, SIO_REG_ENABLE, val | 0x01);
 	}
 
@@ -1726,7 +2419,8 @@ static int __init sensors_w83627ehf_init(void)
 	if (err)
 		goto exit;
 
-	if (!(pdev = platform_device_alloc(DRVNAME, address))) {
+	pdev = platform_device_alloc(DRVNAME, address);
+	if (!pdev) {
 		err = -ENOMEM;
 		pr_err("Device allocation failed\n");
 		goto exit_unregister;
diff --git a/drivers/hwspinlock/Kconfig b/drivers/hwspinlock/Kconfig
new file mode 100644
index 000000000000..eb4af28f8567
--- /dev/null
+++ b/drivers/hwspinlock/Kconfig
@@ -0,0 +1,22 @@
+#
+# Generic HWSPINLOCK framework
+#
+
+config HWSPINLOCK
+	tristate "Generic Hardware Spinlock framework"
+	help
+	  Say y here to support the generic hardware spinlock framework.
+	  You only need to enable this if you have hardware spinlock module
+	  on your system (usually only relevant if your system has remote slave
+	  coprocessors).
+
+	  If unsure, say N.
+
+config HWSPINLOCK_OMAP
+	tristate "OMAP Hardware Spinlock device"
+	depends on HWSPINLOCK && ARCH_OMAP4
+	help
+	  Say y here to support the OMAP Hardware Spinlock device (firstly
+	  introduced in OMAP4).
+
+	  If unsure, say N.
diff --git a/drivers/hwspinlock/Makefile b/drivers/hwspinlock/Makefile
new file mode 100644
index 000000000000..5729a3f7ed3d
--- /dev/null
+++ b/drivers/hwspinlock/Makefile
@@ -0,0 +1,6 @@
+#
+# Generic Hardware Spinlock framework
+#
+
+obj-$(CONFIG_HWSPINLOCK)		+= hwspinlock_core.o
+obj-$(CONFIG_HWSPINLOCK_OMAP)		+= omap_hwspinlock.o
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
new file mode 100644
index 000000000000..43a62714b4fb
--- /dev/null
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -0,0 +1,548 @@
+/*
+ * Hardware spinlock framework
+ *
+ * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Contact: Ohad Ben-Cohen <ohad@wizery.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.
+ */
+
+#define pr_fmt(fmt)    "%s: " fmt, __func__
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/jiffies.h>
+#include <linux/radix-tree.h>
+#include <linux/hwspinlock.h>
+#include <linux/pm_runtime.h>
+
+#include "hwspinlock_internal.h"
+
+/* radix tree tags */
+#define HWSPINLOCK_UNUSED	(0) /* tags an hwspinlock as unused */
+
+/*
+ * A radix tree is used to maintain the available hwspinlock instances.
+ * The tree associates hwspinlock pointers with their integer key id,
+ * and provides easy-to-use API which makes the hwspinlock core code simple
+ * and easy to read.
+ *
+ * Radix trees are quick on lookups, and reasonably efficient in terms of
+ * storage, especially with high density usages such as this framework
+ * requires (a continuous range of integer keys, beginning with zero, is
+ * used as the ID's of the hwspinlock instances).
+ *
+ * The radix tree API supports tagging items in the tree, which this
+ * framework uses to mark unused hwspinlock instances (see the
+ * HWSPINLOCK_UNUSED tag above). As a result, the process of querying the
+ * tree, looking for an unused hwspinlock instance, is now reduced to a
+ * single radix tree API call.
+ */
+static RADIX_TREE(hwspinlock_tree, GFP_KERNEL);
+
+/*
+ * Synchronization of access to the tree is achieved using this spinlock,
+ * as the radix-tree API requires that users provide all synchronisation.
+ */
+static DEFINE_SPINLOCK(hwspinlock_tree_lock);
+
+/**
+ * __hwspin_trylock() - attempt to lock a specific hwspinlock
+ * @hwlock: an hwspinlock which we want to trylock
+ * @mode: controls whether local interrupts are disabled or not
+ * @flags: a pointer where the caller's interrupt state will be saved at (if
+ *         requested)
+ *
+ * This function attempts to lock an hwspinlock, and will immediately
+ * fail if the hwspinlock is already taken.
+ *
+ * Upon a successful return from this function, preemption (and possibly
+ * interrupts) is disabled, so the caller must not sleep, and is advised to
+ * release the hwspinlock as soon as possible. This is required in order to
+ * minimize remote cores polling on the hardware interconnect.
+ *
+ * The user decides whether local interrupts are disabled or not, and if yes,
+ * whether he wants their previous state to be saved. It is up to the user
+ * to choose the appropriate @mode of operation, exactly the same way users
+ * should decide between spin_trylock, spin_trylock_irq and
+ * spin_trylock_irqsave.
+ *
+ * Returns 0 if we successfully locked the hwspinlock or -EBUSY if
+ * the hwspinlock was already taken.
+ * This function will never sleep.
+ */
+int __hwspin_trylock(struct hwspinlock *hwlock, int mode, unsigned long *flags)
+{
+	int ret;
+
+	BUG_ON(!hwlock);
+	BUG_ON(!flags && mode == HWLOCK_IRQSTATE);
+
+	/*
+	 * This spin_lock{_irq, _irqsave} serves three purposes:
+	 *
+	 * 1. Disable preemption, in order to minimize the period of time
+	 *    in which the hwspinlock is taken. This is important in order
+	 *    to minimize the possible polling on the hardware interconnect
+	 *    by a remote user of this lock.
+	 * 2. Make the hwspinlock SMP-safe (so we can take it from
+	 *    additional contexts on the local host).
+	 * 3. Ensure that in_atomic/might_sleep checks catch potential
+	 *    problems with hwspinlock usage (e.g. scheduler checks like
+	 *    'scheduling while atomic' etc.)
+	 */
+	if (mode == HWLOCK_IRQSTATE)
+		ret = spin_trylock_irqsave(&hwlock->lock, *flags);
+	else if (mode == HWLOCK_IRQ)
+		ret = spin_trylock_irq(&hwlock->lock);
+	else
+		ret = spin_trylock(&hwlock->lock);
+
+	/* is lock already taken by another context on the local cpu ? */
+	if (!ret)
+		return -EBUSY;
+
+	/* try to take the hwspinlock device */
+	ret = hwlock->ops->trylock(hwlock);
+
+	/* if hwlock is already taken, undo spin_trylock_* and exit */
+	if (!ret) {
+		if (mode == HWLOCK_IRQSTATE)
+			spin_unlock_irqrestore(&hwlock->lock, *flags);
+		else if (mode == HWLOCK_IRQ)
+			spin_unlock_irq(&hwlock->lock);
+		else
+			spin_unlock(&hwlock->lock);
+
+		return -EBUSY;
+	}
+
+	/*
+	 * We can be sure the other core's memory operations
+	 * are observable to us only _after_ we successfully take
+	 * the hwspinlock, and we must make sure that subsequent memory
+	 * operations (both reads and writes) will not be reordered before
+	 * we actually took the hwspinlock.
+	 *
+	 * Note: the implicit memory barrier of the spinlock above is too
+	 * early, so we need this additional explicit memory barrier.
+	 */
+	mb();
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(__hwspin_trylock);
+
+/**
+ * __hwspin_lock_timeout() - lock an hwspinlock with timeout limit
+ * @hwlock: the hwspinlock to be locked
+ * @timeout: timeout value in msecs
+ * @mode: mode which controls whether local interrupts are disabled or not
+ * @flags: a pointer to where the caller's interrupt state will be saved at (if
+ *         requested)
+ *
+ * This function locks the given @hwlock. If the @hwlock
+ * is already taken, the function will busy loop waiting for it to
+ * be released, but give up after @timeout msecs have elapsed.
+ *
+ * Upon a successful return from this function, preemption is disabled
+ * (and possibly local interrupts, too), so the caller must not sleep,
+ * and is advised to release the hwspinlock as soon as possible.
+ * This is required in order to minimize remote cores polling on the
+ * hardware interconnect.
+ *
+ * The user decides whether local interrupts are disabled or not, and if yes,
+ * whether he wants their previous state to be saved. It is up to the user
+ * to choose the appropriate @mode of operation, exactly the same way users
+ * should decide between spin_lock, spin_lock_irq and spin_lock_irqsave.
+ *
+ * Returns 0 when the @hwlock was successfully taken, and an appropriate
+ * error code otherwise (most notably -ETIMEDOUT if the @hwlock is still
+ * busy after @timeout msecs). The function will never sleep.
+ */
+int __hwspin_lock_timeout(struct hwspinlock *hwlock, unsigned int to,
+					int mode, unsigned long *flags)
+{
+	int ret;
+	unsigned long expire;
+
+	expire = msecs_to_jiffies(to) + jiffies;
+
+	for (;;) {
+		/* Try to take the hwspinlock */
+		ret = __hwspin_trylock(hwlock, mode, flags);
+		if (ret != -EBUSY)
+			break;
+
+		/*
+		 * The lock is already taken, let's check if the user wants
+		 * us to try again
+		 */
+		if (time_is_before_eq_jiffies(expire))
+			return -ETIMEDOUT;
+
+		/*
+		 * Allow platform-specific relax handlers to prevent
+		 * hogging the interconnect (no sleeping, though)
+		 */
+		if (hwlock->ops->relax)
+			hwlock->ops->relax(hwlock);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(__hwspin_lock_timeout);
+
+/**
+ * __hwspin_unlock() - unlock a specific hwspinlock
+ * @hwlock: a previously-acquired hwspinlock which we want to unlock
+ * @mode: controls whether local interrupts needs to be restored or not
+ * @flags: previous caller's interrupt state to restore (if requested)
+ *
+ * This function will unlock a specific hwspinlock, enable preemption and
+ * (possibly) enable interrupts or restore their previous state.
+ * @hwlock must be already locked before calling this function: it is a bug
+ * to call unlock on a @hwlock that is already unlocked.
+ *
+ * The user decides whether local interrupts should be enabled or not, and
+ * if yes, whether he wants their previous state to be restored. It is up
+ * to the user to choose the appropriate @mode of operation, exactly the
+ * same way users decide between spin_unlock, spin_unlock_irq and
+ * spin_unlock_irqrestore.
+ *
+ * The function will never sleep.
+ */
+void __hwspin_unlock(struct hwspinlock *hwlock, int mode, unsigned long *flags)
+{
+	BUG_ON(!hwlock);
+	BUG_ON(!flags && mode == HWLOCK_IRQSTATE);
+
+	/*
+	 * We must make sure that memory operations (both reads and writes),
+	 * done before unlocking the hwspinlock, will not be reordered
+	 * after the lock is released.
+	 *
+	 * That's the purpose of this explicit memory barrier.
+	 *
+	 * Note: the memory barrier induced by the spin_unlock below is too
+	 * late; the other core is going to access memory soon after it will
+	 * take the hwspinlock, and by then we want to be sure our memory
+	 * operations are already observable.
+	 */
+	mb();
+
+	hwlock->ops->unlock(hwlock);
+
+	/* Undo the spin_trylock{_irq, _irqsave} called while locking */
+	if (mode == HWLOCK_IRQSTATE)
+		spin_unlock_irqrestore(&hwlock->lock, *flags);
+	else if (mode == HWLOCK_IRQ)
+		spin_unlock_irq(&hwlock->lock);
+	else
+		spin_unlock(&hwlock->lock);
+}
+EXPORT_SYMBOL_GPL(__hwspin_unlock);
+
+/**
+ * hwspin_lock_register() - register a new hw spinlock
+ * @hwlock: hwspinlock to register.
+ *
+ * This function should be called from the underlying platform-specific
+ * implementation, to register a new hwspinlock instance.
+ *
+ * Can be called from an atomic context (will not sleep) but not from
+ * within interrupt context.
+ *
+ * Returns 0 on success, or an appropriate error code on failure
+ */
+int hwspin_lock_register(struct hwspinlock *hwlock)
+{
+	struct hwspinlock *tmp;
+	int ret;
+
+	if (!hwlock || !hwlock->ops ||
+		!hwlock->ops->trylock || !hwlock->ops->unlock) {
+		pr_err("invalid parameters\n");
+		return -EINVAL;
+	}
+
+	spin_lock_init(&hwlock->lock);
+
+	spin_lock(&hwspinlock_tree_lock);
+
+	ret = radix_tree_insert(&hwspinlock_tree, hwlock->id, hwlock);
+	if (ret)
+		goto out;
+
+	/* mark this hwspinlock as available */
+	tmp = radix_tree_tag_set(&hwspinlock_tree, hwlock->id,
+							HWSPINLOCK_UNUSED);
+
+	/* self-sanity check which should never fail */
+	WARN_ON(tmp != hwlock);
+
+out:
+	spin_unlock(&hwspinlock_tree_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(hwspin_lock_register);
+
+/**
+ * hwspin_lock_unregister() - unregister an hw spinlock
+ * @id: index of the specific hwspinlock to unregister
+ *
+ * This function should be called from the underlying platform-specific
+ * implementation, to unregister an existing (and unused) hwspinlock.
+ *
+ * Can be called from an atomic context (will not sleep) but not from
+ * within interrupt context.
+ *
+ * Returns the address of hwspinlock @id on success, or NULL on failure
+ */
+struct hwspinlock *hwspin_lock_unregister(unsigned int id)
+{
+	struct hwspinlock *hwlock = NULL;
+	int ret;
+
+	spin_lock(&hwspinlock_tree_lock);
+
+	/* make sure the hwspinlock is not in use (tag is set) */
+	ret = radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
+	if (ret == 0) {
+		pr_err("hwspinlock %d still in use (or not present)\n", id);
+		goto out;
+	}
+
+	hwlock = radix_tree_delete(&hwspinlock_tree, id);
+	if (!hwlock) {
+		pr_err("failed to delete hwspinlock %d\n", id);
+		goto out;
+	}
+
+out:
+	spin_unlock(&hwspinlock_tree_lock);
+	return hwlock;
+}
+EXPORT_SYMBOL_GPL(hwspin_lock_unregister);
+
+/**
+ * __hwspin_lock_request() - tag an hwspinlock as used and power it up
+ *
+ * This is an internal function that prepares an hwspinlock instance
+ * before it is given to the user. The function assumes that
+ * hwspinlock_tree_lock is taken.
+ *
+ * Returns 0 or positive to indicate success, and a negative value to
+ * indicate an error (with the appropriate error code)
+ */
+static int __hwspin_lock_request(struct hwspinlock *hwlock)
+{
+	struct hwspinlock *tmp;
+	int ret;
+
+	/* prevent underlying implementation from being removed */
+	if (!try_module_get(hwlock->owner)) {
+		dev_err(hwlock->dev, "%s: can't get owner\n", __func__);
+		return -EINVAL;
+	}
+
+	/* notify PM core that power is now needed */
+	ret = pm_runtime_get_sync(hwlock->dev);
+	if (ret < 0) {
+		dev_err(hwlock->dev, "%s: can't power on device\n", __func__);
+		return ret;
+	}
+
+	/* mark hwspinlock as used, should not fail */
+	tmp = radix_tree_tag_clear(&hwspinlock_tree, hwlock->id,
+							HWSPINLOCK_UNUSED);
+
+	/* self-sanity check that should never fail */
+	WARN_ON(tmp != hwlock);
+
+	return ret;
+}
+
+/**
+ * hwspin_lock_get_id() - retrieve id number of a given hwspinlock
+ * @hwlock: a valid hwspinlock instance
+ *
+ * Returns the id number of a given @hwlock, or -EINVAL if @hwlock is invalid.
+ */
+int hwspin_lock_get_id(struct hwspinlock *hwlock)
+{
+	if (!hwlock) {
+		pr_err("invalid hwlock\n");
+		return -EINVAL;
+	}
+
+	return hwlock->id;
+}
+EXPORT_SYMBOL_GPL(hwspin_lock_get_id);
+
+/**
+ * hwspin_lock_request() - request an hwspinlock
+ *
+ * This function should be called by users of the hwspinlock device,
+ * in order to dynamically assign them an unused hwspinlock.
+ * Usually the user of this lock will then have to communicate the lock's id
+ * to the remote core before it can be used for synchronization (to get the
+ * id of a given hwlock, use hwspin_lock_get_id()).
+ *
+ * Can be called from an atomic context (will not sleep) but not from
+ * within interrupt context (simply because there is no use case for
+ * that yet).
+ *
+ * Returns the address of the assigned hwspinlock, or NULL on error
+ */
+struct hwspinlock *hwspin_lock_request(void)
+{
+	struct hwspinlock *hwlock;
+	int ret;
+
+	spin_lock(&hwspinlock_tree_lock);
+
+	/* look for an unused lock */
+	ret = radix_tree_gang_lookup_tag(&hwspinlock_tree, (void **)&hwlock,
+						0, 1, HWSPINLOCK_UNUSED);
+	if (ret == 0) {
+		pr_warn("a free hwspinlock is not available\n");
+		hwlock = NULL;
+		goto out;
+	}
+
+	/* sanity check that should never fail */
+	WARN_ON(ret > 1);
+
+	/* mark as used and power up */
+	ret = __hwspin_lock_request(hwlock);
+	if (ret < 0)
+		hwlock = NULL;
+
+out:
+	spin_unlock(&hwspinlock_tree_lock);
+	return hwlock;
+}
+EXPORT_SYMBOL_GPL(hwspin_lock_request);
+
+/**
+ * hwspin_lock_request_specific() - request for a specific hwspinlock
+ * @id: index of the specific hwspinlock that is requested
+ *
+ * This function should be called by users of the hwspinlock module,
+ * in order to assign them a specific hwspinlock.
+ * Usually early board code will be calling this function in order to
+ * reserve specific hwspinlock ids for predefined purposes.
+ *
+ * Can be called from an atomic context (will not sleep) but not from
+ * within interrupt context (simply because there is no use case for
+ * that yet).
+ *
+ * Returns the address of the assigned hwspinlock, or NULL on error
+ */
+struct hwspinlock *hwspin_lock_request_specific(unsigned int id)
+{
+	struct hwspinlock *hwlock;
+	int ret;
+
+	spin_lock(&hwspinlock_tree_lock);
+
+	/* make sure this hwspinlock exists */
+	hwlock = radix_tree_lookup(&hwspinlock_tree, id);
+	if (!hwlock) {
+		pr_warn("hwspinlock %u does not exist\n", id);
+		goto out;
+	}
+
+	/* sanity check (this shouldn't happen) */
+	WARN_ON(hwlock->id != id);
+
+	/* make sure this hwspinlock is unused */
+	ret = radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
+	if (ret == 0) {
+		pr_warn("hwspinlock %u is already in use\n", id);
+		hwlock = NULL;
+		goto out;
+	}
+
+	/* mark as used and power up */
+	ret = __hwspin_lock_request(hwlock);
+	if (ret < 0)
+		hwlock = NULL;
+
+out:
+	spin_unlock(&hwspinlock_tree_lock);
+	return hwlock;
+}
+EXPORT_SYMBOL_GPL(hwspin_lock_request_specific);
+
+/**
+ * hwspin_lock_free() - free a specific hwspinlock
+ * @hwlock: the specific hwspinlock to free
+ *
+ * This function mark @hwlock as free again.
+ * Should only be called with an @hwlock that was retrieved from
+ * an earlier call to omap_hwspin_lock_request{_specific}.
+ *
+ * Can be called from an atomic context (will not sleep) but not from
+ * within interrupt context (simply because there is no use case for
+ * that yet).
+ *
+ * Returns 0 on success, or an appropriate error code on failure
+ */
+int hwspin_lock_free(struct hwspinlock *hwlock)
+{
+	struct hwspinlock *tmp;
+	int ret;
+
+	if (!hwlock) {
+		pr_err("invalid hwlock\n");
+		return -EINVAL;
+	}
+
+	spin_lock(&hwspinlock_tree_lock);
+
+	/* make sure the hwspinlock is used */
+	ret = radix_tree_tag_get(&hwspinlock_tree, hwlock->id,
+							HWSPINLOCK_UNUSED);
+	if (ret == 1) {
+		dev_err(hwlock->dev, "%s: hwlock is already free\n", __func__);
+		dump_stack();
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* notify the underlying device that power is not needed */
+	ret = pm_runtime_put(hwlock->dev);
+	if (ret < 0)
+		goto out;
+
+	/* mark this hwspinlock as available */
+	tmp = radix_tree_tag_set(&hwspinlock_tree, hwlock->id,
+							HWSPINLOCK_UNUSED);
+
+	/* sanity check (this shouldn't happen) */
+	WARN_ON(tmp != hwlock);
+
+	module_put(hwlock->owner);
+
+out:
+	spin_unlock(&hwspinlock_tree_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(hwspin_lock_free);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Hardware spinlock interface");
+MODULE_AUTHOR("Ohad Ben-Cohen <ohad@wizery.com>");
diff --git a/drivers/hwspinlock/hwspinlock_internal.h b/drivers/hwspinlock/hwspinlock_internal.h
new file mode 100644
index 000000000000..69935e6b93e5
--- /dev/null
+++ b/drivers/hwspinlock/hwspinlock_internal.h
@@ -0,0 +1,61 @@
+/*
+ * Hardware spinlocks internal header
+ *
+ * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Contact: Ohad Ben-Cohen <ohad@wizery.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __HWSPINLOCK_HWSPINLOCK_H
+#define __HWSPINLOCK_HWSPINLOCK_H
+
+#include <linux/spinlock.h>
+#include <linux/device.h>
+
+/**
+ * struct hwspinlock_ops - platform-specific hwspinlock handlers
+ *
+ * @trylock: make a single attempt to take the lock. returns 0 on
+ *	     failure and true on success. may _not_ sleep.
+ * @unlock:  release the lock. always succeed. may _not_ sleep.
+ * @relax:   optional, platform-specific relax handler, called by hwspinlock
+ *	     core while spinning on a lock, between two successive
+ *	     invocations of @trylock. may _not_ sleep.
+ */
+struct hwspinlock_ops {
+	int (*trylock)(struct hwspinlock *lock);
+	void (*unlock)(struct hwspinlock *lock);
+	void (*relax)(struct hwspinlock *lock);
+};
+
+/**
+ * struct hwspinlock - this struct represents a single hwspinlock instance
+ *
+ * @dev: underlying device, will be used to invoke runtime PM api
+ * @ops: platform-specific hwspinlock handlers
+ * @id: a global, unique, system-wide, index of the lock.
+ * @lock: initialized and used by hwspinlock core
+ * @owner: underlying implementation module, used to maintain module ref count
+ *
+ * Note: currently simplicity was opted for, but later we can squeeze some
+ * memory bytes by grouping the dev, ops and owner members in a single
+ * per-platform struct, and have all hwspinlocks point at it.
+ */
+struct hwspinlock {
+	struct device *dev;
+	const struct hwspinlock_ops *ops;
+	int id;
+	spinlock_t lock;
+	struct module *owner;
+};
+
+#endif /* __HWSPINLOCK_HWSPINLOCK_H */
diff --git a/drivers/hwspinlock/omap_hwspinlock.c b/drivers/hwspinlock/omap_hwspinlock.c
new file mode 100644
index 000000000000..a8f02734c026
--- /dev/null
+++ b/drivers/hwspinlock/omap_hwspinlock.c
@@ -0,0 +1,231 @@
+/*
+ * OMAP hardware spinlock driver
+ *
+ * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Contact: Simon Que <sque@ti.com>
+ *          Hari Kanigeri <h-kanigeri2@ti.com>
+ *          Ohad Ben-Cohen <ohad@wizery.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/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/bitops.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/hwspinlock.h>
+#include <linux/platform_device.h>
+
+#include "hwspinlock_internal.h"
+
+/* Spinlock register offsets */
+#define SYSSTATUS_OFFSET		0x0014
+#define LOCK_BASE_OFFSET		0x0800
+
+#define SPINLOCK_NUMLOCKS_BIT_OFFSET	(24)
+
+/* Possible values of SPINLOCK_LOCK_REG */
+#define SPINLOCK_NOTTAKEN		(0)	/* free */
+#define SPINLOCK_TAKEN			(1)	/* locked */
+
+#define to_omap_hwspinlock(lock)	\
+	container_of(lock, struct omap_hwspinlock, lock)
+
+struct omap_hwspinlock {
+	struct hwspinlock lock;
+	void __iomem *addr;
+};
+
+struct omap_hwspinlock_state {
+	int num_locks;			/* Total number of locks in system */
+	void __iomem *io_base;		/* Mapped base address */
+};
+
+static int omap_hwspinlock_trylock(struct hwspinlock *lock)
+{
+	struct omap_hwspinlock *omap_lock = to_omap_hwspinlock(lock);
+
+	/* attempt to acquire the lock by reading its value */
+	return (SPINLOCK_NOTTAKEN == readl(omap_lock->addr));
+}
+
+static void omap_hwspinlock_unlock(struct hwspinlock *lock)
+{
+	struct omap_hwspinlock *omap_lock = to_omap_hwspinlock(lock);
+
+	/* release the lock by writing 0 to it */
+	writel(SPINLOCK_NOTTAKEN, omap_lock->addr);
+}
+
+/*
+ * relax the OMAP interconnect while spinning on it.
+ *
+ * The specs recommended that the retry delay time will be
+ * just over half of the time that a requester would be
+ * expected to hold the lock.
+ *
+ * The number below is taken from an hardware specs example,
+ * obviously it is somewhat arbitrary.
+ */
+static void omap_hwspinlock_relax(struct hwspinlock *lock)
+{
+	ndelay(50);
+}
+
+static const struct hwspinlock_ops omap_hwspinlock_ops = {
+	.trylock = omap_hwspinlock_trylock,
+	.unlock = omap_hwspinlock_unlock,
+	.relax = omap_hwspinlock_relax,
+};
+
+static int __devinit omap_hwspinlock_probe(struct platform_device *pdev)
+{
+	struct omap_hwspinlock *omap_lock;
+	struct omap_hwspinlock_state *state;
+	struct hwspinlock *lock;
+	struct resource *res;
+	void __iomem *io_base;
+	int i, ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	io_base = ioremap(res->start, resource_size(res));
+	if (!io_base) {
+		ret = -ENOMEM;
+		goto free_state;
+	}
+
+	/* Determine number of locks */
+	i = readl(io_base + SYSSTATUS_OFFSET);
+	i >>= SPINLOCK_NUMLOCKS_BIT_OFFSET;
+
+	/* one of the four lsb's must be set, and nothing else */
+	if (hweight_long(i & 0xf) != 1 || i > 8) {
+		ret = -EINVAL;
+		goto iounmap_base;
+	}
+
+	state->num_locks = i * 32;
+	state->io_base = io_base;
+
+	platform_set_drvdata(pdev, state);
+
+	/*
+	 * runtime PM will make sure the clock of this module is
+	 * enabled iff at least one lock is requested
+	 */
+	pm_runtime_enable(&pdev->dev);
+
+	for (i = 0; i < state->num_locks; i++) {
+		omap_lock = kzalloc(sizeof(*omap_lock), GFP_KERNEL);
+		if (!omap_lock) {
+			ret = -ENOMEM;
+			goto free_locks;
+		}
+
+		omap_lock->lock.dev = &pdev->dev;
+		omap_lock->lock.owner = THIS_MODULE;
+		omap_lock->lock.id = i;
+		omap_lock->lock.ops = &omap_hwspinlock_ops;
+		omap_lock->addr = io_base + LOCK_BASE_OFFSET + sizeof(u32) * i;
+
+		ret = hwspin_lock_register(&omap_lock->lock);
+		if (ret) {
+			kfree(omap_lock);
+			goto free_locks;
+		}
+	}
+
+	return 0;
+
+free_locks:
+	while (--i >= 0) {
+		lock = hwspin_lock_unregister(i);
+		/* this should't happen, but let's give our best effort */
+		if (!lock) {
+			dev_err(&pdev->dev, "%s: cleanups failed\n", __func__);
+			continue;
+		}
+		omap_lock = to_omap_hwspinlock(lock);
+		kfree(omap_lock);
+	}
+	pm_runtime_disable(&pdev->dev);
+iounmap_base:
+	iounmap(io_base);
+free_state:
+	kfree(state);
+	return ret;
+}
+
+static int omap_hwspinlock_remove(struct platform_device *pdev)
+{
+	struct omap_hwspinlock_state *state = platform_get_drvdata(pdev);
+	struct hwspinlock *lock;
+	struct omap_hwspinlock *omap_lock;
+	int i;
+
+	for (i = 0; i < state->num_locks; i++) {
+		lock = hwspin_lock_unregister(i);
+		/* this shouldn't happen at this point. if it does, at least
+		 * don't continue with the remove */
+		if (!lock) {
+			dev_err(&pdev->dev, "%s: failed on %d\n", __func__, i);
+			return -EBUSY;
+		}
+
+		omap_lock = to_omap_hwspinlock(lock);
+		kfree(omap_lock);
+	}
+
+	pm_runtime_disable(&pdev->dev);
+	iounmap(state->io_base);
+	kfree(state);
+
+	return 0;
+}
+
+static struct platform_driver omap_hwspinlock_driver = {
+	.probe		= omap_hwspinlock_probe,
+	.remove		= omap_hwspinlock_remove,
+	.driver		= {
+		.name	= "omap_hwspinlock",
+	},
+};
+
+static int __init omap_hwspinlock_init(void)
+{
+	return platform_driver_register(&omap_hwspinlock_driver);
+}
+/* board init code might need to reserve hwspinlocks for predefined purposes */
+postcore_initcall(omap_hwspinlock_init);
+
+static void __exit omap_hwspinlock_exit(void)
+{
+	platform_driver_unregister(&omap_hwspinlock_driver);
+}
+module_exit(omap_hwspinlock_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Hardware spinlock driver for OMAP");
+MODULE_AUTHOR("Simon Que <sque@ti.com>");
+MODULE_AUTHOR("Hari Kanigeri <h-kanigeri2@ti.com>");
+MODULE_AUTHOR("Ohad Ben-Cohen <ohad@wizery.com>");
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index 23ac61e2db39..beee6b2d361d 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -10,3 +10,4 @@ obj-$(CONFIG_I2C_MUX)		+= i2c-mux.o
 obj-y				+= algos/ busses/ muxes/
 
 ccflags-$(CONFIG_I2C_DEBUG_CORE) := -DDEBUG
+CFLAGS_i2c-core.o := -Wno-deprecated-declarations
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 113505a6434e..14abb7bde741 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -452,6 +452,16 @@ config I2C_MV64XXX
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-mv64xxx.
 
+config I2C_MXS
+	tristate "Freescale i.MX28 I2C interface"
+	depends on SOC_IMX28
+	help
+	  Say Y here if you want to use the I2C bus controller on
+	  the Freescale i.MX28 processors.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-mxs.
+
 config I2C_NOMADIK
 	tristate "ST-Ericsson Nomadik/Ux500 I2C Controller"
 	depends on PLAT_NOMADIK
@@ -523,17 +533,31 @@ config I2C_PNX
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-pnx.
 
+config I2C_PUV3
+	tristate "PKUnity v3 I2C bus support"
+	depends on UNICORE32 && ARCH_PUV3
+	select I2C_ALGOBIT
+	help
+	  This driver supports the I2C IP inside the PKUnity-v3 SoC.
+	  This I2C bus controller is under AMBA/AXI bus.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-puv3.
+
 config I2C_PXA
 	tristate "Intel PXA2XX I2C adapter"
-	depends on ARCH_PXA || ARCH_MMP
+	depends on ARCH_PXA || ARCH_MMP || (X86_32 && PCI && OF)
 	help
 	  If you have devices in the PXA I2C bus, say yes to this option.
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-pxa.
 
+config I2C_PXA_PCI
+	def_bool I2C_PXA && X86_32 && PCI && OF
+
 config I2C_PXA_SLAVE
 	bool "Intel PXA2XX I2C Slave comms support"
-	depends on I2C_PXA
+	depends on I2C_PXA && !X86_32
 	help
 	  Support I2C slave mode communications on the PXA I2C bus.  This
 	  is necessary for systems where the PXA may be a target on the
@@ -607,6 +631,13 @@ config I2C_STU300
 	  This driver can also be built as a module. If so, the module
 	  will be called i2c-stu300.
 
+config I2C_TEGRA
+	tristate "NVIDIA Tegra internal I2C controller"
+	depends on ARCH_TEGRA
+	help
+	  If you say yes to this option, support will be included for the
+	  I2C controller embedded in NVIDIA Tegra SOCs
+
 config I2C_VERSATILE
 	tristate "ARM Versatile/Realview I2C bus support"
 	depends on ARCH_VERSATILE || ARCH_REALVIEW || ARCH_VEXPRESS
@@ -639,15 +670,28 @@ config I2C_XILINX
 	  will be called xilinx_i2c.
 
 config I2C_EG20T
-        tristate "PCH I2C of Intel EG20T"
-        depends on PCI
-        help
-          This driver is for PCH(Platform controller Hub) I2C of EG20T which
-          is an IOH(Input/Output Hub) for x86 embedded processor.
-          This driver can access PCH I2C bus device.
+	tristate "Intel EG20T PCH/OKI SEMICONDUCTOR ML7213 IOH"
+	depends on PCI
+	help
+	  This driver is for PCH(Platform controller Hub) I2C of EG20T which
+	  is an IOH(Input/Output Hub) for x86 embedded processor.
+	  This driver can access PCH I2C bus device.
+
+	  This driver also supports the ML7213, a companion chip for the
+	  Atom E6xx series and compatible with the Intel EG20T PCH.
 
 comment "External I2C/SMBus adapter drivers"
 
+config I2C_DIOLAN_U2C
+	tristate "Diolan U2C-12 USB adapter"
+	depends on USB
+	help
+	  If you say yes to this option, support will be included for Diolan
+	  U2C-12, a USB to I2C interface.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-diolan-u2c.
+
 config I2C_PARPORT
 	tristate "Parallel port adapter"
 	depends on PARPORT
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 9d2d0ec7fb23..e6cf294d3729 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -43,6 +43,7 @@ obj-$(CONFIG_I2C_IOP3XX)	+= i2c-iop3xx.o
 obj-$(CONFIG_I2C_IXP2000)	+= i2c-ixp2000.o
 obj-$(CONFIG_I2C_MPC)		+= i2c-mpc.o
 obj-$(CONFIG_I2C_MV64XXX)	+= i2c-mv64xxx.o
+obj-$(CONFIG_I2C_MXS)		+= i2c-mxs.o
 obj-$(CONFIG_I2C_NOMADIK)	+= i2c-nomadik.o
 obj-$(CONFIG_I2C_NUC900)	+= i2c-nuc900.o
 obj-$(CONFIG_I2C_OCORES)	+= i2c-ocores.o
@@ -51,19 +52,23 @@ obj-$(CONFIG_I2C_PASEMI)	+= i2c-pasemi.o
 obj-$(CONFIG_I2C_PCA_PLATFORM)	+= i2c-pca-platform.o
 obj-$(CONFIG_I2C_PMCMSP)	+= i2c-pmcmsp.o
 obj-$(CONFIG_I2C_PNX)		+= i2c-pnx.o
+obj-$(CONFIG_I2C_PUV3)		+= i2c-puv3.o
 obj-$(CONFIG_I2C_PXA)		+= i2c-pxa.o
+obj-$(CONFIG_I2C_PXA_PCI)	+= i2c-pxa-pci.o
 obj-$(CONFIG_I2C_S3C2410)	+= i2c-s3c2410.o
 obj-$(CONFIG_I2C_S6000)		+= i2c-s6000.o
 obj-$(CONFIG_I2C_SH7760)	+= i2c-sh7760.o
 obj-$(CONFIG_I2C_SH_MOBILE)	+= i2c-sh_mobile.o
 obj-$(CONFIG_I2C_SIMTEC)	+= i2c-simtec.o
 obj-$(CONFIG_I2C_STU300)	+= i2c-stu300.o
+obj-$(CONFIG_I2C_TEGRA)		+= i2c-tegra.o
 obj-$(CONFIG_I2C_VERSATILE)	+= i2c-versatile.o
 obj-$(CONFIG_I2C_OCTEON)	+= i2c-octeon.o
 obj-$(CONFIG_I2C_XILINX)	+= i2c-xiic.o
 obj-$(CONFIG_I2C_EG20T)         += i2c-eg20t.o
 
 # External I2C/SMBus adapter drivers
+obj-$(CONFIG_I2C_DIOLAN_U2C)	+= i2c-diolan-u2c.o
 obj-$(CONFIG_I2C_PARPORT)	+= i2c-parport.o
 obj-$(CONFIG_I2C_PARPORT_LIGHT)	+= i2c-parport-light.o
 obj-$(CONFIG_I2C_TAOS_EVM)	+= i2c-taos-evm.o
diff --git a/drivers/i2c/busses/i2c-cpm.c b/drivers/i2c/busses/i2c-cpm.c
index f2de3be35df3..3a20961bef1e 100644
--- a/drivers/i2c/busses/i2c-cpm.c
+++ b/drivers/i2c/busses/i2c-cpm.c
@@ -634,8 +634,7 @@ static void cpm_i2c_shutdown(struct cpm_i2c *cpm)
 		cpm_muram_free(cpm->i2c_addr);
 }
 
-static int __devinit cpm_i2c_probe(struct platform_device *ofdev,
-			 const struct of_device_id *match)
+static int __devinit cpm_i2c_probe(struct platform_device *ofdev)
 {
 	int result, len;
 	struct cpm_i2c *cpm;
@@ -718,7 +717,7 @@ static const struct of_device_id cpm_i2c_match[] = {
 
 MODULE_DEVICE_TABLE(of, cpm_i2c_match);
 
-static struct of_platform_driver cpm_i2c_driver = {
+static struct platform_driver cpm_i2c_driver = {
 	.probe		= cpm_i2c_probe,
 	.remove		= __devexit_p(cpm_i2c_remove),
 	.driver = {
@@ -730,12 +729,12 @@ static struct of_platform_driver cpm_i2c_driver = {
 
 static int __init cpm_i2c_init(void)
 {
-	return of_register_platform_driver(&cpm_i2c_driver);
+	return platform_driver_register(&cpm_i2c_driver);
 }
 
 static void __exit cpm_i2c_exit(void)
 {
-	of_unregister_platform_driver(&cpm_i2c_driver);
+	platform_driver_unregister(&cpm_i2c_driver);
 }
 
 module_init(cpm_i2c_init);
diff --git a/drivers/i2c/busses/i2c-diolan-u2c.c b/drivers/i2c/busses/i2c-diolan-u2c.c
new file mode 100644
index 000000000000..76366716a854
--- /dev/null
+++ b/drivers/i2c/busses/i2c-diolan-u2c.c
@@ -0,0 +1,535 @@
+/*
+ * Driver for the Diolan u2c-12 USB-I2C adapter
+ *
+ * Copyright (c) 2010-2011 Ericsson AB
+ *
+ * Derived from:
+ *  i2c-tiny-usb.c
+ *  Copyright (C) 2006-2007 Till Harbaum (Till@Harbaum.org)
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation, version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/i2c.h>
+
+#define DRIVER_NAME		"i2c-diolan-u2c"
+
+#define USB_VENDOR_ID_DIOLAN		0x0abf
+#define USB_DEVICE_ID_DIOLAN_U2C	0x3370
+
+#define DIOLAN_OUT_EP		0x02
+#define DIOLAN_IN_EP		0x84
+
+/* commands via USB, must match command ids in the firmware */
+#define CMD_I2C_READ		0x01
+#define CMD_I2C_WRITE		0x02
+#define CMD_I2C_SCAN		0x03	/* Returns list of detected devices */
+#define CMD_I2C_RELEASE_SDA	0x04
+#define CMD_I2C_RELEASE_SCL	0x05
+#define CMD_I2C_DROP_SDA	0x06
+#define CMD_I2C_DROP_SCL	0x07
+#define CMD_I2C_READ_SDA	0x08
+#define CMD_I2C_READ_SCL	0x09
+#define CMD_GET_FW_VERSION	0x0a
+#define CMD_GET_SERIAL		0x0b
+#define CMD_I2C_START		0x0c
+#define CMD_I2C_STOP		0x0d
+#define CMD_I2C_REPEATED_START	0x0e
+#define CMD_I2C_PUT_BYTE	0x0f
+#define CMD_I2C_GET_BYTE	0x10
+#define CMD_I2C_PUT_ACK		0x11
+#define CMD_I2C_GET_ACK		0x12
+#define CMD_I2C_PUT_BYTE_ACK	0x13
+#define CMD_I2C_GET_BYTE_ACK	0x14
+#define CMD_I2C_SET_SPEED	0x1b
+#define CMD_I2C_GET_SPEED	0x1c
+#define CMD_I2C_SET_CLK_SYNC	0x24
+#define CMD_I2C_GET_CLK_SYNC	0x25
+#define CMD_I2C_SET_CLK_SYNC_TO	0x26
+#define CMD_I2C_GET_CLK_SYNC_TO	0x27
+
+#define RESP_OK			0x00
+#define RESP_FAILED		0x01
+#define RESP_BAD_MEMADDR	0x04
+#define RESP_DATA_ERR		0x05
+#define RESP_NOT_IMPLEMENTED	0x06
+#define RESP_NACK		0x07
+#define RESP_TIMEOUT		0x09
+
+#define U2C_I2C_SPEED_FAST	0	/* 400 kHz */
+#define U2C_I2C_SPEED_STD	1	/* 100 kHz */
+#define U2C_I2C_SPEED_2KHZ	242	/* 2 kHz, minimum speed */
+#define U2C_I2C_SPEED(f)	((DIV_ROUND_UP(1000000, (f)) - 10) / 2 + 1)
+
+#define U2C_I2C_FREQ_FAST	400000
+#define U2C_I2C_FREQ_STD	100000
+#define U2C_I2C_FREQ(s)		(1000000 / (2 * (s - 1) + 10))
+
+#define DIOLAN_USB_TIMEOUT	100	/* in ms */
+#define DIOLAN_SYNC_TIMEOUT	20	/* in ms */
+
+#define DIOLAN_OUTBUF_LEN	128
+#define DIOLAN_FLUSH_LEN	(DIOLAN_OUTBUF_LEN - 4)
+#define DIOLAN_INBUF_LEN	256	/* Maximum supported receive length */
+
+/* Structure to hold all of our device specific stuff */
+struct i2c_diolan_u2c {
+	u8 obuffer[DIOLAN_OUTBUF_LEN];	/* output buffer */
+	u8 ibuffer[DIOLAN_INBUF_LEN];	/* input buffer */
+	struct usb_device *usb_dev;	/* the usb device for this device */
+	struct usb_interface *interface;/* the interface for this device */
+	struct i2c_adapter adapter;	/* i2c related things */
+	int olen;			/* Output buffer length */
+	int ocount;			/* Number of enqueued messages */
+};
+
+static uint frequency = U2C_I2C_FREQ_STD;	/* I2C clock frequency in Hz */
+
+module_param(frequency, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(frequency, "I2C clock frequency in hertz");
+
+/* usb layer */
+
+/* Send command to device, and get response. */
+static int diolan_usb_transfer(struct i2c_diolan_u2c *dev)
+{
+	int ret = 0;
+	int actual;
+	int i;
+
+	if (!dev->olen || !dev->ocount)
+		return -EINVAL;
+
+	ret = usb_bulk_msg(dev->usb_dev,
+			   usb_sndbulkpipe(dev->usb_dev, DIOLAN_OUT_EP),
+			   dev->obuffer, dev->olen, &actual,
+			   DIOLAN_USB_TIMEOUT);
+	if (!ret) {
+		for (i = 0; i < dev->ocount; i++) {
+			int tmpret;
+
+			tmpret = usb_bulk_msg(dev->usb_dev,
+					      usb_rcvbulkpipe(dev->usb_dev,
+							      DIOLAN_IN_EP),
+					      dev->ibuffer,
+					      sizeof(dev->ibuffer), &actual,
+					      DIOLAN_USB_TIMEOUT);
+			/*
+			 * Stop command processing if a previous command
+			 * returned an error.
+			 * Note that we still need to retrieve all messages.
+			 */
+			if (ret < 0)
+				continue;
+			ret = tmpret;
+			if (ret == 0 && actual > 0) {
+				switch (dev->ibuffer[actual - 1]) {
+				case RESP_NACK:
+					/*
+					 * Return ENXIO if NACK was received as
+					 * response to the address phase,
+					 * EIO otherwise
+					 */
+					ret = i == 1 ? -ENXIO : -EIO;
+					break;
+				case RESP_TIMEOUT:
+					ret = -ETIMEDOUT;
+					break;
+				case RESP_OK:
+					/* strip off return code */
+					ret = actual - 1;
+					break;
+				default:
+					ret = -EIO;
+					break;
+				}
+			}
+		}
+	}
+	dev->olen = 0;
+	dev->ocount = 0;
+	return ret;
+}
+
+static int diolan_write_cmd(struct i2c_diolan_u2c *dev, bool flush)
+{
+	if (flush || dev->olen >= DIOLAN_FLUSH_LEN)
+		return diolan_usb_transfer(dev);
+	return 0;
+}
+
+/* Send command (no data) */
+static int diolan_usb_cmd(struct i2c_diolan_u2c *dev, u8 command, bool flush)
+{
+	dev->obuffer[dev->olen++] = command;
+	dev->ocount++;
+	return diolan_write_cmd(dev, flush);
+}
+
+/* Send command with one byte of data */
+static int diolan_usb_cmd_data(struct i2c_diolan_u2c *dev, u8 command, u8 data,
+			       bool flush)
+{
+	dev->obuffer[dev->olen++] = command;
+	dev->obuffer[dev->olen++] = data;
+	dev->ocount++;
+	return diolan_write_cmd(dev, flush);
+}
+
+/* Send command with two bytes of data */
+static int diolan_usb_cmd_data2(struct i2c_diolan_u2c *dev, u8 command, u8 d1,
+				u8 d2, bool flush)
+{
+	dev->obuffer[dev->olen++] = command;
+	dev->obuffer[dev->olen++] = d1;
+	dev->obuffer[dev->olen++] = d2;
+	dev->ocount++;
+	return diolan_write_cmd(dev, flush);
+}
+
+/*
+ * Flush input queue.
+ * If we don't do this at startup and the controller has queued up
+ * messages which were not retrieved, it will stop responding
+ * at some point.
+ */
+static void diolan_flush_input(struct i2c_diolan_u2c *dev)
+{
+	int i;
+
+	for (i = 0; i < 10; i++) {
+		int actual = 0;
+		int ret;
+
+		ret = usb_bulk_msg(dev->usb_dev,
+				   usb_rcvbulkpipe(dev->usb_dev, DIOLAN_IN_EP),
+				   dev->ibuffer, sizeof(dev->ibuffer), &actual,
+				   DIOLAN_USB_TIMEOUT);
+		if (ret < 0 || actual == 0)
+			break;
+	}
+	if (i == 10)
+		dev_err(&dev->interface->dev, "Failed to flush input buffer\n");
+}
+
+static int diolan_i2c_start(struct i2c_diolan_u2c *dev)
+{
+	return diolan_usb_cmd(dev, CMD_I2C_START, false);
+}
+
+static int diolan_i2c_repeated_start(struct i2c_diolan_u2c *dev)
+{
+	return diolan_usb_cmd(dev, CMD_I2C_REPEATED_START, false);
+}
+
+static int diolan_i2c_stop(struct i2c_diolan_u2c *dev)
+{
+	return diolan_usb_cmd(dev, CMD_I2C_STOP, true);
+}
+
+static int diolan_i2c_get_byte_ack(struct i2c_diolan_u2c *dev, bool ack,
+				   u8 *byte)
+{
+	int ret;
+
+	ret = diolan_usb_cmd_data(dev, CMD_I2C_GET_BYTE_ACK, ack, true);
+	if (ret > 0)
+		*byte = dev->ibuffer[0];
+	else if (ret == 0)
+		ret = -EIO;
+
+	return ret;
+}
+
+static int diolan_i2c_put_byte_ack(struct i2c_diolan_u2c *dev, u8 byte)
+{
+	return diolan_usb_cmd_data(dev, CMD_I2C_PUT_BYTE_ACK, byte, false);
+}
+
+static int diolan_set_speed(struct i2c_diolan_u2c *dev, u8 speed)
+{
+	return diolan_usb_cmd_data(dev, CMD_I2C_SET_SPEED, speed, true);
+}
+
+/* Enable or disable clock synchronization (stretching) */
+static int diolan_set_clock_synch(struct i2c_diolan_u2c *dev, bool enable)
+{
+	return diolan_usb_cmd_data(dev, CMD_I2C_SET_CLK_SYNC, enable, true);
+}
+
+/* Set clock synchronization timeout in ms */
+static int diolan_set_clock_synch_timeout(struct i2c_diolan_u2c *dev, int ms)
+{
+	int to_val = ms * 10;
+
+	return diolan_usb_cmd_data2(dev, CMD_I2C_SET_CLK_SYNC_TO,
+				    to_val & 0xff, (to_val >> 8) & 0xff, true);
+}
+
+static void diolan_fw_version(struct i2c_diolan_u2c *dev)
+{
+	int ret;
+
+	ret = diolan_usb_cmd(dev, CMD_GET_FW_VERSION, true);
+	if (ret >= 2)
+		dev_info(&dev->interface->dev,
+			 "Diolan U2C firmware version %u.%u\n",
+			 (unsigned int)dev->ibuffer[0],
+			 (unsigned int)dev->ibuffer[1]);
+}
+
+static void diolan_get_serial(struct i2c_diolan_u2c *dev)
+{
+	int ret;
+	u32 serial;
+
+	ret = diolan_usb_cmd(dev, CMD_GET_SERIAL, true);
+	if (ret >= 4) {
+		serial = le32_to_cpu(*(u32 *)dev->ibuffer);
+		dev_info(&dev->interface->dev,
+			 "Diolan U2C serial number %u\n", serial);
+	}
+}
+
+static int diolan_init(struct i2c_diolan_u2c *dev)
+{
+	int speed, ret;
+
+	if (frequency >= 200000) {
+		speed = U2C_I2C_SPEED_FAST;
+		frequency = U2C_I2C_FREQ_FAST;
+	} else if (frequency >= 100000 || frequency == 0) {
+		speed = U2C_I2C_SPEED_STD;
+		frequency = U2C_I2C_FREQ_STD;
+	} else {
+		speed = U2C_I2C_SPEED(frequency);
+		if (speed > U2C_I2C_SPEED_2KHZ)
+			speed = U2C_I2C_SPEED_2KHZ;
+		frequency = U2C_I2C_FREQ(speed);
+	}
+
+	dev_info(&dev->interface->dev,
+		 "Diolan U2C at USB bus %03d address %03d speed %d Hz\n",
+		 dev->usb_dev->bus->busnum, dev->usb_dev->devnum, frequency);
+
+	diolan_flush_input(dev);
+	diolan_fw_version(dev);
+	diolan_get_serial(dev);
+
+	/* Set I2C speed */
+	ret = diolan_set_speed(dev, speed);
+	if (ret < 0)
+		return ret;
+
+	/* Configure I2C clock synchronization */
+	ret = diolan_set_clock_synch(dev, speed != U2C_I2C_SPEED_FAST);
+	if (ret < 0)
+		return ret;
+
+	if (speed != U2C_I2C_SPEED_FAST)
+		ret = diolan_set_clock_synch_timeout(dev, DIOLAN_SYNC_TIMEOUT);
+
+	return ret;
+}
+
+/* i2c layer */
+
+static int diolan_usb_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs,
+			   int num)
+{
+	struct i2c_diolan_u2c *dev = i2c_get_adapdata(adapter);
+	struct i2c_msg *pmsg;
+	int i, j;
+	int ret, sret;
+
+	ret = diolan_i2c_start(dev);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < num; i++) {
+		pmsg = &msgs[i];
+		if (i) {
+			ret = diolan_i2c_repeated_start(dev);
+			if (ret < 0)
+				goto abort;
+		}
+		if (pmsg->flags & I2C_M_RD) {
+			ret =
+			    diolan_i2c_put_byte_ack(dev, (pmsg->addr << 1) | 1);
+			if (ret < 0)
+				goto abort;
+			for (j = 0; j < pmsg->len; j++) {
+				u8 byte;
+				bool ack = j < pmsg->len - 1;
+
+				/*
+				 * Don't send NACK if this is the first byte
+				 * of a SMBUS_BLOCK message.
+				 */
+				if (j == 0 && (pmsg->flags & I2C_M_RECV_LEN))
+					ack = true;
+
+				ret = diolan_i2c_get_byte_ack(dev, ack, &byte);
+				if (ret < 0)
+					goto abort;
+				/*
+				 * Adjust count if first received byte is length
+				 */
+				if (j == 0 && (pmsg->flags & I2C_M_RECV_LEN)) {
+					if (byte == 0
+					    || byte > I2C_SMBUS_BLOCK_MAX) {
+						ret = -EPROTO;
+						goto abort;
+					}
+					pmsg->len += byte;
+				}
+				pmsg->buf[j] = byte;
+			}
+		} else {
+			ret = diolan_i2c_put_byte_ack(dev, pmsg->addr << 1);
+			if (ret < 0)
+				goto abort;
+			for (j = 0; j < pmsg->len; j++) {
+				ret = diolan_i2c_put_byte_ack(dev,
+							      pmsg->buf[j]);
+				if (ret < 0)
+					goto abort;
+			}
+		}
+	}
+abort:
+	sret = diolan_i2c_stop(dev);
+	if (sret < 0 && ret >= 0)
+		ret = sret;
+	return ret;
+}
+
+/*
+ * Return list of supported functionality.
+ */
+static u32 diolan_usb_func(struct i2c_adapter *a)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
+	       I2C_FUNC_SMBUS_READ_BLOCK_DATA | I2C_FUNC_SMBUS_BLOCK_PROC_CALL;
+}
+
+static const struct i2c_algorithm diolan_usb_algorithm = {
+	.master_xfer = diolan_usb_xfer,
+	.functionality = diolan_usb_func,
+};
+
+/* device layer */
+
+static const struct usb_device_id diolan_u2c_table[] = {
+	{ USB_DEVICE(USB_VENDOR_ID_DIOLAN, USB_DEVICE_ID_DIOLAN_U2C) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(usb, diolan_u2c_table);
+
+static void diolan_u2c_free(struct i2c_diolan_u2c *dev)
+{
+	usb_put_dev(dev->usb_dev);
+	kfree(dev);
+}
+
+static int diolan_u2c_probe(struct usb_interface *interface,
+			    const struct usb_device_id *id)
+{
+	struct i2c_diolan_u2c *dev;
+	int ret;
+
+	/* allocate memory for our device state and initialize it */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&interface->dev, "no memory for device state\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	dev->usb_dev = usb_get_dev(interface_to_usbdev(interface));
+	dev->interface = interface;
+
+	/* save our data pointer in this interface device */
+	usb_set_intfdata(interface, dev);
+
+	/* setup i2c adapter description */
+	dev->adapter.owner = THIS_MODULE;
+	dev->adapter.class = I2C_CLASS_HWMON;
+	dev->adapter.algo = &diolan_usb_algorithm;
+	i2c_set_adapdata(&dev->adapter, dev);
+	snprintf(dev->adapter.name, sizeof(dev->adapter.name),
+		 DRIVER_NAME " at bus %03d device %03d",
+		 dev->usb_dev->bus->busnum, dev->usb_dev->devnum);
+
+	dev->adapter.dev.parent = &dev->interface->dev;
+
+	/* initialize diolan i2c interface */
+	ret = diolan_init(dev);
+	if (ret < 0) {
+		dev_err(&interface->dev, "failed to initialize adapter\n");
+		goto error_free;
+	}
+
+	/* and finally attach to i2c layer */
+	ret = i2c_add_adapter(&dev->adapter);
+	if (ret < 0) {
+		dev_err(&interface->dev, "failed to add I2C adapter\n");
+		goto error_free;
+	}
+
+	dev_dbg(&interface->dev, "connected " DRIVER_NAME "\n");
+
+	return 0;
+
+error_free:
+	usb_set_intfdata(interface, NULL);
+	diolan_u2c_free(dev);
+error:
+	return ret;
+}
+
+static void diolan_u2c_disconnect(struct usb_interface *interface)
+{
+	struct i2c_diolan_u2c *dev = usb_get_intfdata(interface);
+
+	i2c_del_adapter(&dev->adapter);
+	usb_set_intfdata(interface, NULL);
+	diolan_u2c_free(dev);
+
+	dev_dbg(&interface->dev, "disconnected\n");
+}
+
+static struct usb_driver diolan_u2c_driver = {
+	.name = DRIVER_NAME,
+	.probe = diolan_u2c_probe,
+	.disconnect = diolan_u2c_disconnect,
+	.id_table = diolan_u2c_table,
+};
+
+static int __init diolan_u2c_init(void)
+{
+	/* register this driver with the USB subsystem */
+	return usb_register(&diolan_u2c_driver);
+}
+
+static void __exit diolan_u2c_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&diolan_u2c_driver);
+}
+
+module_init(diolan_u2c_init);
+module_exit(diolan_u2c_exit);
+
+MODULE_AUTHOR("Guenter Roeck <guenter.roeck@ericsson.com>");
+MODULE_DESCRIPTION(DRIVER_NAME " driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-eg20t.c b/drivers/i2c/busses/i2c-eg20t.c
index 2e067dd2ee51..c57c83734692 100644
--- a/drivers/i2c/busses/i2c-eg20t.c
+++ b/drivers/i2c/busses/i2c-eg20t.c
@@ -131,6 +131,13 @@
 #define pch_pci_dbg(pdev, fmt, arg...)  \
 	dev_dbg(&pdev->dev, "%s :" fmt, __func__, ##arg)
 
+/*
+Set the number of I2C instance max
+Intel EG20T PCH :		1ch
+OKI SEMICONDUCTOR ML7213 IOH :	2ch
+*/
+#define PCH_I2C_MAX_DEV			2
+
 /**
  * struct i2c_algo_pch_data - for I2C driver functionalities
  * @pch_adapter:		stores the reference to i2c_adapter structure
@@ -155,12 +162,14 @@ struct i2c_algo_pch_data {
  * @pch_data:		stores a list of i2c_algo_pch_data
  * @pch_i2c_suspended:	specifies whether the system is suspended or not
  *			perhaps with more lines and words.
+ * @ch_num:		specifies the number of i2c instance
  *
  * pch_data has as many elements as maximum I2C channels
  */
 struct adapter_info {
-	struct i2c_algo_pch_data pch_data;
+	struct i2c_algo_pch_data pch_data[PCH_I2C_MAX_DEV];
 	bool pch_i2c_suspended;
+	int ch_num;
 };
 
 
@@ -169,8 +178,13 @@ static int pch_clk = 50000;	/* specifies I2C clock speed in KHz */
 static wait_queue_head_t pch_event;
 static DEFINE_MUTEX(pch_mutex);
 
+/* Definition for ML7213 by OKI SEMICONDUCTOR */
+#define PCI_VENDOR_ID_ROHM		0x10DB
+#define PCI_DEVICE_ID_ML7213_I2C	0x802D
+
 static struct pci_device_id __devinitdata pch_pcidev_id[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PCH_I2C)},
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_PCH_I2C),   1, },
+	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ML7213_I2C), 2, },
 	{0,}
 };
 
@@ -211,8 +225,7 @@ static void pch_i2c_init(struct i2c_algo_pch_data *adap)
 	/* Initialize I2C registers */
 	iowrite32(0x21, p + PCH_I2CNF);
 
-	pch_setbit(adap->pch_base_address, PCH_I2CCTL,
-			  PCH_I2CCTL_I2CMEN);
+	pch_setbit(adap->pch_base_address, PCH_I2CCTL, PCH_I2CCTL_I2CMEN);
 
 	if (pch_i2c_speed != 400)
 		pch_i2c_speed = 100;
@@ -254,7 +267,7 @@ static inline bool ktime_lt(const ktime_t cmp1, const ktime_t cmp2)
  * @timeout:	waiting time counter (us).
  */
 static s32 pch_i2c_wait_for_bus_idle(struct i2c_algo_pch_data *adap,
-				 s32 timeout)
+				     s32 timeout)
 {
 	void __iomem *p = adap->pch_base_address;
 
@@ -474,8 +487,8 @@ static void pch_i2c_sendnack(struct i2c_algo_pch_data *adap)
  * @last:	specifies whether last message or not.
  * @first:	specifies whether first message or not.
  */
-s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
-		  u32 last, u32 first)
+static s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
+			     u32 last, u32 first)
 {
 	struct i2c_algo_pch_data *adap = i2c_adap->algo_data;
 
@@ -568,10 +581,10 @@ s32 pch_i2c_readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs,
 }
 
 /**
- * pch_i2c_cb_ch0() - Interrupt handler Call back function
+ * pch_i2c_cb() - Interrupt handler Call back function
  * @adap:	Pointer to struct i2c_algo_pch_data.
  */
-static void pch_i2c_cb_ch0(struct i2c_algo_pch_data *adap)
+static void pch_i2c_cb(struct i2c_algo_pch_data *adap)
 {
 	u32 sts;
 	void __iomem *p = adap->pch_base_address;
@@ -599,24 +612,30 @@ static void pch_i2c_cb_ch0(struct i2c_algo_pch_data *adap)
  */
 static irqreturn_t pch_i2c_handler(int irq, void *pData)
 {
-	s32 reg_val;
-
-	struct i2c_algo_pch_data *adap_data = (struct i2c_algo_pch_data *)pData;
-	void __iomem *p = adap_data->pch_base_address;
-	u32 mode = ioread32(p + PCH_I2CMOD) & (BUFFER_MODE | EEPROM_SR_MODE);
-
-	if (mode != NORMAL_MODE) {
-		pch_err(adap_data, "I2C mode is not supported\n");
-		return IRQ_NONE;
+	u32 reg_val;
+	int flag;
+	int i;
+	struct adapter_info *adap_info = pData;
+	void __iomem *p;
+	u32 mode;
+
+	for (i = 0, flag = 0; i < adap_info->ch_num; i++) {
+		p = adap_info->pch_data[i].pch_base_address;
+		mode = ioread32(p + PCH_I2CMOD);
+		mode &= BUFFER_MODE | EEPROM_SR_MODE;
+		if (mode != NORMAL_MODE) {
+			pch_err(adap_info->pch_data,
+				"I2C-%d mode(%d) is not supported\n", mode, i);
+			continue;
+		}
+		reg_val = ioread32(p + PCH_I2CSR);
+		if (reg_val & (I2CMAL_BIT | I2CMCF_BIT | I2CMIF_BIT)) {
+			pch_i2c_cb(&adap_info->pch_data[i]);
+			flag = 1;
+		}
 	}
 
-	reg_val = ioread32(p + PCH_I2CSR);
-	if (reg_val & (I2CMAL_BIT | I2CMCF_BIT | I2CMIF_BIT))
-		pch_i2c_cb_ch0(adap_data);
-	else
-		return IRQ_NONE;
-
-	return IRQ_HANDLED;
+	return flag ? IRQ_HANDLED : IRQ_NONE;
 }
 
 /**
@@ -626,7 +645,7 @@ static irqreturn_t pch_i2c_handler(int irq, void *pData)
  * @num:	number of messages.
  */
 static s32 pch_i2c_xfer(struct i2c_adapter *i2c_adap,
-		    struct i2c_msg *msgs, s32 num)
+			struct i2c_msg *msgs, s32 num)
 {
 	struct i2c_msg *pmsg;
 	u32 i = 0;
@@ -709,11 +728,13 @@ static void pch_i2c_disbl_int(struct i2c_algo_pch_data *adap)
 }
 
 static int __devinit pch_i2c_probe(struct pci_dev *pdev,
-			       const struct pci_device_id *id)
+				   const struct pci_device_id *id)
 {
 	void __iomem *base_addr;
-	s32 ret;
+	int ret;
+	int i, j;
 	struct adapter_info *adap_info;
+	struct i2c_adapter *pch_adap;
 
 	pch_pci_dbg(pdev, "Entered.\n");
 
@@ -743,44 +764,48 @@ static int __devinit pch_i2c_probe(struct pci_dev *pdev,
 		goto err_pci_iomap;
 	}
 
-	adap_info->pch_i2c_suspended = false;
+	/* Set the number of I2C channel instance */
+	adap_info->ch_num = id->driver_data;
 
-	adap_info->pch_data.p_adapter_info = adap_info;
+	for (i = 0; i < adap_info->ch_num; i++) {
+		pch_adap = &adap_info->pch_data[i].pch_adapter;
+		adap_info->pch_i2c_suspended = false;
 
-	adap_info->pch_data.pch_adapter.owner = THIS_MODULE;
-	adap_info->pch_data.pch_adapter.class = I2C_CLASS_HWMON;
-	strcpy(adap_info->pch_data.pch_adapter.name, KBUILD_MODNAME);
-	adap_info->pch_data.pch_adapter.algo = &pch_algorithm;
-	adap_info->pch_data.pch_adapter.algo_data =
-						&adap_info->pch_data;
+		adap_info->pch_data[i].p_adapter_info = adap_info;
 
-	/* (i * 0x80) + base_addr; */
-	adap_info->pch_data.pch_base_address = base_addr;
+		pch_adap->owner = THIS_MODULE;
+		pch_adap->class = I2C_CLASS_HWMON;
+		strcpy(pch_adap->name, KBUILD_MODNAME);
+		pch_adap->algo = &pch_algorithm;
+		pch_adap->algo_data = &adap_info->pch_data[i];
 
-	adap_info->pch_data.pch_adapter.dev.parent = &pdev->dev;
+		/* base_addr + offset; */
+		adap_info->pch_data[i].pch_base_address = base_addr + 0x100 * i;
 
-	ret = i2c_add_adapter(&(adap_info->pch_data.pch_adapter));
+		pch_adap->dev.parent = &pdev->dev;
 
-	if (ret) {
-		pch_pci_err(pdev, "i2c_add_adapter FAILED\n");
-		goto err_i2c_add_adapter;
-	}
+		ret = i2c_add_adapter(pch_adap);
+		if (ret) {
+			pch_pci_err(pdev, "i2c_add_adapter[ch:%d] FAILED\n", i);
+			goto err_i2c_add_adapter;
+		}
 
-	pch_i2c_init(&adap_info->pch_data);
+		pch_i2c_init(&adap_info->pch_data[i]);
+	}
 	ret = request_irq(pdev->irq, pch_i2c_handler, IRQF_SHARED,
-		  KBUILD_MODNAME, &adap_info->pch_data);
+		  KBUILD_MODNAME, adap_info);
 	if (ret) {
 		pch_pci_err(pdev, "request_irq FAILED\n");
-		goto err_request_irq;
+		goto err_i2c_add_adapter;
 	}
 
 	pci_set_drvdata(pdev, adap_info);
 	pch_pci_dbg(pdev, "returns %d.\n", ret);
 	return 0;
 
-err_request_irq:
-	i2c_del_adapter(&(adap_info->pch_data.pch_adapter));
 err_i2c_add_adapter:
+	for (j = 0; j < i; j++)
+		i2c_del_adapter(&adap_info->pch_data[j].pch_adapter);
 	pci_iounmap(pdev, base_addr);
 err_pci_iomap:
 	pci_release_regions(pdev);
@@ -793,17 +818,22 @@ err_pci_enable:
 
 static void __devexit pch_i2c_remove(struct pci_dev *pdev)
 {
+	int i;
 	struct adapter_info *adap_info = pci_get_drvdata(pdev);
 
-	pch_i2c_disbl_int(&adap_info->pch_data);
-	free_irq(pdev->irq, &adap_info->pch_data);
-	i2c_del_adapter(&(adap_info->pch_data.pch_adapter));
+	free_irq(pdev->irq, adap_info);
 
-	if (adap_info->pch_data.pch_base_address) {
-		pci_iounmap(pdev, adap_info->pch_data.pch_base_address);
-		adap_info->pch_data.pch_base_address = 0;
+	for (i = 0; i < adap_info->ch_num; i++) {
+		pch_i2c_disbl_int(&adap_info->pch_data[i]);
+		i2c_del_adapter(&adap_info->pch_data[i].pch_adapter);
 	}
 
+	if (adap_info->pch_data[0].pch_base_address)
+		pci_iounmap(pdev, adap_info->pch_data[0].pch_base_address);
+
+	for (i = 0; i < adap_info->ch_num; i++)
+		adap_info->pch_data[i].pch_base_address = 0;
+
 	pci_set_drvdata(pdev, NULL);
 
 	pci_release_regions(pdev);
@@ -816,17 +846,22 @@ static void __devexit pch_i2c_remove(struct pci_dev *pdev)
 static int pch_i2c_suspend(struct pci_dev *pdev, pm_message_t state)
 {
 	int ret;
+	int i;
 	struct adapter_info *adap_info = pci_get_drvdata(pdev);
-	void __iomem *p = adap_info->pch_data.pch_base_address;
+	void __iomem *p = adap_info->pch_data[0].pch_base_address;
 
 	adap_info->pch_i2c_suspended = true;
 
-	while ((adap_info->pch_data.pch_i2c_xfer_in_progress)) {
-		/* Wait until all channel transfers are completed */
-		msleep(20);
+	for (i = 0; i < adap_info->ch_num; i++) {
+		while ((adap_info->pch_data[i].pch_i2c_xfer_in_progress)) {
+			/* Wait until all channel transfers are completed */
+			msleep(20);
+		}
 	}
+
 	/* Disable the i2c interrupts */
-	pch_i2c_disbl_int(&adap_info->pch_data);
+	for (i = 0; i < adap_info->ch_num; i++)
+		pch_i2c_disbl_int(&adap_info->pch_data[i]);
 
 	pch_pci_dbg(pdev, "I2CSR = %x I2CBUFSTA = %x I2CESRSTA = %x "
 		"invoked function pch_i2c_disbl_int successfully\n",
@@ -849,6 +884,7 @@ static int pch_i2c_suspend(struct pci_dev *pdev, pm_message_t state)
 
 static int pch_i2c_resume(struct pci_dev *pdev)
 {
+	int i;
 	struct adapter_info *adap_info = pci_get_drvdata(pdev);
 
 	pci_set_power_state(pdev, PCI_D0);
@@ -861,7 +897,8 @@ static int pch_i2c_resume(struct pci_dev *pdev)
 
 	pci_enable_wake(pdev, PCI_D3hot, 0);
 
-	pch_i2c_init(&adap_info->pch_data);
+	for (i = 0; i < adap_info->ch_num; i++)
+		pch_i2c_init(&adap_info->pch_data[i]);
 
 	adap_info->pch_i2c_suspended = false;
 
@@ -893,7 +930,7 @@ static void __exit pch_pci_exit(void)
 }
 module_exit(pch_pci_exit);
 
-MODULE_DESCRIPTION("PCH I2C PCI Driver");
+MODULE_DESCRIPTION("Intel EG20T PCH/OKI SEMICONDUCTOR ML7213 IOH I2C Driver");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Tomoya MORINAGA. <tomoya-linux@dsn.okisemi.com>");
 module_param(pch_i2c_speed, int, (S_IRUSR | S_IWUSR));
diff --git a/drivers/i2c/busses/i2c-ibm_iic.c b/drivers/i2c/busses/i2c-ibm_iic.c
index 6e3c38240336..e4f88dca99b5 100644
--- a/drivers/i2c/busses/i2c-ibm_iic.c
+++ b/drivers/i2c/busses/i2c-ibm_iic.c
@@ -691,8 +691,7 @@ static int __devinit iic_request_irq(struct platform_device *ofdev,
 /*
  * Register single IIC interface
  */
-static int __devinit iic_probe(struct platform_device *ofdev,
-			       const struct of_device_id *match)
+static int __devinit iic_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct ibm_iic_private *dev;
@@ -806,7 +805,7 @@ static const struct of_device_id ibm_iic_match[] = {
 	{}
 };
 
-static struct of_platform_driver ibm_iic_driver = {
+static struct platform_driver ibm_iic_driver = {
 	.driver = {
 		.name = "ibm-iic",
 		.owner = THIS_MODULE,
@@ -818,12 +817,12 @@ static struct of_platform_driver ibm_iic_driver = {
 
 static int __init iic_init(void)
 {
-	return of_register_platform_driver(&ibm_iic_driver);
+	return platform_driver_register(&ibm_iic_driver);
 }
 
 static void __exit iic_exit(void)
 {
-	of_unregister_platform_driver(&ibm_iic_driver);
+	platform_driver_unregister(&ibm_iic_driver);
 }
 
 module_init(iic_init);
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index b74e6dc6886c..75b984c519ac 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -560,8 +560,7 @@ static struct i2c_adapter mpc_ops = {
 	.timeout = HZ,
 };
 
-static int __devinit fsl_i2c_probe(struct platform_device *op,
-				   const struct of_device_id *match)
+static int __devinit fsl_i2c_probe(struct platform_device *op)
 {
 	struct mpc_i2c *i2c;
 	const u32 *prop;
@@ -569,6 +568,9 @@ static int __devinit fsl_i2c_probe(struct platform_device *op,
 	int result = 0;
 	int plen;
 
+	if (!op->dev.of_match)
+		return -EINVAL;
+
 	i2c = kzalloc(sizeof(*i2c), GFP_KERNEL);
 	if (!i2c)
 		return -ENOMEM;
@@ -603,8 +605,8 @@ static int __devinit fsl_i2c_probe(struct platform_device *op,
 			clock = *prop;
 	}
 
-	if (match->data) {
-		struct mpc_i2c_data *data = match->data;
+	if (op->dev.of_match->data) {
+		struct mpc_i2c_data *data = op->dev.of_match->data;
 		data->setup(op->dev.of_node, i2c, clock, data->prescaler);
 	} else {
 		/* Backwards compatibility */
@@ -700,7 +702,7 @@ static const struct of_device_id mpc_i2c_of_match[] = {
 MODULE_DEVICE_TABLE(of, mpc_i2c_of_match);
 
 /* Structure for a device driver */
-static struct of_platform_driver mpc_i2c_driver = {
+static struct platform_driver mpc_i2c_driver = {
 	.probe		= fsl_i2c_probe,
 	.remove		= __devexit_p(fsl_i2c_remove),
 	.driver = {
@@ -712,18 +714,12 @@ static struct of_platform_driver mpc_i2c_driver = {
 
 static int __init fsl_i2c_init(void)
 {
-	int rv;
-
-	rv = of_register_platform_driver(&mpc_i2c_driver);
-	if (rv)
-		printk(KERN_ERR DRV_NAME
-		       " of_register_platform_driver failed (%i)\n", rv);
-	return rv;
+	return platform_driver_register(&mpc_i2c_driver);
 }
 
 static void __exit fsl_i2c_exit(void)
 {
-	of_unregister_platform_driver(&mpc_i2c_driver);
+	platform_driver_unregister(&mpc_i2c_driver);
 }
 
 module_init(fsl_i2c_init);
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
new file mode 100644
index 000000000000..8022e2390a5a
--- /dev/null
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -0,0 +1,412 @@
+/*
+ * Freescale MXS I2C bus driver
+ *
+ * Copyright (C) 2011 Wolfram Sang, Pengutronix e.K.
+ *
+ * based on a (non-working) driver which was:
+ *
+ * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * TODO: add dma-support if platform-support for it is available
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+#include <linux/io.h>
+
+#include <mach/common.h>
+
+#define DRIVER_NAME "mxs-i2c"
+
+#define MXS_I2C_CTRL0		(0x00)
+#define MXS_I2C_CTRL0_SET	(0x04)
+
+#define MXS_I2C_CTRL0_SFTRST			0x80000000
+#define MXS_I2C_CTRL0_SEND_NAK_ON_LAST		0x02000000
+#define MXS_I2C_CTRL0_RETAIN_CLOCK		0x00200000
+#define MXS_I2C_CTRL0_POST_SEND_STOP		0x00100000
+#define MXS_I2C_CTRL0_PRE_SEND_START		0x00080000
+#define MXS_I2C_CTRL0_MASTER_MODE		0x00020000
+#define MXS_I2C_CTRL0_DIRECTION			0x00010000
+#define MXS_I2C_CTRL0_XFER_COUNT(v)		((v) & 0x0000FFFF)
+
+#define MXS_I2C_CTRL1		(0x40)
+#define MXS_I2C_CTRL1_SET	(0x44)
+#define MXS_I2C_CTRL1_CLR	(0x48)
+
+#define MXS_I2C_CTRL1_BUS_FREE_IRQ		0x80
+#define MXS_I2C_CTRL1_DATA_ENGINE_CMPLT_IRQ	0x40
+#define MXS_I2C_CTRL1_NO_SLAVE_ACK_IRQ		0x20
+#define MXS_I2C_CTRL1_OVERSIZE_XFER_TERM_IRQ	0x10
+#define MXS_I2C_CTRL1_EARLY_TERM_IRQ		0x08
+#define MXS_I2C_CTRL1_MASTER_LOSS_IRQ		0x04
+#define MXS_I2C_CTRL1_SLAVE_STOP_IRQ		0x02
+#define MXS_I2C_CTRL1_SLAVE_IRQ			0x01
+
+#define MXS_I2C_IRQ_MASK	(MXS_I2C_CTRL1_DATA_ENGINE_CMPLT_IRQ | \
+				 MXS_I2C_CTRL1_NO_SLAVE_ACK_IRQ | \
+				 MXS_I2C_CTRL1_EARLY_TERM_IRQ | \
+				 MXS_I2C_CTRL1_MASTER_LOSS_IRQ | \
+				 MXS_I2C_CTRL1_SLAVE_STOP_IRQ | \
+				 MXS_I2C_CTRL1_SLAVE_IRQ)
+
+#define MXS_I2C_QUEUECTRL	(0x60)
+#define MXS_I2C_QUEUECTRL_SET	(0x64)
+#define MXS_I2C_QUEUECTRL_CLR	(0x68)
+
+#define MXS_I2C_QUEUECTRL_QUEUE_RUN		0x20
+#define MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE	0x04
+
+#define MXS_I2C_QUEUESTAT	(0x70)
+#define MXS_I2C_QUEUESTAT_RD_QUEUE_EMPTY        0x00002000
+
+#define MXS_I2C_QUEUECMD	(0x80)
+
+#define MXS_I2C_QUEUEDATA	(0x90)
+
+#define MXS_I2C_DATA		(0xa0)
+
+
+#define MXS_CMD_I2C_SELECT	(MXS_I2C_CTRL0_RETAIN_CLOCK |	\
+				 MXS_I2C_CTRL0_PRE_SEND_START |	\
+				 MXS_I2C_CTRL0_MASTER_MODE |	\
+				 MXS_I2C_CTRL0_DIRECTION |	\
+				 MXS_I2C_CTRL0_XFER_COUNT(1))
+
+#define MXS_CMD_I2C_WRITE	(MXS_I2C_CTRL0_PRE_SEND_START |	\
+				 MXS_I2C_CTRL0_MASTER_MODE |	\
+				 MXS_I2C_CTRL0_DIRECTION)
+
+#define MXS_CMD_I2C_READ	(MXS_I2C_CTRL0_SEND_NAK_ON_LAST | \
+				 MXS_I2C_CTRL0_MASTER_MODE)
+
+/**
+ * struct mxs_i2c_dev - per device, private MXS-I2C data
+ *
+ * @dev: driver model device node
+ * @regs: IO registers pointer
+ * @cmd_complete: completion object for transaction wait
+ * @cmd_err: error code for last transaction
+ * @adapter: i2c subsystem adapter node
+ */
+struct mxs_i2c_dev {
+	struct device *dev;
+	void __iomem *regs;
+	struct completion cmd_complete;
+	u32 cmd_err;
+	struct i2c_adapter adapter;
+};
+
+/*
+ * TODO: check if calls to here are really needed. If not, we could get rid of
+ * mxs_reset_block and the mach-dependency. Needs an I2C analyzer, probably.
+ */
+static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
+{
+	mxs_reset_block(i2c->regs);
+	writel(MXS_I2C_IRQ_MASK << 8, i2c->regs + MXS_I2C_CTRL1_SET);
+}
+
+static void mxs_i2c_pioq_setup_read(struct mxs_i2c_dev *i2c, u8 addr, int len,
+					int flags)
+{
+	u32 data;
+
+	writel(MXS_CMD_I2C_SELECT, i2c->regs + MXS_I2C_QUEUECMD);
+
+	data = (addr << 1) | I2C_SMBUS_READ;
+	writel(data, i2c->regs + MXS_I2C_DATA);
+
+	data = MXS_CMD_I2C_READ | MXS_I2C_CTRL0_XFER_COUNT(len) | flags;
+	writel(data, i2c->regs + MXS_I2C_QUEUECMD);
+}
+
+static void mxs_i2c_pioq_setup_write(struct mxs_i2c_dev *i2c,
+				    u8 addr, u8 *buf, int len, int flags)
+{
+	u32 data;
+	int i, shifts_left;
+
+	data = MXS_CMD_I2C_WRITE | MXS_I2C_CTRL0_XFER_COUNT(len + 1) | flags;
+	writel(data, i2c->regs + MXS_I2C_QUEUECMD);
+
+	/*
+	 * We have to copy the slave address (u8) and buffer (arbitrary number
+	 * of u8) into the data register (u32). To achieve that, the u8 are put
+	 * into the MSBs of 'data' which is then shifted for the next u8. When
+	 * apropriate, 'data' is written to MXS_I2C_DATA. So, the first u32
+	 * looks like this:
+	 *
+	 *  3          2          1          0
+	 * 10987654|32109876|54321098|76543210
+	 * --------+--------+--------+--------
+	 * buffer+2|buffer+1|buffer+0|slave_addr
+	 */
+
+	data = ((addr << 1) | I2C_SMBUS_WRITE) << 24;
+
+	for (i = 0; i < len; i++) {
+		data >>= 8;
+		data |= buf[i] << 24;
+		if ((i & 3) == 2)
+			writel(data, i2c->regs + MXS_I2C_DATA);
+	}
+
+	/* Write out the remaining bytes if any */
+	shifts_left = 24 - (i & 3) * 8;
+	if (shifts_left)
+		writel(data >> shifts_left, i2c->regs + MXS_I2C_DATA);
+}
+
+/*
+ * TODO: should be replaceable with a waitqueue and RD_QUEUE_IRQ (setting the
+ * rd_threshold to 1). Couldn't get this to work, though.
+ */
+static int mxs_i2c_wait_for_data(struct mxs_i2c_dev *i2c)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	while (readl(i2c->regs + MXS_I2C_QUEUESTAT)
+			& MXS_I2C_QUEUESTAT_RD_QUEUE_EMPTY) {
+			if (time_after(jiffies, timeout))
+				return -ETIMEDOUT;
+			cond_resched();
+	}
+
+	return 0;
+}
+
+static int mxs_i2c_finish_read(struct mxs_i2c_dev *i2c, u8 *buf, int len)
+{
+	u32 data;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		if ((i & 3) == 0) {
+			if (mxs_i2c_wait_for_data(i2c))
+				return -ETIMEDOUT;
+			data = readl(i2c->regs + MXS_I2C_QUEUEDATA);
+		}
+		buf[i] = data & 0xff;
+		data >>= 8;
+	}
+
+	return 0;
+}
+
+/*
+ * Low level master read/write transaction.
+ */
+static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
+				int stop)
+{
+	struct mxs_i2c_dev *i2c = i2c_get_adapdata(adap);
+	int ret;
+	int flags;
+
+	init_completion(&i2c->cmd_complete);
+
+	dev_dbg(i2c->dev, "addr: 0x%04x, len: %d, flags: 0x%x, stop: %d\n",
+		msg->addr, msg->len, msg->flags, stop);
+
+	if (msg->len == 0)
+		return -EINVAL;
+
+	flags = stop ? MXS_I2C_CTRL0_POST_SEND_STOP : 0;
+
+	if (msg->flags & I2C_M_RD)
+		mxs_i2c_pioq_setup_read(i2c, msg->addr, msg->len, flags);
+	else
+		mxs_i2c_pioq_setup_write(i2c, msg->addr, msg->buf, msg->len,
+					flags);
+
+	writel(MXS_I2C_QUEUECTRL_QUEUE_RUN,
+			i2c->regs + MXS_I2C_QUEUECTRL_SET);
+
+	ret = wait_for_completion_timeout(&i2c->cmd_complete,
+						msecs_to_jiffies(1000));
+	if (ret == 0)
+		goto timeout;
+
+	if ((!i2c->cmd_err) && (msg->flags & I2C_M_RD)) {
+		ret = mxs_i2c_finish_read(i2c, msg->buf, msg->len);
+		if (ret)
+			goto timeout;
+	}
+
+	if (i2c->cmd_err == -ENXIO)
+		mxs_i2c_reset(i2c);
+
+	dev_dbg(i2c->dev, "Done with err=%d\n", i2c->cmd_err);
+
+	return i2c->cmd_err;
+
+timeout:
+	dev_dbg(i2c->dev, "Timeout!\n");
+	mxs_i2c_reset(i2c);
+	return -ETIMEDOUT;
+}
+
+static int mxs_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
+			int num)
+{
+	int i;
+	int err;
+
+	for (i = 0; i < num; i++) {
+		err = mxs_i2c_xfer_msg(adap, &msgs[i], i == (num - 1));
+		if (err)
+			return err;
+	}
+
+	return num;
+}
+
+static u32 mxs_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
+}
+
+static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
+{
+	struct mxs_i2c_dev *i2c = dev_id;
+	u32 stat = readl(i2c->regs + MXS_I2C_CTRL1) & MXS_I2C_IRQ_MASK;
+
+	if (!stat)
+		return IRQ_NONE;
+
+	if (stat & MXS_I2C_CTRL1_NO_SLAVE_ACK_IRQ)
+		i2c->cmd_err = -ENXIO;
+	else if (stat & (MXS_I2C_CTRL1_EARLY_TERM_IRQ |
+		    MXS_I2C_CTRL1_MASTER_LOSS_IRQ |
+		    MXS_I2C_CTRL1_SLAVE_STOP_IRQ | MXS_I2C_CTRL1_SLAVE_IRQ))
+		/* MXS_I2C_CTRL1_OVERSIZE_XFER_TERM_IRQ is only for slaves */
+		i2c->cmd_err = -EIO;
+	else
+		i2c->cmd_err = 0;
+
+	complete(&i2c->cmd_complete);
+
+	writel(stat, i2c->regs + MXS_I2C_CTRL1_CLR);
+	return IRQ_HANDLED;
+}
+
+static const struct i2c_algorithm mxs_i2c_algo = {
+	.master_xfer = mxs_i2c_xfer,
+	.functionality = mxs_i2c_func,
+};
+
+static int __devinit mxs_i2c_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mxs_i2c_dev *i2c;
+	struct i2c_adapter *adap;
+	struct resource *res;
+	resource_size_t res_size;
+	int err, irq;
+
+	i2c = devm_kzalloc(dev, sizeof(struct mxs_i2c_dev), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENOENT;
+
+	res_size = resource_size(res);
+	if (!devm_request_mem_region(dev, res->start, res_size, res->name))
+		return -EBUSY;
+
+	i2c->regs = devm_ioremap_nocache(dev, res->start, res_size);
+	if (!i2c->regs)
+		return -EBUSY;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	err = devm_request_irq(dev, irq, mxs_i2c_isr, 0, dev_name(dev), i2c);
+	if (err)
+		return err;
+
+	i2c->dev = dev;
+	platform_set_drvdata(pdev, i2c);
+
+	/* Do reset to enforce correct startup after pinmuxing */
+	mxs_i2c_reset(i2c);
+	writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
+			i2c->regs + MXS_I2C_QUEUECTRL_SET);
+
+	adap = &i2c->adapter;
+	strlcpy(adap->name, "MXS I2C adapter", sizeof(adap->name));
+	adap->owner = THIS_MODULE;
+	adap->algo = &mxs_i2c_algo;
+	adap->dev.parent = dev;
+	adap->nr = pdev->id;
+	i2c_set_adapdata(adap, i2c);
+	err = i2c_add_numbered_adapter(adap);
+	if (err) {
+		dev_err(dev, "Failed to add adapter (%d)\n", err);
+		writel(MXS_I2C_CTRL0_SFTRST,
+				i2c->regs + MXS_I2C_CTRL0_SET);
+		return err;
+	}
+
+	return 0;
+}
+
+static int __devexit mxs_i2c_remove(struct platform_device *pdev)
+{
+	struct mxs_i2c_dev *i2c = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = i2c_del_adapter(&i2c->adapter);
+	if (ret)
+		return -EBUSY;
+
+	writel(MXS_I2C_QUEUECTRL_QUEUE_RUN,
+			i2c->regs + MXS_I2C_QUEUECTRL_CLR);
+	writel(MXS_I2C_CTRL0_SFTRST, i2c->regs + MXS_I2C_CTRL0_SET);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver mxs_i2c_driver = {
+	.driver = {
+		   .name = DRIVER_NAME,
+		   .owner = THIS_MODULE,
+		   },
+	.remove = __devexit_p(mxs_i2c_remove),
+};
+
+static int __init mxs_i2c_init(void)
+{
+	return platform_driver_probe(&mxs_i2c_driver, mxs_i2c_probe);
+}
+subsys_initcall(mxs_i2c_init);
+
+static void __exit mxs_i2c_exit(void)
+{
+	platform_driver_unregister(&mxs_i2c_driver);
+}
+module_exit(mxs_i2c_exit);
+
+MODULE_AUTHOR("Wolfram Sang <w.sang@pengutronix.de>");
+MODULE_DESCRIPTION("MXS I2C Bus Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/i2c/busses/i2c-ocores.c b/drivers/i2c/busses/i2c-ocores.c
index ef3bcb1ce864..beaf68f3199f 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -49,6 +49,7 @@
 #include <linux/init.h>
 #include <linux/errno.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/wait.h>
@@ -305,7 +306,7 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
 		return -EIO;
 	}
 
-	pdata = pdev->dev.platform_data;
+	pdata = mfd_get_data(pdev);
 	if (pdata) {
 		i2c->regstep = pdata->regstep;
 		i2c->clock_khz = pdata->clock_khz;
@@ -330,9 +331,7 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
 	i2c->adap = ocores_adapter;
 	i2c_set_adapdata(&i2c->adap, i2c);
 	i2c->adap.dev.parent = &pdev->dev;
-#ifdef CONFIG_OF
 	i2c->adap.dev.of_node = pdev->dev.of_node;
-#endif
 
 	/* add i2c adapter to i2c tree */
 	ret = i2c_add_adapter(&i2c->adap);
@@ -390,15 +389,11 @@ static int ocores_i2c_resume(struct platform_device *pdev)
 #define ocores_i2c_resume	NULL
 #endif
 
-#ifdef CONFIG_OF
 static struct of_device_id ocores_i2c_match[] = {
-        {
-                .compatible = "opencores,i2c-ocores",
-        },
-        {},
+	{ .compatible = "opencores,i2c-ocores", },
+	{},
 };
 MODULE_DEVICE_TABLE(of, ocores_i2c_match);
-#endif
 
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:ocores-i2c");
@@ -411,9 +406,7 @@ static struct platform_driver ocores_i2c_driver = {
 	.driver  = {
 		.owner = THIS_MODULE,
 		.name = "ocores-i2c",
-#ifdef CONFIG_OF
-                .of_match_table = ocores_i2c_match,
-#endif
+		.of_match_table = ocores_i2c_match,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index b605ff3a1fa0..829a2a1029f7 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -847,11 +847,15 @@ complete:
 			dev_err(dev->dev, "Arbitration lost\n");
 			err |= OMAP_I2C_STAT_AL;
 		}
+		/*
+		 * ProDB0017052: Clear ARDY bit twice
+		 */
 		if (stat & (OMAP_I2C_STAT_ARDY | OMAP_I2C_STAT_NACK |
 					OMAP_I2C_STAT_AL)) {
 			omap_i2c_ack_stat(dev, stat &
 				(OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
-				OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
+				OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR |
+				OMAP_I2C_STAT_ARDY));
 			omap_i2c_complete_cmd(dev, err);
 			return IRQ_HANDLED;
 		}
@@ -1137,12 +1141,41 @@ omap_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_SUSPEND
+static int omap_i2c_suspend(struct device *dev)
+{
+	if (!pm_runtime_suspended(dev))
+		if (dev->bus && dev->bus->pm && dev->bus->pm->runtime_suspend)
+			dev->bus->pm->runtime_suspend(dev);
+
+	return 0;
+}
+
+static int omap_i2c_resume(struct device *dev)
+{
+	if (!pm_runtime_suspended(dev))
+		if (dev->bus && dev->bus->pm && dev->bus->pm->runtime_resume)
+			dev->bus->pm->runtime_resume(dev);
+
+	return 0;
+}
+
+static struct dev_pm_ops omap_i2c_pm_ops = {
+	.suspend = omap_i2c_suspend,
+	.resume = omap_i2c_resume,
+};
+#define OMAP_I2C_PM_OPS (&omap_i2c_pm_ops)
+#else
+#define OMAP_I2C_PM_OPS NULL
+#endif
+
 static struct platform_driver omap_i2c_driver = {
 	.probe		= omap_i2c_probe,
 	.remove		= omap_i2c_remove,
 	.driver		= {
 		.name	= "omap_i2c",
 		.owner	= THIS_MODULE,
+		.pm	= OMAP_I2C_PM_OPS,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
new file mode 100644
index 000000000000..fac673940849
--- /dev/null
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -0,0 +1,306 @@
+/*
+ * I2C driver for PKUnity-v3 SoC
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *	Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *	Copyright (C) 2001-2010 Guan Xuetao
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <mach/hardware.h>
+
+/*
+ * Poll the i2c status register until the specified bit is set.
+ * Returns 0 if timed out (100 msec).
+ */
+static short poll_status(unsigned long bit)
+{
+	int loop_cntr = 1000;
+
+	if (bit & I2C_STATUS_TFNF) {
+		do {
+			udelay(10);
+		} while (!(readl(I2C_STATUS) & bit) && (--loop_cntr > 0));
+	} else {
+		/* RXRDY handler */
+		do {
+			if (readl(I2C_TAR) == I2C_TAR_EEPROM)
+				msleep(20);
+			else
+				udelay(10);
+		} while (!(readl(I2C_RXFLR) & 0xf) && (--loop_cntr > 0));
+	}
+
+	return (loop_cntr > 0);
+}
+
+static int xfer_read(struct i2c_adapter *adap, unsigned char *buf, int length)
+{
+	int i2c_reg = *buf;
+
+	/* Read data */
+	while (length--) {
+		if (!poll_status(I2C_STATUS_TFNF)) {
+			dev_dbg(&adap->dev, "Tx FIFO Not Full timeout\n");
+			return -ETIMEDOUT;
+		}
+
+		/* send addr */
+		writel(i2c_reg | I2C_DATACMD_WRITE, I2C_DATACMD);
+
+		/* get ready to next write */
+		i2c_reg++;
+
+		/* send read CMD */
+		writel(I2C_DATACMD_READ, I2C_DATACMD);
+
+		/* wait until the Rx FIFO have available */
+		if (!poll_status(I2C_STATUS_RFNE)) {
+			dev_dbg(&adap->dev, "RXRDY timeout\n");
+			return -ETIMEDOUT;
+		}
+
+		/* read the data to buf */
+		*buf = (readl(I2C_DATACMD) & I2C_DATACMD_DAT_MASK);
+		buf++;
+	}
+
+	return 0;
+}
+
+static int xfer_write(struct i2c_adapter *adap, unsigned char *buf, int length)
+{
+	int i2c_reg = *buf;
+
+	/* Do nothing but storing the reg_num to a static variable */
+	if (i2c_reg == -1) {
+		printk(KERN_WARNING "Error i2c reg\n");
+		return -ETIMEDOUT;
+	}
+
+	if (length == 1)
+		return 0;
+
+	buf++;
+	length--;
+	while (length--) {
+		/* send addr */
+		writel(i2c_reg | I2C_DATACMD_WRITE, I2C_DATACMD);
+
+		/* send write CMD */
+		writel(*buf | I2C_DATACMD_WRITE, I2C_DATACMD);
+
+		/* wait until the Rx FIFO have available */
+		msleep(20);
+
+		/* read the data to buf */
+		i2c_reg++;
+		buf++;
+	}
+
+	return 0;
+}
+
+/*
+ * Generic i2c master transfer entrypoint.
+ *
+ */
+static int puv3_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *pmsg,
+		int num)
+{
+	int i, ret;
+	unsigned char swap;
+
+	/* Disable i2c */
+	writel(I2C_ENABLE_DISABLE, I2C_ENABLE);
+
+	/* Set the work mode and speed*/
+	writel(I2C_CON_MASTER | I2C_CON_SPEED_STD | I2C_CON_SLAVEDISABLE, I2C_CON);
+
+	writel(pmsg->addr, I2C_TAR);
+
+	/* Enable i2c */
+	writel(I2C_ENABLE_ENABLE, I2C_ENABLE);
+
+	dev_dbg(&adap->dev, "puv3_i2c_xfer: processing %d messages:\n", num);
+
+	for (i = 0; i < num; i++) {
+		dev_dbg(&adap->dev, " #%d: %sing %d byte%s %s 0x%02x\n", i,
+			pmsg->flags & I2C_M_RD ? "read" : "writ",
+			pmsg->len, pmsg->len > 1 ? "s" : "",
+			pmsg->flags & I2C_M_RD ? "from" : "to",	pmsg->addr);
+
+		if (pmsg->len && pmsg->buf) {	/* sanity check */
+			if (pmsg->flags & I2C_M_RD)
+				ret = xfer_read(adap, pmsg->buf, pmsg->len);
+			else
+				ret = xfer_write(adap, pmsg->buf, pmsg->len);
+
+			if (ret)
+				return ret;
+
+		}
+		dev_dbg(&adap->dev, "transfer complete\n");
+		pmsg++;		/* next message */
+	}
+
+	/* XXX: fixup be16_to_cpu in bq27x00_battery.c */
+	if (pmsg->addr == I2C_TAR_PWIC) {
+		swap = pmsg->buf[0];
+		pmsg->buf[0] = pmsg->buf[1];
+		pmsg->buf[1] = swap;
+	}
+
+	return i;
+}
+
+/*
+ * Return list of supported functionality.
+ */
+static u32 puv3_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static struct i2c_algorithm puv3_i2c_algorithm = {
+	.master_xfer	= puv3_i2c_xfer,
+	.functionality	= puv3_i2c_func,
+};
+
+/*
+ * Main initialization routine.
+ */
+static int __devinit puv3_i2c_probe(struct platform_device *pdev)
+{
+	struct i2c_adapter *adapter;
+	struct resource *mem;
+	int rc;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -ENODEV;
+
+	if (!request_mem_region(mem->start, resource_size(mem), "puv3_i2c"))
+		return -EBUSY;
+
+	adapter = kzalloc(sizeof(struct i2c_adapter), GFP_KERNEL);
+	if (adapter == NULL) {
+		dev_err(&pdev->dev, "can't allocate inteface!\n");
+		rc = -ENOMEM;
+		goto fail_nomem;
+	}
+	snprintf(adapter->name, sizeof(adapter->name), "PUV3-I2C at 0x%08x",
+			mem->start);
+	adapter->algo = &puv3_i2c_algorithm;
+	adapter->class = I2C_CLASS_HWMON;
+	adapter->dev.parent = &pdev->dev;
+
+	platform_set_drvdata(pdev, adapter);
+
+	adapter->nr = pdev->id;
+	rc = i2c_add_numbered_adapter(adapter);
+	if (rc) {
+		dev_err(&pdev->dev, "Adapter '%s' registration failed\n",
+				adapter->name);
+		goto fail_add_adapter;
+	}
+
+	dev_info(&pdev->dev, "PKUnity v3 i2c bus adapter.\n");
+	return 0;
+
+fail_add_adapter:
+	platform_set_drvdata(pdev, NULL);
+	kfree(adapter);
+fail_nomem:
+	release_mem_region(mem->start, resource_size(mem));
+
+	return rc;
+}
+
+static int __devexit puv3_i2c_remove(struct platform_device *pdev)
+{
+	struct i2c_adapter *adapter = platform_get_drvdata(pdev);
+	struct resource *mem;
+	int rc;
+
+	rc = i2c_del_adapter(adapter);
+	if (rc) {
+		dev_err(&pdev->dev, "Adapter '%s' delete fail\n",
+				adapter->name);
+		return rc;
+	}
+
+	put_device(&pdev->dev);
+	platform_set_drvdata(pdev, NULL);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, resource_size(mem));
+
+	return rc;
+}
+
+#ifdef CONFIG_PM
+static int puv3_i2c_suspend(struct platform_device *dev, pm_message_t state)
+{
+	int poll_count;
+	/* Disable the IIC */
+	writel(I2C_ENABLE_DISABLE, I2C_ENABLE);
+	for (poll_count = 0; poll_count < 50; poll_count++) {
+		if (readl(I2C_ENSTATUS) & I2C_ENSTATUS_ENABLE)
+			udelay(25);
+	}
+
+	return 0;
+}
+
+static int puv3_i2c_resume(struct platform_device *dev)
+{
+	return 0 ;
+}
+#else
+#define puv3_i2c_suspend NULL
+#define puv3_i2c_resume NULL
+#endif
+
+MODULE_ALIAS("platform:puv3_i2c");
+
+static struct platform_driver puv3_i2c_driver = {
+	.probe		= puv3_i2c_probe,
+	.remove		= __devexit_p(puv3_i2c_remove),
+	.suspend	= puv3_i2c_suspend,
+	.resume		= puv3_i2c_resume,
+	.driver		= {
+		.name	= "PKUnity-v3-I2C",
+		.owner	= THIS_MODULE,
+	}
+};
+
+static int __init puv3_i2c_init(void)
+{
+	return platform_driver_register(&puv3_i2c_driver);
+}
+
+static void __exit puv3_i2c_exit(void)
+{
+	platform_driver_unregister(&puv3_i2c_driver);
+}
+
+module_init(puv3_i2c_init);
+module_exit(puv3_i2c_exit);
+
+MODULE_DESCRIPTION("PKUnity v3 I2C driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-pxa-pci.c b/drivers/i2c/busses/i2c-pxa-pci.c
new file mode 100644
index 000000000000..6659d269b841
--- /dev/null
+++ b/drivers/i2c/busses/i2c-pxa-pci.c
@@ -0,0 +1,176 @@
+/*
+ * The CE4100's I2C device is more or less the same one as found on PXA.
+ * It does not support slave mode, the register slightly moved. This PCI
+ * device provides three bars, every contains a single I2C controller.
+ */
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/i2c/pxa-i2c.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+
+#define CE4100_PCI_I2C_DEVS	3
+
+struct ce4100_devices {
+	struct platform_device *pdev[CE4100_PCI_I2C_DEVS];
+};
+
+static struct platform_device *add_i2c_device(struct pci_dev *dev, int bar)
+{
+	struct platform_device *pdev;
+	struct i2c_pxa_platform_data pdata;
+	struct resource res[2];
+	struct device_node *child;
+	static int devnum;
+	int ret;
+
+	memset(&pdata, 0, sizeof(struct i2c_pxa_platform_data));
+	memset(&res, 0, sizeof(res));
+
+	res[0].flags = IORESOURCE_MEM;
+	res[0].start = pci_resource_start(dev, bar);
+	res[0].end = pci_resource_end(dev, bar);
+
+	res[1].flags = IORESOURCE_IRQ;
+	res[1].start = dev->irq;
+	res[1].end = dev->irq;
+
+	for_each_child_of_node(dev->dev.of_node, child) {
+		const void *prop;
+		struct resource r;
+		int ret;
+
+		ret = of_address_to_resource(child, 0, &r);
+		if (ret < 0)
+			continue;
+		if (r.start != res[0].start)
+			continue;
+		if (r.end != res[0].end)
+			continue;
+		if (r.flags != res[0].flags)
+			continue;
+
+		prop = of_get_property(child, "fast-mode", NULL);
+		if (prop)
+			pdata.fast_mode = 1;
+
+		break;
+	}
+
+	if (!child) {
+		dev_err(&dev->dev, "failed to match a DT node for bar %d.\n",
+				bar);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	pdev = platform_device_alloc("ce4100-i2c", devnum);
+	if (!pdev) {
+		of_node_put(child);
+		ret = -ENOMEM;
+		goto out;
+	}
+	pdev->dev.parent = &dev->dev;
+	pdev->dev.of_node = child;
+
+	ret = platform_device_add_resources(pdev, res, ARRAY_SIZE(res));
+	if (ret)
+		goto err;
+
+	ret = platform_device_add_data(pdev, &pdata, sizeof(pdata));
+	if (ret)
+		goto err;
+
+	ret = platform_device_add(pdev);
+	if (ret)
+		goto err;
+	devnum++;
+	return pdev;
+err:
+	platform_device_put(pdev);
+out:
+	return ERR_PTR(ret);
+}
+
+static int __devinit ce4100_i2c_probe(struct pci_dev *dev,
+		const struct pci_device_id *ent)
+{
+	int ret;
+	int i;
+	struct ce4100_devices *sds;
+
+	ret = pci_enable_device_mem(dev);
+	if (ret)
+		return ret;
+
+	if (!dev->dev.of_node) {
+		dev_err(&dev->dev, "Missing device tree node.\n");
+		return -EINVAL;
+	}
+	sds = kzalloc(sizeof(*sds), GFP_KERNEL);
+	if (!sds)
+		goto err_mem;
+
+	for (i = 0; i < ARRAY_SIZE(sds->pdev); i++) {
+		sds->pdev[i] = add_i2c_device(dev, i);
+		if (IS_ERR(sds->pdev[i])) {
+			while (--i >= 0)
+				platform_device_unregister(sds->pdev[i]);
+			goto err_dev_add;
+		}
+	}
+	pci_set_drvdata(dev, sds);
+	return 0;
+
+err_dev_add:
+	pci_set_drvdata(dev, NULL);
+	kfree(sds);
+err_mem:
+	pci_disable_device(dev);
+	return ret;
+}
+
+static void __devexit ce4100_i2c_remove(struct pci_dev *dev)
+{
+	struct ce4100_devices *sds;
+	unsigned int i;
+
+	sds = pci_get_drvdata(dev);
+	pci_set_drvdata(dev, NULL);
+
+	for (i = 0; i < ARRAY_SIZE(sds->pdev); i++)
+		platform_device_unregister(sds->pdev[i]);
+
+	pci_disable_device(dev);
+	kfree(sds);
+}
+
+static struct pci_device_id ce4100_i2c_devices[] __devinitdata = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x2e68)},
+	{ },
+};
+MODULE_DEVICE_TABLE(pci, ce4100_i2c_devices);
+
+static struct pci_driver ce4100_i2c_driver = {
+	.name           = "ce4100_i2c",
+	.id_table       = ce4100_i2c_devices,
+	.probe          = ce4100_i2c_probe,
+	.remove         = __devexit_p(ce4100_i2c_remove),
+};
+
+static int __init ce4100_i2c_init(void)
+{
+	return pci_register_driver(&ce4100_i2c_driver);
+}
+module_init(ce4100_i2c_init);
+
+static void __exit ce4100_i2c_exit(void)
+{
+	pci_unregister_driver(&ce4100_i2c_driver);
+}
+module_exit(ce4100_i2c_exit);
+
+MODULE_DESCRIPTION("CE4100 PCI-I2C glue code for PXA's driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Sebastian Andrzej Siewior <bigeasy@linutronix.de>");
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index f4c19a97e0b3..a90739be1369 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -29,38 +29,75 @@
 #include <linux/errno.h>
 #include <linux/interrupt.h>
 #include <linux/i2c-pxa.h>
+#include <linux/of_i2c.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/i2c/pxa-i2c.h>
 
 #include <asm/irq.h>
-#include <plat/i2c.h>
+
+#ifdef CONFIG_X86
+#define clk_get(dev, id)	NULL
+#define clk_put(clk)		do { } while (0)
+#define clk_disable(clk)	do { } while (0)
+#define clk_enable(clk)		do { } while (0)
+#endif
+
+struct pxa_reg_layout {
+	u32 ibmr;
+	u32 idbr;
+	u32 icr;
+	u32 isr;
+	u32 isar;
+};
+
+enum pxa_i2c_types {
+	REGS_PXA2XX,
+	REGS_PXA3XX,
+	REGS_CE4100,
+};
 
 /*
- * I2C register offsets will be shifted 0 or 1 bit left, depending on
- * different SoCs
+ * I2C registers definitions
  */
-#define REG_SHIFT_0	(0 << 0)
-#define REG_SHIFT_1	(1 << 0)
-#define REG_SHIFT(d)	((d) & 0x1)
+static struct pxa_reg_layout pxa_reg_layout[] = {
+	[REGS_PXA2XX] = {
+		.ibmr =	0x00,
+		.idbr =	0x10,
+		.icr =	0x20,
+		.isr =	0x30,
+		.isar =	0x40,
+	},
+	[REGS_PXA3XX] = {
+		.ibmr =	0x00,
+		.idbr =	0x08,
+		.icr =	0x10,
+		.isr =	0x18,
+		.isar =	0x20,
+	},
+	[REGS_CE4100] = {
+		.ibmr =	0x14,
+		.idbr =	0x0c,
+		.icr =	0x00,
+		.isr =	0x04,
+		/* no isar register */
+	},
+};
 
 static const struct platform_device_id i2c_pxa_id_table[] = {
-	{ "pxa2xx-i2c",		REG_SHIFT_1 },
-	{ "pxa3xx-pwri2c",	REG_SHIFT_0 },
+	{ "pxa2xx-i2c",		REGS_PXA2XX },
+	{ "pxa3xx-pwri2c",	REGS_PXA3XX },
+	{ "ce4100-i2c",		REGS_CE4100 },
 	{ },
 };
 MODULE_DEVICE_TABLE(platform, i2c_pxa_id_table);
 
 /*
- * I2C registers and bit definitions
+ * I2C bit definitions
  */
-#define IBMR		(0x00)
-#define IDBR		(0x08)
-#define ICR		(0x10)
-#define ISR		(0x18)
-#define ISAR		(0x20)
 
 #define ICR_START	(1 << 0)	   /* start bit */
 #define ICR_STOP	(1 << 1)	   /* stop bit */
@@ -111,7 +148,11 @@ struct pxa_i2c {
 	u32			icrlog[32];
 
 	void __iomem		*reg_base;
-	unsigned int		reg_shift;
+	void __iomem		*reg_ibmr;
+	void __iomem		*reg_idbr;
+	void __iomem		*reg_icr;
+	void __iomem		*reg_isr;
+	void __iomem		*reg_isar;
 
 	unsigned long		iobase;
 	unsigned long		iosize;
@@ -121,11 +162,11 @@ struct pxa_i2c {
 	unsigned int		fast_mode :1;
 };
 
-#define _IBMR(i2c)	((i2c)->reg_base + (0x0 << (i2c)->reg_shift))
-#define _IDBR(i2c)	((i2c)->reg_base + (0x4 << (i2c)->reg_shift))
-#define _ICR(i2c)	((i2c)->reg_base + (0x8 << (i2c)->reg_shift))
-#define _ISR(i2c)	((i2c)->reg_base + (0xc << (i2c)->reg_shift))
-#define _ISAR(i2c)	((i2c)->reg_base + (0x10 << (i2c)->reg_shift))
+#define _IBMR(i2c)	((i2c)->reg_ibmr)
+#define _IDBR(i2c)	((i2c)->reg_idbr)
+#define _ICR(i2c)	((i2c)->reg_icr)
+#define _ISR(i2c)	((i2c)->reg_isr)
+#define _ISAR(i2c)	((i2c)->reg_isar)
 
 /*
  * I2C Slave mode address
@@ -418,7 +459,8 @@ static void i2c_pxa_reset(struct pxa_i2c *i2c)
 	writel(I2C_ISR_INIT, _ISR(i2c));
 	writel(readl(_ICR(i2c)) & ~ICR_UR, _ICR(i2c));
 
-	writel(i2c->slave_addr, _ISAR(i2c));
+	if (i2c->reg_isar)
+		writel(i2c->slave_addr, _ISAR(i2c));
 
 	/* set control register values */
 	writel(I2C_ICR_INIT | (i2c->fast_mode ? ICR_FM : 0), _ICR(i2c));
@@ -729,8 +771,10 @@ static int i2c_pxa_do_xfer(struct pxa_i2c *i2c, struct i2c_msg *msg, int num)
 	 */
 	ret = i2c->msg_idx;
 
-	if (timeout == 0)
+	if (!timeout && i2c->msg_num) {
 		i2c_pxa_scream_blue_murder(i2c, "timeout");
+		ret = I2C_RETRY;
+	}
 
  out:
 	return ret;
@@ -915,11 +959,17 @@ static void i2c_pxa_irq_rxfull(struct pxa_i2c *i2c, u32 isr)
 	writel(icr, _ICR(i2c));
 }
 
+#define VALID_INT_SOURCE	(ISR_SSD | ISR_ALD | ISR_ITE | ISR_IRF | \
+				ISR_SAD | ISR_BED)
 static irqreturn_t i2c_pxa_handler(int this_irq, void *dev_id)
 {
 	struct pxa_i2c *i2c = dev_id;
 	u32 isr = readl(_ISR(i2c));
 
+	isr &= VALID_INT_SOURCE;
+	if (!isr)
+		return IRQ_NONE;
+
 	if (i2c_debug > 2 && 0) {
 		dev_dbg(&i2c->adap.dev, "%s: ISR=%08x, ICR=%08x, IBMR=%02x\n",
 			__func__, isr, readl(_ICR(i2c)), readl(_IBMR(i2c)));
@@ -934,7 +984,7 @@ static irqreturn_t i2c_pxa_handler(int this_irq, void *dev_id)
 	/*
 	 * Always clear all pending IRQs.
 	 */
-	writel(isr & (ISR_SSD|ISR_ALD|ISR_ITE|ISR_IRF|ISR_SAD|ISR_BED), _ISR(i2c));
+	writel(isr, _ISR(i2c));
 
 	if (isr & ISR_SAD)
 		i2c_pxa_slave_start(i2c, isr);
@@ -1001,6 +1051,7 @@ static int i2c_pxa_probe(struct platform_device *dev)
 	struct resource *res;
 	struct i2c_pxa_platform_data *plat = dev->dev.platform_data;
 	const struct platform_device_id *id = platform_get_device_id(dev);
+	enum pxa_i2c_types i2c_type = id->driver_data;
 	int ret;
 	int irq;
 
@@ -1044,7 +1095,13 @@ static int i2c_pxa_probe(struct platform_device *dev)
 		ret = -EIO;
 		goto eremap;
 	}
-	i2c->reg_shift = REG_SHIFT(id->driver_data);
+
+	i2c->reg_ibmr = i2c->reg_base + pxa_reg_layout[i2c_type].ibmr;
+	i2c->reg_idbr = i2c->reg_base + pxa_reg_layout[i2c_type].idbr;
+	i2c->reg_icr = i2c->reg_base + pxa_reg_layout[i2c_type].icr;
+	i2c->reg_isr = i2c->reg_base + pxa_reg_layout[i2c_type].isr;
+	if (i2c_type != REGS_CE4100)
+		i2c->reg_isar = i2c->reg_base + pxa_reg_layout[i2c_type].isar;
 
 	i2c->iobase = res->start;
 	i2c->iosize = resource_size(res);
@@ -1072,7 +1129,7 @@ static int i2c_pxa_probe(struct platform_device *dev)
 		i2c->adap.algo = &i2c_pxa_pio_algorithm;
 	} else {
 		i2c->adap.algo = &i2c_pxa_algorithm;
-		ret = request_irq(irq, i2c_pxa_handler, IRQF_DISABLED,
+		ret = request_irq(irq, i2c_pxa_handler, IRQF_SHARED,
 				  i2c->adap.name, i2c);
 		if (ret)
 			goto ereqirq;
@@ -1082,12 +1139,19 @@ static int i2c_pxa_probe(struct platform_device *dev)
 
 	i2c->adap.algo_data = i2c;
 	i2c->adap.dev.parent = &dev->dev;
+#ifdef CONFIG_OF
+	i2c->adap.dev.of_node = dev->dev.of_node;
+#endif
 
-	ret = i2c_add_numbered_adapter(&i2c->adap);
+	if (i2c_type == REGS_CE4100)
+		ret = i2c_add_adapter(&i2c->adap);
+	else
+		ret = i2c_add_numbered_adapter(&i2c->adap);
 	if (ret < 0) {
 		printk(KERN_INFO "I2C: Failed to add bus\n");
 		goto eadapt;
 	}
+	of_i2c_register_devices(&i2c->adap);
 
 	platform_set_drvdata(dev, i2c);
 
diff --git a/drivers/i2c/busses/i2c-stu300.c b/drivers/i2c/busses/i2c-stu300.c
index 495be451d326..266135ddf7fa 100644
--- a/drivers/i2c/busses/i2c-stu300.c
+++ b/drivers/i2c/busses/i2c-stu300.c
@@ -942,7 +942,7 @@ stu300_probe(struct platform_device *pdev)
 	adap->owner = THIS_MODULE;
 	/* DDC class but actually often used for more generic I2C */
 	adap->class = I2C_CLASS_DDC;
-	strncpy(adap->name, "ST Microelectronics DDC I2C adapter",
+	strlcpy(adap->name, "ST Microelectronics DDC I2C adapter",
 		sizeof(adap->name));
 	adap->nr = bus_nr;
 	adap->algo = &stu300_algo;
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
new file mode 100644
index 000000000000..3921f664c9c3
--- /dev/null
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -0,0 +1,700 @@
+/*
+ * drivers/i2c/busses/i2c-tegra.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Colin Cross <ccross@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/i2c-tegra.h>
+
+#include <asm/unaligned.h>
+
+#include <mach/clk.h>
+
+#define TEGRA_I2C_TIMEOUT (msecs_to_jiffies(1000))
+#define BYTES_PER_FIFO_WORD 4
+
+#define I2C_CNFG				0x000
+#define I2C_CNFG_PACKET_MODE_EN			(1<<10)
+#define I2C_CNFG_NEW_MASTER_FSM			(1<<11)
+#define I2C_SL_CNFG				0x020
+#define I2C_SL_CNFG_NEWSL			(1<<2)
+#define I2C_SL_ADDR1				0x02c
+#define I2C_TX_FIFO				0x050
+#define I2C_RX_FIFO				0x054
+#define I2C_PACKET_TRANSFER_STATUS		0x058
+#define I2C_FIFO_CONTROL			0x05c
+#define I2C_FIFO_CONTROL_TX_FLUSH		(1<<1)
+#define I2C_FIFO_CONTROL_RX_FLUSH		(1<<0)
+#define I2C_FIFO_CONTROL_TX_TRIG_SHIFT		5
+#define I2C_FIFO_CONTROL_RX_TRIG_SHIFT		2
+#define I2C_FIFO_STATUS				0x060
+#define I2C_FIFO_STATUS_TX_MASK			0xF0
+#define I2C_FIFO_STATUS_TX_SHIFT		4
+#define I2C_FIFO_STATUS_RX_MASK			0x0F
+#define I2C_FIFO_STATUS_RX_SHIFT		0
+#define I2C_INT_MASK				0x064
+#define I2C_INT_STATUS				0x068
+#define I2C_INT_PACKET_XFER_COMPLETE		(1<<7)
+#define I2C_INT_ALL_PACKETS_XFER_COMPLETE	(1<<6)
+#define I2C_INT_TX_FIFO_OVERFLOW		(1<<5)
+#define I2C_INT_RX_FIFO_UNDERFLOW		(1<<4)
+#define I2C_INT_NO_ACK				(1<<3)
+#define I2C_INT_ARBITRATION_LOST		(1<<2)
+#define I2C_INT_TX_FIFO_DATA_REQ		(1<<1)
+#define I2C_INT_RX_FIFO_DATA_REQ		(1<<0)
+#define I2C_CLK_DIVISOR				0x06c
+
+#define DVC_CTRL_REG1				0x000
+#define DVC_CTRL_REG1_INTR_EN			(1<<10)
+#define DVC_CTRL_REG2				0x004
+#define DVC_CTRL_REG3				0x008
+#define DVC_CTRL_REG3_SW_PROG			(1<<26)
+#define DVC_CTRL_REG3_I2C_DONE_INTR_EN		(1<<30)
+#define DVC_STATUS				0x00c
+#define DVC_STATUS_I2C_DONE_INTR		(1<<30)
+
+#define I2C_ERR_NONE				0x00
+#define I2C_ERR_NO_ACK				0x01
+#define I2C_ERR_ARBITRATION_LOST		0x02
+
+#define PACKET_HEADER0_HEADER_SIZE_SHIFT	28
+#define PACKET_HEADER0_PACKET_ID_SHIFT		16
+#define PACKET_HEADER0_CONT_ID_SHIFT		12
+#define PACKET_HEADER0_PROTOCOL_I2C		(1<<4)
+
+#define I2C_HEADER_HIGHSPEED_MODE		(1<<22)
+#define I2C_HEADER_CONT_ON_NAK			(1<<21)
+#define I2C_HEADER_SEND_START_BYTE		(1<<20)
+#define I2C_HEADER_READ				(1<<19)
+#define I2C_HEADER_10BIT_ADDR			(1<<18)
+#define I2C_HEADER_IE_ENABLE			(1<<17)
+#define I2C_HEADER_REPEAT_START			(1<<16)
+#define I2C_HEADER_MASTER_ADDR_SHIFT		12
+#define I2C_HEADER_SLAVE_ADDR_SHIFT		1
+
+/**
+ * struct tegra_i2c_dev	- per device i2c context
+ * @dev: device reference for power management
+ * @adapter: core i2c layer adapter information
+ * @clk: clock reference for i2c controller
+ * @i2c_clk: clock reference for i2c bus
+ * @iomem: memory resource for registers
+ * @base: ioremapped registers cookie
+ * @cont_id: i2c controller id, used for for packet header
+ * @irq: irq number of transfer complete interrupt
+ * @is_dvc: identifies the DVC i2c controller, has a different register layout
+ * @msg_complete: transfer completion notifier
+ * @msg_err: error code for completed message
+ * @msg_buf: pointer to current message data
+ * @msg_buf_remaining: size of unsent data in the message buffer
+ * @msg_read: identifies read transfers
+ * @bus_clk_rate: current i2c bus clock rate
+ * @is_suspended: prevents i2c controller accesses after suspend is called
+ */
+struct tegra_i2c_dev {
+	struct device *dev;
+	struct i2c_adapter adapter;
+	struct clk *clk;
+	struct clk *i2c_clk;
+	struct resource *iomem;
+	void __iomem *base;
+	int cont_id;
+	int irq;
+	int is_dvc;
+	struct completion msg_complete;
+	int msg_err;
+	u8 *msg_buf;
+	size_t msg_buf_remaining;
+	int msg_read;
+	unsigned long bus_clk_rate;
+	bool is_suspended;
+};
+
+static void dvc_writel(struct tegra_i2c_dev *i2c_dev, u32 val, unsigned long reg)
+{
+	writel(val, i2c_dev->base + reg);
+}
+
+static u32 dvc_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
+{
+	return readl(i2c_dev->base + reg);
+}
+
+/*
+ * i2c_writel and i2c_readl will offset the register if necessary to talk
+ * to the I2C block inside the DVC block
+ */
+static unsigned long tegra_i2c_reg_addr(struct tegra_i2c_dev *i2c_dev,
+	unsigned long reg)
+{
+	if (i2c_dev->is_dvc)
+		reg += (reg >= I2C_TX_FIFO) ? 0x10 : 0x40;
+	return reg;
+}
+
+static void i2c_writel(struct tegra_i2c_dev *i2c_dev, u32 val,
+	unsigned long reg)
+{
+	writel(val, i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
+}
+
+static u32 i2c_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
+{
+	return readl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
+}
+
+static void i2c_writesl(struct tegra_i2c_dev *i2c_dev, void *data,
+	unsigned long reg, int len)
+{
+	writesl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg), data, len);
+}
+
+static void i2c_readsl(struct tegra_i2c_dev *i2c_dev, void *data,
+	unsigned long reg, int len)
+{
+	readsl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg), data, len);
+}
+
+static void tegra_i2c_mask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
+{
+	u32 int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
+	int_mask &= ~mask;
+	i2c_writel(i2c_dev, int_mask, I2C_INT_MASK);
+}
+
+static void tegra_i2c_unmask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
+{
+	u32 int_mask = i2c_readl(i2c_dev, I2C_INT_MASK);
+	int_mask |= mask;
+	i2c_writel(i2c_dev, int_mask, I2C_INT_MASK);
+}
+
+static int tegra_i2c_flush_fifos(struct tegra_i2c_dev *i2c_dev)
+{
+	unsigned long timeout = jiffies + HZ;
+	u32 val = i2c_readl(i2c_dev, I2C_FIFO_CONTROL);
+	val |= I2C_FIFO_CONTROL_TX_FLUSH | I2C_FIFO_CONTROL_RX_FLUSH;
+	i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
+
+	while (i2c_readl(i2c_dev, I2C_FIFO_CONTROL) &
+		(I2C_FIFO_CONTROL_TX_FLUSH | I2C_FIFO_CONTROL_RX_FLUSH)) {
+		if (time_after(jiffies, timeout)) {
+			dev_warn(i2c_dev->dev, "timeout waiting for fifo flush\n");
+			return -ETIMEDOUT;
+		}
+		msleep(1);
+	}
+	return 0;
+}
+
+static int tegra_i2c_empty_rx_fifo(struct tegra_i2c_dev *i2c_dev)
+{
+	u32 val;
+	int rx_fifo_avail;
+	u8 *buf = i2c_dev->msg_buf;
+	size_t buf_remaining = i2c_dev->msg_buf_remaining;
+	int words_to_transfer;
+
+	val = i2c_readl(i2c_dev, I2C_FIFO_STATUS);
+	rx_fifo_avail = (val & I2C_FIFO_STATUS_RX_MASK) >>
+		I2C_FIFO_STATUS_RX_SHIFT;
+
+	/* Rounds down to not include partial word at the end of buf */
+	words_to_transfer = buf_remaining / BYTES_PER_FIFO_WORD;
+	if (words_to_transfer > rx_fifo_avail)
+		words_to_transfer = rx_fifo_avail;
+
+	i2c_readsl(i2c_dev, buf, I2C_RX_FIFO, words_to_transfer);
+
+	buf += words_to_transfer * BYTES_PER_FIFO_WORD;
+	buf_remaining -= words_to_transfer * BYTES_PER_FIFO_WORD;
+	rx_fifo_avail -= words_to_transfer;
+
+	/*
+	 * If there is a partial word at the end of buf, handle it manually to
+	 * prevent overwriting past the end of buf
+	 */
+	if (rx_fifo_avail > 0 && buf_remaining > 0) {
+		BUG_ON(buf_remaining > 3);
+		val = i2c_readl(i2c_dev, I2C_RX_FIFO);
+		memcpy(buf, &val, buf_remaining);
+		buf_remaining = 0;
+		rx_fifo_avail--;
+	}
+
+	BUG_ON(rx_fifo_avail > 0 && buf_remaining > 0);
+	i2c_dev->msg_buf_remaining = buf_remaining;
+	i2c_dev->msg_buf = buf;
+	return 0;
+}
+
+static int tegra_i2c_fill_tx_fifo(struct tegra_i2c_dev *i2c_dev)
+{
+	u32 val;
+	int tx_fifo_avail;
+	u8 *buf = i2c_dev->msg_buf;
+	size_t buf_remaining = i2c_dev->msg_buf_remaining;
+	int words_to_transfer;
+
+	val = i2c_readl(i2c_dev, I2C_FIFO_STATUS);
+	tx_fifo_avail = (val & I2C_FIFO_STATUS_TX_MASK) >>
+		I2C_FIFO_STATUS_TX_SHIFT;
+
+	/* Rounds down to not include partial word at the end of buf */
+	words_to_transfer = buf_remaining / BYTES_PER_FIFO_WORD;
+	if (words_to_transfer > tx_fifo_avail)
+		words_to_transfer = tx_fifo_avail;
+
+	i2c_writesl(i2c_dev, buf, I2C_TX_FIFO, words_to_transfer);
+
+	buf += words_to_transfer * BYTES_PER_FIFO_WORD;
+	buf_remaining -= words_to_transfer * BYTES_PER_FIFO_WORD;
+	tx_fifo_avail -= words_to_transfer;
+
+	/*
+	 * If there is a partial word at the end of buf, handle it manually to
+	 * prevent reading past the end of buf, which could cross a page
+	 * boundary and fault.
+	 */
+	if (tx_fifo_avail > 0 && buf_remaining > 0) {
+		BUG_ON(buf_remaining > 3);
+		memcpy(&val, buf, buf_remaining);
+		i2c_writel(i2c_dev, val, I2C_TX_FIFO);
+		buf_remaining = 0;
+		tx_fifo_avail--;
+	}
+
+	BUG_ON(tx_fifo_avail > 0 && buf_remaining > 0);
+	i2c_dev->msg_buf_remaining = buf_remaining;
+	i2c_dev->msg_buf = buf;
+	return 0;
+}
+
+/*
+ * One of the Tegra I2C blocks is inside the DVC (Digital Voltage Controller)
+ * block.  This block is identical to the rest of the I2C blocks, except that
+ * it only supports master mode, it has registers moved around, and it needs
+ * some extra init to get it into I2C mode.  The register moves are handled
+ * by i2c_readl and i2c_writel
+ */
+static void tegra_dvc_init(struct tegra_i2c_dev *i2c_dev)
+{
+	u32 val = 0;
+	val = dvc_readl(i2c_dev, DVC_CTRL_REG3);
+	val |= DVC_CTRL_REG3_SW_PROG;
+	val |= DVC_CTRL_REG3_I2C_DONE_INTR_EN;
+	dvc_writel(i2c_dev, val, DVC_CTRL_REG3);
+
+	val = dvc_readl(i2c_dev, DVC_CTRL_REG1);
+	val |= DVC_CTRL_REG1_INTR_EN;
+	dvc_writel(i2c_dev, val, DVC_CTRL_REG1);
+}
+
+static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
+{
+	u32 val;
+	int err = 0;
+
+	clk_enable(i2c_dev->clk);
+
+	tegra_periph_reset_assert(i2c_dev->clk);
+	udelay(2);
+	tegra_periph_reset_deassert(i2c_dev->clk);
+
+	if (i2c_dev->is_dvc)
+		tegra_dvc_init(i2c_dev);
+
+	val = I2C_CNFG_NEW_MASTER_FSM | I2C_CNFG_PACKET_MODE_EN;
+	i2c_writel(i2c_dev, val, I2C_CNFG);
+	i2c_writel(i2c_dev, 0, I2C_INT_MASK);
+	clk_set_rate(i2c_dev->clk, i2c_dev->bus_clk_rate * 8);
+
+	val = 7 << I2C_FIFO_CONTROL_TX_TRIG_SHIFT |
+		0 << I2C_FIFO_CONTROL_RX_TRIG_SHIFT;
+	i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
+
+	if (tegra_i2c_flush_fifos(i2c_dev))
+		err = -ETIMEDOUT;
+
+	clk_disable(i2c_dev->clk);
+	return err;
+}
+
+static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
+{
+	u32 status;
+	const u32 status_err = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
+	struct tegra_i2c_dev *i2c_dev = dev_id;
+
+	status = i2c_readl(i2c_dev, I2C_INT_STATUS);
+
+	if (status == 0) {
+		dev_warn(i2c_dev->dev, "interrupt with no status\n");
+		return IRQ_NONE;
+	}
+
+	if (unlikely(status & status_err)) {
+		if (status & I2C_INT_NO_ACK)
+			i2c_dev->msg_err |= I2C_ERR_NO_ACK;
+		if (status & I2C_INT_ARBITRATION_LOST)
+			i2c_dev->msg_err |= I2C_ERR_ARBITRATION_LOST;
+		complete(&i2c_dev->msg_complete);
+		goto err;
+	}
+
+	if (i2c_dev->msg_read && (status & I2C_INT_RX_FIFO_DATA_REQ)) {
+		if (i2c_dev->msg_buf_remaining)
+			tegra_i2c_empty_rx_fifo(i2c_dev);
+		else
+			BUG();
+	}
+
+	if (!i2c_dev->msg_read && (status & I2C_INT_TX_FIFO_DATA_REQ)) {
+		if (i2c_dev->msg_buf_remaining)
+			tegra_i2c_fill_tx_fifo(i2c_dev);
+		else
+			tegra_i2c_mask_irq(i2c_dev, I2C_INT_TX_FIFO_DATA_REQ);
+	}
+
+	if ((status & I2C_INT_PACKET_XFER_COMPLETE) &&
+			!i2c_dev->msg_buf_remaining)
+		complete(&i2c_dev->msg_complete);
+
+	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+	if (i2c_dev->is_dvc)
+		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+	return IRQ_HANDLED;
+err:
+	/* An error occured, mask all interrupts */
+	tegra_i2c_mask_irq(i2c_dev, I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST |
+		I2C_INT_PACKET_XFER_COMPLETE | I2C_INT_TX_FIFO_DATA_REQ |
+		I2C_INT_RX_FIFO_DATA_REQ);
+	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+	return IRQ_HANDLED;
+}
+
+static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
+	struct i2c_msg *msg, int stop)
+{
+	u32 packet_header;
+	u32 int_mask;
+	int ret;
+
+	tegra_i2c_flush_fifos(i2c_dev);
+	i2c_writel(i2c_dev, 0xFF, I2C_INT_STATUS);
+
+	if (msg->len == 0)
+		return -EINVAL;
+
+	i2c_dev->msg_buf = msg->buf;
+	i2c_dev->msg_buf_remaining = msg->len;
+	i2c_dev->msg_err = I2C_ERR_NONE;
+	i2c_dev->msg_read = (msg->flags & I2C_M_RD);
+	INIT_COMPLETION(i2c_dev->msg_complete);
+
+	packet_header = (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT) |
+			PACKET_HEADER0_PROTOCOL_I2C |
+			(i2c_dev->cont_id << PACKET_HEADER0_CONT_ID_SHIFT) |
+			(1 << PACKET_HEADER0_PACKET_ID_SHIFT);
+	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
+
+	packet_header = msg->len - 1;
+	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
+
+	packet_header = msg->addr << I2C_HEADER_SLAVE_ADDR_SHIFT;
+	packet_header |= I2C_HEADER_IE_ENABLE;
+	if (msg->flags & I2C_M_TEN)
+		packet_header |= I2C_HEADER_10BIT_ADDR;
+	if (msg->flags & I2C_M_IGNORE_NAK)
+		packet_header |= I2C_HEADER_CONT_ON_NAK;
+	if (msg->flags & I2C_M_NOSTART)
+		packet_header |= I2C_HEADER_REPEAT_START;
+	if (msg->flags & I2C_M_RD)
+		packet_header |= I2C_HEADER_READ;
+	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
+
+	if (!(msg->flags & I2C_M_RD))
+		tegra_i2c_fill_tx_fifo(i2c_dev);
+
+	int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
+	if (msg->flags & I2C_M_RD)
+		int_mask |= I2C_INT_RX_FIFO_DATA_REQ;
+	else if (i2c_dev->msg_buf_remaining)
+		int_mask |= I2C_INT_TX_FIFO_DATA_REQ;
+	tegra_i2c_unmask_irq(i2c_dev, int_mask);
+	dev_dbg(i2c_dev->dev, "unmasked irq: %02x\n",
+		i2c_readl(i2c_dev, I2C_INT_MASK));
+
+	ret = wait_for_completion_timeout(&i2c_dev->msg_complete, TEGRA_I2C_TIMEOUT);
+	tegra_i2c_mask_irq(i2c_dev, int_mask);
+
+	if (WARN_ON(ret == 0)) {
+		dev_err(i2c_dev->dev, "i2c transfer timed out\n");
+
+		tegra_i2c_init(i2c_dev);
+		return -ETIMEDOUT;
+	}
+
+	dev_dbg(i2c_dev->dev, "transfer complete: %d %d %d\n",
+		ret, completion_done(&i2c_dev->msg_complete), i2c_dev->msg_err);
+
+	if (likely(i2c_dev->msg_err == I2C_ERR_NONE))
+		return 0;
+
+	tegra_i2c_init(i2c_dev);
+	if (i2c_dev->msg_err == I2C_ERR_NO_ACK) {
+		if (msg->flags & I2C_M_IGNORE_NAK)
+			return 0;
+		return -EREMOTEIO;
+	}
+
+	return -EIO;
+}
+
+static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
+	int num)
+{
+	struct tegra_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
+	int i;
+	int ret = 0;
+
+	if (i2c_dev->is_suspended)
+		return -EBUSY;
+
+	clk_enable(i2c_dev->clk);
+	for (i = 0; i < num; i++) {
+		int stop = (i == (num - 1)) ? 1  : 0;
+		ret = tegra_i2c_xfer_msg(i2c_dev, &msgs[i], stop);
+		if (ret)
+			break;
+	}
+	clk_disable(i2c_dev->clk);
+	return ret ?: i;
+}
+
+static u32 tegra_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C;
+}
+
+static const struct i2c_algorithm tegra_i2c_algo = {
+	.master_xfer	= tegra_i2c_xfer,
+	.functionality	= tegra_i2c_func,
+};
+
+static int tegra_i2c_probe(struct platform_device *pdev)
+{
+	struct tegra_i2c_dev *i2c_dev;
+	struct tegra_i2c_platform_data *pdata = pdev->dev.platform_data;
+	struct resource *res;
+	struct resource *iomem;
+	struct clk *clk;
+	struct clk *i2c_clk;
+	void *base;
+	int irq;
+	int ret = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no mem resource\n");
+		return -EINVAL;
+	}
+	iomem = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!iomem) {
+		dev_err(&pdev->dev, "I2C region already claimed\n");
+		return -EBUSY;
+	}
+
+	base = ioremap(iomem->start, resource_size(iomem));
+	if (!base) {
+		dev_err(&pdev->dev, "Cannot ioremap I2C region\n");
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no irq resource\n");
+		ret = -EINVAL;
+		goto err_iounmap;
+	}
+	irq = res->start;
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "missing controller clock");
+		ret = PTR_ERR(clk);
+		goto err_release_region;
+	}
+
+	i2c_clk = clk_get(&pdev->dev, "i2c");
+	if (IS_ERR(i2c_clk)) {
+		dev_err(&pdev->dev, "missing bus clock");
+		ret = PTR_ERR(i2c_clk);
+		goto err_clk_put;
+	}
+
+	i2c_dev = kzalloc(sizeof(struct tegra_i2c_dev), GFP_KERNEL);
+	if (!i2c_dev) {
+		ret = -ENOMEM;
+		goto err_i2c_clk_put;
+	}
+
+	i2c_dev->base = base;
+	i2c_dev->clk = clk;
+	i2c_dev->i2c_clk = i2c_clk;
+	i2c_dev->iomem = iomem;
+	i2c_dev->adapter.algo = &tegra_i2c_algo;
+	i2c_dev->irq = irq;
+	i2c_dev->cont_id = pdev->id;
+	i2c_dev->dev = &pdev->dev;
+	i2c_dev->bus_clk_rate = pdata ? pdata->bus_clk_rate : 100000;
+
+	if (pdev->id == 3)
+		i2c_dev->is_dvc = 1;
+	init_completion(&i2c_dev->msg_complete);
+
+	platform_set_drvdata(pdev, i2c_dev);
+
+	ret = tegra_i2c_init(i2c_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to initialize i2c controller");
+		goto err_free;
+	}
+
+	ret = request_irq(i2c_dev->irq, tegra_i2c_isr, 0, pdev->name, i2c_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request irq %i\n", i2c_dev->irq);
+		goto err_free;
+	}
+
+	clk_enable(i2c_dev->i2c_clk);
+
+	i2c_set_adapdata(&i2c_dev->adapter, i2c_dev);
+	i2c_dev->adapter.owner = THIS_MODULE;
+	i2c_dev->adapter.class = I2C_CLASS_HWMON;
+	strlcpy(i2c_dev->adapter.name, "Tegra I2C adapter",
+		sizeof(i2c_dev->adapter.name));
+	i2c_dev->adapter.algo = &tegra_i2c_algo;
+	i2c_dev->adapter.dev.parent = &pdev->dev;
+	i2c_dev->adapter.nr = pdev->id;
+
+	ret = i2c_add_numbered_adapter(&i2c_dev->adapter);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to add I2C adapter\n");
+		goto err_free_irq;
+	}
+
+	return 0;
+err_free_irq:
+	free_irq(i2c_dev->irq, i2c_dev);
+err_free:
+	kfree(i2c_dev);
+err_i2c_clk_put:
+	clk_put(i2c_clk);
+err_clk_put:
+	clk_put(clk);
+err_release_region:
+	release_mem_region(iomem->start, resource_size(iomem));
+err_iounmap:
+	iounmap(base);
+	return ret;
+}
+
+static int tegra_i2c_remove(struct platform_device *pdev)
+{
+	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+	i2c_del_adapter(&i2c_dev->adapter);
+	free_irq(i2c_dev->irq, i2c_dev);
+	clk_put(i2c_dev->i2c_clk);
+	clk_put(i2c_dev->clk);
+	release_mem_region(i2c_dev->iomem->start,
+		resource_size(i2c_dev->iomem));
+	iounmap(i2c_dev->base);
+	kfree(i2c_dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_i2c_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+
+	i2c_lock_adapter(&i2c_dev->adapter);
+	i2c_dev->is_suspended = true;
+	i2c_unlock_adapter(&i2c_dev->adapter);
+
+	return 0;
+}
+
+static int tegra_i2c_resume(struct platform_device *pdev)
+{
+	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+	int ret;
+
+	i2c_lock_adapter(&i2c_dev->adapter);
+
+	ret = tegra_i2c_init(i2c_dev);
+
+	if (ret) {
+		i2c_unlock_adapter(&i2c_dev->adapter);
+		return ret;
+	}
+
+	i2c_dev->is_suspended = false;
+
+	i2c_unlock_adapter(&i2c_dev->adapter);
+
+	return 0;
+}
+#endif
+
+static struct platform_driver tegra_i2c_driver = {
+	.probe   = tegra_i2c_probe,
+	.remove  = tegra_i2c_remove,
+#ifdef CONFIG_PM
+	.suspend = tegra_i2c_suspend,
+	.resume  = tegra_i2c_resume,
+#endif
+	.driver  = {
+		.name  = "tegra-i2c",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init tegra_i2c_init_driver(void)
+{
+	return platform_driver_register(&tegra_i2c_driver);
+}
+
+static void __exit tegra_i2c_exit_driver(void)
+{
+	platform_driver_unregister(&tegra_i2c_driver);
+}
+
+subsys_initcall(tegra_i2c_init_driver);
+module_exit(tegra_i2c_exit_driver);
+
+MODULE_DESCRIPTION("nVidia Tegra2 I2C Bus Controller driver");
+MODULE_AUTHOR("Colin Cross");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index a9c419e075a5..9fbd7e6fe32e 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -34,6 +34,7 @@
 #include <linux/errno.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/wait.h>
@@ -704,7 +705,7 @@ static int __devinit xiic_i2c_probe(struct platform_device *pdev)
 	if (irq < 0)
 		goto resource_missing;
 
-	pdata = (struct xiic_i2c_platform_data *) pdev->dev.platform_data;
+	pdata = mfd_get_data(pdev);
 	if (!pdata)
 		return -EINVAL;
 
diff --git a/drivers/i2c/i2c-boardinfo.c b/drivers/i2c/i2c-boardinfo.c
index 7e6a63b57165..3ca2e012e789 100644
--- a/drivers/i2c/i2c-boardinfo.c
+++ b/drivers/i2c/i2c-boardinfo.c
@@ -1,5 +1,5 @@
 /*
- * i2c-boardinfo.h - collect pre-declarations of I2C devices
+ * i2c-boardinfo.c - collect pre-declarations of I2C devices
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index f0bd5bcdf563..e5f76a0372fd 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -537,9 +537,7 @@ i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info)
 	client->dev.parent = &client->adapter->dev;
 	client->dev.bus = &i2c_bus_type;
 	client->dev.type = &i2c_client_type;
-#ifdef CONFIG_OF
 	client->dev.of_node = info->of_node;
-#endif
 
 	dev_set_name(&client->dev, "%d-%04x", i2c_adapter_id(adap),
 		     client->addr);
@@ -799,6 +797,9 @@ static int i2c_do_add_adapter(struct i2c_driver *driver,
 
 	/* Let legacy drivers scan this bus for matching devices */
 	if (driver->attach_adapter) {
+		dev_warn(&adap->dev, "attach_adapter method is deprecated\n");
+		dev_warn(&adap->dev, "Please use another way to instantiate "
+			 "your i2c_client\n");
 		/* We ignore the return code; if it fails, too bad */
 		driver->attach_adapter(adap);
 	}
@@ -983,6 +984,7 @@ static int i2c_do_del_adapter(struct i2c_driver *driver,
 
 	if (!driver->detach_adapter)
 		return 0;
+	dev_warn(&adapter->dev, "detach_adapter method is deprecated\n");
 	res = driver->detach_adapter(adapter);
 	if (res)
 		dev_err(&adapter->dev, "detach_adapter failed (%d) "
@@ -1093,6 +1095,18 @@ EXPORT_SYMBOL(i2c_del_adapter);
 
 /* ------------------------------------------------------------------------- */
 
+int i2c_for_each_dev(void *data, int (*fn)(struct device *, void *))
+{
+	int res;
+
+	mutex_lock(&core_lock);
+	res = bus_for_each_dev(&i2c_bus_type, NULL, data, fn);
+	mutex_unlock(&core_lock);
+
+	return res;
+}
+EXPORT_SYMBOL_GPL(i2c_for_each_dev);
+
 static int __process_new_driver(struct device *dev, void *data)
 {
 	if (dev->type != &i2c_adapter_type)
@@ -1136,9 +1150,7 @@ int i2c_register_driver(struct module *owner, struct i2c_driver *driver)
 
 	INIT_LIST_HEAD(&driver->clients);
 	/* Walk the adapters that are already present */
-	mutex_lock(&core_lock);
-	bus_for_each_dev(&i2c_bus_type, NULL, driver, __process_new_driver);
-	mutex_unlock(&core_lock);
+	i2c_for_each_dev(driver, __process_new_driver);
 
 	return 0;
 }
@@ -1158,9 +1170,7 @@ static int __process_removed_driver(struct device *dev, void *data)
  */
 void i2c_del_driver(struct i2c_driver *driver)
 {
-	mutex_lock(&core_lock);
-	bus_for_each_dev(&i2c_bus_type, NULL, driver, __process_removed_driver);
-	mutex_unlock(&core_lock);
+	i2c_for_each_dev(driver, __process_removed_driver);
 
 	driver_unregister(&driver->driver);
 	pr_debug("i2c-core: driver [%s] unregistered\n", driver->driver.name);
@@ -1583,12 +1593,12 @@ i2c_new_probed_device(struct i2c_adapter *adap,
 }
 EXPORT_SYMBOL_GPL(i2c_new_probed_device);
 
-struct i2c_adapter *i2c_get_adapter(int id)
+struct i2c_adapter *i2c_get_adapter(int nr)
 {
 	struct i2c_adapter *adapter;
 
 	mutex_lock(&core_lock);
-	adapter = idr_find(&i2c_adapter_idr, id);
+	adapter = idr_find(&i2c_adapter_idr, nr);
 	if (adapter && !try_module_get(adapter->owner))
 		adapter = NULL;
 
diff --git a/drivers/i2c/i2c-dev.c b/drivers/i2c/i2c-dev.c
index cec0f3ba97f8..66ac71800a06 100644
--- a/drivers/i2c/i2c-dev.c
+++ b/drivers/i2c/i2c-dev.c
@@ -28,6 +28,8 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/device.h>
+#include <linux/notifier.h>
 #include <linux/fs.h>
 #include <linux/slab.h>
 #include <linux/init.h>
@@ -37,16 +39,13 @@
 #include <linux/jiffies.h>
 #include <linux/uaccess.h>
 
-static struct i2c_driver i2cdev_driver;
-
 /*
  * An i2c_dev represents an i2c_adapter ... an I2C or SMBus master, not a
  * slave (i2c_client) with which messages will be exchanged.  It's coupled
  * with a character special file which is accessed by user mode drivers.
  *
  * The list of i2c_dev structures is parallel to the i2c_adapter lists
- * maintained by the driver model, and is updated using notifications
- * delivered to the i2cdev_driver.
+ * maintained by the driver model, and is updated using bus notifications.
  */
 struct i2c_dev {
 	struct list_head list;
@@ -491,7 +490,6 @@ static int i2cdev_open(struct inode *inode, struct file *file)
 		return -ENOMEM;
 	}
 	snprintf(client->name, I2C_NAME_SIZE, "i2c-dev %d", adap->nr);
-	client->driver = &i2cdev_driver;
 
 	client->adapter = adap;
 	file->private_data = client;
@@ -522,19 +520,18 @@ static const struct file_operations i2cdev_fops = {
 
 /* ------------------------------------------------------------------------- */
 
-/*
- * The legacy "i2cdev_driver" is used primarily to get notifications when
- * I2C adapters are added or removed, so that each one gets an i2c_dev
- * and is thus made available to userspace driver code.
- */
-
 static struct class *i2c_dev_class;
 
-static int i2cdev_attach_adapter(struct i2c_adapter *adap)
+static int i2cdev_attach_adapter(struct device *dev, void *dummy)
 {
+	struct i2c_adapter *adap;
 	struct i2c_dev *i2c_dev;
 	int res;
 
+	if (dev->type != &i2c_adapter_type)
+		return 0;
+	adap = to_i2c_adapter(dev);
+
 	i2c_dev = get_free_i2c_dev(adap);
 	if (IS_ERR(i2c_dev))
 		return PTR_ERR(i2c_dev);
@@ -561,10 +558,15 @@ error:
 	return res;
 }
 
-static int i2cdev_detach_adapter(struct i2c_adapter *adap)
+static int i2cdev_detach_adapter(struct device *dev, void *dummy)
 {
+	struct i2c_adapter *adap;
 	struct i2c_dev *i2c_dev;
 
+	if (dev->type != &i2c_adapter_type)
+		return 0;
+	adap = to_i2c_adapter(dev);
+
 	i2c_dev = i2c_dev_get_by_minor(adap->nr);
 	if (!i2c_dev) /* attach_adapter must have failed */
 		return 0;
@@ -577,12 +579,23 @@ static int i2cdev_detach_adapter(struct i2c_adapter *adap)
 	return 0;
 }
 
-static struct i2c_driver i2cdev_driver = {
-	.driver = {
-		.name	= "dev_driver",
-	},
-	.attach_adapter	= i2cdev_attach_adapter,
-	.detach_adapter	= i2cdev_detach_adapter,
+int i2cdev_notifier_call(struct notifier_block *nb, unsigned long action,
+			 void *data)
+{
+	struct device *dev = data;
+
+	switch (action) {
+	case BUS_NOTIFY_ADD_DEVICE:
+		return i2cdev_attach_adapter(dev, NULL);
+	case BUS_NOTIFY_DEL_DEVICE:
+		return i2cdev_detach_adapter(dev, NULL);
+	}
+
+	return 0;
+}
+
+static struct notifier_block i2cdev_notifier = {
+	.notifier_call = i2cdev_notifier_call,
 };
 
 /* ------------------------------------------------------------------------- */
@@ -607,10 +620,14 @@ static int __init i2c_dev_init(void)
 		goto out_unreg_chrdev;
 	}
 
-	res = i2c_add_driver(&i2cdev_driver);
+	/* Keep track of adapters which will be added or removed later */
+	res = bus_register_notifier(&i2c_bus_type, &i2cdev_notifier);
 	if (res)
 		goto out_unreg_class;
 
+	/* Bind to already existing adapters right away */
+	i2c_for_each_dev(NULL, i2cdev_attach_adapter);
+
 	return 0;
 
 out_unreg_class:
@@ -624,7 +641,8 @@ out:
 
 static void __exit i2c_dev_exit(void)
 {
-	i2c_del_driver(&i2cdev_driver);
+	i2c_for_each_dev(NULL, i2cdev_detach_adapter);
+	bus_unregister_notifier(&i2c_bus_type, &i2cdev_notifier);
 	class_destroy(i2c_dev_class);
 	unregister_chrdev(I2C_MAJOR, "i2c");
 }
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index 1fa091e05690..a46dddf61078 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -62,6 +62,7 @@
 #include <linux/notifier.h>
 #include <linux/cpu.h>
 #include <asm/mwait.h>
+#include <asm/msr.h>
 
 #define INTEL_IDLE_VERSION "0.4"
 #define PREFIX "intel_idle: "
@@ -85,6 +86,12 @@ static int intel_idle(struct cpuidle_device *dev, struct cpuidle_state *state);
 static struct cpuidle_state *cpuidle_state_table;
 
 /*
+ * Hardware C-state auto-demotion may not always be optimal.
+ * Indicate which enable bits to clear here.
+ */
+static unsigned long long auto_demotion_disable_flags;
+
+/*
  * Set this flag for states where the HW flushes the TLB for us
  * and so we don't need cross-calls to keep it consistent.
  * If this flag is set, SW flushes the TLB, so even if the
@@ -100,7 +107,7 @@ static struct cpuidle_state *cpuidle_state_table;
 static struct cpuidle_state nehalem_cstates[MWAIT_MAX_NUM_CSTATES] = {
 	{ /* MWAIT C0 */ },
 	{ /* MWAIT C1 */
-		.name = "NHM-C1",
+		.name = "C1-NHM",
 		.desc = "MWAIT 0x00",
 		.driver_data = (void *) 0x00,
 		.flags = CPUIDLE_FLAG_TIME_VALID,
@@ -108,7 +115,7 @@ static struct cpuidle_state nehalem_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.target_residency = 6,
 		.enter = &intel_idle },
 	{ /* MWAIT C2 */
-		.name = "NHM-C3",
+		.name = "C3-NHM",
 		.desc = "MWAIT 0x10",
 		.driver_data = (void *) 0x10,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -116,7 +123,7 @@ static struct cpuidle_state nehalem_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.target_residency = 80,
 		.enter = &intel_idle },
 	{ /* MWAIT C3 */
-		.name = "NHM-C6",
+		.name = "C6-NHM",
 		.desc = "MWAIT 0x20",
 		.driver_data = (void *) 0x20,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -128,7 +135,7 @@ static struct cpuidle_state nehalem_cstates[MWAIT_MAX_NUM_CSTATES] = {
 static struct cpuidle_state snb_cstates[MWAIT_MAX_NUM_CSTATES] = {
 	{ /* MWAIT C0 */ },
 	{ /* MWAIT C1 */
-		.name = "SNB-C1",
+		.name = "C1-SNB",
 		.desc = "MWAIT 0x00",
 		.driver_data = (void *) 0x00,
 		.flags = CPUIDLE_FLAG_TIME_VALID,
@@ -136,7 +143,7 @@ static struct cpuidle_state snb_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.target_residency = 1,
 		.enter = &intel_idle },
 	{ /* MWAIT C2 */
-		.name = "SNB-C3",
+		.name = "C3-SNB",
 		.desc = "MWAIT 0x10",
 		.driver_data = (void *) 0x10,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -144,7 +151,7 @@ static struct cpuidle_state snb_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.target_residency = 211,
 		.enter = &intel_idle },
 	{ /* MWAIT C3 */
-		.name = "SNB-C6",
+		.name = "C6-SNB",
 		.desc = "MWAIT 0x20",
 		.driver_data = (void *) 0x20,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -152,7 +159,7 @@ static struct cpuidle_state snb_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.target_residency = 345,
 		.enter = &intel_idle },
 	{ /* MWAIT C4 */
-		.name = "SNB-C7",
+		.name = "C7-SNB",
 		.desc = "MWAIT 0x30",
 		.driver_data = (void *) 0x30,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -164,7 +171,7 @@ static struct cpuidle_state snb_cstates[MWAIT_MAX_NUM_CSTATES] = {
 static struct cpuidle_state atom_cstates[MWAIT_MAX_NUM_CSTATES] = {
 	{ /* MWAIT C0 */ },
 	{ /* MWAIT C1 */
-		.name = "ATM-C1",
+		.name = "C1-ATM",
 		.desc = "MWAIT 0x00",
 		.driver_data = (void *) 0x00,
 		.flags = CPUIDLE_FLAG_TIME_VALID,
@@ -172,7 +179,7 @@ static struct cpuidle_state atom_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.target_residency = 4,
 		.enter = &intel_idle },
 	{ /* MWAIT C2 */
-		.name = "ATM-C2",
+		.name = "C2-ATM",
 		.desc = "MWAIT 0x10",
 		.driver_data = (void *) 0x10,
 		.flags = CPUIDLE_FLAG_TIME_VALID,
@@ -181,7 +188,7 @@ static struct cpuidle_state atom_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.enter = &intel_idle },
 	{ /* MWAIT C3 */ },
 	{ /* MWAIT C4 */
-		.name = "ATM-C4",
+		.name = "C4-ATM",
 		.desc = "MWAIT 0x30",
 		.driver_data = (void *) 0x30,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -190,7 +197,7 @@ static struct cpuidle_state atom_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.enter = &intel_idle },
 	{ /* MWAIT C5 */ },
 	{ /* MWAIT C6 */
-		.name = "ATM-C6",
+		.name = "C6-ATM",
 		.desc = "MWAIT 0x52",
 		.driver_data = (void *) 0x52,
 		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
@@ -281,6 +288,15 @@ static struct notifier_block setup_broadcast_notifier = {
 	.notifier_call = setup_broadcast_cpuhp_notify,
 };
 
+static void auto_demotion_disable(void *dummy)
+{
+	unsigned long long msr_bits;
+
+	rdmsrl(MSR_NHM_SNB_PKG_CST_CFG_CTL, msr_bits);
+	msr_bits &= ~auto_demotion_disable_flags;
+	wrmsrl(MSR_NHM_SNB_PKG_CST_CFG_CTL, msr_bits);
+}
+
 /*
  * intel_idle_probe()
  */
@@ -324,11 +340,17 @@ static int intel_idle_probe(void)
 	case 0x25:	/* Westmere */
 	case 0x2C:	/* Westmere */
 		cpuidle_state_table = nehalem_cstates;
+		auto_demotion_disable_flags =
+			(NHM_C1_AUTO_DEMOTE | NHM_C3_AUTO_DEMOTE);
 		break;
 
 	case 0x1C:	/* 28 - Atom Processor */
+		cpuidle_state_table = atom_cstates;
+		break;
+
 	case 0x26:	/* 38 - Lincroft Atom Processor */
 		cpuidle_state_table = atom_cstates;
+		auto_demotion_disable_flags = ATM_LNC_C6_AUTO_DEMOTE;
 		break;
 
 	case 0x2A:	/* SNB */
@@ -436,6 +458,8 @@ static int intel_idle_cpuidle_devices_init(void)
 			return -EIO;
 		}
 	}
+	if (auto_demotion_disable_flags)
+		smp_call_function(auto_demotion_disable, NULL, 1);
 
 	return 0;
 }
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 8aba0ba57de5..2d749937a969 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -193,10 +193,11 @@ static int addr4_resolve(struct sockaddr_in *src_in,
 	fl.nl_u.ip4_u.saddr = src_ip;
 	fl.oif = addr->bound_dev_if;
 
-	ret = ip_route_output_key(&init_net, &rt, &fl);
-	if (ret)
+	rt = ip_route_output_key(&init_net, &fl);
+	if (IS_ERR(rt)) {
+		ret = PTR_ERR(rt);
 		goto out;
-
+	}
 	src_in->sin_family = AF_INET;
 	src_in->sin_addr.s_addr = rt->rt_src;
 
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index d02dcc6e5963..e0ccbc53fbcc 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -354,7 +354,8 @@ static struct rtable *find_route(struct t3cdev *dev, __be32 local_ip,
 			  }
 	};
 
-	if (ip_route_output_flow(&init_net, &rt, &fl, NULL, 0))
+	rt = ip_route_output_flow(&init_net, &fl, NULL);
+	if (IS_ERR(rt))
 		return NULL;
 	return rt;
 }
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 8b00e6c46f01..77b0eef2aad9 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -331,7 +331,8 @@ static struct rtable *find_route(struct c4iw_dev *dev, __be32 local_ip,
 			  }
 	};
 
-	if (ip_route_output_flow(&init_net, &rt, &fl, NULL, 0))
+	rt = ip_route_output_flow(&init_net, &fl, NULL);
+	if (IS_ERR(rt))
 		return NULL;
 	return rt;
 }
diff --git a/drivers/infiniband/hw/ipath/ipath_user_pages.c b/drivers/infiniband/hw/ipath/ipath_user_pages.c
index bab9f74c0665..cfed5399f074 100644
--- a/drivers/infiniband/hw/ipath/ipath_user_pages.c
+++ b/drivers/infiniband/hw/ipath/ipath_user_pages.c
@@ -53,8 +53,8 @@ static void __ipath_release_user_pages(struct page **p, size_t num_pages,
 }
 
 /* call with current->mm->mmap_sem held */
-static int __get_user_pages(unsigned long start_page, size_t num_pages,
-			struct page **p, struct vm_area_struct **vma)
+static int __ipath_get_user_pages(unsigned long start_page, size_t num_pages,
+				  struct page **p, struct vm_area_struct **vma)
 {
 	unsigned long lock_limit;
 	size_t got;
@@ -165,7 +165,7 @@ int ipath_get_user_pages(unsigned long start_page, size_t num_pages,
 
 	down_write(&current->mm->mmap_sem);
 
-	ret = __get_user_pages(start_page, num_pages, p, NULL);
+	ret = __ipath_get_user_pages(start_page, num_pages, p, NULL);
 
 	up_write(&current->mm->mmap_sem);
 
diff --git a/drivers/infiniband/hw/nes/nes.c b/drivers/infiniband/hw/nes/nes.c
index 3b4ec3238ceb..3d7f3664b67b 100644
--- a/drivers/infiniband/hw/nes/nes.c
+++ b/drivers/infiniband/hw/nes/nes.c
@@ -153,7 +153,8 @@ static int nes_inetaddr_event(struct notifier_block *notifier,
 				nesdev, nesdev->netdev[0]->name);
 		netdev = nesdev->netdev[0];
 		nesvnic = netdev_priv(netdev);
-		is_bonded = (netdev->master == event_netdev);
+		is_bonded = netif_is_bond_slave(netdev) &&
+			    (netdev->master == event_netdev);
 		if ((netdev == event_netdev) || is_bonded) {
 			if (nesvnic->rdma_enabled == 0) {
 				nes_debug(NES_DBG_NETDEV, "Returning without processing event for %s since"
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 009ec814d517..e81599cb1fe6 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -1112,13 +1112,14 @@ static int nes_addr_resolve_neigh(struct nes_vnic *nesvnic, u32 dst_ip, int arpi
 
 	memset(&fl, 0, sizeof fl);
 	fl.nl_u.ip4_u.daddr = htonl(dst_ip);
-	if (ip_route_output_key(&init_net, &rt, &fl)) {
+	rt = ip_route_output_key(&init_net, &fl);
+	if (IS_ERR(rt)) {
 		printk(KERN_ERR "%s: ip_route_output_key failed for 0x%08X\n",
 				__func__, dst_ip);
 		return rc;
 	}
 
-	if (nesvnic->netdev->master)
+	if (netif_is_bond_slave(netdev))
 		netdev = nesvnic->netdev->master;
 	else
 		netdev = nesvnic->netdev;
diff --git a/drivers/infiniband/hw/qib/qib_mad.c b/drivers/infiniband/hw/qib/qib_mad.c
index 5ad224e4a38b..4b9e11cc561b 100644
--- a/drivers/infiniband/hw/qib/qib_mad.c
+++ b/drivers/infiniband/hw/qib/qib_mad.c
@@ -705,7 +705,7 @@ static int subn_set_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 	lwe = pip->link_width_enabled;
 	if (lwe) {
 		if (lwe == 0xFF)
-			lwe = ppd->link_width_supported;
+			set_link_width_enabled(ppd, ppd->link_width_supported);
 		else if (lwe >= 16 || (lwe & ~ppd->link_width_supported))
 			smp->status |= IB_SMP_INVALID_FIELD;
 		else if (lwe != ppd->link_width_enabled)
@@ -720,7 +720,8 @@ static int subn_set_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 		 * speeds.
 		 */
 		if (lse == 15)
-			lse = ppd->link_speed_supported;
+			set_link_speed_enabled(ppd,
+					       ppd->link_speed_supported);
 		else if (lse >= 8 || (lse & ~ppd->link_speed_supported))
 			smp->status |= IB_SMP_INVALID_FIELD;
 		else if (lse != ppd->link_speed_enabled)
@@ -849,7 +850,7 @@ static int subn_set_portinfo(struct ib_smp *smp, struct ib_device *ibdev,
 	if (clientrereg)
 		pip->clientrereg_resv_subnetto |= 0x80;
 
-	goto done;
+	goto get_only;
 
 err:
 	smp->status |= IB_SMP_INVALID_FIELD;
diff --git a/drivers/infiniband/hw/qib/qib_user_pages.c b/drivers/infiniband/hw/qib/qib_user_pages.c
index d7a26c1d4f37..7689e49c13c9 100644
--- a/drivers/infiniband/hw/qib/qib_user_pages.c
+++ b/drivers/infiniband/hw/qib/qib_user_pages.c
@@ -51,8 +51,8 @@ static void __qib_release_user_pages(struct page **p, size_t num_pages,
 /*
  * Call with current->mm->mmap_sem held.
  */
-static int __get_user_pages(unsigned long start_page, size_t num_pages,
-			    struct page **p, struct vm_area_struct **vma)
+static int __qib_get_user_pages(unsigned long start_page, size_t num_pages,
+				struct page **p, struct vm_area_struct **vma)
 {
 	unsigned long lock_limit;
 	size_t got;
@@ -136,7 +136,7 @@ int qib_get_user_pages(unsigned long start_page, size_t num_pages,
 
 	down_write(&current->mm->mmap_sem);
 
-	ret = __get_user_pages(start_page, num_pages, p, NULL);
+	ret = __qib_get_user_pages(start_page, num_pages, p, NULL);
 
 	up_write(&current->mm->mmap_sem);
 
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index 7b2fc98e2f2b..8db008de5392 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -532,6 +532,29 @@ iscsi_iser_conn_get_stats(struct iscsi_cls_conn *cls_conn, struct iscsi_stats *s
 	stats->custom[3].value = conn->fmr_unalign_cnt;
 }
 
+static int iscsi_iser_get_ep_param(struct iscsi_endpoint *ep,
+				   enum iscsi_param param, char *buf)
+{
+	struct iser_conn *ib_conn = ep->dd_data;
+	int len;
+
+	switch (param) {
+	case ISCSI_PARAM_CONN_PORT:
+	case ISCSI_PARAM_CONN_ADDRESS:
+		if (!ib_conn || !ib_conn->cma_id)
+			return -ENOTCONN;
+
+		return iscsi_conn_get_addr_param((struct sockaddr_storage *)
+					&ib_conn->cma_id->route.addr.dst_addr,
+					param, buf);
+		break;
+	default:
+		return -ENOSYS;
+	}
+
+	return len;
+}
+
 static struct iscsi_endpoint *
 iscsi_iser_ep_connect(struct Scsi_Host *shost, struct sockaddr *dst_addr,
 		      int non_blocking)
@@ -637,6 +660,8 @@ static struct iscsi_transport iscsi_iser_transport = {
 				  ISCSI_MAX_BURST |
 				  ISCSI_PDU_INORDER_EN |
 				  ISCSI_DATASEQ_INORDER_EN |
+				  ISCSI_CONN_PORT |
+				  ISCSI_CONN_ADDRESS |
 				  ISCSI_EXP_STATSN |
 				  ISCSI_PERSISTENT_PORT |
 				  ISCSI_PERSISTENT_ADDRESS |
@@ -659,6 +684,7 @@ static struct iscsi_transport iscsi_iser_transport = {
 	.destroy_conn           = iscsi_iser_conn_destroy,
 	.set_param              = iscsi_iser_set_param,
 	.get_conn_param		= iscsi_conn_get_param,
+	.get_ep_param		= iscsi_iser_get_ep_param,
 	.get_session_param	= iscsi_session_get_param,
 	.start_conn             = iscsi_iser_conn_start,
 	.stop_conn              = iscsi_iser_conn_stop,
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index c8471a2552e7..7f42d3a454d2 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -321,6 +321,9 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 	struct input_event event;
 	int retval;
 
+	if (count < input_event_size())
+		return -EINVAL;
+
 	retval = mutex_lock_interruptible(&evdev->mutex);
 	if (retval)
 		return retval;
@@ -330,17 +333,16 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 		goto out;
 	}
 
-	while (retval < count) {
-
+	do {
 		if (input_event_from_user(buffer + retval, &event)) {
 			retval = -EFAULT;
 			goto out;
 		}
+		retval += input_event_size();
 
 		input_inject_event(&evdev->handle,
 				   event.type, event.code, event.value);
-		retval += input_event_size();
-	}
+	} while (retval + input_event_size() <= count);
 
  out:
 	mutex_unlock(&evdev->mutex);
diff --git a/drivers/input/gameport/gameport.c b/drivers/input/gameport/gameport.c
index 23cf8fc933ec..5b8f59d6c3e8 100644
--- a/drivers/input/gameport/gameport.c
+++ b/drivers/input/gameport/gameport.c
@@ -360,7 +360,7 @@ static int gameport_queue_event(void *object, struct module *owner,
 	event->owner = owner;
 
 	list_add_tail(&event->node, &gameport_event_list);
-	schedule_work(&gameport_event_work);
+	queue_work(system_long_wq, &gameport_event_work);
 
 out:
 	spin_unlock_irqrestore(&gameport_event_lock, flags);
diff --git a/drivers/input/input-polldev.c b/drivers/input/input-polldev.c
index 0559e309bac9..3037842a60d8 100644
--- a/drivers/input/input-polldev.c
+++ b/drivers/input/input-polldev.c
@@ -192,7 +192,7 @@ static struct attribute_group input_polldev_attribute_group = {
 };
 
 /**
- * input_allocate_polled_device - allocated memory polled device
+ * input_allocate_polled_device - allocate memory for polled device
  *
  * The function allocates memory for a polled device and also
  * for an input device associated with this polled device.
@@ -239,7 +239,7 @@ EXPORT_SYMBOL(input_free_polled_device);
  * with input layer. The device should be allocated with call to
  * input_allocate_polled_device(). Callers should also set up poll()
  * method and set up capabilities (id, name, phys, bits) of the
- * corresponing input_dev structure.
+ * corresponding input_dev structure.
  */
 int input_register_polled_device(struct input_polled_dev *dev)
 {
diff --git a/drivers/input/input.c b/drivers/input/input.c
index 11905b6a3023..d6e8bd8a851c 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -791,22 +791,9 @@ int input_get_keycode(struct input_dev *dev, struct input_keymap_entry *ke)
 	int retval;
 
 	spin_lock_irqsave(&dev->event_lock, flags);
-
-	if (dev->getkeycode) {
-		/*
-		 * Support for legacy drivers, that don't implement the new
-		 * ioctls
-		 */
-		u32 scancode = ke->index;
-
-		memcpy(ke->scancode, &scancode, sizeof(scancode));
-		ke->len = sizeof(scancode);
-		retval = dev->getkeycode(dev, scancode, &ke->keycode);
-	} else {
-		retval = dev->getkeycode_new(dev, ke);
-	}
-
+	retval = dev->getkeycode(dev, ke);
 	spin_unlock_irqrestore(&dev->event_lock, flags);
+
 	return retval;
 }
 EXPORT_SYMBOL(input_get_keycode);
@@ -831,35 +818,7 @@ int input_set_keycode(struct input_dev *dev,
 
 	spin_lock_irqsave(&dev->event_lock, flags);
 
-	if (dev->setkeycode) {
-		/*
-		 * Support for legacy drivers, that don't implement the new
-		 * ioctls
-		 */
-		unsigned int scancode;
-
-		retval = input_scancode_to_scalar(ke, &scancode);
-		if (retval)
-			goto out;
-
-		/*
-		 * We need to know the old scancode, in order to generate a
-		 * keyup effect, if the set operation happens successfully
-		 */
-		if (!dev->getkeycode) {
-			retval = -EINVAL;
-			goto out;
-		}
-
-		retval = dev->getkeycode(dev, scancode, &old_keycode);
-		if (retval)
-			goto out;
-
-		retval = dev->setkeycode(dev, scancode, ke->keycode);
-	} else {
-		retval = dev->setkeycode_new(dev, ke, &old_keycode);
-	}
-
+	retval = dev->setkeycode(dev, ke, &old_keycode);
 	if (retval)
 		goto out;
 
@@ -1846,11 +1805,11 @@ int input_register_device(struct input_dev *dev)
 		dev->rep[REP_PERIOD] = 33;
 	}
 
-	if (!dev->getkeycode && !dev->getkeycode_new)
-		dev->getkeycode_new = input_default_getkeycode;
+	if (!dev->getkeycode)
+		dev->getkeycode = input_default_getkeycode;
 
-	if (!dev->setkeycode && !dev->setkeycode_new)
-		dev->setkeycode_new = input_default_setkeycode;
+	if (!dev->setkeycode)
+		dev->setkeycode = input_default_setkeycode;
 
 	dev_set_name(&dev->dev, "input%ld",
 		     (unsigned long) atomic_inc_return(&input_no) - 1);
diff --git a/drivers/input/keyboard/lm8323.c b/drivers/input/keyboard/lm8323.c
index f7c2a166576b..b732870ecc89 100644
--- a/drivers/input/keyboard/lm8323.c
+++ b/drivers/input/keyboard/lm8323.c
@@ -30,6 +30,7 @@
 #include <linux/delay.h>
 #include <linux/input.h>
 #include <linux/leds.h>
+#include <linux/pm.h>
 #include <linux/i2c/lm8323.h>
 #include <linux/slab.h>
 
@@ -802,8 +803,9 @@ static int __devexit lm8323_remove(struct i2c_client *client)
  * We don't need to explicitly suspend the chip, as it already switches off
  * when there's no activity.
  */
-static int lm8323_suspend(struct i2c_client *client, pm_message_t mesg)
+static int lm8323_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct lm8323_chip *lm = i2c_get_clientdata(client);
 	int i;
 
@@ -821,8 +823,9 @@ static int lm8323_suspend(struct i2c_client *client, pm_message_t mesg)
 	return 0;
 }
 
-static int lm8323_resume(struct i2c_client *client)
+static int lm8323_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct lm8323_chip *lm = i2c_get_clientdata(client);
 	int i;
 
@@ -839,11 +842,10 @@ static int lm8323_resume(struct i2c_client *client)
 
 	return 0;
 }
-#else
-#define lm8323_suspend	NULL
-#define lm8323_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(lm8323_pm_ops, lm8323_suspend, lm8323_resume);
+
 static const struct i2c_device_id lm8323_id[] = {
 	{ "lm8323", 0 },
 	{ }
@@ -852,11 +854,10 @@ static const struct i2c_device_id lm8323_id[] = {
 static struct i2c_driver lm8323_i2c_driver = {
 	.driver = {
 		.name	= "lm8323",
+		.pm	= &lm8323_pm_ops,
 	},
 	.probe		= lm8323_probe,
 	.remove		= __devexit_p(lm8323_remove),
-	.suspend	= lm8323_suspend,
-	.resume		= lm8323_resume,
 	.id_table	= lm8323_id,
 };
 MODULE_DEVICE_TABLE(i2c, lm8323_id);
diff --git a/drivers/input/keyboard/max7359_keypad.c b/drivers/input/keyboard/max7359_keypad.c
index 9091ff5ea808..5afe35ad24d3 100644
--- a/drivers/input/keyboard/max7359_keypad.c
+++ b/drivers/input/keyboard/max7359_keypad.c
@@ -17,6 +17,7 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
+#include <linux/pm.h>
 #include <linux/input.h>
 #include <linux/input/matrix_keypad.h>
 
@@ -271,8 +272,10 @@ static int __devexit max7359_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
-static int max7359_suspend(struct i2c_client *client, pm_message_t mesg)
+static int max7359_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+
 	max7359_fall_deepsleep(client);
 
 	if (device_may_wakeup(&client->dev))
@@ -281,8 +284,10 @@ static int max7359_suspend(struct i2c_client *client, pm_message_t mesg)
 	return 0;
 }
 
-static int max7359_resume(struct i2c_client *client)
+static int max7359_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
+
 	if (device_may_wakeup(&client->dev))
 		disable_irq_wake(client->irq);
 
@@ -291,11 +296,10 @@ static int max7359_resume(struct i2c_client *client)
 
 	return 0;
 }
-#else
-#define max7359_suspend	NULL
-#define max7359_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(max7359_pm, max7359_suspend, max7359_resume);
+
 static const struct i2c_device_id max7359_ids[] = {
 	{ "max7359", 0 },
 	{ }
@@ -305,11 +309,10 @@ MODULE_DEVICE_TABLE(i2c, max7359_ids);
 static struct i2c_driver max7359_i2c_driver = {
 	.driver = {
 		.name = "max7359",
+		.pm   = &max7359_pm,
 	},
 	.probe		= max7359_probe,
 	.remove		= __devexit_p(max7359_remove),
-	.suspend	= max7359_suspend,
-	.resume		= max7359_resume,
 	.id_table	= max7359_ids,
 };
 
diff --git a/drivers/input/keyboard/mcs_touchkey.c b/drivers/input/keyboard/mcs_touchkey.c
index 63b849d7e90b..af1aab324a4c 100644
--- a/drivers/input/keyboard/mcs_touchkey.c
+++ b/drivers/input/keyboard/mcs_touchkey.c
@@ -1,5 +1,5 @@
 /*
- * mcs_touchkey.c - Touchkey driver for MELFAS MCS5000/5080 controller
+ * Touchkey driver for MELFAS MCS5000/5080 controller
  *
  * Copyright (C) 2010 Samsung Electronics Co.Ltd
  * Author: HeungJun Kim <riverful.kim@samsung.com>
@@ -19,6 +19,7 @@
 #include <linux/input.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/pm.h>
 
 /* MCS5000 Touchkey */
 #define MCS5000_TOUCHKEY_STATUS		0x04
@@ -45,6 +46,8 @@ struct mcs_touchkey_chip {
 };
 
 struct mcs_touchkey_data {
+	void (*poweron)(bool);
+
 	struct i2c_client *client;
 	struct input_dev *input_dev;
 	struct mcs_touchkey_chip chip;
@@ -169,6 +172,11 @@ static int __devinit mcs_touchkey_probe(struct i2c_client *client,
 	if (pdata->cfg_pin)
 		pdata->cfg_pin();
 
+	if (pdata->poweron) {
+		data->poweron = pdata->poweron;
+		data->poweron(true);
+	}
+
 	error = request_threaded_irq(client->irq, NULL, mcs_touchkey_interrupt,
 			IRQF_TRIGGER_FALLING, client->dev.driver->name, data);
 	if (error) {
@@ -196,12 +204,57 @@ static int __devexit mcs_touchkey_remove(struct i2c_client *client)
 	struct mcs_touchkey_data *data = i2c_get_clientdata(client);
 
 	free_irq(client->irq, data);
+	if (data->poweron)
+		data->poweron(false);
 	input_unregister_device(data->input_dev);
 	kfree(data);
 
 	return 0;
 }
 
+static void mcs_touchkey_shutdown(struct i2c_client *client)
+{
+	struct mcs_touchkey_data *data = i2c_get_clientdata(client);
+
+	if (data->poweron)
+		data->poweron(false);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mcs_touchkey_suspend(struct device *dev)
+{
+	struct mcs_touchkey_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+
+	/* Disable the work */
+	disable_irq(client->irq);
+
+	/* Finally turn off the power */
+	if (data->poweron)
+		data->poweron(false);
+
+	return 0;
+}
+
+static int mcs_touchkey_resume(struct device *dev)
+{
+	struct mcs_touchkey_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+
+	/* Enable the device first */
+	if (data->poweron)
+		data->poweron(true);
+
+	/* Enable irq again */
+	enable_irq(client->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(mcs_touchkey_pm_ops,
+			 mcs_touchkey_suspend, mcs_touchkey_resume);
+
 static const struct i2c_device_id mcs_touchkey_id[] = {
 	{ "mcs5000_touchkey", MCS5000_TOUCHKEY },
 	{ "mcs5080_touchkey", MCS5080_TOUCHKEY },
@@ -213,9 +266,11 @@ static struct i2c_driver mcs_touchkey_driver = {
 	.driver = {
 		.name	= "mcs_touchkey",
 		.owner	= THIS_MODULE,
+		.pm	= &mcs_touchkey_pm_ops,
 	},
 	.probe		= mcs_touchkey_probe,
 	.remove		= __devexit_p(mcs_touchkey_remove),
+	.shutdown       = mcs_touchkey_shutdown,
 	.id_table	= mcs_touchkey_id,
 };
 
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index 45bd0977d006..c51a3c4a7feb 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -29,6 +29,7 @@
 #include <linux/io.h>
 #include <linux/input.h>
 #include <linux/slab.h>
+#include <linux/pm_runtime.h>
 
 #include <plat/omap4-keypad.h>
 
@@ -80,20 +81,6 @@ struct omap4_keypad {
 	unsigned short keymap[];
 };
 
-static void __devinit omap4_keypad_config(struct omap4_keypad *keypad_data)
-{
-	__raw_writel(OMAP4_VAL_FUNCTIONALCFG,
-			keypad_data->base + OMAP4_KBD_CTRL);
-	__raw_writel(OMAP4_VAL_DEBOUNCINGTIME,
-			keypad_data->base + OMAP4_KBD_DEBOUNCINGTIME);
-	__raw_writel(OMAP4_VAL_IRQDISABLE,
-			keypad_data->base + OMAP4_KBD_IRQSTATUS);
-	__raw_writel(OMAP4_DEF_IRQENABLE_EVENTEN | OMAP4_DEF_IRQENABLE_LONGKEY,
-			keypad_data->base + OMAP4_KBD_IRQENABLE);
-	__raw_writel(OMAP4_DEF_WUP_EVENT_ENA | OMAP4_DEF_WUP_LONG_KEY_ENA,
-			keypad_data->base + OMAP4_KBD_WAKEUPENABLE);
-}
-
 /* Interrupt handler */
 static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 {
@@ -144,6 +131,49 @@ static irqreturn_t omap4_keypad_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int omap4_keypad_open(struct input_dev *input)
+{
+	struct omap4_keypad *keypad_data = input_get_drvdata(input);
+
+	pm_runtime_get_sync(input->dev.parent);
+
+	disable_irq(keypad_data->irq);
+
+	__raw_writel(OMAP4_VAL_FUNCTIONALCFG,
+			keypad_data->base + OMAP4_KBD_CTRL);
+	__raw_writel(OMAP4_VAL_DEBOUNCINGTIME,
+			keypad_data->base + OMAP4_KBD_DEBOUNCINGTIME);
+	__raw_writel(OMAP4_VAL_IRQDISABLE,
+			keypad_data->base + OMAP4_KBD_IRQSTATUS);
+	__raw_writel(OMAP4_DEF_IRQENABLE_EVENTEN | OMAP4_DEF_IRQENABLE_LONGKEY,
+			keypad_data->base + OMAP4_KBD_IRQENABLE);
+	__raw_writel(OMAP4_DEF_WUP_EVENT_ENA | OMAP4_DEF_WUP_LONG_KEY_ENA,
+			keypad_data->base + OMAP4_KBD_WAKEUPENABLE);
+
+	enable_irq(keypad_data->irq);
+
+	return 0;
+}
+
+static void omap4_keypad_close(struct input_dev *input)
+{
+	struct omap4_keypad *keypad_data = input_get_drvdata(input);
+
+	disable_irq(keypad_data->irq);
+
+	/* Disable interrupts */
+	__raw_writel(OMAP4_VAL_IRQDISABLE,
+		     keypad_data->base + OMAP4_KBD_IRQENABLE);
+
+	/* clear pending interrupts */
+	__raw_writel(__raw_readl(keypad_data->base + OMAP4_KBD_IRQSTATUS),
+			keypad_data->base + OMAP4_KBD_IRQSTATUS);
+
+	enable_irq(keypad_data->irq);
+
+	pm_runtime_put_sync(input->dev.parent);
+}
+
 static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 {
 	const struct omap4_keypad_platform_data *pdata;
@@ -225,6 +255,9 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	input_dev->id.product = 0x0001;
 	input_dev->id.version = 0x0001;
 
+	input_dev->open = omap4_keypad_open;
+	input_dev->close = omap4_keypad_close;
+
 	input_dev->keycode	= keypad_data->keymap;
 	input_dev->keycodesize	= sizeof(keypad_data->keymap[0]);
 	input_dev->keycodemax	= max_keys;
@@ -239,8 +272,6 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
 			input_dev->keycode, input_dev->keybit);
 
-	omap4_keypad_config(keypad_data);
-
 	error = request_irq(keypad_data->irq, omap4_keypad_interrupt,
 			     IRQF_TRIGGER_RISING,
 			     "omap4-keypad", keypad_data);
@@ -249,17 +280,19 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 		goto err_free_input;
 	}
 
+	pm_runtime_enable(&pdev->dev);
+
 	error = input_register_device(keypad_data->input);
 	if (error < 0) {
 		dev_err(&pdev->dev, "failed to register input device\n");
-		goto err_free_irq;
+		goto err_pm_disable;
 	}
 
-
 	platform_set_drvdata(pdev, keypad_data);
 	return 0;
 
-err_free_irq:
+err_pm_disable:
+	pm_runtime_disable(&pdev->dev);
 	free_irq(keypad_data->irq, keypad_data);
 err_free_input:
 	input_free_device(input_dev);
@@ -278,6 +311,9 @@ static int __devexit omap4_keypad_remove(struct platform_device *pdev)
 	struct resource *res;
 
 	free_irq(keypad_data->irq, keypad_data);
+
+	pm_runtime_disable(&pdev->dev);
+
 	input_unregister_device(keypad_data->input);
 
 	iounmap(keypad_data->base);
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index ac471b77c18e..99ce9032d08c 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -71,8 +71,9 @@ struct tegra_kbc {
 	spinlock_t lock;
 	unsigned int repoll_dly;
 	unsigned long cp_dly_jiffies;
+	bool use_fn_map;
 	const struct tegra_kbc_platform_data *pdata;
-	unsigned short keycode[KBC_MAX_KEY];
+	unsigned short keycode[KBC_MAX_KEY * 2];
 	unsigned short current_keys[KBC_MAX_KPENT];
 	unsigned int num_pressed_keys;
 	struct timer_list timer;
@@ -178,6 +179,40 @@ static const u32 tegra_kbc_default_keymap[] = {
 	KEY(15, 5, KEY_F2),
 	KEY(15, 6, KEY_CAPSLOCK),
 	KEY(15, 7, KEY_F6),
+
+	/* Software Handled Function Keys */
+	KEY(20, 0, KEY_KP7),
+
+	KEY(21, 0, KEY_KP9),
+	KEY(21, 1, KEY_KP8),
+	KEY(21, 2, KEY_KP4),
+	KEY(21, 4, KEY_KP1),
+
+	KEY(22, 1, KEY_KPSLASH),
+	KEY(22, 2, KEY_KP6),
+	KEY(22, 3, KEY_KP5),
+	KEY(22, 4, KEY_KP3),
+	KEY(22, 5, KEY_KP2),
+	KEY(22, 7, KEY_KP0),
+
+	KEY(27, 1, KEY_KPASTERISK),
+	KEY(27, 3, KEY_KPMINUS),
+	KEY(27, 4, KEY_KPPLUS),
+	KEY(27, 5, KEY_KPDOT),
+
+	KEY(28, 5, KEY_VOLUMEUP),
+
+	KEY(29, 3, KEY_HOME),
+	KEY(29, 4, KEY_END),
+	KEY(29, 5, KEY_BRIGHTNESSDOWN),
+	KEY(29, 6, KEY_VOLUMEDOWN),
+	KEY(29, 7, KEY_BRIGHTNESSUP),
+
+	KEY(30, 0, KEY_NUMLOCK),
+	KEY(30, 1, KEY_SCROLLLOCK),
+	KEY(30, 2, KEY_MUTE),
+
+	KEY(31, 4, KEY_HELP),
 };
 
 static const struct matrix_keymap_data tegra_kbc_default_keymap_data = {
@@ -224,6 +259,7 @@ static void tegra_kbc_report_keys(struct tegra_kbc *kbc)
 	unsigned int i;
 	unsigned int num_down = 0;
 	unsigned long flags;
+	bool fn_keypress = false;
 
 	spin_lock_irqsave(&kbc->lock, flags);
 	for (i = 0; i < KBC_MAX_KPENT; i++) {
@@ -237,11 +273,28 @@ static void tegra_kbc_report_keys(struct tegra_kbc *kbc)
 				MATRIX_SCAN_CODE(row, col, KBC_ROW_SHIFT);
 
 			scancodes[num_down] = scancode;
-			keycodes[num_down++] = kbc->keycode[scancode];
+			keycodes[num_down] = kbc->keycode[scancode];
+			/* If driver uses Fn map, do not report the Fn key. */
+			if ((keycodes[num_down] == KEY_FN) && kbc->use_fn_map)
+				fn_keypress = true;
+			else
+				num_down++;
 		}
 
 		val >>= 8;
 	}
+
+	/*
+	 * If the platform uses Fn keymaps, translate keys on a Fn keypress.
+	 * Function keycodes are KBC_MAX_KEY apart from the plain keycodes.
+	 */
+	if (fn_keypress) {
+		for (i = 0; i < num_down; i++) {
+			scancodes[i] += KBC_MAX_KEY;
+			keycodes[i] = kbc->keycode[scancodes[i]];
+		}
+	}
+
 	spin_unlock_irqrestore(&kbc->lock, flags);
 
 	tegra_kbc_report_released_keys(kbc->idev,
@@ -594,8 +647,11 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 
 	input_dev->keycode = kbc->keycode;
 	input_dev->keycodesize = sizeof(kbc->keycode[0]);
-	input_dev->keycodemax = ARRAY_SIZE(kbc->keycode);
+	input_dev->keycodemax = KBC_MAX_KEY;
+	if (pdata->use_fn_map)
+		input_dev->keycodemax *= 2;
 
+	kbc->use_fn_map = pdata->use_fn_map;
 	keymap_data = pdata->keymap_data ?: &tegra_kbc_default_keymap_data;
 	matrix_keypad_build_keymap(keymap_data, KBC_ROW_SHIFT,
 				   input_dev->keycode, input_dev->keybit);
diff --git a/drivers/input/misc/ad714x-i2c.c b/drivers/input/misc/ad714x-i2c.c
index 2bef8fa56c94..e21deb1baa8a 100644
--- a/drivers/input/misc/ad714x-i2c.c
+++ b/drivers/input/misc/ad714x-i2c.c
@@ -10,23 +10,23 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/types.h>
+#include <linux/pm.h>
 #include "ad714x.h"
 
 #ifdef CONFIG_PM
-static int ad714x_i2c_suspend(struct i2c_client *client, pm_message_t message)
+static int ad714x_i2c_suspend(struct device *dev)
 {
-	return ad714x_disable(i2c_get_clientdata(client));
+	return ad714x_disable(i2c_get_clientdata(to_i2c_client(dev)));
 }
 
-static int ad714x_i2c_resume(struct i2c_client *client)
+static int ad714x_i2c_resume(struct device *dev)
 {
-	return ad714x_enable(i2c_get_clientdata(client));
+	return ad714x_enable(i2c_get_clientdata(to_i2c_client(dev)));
 }
-#else
-# define ad714x_i2c_suspend NULL
-# define ad714x_i2c_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ad714x_i2c_pm, ad714x_i2c_suspend, ad714x_i2c_resume);
+
 static int ad714x_i2c_write(struct device *dev, unsigned short reg,
 				unsigned short data)
 {
@@ -114,11 +114,10 @@ MODULE_DEVICE_TABLE(i2c, ad714x_id);
 static struct i2c_driver ad714x_i2c_driver = {
 	.driver = {
 		.name = "ad714x_captouch",
+		.pm   = &ad714x_i2c_pm,
 	},
 	.probe    = ad714x_i2c_probe,
 	.remove   = __devexit_p(ad714x_i2c_remove),
-	.suspend  = ad714x_i2c_suspend,
-	.resume	  = ad714x_i2c_resume,
 	.id_table = ad714x_id,
 };
 
diff --git a/drivers/input/misc/ad714x-spi.c b/drivers/input/misc/ad714x-spi.c
index 7f8dedfd1bfe..4120dd549305 100644
--- a/drivers/input/misc/ad714x-spi.c
+++ b/drivers/input/misc/ad714x-spi.c
@@ -9,6 +9,7 @@
 #include <linux/input.h>	/* BUS_I2C */
 #include <linux/module.h>
 #include <linux/spi/spi.h>
+#include <linux/pm.h>
 #include <linux/types.h>
 #include "ad714x.h"
 
@@ -16,20 +17,19 @@
 #define AD714x_SPI_READ            BIT(10)
 
 #ifdef CONFIG_PM
-static int ad714x_spi_suspend(struct spi_device *spi, pm_message_t message)
+static int ad714x_spi_suspend(struct device *dev)
 {
-	return ad714x_disable(spi_get_drvdata(spi));
+	return ad714x_disable(spi_get_drvdata(to_spi_device(dev)));
 }
 
-static int ad714x_spi_resume(struct spi_device *spi)
+static int ad714x_spi_resume(struct device *dev)
 {
-	return ad714x_enable(spi_get_drvdata(spi));
+	return ad714x_enable(spi_get_drvdata(to_spi_device(dev)));
 }
-#else
-# define ad714x_spi_suspend NULL
-# define ad714x_spi_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ad714x_spi_pm, ad714x_spi_suspend, ad714x_spi_resume);
+
 static int ad714x_spi_read(struct device *dev, unsigned short reg,
 		unsigned short *data)
 {
@@ -79,11 +79,10 @@ static struct spi_driver ad714x_spi_driver = {
 	.driver = {
 		.name	= "ad714x_captouch",
 		.owner	= THIS_MODULE,
+		.pm	= &ad714x_spi_pm,
 	},
 	.probe		= ad714x_spi_probe,
 	.remove		= __devexit_p(ad714x_spi_remove),
-	.suspend	= ad714x_spi_suspend,
-	.resume		= ad714x_spi_resume,
 };
 
 static __init int ad714x_spi_init(void)
diff --git a/drivers/input/misc/adxl34x-i2c.c b/drivers/input/misc/adxl34x-i2c.c
index 0779724af7e7..ccacf2bb06a4 100644
--- a/drivers/input/misc/adxl34x-i2c.c
+++ b/drivers/input/misc/adxl34x-i2c.c
@@ -11,6 +11,7 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/types.h>
+#include <linux/pm.h>
 #include "adxl34x.h"
 
 static int adxl34x_smbus_read(struct device *dev, unsigned char reg)
@@ -105,8 +106,9 @@ static int __devexit adxl34x_i2c_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
-static int adxl34x_i2c_suspend(struct i2c_client *client, pm_message_t message)
+static int adxl34x_i2c_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct adxl34x *ac = i2c_get_clientdata(client);
 
 	adxl34x_suspend(ac);
@@ -114,19 +116,20 @@ static int adxl34x_i2c_suspend(struct i2c_client *client, pm_message_t message)
 	return 0;
 }
 
-static int adxl34x_i2c_resume(struct i2c_client *client)
+static int adxl34x_i2c_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct adxl34x *ac = i2c_get_clientdata(client);
 
 	adxl34x_resume(ac);
 
 	return 0;
 }
-#else
-# define adxl34x_i2c_suspend NULL
-# define adxl34x_i2c_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adxl34x_i2c_pm, adxl34x_i2c_suspend,
+			 adxl34x_i2c_resume);
+
 static const struct i2c_device_id adxl34x_id[] = {
 	{ "adxl34x", 0 },
 	{ }
@@ -138,11 +141,10 @@ static struct i2c_driver adxl34x_driver = {
 	.driver = {
 		.name = "adxl34x",
 		.owner = THIS_MODULE,
+		.pm = &adxl34x_i2c_pm,
 	},
 	.probe    = adxl34x_i2c_probe,
 	.remove   = __devexit_p(adxl34x_i2c_remove),
-	.suspend  = adxl34x_i2c_suspend,
-	.resume   = adxl34x_i2c_resume,
 	.id_table = adxl34x_id,
 };
 
diff --git a/drivers/input/misc/adxl34x-spi.c b/drivers/input/misc/adxl34x-spi.c
index 782de9e89828..f29de22fdda0 100644
--- a/drivers/input/misc/adxl34x-spi.c
+++ b/drivers/input/misc/adxl34x-spi.c
@@ -10,6 +10,7 @@
 #include <linux/input.h>	/* BUS_SPI */
 #include <linux/module.h>
 #include <linux/spi/spi.h>
+#include <linux/pm.h>
 #include <linux/types.h>
 #include "adxl34x.h"
 
@@ -57,7 +58,7 @@ static int adxl34x_spi_read_block(struct device *dev,
 	return (status < 0) ? status : 0;
 }
 
-static const struct adxl34x_bus_ops adx134x_spi_bops = {
+static const struct adxl34x_bus_ops adxl34x_spi_bops = {
 	.bustype	= BUS_SPI,
 	.write		= adxl34x_spi_write,
 	.read		= adxl34x_spi_read,
@@ -76,7 +77,7 @@ static int __devinit adxl34x_spi_probe(struct spi_device *spi)
 
 	ac = adxl34x_probe(&spi->dev, spi->irq,
 			   spi->max_speed_hz > MAX_FREQ_NO_FIFODELAY,
-			   &adx134x_spi_bops);
+			   &adxl34x_spi_bops);
 
 	if (IS_ERR(ac))
 		return PTR_ERR(ac);
@@ -94,8 +95,9 @@ static int __devexit adxl34x_spi_remove(struct spi_device *spi)
 }
 
 #ifdef CONFIG_PM
-static int adxl34x_spi_suspend(struct spi_device *spi, pm_message_t message)
+static int adxl34x_spi_suspend(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct adxl34x *ac = dev_get_drvdata(&spi->dev);
 
 	adxl34x_suspend(ac);
@@ -103,29 +105,29 @@ static int adxl34x_spi_suspend(struct spi_device *spi, pm_message_t message)
 	return 0;
 }
 
-static int adxl34x_spi_resume(struct spi_device *spi)
+static int adxl34x_spi_resume(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct adxl34x *ac = dev_get_drvdata(&spi->dev);
 
 	adxl34x_resume(ac);
 
 	return 0;
 }
-#else
-# define adxl34x_spi_suspend NULL
-# define adxl34x_spi_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adxl34x_spi_pm, adxl34x_spi_suspend,
+			 adxl34x_spi_resume);
+
 static struct spi_driver adxl34x_driver = {
 	.driver = {
 		.name = "adxl34x",
 		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
+		.pm = &adxl34x_spi_pm,
 	},
 	.probe   = adxl34x_spi_probe,
 	.remove  = __devexit_p(adxl34x_spi_remove),
-	.suspend = adxl34x_spi_suspend,
-	.resume  = adxl34x_spi_resume,
 };
 
 static int __init adxl34x_spi_init(void)
diff --git a/drivers/input/misc/ati_remote2.c b/drivers/input/misc/ati_remote2.c
index 0b0e9be63542..9ccdb82d869a 100644
--- a/drivers/input/misc/ati_remote2.c
+++ b/drivers/input/misc/ati_remote2.c
@@ -612,8 +612,8 @@ static int ati_remote2_input_init(struct ati_remote2 *ar2)
 	idev->open = ati_remote2_open;
 	idev->close = ati_remote2_close;
 
-	idev->getkeycode_new = ati_remote2_getkeycode;
-	idev->setkeycode_new = ati_remote2_setkeycode;
+	idev->getkeycode = ati_remote2_getkeycode;
+	idev->setkeycode = ati_remote2_setkeycode;
 
 	idev->name = ar2->name;
 	idev->phys = ar2->phys;
diff --git a/drivers/input/misc/sparcspkr.c b/drivers/input/misc/sparcspkr.c
index 8e130bf7d32b..0122f5351577 100644
--- a/drivers/input/misc/sparcspkr.c
+++ b/drivers/input/misc/sparcspkr.c
@@ -173,18 +173,16 @@ static int __devinit sparcspkr_probe(struct device *dev)
 	return 0;
 }
 
-static int sparcspkr_shutdown(struct platform_device *dev)
+static void sparcspkr_shutdown(struct platform_device *dev)
 {
 	struct sparcspkr_state *state = dev_get_drvdata(&dev->dev);
 	struct input_dev *input_dev = state->input_dev;
 
 	/* turn off the speaker */
 	state->event(input_dev, EV_SND, SND_BELL, 0);
-
-	return 0;
 }
 
-static int __devinit bbc_beep_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit bbc_beep_probe(struct platform_device *op)
 {
 	struct sparcspkr_state *state;
 	struct bbc_beep_info *info;
@@ -258,7 +256,7 @@ static const struct of_device_id bbc_beep_match[] = {
 	{},
 };
 
-static struct of_platform_driver bbc_beep_driver = {
+static struct platform_driver bbc_beep_driver = {
 	.driver = {
 		.name = "bbcbeep",
 		.owner = THIS_MODULE,
@@ -269,7 +267,7 @@ static struct of_platform_driver bbc_beep_driver = {
 	.shutdown	= sparcspkr_shutdown,
 };
 
-static int __devinit grover_beep_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit grover_beep_probe(struct platform_device *op)
 {
 	struct sparcspkr_state *state;
 	struct grover_beep_info *info;
@@ -340,7 +338,7 @@ static const struct of_device_id grover_beep_match[] = {
 	{},
 };
 
-static struct of_platform_driver grover_beep_driver = {
+static struct platform_driver grover_beep_driver = {
 	.driver = {
 		.name = "groverbeep",
 		.owner = THIS_MODULE,
@@ -353,12 +351,12 @@ static struct of_platform_driver grover_beep_driver = {
 
 static int __init sparcspkr_init(void)
 {
-	int err = of_register_platform_driver(&bbc_beep_driver);
+	int err = platform_driver_register(&bbc_beep_driver);
 
 	if (!err) {
-		err = of_register_platform_driver(&grover_beep_driver);
+		err = platform_driver_register(&grover_beep_driver);
 		if (err)
-			of_unregister_platform_driver(&bbc_beep_driver);
+			platform_driver_unregister(&bbc_beep_driver);
 	}
 
 	return err;
@@ -366,8 +364,8 @@ static int __init sparcspkr_init(void)
 
 static void __exit sparcspkr_exit(void)
 {
-	of_unregister_platform_driver(&bbc_beep_driver);
-	of_unregister_platform_driver(&grover_beep_driver);
+	platform_driver_unregister(&bbc_beep_driver);
+	platform_driver_unregister(&grover_beep_driver);
 }
 
 module_init(sparcspkr_init);
diff --git a/drivers/input/misc/twl4030-vibra.c b/drivers/input/misc/twl4030-vibra.c
index 014dd4ad0d4f..6a11694e3fc7 100644
--- a/drivers/input/misc/twl4030-vibra.c
+++ b/drivers/input/misc/twl4030-vibra.c
@@ -29,6 +29,7 @@
 #include <linux/workqueue.h>
 #include <linux/i2c/twl.h>
 #include <linux/mfd/twl4030-codec.h>
+#include <linux/mfd/core.h>
 #include <linux/input.h>
 #include <linux/slab.h>
 
@@ -196,7 +197,7 @@ static SIMPLE_DEV_PM_OPS(twl4030_vibra_pm_ops,
 
 static int __devinit twl4030_vibra_probe(struct platform_device *pdev)
 {
-	struct twl4030_codec_vibra_data *pdata = pdev->dev.platform_data;
+	struct twl4030_codec_vibra_data *pdata = mfd_get_data(pdev);
 	struct vibra_info *info;
 	int ret;
 
diff --git a/drivers/input/misc/uinput.c b/drivers/input/misc/uinput.c
index 82542a1c1098..364bdf43a381 100644
--- a/drivers/input/misc/uinput.c
+++ b/drivers/input/misc/uinput.c
@@ -347,8 +347,7 @@ static int uinput_setup_device(struct uinput_device *udev, const char __user *bu
 {
 	struct uinput_user_dev	*user_dev;
 	struct input_dev	*dev;
-	char			*name;
-	int			i, size;
+	int			i;
 	int			retval;
 
 	if (count != sizeof(struct uinput_user_dev))
@@ -362,30 +361,25 @@ static int uinput_setup_device(struct uinput_device *udev, const char __user *bu
 
 	dev = udev->dev;
 
-	user_dev = kmalloc(sizeof(struct uinput_user_dev), GFP_KERNEL);
-	if (!user_dev)
-		return -ENOMEM;
-
-	if (copy_from_user(user_dev, buffer, sizeof(struct uinput_user_dev))) {
-		retval = -EFAULT;
-		goto exit;
-	}
+	user_dev = memdup_user(buffer, sizeof(struct uinput_user_dev));
+	if (IS_ERR(user_dev))
+		return PTR_ERR(user_dev);
 
 	udev->ff_effects_max = user_dev->ff_effects_max;
 
-	size = strnlen(user_dev->name, UINPUT_MAX_NAME_SIZE) + 1;
-	if (!size) {
+	/* Ensure name is filled in */
+	if (!user_dev->name[0]) {
 		retval = -EINVAL;
 		goto exit;
 	}
 
 	kfree(dev->name);
-	dev->name = name = kmalloc(size, GFP_KERNEL);
-	if (!name) {
+	dev->name = kstrndup(user_dev->name, UINPUT_MAX_NAME_SIZE,
+			     GFP_KERNEL);
+	if (!dev->name) {
 		retval = -ENOMEM;
 		goto exit;
 	}
-	strlcpy(name, user_dev->name, size);
 
 	dev->id.bustype	= user_dev->id.bustype;
 	dev->id.vendor	= user_dev->id.vendor;
@@ -622,7 +616,6 @@ static long uinput_ioctl_handler(struct file *file, unsigned int cmd,
 	struct uinput_ff_upload ff_up;
 	struct uinput_ff_erase  ff_erase;
 	struct uinput_request   *req;
-	int                     length;
 	char			*phys;
 
 	retval = mutex_lock_interruptible(&udev->mutex);
@@ -689,24 +682,15 @@ static long uinput_ioctl_handler(struct file *file, unsigned int cmd,
 				retval = -EINVAL;
 				goto out;
 			}
-			length = strnlen_user(p, 1024);
-			if (length <= 0) {
-				retval = -EFAULT;
-				break;
+
+			phys = strndup_user(p, 1024);
+			if (IS_ERR(phys)) {
+				retval = PTR_ERR(phys);
+				goto out;
 			}
+
 			kfree(udev->dev->phys);
-			udev->dev->phys = phys = kmalloc(length, GFP_KERNEL);
-			if (!phys) {
-				retval = -ENOMEM;
-				break;
-			}
-			if (copy_from_user(phys, p, length)) {
-				udev->dev->phys = NULL;
-				kfree(phys);
-				retval = -EFAULT;
-				break;
-			}
-			phys[length - 1] = '\0';
+			udev->dev->phys = phys;
 			break;
 
 		case UI_BEGIN_FF_UPLOAD:
diff --git a/drivers/input/mouse/bcm5974.c b/drivers/input/mouse/bcm5974.c
index ee82851afe3e..0efaf667a1c8 100644
--- a/drivers/input/mouse/bcm5974.c
+++ b/drivers/input/mouse/bcm5974.c
@@ -430,10 +430,6 @@ static int report_tp_state(struct bcm5974 *dev, int size)
 		ptest = int2bound(&c->p, raw_p);
 		origin = raw2int(f->origin);
 
-		/* set the integrated button if applicable */
-		if (c->tp_type == TYPE2)
-			ibt = raw2int(dev->tp_data[BUTTON_TYPE2]);
-
 		/* while tracking finger still valid, count all fingers */
 		if (ptest > PRESSURE_LOW && origin) {
 			abs_p = ptest;
@@ -452,6 +448,10 @@ static int report_tp_state(struct bcm5974 *dev, int size)
 		}
 	}
 
+	/* set the integrated button if applicable */
+	if (c->tp_type == TYPE2)
+		ibt = raw2int(dev->tp_data[BUTTON_TYPE2]);
+
 	if (dev->fingers < nmin)
 		dev->fingers = nmin;
 	if (dev->fingers > nmax)
diff --git a/drivers/input/mouse/synaptics.h b/drivers/input/mouse/synaptics.h
index 25e5d042a72c..7453938bf5ef 100644
--- a/drivers/input/mouse/synaptics.h
+++ b/drivers/input/mouse/synaptics.h
@@ -51,6 +51,29 @@
 #define SYN_EXT_CAP_REQUESTS(c)		(((c) & 0x700000) >> 20)
 #define SYN_CAP_MULTI_BUTTON_NO(ec)	(((ec) & 0x00f000) >> 12)
 #define SYN_CAP_PRODUCT_ID(ec)		(((ec) & 0xff0000) >> 16)
+
+/*
+ * The following describes response for the 0x0c query.
+ *
+ * byte	mask	name			meaning
+ * ----	----	-------			------------
+ * 1	0x01	adjustable threshold	capacitive button sensitivity
+ *					can be adjusted
+ * 1	0x02	report max		query 0x0d gives max coord reported
+ * 1	0x04	clearpad		sensor is ClearPad product
+ * 1	0x08	advanced gesture	not particularly meaningful
+ * 1	0x10	clickpad bit 0		1-button ClickPad
+ * 1	0x60	multifinger mode	identifies firmware finger counting
+ *					(not reporting!) algorithm.
+ *					Not particularly meaningful
+ * 1	0x80    covered pad		W clipped to 14, 15 == pad mostly covered
+ * 2	0x01    clickpad bit 1		2-button ClickPad
+ * 2	0x02    deluxe LED controls	touchpad support LED commands
+ *					ala multimedia control bar
+ * 2	0x04	reduced filtering	firmware does less filtering on
+ *					position data, driver should watch
+ *					for noise.
+ */
 #define SYN_CAP_CLICKPAD(ex0c)		((ex0c) & 0x100000) /* 1-button ClickPad */
 #define SYN_CAP_CLICKPAD2BTN(ex0c)	((ex0c) & 0x000100) /* 2-button ClickPad */
 #define SYN_CAP_MAX_DIMENSIONS(ex0c)	((ex0c) & 0x020000)
diff --git a/drivers/input/mouse/synaptics_i2c.c b/drivers/input/mouse/synaptics_i2c.c
index 0ae62f0bcb32..f6aa26d305ed 100644
--- a/drivers/input/mouse/synaptics_i2c.c
+++ b/drivers/input/mouse/synaptics_i2c.c
@@ -18,6 +18,7 @@
 #include <linux/delay.h>
 #include <linux/workqueue.h>
 #include <linux/slab.h>
+#include <linux/pm.h>
 
 #define DRIVER_NAME		"synaptics_i2c"
 /* maximum product id is 15 characters */
@@ -619,8 +620,9 @@ static int __devexit synaptics_i2c_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
-static int synaptics_i2c_suspend(struct i2c_client *client, pm_message_t mesg)
+static int synaptics_i2c_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct synaptics_i2c *touch = i2c_get_clientdata(client);
 
 	cancel_delayed_work_sync(&touch->dwork);
@@ -631,9 +633,10 @@ static int synaptics_i2c_suspend(struct i2c_client *client, pm_message_t mesg)
 	return 0;
 }
 
-static int synaptics_i2c_resume(struct i2c_client *client)
+static int synaptics_i2c_resume(struct device *dev)
 {
 	int ret;
+	struct i2c_client *client = to_i2c_client(dev);
 	struct synaptics_i2c *touch = i2c_get_clientdata(client);
 
 	ret = synaptics_i2c_reset_config(client);
@@ -645,11 +648,11 @@ static int synaptics_i2c_resume(struct i2c_client *client)
 
 	return 0;
 }
-#else
-#define synaptics_i2c_suspend	NULL
-#define synaptics_i2c_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(synaptics_i2c_pm, synaptics_i2c_suspend,
+			 synaptics_i2c_resume);
+
 static const struct i2c_device_id synaptics_i2c_id_table[] = {
 	{ "synaptics_i2c", 0 },
 	{ },
@@ -660,13 +663,12 @@ static struct i2c_driver synaptics_i2c_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
 		.owner	= THIS_MODULE,
+		.pm	= &synaptics_i2c_pm,
 	},
 
 	.probe		= synaptics_i2c_probe,
 	.remove		= __devexit_p(synaptics_i2c_remove),
 
-	.suspend	= synaptics_i2c_suspend,
-	.resume		= synaptics_i2c_resume,
 	.id_table	= synaptics_i2c_id_table,
 };
 
diff --git a/drivers/input/serio/altera_ps2.c b/drivers/input/serio/altera_ps2.c
index 7998560a1904..d363dc4571a3 100644
--- a/drivers/input/serio/altera_ps2.c
+++ b/drivers/input/serio/altera_ps2.c
@@ -19,6 +19,7 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/of.h>
 
 #define DRV_NAME "altera_ps2"
 
@@ -173,6 +174,16 @@ static int __devexit altera_ps2_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id altera_ps2_match[] = {
+	{ .compatible = "ALTR,ps2-1.0", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, altera_ps2_match);
+#else /* CONFIG_OF */
+#define altera_ps2_match NULL
+#endif /* CONFIG_OF */
+
 /*
  * Our device driver structure
  */
@@ -182,6 +193,7 @@ static struct platform_driver altera_ps2_driver = {
 	.driver	= {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
+		.of_match_table = altera_ps2_match,
 	},
 };
 
@@ -189,13 +201,12 @@ static int __init altera_ps2_init(void)
 {
 	return platform_driver_register(&altera_ps2_driver);
 }
+module_init(altera_ps2_init);
 
 static void __exit altera_ps2_exit(void)
 {
 	platform_driver_unregister(&altera_ps2_driver);
 }
-
-module_init(altera_ps2_init);
 module_exit(altera_ps2_exit);
 
 MODULE_DESCRIPTION("Altera University Program PS2 controller driver");
diff --git a/drivers/input/serio/ambakmi.c b/drivers/input/serio/ambakmi.c
index 92563a681d65..12abc50508e5 100644
--- a/drivers/input/serio/ambakmi.c
+++ b/drivers/input/serio/ambakmi.c
@@ -107,7 +107,8 @@ static void amba_kmi_close(struct serio *io)
 	clk_disable(kmi->clk);
 }
 
-static int __devinit amba_kmi_probe(struct amba_device *dev, struct amba_id *id)
+static int __devinit amba_kmi_probe(struct amba_device *dev,
+	const struct amba_id *id)
 {
 	struct amba_kmi_port *kmi;
 	struct serio *io;
diff --git a/drivers/input/serio/i8042-sparcio.h b/drivers/input/serio/i8042-sparcio.h
index c5cc4508d6df..395a9af3adcd 100644
--- a/drivers/input/serio/i8042-sparcio.h
+++ b/drivers/input/serio/i8042-sparcio.h
@@ -49,7 +49,7 @@ static inline void i8042_write_command(int val)
 #define OBP_PS2MS_NAME1		"kdmouse"
 #define OBP_PS2MS_NAME2		"mouse"
 
-static int __devinit sparc_i8042_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit sparc_i8042_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 
@@ -95,7 +95,7 @@ static const struct of_device_id sparc_i8042_match[] = {
 };
 MODULE_DEVICE_TABLE(of, sparc_i8042_match);
 
-static struct of_platform_driver sparc_i8042_driver = {
+static struct platform_driver sparc_i8042_driver = {
 	.driver = {
 		.name = "i8042",
 		.owner = THIS_MODULE,
@@ -116,7 +116,7 @@ static int __init i8042_platform_init(void)
 		if (!kbd_iobase)
 			return -ENODEV;
 	} else {
-		int err = of_register_platform_driver(&sparc_i8042_driver);
+		int err = platform_driver_register(&sparc_i8042_driver);
 		if (err)
 			return err;
 
@@ -140,7 +140,7 @@ static inline void i8042_platform_exit(void)
 	struct device_node *root = of_find_node_by_path("/");
 
 	if (strcmp(root->name, "SUNW,JavaStation-1"))
-		of_unregister_platform_driver(&sparc_i8042_driver);
+		platform_driver_unregister(&sparc_i8042_driver);
 }
 
 #else /* !CONFIG_PCI */
diff --git a/drivers/input/serio/i8042-unicore32io.h b/drivers/input/serio/i8042-unicore32io.h
new file mode 100644
index 000000000000..73f5cc124a36
--- /dev/null
+++ b/drivers/input/serio/i8042-unicore32io.h
@@ -0,0 +1,73 @@
+/*
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *	Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *	Copyright (C) 2001-2011 Guan Xuetao
+ *
+ * This program is free software; you can 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 _I8042_UNICORE32_H
+#define _I8042_UNICORE32_H
+
+#include <mach/hardware.h>
+
+/*
+ * Names.
+ */
+#define I8042_KBD_PHYS_DESC "isa0060/serio0"
+#define I8042_AUX_PHYS_DESC "isa0060/serio1"
+#define I8042_MUX_PHYS_DESC "isa0060/serio%d"
+
+/*
+ * IRQs.
+ */
+#define I8042_KBD_IRQ           IRQ_PS2_KBD
+#define I8042_AUX_IRQ           IRQ_PS2_AUX
+
+/*
+ * Register numbers.
+ */
+#define I8042_COMMAND_REG	PS2_COMMAND
+#define I8042_STATUS_REG	PS2_STATUS
+#define I8042_DATA_REG		PS2_DATA
+
+#define I8042_REGION_START	(resource_size_t)(PS2_DATA)
+#define I8042_REGION_SIZE	(resource_size_t)(16)
+
+static inline int i8042_read_data(void)
+{
+	return readb(I8042_DATA_REG);
+}
+
+static inline int i8042_read_status(void)
+{
+	return readb(I8042_STATUS_REG);
+}
+
+static inline void i8042_write_data(int val)
+{
+	writeb(val, I8042_DATA_REG);
+}
+
+static inline void i8042_write_command(int val)
+{
+	writeb(val, I8042_COMMAND_REG);
+}
+
+static inline int i8042_platform_init(void)
+{
+	if (!request_mem_region(I8042_REGION_START, I8042_REGION_SIZE, "i8042"))
+		return -EBUSY;
+
+	i8042_reset = 1;
+	return 0;
+}
+
+static inline void i8042_platform_exit(void)
+{
+	release_mem_region(I8042_REGION_START, I8042_REGION_SIZE);
+}
+
+#endif /* _I8042_UNICORE32_H */
diff --git a/drivers/input/serio/i8042.h b/drivers/input/serio/i8042.h
index ac1d759d0f55..3452708fbe3b 100644
--- a/drivers/input/serio/i8042.h
+++ b/drivers/input/serio/i8042.h
@@ -26,6 +26,8 @@
 #include "i8042-sparcio.h"
 #elif defined(CONFIG_X86) || defined(CONFIG_IA64)
 #include "i8042-x86ia64io.h"
+#elif defined(CONFIG_UNICORE32)
+#include "i8042-unicore32io.h"
 #else
 #include "i8042-io.h"
 #endif
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index 7c38d1fbabf2..ba70058e2be3 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -299,7 +299,7 @@ static int serio_queue_event(void *object, struct module *owner,
 	event->owner = owner;
 
 	list_add_tail(&event->node, &serio_event_list);
-	schedule_work(&serio_event_work);
+	queue_work(system_long_wq, &serio_event_work);
 
 out:
 	spin_unlock_irqrestore(&serio_event_lock, flags);
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index bb14449fb022..7540bafc95cf 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -232,8 +232,7 @@ static void sxps2_close(struct serio *pserio)
  * It returns 0, if the driver is bound to the PS/2 device, or a negative
  * value if there is an error.
  */
-static int __devinit xps2_of_probe(struct platform_device *ofdev,
-				   const struct of_device_id *match)
+static int __devinit xps2_of_probe(struct platform_device *ofdev)
 {
 	struct resource r_irq; /* Interrupt resources */
 	struct resource r_mem; /* IO mem resources */
@@ -361,7 +360,7 @@ static const struct of_device_id xps2_of_match[] __devinitconst = {
 };
 MODULE_DEVICE_TABLE(of, xps2_of_match);
 
-static struct of_platform_driver xps2_of_driver = {
+static struct platform_driver xps2_of_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -373,12 +372,12 @@ static struct of_platform_driver xps2_of_driver = {
 
 static int __init xps2_init(void)
 {
-	return of_register_platform_driver(&xps2_of_driver);
+	return platform_driver_register(&xps2_of_driver);
 }
 
 static void __exit xps2_cleanup(void)
 {
-	of_unregister_platform_driver(&xps2_of_driver);
+	platform_driver_unregister(&xps2_of_driver);
 }
 
 module_init(xps2_init);
diff --git a/drivers/input/sparse-keymap.c b/drivers/input/sparse-keymap.c
index 7729e547ba65..337bf51bc984 100644
--- a/drivers/input/sparse-keymap.c
+++ b/drivers/input/sparse-keymap.c
@@ -210,8 +210,8 @@ int sparse_keymap_setup(struct input_dev *dev,
 
 	dev->keycode = map;
 	dev->keycodemax = map_size;
-	dev->getkeycode_new = sparse_keymap_getkeycode;
-	dev->setkeycode_new = sparse_keymap_setkeycode;
+	dev->getkeycode = sparse_keymap_getkeycode;
+	dev->setkeycode = sparse_keymap_setkeycode;
 
 	return 0;
 
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 61834ae282e1..2fbd3d39b7eb 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -86,6 +86,18 @@ config TOUCHSCREEN_AD7879_SPI
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7879-spi.
 
+config TOUCHSCREEN_ATMEL_MXT
+	tristate "Atmel mXT I2C Touchscreen"
+	depends on I2C
+	help
+	  Say Y here if you have Atmel mXT series I2C touchscreen,
+	  such as AT42QT602240/ATMXT224, connected to your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called atmel_mxt_ts.
+
 config TOUCHSCREEN_BITSY
 	tristate "Compaq iPAQ H3600 (Bitsy) touchscreen"
 	depends on SA1100_BITSY
@@ -339,18 +351,6 @@ config TOUCHSCREEN_PENMOUNT
 	  To compile this driver as a module, choose M here: the
 	  module will be called penmount.
 
-config TOUCHSCREEN_QT602240
-	tristate "QT602240 I2C Touchscreen"
-	depends on I2C
-	help
-	  Say Y here if you have the AT42QT602240/ATMXT224 I2C touchscreen
-	  connected to your system.
-
-	  If unsure, say N.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called qt602240_ts.
-
 config TOUCHSCREEN_MIGOR
 	tristate "Renesas MIGO-R touchscreen"
 	depends on SH_MIGOR && I2C
@@ -423,6 +423,16 @@ config TOUCHSCREEN_UCB1400
 	  To compile this driver as a module, choose M here: the
 	  module will be called ucb1400_ts.
 
+config TOUCHSCREEN_WM831X
+	tristate "Support for WM831x touchscreen controllers"
+	depends on MFD_WM831X
+	help
+	  This enables support for the touchscreen controller on the WM831x
+	  series of PMICs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called wm831x-ts.
+
 config TOUCHSCREEN_WM97XX
 	tristate "Support for WM97xx AC97 touchscreen controllers"
 	depends on AC97_BUS
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 718bcc814952..756156409b83 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_TOUCHSCREEN_AD7879)	+= ad7879.o
 obj-$(CONFIG_TOUCHSCREEN_AD7879_I2C)	+= ad7879-i2c.o
 obj-$(CONFIG_TOUCHSCREEN_AD7879_SPI)	+= ad7879-spi.o
 obj-$(CONFIG_TOUCHSCREEN_ADS7846)	+= ads7846.o
+obj-$(CONFIG_TOUCHSCREEN_ATMEL_MXT)	+= atmel_mxt_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ATMEL_TSADCC)	+= atmel_tsadcc.o
 obj-$(CONFIG_TOUCHSCREEN_BITSY)		+= h3600_ts_input.o
 obj-$(CONFIG_TOUCHSCREEN_BU21013)       += bu21013_ts.o
@@ -37,7 +38,6 @@ obj-$(CONFIG_TOUCHSCREEN_HTCPEN)	+= htcpen.o
 obj-$(CONFIG_TOUCHSCREEN_USB_COMPOSITE)	+= usbtouchscreen.o
 obj-$(CONFIG_TOUCHSCREEN_PCAP)		+= pcap_ts.o
 obj-$(CONFIG_TOUCHSCREEN_PENMOUNT)	+= penmount.o
-obj-$(CONFIG_TOUCHSCREEN_QT602240)	+= qt602240_ts.o
 obj-$(CONFIG_TOUCHSCREEN_S3C2410)	+= s3c2410_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ST1232)	+= st1232.o
 obj-$(CONFIG_TOUCHSCREEN_STMPE)		+= stmpe-ts.o
@@ -48,6 +48,7 @@ obj-$(CONFIG_TOUCHSCREEN_TOUCHWIN)	+= touchwin.o
 obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
 obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001)	+= wacom_w8001.o
+obj-$(CONFIG_TOUCHSCREEN_WM831X)	+= wm831x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9712)	+= wm9712.o
diff --git a/drivers/input/touchscreen/ad7877.c b/drivers/input/touchscreen/ad7877.c
index a1952fcc083e..714d4e0f9f95 100644
--- a/drivers/input/touchscreen/ad7877.c
+++ b/drivers/input/touchscreen/ad7877.c
@@ -41,6 +41,7 @@
 #include <linux/delay.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
+#include <linux/pm.h>
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/ad7877.h>
@@ -826,39 +827,37 @@ static int __devexit ad7877_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int ad7877_suspend(struct spi_device *spi, pm_message_t message)
+#ifdef CONFIG_PM_SLEEP
+static int ad7877_suspend(struct device *dev)
 {
-	struct ad7877 *ts = dev_get_drvdata(&spi->dev);
+	struct ad7877 *ts = dev_get_drvdata(dev);
 
 	ad7877_disable(ts);
 
 	return 0;
 }
 
-static int ad7877_resume(struct spi_device *spi)
+static int ad7877_resume(struct device *dev)
 {
-	struct ad7877 *ts = dev_get_drvdata(&spi->dev);
+	struct ad7877 *ts = dev_get_drvdata(dev);
 
 	ad7877_enable(ts);
 
 	return 0;
 }
-#else
-#define ad7877_suspend NULL
-#define ad7877_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ad7877_pm, ad7877_suspend, ad7877_resume);
+
 static struct spi_driver ad7877_driver = {
 	.driver = {
 		.name	= "ad7877",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &ad7877_pm,
 	},
 	.probe		= ad7877_probe,
 	.remove		= __devexit_p(ad7877_remove),
-	.suspend	= ad7877_suspend,
-	.resume		= ad7877_resume,
 };
 
 static int __init ad7877_init(void)
diff --git a/drivers/input/touchscreen/ad7879-spi.c b/drivers/input/touchscreen/ad7879-spi.c
index 59c6e68c4325..ddf732f3cafc 100644
--- a/drivers/input/touchscreen/ad7879-spi.c
+++ b/drivers/input/touchscreen/ad7879-spi.c
@@ -7,6 +7,7 @@
  */
 
 #include <linux/input.h>	/* BUS_SPI */
+#include <linux/pm.h>
 #include <linux/spi/spi.h>
 
 #include "ad7879.h"
@@ -20,9 +21,10 @@
 #define AD7879_WRITECMD(reg) (AD7879_CMD(reg))
 #define AD7879_READCMD(reg)  (AD7879_CMD(reg) | AD7879_CMD_READ)
 
-#ifdef CONFIG_PM
-static int ad7879_spi_suspend(struct spi_device *spi, pm_message_t message)
+#ifdef CONFIG_PM_SLEEP
+static int ad7879_spi_suspend(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct ad7879 *ts = spi_get_drvdata(spi);
 
 	ad7879_suspend(ts);
@@ -30,19 +32,19 @@ static int ad7879_spi_suspend(struct spi_device *spi, pm_message_t message)
 	return 0;
 }
 
-static int ad7879_spi_resume(struct spi_device *spi)
+static int ad7879_spi_resume(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct ad7879 *ts = spi_get_drvdata(spi);
 
 	ad7879_resume(ts);
 
 	return 0;
 }
-#else
-# define ad7879_spi_suspend NULL
-# define ad7879_spi_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ad7879_spi_pm, ad7879_spi_suspend, ad7879_spi_resume);
+
 /*
  * ad7879_read/write are only used for initial setup and for sysfs controls.
  * The main traffic is done in ad7879_collect().
@@ -173,11 +175,10 @@ static struct spi_driver ad7879_spi_driver = {
 		.name	= "ad7879",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &ad7879_spi_pm,
 	},
 	.probe		= ad7879_spi_probe,
 	.remove		= __devexit_p(ad7879_spi_remove),
-	.suspend	= ad7879_spi_suspend,
-	.resume		= ad7879_spi_resume,
 };
 
 static int __init ad7879_spi_init(void)
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index 4bf2316e3284..c24946f51256 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -26,6 +26,7 @@
 #include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/pm.h>
 #include <linux/gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/ads7846.h>
@@ -892,9 +893,10 @@ static irqreturn_t ads7846_irq(int irq, void *handle)
 	return IRQ_HANDLED;
 }
 
-static int ads7846_suspend(struct spi_device *spi, pm_message_t message)
+#ifdef CONFIG_PM_SLEEP
+static int ads7846_suspend(struct device *dev)
 {
-	struct ads7846 *ts = dev_get_drvdata(&spi->dev);
+	struct ads7846 *ts = dev_get_drvdata(dev);
 
 	mutex_lock(&ts->lock);
 
@@ -914,9 +916,9 @@ static int ads7846_suspend(struct spi_device *spi, pm_message_t message)
 	return 0;
 }
 
-static int ads7846_resume(struct spi_device *spi)
+static int ads7846_resume(struct device *dev)
 {
-	struct ads7846 *ts = dev_get_drvdata(&spi->dev);
+	struct ads7846 *ts = dev_get_drvdata(dev);
 
 	mutex_lock(&ts->lock);
 
@@ -935,6 +937,9 @@ static int ads7846_resume(struct spi_device *spi)
 
 	return 0;
 }
+#endif
+
+static SIMPLE_DEV_PM_OPS(ads7846_pm, ads7846_suspend, ads7846_resume);
 
 static int __devinit ads7846_setup_pendown(struct spi_device *spi, struct ads7846 *ts)
 {
@@ -1408,11 +1413,10 @@ static struct spi_driver ads7846_driver = {
 		.name	= "ads7846",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &ads7846_pm,
 	},
 	.probe		= ads7846_probe,
 	.remove		= __devexit_p(ads7846_remove),
-	.suspend	= ads7846_suspend,
-	.resume		= ads7846_resume,
 };
 
 static int __init ads7846_init(void)
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
new file mode 100644
index 000000000000..6264ba80c38c
--- /dev/null
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -0,0 +1,1214 @@
+/*
+ * Atmel maXTouch Touchscreen driver
+ *
+ * Copyright (C) 2010 Samsung Electronics Co.Ltd
+ * Author: Joonyoung Shim <jy0922.shim@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/i2c/atmel_mxt_ts.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+
+/* Version */
+#define MXT_VER_20		20
+#define MXT_VER_21		21
+#define MXT_VER_22		22
+
+/* Slave addresses */
+#define MXT_APP_LOW		0x4a
+#define MXT_APP_HIGH		0x4b
+#define MXT_BOOT_LOW		0x24
+#define MXT_BOOT_HIGH		0x25
+
+/* Firmware */
+#define MXT_FW_NAME		"maxtouch.fw"
+
+/* Registers */
+#define MXT_FAMILY_ID		0x00
+#define MXT_VARIANT_ID		0x01
+#define MXT_VERSION		0x02
+#define MXT_BUILD		0x03
+#define MXT_MATRIX_X_SIZE	0x04
+#define MXT_MATRIX_Y_SIZE	0x05
+#define MXT_OBJECT_NUM		0x06
+#define MXT_OBJECT_START	0x07
+
+#define MXT_OBJECT_SIZE		6
+
+/* Object types */
+#define MXT_DEBUG_DIAGNOSTIC	37
+#define MXT_GEN_MESSAGE		5
+#define MXT_GEN_COMMAND		6
+#define MXT_GEN_POWER		7
+#define MXT_GEN_ACQUIRE		8
+#define MXT_TOUCH_MULTI		9
+#define MXT_TOUCH_KEYARRAY	15
+#define MXT_TOUCH_PROXIMITY	23
+#define MXT_PROCI_GRIPFACE	20
+#define MXT_PROCG_NOISE		22
+#define MXT_PROCI_ONETOUCH	24
+#define MXT_PROCI_TWOTOUCH	27
+#define MXT_SPT_COMMSCONFIG	18	/* firmware ver 21 over */
+#define MXT_SPT_GPIOPWM		19
+#define MXT_SPT_SELFTEST	25
+#define MXT_SPT_CTECONFIG	28
+#define MXT_SPT_USERDATA	38	/* firmware ver 21 over */
+
+/* MXT_GEN_COMMAND field */
+#define MXT_COMMAND_RESET	0
+#define MXT_COMMAND_BACKUPNV	1
+#define MXT_COMMAND_CALIBRATE	2
+#define MXT_COMMAND_REPORTALL	3
+#define MXT_COMMAND_DIAGNOSTIC	5
+
+/* MXT_GEN_POWER field */
+#define MXT_POWER_IDLEACQINT	0
+#define MXT_POWER_ACTVACQINT	1
+#define MXT_POWER_ACTV2IDLETO	2
+
+/* MXT_GEN_ACQUIRE field */
+#define MXT_ACQUIRE_CHRGTIME	0
+#define MXT_ACQUIRE_TCHDRIFT	2
+#define MXT_ACQUIRE_DRIFTST	3
+#define MXT_ACQUIRE_TCHAUTOCAL	4
+#define MXT_ACQUIRE_SYNC	5
+#define MXT_ACQUIRE_ATCHCALST	6
+#define MXT_ACQUIRE_ATCHCALSTHR	7
+
+/* MXT_TOUCH_MULTI field */
+#define MXT_TOUCH_CTRL		0
+#define MXT_TOUCH_XORIGIN	1
+#define MXT_TOUCH_YORIGIN	2
+#define MXT_TOUCH_XSIZE		3
+#define MXT_TOUCH_YSIZE		4
+#define MXT_TOUCH_BLEN		6
+#define MXT_TOUCH_TCHTHR	7
+#define MXT_TOUCH_TCHDI		8
+#define MXT_TOUCH_ORIENT	9
+#define MXT_TOUCH_MOVHYSTI	11
+#define MXT_TOUCH_MOVHYSTN	12
+#define MXT_TOUCH_NUMTOUCH	14
+#define MXT_TOUCH_MRGHYST	15
+#define MXT_TOUCH_MRGTHR	16
+#define MXT_TOUCH_AMPHYST	17
+#define MXT_TOUCH_XRANGE_LSB	18
+#define MXT_TOUCH_XRANGE_MSB	19
+#define MXT_TOUCH_YRANGE_LSB	20
+#define MXT_TOUCH_YRANGE_MSB	21
+#define MXT_TOUCH_XLOCLIP	22
+#define MXT_TOUCH_XHICLIP	23
+#define MXT_TOUCH_YLOCLIP	24
+#define MXT_TOUCH_YHICLIP	25
+#define MXT_TOUCH_XEDGECTRL	26
+#define MXT_TOUCH_XEDGEDIST	27
+#define MXT_TOUCH_YEDGECTRL	28
+#define MXT_TOUCH_YEDGEDIST	29
+#define MXT_TOUCH_JUMPLIMIT	30	/* firmware ver 22 over */
+
+/* MXT_PROCI_GRIPFACE field */
+#define MXT_GRIPFACE_CTRL	0
+#define MXT_GRIPFACE_XLOGRIP	1
+#define MXT_GRIPFACE_XHIGRIP	2
+#define MXT_GRIPFACE_YLOGRIP	3
+#define MXT_GRIPFACE_YHIGRIP	4
+#define MXT_GRIPFACE_MAXTCHS	5
+#define MXT_GRIPFACE_SZTHR1	7
+#define MXT_GRIPFACE_SZTHR2	8
+#define MXT_GRIPFACE_SHPTHR1	9
+#define MXT_GRIPFACE_SHPTHR2	10
+#define MXT_GRIPFACE_SUPEXTTO	11
+
+/* MXT_PROCI_NOISE field */
+#define MXT_NOISE_CTRL		0
+#define MXT_NOISE_OUTFLEN	1
+#define MXT_NOISE_GCAFUL_LSB	3
+#define MXT_NOISE_GCAFUL_MSB	4
+#define MXT_NOISE_GCAFLL_LSB	5
+#define MXT_NOISE_GCAFLL_MSB	6
+#define MXT_NOISE_ACTVGCAFVALID	7
+#define MXT_NOISE_NOISETHR	8
+#define MXT_NOISE_FREQHOPSCALE	10
+#define MXT_NOISE_FREQ0		11
+#define MXT_NOISE_FREQ1		12
+#define MXT_NOISE_FREQ2		13
+#define MXT_NOISE_FREQ3		14
+#define MXT_NOISE_FREQ4		15
+#define MXT_NOISE_IDLEGCAFVALID	16
+
+/* MXT_SPT_COMMSCONFIG */
+#define MXT_COMMS_CTRL		0
+#define MXT_COMMS_CMD		1
+
+/* MXT_SPT_CTECONFIG field */
+#define MXT_CTE_CTRL		0
+#define MXT_CTE_CMD		1
+#define MXT_CTE_MODE		2
+#define MXT_CTE_IDLEGCAFDEPTH	3
+#define MXT_CTE_ACTVGCAFDEPTH	4
+#define MXT_CTE_VOLTAGE		5	/* firmware ver 21 over */
+
+#define MXT_VOLTAGE_DEFAULT	2700000
+#define MXT_VOLTAGE_STEP	10000
+
+/* Define for MXT_GEN_COMMAND */
+#define MXT_BOOT_VALUE		0xa5
+#define MXT_BACKUP_VALUE	0x55
+#define MXT_BACKUP_TIME		25	/* msec */
+#define MXT_RESET_TIME		65	/* msec */
+
+#define MXT_FWRESET_TIME	175	/* msec */
+
+/* Command to unlock bootloader */
+#define MXT_UNLOCK_CMD_MSB	0xaa
+#define MXT_UNLOCK_CMD_LSB	0xdc
+
+/* Bootloader mode status */
+#define MXT_WAITING_BOOTLOAD_CMD	0xc0	/* valid 7 6 bit only */
+#define MXT_WAITING_FRAME_DATA	0x80	/* valid 7 6 bit only */
+#define MXT_FRAME_CRC_CHECK	0x02
+#define MXT_FRAME_CRC_FAIL	0x03
+#define MXT_FRAME_CRC_PASS	0x04
+#define MXT_APP_CRC_FAIL	0x40	/* valid 7 8 bit only */
+#define MXT_BOOT_STATUS_MASK	0x3f
+
+/* Touch status */
+#define MXT_SUPPRESS		(1 << 1)
+#define MXT_AMP			(1 << 2)
+#define MXT_VECTOR		(1 << 3)
+#define MXT_MOVE		(1 << 4)
+#define MXT_RELEASE		(1 << 5)
+#define MXT_PRESS		(1 << 6)
+#define MXT_DETECT		(1 << 7)
+
+/* Touchscreen absolute values */
+#define MXT_MAX_XC		0x3ff
+#define MXT_MAX_YC		0x3ff
+#define MXT_MAX_AREA		0xff
+
+#define MXT_MAX_FINGER		10
+
+struct mxt_info {
+	u8 family_id;
+	u8 variant_id;
+	u8 version;
+	u8 build;
+	u8 matrix_xsize;
+	u8 matrix_ysize;
+	u8 object_num;
+};
+
+struct mxt_object {
+	u8 type;
+	u16 start_address;
+	u8 size;
+	u8 instances;
+	u8 num_report_ids;
+
+	/* to map object and message */
+	u8 max_reportid;
+};
+
+struct mxt_message {
+	u8 reportid;
+	u8 message[7];
+	u8 checksum;
+};
+
+struct mxt_finger {
+	int status;
+	int x;
+	int y;
+	int area;
+};
+
+/* Each client has this additional data */
+struct mxt_data {
+	struct i2c_client *client;
+	struct input_dev *input_dev;
+	const struct mxt_platform_data *pdata;
+	struct mxt_object *object_table;
+	struct mxt_info info;
+	struct mxt_finger finger[MXT_MAX_FINGER];
+	unsigned int irq;
+};
+
+static bool mxt_object_readable(unsigned int type)
+{
+	switch (type) {
+	case MXT_GEN_MESSAGE:
+	case MXT_GEN_COMMAND:
+	case MXT_GEN_POWER:
+	case MXT_GEN_ACQUIRE:
+	case MXT_TOUCH_MULTI:
+	case MXT_TOUCH_KEYARRAY:
+	case MXT_TOUCH_PROXIMITY:
+	case MXT_PROCI_GRIPFACE:
+	case MXT_PROCG_NOISE:
+	case MXT_PROCI_ONETOUCH:
+	case MXT_PROCI_TWOTOUCH:
+	case MXT_SPT_COMMSCONFIG:
+	case MXT_SPT_GPIOPWM:
+	case MXT_SPT_SELFTEST:
+	case MXT_SPT_CTECONFIG:
+	case MXT_SPT_USERDATA:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool mxt_object_writable(unsigned int type)
+{
+	switch (type) {
+	case MXT_GEN_COMMAND:
+	case MXT_GEN_POWER:
+	case MXT_GEN_ACQUIRE:
+	case MXT_TOUCH_MULTI:
+	case MXT_TOUCH_KEYARRAY:
+	case MXT_TOUCH_PROXIMITY:
+	case MXT_PROCI_GRIPFACE:
+	case MXT_PROCG_NOISE:
+	case MXT_PROCI_ONETOUCH:
+	case MXT_PROCI_TWOTOUCH:
+	case MXT_SPT_GPIOPWM:
+	case MXT_SPT_SELFTEST:
+	case MXT_SPT_CTECONFIG:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void mxt_dump_message(struct device *dev,
+				  struct mxt_message *message)
+{
+	dev_dbg(dev, "reportid:\t0x%x\n", message->reportid);
+	dev_dbg(dev, "message1:\t0x%x\n", message->message[0]);
+	dev_dbg(dev, "message2:\t0x%x\n", message->message[1]);
+	dev_dbg(dev, "message3:\t0x%x\n", message->message[2]);
+	dev_dbg(dev, "message4:\t0x%x\n", message->message[3]);
+	dev_dbg(dev, "message5:\t0x%x\n", message->message[4]);
+	dev_dbg(dev, "message6:\t0x%x\n", message->message[5]);
+	dev_dbg(dev, "message7:\t0x%x\n", message->message[6]);
+	dev_dbg(dev, "checksum:\t0x%x\n", message->checksum);
+}
+
+static int mxt_check_bootloader(struct i2c_client *client,
+				     unsigned int state)
+{
+	u8 val;
+
+recheck:
+	if (i2c_master_recv(client, &val, 1) != 1) {
+		dev_err(&client->dev, "%s: i2c recv failed\n", __func__);
+		return -EIO;
+	}
+
+	switch (state) {
+	case MXT_WAITING_BOOTLOAD_CMD:
+	case MXT_WAITING_FRAME_DATA:
+		val &= ~MXT_BOOT_STATUS_MASK;
+		break;
+	case MXT_FRAME_CRC_PASS:
+		if (val == MXT_FRAME_CRC_CHECK)
+			goto recheck;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (val != state) {
+		dev_err(&client->dev, "Unvalid bootloader mode state\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mxt_unlock_bootloader(struct i2c_client *client)
+{
+	u8 buf[2];
+
+	buf[0] = MXT_UNLOCK_CMD_LSB;
+	buf[1] = MXT_UNLOCK_CMD_MSB;
+
+	if (i2c_master_send(client, buf, 2) != 2) {
+		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mxt_fw_write(struct i2c_client *client,
+			     const u8 *data, unsigned int frame_size)
+{
+	if (i2c_master_send(client, data, frame_size) != frame_size) {
+		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int __mxt_read_reg(struct i2c_client *client,
+			       u16 reg, u16 len, void *val)
+{
+	struct i2c_msg xfer[2];
+	u8 buf[2];
+
+	buf[0] = reg & 0xff;
+	buf[1] = (reg >> 8) & 0xff;
+
+	/* Write register */
+	xfer[0].addr = client->addr;
+	xfer[0].flags = 0;
+	xfer[0].len = 2;
+	xfer[0].buf = buf;
+
+	/* Read data */
+	xfer[1].addr = client->addr;
+	xfer[1].flags = I2C_M_RD;
+	xfer[1].len = len;
+	xfer[1].buf = val;
+
+	if (i2c_transfer(client->adapter, xfer, 2) != 2) {
+		dev_err(&client->dev, "%s: i2c transfer failed\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mxt_read_reg(struct i2c_client *client, u16 reg, u8 *val)
+{
+	return __mxt_read_reg(client, reg, 1, val);
+}
+
+static int mxt_write_reg(struct i2c_client *client, u16 reg, u8 val)
+{
+	u8 buf[3];
+
+	buf[0] = reg & 0xff;
+	buf[1] = (reg >> 8) & 0xff;
+	buf[2] = val;
+
+	if (i2c_master_send(client, buf, 3) != 3) {
+		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mxt_read_object_table(struct i2c_client *client,
+				      u16 reg, u8 *object_buf)
+{
+	return __mxt_read_reg(client, reg, MXT_OBJECT_SIZE,
+				   object_buf);
+}
+
+static struct mxt_object *
+mxt_get_object(struct mxt_data *data, u8 type)
+{
+	struct mxt_object *object;
+	int i;
+
+	for (i = 0; i < data->info.object_num; i++) {
+		object = data->object_table + i;
+		if (object->type == type)
+			return object;
+	}
+
+	dev_err(&data->client->dev, "Invalid object type\n");
+	return NULL;
+}
+
+static int mxt_read_message(struct mxt_data *data,
+				 struct mxt_message *message)
+{
+	struct mxt_object *object;
+	u16 reg;
+
+	object = mxt_get_object(data, MXT_GEN_MESSAGE);
+	if (!object)
+		return -EINVAL;
+
+	reg = object->start_address;
+	return __mxt_read_reg(data->client, reg,
+			sizeof(struct mxt_message), message);
+}
+
+static int mxt_read_object(struct mxt_data *data,
+				u8 type, u8 offset, u8 *val)
+{
+	struct mxt_object *object;
+	u16 reg;
+
+	object = mxt_get_object(data, type);
+	if (!object)
+		return -EINVAL;
+
+	reg = object->start_address;
+	return __mxt_read_reg(data->client, reg + offset, 1, val);
+}
+
+static int mxt_write_object(struct mxt_data *data,
+				 u8 type, u8 offset, u8 val)
+{
+	struct mxt_object *object;
+	u16 reg;
+
+	object = mxt_get_object(data, type);
+	if (!object)
+		return -EINVAL;
+
+	reg = object->start_address;
+	return mxt_write_reg(data->client, reg + offset, val);
+}
+
+static void mxt_input_report(struct mxt_data *data, int single_id)
+{
+	struct mxt_finger *finger = data->finger;
+	struct input_dev *input_dev = data->input_dev;
+	int status = finger[single_id].status;
+	int finger_num = 0;
+	int id;
+
+	for (id = 0; id < MXT_MAX_FINGER; id++) {
+		if (!finger[id].status)
+			continue;
+
+		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR,
+				finger[id].status != MXT_RELEASE ?
+				finger[id].area : 0);
+		input_report_abs(input_dev, ABS_MT_POSITION_X,
+				finger[id].x);
+		input_report_abs(input_dev, ABS_MT_POSITION_Y,
+				finger[id].y);
+		input_mt_sync(input_dev);
+
+		if (finger[id].status == MXT_RELEASE)
+			finger[id].status = 0;
+		else
+			finger_num++;
+	}
+
+	input_report_key(input_dev, BTN_TOUCH, finger_num > 0);
+
+	if (status != MXT_RELEASE) {
+		input_report_abs(input_dev, ABS_X, finger[single_id].x);
+		input_report_abs(input_dev, ABS_Y, finger[single_id].y);
+	}
+
+	input_sync(input_dev);
+}
+
+static void mxt_input_touchevent(struct mxt_data *data,
+				      struct mxt_message *message, int id)
+{
+	struct mxt_finger *finger = data->finger;
+	struct device *dev = &data->client->dev;
+	u8 status = message->message[0];
+	int x;
+	int y;
+	int area;
+
+	/* Check the touch is present on the screen */
+	if (!(status & MXT_DETECT)) {
+		if (status & MXT_RELEASE) {
+			dev_dbg(dev, "[%d] released\n", id);
+
+			finger[id].status = MXT_RELEASE;
+			mxt_input_report(data, id);
+		}
+		return;
+	}
+
+	/* Check only AMP detection */
+	if (!(status & (MXT_PRESS | MXT_MOVE)))
+		return;
+
+	x = (message->message[1] << 2) | ((message->message[3] & ~0x3f) >> 6);
+	y = (message->message[2] << 2) | ((message->message[3] & ~0xf3) >> 2);
+	area = message->message[4];
+
+	dev_dbg(dev, "[%d] %s x: %d, y: %d, area: %d\n", id,
+		status & MXT_MOVE ? "moved" : "pressed",
+		x, y, area);
+
+	finger[id].status = status & MXT_MOVE ?
+				MXT_MOVE : MXT_PRESS;
+	finger[id].x = x;
+	finger[id].y = y;
+	finger[id].area = area;
+
+	mxt_input_report(data, id);
+}
+
+static irqreturn_t mxt_interrupt(int irq, void *dev_id)
+{
+	struct mxt_data *data = dev_id;
+	struct mxt_message message;
+	struct mxt_object *object;
+	struct device *dev = &data->client->dev;
+	int id;
+	u8 reportid;
+	u8 max_reportid;
+	u8 min_reportid;
+
+	do {
+		if (mxt_read_message(data, &message)) {
+			dev_err(dev, "Failed to read message\n");
+			goto end;
+		}
+
+		reportid = message.reportid;
+
+		/* whether reportid is thing of MXT_TOUCH_MULTI */
+		object = mxt_get_object(data, MXT_TOUCH_MULTI);
+		if (!object)
+			goto end;
+
+		max_reportid = object->max_reportid;
+		min_reportid = max_reportid - object->num_report_ids + 1;
+		id = reportid - min_reportid;
+
+		if (reportid >= min_reportid && reportid <= max_reportid)
+			mxt_input_touchevent(data, &message, id);
+		else
+			mxt_dump_message(dev, &message);
+	} while (reportid != 0xff);
+
+end:
+	return IRQ_HANDLED;
+}
+
+static int mxt_check_reg_init(struct mxt_data *data)
+{
+	const struct mxt_platform_data *pdata = data->pdata;
+	struct mxt_object *object;
+	struct device *dev = &data->client->dev;
+	int index = 0;
+	int i, j, config_offset;
+
+	if (!pdata->config) {
+		dev_dbg(dev, "No cfg data defined, skipping reg init\n");
+		return 0;
+	}
+
+	for (i = 0; i < data->info.object_num; i++) {
+		object = data->object_table + i;
+
+		if (!mxt_object_writable(object->type))
+			continue;
+
+		for (j = 0; j < object->size + 1; j++) {
+			config_offset = index + j;
+			if (config_offset > pdata->config_length) {
+				dev_err(dev, "Not enough config data!\n");
+				return -EINVAL;
+			}
+			mxt_write_object(data, object->type, j,
+					 pdata->config[config_offset]);
+		}
+		index += object->size + 1;
+	}
+
+	return 0;
+}
+
+static int mxt_make_highchg(struct mxt_data *data)
+{
+	struct device *dev = &data->client->dev;
+	struct mxt_message message;
+	int count = 10;
+	int error;
+
+	/* Read dummy message to make high CHG pin */
+	do {
+		error = mxt_read_message(data, &message);
+		if (error)
+			return error;
+	} while (message.reportid != 0xff && --count);
+
+	if (!count) {
+		dev_err(dev, "CHG pin isn't cleared\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static void mxt_handle_pdata(struct mxt_data *data)
+{
+	const struct mxt_platform_data *pdata = data->pdata;
+	u8 voltage;
+
+	/* Set touchscreen lines */
+	mxt_write_object(data, MXT_TOUCH_MULTI, MXT_TOUCH_XSIZE,
+			pdata->x_line);
+	mxt_write_object(data, MXT_TOUCH_MULTI, MXT_TOUCH_YSIZE,
+			pdata->y_line);
+
+	/* Set touchscreen orient */
+	mxt_write_object(data, MXT_TOUCH_MULTI, MXT_TOUCH_ORIENT,
+			pdata->orient);
+
+	/* Set touchscreen burst length */
+	mxt_write_object(data, MXT_TOUCH_MULTI,
+			MXT_TOUCH_BLEN, pdata->blen);
+
+	/* Set touchscreen threshold */
+	mxt_write_object(data, MXT_TOUCH_MULTI,
+			MXT_TOUCH_TCHTHR, pdata->threshold);
+
+	/* Set touchscreen resolution */
+	mxt_write_object(data, MXT_TOUCH_MULTI,
+			MXT_TOUCH_XRANGE_LSB, (pdata->x_size - 1) & 0xff);
+	mxt_write_object(data, MXT_TOUCH_MULTI,
+			MXT_TOUCH_XRANGE_MSB, (pdata->x_size - 1) >> 8);
+	mxt_write_object(data, MXT_TOUCH_MULTI,
+			MXT_TOUCH_YRANGE_LSB, (pdata->y_size - 1) & 0xff);
+	mxt_write_object(data, MXT_TOUCH_MULTI,
+			MXT_TOUCH_YRANGE_MSB, (pdata->y_size - 1) >> 8);
+
+	/* Set touchscreen voltage */
+	if (data->info.version >= MXT_VER_21 && pdata->voltage) {
+		if (pdata->voltage < MXT_VOLTAGE_DEFAULT) {
+			voltage = (MXT_VOLTAGE_DEFAULT - pdata->voltage) /
+				MXT_VOLTAGE_STEP;
+			voltage = 0xff - voltage + 1;
+		} else
+			voltage = (pdata->voltage - MXT_VOLTAGE_DEFAULT) /
+				MXT_VOLTAGE_STEP;
+
+		mxt_write_object(data, MXT_SPT_CTECONFIG,
+				MXT_CTE_VOLTAGE, voltage);
+	}
+}
+
+static int mxt_get_info(struct mxt_data *data)
+{
+	struct i2c_client *client = data->client;
+	struct mxt_info *info = &data->info;
+	int error;
+	u8 val;
+
+	error = mxt_read_reg(client, MXT_FAMILY_ID, &val);
+	if (error)
+		return error;
+	info->family_id = val;
+
+	error = mxt_read_reg(client, MXT_VARIANT_ID, &val);
+	if (error)
+		return error;
+	info->variant_id = val;
+
+	error = mxt_read_reg(client, MXT_VERSION, &val);
+	if (error)
+		return error;
+	info->version = val;
+
+	error = mxt_read_reg(client, MXT_BUILD, &val);
+	if (error)
+		return error;
+	info->build = val;
+
+	error = mxt_read_reg(client, MXT_OBJECT_NUM, &val);
+	if (error)
+		return error;
+	info->object_num = val;
+
+	return 0;
+}
+
+static int mxt_get_object_table(struct mxt_data *data)
+{
+	int error;
+	int i;
+	u16 reg;
+	u8 reportid = 0;
+	u8 buf[MXT_OBJECT_SIZE];
+
+	for (i = 0; i < data->info.object_num; i++) {
+		struct mxt_object *object = data->object_table + i;
+
+		reg = MXT_OBJECT_START + MXT_OBJECT_SIZE * i;
+		error = mxt_read_object_table(data->client, reg, buf);
+		if (error)
+			return error;
+
+		object->type = buf[0];
+		object->start_address = (buf[2] << 8) | buf[1];
+		object->size = buf[3];
+		object->instances = buf[4];
+		object->num_report_ids = buf[5];
+
+		if (object->num_report_ids) {
+			reportid += object->num_report_ids *
+					(object->instances + 1);
+			object->max_reportid = reportid;
+		}
+	}
+
+	return 0;
+}
+
+static int mxt_initialize(struct mxt_data *data)
+{
+	struct i2c_client *client = data->client;
+	struct mxt_info *info = &data->info;
+	int error;
+	u8 val;
+
+	error = mxt_get_info(data);
+	if (error)
+		return error;
+
+	data->object_table = kcalloc(info->object_num,
+				     sizeof(struct mxt_object),
+				     GFP_KERNEL);
+	if (!data->object_table) {
+		dev_err(&client->dev, "Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Get object table information */
+	error = mxt_get_object_table(data);
+	if (error)
+		return error;
+
+	/* Check register init values */
+	error = mxt_check_reg_init(data);
+	if (error)
+		return error;
+
+	error = mxt_make_highchg(data);
+	if (error)
+		return error;
+
+	mxt_handle_pdata(data);
+
+	/* Backup to memory */
+	mxt_write_object(data, MXT_GEN_COMMAND,
+			MXT_COMMAND_BACKUPNV,
+			MXT_BACKUP_VALUE);
+	msleep(MXT_BACKUP_TIME);
+
+	/* Soft reset */
+	mxt_write_object(data, MXT_GEN_COMMAND,
+			MXT_COMMAND_RESET, 1);
+	msleep(MXT_RESET_TIME);
+
+	/* Update matrix size at info struct */
+	error = mxt_read_reg(client, MXT_MATRIX_X_SIZE, &val);
+	if (error)
+		return error;
+	info->matrix_xsize = val;
+
+	error = mxt_read_reg(client, MXT_MATRIX_Y_SIZE, &val);
+	if (error)
+		return error;
+	info->matrix_ysize = val;
+
+	dev_info(&client->dev,
+			"Family ID: %d Variant ID: %d Version: %d Build: %d\n",
+			info->family_id, info->variant_id, info->version,
+			info->build);
+
+	dev_info(&client->dev,
+			"Matrix X Size: %d Matrix Y Size: %d Object Num: %d\n",
+			info->matrix_xsize, info->matrix_ysize,
+			info->object_num);
+
+	return 0;
+}
+
+static ssize_t mxt_object_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	struct mxt_object *object;
+	int count = 0;
+	int i, j;
+	int error;
+	u8 val;
+
+	for (i = 0; i < data->info.object_num; i++) {
+		object = data->object_table + i;
+
+		count += sprintf(buf + count,
+				"Object Table Element %d(Type %d)\n",
+				i + 1, object->type);
+
+		if (!mxt_object_readable(object->type)) {
+			count += sprintf(buf + count, "\n");
+			continue;
+		}
+
+		for (j = 0; j < object->size + 1; j++) {
+			error = mxt_read_object(data,
+						object->type, j, &val);
+			if (error)
+				return error;
+
+			count += sprintf(buf + count,
+					"  Byte %d: 0x%x (%d)\n", j, val, val);
+		}
+
+		count += sprintf(buf + count, "\n");
+	}
+
+	return count;
+}
+
+static int mxt_load_fw(struct device *dev, const char *fn)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	const struct firmware *fw = NULL;
+	unsigned int frame_size;
+	unsigned int pos = 0;
+	int ret;
+
+	ret = request_firmware(&fw, fn, dev);
+	if (ret) {
+		dev_err(dev, "Unable to open firmware %s\n", fn);
+		return ret;
+	}
+
+	/* Change to the bootloader mode */
+	mxt_write_object(data, MXT_GEN_COMMAND,
+			MXT_COMMAND_RESET, MXT_BOOT_VALUE);
+	msleep(MXT_RESET_TIME);
+
+	/* Change to slave address of bootloader */
+	if (client->addr == MXT_APP_LOW)
+		client->addr = MXT_BOOT_LOW;
+	else
+		client->addr = MXT_BOOT_HIGH;
+
+	ret = mxt_check_bootloader(client, MXT_WAITING_BOOTLOAD_CMD);
+	if (ret)
+		goto out;
+
+	/* Unlock bootloader */
+	mxt_unlock_bootloader(client);
+
+	while (pos < fw->size) {
+		ret = mxt_check_bootloader(client,
+						MXT_WAITING_FRAME_DATA);
+		if (ret)
+			goto out;
+
+		frame_size = ((*(fw->data + pos) << 8) | *(fw->data + pos + 1));
+
+		/* We should add 2 at frame size as the the firmware data is not
+		 * included the CRC bytes.
+		 */
+		frame_size += 2;
+
+		/* Write one frame to device */
+		mxt_fw_write(client, fw->data + pos, frame_size);
+
+		ret = mxt_check_bootloader(client,
+						MXT_FRAME_CRC_PASS);
+		if (ret)
+			goto out;
+
+		pos += frame_size;
+
+		dev_dbg(dev, "Updated %d bytes / %zd bytes\n", pos, fw->size);
+	}
+
+out:
+	release_firmware(fw);
+
+	/* Change to slave address of application */
+	if (client->addr == MXT_BOOT_LOW)
+		client->addr = MXT_APP_LOW;
+	else
+		client->addr = MXT_APP_HIGH;
+
+	return ret;
+}
+
+static ssize_t mxt_update_fw_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	unsigned int version;
+	int error;
+
+	if (sscanf(buf, "%u", &version) != 1) {
+		dev_err(dev, "Invalid values\n");
+		return -EINVAL;
+	}
+
+	if (data->info.version < MXT_VER_21 || version < MXT_VER_21) {
+		dev_err(dev, "FW update supported starting with version 21\n");
+		return -EINVAL;
+	}
+
+	disable_irq(data->irq);
+
+	error = mxt_load_fw(dev, MXT_FW_NAME);
+	if (error) {
+		dev_err(dev, "The firmware update failed(%d)\n", error);
+		count = error;
+	} else {
+		dev_dbg(dev, "The firmware update succeeded\n");
+
+		/* Wait for reset */
+		msleep(MXT_FWRESET_TIME);
+
+		kfree(data->object_table);
+		data->object_table = NULL;
+
+		mxt_initialize(data);
+	}
+
+	enable_irq(data->irq);
+
+	return count;
+}
+
+static DEVICE_ATTR(object, 0444, mxt_object_show, NULL);
+static DEVICE_ATTR(update_fw, 0664, NULL, mxt_update_fw_store);
+
+static struct attribute *mxt_attrs[] = {
+	&dev_attr_object.attr,
+	&dev_attr_update_fw.attr,
+	NULL
+};
+
+static const struct attribute_group mxt_attr_group = {
+	.attrs = mxt_attrs,
+};
+
+static void mxt_start(struct mxt_data *data)
+{
+	/* Touch enable */
+	mxt_write_object(data,
+			MXT_TOUCH_MULTI, MXT_TOUCH_CTRL, 0x83);
+}
+
+static void mxt_stop(struct mxt_data *data)
+{
+	/* Touch disable */
+	mxt_write_object(data,
+			MXT_TOUCH_MULTI, MXT_TOUCH_CTRL, 0);
+}
+
+static int mxt_input_open(struct input_dev *dev)
+{
+	struct mxt_data *data = input_get_drvdata(dev);
+
+	mxt_start(data);
+
+	return 0;
+}
+
+static void mxt_input_close(struct input_dev *dev)
+{
+	struct mxt_data *data = input_get_drvdata(dev);
+
+	mxt_stop(data);
+}
+
+static int __devinit mxt_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	const struct mxt_platform_data *pdata = client->dev.platform_data;
+	struct mxt_data *data;
+	struct input_dev *input_dev;
+	int error;
+
+	if (!pdata)
+		return -EINVAL;
+
+	data = kzalloc(sizeof(struct mxt_data), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!data || !input_dev) {
+		dev_err(&client->dev, "Failed to allocate memory\n");
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	input_dev->name = "Atmel maXTouch Touchscreen";
+	input_dev->id.bustype = BUS_I2C;
+	input_dev->dev.parent = &client->dev;
+	input_dev->open = mxt_input_open;
+	input_dev->close = mxt_input_close;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	/* For single touch */
+	input_set_abs_params(input_dev, ABS_X,
+			     0, MXT_MAX_XC, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y,
+			     0, MXT_MAX_YC, 0, 0);
+
+	/* For multi touch */
+	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
+			     0, MXT_MAX_AREA, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
+			     0, MXT_MAX_XC, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
+			     0, MXT_MAX_YC, 0, 0);
+
+	input_set_drvdata(input_dev, data);
+
+	data->client = client;
+	data->input_dev = input_dev;
+	data->pdata = pdata;
+	data->irq = client->irq;
+
+	i2c_set_clientdata(client, data);
+
+	error = mxt_initialize(data);
+	if (error)
+		goto err_free_object;
+
+	error = request_threaded_irq(client->irq, NULL, mxt_interrupt,
+			pdata->irqflags, client->dev.driver->name, data);
+	if (error) {
+		dev_err(&client->dev, "Failed to register interrupt\n");
+		goto err_free_object;
+	}
+
+	error = input_register_device(input_dev);
+	if (error)
+		goto err_free_irq;
+
+	error = sysfs_create_group(&client->dev.kobj, &mxt_attr_group);
+	if (error)
+		goto err_unregister_device;
+
+	return 0;
+
+err_unregister_device:
+	input_unregister_device(input_dev);
+	input_dev = NULL;
+err_free_irq:
+	free_irq(client->irq, data);
+err_free_object:
+	kfree(data->object_table);
+err_free_mem:
+	input_free_device(input_dev);
+	kfree(data);
+	return error;
+}
+
+static int __devexit mxt_remove(struct i2c_client *client)
+{
+	struct mxt_data *data = i2c_get_clientdata(client);
+
+	sysfs_remove_group(&client->dev.kobj, &mxt_attr_group);
+	free_irq(data->irq, data);
+	input_unregister_device(data->input_dev);
+	kfree(data->object_table);
+	kfree(data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mxt_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mxt_data *data = i2c_get_clientdata(client);
+	struct input_dev *input_dev = data->input_dev;
+
+	mutex_lock(&input_dev->mutex);
+
+	if (input_dev->users)
+		mxt_stop(data);
+
+	mutex_unlock(&input_dev->mutex);
+
+	return 0;
+}
+
+static int mxt_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mxt_data *data = i2c_get_clientdata(client);
+	struct input_dev *input_dev = data->input_dev;
+
+	/* Soft reset */
+	mxt_write_object(data, MXT_GEN_COMMAND,
+			MXT_COMMAND_RESET, 1);
+
+	msleep(MXT_RESET_TIME);
+
+	mutex_lock(&input_dev->mutex);
+
+	if (input_dev->users)
+		mxt_start(data);
+
+	mutex_unlock(&input_dev->mutex);
+
+	return 0;
+}
+
+static const struct dev_pm_ops mxt_pm_ops = {
+	.suspend	= mxt_suspend,
+	.resume		= mxt_resume,
+};
+#endif
+
+static const struct i2c_device_id mxt_id[] = {
+	{ "qt602240_ts", 0 },
+	{ "atmel_mxt_ts", 0 },
+	{ "mXT224", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mxt_id);
+
+static struct i2c_driver mxt_driver = {
+	.driver = {
+		.name	= "atmel_mxt_ts",
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &mxt_pm_ops,
+#endif
+	},
+	.probe		= mxt_probe,
+	.remove		= __devexit_p(mxt_remove),
+	.id_table	= mxt_id,
+};
+
+static int __init mxt_init(void)
+{
+	return i2c_add_driver(&mxt_driver);
+}
+
+static void __exit mxt_exit(void)
+{
+	i2c_del_driver(&mxt_driver);
+}
+
+module_init(mxt_init);
+module_exit(mxt_exit);
+
+/* Module information */
+MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
+MODULE_DESCRIPTION("Atmel maXTouch Touchscreen driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/qt602240_ts.c b/drivers/input/touchscreen/qt602240_ts.c
deleted file mode 100644
index 4dcb0e872f6a..000000000000
--- a/drivers/input/touchscreen/qt602240_ts.c
+++ /dev/null
@@ -1,1406 +0,0 @@
-/*
- * AT42QT602240/ATMXT224 Touchscreen driver
- *
- * Copyright (C) 2010 Samsung Electronics Co.Ltd
- * Author: Joonyoung Shim <jy0922.shim@samsung.com>
- *
- * This program is free software; you can redistribute  it and/or modify it
- * under  the terms of  the GNU General  Public License as published by the
- * Free Software Foundation;  either version 2 of the  License, or (at your
- * option) any later version.
- *
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/firmware.h>
-#include <linux/i2c.h>
-#include <linux/i2c/qt602240_ts.h>
-#include <linux/input.h>
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-
-/* Version */
-#define QT602240_VER_20			20
-#define QT602240_VER_21			21
-#define QT602240_VER_22			22
-
-/* Slave addresses */
-#define QT602240_APP_LOW		0x4a
-#define QT602240_APP_HIGH		0x4b
-#define QT602240_BOOT_LOW		0x24
-#define QT602240_BOOT_HIGH		0x25
-
-/* Firmware */
-#define QT602240_FW_NAME		"qt602240.fw"
-
-/* Registers */
-#define QT602240_FAMILY_ID		0x00
-#define QT602240_VARIANT_ID		0x01
-#define QT602240_VERSION		0x02
-#define QT602240_BUILD			0x03
-#define QT602240_MATRIX_X_SIZE		0x04
-#define QT602240_MATRIX_Y_SIZE		0x05
-#define QT602240_OBJECT_NUM		0x06
-#define QT602240_OBJECT_START		0x07
-
-#define QT602240_OBJECT_SIZE		6
-
-/* Object types */
-#define QT602240_DEBUG_DIAGNOSTIC	37
-#define QT602240_GEN_MESSAGE		5
-#define QT602240_GEN_COMMAND		6
-#define QT602240_GEN_POWER		7
-#define QT602240_GEN_ACQUIRE		8
-#define QT602240_TOUCH_MULTI		9
-#define QT602240_TOUCH_KEYARRAY		15
-#define QT602240_TOUCH_PROXIMITY	23
-#define QT602240_PROCI_GRIPFACE		20
-#define QT602240_PROCG_NOISE		22
-#define QT602240_PROCI_ONETOUCH		24
-#define QT602240_PROCI_TWOTOUCH		27
-#define QT602240_SPT_COMMSCONFIG	18	/* firmware ver 21 over */
-#define QT602240_SPT_GPIOPWM		19
-#define QT602240_SPT_SELFTEST		25
-#define QT602240_SPT_CTECONFIG		28
-#define QT602240_SPT_USERDATA		38	/* firmware ver 21 over */
-
-/* QT602240_GEN_COMMAND field */
-#define QT602240_COMMAND_RESET		0
-#define QT602240_COMMAND_BACKUPNV	1
-#define QT602240_COMMAND_CALIBRATE	2
-#define QT602240_COMMAND_REPORTALL	3
-#define QT602240_COMMAND_DIAGNOSTIC	5
-
-/* QT602240_GEN_POWER field */
-#define QT602240_POWER_IDLEACQINT	0
-#define QT602240_POWER_ACTVACQINT	1
-#define QT602240_POWER_ACTV2IDLETO	2
-
-/* QT602240_GEN_ACQUIRE field */
-#define QT602240_ACQUIRE_CHRGTIME	0
-#define QT602240_ACQUIRE_TCHDRIFT	2
-#define QT602240_ACQUIRE_DRIFTST	3
-#define QT602240_ACQUIRE_TCHAUTOCAL	4
-#define QT602240_ACQUIRE_SYNC		5
-#define QT602240_ACQUIRE_ATCHCALST	6
-#define QT602240_ACQUIRE_ATCHCALSTHR	7
-
-/* QT602240_TOUCH_MULTI field */
-#define QT602240_TOUCH_CTRL		0
-#define QT602240_TOUCH_XORIGIN		1
-#define QT602240_TOUCH_YORIGIN		2
-#define QT602240_TOUCH_XSIZE		3
-#define QT602240_TOUCH_YSIZE		4
-#define QT602240_TOUCH_BLEN		6
-#define QT602240_TOUCH_TCHTHR		7
-#define QT602240_TOUCH_TCHDI		8
-#define QT602240_TOUCH_ORIENT		9
-#define QT602240_TOUCH_MOVHYSTI		11
-#define QT602240_TOUCH_MOVHYSTN		12
-#define QT602240_TOUCH_NUMTOUCH		14
-#define QT602240_TOUCH_MRGHYST		15
-#define QT602240_TOUCH_MRGTHR		16
-#define QT602240_TOUCH_AMPHYST		17
-#define QT602240_TOUCH_XRANGE_LSB	18
-#define QT602240_TOUCH_XRANGE_MSB	19
-#define QT602240_TOUCH_YRANGE_LSB	20
-#define QT602240_TOUCH_YRANGE_MSB	21
-#define QT602240_TOUCH_XLOCLIP		22
-#define QT602240_TOUCH_XHICLIP		23
-#define QT602240_TOUCH_YLOCLIP		24
-#define QT602240_TOUCH_YHICLIP		25
-#define QT602240_TOUCH_XEDGECTRL	26
-#define QT602240_TOUCH_XEDGEDIST	27
-#define QT602240_TOUCH_YEDGECTRL	28
-#define QT602240_TOUCH_YEDGEDIST	29
-#define QT602240_TOUCH_JUMPLIMIT	30	/* firmware ver 22 over */
-
-/* QT602240_PROCI_GRIPFACE field */
-#define QT602240_GRIPFACE_CTRL		0
-#define QT602240_GRIPFACE_XLOGRIP	1
-#define QT602240_GRIPFACE_XHIGRIP	2
-#define QT602240_GRIPFACE_YLOGRIP	3
-#define QT602240_GRIPFACE_YHIGRIP	4
-#define QT602240_GRIPFACE_MAXTCHS	5
-#define QT602240_GRIPFACE_SZTHR1	7
-#define QT602240_GRIPFACE_SZTHR2	8
-#define QT602240_GRIPFACE_SHPTHR1	9
-#define QT602240_GRIPFACE_SHPTHR2	10
-#define QT602240_GRIPFACE_SUPEXTTO	11
-
-/* QT602240_PROCI_NOISE field */
-#define QT602240_NOISE_CTRL		0
-#define QT602240_NOISE_OUTFLEN		1
-#define QT602240_NOISE_GCAFUL_LSB	3
-#define QT602240_NOISE_GCAFUL_MSB	4
-#define QT602240_NOISE_GCAFLL_LSB	5
-#define QT602240_NOISE_GCAFLL_MSB	6
-#define QT602240_NOISE_ACTVGCAFVALID	7
-#define QT602240_NOISE_NOISETHR		8
-#define QT602240_NOISE_FREQHOPSCALE	10
-#define QT602240_NOISE_FREQ0		11
-#define QT602240_NOISE_FREQ1		12
-#define QT602240_NOISE_FREQ2		13
-#define QT602240_NOISE_FREQ3		14
-#define QT602240_NOISE_FREQ4		15
-#define QT602240_NOISE_IDLEGCAFVALID	16
-
-/* QT602240_SPT_COMMSCONFIG */
-#define QT602240_COMMS_CTRL		0
-#define QT602240_COMMS_CMD		1
-
-/* QT602240_SPT_CTECONFIG field */
-#define QT602240_CTE_CTRL		0
-#define QT602240_CTE_CMD		1
-#define QT602240_CTE_MODE		2
-#define QT602240_CTE_IDLEGCAFDEPTH	3
-#define QT602240_CTE_ACTVGCAFDEPTH	4
-#define QT602240_CTE_VOLTAGE		5	/* firmware ver 21 over */
-
-#define QT602240_VOLTAGE_DEFAULT	2700000
-#define QT602240_VOLTAGE_STEP		10000
-
-/* Define for QT602240_GEN_COMMAND */
-#define QT602240_BOOT_VALUE		0xa5
-#define QT602240_BACKUP_VALUE		0x55
-#define QT602240_BACKUP_TIME		25	/* msec */
-#define QT602240_RESET_TIME		65	/* msec */
-
-#define QT602240_FWRESET_TIME		175	/* msec */
-
-/* Command to unlock bootloader */
-#define QT602240_UNLOCK_CMD_MSB		0xaa
-#define QT602240_UNLOCK_CMD_LSB		0xdc
-
-/* Bootloader mode status */
-#define QT602240_WAITING_BOOTLOAD_CMD	0xc0	/* valid 7 6 bit only */
-#define QT602240_WAITING_FRAME_DATA	0x80	/* valid 7 6 bit only */
-#define QT602240_FRAME_CRC_CHECK	0x02
-#define QT602240_FRAME_CRC_FAIL		0x03
-#define QT602240_FRAME_CRC_PASS		0x04
-#define QT602240_APP_CRC_FAIL		0x40	/* valid 7 8 bit only */
-#define QT602240_BOOT_STATUS_MASK	0x3f
-
-/* Touch status */
-#define QT602240_SUPPRESS		(1 << 1)
-#define QT602240_AMP			(1 << 2)
-#define QT602240_VECTOR			(1 << 3)
-#define QT602240_MOVE			(1 << 4)
-#define QT602240_RELEASE		(1 << 5)
-#define QT602240_PRESS			(1 << 6)
-#define QT602240_DETECT			(1 << 7)
-
-/* Touchscreen absolute values */
-#define QT602240_MAX_XC			0x3ff
-#define QT602240_MAX_YC			0x3ff
-#define QT602240_MAX_AREA		0xff
-
-#define QT602240_MAX_FINGER		10
-
-/* Initial register values recommended from chip vendor */
-static const u8 init_vals_ver_20[] = {
-	/* QT602240_GEN_COMMAND(6) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_GEN_POWER(7) */
-	0x20, 0xff, 0x32,
-	/* QT602240_GEN_ACQUIRE(8) */
-	0x08, 0x05, 0x05, 0x00, 0x00, 0x00, 0x05, 0x14,
-	/* QT602240_TOUCH_MULTI(9) */
-	0x00, 0x00, 0x00, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x02, 0x00,
-	0x00, 0x01, 0x01, 0x0e, 0x0a, 0x0a, 0x0a, 0x0a, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x88, 0x64,
-	/* QT602240_TOUCH_KEYARRAY(15) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00,
-	/* QT602240_SPT_GPIOPWM(19) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00,
-	/* QT602240_PROCI_GRIPFACE(20) */
-	0x00, 0x64, 0x64, 0x64, 0x64, 0x00, 0x00, 0x1e, 0x14, 0x04,
-	0x1e, 0x00,
-	/* QT602240_PROCG_NOISE(22) */
-	0x05, 0x00, 0x00, 0x19, 0x00, 0xe7, 0xff, 0x04, 0x32, 0x00,
-	0x01, 0x0a, 0x0f, 0x14, 0x00, 0x00, 0xe8,
-	/* QT602240_TOUCH_PROXIMITY(23) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00,
-	/* QT602240_PROCI_ONETOUCH(24) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_SPT_SELFTEST(25) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,
-	/* QT602240_PROCI_TWOTOUCH(27) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_SPT_CTECONFIG(28) */
-	0x00, 0x00, 0x00, 0x04, 0x08,
-};
-
-static const u8 init_vals_ver_21[] = {
-	/* QT602240_GEN_COMMAND(6) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_GEN_POWER(7) */
-	0x20, 0xff, 0x32,
-	/* QT602240_GEN_ACQUIRE(8) */
-	0x0a, 0x00, 0x05, 0x00, 0x00, 0x00, 0x09, 0x23,
-	/* QT602240_TOUCH_MULTI(9) */
-	0x00, 0x00, 0x00, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x02, 0x00,
-	0x00, 0x01, 0x01, 0x0e, 0x0a, 0x0a, 0x0a, 0x0a, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_TOUCH_KEYARRAY(15) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00,
-	/* QT602240_SPT_GPIOPWM(19) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_PROCI_GRIPFACE(20) */
-	0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x50, 0x28, 0x04,
-	0x0f, 0x0a,
-	/* QT602240_PROCG_NOISE(22) */
-	0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x23, 0x00,
-	0x00, 0x05, 0x0f, 0x19, 0x23, 0x2d, 0x03,
-	/* QT602240_TOUCH_PROXIMITY(23) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00,
-	/* QT602240_PROCI_ONETOUCH(24) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_SPT_SELFTEST(25) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,
-	/* QT602240_PROCI_TWOTOUCH(27) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_SPT_CTECONFIG(28) */
-	0x00, 0x00, 0x00, 0x08, 0x10, 0x00,
-};
-
-static const u8 init_vals_ver_22[] = {
-	/* QT602240_GEN_COMMAND(6) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_GEN_POWER(7) */
-	0x20, 0xff, 0x32,
-	/* QT602240_GEN_ACQUIRE(8) */
-	0x0a, 0x00, 0x05, 0x00, 0x00, 0x00, 0x09, 0x23,
-	/* QT602240_TOUCH_MULTI(9) */
-	0x00, 0x00, 0x00, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x02, 0x00,
-	0x00, 0x01, 0x01, 0x0e, 0x0a, 0x0a, 0x0a, 0x0a, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00,
-	/* QT602240_TOUCH_KEYARRAY(15) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00,
-	/* QT602240_SPT_GPIOPWM(19) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_PROCI_GRIPFACE(20) */
-	0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x50, 0x28, 0x04,
-	0x0f, 0x0a,
-	/* QT602240_PROCG_NOISE(22) */
-	0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x23, 0x00,
-	0x00, 0x05, 0x0f, 0x19, 0x23, 0x2d, 0x03,
-	/* QT602240_TOUCH_PROXIMITY(23) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_PROCI_ONETOUCH(24) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_SPT_SELFTEST(25) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,
-	/* QT602240_PROCI_TWOTOUCH(27) */
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	/* QT602240_SPT_CTECONFIG(28) */
-	0x00, 0x00, 0x00, 0x08, 0x10, 0x00,
-};
-
-struct qt602240_info {
-	u8 family_id;
-	u8 variant_id;
-	u8 version;
-	u8 build;
-	u8 matrix_xsize;
-	u8 matrix_ysize;
-	u8 object_num;
-};
-
-struct qt602240_object {
-	u8 type;
-	u16 start_address;
-	u8 size;
-	u8 instances;
-	u8 num_report_ids;
-
-	/* to map object and message */
-	u8 max_reportid;
-};
-
-struct qt602240_message {
-	u8 reportid;
-	u8 message[7];
-	u8 checksum;
-};
-
-struct qt602240_finger {
-	int status;
-	int x;
-	int y;
-	int area;
-};
-
-/* Each client has this additional data */
-struct qt602240_data {
-	struct i2c_client *client;
-	struct input_dev *input_dev;
-	const struct qt602240_platform_data *pdata;
-	struct qt602240_object *object_table;
-	struct qt602240_info info;
-	struct qt602240_finger finger[QT602240_MAX_FINGER];
-	unsigned int irq;
-};
-
-static bool qt602240_object_readable(unsigned int type)
-{
-	switch (type) {
-	case QT602240_GEN_MESSAGE:
-	case QT602240_GEN_COMMAND:
-	case QT602240_GEN_POWER:
-	case QT602240_GEN_ACQUIRE:
-	case QT602240_TOUCH_MULTI:
-	case QT602240_TOUCH_KEYARRAY:
-	case QT602240_TOUCH_PROXIMITY:
-	case QT602240_PROCI_GRIPFACE:
-	case QT602240_PROCG_NOISE:
-	case QT602240_PROCI_ONETOUCH:
-	case QT602240_PROCI_TWOTOUCH:
-	case QT602240_SPT_COMMSCONFIG:
-	case QT602240_SPT_GPIOPWM:
-	case QT602240_SPT_SELFTEST:
-	case QT602240_SPT_CTECONFIG:
-	case QT602240_SPT_USERDATA:
-		return true;
-	default:
-		return false;
-	}
-}
-
-static bool qt602240_object_writable(unsigned int type)
-{
-	switch (type) {
-	case QT602240_GEN_COMMAND:
-	case QT602240_GEN_POWER:
-	case QT602240_GEN_ACQUIRE:
-	case QT602240_TOUCH_MULTI:
-	case QT602240_TOUCH_KEYARRAY:
-	case QT602240_TOUCH_PROXIMITY:
-	case QT602240_PROCI_GRIPFACE:
-	case QT602240_PROCG_NOISE:
-	case QT602240_PROCI_ONETOUCH:
-	case QT602240_PROCI_TWOTOUCH:
-	case QT602240_SPT_GPIOPWM:
-	case QT602240_SPT_SELFTEST:
-	case QT602240_SPT_CTECONFIG:
-		return true;
-	default:
-		return false;
-	}
-}
-
-static void qt602240_dump_message(struct device *dev,
-				  struct qt602240_message *message)
-{
-	dev_dbg(dev, "reportid:\t0x%x\n", message->reportid);
-	dev_dbg(dev, "message1:\t0x%x\n", message->message[0]);
-	dev_dbg(dev, "message2:\t0x%x\n", message->message[1]);
-	dev_dbg(dev, "message3:\t0x%x\n", message->message[2]);
-	dev_dbg(dev, "message4:\t0x%x\n", message->message[3]);
-	dev_dbg(dev, "message5:\t0x%x\n", message->message[4]);
-	dev_dbg(dev, "message6:\t0x%x\n", message->message[5]);
-	dev_dbg(dev, "message7:\t0x%x\n", message->message[6]);
-	dev_dbg(dev, "checksum:\t0x%x\n", message->checksum);
-}
-
-static int qt602240_check_bootloader(struct i2c_client *client,
-				     unsigned int state)
-{
-	u8 val;
-
-recheck:
-	if (i2c_master_recv(client, &val, 1) != 1) {
-		dev_err(&client->dev, "%s: i2c recv failed\n", __func__);
-		return -EIO;
-	}
-
-	switch (state) {
-	case QT602240_WAITING_BOOTLOAD_CMD:
-	case QT602240_WAITING_FRAME_DATA:
-		val &= ~QT602240_BOOT_STATUS_MASK;
-		break;
-	case QT602240_FRAME_CRC_PASS:
-		if (val == QT602240_FRAME_CRC_CHECK)
-			goto recheck;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	if (val != state) {
-		dev_err(&client->dev, "Unvalid bootloader mode state\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int qt602240_unlock_bootloader(struct i2c_client *client)
-{
-	u8 buf[2];
-
-	buf[0] = QT602240_UNLOCK_CMD_LSB;
-	buf[1] = QT602240_UNLOCK_CMD_MSB;
-
-	if (i2c_master_send(client, buf, 2) != 2) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int qt602240_fw_write(struct i2c_client *client,
-			     const u8 *data, unsigned int frame_size)
-{
-	if (i2c_master_send(client, data, frame_size) != frame_size) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int __qt602240_read_reg(struct i2c_client *client,
-			       u16 reg, u16 len, void *val)
-{
-	struct i2c_msg xfer[2];
-	u8 buf[2];
-
-	buf[0] = reg & 0xff;
-	buf[1] = (reg >> 8) & 0xff;
-
-	/* Write register */
-	xfer[0].addr = client->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = 2;
-	xfer[0].buf = buf;
-
-	/* Read data */
-	xfer[1].addr = client->addr;
-	xfer[1].flags = I2C_M_RD;
-	xfer[1].len = len;
-	xfer[1].buf = val;
-
-	if (i2c_transfer(client->adapter, xfer, 2) != 2) {
-		dev_err(&client->dev, "%s: i2c transfer failed\n", __func__);
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int qt602240_read_reg(struct i2c_client *client, u16 reg, u8 *val)
-{
-	return __qt602240_read_reg(client, reg, 1, val);
-}
-
-static int qt602240_write_reg(struct i2c_client *client, u16 reg, u8 val)
-{
-	u8 buf[3];
-
-	buf[0] = reg & 0xff;
-	buf[1] = (reg >> 8) & 0xff;
-	buf[2] = val;
-
-	if (i2c_master_send(client, buf, 3) != 3) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
-	}
-
-	return 0;
-}
-
-static int qt602240_read_object_table(struct i2c_client *client,
-				      u16 reg, u8 *object_buf)
-{
-	return __qt602240_read_reg(client, reg, QT602240_OBJECT_SIZE,
-				   object_buf);
-}
-
-static struct qt602240_object *
-qt602240_get_object(struct qt602240_data *data, u8 type)
-{
-	struct qt602240_object *object;
-	int i;
-
-	for (i = 0; i < data->info.object_num; i++) {
-		object = data->object_table + i;
-		if (object->type == type)
-			return object;
-	}
-
-	dev_err(&data->client->dev, "Invalid object type\n");
-	return NULL;
-}
-
-static int qt602240_read_message(struct qt602240_data *data,
-				 struct qt602240_message *message)
-{
-	struct qt602240_object *object;
-	u16 reg;
-
-	object = qt602240_get_object(data, QT602240_GEN_MESSAGE);
-	if (!object)
-		return -EINVAL;
-
-	reg = object->start_address;
-	return __qt602240_read_reg(data->client, reg,
-			sizeof(struct qt602240_message), message);
-}
-
-static int qt602240_read_object(struct qt602240_data *data,
-				u8 type, u8 offset, u8 *val)
-{
-	struct qt602240_object *object;
-	u16 reg;
-
-	object = qt602240_get_object(data, type);
-	if (!object)
-		return -EINVAL;
-
-	reg = object->start_address;
-	return __qt602240_read_reg(data->client, reg + offset, 1, val);
-}
-
-static int qt602240_write_object(struct qt602240_data *data,
-				 u8 type, u8 offset, u8 val)
-{
-	struct qt602240_object *object;
-	u16 reg;
-
-	object = qt602240_get_object(data, type);
-	if (!object)
-		return -EINVAL;
-
-	reg = object->start_address;
-	return qt602240_write_reg(data->client, reg + offset, val);
-}
-
-static void qt602240_input_report(struct qt602240_data *data, int single_id)
-{
-	struct qt602240_finger *finger = data->finger;
-	struct input_dev *input_dev = data->input_dev;
-	int status = finger[single_id].status;
-	int finger_num = 0;
-	int id;
-
-	for (id = 0; id < QT602240_MAX_FINGER; id++) {
-		if (!finger[id].status)
-			continue;
-
-		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR,
-				finger[id].status != QT602240_RELEASE ?
-				finger[id].area : 0);
-		input_report_abs(input_dev, ABS_MT_POSITION_X,
-				finger[id].x);
-		input_report_abs(input_dev, ABS_MT_POSITION_Y,
-				finger[id].y);
-		input_mt_sync(input_dev);
-
-		if (finger[id].status == QT602240_RELEASE)
-			finger[id].status = 0;
-		else
-			finger_num++;
-	}
-
-	input_report_key(input_dev, BTN_TOUCH, finger_num > 0);
-
-	if (status != QT602240_RELEASE) {
-		input_report_abs(input_dev, ABS_X, finger[single_id].x);
-		input_report_abs(input_dev, ABS_Y, finger[single_id].y);
-	}
-
-	input_sync(input_dev);
-}
-
-static void qt602240_input_touchevent(struct qt602240_data *data,
-				      struct qt602240_message *message, int id)
-{
-	struct qt602240_finger *finger = data->finger;
-	struct device *dev = &data->client->dev;
-	u8 status = message->message[0];
-	int x;
-	int y;
-	int area;
-
-	/* Check the touch is present on the screen */
-	if (!(status & QT602240_DETECT)) {
-		if (status & QT602240_RELEASE) {
-			dev_dbg(dev, "[%d] released\n", id);
-
-			finger[id].status = QT602240_RELEASE;
-			qt602240_input_report(data, id);
-		}
-		return;
-	}
-
-	/* Check only AMP detection */
-	if (!(status & (QT602240_PRESS | QT602240_MOVE)))
-		return;
-
-	x = (message->message[1] << 2) | ((message->message[3] & ~0x3f) >> 6);
-	y = (message->message[2] << 2) | ((message->message[3] & ~0xf3) >> 2);
-	area = message->message[4];
-
-	dev_dbg(dev, "[%d] %s x: %d, y: %d, area: %d\n", id,
-		status & QT602240_MOVE ? "moved" : "pressed",
-		x, y, area);
-
-	finger[id].status = status & QT602240_MOVE ?
-				QT602240_MOVE : QT602240_PRESS;
-	finger[id].x = x;
-	finger[id].y = y;
-	finger[id].area = area;
-
-	qt602240_input_report(data, id);
-}
-
-static irqreturn_t qt602240_interrupt(int irq, void *dev_id)
-{
-	struct qt602240_data *data = dev_id;
-	struct qt602240_message message;
-	struct qt602240_object *object;
-	struct device *dev = &data->client->dev;
-	int id;
-	u8 reportid;
-	u8 max_reportid;
-	u8 min_reportid;
-
-	do {
-		if (qt602240_read_message(data, &message)) {
-			dev_err(dev, "Failed to read message\n");
-			goto end;
-		}
-
-		reportid = message.reportid;
-
-		/* whether reportid is thing of QT602240_TOUCH_MULTI */
-		object = qt602240_get_object(data, QT602240_TOUCH_MULTI);
-		if (!object)
-			goto end;
-
-		max_reportid = object->max_reportid;
-		min_reportid = max_reportid - object->num_report_ids + 1;
-		id = reportid - min_reportid;
-
-		if (reportid >= min_reportid && reportid <= max_reportid)
-			qt602240_input_touchevent(data, &message, id);
-		else
-			qt602240_dump_message(dev, &message);
-	} while (reportid != 0xff);
-
-end:
-	return IRQ_HANDLED;
-}
-
-static int qt602240_check_reg_init(struct qt602240_data *data)
-{
-	struct qt602240_object *object;
-	struct device *dev = &data->client->dev;
-	int index = 0;
-	int i, j;
-	u8 version = data->info.version;
-	u8 *init_vals;
-
-	switch (version) {
-	case QT602240_VER_20:
-		init_vals = (u8 *)init_vals_ver_20;
-		break;
-	case QT602240_VER_21:
-		init_vals = (u8 *)init_vals_ver_21;
-		break;
-	case QT602240_VER_22:
-		init_vals = (u8 *)init_vals_ver_22;
-		break;
-	default:
-		dev_err(dev, "Firmware version %d doesn't support\n", version);
-		return -EINVAL;
-	}
-
-	for (i = 0; i < data->info.object_num; i++) {
-		object = data->object_table + i;
-
-		if (!qt602240_object_writable(object->type))
-			continue;
-
-		for (j = 0; j < object->size + 1; j++)
-			qt602240_write_object(data, object->type, j,
-					init_vals[index + j]);
-
-		index += object->size + 1;
-	}
-
-	return 0;
-}
-
-static int qt602240_check_matrix_size(struct qt602240_data *data)
-{
-	const struct qt602240_platform_data *pdata = data->pdata;
-	struct device *dev = &data->client->dev;
-	int mode = -1;
-	int error;
-	u8 val;
-
-	dev_dbg(dev, "Number of X lines: %d\n", pdata->x_line);
-	dev_dbg(dev, "Number of Y lines: %d\n", pdata->y_line);
-
-	switch (pdata->x_line) {
-	case 0 ... 15:
-		if (pdata->y_line <= 14)
-			mode = 0;
-		break;
-	case 16:
-		if (pdata->y_line <= 12)
-			mode = 1;
-		if (pdata->y_line == 13 || pdata->y_line == 14)
-			mode = 0;
-		break;
-	case 17:
-		if (pdata->y_line <= 11)
-			mode = 2;
-		if (pdata->y_line == 12 || pdata->y_line == 13)
-			mode = 1;
-		break;
-	case 18:
-		if (pdata->y_line <= 10)
-			mode = 3;
-		if (pdata->y_line == 11 || pdata->y_line == 12)
-			mode = 2;
-		break;
-	case 19:
-		if (pdata->y_line <= 9)
-			mode = 4;
-		if (pdata->y_line == 10 || pdata->y_line == 11)
-			mode = 3;
-		break;
-	case 20:
-		mode = 4;
-	}
-
-	if (mode < 0) {
-		dev_err(dev, "Invalid X/Y lines\n");
-		return -EINVAL;
-	}
-
-	error = qt602240_read_object(data, QT602240_SPT_CTECONFIG,
-				QT602240_CTE_MODE, &val);
-	if (error)
-		return error;
-
-	if (mode == val)
-		return 0;
-
-	/* Change the CTE configuration */
-	qt602240_write_object(data, QT602240_SPT_CTECONFIG,
-			QT602240_CTE_CTRL, 1);
-	qt602240_write_object(data, QT602240_SPT_CTECONFIG,
-			QT602240_CTE_MODE, mode);
-	qt602240_write_object(data, QT602240_SPT_CTECONFIG,
-			QT602240_CTE_CTRL, 0);
-
-	return 0;
-}
-
-static int qt602240_make_highchg(struct qt602240_data *data)
-{
-	struct device *dev = &data->client->dev;
-	int count = 10;
-	int error;
-	u8 val;
-
-	/* Read dummy message to make high CHG pin */
-	do {
-		error = qt602240_read_object(data, QT602240_GEN_MESSAGE, 0, &val);
-		if (error)
-			return error;
-	} while ((val != 0xff) && --count);
-
-	if (!count) {
-		dev_err(dev, "CHG pin isn't cleared\n");
-		return -EBUSY;
-	}
-
-	return 0;
-}
-
-static void qt602240_handle_pdata(struct qt602240_data *data)
-{
-	const struct qt602240_platform_data *pdata = data->pdata;
-	u8 voltage;
-
-	/* Set touchscreen lines */
-	qt602240_write_object(data, QT602240_TOUCH_MULTI, QT602240_TOUCH_XSIZE,
-			pdata->x_line);
-	qt602240_write_object(data, QT602240_TOUCH_MULTI, QT602240_TOUCH_YSIZE,
-			pdata->y_line);
-
-	/* Set touchscreen orient */
-	qt602240_write_object(data, QT602240_TOUCH_MULTI, QT602240_TOUCH_ORIENT,
-			pdata->orient);
-
-	/* Set touchscreen burst length */
-	qt602240_write_object(data, QT602240_TOUCH_MULTI,
-			QT602240_TOUCH_BLEN, pdata->blen);
-
-	/* Set touchscreen threshold */
-	qt602240_write_object(data, QT602240_TOUCH_MULTI,
-			QT602240_TOUCH_TCHTHR, pdata->threshold);
-
-	/* Set touchscreen resolution */
-	qt602240_write_object(data, QT602240_TOUCH_MULTI,
-			QT602240_TOUCH_XRANGE_LSB, (pdata->x_size - 1) & 0xff);
-	qt602240_write_object(data, QT602240_TOUCH_MULTI,
-			QT602240_TOUCH_XRANGE_MSB, (pdata->x_size - 1) >> 8);
-	qt602240_write_object(data, QT602240_TOUCH_MULTI,
-			QT602240_TOUCH_YRANGE_LSB, (pdata->y_size - 1) & 0xff);
-	qt602240_write_object(data, QT602240_TOUCH_MULTI,
-			QT602240_TOUCH_YRANGE_MSB, (pdata->y_size - 1) >> 8);
-
-	/* Set touchscreen voltage */
-	if (data->info.version >= QT602240_VER_21 && pdata->voltage) {
-		if (pdata->voltage < QT602240_VOLTAGE_DEFAULT) {
-			voltage = (QT602240_VOLTAGE_DEFAULT - pdata->voltage) /
-				QT602240_VOLTAGE_STEP;
-			voltage = 0xff - voltage + 1;
-		} else
-			voltage = (pdata->voltage - QT602240_VOLTAGE_DEFAULT) /
-				QT602240_VOLTAGE_STEP;
-
-		qt602240_write_object(data, QT602240_SPT_CTECONFIG,
-				QT602240_CTE_VOLTAGE, voltage);
-	}
-}
-
-static int qt602240_get_info(struct qt602240_data *data)
-{
-	struct i2c_client *client = data->client;
-	struct qt602240_info *info = &data->info;
-	int error;
-	u8 val;
-
-	error = qt602240_read_reg(client, QT602240_FAMILY_ID, &val);
-	if (error)
-		return error;
-	info->family_id = val;
-
-	error = qt602240_read_reg(client, QT602240_VARIANT_ID, &val);
-	if (error)
-		return error;
-	info->variant_id = val;
-
-	error = qt602240_read_reg(client, QT602240_VERSION, &val);
-	if (error)
-		return error;
-	info->version = val;
-
-	error = qt602240_read_reg(client, QT602240_BUILD, &val);
-	if (error)
-		return error;
-	info->build = val;
-
-	error = qt602240_read_reg(client, QT602240_OBJECT_NUM, &val);
-	if (error)
-		return error;
-	info->object_num = val;
-
-	return 0;
-}
-
-static int qt602240_get_object_table(struct qt602240_data *data)
-{
-	int error;
-	int i;
-	u16 reg;
-	u8 reportid = 0;
-	u8 buf[QT602240_OBJECT_SIZE];
-
-	for (i = 0; i < data->info.object_num; i++) {
-		struct qt602240_object *object = data->object_table + i;
-
-		reg = QT602240_OBJECT_START + QT602240_OBJECT_SIZE * i;
-		error = qt602240_read_object_table(data->client, reg, buf);
-		if (error)
-			return error;
-
-		object->type = buf[0];
-		object->start_address = (buf[2] << 8) | buf[1];
-		object->size = buf[3];
-		object->instances = buf[4];
-		object->num_report_ids = buf[5];
-
-		if (object->num_report_ids) {
-			reportid += object->num_report_ids *
-					(object->instances + 1);
-			object->max_reportid = reportid;
-		}
-	}
-
-	return 0;
-}
-
-static int qt602240_initialize(struct qt602240_data *data)
-{
-	struct i2c_client *client = data->client;
-	struct qt602240_info *info = &data->info;
-	int error;
-	u8 val;
-
-	error = qt602240_get_info(data);
-	if (error)
-		return error;
-
-	data->object_table = kcalloc(info->object_num,
-				     sizeof(struct qt602240_object),
-				     GFP_KERNEL);
-	if (!data->object_table) {
-		dev_err(&client->dev, "Failed to allocate memory\n");
-		return -ENOMEM;
-	}
-
-	/* Get object table information */
-	error = qt602240_get_object_table(data);
-	if (error)
-		return error;
-
-	/* Check register init values */
-	error = qt602240_check_reg_init(data);
-	if (error)
-		return error;
-
-	/* Check X/Y matrix size */
-	error = qt602240_check_matrix_size(data);
-	if (error)
-		return error;
-
-	error = qt602240_make_highchg(data);
-	if (error)
-		return error;
-
-	qt602240_handle_pdata(data);
-
-	/* Backup to memory */
-	qt602240_write_object(data, QT602240_GEN_COMMAND,
-			QT602240_COMMAND_BACKUPNV,
-			QT602240_BACKUP_VALUE);
-	msleep(QT602240_BACKUP_TIME);
-
-	/* Soft reset */
-	qt602240_write_object(data, QT602240_GEN_COMMAND,
-			QT602240_COMMAND_RESET, 1);
-	msleep(QT602240_RESET_TIME);
-
-	/* Update matrix size at info struct */
-	error = qt602240_read_reg(client, QT602240_MATRIX_X_SIZE, &val);
-	if (error)
-		return error;
-	info->matrix_xsize = val;
-
-	error = qt602240_read_reg(client, QT602240_MATRIX_Y_SIZE, &val);
-	if (error)
-		return error;
-	info->matrix_ysize = val;
-
-	dev_info(&client->dev,
-			"Family ID: %d Variant ID: %d Version: %d Build: %d\n",
-			info->family_id, info->variant_id, info->version,
-			info->build);
-
-	dev_info(&client->dev,
-			"Matrix X Size: %d Matrix Y Size: %d Object Num: %d\n",
-			info->matrix_xsize, info->matrix_ysize,
-			info->object_num);
-
-	return 0;
-}
-
-static ssize_t qt602240_object_show(struct device *dev,
-				    struct device_attribute *attr, char *buf)
-{
-	struct qt602240_data *data = dev_get_drvdata(dev);
-	struct qt602240_object *object;
-	int count = 0;
-	int i, j;
-	int error;
-	u8 val;
-
-	for (i = 0; i < data->info.object_num; i++) {
-		object = data->object_table + i;
-
-		count += sprintf(buf + count,
-				"Object Table Element %d(Type %d)\n",
-				i + 1, object->type);
-
-		if (!qt602240_object_readable(object->type)) {
-			count += sprintf(buf + count, "\n");
-			continue;
-		}
-
-		for (j = 0; j < object->size + 1; j++) {
-			error = qt602240_read_object(data,
-						object->type, j, &val);
-			if (error)
-				return error;
-
-			count += sprintf(buf + count,
-					"  Byte %d: 0x%x (%d)\n", j, val, val);
-		}
-
-		count += sprintf(buf + count, "\n");
-	}
-
-	return count;
-}
-
-static int qt602240_load_fw(struct device *dev, const char *fn)
-{
-	struct qt602240_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	const struct firmware *fw = NULL;
-	unsigned int frame_size;
-	unsigned int pos = 0;
-	int ret;
-
-	ret = request_firmware(&fw, fn, dev);
-	if (ret) {
-		dev_err(dev, "Unable to open firmware %s\n", fn);
-		return ret;
-	}
-
-	/* Change to the bootloader mode */
-	qt602240_write_object(data, QT602240_GEN_COMMAND,
-			QT602240_COMMAND_RESET, QT602240_BOOT_VALUE);
-	msleep(QT602240_RESET_TIME);
-
-	/* Change to slave address of bootloader */
-	if (client->addr == QT602240_APP_LOW)
-		client->addr = QT602240_BOOT_LOW;
-	else
-		client->addr = QT602240_BOOT_HIGH;
-
-	ret = qt602240_check_bootloader(client, QT602240_WAITING_BOOTLOAD_CMD);
-	if (ret)
-		goto out;
-
-	/* Unlock bootloader */
-	qt602240_unlock_bootloader(client);
-
-	while (pos < fw->size) {
-		ret = qt602240_check_bootloader(client,
-						QT602240_WAITING_FRAME_DATA);
-		if (ret)
-			goto out;
-
-		frame_size = ((*(fw->data + pos) << 8) | *(fw->data + pos + 1));
-
-		/* We should add 2 at frame size as the the firmware data is not
-		 * included the CRC bytes.
-		 */
-		frame_size += 2;
-
-		/* Write one frame to device */
-		qt602240_fw_write(client, fw->data + pos, frame_size);
-
-		ret = qt602240_check_bootloader(client,
-						QT602240_FRAME_CRC_PASS);
-		if (ret)
-			goto out;
-
-		pos += frame_size;
-
-		dev_dbg(dev, "Updated %d bytes / %zd bytes\n", pos, fw->size);
-	}
-
-out:
-	release_firmware(fw);
-
-	/* Change to slave address of application */
-	if (client->addr == QT602240_BOOT_LOW)
-		client->addr = QT602240_APP_LOW;
-	else
-		client->addr = QT602240_APP_HIGH;
-
-	return ret;
-}
-
-static ssize_t qt602240_update_fw_store(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf, size_t count)
-{
-	struct qt602240_data *data = dev_get_drvdata(dev);
-	unsigned int version;
-	int error;
-
-	if (sscanf(buf, "%u", &version) != 1) {
-		dev_err(dev, "Invalid values\n");
-		return -EINVAL;
-	}
-
-	if (data->info.version < QT602240_VER_21 || version < QT602240_VER_21) {
-		dev_err(dev, "FW update supported starting with version 21\n");
-		return -EINVAL;
-	}
-
-	disable_irq(data->irq);
-
-	error = qt602240_load_fw(dev, QT602240_FW_NAME);
-	if (error) {
-		dev_err(dev, "The firmware update failed(%d)\n", error);
-		count = error;
-	} else {
-		dev_dbg(dev, "The firmware update succeeded\n");
-
-		/* Wait for reset */
-		msleep(QT602240_FWRESET_TIME);
-
-		kfree(data->object_table);
-		data->object_table = NULL;
-
-		qt602240_initialize(data);
-	}
-
-	enable_irq(data->irq);
-
-	return count;
-}
-
-static DEVICE_ATTR(object, 0444, qt602240_object_show, NULL);
-static DEVICE_ATTR(update_fw, 0664, NULL, qt602240_update_fw_store);
-
-static struct attribute *qt602240_attrs[] = {
-	&dev_attr_object.attr,
-	&dev_attr_update_fw.attr,
-	NULL
-};
-
-static const struct attribute_group qt602240_attr_group = {
-	.attrs = qt602240_attrs,
-};
-
-static void qt602240_start(struct qt602240_data *data)
-{
-	/* Touch enable */
-	qt602240_write_object(data,
-			QT602240_TOUCH_MULTI, QT602240_TOUCH_CTRL, 0x83);
-}
-
-static void qt602240_stop(struct qt602240_data *data)
-{
-	/* Touch disable */
-	qt602240_write_object(data,
-			QT602240_TOUCH_MULTI, QT602240_TOUCH_CTRL, 0);
-}
-
-static int qt602240_input_open(struct input_dev *dev)
-{
-	struct qt602240_data *data = input_get_drvdata(dev);
-
-	qt602240_start(data);
-
-	return 0;
-}
-
-static void qt602240_input_close(struct input_dev *dev)
-{
-	struct qt602240_data *data = input_get_drvdata(dev);
-
-	qt602240_stop(data);
-}
-
-static int __devinit qt602240_probe(struct i2c_client *client,
-		const struct i2c_device_id *id)
-{
-	struct qt602240_data *data;
-	struct input_dev *input_dev;
-	int error;
-
-	if (!client->dev.platform_data)
-		return -EINVAL;
-
-	data = kzalloc(sizeof(struct qt602240_data), GFP_KERNEL);
-	input_dev = input_allocate_device();
-	if (!data || !input_dev) {
-		dev_err(&client->dev, "Failed to allocate memory\n");
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
-
-	input_dev->name = "AT42QT602240/ATMXT224 Touchscreen";
-	input_dev->id.bustype = BUS_I2C;
-	input_dev->dev.parent = &client->dev;
-	input_dev->open = qt602240_input_open;
-	input_dev->close = qt602240_input_close;
-
-	__set_bit(EV_ABS, input_dev->evbit);
-	__set_bit(EV_KEY, input_dev->evbit);
-	__set_bit(BTN_TOUCH, input_dev->keybit);
-
-	/* For single touch */
-	input_set_abs_params(input_dev, ABS_X,
-			     0, QT602240_MAX_XC, 0, 0);
-	input_set_abs_params(input_dev, ABS_Y,
-			     0, QT602240_MAX_YC, 0, 0);
-
-	/* For multi touch */
-	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
-			     0, QT602240_MAX_AREA, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
-			     0, QT602240_MAX_XC, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
-			     0, QT602240_MAX_YC, 0, 0);
-
-	input_set_drvdata(input_dev, data);
-
-	data->client = client;
-	data->input_dev = input_dev;
-	data->pdata = client->dev.platform_data;
-	data->irq = client->irq;
-
-	i2c_set_clientdata(client, data);
-
-	error = qt602240_initialize(data);
-	if (error)
-		goto err_free_object;
-
-	error = request_threaded_irq(client->irq, NULL, qt602240_interrupt,
-			IRQF_TRIGGER_FALLING, client->dev.driver->name, data);
-	if (error) {
-		dev_err(&client->dev, "Failed to register interrupt\n");
-		goto err_free_object;
-	}
-
-	error = input_register_device(input_dev);
-	if (error)
-		goto err_free_irq;
-
-	error = sysfs_create_group(&client->dev.kobj, &qt602240_attr_group);
-	if (error)
-		goto err_unregister_device;
-
-	return 0;
-
-err_unregister_device:
-	input_unregister_device(input_dev);
-	input_dev = NULL;
-err_free_irq:
-	free_irq(client->irq, data);
-err_free_object:
-	kfree(data->object_table);
-err_free_mem:
-	input_free_device(input_dev);
-	kfree(data);
-	return error;
-}
-
-static int __devexit qt602240_remove(struct i2c_client *client)
-{
-	struct qt602240_data *data = i2c_get_clientdata(client);
-
-	sysfs_remove_group(&client->dev.kobj, &qt602240_attr_group);
-	free_irq(data->irq, data);
-	input_unregister_device(data->input_dev);
-	kfree(data->object_table);
-	kfree(data);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int qt602240_suspend(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct qt602240_data *data = i2c_get_clientdata(client);
-	struct input_dev *input_dev = data->input_dev;
-
-	mutex_lock(&input_dev->mutex);
-
-	if (input_dev->users)
-		qt602240_stop(data);
-
-	mutex_unlock(&input_dev->mutex);
-
-	return 0;
-}
-
-static int qt602240_resume(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct qt602240_data *data = i2c_get_clientdata(client);
-	struct input_dev *input_dev = data->input_dev;
-
-	/* Soft reset */
-	qt602240_write_object(data, QT602240_GEN_COMMAND,
-			QT602240_COMMAND_RESET, 1);
-
-	msleep(QT602240_RESET_TIME);
-
-	mutex_lock(&input_dev->mutex);
-
-	if (input_dev->users)
-		qt602240_start(data);
-
-	mutex_unlock(&input_dev->mutex);
-
-	return 0;
-}
-
-static const struct dev_pm_ops qt602240_pm_ops = {
-	.suspend	= qt602240_suspend,
-	.resume		= qt602240_resume,
-};
-#endif
-
-static const struct i2c_device_id qt602240_id[] = {
-	{ "qt602240_ts", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, qt602240_id);
-
-static struct i2c_driver qt602240_driver = {
-	.driver = {
-		.name	= "qt602240_ts",
-		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
-		.pm	= &qt602240_pm_ops,
-#endif
-	},
-	.probe		= qt602240_probe,
-	.remove		= __devexit_p(qt602240_remove),
-	.id_table	= qt602240_id,
-};
-
-static int __init qt602240_init(void)
-{
-	return i2c_add_driver(&qt602240_driver);
-}
-
-static void __exit qt602240_exit(void)
-{
-	i2c_del_driver(&qt602240_driver);
-}
-
-module_init(qt602240_init);
-module_exit(qt602240_exit);
-
-/* Module information */
-MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
-MODULE_DESCRIPTION("AT42QT602240/ATMXT224 Touchscreen driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/tps6507x-ts.c b/drivers/input/touchscreen/tps6507x-ts.c
index c8c136cf7bbc..43031492d733 100644
--- a/drivers/input/touchscreen/tps6507x-ts.c
+++ b/drivers/input/touchscreen/tps6507x-ts.c
@@ -43,7 +43,6 @@ struct tps6507x_ts {
 	struct input_dev	*input_dev;
 	struct device		*dev;
 	char			phys[32];
-	struct workqueue_struct *wq;
 	struct delayed_work	work;
 	unsigned		polling;	/* polling is active */
 	struct ts_event		tc;
@@ -220,8 +219,8 @@ done:
 	poll = 1;
 
 	if (poll) {
-		schd = queue_delayed_work(tsc->wq, &tsc->work,
-					  msecs_to_jiffies(tsc->poll_period));
+		schd = schedule_delayed_work(&tsc->work,
+					msecs_to_jiffies(tsc->poll_period));
 		if (schd)
 			tsc->polling = 1;
 		else {
@@ -303,7 +302,6 @@ static int tps6507x_ts_probe(struct platform_device *pdev)
 	tsc->input_dev = input_dev;
 
 	INIT_DELAYED_WORK(&tsc->work, tps6507x_ts_handler);
-	tsc->wq = create_workqueue("TPS6507x Touchscreen");
 
 	if (init_data) {
 		tsc->poll_period = init_data->poll_period;
@@ -325,8 +323,8 @@ static int tps6507x_ts_probe(struct platform_device *pdev)
 	if (error)
 		goto err2;
 
-	schd = queue_delayed_work(tsc->wq, &tsc->work,
-				  msecs_to_jiffies(tsc->poll_period));
+	schd = schedule_delayed_work(&tsc->work,
+				     msecs_to_jiffies(tsc->poll_period));
 
 	if (schd)
 		tsc->polling = 1;
@@ -341,7 +339,6 @@ static int tps6507x_ts_probe(struct platform_device *pdev)
 
 err2:
 	cancel_delayed_work_sync(&tsc->work);
-	destroy_workqueue(tsc->wq);
 	input_free_device(input_dev);
 err1:
 	kfree(tsc);
@@ -357,7 +354,6 @@ static int __devexit tps6507x_ts_remove(struct platform_device *pdev)
 	struct input_dev *input_dev = tsc->input_dev;
 
 	cancel_delayed_work_sync(&tsc->work);
-	destroy_workqueue(tsc->wq);
 
 	input_unregister_device(input_dev);
 
diff --git a/drivers/input/touchscreen/wm831x-ts.c b/drivers/input/touchscreen/wm831x-ts.c
new file mode 100644
index 000000000000..1022f715d3c2
--- /dev/null
+++ b/drivers/input/touchscreen/wm831x-ts.c
@@ -0,0 +1,363 @@
+/*
+ * Touchscreen driver for WM831x PMICs
+ *
+ * Copyright 2011 Wolfson Microelectronics plc.
+ * Author: Mark Brown <broonie@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 as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/pm.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/mfd/wm831x/core.h>
+#include <linux/mfd/wm831x/irq.h>
+#include <linux/mfd/wm831x/pdata.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+/*
+ * R16424 (0x4028) - Touch Control 1
+ */
+#define WM831X_TCH_ENA                          0x8000  /* TCH_ENA */
+#define WM831X_TCH_CVT_ENA                      0x4000  /* TCH_CVT_ENA */
+#define WM831X_TCH_SLPENA                       0x1000  /* TCH_SLPENA */
+#define WM831X_TCH_Z_ENA                        0x0400  /* TCH_Z_ENA */
+#define WM831X_TCH_Y_ENA                        0x0200  /* TCH_Y_ENA */
+#define WM831X_TCH_X_ENA                        0x0100  /* TCH_X_ENA */
+#define WM831X_TCH_DELAY_MASK                   0x00E0  /* TCH_DELAY - [7:5] */
+#define WM831X_TCH_DELAY_SHIFT                       5  /* TCH_DELAY - [7:5] */
+#define WM831X_TCH_DELAY_WIDTH                       3  /* TCH_DELAY - [7:5] */
+#define WM831X_TCH_RATE_MASK                    0x001F  /* TCH_RATE - [4:0] */
+#define WM831X_TCH_RATE_SHIFT                        0  /* TCH_RATE - [4:0] */
+#define WM831X_TCH_RATE_WIDTH                        5  /* TCH_RATE - [4:0] */
+
+/*
+ * R16425 (0x4029) - Touch Control 2
+ */
+#define WM831X_TCH_PD_WK                        0x2000  /* TCH_PD_WK */
+#define WM831X_TCH_5WIRE                        0x1000  /* TCH_5WIRE */
+#define WM831X_TCH_PDONLY                       0x0800  /* TCH_PDONLY */
+#define WM831X_TCH_ISEL                         0x0100  /* TCH_ISEL */
+#define WM831X_TCH_RPU_MASK                     0x000F  /* TCH_RPU - [3:0] */
+#define WM831X_TCH_RPU_SHIFT                         0  /* TCH_RPU - [3:0] */
+#define WM831X_TCH_RPU_WIDTH                         4  /* TCH_RPU - [3:0] */
+
+/*
+ * R16426-8 (0x402A-C) - Touch Data X/Y/X
+ */
+#define WM831X_TCH_PD                           0x8000  /* TCH_PD1 */
+#define WM831X_TCH_DATA_MASK                    0x0FFF  /* TCH_DATA - [11:0] */
+#define WM831X_TCH_DATA_SHIFT                        0  /* TCH_DATA - [11:0] */
+#define WM831X_TCH_DATA_WIDTH                       12  /* TCH_DATA - [11:0] */
+
+struct wm831x_ts {
+	struct input_dev *input_dev;
+	struct wm831x *wm831x;
+	unsigned int data_irq;
+	unsigned int pd_irq;
+	bool pressure;
+	bool pen_down;
+};
+
+static irqreturn_t wm831x_ts_data_irq(int irq, void *irq_data)
+{
+	struct wm831x_ts *wm831x_ts = irq_data;
+	struct wm831x *wm831x = wm831x_ts->wm831x;
+	static int data_types[] = { ABS_X, ABS_Y, ABS_PRESSURE };
+	u16 data[3];
+	int count;
+	int i, ret;
+
+	if (wm831x_ts->pressure)
+		count = 3;
+	else
+		count = 2;
+
+	wm831x_set_bits(wm831x, WM831X_INTERRUPT_STATUS_1,
+			WM831X_TCHDATA_EINT, WM831X_TCHDATA_EINT);
+
+	ret = wm831x_bulk_read(wm831x, WM831X_TOUCH_DATA_X, count,
+			       data);
+	if (ret != 0) {
+		dev_err(wm831x->dev, "Failed to read touch data: %d\n",
+			ret);
+		return IRQ_NONE;
+	}
+
+	/*
+	 * We get a pen down reading on every reading, report pen up if any
+	 * individual reading does so.
+	 */
+	wm831x_ts->pen_down = true;
+	for (i = 0; i < count; i++) {
+		if (!(data[i] & WM831X_TCH_PD)) {
+			wm831x_ts->pen_down = false;
+			continue;
+		}
+		input_report_abs(wm831x_ts->input_dev, data_types[i],
+				 data[i] & WM831X_TCH_DATA_MASK);
+	}
+
+	if (!wm831x_ts->pen_down) {
+		disable_irq_nosync(wm831x_ts->data_irq);
+
+		/* Don't need data any more */
+		wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_1,
+				WM831X_TCH_X_ENA | WM831X_TCH_Y_ENA |
+				WM831X_TCH_Z_ENA, 0);
+
+		/* Flush any final samples that arrived while reading */
+		wm831x_set_bits(wm831x, WM831X_INTERRUPT_STATUS_1,
+				WM831X_TCHDATA_EINT, WM831X_TCHDATA_EINT);
+
+		wm831x_bulk_read(wm831x, WM831X_TOUCH_DATA_X, count, data);
+
+		if (wm831x_ts->pressure)
+			input_report_abs(wm831x_ts->input_dev,
+					 ABS_PRESSURE, 0);
+
+		input_report_key(wm831x_ts->input_dev, BTN_TOUCH, 0);
+	}
+
+	input_sync(wm831x_ts->input_dev);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t wm831x_ts_pen_down_irq(int irq, void *irq_data)
+{
+	struct wm831x_ts *wm831x_ts = irq_data;
+	struct wm831x *wm831x = wm831x_ts->wm831x;
+	int ena = 0;
+
+	/* Start collecting data */
+	if (wm831x_ts->pressure)
+		ena |= WM831X_TCH_Z_ENA;
+
+	wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_1,
+			WM831X_TCH_X_ENA | WM831X_TCH_Y_ENA | WM831X_TCH_Z_ENA,
+			WM831X_TCH_X_ENA | WM831X_TCH_Y_ENA | ena);
+
+	input_report_key(wm831x_ts->input_dev, BTN_TOUCH, 1);
+	input_sync(wm831x_ts->input_dev);
+
+	wm831x_set_bits(wm831x, WM831X_INTERRUPT_STATUS_1,
+			WM831X_TCHPD_EINT, WM831X_TCHPD_EINT);
+
+	wm831x_ts->pen_down = true;
+	enable_irq(wm831x_ts->data_irq);
+
+	return IRQ_HANDLED;
+}
+
+static int wm831x_ts_input_open(struct input_dev *idev)
+{
+	struct wm831x_ts *wm831x_ts = input_get_drvdata(idev);
+	struct wm831x *wm831x = wm831x_ts->wm831x;
+
+	wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_1,
+			WM831X_TCH_ENA, WM831X_TCH_ENA);
+
+	wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_1,
+			WM831X_TCH_CVT_ENA, WM831X_TCH_CVT_ENA);
+
+	return 0;
+}
+
+static void wm831x_ts_input_close(struct input_dev *idev)
+{
+	struct wm831x_ts *wm831x_ts = input_get_drvdata(idev);
+	struct wm831x *wm831x = wm831x_ts->wm831x;
+
+	wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_1,
+			WM831X_TCH_ENA | WM831X_TCH_CVT_ENA |
+			WM831X_TCH_X_ENA | WM831X_TCH_Y_ENA |
+			WM831X_TCH_Z_ENA, 0);
+
+	if (wm831x_ts->pen_down)
+		disable_irq(wm831x_ts->data_irq);
+}
+
+static __devinit int wm831x_ts_probe(struct platform_device *pdev)
+{
+	struct wm831x_ts *wm831x_ts;
+	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
+	struct wm831x_pdata *core_pdata = dev_get_platdata(pdev->dev.parent);
+	struct wm831x_touch_pdata *pdata = NULL;
+	struct input_dev *input_dev;
+	int error;
+
+	if (core_pdata)
+		pdata = core_pdata->touch;
+
+	wm831x_ts = kzalloc(sizeof(struct wm831x_ts), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!wm831x_ts || !input_dev) {
+		error = -ENOMEM;
+		goto err_alloc;
+	}
+
+	wm831x_ts->wm831x = wm831x;
+	wm831x_ts->input_dev = input_dev;
+
+	/*
+	 * If we have a direct IRQ use it, otherwise use the interrupt
+	 * from the WM831x IRQ controller.
+	 */
+	if (pdata && pdata->data_irq)
+		wm831x_ts->data_irq = pdata->data_irq;
+	else
+		wm831x_ts->data_irq = platform_get_irq_byname(pdev, "TCHDATA");
+
+	if (pdata && pdata->pd_irq)
+		wm831x_ts->pd_irq = pdata->pd_irq;
+	else
+		wm831x_ts->pd_irq = platform_get_irq_byname(pdev, "TCHPD");
+
+	wm831x_ts->pressure = pdata && pdata->pressure;
+
+	/* Five wire touchscreens can't report pressure */
+	if (pdata && pdata->fivewire) {
+		wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_2,
+				WM831X_TCH_5WIRE, WM831X_TCH_5WIRE);
+
+		/* Pressure measurements are not possible for five wire mode */
+		WARN_ON(pdata->pressure && pdata->fivewire);
+		wm831x_ts->pressure = false;
+	} else {
+		wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_2,
+				WM831X_TCH_5WIRE, 0);
+	}
+
+	if (pdata) {
+		switch (pdata->isel) {
+		default:
+			dev_err(&pdev->dev, "Unsupported ISEL setting: %d\n",
+				pdata->isel);
+			/* Fall through */
+		case 200:
+		case 0:
+			wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_2,
+					WM831X_TCH_ISEL, 0);
+			break;
+		case 400:
+			wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_2,
+					WM831X_TCH_ISEL, WM831X_TCH_ISEL);
+			break;
+		}
+	}
+
+	wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_2,
+			WM831X_TCH_PDONLY, 0);
+
+	/* Default to 96 samples/sec */
+	wm831x_set_bits(wm831x, WM831X_TOUCH_CONTROL_1,
+			WM831X_TCH_RATE_MASK, 6);
+
+	error = request_threaded_irq(wm831x_ts->data_irq,
+				     NULL, wm831x_ts_data_irq,
+				     IRQF_ONESHOT,
+				     "Touchscreen data", wm831x_ts);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to request data IRQ %d: %d\n",
+			wm831x_ts->data_irq, error);
+		goto err_alloc;
+	}
+	disable_irq(wm831x_ts->data_irq);
+
+	error = request_threaded_irq(wm831x_ts->pd_irq,
+				     NULL, wm831x_ts_pen_down_irq,
+				     IRQF_ONESHOT,
+				     "Touchscreen pen down", wm831x_ts);
+	if (error) {
+		dev_err(&pdev->dev, "Failed to request pen down IRQ %d: %d\n",
+			wm831x_ts->pd_irq, error);
+		goto err_data_irq;
+	}
+
+	/* set up touch configuration */
+	input_dev->name = "WM831x touchscreen";
+	input_dev->phys = "wm831x";
+	input_dev->open = wm831x_ts_input_open;
+	input_dev->close = wm831x_ts_input_close;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	input_set_abs_params(input_dev, ABS_X, 0, 4095, 5, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, 4095, 5, 0);
+	if (wm831x_ts->pressure)
+		input_set_abs_params(input_dev, ABS_PRESSURE, 0, 4095, 5, 0);
+
+	input_set_drvdata(input_dev, wm831x_ts);
+	input_dev->dev.parent = &pdev->dev;
+
+	error = input_register_device(input_dev);
+	if (error)
+		goto err_pd_irq;
+
+	platform_set_drvdata(pdev, wm831x_ts);
+	return 0;
+
+err_pd_irq:
+	free_irq(wm831x_ts->pd_irq, wm831x_ts);
+err_data_irq:
+	free_irq(wm831x_ts->data_irq, wm831x_ts);
+err_alloc:
+	input_free_device(input_dev);
+	kfree(wm831x_ts);
+
+	return error;
+}
+
+static __devexit int wm831x_ts_remove(struct platform_device *pdev)
+{
+	struct wm831x_ts *wm831x_ts = platform_get_drvdata(pdev);
+
+	free_irq(wm831x_ts->pd_irq, wm831x_ts);
+	free_irq(wm831x_ts->data_irq, wm831x_ts);
+	input_unregister_device(wm831x_ts->input_dev);
+	kfree(wm831x_ts);
+
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+static struct platform_driver wm831x_ts_driver = {
+	.driver = {
+		.name = "wm831x-touch",
+		.owner = THIS_MODULE,
+	},
+	.probe = wm831x_ts_probe,
+	.remove = __devexit_p(wm831x_ts_remove),
+};
+
+static int __init wm831x_ts_init(void)
+{
+	return platform_driver_register(&wm831x_ts_driver);
+}
+module_init(wm831x_ts_init);
+
+static void __exit wm831x_ts_exit(void)
+{
+	platform_driver_unregister(&wm831x_ts_driver);
+}
+module_exit(wm831x_ts_exit);
+
+/* Module information */
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_DESCRIPTION("WM831x PMIC touchscreen driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:wm831x-touch");
diff --git a/drivers/isdn/hardware/eicon/istream.c b/drivers/isdn/hardware/eicon/istream.c
index 18f8798442fa..7bd5baa547be 100644
--- a/drivers/isdn/hardware/eicon/istream.c
+++ b/drivers/isdn/hardware/eicon/istream.c
@@ -62,7 +62,7 @@ void diva_xdi_provide_istream_info (ADAPTER* a,
   stream interface.
   If synchronous service was requested, then function
   does return amount of data written to stream.
-  'final' does indicate that pice of data to be written is
+  'final' does indicate that piece of data to be written is
   final part of frame (necessary only by structured datatransfer)
   return  0 if zero lengh packet was written
   return -1 if stream is full
diff --git a/drivers/isdn/mISDN/hwchannel.c b/drivers/isdn/mISDN/hwchannel.c
index 199f374cf9da..f6e108d0125f 100644
--- a/drivers/isdn/mISDN/hwchannel.c
+++ b/drivers/isdn/mISDN/hwchannel.c
@@ -206,7 +206,7 @@ recv_Bchannel(struct bchannel *bch, unsigned int id)
 	hh->id = id;
 	if (bch->rcount >= 64) {
 		printk(KERN_WARNING "B-channel %p receive queue overflow, "
-			"fushing!\n", bch);
+			"flushing!\n", bch);
 		skb_queue_purge(&bch->rqueue);
 		bch->rcount = 0;
 		return;
@@ -231,7 +231,7 @@ recv_Bchannel_skb(struct bchannel *bch, struct sk_buff *skb)
 {
 	if (bch->rcount >= 64) {
 		printk(KERN_WARNING "B-channel %p receive queue overflow, "
-			"fushing!\n", bch);
+			"flushing!\n", bch);
 		skb_queue_purge(&bch->rqueue);
 		bch->rcount = 0;
 	}
@@ -279,7 +279,7 @@ confirm_Bsend(struct bchannel *bch)
 
 	if (bch->rcount >= 64) {
 		printk(KERN_WARNING "B-channel %p receive queue overflow, "
-			"fushing!\n", bch);
+			"flushing!\n", bch);
 		skb_queue_purge(&bch->rqueue);
 		bch->rcount = 0;
 	}
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index c41eb6180c9c..4bebae733349 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -231,6 +231,26 @@ void led_trigger_event(struct led_trigger *trigger,
 }
 EXPORT_SYMBOL_GPL(led_trigger_event);
 
+void led_trigger_blink(struct led_trigger *trigger,
+		       unsigned long *delay_on,
+		       unsigned long *delay_off)
+{
+	struct list_head *entry;
+
+	if (!trigger)
+		return;
+
+	read_lock(&trigger->leddev_list_lock);
+	list_for_each(entry, &trigger->led_cdevs) {
+		struct led_classdev *led_cdev;
+
+		led_cdev = list_entry(entry, struct led_classdev, trig_list);
+		led_blink_set(led_cdev, delay_on, delay_off);
+	}
+	read_unlock(&trigger->leddev_list_lock);
+}
+EXPORT_SYMBOL_GPL(led_trigger_blink);
+
 void led_trigger_register_simple(const char *name, struct led_trigger **tp)
 {
 	struct led_trigger *trigger;
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 4d9fa38d9ff6..b0480c8fbcbf 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -14,6 +14,8 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/leds.h>
+#include <linux/of_platform.h>
+#include <linux/of_gpio.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
 
@@ -151,96 +153,34 @@ static void delete_gpio_led(struct gpio_led_data *led)
 	gpio_free(led->gpio);
 }
 
-#ifdef CONFIG_LEDS_GPIO_PLATFORM
-static int __devinit gpio_led_probe(struct platform_device *pdev)
-{
-	struct gpio_led_platform_data *pdata = pdev->dev.platform_data;
-	struct gpio_led_data *leds_data;
-	int i, ret = 0;
-
-	if (!pdata)
-		return -EBUSY;
-
-	leds_data = kzalloc(sizeof(struct gpio_led_data) * pdata->num_leds,
-				GFP_KERNEL);
-	if (!leds_data)
-		return -ENOMEM;
-
-	for (i = 0; i < pdata->num_leds; i++) {
-		ret = create_gpio_led(&pdata->leds[i], &leds_data[i],
-				      &pdev->dev, pdata->gpio_blink_set);
-		if (ret < 0)
-			goto err;
-	}
-
-	platform_set_drvdata(pdev, leds_data);
-
-	return 0;
-
-err:
-	for (i = i - 1; i >= 0; i--)
-		delete_gpio_led(&leds_data[i]);
-
-	kfree(leds_data);
-
-	return ret;
-}
+struct gpio_leds_priv {
+	int num_leds;
+	struct gpio_led_data leds[];
+};
 
-static int __devexit gpio_led_remove(struct platform_device *pdev)
+static inline int sizeof_gpio_leds_priv(int num_leds)
 {
-	int i;
-	struct gpio_led_platform_data *pdata = pdev->dev.platform_data;
-	struct gpio_led_data *leds_data;
-
-	leds_data = platform_get_drvdata(pdev);
-
-	for (i = 0; i < pdata->num_leds; i++)
-		delete_gpio_led(&leds_data[i]);
-
-	kfree(leds_data);
-
-	return 0;
+	return sizeof(struct gpio_leds_priv) +
+		(sizeof(struct gpio_led_data) * num_leds);
 }
 
-static struct platform_driver gpio_led_driver = {
-	.probe		= gpio_led_probe,
-	.remove		= __devexit_p(gpio_led_remove),
-	.driver		= {
-		.name	= "leds-gpio",
-		.owner	= THIS_MODULE,
-	},
-};
-
-MODULE_ALIAS("platform:leds-gpio");
-#endif /* CONFIG_LEDS_GPIO_PLATFORM */
-
 /* Code to create from OpenFirmware platform devices */
 #ifdef CONFIG_LEDS_GPIO_OF
-#include <linux/of_platform.h>
-#include <linux/of_gpio.h>
-
-struct gpio_led_of_platform_data {
-	int num_leds;
-	struct gpio_led_data led_data[];
-};
-
-static int __devinit of_gpio_leds_probe(struct platform_device *ofdev,
-					const struct of_device_id *match)
+static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_device *pdev)
 {
-	struct device_node *np = ofdev->dev.of_node, *child;
-	struct gpio_led_of_platform_data *pdata;
+	struct device_node *np = pdev->dev.of_node, *child;
+	struct gpio_leds_priv *priv;
 	int count = 0, ret;
 
-	/* count LEDs defined by this device, so we know how much to allocate */
+	/* count LEDs in this device, so we know how much to allocate */
 	for_each_child_of_node(np, child)
 		count++;
 	if (!count)
-		return 0; /* or ENODEV? */
+		return NULL;
 
-	pdata = kzalloc(sizeof(*pdata) + sizeof(struct gpio_led_data) * count,
-			GFP_KERNEL);
-	if (!pdata)
-		return -ENOMEM;
+	priv = kzalloc(sizeof_gpio_leds_priv(count), GFP_KERNEL);
+	if (!priv)
+		return NULL;
 
 	for_each_child_of_node(np, child) {
 		struct gpio_led led = {};
@@ -256,92 +196,112 @@ static int __devinit of_gpio_leds_probe(struct platform_device *ofdev,
 		if (state) {
 			if (!strcmp(state, "keep"))
 				led.default_state = LEDS_GPIO_DEFSTATE_KEEP;
-			else if(!strcmp(state, "on"))
+			else if (!strcmp(state, "on"))
 				led.default_state = LEDS_GPIO_DEFSTATE_ON;
 			else
 				led.default_state = LEDS_GPIO_DEFSTATE_OFF;
 		}
 
-		ret = create_gpio_led(&led, &pdata->led_data[pdata->num_leds++],
-				      &ofdev->dev, NULL);
+		ret = create_gpio_led(&led, &priv->leds[priv->num_leds++],
+				      &pdev->dev, NULL);
 		if (ret < 0) {
 			of_node_put(child);
 			goto err;
 		}
 	}
 
-	dev_set_drvdata(&ofdev->dev, pdata);
-
-	return 0;
+	return priv;
 
 err:
-	for (count = pdata->num_leds - 2; count >= 0; count--)
-		delete_gpio_led(&pdata->led_data[count]);
+	for (count = priv->num_leds - 2; count >= 0; count--)
+		delete_gpio_led(&priv->leds[count]);
+	kfree(priv);
+	return NULL;
+}
 
-	kfree(pdata);
+static const struct of_device_id of_gpio_leds_match[] = {
+	{ .compatible = "gpio-leds", },
+	{},
+};
+#else
+static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_device *pdev)
+{
+	return NULL;
+}
+#define of_gpio_leds_match NULL
+#endif
 
-	return ret;
+
+static int __devinit gpio_led_probe(struct platform_device *pdev)
+{
+	struct gpio_led_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_leds_priv *priv;
+	int i, ret = 0;
+
+	if (pdata && pdata->num_leds) {
+		priv = kzalloc(sizeof_gpio_leds_priv(pdata->num_leds),
+				GFP_KERNEL);
+		if (!priv)
+			return -ENOMEM;
+
+		priv->num_leds = pdata->num_leds;
+		for (i = 0; i < priv->num_leds; i++) {
+			ret = create_gpio_led(&pdata->leds[i],
+					      &priv->leds[i],
+					      &pdev->dev, pdata->gpio_blink_set);
+			if (ret < 0) {
+				/* On failure: unwind the led creations */
+				for (i = i - 1; i >= 0; i--)
+					delete_gpio_led(&priv->leds[i]);
+				kfree(priv);
+				return ret;
+			}
+		}
+	} else {
+		priv = gpio_leds_create_of(pdev);
+		if (!priv)
+			return -ENODEV;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	return 0;
 }
 
-static int __devexit of_gpio_leds_remove(struct platform_device *ofdev)
+static int __devexit gpio_led_remove(struct platform_device *pdev)
 {
-	struct gpio_led_of_platform_data *pdata = dev_get_drvdata(&ofdev->dev);
+	struct gpio_leds_priv *priv = dev_get_drvdata(&pdev->dev);
 	int i;
 
-	for (i = 0; i < pdata->num_leds; i++)
-		delete_gpio_led(&pdata->led_data[i]);
-
-	kfree(pdata);
+	for (i = 0; i < priv->num_leds; i++)
+		delete_gpio_led(&priv->leds[i]);
 
-	dev_set_drvdata(&ofdev->dev, NULL);
+	dev_set_drvdata(&pdev->dev, NULL);
+	kfree(priv);
 
 	return 0;
 }
 
-static const struct of_device_id of_gpio_leds_match[] = {
-	{ .compatible = "gpio-leds", },
-	{},
-};
-
-static struct of_platform_driver of_gpio_leds_driver = {
-	.driver = {
-		.name = "of_gpio_leds",
-		.owner = THIS_MODULE,
+static struct platform_driver gpio_led_driver = {
+	.probe		= gpio_led_probe,
+	.remove		= __devexit_p(gpio_led_remove),
+	.driver		= {
+		.name	= "leds-gpio",
+		.owner	= THIS_MODULE,
 		.of_match_table = of_gpio_leds_match,
 	},
-	.probe = of_gpio_leds_probe,
-	.remove = __devexit_p(of_gpio_leds_remove),
 };
-#endif
+
+MODULE_ALIAS("platform:leds-gpio");
 
 static int __init gpio_led_init(void)
 {
-	int ret = 0;
-
-#ifdef CONFIG_LEDS_GPIO_PLATFORM	
-	ret = platform_driver_register(&gpio_led_driver);
-	if (ret)
-		return ret;
-#endif
-#ifdef CONFIG_LEDS_GPIO_OF
-	ret = of_register_platform_driver(&of_gpio_leds_driver);
-#endif
-#ifdef CONFIG_LEDS_GPIO_PLATFORM	
-	if (ret)
-		platform_driver_unregister(&gpio_led_driver);
-#endif
-
-	return ret;
+	return platform_driver_register(&gpio_led_driver);
 }
 
 static void __exit gpio_led_exit(void)
 {
-#ifdef CONFIG_LEDS_GPIO_PLATFORM
 	platform_driver_unregister(&gpio_led_driver);
-#endif
-#ifdef CONFIG_LEDS_GPIO_OF
-	of_unregister_platform_driver(&of_gpio_leds_driver);
-#endif
 }
 
 module_init(gpio_led_init);
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index f05bb08d0f09..06a5bb484707 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -22,6 +22,7 @@
 #include <linux/leds.h>
 #include <linux/workqueue.h>
 #include <linux/mfd/mc13783.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 
 struct mc13783_led {
@@ -183,7 +184,7 @@ static int __devinit mc13783_led_setup(struct mc13783_led *led, int max_current)
 
 static int __devinit mc13783_leds_prepare(struct platform_device *pdev)
 {
-	struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	struct mc13783_leds_platform_data *pdata = mfd_get_data(pdev);
 	struct mc13783 *dev = dev_get_drvdata(pdev->dev.parent);
 	int ret = 0;
 	int reg = 0;
@@ -264,7 +265,7 @@ out:
 
 static int __devinit mc13783_led_probe(struct platform_device *pdev)
 {
-	struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	struct mc13783_leds_platform_data *pdata = mfd_get_data(pdev);
 	struct mc13783_led_platform_data *led_cur;
 	struct mc13783_led *led, *led_dat;
 	int ret, i;
@@ -351,7 +352,7 @@ err_free:
 
 static int __devexit mc13783_led_remove(struct platform_device *pdev)
 {
-	struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	struct mc13783_leds_platform_data *pdata = mfd_get_data(pdev);
 	struct mc13783_led *led = platform_get_drvdata(pdev);
 	struct mc13783 *dev = dev_get_drvdata(pdev->dev.parent);
 	int i;
diff --git a/drivers/macintosh/rack-meter.c b/drivers/macintosh/rack-meter.c
index 39f660b2a60d..2637c139777b 100644
--- a/drivers/macintosh/rack-meter.c
+++ b/drivers/macintosh/rack-meter.c
@@ -283,7 +283,7 @@ static void __devinit rackmeter_init_cpu_sniffer(struct rackmeter *rm)
 	}
 }
 
-static void __devexit rackmeter_stop_cpu_sniffer(struct rackmeter *rm)
+static void rackmeter_stop_cpu_sniffer(struct rackmeter *rm)
 {
 	cancel_delayed_work_sync(&rm->cpu[0].sniffer);
 	cancel_delayed_work_sync(&rm->cpu[1].sniffer);
diff --git a/drivers/macintosh/smu.c b/drivers/macintosh/smu.c
index 290cb325a94c..116a49ce74b2 100644
--- a/drivers/macintosh/smu.c
+++ b/drivers/macintosh/smu.c
@@ -645,8 +645,7 @@ static void smu_expose_childs(struct work_struct *unused)
 
 static DECLARE_WORK(smu_expose_childs_work, smu_expose_childs);
 
-static int smu_platform_probe(struct platform_device* dev,
-			      const struct of_device_id *match)
+static int smu_platform_probe(struct platform_device* dev)
 {
 	if (!smu)
 		return -ENODEV;
@@ -669,7 +668,7 @@ static const struct of_device_id smu_platform_match[] =
 	{},
 };
 
-static struct of_platform_driver smu_of_platform_driver =
+static struct platform_driver smu_of_platform_driver =
 {
 	.driver = {
 		.name = "smu",
@@ -689,7 +688,7 @@ static int __init smu_init_sysfs(void)
 	 * I'm a bit too far from figuring out how that works with those
 	 * new chipsets, but that will come back and bite us
 	 */
-	of_register_platform_driver(&smu_of_platform_driver);
+	platform_driver_register(&smu_of_platform_driver);
 	return 0;
 }
 
diff --git a/drivers/macintosh/therm_pm72.c b/drivers/macintosh/therm_pm72.c
index f3a29f264db9..bca2af2e3760 100644
--- a/drivers/macintosh/therm_pm72.c
+++ b/drivers/macintosh/therm_pm72.c
@@ -2210,7 +2210,7 @@ static void fcu_lookup_fans(struct device_node *fcu_node)
 	}
 }
 
-static int fcu_of_probe(struct platform_device* dev, const struct of_device_id *match)
+static int fcu_of_probe(struct platform_device* dev)
 {
 	state = state_detached;
 	of_dev = dev;
@@ -2240,7 +2240,7 @@ static const struct of_device_id fcu_match[] =
 };
 MODULE_DEVICE_TABLE(of, fcu_match);
 
-static struct of_platform_driver fcu_of_platform_driver = 
+static struct platform_driver fcu_of_platform_driver = 
 {
 	.driver = {
 		.name = "temperature",
@@ -2263,12 +2263,12 @@ static int __init therm_pm72_init(void)
 	    !rackmac)
 	    	return -ENODEV;
 
-	return of_register_platform_driver(&fcu_of_platform_driver);
+	return platform_driver_register(&fcu_of_platform_driver);
 }
 
 static void __exit therm_pm72_exit(void)
 {
-	of_unregister_platform_driver(&fcu_of_platform_driver);
+	platform_driver_unregister(&fcu_of_platform_driver);
 }
 
 module_init(therm_pm72_init);
diff --git a/drivers/macintosh/therm_windtunnel.c b/drivers/macintosh/therm_windtunnel.c
index c89f396e4c53..d37819fd5ad3 100644
--- a/drivers/macintosh/therm_windtunnel.c
+++ b/drivers/macintosh/therm_windtunnel.c
@@ -443,8 +443,7 @@ static struct i2c_driver g4fan_driver = {
 /*	initialization / cleanup					*/
 /************************************************************************/
 
-static int
-therm_of_probe( struct platform_device *dev, const struct of_device_id *match )
+static int therm_of_probe(struct platform_device *dev)
 {
 	return i2c_add_driver( &g4fan_driver );
 }
@@ -462,7 +461,7 @@ static const struct of_device_id therm_of_match[] = {{
     }, {}
 };
 
-static struct of_platform_driver therm_of_driver = {
+static struct platform_driver therm_of_driver = {
 	.driver = {
 		.name = "temperature",
 		.owner = THIS_MODULE,
@@ -509,14 +508,14 @@ g4fan_init( void )
 		return -ENODEV;
 	}
 
-	of_register_platform_driver( &therm_of_driver );
+	platform_driver_register( &therm_of_driver );
 	return 0;
 }
 
 static void __exit
 g4fan_exit( void )
 {
-	of_unregister_platform_driver( &therm_of_driver );
+	platform_driver_unregister( &therm_of_driver );
 
 	if( x.of_dev )
 		of_device_unregister( x.of_dev );
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index 98d9ec85e0eb..8420129fc5ee 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -327,4 +327,10 @@ config DM_UEVENT
 	---help---
 	Generate udev events for DM events.
 
+config DM_FLAKEY
+       tristate "Flakey target (EXPERIMENTAL)"
+       depends on BLK_DEV_DM && EXPERIMENTAL
+       ---help---
+         A target that intermittently fails I/O for debugging purposes.
+
 endif # MD
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index d0138606c2e8..448838b1f92a 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_BLK_DEV_MD)	+= md-mod.o
 obj-$(CONFIG_BLK_DEV_DM)	+= dm-mod.o
 obj-$(CONFIG_DM_CRYPT)		+= dm-crypt.o
 obj-$(CONFIG_DM_DELAY)		+= dm-delay.o
+obj-$(CONFIG_DM_FLAKEY)		+= dm-flakey.o
 obj-$(CONFIG_DM_MULTIPATH)	+= dm-multipath.o dm-round-robin.o
 obj-$(CONFIG_DM_MULTIPATH_QL)	+= dm-queue-length.o
 obj-$(CONFIG_DM_MULTIPATH_ST)	+= dm-service-time.o
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 4e054bd91664..7dcae0d74db7 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -1331,20 +1331,29 @@ static int crypt_setkey_allcpus(struct crypt_config *cc)
 
 static int crypt_set_key(struct crypt_config *cc, char *key)
 {
+	int r = -EINVAL;
+	int key_string_len = strlen(key);
+
 	/* The key size may not be changed. */
-	if (cc->key_size != (strlen(key) >> 1))
-		return -EINVAL;
+	if (cc->key_size != (key_string_len >> 1))
+		goto out;
 
 	/* Hyphen (which gives a key_size of zero) means there is no key. */
 	if (!cc->key_size && strcmp(key, "-"))
-		return -EINVAL;
+		goto out;
 
 	if (cc->key_size && crypt_decode_key(cc->key, key, cc->key_size) < 0)
-		return -EINVAL;
+		goto out;
 
 	set_bit(DM_CRYPT_KEY_VALID, &cc->flags);
 
-	return crypt_setkey_allcpus(cc);
+	r = crypt_setkey_allcpus(cc);
+
+out:
+	/* Hex key string not needed after here, so wipe it. */
+	memset(key, '0', key_string_len);
+
+	return r;
 }
 
 static int crypt_wipe_key(struct crypt_config *cc)
diff --git a/drivers/md/dm-flakey.c b/drivers/md/dm-flakey.c
new file mode 100644
index 000000000000..ea790623c30b
--- /dev/null
+++ b/drivers/md/dm-flakey.c
@@ -0,0 +1,212 @@
+/*
+ * Copyright (C) 2003 Sistina Software (UK) Limited.
+ * Copyright (C) 2004, 2010 Red Hat, Inc. All rights reserved.
+ *
+ * This file is released under the GPL.
+ */
+
+#include <linux/device-mapper.h>
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/blkdev.h>
+#include <linux/bio.h>
+#include <linux/slab.h>
+
+#define DM_MSG_PREFIX "flakey"
+
+/*
+ * Flakey: Used for testing only, simulates intermittent,
+ * catastrophic device failure.
+ */
+struct flakey_c {
+	struct dm_dev *dev;
+	unsigned long start_time;
+	sector_t start;
+	unsigned up_interval;
+	unsigned down_interval;
+};
+
+/*
+ * Construct a flakey mapping: <dev_path> <offset> <up interval> <down interval>
+ */
+static int flakey_ctr(struct dm_target *ti, unsigned int argc, char **argv)
+{
+	struct flakey_c *fc;
+	unsigned long long tmp;
+
+	if (argc != 4) {
+		ti->error = "dm-flakey: Invalid argument count";
+		return -EINVAL;
+	}
+
+	fc = kmalloc(sizeof(*fc), GFP_KERNEL);
+	if (!fc) {
+		ti->error = "dm-flakey: Cannot allocate linear context";
+		return -ENOMEM;
+	}
+	fc->start_time = jiffies;
+
+	if (sscanf(argv[1], "%llu", &tmp) != 1) {
+		ti->error = "dm-flakey: Invalid device sector";
+		goto bad;
+	}
+	fc->start = tmp;
+
+	if (sscanf(argv[2], "%u", &fc->up_interval) != 1) {
+		ti->error = "dm-flakey: Invalid up interval";
+		goto bad;
+	}
+
+	if (sscanf(argv[3], "%u", &fc->down_interval) != 1) {
+		ti->error = "dm-flakey: Invalid down interval";
+		goto bad;
+	}
+
+	if (!(fc->up_interval + fc->down_interval)) {
+		ti->error = "dm-flakey: Total (up + down) interval is zero";
+		goto bad;
+	}
+
+	if (fc->up_interval + fc->down_interval < fc->up_interval) {
+		ti->error = "dm-flakey: Interval overflow";
+		goto bad;
+	}
+
+	if (dm_get_device(ti, argv[0], dm_table_get_mode(ti->table), &fc->dev)) {
+		ti->error = "dm-flakey: Device lookup failed";
+		goto bad;
+	}
+
+	ti->num_flush_requests = 1;
+	ti->private = fc;
+	return 0;
+
+bad:
+	kfree(fc);
+	return -EINVAL;
+}
+
+static void flakey_dtr(struct dm_target *ti)
+{
+	struct flakey_c *fc = ti->private;
+
+	dm_put_device(ti, fc->dev);
+	kfree(fc);
+}
+
+static sector_t flakey_map_sector(struct dm_target *ti, sector_t bi_sector)
+{
+	struct flakey_c *fc = ti->private;
+
+	return fc->start + (bi_sector - ti->begin);
+}
+
+static void flakey_map_bio(struct dm_target *ti, struct bio *bio)
+{
+	struct flakey_c *fc = ti->private;
+
+	bio->bi_bdev = fc->dev->bdev;
+	if (bio_sectors(bio))
+		bio->bi_sector = flakey_map_sector(ti, bio->bi_sector);
+}
+
+static int flakey_map(struct dm_target *ti, struct bio *bio,
+		      union map_info *map_context)
+{
+	struct flakey_c *fc = ti->private;
+	unsigned elapsed;
+
+	/* Are we alive ? */
+	elapsed = (jiffies - fc->start_time) / HZ;
+	if (elapsed % (fc->up_interval + fc->down_interval) >= fc->up_interval)
+		return -EIO;
+
+	flakey_map_bio(ti, bio);
+
+	return DM_MAPIO_REMAPPED;
+}
+
+static int flakey_status(struct dm_target *ti, status_type_t type,
+			 char *result, unsigned int maxlen)
+{
+	struct flakey_c *fc = ti->private;
+
+	switch (type) {
+	case STATUSTYPE_INFO:
+		result[0] = '\0';
+		break;
+
+	case STATUSTYPE_TABLE:
+		snprintf(result, maxlen, "%s %llu %u %u", fc->dev->name,
+			 (unsigned long long)fc->start, fc->up_interval,
+			 fc->down_interval);
+		break;
+	}
+	return 0;
+}
+
+static int flakey_ioctl(struct dm_target *ti, unsigned int cmd, unsigned long arg)
+{
+	struct flakey_c *fc = ti->private;
+
+	return __blkdev_driver_ioctl(fc->dev->bdev, fc->dev->mode, cmd, arg);
+}
+
+static int flakey_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
+			struct bio_vec *biovec, int max_size)
+{
+	struct flakey_c *fc = ti->private;
+	struct request_queue *q = bdev_get_queue(fc->dev->bdev);
+
+	if (!q->merge_bvec_fn)
+		return max_size;
+
+	bvm->bi_bdev = fc->dev->bdev;
+	bvm->bi_sector = flakey_map_sector(ti, bvm->bi_sector);
+
+	return min(max_size, q->merge_bvec_fn(q, bvm, biovec));
+}
+
+static int flakey_iterate_devices(struct dm_target *ti, iterate_devices_callout_fn fn, void *data)
+{
+	struct flakey_c *fc = ti->private;
+
+	return fn(ti, fc->dev, fc->start, ti->len, data);
+}
+
+static struct target_type flakey_target = {
+	.name   = "flakey",
+	.version = {1, 1, 0},
+	.module = THIS_MODULE,
+	.ctr    = flakey_ctr,
+	.dtr    = flakey_dtr,
+	.map    = flakey_map,
+	.status = flakey_status,
+	.ioctl	= flakey_ioctl,
+	.merge	= flakey_merge,
+	.iterate_devices = flakey_iterate_devices,
+};
+
+static int __init dm_flakey_init(void)
+{
+	int r = dm_register_target(&flakey_target);
+
+	if (r < 0)
+		DMERR("register failed %d", r);
+
+	return r;
+}
+
+static void __exit dm_flakey_exit(void)
+{
+	dm_unregister_target(&flakey_target);
+}
+
+/* Module hooks */
+module_init(dm_flakey_init);
+module_exit(dm_flakey_exit);
+
+MODULE_DESCRIPTION(DM_NAME " flakey target");
+MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/md/dm-ioctl.c b/drivers/md/dm-ioctl.c
index 6d12775a1061..e7af88b71592 100644
--- a/drivers/md/dm-ioctl.c
+++ b/drivers/md/dm-ioctl.c
@@ -1501,11 +1501,6 @@ static int check_version(unsigned int cmd, struct dm_ioctl __user *user)
 	return r;
 }
 
-static void free_params(struct dm_ioctl *param)
-{
-	vfree(param);
-}
-
 static int copy_params(struct dm_ioctl __user *user, struct dm_ioctl **param)
 {
 	struct dm_ioctl tmp, *dmi;
@@ -1520,13 +1515,22 @@ static int copy_params(struct dm_ioctl __user *user, struct dm_ioctl **param)
 	if (!dmi)
 		return -ENOMEM;
 
-	if (copy_from_user(dmi, user, tmp.data_size)) {
-		vfree(dmi);
-		return -EFAULT;
-	}
+	if (copy_from_user(dmi, user, tmp.data_size))
+		goto bad;
+
+	/* Wipe the user buffer so we do not return it to userspace */
+	if ((tmp.flags & DM_SECURE_DATA_FLAG) &&
+	    clear_user(user, tmp.data_size))
+		goto bad;
 
 	*param = dmi;
 	return 0;
+
+bad:
+	if (tmp.flags & DM_SECURE_DATA_FLAG)
+		memset(dmi, 0, tmp.data_size);
+	vfree(dmi);
+	return -EFAULT;
 }
 
 static int validate_params(uint cmd, struct dm_ioctl *param)
@@ -1534,6 +1538,7 @@ static int validate_params(uint cmd, struct dm_ioctl *param)
 	/* Always clear this flag */
 	param->flags &= ~DM_BUFFER_FULL_FLAG;
 	param->flags &= ~DM_UEVENT_GENERATED_FLAG;
+	param->flags &= ~DM_SECURE_DATA_FLAG;
 
 	/* Ignores parameters */
 	if (cmd == DM_REMOVE_ALL_CMD ||
@@ -1561,10 +1566,11 @@ static int validate_params(uint cmd, struct dm_ioctl *param)
 static int ctl_ioctl(uint command, struct dm_ioctl __user *user)
 {
 	int r = 0;
+	int wipe_buffer;
 	unsigned int cmd;
 	struct dm_ioctl *uninitialized_var(param);
 	ioctl_fn fn = NULL;
-	size_t param_size;
+	size_t input_param_size;
 
 	/* only root can play with this */
 	if (!capable(CAP_SYS_ADMIN))
@@ -1605,6 +1611,8 @@ static int ctl_ioctl(uint command, struct dm_ioctl __user *user)
 	 * Copy the parameters into kernel space.
 	 */
 	r = copy_params(user, &param);
+	input_param_size = param->data_size;
+	wipe_buffer = param->flags & DM_SECURE_DATA_FLAG;
 
 	current->flags &= ~PF_MEMALLOC;
 
@@ -1615,9 +1623,8 @@ static int ctl_ioctl(uint command, struct dm_ioctl __user *user)
 	if (r)
 		goto out;
 
-	param_size = param->data_size;
 	param->data_size = sizeof(*param);
-	r = fn(param, param_size);
+	r = fn(param, input_param_size);
 
 	/*
 	 * Copy the results back to userland.
@@ -1625,8 +1632,11 @@ static int ctl_ioctl(uint command, struct dm_ioctl __user *user)
 	if (!r && copy_to_user(user, param, param->data_size))
 		r = -EFAULT;
 
- out:
-	free_params(param);
+out:
+	if (wipe_buffer)
+		memset(param, 0, input_param_size);
+
+	vfree(param);
 	return r;
 }
 
diff --git a/drivers/md/dm-log-userspace-transfer.c b/drivers/md/dm-log-userspace-transfer.c
index 049eaf12aaab..1f23e048f077 100644
--- a/drivers/md/dm-log-userspace-transfer.c
+++ b/drivers/md/dm-log-userspace-transfer.c
@@ -134,7 +134,7 @@ static void cn_ulog_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 {
 	struct dm_ulog_request *tfr = (struct dm_ulog_request *)(msg + 1);
 
-	if (!cap_raised(nsp->eff_cap, CAP_SYS_ADMIN))
+	if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
 		return;
 
 	spin_lock(&receiving_list_lock);
diff --git a/drivers/md/dm-log.c b/drivers/md/dm-log.c
index 6951536ea29c..8e8a868ca857 100644
--- a/drivers/md/dm-log.c
+++ b/drivers/md/dm-log.c
@@ -543,7 +543,7 @@ static int disk_ctr(struct dm_dirty_log *log, struct dm_target *ti,
 		return -EINVAL;
 	}
 
-	r = dm_get_device(ti, argv[0], FMODE_READ | FMODE_WRITE, &dev);
+	r = dm_get_device(ti, argv[0], dm_table_get_mode(ti->table), &dev);
 	if (r)
 		return r;
 
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index b82d28819e2a..a550a057d991 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -844,8 +844,8 @@ static int multipath_ctr(struct dm_target *ti, unsigned int argc,
 {
 	/* target parameters */
 	static struct param _params[] = {
-		{1, 1024, "invalid number of priority groups"},
-		{1, 1024, "invalid initial priority group number"},
+		{0, 1024, "invalid number of priority groups"},
+		{0, 1024, "invalid initial priority group number"},
 	};
 
 	int r;
@@ -879,6 +879,13 @@ static int multipath_ctr(struct dm_target *ti, unsigned int argc,
 	if (r)
 		goto bad;
 
+	if ((!m->nr_priority_groups && next_pg_num) ||
+	    (m->nr_priority_groups && !next_pg_num)) {
+		ti->error = "invalid initial priority group";
+		r = -EINVAL;
+		goto bad;
+	}
+
 	/* parse the priority groups */
 	while (as.argc) {
 		struct priority_group *pg;
@@ -1065,7 +1072,7 @@ out:
 static int action_dev(struct multipath *m, struct dm_dev *dev,
 		      action_fn action)
 {
-	int r = 0;
+	int r = -EINVAL;
 	struct pgpath *pgpath;
 	struct priority_group *pg;
 
@@ -1283,24 +1290,22 @@ static int do_end_io(struct multipath *m, struct request *clone,
 	if (!error && !clone->errors)
 		return 0;	/* I/O complete */
 
-	if (error == -EOPNOTSUPP)
-		return error;
-
-	if (clone->cmd_flags & REQ_DISCARD)
-		/*
-		 * Pass all discard request failures up.
-		 * FIXME: only fail_path if the discard failed due to a
-		 * transport problem.  This requires precise understanding
-		 * of the underlying failure (e.g. the SCSI sense).
-		 */
+	if (error == -EOPNOTSUPP || error == -EREMOTEIO)
 		return error;
 
 	if (mpio->pgpath)
 		fail_path(mpio->pgpath);
 
 	spin_lock_irqsave(&m->lock, flags);
-	if (!m->nr_valid_paths && !m->queue_if_no_path && !__must_push_back(m))
-		r = -EIO;
+	if (!m->nr_valid_paths) {
+		if (!m->queue_if_no_path) {
+			if (!__must_push_back(m))
+				r = -EIO;
+		} else {
+			if (error == -EBADE)
+				r = error;
+		}
+	}
 	spin_unlock_irqrestore(&m->lock, flags);
 
 	return r;
@@ -1417,7 +1422,7 @@ static int multipath_status(struct dm_target *ti, status_type_t type,
 	else if (m->current_pg)
 		pg_num = m->current_pg->pg_num;
 	else
-			pg_num = 1;
+		pg_num = (m->nr_priority_groups ? 1 : 0);
 
 	DMEMIT("%u ", pg_num);
 
@@ -1671,7 +1676,7 @@ out:
  *---------------------------------------------------------------*/
 static struct target_type multipath_target = {
 	.name = "multipath",
-	.version = {1, 2, 0},
+	.version = {1, 3, 0},
 	.module = THIS_MODULE,
 	.ctr = multipath_ctr,
 	.dtr = multipath_dtr,
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index fdde53cd12b7..a2d330942cb2 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -1080,7 +1080,7 @@ static int snapshot_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	argv++;
 	argc--;
 
-	r = dm_get_device(ti, cow_path, FMODE_READ | FMODE_WRITE, &s->cow);
+	r = dm_get_device(ti, cow_path, dm_table_get_mode(ti->table), &s->cow);
 	if (r) {
 		ti->error = "Cannot get COW device";
 		goto bad_cow;
diff --git a/drivers/md/linear.c b/drivers/md/linear.c
index 8a2f767f26d8..0ed7f6bc2a7f 100644
--- a/drivers/md/linear.c
+++ b/drivers/md/linear.c
@@ -216,7 +216,6 @@ static int linear_run (mddev_t *mddev)
 
 	if (md_check_no_bitmap(mddev))
 		return -EINVAL;
-	mddev->queue->queue_lock = &mddev->queue->__queue_lock;
 	conf = linear_conf(mddev, mddev->raid_disks);
 
 	if (!conf)
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 0cc30ecda4c1..d5ad7723b172 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -553,6 +553,9 @@ static mddev_t * mddev_find(dev_t unit)
 {
 	mddev_t *mddev, *new = NULL;
 
+	if (unit && MAJOR(unit) != MD_MAJOR)
+		unit &= ~((1<<MdpMinorShift)-1);
+
  retry:
 	spin_lock(&all_mddevs_lock);
 
@@ -4138,10 +4141,10 @@ array_size_store(mddev_t *mddev, const char *buf, size_t len)
 	}
 
 	mddev->array_sectors = sectors;
-	set_capacity(mddev->gendisk, mddev->array_sectors);
-	if (mddev->pers)
+	if (mddev->pers) {
+		set_capacity(mddev->gendisk, mddev->array_sectors);
 		revalidate_disk(mddev->gendisk);
-
+	}
 	return len;
 }
 
@@ -4624,6 +4627,7 @@ static int do_md_run(mddev_t *mddev)
 	}
 	set_capacity(mddev->gendisk, mddev->array_sectors);
 	revalidate_disk(mddev->gendisk);
+	mddev->changed = 1;
 	kobject_uevent(&disk_to_dev(mddev->gendisk)->kobj, KOBJ_CHANGE);
 out:
 	return err;
@@ -4712,6 +4716,7 @@ static void md_clean(mddev_t *mddev)
 	mddev->sync_speed_min = mddev->sync_speed_max = 0;
 	mddev->recovery = 0;
 	mddev->in_sync = 0;
+	mddev->changed = 0;
 	mddev->degraded = 0;
 	mddev->safemode = 0;
 	mddev->bitmap_info.offset = 0;
@@ -4827,6 +4832,7 @@ static int do_md_stop(mddev_t * mddev, int mode, int is_open)
 
 		set_capacity(disk, 0);
 		mutex_unlock(&mddev->open_mutex);
+		mddev->changed = 1;
 		revalidate_disk(disk);
 
 		if (mddev->ro)
@@ -6011,7 +6017,7 @@ static int md_open(struct block_device *bdev, fmode_t mode)
 	atomic_inc(&mddev->openers);
 	mutex_unlock(&mddev->open_mutex);
 
-	check_disk_size_change(mddev->gendisk, bdev);
+	check_disk_change(bdev);
  out:
 	return err;
 }
@@ -6026,6 +6032,21 @@ static int md_release(struct gendisk *disk, fmode_t mode)
 
 	return 0;
 }
+
+static int md_media_changed(struct gendisk *disk)
+{
+	mddev_t *mddev = disk->private_data;
+
+	return mddev->changed;
+}
+
+static int md_revalidate(struct gendisk *disk)
+{
+	mddev_t *mddev = disk->private_data;
+
+	mddev->changed = 0;
+	return 0;
+}
 static const struct block_device_operations md_fops =
 {
 	.owner		= THIS_MODULE,
@@ -6036,6 +6057,8 @@ static const struct block_device_operations md_fops =
 	.compat_ioctl	= md_compat_ioctl,
 #endif
 	.getgeo		= md_getgeo,
+	.media_changed  = md_media_changed,
+	.revalidate_disk= md_revalidate,
 };
 
 static int md_thread(void * arg)
@@ -7338,7 +7361,7 @@ static int __init md_init(void)
 {
 	int ret = -ENOMEM;
 
-	md_wq = alloc_workqueue("md", WQ_RESCUER, 0);
+	md_wq = alloc_workqueue("md", WQ_MEM_RECLAIM, 0);
 	if (!md_wq)
 		goto err_wq;
 
diff --git a/drivers/md/md.h b/drivers/md/md.h
index 7e90b8593b2a..12215d437fcc 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -274,6 +274,8 @@ struct mddev_s
 	atomic_t			active;		/* general refcount */
 	atomic_t			openers;	/* number of active opens */
 
+	int				changed;	/* True if we might need to
+							 * reread partition info */
 	int				degraded;	/* whether md should consider
 							 * adding a spare
 							 */
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index 6d7ddf32ef2e..3a62d440e27b 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -435,7 +435,6 @@ static int multipath_run (mddev_t *mddev)
 	 * bookkeeping area. [whatever we allocate in multipath_run(),
 	 * should be freed in multipath_stop()]
 	 */
-	mddev->queue->queue_lock = &mddev->queue->__queue_lock;
 
 	conf = kzalloc(sizeof(multipath_conf_t), GFP_KERNEL);
 	mddev->private = conf;
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index 637a96855edb..c0ac457f1218 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -361,7 +361,6 @@ static int raid0_run(mddev_t *mddev)
 	if (md_check_no_bitmap(mddev))
 		return -EINVAL;
 	blk_queue_max_hw_sectors(mddev->queue, mddev->chunk_sectors);
-	mddev->queue->queue_lock = &mddev->queue->__queue_lock;
 
 	/* if private is not null, we are here after takeover */
 	if (mddev->private == NULL) {
@@ -670,6 +669,7 @@ static void *raid0_takeover_raid1(mddev_t *mddev)
 	mddev->new_layout = 0;
 	mddev->new_chunk_sectors = 128; /* by default set chunk size to 64k */
 	mddev->delta_disks = 1 - mddev->raid_disks;
+	mddev->raid_disks = 1;
 	/* make sure it will be not marked as dirty */
 	mddev->recovery_cp = MaxSector;
 
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index a23ffa397ba9..06cd712807d0 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -593,7 +593,10 @@ static int flush_pending_writes(conf_t *conf)
 	if (conf->pending_bio_list.head) {
 		struct bio *bio;
 		bio = bio_list_get(&conf->pending_bio_list);
+		/* Only take the spinlock to quiet a warning */
+		spin_lock(conf->mddev->queue->queue_lock);
 		blk_remove_plug(conf->mddev->queue);
+		spin_unlock(conf->mddev->queue->queue_lock);
 		spin_unlock_irq(&conf->device_lock);
 		/* flush any pending bitmap writes to
 		 * disk before proceeding w/ I/O */
@@ -959,7 +962,7 @@ static int make_request(mddev_t *mddev, struct bio * bio)
 		atomic_inc(&r1_bio->remaining);
 		spin_lock_irqsave(&conf->device_lock, flags);
 		bio_list_add(&conf->pending_bio_list, mbio);
-		blk_plug_device(mddev->queue);
+		blk_plug_device_unlocked(mddev->queue);
 		spin_unlock_irqrestore(&conf->device_lock, flags);
 	}
 	r1_bio_write_done(r1_bio, bio->bi_vcnt, behind_pages, behind_pages != NULL);
@@ -2021,7 +2024,6 @@ static int run(mddev_t *mddev)
 	if (IS_ERR(conf))
 		return PTR_ERR(conf);
 
-	mddev->queue->queue_lock = &conf->device_lock;
 	list_for_each_entry(rdev, &mddev->disks, same_set) {
 		disk_stack_limits(mddev->gendisk, rdev->bdev,
 				  rdev->data_offset << 9);
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 3b607b28741b..747d061d8e05 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -662,7 +662,10 @@ static int flush_pending_writes(conf_t *conf)
 	if (conf->pending_bio_list.head) {
 		struct bio *bio;
 		bio = bio_list_get(&conf->pending_bio_list);
+		/* Spinlock only taken to quiet a warning */
+		spin_lock(conf->mddev->queue->queue_lock);
 		blk_remove_plug(conf->mddev->queue);
+		spin_unlock(conf->mddev->queue->queue_lock);
 		spin_unlock_irq(&conf->device_lock);
 		/* flush any pending bitmap writes to disk
 		 * before proceeding w/ I/O */
@@ -971,7 +974,7 @@ static int make_request(mddev_t *mddev, struct bio * bio)
 		atomic_inc(&r10_bio->remaining);
 		spin_lock_irqsave(&conf->device_lock, flags);
 		bio_list_add(&conf->pending_bio_list, mbio);
-		blk_plug_device(mddev->queue);
+		blk_plug_device_unlocked(mddev->queue);
 		spin_unlock_irqrestore(&conf->device_lock, flags);
 	}
 
@@ -2304,8 +2307,6 @@ static int run(mddev_t *mddev)
 	if (!conf)
 		goto out;
 
-	mddev->queue->queue_lock = &conf->device_lock;
-
 	mddev->thread = conf->thread;
 	conf->thread = NULL;
 
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 702812824195..78536fdbd87f 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -5204,7 +5204,6 @@ static int run(mddev_t *mddev)
 
 		mddev->queue->backing_dev_info.congested_data = mddev;
 		mddev->queue->backing_dev_info.congested_fn = raid5_congested;
-		mddev->queue->queue_lock = &conf->device_lock;
 		mddev->queue->unplug_fn = raid5_unplug_queue;
 
 		chunk_size = mddev->chunk_sectors << 9;
diff --git a/drivers/media/common/tuners/tda8290.c b/drivers/media/common/tuners/tda8290.c
index bc6a67768af1..8c4852114eeb 100644
--- a/drivers/media/common/tuners/tda8290.c
+++ b/drivers/media/common/tuners/tda8290.c
@@ -658,13 +658,13 @@ static int tda8290_probe(struct tuner_i2c_props *i2c_props)
 #define TDA8290_ID 0x89
 	u8 reg = 0x1f, id;
 	struct i2c_msg msg_read[] = {
-		{ .addr = 0x4b, .flags = 0, .len = 1, .buf = &reg },
-		{ .addr = 0x4b, .flags = I2C_M_RD, .len = 1, .buf = &id },
+		{ .addr = i2c_props->addr, .flags = 0, .len = 1, .buf = &reg },
+		{ .addr = i2c_props->addr, .flags = I2C_M_RD, .len = 1, .buf = &id },
 	};
 
 	/* detect tda8290 */
 	if (i2c_transfer(i2c_props->adap, msg_read, 2) != 2) {
-		printk(KERN_WARNING "%s: tda8290 couldn't read register 0x%02x\n",
+		printk(KERN_WARNING "%s: couldn't read register 0x%02x\n",
 			       __func__, reg);
 		return -ENODEV;
 	}
@@ -685,13 +685,13 @@ static int tda8295_probe(struct tuner_i2c_props *i2c_props)
 #define TDA8295C2_ID 0x8b
 	u8 reg = 0x2f, id;
 	struct i2c_msg msg_read[] = {
-		{ .addr = 0x4b, .flags = 0, .len = 1, .buf = &reg },
-		{ .addr = 0x4b, .flags = I2C_M_RD, .len = 1, .buf = &id },
+		{ .addr = i2c_props->addr, .flags = 0, .len = 1, .buf = &reg },
+		{ .addr = i2c_props->addr, .flags = I2C_M_RD, .len = 1, .buf = &id },
 	};
 
-	/* detect tda8290 */
+	/* detect tda8295 */
 	if (i2c_transfer(i2c_props->adap, msg_read, 2) != 2) {
-		printk(KERN_WARNING "%s: tda8290 couldn't read register 0x%02x\n",
+		printk(KERN_WARNING "%s: couldn't read register 0x%02x\n",
 			       __func__, reg);
 		return -ENODEV;
 	}
diff --git a/drivers/media/common/tuners/tda9887.c b/drivers/media/common/tuners/tda9887.c
index bf14bd79e2fc..cdb645d57438 100644
--- a/drivers/media/common/tuners/tda9887.c
+++ b/drivers/media/common/tuners/tda9887.c
@@ -36,6 +36,8 @@ struct tda9887_priv {
 	unsigned int       mode;
 	unsigned int       audmode;
 	v4l2_std_id        std;
+
+	bool               standby;
 };
 
 /* ---------------------------------------------------------------------- */
@@ -568,7 +570,7 @@ static void tda9887_configure(struct dvb_frontend *fe)
 	tda9887_do_config(fe);
 	tda9887_set_insmod(fe);
 
-	if (priv->mode == T_STANDBY)
+	if (priv->standby)
 		priv->data[1] |= cForcedMuteAudioON;
 
 	tuner_dbg("writing: b=0x%02x c=0x%02x e=0x%02x\n",
@@ -616,7 +618,7 @@ static void tda9887_standby(struct dvb_frontend *fe)
 {
 	struct tda9887_priv *priv = fe->analog_demod_priv;
 
-	priv->mode = T_STANDBY;
+	priv->standby = true;
 
 	tda9887_configure(fe);
 }
@@ -626,6 +628,7 @@ static void tda9887_set_params(struct dvb_frontend *fe,
 {
 	struct tda9887_priv *priv = fe->analog_demod_priv;
 
+	priv->standby = false;
 	priv->mode    = params->mode;
 	priv->audmode = params->audmode;
 	priv->std     = params->std;
@@ -686,7 +689,7 @@ struct dvb_frontend *tda9887_attach(struct dvb_frontend *fe,
 		return NULL;
 	case 1:
 		fe->analog_demod_priv = priv;
-		priv->mode = T_STANDBY;
+		priv->standby = true;
 		tuner_info("tda988[5/6/7] found\n");
 		break;
 	default:
diff --git a/drivers/media/common/tuners/tea5761.c b/drivers/media/common/tuners/tea5761.c
index 925399dffbed..bf78cb9fc52c 100644
--- a/drivers/media/common/tuners/tea5761.c
+++ b/drivers/media/common/tuners/tea5761.c
@@ -23,6 +23,7 @@ struct tea5761_priv {
 	struct tuner_i2c_props i2c_props;
 
 	u32 frequency;
+	bool standby;
 };
 
 /*****************************************************************************/
@@ -135,18 +136,19 @@ static void tea5761_status_dump(unsigned char *buffer)
 }
 
 /* Freq should be specifyed at 62.5 Hz */
-static int set_radio_freq(struct dvb_frontend *fe,
-			  struct analog_parameters *params)
+static int __set_radio_freq(struct dvb_frontend *fe,
+			    unsigned int freq,
+			    bool mono)
 {
 	struct tea5761_priv *priv = fe->tuner_priv;
-	unsigned int frq = params->frequency;
+	unsigned int frq = freq;
 	unsigned char buffer[7] = {0, 0, 0, 0, 0, 0, 0 };
 	unsigned div;
 	int rc;
 
 	tuner_dbg("radio freq counter %d\n", frq);
 
-	if (params->mode == T_STANDBY) {
+	if (priv->standby) {
 		tuner_dbg("TEA5761 set to standby mode\n");
 		buffer[5] |= TEA5761_TNCTRL_MU;
 	} else {
@@ -154,7 +156,7 @@ static int set_radio_freq(struct dvb_frontend *fe,
 	}
 
 
-	if (params->audmode == V4L2_TUNER_MODE_MONO) {
+	if (mono) {
 		tuner_dbg("TEA5761 set to mono\n");
 		buffer[5] |= TEA5761_TNCTRL_MST;
 	} else {
@@ -176,6 +178,26 @@ static int set_radio_freq(struct dvb_frontend *fe,
 	return 0;
 }
 
+static int set_radio_freq(struct dvb_frontend *fe,
+			  struct analog_parameters *params)
+{
+	struct tea5761_priv *priv = fe->analog_demod_priv;
+
+	priv->standby = false;
+
+	return __set_radio_freq(fe, params->frequency,
+				params->audmode == V4L2_TUNER_MODE_MONO);
+}
+
+static int set_radio_sleep(struct dvb_frontend *fe)
+{
+	struct tea5761_priv *priv = fe->analog_demod_priv;
+
+	priv->standby = true;
+
+	return __set_radio_freq(fe, priv->frequency, false);
+}
+
 static int tea5761_read_status(struct dvb_frontend *fe, char *buffer)
 {
 	struct tea5761_priv *priv = fe->tuner_priv;
@@ -284,6 +306,7 @@ static struct dvb_tuner_ops tea5761_tuner_ops = {
 		.name           = "tea5761", // Philips TEA5761HN FM Radio
 	},
 	.set_analog_params = set_radio_freq,
+	.sleep		   = set_radio_sleep,
 	.release           = tea5761_release,
 	.get_frequency     = tea5761_get_frequency,
 	.get_status        = tea5761_get_status,
diff --git a/drivers/media/common/tuners/tuner-types.c b/drivers/media/common/tuners/tuner-types.c
index 58a513bcd747..afba6dc5e080 100644
--- a/drivers/media/common/tuners/tuner-types.c
+++ b/drivers/media/common/tuners/tuner-types.c
@@ -971,6 +971,22 @@ static struct tuner_params tuner_tena_9533_di_params[] = {
 	},
 };
 
+/* ------------ TUNER_TENA_TNF_5337 - Tena tnf5337MFD STD M/N ------------ */
+
+static struct tuner_range tuner_tena_tnf_5337_ntsc_ranges[] = {
+	{ 16 * 166.25 /*MHz*/, 0x86, 0x01, },
+	{ 16 * 466.25 /*MHz*/, 0x86, 0x02, },
+	{ 16 * 999.99        , 0x86, 0x08, },
+};
+
+static struct tuner_params tuner_tena_tnf_5337_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_NTSC,
+		.ranges = tuner_tena_tnf_5337_ntsc_ranges,
+		.count  = ARRAY_SIZE(tuner_tena_tnf_5337_ntsc_ranges),
+	},
+};
+
 /* ------------ TUNER_PHILIPS_FMD1216ME(X)_MK3 - Philips PAL ------------ */
 
 static struct tuner_range tuner_philips_fmd1216me_mk3_pal_ranges[] = {
@@ -1842,6 +1858,11 @@ struct tunertype tuners[] = {
 		.params = tuner_philips_fq1236_mk5_params,
 		.count  = ARRAY_SIZE(tuner_philips_fq1236_mk5_params),
 	},
+	[TUNER_TENA_TNF_5337] = { /* Tena 5337 MFD */
+		.name   = "Tena TNF5337 MFD",
+		.params = tuner_tena_tnf_5337_params,
+		.count  = ARRAY_SIZE(tuner_tena_tnf_5337_params),
+	},
 };
 EXPORT_SYMBOL(tuners);
 
diff --git a/drivers/media/common/tuners/tuner-xc2028.c b/drivers/media/common/tuners/tuner-xc2028.c
index b6ce528e1889..efcbc3ed0aea 100644
--- a/drivers/media/common/tuners/tuner-xc2028.c
+++ b/drivers/media/common/tuners/tuner-xc2028.c
@@ -685,7 +685,7 @@ static int check_firmware(struct dvb_frontend *fe, unsigned int type,
 {
 	struct xc2028_data         *priv = fe->tuner_priv;
 	struct firmware_properties new_fw;
-	int			   rc = 0, is_retry = 0;
+	int			   rc = 0, retry_count = 0;
 	u16			   version, hwmodel;
 	v4l2_std_id		   std0;
 
@@ -855,9 +855,9 @@ read_not_reliable:
 
 fail:
 	memset(&priv->cur_fw, 0, sizeof(priv->cur_fw));
-	if (!is_retry) {
+	if (retry_count < 8) {
 		msleep(50);
-		is_retry = 1;
+		retry_count++;
 		tuner_dbg("Retrying firmware load\n");
 		goto retry;
 	}
@@ -933,7 +933,7 @@ static int generic_set_freq(struct dvb_frontend *fe, u32 freq /* in HZ */,
 	 * that xc2028 will be in a safe state.
 	 * Maybe this might also be needed for DTV.
 	 */
-	if (new_mode == T_ANALOG_TV) {
+	if (new_mode == V4L2_TUNER_ANALOG_TV) {
 		rc = send_seq(priv, {0x00, 0x00});
 
 		/* Analog modes require offset = 0 */
@@ -1054,7 +1054,7 @@ static int xc2028_set_analog_freq(struct dvb_frontend *fe,
 		if (priv->ctrl.input1)
 			type |= INPUT1;
 		return generic_set_freq(fe, (625l * p->frequency) / 10,
-				T_RADIO, type, 0, 0);
+				V4L2_TUNER_RADIO, type, 0, 0);
 	}
 
 	/* if std is not defined, choose one */
@@ -1069,7 +1069,7 @@ static int xc2028_set_analog_freq(struct dvb_frontend *fe,
 	p->std |= parse_audio_std_option();
 
 	return generic_set_freq(fe, 62500l * p->frequency,
-				T_ANALOG_TV, type, p->std, 0);
+				V4L2_TUNER_ANALOG_TV, type, p->std, 0);
 }
 
 static int xc2028_set_params(struct dvb_frontend *fe,
@@ -1174,7 +1174,7 @@ static int xc2028_set_params(struct dvb_frontend *fe,
 	}
 
 	return generic_set_freq(fe, p->frequency,
-				T_DIGITAL_TV, type, 0, demod);
+				V4L2_TUNER_DIGITAL_TV, type, 0, demod);
 }
 
 static int xc2028_sleep(struct dvb_frontend *fe)
diff --git a/drivers/media/common/tuners/xc5000.c b/drivers/media/common/tuners/xc5000.c
index 76ac5cd84af7..1e28f7dcb26b 100644
--- a/drivers/media/common/tuners/xc5000.c
+++ b/drivers/media/common/tuners/xc5000.c
@@ -65,7 +65,7 @@ struct xc5000_priv {
 };
 
 /* Misc Defines */
-#define MAX_TV_STANDARD			23
+#define MAX_TV_STANDARD			24
 #define XC_MAX_I2C_WRITE_LENGTH		64
 
 /* Signal Types */
@@ -92,6 +92,8 @@ struct xc5000_priv {
 #define XREG_IF_OUT       0x05
 #define XREG_SEEK_MODE    0x07
 #define XREG_POWER_DOWN   0x0A /* Obsolete */
+/* Set the output amplitude - SIF for analog, DTVP/DTVN for digital */
+#define XREG_OUTPUT_AMP   0x0B
 #define XREG_SIGNALSOURCE 0x0D /* 0=Air, 1=Cable */
 #define XREG_SMOOTHEDCVBS 0x0E
 #define XREG_XTALFREQ     0x0F
@@ -173,6 +175,7 @@ struct XC_TV_STANDARD {
 #define DTV7			20
 #define FM_Radio_INPUT2 	21
 #define FM_Radio_INPUT1 	22
+#define FM_Radio_INPUT1_MONO	23
 
 static struct XC_TV_STANDARD XC5000_Standard[MAX_TV_STANDARD] = {
 	{"M/N-NTSC/PAL-BTSC", 0x0400, 0x8020},
@@ -197,7 +200,8 @@ static struct XC_TV_STANDARD XC5000_Standard[MAX_TV_STANDARD] = {
 	{"DTV7/8",            0x00C0, 0x801B},
 	{"DTV7",              0x00C0, 0x8007},
 	{"FM Radio-INPUT2",   0x9802, 0x9002},
-	{"FM Radio-INPUT1",   0x0208, 0x9002}
+	{"FM Radio-INPUT1",   0x0208, 0x9002},
+	{"FM Radio-INPUT1_MONO", 0x0278, 0x9002}
 };
 
 static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe);
@@ -683,6 +687,24 @@ static int xc5000_set_params(struct dvb_frontend *fe,
 			return -EINVAL;
 		}
 		priv->rf_mode = XC_RF_MODE_AIR;
+	} else if (fe->ops.info.type == FE_QAM) {
+		dprintk(1, "%s() QAM\n", __func__);
+		switch (params->u.qam.modulation) {
+		case QAM_16:
+		case QAM_32:
+		case QAM_64:
+		case QAM_128:
+		case QAM_256:
+		case QAM_AUTO:
+			dprintk(1, "%s() QAM modulation\n", __func__);
+			priv->bandwidth = BANDWIDTH_8_MHZ;
+			priv->video_standard = DTV7_8;
+			priv->freq_hz = params->frequency - 2750000;
+			priv->rf_mode = XC_RF_MODE_CABLE;
+			break;
+		default:
+			return -EINVAL;
+		}
 	} else {
 		printk(KERN_ERR "xc5000 modulation type not supported!\n");
 		return -EINVAL;
@@ -714,6 +736,8 @@ static int xc5000_set_params(struct dvb_frontend *fe,
 		return -EIO;
 	}
 
+	xc_write_reg(priv, XREG_OUTPUT_AMP, 0x8a);
+
 	xc_tune_channel(priv, priv->freq_hz, XC_TUNE_DIGITAL);
 
 	if (debug)
@@ -818,6 +842,8 @@ tune_channel:
 		return -EREMOTEIO;
 	}
 
+	xc_write_reg(priv, XREG_OUTPUT_AMP, 0x09);
+
 	xc_tune_channel(priv, priv->freq_hz, XC_TUNE_ANALOG);
 
 	if (debug)
@@ -845,6 +871,8 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 		radio_input = FM_Radio_INPUT1;
 	else if  (priv->radio_input == XC5000_RADIO_FM2)
 		radio_input = FM_Radio_INPUT2;
+	else if  (priv->radio_input == XC5000_RADIO_FM1_MONO)
+		radio_input = FM_Radio_INPUT1_MONO;
 	else {
 		dprintk(1, "%s() unknown radio input %d\n", __func__,
 			priv->radio_input);
@@ -871,6 +899,12 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 		return -EREMOTEIO;
 	}
 
+	if ((priv->radio_input == XC5000_RADIO_FM1) ||
+				(priv->radio_input == XC5000_RADIO_FM2))
+		xc_write_reg(priv, XREG_OUTPUT_AMP, 0x09);
+	else if  (priv->radio_input == XC5000_RADIO_FM1_MONO)
+		xc_write_reg(priv, XREG_OUTPUT_AMP, 0x06);
+
 	xc_tune_channel(priv, priv->freq_hz, XC_TUNE_ANALOG);
 
 	return 0;
@@ -1021,6 +1055,23 @@ static int xc5000_release(struct dvb_frontend *fe)
 	return 0;
 }
 
+static int xc5000_set_config(struct dvb_frontend *fe, void *priv_cfg)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+	struct xc5000_config *p = priv_cfg;
+
+	dprintk(1, "%s()\n", __func__);
+
+	if (p->if_khz)
+		priv->if_khz = p->if_khz;
+
+	if (p->radio_input)
+		priv->radio_input = p->radio_input;
+
+	return 0;
+}
+
+
 static const struct dvb_tuner_ops xc5000_tuner_ops = {
 	.info = {
 		.name           = "Xceive XC5000",
@@ -1033,6 +1084,7 @@ static const struct dvb_tuner_ops xc5000_tuner_ops = {
 	.init		   = xc5000_init,
 	.sleep		   = xc5000_sleep,
 
+	.set_config	   = xc5000_set_config,
 	.set_params	   = xc5000_set_params,
 	.set_analog_params = xc5000_set_analog_params,
 	.get_frequency	   = xc5000_get_frequency,
diff --git a/drivers/media/common/tuners/xc5000.h b/drivers/media/common/tuners/xc5000.h
index 3756e73649be..e2957451b532 100644
--- a/drivers/media/common/tuners/xc5000.h
+++ b/drivers/media/common/tuners/xc5000.h
@@ -40,6 +40,7 @@ struct xc5000_config {
 #define XC5000_RADIO_NOT_CONFIGURED		0
 #define XC5000_RADIO_FM1			1
 #define XC5000_RADIO_FM2			2
+#define XC5000_RADIO_FM1_MONO			3
 
 /* For each bridge framework, when it attaches either analog or digital,
  * it has to store a reference back to its _core equivalent structure,
diff --git a/drivers/media/dvb/Kconfig b/drivers/media/dvb/Kconfig
index 161ccfd471cb..ee214c3b63d7 100644
--- a/drivers/media/dvb/Kconfig
+++ b/drivers/media/dvb/Kconfig
@@ -65,7 +65,7 @@ comment "Supported SDMC DM1105 Adapters"
 source "drivers/media/dvb/dm1105/Kconfig"
 
 comment "Supported FireWire (IEEE 1394) Adapters"
-	depends on DVB_CORE && IEEE1394
+	depends on DVB_CORE && FIREWIRE
 source "drivers/media/dvb/firewire/Kconfig"
 
 comment "Supported Earthsoft PT1 Adapters"
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.h b/drivers/media/dvb/dvb-core/dvb_frontend.h
index f9f19be77181..3b860504bf04 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.h
@@ -239,7 +239,6 @@ struct analog_demod_ops {
 	void (*set_params)(struct dvb_frontend *fe,
 			   struct analog_parameters *params);
 	int  (*has_signal)(struct dvb_frontend *fe);
-	int  (*is_stereo)(struct dvb_frontend *fe);
 	int  (*get_afc)(struct dvb_frontend *fe);
 	void (*tuner_status)(struct dvb_frontend *fe);
 	void (*standby)(struct dvb_frontend *fe);
diff --git a/drivers/media/dvb/dvb-usb/Kconfig b/drivers/media/dvb/dvb-usb/Kconfig
index 3d48ba019342..fe4f894183ff 100644
--- a/drivers/media/dvb/dvb-usb/Kconfig
+++ b/drivers/media/dvb/dvb-usb/Kconfig
@@ -358,3 +358,11 @@ config DVB_USB_LME2510
 	select DVB_IX2505V if !DVB_FE_CUSTOMISE
 	help
 	  Say Y here to support the LME DM04/QQBOX DVB-S USB2.0 .
+
+config DVB_USB_TECHNISAT_USB2
+	tristate "Technisat DVB-S/S2 USB2.0 support"
+	depends on DVB_USB
+	select DVB_STB0899 if !DVB_FE_CUSTOMISE
+	select DVB_STB6100 if !DVB_FE_CUSTOMISE
+	help
+	  Say Y here to support the Technisat USB2 DVB-S/S2 device
diff --git a/drivers/media/dvb/dvb-usb/Makefile b/drivers/media/dvb/dvb-usb/Makefile
index 5b1d12f2d591..4bac13da0c39 100644
--- a/drivers/media/dvb/dvb-usb/Makefile
+++ b/drivers/media/dvb/dvb-usb/Makefile
@@ -91,6 +91,9 @@ obj-$(CONFIG_DVB_USB_AZ6027) += dvb-usb-az6027.o
 dvb-usb-lmedm04-objs = lmedm04.o
 obj-$(CONFIG_DVB_USB_LME2510) += dvb-usb-lmedm04.o
 
+dvb-usb-technisat-usb2-objs = technisat-usb2.o
+obj-$(CONFIG_DVB_USB_TECHNISAT_USB2) += dvb-usb-technisat-usb2.o
+
 EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core/ -Idrivers/media/dvb/frontends/
 # due to tuner-xc3028
 EXTRA_CFLAGS += -Idrivers/media/common/tuners
diff --git a/drivers/media/dvb/dvb-usb/dib0700.h b/drivers/media/dvb/dvb-usb/dib0700.h
index 3537d65c04bc..b2a87f2c2c3e 100644
--- a/drivers/media/dvb/dvb-usb/dib0700.h
+++ b/drivers/media/dvb/dvb-usb/dib0700.h
@@ -32,6 +32,7 @@ extern int dvb_usb_dib0700_debug;
 	// 1 Byte: 4MSB(1 = enable streaming, 0 = disable streaming) 4LSB(Video Mode: 0 = MPEG2 188Bytes, 1 = Analog)
 	// 2 Byte: MPEG2 mode:  4MSB(1 = Master Mode, 0 = Slave Mode) 4LSB(Channel 1 = bit0, Channel 2 = bit1)
 	// 2 Byte: Analog mode: 4MSB(0 = 625 lines, 1 = 525 lines)    4LSB(     "                "           )
+#define REQUEST_SET_I2C_PARAM       0x10
 #define REQUEST_SET_RC              0x11
 #define REQUEST_NEW_I2C_READ        0x12
 #define REQUEST_NEW_I2C_WRITE       0x13
@@ -61,6 +62,7 @@ extern struct i2c_algorithm dib0700_i2c_algo;
 extern int dib0700_identify_state(struct usb_device *udev, struct dvb_usb_device_properties *props,
 			struct dvb_usb_device_description **desc, int *cold);
 extern int dib0700_change_protocol(struct rc_dev *dev, u64 rc_type);
+extern int dib0700_set_i2c_speed(struct dvb_usb_device *d, u16 scl_kHz);
 
 extern int dib0700_device_count;
 extern int dvb_usb_dib0700_ir_proto;
diff --git a/drivers/media/dvb/dvb-usb/dib0700_core.c b/drivers/media/dvb/dvb-usb/dib0700_core.c
index 98ffb40728e3..b79af68c54ae 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_core.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_core.c
@@ -186,7 +186,7 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 						 msg[i].len,
 						 USB_CTRL_GET_TIMEOUT);
 			if (result < 0) {
-				err("i2c read error (status = %d)\n", result);
+				deb_info("i2c read error (status = %d)\n", result);
 				break;
 			}
 
@@ -215,7 +215,7 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 						 0, 0, buf, msg[i].len + 4,
 						 USB_CTRL_GET_TIMEOUT);
 			if (result < 0) {
-				err("i2c write error (status = %d)\n", result);
+				deb_info("i2c write error (status = %d)\n", result);
 				break;
 			}
 		}
@@ -328,6 +328,31 @@ static int dib0700_set_clock(struct dvb_usb_device *d, u8 en_pll,
 	return dib0700_ctrl_wr(d, b, 10);
 }
 
+int dib0700_set_i2c_speed(struct dvb_usb_device *d, u16 scl_kHz)
+{
+	u16 divider;
+	u8 b[8];
+
+	if (scl_kHz == 0)
+		return -EINVAL;
+
+	b[0] = REQUEST_SET_I2C_PARAM;
+	divider = (u16) (30000 / scl_kHz);
+	b[2] = (u8) (divider >> 8);
+	b[3] = (u8) (divider & 0xff);
+	divider = (u16) (72000 / scl_kHz);
+	b[4] = (u8) (divider >> 8);
+	b[5] = (u8) (divider & 0xff);
+	divider = (u16) (72000 / scl_kHz); /* clock: 72MHz */
+	b[6] = (u8) (divider >> 8);
+	b[7] = (u8) (divider & 0xff);
+
+	deb_info("setting I2C speed: %04x %04x %04x (%d kHz).",
+		(b[2] << 8) | (b[3]), (b[4] << 8) | b[5], (b[6] << 8) | b[7], scl_kHz);
+	return dib0700_ctrl_wr(d, b, 8);
+}
+
+
 int dib0700_ctrl_clock(struct dvb_usb_device *d, u32 clk_MHz, u8 clock_out_gp3)
 {
 	switch (clk_MHz) {
@@ -459,10 +484,20 @@ int dib0700_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 
 	deb_info("modifying (%d) streaming state for %d\n", onoff, adap->id);
 
-	if (onoff)
-		st->channel_state |=   1 << adap->id;
-	else
-		st->channel_state &= ~(1 << adap->id);
+	st->channel_state &= ~0x3;
+	if ((adap->stream.props.endpoint != 2)
+			&& (adap->stream.props.endpoint != 3)) {
+		deb_info("the endpoint number (%i) is not correct, use the adapter id instead", adap->stream.props.endpoint);
+		if (onoff)
+			st->channel_state |=	1 << (adap->id);
+		else
+			st->channel_state |=	1 << ~(adap->id);
+	} else {
+		if (onoff)
+			st->channel_state |=	1 << (adap->stream.props.endpoint-2);
+		else
+			st->channel_state |=	1 << (3-adap->stream.props.endpoint);
+	}
 
 	b[2] |= st->channel_state;
 
diff --git a/drivers/media/dvb/dvb-usb/dib0700_devices.c b/drivers/media/dvb/dvb-usb/dib0700_devices.c
index defd83964ce2..97af266d7f1d 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_devices.c
@@ -12,6 +12,7 @@
 #include "dib7000m.h"
 #include "dib7000p.h"
 #include "dib8000.h"
+#include "dib9000.h"
 #include "mt2060.h"
 #include "mt2266.h"
 #include "tuner-xc2028.h"
@@ -29,6 +30,7 @@ MODULE_PARM_DESC(force_lna_activation, "force the activation of Low-Noise-Amplif
 
 struct dib0700_adapter_state {
 	int (*set_param_save) (struct dvb_frontend *, struct dvb_frontend_parameters *);
+	const struct firmware *frontend_firmware;
 };
 
 /* Hauppauge Nova-T 500 (aka Bristol)
@@ -870,6 +872,23 @@ static int dib7070p_tuner_attach(struct dvb_usb_adapter *adap)
 	return 0;
 }
 
+static int stk7700p_pid_filter(struct dvb_usb_adapter *adapter, int index,
+		u16 pid, int onoff)
+{
+	struct dib0700_state *st = adapter->dev->priv;
+	if (st->is_dib7000pc)
+		return dib7000p_pid_filter(adapter->fe, index, pid, onoff);
+	return dib7000m_pid_filter(adapter->fe, index, pid, onoff);
+}
+
+static int stk7700p_pid_filter_ctrl(struct dvb_usb_adapter *adapter, int onoff)
+{
+	struct dib0700_state *st = adapter->dev->priv;
+	if (st->is_dib7000pc)
+		return dib7000p_pid_filter_ctrl(adapter->fe, onoff);
+	return dib7000m_pid_filter_ctrl(adapter->fe, onoff);
+}
+
 static int stk70x0p_pid_filter(struct dvb_usb_adapter *adapter, int index, u16 pid, int onoff)
 {
     return dib7000p_pid_filter(adapter->fe, index, pid, onoff);
@@ -1226,13 +1245,13 @@ static int dib807x_tuner_attach(struct dvb_usb_adapter *adap)
 static int stk80xx_pid_filter(struct dvb_usb_adapter *adapter, int index,
 	u16 pid, int onoff)
 {
-    return dib8000_pid_filter(adapter->fe, index, pid, onoff);
+	return dib8000_pid_filter(adapter->fe, index, pid, onoff);
 }
 
 static int stk80xx_pid_filter_ctrl(struct dvb_usb_adapter *adapter,
-	int onoff)
+		int onoff)
 {
-    return dib8000_pid_filter_ctrl(adapter->fe, onoff);
+	return dib8000_pid_filter_ctrl(adapter->fe, onoff);
 }
 
 /* STK807x */
@@ -1304,11 +1323,11 @@ static int stk807xpvr_frontend_attach1(struct dvb_usb_adapter *adap)
 
 /* STK8096GP */
 struct dibx000_agc_config dib8090_agc_config[2] = {
-    {
+	{
 	BAND_UHF | BAND_VHF | BAND_LBAND | BAND_SBAND,
 	/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=1,
-     * P_agc_inv_pwm1=0, P_agc_inv_pwm2=0, P_agc_inh_dc_rv_est=0,
-     * P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
+	 * P_agc_inv_pwm1=0, P_agc_inv_pwm2=0, P_agc_inh_dc_rv_est=0,
+	 * P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
 	(0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8)
 	| (3 << 5) | (0 << 4) | (5 << 1) | (0 << 0),
 
@@ -1345,12 +1364,12 @@ struct dibx000_agc_config dib8090_agc_config[2] = {
 	51,
 
 	0,
-    },
-    {
+	},
+	{
 	BAND_CBAND,
 	/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=1,
-     * P_agc_inv_pwm1=0, P_agc_inv_pwm2=0, P_agc_inh_dc_rv_est=0,
-     * P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
+	 * P_agc_inv_pwm1=0, P_agc_inv_pwm2=0, P_agc_inh_dc_rv_est=0,
+	 * P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
 	(0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8)
 	| (3 << 5) | (0 << 4) | (5 << 1) | (0 << 0),
 
@@ -1387,135 +1406,153 @@ struct dibx000_agc_config dib8090_agc_config[2] = {
 	51,
 
 	0,
-    }
+	}
 };
 
 static struct dibx000_bandwidth_config dib8090_pll_config_12mhz = {
-    54000, 13500,
-    1, 18, 3, 1, 0,
-    0, 0, 1, 1, 2,
-    (3 << 14) | (1 << 12) | (599 << 0),
-    (0 << 25) | 0,
-    20199727,
-    12000000,
+	54000, 13500,
+	1, 18, 3, 1, 0,
+	0, 0, 1, 1, 2,
+	(3 << 14) | (1 << 12) | (599 << 0),
+	(0 << 25) | 0,
+	20199727,
+	12000000,
 };
 
 static int dib8090_get_adc_power(struct dvb_frontend *fe)
 {
-    return dib8000_get_adc_power(fe, 1);
+	return dib8000_get_adc_power(fe, 1);
 }
 
-static struct dib8000_config dib809x_dib8000_config = {
-    .output_mpeg2_in_188_bytes = 1,
-
-    .agc_config_count = 2,
-    .agc = dib8090_agc_config,
-    .agc_control = dib0090_dcc_freq,
-    .pll = &dib8090_pll_config_12mhz,
-    .tuner_is_baseband = 1,
-
-    .gpio_dir = DIB8000_GPIO_DEFAULT_DIRECTIONS,
-    .gpio_val = DIB8000_GPIO_DEFAULT_VALUES,
-    .gpio_pwm_pos = DIB8000_GPIO_DEFAULT_PWM_POS,
-
-    .hostbus_diversity = 1,
-    .div_cfg = 0x31,
-    .output_mode = OUTMODE_MPEG2_FIFO,
-    .drives = 0x2d98,
-    .diversity_delay = 144,
-    .refclksel = 3,
+static struct dib8000_config dib809x_dib8000_config[2] = {
+	{
+	.output_mpeg2_in_188_bytes = 1,
+
+	.agc_config_count = 2,
+	.agc = dib8090_agc_config,
+	.agc_control = dib0090_dcc_freq,
+	.pll = &dib8090_pll_config_12mhz,
+	.tuner_is_baseband = 1,
+
+	.gpio_dir = DIB8000_GPIO_DEFAULT_DIRECTIONS,
+	.gpio_val = DIB8000_GPIO_DEFAULT_VALUES,
+	.gpio_pwm_pos = DIB8000_GPIO_DEFAULT_PWM_POS,
+
+	.hostbus_diversity = 1,
+	.div_cfg = 0x31,
+	.output_mode = OUTMODE_MPEG2_FIFO,
+	.drives = 0x2d98,
+	.diversity_delay = 48,
+	.refclksel = 3,
+	}, {
+	.output_mpeg2_in_188_bytes = 1,
+
+	.agc_config_count = 2,
+	.agc = dib8090_agc_config,
+	.agc_control = dib0090_dcc_freq,
+	.pll = &dib8090_pll_config_12mhz,
+	.tuner_is_baseband = 1,
+
+	.gpio_dir = DIB8000_GPIO_DEFAULT_DIRECTIONS,
+	.gpio_val = DIB8000_GPIO_DEFAULT_VALUES,
+	.gpio_pwm_pos = DIB8000_GPIO_DEFAULT_PWM_POS,
+
+	.hostbus_diversity = 1,
+	.div_cfg = 0x31,
+	.output_mode = OUTMODE_DIVERSITY,
+	.drives = 0x2d08,
+	.diversity_delay = 1,
+	.refclksel = 3,
+	}
+};
+
+static struct dib0090_wbd_slope dib8090_wbd_table[] = {
+	/* max freq ; cold slope ; cold offset ; warm slope ; warm offset ; wbd gain */
+	{ 120,     0, 500,  0,   500, 4 }, /* CBAND */
+	{ 170,     0, 450,  0,   450, 4 }, /* CBAND */
+	{ 380,    48, 373, 28,   259, 6 }, /* VHF */
+	{ 860,    34, 700, 36,   616, 6 }, /* high UHF */
+	{ 0xFFFF, 34, 700, 36,   616, 6 }, /* default */
 };
 
 static struct dib0090_config dib809x_dib0090_config = {
-    .io.pll_bypass = 1,
-    .io.pll_range = 1,
-    .io.pll_prediv = 1,
-    .io.pll_loopdiv = 20,
-    .io.adc_clock_ratio = 8,
-    .io.pll_int_loop_filt = 0,
-    .io.clock_khz = 12000,
-    .reset = dib80xx_tuner_reset,
-    .sleep = dib80xx_tuner_sleep,
-    .clkouttobamse = 1,
-    .analog_output = 1,
-    .i2c_address = DEFAULT_DIB0090_I2C_ADDRESS,
-    .wbd_vhf_offset = 100,
-    .wbd_cband_offset = 450,
-    .use_pwm_agc = 1,
-    .clkoutdrive = 1,
-    .get_adc_power = dib8090_get_adc_power,
-	.freq_offset_khz_uhf = 0,
+	.io.pll_bypass = 1,
+	.io.pll_range = 1,
+	.io.pll_prediv = 1,
+	.io.pll_loopdiv = 20,
+	.io.adc_clock_ratio = 8,
+	.io.pll_int_loop_filt = 0,
+	.io.clock_khz = 12000,
+	.reset = dib80xx_tuner_reset,
+	.sleep = dib80xx_tuner_sleep,
+	.clkouttobamse = 1,
+	.analog_output = 1,
+	.i2c_address = DEFAULT_DIB0090_I2C_ADDRESS,
+	.use_pwm_agc = 1,
+	.clkoutdrive = 1,
+	.get_adc_power = dib8090_get_adc_power,
+	.freq_offset_khz_uhf = -63,
 	.freq_offset_khz_vhf = -143,
+	.wbd = dib8090_wbd_table,
+	.fref_clock_ratio = 6,
 };
 
 static int dib8096_set_param_override(struct dvb_frontend *fe,
 		struct dvb_frontend_parameters *fep)
 {
-    struct dvb_usb_adapter *adap = fe->dvb->priv;
-    struct dib0700_adapter_state *state = adap->priv;
-    u8 band = BAND_OF_FREQUENCY(fep->frequency/1000);
-    u16 offset;
-    int ret = 0;
-    enum frontend_tune_state tune_state = CT_SHUTDOWN;
-    u16 ltgain, rf_gain_limit;
-
-    ret = state->set_param_save(fe, fep);
-    if (ret < 0)
-	return ret;
-
-    switch (band) {
-    case BAND_VHF:
-	    offset = 100;
-	    break;
-    case BAND_UHF:
-	    offset = 550;
-	    break;
-    default:
-	    offset = 0;
-	    break;
-    }
-    offset += (dib0090_get_wbd_offset(fe) * 8 * 18 / 33 + 1) / 2;
-    dib8000_set_wbd_ref(fe, offset);
-
-
-    if (band == BAND_CBAND) {
-	deb_info("tuning in CBAND - soft-AGC startup\n");
-	/* TODO specific wbd target for dib0090 - needed for startup ? */
-	dib0090_set_tune_state(fe, CT_AGC_START);
-	do {
-		ret = dib0090_gain_control(fe);
-		msleep(ret);
-		tune_state = dib0090_get_tune_state(fe);
-		if (tune_state == CT_AGC_STEP_0)
-			dib8000_set_gpio(fe, 6, 0, 1);
-		else if (tune_state == CT_AGC_STEP_1) {
-			dib0090_get_current_gain(fe, NULL, NULL, &rf_gain_limit, &ltgain);
-			if (rf_gain_limit == 0)
-				dib8000_set_gpio(fe, 6, 0, 0);
-		}
-	} while (tune_state < CT_AGC_STOP);
-	dib0090_pwm_gain_reset(fe);
-	dib8000_pwm_agc_reset(fe);
-	dib8000_set_tune_state(fe, CT_DEMOD_START);
-    } else {
-	deb_info("not tuning in CBAND - standard AGC startup\n");
-	dib0090_pwm_gain_reset(fe);
-    }
+	struct dvb_usb_adapter *adap = fe->dvb->priv;
+	struct dib0700_adapter_state *state = adap->priv;
+	u8 band = BAND_OF_FREQUENCY(fep->frequency/1000);
+	u16 target;
+	int ret = 0;
+	enum frontend_tune_state tune_state = CT_SHUTDOWN;
+	u16 ltgain, rf_gain_limit;
+
+	ret = state->set_param_save(fe, fep);
+	if (ret < 0)
+		return ret;
+
+	target = (dib0090_get_wbd_offset(fe) * 8 * 18 / 33 + 1) / 2;
+	dib8000_set_wbd_ref(fe, target);
+
+
+	if (band == BAND_CBAND) {
+		deb_info("tuning in CBAND - soft-AGC startup\n");
+		dib0090_set_tune_state(fe, CT_AGC_START);
+		do {
+			ret = dib0090_gain_control(fe);
+			msleep(ret);
+			tune_state = dib0090_get_tune_state(fe);
+			if (tune_state == CT_AGC_STEP_0)
+				dib8000_set_gpio(fe, 6, 0, 1);
+			else if (tune_state == CT_AGC_STEP_1) {
+				dib0090_get_current_gain(fe, NULL, NULL, &rf_gain_limit, &ltgain);
+				if (rf_gain_limit == 0)
+					dib8000_set_gpio(fe, 6, 0, 0);
+			}
+		} while (tune_state < CT_AGC_STOP);
+		dib0090_pwm_gain_reset(fe);
+		dib8000_pwm_agc_reset(fe);
+		dib8000_set_tune_state(fe, CT_DEMOD_START);
+	} else {
+		deb_info("not tuning in CBAND - standard AGC startup\n");
+		dib0090_pwm_gain_reset(fe);
+	}
 
-    return 0;
+	return 0;
 }
 
 static int dib809x_tuner_attach(struct dvb_usb_adapter *adap)
 {
-    struct dib0700_adapter_state *st = adap->priv;
-    struct i2c_adapter *tun_i2c = dib8000_get_i2c_master(adap->fe, DIBX000_I2C_INTERFACE_TUNER, 1);
+	struct dib0700_adapter_state *st = adap->priv;
+	struct i2c_adapter *tun_i2c = dib8000_get_i2c_master(adap->fe, DIBX000_I2C_INTERFACE_TUNER, 1);
 
-    if (dvb_attach(dib0090_register, adap->fe, tun_i2c, &dib809x_dib0090_config) == NULL)
-	return -ENODEV;
+	if (dvb_attach(dib0090_register, adap->fe, tun_i2c, &dib809x_dib0090_config) == NULL)
+		return -ENODEV;
 
-    st->set_param_save = adap->fe->ops.tuner_ops.set_params;
-    adap->fe->ops.tuner_ops.set_params = dib8096_set_param_override;
-    return 0;
+	st->set_param_save = adap->fe->ops.tuner_ops.set_params;
+	adap->fe->ops.tuner_ops.set_params = dib8096_set_param_override;
+	return 0;
 }
 
 static int stk809x_frontend_attach(struct dvb_usb_adapter *adap)
@@ -1537,11 +1574,931 @@ static int stk809x_frontend_attach(struct dvb_usb_adapter *adap)
 
 	dib8000_i2c_enumeration(&adap->dev->i2c_adap, 1, 18, 0x80);
 
-	adap->fe = dvb_attach(dib8000_attach, &adap->dev->i2c_adap, 0x80, &dib809x_dib8000_config);
+	adap->fe = dvb_attach(dib8000_attach, &adap->dev->i2c_adap, 0x80, &dib809x_dib8000_config[0]);
+
+	return adap->fe == NULL ?  -ENODEV : 0;
+}
+
+static int nim8096md_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *st = adap->priv;
+	struct i2c_adapter *tun_i2c;
+	struct dvb_frontend *fe_slave  = dib8000_get_slave_frontend(adap->fe, 1);
+
+	if (fe_slave) {
+		tun_i2c = dib8000_get_i2c_master(fe_slave, DIBX000_I2C_INTERFACE_TUNER, 1);
+		if (dvb_attach(dib0090_register, fe_slave, tun_i2c, &dib809x_dib0090_config) == NULL)
+			return -ENODEV;
+		fe_slave->dvb = adap->fe->dvb;
+		fe_slave->ops.tuner_ops.set_params = dib8096_set_param_override;
+	}
+	tun_i2c = dib8000_get_i2c_master(adap->fe, DIBX000_I2C_INTERFACE_TUNER, 1);
+	if (dvb_attach(dib0090_register, adap->fe, tun_i2c, &dib809x_dib0090_config) == NULL)
+		return -ENODEV;
+
+	st->set_param_save = adap->fe->ops.tuner_ops.set_params;
+	adap->fe->ops.tuner_ops.set_params = dib8096_set_param_override;
+
+	return 0;
+}
+
+static int nim8096md_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvb_frontend *fe_slave;
+
+	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 0);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
+	msleep(1000);
+	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
+
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
+
+	dib0700_ctrl_clock(adap->dev, 72, 1);
+
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
+
+	dib8000_i2c_enumeration(&adap->dev->i2c_adap, 2, 18, 0x80);
+
+	adap->fe = dvb_attach(dib8000_attach, &adap->dev->i2c_adap, 0x80, &dib809x_dib8000_config[0]);
+	if (adap->fe == NULL)
+		return -ENODEV;
+
+	fe_slave = dvb_attach(dib8000_attach, &adap->dev->i2c_adap, 0x82, &dib809x_dib8000_config[1]);
+	dib8000_set_slave_frontend(adap->fe, fe_slave);
+
+	return fe_slave == NULL ?  -ENODEV : 0;
+}
+
+/* STK9090M */
+static int dib90x0_pid_filter(struct dvb_usb_adapter *adapter, int index, u16 pid, int onoff)
+{
+	return dib9000_fw_pid_filter(adapter->fe, index, pid, onoff);
+}
+
+static int dib90x0_pid_filter_ctrl(struct dvb_usb_adapter *adapter, int onoff)
+{
+	return dib9000_fw_pid_filter_ctrl(adapter->fe, onoff);
+}
+
+static int dib90x0_tuner_reset(struct dvb_frontend *fe, int onoff)
+{
+	return dib9000_set_gpio(fe, 5, 0, !onoff);
+}
+
+static int dib90x0_tuner_sleep(struct dvb_frontend *fe, int onoff)
+{
+	return dib9000_set_gpio(fe, 0, 0, onoff);
+}
+
+static int dib01x0_pmu_update(struct i2c_adapter *i2c, u16 *data, u8 len)
+{
+	u8 wb[4] = { 0xc >> 8, 0xc & 0xff, 0, 0 };
+	u8 rb[2];
+	struct i2c_msg msg[2] = {
+		{.addr = 0x1e >> 1, .flags = 0, .buf = wb, .len = 2},
+		{.addr = 0x1e >> 1, .flags = I2C_M_RD, .buf = rb, .len = 2},
+	};
+	u8 index_data;
+
+	dibx000_i2c_set_speed(i2c, 250);
+
+	if (i2c_transfer(i2c, msg, 2) != 2)
+		return -EIO;
+
+	switch (rb[0] << 8 | rb[1]) {
+	case 0:
+			deb_info("Found DiB0170 rev1: This version of DiB0170 is not supported any longer.\n");
+			return -EIO;
+	case 1:
+			deb_info("Found DiB0170 rev2");
+			break;
+	case 2:
+			deb_info("Found DiB0190 rev2");
+			break;
+	default:
+			deb_info("DiB01x0 not found");
+			return -EIO;
+	}
+
+	for (index_data = 0; index_data < len; index_data += 2) {
+		wb[2] = (data[index_data + 1] >> 8) & 0xff;
+		wb[3] = (data[index_data + 1]) & 0xff;
+
+		if (data[index_data] == 0) {
+			wb[0] = (data[index_data] >> 8) & 0xff;
+			wb[1] = (data[index_data]) & 0xff;
+			msg[0].len = 2;
+			if (i2c_transfer(i2c, msg, 2) != 2)
+				return -EIO;
+			wb[2] |= rb[0];
+			wb[3] |= rb[1] & ~(3 << 4);
+		}
+
+		wb[0] = (data[index_data] >> 8)&0xff;
+		wb[1] = (data[index_data])&0xff;
+		msg[0].len = 4;
+		if (i2c_transfer(i2c, &msg[0], 1) != 1)
+			return -EIO;
+	}
+	return 0;
+}
+
+static struct dib9000_config stk9090m_config = {
+	.output_mpeg2_in_188_bytes = 1,
+	.output_mode = OUTMODE_MPEG2_FIFO,
+	.vcxo_timer = 279620,
+	.timing_frequency = 20452225,
+	.demod_clock_khz = 60000,
+	.xtal_clock_khz = 30000,
+	.if_drives = (0 << 15) | (1 << 13) | (0 << 12) | (3 << 10) | (0 << 9) | (1 << 7) | (0 << 6) | (0 << 4) | (1 << 3) | (1 << 1) | (0),
+	.subband = {
+		2,
+		{
+			{ 240, { BOARD_GPIO_COMPONENT_DEMOD, BOARD_GPIO_FUNCTION_SUBBAND_GPIO, 0x0008, 0x0000, 0x0008 } }, /* GPIO 3 to 1 for VHF */
+			{ 890, { BOARD_GPIO_COMPONENT_DEMOD, BOARD_GPIO_FUNCTION_SUBBAND_GPIO, 0x0008, 0x0000, 0x0000 } }, /* GPIO 3 to 0 for UHF */
+			{ 0 },
+		},
+	},
+	.gpio_function = {
+		{ .component = BOARD_GPIO_COMPONENT_DEMOD, .function = BOARD_GPIO_FUNCTION_COMPONENT_ON, .mask = 0x10 | 0x21, .direction = 0 & ~0x21, .value = (0x10 & ~0x1) | 0x20 },
+		{ .component = BOARD_GPIO_COMPONENT_DEMOD, .function = BOARD_GPIO_FUNCTION_COMPONENT_OFF, .mask = 0x10 | 0x21, .direction = 0 & ~0x21, .value = 0 | 0x21 },
+	},
+};
+
+static struct dib9000_config nim9090md_config[2] = {
+	{
+		.output_mpeg2_in_188_bytes = 1,
+		.output_mode = OUTMODE_MPEG2_FIFO,
+		.vcxo_timer = 279620,
+		.timing_frequency = 20452225,
+		.demod_clock_khz = 60000,
+		.xtal_clock_khz = 30000,
+		.if_drives = (0 << 15) | (1 << 13) | (0 << 12) | (3 << 10) | (0 << 9) | (1 << 7) | (0 << 6) | (0 << 4) | (1 << 3) | (1 << 1) | (0),
+	}, {
+		.output_mpeg2_in_188_bytes = 1,
+		.output_mode = OUTMODE_DIVERSITY,
+		.vcxo_timer = 279620,
+		.timing_frequency = 20452225,
+		.demod_clock_khz = 60000,
+		.xtal_clock_khz = 30000,
+		.if_drives = (0 << 15) | (1 << 13) | (0 << 12) | (3 << 10) | (0 << 9) | (1 << 7) | (0 << 6) | (0 << 4) | (1 << 3) | (1 << 1) | (0),
+		.subband = {
+			2,
+			{
+				{ 240, { BOARD_GPIO_COMPONENT_DEMOD, BOARD_GPIO_FUNCTION_SUBBAND_GPIO, 0x0006, 0x0000, 0x0006 } }, /* GPIO 1 and 2 to 1 for VHF */
+				{ 890, { BOARD_GPIO_COMPONENT_DEMOD, BOARD_GPIO_FUNCTION_SUBBAND_GPIO, 0x0006, 0x0000, 0x0000 } }, /* GPIO 1 and 2 to 0 for UHF */
+				{ 0 },
+			},
+		},
+		.gpio_function = {
+			{ .component = BOARD_GPIO_COMPONENT_DEMOD, .function = BOARD_GPIO_FUNCTION_COMPONENT_ON, .mask = 0x10 | 0x21, .direction = 0 & ~0x21, .value = (0x10 & ~0x1) | 0x20 },
+			{ .component = BOARD_GPIO_COMPONENT_DEMOD, .function = BOARD_GPIO_FUNCTION_COMPONENT_OFF, .mask = 0x10 | 0x21, .direction = 0 & ~0x21, .value = 0 | 0x21 },
+		},
+	}
+};
+
+static struct dib0090_config dib9090_dib0090_config = {
+	.io.pll_bypass = 0,
+	.io.pll_range = 1,
+	.io.pll_prediv = 1,
+	.io.pll_loopdiv = 8,
+	.io.adc_clock_ratio = 8,
+	.io.pll_int_loop_filt = 0,
+	.io.clock_khz = 30000,
+	.reset = dib90x0_tuner_reset,
+	.sleep = dib90x0_tuner_sleep,
+	.clkouttobamse = 0,
+	.analog_output = 0,
+	.use_pwm_agc = 0,
+	.clkoutdrive = 0,
+	.freq_offset_khz_uhf = 0,
+	.freq_offset_khz_vhf = 0,
+};
+
+static struct dib0090_config nim9090md_dib0090_config[2] = {
+	{
+		.io.pll_bypass = 0,
+		.io.pll_range = 1,
+		.io.pll_prediv = 1,
+		.io.pll_loopdiv = 8,
+		.io.adc_clock_ratio = 8,
+		.io.pll_int_loop_filt = 0,
+		.io.clock_khz = 30000,
+		.reset = dib90x0_tuner_reset,
+		.sleep = dib90x0_tuner_sleep,
+		.clkouttobamse = 1,
+		.analog_output = 0,
+		.use_pwm_agc = 0,
+		.clkoutdrive = 0,
+		.freq_offset_khz_uhf = 0,
+		.freq_offset_khz_vhf = 0,
+	}, {
+		.io.pll_bypass = 0,
+		.io.pll_range = 1,
+		.io.pll_prediv = 1,
+		.io.pll_loopdiv = 8,
+		.io.adc_clock_ratio = 8,
+		.io.pll_int_loop_filt = 0,
+		.io.clock_khz = 30000,
+		.reset = dib90x0_tuner_reset,
+		.sleep = dib90x0_tuner_sleep,
+		.clkouttobamse = 0,
+		.analog_output = 0,
+		.use_pwm_agc = 0,
+		.clkoutdrive = 0,
+		.freq_offset_khz_uhf = 0,
+		.freq_offset_khz_vhf = 0,
+	}
+};
+
+
+static int stk9090m_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *state = adap->priv;
+	struct dib0700_state *st = adap->dev->priv;
+	u32 fw_version;
+
+	/* Make use of the new i2c functions from FW 1.20 */
+	dib0700_get_version(adap->dev, NULL, NULL, &fw_version, NULL);
+	if (fw_version >= 0x10200)
+		st->fw_use_new_i2c_api = 1;
+	dib0700_set_i2c_speed(adap->dev, 340);
+
+	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
+
+	dib0700_ctrl_clock(adap->dev, 72, 1);
+
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
+
+	dib9000_i2c_enumeration(&adap->dev->i2c_adap, 1, 0x10, 0x80);
+
+	if (request_firmware(&state->frontend_firmware, "dib9090.fw", &adap->dev->udev->dev)) {
+		deb_info("%s: Upload failed. (file not found?)\n", __func__);
+		return -ENODEV;
+	} else {
+		deb_info("%s: firmware read %Zu bytes.\n", __func__, state->frontend_firmware->size);
+	}
+	stk9090m_config.microcode_B_fe_size = state->frontend_firmware->size;
+	stk9090m_config.microcode_B_fe_buffer = state->frontend_firmware->data;
+
+	adap->fe = dvb_attach(dib9000_attach, &adap->dev->i2c_adap, 0x80, &stk9090m_config);
+
+	return adap->fe == NULL ?  -ENODEV : 0;
+}
+
+static int dib9090_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *state = adap->priv;
+	struct i2c_adapter *i2c = dib9000_get_tuner_interface(adap->fe);
+	u16 data_dib190[10] = {
+		1, 0x1374,
+		2, 0x01a2,
+		7, 0x0020,
+		0, 0x00ef,
+		8, 0x0486,
+	};
+
+	if (dvb_attach(dib0090_fw_register, adap->fe, i2c, &dib9090_dib0090_config) == NULL)
+		return -ENODEV;
+	i2c = dib9000_get_i2c_master(adap->fe, DIBX000_I2C_INTERFACE_GPIO_1_2, 0);
+	if (dib01x0_pmu_update(i2c, data_dib190, 10) != 0)
+		return -ENODEV;
+	dib0700_set_i2c_speed(adap->dev, 2000);
+	if (dib9000_firmware_post_pll_init(adap->fe) < 0)
+		return -ENODEV;
+	release_firmware(state->frontend_firmware);
+	return 0;
+}
+
+static int nim9090md_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *state = adap->priv;
+	struct dib0700_state *st = adap->dev->priv;
+	struct i2c_adapter *i2c;
+	struct dvb_frontend *fe_slave;
+	u32 fw_version;
+
+	/* Make use of the new i2c functions from FW 1.20 */
+	dib0700_get_version(adap->dev, NULL, NULL, &fw_version, NULL);
+	if (fw_version >= 0x10200)
+		st->fw_use_new_i2c_api = 1;
+	dib0700_set_i2c_speed(adap->dev, 340);
+
+	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
+
+	dib0700_ctrl_clock(adap->dev, 72, 1);
+
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
+
+	if (request_firmware(&state->frontend_firmware, "dib9090.fw", &adap->dev->udev->dev)) {
+		deb_info("%s: Upload failed. (file not found?)\n", __func__);
+		return -EIO;
+	} else {
+		deb_info("%s: firmware read %Zu bytes.\n", __func__, state->frontend_firmware->size);
+	}
+	nim9090md_config[0].microcode_B_fe_size = state->frontend_firmware->size;
+	nim9090md_config[0].microcode_B_fe_buffer = state->frontend_firmware->data;
+	nim9090md_config[1].microcode_B_fe_size = state->frontend_firmware->size;
+	nim9090md_config[1].microcode_B_fe_buffer = state->frontend_firmware->data;
+
+	dib9000_i2c_enumeration(&adap->dev->i2c_adap, 1, 0x20, 0x80);
+	adap->fe = dvb_attach(dib9000_attach, &adap->dev->i2c_adap, 0x80, &nim9090md_config[0]);
+
+	if (adap->fe == NULL)
+		return -ENODEV;
+
+	i2c = dib9000_get_i2c_master(adap->fe, DIBX000_I2C_INTERFACE_GPIO_3_4, 0);
+	dib9000_i2c_enumeration(i2c, 1, 0x12, 0x82);
+
+	fe_slave = dvb_attach(dib9000_attach, i2c, 0x82, &nim9090md_config[1]);
+	dib9000_set_slave_frontend(adap->fe, fe_slave);
+
+	return fe_slave == NULL ?  -ENODEV : 0;
+}
+
+static int nim9090md_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *state = adap->priv;
+	struct i2c_adapter *i2c;
+	struct dvb_frontend *fe_slave;
+	u16 data_dib190[10] = {
+		1, 0x5374,
+		2, 0x01ae,
+		7, 0x0020,
+		0, 0x00ef,
+		8, 0x0406,
+	};
+	i2c = dib9000_get_tuner_interface(adap->fe);
+	if (dvb_attach(dib0090_fw_register, adap->fe, i2c, &nim9090md_dib0090_config[0]) == NULL)
+		return -ENODEV;
+	i2c = dib9000_get_i2c_master(adap->fe, DIBX000_I2C_INTERFACE_GPIO_1_2, 0);
+	if (dib01x0_pmu_update(i2c, data_dib190, 10) < 0)
+		return -ENODEV;
+	dib0700_set_i2c_speed(adap->dev, 2000);
+	if (dib9000_firmware_post_pll_init(adap->fe) < 0)
+		return -ENODEV;
+
+	fe_slave = dib9000_get_slave_frontend(adap->fe, 1);
+	if (fe_slave != NULL) {
+		i2c = dib9000_get_component_bus_interface(adap->fe);
+		dib9000_set_i2c_adapter(fe_slave, i2c);
+
+		i2c = dib9000_get_tuner_interface(fe_slave);
+		if (dvb_attach(dib0090_fw_register, fe_slave, i2c, &nim9090md_dib0090_config[1]) == NULL)
+			return -ENODEV;
+		fe_slave->dvb = adap->fe->dvb;
+		dib9000_fw_set_component_bus_speed(adap->fe, 2000);
+		if (dib9000_firmware_post_pll_init(fe_slave) < 0)
+			return -ENODEV;
+	}
+	release_firmware(state->frontend_firmware);
+
+	return 0;
+}
+
+/* NIM7090 */
+struct dib7090p_best_adc {
+	u32 timf;
+	u32 pll_loopdiv;
+	u32 pll_prediv;
+};
+
+static int dib7090p_get_best_sampling(struct dvb_frontend *fe , struct dib7090p_best_adc *adc)
+{
+	u8 spur = 0, prediv = 0, loopdiv = 0, min_prediv = 1, max_prediv = 1;
+
+	u16 xtal = 12000;
+	u32 fcp_min = 1900;  /* PLL Minimum Frequency comparator KHz */
+	u32 fcp_max = 20000; /* PLL Maximum Frequency comparator KHz */
+	u32 fdem_max = 76000;
+	u32 fdem_min = 69500;
+	u32 fcp = 0, fs = 0, fdem = 0;
+	u32 harmonic_id = 0;
+
+	adc->pll_loopdiv = loopdiv;
+	adc->pll_prediv = prediv;
+	adc->timf = 0;
+
+	deb_info("bandwidth = %d fdem_min =%d", fe->dtv_property_cache.bandwidth_hz, fdem_min);
+
+	/* Find Min and Max prediv */
+	while ((xtal/max_prediv) >= fcp_min)
+		max_prediv++;
+
+	max_prediv--;
+	min_prediv = max_prediv;
+	while ((xtal/min_prediv) <= fcp_max) {
+		min_prediv--;
+		if (min_prediv == 1)
+			break;
+	}
+	deb_info("MIN prediv = %d : MAX prediv = %d", min_prediv, max_prediv);
+
+	min_prediv = 2;
+
+	for (prediv = min_prediv ; prediv < max_prediv; prediv++) {
+		fcp = xtal / prediv;
+		if (fcp > fcp_min && fcp < fcp_max) {
+			for (loopdiv = 1 ; loopdiv < 64 ; loopdiv++) {
+				fdem = ((xtal/prediv) * loopdiv);
+				fs   = fdem / 4;
+				/* test min/max system restrictions */
+
+				if ((fdem >= fdem_min) && (fdem <= fdem_max) && (fs >= fe->dtv_property_cache.bandwidth_hz/1000)) {
+					spur = 0;
+					/* test fs harmonics positions */
+					for (harmonic_id = (fe->dtv_property_cache.frequency / (1000*fs)) ;  harmonic_id <= ((fe->dtv_property_cache.frequency / (1000*fs))+1) ; harmonic_id++) {
+						if (((fs*harmonic_id) >= ((fe->dtv_property_cache.frequency/1000) - (fe->dtv_property_cache.bandwidth_hz/2000))) &&  ((fs*harmonic_id) <= ((fe->dtv_property_cache.frequency/1000) + (fe->dtv_property_cache.bandwidth_hz/2000)))) {
+							spur = 1;
+							break;
+						}
+					}
+
+					if (!spur) {
+						adc->pll_loopdiv = loopdiv;
+						adc->pll_prediv = prediv;
+						adc->timf = 2396745143UL/fdem*(1 << 9);
+						adc->timf += ((2396745143UL%fdem) << 9)/fdem;
+						deb_info("loopdiv=%i prediv=%i timf=%i", loopdiv, prediv, adc->timf);
+						break;
+					}
+				}
+			}
+		}
+		if (!spur)
+			break;
+	}
+
+
+	if (adc->pll_loopdiv == 0 && adc->pll_prediv == 0)
+		return -EINVAL;
+	else
+		return 0;
+}
+
+static int dib7090_agc_startup(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
+{
+	struct dvb_usb_adapter *adap = fe->dvb->priv;
+	struct dib0700_adapter_state *state = adap->priv;
+	struct dibx000_bandwidth_config pll;
+	u16 target;
+	struct dib7090p_best_adc adc;
+	int ret;
+
+	ret = state->set_param_save(fe, fep);
+	if (ret < 0)
+		return ret;
+
+	memset(&pll, 0, sizeof(struct dibx000_bandwidth_config));
+	dib0090_pwm_gain_reset(fe);
+	target = (dib0090_get_wbd_offset(fe) * 8 + 1) / 2;
+	dib7000p_set_wbd_ref(fe, target);
+
+	if (dib7090p_get_best_sampling(fe, &adc) == 0) {
+		pll.pll_ratio  = adc.pll_loopdiv;
+		pll.pll_prediv = adc.pll_prediv;
+
+		dib7000p_update_pll(fe, &pll);
+		dib7000p_ctrl_timf(fe, DEMOD_TIMF_SET, adc.timf);
+	}
+	return 0;
+}
+
+static struct dib0090_wbd_slope dib7090_wbd_table[] = {
+	{ 380,   81, 850, 64, 540,  4},
+	{ 860,   51, 866, 21,  375, 4},
+	{1700,    0, 250, 0,   100, 6},
+	{2600,    0, 250, 0,   100, 6},
+	{ 0xFFFF, 0,   0, 0,   0,   0},
+};
+
+struct dibx000_agc_config dib7090_agc_config[2] = {
+	{
+		.band_caps      = BAND_UHF,
+		/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=1, P_agc_inv_pwm1=0, P_agc_inv_pwm2=0,
+		* P_agc_inh_dc_rv_est=0, P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
+		.setup          = (0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8) | (3 << 5) | (0 << 4) | (5 << 1) | (0 << 0),
+
+		.inv_gain       = 687,
+		.time_stabiliz  = 10,
+
+		.alpha_level    = 0,
+		.thlock         = 118,
+
+		.wbd_inv        = 0,
+		.wbd_ref        = 1200,
+		.wbd_sel        = 3,
+		.wbd_alpha      = 5,
+
+		.agc1_max       = 65535,
+		.agc1_min       = 0,
+
+		.agc2_max       = 65535,
+		.agc2_min       = 0,
+
+		.agc1_pt1       = 0,
+		.agc1_pt2       = 32,
+		.agc1_pt3       = 114,
+		.agc1_slope1    = 143,
+		.agc1_slope2    = 144,
+		.agc2_pt1       = 114,
+		.agc2_pt2       = 227,
+		.agc2_slope1    = 116,
+		.agc2_slope2    = 117,
+
+		.alpha_mant     = 18,
+		.alpha_exp      = 0,
+		.beta_mant      = 20,
+		.beta_exp       = 59,
+
+		.perform_agc_softsplit = 0,
+	} , {
+		.band_caps      = BAND_FM | BAND_VHF | BAND_CBAND,
+		/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=1, P_agc_inv_pwm1=0, P_agc_inv_pwm2=0,
+		* P_agc_inh_dc_rv_est=0, P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
+		.setup          = (0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8) | (3 << 5) | (0 << 4) | (5 << 1) | (0 << 0),
+
+		.inv_gain       = 732,
+		.time_stabiliz  = 10,
+
+		.alpha_level    = 0,
+		.thlock         = 118,
+
+		.wbd_inv        = 0,
+		.wbd_ref        = 1200,
+		.wbd_sel        = 3,
+		.wbd_alpha      = 5,
+
+		.agc1_max       = 65535,
+		.agc1_min       = 0,
+
+		.agc2_max       = 65535,
+		.agc2_min       = 0,
+
+		.agc1_pt1       = 0,
+		.agc1_pt2       = 0,
+		.agc1_pt3       = 98,
+		.agc1_slope1    = 0,
+		.agc1_slope2    = 167,
+		.agc1_pt1       = 98,
+		.agc2_pt2       = 255,
+		.agc2_slope1    = 104,
+		.agc2_slope2    = 0,
+
+		.alpha_mant     = 18,
+		.alpha_exp      = 0,
+		.beta_mant      = 20,
+		.beta_exp       = 59,
+
+		.perform_agc_softsplit = 0,
+	}
+};
+
+static struct dibx000_bandwidth_config dib7090_clock_config_12_mhz = {
+	60000, 15000,
+	1, 5, 0, 0, 0,
+	0, 0, 1, 1, 2,
+	(3 << 14) | (1 << 12) | (524 << 0),
+	(0 << 25) | 0,
+	20452225,
+	15000000,
+};
+
+static struct dib7000p_config nim7090_dib7000p_config = {
+	.output_mpeg2_in_188_bytes  = 1,
+	.hostbus_diversity			= 1,
+	.tuner_is_baseband			= 1,
+	.update_lna					= NULL,
+
+	.agc_config_count			= 2,
+	.agc						= dib7090_agc_config,
+
+	.bw							= &dib7090_clock_config_12_mhz,
+
+	.gpio_dir					= DIB7000P_GPIO_DEFAULT_DIRECTIONS,
+	.gpio_val					= DIB7000P_GPIO_DEFAULT_VALUES,
+	.gpio_pwm_pos				= DIB7000P_GPIO_DEFAULT_PWM_POS,
+
+	.pwm_freq_div				= 0,
+
+	.agc_control				= dib7090_agc_restart,
+
+	.spur_protect				= 0,
+	.disable_sample_and_hold	= 0,
+	.enable_current_mirror		= 0,
+	.diversity_delay			= 0,
+
+	.output_mode				= OUTMODE_MPEG2_FIFO,
+	.enMpegOutput				= 1,
+};
+
+static struct dib7000p_config tfe7090pvr_dib7000p_config[2] = {
+	{
+		.output_mpeg2_in_188_bytes  = 1,
+		.hostbus_diversity			= 1,
+		.tuner_is_baseband			= 1,
+		.update_lna					= NULL,
+
+		.agc_config_count			= 2,
+		.agc						= dib7090_agc_config,
+
+		.bw							= &dib7090_clock_config_12_mhz,
+
+		.gpio_dir					= DIB7000P_GPIO_DEFAULT_DIRECTIONS,
+		.gpio_val					= DIB7000P_GPIO_DEFAULT_VALUES,
+		.gpio_pwm_pos				= DIB7000P_GPIO_DEFAULT_PWM_POS,
+
+		.pwm_freq_div				= 0,
+
+		.agc_control				= dib7090_agc_restart,
+
+		.spur_protect				= 0,
+		.disable_sample_and_hold	= 0,
+		.enable_current_mirror		= 0,
+		.diversity_delay			= 0,
+
+		.output_mode				= OUTMODE_MPEG2_PAR_GATED_CLK,
+		.default_i2c_addr			= 0x90,
+		.enMpegOutput				= 1,
+	}, {
+		.output_mpeg2_in_188_bytes  = 1,
+		.hostbus_diversity			= 1,
+		.tuner_is_baseband			= 1,
+		.update_lna					= NULL,
+
+		.agc_config_count			= 2,
+		.agc						= dib7090_agc_config,
+
+		.bw							= &dib7090_clock_config_12_mhz,
+
+		.gpio_dir					= DIB7000P_GPIO_DEFAULT_DIRECTIONS,
+		.gpio_val					= DIB7000P_GPIO_DEFAULT_VALUES,
+		.gpio_pwm_pos				= DIB7000P_GPIO_DEFAULT_PWM_POS,
+
+		.pwm_freq_div				= 0,
+
+		.agc_control				= dib7090_agc_restart,
+
+		.spur_protect				= 0,
+		.disable_sample_and_hold	= 0,
+		.enable_current_mirror		= 0,
+		.diversity_delay			= 0,
+
+		.output_mode				= OUTMODE_MPEG2_PAR_GATED_CLK,
+		.default_i2c_addr			= 0x92,
+		.enMpegOutput				= 0,
+	}
+};
+
+static const struct dib0090_config nim7090_dib0090_config = {
+	.io.clock_khz = 12000,
+	.io.pll_bypass = 0,
+	.io.pll_range = 0,
+	.io.pll_prediv = 3,
+	.io.pll_loopdiv = 6,
+	.io.adc_clock_ratio = 0,
+	.io.pll_int_loop_filt = 0,
+	.reset = dib7090_tuner_sleep,
+	.sleep = dib7090_tuner_sleep,
+
+	.freq_offset_khz_uhf = 0,
+	.freq_offset_khz_vhf = 0,
+
+	.get_adc_power = dib7090_get_adc_power,
+
+	.clkouttobamse = 1,
+	.analog_output = 0,
+
+	.wbd_vhf_offset = 0,
+	.wbd_cband_offset = 0,
+	.use_pwm_agc = 1,
+	.clkoutdrive = 0,
+
+	.fref_clock_ratio = 0,
+
+	.wbd = dib7090_wbd_table,
+
+	.ls_cfg_pad_drv = 0,
+	.data_tx_drv = 0,
+	.low_if = NULL,
+	.in_soc = 1,
+};
+
+static const struct dib0090_config tfe7090pvr_dib0090_config[2] = {
+	{
+		.io.clock_khz = 12000,
+		.io.pll_bypass = 0,
+		.io.pll_range = 0,
+		.io.pll_prediv = 3,
+		.io.pll_loopdiv = 6,
+		.io.adc_clock_ratio = 0,
+		.io.pll_int_loop_filt = 0,
+		.reset = dib7090_tuner_sleep,
+		.sleep = dib7090_tuner_sleep,
+
+		.freq_offset_khz_uhf = 50,
+		.freq_offset_khz_vhf = 70,
+
+		.get_adc_power = dib7090_get_adc_power,
+
+		.clkouttobamse = 1,
+		.analog_output = 0,
+
+		.wbd_vhf_offset = 0,
+		.wbd_cband_offset = 0,
+		.use_pwm_agc = 1,
+		.clkoutdrive = 0,
+
+		.fref_clock_ratio = 0,
+
+		.wbd = dib7090_wbd_table,
+
+		.ls_cfg_pad_drv = 0,
+		.data_tx_drv = 0,
+		.low_if = NULL,
+		.in_soc = 1,
+	}, {
+		.io.clock_khz = 12000,
+		.io.pll_bypass = 0,
+		.io.pll_range = 0,
+		.io.pll_prediv = 3,
+		.io.pll_loopdiv = 6,
+		.io.adc_clock_ratio = 0,
+		.io.pll_int_loop_filt = 0,
+		.reset = dib7090_tuner_sleep,
+		.sleep = dib7090_tuner_sleep,
+
+		.freq_offset_khz_uhf = -50,
+		.freq_offset_khz_vhf = -70,
+
+		.get_adc_power = dib7090_get_adc_power,
+
+		.clkouttobamse = 1,
+		.analog_output = 0,
+
+		.wbd_vhf_offset = 0,
+		.wbd_cband_offset = 0,
+		.use_pwm_agc = 1,
+		.clkoutdrive = 0,
+
+		.fref_clock_ratio = 0,
+
+		.wbd = dib7090_wbd_table,
+
+		.ls_cfg_pad_drv = 0,
+		.data_tx_drv = 0,
+		.low_if = NULL,
+		.in_soc = 1,
+	}
+};
+
+static int nim7090_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
+
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
+
+	if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap, 1, 0x10, &nim7090_dib7000p_config) != 0) {
+		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n", __func__);
+		return -ENODEV;
+	}
+	adap->fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap, 0x80, &nim7090_dib7000p_config);
 
 	return adap->fe == NULL ?  -ENODEV : 0;
 }
 
+static int nim7090_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *st = adap->priv;
+	struct i2c_adapter *tun_i2c = dib7090_get_i2c_tuner(adap->fe);
+
+	if (dvb_attach(dib0090_register, adap->fe, tun_i2c, &nim7090_dib0090_config) == NULL)
+		return -ENODEV;
+
+	dib7000p_set_gpio(adap->fe, 8, 0, 1);
+
+	st->set_param_save = adap->fe->ops.tuner_ops.set_params;
+	adap->fe->ops.tuner_ops.set_params = dib7090_agc_startup;
+	return 0;
+}
+
+static int tfe7090pvr_frontend0_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_state *st = adap->dev->priv;
+
+	/* The TFE7090 requires the dib0700 to not be in master mode */
+	st->disable_streaming_master_mode = 1;
+
+	dib0700_set_gpio(adap->dev, GPIO6, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO9, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO4, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO7, GPIO_OUT, 1);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 0);
+
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO10, GPIO_OUT, 1);
+	msleep(20);
+	dib0700_set_gpio(adap->dev, GPIO0, GPIO_OUT, 1);
+
+	/* initialize IC 0 */
+	if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap, 1, 0x20, &tfe7090pvr_dib7000p_config[0]) != 0) {
+		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n", __func__);
+		return -ENODEV;
+	}
+
+	dib0700_set_i2c_speed(adap->dev, 340);
+	adap->fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap, 0x90, &tfe7090pvr_dib7000p_config[0]);
+
+	dib7090_slave_reset(adap->fe);
+
+	if (adap->fe == NULL)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int tfe7090pvr_frontend1_attach(struct dvb_usb_adapter *adap)
+{
+	struct i2c_adapter *i2c;
+
+	if (adap->dev->adapter[0].fe == NULL) {
+		err("the master dib7090 has to be initialized first");
+		return -ENODEV; /* the master device has not been initialized */
+	}
+
+	i2c = dib7000p_get_i2c_master(adap->dev->adapter[0].fe, DIBX000_I2C_INTERFACE_GPIO_6_7, 1);
+	if (dib7000p_i2c_enumeration(i2c, 1, 0x10, &tfe7090pvr_dib7000p_config[1]) != 0) {
+		err("%s: dib7000p_i2c_enumeration failed.  Cannot continue\n", __func__);
+		return -ENODEV;
+	}
+
+	adap->fe = dvb_attach(dib7000p_attach, i2c, 0x92, &tfe7090pvr_dib7000p_config[1]);
+	dib0700_set_i2c_speed(adap->dev, 200);
+
+	return adap->fe == NULL ? -ENODEV : 0;
+}
+
+static int tfe7090pvr_tuner0_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *st = adap->priv;
+	struct i2c_adapter *tun_i2c = dib7090_get_i2c_tuner(adap->fe);
+
+	if (dvb_attach(dib0090_register, adap->fe, tun_i2c, &tfe7090pvr_dib0090_config[0]) == NULL)
+		return -ENODEV;
+
+	dib7000p_set_gpio(adap->fe, 8, 0, 1);
+
+	st->set_param_save = adap->fe->ops.tuner_ops.set_params;
+	adap->fe->ops.tuner_ops.set_params = dib7090_agc_startup;
+	return 0;
+}
+
+static int tfe7090pvr_tuner1_attach(struct dvb_usb_adapter *adap)
+{
+	struct dib0700_adapter_state *st = adap->priv;
+	struct i2c_adapter *tun_i2c = dib7090_get_i2c_tuner(adap->fe);
+
+	if (dvb_attach(dib0090_register, adap->fe, tun_i2c, &tfe7090pvr_dib0090_config[1]) == NULL)
+		return -ENODEV;
+
+	dib7000p_set_gpio(adap->fe, 8, 0, 1);
+
+	st->set_param_save = adap->fe->ops.tuner_ops.set_params;
+	adap->fe->ops.tuner_ops.set_params = dib7090_agc_startup;
+	return 0;
+}
+
 /* STK7070PD */
 static struct dib7000p_config stk7070pd_dib7000p_config[2] = {
 	{
@@ -1839,6 +2796,12 @@ struct usb_device_id dib0700_usb_id_table[] = {
 	{ USB_DEVICE(USB_VID_PINNACLE,	USB_PID_PINNACLE_PCTV282E) },
 	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_STK8096GP) },
 	{ USB_DEVICE(USB_VID_ELGATO,    USB_PID_ELGATO_EYETV_DIVERSITY) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_NIM9090M) },
+/* 70 */{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_NIM8096MD) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_NIM9090MD) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_NIM7090) },
+	{ USB_DEVICE(USB_VID_DIBCOM,    USB_PID_DIBCOM_TFE7090PVR) },
+	{ USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_AIRSTAR_TELESTICK_2) },
 	{ 0 }		/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, dib0700_usb_id_table);
@@ -1875,8 +2838,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 			{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
-				.pid_filter       = stk70x0p_pid_filter,
-				.pid_filter_ctrl  = stk70x0p_pid_filter_ctrl,
+				.pid_filter       = stk7700p_pid_filter,
+				.pid_filter_ctrl  = stk7700p_pid_filter_ctrl,
 				.frontend_attach  = stk7700p_frontend_attach,
 				.tuner_attach     = stk7700p_tuner_attach,
 
@@ -2448,7 +3411,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 			},
 		},
 
-		.num_device_descs = 2,
+		.num_device_descs = 3,
 		.devices = {
 			{   "DiBcom STK7770P reference design",
 				{ &dib0700_usb_id_table[59], NULL },
@@ -2460,6 +3423,10 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 					&dib0700_usb_id_table[60], NULL},
 				{ NULL },
 			},
+			{   "TechniSat AirStar TeleStick 2",
+				{ &dib0700_usb_id_table[74], NULL },
+				{ NULL },
+			},
 		},
 
 		.rc.core = {
@@ -2602,6 +3569,205 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 					    RC_TYPE_NEC,
 			.change_protocol  = dib0700_change_protocol,
 		},
+	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
+					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+				.pid_filter_count = 32,
+				.pid_filter = dib90x0_pid_filter,
+				.pid_filter_ctrl = dib90x0_pid_filter_ctrl,
+				.frontend_attach  = stk9090m_frontend_attach,
+				.tuner_attach     = dib9090_tuner_attach,
+
+				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
+
+				.size_of_priv =
+					sizeof(struct dib0700_adapter_state),
+			},
+		},
+
+		.num_device_descs = 1,
+		.devices = {
+			{   "DiBcom STK9090M reference design",
+				{ &dib0700_usb_id_table[69], NULL },
+				{ NULL },
+			},
+		},
+
+		.rc.core = {
+			.rc_interval      = DEFAULT_RC_INTERVAL,
+			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
+			.module_name	  = "dib0700",
+			.rc_query         = dib0700_rc_query_old_firmware,
+			.allowed_protos   = RC_TYPE_RC5 |
+					    RC_TYPE_RC6 |
+					    RC_TYPE_NEC,
+			.change_protocol  = dib0700_change_protocol,
+		},
+	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
+					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+				.pid_filter_count = 32,
+				.pid_filter = stk80xx_pid_filter,
+				.pid_filter_ctrl = stk80xx_pid_filter_ctrl,
+				.frontend_attach  = nim8096md_frontend_attach,
+				.tuner_attach     = nim8096md_tuner_attach,
+
+				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
+
+				.size_of_priv =
+					sizeof(struct dib0700_adapter_state),
+			},
+		},
+
+		.num_device_descs = 1,
+		.devices = {
+			{   "DiBcom NIM8096MD reference design",
+				{ &dib0700_usb_id_table[70], NULL },
+				{ NULL },
+			},
+		},
+
+		.rc.core = {
+			.rc_interval      = DEFAULT_RC_INTERVAL,
+			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
+			.module_name	  = "dib0700",
+			.rc_query         = dib0700_rc_query_old_firmware,
+			.allowed_protos   = RC_TYPE_RC5 |
+					    RC_TYPE_RC6 |
+					    RC_TYPE_NEC,
+			.change_protocol  = dib0700_change_protocol,
+		},
+	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
+					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+				.pid_filter_count = 32,
+				.pid_filter = dib90x0_pid_filter,
+				.pid_filter_ctrl = dib90x0_pid_filter_ctrl,
+				.frontend_attach  = nim9090md_frontend_attach,
+				.tuner_attach     = nim9090md_tuner_attach,
+
+				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
+
+				.size_of_priv =
+					sizeof(struct dib0700_adapter_state),
+			},
+		},
+
+		.num_device_descs = 1,
+		.devices = {
+			{   "DiBcom NIM9090MD reference design",
+				{ &dib0700_usb_id_table[71], NULL },
+				{ NULL },
+			},
+		},
+
+		.rc.core = {
+			.rc_interval      = DEFAULT_RC_INTERVAL,
+			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
+			.module_name	  = "dib0700",
+			.rc_query         = dib0700_rc_query_old_firmware,
+			.allowed_protos   = RC_TYPE_RC5 |
+					    RC_TYPE_RC6 |
+					    RC_TYPE_NEC,
+			.change_protocol  = dib0700_change_protocol,
+		},
+	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
+		.num_adapters = 1,
+		.adapter = {
+			{
+				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
+					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+				.pid_filter_count = 32,
+				.pid_filter = stk70x0p_pid_filter,
+				.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
+				.frontend_attach  = nim7090_frontend_attach,
+				.tuner_attach     = nim7090_tuner_attach,
+
+				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
+
+				.size_of_priv =
+					sizeof(struct dib0700_adapter_state),
+			},
+		},
+
+		.num_device_descs = 1,
+		.devices = {
+			{   "DiBcom NIM7090 reference design",
+				{ &dib0700_usb_id_table[72], NULL },
+				{ NULL },
+			},
+		},
+
+		.rc.core = {
+			.rc_interval      = DEFAULT_RC_INTERVAL,
+			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
+			.module_name	  = "dib0700",
+			.rc_query         = dib0700_rc_query_old_firmware,
+			.allowed_protos   = RC_TYPE_RC5 |
+					    RC_TYPE_RC6 |
+					    RC_TYPE_NEC,
+			.change_protocol  = dib0700_change_protocol,
+		},
+	}, { DIB0700_DEFAULT_DEVICE_PROPERTIES,
+		.num_adapters = 2,
+		.adapter = {
+			{
+				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
+					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+				.pid_filter_count = 32,
+				.pid_filter = stk70x0p_pid_filter,
+				.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
+				.frontend_attach  = tfe7090pvr_frontend0_attach,
+				.tuner_attach     = tfe7090pvr_tuner0_attach,
+
+				DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
+
+				.size_of_priv =
+					sizeof(struct dib0700_adapter_state),
+			},
+			{
+				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
+					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
+				.pid_filter_count = 32,
+				.pid_filter = stk70x0p_pid_filter,
+				.pid_filter_ctrl = stk70x0p_pid_filter_ctrl,
+				.frontend_attach  = tfe7090pvr_frontend1_attach,
+				.tuner_attach     = tfe7090pvr_tuner1_attach,
+
+				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
+
+				.size_of_priv =
+					sizeof(struct dib0700_adapter_state),
+			},
+		},
+
+		.num_device_descs = 1,
+		.devices = {
+			{   "DiBcom TFE7090PVR reference design",
+				{ &dib0700_usb_id_table[73], NULL },
+				{ NULL },
+			},
+		},
+
+		.rc.core = {
+			.rc_interval      = DEFAULT_RC_INTERVAL,
+			.rc_codes         = RC_MAP_DIB0700_RC5_TABLE,
+			.module_name	  = "dib0700",
+			.rc_query         = dib0700_rc_query_old_firmware,
+			.allowed_protos   = RC_TYPE_RC5 |
+					    RC_TYPE_RC6 |
+					    RC_TYPE_NEC,
+			.change_protocol  = dib0700_change_protocol,
+		},
 	},
 };
 
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
index 1a6310b61923..3a8b7446b7b0 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
@@ -106,8 +106,13 @@
 #define USB_PID_DIBCOM_STK807XP				0x1f90
 #define USB_PID_DIBCOM_STK807XPVR			0x1f98
 #define USB_PID_DIBCOM_STK8096GP                        0x1fa0
+#define USB_PID_DIBCOM_NIM8096MD                        0x1fa8
 #define USB_PID_DIBCOM_ANCHOR_2135_COLD			0x2131
 #define USB_PID_DIBCOM_STK7770P				0x1e80
+#define USB_PID_DIBCOM_NIM7090				0x1bb2
+#define USB_PID_DIBCOM_TFE7090PVR			0x1bb4
+#define USB_PID_DIBCOM_NIM9090M				0x2383
+#define USB_PID_DIBCOM_NIM9090MD			0x2384
 #define USB_PID_DPOSH_M9206_COLD			0x9206
 #define USB_PID_DPOSH_M9206_WARM			0xa090
 #define USB_PID_E3C_EC168				0x1689
@@ -312,4 +317,6 @@
 #define USB_PID_TERRATEC_DVBS2CI_V2			0x10ac
 #define USB_PID_TECHNISAT_USB2_HDCI_V1			0x0001
 #define USB_PID_TECHNISAT_USB2_HDCI_V2			0x0002
+#define USB_PID_TECHNISAT_AIRSTAR_TELESTICK_2		0x0004
+#define USB_PID_TECHNISAT_USB2_DVB_S2			0x0500
 #endif
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-remote.c b/drivers/media/dvb/dvb-usb/dvb-usb-remote.c
index 23005b3cf30b..41bacff24960 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-remote.c
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-remote.c
@@ -8,60 +8,71 @@
 #include "dvb-usb-common.h"
 #include <linux/usb/input.h>
 
+static unsigned int
+legacy_dvb_usb_get_keymap_index(const struct input_keymap_entry *ke,
+				struct rc_map_table *keymap,
+				unsigned int keymap_size)
+{
+	unsigned int index;
+	unsigned int scancode;
+
+	if (ke->flags & INPUT_KEYMAP_BY_INDEX) {
+		index = ke->index;
+	} else {
+		if (input_scancode_to_scalar(ke, &scancode))
+			return keymap_size;
+
+		/* See if we can match the raw key code. */
+		for (index = 0; index < keymap_size; index++)
+			if (keymap[index].scancode == scancode)
+				break;
+
+		/* See if there is an unused hole in the map */
+		if (index >= keymap_size) {
+			for (index = 0; index < keymap_size; index++) {
+				if (keymap[index].keycode == KEY_RESERVED ||
+				    keymap[index].keycode == KEY_UNKNOWN) {
+					break;
+				}
+			}
+		}
+	}
+
+	return index;
+}
+
 static int legacy_dvb_usb_getkeycode(struct input_dev *dev,
-				unsigned int scancode, unsigned int *keycode)
+				     struct input_keymap_entry *ke)
 {
 	struct dvb_usb_device *d = input_get_drvdata(dev);
-
 	struct rc_map_table *keymap = d->props.rc.legacy.rc_map_table;
-	int i;
+	unsigned int keymap_size = d->props.rc.legacy.rc_map_size;
+	unsigned int index;
 
-	/* See if we can match the raw key code. */
-	for (i = 0; i < d->props.rc.legacy.rc_map_size; i++)
-		if (keymap[i].scancode == scancode) {
-			*keycode = keymap[i].keycode;
-			return 0;
-		}
+	index = legacy_dvb_usb_get_keymap_index(ke, keymap, keymap_size);
+	if (index >= keymap_size)
+		return -EINVAL;
 
-	/*
-	 * If is there extra space, returns KEY_RESERVED,
-	 * otherwise, input core won't let legacy_dvb_usb_setkeycode
-	 * to work
-	 */
-	for (i = 0; i < d->props.rc.legacy.rc_map_size; i++)
-		if (keymap[i].keycode == KEY_RESERVED ||
-		    keymap[i].keycode == KEY_UNKNOWN) {
-			*keycode = KEY_RESERVED;
-			return 0;
-		}
+	ke->keycode = keymap[index].keycode;
+	if (ke->keycode == KEY_UNKNOWN)
+		ke->keycode = KEY_RESERVED;
+	ke->len = sizeof(keymap[index].scancode);
+	memcpy(&ke->scancode, &keymap[index].scancode, ke->len);
+	ke->index = index;
 
-	return -EINVAL;
+	return 0;
 }
 
 static int legacy_dvb_usb_setkeycode(struct input_dev *dev,
-				unsigned int scancode, unsigned int keycode)
+				     const struct input_keymap_entry *ke,
+				     unsigned int *old_keycode)
 {
 	struct dvb_usb_device *d = input_get_drvdata(dev);
-
 	struct rc_map_table *keymap = d->props.rc.legacy.rc_map_table;
-	int i;
-
-	/* Search if it is replacing an existing keycode */
-	for (i = 0; i < d->props.rc.legacy.rc_map_size; i++)
-		if (keymap[i].scancode == scancode) {
-			keymap[i].keycode = keycode;
-			return 0;
-		}
-
-	/* Search if is there a clean entry. If so, use it */
-	for (i = 0; i < d->props.rc.legacy.rc_map_size; i++)
-		if (keymap[i].keycode == KEY_RESERVED ||
-		    keymap[i].keycode == KEY_UNKNOWN) {
-			keymap[i].scancode = scancode;
-			keymap[i].keycode = keycode;
-			return 0;
-		}
+	unsigned int keymap_size = d->props.rc.legacy.rc_map_size;
+	unsigned int index;
 
+	index = legacy_dvb_usb_get_keymap_index(ke, keymap, keymap_size);
 	/*
 	 * FIXME: Currently, it is not possible to increase the size of
 	 * scancode table. For it to happen, one possibility
@@ -69,8 +80,24 @@ static int legacy_dvb_usb_setkeycode(struct input_dev *dev,
 	 * copying data, appending the new key on it, and freeing
 	 * the old one - or maybe just allocating some spare space
 	 */
+	if (index >= keymap_size)
+		return -EINVAL;
+
+	*old_keycode = keymap[index].keycode;
+	keymap->keycode = ke->keycode;
+	__set_bit(ke->keycode, dev->keybit);
+
+	if (*old_keycode != KEY_RESERVED) {
+		__clear_bit(*old_keycode, dev->keybit);
+		for (index = 0; index < keymap_size; index++) {
+			if (keymap[index].keycode == *old_keycode) {
+				__set_bit(*old_keycode, dev->keybit);
+				break;
+			}
+		}
+	}
 
-	return -EINVAL;
+	return 0;
 }
 
 /* Remote-control poll function - called every dib->rc_query_interval ms to see
@@ -246,7 +273,7 @@ static int rc_core_dvb_usb_remote_init(struct dvb_usb_device *d)
 	dev->map_name = d->props.rc.core.rc_codes;
 	dev->change_protocol = d->props.rc.core.change_protocol;
 	dev->allowed_protos = d->props.rc.core.allowed_protos;
-	dev->driver_type = RC_DRIVER_SCANCODE;
+	dev->driver_type = d->props.rc.core.driver_type;
 	usb_to_input_id(d->udev, &dev->input_id);
 	dev->input_name = "IR-receiver inside an USB DVB receiver";
 	dev->input_phys = d->rc_phys;
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb.h b/drivers/media/dvb/dvb-usb/dvb-usb.h
index 65fa9268e7f7..76a80968482a 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb.h
@@ -181,6 +181,7 @@ struct dvb_rc_legacy {
  * @rc_codes: name of rc codes table
  * @protocol: type of protocol(s) currently used by the driver
  * @allowed_protos: protocol(s) supported by the driver
+ * @driver_type: Used to point if a device supports raw mode
  * @change_protocol: callback to change protocol
  * @rc_query: called to query an event event.
  * @rc_interval: time in ms between two queries.
@@ -190,6 +191,7 @@ struct dvb_rc {
 	char *rc_codes;
 	u64 protocol;
 	u64 allowed_protos;
+	enum rc_driver_type driver_type;
 	int (*change_protocol)(struct rc_dev *dev, u64 rc_type);
 	char *module_name;
 	int (*rc_query) (struct dvb_usb_device *d);
diff --git a/drivers/media/dvb/dvb-usb/dw2102.c b/drivers/media/dvb/dvb-usb/dw2102.c
index 2c307ba0d28b..98cf30270f7d 100644
--- a/drivers/media/dvb/dvb-usb/dw2102.c
+++ b/drivers/media/dvb/dvb-usb/dw2102.c
@@ -1,15 +1,16 @@
 /* DVB USB framework compliant Linux driver for the
-*	DVBWorld DVB-S 2101, 2102, DVB-S2 2104, DVB-C 3101,
-*	TeVii S600, S630, S650,
-*	Prof 1100, 7500 Cards
-* Copyright (C) 2008,2009 Igor M. Liplianin (liplianin@me.by)
-*
-*	This program is free software; you can redistribute it and/or modify it
-*	under the terms of the GNU General Public License as published by the
-*	Free Software Foundation, version 2.
-*
-* see Documentation/dvb/README.dvb-usb for more information
-*/
+ *	DVBWorld DVB-S 2101, 2102, DVB-S2 2104, DVB-C 3101,
+ *	TeVii S600, S630, S650, S660, S480,
+ *	Prof 1100, 7500,
+ *	Geniatech SU3000 Cards
+ * Copyright (C) 2008-2011 Igor M. Liplianin (liplianin@me.by)
+ *
+ *	This program is free software; you can redistribute it and/or modify it
+ *	under the terms of the GNU General Public License as published by the
+ *	Free Software Foundation, version 2.
+ *
+ * see Documentation/dvb/README.dvb-usb for more information
+ */
 #include "dw2102.h"
 #include "si21xx.h"
 #include "stv0299.h"
@@ -55,6 +56,14 @@
 #define USB_PID_TEVII_S660 0xd660
 #endif
 
+#ifndef USB_PID_TEVII_S480_1
+#define USB_PID_TEVII_S480_1 0xd481
+#endif
+
+#ifndef USB_PID_TEVII_S480_2
+#define USB_PID_TEVII_S480_2 0xd482
+#endif
+
 #ifndef USB_PID_PROF_1100
 #define USB_PID_PROF_1100 0xb012
 #endif
@@ -67,7 +76,9 @@
 #define REG_21_SYMBOLRATE_BYTE2 0x21
 /* on my own*/
 #define DW2102_VOLTAGE_CTRL (0x1800)
+#define SU3000_STREAM_CTRL (0x1900)
 #define DW2102_RC_QUERY (0x1a00)
+#define DW2102_LED_CTRL (0x1b00)
 
 #define	err_str "did not find the firmware file. (%s) " \
 		"Please see linux/Documentation/dvb/ for more details " \
@@ -78,6 +89,14 @@ struct rc_map_dvb_usb_table_table {
 	int rc_keys_size;
 };
 
+struct su3000_state {
+	u8 initialized;
+};
+
+struct s6x0_state {
+	int (*old_set_voltage)(struct dvb_frontend *f, fe_sec_voltage_t v);
+};
+
 /* debug */
 static int dvb_usb_dw2102_debug;
 module_param_named(debug, dvb_usb_dw2102_debug, int, 0644);
@@ -87,7 +106,8 @@ MODULE_PARM_DESC(debug, "set debugging level (1=info 2=xfer 4=rc(or-able))."
 /* keymaps */
 static int ir_keymap;
 module_param_named(keymap, ir_keymap, int, 0644);
-MODULE_PARM_DESC(keymap, "set keymap 0=default 1=dvbworld 2=tevii 3=tbs  ...");
+MODULE_PARM_DESC(keymap, "set keymap 0=default 1=dvbworld 2=tevii 3=tbs  ..."
+			" 256=none");
 
 /* demod probe */
 static int demod_probe = 1;
@@ -136,8 +156,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		/* read stv0299 register */
 		value = msg[0].buf[0];/* register */
 		for (i = 0; i < msg[1].len; i++) {
-			value = value + i;
-			ret = dw210x_op_rw(d->udev, 0xb5, value, 0,
+			ret = dw210x_op_rw(d->udev, 0xb5, value + i, 0,
 					buf6, 2, DW210X_READ_MSG);
 			msg[1].buf[i] = buf6[0];
 		}
@@ -483,10 +502,10 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 	for (j = 0; j < num; j++) {
 		switch (msg[j].addr) {
 		case (DW2102_RC_QUERY): {
-			u8 ibuf[4];
+			u8 ibuf[5];
 			ret  = dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					ibuf, 4, DW210X_READ_MSG);
-			memcpy(msg[j].buf, ibuf + 1, 2);
+					ibuf, 5, DW210X_READ_MSG);
+			memcpy(msg[j].buf, ibuf + 3, 2);
 			break;
 		}
 		case (DW2102_VOLTAGE_CTRL): {
@@ -502,6 +521,15 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 					obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
+		case (DW2102_LED_CTRL): {
+			u8 obuf[2];
+
+			obuf[0] = 5;
+			obuf[1] = msg[j].buf[0];
+			ret = dw210x_op_rw(d->udev, 0x8a, 0, 0,
+					obuf, 2, DW210X_WRITE_MSG);
+			break;
+		}
 		/*case 0x55: cx24116
 		case 0x6a: stv0903
 		case 0x68: ds3000, stv0903
@@ -535,14 +563,15 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 					i += 16;
 					len -= 16;
 				} while (len > 0);
-			} else if ((udev->descriptor.idProduct == 0x7500)
-					&& (j < (num - 1))) {
+			} else if (j < (num - 1)) {
 				/* write register addr before read */
 				u8 obuf[msg[j].len + 2];
 				obuf[0] = msg[j + 1].len;
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev, 0x92, 0, 0,
+				ret = dw210x_op_rw(d->udev,
+						udev->descriptor.idProduct ==
+						0x7500 ? 0x92 : 0x90, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
 				break;
@@ -552,8 +581,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[0] = msg[j].len + 1;
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
-				ret = dw210x_op_rw(d->udev,
-						(num > 1 ? 0x90 : 0x80), 0, 0,
+				ret = dw210x_op_rw(d->udev, 0x80, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
 				break;
@@ -561,14 +589,76 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			break;
 		}
 		}
-
-		msleep(3);
 	}
 
 	mutex_unlock(&d->i2c_mutex);
 	return num;
 }
 
+static int su3000_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
+								int num)
+{
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	u8 obuf[0x40], ibuf[0x40];
+
+	if (!d)
+		return -ENODEV;
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	switch (num) {
+	case 1:
+		switch (msg[0].addr) {
+		case SU3000_STREAM_CTRL:
+			obuf[0] = msg[0].buf[0] + 0x36;
+			obuf[1] = 3;
+			obuf[2] = 0;
+			if (dvb_usb_generic_rw(d, obuf, 3, ibuf, 0, 0) < 0)
+				err("i2c transfer failed.");
+			break;
+		case DW2102_RC_QUERY:
+			obuf[0] = 0x10;
+			if (dvb_usb_generic_rw(d, obuf, 1, ibuf, 2, 0) < 0)
+				err("i2c transfer failed.");
+			msg[0].buf[1] = ibuf[0];
+			msg[0].buf[0] = ibuf[1];
+			break;
+		default:
+			/* always i2c write*/
+			obuf[0] = 0x08;
+			obuf[1] = msg[0].addr;
+			obuf[2] = msg[0].len;
+
+			memcpy(&obuf[3], msg[0].buf, msg[0].len);
+
+			if (dvb_usb_generic_rw(d, obuf, msg[0].len + 3,
+						ibuf, 1, 0) < 0)
+				err("i2c transfer failed.");
+
+		}
+		break;
+	case 2:
+		/* always i2c read */
+		obuf[0] = 0x09;
+		obuf[1] = msg[0].len;
+		obuf[2] = msg[1].len;
+		obuf[3] = msg[0].addr;
+		memcpy(&obuf[4], msg[0].buf, msg[0].len);
+
+		if (dvb_usb_generic_rw(d, obuf, msg[0].len + 4,
+					ibuf, msg[1].len + 1, 0) < 0)
+			err("i2c transfer failed.");
+
+		memcpy(msg[1].buf, &ibuf[1], msg[1].len);
+		break;
+	default:
+		warn("more than 2 i2c messages at a time is not handled yet.");
+		break;
+	}
+	mutex_unlock(&d->i2c_mutex);
+	return num;
+}
+
 static u32 dw210x_i2c_func(struct i2c_adapter *adapter)
 {
 	return I2C_FUNC_I2C;
@@ -604,6 +694,11 @@ static struct i2c_algorithm s6x0_i2c_algo = {
 	.functionality = dw210x_i2c_func,
 };
 
+static struct i2c_algorithm su3000_i2c_algo = {
+	.master_xfer = su3000_i2c_transfer,
+	.functionality = dw210x_i2c_func,
+};
+
 static int dw210x_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
 {
 	int i;
@@ -668,6 +763,82 @@ static int s6x0_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
 	return 0;
 };
 
+static int su3000_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
+{
+	static u8 command_start[] = {0x00};
+	static u8 command_stop[] = {0x01};
+	struct i2c_msg msg = {
+		.addr = SU3000_STREAM_CTRL,
+		.flags = 0,
+		.buf = onoff ? command_start : command_stop,
+		.len = 1
+	};
+
+	i2c_transfer(&adap->dev->i2c_adap, &msg, 1);
+
+	return 0;
+}
+
+static int su3000_power_ctrl(struct dvb_usb_device *d, int i)
+{
+	struct su3000_state *state = (struct su3000_state *)d->priv;
+	u8 obuf[] = {0xde, 0};
+
+	info("%s: %d, initialized %d\n", __func__, i, state->initialized);
+
+	if (i && !state->initialized) {
+		state->initialized = 1;
+		/* reset board */
+		dvb_usb_generic_rw(d, obuf, 2, NULL, 0, 0);
+	}
+
+	return 0;
+}
+
+static int su3000_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
+{
+	int i;
+	u8 obuf[] = { 0x1f, 0xf0 };
+	u8 ibuf[] = { 0 };
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x51,
+			.flags = 0,
+			.buf = obuf,
+			.len = 2,
+		}, {
+			.addr = 0x51,
+			.flags = I2C_M_RD,
+			.buf = ibuf,
+			.len = 1,
+
+		}
+	};
+
+	for (i = 0; i < 6; i++) {
+		obuf[1] = 0xf0 + i;
+		if (i2c_transfer(&d->i2c_adap, msg, 2) != 2)
+			break;
+		else
+			mac[i] = ibuf[0];
+
+		debug_dump(mac, 6, printk);
+	}
+
+	return 0;
+}
+
+static int su3000_identify_state(struct usb_device *udev,
+				 struct dvb_usb_device_properties *props,
+				 struct dvb_usb_device_description **desc,
+				 int *cold)
+{
+	info("%s\n", __func__);
+
+	*cold = 0;
+	return 0;
+}
+
 static int dw210x_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 {
 	static u8 command_13v[] = {0x00, 0x01};
@@ -692,6 +863,37 @@ static int dw210x_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 	return 0;
 }
 
+static int s660_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+{
+	struct dvb_usb_adapter *d =
+		(struct dvb_usb_adapter *)(fe->dvb->priv);
+	struct s6x0_state *st = (struct s6x0_state *)d->dev->priv;
+
+	dw210x_set_voltage(fe, voltage);
+	if (st->old_set_voltage)
+		st->old_set_voltage(fe, voltage);
+
+	return 0;
+}
+
+static void dw210x_led_ctrl(struct dvb_frontend *fe, int offon)
+{
+	static u8 led_off[] = { 0 };
+	static u8 led_on[] = { 1 };
+	struct i2c_msg msg = {
+		.addr = DW2102_LED_CTRL,
+		.flags = 0,
+		.buf = led_off,
+		.len = 1
+	};
+	struct dvb_usb_adapter *udev_adap =
+		(struct dvb_usb_adapter *)(fe->dvb->priv);
+
+	if (offon)
+		msg.buf = led_on;
+	i2c_transfer(&udev_adap->dev->i2c_adap, &msg, 1);
+}
+
 static struct stv0299_config sharp_z0194a_config = {
 	.demod_address = 0x68,
 	.inittab = sharp_z0194a_inittab,
@@ -771,6 +973,12 @@ static struct stv0900_config prof_7500_stv0900_config = {
 	.tun1_adc = 0,/* 2 Vpp */
 	.path1_mode = 3,
 	.tun1_type = 3,
+	.set_lock_led = dw210x_led_ctrl,
+};
+
+static struct ds3000_config su3000_ds3000_config = {
+	.demod_address = 0x68,
+	.ci_mode = 1,
 };
 
 static int dw2104_frontend_attach(struct dvb_usb_adapter *d)
@@ -885,7 +1093,7 @@ static int dw3101_frontend_attach(struct dvb_usb_adapter *d)
 	return -EIO;
 }
 
-static int s6x0_frontend_attach(struct dvb_usb_adapter *d)
+static int zl100313_frontend_attach(struct dvb_usb_adapter *d)
 {
 	d->fe = dvb_attach(mt312_attach, &zl313_config,
 			&d->dev->i2c_adap);
@@ -898,41 +1106,108 @@ static int s6x0_frontend_attach(struct dvb_usb_adapter *d)
 		}
 	}
 
+	return -EIO;
+}
+
+static int stv0288_frontend_attach(struct dvb_usb_adapter *d)
+{
+	u8 obuf[] = {7, 1};
+
 	d->fe = dvb_attach(stv0288_attach, &earda_config,
 			&d->dev->i2c_adap);
-	if (d->fe != NULL) {
-		if (dvb_attach(stb6000_attach, d->fe, 0x61,
-				&d->dev->i2c_adap)) {
-			d->fe->ops.set_voltage = dw210x_set_voltage;
-			info("Attached stv0288+stb6000!\n");
-			return 0;
-		}
-	}
+
+	if (d->fe == NULL)
+		return -EIO;
+
+	if (NULL == dvb_attach(stb6000_attach, d->fe, 0x61, &d->dev->i2c_adap))
+		return -EIO;
+
+	d->fe->ops.set_voltage = dw210x_set_voltage;
+
+	dw210x_op_rw(d->dev->udev, 0x8a, 0, 0, obuf, 2, DW210X_WRITE_MSG);
+
+	info("Attached stv0288+stb6000!\n");
+
+	return 0;
+
+}
+
+static int ds3000_frontend_attach(struct dvb_usb_adapter *d)
+{
+	struct s6x0_state *st = (struct s6x0_state *)d->dev->priv;
+	u8 obuf[] = {7, 1};
 
 	d->fe = dvb_attach(ds3000_attach, &dw2104_ds3000_config,
 			&d->dev->i2c_adap);
-	if (d->fe != NULL) {
-		d->fe->ops.set_voltage = dw210x_set_voltage;
-		info("Attached ds3000+ds2020!\n");
-		return 0;
-	}
 
-	return -EIO;
+	if (d->fe == NULL)
+		return -EIO;
+
+	st->old_set_voltage = d->fe->ops.set_voltage;
+	d->fe->ops.set_voltage = s660_set_voltage;
+
+	dw210x_op_rw(d->dev->udev, 0x8a, 0, 0, obuf, 2, DW210X_WRITE_MSG);
+
+	info("Attached ds3000+ds2020!\n");
+
+	return 0;
 }
 
 static int prof_7500_frontend_attach(struct dvb_usb_adapter *d)
 {
+	u8 obuf[] = {7, 1};
+
 	d->fe = dvb_attach(stv0900_attach, &prof_7500_stv0900_config,
 					&d->dev->i2c_adap, 0);
 	if (d->fe == NULL)
 		return -EIO;
+
 	d->fe->ops.set_voltage = dw210x_set_voltage;
 
+	dw210x_op_rw(d->dev->udev, 0x8a, 0, 0, obuf, 2, DW210X_WRITE_MSG);
+
 	info("Attached STV0900+STB6100A!\n");
 
 	return 0;
 }
 
+static int su3000_frontend_attach(struct dvb_usb_adapter *d)
+{
+	u8 obuf[3] = { 0xe, 0x80, 0 };
+	u8 ibuf[] = { 0 };
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 3, ibuf, 1, 0) < 0)
+		err("command 0x0e transfer failed.");
+
+	obuf[0] = 0xe;
+	obuf[1] = 0x83;
+	obuf[2] = 0;
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 3, ibuf, 1, 0) < 0)
+		err("command 0x0e transfer failed.");
+
+	obuf[0] = 0xe;
+	obuf[1] = 0x83;
+	obuf[2] = 1;
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 3, ibuf, 1, 0) < 0)
+		err("command 0x0e transfer failed.");
+
+	obuf[0] = 0x51;
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 1, ibuf, 1, 0) < 0)
+		err("command 0x51 transfer failed.");
+
+	d->fe = dvb_attach(ds3000_attach, &su3000_ds3000_config,
+					&d->dev->i2c_adap);
+	if (d->fe == NULL)
+		return -EIO;
+
+	info("Attached DS3000!\n");
+
+	return 0;
+}
+
 static int dw2102_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	dvb_attach(dvb_pll_attach, adap->fe, 0x60,
@@ -1067,10 +1342,49 @@ static struct rc_map_table rc_map_tbs_table[] = {
 	{ 0xf89b, KEY_MODE }
 };
 
+static struct rc_map_table rc_map_su3000_table[] = {
+	{ 0x25, KEY_POWER },	/* right-bottom Red */
+	{ 0x0a, KEY_MUTE },	/* -/-- */
+	{ 0x01, KEY_1 },
+	{ 0x02, KEY_2 },
+	{ 0x03, KEY_3 },
+	{ 0x04, KEY_4 },
+	{ 0x05, KEY_5 },
+	{ 0x06, KEY_6 },
+	{ 0x07, KEY_7 },
+	{ 0x08, KEY_8 },
+	{ 0x09, KEY_9 },
+	{ 0x00, KEY_0 },
+	{ 0x20, KEY_UP },	/* CH+ */
+	{ 0x21, KEY_DOWN },	/* CH+ */
+	{ 0x12, KEY_VOLUMEUP },	/* Brightness Up */
+	{ 0x13, KEY_VOLUMEDOWN },/* Brightness Down */
+	{ 0x1f, KEY_RECORD },
+	{ 0x17, KEY_PLAY },
+	{ 0x16, KEY_PAUSE },
+	{ 0x0b, KEY_STOP },
+	{ 0x27, KEY_FASTFORWARD },/* >> */
+	{ 0x26, KEY_REWIND },	/* << */
+	{ 0x0d, KEY_OK },	/* Mute */
+	{ 0x11, KEY_LEFT },	/* VOL- */
+	{ 0x10, KEY_RIGHT },	/* VOL+ */
+	{ 0x29, KEY_BACK },	/* button under 9 */
+	{ 0x2c, KEY_MENU },	/* TTX */
+	{ 0x2b, KEY_EPG },	/* EPG */
+	{ 0x1e, KEY_RED },	/* OSD */
+	{ 0x0e, KEY_GREEN },	/* Window */
+	{ 0x2d, KEY_YELLOW },	/* button under << */
+	{ 0x0f, KEY_BLUE },	/* bottom yellow button */
+	{ 0x14, KEY_AUDIO },	/* Snapshot */
+	{ 0x38, KEY_TV },	/* TV/Radio */
+	{ 0x0c, KEY_ESC }	/* upper Red buttton */
+};
+
 static struct rc_map_dvb_usb_table_table keys_tables[] = {
 	{ rc_map_dw210x_table, ARRAY_SIZE(rc_map_dw210x_table) },
 	{ rc_map_tevii_table, ARRAY_SIZE(rc_map_tevii_table) },
 	{ rc_map_tbs_table, ARRAY_SIZE(rc_map_tbs_table) },
+	{ rc_map_su3000_table, ARRAY_SIZE(rc_map_su3000_table) },
 };
 
 static int dw2102_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
@@ -1089,7 +1403,8 @@ static int dw2102_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 	if ((ir_keymap > 0) && (ir_keymap <= ARRAY_SIZE(keys_tables))) {
 		keymap = keys_tables[ir_keymap - 1].rc_keys ;
 		keymap_size = keys_tables[ir_keymap - 1].rc_keys_size;
-	}
+	} else if (ir_keymap > ARRAY_SIZE(keys_tables))
+		return 0; /* none */
 
 	*state = REMOTE_NO_KEY_PRESSED;
 	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
@@ -1125,6 +1440,11 @@ static struct usb_device_id dw2102_table[] = {
 	{USB_DEVICE(0x3011, USB_PID_PROF_1100)},
 	{USB_DEVICE(0x9022, USB_PID_TEVII_S660)},
 	{USB_DEVICE(0x3034, 0x7500)},
+	{USB_DEVICE(0x1f4d, 0x3000)},
+	{USB_DEVICE(USB_VID_TERRATEC, 0x00a8)},
+	{USB_DEVICE(0x9022, USB_PID_TEVII_S480_1)},
+	{USB_DEVICE(0x9022, USB_PID_TEVII_S480_2)},
+	{USB_DEVICE(0x1f4d, 0x3100)},
 	{ }
 };
 
@@ -1184,11 +1504,6 @@ static int dw2102_load_firmware(struct usb_device *dev,
 		}
 		/* init registers */
 		switch (dev->descriptor.idProduct) {
-		case USB_PID_PROF_1100:
-			s6x0_properties.rc.legacy.rc_map_table = rc_map_tbs_table;
-			s6x0_properties.rc.legacy.rc_map_size =
-					ARRAY_SIZE(rc_map_tbs_table);
-			break;
 		case USB_PID_TEVII_S650:
 			dw2104_properties.rc.legacy.rc_map_table = rc_map_tevii_table;
 			dw2104_properties.rc.legacy.rc_map_size =
@@ -1271,8 +1586,6 @@ static struct dvb_usb_device_properties dw2102_properties = {
 	.adapter = {
 		{
 			.frontend_attach = dw2102_frontend_attach,
-			.streaming_ctrl = NULL,
-			.tuner_attach = NULL,
 			.stream = {
 				.type = USB_BULK,
 				.count = 8,
@@ -1324,8 +1637,6 @@ static struct dvb_usb_device_properties dw2104_properties = {
 	.adapter = {
 		{
 			.frontend_attach = dw2104_frontend_attach,
-			.streaming_ctrl = NULL,
-			/*.tuner_attach = dw2104_tuner_attach,*/
 			.stream = {
 				.type = USB_BULK,
 				.count = 8,
@@ -1373,7 +1684,6 @@ static struct dvb_usb_device_properties dw3101_properties = {
 	.adapter = {
 		{
 			.frontend_attach = dw3101_frontend_attach,
-			.streaming_ctrl = NULL,
 			.tuner_attach = dw3101_tuner_attach,
 			.stream = {
 				.type = USB_BULK,
@@ -1399,6 +1709,7 @@ static struct dvb_usb_device_properties dw3101_properties = {
 static struct dvb_usb_device_properties s6x0_properties = {
 	.caps = DVB_USB_IS_AN_I2C_ADAPTER,
 	.usb_ctrl = DEVICE_SPECIFIC,
+	.size_of_priv = sizeof(struct s6x0_state),
 	.firmware = "dvb-usb-s630.fw",
 	.no_reconnect = 1,
 
@@ -1416,9 +1727,7 @@ static struct dvb_usb_device_properties s6x0_properties = {
 	.read_mac_address = s6x0_read_mac_address,
 	.adapter = {
 		{
-			.frontend_attach = s6x0_frontend_attach,
-			.streaming_ctrl = NULL,
-			.tuner_attach = NULL,
+			.frontend_attach = zl100313_frontend_attach,
 			.stream = {
 				.type = USB_BULK,
 				.count = 8,
@@ -1431,23 +1740,41 @@ static struct dvb_usb_device_properties s6x0_properties = {
 			},
 		}
 	},
-	.num_device_descs = 3,
+	.num_device_descs = 1,
 	.devices = {
 		{"TeVii S630 USB",
 			{&dw2102_table[6], NULL},
 			{NULL},
 		},
-		{"Prof 1100 USB ",
-			{&dw2102_table[7], NULL},
-			{NULL},
-		},
-		{"TeVii S660 USB",
-			{&dw2102_table[8], NULL},
-			{NULL},
-		},
 	}
 };
 
+struct dvb_usb_device_properties *p1100;
+static struct dvb_usb_device_description d1100 = {
+	"Prof 1100 USB ",
+	{&dw2102_table[7], NULL},
+	{NULL},
+};
+
+struct dvb_usb_device_properties *s660;
+static struct dvb_usb_device_description d660 = {
+	"TeVii S660 USB",
+	{&dw2102_table[8], NULL},
+	{NULL},
+};
+
+static struct dvb_usb_device_description d480_1 = {
+	"TeVii S480.1 USB",
+	{&dw2102_table[12], NULL},
+	{NULL},
+};
+
+static struct dvb_usb_device_description d480_2 = {
+	"TeVii S480.2 USB",
+	{&dw2102_table[13], NULL},
+	{NULL},
+};
+
 struct dvb_usb_device_properties *p7500;
 static struct dvb_usb_device_description d7500 = {
 	"Prof 7500 USB DVB-S2",
@@ -1455,17 +1782,97 @@ static struct dvb_usb_device_description d7500 = {
 	{NULL},
 };
 
+static struct dvb_usb_device_properties su3000_properties = {
+	.caps = DVB_USB_IS_AN_I2C_ADAPTER,
+	.usb_ctrl = DEVICE_SPECIFIC,
+	.size_of_priv = sizeof(struct su3000_state),
+	.power_ctrl = su3000_power_ctrl,
+	.num_adapters = 1,
+	.identify_state	= su3000_identify_state,
+	.i2c_algo = &su3000_i2c_algo,
+
+	.rc.legacy = {
+		.rc_map_table = rc_map_su3000_table,
+		.rc_map_size = ARRAY_SIZE(rc_map_su3000_table),
+		.rc_interval = 150,
+		.rc_query = dw2102_rc_query,
+	},
+
+	.read_mac_address = su3000_read_mac_address,
+
+	.generic_bulk_ctrl_endpoint = 0x01,
+
+	.adapter = {
+		{
+			.streaming_ctrl   = su3000_streaming_ctrl,
+			.frontend_attach  = su3000_frontend_attach,
+			.stream = {
+				.type = USB_BULK,
+				.count = 8,
+				.endpoint = 0x82,
+				.u = {
+					.bulk = {
+						.buffersize = 4096,
+					}
+				}
+			}
+		}
+	},
+	.num_device_descs = 3,
+	.devices = {
+		{ "SU3000HD DVB-S USB2.0",
+			{ &dw2102_table[10], NULL },
+			{ NULL },
+		},
+		{ "Terratec Cinergy S2 USB HD",
+			{ &dw2102_table[11], NULL },
+			{ NULL },
+		},
+		{ "X3M TV SPC1400HD PCI",
+			{ &dw2102_table[14], NULL },
+			{ NULL },
+		},
+	}
+};
+
 static int dw2102_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
+	p1100 = kzalloc(sizeof(struct dvb_usb_device_properties), GFP_KERNEL);
+	if (!p1100)
+		return -ENOMEM;
+	/* copy default structure */
+	memcpy(p1100, &s6x0_properties,
+			sizeof(struct dvb_usb_device_properties));
+	/* fill only different fields */
+	p1100->firmware = "dvb-usb-p1100.fw";
+	p1100->devices[0] = d1100;
+	p1100->rc.legacy.rc_map_table = rc_map_tbs_table;
+	p1100->rc.legacy.rc_map_size = ARRAY_SIZE(rc_map_tbs_table);
+	p1100->adapter->frontend_attach = stv0288_frontend_attach;
+
+	s660 = kzalloc(sizeof(struct dvb_usb_device_properties), GFP_KERNEL);
+	if (!s660) {
+		kfree(p1100);
+		return -ENOMEM;
+	}
+	memcpy(s660, &s6x0_properties,
+			sizeof(struct dvb_usb_device_properties));
+	s660->firmware = "dvb-usb-s660.fw";
+	s660->num_device_descs = 3;
+	s660->devices[0] = d660;
+	s660->devices[1] = d480_1;
+	s660->devices[2] = d480_2;
+	s660->adapter->frontend_attach = ds3000_frontend_attach;
 
 	p7500 = kzalloc(sizeof(struct dvb_usb_device_properties), GFP_KERNEL);
-	if (!p7500)
+	if (!p7500) {
+		kfree(p1100);
+		kfree(s660);
 		return -ENOMEM;
-	/* copy default structure */
+	}
 	memcpy(p7500, &s6x0_properties,
 			sizeof(struct dvb_usb_device_properties));
-	/* fill only different fields */
 	p7500->firmware = "dvb-usb-p7500.fw";
 	p7500->devices[0] = d7500;
 	p7500->rc.legacy.rc_map_table = rc_map_tbs_table;
@@ -1480,8 +1887,14 @@ static int dw2102_probe(struct usb_interface *intf,
 			THIS_MODULE, NULL, adapter_nr) ||
 	    0 == dvb_usb_device_init(intf, &s6x0_properties,
 			THIS_MODULE, NULL, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf, p1100,
+			THIS_MODULE, NULL, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf, s660,
+			THIS_MODULE, NULL, adapter_nr) ||
 	    0 == dvb_usb_device_init(intf, p7500,
-			THIS_MODULE, NULL, adapter_nr))
+			THIS_MODULE, NULL, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf, &su3000_properties,
+				     THIS_MODULE, NULL, adapter_nr))
 		return 0;
 
 	return -ENODEV;
@@ -1514,7 +1927,8 @@ module_exit(dw2102_module_exit);
 MODULE_AUTHOR("Igor M. Liplianin (c) liplianin@me.by");
 MODULE_DESCRIPTION("Driver for DVBWorld DVB-S 2101, 2102, DVB-S2 2104,"
 				" DVB-C 3101 USB2.0,"
-				" TeVii S600, S630, S650, S660 USB2.0,"
-				" Prof 1100, 7500 USB2.0 devices");
+				" TeVii S600, S630, S650, S660, S480,"
+				" Prof 1100, 7500 USB2.0,"
+				" Geniatech SU3000 devices");
 MODULE_VERSION("0.1");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/lmedm04.c b/drivers/media/dvb/dvb-usb/lmedm04.c
index 9eea4188303b..46ccd01a7696 100644
--- a/drivers/media/dvb/dvb-usb/lmedm04.c
+++ b/drivers/media/dvb/dvb-usb/lmedm04.c
@@ -659,7 +659,7 @@ static int lme2510_download_firmware(struct usb_device *dev,
 }
 
 /* Default firmware for LME2510C */
-const char lme_firmware[50] = "dvb-usb-lme2510c-s7395.fw";
+char lme_firmware[50] = "dvb-usb-lme2510c-s7395.fw";
 
 static void lme_coldreset(struct usb_device *dev)
 {
@@ -1006,7 +1006,7 @@ static struct dvb_usb_device_properties lme2510c_properties = {
 	.caps = DVB_USB_IS_AN_I2C_ADAPTER,
 	.usb_ctrl = DEVICE_SPECIFIC,
 	.download_firmware = lme2510_download_firmware,
-	.firmware = lme_firmware,
+	.firmware = (const char *)&lme_firmware,
 	.size_of_priv = sizeof(struct lme2510_state),
 	.num_adapters = 1,
 	.adapter = {
@@ -1109,5 +1109,5 @@ module_exit(lme2510_module_exit);
 
 MODULE_AUTHOR("Malcolm Priestley <tvboxspy@gmail.com>");
 MODULE_DESCRIPTION("LME2510(C) DVB-S USB2.0");
-MODULE_VERSION("1.74");
+MODULE_VERSION("1.75");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/dvb-usb/technisat-usb2.c b/drivers/media/dvb/dvb-usb/technisat-usb2.c
new file mode 100644
index 000000000000..08f8842ad280
--- /dev/null
+++ b/drivers/media/dvb/dvb-usb/technisat-usb2.c
@@ -0,0 +1,807 @@
+/*
+ * Linux driver for Technisat DVB-S/S2 USB 2.0 device
+ *
+ * Copyright (C) 2010 Patrick Boettcher,
+ *                    Kernel Labs Inc. PO Box 745, St James, NY 11780
+ *
+ * Development was sponsored by Technisat Digital UK Limited, whose
+ * registered office is Witan Gate House 500 - 600 Witan Gate West,
+ * Milton Keynes, MK9 1SH
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ * THIS PROGRAM IS PROVIDED "AS IS" AND BOTH THE COPYRIGHT HOLDER AND
+ * TECHNISAT DIGITAL UK LTD DISCLAIM ALL WARRANTIES WITH REGARD TO
+ * THIS PROGRAM INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY OR
+ * FITNESS FOR A PARTICULAR PURPOSE.  NEITHER THE COPYRIGHT HOLDER
+ * NOR TECHNISAT DIGITAL UK LIMITED SHALL 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 PROGRAM. See the
+ * GNU General Public License for more details.
+ */
+
+#define DVB_USB_LOG_PREFIX "technisat-usb2"
+#include "dvb-usb.h"
+
+#include "stv6110x.h"
+#include "stv090x.h"
+
+/* module parameters */
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug,
+		"set debugging level (bit-mask: 1=info,2=eeprom,4=i2c,8=rc)." \
+		DVB_USB_DEBUG_STATUS);
+
+/* disables all LED control command and
+ * also does not start the signal polling thread */
+static int disable_led_control;
+module_param(disable_led_control, int, 0444);
+MODULE_PARM_DESC(disable_led_control,
+		"disable LED control of the device "
+		"(default: 0 - LED control is active).");
+
+/* device private data */
+struct technisat_usb2_state {
+	struct dvb_usb_device *dev;
+	struct delayed_work green_led_work;
+	u8 power_state;
+
+	u16 last_scan_code;
+};
+
+/* debug print helpers */
+#define deb_info(args...)    dprintk(debug, 0x01, args)
+#define deb_eeprom(args...)  dprintk(debug, 0x02, args)
+#define deb_i2c(args...)     dprintk(debug, 0x04, args)
+#define deb_rc(args...)      dprintk(debug, 0x08, args)
+
+/* vendor requests */
+#define SET_IFCLK_TO_EXTERNAL_TSCLK_VENDOR_REQUEST 0xB3
+#define SET_FRONT_END_RESET_VENDOR_REQUEST         0xB4
+#define GET_VERSION_INFO_VENDOR_REQUEST            0xB5
+#define SET_GREEN_LED_VENDOR_REQUEST               0xB6
+#define SET_RED_LED_VENDOR_REQUEST                 0xB7
+#define GET_IR_DATA_VENDOR_REQUEST                 0xB8
+#define SET_LED_TIMER_DIVIDER_VENDOR_REQUEST       0xB9
+#define SET_USB_REENUMERATION                      0xBA
+
+/* i2c-access methods */
+#define I2C_SPEED_100KHZ_BIT 0x40
+
+#define I2C_STATUS_NAK 7
+#define I2C_STATUS_OK 8
+
+static int technisat_usb2_i2c_access(struct usb_device *udev,
+		u8 device_addr, u8 *tx, u8 txlen, u8 *rx, u8 rxlen)
+{
+	u8 b[64];
+	int ret, actual_length;
+
+	deb_i2c("i2c-access: %02x, tx: ", device_addr);
+	debug_dump(tx, txlen, deb_i2c);
+	deb_i2c(" ");
+
+	if (txlen > 62) {
+		err("i2c TX buffer can't exceed 62 bytes (dev 0x%02x)",
+				device_addr);
+		txlen = 62;
+	}
+	if (rxlen > 62) {
+		err("i2c RX buffer can't exceed 62 bytes (dev 0x%02x)",
+				device_addr);
+		txlen = 62;
+	}
+
+	b[0] = I2C_SPEED_100KHZ_BIT;
+	b[1] = device_addr << 1;
+
+	if (rx != NULL) {
+		b[0] |= rxlen;
+		b[1] |= 1;
+	}
+
+	memcpy(&b[2], tx, txlen);
+	ret = usb_bulk_msg(udev,
+			usb_sndbulkpipe(udev, 0x01),
+			b, 2 + txlen,
+			NULL, 1000);
+
+	if (ret < 0) {
+		err("i2c-error: out failed %02x = %d", device_addr, ret);
+		return -ENODEV;
+	}
+
+	ret = usb_bulk_msg(udev,
+			usb_rcvbulkpipe(udev, 0x01),
+			b, 64, &actual_length, 1000);
+	if (ret < 0) {
+		err("i2c-error: in failed %02x = %d", device_addr, ret);
+		return -ENODEV;
+	}
+
+	if (b[0] != I2C_STATUS_OK) {
+		err("i2c-error: %02x = %d", device_addr, b[0]);
+		/* handle tuner-i2c-nak */
+		if (!(b[0] == I2C_STATUS_NAK &&
+				device_addr == 0x60
+				/* && device_is_technisat_usb2 */))
+			return -ENODEV;
+	}
+
+	deb_i2c("status: %d, ", b[0]);
+
+	if (rx != NULL) {
+		memcpy(rx, &b[2], rxlen);
+
+		deb_i2c("rx (%d): ", rxlen);
+		debug_dump(rx, rxlen, deb_i2c);
+	}
+
+	deb_i2c("\n");
+
+	return 0;
+}
+
+static int technisat_usb2_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msg,
+				int num)
+{
+	int ret = 0, i;
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+
+	/* Ensure nobody else hits the i2c bus while we're sending our
+	   sequence of messages, (such as the remote control thread) */
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	for (i = 0; i < num; i++) {
+		if (i+1 < num && msg[i+1].flags & I2C_M_RD) {
+			ret = technisat_usb2_i2c_access(d->udev, msg[i+1].addr,
+						msg[i].buf, msg[i].len,
+						msg[i+1].buf, msg[i+1].len);
+			if (ret != 0)
+				break;
+			i++;
+		} else {
+			ret = technisat_usb2_i2c_access(d->udev, msg[i].addr,
+						msg[i].buf, msg[i].len,
+						NULL, 0);
+			if (ret != 0)
+				break;
+		}
+	}
+
+	if (ret == 0)
+		ret = i;
+
+	mutex_unlock(&d->i2c_mutex);
+
+	return ret;
+}
+
+static u32 technisat_usb2_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
+
+static struct i2c_algorithm technisat_usb2_i2c_algo = {
+	.master_xfer   = technisat_usb2_i2c_xfer,
+	.functionality = technisat_usb2_i2c_func,
+};
+
+#if 0
+static void technisat_usb2_frontend_reset(struct usb_device *udev)
+{
+	usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			SET_FRONT_END_RESET_VENDOR_REQUEST,
+			USB_TYPE_VENDOR | USB_DIR_OUT,
+			10, 0,
+			NULL, 0, 500);
+}
+#endif
+
+/* LED control */
+enum technisat_usb2_led_state {
+	LED_OFF,
+	LED_BLINK,
+	LED_ON,
+	LED_UNDEFINED
+};
+
+static int technisat_usb2_set_led(struct dvb_usb_device *d, int red, enum technisat_usb2_led_state state)
+{
+	int ret;
+
+	u8 led[8] = {
+		red ? SET_RED_LED_VENDOR_REQUEST : SET_GREEN_LED_VENDOR_REQUEST,
+		0
+	};
+
+	if (disable_led_control && state != LED_OFF)
+		return 0;
+
+	switch (state) {
+	case LED_ON:
+		led[1] = 0x82;
+		break;
+	case LED_BLINK:
+		led[1] = 0x82;
+		if (red) {
+			led[2] = 0x02;
+			led[3] = 10;
+			led[4] = 10;
+		} else {
+			led[2] = 0xff;
+			led[3] = 50;
+			led[4] = 50;
+		}
+		led[5] = 1;
+		break;
+
+	default:
+	case LED_OFF:
+		led[1] = 0x80;
+		break;
+	}
+
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	ret = usb_control_msg(d->udev, usb_sndctrlpipe(d->udev, 0),
+		red ? SET_RED_LED_VENDOR_REQUEST : SET_GREEN_LED_VENDOR_REQUEST,
+		USB_TYPE_VENDOR | USB_DIR_OUT,
+		0, 0,
+		led, sizeof(led), 500);
+
+	mutex_unlock(&d->i2c_mutex);
+	return ret;
+}
+
+static int technisat_usb2_set_led_timer(struct dvb_usb_device *d, u8 red, u8 green)
+{
+	int ret;
+	u8 b = 0;
+
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	ret = usb_control_msg(d->udev, usb_sndctrlpipe(d->udev, 0),
+		SET_LED_TIMER_DIVIDER_VENDOR_REQUEST,
+		USB_TYPE_VENDOR | USB_DIR_OUT,
+		(red << 8) | green, 0,
+		&b, 1, 500);
+
+	mutex_unlock(&d->i2c_mutex);
+
+	return ret;
+}
+
+static void technisat_usb2_green_led_control(struct work_struct *work)
+{
+	struct technisat_usb2_state *state =
+		container_of(work, struct technisat_usb2_state, green_led_work.work);
+	struct dvb_frontend *fe = state->dev->adapter[0].fe;
+
+	if (state->power_state == 0)
+		goto schedule;
+
+	if (fe != NULL) {
+		enum fe_status status;
+
+		if (fe->ops.read_status(fe, &status) != 0)
+			goto schedule;
+
+		if (status & FE_HAS_LOCK) {
+			u32 ber;
+
+			if (fe->ops.read_ber(fe, &ber) != 0)
+				goto schedule;
+
+			if (ber > 1000)
+				technisat_usb2_set_led(state->dev, 0, LED_BLINK);
+			else
+				technisat_usb2_set_led(state->dev, 0, LED_ON);
+		} else
+			technisat_usb2_set_led(state->dev, 0, LED_OFF);
+	}
+
+schedule:
+	schedule_delayed_work(&state->green_led_work,
+			msecs_to_jiffies(500));
+}
+
+/* method to find out whether the firmware has to be downloaded or not */
+static int technisat_usb2_identify_state(struct usb_device *udev,
+		struct dvb_usb_device_properties *props,
+		struct dvb_usb_device_description **desc, int *cold)
+{
+	int ret;
+	u8 version[3];
+
+	/* first select the interface */
+	if (usb_set_interface(udev, 0, 1) != 0)
+		err("could not set alternate setting to 0");
+	else
+		info("set alternate setting");
+
+	*cold = 0; /* by default do not download a firmware - just in case something is wrong */
+
+	ret = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+		GET_VERSION_INFO_VENDOR_REQUEST,
+		USB_TYPE_VENDOR | USB_DIR_IN,
+		0, 0,
+		version, sizeof(version), 500);
+
+	if (ret < 0)
+		*cold = 1;
+	else {
+		info("firmware version: %d.%d", version[1], version[2]);
+		*cold = 0;
+	}
+
+	return 0;
+}
+
+/* power control */
+static int technisat_usb2_power_ctrl(struct dvb_usb_device *d, int level)
+{
+	struct technisat_usb2_state *state = d->priv;
+
+	state->power_state = level;
+
+	if (disable_led_control)
+		return 0;
+
+	/* green led is turned off in any case - will be turned on when tuning */
+	technisat_usb2_set_led(d, 0, LED_OFF);
+	/* red led is turned on all the time */
+	technisat_usb2_set_led(d, 1, LED_ON);
+	return 0;
+}
+
+/* mac address reading - from the eeprom */
+#if 0
+static void technisat_usb2_eeprom_dump(struct dvb_usb_device *d)
+{
+	u8 reg;
+	u8 b[16];
+	int i, j;
+
+	/* full EEPROM dump */
+	for (j = 0; j < 256 * 4; j += 16) {
+		reg = j;
+		if (technisat_usb2_i2c_access(d->udev, 0x50 + j / 256, &reg, 1, b, 16) != 0)
+			break;
+
+		deb_eeprom("EEPROM: %01x%02x: ", j / 256, reg);
+		for (i = 0; i < 16; i++)
+			deb_eeprom("%02x ", b[i]);
+		deb_eeprom("\n");
+	}
+}
+#endif
+
+static u8 technisat_usb2_calc_lrc(const u8 *b, u16 length)
+{
+	u8 lrc = 0;
+	while (--length)
+		lrc ^= *b++;
+	return lrc;
+}
+
+static int technisat_usb2_eeprom_lrc_read(struct dvb_usb_device *d,
+	u16 offset, u8 *b, u16 length, u8 tries)
+{
+	u8 bo = offset & 0xff;
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x50 | ((offset >> 8) & 0x3),
+			.buf = &bo,
+			.len = 1
+		}, {
+			.addr = 0x50 | ((offset >> 8) & 0x3),
+			.flags	= I2C_M_RD,
+			.buf = b,
+			.len = length
+		}
+	};
+
+	while (tries--) {
+		int status;
+
+		if (i2c_transfer(&d->i2c_adap, msg, 2) != 2)
+			break;
+
+		status =
+			technisat_usb2_calc_lrc(b, length - 1) == b[length - 1];
+
+		if (status)
+			return 0;
+	}
+
+	return -EREMOTEIO;
+}
+
+#define EEPROM_MAC_START 0x3f8
+#define EEPROM_MAC_TOTAL 8
+static int technisat_usb2_read_mac_address(struct dvb_usb_device *d,
+		u8 mac[])
+{
+	u8 buf[EEPROM_MAC_TOTAL];
+
+	if (technisat_usb2_eeprom_lrc_read(d, EEPROM_MAC_START,
+				buf, EEPROM_MAC_TOTAL, 4) != 0)
+		return -ENODEV;
+
+	memcpy(mac, buf, 6);
+	return 0;
+}
+
+/* frontend attach */
+static int technisat_usb2_set_voltage(struct dvb_frontend *fe,
+		fe_sec_voltage_t voltage)
+{
+	int i;
+	u8 gpio[3] = { 0 }; /* 0 = 2, 1 = 3, 2 = 4 */
+
+	gpio[2] = 1; /* high - voltage ? */
+
+	switch (voltage) {
+	case SEC_VOLTAGE_13:
+		gpio[0] = 1;
+		break;
+	case SEC_VOLTAGE_18:
+		gpio[0] = 1;
+		gpio[1] = 1;
+		break;
+	default:
+	case SEC_VOLTAGE_OFF:
+		break;
+	}
+
+	for (i = 0; i < 3; i++)
+		if (stv090x_set_gpio(fe, i+2, 0, gpio[i], 0) != 0)
+			return -EREMOTEIO;
+	return 0;
+}
+
+static struct stv090x_config technisat_usb2_stv090x_config = {
+	.device         = STV0903,
+	.demod_mode     = STV090x_SINGLE,
+	.clk_mode       = STV090x_CLK_EXT,
+
+	.xtal           = 8000000,
+	.address        = 0x68,
+
+	.ts1_mode       = STV090x_TSMODE_DVBCI,
+	.ts1_clk        = 13400000,
+	.ts1_tei        = 1,
+
+	.repeater_level = STV090x_RPTLEVEL_64,
+
+	.tuner_bbgain   = 6,
+};
+
+static struct stv6110x_config technisat_usb2_stv6110x_config = {
+	.addr           = 0x60,
+	.refclk         = 16000000,
+	.clk_div        = 2,
+};
+
+static int technisat_usb2_frontend_attach(struct dvb_usb_adapter *a)
+{
+	struct usb_device *udev = a->dev->udev;
+	int ret;
+
+	a->fe = dvb_attach(stv090x_attach, &technisat_usb2_stv090x_config,
+			&a->dev->i2c_adap, STV090x_DEMODULATOR_0);
+
+	if (a->fe) {
+		struct stv6110x_devctl *ctl;
+
+		ctl = dvb_attach(stv6110x_attach,
+				a->fe,
+				&technisat_usb2_stv6110x_config,
+				&a->dev->i2c_adap);
+
+		if (ctl) {
+			technisat_usb2_stv090x_config.tuner_init          = ctl->tuner_init;
+			technisat_usb2_stv090x_config.tuner_sleep         = ctl->tuner_sleep;
+			technisat_usb2_stv090x_config.tuner_set_mode      = ctl->tuner_set_mode;
+			technisat_usb2_stv090x_config.tuner_set_frequency = ctl->tuner_set_frequency;
+			technisat_usb2_stv090x_config.tuner_get_frequency = ctl->tuner_get_frequency;
+			technisat_usb2_stv090x_config.tuner_set_bandwidth = ctl->tuner_set_bandwidth;
+			technisat_usb2_stv090x_config.tuner_get_bandwidth = ctl->tuner_get_bandwidth;
+			technisat_usb2_stv090x_config.tuner_set_bbgain    = ctl->tuner_set_bbgain;
+			technisat_usb2_stv090x_config.tuner_get_bbgain    = ctl->tuner_get_bbgain;
+			technisat_usb2_stv090x_config.tuner_set_refclk    = ctl->tuner_set_refclk;
+			technisat_usb2_stv090x_config.tuner_get_status    = ctl->tuner_get_status;
+
+			/* call the init function once to initialize
+			   tuner's clock output divider and demod's
+			   master clock */
+			if (a->fe->ops.init)
+				a->fe->ops.init(a->fe);
+
+			if (mutex_lock_interruptible(&a->dev->i2c_mutex) < 0)
+				return -EAGAIN;
+
+			ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+					SET_IFCLK_TO_EXTERNAL_TSCLK_VENDOR_REQUEST,
+					USB_TYPE_VENDOR | USB_DIR_OUT,
+					0, 0,
+					NULL, 0, 500);
+			mutex_unlock(&a->dev->i2c_mutex);
+
+			if (ret != 0)
+				err("could not set IF_CLK to external");
+
+			a->fe->ops.set_voltage = technisat_usb2_set_voltage;
+
+			/* if everything was successful assign a nice name to the frontend */
+			strlcpy(a->fe->ops.info.name, a->dev->desc->name,
+					sizeof(a->fe->ops.info.name));
+		} else {
+			dvb_frontend_detach(a->fe);
+			a->fe = NULL;
+		}
+	}
+
+	technisat_usb2_set_led_timer(a->dev, 1, 1);
+
+	return a->fe == NULL ? -ENODEV : 0;
+}
+
+/* Remote control */
+
+/* the device is giving providing raw IR-signals to the host mapping
+ * it only to one remote control is just the default implementation
+ */
+#define NOMINAL_IR_BIT_TRANSITION_TIME_US 889
+#define NOMINAL_IR_BIT_TIME_US (2 * NOMINAL_IR_BIT_TRANSITION_TIME_US)
+
+#define FIRMWARE_CLOCK_TICK 83333
+#define FIRMWARE_CLOCK_DIVISOR 256
+
+#define IR_PERCENT_TOLERANCE 15
+
+#define NOMINAL_IR_BIT_TRANSITION_TICKS ((NOMINAL_IR_BIT_TRANSITION_TIME_US * 1000 * 1000) / FIRMWARE_CLOCK_TICK)
+#define NOMINAL_IR_BIT_TRANSITION_TICK_COUNT (NOMINAL_IR_BIT_TRANSITION_TICKS / FIRMWARE_CLOCK_DIVISOR)
+
+#define NOMINAL_IR_BIT_TIME_TICKS ((NOMINAL_IR_BIT_TIME_US * 1000 * 1000) / FIRMWARE_CLOCK_TICK)
+#define NOMINAL_IR_BIT_TIME_TICK_COUNT (NOMINAL_IR_BIT_TIME_TICKS / FIRMWARE_CLOCK_DIVISOR)
+
+#define MINIMUM_IR_BIT_TRANSITION_TICK_COUNT (NOMINAL_IR_BIT_TRANSITION_TICK_COUNT - ((NOMINAL_IR_BIT_TRANSITION_TICK_COUNT * IR_PERCENT_TOLERANCE) / 100))
+#define MAXIMUM_IR_BIT_TRANSITION_TICK_COUNT (NOMINAL_IR_BIT_TRANSITION_TICK_COUNT + ((NOMINAL_IR_BIT_TRANSITION_TICK_COUNT * IR_PERCENT_TOLERANCE) / 100))
+
+#define MINIMUM_IR_BIT_TIME_TICK_COUNT (NOMINAL_IR_BIT_TIME_TICK_COUNT - ((NOMINAL_IR_BIT_TIME_TICK_COUNT * IR_PERCENT_TOLERANCE) / 100))
+#define MAXIMUM_IR_BIT_TIME_TICK_COUNT (NOMINAL_IR_BIT_TIME_TICK_COUNT + ((NOMINAL_IR_BIT_TIME_TICK_COUNT * IR_PERCENT_TOLERANCE) / 100))
+
+static int technisat_usb2_get_ir(struct dvb_usb_device *d)
+{
+	u8 buf[62], *b;
+	int ret;
+	struct ir_raw_event ev;
+
+	buf[0] = GET_IR_DATA_VENDOR_REQUEST;
+	buf[1] = 0x08;
+	buf[2] = 0x8f;
+	buf[3] = MINIMUM_IR_BIT_TRANSITION_TICK_COUNT;
+	buf[4] = MAXIMUM_IR_BIT_TIME_TICK_COUNT;
+
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+	ret = usb_control_msg(d->udev, usb_sndctrlpipe(d->udev, 0),
+			GET_IR_DATA_VENDOR_REQUEST,
+			USB_TYPE_VENDOR | USB_DIR_OUT,
+			0, 0,
+			buf, 5, 500);
+	if (ret < 0)
+		goto unlock;
+
+	buf[1] = 0;
+	buf[2] = 0;
+	ret = usb_control_msg(d->udev, usb_rcvctrlpipe(d->udev, 0),
+			GET_IR_DATA_VENDOR_REQUEST,
+			USB_TYPE_VENDOR | USB_DIR_IN,
+			0x8080, 0,
+			buf, sizeof(buf), 500);
+
+unlock:
+	mutex_unlock(&d->i2c_mutex);
+
+	if (ret < 0)
+		return ret;
+
+	if (ret == 1)
+		return 0; /* no key pressed */
+
+	/* decoding */
+	b = buf+1;
+
+#if 0
+	deb_rc("RC: %d ", ret);
+	debug_dump(b, ret, deb_rc);
+#endif
+
+	ev.pulse = 0;
+	while (1) {
+		ev.pulse = !ev.pulse;
+		ev.duration = (*b * FIRMWARE_CLOCK_DIVISOR * FIRMWARE_CLOCK_TICK) / 1000;
+		ir_raw_event_store(d->rc_dev, &ev);
+
+		b++;
+		if (*b == 0xff) {
+			ev.pulse = 0;
+			ev.duration = 888888*2;
+			ir_raw_event_store(d->rc_dev, &ev);
+			break;
+		}
+	}
+
+	ir_raw_event_handle(d->rc_dev);
+
+	return 1;
+}
+
+static int technisat_usb2_rc_query(struct dvb_usb_device *d)
+{
+	int ret = technisat_usb2_get_ir(d);
+
+	if (ret < 0)
+		return ret;
+
+	if (ret == 0)
+		return 0;
+
+	if (!disable_led_control)
+		technisat_usb2_set_led(d, 1, LED_BLINK);
+
+	return 0;
+}
+
+/* DVB-USB and USB stuff follows */
+static struct usb_device_id technisat_usb2_id_table[] = {
+	{ USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_DVB_S2) },
+	{ 0 }		/* Terminating entry */
+};
+
+/* device description */
+static struct dvb_usb_device_properties technisat_usb2_devices = {
+	.caps              = DVB_USB_IS_AN_I2C_ADAPTER,
+
+	.usb_ctrl          = CYPRESS_FX2,
+
+	.identify_state    = technisat_usb2_identify_state,
+	.firmware          = "dvb-usb-SkyStar_USB_HD_FW_v17_63.HEX.fw",
+
+	.size_of_priv      = sizeof(struct technisat_usb2_state),
+
+	.i2c_algo          = &technisat_usb2_i2c_algo,
+
+	.power_ctrl        = technisat_usb2_power_ctrl,
+	.read_mac_address  = technisat_usb2_read_mac_address,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+			.frontend_attach  = technisat_usb2_frontend_attach,
+
+			.stream = {
+				.type = USB_ISOC,
+				.count = 8,
+				.endpoint = 0x2,
+				.u = {
+					.isoc = {
+						.framesperurb = 32,
+						.framesize = 2048,
+						.interval = 3,
+					}
+				}
+			},
+
+			.size_of_priv = 0,
+		},
+	},
+
+	.num_device_descs = 1,
+	.devices = {
+		{   "Technisat SkyStar USB HD (DVB-S/S2)",
+			{ &technisat_usb2_id_table[0], NULL },
+			{ NULL },
+		},
+	},
+
+	.rc.core = {
+		.rc_interval = 100,
+		.rc_codes    = RC_MAP_TECHNISAT_USB2,
+		.module_name = "technisat-usb2",
+		.rc_query    = technisat_usb2_rc_query,
+		.allowed_protos = RC_TYPE_ALL,
+		.driver_type    = RC_DRIVER_IR_RAW,
+	}
+};
+
+static int technisat_usb2_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	struct dvb_usb_device *dev;
+
+	if (dvb_usb_device_init(intf, &technisat_usb2_devices, THIS_MODULE,
+				&dev, adapter_nr) != 0)
+		return -ENODEV;
+
+	if (dev) {
+		struct technisat_usb2_state *state = dev->priv;
+		state->dev = dev;
+
+		if (!disable_led_control) {
+			INIT_DELAYED_WORK(&state->green_led_work,
+					technisat_usb2_green_led_control);
+			schedule_delayed_work(&state->green_led_work,
+					msecs_to_jiffies(500));
+		}
+	}
+
+	return 0;
+}
+
+static void technisat_usb2_disconnect(struct usb_interface *intf)
+{
+	struct dvb_usb_device *dev = usb_get_intfdata(intf);
+
+	/* work and stuff was only created when the device is is hot-state */
+	if (dev != NULL) {
+		struct technisat_usb2_state *state = dev->priv;
+		if (state != NULL) {
+			cancel_delayed_work_sync(&state->green_led_work);
+			flush_scheduled_work();
+		}
+	}
+
+	dvb_usb_device_exit(intf);
+}
+
+static struct usb_driver technisat_usb2_driver = {
+	.name       = "dvb_usb_technisat_usb2",
+	.probe      = technisat_usb2_probe,
+	.disconnect = technisat_usb2_disconnect,
+	.id_table   = technisat_usb2_id_table,
+};
+
+/* module stuff */
+static int __init technisat_usb2_module_init(void)
+{
+	int result = usb_register(&technisat_usb2_driver);
+	if (result) {
+		err("usb_register failed. Code %d", result);
+		return result;
+	}
+
+	return 0;
+}
+
+static void __exit technisat_usb2_module_exit(void)
+{
+	usb_deregister(&technisat_usb2_driver);
+}
+
+module_init(technisat_usb2_module_init);
+module_exit(technisat_usb2_module_exit);
+
+MODULE_AUTHOR("Patrick Boettcher <pboettcher@kernellabs.com>");
+MODULE_DESCRIPTION("Driver for Technisat DVB-S/S2 USB 2.0 device");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/firewire/Kconfig b/drivers/media/dvb/firewire/Kconfig
index 4afa29256df1..f3e9448c3955 100644
--- a/drivers/media/dvb/firewire/Kconfig
+++ b/drivers/media/dvb/firewire/Kconfig
@@ -1,6 +1,6 @@
 config DVB_FIREDTV
 	tristate "FireDTV and FloppyDTV"
-	depends on DVB_CORE && (FIREWIRE || IEEE1394)
+	depends on DVB_CORE && FIREWIRE
 	help
 	  Support for DVB receivers from Digital Everywhere
 	  which are connected via IEEE 1394 (FireWire).
@@ -13,12 +13,6 @@ config DVB_FIREDTV
 
 if DVB_FIREDTV
 
-config DVB_FIREDTV_FIREWIRE
-	def_bool FIREWIRE = y || (FIREWIRE = m && DVB_FIREDTV = m)
-
-config DVB_FIREDTV_IEEE1394
-	def_bool IEEE1394 = y || (IEEE1394 = m && DVB_FIREDTV = m)
-
 config DVB_FIREDTV_INPUT
 	def_bool INPUT = y || (INPUT = m && DVB_FIREDTV = m)
 
diff --git a/drivers/media/dvb/firewire/Makefile b/drivers/media/dvb/firewire/Makefile
index da84203d51c6..357b3aab186b 100644
--- a/drivers/media/dvb/firewire/Makefile
+++ b/drivers/media/dvb/firewire/Makefile
@@ -1,9 +1,6 @@
 obj-$(CONFIG_DVB_FIREDTV) += firedtv.o
 
-firedtv-y := firedtv-avc.o firedtv-ci.o firedtv-dvb.o firedtv-fe.o
-firedtv-$(CONFIG_DVB_FIREDTV_FIREWIRE) += firedtv-fw.o
-firedtv-$(CONFIG_DVB_FIREDTV_IEEE1394) += firedtv-1394.o
+firedtv-y := firedtv-avc.o firedtv-ci.o firedtv-dvb.o firedtv-fe.o firedtv-fw.o
 firedtv-$(CONFIG_DVB_FIREDTV_INPUT)    += firedtv-rc.o
 
 ccflags-y += -Idrivers/media/dvb/dvb-core
-ccflags-$(CONFIG_DVB_FIREDTV_IEEE1394) += -Idrivers/ieee1394
diff --git a/drivers/media/dvb/firewire/firedtv-1394.c b/drivers/media/dvb/firewire/firedtv-1394.c
deleted file mode 100644
index b34ca7afb0e6..000000000000
--- a/drivers/media/dvb/firewire/firedtv-1394.c
+++ /dev/null
@@ -1,300 +0,0 @@
-/*
- * FireDTV driver -- ieee1394 I/O backend
- *
- * Copyright (C) 2004 Andreas Monitzer <andy@monitzer.com>
- * Copyright (C) 2007-2008 Ben Backx <ben@bbackx.com>
- * Copyright (C) 2008 Henrik Kurelid <henrik@kurelid.se>
- *
- *	This program is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU General Public License as
- *	published by the Free Software Foundation; either version 2 of
- *	the License, or (at your option) any later version.
- */
-
-#include <linux/device.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/slab.h>
-#include <linux/spinlock.h>
-#include <linux/types.h>
-
-#include <dma.h>
-#include <csr1212.h>
-#include <highlevel.h>
-#include <hosts.h>
-#include <ieee1394.h>
-#include <iso.h>
-#include <nodemgr.h>
-
-#include <dvb_demux.h>
-
-#include "firedtv.h"
-
-static LIST_HEAD(node_list);
-static DEFINE_SPINLOCK(node_list_lock);
-
-#define CIP_HEADER_SIZE			8
-#define MPEG2_TS_HEADER_SIZE		4
-#define MPEG2_TS_SOURCE_PACKET_SIZE	(4 + 188)
-
-static void rawiso_activity_cb(struct hpsb_iso *iso)
-{
-	struct firedtv *f, *fdtv = NULL;
-	unsigned int i, num, packet;
-	unsigned char *buf;
-	unsigned long flags;
-	int count;
-
-	spin_lock_irqsave(&node_list_lock, flags);
-	list_for_each_entry(f, &node_list, list)
-		if (f->backend_data == iso) {
-			fdtv = f;
-			break;
-		}
-	spin_unlock_irqrestore(&node_list_lock, flags);
-
-	packet = iso->first_packet;
-	num = hpsb_iso_n_ready(iso);
-
-	if (!fdtv) {
-		pr_err("received at unknown iso channel\n");
-		goto out;
-	}
-
-	for (i = 0; i < num; i++, packet = (packet + 1) % iso->buf_packets) {
-		buf = dma_region_i(&iso->data_buf, unsigned char,
-			iso->infos[packet].offset + CIP_HEADER_SIZE);
-		count = (iso->infos[packet].len - CIP_HEADER_SIZE) /
-			MPEG2_TS_SOURCE_PACKET_SIZE;
-
-		/* ignore empty packet */
-		if (iso->infos[packet].len <= CIP_HEADER_SIZE)
-			continue;
-
-		while (count--) {
-			if (buf[MPEG2_TS_HEADER_SIZE] == 0x47)
-				dvb_dmx_swfilter_packets(&fdtv->demux,
-						&buf[MPEG2_TS_HEADER_SIZE], 1);
-			else
-				dev_err(fdtv->device,
-					"skipping invalid packet\n");
-			buf += MPEG2_TS_SOURCE_PACKET_SIZE;
-		}
-	}
-out:
-	hpsb_iso_recv_release_packets(iso, num);
-}
-
-static inline struct node_entry *node_of(struct firedtv *fdtv)
-{
-	return container_of(fdtv->device, struct unit_directory, device)->ne;
-}
-
-static int node_lock(struct firedtv *fdtv, u64 addr, void *data)
-{
-	quadlet_t *d = data;
-	int ret;
-
-	ret = hpsb_node_lock(node_of(fdtv), addr,
-			     EXTCODE_COMPARE_SWAP, &d[1], d[0]);
-	d[0] = d[1];
-
-	return ret;
-}
-
-static int node_read(struct firedtv *fdtv, u64 addr, void *data)
-{
-	return hpsb_node_read(node_of(fdtv), addr, data, 4);
-}
-
-static int node_write(struct firedtv *fdtv, u64 addr, void *data, size_t len)
-{
-	return hpsb_node_write(node_of(fdtv), addr, data, len);
-}
-
-#define FDTV_ISO_BUFFER_PACKETS 256
-#define FDTV_ISO_BUFFER_SIZE (FDTV_ISO_BUFFER_PACKETS * 200)
-
-static int start_iso(struct firedtv *fdtv)
-{
-	struct hpsb_iso *iso_handle;
-	int ret;
-
-	iso_handle = hpsb_iso_recv_init(node_of(fdtv)->host,
-				FDTV_ISO_BUFFER_SIZE, FDTV_ISO_BUFFER_PACKETS,
-				fdtv->isochannel, HPSB_ISO_DMA_DEFAULT,
-				-1, /* stat.config.irq_interval */
-				rawiso_activity_cb);
-	if (iso_handle == NULL) {
-		dev_err(fdtv->device, "cannot initialize iso receive\n");
-		return -ENOMEM;
-	}
-	fdtv->backend_data = iso_handle;
-
-	ret = hpsb_iso_recv_start(iso_handle, -1, -1, 0);
-	if (ret != 0) {
-		dev_err(fdtv->device, "cannot start iso receive\n");
-		hpsb_iso_shutdown(iso_handle);
-		fdtv->backend_data = NULL;
-	}
-	return ret;
-}
-
-static void stop_iso(struct firedtv *fdtv)
-{
-	struct hpsb_iso *iso_handle = fdtv->backend_data;
-
-	if (iso_handle != NULL) {
-		hpsb_iso_stop(iso_handle);
-		hpsb_iso_shutdown(iso_handle);
-	}
-	fdtv->backend_data = NULL;
-}
-
-static const struct firedtv_backend fdtv_1394_backend = {
-	.lock		= node_lock,
-	.read		= node_read,
-	.write		= node_write,
-	.start_iso	= start_iso,
-	.stop_iso	= stop_iso,
-};
-
-static void fcp_request(struct hpsb_host *host, int nodeid, int direction,
-			int cts, u8 *data, size_t length)
-{
-	struct firedtv *f, *fdtv = NULL;
-	unsigned long flags;
-	int su;
-
-	if (length == 0 || (data[0] & 0xf0) != 0)
-		return;
-
-	su = data[1] & 0x7;
-
-	spin_lock_irqsave(&node_list_lock, flags);
-	list_for_each_entry(f, &node_list, list)
-		if (node_of(f)->host == host &&
-		    node_of(f)->nodeid == nodeid &&
-		    (f->subunit == su || (f->subunit == 0 && su == 0x7))) {
-			fdtv = f;
-			break;
-		}
-	spin_unlock_irqrestore(&node_list_lock, flags);
-
-	if (fdtv)
-		avc_recv(fdtv, data, length);
-}
-
-static int node_probe(struct device *dev)
-{
-	struct unit_directory *ud =
-			container_of(dev, struct unit_directory, device);
-	struct firedtv *fdtv;
-	int kv_len, err;
-	void *kv_str;
-
-	if (ud->model_name_kv) {
-		kv_len = (ud->model_name_kv->value.leaf.len - 2) * 4;
-		kv_str = CSR1212_TEXTUAL_DESCRIPTOR_LEAF_DATA(ud->model_name_kv);
-	} else {
-		kv_len = 0;
-		kv_str = NULL;
-	}
-	fdtv = fdtv_alloc(dev, &fdtv_1394_backend, kv_str, kv_len);
-	if (!fdtv)
-		return -ENOMEM;
-
-	/*
-	 * Work around a bug in udev's path_id script:  Use the fw-host's dev
-	 * instead of the unit directory's dev as parent of the input device.
-	 */
-	err = fdtv_register_rc(fdtv, dev->parent->parent);
-	if (err)
-		goto fail_free;
-
-	spin_lock_irq(&node_list_lock);
-	list_add_tail(&fdtv->list, &node_list);
-	spin_unlock_irq(&node_list_lock);
-
-	err = avc_identify_subunit(fdtv);
-	if (err)
-		goto fail;
-
-	err = fdtv_dvb_register(fdtv);
-	if (err)
-		goto fail;
-
-	avc_register_remote_control(fdtv);
-
-	return 0;
-fail:
-	spin_lock_irq(&node_list_lock);
-	list_del(&fdtv->list);
-	spin_unlock_irq(&node_list_lock);
-	fdtv_unregister_rc(fdtv);
-fail_free:
-	kfree(fdtv);
-
-	return err;
-}
-
-static int node_remove(struct device *dev)
-{
-	struct firedtv *fdtv = dev_get_drvdata(dev);
-
-	fdtv_dvb_unregister(fdtv);
-
-	spin_lock_irq(&node_list_lock);
-	list_del(&fdtv->list);
-	spin_unlock_irq(&node_list_lock);
-
-	fdtv_unregister_rc(fdtv);
-	kfree(fdtv);
-
-	return 0;
-}
-
-static int node_update(struct unit_directory *ud)
-{
-	struct firedtv *fdtv = dev_get_drvdata(&ud->device);
-
-	if (fdtv->isochannel >= 0)
-		cmp_establish_pp_connection(fdtv, fdtv->subunit,
-					    fdtv->isochannel);
-	return 0;
-}
-
-static struct hpsb_protocol_driver fdtv_driver = {
-	.name		= "firedtv",
-	.id_table	= fdtv_id_table,
-	.update		= node_update,
-	.driver         = {
-		.probe  = node_probe,
-		.remove = node_remove,
-	},
-};
-
-static struct hpsb_highlevel fdtv_highlevel = {
-	.name		= "firedtv",
-	.fcp_request	= fcp_request,
-};
-
-int __init fdtv_1394_init(void)
-{
-	int ret;
-
-	hpsb_register_highlevel(&fdtv_highlevel);
-	ret = hpsb_register_protocol(&fdtv_driver);
-	if (ret) {
-		printk(KERN_ERR "firedtv: failed to register protocol\n");
-		hpsb_unregister_highlevel(&fdtv_highlevel);
-	}
-	return ret;
-}
-
-void __exit fdtv_1394_exit(void)
-{
-	hpsb_unregister_protocol(&fdtv_driver);
-	hpsb_unregister_highlevel(&fdtv_highlevel);
-}
diff --git a/drivers/media/dvb/firewire/firedtv-avc.c b/drivers/media/dvb/firewire/firedtv-avc.c
index f0f1842fab60..fc5ccd8c923a 100644
--- a/drivers/media/dvb/firewire/firedtv-avc.c
+++ b/drivers/media/dvb/firewire/firedtv-avc.c
@@ -241,8 +241,8 @@ static int avc_write(struct firedtv *fdtv)
 		if (unlikely(avc_debug))
 			debug_fcp(fdtv->avc_data, fdtv->avc_data_length);
 
-		err = fdtv->backend->write(fdtv, FCP_COMMAND_REGISTER,
-				fdtv->avc_data, fdtv->avc_data_length);
+		err = fdtv_write(fdtv, FCP_COMMAND_REGISTER,
+				 fdtv->avc_data, fdtv->avc_data_length);
 		if (err) {
 			dev_err(fdtv->device, "FCP command write failed\n");
 
@@ -1322,7 +1322,7 @@ static int cmp_read(struct firedtv *fdtv, u64 addr, __be32 *data)
 
 	mutex_lock(&fdtv->avc_mutex);
 
-	ret = fdtv->backend->read(fdtv, addr, data);
+	ret = fdtv_read(fdtv, addr, data);
 	if (ret < 0)
 		dev_err(fdtv->device, "CMP: read I/O error\n");
 
@@ -1340,7 +1340,7 @@ static int cmp_lock(struct firedtv *fdtv, u64 addr, __be32 data[])
 	/* data[] is stack-allocated and should not be DMA-mapped. */
 	memcpy(fdtv->avc_data, data, 8);
 
-	ret = fdtv->backend->lock(fdtv, addr, fdtv->avc_data);
+	ret = fdtv_lock(fdtv, addr, fdtv->avc_data);
 	if (ret < 0)
 		dev_err(fdtv->device, "CMP: lock I/O error\n");
 	else
@@ -1405,10 +1405,7 @@ repeat:
 		/* FIXME: this is for the worst case - optimize */
 		set_opcr_overhead_id(opcr, 0);
 
-		/*
-		 * FIXME: allocate isochronous channel and bandwidth at IRM
-		 * fdtv->backend->alloc_resources(fdtv, channels_mask, bw);
-		 */
+		/* FIXME: allocate isochronous channel and bandwidth at IRM */
 	}
 
 	set_opcr_p2p_connections(opcr, get_opcr_p2p_connections(*opcr) + 1);
@@ -1424,8 +1421,6 @@ repeat:
 		/*
 		 * FIXME: if old_opcr.P2P_Connections > 0,
 		 * deallocate isochronous channel and bandwidth at IRM
-		 * if (...)
-		 *	fdtv->backend->dealloc_resources(fdtv, channel, bw);
 		 */
 
 		if (++attempts < 6) /* arbitrary limit */
diff --git a/drivers/media/dvb/firewire/firedtv-dvb.c b/drivers/media/dvb/firewire/firedtv-dvb.c
index 079e8c5b0475..fd8bbbfa5c59 100644
--- a/drivers/media/dvb/firewire/firedtv-dvb.c
+++ b/drivers/media/dvb/firewire/firedtv-dvb.c
@@ -14,14 +14,9 @@
 #include <linux/device.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
-#include <linux/mod_devicetable.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
-#include <linux/slab.h>
-#include <linux/string.h>
 #include <linux/types.h>
-#include <linux/wait.h>
-#include <linux/workqueue.h>
 
 #include <dmxdev.h>
 #include <dvb_demux.h>
@@ -166,11 +161,11 @@ int fdtv_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
-int fdtv_dvb_register(struct firedtv *fdtv)
+int fdtv_dvb_register(struct firedtv *fdtv, const char *name)
 {
 	int err;
 
-	err = dvb_register_adapter(&fdtv->adapter, fdtv_model_names[fdtv->type],
+	err = dvb_register_adapter(&fdtv->adapter, name,
 				   THIS_MODULE, fdtv->device, adapter_nr);
 	if (err < 0)
 		goto fail_log;
@@ -210,7 +205,7 @@ int fdtv_dvb_register(struct firedtv *fdtv)
 
 	dvb_net_init(&fdtv->adapter, &fdtv->dvbnet, &fdtv->demux.dmx);
 
-	fdtv_frontend_init(fdtv);
+	fdtv_frontend_init(fdtv, name);
 	err = dvb_register_frontend(&fdtv->adapter, &fdtv->fe);
 	if (err)
 		goto fail_net_release;
@@ -248,127 +243,3 @@ void fdtv_dvb_unregister(struct firedtv *fdtv)
 	dvb_dmx_release(&fdtv->demux);
 	dvb_unregister_adapter(&fdtv->adapter);
 }
-
-const char *fdtv_model_names[] = {
-	[FIREDTV_UNKNOWN] = "unknown type",
-	[FIREDTV_DVB_S]   = "FireDTV S/CI",
-	[FIREDTV_DVB_C]   = "FireDTV C/CI",
-	[FIREDTV_DVB_T]   = "FireDTV T/CI",
-	[FIREDTV_DVB_S2]  = "FireDTV S2  ",
-};
-
-struct firedtv *fdtv_alloc(struct device *dev,
-			   const struct firedtv_backend *backend,
-			   const char *name, size_t name_len)
-{
-	struct firedtv *fdtv;
-	int i;
-
-	fdtv = kzalloc(sizeof(*fdtv), GFP_KERNEL);
-	if (!fdtv)
-		return NULL;
-
-	dev_set_drvdata(dev, fdtv);
-	fdtv->device		= dev;
-	fdtv->isochannel	= -1;
-	fdtv->voltage		= 0xff;
-	fdtv->tone		= 0xff;
-	fdtv->backend		= backend;
-
-	mutex_init(&fdtv->avc_mutex);
-	init_waitqueue_head(&fdtv->avc_wait);
-	mutex_init(&fdtv->demux_mutex);
-	INIT_WORK(&fdtv->remote_ctrl_work, avc_remote_ctrl_work);
-
-	for (i = ARRAY_SIZE(fdtv_model_names); --i; )
-		if (strlen(fdtv_model_names[i]) <= name_len &&
-		    strncmp(name, fdtv_model_names[i], name_len) == 0)
-			break;
-	fdtv->type = i;
-
-	return fdtv;
-}
-
-#define MATCH_FLAGS (IEEE1394_MATCH_VENDOR_ID | IEEE1394_MATCH_MODEL_ID | \
-		     IEEE1394_MATCH_SPECIFIER_ID | IEEE1394_MATCH_VERSION)
-
-#define DIGITAL_EVERYWHERE_OUI	0x001287
-#define AVC_UNIT_SPEC_ID_ENTRY	0x00a02d
-#define AVC_SW_VERSION_ENTRY	0x010001
-
-const struct ieee1394_device_id fdtv_id_table[] = {
-	{
-		/* FloppyDTV S/CI and FloppyDTV S2 */
-		.match_flags	= MATCH_FLAGS,
-		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
-		.model_id	= 0x000024,
-		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
-		.version	= AVC_SW_VERSION_ENTRY,
-	}, {
-		/* FloppyDTV T/CI */
-		.match_flags	= MATCH_FLAGS,
-		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
-		.model_id	= 0x000025,
-		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
-		.version	= AVC_SW_VERSION_ENTRY,
-	}, {
-		/* FloppyDTV C/CI */
-		.match_flags	= MATCH_FLAGS,
-		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
-		.model_id	= 0x000026,
-		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
-		.version	= AVC_SW_VERSION_ENTRY,
-	}, {
-		/* FireDTV S/CI and FloppyDTV S2 */
-		.match_flags	= MATCH_FLAGS,
-		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
-		.model_id	= 0x000034,
-		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
-		.version	= AVC_SW_VERSION_ENTRY,
-	}, {
-		/* FireDTV T/CI */
-		.match_flags	= MATCH_FLAGS,
-		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
-		.model_id	= 0x000035,
-		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
-		.version	= AVC_SW_VERSION_ENTRY,
-	}, {
-		/* FireDTV C/CI */
-		.match_flags	= MATCH_FLAGS,
-		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
-		.model_id	= 0x000036,
-		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
-		.version	= AVC_SW_VERSION_ENTRY,
-	}, {}
-};
-MODULE_DEVICE_TABLE(ieee1394, fdtv_id_table);
-
-static int __init fdtv_init(void)
-{
-	int ret;
-
-	ret = fdtv_fw_init();
-	if (ret < 0)
-		return ret;
-
-	ret = fdtv_1394_init();
-	if (ret < 0)
-		fdtv_fw_exit();
-
-	return ret;
-}
-
-static void __exit fdtv_exit(void)
-{
-	fdtv_1394_exit();
-	fdtv_fw_exit();
-}
-
-module_init(fdtv_init);
-module_exit(fdtv_exit);
-
-MODULE_AUTHOR("Andreas Monitzer <andy@monitzer.com>");
-MODULE_AUTHOR("Ben Backx <ben@bbackx.com>");
-MODULE_DESCRIPTION("FireDTV DVB Driver");
-MODULE_LICENSE("GPL");
-MODULE_SUPPORTED_DEVICE("FireDTV DVB");
diff --git a/drivers/media/dvb/firewire/firedtv-fe.c b/drivers/media/dvb/firewire/firedtv-fe.c
index d10920e2f3a2..8748a61be73d 100644
--- a/drivers/media/dvb/firewire/firedtv-fe.c
+++ b/drivers/media/dvb/firewire/firedtv-fe.c
@@ -36,14 +36,14 @@ static int fdtv_dvb_init(struct dvb_frontend *fe)
 		return err;
 	}
 
-	return fdtv->backend->start_iso(fdtv);
+	return fdtv_start_iso(fdtv);
 }
 
 static int fdtv_sleep(struct dvb_frontend *fe)
 {
 	struct firedtv *fdtv = fe->sec_priv;
 
-	fdtv->backend->stop_iso(fdtv);
+	fdtv_stop_iso(fdtv);
 	cmp_break_pp_connection(fdtv, fdtv->subunit, fdtv->isochannel);
 	fdtv->isochannel = -1;
 	return 0;
@@ -165,7 +165,7 @@ static int fdtv_set_property(struct dvb_frontend *fe, struct dtv_property *tvp)
 	return 0;
 }
 
-void fdtv_frontend_init(struct firedtv *fdtv)
+void fdtv_frontend_init(struct firedtv *fdtv, const char *name)
 {
 	struct dvb_frontend_ops *ops = &fdtv->fe.ops;
 	struct dvb_frontend_info *fi = &ops->info;
@@ -266,7 +266,7 @@ void fdtv_frontend_init(struct firedtv *fdtv)
 		dev_err(fdtv->device, "no frontend for model type %d\n",
 			fdtv->type);
 	}
-	strcpy(fi->name, fdtv_model_names[fdtv->type]);
+	strcpy(fi->name, name);
 
 	fdtv->fe.dvb = &fdtv->adapter;
 	fdtv->fe.sec_priv = fdtv;
diff --git a/drivers/media/dvb/firewire/firedtv-fw.c b/drivers/media/dvb/firewire/firedtv-fw.c
index 7424b0493f9d..8022b743af91 100644
--- a/drivers/media/dvb/firewire/firedtv-fw.c
+++ b/drivers/media/dvb/firewire/firedtv-fw.c
@@ -9,11 +9,18 @@
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/mm.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
+#include <linux/string.h>
 #include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
 
 #include <asm/page.h>
+#include <asm/system.h>
 
 #include <dvb_demux.h>
 
@@ -41,17 +48,17 @@ static int node_req(struct firedtv *fdtv, u64 addr, void *data, size_t len,
 	return rcode != RCODE_COMPLETE ? -EIO : 0;
 }
 
-static int node_lock(struct firedtv *fdtv, u64 addr, void *data)
+int fdtv_lock(struct firedtv *fdtv, u64 addr, void *data)
 {
 	return node_req(fdtv, addr, data, 8, TCODE_LOCK_COMPARE_SWAP);
 }
 
-static int node_read(struct firedtv *fdtv, u64 addr, void *data)
+int fdtv_read(struct firedtv *fdtv, u64 addr, void *data)
 {
 	return node_req(fdtv, addr, data, 4, TCODE_READ_QUADLET_REQUEST);
 }
 
-static int node_write(struct firedtv *fdtv, u64 addr, void *data, size_t len)
+int fdtv_write(struct firedtv *fdtv, u64 addr, void *data, size_t len)
 {
 	return node_req(fdtv, addr, data, len, TCODE_WRITE_BLOCK_REQUEST);
 }
@@ -67,7 +74,7 @@ static int node_write(struct firedtv *fdtv, u64 addr, void *data, size_t len)
 #define N_PAGES			DIV_ROUND_UP(N_PACKETS, PACKETS_PER_PAGE)
 #define IRQ_INTERVAL		16
 
-struct firedtv_receive_context {
+struct fdtv_ir_context {
 	struct fw_iso_context *context;
 	struct fw_iso_buffer buffer;
 	int interrupt_packet;
@@ -75,7 +82,7 @@ struct firedtv_receive_context {
 	char *pages[N_PAGES];
 };
 
-static int queue_iso(struct firedtv_receive_context *ctx, int index)
+static int queue_iso(struct fdtv_ir_context *ctx, int index)
 {
 	struct fw_iso_packet p;
 
@@ -92,7 +99,7 @@ static void handle_iso(struct fw_iso_context *context, u32 cycle,
 		       size_t header_length, void *header, void *data)
 {
 	struct firedtv *fdtv = data;
-	struct firedtv_receive_context *ctx = fdtv->backend_data;
+	struct fdtv_ir_context *ctx = fdtv->ir_context;
 	__be32 *h, *h_end;
 	int length, err, i = ctx->current_packet;
 	char *p, *p_end;
@@ -121,9 +128,9 @@ static void handle_iso(struct fw_iso_context *context, u32 cycle,
 	ctx->current_packet = i;
 }
 
-static int start_iso(struct firedtv *fdtv)
+int fdtv_start_iso(struct firedtv *fdtv)
 {
-	struct firedtv_receive_context *ctx;
+	struct fdtv_ir_context *ctx;
 	struct fw_device *device = device_of(fdtv);
 	int i, err;
 
@@ -161,7 +168,7 @@ static int start_iso(struct firedtv *fdtv)
 	if (err)
 		goto fail;
 
-	fdtv->backend_data = ctx;
+	fdtv->ir_context = ctx;
 
 	return 0;
 fail:
@@ -174,9 +181,9 @@ fail_free:
 	return err;
 }
 
-static void stop_iso(struct firedtv *fdtv)
+void fdtv_stop_iso(struct firedtv *fdtv)
 {
-	struct firedtv_receive_context *ctx = fdtv->backend_data;
+	struct fdtv_ir_context *ctx = fdtv->ir_context;
 
 	fw_iso_context_stop(ctx->context);
 	fw_iso_buffer_destroy(&ctx->buffer, device_of(fdtv)->card);
@@ -184,14 +191,6 @@ static void stop_iso(struct firedtv *fdtv)
 	kfree(ctx);
 }
 
-static const struct firedtv_backend backend = {
-	.lock		= node_lock,
-	.read		= node_read,
-	.write		= node_write,
-	.start_iso	= start_iso,
-	.stop_iso	= stop_iso,
-};
-
 static void handle_fcp(struct fw_card *card, struct fw_request *request,
 		       int tcode, int destination, int source, int generation,
 		       unsigned long long offset, void *payload, size_t length,
@@ -238,6 +237,14 @@ static const struct fw_address_region fcp_region = {
 	.end	= CSR_REGISTER_BASE + CSR_FCP_END,
 };
 
+static const char * const model_names[] = {
+	[FIREDTV_UNKNOWN] = "unknown type",
+	[FIREDTV_DVB_S]   = "FireDTV S/CI",
+	[FIREDTV_DVB_C]   = "FireDTV C/CI",
+	[FIREDTV_DVB_T]   = "FireDTV T/CI",
+	[FIREDTV_DVB_S2]  = "FireDTV S2  ",
+};
+
 /* Adjust the template string if models with longer names appear. */
 #define MAX_MODEL_NAME_LEN sizeof("FireDTV ????")
 
@@ -245,15 +252,31 @@ static int node_probe(struct device *dev)
 {
 	struct firedtv *fdtv;
 	char name[MAX_MODEL_NAME_LEN];
-	int name_len, err;
-
-	name_len = fw_csr_string(fw_unit(dev)->directory, CSR_MODEL,
-				 name, sizeof(name));
+	int name_len, i, err;
 
-	fdtv = fdtv_alloc(dev, &backend, name, name_len >= 0 ? name_len : 0);
+	fdtv = kzalloc(sizeof(*fdtv), GFP_KERNEL);
 	if (!fdtv)
 		return -ENOMEM;
 
+	dev_set_drvdata(dev, fdtv);
+	fdtv->device		= dev;
+	fdtv->isochannel	= -1;
+	fdtv->voltage		= 0xff;
+	fdtv->tone		= 0xff;
+
+	mutex_init(&fdtv->avc_mutex);
+	init_waitqueue_head(&fdtv->avc_wait);
+	mutex_init(&fdtv->demux_mutex);
+	INIT_WORK(&fdtv->remote_ctrl_work, avc_remote_ctrl_work);
+
+	name_len = fw_csr_string(fw_unit(dev)->directory, CSR_MODEL,
+				 name, sizeof(name));
+	for (i = ARRAY_SIZE(model_names); --i; )
+		if (strlen(model_names[i]) <= name_len &&
+		    strncmp(name, model_names[i], name_len) == 0)
+			break;
+	fdtv->type = i;
+
 	err = fdtv_register_rc(fdtv, dev);
 	if (err)
 		goto fail_free;
@@ -266,7 +289,7 @@ static int node_probe(struct device *dev)
 	if (err)
 		goto fail;
 
-	err = fdtv_dvb_register(fdtv);
+	err = fdtv_dvb_register(fdtv, model_names[fdtv->type]);
 	if (err)
 		goto fail;
 
@@ -309,6 +332,60 @@ static void node_update(struct fw_unit *unit)
 					    fdtv->isochannel);
 }
 
+#define MATCH_FLAGS (IEEE1394_MATCH_VENDOR_ID | IEEE1394_MATCH_MODEL_ID | \
+		     IEEE1394_MATCH_SPECIFIER_ID | IEEE1394_MATCH_VERSION)
+
+#define DIGITAL_EVERYWHERE_OUI	0x001287
+#define AVC_UNIT_SPEC_ID_ENTRY	0x00a02d
+#define AVC_SW_VERSION_ENTRY	0x010001
+
+static const struct ieee1394_device_id fdtv_id_table[] = {
+	{
+		/* FloppyDTV S/CI and FloppyDTV S2 */
+		.match_flags	= MATCH_FLAGS,
+		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
+		.model_id	= 0x000024,
+		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
+		.version	= AVC_SW_VERSION_ENTRY,
+	}, {
+		/* FloppyDTV T/CI */
+		.match_flags	= MATCH_FLAGS,
+		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
+		.model_id	= 0x000025,
+		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
+		.version	= AVC_SW_VERSION_ENTRY,
+	}, {
+		/* FloppyDTV C/CI */
+		.match_flags	= MATCH_FLAGS,
+		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
+		.model_id	= 0x000026,
+		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
+		.version	= AVC_SW_VERSION_ENTRY,
+	}, {
+		/* FireDTV S/CI and FloppyDTV S2 */
+		.match_flags	= MATCH_FLAGS,
+		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
+		.model_id	= 0x000034,
+		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
+		.version	= AVC_SW_VERSION_ENTRY,
+	}, {
+		/* FireDTV T/CI */
+		.match_flags	= MATCH_FLAGS,
+		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
+		.model_id	= 0x000035,
+		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
+		.version	= AVC_SW_VERSION_ENTRY,
+	}, {
+		/* FireDTV C/CI */
+		.match_flags	= MATCH_FLAGS,
+		.vendor_id	= DIGITAL_EVERYWHERE_OUI,
+		.model_id	= 0x000036,
+		.specifier_id	= AVC_UNIT_SPEC_ID_ENTRY,
+		.version	= AVC_SW_VERSION_ENTRY,
+	}, {}
+};
+MODULE_DEVICE_TABLE(ieee1394, fdtv_id_table);
+
 static struct fw_driver fdtv_driver = {
 	.driver   = {
 		.owner  = THIS_MODULE,
@@ -321,7 +398,7 @@ static struct fw_driver fdtv_driver = {
 	.id_table = fdtv_id_table,
 };
 
-int __init fdtv_fw_init(void)
+static int __init fdtv_init(void)
 {
 	int ret;
 
@@ -329,11 +406,24 @@ int __init fdtv_fw_init(void)
 	if (ret < 0)
 		return ret;
 
-	return driver_register(&fdtv_driver.driver);
+	ret = driver_register(&fdtv_driver.driver);
+	if (ret < 0)
+		fw_core_remove_address_handler(&fcp_handler);
+
+	return ret;
 }
 
-void fdtv_fw_exit(void)
+static void __exit fdtv_exit(void)
 {
 	driver_unregister(&fdtv_driver.driver);
 	fw_core_remove_address_handler(&fcp_handler);
 }
+
+module_init(fdtv_init);
+module_exit(fdtv_exit);
+
+MODULE_AUTHOR("Andreas Monitzer <andy@monitzer.com>");
+MODULE_AUTHOR("Ben Backx <ben@bbackx.com>");
+MODULE_DESCRIPTION("FireDTV DVB Driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("FireDTV DVB");
diff --git a/drivers/media/dvb/firewire/firedtv.h b/drivers/media/dvb/firewire/firedtv.h
index 78cc28f36914..bd00b04e079d 100644
--- a/drivers/media/dvb/firewire/firedtv.h
+++ b/drivers/media/dvb/firewire/firedtv.h
@@ -70,15 +70,7 @@ enum model_type {
 
 struct device;
 struct input_dev;
-struct firedtv;
-
-struct firedtv_backend {
-	int (*lock)(struct firedtv *fdtv, u64 addr, void *data);
-	int (*read)(struct firedtv *fdtv, u64 addr, void *data);
-	int (*write)(struct firedtv *fdtv, u64 addr, void *data, size_t len);
-	int (*start_iso)(struct firedtv *fdtv);
-	void (*stop_iso)(struct firedtv *fdtv);
-};
+struct fdtv_ir_context;
 
 struct firedtv {
 	struct device *device;
@@ -104,12 +96,11 @@ struct firedtv {
 	enum model_type		type;
 	char			subunit;
 	char			isochannel;
+	struct fdtv_ir_context	*ir_context;
+
 	fe_sec_voltage_t	voltage;
 	fe_sec_tone_mode_t	tone;
 
-	const struct firedtv_backend *backend;
-	void			*backend_data;
-
 	struct mutex		demux_mutex;
 	unsigned long		channel_active;
 	u16			channel_pid[16];
@@ -118,15 +109,6 @@ struct firedtv {
 	u8			avc_data[512];
 };
 
-/* firedtv-1394.c */
-#ifdef CONFIG_DVB_FIREDTV_IEEE1394
-int fdtv_1394_init(void);
-void fdtv_1394_exit(void);
-#else
-static inline int fdtv_1394_init(void) { return 0; }
-static inline void fdtv_1394_exit(void) {}
-#endif
-
 /* firedtv-avc.c */
 int avc_recv(struct firedtv *fdtv, void *data, size_t length);
 int avc_tuner_status(struct firedtv *fdtv, struct firedtv_tuner_status *stat);
@@ -158,25 +140,18 @@ void fdtv_ca_release(struct firedtv *fdtv);
 /* firedtv-dvb.c */
 int fdtv_start_feed(struct dvb_demux_feed *dvbdmxfeed);
 int fdtv_stop_feed(struct dvb_demux_feed *dvbdmxfeed);
-int fdtv_dvb_register(struct firedtv *fdtv);
+int fdtv_dvb_register(struct firedtv *fdtv, const char *name);
 void fdtv_dvb_unregister(struct firedtv *fdtv);
-struct firedtv *fdtv_alloc(struct device *dev,
-			   const struct firedtv_backend *backend,
-			   const char *name, size_t name_len);
-extern const char *fdtv_model_names[];
-extern const struct ieee1394_device_id fdtv_id_table[];
 
 /* firedtv-fe.c */
-void fdtv_frontend_init(struct firedtv *fdtv);
+void fdtv_frontend_init(struct firedtv *fdtv, const char *name);
 
 /* firedtv-fw.c */
-#ifdef CONFIG_DVB_FIREDTV_FIREWIRE
-int fdtv_fw_init(void);
-void fdtv_fw_exit(void);
-#else
-static inline int fdtv_fw_init(void) { return 0; }
-static inline void fdtv_fw_exit(void) {}
-#endif
+int fdtv_lock(struct firedtv *fdtv, u64 addr, void *data);
+int fdtv_read(struct firedtv *fdtv, u64 addr, void *data);
+int fdtv_write(struct firedtv *fdtv, u64 addr, void *data, size_t len);
+int fdtv_start_iso(struct firedtv *fdtv);
+void fdtv_stop_iso(struct firedtv *fdtv);
 
 /* firedtv-rc.c */
 #ifdef CONFIG_DVB_FIREDTV_INPUT
diff --git a/drivers/media/dvb/frontends/Kconfig b/drivers/media/dvb/frontends/Kconfig
index b8519ba511e5..83093d1f4f74 100644
--- a/drivers/media/dvb/frontends/Kconfig
+++ b/drivers/media/dvb/frontends/Kconfig
@@ -349,6 +349,14 @@ config DVB_DIB7000P
 	  A DVB-T tuner module. Designed for mobile usage. Say Y when you want
 	  to support this frontend.
 
+config DVB_DIB9000
+	tristate "DiBcom 9000"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+	help
+	  A DVB-T tuner module. Designed for mobile usage. Say Y when you want
+	  to support this frontend.
+
 config DVB_TDA10048
 	tristate "Philips TDA10048HN based"
 	depends on DVB_CORE && I2C
@@ -370,6 +378,13 @@ config DVB_EC100
 	help
 	  Say Y when you want to support this frontend.
 
+config DVB_STV0367
+	tristate "ST STV0367 based"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+	help
+	  A DVB-T/C tuner module. Say Y when you want to support this frontend.
+
 comment "DVB-C (cable) frontends"
 	depends on DVB_CORE
 
diff --git a/drivers/media/dvb/frontends/Makefile b/drivers/media/dvb/frontends/Makefile
index b1d9525aa7e3..3b0c4bdc4b2b 100644
--- a/drivers/media/dvb/frontends/Makefile
+++ b/drivers/media/dvb/frontends/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_DVB_DIB3000MC) += dib3000mc.o dibx000_common.o
 obj-$(CONFIG_DVB_DIB7000M) += dib7000m.o dibx000_common.o
 obj-$(CONFIG_DVB_DIB7000P) += dib7000p.o dibx000_common.o
 obj-$(CONFIG_DVB_DIB8000) += dib8000.o dibx000_common.o
+obj-$(CONFIG_DVB_DIB9000) += dib9000.o dibx000_common.o
 obj-$(CONFIG_DVB_MT312) += mt312.o
 obj-$(CONFIG_DVB_VES1820) += ves1820.o
 obj-$(CONFIG_DVB_VES1X93) += ves1x93.o
@@ -83,3 +84,4 @@ obj-$(CONFIG_DVB_DS3000) += ds3000.o
 obj-$(CONFIG_DVB_MB86A16) += mb86a16.o
 obj-$(CONFIG_DVB_MB86A20S) += mb86a20s.o
 obj-$(CONFIG_DVB_IX2505V) += ix2505v.o
+obj-$(CONFIG_DVB_STV0367) += stv0367.o
diff --git a/drivers/media/dvb/frontends/dib0090.c b/drivers/media/dvb/frontends/dib0090.c
index 65240b7801e8..52ff1a252a90 100644
--- a/drivers/media/dvb/frontends/dib0090.c
+++ b/drivers/media/dvb/frontends/dib0090.c
@@ -45,6 +45,7 @@ MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
 	} \
 } while (0)
 
+#define CONFIG_SYS_DVBT
 #define CONFIG_SYS_ISDBT
 #define CONFIG_BAND_CBAND
 #define CONFIG_BAND_VHF
@@ -76,6 +77,34 @@ MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
 #define EN_SBD		 0x44E9
 #define EN_CAB		 0x88E9
 
+/* Calibration defines */
+#define      DC_CAL 0x1
+#define     WBD_CAL 0x2
+#define    TEMP_CAL 0x4
+#define CAPTRIM_CAL 0x8
+
+#define KROSUS_PLL_LOCKED   0x800
+#define KROSUS              0x2
+
+/* Use those defines to identify SOC version */
+#define SOC               0x02
+#define SOC_7090_P1G_11R1 0x82
+#define SOC_7090_P1G_21R1 0x8a
+#define SOC_8090_P1G_11R1 0x86
+#define SOC_8090_P1G_21R1 0x8e
+
+/* else use thos ones to check */
+#define P1A_B      0x0
+#define P1C	   0x1
+#define P1D_E_F    0x3
+#define P1G	   0x7
+#define P1G_21R2   0xf
+
+#define MP001 0x1		/* Single 9090/8096 */
+#define MP005 0x4		/* Single Sband */
+#define MP008 0x6		/* Dual diversity VHF-UHF-LBAND */
+#define MP009 0x7		/* Dual diversity 29098 CBAND-UHF-LBAND-SBAND */
+
 #define pgm_read_word(w) (*w)
 
 struct dc_calibration;
@@ -84,7 +113,7 @@ struct dib0090_tuning {
 	u32 max_freq;		/* for every frequency less than or equal to that field: this information is correct */
 	u8 switch_trim;
 	u8 lna_tune;
-	u8 lna_bias;
+	u16 lna_bias;
 	u16 v2i;
 	u16 mix;
 	u16 load;
@@ -99,13 +128,19 @@ struct dib0090_pll {
 	u8 topresc;
 };
 
+struct dib0090_identity {
+	u8 version;
+	u8 product;
+	u8 p1g;
+	u8 in_soc;
+};
+
 struct dib0090_state {
 	struct i2c_adapter *i2c;
 	struct dvb_frontend *fe;
 	const struct dib0090_config *config;
 
 	u8 current_band;
-	u16 revision;
 	enum frontend_tune_state tune_state;
 	u32 current_rf;
 
@@ -143,7 +178,26 @@ struct dib0090_state {
 	u8 tuner_is_tuned;
 	u8 agc_freeze;
 
-	u8 reset;
+	struct dib0090_identity identity;
+
+	u32 rf_request;
+	u8 current_standard;
+
+	u8 calibrate;
+	u32 rest;
+	u16 bias;
+	s16 temperature;
+
+	u8 wbd_calibration_gain;
+	const struct dib0090_wbd_slope *current_wbd_table;
+	u16 wbdmux;
+};
+
+struct dib0090_fw_state {
+	struct i2c_adapter *i2c;
+	struct dvb_frontend *fe;
+	struct dib0090_identity identity;
+	const struct dib0090_config *config;
 };
 
 static u16 dib0090_read_reg(struct dib0090_state *state, u8 reg)
@@ -171,6 +225,28 @@ static int dib0090_write_reg(struct dib0090_state *state, u32 reg, u16 val)
 	return 0;
 }
 
+static u16 dib0090_fw_read_reg(struct dib0090_fw_state *state, u8 reg)
+{
+	u8 b[2];
+	struct i2c_msg msg = {.addr = reg, .flags = I2C_M_RD, .buf = b, .len = 2 };
+	if (i2c_transfer(state->i2c, &msg, 1) != 1) {
+		printk(KERN_WARNING "DiB0090 I2C read failed\n");
+		return 0;
+	}
+	return (b[0] << 8) | b[1];
+}
+
+static int dib0090_fw_write_reg(struct dib0090_fw_state *state, u8 reg, u16 val)
+{
+	u8 b[2] = { val >> 8, val & 0xff };
+	struct i2c_msg msg = {.addr = reg, .flags = 0, .buf = b, .len = 2 };
+	if (i2c_transfer(state->i2c, &msg, 1) != 1) {
+		printk(KERN_WARNING "DiB0090 I2C write failed\n");
+		return -EREMOTEIO;
+	}
+	return 0;
+}
+
 #define HARD_RESET(state) do {  if (cfg->reset) {  if (cfg->sleep) cfg->sleep(fe, 0); msleep(10);  cfg->reset(fe, 1); msleep(10);  cfg->reset(fe, 0); msleep(10);  }  } while (0)
 #define ADC_TARGET -220
 #define GAIN_ALPHA 5
@@ -183,89 +259,327 @@ static void dib0090_write_regs(struct dib0090_state *state, u8 r, const u16 * b,
 	} while (--c);
 }
 
-static u16 dib0090_identify(struct dvb_frontend *fe)
+static int dib0090_identify(struct dvb_frontend *fe)
 {
 	struct dib0090_state *state = fe->tuner_priv;
 	u16 v;
+	struct dib0090_identity *identity = &state->identity;
 
 	v = dib0090_read_reg(state, 0x1a);
 
-#ifdef FIRMWARE_FIREFLY
-	/* pll is not locked locked */
-	if (!(v & 0x800))
-		dprintk("FE%d : Identification : pll is not yet locked", fe->id);
-#endif
+	identity->p1g = 0;
+	identity->in_soc = 0;
+
+	dprintk("Tuner identification (Version = 0x%04x)", v);
 
 	/* without PLL lock info */
-	v &= 0x3ff;
-	dprintk("P/V: %04x:", v);
+	v &= ~KROSUS_PLL_LOCKED;
 
-	if ((v >> 8) & 0xf)
-		dprintk("FE%d : Product ID = 0x%x : KROSUS", fe->id, (v >> 8) & 0xf);
-	else
-		return 0xff;
-
-	v &= 0xff;
-	if (((v >> 5) & 0x7) == 0x1)
-		dprintk("FE%d : MP001 : 9090/8096", fe->id);
-	else if (((v >> 5) & 0x7) == 0x4)
-		dprintk("FE%d : MP005 : Single Sband", fe->id);
-	else if (((v >> 5) & 0x7) == 0x6)
-		dprintk("FE%d : MP008 : diversity VHF-UHF-LBAND", fe->id);
-	else if (((v >> 5) & 0x7) == 0x7)
-		dprintk("FE%d : MP009 : diversity 29098 CBAND-UHF-LBAND-SBAND", fe->id);
-	else
-		return 0xff;
-
-	/* revision only */
-	if ((v & 0x1f) == 0x3)
-		dprintk("FE%d : P1-D/E/F detected", fe->id);
-	else if ((v & 0x1f) == 0x1)
-		dprintk("FE%d : P1C detected", fe->id);
-	else if ((v & 0x1f) == 0x0) {
-#ifdef CONFIG_TUNER_DIB0090_P1B_SUPPORT
-		dprintk("FE%d : P1-A/B detected: using previous driver - support will be removed soon", fe->id);
-		dib0090_p1b_register(fe);
-#else
-		dprintk("FE%d : P1-A/B detected: driver is deactivated - not available", fe->id);
-		return 0xff;
-#endif
+	identity->version = v & 0xff;
+	identity->product = (v >> 8) & 0xf;
+
+	if (identity->product != KROSUS)
+		goto identification_error;
+
+	if ((identity->version & 0x3) == SOC) {
+		identity->in_soc = 1;
+		switch (identity->version) {
+		case SOC_8090_P1G_11R1:
+			dprintk("SOC 8090 P1-G11R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		case SOC_8090_P1G_21R1:
+			dprintk("SOC 8090 P1-G21R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		case SOC_7090_P1G_11R1:
+			dprintk("SOC 7090 P1-G11R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		case SOC_7090_P1G_21R1:
+			dprintk("SOC 7090 P1-G21R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		default:
+			goto identification_error;
+		}
+	} else {
+		switch ((identity->version >> 5) & 0x7) {
+		case MP001:
+			dprintk("MP001 : 9090/8096");
+			break;
+		case MP005:
+			dprintk("MP005 : Single Sband");
+			break;
+		case MP008:
+			dprintk("MP008 : diversity VHF-UHF-LBAND");
+			break;
+		case MP009:
+			dprintk("MP009 : diversity 29098 CBAND-UHF-LBAND-SBAND");
+			break;
+		default:
+			goto identification_error;
+		}
+
+		switch (identity->version & 0x1f) {
+		case P1G_21R2:
+			dprintk("P1G_21R2 detected");
+			identity->p1g = 1;
+			break;
+		case P1G:
+			dprintk("P1G detected");
+			identity->p1g = 1;
+			break;
+		case P1D_E_F:
+			dprintk("P1D/E/F detected");
+			break;
+		case P1C:
+			dprintk("P1C detected");
+			break;
+		case P1A_B:
+			dprintk("P1-A/B detected: driver is deactivated - not available");
+			goto identification_error;
+			break;
+		default:
+			goto identification_error;
+		}
 	}
 
-	return v;
+	return 0;
+
+identification_error:
+	return -EIO;
+}
+
+static int dib0090_fw_identify(struct dvb_frontend *fe)
+{
+	struct dib0090_fw_state *state = fe->tuner_priv;
+	struct dib0090_identity *identity = &state->identity;
+
+	u16 v = dib0090_fw_read_reg(state, 0x1a);
+	identity->p1g = 0;
+	identity->in_soc = 0;
+
+	dprintk("FE: Tuner identification (Version = 0x%04x)", v);
+
+	/* without PLL lock info */
+	v &= ~KROSUS_PLL_LOCKED;
+
+	identity->version = v & 0xff;
+	identity->product = (v >> 8) & 0xf;
+
+	if (identity->product != KROSUS)
+		goto identification_error;
+
+	if ((identity->version & 0x3) == SOC) {
+		identity->in_soc = 1;
+		switch (identity->version) {
+		case SOC_8090_P1G_11R1:
+			dprintk("SOC 8090 P1-G11R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		case SOC_8090_P1G_21R1:
+			dprintk("SOC 8090 P1-G21R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		case SOC_7090_P1G_11R1:
+			dprintk("SOC 7090 P1-G11R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		case SOC_7090_P1G_21R1:
+			dprintk("SOC 7090 P1-G21R1 Has been detected");
+			identity->p1g = 1;
+			break;
+		default:
+			goto identification_error;
+		}
+	} else {
+		switch ((identity->version >> 5) & 0x7) {
+		case MP001:
+			dprintk("MP001 : 9090/8096");
+			break;
+		case MP005:
+			dprintk("MP005 : Single Sband");
+			break;
+		case MP008:
+			dprintk("MP008 : diversity VHF-UHF-LBAND");
+			break;
+		case MP009:
+			dprintk("MP009 : diversity 29098 CBAND-UHF-LBAND-SBAND");
+			break;
+		default:
+			goto identification_error;
+		}
+
+		switch (identity->version & 0x1f) {
+		case P1G_21R2:
+			dprintk("P1G_21R2 detected");
+			identity->p1g = 1;
+			break;
+		case P1G:
+			dprintk("P1G detected");
+			identity->p1g = 1;
+			break;
+		case P1D_E_F:
+			dprintk("P1D/E/F detected");
+			break;
+		case P1C:
+			dprintk("P1C detected");
+			break;
+		case P1A_B:
+			dprintk("P1-A/B detected: driver is deactivated - not available");
+			goto identification_error;
+			break;
+		default:
+			goto identification_error;
+		}
+	}
+
+	return 0;
+
+identification_error:
+	return -EIO;;
 }
 
 static void dib0090_reset_digital(struct dvb_frontend *fe, const struct dib0090_config *cfg)
 {
 	struct dib0090_state *state = fe->tuner_priv;
+	u16 PllCfg, i, v;
 
 	HARD_RESET(state);
 
-	dib0090_write_reg(state, 0x24, EN_PLL);
+	dib0090_write_reg(state, 0x24, EN_PLL | EN_CRYSTAL);
 	dib0090_write_reg(state, 0x1b, EN_DIGCLK | EN_PLL | EN_CRYSTAL);	/* PLL, DIG_CLK and CRYSTAL remain */
 
-	/* adcClkOutRatio=8->7, release reset */
-	dib0090_write_reg(state, 0x20, ((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (0 << 4) | 0);
+	if (!cfg->in_soc) {
+		/* adcClkOutRatio=8->7, release reset */
+		dib0090_write_reg(state, 0x20, ((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (0 << 4) | 0);
+		if (cfg->clkoutdrive != 0)
+			dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
+					  | (cfg->clkoutdrive << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
+		else
+			dib0090_write_reg(state, 0x23, (0 << 15) | ((!cfg->analog_output) << 14) | (2 << 10) | (1 << 9) | (0 << 8)
+					  | (7 << 5) | (cfg->clkouttobamse << 4) | (0 << 2) | (0));
+	}
+
+	/* Read Pll current config * */
+	PllCfg = dib0090_read_reg(state, 0x21);
+
+	/** Reconfigure PLL if current setting is different from default setting **/
+	if ((PllCfg & 0x1FFF) != ((cfg->io.pll_range << 12) | (cfg->io.pll_loopdiv << 6) | (cfg->io.pll_prediv)) && (!cfg->in_soc)
+			&& !cfg->io.pll_bypass) {
+
+		/* Set Bypass mode */
+		PllCfg |= (1 << 15);
+		dib0090_write_reg(state, 0x21, PllCfg);
+
+		/* Set Reset Pll */
+		PllCfg &= ~(1 << 13);
+		dib0090_write_reg(state, 0x21, PllCfg);
+
+	/*** Set new Pll configuration in bypass and reset state ***/
+		PllCfg = (1 << 15) | (0 << 13) | (cfg->io.pll_range << 12) | (cfg->io.pll_loopdiv << 6) | (cfg->io.pll_prediv);
+		dib0090_write_reg(state, 0x21, PllCfg);
+
+		/* Remove Reset Pll */
+		PllCfg |= (1 << 13);
+		dib0090_write_reg(state, 0x21, PllCfg);
+
+	/*** Wait for PLL lock ***/
+		i = 100;
+		do {
+			v = !!(dib0090_read_reg(state, 0x1a) & 0x800);
+			if (v)
+				break;
+		} while (--i);
+
+		if (i == 0) {
+			dprintk("Pll: Unable to lock Pll");
+			return;
+		}
+
+		/* Finally Remove Bypass mode */
+		PllCfg &= ~(1 << 15);
+		dib0090_write_reg(state, 0x21, PllCfg);
+	}
+
+	if (cfg->io.pll_bypass) {
+		PllCfg |= (cfg->io.pll_bypass << 15);
+		dib0090_write_reg(state, 0x21, PllCfg);
+	}
+}
+
+static int dib0090_fw_reset_digital(struct dvb_frontend *fe, const struct dib0090_config *cfg)
+{
+	struct dib0090_fw_state *state = fe->tuner_priv;
+	u16 PllCfg;
+	u16 v;
+	int i;
+
+	dprintk("fw reset digital");
+	HARD_RESET(state);
+
+	dib0090_fw_write_reg(state, 0x24, EN_PLL | EN_CRYSTAL);
+	dib0090_fw_write_reg(state, 0x1b, EN_DIGCLK | EN_PLL | EN_CRYSTAL);	/* PLL, DIG_CLK and CRYSTAL remain */
+
+	dib0090_fw_write_reg(state, 0x20,
+			((cfg->io.adc_clock_ratio - 1) << 11) | (0 << 10) | (1 << 9) | (1 << 8) | (cfg->data_tx_drv << 4) | cfg->ls_cfg_pad_drv);
+
+	v = (0 << 15) | ((!cfg->analog_output) << 14) | (1 << 9) | (0 << 8) | (cfg->clkouttobamse << 4) | (0 << 2) | (0);
 	if (cfg->clkoutdrive != 0)
-		dib0090_write_reg(state, 0x23,
-				  (0 << 15) | ((!cfg->analog_output) << 14) | (1 << 10) | (1 << 9) | (0 << 8) | (cfg->clkoutdrive << 5) | (cfg->
-																	   clkouttobamse
-																	   << 4) | (0
-																		    <<
-																		    2)
-				  | (0));
+		v |= cfg->clkoutdrive << 5;
 	else
-		dib0090_write_reg(state, 0x23,
-				  (0 << 15) | ((!cfg->analog_output) << 14) | (1 << 10) | (1 << 9) | (0 << 8) | (7 << 5) | (cfg->
-															    clkouttobamse << 4) | (0
-																		   <<
-																		   2)
-				  | (0));
+		v |= 7 << 5;
+
+	v |= 2 << 10;
+	dib0090_fw_write_reg(state, 0x23, v);
+
+	/* Read Pll current config * */
+	PllCfg = dib0090_fw_read_reg(state, 0x21);
+
+	/** Reconfigure PLL if current setting is different from default setting **/
+	if ((PllCfg & 0x1FFF) != ((cfg->io.pll_range << 12) | (cfg->io.pll_loopdiv << 6) | (cfg->io.pll_prediv)) && !cfg->io.pll_bypass) {
 
-	/* enable pll, de-activate reset, ratio: 2/1 = 60MHz */
-	dib0090_write_reg(state, 0x21,
-			  (cfg->io.pll_bypass << 15) | (1 << 13) | (cfg->io.pll_range << 12) | (cfg->io.pll_loopdiv << 6) | (cfg->io.pll_prediv));
+		/* Set Bypass mode */
+		PllCfg |= (1 << 15);
+		dib0090_fw_write_reg(state, 0x21, PllCfg);
 
+		/* Set Reset Pll */
+		PllCfg &= ~(1 << 13);
+		dib0090_fw_write_reg(state, 0x21, PllCfg);
+
+	/*** Set new Pll configuration in bypass and reset state ***/
+		PllCfg = (1 << 15) | (0 << 13) | (cfg->io.pll_range << 12) | (cfg->io.pll_loopdiv << 6) | (cfg->io.pll_prediv);
+		dib0090_fw_write_reg(state, 0x21, PllCfg);
+
+		/* Remove Reset Pll */
+		PllCfg |= (1 << 13);
+		dib0090_fw_write_reg(state, 0x21, PllCfg);
+
+	/*** Wait for PLL lock ***/
+		i = 100;
+		do {
+			v = !!(dib0090_fw_read_reg(state, 0x1a) & 0x800);
+			if (v)
+				break;
+		} while (--i);
+
+		if (i == 0) {
+			dprintk("Pll: Unable to lock Pll");
+			return -EIO;
+		}
+
+		/* Finally Remove Bypass mode */
+		PllCfg &= ~(1 << 15);
+		dib0090_fw_write_reg(state, 0x21, PllCfg);
+	}
+
+	if (cfg->io.pll_bypass) {
+		PllCfg |= (cfg->io.pll_bypass << 15);
+		dib0090_fw_write_reg(state, 0x21, PllCfg);
+	}
+
+	return dib0090_fw_identify(fe);
 }
 
 static int dib0090_wakeup(struct dvb_frontend *fe)
@@ -273,6 +587,9 @@ static int dib0090_wakeup(struct dvb_frontend *fe)
 	struct dib0090_state *state = fe->tuner_priv;
 	if (state->config->sleep)
 		state->config->sleep(fe, 0);
+
+	/* enable dataTX in case we have been restarted in the wrong moment */
+	dib0090_write_reg(state, 0x23, dib0090_read_reg(state, 0x23) | (1 << 14));
 	return 0;
 }
 
@@ -292,8 +609,75 @@ void dib0090_dcc_freq(struct dvb_frontend *fe, u8 fast)
 	else
 		dib0090_write_reg(state, 0x04, 1);
 }
+
 EXPORT_SYMBOL(dib0090_dcc_freq);
 
+static const u16 bb_ramp_pwm_normal_socs[] = {
+	550,			/* max BB gain in 10th of dB */
+	(1 << 9) | 8,		/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> BB_RAMP2 */
+	440,
+	(4 << 9) | 0,		/* BB_RAMP3 = 26dB */
+	(0 << 9) | 208,		/* BB_RAMP4 */
+	(4 << 9) | 208,		/* BB_RAMP5 = 29dB */
+	(0 << 9) | 440,		/* BB_RAMP6 */
+};
+
+static const u16 rf_ramp_pwm_cband_7090[] = {
+	280,			/* max RF gain in 10th of dB */
+	18,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	504,			/* ramp_max = maximum X used on the ramp */
+	(29 << 10) | 364,	/* RF_RAMP5, LNA 1 = 8dB */
+	(0 << 10) | 504,	/* RF_RAMP6, LNA 1 */
+	(60 << 10) | 228,	/* RF_RAMP7, LNA 2 = 7.7dB */
+	(0 << 10) | 364,	/* RF_RAMP8, LNA 2 */
+	(34 << 10) | 109,	/* GAIN_4_1, LNA 3 = 6.8dB */
+	(0 << 10) | 228,	/* GAIN_4_2, LNA 3 */
+	(37 << 10) | 0,		/* RF_RAMP3, LNA 4 = 6.2dB */
+	(0 << 10) | 109,	/* RF_RAMP4, LNA 4 */
+};
+
+static const u16 rf_ramp_pwm_cband_8090[] = {
+	345,			/* max RF gain in 10th of dB */
+	29,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1000,			/* ramp_max = maximum X used on the ramp */
+	(35 << 10) | 772,	/* RF_RAMP3, LNA 1 = 8dB */
+	(0 << 10) | 1000,	/* RF_RAMP4, LNA 1 */
+	(58 << 10) | 496,	/* RF_RAMP5, LNA 2 = 9.5dB */
+	(0 << 10) | 772,	/* RF_RAMP6, LNA 2 */
+	(27 << 10) | 200,	/* RF_RAMP7, LNA 3 = 10.5dB */
+	(0 << 10) | 496,	/* RF_RAMP8, LNA 3 */
+	(40 << 10) | 0,		/* GAIN_4_1, LNA 4 = 7dB */
+	(0 << 10) | 200,	/* GAIN_4_2, LNA 4 */
+};
+
+static const u16 rf_ramp_pwm_uhf_7090[] = {
+	407,			/* max RF gain in 10th of dB */
+	13,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	529,			/* ramp_max = maximum X used on the ramp */
+	(23 << 10) | 0,		/* RF_RAMP3, LNA 1 = 14.7dB */
+	(0 << 10) | 176,	/* RF_RAMP4, LNA 1 */
+	(63 << 10) | 400,	/* RF_RAMP5, LNA 2 = 8dB */
+	(0 << 10) | 529,	/* RF_RAMP6, LNA 2 */
+	(48 << 10) | 316,	/* RF_RAMP7, LNA 3 = 6.8dB */
+	(0 << 10) | 400,	/* RF_RAMP8, LNA 3 */
+	(29 << 10) | 176,	/* GAIN_4_1, LNA 4 = 11.5dB */
+	(0 << 10) | 316,	/* GAIN_4_2, LNA 4 */
+};
+
+static const u16 rf_ramp_pwm_uhf_8090[] = {
+	388,			/* max RF gain in 10th of dB */
+	26,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> RF_RAMP2 */
+	1008,			/* ramp_max = maximum X used on the ramp */
+	(11 << 10) | 0,		/* RF_RAMP3, LNA 1 = 14.7dB */
+	(0 << 10) | 369,	/* RF_RAMP4, LNA 1 */
+	(41 << 10) | 809,	/* RF_RAMP5, LNA 2 = 8dB */
+	(0 << 10) | 1008,	/* RF_RAMP6, LNA 2 */
+	(27 << 10) | 659,	/* RF_RAMP7, LNA 3 = 6dB */
+	(0 << 10) | 809,	/* RF_RAMP8, LNA 3 */
+	(14 << 10) | 369,	/* GAIN_4_1, LNA 4 = 11.5dB */
+	(0 << 10) | 659,	/* GAIN_4_2, LNA 4 */
+};
+
 static const u16 rf_ramp_pwm_cband[] = {
 	0,			/* max RF gain in 10th of dB */
 	0,			/* ramp_slope = 1dB of gain -> clock_ticks_per_db = clk_khz / ramp_slope -> 0x2b */
@@ -326,6 +710,16 @@ static const u16 rf_ramp_uhf[] = {
 	0, 0, 127,		/* CBAND :  0.0 dB */
 };
 
+static const u16 rf_ramp_cband_broadmatching[] =	/* for p1G only */
+{
+	314,			/* Calibrated at 200MHz order has been changed g4-g3-g2-g1 */
+	84, 314, 127,		/* LNA1 */
+	80, 230, 255,		/* LNA2 */
+	80, 150, 127,		/* LNA3  It was measured 12dB, do not lock if 120 */
+	70, 70, 127,		/* LNA4 */
+	0, 0, 127,		/* CBAND */
+};
+
 static const u16 rf_ramp_cband[] = {
 	332,			/* max RF gain in 10th of dB */
 	132, 252, 127,		/* LNA1,  dB */
@@ -380,8 +774,8 @@ static const u16 bb_ramp_pwm_normal[] = {
 };
 
 struct slope {
-	int16_t range;
-	int16_t slope;
+	s16 range;
+	s16 slope;
 };
 static u16 slopes_to_scale(const struct slope *slopes, u8 num, s16 val)
 {
@@ -597,19 +991,39 @@ void dib0090_pwm_gain_reset(struct dvb_frontend *fe)
 #endif
 #ifdef CONFIG_BAND_CBAND
 		if (state->current_band == BAND_CBAND) {
-			dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband);
-			dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+			if (state->identity.in_soc) {
+				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
+				if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
+					dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_8090);
+				else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
+					dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband_7090);
+			} else {
+				dib0090_set_rframp_pwm(state, rf_ramp_pwm_cband);
+				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+			}
 		} else
 #endif
 #ifdef CONFIG_BAND_VHF
 		if (state->current_band == BAND_VHF) {
-			dib0090_set_rframp_pwm(state, rf_ramp_pwm_vhf);
-			dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+			if (state->identity.in_soc) {
+				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
+			} else {
+				dib0090_set_rframp_pwm(state, rf_ramp_pwm_vhf);
+				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+			}
 		} else
 #endif
 		{
-			dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf);
-			dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+			if (state->identity.in_soc) {
+				if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
+					dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf_8090);
+				else if (state->identity.version == SOC_7090_P1G_11R1 || state->identity.version == SOC_7090_P1G_21R1)
+					dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf_7090);
+				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal_socs);
+			} else {
+				dib0090_set_rframp_pwm(state, rf_ramp_pwm_uhf);
+				dib0090_set_bbramp_pwm(state, bb_ramp_pwm_normal);
+			}
 		}
 
 		if (state->rf_ramp[0] != 0)
@@ -617,11 +1031,21 @@ void dib0090_pwm_gain_reset(struct dvb_frontend *fe)
 		else
 			dib0090_write_reg(state, 0x32, (0 << 11));
 
+		dib0090_write_reg(state, 0x04, 0x01);
 		dib0090_write_reg(state, 0x39, (1 << 10));
 	}
 }
+
 EXPORT_SYMBOL(dib0090_pwm_gain_reset);
 
+static u32 dib0090_get_slow_adc_val(struct dib0090_state *state)
+{
+	u16 adc_val = dib0090_read_reg(state, 0x1d);
+	if (state->identity.in_soc)
+		adc_val >>= 2;
+	return adc_val;
+}
+
 int dib0090_gain_control(struct dvb_frontend *fe)
 {
 	struct dib0090_state *state = fe->tuner_priv;
@@ -643,18 +1067,21 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 		} else
 #endif
 #ifdef CONFIG_BAND_VHF
-		if (state->current_band == BAND_VHF) {
+		if (state->current_band == BAND_VHF && !state->identity.p1g) {
 			dib0090_set_rframp(state, rf_ramp_vhf);
 			dib0090_set_bbramp(state, bb_ramp_boost);
 		} else
 #endif
 #ifdef CONFIG_BAND_CBAND
-		if (state->current_band == BAND_CBAND) {
+		if (state->current_band == BAND_CBAND && !state->identity.p1g) {
 			dib0090_set_rframp(state, rf_ramp_cband);
 			dib0090_set_bbramp(state, bb_ramp_boost);
 		} else
 #endif
-		{
+		if ((state->current_band == BAND_CBAND || state->current_band == BAND_VHF) && state->identity.p1g) {
+			dib0090_set_rframp(state, rf_ramp_cband_broadmatching);
+			dib0090_set_bbramp(state, bb_ramp_boost);
+		} else {
 			dib0090_set_rframp(state, rf_ramp_uhf);
 			dib0090_set_bbramp(state, bb_ramp_boost);
 		}
@@ -669,17 +1096,25 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 
 		*tune_state = CT_AGC_STEP_0;
 	} else if (!state->agc_freeze) {
-		s16 wbd;
+		s16 wbd = 0, i, cnt;
 
 		int adc;
-		wbd_val = dib0090_read_reg(state, 0x1d);
+		wbd_val = dib0090_get_slow_adc_val(state);
 
-		/* read and calc the wbd power */
-		wbd = dib0090_wbd_to_db(state, wbd_val);
+		if (*tune_state == CT_AGC_STEP_0)
+			cnt = 5;
+		else
+			cnt = 1;
+
+		for (i = 0; i < cnt; i++) {
+			wbd_val = dib0090_get_slow_adc_val(state);
+			wbd += dib0090_wbd_to_db(state, wbd_val);
+		}
+		wbd /= cnt;
 		wbd_error = state->wbd_target - wbd;
 
 		if (*tune_state == CT_AGC_STEP_0) {
-			if (wbd_error < 0 && state->rf_gain_limit > 0) {
+			if (wbd_error < 0 && state->rf_gain_limit > 0 && !state->identity.p1g) {
 #ifdef CONFIG_BAND_CBAND
 				/* in case of CBAND tune reduce first the lt_gain2 before adjusting the RF gain */
 				u8 ltg2 = (state->rf_lt_def >> 10) & 0x7;
@@ -700,39 +1135,39 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 			adc_error = (s16) (((s32) ADC_TARGET) - adc);
 #ifdef CONFIG_STANDARD_DAB
 			if (state->fe->dtv_property_cache.delivery_system == STANDARD_DAB)
-				adc_error += 130;
+				adc_error -= 10;
 #endif
 #ifdef CONFIG_STANDARD_DVBT
 			if (state->fe->dtv_property_cache.delivery_system == STANDARD_DVBT &&
-			    (state->fe->dtv_property_cache.modulation == QAM_64 || state->fe->dtv_property_cache.modulation == QAM_16))
+					(state->fe->dtv_property_cache.modulation == QAM_64 || state->fe->dtv_property_cache.modulation == QAM_16))
 				adc_error += 60;
 #endif
 #ifdef CONFIG_SYS_ISDBT
 			if ((state->fe->dtv_property_cache.delivery_system == SYS_ISDBT) && (((state->fe->dtv_property_cache.layer[0].segment_count >
-											       0)
-											      &&
-											      ((state->fe->dtv_property_cache.layer[0].modulation ==
-												QAM_64)
-											       || (state->fe->dtv_property_cache.layer[0].
-												   modulation == QAM_16)))
-											     ||
-											     ((state->fe->dtv_property_cache.layer[1].segment_count >
-											       0)
-											      &&
-											      ((state->fe->dtv_property_cache.layer[1].modulation ==
-												QAM_64)
-											       || (state->fe->dtv_property_cache.layer[1].
-												   modulation == QAM_16)))
-											     ||
-											     ((state->fe->dtv_property_cache.layer[2].segment_count >
-											       0)
-											      &&
-											      ((state->fe->dtv_property_cache.layer[2].modulation ==
-												QAM_64)
-											       || (state->fe->dtv_property_cache.layer[2].
-												   modulation == QAM_16)))
-			    )
-			    )
+								0)
+							&&
+							((state->fe->dtv_property_cache.layer[0].modulation ==
+							  QAM_64)
+							 || (state->fe->dtv_property_cache.
+								 layer[0].modulation == QAM_16)))
+						||
+						((state->fe->dtv_property_cache.layer[1].segment_count >
+						  0)
+						 &&
+						 ((state->fe->dtv_property_cache.layer[1].modulation ==
+						   QAM_64)
+						  || (state->fe->dtv_property_cache.
+							  layer[1].modulation == QAM_16)))
+						||
+						((state->fe->dtv_property_cache.layer[2].segment_count >
+						  0)
+						 &&
+						 ((state->fe->dtv_property_cache.layer[2].modulation ==
+						   QAM_64)
+						  || (state->fe->dtv_property_cache.
+							  layer[2].modulation == QAM_16)))
+						)
+				)
 				adc_error += 60;
 #endif
 
@@ -760,9 +1195,9 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 		}
 #ifdef DEBUG_AGC
 		dprintk
-		    ("FE: %d, tune state %d, ADC = %3ddB (ADC err %3d) WBD %3ddB (WBD err %3d, WBD val SADC: %4d), RFGainLimit (TOP): %3d, signal: %3ddBm",
-		     (u32) fe->id, (u32) *tune_state, (u32) adc, (u32) adc_error, (u32) wbd, (u32) wbd_error, (u32) wbd_val,
-		     (u32) state->rf_gain_limit >> WBD_ALPHA, (s32) 200 + adc - (state->current_gain >> GAIN_ALPHA));
+			("tune state %d, ADC = %3ddB (ADC err %3d) WBD %3ddB (WBD err %3d, WBD val SADC: %4d), RFGainLimit (TOP): %3d, signal: %3ddBm",
+			 (u32) *tune_state, (u32) adc, (u32) adc_error, (u32) wbd, (u32) wbd_error, (u32) wbd_val,
+			 (u32) state->rf_gain_limit >> WBD_ALPHA, (s32) 200 + adc - (state->current_gain >> GAIN_ALPHA));
 #endif
 	}
 
@@ -771,6 +1206,7 @@ int dib0090_gain_control(struct dvb_frontend *fe)
 		dib0090_gain_apply(state, adc_error, wbd_error, apply_gain_immediatly);
 	return ret;
 }
+
 EXPORT_SYMBOL(dib0090_gain_control);
 
 void dib0090_get_current_gain(struct dvb_frontend *fe, u16 * rf, u16 * bb, u16 * rf_gain_limit, u16 * rflt)
@@ -785,13 +1221,47 @@ void dib0090_get_current_gain(struct dvb_frontend *fe, u16 * rf, u16 * bb, u16 *
 	if (rflt)
 		*rflt = (state->rf_lt_def >> 10) & 0x7;
 }
+
 EXPORT_SYMBOL(dib0090_get_current_gain);
 
-u16 dib0090_get_wbd_offset(struct dvb_frontend *tuner)
+u16 dib0090_get_wbd_offset(struct dvb_frontend *fe)
 {
-	struct dib0090_state *st = tuner->tuner_priv;
-	return st->wbd_offset;
+	struct dib0090_state *state = fe->tuner_priv;
+	u32 f_MHz = state->fe->dtv_property_cache.frequency / 1000000;
+	s32 current_temp = state->temperature;
+	s32 wbd_thot, wbd_tcold;
+	const struct dib0090_wbd_slope *wbd = state->current_wbd_table;
+
+	while (f_MHz > wbd->max_freq)
+		wbd++;
+
+	dprintk("using wbd-table-entry with max freq %d", wbd->max_freq);
+
+	if (current_temp < 0)
+		current_temp = 0;
+	if (current_temp > 128)
+		current_temp = 128;
+
+	state->wbdmux &= ~(7 << 13);
+	if (wbd->wbd_gain != 0)
+		state->wbdmux |= (wbd->wbd_gain << 13);
+	else
+		state->wbdmux |= (4 << 13);
+
+	dib0090_write_reg(state, 0x10, state->wbdmux);
+
+	wbd_thot = wbd->offset_hot - (((u32) wbd->slope_hot * f_MHz) >> 6);
+	wbd_tcold = wbd->offset_cold - (((u32) wbd->slope_cold * f_MHz) >> 6);
+
+	wbd_tcold += ((wbd_thot - wbd_tcold) * current_temp) >> 7;
+
+	state->wbd_target = dib0090_wbd_to_db(state, state->wbd_offset + wbd_tcold);
+	dprintk("wbd-target: %d dB", (u32) state->wbd_target);
+	dprintk("wbd offset applied is %d", wbd_tcold);
+
+	return state->wbd_offset + wbd_tcold;
 }
+
 EXPORT_SYMBOL(dib0090_get_wbd_offset);
 
 static const u16 dib0090_defaults[] = {
@@ -801,7 +1271,7 @@ static const u16 dib0090_defaults[] = {
 	0x99a0,
 	0x6008,
 	0x0000,
-	0x8acb,
+	0x8bcb,
 	0x0000,
 	0x0405,
 	0x0000,
@@ -829,8 +1299,6 @@ static const u16 dib0090_defaults[] = {
 	1, 0x39,
 	0x0000,
 
-	1, 0x1b,
-	EN_IQADC | EN_BB | EN_BIAS | EN_DIGCLK | EN_PLL | EN_CRYSTAL,
 	2, 0x1e,
 	0x07FF,
 	0x0007,
@@ -844,50 +1312,125 @@ static const u16 dib0090_defaults[] = {
 	0
 };
 
-static int dib0090_reset(struct dvb_frontend *fe)
-{
-	struct dib0090_state *state = fe->tuner_priv;
-	u16 l, r, *n;
+static const u16 dib0090_p1g_additionnal_defaults[] = {
+	1, 0x05,
+	0xabcd,
 
-	dib0090_reset_digital(fe, state->config);
-	state->revision = dib0090_identify(fe);
+	1, 0x11,
+	0x00b4,
 
-	/* Revision definition */
-	if (state->revision == 0xff)
-		return -EINVAL;
-#ifdef EFUSE
-	else if ((state->revision & 0x1f) >= 3)	/* Update the efuse : Only available for KROSUS > P1C */
-		dib0090_set_EFUSE(state);
-#endif
+	1, 0x1c,
+	0xfffd,
 
-#ifdef CONFIG_TUNER_DIB0090_P1B_SUPPORT
-	if (!(state->revision & 0x1))	/* it is P1B - reset is already done */
-		return 0;
-#endif
+	1, 0x40,
+	0x108,
+	0
+};
+
+static void dib0090_set_default_config(struct dib0090_state *state, const u16 * n)
+{
+	u16 l, r;
 
-	/* Upload the default values */
-	n = (u16 *) dib0090_defaults;
 	l = pgm_read_word(n++);
 	while (l) {
 		r = pgm_read_word(n++);
 		do {
-			/* DEBUG_TUNER */
-			/* dprintk("%d, %d, %d", l, r, pgm_read_word(n)); */
 			dib0090_write_reg(state, r, pgm_read_word(n++));
 			r++;
 		} while (--l);
 		l = pgm_read_word(n++);
 	}
+}
+
+#define CAP_VALUE_MIN (u8)  9
+#define CAP_VALUE_MAX (u8) 40
+#define HR_MIN	      (u8) 25
+#define HR_MAX	      (u8) 40
+#define POLY_MIN      (u8)  0
+#define POLY_MAX      (u8)  8
+
+void dib0090_set_EFUSE(struct dib0090_state *state)
+{
+	u8 c, h, n;
+	u16 e2, e4;
+	u16 cal;
+
+	e2 = dib0090_read_reg(state, 0x26);
+	e4 = dib0090_read_reg(state, 0x28);
+
+	if ((state->identity.version == P1D_E_F) ||
+			(state->identity.version == P1G) || (e2 == 0xffff)) {
+
+		dib0090_write_reg(state, 0x22, 0x10);
+		cal = (dib0090_read_reg(state, 0x22) >> 6) & 0x3ff;
+
+		if ((cal < 670) || (cal == 1023))
+			cal = 850;
+		n = 165 - ((cal * 10)>>6) ;
+		e2 = e4 = (3<<12) | (34<<6) | (n);
+	}
+
+	if (e2 != e4)
+		e2 &= e4; /* Remove the redundancy  */
+
+	if (e2 != 0xffff) {
+		c = e2 & 0x3f;
+		n = (e2 >> 12) & 0xf;
+		h = (e2 >> 6) & 0x3f;
+
+		if ((c >= CAP_VALUE_MAX) || (c <= CAP_VALUE_MIN))
+			c = 32;
+		if ((h >= HR_MAX) || (h <= HR_MIN))
+			h = 34;
+		if ((n >= POLY_MAX) || (n <= POLY_MIN))
+			n = 3;
+
+		dib0090_write_reg(state, 0x13, (h << 10)) ;
+		e2 = (n<<11) | ((h>>2)<<6) | (c);
+		dib0090_write_reg(state, 0x2, e2) ; /* Load the BB_2 */
+	}
+}
+
+static int dib0090_reset(struct dvb_frontend *fe)
+{
+	struct dib0090_state *state = fe->tuner_priv;
+
+	dib0090_reset_digital(fe, state->config);
+	if (dib0090_identify(fe) < 0)
+		return -EIO;
+
+#ifdef CONFIG_TUNER_DIB0090_P1B_SUPPORT
+	if (!(state->identity.version & 0x1))	/* it is P1B - reset is already done */
+		return 0;
+#endif
+
+	if (!state->identity.in_soc) {
+		if ((dib0090_read_reg(state, 0x1a) >> 5) & 0x2)
+			dib0090_write_reg(state, 0x1b, (EN_IQADC | EN_BB | EN_BIAS | EN_DIGCLK | EN_PLL | EN_CRYSTAL));
+		else
+			dib0090_write_reg(state, 0x1b, (EN_DIGCLK | EN_PLL | EN_CRYSTAL));
+	}
+
+	dib0090_set_default_config(state, dib0090_defaults);
+
+	if (state->identity.in_soc)
+		dib0090_write_reg(state, 0x18, 0x2910);  /* charge pump current = 0 */
+
+	if (state->identity.p1g)
+		dib0090_set_default_config(state, dib0090_p1g_additionnal_defaults);
+
+	/* Update the efuse : Only available for KROSUS > P1C  and SOC as well*/
+	if (((state->identity.version & 0x1f) >= P1D_E_F) || (state->identity.in_soc))
+		dib0090_set_EFUSE(state);
 
 	/* Congigure in function of the crystal */
 	if (state->config->io.clock_khz >= 24000)
-		l = 1;
+		dib0090_write_reg(state, 0x14, 1);
 	else
-		l = 2;
-	dib0090_write_reg(state, 0x14, l);
+		dib0090_write_reg(state, 0x14, 2);
 	dprintk("Pll lock : %d", (dib0090_read_reg(state, 0x1a) >> 11) & 0x1);
 
-	state->reset = 3;	/* enable iq-offset-calibration and wbd-calibration when tuning next time */
+	state->calibrate = DC_CAL | WBD_CAL | TEMP_CAL;	/* enable iq-offset-calibration and wbd-calibration when tuning next time */
 
 	return 0;
 }
@@ -927,11 +1470,11 @@ static int dib0090_get_offset(struct dib0090_state *state, enum frontend_tune_st
 }
 
 struct dc_calibration {
-	uint8_t addr;
-	uint8_t offset;
-	uint8_t pga:1;
-	uint16_t bb1;
-	uint8_t i:1;
+	u8 addr;
+	u8 offset;
+	u8 pga:1;
+	u16 bb1;
+	u8 i:1;
 };
 
 static const struct dc_calibration dc_table[] = {
@@ -944,6 +1487,17 @@ static const struct dc_calibration dc_table[] = {
 	{0},
 };
 
+static const struct dc_calibration dc_p1g_table[] = {
+	/* Step1 BB gain1= 26 with boost 1, gain 2 = 0 */
+	/* addr ; trim reg offset ; pga ; CTRL_BB1 value ; i or q */
+	{0x06, 5, 1, (1 << 13) | (0 << 8) | (15 << 3), 1},
+	{0x07, 11, 1, (1 << 13) | (0 << 8) | (15 << 3), 0},
+	/* Step 2 BB gain 1 = 26 with boost = 1 & gain 2 = 29 */
+	{0x06, 0, 0, (1 << 13) | (29 << 8) | (15 << 3), 1},
+	{0x06, 10, 0, (1 << 13) | (29 << 8) | (15 << 3), 0},
+	{0},
+};
+
 static void dib0090_set_trim(struct dib0090_state *state)
 {
 	u16 *val;
@@ -962,41 +1516,45 @@ static void dib0090_set_trim(struct dib0090_state *state)
 static int dib0090_dc_offset_calibration(struct dib0090_state *state, enum frontend_tune_state *tune_state)
 {
 	int ret = 0;
+	u16 reg;
 
 	switch (*tune_state) {
-
 	case CT_TUNER_START:
-		/* init */
-		dprintk("Internal DC calibration");
-
-		/* the LNA is off */
-		dib0090_write_reg(state, 0x24, 0x02ed);
+		dprintk("Start DC offset calibration");
 
 		/* force vcm2 = 0.8V */
 		state->bb6 = 0;
 		state->bb7 = 0x040d;
 
+		/* the LNA AND LO are off */
+		reg = dib0090_read_reg(state, 0x24) & 0x0ffb;	/* shutdown lna and lo */
+		dib0090_write_reg(state, 0x24, reg);
+
+		state->wbdmux = dib0090_read_reg(state, 0x10);
+		dib0090_write_reg(state, 0x10, (state->wbdmux & ~(0xff << 3)) | (0x7 << 3) | 0x3);
+		dib0090_write_reg(state, 0x23, dib0090_read_reg(state, 0x23) & ~(1 << 14));
+
 		state->dc = dc_table;
 
+		if (state->identity.p1g)
+			state->dc = dc_p1g_table;
 		*tune_state = CT_TUNER_STEP_0;
 
 		/* fall through */
 
 	case CT_TUNER_STEP_0:
+		dprintk("Sart/continue DC calibration for %s path", (state->dc->i == 1) ? "I" : "Q");
 		dib0090_write_reg(state, 0x01, state->dc->bb1);
 		dib0090_write_reg(state, 0x07, state->bb7 | (state->dc->i << 7));
 
 		state->step = 0;
-
 		state->min_adc_diff = 1023;
-
 		*tune_state = CT_TUNER_STEP_1;
 		ret = 50;
 		break;
 
 	case CT_TUNER_STEP_1:
 		dib0090_set_trim(state);
-
 		*tune_state = CT_TUNER_STEP_2;
 		break;
 
@@ -1007,7 +1565,13 @@ static int dib0090_dc_offset_calibration(struct dib0090_state *state, enum front
 		break;
 
 	case CT_TUNER_STEP_5:	/* found an offset */
-		dprintk("FE%d: IQC read=%d, current=%x", state->fe->id, (u32) state->adc_diff, state->step);
+		dprintk("adc_diff = %d, current step= %d", (u32) state->adc_diff, state->step);
+		if (state->step == 0 && state->adc_diff < 0) {
+			state->min_adc_diff = -1023;
+			dprintk("Change of sign of the minimum adc diff");
+		}
+
+		dprintk("adc_diff = %d, min_adc_diff = %d current_step = %d", state->adc_diff, state->min_adc_diff, state->step);
 
 		/* first turn for this frequency */
 		if (state->step == 0) {
@@ -1017,20 +1581,21 @@ static int dib0090_dc_offset_calibration(struct dib0090_state *state, enum front
 				state->step = 0x10;
 		}
 
-		state->adc_diff = ABS(state->adc_diff);
-
-		if (state->adc_diff < state->min_adc_diff && steps(state->step) < 15) {	/* stop search when the delta to 0 is increasing */
+		/* Look for a change of Sign in the Adc_diff.min_adc_diff is used to STORE the setp N-1 */
+		if ((state->adc_diff & 0x8000) == (state->min_adc_diff & 0x8000) && steps(state->step) < 15) {
+			/* stop search when the delta the sign is changing and Steps =15 and Step=0 is force for continuance */
 			state->step++;
 			state->min_adc_diff = state->adc_diff;
 			*tune_state = CT_TUNER_STEP_1;
 		} else {
-
 			/* the minimum was what we have seen in the step before */
-			state->step--;
-			dib0090_set_trim(state);
+			if (ABS(state->adc_diff) > ABS(state->min_adc_diff)) {
+				dprintk("Since adc_diff N = %d  > adc_diff step N-1 = %d, Come back one step", state->adc_diff, state->min_adc_diff);
+				state->step--;
+			}
 
-			dprintk("FE%d: BB Offset Cal, BBreg=%hd,Offset=%hd,Value Set=%hd", state->fe->id, state->dc->addr, state->adc_diff,
-				state->step);
+			dib0090_set_trim(state);
+			dprintk("BB Offset Cal, BBreg=%hd,Offset=%hd,Value Set=%hd", state->dc->addr, state->adc_diff, state->step);
 
 			state->dc++;
 			if (state->dc->addr == 0)	/* done */
@@ -1045,7 +1610,7 @@ static int dib0090_dc_offset_calibration(struct dib0090_state *state, enum front
 		dib0090_write_reg(state, 0x07, state->bb7 & ~0x0008);
 		dib0090_write_reg(state, 0x1f, 0x7);
 		*tune_state = CT_TUNER_START;	/* reset done -> real tuning can now begin */
-		state->reset &= ~0x1;
+		state->calibrate &= ~DC_CAL;
 	default:
 		break;
 	}
@@ -1054,21 +1619,43 @@ static int dib0090_dc_offset_calibration(struct dib0090_state *state, enum front
 
 static int dib0090_wbd_calibration(struct dib0090_state *state, enum frontend_tune_state *tune_state)
 {
+	u8 wbd_gain;
+	const struct dib0090_wbd_slope *wbd = state->current_wbd_table;
+
 	switch (*tune_state) {
 	case CT_TUNER_START:
-		/* WBD-mode=log, Bias=2, Gain=6, Testmode=1, en=1, WBDMUX=1 */
-		dib0090_write_reg(state, 0x10, 0xdb09 | (1 << 10));
-		dib0090_write_reg(state, 0x24, EN_UHF & 0x0fff);
+		while (state->current_rf / 1000 > wbd->max_freq)
+			wbd++;
+		if (wbd->wbd_gain != 0)
+			wbd_gain = wbd->wbd_gain;
+		else {
+			wbd_gain = 4;
+#if defined(CONFIG_BAND_LBAND) || defined(CONFIG_BAND_SBAND)
+			if ((state->current_band == BAND_LBAND) || (state->current_band == BAND_SBAND))
+				wbd_gain = 2;
+#endif
+		}
+
+		if (wbd_gain == state->wbd_calibration_gain) {	/* the WBD calibration has already been done */
+			*tune_state = CT_TUNER_START;
+			state->calibrate &= ~WBD_CAL;
+			return 0;
+		}
+
+		dib0090_write_reg(state, 0x10, 0x1b81 | (1 << 10) | (wbd_gain << 13) | (1 << 3));
 
+		dib0090_write_reg(state, 0x24, ((EN_UHF & 0x0fff) | (1 << 1)));
 		*tune_state = CT_TUNER_STEP_0;
+		state->wbd_calibration_gain = wbd_gain;
 		return 90;	/* wait for the WBDMUX to switch and for the ADC to sample */
+
 	case CT_TUNER_STEP_0:
-		state->wbd_offset = dib0090_read_reg(state, 0x1d);
+		state->wbd_offset = dib0090_get_slow_adc_val(state);
 		dprintk("WBD calibration offset = %d", state->wbd_offset);
-
 		*tune_state = CT_TUNER_START;	/* reset done -> real tuning can now begin */
-		state->reset &= ~0x2;
+		state->calibrate &= ~WBD_CAL;
 		break;
+
 	default:
 		break;
 	}
@@ -1092,6 +1679,15 @@ static void dib0090_set_bandwidth(struct dib0090_state *state)
 	state->bb_1_def |= tmp;
 
 	dib0090_write_reg(state, 0x01, state->bb_1_def);	/* be sure that we have the right bb-filter */
+
+	dib0090_write_reg(state, 0x03, 0x6008);	/* = 0x6008 : vcm3_trim = 1 ; filter2_gm1_trim = 8 ; filter2_cutoff_freq = 0 */
+	dib0090_write_reg(state, 0x04, 0x1);	/* 0 = 1KHz ; 1 = 50Hz ; 2 = 150Hz ; 3 = 50KHz ; 4 = servo fast */
+	if (state->identity.in_soc) {
+		dib0090_write_reg(state, 0x05, 0x9bcf); /* attenuator_ibias_tri = 2 ; input_stage_ibias_tr = 1 ; nc = 11 ; ext_gm_trim = 1 ; obuf_ibias_trim = 4 ; filter13_gm2_ibias_t = 15 */
+	} else {
+		dib0090_write_reg(state, 0x02, (5 << 11) | (8 << 6) | (22 & 0x3f));	/* 22 = cap_value */
+		dib0090_write_reg(state, 0x05, 0xabcd);	/* = 0xabcd : attenuator_ibias_tri = 2 ; input_stage_ibias_tr = 2 ; nc = 11 ; ext_gm_trim = 1 ; obuf_ibias_trim = 4 ; filter13_gm2_ibias_t = 13 */
+	}
 }
 
 static const struct dib0090_pll dib0090_pll_table[] = {
@@ -1180,6 +1776,255 @@ static const struct dib0090_tuning dib0090_tuning_table[] = {
 #endif
 };
 
+static const struct dib0090_tuning dib0090_p1g_tuning_table[] = {
+#ifdef CONFIG_BAND_CBAND
+	{170000, 4, 1, 0x820f, 0x300, 0x2d22, 0x82cb, EN_CAB},
+#endif
+#ifdef CONFIG_BAND_VHF
+	{184000, 1, 1, 15, 0x300, 0x4d12, 0xb94e, EN_VHF},
+	{227000, 1, 3, 15, 0x300, 0x4d12, 0xb94e, EN_VHF},
+	{380000, 1, 7, 15, 0x300, 0x4d12, 0xb94e, EN_VHF},
+#endif
+#ifdef CONFIG_BAND_UHF
+	{510000, 2, 0, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{540000, 2, 1, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{600000, 2, 3, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{630000, 2, 4, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{680000, 2, 5, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{720000, 2, 6, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{900000, 2, 7, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+#endif
+#ifdef CONFIG_BAND_LBAND
+	{1500000, 4, 0, 20, 0x300, 0x1912, 0x82c9, EN_LBD},
+	{1600000, 4, 1, 20, 0x300, 0x1912, 0x82c9, EN_LBD},
+	{1800000, 4, 3, 20, 0x300, 0x1912, 0x82c9, EN_LBD},
+#endif
+#ifdef CONFIG_BAND_SBAND
+	{2300000, 1, 4, 20, 0x300, 0x2d2A, 0x82c7, EN_SBD},
+	{2900000, 1, 7, 20, 0x280, 0x2deb, 0x8347, EN_SBD},
+#endif
+};
+
+static const struct dib0090_pll dib0090_p1g_pll_table[] = {
+#ifdef CONFIG_BAND_CBAND
+	{57000, 0, 11, 48, 6},
+	{70000, 1, 11, 48, 6},
+	{86000, 0, 10, 32, 4},
+	{105000, 1, 10, 32, 4},
+	{115000, 0, 9, 24, 6},
+	{140000, 1, 9, 24, 6},
+	{170000, 0, 8, 16, 4},
+#endif
+#ifdef CONFIG_BAND_VHF
+	{200000, 1, 8, 16, 4},
+	{230000, 0, 7, 12, 6},
+	{280000, 1, 7, 12, 6},
+	{340000, 0, 6, 8, 4},
+	{380000, 1, 6, 8, 4},
+	{455000, 0, 5, 6, 6},
+#endif
+#ifdef CONFIG_BAND_UHF
+	{580000, 1, 5, 6, 6},
+	{680000, 0, 4, 4, 4},
+	{860000, 1, 4, 4, 4},
+#endif
+#ifdef CONFIG_BAND_LBAND
+	{1800000, 1, 2, 2, 4},
+#endif
+#ifdef CONFIG_BAND_SBAND
+	{2900000, 0, 1, 1, 6},
+#endif
+};
+
+static const struct dib0090_tuning dib0090_p1g_tuning_table_fm_vhf_on_cband[] = {
+#ifdef CONFIG_BAND_CBAND
+	{184000, 4, 3, 0x4187, 0x2c0, 0x2d22, 0x81cb, EN_CAB},
+	{227000, 4, 3, 0x4187, 0x2c0, 0x2d22, 0x81cb, EN_CAB},
+	{380000, 4, 3, 0x4187, 0x2c0, 0x2d22, 0x81cb, EN_CAB},
+#endif
+#ifdef CONFIG_BAND_UHF
+	{520000, 2, 0, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{550000, 2, 2, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{650000, 2, 3, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{750000, 2, 5, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{850000, 2, 6, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+	{900000, 2, 7, 15, 0x300, 0x1d12, 0xb9ce, EN_UHF},
+#endif
+#ifdef CONFIG_BAND_LBAND
+	{1500000, 4, 0, 20, 0x300, 0x1912, 0x82c9, EN_LBD},
+	{1600000, 4, 1, 20, 0x300, 0x1912, 0x82c9, EN_LBD},
+	{1800000, 4, 3, 20, 0x300, 0x1912, 0x82c9, EN_LBD},
+#endif
+#ifdef CONFIG_BAND_SBAND
+	{2300000, 1, 4, 20, 0x300, 0x2d2A, 0x82c7, EN_SBD},
+	{2900000, 1, 7, 20, 0x280, 0x2deb, 0x8347, EN_SBD},
+#endif
+};
+
+static const struct dib0090_tuning dib0090_tuning_table_cband_7090[] = {
+#ifdef CONFIG_BAND_CBAND
+	{300000, 4, 3, 0x018F, 0x2c0, 0x2d22, 0xb9ce, EN_CAB},
+	{380000, 4, 10, 0x018F, 0x2c0, 0x2d22, 0xb9ce, EN_CAB},
+	{570000, 4, 10, 0x8190, 0x2c0, 0x2d22, 0xb9ce, EN_CAB},
+	{858000, 4, 5, 0x8190, 0x2c0, 0x2d22, 0xb9ce, EN_CAB},
+#endif
+};
+
+static int dib0090_captrim_search(struct dib0090_state *state, enum frontend_tune_state *tune_state)
+{
+	int ret = 0;
+	u16 lo4 = 0xe900;
+
+	s16 adc_target;
+	u16 adc;
+	s8 step_sign;
+	u8 force_soft_search = 0;
+
+	if (state->identity.version == SOC_8090_P1G_11R1 || state->identity.version == SOC_8090_P1G_21R1)
+		force_soft_search = 1;
+
+	if (*tune_state == CT_TUNER_START) {
+		dprintk("Start Captrim search : %s", (force_soft_search == 1) ? "FORCE SOFT SEARCH" : "AUTO");
+		dib0090_write_reg(state, 0x10, 0x2B1);
+		dib0090_write_reg(state, 0x1e, 0x0032);
+
+		if (!state->tuner_is_tuned) {
+			/* prepare a complete captrim */
+			if (!state->identity.p1g || force_soft_search)
+				state->step = state->captrim = state->fcaptrim = 64;
+
+			state->current_rf = state->rf_request;
+		} else {	/* we are already tuned to this frequency - the configuration is correct  */
+			if (!state->identity.p1g || force_soft_search) {
+				/* do a minimal captrim even if the frequency has not changed */
+				state->step = 4;
+				state->captrim = state->fcaptrim = dib0090_read_reg(state, 0x18) & 0x7f;
+			}
+		}
+		state->adc_diff = 3000;
+		*tune_state = CT_TUNER_STEP_0;
+
+	} else if (*tune_state == CT_TUNER_STEP_0) {
+		if (state->identity.p1g && !force_soft_search) {
+			u8 ratio = 31;
+
+			dib0090_write_reg(state, 0x40, (3 << 7) | (ratio << 2) | (1 << 1) | 1);
+			dib0090_read_reg(state, 0x40);
+			ret = 50;
+		} else {
+			state->step /= 2;
+			dib0090_write_reg(state, 0x18, lo4 | state->captrim);
+
+			if (state->identity.in_soc)
+				ret = 25;
+		}
+		*tune_state = CT_TUNER_STEP_1;
+
+	} else if (*tune_state == CT_TUNER_STEP_1) {
+		if (state->identity.p1g && !force_soft_search) {
+			dib0090_write_reg(state, 0x40, 0x18c | (0 << 1) | 0);
+			dib0090_read_reg(state, 0x40);
+
+			state->fcaptrim = dib0090_read_reg(state, 0x18) & 0x7F;
+			dprintk("***Final Captrim= 0x%x", state->fcaptrim);
+			*tune_state = CT_TUNER_STEP_3;
+
+		} else {
+			/* MERGE for all krosus before P1G */
+			adc = dib0090_get_slow_adc_val(state);
+			dprintk("CAPTRIM=%d; ADC = %d (ADC) & %dmV", (u32) state->captrim, (u32) adc, (u32) (adc) * (u32) 1800 / (u32) 1024);
+
+			if (state->rest == 0 || state->identity.in_soc) {	/* Just for 8090P SOCS where auto captrim HW bug : TO CHECK IN ACI for SOCS !!! if 400 for 8090p SOC => tune issue !!! */
+				adc_target = 200;
+			} else
+				adc_target = 400;
+
+			if (adc >= adc_target) {
+				adc -= adc_target;
+				step_sign = -1;
+			} else {
+				adc = adc_target - adc;
+				step_sign = 1;
+			}
+
+			if (adc < state->adc_diff) {
+				dprintk("CAPTRIM=%d is closer to target (%d/%d)", (u32) state->captrim, (u32) adc, (u32) state->adc_diff);
+				state->adc_diff = adc;
+				state->fcaptrim = state->captrim;
+			}
+
+			state->captrim += step_sign * state->step;
+			if (state->step >= 1)
+				*tune_state = CT_TUNER_STEP_0;
+			else
+				*tune_state = CT_TUNER_STEP_2;
+
+			ret = 25;
+		}
+	} else if (*tune_state == CT_TUNER_STEP_2) {	/* this step is only used by krosus < P1G */
+		/*write the final cptrim config */
+		dib0090_write_reg(state, 0x18, lo4 | state->fcaptrim);
+
+		*tune_state = CT_TUNER_STEP_3;
+
+	} else if (*tune_state == CT_TUNER_STEP_3) {
+		state->calibrate &= ~CAPTRIM_CAL;
+		*tune_state = CT_TUNER_STEP_0;
+	}
+
+	return ret;
+}
+
+static int dib0090_get_temperature(struct dib0090_state *state, enum frontend_tune_state *tune_state)
+{
+	int ret = 15;
+	s16 val;
+
+	switch (*tune_state) {
+	case CT_TUNER_START:
+		state->wbdmux = dib0090_read_reg(state, 0x10);
+		dib0090_write_reg(state, 0x10, (state->wbdmux & ~(0xff << 3)) | (0x8 << 3));
+
+		state->bias = dib0090_read_reg(state, 0x13);
+		dib0090_write_reg(state, 0x13, state->bias | (0x3 << 8));
+
+		*tune_state = CT_TUNER_STEP_0;
+		/* wait for the WBDMUX to switch and for the ADC to sample */
+		break;
+
+	case CT_TUNER_STEP_0:
+		state->adc_diff = dib0090_get_slow_adc_val(state);
+		dib0090_write_reg(state, 0x13, (state->bias & ~(0x3 << 8)) | (0x2 << 8));
+		*tune_state = CT_TUNER_STEP_1;
+		break;
+
+	case CT_TUNER_STEP_1:
+		val = dib0090_get_slow_adc_val(state);
+		state->temperature = ((s16) ((val - state->adc_diff) * 180) >> 8) + 55;
+
+		dprintk("temperature: %d C", state->temperature - 30);
+
+		*tune_state = CT_TUNER_STEP_2;
+		break;
+
+	case CT_TUNER_STEP_2:
+		dib0090_write_reg(state, 0x13, state->bias);
+		dib0090_write_reg(state, 0x10, state->wbdmux);	/* write back original WBDMUX */
+
+		*tune_state = CT_TUNER_START;
+		state->calibrate &= ~TEMP_CAL;
+		if (state->config->analog_output == 0)
+			dib0090_write_reg(state, 0x23, dib0090_read_reg(state, 0x23) | (1 << 14));
+
+		break;
+
+	default:
+		ret = 0;
+		break;
+	}
+	return ret;
+}
+
 #define WBD     0x781		/* 1 1 1 1 0000 0 0 1 */
 static int dib0090_tune(struct dvb_frontend *fe)
 {
@@ -1188,87 +2033,131 @@ static int dib0090_tune(struct dvb_frontend *fe)
 	const struct dib0090_pll *pll = state->current_pll_table_index;
 	enum frontend_tune_state *tune_state = &state->tune_state;
 
-	u32 rf;
-	u16 lo4 = 0xe900, lo5, lo6, Den;
+	u16 lo5, lo6, Den, tmp;
 	u32 FBDiv, Rest, FREF, VCOF_kHz = 0;
-	u16 tmp, adc;
-	int8_t step_sign;
 	int ret = 10;		/* 1ms is the default delay most of the time */
 	u8 c, i;
 
-	state->current_band = (u8) BAND_OF_FREQUENCY(fe->dtv_property_cache.frequency / 1000);
-	rf = fe->dtv_property_cache.frequency / 1000 + (state->current_band ==
-							BAND_UHF ? state->config->freq_offset_khz_uhf : state->config->freq_offset_khz_vhf);
-	/* in any case we first need to do a reset if needed */
-	if (state->reset & 0x1)
-		return dib0090_dc_offset_calibration(state, tune_state);
-	else if (state->reset & 0x2)
-		return dib0090_wbd_calibration(state, tune_state);
-
-    /************************* VCO ***************************/
+	/************************* VCO ***************************/
 	/* Default values for FG                                 */
 	/* from these are needed :                               */
 	/* Cp,HFdiv,VCOband,SD,Num,Den,FB and REFDiv             */
 
-#ifdef CONFIG_SYS_ISDBT
-	if (state->fe->dtv_property_cache.delivery_system == SYS_ISDBT && state->fe->dtv_property_cache.isdbt_sb_mode == 1)
-		rf += 850;
-#endif
+	/* in any case we first need to do a calibration if needed */
+	if (*tune_state == CT_TUNER_START) {
+		/* deactivate DataTX before some calibrations */
+		if (state->calibrate & (DC_CAL | TEMP_CAL | WBD_CAL))
+			dib0090_write_reg(state, 0x23, dib0090_read_reg(state, 0x23) & ~(1 << 14));
+		else
+			/* Activate DataTX in case a calibration has been done before */
+			if (state->config->analog_output == 0)
+				dib0090_write_reg(state, 0x23, dib0090_read_reg(state, 0x23) | (1 << 14));
+	}
 
-	if (state->current_rf != rf) {
-		state->tuner_is_tuned = 0;
+	if (state->calibrate & DC_CAL)
+		return dib0090_dc_offset_calibration(state, tune_state);
+	else if (state->calibrate & WBD_CAL) {
+		if (state->current_rf == 0)
+			state->current_rf = state->fe->dtv_property_cache.frequency / 1000;
+		return dib0090_wbd_calibration(state, tune_state);
+	} else if (state->calibrate & TEMP_CAL)
+		return dib0090_get_temperature(state, tune_state);
+	else if (state->calibrate & CAPTRIM_CAL)
+		return dib0090_captrim_search(state, tune_state);
 
-		tune = dib0090_tuning_table;
+	if (*tune_state == CT_TUNER_START) {
+		/* if soc and AGC pwm control, disengage mux to be able to R/W access to 0x01 register to set the right filter (cutoff_freq_select) during the tune sequence, otherwise, SOC SERPAR error when accessing to 0x01 */
+		if (state->config->use_pwm_agc && state->identity.in_soc) {
+			tmp = dib0090_read_reg(state, 0x39);
+			if ((tmp >> 10) & 0x1)
+				dib0090_write_reg(state, 0x39, tmp & ~(1 << 10));
+		}
 
-		tmp = (state->revision >> 5) & 0x7;
-		if (tmp == 0x4 || tmp == 0x7) {
-			/* CBAND tuner version for VHF */
-			if (state->current_band == BAND_FM || state->current_band == BAND_VHF) {
-				/* Force CBAND */
-				state->current_band = BAND_CBAND;
-				tune = dib0090_tuning_table_fm_vhf_on_cband;
+		state->current_band = (u8) BAND_OF_FREQUENCY(state->fe->dtv_property_cache.frequency / 1000);
+		state->rf_request =
+			state->fe->dtv_property_cache.frequency / 1000 + (state->current_band ==
+					BAND_UHF ? state->config->freq_offset_khz_uhf : state->config->
+					freq_offset_khz_vhf);
+
+		/* in ISDB-T 1seg we shift tuning frequency */
+		if ((state->fe->dtv_property_cache.delivery_system == SYS_ISDBT && state->fe->dtv_property_cache.isdbt_sb_mode == 1
+					&& state->fe->dtv_property_cache.isdbt_partial_reception == 0)) {
+			const struct dib0090_low_if_offset_table *LUT_offset = state->config->low_if;
+			u8 found_offset = 0;
+			u32 margin_khz = 100;
+
+			if (LUT_offset != NULL) {
+				while (LUT_offset->RF_freq != 0xffff) {
+					if (((state->rf_request > (LUT_offset->RF_freq - margin_khz))
+								&& (state->rf_request < (LUT_offset->RF_freq + margin_khz)))
+							&& LUT_offset->std == state->fe->dtv_property_cache.delivery_system) {
+						state->rf_request += LUT_offset->offset_khz;
+						found_offset = 1;
+						break;
+					}
+					LUT_offset++;
+				}
 			}
+
+			if (found_offset == 0)
+				state->rf_request += 400;
 		}
+		if (state->current_rf != state->rf_request || (state->current_standard != state->fe->dtv_property_cache.delivery_system)) {
+			state->tuner_is_tuned = 0;
+			state->current_rf = 0;
+			state->current_standard = 0;
 
-		pll = dib0090_pll_table;
-		/* Look for the interval */
-		while (rf > tune->max_freq)
-			tune++;
-		while (rf > pll->max_freq)
-			pll++;
-		state->current_tune_table_index = tune;
-		state->current_pll_table_index = pll;
-	}
+			tune = dib0090_tuning_table;
+			if (state->identity.p1g)
+				tune = dib0090_p1g_tuning_table;
 
-	if (*tune_state == CT_TUNER_START) {
+			tmp = (state->identity.version >> 5) & 0x7;
 
-		if (state->tuner_is_tuned == 0)
-			state->current_rf = 0;
+			if (state->identity.in_soc) {
+				if (state->config->force_cband_input) {	/* Use the CBAND input for all band */
+					if (state->current_band & BAND_CBAND || state->current_band & BAND_FM || state->current_band & BAND_VHF
+							|| state->current_band & BAND_UHF) {
+						state->current_band = BAND_CBAND;
+						tune = dib0090_tuning_table_cband_7090;
+					}
+				} else {	/* Use the CBAND input for all band under UHF */
+					if (state->current_band & BAND_CBAND || state->current_band & BAND_FM || state->current_band & BAND_VHF) {
+						state->current_band = BAND_CBAND;
+						tune = dib0090_tuning_table_cband_7090;
+					}
+				}
+			} else
+			 if (tmp == 0x4 || tmp == 0x7) {
+				/* CBAND tuner version for VHF */
+				if (state->current_band == BAND_FM || state->current_band == BAND_CBAND || state->current_band == BAND_VHF) {
+					state->current_band = BAND_CBAND;	/* Force CBAND */
+
+					tune = dib0090_tuning_table_fm_vhf_on_cband;
+					if (state->identity.p1g)
+						tune = dib0090_p1g_tuning_table_fm_vhf_on_cband;
+				}
+			}
 
-		if (state->current_rf != rf) {
+			pll = dib0090_pll_table;
+			if (state->identity.p1g)
+				pll = dib0090_p1g_pll_table;
 
-			dib0090_write_reg(state, 0x0b, 0xb800 | (tune->switch_trim));
+			/* Look for the interval */
+			while (state->rf_request > tune->max_freq)
+				tune++;
+			while (state->rf_request > pll->max_freq)
+				pll++;
 
-			/* external loop filter, otherwise:
-			 * lo5 = (0 << 15) | (0 << 12) | (0 << 11) | (3 << 9) | (4 << 6) | (3 << 4) | 4;
-			 * lo6 = 0x0e34 */
-			if (pll->vco_band)
-				lo5 = 0x049e;
-			else if (state->config->analog_output)
-				lo5 = 0x041d;
-			else
-				lo5 = 0x041c;
-
-			lo5 |= (pll->hfdiv_code << 11) | (pll->vco_band << 7);	/* bit 15 is the split to the slave, we do not do it here */
+			state->current_tune_table_index = tune;
+			state->current_pll_table_index = pll;
 
-			if (!state->config->io.pll_int_loop_filt)
-				lo6 = 0xff28;
-			else
-				lo6 = (state->config->io.pll_int_loop_filt << 3);
+			dib0090_write_reg(state, 0x0b, 0xb800 | (tune->switch_trim));
 
-			VCOF_kHz = (pll->hfdiv * rf) * 2;
+			VCOF_kHz = (pll->hfdiv * state->rf_request) * 2;
 
 			FREF = state->config->io.clock_khz;
+			if (state->config->fref_clock_ratio != 0)
+				FREF /= state->config->fref_clock_ratio;
 
 			FBDiv = (VCOF_kHz / pll->topresc / FREF);
 			Rest = (VCOF_kHz / pll->topresc) - FBDiv * FREF;
@@ -1283,144 +2172,132 @@ static int dib0090_tune(struct dvb_frontend *fe)
 			} else if (Rest > (FREF - 2 * LPF))
 				Rest = FREF - 2 * LPF;
 			Rest = (Rest * 6528) / (FREF / 10);
+			state->rest = Rest;
 
-			Den = 1;
+			/* external loop filter, otherwise:
+			 * lo5 = (0 << 15) | (0 << 12) | (0 << 11) | (3 << 9) | (4 << 6) | (3 << 4) | 4;
+			 * lo6 = 0x0e34 */
+
+			if (Rest == 0) {
+				if (pll->vco_band)
+					lo5 = 0x049f;
+				else
+					lo5 = 0x041f;
+			} else {
+				if (pll->vco_band)
+					lo5 = 0x049e;
+				else if (state->config->analog_output)
+					lo5 = 0x041d;
+				else
+					lo5 = 0x041c;
+			}
+
+			if (state->identity.p1g) {	/* Bias is done automatically in P1G */
+				if (state->identity.in_soc) {
+					if (state->identity.version == SOC_8090_P1G_11R1)
+						lo5 = 0x46f;
+					else
+						lo5 = 0x42f;
+				} else
+					lo5 = 0x42c;
+			}
+
+			lo5 |= (pll->hfdiv_code << 11) | (pll->vco_band << 7);	/* bit 15 is the split to the slave, we do not do it here */
 
-			dprintk(" *****  ******* Rest value = %d", Rest);
+			if (!state->config->io.pll_int_loop_filt) {
+				if (state->identity.in_soc)
+					lo6 = 0xff98;
+				else if (state->identity.p1g || (Rest == 0))
+					lo6 = 0xfff8;
+				else
+					lo6 = 0xff28;
+			} else
+				lo6 = (state->config->io.pll_int_loop_filt << 3);
+
+			Den = 1;
 
 			if (Rest > 0) {
 				if (state->config->analog_output)
 					lo6 |= (1 << 2) | 2;
-				else
-					lo6 |= (1 << 2) | 1;
+				else {
+					if (state->identity.in_soc)
+						lo6 |= (1 << 2) | 2;
+					else
+						lo6 |= (1 << 2) | 2;
+				}
 				Den = 255;
 			}
-#ifdef CONFIG_BAND_SBAND
-			if (state->current_band == BAND_SBAND)
-				lo6 &= 0xfffb;
-#endif
-
 			dib0090_write_reg(state, 0x15, (u16) FBDiv);
-
-			dib0090_write_reg(state, 0x16, (Den << 8) | 1);
-
+			if (state->config->fref_clock_ratio != 0)
+				dib0090_write_reg(state, 0x16, (Den << 8) | state->config->fref_clock_ratio);
+			else
+				dib0090_write_reg(state, 0x16, (Den << 8) | 1);
 			dib0090_write_reg(state, 0x17, (u16) Rest);
-
 			dib0090_write_reg(state, 0x19, lo5);
-
 			dib0090_write_reg(state, 0x1c, lo6);
 
 			lo6 = tune->tuner_enable;
 			if (state->config->analog_output)
 				lo6 = (lo6 & 0xff9f) | 0x2;
 
-			dib0090_write_reg(state, 0x24, lo6 | EN_LO
-#ifdef CONFIG_DIB0090_USE_PWM_AGC
-					  | state->config->use_pwm_agc * EN_CRYSTAL
-#endif
-			    );
-
-			state->current_rf = rf;
-
-			/* prepare a complete captrim */
-			state->step = state->captrim = state->fcaptrim = 64;
-
-		} else {	/* we are already tuned to this frequency - the configuration is correct  */
+			dib0090_write_reg(state, 0x24, lo6 | EN_LO | state->config->use_pwm_agc * EN_CRYSTAL);
 
-			/* do a minimal captrim even if the frequency has not changed */
-			state->step = 4;
-			state->captrim = state->fcaptrim = dib0090_read_reg(state, 0x18) & 0x7f;
 		}
-		state->adc_diff = 3000;
-
-		dib0090_write_reg(state, 0x10, 0x2B1);
 
-		dib0090_write_reg(state, 0x1e, 0x0032);
+		state->current_rf = state->rf_request;
+		state->current_standard = state->fe->dtv_property_cache.delivery_system;
 
 		ret = 20;
-		*tune_state = CT_TUNER_STEP_1;
-	} else if (*tune_state == CT_TUNER_STEP_0) {
-		/* nothing */
-	} else if (*tune_state == CT_TUNER_STEP_1) {
-		state->step /= 2;
-		dib0090_write_reg(state, 0x18, lo4 | state->captrim);
-		*tune_state = CT_TUNER_STEP_2;
-	} else if (*tune_state == CT_TUNER_STEP_2) {
+		state->calibrate = CAPTRIM_CAL;	/* captrim serach now */
+	}
 
-		adc = dib0090_read_reg(state, 0x1d);
-		dprintk("FE %d CAPTRIM=%d; ADC = %d (ADC) & %dmV", (u32) fe->id, (u32) state->captrim, (u32) adc,
-			(u32) (adc) * (u32) 1800 / (u32) 1024);
+	else if (*tune_state == CT_TUNER_STEP_0) {	/* Warning : because of captrim cal, if you change this step, change it also in _cal.c file because it is the step following captrim cal state machine */
+		const struct dib0090_wbd_slope *wbd = state->current_wbd_table;
 
-		if (adc >= 400) {
-			adc -= 400;
-			step_sign = -1;
-		} else {
-			adc = 400 - adc;
-			step_sign = 1;
-		}
+		while (state->current_rf / 1000 > wbd->max_freq)
+			wbd++;
 
-		if (adc < state->adc_diff) {
-			dprintk("FE %d CAPTRIM=%d is closer to target (%d/%d)", (u32) fe->id, (u32) state->captrim, (u32) adc, (u32) state->adc_diff);
-			state->adc_diff = adc;
-			state->fcaptrim = state->captrim;
-
-		}
+		dib0090_write_reg(state, 0x1e, 0x07ff);
+		dprintk("Final Captrim: %d", (u32) state->fcaptrim);
+		dprintk("HFDIV code: %d", (u32) pll->hfdiv_code);
+		dprintk("VCO = %d", (u32) pll->vco_band);
+		dprintk("VCOF in kHz: %d ((%d*%d) << 1))", (u32) ((pll->hfdiv * state->rf_request) * 2), (u32) pll->hfdiv, (u32) state->rf_request);
+		dprintk("REFDIV: %d, FREF: %d", (u32) 1, (u32) state->config->io.clock_khz);
+		dprintk("FBDIV: %d, Rest: %d", (u32) dib0090_read_reg(state, 0x15), (u32) dib0090_read_reg(state, 0x17));
+		dprintk("Num: %d, Den: %d, SD: %d", (u32) dib0090_read_reg(state, 0x17), (u32) (dib0090_read_reg(state, 0x16) >> 8),
+			(u32) dib0090_read_reg(state, 0x1c) & 0x3);
 
-		state->captrim += step_sign * state->step;
-		if (state->step >= 1)
-			*tune_state = CT_TUNER_STEP_1;
-		else
-			*tune_state = CT_TUNER_STEP_3;
+#define WBD     0x781		/* 1 1 1 1 0000 0 0 1 */
+		c = 4;
+		i = 3;
 
-		ret = 15;
-	} else if (*tune_state == CT_TUNER_STEP_3) {
-		/*write the final cptrim config */
-		dib0090_write_reg(state, 0x18, lo4 | state->fcaptrim);
+		if (wbd->wbd_gain != 0)
+			c = wbd->wbd_gain;
 
-#ifdef CONFIG_TUNER_DIB0090_CAPTRIM_MEMORY
-		state->memory[state->memory_index].cap = state->fcaptrim;
-#endif
+		state->wbdmux = (c << 13) | (i << 11) | (WBD | (state->config->use_pwm_agc << 1));
+		dib0090_write_reg(state, 0x10, state->wbdmux);
 
-		*tune_state = CT_TUNER_STEP_4;
-	} else if (*tune_state == CT_TUNER_STEP_4) {
-		dib0090_write_reg(state, 0x1e, 0x07ff);
-
-		dprintk("FE %d Final Captrim: %d", (u32) fe->id, (u32) state->fcaptrim);
-		dprintk("FE %d HFDIV code: %d", (u32) fe->id, (u32) pll->hfdiv_code);
-		dprintk("FE %d VCO = %d", (u32) fe->id, (u32) pll->vco_band);
-		dprintk("FE %d VCOF in kHz: %d ((%d*%d) << 1))", (u32) fe->id, (u32) ((pll->hfdiv * rf) * 2), (u32) pll->hfdiv, (u32) rf);
-		dprintk("FE %d REFDIV: %d, FREF: %d", (u32) fe->id, (u32) 1, (u32) state->config->io.clock_khz);
-		dprintk("FE %d FBDIV: %d, Rest: %d", (u32) fe->id, (u32) dib0090_read_reg(state, 0x15), (u32) dib0090_read_reg(state, 0x17));
-		dprintk("FE %d Num: %d, Den: %d, SD: %d", (u32) fe->id, (u32) dib0090_read_reg(state, 0x17),
-			(u32) (dib0090_read_reg(state, 0x16) >> 8), (u32) dib0090_read_reg(state, 0x1c) & 0x3);
+		if ((tune->tuner_enable == EN_CAB) && state->identity.p1g) {
+			dprintk("P1G : The cable band is selected and lna_tune = %d", tune->lna_tune);
+			dib0090_write_reg(state, 0x09, tune->lna_bias);
+			dib0090_write_reg(state, 0x0b, 0xb800 | (tune->lna_tune << 6) | (tune->switch_trim));
+		} else
+			dib0090_write_reg(state, 0x09, (tune->lna_tune << 5) | tune->lna_bias);
 
-		c = 4;
-		i = 3;
-#if defined(CONFIG_BAND_LBAND) || defined(CONFIG_BAND_SBAND)
-		if ((state->current_band == BAND_LBAND) || (state->current_band == BAND_SBAND)) {
-			c = 2;
-			i = 2;
-		}
-#endif
-		dib0090_write_reg(state, 0x10, (c << 13) | (i << 11) | (WBD
-#ifdef CONFIG_DIB0090_USE_PWM_AGC
-									| (state->config->use_pwm_agc << 1)
-#endif
-				  ));
-		dib0090_write_reg(state, 0x09, (tune->lna_tune << 5) | (tune->lna_bias << 0));
 		dib0090_write_reg(state, 0x0c, tune->v2i);
 		dib0090_write_reg(state, 0x0d, tune->mix);
 		dib0090_write_reg(state, 0x0e, tune->load);
+		*tune_state = CT_TUNER_STEP_1;
 
-		*tune_state = CT_TUNER_STEP_5;
-	} else if (*tune_state == CT_TUNER_STEP_5) {
-
+	} else if (*tune_state == CT_TUNER_STEP_1) {
 		/* initialize the lt gain register */
 		state->rf_lt_def = 0x7c00;
-		dib0090_write_reg(state, 0x0f, state->rf_lt_def);
 
 		dib0090_set_bandwidth(state);
 		state->tuner_is_tuned = 1;
+
+		state->calibrate |= WBD_CAL;
+		state->calibrate |= TEMP_CAL;
 		*tune_state = CT_TUNER_STOP;
 	} else
 		ret = FE_CALLBACK_TIME_NEVER;
@@ -1440,6 +2317,7 @@ enum frontend_tune_state dib0090_get_tune_state(struct dvb_frontend *fe)
 
 	return state->tune_state;
 }
+
 EXPORT_SYMBOL(dib0090_get_tune_state);
 
 int dib0090_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
@@ -1449,6 +2327,7 @@ int dib0090_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tun
 	state->tune_state = tune_state;
 	return 0;
 }
+
 EXPORT_SYMBOL(dib0090_set_tune_state);
 
 static int dib0090_get_frequency(struct dvb_frontend *fe, u32 * frequency)
@@ -1462,7 +2341,7 @@ static int dib0090_get_frequency(struct dvb_frontend *fe, u32 * frequency)
 static int dib0090_set_params(struct dvb_frontend *fe, struct dvb_frontend_parameters *p)
 {
 	struct dib0090_state *state = fe->tuner_priv;
-	uint32_t ret;
+	u32 ret;
 
 	state->tune_state = CT_TUNER_START;
 
@@ -1492,6 +2371,29 @@ static const struct dvb_tuner_ops dib0090_ops = {
 	.get_frequency = dib0090_get_frequency,
 };
 
+static const struct dvb_tuner_ops dib0090_fw_ops = {
+	.info = {
+		 .name = "DiBcom DiB0090",
+		 .frequency_min = 45000000,
+		 .frequency_max = 860000000,
+		 .frequency_step = 1000,
+		 },
+	.release = dib0090_release,
+
+	.init = NULL,
+	.sleep = NULL,
+	.set_params = NULL,
+	.get_frequency = NULL,
+};
+
+static const struct dib0090_wbd_slope dib0090_wbd_table_default[] = {
+	{470, 0, 250, 0, 100, 4},
+	{860, 51, 866, 21, 375, 4},
+	{1700, 0, 800, 0, 850, 4},
+	{2900, 0, 250, 0, 100, 6},
+	{0xFFFF, 0, 0, 0, 0, 0},
+};
+
 struct dvb_frontend *dib0090_register(struct dvb_frontend *fe, struct i2c_adapter *i2c, const struct dib0090_config *config)
 {
 	struct dib0090_state *st = kzalloc(sizeof(struct dib0090_state), GFP_KERNEL);
@@ -1503,6 +2405,11 @@ struct dvb_frontend *dib0090_register(struct dvb_frontend *fe, struct i2c_adapte
 	st->fe = fe;
 	fe->tuner_priv = st;
 
+	if (config->wbd == NULL)
+		st->current_wbd_table = dib0090_wbd_table_default;
+	else
+		st->current_wbd_table = config->wbd;
+
 	if (dib0090_reset(fe) != 0)
 		goto free_mem;
 
@@ -1515,8 +2422,34 @@ struct dvb_frontend *dib0090_register(struct dvb_frontend *fe, struct i2c_adapte
 	fe->tuner_priv = NULL;
 	return NULL;
 }
+
 EXPORT_SYMBOL(dib0090_register);
 
+struct dvb_frontend *dib0090_fw_register(struct dvb_frontend *fe, struct i2c_adapter *i2c, const struct dib0090_config *config)
+{
+	struct dib0090_fw_state *st = kzalloc(sizeof(struct dib0090_fw_state), GFP_KERNEL);
+	if (st == NULL)
+		return NULL;
+
+	st->config = config;
+	st->i2c = i2c;
+	st->fe = fe;
+	fe->tuner_priv = st;
+
+	if (dib0090_fw_reset_digital(fe, st->config) != 0)
+		goto free_mem;
+
+	dprintk("DiB0090 FW: successfully identified");
+	memcpy(&fe->ops.tuner_ops, &dib0090_fw_ops, sizeof(struct dvb_tuner_ops));
+
+	return fe;
+free_mem:
+	kfree(st);
+	fe->tuner_priv = NULL;
+	return NULL;
+}
+EXPORT_SYMBOL(dib0090_fw_register);
+
 MODULE_AUTHOR("Patrick Boettcher <pboettcher@dibcom.fr>");
 MODULE_AUTHOR("Olivier Grenie <olivier.grenie@dibcom.fr>");
 MODULE_DESCRIPTION("Driver for the DiBcom 0090 base-band RF Tuner");
diff --git a/drivers/media/dvb/frontends/dib0090.h b/drivers/media/dvb/frontends/dib0090.h
index aa7711e88776..13d85244ec16 100644
--- a/drivers/media/dvb/frontends/dib0090.h
+++ b/drivers/media/dvb/frontends/dib0090.h
@@ -27,6 +27,21 @@ struct dib0090_io_config {
 	u16 pll_int_loop_filt;
 };
 
+struct dib0090_wbd_slope {
+	u16 max_freq;		/* for every frequency less than or equal to that field: this information is correct */
+	u16 slope_cold;
+	u16 offset_cold;
+	u16 slope_hot;
+	u16 offset_hot;
+	u8 wbd_gain;
+};
+
+struct dib0090_low_if_offset_table {
+	int std;
+	u32 RF_freq;
+	s32 offset_khz;
+};
+
 struct dib0090_config {
 	struct dib0090_io_config io;
 	int (*reset) (struct dvb_frontend *, int);
@@ -47,10 +62,20 @@ struct dib0090_config {
 	u16 wbd_cband_offset;
 	u8 use_pwm_agc;
 	u8 clkoutdrive;
+
+	u8 ls_cfg_pad_drv;
+	u8 data_tx_drv;
+
+	u8 in_soc;
+	const struct dib0090_low_if_offset_table *low_if;
+	u8 fref_clock_ratio;
+	u16 force_cband_input;
+	struct dib0090_wbd_slope *wbd;
 };
 
 #if defined(CONFIG_DVB_TUNER_DIB0090) || (defined(CONFIG_DVB_TUNER_DIB0090_MODULE) && defined(MODULE))
 extern struct dvb_frontend *dib0090_register(struct dvb_frontend *fe, struct i2c_adapter *i2c, const struct dib0090_config *config);
+extern struct dvb_frontend *dib0090_fw_register(struct dvb_frontend *fe, struct i2c_adapter *i2c, const struct dib0090_config *config);
 extern void dib0090_dcc_freq(struct dvb_frontend *fe, u8 fast);
 extern void dib0090_pwm_gain_reset(struct dvb_frontend *fe);
 extern u16 dib0090_get_wbd_offset(struct dvb_frontend *tuner);
@@ -65,6 +90,12 @@ static inline struct dvb_frontend *dib0090_register(struct dvb_frontend *fe, str
 	return NULL;
 }
 
+static inline struct dvb_frontend *dib0090_fw_register(struct dvb_frontend *fe, struct i2c_adapter *i2c, struct dib0090_config *config)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
 static inline void dib0090_dcc_freq(struct dvb_frontend *fe, u8 fast)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
diff --git a/drivers/media/dvb/frontends/dib7000m.c b/drivers/media/dvb/frontends/dib7000m.c
index c7f5ccf54aa5..289a79837f24 100644
--- a/drivers/media/dvb/frontends/dib7000m.c
+++ b/drivers/media/dvb/frontends/dib7000m.c
@@ -1285,6 +1285,25 @@ struct i2c_adapter * dib7000m_get_i2c_master(struct dvb_frontend *demod, enum di
 }
 EXPORT_SYMBOL(dib7000m_get_i2c_master);
 
+int dib7000m_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
+{
+	struct dib7000m_state *state = fe->demodulator_priv;
+	u16 val = dib7000m_read_word(state, 294 + state->reg_offs) & 0xffef;
+	val |= (onoff & 0x1) << 4;
+	dprintk("PID filter enabled %d", onoff);
+	return dib7000m_write_word(state, 294 + state->reg_offs, val);
+}
+EXPORT_SYMBOL(dib7000m_pid_filter_ctrl);
+
+int dib7000m_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
+{
+	struct dib7000m_state *state = fe->demodulator_priv;
+	dprintk("PID filter: index %x, PID %d, OnOff %d", id, pid, onoff);
+	return dib7000m_write_word(state, 300 + state->reg_offs + id,
+			onoff ? (1 << 13) | pid : 0);
+}
+EXPORT_SYMBOL(dib7000m_pid_filter);
+
 #if 0
 /* used with some prototype boards */
 int dib7000m_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods,
diff --git a/drivers/media/dvb/frontends/dib7000m.h b/drivers/media/dvb/frontends/dib7000m.h
index 113819ce9f0d..81fcf2241c64 100644
--- a/drivers/media/dvb/frontends/dib7000m.h
+++ b/drivers/media/dvb/frontends/dib7000m.h
@@ -46,6 +46,8 @@ extern struct dvb_frontend *dib7000m_attach(struct i2c_adapter *i2c_adap,
 extern struct i2c_adapter *dib7000m_get_i2c_master(struct dvb_frontend *,
 						   enum dibx000_i2c_interface,
 						   int);
+extern int dib7000m_pid_filter(struct dvb_frontend *, u8 id, u16 pid, u8 onoff);
+extern int dib7000m_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff);
 #else
 static inline
 struct dvb_frontend *dib7000m_attach(struct i2c_adapter *i2c_adap,
@@ -63,6 +65,19 @@ struct i2c_adapter *dib7000m_get_i2c_master(struct dvb_frontend *demod,
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
+static inline int dib7000m_pid_filter(struct dvb_frontend *fe, u8 id,
+						u16 pid, u8 onoff)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib7000m_pid_filter_ctrl(struct dvb_frontend *fe,
+						uint8_t onoff)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
 #endif
 
 /* TODO
diff --git a/drivers/media/dvb/frontends/dib7000p.c b/drivers/media/dvb/frontends/dib7000p.c
index 6aa02cb80733..900af60b9d36 100644
--- a/drivers/media/dvb/frontends/dib7000p.c
+++ b/drivers/media/dvb/frontends/dib7000p.c
@@ -26,24 +26,29 @@ MODULE_PARM_DESC(buggy_sfn_workaround, "Enable work-around for buggy SFNs (defau
 
 #define dprintk(args...) do { if (debug) { printk(KERN_DEBUG "DiB7000P: "); printk(args); printk("\n"); } } while (0)
 
+struct i2c_device {
+	struct i2c_adapter *i2c_adap;
+	u8 i2c_addr;
+};
+
 struct dib7000p_state {
 	struct dvb_frontend demod;
-    struct dib7000p_config cfg;
+	struct dib7000p_config cfg;
 
 	u8 i2c_addr;
-	struct i2c_adapter   *i2c_adap;
+	struct i2c_adapter *i2c_adap;
 
 	struct dibx000_i2c_master i2c_master;
 
 	u16 wbd_ref;
 
-	u8  current_band;
+	u8 current_band;
 	u32 current_bandwidth;
 	struct dibx000_agc_config *current_agc;
 	u32 timf;
 
-	u8 div_force_off : 1;
-	u8 div_state : 1;
+	u8 div_force_off:1;
+	u8 div_state:1;
 	u16 div_sync_wait;
 
 	u8 agc_state;
@@ -51,7 +56,13 @@ struct dib7000p_state {
 	u16 gpio_dir;
 	u16 gpio_val;
 
-	u8 sfn_workaround_active :1;
+	u8 sfn_workaround_active:1;
+
+#define SOC7090 0x7090
+	u16 version;
+
+	u16 tuner_enable;
+	struct i2c_adapter dib7090_tuner_adap;
 };
 
 enum dib7000p_power_mode {
@@ -60,17 +71,20 @@ enum dib7000p_power_mode {
 	DIB7000P_POWER_INTERFACE_ONLY,
 };
 
+static int dib7090_set_output_mode(struct dvb_frontend *fe, int mode);
+static int dib7090_set_diversity_in(struct dvb_frontend *fe, int onoff);
+
 static u16 dib7000p_read_word(struct dib7000p_state *state, u16 reg)
 {
 	u8 wb[2] = { reg >> 8, reg & 0xff };
 	u8 rb[2];
 	struct i2c_msg msg[2] = {
-		{ .addr = state->i2c_addr >> 1, .flags = 0,        .buf = wb, .len = 2 },
-		{ .addr = state->i2c_addr >> 1, .flags = I2C_M_RD, .buf = rb, .len = 2 },
+		{.addr = state->i2c_addr >> 1, .flags = 0, .buf = wb, .len = 2},
+		{.addr = state->i2c_addr >> 1, .flags = I2C_M_RD, .buf = rb, .len = 2},
 	};
 
 	if (i2c_transfer(state->i2c_adap, msg, 2) != 2)
-		dprintk("i2c read error on %d",reg);
+		dprintk("i2c read error on %d", reg);
 
 	return (rb[0] << 8) | rb[1];
 }
@@ -86,7 +100,8 @@ static int dib7000p_write_word(struct dib7000p_state *state, u16 reg, u16 val)
 	};
 	return i2c_transfer(state->i2c_adap, &msg, 1) != 1 ? -EREMOTEIO : 0;
 }
-static void dib7000p_write_tab(struct dib7000p_state *state, u16 *buf)
+
+static void dib7000p_write_tab(struct dib7000p_state *state, u16 * buf)
 {
 	u16 l = 0, r, *n;
 	n = buf;
@@ -104,54 +119,54 @@ static void dib7000p_write_tab(struct dib7000p_state *state, u16 *buf)
 
 static int dib7000p_set_output_mode(struct dib7000p_state *state, int mode)
 {
-	int    ret = 0;
+	int ret = 0;
 	u16 outreg, fifo_threshold, smo_mode;
 
 	outreg = 0;
 	fifo_threshold = 1792;
 	smo_mode = (dib7000p_read_word(state, 235) & 0x0050) | (1 << 1);
 
-	dprintk( "setting output mode for demod %p to %d",
-			&state->demod, mode);
+	dprintk("setting output mode for demod %p to %d", &state->demod, mode);
 
 	switch (mode) {
-		case OUTMODE_MPEG2_PAR_GATED_CLK:   // STBs with parallel gated clock
-			outreg = (1 << 10);  /* 0x0400 */
-			break;
-		case OUTMODE_MPEG2_PAR_CONT_CLK:    // STBs with parallel continues clock
-			outreg = (1 << 10) | (1 << 6); /* 0x0440 */
-			break;
-		case OUTMODE_MPEG2_SERIAL:          // STBs with serial input
-			outreg = (1 << 10) | (2 << 6) | (0 << 1); /* 0x0480 */
-			break;
-		case OUTMODE_DIVERSITY:
-			if (state->cfg.hostbus_diversity)
-				outreg = (1 << 10) | (4 << 6); /* 0x0500 */
-			else
-				outreg = (1 << 11);
-			break;
-		case OUTMODE_MPEG2_FIFO:            // e.g. USB feeding
-			smo_mode |= (3 << 1);
-			fifo_threshold = 512;
-			outreg = (1 << 10) | (5 << 6);
-			break;
-		case OUTMODE_ANALOG_ADC:
-			outreg = (1 << 10) | (3 << 6);
-			break;
-		case OUTMODE_HIGH_Z:  // disable
-			outreg = 0;
-			break;
-		default:
-			dprintk( "Unhandled output_mode passed to be set for demod %p",&state->demod);
-			break;
+	case OUTMODE_MPEG2_PAR_GATED_CLK:
+		outreg = (1 << 10);	/* 0x0400 */
+		break;
+	case OUTMODE_MPEG2_PAR_CONT_CLK:
+		outreg = (1 << 10) | (1 << 6);	/* 0x0440 */
+		break;
+	case OUTMODE_MPEG2_SERIAL:
+		outreg = (1 << 10) | (2 << 6) | (0 << 1);	/* 0x0480 */
+		break;
+	case OUTMODE_DIVERSITY:
+		if (state->cfg.hostbus_diversity)
+			outreg = (1 << 10) | (4 << 6);	/* 0x0500 */
+		else
+			outreg = (1 << 11);
+		break;
+	case OUTMODE_MPEG2_FIFO:
+		smo_mode |= (3 << 1);
+		fifo_threshold = 512;
+		outreg = (1 << 10) | (5 << 6);
+		break;
+	case OUTMODE_ANALOG_ADC:
+		outreg = (1 << 10) | (3 << 6);
+		break;
+	case OUTMODE_HIGH_Z:
+		outreg = 0;
+		break;
+	default:
+		dprintk("Unhandled output_mode passed to be set for demod %p", &state->demod);
+		break;
 	}
 
 	if (state->cfg.output_mpeg2_in_188_bytes)
-		smo_mode |= (1 << 5) ;
+		smo_mode |= (1 << 5);
 
-	ret |= dib7000p_write_word(state,  235, smo_mode);
-	ret |= dib7000p_write_word(state,  236, fifo_threshold); /* synchronous fread */
-	ret |= dib7000p_write_word(state, 1286, outreg);         /* P_Div_active */
+	ret |= dib7000p_write_word(state, 235, smo_mode);
+	ret |= dib7000p_write_word(state, 236, fifo_threshold);	/* synchronous fread */
+	if (state->version != SOC7090)
+		ret |= dib7000p_write_word(state, 1286, outreg);	/* P_Div_active */
 
 	return ret;
 }
@@ -161,13 +176,13 @@ static int dib7000p_set_diversity_in(struct dvb_frontend *demod, int onoff)
 	struct dib7000p_state *state = demod->demodulator_priv;
 
 	if (state->div_force_off) {
-		dprintk( "diversity combination deactivated - forced by COFDM parameters");
+		dprintk("diversity combination deactivated - forced by COFDM parameters");
 		onoff = 0;
 		dib7000p_write_word(state, 207, 0);
 	} else
 		dib7000p_write_word(state, 207, (state->div_sync_wait << 4) | (1 << 2) | (2 << 0));
 
-	state->div_state = (u8)onoff;
+	state->div_state = (u8) onoff;
 
 	if (onoff) {
 		dib7000p_write_word(state, 204, 6);
@@ -184,37 +199,48 @@ static int dib7000p_set_diversity_in(struct dvb_frontend *demod, int onoff)
 static int dib7000p_set_power_mode(struct dib7000p_state *state, enum dib7000p_power_mode mode)
 {
 	/* by default everything is powered off */
-	u16 reg_774 = 0xffff, reg_775 = 0xffff, reg_776 = 0x0007, reg_899  = 0x0003,
-		reg_1280 = (0xfe00) | (dib7000p_read_word(state, 1280) & 0x01ff);
+	u16 reg_774 = 0x3fff, reg_775 = 0xffff, reg_776 = 0x0007, reg_899 = 0x0003, reg_1280 = (0xfe00) | (dib7000p_read_word(state, 1280) & 0x01ff);
 
 	/* now, depending on the requested mode, we power on */
 	switch (mode) {
 		/* power up everything in the demod */
-		case DIB7000P_POWER_ALL:
-			reg_774 = 0x0000; reg_775 = 0x0000; reg_776 = 0x0; reg_899 = 0x0; reg_1280 &= 0x01ff;
-			break;
-
-		case DIB7000P_POWER_ANALOG_ADC:
-			/* dem, cfg, iqc, sad, agc */
-			reg_774 &= ~((1 << 15) | (1 << 14) | (1 << 11) | (1 << 10) | (1 << 9));
-			/* nud */
-			reg_776 &= ~((1 << 0));
-			/* Dout */
+	case DIB7000P_POWER_ALL:
+		reg_774 = 0x0000;
+		reg_775 = 0x0000;
+		reg_776 = 0x0;
+		reg_899 = 0x0;
+		if (state->version == SOC7090)
+			reg_1280 &= 0x001f;
+		else
+			reg_1280 &= 0x01ff;
+		break;
+
+	case DIB7000P_POWER_ANALOG_ADC:
+		/* dem, cfg, iqc, sad, agc */
+		reg_774 &= ~((1 << 15) | (1 << 14) | (1 << 11) | (1 << 10) | (1 << 9));
+		/* nud */
+		reg_776 &= ~((1 << 0));
+		/* Dout */
+		if (state->version != SOC7090)
 			reg_1280 &= ~((1 << 11));
-			/* fall through wanted to enable the interfaces */
+		reg_1280 &= ~(1 << 6);
+		/* fall through wanted to enable the interfaces */
 
 		/* just leave power on the control-interfaces: GPIO and (I2C or SDIO) */
-		case DIB7000P_POWER_INTERFACE_ONLY: /* TODO power up either SDIO or I2C */
+	case DIB7000P_POWER_INTERFACE_ONLY:	/* TODO power up either SDIO or I2C */
+		if (state->version == SOC7090)
+			reg_1280 &= ~((1 << 7) | (1 << 5));
+		else
 			reg_1280 &= ~((1 << 14) | (1 << 13) | (1 << 12) | (1 << 10));
-			break;
+		break;
 
 /* TODO following stuff is just converted from the dib7000-driver - check when is used what */
 	}
 
-	dib7000p_write_word(state,  774,  reg_774);
-	dib7000p_write_word(state,  775,  reg_775);
-	dib7000p_write_word(state,  776,  reg_776);
-	dib7000p_write_word(state,  899,  reg_899);
+	dib7000p_write_word(state, 774, reg_774);
+	dib7000p_write_word(state, 775, reg_775);
+	dib7000p_write_word(state, 776, reg_776);
+	dib7000p_write_word(state, 899, reg_899);
 	dib7000p_write_word(state, 1280, reg_1280);
 
 	return 0;
@@ -222,40 +248,57 @@ static int dib7000p_set_power_mode(struct dib7000p_state *state, enum dib7000p_p
 
 static void dib7000p_set_adc_state(struct dib7000p_state *state, enum dibx000_adc_states no)
 {
-	u16 reg_908 = dib7000p_read_word(state, 908),
-	       reg_909 = dib7000p_read_word(state, 909);
+	u16 reg_908 = dib7000p_read_word(state, 908), reg_909 = dib7000p_read_word(state, 909);
+	u16 reg;
 
 	switch (no) {
-		case DIBX000_SLOW_ADC_ON:
+	case DIBX000_SLOW_ADC_ON:
+		if (state->version == SOC7090) {
+			reg = dib7000p_read_word(state, 1925);
+
+			dib7000p_write_word(state, 1925, reg | (1 << 4) | (1 << 2));	/* en_slowAdc = 1 & reset_sladc = 1 */
+
+			reg = dib7000p_read_word(state, 1925);	/* read acces to make it works... strange ... */
+			msleep(200);
+			dib7000p_write_word(state, 1925, reg & ~(1 << 4));	/* en_slowAdc = 1 & reset_sladc = 0 */
+
+			reg = dib7000p_read_word(state, 72) & ~((0x3 << 14) | (0x3 << 12));
+			dib7000p_write_word(state, 72, reg | (1 << 14) | (3 << 12) | 524);	/* ref = Vin1 => Vbg ; sel = Vin0 or Vin3 ; (Vin2 = Vcm) */
+		} else {
 			reg_909 |= (1 << 1) | (1 << 0);
 			dib7000p_write_word(state, 909, reg_909);
 			reg_909 &= ~(1 << 1);
-			break;
+		}
+		break;
 
-		case DIBX000_SLOW_ADC_OFF:
-			reg_909 |=  (1 << 1) | (1 << 0);
-			break;
+	case DIBX000_SLOW_ADC_OFF:
+		if (state->version == SOC7090) {
+			reg = dib7000p_read_word(state, 1925);
+			dib7000p_write_word(state, 1925, (reg & ~(1 << 2)) | (1 << 4));	/* reset_sladc = 1 en_slowAdc = 0 */
+		} else
+			reg_909 |= (1 << 1) | (1 << 0);
+		break;
 
-		case DIBX000_ADC_ON:
-			reg_908 &= 0x0fff;
-			reg_909 &= 0x0003;
-			break;
+	case DIBX000_ADC_ON:
+		reg_908 &= 0x0fff;
+		reg_909 &= 0x0003;
+		break;
 
-		case DIBX000_ADC_OFF: // leave the VBG voltage on
-			reg_908 |= (1 << 14) | (1 << 13) | (1 << 12);
-			reg_909 |= (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2);
-			break;
+	case DIBX000_ADC_OFF:
+		reg_908 |= (1 << 14) | (1 << 13) | (1 << 12);
+		reg_909 |= (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2);
+		break;
 
-		case DIBX000_VBG_ENABLE:
-			reg_908 &= ~(1 << 15);
-			break;
+	case DIBX000_VBG_ENABLE:
+		reg_908 &= ~(1 << 15);
+		break;
 
-		case DIBX000_VBG_DISABLE:
-			reg_908 |= (1 << 15);
-			break;
+	case DIBX000_VBG_DISABLE:
+		reg_908 |= (1 << 15);
+		break;
 
-		default:
-			break;
+	default:
+		break;
 	}
 
 //	dprintk( "908: %x, 909: %x\n", reg_908, reg_909);
@@ -275,17 +318,17 @@ static int dib7000p_set_bandwidth(struct dib7000p_state *state, u32 bw)
 	state->current_bandwidth = bw;
 
 	if (state->timf == 0) {
-		dprintk( "using default timf");
+		dprintk("using default timf");
 		timf = state->cfg.bw->timf;
 	} else {
-		dprintk( "using updated timf");
+		dprintk("using updated timf");
 		timf = state->timf;
 	}
 
 	timf = timf * (bw / 50) / 160;
 
 	dib7000p_write_word(state, 23, (u16) ((timf >> 16) & 0xffff));
-	dib7000p_write_word(state, 24, (u16) ((timf      ) & 0xffff));
+	dib7000p_write_word(state, 24, (u16) ((timf) & 0xffff));
 
 	return 0;
 }
@@ -293,9 +336,12 @@ static int dib7000p_set_bandwidth(struct dib7000p_state *state, u32 bw)
 static int dib7000p_sad_calib(struct dib7000p_state *state)
 {
 /* internal */
-//	dib7000p_write_word(state, 72, (3 << 14) | (1 << 12) | (524 << 0)); // sampling clock of the SAD is writting in set_bandwidth
 	dib7000p_write_word(state, 73, (0 << 1) | (0 << 0));
-	dib7000p_write_word(state, 74, 776); // 0.625*3.3 / 4096
+
+	if (state->version == SOC7090)
+		dib7000p_write_word(state, 74, 2048);
+	else
+		dib7000p_write_word(state, 74, 776);
 
 	/* do the calibration */
 	dib7000p_write_word(state, 73, (1 << 0));
@@ -314,37 +360,91 @@ int dib7000p_set_wbd_ref(struct dvb_frontend *demod, u16 value)
 	state->wbd_ref = value;
 	return dib7000p_write_word(state, 105, (dib7000p_read_word(state, 105) & 0xf000) | value);
 }
-
 EXPORT_SYMBOL(dib7000p_set_wbd_ref);
+
 static void dib7000p_reset_pll(struct dib7000p_state *state)
 {
 	struct dibx000_bandwidth_config *bw = &state->cfg.bw[0];
 	u16 clk_cfg0;
 
-	/* force PLL bypass */
-	clk_cfg0 = (1 << 15) | ((bw->pll_ratio & 0x3f) << 9) |
-		(bw->modulo << 7) | (bw->ADClkSrc << 6) | (bw->IO_CLK_en_core << 5) |
-		(bw->bypclk_div << 2) | (bw->enable_refdiv << 1) | (0 << 0);
+	if (state->version == SOC7090) {
+		dib7000p_write_word(state, 1856, (!bw->pll_reset << 13) | (bw->pll_range << 12) | (bw->pll_ratio << 6) | (bw->pll_prediv));
+
+		while (((dib7000p_read_word(state, 1856) >> 15) & 0x1) != 1)
+			;
 
-	dib7000p_write_word(state, 900, clk_cfg0);
+		dib7000p_write_word(state, 1857, dib7000p_read_word(state, 1857) | (!bw->pll_bypass << 15));
+	} else {
+		/* force PLL bypass */
+		clk_cfg0 = (1 << 15) | ((bw->pll_ratio & 0x3f) << 9) |
+			(bw->modulo << 7) | (bw->ADClkSrc << 6) | (bw->IO_CLK_en_core << 5) | (bw->bypclk_div << 2) | (bw->enable_refdiv << 1) | (0 << 0);
+
+		dib7000p_write_word(state, 900, clk_cfg0);
 
-	/* P_pll_cfg */
-	dib7000p_write_word(state, 903, (bw->pll_prediv << 5) | (((bw->pll_ratio >> 6) & 0x3) << 3) | (bw->pll_range << 1) | bw->pll_reset);
-	clk_cfg0 = (bw->pll_bypass << 15) | (clk_cfg0 & 0x7fff);
-	dib7000p_write_word(state, 900, clk_cfg0);
+		/* P_pll_cfg */
+		dib7000p_write_word(state, 903, (bw->pll_prediv << 5) | (((bw->pll_ratio >> 6) & 0x3) << 3) | (bw->pll_range << 1) | bw->pll_reset);
+		clk_cfg0 = (bw->pll_bypass << 15) | (clk_cfg0 & 0x7fff);
+		dib7000p_write_word(state, 900, clk_cfg0);
+	}
 
-	dib7000p_write_word(state, 18, (u16) (((bw->internal*1000) >> 16) & 0xffff));
-	dib7000p_write_word(state, 19, (u16) ( (bw->internal*1000       ) & 0xffff));
-	dib7000p_write_word(state, 21, (u16) ( (bw->ifreq          >> 16) & 0xffff));
-	dib7000p_write_word(state, 22, (u16) ( (bw->ifreq               ) & 0xffff));
+	dib7000p_write_word(state, 18, (u16) (((bw->internal * 1000) >> 16) & 0xffff));
+	dib7000p_write_word(state, 19, (u16) ((bw->internal * 1000) & 0xffff));
+	dib7000p_write_word(state, 21, (u16) ((bw->ifreq >> 16) & 0xffff));
+	dib7000p_write_word(state, 22, (u16) ((bw->ifreq) & 0xffff));
 
 	dib7000p_write_word(state, 72, bw->sad_cfg);
 }
 
+static u32 dib7000p_get_internal_freq(struct dib7000p_state *state)
+{
+	u32 internal = (u32) dib7000p_read_word(state, 18) << 16;
+	internal |= (u32) dib7000p_read_word(state, 19);
+	internal /= 1000;
+
+	return internal;
+}
+
+int dib7000p_update_pll(struct dvb_frontend *fe, struct dibx000_bandwidth_config *bw)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	u16 reg_1857, reg_1856 = dib7000p_read_word(state, 1856);
+	u8 loopdiv, prediv;
+	u32 internal, xtal;
+
+	/* get back old values */
+	prediv = reg_1856 & 0x3f;
+	loopdiv = (reg_1856 >> 6) & 0x3f;
+
+	if ((bw != NULL) && (bw->pll_prediv != prediv || bw->pll_ratio != loopdiv)) {
+		dprintk("Updating pll (prediv: old =  %d new = %d ; loopdiv : old = %d new = %d)", prediv, bw->pll_prediv, loopdiv, bw->pll_ratio);
+		reg_1856 &= 0xf000;
+		reg_1857 = dib7000p_read_word(state, 1857);
+		dib7000p_write_word(state, 1857, reg_1857 & ~(1 << 15));
+
+		dib7000p_write_word(state, 1856, reg_1856 | ((bw->pll_ratio & 0x3f) << 6) | (bw->pll_prediv & 0x3f));
+
+		/* write new system clk into P_sec_len */
+		internal = dib7000p_get_internal_freq(state);
+		xtal = (internal / loopdiv) * prediv;
+		internal = 1000 * (xtal / bw->pll_prediv) * bw->pll_ratio;	/* new internal */
+		dib7000p_write_word(state, 18, (u16) ((internal >> 16) & 0xffff));
+		dib7000p_write_word(state, 19, (u16) (internal & 0xffff));
+
+		dib7000p_write_word(state, 1857, reg_1857 | (1 << 15));
+
+		while (((dib7000p_read_word(state, 1856) >> 15) & 0x1) != 1)
+			dprintk("Waiting for PLL to lock");
+
+		return 0;
+	}
+	return -EIO;
+}
+EXPORT_SYMBOL(dib7000p_update_pll);
+
 static int dib7000p_reset_gpio(struct dib7000p_state *st)
 {
 	/* reset the GPIOs */
-	dprintk( "gpio dir: %x: val: %x, pwm_pos: %x",st->gpio_dir, st->gpio_val,st->cfg.gpio_pwm_pos);
+	dprintk("gpio dir: %x: val: %x, pwm_pos: %x", st->gpio_dir, st->gpio_val, st->cfg.gpio_pwm_pos);
 
 	dib7000p_write_word(st, 1029, st->gpio_dir);
 	dib7000p_write_word(st, 1030, st->gpio_val);
@@ -360,13 +460,13 @@ static int dib7000p_reset_gpio(struct dib7000p_state *st)
 static int dib7000p_cfg_gpio(struct dib7000p_state *st, u8 num, u8 dir, u8 val)
 {
 	st->gpio_dir = dib7000p_read_word(st, 1029);
-	st->gpio_dir &= ~(1 << num);         /* reset the direction bit */
-	st->gpio_dir |=  (dir & 0x1) << num; /* set the new direction */
+	st->gpio_dir &= ~(1 << num);	/* reset the direction bit */
+	st->gpio_dir |= (dir & 0x1) << num;	/* set the new direction */
 	dib7000p_write_word(st, 1029, st->gpio_dir);
 
 	st->gpio_val = dib7000p_read_word(st, 1030);
-	st->gpio_val &= ~(1 << num);          /* reset the direction bit */
-	st->gpio_val |=  (val & 0x01) << num; /* set the new value */
+	st->gpio_val &= ~(1 << num);	/* reset the direction bit */
+	st->gpio_val |= (val & 0x01) << num;	/* set the new value */
 	dib7000p_write_word(st, 1030, st->gpio_val);
 
 	return 0;
@@ -377,96 +477,94 @@ int dib7000p_set_gpio(struct dvb_frontend *demod, u8 num, u8 dir, u8 val)
 	struct dib7000p_state *state = demod->demodulator_priv;
 	return dib7000p_cfg_gpio(state, num, dir, val);
 }
-
 EXPORT_SYMBOL(dib7000p_set_gpio);
-static u16 dib7000p_defaults[] =
 
-{
+static u16 dib7000p_defaults[] = {
 	// auto search configuration
 	3, 2,
-		0x0004,
-		0x1000,
-		0x0814, /* Equal Lock */
+	0x0004,
+	0x1000,
+	0x0814,			/* Equal Lock */
 
 	12, 6,
-		0x001b,
-		0x7740,
-		0x005b,
-		0x8d80,
-		0x01c9,
-		0xc380,
-		0x0000,
-		0x0080,
-		0x0000,
-		0x0090,
-		0x0001,
-		0xd4c0,
+	0x001b,
+	0x7740,
+	0x005b,
+	0x8d80,
+	0x01c9,
+	0xc380,
+	0x0000,
+	0x0080,
+	0x0000,
+	0x0090,
+	0x0001,
+	0xd4c0,
 
 	1, 26,
-		0x6680, // P_timf_alpha=6, P_corm_alpha=6, P_corm_thres=128 default: 6,4,26
+	0x6680,
 
 	/* set ADC level to -16 */
 	11, 79,
-		(1 << 13) - 825 - 117,
-		(1 << 13) - 837 - 117,
-		(1 << 13) - 811 - 117,
-		(1 << 13) - 766 - 117,
-		(1 << 13) - 737 - 117,
-		(1 << 13) - 693 - 117,
-		(1 << 13) - 648 - 117,
-		(1 << 13) - 619 - 117,
-		(1 << 13) - 575 - 117,
-		(1 << 13) - 531 - 117,
-		(1 << 13) - 501 - 117,
+	(1 << 13) - 825 - 117,
+	(1 << 13) - 837 - 117,
+	(1 << 13) - 811 - 117,
+	(1 << 13) - 766 - 117,
+	(1 << 13) - 737 - 117,
+	(1 << 13) - 693 - 117,
+	(1 << 13) - 648 - 117,
+	(1 << 13) - 619 - 117,
+	(1 << 13) - 575 - 117,
+	(1 << 13) - 531 - 117,
+	(1 << 13) - 501 - 117,
 
 	1, 142,
-		0x0410, // P_palf_filter_on=1, P_palf_filter_freeze=0, P_palf_alpha_regul=16
+	0x0410,
 
 	/* disable power smoothing */
 	8, 145,
-		0,
-		0,
-		0,
-		0,
-		0,
-		0,
-		0,
-		0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
 
 	1, 154,
-		1 << 13, // P_fft_freq_dir=1, P_fft_nb_to_cut=0
+	1 << 13,
 
 	1, 168,
-		0x0ccd, // P_pha3_thres, default 0x3000
-
-//	1, 169,
-//		0x0010, // P_cti_use_cpe=0, P_cti_use_prog=0, P_cti_win_len=16, default: 0x0010
+	0x0ccd,
 
 	1, 183,
-		0x200f, // P_cspu_regul=512, P_cspu_win_cut=15, default: 0x2005
+	0x200f,
+
+	1, 212,
+		0x169,
 
 	5, 187,
-		0x023d, // P_adp_regul_cnt=573, default: 410
-		0x00a4, // P_adp_noise_cnt=
-		0x00a4, // P_adp_regul_ext
-		0x7ff0, // P_adp_noise_ext
-		0x3ccc, // P_adp_fil
+	0x023d,
+	0x00a4,
+	0x00a4,
+	0x7ff0,
+	0x3ccc,
 
 	1, 198,
-		0x800, // P_equal_thres_wgn
+	0x800,
 
 	1, 222,
-		0x0010, // P_fec_ber_rs_len=2
+	0x0010,
 
 	1, 235,
-		0x0062, // P_smo_mode, P_smo_rs_discard, P_smo_fifo_flush, P_smo_pid_parse, P_smo_error_discard
+	0x0062,
 
 	2, 901,
-		0x0006, // P_clk_cfg1
-		(3 << 10) | (1 << 6), // P_divclksel=3 P_divbitsel=1
+	0x0006,
+	(3 << 10) | (1 << 6),
 
 	1, 905,
-		0x2c8e, // Tuner IO bank: max drive (14mA) + divout pads max drive
+	0x2c8e,
 
 	0,
 };
@@ -475,51 +573,64 @@ static int dib7000p_demod_reset(struct dib7000p_state *state)
 {
 	dib7000p_set_power_mode(state, DIB7000P_POWER_ALL);
 
+	if (state->version == SOC7090)
+		dibx000_reset_i2c_master(&state->i2c_master);
+
 	dib7000p_set_adc_state(state, DIBX000_VBG_ENABLE);
 
 	/* restart all parts */
-	dib7000p_write_word(state,  770, 0xffff);
-	dib7000p_write_word(state,  771, 0xffff);
-	dib7000p_write_word(state,  772, 0x001f);
-	dib7000p_write_word(state,  898, 0x0003);
-	/* except i2c, sdio, gpio - control interfaces */
-	dib7000p_write_word(state, 1280, 0x01fc - ((1 << 7) | (1 << 6) | (1 << 5)) );
-
-	dib7000p_write_word(state,  770, 0);
-	dib7000p_write_word(state,  771, 0);
-	dib7000p_write_word(state,  772, 0);
-	dib7000p_write_word(state,  898, 0);
+	dib7000p_write_word(state, 770, 0xffff);
+	dib7000p_write_word(state, 771, 0xffff);
+	dib7000p_write_word(state, 772, 0x001f);
+	dib7000p_write_word(state, 898, 0x0003);
+	dib7000p_write_word(state, 1280, 0x001f - ((1 << 4) | (1 << 3)));
+
+	dib7000p_write_word(state, 770, 0);
+	dib7000p_write_word(state, 771, 0);
+	dib7000p_write_word(state, 772, 0);
+	dib7000p_write_word(state, 898, 0);
 	dib7000p_write_word(state, 1280, 0);
 
 	/* default */
 	dib7000p_reset_pll(state);
 
 	if (dib7000p_reset_gpio(state) != 0)
-		dprintk( "GPIO reset was not successful.");
-
-	if (dib7000p_set_output_mode(state, OUTMODE_HIGH_Z) != 0)
-		dprintk( "OUTPUT_MODE could not be reset.");
+		dprintk("GPIO reset was not successful.");
 
-	/* unforce divstr regardless whether i2c enumeration was done or not */
-	dib7000p_write_word(state, 1285, dib7000p_read_word(state, 1285) & ~(1 << 1) );
+	if (state->version == SOC7090) {
+		dib7000p_write_word(state, 899, 0);
 
-	dib7000p_set_bandwidth(state, 8000);
+		/* impulse noise */
+		dib7000p_write_word(state, 42, (1<<5) | 3); /* P_iqc_thsat_ipc = 1 ; P_iqc_win2 = 3 */
+		dib7000p_write_word(state, 43, 0x2d4); /*-300 fag P_iqc_dect_min = -280 */
+		dib7000p_write_word(state, 44, 300); /* 300 fag P_iqc_dect_min = +280 */
+		dib7000p_write_word(state, 273, (1<<6) | 30);
+	}
+	if (dib7000p_set_output_mode(state, OUTMODE_HIGH_Z) != 0)
+		dprintk("OUTPUT_MODE could not be reset.");
 
 	dib7000p_set_adc_state(state, DIBX000_SLOW_ADC_ON);
 	dib7000p_sad_calib(state);
 	dib7000p_set_adc_state(state, DIBX000_SLOW_ADC_OFF);
 
-	// P_iqc_alpha_pha, P_iqc_alpha_amp_dcc_alpha, ...
-	if(state->cfg.tuner_is_baseband)
-		dib7000p_write_word(state, 36,0x0755);
-	else
-		dib7000p_write_word(state, 36,0x1f55);
+	/* unforce divstr regardless whether i2c enumeration was done or not */
+	dib7000p_write_word(state, 1285, dib7000p_read_word(state, 1285) & ~(1 << 1));
+
+	dib7000p_set_bandwidth(state, 8000);
+
+	if (state->version == SOC7090) {
+		dib7000p_write_word(state, 36, 0x5755);/* P_iqc_impnc_on =1 & P_iqc_corr_inh = 1 for impulsive noise */
+	} else {
+		if (state->cfg.tuner_is_baseband)
+			dib7000p_write_word(state, 36, 0x0755);
+		else
+			dib7000p_write_word(state, 36, 0x1f55);
+	}
 
 	dib7000p_write_tab(state, dib7000p_defaults);
 
 	dib7000p_set_power_mode(state, DIB7000P_POWER_INTERFACE_ONLY);
 
-
 	return 0;
 }
 
@@ -527,9 +638,9 @@ static void dib7000p_pll_clk_cfg(struct dib7000p_state *state)
 {
 	u16 tmp = 0;
 	tmp = dib7000p_read_word(state, 903);
-	dib7000p_write_word(state, 903, (tmp | 0x1));   //pwr-up pll
+	dib7000p_write_word(state, 903, (tmp | 0x1));
 	tmp = dib7000p_read_word(state, 900);
-	dib7000p_write_word(state, 900, (tmp & 0x7fff) | (1 << 6));     //use High freq clock
+	dib7000p_write_word(state, 900, (tmp & 0x7fff) | (1 << 6));
 }
 
 static void dib7000p_restart_agc(struct dib7000p_state *state)
@@ -543,11 +654,9 @@ static int dib7000p_update_lna(struct dib7000p_state *state)
 {
 	u16 dyn_gain;
 
-	// when there is no LNA to program return immediatly
 	if (state->cfg.update_lna) {
-		// read dyn_gain here (because it is demod-dependent and not fe)
 		dyn_gain = dib7000p_read_word(state, 394);
-		if (state->cfg.update_lna(&state->demod,dyn_gain)) { // LNA has changed
+		if (state->cfg.update_lna(&state->demod, dyn_gain)) {
 			dib7000p_restart_agc(state);
 			return 1;
 		}
@@ -571,24 +680,24 @@ static int dib7000p_set_agc_config(struct dib7000p_state *state, u8 band)
 		}
 
 	if (agc == NULL) {
-		dprintk( "no valid AGC configuration found for band 0x%02x",band);
+		dprintk("no valid AGC configuration found for band 0x%02x", band);
 		return -EINVAL;
 	}
 
 	state->current_agc = agc;
 
 	/* AGC */
-	dib7000p_write_word(state, 75 ,  agc->setup );
-	dib7000p_write_word(state, 76 ,  agc->inv_gain );
-	dib7000p_write_word(state, 77 ,  agc->time_stabiliz );
+	dib7000p_write_word(state, 75, agc->setup);
+	dib7000p_write_word(state, 76, agc->inv_gain);
+	dib7000p_write_word(state, 77, agc->time_stabiliz);
 	dib7000p_write_word(state, 100, (agc->alpha_level << 12) | agc->thlock);
 
 	// Demod AGC loop configuration
 	dib7000p_write_word(state, 101, (agc->alpha_mant << 5) | agc->alpha_exp);
-	dib7000p_write_word(state, 102, (agc->beta_mant << 6)  | agc->beta_exp);
+	dib7000p_write_word(state, 102, (agc->beta_mant << 6) | agc->beta_exp);
 
 	/* AGC continued */
-	dprintk( "WBD: ref: %d, sel: %d, active: %d, alpha: %d",
+	dprintk("WBD: ref: %d, sel: %d, active: %d, alpha: %d",
 		state->wbd_ref != 0 ? state->wbd_ref : agc->wbd_ref, agc->wbd_sel, !agc->perform_agc_softsplit, agc->wbd_sel);
 
 	if (state->wbd_ref != 0)
@@ -598,101 +707,135 @@ static int dib7000p_set_agc_config(struct dib7000p_state *state, u8 band)
 
 	dib7000p_write_word(state, 106, (agc->wbd_sel << 13) | (agc->wbd_alpha << 9) | (agc->perform_agc_softsplit << 8));
 
-	dib7000p_write_word(state, 107,  agc->agc1_max);
-	dib7000p_write_word(state, 108,  agc->agc1_min);
-	dib7000p_write_word(state, 109,  agc->agc2_max);
-	dib7000p_write_word(state, 110,  agc->agc2_min);
-	dib7000p_write_word(state, 111, (agc->agc1_pt1    << 8) | agc->agc1_pt2);
-	dib7000p_write_word(state, 112,  agc->agc1_pt3);
+	dib7000p_write_word(state, 107, agc->agc1_max);
+	dib7000p_write_word(state, 108, agc->agc1_min);
+	dib7000p_write_word(state, 109, agc->agc2_max);
+	dib7000p_write_word(state, 110, agc->agc2_min);
+	dib7000p_write_word(state, 111, (agc->agc1_pt1 << 8) | agc->agc1_pt2);
+	dib7000p_write_word(state, 112, agc->agc1_pt3);
 	dib7000p_write_word(state, 113, (agc->agc1_slope1 << 8) | agc->agc1_slope2);
-	dib7000p_write_word(state, 114, (agc->agc2_pt1    << 8) | agc->agc2_pt2);
+	dib7000p_write_word(state, 114, (agc->agc2_pt1 << 8) | agc->agc2_pt2);
 	dib7000p_write_word(state, 115, (agc->agc2_slope1 << 8) | agc->agc2_slope2);
 	return 0;
 }
 
+static void dib7000p_set_dds(struct dib7000p_state *state, s32 offset_khz)
+{
+	u32 internal = dib7000p_get_internal_freq(state);
+	s32 unit_khz_dds_val = 67108864 / (internal);	/* 2**26 / Fsampling is the unit 1KHz offset */
+	u32 abs_offset_khz = ABS(offset_khz);
+	u32 dds = state->cfg.bw->ifreq & 0x1ffffff;
+	u8 invert = !!(state->cfg.bw->ifreq & (1 << 25));
+
+	dprintk("setting a frequency offset of %dkHz internal freq = %d invert = %d", offset_khz, internal, invert);
+
+	if (offset_khz < 0)
+		unit_khz_dds_val *= -1;
+
+	/* IF tuner */
+	if (invert)
+		dds -= (abs_offset_khz * unit_khz_dds_val);	/* /100 because of /100 on the unit_khz_dds_val line calc for better accuracy */
+	else
+		dds += (abs_offset_khz * unit_khz_dds_val);
+
+	if (abs_offset_khz <= (internal / 2)) {	/* Max dds offset is the half of the demod freq */
+		dib7000p_write_word(state, 21, (u16) (((dds >> 16) & 0x1ff) | (0 << 10) | (invert << 9)));
+		dib7000p_write_word(state, 22, (u16) (dds & 0xffff));
+	}
+}
+
 static int dib7000p_agc_startup(struct dvb_frontend *demod, struct dvb_frontend_parameters *ch)
 {
 	struct dib7000p_state *state = demod->demodulator_priv;
 	int ret = -1;
 	u8 *agc_state = &state->agc_state;
 	u8 agc_split;
+	u16 reg;
+	u32 upd_demod_gain_period = 0x1000;
 
 	switch (state->agc_state) {
-		case 0:
-			// set power-up level: interf+analog+AGC
-			dib7000p_set_power_mode(state, DIB7000P_POWER_ALL);
+	case 0:
+		dib7000p_set_power_mode(state, DIB7000P_POWER_ALL);
+		if (state->version == SOC7090) {
+			reg = dib7000p_read_word(state, 0x79b) & 0xff00;
+			dib7000p_write_word(state, 0x79a, upd_demod_gain_period & 0xFFFF);	/* lsb */
+			dib7000p_write_word(state, 0x79b, reg | (1 << 14) | ((upd_demod_gain_period >> 16) & 0xFF));
+
+			/* enable adc i & q */
+			reg = dib7000p_read_word(state, 0x780);
+			dib7000p_write_word(state, 0x780, (reg | (0x3)) & (~(1 << 7)));
+		} else {
 			dib7000p_set_adc_state(state, DIBX000_ADC_ON);
 			dib7000p_pll_clk_cfg(state);
+		}
 
-			if (dib7000p_set_agc_config(state, BAND_OF_FREQUENCY(ch->frequency/1000)) != 0)
-				return -1;
-
-			ret = 7;
-			(*agc_state)++;
-			break;
+		if (dib7000p_set_agc_config(state, BAND_OF_FREQUENCY(ch->frequency / 1000)) != 0)
+			return -1;
 
-		case 1:
-			// AGC initialization
-			if (state->cfg.agc_control)
-				state->cfg.agc_control(&state->demod, 1);
-
-			dib7000p_write_word(state, 78, 32768);
-			if (!state->current_agc->perform_agc_softsplit) {
-				/* we are using the wbd - so slow AGC startup */
-				/* force 0 split on WBD and restart AGC */
-				dib7000p_write_word(state, 106, (state->current_agc->wbd_sel << 13) | (state->current_agc->wbd_alpha << 9) | (1 << 8));
-				(*agc_state)++;
-				ret = 5;
-			} else {
-				/* default AGC startup */
-				(*agc_state) = 4;
-				/* wait AGC rough lock time */
-				ret = 7;
-			}
+		dib7000p_set_dds(state, 0);
+		ret = 7;
+		(*agc_state)++;
+		break;
 
-			dib7000p_restart_agc(state);
-			break;
+	case 1:
+		if (state->cfg.agc_control)
+			state->cfg.agc_control(&state->demod, 1);
 
-		case 2: /* fast split search path after 5sec */
-			dib7000p_write_word(state,  75, state->current_agc->setup | (1 << 4)); /* freeze AGC loop */
-			dib7000p_write_word(state, 106, (state->current_agc->wbd_sel << 13) | (2 << 9) | (0 << 8)); /* fast split search 0.25kHz */
+		dib7000p_write_word(state, 78, 32768);
+		if (!state->current_agc->perform_agc_softsplit) {
+			/* we are using the wbd - so slow AGC startup */
+			/* force 0 split on WBD and restart AGC */
+			dib7000p_write_word(state, 106, (state->current_agc->wbd_sel << 13) | (state->current_agc->wbd_alpha << 9) | (1 << 8));
 			(*agc_state)++;
-			ret = 14;
-			break;
+			ret = 5;
+		} else {
+			/* default AGC startup */
+			(*agc_state) = 4;
+			/* wait AGC rough lock time */
+			ret = 7;
+		}
 
-	case 3: /* split search ended */
-			agc_split = (u8)dib7000p_read_word(state, 396); /* store the split value for the next time */
-			dib7000p_write_word(state, 78, dib7000p_read_word(state, 394)); /* set AGC gain start value */
+		dib7000p_restart_agc(state);
+		break;
 
-			dib7000p_write_word(state, 75,  state->current_agc->setup);   /* std AGC loop */
-			dib7000p_write_word(state, 106, (state->current_agc->wbd_sel << 13) | (state->current_agc->wbd_alpha << 9) | agc_split); /* standard split search */
+	case 2:		/* fast split search path after 5sec */
+		dib7000p_write_word(state, 75, state->current_agc->setup | (1 << 4));	/* freeze AGC loop */
+		dib7000p_write_word(state, 106, (state->current_agc->wbd_sel << 13) | (2 << 9) | (0 << 8));	/* fast split search 0.25kHz */
+		(*agc_state)++;
+		ret = 14;
+		break;
 
-			dib7000p_restart_agc(state);
+	case 3:		/* split search ended */
+		agc_split = (u8) dib7000p_read_word(state, 396);	/* store the split value for the next time */
+		dib7000p_write_word(state, 78, dib7000p_read_word(state, 394));	/* set AGC gain start value */
 
-			dprintk( "SPLIT %p: %hd", demod, agc_split);
+		dib7000p_write_word(state, 75, state->current_agc->setup);	/* std AGC loop */
+		dib7000p_write_word(state, 106, (state->current_agc->wbd_sel << 13) | (state->current_agc->wbd_alpha << 9) | agc_split);	/* standard split search */
 
-			(*agc_state)++;
-			ret = 5;
-			break;
+		dib7000p_restart_agc(state);
 
-		case 4: /* LNA startup */
-			// wait AGC accurate lock time
-			ret = 7;
+		dprintk("SPLIT %p: %hd", demod, agc_split);
 
-			if (dib7000p_update_lna(state))
-				// wait only AGC rough lock time
-				ret = 5;
-			else // nothing was done, go to the next state
-				(*agc_state)++;
-			break;
+		(*agc_state)++;
+		ret = 5;
+		break;
 
-		case 5:
-			if (state->cfg.agc_control)
-				state->cfg.agc_control(&state->demod, 0);
+	case 4:		/* LNA startup */
+		ret = 7;
+
+		if (dib7000p_update_lna(state))
+			ret = 5;
+		else
 			(*agc_state)++;
-			break;
-		default:
-			break;
+		break;
+
+	case 5:
+		if (state->cfg.agc_control)
+			state->cfg.agc_control(&state->demod, 0);
+		(*agc_state)++;
+		break;
+	default:
+		break;
 	}
 	return ret;
 }
@@ -703,45 +846,89 @@ static void dib7000p_update_timf(struct dib7000p_state *state)
 	state->timf = timf * 160 / (state->current_bandwidth / 50);
 	dib7000p_write_word(state, 23, (u16) (timf >> 16));
 	dib7000p_write_word(state, 24, (u16) (timf & 0xffff));
-	dprintk( "updated timf_frequency: %d (default: %d)",state->timf, state->cfg.bw->timf);
+	dprintk("updated timf_frequency: %d (default: %d)", state->timf, state->cfg.bw->timf);
+
+}
 
+u32 dib7000p_ctrl_timf(struct dvb_frontend *fe, u8 op, u32 timf)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	switch (op) {
+	case DEMOD_TIMF_SET:
+		state->timf = timf;
+		break;
+	case DEMOD_TIMF_UPDATE:
+		dib7000p_update_timf(state);
+		break;
+	case DEMOD_TIMF_GET:
+		break;
+	}
+	dib7000p_set_bandwidth(state, state->current_bandwidth);
+	return state->timf;
 }
+EXPORT_SYMBOL(dib7000p_ctrl_timf);
 
 static void dib7000p_set_channel(struct dib7000p_state *state, struct dvb_frontend_parameters *ch, u8 seq)
 {
 	u16 value, est[4];
 
-    dib7000p_set_bandwidth(state, BANDWIDTH_TO_KHZ(ch->u.ofdm.bandwidth));
+	dib7000p_set_bandwidth(state, BANDWIDTH_TO_KHZ(ch->u.ofdm.bandwidth));
 
 	/* nfft, guard, qam, alpha */
 	value = 0;
 	switch (ch->u.ofdm.transmission_mode) {
-		case TRANSMISSION_MODE_2K: value |= (0 << 7); break;
-		case TRANSMISSION_MODE_4K: value |= (2 << 7); break;
-		default:
-		case TRANSMISSION_MODE_8K: value |= (1 << 7); break;
+	case TRANSMISSION_MODE_2K:
+		value |= (0 << 7);
+		break;
+	case TRANSMISSION_MODE_4K:
+		value |= (2 << 7);
+		break;
+	default:
+	case TRANSMISSION_MODE_8K:
+		value |= (1 << 7);
+		break;
 	}
 	switch (ch->u.ofdm.guard_interval) {
-		case GUARD_INTERVAL_1_32: value |= (0 << 5); break;
-		case GUARD_INTERVAL_1_16: value |= (1 << 5); break;
-		case GUARD_INTERVAL_1_4:  value |= (3 << 5); break;
-		default:
-		case GUARD_INTERVAL_1_8:  value |= (2 << 5); break;
+	case GUARD_INTERVAL_1_32:
+		value |= (0 << 5);
+		break;
+	case GUARD_INTERVAL_1_16:
+		value |= (1 << 5);
+		break;
+	case GUARD_INTERVAL_1_4:
+		value |= (3 << 5);
+		break;
+	default:
+	case GUARD_INTERVAL_1_8:
+		value |= (2 << 5);
+		break;
 	}
 	switch (ch->u.ofdm.constellation) {
-		case QPSK:  value |= (0 << 3); break;
-		case QAM_16: value |= (1 << 3); break;
-		default:
-		case QAM_64: value |= (2 << 3); break;
+	case QPSK:
+		value |= (0 << 3);
+		break;
+	case QAM_16:
+		value |= (1 << 3);
+		break;
+	default:
+	case QAM_64:
+		value |= (2 << 3);
+		break;
 	}
 	switch (HIERARCHY_1) {
-		case HIERARCHY_2: value |= 2; break;
-		case HIERARCHY_4: value |= 4; break;
-		default:
-		case HIERARCHY_1: value |= 1; break;
+	case HIERARCHY_2:
+		value |= 2;
+		break;
+	case HIERARCHY_4:
+		value |= 4;
+		break;
+	default:
+	case HIERARCHY_1:
+		value |= 1;
+		break;
 	}
 	dib7000p_write_word(state, 0, value);
-	dib7000p_write_word(state, 5, (seq << 4) | 1); /* do not force tps, search list 0 */
+	dib7000p_write_word(state, 5, (seq << 4) | 1);	/* do not force tps, search list 0 */
 
 	/* P_dintl_native, P_dintlv_inv, P_hrch, P_code_rate, P_select_hp */
 	value = 0;
@@ -752,39 +939,63 @@ static void dib7000p_set_channel(struct dib7000p_state *state, struct dvb_fronte
 	if (1 == 1)
 		value |= 1;
 	switch ((ch->u.ofdm.hierarchy_information == 0 || 1 == 1) ? ch->u.ofdm.code_rate_HP : ch->u.ofdm.code_rate_LP) {
-		case FEC_2_3: value |= (2 << 1); break;
-		case FEC_3_4: value |= (3 << 1); break;
-		case FEC_5_6: value |= (5 << 1); break;
-		case FEC_7_8: value |= (7 << 1); break;
-		default:
-		case FEC_1_2: value |= (1 << 1); break;
+	case FEC_2_3:
+		value |= (2 << 1);
+		break;
+	case FEC_3_4:
+		value |= (3 << 1);
+		break;
+	case FEC_5_6:
+		value |= (5 << 1);
+		break;
+	case FEC_7_8:
+		value |= (7 << 1);
+		break;
+	default:
+	case FEC_1_2:
+		value |= (1 << 1);
+		break;
 	}
 	dib7000p_write_word(state, 208, value);
 
 	/* offset loop parameters */
-	dib7000p_write_word(state, 26, 0x6680); // timf(6xxx)
-	dib7000p_write_word(state, 32, 0x0003); // pha_off_max(xxx3)
-	dib7000p_write_word(state, 29, 0x1273); // isi
-	dib7000p_write_word(state, 33, 0x0005); // sfreq(xxx5)
+	dib7000p_write_word(state, 26, 0x6680);
+	dib7000p_write_word(state, 32, 0x0003);
+	dib7000p_write_word(state, 29, 0x1273);
+	dib7000p_write_word(state, 33, 0x0005);
 
 	/* P_dvsy_sync_wait */
 	switch (ch->u.ofdm.transmission_mode) {
-		case TRANSMISSION_MODE_8K: value = 256; break;
-		case TRANSMISSION_MODE_4K: value = 128; break;
-		case TRANSMISSION_MODE_2K:
-		default: value = 64; break;
+	case TRANSMISSION_MODE_8K:
+		value = 256;
+		break;
+	case TRANSMISSION_MODE_4K:
+		value = 128;
+		break;
+	case TRANSMISSION_MODE_2K:
+	default:
+		value = 64;
+		break;
 	}
 	switch (ch->u.ofdm.guard_interval) {
-		case GUARD_INTERVAL_1_16: value *= 2; break;
-		case GUARD_INTERVAL_1_8:  value *= 4; break;
-		case GUARD_INTERVAL_1_4:  value *= 8; break;
-		default:
-		case GUARD_INTERVAL_1_32: value *= 1; break;
+	case GUARD_INTERVAL_1_16:
+		value *= 2;
+		break;
+	case GUARD_INTERVAL_1_8:
+		value *= 4;
+		break;
+	case GUARD_INTERVAL_1_4:
+		value *= 8;
+		break;
+	default:
+	case GUARD_INTERVAL_1_32:
+		value *= 1;
+		break;
 	}
 	if (state->cfg.diversity_delay == 0)
-		state->div_sync_wait = (value * 3) / 2 + 48; // add 50% SFN margin + compensate for one DVSY-fifo
+		state->div_sync_wait = (value * 3) / 2 + 48;
 	else
-		state->div_sync_wait = (value * 3) / 2 + state->cfg.diversity_delay; // add 50% SFN margin + compensate for one DVSY-fifo
+		state->div_sync_wait = (value * 3) / 2 + state->cfg.diversity_delay;
 
 	/* deactive the possibility of diversity reception if extended interleaver */
 	state->div_force_off = !1 && ch->u.ofdm.transmission_mode != TRANSMISSION_MODE_8K;
@@ -792,24 +1003,24 @@ static void dib7000p_set_channel(struct dib7000p_state *state, struct dvb_fronte
 
 	/* channel estimation fine configuration */
 	switch (ch->u.ofdm.constellation) {
-		case QAM_64:
-			est[0] = 0x0148;       /* P_adp_regul_cnt 0.04 */
-			est[1] = 0xfff0;       /* P_adp_noise_cnt -0.002 */
-			est[2] = 0x00a4;       /* P_adp_regul_ext 0.02 */
-			est[3] = 0xfff8;       /* P_adp_noise_ext -0.001 */
-			break;
-		case QAM_16:
-			est[0] = 0x023d;       /* P_adp_regul_cnt 0.07 */
-			est[1] = 0xffdf;       /* P_adp_noise_cnt -0.004 */
-			est[2] = 0x00a4;       /* P_adp_regul_ext 0.02 */
-			est[3] = 0xfff0;       /* P_adp_noise_ext -0.002 */
-			break;
-		default:
-			est[0] = 0x099a;       /* P_adp_regul_cnt 0.3 */
-			est[1] = 0xffae;       /* P_adp_noise_cnt -0.01 */
-			est[2] = 0x0333;       /* P_adp_regul_ext 0.1 */
-			est[3] = 0xfff8;       /* P_adp_noise_ext -0.002 */
-			break;
+	case QAM_64:
+		est[0] = 0x0148;	/* P_adp_regul_cnt 0.04 */
+		est[1] = 0xfff0;	/* P_adp_noise_cnt -0.002 */
+		est[2] = 0x00a4;	/* P_adp_regul_ext 0.02 */
+		est[3] = 0xfff8;	/* P_adp_noise_ext -0.001 */
+		break;
+	case QAM_16:
+		est[0] = 0x023d;	/* P_adp_regul_cnt 0.07 */
+		est[1] = 0xffdf;	/* P_adp_noise_cnt -0.004 */
+		est[2] = 0x00a4;	/* P_adp_regul_ext 0.02 */
+		est[3] = 0xfff0;	/* P_adp_noise_ext -0.002 */
+		break;
+	default:
+		est[0] = 0x099a;	/* P_adp_regul_cnt 0.3 */
+		est[1] = 0xffae;	/* P_adp_noise_cnt -0.01 */
+		est[2] = 0x0333;	/* P_adp_regul_ext 0.1 */
+		est[3] = 0xfff8;	/* P_adp_noise_ext -0.002 */
+		break;
 	}
 	for (value = 0; value < 4; value++)
 		dib7000p_write_word(state, 187 + value, est[value]);
@@ -820,14 +1031,15 @@ static int dib7000p_autosearch_start(struct dvb_frontend *demod, struct dvb_fron
 	struct dib7000p_state *state = demod->demodulator_priv;
 	struct dvb_frontend_parameters schan;
 	u32 value, factor;
+	u32 internal = dib7000p_get_internal_freq(state);
 
 	schan = *ch;
 	schan.u.ofdm.constellation = QAM_64;
-	schan.u.ofdm.guard_interval         = GUARD_INTERVAL_1_32;
-	schan.u.ofdm.transmission_mode          = TRANSMISSION_MODE_8K;
-	schan.u.ofdm.code_rate_HP  = FEC_2_3;
-	schan.u.ofdm.code_rate_LP  = FEC_3_4;
-	schan.u.ofdm.hierarchy_information          = 0;
+	schan.u.ofdm.guard_interval = GUARD_INTERVAL_1_32;
+	schan.u.ofdm.transmission_mode = TRANSMISSION_MODE_8K;
+	schan.u.ofdm.code_rate_HP = FEC_2_3;
+	schan.u.ofdm.code_rate_LP = FEC_3_4;
+	schan.u.ofdm.hierarchy_information = 0;
 
 	dib7000p_set_channel(state, &schan, 7);
 
@@ -837,16 +1049,15 @@ static int dib7000p_autosearch_start(struct dvb_frontend *demod, struct dvb_fron
 	else
 		factor = 6;
 
-	// always use the setting for 8MHz here lock_time for 7,6 MHz are longer
-	value = 30 * state->cfg.bw->internal * factor;
-	dib7000p_write_word(state, 6,  (u16) ((value >> 16) & 0xffff)); // lock0 wait time
-	dib7000p_write_word(state, 7,  (u16)  (value        & 0xffff)); // lock0 wait time
-	value = 100 * state->cfg.bw->internal * factor;
-	dib7000p_write_word(state, 8,  (u16) ((value >> 16) & 0xffff)); // lock1 wait time
-	dib7000p_write_word(state, 9,  (u16)  (value        & 0xffff)); // lock1 wait time
-	value = 500 * state->cfg.bw->internal * factor;
-	dib7000p_write_word(state, 10, (u16) ((value >> 16) & 0xffff)); // lock2 wait time
-	dib7000p_write_word(state, 11, (u16)  (value        & 0xffff)); // lock2 wait time
+	value = 30 * internal * factor;
+	dib7000p_write_word(state, 6, (u16) ((value >> 16) & 0xffff));
+	dib7000p_write_word(state, 7, (u16) (value & 0xffff));
+	value = 100 * internal * factor;
+	dib7000p_write_word(state, 8, (u16) ((value >> 16) & 0xffff));
+	dib7000p_write_word(state, 9, (u16) (value & 0xffff));
+	value = 500 * internal * factor;
+	dib7000p_write_word(state, 10, (u16) ((value >> 16) & 0xffff));
+	dib7000p_write_word(state, 11, (u16) (value & 0xffff));
 
 	value = dib7000p_read_word(state, 0);
 	dib7000p_write_word(state, 0, (u16) ((1 << 9) | value));
@@ -861,101 +1072,101 @@ static int dib7000p_autosearch_is_irq(struct dvb_frontend *demod)
 	struct dib7000p_state *state = demod->demodulator_priv;
 	u16 irq_pending = dib7000p_read_word(state, 1284);
 
-	if (irq_pending & 0x1) // failed
+	if (irq_pending & 0x1)
 		return 1;
 
-	if (irq_pending & 0x2) // succeeded
+	if (irq_pending & 0x2)
 		return 2;
 
-	return 0; // still pending
+	return 0;
 }
 
 static void dib7000p_spur_protect(struct dib7000p_state *state, u32 rf_khz, u32 bw)
 {
-	static s16 notch[]={16143, 14402, 12238, 9713, 6902, 3888, 759, -2392};
-	static u8 sine [] ={0, 2, 3, 5, 6, 8, 9, 11, 13, 14, 16, 17, 19, 20, 22,
-	24, 25, 27, 28, 30, 31, 33, 34, 36, 38, 39, 41, 42, 44, 45, 47, 48, 50, 51,
-	53, 55, 56, 58, 59, 61, 62, 64, 65, 67, 68, 70, 71, 73, 74, 76, 77, 79, 80,
-	82, 83, 85, 86, 88, 89, 91, 92, 94, 95, 97, 98, 99, 101, 102, 104, 105,
-	107, 108, 109, 111, 112, 114, 115, 117, 118, 119, 121, 122, 123, 125, 126,
-	128, 129, 130, 132, 133, 134, 136, 137, 138, 140, 141, 142, 144, 145, 146,
-	147, 149, 150, 151, 152, 154, 155, 156, 157, 159, 160, 161, 162, 164, 165,
-	166, 167, 168, 170, 171, 172, 173, 174, 175, 177, 178, 179, 180, 181, 182,
-	183, 184, 185, 186, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198,
-	199, 200, 201, 202, 203, 204, 205, 206, 207, 207, 208, 209, 210, 211, 212,
-	213, 214, 215, 215, 216, 217, 218, 219, 220, 220, 221, 222, 223, 224, 224,
-	225, 226, 227, 227, 228, 229, 229, 230, 231, 231, 232, 233, 233, 234, 235,
-	235, 236, 237, 237, 238, 238, 239, 239, 240, 241, 241, 242, 242, 243, 243,
-	244, 244, 245, 245, 245, 246, 246, 247, 247, 248, 248, 248, 249, 249, 249,
-	250, 250, 250, 251, 251, 251, 252, 252, 252, 252, 253, 253, 253, 253, 254,
-	254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
-	255, 255, 255, 255, 255, 255};
+	static s16 notch[] = { 16143, 14402, 12238, 9713, 6902, 3888, 759, -2392 };
+	static u8 sine[] = { 0, 2, 3, 5, 6, 8, 9, 11, 13, 14, 16, 17, 19, 20, 22,
+		24, 25, 27, 28, 30, 31, 33, 34, 36, 38, 39, 41, 42, 44, 45, 47, 48, 50, 51,
+		53, 55, 56, 58, 59, 61, 62, 64, 65, 67, 68, 70, 71, 73, 74, 76, 77, 79, 80,
+		82, 83, 85, 86, 88, 89, 91, 92, 94, 95, 97, 98, 99, 101, 102, 104, 105,
+		107, 108, 109, 111, 112, 114, 115, 117, 118, 119, 121, 122, 123, 125, 126,
+		128, 129, 130, 132, 133, 134, 136, 137, 138, 140, 141, 142, 144, 145, 146,
+		147, 149, 150, 151, 152, 154, 155, 156, 157, 159, 160, 161, 162, 164, 165,
+		166, 167, 168, 170, 171, 172, 173, 174, 175, 177, 178, 179, 180, 181, 182,
+		183, 184, 185, 186, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198,
+		199, 200, 201, 202, 203, 204, 205, 206, 207, 207, 208, 209, 210, 211, 212,
+		213, 214, 215, 215, 216, 217, 218, 219, 220, 220, 221, 222, 223, 224, 224,
+		225, 226, 227, 227, 228, 229, 229, 230, 231, 231, 232, 233, 233, 234, 235,
+		235, 236, 237, 237, 238, 238, 239, 239, 240, 241, 241, 242, 242, 243, 243,
+		244, 244, 245, 245, 245, 246, 246, 247, 247, 248, 248, 248, 249, 249, 249,
+		250, 250, 250, 251, 251, 251, 252, 252, 252, 252, 253, 253, 253, 253, 254,
+		254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+		255, 255, 255, 255, 255, 255
+	};
 
 	u32 xtal = state->cfg.bw->xtal_hz / 1000;
 	int f_rel = DIV_ROUND_CLOSEST(rf_khz, xtal) * xtal - rf_khz;
 	int k;
-	int coef_re[8],coef_im[8];
+	int coef_re[8], coef_im[8];
 	int bw_khz = bw;
 	u32 pha;
 
-	dprintk( "relative position of the Spur: %dk (RF: %dk, XTAL: %dk)", f_rel, rf_khz, xtal);
-
+	dprintk("relative position of the Spur: %dk (RF: %dk, XTAL: %dk)", f_rel, rf_khz, xtal);
 
-	if (f_rel < -bw_khz/2 || f_rel > bw_khz/2)
+	if (f_rel < -bw_khz / 2 || f_rel > bw_khz / 2)
 		return;
 
 	bw_khz /= 100;
 
-	dib7000p_write_word(state, 142 ,0x0610);
+	dib7000p_write_word(state, 142, 0x0610);
 
 	for (k = 0; k < 8; k++) {
-		pha = ((f_rel * (k+1) * 112 * 80/bw_khz) /1000) & 0x3ff;
+		pha = ((f_rel * (k + 1) * 112 * 80 / bw_khz) / 1000) & 0x3ff;
 
-		if (pha==0) {
+		if (pha == 0) {
 			coef_re[k] = 256;
 			coef_im[k] = 0;
-		} else if(pha < 256) {
-			coef_re[k] = sine[256-(pha&0xff)];
-			coef_im[k] = sine[pha&0xff];
+		} else if (pha < 256) {
+			coef_re[k] = sine[256 - (pha & 0xff)];
+			coef_im[k] = sine[pha & 0xff];
 		} else if (pha == 256) {
 			coef_re[k] = 0;
 			coef_im[k] = 256;
 		} else if (pha < 512) {
-			coef_re[k] = -sine[pha&0xff];
-			coef_im[k] = sine[256 - (pha&0xff)];
+			coef_re[k] = -sine[pha & 0xff];
+			coef_im[k] = sine[256 - (pha & 0xff)];
 		} else if (pha == 512) {
 			coef_re[k] = -256;
 			coef_im[k] = 0;
 		} else if (pha < 768) {
-			coef_re[k] = -sine[256-(pha&0xff)];
-			coef_im[k] = -sine[pha&0xff];
+			coef_re[k] = -sine[256 - (pha & 0xff)];
+			coef_im[k] = -sine[pha & 0xff];
 		} else if (pha == 768) {
 			coef_re[k] = 0;
 			coef_im[k] = -256;
 		} else {
-			coef_re[k] = sine[pha&0xff];
-			coef_im[k] = -sine[256 - (pha&0xff)];
+			coef_re[k] = sine[pha & 0xff];
+			coef_im[k] = -sine[256 - (pha & 0xff)];
 		}
 
 		coef_re[k] *= notch[k];
-		coef_re[k] += (1<<14);
-		if (coef_re[k] >= (1<<24))
-			coef_re[k]  = (1<<24) - 1;
-		coef_re[k] /= (1<<15);
+		coef_re[k] += (1 << 14);
+		if (coef_re[k] >= (1 << 24))
+			coef_re[k] = (1 << 24) - 1;
+		coef_re[k] /= (1 << 15);
 
 		coef_im[k] *= notch[k];
-		coef_im[k] += (1<<14);
-		if (coef_im[k] >= (1<<24))
-			coef_im[k]  = (1<<24)-1;
-		coef_im[k] /= (1<<15);
+		coef_im[k] += (1 << 14);
+		if (coef_im[k] >= (1 << 24))
+			coef_im[k] = (1 << 24) - 1;
+		coef_im[k] /= (1 << 15);
 
-		dprintk( "PALF COEF: %d re: %d im: %d", k, coef_re[k], coef_im[k]);
+		dprintk("PALF COEF: %d re: %d im: %d", k, coef_re[k], coef_im[k]);
 
 		dib7000p_write_word(state, 143, (0 << 14) | (k << 10) | (coef_re[k] & 0x3ff));
 		dib7000p_write_word(state, 144, coef_im[k] & 0x3ff);
 		dib7000p_write_word(state, 143, (1 << 14) | (k << 10) | (coef_re[k] & 0x3ff));
 	}
-	dib7000p_write_word(state,143 ,0);
+	dib7000p_write_word(state, 143, 0);
 }
 
 static int dib7000p_tune(struct dvb_frontend *demod, struct dvb_frontend_parameters *ch)
@@ -976,11 +1187,11 @@ static int dib7000p_tune(struct dvb_frontend *demod, struct dvb_frontend_paramet
 	/* P_ctrl_inh_cor=0, P_ctrl_alpha_cor=4, P_ctrl_inh_isi=0, P_ctrl_alpha_isi=3, P_ctrl_inh_cor4=1, P_ctrl_alpha_cor4=3 */
 	tmp = (0 << 14) | (4 << 10) | (0 << 9) | (3 << 5) | (1 << 4) | (0x3);
 	if (state->sfn_workaround_active) {
-		dprintk( "SFN workaround is active");
+		dprintk("SFN workaround is active");
 		tmp |= (1 << 9);
-		dib7000p_write_word(state, 166, 0x4000); // P_pha3_force_pha_shift
+		dib7000p_write_word(state, 166, 0x4000);
 	} else {
-		dib7000p_write_word(state, 166, 0x0000); // P_pha3_force_pha_shift
+		dib7000p_write_word(state, 166, 0x0000);
 	}
 	dib7000p_write_word(state, 29, tmp);
 
@@ -993,51 +1204,72 @@ static int dib7000p_tune(struct dvb_frontend *demod, struct dvb_frontend_paramet
 	/* P_timf_alpha, P_corm_alpha=6, P_corm_thres=0x80 */
 	tmp = (6 << 8) | 0x80;
 	switch (ch->u.ofdm.transmission_mode) {
-		case TRANSMISSION_MODE_2K: tmp |= (7 << 12); break;
-		case TRANSMISSION_MODE_4K: tmp |= (8 << 12); break;
-		default:
-		case TRANSMISSION_MODE_8K: tmp |= (9 << 12); break;
+	case TRANSMISSION_MODE_2K:
+		tmp |= (2 << 12);
+		break;
+	case TRANSMISSION_MODE_4K:
+		tmp |= (3 << 12);
+		break;
+	default:
+	case TRANSMISSION_MODE_8K:
+		tmp |= (4 << 12);
+		break;
 	}
-	dib7000p_write_word(state, 26, tmp);  /* timf_a(6xxx) */
+	dib7000p_write_word(state, 26, tmp);	/* timf_a(6xxx) */
 
 	/* P_ctrl_freeze_pha_shift=0, P_ctrl_pha_off_max */
 	tmp = (0 << 4);
 	switch (ch->u.ofdm.transmission_mode) {
-		case TRANSMISSION_MODE_2K: tmp |= 0x6; break;
-		case TRANSMISSION_MODE_4K: tmp |= 0x7; break;
-		default:
-		case TRANSMISSION_MODE_8K: tmp |= 0x8; break;
+	case TRANSMISSION_MODE_2K:
+		tmp |= 0x6;
+		break;
+	case TRANSMISSION_MODE_4K:
+		tmp |= 0x7;
+		break;
+	default:
+	case TRANSMISSION_MODE_8K:
+		tmp |= 0x8;
+		break;
 	}
-	dib7000p_write_word(state, 32,  tmp);
+	dib7000p_write_word(state, 32, tmp);
 
 	/* P_ctrl_sfreq_inh=0, P_ctrl_sfreq_step */
 	tmp = (0 << 4);
 	switch (ch->u.ofdm.transmission_mode) {
-		case TRANSMISSION_MODE_2K: tmp |= 0x6; break;
-		case TRANSMISSION_MODE_4K: tmp |= 0x7; break;
-		default:
-		case TRANSMISSION_MODE_8K: tmp |= 0x8; break;
+	case TRANSMISSION_MODE_2K:
+		tmp |= 0x6;
+		break;
+	case TRANSMISSION_MODE_4K:
+		tmp |= 0x7;
+		break;
+	default:
+	case TRANSMISSION_MODE_8K:
+		tmp |= 0x8;
+		break;
 	}
-	dib7000p_write_word(state, 33,  tmp);
+	dib7000p_write_word(state, 33, tmp);
 
-	tmp = dib7000p_read_word(state,509);
+	tmp = dib7000p_read_word(state, 509);
 	if (!((tmp >> 6) & 0x1)) {
 		/* restart the fec */
-		tmp = dib7000p_read_word(state,771);
+		tmp = dib7000p_read_word(state, 771);
 		dib7000p_write_word(state, 771, tmp | (1 << 1));
 		dib7000p_write_word(state, 771, tmp);
-		msleep(10);
-		tmp = dib7000p_read_word(state,509);
+		msleep(40);
+		tmp = dib7000p_read_word(state, 509);
 	}
-
 	// we achieved a lock - it's time to update the osc freq
-	if ((tmp >> 6) & 0x1)
+	if ((tmp >> 6) & 0x1) {
 		dib7000p_update_timf(state);
+		/* P_timf_alpha += 2 */
+		tmp = dib7000p_read_word(state, 26);
+		dib7000p_write_word(state, 26, (tmp & ~(0xf << 12)) | ((((tmp >> 12) & 0xf) + 5) << 12));
+	}
 
 	if (state->cfg.spur_protect)
-		dib7000p_spur_protect(state, ch->frequency/1000, BANDWIDTH_TO_KHZ(ch->u.ofdm.bandwidth));
+		dib7000p_spur_protect(state, ch->frequency / 1000, BANDWIDTH_TO_KHZ(ch->u.ofdm.bandwidth));
 
-    dib7000p_set_bandwidth(state, BANDWIDTH_TO_KHZ(ch->u.ofdm.bandwidth));
+	dib7000p_set_bandwidth(state, BANDWIDTH_TO_KHZ(ch->u.ofdm.bandwidth));
 	return 0;
 }
 
@@ -1046,63 +1278,82 @@ static int dib7000p_wakeup(struct dvb_frontend *demod)
 	struct dib7000p_state *state = demod->demodulator_priv;
 	dib7000p_set_power_mode(state, DIB7000P_POWER_ALL);
 	dib7000p_set_adc_state(state, DIBX000_SLOW_ADC_ON);
+	if (state->version == SOC7090)
+		dib7000p_sad_calib(state);
 	return 0;
 }
 
 static int dib7000p_sleep(struct dvb_frontend *demod)
 {
 	struct dib7000p_state *state = demod->demodulator_priv;
+	if (state->version == SOC7090)
+		return dib7090_set_output_mode(demod, OUTMODE_HIGH_Z) | dib7000p_set_power_mode(state, DIB7000P_POWER_INTERFACE_ONLY);
 	return dib7000p_set_output_mode(state, OUTMODE_HIGH_Z) | dib7000p_set_power_mode(state, DIB7000P_POWER_INTERFACE_ONLY);
 }
 
 static int dib7000p_identify(struct dib7000p_state *st)
 {
 	u16 value;
-	dprintk( "checking demod on I2C address: %d (%x)",
-		st->i2c_addr, st->i2c_addr);
+	dprintk("checking demod on I2C address: %d (%x)", st->i2c_addr, st->i2c_addr);
 
 	if ((value = dib7000p_read_word(st, 768)) != 0x01b3) {
-		dprintk( "wrong Vendor ID (read=0x%x)",value);
+		dprintk("wrong Vendor ID (read=0x%x)", value);
 		return -EREMOTEIO;
 	}
 
 	if ((value = dib7000p_read_word(st, 769)) != 0x4000) {
-		dprintk( "wrong Device ID (%x)",value);
+		dprintk("wrong Device ID (%x)", value);
 		return -EREMOTEIO;
 	}
 
 	return 0;
 }
 
-
-static int dib7000p_get_frontend(struct dvb_frontend* fe,
-				struct dvb_frontend_parameters *fep)
+static int dib7000p_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
-	u16 tps = dib7000p_read_word(state,463);
+	u16 tps = dib7000p_read_word(state, 463);
 
 	fep->inversion = INVERSION_AUTO;
 
 	fep->u.ofdm.bandwidth = BANDWIDTH_TO_INDEX(state->current_bandwidth);
 
 	switch ((tps >> 8) & 0x3) {
-		case 0: fep->u.ofdm.transmission_mode = TRANSMISSION_MODE_2K; break;
-		case 1: fep->u.ofdm.transmission_mode = TRANSMISSION_MODE_8K; break;
-		/* case 2: fep->u.ofdm.transmission_mode = TRANSMISSION_MODE_4K; break; */
+	case 0:
+		fep->u.ofdm.transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case 1:
+		fep->u.ofdm.transmission_mode = TRANSMISSION_MODE_8K;
+		break;
+	/* case 2: fep->u.ofdm.transmission_mode = TRANSMISSION_MODE_4K; break; */
 	}
 
 	switch (tps & 0x3) {
-		case 0: fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_32; break;
-		case 1: fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_16; break;
-		case 2: fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_8; break;
-		case 3: fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_4; break;
+	case 0:
+		fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case 1:
+		fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case 2:
+		fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case 3:
+		fep->u.ofdm.guard_interval = GUARD_INTERVAL_1_4;
+		break;
 	}
 
 	switch ((tps >> 14) & 0x3) {
-		case 0: fep->u.ofdm.constellation = QPSK; break;
-		case 1: fep->u.ofdm.constellation = QAM_16; break;
-		case 2:
-		default: fep->u.ofdm.constellation = QAM_64; break;
+	case 0:
+		fep->u.ofdm.constellation = QPSK;
+		break;
+	case 1:
+		fep->u.ofdm.constellation = QAM_16;
+		break;
+	case 2:
+	default:
+		fep->u.ofdm.constellation = QAM_64;
+		break;
 	}
 
 	/* as long as the frontend_param structure is fixed for hierarchical transmission I refuse to use it */
@@ -1110,22 +1361,42 @@ static int dib7000p_get_frontend(struct dvb_frontend* fe,
 
 	fep->u.ofdm.hierarchy_information = HIERARCHY_NONE;
 	switch ((tps >> 5) & 0x7) {
-		case 1: fep->u.ofdm.code_rate_HP = FEC_1_2; break;
-		case 2: fep->u.ofdm.code_rate_HP = FEC_2_3; break;
-		case 3: fep->u.ofdm.code_rate_HP = FEC_3_4; break;
-		case 5: fep->u.ofdm.code_rate_HP = FEC_5_6; break;
-		case 7:
-		default: fep->u.ofdm.code_rate_HP = FEC_7_8; break;
+	case 1:
+		fep->u.ofdm.code_rate_HP = FEC_1_2;
+		break;
+	case 2:
+		fep->u.ofdm.code_rate_HP = FEC_2_3;
+		break;
+	case 3:
+		fep->u.ofdm.code_rate_HP = FEC_3_4;
+		break;
+	case 5:
+		fep->u.ofdm.code_rate_HP = FEC_5_6;
+		break;
+	case 7:
+	default:
+		fep->u.ofdm.code_rate_HP = FEC_7_8;
+		break;
 
 	}
 
 	switch ((tps >> 2) & 0x7) {
-		case 1: fep->u.ofdm.code_rate_LP = FEC_1_2; break;
-		case 2: fep->u.ofdm.code_rate_LP = FEC_2_3; break;
-		case 3: fep->u.ofdm.code_rate_LP = FEC_3_4; break;
-		case 5: fep->u.ofdm.code_rate_LP = FEC_5_6; break;
-		case 7:
-		default: fep->u.ofdm.code_rate_LP = FEC_7_8; break;
+	case 1:
+		fep->u.ofdm.code_rate_LP = FEC_1_2;
+		break;
+	case 2:
+		fep->u.ofdm.code_rate_LP = FEC_2_3;
+		break;
+	case 3:
+		fep->u.ofdm.code_rate_LP = FEC_3_4;
+		break;
+	case 5:
+		fep->u.ofdm.code_rate_LP = FEC_5_6;
+		break;
+	case 7:
+	default:
+		fep->u.ofdm.code_rate_LP = FEC_7_8;
+		break;
 	}
 
 	/* native interleaver: (dib7000p_read_word(state, 464) >>  5) & 0x1 */
@@ -1133,15 +1404,18 @@ static int dib7000p_get_frontend(struct dvb_frontend* fe,
 	return 0;
 }
 
-static int dib7000p_set_frontend(struct dvb_frontend* fe,
-				struct dvb_frontend_parameters *fep)
+static int dib7000p_set_frontend(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	int time, ret;
 
-	dib7000p_set_output_mode(state, OUTMODE_HIGH_Z);
+	if (state->version == SOC7090) {
+		dib7090_set_diversity_in(fe, 0);
+		dib7090_set_output_mode(fe, OUTMODE_HIGH_Z);
+	} else
+		dib7000p_set_output_mode(state, OUTMODE_HIGH_Z);
 
-    /* maybe the parameter has been changed */
+	/* maybe the parameter has been changed */
 	state->sfn_workaround_active = buggy_sfn_workaround;
 
 	if (fe->ops.tuner_ops.set_params)
@@ -1156,9 +1430,7 @@ static int dib7000p_set_frontend(struct dvb_frontend* fe,
 	} while (time != -1);
 
 	if (fep->u.ofdm.transmission_mode == TRANSMISSION_MODE_AUTO ||
-		fep->u.ofdm.guard_interval    == GUARD_INTERVAL_AUTO ||
-		fep->u.ofdm.constellation     == QAM_AUTO ||
-		fep->u.ofdm.code_rate_HP      == FEC_AUTO) {
+		fep->u.ofdm.guard_interval == GUARD_INTERVAL_AUTO || fep->u.ofdm.constellation == QAM_AUTO || fep->u.ofdm.code_rate_HP == FEC_AUTO) {
 		int i = 800, found;
 
 		dib7000p_autosearch_start(fe, fep);
@@ -1167,9 +1439,9 @@ static int dib7000p_set_frontend(struct dvb_frontend* fe,
 			found = dib7000p_autosearch_is_irq(fe);
 		} while (found == 0 && i--);
 
-		dprintk("autosearch returns: %d",found);
+		dprintk("autosearch returns: %d", found);
 		if (found == 0 || found == 1)
-			return 0; // no channel found
+			return 0;
 
 		dib7000p_get_frontend(fe, fep);
 	}
@@ -1177,11 +1449,15 @@ static int dib7000p_set_frontend(struct dvb_frontend* fe,
 	ret = dib7000p_tune(fe, fep);
 
 	/* make this a config parameter */
-	dib7000p_set_output_mode(state, state->cfg.output_mode);
-    return ret;
+	if (state->version == SOC7090)
+		dib7090_set_output_mode(fe, state->cfg.output_mode);
+	else
+		dib7000p_set_output_mode(state, state->cfg.output_mode);
+
+	return ret;
 }
 
-static int dib7000p_read_status(struct dvb_frontend *fe, fe_status_t *stat)
+static int dib7000p_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	u16 lock = dib7000p_read_word(state, 509);
@@ -1196,27 +1472,27 @@ static int dib7000p_read_status(struct dvb_frontend *fe, fe_status_t *stat)
 		*stat |= FE_HAS_VITERBI;
 	if (lock & 0x0010)
 		*stat |= FE_HAS_SYNC;
-    if ((lock & 0x0038) == 0x38)
+	if ((lock & 0x0038) == 0x38)
 		*stat |= FE_HAS_LOCK;
 
 	return 0;
 }
 
-static int dib7000p_read_ber(struct dvb_frontend *fe, u32 *ber)
+static int dib7000p_read_ber(struct dvb_frontend *fe, u32 * ber)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	*ber = (dib7000p_read_word(state, 500) << 16) | dib7000p_read_word(state, 501);
 	return 0;
 }
 
-static int dib7000p_read_unc_blocks(struct dvb_frontend *fe, u32 *unc)
+static int dib7000p_read_unc_blocks(struct dvb_frontend *fe, u32 * unc)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	*unc = dib7000p_read_word(state, 506);
 	return 0;
 }
 
-static int dib7000p_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
+static int dib7000p_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	u16 val = dib7000p_read_word(state, 394);
@@ -1224,7 +1500,7 @@ static int dib7000p_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 	return 0;
 }
 
-static int dib7000p_read_snr(struct dvb_frontend* fe, u16 *snr)
+static int dib7000p_read_snr(struct dvb_frontend *fe, u16 * snr)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	u16 val;
@@ -1240,19 +1516,17 @@ static int dib7000p_read_snr(struct dvb_frontend* fe, u16 *snr)
 		noise_exp -= 0x40;
 
 	signal_mant = (val >> 6) & 0xFF;
-	signal_exp  = (val & 0x3F);
+	signal_exp = (val & 0x3F);
 	if ((signal_exp & 0x20) != 0)
 		signal_exp -= 0x40;
 
 	if (signal_mant != 0)
-		result = intlog10(2) * 10 * signal_exp + 10 *
-			intlog10(signal_mant);
+		result = intlog10(2) * 10 * signal_exp + 10 * intlog10(signal_mant);
 	else
 		result = intlog10(2) * 10 * signal_exp - 100;
 
 	if (noise_mant != 0)
-		result -= intlog10(2) * 10 * noise_exp + 10 *
-			intlog10(noise_mant);
+		result -= intlog10(2) * 10 * noise_exp + 10 * intlog10(noise_mant);
 	else
 		result -= intlog10(2) * 10 * noise_exp - 100;
 
@@ -1260,7 +1534,7 @@ static int dib7000p_read_snr(struct dvb_frontend* fe, u16 *snr)
 	return 0;
 }
 
-static int dib7000p_fe_get_tune_settings(struct dvb_frontend* fe, struct dvb_frontend_tune_settings *tune)
+static int dib7000p_fe_get_tune_settings(struct dvb_frontend *fe, struct dvb_frontend_tune_settings *tune)
 {
 	tune->min_delay_ms = 1000;
 	return 0;
@@ -1270,6 +1544,7 @@ static void dib7000p_release(struct dvb_frontend *demod)
 {
 	struct dib7000p_state *st = demod->demodulator_priv;
 	dibx000_exit_i2c_master(&st->i2c_master);
+	i2c_del_adapter(&st->dib7090_tuner_adap);
 	kfree(st);
 }
 
@@ -1277,8 +1552,8 @@ int dib7000pc_detection(struct i2c_adapter *i2c_adap)
 {
 	u8 tx[2], rx[2];
 	struct i2c_msg msg[2] = {
-		{ .addr = 18 >> 1, .flags = 0,        .buf = tx, .len = 2 },
-		{ .addr = 18 >> 1, .flags = I2C_M_RD, .buf = rx, .len = 2 },
+		{.addr = 18 >> 1, .flags = 0, .buf = tx, .len = 2},
+		{.addr = 18 >> 1, .flags = I2C_M_RD, .buf = rx, .len = 2},
 	};
 
 	tx[0] = 0x03;
@@ -1303,7 +1578,7 @@ int dib7000pc_detection(struct i2c_adapter *i2c_adap)
 }
 EXPORT_SYMBOL(dib7000pc_detection);
 
-struct i2c_adapter * dib7000p_get_i2c_master(struct dvb_frontend *demod, enum dibx000_i2c_interface intf, int gating)
+struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *demod, enum dibx000_i2c_interface intf, int gating)
 {
 	struct dib7000p_state *st = demod->demodulator_priv;
 	return dibx000_get_i2c_adapter(&st->i2c_master, intf, gating);
@@ -1312,19 +1587,19 @@ EXPORT_SYMBOL(dib7000p_get_i2c_master);
 
 int dib7000p_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 {
-    struct dib7000p_state *state = fe->demodulator_priv;
-    u16 val = dib7000p_read_word(state, 235) & 0xffef;
-    val |= (onoff & 0x1) << 4;
-    dprintk("PID filter enabled %d", onoff);
-    return dib7000p_write_word(state, 235, val);
+	struct dib7000p_state *state = fe->demodulator_priv;
+	u16 val = dib7000p_read_word(state, 235) & 0xffef;
+	val |= (onoff & 0x1) << 4;
+	dprintk("PID filter enabled %d", onoff);
+	return dib7000p_write_word(state, 235, val);
 }
 EXPORT_SYMBOL(dib7000p_pid_filter_ctrl);
 
 int dib7000p_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
 {
-    struct dib7000p_state *state = fe->demodulator_priv;
-    dprintk("PID filter: index %x, PID %d, OnOff %d", id, pid, onoff);
-    return dib7000p_write_word(state, 241 + id, onoff ? (1 << 13) | pid : 0);
+	struct dib7000p_state *state = fe->demodulator_priv;
+	dprintk("PID filter: index %x, PID %d, OnOff %d", id, pid, onoff);
+	return dib7000p_write_word(state, 241 + id, onoff ? (1 << 13) | pid : 0);
 }
 EXPORT_SYMBOL(dib7000p_pid_filter);
 
@@ -1340,16 +1615,19 @@ int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 defau
 
 	dpst->i2c_adap = i2c;
 
-	for (k = no_of_demods-1; k >= 0; k--) {
+	for (k = no_of_demods - 1; k >= 0; k--) {
 		dpst->cfg = cfg[k];
 
 		/* designated i2c address */
-		new_addr          = (0x40 + k) << 1;
+		if (cfg[k].default_i2c_addr != 0)
+			new_addr = cfg[k].default_i2c_addr + (k << 1);
+		else
+			new_addr = (0x40 + k) << 1;
 		dpst->i2c_addr = new_addr;
-		dib7000p_write_word(dpst, 1287, 0x0003); /* sram lead in, rdy */
+		dib7000p_write_word(dpst, 1287, 0x0003);	/* sram lead in, rdy */
 		if (dib7000p_identify(dpst) != 0) {
 			dpst->i2c_addr = default_addr;
-			dib7000p_write_word(dpst, 1287, 0x0003); /* sram lead in, rdy */
+			dib7000p_write_word(dpst, 1287, 0x0003);	/* sram lead in, rdy */
 			if (dib7000p_identify(dpst) != 0) {
 				dprintk("DiB7000P #%d: not identified\n", k);
 				kfree(dpst);
@@ -1368,7 +1646,10 @@ int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 defau
 
 	for (k = 0; k < no_of_demods; k++) {
 		dpst->cfg = cfg[k];
-		dpst->i2c_addr = (0x40 + k) << 1;
+		if (cfg[k].default_i2c_addr != 0)
+			dpst->i2c_addr = (cfg[k].default_i2c_addr + k) << 1;
+		else
+			dpst->i2c_addr = (0x40 + k) << 1;
 
 		// unforce divstr
 		dib7000p_write_word(dpst, 1285, dpst->i2c_addr << 2);
@@ -1382,8 +1663,613 @@ int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 defau
 }
 EXPORT_SYMBOL(dib7000p_i2c_enumeration);
 
+static const s32 lut_1000ln_mant[] = {
+	6908, 6956, 7003, 7047, 7090, 7131, 7170, 7208, 7244, 7279, 7313, 7346, 7377, 7408, 7438, 7467, 7495, 7523, 7549, 7575, 7600
+};
+
+static s32 dib7000p_get_adc_power(struct dvb_frontend *fe)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	u32 tmp_val = 0, exp = 0, mant = 0;
+	s32 pow_i;
+	u16 buf[2];
+	u8 ix = 0;
+
+	buf[0] = dib7000p_read_word(state, 0x184);
+	buf[1] = dib7000p_read_word(state, 0x185);
+	pow_i = (buf[0] << 16) | buf[1];
+	dprintk("raw pow_i = %d", pow_i);
+
+	tmp_val = pow_i;
+	while (tmp_val >>= 1)
+		exp++;
+
+	mant = (pow_i * 1000 / (1 << exp));
+	dprintk(" mant = %d exp = %d", mant / 1000, exp);
+
+	ix = (u8) ((mant - 1000) / 100);	/* index of the LUT */
+	dprintk(" ix = %d", ix);
+
+	pow_i = (lut_1000ln_mant[ix] + 693 * (exp - 20) - 6908);
+	pow_i = (pow_i << 8) / 1000;
+	dprintk(" pow_i = %d", pow_i);
+
+	return pow_i;
+}
+
+static int map_addr_to_serpar_number(struct i2c_msg *msg)
+{
+	if ((msg->buf[0] <= 15))
+		msg->buf[0] -= 1;
+	else if (msg->buf[0] == 17)
+		msg->buf[0] = 15;
+	else if (msg->buf[0] == 16)
+		msg->buf[0] = 17;
+	else if (msg->buf[0] == 19)
+		msg->buf[0] = 16;
+	else if (msg->buf[0] >= 21 && msg->buf[0] <= 25)
+		msg->buf[0] -= 3;
+	else if (msg->buf[0] == 28)
+		msg->buf[0] = 23;
+	else
+		return -EINVAL;
+	return 0;
+}
+
+static int w7090p_tuner_write_serpar(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dib7000p_state *state = i2c_get_adapdata(i2c_adap);
+	u8 n_overflow = 1;
+	u16 i = 1000;
+	u16 serpar_num = msg[0].buf[0];
+
+	while (n_overflow == 1 && i) {
+		n_overflow = (dib7000p_read_word(state, 1984) >> 1) & 0x1;
+		i--;
+		if (i == 0)
+			dprintk("Tuner ITF: write busy (overflow)");
+	}
+	dib7000p_write_word(state, 1985, (1 << 6) | (serpar_num & 0x3f));
+	dib7000p_write_word(state, 1986, (msg[0].buf[1] << 8) | msg[0].buf[2]);
+
+	return num;
+}
+
+static int w7090p_tuner_read_serpar(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dib7000p_state *state = i2c_get_adapdata(i2c_adap);
+	u8 n_overflow = 1, n_empty = 1;
+	u16 i = 1000;
+	u16 serpar_num = msg[0].buf[0];
+	u16 read_word;
+
+	while (n_overflow == 1 && i) {
+		n_overflow = (dib7000p_read_word(state, 1984) >> 1) & 0x1;
+		i--;
+		if (i == 0)
+			dprintk("TunerITF: read busy (overflow)");
+	}
+	dib7000p_write_word(state, 1985, (0 << 6) | (serpar_num & 0x3f));
+
+	i = 1000;
+	while (n_empty == 1 && i) {
+		n_empty = dib7000p_read_word(state, 1984) & 0x1;
+		i--;
+		if (i == 0)
+			dprintk("TunerITF: read busy (empty)");
+	}
+	read_word = dib7000p_read_word(state, 1987);
+	msg[1].buf[0] = (read_word >> 8) & 0xff;
+	msg[1].buf[1] = (read_word) & 0xff;
+
+	return num;
+}
+
+static int w7090p_tuner_rw_serpar(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	if (map_addr_to_serpar_number(&msg[0]) == 0) {	/* else = Tuner regs to ignore : DIG_CFG, CTRL_RF_LT, PLL_CFG, PWM1_REG, ADCCLK, DIG_CFG_3; SLEEP_EN... */
+		if (num == 1) {	/* write */
+			return w7090p_tuner_write_serpar(i2c_adap, msg, 1);
+		} else {	/* read */
+			return w7090p_tuner_read_serpar(i2c_adap, msg, 2);
+		}
+	}
+	return num;
+}
+
+int dib7090p_rw_on_apb(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num, u16 apb_address)
+{
+	struct dib7000p_state *state = i2c_get_adapdata(i2c_adap);
+	u16 word;
+
+	if (num == 1) {		/* write */
+		dib7000p_write_word(state, apb_address, ((msg[0].buf[1] << 8) | (msg[0].buf[2])));
+	} else {
+		word = dib7000p_read_word(state, apb_address);
+		msg[1].buf[0] = (word >> 8) & 0xff;
+		msg[1].buf[1] = (word) & 0xff;
+	}
+
+	return num;
+}
+
+static int dib7090_tuner_xfer(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dib7000p_state *state = i2c_get_adapdata(i2c_adap);
+
+	u16 apb_address = 0, word;
+	int i = 0;
+	switch (msg[0].buf[0]) {
+	case 0x12:
+		apb_address = 1920;
+		break;
+	case 0x14:
+		apb_address = 1921;
+		break;
+	case 0x24:
+		apb_address = 1922;
+		break;
+	case 0x1a:
+		apb_address = 1923;
+		break;
+	case 0x22:
+		apb_address = 1924;
+		break;
+	case 0x33:
+		apb_address = 1926;
+		break;
+	case 0x34:
+		apb_address = 1927;
+		break;
+	case 0x35:
+		apb_address = 1928;
+		break;
+	case 0x36:
+		apb_address = 1929;
+		break;
+	case 0x37:
+		apb_address = 1930;
+		break;
+	case 0x38:
+		apb_address = 1931;
+		break;
+	case 0x39:
+		apb_address = 1932;
+		break;
+	case 0x2a:
+		apb_address = 1935;
+		break;
+	case 0x2b:
+		apb_address = 1936;
+		break;
+	case 0x2c:
+		apb_address = 1937;
+		break;
+	case 0x2d:
+		apb_address = 1938;
+		break;
+	case 0x2e:
+		apb_address = 1939;
+		break;
+	case 0x2f:
+		apb_address = 1940;
+		break;
+	case 0x30:
+		apb_address = 1941;
+		break;
+	case 0x31:
+		apb_address = 1942;
+		break;
+	case 0x32:
+		apb_address = 1943;
+		break;
+	case 0x3e:
+		apb_address = 1944;
+		break;
+	case 0x3f:
+		apb_address = 1945;
+		break;
+	case 0x40:
+		apb_address = 1948;
+		break;
+	case 0x25:
+		apb_address = 914;
+		break;
+	case 0x26:
+		apb_address = 915;
+		break;
+	case 0x27:
+		apb_address = 916;
+		break;
+	case 0x28:
+		apb_address = 917;
+		break;
+	case 0x1d:
+		i = ((dib7000p_read_word(state, 72) >> 12) & 0x3);
+		word = dib7000p_read_word(state, 384 + i);
+		msg[1].buf[0] = (word >> 8) & 0xff;
+		msg[1].buf[1] = (word) & 0xff;
+		return num;
+	case 0x1f:
+		if (num == 1) {	/* write */
+			word = (u16) ((msg[0].buf[1] << 8) | msg[0].buf[2]);
+			word &= 0x3;
+			word = (dib7000p_read_word(state, 72) & ~(3 << 12)) | (word << 12);
+			dib7000p_write_word(state, 72, word);	/* Set the proper input */
+			return num;
+		}
+	}
+
+	if (apb_address != 0)	/* R/W acces via APB */
+		return dib7090p_rw_on_apb(i2c_adap, msg, num, apb_address);
+	else			/* R/W access via SERPAR  */
+		return w7090p_tuner_rw_serpar(i2c_adap, msg, num);
+
+	return 0;
+}
+
+static u32 dib7000p_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
+
+static struct i2c_algorithm dib7090_tuner_xfer_algo = {
+	.master_xfer = dib7090_tuner_xfer,
+	.functionality = dib7000p_i2c_func,
+};
+
+struct i2c_adapter *dib7090_get_i2c_tuner(struct dvb_frontend *fe)
+{
+	struct dib7000p_state *st = fe->demodulator_priv;
+	return &st->dib7090_tuner_adap;
+}
+EXPORT_SYMBOL(dib7090_get_i2c_tuner);
+
+static int dib7090_host_bus_drive(struct dib7000p_state *state, u8 drive)
+{
+	u16 reg;
+
+	/* drive host bus 2, 3, 4 */
+	reg = dib7000p_read_word(state, 1798) & ~((0x7) | (0x7 << 6) | (0x7 << 12));
+	reg |= (drive << 12) | (drive << 6) | drive;
+	dib7000p_write_word(state, 1798, reg);
+
+	/* drive host bus 5,6 */
+	reg = dib7000p_read_word(state, 1799) & ~((0x7 << 2) | (0x7 << 8));
+	reg |= (drive << 8) | (drive << 2);
+	dib7000p_write_word(state, 1799, reg);
+
+	/* drive host bus 7, 8, 9 */
+	reg = dib7000p_read_word(state, 1800) & ~((0x7) | (0x7 << 6) | (0x7 << 12));
+	reg |= (drive << 12) | (drive << 6) | drive;
+	dib7000p_write_word(state, 1800, reg);
+
+	/* drive host bus 10, 11 */
+	reg = dib7000p_read_word(state, 1801) & ~((0x7 << 2) | (0x7 << 8));
+	reg |= (drive << 8) | (drive << 2);
+	dib7000p_write_word(state, 1801, reg);
+
+	/* drive host bus 12, 13, 14 */
+	reg = dib7000p_read_word(state, 1802) & ~((0x7) | (0x7 << 6) | (0x7 << 12));
+	reg |= (drive << 12) | (drive << 6) | drive;
+	dib7000p_write_word(state, 1802, reg);
+
+	return 0;
+}
+
+static u32 dib7090_calcSyncFreq(u32 P_Kin, u32 P_Kout, u32 insertExtSynchro, u32 syncSize)
+{
+	u32 quantif = 3;
+	u32 nom = (insertExtSynchro * P_Kin + syncSize);
+	u32 denom = P_Kout;
+	u32 syncFreq = ((nom << quantif) / denom);
+
+	if ((syncFreq & ((1 << quantif) - 1)) != 0)
+		syncFreq = (syncFreq >> quantif) + 1;
+	else
+		syncFreq = (syncFreq >> quantif);
+
+	if (syncFreq != 0)
+		syncFreq = syncFreq - 1;
+
+	return syncFreq;
+}
+
+static int dib7090_cfg_DibTx(struct dib7000p_state *state, u32 P_Kin, u32 P_Kout, u32 insertExtSynchro, u32 synchroMode, u32 syncWord, u32 syncSize)
+{
+	u8 index_buf;
+	u16 rx_copy_buf[22];
+
+	dprintk("Configure DibStream Tx");
+	for (index_buf = 0; index_buf < 22; index_buf++)
+		rx_copy_buf[index_buf] = dib7000p_read_word(state, 1536+index_buf);
+
+	dib7000p_write_word(state, 1615, 1);
+	dib7000p_write_word(state, 1603, P_Kin);
+	dib7000p_write_word(state, 1605, P_Kout);
+	dib7000p_write_word(state, 1606, insertExtSynchro);
+	dib7000p_write_word(state, 1608, synchroMode);
+	dib7000p_write_word(state, 1609, (syncWord >> 16) & 0xffff);
+	dib7000p_write_word(state, 1610, syncWord & 0xffff);
+	dib7000p_write_word(state, 1612, syncSize);
+	dib7000p_write_word(state, 1615, 0);
+
+	for (index_buf = 0; index_buf < 22; index_buf++)
+		dib7000p_write_word(state, 1536+index_buf, rx_copy_buf[index_buf]);
+
+	return 0;
+}
+
+static int dib7090_cfg_DibRx(struct dib7000p_state *state, u32 P_Kin, u32 P_Kout, u32 synchroMode, u32 insertExtSynchro, u32 syncWord, u32 syncSize,
+		u32 dataOutRate)
+{
+	u32 syncFreq;
+
+	dprintk("Configure DibStream Rx");
+	if ((P_Kin != 0) && (P_Kout != 0)) {
+		syncFreq = dib7090_calcSyncFreq(P_Kin, P_Kout, insertExtSynchro, syncSize);
+		dib7000p_write_word(state, 1542, syncFreq);
+	}
+	dib7000p_write_word(state, 1554, 1);
+	dib7000p_write_word(state, 1536, P_Kin);
+	dib7000p_write_word(state, 1537, P_Kout);
+	dib7000p_write_word(state, 1539, synchroMode);
+	dib7000p_write_word(state, 1540, (syncWord >> 16) & 0xffff);
+	dib7000p_write_word(state, 1541, syncWord & 0xffff);
+	dib7000p_write_word(state, 1543, syncSize);
+	dib7000p_write_word(state, 1544, dataOutRate);
+	dib7000p_write_word(state, 1554, 0);
+
+	return 0;
+}
+
+static int dib7090_enDivOnHostBus(struct dib7000p_state *state)
+{
+	u16 reg;
+
+	dprintk("Enable Diversity on host bus");
+	reg = (1 << 8) | (1 << 5);
+	dib7000p_write_word(state, 1288, reg);
+
+	return dib7090_cfg_DibTx(state, 5, 5, 0, 0, 0, 0);
+}
+
+static int dib7090_enAdcOnHostBus(struct dib7000p_state *state)
+{
+	u16 reg;
+
+	dprintk("Enable ADC on host bus");
+	reg = (1 << 7) | (1 << 5);
+	dib7000p_write_word(state, 1288, reg);
+
+	return dib7090_cfg_DibTx(state, 20, 5, 10, 0, 0, 0);
+}
+
+static int dib7090_enMpegOnHostBus(struct dib7000p_state *state)
+{
+	u16 reg;
+
+	dprintk("Enable Mpeg on host bus");
+	reg = (1 << 9) | (1 << 5);
+	dib7000p_write_word(state, 1288, reg);
+
+	return dib7090_cfg_DibTx(state, 8, 5, 0, 0, 0, 0);
+}
+
+static int dib7090_enMpegInput(struct dib7000p_state *state)
+{
+	dprintk("Enable Mpeg input");
+	return dib7090_cfg_DibRx(state, 8, 5, 0, 0, 0, 8, 0);	/*outputRate = 8 */
+}
+
+static int dib7090_enMpegMux(struct dib7000p_state *state, u16 pulseWidth, u16 enSerialMode, u16 enSerialClkDiv2)
+{
+	u16 reg = (1 << 7) | ((pulseWidth & 0x1f) << 2) | ((enSerialMode & 0x1) << 1) | (enSerialClkDiv2 & 0x1);
+
+	dprintk("Enable Mpeg mux");
+	dib7000p_write_word(state, 1287, reg);
+
+	reg &= ~(1 << 7);
+	dib7000p_write_word(state, 1287, reg);
+
+	reg = (1 << 4);
+	dib7000p_write_word(state, 1288, reg);
+
+	return 0;
+}
+
+static int dib7090_disableMpegMux(struct dib7000p_state *state)
+{
+	u16 reg;
+
+	dprintk("Disable Mpeg mux");
+	dib7000p_write_word(state, 1288, 0);
+
+	reg = dib7000p_read_word(state, 1287);
+	reg &= ~(1 << 7);
+	dib7000p_write_word(state, 1287, reg);
+
+	return 0;
+}
+
+static int dib7090_set_input_mode(struct dvb_frontend *fe, int mode)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+
+	switch (mode) {
+	case INPUT_MODE_DIVERSITY:
+			dprintk("Enable diversity INPUT");
+			dib7090_cfg_DibRx(state, 5, 5, 0, 0, 0, 0, 0);
+			break;
+	case INPUT_MODE_MPEG:
+			dprintk("Enable Mpeg INPUT");
+			dib7090_cfg_DibRx(state, 8, 5, 0, 0, 0, 8, 0); /*outputRate = 8 */
+			break;
+	case INPUT_MODE_OFF:
+	default:
+			dprintk("Disable INPUT");
+			dib7090_cfg_DibRx(state, 0, 0, 0, 0, 0, 0, 0);
+			break;
+	}
+	return 0;
+}
+
+static int dib7090_set_diversity_in(struct dvb_frontend *fe, int onoff)
+{
+	switch (onoff) {
+	case 0:		/* only use the internal way - not the diversity input */
+		dib7090_set_input_mode(fe, INPUT_MODE_MPEG);
+		break;
+	case 1:		/* both ways */
+	case 2:		/* only the diversity input */
+		dib7090_set_input_mode(fe, INPUT_MODE_DIVERSITY);
+		break;
+	}
+
+	return 0;
+}
+
+static int dib7090_set_output_mode(struct dvb_frontend *fe, int mode)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+
+	u16 outreg, smo_mode, fifo_threshold;
+	u8 prefer_mpeg_mux_use = 1;
+	int ret = 0;
+
+	dib7090_host_bus_drive(state, 1);
+
+	fifo_threshold = 1792;
+	smo_mode = (dib7000p_read_word(state, 235) & 0x0050) | (1 << 1);
+	outreg = dib7000p_read_word(state, 1286) & ~((1 << 10) | (0x7 << 6) | (1 << 1));
+
+	switch (mode) {
+	case OUTMODE_HIGH_Z:
+		outreg = 0;
+		break;
+
+	case OUTMODE_MPEG2_SERIAL:
+		if (prefer_mpeg_mux_use) {
+			dprintk("Sip 7090P setting output mode TS_SERIAL using Mpeg Mux");
+			dib7090_enMpegOnHostBus(state);
+			dib7090_enMpegInput(state);
+			if (state->cfg.enMpegOutput == 1)
+				dib7090_enMpegMux(state, 3, 1, 1);
+
+		} else {	/* Use Smooth block */
+			dprintk("Sip 7090P setting output mode TS_SERIAL using Smooth bloc");
+			dib7090_disableMpegMux(state);
+			dib7000p_write_word(state, 1288, (1 << 6));
+			outreg |= (2 << 6) | (0 << 1);
+		}
+		break;
+
+	case OUTMODE_MPEG2_PAR_GATED_CLK:
+		if (prefer_mpeg_mux_use) {
+			dprintk("Sip 7090P setting output mode TS_PARALLEL_GATED using Mpeg Mux");
+			dib7090_enMpegOnHostBus(state);
+			dib7090_enMpegInput(state);
+			if (state->cfg.enMpegOutput == 1)
+				dib7090_enMpegMux(state, 2, 0, 0);
+		} else {	/* Use Smooth block */
+			dprintk("Sip 7090P setting output mode TS_PARALLEL_GATED using Smooth block");
+			dib7090_disableMpegMux(state);
+			dib7000p_write_word(state, 1288, (1 << 6));
+			outreg |= (0 << 6);
+		}
+		break;
+
+	case OUTMODE_MPEG2_PAR_CONT_CLK:	/* Using Smooth block only */
+		dprintk("Sip 7090P setting output mode TS_PARALLEL_CONT using Smooth block");
+		dib7090_disableMpegMux(state);
+		dib7000p_write_word(state, 1288, (1 << 6));
+		outreg |= (1 << 6);
+		break;
+
+	case OUTMODE_MPEG2_FIFO:	/* Using Smooth block because not supported by new Mpeg Mux bloc */
+		dprintk("Sip 7090P setting output mode TS_FIFO using Smooth block");
+		dib7090_disableMpegMux(state);
+		dib7000p_write_word(state, 1288, (1 << 6));
+		outreg |= (5 << 6);
+		smo_mode |= (3 << 1);
+		fifo_threshold = 512;
+		break;
+
+	case OUTMODE_DIVERSITY:
+		dprintk("Sip 7090P setting output mode MODE_DIVERSITY");
+		dib7090_disableMpegMux(state);
+		dib7090_enDivOnHostBus(state);
+		break;
+
+	case OUTMODE_ANALOG_ADC:
+		dprintk("Sip 7090P setting output mode MODE_ANALOG_ADC");
+		dib7090_enAdcOnHostBus(state);
+		break;
+	}
+
+	if (state->cfg.output_mpeg2_in_188_bytes)
+		smo_mode |= (1 << 5);
+
+	ret |= dib7000p_write_word(state, 235, smo_mode);
+	ret |= dib7000p_write_word(state, 236, fifo_threshold);	/* synchronous fread */
+	ret |= dib7000p_write_word(state, 1286, outreg | (1 << 10));	/* allways set Dout active = 1 !!! */
+
+	return ret;
+}
+
+int dib7090_tuner_sleep(struct dvb_frontend *fe, int onoff)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	u16 en_cur_state;
+
+	dprintk("sleep dib7090: %d", onoff);
+
+	en_cur_state = dib7000p_read_word(state, 1922);
+
+	if (en_cur_state > 0xff)
+		state->tuner_enable = en_cur_state;
+
+	if (onoff)
+		en_cur_state &= 0x00ff;
+	else {
+		if (state->tuner_enable != 0)
+			en_cur_state = state->tuner_enable;
+	}
+
+	dib7000p_write_word(state, 1922, en_cur_state);
+
+	return 0;
+}
+EXPORT_SYMBOL(dib7090_tuner_sleep);
+
+int dib7090_agc_restart(struct dvb_frontend *fe, u8 restart)
+{
+	dprintk("AGC restart callback: %d", restart);
+	return 0;
+}
+EXPORT_SYMBOL(dib7090_agc_restart);
+
+int dib7090_get_adc_power(struct dvb_frontend *fe)
+{
+	return dib7000p_get_adc_power(fe);
+}
+EXPORT_SYMBOL(dib7090_get_adc_power);
+
+int dib7090_slave_reset(struct dvb_frontend *fe)
+{
+	struct dib7000p_state *state = fe->demodulator_priv;
+	u16 reg;
+
+	reg = dib7000p_read_word(state, 1794);
+	dib7000p_write_word(state, 1794, reg | (4 << 12));
+
+	dib7000p_write_word(state, 1032, 0xffff);
+	return 0;
+}
+EXPORT_SYMBOL(dib7090_slave_reset);
+
 static struct dvb_frontend_ops dib7000p_ops;
-struct dvb_frontend * dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg)
+struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg)
 {
 	struct dvb_frontend *demod;
 	struct dib7000p_state *st;
@@ -1400,28 +2286,41 @@ struct dvb_frontend * dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
 	/* Ensure the output mode remains at the previous default if it's
 	 * not specifically set by the caller.
 	 */
-	if ((st->cfg.output_mode != OUTMODE_MPEG2_SERIAL) &&
-	    (st->cfg.output_mode != OUTMODE_MPEG2_PAR_GATED_CLK))
+	if ((st->cfg.output_mode != OUTMODE_MPEG2_SERIAL) && (st->cfg.output_mode != OUTMODE_MPEG2_PAR_GATED_CLK))
 		st->cfg.output_mode = OUTMODE_MPEG2_FIFO;
 
-	demod                   = &st->demod;
+	demod = &st->demod;
 	demod->demodulator_priv = st;
 	memcpy(&st->demod.ops, &dib7000p_ops, sizeof(struct dvb_frontend_ops));
 
-    dib7000p_write_word(st, 1287, 0x0003); /* sram lead in, rdy */
+	dib7000p_write_word(st, 1287, 0x0003);	/* sram lead in, rdy */
 
 	if (dib7000p_identify(st) != 0)
 		goto error;
 
+	st->version = dib7000p_read_word(st, 897);
+
 	/* FIXME: make sure the dev.parent field is initialized, or else
-	request_firmware() will hit an OOPS (this should be moved somewhere
-	more common) */
-	st->i2c_master.gated_tuner_i2c_adap.dev.parent = i2c_adap->dev.parent;
+		request_firmware() will hit an OOPS (this should be moved somewhere
+		more common) */
 
 	dibx000_init_i2c_master(&st->i2c_master, DIB7000P, st->i2c_adap, st->i2c_addr);
 
+	/* init 7090 tuner adapter */
+	strncpy(st->dib7090_tuner_adap.name, "DiB7090 tuner interface", sizeof(st->dib7090_tuner_adap.name));
+	st->dib7090_tuner_adap.algo = &dib7090_tuner_xfer_algo;
+	st->dib7090_tuner_adap.algo_data = NULL;
+	st->dib7090_tuner_adap.dev.parent = st->i2c_adap->dev.parent;
+	i2c_set_adapdata(&st->dib7090_tuner_adap, st);
+	i2c_add_adapter(&st->dib7090_tuner_adap);
+
 	dib7000p_demod_reset(st);
 
+	if (st->version == SOC7090) {
+		dib7090_set_output_mode(demod, st->cfg.output_mode);
+		dib7090_set_diversity_in(demod, 0);
+	}
+
 	return demod;
 
 error:
@@ -1432,37 +2331,35 @@ EXPORT_SYMBOL(dib7000p_attach);
 
 static struct dvb_frontend_ops dib7000p_ops = {
 	.info = {
-		.name = "DiBcom 7000PC",
-		.type = FE_OFDM,
-		.frequency_min      = 44250000,
-		.frequency_max      = 867250000,
-		.frequency_stepsize = 62500,
-		.caps = FE_CAN_INVERSION_AUTO |
-			FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
-			FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
-			FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 | FE_CAN_QAM_AUTO |
-			FE_CAN_TRANSMISSION_MODE_AUTO |
-			FE_CAN_GUARD_INTERVAL_AUTO |
-			FE_CAN_RECOVER |
-			FE_CAN_HIERARCHY_AUTO,
-	},
-
-	.release              = dib7000p_release,
-
-	.init                 = dib7000p_wakeup,
-	.sleep                = dib7000p_sleep,
-
-	.set_frontend         = dib7000p_set_frontend,
-	.get_tune_settings    = dib7000p_fe_get_tune_settings,
-	.get_frontend         = dib7000p_get_frontend,
-
-	.read_status          = dib7000p_read_status,
-	.read_ber             = dib7000p_read_ber,
+		 .name = "DiBcom 7000PC",
+		 .type = FE_OFDM,
+		 .frequency_min = 44250000,
+		 .frequency_max = 867250000,
+		 .frequency_stepsize = 62500,
+		 .caps = FE_CAN_INVERSION_AUTO |
+		 FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
+		 FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+		 FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 | FE_CAN_QAM_AUTO |
+		 FE_CAN_TRANSMISSION_MODE_AUTO | FE_CAN_GUARD_INTERVAL_AUTO | FE_CAN_RECOVER | FE_CAN_HIERARCHY_AUTO,
+		 },
+
+	.release = dib7000p_release,
+
+	.init = dib7000p_wakeup,
+	.sleep = dib7000p_sleep,
+
+	.set_frontend = dib7000p_set_frontend,
+	.get_tune_settings = dib7000p_fe_get_tune_settings,
+	.get_frontend = dib7000p_get_frontend,
+
+	.read_status = dib7000p_read_status,
+	.read_ber = dib7000p_read_ber,
 	.read_signal_strength = dib7000p_read_signal_strength,
-	.read_snr             = dib7000p_read_snr,
-	.read_ucblocks        = dib7000p_read_unc_blocks,
+	.read_snr = dib7000p_read_snr,
+	.read_ucblocks = dib7000p_read_unc_blocks,
 };
 
+MODULE_AUTHOR("Olivier Grenie <ogrenie@dibcom.fr>");
 MODULE_AUTHOR("Patrick Boettcher <pboettcher@dibcom.fr>");
 MODULE_DESCRIPTION("Driver for the DiBcom 7000PC COFDM demodulator");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/frontends/dib7000p.h b/drivers/media/dvb/frontends/dib7000p.h
index da17345bf5bd..0179f9474bac 100644
--- a/drivers/media/dvb/frontends/dib7000p.h
+++ b/drivers/media/dvb/frontends/dib7000p.h
@@ -33,59 +33,54 @@ struct dib7000p_config {
 	int (*agc_control) (struct dvb_frontend *, u8 before);
 
 	u8 output_mode;
-	u8 disable_sample_and_hold : 1;
+	u8 disable_sample_and_hold:1;
 
-	u8 enable_current_mirror : 1;
-	u8 diversity_delay;
+	u8 enable_current_mirror:1;
+	u16 diversity_delay;
 
+	u8 default_i2c_addr;
+	u8 enMpegOutput:1;
 };
 
 #define DEFAULT_DIB7000P_I2C_ADDRESS 18
 
 #if defined(CONFIG_DVB_DIB7000P) || (defined(CONFIG_DVB_DIB7000P_MODULE) && \
-				     defined(MODULE))
-extern struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap,
-					    u8 i2c_addr,
-					    struct dib7000p_config *cfg);
-extern struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *,
-						   enum dibx000_i2c_interface,
-						   int);
-extern int dib7000p_i2c_enumeration(struct i2c_adapter *i2c,
-				    int no_of_demods, u8 default_addr,
-				    struct dib7000p_config cfg[]);
+					defined(MODULE))
+extern struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg);
+extern struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *, enum dibx000_i2c_interface, int);
+extern int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 default_addr, struct dib7000p_config cfg[]);
 extern int dib7000p_set_gpio(struct dvb_frontend *, u8 num, u8 dir, u8 val);
 extern int dib7000p_set_wbd_ref(struct dvb_frontend *, u16 value);
 extern int dib7000pc_detection(struct i2c_adapter *i2c_adap);
 extern int dib7000p_pid_filter(struct dvb_frontend *, u8 id, u16 pid, u8 onoff);
 extern int dib7000p_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff);
+extern int dib7000p_update_pll(struct dvb_frontend *fe, struct dibx000_bandwidth_config *bw);
+extern u32 dib7000p_ctrl_timf(struct dvb_frontend *fe, u8 op, u32 timf);
+extern int dib7090_agc_restart(struct dvb_frontend *fe, u8 restart);
+extern int dib7090_tuner_sleep(struct dvb_frontend *fe, int onoff);
+extern int dib7090_get_adc_power(struct dvb_frontend *fe);
+extern struct i2c_adapter *dib7090_get_i2c_tuner(struct dvb_frontend *fe);
+extern int dib7090_slave_reset(struct dvb_frontend *fe);
 #else
-static inline
-struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
-				     struct dib7000p_config *cfg)
+static inline struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib7000p_config *cfg)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
 
-static inline
-struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *fe,
-					    enum dibx000_i2c_interface i,
-					    int x)
+static inline struct i2c_adapter *dib7000p_get_i2c_master(struct dvb_frontend *fe, enum dibx000_i2c_interface i, int x)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
 
-static inline int dib7000p_i2c_enumeration(struct i2c_adapter *i2c,
-					   int no_of_demods, u8 default_addr,
-					   struct dib7000p_config cfg[])
+static inline int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 default_addr, struct dib7000p_config cfg[])
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
 }
 
-static inline int dib7000p_set_gpio(struct dvb_frontend *fe,
-				    u8 num, u8 dir, u8 val)
+static inline int dib7000p_set_gpio(struct dvb_frontend *fe, u8 num, u8 dir, u8 val)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
@@ -102,16 +97,59 @@ static inline int dib7000pc_detection(struct i2c_adapter *i2c_adap)
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return -ENODEV;
 }
+
 static inline int dib7000p_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
 {
-    printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
-    return -ENODEV;
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
 }
 
 static inline int dib7000p_pid_filter_ctrl(struct dvb_frontend *fe, uint8_t onoff)
 {
-    printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
-    return -ENODEV;
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib7000p_update_pll(struct dvb_frontend *fe, struct dibx000_bandwidth_config *bw)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline u32 dib7000p_ctrl_timf(struct dvb_frontend *fe, u8 op, u32 timf)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return 0;
+}
+
+static inline int dib7090_agc_restart(struct dvb_frontend *fe, u8 restart)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib7090_tuner_sleep(struct dvb_frontend *fe, int onoff)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib7090_get_adc_power(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline struct i2c_adapter *dib7090_get_i2c_tuner(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline int dib7090_slave_reset(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
 }
 #endif
 
diff --git a/drivers/media/dvb/frontends/dib8000.c b/drivers/media/dvb/frontends/dib8000.c
index df17b91b3250..c1c3e26906e2 100644
--- a/drivers/media/dvb/frontends/dib8000.c
+++ b/drivers/media/dvb/frontends/dib8000.c
@@ -22,6 +22,7 @@
 #define LAYER_C   3
 
 #define FE_CALLBACK_TIME_NEVER 0xffffffff
+#define MAX_NUMBER_OF_FRONTENDS 6
 
 static int debug;
 module_param(debug, int, 0644);
@@ -37,7 +38,6 @@ struct i2c_device {
 };
 
 struct dib8000_state {
-	struct dvb_frontend fe;
 	struct dib8000_config cfg;
 
 	struct i2c_device i2c;
@@ -68,6 +68,8 @@ struct dib8000_state {
 	u8 isdbt_cfg_loaded;
 	enum frontend_tune_state tune_state;
 	u32 status;
+
+	struct dvb_frontend *fe[MAX_NUMBER_OF_FRONTENDS];
 };
 
 enum dib8000_power_mode {
@@ -122,111 +124,111 @@ static int dib8000_write_word(struct dib8000_state *state, u16 reg, u16 val)
 	return dib8000_i2c_write16(&state->i2c, reg, val);
 }
 
-static const int16_t coeff_2k_sb_1seg_dqpsk[8] = {
+static const s16 coeff_2k_sb_1seg_dqpsk[8] = {
 	(769 << 5) | 0x0a, (745 << 5) | 0x03, (595 << 5) | 0x0d, (769 << 5) | 0x0a, (920 << 5) | 0x09, (784 << 5) | 0x02, (519 << 5) | 0x0c,
-	    (920 << 5) | 0x09
+		(920 << 5) | 0x09
 };
 
-static const int16_t coeff_2k_sb_1seg[8] = {
+static const s16 coeff_2k_sb_1seg[8] = {
 	(692 << 5) | 0x0b, (683 << 5) | 0x01, (519 << 5) | 0x09, (692 << 5) | 0x0b, 0 | 0x1f, 0 | 0x1f, 0 | 0x1f, 0 | 0x1f
 };
 
-static const int16_t coeff_2k_sb_3seg_0dqpsk_1dqpsk[8] = {
+static const s16 coeff_2k_sb_3seg_0dqpsk_1dqpsk[8] = {
 	(832 << 5) | 0x10, (912 << 5) | 0x05, (900 << 5) | 0x12, (832 << 5) | 0x10, (-931 << 5) | 0x0f, (912 << 5) | 0x04, (807 << 5) | 0x11,
-	    (-931 << 5) | 0x0f
+		(-931 << 5) | 0x0f
 };
 
-static const int16_t coeff_2k_sb_3seg_0dqpsk[8] = {
+static const s16 coeff_2k_sb_3seg_0dqpsk[8] = {
 	(622 << 5) | 0x0c, (941 << 5) | 0x04, (796 << 5) | 0x10, (622 << 5) | 0x0c, (982 << 5) | 0x0c, (519 << 5) | 0x02, (572 << 5) | 0x0e,
-	    (982 << 5) | 0x0c
+		(982 << 5) | 0x0c
 };
 
-static const int16_t coeff_2k_sb_3seg_1dqpsk[8] = {
+static const s16 coeff_2k_sb_3seg_1dqpsk[8] = {
 	(699 << 5) | 0x14, (607 << 5) | 0x04, (944 << 5) | 0x13, (699 << 5) | 0x14, (-720 << 5) | 0x0d, (640 << 5) | 0x03, (866 << 5) | 0x12,
-	    (-720 << 5) | 0x0d
+		(-720 << 5) | 0x0d
 };
 
-static const int16_t coeff_2k_sb_3seg[8] = {
+static const s16 coeff_2k_sb_3seg[8] = {
 	(664 << 5) | 0x0c, (925 << 5) | 0x03, (937 << 5) | 0x10, (664 << 5) | 0x0c, (-610 << 5) | 0x0a, (697 << 5) | 0x01, (836 << 5) | 0x0e,
-	    (-610 << 5) | 0x0a
+		(-610 << 5) | 0x0a
 };
 
-static const int16_t coeff_4k_sb_1seg_dqpsk[8] = {
+static const s16 coeff_4k_sb_1seg_dqpsk[8] = {
 	(-955 << 5) | 0x0e, (687 << 5) | 0x04, (818 << 5) | 0x10, (-955 << 5) | 0x0e, (-922 << 5) | 0x0d, (750 << 5) | 0x03, (665 << 5) | 0x0f,
-	    (-922 << 5) | 0x0d
+		(-922 << 5) | 0x0d
 };
 
-static const int16_t coeff_4k_sb_1seg[8] = {
+static const s16 coeff_4k_sb_1seg[8] = {
 	(638 << 5) | 0x0d, (683 << 5) | 0x02, (638 << 5) | 0x0d, (638 << 5) | 0x0d, (-655 << 5) | 0x0a, (517 << 5) | 0x00, (698 << 5) | 0x0d,
-	    (-655 << 5) | 0x0a
+		(-655 << 5) | 0x0a
 };
 
-static const int16_t coeff_4k_sb_3seg_0dqpsk_1dqpsk[8] = {
+static const s16 coeff_4k_sb_3seg_0dqpsk_1dqpsk[8] = {
 	(-707 << 5) | 0x14, (910 << 5) | 0x06, (889 << 5) | 0x16, (-707 << 5) | 0x14, (-958 << 5) | 0x13, (993 << 5) | 0x05, (523 << 5) | 0x14,
-	    (-958 << 5) | 0x13
+		(-958 << 5) | 0x13
 };
 
-static const int16_t coeff_4k_sb_3seg_0dqpsk[8] = {
+static const s16 coeff_4k_sb_3seg_0dqpsk[8] = {
 	(-723 << 5) | 0x13, (910 << 5) | 0x05, (777 << 5) | 0x14, (-723 << 5) | 0x13, (-568 << 5) | 0x0f, (547 << 5) | 0x03, (696 << 5) | 0x12,
-	    (-568 << 5) | 0x0f
+		(-568 << 5) | 0x0f
 };
 
-static const int16_t coeff_4k_sb_3seg_1dqpsk[8] = {
+static const s16 coeff_4k_sb_3seg_1dqpsk[8] = {
 	(-940 << 5) | 0x15, (607 << 5) | 0x05, (915 << 5) | 0x16, (-940 << 5) | 0x15, (-848 << 5) | 0x13, (683 << 5) | 0x04, (543 << 5) | 0x14,
-	    (-848 << 5) | 0x13
+		(-848 << 5) | 0x13
 };
 
-static const int16_t coeff_4k_sb_3seg[8] = {
+static const s16 coeff_4k_sb_3seg[8] = {
 	(612 << 5) | 0x12, (910 << 5) | 0x04, (864 << 5) | 0x14, (612 << 5) | 0x12, (-869 << 5) | 0x13, (683 << 5) | 0x02, (869 << 5) | 0x12,
-	    (-869 << 5) | 0x13
+		(-869 << 5) | 0x13
 };
 
-static const int16_t coeff_8k_sb_1seg_dqpsk[8] = {
+static const s16 coeff_8k_sb_1seg_dqpsk[8] = {
 	(-835 << 5) | 0x12, (684 << 5) | 0x05, (735 << 5) | 0x14, (-835 << 5) | 0x12, (-598 << 5) | 0x10, (781 << 5) | 0x04, (739 << 5) | 0x13,
-	    (-598 << 5) | 0x10
+		(-598 << 5) | 0x10
 };
 
-static const int16_t coeff_8k_sb_1seg[8] = {
+static const s16 coeff_8k_sb_1seg[8] = {
 	(673 << 5) | 0x0f, (683 << 5) | 0x03, (808 << 5) | 0x12, (673 << 5) | 0x0f, (585 << 5) | 0x0f, (512 << 5) | 0x01, (780 << 5) | 0x0f,
-	    (585 << 5) | 0x0f
+		(585 << 5) | 0x0f
 };
 
-static const int16_t coeff_8k_sb_3seg_0dqpsk_1dqpsk[8] = {
+static const s16 coeff_8k_sb_3seg_0dqpsk_1dqpsk[8] = {
 	(863 << 5) | 0x17, (930 << 5) | 0x07, (878 << 5) | 0x19, (863 << 5) | 0x17, (0 << 5) | 0x14, (521 << 5) | 0x05, (980 << 5) | 0x18,
-	    (0 << 5) | 0x14
+		(0 << 5) | 0x14
 };
 
-static const int16_t coeff_8k_sb_3seg_0dqpsk[8] = {
+static const s16 coeff_8k_sb_3seg_0dqpsk[8] = {
 	(-924 << 5) | 0x17, (910 << 5) | 0x06, (774 << 5) | 0x17, (-924 << 5) | 0x17, (-877 << 5) | 0x15, (565 << 5) | 0x04, (553 << 5) | 0x15,
-	    (-877 << 5) | 0x15
+		(-877 << 5) | 0x15
 };
 
-static const int16_t coeff_8k_sb_3seg_1dqpsk[8] = {
+static const s16 coeff_8k_sb_3seg_1dqpsk[8] = {
 	(-921 << 5) | 0x19, (607 << 5) | 0x06, (881 << 5) | 0x19, (-921 << 5) | 0x19, (-921 << 5) | 0x14, (713 << 5) | 0x05, (1018 << 5) | 0x18,
-	    (-921 << 5) | 0x14
+		(-921 << 5) | 0x14
 };
 
-static const int16_t coeff_8k_sb_3seg[8] = {
+static const s16 coeff_8k_sb_3seg[8] = {
 	(514 << 5) | 0x14, (910 << 5) | 0x05, (861 << 5) | 0x17, (514 << 5) | 0x14, (690 << 5) | 0x14, (683 << 5) | 0x03, (662 << 5) | 0x15,
-	    (690 << 5) | 0x14
+		(690 << 5) | 0x14
 };
 
-static const int16_t ana_fe_coeff_3seg[24] = {
+static const s16 ana_fe_coeff_3seg[24] = {
 	81, 80, 78, 74, 68, 61, 54, 45, 37, 28, 19, 11, 4, 1022, 1017, 1013, 1010, 1008, 1008, 1008, 1008, 1010, 1014, 1017
 };
 
-static const int16_t ana_fe_coeff_1seg[24] = {
+static const s16 ana_fe_coeff_1seg[24] = {
 	249, 226, 164, 82, 5, 981, 970, 988, 1018, 20, 31, 26, 8, 1012, 1000, 1018, 1012, 8, 15, 14, 9, 3, 1017, 1003
 };
 
-static const int16_t ana_fe_coeff_13seg[24] = {
+static const s16 ana_fe_coeff_13seg[24] = {
 	396, 305, 105, -51, -77, -12, 41, 31, -11, -30, -11, 14, 15, -2, -13, -7, 5, 8, 1, -6, -7, -3, 0, 1
 };
 
 static u16 fft_to_mode(struct dib8000_state *state)
 {
 	u16 mode;
-	switch (state->fe.dtv_property_cache.transmission_mode) {
+	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
 	case TRANSMISSION_MODE_2K:
 		mode = 1;
 		break;
@@ -249,16 +251,18 @@ static void dib8000_set_acquisition_mode(struct dib8000_state *state)
 	dprintk("acquisition mode activated");
 	dib8000_write_word(state, 298, nud);
 }
-
-static int dib8000_set_output_mode(struct dib8000_state *state, int mode)
+static int dib8000_set_output_mode(struct dvb_frontend *fe, int mode)
 {
+	struct dib8000_state *state = fe->demodulator_priv;
+
 	u16 outreg, fifo_threshold, smo_mode, sram = 0x0205;	/* by default SDRAM deintlv is enabled */
 
 	outreg = 0;
 	fifo_threshold = 1792;
 	smo_mode = (dib8000_read_word(state, 299) & 0x0050) | (1 << 1);
 
-	dprintk("-I-  Setting output mode for demod %p to %d", &state->fe, mode);
+	dprintk("-I-	Setting output mode for demod %p to %d",
+			&state->fe[0], mode);
 
 	switch (mode) {
 	case OUTMODE_MPEG2_PAR_GATED_CLK:	// STBs with parallel gated clock
@@ -292,7 +296,8 @@ static int dib8000_set_output_mode(struct dib8000_state *state, int mode)
 		break;
 
 	default:
-		dprintk("Unhandled output_mode passed to be set for demod %p", &state->fe);
+		dprintk("Unhandled output_mode passed to be set for demod %p",
+				&state->fe[0]);
 		return -EINVAL;
 	}
 
@@ -342,7 +347,8 @@ static void dib8000_set_power_mode(struct dib8000_state *state, enum dib8000_pow
 {
 	/* by default everything is going to be powered off */
 	u16 reg_774 = 0x3fff, reg_775 = 0xffff, reg_776 = 0xffff,
-	    reg_900 = (dib8000_read_word(state, 900) & 0xfffc) | 0x3, reg_1280 = (dib8000_read_word(state, 1280) & 0x00ff) | 0xff00;
+		reg_900 = (dib8000_read_word(state, 900) & 0xfffc) | 0x3,
+		reg_1280 = (dib8000_read_word(state, 1280) & 0x00ff) | 0xff00;
 
 	/* now, depending on the requested mode, we power on */
 	switch (mode) {
@@ -411,8 +417,9 @@ static int dib8000_set_adc_state(struct dib8000_state *state, enum dibx000_adc_s
 	return ret;
 }
 
-static int dib8000_set_bandwidth(struct dib8000_state *state, u32 bw)
+static int dib8000_set_bandwidth(struct dvb_frontend *fe, u32 bw)
 {
+	struct dib8000_state *state = fe->demodulator_priv;
 	u32 timf;
 
 	if (bw == 0)
@@ -478,7 +485,8 @@ static void dib8000_reset_pll(struct dib8000_state *state)
 
 	// clk_cfg1
 	clk_cfg1 = (1 << 10) | (0 << 9) | (pll->IO_CLK_en_core << 8) |
-	    (pll->bypclk_div << 5) | (pll->enable_refdiv << 4) | (1 << 3) | (pll->pll_range << 1) | (pll->pll_reset << 0);
+		(pll->bypclk_div << 5) | (pll->enable_refdiv << 4) | (1 << 3) |
+		(pll->pll_range << 1) | (pll->pll_reset << 0);
 
 	dib8000_write_word(state, 902, clk_cfg1);
 	clk_cfg1 = (clk_cfg1 & 0xfff7) | (pll->pll_bypass << 3);
@@ -488,11 +496,12 @@ static void dib8000_reset_pll(struct dib8000_state *state)
 
 	/* smpl_cfg: P_refclksel=2, P_ensmplsel=1 nodivsmpl=1 */
 	if (state->cfg.pll->ADClkSrc == 0)
-		dib8000_write_word(state, 904, (0 << 15) | (0 << 12) | (0 << 10) | (pll->modulo << 8) | (pll->ADClkSrc << 7) | (0 << 1));
+		dib8000_write_word(state, 904, (0 << 15) | (0 << 12) | (0 << 10) |
+				(pll->modulo << 8) | (pll->ADClkSrc << 7) | (0 << 1));
 	else if (state->cfg.refclksel != 0)
-		dib8000_write_word(state, 904,
-				   (0 << 15) | (1 << 12) | ((state->cfg.refclksel & 0x3) << 10) | (pll->modulo << 8) | (pll->
-															ADClkSrc << 7) | (0 << 1));
+		dib8000_write_word(state, 904, (0 << 15) | (1 << 12) |
+				((state->cfg.refclksel & 0x3) << 10) | (pll->modulo << 8) |
+				(pll->ADClkSrc << 7) | (0 << 1));
 	else
 		dib8000_write_word(state, 904, (0 << 15) | (1 << 12) | (3 << 10) | (pll->modulo << 8) | (pll->ADClkSrc << 7) | (0 << 1));
 
@@ -560,7 +569,7 @@ static const u16 dib8000_defaults[] = {
 	0xd4c0,
 
 	/*1, 32,
-	   0x6680 // P_corm_thres Lock algorithms configuration */
+		0x6680 // P_corm_thres Lock algorithms configuration */
 
 	11, 80,			/* set ADC level to -16 */
 	(1 << 13) - 825 - 117,
@@ -623,14 +632,14 @@ static const u16 dib8000_defaults[] = {
 	1, 285,
 	0x0020,			//p_fec_
 	1, 299,
-	0x0062,			// P_smo_mode, P_smo_rs_discard, P_smo_fifo_flush, P_smo_pid_parse, P_smo_error_discard
+	0x0062,			/* P_smo_mode, P_smo_rs_discard, P_smo_fifo_flush, P_smo_pid_parse, P_smo_error_discard */
 
 	1, 338,
 	(1 << 12) |		// P_ctrl_corm_thres4pre_freq_inh=1
-	    (1 << 10) |		// P_ctrl_pre_freq_mode_sat=1
-	    (0 << 9) |		// P_ctrl_pre_freq_inh=0
-	    (3 << 5) |		// P_ctrl_pre_freq_step=3
-	    (1 << 0),		// P_pre_freq_win_len=1
+		(1 << 10) |
+		(0 << 9) |		/* P_ctrl_pre_freq_inh=0 */
+		(3 << 5) |		/* P_ctrl_pre_freq_step=3 */
+		(1 << 0),		/* P_pre_freq_win_len=1 */
 
 	1, 903,
 	(0 << 4) | 2,		// P_divclksel=0 P_divbitsel=2 (was clk=3,bit=1 for MPW)
@@ -717,7 +726,7 @@ static int dib8000_reset(struct dvb_frontend *fe)
 	if (dib8000_reset_gpio(state) != 0)
 		dprintk("GPIO reset was not successful.");
 
-	if (dib8000_set_output_mode(state, OUTMODE_HIGH_Z) != 0)
+	if (dib8000_set_output_mode(fe, OUTMODE_HIGH_Z) != 0)
 		dprintk("OUTPUT_MODE could not be resetted.");
 
 	state->current_agc = NULL;
@@ -752,7 +761,7 @@ static int dib8000_reset(struct dvb_frontend *fe)
 	/* unforce divstr regardless whether i2c enumeration was done or not */
 	dib8000_write_word(state, 1285, dib8000_read_word(state, 1285) & ~(1 << 1));
 
-	dib8000_set_bandwidth(state, 6000);
+	dib8000_set_bandwidth(fe, 6000);
 
 	dib8000_set_adc_state(state, DIBX000_SLOW_ADC_ON);
 	dib8000_sad_calib(state);
@@ -778,7 +787,7 @@ static int dib8000_update_lna(struct dib8000_state *state)
 		// read dyn_gain here (because it is demod-dependent and not tuner)
 		dyn_gain = dib8000_read_word(state, 390);
 
-		if (state->cfg.update_lna(&state->fe, dyn_gain)) {	// LNA has changed
+		if (state->cfg.update_lna(state->fe[0], dyn_gain)) {
 			dib8000_restart_agc(state);
 			return 1;
 		}
@@ -865,7 +874,8 @@ static int dib8000_agc_soft_split(struct dib8000_state *state)
 		split_offset = state->current_agc->split.max;
 	else
 		split_offset = state->current_agc->split.max *
-		    (agc - state->current_agc->split.min_thres) / (state->current_agc->split.max_thres - state->current_agc->split.min_thres);
+			(agc - state->current_agc->split.min_thres) /
+			(state->current_agc->split.max_thres - state->current_agc->split.min_thres);
 
 	dprintk("AGC split_offset: %d", split_offset);
 
@@ -900,7 +910,7 @@ static int dib8000_agc_startup(struct dvb_frontend *fe)
 	case CT_AGC_STEP_0:
 		//AGC initialization
 		if (state->cfg.agc_control)
-			state->cfg.agc_control(&state->fe, 1);
+			state->cfg.agc_control(fe, 1);
 
 		dib8000_restart_agc(state);
 
@@ -924,7 +934,7 @@ static int dib8000_agc_startup(struct dvb_frontend *fe)
 		dib8000_agc_soft_split(state);
 
 		if (state->cfg.agc_control)
-			state->cfg.agc_control(&state->fe, 0);
+			state->cfg.agc_control(fe, 0);
 
 		*tune_state = CT_AGC_STOP;
 		break;
@@ -936,29 +946,28 @@ static int dib8000_agc_startup(struct dvb_frontend *fe)
 
 }
 
-static const int32_t lut_1000ln_mant[] =
+static const s32 lut_1000ln_mant[] =
 {
 	908, 7003, 7090, 7170, 7244, 7313, 7377, 7438, 7495, 7549, 7600
 };
 
-int32_t dib8000_get_adc_power(struct dvb_frontend *fe, uint8_t mode)
+s32 dib8000_get_adc_power(struct dvb_frontend *fe, u8 mode)
 {
-    struct dib8000_state *state = fe->demodulator_priv;
-    uint32_t ix = 0, tmp_val = 0, exp = 0, mant = 0;
-    int32_t val;
-
-    val = dib8000_read32(state, 384);
-    /* mode = 1 : ln_agcpower calc using mant-exp conversion and mantis look up table */
-    if (mode) {
-	tmp_val = val;
-	while (tmp_val >>= 1)
-		exp++;
-	mant = (val * 1000 / (1<<exp));
-	ix = (uint8_t)((mant-1000)/100); /* index of the LUT */
-	val = (lut_1000ln_mant[ix] + 693*(exp-20) - 6908); /* 1000 * ln(adcpower_real) ; 693 = 1000ln(2) ; 6908 = 1000*ln(1000) ; 20 comes from adc_real = adc_pow_int / 2**20 */
-	val = (val*256)/1000;
-    }
-    return val;
+	struct dib8000_state *state = fe->demodulator_priv;
+	u32 ix = 0, tmp_val = 0, exp = 0, mant = 0;
+	s32 val;
+
+	val = dib8000_read32(state, 384);
+	if (mode) {
+		tmp_val = val;
+		while (tmp_val >>= 1)
+			exp++;
+		mant = (val * 1000 / (1<<exp));
+		ix = (u8)((mant-1000)/100); /* index of the LUT */
+		val = (lut_1000ln_mant[ix] + 693*(exp-20) - 6908);
+		val = (val*256)/1000;
+	}
+	return val;
 }
 EXPORT_SYMBOL(dib8000_get_adc_power);
 
@@ -1002,22 +1011,23 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 		dib8000_write_word(state, 285, dib8000_read_word(state, 285) & 0x60);
 
 	i = dib8000_read_word(state, 26) & 1;	// P_dds_invspec
-	dib8000_write_word(state, 26, state->fe.dtv_property_cache.inversion ^ i);
+	dib8000_write_word(state, 26, state->fe[0]->dtv_property_cache.inversion^i);
 
-	if (state->fe.dtv_property_cache.isdbt_sb_mode) {
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
 		//compute new dds_freq for the seg and adjust prbs
 		int seg_offset =
-		    state->fe.dtv_property_cache.isdbt_sb_segment_idx - (state->fe.dtv_property_cache.isdbt_sb_segment_count / 2) -
-		    (state->fe.dtv_property_cache.isdbt_sb_segment_count % 2);
+			state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx -
+			(state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) -
+			(state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2);
 		int clk = state->cfg.pll->internal;
 		u32 segtodds = ((u32) (430 << 23) / clk) << 3;	// segtodds = SegBW / Fclk * pow(2,26)
 		int dds_offset = seg_offset * segtodds;
 		int new_dds, sub_channel;
-		if ((state->fe.dtv_property_cache.isdbt_sb_segment_count % 2) == 0)	// if even
+		if ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
 			dds_offset -= (int)(segtodds / 2);
 
 		if (state->cfg.pll->ifreq == 0) {
-			if ((state->fe.dtv_property_cache.inversion ^ i) == 0) {
+			if ((state->fe[0]->dtv_property_cache.inversion ^ i) == 0) {
 				dib8000_write_word(state, 26, dib8000_read_word(state, 26) | 1);
 				new_dds = dds_offset;
 			} else
@@ -1027,35 +1037,35 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			//  - the segment of center frequency with an odd total number of segments
 			//  - the segment to the left of center frequency with an even total number of segments
 			//  - the segment to the right of center frequency with an even total number of segments
-			if ((state->fe.dtv_property_cache.delivery_system == SYS_ISDBT) && (state->fe.dtv_property_cache.isdbt_sb_mode == 1)
-			    &&
-			    (((state->fe.dtv_property_cache.isdbt_sb_segment_count % 2)
-			      && (state->fe.dtv_property_cache.isdbt_sb_segment_idx ==
-				  ((state->fe.dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-			     || (((state->fe.dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-				 && (state->fe.dtv_property_cache.isdbt_sb_segment_idx == (state->fe.dtv_property_cache.isdbt_sb_segment_count / 2)))
-			     || (((state->fe.dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-				 && (state->fe.dtv_property_cache.isdbt_sb_segment_idx ==
-				     ((state->fe.dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-			    )) {
+			if ((state->fe[0]->dtv_property_cache.delivery_system == SYS_ISDBT)
+				&& (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1)
+					&& (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2)
+					  && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx ==
+				  ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
+					 || (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
+						 && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx == (state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2)))
+					 || (((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
+						 && (state->fe[0]->dtv_property_cache.isdbt_sb_segment_idx ==
+							 ((state->fe[0]->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
+					)) {
 				new_dds -= ((u32) (850 << 22) / clk) << 4;	// new_dds = 850 (freq shift in KHz) / Fclk * pow(2,26)
 			}
 		} else {
-			if ((state->fe.dtv_property_cache.inversion ^ i) == 0)
+			if ((state->fe[0]->dtv_property_cache.inversion ^ i) == 0)
 				new_dds = state->cfg.pll->ifreq - dds_offset;
 			else
 				new_dds = state->cfg.pll->ifreq + dds_offset;
 		}
 		dib8000_write_word(state, 27, (u16) ((new_dds >> 16) & 0x01ff));
 		dib8000_write_word(state, 28, (u16) (new_dds & 0xffff));
-		if (state->fe.dtv_property_cache.isdbt_sb_segment_count % 2)	// if odd
-			sub_channel = ((state->fe.dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset) + 1) % 41) / 3;
-		else		// if even
-			sub_channel = ((state->fe.dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset)) % 41) / 3;
+		if (state->fe[0]->dtv_property_cache.isdbt_sb_segment_count % 2)
+			sub_channel = ((state->fe[0]->dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset) + 1) % 41) / 3;
+		else
+			sub_channel = ((state->fe[0]->dtv_property_cache.isdbt_sb_subchannel + (3 * seg_offset)) % 41) / 3;
 		sub_channel -= 6;
 
-		if (state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K
-		    || state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_4K) {
+		if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K
+				|| state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_4K) {
 			dib8000_write_word(state, 219, dib8000_read_word(state, 219) | 0x1);	//adp_pass =1
 			dib8000_write_word(state, 190, dib8000_read_word(state, 190) | (0x1 << 14));	//pha3_force_pha_shift = 1
 		} else {
@@ -1063,7 +1073,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			dib8000_write_word(state, 190, dib8000_read_word(state, 190) & 0xbfff);	//pha3_force_pha_shift = 0
 		}
 
-		switch (state->fe.dtv_property_cache.transmission_mode) {
+		switch (state->fe[0]->dtv_property_cache.transmission_mode) {
 		case TRANSMISSION_MODE_2K:
 			switch (sub_channel) {
 			case -6:
@@ -1209,7 +1219,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			}
 			break;
 		}
-	} else {		// if not state->fe.dtv_property_cache.isdbt_sb_mode
+	} else {
 		dib8000_write_word(state, 27, (u16) ((state->cfg.pll->ifreq >> 16) & 0x01ff));
 		dib8000_write_word(state, 28, (u16) (state->cfg.pll->ifreq & 0xffff));
 		dib8000_write_word(state, 26, (u16) ((state->cfg.pll->ifreq >> 25) & 0x0003));
@@ -1218,7 +1228,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	dib8000_write_word(state, 10, (seq << 4));
 	//  dib8000_write_word(state, 287, (dib8000_read_word(state, 287) & 0xe000) | 0x1000);
 
-	switch (state->fe.dtv_property_cache.guard_interval) {
+	switch (state->fe[0]->dtv_property_cache.guard_interval) {
 	case GUARD_INTERVAL_1_32:
 		guard = 0;
 		break;
@@ -1238,7 +1248,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 
 	max_constellation = DQPSK;
 	for (i = 0; i < 3; i++) {
-		switch (state->fe.dtv_property_cache.layer[i].modulation) {
+		switch (state->fe[0]->dtv_property_cache.layer[i].modulation) {
 		case DQPSK:
 			constellation = 0;
 			break;
@@ -1254,7 +1264,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			break;
 		}
 
-		switch (state->fe.dtv_property_cache.layer[i].fec) {
+		switch (state->fe[0]->dtv_property_cache.layer[i].fec) {
 		case FEC_1_2:
 			crate = 1;
 			break;
@@ -1273,26 +1283,26 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			break;
 		}
 
-		if ((state->fe.dtv_property_cache.layer[i].interleaving > 0) &&
-		    ((state->fe.dtv_property_cache.layer[i].interleaving <= 3) ||
-		     (state->fe.dtv_property_cache.layer[i].interleaving == 4 && state->fe.dtv_property_cache.isdbt_sb_mode == 1))
-		    )
-			timeI = state->fe.dtv_property_cache.layer[i].interleaving;
+		if ((state->fe[0]->dtv_property_cache.layer[i].interleaving > 0) &&
+				((state->fe[0]->dtv_property_cache.layer[i].interleaving <= 3) ||
+				 (state->fe[0]->dtv_property_cache.layer[i].interleaving == 4 && state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1))
+			)
+			timeI = state->fe[0]->dtv_property_cache.layer[i].interleaving;
 		else
 			timeI = 0;
-		dib8000_write_word(state, 2 + i, (constellation << 10) | ((state->fe.dtv_property_cache.layer[i].segment_count & 0xf) << 6) |
-				   (crate << 3) | timeI);
-		if (state->fe.dtv_property_cache.layer[i].segment_count > 0) {
+		dib8000_write_word(state, 2 + i, (constellation << 10) | ((state->fe[0]->dtv_property_cache.layer[i].segment_count & 0xf) << 6) |
+					(crate << 3) | timeI);
+		if (state->fe[0]->dtv_property_cache.layer[i].segment_count > 0) {
 			switch (max_constellation) {
 			case DQPSK:
 			case QPSK:
-				if (state->fe.dtv_property_cache.layer[i].modulation == QAM_16 ||
-				    state->fe.dtv_property_cache.layer[i].modulation == QAM_64)
-					max_constellation = state->fe.dtv_property_cache.layer[i].modulation;
+				if (state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_16 ||
+					state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_64)
+					max_constellation = state->fe[0]->dtv_property_cache.layer[i].modulation;
 				break;
 			case QAM_16:
-				if (state->fe.dtv_property_cache.layer[i].modulation == QAM_64)
-					max_constellation = state->fe.dtv_property_cache.layer[i].modulation;
+				if (state->fe[0]->dtv_property_cache.layer[i].modulation == QAM_64)
+					max_constellation = state->fe[0]->dtv_property_cache.layer[i].modulation;
 				break;
 			}
 		}
@@ -1303,34 +1313,34 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	//dib8000_write_word(state, 5, 13); /*p_last_seg = 13*/
 
 	dib8000_write_word(state, 274, (dib8000_read_word(state, 274) & 0xffcf) |
-			   ((state->fe.dtv_property_cache.isdbt_partial_reception & 1) << 5) | ((state->fe.dtv_property_cache.
+				((state->fe[0]->dtv_property_cache.isdbt_partial_reception & 1) << 5) | ((state->fe[0]->dtv_property_cache.
 												 isdbt_sb_mode & 1) << 4));
 
-	dprintk("mode = %d ; guard = %d", mode, state->fe.dtv_property_cache.guard_interval);
+	dprintk("mode = %d ; guard = %d", mode, state->fe[0]->dtv_property_cache.guard_interval);
 
 	/* signal optimization parameter */
 
-	if (state->fe.dtv_property_cache.isdbt_partial_reception) {
-		seg_diff_mask = (state->fe.dtv_property_cache.layer[0].modulation == DQPSK) << permu_seg[0];
+	if (state->fe[0]->dtv_property_cache.isdbt_partial_reception) {
+		seg_diff_mask = (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) << permu_seg[0];
 		for (i = 1; i < 3; i++)
 			nbseg_diff +=
-			    (state->fe.dtv_property_cache.layer[i].modulation == DQPSK) * state->fe.dtv_property_cache.layer[i].segment_count;
+				(state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * state->fe[0]->dtv_property_cache.layer[i].segment_count;
 		for (i = 0; i < nbseg_diff; i++)
 			seg_diff_mask |= 1 << permu_seg[i + 1];
 	} else {
 		for (i = 0; i < 3; i++)
 			nbseg_diff +=
-			    (state->fe.dtv_property_cache.layer[i].modulation == DQPSK) * state->fe.dtv_property_cache.layer[i].segment_count;
+				(state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * state->fe[0]->dtv_property_cache.layer[i].segment_count;
 		for (i = 0; i < nbseg_diff; i++)
 			seg_diff_mask |= 1 << permu_seg[i];
 	}
 	dprintk("nbseg_diff = %X (%d)", seg_diff_mask, seg_diff_mask);
 
 	state->differential_constellation = (seg_diff_mask != 0);
-	dib8000_set_diversity_in(&state->fe, state->diversity_onoff);
+	dib8000_set_diversity_in(state->fe[0], state->diversity_onoff);
 
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {	// ISDB-Tsb
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 1)	// 3-segments
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 1)
 			seg_mask13 = 0x00E0;
 		else		// 1-segment
 			seg_mask13 = 0x0040;
@@ -1340,7 +1350,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	// WRITE: Mode & Diff mask
 	dib8000_write_word(state, 0, (mode << 13) | seg_diff_mask);
 
-	if ((seg_diff_mask) || (state->fe.dtv_property_cache.isdbt_sb_mode))
+	if ((seg_diff_mask) || (state->fe[0]->dtv_property_cache.isdbt_sb_mode))
 		dib8000_write_word(state, 268, (dib8000_read_word(state, 268) & 0xF9FF) | 0x0200);
 	else
 		dib8000_write_word(state, 268, (2 << 9) | 39);	//init value
@@ -1351,26 +1361,25 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 
 	dib8000_write_word(state, 353, seg_mask13);	// ADDR 353
 
-/*     // P_small_narrow_band=0, P_small_last_seg=13, P_small_offset_num_car=5 */
-	// dib8000_write_word(state, 351, (state->fe.dtv_property_cache.isdbt_sb_mode << 8) | (13 << 4) | 5 );
+/*	// P_small_narrow_band=0, P_small_last_seg=13, P_small_offset_num_car=5 */
 
 	// ---- SMALL ----
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {
-		switch (state->fe.dtv_property_cache.transmission_mode) {
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+		switch (state->fe[0]->dtv_property_cache.transmission_mode) {
 		case TRANSMISSION_MODE_2K:
-			if (state->fe.dtv_property_cache.isdbt_partial_reception == 0) {	// 1-seg
-				if (state->fe.dtv_property_cache.layer[0].modulation == DQPSK)	// DQPSK
+			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
 					ncoeff = coeff_2k_sb_1seg_dqpsk;
 				else	// QPSK or QAM
 					ncoeff = coeff_2k_sb_1seg;
 			} else {	// 3-segments
-				if (state->fe.dtv_property_cache.layer[0].modulation == DQPSK) {	// DQPSK on central segment
-					if (state->fe.dtv_property_cache.layer[1].modulation == DQPSK)	// DQPSK on external segments
+				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
+					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK)
 						ncoeff = coeff_2k_sb_3seg_0dqpsk_1dqpsk;
 					else	// QPSK or QAM on external segments
 						ncoeff = coeff_2k_sb_3seg_0dqpsk;
 				} else {	// QPSK or QAM on central segment
-					if (state->fe.dtv_property_cache.layer[1].modulation == DQPSK)	// DQPSK on external segments
+					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK)
 						ncoeff = coeff_2k_sb_3seg_1dqpsk;
 					else	// QPSK or QAM on external segments
 						ncoeff = coeff_2k_sb_3seg;
@@ -1379,20 +1388,20 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			break;
 
 		case TRANSMISSION_MODE_4K:
-			if (state->fe.dtv_property_cache.isdbt_partial_reception == 0) {	// 1-seg
-				if (state->fe.dtv_property_cache.layer[0].modulation == DQPSK)	// DQPSK
+			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
 					ncoeff = coeff_4k_sb_1seg_dqpsk;
 				else	// QPSK or QAM
 					ncoeff = coeff_4k_sb_1seg;
 			} else {	// 3-segments
-				if (state->fe.dtv_property_cache.layer[0].modulation == DQPSK) {	// DQPSK on central segment
-					if (state->fe.dtv_property_cache.layer[1].modulation == DQPSK) {	// DQPSK on external segments
+				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
+					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
 						ncoeff = coeff_4k_sb_3seg_0dqpsk_1dqpsk;
 					} else {	// QPSK or QAM on external segments
 						ncoeff = coeff_4k_sb_3seg_0dqpsk;
 					}
 				} else {	// QPSK or QAM on central segment
-					if (state->fe.dtv_property_cache.layer[1].modulation == DQPSK) {	// DQPSK on external segments
+					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
 						ncoeff = coeff_4k_sb_3seg_1dqpsk;
 					} else	// QPSK or QAM on external segments
 						ncoeff = coeff_4k_sb_3seg;
@@ -1403,20 +1412,20 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 		case TRANSMISSION_MODE_AUTO:
 		case TRANSMISSION_MODE_8K:
 		default:
-			if (state->fe.dtv_property_cache.isdbt_partial_reception == 0) {	// 1-seg
-				if (state->fe.dtv_property_cache.layer[0].modulation == DQPSK)	// DQPSK
+			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
+				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK)
 					ncoeff = coeff_8k_sb_1seg_dqpsk;
 				else	// QPSK or QAM
 					ncoeff = coeff_8k_sb_1seg;
 			} else {	// 3-segments
-				if (state->fe.dtv_property_cache.layer[0].modulation == DQPSK) {	// DQPSK on central segment
-					if (state->fe.dtv_property_cache.layer[1].modulation == DQPSK) {	// DQPSK on external segments
+				if (state->fe[0]->dtv_property_cache.layer[0].modulation == DQPSK) {
+					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
 						ncoeff = coeff_8k_sb_3seg_0dqpsk_1dqpsk;
 					} else {	// QPSK or QAM on external segments
 						ncoeff = coeff_8k_sb_3seg_0dqpsk;
 					}
 				} else {	// QPSK or QAM on central segment
-					if (state->fe.dtv_property_cache.layer[1].modulation == DQPSK) {	// DQPSK on external segments
+					if (state->fe[0]->dtv_property_cache.layer[1].modulation == DQPSK) {
 						ncoeff = coeff_8k_sb_3seg_1dqpsk;
 					} else	// QPSK or QAM on external segments
 						ncoeff = coeff_8k_sb_3seg;
@@ -1430,22 +1439,22 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 
 	// P_small_coef_ext_enable=ISDB-Tsb, P_small_narrow_band=ISDB-Tsb, P_small_last_seg=13, P_small_offset_num_car=5
 	dib8000_write_word(state, 351,
-			   (state->fe.dtv_property_cache.isdbt_sb_mode << 9) | (state->fe.dtv_property_cache.isdbt_sb_mode << 8) | (13 << 4) | 5);
+				(state->fe[0]->dtv_property_cache.isdbt_sb_mode << 9) | (state->fe[0]->dtv_property_cache.isdbt_sb_mode << 8) | (13 << 4) | 5);
 
 	// ---- COFF ----
 	// Carloff, the most robust
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {	// Sound Broadcasting mode - use both TMCC and AC pilots
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
 
 		// P_coff_cpil_alpha=4, P_coff_inh=0, P_coff_cpil_winlen=64
 		// P_coff_narrow_band=1, P_coff_square_val=1, P_coff_one_seg=~partial_rcpt, P_coff_use_tmcc=1, P_coff_use_ac=1
 		dib8000_write_word(state, 187,
-				   (4 << 12) | (0 << 11) | (63 << 5) | (0x3 << 3) | ((~state->fe.dtv_property_cache.isdbt_partial_reception & 1) << 2)
-				   | 0x3);
+					(4 << 12) | (0 << 11) | (63 << 5) | (0x3 << 3) | ((~state->fe[0]->dtv_property_cache.isdbt_partial_reception & 1) << 2)
+					| 0x3);
 
-/*             // P_small_coef_ext_enable = 1 */
-/*             dib8000_write_word(state, 351, dib8000_read_word(state, 351) | 0x200); */
+/*		// P_small_coef_ext_enable = 1 */
+/*		dib8000_write_word(state, 351, dib8000_read_word(state, 351) | 0x200); */
 
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 0) {	// Sound Broadcasting mode 1 seg
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
 
 			// P_coff_winlen=63, P_coff_thres_lock=15, P_coff_one_seg_width= (P_mode == 3) , P_coff_one_seg_sym= (P_mode-1)
 			if (mode == 3)
@@ -1469,10 +1478,10 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 			dib8000_write_word(state, 186, 80);
 		} else {	// Sound Broadcasting mode 3 seg
 			// P_coff_one_seg_sym= 1, P_coff_one_seg_width= 1, P_coff_winlen=63, P_coff_thres_lock=15
-			/*                 if (mode == 3) */
-			/*                     dib8000_write_word(state, 180, 0x2fca | ((0) << 14)); */
-			/*                 else */
-			/*                     dib8000_write_word(state, 180, 0x2fca | ((1) << 14)); */
+			/*	if (mode == 3) */
+			/*		dib8000_write_word(state, 180, 0x2fca | ((0) << 14)); */
+			/*	else */
+			/*		dib8000_write_word(state, 180, 0x2fca | ((1) << 14)); */
 			dib8000_write_word(state, 180, 0x1fcf | (1 << 14));
 
 			// P_ctrl_corm_thres4pre_freq_inh = 1, P_ctrl_pre_freq_mode_sat=1,
@@ -1509,7 +1518,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 		dib8000_write_word(state, 341, (4 << 3) | (1 << 2) | (1 << 1) | (1 << 0));
 	}
 	// ---- FFT ----
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1 && state->fe.dtv_property_cache.isdbt_partial_reception == 0)	// 1-seg
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1 && state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
 		dib8000_write_word(state, 178, 64);	// P_fft_powrange=64
 	else
 		dib8000_write_word(state, 178, 32);	// P_fft_powrange=32
@@ -1518,12 +1527,12 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	 * 6bits; p_coff_thres_lock 6bits (for coff lock if needed)
 	 */
 	/* if ( ( nbseg_diff>0)&&(nbseg_diff<13))
-	   dib8000_write_word(state, 187, (dib8000_read_word(state, 187) & 0xfffb) | (1 << 3)); */
+		dib8000_write_word(state, 187, (dib8000_read_word(state, 187) & 0xfffb) | (1 << 3)); */
 
 	dib8000_write_word(state, 189, ~seg_mask13 | seg_diff_mask);	/* P_lmod4_seg_inh       */
 	dib8000_write_word(state, 192, ~seg_mask13 | seg_diff_mask);	/* P_pha3_seg_inh        */
 	dib8000_write_word(state, 225, ~seg_mask13 | seg_diff_mask);	/* P_tac_seg_inh         */
-	if ((!state->fe.dtv_property_cache.isdbt_sb_mode) && (state->cfg.pll->ifreq == 0))
+	if ((!state->fe[0]->dtv_property_cache.isdbt_sb_mode) && (state->cfg.pll->ifreq == 0))
 		dib8000_write_word(state, 266, ~seg_mask13 | seg_diff_mask | 0x40);	/* P_equal_noise_seg_inh */
 	else
 		dib8000_write_word(state, 266, ~seg_mask13 | seg_diff_mask);	/* P_equal_noise_seg_inh */
@@ -1538,8 +1547,8 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	dib8000_write_word(state, 211, seg_mask13 & (~seg_diff_mask));	/* P_des_seg_enabled     */
 
 	/* offset loop parameters */
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 0)	// Sound Broadcasting mode 1 seg
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
 			/* P_timf_alpha = (11-P_mode), P_corm_alpha=6, P_corm_thres=0x80 */
 			dib8000_write_word(state, 32, ((11 - mode) << 12) | (6 << 8) | 0x40);
 
@@ -1551,8 +1560,8 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 		/* P_timf_alpha = (9-P_mode, P_corm_alpha=6, P_corm_thres=0x80 */
 		dib8000_write_word(state, 32, ((9 - mode) << 12) | (6 << 8) | 0x80);
 
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 0)	// Sound Broadcasting mode 1 seg
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
 			/* P_ctrl_pha_off_max=3   P_ctrl_sfreq_inh =0  P_ctrl_sfreq_step = (11-P_mode)  */
 			dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (10 - mode));
 
@@ -1564,7 +1573,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 		dib8000_write_word(state, 37, (3 << 5) | (0 << 4) | (8 - mode));
 
 	/* P_dvsy_sync_wait - reuse mode */
-	switch (state->fe.dtv_property_cache.transmission_mode) {
+	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
 	case TRANSMISSION_MODE_8K:
 		mode = 256;
 		break;
@@ -1624,15 +1633,15 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	}
 
 	// ---- ANA_FE ----
-	if (state->fe.dtv_property_cache.isdbt_sb_mode) {
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 1)	// 3-segments
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 1)
 			ana_fe = ana_fe_coeff_3seg;
 		else		// 1-segment
 			ana_fe = ana_fe_coeff_1seg;
 	} else
 		ana_fe = ana_fe_coeff_13seg;
 
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1 || state->isdbt_cfg_loaded == 0)
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1 || state->isdbt_cfg_loaded == 0)
 		for (mode = 0; mode < 24; mode++)
 			dib8000_write_word(state, 117 + mode, ana_fe[mode]);
 
@@ -1648,11 +1657,11 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	// "P_cspu_left_edge"  not used => do not care
 	// "P_cspu_right_edge" not used => do not care
 
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {	// ISDB-Tsb
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
 		dib8000_write_word(state, 228, 1);	// P_2d_mode_byp=1
 		dib8000_write_word(state, 205, dib8000_read_word(state, 205) & 0xfff0);	// P_cspu_win_cut = 0
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 0	// 1-segment
-		    && state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K) {
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0
+			&& state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_2K) {
 			//dib8000_write_word(state, 219, dib8000_read_word(state, 219) & 0xfffe); // P_adp_pass = 0
 			dib8000_write_word(state, 265, 15);	// P_equal_noise_sel = 15
 		}
@@ -1664,7 +1673,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	// ---- TMCC ----
 	for (i = 0; i < 3; i++)
 		tmcc_pow +=
-		    (((state->fe.dtv_property_cache.layer[i].modulation == DQPSK) * 4 + 1) * state->fe.dtv_property_cache.layer[i].segment_count);
+			(((state->fe[0]->dtv_property_cache.layer[i].modulation == DQPSK) * 4 + 1) * state->fe[0]->dtv_property_cache.layer[i].segment_count);
 	// Quantif of "P_tmcc_dec_thres_?k" is (0, 5+mode, 9);
 	// Threshold is set at 1/4 of max power.
 	tmcc_pow *= (1 << (9 - 2));
@@ -1678,7 +1687,7 @@ static void dib8000_set_channel(struct dib8000_state *state, u8 seq, u8 autosear
 	if (state->isdbt_cfg_loaded == 0)
 		dib8000_write_word(state, 250, 3285);	/*p_2d_hspeed_thr0 */
 
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1)
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1)
 		state->isdbt_cfg_loaded = 0;
 	else
 		state->isdbt_cfg_loaded = 1;
@@ -1693,38 +1702,38 @@ static int dib8000_autosearch_start(struct dvb_frontend *fe)
 
 	int slist = 0;
 
-	state->fe.dtv_property_cache.inversion = 0;
-	if (!state->fe.dtv_property_cache.isdbt_sb_mode)
-		state->fe.dtv_property_cache.layer[0].segment_count = 13;
-	state->fe.dtv_property_cache.layer[0].modulation = QAM_64;
-	state->fe.dtv_property_cache.layer[0].fec = FEC_2_3;
-	state->fe.dtv_property_cache.layer[0].interleaving = 0;
+	state->fe[0]->dtv_property_cache.inversion = 0;
+	if (!state->fe[0]->dtv_property_cache.isdbt_sb_mode)
+		state->fe[0]->dtv_property_cache.layer[0].segment_count = 13;
+	state->fe[0]->dtv_property_cache.layer[0].modulation = QAM_64;
+	state->fe[0]->dtv_property_cache.layer[0].fec = FEC_2_3;
+	state->fe[0]->dtv_property_cache.layer[0].interleaving = 0;
 
 	//choose the right list, in sb, always do everything
-	if (state->fe.dtv_property_cache.isdbt_sb_mode) {
-		state->fe.dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
-		state->fe.dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode) {
+		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
+		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
 		slist = 7;
 		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));
 	} else {
-		if (state->fe.dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) {
-			if (state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
+		if (state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) {
+			if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
 				slist = 7;
 				dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));	// P_mode = 1 to have autosearch start ok with mode2
 			} else
 				slist = 3;
 		} else {
-			if (state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
+			if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) {
 				slist = 2;
 				dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x9fff) | (1 << 13));	// P_mode = 1
 			} else
 				slist = 0;
 		}
 
-		if (state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO)
-			state->fe.dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
-		if (state->fe.dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO)
-			state->fe.dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
+		if (state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO)
+			state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
+		if (state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO)
+			state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
 
 		dprintk("using list for autosearch : %d", slist);
 		dib8000_set_channel(state, (unsigned char)slist, 1);
@@ -1786,7 +1795,7 @@ static int dib8000_tune(struct dvb_frontend *fe)
 	if (state == NULL)
 		return -EINVAL;
 
-	dib8000_set_bandwidth(state, state->fe.dtv_property_cache.bandwidth_hz / 1000);
+	dib8000_set_bandwidth(fe, state->fe[0]->dtv_property_cache.bandwidth_hz / 1000);
 	dib8000_set_channel(state, 0, 0);
 
 	// restart demod
@@ -1799,17 +1808,16 @@ static int dib8000_tune(struct dvb_frontend *fe)
 
 	// never achieved a lock before - wait for timfreq to update
 	if (state->timf == 0) {
-		if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {
-			if (state->fe.dtv_property_cache.isdbt_partial_reception == 0)	// Sound Broadcasting mode 1 seg
+		if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+			if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0)
 				msleep(300);
 			else	// Sound Broadcasting mode 3 seg
 				msleep(500);
 		} else		// 13 seg
 			msleep(200);
 	}
-	//dump_reg(state);
-	if (state->fe.dtv_property_cache.isdbt_sb_mode == 1) {
-		if (state->fe.dtv_property_cache.isdbt_partial_reception == 0) {	// Sound Broadcasting mode 1 seg
+	if (state->fe[0]->dtv_property_cache.isdbt_sb_mode == 1) {
+		if (state->fe[0]->dtv_property_cache.isdbt_partial_reception == 0) {
 
 			/* P_timf_alpha = (13-P_mode) , P_corm_alpha=6, P_corm_thres=0x40  alpha to check on board */
 			dib8000_write_word(state, 32, ((13 - mode) << 12) | (6 << 8) | 0x40);
@@ -1854,26 +1862,38 @@ static int dib8000_tune(struct dvb_frontend *fe)
 static int dib8000_wakeup(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	int ret;
 
 	dib8000_set_power_mode(state, DIB8000M_POWER_ALL);
 	dib8000_set_adc_state(state, DIBX000_ADC_ON);
 	if (dib8000_set_adc_state(state, DIBX000_SLOW_ADC_ON) != 0)
 		dprintk("could not start Slow ADC");
 
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		ret = state->fe[index_frontend]->ops.init(state->fe[index_frontend]);
+		if (ret < 0)
+			return ret;
+	}
+
 	return 0;
 }
 
 static int dib8000_sleep(struct dvb_frontend *fe)
 {
-	struct dib8000_state *st = fe->demodulator_priv;
-	if (1) {
-		dib8000_set_output_mode(st, OUTMODE_HIGH_Z);
-		dib8000_set_power_mode(st, DIB8000M_POWER_INTERFACE_ONLY);
-		return dib8000_set_adc_state(st, DIBX000_SLOW_ADC_OFF) | dib8000_set_adc_state(st, DIBX000_ADC_OFF);
-	} else {
+	struct dib8000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	int ret;
 
-		return 0;
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		ret = state->fe[index_frontend]->ops.sleep(state->fe[index_frontend]);
+		if (ret < 0)
+			return ret;
 	}
+
+	dib8000_set_output_mode(fe, OUTMODE_HIGH_Z);
+	dib8000_set_power_mode(state, DIB8000M_POWER_INTERFACE_ONLY);
+	return dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF) | dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 }
 
 enum frontend_tune_state dib8000_get_tune_state(struct dvb_frontend *fe)
@@ -1891,16 +1911,40 @@ int dib8000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tun
 }
 EXPORT_SYMBOL(dib8000_set_tune_state);
 
-
-
-
 static int dib8000_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 i, val = 0;
+	fe_status_t stat;
+	u8 index_frontend, sub_index_frontend;
 
 	fe->dtv_property_cache.bandwidth_hz = 6000000;
 
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		state->fe[index_frontend]->ops.read_status(state->fe[index_frontend], &stat);
+		if (stat&FE_HAS_SYNC) {
+			dprintk("TMCC lock on the slave%i", index_frontend);
+			/* synchronize the cache with the other frontends */
+			state->fe[index_frontend]->ops.get_frontend(state->fe[index_frontend], fep);
+			for (sub_index_frontend = 0; (sub_index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[sub_index_frontend] != NULL); sub_index_frontend++) {
+				if (sub_index_frontend != index_frontend) {
+					state->fe[sub_index_frontend]->dtv_property_cache.isdbt_sb_mode = state->fe[index_frontend]->dtv_property_cache.isdbt_sb_mode;
+					state->fe[sub_index_frontend]->dtv_property_cache.inversion = state->fe[index_frontend]->dtv_property_cache.inversion;
+					state->fe[sub_index_frontend]->dtv_property_cache.transmission_mode = state->fe[index_frontend]->dtv_property_cache.transmission_mode;
+					state->fe[sub_index_frontend]->dtv_property_cache.guard_interval = state->fe[index_frontend]->dtv_property_cache.guard_interval;
+					state->fe[sub_index_frontend]->dtv_property_cache.isdbt_partial_reception = state->fe[index_frontend]->dtv_property_cache.isdbt_partial_reception;
+					for (i = 0; i < 3; i++) {
+						state->fe[sub_index_frontend]->dtv_property_cache.layer[i].segment_count = state->fe[index_frontend]->dtv_property_cache.layer[i].segment_count;
+						state->fe[sub_index_frontend]->dtv_property_cache.layer[i].interleaving = state->fe[index_frontend]->dtv_property_cache.layer[i].interleaving;
+						state->fe[sub_index_frontend]->dtv_property_cache.layer[i].fec = state->fe[index_frontend]->dtv_property_cache.layer[i].fec;
+						state->fe[sub_index_frontend]->dtv_property_cache.layer[i].modulation = state->fe[index_frontend]->dtv_property_cache.layer[i].modulation;
+					}
+				}
+			}
+			return 0;
+		}
+	}
+
 	fe->dtv_property_cache.isdbt_sb_mode = dib8000_read_word(state, 508) & 0x1;
 
 	val = dib8000_read_word(state, 570);
@@ -1992,112 +2036,200 @@ static int dib8000_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 			break;
 		}
 	}
+
+	/* synchronize the cache with the other frontends */
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		state->fe[index_frontend]->dtv_property_cache.isdbt_sb_mode = fe->dtv_property_cache.isdbt_sb_mode;
+		state->fe[index_frontend]->dtv_property_cache.inversion = fe->dtv_property_cache.inversion;
+		state->fe[index_frontend]->dtv_property_cache.transmission_mode = fe->dtv_property_cache.transmission_mode;
+		state->fe[index_frontend]->dtv_property_cache.guard_interval = fe->dtv_property_cache.guard_interval;
+		state->fe[index_frontend]->dtv_property_cache.isdbt_partial_reception = fe->dtv_property_cache.isdbt_partial_reception;
+		for (i = 0; i < 3; i++) {
+			state->fe[index_frontend]->dtv_property_cache.layer[i].segment_count = fe->dtv_property_cache.layer[i].segment_count;
+			state->fe[index_frontend]->dtv_property_cache.layer[i].interleaving = fe->dtv_property_cache.layer[i].interleaving;
+			state->fe[index_frontend]->dtv_property_cache.layer[i].fec = fe->dtv_property_cache.layer[i].fec;
+			state->fe[index_frontend]->dtv_property_cache.layer[i].modulation = fe->dtv_property_cache.layer[i].modulation;
+		}
+	}
 	return 0;
 }
 
 static int dib8000_set_frontend(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
+	u8 nbr_pending, exit_condition, index_frontend;
+	s8 index_frontend_success = -1;
 	int time, ret;
+	int  time_slave = FE_CALLBACK_TIME_NEVER;
 
-	fe->dtv_property_cache.delivery_system = SYS_ISDBT;
+	if (state->fe[0]->dtv_property_cache.frequency == 0) {
+		dprintk("dib8000: must at least specify frequency ");
+		return 0;
+	}
 
-	dib8000_set_output_mode(state, OUTMODE_HIGH_Z);
+	if (state->fe[0]->dtv_property_cache.bandwidth_hz == 0) {
+		dprintk("dib8000: no bandwidth specified, set to default ");
+		state->fe[0]->dtv_property_cache.bandwidth_hz = 6000000;
+	}
+
+	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		/* synchronization of the cache */
+		state->fe[index_frontend]->dtv_property_cache.delivery_system = SYS_ISDBT;
+		memcpy(&state->fe[index_frontend]->dtv_property_cache, &fe->dtv_property_cache, sizeof(struct dtv_frontend_properties));
+
+		dib8000_set_output_mode(state->fe[index_frontend], OUTMODE_HIGH_Z);
+		if (state->fe[index_frontend]->ops.tuner_ops.set_params)
+			state->fe[index_frontend]->ops.tuner_ops.set_params(state->fe[index_frontend], fep);
 
-	if (fe->ops.tuner_ops.set_params)
-		fe->ops.tuner_ops.set_params(fe, fep);
+		dib8000_set_tune_state(state->fe[index_frontend], CT_AGC_START);
+	}
 
 	/* start up the AGC */
-	state->tune_state = CT_AGC_START;
 	do {
-		time = dib8000_agc_startup(fe);
+		time = dib8000_agc_startup(state->fe[0]);
+		for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			time_slave = dib8000_agc_startup(state->fe[index_frontend]);
+			if (time == FE_CALLBACK_TIME_NEVER)
+				time = time_slave;
+			else if ((time_slave != FE_CALLBACK_TIME_NEVER) && (time_slave > time))
+				time = time_slave;
+		}
 		if (time != FE_CALLBACK_TIME_NEVER)
 			msleep(time / 10);
 		else
 			break;
-	} while (state->tune_state != CT_AGC_STOP);
-
-	if (state->fe.dtv_property_cache.frequency == 0) {
-		dprintk("dib8000: must at least specify frequency ");
-		return 0;
-	}
-
-	if (state->fe.dtv_property_cache.bandwidth_hz == 0) {
-		dprintk("dib8000: no bandwidth specified, set to default ");
-		state->fe.dtv_property_cache.bandwidth_hz = 6000000;
-	}
+		exit_condition = 1;
+		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			if (dib8000_get_tune_state(state->fe[index_frontend]) != CT_AGC_STOP) {
+				exit_condition = 0;
+				break;
+			}
+		}
+	} while (exit_condition == 0);
+
+	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		dib8000_set_tune_state(state->fe[index_frontend], CT_DEMOD_START);
+
+	if ((state->fe[0]->dtv_property_cache.delivery_system != SYS_ISDBT) ||
+			(state->fe[0]->dtv_property_cache.inversion == INVERSION_AUTO) ||
+			(state->fe[0]->dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) ||
+			(state->fe[0]->dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) ||
+			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 0)) != 0) &&
+			 (state->fe[0]->dtv_property_cache.layer[0].segment_count != 0xff) &&
+			 (state->fe[0]->dtv_property_cache.layer[0].segment_count != 0) &&
+			 ((state->fe[0]->dtv_property_cache.layer[0].modulation == QAM_AUTO) ||
+			  (state->fe[0]->dtv_property_cache.layer[0].fec == FEC_AUTO))) ||
+			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 1)) != 0) &&
+			 (state->fe[0]->dtv_property_cache.layer[1].segment_count != 0xff) &&
+			 (state->fe[0]->dtv_property_cache.layer[1].segment_count != 0) &&
+			 ((state->fe[0]->dtv_property_cache.layer[1].modulation == QAM_AUTO) ||
+			  (state->fe[0]->dtv_property_cache.layer[1].fec == FEC_AUTO))) ||
+			(((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 2)) != 0) &&
+			 (state->fe[0]->dtv_property_cache.layer[2].segment_count != 0xff) &&
+			 (state->fe[0]->dtv_property_cache.layer[2].segment_count != 0) &&
+			 ((state->fe[0]->dtv_property_cache.layer[2].modulation == QAM_AUTO) ||
+			  (state->fe[0]->dtv_property_cache.layer[2].fec == FEC_AUTO))) ||
+			(((state->fe[0]->dtv_property_cache.layer[0].segment_count == 0) ||
+			  ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (1 << 0)) == 0)) &&
+			 ((state->fe[0]->dtv_property_cache.layer[1].segment_count == 0) ||
+			  ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (2 << 0)) == 0)) &&
+			 ((state->fe[0]->dtv_property_cache.layer[2].segment_count == 0) || ((state->fe[0]->dtv_property_cache.isdbt_layer_enabled & (3 << 0)) == 0)))) {
+		int i = 80000;
+		u8 found = 0;
+		u8 tune_failed = 0;
+
+		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			dib8000_set_bandwidth(state->fe[index_frontend], fe->dtv_property_cache.bandwidth_hz / 1000);
+			dib8000_autosearch_start(state->fe[index_frontend]);
+		}
 
-	state->tune_state = CT_DEMOD_START;
-
-	if ((state->fe.dtv_property_cache.delivery_system != SYS_ISDBT) ||
-	    (state->fe.dtv_property_cache.inversion == INVERSION_AUTO) ||
-	    (state->fe.dtv_property_cache.transmission_mode == TRANSMISSION_MODE_AUTO) ||
-	    (state->fe.dtv_property_cache.guard_interval == GUARD_INTERVAL_AUTO) ||
-	    (((state->fe.dtv_property_cache.isdbt_layer_enabled & (1 << 0)) != 0) &&
-	     (state->fe.dtv_property_cache.layer[0].segment_count != 0xff) &&
-	     (state->fe.dtv_property_cache.layer[0].segment_count != 0) &&
-	     ((state->fe.dtv_property_cache.layer[0].modulation == QAM_AUTO) ||
-	      (state->fe.dtv_property_cache.layer[0].fec == FEC_AUTO))) ||
-	    (((state->fe.dtv_property_cache.isdbt_layer_enabled & (1 << 1)) != 0) &&
-	     (state->fe.dtv_property_cache.layer[1].segment_count != 0xff) &&
-	     (state->fe.dtv_property_cache.layer[1].segment_count != 0) &&
-	     ((state->fe.dtv_property_cache.layer[1].modulation == QAM_AUTO) ||
-	      (state->fe.dtv_property_cache.layer[1].fec == FEC_AUTO))) ||
-	    (((state->fe.dtv_property_cache.isdbt_layer_enabled & (1 << 2)) != 0) &&
-	     (state->fe.dtv_property_cache.layer[2].segment_count != 0xff) &&
-	     (state->fe.dtv_property_cache.layer[2].segment_count != 0) &&
-	     ((state->fe.dtv_property_cache.layer[2].modulation == QAM_AUTO) ||
-	      (state->fe.dtv_property_cache.layer[2].fec == FEC_AUTO))) ||
-	    (((state->fe.dtv_property_cache.layer[0].segment_count == 0) ||
-	      ((state->fe.dtv_property_cache.isdbt_layer_enabled & (1 << 0)) == 0)) &&
-	     ((state->fe.dtv_property_cache.layer[1].segment_count == 0) ||
-	      ((state->fe.dtv_property_cache.isdbt_layer_enabled & (2 << 0)) == 0)) &&
-	     ((state->fe.dtv_property_cache.layer[2].segment_count == 0) || ((state->fe.dtv_property_cache.isdbt_layer_enabled & (3 << 0)) == 0)))) {
-		int i = 800, found;
-
-		dib8000_set_bandwidth(state, fe->dtv_property_cache.bandwidth_hz / 1000);
-		dib8000_autosearch_start(fe);
 		do {
-			msleep(10);
-			found = dib8000_autosearch_irq(fe);
-		} while (found == 0 && i--);
+			msleep(20);
+			nbr_pending = 0;
+			exit_condition = 0; /* 0: tune pending; 1: tune failed; 2:tune success */
+			for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+				if (((tune_failed >> index_frontend) & 0x1) == 0) {
+					found = dib8000_autosearch_irq(state->fe[index_frontend]);
+					switch (found) {
+					case 0: /* tune pending */
+						 nbr_pending++;
+						 break;
+					case 2:
+						 dprintk("autosearch succeed on the frontend%i", index_frontend);
+						 exit_condition = 2;
+						 index_frontend_success = index_frontend;
+						 break;
+					default:
+						 dprintk("unhandled autosearch result");
+					case 1:
+						 dprintk("autosearch failed for the frontend%i", index_frontend);
+						 break;
+					}
+				}
+			}
 
-		dprintk("Frequency %d Hz, autosearch returns: %d", fep->frequency, found);
+			/* if all tune are done and no success, exit: tune failed */
+			if ((nbr_pending == 0) && (exit_condition == 0))
+				exit_condition = 1;
+		} while ((exit_condition == 0) && i--);
 
-		if (found == 0 || found == 1)
-			return 0;	// no channel found
+		if (exit_condition == 1) { /* tune failed */
+			dprintk("tune failed");
+			return 0;
+		}
+
+		dprintk("tune success on frontend%i", index_frontend_success);
 
 		dib8000_get_frontend(fe, fep);
 	}
 
-	ret = dib8000_tune(fe);
+	for (index_frontend = 0, ret = 0; (ret >= 0) && (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		ret = dib8000_tune(state->fe[index_frontend]);
+
+	/* set output mode and diversity input */
+	dib8000_set_output_mode(state->fe[0], state->cfg.output_mode);
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		dib8000_set_output_mode(state->fe[index_frontend], OUTMODE_DIVERSITY);
+		dib8000_set_diversity_in(state->fe[index_frontend-1], 1);
+	}
 
-	/* make this a config parameter */
-	dib8000_set_output_mode(state, state->cfg.output_mode);
+	/* turn off the diversity of the last chip */
+	dib8000_set_diversity_in(state->fe[index_frontend-1], 0);
 
 	return ret;
 }
 
+static u16 dib8000_read_lock(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	return dib8000_read_word(state, 568);
+}
+
 static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-	u16 lock = dib8000_read_word(state, 568);
+	u16 lock_slave = 0, lock = dib8000_read_word(state, 568);
+	u8 index_frontend;
+
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		lock_slave |= dib8000_read_lock(state->fe[index_frontend]);
 
 	*stat = 0;
 
-	if ((lock >> 13) & 1)
+	if (((lock >> 13) & 1) || ((lock_slave >> 13) & 1))
 		*stat |= FE_HAS_SIGNAL;
 
-	if ((lock >> 8) & 1) /* Equal */
+	if (((lock >> 8) & 1) || ((lock_slave >> 8) & 1)) /* Equal */
 		*stat |= FE_HAS_CARRIER;
 
-	if (((lock >> 1) & 0xf) == 0xf) /* TMCC_SYNC */
+	if ((((lock >> 1) & 0xf) == 0xf) || (((lock_slave >> 1) & 0xf) == 0xf)) /* TMCC_SYNC */
 		*stat |= FE_HAS_SYNC;
 
-	if (((lock >> 12) & 1) && ((lock >> 5) & 7)) /* FEC MPEG */
+	if ((((lock >> 12) & 1) || ((lock_slave >> 12) & 1)) && ((lock >> 5) & 7)) /* FEC MPEG */
 		*stat |= FE_HAS_LOCK;
 
-	if ((lock >> 12) & 1) {
+	if (((lock >> 12) & 1) || ((lock_slave >> 12) & 1)) {
 		lock = dib8000_read_word(state, 554); /* Viterbi Layer A */
 		if (lock & 0x01)
 			*stat |= FE_HAS_VITERBI;
@@ -2131,44 +2263,120 @@ static int dib8000_read_unc_blocks(struct dvb_frontend *fe, u32 * unc)
 static int dib8000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
-	u16 val = dib8000_read_word(state, 390);
-	*strength = 65535 - val;
+	u8 index_frontend;
+	u16 val;
+
+	*strength = 0;
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		state->fe[index_frontend]->ops.read_signal_strength(state->fe[index_frontend], &val);
+		if (val > 65535 - *strength)
+			*strength = 65535;
+		else
+			*strength += val;
+	}
+
+	val = 65535 - dib8000_read_word(state, 390);
+	if (val > 65535 - *strength)
+		*strength = 65535;
+	else
+		*strength += val;
 	return 0;
 }
 
-static int dib8000_read_snr(struct dvb_frontend *fe, u16 * snr)
+static u32 dib8000_get_snr(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
+	u32 n, s, exp;
 	u16 val;
-	s32 signal_mant, signal_exp, noise_mant, noise_exp;
-	u32 result = 0;
 
 	val = dib8000_read_word(state, 542);
-	noise_mant = (val >> 6) & 0xff;
-	noise_exp = (val & 0x3f);
+	n = (val >> 6) & 0xff;
+	exp = (val & 0x3f);
+	if ((exp & 0x20) != 0)
+		exp -= 0x40;
+	n <<= exp+16;
 
 	val = dib8000_read_word(state, 543);
-	signal_mant = (val >> 6) & 0xff;
-	signal_exp = (val & 0x3f);
+	s = (val >> 6) & 0xff;
+	exp = (val & 0x3f);
+	if ((exp & 0x20) != 0)
+		exp -= 0x40;
+	s <<= exp+16;
+
+	if (n > 0) {
+		u32 t = (s/n) << 16;
+		return t + ((s << 16) - n*t) / n;
+	}
+	return 0xffffffff;
+}
 
-	if ((noise_exp & 0x20) != 0)
-		noise_exp -= 0x40;
-	if ((signal_exp & 0x20) != 0)
-		signal_exp -= 0x40;
+static int dib8000_read_snr(struct dvb_frontend *fe, u16 * snr)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	u32 snr_master;
 
-	if (signal_mant != 0)
-		result = intlog10(2) * 10 * signal_exp + 10 * intlog10(signal_mant);
-	else
-		result = intlog10(2) * 10 * signal_exp - 100;
-	if (noise_mant != 0)
-		result -= intlog10(2) * 10 * noise_exp + 10 * intlog10(noise_mant);
+	snr_master = dib8000_get_snr(fe);
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		snr_master += dib8000_get_snr(state->fe[index_frontend]);
+
+	if (snr_master != 0) {
+		snr_master = 10*intlog10(snr_master>>16);
+		*snr = snr_master / ((1 << 24) / 10);
+	}
 	else
-		result -= intlog10(2) * 10 * noise_exp - 100;
+		*snr = 0;
 
-	*snr = result / ((1 << 24) / 10);
 	return 0;
 }
 
+int dib8000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	u8 index_frontend = 1;
+
+	while ((index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL))
+		index_frontend++;
+	if (index_frontend < MAX_NUMBER_OF_FRONTENDS) {
+		dprintk("set slave fe %p to index %i", fe_slave, index_frontend);
+		state->fe[index_frontend] = fe_slave;
+		return 0;
+	}
+
+	dprintk("too many slave frontend");
+	return -ENOMEM;
+}
+EXPORT_SYMBOL(dib8000_set_slave_frontend);
+
+int dib8000_remove_slave_frontend(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	u8 index_frontend = 1;
+
+	while ((index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL))
+		index_frontend++;
+	if (index_frontend != 1) {
+		dprintk("remove slave fe %p (index %i)", state->fe[index_frontend-1], index_frontend-1);
+		state->fe[index_frontend] = NULL;
+		return 0;
+	}
+
+	dprintk("no frontend to be removed");
+	return -ENODEV;
+}
+EXPORT_SYMBOL(dib8000_remove_slave_frontend);
+
+struct dvb_frontend *dib8000_get_slave_frontend(struct dvb_frontend *fe, int slave_index)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+
+	if (slave_index >= MAX_NUMBER_OF_FRONTENDS)
+		return NULL;
+	return state->fe[slave_index];
+}
+EXPORT_SYMBOL(dib8000_get_slave_frontend);
+
+
 int dib8000_i2c_enumeration(struct i2c_adapter *host, int no_of_demods, u8 default_addr, u8 first_addr)
 {
 	int k = 0;
@@ -2227,7 +2435,13 @@ static int dib8000_fe_get_tune_settings(struct dvb_frontend *fe, struct dvb_fron
 static void dib8000_release(struct dvb_frontend *fe)
 {
 	struct dib8000_state *st = fe->demodulator_priv;
+	u8 index_frontend;
+
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (st->fe[index_frontend] != NULL); index_frontend++)
+		dvb_frontend_detach(st->fe[index_frontend]);
+
 	dibx000_exit_i2c_master(&st->i2c_master);
+	kfree(st->fe[0]);
 	kfree(st);
 }
 
@@ -2242,19 +2456,19 @@ EXPORT_SYMBOL(dib8000_get_i2c_master);
 int dib8000_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 {
 	struct dib8000_state *st = fe->demodulator_priv;
-    u16 val = dib8000_read_word(st, 299) & 0xffef;
-    val |= (onoff & 0x1) << 4;
+	u16 val = dib8000_read_word(st, 299) & 0xffef;
+	val |= (onoff & 0x1) << 4;
 
-    dprintk("pid filter enabled %d", onoff);
-    return dib8000_write_word(st, 299, val);
+	dprintk("pid filter enabled %d", onoff);
+	return dib8000_write_word(st, 299, val);
 }
 EXPORT_SYMBOL(dib8000_pid_filter_ctrl);
 
 int dib8000_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
 {
 	struct dib8000_state *st = fe->demodulator_priv;
-    dprintk("Index %x, PID %d, OnOff %d", id, pid, onoff);
-    return dib8000_write_word(st, 305 + id, onoff ? (1 << 13) | pid : 0);
+	dprintk("Index %x, PID %d, OnOff %d", id, pid, onoff);
+	return dib8000_write_word(st, 305 + id, onoff ? (1 << 13) | pid : 0);
 }
 EXPORT_SYMBOL(dib8000_pid_filter);
 
@@ -2298,6 +2512,9 @@ struct dvb_frontend *dib8000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, s
 	state = kzalloc(sizeof(struct dib8000_state), GFP_KERNEL);
 	if (state == NULL)
 		return NULL;
+	fe = kzalloc(sizeof(struct dvb_frontend), GFP_KERNEL);
+	if (fe == NULL)
+		goto error;
 
 	memcpy(&state->cfg, cfg, sizeof(struct dib8000_config));
 	state->i2c.adap = i2c_adap;
@@ -2311,9 +2528,9 @@ struct dvb_frontend *dib8000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, s
 	if ((state->cfg.output_mode != OUTMODE_MPEG2_SERIAL) && (state->cfg.output_mode != OUTMODE_MPEG2_PAR_GATED_CLK))
 		state->cfg.output_mode = OUTMODE_MPEG2_FIFO;
 
-	fe = &state->fe;
+	state->fe[0] = fe;
 	fe->demodulator_priv = state;
-	memcpy(&state->fe.ops, &dib8000_ops, sizeof(struct dvb_frontend_ops));
+	memcpy(&state->fe[0]->ops, &dib8000_ops, sizeof(struct dvb_frontend_ops));
 
 	state->timf_default = cfg->pll->timf;
 
diff --git a/drivers/media/dvb/frontends/dib8000.h b/drivers/media/dvb/frontends/dib8000.h
index e0a9ded11df4..617f9eba3a09 100644
--- a/drivers/media/dvb/frontends/dib8000.h
+++ b/drivers/media/dvb/frontends/dib8000.h
@@ -50,6 +50,9 @@ extern int dib8000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_st
 extern enum frontend_tune_state dib8000_get_tune_state(struct dvb_frontend *fe);
 extern void dib8000_pwm_agc_reset(struct dvb_frontend *fe);
 extern s32 dib8000_get_adc_power(struct dvb_frontend *fe, u8 mode);
+extern int dib8000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave);
+extern int dib8000_remove_slave_frontend(struct dvb_frontend *fe);
+extern struct dvb_frontend *dib8000_get_slave_frontend(struct dvb_frontend *fe, int slave_index);
 #else
 static inline struct dvb_frontend *dib8000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib8000_config *cfg)
 {
@@ -111,6 +114,23 @@ static inline s32 dib8000_get_adc_power(struct dvb_frontend *fe, u8 mode)
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return 0;
 }
+static inline int dib8000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+int dib8000_remove_slave_frontend(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline struct dvb_frontend *dib8000_get_slave_frontend(struct dvb_frontend *fe, int slave_index)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
 #endif
 
 #endif
diff --git a/drivers/media/dvb/frontends/dib9000.c b/drivers/media/dvb/frontends/dib9000.c
new file mode 100644
index 000000000000..91518761a2da
--- /dev/null
+++ b/drivers/media/dvb/frontends/dib9000.c
@@ -0,0 +1,2351 @@
+/*
+ * Linux-DVB Driver for DiBcom's DiB9000 and demodulator-family.
+ *
+ * Copyright (C) 2005-10 DiBcom (http://www.dibcom.fr/)
+ *
+ * This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ */
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+
+#include "dvb_math.h"
+#include "dvb_frontend.h"
+
+#include "dib9000.h"
+#include "dibx000_common.h"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
+
+#define dprintk(args...) do { if (debug) { printk(KERN_DEBUG "DiB9000: "); printk(args); printk("\n"); } } while (0)
+#define MAX_NUMBER_OF_FRONTENDS 6
+
+struct i2c_device {
+	struct i2c_adapter *i2c_adap;
+	u8 i2c_addr;
+};
+
+/* lock */
+#define DIB_LOCK struct mutex
+#define DibAcquireLock(lock) do { if (mutex_lock_interruptible(lock) < 0) dprintk("could not get the lock"); } while (0)
+#define DibReleaseLock(lock) mutex_unlock(lock)
+#define DibInitLock(lock) mutex_init(lock)
+#define DibFreeLock(lock)
+
+struct dib9000_state {
+	struct i2c_device i2c;
+
+	struct dibx000_i2c_master i2c_master;
+	struct i2c_adapter tuner_adap;
+	struct i2c_adapter component_bus;
+
+	u16 revision;
+	u8 reg_offs;
+
+	enum frontend_tune_state tune_state;
+	u32 status;
+	struct dvb_frontend_parametersContext channel_status;
+
+	u8 fe_id;
+
+#define DIB9000_GPIO_DEFAULT_DIRECTIONS 0xffff
+	u16 gpio_dir;
+#define DIB9000_GPIO_DEFAULT_VALUES     0x0000
+	u16 gpio_val;
+#define DIB9000_GPIO_DEFAULT_PWM_POS    0xffff
+	u16 gpio_pwm_pos;
+
+	union {			/* common for all chips */
+		struct {
+			u8 mobile_mode:1;
+		} host;
+
+		struct {
+			struct dib9000_fe_memory_map {
+				u16 addr;
+				u16 size;
+			} fe_mm[18];
+			u8 memcmd;
+
+			DIB_LOCK mbx_if_lock;	/* to protect read/write operations */
+			DIB_LOCK mbx_lock;	/* to protect the whole mailbox handling */
+
+			DIB_LOCK mem_lock;	/* to protect the memory accesses */
+			DIB_LOCK mem_mbx_lock;	/* to protect the memory-based mailbox */
+
+#define MBX_MAX_WORDS (256 - 200 - 2)
+#define DIB9000_MSG_CACHE_SIZE 2
+			u16 message_cache[DIB9000_MSG_CACHE_SIZE][MBX_MAX_WORDS];
+			u8 fw_is_running;
+		} risc;
+	} platform;
+
+	union {			/* common for all platforms */
+		struct {
+			struct dib9000_config cfg;
+		} d9;
+	} chip;
+
+	struct dvb_frontend *fe[MAX_NUMBER_OF_FRONTENDS];
+	u16 component_bus_speed;
+};
+
+u32 fe_info[44] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0
+};
+
+enum dib9000_power_mode {
+	DIB9000_POWER_ALL = 0,
+
+	DIB9000_POWER_NO,
+	DIB9000_POWER_INTERF_ANALOG_AGC,
+	DIB9000_POWER_COR4_DINTLV_ICIRM_EQUAL_CFROD,
+	DIB9000_POWER_COR4_CRY_ESRAM_MOUT_NUD,
+	DIB9000_POWER_INTERFACE_ONLY,
+};
+
+enum dib9000_out_messages {
+	OUT_MSG_HBM_ACK,
+	OUT_MSG_HOST_BUF_FAIL,
+	OUT_MSG_REQ_VERSION,
+	OUT_MSG_BRIDGE_I2C_W,
+	OUT_MSG_BRIDGE_I2C_R,
+	OUT_MSG_BRIDGE_APB_W,
+	OUT_MSG_BRIDGE_APB_R,
+	OUT_MSG_SCAN_CHANNEL,
+	OUT_MSG_MONIT_DEMOD,
+	OUT_MSG_CONF_GPIO,
+	OUT_MSG_DEBUG_HELP,
+	OUT_MSG_SUBBAND_SEL,
+	OUT_MSG_ENABLE_TIME_SLICE,
+	OUT_MSG_FE_FW_DL,
+	OUT_MSG_FE_CHANNEL_SEARCH,
+	OUT_MSG_FE_CHANNEL_TUNE,
+	OUT_MSG_FE_SLEEP,
+	OUT_MSG_FE_SYNC,
+	OUT_MSG_CTL_MONIT,
+
+	OUT_MSG_CONF_SVC,
+	OUT_MSG_SET_HBM,
+	OUT_MSG_INIT_DEMOD,
+	OUT_MSG_ENABLE_DIVERSITY,
+	OUT_MSG_SET_OUTPUT_MODE,
+	OUT_MSG_SET_PRIORITARY_CHANNEL,
+	OUT_MSG_ACK_FRG,
+	OUT_MSG_INIT_PMU,
+};
+
+enum dib9000_in_messages {
+	IN_MSG_DATA,
+	IN_MSG_FRAME_INFO,
+	IN_MSG_CTL_MONIT,
+	IN_MSG_ACK_FREE_ITEM,
+	IN_MSG_DEBUG_BUF,
+	IN_MSG_MPE_MONITOR,
+	IN_MSG_RAWTS_MONITOR,
+	IN_MSG_END_BRIDGE_I2C_RW,
+	IN_MSG_END_BRIDGE_APB_RW,
+	IN_MSG_VERSION,
+	IN_MSG_END_OF_SCAN,
+	IN_MSG_MONIT_DEMOD,
+	IN_MSG_ERROR,
+	IN_MSG_FE_FW_DL_DONE,
+	IN_MSG_EVENT,
+	IN_MSG_ACK_CHANGE_SVC,
+	IN_MSG_HBM_PROF,
+};
+
+/* memory_access requests */
+#define FE_MM_W_CHANNEL                   0
+#define FE_MM_W_FE_INFO                   1
+#define FE_MM_RW_SYNC                     2
+
+#define FE_SYNC_CHANNEL          1
+#define FE_SYNC_W_GENERIC_MONIT	 2
+#define FE_SYNC_COMPONENT_ACCESS 3
+
+#define FE_MM_R_CHANNEL_SEARCH_STATE      3
+#define FE_MM_R_CHANNEL_UNION_CONTEXT     4
+#define FE_MM_R_FE_INFO                   5
+#define FE_MM_R_FE_MONITOR                6
+
+#define FE_MM_W_CHANNEL_HEAD              7
+#define FE_MM_W_CHANNEL_UNION             8
+#define FE_MM_W_CHANNEL_CONTEXT           9
+#define FE_MM_R_CHANNEL_UNION            10
+#define FE_MM_R_CHANNEL_CONTEXT          11
+#define FE_MM_R_CHANNEL_TUNE_STATE       12
+
+#define FE_MM_R_GENERIC_MONITORING_SIZE	 13
+#define FE_MM_W_GENERIC_MONITORING	     14
+#define FE_MM_R_GENERIC_MONITORING	     15
+
+#define FE_MM_W_COMPONENT_ACCESS         16
+#define FE_MM_RW_COMPONENT_ACCESS_BUFFER 17
+static int dib9000_risc_apb_access_read(struct dib9000_state *state, u32 address, u16 attribute, const u8 * tx, u32 txlen, u8 * b, u32 len);
+static int dib9000_risc_apb_access_write(struct dib9000_state *state, u32 address, u16 attribute, const u8 * b, u32 len);
+
+static u16 to_fw_output_mode(u16 mode)
+{
+	switch (mode) {
+	case OUTMODE_HIGH_Z:
+		return 0;
+	case OUTMODE_MPEG2_PAR_GATED_CLK:
+		return 4;
+	case OUTMODE_MPEG2_PAR_CONT_CLK:
+		return 8;
+	case OUTMODE_MPEG2_SERIAL:
+		return 16;
+	case OUTMODE_DIVERSITY:
+		return 128;
+	case OUTMODE_MPEG2_FIFO:
+		return 2;
+	case OUTMODE_ANALOG_ADC:
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+static u16 dib9000_read16_attr(struct dib9000_state *state, u16 reg, u8 * b, u32 len, u16 attribute)
+{
+	u32 chunk_size = 126;
+	u32 l;
+	int ret;
+	u8 wb[2] = { reg >> 8, reg & 0xff };
+	struct i2c_msg msg[2] = {
+		{.addr = state->i2c.i2c_addr >> 1, .flags = 0, .buf = wb, .len = 2},
+		{.addr = state->i2c.i2c_addr >> 1, .flags = I2C_M_RD, .buf = b, .len = len},
+	};
+
+	if (state->platform.risc.fw_is_running && (reg < 1024))
+		return dib9000_risc_apb_access_read(state, reg, attribute, NULL, 0, b, len);
+
+	if (attribute & DATA_BUS_ACCESS_MODE_8BIT)
+		wb[0] |= (1 << 5);
+	if (attribute & DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT)
+		wb[0] |= (1 << 4);
+
+	do {
+		l = len < chunk_size ? len : chunk_size;
+		msg[1].len = l;
+		msg[1].buf = b;
+		ret = i2c_transfer(state->i2c.i2c_adap, msg, 2) != 2 ? -EREMOTEIO : 0;
+		if (ret != 0) {
+			dprintk("i2c read error on %d", reg);
+			return -EREMOTEIO;
+		}
+
+		b += l;
+		len -= l;
+
+		if (!(attribute & DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT))
+			reg += l / 2;
+	} while ((ret == 0) && len);
+
+	return 0;
+}
+
+static u16 dib9000_i2c_read16(struct i2c_device *i2c, u16 reg)
+{
+	u8 b[2];
+	u8 wb[2] = { reg >> 8, reg & 0xff };
+	struct i2c_msg msg[2] = {
+		{.addr = i2c->i2c_addr >> 1, .flags = 0, .buf = wb, .len = 2},
+		{.addr = i2c->i2c_addr >> 1, .flags = I2C_M_RD, .buf = b, .len = 2},
+	};
+
+	if (i2c_transfer(i2c->i2c_adap, msg, 2) != 2) {
+		dprintk("read register %x error", reg);
+		return 0;
+	}
+
+	return (b[0] << 8) | b[1];
+}
+
+static inline u16 dib9000_read_word(struct dib9000_state *state, u16 reg)
+{
+	u8 b[2];
+	if (dib9000_read16_attr(state, reg, b, 2, 0) != 0)
+		return 0;
+	return (b[0] << 8 | b[1]);
+}
+
+static inline u16 dib9000_read_word_attr(struct dib9000_state *state, u16 reg, u16 attribute)
+{
+	u8 b[2];
+	if (dib9000_read16_attr(state, reg, b, 2, attribute) != 0)
+		return 0;
+	return (b[0] << 8 | b[1]);
+}
+
+#define dib9000_read16_noinc_attr(state, reg, b, len, attribute) dib9000_read16_attr(state, reg, b, len, (attribute) | DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT)
+
+static u16 dib9000_write16_attr(struct dib9000_state *state, u16 reg, const u8 * buf, u32 len, u16 attribute)
+{
+	u8 b[255];
+	u32 chunk_size = 126;
+	u32 l;
+	int ret;
+
+	struct i2c_msg msg = {
+		.addr = state->i2c.i2c_addr >> 1, .flags = 0, .buf = b, .len = len + 2
+	};
+
+	if (state->platform.risc.fw_is_running && (reg < 1024)) {
+		if (dib9000_risc_apb_access_write
+		    (state, reg, DATA_BUS_ACCESS_MODE_16BIT | DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT | attribute, buf, len) != 0)
+			return -EINVAL;
+		return 0;
+	}
+
+	b[0] = (reg >> 8) & 0xff;
+	b[1] = (reg) & 0xff;
+
+	if (attribute & DATA_BUS_ACCESS_MODE_8BIT)
+		b[0] |= (1 << 5);
+	if (attribute & DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT)
+		b[0] |= (1 << 4);
+
+	do {
+		l = len < chunk_size ? len : chunk_size;
+		msg.len = l + 2;
+		memcpy(&b[2], buf, l);
+
+		ret = i2c_transfer(state->i2c.i2c_adap, &msg, 1) != 1 ? -EREMOTEIO : 0;
+
+		buf += l;
+		len -= l;
+
+		if (!(attribute & DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT))
+			reg += l / 2;
+	} while ((ret == 0) && len);
+
+	return ret;
+}
+
+static int dib9000_i2c_write16(struct i2c_device *i2c, u16 reg, u16 val)
+{
+	u8 b[4] = { (reg >> 8) & 0xff, reg & 0xff, (val >> 8) & 0xff, val & 0xff };
+	struct i2c_msg msg = {
+		.addr = i2c->i2c_addr >> 1, .flags = 0, .buf = b, .len = 4
+	};
+
+	return i2c_transfer(i2c->i2c_adap, &msg, 1) != 1 ? -EREMOTEIO : 0;
+}
+
+static inline int dib9000_write_word(struct dib9000_state *state, u16 reg, u16 val)
+{
+	u8 b[2] = { val >> 8, val & 0xff };
+	return dib9000_write16_attr(state, reg, b, 2, 0);
+}
+
+static inline int dib9000_write_word_attr(struct dib9000_state *state, u16 reg, u16 val, u16 attribute)
+{
+	u8 b[2] = { val >> 8, val & 0xff };
+	return dib9000_write16_attr(state, reg, b, 2, attribute);
+}
+
+#define dib9000_write(state, reg, buf, len) dib9000_write16_attr(state, reg, buf, len, 0)
+#define dib9000_write16_noinc(state, reg, buf, len) dib9000_write16_attr(state, reg, buf, len, DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT)
+#define dib9000_write16_noinc_attr(state, reg, buf, len, attribute) dib9000_write16_attr(state, reg, buf, len, DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT | (attribute))
+
+#define dib9000_mbx_send(state, id, data, len) dib9000_mbx_send_attr(state, id, data, len, 0)
+#define dib9000_mbx_get_message(state, id, msg, len) dib9000_mbx_get_message_attr(state, id, msg, len, 0)
+
+#define MAC_IRQ      (1 << 1)
+#define IRQ_POL_MSK  (1 << 4)
+
+#define dib9000_risc_mem_read_chunks(state, b, len) dib9000_read16_attr(state, 1063, b, len, DATA_BUS_ACCESS_MODE_8BIT | DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT)
+#define dib9000_risc_mem_write_chunks(state, buf, len) dib9000_write16_attr(state, 1063, buf, len, DATA_BUS_ACCESS_MODE_8BIT | DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT)
+
+static void dib9000_risc_mem_setup_cmd(struct dib9000_state *state, u32 addr, u32 len, u8 reading)
+{
+	u8 b[14] = { 0 };
+
+/*      dprintk("%d memcmd: %d %d %d\n", state->fe_id, addr, addr+len, len); */
+/*      b[0] = 0 << 7; */
+	b[1] = 1;
+
+/*      b[2] = 0; */
+/*      b[3] = 0; */
+	b[4] = (u8) (addr >> 8);
+	b[5] = (u8) (addr & 0xff);
+
+/*      b[10] = 0; */
+/*      b[11] = 0; */
+	b[12] = (u8) (addr >> 8);
+	b[13] = (u8) (addr & 0xff);
+
+	addr += len;
+/*      b[6] = 0; */
+/*      b[7] = 0; */
+	b[8] = (u8) (addr >> 8);
+	b[9] = (u8) (addr & 0xff);
+
+	dib9000_write(state, 1056, b, 14);
+	if (reading)
+		dib9000_write_word(state, 1056, (1 << 15) | 1);
+	state->platform.risc.memcmd = -1;	/* if it was called directly reset it - to force a future setup-call to set it */
+}
+
+static void dib9000_risc_mem_setup(struct dib9000_state *state, u8 cmd)
+{
+	struct dib9000_fe_memory_map *m = &state->platform.risc.fe_mm[cmd & 0x7f];
+	/* decide whether we need to "refresh" the memory controller */
+	if (state->platform.risc.memcmd == cmd &&	/* same command */
+	    !(cmd & 0x80 && m->size < 67))	/* and we do not want to read something with less than 67 bytes looping - working around a bug in the memory controller */
+		return;
+	dib9000_risc_mem_setup_cmd(state, m->addr, m->size, cmd & 0x80);
+	state->platform.risc.memcmd = cmd;
+}
+
+static int dib9000_risc_mem_read(struct dib9000_state *state, u8 cmd, u8 * b, u16 len)
+{
+	if (!state->platform.risc.fw_is_running)
+		return -EIO;
+
+	DibAcquireLock(&state->platform.risc.mem_lock);
+	dib9000_risc_mem_setup(state, cmd | 0x80);
+	dib9000_risc_mem_read_chunks(state, b, len);
+	DibReleaseLock(&state->platform.risc.mem_lock);
+	return 0;
+}
+
+static int dib9000_risc_mem_write(struct dib9000_state *state, u8 cmd, const u8 * b)
+{
+	struct dib9000_fe_memory_map *m = &state->platform.risc.fe_mm[cmd];
+	if (!state->platform.risc.fw_is_running)
+		return -EIO;
+
+	DibAcquireLock(&state->platform.risc.mem_lock);
+	dib9000_risc_mem_setup(state, cmd);
+	dib9000_risc_mem_write_chunks(state, b, m->size);
+	DibReleaseLock(&state->platform.risc.mem_lock);
+	return 0;
+}
+
+static int dib9000_firmware_download(struct dib9000_state *state, u8 risc_id, u16 key, const u8 * code, u32 len)
+{
+	u16 offs;
+
+	if (risc_id == 1)
+		offs = 16;
+	else
+		offs = 0;
+
+	/* config crtl reg */
+	dib9000_write_word(state, 1024 + offs, 0x000f);
+	dib9000_write_word(state, 1025 + offs, 0);
+	dib9000_write_word(state, 1031 + offs, key);
+
+	dprintk("going to download %dB of microcode", len);
+	if (dib9000_write16_noinc(state, 1026 + offs, (u8 *) code, (u16) len) != 0) {
+		dprintk("error while downloading microcode for RISC %c", 'A' + risc_id);
+		return -EIO;
+	}
+
+	dprintk("Microcode for RISC %c loaded", 'A' + risc_id);
+
+	return 0;
+}
+
+static int dib9000_mbx_host_init(struct dib9000_state *state, u8 risc_id)
+{
+	u16 mbox_offs;
+	u16 reset_reg;
+	u16 tries = 1000;
+
+	if (risc_id == 1)
+		mbox_offs = 16;
+	else
+		mbox_offs = 0;
+
+	/* Reset mailbox  */
+	dib9000_write_word(state, 1027 + mbox_offs, 0x8000);
+
+	/* Read reset status */
+	do {
+		reset_reg = dib9000_read_word(state, 1027 + mbox_offs);
+		msleep(100);
+	} while ((reset_reg & 0x8000) && --tries);
+
+	if (reset_reg & 0x8000) {
+		dprintk("MBX: init ERROR, no response from RISC %c", 'A' + risc_id);
+		return -EIO;
+	}
+	dprintk("MBX: initialized");
+	return 0;
+}
+
+#define MAX_MAILBOX_TRY 100
+static int dib9000_mbx_send_attr(struct dib9000_state *state, u8 id, u16 * data, u8 len, u16 attr)
+{
+	u8 *d, b[2];
+	u16 tmp;
+	u16 size;
+	u32 i;
+	int ret = 0;
+
+	if (!state->platform.risc.fw_is_running)
+		return -EINVAL;
+
+	DibAcquireLock(&state->platform.risc.mbx_if_lock);
+	tmp = MAX_MAILBOX_TRY;
+	do {
+		size = dib9000_read_word_attr(state, 1043, attr) & 0xff;
+		if ((size + len + 1) > MBX_MAX_WORDS && --tmp) {
+			dprintk("MBX: RISC mbx full, retrying");
+			msleep(100);
+		} else
+			break;
+	} while (1);
+
+	/*dprintk( "MBX: size: %d", size); */
+
+	if (tmp == 0) {
+		ret = -EINVAL;
+		goto out;
+	}
+#ifdef DUMP_MSG
+	dprintk("--> %02x %d ", id, len + 1);
+	for (i = 0; i < len; i++)
+		dprintk("%04x ", data[i]);
+	dprintk("\n");
+#endif
+
+	/* byte-order conversion - works on big (where it is not necessary) or little endian */
+	d = (u8 *) data;
+	for (i = 0; i < len; i++) {
+		tmp = data[i];
+		*d++ = tmp >> 8;
+		*d++ = tmp & 0xff;
+	}
+
+	/* write msg */
+	b[0] = id;
+	b[1] = len + 1;
+	if (dib9000_write16_noinc_attr(state, 1045, b, 2, attr) != 0 || dib9000_write16_noinc_attr(state, 1045, (u8 *) data, len * 2, attr) != 0) {
+		ret = -EIO;
+		goto out;
+	}
+
+	/* update register nb_mes_in_RX */
+	ret = (u8) dib9000_write_word_attr(state, 1043, 1 << 14, attr);
+
+out:
+	DibReleaseLock(&state->platform.risc.mbx_if_lock);
+
+	return ret;
+}
+
+static u8 dib9000_mbx_read(struct dib9000_state *state, u16 * data, u8 risc_id, u16 attr)
+{
+#ifdef DUMP_MSG
+	u16 *d = data;
+#endif
+
+	u16 tmp, i;
+	u8 size;
+	u8 mc_base;
+
+	if (!state->platform.risc.fw_is_running)
+		return 0;
+
+	DibAcquireLock(&state->platform.risc.mbx_if_lock);
+	if (risc_id == 1)
+		mc_base = 16;
+	else
+		mc_base = 0;
+
+	/* Length and type in the first word */
+	*data = dib9000_read_word_attr(state, 1029 + mc_base, attr);
+
+	size = *data & 0xff;
+	if (size <= MBX_MAX_WORDS) {
+		data++;
+		size--;		/* Initial word already read */
+
+		dib9000_read16_noinc_attr(state, 1029 + mc_base, (u8 *) data, size * 2, attr);
+
+		/* to word conversion */
+		for (i = 0; i < size; i++) {
+			tmp = *data;
+			*data = (tmp >> 8) | (tmp << 8);
+			data++;
+		}
+
+#ifdef DUMP_MSG
+		dprintk("<-- ");
+		for (i = 0; i < size + 1; i++)
+			dprintk("%04x ", d[i]);
+		dprintk("\n");
+#endif
+	} else {
+		dprintk("MBX: message is too big for message cache (%d), flushing message", size);
+		size--;		/* Initial word already read */
+		while (size--)
+			dib9000_read16_noinc_attr(state, 1029 + mc_base, (u8 *) data, 2, attr);
+	}
+	/* Update register nb_mes_in_TX */
+	dib9000_write_word_attr(state, 1028 + mc_base, 1 << 14, attr);
+
+	DibReleaseLock(&state->platform.risc.mbx_if_lock);
+
+	return size + 1;
+}
+
+static int dib9000_risc_debug_buf(struct dib9000_state *state, u16 * data, u8 size)
+{
+	u32 ts = data[1] << 16 | data[0];
+	char *b = (char *)&data[2];
+
+	b[2 * (size - 2) - 1] = '\0';	/* Bullet proof the buffer */
+	if (*b == '~') {
+		b++;
+		dprintk(b);
+	} else
+		dprintk("RISC%d: %d.%04d %s", state->fe_id, ts / 10000, ts % 10000, *b ? b : "<emtpy>");
+	return 1;
+}
+
+static int dib9000_mbx_fetch_to_cache(struct dib9000_state *state, u16 attr)
+{
+	int i;
+	u8 size;
+	u16 *block;
+	/* find a free slot */
+	for (i = 0; i < DIB9000_MSG_CACHE_SIZE; i++) {
+		block = state->platform.risc.message_cache[i];
+		if (*block == 0) {
+			size = dib9000_mbx_read(state, block, 1, attr);
+
+/*                      dprintk( "MBX: fetched %04x message to cache", *block); */
+
+			switch (*block >> 8) {
+			case IN_MSG_DEBUG_BUF:
+				dib9000_risc_debug_buf(state, block + 1, size);	/* debug-messages are going to be printed right away */
+				*block = 0;	/* free the block */
+				break;
+#if 0
+			case IN_MSG_DATA:	/* FE-TRACE */
+				dib9000_risc_data_process(state, block + 1, size);
+				*block = 0;
+				break;
+#endif
+			default:
+				break;
+			}
+
+			return 1;
+		}
+	}
+	dprintk("MBX: no free cache-slot found for new message...");
+	return -1;
+}
+
+static u8 dib9000_mbx_count(struct dib9000_state *state, u8 risc_id, u16 attr)
+{
+	if (risc_id == 0)
+		return (u8) (dib9000_read_word_attr(state, 1028, attr) >> 10) & 0x1f;	/* 5 bit field */
+	else
+		return (u8) (dib9000_read_word_attr(state, 1044, attr) >> 8) & 0x7f;	/* 7 bit field */
+}
+
+static int dib9000_mbx_process(struct dib9000_state *state, u16 attr)
+{
+	int ret = 0;
+	u16 tmp;
+
+	if (!state->platform.risc.fw_is_running)
+		return -1;
+
+	DibAcquireLock(&state->platform.risc.mbx_lock);
+
+	if (dib9000_mbx_count(state, 1, attr))	/* 1=RiscB */
+		ret = dib9000_mbx_fetch_to_cache(state, attr);
+
+	tmp = dib9000_read_word_attr(state, 1229, attr);	/* Clear the IRQ */
+/*      if (tmp) */
+/*              dprintk( "cleared IRQ: %x", tmp); */
+	DibReleaseLock(&state->platform.risc.mbx_lock);
+
+	return ret;
+}
+
+static int dib9000_mbx_get_message_attr(struct dib9000_state *state, u16 id, u16 * msg, u8 * size, u16 attr)
+{
+	u8 i;
+	u16 *block;
+	u16 timeout = 30;
+
+	*msg = 0;
+	do {
+		/* dib9000_mbx_get_from_cache(); */
+		for (i = 0; i < DIB9000_MSG_CACHE_SIZE; i++) {
+			block = state->platform.risc.message_cache[i];
+			if ((*block >> 8) == id) {
+				*size = (*block & 0xff) - 1;
+				memcpy(msg, block + 1, (*size) * 2);
+				*block = 0;	/* free the block */
+				i = 0;	/* signal that we found a message */
+				break;
+			}
+		}
+
+		if (i == 0)
+			break;
+
+		if (dib9000_mbx_process(state, attr) == -1)	/* try to fetch one message - if any */
+			return -1;
+
+	} while (--timeout);
+
+	if (timeout == 0) {
+		dprintk("waiting for message %d timed out", id);
+		return -1;
+	}
+
+	return i == 0;
+}
+
+static int dib9000_risc_check_version(struct dib9000_state *state)
+{
+	u8 r[4];
+	u8 size;
+	u16 fw_version = 0;
+
+	if (dib9000_mbx_send(state, OUT_MSG_REQ_VERSION, &fw_version, 1) != 0)
+		return -EIO;
+
+	if (dib9000_mbx_get_message(state, IN_MSG_VERSION, (u16 *) r, &size) < 0)
+		return -EIO;
+
+	fw_version = (r[0] << 8) | r[1];
+	dprintk("RISC: ver: %d.%02d (IC: %d)", fw_version >> 10, fw_version & 0x3ff, (r[2] << 8) | r[3]);
+
+	if ((fw_version >> 10) != 7)
+		return -EINVAL;
+
+	switch (fw_version & 0x3ff) {
+	case 11:
+	case 12:
+	case 14:
+	case 15:
+	case 16:
+	case 17:
+		break;
+	default:
+		dprintk("RISC: invalid firmware version");
+		return -EINVAL;
+	}
+
+	dprintk("RISC: valid firmware version");
+	return 0;
+}
+
+static int dib9000_fw_boot(struct dib9000_state *state, const u8 * codeA, u32 lenA, const u8 * codeB, u32 lenB)
+{
+	/* Reconfig pool mac ram */
+	dib9000_write_word(state, 1225, 0x02);	/* A: 8k C, 4 k D - B: 32k C 6 k D - IRAM 96k */
+	dib9000_write_word(state, 1226, 0x05);
+
+	/* Toggles IP crypto to Host APB interface. */
+	dib9000_write_word(state, 1542, 1);
+
+	/* Set jump and no jump in the dma box */
+	dib9000_write_word(state, 1074, 0);
+	dib9000_write_word(state, 1075, 0);
+
+	/* Set MAC as APB Master. */
+	dib9000_write_word(state, 1237, 0);
+
+	/* Reset the RISCs */
+	if (codeA != NULL)
+		dib9000_write_word(state, 1024, 2);
+	else
+		dib9000_write_word(state, 1024, 15);
+	if (codeB != NULL)
+		dib9000_write_word(state, 1040, 2);
+
+	if (codeA != NULL)
+		dib9000_firmware_download(state, 0, 0x1234, codeA, lenA);
+	if (codeB != NULL)
+		dib9000_firmware_download(state, 1, 0x1234, codeB, lenB);
+
+	/* Run the RISCs */
+	if (codeA != NULL)
+		dib9000_write_word(state, 1024, 0);
+	if (codeB != NULL)
+		dib9000_write_word(state, 1040, 0);
+
+	if (codeA != NULL)
+		if (dib9000_mbx_host_init(state, 0) != 0)
+			return -EIO;
+	if (codeB != NULL)
+		if (dib9000_mbx_host_init(state, 1) != 0)
+			return -EIO;
+
+	msleep(100);
+	state->platform.risc.fw_is_running = 1;
+
+	if (dib9000_risc_check_version(state) != 0)
+		return -EINVAL;
+
+	state->platform.risc.memcmd = 0xff;
+	return 0;
+}
+
+static u16 dib9000_identify(struct i2c_device *client)
+{
+	u16 value;
+
+	value = dib9000_i2c_read16(client, 896);
+	if (value != 0x01b3) {
+		dprintk("wrong Vendor ID (0x%x)", value);
+		return 0;
+	}
+
+	value = dib9000_i2c_read16(client, 897);
+	if (value != 0x4000 && value != 0x4001 && value != 0x4002 && value != 0x4003 && value != 0x4004 && value != 0x4005) {
+		dprintk("wrong Device ID (0x%x)", value);
+		return 0;
+	}
+
+	/* protect this driver to be used with 7000PC */
+	if (value == 0x4000 && dib9000_i2c_read16(client, 769) == 0x4000) {
+		dprintk("this driver does not work with DiB7000PC");
+		return 0;
+	}
+
+	switch (value) {
+	case 0x4000:
+		dprintk("found DiB7000MA/PA/MB/PB");
+		break;
+	case 0x4001:
+		dprintk("found DiB7000HC");
+		break;
+	case 0x4002:
+		dprintk("found DiB7000MC");
+		break;
+	case 0x4003:
+		dprintk("found DiB9000A");
+		break;
+	case 0x4004:
+		dprintk("found DiB9000H");
+		break;
+	case 0x4005:
+		dprintk("found DiB9000M");
+		break;
+	}
+
+	return value;
+}
+
+static void dib9000_set_power_mode(struct dib9000_state *state, enum dib9000_power_mode mode)
+{
+	/* by default everything is going to be powered off */
+	u16 reg_903 = 0x3fff, reg_904 = 0xffff, reg_905 = 0xffff, reg_906;
+	u8 offset;
+
+	if (state->revision == 0x4003 || state->revision == 0x4004 || state->revision == 0x4005)
+		offset = 1;
+	else
+		offset = 0;
+
+	reg_906 = dib9000_read_word(state, 906 + offset) | 0x3;	/* keep settings for RISC */
+
+	/* now, depending on the requested mode, we power on */
+	switch (mode) {
+		/* power up everything in the demod */
+	case DIB9000_POWER_ALL:
+		reg_903 = 0x0000;
+		reg_904 = 0x0000;
+		reg_905 = 0x0000;
+		reg_906 = 0x0000;
+		break;
+
+		/* just leave power on the control-interfaces: GPIO and (I2C or SDIO or SRAM) */
+	case DIB9000_POWER_INTERFACE_ONLY:	/* TODO power up either SDIO or I2C or SRAM */
+		reg_905 &= ~((1 << 7) | (1 << 6) | (1 << 5) | (1 << 2));
+		break;
+
+	case DIB9000_POWER_INTERF_ANALOG_AGC:
+		reg_903 &= ~((1 << 15) | (1 << 14) | (1 << 11) | (1 << 10));
+		reg_905 &= ~((1 << 7) | (1 << 6) | (1 << 5) | (1 << 4) | (1 << 2));
+		reg_906 &= ~((1 << 0));
+		break;
+
+	case DIB9000_POWER_COR4_DINTLV_ICIRM_EQUAL_CFROD:
+		reg_903 = 0x0000;
+		reg_904 = 0x801f;
+		reg_905 = 0x0000;
+		reg_906 &= ~((1 << 0));
+		break;
+
+	case DIB9000_POWER_COR4_CRY_ESRAM_MOUT_NUD:
+		reg_903 = 0x0000;
+		reg_904 = 0x8000;
+		reg_905 = 0x010b;
+		reg_906 &= ~((1 << 0));
+		break;
+	default:
+	case DIB9000_POWER_NO:
+		break;
+	}
+
+	/* always power down unused parts */
+	if (!state->platform.host.mobile_mode)
+		reg_904 |= (1 << 7) | (1 << 6) | (1 << 4) | (1 << 2) | (1 << 1);
+
+	/* P_sdio_select_clk = 0 on MC and after */
+	if (state->revision != 0x4000)
+		reg_906 <<= 1;
+
+	dib9000_write_word(state, 903 + offset, reg_903);
+	dib9000_write_word(state, 904 + offset, reg_904);
+	dib9000_write_word(state, 905 + offset, reg_905);
+	dib9000_write_word(state, 906 + offset, reg_906);
+}
+
+static int dib9000_fw_reset(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+
+	dib9000_write_word(state, 1817, 0x0003);
+
+	dib9000_write_word(state, 1227, 1);
+	dib9000_write_word(state, 1227, 0);
+
+	switch ((state->revision = dib9000_identify(&state->i2c))) {
+	case 0x4003:
+	case 0x4004:
+	case 0x4005:
+		state->reg_offs = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* reset the i2c-master to use the host interface */
+	dibx000_reset_i2c_master(&state->i2c_master);
+
+	dib9000_set_power_mode(state, DIB9000_POWER_ALL);
+
+	/* unforce divstr regardless whether i2c enumeration was done or not */
+	dib9000_write_word(state, 1794, dib9000_read_word(state, 1794) & ~(1 << 1));
+	dib9000_write_word(state, 1796, 0);
+	dib9000_write_word(state, 1805, 0x805);
+
+	/* restart all parts */
+	dib9000_write_word(state, 898, 0xffff);
+	dib9000_write_word(state, 899, 0xffff);
+	dib9000_write_word(state, 900, 0x0001);
+	dib9000_write_word(state, 901, 0xff19);
+	dib9000_write_word(state, 902, 0x003c);
+
+	dib9000_write_word(state, 898, 0);
+	dib9000_write_word(state, 899, 0);
+	dib9000_write_word(state, 900, 0);
+	dib9000_write_word(state, 901, 0);
+	dib9000_write_word(state, 902, 0);
+
+	dib9000_write_word(state, 911, state->chip.d9.cfg.if_drives);
+
+	dib9000_set_power_mode(state, DIB9000_POWER_INTERFACE_ONLY);
+
+	return 0;
+}
+
+static int dib9000_risc_apb_access_read(struct dib9000_state *state, u32 address, u16 attribute, const u8 * tx, u32 txlen, u8 * b, u32 len)
+{
+	u16 mb[10];
+	u8 i, s;
+
+	if (address >= 1024 || !state->platform.risc.fw_is_running)
+		return -EINVAL;
+
+	/* dprintk( "APB access thru rd fw %d %x", address, attribute); */
+
+	mb[0] = (u16) address;
+	mb[1] = len / 2;
+	dib9000_mbx_send_attr(state, OUT_MSG_BRIDGE_APB_R, mb, 2, attribute);
+	switch (dib9000_mbx_get_message_attr(state, IN_MSG_END_BRIDGE_APB_RW, mb, &s, attribute)) {
+	case 1:
+		s--;
+		for (i = 0; i < s; i++) {
+			b[i * 2] = (mb[i + 1] >> 8) & 0xff;
+			b[i * 2 + 1] = (mb[i + 1]) & 0xff;
+		}
+		return 0;
+	default:
+		return -EIO;
+	}
+	return -EIO;
+}
+
+static int dib9000_risc_apb_access_write(struct dib9000_state *state, u32 address, u16 attribute, const u8 * b, u32 len)
+{
+	u16 mb[10];
+	u8 s, i;
+
+	if (address >= 1024 || !state->platform.risc.fw_is_running)
+		return -EINVAL;
+
+	/* dprintk( "APB access thru wr fw %d %x", address, attribute); */
+
+	mb[0] = (unsigned short)address;
+	for (i = 0; i < len && i < 20; i += 2)
+		mb[1 + (i / 2)] = (b[i] << 8 | b[i + 1]);
+
+	dib9000_mbx_send_attr(state, OUT_MSG_BRIDGE_APB_W, mb, 1 + len / 2, attribute);
+	return dib9000_mbx_get_message_attr(state, IN_MSG_END_BRIDGE_APB_RW, mb, &s, attribute) == 1 ? 0 : -EINVAL;
+}
+
+static int dib9000_fw_memmbx_sync(struct dib9000_state *state, u8 i)
+{
+	u8 index_loop = 10;
+
+	if (!state->platform.risc.fw_is_running)
+		return 0;
+	dib9000_risc_mem_write(state, FE_MM_RW_SYNC, &i);
+	do {
+		dib9000_risc_mem_read(state, FE_MM_RW_SYNC, &i, 1);
+	} while (i && index_loop--);
+
+	if (index_loop > 0)
+		return 0;
+	return -EIO;
+}
+
+static int dib9000_fw_init(struct dib9000_state *state)
+{
+	struct dibGPIOFunction *f;
+	u16 b[40] = { 0 };
+	u8 i;
+	u8 size;
+
+	if (dib9000_fw_boot(state, NULL, 0, state->chip.d9.cfg.microcode_B_fe_buffer, state->chip.d9.cfg.microcode_B_fe_size) != 0)
+		return -EIO;
+
+	/* initialize the firmware */
+	for (i = 0; i < ARRAY_SIZE(state->chip.d9.cfg.gpio_function); i++) {
+		f = &state->chip.d9.cfg.gpio_function[i];
+		if (f->mask) {
+			switch (f->function) {
+			case BOARD_GPIO_FUNCTION_COMPONENT_ON:
+				b[0] = (u16) f->mask;
+				b[1] = (u16) f->direction;
+				b[2] = (u16) f->value;
+				break;
+			case BOARD_GPIO_FUNCTION_COMPONENT_OFF:
+				b[3] = (u16) f->mask;
+				b[4] = (u16) f->direction;
+				b[5] = (u16) f->value;
+				break;
+			}
+		}
+	}
+	if (dib9000_mbx_send(state, OUT_MSG_CONF_GPIO, b, 15) != 0)
+		return -EIO;
+
+	/* subband */
+	b[0] = state->chip.d9.cfg.subband.size;	/* type == 0 -> GPIO - PWM not yet supported */
+	for (i = 0; i < state->chip.d9.cfg.subband.size; i++) {
+		b[1 + i * 4] = state->chip.d9.cfg.subband.subband[i].f_mhz;
+		b[2 + i * 4] = (u16) state->chip.d9.cfg.subband.subband[i].gpio.mask;
+		b[3 + i * 4] = (u16) state->chip.d9.cfg.subband.subband[i].gpio.direction;
+		b[4 + i * 4] = (u16) state->chip.d9.cfg.subband.subband[i].gpio.value;
+	}
+	b[1 + i * 4] = 0;	/* fe_id */
+	if (dib9000_mbx_send(state, OUT_MSG_SUBBAND_SEL, b, 2 + 4 * i) != 0)
+		return -EIO;
+
+	/* 0 - id, 1 - no_of_frontends */
+	b[0] = (0 << 8) | 1;
+	/* 0 = i2c-address demod, 0 = tuner */
+	b[1] = (0 << 8) | (0);
+	b[2] = (u16) (((state->chip.d9.cfg.xtal_clock_khz * 1000) >> 16) & 0xffff);
+	b[3] = (u16) (((state->chip.d9.cfg.xtal_clock_khz * 1000)) & 0xffff);
+	b[4] = (u16) ((state->chip.d9.cfg.vcxo_timer >> 16) & 0xffff);
+	b[5] = (u16) ((state->chip.d9.cfg.vcxo_timer) & 0xffff);
+	b[6] = (u16) ((state->chip.d9.cfg.timing_frequency >> 16) & 0xffff);
+	b[7] = (u16) ((state->chip.d9.cfg.timing_frequency) & 0xffff);
+	b[29] = state->chip.d9.cfg.if_drives;
+	if (dib9000_mbx_send(state, OUT_MSG_INIT_DEMOD, b, ARRAY_SIZE(b)) != 0)
+		return -EIO;
+
+	if (dib9000_mbx_send(state, OUT_MSG_FE_FW_DL, NULL, 0) != 0)
+		return -EIO;
+
+	if (dib9000_mbx_get_message(state, IN_MSG_FE_FW_DL_DONE, b, &size) < 0)
+		return -EIO;
+
+	if (size > ARRAY_SIZE(b)) {
+		dprintk("error : firmware returned %dbytes needed but the used buffer has only %dbytes\n Firmware init ABORTED", size,
+			(int)ARRAY_SIZE(b));
+		return -EINVAL;
+	}
+
+	for (i = 0; i < size; i += 2) {
+		state->platform.risc.fe_mm[i / 2].addr = b[i + 0];
+		state->platform.risc.fe_mm[i / 2].size = b[i + 1];
+	}
+
+	return 0;
+}
+
+static void dib9000_fw_set_channel_head(struct dib9000_state *state, struct dvb_frontend_parameters *ch)
+{
+	u8 b[9];
+	u32 freq = state->fe[0]->dtv_property_cache.frequency / 1000;
+	if (state->fe_id % 2)
+		freq += 101;
+
+	b[0] = (u8) ((freq >> 0) & 0xff);
+	b[1] = (u8) ((freq >> 8) & 0xff);
+	b[2] = (u8) ((freq >> 16) & 0xff);
+	b[3] = (u8) ((freq >> 24) & 0xff);
+	b[4] = (u8) ((state->fe[0]->dtv_property_cache.bandwidth_hz / 1000 >> 0) & 0xff);
+	b[5] = (u8) ((state->fe[0]->dtv_property_cache.bandwidth_hz / 1000 >> 8) & 0xff);
+	b[6] = (u8) ((state->fe[0]->dtv_property_cache.bandwidth_hz / 1000 >> 16) & 0xff);
+	b[7] = (u8) ((state->fe[0]->dtv_property_cache.bandwidth_hz / 1000 >> 24) & 0xff);
+	b[8] = 0x80;		/* do not wait for CELL ID when doing autosearch */
+	if (state->fe[0]->dtv_property_cache.delivery_system == SYS_DVBT)
+		b[8] |= 1;
+	dib9000_risc_mem_write(state, FE_MM_W_CHANNEL_HEAD, b);
+}
+
+static int dib9000_fw_get_channel(struct dvb_frontend *fe, struct dvb_frontend_parameters *channel)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	struct dibDVBTChannel {
+		s8 spectrum_inversion;
+
+		s8 nfft;
+		s8 guard;
+		s8 constellation;
+
+		s8 hrch;
+		s8 alpha;
+		s8 code_rate_hp;
+		s8 code_rate_lp;
+		s8 select_hp;
+
+		s8 intlv_native;
+	};
+	struct dibDVBTChannel ch;
+	int ret = 0;
+
+	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
+		goto error;
+		ret = -EIO;
+	}
+
+	dib9000_risc_mem_read(state, FE_MM_R_CHANNEL_UNION, (u8 *) &ch, sizeof(struct dibDVBTChannel));
+
+	switch (ch.spectrum_inversion & 0x7) {
+	case 1:
+		state->fe[0]->dtv_property_cache.inversion = INVERSION_ON;
+		break;
+	case 0:
+		state->fe[0]->dtv_property_cache.inversion = INVERSION_OFF;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.inversion = INVERSION_AUTO;
+		break;
+	}
+	switch (ch.nfft) {
+	case 0:
+		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case 2:
+		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_4K;
+		break;
+	case 1:
+		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_8K;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.transmission_mode = TRANSMISSION_MODE_AUTO;
+		break;
+	}
+	switch (ch.guard) {
+	case 0:
+		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case 1:
+		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case 2:
+		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case 3:
+		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_1_4;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.guard_interval = GUARD_INTERVAL_AUTO;
+		break;
+	}
+	switch (ch.constellation) {
+	case 2:
+		state->fe[0]->dtv_property_cache.modulation = QAM_64;
+		break;
+	case 1:
+		state->fe[0]->dtv_property_cache.modulation = QAM_16;
+		break;
+	case 0:
+		state->fe[0]->dtv_property_cache.modulation = QPSK;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.modulation = QAM_AUTO;
+		break;
+	}
+	switch (ch.hrch) {
+	case 0:
+		state->fe[0]->dtv_property_cache.hierarchy = HIERARCHY_NONE;
+		break;
+	case 1:
+		state->fe[0]->dtv_property_cache.hierarchy = HIERARCHY_1;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.hierarchy = HIERARCHY_AUTO;
+		break;
+	}
+	switch (ch.code_rate_hp) {
+	case 1:
+		state->fe[0]->dtv_property_cache.code_rate_HP = FEC_1_2;
+		break;
+	case 2:
+		state->fe[0]->dtv_property_cache.code_rate_HP = FEC_2_3;
+		break;
+	case 3:
+		state->fe[0]->dtv_property_cache.code_rate_HP = FEC_3_4;
+		break;
+	case 5:
+		state->fe[0]->dtv_property_cache.code_rate_HP = FEC_5_6;
+		break;
+	case 7:
+		state->fe[0]->dtv_property_cache.code_rate_HP = FEC_7_8;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.code_rate_HP = FEC_AUTO;
+		break;
+	}
+	switch (ch.code_rate_lp) {
+	case 1:
+		state->fe[0]->dtv_property_cache.code_rate_LP = FEC_1_2;
+		break;
+	case 2:
+		state->fe[0]->dtv_property_cache.code_rate_LP = FEC_2_3;
+		break;
+	case 3:
+		state->fe[0]->dtv_property_cache.code_rate_LP = FEC_3_4;
+		break;
+	case 5:
+		state->fe[0]->dtv_property_cache.code_rate_LP = FEC_5_6;
+		break;
+	case 7:
+		state->fe[0]->dtv_property_cache.code_rate_LP = FEC_7_8;
+		break;
+	default:
+	case -1:
+		state->fe[0]->dtv_property_cache.code_rate_LP = FEC_AUTO;
+		break;
+	}
+
+error:
+	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+	return ret;
+}
+
+static int dib9000_fw_set_channel_union(struct dvb_frontend *fe, struct dvb_frontend_parameters *channel)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	struct dibDVBTChannel {
+		s8 spectrum_inversion;
+
+		s8 nfft;
+		s8 guard;
+		s8 constellation;
+
+		s8 hrch;
+		s8 alpha;
+		s8 code_rate_hp;
+		s8 code_rate_lp;
+		s8 select_hp;
+
+		s8 intlv_native;
+	};
+	struct dibDVBTChannel ch;
+
+	switch (state->fe[0]->dtv_property_cache.inversion) {
+	case INVERSION_ON:
+		ch.spectrum_inversion = 1;
+		break;
+	case INVERSION_OFF:
+		ch.spectrum_inversion = 0;
+		break;
+	default:
+	case INVERSION_AUTO:
+		ch.spectrum_inversion = -1;
+		break;
+	}
+	switch (state->fe[0]->dtv_property_cache.transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+		ch.nfft = 0;
+		break;
+	case TRANSMISSION_MODE_4K:
+		ch.nfft = 2;
+		break;
+	case TRANSMISSION_MODE_8K:
+		ch.nfft = 1;
+		break;
+	default:
+	case TRANSMISSION_MODE_AUTO:
+		ch.nfft = 1;
+		break;
+	}
+	switch (state->fe[0]->dtv_property_cache.guard_interval) {
+	case GUARD_INTERVAL_1_32:
+		ch.guard = 0;
+		break;
+	case GUARD_INTERVAL_1_16:
+		ch.guard = 1;
+		break;
+	case GUARD_INTERVAL_1_8:
+		ch.guard = 2;
+		break;
+	case GUARD_INTERVAL_1_4:
+		ch.guard = 3;
+		break;
+	default:
+	case GUARD_INTERVAL_AUTO:
+		ch.guard = -1;
+		break;
+	}
+	switch (state->fe[0]->dtv_property_cache.modulation) {
+	case QAM_64:
+		ch.constellation = 2;
+		break;
+	case QAM_16:
+		ch.constellation = 1;
+		break;
+	case QPSK:
+		ch.constellation = 0;
+		break;
+	default:
+	case QAM_AUTO:
+		ch.constellation = -1;
+		break;
+	}
+	switch (state->fe[0]->dtv_property_cache.hierarchy) {
+	case HIERARCHY_NONE:
+		ch.hrch = 0;
+		break;
+	case HIERARCHY_1:
+	case HIERARCHY_2:
+	case HIERARCHY_4:
+		ch.hrch = 1;
+		break;
+	default:
+	case HIERARCHY_AUTO:
+		ch.hrch = -1;
+		break;
+	}
+	ch.alpha = 1;
+	switch (state->fe[0]->dtv_property_cache.code_rate_HP) {
+	case FEC_1_2:
+		ch.code_rate_hp = 1;
+		break;
+	case FEC_2_3:
+		ch.code_rate_hp = 2;
+		break;
+	case FEC_3_4:
+		ch.code_rate_hp = 3;
+		break;
+	case FEC_5_6:
+		ch.code_rate_hp = 5;
+		break;
+	case FEC_7_8:
+		ch.code_rate_hp = 7;
+		break;
+	default:
+	case FEC_AUTO:
+		ch.code_rate_hp = -1;
+		break;
+	}
+	switch (state->fe[0]->dtv_property_cache.code_rate_LP) {
+	case FEC_1_2:
+		ch.code_rate_lp = 1;
+		break;
+	case FEC_2_3:
+		ch.code_rate_lp = 2;
+		break;
+	case FEC_3_4:
+		ch.code_rate_lp = 3;
+		break;
+	case FEC_5_6:
+		ch.code_rate_lp = 5;
+		break;
+	case FEC_7_8:
+		ch.code_rate_lp = 7;
+		break;
+	default:
+	case FEC_AUTO:
+		ch.code_rate_lp = -1;
+		break;
+	}
+	ch.select_hp = 1;
+	ch.intlv_native = 1;
+
+	dib9000_risc_mem_write(state, FE_MM_W_CHANNEL_UNION, (u8 *) &ch);
+
+	return 0;
+}
+
+static int dib9000_fw_tune(struct dvb_frontend *fe, struct dvb_frontend_parameters *ch)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	int ret = 10, search = state->channel_status.status == CHANNEL_STATUS_PARAMETERS_UNKNOWN;
+	s8 i;
+
+	switch (state->tune_state) {
+	case CT_DEMOD_START:
+		dib9000_fw_set_channel_head(state, ch);
+
+		/* write the channel context - a channel is initialized to 0, so it is OK */
+		dib9000_risc_mem_write(state, FE_MM_W_CHANNEL_CONTEXT, (u8 *) fe_info);
+		dib9000_risc_mem_write(state, FE_MM_W_FE_INFO, (u8 *) fe_info);
+
+		if (search)
+			dib9000_mbx_send(state, OUT_MSG_FE_CHANNEL_SEARCH, NULL, 0);
+		else {
+			dib9000_fw_set_channel_union(fe, ch);
+			dib9000_mbx_send(state, OUT_MSG_FE_CHANNEL_TUNE, NULL, 0);
+		}
+		state->tune_state = CT_DEMOD_STEP_1;
+		break;
+	case CT_DEMOD_STEP_1:
+		if (search)
+			dib9000_risc_mem_read(state, FE_MM_R_CHANNEL_SEARCH_STATE, (u8 *) &i, 1);
+		else
+			dib9000_risc_mem_read(state, FE_MM_R_CHANNEL_TUNE_STATE, (u8 *) &i, 1);
+		switch (i) {	/* something happened */
+		case 0:
+			break;
+		case -2:	/* tps locks are "slower" than MPEG locks -> even in autosearch data is OK here */
+			if (search)
+				state->status = FE_STATUS_DEMOD_SUCCESS;
+			else {
+				state->tune_state = CT_DEMOD_STOP;
+				state->status = FE_STATUS_LOCKED;
+			}
+			break;
+		default:
+			state->status = FE_STATUS_TUNE_FAILED;
+			state->tune_state = CT_DEMOD_STOP;
+			break;
+		}
+		break;
+	default:
+		ret = FE_CALLBACK_TIME_NEVER;
+		break;
+	}
+
+	return ret;
+}
+
+static int dib9000_fw_set_diversity_in(struct dvb_frontend *fe, int onoff)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u16 mode = (u16) onoff;
+	return dib9000_mbx_send(state, OUT_MSG_ENABLE_DIVERSITY, &mode, 1);
+}
+
+static int dib9000_fw_set_output_mode(struct dvb_frontend *fe, int mode)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u16 outreg, smo_mode;
+
+	dprintk("setting output mode for demod %p to %d", fe, mode);
+
+	switch (mode) {
+	case OUTMODE_MPEG2_PAR_GATED_CLK:
+		outreg = (1 << 10);	/* 0x0400 */
+		break;
+	case OUTMODE_MPEG2_PAR_CONT_CLK:
+		outreg = (1 << 10) | (1 << 6);	/* 0x0440 */
+		break;
+	case OUTMODE_MPEG2_SERIAL:
+		outreg = (1 << 10) | (2 << 6) | (0 << 1);	/* 0x0482 */
+		break;
+	case OUTMODE_DIVERSITY:
+		outreg = (1 << 10) | (4 << 6);	/* 0x0500 */
+		break;
+	case OUTMODE_MPEG2_FIFO:
+		outreg = (1 << 10) | (5 << 6);
+		break;
+	case OUTMODE_HIGH_Z:
+		outreg = 0;
+		break;
+	default:
+		dprintk("Unhandled output_mode passed to be set for demod %p", &state->fe[0]);
+		return -EINVAL;
+	}
+
+	dib9000_write_word(state, 1795, outreg);
+
+	switch (mode) {
+	case OUTMODE_MPEG2_PAR_GATED_CLK:
+	case OUTMODE_MPEG2_PAR_CONT_CLK:
+	case OUTMODE_MPEG2_SERIAL:
+	case OUTMODE_MPEG2_FIFO:
+		smo_mode = (dib9000_read_word(state, 295) & 0x0010) | (1 << 1);
+		if (state->chip.d9.cfg.output_mpeg2_in_188_bytes)
+			smo_mode |= (1 << 5);
+		dib9000_write_word(state, 295, smo_mode);
+		break;
+	}
+
+	outreg = to_fw_output_mode(mode);
+	return dib9000_mbx_send(state, OUT_MSG_SET_OUTPUT_MODE, &outreg, 1);
+}
+
+static int dib9000_tuner_xfer(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dib9000_state *state = i2c_get_adapdata(i2c_adap);
+	u16 i, len, t, index_msg;
+
+	for (index_msg = 0; index_msg < num; index_msg++) {
+		if (msg[index_msg].flags & I2C_M_RD) {	/* read */
+			len = msg[index_msg].len;
+			if (len > 16)
+				len = 16;
+
+			if (dib9000_read_word(state, 790) != 0)
+				dprintk("TunerITF: read busy");
+
+			dib9000_write_word(state, 784, (u16) (msg[index_msg].addr));
+			dib9000_write_word(state, 787, (len / 2) - 1);
+			dib9000_write_word(state, 786, 1);	/* start read */
+
+			i = 1000;
+			while (dib9000_read_word(state, 790) != (len / 2) && i)
+				i--;
+
+			if (i == 0)
+				dprintk("TunerITF: read failed");
+
+			for (i = 0; i < len; i += 2) {
+				t = dib9000_read_word(state, 785);
+				msg[index_msg].buf[i] = (t >> 8) & 0xff;
+				msg[index_msg].buf[i + 1] = (t) & 0xff;
+			}
+			if (dib9000_read_word(state, 790) != 0)
+				dprintk("TunerITF: read more data than expected");
+		} else {
+			i = 1000;
+			while (dib9000_read_word(state, 789) && i)
+				i--;
+			if (i == 0)
+				dprintk("TunerITF: write busy");
+
+			len = msg[index_msg].len;
+			if (len > 16)
+				len = 16;
+
+			for (i = 0; i < len; i += 2)
+				dib9000_write_word(state, 785, (msg[index_msg].buf[i] << 8) | msg[index_msg].buf[i + 1]);
+			dib9000_write_word(state, 784, (u16) msg[index_msg].addr);
+			dib9000_write_word(state, 787, (len / 2) - 1);
+			dib9000_write_word(state, 786, 0);	/* start write */
+
+			i = 1000;
+			while (dib9000_read_word(state, 791) > 0 && i)
+				i--;
+			if (i == 0)
+				dprintk("TunerITF: write failed");
+		}
+	}
+	return num;
+}
+
+int dib9000_fw_set_component_bus_speed(struct dvb_frontend *fe, u16 speed)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+
+	state->component_bus_speed = speed;
+	return 0;
+}
+EXPORT_SYMBOL(dib9000_fw_set_component_bus_speed);
+
+static int dib9000_fw_component_bus_xfer(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dib9000_state *state = i2c_get_adapdata(i2c_adap);
+	u8 type = 0;		/* I2C */
+	u8 port = DIBX000_I2C_INTERFACE_GPIO_3_4;
+	u16 scl = state->component_bus_speed;	/* SCL frequency */
+	struct dib9000_fe_memory_map *m = &state->platform.risc.fe_mm[FE_MM_RW_COMPONENT_ACCESS_BUFFER];
+	u8 p[13] = { 0 };
+
+	p[0] = type;
+	p[1] = port;
+	p[2] = msg[0].addr << 1;
+
+	p[3] = (u8) scl & 0xff;	/* scl */
+	p[4] = (u8) (scl >> 8);
+
+	p[7] = 0;
+	p[8] = 0;
+
+	p[9] = (u8) (msg[0].len);
+	p[10] = (u8) (msg[0].len >> 8);
+	if ((num > 1) && (msg[1].flags & I2C_M_RD)) {
+		p[11] = (u8) (msg[1].len);
+		p[12] = (u8) (msg[1].len >> 8);
+	} else {
+		p[11] = 0;
+		p[12] = 0;
+	}
+
+	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
+
+	dib9000_risc_mem_write(state, FE_MM_W_COMPONENT_ACCESS, p);
+
+	{			/* write-part */
+		dib9000_risc_mem_setup_cmd(state, m->addr, msg[0].len, 0);
+		dib9000_risc_mem_write_chunks(state, msg[0].buf, msg[0].len);
+	}
+
+	/* do the transaction */
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_COMPONENT_ACCESS) < 0) {
+		DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+		return 0;
+	}
+
+	/* read back any possible result */
+	if ((num > 1) && (msg[1].flags & I2C_M_RD))
+		dib9000_risc_mem_read(state, FE_MM_RW_COMPONENT_ACCESS_BUFFER, msg[1].buf, msg[1].len);
+
+	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+
+	return num;
+}
+
+static u32 dib9000_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
+
+static struct i2c_algorithm dib9000_tuner_algo = {
+	.master_xfer = dib9000_tuner_xfer,
+	.functionality = dib9000_i2c_func,
+};
+
+static struct i2c_algorithm dib9000_component_bus_algo = {
+	.master_xfer = dib9000_fw_component_bus_xfer,
+	.functionality = dib9000_i2c_func,
+};
+
+struct i2c_adapter *dib9000_get_tuner_interface(struct dvb_frontend *fe)
+{
+	struct dib9000_state *st = fe->demodulator_priv;
+	return &st->tuner_adap;
+}
+EXPORT_SYMBOL(dib9000_get_tuner_interface);
+
+struct i2c_adapter *dib9000_get_component_bus_interface(struct dvb_frontend *fe)
+{
+	struct dib9000_state *st = fe->demodulator_priv;
+	return &st->component_bus;
+}
+EXPORT_SYMBOL(dib9000_get_component_bus_interface);
+
+struct i2c_adapter *dib9000_get_i2c_master(struct dvb_frontend *fe, enum dibx000_i2c_interface intf, int gating)
+{
+	struct dib9000_state *st = fe->demodulator_priv;
+	return dibx000_get_i2c_adapter(&st->i2c_master, intf, gating);
+}
+EXPORT_SYMBOL(dib9000_get_i2c_master);
+
+int dib9000_set_i2c_adapter(struct dvb_frontend *fe, struct i2c_adapter *i2c)
+{
+	struct dib9000_state *st = fe->demodulator_priv;
+
+	st->i2c.i2c_adap = i2c;
+	return 0;
+}
+EXPORT_SYMBOL(dib9000_set_i2c_adapter);
+
+static int dib9000_cfg_gpio(struct dib9000_state *st, u8 num, u8 dir, u8 val)
+{
+	st->gpio_dir = dib9000_read_word(st, 773);
+	st->gpio_dir &= ~(1 << num);	/* reset the direction bit */
+	st->gpio_dir |= (dir & 0x1) << num;	/* set the new direction */
+	dib9000_write_word(st, 773, st->gpio_dir);
+
+	st->gpio_val = dib9000_read_word(st, 774);
+	st->gpio_val &= ~(1 << num);	/* reset the direction bit */
+	st->gpio_val |= (val & 0x01) << num;	/* set the new value */
+	dib9000_write_word(st, 774, st->gpio_val);
+
+	dprintk("gpio dir: %04x: gpio val: %04x", st->gpio_dir, st->gpio_val);
+
+	return 0;
+}
+
+int dib9000_set_gpio(struct dvb_frontend *fe, u8 num, u8 dir, u8 val)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	return dib9000_cfg_gpio(state, num, dir, val);
+}
+EXPORT_SYMBOL(dib9000_set_gpio);
+
+int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u16 val = dib9000_read_word(state, 294 + 1) & 0xffef;
+	val |= (onoff & 0x1) << 4;
+
+	dprintk("PID filter enabled %d", onoff);
+	return dib9000_write_word(state, 294 + 1, val);
+}
+EXPORT_SYMBOL(dib9000_fw_pid_filter_ctrl);
+
+int dib9000_fw_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	dprintk("Index %x, PID %d, OnOff %d", id, pid, onoff);
+	return dib9000_write_word(state, 300 + 1 + id, onoff ? (1 << 13) | pid : 0);
+}
+EXPORT_SYMBOL(dib9000_fw_pid_filter);
+
+int dib9000_firmware_post_pll_init(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	return dib9000_fw_init(state);
+}
+EXPORT_SYMBOL(dib9000_firmware_post_pll_init);
+
+static void dib9000_release(struct dvb_frontend *demod)
+{
+	struct dib9000_state *st = demod->demodulator_priv;
+	u8 index_frontend;
+
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (st->fe[index_frontend] != NULL); index_frontend++)
+		dvb_frontend_detach(st->fe[index_frontend]);
+
+	DibFreeLock(&state->platform.risc.mbx_if_lock);
+	DibFreeLock(&state->platform.risc.mbx_lock);
+	DibFreeLock(&state->platform.risc.mem_lock);
+	DibFreeLock(&state->platform.risc.mem_mbx_lock);
+	dibx000_exit_i2c_master(&st->i2c_master);
+
+	i2c_del_adapter(&st->tuner_adap);
+	i2c_del_adapter(&st->component_bus);
+	kfree(st->fe[0]);
+	kfree(st);
+}
+
+static int dib9000_wakeup(struct dvb_frontend *fe)
+{
+	return 0;
+}
+
+static int dib9000_sleep(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	int ret;
+
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		ret = state->fe[index_frontend]->ops.sleep(state->fe[index_frontend]);
+		if (ret < 0)
+			return ret;
+	}
+	return dib9000_mbx_send(state, OUT_MSG_FE_SLEEP, NULL, 0);
+}
+
+static int dib9000_fe_get_tune_settings(struct dvb_frontend *fe, struct dvb_frontend_tune_settings *tune)
+{
+	tune->min_delay_ms = 1000;
+	return 0;
+}
+
+static int dib9000_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend, sub_index_frontend;
+	fe_status_t stat;
+	int ret;
+
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		state->fe[index_frontend]->ops.read_status(state->fe[index_frontend], &stat);
+		if (stat & FE_HAS_SYNC) {
+			dprintk("TPS lock on the slave%i", index_frontend);
+
+			/* synchronize the cache with the other frontends */
+			state->fe[index_frontend]->ops.get_frontend(state->fe[index_frontend], fep);
+			for (sub_index_frontend = 0; (sub_index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[sub_index_frontend] != NULL);
+			     sub_index_frontend++) {
+				if (sub_index_frontend != index_frontend) {
+					state->fe[sub_index_frontend]->dtv_property_cache.modulation =
+					    state->fe[index_frontend]->dtv_property_cache.modulation;
+					state->fe[sub_index_frontend]->dtv_property_cache.inversion =
+					    state->fe[index_frontend]->dtv_property_cache.inversion;
+					state->fe[sub_index_frontend]->dtv_property_cache.transmission_mode =
+					    state->fe[index_frontend]->dtv_property_cache.transmission_mode;
+					state->fe[sub_index_frontend]->dtv_property_cache.guard_interval =
+					    state->fe[index_frontend]->dtv_property_cache.guard_interval;
+					state->fe[sub_index_frontend]->dtv_property_cache.hierarchy =
+					    state->fe[index_frontend]->dtv_property_cache.hierarchy;
+					state->fe[sub_index_frontend]->dtv_property_cache.code_rate_HP =
+					    state->fe[index_frontend]->dtv_property_cache.code_rate_HP;
+					state->fe[sub_index_frontend]->dtv_property_cache.code_rate_LP =
+					    state->fe[index_frontend]->dtv_property_cache.code_rate_LP;
+					state->fe[sub_index_frontend]->dtv_property_cache.rolloff =
+					    state->fe[index_frontend]->dtv_property_cache.rolloff;
+				}
+			}
+			return 0;
+		}
+	}
+
+	/* get the channel from master chip */
+	ret = dib9000_fw_get_channel(fe, fep);
+	if (ret != 0)
+		return ret;
+
+	/* synchronize the cache with the other frontends */
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		state->fe[index_frontend]->dtv_property_cache.inversion = fe->dtv_property_cache.inversion;
+		state->fe[index_frontend]->dtv_property_cache.transmission_mode = fe->dtv_property_cache.transmission_mode;
+		state->fe[index_frontend]->dtv_property_cache.guard_interval = fe->dtv_property_cache.guard_interval;
+		state->fe[index_frontend]->dtv_property_cache.modulation = fe->dtv_property_cache.modulation;
+		state->fe[index_frontend]->dtv_property_cache.hierarchy = fe->dtv_property_cache.hierarchy;
+		state->fe[index_frontend]->dtv_property_cache.code_rate_HP = fe->dtv_property_cache.code_rate_HP;
+		state->fe[index_frontend]->dtv_property_cache.code_rate_LP = fe->dtv_property_cache.code_rate_LP;
+		state->fe[index_frontend]->dtv_property_cache.rolloff = fe->dtv_property_cache.rolloff;
+	}
+
+	return 0;
+}
+
+static int dib9000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	state->tune_state = tune_state;
+	if (tune_state == CT_DEMOD_START)
+		state->status = FE_STATUS_TUNE_PENDING;
+
+	return 0;
+}
+
+static u32 dib9000_get_status(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	return state->status;
+}
+
+static int dib9000_set_channel_status(struct dvb_frontend *fe, struct dvb_frontend_parametersContext *channel_status)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+
+	memcpy(&state->channel_status, channel_status, sizeof(struct dvb_frontend_parametersContext));
+	return 0;
+}
+
+static int dib9000_set_frontend(struct dvb_frontend *fe, struct dvb_frontend_parameters *fep)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	int sleep_time, sleep_time_slave;
+	u32 frontend_status;
+	u8 nbr_pending, exit_condition, index_frontend, index_frontend_success;
+	struct dvb_frontend_parametersContext channel_status;
+
+	/* check that the correct parameters are set */
+	if (state->fe[0]->dtv_property_cache.frequency == 0) {
+		dprintk("dib9000: must specify frequency ");
+		return 0;
+	}
+
+	if (state->fe[0]->dtv_property_cache.bandwidth_hz == 0) {
+		dprintk("dib9000: must specify bandwidth ");
+		return 0;
+	}
+	fe->dtv_property_cache.delivery_system = SYS_DVBT;
+
+	/* set the master status */
+	if (fep->u.ofdm.transmission_mode == TRANSMISSION_MODE_AUTO ||
+	    fep->u.ofdm.guard_interval == GUARD_INTERVAL_AUTO || fep->u.ofdm.constellation == QAM_AUTO || fep->u.ofdm.code_rate_HP == FEC_AUTO) {
+		/* no channel specified, autosearch the channel */
+		state->channel_status.status = CHANNEL_STATUS_PARAMETERS_UNKNOWN;
+	} else
+		state->channel_status.status = CHANNEL_STATUS_PARAMETERS_SET;
+
+	/* set mode and status for the different frontends */
+	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		dib9000_fw_set_diversity_in(state->fe[index_frontend], 1);
+
+		/* synchronization of the cache */
+		memcpy(&state->fe[index_frontend]->dtv_property_cache, &fe->dtv_property_cache, sizeof(struct dtv_frontend_properties));
+
+		state->fe[index_frontend]->dtv_property_cache.delivery_system = SYS_DVBT;
+		dib9000_fw_set_output_mode(state->fe[index_frontend], OUTMODE_HIGH_Z);
+
+		dib9000_set_channel_status(state->fe[index_frontend], &state->channel_status);
+		dib9000_set_tune_state(state->fe[index_frontend], CT_DEMOD_START);
+	}
+
+	/* actual tune */
+	exit_condition = 0;	/* 0: tune pending; 1: tune failed; 2:tune success */
+	index_frontend_success = 0;
+	do {
+		sleep_time = dib9000_fw_tune(state->fe[0], NULL);
+		for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			sleep_time_slave = dib9000_fw_tune(state->fe[index_frontend], NULL);
+			if (sleep_time == FE_CALLBACK_TIME_NEVER)
+				sleep_time = sleep_time_slave;
+			else if ((sleep_time_slave != FE_CALLBACK_TIME_NEVER) && (sleep_time_slave > sleep_time))
+				sleep_time = sleep_time_slave;
+		}
+		if (sleep_time != FE_CALLBACK_TIME_NEVER)
+			msleep(sleep_time / 10);
+		else
+			break;
+
+		nbr_pending = 0;
+		exit_condition = 0;
+		index_frontend_success = 0;
+		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			frontend_status = -dib9000_get_status(state->fe[index_frontend]);
+			if (frontend_status > -FE_STATUS_TUNE_PENDING) {
+				exit_condition = 2;	/* tune success */
+				index_frontend_success = index_frontend;
+				break;
+			}
+			if (frontend_status == -FE_STATUS_TUNE_PENDING)
+				nbr_pending++;	/* some frontends are still tuning */
+		}
+		if ((exit_condition != 2) && (nbr_pending == 0))
+			exit_condition = 1;	/* if all tune are done and no success, exit: tune failed */
+
+	} while (exit_condition == 0);
+
+	/* check the tune result */
+	if (exit_condition == 1) {	/* tune failed */
+		dprintk("tune failed");
+		return 0;
+	}
+
+	dprintk("tune success on frontend%i", index_frontend_success);
+
+	/* synchronize all the channel cache */
+	dib9000_get_frontend(state->fe[0], fep);
+
+	/* retune the other frontends with the found channel */
+	channel_status.status = CHANNEL_STATUS_PARAMETERS_SET;
+	for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		/* only retune the frontends which was not tuned success */
+		if (index_frontend != index_frontend_success) {
+			dib9000_set_channel_status(state->fe[index_frontend], &channel_status);
+			dib9000_set_tune_state(state->fe[index_frontend], CT_DEMOD_START);
+		}
+	}
+	do {
+		sleep_time = FE_CALLBACK_TIME_NEVER;
+		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			if (index_frontend != index_frontend_success) {
+				sleep_time_slave = dib9000_fw_tune(state->fe[index_frontend], NULL);
+				if (sleep_time == FE_CALLBACK_TIME_NEVER)
+					sleep_time = sleep_time_slave;
+				else if ((sleep_time_slave != FE_CALLBACK_TIME_NEVER) && (sleep_time_slave > sleep_time))
+					sleep_time = sleep_time_slave;
+			}
+		}
+		if (sleep_time != FE_CALLBACK_TIME_NEVER)
+			msleep(sleep_time / 10);
+		else
+			break;
+
+		nbr_pending = 0;
+		for (index_frontend = 0; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+			if (index_frontend != index_frontend_success) {
+				frontend_status = -dib9000_get_status(state->fe[index_frontend]);
+				if ((index_frontend != index_frontend_success) && (frontend_status == -FE_STATUS_TUNE_PENDING))
+					nbr_pending++;	/* some frontends are still tuning */
+			}
+		}
+	} while (nbr_pending != 0);
+
+	/* set the output mode */
+	dib9000_fw_set_output_mode(state->fe[0], state->chip.d9.cfg.output_mode);
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		dib9000_fw_set_output_mode(state->fe[index_frontend], OUTMODE_DIVERSITY);
+
+	/* turn off the diversity for the last frontend */
+	dib9000_fw_set_diversity_in(state->fe[index_frontend - 1], 0);
+
+	return 0;
+}
+
+static u16 dib9000_read_lock(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+
+	return dib9000_read_word(state, 535);
+}
+
+static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	u16 lock = 0, lock_slave = 0;
+
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		lock_slave |= dib9000_read_lock(state->fe[index_frontend]);
+
+	lock = dib9000_read_word(state, 535);
+
+	*stat = 0;
+
+	if ((lock & 0x8000) || (lock_slave & 0x8000))
+		*stat |= FE_HAS_SIGNAL;
+	if ((lock & 0x3000) || (lock_slave & 0x3000))
+		*stat |= FE_HAS_CARRIER;
+	if ((lock & 0x0100) || (lock_slave & 0x0100))
+		*stat |= FE_HAS_VITERBI;
+	if (((lock & 0x0038) == 0x38) || ((lock_slave & 0x0038) == 0x38))
+		*stat |= FE_HAS_SYNC;
+	if ((lock & 0x0008) || (lock_slave & 0x0008))
+		*stat |= FE_HAS_LOCK;
+
+	return 0;
+}
+
+static int dib9000_read_ber(struct dvb_frontend *fe, u32 * ber)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u16 c[16];
+
+	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
+		return -EIO;
+	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, sizeof(c));
+	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+
+	*ber = c[10] << 16 | c[11];
+	return 0;
+}
+
+static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	u16 c[16];
+	u16 val;
+
+	*strength = 0;
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
+		state->fe[index_frontend]->ops.read_signal_strength(state->fe[index_frontend], &val);
+		if (val > 65535 - *strength)
+			*strength = 65535;
+		else
+			*strength += val;
+	}
+
+	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
+		return -EIO;
+	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, sizeof(c));
+	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+
+	val = 65535 - c[4];
+	if (val > 65535 - *strength)
+		*strength = 65535;
+	else
+		*strength += val;
+	return 0;
+}
+
+static u32 dib9000_get_snr(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u16 c[16];
+	u32 n, s, exp;
+	u16 val;
+
+	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
+		return -EIO;
+	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, sizeof(c));
+	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+
+	val = c[7];
+	n = (val >> 4) & 0xff;
+	exp = ((val & 0xf) << 2);
+	val = c[8];
+	exp += ((val >> 14) & 0x3);
+	if ((exp & 0x20) != 0)
+		exp -= 0x40;
+	n <<= exp + 16;
+
+	s = (val >> 6) & 0xFF;
+	exp = (val & 0x3F);
+	if ((exp & 0x20) != 0)
+		exp -= 0x40;
+	s <<= exp + 16;
+
+	if (n > 0) {
+		u32 t = (s / n) << 16;
+		return t + ((s << 16) - n * t) / n;
+	}
+	return 0xffffffff;
+}
+
+static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend;
+	u32 snr_master;
+
+	snr_master = dib9000_get_snr(fe);
+	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
+		snr_master += dib9000_get_snr(state->fe[index_frontend]);
+
+	if ((snr_master >> 16) != 0) {
+		snr_master = 10 * intlog10(snr_master >> 16);
+		*snr = snr_master / ((1 << 24) / 10);
+	} else
+		*snr = 0;
+
+	return 0;
+}
+
+static int dib9000_read_unc_blocks(struct dvb_frontend *fe, u32 * unc)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u16 c[16];
+
+	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
+		return -EIO;
+	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, sizeof(c));
+	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
+
+	*unc = c[12];
+	return 0;
+}
+
+int dib9000_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 default_addr, u8 first_addr)
+{
+	int k = 0;
+	u8 new_addr = 0;
+	struct i2c_device client = {.i2c_adap = i2c };
+
+	client.i2c_addr = default_addr + 16;
+	dib9000_i2c_write16(&client, 1796, 0x0);
+
+	for (k = no_of_demods - 1; k >= 0; k--) {
+		/* designated i2c address */
+		new_addr = first_addr + (k << 1);
+		client.i2c_addr = default_addr;
+
+		dib9000_i2c_write16(&client, 1817, 3);
+		dib9000_i2c_write16(&client, 1796, 0);
+		dib9000_i2c_write16(&client, 1227, 1);
+		dib9000_i2c_write16(&client, 1227, 0);
+
+		client.i2c_addr = new_addr;
+		dib9000_i2c_write16(&client, 1817, 3);
+		dib9000_i2c_write16(&client, 1796, 0);
+		dib9000_i2c_write16(&client, 1227, 1);
+		dib9000_i2c_write16(&client, 1227, 0);
+
+		if (dib9000_identify(&client) == 0) {
+			client.i2c_addr = default_addr;
+			if (dib9000_identify(&client) == 0) {
+				dprintk("DiB9000 #%d: not identified", k);
+				return -EIO;
+			}
+		}
+
+		dib9000_i2c_write16(&client, 1795, (1 << 10) | (4 << 6));
+		dib9000_i2c_write16(&client, 1794, (new_addr << 2) | 2);
+
+		dprintk("IC %d initialized (to i2c_address 0x%x)", k, new_addr);
+	}
+
+	for (k = 0; k < no_of_demods; k++) {
+		new_addr = first_addr | (k << 1);
+		client.i2c_addr = new_addr;
+
+		dib9000_i2c_write16(&client, 1794, (new_addr << 2));
+		dib9000_i2c_write16(&client, 1795, 0);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(dib9000_i2c_enumeration);
+
+int dib9000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend = 1;
+
+	while ((index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL))
+		index_frontend++;
+	if (index_frontend < MAX_NUMBER_OF_FRONTENDS) {
+		dprintk("set slave fe %p to index %i", fe_slave, index_frontend);
+		state->fe[index_frontend] = fe_slave;
+		return 0;
+	}
+
+	dprintk("too many slave frontend");
+	return -ENOMEM;
+}
+EXPORT_SYMBOL(dib9000_set_slave_frontend);
+
+int dib9000_remove_slave_frontend(struct dvb_frontend *fe)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+	u8 index_frontend = 1;
+
+	while ((index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL))
+		index_frontend++;
+	if (index_frontend != 1) {
+		dprintk("remove slave fe %p (index %i)", state->fe[index_frontend - 1], index_frontend - 1);
+		state->fe[index_frontend] = NULL;
+		return 0;
+	}
+
+	dprintk("no frontend to be removed");
+	return -ENODEV;
+}
+EXPORT_SYMBOL(dib9000_remove_slave_frontend);
+
+struct dvb_frontend *dib9000_get_slave_frontend(struct dvb_frontend *fe, int slave_index)
+{
+	struct dib9000_state *state = fe->demodulator_priv;
+
+	if (slave_index >= MAX_NUMBER_OF_FRONTENDS)
+		return NULL;
+	return state->fe[slave_index];
+}
+EXPORT_SYMBOL(dib9000_get_slave_frontend);
+
+static struct dvb_frontend_ops dib9000_ops;
+struct dvb_frontend *dib9000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, const struct dib9000_config *cfg)
+{
+	struct dvb_frontend *fe;
+	struct dib9000_state *st;
+	st = kzalloc(sizeof(struct dib9000_state), GFP_KERNEL);
+	if (st == NULL)
+		return NULL;
+	fe = kzalloc(sizeof(struct dvb_frontend), GFP_KERNEL);
+	if (fe == NULL)
+		return NULL;
+
+	memcpy(&st->chip.d9.cfg, cfg, sizeof(struct dib9000_config));
+	st->i2c.i2c_adap = i2c_adap;
+	st->i2c.i2c_addr = i2c_addr;
+
+	st->gpio_dir = DIB9000_GPIO_DEFAULT_DIRECTIONS;
+	st->gpio_val = DIB9000_GPIO_DEFAULT_VALUES;
+	st->gpio_pwm_pos = DIB9000_GPIO_DEFAULT_PWM_POS;
+
+	DibInitLock(&st->platform.risc.mbx_if_lock);
+	DibInitLock(&st->platform.risc.mbx_lock);
+	DibInitLock(&st->platform.risc.mem_lock);
+	DibInitLock(&st->platform.risc.mem_mbx_lock);
+
+	st->fe[0] = fe;
+	fe->demodulator_priv = st;
+	memcpy(&st->fe[0]->ops, &dib9000_ops, sizeof(struct dvb_frontend_ops));
+
+	/* Ensure the output mode remains at the previous default if it's
+	 * not specifically set by the caller.
+	 */
+	if ((st->chip.d9.cfg.output_mode != OUTMODE_MPEG2_SERIAL) && (st->chip.d9.cfg.output_mode != OUTMODE_MPEG2_PAR_GATED_CLK))
+		st->chip.d9.cfg.output_mode = OUTMODE_MPEG2_FIFO;
+
+	if (dib9000_identify(&st->i2c) == 0)
+		goto error;
+
+	dibx000_init_i2c_master(&st->i2c_master, DIB7000MC, st->i2c.i2c_adap, st->i2c.i2c_addr);
+
+	st->tuner_adap.dev.parent = i2c_adap->dev.parent;
+	strncpy(st->tuner_adap.name, "DIB9000_FW TUNER ACCESS", sizeof(st->tuner_adap.name));
+	st->tuner_adap.algo = &dib9000_tuner_algo;
+	st->tuner_adap.algo_data = NULL;
+	i2c_set_adapdata(&st->tuner_adap, st);
+	if (i2c_add_adapter(&st->tuner_adap) < 0)
+		goto error;
+
+	st->component_bus.dev.parent = i2c_adap->dev.parent;
+	strncpy(st->component_bus.name, "DIB9000_FW COMPONENT BUS ACCESS", sizeof(st->component_bus.name));
+	st->component_bus.algo = &dib9000_component_bus_algo;
+	st->component_bus.algo_data = NULL;
+	st->component_bus_speed = 340;
+	i2c_set_adapdata(&st->component_bus, st);
+	if (i2c_add_adapter(&st->component_bus) < 0)
+		goto component_bus_add_error;
+
+	dib9000_fw_reset(fe);
+
+	return fe;
+
+component_bus_add_error:
+	i2c_del_adapter(&st->tuner_adap);
+error:
+	kfree(st);
+	return NULL;
+}
+EXPORT_SYMBOL(dib9000_attach);
+
+static struct dvb_frontend_ops dib9000_ops = {
+	.info = {
+		 .name = "DiBcom 9000",
+		 .type = FE_OFDM,
+		 .frequency_min = 44250000,
+		 .frequency_max = 867250000,
+		 .frequency_stepsize = 62500,
+		 .caps = FE_CAN_INVERSION_AUTO |
+		 FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
+		 FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+		 FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 | FE_CAN_QAM_AUTO |
+		 FE_CAN_TRANSMISSION_MODE_AUTO | FE_CAN_GUARD_INTERVAL_AUTO | FE_CAN_RECOVER | FE_CAN_HIERARCHY_AUTO,
+		 },
+
+	.release = dib9000_release,
+
+	.init = dib9000_wakeup,
+	.sleep = dib9000_sleep,
+
+	.set_frontend = dib9000_set_frontend,
+	.get_tune_settings = dib9000_fe_get_tune_settings,
+	.get_frontend = dib9000_get_frontend,
+
+	.read_status = dib9000_read_status,
+	.read_ber = dib9000_read_ber,
+	.read_signal_strength = dib9000_read_signal_strength,
+	.read_snr = dib9000_read_snr,
+	.read_ucblocks = dib9000_read_unc_blocks,
+};
+
+MODULE_AUTHOR("Patrick Boettcher <pboettcher@dibcom.fr>");
+MODULE_AUTHOR("Olivier Grenie <ogrenie@dibcom.fr>");
+MODULE_DESCRIPTION("Driver for the DiBcom 9000 COFDM demodulator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/frontends/dib9000.h b/drivers/media/dvb/frontends/dib9000.h
new file mode 100644
index 000000000000..b5781a48034c
--- /dev/null
+++ b/drivers/media/dvb/frontends/dib9000.h
@@ -0,0 +1,131 @@
+#ifndef DIB9000_H
+#define DIB9000_H
+
+#include "dibx000_common.h"
+
+struct dib9000_config {
+	u8 dvbt_mode;
+	u8 output_mpeg2_in_188_bytes;
+	u8 hostbus_diversity;
+	struct dibx000_bandwidth_config *bw;
+
+	u16 if_drives;
+
+	u32 timing_frequency;
+	u32 xtal_clock_khz;
+	u32 vcxo_timer;
+	u32 demod_clock_khz;
+
+	const u8 *microcode_B_fe_buffer;
+	u32 microcode_B_fe_size;
+
+	struct dibGPIOFunction gpio_function[2];
+	struct dibSubbandSelection subband;
+
+	u8 output_mode;
+};
+
+#define DEFAULT_DIB9000_I2C_ADDRESS 18
+
+#if defined(CONFIG_DVB_DIB9000) || (defined(CONFIG_DVB_DIB9000_MODULE) && defined(MODULE))
+extern struct dvb_frontend *dib9000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, const struct dib9000_config *cfg);
+extern int dib9000_i2c_enumeration(struct i2c_adapter *host, int no_of_demods, u8 default_addr, u8 first_addr);
+extern struct i2c_adapter *dib9000_get_tuner_interface(struct dvb_frontend *fe);
+extern struct i2c_adapter *dib9000_get_i2c_master(struct dvb_frontend *fe, enum dibx000_i2c_interface intf, int gating);
+extern int dib9000_set_gpio(struct dvb_frontend *fe, u8 num, u8 dir, u8 val);
+extern int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff);
+extern int dib9000_fw_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff);
+extern int dib9000_firmware_post_pll_init(struct dvb_frontend *fe);
+extern int dib9000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave);
+extern int dib9000_remove_slave_frontend(struct dvb_frontend *fe);
+extern struct dvb_frontend *dib9000_get_slave_frontend(struct dvb_frontend *fe, int slave_index);
+extern struct i2c_adapter *dib9000_get_component_bus_interface(struct dvb_frontend *fe);
+extern int dib9000_set_i2c_adapter(struct dvb_frontend *fe, struct i2c_adapter *i2c);
+extern int dib9000_fw_set_component_bus_speed(struct dvb_frontend *fe, u16 speed);
+#else
+static inline struct dvb_frontend *dib9000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, struct dib9000_config *cfg)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline struct i2c_adapter *dib9000_get_i2c_master(struct dvb_frontend *fe, enum dibx000_i2c_interface intf, int gating)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline int dib9000_i2c_enumeration(struct i2c_adapter *host, int no_of_demods, u8 default_addr, u8 first_addr)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline struct i2c_adapter *dib9000_get_tuner_interface(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline int dib9000_set_gpio(struct dvb_frontend *fe, u8 num, u8 dir, u8 val)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib9000_fw_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib9000_firmware_post_pll_init(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib9000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+int dib9000_remove_slave_frontend(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline struct dvb_frontend *dib9000_get_slave_frontend(struct dvb_frontend *fe, int slave_index)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline struct i2c_adapter *dib9000_get_component_bus_interface(struct dvb_frontend *fe)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+
+static inline int dib9000_set_i2c_adapter(struct dvb_frontend *fe, struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+
+static inline int dib9000_fw_set_component_bus_speed(struct dvb_frontend *fe, u16 speed)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
+#endif
+
+#endif
diff --git a/drivers/media/dvb/frontends/dibx000_common.c b/drivers/media/dvb/frontends/dibx000_common.c
index 2311c0a3406c..f6938f97feb4 100644
--- a/drivers/media/dvb/frontends/dibx000_common.c
+++ b/drivers/media/dvb/frontends/dibx000_common.c
@@ -17,9 +17,145 @@ static int dibx000_write_word(struct dibx000_i2c_master *mst, u16 reg, u16 val)
 	struct i2c_msg msg = {
 		.addr = mst->i2c_addr,.flags = 0,.buf = b,.len = 4
 	};
+
 	return i2c_transfer(mst->i2c_adap, &msg, 1) != 1 ? -EREMOTEIO : 0;
 }
 
+static u16 dibx000_read_word(struct dibx000_i2c_master *mst, u16 reg)
+{
+	u8 wb[2] = { reg >> 8, reg & 0xff };
+	u8 rb[2];
+	struct i2c_msg msg[2] = {
+		{.addr = mst->i2c_addr, .flags = 0, .buf = wb, .len = 2},
+		{.addr = mst->i2c_addr, .flags = I2C_M_RD, .buf = rb, .len = 2},
+	};
+
+	if (i2c_transfer(mst->i2c_adap, msg, 2) != 2)
+		dprintk("i2c read error on %d", reg);
+
+	return (rb[0] << 8) | rb[1];
+}
+
+static int dibx000_is_i2c_done(struct dibx000_i2c_master *mst)
+{
+	int i = 100;
+	u16 status;
+
+	while (((status = dibx000_read_word(mst, mst->base_reg + 2)) & 0x0100) == 0 && --i > 0)
+		;
+
+	/* i2c timed out */
+	if (i == 0)
+		return -EREMOTEIO;
+
+	/* no acknowledge */
+	if ((status & 0x0080) == 0)
+		return -EREMOTEIO;
+
+	return 0;
+}
+
+static int dibx000_master_i2c_write(struct dibx000_i2c_master *mst, struct i2c_msg *msg, u8 stop)
+{
+	u16 data;
+	u16 da;
+	u16 i;
+	u16 txlen = msg->len, len;
+	const u8 *b = msg->buf;
+
+	while (txlen) {
+		dibx000_read_word(mst, mst->base_reg + 2);
+
+		len = txlen > 8 ? 8 : txlen;
+		for (i = 0; i < len; i += 2) {
+			data = *b++ << 8;
+			if (i+1 < len)
+				data |= *b++;
+			dibx000_write_word(mst, mst->base_reg, data);
+		}
+		da = (((u8) (msg->addr))  << 9) |
+			(1           << 8) |
+			(1           << 7) |
+			(0           << 6) |
+			(0           << 5) |
+			((len & 0x7) << 2) |
+			(0           << 1) |
+			(0           << 0);
+
+		if (txlen == msg->len)
+			da |= 1 << 5; /* start */
+
+		if (txlen-len == 0 && stop)
+			da |= 1 << 6; /* stop */
+
+		dibx000_write_word(mst, mst->base_reg+1, da);
+
+		if (dibx000_is_i2c_done(mst) != 0)
+			return -EREMOTEIO;
+		txlen -= len;
+	}
+
+	return 0;
+}
+
+static int dibx000_master_i2c_read(struct dibx000_i2c_master *mst, struct i2c_msg *msg)
+{
+	u16 da;
+	u8 *b = msg->buf;
+	u16 rxlen = msg->len, len;
+
+	while (rxlen) {
+		len = rxlen > 8 ? 8 : rxlen;
+		da = (((u8) (msg->addr)) << 9) |
+			(1           << 8) |
+			(1           << 7) |
+			(0           << 6) |
+			(0           << 5) |
+			((len & 0x7) << 2) |
+			(1           << 1) |
+			(0           << 0);
+
+		if (rxlen == msg->len)
+			da |= 1 << 5; /* start */
+
+		if (rxlen-len == 0)
+			da |= 1 << 6; /* stop */
+		dibx000_write_word(mst, mst->base_reg+1, da);
+
+		if (dibx000_is_i2c_done(mst) != 0)
+			return -EREMOTEIO;
+
+		rxlen -= len;
+
+		while (len) {
+			da = dibx000_read_word(mst, mst->base_reg);
+			*b++ = (da >> 8) & 0xff;
+			len--;
+			if (len >= 1) {
+				*b++ =  da   & 0xff;
+				len--;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int dibx000_i2c_set_speed(struct i2c_adapter *i2c_adap, u16 speed)
+{
+	struct dibx000_i2c_master *mst = i2c_get_adapdata(i2c_adap);
+
+	if (mst->device_rev < DIB7000MC && speed < 235)
+		speed = 235;
+	return dibx000_write_word(mst, mst->base_reg + 3, (u16)(60000 / speed));
+
+}
+EXPORT_SYMBOL(dibx000_i2c_set_speed);
+
+static u32 dibx000_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
 
 static int dibx000_i2c_select_interface(struct dibx000_i2c_master *mst,
 					enum dibx000_i2c_interface intf)
@@ -32,6 +168,60 @@ static int dibx000_i2c_select_interface(struct dibx000_i2c_master *mst,
 	return 0;
 }
 
+static int dibx000_i2c_master_xfer_gpio12(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dibx000_i2c_master *mst = i2c_get_adapdata(i2c_adap);
+	int msg_index;
+	int ret = 0;
+
+	dibx000_i2c_select_interface(mst, DIBX000_I2C_INTERFACE_GPIO_1_2);
+	for (msg_index = 0; msg_index < num; msg_index++) {
+		if (msg[msg_index].flags & I2C_M_RD) {
+			ret = dibx000_master_i2c_read(mst, &msg[msg_index]);
+			if (ret != 0)
+				return 0;
+		} else {
+			ret = dibx000_master_i2c_write(mst, &msg[msg_index], 1);
+			if (ret != 0)
+				return 0;
+		}
+	}
+
+	return num;
+}
+
+static int dibx000_i2c_master_xfer_gpio34(struct i2c_adapter *i2c_adap, struct i2c_msg msg[], int num)
+{
+	struct dibx000_i2c_master *mst = i2c_get_adapdata(i2c_adap);
+	int msg_index;
+	int ret = 0;
+
+	dibx000_i2c_select_interface(mst, DIBX000_I2C_INTERFACE_GPIO_3_4);
+	for (msg_index = 0; msg_index < num; msg_index++) {
+		if (msg[msg_index].flags & I2C_M_RD) {
+			ret = dibx000_master_i2c_read(mst, &msg[msg_index]);
+			if (ret != 0)
+				return 0;
+		} else {
+			ret = dibx000_master_i2c_write(mst, &msg[msg_index], 1);
+			if (ret != 0)
+				return 0;
+		}
+	}
+
+	return num;
+}
+
+static struct i2c_algorithm dibx000_i2c_master_gpio12_xfer_algo = {
+	.master_xfer = dibx000_i2c_master_xfer_gpio12,
+	.functionality = dibx000_i2c_func,
+};
+
+static struct i2c_algorithm dibx000_i2c_master_gpio34_xfer_algo = {
+	.master_xfer = dibx000_i2c_master_xfer_gpio34,
+	.functionality = dibx000_i2c_func,
+};
+
 static int dibx000_i2c_gate_ctrl(struct dibx000_i2c_master *mst, u8 tx[4],
 				 u8 addr, int onoff)
 {
@@ -54,11 +244,37 @@ static int dibx000_i2c_gate_ctrl(struct dibx000_i2c_master *mst, u8 tx[4],
 	return 0;
 }
 
-static u32 dibx000_i2c_func(struct i2c_adapter *adapter)
+static int dibx000_i2c_gated_gpio67_xfer(struct i2c_adapter *i2c_adap,
+					struct i2c_msg msg[], int num)
 {
-	return I2C_FUNC_I2C;
+	struct dibx000_i2c_master *mst = i2c_get_adapdata(i2c_adap);
+	struct i2c_msg m[2 + num];
+	u8 tx_open[4], tx_close[4];
+
+	memset(m, 0, sizeof(struct i2c_msg) * (2 + num));
+
+	dibx000_i2c_select_interface(mst, DIBX000_I2C_INTERFACE_GPIO_6_7);
+
+	dibx000_i2c_gate_ctrl(mst, tx_open, msg[0].addr, 1);
+	m[0].addr = mst->i2c_addr;
+	m[0].buf = tx_open;
+	m[0].len = 4;
+
+	memcpy(&m[1], msg, sizeof(struct i2c_msg) * num);
+
+	dibx000_i2c_gate_ctrl(mst, tx_close, 0, 0);
+	m[num + 1].addr = mst->i2c_addr;
+	m[num + 1].buf = tx_close;
+	m[num + 1].len = 4;
+
+	return i2c_transfer(mst->i2c_adap, m, 2 + num) == 2 + num ? num : -EIO;
 }
 
+static struct i2c_algorithm dibx000_i2c_gated_gpio67_algo = {
+	.master_xfer = dibx000_i2c_gated_gpio67_xfer,
+	.functionality = dibx000_i2c_func,
+};
+
 static int dibx000_i2c_gated_tuner_xfer(struct i2c_adapter *i2c_adap,
 					struct i2c_msg msg[], int num)
 {
@@ -91,8 +307,8 @@ static struct i2c_algorithm dibx000_i2c_gated_tuner_algo = {
 };
 
 struct i2c_adapter *dibx000_get_i2c_adapter(struct dibx000_i2c_master *mst,
-					    enum dibx000_i2c_interface intf,
-					    int gating)
+						enum dibx000_i2c_interface intf,
+						int gating)
 {
 	struct i2c_adapter *i2c = NULL;
 
@@ -101,6 +317,18 @@ struct i2c_adapter *dibx000_get_i2c_adapter(struct dibx000_i2c_master *mst,
 		if (gating)
 			i2c = &mst->gated_tuner_i2c_adap;
 		break;
+	case DIBX000_I2C_INTERFACE_GPIO_1_2:
+		if (!gating)
+			i2c = &mst->master_i2c_adap_gpio12;
+		break;
+	case DIBX000_I2C_INTERFACE_GPIO_3_4:
+		if (!gating)
+			i2c = &mst->master_i2c_adap_gpio34;
+		break;
+	case DIBX000_I2C_INTERFACE_GPIO_6_7:
+		if (gating)
+			i2c = &mst->master_i2c_adap_gpio67;
+		break;
 	default:
 		printk(KERN_ERR "DiBX000: incorrect I2C interface selected\n");
 		break;
@@ -126,8 +354,8 @@ void dibx000_reset_i2c_master(struct dibx000_i2c_master *mst)
 EXPORT_SYMBOL(dibx000_reset_i2c_master);
 
 static int i2c_adapter_init(struct i2c_adapter *i2c_adap,
-			    struct i2c_algorithm *algo, const char *name,
-			    struct dibx000_i2c_master *mst)
+				struct i2c_algorithm *algo, const char *name,
+				struct dibx000_i2c_master *mst)
 {
 	strncpy(i2c_adap->name, name, sizeof(i2c_adap->name));
 	i2c_adap->algo = algo;
@@ -139,7 +367,7 @@ static int i2c_adapter_init(struct i2c_adapter *i2c_adap,
 }
 
 int dibx000_init_i2c_master(struct dibx000_i2c_master *mst, u16 device_rev,
-			    struct i2c_adapter *i2c_adap, u8 i2c_addr)
+				struct i2c_adapter *i2c_adap, u8 i2c_addr)
 {
 	u8 tx[4];
 	struct i2c_msg m = {.addr = i2c_addr >> 1,.buf = tx,.len = 4 };
@@ -153,11 +381,33 @@ int dibx000_init_i2c_master(struct dibx000_i2c_master *mst, u16 device_rev,
 	else
 		mst->base_reg = 768;
 
+	mst->gated_tuner_i2c_adap.dev.parent = mst->i2c_adap->dev.parent;
+	if (i2c_adapter_init
+			(&mst->gated_tuner_i2c_adap, &dibx000_i2c_gated_tuner_algo,
+			 "DiBX000 tuner I2C bus", mst) != 0)
+		printk(KERN_ERR
+				"DiBX000: could not initialize the tuner i2c_adapter\n");
+
+	mst->master_i2c_adap_gpio12.dev.parent = mst->i2c_adap->dev.parent;
+	if (i2c_adapter_init
+			(&mst->master_i2c_adap_gpio12, &dibx000_i2c_master_gpio12_xfer_algo,
+			 "DiBX000 master GPIO12 I2C bus", mst) != 0)
+		printk(KERN_ERR
+				"DiBX000: could not initialize the master i2c_adapter\n");
+
+	mst->master_i2c_adap_gpio34.dev.parent = mst->i2c_adap->dev.parent;
+	if (i2c_adapter_init
+			(&mst->master_i2c_adap_gpio34, &dibx000_i2c_master_gpio34_xfer_algo,
+			 "DiBX000 master GPIO34 I2C bus", mst) != 0)
+		printk(KERN_ERR
+				"DiBX000: could not initialize the master i2c_adapter\n");
+
+	mst->master_i2c_adap_gpio67.dev.parent = mst->i2c_adap->dev.parent;
 	if (i2c_adapter_init
-	    (&mst->gated_tuner_i2c_adap, &dibx000_i2c_gated_tuner_algo,
-	     "DiBX000 tuner I2C bus", mst) != 0)
+			(&mst->master_i2c_adap_gpio67, &dibx000_i2c_gated_gpio67_algo,
+			 "DiBX000 master GPIO67 I2C bus", mst) != 0)
 		printk(KERN_ERR
-		       "DiBX000: could not initialize the tuner i2c_adapter\n");
+				"DiBX000: could not initialize the master i2c_adapter\n");
 
 	/* initialize the i2c-master by closing the gate */
 	dibx000_i2c_gate_ctrl(mst, tx, 0, 0);
@@ -170,16 +420,19 @@ EXPORT_SYMBOL(dibx000_init_i2c_master);
 void dibx000_exit_i2c_master(struct dibx000_i2c_master *mst)
 {
 	i2c_del_adapter(&mst->gated_tuner_i2c_adap);
+	i2c_del_adapter(&mst->master_i2c_adap_gpio12);
+	i2c_del_adapter(&mst->master_i2c_adap_gpio34);
+	i2c_del_adapter(&mst->master_i2c_adap_gpio67);
 }
 EXPORT_SYMBOL(dibx000_exit_i2c_master);
 
 
 u32 systime(void)
 {
-    struct timespec t;
+	struct timespec t;
 
-    t = current_kernel_time();
-    return (t.tv_sec * 10000) + (t.tv_nsec / 100000);
+	t = current_kernel_time();
+	return (t.tv_sec * 10000) + (t.tv_nsec / 100000);
 }
 EXPORT_SYMBOL(systime);
 
diff --git a/drivers/media/dvb/frontends/dibx000_common.h b/drivers/media/dvb/frontends/dibx000_common.h
index 4f5d141a308d..977d343369aa 100644
--- a/drivers/media/dvb/frontends/dibx000_common.h
+++ b/drivers/media/dvb/frontends/dibx000_common.h
@@ -4,7 +4,8 @@
 enum dibx000_i2c_interface {
 	DIBX000_I2C_INTERFACE_TUNER = 0,
 	DIBX000_I2C_INTERFACE_GPIO_1_2 = 1,
-	DIBX000_I2C_INTERFACE_GPIO_3_4 = 2
+	DIBX000_I2C_INTERFACE_GPIO_3_4 = 2,
+	DIBX000_I2C_INTERFACE_GPIO_6_7 = 3
 };
 
 struct dibx000_i2c_master {
@@ -17,8 +18,11 @@ struct dibx000_i2c_master {
 
 	enum dibx000_i2c_interface selected_interface;
 
-//      struct i2c_adapter  tuner_i2c_adap;
+/*	struct i2c_adapter  tuner_i2c_adap; */
 	struct i2c_adapter gated_tuner_i2c_adap;
+	struct i2c_adapter master_i2c_adap_gpio12;
+	struct i2c_adapter master_i2c_adap_gpio34;
+	struct i2c_adapter master_i2c_adap_gpio67;
 
 	struct i2c_adapter *i2c_adap;
 	u8 i2c_addr;
@@ -27,14 +31,15 @@ struct dibx000_i2c_master {
 };
 
 extern int dibx000_init_i2c_master(struct dibx000_i2c_master *mst,
-				   u16 device_rev, struct i2c_adapter *i2c_adap,
-				   u8 i2c_addr);
+					u16 device_rev, struct i2c_adapter *i2c_adap,
+					u8 i2c_addr);
 extern struct i2c_adapter *dibx000_get_i2c_adapter(struct dibx000_i2c_master
-						   *mst,
-						   enum dibx000_i2c_interface
-						   intf, int gating);
+							*mst,
+							enum dibx000_i2c_interface
+							intf, int gating);
 extern void dibx000_exit_i2c_master(struct dibx000_i2c_master *mst);
 extern void dibx000_reset_i2c_master(struct dibx000_i2c_master *mst);
+extern int dibx000_i2c_set_speed(struct i2c_adapter *i2c_adap, u16 speed);
 
 extern u32 systime(void);
 
@@ -42,7 +47,7 @@ extern u32 systime(void);
 #define BAND_UHF   0x02
 #define BAND_VHF   0x04
 #define BAND_SBAND 0x08
-#define BAND_FM    0x10
+#define BAND_FM	   0x10
 #define BAND_CBAND 0x20
 
 #define BAND_OF_FREQUENCY(freq_kHz) ((freq_kHz) <= 170000 ? BAND_CBAND : \
@@ -135,9 +140,9 @@ enum dibx000_adc_states {
 	DIBX000_VBG_DISABLE,
 };
 
-#define BANDWIDTH_TO_KHZ(v) ( (v) == BANDWIDTH_8_MHZ  ? 8000 : \
-			     (v) == BANDWIDTH_7_MHZ  ? 7000 : \
-			     (v) == BANDWIDTH_6_MHZ  ? 6000 : 8000 )
+#define BANDWIDTH_TO_KHZ(v) ((v) == BANDWIDTH_8_MHZ  ? 8000 : \
+				(v) == BANDWIDTH_7_MHZ  ? 7000 : \
+				(v) == BANDWIDTH_6_MHZ  ? 6000 : 8000)
 
 #define BANDWIDTH_TO_INDEX(v) ( \
 	(v) == 8000 ? BANDWIDTH_8_MHZ : \
@@ -153,53 +158,57 @@ enum dibx000_adc_states {
 #define OUTMODE_MPEG2_FIFO          5
 #define OUTMODE_ANALOG_ADC          6
 
+#define INPUT_MODE_OFF                0x11
+#define INPUT_MODE_DIVERSITY          0x12
+#define INPUT_MODE_MPEG               0x13
+
 enum frontend_tune_state {
-    CT_TUNER_START = 10,
-    CT_TUNER_STEP_0,
-    CT_TUNER_STEP_1,
-    CT_TUNER_STEP_2,
-    CT_TUNER_STEP_3,
-    CT_TUNER_STEP_4,
-    CT_TUNER_STEP_5,
-    CT_TUNER_STEP_6,
-    CT_TUNER_STEP_7,
-    CT_TUNER_STOP,
-
-    CT_AGC_START = 20,
-    CT_AGC_STEP_0,
-    CT_AGC_STEP_1,
-    CT_AGC_STEP_2,
-    CT_AGC_STEP_3,
-    CT_AGC_STEP_4,
-    CT_AGC_STOP,
+	CT_TUNER_START = 10,
+	CT_TUNER_STEP_0,
+	CT_TUNER_STEP_1,
+	CT_TUNER_STEP_2,
+	CT_TUNER_STEP_3,
+	CT_TUNER_STEP_4,
+	CT_TUNER_STEP_5,
+	CT_TUNER_STEP_6,
+	CT_TUNER_STEP_7,
+	CT_TUNER_STOP,
+
+	CT_AGC_START = 20,
+	CT_AGC_STEP_0,
+	CT_AGC_STEP_1,
+	CT_AGC_STEP_2,
+	CT_AGC_STEP_3,
+	CT_AGC_STEP_4,
+	CT_AGC_STOP,
 
 	CT_DEMOD_START = 30,
-    CT_DEMOD_STEP_1,
-    CT_DEMOD_STEP_2,
-    CT_DEMOD_STEP_3,
-    CT_DEMOD_STEP_4,
-    CT_DEMOD_STEP_5,
-    CT_DEMOD_STEP_6,
-    CT_DEMOD_STEP_7,
-    CT_DEMOD_STEP_8,
-    CT_DEMOD_STEP_9,
-    CT_DEMOD_STEP_10,
-    CT_DEMOD_SEARCH_NEXT = 41,
-    CT_DEMOD_STEP_LOCKED,
-    CT_DEMOD_STOP,
-
-    CT_DONE = 100,
-    CT_SHUTDOWN,
+	CT_DEMOD_STEP_1,
+	CT_DEMOD_STEP_2,
+	CT_DEMOD_STEP_3,
+	CT_DEMOD_STEP_4,
+	CT_DEMOD_STEP_5,
+	CT_DEMOD_STEP_6,
+	CT_DEMOD_STEP_7,
+	CT_DEMOD_STEP_8,
+	CT_DEMOD_STEP_9,
+	CT_DEMOD_STEP_10,
+	CT_DEMOD_SEARCH_NEXT = 41,
+	CT_DEMOD_STEP_LOCKED,
+	CT_DEMOD_STOP,
+
+	CT_DONE = 100,
+	CT_SHUTDOWN,
 
 };
 
 struct dvb_frontend_parametersContext {
 #define CHANNEL_STATUS_PARAMETERS_UNKNOWN   0x01
 #define CHANNEL_STATUS_PARAMETERS_SET       0x02
-    u8 status;
-    u32 tune_time_estimation[2];
-    s32 tps_available;
-    u16 tps[9];
+	u8 status;
+	u32 tune_time_estimation[2];
+	s32 tps_available;
+	u16 tps[9];
 };
 
 #define FE_STATUS_TUNE_FAILED          0
@@ -216,4 +225,49 @@ struct dvb_frontend_parametersContext {
 
 #define ABS(x) ((x < 0) ? (-x) : (x))
 
+#define DATA_BUS_ACCESS_MODE_8BIT                 0x01
+#define DATA_BUS_ACCESS_MODE_16BIT                0x02
+#define DATA_BUS_ACCESS_MODE_NO_ADDRESS_INCREMENT 0x10
+
+struct dibGPIOFunction {
+#define BOARD_GPIO_COMPONENT_BUS_ADAPTER 1
+#define BOARD_GPIO_COMPONENT_DEMOD       2
+	u8 component;
+
+#define BOARD_GPIO_FUNCTION_BOARD_ON      1
+#define BOARD_GPIO_FUNCTION_BOARD_OFF     2
+#define BOARD_GPIO_FUNCTION_COMPONENT_ON  3
+#define BOARD_GPIO_FUNCTION_COMPONENT_OFF 4
+#define BOARD_GPIO_FUNCTION_SUBBAND_PWM   5
+#define BOARD_GPIO_FUNCTION_SUBBAND_GPIO   6
+	u8 function;
+
+/* mask, direction and value are used specify which GPIO to change GPIO0
+ * is LSB and possible GPIO31 is MSB.  The same bit-position as in the
+ * mask is used for the direction and the value. Direction == 1 is OUT,
+ * 0 == IN. For direction "OUT" value is either 1 or 0, for direction IN
+ * value has no meaning.
+ *
+ * In case of BOARD_GPIO_FUNCTION_PWM mask is giving the GPIO to be
+ * used to do the PWM. Direction gives the PWModulator to be used.
+ * Value gives the PWM value in device-dependent scale.
+ */
+	u32 mask;
+	u32 direction;
+	u32 value;
+};
+
+#define MAX_NB_SUBBANDS   8
+struct dibSubbandSelection {
+	u8  size; /* Actual number of subbands. */
+	struct {
+		u16 f_mhz;
+		struct dibGPIOFunction gpio;
+	} subband[MAX_NB_SUBBANDS];
+};
+
+#define DEMOD_TIMF_SET    0x00
+#define DEMOD_TIMF_GET    0x01
+#define DEMOD_TIMF_UPDATE 0x02
+
 #endif
diff --git a/drivers/media/dvb/frontends/ds3000.c b/drivers/media/dvb/frontends/ds3000.c
index fc61d9230db8..90bf573308b0 100644
--- a/drivers/media/dvb/frontends/ds3000.c
+++ b/drivers/media/dvb/frontends/ds3000.c
@@ -229,31 +229,11 @@ static u8 ds3000_dvbs2_init_tab[] = {
 	0xb8, 0x00,
 };
 
-/* DS3000 doesn't need some parameters as input and auto-detects them */
-/* save input from the application of those parameters */
-struct ds3000_tuning {
-	u32 frequency;
-	u32 symbol_rate;
-	fe_spectral_inversion_t inversion;
-	enum fe_code_rate fec;
-
-	/* input values */
-	u8 inversion_val;
-	fe_modulation_t delivery;
-	u8 rolloff;
-};
-
 struct ds3000_state {
 	struct i2c_adapter *i2c;
 	const struct ds3000_config *config;
-
 	struct dvb_frontend frontend;
-
-	struct ds3000_tuning dcur;
-	struct ds3000_tuning dnxt;
-
 	u8 skip_fw_load;
-
 	/* previous uncorrected block counter for DVB-S2 */
 	u16 prevUCBS2;
 };
@@ -305,7 +285,7 @@ static int ds3000_writeFW(struct ds3000_state *state, int reg,
 	struct i2c_msg msg;
 	u8 *buf;
 
-	buf = kmalloc(3, GFP_KERNEL);
+	buf = kmalloc(33, GFP_KERNEL);
 	if (buf == NULL) {
 		printk(KERN_ERR "Unable to kmalloc\n");
 		ret = -ENOMEM;
@@ -317,10 +297,10 @@ static int ds3000_writeFW(struct ds3000_state *state, int reg,
 	msg.addr = state->config->demod_address;
 	msg.flags = 0;
 	msg.buf = buf;
-	msg.len = 3;
+	msg.len = 33;
 
-	for (i = 0; i < len; i += 2) {
-		memcpy(buf + 1, data + i, 2);
+	for (i = 0; i < len; i += 32) {
+		memcpy(buf + 1, data + i, 32);
 
 		dprintk("%s: write reg 0x%02x, len = %d\n", __func__, reg, len);
 
@@ -401,45 +381,6 @@ static int ds3000_tuner_readreg(struct ds3000_state *state, u8 reg)
 	return b1[0];
 }
 
-static int ds3000_set_inversion(struct ds3000_state *state,
-					fe_spectral_inversion_t inversion)
-{
-	dprintk("%s(%d)\n", __func__, inversion);
-
-	switch (inversion) {
-	case INVERSION_OFF:
-	case INVERSION_ON:
-	case INVERSION_AUTO:
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	state->dnxt.inversion = inversion;
-
-	return 0;
-}
-
-static int ds3000_set_symbolrate(struct ds3000_state *state, u32 rate)
-{
-	int ret = 0;
-
-	dprintk("%s()\n", __func__);
-
-	dprintk("%s() symbol_rate = %d\n", __func__, state->dnxt.symbol_rate);
-
-	/*  check if symbol rate is within limits */
-	if ((state->dnxt.symbol_rate >
-				state->frontend.ops.info.symbol_rate_max) ||
-	    (state->dnxt.symbol_rate <
-				state->frontend.ops.info.symbol_rate_min))
-		ret = -EOPNOTSUPP;
-
-	state->dnxt.symbol_rate = rate;
-
-	return ret;
-}
-
 static int ds3000_load_firmware(struct dvb_frontend *fe,
 					const struct firmware *fw);
 
@@ -509,23 +450,31 @@ static int ds3000_load_firmware(struct dvb_frontend *fe,
 	return 0;
 }
 
-static void ds3000_dump_registers(struct dvb_frontend *fe)
+static int ds3000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
-	int x, y, reg = 0, val;
-
-	for (y = 0; y < 16; y++) {
-		dprintk("%s: %02x: ", __func__, y);
-		for (x = 0; x < 16; x++) {
-			reg = (y << 4) + x;
-			val = ds3000_readreg(state, reg);
-			if (x != 15)
-				dprintk("%02x ",  val);
-			else
-				dprintk("%02x\n", val);
-		}
+	u8 data;
+
+	dprintk("%s(%d)\n", __func__, voltage);
+
+	data = ds3000_readreg(state, 0xa2);
+	data |= 0x03; /* bit0 V/H, bit1 off/on */
+
+	switch (voltage) {
+	case SEC_VOLTAGE_18:
+		data &= ~0x03;
+		break;
+	case SEC_VOLTAGE_13:
+		data &= ~0x03;
+		data |= 0x01;
+		break;
+	case SEC_VOLTAGE_OFF:
+		break;
 	}
-	dprintk("%s: -- DS3000 DUMP DONE --\n", __func__);
+
+	ds3000_writereg(state, 0xa2, data);
+
+	return 0;
 }
 
 static int ds3000_read_status(struct dvb_frontend *fe, fe_status_t* status)
@@ -562,16 +511,6 @@ static int ds3000_read_status(struct dvb_frontend *fe, fe_status_t* status)
 	return 0;
 }
 
-#define FE_IS_TUNED (FE_HAS_SIGNAL + FE_HAS_LOCK)
-static int ds3000_is_tuned(struct dvb_frontend *fe)
-{
-	fe_status_t tunerstat;
-
-	ds3000_read_status(fe, &tunerstat);
-
-	return ((tunerstat & FE_IS_TUNED) == FE_IS_TUNED);
-}
-
 /* read DS3000 BER value */
 static int ds3000_read_ber(struct dvb_frontend *fe, u32* ber)
 {
@@ -792,13 +731,6 @@ static int ds3000_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 	return 0;
 }
 
-/* Overwrite the current tuning params, we are about to tune */
-static void ds3000_clone_params(struct dvb_frontend *fe)
-{
-	struct ds3000_state *state = fe->demodulator_priv;
-	memcpy(&state->dcur, &state->dnxt, sizeof(state->dcur));
-}
-
 static int ds3000_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
@@ -1016,287 +948,298 @@ static int ds3000_get_property(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int ds3000_tune(struct dvb_frontend *fe,
+static int ds3000_set_carrier_offset(struct dvb_frontend *fe,
+					s32 carrier_offset_khz)
+{
+	struct ds3000_state *state = fe->demodulator_priv;
+	s32 tmp;
+
+	tmp = carrier_offset_khz;
+	tmp *= 65536;
+	tmp = (2 * tmp + DS3000_SAMPLE_RATE) / (2 * DS3000_SAMPLE_RATE);
+
+	if (tmp < 0)
+		tmp += 65536;
+
+	ds3000_writereg(state, 0x5f, tmp >> 8);
+	ds3000_writereg(state, 0x5e, tmp & 0xff);
+
+	return 0;
+}
+
+static int ds3000_set_frontend(struct dvb_frontend *fe,
 				struct dvb_frontend_parameters *p)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	int ret = 0, retune, i;
-	u8 status, mlpf, mlpf_new, mlpf_max, mlpf_min, nlpf;
+	int i;
+	fe_status_t status;
+	u8 mlpf, mlpf_new, mlpf_max, mlpf_min, nlpf, div4;
+	s32 offset_khz;
 	u16 value, ndiv;
 	u32 f3db;
 
 	dprintk("%s() ", __func__);
 
-	/* Load the firmware if required */
-	ret = ds3000_firmware_ondemand(fe);
-	if (ret != 0) {
-		printk(KERN_ERR "%s: Unable initialise the firmware\n",
-								__func__);
-		return ret;
+	if (state->config->set_ts_params)
+		state->config->set_ts_params(fe, 0);
+	/* Tune */
+	/* unknown */
+	ds3000_tuner_writereg(state, 0x07, 0x02);
+	ds3000_tuner_writereg(state, 0x10, 0x00);
+	ds3000_tuner_writereg(state, 0x60, 0x79);
+	ds3000_tuner_writereg(state, 0x08, 0x01);
+	ds3000_tuner_writereg(state, 0x00, 0x01);
+	div4 = 0;
+
+	/* calculate and set freq divider */
+	if (p->frequency < 1146000) {
+		ds3000_tuner_writereg(state, 0x10, 0x11);
+		div4 = 1;
+		ndiv = ((p->frequency * (6 + 8) * 4) +
+				(DS3000_XTAL_FREQ / 2)) /
+				DS3000_XTAL_FREQ - 1024;
+	} else {
+		ds3000_tuner_writereg(state, 0x10, 0x01);
+		ndiv = ((p->frequency * (6 + 8) * 2) +
+				(DS3000_XTAL_FREQ / 2)) /
+				DS3000_XTAL_FREQ - 1024;
 	}
 
-	state->dnxt.delivery = c->modulation;
-	state->dnxt.frequency = c->frequency;
-	state->dnxt.rolloff = 2; /* fixme */
-	state->dnxt.fec = c->fec_inner;
+	ds3000_tuner_writereg(state, 0x01, (ndiv & 0x0f00) >> 8);
+	ds3000_tuner_writereg(state, 0x02, ndiv & 0x00ff);
+
+	/* set pll */
+	ds3000_tuner_writereg(state, 0x03, 0x06);
+	ds3000_tuner_writereg(state, 0x51, 0x0f);
+	ds3000_tuner_writereg(state, 0x51, 0x1f);
+	ds3000_tuner_writereg(state, 0x50, 0x10);
+	ds3000_tuner_writereg(state, 0x50, 0x00);
+	msleep(5);
+
+	/* unknown */
+	ds3000_tuner_writereg(state, 0x51, 0x17);
+	ds3000_tuner_writereg(state, 0x51, 0x1f);
+	ds3000_tuner_writereg(state, 0x50, 0x08);
+	ds3000_tuner_writereg(state, 0x50, 0x00);
+	msleep(5);
+
+	value = ds3000_tuner_readreg(state, 0x3d);
+	value &= 0x0f;
+	if ((value > 4) && (value < 15)) {
+		value -= 3;
+		if (value < 4)
+			value = 4;
+		value = ((value << 3) | 0x01) & 0x79;
+	}
 
-	ret = ds3000_set_inversion(state, p->inversion);
-	if (ret !=  0)
-		return ret;
+	ds3000_tuner_writereg(state, 0x60, value);
+	ds3000_tuner_writereg(state, 0x51, 0x17);
+	ds3000_tuner_writereg(state, 0x51, 0x1f);
+	ds3000_tuner_writereg(state, 0x50, 0x08);
+	ds3000_tuner_writereg(state, 0x50, 0x00);
+
+	/* set low-pass filter period */
+	ds3000_tuner_writereg(state, 0x04, 0x2e);
+	ds3000_tuner_writereg(state, 0x51, 0x1b);
+	ds3000_tuner_writereg(state, 0x51, 0x1f);
+	ds3000_tuner_writereg(state, 0x50, 0x04);
+	ds3000_tuner_writereg(state, 0x50, 0x00);
+	msleep(5);
+
+	f3db = ((c->symbol_rate / 1000) << 2) / 5 + 2000;
+	if ((c->symbol_rate / 1000) < 5000)
+		f3db += 3000;
+	if (f3db < 7000)
+		f3db = 7000;
+	if (f3db > 40000)
+		f3db = 40000;
+
+	/* set low-pass filter baseband */
+	value = ds3000_tuner_readreg(state, 0x26);
+	mlpf = 0x2e * 207 / ((value << 1) + 151);
+	mlpf_max = mlpf * 135 / 100;
+	mlpf_min = mlpf * 78 / 100;
+	if (mlpf_max > 63)
+		mlpf_max = 63;
+
+	/* rounded to the closest integer */
+	nlpf = ((mlpf * f3db * 1000) + (2766 * DS3000_XTAL_FREQ / 2))
+			/ (2766 * DS3000_XTAL_FREQ);
+	if (nlpf > 23)
+		nlpf = 23;
+	if (nlpf < 1)
+		nlpf = 1;
+
+	/* rounded to the closest integer */
+	mlpf_new = ((DS3000_XTAL_FREQ * nlpf * 2766) +
+			(1000 * f3db / 2)) / (1000 * f3db);
+
+	if (mlpf_new < mlpf_min) {
+		nlpf++;
+		mlpf_new = ((DS3000_XTAL_FREQ * nlpf * 2766) +
+				(1000 * f3db / 2)) / (1000 * f3db);
+	}
 
-	ret = ds3000_set_symbolrate(state, c->symbol_rate);
-	if (ret !=  0)
-		return ret;
+	if (mlpf_new > mlpf_max)
+		mlpf_new = mlpf_max;
+
+	ds3000_tuner_writereg(state, 0x04, mlpf_new);
+	ds3000_tuner_writereg(state, 0x06, nlpf);
+	ds3000_tuner_writereg(state, 0x51, 0x1b);
+	ds3000_tuner_writereg(state, 0x51, 0x1f);
+	ds3000_tuner_writereg(state, 0x50, 0x04);
+	ds3000_tuner_writereg(state, 0x50, 0x00);
+	msleep(5);
+
+	/* unknown */
+	ds3000_tuner_writereg(state, 0x51, 0x1e);
+	ds3000_tuner_writereg(state, 0x51, 0x1f);
+	ds3000_tuner_writereg(state, 0x50, 0x01);
+	ds3000_tuner_writereg(state, 0x50, 0x00);
+	msleep(60);
+
+	offset_khz = (ndiv - ndiv % 2 + 1024) * DS3000_XTAL_FREQ
+		/ (6 + 8) / (div4 + 1) / 2 - p->frequency;
+
+	/* ds3000 global reset */
+	ds3000_writereg(state, 0x07, 0x80);
+	ds3000_writereg(state, 0x07, 0x00);
+	/* ds3000 build-in uC reset */
+	ds3000_writereg(state, 0xb2, 0x01);
+	/* ds3000 software reset */
+	ds3000_writereg(state, 0x00, 0x01);
 
-	/* discard the 'current' tuning parameters and prepare to tune */
-	ds3000_clone_params(fe);
-
-	retune = 1;	/* try 1 times */
-	dprintk("%s:   retune = %d\n", __func__, retune);
-	dprintk("%s:   frequency   = %d\n", __func__, state->dcur.frequency);
-	dprintk("%s:   symbol_rate = %d\n", __func__, state->dcur.symbol_rate);
-	dprintk("%s:   FEC	 = %d \n", __func__,
-		state->dcur.fec);
-	dprintk("%s:   Inversion   = %d\n", __func__, state->dcur.inversion);
-
-	do {
-		/* Reset status register */
-		status = 0;
-		/* Tune */
-		/* TS2020 init */
-		ds3000_tuner_writereg(state, 0x42, 0x73);
-		ds3000_tuner_writereg(state, 0x05, 0x01);
-		ds3000_tuner_writereg(state, 0x62, 0xf5);
-		/* unknown */
-		ds3000_tuner_writereg(state, 0x07, 0x02);
-		ds3000_tuner_writereg(state, 0x10, 0x00);
-		ds3000_tuner_writereg(state, 0x60, 0x79);
-		ds3000_tuner_writereg(state, 0x08, 0x01);
-		ds3000_tuner_writereg(state, 0x00, 0x01);
-		/* calculate and set freq divider */
-		if (state->dcur.frequency < 1146000) {
-			ds3000_tuner_writereg(state, 0x10, 0x11);
-			ndiv = ((state->dcur.frequency * (6 + 8) * 4) +
-					(DS3000_XTAL_FREQ / 2)) /
-					DS3000_XTAL_FREQ - 1024;
-		} else {
-			ds3000_tuner_writereg(state, 0x10, 0x01);
-			ndiv = ((state->dcur.frequency * (6 + 8) * 2) +
-					(DS3000_XTAL_FREQ / 2)) /
-					DS3000_XTAL_FREQ - 1024;
-		}
+	switch (c->delivery_system) {
+	case SYS_DVBS:
+		/* initialise the demod in DVB-S mode */
+		for (i = 0; i < sizeof(ds3000_dvbs_init_tab); i += 2)
+			ds3000_writereg(state,
+				ds3000_dvbs_init_tab[i],
+				ds3000_dvbs_init_tab[i + 1]);
+		value = ds3000_readreg(state, 0xfe);
+		value &= 0xc0;
+		value |= 0x1b;
+		ds3000_writereg(state, 0xfe, value);
+		break;
+	case SYS_DVBS2:
+		/* initialise the demod in DVB-S2 mode */
+		for (i = 0; i < sizeof(ds3000_dvbs2_init_tab); i += 2)
+			ds3000_writereg(state,
+				ds3000_dvbs2_init_tab[i],
+				ds3000_dvbs2_init_tab[i + 1]);
+		ds3000_writereg(state, 0xfe, 0x98);
+		break;
+	default:
+		return 1;
+	}
 
-		ds3000_tuner_writereg(state, 0x01, (ndiv & 0x0f00) >> 8);
-		ds3000_tuner_writereg(state, 0x02, ndiv & 0x00ff);
-
-		/* set pll */
-		ds3000_tuner_writereg(state, 0x03, 0x06);
-		ds3000_tuner_writereg(state, 0x51, 0x0f);
-		ds3000_tuner_writereg(state, 0x51, 0x1f);
-		ds3000_tuner_writereg(state, 0x50, 0x10);
-		ds3000_tuner_writereg(state, 0x50, 0x00);
-		msleep(5);
-
-		/* unknown */
-		ds3000_tuner_writereg(state, 0x51, 0x17);
-		ds3000_tuner_writereg(state, 0x51, 0x1f);
-		ds3000_tuner_writereg(state, 0x50, 0x08);
-		ds3000_tuner_writereg(state, 0x50, 0x00);
-		msleep(5);
-
-		value = ds3000_tuner_readreg(state, 0x3d);
-		value &= 0x0f;
-		if ((value > 4) && (value < 15)) {
-			value -= 3;
-			if (value < 4)
-				value = 4;
-			value = ((value << 3) | 0x01) & 0x79;
-		}
+	/* enable 27MHz clock output */
+	ds3000_writereg(state, 0x29, 0x80);
+	/* enable ac coupling */
+	ds3000_writereg(state, 0x25, 0x8a);
+
+	/* enhance symbol rate performance */
+	if ((c->symbol_rate / 1000) <= 5000) {
+		value = 29777 / (c->symbol_rate / 1000) + 1;
+		if (value % 2 != 0)
+			value++;
+		ds3000_writereg(state, 0xc3, 0x0d);
+		ds3000_writereg(state, 0xc8, value);
+		ds3000_writereg(state, 0xc4, 0x10);
+		ds3000_writereg(state, 0xc7, 0x0e);
+	} else if ((c->symbol_rate / 1000) <= 10000) {
+		value = 92166 / (c->symbol_rate / 1000) + 1;
+		if (value % 2 != 0)
+			value++;
+		ds3000_writereg(state, 0xc3, 0x07);
+		ds3000_writereg(state, 0xc8, value);
+		ds3000_writereg(state, 0xc4, 0x09);
+		ds3000_writereg(state, 0xc7, 0x12);
+	} else if ((c->symbol_rate / 1000) <= 20000) {
+		value = 64516 / (c->symbol_rate / 1000) + 1;
+		ds3000_writereg(state, 0xc3, value);
+		ds3000_writereg(state, 0xc8, 0x0e);
+		ds3000_writereg(state, 0xc4, 0x07);
+		ds3000_writereg(state, 0xc7, 0x18);
+	} else {
+		value = 129032 / (c->symbol_rate / 1000) + 1;
+		ds3000_writereg(state, 0xc3, value);
+		ds3000_writereg(state, 0xc8, 0x0a);
+		ds3000_writereg(state, 0xc4, 0x05);
+		ds3000_writereg(state, 0xc7, 0x24);
+	}
 
-		ds3000_tuner_writereg(state, 0x60, value);
-		ds3000_tuner_writereg(state, 0x51, 0x17);
-		ds3000_tuner_writereg(state, 0x51, 0x1f);
-		ds3000_tuner_writereg(state, 0x50, 0x08);
-		ds3000_tuner_writereg(state, 0x50, 0x00);
-
-		/* set low-pass filter period */
-		ds3000_tuner_writereg(state, 0x04, 0x2e);
-		ds3000_tuner_writereg(state, 0x51, 0x1b);
-		ds3000_tuner_writereg(state, 0x51, 0x1f);
-		ds3000_tuner_writereg(state, 0x50, 0x04);
-		ds3000_tuner_writereg(state, 0x50, 0x00);
-		msleep(5);
-
-		f3db = ((state->dcur.symbol_rate / 1000) << 2) / 5 + 2000;
-		if ((state->dcur.symbol_rate / 1000) < 5000)
-			f3db += 3000;
-		if (f3db < 7000)
-			f3db = 7000;
-		if (f3db > 40000)
-			f3db = 40000;
-
-		/* set low-pass filter baseband */
-		value = ds3000_tuner_readreg(state, 0x26);
-		mlpf = 0x2e * 207 / ((value << 1) + 151);
-		mlpf_max = mlpf * 135 / 100;
-		mlpf_min = mlpf * 78 / 100;
-		if (mlpf_max > 63)
-			mlpf_max = 63;
-
-		/* rounded to the closest integer */
-		nlpf = ((mlpf * f3db * 1000) + (2766 * DS3000_XTAL_FREQ / 2))
-				/ (2766 * DS3000_XTAL_FREQ);
-		if (nlpf > 23)
-			nlpf = 23;
-		if (nlpf < 1)
-			nlpf = 1;
-
-		/* rounded to the closest integer */
-		mlpf_new = ((DS3000_XTAL_FREQ * nlpf * 2766) +
-				(1000 * f3db / 2)) / (1000 * f3db);
+	/* normalized symbol rate rounded to the closest integer */
+	value = (((c->symbol_rate / 1000) << 16) +
+			(DS3000_SAMPLE_RATE / 2)) / DS3000_SAMPLE_RATE;
+	ds3000_writereg(state, 0x61, value & 0x00ff);
+	ds3000_writereg(state, 0x62, (value & 0xff00) >> 8);
 
-		if (mlpf_new < mlpf_min) {
-			nlpf++;
-			mlpf_new = ((DS3000_XTAL_FREQ * nlpf * 2766) +
-					(1000 * f3db / 2)) / (1000 * f3db);
-		}
+	/* co-channel interference cancellation disabled */
+	ds3000_writereg(state, 0x56, 0x00);
+
+	/* equalizer disabled */
+	ds3000_writereg(state, 0x76, 0x00);
 
-		if (mlpf_new > mlpf_max)
-			mlpf_new = mlpf_max;
-
-		ds3000_tuner_writereg(state, 0x04, mlpf_new);
-		ds3000_tuner_writereg(state, 0x06, nlpf);
-		ds3000_tuner_writereg(state, 0x51, 0x1b);
-		ds3000_tuner_writereg(state, 0x51, 0x1f);
-		ds3000_tuner_writereg(state, 0x50, 0x04);
-		ds3000_tuner_writereg(state, 0x50, 0x00);
-		msleep(5);
-
-		/* unknown */
-		ds3000_tuner_writereg(state, 0x51, 0x1e);
-		ds3000_tuner_writereg(state, 0x51, 0x1f);
-		ds3000_tuner_writereg(state, 0x50, 0x01);
-		ds3000_tuner_writereg(state, 0x50, 0x00);
-		msleep(60);
-
-		/* ds3000 global reset */
-		ds3000_writereg(state, 0x07, 0x80);
-		ds3000_writereg(state, 0x07, 0x00);
-		/* ds3000 build-in uC reset */
-		ds3000_writereg(state, 0xb2, 0x01);
-		/* ds3000 software reset */
-		ds3000_writereg(state, 0x00, 0x01);
+	/*ds3000_writereg(state, 0x08, 0x03);
+	ds3000_writereg(state, 0xfd, 0x22);
+	ds3000_writereg(state, 0x08, 0x07);
+	ds3000_writereg(state, 0xfd, 0x42);
+	ds3000_writereg(state, 0x08, 0x07);*/
 
+	if (state->config->ci_mode) {
 		switch (c->delivery_system) {
 		case SYS_DVBS:
-			/* initialise the demod in DVB-S mode */
-			for (i = 0; i < sizeof(ds3000_dvbs_init_tab); i += 2)
-				ds3000_writereg(state,
-					ds3000_dvbs_init_tab[i],
-					ds3000_dvbs_init_tab[i + 1]);
-			value = ds3000_readreg(state, 0xfe);
-			value &= 0xc0;
-			value |= 0x1b;
-			ds3000_writereg(state, 0xfe, value);
-			break;
+		default:
+			ds3000_writereg(state, 0xfd, 0x80);
+		break;
 		case SYS_DVBS2:
-			/* initialise the demod in DVB-S2 mode */
-			for (i = 0; i < sizeof(ds3000_dvbs2_init_tab); i += 2)
-				ds3000_writereg(state,
-					ds3000_dvbs2_init_tab[i],
-					ds3000_dvbs2_init_tab[i + 1]);
-			ds3000_writereg(state, 0xfe, 0x54);
+			ds3000_writereg(state, 0xfd, 0x01);
 			break;
-		default:
-			return 1;
 		}
+	}
 
-		/* enable 27MHz clock output */
-		ds3000_writereg(state, 0x29, 0x80);
-		/* enable ac coupling */
-		ds3000_writereg(state, 0x25, 0x8a);
-
-		/* enhance symbol rate performance */
-		if ((state->dcur.symbol_rate / 1000) <= 5000) {
-			value = 29777 / (state->dcur.symbol_rate / 1000) + 1;
-			if (value % 2 != 0)
-				value++;
-			ds3000_writereg(state, 0xc3, 0x0d);
-			ds3000_writereg(state, 0xc8, value);
-			ds3000_writereg(state, 0xc4, 0x10);
-			ds3000_writereg(state, 0xc7, 0x0e);
-		} else if ((state->dcur.symbol_rate / 1000) <= 10000) {
-			value = 92166 / (state->dcur.symbol_rate / 1000) + 1;
-			if (value % 2 != 0)
-				value++;
-			ds3000_writereg(state, 0xc3, 0x07);
-			ds3000_writereg(state, 0xc8, value);
-			ds3000_writereg(state, 0xc4, 0x09);
-			ds3000_writereg(state, 0xc7, 0x12);
-		} else if ((state->dcur.symbol_rate / 1000) <= 20000) {
-			value = 64516 / (state->dcur.symbol_rate / 1000) + 1;
-			ds3000_writereg(state, 0xc3, value);
-			ds3000_writereg(state, 0xc8, 0x0e);
-			ds3000_writereg(state, 0xc4, 0x07);
-			ds3000_writereg(state, 0xc7, 0x18);
-		} else {
-			value = 129032 / (state->dcur.symbol_rate / 1000) + 1;
-			ds3000_writereg(state, 0xc3, value);
-			ds3000_writereg(state, 0xc8, 0x0a);
-			ds3000_writereg(state, 0xc4, 0x05);
-			ds3000_writereg(state, 0xc7, 0x24);
-		}
+	/* ds3000 out of software reset */
+	ds3000_writereg(state, 0x00, 0x00);
+	/* start ds3000 build-in uC */
+	ds3000_writereg(state, 0xb2, 0x00);
 
-		/* normalized symbol rate rounded to the closest integer */
-		value = (((state->dcur.symbol_rate / 1000) << 16) +
-				(DS3000_SAMPLE_RATE / 2)) / DS3000_SAMPLE_RATE;
-		ds3000_writereg(state, 0x61, value & 0x00ff);
-		ds3000_writereg(state, 0x62, (value & 0xff00) >> 8);
-
-		/* co-channel interference cancellation disabled */
-		ds3000_writereg(state, 0x56, 0x00);
-
-		/* equalizer disabled */
-		ds3000_writereg(state, 0x76, 0x00);
-
-		/*ds3000_writereg(state, 0x08, 0x03);
-		ds3000_writereg(state, 0xfd, 0x22);
-		ds3000_writereg(state, 0x08, 0x07);
-		ds3000_writereg(state, 0xfd, 0x42);
-		ds3000_writereg(state, 0x08, 0x07);*/
-
-		/* ds3000 out of software reset */
-		ds3000_writereg(state, 0x00, 0x00);
-		/* start ds3000 build-in uC */
-		ds3000_writereg(state, 0xb2, 0x00);
-
-		/* TODO: calculate and set carrier offset */
-
-		/* wait before retrying */
-		for (i = 0; i < 30 ; i++) {
-			if (ds3000_is_tuned(fe)) {
-				dprintk("%s: Tuned\n", __func__);
-				ds3000_dump_registers(fe);
-				goto tuned;
-			}
-			msleep(1);
-		}
+	ds3000_set_carrier_offset(fe, offset_khz);
 
-		dprintk("%s: Not tuned\n", __func__);
-		ds3000_dump_registers(fe);
+	for (i = 0; i < 30 ; i++) {
+		ds3000_read_status(fe, &status);
+		if (status && FE_HAS_LOCK)
+			break;
 
-	} while (--retune);
+		msleep(10);
+	}
 
-tuned:
-	return ret;
+	return 0;
+}
+
+static int ds3000_tune(struct dvb_frontend *fe,
+			struct dvb_frontend_parameters *p,
+			unsigned int mode_flags,
+			unsigned int *delay,
+			fe_status_t *status)
+{
+	if (p) {
+		int ret = ds3000_set_frontend(fe, p);
+		if (ret)
+			return ret;
+	}
+
+	*delay = HZ / 5;
+
+	return ds3000_read_status(fe, status);
 }
 
 static enum dvbfe_algo ds3000_get_algo(struct dvb_frontend *fe)
 {
 	dprintk("%s()\n", __func__);
-	return DVBFE_ALGO_SW;
+	return DVBFE_ALGO_HW;
 }
 
 /*
@@ -1306,7 +1249,25 @@ static enum dvbfe_algo ds3000_get_algo(struct dvb_frontend *fe)
  */
 static int ds3000_initfe(struct dvb_frontend *fe)
 {
+	struct ds3000_state *state = fe->demodulator_priv;
+	int ret;
+
 	dprintk("%s()\n", __func__);
+	/* hard reset */
+	ds3000_writereg(state, 0x08, 0x01 | ds3000_readreg(state, 0x08));
+	msleep(1);
+
+	/* TS2020 init */
+	ds3000_tuner_writereg(state, 0x42, 0x73);
+	ds3000_tuner_writereg(state, 0x05, 0x01);
+	ds3000_tuner_writereg(state, 0x62, 0xf5);
+	/* Load the firmware if required */
+	ret = ds3000_firmware_ondemand(fe);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: Unable initialize firmware\n", __func__);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -1345,6 +1306,7 @@ static struct dvb_frontend_ops ds3000_ops = {
 	.read_signal_strength = ds3000_read_signal_strength,
 	.read_snr = ds3000_read_snr,
 	.read_ucblocks = ds3000_read_ucblocks,
+	.set_voltage = ds3000_set_voltage,
 	.set_tone = ds3000_set_tone,
 	.diseqc_send_master_cmd = ds3000_send_diseqc_msg,
 	.diseqc_send_burst = ds3000_diseqc_send_burst,
@@ -1352,7 +1314,8 @@ static struct dvb_frontend_ops ds3000_ops = {
 
 	.set_property = ds3000_set_property,
 	.get_property = ds3000_get_property,
-	.set_frontend = ds3000_tune,
+	.set_frontend = ds3000_set_frontend,
+	.tune = ds3000_tune,
 };
 
 module_param(debug, int, 0644);
diff --git a/drivers/media/dvb/frontends/ds3000.h b/drivers/media/dvb/frontends/ds3000.h
index 67f67038740a..1b736888ea37 100644
--- a/drivers/media/dvb/frontends/ds3000.h
+++ b/drivers/media/dvb/frontends/ds3000.h
@@ -27,6 +27,9 @@
 struct ds3000_config {
 	/* the demodulator's i2c address */
 	u8 demod_address;
+	u8 ci_mode;
+	/* Set device param to start dma */
+	int (*set_ts_params)(struct dvb_frontend *fe, int is_punctured);
 };
 
 #if defined(CONFIG_DVB_DS3000) || \
diff --git a/drivers/media/dvb/frontends/dvb-pll.c b/drivers/media/dvb/frontends/dvb-pll.c
index 4d4d0bb5920a..62a65efdf8d6 100644
--- a/drivers/media/dvb/frontends/dvb-pll.c
+++ b/drivers/media/dvb/frontends/dvb-pll.c
@@ -64,6 +64,7 @@ struct dvb_pll_desc {
 	void (*set)(struct dvb_frontend *fe, u8 *buf,
 		    const struct dvb_frontend_parameters *params);
 	u8   *initdata;
+	u8   *initdata2;
 	u8   *sleepdata;
 	int  count;
 	struct {
@@ -321,26 +322,73 @@ static struct dvb_pll_desc dvb_pll_philips_sd1878_tda8261 = {
 static void opera1_bw(struct dvb_frontend *fe, u8 *buf,
 		      const struct dvb_frontend_parameters *params)
 {
-	if (params->u.ofdm.bandwidth == BANDWIDTH_8_MHZ)
-		buf[2] |= 0x08;
+	struct dvb_pll_priv *priv = fe->tuner_priv;
+	u32 b_w  = (params->u.qpsk.symbol_rate * 27) / 32000;
+	struct i2c_msg msg = {
+		.addr = priv->pll_i2c_address,
+		.flags = 0,
+		.buf = buf,
+		.len = 4
+	};
+	int result;
+	u8 lpf;
+
+	if (fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+
+	result = i2c_transfer(priv->i2c, &msg, 1);
+	if (result != 1)
+		printk(KERN_ERR "%s: i2c_transfer failed:%d",
+			__func__, result);
+
+	if (b_w <= 10000)
+		lpf = 0xc;
+	else if (b_w <= 12000)
+		lpf = 0x2;
+	else if (b_w <= 14000)
+		lpf = 0xa;
+	else if (b_w <= 16000)
+		lpf = 0x6;
+	else if (b_w <= 18000)
+		lpf = 0xe;
+	else if (b_w <= 20000)
+		lpf = 0x1;
+	else if (b_w <= 22000)
+		lpf = 0x9;
+	else if (b_w <= 24000)
+		lpf = 0x5;
+	else if (b_w <= 26000)
+		lpf = 0xd;
+	else if (b_w <= 28000)
+		lpf = 0x3;
+		else
+		lpf = 0xb;
+	buf[2] ^= 0x1c; /* Flip bits 3-5 */
+	/* Set lpf */
+	buf[2] |= ((lpf >> 2) & 0x3) << 3;
+	buf[3] |= (lpf & 0x3) << 2;
+
+	return;
 }
 
 static struct dvb_pll_desc dvb_pll_opera1 = {
 	.name  = "Opera Tuner",
 	.min   =  900000,
 	.max   = 2250000,
+	.initdata = (u8[]){ 4, 0x08, 0xe5, 0xe1, 0x00 },
+	.initdata2 = (u8[]){ 4, 0x08, 0xe5, 0xe5, 0x00 },
 	.iffreq= 0,
 	.set   = opera1_bw,
 	.count = 8,
 	.entries = {
-		{ 1064000, 500, 0xe5, 0xc6 },
-		{ 1169000, 500, 0xe5, 0xe6 },
-		{ 1299000, 500, 0xe5, 0x24 },
-		{ 1444000, 500, 0xe5, 0x44 },
-		{ 1606000, 500, 0xe5, 0x64 },
-		{ 1777000, 500, 0xe5, 0x84 },
-		{ 1941000, 500, 0xe5, 0xa4 },
-		{ 2250000, 500, 0xe5, 0xc4 },
+		{ 1064000, 500, 0xf9, 0xc2 },
+		{ 1169000, 500, 0xf9, 0xe2 },
+		{ 1299000, 500, 0xf9, 0x20 },
+		{ 1444000, 500, 0xf9, 0x40 },
+		{ 1606000, 500, 0xf9, 0x60 },
+		{ 1777000, 500, 0xf9, 0x80 },
+		{ 1941000, 500, 0xf9, 0xa0 },
+		{ 2250000, 500, 0xf9, 0xc0 },
 	}
 };
 
@@ -648,8 +696,17 @@ static int dvb_pll_init(struct dvb_frontend *fe)
 		int result;
 		if (fe->ops.i2c_gate_ctrl)
 			fe->ops.i2c_gate_ctrl(fe, 1);
-		if ((result = i2c_transfer(priv->i2c, &msg, 1)) != 1) {
+		result = i2c_transfer(priv->i2c, &msg, 1);
+		if (result != 1)
 			return result;
+		if (priv->pll_desc->initdata2) {
+			msg.buf = priv->pll_desc->initdata2 + 1;
+			msg.len = priv->pll_desc->initdata2[0];
+			if (fe->ops.i2c_gate_ctrl)
+				fe->ops.i2c_gate_ctrl(fe, 1);
+			result = i2c_transfer(priv->i2c, &msg, 1);
+			if (result != 1)
+				return result;
 		}
 		return 0;
 	}
diff --git a/drivers/media/dvb/frontends/stv0367.c b/drivers/media/dvb/frontends/stv0367.c
new file mode 100644
index 000000000000..eecdf23642eb
--- /dev/null
+++ b/drivers/media/dvb/frontends/stv0367.c
@@ -0,0 +1,3441 @@
+/*
+ * stv0367.c
+ *
+ * Driver for ST STV0367 DVB-T & DVB-C demodulator IC.
+ *
+ * Copyright (C) ST Microelectronics.
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+
+#include "stv0367.h"
+#include "stv0367_regs.h"
+#include "stv0367_priv.h"
+
+static int stvdebug;
+module_param_named(debug, stvdebug, int, 0644);
+
+static int i2cdebug;
+module_param_named(i2c_debug, i2cdebug, int, 0644);
+
+#define dprintk(args...) \
+	do { \
+		if (stvdebug) \
+			printk(KERN_DEBUG args); \
+	} while (0)
+	/* DVB-C */
+
+struct stv0367cab_state {
+	enum stv0367_cab_signal_type	state;
+	u32	mclk;
+	u32	adc_clk;
+	s32	search_range;
+	s32	derot_offset;
+	/* results */
+	int locked;			/* channel found		*/
+	u32 freq_khz;			/* found frequency (in kHz)	*/
+	u32 symbol_rate;		/* found symbol rate (in Bds)	*/
+	enum stv0367cab_mod modulation;	/* modulation			*/
+	fe_spectral_inversion_t	spect_inv; /* Spectrum Inversion	*/
+};
+
+struct stv0367ter_state {
+	/* DVB-T */
+	enum stv0367_ter_signal_type state;
+	enum stv0367_ter_if_iq_mode if_iq_mode;
+	enum stv0367_ter_mode mode;/* mode 2K or 8K */
+	fe_guard_interval_t guard;
+	enum stv0367_ter_hierarchy hierarchy;
+	u32 frequency;
+	fe_spectral_inversion_t  sense; /*  current search spectrum */
+	u8  force; /* force mode/guard */
+	u8  bw; /* channel width 6, 7 or 8 in MHz */
+	u8  pBW; /* channel width used during previous lock */
+	u32 pBER;
+	u32 pPER;
+	u32 ucblocks;
+	s8  echo_pos; /* echo position */
+	u8  first_lock;
+	u8  unlock_counter;
+	u32 agc_val;
+};
+
+struct stv0367_state {
+	struct dvb_frontend fe;
+	struct i2c_adapter *i2c;
+	/* config settings */
+	const struct stv0367_config *config;
+	u8 chip_id;
+	/* DVB-C */
+	struct stv0367cab_state *cab_state;
+	/* DVB-T */
+	struct stv0367ter_state *ter_state;
+};
+
+struct st_register {
+	u16	addr;
+	u8	value;
+};
+
+/* values for STV4100 XTAL=30M int clk=53.125M*/
+static struct st_register def0367ter[STV0367TER_NBREGS] = {
+	{R367TER_ID,		0x60},
+	{R367TER_I2CRPT,	0xa0},
+	/* {R367TER_I2CRPT,	0x22},*/
+	{R367TER_TOPCTRL,	0x00},/* for xc5000; was 0x02 */
+	{R367TER_IOCFG0,	0x40},
+	{R367TER_DAC0R,		0x00},
+	{R367TER_IOCFG1,	0x00},
+	{R367TER_DAC1R,		0x00},
+	{R367TER_IOCFG2,	0x62},
+	{R367TER_SDFR,		0x00},
+	{R367TER_STATUS,	0xf8},
+	{R367TER_AUX_CLK,	0x0a},
+	{R367TER_FREESYS1,	0x00},
+	{R367TER_FREESYS2,	0x00},
+	{R367TER_FREESYS3,	0x00},
+	{R367TER_GPIO_CFG,	0x55},
+	{R367TER_GPIO_CMD,	0x00},
+	{R367TER_AGC2MAX,	0xff},
+	{R367TER_AGC2MIN,	0x00},
+	{R367TER_AGC1MAX,	0xff},
+	{R367TER_AGC1MIN,	0x00},
+	{R367TER_AGCR,		0xbc},
+	{R367TER_AGC2TH,	0x00},
+	{R367TER_AGC12C,	0x00},
+	{R367TER_AGCCTRL1,	0x85},
+	{R367TER_AGCCTRL2,	0x1f},
+	{R367TER_AGC1VAL1,	0x00},
+	{R367TER_AGC1VAL2,	0x00},
+	{R367TER_AGC2VAL1,	0x6f},
+	{R367TER_AGC2VAL2,	0x05},
+	{R367TER_AGC2PGA,	0x00},
+	{R367TER_OVF_RATE1,	0x00},
+	{R367TER_OVF_RATE2,	0x00},
+	{R367TER_GAIN_SRC1,	0xaa},/* for xc5000; was 0x2b */
+	{R367TER_GAIN_SRC2,	0xd6},/* for xc5000; was 0x04 */
+	{R367TER_INC_DEROT1,	0x55},
+	{R367TER_INC_DEROT2,	0x55},
+	{R367TER_PPM_CPAMP_DIR,	0x2c},
+	{R367TER_PPM_CPAMP_INV,	0x00},
+	{R367TER_FREESTFE_1,	0x00},
+	{R367TER_FREESTFE_2,	0x1c},
+	{R367TER_DCOFFSET,	0x00},
+	{R367TER_EN_PROCESS,	0x05},
+	{R367TER_SDI_SMOOTHER,	0x80},
+	{R367TER_FE_LOOP_OPEN,	0x1c},
+	{R367TER_FREQOFF1,	0x00},
+	{R367TER_FREQOFF2,	0x00},
+	{R367TER_FREQOFF3,	0x00},
+	{R367TER_TIMOFF1,	0x00},
+	{R367TER_TIMOFF2,	0x00},
+	{R367TER_EPQ,		0x02},
+	{R367TER_EPQAUTO,	0x01},
+	{R367TER_SYR_UPDATE,	0xf5},
+	{R367TER_CHPFREE,	0x00},
+	{R367TER_PPM_STATE_MAC,	0x23},
+	{R367TER_INR_THRESHOLD,	0xff},
+	{R367TER_EPQ_TPS_ID_CELL, 0xf9},
+	{R367TER_EPQ_CFG,	0x00},
+	{R367TER_EPQ_STATUS,	0x01},
+	{R367TER_AUTORELOCK,	0x81},
+	{R367TER_BER_THR_VMSB,	0x00},
+	{R367TER_BER_THR_MSB,	0x00},
+	{R367TER_BER_THR_LSB,	0x00},
+	{R367TER_CCD,		0x83},
+	{R367TER_SPECTR_CFG,	0x00},
+	{R367TER_CHC_DUMMY,	0x18},
+	{R367TER_INC_CTL,	0x88},
+	{R367TER_INCTHRES_COR1,	0xb4},
+	{R367TER_INCTHRES_COR2,	0x96},
+	{R367TER_INCTHRES_DET1,	0x0e},
+	{R367TER_INCTHRES_DET2,	0x11},
+	{R367TER_IIR_CELLNB,	0x8d},
+	{R367TER_IIRCX_COEFF1_MSB, 0x00},
+	{R367TER_IIRCX_COEFF1_LSB, 0x00},
+	{R367TER_IIRCX_COEFF2_MSB, 0x09},
+	{R367TER_IIRCX_COEFF2_LSB, 0x18},
+	{R367TER_IIRCX_COEFF3_MSB, 0x14},
+	{R367TER_IIRCX_COEFF3_LSB, 0x9c},
+	{R367TER_IIRCX_COEFF4_MSB, 0x00},
+	{R367TER_IIRCX_COEFF4_LSB, 0x00},
+	{R367TER_IIRCX_COEFF5_MSB, 0x36},
+	{R367TER_IIRCX_COEFF5_LSB, 0x42},
+	{R367TER_FEPATH_CFG,	0x00},
+	{R367TER_PMC1_FUNC,	0x65},
+	{R367TER_PMC1_FOR,	0x00},
+	{R367TER_PMC2_FUNC,	0x00},
+	{R367TER_STATUS_ERR_DA,	0xe0},
+	{R367TER_DIG_AGC_R,	0xfe},
+	{R367TER_COMAGC_TARMSB,	0x0b},
+	{R367TER_COM_AGC_TAR_ENMODE, 0x41},
+	{R367TER_COM_AGC_CFG,	0x3e},
+	{R367TER_COM_AGC_GAIN1, 0x39},
+	{R367TER_AUT_AGC_TARGETMSB, 0x0b},
+	{R367TER_LOCK_DET_MSB,	0x01},
+	{R367TER_AGCTAR_LOCK_LSBS, 0x40},
+	{R367TER_AUT_GAIN_EN,	0xf4},
+	{R367TER_AUT_CFG,	0xf0},
+	{R367TER_LOCKN,		0x23},
+	{R367TER_INT_X_3,	0x00},
+	{R367TER_INT_X_2,	0x03},
+	{R367TER_INT_X_1,	0x8d},
+	{R367TER_INT_X_0,	0xa0},
+	{R367TER_MIN_ERRX_MSB,	0x00},
+	{R367TER_COR_CTL,	0x23},
+	{R367TER_COR_STAT,	0xf6},
+	{R367TER_COR_INTEN,	0x00},
+	{R367TER_COR_INTSTAT,	0x3f},
+	{R367TER_COR_MODEGUARD,	0x03},
+	{R367TER_AGC_CTL,	0x08},
+	{R367TER_AGC_MANUAL1,	0x00},
+	{R367TER_AGC_MANUAL2,	0x00},
+	{R367TER_AGC_TARG,	0x16},
+	{R367TER_AGC_GAIN1,	0x53},
+	{R367TER_AGC_GAIN2,	0x1d},
+	{R367TER_RESERVED_1,	0x00},
+	{R367TER_RESERVED_2,	0x00},
+	{R367TER_RESERVED_3,	0x00},
+	{R367TER_CAS_CTL,	0x44},
+	{R367TER_CAS_FREQ,	0xb3},
+	{R367TER_CAS_DAGCGAIN,	0x12},
+	{R367TER_SYR_CTL,	0x04},
+	{R367TER_SYR_STAT,	0x10},
+	{R367TER_SYR_NCO1,	0x00},
+	{R367TER_SYR_NCO2,	0x00},
+	{R367TER_SYR_OFFSET1,	0x00},
+	{R367TER_SYR_OFFSET2,	0x00},
+	{R367TER_FFT_CTL,	0x00},
+	{R367TER_SCR_CTL,	0x70},
+	{R367TER_PPM_CTL1,	0xf8},
+	{R367TER_TRL_CTL,	0x14},/* for xc5000; was 0xac */
+	{R367TER_TRL_NOMRATE1,	0xae},/* for xc5000; was 0x1e */
+	{R367TER_TRL_NOMRATE2,	0x56},/* for xc5000; was 0x58 */
+	{R367TER_TRL_TIME1,	0x1d},
+	{R367TER_TRL_TIME2,	0xfc},
+	{R367TER_CRL_CTL,	0x24},
+	{R367TER_CRL_FREQ1,	0xad},
+	{R367TER_CRL_FREQ2,	0x9d},
+	{R367TER_CRL_FREQ3,	0xff},
+	{R367TER_CHC_CTL,	0x01},
+	{R367TER_CHC_SNR,	0xf0},
+	{R367TER_BDI_CTL,	0x00},
+	{R367TER_DMP_CTL,	0x00},
+	{R367TER_TPS_RCVD1,	0x30},
+	{R367TER_TPS_RCVD2,	0x02},
+	{R367TER_TPS_RCVD3,	0x01},
+	{R367TER_TPS_RCVD4,	0x00},
+	{R367TER_TPS_ID_CELL1,	0x00},
+	{R367TER_TPS_ID_CELL2,	0x00},
+	{R367TER_TPS_RCVD5_SET1, 0x02},
+	{R367TER_TPS_SET2,	0x02},
+	{R367TER_TPS_SET3,	0x01},
+	{R367TER_TPS_CTL,	0x00},
+	{R367TER_CTL_FFTOSNUM,	0x34},
+	{R367TER_TESTSELECT,	0x09},
+	{R367TER_MSC_REV,	0x0a},
+	{R367TER_PIR_CTL,	0x00},
+	{R367TER_SNR_CARRIER1,	0xa1},
+	{R367TER_SNR_CARRIER2,	0x9a},
+	{R367TER_PPM_CPAMP,	0x2c},
+	{R367TER_TSM_AP0,	0x00},
+	{R367TER_TSM_AP1,	0x00},
+	{R367TER_TSM_AP2 ,	0x00},
+	{R367TER_TSM_AP3,	0x00},
+	{R367TER_TSM_AP4,	0x00},
+	{R367TER_TSM_AP5,	0x00},
+	{R367TER_TSM_AP6,	0x00},
+	{R367TER_TSM_AP7,	0x00},
+	{R367TER_TSTRES,	0x00},
+	{R367TER_ANACTRL,	0x0D},/* PLL stoped, restart at init!!! */
+	{R367TER_TSTBUS,	0x00},
+	{R367TER_TSTRATE,	0x00},
+	{R367TER_CONSTMODE,	0x01},
+	{R367TER_CONSTCARR1,	0x00},
+	{R367TER_CONSTCARR2,	0x00},
+	{R367TER_ICONSTEL,	0x0a},
+	{R367TER_QCONSTEL,	0x15},
+	{R367TER_TSTBISTRES0,	0x00},
+	{R367TER_TSTBISTRES1,	0x00},
+	{R367TER_TSTBISTRES2,	0x28},
+	{R367TER_TSTBISTRES3,	0x00},
+	{R367TER_RF_AGC1,	0xff},
+	{R367TER_RF_AGC2,	0x83},
+	{R367TER_ANADIGCTRL,	0x19},
+	{R367TER_PLLMDIV,	0x01},/* for xc5000; was 0x0c */
+	{R367TER_PLLNDIV,	0x06},/* for xc5000; was 0x55 */
+	{R367TER_PLLSETUP,	0x18},
+	{R367TER_DUAL_AD12,	0x04},/* for xc5000; was 0x00 */
+	{R367TER_TSTBIST,	0x00},
+	{R367TER_PAD_COMP_CTRL,	0x00},
+	{R367TER_PAD_COMP_WR,	0x00},
+	{R367TER_PAD_COMP_RD,	0xe0},
+	{R367TER_SYR_TARGET_FFTADJT_MSB, 0x00},
+	{R367TER_SYR_TARGET_FFTADJT_LSB, 0x00},
+	{R367TER_SYR_TARGET_CHCADJT_MSB, 0x00},
+	{R367TER_SYR_TARGET_CHCADJT_LSB, 0x00},
+	{R367TER_SYR_FLAG,	0x00},
+	{R367TER_CRL_TARGET1,	0x00},
+	{R367TER_CRL_TARGET2,	0x00},
+	{R367TER_CRL_TARGET3,	0x00},
+	{R367TER_CRL_TARGET4,	0x00},
+	{R367TER_CRL_FLAG,	0x00},
+	{R367TER_TRL_TARGET1,	0x00},
+	{R367TER_TRL_TARGET2,	0x00},
+	{R367TER_TRL_CHC,	0x00},
+	{R367TER_CHC_SNR_TARG,	0x00},
+	{R367TER_TOP_TRACK,	0x00},
+	{R367TER_TRACKER_FREE1,	0x00},
+	{R367TER_ERROR_CRL1,	0x00},
+	{R367TER_ERROR_CRL2,	0x00},
+	{R367TER_ERROR_CRL3,	0x00},
+	{R367TER_ERROR_CRL4,	0x00},
+	{R367TER_DEC_NCO1,	0x2c},
+	{R367TER_DEC_NCO2,	0x0f},
+	{R367TER_DEC_NCO3,	0x20},
+	{R367TER_SNR,		0xf1},
+	{R367TER_SYR_FFTADJ1,	0x00},
+	{R367TER_SYR_FFTADJ2,	0x00},
+	{R367TER_SYR_CHCADJ1,	0x00},
+	{R367TER_SYR_CHCADJ2,	0x00},
+	{R367TER_SYR_OFF,	0x00},
+	{R367TER_PPM_OFFSET1,	0x00},
+	{R367TER_PPM_OFFSET2,	0x03},
+	{R367TER_TRACKER_FREE2,	0x00},
+	{R367TER_DEBG_LT10,	0x00},
+	{R367TER_DEBG_LT11,	0x00},
+	{R367TER_DEBG_LT12,	0x00},
+	{R367TER_DEBG_LT13,	0x00},
+	{R367TER_DEBG_LT14,	0x00},
+	{R367TER_DEBG_LT15,	0x00},
+	{R367TER_DEBG_LT16,	0x00},
+	{R367TER_DEBG_LT17,	0x00},
+	{R367TER_DEBG_LT18,	0x00},
+	{R367TER_DEBG_LT19,	0x00},
+	{R367TER_DEBG_LT1A,	0x00},
+	{R367TER_DEBG_LT1B,	0x00},
+	{R367TER_DEBG_LT1C,	0x00},
+	{R367TER_DEBG_LT1D,	0x00},
+	{R367TER_DEBG_LT1E,	0x00},
+	{R367TER_DEBG_LT1F,	0x00},
+	{R367TER_RCCFGH,	0x00},
+	{R367TER_RCCFGM,	0x00},
+	{R367TER_RCCFGL,	0x00},
+	{R367TER_RCINSDELH,	0x00},
+	{R367TER_RCINSDELM,	0x00},
+	{R367TER_RCINSDELL,	0x00},
+	{R367TER_RCSTATUS,	0x00},
+	{R367TER_RCSPEED,	0x6f},
+	{R367TER_RCDEBUGM,	0xe7},
+	{R367TER_RCDEBUGL,	0x9b},
+	{R367TER_RCOBSCFG,	0x00},
+	{R367TER_RCOBSM,	0x00},
+	{R367TER_RCOBSL,	0x00},
+	{R367TER_RCFECSPY,	0x00},
+	{R367TER_RCFSPYCFG,	0x00},
+	{R367TER_RCFSPYDATA,	0x00},
+	{R367TER_RCFSPYOUT,	0x00},
+	{R367TER_RCFSTATUS,	0x00},
+	{R367TER_RCFGOODPACK,	0x00},
+	{R367TER_RCFPACKCNT,	0x00},
+	{R367TER_RCFSPYMISC,	0x00},
+	{R367TER_RCFBERCPT4,	0x00},
+	{R367TER_RCFBERCPT3,	0x00},
+	{R367TER_RCFBERCPT2,	0x00},
+	{R367TER_RCFBERCPT1,	0x00},
+	{R367TER_RCFBERCPT0,	0x00},
+	{R367TER_RCFBERERR2,	0x00},
+	{R367TER_RCFBERERR1,	0x00},
+	{R367TER_RCFBERERR0,	0x00},
+	{R367TER_RCFSTATESM,	0x00},
+	{R367TER_RCFSTATESL,	0x00},
+	{R367TER_RCFSPYBER,	0x00},
+	{R367TER_RCFSPYDISTM,	0x00},
+	{R367TER_RCFSPYDISTL,	0x00},
+	{R367TER_RCFSPYOBS7,	0x00},
+	{R367TER_RCFSPYOBS6,	0x00},
+	{R367TER_RCFSPYOBS5,	0x00},
+	{R367TER_RCFSPYOBS4,	0x00},
+	{R367TER_RCFSPYOBS3,	0x00},
+	{R367TER_RCFSPYOBS2,	0x00},
+	{R367TER_RCFSPYOBS1,	0x00},
+	{R367TER_RCFSPYOBS0,	0x00},
+	{R367TER_TSGENERAL,	0x00},
+	{R367TER_RC1SPEED,	0x6f},
+	{R367TER_TSGSTATUS,	0x18},
+	{R367TER_FECM,		0x01},
+	{R367TER_VTH12,		0xff},
+	{R367TER_VTH23,		0xa1},
+	{R367TER_VTH34,		0x64},
+	{R367TER_VTH56,		0x40},
+	{R367TER_VTH67,		0x00},
+	{R367TER_VTH78,		0x2c},
+	{R367TER_VITCURPUN,	0x12},
+	{R367TER_VERROR,	0x01},
+	{R367TER_PRVIT,		0x3f},
+	{R367TER_VAVSRVIT,	0x00},
+	{R367TER_VSTATUSVIT,	0xbd},
+	{R367TER_VTHINUSE,	0xa1},
+	{R367TER_KDIV12,	0x20},
+	{R367TER_KDIV23,	0x40},
+	{R367TER_KDIV34,	0x20},
+	{R367TER_KDIV56,	0x30},
+	{R367TER_KDIV67,	0x00},
+	{R367TER_KDIV78,	0x30},
+	{R367TER_SIGPOWER,	0x54},
+	{R367TER_DEMAPVIT,	0x40},
+	{R367TER_VITSCALE,	0x00},
+	{R367TER_FFEC1PRG,	0x00},
+	{R367TER_FVITCURPUN,	0x12},
+	{R367TER_FVERROR,	0x01},
+	{R367TER_FVSTATUSVIT,	0xbd},
+	{R367TER_DEBUG_LT1,	0x00},
+	{R367TER_DEBUG_LT2,	0x00},
+	{R367TER_DEBUG_LT3,	0x00},
+	{R367TER_TSTSFMET,	0x00},
+	{R367TER_SELOUT,	0x00},
+	{R367TER_TSYNC,		0x00},
+	{R367TER_TSTERR,	0x00},
+	{R367TER_TSFSYNC,	0x00},
+	{R367TER_TSTSFERR,	0x00},
+	{R367TER_TSTTSSF1,	0x01},
+	{R367TER_TSTTSSF2,	0x1f},
+	{R367TER_TSTTSSF3,	0x00},
+	{R367TER_TSTTS1,	0x00},
+	{R367TER_TSTTS2,	0x1f},
+	{R367TER_TSTTS3,	0x01},
+	{R367TER_TSTTS4,	0x00},
+	{R367TER_TSTTSRC,	0x00},
+	{R367TER_TSTTSRS,	0x00},
+	{R367TER_TSSTATEM,	0xb0},
+	{R367TER_TSSTATEL,	0x40},
+	{R367TER_TSCFGH,	0xC0},
+	{R367TER_TSCFGM,	0xc0},/* for xc5000; was 0x00 */
+	{R367TER_TSCFGL,	0x20},
+	{R367TER_TSSYNC,	0x00},
+	{R367TER_TSINSDELH,	0x00},
+	{R367TER_TSINSDELM,	0x00},
+	{R367TER_TSINSDELL,	0x00},
+	{R367TER_TSDIVN,	0x03},
+	{R367TER_TSDIVPM,	0x00},
+	{R367TER_TSDIVPL,	0x00},
+	{R367TER_TSDIVQM,	0x00},
+	{R367TER_TSDIVQL,	0x00},
+	{R367TER_TSDILSTKM,	0x00},
+	{R367TER_TSDILSTKL,	0x00},
+	{R367TER_TSSPEED,	0x40},/* for xc5000; was 0x6f */
+	{R367TER_TSSTATUS,	0x81},
+	{R367TER_TSSTATUS2,	0x6a},
+	{R367TER_TSBITRATEM,	0x0f},
+	{R367TER_TSBITRATEL,	0xc6},
+	{R367TER_TSPACKLENM,	0x00},
+	{R367TER_TSPACKLENL,	0xfc},
+	{R367TER_TSBLOCLENM,	0x0a},
+	{R367TER_TSBLOCLENL,	0x80},
+	{R367TER_TSDLYH,	0x90},
+	{R367TER_TSDLYM,	0x68},
+	{R367TER_TSDLYL,	0x01},
+	{R367TER_TSNPDAV,	0x00},
+	{R367TER_TSBUFSTATH,	0x00},
+	{R367TER_TSBUFSTATM,	0x00},
+	{R367TER_TSBUFSTATL,	0x00},
+	{R367TER_TSDEBUGM,	0xcf},
+	{R367TER_TSDEBUGL,	0x1e},
+	{R367TER_TSDLYSETH,	0x00},
+	{R367TER_TSDLYSETM,	0x68},
+	{R367TER_TSDLYSETL,	0x00},
+	{R367TER_TSOBSCFG,	0x00},
+	{R367TER_TSOBSM,	0x47},
+	{R367TER_TSOBSL,	0x1f},
+	{R367TER_ERRCTRL1,	0x95},
+	{R367TER_ERRCNT1H,	0x80},
+	{R367TER_ERRCNT1M,	0x00},
+	{R367TER_ERRCNT1L,	0x00},
+	{R367TER_ERRCTRL2,	0x95},
+	{R367TER_ERRCNT2H,	0x00},
+	{R367TER_ERRCNT2M,	0x00},
+	{R367TER_ERRCNT2L,	0x00},
+	{R367TER_FECSPY,	0x88},
+	{R367TER_FSPYCFG,	0x2c},
+	{R367TER_FSPYDATA,	0x3a},
+	{R367TER_FSPYOUT,	0x06},
+	{R367TER_FSTATUS,	0x61},
+	{R367TER_FGOODPACK,	0xff},
+	{R367TER_FPACKCNT,	0xff},
+	{R367TER_FSPYMISC,	0x66},
+	{R367TER_FBERCPT4,	0x00},
+	{R367TER_FBERCPT3,	0x00},
+	{R367TER_FBERCPT2,	0x36},
+	{R367TER_FBERCPT1,	0x36},
+	{R367TER_FBERCPT0,	0x14},
+	{R367TER_FBERERR2,	0x00},
+	{R367TER_FBERERR1,	0x03},
+	{R367TER_FBERERR0,	0x28},
+	{R367TER_FSTATESM,	0x00},
+	{R367TER_FSTATESL,	0x02},
+	{R367TER_FSPYBER,	0x00},
+	{R367TER_FSPYDISTM,	0x01},
+	{R367TER_FSPYDISTL,	0x9f},
+	{R367TER_FSPYOBS7,	0xc9},
+	{R367TER_FSPYOBS6,	0x99},
+	{R367TER_FSPYOBS5,	0x08},
+	{R367TER_FSPYOBS4,	0xec},
+	{R367TER_FSPYOBS3,	0x01},
+	{R367TER_FSPYOBS2,	0x0f},
+	{R367TER_FSPYOBS1,	0xf5},
+	{R367TER_FSPYOBS0,	0x08},
+	{R367TER_SFDEMAP,	0x40},
+	{R367TER_SFERROR,	0x00},
+	{R367TER_SFAVSR,	0x30},
+	{R367TER_SFECSTATUS,	0xcc},
+	{R367TER_SFKDIV12,	0x20},
+	{R367TER_SFKDIV23,	0x40},
+	{R367TER_SFKDIV34,	0x20},
+	{R367TER_SFKDIV56,	0x20},
+	{R367TER_SFKDIV67,	0x00},
+	{R367TER_SFKDIV78,	0x20},
+	{R367TER_SFDILSTKM,	0x00},
+	{R367TER_SFDILSTKL,	0x00},
+	{R367TER_SFSTATUS,	0xb5},
+	{R367TER_SFDLYH,	0x90},
+	{R367TER_SFDLYM,	0x60},
+	{R367TER_SFDLYL,	0x01},
+	{R367TER_SFDLYSETH,	0xc0},
+	{R367TER_SFDLYSETM,	0x60},
+	{R367TER_SFDLYSETL,	0x00},
+	{R367TER_SFOBSCFG,	0x00},
+	{R367TER_SFOBSM,	0x47},
+	{R367TER_SFOBSL,	0x05},
+	{R367TER_SFECINFO,	0x40},
+	{R367TER_SFERRCTRL,	0x74},
+	{R367TER_SFERRCNTH,	0x80},
+	{R367TER_SFERRCNTM ,	0x00},
+	{R367TER_SFERRCNTL,	0x00},
+	{R367TER_SYMBRATEM,	0x2f},
+	{R367TER_SYMBRATEL,	0x50},
+	{R367TER_SYMBSTATUS,	0x7f},
+	{R367TER_SYMBCFG,	0x00},
+	{R367TER_SYMBFIFOM,	0xf4},
+	{R367TER_SYMBFIFOL,	0x0d},
+	{R367TER_SYMBOFFSM,	0xf0},
+	{R367TER_SYMBOFFSL,	0x2d},
+	{R367TER_DEBUG_LT4,	0x00},
+	{R367TER_DEBUG_LT5,	0x00},
+	{R367TER_DEBUG_LT6,	0x00},
+	{R367TER_DEBUG_LT7,	0x00},
+	{R367TER_DEBUG_LT8,	0x00},
+	{R367TER_DEBUG_LT9,	0x00},
+};
+
+#define RF_LOOKUP_TABLE_SIZE  31
+#define RF_LOOKUP_TABLE2_SIZE 16
+/* RF Level (for RF AGC->AGC1) Lookup Table, depends on the board and tuner.*/
+s32 stv0367cab_RF_LookUp1[RF_LOOKUP_TABLE_SIZE][RF_LOOKUP_TABLE_SIZE] = {
+	{/*AGC1*/
+		48, 50, 51, 53, 54, 56, 57, 58, 60, 61, 62, 63,
+		64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75,
+		76, 77, 78, 80, 83, 85, 88,
+	}, {/*RF(dbm)*/
+		22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33,
+		34, 35, 36, 37, 38, 39, 41, 42, 43, 44, 46, 47,
+		49, 50, 52, 53, 54, 55, 56,
+	}
+};
+/* RF Level (for IF AGC->AGC2) Lookup Table, depends on the board and tuner.*/
+s32 stv0367cab_RF_LookUp2[RF_LOOKUP_TABLE2_SIZE][RF_LOOKUP_TABLE2_SIZE] = {
+	{/*AGC2*/
+		28, 29, 31, 32, 34, 35, 36, 37,
+		38, 39, 40, 41, 42, 43, 44, 45,
+	}, {/*RF(dbm)*/
+		57, 58, 59, 60, 61, 62, 63, 64,
+		65, 66, 67, 68, 69, 70, 71, 72,
+	}
+};
+
+static struct st_register def0367cab[STV0367CAB_NBREGS] = {
+	{R367CAB_ID,		0x60},
+	{R367CAB_I2CRPT,	0xa0},
+	/*{R367CAB_I2CRPT,	0x22},*/
+	{R367CAB_TOPCTRL,	0x10},
+	{R367CAB_IOCFG0,	0x80},
+	{R367CAB_DAC0R,		0x00},
+	{R367CAB_IOCFG1,	0x00},
+	{R367CAB_DAC1R,		0x00},
+	{R367CAB_IOCFG2,	0x00},
+	{R367CAB_SDFR,		0x00},
+	{R367CAB_AUX_CLK,	0x00},
+	{R367CAB_FREESYS1,	0x00},
+	{R367CAB_FREESYS2,	0x00},
+	{R367CAB_FREESYS3,	0x00},
+	{R367CAB_GPIO_CFG,	0x55},
+	{R367CAB_GPIO_CMD,	0x01},
+	{R367CAB_TSTRES,	0x00},
+	{R367CAB_ANACTRL,	0x0d},/* was 0x00 need to check - I.M.L.*/
+	{R367CAB_TSTBUS,	0x00},
+	{R367CAB_RF_AGC1,	0xea},
+	{R367CAB_RF_AGC2,	0x82},
+	{R367CAB_ANADIGCTRL,	0x0b},
+	{R367CAB_PLLMDIV,	0x01},
+	{R367CAB_PLLNDIV,	0x08},
+	{R367CAB_PLLSETUP,	0x18},
+	{R367CAB_DUAL_AD12,	0x04},
+	{R367CAB_TSTBIST,	0x00},
+	{R367CAB_CTRL_1,	0x00},
+	{R367CAB_CTRL_2,	0x03},
+	{R367CAB_IT_STATUS1,	0x2b},
+	{R367CAB_IT_STATUS2,	0x08},
+	{R367CAB_IT_EN1,	0x00},
+	{R367CAB_IT_EN2,	0x00},
+	{R367CAB_CTRL_STATUS,	0x04},
+	{R367CAB_TEST_CTL,	0x00},
+	{R367CAB_AGC_CTL,	0x73},
+	{R367CAB_AGC_IF_CFG,	0x50},
+	{R367CAB_AGC_RF_CFG,	0x00},
+	{R367CAB_AGC_PWM_CFG,	0x03},
+	{R367CAB_AGC_PWR_REF_L,	0x5a},
+	{R367CAB_AGC_PWR_REF_H,	0x00},
+	{R367CAB_AGC_RF_TH_L,	0xff},
+	{R367CAB_AGC_RF_TH_H,	0x07},
+	{R367CAB_AGC_IF_LTH_L,	0x00},
+	{R367CAB_AGC_IF_LTH_H,	0x08},
+	{R367CAB_AGC_IF_HTH_L,	0xff},
+	{R367CAB_AGC_IF_HTH_H,	0x07},
+	{R367CAB_AGC_PWR_RD_L,	0xa0},
+	{R367CAB_AGC_PWR_RD_M,	0xe9},
+	{R367CAB_AGC_PWR_RD_H,	0x03},
+	{R367CAB_AGC_PWM_IFCMD_L,	0xe4},
+	{R367CAB_AGC_PWM_IFCMD_H,	0x00},
+	{R367CAB_AGC_PWM_RFCMD_L,	0xff},
+	{R367CAB_AGC_PWM_RFCMD_H,	0x07},
+	{R367CAB_IQDEM_CFG,	0x01},
+	{R367CAB_MIX_NCO_LL,	0x22},
+	{R367CAB_MIX_NCO_HL,	0x96},
+	{R367CAB_MIX_NCO_HH,	0x55},
+	{R367CAB_SRC_NCO_LL,	0xff},
+	{R367CAB_SRC_NCO_LH,	0x0c},
+	{R367CAB_SRC_NCO_HL,	0xf5},
+	{R367CAB_SRC_NCO_HH,	0x20},
+	{R367CAB_IQDEM_GAIN_SRC_L,	0x06},
+	{R367CAB_IQDEM_GAIN_SRC_H,	0x01},
+	{R367CAB_IQDEM_DCRM_CFG_LL,	0xfe},
+	{R367CAB_IQDEM_DCRM_CFG_LH,	0xff},
+	{R367CAB_IQDEM_DCRM_CFG_HL,	0x0f},
+	{R367CAB_IQDEM_DCRM_CFG_HH,	0x00},
+	{R367CAB_IQDEM_ADJ_COEFF0,	0x34},
+	{R367CAB_IQDEM_ADJ_COEFF1,	0xae},
+	{R367CAB_IQDEM_ADJ_COEFF2,	0x46},
+	{R367CAB_IQDEM_ADJ_COEFF3,	0x77},
+	{R367CAB_IQDEM_ADJ_COEFF4,	0x96},
+	{R367CAB_IQDEM_ADJ_COEFF5,	0x69},
+	{R367CAB_IQDEM_ADJ_COEFF6,	0xc7},
+	{R367CAB_IQDEM_ADJ_COEFF7,	0x01},
+	{R367CAB_IQDEM_ADJ_EN,	0x04},
+	{R367CAB_IQDEM_ADJ_AGC_REF,	0x94},
+	{R367CAB_ALLPASSFILT1,	0xc9},
+	{R367CAB_ALLPASSFILT2,	0x2d},
+	{R367CAB_ALLPASSFILT3,	0xa3},
+	{R367CAB_ALLPASSFILT4,	0xfb},
+	{R367CAB_ALLPASSFILT5,	0xf6},
+	{R367CAB_ALLPASSFILT6,	0x45},
+	{R367CAB_ALLPASSFILT7,	0x6f},
+	{R367CAB_ALLPASSFILT8,	0x7e},
+	{R367CAB_ALLPASSFILT9,	0x05},
+	{R367CAB_ALLPASSFILT10,	0x0a},
+	{R367CAB_ALLPASSFILT11,	0x51},
+	{R367CAB_TRL_AGC_CFG,	0x20},
+	{R367CAB_TRL_LPF_CFG,	0x28},
+	{R367CAB_TRL_LPF_ACQ_GAIN,	0x44},
+	{R367CAB_TRL_LPF_TRK_GAIN,	0x22},
+	{R367CAB_TRL_LPF_OUT_GAIN,	0x03},
+	{R367CAB_TRL_LOCKDET_LTH,	0x04},
+	{R367CAB_TRL_LOCKDET_HTH,	0x11},
+	{R367CAB_TRL_LOCKDET_TRGVAL,	0x20},
+	{R367CAB_IQ_QAM,	0x01},
+	{R367CAB_FSM_STATE,	0xa0},
+	{R367CAB_FSM_CTL,	0x08},
+	{R367CAB_FSM_STS,	0x0c},
+	{R367CAB_FSM_SNR0_HTH,	0x00},
+	{R367CAB_FSM_SNR1_HTH,	0x00},
+	{R367CAB_FSM_SNR2_HTH,	0x23},/* 0x00 */
+	{R367CAB_FSM_SNR0_LTH,	0x00},
+	{R367CAB_FSM_SNR1_LTH,	0x00},
+	{R367CAB_FSM_EQA1_HTH,	0x00},
+	{R367CAB_FSM_TEMPO,	0x32},
+	{R367CAB_FSM_CONFIG,	0x03},
+	{R367CAB_EQU_I_TESTTAP_L,	0x11},
+	{R367CAB_EQU_I_TESTTAP_M,	0x00},
+	{R367CAB_EQU_I_TESTTAP_H,	0x00},
+	{R367CAB_EQU_TESTAP_CFG,	0x00},
+	{R367CAB_EQU_Q_TESTTAP_L,	0xff},
+	{R367CAB_EQU_Q_TESTTAP_M,	0x00},
+	{R367CAB_EQU_Q_TESTTAP_H,	0x00},
+	{R367CAB_EQU_TAP_CTRL,	0x00},
+	{R367CAB_EQU_CTR_CRL_CONTROL_L,	0x11},
+	{R367CAB_EQU_CTR_CRL_CONTROL_H,	0x05},
+	{R367CAB_EQU_CTR_HIPOW_L,	0x00},
+	{R367CAB_EQU_CTR_HIPOW_H,	0x00},
+	{R367CAB_EQU_I_EQU_LO,	0xef},
+	{R367CAB_EQU_I_EQU_HI,	0x00},
+	{R367CAB_EQU_Q_EQU_LO,	0xee},
+	{R367CAB_EQU_Q_EQU_HI,	0x00},
+	{R367CAB_EQU_MAPPER,	0xc5},
+	{R367CAB_EQU_SWEEP_RATE,	0x80},
+	{R367CAB_EQU_SNR_LO,	0x64},
+	{R367CAB_EQU_SNR_HI,	0x03},
+	{R367CAB_EQU_GAMMA_LO,	0x00},
+	{R367CAB_EQU_GAMMA_HI,	0x00},
+	{R367CAB_EQU_ERR_GAIN,	0x36},
+	{R367CAB_EQU_RADIUS,	0xaa},
+	{R367CAB_EQU_FFE_MAINTAP,	0x00},
+	{R367CAB_EQU_FFE_LEAKAGE,	0x63},
+	{R367CAB_EQU_FFE_MAINTAP_POS,	0xdf},
+	{R367CAB_EQU_GAIN_WIDE,	0x88},
+	{R367CAB_EQU_GAIN_NARROW,	0x41},
+	{R367CAB_EQU_CTR_LPF_GAIN,	0xd1},
+	{R367CAB_EQU_CRL_LPF_GAIN,	0xa7},
+	{R367CAB_EQU_GLOBAL_GAIN,	0x06},
+	{R367CAB_EQU_CRL_LD_SEN,	0x85},
+	{R367CAB_EQU_CRL_LD_VAL,	0xe2},
+	{R367CAB_EQU_CRL_TFR,	0x20},
+	{R367CAB_EQU_CRL_BISTH_LO,	0x00},
+	{R367CAB_EQU_CRL_BISTH_HI,	0x00},
+	{R367CAB_EQU_SWEEP_RANGE_LO,	0x00},
+	{R367CAB_EQU_SWEEP_RANGE_HI,	0x00},
+	{R367CAB_EQU_CRL_LIMITER,	0x40},
+	{R367CAB_EQU_MODULUS_MAP,	0x90},
+	{R367CAB_EQU_PNT_GAIN,	0xa7},
+	{R367CAB_FEC_AC_CTR_0,	0x16},
+	{R367CAB_FEC_AC_CTR_1,	0x0b},
+	{R367CAB_FEC_AC_CTR_2,	0x88},
+	{R367CAB_FEC_AC_CTR_3,	0x02},
+	{R367CAB_FEC_STATUS,	0x12},
+	{R367CAB_RS_COUNTER_0,	0x7d},
+	{R367CAB_RS_COUNTER_1,	0xd0},
+	{R367CAB_RS_COUNTER_2,	0x19},
+	{R367CAB_RS_COUNTER_3,	0x0b},
+	{R367CAB_RS_COUNTER_4,	0xa3},
+	{R367CAB_RS_COUNTER_5,	0x00},
+	{R367CAB_BERT_0,	0x01},
+	{R367CAB_BERT_1,	0x25},
+	{R367CAB_BERT_2,	0x41},
+	{R367CAB_BERT_3,	0x39},
+	{R367CAB_OUTFORMAT_0,	0xc2},
+	{R367CAB_OUTFORMAT_1,	0x22},
+	{R367CAB_SMOOTHER_2,	0x28},
+	{R367CAB_TSMF_CTRL_0,	0x01},
+	{R367CAB_TSMF_CTRL_1,	0xc6},
+	{R367CAB_TSMF_CTRL_3,	0x43},
+	{R367CAB_TS_ON_ID_0,	0x00},
+	{R367CAB_TS_ON_ID_1,	0x00},
+	{R367CAB_TS_ON_ID_2,	0x00},
+	{R367CAB_TS_ON_ID_3,	0x00},
+	{R367CAB_RE_STATUS_0,	0x00},
+	{R367CAB_RE_STATUS_1,	0x00},
+	{R367CAB_RE_STATUS_2,	0x00},
+	{R367CAB_RE_STATUS_3,	0x00},
+	{R367CAB_TS_STATUS_0,	0x00},
+	{R367CAB_TS_STATUS_1,	0x00},
+	{R367CAB_TS_STATUS_2,	0xa0},
+	{R367CAB_TS_STATUS_3,	0x00},
+	{R367CAB_T_O_ID_0,	0x00},
+	{R367CAB_T_O_ID_1,	0x00},
+	{R367CAB_T_O_ID_2,	0x00},
+	{R367CAB_T_O_ID_3,	0x00},
+};
+
+static
+int stv0367_writeregs(struct stv0367_state *state, u16 reg, u8 *data, int len)
+{
+	u8 buf[len + 2];
+	struct i2c_msg msg = {
+		.addr = state->config->demod_address,
+		.flags = 0,
+		.buf = buf,
+		.len = len + 2
+	};
+	int ret;
+
+	buf[0] = MSB(reg);
+	buf[1] = LSB(reg);
+	memcpy(buf + 2, data, len);
+
+	if (i2cdebug)
+		printk(KERN_DEBUG "%s: %02x: %02x\n", __func__, reg, buf[2]);
+
+	ret = i2c_transfer(state->i2c, &msg, 1);
+	if (ret != 1)
+		printk(KERN_ERR "%s: i2c write error!\n", __func__);
+
+	return (ret != 1) ? -EREMOTEIO : 0;
+}
+
+static int stv0367_writereg(struct stv0367_state *state, u16 reg, u8 data)
+{
+	return stv0367_writeregs(state, reg, &data, 1);
+}
+
+static u8 stv0367_readreg(struct stv0367_state *state, u16 reg)
+{
+	u8 b0[] = { 0, 0 };
+	u8 b1[] = { 0 };
+	struct i2c_msg msg[] = {
+		{
+			.addr = state->config->demod_address,
+			.flags = 0,
+			.buf = b0,
+			.len = 2
+		}, {
+			.addr = state->config->demod_address,
+			.flags = I2C_M_RD,
+			.buf = b1,
+			.len = 1
+		}
+	};
+	int ret;
+
+	b0[0] = MSB(reg);
+	b0[1] = LSB(reg);
+
+	ret = i2c_transfer(state->i2c, msg, 2);
+	if (ret != 2)
+		printk(KERN_ERR "%s: i2c read error\n", __func__);
+
+	if (i2cdebug)
+		printk(KERN_DEBUG "%s: %02x: %02x\n", __func__, reg, b1[0]);
+
+	return b1[0];
+}
+
+static void extract_mask_pos(u32 label, u8 *mask, u8 *pos)
+{
+	u8 position = 0, i = 0;
+
+	(*mask) = label & 0xff;
+
+	while ((position == 0) && (i < 8)) {
+		position = ((*mask) >> i) & 0x01;
+		i++;
+	}
+
+	(*pos) = (i - 1);
+}
+
+static void stv0367_writebits(struct stv0367_state *state, u32 label, u8 val)
+{
+	u8 reg, mask, pos;
+
+	reg = stv0367_readreg(state, (label >> 16) & 0xffff);
+	extract_mask_pos(label, &mask, &pos);
+
+	val = mask & (val << pos);
+
+	reg = (reg & (~mask)) | val;
+	stv0367_writereg(state, (label >> 16) & 0xffff, reg);
+
+}
+
+static void stv0367_setbits(u8 *reg, u32 label, u8 val)
+{
+	u8 mask, pos;
+
+	extract_mask_pos(label, &mask, &pos);
+
+	val = mask & (val << pos);
+
+	(*reg) = ((*reg) & (~mask)) | val;
+}
+
+static u8 stv0367_readbits(struct stv0367_state *state, u32 label)
+{
+	u8 val = 0xff;
+	u8 mask, pos;
+
+	extract_mask_pos(label, &mask, &pos);
+
+	val = stv0367_readreg(state, label >> 16);
+	val = (val & mask) >> pos;
+
+	return val;
+}
+
+u8 stv0367_getbits(u8 reg, u32 label)
+{
+	u8 mask, pos;
+
+	extract_mask_pos(label, &mask, &pos);
+
+	return (reg & mask) >> pos;
+}
+
+static int stv0367ter_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	u8 tmp = stv0367_readreg(state, R367TER_I2CRPT);
+
+	dprintk("%s:\n", __func__);
+
+	if (enable) {
+		stv0367_setbits(&tmp, F367TER_STOP_ENABLE, 0);
+		stv0367_setbits(&tmp, F367TER_I2CT_ON, 1);
+	} else {
+		stv0367_setbits(&tmp, F367TER_STOP_ENABLE, 1);
+		stv0367_setbits(&tmp, F367TER_I2CT_ON, 0);
+	}
+
+	stv0367_writereg(state, R367TER_I2CRPT, tmp);
+
+	return 0;
+}
+
+static u32 stv0367_get_tuner_freq(struct dvb_frontend *fe)
+{
+	struct dvb_frontend_ops	*frontend_ops = NULL;
+	struct dvb_tuner_ops	*tuner_ops = NULL;
+	u32 freq = 0;
+	u32 err = 0;
+
+	dprintk("%s:\n", __func__);
+
+
+	if (&fe->ops)
+		frontend_ops = &fe->ops;
+	if (&frontend_ops->tuner_ops)
+		tuner_ops = &frontend_ops->tuner_ops;
+	if (tuner_ops->get_frequency) {
+		err = tuner_ops->get_frequency(fe, &freq);
+		if (err < 0) {
+			printk(KERN_ERR "%s: Invalid parameter\n", __func__);
+			return err;
+		}
+
+		dprintk("%s: frequency=%d\n", __func__, freq);
+
+	} else
+		return -1;
+
+	return freq;
+}
+
+static u16 CellsCoeffs_8MHz_367cofdm[3][6][5] = {
+	{
+		{0x10EF, 0xE205, 0x10EF, 0xCE49, 0x6DA7}, /* CELL 1 COEFFS 27M*/
+		{0x2151, 0xc557, 0x2151, 0xc705, 0x6f93}, /* CELL 2 COEFFS */
+		{0x2503, 0xc000, 0x2503, 0xc375, 0x7194}, /* CELL 3 COEFFS */
+		{0x20E9, 0xca94, 0x20e9, 0xc153, 0x7194}, /* CELL 4 COEFFS */
+		{0x06EF, 0xF852, 0x06EF, 0xC057, 0x7207}, /* CELL 5 COEFFS */
+		{0x0000, 0x0ECC, 0x0ECC, 0x0000, 0x3647} /* CELL 6 COEFFS */
+	}, {
+		{0x10A0, 0xE2AF, 0x10A1, 0xCE76, 0x6D6D}, /* CELL 1 COEFFS 25M*/
+		{0x20DC, 0xC676, 0x20D9, 0xC80A, 0x6F29},
+		{0x2532, 0xC000, 0x251D, 0xC391, 0x706F},
+		{0x1F7A, 0xCD2B, 0x2032, 0xC15E, 0x711F},
+		{0x0698, 0xFA5E, 0x0568, 0xC059, 0x7193},
+		{0x0000, 0x0918, 0x149C, 0x0000, 0x3642} /* CELL 6 COEFFS */
+	}, {
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000}, /* 30M */
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000}
+	}
+};
+
+static u16 CellsCoeffs_7MHz_367cofdm[3][6][5] = {
+	{
+		{0x12CA, 0xDDAF, 0x12CA, 0xCCEB, 0x6FB1}, /* CELL 1 COEFFS 27M*/
+		{0x2329, 0xC000, 0x2329, 0xC6B0, 0x725F}, /* CELL 2 COEFFS */
+		{0x2394, 0xC000, 0x2394, 0xC2C7, 0x7410}, /* CELL 3 COEFFS */
+		{0x251C, 0xC000, 0x251C, 0xC103, 0x74D9}, /* CELL 4 COEFFS */
+		{0x0804, 0xF546, 0x0804, 0xC040, 0x7544}, /* CELL 5 COEFFS */
+		{0x0000, 0x0CD9, 0x0CD9, 0x0000, 0x370A} /* CELL 6 COEFFS */
+	}, {
+		{0x1285, 0xDE47, 0x1285, 0xCD17, 0x6F76}, /*25M*/
+		{0x234C, 0xC000, 0x2348, 0xC6DA, 0x7206},
+		{0x23B4, 0xC000, 0x23AC, 0xC2DB, 0x73B3},
+		{0x253D, 0xC000, 0x25B6, 0xC10B, 0x747F},
+		{0x0721, 0xF79C, 0x065F, 0xC041, 0x74EB},
+		{0x0000, 0x08FA, 0x1162, 0x0000, 0x36FF}
+	}, {
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000}, /* 30M */
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000}
+	}
+};
+
+static u16 CellsCoeffs_6MHz_367cofdm[3][6][5] = {
+	{
+		{0x1699, 0xD5B8, 0x1699, 0xCBC3, 0x713B}, /* CELL 1 COEFFS 27M*/
+		{0x2245, 0xC000, 0x2245, 0xC568, 0x74D5}, /* CELL 2 COEFFS */
+		{0x227F, 0xC000, 0x227F, 0xC1FC, 0x76C6}, /* CELL 3 COEFFS */
+		{0x235E, 0xC000, 0x235E, 0xC0A7, 0x778A}, /* CELL 4 COEFFS */
+		{0x0ECB, 0xEA0B, 0x0ECB, 0xC027, 0x77DD}, /* CELL 5 COEFFS */
+		{0x0000, 0x0B68, 0x0B68, 0x0000, 0xC89A}, /* CELL 6 COEFFS */
+	}, {
+		{0x1655, 0xD64E, 0x1658, 0xCBEF, 0x70FE}, /*25M*/
+		{0x225E, 0xC000, 0x2256, 0xC589, 0x7489},
+		{0x2293, 0xC000, 0x2295, 0xC209, 0x767E},
+		{0x2377, 0xC000, 0x23AA, 0xC0AB, 0x7746},
+		{0x0DC7, 0xEBC8, 0x0D07, 0xC027, 0x7799},
+		{0x0000, 0x0888, 0x0E9C, 0x0000, 0x3757}
+
+	}, {
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000}, /* 30M */
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000},
+		{0x0000, 0x0000, 0x0000, 0x0000, 0x0000}
+	}
+};
+
+static u32 stv0367ter_get_mclk(struct stv0367_state *state, u32 ExtClk_Hz)
+{
+	u32 mclk_Hz = 0; /* master clock frequency (Hz) */
+	u32 m, n, p;
+
+	dprintk("%s:\n", __func__);
+
+	if (stv0367_readbits(state, F367TER_BYPASS_PLLXN) == 0) {
+		n = (u32)stv0367_readbits(state, F367TER_PLL_NDIV);
+		if (n == 0)
+			n = n + 1;
+
+		m = (u32)stv0367_readbits(state, F367TER_PLL_MDIV);
+		if (m == 0)
+			m = m + 1;
+
+		p = (u32)stv0367_readbits(state, F367TER_PLL_PDIV);
+		if (p > 5)
+			p = 5;
+
+		mclk_Hz = ((ExtClk_Hz / 2) * n) / (m * (1 << p));
+
+		dprintk("N=%d M=%d P=%d mclk_Hz=%d ExtClk_Hz=%d\n",
+				n, m, p, mclk_Hz, ExtClk_Hz);
+	} else
+		mclk_Hz = ExtClk_Hz;
+
+	dprintk("%s: mclk_Hz=%d\n", __func__, mclk_Hz);
+
+	return mclk_Hz;
+}
+
+static int stv0367ter_filt_coeff_init(struct stv0367_state *state,
+				u16 CellsCoeffs[2][6][5], u32 DemodXtal)
+{
+	int i, j, k, freq;
+
+	dprintk("%s:\n", __func__);
+
+	freq = stv0367ter_get_mclk(state, DemodXtal);
+
+	if (freq == 53125000)
+		k = 1; /* equivalent to Xtal 25M on 362*/
+	else if (freq == 54000000)
+		k = 0; /* equivalent to Xtal 27M on 362*/
+	else if (freq == 52500000)
+		k = 2; /* equivalent to Xtal 30M on 362*/
+	else
+		return 0;
+
+	for (i = 1; i <= 6; i++) {
+		stv0367_writebits(state, F367TER_IIR_CELL_NB, i - 1);
+
+		for (j = 1; j <= 5; j++) {
+			stv0367_writereg(state,
+				(R367TER_IIRCX_COEFF1_MSB + 2 * (j - 1)),
+				MSB(CellsCoeffs[k][i-1][j-1]));
+			stv0367_writereg(state,
+				(R367TER_IIRCX_COEFF1_LSB + 2 * (j - 1)),
+				LSB(CellsCoeffs[k][i-1][j-1]));
+		}
+	}
+
+	return 1;
+
+}
+
+static void stv0367ter_agc_iir_lock_detect_set(struct stv0367_state *state)
+{
+	dprintk("%s:\n", __func__);
+
+	stv0367_writebits(state, F367TER_LOCK_DETECT_LSB, 0x00);
+
+	/* Lock detect 1 */
+	stv0367_writebits(state, F367TER_LOCK_DETECT_CHOICE, 0x00);
+	stv0367_writebits(state, F367TER_LOCK_DETECT_MSB, 0x06);
+	stv0367_writebits(state, F367TER_AUT_AGC_TARGET_LSB, 0x04);
+
+	/* Lock detect 2 */
+	stv0367_writebits(state, F367TER_LOCK_DETECT_CHOICE, 0x01);
+	stv0367_writebits(state, F367TER_LOCK_DETECT_MSB, 0x06);
+	stv0367_writebits(state, F367TER_AUT_AGC_TARGET_LSB, 0x04);
+
+	/* Lock detect 3 */
+	stv0367_writebits(state, F367TER_LOCK_DETECT_CHOICE, 0x02);
+	stv0367_writebits(state, F367TER_LOCK_DETECT_MSB, 0x01);
+	stv0367_writebits(state, F367TER_AUT_AGC_TARGET_LSB, 0x00);
+
+	/* Lock detect 4 */
+	stv0367_writebits(state, F367TER_LOCK_DETECT_CHOICE, 0x03);
+	stv0367_writebits(state, F367TER_LOCK_DETECT_MSB, 0x01);
+	stv0367_writebits(state, F367TER_AUT_AGC_TARGET_LSB, 0x00);
+
+}
+
+static int stv0367_iir_filt_init(struct stv0367_state *state, u8 Bandwidth,
+							u32 DemodXtalValue)
+{
+	dprintk("%s:\n", __func__);
+
+	stv0367_writebits(state, F367TER_NRST_IIR, 0);
+
+	switch (Bandwidth) {
+	case 6:
+		if (!stv0367ter_filt_coeff_init(state,
+				CellsCoeffs_6MHz_367cofdm,
+				DemodXtalValue))
+			return 0;
+		break;
+	case 7:
+		if (!stv0367ter_filt_coeff_init(state,
+				CellsCoeffs_7MHz_367cofdm,
+				DemodXtalValue))
+			return 0;
+		break;
+	case 8:
+		if (!stv0367ter_filt_coeff_init(state,
+				CellsCoeffs_8MHz_367cofdm,
+				DemodXtalValue))
+			return 0;
+		break;
+	default:
+		return 0;
+	}
+
+	stv0367_writebits(state, F367TER_NRST_IIR, 1);
+
+	return 1;
+}
+
+static void stv0367ter_agc_iir_rst(struct stv0367_state *state)
+{
+
+	u8 com_n;
+
+	dprintk("%s:\n", __func__);
+
+	com_n = stv0367_readbits(state, F367TER_COM_N);
+
+	stv0367_writebits(state, F367TER_COM_N, 0x07);
+
+	stv0367_writebits(state, F367TER_COM_SOFT_RSTN, 0x00);
+	stv0367_writebits(state, F367TER_COM_AGC_ON, 0x00);
+
+	stv0367_writebits(state, F367TER_COM_SOFT_RSTN, 0x01);
+	stv0367_writebits(state, F367TER_COM_AGC_ON, 0x01);
+
+	stv0367_writebits(state, F367TER_COM_N, com_n);
+
+}
+
+static int stv0367ter_duration(s32 mode, int tempo1, int tempo2, int tempo3)
+{
+	int local_tempo = 0;
+	switch (mode) {
+	case 0:
+		local_tempo = tempo1;
+		break;
+	case 1:
+		local_tempo = tempo2;
+		break ;
+
+	case 2:
+		local_tempo = tempo3;
+		break;
+
+	default:
+		break;
+	}
+	/*	msleep(local_tempo);  */
+	return local_tempo;
+}
+
+static enum
+stv0367_ter_signal_type stv0367ter_check_syr(struct stv0367_state *state)
+{
+	int wd = 100;
+	unsigned short int SYR_var;
+	s32 SYRStatus;
+
+	dprintk("%s:\n", __func__);
+
+	SYR_var = stv0367_readbits(state, F367TER_SYR_LOCK);
+
+	while ((!SYR_var) && (wd > 0)) {
+		usleep_range(2000, 3000);
+		wd -= 2;
+		SYR_var = stv0367_readbits(state, F367TER_SYR_LOCK);
+	}
+
+	if (!SYR_var)
+		SYRStatus = FE_TER_NOSYMBOL;
+	else
+		SYRStatus =  FE_TER_SYMBOLOK;
+
+	dprintk("stv0367ter_check_syr SYRStatus %s\n",
+				SYR_var == 0 ? "No Symbol" : "OK");
+
+	return SYRStatus;
+}
+
+static enum
+stv0367_ter_signal_type stv0367ter_check_cpamp(struct stv0367_state *state,
+								s32 FFTmode)
+{
+
+	s32  CPAMPvalue = 0, CPAMPStatus, CPAMPMin;
+	int wd = 0;
+
+	dprintk("%s:\n", __func__);
+
+	switch (FFTmode) {
+	case 0: /*2k mode*/
+		CPAMPMin = 20;
+		wd = 10;
+		break;
+	case 1: /*8k mode*/
+		CPAMPMin = 80;
+		wd = 55;
+		break;
+	case 2: /*4k mode*/
+		CPAMPMin = 40;
+		wd = 30;
+		break;
+	default:
+		CPAMPMin = 0xffff;  /*drives to NOCPAMP	*/
+		break;
+	}
+
+	dprintk("%s: CPAMPMin=%d wd=%d\n", __func__, CPAMPMin, wd);
+
+	CPAMPvalue = stv0367_readbits(state, F367TER_PPM_CPAMP_DIRECT);
+	while ((CPAMPvalue < CPAMPMin) && (wd > 0)) {
+		usleep_range(1000, 2000);
+		wd -= 1;
+		CPAMPvalue = stv0367_readbits(state, F367TER_PPM_CPAMP_DIRECT);
+		/*dprintk("CPAMPvalue= %d at wd=%d\n",CPAMPvalue,wd); */
+	}
+	dprintk("******last CPAMPvalue= %d at wd=%d\n", CPAMPvalue, wd);
+	if (CPAMPvalue < CPAMPMin) {
+		CPAMPStatus = FE_TER_NOCPAMP;
+		printk(KERN_ERR "CPAMP failed\n");
+	} else {
+		printk(KERN_ERR "CPAMP OK !\n");
+		CPAMPStatus = FE_TER_CPAMPOK;
+	}
+
+	return CPAMPStatus;
+}
+
+enum
+stv0367_ter_signal_type stv0367ter_lock_algo(struct stv0367_state *state)
+{
+	enum stv0367_ter_signal_type ret_flag;
+	short int wd, tempo;
+	u8 try, u_var1 = 0, u_var2 = 0, u_var3 = 0, u_var4 = 0, mode, guard;
+	u8 tmp, tmp2;
+
+	dprintk("%s:\n", __func__);
+
+	if (state == NULL)
+		return FE_TER_SWNOK;
+
+	try = 0;
+	do {
+		ret_flag = FE_TER_LOCKOK;
+
+		stv0367_writebits(state, F367TER_CORE_ACTIVE, 0);
+
+		if (state->config->if_iq_mode != 0)
+			stv0367_writebits(state, F367TER_COM_N, 0x07);
+
+		stv0367_writebits(state, F367TER_GUARD, 3);/* suggest 2k 1/4 */
+		stv0367_writebits(state, F367TER_MODE, 0);
+		stv0367_writebits(state, F367TER_SYR_TR_DIS, 0);
+		usleep_range(5000, 10000);
+
+		stv0367_writebits(state, F367TER_CORE_ACTIVE, 1);
+
+
+		if (stv0367ter_check_syr(state) == FE_TER_NOSYMBOL)
+			return FE_TER_NOSYMBOL;
+		else { /*
+			if chip locked on wrong mode first try,
+			it must lock correctly second try */
+			mode = stv0367_readbits(state, F367TER_SYR_MODE);
+			if (stv0367ter_check_cpamp(state, mode) ==
+							FE_TER_NOCPAMP) {
+				if (try == 0)
+					ret_flag = FE_TER_NOCPAMP;
+
+			}
+		}
+
+		try++;
+	} while ((try < 10) && (ret_flag != FE_TER_LOCKOK));
+
+	tmp  = stv0367_readreg(state, R367TER_SYR_STAT);
+	tmp2 = stv0367_readreg(state, R367TER_STATUS);
+	dprintk("state=0x%x\n", (int)state);
+	dprintk("LOCK OK! mode=%d SYR_STAT=0x%x R367TER_STATUS=0x%x\n",
+							mode, tmp, tmp2);
+
+	tmp  = stv0367_readreg(state, R367TER_PRVIT);
+	tmp2 = stv0367_readreg(state, R367TER_I2CRPT);
+	dprintk("PRVIT=0x%x I2CRPT=0x%x\n", tmp, tmp2);
+
+	tmp  = stv0367_readreg(state, R367TER_GAIN_SRC1);
+	dprintk("GAIN_SRC1=0x%x\n", tmp);
+
+	if ((mode != 0) && (mode != 1) && (mode != 2))
+		return FE_TER_SWNOK;
+
+	/*guard=stv0367_readbits(state,F367TER_SYR_GUARD); */
+
+	/*supress EPQ auto for SYR_GARD 1/16 or 1/32
+	and set channel predictor in automatic */
+#if 0
+	switch (guard) {
+
+	case 0:
+	case 1:
+		stv0367_writebits(state, F367TER_AUTO_LE_EN, 0);
+		stv0367_writereg(state, R367TER_CHC_CTL, 0x01);
+		break;
+	case 2:
+	case 3:
+		stv0367_writebits(state, F367TER_AUTO_LE_EN, 1);
+		stv0367_writereg(state, R367TER_CHC_CTL, 0x11);
+		break;
+
+	default:
+		return FE_TER_SWNOK;
+	}
+#endif
+
+	/*reset fec an reedsolo FOR 367 only*/
+	stv0367_writebits(state, F367TER_RST_SFEC, 1);
+	stv0367_writebits(state, F367TER_RST_REEDSOLO, 1);
+	usleep_range(1000, 2000);
+	stv0367_writebits(state, F367TER_RST_SFEC, 0);
+	stv0367_writebits(state, F367TER_RST_REEDSOLO, 0);
+
+	u_var1 = stv0367_readbits(state, F367TER_LK);
+	u_var2 = stv0367_readbits(state, F367TER_PRF);
+	u_var3 = stv0367_readbits(state, F367TER_TPS_LOCK);
+	/*	u_var4=stv0367_readbits(state,F367TER_TSFIFO_LINEOK); */
+
+	wd = stv0367ter_duration(mode, 125, 500, 250);
+	tempo = stv0367ter_duration(mode, 4, 16, 8);
+
+	/*while ( ((!u_var1)||(!u_var2)||(!u_var3)||(!u_var4))  && (wd>=0)) */
+	while (((!u_var1) || (!u_var2) || (!u_var3)) && (wd >= 0)) {
+		usleep_range(1000 * tempo, 1000 * (tempo + 1));
+		wd -= tempo;
+		u_var1 = stv0367_readbits(state, F367TER_LK);
+		u_var2 = stv0367_readbits(state, F367TER_PRF);
+		u_var3 = stv0367_readbits(state, F367TER_TPS_LOCK);
+		/*u_var4=stv0367_readbits(state, F367TER_TSFIFO_LINEOK); */
+	}
+
+	if (!u_var1)
+		return FE_TER_NOLOCK;
+
+
+	if (!u_var2)
+		return FE_TER_NOPRFOUND;
+
+	if (!u_var3)
+		return FE_TER_NOTPS;
+
+	guard = stv0367_readbits(state, F367TER_SYR_GUARD);
+	stv0367_writereg(state, R367TER_CHC_CTL, 0x11);
+	switch (guard) {
+	case 0:
+	case 1:
+		stv0367_writebits(state, F367TER_AUTO_LE_EN, 0);
+		/*stv0367_writereg(state,R367TER_CHC_CTL, 0x1);*/
+		stv0367_writebits(state, F367TER_SYR_FILTER, 0);
+		break;
+	case 2:
+	case 3:
+		stv0367_writebits(state, F367TER_AUTO_LE_EN, 1);
+		/*stv0367_writereg(state,R367TER_CHC_CTL, 0x11);*/
+		stv0367_writebits(state, F367TER_SYR_FILTER, 1);
+		break;
+
+	default:
+		return FE_TER_SWNOK;
+	}
+
+	/* apply Sfec workaround if 8K 64QAM CR!=1/2*/
+	if ((stv0367_readbits(state, F367TER_TPS_CONST) == 2) &&
+			(mode == 1) &&
+			(stv0367_readbits(state, F367TER_TPS_HPCODE) != 0)) {
+		stv0367_writereg(state, R367TER_SFDLYSETH, 0xc0);
+		stv0367_writereg(state, R367TER_SFDLYSETM, 0x60);
+		stv0367_writereg(state, R367TER_SFDLYSETL, 0x0);
+	} else
+		stv0367_writereg(state, R367TER_SFDLYSETH, 0x0);
+
+	wd = stv0367ter_duration(mode, 125, 500, 250);
+	u_var4 = stv0367_readbits(state, F367TER_TSFIFO_LINEOK);
+
+	while ((!u_var4) && (wd >= 0)) {
+		usleep_range(1000 * tempo, 1000 * (tempo + 1));
+		wd -= tempo;
+		u_var4 = stv0367_readbits(state, F367TER_TSFIFO_LINEOK);
+	}
+
+	if (!u_var4)
+		return FE_TER_NOLOCK;
+
+	/* for 367 leave COM_N at 0x7 for IQ_mode*/
+	/*if(ter_state->if_iq_mode!=FE_TER_NORMAL_IF_TUNER) {
+		tempo=0;
+		while ((stv0367_readbits(state,F367TER_COM_USEGAINTRK)!=1) &&
+		(stv0367_readbits(state,F367TER_COM_AGCLOCK)!=1)&&(tempo<100)) {
+			ChipWaitOrAbort(state,1);
+			tempo+=1;
+		}
+
+		stv0367_writebits(state,F367TER_COM_N,0x17);
+	} */
+
+	stv0367_writebits(state, F367TER_SYR_TR_DIS, 1);
+
+	dprintk("FE_TER_LOCKOK !!!\n");
+
+	return	FE_TER_LOCKOK;
+
+}
+
+static void stv0367ter_set_ts_mode(struct stv0367_state *state,
+					enum stv0367_ts_mode PathTS)
+{
+
+	dprintk("%s:\n", __func__);
+
+	if (state == NULL)
+		return;
+
+	stv0367_writebits(state, F367TER_TS_DIS, 0);
+	switch (PathTS) {
+	default:
+		/*for removing warning :default we can assume in parallel mode*/
+	case STV0367_PARALLEL_PUNCT_CLOCK:
+		stv0367_writebits(state, F367TER_TSFIFO_SERIAL, 0);
+		stv0367_writebits(state, F367TER_TSFIFO_DVBCI, 0);
+		break;
+	case STV0367_SERIAL_PUNCT_CLOCK:
+		stv0367_writebits(state, F367TER_TSFIFO_SERIAL, 1);
+		stv0367_writebits(state, F367TER_TSFIFO_DVBCI, 1);
+		break;
+	}
+}
+
+static void stv0367ter_set_clk_pol(struct stv0367_state *state,
+					enum stv0367_clk_pol clock)
+{
+
+	dprintk("%s:\n", __func__);
+
+	if (state == NULL)
+		return;
+
+	switch (clock) {
+	case STV0367_RISINGEDGE_CLOCK:
+		stv0367_writebits(state, F367TER_TS_BYTE_CLK_INV, 1);
+		break;
+	case STV0367_FALLINGEDGE_CLOCK:
+		stv0367_writebits(state, F367TER_TS_BYTE_CLK_INV, 0);
+		break;
+		/*case FE_TER_CLOCK_POLARITY_DEFAULT:*/
+	default:
+		stv0367_writebits(state, F367TER_TS_BYTE_CLK_INV, 0);
+		break;
+	}
+}
+
+#if 0
+static void stv0367ter_core_sw(struct stv0367_state *state)
+{
+
+	dprintk("%s:\n", __func__);
+
+	stv0367_writebits(state, F367TER_CORE_ACTIVE, 0);
+	stv0367_writebits(state, F367TER_CORE_ACTIVE, 1);
+	msleep(350);
+}
+#endif
+static int stv0367ter_standby(struct dvb_frontend *fe, u8 standby_on)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	dprintk("%s:\n", __func__);
+
+	if (standby_on) {
+		stv0367_writebits(state, F367TER_STDBY, 1);
+		stv0367_writebits(state, F367TER_STDBY_FEC, 1);
+		stv0367_writebits(state, F367TER_STDBY_CORE, 1);
+	} else {
+		stv0367_writebits(state, F367TER_STDBY, 0);
+		stv0367_writebits(state, F367TER_STDBY_FEC, 0);
+		stv0367_writebits(state, F367TER_STDBY_CORE, 0);
+	}
+
+	return 0;
+}
+
+static int stv0367ter_sleep(struct dvb_frontend *fe)
+{
+	return stv0367ter_standby(fe, 1);
+}
+
+int stv0367ter_init(struct dvb_frontend *fe)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+	int i;
+
+	dprintk("%s:\n", __func__);
+
+	ter_state->pBER = 0;
+
+	for (i = 0; i < STV0367TER_NBREGS; i++)
+		stv0367_writereg(state, def0367ter[i].addr,
+					def0367ter[i].value);
+
+	switch (state->config->xtal) {
+		/*set internal freq to 53.125MHz */
+	case 25000000:
+		stv0367_writereg(state, R367TER_PLLMDIV, 0xa);
+		stv0367_writereg(state, R367TER_PLLNDIV, 0x55);
+		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
+		break;
+	default:
+	case 27000000:
+		dprintk("FE_STV0367TER_SetCLKgen for 27Mhz\n");
+		stv0367_writereg(state, R367TER_PLLMDIV, 0x1);
+		stv0367_writereg(state, R367TER_PLLNDIV, 0x8);
+		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
+		break;
+	case 30000000:
+		stv0367_writereg(state, R367TER_PLLMDIV, 0xc);
+		stv0367_writereg(state, R367TER_PLLNDIV, 0x55);
+		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
+		break;
+	}
+
+	stv0367_writereg(state, R367TER_I2CRPT, 0xa0);
+	stv0367_writereg(state, R367TER_ANACTRL, 0x00);
+
+	/*Set TS1 and TS2 to serial or parallel mode */
+	stv0367ter_set_ts_mode(state, state->config->ts_mode);
+	stv0367ter_set_clk_pol(state, state->config->clk_pol);
+
+	state->chip_id = stv0367_readreg(state, R367TER_ID);
+	ter_state->first_lock = 0;
+	ter_state->unlock_counter = 2;
+
+	return 0;
+}
+
+static int stv0367ter_algo(struct dvb_frontend *fe,
+				struct dvb_frontend_parameters *param)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+	int offset = 0, tempo = 0;
+	u8 u_var;
+	u8 /*constell,*/ counter, tps_rcvd[2];
+	s8 step;
+	s32 timing_offset = 0;
+	u32 trl_nomrate = 0, InternalFreq = 0, temp = 0;
+
+	dprintk("%s:\n", __func__);
+
+	ter_state->frequency = param->frequency;
+	ter_state->force = FE_TER_FORCENONE
+			+ stv0367_readbits(state, F367TER_FORCE) * 2;
+	ter_state->if_iq_mode = state->config->if_iq_mode;
+	switch (state->config->if_iq_mode) {
+	case FE_TER_NORMAL_IF_TUNER:  /* Normal IF mode */
+		dprintk("ALGO: FE_TER_NORMAL_IF_TUNER selected\n");
+		stv0367_writebits(state, F367TER_TUNER_BB, 0);
+		stv0367_writebits(state, F367TER_LONGPATH_IF, 0);
+		stv0367_writebits(state, F367TER_DEMUX_SWAP, 0);
+		break;
+	case FE_TER_LONGPATH_IF_TUNER:  /* Long IF mode */
+		dprintk("ALGO: FE_TER_LONGPATH_IF_TUNER selected\n");
+		stv0367_writebits(state, F367TER_TUNER_BB, 0);
+		stv0367_writebits(state, F367TER_LONGPATH_IF, 1);
+		stv0367_writebits(state, F367TER_DEMUX_SWAP, 1);
+		break;
+	case FE_TER_IQ_TUNER:  /* IQ mode */
+		dprintk("ALGO: FE_TER_IQ_TUNER selected\n");
+		stv0367_writebits(state, F367TER_TUNER_BB, 1);
+		stv0367_writebits(state, F367TER_PPM_INVSEL, 0);
+		break;
+	default:
+		printk(KERN_ERR "ALGO: wrong TUNER type selected\n");
+		return -EINVAL;
+	}
+
+	usleep_range(5000, 7000);
+
+	switch (param->inversion) {
+	case INVERSION_AUTO:
+	default:
+		dprintk("%s: inversion AUTO\n", __func__);
+		if (ter_state->if_iq_mode == FE_TER_IQ_TUNER)
+			stv0367_writebits(state, F367TER_IQ_INVERT,
+						ter_state->sense);
+		else
+			stv0367_writebits(state, F367TER_INV_SPECTR,
+						ter_state->sense);
+
+		break;
+	case INVERSION_ON:
+	case INVERSION_OFF:
+		if (ter_state->if_iq_mode == FE_TER_IQ_TUNER)
+			stv0367_writebits(state, F367TER_IQ_INVERT,
+						param->inversion);
+		else
+			stv0367_writebits(state, F367TER_INV_SPECTR,
+						param->inversion);
+
+		break;
+	}
+
+	if ((ter_state->if_iq_mode != FE_TER_NORMAL_IF_TUNER) &&
+				(ter_state->pBW != ter_state->bw)) {
+		stv0367ter_agc_iir_lock_detect_set(state);
+
+		/*set fine agc target to 180 for LPIF or IQ mode*/
+		/* set Q_AGCTarget */
+		stv0367_writebits(state, F367TER_SEL_IQNTAR, 1);
+		stv0367_writebits(state, F367TER_AUT_AGC_TARGET_MSB, 0xB);
+		/*stv0367_writebits(state,AUT_AGC_TARGET_LSB,0x04); */
+
+		/* set Q_AGCTarget */
+		stv0367_writebits(state, F367TER_SEL_IQNTAR, 0);
+		stv0367_writebits(state, F367TER_AUT_AGC_TARGET_MSB, 0xB);
+		/*stv0367_writebits(state,AUT_AGC_TARGET_LSB,0x04); */
+
+		if (!stv0367_iir_filt_init(state, ter_state->bw,
+						state->config->xtal))
+			return -EINVAL;
+		/*set IIR filter once for 6,7 or 8MHz BW*/
+		ter_state->pBW = ter_state->bw;
+
+		stv0367ter_agc_iir_rst(state);
+	}
+
+	if (ter_state->hierarchy == FE_TER_HIER_LOW_PRIO)
+		stv0367_writebits(state, F367TER_BDI_LPSEL, 0x01);
+	else
+		stv0367_writebits(state, F367TER_BDI_LPSEL, 0x00);
+
+	InternalFreq = stv0367ter_get_mclk(state, state->config->xtal) / 1000;
+	temp = (int)
+		((((ter_state->bw * 64 * (1 << 15) * 100)
+						/ (InternalFreq)) * 10) / 7);
+
+	stv0367_writebits(state, F367TER_TRL_NOMRATE_LSB, temp % 2);
+	temp = temp / 2;
+	stv0367_writebits(state, F367TER_TRL_NOMRATE_HI, temp / 256);
+	stv0367_writebits(state, F367TER_TRL_NOMRATE_LO, temp % 256);
+
+	temp = stv0367_readbits(state, F367TER_TRL_NOMRATE_HI) * 512 +
+			stv0367_readbits(state, F367TER_TRL_NOMRATE_LO) * 2 +
+			stv0367_readbits(state, F367TER_TRL_NOMRATE_LSB);
+	temp = (int)(((1 << 17) * ter_state->bw * 1000) / (7 * (InternalFreq)));
+	stv0367_writebits(state, F367TER_GAIN_SRC_HI, temp / 256);
+	stv0367_writebits(state, F367TER_GAIN_SRC_LO, temp % 256);
+	temp = stv0367_readbits(state, F367TER_GAIN_SRC_HI) * 256 +
+			stv0367_readbits(state, F367TER_GAIN_SRC_LO);
+
+	temp = (int)
+		((InternalFreq - state->config->if_khz) * (1 << 16)
+							/ (InternalFreq));
+
+	dprintk("DEROT temp=0x%x\n", temp);
+	stv0367_writebits(state, F367TER_INC_DEROT_HI, temp / 256);
+	stv0367_writebits(state, F367TER_INC_DEROT_LO, temp % 256);
+
+	ter_state->echo_pos = 0;
+	ter_state->ucblocks = 0; /* liplianin */
+	ter_state->pBER = 0; /* liplianin */
+	stv0367_writebits(state, F367TER_LONG_ECHO, ter_state->echo_pos);
+
+	if (stv0367ter_lock_algo(state) != FE_TER_LOCKOK)
+		return 0;
+
+	ter_state->state = FE_TER_LOCKOK;
+	/* update results */
+	tps_rcvd[0] = stv0367_readreg(state, R367TER_TPS_RCVD2);
+	tps_rcvd[1] = stv0367_readreg(state, R367TER_TPS_RCVD3);
+
+	ter_state->mode = stv0367_readbits(state, F367TER_SYR_MODE);
+	ter_state->guard = stv0367_readbits(state, F367TER_SYR_GUARD);
+
+	ter_state->first_lock = 1; /* we know sense now :) */
+
+	ter_state->agc_val =
+			(stv0367_readbits(state, F367TER_AGC1_VAL_LO) << 16) +
+			(stv0367_readbits(state, F367TER_AGC1_VAL_HI) << 24) +
+			stv0367_readbits(state, F367TER_AGC2_VAL_LO) +
+			(stv0367_readbits(state, F367TER_AGC2_VAL_HI) << 8);
+
+	/* Carrier offset calculation */
+	stv0367_writebits(state, F367TER_FREEZE, 1);
+	offset = (stv0367_readbits(state, F367TER_CRL_FOFFSET_VHI) << 16) ;
+	offset += (stv0367_readbits(state, F367TER_CRL_FOFFSET_HI) << 8);
+	offset += (stv0367_readbits(state, F367TER_CRL_FOFFSET_LO));
+	stv0367_writebits(state, F367TER_FREEZE, 0);
+	if (offset > 8388607)
+		offset -= 16777216;
+
+	offset = offset * 2 / 16384;
+
+	if (ter_state->mode == FE_TER_MODE_2K)
+		offset = (offset * 4464) / 1000;/*** 1 FFT BIN=4.464khz***/
+	else if (ter_state->mode == FE_TER_MODE_4K)
+		offset = (offset * 223) / 100;/*** 1 FFT BIN=2.23khz***/
+	else  if (ter_state->mode == FE_TER_MODE_8K)
+		offset = (offset * 111) / 100;/*** 1 FFT BIN=1.1khz***/
+
+	if (stv0367_readbits(state, F367TER_PPM_INVSEL) == 1) {
+		if ((stv0367_readbits(state, F367TER_INV_SPECTR) ==
+				(stv0367_readbits(state,
+					F367TER_STATUS_INV_SPECRUM) == 1)))
+			offset = offset * -1;
+	}
+
+	if (ter_state->bw == 6)
+		offset = (offset * 6) / 8;
+	else if (ter_state->bw == 7)
+		offset = (offset * 7) / 8;
+
+	ter_state->frequency += offset;
+
+	tempo = 10;  /* exit even if timing_offset stays null */
+	while ((timing_offset == 0) && (tempo > 0)) {
+		usleep_range(10000, 20000);	/*was 20ms  */
+		/* fine tuning of timing offset if required */
+		timing_offset = stv0367_readbits(state, F367TER_TRL_TOFFSET_LO)
+				+ 256 * stv0367_readbits(state,
+							F367TER_TRL_TOFFSET_HI);
+		if (timing_offset >= 32768)
+			timing_offset -= 65536;
+		trl_nomrate = (512 * stv0367_readbits(state,
+							F367TER_TRL_NOMRATE_HI)
+			+ stv0367_readbits(state, F367TER_TRL_NOMRATE_LO) * 2
+			+ stv0367_readbits(state, F367TER_TRL_NOMRATE_LSB));
+
+		timing_offset = ((signed)(1000000 / trl_nomrate) *
+							timing_offset) / 2048;
+		tempo--;
+	}
+
+	if (timing_offset <= 0) {
+		timing_offset = (timing_offset - 11) / 22;
+		step = -1;
+	} else {
+		timing_offset = (timing_offset + 11) / 22;
+		step = 1;
+	}
+
+	for (counter = 0; counter < abs(timing_offset); counter++) {
+		trl_nomrate += step;
+		stv0367_writebits(state, F367TER_TRL_NOMRATE_LSB,
+						trl_nomrate % 2);
+		stv0367_writebits(state, F367TER_TRL_NOMRATE_LO,
+						trl_nomrate / 2);
+		usleep_range(1000, 2000);
+	}
+
+	usleep_range(5000, 6000);
+	/* unlocks could happen in case of trl centring big step,
+	then a core off/on restarts demod */
+	u_var = stv0367_readbits(state, F367TER_LK);
+
+	if (!u_var) {
+		stv0367_writebits(state, F367TER_CORE_ACTIVE, 0);
+		msleep(20);
+		stv0367_writebits(state, F367TER_CORE_ACTIVE, 1);
+	}
+
+	return 0;
+}
+
+static int stv0367ter_set_frontend(struct dvb_frontend *fe,
+				struct dvb_frontend_parameters *param)
+{
+	struct dvb_ofdm_parameters *op = &param->u.ofdm;
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+
+	/*u8 trials[2]; */
+	s8 num_trials, index;
+	u8 SenseTrials[] = { INVERSION_ON, INVERSION_OFF };
+
+	stv0367ter_init(fe);
+
+	if (fe->ops.tuner_ops.set_params) {
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 1);
+		fe->ops.tuner_ops.set_params(fe, param);
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 0);
+	}
+
+	switch (op->transmission_mode) {
+	default:
+	case TRANSMISSION_MODE_AUTO:
+	case TRANSMISSION_MODE_2K:
+		ter_state->mode = FE_TER_MODE_2K;
+		break;
+/*	case TRANSMISSION_MODE_4K:
+		pLook.mode = FE_TER_MODE_4K;
+		break;*/
+	case TRANSMISSION_MODE_8K:
+		ter_state->mode = FE_TER_MODE_8K;
+		break;
+	}
+
+	switch (op->guard_interval) {
+	default:
+	case GUARD_INTERVAL_1_32:
+	case GUARD_INTERVAL_1_16:
+	case GUARD_INTERVAL_1_8:
+	case GUARD_INTERVAL_1_4:
+		ter_state->guard = op->guard_interval;
+		break;
+	case GUARD_INTERVAL_AUTO:
+		ter_state->guard = GUARD_INTERVAL_1_32;
+		break;
+	}
+
+	switch (op->bandwidth) {
+	case BANDWIDTH_6_MHZ:
+		ter_state->bw = FE_TER_CHAN_BW_6M;
+		break;
+	case BANDWIDTH_7_MHZ:
+		ter_state->bw = FE_TER_CHAN_BW_7M;
+		break;
+	case BANDWIDTH_8_MHZ:
+	default:
+		ter_state->bw = FE_TER_CHAN_BW_8M;
+	}
+
+	ter_state->hierarchy = FE_TER_HIER_NONE;
+
+	switch (param->inversion) {
+	case INVERSION_OFF:
+	case INVERSION_ON:
+		num_trials = 1;
+		break;
+	default:
+		num_trials = 2;
+		if (ter_state->first_lock)
+			num_trials = 1;
+		break;
+	}
+
+	ter_state->state = FE_TER_NOLOCK;
+	index = 0;
+
+	while (((index) < num_trials) && (ter_state->state != FE_TER_LOCKOK)) {
+		if (!ter_state->first_lock) {
+			if (param->inversion == INVERSION_AUTO)
+				ter_state->sense = SenseTrials[index];
+
+		}
+		stv0367ter_algo(fe,/* &pLook, result,*/ param);
+
+		if ((ter_state->state == FE_TER_LOCKOK) &&
+				(param->inversion == INVERSION_AUTO) &&
+								(index == 1)) {
+			/* invert spectrum sense */
+			SenseTrials[index] = SenseTrials[0];
+			SenseTrials[(index + 1) % 2] = (SenseTrials[1] + 1) % 2;
+		}
+
+		index++;
+	}
+
+	return 0;
+}
+
+static int stv0367ter_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+	u32 errs = 0;
+
+	/*wait for counting completion*/
+	if (stv0367_readbits(state, F367TER_SFERRC_OLDVALUE) == 0) {
+		errs =
+			((u32)stv0367_readbits(state, F367TER_ERR_CNT1)
+			* (1 << 16))
+			+ ((u32)stv0367_readbits(state, F367TER_ERR_CNT1_HI)
+			* (1 << 8))
+			+ ((u32)stv0367_readbits(state, F367TER_ERR_CNT1_LO));
+		ter_state->ucblocks = errs;
+	}
+
+	(*ucblocks) = ter_state->ucblocks;
+
+	return 0;
+}
+
+static int stv0367ter_get_frontend(struct dvb_frontend *fe,
+				  struct dvb_frontend_parameters *param)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+	struct dvb_ofdm_parameters *op = &param->u.ofdm;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	int error = 0;
+	enum stv0367_ter_mode mode;
+	int constell = 0,/* snr = 0,*/ Data = 0;
+
+	param->frequency = stv0367_get_tuner_freq(fe);
+	if (param->frequency < 0)
+		param->frequency = c->frequency;
+
+	constell = stv0367_readbits(state, F367TER_TPS_CONST);
+	if (constell == 0)
+		op->constellation = QPSK;
+	else if (constell == 1)
+		op->constellation = QAM_16;
+	else
+		op->constellation = QAM_64;
+
+	param->inversion = stv0367_readbits(state, F367TER_INV_SPECTR);
+
+	/* Get the Hierarchical mode */
+	Data = stv0367_readbits(state, F367TER_TPS_HIERMODE);
+
+	switch (Data) {
+	case 0:
+		op->hierarchy_information = HIERARCHY_NONE;
+		break;
+	case 1:
+		op->hierarchy_information = HIERARCHY_1;
+		break;
+	case 2:
+		op->hierarchy_information = HIERARCHY_2;
+		break;
+	case 3:
+		op->hierarchy_information = HIERARCHY_4;
+		break;
+	default:
+		op->hierarchy_information = HIERARCHY_AUTO;
+		break; /* error */
+	}
+
+	/* Get the FEC Rate */
+	if (ter_state->hierarchy == FE_TER_HIER_LOW_PRIO)
+		Data = stv0367_readbits(state, F367TER_TPS_LPCODE);
+	else
+		Data = stv0367_readbits(state, F367TER_TPS_HPCODE);
+
+	switch (Data) {
+	case 0:
+		op->code_rate_HP = FEC_1_2;
+		break;
+	case 1:
+		op->code_rate_HP = FEC_2_3;
+		break;
+	case 2:
+		op->code_rate_HP = FEC_3_4;
+		break;
+	case 3:
+		op->code_rate_HP = FEC_5_6;
+		break;
+	case 4:
+		op->code_rate_HP = FEC_7_8;
+		break;
+	default:
+		op->code_rate_HP = FEC_AUTO;
+		break; /* error */
+	}
+
+	mode = stv0367_readbits(state, F367TER_SYR_MODE);
+
+	switch (mode) {
+	case FE_TER_MODE_2K:
+		op->transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+/*	case FE_TER_MODE_4K:
+		op->transmission_mode = TRANSMISSION_MODE_4K;
+		break;*/
+	case FE_TER_MODE_8K:
+		op->transmission_mode = TRANSMISSION_MODE_8K;
+		break;
+	default:
+		op->transmission_mode = TRANSMISSION_MODE_AUTO;
+	}
+
+	op->guard_interval = stv0367_readbits(state, F367TER_SYR_GUARD);
+
+	return error;
+}
+
+static int stv0367ter_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	u32 snru32 = 0;
+	int cpt = 0;
+	u8 cut = stv0367_readbits(state, F367TER_IDENTIFICATIONREG);
+
+	while (cpt < 10) {
+		usleep_range(2000, 3000);
+		if (cut == 0x50) /*cut 1.0 cut 1.1*/
+			snru32 += stv0367_readbits(state, F367TER_CHCSNR) / 4;
+		else /*cu2.0*/
+			snru32 += 125 * stv0367_readbits(state, F367TER_CHCSNR);
+
+		cpt++;
+	}
+
+	snru32 /= 10;/*average on 10 values*/
+
+	*snr = snru32 / 1000;
+
+	return 0;
+}
+
+#if 0
+static int stv0367ter_status(struct dvb_frontend *fe)
+{
+
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+	int locked = FALSE;
+
+	locked = (stv0367_readbits(state, F367TER_LK));
+	if (!locked)
+		ter_state->unlock_counter += 1;
+	else
+		ter_state->unlock_counter = 0;
+
+	if (ter_state->unlock_counter > 2) {
+		if (!stv0367_readbits(state, F367TER_TPS_LOCK) ||
+				(!stv0367_readbits(state, F367TER_LK))) {
+			stv0367_writebits(state, F367TER_CORE_ACTIVE, 0);
+			usleep_range(2000, 3000);
+			stv0367_writebits(state, F367TER_CORE_ACTIVE, 1);
+			msleep(350);
+			locked = (stv0367_readbits(state, F367TER_TPS_LOCK)) &&
+					(stv0367_readbits(state, F367TER_LK));
+		}
+
+	}
+
+	return locked;
+}
+#endif
+static int stv0367ter_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	dprintk("%s:\n", __func__);
+
+	*status = 0;
+
+	if (stv0367_readbits(state, F367TER_LK)) {
+		*status |= FE_HAS_LOCK;
+		dprintk("%s: stv0367 has locked\n", __func__);
+	}
+
+	return 0;
+}
+
+static int stv0367ter_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367ter_state *ter_state = state->ter_state;
+	u32 Errors = 0, tber = 0, temporary = 0;
+	int abc = 0, def = 0;
+
+
+	/*wait for counting completion*/
+	if (stv0367_readbits(state, F367TER_SFERRC_OLDVALUE) == 0)
+		Errors = ((u32)stv0367_readbits(state, F367TER_SFEC_ERR_CNT)
+			* (1 << 16))
+			+ ((u32)stv0367_readbits(state, F367TER_SFEC_ERR_CNT_HI)
+			* (1 << 8))
+			+ ((u32)stv0367_readbits(state,
+						F367TER_SFEC_ERR_CNT_LO));
+	/*measurement not completed, load previous value*/
+	else {
+		tber = ter_state->pBER;
+		return 0;
+	}
+
+	abc = stv0367_readbits(state, F367TER_SFEC_ERR_SOURCE);
+	def = stv0367_readbits(state, F367TER_SFEC_NUM_EVENT);
+
+	if (Errors == 0) {
+		tber = 0;
+	} else if (abc == 0x7) {
+		if (Errors <= 4) {
+			temporary = (Errors * 1000000000) / (8 * (1 << 14));
+			temporary =  temporary;
+		} else if (Errors <= 42) {
+			temporary = (Errors * 100000000) / (8 * (1 << 14));
+			temporary = temporary * 10;
+		} else if (Errors <= 429) {
+			temporary = (Errors * 10000000) / (8 * (1 << 14));
+			temporary = temporary * 100;
+		} else if (Errors <= 4294) {
+			temporary = (Errors * 1000000) / (8 * (1 << 14));
+			temporary = temporary * 1000;
+		} else if (Errors <= 42949) {
+			temporary = (Errors * 100000) / (8 * (1 << 14));
+			temporary = temporary * 10000;
+		} else if (Errors <= 429496) {
+			temporary = (Errors * 10000) / (8 * (1 << 14));
+			temporary = temporary * 100000;
+		} else { /*if (Errors<4294967) 2^22 max error*/
+			temporary = (Errors * 1000) / (8 * (1 << 14));
+			temporary = temporary * 100000;	/* still to *10 */
+		}
+
+		/* Byte error*/
+		if (def == 2)
+			/*tber=Errors/(8*(1 <<14));*/
+			tber = temporary;
+		else if (def == 3)
+			/*tber=Errors/(8*(1 <<16));*/
+			tber = temporary / 4;
+		else if (def == 4)
+			/*tber=Errors/(8*(1 <<18));*/
+			tber = temporary / 16;
+		else if (def == 5)
+			/*tber=Errors/(8*(1 <<20));*/
+			tber = temporary / 64;
+		else if (def == 6)
+			/*tber=Errors/(8*(1 <<22));*/
+			tber = temporary / 256;
+		else
+			/* should not pass here*/
+			tber = 0;
+
+		if ((Errors < 4294967) && (Errors > 429496))
+			tber *= 10;
+
+	}
+
+	/* save actual value */
+	ter_state->pBER = tber;
+
+	(*ber) = tber;
+
+	return 0;
+}
+#if 0
+static u32 stv0367ter_get_per(struct stv0367_state *state)
+{
+	struct stv0367ter_state *ter_state = state->ter_state;
+	u32 Errors = 0, Per = 0, temporary = 0;
+	int abc = 0, def = 0, cpt = 0;
+
+	while (((stv0367_readbits(state, F367TER_SFERRC_OLDVALUE) == 1) &&
+			(cpt < 400)) || ((Errors == 0) && (cpt < 400))) {
+		usleep_range(1000, 2000);
+		Errors = ((u32)stv0367_readbits(state, F367TER_ERR_CNT1)
+			* (1 << 16))
+			+ ((u32)stv0367_readbits(state, F367TER_ERR_CNT1_HI)
+			* (1 << 8))
+			+ ((u32)stv0367_readbits(state, F367TER_ERR_CNT1_LO));
+		cpt++;
+	}
+	abc = stv0367_readbits(state, F367TER_ERR_SRC1);
+	def = stv0367_readbits(state, F367TER_NUM_EVT1);
+
+	if (Errors == 0)
+		Per = 0;
+	else if (abc == 0x9) {
+		if (Errors <= 4) {
+			temporary = (Errors * 1000000000) / (8 * (1 << 8));
+			temporary =  temporary;
+		} else if (Errors <= 42) {
+			temporary = (Errors * 100000000) / (8 * (1 << 8));
+			temporary = temporary * 10;
+		} else if (Errors <= 429) {
+			temporary = (Errors * 10000000) / (8 * (1 << 8));
+			temporary = temporary * 100;
+		} else if (Errors <= 4294) {
+			temporary = (Errors * 1000000) / (8 * (1 << 8));
+			temporary = temporary * 1000;
+		} else if (Errors <= 42949) {
+			temporary = (Errors * 100000) / (8 * (1 << 8));
+			temporary = temporary * 10000;
+		} else { /*if(Errors<=429496)  2^16 errors max*/
+			temporary = (Errors * 10000) / (8 * (1 << 8));
+			temporary = temporary * 100000;
+		}
+
+		/* pkt error*/
+		if (def == 2)
+			/*Per=Errors/(1 << 8);*/
+			Per = temporary;
+		else if (def == 3)
+			/*Per=Errors/(1 << 10);*/
+			Per = temporary / 4;
+		else if (def == 4)
+			/*Per=Errors/(1 << 12);*/
+			Per = temporary / 16;
+		else if (def == 5)
+			/*Per=Errors/(1 << 14);*/
+			Per = temporary / 64;
+		else if (def == 6)
+			/*Per=Errors/(1 << 16);*/
+			Per = temporary / 256;
+		else
+			Per = 0;
+
+	}
+	/* save actual value */
+	ter_state->pPER = Per;
+
+	return Per;
+}
+#endif
+static int stv0367_get_tune_settings(struct dvb_frontend *fe,
+					struct dvb_frontend_tune_settings
+					*fe_tune_settings)
+{
+	fe_tune_settings->min_delay_ms = 1000;
+	fe_tune_settings->step_size = 0;
+	fe_tune_settings->max_drift = 0;
+
+	return 0;
+}
+
+static void stv0367_release(struct dvb_frontend *fe)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	kfree(state->ter_state);
+	kfree(state->cab_state);
+	kfree(state);
+}
+
+static struct dvb_frontend_ops stv0367ter_ops = {
+	.info = {
+		.name			= "ST STV0367 DVB-T",
+		.type			= FE_OFDM,
+		.frequency_min		= 47000000,
+		.frequency_max		= 862000000,
+		.frequency_stepsize	= 15625,
+		.frequency_tolerance	= 0,
+		.caps = FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 |
+			FE_CAN_FEC_3_4 | FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 |
+			FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 |
+			FE_CAN_QAM_128 | FE_CAN_QAM_256 | FE_CAN_QAM_AUTO |
+			FE_CAN_TRANSMISSION_MODE_AUTO | FE_CAN_RECOVER |
+			FE_CAN_INVERSION_AUTO |
+			FE_CAN_MUTE_TS
+	},
+	.release = stv0367_release,
+	.init = stv0367ter_init,
+	.sleep = stv0367ter_sleep,
+	.i2c_gate_ctrl = stv0367ter_gate_ctrl,
+	.set_frontend = stv0367ter_set_frontend,
+	.get_frontend = stv0367ter_get_frontend,
+	.get_tune_settings = stv0367_get_tune_settings,
+	.read_status = stv0367ter_read_status,
+	.read_ber = stv0367ter_read_ber,/* too slow */
+/*	.read_signal_strength = stv0367_read_signal_strength,*/
+	.read_snr = stv0367ter_read_snr,
+	.read_ucblocks = stv0367ter_read_ucblocks,
+};
+
+struct dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
+				   struct i2c_adapter *i2c)
+{
+	struct stv0367_state *state = NULL;
+	struct stv0367ter_state *ter_state = NULL;
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct stv0367_state), GFP_KERNEL);
+	if (state == NULL)
+		goto error;
+	ter_state = kzalloc(sizeof(struct stv0367ter_state), GFP_KERNEL);
+	if (ter_state == NULL)
+		goto error;
+
+	/* setup the state */
+	state->i2c = i2c;
+	state->config = config;
+	state->ter_state = ter_state;
+	state->fe.ops = stv0367ter_ops;
+	state->fe.demodulator_priv = state;
+	state->chip_id = stv0367_readreg(state, 0xf000);
+
+	dprintk("%s: chip_id = 0x%x\n", __func__, state->chip_id);
+
+	/* check if the demod is there */
+	if ((state->chip_id != 0x50) && (state->chip_id != 0x60))
+		goto error;
+
+	return &state->fe;
+
+error:
+	kfree(ter_state);
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(stv0367ter_attach);
+
+static int stv0367cab_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	dprintk("%s:\n", __func__);
+
+	stv0367_writebits(state, F367CAB_I2CT_ON, (enable > 0) ? 1 : 0);
+
+	return 0;
+}
+
+static u32 stv0367cab_get_mclk(struct dvb_frontend *fe, u32 ExtClk_Hz)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	u32 mclk_Hz = 0;/* master clock frequency (Hz) */
+	u32 M, N, P;
+
+
+	if (stv0367_readbits(state, F367CAB_BYPASS_PLLXN) == 0) {
+		N = (u32)stv0367_readbits(state, F367CAB_PLL_NDIV);
+		if (N == 0)
+			N = N + 1;
+
+		M = (u32)stv0367_readbits(state, F367CAB_PLL_MDIV);
+		if (M == 0)
+			M = M + 1;
+
+		P = (u32)stv0367_readbits(state, F367CAB_PLL_PDIV);
+
+		if (P > 5)
+			P = 5;
+
+		mclk_Hz = ((ExtClk_Hz / 2) * N) / (M * (1 << P));
+		dprintk("stv0367cab_get_mclk BYPASS_PLLXN mclk_Hz=%d\n",
+								mclk_Hz);
+	} else
+		mclk_Hz = ExtClk_Hz;
+
+	dprintk("stv0367cab_get_mclk final mclk_Hz=%d\n", mclk_Hz);
+
+	return mclk_Hz;
+}
+
+static u32 stv0367cab_get_adc_freq(struct dvb_frontend *fe, u32 ExtClk_Hz)
+{
+	u32 ADCClk_Hz = ExtClk_Hz;
+
+	ADCClk_Hz = stv0367cab_get_mclk(fe, ExtClk_Hz);
+
+	return ADCClk_Hz;
+}
+
+enum stv0367cab_mod stv0367cab_SetQamSize(struct stv0367_state *state,
+					u32 SymbolRate,
+					enum stv0367cab_mod QAMSize)
+{
+	/* Set QAM size */
+	stv0367_writebits(state, F367CAB_QAM_MODE, QAMSize);
+
+	/* Set Registers settings specific to the QAM size */
+	switch (QAMSize) {
+	case FE_CAB_MOD_QAM4:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x00);
+		break;
+	case FE_CAB_MOD_QAM16:
+		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x64);
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x00);
+		stv0367_writereg(state, R367CAB_FSM_STATE, 0x90);
+		stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa7);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LD_SEN, 0x95);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LIMITER, 0x40);
+		stv0367_writereg(state, R367CAB_EQU_PNT_GAIN, 0x8a);
+		break;
+	case FE_CAB_MOD_QAM32:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x00);
+		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x6e);
+		stv0367_writereg(state, R367CAB_FSM_STATE, 0xb0);
+		stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xb7);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LD_SEN, 0x9d);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LIMITER, 0x7f);
+		stv0367_writereg(state, R367CAB_EQU_PNT_GAIN, 0xa7);
+		break;
+	case FE_CAB_MOD_QAM64:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x82);
+		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x5a);
+		if (SymbolRate > 45000000) {
+			stv0367_writereg(state, R367CAB_FSM_STATE, 0xb0);
+			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
+			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa5);
+		} else if (SymbolRate > 25000000) {
+			stv0367_writereg(state, R367CAB_FSM_STATE, 0xa0);
+			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
+			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa6);
+		} else {
+			stv0367_writereg(state, R367CAB_FSM_STATE, 0xa0);
+			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xd1);
+			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa7);
+		}
+		stv0367_writereg(state, R367CAB_EQU_CRL_LD_SEN, 0x95);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LIMITER, 0x40);
+		stv0367_writereg(state, R367CAB_EQU_PNT_GAIN, 0x99);
+		break;
+	case FE_CAB_MOD_QAM128:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x00);
+		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x76);
+		stv0367_writereg(state, R367CAB_FSM_STATE, 0x90);
+		stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xb1);
+		if (SymbolRate > 45000000)
+			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa7);
+		else if (SymbolRate > 25000000)
+			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa6);
+		else
+			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0x97);
+
+		stv0367_writereg(state, R367CAB_EQU_CRL_LD_SEN, 0x8e);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LIMITER, 0x7f);
+		stv0367_writereg(state, R367CAB_EQU_PNT_GAIN, 0xa7);
+		break;
+	case FE_CAB_MOD_QAM256:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x94);
+		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x5a);
+		stv0367_writereg(state, R367CAB_FSM_STATE, 0xa0);
+		if (SymbolRate > 45000000)
+			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
+		else if (SymbolRate > 25000000)
+			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
+		else
+			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xd1);
+
+		stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa7);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LD_SEN, 0x85);
+		stv0367_writereg(state, R367CAB_EQU_CRL_LIMITER, 0x40);
+		stv0367_writereg(state, R367CAB_EQU_PNT_GAIN, 0xa7);
+		break;
+	case FE_CAB_MOD_QAM512:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x00);
+		break;
+	case FE_CAB_MOD_QAM1024:
+		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x00);
+		break;
+	default:
+		break;
+	}
+
+	return QAMSize;
+}
+
+static u32 stv0367cab_set_derot_freq(struct stv0367_state *state,
+					u32 adc_hz, s32 derot_hz)
+{
+	u32 sampled_if = 0;
+	u32 adc_khz;
+
+	adc_khz = adc_hz / 1000;
+
+	dprintk("%s: adc_hz=%d derot_hz=%d\n", __func__, adc_hz, derot_hz);
+
+	if (adc_khz != 0) {
+		if (derot_hz < 1000000)
+			derot_hz = adc_hz / 4; /* ZIF operation */
+		if (derot_hz > adc_hz)
+			derot_hz = derot_hz - adc_hz;
+		sampled_if = (u32)derot_hz / 1000;
+		sampled_if *= 32768;
+		sampled_if /= adc_khz;
+		sampled_if *= 256;
+	}
+
+	if (sampled_if > 8388607)
+		sampled_if = 8388607;
+
+	dprintk("%s: sampled_if=0x%x\n", __func__, sampled_if);
+
+	stv0367_writereg(state, R367CAB_MIX_NCO_LL, sampled_if);
+	stv0367_writereg(state, R367CAB_MIX_NCO_HL, (sampled_if >> 8));
+	stv0367_writebits(state, F367CAB_MIX_NCO_INC_HH, (sampled_if >> 16));
+
+	return derot_hz;
+}
+
+static u32 stv0367cab_get_derot_freq(struct stv0367_state *state, u32 adc_hz)
+{
+	u32 sampled_if;
+
+	sampled_if = stv0367_readbits(state, F367CAB_MIX_NCO_INC_LL) +
+			(stv0367_readbits(state, F367CAB_MIX_NCO_INC_HL) << 8) +
+			(stv0367_readbits(state, F367CAB_MIX_NCO_INC_HH) << 16);
+
+	sampled_if /= 256;
+	sampled_if *= (adc_hz / 1000);
+	sampled_if += 1;
+	sampled_if /= 32768;
+
+	return sampled_if;
+}
+
+static u32 stv0367cab_set_srate(struct stv0367_state *state, u32 adc_hz,
+			u32 mclk_hz, u32 SymbolRate,
+			enum stv0367cab_mod QAMSize)
+{
+	u32 QamSizeCorr = 0;
+	u32 u32_tmp = 0, u32_tmp1 = 0;
+	u32 adp_khz;
+
+	dprintk("%s:\n", __func__);
+
+	/* Set Correction factor of SRC gain */
+	switch (QAMSize) {
+	case FE_CAB_MOD_QAM4:
+		QamSizeCorr = 1110;
+		break;
+	case FE_CAB_MOD_QAM16:
+		QamSizeCorr = 1032;
+		break;
+	case FE_CAB_MOD_QAM32:
+		QamSizeCorr =  954;
+		break;
+	case FE_CAB_MOD_QAM64:
+		QamSizeCorr =  983;
+		break;
+	case FE_CAB_MOD_QAM128:
+		QamSizeCorr =  957;
+		break;
+	case FE_CAB_MOD_QAM256:
+		QamSizeCorr =  948;
+		break;
+	case FE_CAB_MOD_QAM512:
+		QamSizeCorr =    0;
+		break;
+	case FE_CAB_MOD_QAM1024:
+		QamSizeCorr =  944;
+		break;
+	default:
+		break;
+	}
+
+	/* Transfer ratio calculation */
+	if (adc_hz != 0) {
+		u32_tmp = 256 * SymbolRate;
+		u32_tmp = u32_tmp / adc_hz;
+	}
+	stv0367_writereg(state, R367CAB_EQU_CRL_TFR, (u8)u32_tmp);
+
+	/* Symbol rate and SRC gain calculation */
+	adp_khz = (mclk_hz >> 1) / 1000;/* TRL works at half the system clock */
+	if (adp_khz != 0) {
+		u32_tmp = SymbolRate;
+		u32_tmp1 = SymbolRate;
+
+		if (u32_tmp < 2097152) { /* 2097152 = 2^21 */
+			/* Symbol rate calculation */
+			u32_tmp *= 2048; /* 2048 = 2^11 */
+			u32_tmp = u32_tmp / adp_khz;
+			u32_tmp = u32_tmp * 16384; /* 16384 = 2^14 */
+			u32_tmp /= 125 ; /* 125 = 1000/2^3 */
+			u32_tmp = u32_tmp * 8; /* 8 = 2^3 */
+
+			/* SRC Gain Calculation */
+			u32_tmp1 *= 2048; /* *2*2^10 */
+			u32_tmp1 /= 439; /* *2/878 */
+			u32_tmp1 *= 256; /* *2^8 */
+			u32_tmp1 = u32_tmp1 / adp_khz; /* /(AdpClk in kHz) */
+			u32_tmp1 *= QamSizeCorr * 9; /* *1000*corr factor */
+			u32_tmp1 = u32_tmp1 / 10000000;
+
+		} else if (u32_tmp < 4194304) { /* 4194304 = 2**22 */
+			/* Symbol rate calculation */
+			u32_tmp *= 1024 ; /* 1024 = 2**10 */
+			u32_tmp = u32_tmp / adp_khz;
+			u32_tmp = u32_tmp * 16384; /* 16384 = 2**14 */
+			u32_tmp /= 125 ; /* 125 = 1000/2**3 */
+			u32_tmp = u32_tmp * 16; /* 16 = 2**4 */
+
+			/* SRC Gain Calculation */
+			u32_tmp1 *= 1024; /* *2*2^9 */
+			u32_tmp1 /= 439; /* *2/878 */
+			u32_tmp1 *= 256; /* *2^8 */
+			u32_tmp1 = u32_tmp1 / adp_khz; /* /(AdpClk in kHz)*/
+			u32_tmp1 *= QamSizeCorr * 9; /* *1000*corr factor */
+			u32_tmp1 = u32_tmp1 / 5000000;
+		} else if (u32_tmp < 8388607) { /* 8388607 = 2**23 */
+			/* Symbol rate calculation */
+			u32_tmp *= 512 ; /* 512 = 2**9 */
+			u32_tmp = u32_tmp / adp_khz;
+			u32_tmp = u32_tmp * 16384; /* 16384 = 2**14 */
+			u32_tmp /= 125 ; /* 125 = 1000/2**3 */
+			u32_tmp = u32_tmp * 32; /* 32 = 2**5 */
+
+			/* SRC Gain Calculation */
+			u32_tmp1 *= 512; /* *2*2^8 */
+			u32_tmp1 /= 439; /* *2/878 */
+			u32_tmp1 *= 256; /* *2^8 */
+			u32_tmp1 = u32_tmp1 / adp_khz; /* /(AdpClk in kHz) */
+			u32_tmp1 *= QamSizeCorr * 9; /* *1000*corr factor */
+			u32_tmp1 = u32_tmp1 / 2500000;
+		} else {
+			/* Symbol rate calculation */
+			u32_tmp *= 256 ; /* 256 = 2**8 */
+			u32_tmp = u32_tmp / adp_khz;
+			u32_tmp = u32_tmp * 16384; /* 16384 = 2**13 */
+			u32_tmp /= 125 ; /* 125 = 1000/2**3 */
+			u32_tmp = u32_tmp * 64; /* 64 = 2**6 */
+
+			/* SRC Gain Calculation */
+			u32_tmp1 *= 256; /* 2*2^7 */
+			u32_tmp1 /= 439; /* *2/878 */
+			u32_tmp1 *= 256; /* *2^8 */
+			u32_tmp1 = u32_tmp1 / adp_khz; /* /(AdpClk in kHz) */
+			u32_tmp1 *= QamSizeCorr * 9; /* *1000*corr factor */
+			u32_tmp1 = u32_tmp1 / 1250000;
+		}
+	}
+#if 0
+	/* Filters' coefficients are calculated and written
+	into registers only if the filters are enabled */
+	if (stv0367_readbits(state, F367CAB_ADJ_EN)) {
+		stv0367cab_SetIirAdjacentcoefficient(state, mclk_hz,
+								SymbolRate);
+		/* AllPass filter must be enabled
+		when the adjacents filter is used */
+		stv0367_writebits(state, F367CAB_ALLPASSFILT_EN, 1);
+		stv0367cab_SetAllPasscoefficient(state, mclk_hz, SymbolRate);
+	} else
+		/* AllPass filter must be disabled
+		when the adjacents filter is not used */
+#endif
+	stv0367_writebits(state, F367CAB_ALLPASSFILT_EN, 0);
+
+	stv0367_writereg(state, R367CAB_SRC_NCO_LL, u32_tmp);
+	stv0367_writereg(state, R367CAB_SRC_NCO_LH, (u32_tmp >> 8));
+	stv0367_writereg(state, R367CAB_SRC_NCO_HL, (u32_tmp >> 16));
+	stv0367_writereg(state, R367CAB_SRC_NCO_HH, (u32_tmp >> 24));
+
+	stv0367_writereg(state, R367CAB_IQDEM_GAIN_SRC_L, u32_tmp1 & 0x00ff);
+	stv0367_writebits(state, F367CAB_GAIN_SRC_HI, (u32_tmp1 >> 8) & 0x00ff);
+
+	return SymbolRate ;
+}
+
+static u32 stv0367cab_GetSymbolRate(struct stv0367_state *state, u32 mclk_hz)
+{
+	u32 regsym;
+	u32 adp_khz;
+
+	regsym = stv0367_readreg(state, R367CAB_SRC_NCO_LL) +
+		(stv0367_readreg(state, R367CAB_SRC_NCO_LH) << 8) +
+		(stv0367_readreg(state, R367CAB_SRC_NCO_HL) << 16) +
+		(stv0367_readreg(state, R367CAB_SRC_NCO_HH) << 24);
+
+	adp_khz = (mclk_hz >> 1) / 1000;/* TRL works at half the system clock */
+
+	if (regsym < 134217728) {		/* 134217728L = 2**27*/
+		regsym = regsym * 32;		/* 32 = 2**5 */
+		regsym = regsym / 32768;	/* 32768L = 2**15 */
+		regsym = adp_khz * regsym;	/* AdpClk in kHz */
+		regsym = regsym / 128;		/* 128 = 2**7 */
+		regsym *= 125 ;			/* 125 = 1000/2**3 */
+		regsym /= 2048 ;		/* 2048 = 2**11	*/
+	} else if (regsym < 268435456) {	/* 268435456L = 2**28 */
+		regsym = regsym * 16;		/* 16 = 2**4 */
+		regsym = regsym / 32768;	/* 32768L = 2**15 */
+		regsym = adp_khz * regsym;	/* AdpClk in kHz */
+		regsym = regsym / 128;		/* 128 = 2**7 */
+		regsym *= 125 ;			/* 125 = 1000/2**3*/
+		regsym /= 1024 ;		/* 256 = 2**10*/
+	} else if (regsym < 536870912) {	/* 536870912L = 2**29*/
+		regsym = regsym * 8;		/* 8 = 2**3 */
+		regsym = regsym / 32768;	/* 32768L = 2**15 */
+		regsym = adp_khz * regsym;	/* AdpClk in kHz */
+		regsym = regsym / 128;		/* 128 = 2**7 */
+		regsym *= 125 ;			/* 125 = 1000/2**3 */
+		regsym /= 512 ;			/* 128 = 2**9 */
+	} else {
+		regsym = regsym * 4;		/* 4 = 2**2 */
+		regsym = regsym / 32768;	/* 32768L = 2**15 */
+		regsym = adp_khz * regsym;	/* AdpClk in kHz */
+		regsym = regsym / 128;		/* 128 = 2**7 */
+		regsym *= 125 ;			/* 125 = 1000/2**3 */
+		regsym /= 256 ;			/* 64 = 2**8 */
+	}
+
+	return regsym;
+}
+
+static int stv0367cab_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	dprintk("%s:\n", __func__);
+
+	*status = 0;
+
+	if (stv0367_readbits(state, F367CAB_QAMFEC_LOCK)) {
+		*status |= FE_HAS_LOCK;
+		dprintk("%s: stv0367 has locked\n", __func__);
+	}
+
+	return 0;
+}
+
+static int stv0367cab_standby(struct dvb_frontend *fe, u8 standby_on)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	dprintk("%s:\n", __func__);
+
+	if (standby_on) {
+		stv0367_writebits(state, F367CAB_BYPASS_PLLXN, 0x03);
+		stv0367_writebits(state, F367CAB_STDBY_PLLXN, 0x01);
+		stv0367_writebits(state, F367CAB_STDBY, 1);
+		stv0367_writebits(state, F367CAB_STDBY_CORE, 1);
+		stv0367_writebits(state, F367CAB_EN_BUFFER_I, 0);
+		stv0367_writebits(state, F367CAB_EN_BUFFER_Q, 0);
+		stv0367_writebits(state, F367CAB_POFFQ, 1);
+		stv0367_writebits(state, F367CAB_POFFI, 1);
+	} else {
+		stv0367_writebits(state, F367CAB_STDBY_PLLXN, 0x00);
+		stv0367_writebits(state, F367CAB_BYPASS_PLLXN, 0x00);
+		stv0367_writebits(state, F367CAB_STDBY, 0);
+		stv0367_writebits(state, F367CAB_STDBY_CORE, 0);
+		stv0367_writebits(state, F367CAB_EN_BUFFER_I, 1);
+		stv0367_writebits(state, F367CAB_EN_BUFFER_Q, 1);
+		stv0367_writebits(state, F367CAB_POFFQ, 0);
+		stv0367_writebits(state, F367CAB_POFFI, 0);
+	}
+
+	return 0;
+}
+
+static int stv0367cab_sleep(struct dvb_frontend *fe)
+{
+	return stv0367cab_standby(fe, 1);
+}
+
+int stv0367cab_init(struct dvb_frontend *fe)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367cab_state *cab_state = state->cab_state;
+	int i;
+
+	dprintk("%s:\n", __func__);
+
+	for (i = 0; i < STV0367CAB_NBREGS; i++)
+		stv0367_writereg(state, def0367cab[i].addr,
+						def0367cab[i].value);
+
+	switch (state->config->ts_mode) {
+	case STV0367_DVBCI_CLOCK:
+		dprintk("Setting TSMode = STV0367_DVBCI_CLOCK\n");
+		stv0367_writebits(state, F367CAB_OUTFORMAT, 0x03);
+		break;
+	case STV0367_SERIAL_PUNCT_CLOCK:
+	case STV0367_SERIAL_CONT_CLOCK:
+		stv0367_writebits(state, F367CAB_OUTFORMAT, 0x01);
+		break;
+	case STV0367_PARALLEL_PUNCT_CLOCK:
+	case STV0367_OUTPUTMODE_DEFAULT:
+		stv0367_writebits(state, F367CAB_OUTFORMAT, 0x00);
+		break;
+	}
+
+	switch (state->config->clk_pol) {
+	case STV0367_RISINGEDGE_CLOCK:
+		stv0367_writebits(state, F367CAB_CLK_POLARITY, 0x00);
+		break;
+	case STV0367_FALLINGEDGE_CLOCK:
+	case STV0367_CLOCKPOLARITY_DEFAULT:
+		stv0367_writebits(state, F367CAB_CLK_POLARITY, 0x01);
+		break;
+	}
+
+	stv0367_writebits(state, F367CAB_SYNC_STRIP, 0x00);
+
+	stv0367_writebits(state, F367CAB_CT_NBST, 0x01);
+
+	stv0367_writebits(state, F367CAB_TS_SWAP, 0x01);
+
+	stv0367_writebits(state, F367CAB_FIFO_BYPASS, 0x00);
+
+	stv0367_writereg(state, R367CAB_ANACTRL, 0x00);/*PLL enabled and used */
+
+	cab_state->mclk = stv0367cab_get_mclk(fe, state->config->xtal);
+	cab_state->adc_clk = stv0367cab_get_adc_freq(fe, state->config->xtal);
+
+	return 0;
+}
+static
+enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
+				struct dvb_frontend_parameters *param)
+{
+	struct dvb_qam_parameters *op = &param->u.qam;
+	struct stv0367cab_state *cab_state = state->cab_state;
+	enum stv0367_cab_signal_type signalType = FE_CAB_NOAGC;
+	u32	QAMFEC_Lock, QAM_Lock, u32_tmp,
+		LockTime, TRLTimeOut, AGCTimeOut, CRLSymbols,
+		CRLTimeOut, EQLTimeOut, DemodTimeOut, FECTimeOut;
+	u8	TrackAGCAccum;
+	s32	tmp;
+
+	dprintk("%s:\n", __func__);
+
+	/* Timeouts calculation */
+	/* A max lock time of 25 ms is allowed for delayed AGC */
+	AGCTimeOut = 25;
+	/* 100000 symbols needed by the TRL as a maximum value */
+	TRLTimeOut = 100000000 / op->symbol_rate;
+	/* CRLSymbols is the needed number of symbols to achieve a lock
+	   within [-4%, +4%] of the symbol rate.
+	   CRL timeout is calculated
+	   for a lock within [-search_range, +search_range].
+	   EQL timeout can be changed depending on
+	   the micro-reflections we want to handle.
+	   A characterization must be performed
+	   with these echoes to get new timeout values.
+	*/
+	switch (op->modulation) {
+	case QAM_16:
+		CRLSymbols = 150000;
+		EQLTimeOut = 100;
+		break;
+	case QAM_32:
+		CRLSymbols = 250000;
+		EQLTimeOut = 100;
+		break;
+	case QAM_64:
+		CRLSymbols = 200000;
+		EQLTimeOut = 100;
+		break;
+	case QAM_128:
+		CRLSymbols = 250000;
+		EQLTimeOut = 100;
+		break;
+	case QAM_256:
+		CRLSymbols = 250000;
+		EQLTimeOut = 100;
+		break;
+	default:
+		CRLSymbols = 200000;
+		EQLTimeOut = 100;
+		break;
+	}
+#if 0
+	if (pIntParams->search_range < 0) {
+		CRLTimeOut = (25 * CRLSymbols *
+				(-pIntParams->search_range / 1000)) /
+					(pIntParams->symbol_rate / 1000);
+	} else
+#endif
+	CRLTimeOut = (25 * CRLSymbols * (cab_state->search_range / 1000)) /
+					(op->symbol_rate / 1000);
+
+	CRLTimeOut = (1000 * CRLTimeOut) / op->symbol_rate;
+	/* Timeouts below 50ms are coerced */
+	if (CRLTimeOut < 50)
+		CRLTimeOut = 50;
+	/* A maximum of 100 TS packets is needed to get FEC lock even in case
+	the spectrum inversion needs to be changed.
+	   This is equal to 20 ms in case of the lowest symbol rate of 0.87Msps
+	*/
+	FECTimeOut = 20;
+	DemodTimeOut = AGCTimeOut + TRLTimeOut + CRLTimeOut + EQLTimeOut;
+
+	dprintk("%s: DemodTimeOut=%d\n", __func__, DemodTimeOut);
+
+	/* Reset the TRL to ensure nothing starts until the
+	   AGC is stable which ensures a better lock time
+	*/
+	stv0367_writereg(state, R367CAB_CTRL_1, 0x04);
+	/* Set AGC accumulation time to minimum and lock threshold to maximum
+	in order to speed up the AGC lock */
+	TrackAGCAccum = stv0367_readbits(state, F367CAB_AGC_ACCUMRSTSEL);
+	stv0367_writebits(state, F367CAB_AGC_ACCUMRSTSEL, 0x0);
+	/* Modulus Mapper is disabled */
+	stv0367_writebits(state, F367CAB_MODULUSMAP_EN, 0);
+	/* Disable the sweep function */
+	stv0367_writebits(state, F367CAB_SWEEP_EN, 0);
+	/* The sweep function is never used, Sweep rate must be set to 0 */
+	/* Set the derotator frequency in Hz */
+	stv0367cab_set_derot_freq(state, cab_state->adc_clk,
+		(1000 * (s32)state->config->if_khz + cab_state->derot_offset));
+	/* Disable the Allpass Filter when the symbol rate is out of range */
+	if ((op->symbol_rate > 10800000) | (op->symbol_rate < 1800000)) {
+		stv0367_writebits(state, F367CAB_ADJ_EN, 0);
+		stv0367_writebits(state, F367CAB_ALLPASSFILT_EN, 0);
+	}
+#if 0
+	/* Check if the tuner is locked */
+	tuner_lock = stv0367cab_tuner_get_status(fe);
+	if (tuner_lock == 0)
+		return FE_367CAB_NOTUNER;
+#endif
+	/* Relase the TRL to start demodulator acquisition */
+	/* Wait for QAM lock */
+	LockTime = 0;
+	stv0367_writereg(state, R367CAB_CTRL_1, 0x00);
+	do {
+		QAM_Lock = stv0367_readbits(state, F367CAB_FSM_STATUS);
+		if ((LockTime >= (DemodTimeOut - EQLTimeOut)) &&
+							(QAM_Lock == 0x04))
+			/*
+			 * We don't wait longer, the frequency/phase offset
+			 * must be too big
+			 */
+			LockTime = DemodTimeOut;
+		else if ((LockTime >= (AGCTimeOut + TRLTimeOut)) &&
+							(QAM_Lock == 0x02))
+			/*
+			 * We don't wait longer, either there is no signal or
+			 * it is not the right symbol rate or it is an analog
+			 * carrier
+			 */
+		{
+			LockTime = DemodTimeOut;
+			u32_tmp = stv0367_readbits(state,
+						F367CAB_AGC_PWR_WORD_LO) +
+					(stv0367_readbits(state,
+						F367CAB_AGC_PWR_WORD_ME) << 8) +
+					(stv0367_readbits(state,
+						F367CAB_AGC_PWR_WORD_HI) << 16);
+			if (u32_tmp >= 131072)
+				u32_tmp = 262144 - u32_tmp;
+			u32_tmp = u32_tmp / (1 << (11 - stv0367_readbits(state,
+							F367CAB_AGC_IF_BWSEL)));
+
+			if (u32_tmp < stv0367_readbits(state,
+						F367CAB_AGC_PWRREF_LO) +
+					256 * stv0367_readbits(state,
+						F367CAB_AGC_PWRREF_HI) - 10)
+				QAM_Lock = 0x0f;
+		} else {
+			usleep_range(10000, 20000);
+			LockTime += 10;
+		}
+		dprintk("QAM_Lock=0x%x LockTime=%d\n", QAM_Lock, LockTime);
+		tmp = stv0367_readreg(state, R367CAB_IT_STATUS1);
+
+		dprintk("R367CAB_IT_STATUS1=0x%x\n", tmp);
+
+	} while (((QAM_Lock != 0x0c) && (QAM_Lock != 0x0b)) &&
+						(LockTime < DemodTimeOut));
+
+	dprintk("QAM_Lock=0x%x\n", QAM_Lock);
+
+	tmp = stv0367_readreg(state, R367CAB_IT_STATUS1);
+	dprintk("R367CAB_IT_STATUS1=0x%x\n", tmp);
+	tmp = stv0367_readreg(state, R367CAB_IT_STATUS2);
+	dprintk("R367CAB_IT_STATUS2=0x%x\n", tmp);
+
+	tmp  = stv0367cab_get_derot_freq(state, cab_state->adc_clk);
+	dprintk("stv0367cab_get_derot_freq=0x%x\n", tmp);
+
+	if ((QAM_Lock == 0x0c) || (QAM_Lock == 0x0b)) {
+		/* Wait for FEC lock */
+		LockTime = 0;
+		do {
+			usleep_range(5000, 7000);
+			LockTime += 5;
+			QAMFEC_Lock = stv0367_readbits(state,
+							F367CAB_QAMFEC_LOCK);
+		} while (!QAMFEC_Lock && (LockTime < FECTimeOut));
+	} else
+		QAMFEC_Lock = 0;
+
+	if (QAMFEC_Lock) {
+		signalType = FE_CAB_DATAOK;
+		cab_state->modulation = op->modulation;
+		cab_state->spect_inv = stv0367_readbits(state,
+							F367CAB_QUAD_INV);
+#if 0
+/* not clear for me */
+		if (state->config->if_khz != 0) {
+			if (state->config->if_khz > cab_state->adc_clk / 1000) {
+				cab_state->freq_khz =
+					FE_Cab_TunerGetFrequency(pIntParams->hTuner)
+				- stv0367cab_get_derot_freq(state, cab_state->adc_clk)
+				- cab_state->adc_clk / 1000 + state->config->if_khz;
+			} else {
+				cab_state->freq_khz =
+						FE_Cab_TunerGetFrequency(pIntParams->hTuner)
+						- stv0367cab_get_derot_freq(state, cab_state->adc_clk)
+										+ state->config->if_khz;
+			}
+		} else {
+			cab_state->freq_khz =
+				FE_Cab_TunerGetFrequency(pIntParams->hTuner) +
+				stv0367cab_get_derot_freq(state,
+							cab_state->adc_clk) -
+				cab_state->adc_clk / 4000;
+		}
+#endif
+		cab_state->symbol_rate = stv0367cab_GetSymbolRate(state,
+							cab_state->mclk);
+		cab_state->locked = 1;
+
+		/* stv0367_setbits(state, F367CAB_AGC_ACCUMRSTSEL,7);*/
+	} else {
+		switch (QAM_Lock) {
+		case 1:
+			signalType = FE_CAB_NOAGC;
+			break;
+		case 2:
+			signalType = FE_CAB_NOTIMING;
+			break;
+		case 3:
+			signalType = FE_CAB_TIMINGOK;
+			break;
+		case 4:
+			signalType = FE_CAB_NOCARRIER;
+			break;
+		case 5:
+			signalType = FE_CAB_CARRIEROK;
+			break;
+		case 7:
+			signalType = FE_CAB_NOBLIND;
+			break;
+		case 8:
+			signalType = FE_CAB_BLINDOK;
+			break;
+		case 10:
+			signalType = FE_CAB_NODEMOD;
+			break;
+		case 11:
+			signalType = FE_CAB_DEMODOK;
+			break;
+		case 12:
+			signalType = FE_CAB_DEMODOK;
+			break;
+		case 13:
+			signalType = FE_CAB_NODEMOD;
+			break;
+		case 14:
+			signalType = FE_CAB_NOBLIND;
+			break;
+		case 15:
+			signalType = FE_CAB_NOSIGNAL;
+			break;
+		default:
+			break;
+		}
+
+	}
+
+	/* Set the AGC control values to tracking values */
+	stv0367_writebits(state, F367CAB_AGC_ACCUMRSTSEL, TrackAGCAccum);
+	return signalType;
+}
+
+static int stv0367cab_set_frontend(struct dvb_frontend *fe,
+				struct dvb_frontend_parameters *param)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367cab_state *cab_state = state->cab_state;
+	struct dvb_qam_parameters *op = &param->u.qam;
+	enum stv0367cab_mod QAMSize = 0;
+
+	dprintk("%s: freq = %d, srate = %d\n", __func__,
+					param->frequency, op->symbol_rate);
+
+	cab_state->derot_offset = 0;
+
+	switch (op->modulation) {
+	case QAM_16:
+		QAMSize = FE_CAB_MOD_QAM16;
+		break;
+	case QAM_32:
+		QAMSize = FE_CAB_MOD_QAM32;
+		break;
+	case QAM_64:
+		QAMSize = FE_CAB_MOD_QAM64;
+		break;
+	case QAM_128:
+		QAMSize = FE_CAB_MOD_QAM128;
+		break;
+	case QAM_256:
+		QAMSize = FE_CAB_MOD_QAM256;
+		break;
+	default:
+		break;
+	}
+
+	stv0367cab_init(fe);
+
+	/* Tuner Frequency Setting */
+	if (fe->ops.tuner_ops.set_params) {
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 1);
+		fe->ops.tuner_ops.set_params(fe, param);
+		if (fe->ops.i2c_gate_ctrl)
+			fe->ops.i2c_gate_ctrl(fe, 0);
+	}
+
+	stv0367cab_SetQamSize(
+			state,
+			op->symbol_rate,
+			QAMSize);
+
+	stv0367cab_set_srate(state,
+			cab_state->adc_clk,
+			cab_state->mclk,
+			op->symbol_rate,
+			QAMSize);
+	/* Search algorithm launch, [-1.1*RangeOffset, +1.1*RangeOffset] scan */
+	cab_state->state = stv0367cab_algo(state, param);
+	return 0;
+}
+
+static int stv0367cab_get_frontend(struct dvb_frontend *fe,
+				  struct dvb_frontend_parameters *param)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	struct stv0367cab_state *cab_state = state->cab_state;
+	struct dvb_qam_parameters *op = &param->u.qam;
+
+	enum stv0367cab_mod QAMSize;
+
+	dprintk("%s:\n", __func__);
+
+	op->symbol_rate = stv0367cab_GetSymbolRate(state, cab_state->mclk);
+
+	QAMSize = stv0367_readbits(state, F367CAB_QAM_MODE);
+	switch (QAMSize) {
+	case FE_CAB_MOD_QAM16:
+		op->modulation = QAM_16;
+		break;
+	case FE_CAB_MOD_QAM32:
+		op->modulation = QAM_32;
+		break;
+	case FE_CAB_MOD_QAM64:
+		op->modulation = QAM_64;
+		break;
+	case FE_CAB_MOD_QAM128:
+		op->modulation = QAM_128;
+		break;
+	case QAM_256:
+		op->modulation = QAM_256;
+		break;
+	default:
+		break;
+	}
+
+	param->frequency = stv0367_get_tuner_freq(fe);
+
+	dprintk("%s: tuner frequency = %d\n", __func__, param->frequency);
+
+	if (state->config->if_khz == 0) {
+		param->frequency +=
+			(stv0367cab_get_derot_freq(state, cab_state->adc_clk) -
+			cab_state->adc_clk / 4000);
+		return 0;
+	}
+
+	if (state->config->if_khz > cab_state->adc_clk / 1000)
+		param->frequency += (state->config->if_khz
+			- stv0367cab_get_derot_freq(state, cab_state->adc_clk)
+			- cab_state->adc_clk / 1000);
+	else
+		param->frequency += (state->config->if_khz
+			- stv0367cab_get_derot_freq(state, cab_state->adc_clk));
+
+	return 0;
+}
+
+#if 0
+void stv0367cab_GetErrorCount(state, enum stv0367cab_mod QAMSize,
+			u32 symbol_rate, FE_367qam_Monitor *Monitor_results)
+{
+	stv0367cab_OptimiseNByteAndGetBER(state, QAMSize, symbol_rate, Monitor_results);
+	stv0367cab_GetPacketsCount(state, Monitor_results);
+
+	return;
+}
+
+static int stv0367cab_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	return 0;
+}
+#endif
+static s32 stv0367cab_get_rf_lvl(struct stv0367_state *state)
+{
+	s32 rfLevel = 0;
+	s32 RfAgcPwm = 0, IfAgcPwm = 0;
+	u8 i;
+
+	stv0367_writebits(state, F367CAB_STDBY_ADCGP, 0x0);
+
+	RfAgcPwm =
+		(stv0367_readbits(state, F367CAB_RF_AGC1_LEVEL_LO) & 0x03) +
+		(stv0367_readbits(state, F367CAB_RF_AGC1_LEVEL_HI) << 2);
+	RfAgcPwm = 100 * RfAgcPwm / 1023;
+
+	IfAgcPwm =
+		stv0367_readbits(state, F367CAB_AGC_IF_PWMCMD_LO) +
+		(stv0367_readbits(state, F367CAB_AGC_IF_PWMCMD_HI) << 8);
+	if (IfAgcPwm >= 2048)
+		IfAgcPwm -= 2048;
+	else
+		IfAgcPwm += 2048;
+
+	IfAgcPwm = 100 * IfAgcPwm / 4095;
+
+	/* For DTT75467 on NIM */
+	if (RfAgcPwm < 90  && IfAgcPwm < 28) {
+		for (i = 0; i < RF_LOOKUP_TABLE_SIZE; i++) {
+			if (RfAgcPwm <= stv0367cab_RF_LookUp1[0][i]) {
+				rfLevel = (-1) * stv0367cab_RF_LookUp1[1][i];
+				break;
+			}
+		}
+		if (i == RF_LOOKUP_TABLE_SIZE)
+			rfLevel = -56;
+	} else { /*if IF AGC>10*/
+		for (i = 0; i < RF_LOOKUP_TABLE2_SIZE; i++) {
+			if (IfAgcPwm <= stv0367cab_RF_LookUp2[0][i]) {
+				rfLevel = (-1) * stv0367cab_RF_LookUp2[1][i];
+				break;
+			}
+		}
+		if (i == RF_LOOKUP_TABLE2_SIZE)
+			rfLevel = -72;
+	}
+	return rfLevel;
+}
+
+static int stv0367cab_read_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	s32 signal =  stv0367cab_get_rf_lvl(state);
+
+	dprintk("%s: signal=%d dBm\n", __func__, signal);
+
+	if (signal <= -72)
+		*strength = 65535;
+	else
+		*strength = (22 + signal) * (-1311);
+
+	dprintk("%s: strength=%d\n", __func__, (*strength));
+
+	return 0;
+}
+
+static int stv0367cab_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+	u32 noisepercentage;
+	enum stv0367cab_mod QAMSize;
+	u32 regval = 0, temp = 0;
+	int power, i;
+
+	QAMSize = stv0367_readbits(state, F367CAB_QAM_MODE);
+	switch (QAMSize) {
+	case FE_CAB_MOD_QAM4:
+		power = 21904;
+		break;
+	case FE_CAB_MOD_QAM16:
+		power = 20480;
+		break;
+	case FE_CAB_MOD_QAM32:
+		power = 23040;
+		break;
+	case FE_CAB_MOD_QAM64:
+		power = 21504;
+		break;
+	case FE_CAB_MOD_QAM128:
+		power = 23616;
+		break;
+	case FE_CAB_MOD_QAM256:
+		power = 21760;
+		break;
+	case FE_CAB_MOD_QAM512:
+		power = 1;
+		break;
+	case FE_CAB_MOD_QAM1024:
+		power = 21280;
+		break;
+	default:
+		power = 1;
+		break;
+	}
+
+	for (i = 0; i < 10; i++) {
+		regval += (stv0367_readbits(state, F367CAB_SNR_LO)
+			+ 256 * stv0367_readbits(state, F367CAB_SNR_HI));
+	}
+
+	regval /= 10; /*for average over 10 times in for loop above*/
+	if (regval != 0) {
+		temp = power
+			* (1 << (3 + stv0367_readbits(state, F367CAB_SNR_PER)));
+		temp /= regval;
+	}
+
+	/* table values, not needed to calculate logarithms */
+	if (temp >= 5012)
+		noisepercentage = 100;
+	else if (temp >= 3981)
+		noisepercentage = 93;
+	else if (temp >= 3162)
+		noisepercentage = 86;
+	else if (temp >= 2512)
+		noisepercentage = 79;
+	else if (temp >= 1995)
+		noisepercentage = 72;
+	else if (temp >= 1585)
+		noisepercentage = 65;
+	else if (temp >= 1259)
+		noisepercentage = 58;
+	else if (temp >= 1000)
+		noisepercentage = 50;
+	else if (temp >= 794)
+		noisepercentage = 43;
+	else if (temp >= 501)
+		noisepercentage = 36;
+	else if (temp >= 316)
+		noisepercentage = 29;
+	else if (temp >= 200)
+		noisepercentage = 22;
+	else if (temp >= 158)
+		noisepercentage = 14;
+	else if (temp >= 126)
+		noisepercentage = 7;
+	else
+		noisepercentage = 0;
+
+	dprintk("%s: noisepercentage=%d\n", __func__, noisepercentage);
+
+	*snr = (noisepercentage * 65535) / 100;
+
+	return 0;
+}
+
+static struct dvb_frontend_ops stv0367cab_ops = {
+	.info = {
+		.name = "ST STV0367 DVB-C",
+		.type = FE_QAM,
+		.frequency_min = 47000000,
+		.frequency_max = 862000000,
+		.frequency_stepsize = 62500,
+		.symbol_rate_min = 870000,
+		.symbol_rate_max = 11700000,
+		.caps = 0x400 |/* FE_CAN_QAM_4 */
+			FE_CAN_QAM_16 | FE_CAN_QAM_32  |
+			FE_CAN_QAM_64 | FE_CAN_QAM_128 |
+			FE_CAN_QAM_256 | FE_CAN_FEC_AUTO
+	},
+	.release				= stv0367_release,
+	.init					= stv0367cab_init,
+	.sleep					= stv0367cab_sleep,
+	.i2c_gate_ctrl				= stv0367cab_gate_ctrl,
+	.set_frontend				= stv0367cab_set_frontend,
+	.get_frontend				= stv0367cab_get_frontend,
+	.read_status				= stv0367cab_read_status,
+/*	.read_ber				= stv0367cab_read_ber, */
+	.read_signal_strength			= stv0367cab_read_strength,
+	.read_snr				= stv0367cab_read_snr,
+/*	.read_ucblocks				= stv0367cab_read_ucblcks,*/
+	.get_tune_settings			= stv0367_get_tune_settings,
+};
+
+struct dvb_frontend *stv0367cab_attach(const struct stv0367_config *config,
+				   struct i2c_adapter *i2c)
+{
+	struct stv0367_state *state = NULL;
+	struct stv0367cab_state *cab_state = NULL;
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct stv0367_state), GFP_KERNEL);
+	if (state == NULL)
+		goto error;
+	cab_state = kzalloc(sizeof(struct stv0367cab_state), GFP_KERNEL);
+	if (cab_state == NULL)
+		goto error;
+
+	/* setup the state */
+	state->i2c = i2c;
+	state->config = config;
+	cab_state->search_range = 280000;
+	state->cab_state = cab_state;
+	state->fe.ops = stv0367cab_ops;
+	state->fe.demodulator_priv = state;
+	state->chip_id = stv0367_readreg(state, 0xf000);
+
+	dprintk("%s: chip_id = 0x%x\n", __func__, state->chip_id);
+
+	/* check if the demod is there */
+	if ((state->chip_id != 0x50) && (state->chip_id != 0x60))
+		goto error;
+
+	return &state->fe;
+
+error:
+	kfree(cab_state);
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(stv0367cab_attach);
+
+MODULE_PARM_DESC(debug, "Set debug");
+MODULE_PARM_DESC(i2c_debug, "Set i2c debug");
+
+MODULE_AUTHOR("Igor M. Liplianin");
+MODULE_DESCRIPTION("ST STV0367 DVB-C/T demodulator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/frontends/stv0367.h b/drivers/media/dvb/frontends/stv0367.h
new file mode 100644
index 000000000000..93cc4a57eea0
--- /dev/null
+++ b/drivers/media/dvb/frontends/stv0367.h
@@ -0,0 +1,66 @@
+/*
+ * stv0367.h
+ *
+ * Driver for ST STV0367 DVB-T & DVB-C demodulator IC.
+ *
+ * Copyright (C) ST Microelectronics.
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef STV0367_H
+#define STV0367_H
+
+#include <linux/dvb/frontend.h>
+#include "dvb_frontend.h"
+
+struct stv0367_config {
+	u8 demod_address;
+	u32 xtal;
+	u32 if_khz;/*4500*/
+	int if_iq_mode;
+	int ts_mode;
+	int clk_pol;
+};
+
+#if defined(CONFIG_DVB_STV0367) || (defined(CONFIG_DVB_STV0367_MODULE) \
+							&& defined(MODULE))
+extern struct
+dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
+					struct i2c_adapter *i2c);
+extern struct
+dvb_frontend *stv0367cab_attach(const struct stv0367_config *config,
+					struct i2c_adapter *i2c);
+#else
+static inline struct
+dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
+					struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+static inline struct
+dvb_frontend *stv0367cab_attach(const struct stv0367_config *config,
+					struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/dvb/frontends/stv0367_priv.h b/drivers/media/dvb/frontends/stv0367_priv.h
new file mode 100644
index 000000000000..995db0689ddd
--- /dev/null
+++ b/drivers/media/dvb/frontends/stv0367_priv.h
@@ -0,0 +1,212 @@
+/*
+ * stv0367_priv.h
+ *
+ * Driver for ST STV0367 DVB-T & DVB-C demodulator IC.
+ *
+ * Copyright (C) ST Microelectronics.
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+/* Common driver error constants */
+
+#ifndef STV0367_PRIV_H
+#define STV0367_PRIV_H
+
+#ifndef TRUE
+    #define TRUE (1 == 1)
+#endif
+#ifndef FALSE
+    #define FALSE (!TRUE)
+#endif
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+/* MACRO definitions */
+#define ABS(X) ((X) < 0 ? (-1 * (X)) : (X))
+#define MAX(X, Y) ((X) >= (Y) ? (X) : (Y))
+#define MIN(X, Y) ((X) <= (Y) ? (X) : (Y))
+#define INRANGE(X, Y, Z) \
+	((((X) <= (Y)) && ((Y) <= (Z))) || \
+	(((Z) <= (Y)) && ((Y) <= (X))) ? 1 : 0)
+
+#ifndef MAKEWORD
+#define MAKEWORD(X, Y) (((X) << 8) + (Y))
+#endif
+
+#define LSB(X) (((X) & 0xff))
+#define MSB(Y) (((Y) >> 8) & 0xff)
+#define MMSB(Y)(((Y) >> 16) & 0xff)
+
+enum stv0367_ter_signal_type {
+	FE_TER_NOAGC = 0,
+	FE_TER_AGCOK = 5,
+	FE_TER_NOTPS = 6,
+	FE_TER_TPSOK = 7,
+	FE_TER_NOSYMBOL = 8,
+	FE_TER_BAD_CPQ = 9,
+	FE_TER_PRFOUNDOK = 10,
+	FE_TER_NOPRFOUND = 11,
+	FE_TER_LOCKOK = 12,
+	FE_TER_NOLOCK = 13,
+	FE_TER_SYMBOLOK = 15,
+	FE_TER_CPAMPOK = 16,
+	FE_TER_NOCPAMP = 17,
+	FE_TER_SWNOK = 18
+};
+
+enum stv0367_ts_mode {
+	STV0367_OUTPUTMODE_DEFAULT,
+	STV0367_SERIAL_PUNCT_CLOCK,
+	STV0367_SERIAL_CONT_CLOCK,
+	STV0367_PARALLEL_PUNCT_CLOCK,
+	STV0367_DVBCI_CLOCK
+};
+
+enum stv0367_clk_pol {
+	STV0367_CLOCKPOLARITY_DEFAULT,
+	STV0367_RISINGEDGE_CLOCK,
+	STV0367_FALLINGEDGE_CLOCK
+};
+
+enum stv0367_ter_bw {
+	FE_TER_CHAN_BW_6M = 6,
+	FE_TER_CHAN_BW_7M = 7,
+	FE_TER_CHAN_BW_8M = 8
+};
+
+#if 0
+enum FE_TER_Rate_TPS {
+	FE_TER_TPS_1_2 = 0,
+	FE_TER_TPS_2_3 = 1,
+	FE_TER_TPS_3_4 = 2,
+	FE_TER_TPS_5_6 = 3,
+	FE_TER_TPS_7_8 = 4
+};
+#endif
+
+enum stv0367_ter_mode {
+	FE_TER_MODE_2K,
+	FE_TER_MODE_8K,
+	FE_TER_MODE_4K
+};
+#if 0
+enum FE_TER_Hierarchy_Alpha {
+	FE_TER_HIER_ALPHA_NONE,	/* Regular modulation */
+	FE_TER_HIER_ALPHA_1,	/* Hierarchical modulation a = 1*/
+	FE_TER_HIER_ALPHA_2,	/* Hierarchical modulation a = 2*/
+	FE_TER_HIER_ALPHA_4	/* Hierarchical modulation a = 4*/
+};
+#endif
+enum stv0367_ter_hierarchy {
+	FE_TER_HIER_NONE,	/*Hierarchy None*/
+	FE_TER_HIER_LOW_PRIO,	/*Hierarchy : Low Priority*/
+	FE_TER_HIER_HIGH_PRIO,	/*Hierarchy : High Priority*/
+	FE_TER_HIER_PRIO_ANY	/*Hierarchy  :Any*/
+};
+
+#if 0
+enum fe_stv0367_ter_spec {
+	FE_TER_INVERSION_NONE = 0,
+	FE_TER_INVERSION = 1,
+	FE_TER_INVERSION_AUTO = 2,
+	FE_TER_INVERSION_UNK  = 4
+};
+#endif
+
+enum stv0367_ter_if_iq_mode {
+	FE_TER_NORMAL_IF_TUNER = 0,
+	FE_TER_LONGPATH_IF_TUNER = 1,
+	FE_TER_IQ_TUNER = 2
+
+};
+
+#if 0
+enum FE_TER_FECRate {
+	FE_TER_FEC_NONE = 0x00,	/* no FEC rate specified */
+	FE_TER_FEC_ALL = 0xFF,	 /* Logical OR of all FECs */
+	FE_TER_FEC_1_2 = 1,
+	FE_TER_FEC_2_3 = (1 << 1),
+	FE_TER_FEC_3_4 = (1 << 2),
+	FE_TER_FEC_4_5 = (1 << 3),
+	FE_TER_FEC_5_6 = (1 << 4),
+	FE_TER_FEC_6_7 = (1 << 5),
+	FE_TER_FEC_7_8 = (1 << 6),
+	FE_TER_FEC_8_9 = (1 << 7)
+};
+
+enum FE_TER_Rate {
+	FE_TER_FE_1_2 = 0,
+	FE_TER_FE_2_3 = 1,
+	FE_TER_FE_3_4 = 2,
+	FE_TER_FE_5_6 = 3,
+	FE_TER_FE_6_7 = 4,
+	FE_TER_FE_7_8 = 5
+};
+#endif
+
+enum stv0367_ter_force {
+	FE_TER_FORCENONE = 0,
+	FE_TER_FORCE_M_G = 1
+};
+
+enum  stv0367cab_mod {
+	FE_CAB_MOD_QAM4,
+	FE_CAB_MOD_QAM16,
+	FE_CAB_MOD_QAM32,
+	FE_CAB_MOD_QAM64,
+	FE_CAB_MOD_QAM128,
+	FE_CAB_MOD_QAM256,
+	FE_CAB_MOD_QAM512,
+	FE_CAB_MOD_QAM1024
+};
+#if 0
+enum {
+	FE_CAB_FEC_A = 1,	/* J83 Annex A */
+	FE_CAB_FEC_B = (1 << 1),/* J83 Annex B */
+	FE_CAB_FEC_C = (1 << 2)	/* J83 Annex C */
+} FE_CAB_FECType_t;
+#endif
+struct stv0367_cab_signal_info {
+	int locked;
+	u32 frequency; /* kHz */
+	u32 symbol_rate; /* Mbds */
+	enum stv0367cab_mod modulation;
+	fe_spectral_inversion_t spect_inv;
+	s32 Power_dBmx10;	/* Power of the RF signal (dBm x 10) */
+	u32	CN_dBx10;	/* Carrier to noise ratio (dB x 10) */
+	u32	BER;		/* Bit error rate (x 10000000)	*/
+};
+
+enum stv0367_cab_signal_type {
+	FE_CAB_NOTUNER,
+	FE_CAB_NOAGC,
+	FE_CAB_NOSIGNAL,
+	FE_CAB_NOTIMING,
+	FE_CAB_TIMINGOK,
+	FE_CAB_NOCARRIER,
+	FE_CAB_CARRIEROK,
+	FE_CAB_NOBLIND,
+	FE_CAB_BLINDOK,
+	FE_CAB_NODEMOD,
+	FE_CAB_DEMODOK,
+	FE_CAB_DATAOK
+};
+
+#endif
diff --git a/drivers/media/dvb/frontends/stv0367_regs.h b/drivers/media/dvb/frontends/stv0367_regs.h
new file mode 100644
index 000000000000..a96fbdc7e25e
--- /dev/null
+++ b/drivers/media/dvb/frontends/stv0367_regs.h
@@ -0,0 +1,3614 @@
+/*
+ * stv0367_regs.h
+ *
+ * Driver for ST STV0367 DVB-T & DVB-C demodulator IC.
+ *
+ * Copyright (C) ST Microelectronics.
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef STV0367_REGS_H
+#define STV0367_REGS_H
+
+/* ID */
+#define	R367TER_ID	0xf000
+#define	F367TER_IDENTIFICATIONREG	0xf00000ff
+
+/* I2CRPT */
+#define	R367TER_I2CRPT	0xf001
+#define	F367TER_I2CT_ON	0xf0010080
+#define	F367TER_ENARPT_LEVEL	0xf0010070
+#define	F367TER_SCLT_DELAY	0xf0010008
+#define	F367TER_SCLT_NOD	0xf0010004
+#define	F367TER_STOP_ENABLE	0xf0010002
+#define	F367TER_SDAT_NOD	0xf0010001
+
+/* TOPCTRL */
+#define	R367TER_TOPCTRL	0xf002
+#define	F367TER_STDBY	0xf0020080
+#define	F367TER_STDBY_FEC	0xf0020040
+#define	F367TER_STDBY_CORE	0xf0020020
+#define	F367TER_QAM_COFDM	0xf0020010
+#define	F367TER_TS_DIS	0xf0020008
+#define	F367TER_DIR_CLK_216	0xf0020004
+#define	F367TER_TUNER_BB	0xf0020002
+#define	F367TER_DVBT_H	0xf0020001
+
+/* IOCFG0 */
+#define	R367TER_IOCFG0	0xf003
+#define	F367TER_OP0_SD	0xf0030080
+#define	F367TER_OP0_VAL	0xf0030040
+#define	F367TER_OP0_OD	0xf0030020
+#define	F367TER_OP0_INV	0xf0030010
+#define	F367TER_OP0_DACVALUE_HI	0xf003000f
+
+/* DAc0R */
+#define	R367TER_DAC0R	0xf004
+#define	F367TER_OP0_DACVALUE_LO	0xf00400ff
+
+/* IOCFG1 */
+#define	R367TER_IOCFG1	0xf005
+#define	F367TER_IP0	0xf0050040
+#define	F367TER_OP1_OD	0xf0050020
+#define	F367TER_OP1_INV	0xf0050010
+#define	F367TER_OP1_DACVALUE_HI	0xf005000f
+
+/* DAC1R */
+#define	R367TER_DAC1R	0xf006
+#define	F367TER_OP1_DACVALUE_LO	0xf00600ff
+
+/* IOCFG2 */
+#define	R367TER_IOCFG2	0xf007
+#define	F367TER_OP2_LOCK_CONF	0xf00700e0
+#define	F367TER_OP2_OD	0xf0070010
+#define	F367TER_OP2_VAL	0xf0070008
+#define	F367TER_OP1_LOCK_CONF	0xf0070007
+
+/* SDFR */
+#define	R367TER_SDFR	0xf008
+#define	F367TER_OP0_FREQ	0xf00800f0
+#define	F367TER_OP1_FREQ	0xf008000f
+
+/* STATUS */
+#define	R367TER_STATUS	0xf009
+#define	F367TER_TPS_LOCK	0xf0090080
+#define	F367TER_SYR_LOCK	0xf0090040
+#define	F367TER_AGC_LOCK	0xf0090020
+#define	F367TER_PRF	0xf0090010
+#define	F367TER_LK	0xf0090008
+#define	F367TER_PR	0xf0090007
+
+/* AUX_CLK */
+#define	R367TER_AUX_CLK	0xf00a
+#define	F367TER_AUXFEC_CTL	0xf00a00c0
+#define	F367TER_DIS_CKX4	0xf00a0020
+#define	F367TER_CKSEL	0xf00a0018
+#define	F367TER_CKDIV_PROG	0xf00a0006
+#define	F367TER_AUXCLK_ENA	0xf00a0001
+
+/* FREESYS1 */
+#define	R367TER_FREESYS1	0xf00b
+#define	F367TER_FREE_SYS1	0xf00b00ff
+
+/* FREESYS2 */
+#define	R367TER_FREESYS2	0xf00c
+#define	F367TER_FREE_SYS2	0xf00c00ff
+
+/* FREESYS3 */
+#define	R367TER_FREESYS3	0xf00d
+#define	F367TER_FREE_SYS3	0xf00d00ff
+
+/* GPIO_CFG */
+#define	R367TER_GPIO_CFG	0xf00e
+#define	F367TER_GPIO7_NOD	0xf00e0080
+#define	F367TER_GPIO7_CFG	0xf00e0040
+#define	F367TER_GPIO6_NOD	0xf00e0020
+#define	F367TER_GPIO6_CFG	0xf00e0010
+#define	F367TER_GPIO5_NOD	0xf00e0008
+#define	F367TER_GPIO5_CFG	0xf00e0004
+#define	F367TER_GPIO4_NOD	0xf00e0002
+#define	F367TER_GPIO4_CFG	0xf00e0001
+
+/* GPIO_CMD */
+#define	R367TER_GPIO_CMD	0xf00f
+#define	F367TER_GPIO7_VAL	0xf00f0008
+#define	F367TER_GPIO6_VAL	0xf00f0004
+#define	F367TER_GPIO5_VAL	0xf00f0002
+#define	F367TER_GPIO4_VAL	0xf00f0001
+
+/* AGC2MAX */
+#define	R367TER_AGC2MAX	0xf010
+#define	F367TER_AGC2_MAX	0xf01000ff
+
+/* AGC2MIN */
+#define	R367TER_AGC2MIN	0xf011
+#define	F367TER_AGC2_MIN	0xf01100ff
+
+/* AGC1MAX */
+#define	R367TER_AGC1MAX	0xf012
+#define	F367TER_AGC1_MAX	0xf01200ff
+
+/* AGC1MIN */
+#define	R367TER_AGC1MIN	0xf013
+#define	F367TER_AGC1_MIN	0xf01300ff
+
+/* AGCR */
+#define	R367TER_AGCR	0xf014
+#define	F367TER_RATIO_A	0xf01400e0
+#define	F367TER_RATIO_B	0xf0140018
+#define	F367TER_RATIO_C	0xf0140007
+
+/* AGC2TH */
+#define	R367TER_AGC2TH	0xf015
+#define	F367TER_AGC2_THRES	0xf01500ff
+
+/* AGC12c */
+#define	R367TER_AGC12C	0xf016
+#define	F367TER_AGC1_IV	0xf0160080
+#define	F367TER_AGC1_OD	0xf0160040
+#define	F367TER_AGC1_LOAD	0xf0160020
+#define	F367TER_AGC2_IV	0xf0160010
+#define	F367TER_AGC2_OD	0xf0160008
+#define	F367TER_AGC2_LOAD	0xf0160004
+#define	F367TER_AGC12_MODE	0xf0160003
+
+/* AGCCTRL1 */
+#define	R367TER_AGCCTRL1	0xf017
+#define	F367TER_DAGC_ON	0xf0170080
+#define	F367TER_INVERT_AGC12	0xf0170040
+#define	F367TER_AGC1_MODE	0xf0170008
+#define	F367TER_AGC2_MODE	0xf0170007
+
+/* AGCCTRL2 */
+#define	R367TER_AGCCTRL2	0xf018
+#define	F367TER_FRZ2_CTRL	0xf0180060
+#define	F367TER_FRZ1_CTRL	0xf0180018
+#define	F367TER_TIME_CST	0xf0180007
+
+/* AGC1VAL1 */
+#define	R367TER_AGC1VAL1	0xf019
+#define	F367TER_AGC1_VAL_LO	0xf01900ff
+
+/* AGC1VAL2 */
+#define	R367TER_AGC1VAL2	0xf01a
+#define	F367TER_AGC1_VAL_HI	0xf01a000f
+
+/* AGC2VAL1 */
+#define	R367TER_AGC2VAL1	0xf01b
+#define	F367TER_AGC2_VAL_LO	0xf01b00ff
+
+/* AGC2VAL2 */
+#define	R367TER_AGC2VAL2	0xf01c
+#define	F367TER_AGC2_VAL_HI	0xf01c000f
+
+/* AGC2PGA */
+#define	R367TER_AGC2PGA	0xf01d
+#define	F367TER_AGC2_PGA	0xf01d00ff
+
+/* OVF_RATE1 */
+#define	R367TER_OVF_RATE1	0xf01e
+#define	F367TER_OVF_RATE_HI	0xf01e000f
+
+/* OVF_RATE2 */
+#define	R367TER_OVF_RATE2	0xf01f
+#define	F367TER_OVF_RATE_LO	0xf01f00ff
+
+/* GAIN_SRC1 */
+#define	R367TER_GAIN_SRC1	0xf020
+#define	F367TER_INV_SPECTR	0xf0200080
+#define	F367TER_IQ_INVERT	0xf0200040
+#define	F367TER_INR_BYPASS	0xf0200020
+#define	F367TER_STATUS_INV_SPECRUM	0xf0200010
+#define	F367TER_GAIN_SRC_HI	0xf020000f
+
+/* GAIN_SRC2 */
+#define	R367TER_GAIN_SRC2	0xf021
+#define	F367TER_GAIN_SRC_LO	0xf02100ff
+
+/* INC_DEROT1 */
+#define	R367TER_INC_DEROT1	0xf022
+#define	F367TER_INC_DEROT_HI	0xf02200ff
+
+/* INC_DEROT2 */
+#define	R367TER_INC_DEROT2	0xf023
+#define	F367TER_INC_DEROT_LO	0xf02300ff
+
+/* PPM_CPAMP_DIR */
+#define	R367TER_PPM_CPAMP_DIR	0xf024
+#define	F367TER_PPM_CPAMP_DIRECT	0xf02400ff
+
+/* PPM_CPAMP_INV */
+#define	R367TER_PPM_CPAMP_INV	0xf025
+#define	F367TER_PPM_CPAMP_INVER	0xf02500ff
+
+/* FREESTFE_1 */
+#define	R367TER_FREESTFE_1	0xf026
+#define	F367TER_SYMBOL_NUMBER_INC	0xf02600c0
+#define	F367TER_SEL_LSB	0xf0260004
+#define	F367TER_AVERAGE_ON	0xf0260002
+#define	F367TER_DC_ADJ	0xf0260001
+
+/* FREESTFE_2 */
+#define	R367TER_FREESTFE_2	0xf027
+#define	F367TER_SEL_SRCOUT	0xf02700c0
+#define	F367TER_SEL_SYRTHR	0xf027001f
+
+/* DCOFFSET */
+#define	R367TER_DCOFFSET	0xf028
+#define	F367TER_SELECT_I_Q	0xf0280080
+#define	F367TER_DC_OFFSET	0xf028007f
+
+/* EN_PROCESS */
+#define	R367TER_EN_PROCESS	0xf029
+#define	F367TER_FREE	0xf02900f0
+#define	F367TER_ENAB_MANUAL	0xf0290001
+
+/* SDI_SMOOTHER */
+#define	R367TER_SDI_SMOOTHER	0xf02a
+#define	F367TER_DIS_SMOOTH	0xf02a0080
+#define	F367TER_SDI_INC_SMOOTHER	0xf02a007f
+
+/* FE_LOOP_OPEN */
+#define	R367TER_FE_LOOP_OPEN	0xf02b
+#define	F367TER_TRL_LOOP_OP	0xf02b0002
+#define	F367TER_CRL_LOOP_OP	0xf02b0001
+
+/* FREQOFF1 */
+#define	R367TER_FREQOFF1	0xf02c
+#define	F367TER_FREQ_OFFSET_LOOP_OPEN_VHI	0xf02c00ff
+
+/* FREQOFF2 */
+#define	R367TER_FREQOFF2	0xf02d
+#define	F367TER_FREQ_OFFSET_LOOP_OPEN_HI	0xf02d00ff
+
+/* FREQOFF3 */
+#define	R367TER_FREQOFF3	0xf02e
+#define	F367TER_FREQ_OFFSET_LOOP_OPEN_LO	0xf02e00ff
+
+/* TIMOFF1 */
+#define	R367TER_TIMOFF1	0xf02f
+#define	F367TER_TIM_OFFSET_LOOP_OPEN_HI	0xf02f00ff
+
+/* TIMOFF2 */
+#define	R367TER_TIMOFF2	0xf030
+#define	F367TER_TIM_OFFSET_LOOP_OPEN_LO	0xf03000ff
+
+/* EPQ */
+#define	R367TER_EPQ	0xf031
+#define	F367TER_EPQ1	0xf03100ff
+
+/* EPQAUTO */
+#define	R367TER_EPQAUTO	0xf032
+#define	F367TER_EPQ2	0xf03200ff
+
+/* SYR_UPDATE */
+#define	R367TER_SYR_UPDATE	0xf033
+#define	F367TER_SYR_PROTV	0xf0330080
+#define	F367TER_SYR_PROTV_GAIN	0xf0330060
+#define	F367TER_SYR_FILTER	0xf0330010
+#define	F367TER_SYR_TRACK_THRES	0xf033000c
+
+/* CHPFREE */
+#define	R367TER_CHPFREE	0xf034
+#define	F367TER_CHP_FREE	0xf03400ff
+
+/* PPM_STATE_MAC */
+#define	R367TER_PPM_STATE_MAC	0xf035
+#define	F367TER_PPM_STATE_MACHINE_DECODER	0xf035003f
+
+/* INR_THRESHOLD */
+#define	R367TER_INR_THRESHOLD	0xf036
+#define	F367TER_INR_THRESH	0xf03600ff
+
+/* EPQ_TPS_ID_CELL */
+#define	R367TER_EPQ_TPS_ID_CELL	0xf037
+#define	F367TER_ENABLE_LGTH_TO_CF	0xf0370080
+#define	F367TER_DIS_TPS_RSVD	0xf0370040
+#define	F367TER_DIS_BCH	0xf0370020
+#define	F367TER_DIS_ID_CEL	0xf0370010
+#define	F367TER_TPS_ADJUST_SYM	0xf037000f
+
+/* EPQ_CFG */
+#define	R367TER_EPQ_CFG	0xf038
+#define	F367TER_EPQ_RANGE	0xf0380002
+#define	F367TER_EPQ_SOFT	0xf0380001
+
+/* EPQ_STATUS */
+#define	R367TER_EPQ_STATUS	0xf039
+#define	F367TER_SLOPE_INC	0xf03900fc
+#define	F367TER_TPS_FIELD	0xf0390003
+
+/* AUTORELOCK */
+#define	R367TER_AUTORELOCK	0xf03a
+#define	F367TER_BYPASS_BER_TEMPO	0xf03a0080
+#define	F367TER_BER_TEMPO	0xf03a0070
+#define	F367TER_BYPASS_COFDM_TEMPO	0xf03a0008
+#define	F367TER_COFDM_TEMPO	0xf03a0007
+
+/* BER_THR_VMSB */
+#define	R367TER_BER_THR_VMSB	0xf03b
+#define	F367TER_BER_THRESHOLD_HI	0xf03b00ff
+
+/* BER_THR_MSB */
+#define	R367TER_BER_THR_MSB	0xf03c
+#define	F367TER_BER_THRESHOLD_MID	0xf03c00ff
+
+/* BER_THR_LSB */
+#define	R367TER_BER_THR_LSB	0xf03d
+#define	F367TER_BER_THRESHOLD_LO	0xf03d00ff
+
+/* CCD */
+#define	R367TER_CCD	0xf03e
+#define	F367TER_CCD_DETECTED	0xf03e0080
+#define	F367TER_CCD_RESET	0xf03e0040
+#define	F367TER_CCD_THRESHOLD	0xf03e000f
+
+/* SPECTR_CFG */
+#define	R367TER_SPECTR_CFG	0xf03f
+#define	F367TER_SPECT_CFG	0xf03f0003
+
+/* CONSTMU_MSB */
+#define	R367TER_CONSTMU_MSB	0xf040
+#define	F367TER_CONSTMU_FREEZE	0xf0400080
+#define	F367TER_CONSTNU_FORCE_EN	0xf0400040
+#define	F367TER_CONST_MU_MSB	0xf040003f
+
+/* CONSTMU_LSB */
+#define	R367TER_CONSTMU_LSB	0xf041
+#define	F367TER_CONST_MU_LSB	0xf04100ff
+
+/* CONSTMU_MAX_MSB */
+#define	R367TER_CONSTMU_MAX_MSB	0xf042
+#define	F367TER_CONST_MU_MAX_MSB	0xf042003f
+
+/* CONSTMU_MAX_LSB */
+#define	R367TER_CONSTMU_MAX_LSB	0xf043
+#define	F367TER_CONST_MU_MAX_LSB	0xf04300ff
+
+/* ALPHANOISE */
+#define	R367TER_ALPHANOISE	0xf044
+#define	F367TER_USE_ALLFILTER	0xf0440080
+#define	F367TER_INTER_ON	0xf0440040
+#define	F367TER_ALPHA_NOISE	0xf044001f
+
+/* MAXGP_MSB */
+#define	R367TER_MAXGP_MSB	0xf045
+#define	F367TER_MUFILTER_LENGTH	0xf04500f0
+#define	F367TER_MAX_GP_MSB	0xf045000f
+
+/* MAXGP_LSB */
+#define	R367TER_MAXGP_LSB	0xf046
+#define	F367TER_MAX_GP_LSB	0xf04600ff
+
+/* ALPHAMSB */
+#define	R367TER_ALPHAMSB	0xf047
+#define	F367TER_CHC_DATARATE	0xf04700c0
+#define	F367TER_ALPHA_MSB	0xf047003f
+
+/* ALPHALSB */
+#define	R367TER_ALPHALSB	0xf048
+#define	F367TER_ALPHA_LSB	0xf04800ff
+
+/* PILOT_ACCU */
+#define	R367TER_PILOT_ACCU	0xf049
+#define	F367TER_USE_SCAT4ADDAPT	0xf0490080
+#define	F367TER_PILOT_ACC	0xf049001f
+
+/* PILOTMU_ACCU */
+#define	R367TER_PILOTMU_ACCU	0xf04a
+#define	F367TER_DISCARD_BAD_SP	0xf04a0080
+#define	F367TER_DISCARD_BAD_CP	0xf04a0040
+#define	F367TER_PILOT_MU_ACCU	0xf04a001f
+
+/* FILT_CHANNEL_EST */
+#define	R367TER_FILT_CHANNEL_EST	0xf04b
+#define	F367TER_USE_FILT_PILOT	0xf04b0080
+#define	F367TER_FILT_CHANNEL	0xf04b007f
+
+/* ALPHA_NOPISE_FREQ */
+#define	R367TER_ALPHA_NOPISE_FREQ	0xf04c
+#define	F367TER_NOISE_FREQ_FILT	0xf04c0040
+#define	F367TER_ALPHA_NOISE_FREQ	0xf04c003f
+
+/* RATIO_PILOT */
+#define	R367TER_RATIO_PILOT	0xf04d
+#define	F367TER_RATIO_MEAN_SP	0xf04d00f0
+#define	F367TER_RATIO_MEAN_CP	0xf04d000f
+
+/* CHC_CTL */
+#define	R367TER_CHC_CTL	0xf04e
+#define	F367TER_TRACK_EN	0xf04e0080
+#define	F367TER_NOISE_NORM_EN	0xf04e0040
+#define	F367TER_FORCE_CHC_RESET	0xf04e0020
+#define	F367TER_SHORT_TIME	0xf04e0010
+#define	F367TER_FORCE_STATE_EN	0xf04e0008
+#define	F367TER_FORCE_STATE	0xf04e0007
+
+/* EPQ_ADJUST */
+#define	R367TER_EPQ_ADJUST	0xf04f
+#define	F367TER_ADJUST_SCAT_IND	0xf04f00c0
+#define	F367TER_ONE_SYMBOL	0xf04f0010
+#define	F367TER_EPQ_DECAY	0xf04f000e
+#define	F367TER_HOLD_SLOPE	0xf04f0001
+
+/* EPQ_THRES */
+#define	R367TER_EPQ_THRES	0xf050
+#define	F367TER_EPQ_THR	0xf05000ff
+
+/* OMEGA_CTL */
+#define	R367TER_OMEGA_CTL	0xf051
+#define	F367TER_OMEGA_RST	0xf0510080
+#define	F367TER_FREEZE_OMEGA	0xf0510040
+#define	F367TER_OMEGA_SEL	0xf051003f
+
+/* GP_CTL */
+#define	R367TER_GP_CTL	0xf052
+#define	F367TER_CHC_STATE	0xf05200e0
+#define	F367TER_FREEZE_GP	0xf0520010
+#define	F367TER_GP_SEL	0xf052000f
+
+/* MUMSB */
+#define	R367TER_MUMSB	0xf053
+#define	F367TER_MU_MSB	0xf053007f
+
+/* MULSB */
+#define	R367TER_MULSB	0xf054
+#define	F367TER_MU_LSB	0xf05400ff
+
+/* GPMSB */
+#define	R367TER_GPMSB	0xf055
+#define	F367TER_CSI_THRESHOLD	0xf05500e0
+#define	F367TER_GP_MSB	0xf055000f
+
+/* GPLSB */
+#define	R367TER_GPLSB	0xf056
+#define	F367TER_GP_LSB	0xf05600ff
+
+/* OMEGAMSB */
+#define	R367TER_OMEGAMSB	0xf057
+#define	F367TER_OMEGA_MSB	0xf057007f
+
+/* OMEGALSB */
+#define	R367TER_OMEGALSB	0xf058
+#define	F367TER_OMEGA_LSB	0xf05800ff
+
+/* SCAT_NB */
+#define	R367TER_SCAT_NB	0xf059
+#define	F367TER_CHC_TEST	0xf05900f8
+#define	F367TER_SCAT_NUMB	0xf0590003
+
+/* CHC_DUMMY */
+#define	R367TER_CHC_DUMMY	0xf05a
+#define	F367TER_CHC_DUM	0xf05a00ff
+
+/* INC_CTL */
+#define	R367TER_INC_CTL	0xf05b
+#define	F367TER_INC_BYPASS	0xf05b0080
+#define	F367TER_INC_NDEPTH	0xf05b000c
+#define	F367TER_INC_MADEPTH	0xf05b0003
+
+/* INCTHRES_COR1 */
+#define	R367TER_INCTHRES_COR1	0xf05c
+#define	F367TER_INC_THRES_COR1	0xf05c00ff
+
+/* INCTHRES_COR2 */
+#define	R367TER_INCTHRES_COR2	0xf05d
+#define	F367TER_INC_THRES_COR2	0xf05d00ff
+
+/* INCTHRES_DET1 */
+#define	R367TER_INCTHRES_DET1	0xf05e
+#define	F367TER_INC_THRES_DET1	0xf05e003f
+
+/* INCTHRES_DET2 */
+#define	R367TER_INCTHRES_DET2	0xf05f
+#define	F367TER_INC_THRES_DET2	0xf05f003f
+
+/* IIR_CELLNB */
+#define	R367TER_IIR_CELLNB	0xf060
+#define	F367TER_NRST_IIR	0xf0600080
+#define	F367TER_IIR_CELL_NB	0xf0600007
+
+/* IIRCX_COEFF1_MSB */
+#define	R367TER_IIRCX_COEFF1_MSB	0xf061
+#define	F367TER_IIR_CX_COEFF1_MSB	0xf06100ff
+
+/* IIRCX_COEFF1_LSB */
+#define	R367TER_IIRCX_COEFF1_LSB	0xf062
+#define	F367TER_IIR_CX_COEFF1_LSB	0xf06200ff
+
+/* IIRCX_COEFF2_MSB */
+#define	R367TER_IIRCX_COEFF2_MSB	0xf063
+#define	F367TER_IIR_CX_COEFF2_MSB	0xf06300ff
+
+/* IIRCX_COEFF2_LSB */
+#define	R367TER_IIRCX_COEFF2_LSB	0xf064
+#define	F367TER_IIR_CX_COEFF2_LSB	0xf06400ff
+
+/* IIRCX_COEFF3_MSB */
+#define	R367TER_IIRCX_COEFF3_MSB	0xf065
+#define	F367TER_IIR_CX_COEFF3_MSB	0xf06500ff
+
+/* IIRCX_COEFF3_LSB */
+#define	R367TER_IIRCX_COEFF3_LSB	0xf066
+#define	F367TER_IIR_CX_COEFF3_LSB	0xf06600ff
+
+/* IIRCX_COEFF4_MSB */
+#define	R367TER_IIRCX_COEFF4_MSB	0xf067
+#define	F367TER_IIR_CX_COEFF4_MSB	0xf06700ff
+
+/* IIRCX_COEFF4_LSB */
+#define	R367TER_IIRCX_COEFF4_LSB	0xf068
+#define	F367TER_IIR_CX_COEFF4_LSB	0xf06800ff
+
+/* IIRCX_COEFF5_MSB */
+#define	R367TER_IIRCX_COEFF5_MSB	0xf069
+#define	F367TER_IIR_CX_COEFF5_MSB	0xf06900ff
+
+/* IIRCX_COEFF5_LSB */
+#define	R367TER_IIRCX_COEFF5_LSB	0xf06a
+#define	F367TER_IIR_CX_COEFF5_LSB	0xf06a00ff
+
+/* FEPATH_CFG */
+#define	R367TER_FEPATH_CFG	0xf06b
+#define	F367TER_DEMUX_SWAP	0xf06b0004
+#define	F367TER_DIGAGC_SWAP	0xf06b0002
+#define	F367TER_LONGPATH_IF	0xf06b0001
+
+/* PMC1_FUNC */
+#define	R367TER_PMC1_FUNC	0xf06c
+#define	F367TER_SOFT_RSTN	0xf06c0080
+#define	F367TER_PMC1_AVERAGE_TIME	0xf06c0078
+#define	F367TER_PMC1_WAIT_TIME	0xf06c0006
+#define	F367TER_PMC1_2N_SEL	0xf06c0001
+
+/* PMC1_FOR */
+#define	R367TER_PMC1_FOR	0xf06d
+#define	F367TER_PMC1_FORCE	0xf06d0080
+#define	F367TER_PMC1_FORCE_VALUE	0xf06d007c
+
+/* PMC2_FUNC */
+#define	R367TER_PMC2_FUNC	0xf06e
+#define	F367TER_PMC2_SOFT_STN	0xf06e0080
+#define	F367TER_PMC2_ACCU_TIME	0xf06e0070
+#define	F367TER_PMC2_CMDP_MN	0xf06e0008
+#define	F367TER_PMC2_SWAP	0xf06e0004
+
+/* STATUS_ERR_DA */
+#define	R367TER_STATUS_ERR_DA	0xf06f
+#define	F367TER_COM_USEGAINTRK	0xf06f0080
+#define	F367TER_COM_AGCLOCK	0xf06f0040
+#define	F367TER_AUT_AGCLOCK	0xf06f0020
+#define	F367TER_MIN_ERR_X_LSB	0xf06f000f
+
+/* DIG_AGC_R */
+#define	R367TER_DIG_AGC_R	0xf070
+#define	F367TER_COM_SOFT_RSTN	0xf0700080
+#define	F367TER_COM_AGC_ON	0xf0700040
+#define	F367TER_COM_EARLY	0xf0700020
+#define	F367TER_AUT_SOFT_RESETN	0xf0700010
+#define	F367TER_AUT_AGC_ON	0xf0700008
+#define	F367TER_AUT_EARLY	0xf0700004
+#define	F367TER_AUT_ROT_EN	0xf0700002
+#define	F367TER_LOCK_SOFT_RESETN	0xf0700001
+
+/* COMAGC_TARMSB */
+#define	R367TER_COMAGC_TARMSB	0xf071
+#define	F367TER_COM_AGC_TARGET_MSB	0xf07100ff
+
+/* COM_AGC_TAR_ENMODE */
+#define	R367TER_COM_AGC_TAR_ENMODE	0xf072
+#define	F367TER_COM_AGC_TARGET_LSB	0xf07200f0
+#define	F367TER_COM_ENMODE	0xf072000f
+
+/* COM_AGC_CFG */
+#define	R367TER_COM_AGC_CFG	0xf073
+#define	F367TER_COM_N	0xf07300f8
+#define	F367TER_COM_STABMODE	0xf0730006
+#define	F367TER_ERR_SEL	0xf0730001
+
+/* COM_AGC_GAIN1 */
+#define	R367TER_COM_AGC_GAIN1	0xf074
+#define	F367TER_COM_GAIN1aCK	0xf07400f0
+#define	F367TER_COM_GAIN1TRK	0xf074000f
+
+/* AUT_AGC_TARGETMSB */
+#define	R367TER_AUT_AGC_TARGETMSB	0xf075
+#define	F367TER_AUT_AGC_TARGET_MSB	0xf07500ff
+
+/* LOCK_DET_MSB */
+#define	R367TER_LOCK_DET_MSB	0xf076
+#define	F367TER_LOCK_DETECT_MSB	0xf07600ff
+
+/* AGCTAR_LOCK_LSBS */
+#define	R367TER_AGCTAR_LOCK_LSBS	0xf077
+#define	F367TER_AUT_AGC_TARGET_LSB	0xf07700f0
+#define	F367TER_LOCK_DETECT_LSB	0xf077000f
+
+/* AUT_GAIN_EN */
+#define	R367TER_AUT_GAIN_EN	0xf078
+#define	F367TER_AUT_ENMODE	0xf07800f0
+#define	F367TER_AUT_GAIN2	0xf078000f
+
+/* AUT_CFG */
+#define	R367TER_AUT_CFG	0xf079
+#define	F367TER_AUT_N	0xf07900f8
+#define	F367TER_INT_CHOICE	0xf0790006
+#define	F367TER_INT_LOAD	0xf0790001
+
+/* LOCKN */
+#define	R367TER_LOCKN	0xf07a
+#define	F367TER_LOCK_N	0xf07a00f8
+#define	F367TER_SEL_IQNTAR	0xf07a0004
+#define	F367TER_LOCK_DETECT_CHOICE	0xf07a0003
+
+/* INT_X_3 */
+#define	R367TER_INT_X_3	0xf07b
+#define	F367TER_INT_X3	0xf07b00ff
+
+/* INT_X_2 */
+#define	R367TER_INT_X_2	0xf07c
+#define	F367TER_INT_X2	0xf07c00ff
+
+/* INT_X_1 */
+#define	R367TER_INT_X_1	0xf07d
+#define	F367TER_INT_X1	0xf07d00ff
+
+/* INT_X_0 */
+#define	R367TER_INT_X_0	0xf07e
+#define	F367TER_INT_X0	0xf07e00ff
+
+/* MIN_ERRX_MSB */
+#define	R367TER_MIN_ERRX_MSB	0xf07f
+#define	F367TER_MIN_ERR_X_MSB	0xf07f00ff
+
+/* COR_CTL */
+#define	R367TER_COR_CTL	0xf080
+#define	F367TER_CORE_ACTIVE	0xf0800020
+#define	F367TER_HOLD	0xf0800010
+#define	F367TER_CORE_STATE_CTL	0xf080000f
+
+/* COR_STAT */
+#define	R367TER_COR_STAT	0xf081
+#define	F367TER_SCATT_LOCKED	0xf0810080
+#define	F367TER_TPS_LOCKED	0xf0810040
+#define	F367TER_SYR_LOCKED_COR	0xf0810020
+#define	F367TER_AGC_LOCKED_STAT	0xf0810010
+#define	F367TER_CORE_STATE_STAT	0xf081000f
+
+/* COR_INTEN */
+#define	R367TER_COR_INTEN	0xf082
+#define	F367TER_INTEN	0xf0820080
+#define	F367TER_INTEN_SYR	0xf0820020
+#define	F367TER_INTEN_FFT	0xf0820010
+#define	F367TER_INTEN_AGC	0xf0820008
+#define	F367TER_INTEN_TPS1	0xf0820004
+#define	F367TER_INTEN_TPS2	0xf0820002
+#define	F367TER_INTEN_TPS3	0xf0820001
+
+/* COR_INTSTAT */
+#define	R367TER_COR_INTSTAT	0xf083
+#define	F367TER_INTSTAT_SYR	0xf0830020
+#define	F367TER_INTSTAT_FFT	0xf0830010
+#define	F367TER_INTSAT_AGC	0xf0830008
+#define	F367TER_INTSTAT_TPS1	0xf0830004
+#define	F367TER_INTSTAT_TPS2	0xf0830002
+#define	F367TER_INTSTAT_TPS3	0xf0830001
+
+/* COR_MODEGUARD */
+#define	R367TER_COR_MODEGUARD	0xf084
+#define	F367TER_FORCE	0xf0840010
+#define	F367TER_MODE	0xf084000c
+#define	F367TER_GUARD	0xf0840003
+
+/* AGC_CTL */
+#define	R367TER_AGC_CTL	0xf085
+#define	F367TER_AGC_TIMING_FACTOR	0xf08500e0
+#define	F367TER_AGC_LAST	0xf0850010
+#define	F367TER_AGC_GAIN	0xf085000c
+#define	F367TER_AGC_NEG	0xf0850002
+#define	F367TER_AGC_SET	0xf0850001
+
+/* AGC_MANUAL1 */
+#define	R367TER_AGC_MANUAL1	0xf086
+#define	F367TER_AGC_VAL_LO	0xf08600ff
+
+/* AGC_MANUAL2 */
+#define	R367TER_AGC_MANUAL2	0xf087
+#define	F367TER_AGC_VAL_HI	0xf087000f
+
+/* AGC_TARG */
+#define	R367TER_AGC_TARG	0xf088
+#define	F367TER_AGC_TARGET	0xf08800ff
+
+/* AGC_GAIN1 */
+#define	R367TER_AGC_GAIN1	0xf089
+#define	F367TER_AGC_GAIN_LO	0xf08900ff
+
+/* AGC_GAIN2 */
+#define	R367TER_AGC_GAIN2	0xf08a
+#define	F367TER_AGC_LOCKED_GAIN2	0xf08a0010
+#define	F367TER_AGC_GAIN_HI	0xf08a000f
+
+/* RESERVED_1 */
+#define	R367TER_RESERVED_1	0xf08b
+#define	F367TER_RESERVED1	0xf08b00ff
+
+/* RESERVED_2 */
+#define	R367TER_RESERVED_2	0xf08c
+#define	F367TER_RESERVED2	0xf08c00ff
+
+/* RESERVED_3 */
+#define	R367TER_RESERVED_3	0xf08d
+#define	F367TER_RESERVED3	0xf08d00ff
+
+/* CAS_CTL */
+#define	R367TER_CAS_CTL	0xf08e
+#define	F367TER_CCS_ENABLE	0xf08e0080
+#define	F367TER_ACS_DISABLE	0xf08e0040
+#define	F367TER_DAGC_DIS	0xf08e0020
+#define	F367TER_DAGC_GAIN	0xf08e0018
+#define	F367TER_CCSMU	0xf08e0007
+
+/* CAS_FREQ */
+#define	R367TER_CAS_FREQ	0xf08f
+#define	F367TER_CCS_FREQ	0xf08f00ff
+
+/* CAS_DAGCGAIN */
+#define	R367TER_CAS_DAGCGAIN	0xf090
+#define	F367TER_CAS_DAGC_GAIN	0xf09000ff
+
+/* SYR_CTL */
+#define	R367TER_SYR_CTL	0xf091
+#define	F367TER_SICTH_ENABLE	0xf0910080
+#define	F367TER_LONG_ECHO	0xf0910078
+#define	F367TER_AUTO_LE_EN	0xf0910004
+#define	F367TER_SYR_BYPASS	0xf0910002
+#define	F367TER_SYR_TR_DIS	0xf0910001
+
+/* SYR_STAT */
+#define	R367TER_SYR_STAT	0xf092
+#define	F367TER_SYR_LOCKED_STAT	0xf0920010
+#define	F367TER_SYR_MODE	0xf092000c
+#define	F367TER_SYR_GUARD	0xf0920003
+
+/* SYR_NCO1 */
+#define	R367TER_SYR_NCO1	0xf093
+#define	F367TER_SYR_NCO_LO	0xf09300ff
+
+/* SYR_NCO2 */
+#define	R367TER_SYR_NCO2	0xf094
+#define	F367TER_SYR_NCO_HI	0xf094003f
+
+/* SYR_OFFSET1 */
+#define	R367TER_SYR_OFFSET1	0xf095
+#define	F367TER_SYR_OFFSET_LO	0xf09500ff
+
+/* SYR_OFFSET2 */
+#define	R367TER_SYR_OFFSET2	0xf096
+#define	F367TER_SYR_OFFSET_HI	0xf096003f
+
+/* FFT_CTL */
+#define	R367TER_FFT_CTL	0xf097
+#define	F367TER_SHIFT_FFT_TRIG	0xf0970018
+#define	F367TER_FFT_TRIGGER	0xf0970004
+#define	F367TER_FFT_MANUAL	0xf0970002
+#define	F367TER_IFFT_MODE	0xf0970001
+
+/* SCR_CTL */
+#define	R367TER_SCR_CTL	0xf098
+#define	F367TER_SYRADJDECAY	0xf0980070
+#define	F367TER_SCR_CPEDIS	0xf0980002
+#define	F367TER_SCR_DIS	0xf0980001
+
+/* PPM_CTL1 */
+#define	R367TER_PPM_CTL1	0xf099
+#define	F367TER_PPM_MAXFREQ	0xf0990030
+#define	F367TER_PPM_MAXTIM	0xf0990008
+#define	F367TER_PPM_INVSEL	0xf0990004
+#define	F367TER_PPM_SCATDIS	0xf0990002
+#define	F367TER_PPM_BYP	0xf0990001
+
+/* TRL_CTL */
+#define	R367TER_TRL_CTL	0xf09a
+#define	F367TER_TRL_NOMRATE_LSB	0xf09a0080
+#define	F367TER_TRL_GAIN_FACTOR	0xf09a0078
+#define	F367TER_TRL_LOOPGAIN	0xf09a0007
+
+/* TRL_NOMRATE1 */
+#define	R367TER_TRL_NOMRATE1	0xf09b
+#define	F367TER_TRL_NOMRATE_LO	0xf09b00ff
+
+/* TRL_NOMRATE2 */
+#define	R367TER_TRL_NOMRATE2	0xf09c
+#define	F367TER_TRL_NOMRATE_HI	0xf09c00ff
+
+/* TRL_TIME1 */
+#define	R367TER_TRL_TIME1	0xf09d
+#define	F367TER_TRL_TOFFSET_LO	0xf09d00ff
+
+/* TRL_TIME2 */
+#define	R367TER_TRL_TIME2	0xf09e
+#define	F367TER_TRL_TOFFSET_HI	0xf09e00ff
+
+/* CRL_CTL */
+#define	R367TER_CRL_CTL	0xf09f
+#define	F367TER_CRL_DIS	0xf09f0080
+#define	F367TER_CRL_GAIN_FACTOR	0xf09f0078
+#define	F367TER_CRL_LOOPGAIN	0xf09f0007
+
+/* CRL_FREQ1 */
+#define	R367TER_CRL_FREQ1	0xf0a0
+#define	F367TER_CRL_FOFFSET_LO	0xf0a000ff
+
+/* CRL_FREQ2 */
+#define	R367TER_CRL_FREQ2	0xf0a1
+#define	F367TER_CRL_FOFFSET_HI	0xf0a100ff
+
+/* CRL_FREQ3 */
+#define	R367TER_CRL_FREQ3	0xf0a2
+#define	F367TER_CRL_FOFFSET_VHI	0xf0a200ff
+
+/* TPS_SFRAME_CTL */
+#define	R367TER_TPS_SFRAME_CTL	0xf0a3
+#define	F367TER_TPS_SFRAME_SYNC	0xf0a30001
+
+/* CHC_SNR */
+#define	R367TER_CHC_SNR	0xf0a4
+#define	F367TER_CHCSNR	0xf0a400ff
+
+/* BDI_CTL */
+#define	R367TER_BDI_CTL	0xf0a5
+#define	F367TER_BDI_LPSEL	0xf0a50002
+#define	F367TER_BDI_SERIAL	0xf0a50001
+
+/* DMP_CTL */
+#define	R367TER_DMP_CTL	0xf0a6
+#define	F367TER_DMP_SCALING_FACTOR	0xf0a6001e
+#define	F367TER_DMP_SDDIS	0xf0a60001
+
+/* TPS_RCVD1 */
+#define	R367TER_TPS_RCVD1	0xf0a7
+#define	F367TER_TPS_CHANGE	0xf0a70040
+#define	F367TER_BCH_OK	0xf0a70020
+#define	F367TER_TPS_SYNC	0xf0a70010
+#define	F367TER_TPS_FRAME	0xf0a70003
+
+/* TPS_RCVD2 */
+#define	R367TER_TPS_RCVD2	0xf0a8
+#define	F367TER_TPS_HIERMODE	0xf0a80070
+#define	F367TER_TPS_CONST	0xf0a80003
+
+/* TPS_RCVD3 */
+#define	R367TER_TPS_RCVD3	0xf0a9
+#define	F367TER_TPS_LPCODE	0xf0a90070
+#define	F367TER_TPS_HPCODE	0xf0a90007
+
+/* TPS_RCVD4 */
+#define	R367TER_TPS_RCVD4	0xf0aa
+#define	F367TER_TPS_GUARD	0xf0aa0030
+#define	F367TER_TPS_MODE	0xf0aa0003
+
+/* TPS_ID_CELL1 */
+#define	R367TER_TPS_ID_CELL1	0xf0ab
+#define	F367TER_TPS_ID_CELL_LO	0xf0ab00ff
+
+/* TPS_ID_CELL2 */
+#define	R367TER_TPS_ID_CELL2	0xf0ac
+#define	F367TER_TPS_ID_CELL_HI	0xf0ac00ff
+
+/* TPS_RCVD5_SET1 */
+#define	R367TER_TPS_RCVD5_SET1	0xf0ad
+#define	F367TER_TPS_NA	0xf0ad00fC
+#define	F367TER_TPS_SETFRAME	0xf0ad0003
+
+/* TPS_SET2 */
+#define	R367TER_TPS_SET2	0xf0ae
+#define	F367TER_TPS_SETHIERMODE	0xf0ae0070
+#define	F367TER_TPS_SETCONST	0xf0ae0003
+
+/* TPS_SET3 */
+#define	R367TER_TPS_SET3	0xf0af
+#define	F367TER_TPS_SETLPCODE	0xf0af0070
+#define	F367TER_TPS_SETHPCODE	0xf0af0007
+
+/* TPS_CTL */
+#define	R367TER_TPS_CTL	0xf0b0
+#define	F367TER_TPS_IMM	0xf0b00004
+#define	F367TER_TPS_BCHDIS	0xf0b00002
+#define	F367TER_TPS_UPDDIS	0xf0b00001
+
+/* CTL_FFTOSNUM */
+#define	R367TER_CTL_FFTOSNUM	0xf0b1
+#define	F367TER_SYMBOL_NUMBER	0xf0b1007f
+
+/* TESTSELECT */
+#define	R367TER_TESTSELECT	0xf0b2
+#define	F367TER_TEST_SELECT	0xf0b2001f
+
+/* MSC_REV */
+#define	R367TER_MSC_REV	0xf0b3
+#define	F367TER_REV_NUMBER	0xf0b300ff
+
+/* PIR_CTL */
+#define	R367TER_PIR_CTL	0xf0b4
+#define	F367TER_FREEZE	0xf0b40001
+
+/* SNR_CARRIER1 */
+#define	R367TER_SNR_CARRIER1	0xf0b5
+#define	F367TER_SNR_CARRIER_LO	0xf0b500ff
+
+/* SNR_CARRIER2 */
+#define	R367TER_SNR_CARRIER2	0xf0b6
+#define	F367TER_MEAN	0xf0b600c0
+#define	F367TER_SNR_CARRIER_HI	0xf0b6001f
+
+/* PPM_CPAMP */
+#define	R367TER_PPM_CPAMP	0xf0b7
+#define	F367TER_PPM_CPC	0xf0b700ff
+
+/* TSM_AP0 */
+#define	R367TER_TSM_AP0	0xf0b8
+#define	F367TER_ADDRESS_BYTE_0	0xf0b800ff
+
+/* TSM_AP1 */
+#define	R367TER_TSM_AP1	0xf0b9
+#define	F367TER_ADDRESS_BYTE_1	0xf0b900ff
+
+/* TSM_AP2 */
+#define	R367TER_TSM_AP2	0xf0bA
+#define	F367TER_DATA_BYTE_0	0xf0ba00ff
+
+/* TSM_AP3 */
+#define	R367TER_TSM_AP3	0xf0bB
+#define	F367TER_DATA_BYTE_1	0xf0bb00ff
+
+/* TSM_AP4 */
+#define	R367TER_TSM_AP4	0xf0bC
+#define	F367TER_DATA_BYTE_2	0xf0bc00ff
+
+/* TSM_AP5 */
+#define	R367TER_TSM_AP5	0xf0bD
+#define	F367TER_DATA_BYTE_3	0xf0bd00ff
+
+/* TSM_AP6 */
+#define	R367TER_TSM_AP6	0xf0bE
+#define	F367TER_TSM_AP_6	0xf0be00ff
+
+/* TSM_AP7 */
+#define	R367TER_TSM_AP7	0xf0bF
+#define	F367TER_MEM_SELECT_BYTE	0xf0bf00ff
+
+/* TSTRES */
+#define	R367TER_TSTRES	0xf0c0
+#define	F367TER_FRES_DISPLAY	0xf0c00080
+#define	F367TER_FRES_FIFO_AD	0xf0c00020
+#define	F367TER_FRESRS	0xf0c00010
+#define	F367TER_FRESACS	0xf0c00008
+#define	F367TER_FRESFEC	0xf0c00004
+#define	F367TER_FRES_PRIF	0xf0c00002
+#define	F367TER_FRESCORE	0xf0c00001
+
+/* ANACTRL */
+#define	R367TER_ANACTRL	0xf0c1
+#define	F367TER_BYPASS_XTAL	0xf0c10040
+#define	F367TER_BYPASS_PLLXN	0xf0c1000c
+#define	F367TER_DIS_PAD_OSC	0xf0c10002
+#define	F367TER_STDBY_PLLXN	0xf0c10001
+
+/* TSTBUS */
+#define	R367TER_TSTBUS	0xf0c2
+#define	F367TER_TS_BYTE_CLK_INV	0xf0c20080
+#define	F367TER_CFG_IP	0xf0c20070
+#define	F367TER_CFG_TST	0xf0c2000f
+
+/* TSTRATE */
+#define	R367TER_TSTRATE	0xf0c6
+#define	F367TER_FORCEPHA	0xf0c60080
+#define	F367TER_FNEWPHA	0xf0c60010
+#define	F367TER_FROT90	0xf0c60008
+#define	F367TER_FR	0xf0c60007
+
+/* CONSTMODE */
+#define	R367TER_CONSTMODE	0xf0cb
+#define	F367TER_TST_PRIF	0xf0cb00e0
+#define	F367TER_CAR_TYPE	0xf0cb0018
+#define	F367TER_CONST_MODE	0xf0cb0003
+
+/* CONSTCARR1 */
+#define	R367TER_CONSTCARR1	0xf0cc
+#define	F367TER_CONST_CARR_LO	0xf0cc00ff
+
+/* CONSTCARR2 */
+#define	R367TER_CONSTCARR2	0xf0cd
+#define	F367TER_CONST_CARR_HI	0xf0cd001f
+
+/* ICONSTEL */
+#define	R367TER_ICONSTEL	0xf0ce
+#define	F367TER_PICONSTEL	0xf0ce00ff
+
+/* QCONSTEL */
+#define	R367TER_QCONSTEL	0xf0cf
+#define	F367TER_PQCONSTEL	0xf0cf00ff
+
+/* TSTBISTRES0 */
+#define	R367TER_TSTBISTRES0	0xf0d0
+#define	F367TER_BEND_PPM	0xf0d00080
+#define	F367TER_BBAD_PPM	0xf0d00040
+#define	F367TER_BEND_FFTW	0xf0d00020
+#define	F367TER_BBAD_FFTW	0xf0d00010
+#define	F367TER_BEND_FFT_BUF	0xf0d00008
+#define	F367TER_BBAD_FFT_BUF	0xf0d00004
+#define	F367TER_BEND_SYR	0xf0d00002
+#define	F367TER_BBAD_SYR	0xf0d00001
+
+/* TSTBISTRES1 */
+#define	R367TER_TSTBISTRES1	0xf0d1
+#define	F367TER_BEND_CHC_CP	0xf0d10080
+#define	F367TER_BBAD_CHC_CP	0xf0d10040
+#define	F367TER_BEND_CHCI	0xf0d10020
+#define	F367TER_BBAD_CHCI	0xf0d10010
+#define	F367TER_BEND_BDI	0xf0d10008
+#define	F367TER_BBAD_BDI	0xf0d10004
+#define	F367TER_BEND_SDI	0xf0d10002
+#define	F367TER_BBAD_SDI	0xf0d10001
+
+/* TSTBISTRES2 */
+#define	R367TER_TSTBISTRES2	0xf0d2
+#define	F367TER_BEND_CHC_INC	0xf0d20080
+#define	F367TER_BBAD_CHC_INC	0xf0d20040
+#define	F367TER_BEND_CHC_SPP	0xf0d20020
+#define	F367TER_BBAD_CHC_SPP	0xf0d20010
+#define	F367TER_BEND_CHC_CPP	0xf0d20008
+#define	F367TER_BBAD_CHC_CPP	0xf0d20004
+#define	F367TER_BEND_CHC_SP	0xf0d20002
+#define	F367TER_BBAD_CHC_SP	0xf0d20001
+
+/* TSTBISTRES3 */
+#define	R367TER_TSTBISTRES3	0xf0d3
+#define	F367TER_BEND_QAM	0xf0d30080
+#define	F367TER_BBAD_QAM	0xf0d30040
+#define	F367TER_BEND_SFEC_VIT	0xf0d30020
+#define	F367TER_BBAD_SFEC_VIT	0xf0d30010
+#define	F367TER_BEND_SFEC_DLINE	0xf0d30008
+#define	F367TER_BBAD_SFEC_DLINE	0xf0d30004
+#define	F367TER_BEND_SFEC_HW	0xf0d30002
+#define	F367TER_BBAD_SFEC_HW	0xf0d30001
+
+/* RF_AGC1 */
+#define	R367TER_RF_AGC1	0xf0d4
+#define	F367TER_RF_AGC1_LEVEL_HI	0xf0d400ff
+
+/* RF_AGC2 */
+#define	R367TER_RF_AGC2	0xf0d5
+#define	F367TER_REF_ADGP	0xf0d50080
+#define	F367TER_STDBY_ADCGP	0xf0d50020
+#define	F367TER_CHANNEL_SEL	0xf0d5001c
+#define	F367TER_RF_AGC1_LEVEL_LO	0xf0d50003
+
+/* ANADIGCTRL */
+#define	R367TER_ANADIGCTRL	0xf0d7
+#define	F367TER_SEL_CLKDEM	0xf0d70020
+#define	F367TER_EN_BUFFER_Q	0xf0d70010
+#define	F367TER_EN_BUFFER_I	0xf0d70008
+#define	F367TER_ADC_RIS_EGDE	0xf0d70004
+#define	F367TER_SGN_ADC	0xf0d70002
+#define	F367TER_SEL_AD12_SYNC	0xf0d70001
+
+/* PLLMDIV */
+#define	R367TER_PLLMDIV	0xf0d8
+#define	F367TER_PLL_MDIV	0xf0d800ff
+
+/* PLLNDIV */
+#define	R367TER_PLLNDIV	0xf0d9
+#define	F367TER_PLL_NDIV	0xf0d900ff
+
+/* PLLSETUP */
+#define	R367TER_PLLSETUP	0xf0dA
+#define	F367TER_PLL_PDIV	0xf0da0070
+#define	F367TER_PLL_KDIV	0xf0da000f
+
+/* DUAL_AD12 */
+#define	R367TER_DUAL_AD12	0xf0dB
+#define	F367TER_FS20M	0xf0db0020
+#define	F367TER_FS50M	0xf0db0010
+#define	F367TER_INMODe0	0xf0db0008
+#define	F367TER_POFFQ	0xf0db0004
+#define	F367TER_POFFI	0xf0db0002
+#define	F367TER_INMODE1	0xf0db0001
+
+/* TSTBIST */
+#define	R367TER_TSTBIST	0xf0dC
+#define	F367TER_TST_BYP_CLK	0xf0dc0080
+#define	F367TER_TST_GCLKENA_STD	0xf0dc0040
+#define	F367TER_TST_GCLKENA	0xf0dc0020
+#define	F367TER_TST_MEMBIST	0xf0dc001f
+
+/* PAD_COMP_CTRL */
+#define	R367TER_PAD_COMP_CTRL	0xf0dD
+#define	F367TER_COMPTQ	0xf0dd0010
+#define	F367TER_COMPEN	0xf0dd0008
+#define	F367TER_FREEZE2	0xf0dd0004
+#define	F367TER_SLEEP_INHBT	0xf0dd0002
+#define	F367TER_CHIP_SLEEP	0xf0dd0001
+
+/* PAD_COMP_WR */
+#define	R367TER_PAD_COMP_WR	0xf0de
+#define	F367TER_WR_ASRC	0xf0de007f
+
+/* PAD_COMP_RD */
+#define	R367TER_PAD_COMP_RD	0xf0df
+#define	F367TER_COMPOK	0xf0df0080
+#define	F367TER_RD_ASRC	0xf0df007f
+
+/* SYR_TARGET_FFTADJT_MSB */
+#define	R367TER_SYR_TARGET_FFTADJT_MSB	0xf100
+#define	F367TER_SYR_START	0xf1000080
+#define	F367TER_SYR_TARGET_FFTADJ_HI	0xf100000f
+
+/* SYR_TARGET_FFTADJT_LSB */
+#define	R367TER_SYR_TARGET_FFTADJT_LSB	0xf101
+#define	F367TER_SYR_TARGET_FFTADJ_LO	0xf10100ff
+
+/* SYR_TARGET_CHCADJT_MSB */
+#define	R367TER_SYR_TARGET_CHCADJT_MSB	0xf102
+#define	F367TER_SYR_TARGET_CHCADJ_HI	0xf102000f
+
+/* SYR_TARGET_CHCADJT_LSB */
+#define	R367TER_SYR_TARGET_CHCADJT_LSB	0xf103
+#define	F367TER_SYR_TARGET_CHCADJ_LO	0xf10300ff
+
+/* SYR_FLAG */
+#define	R367TER_SYR_FLAG	0xf104
+#define	F367TER_TRIG_FLG1	0xf1040080
+#define	F367TER_TRIG_FLG0	0xf1040040
+#define	F367TER_FFT_FLG1	0xf1040008
+#define	F367TER_FFT_FLG0	0xf1040004
+#define	F367TER_CHC_FLG1	0xf1040002
+#define	F367TER_CHC_FLG0	0xf1040001
+
+/* CRL_TARGET1 */
+#define	R367TER_CRL_TARGET1	0xf105
+#define	F367TER_CRL_START	0xf1050080
+#define	F367TER_CRL_TARGET_VHI	0xf105000f
+
+/* CRL_TARGET2 */
+#define	R367TER_CRL_TARGET2	0xf106
+#define	F367TER_CRL_TARGET_HI	0xf10600ff
+
+/* CRL_TARGET3 */
+#define	R367TER_CRL_TARGET3	0xf107
+#define	F367TER_CRL_TARGET_LO	0xf10700ff
+
+/* CRL_TARGET4 */
+#define	R367TER_CRL_TARGET4	0xf108
+#define	F367TER_CRL_TARGET_VLO	0xf10800ff
+
+/* CRL_FLAG */
+#define	R367TER_CRL_FLAG	0xf109
+#define	F367TER_CRL_FLAG1	0xf1090002
+#define	F367TER_CRL_FLAG0	0xf1090001
+
+/* TRL_TARGET1 */
+#define	R367TER_TRL_TARGET1	0xf10a
+#define	F367TER_TRL_TARGET_HI	0xf10a00ff
+
+/* TRL_TARGET2 */
+#define	R367TER_TRL_TARGET2	0xf10b
+#define	F367TER_TRL_TARGET_LO	0xf10b00ff
+
+/* TRL_CHC */
+#define	R367TER_TRL_CHC	0xf10c
+#define	F367TER_TRL_START	0xf10c0080
+#define	F367TER_CHC_START	0xf10c0040
+#define	F367TER_TRL_FLAG1	0xf10c0002
+#define	F367TER_TRL_FLAG0	0xf10c0001
+
+/* CHC_SNR_TARG */
+#define	R367TER_CHC_SNR_TARG	0xf10d
+#define	F367TER_CHC_SNR_TARGET	0xf10d00ff
+
+/* TOP_TRACK */
+#define	R367TER_TOP_TRACK	0xf10e
+#define	F367TER_TOP_START	0xf10e0080
+#define	F367TER_FIRST_FLAG	0xf10e0070
+#define	F367TER_TOP_FLAG1	0xf10e0008
+#define	F367TER_TOP_FLAG0	0xf10e0004
+#define	F367TER_CHC_FLAG1	0xf10e0002
+#define	F367TER_CHC_FLAG0	0xf10e0001
+
+/* TRACKER_FREE1 */
+#define	R367TER_TRACKER_FREE1	0xf10f
+#define	F367TER_TRACKER_FREE_1	0xf10f00ff
+
+/* ERROR_CRL1 */
+#define	R367TER_ERROR_CRL1	0xf110
+#define	F367TER_ERROR_CRL_VHI	0xf11000ff
+
+/* ERROR_CRL2 */
+#define	R367TER_ERROR_CRL2	0xf111
+#define	F367TER_ERROR_CRL_HI	0xf11100ff
+
+/* ERROR_CRL3 */
+#define	R367TER_ERROR_CRL3	0xf112
+#define	F367TER_ERROR_CRL_LOI	0xf11200ff
+
+/* ERROR_CRL4 */
+#define	R367TER_ERROR_CRL4	0xf113
+#define	F367TER_ERROR_CRL_VLO	0xf11300ff
+
+/* DEC_NCO1 */
+#define	R367TER_DEC_NCO1	0xf114
+#define	F367TER_DEC_NCO_VHI	0xf11400ff
+
+/* DEC_NCO2 */
+#define	R367TER_DEC_NCO2	0xf115
+#define	F367TER_DEC_NCO_HI	0xf11500ff
+
+/* DEC_NCO3 */
+#define	R367TER_DEC_NCO3	0xf116
+#define	F367TER_DEC_NCO_LO	0xf11600ff
+
+/* SNR */
+#define	R367TER_SNR	0xf117
+#define	F367TER_SNRATIO	0xf11700ff
+
+/* SYR_FFTADJ1 */
+#define	R367TER_SYR_FFTADJ1	0xf118
+#define	F367TER_SYR_FFTADJ_HI	0xf11800ff
+
+/* SYR_FFTADJ2 */
+#define	R367TER_SYR_FFTADJ2	0xf119
+#define	F367TER_SYR_FFTADJ_LO	0xf11900ff
+
+/* SYR_CHCADJ1 */
+#define	R367TER_SYR_CHCADJ1	0xf11a
+#define	F367TER_SYR_CHCADJ_HI	0xf11a00ff
+
+/* SYR_CHCADJ2 */
+#define	R367TER_SYR_CHCADJ2	0xf11b
+#define	F367TER_SYR_CHCADJ_LO	0xf11b00ff
+
+/* SYR_OFF */
+#define	R367TER_SYR_OFF	0xf11c
+#define	F367TER_SYR_OFFSET	0xf11c00ff
+
+/* PPM_OFFSET1 */
+#define	R367TER_PPM_OFFSET1	0xf11d
+#define	F367TER_PPM_OFFSET_HI	0xf11d00ff
+
+/* PPM_OFFSET2 */
+#define	R367TER_PPM_OFFSET2	0xf11e
+#define	F367TER_PPM_OFFSET_LO	0xf11e00ff
+
+/* TRACKER_FREE2 */
+#define	R367TER_TRACKER_FREE2	0xf11f
+#define	F367TER_TRACKER_FREE_2	0xf11f00ff
+
+/* DEBG_LT10 */
+#define	R367TER_DEBG_LT10	0xf120
+#define	F367TER_DEBUG_LT10	0xf12000ff
+
+/* DEBG_LT11 */
+#define	R367TER_DEBG_LT11	0xf121
+#define	F367TER_DEBUG_LT11	0xf12100ff
+
+/* DEBG_LT12 */
+#define	R367TER_DEBG_LT12	0xf122
+#define	F367TER_DEBUG_LT12	0xf12200ff
+
+/* DEBG_LT13 */
+#define	R367TER_DEBG_LT13	0xf123
+#define	F367TER_DEBUG_LT13	0xf12300ff
+
+/* DEBG_LT14 */
+#define	R367TER_DEBG_LT14	0xf124
+#define	F367TER_DEBUG_LT14	0xf12400ff
+
+/* DEBG_LT15 */
+#define	R367TER_DEBG_LT15	0xf125
+#define	F367TER_DEBUG_LT15	0xf12500ff
+
+/* DEBG_LT16 */
+#define	R367TER_DEBG_LT16	0xf126
+#define	F367TER_DEBUG_LT16	0xf12600ff
+
+/* DEBG_LT17 */
+#define	R367TER_DEBG_LT17	0xf127
+#define	F367TER_DEBUG_LT17	0xf12700ff
+
+/* DEBG_LT18 */
+#define	R367TER_DEBG_LT18	0xf128
+#define	F367TER_DEBUG_LT18	0xf12800ff
+
+/* DEBG_LT19 */
+#define	R367TER_DEBG_LT19	0xf129
+#define	F367TER_DEBUG_LT19	0xf12900ff
+
+/* DEBG_LT1a */
+#define	R367TER_DEBG_LT1A	0xf12a
+#define	F367TER_DEBUG_LT1A	0xf12a00ff
+
+/* DEBG_LT1b */
+#define	R367TER_DEBG_LT1B	0xf12b
+#define	F367TER_DEBUG_LT1B	0xf12b00ff
+
+/* DEBG_LT1c */
+#define	R367TER_DEBG_LT1C	0xf12c
+#define	F367TER_DEBUG_LT1C	0xf12c00ff
+
+/* DEBG_LT1D */
+#define	R367TER_DEBG_LT1D	0xf12d
+#define	F367TER_DEBUG_LT1D	0xf12d00ff
+
+/* DEBG_LT1E */
+#define	R367TER_DEBG_LT1E	0xf12e
+#define	F367TER_DEBUG_LT1E	0xf12e00ff
+
+/* DEBG_LT1F */
+#define	R367TER_DEBG_LT1F	0xf12f
+#define	F367TER_DEBUG_LT1F	0xf12f00ff
+
+/* RCCFGH */
+#define	R367TER_RCCFGH	0xf200
+#define	F367TER_TSRCFIFO_DVBCI	0xf2000080
+#define	F367TER_TSRCFIFO_SERIAL	0xf2000040
+#define	F367TER_TSRCFIFO_DISABLE	0xf2000020
+#define	F367TER_TSFIFO_2TORC	0xf2000010
+#define	F367TER_TSRCFIFO_HSGNLOUT	0xf2000008
+#define	F367TER_TSRCFIFO_ERRMODE	0xf2000006
+#define	F367TER_RCCFGH_0	0xf2000001
+
+/* RCCFGM */
+#define	R367TER_RCCFGM	0xf201
+#define	F367TER_TSRCFIFO_MANSPEED	0xf20100c0
+#define	F367TER_TSRCFIFO_PERMDATA	0xf2010020
+#define	F367TER_TSRCFIFO_NONEWSGNL	0xf2010010
+#define	F367TER_RCBYTE_OVERSAMPLING	0xf201000e
+#define	F367TER_TSRCFIFO_INVDATA	0xf2010001
+
+/* RCCFGL */
+#define	R367TER_RCCFGL	0xf202
+#define	F367TER_TSRCFIFO_BCLKDEL1cK	0xf20200c0
+#define	F367TER_RCCFGL_5	0xf2020020
+#define	F367TER_TSRCFIFO_DUTY50	0xf2020010
+#define	F367TER_TSRCFIFO_NSGNL2dATA	0xf2020008
+#define	F367TER_TSRCFIFO_DISSERMUX	0xf2020004
+#define	F367TER_RCCFGL_1	0xf2020002
+#define	F367TER_TSRCFIFO_STOPCKDIS	0xf2020001
+
+/* RCINSDELH */
+#define	R367TER_RCINSDELH	0xf203
+#define	F367TER_TSRCDEL_SYNCBYTE	0xf2030080
+#define	F367TER_TSRCDEL_XXHEADER	0xf2030040
+#define	F367TER_TSRCDEL_BBHEADER	0xf2030020
+#define	F367TER_TSRCDEL_DATAFIELD	0xf2030010
+#define	F367TER_TSRCINSDEL_ISCR	0xf2030008
+#define	F367TER_TSRCINSDEL_NPD	0xf2030004
+#define	F367TER_TSRCINSDEL_RSPARITY	0xf2030002
+#define	F367TER_TSRCINSDEL_CRC8	0xf2030001
+
+/* RCINSDELM */
+#define	R367TER_RCINSDELM	0xf204
+#define	F367TER_TSRCINS_BBPADDING	0xf2040080
+#define	F367TER_TSRCINS_BCHFEC	0xf2040040
+#define	F367TER_TSRCINS_LDPCFEC	0xf2040020
+#define	F367TER_TSRCINS_EMODCOD	0xf2040010
+#define	F367TER_TSRCINS_TOKEN	0xf2040008
+#define	F367TER_TSRCINS_XXXERR	0xf2040004
+#define	F367TER_TSRCINS_MATYPE	0xf2040002
+#define	F367TER_TSRCINS_UPL	0xf2040001
+
+/* RCINSDELL */
+#define	R367TER_RCINSDELL	0xf205
+#define	F367TER_TSRCINS_DFL	0xf2050080
+#define	F367TER_TSRCINS_SYNCD	0xf2050040
+#define	F367TER_TSRCINS_BLOCLEN	0xf2050020
+#define	F367TER_TSRCINS_SIGPCOUNT	0xf2050010
+#define	F367TER_TSRCINS_FIFO	0xf2050008
+#define	F367TER_TSRCINS_REALPACK	0xf2050004
+#define	F367TER_TSRCINS_TSCONFIG	0xf2050002
+#define	F367TER_TSRCINS_LATENCY	0xf2050001
+
+/* RCSTATUS */
+#define	R367TER_RCSTATUS	0xf206
+#define	F367TER_TSRCFIFO_LINEOK	0xf2060080
+#define	F367TER_TSRCFIFO_ERROR	0xf2060040
+#define	F367TER_TSRCFIFO_DATA7	0xf2060020
+#define	F367TER_RCSTATUS_4	0xf2060010
+#define	F367TER_TSRCFIFO_DEMODSEL	0xf2060008
+#define	F367TER_TSRC1FIFOSPEED_STORE	0xf2060004
+#define	F367TER_RCSTATUS_1	0xf2060002
+#define	F367TER_TSRCSERIAL_IMPOSSIBLE	0xf2060001
+
+/* RCSPEED */
+#define	R367TER_RCSPEED	0xf207
+#define	F367TER_TSRCFIFO_OUTSPEED	0xf20700ff
+
+/* RCDEBUGM */
+#define	R367TER_RCDEBUGM	0xf208
+#define	F367TER_SD_UNSYNC	0xf2080080
+#define	F367TER_ULFLOCK_DETECTM	0xf2080040
+#define	F367TER_SUL_SELECTOS	0xf2080020
+#define	F367TER_DILUL_NOSCRBLE	0xf2080010
+#define	F367TER_NUL_SCRB	0xf2080008
+#define	F367TER_UL_SCRB	0xf2080004
+#define	F367TER_SCRAULBAD	0xf2080002
+#define	F367TER_SCRAUL_UNSYNC	0xf2080001
+
+/* RCDEBUGL */
+#define	R367TER_RCDEBUGL	0xf209
+#define	F367TER_RS_ERR	0xf2090080
+#define	F367TER_LLFLOCK_DETECTM	0xf2090040
+#define	F367TER_NOT_SUL_SELECTOS	0xf2090020
+#define	F367TER_DILLL_NOSCRBLE	0xf2090010
+#define	F367TER_NLL_SCRB	0xf2090008
+#define	F367TER_LL_SCRB	0xf2090004
+#define	F367TER_SCRALLBAD	0xf2090002
+#define	F367TER_SCRALL_UNSYNC	0xf2090001
+
+/* RCOBSCFG */
+#define	R367TER_RCOBSCFG	0xf20a
+#define	F367TER_TSRCFIFO_OBSCFG	0xf20a00ff
+
+/* RCOBSM */
+#define	R367TER_RCOBSM	0xf20b
+#define	F367TER_TSRCFIFO_OBSDATA_HI	0xf20b00ff
+
+/* RCOBSL */
+#define	R367TER_RCOBSL	0xf20c
+#define	F367TER_TSRCFIFO_OBSDATA_LO	0xf20c00ff
+
+/* RCFECSPY */
+#define	R367TER_RCFECSPY	0xf210
+#define	F367TER_SPYRC_ENABLE	0xf2100080
+#define	F367TER_RCNO_SYNCBYTE	0xf2100040
+#define	F367TER_RCSERIAL_MODE	0xf2100020
+#define	F367TER_RCUNUSUAL_PACKET	0xf2100010
+#define	F367TER_BERRCMETER_DATAMODE	0xf210000c
+#define	F367TER_BERRCMETER_LMODE	0xf2100002
+#define	F367TER_BERRCMETER_RESET	0xf2100001
+
+/* RCFSPYCFG */
+#define	R367TER_RCFSPYCFG	0xf211
+#define	F367TER_FECSPYRC_INPUT	0xf21100c0
+#define	F367TER_RCRST_ON_ERROR	0xf2110020
+#define	F367TER_RCONE_SHOT	0xf2110010
+#define	F367TER_RCI2C_MODE	0xf211000c
+#define	F367TER_SPYRC_HSTERESIS	0xf2110003
+
+/* RCFSPYDATA */
+#define	R367TER_RCFSPYDATA	0xf212
+#define	F367TER_SPYRC_STUFFING	0xf2120080
+#define	F367TER_RCNOERR_PKTJITTER	0xf2120040
+#define	F367TER_SPYRC_CNULLPKT	0xf2120020
+#define	F367TER_SPYRC_OUTDATA_MODE	0xf212001f
+
+/* RCFSPYOUT */
+#define	R367TER_RCFSPYOUT	0xf213
+#define	F367TER_FSPYRC_DIRECT	0xf2130080
+#define	F367TER_RCFSPYOUT_6	0xf2130040
+#define	F367TER_SPYRC_OUTDATA_BUS	0xf2130038
+#define	F367TER_RCSTUFF_MODE	0xf2130007
+
+/* RCFSTATUS */
+#define	R367TER_RCFSTATUS	0xf214
+#define	F367TER_SPYRC_ENDSIM	0xf2140080
+#define	F367TER_RCVALID_SIM	0xf2140040
+#define	F367TER_RCFOUND_SIGNAL	0xf2140020
+#define	F367TER_RCDSS_SYNCBYTE	0xf2140010
+#define	F367TER_RCRESULT_STATE	0xf214000f
+
+/* RCFGOODPACK */
+#define	R367TER_RCFGOODPACK	0xf215
+#define	F367TER_RCGOOD_PACKET	0xf21500ff
+
+/* RCFPACKCNT */
+#define	R367TER_RCFPACKCNT	0xf216
+#define	F367TER_RCPACKET_COUNTER	0xf21600ff
+
+/* RCFSPYMISC */
+#define	R367TER_RCFSPYMISC	0xf217
+#define	F367TER_RCLABEL_COUNTER	0xf21700ff
+
+/* RCFBERCPT4 */
+#define	R367TER_RCFBERCPT4	0xf218
+#define	F367TER_FBERRCMETER_CPT_MMMMSB	0xf21800ff
+
+/* RCFBERCPT3 */
+#define	R367TER_RCFBERCPT3	0xf219
+#define	F367TER_FBERRCMETER_CPT_MMMSB	0xf21900ff
+
+/* RCFBERCPT2 */
+#define	R367TER_RCFBERCPT2	0xf21a
+#define	F367TER_FBERRCMETER_CPT_MMSB	0xf21a00ff
+
+/* RCFBERCPT1 */
+#define	R367TER_RCFBERCPT1	0xf21b
+#define	F367TER_FBERRCMETER_CPT_MSB	0xf21b00ff
+
+/* RCFBERCPT0 */
+#define	R367TER_RCFBERCPT0	0xf21c
+#define	F367TER_FBERRCMETER_CPT_LSB	0xf21c00ff
+
+/* RCFBERERR2 */
+#define	R367TER_RCFBERERR2	0xf21d
+#define	F367TER_FBERRCMETER_ERR_HI	0xf21d00ff
+
+/* RCFBERERR1 */
+#define	R367TER_RCFBERERR1	0xf21e
+#define	F367TER_FBERRCMETER_ERR	0xf21e00ff
+
+/* RCFBERERR0 */
+#define	R367TER_RCFBERERR0	0xf21f
+#define	F367TER_FBERRCMETER_ERR_LO	0xf21f00ff
+
+/* RCFSTATESM */
+#define	R367TER_RCFSTATESM	0xf220
+#define	F367TER_RCRSTATE_F	0xf2200080
+#define	F367TER_RCRSTATE_E	0xf2200040
+#define	F367TER_RCRSTATE_D	0xf2200020
+#define	F367TER_RCRSTATE_C	0xf2200010
+#define	F367TER_RCRSTATE_B	0xf2200008
+#define	F367TER_RCRSTATE_A	0xf2200004
+#define	F367TER_RCRSTATE_9	0xf2200002
+#define	F367TER_RCRSTATE_8	0xf2200001
+
+/* RCFSTATESL */
+#define	R367TER_RCFSTATESL	0xf221
+#define	F367TER_RCRSTATE_7	0xf2210080
+#define	F367TER_RCRSTATE_6	0xf2210040
+#define	F367TER_RCRSTATE_5	0xf2210020
+#define	F367TER_RCRSTATE_4	0xf2210010
+#define	F367TER_RCRSTATE_3	0xf2210008
+#define	F367TER_RCRSTATE_2	0xf2210004
+#define	F367TER_RCRSTATE_1	0xf2210002
+#define	F367TER_RCRSTATE_0	0xf2210001
+
+/* RCFSPYBER */
+#define	R367TER_RCFSPYBER	0xf222
+#define	F367TER_RCFSPYBER_7	0xf2220080
+#define	F367TER_SPYRCOBS_XORREAD	0xf2220040
+#define	F367TER_FSPYRCBER_OBSMODE	0xf2220020
+#define	F367TER_FSPYRCBER_SYNCBYT	0xf2220010
+#define	F367TER_FSPYRCBER_UNSYNC	0xf2220008
+#define	F367TER_FSPYRCBER_CTIME	0xf2220007
+
+/* RCFSPYDISTM */
+#define	R367TER_RCFSPYDISTM	0xf223
+#define	F367TER_RCPKTTIME_DISTANCE_HI	0xf22300ff
+
+/* RCFSPYDISTL */
+#define	R367TER_RCFSPYDISTL	0xf224
+#define	F367TER_RCPKTTIME_DISTANCE_LO	0xf22400ff
+
+/* RCFSPYOBS7 */
+#define	R367TER_RCFSPYOBS7	0xf228
+#define	F367TER_RCSPYOBS_SPYFAIL	0xf2280080
+#define	F367TER_RCSPYOBS_SPYFAIL1	0xf2280040
+#define	F367TER_RCSPYOBS_ERROR	0xf2280020
+#define	F367TER_RCSPYOBS_STROUT	0xf2280010
+#define	F367TER_RCSPYOBS_RESULTSTATE1	0xf228000f
+
+/* RCFSPYOBS6 */
+#define	R367TER_RCFSPYOBS6	0xf229
+#define	F367TER_RCSPYOBS_RESULTSTATe0	0xf22900f0
+#define	F367TER_RCSPYOBS_RESULTSTATEM1	0xf229000f
+
+/* RCFSPYOBS5 */
+#define	R367TER_RCFSPYOBS5	0xf22a
+#define	F367TER_RCSPYOBS_BYTEOFPACKET1	0xf22a00ff
+
+/* RCFSPYOBS4 */
+#define	R367TER_RCFSPYOBS4	0xf22b
+#define	F367TER_RCSPYOBS_BYTEVALUE1	0xf22b00ff
+
+/* RCFSPYOBS3 */
+#define	R367TER_RCFSPYOBS3	0xf22c
+#define	F367TER_RCSPYOBS_DATA1	0xf22c00ff
+
+/* RCFSPYOBS2 */
+#define	R367TER_RCFSPYOBS2	0xf22d
+#define	F367TER_RCSPYOBS_DATa0	0xf22d00ff
+
+/* RCFSPYOBS1 */
+#define	R367TER_RCFSPYOBS1	0xf22e
+#define	F367TER_RCSPYOBS_DATAM1	0xf22e00ff
+
+/* RCFSPYOBS0 */
+#define	R367TER_RCFSPYOBS0	0xf22f
+#define	F367TER_RCSPYOBS_DATAM2	0xf22f00ff
+
+/* TSGENERAL */
+#define	R367TER_TSGENERAL	0xf230
+#define	F367TER_TSGENERAL_7	0xf2300080
+#define	F367TER_TSGENERAL_6	0xf2300040
+#define	F367TER_TSFIFO_BCLK1aLL	0xf2300020
+#define	F367TER_TSGENERAL_4	0xf2300010
+#define	F367TER_MUXSTREAM_OUTMODE	0xf2300008
+#define	F367TER_TSFIFO_PERMPARAL	0xf2300006
+#define	F367TER_RST_REEDSOLO	0xf2300001
+
+/* RC1SPEED */
+#define	R367TER_RC1SPEED	0xf231
+#define	F367TER_TSRCFIFO1_OUTSPEED	0xf23100ff
+
+/* TSGSTATUS */
+#define	R367TER_TSGSTATUS	0xf232
+#define	F367TER_TSGSTATUS_7	0xf2320080
+#define	F367TER_TSGSTATUS_6	0xf2320040
+#define	F367TER_RSMEM_FULL	0xf2320020
+#define	F367TER_RS_MULTCALC	0xf2320010
+#define	F367TER_RSIN_OVERTIME	0xf2320008
+#define	F367TER_TSFIFO3_DEMODSEL	0xf2320004
+#define	F367TER_TSFIFO2_DEMODSEL	0xf2320002
+#define	F367TER_TSFIFO1_DEMODSEL	0xf2320001
+
+
+/* FECM */
+#define	R367TER_FECM	0xf233
+#define	F367TER_DSS_DVB	0xf2330080
+#define	F367TER_DEMOD_BYPASS	0xf2330040
+#define	F367TER_CMP_SLOWMODE	0xf2330020
+#define	F367TER_DSS_SRCH	0xf2330010
+#define	F367TER_FECM_3	0xf2330008
+#define	F367TER_DIFF_MODEVIT	0xf2330004
+#define	F367TER_SYNCVIT	0xf2330002
+#define	F367TER_I2CSYM	0xf2330001
+
+/* VTH12 */
+#define	R367TER_VTH12	0xf234
+#define	F367TER_VTH_12	0xf23400ff
+
+/* VTH23 */
+#define	R367TER_VTH23	0xf235
+#define	F367TER_VTH_23	0xf23500ff
+
+/* VTH34 */
+#define	R367TER_VTH34	0xf236
+#define	F367TER_VTH_34	0xf23600ff
+
+/* VTH56 */
+#define	R367TER_VTH56	0xf237
+#define	F367TER_VTH_56	0xf23700ff
+
+/* VTH67 */
+#define	R367TER_VTH67	0xf238
+#define	F367TER_VTH_67	0xf23800ff
+
+/* VTH78 */
+#define	R367TER_VTH78	0xf239
+#define	F367TER_VTH_78	0xf23900ff
+
+/* VITCURPUN */
+#define	R367TER_VITCURPUN	0xf23a
+#define	F367TER_VIT_MAPPING	0xf23a00e0
+#define	F367TER_VIT_CURPUN	0xf23a001f
+
+/* VERROR */
+#define	R367TER_VERROR	0xf23b
+#define	F367TER_REGERR_VIT	0xf23b00ff
+
+/* PRVIT */
+#define	R367TER_PRVIT	0xf23c
+#define	F367TER_PRVIT_7	0xf23c0080
+#define	F367TER_DIS_VTHLOCK	0xf23c0040
+#define	F367TER_E7_8VIT	0xf23c0020
+#define	F367TER_E6_7VIT	0xf23c0010
+#define	F367TER_E5_6VIT	0xf23c0008
+#define	F367TER_E3_4VIT	0xf23c0004
+#define	F367TER_E2_3VIT	0xf23c0002
+#define	F367TER_E1_2VIT	0xf23c0001
+
+/* VAVSRVIT */
+#define	R367TER_VAVSRVIT	0xf23d
+#define	F367TER_AMVIT	0xf23d0080
+#define	F367TER_FROZENVIT	0xf23d0040
+#define	F367TER_SNVIT	0xf23d0030
+#define	F367TER_TOVVIT	0xf23d000c
+#define	F367TER_HYPVIT	0xf23d0003
+
+/* VSTATUSVIT */
+#define	R367TER_VSTATUSVIT	0xf23e
+#define	F367TER_VITERBI_ON	0xf23e0080
+#define	F367TER_END_LOOPVIT	0xf23e0040
+#define	F367TER_VITERBI_DEPRF	0xf23e0020
+#define	F367TER_PRFVIT	0xf23e0010
+#define	F367TER_LOCKEDVIT	0xf23e0008
+#define	F367TER_VITERBI_DELOCK	0xf23e0004
+#define	F367TER_VIT_DEMODSEL	0xf23e0002
+#define	F367TER_VITERBI_COMPOUT	0xf23e0001
+
+/* VTHINUSE */
+#define	R367TER_VTHINUSE	0xf23f
+#define	F367TER_VIT_INUSE	0xf23f00ff
+
+/* KDIV12 */
+#define	R367TER_KDIV12	0xf240
+#define	F367TER_KDIV12_MANUAL	0xf2400080
+#define	F367TER_K_DIVIDER_12	0xf240007f
+
+/* KDIV23 */
+#define	R367TER_KDIV23	0xf241
+#define	F367TER_KDIV23_MANUAL	0xf2410080
+#define	F367TER_K_DIVIDER_23	0xf241007f
+
+/* KDIV34 */
+#define	R367TER_KDIV34	0xf242
+#define	F367TER_KDIV34_MANUAL	0xf2420080
+#define	F367TER_K_DIVIDER_34	0xf242007f
+
+/* KDIV56 */
+#define	R367TER_KDIV56	0xf243
+#define	F367TER_KDIV56_MANUAL	0xf2430080
+#define	F367TER_K_DIVIDER_56	0xf243007f
+
+/* KDIV67 */
+#define	R367TER_KDIV67	0xf244
+#define	F367TER_KDIV67_MANUAL	0xf2440080
+#define	F367TER_K_DIVIDER_67	0xf244007f
+
+/* KDIV78 */
+#define	R367TER_KDIV78	0xf245
+#define	F367TER_KDIV78_MANUAL	0xf2450080
+#define	F367TER_K_DIVIDER_78	0xf245007f
+
+/* SIGPOWER */
+#define	R367TER_SIGPOWER	0xf246
+#define	F367TER_SIGPOWER_MANUAL	0xf2460080
+#define	F367TER_SIG_POWER	0xf246007f
+
+/* DEMAPVIT */
+#define	R367TER_DEMAPVIT	0xf247
+#define	F367TER_DEMAPVIT_7	0xf2470080
+#define	F367TER_K_DIVIDER_VIT	0xf247007f
+
+/* VITSCALE */
+#define	R367TER_VITSCALE	0xf248
+#define	F367TER_NVTH_NOSRANGE	0xf2480080
+#define	F367TER_VERROR_MAXMODE	0xf2480040
+#define	F367TER_KDIV_MODE	0xf2480030
+#define	F367TER_NSLOWSN_LOCKED	0xf2480008
+#define	F367TER_DELOCK_PRFLOSS	0xf2480004
+#define	F367TER_DIS_RSFLOCK	0xf2480002
+#define	F367TER_VITSCALE_0	0xf2480001
+
+/* FFEC1PRG */
+#define	R367TER_FFEC1PRG	0xf249
+#define	F367TER_FDSS_DVB	0xf2490080
+#define	F367TER_FDSS_SRCH	0xf2490040
+#define	F367TER_FFECPROG_5	0xf2490020
+#define	F367TER_FFECPROG_4	0xf2490010
+#define	F367TER_FFECPROG_3	0xf2490008
+#define	F367TER_FFECPROG_2	0xf2490004
+#define	F367TER_FTS1_DISABLE	0xf2490002
+#define	F367TER_FTS2_DISABLE	0xf2490001
+
+/* FVITCURPUN */
+#define	R367TER_FVITCURPUN	0xf24a
+#define	F367TER_FVIT_MAPPING	0xf24a00e0
+#define	F367TER_FVIT_CURPUN	0xf24a001f
+
+/* FVERROR */
+#define	R367TER_FVERROR	0xf24b
+#define	F367TER_FREGERR_VIT	0xf24b00ff
+
+/* FVSTATUSVIT */
+#define	R367TER_FVSTATUSVIT	0xf24c
+#define	F367TER_FVITERBI_ON	0xf24c0080
+#define	F367TER_F1END_LOOPVIT	0xf24c0040
+#define	F367TER_FVITERBI_DEPRF	0xf24c0020
+#define	F367TER_FPRFVIT	0xf24c0010
+#define	F367TER_FLOCKEDVIT	0xf24c0008
+#define	F367TER_FVITERBI_DELOCK	0xf24c0004
+#define	F367TER_FVIT_DEMODSEL	0xf24c0002
+#define	F367TER_FVITERBI_COMPOUT	0xf24c0001
+
+/* DEBUG_LT1 */
+#define	R367TER_DEBUG_LT1	0xf24d
+#define	F367TER_DBG_LT1	0xf24d00ff
+
+/* DEBUG_LT2 */
+#define	R367TER_DEBUG_LT2	0xf24e
+#define	F367TER_DBG_LT2	0xf24e00ff
+
+/* DEBUG_LT3 */
+#define	R367TER_DEBUG_LT3	0xf24f
+#define	F367TER_DBG_LT3	0xf24f00ff
+
+/*	TSTSFMET */
+#define	R367TER_TSTSFMET	0xf250
+#define F367TER_TSTSFEC_METRIQUES	0xf25000ff
+
+/*	SELOUT */
+#define	R367TER_SELOUT	0xf252
+#define	F367TER_EN_SYNC	0xf2520080
+#define	F367TER_EN_TBUSDEMAP	0xf2520040
+#define	F367TER_SELOUT_5	0xf2520020
+#define	F367TER_SELOUT_4	0xf2520010
+#define	F367TER_TSTSYNCHRO_MODE	0xf2520002
+
+/*	TSYNC */
+#define R367TER_TSYNC	0xf253
+#define F367TER_CURPUN_INCMODE	0xf2530080
+#define F367TER_CERR_TSTMODE	0xf2530040
+#define F367TER_SHIFTSOF_MODE	0xf2530030
+#define F367TER_SLOWPHA_MODE	0xf2530008
+#define F367TER_PXX_BYPALL	0xf2530004
+#define F367TER_FROTA45_FIRST	0xf2530002
+#define F367TER_TST_BCHERROR	0xf2530001
+
+/*	TSTERR */
+#define R367TER_TSTERR	0xf254
+#define F367TER_TST_LONGPKT	0xf2540080
+#define F367TER_TST_ISSYION	0xf2540040
+#define F367TER_TST_NPDON	0xf2540020
+#define F367TER_TSTERR_4	0xf2540010
+#define F367TER_TRACEBACK_MODE	0xf2540008
+#define F367TER_TST_RSPARITY	0xf2540004
+#define F367TER_METRIQUE_MODE	0xf2540003
+
+/*	TSFSYNC */
+#define R367TER_TSFSYNC	0xf255
+#define F367TER_EN_SFECSYNC	0xf2550080
+#define F367TER_EN_SFECDEMAP	0xf2550040
+#define F367TER_SFCERR_TSTMODE	0xf2550020
+#define F367TER_SFECPXX_BYPALL	0xf2550010
+#define F367TER_SFECTSTSYNCHRO_MODE 0xf255000f
+
+/*	TSTSFERR */
+#define R367TER_TSTSFERR	0xf256
+#define F367TER_TSTSTERR_7	0xf2560080
+#define F367TER_TSTSTERR_6	0xf2560040
+#define F367TER_TSTSTERR_5	0xf2560020
+#define F367TER_TSTSTERR_4	0xf2560010
+#define F367TER_SFECTRACEBACK_MODE	0xf2560008
+#define F367TER_SFEC_NCONVPROG	0xf2560004
+#define F367TER_SFECMETRIQUE_MODE	0xf2560003
+
+/*	TSTTSSF1 */
+#define R367TER_TSTTSSF1	0xf258
+#define F367TER_TSTERSSF	0xf2580080
+#define F367TER_TSTTSSFEN	0xf2580040
+#define F367TER_SFEC_OUTMODE	0xf2580030
+#define F367TER_XLSF_NOFTHRESHOLD  0xf2580008
+#define F367TER_TSTTSSF_STACKSEL	0xf2580007
+
+/*	TSTTSSF2 */
+#define R367TER_TSTTSSF2	0xf259
+#define F367TER_DILSF_DBBHEADER	0xf2590080
+#define F367TER_TSTTSSF_DISBUG	0xf2590040
+#define F367TER_TSTTSSF_NOBADSTART	0xf2590020
+#define F367TER_TSTTSSF_SELECT	0xf259001f
+
+/*	TSTTSSF3 */
+#define R367TER_TSTTSSF3	0xf25a
+#define F367TER_TSTTSSF3_7	0xf25a0080
+#define F367TER_TSTTSSF3_6	0xf25a0040
+#define F367TER_TSTTSSF3_5	0xf25a0020
+#define F367TER_TSTTSSF3_4	0xf25a0010
+#define F367TER_TSTTSSF3_3	0xf25a0008
+#define F367TER_TSTTSSF3_2	0xf25a0004
+#define F367TER_TSTTSSF3_1	0xf25a0002
+#define F367TER_DISSF_CLKENABLE	0xf25a0001
+
+/*	TSTTS1 */
+#define R367TER_TSTTS1	0xf25c
+#define F367TER_TSTERS	0xf25c0080
+#define F367TER_TSFIFO_DSSSYNCB	0xf25c0040
+#define F367TER_TSTTS_FSPYBEFRS	0xf25c0020
+#define F367TER_NFORCE_SYNCBYTE	0xf25c0010
+#define F367TER_XL_NOFTHRESHOLD	0xf25c0008
+#define F367TER_TSTTS_FRFORCEPKT	0xf25c0004
+#define F367TER_DESCR_NOTAUTO	0xf25c0002
+#define F367TER_TSTTSEN	0xf25c0001
+
+/*	TSTTS2 */
+#define R367TER_TSTTS2	0xf25d
+#define F367TER_DIL_DBBHEADER	0xf25d0080
+#define F367TER_TSTTS_NOBADXXX	0xf25d0040
+#define F367TER_TSFIFO_DELSPEEDUP	0xf25d0020
+#define F367TER_TSTTS_SELECT	0xf25d001f
+
+/*	TSTTS3 */
+#define R367TER_TSTTS3	0xf25e
+#define F367TER_TSTTS_NOPKTGAIN	0xf25e0080
+#define F367TER_TSTTS_NOPKTENE	0xf25e0040
+#define F367TER_TSTTS_ISOLATION	0xf25e0020
+#define F367TER_TSTTS_DISBUG	0xf25e0010
+#define F367TER_TSTTS_NOBADSTART	0xf25e0008
+#define F367TER_TSTTS_STACKSEL	0xf25e0007
+
+/*	TSTTS4 */
+#define R367TER_TSTTS4	0xf25f
+#define F367TER_TSTTS4_7	0xf25f0080
+#define F367TER_TSTTS4_6	0xf25f0040
+#define F367TER_TSTTS4_5	0xf25f0020
+#define F367TER_TSTTS_DISDSTATE	0xf25f0010
+#define F367TER_TSTTS_FASTNOSYNC	0xf25f0008
+#define F367TER_EXT_FECSPYIN	0xf25f0004
+#define F367TER_TSTTS_NODPZERO	0xf25f0002
+#define F367TER_TSTTS_NODIV3	0xf25f0001
+
+/*	TSTTSRC */
+#define R367TER_TSTTSRC	0xf26c
+#define F367TER_TSTTSRC_7	0xf26c0080
+#define F367TER_TSRCFIFO_DSSSYNCB	0xf26c0040
+#define F367TER_TSRCFIFO_DPUNACTIVE	0xf26c0020
+#define F367TER_TSRCFIFO_DELSPEEDUP	0xf26c0010
+#define F367TER_TSTTSRC_NODIV3	0xf26c0008
+#define F367TER_TSTTSRC_FRFORCEPKT	0xf26c0004
+#define F367TER_SAT25_SDDORIGINE	0xf26c0002
+#define F367TER_TSTTSRC_INACTIVE	0xf26c0001
+
+/*	TSTTSRS */
+#define R367TER_TSTTSRS	0xf26d
+#define F367TER_TSTTSRS_7	0xf26d0080
+#define F367TER_TSTTSRS_6	0xf26d0040
+#define F367TER_TSTTSRS_5	0xf26d0020
+#define F367TER_TSTTSRS_4	0xf26d0010
+#define F367TER_TSTTSRS_3	0xf26d0008
+#define F367TER_TSTTSRS_2	0xf26d0004
+#define F367TER_TSTRS_DISRS2	0xf26d0002
+#define F367TER_TSTRS_DISRS1	0xf26d0001
+
+/* TSSTATEM */
+#define	R367TER_TSSTATEM	0xf270
+#define	F367TER_TSDIL_ON	0xf2700080
+#define	F367TER_TSSKIPRS_ON	0xf2700040
+#define	F367TER_TSRS_ON	0xf2700020
+#define	F367TER_TSDESCRAMB_ON	0xf2700010
+#define	F367TER_TSFRAME_MODE	0xf2700008
+#define	F367TER_TS_DISABLE	0xf2700004
+#define	F367TER_TSACM_MODE	0xf2700002
+#define	F367TER_TSOUT_NOSYNC	0xf2700001
+
+/* TSSTATEL */
+#define	R367TER_TSSTATEL	0xf271
+#define	F367TER_TSNOSYNCBYTE	0xf2710080
+#define	F367TER_TSPARITY_ON	0xf2710040
+#define	F367TER_TSSYNCOUTRS_ON	0xf2710020
+#define	F367TER_TSDVBS2_MODE	0xf2710010
+#define	F367TER_TSISSYI_ON	0xf2710008
+#define	F367TER_TSNPD_ON	0xf2710004
+#define	F367TER_TSCRC8_ON	0xf2710002
+#define	F367TER_TSDSS_PACKET	0xf2710001
+
+/* TSCFGH */
+#define	R367TER_TSCFGH	0xf272
+#define	F367TER_TSFIFO_DVBCI	0xf2720080
+#define	F367TER_TSFIFO_SERIAL	0xf2720040
+#define	F367TER_TSFIFO_TEIUPDATE	0xf2720020
+#define	F367TER_TSFIFO_DUTY50	0xf2720010
+#define	F367TER_TSFIFO_HSGNLOUT	0xf2720008
+#define	F367TER_TSFIFO_ERRMODE	0xf2720006
+#define	F367TER_RST_HWARE	0xf2720001
+
+/* TSCFGM */
+#define	R367TER_TSCFGM	0xf273
+#define	F367TER_TSFIFO_MANSPEED	0xf27300c0
+#define	F367TER_TSFIFO_PERMDATA	0xf2730020
+#define	F367TER_TSFIFO_NONEWSGNL	0xf2730010
+#define	F367TER_TSFIFO_BITSPEED	0xf2730008
+#define	F367TER_NPD_SPECDVBS2	0xf2730004
+#define	F367TER_TSFIFO_STOPCKDIS	0xf2730002
+#define	F367TER_TSFIFO_INVDATA	0xf2730001
+
+/* TSCFGL */
+#define	R367TER_TSCFGL	0xf274
+#define	F367TER_TSFIFO_BCLKDEL1cK	0xf27400c0
+#define	F367TER_BCHERROR_MODE	0xf2740030
+#define	F367TER_TSFIFO_NSGNL2dATA	0xf2740008
+#define	F367TER_TSFIFO_EMBINDVB	0xf2740004
+#define	F367TER_TSFIFO_DPUNACT	0xf2740002
+#define	F367TER_TSFIFO_NPDOFF	0xf2740001
+
+/* TSSYNC */
+#define	R367TER_TSSYNC	0xf275
+#define	F367TER_TSFIFO_PERMUTE	0xf2750080
+#define	F367TER_TSFIFO_FISCR3B	0xf2750060
+#define	F367TER_TSFIFO_SYNCMODE	0xf2750018
+#define	F367TER_TSFIFO_SYNCSEL	0xf2750007
+
+/* TSINSDELH */
+#define	R367TER_TSINSDELH	0xf276
+#define	F367TER_TSDEL_SYNCBYTE	0xf2760080
+#define	F367TER_TSDEL_XXHEADER	0xf2760040
+#define	F367TER_TSDEL_BBHEADER	0xf2760020
+#define	F367TER_TSDEL_DATAFIELD	0xf2760010
+#define	F367TER_TSINSDEL_ISCR	0xf2760008
+#define	F367TER_TSINSDEL_NPD	0xf2760004
+#define	F367TER_TSINSDEL_RSPARITY	0xf2760002
+#define	F367TER_TSINSDEL_CRC8	0xf2760001
+
+/* TSINSDELM */
+#define	R367TER_TSINSDELM	0xf277
+#define	F367TER_TSINS_BBPADDING	0xf2770080
+#define	F367TER_TSINS_BCHFEC	0xf2770040
+#define	F367TER_TSINS_LDPCFEC	0xf2770020
+#define	F367TER_TSINS_EMODCOD	0xf2770010
+#define	F367TER_TSINS_TOKEN	0xf2770008
+#define	F367TER_TSINS_XXXERR	0xf2770004
+#define	F367TER_TSINS_MATYPE	0xf2770002
+#define	F367TER_TSINS_UPL	0xf2770001
+
+/* TSINSDELL */
+#define	R367TER_TSINSDELL	0xf278
+#define	F367TER_TSINS_DFL	0xf2780080
+#define	F367TER_TSINS_SYNCD	0xf2780040
+#define	F367TER_TSINS_BLOCLEN	0xf2780020
+#define	F367TER_TSINS_SIGPCOUNT	0xf2780010
+#define	F367TER_TSINS_FIFO	0xf2780008
+#define	F367TER_TSINS_REALPACK	0xf2780004
+#define	F367TER_TSINS_TSCONFIG	0xf2780002
+#define	F367TER_TSINS_LATENCY	0xf2780001
+
+/* TSDIVN */
+#define	R367TER_TSDIVN	0xf279
+#define	F367TER_TSFIFO_LOWSPEED	0xf2790080
+#define	F367TER_BYTE_OVERSAMPLING	0xf2790070
+#define	F367TER_TSMANUAL_PACKETNBR	0xf279000f
+
+/* TSDIVPM */
+#define	R367TER_TSDIVPM	0xf27a
+#define	F367TER_TSMANUAL_P_HI	0xf27a00ff
+
+/* TSDIVPL */
+#define	R367TER_TSDIVPL	0xf27b
+#define	F367TER_TSMANUAL_P_LO	0xf27b00ff
+
+/* TSDIVQM */
+#define	R367TER_TSDIVQM	0xf27c
+#define	F367TER_TSMANUAL_Q_HI	0xf27c00ff
+
+/* TSDIVQL */
+#define	R367TER_TSDIVQL	0xf27d
+#define	F367TER_TSMANUAL_Q_LO	0xf27d00ff
+
+/* TSDILSTKM */
+#define	R367TER_TSDILSTKM	0xf27e
+#define	F367TER_TSFIFO_DILSTK_HI	0xf27e00ff
+
+/* TSDILSTKL */
+#define	R367TER_TSDILSTKL	0xf27f
+#define	F367TER_TSFIFO_DILSTK_LO	0xf27f00ff
+
+/* TSSPEED */
+#define	R367TER_TSSPEED	0xf280
+#define	F367TER_TSFIFO_OUTSPEED	0xf28000ff
+
+/* TSSTATUS */
+#define	R367TER_TSSTATUS	0xf281
+#define	F367TER_TSFIFO_LINEOK	0xf2810080
+#define	F367TER_TSFIFO_ERROR	0xf2810040
+#define	F367TER_TSFIFO_DATA7	0xf2810020
+#define	F367TER_TSFIFO_NOSYNC	0xf2810010
+#define	F367TER_ISCR_INITIALIZED	0xf2810008
+#define	F367TER_ISCR_UPDATED	0xf2810004
+#define	F367TER_SOFFIFO_UNREGUL	0xf2810002
+#define	F367TER_DIL_READY	0xf2810001
+
+/* TSSTATUS2 */
+#define	R367TER_TSSTATUS2	0xf282
+#define	F367TER_TSFIFO_DEMODSEL	0xf2820080
+#define	F367TER_TSFIFOSPEED_STORE	0xf2820040
+#define	F367TER_DILXX_RESET	0xf2820020
+#define	F367TER_TSSERIAL_IMPOSSIBLE	0xf2820010
+#define	F367TER_TSFIFO_UNDERSPEED	0xf2820008
+#define	F367TER_BITSPEED_EVENT	0xf2820004
+#define	F367TER_UL_SCRAMBDETECT	0xf2820002
+#define	F367TER_ULDTV67_FALSELOCK	0xf2820001
+
+/* TSBITRATEM */
+#define	R367TER_TSBITRATEM	0xf283
+#define	F367TER_TSFIFO_BITRATE_HI	0xf28300ff
+
+/* TSBITRATEL */
+#define	R367TER_TSBITRATEL	0xf284
+#define	F367TER_TSFIFO_BITRATE_LO	0xf28400ff
+
+/* TSPACKLENM */
+#define	R367TER_TSPACKLENM	0xf285
+#define	F367TER_TSFIFO_PACKCPT	0xf28500e0
+#define	F367TER_DIL_RPLEN_HI	0xf285001f
+
+/* TSPACKLENL */
+#define	R367TER_TSPACKLENL	0xf286
+#define	F367TER_DIL_RPLEN_LO	0xf28600ff
+
+/* TSBLOCLENM */
+#define	R367TER_TSBLOCLENM	0xf287
+#define	F367TER_TSFIFO_PFLEN_HI	0xf28700ff
+
+/* TSBLOCLENL */
+#define	R367TER_TSBLOCLENL	0xf288
+#define	F367TER_TSFIFO_PFLEN_LO	0xf28800ff
+
+/* TSDLYH */
+#define	R367TER_TSDLYH	0xf289
+#define	F367TER_SOFFIFO_TSTIMEVALID	0xf2890080
+#define	F367TER_SOFFIFO_SPEEDUP	0xf2890040
+#define	F367TER_SOFFIFO_STOP	0xf2890020
+#define	F367TER_SOFFIFO_REGULATED	0xf2890010
+#define	F367TER_SOFFIFO_REALSBOFF_HI	0xf289000f
+
+/* TSDLYM */
+#define	R367TER_TSDLYM	0xf28a
+#define	F367TER_SOFFIFO_REALSBOFF_MED	0xf28a00ff
+
+/* TSDLYL */
+#define	R367TER_TSDLYL	0xf28b
+#define	F367TER_SOFFIFO_REALSBOFF_LO	0xf28b00ff
+
+/* TSNPDAV */
+#define	R367TER_TSNPDAV	0xf28c
+#define	F367TER_TSNPD_AVERAGE	0xf28c00ff
+
+/* TSBUFSTATH */
+#define	R367TER_TSBUFSTATH	0xf28d
+#define	F367TER_TSISCR_3BYTES	0xf28d0080
+#define	F367TER_TSISCR_NEWDATA	0xf28d0040
+#define	F367TER_TSISCR_BUFSTAT_HI	0xf28d003f
+
+/* TSBUFSTATM */
+#define	R367TER_TSBUFSTATM	0xf28e
+#define	F367TER_TSISCR_BUFSTAT_MED	0xf28e00ff
+
+/* TSBUFSTATL */
+#define	R367TER_TSBUFSTATL	0xf28f
+#define	F367TER_TSISCR_BUFSTAT_LO	0xf28f00ff
+
+/* TSDEBUGM */
+#define	R367TER_TSDEBUGM	0xf290
+#define	F367TER_TSFIFO_ILLPACKET	0xf2900080
+#define	F367TER_DIL_NOSYNC	0xf2900040
+#define	F367TER_DIL_ISCR	0xf2900020
+#define	F367TER_DILOUT_BSYNCB	0xf2900010
+#define	F367TER_TSFIFO_EMPTYPKT	0xf2900008
+#define	F367TER_TSFIFO_EMPTYRD	0xf2900004
+#define	F367TER_SOFFIFO_STOPM	0xf2900002
+#define	F367TER_SOFFIFO_SPEEDUPM	0xf2900001
+
+/* TSDEBUGL */
+#define	R367TER_TSDEBUGL	0xf291
+#define	F367TER_TSFIFO_PACKLENFAIL	0xf2910080
+#define	F367TER_TSFIFO_SYNCBFAIL	0xf2910040
+#define	F367TER_TSFIFO_VITLIBRE	0xf2910020
+#define	F367TER_TSFIFO_BOOSTSPEEDM	0xf2910010
+#define	F367TER_TSFIFO_UNDERSPEEDM	0xf2910008
+#define	F367TER_TSFIFO_ERROR_EVNT	0xf2910004
+#define	F367TER_TSFIFO_FULL	0xf2910002
+#define	F367TER_TSFIFO_OVERFLOWM	0xf2910001
+
+/* TSDLYSETH */
+#define	R367TER_TSDLYSETH	0xf292
+#define	F367TER_SOFFIFO_OFFSET	0xf29200e0
+#define	F367TER_SOFFIFO_SYMBOFFSET_HI	0xf292001f
+
+/* TSDLYSETM */
+#define	R367TER_TSDLYSETM	0xf293
+#define	F367TER_SOFFIFO_SYMBOFFSET_MED	0xf29300ff
+
+/* TSDLYSETL */
+#define	R367TER_TSDLYSETL	0xf294
+#define	F367TER_SOFFIFO_SYMBOFFSET_LO	0xf29400ff
+
+/* TSOBSCFG */
+#define	R367TER_TSOBSCFG	0xf295
+#define	F367TER_TSFIFO_OBSCFG	0xf29500ff
+
+/* TSOBSM */
+#define	R367TER_TSOBSM	0xf296
+#define	F367TER_TSFIFO_OBSDATA_HI	0xf29600ff
+
+/* TSOBSL */
+#define	R367TER_TSOBSL	0xf297
+#define	F367TER_TSFIFO_OBSDATA_LO	0xf29700ff
+
+/* ERRCTRL1 */
+#define	R367TER_ERRCTRL1	0xf298
+#define	F367TER_ERR_SRC1	0xf29800f0
+#define	F367TER_ERRCTRL1_3	0xf2980008
+#define	F367TER_NUM_EVT1	0xf2980007
+
+/* ERRCNT1H */
+#define	R367TER_ERRCNT1H	0xf299
+#define	F367TER_ERRCNT1_OLDVALUE	0xf2990080
+#define	F367TER_ERR_CNT1	0xf299007f
+
+/* ERRCNT1M */
+#define	R367TER_ERRCNT1M	0xf29a
+#define	F367TER_ERR_CNT1_HI	0xf29a00ff
+
+/* ERRCNT1L */
+#define	R367TER_ERRCNT1L	0xf29b
+#define	F367TER_ERR_CNT1_LO	0xf29b00ff
+
+/* ERRCTRL2 */
+#define	R367TER_ERRCTRL2	0xf29c
+#define	F367TER_ERR_SRC2	0xf29c00f0
+#define	F367TER_ERRCTRL2_3	0xf29c0008
+#define	F367TER_NUM_EVT2	0xf29c0007
+
+/* ERRCNT2H */
+#define	R367TER_ERRCNT2H	0xf29d
+#define	F367TER_ERRCNT2_OLDVALUE	0xf29d0080
+#define	F367TER_ERR_CNT2_HI	0xf29d007f
+
+/* ERRCNT2M */
+#define	R367TER_ERRCNT2M	0xf29e
+#define	F367TER_ERR_CNT2_MED	0xf29e00ff
+
+/* ERRCNT2L */
+#define	R367TER_ERRCNT2L	0xf29f
+#define	F367TER_ERR_CNT2_LO	0xf29f00ff
+
+/* FECSPY */
+#define	R367TER_FECSPY	0xf2a0
+#define	F367TER_SPY_ENABLE	0xf2a00080
+#define	F367TER_NO_SYNCBYTE	0xf2a00040
+#define	F367TER_SERIAL_MODE	0xf2a00020
+#define	F367TER_UNUSUAL_PACKET	0xf2a00010
+#define	F367TER_BERMETER_DATAMODE	0xf2a0000c
+#define	F367TER_BERMETER_LMODE	0xf2a00002
+#define	F367TER_BERMETER_RESET	0xf2a00001
+
+/* FSPYCFG */
+#define	R367TER_FSPYCFG	0xf2a1
+#define	F367TER_FECSPY_INPUT	0xf2a100c0
+#define	F367TER_RST_ON_ERROR	0xf2a10020
+#define	F367TER_ONE_SHOT	0xf2a10010
+#define	F367TER_I2C_MOD	0xf2a1000c
+#define	F367TER_SPY_HYSTERESIS	0xf2a10003
+
+/* FSPYDATA */
+#define	R367TER_FSPYDATA	0xf2a2
+#define	F367TER_SPY_STUFFING	0xf2a20080
+#define	F367TER_NOERROR_PKTJITTER	0xf2a20040
+#define	F367TER_SPY_CNULLPKT	0xf2a20020
+#define	F367TER_SPY_OUTDATA_MODE	0xf2a2001f
+
+/* FSPYOUT */
+#define	R367TER_FSPYOUT	0xf2a3
+#define	F367TER_FSPY_DIRECT	0xf2a30080
+#define	F367TER_FSPYOUT_6	0xf2a30040
+#define	F367TER_SPY_OUTDATA_BUS	0xf2a30038
+#define	F367TER_STUFF_MODE	0xf2a30007
+
+/* FSTATUS */
+#define	R367TER_FSTATUS	0xf2a4
+#define	F367TER_SPY_ENDSIM	0xf2a40080
+#define	F367TER_VALID_SIM	0xf2a40040
+#define	F367TER_FOUND_SIGNAL	0xf2a40020
+#define	F367TER_DSS_SYNCBYTE	0xf2a40010
+#define	F367TER_RESULT_STATE	0xf2a4000f
+
+/* FGOODPACK */
+#define	R367TER_FGOODPACK	0xf2a5
+#define	F367TER_FGOOD_PACKET	0xf2a500ff
+
+/* FPACKCNT */
+#define	R367TER_FPACKCNT	0xf2a6
+#define	F367TER_FPACKET_COUNTER	0xf2a600ff
+
+/* FSPYMISC */
+#define	R367TER_FSPYMISC	0xf2a7
+#define	F367TER_FLABEL_COUNTER	0xf2a700ff
+
+/* FBERCPT4 */
+#define	R367TER_FBERCPT4	0xf2a8
+#define	F367TER_FBERMETER_CPT5	0xf2a800ff
+
+/* FBERCPT3 */
+#define	R367TER_FBERCPT3	0xf2a9
+#define	F367TER_FBERMETER_CPT4	0xf2a900ff
+
+/* FBERCPT2 */
+#define	R367TER_FBERCPT2	0xf2aa
+#define	F367TER_FBERMETER_CPT3	0xf2aa00ff
+
+/* FBERCPT1 */
+#define	R367TER_FBERCPT1	0xf2ab
+#define	F367TER_FBERMETER_CPT2	0xf2ab00ff
+
+/* FBERCPT0 */
+#define	R367TER_FBERCPT0	0xf2ac
+#define	F367TER_FBERMETER_CPT1	0xf2ac00ff
+
+/* FBERERR2 */
+#define	R367TER_FBERERR2	0xf2ad
+#define	F367TER_FBERMETER_ERR_HI	0xf2ad00ff
+
+/* FBERERR1 */
+#define	R367TER_FBERERR1	0xf2ae
+#define	F367TER_FBERMETER_ERR_MED	0xf2ae00ff
+
+/* FBERERR0 */
+#define	R367TER_FBERERR0	0xf2af
+#define	F367TER_FBERMETER_ERR_LO	0xf2af00ff
+
+/* FSTATESM */
+#define	R367TER_FSTATESM	0xf2b0
+#define	F367TER_RSTATE_F	0xf2b00080
+#define	F367TER_RSTATE_E	0xf2b00040
+#define	F367TER_RSTATE_D	0xf2b00020
+#define	F367TER_RSTATE_C	0xf2b00010
+#define	F367TER_RSTATE_B	0xf2b00008
+#define	F367TER_RSTATE_A	0xf2b00004
+#define	F367TER_RSTATE_9	0xf2b00002
+#define	F367TER_RSTATE_8	0xf2b00001
+
+/* FSTATESL */
+#define	R367TER_FSTATESL	0xf2b1
+#define	F367TER_RSTATE_7	0xf2b10080
+#define	F367TER_RSTATE_6	0xf2b10040
+#define	F367TER_RSTATE_5	0xf2b10020
+#define	F367TER_RSTATE_4	0xf2b10010
+#define	F367TER_RSTATE_3	0xf2b10008
+#define	F367TER_RSTATE_2	0xf2b10004
+#define	F367TER_RSTATE_1	0xf2b10002
+#define	F367TER_RSTATE_0	0xf2b10001
+
+/* FSPYBER */
+#define	R367TER_FSPYBER	0xf2b2
+#define	F367TER_FSPYBER_7	0xf2b20080
+#define	F367TER_FSPYOBS_XORREAD	0xf2b20040
+#define	F367TER_FSPYBER_OBSMODE	0xf2b20020
+#define	F367TER_FSPYBER_SYNCBYTE	0xf2b20010
+#define	F367TER_FSPYBER_UNSYNC	0xf2b20008
+#define	F367TER_FSPYBER_CTIME	0xf2b20007
+
+/* FSPYDISTM */
+#define	R367TER_FSPYDISTM	0xf2b3
+#define	F367TER_PKTTIME_DISTANCE_HI	0xf2b300ff
+
+/* FSPYDISTL */
+#define	R367TER_FSPYDISTL	0xf2b4
+#define	F367TER_PKTTIME_DISTANCE_LO	0xf2b400ff
+
+/* FSPYOBS7 */
+#define	R367TER_FSPYOBS7	0xf2b8
+#define	F367TER_FSPYOBS_SPYFAIL	0xf2b80080
+#define	F367TER_FSPYOBS_SPYFAIL1	0xf2b80040
+#define	F367TER_FSPYOBS_ERROR	0xf2b80020
+#define	F367TER_FSPYOBS_STROUT	0xf2b80010
+#define	F367TER_FSPYOBS_RESULTSTATE1	0xf2b8000f
+
+/* FSPYOBS6 */
+#define	R367TER_FSPYOBS6	0xf2b9
+#define	F367TER_FSPYOBS_RESULTSTATe0	0xf2b900f0
+#define	F367TER_FSPYOBS_RESULTSTATEM1	0xf2b9000f
+
+/* FSPYOBS5 */
+#define	R367TER_FSPYOBS5	0xf2ba
+#define	F367TER_FSPYOBS_BYTEOFPACKET1	0xf2ba00ff
+
+/* FSPYOBS4 */
+#define	R367TER_FSPYOBS4	0xf2bb
+#define	F367TER_FSPYOBS_BYTEVALUE1	0xf2bb00ff
+
+/* FSPYOBS3 */
+#define	R367TER_FSPYOBS3	0xf2bc
+#define	F367TER_FSPYOBS_DATA1	0xf2bc00ff
+
+/* FSPYOBS2 */
+#define	R367TER_FSPYOBS2	0xf2bd
+#define	F367TER_FSPYOBS_DATa0	0xf2bd00ff
+
+/* FSPYOBS1 */
+#define	R367TER_FSPYOBS1	0xf2be
+#define	F367TER_FSPYOBS_DATAM1	0xf2be00ff
+
+/* FSPYOBS0 */
+#define	R367TER_FSPYOBS0	0xf2bf
+#define	F367TER_FSPYOBS_DATAM2	0xf2bf00ff
+
+/* SFDEMAP */
+#define	R367TER_SFDEMAP	0xf2c0
+#define	F367TER_SFDEMAP_7	0xf2c00080
+#define	F367TER_SFEC_K_DIVIDER_VIT	0xf2c0007f
+
+/* SFERROR */
+#define	R367TER_SFERROR	0xf2c1
+#define	F367TER_SFEC_REGERR_VIT	0xf2c100ff
+
+/* SFAVSR */
+#define	R367TER_SFAVSR	0xf2c2
+#define	F367TER_SFEC_SUMERRORS	0xf2c20080
+#define	F367TER_SERROR_MAXMODE	0xf2c20040
+#define	F367TER_SN_SFEC	0xf2c20030
+#define	F367TER_KDIV_MODE_SFEC	0xf2c2000c
+#define	F367TER_SFAVSR_1	0xf2c20002
+#define	F367TER_SFAVSR_0	0xf2c20001
+
+/* SFECSTATUS */
+#define	R367TER_SFECSTATUS	0xf2c3
+#define	F367TER_SFEC_ON	0xf2c30080
+#define	F367TER_SFSTATUS_6	0xf2c30040
+#define	F367TER_SFSTATUS_5	0xf2c30020
+#define	F367TER_SFSTATUS_4	0xf2c30010
+#define	F367TER_LOCKEDSFEC	0xf2c30008
+#define	F367TER_SFEC_DELOCK	0xf2c30004
+#define	F367TER_SFEC_DEMODSEL1	0xf2c30002
+#define	F367TER_SFEC_OVFON	0xf2c30001
+
+/* SFKDIV12 */
+#define	R367TER_SFKDIV12	0xf2c4
+#define	F367TER_SFECKDIV12_MAN	0xf2c40080
+#define	F367TER_SFEC_K_DIVIDER_12	0xf2c4007f
+
+/* SFKDIV23 */
+#define	R367TER_SFKDIV23	0xf2c5
+#define	F367TER_SFECKDIV23_MAN	0xf2c50080
+#define	F367TER_SFEC_K_DIVIDER_23	0xf2c5007f
+
+/* SFKDIV34 */
+#define	R367TER_SFKDIV34	0xf2c6
+#define	F367TER_SFECKDIV34_MAN	0xf2c60080
+#define	F367TER_SFEC_K_DIVIDER_34	0xf2c6007f
+
+/* SFKDIV56 */
+#define	R367TER_SFKDIV56	0xf2c7
+#define	F367TER_SFECKDIV56_MAN	0xf2c70080
+#define	F367TER_SFEC_K_DIVIDER_56	0xf2c7007f
+
+/* SFKDIV67 */
+#define	R367TER_SFKDIV67	0xf2c8
+#define	F367TER_SFECKDIV67_MAN	0xf2c80080
+#define	F367TER_SFEC_K_DIVIDER_67	0xf2c8007f
+
+/* SFKDIV78 */
+#define	R367TER_SFKDIV78	0xf2c9
+#define	F367TER_SFECKDIV78_MAN	0xf2c90080
+#define	F367TER_SFEC_K_DIVIDER_78	0xf2c9007f
+
+/* SFDILSTKM */
+#define	R367TER_SFDILSTKM	0xf2ca
+#define	F367TER_SFEC_PACKCPT	0xf2ca00e0
+#define	F367TER_SFEC_DILSTK_HI	0xf2ca001f
+
+/* SFDILSTKL */
+#define	R367TER_SFDILSTKL	0xf2cb
+#define	F367TER_SFEC_DILSTK_LO	0xf2cb00ff
+
+/* SFSTATUS */
+#define	R367TER_SFSTATUS	0xf2cc
+#define	F367TER_SFEC_LINEOK	0xf2cc0080
+#define	F367TER_SFEC_ERROR	0xf2cc0040
+#define	F367TER_SFEC_DATA7	0xf2cc0020
+#define	F367TER_SFEC_OVERFLOW	0xf2cc0010
+#define	F367TER_SFEC_DEMODSEL2	0xf2cc0008
+#define	F367TER_SFEC_NOSYNC	0xf2cc0004
+#define	F367TER_SFEC_UNREGULA	0xf2cc0002
+#define	F367TER_SFEC_READY	0xf2cc0001
+
+/* SFDLYH */
+#define	R367TER_SFDLYH	0xf2cd
+#define	F367TER_SFEC_TSTIMEVALID	0xf2cd0080
+#define	F367TER_SFEC_SPEEDUP	0xf2cd0040
+#define	F367TER_SFEC_STOP	0xf2cd0020
+#define	F367TER_SFEC_REGULATED	0xf2cd0010
+#define	F367TER_SFEC_REALSYMBOFFSET	0xf2cd000f
+
+/* SFDLYM */
+#define	R367TER_SFDLYM	0xf2ce
+#define	F367TER_SFEC_REALSYMBOFFSET_HI	0xf2ce00ff
+
+/* SFDLYL */
+#define	R367TER_SFDLYL	0xf2cf
+#define	F367TER_SFEC_REALSYMBOFFSET_LO	0xf2cf00ff
+
+/* SFDLYSETH */
+#define	R367TER_SFDLYSETH	0xf2d0
+#define	F367TER_SFEC_OFFSET	0xf2d000e0
+#define	F367TER_SFECDLYSETH_4	0xf2d00010
+#define	F367TER_RST_SFEC	0xf2d00008
+#define	F367TER_SFECDLYSETH_2	0xf2d00004
+#define	F367TER_SFEC_DISABLE	0xf2d00002
+#define	F367TER_SFEC_UNREGUL	0xf2d00001
+
+/* SFDLYSETM */
+#define	R367TER_SFDLYSETM	0xf2d1
+#define	F367TER_SFECDLYSETM_7	0xf2d10080
+#define	F367TER_SFEC_SYMBOFFSET_HI	0xf2d1007f
+
+/* SFDLYSETL */
+#define	R367TER_SFDLYSETL	0xf2d2
+#define	F367TER_SFEC_SYMBOFFSET_LO	0xf2d200ff
+
+/* SFOBSCFG */
+#define	R367TER_SFOBSCFG	0xf2d3
+#define	F367TER_SFEC_OBSCFG	0xf2d300ff
+
+/* SFOBSM */
+#define	R367TER_SFOBSM	0xf2d4
+#define	F367TER_SFEC_OBSDATA_HI	0xf2d400ff
+
+/* SFOBSL */
+#define	R367TER_SFOBSL	0xf2d5
+#define	F367TER_SFEC_OBSDATA_LO	0xf2d500ff
+
+/* SFECINFO */
+#define	R367TER_SFECINFO	0xf2d6
+#define	F367TER_SFECINFO_7	0xf2d60080
+#define	F367TER_SFEC_SYNCDLSB	0xf2d60070
+#define	F367TER_SFCE_S1cPHASE	0xf2d6000f
+
+/* SFERRCTRL */
+#define	R367TER_SFERRCTRL	0xf2d8
+#define	F367TER_SFEC_ERR_SOURCE	0xf2d800f0
+#define	F367TER_SFERRCTRL_3	0xf2d80008
+#define	F367TER_SFEC_NUM_EVENT	0xf2d80007
+
+/* SFERRCNTH */
+#define	R367TER_SFERRCNTH	0xf2d9
+#define	F367TER_SFERRC_OLDVALUE	0xf2d90080
+#define	F367TER_SFEC_ERR_CNT	0xf2d9007f
+
+/* SFERRCNTM */
+#define	R367TER_SFERRCNTM	0xf2da
+#define	F367TER_SFEC_ERR_CNT_HI	0xf2da00ff
+
+/* SFERRCNTL */
+#define	R367TER_SFERRCNTL	0xf2db
+#define	F367TER_SFEC_ERR_CNT_LO	0xf2db00ff
+
+/* SYMBRATEM */
+#define	R367TER_SYMBRATEM	0xf2e0
+#define	F367TER_DEFGEN_SYMBRATE_HI	0xf2e000ff
+
+/* SYMBRATEL */
+#define	R367TER_SYMBRATEL	0xf2e1
+#define	F367TER_DEFGEN_SYMBRATE_LO	0xf2e100ff
+
+/* SYMBSTATUS */
+#define	R367TER_SYMBSTATUS	0xf2e2
+#define	F367TER_SYMBDLINE2_OFF	0xf2e20080
+#define	F367TER_SDDL_REINIT1	0xf2e20040
+#define	F367TER_SDD_REINIT1	0xf2e20020
+#define	F367TER_TOKENID_ERROR	0xf2e20010
+#define	F367TER_SYMBRATE_OVERFLOW	0xf2e20008
+#define	F367TER_SYMBRATE_UNDERFLOW	0xf2e20004
+#define	F367TER_TOKENID_RSTEVENT	0xf2e20002
+#define	F367TER_TOKENID_RESET1	0xf2e20001
+
+/* SYMBCFG */
+#define	R367TER_SYMBCFG	0xf2e3
+#define	F367TER_SYMBCFG_7	0xf2e30080
+#define	F367TER_SYMBCFG_6	0xf2e30040
+#define	F367TER_SYMBCFG_5	0xf2e30020
+#define	F367TER_SYMBCFG_4	0xf2e30010
+#define	F367TER_SYMRATE_FSPEED	0xf2e3000c
+#define	F367TER_SYMRATE_SSPEED	0xf2e30003
+
+/* SYMBFIFOM */
+#define	R367TER_SYMBFIFOM	0xf2e4
+#define	F367TER_SYMBFIFOM_7	0xf2e40080
+#define	F367TER_SYMBFIFOM_6	0xf2e40040
+#define	F367TER_DEFGEN_SYMFIFO_HI	0xf2e4003f
+
+/* SYMBFIFOL */
+#define	R367TER_SYMBFIFOL	0xf2e5
+#define	F367TER_DEFGEN_SYMFIFO_LO	0xf2e500ff
+
+/* SYMBOFFSM */
+#define	R367TER_SYMBOFFSM	0xf2e6
+#define	F367TER_TOKENID_RESET2	0xf2e60080
+#define	F367TER_SDDL_REINIT2	0xf2e60040
+#define	F367TER_SDD_REINIT2	0xf2e60020
+#define	F367TER_SYMBOFFSM_4	0xf2e60010
+#define	F367TER_SYMBOFFSM_3	0xf2e60008
+#define	F367TER_DEFGEN_SYMBOFFSET_HI	0xf2e60007
+
+/* SYMBOFFSL */
+#define	R367TER_SYMBOFFSL	0xf2e7
+#define	F367TER_DEFGEN_SYMBOFFSET_LO	0xf2e700ff
+
+/* DEBUG_LT4 */
+#define	R367TER_DEBUG_LT4	0xf400
+#define	F367TER_F_DEBUG_LT4	0xf40000ff
+
+/* DEBUG_LT5 */
+#define	R367TER_DEBUG_LT5	0xf401
+#define	F367TER_F_DEBUG_LT5	0xf40100ff
+
+/* DEBUG_LT6 */
+#define	R367TER_DEBUG_LT6	0xf402
+#define	F367TER_F_DEBUG_LT6	0xf40200ff
+
+/* DEBUG_LT7 */
+#define	R367TER_DEBUG_LT7	0xf403
+#define	F367TER_F_DEBUG_LT7	0xf40300ff
+
+/* DEBUG_LT8 */
+#define	R367TER_DEBUG_LT8	0xf404
+#define	F367TER_F_DEBUG_LT8	0xf40400ff
+
+/* DEBUG_LT9 */
+#define	R367TER_DEBUG_LT9	0xf405
+#define	F367TER_F_DEBUG_LT9	0xf40500ff
+
+#define STV0367TER_NBREGS	445
+
+/* ID */
+#define	R367CAB_ID	0xf000
+#define	F367CAB_IDENTIFICATIONREGISTER	0xf00000ff
+
+/* I2CRPT */
+#define	R367CAB_I2CRPT	0xf001
+#define	F367CAB_I2CT_ON	0xf0010080
+#define	F367CAB_ENARPT_LEVEL	0xf0010070
+#define	F367CAB_SCLT_DELAY	0xf0010008
+#define	F367CAB_SCLT_NOD	0xf0010004
+#define	F367CAB_STOP_ENABLE	0xf0010002
+#define	F367CAB_SDAT_NOD	0xf0010001
+
+/* TOPCTRL */
+#define	R367CAB_TOPCTRL	0xf002
+#define	F367CAB_STDBY	0xf0020080
+#define	F367CAB_STDBY_CORE	0xf0020020
+#define	F367CAB_QAM_COFDM	0xf0020010
+#define	F367CAB_TS_DIS	0xf0020008
+#define	F367CAB_DIR_CLK_216	0xf0020004
+
+/* IOCFG0 */
+#define	R367CAB_IOCFG0	0xf003
+#define	F367CAB_OP0_SD	0xf0030080
+#define	F367CAB_OP0_VAL	0xf0030040
+#define	F367CAB_OP0_OD	0xf0030020
+#define	F367CAB_OP0_INV	0xf0030010
+#define	F367CAB_OP0_DACVALUE_HI	0xf003000f
+
+/* DAc0R */
+#define	R367CAB_DAC0R	0xf004
+#define	F367CAB_OP0_DACVALUE_LO	0xf00400ff
+
+/* IOCFG1 */
+#define	R367CAB_IOCFG1	0xf005
+#define	F367CAB_IP0	0xf0050040
+#define	F367CAB_OP1_OD	0xf0050020
+#define	F367CAB_OP1_INV	0xf0050010
+#define	F367CAB_OP1_DACVALUE_HI	0xf005000f
+
+/* DAC1R */
+#define	R367CAB_DAC1R	0xf006
+#define	F367CAB_OP1_DACVALUE_LO	0xf00600ff
+
+/* IOCFG2 */
+#define	R367CAB_IOCFG2	0xf007
+#define	F367CAB_OP2_LOCK_CONF	0xf00700e0
+#define	F367CAB_OP2_OD	0xf0070010
+#define	F367CAB_OP2_VAL	0xf0070008
+#define	F367CAB_OP1_LOCK_CONF	0xf0070007
+
+/* SDFR */
+#define	R367CAB_SDFR	0xf008
+#define	F367CAB_OP0_FREQ	0xf00800f0
+#define	F367CAB_OP1_FREQ	0xf008000f
+
+/* AUX_CLK */
+#define	R367CAB_AUX_CLK	0xf00a
+#define	F367CAB_AUXFEC_CTL	0xf00a00c0
+#define	F367CAB_DIS_CKX4	0xf00a0020
+#define	F367CAB_CKSEL	0xf00a0018
+#define	F367CAB_CKDIV_PROG	0xf00a0006
+#define	F367CAB_AUXCLK_ENA	0xf00a0001
+
+/* FREESYS1 */
+#define	R367CAB_FREESYS1	0xf00b
+#define	F367CAB_FREESYS_1	0xf00b00ff
+
+/* FREESYS2 */
+#define	R367CAB_FREESYS2	0xf00c
+#define	F367CAB_FREESYS_2	0xf00c00ff
+
+/* FREESYS3 */
+#define	R367CAB_FREESYS3	0xf00d
+#define	F367CAB_FREESYS_3	0xf00d00ff
+
+/* GPIO_CFG */
+#define	R367CAB_GPIO_CFG	0xf00e
+#define	F367CAB_GPIO7_OD	0xf00e0080
+#define	F367CAB_GPIO7_CFG	0xf00e0040
+#define	F367CAB_GPIO6_OD	0xf00e0020
+#define	F367CAB_GPIO6_CFG	0xf00e0010
+#define	F367CAB_GPIO5_OD	0xf00e0008
+#define	F367CAB_GPIO5_CFG	0xf00e0004
+#define	F367CAB_GPIO4_OD	0xf00e0002
+#define	F367CAB_GPIO4_CFG	0xf00e0001
+
+/* GPIO_CMD */
+#define	R367CAB_GPIO_CMD	0xf00f
+#define	F367CAB_GPIO7_VAL	0xf00f0008
+#define	F367CAB_GPIO6_VAL	0xf00f0004
+#define	F367CAB_GPIO5_VAL	0xf00f0002
+#define	F367CAB_GPIO4_VAL	0xf00f0001
+
+/* TSTRES */
+#define	R367CAB_TSTRES	0xf0c0
+#define	F367CAB_FRES_DISPLAY	0xf0c00080
+#define	F367CAB_FRES_FIFO_AD	0xf0c00020
+#define	F367CAB_FRESRS	0xf0c00010
+#define	F367CAB_FRESACS	0xf0c00008
+#define	F367CAB_FRESFEC	0xf0c00004
+#define	F367CAB_FRES_PRIF	0xf0c00002
+#define	F367CAB_FRESCORE	0xf0c00001
+
+/* ANACTRL */
+#define	R367CAB_ANACTRL	0xf0c1
+#define	F367CAB_BYPASS_XTAL	0xf0c10040
+#define	F367CAB_BYPASS_PLLXN	0xf0c1000c
+#define	F367CAB_DIS_PAD_OSC	0xf0c10002
+#define	F367CAB_STDBY_PLLXN	0xf0c10001
+
+/* TSTBUS */
+#define	R367CAB_TSTBUS	0xf0c2
+#define	F367CAB_TS_BYTE_CLK_INV	0xf0c20080
+#define	F367CAB_CFG_IP	0xf0c20070
+#define	F367CAB_CFG_TST	0xf0c2000f
+
+/* RF_AGC1 */
+#define	R367CAB_RF_AGC1	0xf0d4
+#define	F367CAB_RF_AGC1_LEVEL_HI	0xf0d400ff
+
+/* RF_AGC2 */
+#define	R367CAB_RF_AGC2	0xf0d5
+#define	F367CAB_REF_ADGP	0xf0d50080
+#define	F367CAB_STDBY_ADCGP	0xf0d50020
+#define	F367CAB_RF_AGC1_LEVEL_LO	0xf0d50003
+
+/* ANADIGCTRL */
+#define	R367CAB_ANADIGCTRL	0xf0d7
+#define	F367CAB_SEL_CLKDEM	0xf0d70020
+#define	F367CAB_EN_BUFFER_Q	0xf0d70010
+#define	F367CAB_EN_BUFFER_I	0xf0d70008
+#define	F367CAB_ADC_RIS_EGDE	0xf0d70004
+#define	F367CAB_SGN_ADC	0xf0d70002
+#define	F367CAB_SEL_AD12_SYNC	0xf0d70001
+
+/* PLLMDIV */
+#define	R367CAB_PLLMDIV	0xf0d8
+#define	F367CAB_PLL_MDIV	0xf0d800ff
+
+/* PLLNDIV */
+#define	R367CAB_PLLNDIV	0xf0d9
+#define	F367CAB_PLL_NDIV	0xf0d900ff
+
+/* PLLSETUP */
+#define	R367CAB_PLLSETUP	0xf0da
+#define	F367CAB_PLL_PDIV	0xf0da0070
+#define	F367CAB_PLL_KDIV	0xf0da000f
+
+/* DUAL_AD12 */
+#define	R367CAB_DUAL_AD12	0xf0db
+#define	F367CAB_FS20M	0xf0db0020
+#define	F367CAB_FS50M	0xf0db0010
+#define	F367CAB_INMODe0	0xf0db0008
+#define	F367CAB_POFFQ	0xf0db0004
+#define	F367CAB_POFFI	0xf0db0002
+#define	F367CAB_INMODE1	0xf0db0001
+
+/* TSTBIST */
+#define	R367CAB_TSTBIST	0xf0dc
+#define	F367CAB_TST_BYP_CLK	0xf0dc0080
+#define	F367CAB_TST_GCLKENA_STD	0xf0dc0040
+#define	F367CAB_TST_GCLKENA	0xf0dc0020
+#define	F367CAB_TST_MEMBIST	0xf0dc001f
+
+/* CTRL_1 */
+#define	R367CAB_CTRL_1	0xf402
+#define	F367CAB_SOFT_RST	0xf4020080
+#define	F367CAB_EQU_RST	0xf4020008
+#define	F367CAB_CRL_RST	0xf4020004
+#define	F367CAB_TRL_RST	0xf4020002
+#define	F367CAB_AGC_RST	0xf4020001
+
+/* CTRL_2 */
+#define	R367CAB_CTRL_2	0xf403
+#define	F367CAB_DEINT_RST	0xf4030008
+#define	F367CAB_RS_RST	0xf4030004
+
+/* IT_STATUS1 */
+#define	R367CAB_IT_STATUS1	0xf408
+#define	F367CAB_SWEEP_OUT	0xf4080080
+#define	F367CAB_FSM_CRL	0xf4080040
+#define	F367CAB_CRL_LOCK	0xf4080020
+#define	F367CAB_MFSM	0xf4080010
+#define	F367CAB_TRL_LOCK	0xf4080008
+#define	F367CAB_TRL_AGC_LIMIT	0xf4080004
+#define	F367CAB_ADJ_AGC_LOCK	0xf4080002
+#define	F367CAB_AGC_QAM_LOCK	0xf4080001
+
+/* IT_STATUS2 */
+#define	R367CAB_IT_STATUS2	0xf409
+#define	F367CAB_TSMF_CNT	0xf4090080
+#define	F367CAB_TSMF_EOF	0xf4090040
+#define	F367CAB_TSMF_RDY	0xf4090020
+#define	F367CAB_FEC_NOCORR	0xf4090010
+#define	F367CAB_SYNCSTATE	0xf4090008
+#define	F367CAB_DEINT_LOCK	0xf4090004
+#define	F367CAB_FADDING_FRZ	0xf4090002
+#define	F367CAB_TAPMON_ALARM	0xf4090001
+
+/* IT_EN1 */
+#define	R367CAB_IT_EN1	0xf40a
+#define	F367CAB_SWEEP_OUTE	0xf40a0080
+#define	F367CAB_FSM_CRLE	0xf40a0040
+#define	F367CAB_CRL_LOCKE	0xf40a0020
+#define	F367CAB_MFSME	0xf40a0010
+#define	F367CAB_TRL_LOCKE	0xf40a0008
+#define	F367CAB_TRL_AGC_LIMITE	0xf40a0004
+#define	F367CAB_ADJ_AGC_LOCKE	0xf40a0002
+#define	F367CAB_AGC_LOCKE	0xf40a0001
+
+/* IT_EN2 */
+#define	R367CAB_IT_EN2	0xf40b
+#define	F367CAB_TSMF_CNTE	0xf40b0080
+#define	F367CAB_TSMF_EOFE	0xf40b0040
+#define	F367CAB_TSMF_RDYE	0xf40b0020
+#define	F367CAB_FEC_NOCORRE	0xf40b0010
+#define	F367CAB_SYNCSTATEE	0xf40b0008
+#define	F367CAB_DEINT_LOCKE	0xf40b0004
+#define	F367CAB_FADDING_FRZE	0xf40b0002
+#define	F367CAB_TAPMON_ALARME	0xf40b0001
+
+/* CTRL_STATUS */
+#define	R367CAB_CTRL_STATUS	0xf40c
+#define	F367CAB_QAMFEC_LOCK	0xf40c0004
+#define	F367CAB_TSMF_LOCK	0xf40c0002
+#define	F367CAB_TSMF_ERROR	0xf40c0001
+
+/* TEST_CTL */
+#define	R367CAB_TEST_CTL	0xf40f
+#define	F367CAB_TST_BLK_SEL	0xf40f0060
+#define	F367CAB_TST_BUS_SEL	0xf40f001f
+
+/* AGC_CTL */
+#define	R367CAB_AGC_CTL	0xf410
+#define	F367CAB_AGC_LCK_TH	0xf41000f0
+#define	F367CAB_AGC_ACCUMRSTSEL	0xf4100007
+
+/* AGC_IF_CFG */
+#define	R367CAB_AGC_IF_CFG	0xf411
+#define	F367CAB_AGC_IF_BWSEL	0xf41100f0
+#define	F367CAB_AGC_IF_FREEZE	0xf4110002
+
+/* AGC_RF_CFG */
+#define	R367CAB_AGC_RF_CFG	0xf412
+#define	F367CAB_AGC_RF_BWSEL	0xf4120070
+#define	F367CAB_AGC_RF_FREEZE	0xf4120002
+
+/* AGC_PWM_CFG */
+#define	R367CAB_AGC_PWM_CFG	0xf413
+#define	F367CAB_AGC_RF_PWM_TST	0xf4130080
+#define	F367CAB_AGC_RF_PWM_INV	0xf4130040
+#define	F367CAB_AGC_IF_PWM_TST	0xf4130008
+#define	F367CAB_AGC_IF_PWM_INV	0xf4130004
+#define	F367CAB_AGC_PWM_CLKDIV	0xf4130003
+
+/* AGC_PWR_REF_L */
+#define	R367CAB_AGC_PWR_REF_L	0xf414
+#define	F367CAB_AGC_PWRREF_LO	0xf41400ff
+
+/* AGC_PWR_REF_H */
+#define	R367CAB_AGC_PWR_REF_H	0xf415
+#define	F367CAB_AGC_PWRREF_HI	0xf4150003
+
+/* AGC_RF_TH_L */
+#define	R367CAB_AGC_RF_TH_L	0xf416
+#define	F367CAB_AGC_RF_TH_LO	0xf41600ff
+
+/* AGC_RF_TH_H */
+#define	R367CAB_AGC_RF_TH_H	0xf417
+#define	F367CAB_AGC_RF_TH_HI	0xf417000f
+
+/* AGC_IF_LTH_L */
+#define	R367CAB_AGC_IF_LTH_L	0xf418
+#define	F367CAB_AGC_IF_THLO_LO	0xf41800ff
+
+/* AGC_IF_LTH_H */
+#define	R367CAB_AGC_IF_LTH_H	0xf419
+#define	F367CAB_AGC_IF_THLO_HI	0xf419000f
+
+/* AGC_IF_HTH_L */
+#define	R367CAB_AGC_IF_HTH_L	0xf41a
+#define	F367CAB_AGC_IF_THHI_LO	0xf41a00ff
+
+/* AGC_IF_HTH_H */
+#define	R367CAB_AGC_IF_HTH_H	0xf41b
+#define	F367CAB_AGC_IF_THHI_HI	0xf41b000f
+
+/* AGC_PWR_RD_L */
+#define	R367CAB_AGC_PWR_RD_L	0xf41c
+#define	F367CAB_AGC_PWR_WORD_LO	0xf41c00ff
+
+/* AGC_PWR_RD_M */
+#define	R367CAB_AGC_PWR_RD_M	0xf41d
+#define	F367CAB_AGC_PWR_WORD_ME	0xf41d00ff
+
+/* AGC_PWR_RD_H */
+#define	R367CAB_AGC_PWR_RD_H	0xf41e
+#define	F367CAB_AGC_PWR_WORD_HI	0xf41e0003
+
+/* AGC_PWM_IFCMD_L */
+#define	R367CAB_AGC_PWM_IFCMD_L	0xf420
+#define	F367CAB_AGC_IF_PWMCMD_LO	0xf42000ff
+
+/* AGC_PWM_IFCMD_H */
+#define	R367CAB_AGC_PWM_IFCMD_H	0xf421
+#define	F367CAB_AGC_IF_PWMCMD_HI	0xf421000f
+
+/* AGC_PWM_RFCMD_L */
+#define	R367CAB_AGC_PWM_RFCMD_L	0xf422
+#define	F367CAB_AGC_RF_PWMCMD_LO	0xf42200ff
+
+/* AGC_PWM_RFCMD_H */
+#define	R367CAB_AGC_PWM_RFCMD_H	0xf423
+#define	F367CAB_AGC_RF_PWMCMD_HI	0xf423000f
+
+/* IQDEM_CFG */
+#define	R367CAB_IQDEM_CFG	0xf424
+#define	F367CAB_IQDEM_CLK_SEL	0xf4240004
+#define	F367CAB_IQDEM_INVIQ	0xf4240002
+#define	F367CAB_IQDEM_A2dTYPE	0xf4240001
+
+/* MIX_NCO_LL */
+#define	R367CAB_MIX_NCO_LL	0xf425
+#define	F367CAB_MIX_NCO_INC_LL	0xf42500ff
+
+/* MIX_NCO_HL */
+#define	R367CAB_MIX_NCO_HL	0xf426
+#define	F367CAB_MIX_NCO_INC_HL	0xf42600ff
+
+/* MIX_NCO_HH */
+#define	R367CAB_MIX_NCO_HH	0xf427
+#define	F367CAB_MIX_NCO_INVCNST	0xf4270080
+#define	F367CAB_MIX_NCO_INC_HH	0xf427007f
+
+/* SRC_NCO_LL */
+#define	R367CAB_SRC_NCO_LL	0xf428
+#define	F367CAB_SRC_NCO_INC_LL	0xf42800ff
+
+/* SRC_NCO_LH */
+#define	R367CAB_SRC_NCO_LH	0xf429
+#define	F367CAB_SRC_NCO_INC_LH	0xf42900ff
+
+/* SRC_NCO_HL */
+#define	R367CAB_SRC_NCO_HL	0xf42a
+#define	F367CAB_SRC_NCO_INC_HL	0xf42a00ff
+
+/* SRC_NCO_HH */
+#define	R367CAB_SRC_NCO_HH	0xf42b
+#define	F367CAB_SRC_NCO_INC_HH	0xf42b007f
+
+/* IQDEM_GAIN_SRC_L */
+#define	R367CAB_IQDEM_GAIN_SRC_L	0xf42c
+#define	F367CAB_GAIN_SRC_LO	0xf42c00ff
+
+/* IQDEM_GAIN_SRC_H */
+#define	R367CAB_IQDEM_GAIN_SRC_H	0xf42d
+#define	F367CAB_GAIN_SRC_HI	0xf42d0003
+
+/* IQDEM_DCRM_CFG_LL */
+#define	R367CAB_IQDEM_DCRM_CFG_LL	0xf430
+#define	F367CAB_DCRM0_DCIN_L	0xf43000ff
+
+/* IQDEM_DCRM_CFG_LH */
+#define	R367CAB_IQDEM_DCRM_CFG_LH	0xf431
+#define	F367CAB_DCRM1_I_DCIN_L	0xf43100fc
+#define	F367CAB_DCRM0_DCIN_H	0xf4310003
+
+/* IQDEM_DCRM_CFG_HL */
+#define	R367CAB_IQDEM_DCRM_CFG_HL	0xf432
+#define	F367CAB_DCRM1_Q_DCIN_L	0xf43200f0
+#define	F367CAB_DCRM1_I_DCIN_H	0xf432000f
+
+/* IQDEM_DCRM_CFG_HH */
+#define	R367CAB_IQDEM_DCRM_CFG_HH	0xf433
+#define	F367CAB_DCRM1_FRZ	0xf4330080
+#define	F367CAB_DCRM0_FRZ	0xf4330040
+#define	F367CAB_DCRM1_Q_DCIN_H	0xf433003f
+
+/* IQDEM_ADJ_COEFf0 */
+#define	R367CAB_IQDEM_ADJ_COEFF0	0xf434
+#define	F367CAB_ADJIIR_COEFF10_L	0xf43400ff
+
+/* IQDEM_ADJ_COEFF1 */
+#define	R367CAB_IQDEM_ADJ_COEFF1	0xf435
+#define	F367CAB_ADJIIR_COEFF11_L	0xf43500fc
+#define	F367CAB_ADJIIR_COEFF10_H	0xf4350003
+
+/* IQDEM_ADJ_COEFF2 */
+#define	R367CAB_IQDEM_ADJ_COEFF2	0xf436
+#define	F367CAB_ADJIIR_COEFF12_L	0xf43600f0
+#define	F367CAB_ADJIIR_COEFF11_H	0xf436000f
+
+/* IQDEM_ADJ_COEFF3 */
+#define	R367CAB_IQDEM_ADJ_COEFF3	0xf437
+#define	F367CAB_ADJIIR_COEFF20_L	0xf43700c0
+#define	F367CAB_ADJIIR_COEFF12_H	0xf437003f
+
+/* IQDEM_ADJ_COEFF4 */
+#define	R367CAB_IQDEM_ADJ_COEFF4	0xf438
+#define	F367CAB_ADJIIR_COEFF20_H	0xf43800ff
+
+/* IQDEM_ADJ_COEFF5 */
+#define	R367CAB_IQDEM_ADJ_COEFF5	0xf439
+#define	F367CAB_ADJIIR_COEFF21_L	0xf43900ff
+
+/* IQDEM_ADJ_COEFF6 */
+#define	R367CAB_IQDEM_ADJ_COEFF6	0xf43a
+#define	F367CAB_ADJIIR_COEFF22_L	0xf43a00fc
+#define	F367CAB_ADJIIR_COEFF21_H	0xf43a0003
+
+/* IQDEM_ADJ_COEFF7 */
+#define	R367CAB_IQDEM_ADJ_COEFF7	0xf43b
+#define	F367CAB_ADJIIR_COEFF22_H	0xf43b000f
+
+/* IQDEM_ADJ_EN */
+#define	R367CAB_IQDEM_ADJ_EN	0xf43c
+#define	F367CAB_ALLPASSFILT_EN	0xf43c0008
+#define	F367CAB_ADJ_AGC_EN	0xf43c0004
+#define	F367CAB_ADJ_COEFF_FRZ	0xf43c0002
+#define	F367CAB_ADJ_EN	0xf43c0001
+
+/* IQDEM_ADJ_AGC_REF */
+#define	R367CAB_IQDEM_ADJ_AGC_REF	0xf43d
+#define	F367CAB_ADJ_AGC_REF	0xf43d00ff
+
+/* ALLPASSFILT1 */
+#define	R367CAB_ALLPASSFILT1	0xf440
+#define	F367CAB_ALLPASSFILT_COEFF1_LO	0xf44000ff
+
+/* ALLPASSFILT2 */
+#define	R367CAB_ALLPASSFILT2	0xf441
+#define	F367CAB_ALLPASSFILT_COEFF1_ME	0xf44100ff
+
+/* ALLPASSFILT3 */
+#define	R367CAB_ALLPASSFILT3	0xf442
+#define	F367CAB_ALLPASSFILT_COEFF2_LO	0xf44200c0
+#define	F367CAB_ALLPASSFILT_COEFF1_HI	0xf442003f
+
+/* ALLPASSFILT4 */
+#define	R367CAB_ALLPASSFILT4	0xf443
+#define	F367CAB_ALLPASSFILT_COEFF2_MEL	0xf44300ff
+
+/* ALLPASSFILT5 */
+#define	R367CAB_ALLPASSFILT5	0xf444
+#define	F367CAB_ALLPASSFILT_COEFF2_MEH	0xf44400ff
+
+/* ALLPASSFILT6 */
+#define	R367CAB_ALLPASSFILT6	0xf445
+#define	F367CAB_ALLPASSFILT_COEFF3_LO	0xf44500f0
+#define	F367CAB_ALLPASSFILT_COEFF2_HI	0xf445000f
+
+/* ALLPASSFILT7 */
+#define	R367CAB_ALLPASSFILT7	0xf446
+#define	F367CAB_ALLPASSFILT_COEFF3_MEL	0xf44600ff
+
+/* ALLPASSFILT8 */
+#define	R367CAB_ALLPASSFILT8	0xf447
+#define	F367CAB_ALLPASSFILT_COEFF3_MEH	0xf44700ff
+
+/* ALLPASSFILT9 */
+#define	R367CAB_ALLPASSFILT9	0xf448
+#define	F367CAB_ALLPASSFILT_COEFF4_LO	0xf44800fc
+#define	F367CAB_ALLPASSFILT_COEFF3_HI	0xf4480003
+
+/* ALLPASSFILT10 */
+#define	R367CAB_ALLPASSFILT10	0xf449
+#define	F367CAB_ALLPASSFILT_COEFF4_ME	0xf44900ff
+
+/* ALLPASSFILT11 */
+#define	R367CAB_ALLPASSFILT11	0xf44a
+#define	F367CAB_ALLPASSFILT_COEFF4_HI	0xf44a00ff
+
+/* TRL_AGC_CFG */
+#define	R367CAB_TRL_AGC_CFG	0xf450
+#define	F367CAB_TRL_AGC_FREEZE	0xf4500080
+#define	F367CAB_TRL_AGC_REF	0xf450007f
+
+/* TRL_LPF_CFG */
+#define	R367CAB_TRL_LPF_CFG	0xf454
+#define	F367CAB_NYQPOINT_INV	0xf4540040
+#define	F367CAB_TRL_SHIFT	0xf4540030
+#define	F367CAB_NYQ_COEFF_SEL	0xf454000c
+#define	F367CAB_TRL_LPF_FREEZE	0xf4540002
+#define	F367CAB_TRL_LPF_CRT	0xf4540001
+
+/* TRL_LPF_ACQ_GAIN */
+#define	R367CAB_TRL_LPF_ACQ_GAIN	0xf455
+#define	F367CAB_TRL_GDIR_ACQ	0xf4550070
+#define	F367CAB_TRL_GINT_ACQ	0xf4550007
+
+/* TRL_LPF_TRK_GAIN */
+#define	R367CAB_TRL_LPF_TRK_GAIN	0xf456
+#define	F367CAB_TRL_GDIR_TRK	0xf4560070
+#define	F367CAB_TRL_GINT_TRK	0xf4560007
+
+/* TRL_LPF_OUT_GAIN */
+#define	R367CAB_TRL_LPF_OUT_GAIN	0xf457
+#define	F367CAB_TRL_GAIN_OUT	0xf4570007
+
+/* TRL_LOCKDET_LTH */
+#define	R367CAB_TRL_LOCKDET_LTH	0xf458
+#define	F367CAB_TRL_LCK_THLO	0xf4580007
+
+/* TRL_LOCKDET_HTH */
+#define	R367CAB_TRL_LOCKDET_HTH	0xf459
+#define	F367CAB_TRL_LCK_THHI	0xf45900ff
+
+/* TRL_LOCKDET_TRGVAL */
+#define	R367CAB_TRL_LOCKDET_TRGVAL	0xf45a
+#define	F367CAB_TRL_LCK_TRG	0xf45a00ff
+
+/* IQ_QAM */
+#define	R367CAB_IQ_QAM	0xf45c
+#define	F367CAB_IQ_INPUT	0xf45c0008
+#define	F367CAB_DETECT_MODE	0xf45c0007
+
+/* FSM_STATE */
+#define	R367CAB_FSM_STATE	0xf460
+#define	F367CAB_CRL_DFE	0xf4600080
+#define	F367CAB_DFE_START	0xf4600040
+#define	F367CAB_CTRLG_START	0xf4600030
+#define	F367CAB_FSM_FORCESTATE	0xf460000f
+
+/* FSM_CTL */
+#define	R367CAB_FSM_CTL	0xf461
+#define	F367CAB_FEC2_EN	0xf4610040
+#define	F367CAB_SIT_EN	0xf4610020
+#define	F367CAB_TRL_AHEAD	0xf4610010
+#define	F367CAB_TRL2_EN	0xf4610008
+#define	F367CAB_FSM_EQA1_EN	0xf4610004
+#define	F367CAB_FSM_BKP_DIS	0xf4610002
+#define	F367CAB_FSM_FORCE_EN	0xf4610001
+
+/* FSM_STS */
+#define	R367CAB_FSM_STS	0xf462
+#define	F367CAB_FSM_STATUS	0xf462000f
+
+/* FSM_SNR0_HTH */
+#define	R367CAB_FSM_SNR0_HTH	0xf463
+#define	F367CAB_SNR0_HTH	0xf46300ff
+
+/* FSM_SNR1_HTH */
+#define	R367CAB_FSM_SNR1_HTH	0xf464
+#define	F367CAB_SNR1_HTH	0xf46400ff
+
+/* FSM_SNR2_HTH */
+#define	R367CAB_FSM_SNR2_HTH	0xf465
+#define	F367CAB_SNR2_HTH	0xf46500ff
+
+/* FSM_SNR0_LTH */
+#define	R367CAB_FSM_SNR0_LTH	0xf466
+#define	F367CAB_SNR0_LTH	0xf46600ff
+
+/* FSM_SNR1_LTH */
+#define	R367CAB_FSM_SNR1_LTH	0xf467
+#define	F367CAB_SNR1_LTH	0xf46700ff
+
+/* FSM_EQA1_HTH */
+#define	R367CAB_FSM_EQA1_HTH	0xf468
+#define	F367CAB_SNR3_HTH_LO	0xf46800f0
+#define	F367CAB_EQA1_HTH	0xf468000f
+
+/* FSM_TEMPO */
+#define	R367CAB_FSM_TEMPO	0xf469
+#define	F367CAB_SIT	0xf46900c0
+#define	F367CAB_WST	0xf4690038
+#define	F367CAB_ELT	0xf4690006
+#define	F367CAB_SNR3_HTH_HI	0xf4690001
+
+/* FSM_CONFIG */
+#define	R367CAB_FSM_CONFIG	0xf46a
+#define	F367CAB_FEC2_DFEOFF	0xf46a0004
+#define	F367CAB_PRIT_STATE	0xf46a0002
+#define	F367CAB_MODMAP_STATE	0xf46a0001
+
+/* EQU_I_TESTTAP_L */
+#define	R367CAB_EQU_I_TESTTAP_L	0xf474
+#define	F367CAB_I_TEST_TAP_L	0xf47400ff
+
+/* EQU_I_TESTTAP_M */
+#define	R367CAB_EQU_I_TESTTAP_M	0xf475
+#define	F367CAB_I_TEST_TAP_M	0xf47500ff
+
+/* EQU_I_TESTTAP_H */
+#define	R367CAB_EQU_I_TESTTAP_H	0xf476
+#define	F367CAB_I_TEST_TAP_H	0xf476001f
+
+/* EQU_TESTAP_CFG */
+#define	R367CAB_EQU_TESTAP_CFG	0xf477
+#define	F367CAB_TEST_FFE_DFE_SEL	0xf4770040
+#define	F367CAB_TEST_TAP_SELECT	0xf477003f
+
+/* EQU_Q_TESTTAP_L */
+#define	R367CAB_EQU_Q_TESTTAP_L	0xf478
+#define	F367CAB_Q_TEST_TAP_L	0xf47800ff
+
+/* EQU_Q_TESTTAP_M */
+#define	R367CAB_EQU_Q_TESTTAP_M	0xf479
+#define	F367CAB_Q_TEST_TAP_M	0xf47900ff
+
+/* EQU_Q_TESTTAP_H */
+#define	R367CAB_EQU_Q_TESTTAP_H	0xf47a
+#define	F367CAB_Q_TEST_TAP_H	0xf47a001f
+
+/* EQU_TAP_CTRL */
+#define	R367CAB_EQU_TAP_CTRL	0xf47b
+#define	F367CAB_MTAP_FRZ	0xf47b0010
+#define	F367CAB_PRE_FREEZE	0xf47b0008
+#define	F367CAB_DFE_TAPMON_EN	0xf47b0004
+#define	F367CAB_FFE_TAPMON_EN	0xf47b0002
+#define	F367CAB_MTAP_ONLY	0xf47b0001
+
+/* EQU_CTR_CRL_CONTROL_L */
+#define	R367CAB_EQU_CTR_CRL_CONTROL_L	0xf47c
+#define	F367CAB_EQU_CTR_CRL_CONTROL_LO	0xf47c00ff
+
+/* EQU_CTR_CRL_CONTROL_H */
+#define	R367CAB_EQU_CTR_CRL_CONTROL_H	0xf47d
+#define	F367CAB_EQU_CTR_CRL_CONTROL_HI	0xf47d00ff
+
+/* EQU_CTR_HIPOW_L */
+#define	R367CAB_EQU_CTR_HIPOW_L	0xf47e
+#define	F367CAB_CTR_HIPOW_L	0xf47e00ff
+
+/* EQU_CTR_HIPOW_H */
+#define	R367CAB_EQU_CTR_HIPOW_H	0xf47f
+#define	F367CAB_CTR_HIPOW_H	0xf47f00ff
+
+/* EQU_I_EQU_LO */
+#define	R367CAB_EQU_I_EQU_LO	0xf480
+#define	F367CAB_EQU_I_EQU_L	0xf48000ff
+
+/* EQU_I_EQU_HI */
+#define	R367CAB_EQU_I_EQU_HI	0xf481
+#define	F367CAB_EQU_I_EQU_H	0xf4810003
+
+/* EQU_Q_EQU_LO */
+#define	R367CAB_EQU_Q_EQU_LO	0xf482
+#define	F367CAB_EQU_Q_EQU_L	0xf48200ff
+
+/* EQU_Q_EQU_HI */
+#define	R367CAB_EQU_Q_EQU_HI	0xf483
+#define	F367CAB_EQU_Q_EQU_H	0xf4830003
+
+/* EQU_MAPPER */
+#define	R367CAB_EQU_MAPPER	0xf484
+#define	F367CAB_QUAD_AUTO	0xf4840080
+#define	F367CAB_QUAD_INV	0xf4840040
+#define	F367CAB_QAM_MODE	0xf4840007
+
+/* EQU_SWEEP_RATE */
+#define	R367CAB_EQU_SWEEP_RATE	0xf485
+#define	F367CAB_SNR_PER	0xf48500c0
+#define	F367CAB_SWEEP_RATE	0xf485003f
+
+/* EQU_SNR_LO */
+#define	R367CAB_EQU_SNR_LO	0xf486
+#define	F367CAB_SNR_LO	0xf48600ff
+
+/* EQU_SNR_HI */
+#define	R367CAB_EQU_SNR_HI	0xf487
+#define	F367CAB_SNR_HI	0xf48700ff
+
+/* EQU_GAMMA_LO */
+#define	R367CAB_EQU_GAMMA_LO	0xf488
+#define	F367CAB_GAMMA_LO	0xf48800ff
+
+/* EQU_GAMMA_HI */
+#define	R367CAB_EQU_GAMMA_HI	0xf489
+#define	F367CAB_GAMMA_ME	0xf48900ff
+
+/* EQU_ERR_GAIN */
+#define	R367CAB_EQU_ERR_GAIN	0xf48a
+#define	F367CAB_EQA1MU	0xf48a0070
+#define	F367CAB_CRL2MU	0xf48a000e
+#define	F367CAB_GAMMA_HI	0xf48a0001
+
+/* EQU_RADIUS */
+#define	R367CAB_EQU_RADIUS	0xf48b
+#define	F367CAB_RADIUS	0xf48b00ff
+
+/* EQU_FFE_MAINTAP */
+#define	R367CAB_EQU_FFE_MAINTAP	0xf48c
+#define	F367CAB_FFE_MAINTAP_INIT	0xf48c00ff
+
+/* EQU_FFE_LEAKAGE */
+#define	R367CAB_EQU_FFE_LEAKAGE	0xf48e
+#define	F367CAB_LEAK_PER	0xf48e00f0
+#define	F367CAB_EQU_OUTSEL	0xf48e0002
+#define	F367CAB_PNT2dFE	0xf48e0001
+
+/* EQU_FFE_MAINTAP_POS */
+#define	R367CAB_EQU_FFE_MAINTAP_POS	0xf48f
+#define	F367CAB_FFE_LEAK_EN	0xf48f0080
+#define	F367CAB_DFE_LEAK_EN	0xf48f0040
+#define	F367CAB_FFE_MAINTAP_POS	0xf48f003f
+
+/* EQU_GAIN_WIDE */
+#define	R367CAB_EQU_GAIN_WIDE	0xf490
+#define	F367CAB_DFE_GAIN_WIDE	0xf49000f0
+#define	F367CAB_FFE_GAIN_WIDE	0xf490000f
+
+/* EQU_GAIN_NARROW */
+#define	R367CAB_EQU_GAIN_NARROW	0xf491
+#define	F367CAB_DFE_GAIN_NARROW	0xf49100f0
+#define	F367CAB_FFE_GAIN_NARROW	0xf491000f
+
+/* EQU_CTR_LPF_GAIN */
+#define	R367CAB_EQU_CTR_LPF_GAIN	0xf492
+#define	F367CAB_CTR_GTO	0xf4920080
+#define	F367CAB_CTR_GDIR	0xf4920070
+#define	F367CAB_SWEEP_EN	0xf4920008
+#define	F367CAB_CTR_GINT	0xf4920007
+
+/* EQU_CRL_LPF_GAIN */
+#define	R367CAB_EQU_CRL_LPF_GAIN	0xf493
+#define	F367CAB_CRL_GTO	0xf4930080
+#define	F367CAB_CRL_GDIR	0xf4930070
+#define	F367CAB_SWEEP_DIR	0xf4930008
+#define	F367CAB_CRL_GINT	0xf4930007
+
+/* EQU_GLOBAL_GAIN */
+#define	R367CAB_EQU_GLOBAL_GAIN	0xf494
+#define	F367CAB_CRL_GAIN	0xf49400f8
+#define	F367CAB_CTR_INC_GAIN	0xf4940004
+#define	F367CAB_CTR_FRAC	0xf4940003
+
+/* EQU_CRL_LD_SEN */
+#define	R367CAB_EQU_CRL_LD_SEN	0xf495
+#define	F367CAB_CTR_BADPOINT_EN	0xf4950080
+#define	F367CAB_CTR_GAIN	0xf4950070
+#define	F367CAB_LIMANEN	0xf4950008
+#define	F367CAB_CRL_LD_SEN	0xf4950007
+
+/* EQU_CRL_LD_VAL */
+#define	R367CAB_EQU_CRL_LD_VAL	0xf496
+#define	F367CAB_CRL_BISTH_LIMIT	0xf4960080
+#define	F367CAB_CARE_EN	0xf4960040
+#define	F367CAB_CRL_LD_PER	0xf4960030
+#define	F367CAB_CRL_LD_WST	0xf496000c
+#define	F367CAB_CRL_LD_TFS	0xf4960003
+
+/* EQU_CRL_TFR */
+#define	R367CAB_EQU_CRL_TFR	0xf497
+#define	F367CAB_CRL_LD_TFR	0xf49700ff
+
+/* EQU_CRL_BISTH_LO */
+#define	R367CAB_EQU_CRL_BISTH_LO	0xf498
+#define	F367CAB_CRL_BISTH_LO	0xf49800ff
+
+/* EQU_CRL_BISTH_HI */
+#define	R367CAB_EQU_CRL_BISTH_HI	0xf499
+#define	F367CAB_CRL_BISTH_HI	0xf49900ff
+
+/* EQU_SWEEP_RANGE_LO */
+#define	R367CAB_EQU_SWEEP_RANGE_LO	0xf49a
+#define	F367CAB_SWEEP_RANGE_LO	0xf49a00ff
+
+/* EQU_SWEEP_RANGE_HI */
+#define	R367CAB_EQU_SWEEP_RANGE_HI	0xf49b
+#define	F367CAB_SWEEP_RANGE_HI	0xf49b00ff
+
+/* EQU_CRL_LIMITER */
+#define	R367CAB_EQU_CRL_LIMITER	0xf49c
+#define	F367CAB_BISECTOR_EN	0xf49c0080
+#define	F367CAB_PHEST128_EN	0xf49c0040
+#define	F367CAB_CRL_LIM	0xf49c003f
+
+/* EQU_MODULUS_MAP */
+#define	R367CAB_EQU_MODULUS_MAP	0xf49d
+#define	F367CAB_PNT_DEPTH	0xf49d00e0
+#define	F367CAB_MODULUS_CMP	0xf49d001f
+
+/* EQU_PNT_GAIN */
+#define	R367CAB_EQU_PNT_GAIN	0xf49e
+#define	F367CAB_PNT_EN	0xf49e0080
+#define	F367CAB_MODULUSMAP_EN	0xf49e0040
+#define	F367CAB_PNT_GAIN	0xf49e003f
+
+/* FEC_AC_CTR_0 */
+#define	R367CAB_FEC_AC_CTR_0	0xf4a8
+#define	F367CAB_BE_BYPASS	0xf4a80020
+#define	F367CAB_REFRESH47	0xf4a80010
+#define	F367CAB_CT_NBST	0xf4a80008
+#define	F367CAB_TEI_ENA	0xf4a80004
+#define	F367CAB_DS_ENA	0xf4a80002
+#define	F367CAB_TSMF_EN	0xf4a80001
+
+/* FEC_AC_CTR_1 */
+#define	R367CAB_FEC_AC_CTR_1	0xf4a9
+#define	F367CAB_DEINT_DEPTH	0xf4a900ff
+
+/* FEC_AC_CTR_2 */
+#define	R367CAB_FEC_AC_CTR_2	0xf4aa
+#define	F367CAB_DEINT_M	0xf4aa00f8
+#define	F367CAB_DIS_UNLOCK	0xf4aa0004
+#define	F367CAB_DESCR_MODE	0xf4aa0003
+
+/* FEC_AC_CTR_3 */
+#define	R367CAB_FEC_AC_CTR_3	0xf4ab
+#define	F367CAB_DI_UNLOCK	0xf4ab0080
+#define	F367CAB_DI_FREEZE	0xf4ab0040
+#define	F367CAB_MISMATCH	0xf4ab0030
+#define	F367CAB_ACQ_MODE	0xf4ab000c
+#define	F367CAB_TRK_MODE	0xf4ab0003
+
+/* FEC_STATUS */
+#define	R367CAB_FEC_STATUS	0xf4ac
+#define	F367CAB_DEINT_SMCNTR	0xf4ac00e0
+#define	F367CAB_DEINT_SYNCSTATE	0xf4ac0018
+#define	F367CAB_DEINT_SYNLOST	0xf4ac0004
+#define	F367CAB_DESCR_SYNCSTATE	0xf4ac0002
+
+/* RS_COUNTER_0 */
+#define	R367CAB_RS_COUNTER_0	0xf4ae
+#define	F367CAB_BK_CT_L	0xf4ae00ff
+
+/* RS_COUNTER_1 */
+#define	R367CAB_RS_COUNTER_1	0xf4af
+#define	F367CAB_BK_CT_H	0xf4af00ff
+
+/* RS_COUNTER_2 */
+#define	R367CAB_RS_COUNTER_2	0xf4b0
+#define	F367CAB_CORR_CT_L	0xf4b000ff
+
+/* RS_COUNTER_3 */
+#define	R367CAB_RS_COUNTER_3	0xf4b1
+#define	F367CAB_CORR_CT_H	0xf4b100ff
+
+/* RS_COUNTER_4 */
+#define	R367CAB_RS_COUNTER_4	0xf4b2
+#define	F367CAB_UNCORR_CT_L	0xf4b200ff
+
+/* RS_COUNTER_5 */
+#define	R367CAB_RS_COUNTER_5	0xf4b3
+#define	F367CAB_UNCORR_CT_H	0xf4b300ff
+
+/* BERT_0 */
+#define	R367CAB_BERT_0	0xf4b4
+#define	F367CAB_RS_NOCORR	0xf4b40004
+#define	F367CAB_CT_HOLD	0xf4b40002
+#define	F367CAB_CT_CLEAR	0xf4b40001
+
+/* BERT_1 */
+#define	R367CAB_BERT_1	0xf4b5
+#define	F367CAB_BERT_ON	0xf4b50020
+#define	F367CAB_BERT_ERR_SRC	0xf4b50010
+#define	F367CAB_BERT_ERR_MODE	0xf4b50008
+#define	F367CAB_BERT_NBYTE	0xf4b50007
+
+/* BERT_2 */
+#define	R367CAB_BERT_2	0xf4b6
+#define	F367CAB_BERT_ERRCOUNT_L	0xf4b600ff
+
+/* BERT_3 */
+#define	R367CAB_BERT_3	0xf4b7
+#define	F367CAB_BERT_ERRCOUNT_H	0xf4b700ff
+
+/* OUTFORMAT_0 */
+#define	R367CAB_OUTFORMAT_0	0xf4b8
+#define	F367CAB_CLK_POLARITY	0xf4b80080
+#define	F367CAB_FEC_TYPE	0xf4b80040
+#define	F367CAB_SYNC_STRIP	0xf4b80008
+#define	F367CAB_TS_SWAP	0xf4b80004
+#define	F367CAB_OUTFORMAT	0xf4b80003
+
+/* OUTFORMAT_1 */
+#define	R367CAB_OUTFORMAT_1	0xf4b9
+#define	F367CAB_CI_DIVRANGE	0xf4b900ff
+
+/* SMOOTHER_2 */
+#define	R367CAB_SMOOTHER_2	0xf4be
+#define	F367CAB_FIFO_BYPASS	0xf4be0020
+
+/* TSMF_CTRL_0 */
+#define	R367CAB_TSMF_CTRL_0	0xf4c0
+#define	F367CAB_TS_NUMBER	0xf4c0001e
+#define	F367CAB_SEL_MODE	0xf4c00001
+
+/* TSMF_CTRL_1 */
+#define	R367CAB_TSMF_CTRL_1	0xf4c1
+#define	F367CAB_CHECK_ERROR_BIT	0xf4c10080
+#define	F367CAB_CHCK_F_SYNC	0xf4c10040
+#define	F367CAB_H_MODE	0xf4c10008
+#define	F367CAB_D_V_MODE	0xf4c10004
+#define	F367CAB_MODE	0xf4c10003
+
+/* TSMF_CTRL_3 */
+#define	R367CAB_TSMF_CTRL_3	0xf4c3
+#define	F367CAB_SYNC_IN_COUNT	0xf4c300f0
+#define	F367CAB_SYNC_OUT_COUNT	0xf4c3000f
+
+/* TS_ON_ID_0 */
+#define	R367CAB_TS_ON_ID_0	0xf4c4
+#define	F367CAB_TS_ID_L	0xf4c400ff
+
+/* TS_ON_ID_1 */
+#define	R367CAB_TS_ON_ID_1	0xf4c5
+#define	F367CAB_TS_ID_H	0xf4c500ff
+
+/* TS_ON_ID_2 */
+#define	R367CAB_TS_ON_ID_2	0xf4c6
+#define	F367CAB_ON_ID_L	0xf4c600ff
+
+/* TS_ON_ID_3 */
+#define	R367CAB_TS_ON_ID_3	0xf4c7
+#define	F367CAB_ON_ID_H	0xf4c700ff
+
+/* RE_STATUS_0 */
+#define	R367CAB_RE_STATUS_0	0xf4c8
+#define	F367CAB_RECEIVE_STATUS_L	0xf4c800ff
+
+/* RE_STATUS_1 */
+#define	R367CAB_RE_STATUS_1	0xf4c9
+#define	F367CAB_RECEIVE_STATUS_LH	0xf4c900ff
+
+/* RE_STATUS_2 */
+#define	R367CAB_RE_STATUS_2	0xf4ca
+#define	F367CAB_RECEIVE_STATUS_HL	0xf4ca00ff
+
+/* RE_STATUS_3 */
+#define	R367CAB_RE_STATUS_3	0xf4cb
+#define	F367CAB_RECEIVE_STATUS_HH	0xf4cb003f
+
+/* TS_STATUS_0 */
+#define	R367CAB_TS_STATUS_0	0xf4cc
+#define	F367CAB_TS_STATUS_L	0xf4cc00ff
+
+/* TS_STATUS_1 */
+#define	R367CAB_TS_STATUS_1	0xf4cd
+#define	F367CAB_TS_STATUS_H	0xf4cd007f
+
+/* TS_STATUS_2 */
+#define	R367CAB_TS_STATUS_2	0xf4ce
+#define	F367CAB_ERROR	0xf4ce0080
+#define	F367CAB_EMERGENCY	0xf4ce0040
+#define	F367CAB_CRE_TS	0xf4ce0030
+#define	F367CAB_VER	0xf4ce000e
+#define	F367CAB_M_LOCK	0xf4ce0001
+
+/* TS_STATUS_3 */
+#define	R367CAB_TS_STATUS_3	0xf4cf
+#define	F367CAB_UPDATE_READY	0xf4cf0080
+#define	F367CAB_END_FRAME_HEADER	0xf4cf0040
+#define	F367CAB_CONTCNT	0xf4cf0020
+#define	F367CAB_TS_IDENTIFIER_SEL	0xf4cf000f
+
+/* T_O_ID_0 */
+#define	R367CAB_T_O_ID_0	0xf4d0
+#define	F367CAB_ON_ID_I_L	0xf4d000ff
+
+/* T_O_ID_1 */
+#define	R367CAB_T_O_ID_1	0xf4d1
+#define	F367CAB_ON_ID_I_H	0xf4d100ff
+
+/* T_O_ID_2 */
+#define	R367CAB_T_O_ID_2	0xf4d2
+#define	F367CAB_TS_ID_I_L	0xf4d200ff
+
+/* T_O_ID_3 */
+#define	R367CAB_T_O_ID_3	0xf4d3
+#define	F367CAB_TS_ID_I_H	0xf4d300ff
+
+#define STV0367CAB_NBREGS	187
+
+#endif
diff --git a/drivers/media/dvb/frontends/stv0900.h b/drivers/media/dvb/frontends/stv0900.h
index e3e35d1ce838..91c7ee8b2313 100644
--- a/drivers/media/dvb/frontends/stv0900.h
+++ b/drivers/media/dvb/frontends/stv0900.h
@@ -53,6 +53,8 @@ struct stv0900_config {
 	u8 tun2_type;
 	/* Set device param to start dma */
 	int (*set_ts_params)(struct dvb_frontend *fe, int is_punctured);
+	/* Hook for Lock LED */
+	void (*set_lock_led)(struct dvb_frontend *fe, int offon);
 };
 
 #if defined(CONFIG_DVB_STV0900) || (defined(CONFIG_DVB_STV0900_MODULE) \
diff --git a/drivers/media/dvb/frontends/stv0900_core.c b/drivers/media/dvb/frontends/stv0900_core.c
index 4f5e7d3a0e61..0ca316d6fffa 100644
--- a/drivers/media/dvb/frontends/stv0900_core.c
+++ b/drivers/media/dvb/frontends/stv0900_core.c
@@ -1604,6 +1604,9 @@ static enum dvbfe_search stv0900_search(struct dvb_frontend *fe,
 	p_search.standard = STV0900_AUTO_SEARCH;
 	p_search.iq_inversion = STV0900_IQ_AUTO;
 	p_search.search_algo = STV0900_BLIND_SEARCH;
+	/* Speeds up DVB-S searching */
+	if (c->delivery_system == SYS_DVBS)
+		p_search.standard = STV0900_SEARCH_DVBS1;
 
 	intp->srch_standard[demod] = p_search.standard;
 	intp->symbol_rate[demod] = p_search.symbol_rate;
@@ -1660,8 +1663,14 @@ static int stv0900_read_status(struct dvb_frontend *fe, enum fe_status *status)
 			| FE_HAS_VITERBI
 			| FE_HAS_SYNC
 			| FE_HAS_LOCK;
-	} else
+		if (state->config->set_lock_led)
+			state->config->set_lock_led(fe, 1);
+	} else {
+		*status = 0;
+		if (state->config->set_lock_led)
+			state->config->set_lock_led(fe, 0);
 		dprintk("DEMOD LOCK FAIL\n");
+	}
 
 	return 0;
 }
@@ -1831,6 +1840,9 @@ static void stv0900_release(struct dvb_frontend *fe)
 
 	dprintk("%s\n", __func__);
 
+	if (state->config->set_lock_led)
+		state->config->set_lock_led(fe, 0);
+
 	if ((--(state->internal->dmds_used)) <= 0) {
 
 		dprintk("%s: Actually removing\n", __func__);
@@ -1842,6 +1854,18 @@ static void stv0900_release(struct dvb_frontend *fe)
 	kfree(state);
 }
 
+static int stv0900_sleep(struct dvb_frontend *fe)
+{
+	struct stv0900_state *state = fe->demodulator_priv;
+
+	dprintk("%s\n", __func__);
+
+	if (state->config->set_lock_led)
+		state->config->set_lock_led(fe, 0);
+
+	return 0;
+}
+
 static int stv0900_get_frontend(struct dvb_frontend *fe,
 				struct dvb_frontend_parameters *p)
 {
@@ -1876,6 +1900,7 @@ static struct dvb_frontend_ops stv0900_ops = {
 	.release			= stv0900_release,
 	.init				= stv0900_init,
 	.get_frontend                   = stv0900_get_frontend,
+	.sleep				= stv0900_sleep,
 	.get_frontend_algo		= stv0900_frontend_algo,
 	.i2c_gate_ctrl			= stv0900_i2c_gate_ctrl,
 	.diseqc_send_master_cmd		= stv0900_send_master_cmd,
diff --git a/drivers/media/dvb/frontends/stv090x.c b/drivers/media/dvb/frontends/stv090x.c
index 4e0fc2c8a41c..41d0f0a6655d 100644
--- a/drivers/media/dvb/frontends/stv090x.c
+++ b/drivers/media/dvb/frontends/stv090x.c
@@ -767,8 +767,12 @@ static int stv090x_i2c_gate_ctrl(struct stv090x_state *state, int enable)
 	 * In case of any error, the lock is unlocked and exit within the
 	 * relevant operations themselves.
 	 */
-	if (enable)
-		mutex_lock(&state->internal->tuner_lock);
+	if (enable) {
+		if (state->config->tuner_i2c_lock)
+			state->config->tuner_i2c_lock(&state->frontend, 1);
+		else
+			mutex_lock(&state->internal->tuner_lock);
+	}
 
 	reg = STV090x_READ_DEMOD(state, I2CRPT);
 	if (enable) {
@@ -784,13 +788,20 @@ static int stv090x_i2c_gate_ctrl(struct stv090x_state *state, int enable)
 			goto err;
 	}
 
-	if (!enable)
-		mutex_unlock(&state->internal->tuner_lock);
+	if (!enable) {
+		if (state->config->tuner_i2c_lock)
+			state->config->tuner_i2c_lock(&state->frontend, 0);
+		else
+			mutex_unlock(&state->internal->tuner_lock);
+	}
 
 	return 0;
 err:
 	dprintk(FE_ERROR, 1, "I/O error");
-	mutex_unlock(&state->internal->tuner_lock);
+	if (state->config->tuner_i2c_lock)
+		state->config->tuner_i2c_lock(&state->frontend, 0);
+	else
+		mutex_unlock(&state->internal->tuner_lock);
 	return -1;
 }
 
@@ -2883,10 +2894,12 @@ static int stv090x_optimize_track(struct stv090x_state *state)
 		STV090x_SETFIELD_Px(reg, DVBS2_ENABLE_FIELD, 1);
 		if (STV090x_WRITE_DEMOD(state, DMDCFGMD, reg) < 0)
 			goto err;
-		if (STV090x_WRITE_DEMOD(state, ACLC, 0) < 0)
-			goto err;
-		if (STV090x_WRITE_DEMOD(state, BCLC, 0) < 0)
-			goto err;
+		if (state->internal->dev_ver >= 0x30) {
+			if (STV090x_WRITE_DEMOD(state, ACLC, 0) < 0)
+				goto err;
+			if (STV090x_WRITE_DEMOD(state, BCLC, 0) < 0)
+				goto err;
+		}
 		if (state->frame_len == STV090x_LONG_FRAME) {
 			reg = STV090x_READ_DEMOD(state, DMDMODCOD);
 			modcod = STV090x_GETFIELD_Px(reg, DEMOD_MODCOD_FIELD);
@@ -3846,6 +3859,7 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 {
 	struct stv090x_state *state = fe->demodulator_priv;
 	u32 reg;
+	u8 full_standby = 0;
 
 	if (stv090x_i2c_gate_ctrl(state, 1) < 0)
 		goto err;
@@ -3858,24 +3872,119 @@ static int stv090x_sleep(struct dvb_frontend *fe)
 	if (stv090x_i2c_gate_ctrl(state, 0) < 0)
 		goto err;
 
-	dprintk(FE_DEBUG, 1, "Set %s to sleep",
-		state->device == STV0900 ? "STV0900" : "STV0903");
+	dprintk(FE_DEBUG, 1, "Set %s(%d) to sleep",
+		state->device == STV0900 ? "STV0900" : "STV0903",
+		state->demod);
 
-	reg = stv090x_read_reg(state, STV090x_SYNTCTRL);
-	STV090x_SETFIELD(reg, STANDBY_FIELD, 0x01);
-	if (stv090x_write_reg(state, STV090x_SYNTCTRL, reg) < 0)
-		goto err;
+	mutex_lock(&state->internal->demod_lock);
 
-	reg = stv090x_read_reg(state, STV090x_TSTTNR1);
-	STV090x_SETFIELD(reg, ADC1_PON_FIELD, 0);
-	if (stv090x_write_reg(state, STV090x_TSTTNR1, reg) < 0)
-		goto err;
+	switch (state->demod) {
+	case STV090x_DEMODULATOR_0:
+		/* power off ADC 1 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR1);
+		STV090x_SETFIELD(reg, ADC1_PON_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_TSTTNR1, reg) < 0)
+			goto err;
+		/* power off DiSEqC 1 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR2);
+		STV090x_SETFIELD(reg, DISEQC1_PON_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_TSTTNR2, reg) < 0)
+			goto err;
+
+		/* check whether path 2 is already sleeping, that is when
+		   ADC2 is off */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR3);
+		if (STV090x_GETFIELD(reg, ADC2_PON_FIELD) == 0)
+			full_standby = 1;
+
+		/* stop clocks */
+		reg = stv090x_read_reg(state, STV090x_STOPCLK1);
+		/* packet delineator 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKPKDT1_FIELD, 1);
+		/* ADC 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKADCI1_FIELD, 1);
+		/* FEC clock is shared between the two paths, only stop it
+		   when full standby is possible */
+		if (full_standby)
+			STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
+			goto err;
+		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
+		/* sampling 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKSAMP1_FIELD, 1);
+		/* viterbi 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKVIT1_FIELD, 1);
+		/* TS clock is shared between the two paths, only stop it
+		   when full standby is possible */
+		if (full_standby)
+			STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
+			goto err;
+		break;
+
+	case STV090x_DEMODULATOR_1:
+		/* power off ADC 2 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR3);
+		STV090x_SETFIELD(reg, ADC2_PON_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_TSTTNR3, reg) < 0)
+			goto err;
+		/* power off DiSEqC 2 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR4);
+		STV090x_SETFIELD(reg, DISEQC2_PON_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_TSTTNR4, reg) < 0)
+			goto err;
+
+		/* check whether path 1 is already sleeping, that is when
+		   ADC1 is off */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR1);
+		if (STV090x_GETFIELD(reg, ADC1_PON_FIELD) == 0)
+			full_standby = 1;
+
+		/* stop clocks */
+		reg = stv090x_read_reg(state, STV090x_STOPCLK1);
+		/* packet delineator 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKPKDT2_FIELD, 1);
+		/* ADC 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKADCI2_FIELD, 1);
+		/* FEC clock is shared between the two paths, only stop it
+		   when full standby is possible */
+		if (full_standby)
+			STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
+			goto err;
+		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
+		/* sampling 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKSAMP2_FIELD, 1);
+		/* viterbi 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKVIT2_FIELD, 1);
+		/* TS clock is shared between the two paths, only stop it
+		   when full standby is possible */
+		if (full_standby)
+			STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
+			goto err;
+		break;
 
+	default:
+		dprintk(FE_ERROR, 1, "Wrong demodulator!");
+		break;
+	}
+
+	if (full_standby) {
+		/* general power off */
+		reg = stv090x_read_reg(state, STV090x_SYNTCTRL);
+		STV090x_SETFIELD(reg, STANDBY_FIELD, 0x01);
+		if (stv090x_write_reg(state, STV090x_SYNTCTRL, reg) < 0)
+			goto err;
+	}
+
+	mutex_unlock(&state->internal->demod_lock);
 	return 0;
 
 err_gateoff:
 	stv090x_i2c_gate_ctrl(state, 0);
 err:
+	mutex_unlock(&state->internal->demod_lock);
 	dprintk(FE_ERROR, 1, "I/O error");
 	return -1;
 }
@@ -3885,21 +3994,94 @@ static int stv090x_wakeup(struct dvb_frontend *fe)
 	struct stv090x_state *state = fe->demodulator_priv;
 	u32 reg;
 
-	dprintk(FE_DEBUG, 1, "Wake %s from standby",
-		state->device == STV0900 ? "STV0900" : "STV0903");
+	dprintk(FE_DEBUG, 1, "Wake %s(%d) from standby",
+		state->device == STV0900 ? "STV0900" : "STV0903",
+		state->demod);
+
+	mutex_lock(&state->internal->demod_lock);
 
+	/* general power on */
 	reg = stv090x_read_reg(state, STV090x_SYNTCTRL);
 	STV090x_SETFIELD(reg, STANDBY_FIELD, 0x00);
 	if (stv090x_write_reg(state, STV090x_SYNTCTRL, reg) < 0)
 		goto err;
 
-	reg = stv090x_read_reg(state, STV090x_TSTTNR1);
-	STV090x_SETFIELD(reg, ADC1_PON_FIELD, 1);
-	if (stv090x_write_reg(state, STV090x_TSTTNR1, reg) < 0)
-		goto err;
+	switch (state->demod) {
+	case STV090x_DEMODULATOR_0:
+		/* power on ADC 1 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR1);
+		STV090x_SETFIELD(reg, ADC1_PON_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_TSTTNR1, reg) < 0)
+			goto err;
+		/* power on DiSEqC 1 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR2);
+		STV090x_SETFIELD(reg, DISEQC1_PON_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_TSTTNR2, reg) < 0)
+			goto err;
+
+		/* activate clocks */
+		reg = stv090x_read_reg(state, STV090x_STOPCLK1);
+		/* packet delineator 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKPKDT1_FIELD, 0);
+		/* ADC 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKADCI1_FIELD, 0);
+		/* FEC clock */
+		STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
+			goto err;
+		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
+		/* sampling 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKSAMP1_FIELD, 0);
+		/* viterbi 1 clock */
+		STV090x_SETFIELD(reg, STOP_CLKVIT1_FIELD, 0);
+		/* TS clock */
+		STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
+			goto err;
+		break;
 
+	case STV090x_DEMODULATOR_1:
+		/* power on ADC 2 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR3);
+		STV090x_SETFIELD(reg, ADC2_PON_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_TSTTNR3, reg) < 0)
+			goto err;
+		/* power on DiSEqC 2 */
+		reg = stv090x_read_reg(state, STV090x_TSTTNR4);
+		STV090x_SETFIELD(reg, DISEQC2_PON_FIELD, 1);
+		if (stv090x_write_reg(state, STV090x_TSTTNR4, reg) < 0)
+			goto err;
+
+		/* activate clocks */
+		reg = stv090x_read_reg(state, STV090x_STOPCLK1);
+		/* packet delineator 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKPKDT2_FIELD, 0);
+		/* ADC 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKADCI2_FIELD, 0);
+		/* FEC clock */
+		STV090x_SETFIELD(reg, STOP_CLKFEC_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_STOPCLK1, reg) < 0)
+			goto err;
+		reg = stv090x_read_reg(state, STV090x_STOPCLK2);
+		/* sampling 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKSAMP2_FIELD, 0);
+		/* viterbi 2 clock */
+		STV090x_SETFIELD(reg, STOP_CLKVIT2_FIELD, 0);
+		/* TS clock */
+		STV090x_SETFIELD(reg, STOP_CLKTS_FIELD, 0);
+		if (stv090x_write_reg(state, STV090x_STOPCLK2, reg) < 0)
+			goto err;
+		break;
+
+	default:
+		dprintk(FE_ERROR, 1, "Wrong demodulator!");
+		break;
+	}
+
+	mutex_unlock(&state->internal->demod_lock);
 	return 0;
 err:
+	mutex_unlock(&state->internal->demod_lock);
 	dprintk(FE_ERROR, 1, "I/O error");
 	return -1;
 }
@@ -4169,6 +4351,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 	switch (state->config->ts1_mode) {
 	case STV090x_TSMODE_PARALLEL_PUNCTURED:
 		reg = stv090x_read_reg(state, STV090x_P1_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts1_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x00);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x00);
 		if (stv090x_write_reg(state, STV090x_P1_TSCFGH, reg) < 0)
@@ -4177,6 +4360,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 
 	case STV090x_TSMODE_DVBCI:
 		reg = stv090x_read_reg(state, STV090x_P1_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts1_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x00);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x01);
 		if (stv090x_write_reg(state, STV090x_P1_TSCFGH, reg) < 0)
@@ -4185,6 +4369,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 
 	case STV090x_TSMODE_SERIAL_PUNCTURED:
 		reg = stv090x_read_reg(state, STV090x_P1_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts1_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x01);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x00);
 		if (stv090x_write_reg(state, STV090x_P1_TSCFGH, reg) < 0)
@@ -4193,6 +4378,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 
 	case STV090x_TSMODE_SERIAL_CONTINUOUS:
 		reg = stv090x_read_reg(state, STV090x_P1_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts1_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x01);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x01);
 		if (stv090x_write_reg(state, STV090x_P1_TSCFGH, reg) < 0)
@@ -4206,6 +4392,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 	switch (state->config->ts2_mode) {
 	case STV090x_TSMODE_PARALLEL_PUNCTURED:
 		reg = stv090x_read_reg(state, STV090x_P2_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts2_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x00);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x00);
 		if (stv090x_write_reg(state, STV090x_P2_TSCFGH, reg) < 0)
@@ -4214,6 +4401,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 
 	case STV090x_TSMODE_DVBCI:
 		reg = stv090x_read_reg(state, STV090x_P2_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts2_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x00);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x01);
 		if (stv090x_write_reg(state, STV090x_P2_TSCFGH, reg) < 0)
@@ -4222,6 +4410,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 
 	case STV090x_TSMODE_SERIAL_PUNCTURED:
 		reg = stv090x_read_reg(state, STV090x_P2_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts2_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x01);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x00);
 		if (stv090x_write_reg(state, STV090x_P2_TSCFGH, reg) < 0)
@@ -4230,6 +4419,7 @@ static int stv090x_set_tspath(struct stv090x_state *state)
 
 	case STV090x_TSMODE_SERIAL_CONTINUOUS:
 		reg = stv090x_read_reg(state, STV090x_P2_TSCFGH);
+		STV090x_SETFIELD_Px(reg, TSFIFO_TEIUPDATE_FIELD, state->config->ts2_tei);
 		STV090x_SETFIELD_Px(reg, TSFIFO_SERIAL_FIELD, 0x01);
 		STV090x_SETFIELD_Px(reg, TSFIFO_DVBCI_FIELD, 0x01);
 		if (stv090x_write_reg(state, STV090x_P2_TSCFGH, reg) < 0)
@@ -4506,16 +4696,26 @@ static int stv090x_setup(struct dvb_frontend *fe)
 	if (stv090x_write_reg(state, STV090x_TSTRES0, 0x00) < 0)
 		goto err;
 
-	/* workaround for stuck DiSEqC output */
-	if (config->diseqc_envelope_mode)
-		stv090x_send_diseqc_burst(fe, SEC_MINI_A);
-
 	return 0;
 err:
 	dprintk(FE_ERROR, 1, "I/O error");
 	return -1;
 }
 
+int stv090x_set_gpio(struct dvb_frontend *fe, u8 gpio, u8 dir, u8 value,
+		u8 xor_value)
+{
+	struct stv090x_state *state = fe->demodulator_priv;
+	u8 reg = 0;
+
+	STV090x_SETFIELD(reg, GPIOx_OPD_FIELD, dir);
+	STV090x_SETFIELD(reg, GPIOx_CONFIG_FIELD, value);
+	STV090x_SETFIELD(reg, GPIOx_XOR_FIELD, xor_value);
+
+	return stv090x_write_reg(state, STV090x_GPIOxCFG(gpio), reg);
+}
+EXPORT_SYMBOL(stv090x_set_gpio);
+
 static struct dvb_frontend_ops stv090x_ops = {
 
 	.info = {
@@ -4580,39 +4780,35 @@ struct dvb_frontend *stv090x_attach(const struct stv090x_config *config,
 		state->internal = temp_int->internal;
 		state->internal->num_used++;
 		dprintk(FE_INFO, 1, "Found Internal Structure!");
-		dprintk(FE_ERROR, 1, "Attaching %s demodulator(%d) Cut=0x%02x",
-			state->device == STV0900 ? "STV0900" : "STV0903",
-			demod,
-			state->internal->dev_ver);
-		return &state->frontend;
 	} else {
 		state->internal = kmalloc(sizeof(struct stv090x_internal),
 					  GFP_KERNEL);
+		if (!state->internal)
+			goto error;
 		temp_int = append_internal(state->internal);
+		if (!temp_int) {
+			kfree(state->internal);
+			goto error;
+		}
 		state->internal->num_used = 1;
 		state->internal->mclk = 0;
 		state->internal->dev_ver = 0;
 		state->internal->i2c_adap = state->i2c;
 		state->internal->i2c_addr = state->config->address;
 		dprintk(FE_INFO, 1, "Create New Internal Structure!");
-	}
 
-	mutex_init(&state->internal->demod_lock);
-	mutex_init(&state->internal->tuner_lock);
+		mutex_init(&state->internal->demod_lock);
+		mutex_init(&state->internal->tuner_lock);
 
-	if (stv090x_sleep(&state->frontend) < 0) {
-		dprintk(FE_ERROR, 1, "Error putting device to sleep");
-		goto error;
+		if (stv090x_setup(&state->frontend) < 0) {
+			dprintk(FE_ERROR, 1, "Error setting up device");
+			goto err_remove;
+		}
 	}
 
-	if (stv090x_setup(&state->frontend) < 0) {
-		dprintk(FE_ERROR, 1, "Error setting up device");
-		goto error;
-	}
-	if (stv090x_wakeup(&state->frontend) < 0) {
-		dprintk(FE_ERROR, 1, "Error waking device");
-		goto error;
-	}
+	/* workaround for stuck DiSEqC output */
+	if (config->diseqc_envelope_mode)
+		stv090x_send_diseqc_burst(&state->frontend, SEC_MINI_A);
 
 	dprintk(FE_ERROR, 1, "Attaching %s demodulator(%d) Cut=0x%02x",
 	       state->device == STV0900 ? "STV0900" : "STV0903",
@@ -4621,6 +4817,9 @@ struct dvb_frontend *stv090x_attach(const struct stv090x_config *config,
 
 	return &state->frontend;
 
+err_remove:
+	remove_dev(state->internal);
+	kfree(state->internal);
 error:
 	kfree(state);
 	return NULL;
diff --git a/drivers/media/dvb/frontends/stv090x.h b/drivers/media/dvb/frontends/stv090x.h
index dd1b93ae4e9d..29cdc2b71314 100644
--- a/drivers/media/dvb/frontends/stv090x.h
+++ b/drivers/media/dvb/frontends/stv090x.h
@@ -78,6 +78,9 @@ struct stv090x_config {
 	u32 ts1_clk;
 	u32 ts2_clk;
 
+	u8 ts1_tei : 1;
+	u8 ts2_tei : 1;
+
 	enum stv090x_i2crpt	repeater_level;
 
 	u8			tuner_bbgain; /* default: 10db */
@@ -97,6 +100,7 @@ struct stv090x_config {
 	int (*tuner_get_bbgain) (struct dvb_frontend *fe, u32 *gain);
 	int (*tuner_set_refclk)  (struct dvb_frontend *fe, u32 refclk);
 	int (*tuner_get_status) (struct dvb_frontend *fe, u32 *status);
+	void (*tuner_i2c_lock) (struct dvb_frontend *fe, int lock);
 };
 
 #if defined(CONFIG_DVB_STV090x) || (defined(CONFIG_DVB_STV090x_MODULE) && defined(MODULE))
@@ -104,6 +108,11 @@ struct stv090x_config {
 extern struct dvb_frontend *stv090x_attach(const struct stv090x_config *config,
 					   struct i2c_adapter *i2c,
 					   enum stv090x_demodulator demod);
+
+/* dir = 0 -> output, dir = 1 -> input/open-drain */
+extern int stv090x_set_gpio(struct dvb_frontend *fe, u8 gpio,
+		u8 dir, u8 value, u8 xor_value);
+
 #else
 
 static inline struct dvb_frontend *stv090x_attach(const struct stv090x_config *config,
@@ -113,6 +122,13 @@ static inline struct dvb_frontend *stv090x_attach(const struct stv090x_config *c
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
+
+static inline int stv090x_set_gpio(struct dvb_frontend *fe, u8 gpio,
+		u8 opd, u8 value, u8 xor_value)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return -ENODEV;
+}
 #endif /* CONFIG_DVB_STV090x */
 
 #endif /* __STV090x_H */
diff --git a/drivers/media/dvb/frontends/stv090x_reg.h b/drivers/media/dvb/frontends/stv090x_reg.h
index 2502855dd784..93741ee14297 100644
--- a/drivers/media/dvb/frontends/stv090x_reg.h
+++ b/drivers/media/dvb/frontends/stv090x_reg.h
@@ -1327,10 +1327,10 @@
 #define STV090x_WIDTH_Px_NOSPLHT_UNNORMED_FIELD	8
 
 #define STV090x_Px_NOSPLHy(__x, __y)		(0xf48f - (__x - 1) * 0x200 - __y * 0x1)
-#define STv090x_P1_NOSPLH0			STV090x_Px_NOSPLHy(1, 0)
-#define STv090x_P1_NOSPLH1			STV090x_Px_NOSPLHy(1, 1)
-#define STv090x_P2_NOSPLH0			STV090x_Px_NOSPLHy(2, 0)
-#define STv090x_P2_NOSPLH1			STV090x_Px_NOSPLHy(2, 1)
+#define STV090x_P1_NOSPLH0			STV090x_Px_NOSPLHy(1, 0)
+#define STV090x_P1_NOSPLH1			STV090x_Px_NOSPLHy(1, 1)
+#define STV090x_P2_NOSPLH0			STV090x_Px_NOSPLHy(2, 0)
+#define STV090x_P2_NOSPLH1			STV090x_Px_NOSPLHy(2, 1)
 #define STV090x_OFFST_Px_NOSPLH_UNNORMED_FIELD	0
 #define STV090x_WIDTH_Px_NOSPLH_UNNORMED_FIELD	8
 
@@ -1406,7 +1406,7 @@
 
 #define STV090x_Px_BCLC2S28(__x)		(0xf49d - (__x - 1) * 0x200)
 #define STV090x_P1_BCLC2S28			STV090x_Px_BCLC2S28(1)
-#define STV090x_P2_BCLC2S28			STV090x_Px_BCLC2S28(1)
+#define STV090x_P2_BCLC2S28			STV090x_Px_BCLC2S28(2)
 #define STV090x_OFFST_Px_CAR2S2_8_BETA_M_FIELD	4
 #define STV090x_WIDTH_Px_CAR2S2_8_BETA_M_FIELD	2
 #define STV090x_OFFST_Px_CAR2S2_8_BETA_E_FIELD	0
@@ -1414,7 +1414,7 @@
 
 #define STV090x_Px_BCLC2S216A(__x)		(0xf49e - (__x - 1) * 0x200)
 #define STV090x_P1_BCLC2S216A			STV090x_Px_BCLC2S216A(1)
-#define STV090x_P2_BCLC2S216A			STV090x_Px_BCLC2S216A(1)
+#define STV090x_P2_BCLC2S216A			STV090x_Px_BCLC2S216A(2)
 #define STV090x_OFFST_Px_CAR2S2_16A_BETA_M_FIELD	4
 #define STV090x_WIDTH_Px_CAR2S2_16A_BETA_M_FIELD	2
 #define STV090x_OFFST_Px_CAR2S2_16A_BETA_E_FIELD	0
@@ -1422,7 +1422,7 @@
 
 #define STV090x_Px_BCLC2S232A(__x)		(0xf49f - (__x - 1) * 0x200)
 #define STV090x_P1_BCLC2S232A			STV090x_Px_BCLC2S232A(1)
-#define STV090x_P2_BCLC2S232A			STV090x_Px_BCLC2S232A(1)
+#define STV090x_P2_BCLC2S232A			STV090x_Px_BCLC2S232A(2)
 #define STV090x_OFFST_Px_CAR2S2_32A_BETA_M_FIELD	4
 #define STV090x_WIDTH_Px_CAR2S2_32A_BETA_M_FIELD	2
 #define STV090x_OFFST_Px_CAR2S2_32A_BETA_E_FIELD	0
@@ -1602,7 +1602,7 @@
 
 #define STV090x_Px_CCIACC(__x)			(0xf4c4 - (__x - 1) * 0x200)
 #define STV090x_P1_CCIACC			STV090x_Px_CCIACC(1)
-#define STV090x_P2_CCIACC			STV090x_Px_CCIACC(1)
+#define STV090x_P2_CCIACC			STV090x_Px_CCIACC(2)
 #define STV090x_OFFST_Px_CCI_VALUE_FIELD	0
 #define STV090x_WIDTH_Px_CCI_VALUE_FIELD	8
 
diff --git a/drivers/media/dvb/mantis/mantis_pci.c b/drivers/media/dvb/mantis/mantis_pci.c
index 59feeb84aec7..10a432a79d00 100644
--- a/drivers/media/dvb/mantis/mantis_pci.c
+++ b/drivers/media/dvb/mantis/mantis_pci.c
@@ -22,7 +22,6 @@
 #include <linux/moduleparam.h>
 #include <linux/kernel.h>
 #include <asm/io.h>
-#include <asm/pgtable.h>
 #include <asm/page.h>
 #include <linux/kmod.h>
 #include <linux/vmalloc.h>
diff --git a/drivers/media/dvb/ngene/Makefile b/drivers/media/dvb/ngene/Makefile
index 0608aabb14ee..2bc96874d044 100644
--- a/drivers/media/dvb/ngene/Makefile
+++ b/drivers/media/dvb/ngene/Makefile
@@ -9,3 +9,6 @@ obj-$(CONFIG_DVB_NGENE) += ngene.o
 EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core/
 EXTRA_CFLAGS += -Idrivers/media/dvb/frontends/
 EXTRA_CFLAGS += -Idrivers/media/common/tuners/
+
+# For the staging CI driver cxd2099
+EXTRA_CFLAGS += -Idrivers/staging/cxd2099/
diff --git a/drivers/media/dvb/ngene/ngene-cards.c b/drivers/media/dvb/ngene/ngene-cards.c
index 4692a41ad95b..fcf4be901ec8 100644
--- a/drivers/media/dvb/ngene/ngene-cards.c
+++ b/drivers/media/dvb/ngene/ngene-cards.c
@@ -48,20 +48,27 @@
 
 static int tuner_attach_stv6110(struct ngene_channel *chan)
 {
+	struct i2c_adapter *i2c;
 	struct stv090x_config *feconf = (struct stv090x_config *)
 		chan->dev->card_info->fe_config[chan->number];
 	struct stv6110x_config *tunerconf = (struct stv6110x_config *)
 		chan->dev->card_info->tuner_config[chan->number];
 	struct stv6110x_devctl *ctl;
 
-	ctl = dvb_attach(stv6110x_attach, chan->fe, tunerconf,
-			 &chan->i2c_adapter);
+	/* tuner 1+2: i2c adapter #0, tuner 3+4: i2c adapter #1 */
+	if (chan->number < 2)
+		i2c = &chan->dev->channel[0].i2c_adapter;
+	else
+		i2c = &chan->dev->channel[1].i2c_adapter;
+
+	ctl = dvb_attach(stv6110x_attach, chan->fe, tunerconf, i2c);
 	if (ctl == NULL) {
 		printk(KERN_ERR	DEVICE_NAME ": No STV6110X found!\n");
 		return -ENODEV;
 	}
 
 	feconf->tuner_init          = ctl->tuner_init;
+	feconf->tuner_sleep         = ctl->tuner_sleep;
 	feconf->tuner_set_mode      = ctl->tuner_set_mode;
 	feconf->tuner_set_frequency = ctl->tuner_set_frequency;
 	feconf->tuner_get_frequency = ctl->tuner_get_frequency;
@@ -78,29 +85,106 @@ static int tuner_attach_stv6110(struct ngene_channel *chan)
 
 static int demod_attach_stv0900(struct ngene_channel *chan)
 {
+	struct i2c_adapter *i2c;
 	struct stv090x_config *feconf = (struct stv090x_config *)
 		chan->dev->card_info->fe_config[chan->number];
 
-	chan->fe = dvb_attach(stv090x_attach,
-			feconf,
-			&chan->i2c_adapter,
-			chan->number == 0 ? STV090x_DEMODULATOR_0 :
-					    STV090x_DEMODULATOR_1);
+	/* tuner 1+2: i2c adapter #0, tuner 3+4: i2c adapter #1 */
+	/* Note: Both adapters share the same i2c bus, but the demod     */
+	/*       driver requires that each demod has its own i2c adapter */
+	if (chan->number < 2)
+		i2c = &chan->dev->channel[0].i2c_adapter;
+	else
+		i2c = &chan->dev->channel[1].i2c_adapter;
+
+	chan->fe = dvb_attach(stv090x_attach, feconf, i2c,
+			(chan->number & 1) == 0 ? STV090x_DEMODULATOR_0
+						: STV090x_DEMODULATOR_1);
 	if (chan->fe == NULL) {
 		printk(KERN_ERR	DEVICE_NAME ": No STV0900 found!\n");
 		return -ENODEV;
 	}
 
-	if (!dvb_attach(lnbh24_attach, chan->fe, &chan->i2c_adapter, 0,
+	/* store channel info */
+	if (feconf->tuner_i2c_lock)
+		chan->fe->analog_demod_priv = chan;
+
+	if (!dvb_attach(lnbh24_attach, chan->fe, i2c, 0,
 			0, chan->dev->card_info->lnb[chan->number])) {
 		printk(KERN_ERR DEVICE_NAME ": No LNBH24 found!\n");
 		dvb_frontend_detach(chan->fe);
+		chan->fe = NULL;
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void cineS2_tuner_i2c_lock(struct dvb_frontend *fe, int lock)
+{
+	struct ngene_channel *chan = fe->analog_demod_priv;
+
+	if (lock)
+		down(&chan->dev->pll_mutex);
+	else
+		up(&chan->dev->pll_mutex);
+}
+
+static int cineS2_probe(struct ngene_channel *chan)
+{
+	struct i2c_adapter *i2c;
+	struct stv090x_config *fe_conf;
+	u8 buf[3];
+	struct i2c_msg i2c_msg = { .flags = 0, .buf = buf };
+	int rc;
+
+	/* tuner 1+2: i2c adapter #0, tuner 3+4: i2c adapter #1 */
+	if (chan->number < 2)
+		i2c = &chan->dev->channel[0].i2c_adapter;
+	else
+		i2c = &chan->dev->channel[1].i2c_adapter;
+
+	fe_conf = chan->dev->card_info->fe_config[chan->number];
+	i2c_msg.addr = fe_conf->address;
+
+	/* probe demod */
+	i2c_msg.len = 2;
+	buf[0] = 0xf1;
+	buf[1] = 0x00;
+	rc = i2c_transfer(i2c, &i2c_msg, 1);
+	if (rc != 1)
+		return -ENODEV;
+
+	/* demod found, attach it */
+	rc = demod_attach_stv0900(chan);
+	if (rc < 0 || chan->number < 2)
+		return rc;
+
+	/* demod #2: reprogram outputs DPN1 & DPN2 */
+	i2c_msg.len = 3;
+	buf[0] = 0xf1;
+	switch (chan->number) {
+	case 2:
+		buf[1] = 0x5c;
+		buf[2] = 0xc2;
+		break;
+	case 3:
+		buf[1] = 0x61;
+		buf[2] = 0xcc;
+		break;
+	default:
 		return -ENODEV;
 	}
+	rc = i2c_transfer(i2c, &i2c_msg, 1);
+	if (rc != 1) {
+		printk(KERN_ERR DEVICE_NAME ": could not setup DPNx\n");
+		return -EIO;
+	}
 
 	return 0;
 }
 
+
 static struct lgdt330x_config aver_m780 = {
 	.demod_address = 0xb2 >> 1,
 	.demod_chip    = LGDT3303,
@@ -151,6 +235,29 @@ static struct stv090x_config fe_cineS2 = {
 	.adc2_range	= STV090x_ADC_1Vpp,
 
 	.diseqc_envelope_mode = true,
+
+	.tuner_i2c_lock = cineS2_tuner_i2c_lock,
+};
+
+static struct stv090x_config fe_cineS2_2 = {
+	.device         = STV0900,
+	.demod_mode     = STV090x_DUAL,
+	.clk_mode       = STV090x_CLK_EXT,
+
+	.xtal           = 27000000,
+	.address        = 0x69,
+
+	.ts1_mode       = STV090x_TSMODE_SERIAL_PUNCTURED,
+	.ts2_mode       = STV090x_TSMODE_SERIAL_PUNCTURED,
+
+	.repeater_level = STV090x_RPTLEVEL_16,
+
+	.adc1_range	= STV090x_ADC_1Vpp,
+	.adc2_range	= STV090x_ADC_1Vpp,
+
+	.diseqc_envelope_mode = true,
+
+	.tuner_i2c_lock = cineS2_tuner_i2c_lock,
 };
 
 static struct stv6110x_config tuner_cineS2_0 = {
@@ -175,7 +282,8 @@ static struct ngene_info ngene_info_cineS2 = {
 	.tuner_config	= {&tuner_cineS2_0, &tuner_cineS2_1},
 	.lnb		= {0x0b, 0x08},
 	.tsf		= {3, 3},
-	.fw_version	= 15,
+	.fw_version	= 18,
+	.msi_supported	= true,
 };
 
 static struct ngene_info ngene_info_satixS2 = {
@@ -188,46 +296,54 @@ static struct ngene_info ngene_info_satixS2 = {
 	.tuner_config	= {&tuner_cineS2_0, &tuner_cineS2_1},
 	.lnb		= {0x0b, 0x08},
 	.tsf		= {3, 3},
-	.fw_version	= 15,
+	.fw_version	= 18,
+	.msi_supported	= true,
 };
 
 static struct ngene_info ngene_info_satixS2v2 = {
 	.type		= NGENE_SIDEWINDER,
 	.name		= "Mystique SaTiX-S2 Dual (v2)",
-	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN},
-	.demod_attach	= {demod_attach_stv0900, demod_attach_stv0900},
-	.tuner_attach	= {tuner_attach_stv6110, tuner_attach_stv6110},
-	.fe_config	= {&fe_cineS2, &fe_cineS2},
-	.tuner_config	= {&tuner_cineS2_0, &tuner_cineS2_1},
-	.lnb		= {0x0a, 0x08},
+	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN,
+			   NGENE_IO_TSOUT},
+	.demod_attach	= {demod_attach_stv0900, demod_attach_stv0900, cineS2_probe, cineS2_probe},
+	.tuner_attach	= {tuner_attach_stv6110, tuner_attach_stv6110, tuner_attach_stv6110, tuner_attach_stv6110},
+	.fe_config	= {&fe_cineS2, &fe_cineS2, &fe_cineS2_2, &fe_cineS2_2},
+	.tuner_config	= {&tuner_cineS2_0, &tuner_cineS2_1, &tuner_cineS2_0, &tuner_cineS2_1},
+	.lnb		= {0x0a, 0x08, 0x0b, 0x09},
 	.tsf		= {3, 3},
-	.fw_version	= 15,
+	.fw_version	= 18,
+	.msi_supported	= true,
 };
 
 static struct ngene_info ngene_info_cineS2v5 = {
 	.type		= NGENE_SIDEWINDER,
 	.name		= "Linux4Media cineS2 DVB-S2 Twin Tuner (v5)",
-	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN},
-	.demod_attach	= {demod_attach_stv0900, demod_attach_stv0900},
-	.tuner_attach	= {tuner_attach_stv6110, tuner_attach_stv6110},
-	.fe_config	= {&fe_cineS2, &fe_cineS2},
-	.tuner_config	= {&tuner_cineS2_0, &tuner_cineS2_1},
-	.lnb		= {0x0a, 0x08},
+	.io_type	= {NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN,
+			   NGENE_IO_TSOUT},
+	.demod_attach	= {demod_attach_stv0900, demod_attach_stv0900, cineS2_probe, cineS2_probe},
+	.tuner_attach	= {tuner_attach_stv6110, tuner_attach_stv6110, tuner_attach_stv6110, tuner_attach_stv6110},
+	.fe_config	= {&fe_cineS2, &fe_cineS2, &fe_cineS2_2, &fe_cineS2_2},
+	.tuner_config	= {&tuner_cineS2_0, &tuner_cineS2_1, &tuner_cineS2_0, &tuner_cineS2_1},
+	.lnb		= {0x0a, 0x08, 0x0b, 0x09},
 	.tsf		= {3, 3},
-	.fw_version	= 15,
+	.fw_version	= 18,
+	.msi_supported	= true,
 };
 
+
 static struct ngene_info ngene_info_duoFlexS2 = {
 	.type           = NGENE_SIDEWINDER,
 	.name           = "Digital Devices DuoFlex S2 miniPCIe",
-	.io_type        = {NGENE_IO_TSIN, NGENE_IO_TSIN},
-	.demod_attach   = {demod_attach_stv0900, demod_attach_stv0900},
-	.tuner_attach   = {tuner_attach_stv6110, tuner_attach_stv6110},
-	.fe_config      = {&fe_cineS2, &fe_cineS2},
-	.tuner_config   = {&tuner_cineS2_0, &tuner_cineS2_1},
-	.lnb            = {0x0a, 0x08},
+	.io_type        = {NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN, NGENE_IO_TSIN,
+			   NGENE_IO_TSOUT},
+	.demod_attach   = {cineS2_probe, cineS2_probe, cineS2_probe, cineS2_probe},
+	.tuner_attach   = {tuner_attach_stv6110, tuner_attach_stv6110, tuner_attach_stv6110, tuner_attach_stv6110},
+	.fe_config      = {&fe_cineS2, &fe_cineS2, &fe_cineS2_2, &fe_cineS2_2},
+	.tuner_config   = {&tuner_cineS2_0, &tuner_cineS2_1, &tuner_cineS2_0, &tuner_cineS2_1},
+	.lnb            = {0x0a, 0x08, 0x0b, 0x09},
 	.tsf            = {3, 3},
-	.fw_version     = 15,
+	.fw_version     = 18,
+	.msi_supported	= true,
 };
 
 static struct ngene_info ngene_info_m780 = {
@@ -321,6 +437,7 @@ static struct pci_driver ngene_pci_driver = {
 	.probe       = ngene_probe,
 	.remove      = __devexit_p(ngene_remove),
 	.err_handler = &ngene_errors,
+	.shutdown    = ngene_shutdown,
 };
 
 static __init int module_init_ngene(void)
diff --git a/drivers/media/dvb/ngene/ngene-core.c b/drivers/media/dvb/ngene/ngene-core.c
index dc073bdc623a..175a0f6c2a4c 100644
--- a/drivers/media/dvb/ngene/ngene-core.c
+++ b/drivers/media/dvb/ngene/ngene-core.c
@@ -45,6 +45,9 @@ static int one_adapter = 1;
 module_param(one_adapter, int, 0444);
 MODULE_PARM_DESC(one_adapter, "Use only one adapter.");
 
+static int shutdown_workaround;
+module_param(shutdown_workaround, int, 0644);
+MODULE_PARM_DESC(shutdown_workaround, "Activate workaround for shutdown problem with some chipsets.");
 
 static int debug;
 module_param(debug, int, 0444);
@@ -143,7 +146,7 @@ static void demux_tasklet(unsigned long data)
 			}
 		} else {
 			if (chan->HWState == HWSTATE_RUN) {
-				u32 Flags = 0;
+				u32 Flags = chan->DataFormatFlags;
 				IBufferExchange *exch1 = chan->pBufferExchange;
 				IBufferExchange *exch2 = chan->pBufferExchange2;
 				if (Cur->ngeneBuffer.SR.Flags & 0x01)
@@ -474,9 +477,9 @@ static u8 SPDIFConfiguration[10] = {
 
 /* Set NGENE I2S Config to transport stream compatible mode */
 
-static u8 TS_I2SConfiguration[4] = { 0x3E, 0x1A, 0x00, 0x00 }; /*3e 18 00 00 ?*/
+static u8 TS_I2SConfiguration[4] = { 0x3E, 0x18, 0x00, 0x00 };
 
-static u8 TS_I2SOutConfiguration[4] = { 0x80, 0x20, 0x00, 0x00 };
+static u8 TS_I2SOutConfiguration[4] = { 0x80, 0x04, 0x00, 0x00 };
 
 static u8 ITUDecoderSetup[4][16] = {
 	{0x1c, 0x13, 0x01, 0x68, 0x3d, 0x90, 0x14, 0x20,  /* SDTV */
@@ -749,13 +752,11 @@ void set_transfer(struct ngene_channel *chan, int state)
 		if (chan->mode & NGENE_IO_TSOUT) {
 			chan->pBufferExchange = tsout_exchange;
 			/* 0x66666666 = 50MHz *2^33 /250MHz */
-			chan->AudioDTOValue = 0x66666666;
-			/* set_dto(chan, 38810700+1000); */
-			/* set_dto(chan, 19392658); */
+			chan->AudioDTOValue = 0x80000000;
+			chan->AudioDTOUpdated = 1;
 		}
 		if (chan->mode & NGENE_IO_TSIN)
 			chan->pBufferExchange = tsin_exchange;
-		/* ngwritel(0, 0x9310); */
 		spin_unlock_irq(&chan->state_lock);
 	} else
 		;/* printk(KERN_INFO DEVICE_NAME ": lock=%08x\n",
@@ -1168,6 +1169,7 @@ static void ngene_release_buffers(struct ngene *dev)
 		iounmap(dev->iomem);
 	free_common_buffers(dev);
 	vfree(dev->tsout_buf);
+	vfree(dev->tsin_buf);
 	vfree(dev->ain_buf);
 	vfree(dev->vin_buf);
 	vfree(dev);
@@ -1184,6 +1186,13 @@ static int ngene_get_buffers(struct ngene *dev)
 		dvb_ringbuffer_init(&dev->tsout_rbuf,
 				    dev->tsout_buf, TSOUT_BUF_SIZE);
 	}
+	if (dev->card_info->io_type[2]&NGENE_IO_TSIN) {
+		dev->tsin_buf = vmalloc(TSIN_BUF_SIZE);
+		if (!dev->tsin_buf)
+			return -ENOMEM;
+		dvb_ringbuffer_init(&dev->tsin_rbuf,
+				    dev->tsin_buf, TSIN_BUF_SIZE);
+	}
 	if (dev->card_info->io_type[2] & NGENE_IO_AIN) {
 		dev->ain_buf = vmalloc(AIN_BUF_SIZE);
 		if (!dev->ain_buf)
@@ -1257,6 +1266,10 @@ static int ngene_load_firm(struct ngene *dev)
 		fw_name = "ngene_17.fw";
 		dev->cmd_timeout_workaround = true;
 		break;
+	case 18:
+		size = 0;
+		fw_name = "ngene_18.fw";
+		break;
 	}
 
 	if (request_firmware(&fw, fw_name, &dev->pci_dev->dev) < 0) {
@@ -1266,6 +1279,8 @@ static int ngene_load_firm(struct ngene *dev)
 			": Copy %s to your hotplug directory!\n", fw_name);
 		return -1;
 	}
+	if (size == 0)
+		size = fw->size;
 	if (size != fw->size) {
 		printk(KERN_ERR DEVICE_NAME
 			": Firmware %s has invalid size!", fw_name);
@@ -1301,6 +1316,35 @@ static void ngene_stop(struct ngene *dev)
 #endif
 }
 
+static int ngene_buffer_config(struct ngene *dev)
+{
+	int stat;
+
+	if (dev->card_info->fw_version >= 17) {
+		u8 tsin12_config[6]   = { 0x60, 0x60, 0x00, 0x00, 0x00, 0x00 };
+		u8 tsin1234_config[6] = { 0x30, 0x30, 0x00, 0x30, 0x30, 0x00 };
+		u8 tsio1235_config[6] = { 0x30, 0x30, 0x00, 0x28, 0x00, 0x38 };
+		u8 *bconf = tsin12_config;
+
+		if (dev->card_info->io_type[2]&NGENE_IO_TSIN &&
+		    dev->card_info->io_type[3]&NGENE_IO_TSIN) {
+			bconf = tsin1234_config;
+			if (dev->card_info->io_type[4]&NGENE_IO_TSOUT &&
+			    dev->ci.en)
+				bconf = tsio1235_config;
+		}
+		stat = ngene_command_config_free_buf(dev, bconf);
+	} else {
+		int bconf = BUFFER_CONFIG_4422;
+
+		if (dev->card_info->io_type[3] == NGENE_IO_TSIN)
+			bconf = BUFFER_CONFIG_3333;
+		stat = ngene_command_config_buf(dev, bconf);
+	}
+	return stat;
+}
+
+
 static int ngene_start(struct ngene *dev)
 {
 	int stat;
@@ -1365,23 +1409,6 @@ static int ngene_start(struct ngene *dev)
 	if (stat < 0)
 		goto fail;
 
-	if (dev->card_info->fw_version == 17) {
-		u8 tsin4_config[6] = {
-			3072 / 64, 3072 / 64, 0, 3072 / 64, 3072 / 64, 0};
-		u8 default_config[6] = {
-			4096 / 64, 4096 / 64, 0, 2048 / 64, 2048 / 64, 0};
-		u8 *bconf = default_config;
-
-		if (dev->card_info->io_type[3] == NGENE_IO_TSIN)
-			bconf = tsin4_config;
-		dprintk(KERN_DEBUG DEVICE_NAME ": FW 17 buffer config\n");
-		stat = ngene_command_config_free_buf(dev, bconf);
-	} else {
-		int bconf = BUFFER_CONFIG_4422;
-		if (dev->card_info->io_type[3] == NGENE_IO_TSIN)
-			bconf = BUFFER_CONFIG_3333;
-		stat = ngene_command_config_buf(dev, bconf);
-	}
 	if (!stat)
 		return stat;
 
@@ -1397,9 +1424,6 @@ fail2:
 	return stat;
 }
 
-
-
-
 /****************************************************************************/
 /****************************************************************************/
 /****************************************************************************/
@@ -1408,20 +1432,25 @@ static void release_channel(struct ngene_channel *chan)
 {
 	struct dvb_demux *dvbdemux = &chan->demux;
 	struct ngene *dev = chan->dev;
-	struct ngene_info *ni = dev->card_info;
-	int io = ni->io_type[chan->number];
 
-	if (chan->dev->cmd_timeout_workaround && chan->running)
+	if (chan->running)
 		set_transfer(chan, 0);
 
 	tasklet_kill(&chan->demux_tasklet);
 
-	if (io & (NGENE_IO_TSIN | NGENE_IO_TSOUT)) {
-		if (chan->fe) {
-			dvb_unregister_frontend(chan->fe);
-			dvb_frontend_detach(chan->fe);
-			chan->fe = NULL;
-		}
+	if (chan->ci_dev) {
+		dvb_unregister_device(chan->ci_dev);
+		chan->ci_dev = NULL;
+	}
+
+	if (chan->fe) {
+		dvb_unregister_frontend(chan->fe);
+		dvb_frontend_detach(chan->fe);
+		chan->fe = NULL;
+	}
+
+	if (chan->has_demux) {
+		dvb_net_release(&chan->dvbnet);
 		dvbdemux->dmx.close(&dvbdemux->dmx);
 		dvbdemux->dmx.remove_frontend(&dvbdemux->dmx,
 					      &chan->hw_frontend);
@@ -1429,9 +1458,12 @@ static void release_channel(struct ngene_channel *chan)
 					      &chan->mem_frontend);
 		dvb_dmxdev_release(&chan->dmxdev);
 		dvb_dmx_release(&chan->demux);
+		chan->has_demux = false;
+	}
 
-		if (chan->number == 0 || !one_adapter)
-			dvb_unregister_adapter(&dev->adapter[chan->number]);
+	if (chan->has_adapter) {
+		dvb_unregister_adapter(&dev->adapter[chan->number]);
+		chan->has_adapter = false;
 	}
 }
 
@@ -1449,9 +1481,27 @@ static int init_channel(struct ngene_channel *chan)
 	chan->type = io;
 	chan->mode = chan->type;	/* for now only one mode */
 
+	if (io & NGENE_IO_TSIN) {
+		chan->fe = NULL;
+		if (ni->demod_attach[nr]) {
+			ret = ni->demod_attach[nr](chan);
+			if (ret < 0)
+				goto err;
+		}
+		if (chan->fe && ni->tuner_attach[nr]) {
+			ret = ni->tuner_attach[nr](chan);
+			if (ret < 0)
+				goto err;
+		}
+	}
+
+	if (!dev->ci.en && (io & NGENE_IO_TSOUT))
+		return 0;
+
 	if (io & (NGENE_IO_TSIN | NGENE_IO_TSOUT)) {
 		if (nr >= STREAM_AUDIOIN1)
 			chan->DataFormatFlags = DF_SWAP32;
+
 		if (nr == 0 || !one_adapter || dev->first_adapter == NULL) {
 			adapter = &dev->adapter[nr];
 			ret = dvb_register_adapter(adapter, "nGene",
@@ -1459,40 +1509,50 @@ static int init_channel(struct ngene_channel *chan)
 						   &chan->dev->pci_dev->dev,
 						   adapter_nr);
 			if (ret < 0)
-				return ret;
+				goto err;
 			if (dev->first_adapter == NULL)
 				dev->first_adapter = adapter;
-		} else {
+			chan->has_adapter = true;
+		} else
 			adapter = dev->first_adapter;
-		}
+	}
 
+	if (dev->ci.en && (io & NGENE_IO_TSOUT)) {
+		dvb_ca_en50221_init(adapter, dev->ci.en, 0, 1);
+		set_transfer(chan, 1);
+		set_transfer(&chan->dev->channel[2], 1);
+		dvb_register_device(adapter, &chan->ci_dev,
+				    &ngene_dvbdev_ci, (void *) chan,
+				    DVB_DEVICE_SEC);
+		if (!chan->ci_dev)
+			goto err;
+	}
+
+	if (chan->fe) {
+		if (dvb_register_frontend(adapter, chan->fe) < 0)
+			goto err;
+		chan->has_demux = true;
+	}
+
+	if (chan->has_demux) {
 		ret = my_dvb_dmx_ts_card_init(dvbdemux, "SW demux",
 					      ngene_start_feed,
 					      ngene_stop_feed, chan);
 		ret = my_dvb_dmxdev_ts_card_init(&chan->dmxdev, &chan->demux,
 						 &chan->hw_frontend,
 						 &chan->mem_frontend, adapter);
+		ret = dvb_net_init(adapter, &chan->dvbnet, &chan->demux.dmx);
 	}
 
-	if (io & NGENE_IO_TSIN) {
+	return ret;
+
+err:
+	if (chan->fe) {
+		dvb_frontend_detach(chan->fe);
 		chan->fe = NULL;
-		if (ni->demod_attach[nr])
-			ni->demod_attach[nr](chan);
-		if (chan->fe) {
-			if (dvb_register_frontend(adapter, chan->fe) < 0) {
-				if (chan->fe->ops.release)
-					chan->fe->ops.release(chan->fe);
-				chan->fe = NULL;
-			}
-		}
-		if (chan->fe && ni->tuner_attach[nr])
-			if (ni->tuner_attach[nr] (chan) < 0) {
-				printk(KERN_ERR DEVICE_NAME
-				       ": Tuner attach failed on channel %d!\n",
-				       nr);
-			}
 	}
-	return ret;
+	release_channel(chan);
+	return 0;
 }
 
 static int init_channels(struct ngene *dev)
@@ -1510,6 +1570,57 @@ static int init_channels(struct ngene *dev)
 	return 0;
 }
 
+static void cxd_attach(struct ngene *dev)
+{
+	struct ngene_ci *ci = &dev->ci;
+
+	ci->en = cxd2099_attach(0x40, dev, &dev->channel[0].i2c_adapter);
+	ci->dev = dev;
+	return;
+}
+
+static void cxd_detach(struct ngene *dev)
+{
+	struct ngene_ci *ci = &dev->ci;
+
+	dvb_ca_en50221_release(ci->en);
+	kfree(ci->en);
+	ci->en = 0;
+}
+
+/***********************************/
+/* workaround for shutdown failure */
+/***********************************/
+
+static void ngene_unlink(struct ngene *dev)
+{
+	struct ngene_command com;
+
+	com.cmd.hdr.Opcode = CMD_MEM_WRITE;
+	com.cmd.hdr.Length = 3;
+	com.cmd.MemoryWrite.address = 0x910c;
+	com.cmd.MemoryWrite.data = 0xff;
+	com.in_len = 3;
+	com.out_len = 1;
+
+	down(&dev->cmd_mutex);
+	ngwritel(0, NGENE_INT_ENABLE);
+	ngene_command_mutex(dev, &com);
+	up(&dev->cmd_mutex);
+}
+
+void ngene_shutdown(struct pci_dev *pdev)
+{
+	struct ngene *dev = (struct ngene *)pci_get_drvdata(pdev);
+
+	if (!dev || !shutdown_workaround)
+		return;
+
+	printk(KERN_INFO DEVICE_NAME ": shutdown workaround...\n");
+	ngene_unlink(dev);
+	pci_disable_device(pdev);
+}
+
 /****************************************************************************/
 /* device probe/remove calls ************************************************/
 /****************************************************************************/
@@ -1522,6 +1633,8 @@ void __devexit ngene_remove(struct pci_dev *pdev)
 	tasklet_kill(&dev->event_tasklet);
 	for (i = MAX_STREAM - 1; i >= 0; i--)
 		release_channel(&dev->channel[i]);
+	if (dev->ci.en)
+		cxd_detach(dev);
 	ngene_stop(dev);
 	ngene_release_buffers(dev);
 	pci_set_drvdata(pdev, NULL);
@@ -1557,6 +1670,13 @@ int __devinit ngene_probe(struct pci_dev *pci_dev,
 	if (stat < 0)
 		goto fail1;
 
+	cxd_attach(dev);
+
+	stat = ngene_buffer_config(dev);
+	if (stat < 0)
+		goto fail1;
+
+
 	dev->i2c_current_bus = -1;
 
 	/* Register DVB adapters and devices for both channels */
diff --git a/drivers/media/dvb/ngene/ngene-dvb.c b/drivers/media/dvb/ngene/ngene-dvb.c
index 3832e5983c19..0b4943233166 100644
--- a/drivers/media/dvb/ngene/ngene-dvb.c
+++ b/drivers/media/dvb/ngene/ngene-dvb.c
@@ -47,6 +47,64 @@
 /* COMMAND API interface ****************************************************/
 /****************************************************************************/
 
+static ssize_t ts_write(struct file *file, const char *buf,
+			size_t count, loff_t *ppos)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct ngene_channel *chan = dvbdev->priv;
+	struct ngene *dev = chan->dev;
+
+	if (wait_event_interruptible(dev->tsout_rbuf.queue,
+				     dvb_ringbuffer_free
+				     (&dev->tsout_rbuf) >= count) < 0)
+		return 0;
+
+	dvb_ringbuffer_write(&dev->tsout_rbuf, buf, count);
+
+	return count;
+}
+
+static ssize_t ts_read(struct file *file, char *buf,
+		       size_t count, loff_t *ppos)
+{
+	struct dvb_device *dvbdev = file->private_data;
+	struct ngene_channel *chan = dvbdev->priv;
+	struct ngene *dev = chan->dev;
+	int left, avail;
+
+	left = count;
+	while (left) {
+		if (wait_event_interruptible(
+			    dev->tsin_rbuf.queue,
+			    dvb_ringbuffer_avail(&dev->tsin_rbuf) > 0) < 0)
+			return -EAGAIN;
+		avail = dvb_ringbuffer_avail(&dev->tsin_rbuf);
+		if (avail > left)
+			avail = left;
+		dvb_ringbuffer_read_user(&dev->tsin_rbuf, buf, avail);
+		left -= avail;
+		buf += avail;
+	}
+	return count;
+}
+
+static const struct file_operations ci_fops = {
+	.owner   = THIS_MODULE,
+	.read    = ts_read,
+	.write   = ts_write,
+	.open    = dvb_generic_open,
+	.release = dvb_generic_release,
+};
+
+struct dvb_device ngene_dvbdev_ci = {
+	.priv    = 0,
+	.readers = -1,
+	.writers = -1,
+	.users   = -1,
+	.fops    = &ci_fops,
+};
+
+
 /****************************************************************************/
 /* DVB functions and API interface ******************************************/
 /****************************************************************************/
@@ -63,10 +121,21 @@ static void swap_buffer(u32 *p, u32 len)
 void *tsin_exchange(void *priv, void *buf, u32 len, u32 clock, u32 flags)
 {
 	struct ngene_channel *chan = priv;
+	struct ngene *dev = chan->dev;
 
 
-	if (chan->users > 0)
+	if (flags & DF_SWAP32)
+		swap_buffer(buf, len);
+	if (dev->ci.en && chan->number == 2) {
+		if (dvb_ringbuffer_free(&dev->tsin_rbuf) > len) {
+			dvb_ringbuffer_write(&dev->tsin_rbuf, buf, len);
+			wake_up_interruptible(&dev->tsin_rbuf.queue);
+		}
+		return 0;
+	}
+	if (chan->users > 0) {
 		dvb_dmx_swfilter(&chan->demux, buf, len);
+	}
 	return NULL;
 }
 
diff --git a/drivers/media/dvb/ngene/ngene.h b/drivers/media/dvb/ngene/ngene.h
index 8fb4200f83f8..40fce9e3ae66 100644
--- a/drivers/media/dvb/ngene/ngene.h
+++ b/drivers/media/dvb/ngene/ngene.h
@@ -36,8 +36,11 @@
 #include "dmxdev.h"
 #include "dvbdev.h"
 #include "dvb_demux.h"
+#include "dvb_ca_en50221.h"
 #include "dvb_frontend.h"
 #include "dvb_ringbuffer.h"
+#include "dvb_net.h"
+#include "cxd2099.h"
 
 #define DEVICE_NAME "ngene"
 
@@ -636,14 +639,18 @@ struct ngene_channel {
 	int                   number;
 	int                   type;
 	int                   mode;
+	bool                  has_adapter;
+	bool                  has_demux;
 
 	struct dvb_frontend  *fe;
 	struct dmxdev         dmxdev;
 	struct dvb_demux      demux;
+	struct dvb_net        dvbnet;
 	struct dmx_frontend   hw_frontend;
 	struct dmx_frontend   mem_frontend;
 	int                   users;
 	struct video_device  *v4l_dev;
+	struct dvb_device    *ci_dev;
 	struct tasklet_struct demux_tasklet;
 
 	struct SBufferHeader *nextBuffer;
@@ -710,6 +717,15 @@ struct ngene_channel {
 	int running;
 };
 
+
+struct ngene_ci {
+	struct device         device;
+	struct i2c_adapter    i2c_adapter;
+
+	struct ngene         *dev;
+	struct dvb_ca_en50221 *en;
+};
+
 struct ngene;
 
 typedef void (rx_cb_t)(struct ngene *, u32, u8);
@@ -774,6 +790,10 @@ struct ngene {
 #define TSOUT_BUF_SIZE (512*188*8)
 	struct dvb_ringbuffer tsout_rbuf;
 
+	u8                   *tsin_buf;
+#define TSIN_BUF_SIZE (512*188*8)
+	struct dvb_ringbuffer tsin_rbuf;
+
 	u8                   *ain_buf;
 #define AIN_BUF_SIZE (128*1024)
 	struct dvb_ringbuffer ain_rbuf;
@@ -785,6 +805,8 @@ struct ngene {
 
 	unsigned long         exp_val;
 	int prev_cmd;
+
+	struct ngene_ci       ci;
 };
 
 struct ngene_info {
@@ -863,6 +885,7 @@ struct ngene_buffer {
 int __devinit ngene_probe(struct pci_dev *pci_dev,
 			  const struct pci_device_id *id);
 void __devexit ngene_remove(struct pci_dev *pdev);
+void ngene_shutdown(struct pci_dev *pdev);
 int ngene_command(struct ngene *dev, struct ngene_command *com);
 int ngene_command_gpio_set(struct ngene *dev, u8 select, u8 level);
 void set_transfer(struct ngene_channel *chan, int state);
@@ -872,6 +895,7 @@ void FillTSBuffer(void *Buffer, int Length, u32 Flags);
 int ngene_i2c_init(struct ngene *dev, int dev_nr);
 
 /* Provided by ngene-dvb.c */
+extern struct dvb_device ngene_dvbdev_ci;
 void *tsout_exchange(void *priv, void *buf, u32 len, u32 clock, u32 flags);
 void *tsin_exchange(void *priv, void *buf, u32 len, u32 clock, u32 flags);
 int ngene_start_feed(struct dvb_demux_feed *dvbdmxfeed);
diff --git a/drivers/media/dvb/ttusb-budget/dvb-ttusb-budget.c b/drivers/media/dvb/ttusb-budget/dvb-ttusb-budget.c
index 40625b26ac10..cbe2f0de1442 100644
--- a/drivers/media/dvb/ttusb-budget/dvb-ttusb-budget.c
+++ b/drivers/media/dvb/ttusb-budget/dvb-ttusb-budget.c
@@ -334,6 +334,7 @@ static int ttusb_boot_dsp(struct ttusb *ttusb)
 	err = ttusb_cmd(ttusb, b, 4, 0);
 
       done:
+	release_firmware(fw);
 	if (err) {
 		dprintk("%s: usb_bulk_msg() failed, return value %i!\n",
 			__func__, err);
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index ecdffa6aac66..299994c3aa74 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -441,6 +441,7 @@ config RADIO_TIMBERDALE
 config RADIO_WL1273
 	tristate "Texas Instruments WL1273 I2C FM Radio"
 	depends on I2C && VIDEO_V4L2
+	select MFD_CORE
 	select MFD_WL1273_CORE
 	select FW_LOADER
 	---help---
@@ -454,4 +455,7 @@ config RADIO_WL1273
 	  To compile this driver as a module, choose M here: the
 	  module will be called radio-wl1273.
 
+# TI's ST based wl128x FM radio
+source "drivers/media/radio/wl128x/Kconfig"
+
 endif # RADIO_ADAPTERS
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index 717656d2f749..2faa33371986 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -26,5 +26,6 @@ obj-$(CONFIG_RADIO_SAA7706H) += saa7706h.o
 obj-$(CONFIG_RADIO_TEF6862) += tef6862.o
 obj-$(CONFIG_RADIO_TIMBERDALE) += radio-timb.o
 obj-$(CONFIG_RADIO_WL1273) += radio-wl1273.o
+obj-$(CONFIG_RADIO_WL128X) += wl128x/
 
 EXTRA_CFLAGS += -Isound
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index a185610b376b..1e3a8dd820a4 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -21,6 +21,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
@@ -148,7 +149,7 @@ static const struct v4l2_file_operations timbradio_fops = {
 
 static int __devinit timbradio_probe(struct platform_device *pdev)
 {
-	struct timb_radio_platform_data *pdata = pdev->dev.platform_data;
+	struct timb_radio_platform_data *pdata = mfd_get_data(pdev);
 	struct timbradio *tr;
 	int err;
 
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index 7ecc8e657663..3ee08bd0b6aa 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -1,7 +1,7 @@
 /*
  * Driver for the Texas Instruments WL1273 FM radio.
  *
- * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2011 Nokia Corporation
  * Author: Matti J. Aaltonen <matti.j.aaltonen@nokia.com>
  *
  * This program is free software; you can redistribute it and/or
@@ -104,58 +104,6 @@ static unsigned int rds_buf = 100;
 module_param(rds_buf, uint, 0);
 MODULE_PARM_DESC(rds_buf, "Number of RDS buffer entries. Default = 100");
 
-static int wl1273_fm_read_reg(struct wl1273_core *core, u8 reg, u16 *value)
-{
-	struct i2c_client *client = core->client;
-	u8 b[2];
-	int r;
-
-	r = i2c_smbus_read_i2c_block_data(client, reg, sizeof(b), b);
-	if (r != 2) {
-		dev_err(&client->dev, "%s: Read: %d fails.\n", __func__, reg);
-		return -EREMOTEIO;
-	}
-
-	*value = (u16)b[0] << 8 | b[1];
-
-	return 0;
-}
-
-static int wl1273_fm_write_cmd(struct wl1273_core *core, u8 cmd, u16 param)
-{
-	struct i2c_client *client = core->client;
-	u8 buf[] = { (param >> 8) & 0xff, param & 0xff };
-	int r;
-
-	r = i2c_smbus_write_i2c_block_data(client, cmd, sizeof(buf), buf);
-	if (r) {
-		dev_err(&client->dev, "%s: Cmd: %d fails.\n", __func__, cmd);
-		return r;
-	}
-
-	return 0;
-}
-
-static int wl1273_fm_write_data(struct wl1273_core *core, u8 *data, u16 len)
-{
-	struct i2c_client *client = core->client;
-	struct i2c_msg msg;
-	int r;
-
-	msg.addr = client->addr;
-	msg.flags = 0;
-	msg.buf = data;
-	msg.len = len;
-
-	r = i2c_transfer(client->adapter, &msg, 1);
-	if (r != 1) {
-		dev_err(&client->dev, "%s: write error.\n", __func__);
-		return -EREMOTEIO;
-	}
-
-	return 0;
-}
-
 static int wl1273_fm_write_fw(struct wl1273_core *core,
 			      __u8 *fw, int len)
 {
@@ -188,94 +136,6 @@ static int wl1273_fm_write_fw(struct wl1273_core *core,
 	return r;
 }
 
-/**
- * wl1273_fm_set_audio() -	Set audio mode.
- * @core:			A pointer to the device struct.
- * @new_mode:			The new audio mode.
- *
- * Audio modes are WL1273_AUDIO_DIGITAL and WL1273_AUDIO_ANALOG.
- */
-static int wl1273_fm_set_audio(struct wl1273_core *core, unsigned int new_mode)
-{
-	int r = 0;
-
-	if (core->mode == WL1273_MODE_OFF ||
-	    core->mode == WL1273_MODE_SUSPENDED)
-		return -EPERM;
-
-	if (core->mode == WL1273_MODE_RX && new_mode == WL1273_AUDIO_DIGITAL) {
-		r = wl1273_fm_write_cmd(core, WL1273_PCM_MODE_SET,
-					WL1273_PCM_DEF_MODE);
-		if (r)
-			goto out;
-
-		r = wl1273_fm_write_cmd(core, WL1273_I2S_MODE_CONFIG_SET,
-					core->i2s_mode);
-		if (r)
-			goto out;
-
-		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_ENABLE,
-					WL1273_AUDIO_ENABLE_I2S);
-		if (r)
-			goto out;
-
-	} else if (core->mode == WL1273_MODE_RX &&
-		   new_mode == WL1273_AUDIO_ANALOG) {
-		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_ENABLE,
-					WL1273_AUDIO_ENABLE_ANALOG);
-		if (r)
-			goto out;
-
-	} else if (core->mode == WL1273_MODE_TX &&
-		   new_mode == WL1273_AUDIO_DIGITAL) {
-		r = wl1273_fm_write_cmd(core, WL1273_I2S_MODE_CONFIG_SET,
-					core->i2s_mode);
-		if (r)
-			goto out;
-
-		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_IO_SET,
-					WL1273_AUDIO_IO_SET_I2S);
-		if (r)
-			goto out;
-
-	} else if (core->mode == WL1273_MODE_TX &&
-		   new_mode == WL1273_AUDIO_ANALOG) {
-		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_IO_SET,
-					WL1273_AUDIO_IO_SET_ANALOG);
-		if (r)
-			goto out;
-	}
-
-	core->audio_mode = new_mode;
-out:
-	return r;
-}
-
-/**
- * wl1273_fm_set_volume() -	Set volume.
- * @core:			A pointer to the device struct.
- * @volume:			The new volume value.
- */
-static int wl1273_fm_set_volume(struct wl1273_core *core, unsigned int volume)
-{
-	u16 val;
-	int r;
-
-	if (volume > WL1273_MAX_VOLUME)
-		return -EINVAL;
-
-	if (core->volume == volume)
-		return 0;
-
-	val = volume;
-	r = wl1273_fm_read_reg(core, WL1273_VOLUME_SET, &val);
-	if (r)
-		return r;
-
-	core->volume = volume;
-	return 0;
-}
-
 #define WL1273_FIFO_HAS_DATA(status)	(1 << 5 & status)
 #define WL1273_RDS_CORRECTABLE_ERROR	(1 << 3)
 #define WL1273_RDS_UNCORRECTABLE_ERROR	(1 << 4)
@@ -306,7 +166,7 @@ static int wl1273_fm_rds(struct wl1273_device *radio)
 	if (core->mode != WL1273_MODE_RX)
 		return 0;
 
-	r = wl1273_fm_read_reg(core, WL1273_RDS_SYNC_GET, &val);
+	r = core->read(core, WL1273_RDS_SYNC_GET, &val);
 	if (r)
 		return r;
 
@@ -374,7 +234,7 @@ static irqreturn_t wl1273_fm_irq_thread_handler(int irq, void *dev_id)
 	u16 flags;
 	int r;
 
-	r = wl1273_fm_read_reg(core, WL1273_FLAG_GET, &flags);
+	r = core->read(core, WL1273_FLAG_GET, &flags);
 	if (r)
 		goto out;
 
@@ -398,7 +258,7 @@ static irqreturn_t wl1273_fm_irq_thread_handler(int irq, void *dev_id)
 	if (flags & WL1273_LEV_EVENT) {
 		u16 level;
 
-		r = wl1273_fm_read_reg(core, WL1273_RSSI_LVL_GET, &level);
+		r = core->read(core, WL1273_RSSI_LVL_GET, &level);
 		if (r)
 			goto out;
 
@@ -439,8 +299,8 @@ static irqreturn_t wl1273_fm_irq_thread_handler(int irq, void *dev_id)
 		dev_dbg(radio->dev, "IRQ: FR:\n");
 
 		if (core->mode == WL1273_MODE_RX) {
-			r = wl1273_fm_write_cmd(core, WL1273_TUNER_MODE_SET,
-						TUNER_MODE_STOP_SEARCH);
+			r = core->write(core, WL1273_TUNER_MODE_SET,
+					TUNER_MODE_STOP_SEARCH);
 			if (r) {
 				dev_err(radio->dev,
 					"%s: TUNER_MODE_SET fails: %d\n",
@@ -448,7 +308,7 @@ static irqreturn_t wl1273_fm_irq_thread_handler(int irq, void *dev_id)
 				goto out;
 			}
 
-			r = wl1273_fm_read_reg(core, WL1273_FREQ_SET, &freq);
+			r = core->read(core, WL1273_FREQ_SET, &freq);
 			if (r)
 				goto out;
 
@@ -467,7 +327,7 @@ static irqreturn_t wl1273_fm_irq_thread_handler(int irq, void *dev_id)
 			dev_dbg(radio->dev, "%dkHz\n", radio->rx_frequency);
 
 		} else {
-			r = wl1273_fm_read_reg(core, WL1273_CHANL_SET, &freq);
+			r = core->read(core, WL1273_CHANL_SET, &freq);
 			if (r)
 				goto out;
 
@@ -477,8 +337,7 @@ static irqreturn_t wl1273_fm_irq_thread_handler(int irq, void *dev_id)
 	}
 
 out:
-	wl1273_fm_write_cmd(core, WL1273_INT_MASK_SET,
-			    radio->irq_flags);
+	core->write(core, WL1273_INT_MASK_SET, radio->irq_flags);
 	complete(&radio->busy);
 
 	return IRQ_HANDLED;
@@ -512,7 +371,7 @@ static int wl1273_fm_set_tx_freq(struct wl1273_device *radio, unsigned int freq)
 	dev_dbg(radio->dev, "%s: freq: %d kHz\n", __func__, freq);
 
 	/* Set the current tx channel */
-	r = wl1273_fm_write_cmd(core, WL1273_CHANL_SET, freq / 10);
+	r = core->write(core, WL1273_CHANL_SET, freq / 10);
 	if (r)
 		return r;
 
@@ -526,7 +385,7 @@ static int wl1273_fm_set_tx_freq(struct wl1273_device *radio, unsigned int freq)
 	dev_dbg(radio->dev, "WL1273_CHANL_SET: %d\n", r);
 
 	/* Enable the output power */
-	r = wl1273_fm_write_cmd(core, WL1273_POWER_ENB_SET, 1);
+	r = core->write(core, WL1273_POWER_ENB_SET, 1);
 	if (r)
 		return r;
 
@@ -566,20 +425,20 @@ static int wl1273_fm_set_rx_freq(struct wl1273_device *radio, unsigned int freq)
 
 	dev_dbg(radio->dev, "%s: %dkHz\n", __func__, freq);
 
-	wl1273_fm_write_cmd(core, WL1273_INT_MASK_SET, radio->irq_flags);
+	core->write(core, WL1273_INT_MASK_SET, radio->irq_flags);
 
 	if (radio->band == WL1273_BAND_JAPAN)
 		f = (freq - WL1273_BAND_JAPAN_LOW) / 50;
 	else
 		f = (freq - WL1273_BAND_OTHER_LOW) / 50;
 
-	r = wl1273_fm_write_cmd(core, WL1273_FREQ_SET, f);
+	r = core->write(core, WL1273_FREQ_SET, f);
 	if (r) {
 		dev_err(radio->dev, "FREQ_SET fails\n");
 		goto err;
 	}
 
-	r = wl1273_fm_write_cmd(core, WL1273_TUNER_MODE_SET, TUNER_MODE_PRESET);
+	r = core->write(core, WL1273_TUNER_MODE_SET, TUNER_MODE_PRESET);
 	if (r) {
 		dev_err(radio->dev, "TUNER_MODE_SET fails\n");
 		goto err;
@@ -609,7 +468,7 @@ static int wl1273_fm_get_freq(struct wl1273_device *radio)
 	int r;
 
 	if (core->mode == WL1273_MODE_RX) {
-		r = wl1273_fm_read_reg(core, WL1273_FREQ_SET, &f);
+		r = core->read(core, WL1273_FREQ_SET, &f);
 		if (r)
 			return r;
 
@@ -619,7 +478,7 @@ static int wl1273_fm_get_freq(struct wl1273_device *radio)
 		else
 			freq = WL1273_BAND_OTHER_LOW + 50 * f;
 	} else {
-		r = wl1273_fm_read_reg(core, WL1273_CHANL_SET, &f);
+		r = core->read(core, WL1273_CHANL_SET, &f);
 		if (r)
 			return r;
 
@@ -670,7 +529,7 @@ static int wl1273_fm_upload_firmware_patch(struct wl1273_device *radio)
 	}
 
 	/* ignore possible error here */
-	wl1273_fm_write_cmd(core, WL1273_RESET, 0);
+	core->write(core, WL1273_RESET, 0);
 
 	dev_dbg(dev, "%s - download OK, r: %d\n", __func__, r);
 out:
@@ -683,14 +542,14 @@ static int wl1273_fm_stop(struct wl1273_device *radio)
 	struct wl1273_core *core = radio->core;
 
 	if (core->mode == WL1273_MODE_RX) {
-		int r = wl1273_fm_write_cmd(core, WL1273_POWER_SET,
+		int r = core->write(core, WL1273_POWER_SET,
 				    WL1273_POWER_SET_OFF);
 		if (r)
 			dev_err(radio->dev, "%s: POWER_SET fails: %d\n",
 				__func__, r);
 	} else if (core->mode == WL1273_MODE_TX) {
-		int r = wl1273_fm_write_cmd(core, WL1273_PUPD_SET,
-					    WL1273_PUPD_SET_OFF);
+		int r = core->write(core, WL1273_PUPD_SET,
+				    WL1273_PUPD_SET_OFF);
 		if (r)
 			dev_err(radio->dev,
 				"%s: PUPD_SET fails: %d\n", __func__, r);
@@ -725,11 +584,11 @@ static int wl1273_fm_start(struct wl1273_device *radio, int new_mode)
 			val |= WL1273_POWER_SET_RDS;
 
 		/* If this fails try again */
-		r = wl1273_fm_write_cmd(core, WL1273_POWER_SET, val);
+		r = core->write(core, WL1273_POWER_SET, val);
 		if (r) {
 			msleep(100);
 
-			r = wl1273_fm_write_cmd(core, WL1273_POWER_SET, val);
+			r = core->write(core, WL1273_POWER_SET, val);
 			if (r) {
 				dev_err(dev, "%s: POWER_SET fails\n", __func__);
 				goto fail;
@@ -742,11 +601,10 @@ static int wl1273_fm_start(struct wl1273_device *radio, int new_mode)
 
 	} else if (new_mode == WL1273_MODE_TX) {
 		/* If this fails try again once */
-		r = wl1273_fm_write_cmd(core, WL1273_PUPD_SET,
-					WL1273_PUPD_SET_ON);
+		r = core->write(core, WL1273_PUPD_SET, WL1273_PUPD_SET_ON);
 		if (r) {
 			msleep(100);
-			r = wl1273_fm_write_cmd(core, WL1273_PUPD_SET,
+			r = core->write(core, WL1273_PUPD_SET,
 					WL1273_PUPD_SET_ON);
 			if (r) {
 				dev_err(dev, "%s: PUPD_SET fails\n", __func__);
@@ -755,9 +613,9 @@ static int wl1273_fm_start(struct wl1273_device *radio, int new_mode)
 		}
 
 		if (radio->rds_on)
-			r = wl1273_fm_write_cmd(core, WL1273_RDS_DATA_ENB, 1);
+			r = core->write(core, WL1273_RDS_DATA_ENB, 1);
 		else
-			r = wl1273_fm_write_cmd(core, WL1273_RDS_DATA_ENB, 0);
+			r = core->write(core, WL1273_RDS_DATA_ENB, 0);
 	} else {
 		dev_warn(dev, "%s: Illegal mode.\n", __func__);
 	}
@@ -777,14 +635,14 @@ static int wl1273_fm_start(struct wl1273_device *radio, int new_mode)
 			if (radio->rds_on)
 				val |= WL1273_POWER_SET_RDS;
 
-			r = wl1273_fm_write_cmd(core, WL1273_POWER_SET, val);
+			r = core->write(core, WL1273_POWER_SET, val);
 			if (r) {
 				dev_err(dev, "%s: POWER_SET fails\n", __func__);
 				goto fail;
 			}
 		} else if (new_mode == WL1273_MODE_TX) {
-			r = wl1273_fm_write_cmd(core, WL1273_PUPD_SET,
-						WL1273_PUPD_SET_ON);
+			r = core->write(core, WL1273_PUPD_SET,
+					WL1273_PUPD_SET_ON);
 			if (r) {
 				dev_err(dev, "%s: PUPD_SET fails\n", __func__);
 				goto fail;
@@ -808,10 +666,10 @@ static int wl1273_fm_suspend(struct wl1273_device *radio)
 
 	/* Cannot go from OFF to SUSPENDED */
 	if (core->mode == WL1273_MODE_RX)
-		r = wl1273_fm_write_cmd(core, WL1273_POWER_SET,
+		r = core->write(core, WL1273_POWER_SET,
 				WL1273_POWER_SET_RETENTION);
 	else if (core->mode == WL1273_MODE_TX)
-		r = wl1273_fm_write_cmd(core, WL1273_PUPD_SET,
+		r = core->write(core, WL1273_PUPD_SET,
 				WL1273_PUPD_SET_RETENTION);
 	else
 		r = -EINVAL;
@@ -852,8 +710,7 @@ static int wl1273_fm_set_mode(struct wl1273_device *radio, int mode)
 		}
 
 		core->mode = mode;
-		r = wl1273_fm_write_cmd(core, WL1273_INT_MASK_SET,
-					radio->irq_flags);
+		r = core->write(core, WL1273_INT_MASK_SET, radio->irq_flags);
 		if (r) {
 			dev_err(dev, "INT_MASK_SET fails.\n");
 			goto out;
@@ -951,22 +808,21 @@ static int wl1273_fm_set_seek(struct wl1273_device *radio,
 	INIT_COMPLETION(radio->busy);
 	dev_dbg(radio->dev, "%s: BUSY\n", __func__);
 
-	r = wl1273_fm_write_cmd(core, WL1273_INT_MASK_SET, radio->irq_flags);
+	r = core->write(core, WL1273_INT_MASK_SET, radio->irq_flags);
 	if (r)
 		goto out;
 
 	dev_dbg(radio->dev, "%s\n", __func__);
 
-	r = wl1273_fm_write_cmd(core, WL1273_SEARCH_LVL_SET, level);
+	r = core->write(core, WL1273_SEARCH_LVL_SET, level);
 	if (r)
 		goto out;
 
-	r = wl1273_fm_write_cmd(core, WL1273_SEARCH_DIR_SET, dir);
+	r = core->write(core, WL1273_SEARCH_DIR_SET, dir);
 	if (r)
 		goto out;
 
-	r = wl1273_fm_write_cmd(core, WL1273_TUNER_MODE_SET,
-				TUNER_MODE_AUTO_SEEK);
+	r = core->write(core, WL1273_TUNER_MODE_SET, TUNER_MODE_AUTO_SEEK);
 	if (r)
 		goto out;
 
@@ -994,8 +850,7 @@ static int wl1273_fm_set_seek(struct wl1273_device *radio,
 	INIT_COMPLETION(radio->busy);
 	dev_dbg(radio->dev, "%s: BUSY\n", __func__);
 
-	r = wl1273_fm_write_cmd(core, WL1273_TUNER_MODE_SET,
-				TUNER_MODE_AUTO_SEEK);
+	r = core->write(core, WL1273_TUNER_MODE_SET, TUNER_MODE_AUTO_SEEK);
 	if (r)
 		goto out;
 
@@ -1020,7 +875,7 @@ static unsigned int wl1273_fm_get_tx_ctune(struct wl1273_device *radio)
 	    core->mode == WL1273_MODE_SUSPENDED)
 		return -EPERM;
 
-	r = wl1273_fm_read_reg(core, WL1273_READ_FMANT_TUNE_VALUE, &val);
+	r = core->read(core, WL1273_READ_FMANT_TUNE_VALUE, &val);
 	if (r) {
 		dev_err(dev, "%s: read error: %d\n", __func__, r);
 		goto out;
@@ -1066,7 +921,7 @@ static int wl1273_fm_set_preemphasis(struct wl1273_device *radio,
 		goto out;
 	}
 
-	r = wl1273_fm_write_cmd(core, WL1273_PREMPH_SET, em);
+	r = core->write(core, WL1273_PREMPH_SET, em);
 	if (r)
 		goto out;
 
@@ -1086,7 +941,7 @@ static int wl1273_fm_rds_on(struct wl1273_device *radio)
 	if (radio->rds_on)
 		return 0;
 
-	r = wl1273_fm_write_cmd(core, WL1273_POWER_SET,
+	r = core->write(core, WL1273_POWER_SET,
 			WL1273_POWER_SET_FM | WL1273_POWER_SET_RDS);
 	if (r)
 		goto out;
@@ -1108,7 +963,7 @@ static int wl1273_fm_rds_off(struct wl1273_device *radio)
 
 	radio->irq_flags &= ~WL1273_RDS_EVENT;
 
-	r = wl1273_fm_write_cmd(core, WL1273_INT_MASK_SET, radio->irq_flags);
+	r = core->write(core, WL1273_INT_MASK_SET, radio->irq_flags);
 	if (r)
 		goto out;
 
@@ -1120,7 +975,7 @@ static int wl1273_fm_rds_off(struct wl1273_device *radio)
 
 	dev_dbg(radio->dev, "%s\n", __func__);
 
-	r = wl1273_fm_write_cmd(core, WL1273_POWER_SET, WL1273_POWER_SET_FM);
+	r = core->write(core, WL1273_POWER_SET, WL1273_POWER_SET_FM);
 	if (r)
 		goto out;
 
@@ -1143,14 +998,14 @@ static int wl1273_fm_set_rds(struct wl1273_device *radio, unsigned int new_mode)
 		return -EPERM;
 
 	if (new_mode == WL1273_RDS_RESET) {
-		r = wl1273_fm_write_cmd(core, WL1273_RDS_CNTRL_SET, 1);
+		r = core->write(core, WL1273_RDS_CNTRL_SET, 1);
 		return r;
 	}
 
 	if (core->mode == WL1273_MODE_TX && new_mode == WL1273_RDS_OFF) {
-		r = wl1273_fm_write_cmd(core, WL1273_RDS_DATA_ENB, 0);
+		r = core->write(core, WL1273_RDS_DATA_ENB, 0);
 	} else if (core->mode == WL1273_MODE_TX && new_mode == WL1273_RDS_ON) {
-		r = wl1273_fm_write_cmd(core, WL1273_RDS_DATA_ENB, 1);
+		r = core->write(core, WL1273_RDS_DATA_ENB, 1);
 	} else if (core->mode == WL1273_MODE_RX && new_mode == WL1273_RDS_OFF) {
 		r = wl1273_fm_rds_off(radio);
 	} else if (core->mode == WL1273_MODE_RX && new_mode == WL1273_RDS_ON) {
@@ -1171,12 +1026,13 @@ static ssize_t wl1273_fm_fops_write(struct file *file, const char __user *buf,
 				    size_t count, loff_t *ppos)
 {
 	struct wl1273_device *radio = video_get_drvdata(video_devdata(file));
+	struct wl1273_core *core = radio->core;
 	u16 val;
 	int r;
 
 	dev_dbg(radio->dev, "%s\n", __func__);
 
-	if (radio->core->mode != WL1273_MODE_TX)
+	if (core->mode != WL1273_MODE_TX)
 		return count;
 
 	if (radio->rds_users == 0) {
@@ -1184,7 +1040,7 @@ static ssize_t wl1273_fm_fops_write(struct file *file, const char __user *buf,
 		return 0;
 	}
 
-	if (mutex_lock_interruptible(&radio->core->lock))
+	if (mutex_lock_interruptible(&core->lock))
 		return -EINTR;
 	/*
 	 * Multiple processes can open the device, but only
@@ -1202,7 +1058,7 @@ static ssize_t wl1273_fm_fops_write(struct file *file, const char __user *buf,
 	else
 		val = count;
 
-	wl1273_fm_write_cmd(radio->core, WL1273_RDS_CONFIG_DATA_SET, val);
+	core->write(core, WL1273_RDS_CONFIG_DATA_SET, val);
 
 	if (copy_from_user(radio->write_buf + 1, buf, val)) {
 		r = -EFAULT;
@@ -1213,11 +1069,11 @@ static ssize_t wl1273_fm_fops_write(struct file *file, const char __user *buf,
 	dev_dbg(radio->dev, "From user: \"%s\"\n", radio->write_buf);
 
 	radio->write_buf[0] = WL1273_RDS_DATA_SET;
-	wl1273_fm_write_data(radio->core, radio->write_buf, val + 1);
+	core->write_data(core, radio->write_buf, val + 1);
 
 	r = val;
 out:
-	mutex_unlock(&radio->core->lock);
+	mutex_unlock(&core->lock);
 
 	return r;
 }
@@ -1263,8 +1119,8 @@ static int wl1273_fm_fops_open(struct file *file)
 
 		radio->irq_flags |= WL1273_RDS_EVENT;
 
-		r = wl1273_fm_write_cmd(core, WL1273_INT_MASK_SET,
-					radio->irq_flags);
+		r = core->write(core, WL1273_INT_MASK_SET,
+				radio->irq_flags);
 		if (r) {
 			mutex_unlock(&core->lock);
 			goto out;
@@ -1295,9 +1151,9 @@ static int wl1273_fm_fops_release(struct file *file)
 			radio->irq_flags &= ~WL1273_RDS_EVENT;
 
 			if (core->mode == WL1273_MODE_RX) {
-				r = wl1273_fm_write_cmd(core,
-							WL1273_INT_MASK_SET,
-							radio->irq_flags);
+				r = core->write(core,
+						WL1273_INT_MASK_SET,
+						radio->irq_flags);
 				if (r) {
 					mutex_unlock(&core->lock);
 					goto out;
@@ -1324,7 +1180,7 @@ static ssize_t wl1273_fm_fops_read(struct file *file, char __user *buf,
 
 	dev_dbg(radio->dev, "%s\n", __func__);
 
-	if (radio->core->mode != WL1273_MODE_RX)
+	if (core->mode != WL1273_MODE_RX)
 		return 0;
 
 	if (radio->rds_users == 0) {
@@ -1345,7 +1201,7 @@ static ssize_t wl1273_fm_fops_read(struct file *file, char __user *buf,
 	}
 	radio->owner = file;
 
-	r = wl1273_fm_read_reg(core, WL1273_RDS_SYNC_GET, &val);
+	r = core->read(core, WL1273_RDS_SYNC_GET, &val);
 	if (r) {
 		dev_err(radio->dev, "%s: Get RDS_SYNC fails.\n", __func__);
 		goto out;
@@ -1466,23 +1322,24 @@ static int wl1273_fm_vidioc_s_input(struct file *file, void *priv,
  */
 static int wl1273_fm_set_tx_power(struct wl1273_device *radio, u16 power)
 {
+	struct wl1273_core *core = radio->core;
 	int r;
 
-	if (radio->core->mode == WL1273_MODE_OFF ||
-	    radio->core->mode == WL1273_MODE_SUSPENDED)
+	if (core->mode == WL1273_MODE_OFF ||
+	    core->mode == WL1273_MODE_SUSPENDED)
 		return -EPERM;
 
-	mutex_lock(&radio->core->lock);
+	mutex_lock(&core->lock);
 
 	/* Convert the dBuV value to chip presentation */
-	r = wl1273_fm_write_cmd(radio->core, WL1273_POWER_LEV_SET, 122 - power);
+	r = core->write(core, WL1273_POWER_LEV_SET, 122 - power);
 	if (r)
 		goto out;
 
 	radio->tx_power = power;
 
 out:
-	mutex_unlock(&radio->core->lock);
+	mutex_unlock(&core->lock);
 	return r;
 }
 
@@ -1493,23 +1350,24 @@ out:
 static int wl1273_fm_tx_set_spacing(struct wl1273_device *radio,
 				    unsigned int spacing)
 {
+	struct wl1273_core *core = radio->core;
 	int r;
 
 	if (spacing == 0) {
-		r = wl1273_fm_write_cmd(radio->core, WL1273_SCAN_SPACING_SET,
-					WL1273_SPACING_100kHz);
+		r = core->write(core, WL1273_SCAN_SPACING_SET,
+				WL1273_SPACING_100kHz);
 		radio->spacing = 100;
 	} else if (spacing - 50000 < 25000) {
-		r = wl1273_fm_write_cmd(radio->core, WL1273_SCAN_SPACING_SET,
-					WL1273_SPACING_50kHz);
+		r = core->write(core, WL1273_SCAN_SPACING_SET,
+				WL1273_SPACING_50kHz);
 		radio->spacing = 50;
 	} else if (spacing - 100000 < 50000) {
-		r = wl1273_fm_write_cmd(radio->core, WL1273_SCAN_SPACING_SET,
-					WL1273_SPACING_100kHz);
+		r = core->write(core, WL1273_SCAN_SPACING_SET,
+				WL1273_SPACING_100kHz);
 		radio->spacing = 100;
 	} else {
-		r = wl1273_fm_write_cmd(radio->core, WL1273_SCAN_SPACING_SET,
-					WL1273_SPACING_200kHz);
+		r = core->write(core, WL1273_SCAN_SPACING_SET,
+				WL1273_SPACING_200kHz);
 		radio->spacing = 200;
 	}
 
@@ -1567,17 +1425,17 @@ static int wl1273_fm_vidioc_s_ctrl(struct v4l2_ctrl *ctrl)
 			return -EINTR;
 
 		if (core->mode == WL1273_MODE_RX && ctrl->val)
-			r = wl1273_fm_write_cmd(core,
-						WL1273_MUTE_STATUS_SET,
-						WL1273_MUTE_HARD_LEFT |
-						WL1273_MUTE_HARD_RIGHT);
+			r = core->write(core,
+					WL1273_MUTE_STATUS_SET,
+					WL1273_MUTE_HARD_LEFT |
+					WL1273_MUTE_HARD_RIGHT);
 		else if (core->mode == WL1273_MODE_RX)
-			r = wl1273_fm_write_cmd(core,
-						WL1273_MUTE_STATUS_SET, 0x0);
+			r = core->write(core,
+					WL1273_MUTE_STATUS_SET, 0x0);
 		else if (core->mode == WL1273_MODE_TX && ctrl->val)
-			r = wl1273_fm_write_cmd(core, WL1273_MUTE, 1);
+			r = core->write(core, WL1273_MUTE, 1);
 		else if (core->mode == WL1273_MODE_TX)
-			r = wl1273_fm_write_cmd(core, WL1273_MUTE, 0);
+			r = core->write(core, WL1273_MUTE, 0);
 
 		mutex_unlock(&core->lock);
 		break;
@@ -1672,7 +1530,7 @@ static int wl1273_fm_vidioc_g_tuner(struct file *file, void *priv,
 	if (mutex_lock_interruptible(&core->lock))
 		return -EINTR;
 
-	r = wl1273_fm_read_reg(core, WL1273_STEREO_GET, &val);
+	r = core->read(core, WL1273_STEREO_GET, &val);
 	if (r)
 		goto out;
 
@@ -1681,7 +1539,7 @@ static int wl1273_fm_vidioc_g_tuner(struct file *file, void *priv,
 	else
 		tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
 
-	r = wl1273_fm_read_reg(core, WL1273_RSSI_LVL_GET, &val);
+	r = core->read(core, WL1273_RSSI_LVL_GET, &val);
 	if (r)
 		goto out;
 
@@ -1690,7 +1548,7 @@ static int wl1273_fm_vidioc_g_tuner(struct file *file, void *priv,
 
 	tuner->afc = 0;
 
-	r = wl1273_fm_read_reg(core, WL1273_RDS_SYNC_GET, &val);
+	r = core->read(core, WL1273_RDS_SYNC_GET, &val);
 	if (r)
 		goto out;
 
@@ -1736,8 +1594,7 @@ static int wl1273_fm_vidioc_s_tuner(struct file *file, void *priv,
 		dev_warn(radio->dev, "%s: RDS fails: %d\n", __func__, r);
 
 	if (tuner->audmode == V4L2_TUNER_MODE_MONO) {
-		r = wl1273_fm_write_cmd(core, WL1273_MOST_MODE_SET,
-					WL1273_RX_MONO);
+		r = core->write(core, WL1273_MOST_MODE_SET, WL1273_RX_MONO);
 		if (r < 0) {
 			dev_warn(radio->dev, "%s: MOST_MODE fails: %d\n",
 				 __func__, r);
@@ -1745,8 +1602,7 @@ static int wl1273_fm_vidioc_s_tuner(struct file *file, void *priv,
 		}
 		radio->stereo = false;
 	} else if (tuner->audmode == V4L2_TUNER_MODE_STEREO) {
-		r = wl1273_fm_write_cmd(core, WL1273_MOST_MODE_SET,
-					WL1273_RX_STEREO);
+		r = core->write(core, WL1273_MOST_MODE_SET, WL1273_RX_STEREO);
 		if (r < 0) {
 			dev_warn(radio->dev, "%s: MOST_MODE fails: %d\n",
 				 __func__, r);
@@ -1885,10 +1741,10 @@ static int wl1273_fm_vidioc_s_modulator(struct file *file, void *priv,
 		r = wl1273_fm_set_rds(radio, WL1273_RDS_OFF);
 
 	if (modulator->txsubchans & V4L2_TUNER_SUB_MONO)
-		r = wl1273_fm_write_cmd(core, WL1273_MONO_SET, WL1273_TX_MONO);
+		r = core->write(core, WL1273_MONO_SET, WL1273_TX_MONO);
 	else
-		r = wl1273_fm_write_cmd(core, WL1273_MONO_SET,
-					WL1273_RX_STEREO);
+		r = core->write(core, WL1273_MONO_SET,
+				WL1273_RX_STEREO);
 	if (r < 0)
 		dev_warn(radio->dev, WL1273_FM_DRIVER_NAME
 			 "MONO_SET fails: %d\n", r);
@@ -1923,7 +1779,7 @@ static int wl1273_fm_vidioc_g_modulator(struct file *file, void *priv,
 	if (mutex_lock_interruptible(&core->lock))
 		return -EINTR;
 
-	r = wl1273_fm_read_reg(core, WL1273_MONO_SET, &val);
+	r = core->read(core, WL1273_MONO_SET, &val);
 	if (r)
 		goto out;
 
@@ -1960,38 +1816,38 @@ static int wl1273_fm_vidioc_log_status(struct file *file, void *priv)
 		return 0;
 	}
 
-	r = wl1273_fm_read_reg(core, WL1273_ASIC_ID_GET, &val);
+	r = core->read(core, WL1273_ASIC_ID_GET, &val);
 	if (r)
 		dev_err(dev, "%s: Get ASIC_ID fails.\n", __func__);
 	else
 		dev_info(dev, "ASIC_ID: 0x%04x\n", val);
 
-	r = wl1273_fm_read_reg(core, WL1273_ASIC_VER_GET, &val);
+	r = core->read(core, WL1273_ASIC_VER_GET, &val);
 	if (r)
 		dev_err(dev, "%s: Get ASIC_VER fails.\n", __func__);
 	else
 		dev_info(dev, "ASIC Version: 0x%04x\n", val);
 
-	r = wl1273_fm_read_reg(core, WL1273_FIRM_VER_GET, &val);
+	r = core->read(core, WL1273_FIRM_VER_GET, &val);
 	if (r)
 		dev_err(dev, "%s: Get FIRM_VER fails.\n", __func__);
 	else
 		dev_info(dev, "FW version: %d(0x%04x)\n", val, val);
 
-	r = wl1273_fm_read_reg(core, WL1273_BAND_SET, &val);
+	r = core->read(core, WL1273_BAND_SET, &val);
 	if (r)
 		dev_err(dev, "%s: Get BAND fails.\n", __func__);
 	else
 		dev_info(dev, "BAND: %d\n", val);
 
 	if (core->mode == WL1273_MODE_TX) {
-		r = wl1273_fm_read_reg(core, WL1273_PUPD_SET, &val);
+		r = core->read(core, WL1273_PUPD_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get PUPD fails.\n", __func__);
 		else
 			dev_info(dev, "PUPD: 0x%04x\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_CHANL_SET, &val);
+		r = core->read(core, WL1273_CHANL_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get CHANL fails.\n", __func__);
 		else
@@ -1999,13 +1855,13 @@ static int wl1273_fm_vidioc_log_status(struct file *file, void *priv)
 	} else if (core->mode == WL1273_MODE_RX) {
 		int bf = radio->rangelow;
 
-		r = wl1273_fm_read_reg(core, WL1273_FREQ_SET, &val);
+		r = core->read(core, WL1273_FREQ_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get FREQ fails.\n", __func__);
 		else
 			dev_info(dev, "RX Frequency: %dkHz\n", bf + val*50);
 
-		r = wl1273_fm_read_reg(core, WL1273_MOST_MODE_SET, &val);
+		r = core->read(core, WL1273_MOST_MODE_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get MOST_MODE fails.\n",
 				__func__);
@@ -2016,7 +1872,7 @@ static int wl1273_fm_vidioc_log_status(struct file *file, void *priv)
 		else
 			dev_info(dev, "MOST_MODE: Unexpected value: %d\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_MOST_BLEND_SET, &val);
+		r = core->read(core, WL1273_MOST_BLEND_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get MOST_BLEND fails.\n", __func__);
 		else if (val == 0)
@@ -2027,7 +1883,7 @@ static int wl1273_fm_vidioc_log_status(struct file *file, void *priv)
 		else
 			dev_info(dev, "MOST_BLEND: Unexpected val: %d\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_STEREO_GET, &val);
+		r = core->read(core, WL1273_STEREO_GET, &val);
 		if (r)
 			dev_err(dev, "%s: Get STEREO fails.\n", __func__);
 		else if (val == 0)
@@ -2037,25 +1893,25 @@ static int wl1273_fm_vidioc_log_status(struct file *file, void *priv)
 		else
 			dev_info(dev, "STEREO: Unexpected value: %d\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_RSSI_LVL_GET, &val);
+		r = core->read(core, WL1273_RSSI_LVL_GET, &val);
 		if (r)
 			dev_err(dev, "%s: Get RSSI_LVL fails.\n", __func__);
 		else
 			dev_info(dev, "RX signal strength: %d\n", (s16) val);
 
-		r = wl1273_fm_read_reg(core, WL1273_POWER_SET, &val);
+		r = core->read(core, WL1273_POWER_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get POWER fails.\n", __func__);
 		else
 			dev_info(dev, "POWER: 0x%04x\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_INT_MASK_SET, &val);
+		r = core->read(core, WL1273_INT_MASK_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get INT_MASK fails.\n", __func__);
 		else
 			dev_info(dev, "INT_MASK: 0x%04x\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_RDS_SYNC_GET, &val);
+		r = core->read(core, WL1273_RDS_SYNC_GET, &val);
 		if (r)
 			dev_err(dev, "%s: Get RDS_SYNC fails.\n",
 				__func__);
@@ -2067,14 +1923,14 @@ static int wl1273_fm_vidioc_log_status(struct file *file, void *priv)
 		else
 			dev_info(dev, "RDS_SYNC: Unexpected value: %d\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_I2S_MODE_CONFIG_SET, &val);
+		r = core->read(core, WL1273_I2S_MODE_CONFIG_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get I2S_MODE_CONFIG fails.\n",
 				__func__);
 		else
 			dev_info(dev, "I2S_MODE_CONFIG: 0x%04x\n", val);
 
-		r = wl1273_fm_read_reg(core, WL1273_VOLUME_SET, &val);
+		r = core->read(core, WL1273_VOLUME_SET, &val);
 		if (r)
 			dev_err(dev, "%s: Get VOLUME fails.\n", __func__);
 		else
@@ -2138,7 +1994,7 @@ static int wl1273_fm_radio_remove(struct platform_device *pdev)
 
 static int __devinit wl1273_fm_radio_probe(struct platform_device *pdev)
 {
-	struct wl1273_core **core = pdev->dev.platform_data;
+	struct wl1273_core **core = mfd_get_data(pdev);
 	struct wl1273_device *radio;
 	struct v4l2_ctrl *ctrl;
 	int r = 0;
@@ -2184,10 +2040,6 @@ static int __devinit wl1273_fm_radio_probe(struct platform_device *pdev)
 	radio->stereo = true;
 	radio->bus_type = "I2C";
 
-	radio->core->write = wl1273_fm_write_cmd;
-	radio->core->set_audio = wl1273_fm_set_audio;
-	radio->core->set_volume = wl1273_fm_set_volume;
-
 	if (radio->core->pdata->request_resources) {
 		r = radio->core->pdata->request_resources(radio->core->client);
 		if (r) {
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 60c176fe328e..38ae6cd65790 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -460,7 +460,6 @@ static ssize_t si470x_fops_read(struct file *file, char __user *buf,
 	count /= 3;
 
 	/* copy RDS block out of internal buffer and to user buffer */
-	mutex_lock(&radio->lock);
 	while (block_count < count) {
 		if (radio->rd_index == radio->wr_index)
 			break;
diff --git a/drivers/media/radio/wl128x/Kconfig b/drivers/media/radio/wl128x/Kconfig
new file mode 100644
index 000000000000..749f67b192e7
--- /dev/null
+++ b/drivers/media/radio/wl128x/Kconfig
@@ -0,0 +1,17 @@
+#
+# TI's wl128x FM driver based on TI's ST driver.
+#
+menu "Texas Instruments WL128x FM driver (ST based)"
+config RADIO_WL128X
+	tristate "Texas Instruments WL128x FM Radio"
+	depends on VIDEO_V4L2 && RFKILL
+	select TI_ST
+	help
+	Choose Y here if you have this FM radio chip.
+
+	In order to control your radio card, you will need to use programs
+	that are compatible with the Video For Linux 2 API.  Information on
+	this API and pointers to "v4l2" programs may be found at
+	<file:Documentation/video4linux/API.html>.
+
+endmenu
diff --git a/drivers/media/radio/wl128x/Makefile b/drivers/media/radio/wl128x/Makefile
new file mode 100644
index 000000000000..32a0ead09845
--- /dev/null
+++ b/drivers/media/radio/wl128x/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for TI's shared transport driver based wl128x
+# FM radio.
+#
+obj-$(CONFIG_RADIO_WL128X)	+= fm_drv.o
+fm_drv-objs		:= fmdrv_common.o fmdrv_rx.o fmdrv_tx.o fmdrv_v4l2.o
diff --git a/drivers/media/radio/wl128x/fmdrv.h b/drivers/media/radio/wl128x/fmdrv.h
new file mode 100644
index 000000000000..5db6fd14cf3c
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv.h
@@ -0,0 +1,244 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *
+ *  Common header for all FM driver sub-modules.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef _FM_DRV_H
+#define _FM_DRV_H
+
+#include <linux/skbuff.h>
+#include <linux/interrupt.h>
+#include <sound/core.h>
+#include <sound/initval.h>
+#include <linux/timer.h>
+#include <linux/version.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+
+#define FM_DRV_VERSION            "0.10"
+/* Should match with FM_DRV_VERSION */
+#define FM_DRV_RADIO_VERSION      KERNEL_VERSION(0, 0, 1)
+#define FM_DRV_NAME               "ti_fmdrv"
+#define FM_DRV_CARD_SHORT_NAME    "TI FM Radio"
+#define FM_DRV_CARD_LONG_NAME     "Texas Instruments FM Radio"
+
+/* Flag info */
+#define FM_INTTASK_RUNNING            0
+#define FM_INTTASK_SCHEDULE_PENDING   1
+#define FM_FW_DW_INPROGRESS     2
+#define FM_CORE_READY                 3
+#define FM_CORE_TRANSPORT_READY       4
+#define FM_AF_SWITCH_INPROGRESS	      5
+#define FM_CORE_TX_XMITING	      6
+
+#define FM_TUNE_COMPLETE	      0x1
+#define FM_BAND_LIMIT		      0x2
+
+#define FM_DRV_TX_TIMEOUT      (5*HZ)	/* 5 seconds */
+#define FM_DRV_RX_SEEK_TIMEOUT (20*HZ)	/* 20 seconds */
+
+#define NO_OF_ENTRIES_IN_ARRAY(array) (sizeof(array) / sizeof(array[0]))
+
+#define fmerr(format, ...) \
+	printk(KERN_ERR "fmdrv: " format, ## __VA_ARGS__)
+#define fmwarn(format, ...) \
+	printk(KERN_WARNING "fmdrv: " format, ##__VA_ARGS__)
+#ifdef DEBUG
+#define fmdbg(format, ...) \
+	printk(KERN_DEBUG "fmdrv: " format, ## __VA_ARGS__)
+#else /* DEBUG */
+#define fmdbg(format, ...)
+#endif
+enum {
+	FM_MODE_OFF,
+	FM_MODE_TX,
+	FM_MODE_RX,
+	FM_MODE_ENTRY_MAX
+};
+
+#define FM_RX_RDS_INFO_FIELD_MAX	8	/* 4 Group * 2 Bytes */
+
+/* RX RDS data format */
+struct fm_rdsdata_format {
+	union {
+		struct {
+			u8 buff[FM_RX_RDS_INFO_FIELD_MAX];
+		} groupdatabuff;
+		struct {
+			u16 pidata;
+			u8 blk_b[2];
+			u8 blk_c[2];
+			u8 blk_d[2];
+		} groupgeneral;
+		struct {
+			u16 pidata;
+			u8 blk_b[2];
+			u8 af[2];
+			u8 ps[2];
+		} group0A;
+		struct {
+			u16 pi[2];
+			u8 blk_b[2];
+			u8 ps[2];
+		} group0B;
+	} data;
+};
+
+/* FM region (Europe/US, Japan) info */
+struct region_info {
+	u32 chanl_space;
+	u32 bot_freq;
+	u32 top_freq;
+	u8 fm_band;
+};
+struct fmdev;
+typedef void (*int_handler_prototype) (struct fmdev *);
+
+/* FM Interrupt processing related info */
+struct fm_irq {
+	u8 stage;
+	u16 flag;	/* FM interrupt flag */
+	u16 mask;	/* FM interrupt mask */
+	/* Interrupt process timeout handler */
+	struct timer_list timer;
+	u8 retry;
+	int_handler_prototype *handlers;
+};
+
+/* RDS info */
+struct fm_rds {
+	u8 flag;	/* RX RDS on/off status */
+	u8 last_blk_idx;	/* Last received RDS block */
+
+	/* RDS buffer */
+	wait_queue_head_t read_queue;
+	u32 buf_size;	/* Size is always multiple of 3 */
+	u32 wr_idx;
+	u32 rd_idx;
+	u8 *buff;
+};
+
+#define FM_RDS_MAX_AF_LIST		25
+
+/*
+ * Current RX channel Alternate Frequency cache.
+ * This info is used to switch to other freq (AF)
+ * when current channel signal strengh is below RSSI threshold.
+ */
+struct tuned_station_info {
+	u16 picode;
+	u32 af_cache[FM_RDS_MAX_AF_LIST];
+	u8 afcache_size;
+	u8 af_list_max;
+};
+
+/* FM RX mode info */
+struct fm_rx {
+	struct region_info region;	/* Current selected band */
+	u32 freq;	/* Current RX frquency */
+	u8 mute_mode;	/* Current mute mode */
+	u8 deemphasis_mode; /* Current deemphasis mode */
+	/* RF dependent soft mute mode */
+	u8 rf_depend_mute;
+	u16 volume;	/* Current volume level */
+	u16 rssi_threshold;	/* Current RSSI threshold level */
+	/* Holds the index of the current AF jump */
+	u8 afjump_idx;
+	/* Will hold the frequency before the jump */
+	u32 freq_before_jump;
+	u8 rds_mode;	/* RDS operation mode (RDS/RDBS) */
+	u8 af_mode;	/* Alternate frequency on/off */
+	struct tuned_station_info stat_info;
+	struct fm_rds rds;
+};
+
+#define FMTX_RDS_TXT_STR_SIZE	25
+/*
+ * FM TX RDS data
+ *
+ * @ text_type: is the text following PS or RT
+ * @ text: radio text string which could either be PS or RT
+ * @ af_freq: alternate frequency for Tx
+ * TODO: to be declared in application
+ */
+struct tx_rds {
+	u8 text_type;
+	u8 text[FMTX_RDS_TXT_STR_SIZE];
+	u8 flag;
+	u32 af_freq;
+};
+/*
+ * FM TX global data
+ *
+ * @ pwr_lvl: Power Level of the Transmission from mixer control
+ * @ xmit_state: Transmission state = Updated locally upon Start/Stop
+ * @ audio_io: i2S/Analog
+ * @ tx_frq: Transmission frequency
+ */
+struct fmtx_data {
+	u8 pwr_lvl;
+	u8 xmit_state;
+	u8 audio_io;
+	u8 region;
+	u16 aud_mode;
+	u32 preemph;
+	u32 tx_frq;
+	struct tx_rds rds;
+};
+
+/* FM driver operation structure */
+struct fmdev {
+	struct video_device *radio_dev;	/* V4L2 video device pointer */
+	struct snd_card *card;	/* Card which holds FM mixer controls */
+	u16 asci_id;
+	spinlock_t rds_buff_lock; /* To protect access to RDS buffer */
+	spinlock_t resp_skb_lock; /* To protect access to received SKB */
+
+	long flag;		/*  FM driver state machine info */
+	u8 streg_cbdata; /* status of ST registration */
+
+	struct sk_buff_head rx_q;	/* RX queue */
+	struct tasklet_struct rx_task;	/* RX Tasklet */
+
+	struct sk_buff_head tx_q;	/* TX queue */
+	struct tasklet_struct tx_task;	/* TX Tasklet */
+	unsigned long last_tx_jiffies;	/* Timestamp of last pkt sent */
+	atomic_t tx_cnt;	/* Number of packets can send at a time */
+
+	struct sk_buff *resp_skb;	/* Response from the chip */
+	/* Main task completion handler */
+	struct completion maintask_comp;
+	/* Opcode of last command sent to the chip */
+	u8 pre_op;
+	/* Handler used for wakeup when response packet is received */
+	struct completion *resp_comp;
+	struct fm_irq irq_info;
+	u8 curr_fmmode; /* Current FM chip mode (TX, RX, OFF) */
+	struct fm_rx rx;	/* FM receiver info */
+	struct fmtx_data tx_data;
+
+	/* V4L2 ctrl framwork handler*/
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* For core assisted locking */
+	struct mutex mutex;
+};
+#endif
diff --git a/drivers/media/radio/wl128x/fmdrv_common.c b/drivers/media/radio/wl128x/fmdrv_common.c
new file mode 100644
index 000000000000..12f4c656e8e8
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_common.c
@@ -0,0 +1,1677 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *
+ *  This sub-module of FM driver is common for FM RX and TX
+ *  functionality. This module is responsible for:
+ *  1) Forming group of Channel-8 commands to perform particular
+ *     functionality (eg., frequency set require more than
+ *     one Channel-8 command to be sent to the chip).
+ *  2) Sending each Channel-8 command to the chip and reading
+ *     response back over Shared Transport.
+ *  3) Managing TX and RX Queues and Tasklets.
+ *  4) Handling FM Interrupt packet and taking appropriate action.
+ *  5) Loading FM firmware to the chip (common, FM TX, and FM RX
+ *     firmware files based on mode selection)
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *  Author: Raja Mani <raja_mani@ti.com>
+ *  Author: Manjunatha Halli <manjunatha_halli@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, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/firmware.h>
+#include <linux/delay.h>
+#include "fmdrv.h"
+#include "fmdrv_v4l2.h"
+#include "fmdrv_common.h"
+#include <linux/ti_wilink_st.h>
+#include "fmdrv_rx.h"
+#include "fmdrv_tx.h"
+
+/* Region info */
+static struct region_info region_configs[] = {
+	/* Europe/US */
+	{
+	 .chanl_space = FM_CHANNEL_SPACING_200KHZ * FM_FREQ_MUL,
+	 .bot_freq = 87500,	/* 87.5 MHz */
+	 .top_freq = 108000,	/* 108 MHz */
+	 .fm_band = 0,
+	 },
+	/* Japan */
+	{
+	 .chanl_space = FM_CHANNEL_SPACING_200KHZ * FM_FREQ_MUL,
+	 .bot_freq = 76000,	/* 76 MHz */
+	 .top_freq = 90000,	/* 90 MHz */
+	 .fm_band = 1,
+	 },
+};
+
+/* Band selection */
+static u8 default_radio_region;	/* Europe/US */
+module_param(default_radio_region, byte, 0);
+MODULE_PARM_DESC(default_radio_region, "Region: 0=Europe/US, 1=Japan");
+
+/* RDS buffer blocks */
+static u32 default_rds_buf = 300;
+module_param(default_rds_buf, uint, 0444);
+MODULE_PARM_DESC(rds_buf, "RDS buffer entries");
+
+/* Radio Nr */
+static u32 radio_nr = -1;
+module_param(radio_nr, int, 0444);
+MODULE_PARM_DESC(radio_nr, "Radio Nr");
+
+/* FM irq handlers forward declaration */
+static void fm_irq_send_flag_getcmd(struct fmdev *);
+static void fm_irq_handle_flag_getcmd_resp(struct fmdev *);
+static void fm_irq_handle_hw_malfunction(struct fmdev *);
+static void fm_irq_handle_rds_start(struct fmdev *);
+static void fm_irq_send_rdsdata_getcmd(struct fmdev *);
+static void fm_irq_handle_rdsdata_getcmd_resp(struct fmdev *);
+static void fm_irq_handle_rds_finish(struct fmdev *);
+static void fm_irq_handle_tune_op_ended(struct fmdev *);
+static void fm_irq_handle_power_enb(struct fmdev *);
+static void fm_irq_handle_low_rssi_start(struct fmdev *);
+static void fm_irq_afjump_set_pi(struct fmdev *);
+static void fm_irq_handle_set_pi_resp(struct fmdev *);
+static void fm_irq_afjump_set_pimask(struct fmdev *);
+static void fm_irq_handle_set_pimask_resp(struct fmdev *);
+static void fm_irq_afjump_setfreq(struct fmdev *);
+static void fm_irq_handle_setfreq_resp(struct fmdev *);
+static void fm_irq_afjump_enableint(struct fmdev *);
+static void fm_irq_afjump_enableint_resp(struct fmdev *);
+static void fm_irq_start_afjump(struct fmdev *);
+static void fm_irq_handle_start_afjump_resp(struct fmdev *);
+static void fm_irq_afjump_rd_freq(struct fmdev *);
+static void fm_irq_afjump_rd_freq_resp(struct fmdev *);
+static void fm_irq_handle_low_rssi_finish(struct fmdev *);
+static void fm_irq_send_intmsk_cmd(struct fmdev *);
+static void fm_irq_handle_intmsk_cmd_resp(struct fmdev *);
+
+/*
+ * When FM common module receives interrupt packet, following handlers
+ * will be executed one after another to service the interrupt(s)
+ */
+enum fmc_irq_handler_index {
+	FM_SEND_FLAG_GETCMD_IDX,
+	FM_HANDLE_FLAG_GETCMD_RESP_IDX,
+
+	/* HW malfunction irq handler */
+	FM_HW_MAL_FUNC_IDX,
+
+	/* RDS threshold reached irq handler */
+	FM_RDS_START_IDX,
+	FM_RDS_SEND_RDS_GETCMD_IDX,
+	FM_RDS_HANDLE_RDS_GETCMD_RESP_IDX,
+	FM_RDS_FINISH_IDX,
+
+	/* Tune operation ended irq handler */
+	FM_HW_TUNE_OP_ENDED_IDX,
+
+	/* TX power enable irq handler */
+	FM_HW_POWER_ENB_IDX,
+
+	/* Low RSSI irq handler */
+	FM_LOW_RSSI_START_IDX,
+	FM_AF_JUMP_SETPI_IDX,
+	FM_AF_JUMP_HANDLE_SETPI_RESP_IDX,
+	FM_AF_JUMP_SETPI_MASK_IDX,
+	FM_AF_JUMP_HANDLE_SETPI_MASK_RESP_IDX,
+	FM_AF_JUMP_SET_AF_FREQ_IDX,
+	FM_AF_JUMP_HANDLE_SET_AFFREQ_RESP_IDX,
+	FM_AF_JUMP_ENABLE_INT_IDX,
+	FM_AF_JUMP_ENABLE_INT_RESP_IDX,
+	FM_AF_JUMP_START_AFJUMP_IDX,
+	FM_AF_JUMP_HANDLE_START_AFJUMP_RESP_IDX,
+	FM_AF_JUMP_RD_FREQ_IDX,
+	FM_AF_JUMP_RD_FREQ_RESP_IDX,
+	FM_LOW_RSSI_FINISH_IDX,
+
+	/* Interrupt process post action */
+	FM_SEND_INTMSK_CMD_IDX,
+	FM_HANDLE_INTMSK_CMD_RESP_IDX,
+};
+
+/* FM interrupt handler table */
+static int_handler_prototype int_handler_table[] = {
+	fm_irq_send_flag_getcmd,
+	fm_irq_handle_flag_getcmd_resp,
+	fm_irq_handle_hw_malfunction,
+	fm_irq_handle_rds_start, /* RDS threshold reached irq handler */
+	fm_irq_send_rdsdata_getcmd,
+	fm_irq_handle_rdsdata_getcmd_resp,
+	fm_irq_handle_rds_finish,
+	fm_irq_handle_tune_op_ended,
+	fm_irq_handle_power_enb, /* TX power enable irq handler */
+	fm_irq_handle_low_rssi_start,
+	fm_irq_afjump_set_pi,
+	fm_irq_handle_set_pi_resp,
+	fm_irq_afjump_set_pimask,
+	fm_irq_handle_set_pimask_resp,
+	fm_irq_afjump_setfreq,
+	fm_irq_handle_setfreq_resp,
+	fm_irq_afjump_enableint,
+	fm_irq_afjump_enableint_resp,
+	fm_irq_start_afjump,
+	fm_irq_handle_start_afjump_resp,
+	fm_irq_afjump_rd_freq,
+	fm_irq_afjump_rd_freq_resp,
+	fm_irq_handle_low_rssi_finish,
+	fm_irq_send_intmsk_cmd, /* Interrupt process post action */
+	fm_irq_handle_intmsk_cmd_resp
+};
+
+long (*g_st_write) (struct sk_buff *skb);
+static struct completion wait_for_fmdrv_reg_comp;
+
+static inline void fm_irq_call(struct fmdev *fmdev)
+{
+	fmdev->irq_info.handlers[fmdev->irq_info.stage](fmdev);
+}
+
+/* Continue next function in interrupt handler table */
+static inline void fm_irq_call_stage(struct fmdev *fmdev, u8 stage)
+{
+	fmdev->irq_info.stage = stage;
+	fm_irq_call(fmdev);
+}
+
+static inline void fm_irq_timeout_stage(struct fmdev *fmdev, u8 stage)
+{
+	fmdev->irq_info.stage = stage;
+	mod_timer(&fmdev->irq_info.timer, jiffies + FM_DRV_TX_TIMEOUT);
+}
+
+#ifdef FM_DUMP_TXRX_PKT
+ /* To dump outgoing FM Channel-8 packets */
+inline void dump_tx_skb_data(struct sk_buff *skb)
+{
+	int len, len_org;
+	u8 index;
+	struct fm_cmd_msg_hdr *cmd_hdr;
+
+	cmd_hdr = (struct fm_cmd_msg_hdr *)skb->data;
+	printk(KERN_INFO "<<%shdr:%02x len:%02x opcode:%02x type:%s dlen:%02x",
+	       fm_cb(skb)->completion ? " " : "*", cmd_hdr->hdr,
+	       cmd_hdr->len, cmd_hdr->op,
+	       cmd_hdr->rd_wr ? "RD" : "WR", cmd_hdr->dlen);
+
+	len_org = skb->len - FM_CMD_MSG_HDR_SIZE;
+	if (len_org > 0) {
+		printk("\n   data(%d): ", cmd_hdr->dlen);
+		len = min(len_org, 14);
+		for (index = 0; index < len; index++)
+			printk("%x ",
+			       skb->data[FM_CMD_MSG_HDR_SIZE + index]);
+		printk("%s", (len_org > 14) ? ".." : "");
+	}
+	printk("\n");
+}
+
+ /* To dump incoming FM Channel-8 packets */
+inline void dump_rx_skb_data(struct sk_buff *skb)
+{
+	int len, len_org;
+	u8 index;
+	struct fm_event_msg_hdr *evt_hdr;
+
+	evt_hdr = (struct fm_event_msg_hdr *)skb->data;
+	printk(KERN_INFO ">> hdr:%02x len:%02x sts:%02x numhci:%02x "
+	    "opcode:%02x type:%s dlen:%02x", evt_hdr->hdr, evt_hdr->len,
+	    evt_hdr->status, evt_hdr->num_fm_hci_cmds, evt_hdr->op,
+	    (evt_hdr->rd_wr) ? "RD" : "WR", evt_hdr->dlen);
+
+	len_org = skb->len - FM_EVT_MSG_HDR_SIZE;
+	if (len_org > 0) {
+		printk("\n   data(%d): ", evt_hdr->dlen);
+		len = min(len_org, 14);
+		for (index = 0; index < len; index++)
+			printk("%x ",
+			       skb->data[FM_EVT_MSG_HDR_SIZE + index]);
+		printk("%s", (len_org > 14) ? ".." : "");
+	}
+	printk("\n");
+}
+#endif
+
+void fmc_update_region_info(struct fmdev *fmdev, u8 region_to_set)
+{
+	fmdev->rx.region = region_configs[region_to_set];
+}
+
+/*
+ * FM common sub-module will schedule this tasklet whenever it receives
+ * FM packet from ST driver.
+ */
+static void recv_tasklet(unsigned long arg)
+{
+	struct fmdev *fmdev;
+	struct fm_irq *irq_info;
+	struct fm_event_msg_hdr *evt_hdr;
+	struct sk_buff *skb;
+	u8 num_fm_hci_cmds;
+	unsigned long flags;
+
+	fmdev = (struct fmdev *)arg;
+	irq_info = &fmdev->irq_info;
+	/* Process all packets in the RX queue */
+	while ((skb = skb_dequeue(&fmdev->rx_q))) {
+		if (skb->len < sizeof(struct fm_event_msg_hdr)) {
+			fmerr("skb(%p) has only %d bytes"
+				"atleast need %d bytes to decode\n", skb,
+				skb->len, sizeof(struct fm_event_msg_hdr));
+			kfree_skb(skb);
+			continue;
+		}
+
+		evt_hdr = (void *)skb->data;
+		num_fm_hci_cmds = evt_hdr->num_fm_hci_cmds;
+
+		/* FM interrupt packet? */
+		if (evt_hdr->op == FM_INTERRUPT) {
+			/* FM interrupt handler started already? */
+			if (!test_bit(FM_INTTASK_RUNNING, &fmdev->flag)) {
+				set_bit(FM_INTTASK_RUNNING, &fmdev->flag);
+				if (irq_info->stage != 0) {
+					fmerr("Inval stage resetting to zero\n");
+					irq_info->stage = 0;
+				}
+
+				/*
+				 * Execute first function in interrupt handler
+				 * table.
+				 */
+				irq_info->handlers[irq_info->stage](fmdev);
+			} else {
+				set_bit(FM_INTTASK_SCHEDULE_PENDING, &fmdev->flag);
+			}
+			kfree_skb(skb);
+		}
+		/* Anyone waiting for this with completion handler? */
+		else if (evt_hdr->op == fmdev->pre_op && fmdev->resp_comp != NULL) {
+
+			spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
+			fmdev->resp_skb = skb;
+			spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
+			complete(fmdev->resp_comp);
+
+			fmdev->resp_comp = NULL;
+			atomic_set(&fmdev->tx_cnt, 1);
+		}
+		/* Is this for interrupt handler? */
+		else if (evt_hdr->op == fmdev->pre_op && fmdev->resp_comp == NULL) {
+			if (fmdev->resp_skb != NULL)
+				fmerr("Response SKB ptr not NULL\n");
+
+			spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
+			fmdev->resp_skb = skb;
+			spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
+
+			/* Execute interrupt handler where state index points */
+			irq_info->handlers[irq_info->stage](fmdev);
+
+			kfree_skb(skb);
+			atomic_set(&fmdev->tx_cnt, 1);
+		} else {
+			fmerr("Nobody claimed SKB(%p),purging\n", skb);
+		}
+
+		/*
+		 * Check flow control field. If Num_FM_HCI_Commands field is
+		 * not zero, schedule FM TX tasklet.
+		 */
+		if (num_fm_hci_cmds && atomic_read(&fmdev->tx_cnt))
+			if (!skb_queue_empty(&fmdev->tx_q))
+				tasklet_schedule(&fmdev->tx_task);
+	}
+}
+
+/* FM send tasklet: is scheduled when FM packet has to be sent to chip */
+static void send_tasklet(unsigned long arg)
+{
+	struct fmdev *fmdev;
+	struct sk_buff *skb;
+	int len;
+
+	fmdev = (struct fmdev *)arg;
+
+	if (!atomic_read(&fmdev->tx_cnt))
+		return;
+
+	/* Check, is there any timeout happenned to last transmitted packet */
+	if ((jiffies - fmdev->last_tx_jiffies) > FM_DRV_TX_TIMEOUT) {
+		fmerr("TX timeout occurred\n");
+		atomic_set(&fmdev->tx_cnt, 1);
+	}
+
+	/* Send queued FM TX packets */
+	skb = skb_dequeue(&fmdev->tx_q);
+	if (!skb)
+		return;
+
+	atomic_dec(&fmdev->tx_cnt);
+	fmdev->pre_op = fm_cb(skb)->fm_op;
+
+	if (fmdev->resp_comp != NULL)
+		fmerr("Response completion handler is not NULL\n");
+
+	fmdev->resp_comp = fm_cb(skb)->completion;
+
+	/* Write FM packet to ST driver */
+	len = g_st_write(skb);
+	if (len < 0) {
+		kfree_skb(skb);
+		fmdev->resp_comp = NULL;
+		fmerr("TX tasklet failed to send skb(%p)\n", skb);
+		atomic_set(&fmdev->tx_cnt, 1);
+	} else {
+		fmdev->last_tx_jiffies = jiffies;
+	}
+}
+
+/*
+ * Queues FM Channel-8 packet to FM TX queue and schedules FM TX tasklet for
+ * transmission
+ */
+static u32 fm_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type,	void *payload,
+		int payload_len, struct completion *wait_completion)
+{
+	struct sk_buff *skb;
+	struct fm_cmd_msg_hdr *hdr;
+	int size;
+
+	if (fm_op >= FM_INTERRUPT) {
+		fmerr("Invalid fm opcode - %d\n", fm_op);
+		return -EINVAL;
+	}
+	if (test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag) && payload == NULL) {
+		fmerr("Payload data is NULL during fw download\n");
+		return -EINVAL;
+	}
+	if (!test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag))
+		size =
+		    FM_CMD_MSG_HDR_SIZE + ((payload == NULL) ? 0 : payload_len);
+	else
+		size = payload_len;
+
+	skb = alloc_skb(size, GFP_ATOMIC);
+	if (!skb) {
+		fmerr("No memory to create new SKB\n");
+		return -ENOMEM;
+	}
+	/*
+	 * Don't fill FM header info for the commands which come from
+	 * FM firmware file.
+	 */
+	if (!test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag) ||
+			test_bit(FM_INTTASK_RUNNING, &fmdev->flag)) {
+		/* Fill command header info */
+		hdr = (struct fm_cmd_msg_hdr *)skb_put(skb, FM_CMD_MSG_HDR_SIZE);
+		hdr->hdr = FM_PKT_LOGICAL_CHAN_NUMBER;	/* 0x08 */
+
+		/* 3 (fm_opcode,rd_wr,dlen) + payload len) */
+		hdr->len = ((payload == NULL) ? 0 : payload_len) + 3;
+
+		/* FM opcode */
+		hdr->op = fm_op;
+
+		/* read/write type */
+		hdr->rd_wr = type;
+		hdr->dlen = payload_len;
+		fm_cb(skb)->fm_op = fm_op;
+
+		/*
+		 * If firmware download has finished and the command is
+		 * not a read command then payload is != NULL - a write
+		 * command with u16 payload - convert to be16
+		 */
+		if (payload != NULL)
+			*(u16 *)payload = cpu_to_be16(*(u16 *)payload);
+
+	} else if (payload != NULL) {
+		fm_cb(skb)->fm_op = *((u8 *)payload + 2);
+	}
+	if (payload != NULL)
+		memcpy(skb_put(skb, payload_len), payload, payload_len);
+
+	fm_cb(skb)->completion = wait_completion;
+	skb_queue_tail(&fmdev->tx_q, skb);
+	tasklet_schedule(&fmdev->tx_task);
+
+	return 0;
+}
+
+/* Sends FM Channel-8 command to the chip and waits for the response */
+u32 fmc_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type, void *payload,
+		unsigned int payload_len, void *response, int *response_len)
+{
+	struct sk_buff *skb;
+	struct fm_event_msg_hdr *evt_hdr;
+	unsigned long flags;
+	u32 ret;
+
+	init_completion(&fmdev->maintask_comp);
+	ret = fm_send_cmd(fmdev, fm_op, type, payload, payload_len,
+			    &fmdev->maintask_comp);
+	if (ret)
+		return ret;
+
+	ret = wait_for_completion_timeout(&fmdev->maintask_comp, FM_DRV_TX_TIMEOUT);
+	if (!ret) {
+		fmerr("Timeout(%d sec),didn't get reg"
+			   "completion signal from RX tasklet\n",
+			   jiffies_to_msecs(FM_DRV_TX_TIMEOUT) / 1000);
+		return -ETIMEDOUT;
+	}
+	if (!fmdev->resp_skb) {
+		fmerr("Reponse SKB is missing\n");
+		return -EFAULT;
+	}
+	spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
+	skb = fmdev->resp_skb;
+	fmdev->resp_skb = NULL;
+	spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
+
+	evt_hdr = (void *)skb->data;
+	if (evt_hdr->status != 0) {
+		fmerr("Received event pkt status(%d) is not zero\n",
+			   evt_hdr->status);
+		kfree_skb(skb);
+		return -EIO;
+	}
+	/* Send response data to caller */
+	if (response != NULL && response_len != NULL && evt_hdr->dlen) {
+		/* Skip header info and copy only response data */
+		skb_pull(skb, sizeof(struct fm_event_msg_hdr));
+		memcpy(response, skb->data, evt_hdr->dlen);
+		*response_len = evt_hdr->dlen;
+	} else if (response_len != NULL && evt_hdr->dlen == 0) {
+		*response_len = 0;
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+
+/* --- Helper functions used in FM interrupt handlers ---*/
+static inline u32 check_cmdresp_status(struct fmdev *fmdev,
+		struct sk_buff **skb)
+{
+	struct fm_event_msg_hdr *fm_evt_hdr;
+	unsigned long flags;
+
+	del_timer(&fmdev->irq_info.timer);
+
+	spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
+	*skb = fmdev->resp_skb;
+	fmdev->resp_skb = NULL;
+	spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
+
+	fm_evt_hdr = (void *)(*skb)->data;
+	if (fm_evt_hdr->status != 0) {
+		fmerr("irq: opcode %x response status is not zero "
+				"Initiating irq recovery process\n",
+				fm_evt_hdr->op);
+
+		mod_timer(&fmdev->irq_info.timer, jiffies + FM_DRV_TX_TIMEOUT);
+		return -1;
+	}
+
+	return 0;
+}
+
+static inline void fm_irq_common_cmd_resp_helper(struct fmdev *fmdev, u8 stage)
+{
+	struct sk_buff *skb;
+
+	if (!check_cmdresp_status(fmdev, &skb))
+		fm_irq_call_stage(fmdev, stage);
+}
+
+/*
+ * Interrupt process timeout handler.
+ * One of the irq handler did not get proper response from the chip. So take
+ * recovery action here. FM interrupts are disabled in the beginning of
+ * interrupt process. Therefore reset stage index to re-enable default
+ * interrupts. So that next interrupt will be processed as usual.
+ */
+static void int_timeout_handler(unsigned long data)
+{
+	struct fmdev *fmdev;
+	struct fm_irq *fmirq;
+
+	fmdbg("irq: timeout,trying to re-enable fm interrupts\n");
+	fmdev = (struct fmdev *)data;
+	fmirq = &fmdev->irq_info;
+	fmirq->retry++;
+
+	if (fmirq->retry > FM_IRQ_TIMEOUT_RETRY_MAX) {
+		/* Stop recovery action (interrupt reenable process) and
+		 * reset stage index & retry count values */
+		fmirq->stage = 0;
+		fmirq->retry = 0;
+		fmerr("Recovery action failed during"
+				"irq processing, max retry reached\n");
+		return;
+	}
+	fm_irq_call_stage(fmdev, FM_SEND_INTMSK_CMD_IDX);
+}
+
+/* --------- FM interrupt handlers ------------*/
+static void fm_irq_send_flag_getcmd(struct fmdev *fmdev)
+{
+	u16 flag;
+
+	/* Send FLAG_GET command , to know the source of interrupt */
+	if (!fm_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, sizeof(flag), NULL))
+		fm_irq_timeout_stage(fmdev, FM_HANDLE_FLAG_GETCMD_RESP_IDX);
+}
+
+static void fm_irq_handle_flag_getcmd_resp(struct fmdev *fmdev)
+{
+	struct sk_buff *skb;
+	struct fm_event_msg_hdr *fm_evt_hdr;
+
+	if (check_cmdresp_status(fmdev, &skb))
+		return;
+
+	fm_evt_hdr = (void *)skb->data;
+
+	/* Skip header info and copy only response data */
+	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
+	memcpy(&fmdev->irq_info.flag, skb->data, fm_evt_hdr->dlen);
+
+	fmdev->irq_info.flag = be16_to_cpu(fmdev->irq_info.flag);
+	fmdbg("irq: flag register(0x%x)\n", fmdev->irq_info.flag);
+
+	/* Continue next function in interrupt handler table */
+	fm_irq_call_stage(fmdev, FM_HW_MAL_FUNC_IDX);
+}
+
+static void fm_irq_handle_hw_malfunction(struct fmdev *fmdev)
+{
+	if (fmdev->irq_info.flag & FM_MAL_EVENT & fmdev->irq_info.mask)
+		fmerr("irq: HW MAL int received - do nothing\n");
+
+	/* Continue next function in interrupt handler table */
+	fm_irq_call_stage(fmdev, FM_RDS_START_IDX);
+}
+
+static void fm_irq_handle_rds_start(struct fmdev *fmdev)
+{
+	if (fmdev->irq_info.flag & FM_RDS_EVENT & fmdev->irq_info.mask) {
+		fmdbg("irq: rds threshold reached\n");
+		fmdev->irq_info.stage = FM_RDS_SEND_RDS_GETCMD_IDX;
+	} else {
+		/* Continue next function in interrupt handler table */
+		fmdev->irq_info.stage = FM_HW_TUNE_OP_ENDED_IDX;
+	}
+
+	fm_irq_call(fmdev);
+}
+
+static void fm_irq_send_rdsdata_getcmd(struct fmdev *fmdev)
+{
+	/* Send the command to read RDS data from the chip */
+	if (!fm_send_cmd(fmdev, RDS_DATA_GET, REG_RD, NULL,
+			    (FM_RX_RDS_FIFO_THRESHOLD * 3), NULL))
+		fm_irq_timeout_stage(fmdev, FM_RDS_HANDLE_RDS_GETCMD_RESP_IDX);
+}
+
+/* Keeps track of current RX channel AF (Alternate Frequency) */
+static void fm_rx_update_af_cache(struct fmdev *fmdev, u8 af)
+{
+	struct tuned_station_info *stat_info = &fmdev->rx.stat_info;
+	u8 reg_idx = fmdev->rx.region.fm_band;
+	u8 index;
+	u32 freq;
+
+	/* First AF indicates the number of AF follows. Reset the list */
+	if ((af >= FM_RDS_1_AF_FOLLOWS) && (af <= FM_RDS_25_AF_FOLLOWS)) {
+		fmdev->rx.stat_info.af_list_max = (af - FM_RDS_1_AF_FOLLOWS + 1);
+		fmdev->rx.stat_info.afcache_size = 0;
+		fmdbg("No of expected AF : %d\n", fmdev->rx.stat_info.af_list_max);
+		return;
+	}
+
+	if (af < FM_RDS_MIN_AF)
+		return;
+	if (reg_idx == FM_BAND_EUROPE_US && af > FM_RDS_MAX_AF)
+		return;
+	if (reg_idx == FM_BAND_JAPAN && af > FM_RDS_MAX_AF_JAPAN)
+		return;
+
+	freq = fmdev->rx.region.bot_freq + (af * 100);
+	if (freq == fmdev->rx.freq) {
+		fmdbg("Current freq(%d) is matching with received AF(%d)\n",
+				fmdev->rx.freq, freq);
+		return;
+	}
+	/* Do check in AF cache */
+	for (index = 0; index < stat_info->afcache_size; index++) {
+		if (stat_info->af_cache[index] == freq)
+			break;
+	}
+	/* Reached the limit of the list - ignore the next AF */
+	if (index == stat_info->af_list_max) {
+		fmdbg("AF cache is full\n");
+		return;
+	}
+	/*
+	 * If we reached the end of the list then this AF is not
+	 * in the list - add it.
+	 */
+	if (index == stat_info->afcache_size) {
+		fmdbg("Storing AF %d to cache index %d\n", freq, index);
+		stat_info->af_cache[index] = freq;
+		stat_info->afcache_size++;
+	}
+}
+
+/*
+ * Converts RDS buffer data from big endian format
+ * to little endian format.
+ */
+static void fm_rdsparse_swapbytes(struct fmdev *fmdev,
+		struct fm_rdsdata_format *rds_format)
+{
+	u8 byte1;
+	u8 index = 0;
+	u8 *rds_buff;
+
+	/*
+	 * Since in Orca the 2 RDS Data bytes are in little endian and
+	 * in Dolphin they are in big endian, the parsing of the RDS data
+	 * is chip dependent
+	 */
+	if (fmdev->asci_id != 0x6350) {
+		rds_buff = &rds_format->data.groupdatabuff.buff[0];
+		while (index + 1 < FM_RX_RDS_INFO_FIELD_MAX) {
+			byte1 = rds_buff[index];
+			rds_buff[index] = rds_buff[index + 1];
+			rds_buff[index + 1] = byte1;
+			index += 2;
+		}
+	}
+}
+
+static void fm_irq_handle_rdsdata_getcmd_resp(struct fmdev *fmdev)
+{
+	struct sk_buff *skb;
+	struct fm_rdsdata_format rds_fmt;
+	struct fm_rds *rds = &fmdev->rx.rds;
+	unsigned long group_idx, flags;
+	u8 *rds_data, meta_data, tmpbuf[3];
+	u8 type, blk_idx;
+	u16 cur_picode;
+	u32 rds_len;
+
+	if (check_cmdresp_status(fmdev, &skb))
+		return;
+
+	/* Skip header info */
+	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
+	rds_data = skb->data;
+	rds_len = skb->len;
+
+	/* Parse the RDS data */
+	while (rds_len >= FM_RDS_BLK_SIZE) {
+		meta_data = rds_data[2];
+		/* Get the type: 0=A, 1=B, 2=C, 3=C', 4=D, 5=E */
+		type = (meta_data & 0x07);
+
+		/* Transform the blk type into index sequence (0, 1, 2, 3, 4) */
+		blk_idx = (type <= FM_RDS_BLOCK_C ? type : (type - 1));
+		fmdbg("Block index:%d(%s)\n", blk_idx,
+			   (meta_data & FM_RDS_STATUS_ERR_MASK) ? "Bad" : "Ok");
+
+		if ((meta_data & FM_RDS_STATUS_ERR_MASK) != 0)
+			break;
+
+		if (blk_idx < FM_RDS_BLK_IDX_A || blk_idx > FM_RDS_BLK_IDX_D) {
+			fmdbg("Block sequence mismatch\n");
+			rds->last_blk_idx = -1;
+			break;
+		}
+
+		/* Skip checkword (control) byte and copy only data byte */
+		memcpy(&rds_fmt.data.groupdatabuff.
+				buff[blk_idx * (FM_RDS_BLK_SIZE - 1)],
+				rds_data, (FM_RDS_BLK_SIZE - 1));
+
+		rds->last_blk_idx = blk_idx;
+
+		/* If completed a whole group then handle it */
+		if (blk_idx == FM_RDS_BLK_IDX_D) {
+			fmdbg("Good block received\n");
+			fm_rdsparse_swapbytes(fmdev, &rds_fmt);
+
+			/*
+			 * Extract PI code and store in local cache.
+			 * We need this during AF switch processing.
+			 */
+			cur_picode = be16_to_cpu(rds_fmt.data.groupgeneral.pidata);
+			if (fmdev->rx.stat_info.picode != cur_picode)
+				fmdev->rx.stat_info.picode = cur_picode;
+
+			fmdbg("picode:%d\n", cur_picode);
+
+			group_idx = (rds_fmt.data.groupgeneral.blk_b[0] >> 3);
+			fmdbg("(fmdrv):Group:%ld%s\n", group_idx/2,
+					(group_idx % 2) ? "B" : "A");
+
+			group_idx = 1 << (rds_fmt.data.groupgeneral.blk_b[0] >> 3);
+			if (group_idx == FM_RDS_GROUP_TYPE_MASK_0A) {
+				fm_rx_update_af_cache(fmdev, rds_fmt.data.group0A.af[0]);
+				fm_rx_update_af_cache(fmdev, rds_fmt.data.group0A.af[1]);
+			}
+		}
+		rds_len -= FM_RDS_BLK_SIZE;
+		rds_data += FM_RDS_BLK_SIZE;
+	}
+
+	/* Copy raw rds data to internal rds buffer */
+	rds_data = skb->data;
+	rds_len = skb->len;
+
+	spin_lock_irqsave(&fmdev->rds_buff_lock, flags);
+	while (rds_len > 0) {
+		/*
+		 * Fill RDS buffer as per V4L2 specification.
+		 * Store control byte
+		 */
+		type = (rds_data[2] & 0x07);
+		blk_idx = (type <= FM_RDS_BLOCK_C ? type : (type - 1));
+		tmpbuf[2] = blk_idx;	/* Offset name */
+		tmpbuf[2] |= blk_idx << 3;	/* Received offset */
+
+		/* Store data byte */
+		tmpbuf[0] = rds_data[0];
+		tmpbuf[1] = rds_data[1];
+
+		memcpy(&rds->buff[rds->wr_idx], &tmpbuf, FM_RDS_BLK_SIZE);
+		rds->wr_idx = (rds->wr_idx + FM_RDS_BLK_SIZE) % rds->buf_size;
+
+		/* Check for overflow & start over */
+		if (rds->wr_idx == rds->rd_idx) {
+			fmdbg("RDS buffer overflow\n");
+			rds->wr_idx = 0;
+			rds->rd_idx = 0;
+			break;
+		}
+		rds_len -= FM_RDS_BLK_SIZE;
+		rds_data += FM_RDS_BLK_SIZE;
+	}
+	spin_unlock_irqrestore(&fmdev->rds_buff_lock, flags);
+
+	/* Wakeup read queue */
+	if (rds->wr_idx != rds->rd_idx)
+		wake_up_interruptible(&rds->read_queue);
+
+	fm_irq_call_stage(fmdev, FM_RDS_FINISH_IDX);
+}
+
+static void fm_irq_handle_rds_finish(struct fmdev *fmdev)
+{
+	fm_irq_call_stage(fmdev, FM_HW_TUNE_OP_ENDED_IDX);
+}
+
+static void fm_irq_handle_tune_op_ended(struct fmdev *fmdev)
+{
+	if (fmdev->irq_info.flag & (FM_FR_EVENT | FM_BL_EVENT) & fmdev->
+	    irq_info.mask) {
+		fmdbg("irq: tune ended/bandlimit reached\n");
+		if (test_and_clear_bit(FM_AF_SWITCH_INPROGRESS, &fmdev->flag)) {
+			fmdev->irq_info.stage = FM_AF_JUMP_RD_FREQ_IDX;
+		} else {
+			complete(&fmdev->maintask_comp);
+			fmdev->irq_info.stage = FM_HW_POWER_ENB_IDX;
+		}
+	} else
+		fmdev->irq_info.stage = FM_HW_POWER_ENB_IDX;
+
+	fm_irq_call(fmdev);
+}
+
+static void fm_irq_handle_power_enb(struct fmdev *fmdev)
+{
+	if (fmdev->irq_info.flag & FM_POW_ENB_EVENT) {
+		fmdbg("irq: Power Enabled/Disabled\n");
+		complete(&fmdev->maintask_comp);
+	}
+
+	fm_irq_call_stage(fmdev, FM_LOW_RSSI_START_IDX);
+}
+
+static void fm_irq_handle_low_rssi_start(struct fmdev *fmdev)
+{
+	if ((fmdev->rx.af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON) &&
+	    (fmdev->irq_info.flag & FM_LEV_EVENT & fmdev->irq_info.mask) &&
+	    (fmdev->rx.freq != FM_UNDEFINED_FREQ) &&
+	    (fmdev->rx.stat_info.afcache_size != 0)) {
+		fmdbg("irq: rssi level has fallen below threshold level\n");
+
+		/* Disable further low RSSI interrupts */
+		fmdev->irq_info.mask &= ~FM_LEV_EVENT;
+
+		fmdev->rx.afjump_idx = 0;
+		fmdev->rx.freq_before_jump = fmdev->rx.freq;
+		fmdev->irq_info.stage = FM_AF_JUMP_SETPI_IDX;
+	} else {
+		/* Continue next function in interrupt handler table */
+		fmdev->irq_info.stage = FM_SEND_INTMSK_CMD_IDX;
+	}
+
+	fm_irq_call(fmdev);
+}
+
+static void fm_irq_afjump_set_pi(struct fmdev *fmdev)
+{
+	u16 payload;
+
+	/* Set PI code - must be updated if the AF list is not empty */
+	payload = fmdev->rx.stat_info.picode;
+	if (!fm_send_cmd(fmdev, RDS_PI_SET, REG_WR, &payload, sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_SETPI_RESP_IDX);
+}
+
+static void fm_irq_handle_set_pi_resp(struct fmdev *fmdev)
+{
+	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_SETPI_MASK_IDX);
+}
+
+/*
+ * Set PI mask.
+ * 0xFFFF = Enable PI code matching
+ * 0x0000 = Disable PI code matching
+ */
+static void fm_irq_afjump_set_pimask(struct fmdev *fmdev)
+{
+	u16 payload;
+
+	payload = 0x0000;
+	if (!fm_send_cmd(fmdev, RDS_PI_MASK_SET, REG_WR, &payload, sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_SETPI_MASK_RESP_IDX);
+}
+
+static void fm_irq_handle_set_pimask_resp(struct fmdev *fmdev)
+{
+	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_SET_AF_FREQ_IDX);
+}
+
+static void fm_irq_afjump_setfreq(struct fmdev *fmdev)
+{
+	u16 frq_index;
+	u16 payload;
+
+	fmdbg("Swtich to %d KHz\n", fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx]);
+	frq_index = (fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx] -
+	     fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
+
+	payload = frq_index;
+	if (!fm_send_cmd(fmdev, AF_FREQ_SET, REG_WR, &payload, sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_SET_AFFREQ_RESP_IDX);
+}
+
+static void fm_irq_handle_setfreq_resp(struct fmdev *fmdev)
+{
+	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_ENABLE_INT_IDX);
+}
+
+static void fm_irq_afjump_enableint(struct fmdev *fmdev)
+{
+	u16 payload;
+
+	/* Enable FR (tuning operation ended) interrupt */
+	payload = FM_FR_EVENT;
+	if (!fm_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload, sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_ENABLE_INT_RESP_IDX);
+}
+
+static void fm_irq_afjump_enableint_resp(struct fmdev *fmdev)
+{
+	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_START_AFJUMP_IDX);
+}
+
+static void fm_irq_start_afjump(struct fmdev *fmdev)
+{
+	u16 payload;
+
+	payload = FM_TUNER_AF_JUMP_MODE;
+	if (!fm_send_cmd(fmdev, TUNER_MODE_SET, REG_WR, &payload,
+			sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_START_AFJUMP_RESP_IDX);
+}
+
+static void fm_irq_handle_start_afjump_resp(struct fmdev *fmdev)
+{
+	struct sk_buff *skb;
+
+	if (check_cmdresp_status(fmdev, &skb))
+		return;
+
+	fmdev->irq_info.stage = FM_SEND_FLAG_GETCMD_IDX;
+	set_bit(FM_AF_SWITCH_INPROGRESS, &fmdev->flag);
+	clear_bit(FM_INTTASK_RUNNING, &fmdev->flag);
+}
+
+static void fm_irq_afjump_rd_freq(struct fmdev *fmdev)
+{
+	u16 payload;
+
+	if (!fm_send_cmd(fmdev, FREQ_SET, REG_RD, NULL, sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_RD_FREQ_RESP_IDX);
+}
+
+static void fm_irq_afjump_rd_freq_resp(struct fmdev *fmdev)
+{
+	struct sk_buff *skb;
+	u16 read_freq;
+	u32 curr_freq, jumped_freq;
+
+	if (check_cmdresp_status(fmdev, &skb))
+		return;
+
+	/* Skip header info and copy only response data */
+	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
+	memcpy(&read_freq, skb->data, sizeof(read_freq));
+	read_freq = be16_to_cpu(read_freq);
+	curr_freq = fmdev->rx.region.bot_freq + ((u32)read_freq * FM_FREQ_MUL);
+
+	jumped_freq = fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx];
+
+	/* If the frequency was changed the jump succeeded */
+	if ((curr_freq != fmdev->rx.freq_before_jump) && (curr_freq == jumped_freq)) {
+		fmdbg("Successfully switched to alternate freq %d\n", curr_freq);
+		fmdev->rx.freq = curr_freq;
+		fm_rx_reset_rds_cache(fmdev);
+
+		/* AF feature is on, enable low level RSSI interrupt */
+		if (fmdev->rx.af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON)
+			fmdev->irq_info.mask |= FM_LEV_EVENT;
+
+		fmdev->irq_info.stage = FM_LOW_RSSI_FINISH_IDX;
+	} else {		/* jump to the next freq in the AF list */
+		fmdev->rx.afjump_idx++;
+
+		/* If we reached the end of the list - stop searching */
+		if (fmdev->rx.afjump_idx >= fmdev->rx.stat_info.afcache_size) {
+			fmdbg("AF switch processing failed\n");
+			fmdev->irq_info.stage = FM_LOW_RSSI_FINISH_IDX;
+		} else {	/* AF List is not over - try next one */
+
+			fmdbg("Trying next freq in AF cache\n");
+			fmdev->irq_info.stage = FM_AF_JUMP_SETPI_IDX;
+		}
+	}
+	fm_irq_call(fmdev);
+}
+
+static void fm_irq_handle_low_rssi_finish(struct fmdev *fmdev)
+{
+	fm_irq_call_stage(fmdev, FM_SEND_INTMSK_CMD_IDX);
+}
+
+static void fm_irq_send_intmsk_cmd(struct fmdev *fmdev)
+{
+	u16 payload;
+
+	/* Re-enable FM interrupts */
+	payload = fmdev->irq_info.mask;
+
+	if (!fm_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL))
+		fm_irq_timeout_stage(fmdev, FM_HANDLE_INTMSK_CMD_RESP_IDX);
+}
+
+static void fm_irq_handle_intmsk_cmd_resp(struct fmdev *fmdev)
+{
+	struct sk_buff *skb;
+
+	if (check_cmdresp_status(fmdev, &skb))
+		return;
+	/*
+	 * This is last function in interrupt table to be executed.
+	 * So, reset stage index to 0.
+	 */
+	fmdev->irq_info.stage = FM_SEND_FLAG_GETCMD_IDX;
+
+	/* Start processing any pending interrupt */
+	if (test_and_clear_bit(FM_INTTASK_SCHEDULE_PENDING, &fmdev->flag))
+		fmdev->irq_info.handlers[fmdev->irq_info.stage](fmdev);
+	else
+		clear_bit(FM_INTTASK_RUNNING, &fmdev->flag);
+}
+
+/* Returns availability of RDS data in internel buffer */
+u32 fmc_is_rds_data_available(struct fmdev *fmdev, struct file *file,
+				struct poll_table_struct *pts)
+{
+	poll_wait(file, &fmdev->rx.rds.read_queue, pts);
+	if (fmdev->rx.rds.rd_idx != fmdev->rx.rds.wr_idx)
+		return 0;
+
+	return -EAGAIN;
+}
+
+/* Copies RDS data from internal buffer to user buffer */
+u32 fmc_transfer_rds_from_internal_buff(struct fmdev *fmdev, struct file *file,
+		u8 __user *buf, size_t count)
+{
+	u32 block_count;
+	unsigned long flags;
+	int ret;
+
+	if (fmdev->rx.rds.wr_idx == fmdev->rx.rds.rd_idx) {
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+
+		ret = wait_event_interruptible(fmdev->rx.rds.read_queue,
+				(fmdev->rx.rds.wr_idx != fmdev->rx.rds.rd_idx));
+		if (ret)
+			return -EINTR;
+	}
+
+	/* Calculate block count from byte count */
+	count /= 3;
+	block_count = 0;
+	ret = 0;
+
+	spin_lock_irqsave(&fmdev->rds_buff_lock, flags);
+
+	while (block_count < count) {
+		if (fmdev->rx.rds.wr_idx == fmdev->rx.rds.rd_idx)
+			break;
+
+		if (copy_to_user(buf, &fmdev->rx.rds.buff[fmdev->rx.rds.rd_idx],
+					FM_RDS_BLK_SIZE))
+			break;
+
+		fmdev->rx.rds.rd_idx += FM_RDS_BLK_SIZE;
+		if (fmdev->rx.rds.rd_idx >= fmdev->rx.rds.buf_size)
+			fmdev->rx.rds.rd_idx = 0;
+
+		block_count++;
+		buf += FM_RDS_BLK_SIZE;
+		ret += FM_RDS_BLK_SIZE;
+	}
+	spin_unlock_irqrestore(&fmdev->rds_buff_lock, flags);
+	return ret;
+}
+
+u32 fmc_set_freq(struct fmdev *fmdev, u32 freq_to_set)
+{
+	switch (fmdev->curr_fmmode) {
+	case FM_MODE_RX:
+		return fm_rx_set_freq(fmdev, freq_to_set);
+
+	case FM_MODE_TX:
+		return fm_tx_set_freq(fmdev, freq_to_set);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+u32 fmc_get_freq(struct fmdev *fmdev, u32 *cur_tuned_frq)
+{
+	if (fmdev->rx.freq == FM_UNDEFINED_FREQ) {
+		fmerr("RX frequency is not set\n");
+		return -EPERM;
+	}
+	if (cur_tuned_frq == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	switch (fmdev->curr_fmmode) {
+	case FM_MODE_RX:
+		*cur_tuned_frq = fmdev->rx.freq;
+		return 0;
+
+	case FM_MODE_TX:
+		*cur_tuned_frq = 0;	/* TODO : Change this later */
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+
+}
+
+u32 fmc_set_region(struct fmdev *fmdev, u8 region_to_set)
+{
+	switch (fmdev->curr_fmmode) {
+	case FM_MODE_RX:
+		return fm_rx_set_region(fmdev, region_to_set);
+
+	case FM_MODE_TX:
+		return fm_tx_set_region(fmdev, region_to_set);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+u32 fmc_set_mute_mode(struct fmdev *fmdev, u8 mute_mode_toset)
+{
+	switch (fmdev->curr_fmmode) {
+	case FM_MODE_RX:
+		return fm_rx_set_mute_mode(fmdev, mute_mode_toset);
+
+	case FM_MODE_TX:
+		return fm_tx_set_mute_mode(fmdev, mute_mode_toset);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+u32 fmc_set_stereo_mono(struct fmdev *fmdev, u16 mode)
+{
+	switch (fmdev->curr_fmmode) {
+	case FM_MODE_RX:
+		return fm_rx_set_stereo_mono(fmdev, mode);
+
+	case FM_MODE_TX:
+		return fm_tx_set_stereo_mono(fmdev, mode);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+u32 fmc_set_rds_mode(struct fmdev *fmdev, u8 rds_en_dis)
+{
+	switch (fmdev->curr_fmmode) {
+	case FM_MODE_RX:
+		return fm_rx_set_rds_mode(fmdev, rds_en_dis);
+
+	case FM_MODE_TX:
+		return fm_tx_set_rds_mode(fmdev, rds_en_dis);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+/* Sends power off command to the chip */
+static u32 fm_power_down(struct fmdev *fmdev)
+{
+	u16 payload;
+	u32 ret;
+
+	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
+		fmerr("FM core is not ready\n");
+		return -EPERM;
+	}
+	if (fmdev->curr_fmmode == FM_MODE_OFF) {
+		fmdbg("FM chip is already in OFF state\n");
+		return 0;
+	}
+
+	payload = 0x0;
+	ret = fmc_send_cmd(fmdev, FM_POWER_MODE, REG_WR, &payload,
+		sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return fmc_release(fmdev);
+}
+
+/* Reads init command from FM firmware file and loads to the chip */
+static u32 fm_download_firmware(struct fmdev *fmdev, const u8 *fw_name)
+{
+	const struct firmware *fw_entry;
+	struct bts_header *fw_header;
+	struct bts_action *action;
+	struct bts_action_delay *delay;
+	u8 *fw_data;
+	int ret, fw_len, cmd_cnt;
+
+	cmd_cnt = 0;
+	set_bit(FM_FW_DW_INPROGRESS, &fmdev->flag);
+
+	ret = request_firmware(&fw_entry, fw_name,
+				&fmdev->radio_dev->dev);
+	if (ret < 0) {
+		fmerr("Unable to read firmware(%s) content\n", fw_name);
+		return ret;
+	}
+	fmdbg("Firmware(%s) length : %d bytes\n", fw_name, fw_entry->size);
+
+	fw_data = (void *)fw_entry->data;
+	fw_len = fw_entry->size;
+
+	fw_header = (struct bts_header *)fw_data;
+	if (fw_header->magic != FM_FW_FILE_HEADER_MAGIC) {
+		fmerr("%s not a legal TI firmware file\n", fw_name);
+		ret = -EINVAL;
+		goto rel_fw;
+	}
+	fmdbg("FW(%s) magic number : 0x%x\n", fw_name, fw_header->magic);
+
+	/* Skip file header info , we already verified it */
+	fw_data += sizeof(struct bts_header);
+	fw_len -= sizeof(struct bts_header);
+
+	while (fw_data && fw_len > 0) {
+		action = (struct bts_action *)fw_data;
+
+		switch (action->type) {
+		case ACTION_SEND_COMMAND:	/* Send */
+			if (fmc_send_cmd(fmdev, 0, 0, action->data,
+						action->size, NULL, NULL))
+				goto rel_fw;
+
+			cmd_cnt++;
+			break;
+
+		case ACTION_DELAY:	/* Delay */
+			delay = (struct bts_action_delay *)action->data;
+			mdelay(delay->msec);
+			break;
+		}
+
+		fw_data += (sizeof(struct bts_action) + (action->size));
+		fw_len -= (sizeof(struct bts_action) + (action->size));
+	}
+	fmdbg("Firmware commands(%d) loaded to chip\n", cmd_cnt);
+rel_fw:
+	release_firmware(fw_entry);
+	clear_bit(FM_FW_DW_INPROGRESS, &fmdev->flag);
+
+	return ret;
+}
+
+/* Loads default RX configuration to the chip */
+static u32 load_default_rx_configuration(struct fmdev *fmdev)
+{
+	int ret;
+
+	ret = fm_rx_set_volume(fmdev, FM_DEFAULT_RX_VOLUME);
+	if (ret < 0)
+		return ret;
+
+	return fm_rx_set_rssi_threshold(fmdev, FM_DEFAULT_RSSI_THRESHOLD);
+}
+
+/* Does FM power on sequence */
+static u32 fm_power_up(struct fmdev *fmdev, u8 mode)
+{
+	u16 payload, asic_id, asic_ver;
+	int resp_len, ret;
+	u8 fw_name[50];
+
+	if (mode >= FM_MODE_ENTRY_MAX) {
+		fmerr("Invalid firmware download option\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Initialize FM common module. FM GPIO toggling is
+	 * taken care in Shared Transport driver.
+	 */
+	ret = fmc_prepare(fmdev);
+	if (ret < 0) {
+		fmerr("Unable to prepare FM Common\n");
+		return ret;
+	}
+
+	payload = FM_ENABLE;
+	if (fmc_send_cmd(fmdev, FM_POWER_MODE, REG_WR, &payload,
+			sizeof(payload), NULL, NULL))
+		goto rel;
+
+	/* Allow the chip to settle down in Channel-8 mode */
+	msleep(20);
+
+	if (fmc_send_cmd(fmdev, ASIC_ID_GET, REG_RD, NULL,
+			sizeof(asic_id), &asic_id, &resp_len))
+		goto rel;
+
+	if (fmc_send_cmd(fmdev, ASIC_VER_GET, REG_RD, NULL,
+			sizeof(asic_ver), &asic_ver, &resp_len))
+		goto rel;
+
+	fmdbg("ASIC ID: 0x%x , ASIC Version: %d\n",
+		be16_to_cpu(asic_id), be16_to_cpu(asic_ver));
+
+	sprintf(fw_name, "%s_%x.%d.bts", FM_FMC_FW_FILE_START,
+		be16_to_cpu(asic_id), be16_to_cpu(asic_ver));
+
+	ret = fm_download_firmware(fmdev, fw_name);
+	if (ret < 0) {
+		fmdbg("Failed to download firmware file %s\n", fw_name);
+		goto rel;
+	}
+	sprintf(fw_name, "%s_%x.%d.bts", (mode == FM_MODE_RX) ?
+			FM_RX_FW_FILE_START : FM_TX_FW_FILE_START,
+			be16_to_cpu(asic_id), be16_to_cpu(asic_ver));
+
+	ret = fm_download_firmware(fmdev, fw_name);
+	if (ret < 0) {
+		fmdbg("Failed to download firmware file %s\n", fw_name);
+		goto rel;
+	} else
+		return ret;
+rel:
+	return fmc_release(fmdev);
+}
+
+/* Set FM Modes(TX, RX, OFF) */
+u32 fmc_set_mode(struct fmdev *fmdev, u8 fm_mode)
+{
+	int ret = 0;
+
+	if (fm_mode >= FM_MODE_ENTRY_MAX) {
+		fmerr("Invalid FM mode\n");
+		return -EINVAL;
+	}
+	if (fmdev->curr_fmmode == fm_mode) {
+		fmdbg("Already fm is in mode(%d)\n", fm_mode);
+		return ret;
+	}
+
+	switch (fm_mode) {
+	case FM_MODE_OFF:	/* OFF Mode */
+		ret = fm_power_down(fmdev);
+		if (ret < 0) {
+			fmerr("Failed to set OFF mode\n");
+			return ret;
+		}
+		break;
+
+	case FM_MODE_TX:	/* TX Mode */
+	case FM_MODE_RX:	/* RX Mode */
+		/* Power down before switching to TX or RX mode */
+		if (fmdev->curr_fmmode != FM_MODE_OFF) {
+			ret = fm_power_down(fmdev);
+			if (ret < 0) {
+				fmerr("Failed to set OFF mode\n");
+				return ret;
+			}
+			msleep(30);
+		}
+		ret = fm_power_up(fmdev, fm_mode);
+		if (ret < 0) {
+			fmerr("Failed to load firmware\n");
+			return ret;
+		}
+	}
+	fmdev->curr_fmmode = fm_mode;
+
+	/* Set default configuration */
+	if (fmdev->curr_fmmode == FM_MODE_RX) {
+		fmdbg("Loading default rx configuration..\n");
+		ret = load_default_rx_configuration(fmdev);
+		if (ret < 0)
+			fmerr("Failed to load default values\n");
+	}
+
+	return ret;
+}
+
+/* Returns current FM mode (TX, RX, OFF) */
+u32 fmc_get_mode(struct fmdev *fmdev, u8 *fmmode)
+{
+	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
+		fmerr("FM core is not ready\n");
+		return -EPERM;
+	}
+	if (fmmode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*fmmode = fmdev->curr_fmmode;
+	return 0;
+}
+
+/* Called by ST layer when FM packet is available */
+static long fm_st_receive(void *arg, struct sk_buff *skb)
+{
+	struct fmdev *fmdev;
+
+	fmdev = (struct fmdev *)arg;
+
+	if (skb == NULL) {
+		fmerr("Invalid SKB received from ST\n");
+		return -EFAULT;
+	}
+
+	if (skb->cb[0] != FM_PKT_LOGICAL_CHAN_NUMBER) {
+		fmerr("Received SKB (%p) is not FM Channel 8 pkt\n", skb);
+		return -EINVAL;
+	}
+
+	memcpy(skb_push(skb, 1), &skb->cb[0], 1);
+	skb_queue_tail(&fmdev->rx_q, skb);
+	tasklet_schedule(&fmdev->rx_task);
+
+	return 0;
+}
+
+/*
+ * Called by ST layer to indicate protocol registration completion
+ * status.
+ */
+static void fm_st_reg_comp_cb(void *arg, char data)
+{
+	struct fmdev *fmdev;
+
+	fmdev = (struct fmdev *)arg;
+	fmdev->streg_cbdata = data;
+	complete(&wait_for_fmdrv_reg_comp);
+}
+
+/*
+ * This function will be called from FM V4L2 open function.
+ * Register with ST driver and initialize driver data.
+ */
+u32 fmc_prepare(struct fmdev *fmdev)
+{
+	static struct st_proto_s fm_st_proto;
+	u32 ret;
+
+	if (test_bit(FM_CORE_READY, &fmdev->flag)) {
+		fmdbg("FM Core is already up\n");
+		return 0;
+	}
+
+	memset(&fm_st_proto, 0, sizeof(fm_st_proto));
+	fm_st_proto.type = ST_FM;
+	fm_st_proto.recv = fm_st_receive;
+	fm_st_proto.match_packet = NULL;
+	fm_st_proto.reg_complete_cb = fm_st_reg_comp_cb;
+	fm_st_proto.write = NULL; /* TI ST driver will fill write pointer */
+	fm_st_proto.priv_data = fmdev;
+
+	ret = st_register(&fm_st_proto);
+	if (ret == -EINPROGRESS) {
+		init_completion(&wait_for_fmdrv_reg_comp);
+		fmdev->streg_cbdata = -EINPROGRESS;
+		fmdbg("%s waiting for ST reg completion signal\n", __func__);
+
+		ret = wait_for_completion_timeout(&wait_for_fmdrv_reg_comp,
+				FM_ST_REG_TIMEOUT);
+
+		if (!ret) {
+			fmerr("Timeout(%d sec), didn't get reg "
+					"completion signal from ST\n",
+					jiffies_to_msecs(FM_ST_REG_TIMEOUT) / 1000);
+			return -ETIMEDOUT;
+		}
+		if (fmdev->streg_cbdata != 0) {
+			fmerr("ST reg comp CB called with error "
+					"status %d\n", fmdev->streg_cbdata);
+			return -EAGAIN;
+		}
+
+		ret = 0;
+	} else if (ret == -1) {
+		fmerr("st_register failed %d\n", ret);
+		return -EAGAIN;
+	}
+
+	if (fm_st_proto.write != NULL) {
+		g_st_write = fm_st_proto.write;
+	} else {
+		fmerr("Failed to get ST write func pointer\n");
+		ret = st_unregister(ST_FM);
+		if (ret < 0)
+			fmerr("st_unregister failed %d\n", ret);
+		return -EAGAIN;
+	}
+
+	spin_lock_init(&fmdev->rds_buff_lock);
+	spin_lock_init(&fmdev->resp_skb_lock);
+
+	/* Initialize TX queue and TX tasklet */
+	skb_queue_head_init(&fmdev->tx_q);
+	tasklet_init(&fmdev->tx_task, send_tasklet, (unsigned long)fmdev);
+
+	/* Initialize RX Queue and RX tasklet */
+	skb_queue_head_init(&fmdev->rx_q);
+	tasklet_init(&fmdev->rx_task, recv_tasklet, (unsigned long)fmdev);
+
+	fmdev->irq_info.stage = 0;
+	atomic_set(&fmdev->tx_cnt, 1);
+	fmdev->resp_comp = NULL;
+
+	init_timer(&fmdev->irq_info.timer);
+	fmdev->irq_info.timer.function = &int_timeout_handler;
+	fmdev->irq_info.timer.data = (unsigned long)fmdev;
+	/*TODO: add FM_STIC_EVENT later */
+	fmdev->irq_info.mask = FM_MAL_EVENT;
+
+	/* Region info */
+	memcpy(&fmdev->rx.region, &region_configs[default_radio_region],
+			sizeof(struct region_info));
+
+	fmdev->rx.mute_mode = FM_MUTE_OFF;
+	fmdev->rx.rf_depend_mute = FM_RX_RF_DEPENDENT_MUTE_OFF;
+	fmdev->rx.rds.flag = FM_RDS_DISABLE;
+	fmdev->rx.freq = FM_UNDEFINED_FREQ;
+	fmdev->rx.rds_mode = FM_RDS_SYSTEM_RDS;
+	fmdev->rx.af_mode = FM_RX_RDS_AF_SWITCH_MODE_OFF;
+	fmdev->irq_info.retry = 0;
+
+	fm_rx_reset_rds_cache(fmdev);
+	init_waitqueue_head(&fmdev->rx.rds.read_queue);
+
+	fm_rx_reset_station_info(fmdev);
+	set_bit(FM_CORE_READY, &fmdev->flag);
+
+	return ret;
+}
+
+/*
+ * This function will be called from FM V4L2 release function.
+ * Unregister from ST driver.
+ */
+u32 fmc_release(struct fmdev *fmdev)
+{
+	u32 ret;
+
+	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
+		fmdbg("FM Core is already down\n");
+		return 0;
+	}
+	/* Sevice pending read */
+	wake_up_interruptible(&fmdev->rx.rds.read_queue);
+
+	tasklet_kill(&fmdev->tx_task);
+	tasklet_kill(&fmdev->rx_task);
+
+	skb_queue_purge(&fmdev->tx_q);
+	skb_queue_purge(&fmdev->rx_q);
+
+	fmdev->resp_comp = NULL;
+	fmdev->rx.freq = 0;
+
+	ret = st_unregister(ST_FM);
+	if (ret < 0)
+		fmerr("Failed to de-register FM from ST %d\n", ret);
+	else
+		fmdbg("Successfully unregistered from ST\n");
+
+	clear_bit(FM_CORE_READY, &fmdev->flag);
+	return ret;
+}
+
+/*
+ * Module init function. Ask FM V4L module to register video device.
+ * Allocate memory for FM driver context and RX RDS buffer.
+ */
+static int __init fm_drv_init(void)
+{
+	struct fmdev *fmdev = NULL;
+	u32 ret = -ENOMEM;
+
+	fmdbg("FM driver version %s\n", FM_DRV_VERSION);
+
+	fmdev = kzalloc(sizeof(struct fmdev), GFP_KERNEL);
+	if (NULL == fmdev) {
+		fmerr("Can't allocate operation structure memory\n");
+		return ret;
+	}
+	fmdev->rx.rds.buf_size = default_rds_buf * FM_RDS_BLK_SIZE;
+	fmdev->rx.rds.buff = kzalloc(fmdev->rx.rds.buf_size, GFP_KERNEL);
+	if (NULL == fmdev->rx.rds.buff) {
+		fmerr("Can't allocate rds ring buffer\n");
+		goto rel_dev;
+	}
+
+	ret = fm_v4l2_init_video_device(fmdev, radio_nr);
+	if (ret < 0)
+		goto rel_rdsbuf;
+
+	fmdev->irq_info.handlers = int_handler_table;
+	fmdev->curr_fmmode = FM_MODE_OFF;
+	fmdev->tx_data.pwr_lvl = FM_PWR_LVL_DEF;
+	fmdev->tx_data.preemph = FM_TX_PREEMPH_50US;
+	return ret;
+
+rel_rdsbuf:
+	kfree(fmdev->rx.rds.buff);
+rel_dev:
+	kfree(fmdev);
+
+	return ret;
+}
+
+/* Module exit function. Ask FM V4L module to unregister video device */
+static void __exit fm_drv_exit(void)
+{
+	struct fmdev *fmdev = NULL;
+
+	fmdev = fm_v4l2_deinit_video_device();
+	if (fmdev != NULL) {
+		kfree(fmdev->rx.rds.buff);
+		kfree(fmdev);
+	}
+}
+
+module_init(fm_drv_init);
+module_exit(fm_drv_exit);
+
+/* ------------- Module Info ------------- */
+MODULE_AUTHOR("Manjunatha Halli <manjunatha_halli@ti.com>");
+MODULE_DESCRIPTION("FM Driver for TI's Connectivity chip. " FM_DRV_VERSION);
+MODULE_VERSION(FM_DRV_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/wl128x/fmdrv_common.h b/drivers/media/radio/wl128x/fmdrv_common.h
new file mode 100644
index 000000000000..427c4164cece
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_common.h
@@ -0,0 +1,402 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *  FM Common module header file
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef _FMDRV_COMMON_H
+#define _FMDRV_COMMON_H
+
+#define FM_ST_REG_TIMEOUT   msecs_to_jiffies(6000)	/* 6 sec */
+#define FM_PKT_LOGICAL_CHAN_NUMBER  0x08   /* Logical channel 8 */
+
+#define REG_RD       0x1
+#define REG_WR      0x0
+
+struct fm_reg_table {
+	u8 opcode;
+	u8 type;
+	u8 *name;
+};
+
+#define STEREO_GET               0
+#define RSSI_LVL_GET             1
+#define IF_COUNT_GET             2
+#define FLAG_GET                 3
+#define RDS_SYNC_GET             4
+#define RDS_DATA_GET             5
+#define FREQ_SET                 10
+#define AF_FREQ_SET              11
+#define MOST_MODE_SET            12
+#define MOST_BLEND_SET           13
+#define DEMPH_MODE_SET           14
+#define SEARCH_LVL_SET           15
+#define BAND_SET                 16
+#define MUTE_STATUS_SET          17
+#define RDS_PAUSE_LVL_SET        18
+#define RDS_PAUSE_DUR_SET        19
+#define RDS_MEM_SET              20
+#define RDS_BLK_B_SET            21
+#define RDS_MSK_B_SET            22
+#define RDS_PI_MASK_SET          23
+#define RDS_PI_SET               24
+#define RDS_SYSTEM_SET           25
+#define INT_MASK_SET             26
+#define SEARCH_DIR_SET           27
+#define VOLUME_SET               28
+#define AUDIO_ENABLE_SET         29
+#define PCM_MODE_SET             30
+#define I2S_MODE_CONFIG_SET      31
+#define POWER_SET                32
+#define INTX_CONFIG_SET          33
+#define PULL_EN_SET              34
+#define HILO_SET                 35
+#define SWITCH2FREF              36
+#define FREQ_DRIFT_REPORT        37
+
+#define PCE_GET                  40
+#define FIRM_VER_GET             41
+#define ASIC_VER_GET             42
+#define ASIC_ID_GET              43
+#define MAN_ID_GET               44
+#define TUNER_MODE_SET           45
+#define STOP_SEARCH              46
+#define RDS_CNTRL_SET            47
+
+#define WRITE_HARDWARE_REG       100
+#define CODE_DOWNLOAD            101
+#define RESET                    102
+
+#define FM_POWER_MODE            254
+#define FM_INTERRUPT             255
+
+/* Transmitter API */
+
+#define CHANL_SET                55
+#define CHANL_BW_SET		56
+#define REF_SET                  57
+#define POWER_ENB_SET            90
+#define POWER_ATT_SET            58
+#define POWER_LEV_SET            59
+#define AUDIO_DEV_SET            60
+#define PILOT_DEV_SET            61
+#define RDS_DEV_SET              62
+#define TX_BAND_SET              65
+#define PUPD_SET                 91
+#define AUDIO_IO_SET             63
+#define PREMPH_SET               64
+#define MONO_SET                 66
+#define MUTE                     92
+#define MPX_LMT_ENABLE           67
+#define PI_SET                   93
+#define ECC_SET                  69
+#define PTY                      70
+#define AF                       71
+#define DISPLAY_MODE             74
+#define RDS_REP_SET              77
+#define RDS_CONFIG_DATA_SET      98
+#define RDS_DATA_SET             99
+#define RDS_DATA_ENB             94
+#define TA_SET                   78
+#define TP_SET                   79
+#define DI_SET                   80
+#define MS_SET                   81
+#define PS_SCROLL_SPEED          82
+#define TX_AUDIO_LEVEL_TEST      96
+#define TX_AUDIO_LEVEL_TEST_THRESHOLD    73
+#define TX_AUDIO_INPUT_LEVEL_RANGE_SET   54
+#define RX_ANTENNA_SELECT        87
+#define I2C_DEV_ADDR_SET         86
+#define REF_ERR_CALIB_PARAM_SET          88
+#define REF_ERR_CALIB_PERIODICITY_SET    89
+#define SOC_INT_TRIGGER                  52
+#define SOC_AUDIO_PATH_SET               83
+#define SOC_PCMI_OVERRIDE                84
+#define SOC_I2S_OVERRIDE         85
+#define RSSI_BLOCK_SCAN_FREQ_SET 95
+#define RSSI_BLOCK_SCAN_START    97
+#define RSSI_BLOCK_SCAN_DATA_GET  5
+#define READ_FMANT_TUNE_VALUE            104
+
+/* SKB helpers */
+struct fm_skb_cb {
+	__u8 fm_op;
+	struct completion *completion;
+};
+
+#define fm_cb(skb) ((struct fm_skb_cb *)(skb->cb))
+
+/* FM Channel-8 command message format */
+struct fm_cmd_msg_hdr {
+	__u8 hdr;		/* Logical Channel-8 */
+	__u8 len;		/* Number of bytes follows */
+	__u8 op;		/* FM Opcode */
+	__u8 rd_wr;		/* Read/Write command */
+	__u8 dlen;		/* Length of payload */
+} __attribute__ ((packed));
+
+#define FM_CMD_MSG_HDR_SIZE    5	/* sizeof(struct fm_cmd_msg_hdr) */
+
+/* FM Channel-8 event messgage format */
+struct fm_event_msg_hdr {
+	__u8 header;		/* Logical Channel-8 */
+	__u8 len;		/* Number of bytes follows */
+	__u8 status;		/* Event status */
+	__u8 num_fm_hci_cmds;	/* Number of pkts the host allowed to send */
+	__u8 op;		/* FM Opcode */
+	__u8 rd_wr;		/* Read/Write command */
+	__u8 dlen;		/* Length of payload */
+} __attribute__ ((packed));
+
+#define FM_EVT_MSG_HDR_SIZE     7	/* sizeof(struct fm_event_msg_hdr) */
+
+/* TI's magic number in firmware file */
+#define FM_FW_FILE_HEADER_MAGIC	     0x42535442
+
+#define FM_ENABLE   1
+#define FM_DISABLE  0
+
+/* FLAG_GET register bits */
+#define FM_FR_EVENT		(1 << 0)
+#define FM_BL_EVENT		(1 << 1)
+#define FM_RDS_EVENT		(1 << 2)
+#define FM_BBLK_EVENT		(1 << 3)
+#define FM_LSYNC_EVENT		(1 << 4)
+#define FM_LEV_EVENT		(1 << 5)
+#define FM_IFFR_EVENT		(1 << 6)
+#define FM_PI_EVENT		(1 << 7)
+#define FM_PD_EVENT		(1 << 8)
+#define FM_STIC_EVENT		(1 << 9)
+#define FM_MAL_EVENT		(1 << 10)
+#define FM_POW_ENB_EVENT	(1 << 11)
+
+/*
+ * Firmware files of FM. ASIC ID and ASIC version will be appened to this,
+ * later.
+ */
+#define FM_FMC_FW_FILE_START      ("fmc_ch8")
+#define FM_RX_FW_FILE_START       ("fm_rx_ch8")
+#define FM_TX_FW_FILE_START       ("fm_tx_ch8")
+
+#define FM_UNDEFINED_FREQ		   0xFFFFFFFF
+
+/* Band types */
+#define FM_BAND_EUROPE_US	0
+#define FM_BAND_JAPAN		1
+
+/* Seek directions */
+#define FM_SEARCH_DIRECTION_DOWN	0
+#define FM_SEARCH_DIRECTION_UP		1
+
+/* Tunner modes */
+#define FM_TUNER_STOP_SEARCH_MODE	0
+#define FM_TUNER_PRESET_MODE		1
+#define FM_TUNER_AUTONOMOUS_SEARCH_MODE	2
+#define FM_TUNER_AF_JUMP_MODE		3
+
+/* Min and Max volume */
+#define FM_RX_VOLUME_MIN	0
+#define FM_RX_VOLUME_MAX	70
+
+/* Volume gain step */
+#define FM_RX_VOLUME_GAIN_STEP	0x370
+
+/* Mute modes */
+#define	FM_MUTE_ON		0
+#define FM_MUTE_OFF		1
+#define	FM_MUTE_ATTENUATE	2
+
+#define FM_RX_UNMUTE_MODE		0x00
+#define FM_RX_RF_DEP_MODE		0x01
+#define FM_RX_AC_MUTE_MODE		0x02
+#define FM_RX_HARD_MUTE_LEFT_MODE	0x04
+#define FM_RX_HARD_MUTE_RIGHT_MODE	0x08
+#define FM_RX_SOFT_MUTE_FORCE_MODE	0x10
+
+/* RF dependent mute mode */
+#define FM_RX_RF_DEPENDENT_MUTE_ON	1
+#define FM_RX_RF_DEPENDENT_MUTE_OFF	0
+
+/* RSSI threshold min and max */
+#define FM_RX_RSSI_THRESHOLD_MIN	-128
+#define FM_RX_RSSI_THRESHOLD_MAX	127
+
+/* Stereo/Mono mode */
+#define FM_STEREO_MODE		0
+#define FM_MONO_MODE		1
+#define FM_STEREO_SOFT_BLEND	1
+
+/* FM RX De-emphasis filter modes */
+#define FM_RX_EMPHASIS_FILTER_50_USEC	0
+#define FM_RX_EMPHASIS_FILTER_75_USEC	1
+
+/* FM RDS modes */
+#define FM_RDS_DISABLE	0
+#define FM_RDS_ENABLE	1
+
+#define FM_NO_PI_CODE	0
+
+/* FM and RX RDS block enable/disable  */
+#define FM_RX_PWR_SET_FM_ON_RDS_OFF		0x1
+#define FM_RX_PWR_SET_FM_AND_RDS_BLK_ON		0x3
+#define FM_RX_PWR_SET_FM_AND_RDS_BLK_OFF	0x0
+
+/* RX RDS */
+#define FM_RX_RDS_FLUSH_FIFO		0x1
+#define FM_RX_RDS_FIFO_THRESHOLD	64	/* tuples */
+#define FM_RDS_BLK_SIZE		3	/* 3 bytes */
+
+/* RDS block types */
+#define FM_RDS_BLOCK_A		0
+#define FM_RDS_BLOCK_B		1
+#define FM_RDS_BLOCK_C		2
+#define FM_RDS_BLOCK_Ctag	3
+#define FM_RDS_BLOCK_D		4
+#define FM_RDS_BLOCK_E		5
+
+#define FM_RDS_BLK_IDX_A		0
+#define FM_RDS_BLK_IDX_B		1
+#define FM_RDS_BLK_IDX_C		2
+#define FM_RDS_BLK_IDX_D		3
+#define FM_RDS_BLK_IDX_UNKNOWN	0xF0
+
+#define FM_RDS_STATUS_ERR_MASK	0x18
+
+/*
+ * Represents an RDS group type & version.
+ * There are 15 groups, each group has 2 versions: A and B.
+ */
+#define FM_RDS_GROUP_TYPE_MASK_0A	    ((unsigned long)1<<0)
+#define FM_RDS_GROUP_TYPE_MASK_0B	    ((unsigned long)1<<1)
+#define FM_RDS_GROUP_TYPE_MASK_1A	    ((unsigned long)1<<2)
+#define FM_RDS_GROUP_TYPE_MASK_1B	    ((unsigned long)1<<3)
+#define FM_RDS_GROUP_TYPE_MASK_2A	    ((unsigned long)1<<4)
+#define FM_RDS_GROUP_TYPE_MASK_2B	    ((unsigned long)1<<5)
+#define FM_RDS_GROUP_TYPE_MASK_3A	    ((unsigned long)1<<6)
+#define FM_RDS_GROUP_TYPE_MASK_3B           ((unsigned long)1<<7)
+#define FM_RDS_GROUP_TYPE_MASK_4A	    ((unsigned long)1<<8)
+#define FM_RDS_GROUP_TYPE_MASK_4B	    ((unsigned long)1<<9)
+#define FM_RDS_GROUP_TYPE_MASK_5A	    ((unsigned long)1<<10)
+#define FM_RDS_GROUP_TYPE_MASK_5B	    ((unsigned long)1<<11)
+#define FM_RDS_GROUP_TYPE_MASK_6A	    ((unsigned long)1<<12)
+#define FM_RDS_GROUP_TYPE_MASK_6B	    ((unsigned long)1<<13)
+#define FM_RDS_GROUP_TYPE_MASK_7A	    ((unsigned long)1<<14)
+#define FM_RDS_GROUP_TYPE_MASK_7B	    ((unsigned long)1<<15)
+#define FM_RDS_GROUP_TYPE_MASK_8A           ((unsigned long)1<<16)
+#define FM_RDS_GROUP_TYPE_MASK_8B	    ((unsigned long)1<<17)
+#define FM_RDS_GROUP_TYPE_MASK_9A	    ((unsigned long)1<<18)
+#define FM_RDS_GROUP_TYPE_MASK_9B	    ((unsigned long)1<<19)
+#define FM_RDS_GROUP_TYPE_MASK_10A	    ((unsigned long)1<<20)
+#define FM_RDS_GROUP_TYPE_MASK_10B	    ((unsigned long)1<<21)
+#define FM_RDS_GROUP_TYPE_MASK_11A	    ((unsigned long)1<<22)
+#define FM_RDS_GROUP_TYPE_MASK_11B	    ((unsigned long)1<<23)
+#define FM_RDS_GROUP_TYPE_MASK_12A	    ((unsigned long)1<<24)
+#define FM_RDS_GROUP_TYPE_MASK_12B	    ((unsigned long)1<<25)
+#define FM_RDS_GROUP_TYPE_MASK_13A	    ((unsigned long)1<<26)
+#define FM_RDS_GROUP_TYPE_MASK_13B	    ((unsigned long)1<<27)
+#define FM_RDS_GROUP_TYPE_MASK_14A	    ((unsigned long)1<<28)
+#define FM_RDS_GROUP_TYPE_MASK_14B	    ((unsigned long)1<<29)
+#define FM_RDS_GROUP_TYPE_MASK_15A	    ((unsigned long)1<<30)
+#define FM_RDS_GROUP_TYPE_MASK_15B	    ((unsigned long)1<<31)
+
+/* RX Alternate Frequency info */
+#define FM_RDS_MIN_AF		          1
+#define FM_RDS_MAX_AF		        204
+#define FM_RDS_MAX_AF_JAPAN	        140
+#define FM_RDS_1_AF_FOLLOWS	        225
+#define FM_RDS_25_AF_FOLLOWS	        249
+
+/* RDS system type (RDS/RBDS) */
+#define FM_RDS_SYSTEM_RDS		0
+#define FM_RDS_SYSTEM_RBDS		1
+
+/* AF on/off */
+#define FM_RX_RDS_AF_SWITCH_MODE_ON	1
+#define FM_RX_RDS_AF_SWITCH_MODE_OFF	0
+
+/* Retry count when interrupt process goes wrong */
+#define FM_IRQ_TIMEOUT_RETRY_MAX	5	/* 5 times */
+
+/* Audio IO set values */
+#define FM_RX_AUDIO_ENABLE_I2S	0x01
+#define FM_RX_AUDIO_ENABLE_ANALOG	0x02
+#define FM_RX_AUDIO_ENABLE_I2S_AND_ANALOG	0x03
+#define FM_RX_AUDIO_ENABLE_DISABLE	0x00
+
+/* HI/LO set values */
+#define FM_RX_IFFREQ_TO_HI_SIDE		0x0
+#define FM_RX_IFFREQ_TO_LO_SIDE		0x1
+#define FM_RX_IFFREQ_HILO_AUTOMATIC	0x2
+
+/*
+ * Default RX mode configuration. Chip will be configured
+ * with this default values after loading RX firmware.
+ */
+#define FM_DEFAULT_RX_VOLUME		10
+#define FM_DEFAULT_RSSI_THRESHOLD	3
+
+/* Range for TX power level in units for dB/uV */
+#define FM_PWR_LVL_LOW			91
+#define FM_PWR_LVL_HIGH			122
+
+/* Chip specific default TX power level value */
+#define FM_PWR_LVL_DEF			4
+
+/* FM TX Pre-emphasis filter values */
+#define FM_TX_PREEMPH_OFF		1
+#define FM_TX_PREEMPH_50US		0
+#define FM_TX_PREEMPH_75US		2
+
+/* FM TX antenna impedence values */
+#define FM_TX_ANT_IMP_50		0
+#define FM_TX_ANT_IMP_200		1
+#define FM_TX_ANT_IMP_500		2
+
+/* Functions exported by FM common sub-module */
+u32 fmc_prepare(struct fmdev *);
+u32 fmc_release(struct fmdev *);
+
+void fmc_update_region_info(struct fmdev *, u8);
+u32 fmc_send_cmd(struct fmdev *, u8, u16,
+				void *, unsigned int, void *, int *);
+u32 fmc_is_rds_data_available(struct fmdev *, struct file *,
+				struct poll_table_struct *);
+u32 fmc_transfer_rds_from_internal_buff(struct fmdev *, struct file *,
+					u8 __user *, size_t);
+
+u32 fmc_set_freq(struct fmdev *, u32);
+u32 fmc_set_mode(struct fmdev *, u8);
+u32 fmc_set_region(struct fmdev *, u8);
+u32 fmc_set_mute_mode(struct fmdev *, u8);
+u32 fmc_set_stereo_mono(struct fmdev *, u16);
+u32 fmc_set_rds_mode(struct fmdev *, u8);
+
+u32 fmc_get_freq(struct fmdev *, u32 *);
+u32 fmc_get_region(struct fmdev *, u8 *);
+u32 fmc_get_mode(struct fmdev *, u8 *);
+
+/*
+ * channel spacing
+ */
+#define FM_CHANNEL_SPACING_50KHZ 1
+#define FM_CHANNEL_SPACING_100KHZ 2
+#define FM_CHANNEL_SPACING_200KHZ 4
+#define FM_FREQ_MUL 50
+
+#endif
+
diff --git a/drivers/media/radio/wl128x/fmdrv_rx.c b/drivers/media/radio/wl128x/fmdrv_rx.c
new file mode 100644
index 000000000000..ec529b55b040
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_rx.c
@@ -0,0 +1,847 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *  This sub-module of FM driver implements FM RX functionality.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *  Author: Raja Mani <raja_mani@ti.com>
+ *  Author: Manjunatha Halli <manjunatha_halli@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, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include "fmdrv.h"
+#include "fmdrv_common.h"
+#include "fmdrv_rx.h"
+
+void fm_rx_reset_rds_cache(struct fmdev *fmdev)
+{
+	fmdev->rx.rds.flag = FM_RDS_DISABLE;
+	fmdev->rx.rds.last_blk_idx = 0;
+	fmdev->rx.rds.wr_idx = 0;
+	fmdev->rx.rds.rd_idx = 0;
+
+	if (fmdev->rx.af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON)
+		fmdev->irq_info.mask |= FM_LEV_EVENT;
+}
+
+void fm_rx_reset_station_info(struct fmdev *fmdev)
+{
+	fmdev->rx.stat_info.picode = FM_NO_PI_CODE;
+	fmdev->rx.stat_info.afcache_size = 0;
+	fmdev->rx.stat_info.af_list_max = 0;
+}
+
+u32 fm_rx_set_freq(struct fmdev *fmdev, u32 freq)
+{
+	unsigned long timeleft;
+	u16 payload, curr_frq, intr_flag;
+	u32 curr_frq_in_khz;
+	u32 ret, resp_len;
+
+	if (freq < fmdev->rx.region.bot_freq || freq > fmdev->rx.region.top_freq) {
+		fmerr("Invalid frequency %d\n", freq);
+		return -EINVAL;
+	}
+
+	/* Set audio enable */
+	payload = FM_RX_AUDIO_ENABLE_I2S_AND_ANALOG;
+
+	ret = fmc_send_cmd(fmdev, AUDIO_ENABLE_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Set hilo to automatic selection */
+	payload = FM_RX_IFFREQ_HILO_AUTOMATIC;
+	ret = fmc_send_cmd(fmdev, HILO_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Calculate frequency index and set*/
+	payload = (freq - fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
+
+	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Read flags - just to clear any pending interrupts if we had */
+	ret = fmc_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, 2, NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Enable FR, BL interrupts */
+	intr_flag = fmdev->irq_info.mask;
+	fmdev->irq_info.mask = (FM_FR_EVENT | FM_BL_EVENT);
+	payload = fmdev->irq_info.mask;
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Start tune */
+	payload = FM_TUNER_PRESET_MODE;
+	ret = fmc_send_cmd(fmdev, TUNER_MODE_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		goto exit;
+
+	/* Wait for tune ended interrupt */
+	init_completion(&fmdev->maintask_comp);
+	timeleft = wait_for_completion_timeout(&fmdev->maintask_comp,
+			FM_DRV_TX_TIMEOUT);
+	if (!timeleft) {
+		fmerr("Timeout(%d sec),didn't get tune ended int\n",
+			   jiffies_to_msecs(FM_DRV_TX_TIMEOUT) / 1000);
+		ret = -ETIMEDOUT;
+		goto exit;
+	}
+
+	/* Read freq back to confirm */
+	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_RD, NULL, 2, &curr_frq, &resp_len);
+	if (ret < 0)
+		goto exit;
+
+	curr_frq = be16_to_cpu(curr_frq);
+	curr_frq_in_khz = (fmdev->rx.region.bot_freq + ((u32)curr_frq * FM_FREQ_MUL));
+
+	if (curr_frq_in_khz != freq) {
+		pr_info("Frequency is set to (%d) but "
+			   "requested freq is (%d)\n", curr_frq_in_khz, freq);
+	}
+
+	/* Update local cache  */
+	fmdev->rx.freq = curr_frq_in_khz;
+exit:
+	/* Re-enable default FM interrupts */
+	fmdev->irq_info.mask = intr_flag;
+	payload = fmdev->irq_info.mask;
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Reset RDS cache and current station pointers */
+	fm_rx_reset_rds_cache(fmdev);
+	fm_rx_reset_station_info(fmdev);
+
+	return ret;
+}
+
+static u32 fm_rx_set_channel_spacing(struct fmdev *fmdev, u32 spacing)
+{
+	u16 payload;
+	u32 ret;
+
+	if (spacing > 0 && spacing <= 50000)
+		spacing = FM_CHANNEL_SPACING_50KHZ;
+	else if (spacing > 50000 && spacing <= 100000)
+		spacing = FM_CHANNEL_SPACING_100KHZ;
+	else
+		spacing = FM_CHANNEL_SPACING_200KHZ;
+
+	/* set channel spacing */
+	payload = spacing;
+	ret = fmc_send_cmd(fmdev, CHANL_BW_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->rx.region.chanl_space = spacing * FM_FREQ_MUL;
+
+	return ret;
+}
+
+u32 fm_rx_seek(struct fmdev *fmdev, u32 seek_upward,
+		u32 wrap_around, u32 spacing)
+{
+	u32 resp_len;
+	u16 curr_frq, next_frq, last_frq;
+	u16 payload, int_reason, intr_flag;
+	u16 offset, space_idx;
+	unsigned long timeleft;
+	u32 ret;
+
+	/* Set channel spacing */
+	ret = fm_rx_set_channel_spacing(fmdev, spacing);
+	if (ret < 0) {
+		fmerr("Failed to set channel spacing\n");
+		return ret;
+	}
+
+	/* Read the current frequency from chip */
+	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_RD, NULL,
+			sizeof(curr_frq), &curr_frq, &resp_len);
+	if (ret < 0)
+		return ret;
+
+	curr_frq = be16_to_cpu(curr_frq);
+	last_frq = (fmdev->rx.region.top_freq - fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
+
+	/* Check the offset in order to be aligned to the channel spacing*/
+	space_idx = fmdev->rx.region.chanl_space / FM_FREQ_MUL;
+	offset = curr_frq % space_idx;
+
+	next_frq = seek_upward ? curr_frq + space_idx /* Seek Up */ :
+				curr_frq - space_idx /* Seek Down */ ;
+
+	/*
+	 * Add or subtract offset in order to stay aligned to the channel
+	 * spacing.
+	 */
+	if ((short)next_frq < 0)
+		next_frq = last_frq - offset;
+	else if (next_frq > last_frq)
+		next_frq = 0 + offset;
+
+again:
+	/* Set calculated next frequency to perform seek */
+	payload = next_frq;
+	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Set search direction (0:Seek Down, 1:Seek Up) */
+	payload = (seek_upward ? FM_SEARCH_DIRECTION_UP : FM_SEARCH_DIRECTION_DOWN);
+	ret = fmc_send_cmd(fmdev, SEARCH_DIR_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Read flags - just to clear any pending interrupts if we had */
+	ret = fmc_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, 2, NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Enable FR, BL interrupts */
+	intr_flag = fmdev->irq_info.mask;
+	fmdev->irq_info.mask = (FM_FR_EVENT | FM_BL_EVENT);
+	payload = fmdev->irq_info.mask;
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Start seek */
+	payload = FM_TUNER_AUTONOMOUS_SEARCH_MODE;
+	ret = fmc_send_cmd(fmdev, TUNER_MODE_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Wait for tune ended/band limit reached interrupt */
+	init_completion(&fmdev->maintask_comp);
+	timeleft = wait_for_completion_timeout(&fmdev->maintask_comp,
+			FM_DRV_RX_SEEK_TIMEOUT);
+	if (!timeleft) {
+		fmerr("Timeout(%d sec),didn't get tune ended int\n",
+			   jiffies_to_msecs(FM_DRV_RX_SEEK_TIMEOUT) / 1000);
+		return -ETIMEDOUT;
+	}
+
+	int_reason = fmdev->irq_info.flag & (FM_TUNE_COMPLETE | FM_BAND_LIMIT);
+
+	/* Re-enable default FM interrupts */
+	fmdev->irq_info.mask = intr_flag;
+	payload = fmdev->irq_info.mask;
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	if (int_reason & FM_BL_EVENT) {
+		if (wrap_around == 0) {
+			fmdev->rx.freq = seek_upward ?
+				fmdev->rx.region.top_freq :
+				fmdev->rx.region.bot_freq;
+		} else {
+			fmdev->rx.freq = seek_upward ?
+				fmdev->rx.region.bot_freq :
+				fmdev->rx.region.top_freq;
+			/* Calculate frequency index to write */
+			next_frq = (fmdev->rx.freq -
+					fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
+			goto again;
+		}
+	} else {
+		/* Read freq to know where operation tune operation stopped */
+		ret = fmc_send_cmd(fmdev, FREQ_SET, REG_RD, NULL, 2,
+				&curr_frq, &resp_len);
+		if (ret < 0)
+			return ret;
+
+		curr_frq = be16_to_cpu(curr_frq);
+		fmdev->rx.freq = (fmdev->rx.region.bot_freq +
+				((u32)curr_frq * FM_FREQ_MUL));
+
+	}
+	/* Reset RDS cache and current station pointers */
+	fm_rx_reset_rds_cache(fmdev);
+	fm_rx_reset_station_info(fmdev);
+
+	return ret;
+}
+
+u32 fm_rx_set_volume(struct fmdev *fmdev, u16 vol_to_set)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (vol_to_set < FM_RX_VOLUME_MIN || vol_to_set > FM_RX_VOLUME_MAX) {
+		fmerr("Volume is not within(%d-%d) range\n",
+			   FM_RX_VOLUME_MIN, FM_RX_VOLUME_MAX);
+		return -EINVAL;
+	}
+	vol_to_set *= FM_RX_VOLUME_GAIN_STEP;
+
+	payload = vol_to_set;
+	ret = fmc_send_cmd(fmdev, VOLUME_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->rx.volume = vol_to_set;
+	return ret;
+}
+
+/* Get volume */
+u32 fm_rx_get_volume(struct fmdev *fmdev, u16 *curr_vol)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (curr_vol == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*curr_vol = fmdev->rx.volume / FM_RX_VOLUME_GAIN_STEP;
+
+	return 0;
+}
+
+/* To get current band's bottom and top frequency */
+u32 fm_rx_get_band_freq_range(struct fmdev *fmdev, u32 *bot_freq, u32 *top_freq)
+{
+	if (bot_freq != NULL)
+		*bot_freq = fmdev->rx.region.bot_freq;
+
+	if (top_freq != NULL)
+		*top_freq = fmdev->rx.region.top_freq;
+
+	return 0;
+}
+
+/* Returns current band index (0-Europe/US; 1-Japan) */
+void fm_rx_get_region(struct fmdev *fmdev, u8 *region)
+{
+	*region = fmdev->rx.region.fm_band;
+}
+
+/* Sets band (0-Europe/US; 1-Japan) */
+u32 fm_rx_set_region(struct fmdev *fmdev, u8 region_to_set)
+{
+	u16 payload;
+	u32 new_frq = 0;
+	u32 ret;
+
+	if (region_to_set != FM_BAND_EUROPE_US &&
+	    region_to_set != FM_BAND_JAPAN) {
+		fmerr("Invalid band\n");
+		return -EINVAL;
+	}
+
+	if (fmdev->rx.region.fm_band == region_to_set) {
+		fmerr("Requested band is already configured\n");
+		return 0;
+	}
+
+	/* Send cmd to set the band  */
+	payload = (u16)region_to_set;
+	ret = fmc_send_cmd(fmdev, BAND_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmc_update_region_info(fmdev, region_to_set);
+
+	/* Check whether current RX frequency is within band boundary */
+	if (fmdev->rx.freq < fmdev->rx.region.bot_freq)
+		new_frq = fmdev->rx.region.bot_freq;
+	else if (fmdev->rx.freq > fmdev->rx.region.top_freq)
+		new_frq = fmdev->rx.region.top_freq;
+
+	if (new_frq) {
+		fmdbg("Current freq is not within band limit boundary,"
+				"switching to %d KHz\n", new_frq);
+		 /* Current RX frequency is not in range. So, update it */
+		ret = fm_rx_set_freq(fmdev, new_frq);
+	}
+
+	return ret;
+}
+
+/* Reads current mute mode (Mute Off/On/Attenuate)*/
+u32 fm_rx_get_mute_mode(struct fmdev *fmdev, u8 *curr_mute_mode)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (curr_mute_mode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*curr_mute_mode = fmdev->rx.mute_mode;
+
+	return 0;
+}
+
+static u32 fm_config_rx_mute_reg(struct fmdev *fmdev)
+{
+	u16 payload, muteval;
+	u32 ret;
+
+	muteval = 0;
+	switch (fmdev->rx.mute_mode) {
+	case FM_MUTE_ON:
+		muteval = FM_RX_AC_MUTE_MODE;
+		break;
+
+	case FM_MUTE_OFF:
+		muteval = FM_RX_UNMUTE_MODE;
+		break;
+
+	case FM_MUTE_ATTENUATE:
+		muteval = FM_RX_SOFT_MUTE_FORCE_MODE;
+		break;
+	}
+	if (fmdev->rx.rf_depend_mute == FM_RX_RF_DEPENDENT_MUTE_ON)
+		muteval |= FM_RX_RF_DEP_MODE;
+	else
+		muteval &= ~FM_RX_RF_DEP_MODE;
+
+	payload = muteval;
+	ret = fmc_send_cmd(fmdev, MUTE_STATUS_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/* Configures mute mode (Mute Off/On/Attenuate) */
+u32 fm_rx_set_mute_mode(struct fmdev *fmdev, u8 mute_mode_toset)
+{
+	u8 org_state;
+	u32 ret;
+
+	if (fmdev->rx.mute_mode == mute_mode_toset)
+		return 0;
+
+	org_state = fmdev->rx.mute_mode;
+	fmdev->rx.mute_mode = mute_mode_toset;
+
+	ret = fm_config_rx_mute_reg(fmdev);
+	if (ret < 0) {
+		fmdev->rx.mute_mode = org_state;
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Gets RF dependent soft mute mode enable/disable status */
+u32 fm_rx_get_rfdepend_softmute(struct fmdev *fmdev, u8 *curr_mute_mode)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (curr_mute_mode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*curr_mute_mode = fmdev->rx.rf_depend_mute;
+
+	return 0;
+}
+
+/* Sets RF dependent soft mute mode */
+u32 fm_rx_set_rfdepend_softmute(struct fmdev *fmdev, u8 rfdepend_mute)
+{
+	u8 org_state;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (rfdepend_mute != FM_RX_RF_DEPENDENT_MUTE_ON &&
+	    rfdepend_mute != FM_RX_RF_DEPENDENT_MUTE_OFF) {
+		fmerr("Invalid RF dependent soft mute\n");
+		return -EINVAL;
+	}
+	if (fmdev->rx.rf_depend_mute == rfdepend_mute)
+		return 0;
+
+	org_state = fmdev->rx.rf_depend_mute;
+	fmdev->rx.rf_depend_mute = rfdepend_mute;
+
+	ret = fm_config_rx_mute_reg(fmdev);
+	if (ret < 0) {
+		fmdev->rx.rf_depend_mute = org_state;
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Returns the signal strength level of current channel */
+u32 fm_rx_get_rssi_level(struct fmdev *fmdev, u16 *rssilvl)
+{
+	u16 curr_rssi_lel;
+	u32 resp_len;
+	u32 ret;
+
+	if (rssilvl == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+	/* Read current RSSI level */
+	ret = fmc_send_cmd(fmdev, RSSI_LVL_GET, REG_RD, NULL, 2,
+			&curr_rssi_lel, &resp_len);
+	if (ret < 0)
+		return ret;
+
+	*rssilvl = be16_to_cpu(curr_rssi_lel);
+
+	return 0;
+}
+
+/*
+ * Sets the signal strength level that once reached
+ * will stop the auto search process
+ */
+u32 fm_rx_set_rssi_threshold(struct fmdev *fmdev, short rssi_lvl_toset)
+{
+	u16 payload;
+	u32 ret;
+
+	if (rssi_lvl_toset < FM_RX_RSSI_THRESHOLD_MIN ||
+			rssi_lvl_toset > FM_RX_RSSI_THRESHOLD_MAX) {
+		fmerr("Invalid RSSI threshold level\n");
+		return -EINVAL;
+	}
+	payload = (u16)rssi_lvl_toset;
+	ret = fmc_send_cmd(fmdev, SEARCH_LVL_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->rx.rssi_threshold = rssi_lvl_toset;
+
+	return 0;
+}
+
+/* Returns current RX RSSI threshold value */
+u32 fm_rx_get_rssi_threshold(struct fmdev *fmdev, short *curr_rssi_lvl)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (curr_rssi_lvl == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*curr_rssi_lvl = fmdev->rx.rssi_threshold;
+
+	return 0;
+}
+
+/* Sets RX stereo/mono modes */
+u32 fm_rx_set_stereo_mono(struct fmdev *fmdev, u16 mode)
+{
+	u16 payload;
+	u32 ret;
+
+	if (mode != FM_STEREO_MODE && mode != FM_MONO_MODE) {
+		fmerr("Invalid mode\n");
+		return -EINVAL;
+	}
+
+	/* Set stereo/mono mode */
+	payload = (u16)mode;
+	ret = fmc_send_cmd(fmdev, MOST_MODE_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Set stereo blending mode */
+	payload = FM_STEREO_SOFT_BLEND;
+	ret = fmc_send_cmd(fmdev, MOST_BLEND_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/* Gets current RX stereo/mono mode */
+u32 fm_rx_get_stereo_mono(struct fmdev *fmdev, u16 *mode)
+{
+	u16 curr_mode;
+	u32 ret, resp_len;
+
+	if (mode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	ret = fmc_send_cmd(fmdev, MOST_MODE_SET, REG_RD, NULL, 2,
+			&curr_mode, &resp_len);
+	if (ret < 0)
+		return ret;
+
+	*mode = be16_to_cpu(curr_mode);
+
+	return 0;
+}
+
+/* Choose RX de-emphasis filter mode (50us/75us) */
+u32 fm_rx_set_deemphasis_mode(struct fmdev *fmdev, u16 mode)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (mode != FM_RX_EMPHASIS_FILTER_50_USEC &&
+			mode != FM_RX_EMPHASIS_FILTER_75_USEC) {
+		fmerr("Invalid rx de-emphasis mode (%d)\n", mode);
+		return -EINVAL;
+	}
+
+	payload = mode;
+	ret = fmc_send_cmd(fmdev, DEMPH_MODE_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->rx.deemphasis_mode = mode;
+
+	return 0;
+}
+
+/* Gets current RX de-emphasis filter mode */
+u32 fm_rx_get_deemph_mode(struct fmdev *fmdev, u16 *curr_deemphasis_mode)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (curr_deemphasis_mode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*curr_deemphasis_mode = fmdev->rx.deemphasis_mode;
+
+	return 0;
+}
+
+/* Enable/Disable RX RDS */
+u32 fm_rx_set_rds_mode(struct fmdev *fmdev, u8 rds_en_dis)
+{
+	u16 payload;
+	u32 ret;
+
+	if (rds_en_dis != FM_RDS_ENABLE && rds_en_dis != FM_RDS_DISABLE) {
+		fmerr("Invalid rds option\n");
+		return -EINVAL;
+	}
+
+	if (rds_en_dis == FM_RDS_ENABLE
+	    && fmdev->rx.rds.flag == FM_RDS_DISABLE) {
+		/* Turn on RX RDS and RDS circuit */
+		payload = FM_RX_PWR_SET_FM_AND_RDS_BLK_ON;
+		ret = fmc_send_cmd(fmdev, POWER_SET, REG_WR, &payload,
+				sizeof(payload), NULL, NULL);
+		if (ret < 0)
+			return ret;
+
+		/* Clear and reset RDS FIFO */
+		payload = FM_RX_RDS_FLUSH_FIFO;
+		ret = fmc_send_cmd(fmdev, RDS_CNTRL_SET, REG_WR, &payload,
+		sizeof(payload), NULL, NULL);
+		if (ret < 0)
+			return ret;
+
+		/* Read flags - just to clear any pending interrupts. */
+		ret = fmc_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, 2,
+				NULL, NULL);
+		if (ret < 0)
+			return ret;
+
+		/* Set RDS FIFO threshold value */
+		payload = FM_RX_RDS_FIFO_THRESHOLD;
+		ret = fmc_send_cmd(fmdev, RDS_MEM_SET, REG_WR, &payload,
+		sizeof(payload), NULL, NULL);
+		if (ret < 0)
+			return ret;
+
+		/* Enable RDS interrupt */
+		fmdev->irq_info.mask |= FM_RDS_EVENT;
+		payload = fmdev->irq_info.mask;
+		ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+				sizeof(payload), NULL, NULL);
+		if (ret < 0) {
+			fmdev->irq_info.mask &= ~FM_RDS_EVENT;
+			return ret;
+		}
+
+		/* Update our local flag */
+		fmdev->rx.rds.flag = FM_RDS_ENABLE;
+	} else if (rds_en_dis == FM_RDS_DISABLE
+		   && fmdev->rx.rds.flag == FM_RDS_ENABLE) {
+		/* Turn off RX RDS */
+		payload = FM_RX_PWR_SET_FM_ON_RDS_OFF;
+		ret = fmc_send_cmd(fmdev, POWER_SET, REG_WR, &payload,
+				sizeof(payload), NULL, NULL);
+		if (ret < 0)
+			return ret;
+
+		/* Reset RDS pointers */
+		fmdev->rx.rds.last_blk_idx = 0;
+		fmdev->rx.rds.wr_idx = 0;
+		fmdev->rx.rds.rd_idx = 0;
+		fm_rx_reset_station_info(fmdev);
+
+		/* Update RDS local cache */
+		fmdev->irq_info.mask &= ~(FM_RDS_EVENT);
+		fmdev->rx.rds.flag = FM_RDS_DISABLE;
+	}
+
+	return 0;
+}
+
+/* Returns current RX RDS enable/disable status */
+u32 fm_rx_get_rds_mode(struct fmdev *fmdev, u8 *curr_rds_en_dis)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (curr_rds_en_dis == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*curr_rds_en_dis = fmdev->rx.rds.flag;
+
+	return 0;
+}
+
+/* Sets RDS operation mode (RDS/RDBS) */
+u32 fm_rx_set_rds_system(struct fmdev *fmdev, u8 rds_mode)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (rds_mode != FM_RDS_SYSTEM_RDS && rds_mode != FM_RDS_SYSTEM_RBDS) {
+		fmerr("Invalid rds mode\n");
+		return -EINVAL;
+	}
+	/* Set RDS operation mode */
+	payload = (u16)rds_mode;
+	ret = fmc_send_cmd(fmdev, RDS_SYSTEM_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->rx.rds_mode = rds_mode;
+
+	return 0;
+}
+
+/* Returns current RDS operation mode */
+u32 fm_rx_get_rds_system(struct fmdev *fmdev, u8 *rds_mode)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (rds_mode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*rds_mode = fmdev->rx.rds_mode;
+
+	return 0;
+}
+
+/* Configures Alternate Frequency switch mode */
+u32 fm_rx_set_af_switch(struct fmdev *fmdev, u8 af_mode)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (af_mode != FM_RX_RDS_AF_SWITCH_MODE_ON &&
+	    af_mode != FM_RX_RDS_AF_SWITCH_MODE_OFF) {
+		fmerr("Invalid af mode\n");
+		return -EINVAL;
+	}
+	/* Enable/disable low RSSI interrupt based on af_mode */
+	if (af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON)
+		fmdev->irq_info.mask |= FM_LEV_EVENT;
+	else
+		fmdev->irq_info.mask &= ~FM_LEV_EVENT;
+
+	payload = fmdev->irq_info.mask;
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->rx.af_mode = af_mode;
+
+	return 0;
+}
+
+/* Returns Alternate Frequency switch status */
+u32 fm_rx_get_af_switch(struct fmdev *fmdev, u8 *af_mode)
+{
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	if (af_mode == NULL) {
+		fmerr("Invalid memory\n");
+		return -ENOMEM;
+	}
+
+	*af_mode = fmdev->rx.af_mode;
+
+	return 0;
+}
diff --git a/drivers/media/radio/wl128x/fmdrv_rx.h b/drivers/media/radio/wl128x/fmdrv_rx.h
new file mode 100644
index 000000000000..329e62f6be76
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_rx.h
@@ -0,0 +1,59 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *  FM RX module header.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef _FMDRV_RX_H
+#define _FMDRV_RX_H
+
+u32 fm_rx_set_freq(struct fmdev *, u32);
+u32 fm_rx_set_mute_mode(struct fmdev *, u8);
+u32 fm_rx_set_stereo_mono(struct fmdev *, u16);
+u32 fm_rx_set_rds_mode(struct fmdev *, u8);
+u32 fm_rx_set_rds_system(struct fmdev *, u8);
+u32 fm_rx_set_volume(struct fmdev *, u16);
+u32 fm_rx_set_rssi_threshold(struct fmdev *, short);
+u32 fm_rx_set_region(struct fmdev *, u8);
+u32 fm_rx_set_rfdepend_softmute(struct fmdev *, u8);
+u32 fm_rx_set_deemphasis_mode(struct fmdev *, u16);
+u32 fm_rx_set_af_switch(struct fmdev *, u8);
+
+void fm_rx_reset_rds_cache(struct fmdev *);
+void fm_rx_reset_station_info(struct fmdev *);
+
+u32 fm_rx_seek(struct fmdev *, u32, u32, u32);
+
+u32 fm_rx_get_rds_mode(struct fmdev *, u8 *);
+u32 fm_rx_get_rds_system(struct fmdev *, u8 *);
+u32 fm_rx_get_mute_mode(struct fmdev *, u8 *);
+u32 fm_rx_get_volume(struct fmdev *, u16 *);
+u32 fm_rx_get_band_freq_range(struct fmdev *,
+					u32 *, u32 *);
+u32 fm_rx_get_stereo_mono(struct fmdev *, u16 *);
+u32 fm_rx_get_rssi_level(struct fmdev *, u16 *);
+u32 fm_rx_get_rssi_threshold(struct fmdev *, short *);
+u32 fm_rx_get_rfdepend_softmute(struct fmdev *, u8 *);
+u32 fm_rx_get_deemph_mode(struct fmdev *, u16 *);
+u32 fm_rx_get_af_switch(struct fmdev *, u8 *);
+void fm_rx_get_region(struct fmdev *, u8 *);
+
+u32 fm_rx_set_chanl_spacing(struct fmdev *, u8);
+u32 fm_rx_get_chanl_spacing(struct fmdev *, u8 *);
+#endif
+
diff --git a/drivers/media/radio/wl128x/fmdrv_tx.c b/drivers/media/radio/wl128x/fmdrv_tx.c
new file mode 100644
index 000000000000..be54068b56a8
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_tx.c
@@ -0,0 +1,425 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *  This sub-module of FM driver implements FM TX functionality.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  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/delay.h>
+#include "fmdrv.h"
+#include "fmdrv_common.h"
+#include "fmdrv_tx.h"
+
+u32 fm_tx_set_stereo_mono(struct fmdev *fmdev, u16 mode)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->tx_data.aud_mode == mode)
+		return 0;
+
+	fmdbg("stereo mode: %d\n", mode);
+
+	/* Set Stereo/Mono mode */
+	payload = (1 - mode);
+	ret = fmc_send_cmd(fmdev, MONO_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fmdev->tx_data.aud_mode = mode;
+
+	return ret;
+}
+
+static u32 set_rds_text(struct fmdev *fmdev, u8 *rds_text)
+{
+	u16 payload;
+	u32 ret;
+
+	ret = fmc_send_cmd(fmdev, RDS_DATA_SET, REG_WR, rds_text,
+			strlen(rds_text), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Scroll mode */
+	payload = (u16)0x1;
+	ret = fmc_send_cmd(fmdev, DISPLAY_MODE, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static u32 set_rds_data_mode(struct fmdev *fmdev, u8 mode)
+{
+	u16 payload;
+	u32 ret;
+
+	/* Setting unique PI TODO: how unique? */
+	payload = (u16)0xcafe;
+	ret = fmc_send_cmd(fmdev, PI_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Set decoder id */
+	payload = (u16)0xa;
+	ret = fmc_send_cmd(fmdev, DI_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* TODO: RDS_MODE_GET? */
+	return 0;
+}
+
+static u32 set_rds_len(struct fmdev *fmdev, u8 type, u16 len)
+{
+	u16 payload;
+	u32 ret;
+
+	len |= type << 8;
+	payload = len;
+	ret = fmc_send_cmd(fmdev, RDS_CONFIG_DATA_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* TODO: LENGTH_GET? */
+	return 0;
+}
+
+u32 fm_tx_set_rds_mode(struct fmdev *fmdev, u8 rds_en_dis)
+{
+	u16 payload;
+	u32 ret;
+	u8 rds_text[] = "Zoom2\n";
+
+	fmdbg("rds_en_dis:%d(E:%d, D:%d)\n", rds_en_dis,
+		   FM_RDS_ENABLE, FM_RDS_DISABLE);
+
+	if (rds_en_dis == FM_RDS_ENABLE) {
+		/* Set RDS length */
+		set_rds_len(fmdev, 0, strlen(rds_text));
+
+		/* Set RDS text */
+		set_rds_text(fmdev, rds_text);
+
+		/* Set RDS mode */
+		set_rds_data_mode(fmdev, 0x0);
+	}
+
+	/* Send command to enable RDS */
+	if (rds_en_dis == FM_RDS_ENABLE)
+		payload = 0x01;
+	else
+		payload = 0x00;
+
+	ret = fmc_send_cmd(fmdev, RDS_DATA_ENB, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	if (rds_en_dis == FM_RDS_ENABLE) {
+		/* Set RDS length */
+		set_rds_len(fmdev, 0, strlen(rds_text));
+
+		/* Set RDS text */
+		set_rds_text(fmdev, rds_text);
+	}
+	fmdev->tx_data.rds.flag = rds_en_dis;
+
+	return 0;
+}
+
+u32 fm_tx_set_radio_text(struct fmdev *fmdev, u8 *rds_text, u8 rds_type)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX)
+		return -EPERM;
+
+	fm_tx_set_rds_mode(fmdev, 0);
+
+	/* Set RDS length */
+	set_rds_len(fmdev, rds_type, strlen(rds_text));
+
+	/* Set RDS text */
+	set_rds_text(fmdev, rds_text);
+
+	/* Set RDS mode */
+	set_rds_data_mode(fmdev, 0x0);
+
+	payload = 1;
+	ret = fmc_send_cmd(fmdev, RDS_DATA_ENB, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+u32 fm_tx_set_af(struct fmdev *fmdev, u32 af)
+{
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX)
+		return -EPERM;
+
+	fmdbg("AF: %d\n", af);
+
+	af = (af - 87500) / 100;
+	payload = (u16)af;
+	ret = fmc_send_cmd(fmdev, TA_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+u32 fm_tx_set_region(struct fmdev *fmdev, u8 region)
+{
+	u16 payload;
+	u32 ret;
+
+	if (region != FM_BAND_EUROPE_US && region != FM_BAND_JAPAN) {
+		fmerr("Invalid band\n");
+		return -EINVAL;
+	}
+
+	/* Send command to set the band */
+	payload = (u16)region;
+	ret = fmc_send_cmd(fmdev, TX_BAND_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+u32 fm_tx_set_mute_mode(struct fmdev *fmdev, u8 mute_mode_toset)
+{
+	u16 payload;
+	u32 ret;
+
+	fmdbg("tx: mute mode %d\n", mute_mode_toset);
+
+	payload = mute_mode_toset;
+	ret = fmc_send_cmd(fmdev, MUTE, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/* Set TX Audio I/O */
+static u32 set_audio_io(struct fmdev *fmdev)
+{
+	struct fmtx_data *tx = &fmdev->tx_data;
+	u16 payload;
+	u32 ret;
+
+	/* Set Audio I/O Enable */
+	payload = tx->audio_io;
+	ret = fmc_send_cmd(fmdev, AUDIO_IO_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* TODO: is audio set? */
+	return 0;
+}
+
+/* Start TX Transmission */
+static u32 enable_xmit(struct fmdev *fmdev, u8 new_xmit_state)
+{
+	struct fmtx_data *tx = &fmdev->tx_data;
+	unsigned long timeleft;
+	u16 payload;
+	u32 ret;
+
+	/* Enable POWER_ENB interrupts */
+	payload = FM_POW_ENB_EVENT;
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Set Power Enable */
+	payload = new_xmit_state;
+	ret = fmc_send_cmd(fmdev, POWER_ENB_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* Wait for Power Enabled */
+	init_completion(&fmdev->maintask_comp);
+	timeleft = wait_for_completion_timeout(&fmdev->maintask_comp,
+			FM_DRV_TX_TIMEOUT);
+	if (!timeleft) {
+		fmerr("Timeout(%d sec),didn't get tune ended interrupt\n",
+			   jiffies_to_msecs(FM_DRV_TX_TIMEOUT) / 1000);
+		return -ETIMEDOUT;
+	}
+
+	set_bit(FM_CORE_TX_XMITING, &fmdev->flag);
+	tx->xmit_state = new_xmit_state;
+
+	return 0;
+}
+
+/* Set TX power level */
+u32 fm_tx_set_pwr_lvl(struct fmdev *fmdev, u8 new_pwr_lvl)
+{
+	u16 payload;
+	struct fmtx_data *tx = &fmdev->tx_data;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX)
+		return -EPERM;
+	fmdbg("tx: pwr_level_to_set %ld\n", (long int)new_pwr_lvl);
+
+	/* If the core isn't ready update global variable */
+	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
+		tx->pwr_lvl = new_pwr_lvl;
+		return 0;
+	}
+
+	/* Set power level: Application will specify power level value in
+	 * units of dB/uV, whereas range and step are specific to FM chip.
+	 * For TI's WL chips, convert application specified power level value
+	 * to chip specific value by subtracting 122 from it. Refer to TI FM
+	 * data sheet for details.
+	 * */
+
+	payload = (FM_PWR_LVL_HIGH - new_pwr_lvl);
+	ret = fmc_send_cmd(fmdev, POWER_LEV_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	/* TODO: is the power level set? */
+	tx->pwr_lvl = new_pwr_lvl;
+
+	return 0;
+}
+
+/*
+ * Sets FM TX pre-emphasis filter value (OFF, 50us, or 75us)
+ * Convert V4L2 specified filter values to chip specific filter values.
+ */
+u32 fm_tx_set_preemph_filter(struct fmdev *fmdev, u32 preemphasis)
+{
+	struct fmtx_data *tx = &fmdev->tx_data;
+	u16 payload;
+	u32 ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX)
+		return -EPERM;
+
+	switch (preemphasis) {
+	case V4L2_PREEMPHASIS_DISABLED:
+		payload = FM_TX_PREEMPH_OFF;
+		break;
+	case V4L2_PREEMPHASIS_50_uS:
+		payload = FM_TX_PREEMPH_50US;
+		break;
+	case V4L2_PREEMPHASIS_75_uS:
+		payload = FM_TX_PREEMPH_75US;
+		break;
+	}
+
+	ret = fmc_send_cmd(fmdev, PREMPH_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	tx->preemph = payload;
+
+	return ret;
+}
+
+/* Get the TX tuning capacitor value.*/
+u32 fm_tx_get_tune_cap_val(struct fmdev *fmdev)
+{
+	u16 curr_val;
+	u32 ret, resp_len;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX)
+		return -EPERM;
+
+	ret = fmc_send_cmd(fmdev, READ_FMANT_TUNE_VALUE, REG_RD,
+			NULL, sizeof(curr_val), &curr_val, &resp_len);
+	if (ret < 0)
+		return ret;
+
+	curr_val = be16_to_cpu(curr_val);
+
+	return curr_val;
+}
+
+/* Set TX Frequency */
+u32 fm_tx_set_freq(struct fmdev *fmdev, u32 freq_to_set)
+{
+	struct fmtx_data *tx = &fmdev->tx_data;
+	u16 payload, chanl_index;
+	u32 ret;
+
+	if (test_bit(FM_CORE_TX_XMITING, &fmdev->flag)) {
+		enable_xmit(fmdev, 0);
+		clear_bit(FM_CORE_TX_XMITING, &fmdev->flag);
+	}
+
+	/* Enable FR, BL interrupts */
+	payload = (FM_FR_EVENT | FM_BL_EVENT);
+	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	tx->tx_frq = (unsigned long)freq_to_set;
+	fmdbg("tx: freq_to_set %ld\n", (long int)tx->tx_frq);
+
+	chanl_index = freq_to_set / 10;
+
+	/* Set current tuner channel */
+	payload = chanl_index;
+	ret = fmc_send_cmd(fmdev, CHANL_SET, REG_WR, &payload,
+			sizeof(payload), NULL, NULL);
+	if (ret < 0)
+		return ret;
+
+	fm_tx_set_pwr_lvl(fmdev, tx->pwr_lvl);
+	fm_tx_set_preemph_filter(fmdev, tx->preemph);
+
+	tx->audio_io = 0x01;	/* I2S */
+	set_audio_io(fmdev);
+
+	enable_xmit(fmdev, 0x01);	/* Enable transmission */
+
+	tx->aud_mode = FM_STEREO_MODE;
+	tx->rds.flag = FM_RDS_DISABLE;
+
+	return 0;
+}
+
diff --git a/drivers/media/radio/wl128x/fmdrv_tx.h b/drivers/media/radio/wl128x/fmdrv_tx.h
new file mode 100644
index 000000000000..e393a2bdd49e
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_tx.h
@@ -0,0 +1,37 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *  FM TX module header.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef _FMDRV_TX_H
+#define _FMDRV_TX_H
+
+u32 fm_tx_set_freq(struct fmdev *, u32);
+u32 fm_tx_set_pwr_lvl(struct fmdev *, u8);
+u32 fm_tx_set_region(struct fmdev *, u8);
+u32 fm_tx_set_mute_mode(struct fmdev *, u8);
+u32 fm_tx_set_stereo_mono(struct fmdev *, u16);
+u32 fm_tx_set_rds_mode(struct fmdev *, u8);
+u32 fm_tx_set_radio_text(struct fmdev *, u8 *, u8);
+u32 fm_tx_set_af(struct fmdev *, u32);
+u32 fm_tx_set_preemph_filter(struct fmdev *, u32);
+u32 fm_tx_get_tune_cap_val(struct fmdev *);
+
+#endif
+
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
new file mode 100644
index 000000000000..d50e5ac75ab6
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.c
@@ -0,0 +1,580 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *  This file provides interfaces to V4L2 subsystem.
+ *
+ *  This module registers with V4L2 subsystem as Radio
+ *  data system interface (/dev/radio). During the registration,
+ *  it will expose two set of function pointers.
+ *
+ *    1) File operation related API (open, close, read, write, poll...etc).
+ *    2) Set of V4L2 IOCTL complaint API.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *  Author: Raja Mani <raja_mani@ti.com>
+ *  Author: Manjunatha Halli <manjunatha_halli@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, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include "fmdrv.h"
+#include "fmdrv_v4l2.h"
+#include "fmdrv_common.h"
+#include "fmdrv_rx.h"
+#include "fmdrv_tx.h"
+
+static struct video_device *gradio_dev;
+static u8 radio_disconnected;
+
+/* -- V4L2 RADIO (/dev/radioX) device file operation interfaces --- */
+
+/* Read RX RDS data */
+static ssize_t fm_v4l2_fops_read(struct file *file, char __user * buf,
+					size_t count, loff_t *ppos)
+{
+	u8 rds_mode;
+	int ret;
+	struct fmdev *fmdev;
+
+	fmdev = video_drvdata(file);
+
+	if (!radio_disconnected) {
+		fmerr("FM device is already disconnected\n");
+		return -EIO;
+	}
+
+	/* Turn on RDS mode , if it is disabled */
+	ret = fm_rx_get_rds_mode(fmdev, &rds_mode);
+	if (ret < 0) {
+		fmerr("Unable to read current rds mode\n");
+		return ret;
+	}
+
+	if (rds_mode == FM_RDS_DISABLE) {
+		ret = fmc_set_rds_mode(fmdev, FM_RDS_ENABLE);
+		if (ret < 0) {
+			fmerr("Failed to enable rds mode\n");
+			return ret;
+		}
+	}
+
+	/* Copy RDS data from internal buffer to user buffer */
+	return fmc_transfer_rds_from_internal_buff(fmdev, file, buf, count);
+}
+
+/* Write TX RDS data */
+static ssize_t fm_v4l2_fops_write(struct file *file, const char __user * buf,
+		size_t count, loff_t *ppos)
+{
+	struct tx_rds rds;
+	int ret;
+	struct fmdev *fmdev;
+
+	ret = copy_from_user(&rds, buf, sizeof(rds));
+	fmdbg("(%d)type: %d, text %s, af %d\n",
+		   ret, rds.text_type, rds.text, rds.af_freq);
+
+	fmdev = video_drvdata(file);
+	fm_tx_set_radio_text(fmdev, rds.text, rds.text_type);
+	fm_tx_set_af(fmdev, rds.af_freq);
+
+	return 0;
+}
+
+static u32 fm_v4l2_fops_poll(struct file *file, struct poll_table_struct *pts)
+{
+	int ret;
+	struct fmdev *fmdev;
+
+	fmdev = video_drvdata(file);
+	ret = fmc_is_rds_data_available(fmdev, file, pts);
+	if (ret < 0)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+/*
+ * Handle open request for "/dev/radioX" device.
+ * Start with FM RX mode as default.
+ */
+static int fm_v4l2_fops_open(struct file *file)
+{
+	int ret;
+	struct fmdev *fmdev = NULL;
+
+	/* Don't allow multiple open */
+	if (radio_disconnected) {
+		fmerr("FM device is already opened\n");
+		return -EBUSY;
+	}
+
+	fmdev = video_drvdata(file);
+
+	ret = fmc_prepare(fmdev);
+	if (ret < 0) {
+		fmerr("Unable to prepare FM CORE\n");
+		return ret;
+	}
+
+	fmdbg("Load FM RX firmware..\n");
+
+	ret = fmc_set_mode(fmdev, FM_MODE_RX);
+	if (ret < 0) {
+		fmerr("Unable to load FM RX firmware\n");
+		return ret;
+	}
+	radio_disconnected = 1;
+
+	return ret;
+}
+
+static int fm_v4l2_fops_release(struct file *file)
+{
+	int ret;
+	struct fmdev *fmdev;
+
+	fmdev = video_drvdata(file);
+	if (!radio_disconnected) {
+		fmdbg("FM device is already closed\n");
+		return 0;
+	}
+
+	ret = fmc_set_mode(fmdev, FM_MODE_OFF);
+	if (ret < 0) {
+		fmerr("Unable to turn off the chip\n");
+		return ret;
+	}
+
+	ret = fmc_release(fmdev);
+	if (ret < 0) {
+		fmerr("FM CORE release failed\n");
+		return ret;
+	}
+	radio_disconnected = 0;
+
+	return ret;
+}
+
+/* V4L2 RADIO (/dev/radioX) device IOCTL interfaces */
+static int fm_v4l2_vidioc_querycap(struct file *file, void *priv,
+		struct v4l2_capability *capability)
+{
+	strlcpy(capability->driver, FM_DRV_NAME, sizeof(capability->driver));
+	strlcpy(capability->card, FM_DRV_CARD_SHORT_NAME,
+			sizeof(capability->card));
+	sprintf(capability->bus_info, "UART");
+	capability->version = FM_DRV_RADIO_VERSION;
+	capability->capabilities = V4L2_CAP_HW_FREQ_SEEK | V4L2_CAP_TUNER |
+		V4L2_CAP_RADIO | V4L2_CAP_MODULATOR |
+		V4L2_CAP_AUDIO | V4L2_CAP_READWRITE |
+		V4L2_CAP_RDS_CAPTURE;
+
+	return 0;
+}
+
+static int fm_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fmdev *fmdev = container_of(ctrl->handler,
+			struct fmdev, ctrl_handler);
+
+	switch (ctrl->id) {
+	case  V4L2_CID_TUNE_ANTENNA_CAPACITOR:
+		ctrl->val = fm_tx_get_tune_cap_val(fmdev);
+		break;
+	default:
+		fmwarn("%s: Unknown IOCTL: %d\n", __func__, ctrl->id);
+		break;
+	}
+
+	return 0;
+}
+
+static int fm_v4l2_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fmdev *fmdev = container_of(ctrl->handler,
+			struct fmdev, ctrl_handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_VOLUME:	/* set volume */
+		return fm_rx_set_volume(fmdev, (u16)ctrl->val);
+
+	case V4L2_CID_AUDIO_MUTE:	/* set mute */
+		return fmc_set_mute_mode(fmdev, (u8)ctrl->val);
+
+	case V4L2_CID_TUNE_POWER_LEVEL:
+		/* set TX power level - ext control */
+		return fm_tx_set_pwr_lvl(fmdev, (u8)ctrl->val);
+
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		return fm_tx_set_preemph_filter(fmdev, (u8) ctrl->val);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int fm_v4l2_vidioc_g_audio(struct file *file, void *priv,
+		struct v4l2_audio *audio)
+{
+	memset(audio, 0, sizeof(*audio));
+	strcpy(audio->name, "Radio");
+	audio->capability = V4L2_AUDCAP_STEREO;
+
+	return 0;
+}
+
+static int fm_v4l2_vidioc_s_audio(struct file *file, void *priv,
+		struct v4l2_audio *audio)
+{
+	if (audio->index != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+/* Get tuner attributes. If current mode is NOT RX, return error */
+static int fm_v4l2_vidioc_g_tuner(struct file *file, void *priv,
+		struct v4l2_tuner *tuner)
+{
+	struct fmdev *fmdev = video_drvdata(file);
+	u32 bottom_freq;
+	u32 top_freq;
+	u16 stereo_mono_mode;
+	u16 rssilvl;
+	int ret;
+
+	if (tuner->index != 0)
+		return -EINVAL;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX)
+		return -EPERM;
+
+	ret = fm_rx_get_band_freq_range(fmdev, &bottom_freq, &top_freq);
+	if (ret != 0)
+		return ret;
+
+	ret = fm_rx_get_stereo_mono(fmdev, &stereo_mono_mode);
+	if (ret != 0)
+		return ret;
+
+	ret = fm_rx_get_rssi_level(fmdev, &rssilvl);
+	if (ret != 0)
+		return ret;
+
+	strcpy(tuner->name, "FM");
+	tuner->type = V4L2_TUNER_RADIO;
+	/* Store rangelow and rangehigh freq in unit of 62.5 Hz */
+	tuner->rangelow = bottom_freq * 16;
+	tuner->rangehigh = top_freq * 16;
+	tuner->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO |
+	((fmdev->rx.rds.flag == FM_RDS_ENABLE) ? V4L2_TUNER_SUB_RDS : 0);
+	tuner->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
+			    V4L2_TUNER_CAP_LOW;
+	tuner->audmode = (stereo_mono_mode ?
+			  V4L2_TUNER_MODE_MONO : V4L2_TUNER_MODE_STEREO);
+
+	/*
+	 * Actual rssi value lies in between -128 to +127.
+	 * Convert this range from 0 to 255 by adding +128
+	 */
+	rssilvl += 128;
+
+	/*
+	 * Return signal strength value should be within 0 to 65535.
+	 * Find out correct signal radio by multiplying (65535/255) = 257
+	 */
+	tuner->signal = rssilvl * 257;
+	tuner->afc = 0;
+
+	return ret;
+}
+
+/*
+ * Set tuner attributes. If current mode is NOT RX, set to RX.
+ * Currently, we set only audio mode (mono/stereo) and RDS state (on/off).
+ * Should we set other tuner attributes, too?
+ */
+static int fm_v4l2_vidioc_s_tuner(struct file *file, void *priv,
+		struct v4l2_tuner *tuner)
+{
+	struct fmdev *fmdev = video_drvdata(file);
+	u16 aud_mode;
+	u8 rds_mode;
+	int ret;
+
+	if (tuner->index != 0)
+		return -EINVAL;
+
+	aud_mode = (tuner->audmode == V4L2_TUNER_MODE_STEREO) ?
+			FM_STEREO_MODE : FM_MONO_MODE;
+	rds_mode = (tuner->rxsubchans & V4L2_TUNER_SUB_RDS) ?
+			FM_RDS_ENABLE : FM_RDS_DISABLE;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX) {
+		ret = fmc_set_mode(fmdev, FM_MODE_RX);
+		if (ret < 0) {
+			fmerr("Failed to set RX mode\n");
+			return ret;
+		}
+	}
+
+	ret = fmc_set_stereo_mono(fmdev, aud_mode);
+	if (ret < 0) {
+		fmerr("Failed to set RX stereo/mono mode\n");
+		return ret;
+	}
+
+	ret = fmc_set_rds_mode(fmdev, rds_mode);
+	if (ret < 0)
+		fmerr("Failed to set RX RDS mode\n");
+
+	return ret;
+}
+
+/* Get tuner or modulator radio frequency */
+static int fm_v4l2_vidioc_g_freq(struct file *file, void *priv,
+		struct v4l2_frequency *freq)
+{
+	struct fmdev *fmdev = video_drvdata(file);
+	int ret;
+
+	ret = fmc_get_freq(fmdev, &freq->frequency);
+	if (ret < 0) {
+		fmerr("Failed to get frequency\n");
+		return ret;
+	}
+
+	/* Frequency unit of 62.5 Hz*/
+	freq->frequency = (u32) freq->frequency * 16;
+
+	return 0;
+}
+
+/* Set tuner or modulator radio frequency */
+static int fm_v4l2_vidioc_s_freq(struct file *file, void *priv,
+		struct v4l2_frequency *freq)
+{
+	struct fmdev *fmdev = video_drvdata(file);
+
+	/*
+	 * As V4L2_TUNER_CAP_LOW is set 1 user sends the frequency
+	 * in units of 62.5 Hz.
+	 */
+	freq->frequency = (u32)(freq->frequency / 16);
+
+	return fmc_set_freq(fmdev, freq->frequency);
+}
+
+/* Set hardware frequency seek. If current mode is NOT RX, set it RX. */
+static int fm_v4l2_vidioc_s_hw_freq_seek(struct file *file, void *priv,
+		struct v4l2_hw_freq_seek *seek)
+{
+	struct fmdev *fmdev = video_drvdata(file);
+	int ret;
+
+	if (fmdev->curr_fmmode != FM_MODE_RX) {
+		ret = fmc_set_mode(fmdev, FM_MODE_RX);
+		if (ret != 0) {
+			fmerr("Failed to set RX mode\n");
+			return ret;
+		}
+	}
+
+	ret = fm_rx_seek(fmdev, seek->seek_upward, seek->wrap_around,
+			seek->spacing);
+	if (ret < 0)
+		fmerr("RX seek failed - %d\n", ret);
+
+	return ret;
+}
+/* Get modulator attributes. If mode is not TX, return no attributes. */
+static int fm_v4l2_vidioc_g_modulator(struct file *file, void *priv,
+		struct v4l2_modulator *mod)
+{
+	struct fmdev *fmdev = video_drvdata(file);;
+
+	if (mod->index != 0)
+		return -EINVAL;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX)
+		return -EPERM;
+
+	mod->txsubchans = ((fmdev->tx_data.aud_mode == FM_STEREO_MODE) ?
+				V4L2_TUNER_SUB_STEREO : V4L2_TUNER_SUB_MONO) |
+				((fmdev->tx_data.rds.flag == FM_RDS_ENABLE) ?
+				V4L2_TUNER_SUB_RDS : 0);
+
+	mod->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
+				V4L2_TUNER_CAP_LOW;
+
+	return 0;
+}
+
+/* Set modulator attributes. If mode is not TX, set to TX. */
+static int fm_v4l2_vidioc_s_modulator(struct file *file, void *priv,
+		struct v4l2_modulator *mod)
+{
+	struct fmdev *fmdev = video_drvdata(file);
+	u8 rds_mode;
+	u16 aud_mode;
+	int ret;
+
+	if (mod->index != 0)
+		return -EINVAL;
+
+	if (fmdev->curr_fmmode != FM_MODE_TX) {
+		ret = fmc_set_mode(fmdev, FM_MODE_TX);
+		if (ret != 0) {
+			fmerr("Failed to set TX mode\n");
+			return ret;
+		}
+	}
+
+	aud_mode = (mod->txsubchans & V4L2_TUNER_SUB_STEREO) ?
+			FM_STEREO_MODE : FM_MONO_MODE;
+	rds_mode = (mod->txsubchans & V4L2_TUNER_SUB_RDS) ?
+			FM_RDS_ENABLE : FM_RDS_DISABLE;
+	ret = fm_tx_set_stereo_mono(fmdev, aud_mode);
+	if (ret < 0) {
+		fmerr("Failed to set mono/stereo mode for TX\n");
+		return ret;
+	}
+	ret = fm_tx_set_rds_mode(fmdev, rds_mode);
+	if (ret < 0)
+		fmerr("Failed to set rds mode for TX\n");
+
+	return ret;
+}
+
+static const struct v4l2_file_operations fm_drv_fops = {
+	.owner = THIS_MODULE,
+	.read = fm_v4l2_fops_read,
+	.write = fm_v4l2_fops_write,
+	.poll = fm_v4l2_fops_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.open = fm_v4l2_fops_open,
+	.release = fm_v4l2_fops_release,
+};
+
+static const struct v4l2_ctrl_ops fm_ctrl_ops = {
+	.s_ctrl = fm_v4l2_s_ctrl,
+	.g_volatile_ctrl = fm_g_volatile_ctrl,
+};
+static const struct v4l2_ioctl_ops fm_drv_ioctl_ops = {
+	.vidioc_querycap = fm_v4l2_vidioc_querycap,
+	.vidioc_g_audio = fm_v4l2_vidioc_g_audio,
+	.vidioc_s_audio = fm_v4l2_vidioc_s_audio,
+	.vidioc_g_tuner = fm_v4l2_vidioc_g_tuner,
+	.vidioc_s_tuner = fm_v4l2_vidioc_s_tuner,
+	.vidioc_g_frequency = fm_v4l2_vidioc_g_freq,
+	.vidioc_s_frequency = fm_v4l2_vidioc_s_freq,
+	.vidioc_s_hw_freq_seek = fm_v4l2_vidioc_s_hw_freq_seek,
+	.vidioc_g_modulator = fm_v4l2_vidioc_g_modulator,
+	.vidioc_s_modulator = fm_v4l2_vidioc_s_modulator
+};
+
+/* V4L2 RADIO device parent structure */
+static struct video_device fm_viddev_template = {
+	.fops = &fm_drv_fops,
+	.ioctl_ops = &fm_drv_ioctl_ops,
+	.name = FM_DRV_NAME,
+	.release = video_device_release,
+};
+
+int fm_v4l2_init_video_device(struct fmdev *fmdev, int radio_nr)
+{
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	/* Init mutex for core locking */
+	mutex_init(&fmdev->mutex);
+
+	/* Allocate new video device */
+	gradio_dev = video_device_alloc();
+	if (NULL == gradio_dev) {
+		fmerr("Can't allocate video device\n");
+		return -ENOMEM;
+	}
+
+	/* Setup FM driver's V4L2 properties */
+	memcpy(gradio_dev, &fm_viddev_template, sizeof(fm_viddev_template));
+
+	video_set_drvdata(gradio_dev, fmdev);
+
+	gradio_dev->lock = &fmdev->mutex;
+
+	/* Register with V4L2 subsystem as RADIO device */
+	if (video_register_device(gradio_dev, VFL_TYPE_RADIO, radio_nr)) {
+		video_device_release(gradio_dev);
+		fmerr("Could not register video device\n");
+		return -ENOMEM;
+	}
+
+	fmdev->radio_dev = gradio_dev;
+
+	/* Register to v4l2 ctrl handler framework */
+	fmdev->radio_dev->ctrl_handler = &fmdev->ctrl_handler;
+
+	ret = v4l2_ctrl_handler_init(&fmdev->ctrl_handler, 5);
+	if (ret < 0) {
+		fmerr("(fmdev): Can't init ctrl handler\n");
+		v4l2_ctrl_handler_free(&fmdev->ctrl_handler);
+		return -EBUSY;
+	}
+
+	/*
+	 * Following controls are handled by V4L2 control framework.
+	 * Added in ascending ID order.
+	 */
+	v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, FM_RX_VOLUME_MIN,
+			FM_RX_VOLUME_MAX, 1, FM_RX_VOLUME_MAX);
+
+	v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+
+	v4l2_ctrl_new_std_menu(&fmdev->ctrl_handler, &fm_ctrl_ops,
+			V4L2_CID_TUNE_PREEMPHASIS, V4L2_PREEMPHASIS_75_uS,
+			0, V4L2_PREEMPHASIS_75_uS);
+
+	v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
+			V4L2_CID_TUNE_POWER_LEVEL, FM_PWR_LVL_LOW,
+			FM_PWR_LVL_HIGH, 1, FM_PWR_LVL_HIGH);
+
+	ctrl = v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
+			V4L2_CID_TUNE_ANTENNA_CAPACITOR, 0,
+			255, 1, 255);
+
+	if (ctrl)
+		ctrl->is_volatile = 1;
+
+	return 0;
+}
+
+void *fm_v4l2_deinit_video_device(void)
+{
+	struct fmdev *fmdev;
+
+
+	fmdev = video_get_drvdata(gradio_dev);
+
+	/* Unregister to v4l2 ctrl handler framework*/
+	v4l2_ctrl_handler_free(&fmdev->ctrl_handler);
+
+	/* Unregister RADIO device from V4L2 subsystem */
+	video_unregister_device(gradio_dev);
+
+	return fmdev;
+}
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.h b/drivers/media/radio/wl128x/fmdrv_v4l2.h
new file mode 100644
index 000000000000..0ba79d745e2f
--- /dev/null
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.h
@@ -0,0 +1,33 @@
+/*
+ *  FM Driver for Connectivity chip of Texas Instruments.
+ *
+ *  FM V4L2 module header.
+ *
+ *  Copyright (C) 2011 Texas Instruments
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef _FMDRV_V4L2_H
+#define _FMDRV_V4L2_H
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+
+int fm_v4l2_init_video_device(struct fmdev *, int);
+void *fm_v4l2_deinit_video_device(void);
+
+#endif
diff --git a/drivers/media/rc/ir-raw.c b/drivers/media/rc/ir-raw.c
index 73230ff93b8a..01f258a2a57a 100644
--- a/drivers/media/rc/ir-raw.c
+++ b/drivers/media/rc/ir-raw.c
@@ -112,7 +112,7 @@ int ir_raw_event_store_edge(struct rc_dev *dev, enum raw_event_type type)
 {
 	ktime_t			now;
 	s64			delta; /* ns */
-	struct ir_raw_event	ev;
+	DEFINE_IR_RAW_EVENT(ev);
 	int			rc = 0;
 
 	if (!dev->raw)
@@ -125,7 +125,6 @@ int ir_raw_event_store_edge(struct rc_dev *dev, enum raw_event_type type)
 	 * being called for the first time, note that delta can't
 	 * possibly be negative.
 	 */
-	ev.duration = 0;
 	if (delta > IR_MAX_DURATION || !dev->raw->last_type)
 		type |= IR_START_EVENT;
 	else
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index 0659e9f50144..f0c80559b303 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -74,6 +74,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-real-audio-220-32-keys.o \
 			rc-streamzap.o \
 			rc-tbs-nec.o \
+			rc-technisat-usb2.o \
 			rc-terratec-cinergy-xs.o \
 			rc-terratec-slim.o \
 			rc-tevii-nec.o \
diff --git a/drivers/media/rc/keymaps/rc-technisat-usb2.c b/drivers/media/rc/keymaps/rc-technisat-usb2.c
new file mode 100644
index 000000000000..4afe5774f192
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-technisat-usb2.c
@@ -0,0 +1,93 @@
+/* rc-technisat-usb2.c - Keytable for SkyStar HD USB
+ *
+ * Copyright (C) 2010 Patrick Boettcher,
+ *                    Kernel Labs Inc. PO Box 745, St James, NY 11780
+ *
+ * Development was sponsored by Technisat Digital UK Limited, whose
+ * registered office is Witan Gate House 500 - 600 Witan Gate West,
+ * Milton Keynes, MK9 1SH
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ * THIS PROGRAM IS PROVIDED "AS IS" AND BOTH THE COPYRIGHT HOLDER AND
+ * TECHNISAT DIGITAL UK LTD DISCLAIM ALL WARRANTIES WITH REGARD TO
+ * THIS PROGRAM INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY OR
+ * FITNESS FOR A PARTICULAR PURPOSE.  NEITHER THE COPYRIGHT HOLDER
+ * NOR TECHNISAT DIGITAL UK LIMITED SHALL 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 PROGRAM. See the
+ * GNU General Public License for more details.
+ */
+
+#include <media/rc-map.h>
+
+static struct rc_map_table technisat_usb2[] = {
+	{0x0a0c, KEY_POWER},
+	{0x0a01, KEY_1},
+	{0x0a02, KEY_2},
+	{0x0a03, KEY_3},
+	{0x0a0d, KEY_MUTE},
+	{0x0a04, KEY_4},
+	{0x0a05, KEY_5},
+	{0x0a06, KEY_6},
+	{0x0a38, KEY_VIDEO}, /* EXT */
+	{0x0a07, KEY_7},
+	{0x0a08, KEY_8},
+	{0x0a09, KEY_9},
+	{0x0a00, KEY_0},
+	{0x0a4f, KEY_INFO},
+	{0x0a20, KEY_CHANNELUP},
+	{0x0a52, KEY_MENU},
+	{0x0a11, KEY_VOLUMEUP},
+	{0x0a57, KEY_OK},
+	{0x0a10, KEY_VOLUMEDOWN},
+	{0x0a2f, KEY_EPG},
+	{0x0a21, KEY_CHANNELDOWN},
+	{0x0a22, KEY_REFRESH},
+	{0x0a3c, KEY_TEXT},
+	{0x0a76, KEY_ENTER}, /* HOOK */
+	{0x0a0f, KEY_HELP},
+	{0x0a6b, KEY_RED},
+	{0x0a6c, KEY_GREEN},
+	{0x0a6d, KEY_YELLOW},
+	{0x0a6e, KEY_BLUE},
+	{0x0a29, KEY_STOP},
+	{0x0a23, KEY_LANGUAGE},
+	{0x0a53, KEY_TV},
+	{0x0a0a, KEY_PROGRAM},
+};
+
+static struct rc_map_list technisat_usb2_map = {
+	.map = {
+		.scan    = technisat_usb2,
+		.size    = ARRAY_SIZE(technisat_usb2),
+		.rc_type = RC_TYPE_RC5,
+		.name    = RC_MAP_TECHNISAT_USB2,
+	}
+};
+
+static int __init init_rc_map(void)
+{
+	return rc_map_register(&technisat_usb2_map);
+}
+
+static void __exit exit_rc_map(void)
+{
+	rc_map_unregister(&technisat_usb2_map);
+}
+
+module_init(init_rc_map)
+module_exit(exit_rc_map)
+
+MODULE_AUTHOR("Patrick Boettcher <pboettcher@kernellabs.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 6df0a4980645..e4f8eac7f717 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -148,6 +148,7 @@ enum mceusb_model_type {
 	MCE_GEN2_TX_INV,
 	POLARIS_EVK,
 	CX_HYBRID_TV,
+	MULTIFUNCTION,
 };
 
 struct mceusb_model {
@@ -155,9 +156,10 @@ struct mceusb_model {
 	u32 mce_gen2:1;
 	u32 mce_gen3:1;
 	u32 tx_mask_normal:1;
-	u32 is_polaris:1;
 	u32 no_tx:1;
 
+	int ir_intfnum;
+
 	const char *rc_map;	/* Allow specify a per-board map */
 	const char *name;	/* per-board name */
 };
@@ -179,7 +181,6 @@ static const struct mceusb_model mceusb_model[] = {
 		.tx_mask_normal = 1,
 	},
 	[POLARIS_EVK] = {
-		.is_polaris = 1,
 		/*
 		 * In fact, the EVK is shipped without
 		 * remotes, but we should have something handy,
@@ -189,10 +190,13 @@ static const struct mceusb_model mceusb_model[] = {
 		.name = "Conexant Hybrid TV (cx231xx) MCE IR",
 	},
 	[CX_HYBRID_TV] = {
-		.is_polaris = 1,
 		.no_tx = 1, /* tx isn't wired up at all */
 		.name = "Conexant Hybrid TV (cx231xx) MCE IR",
 	},
+	[MULTIFUNCTION] = {
+		.mce_gen2 = 1,
+		.ir_intfnum = 2,
+	},
 };
 
 static struct usb_device_id mceusb_dev_table[] = {
@@ -216,8 +220,9 @@ static struct usb_device_id mceusb_dev_table[] = {
 	{ USB_DEVICE(VENDOR_PHILIPS, 0x206c) },
 	/* Philips/Spinel plus IR transceiver for ASUS */
 	{ USB_DEVICE(VENDOR_PHILIPS, 0x2088) },
-	/* Realtek MCE IR Receiver */
-	{ USB_DEVICE(VENDOR_REALTEK, 0x0161) },
+	/* Realtek MCE IR Receiver and card reader */
+	{ USB_DEVICE(VENDOR_REALTEK, 0x0161),
+	  .driver_info = MULTIFUNCTION },
 	/* SMK/Toshiba G83C0004D410 */
 	{ USB_DEVICE(VENDOR_SMK, 0x031d),
 	  .driver_info = MCE_GEN2_TX_INV },
@@ -1101,7 +1106,7 @@ static int __devinit mceusb_dev_probe(struct usb_interface *intf,
 	bool is_gen3;
 	bool is_microsoft_gen1;
 	bool tx_mask_normal;
-	bool is_polaris;
+	int ir_intfnum;
 
 	dev_dbg(&intf->dev, "%s called\n", __func__);
 
@@ -1110,13 +1115,11 @@ static int __devinit mceusb_dev_probe(struct usb_interface *intf,
 	is_gen3 = mceusb_model[model].mce_gen3;
 	is_microsoft_gen1 = mceusb_model[model].mce_gen1;
 	tx_mask_normal = mceusb_model[model].tx_mask_normal;
-	is_polaris = mceusb_model[model].is_polaris;
+	ir_intfnum = mceusb_model[model].ir_intfnum;
 
-	if (is_polaris) {
-		/* Interface 0 is IR */
-		if (idesc->desc.bInterfaceNumber)
-			return -ENODEV;
-	}
+	/* There are multi-function devices with non-IR interfaces */
+	if (idesc->desc.bInterfaceNumber != ir_intfnum)
+		return -ENODEV;
 
 	/* step through the endpoints to find first bulk in and out endpoint */
 	for (i = 0; i < idesc->desc.bNumEndpoints; ++i) {
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 273d9d674792..d4d64492a057 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -385,8 +385,9 @@ static void nvt_cir_regs_init(struct nvt_dev *nvt)
 
 static void nvt_cir_wake_regs_init(struct nvt_dev *nvt)
 {
-	/* set number of bytes needed for wake key comparison (default 67) */
-	nvt_cir_wake_reg_write(nvt, CIR_WAKE_FIFO_LEN, CIR_WAKE_FIFO_CMP_DEEP);
+	/* set number of bytes needed for wake from s3 (default 65) */
+	nvt_cir_wake_reg_write(nvt, CIR_WAKE_FIFO_CMP_BYTES,
+			       CIR_WAKE_FIFO_CMP_DEEP);
 
 	/* set tolerance/variance allowed per byte during wake compare */
 	nvt_cir_wake_reg_write(nvt, CIR_WAKE_CMP_TOLERANCE,
diff --git a/drivers/media/rc/nuvoton-cir.h b/drivers/media/rc/nuvoton-cir.h
index 1df82351cb03..048135eea702 100644
--- a/drivers/media/rc/nuvoton-cir.h
+++ b/drivers/media/rc/nuvoton-cir.h
@@ -305,8 +305,11 @@ struct nvt_dev {
 #define CIR_WAKE_IRFIFOSTS_RX_EMPTY	0x20
 #define CIR_WAKE_IRFIFOSTS_RX_FULL	0x10
 
-/* CIR Wake FIFO buffer is 67 bytes long */
-#define CIR_WAKE_FIFO_LEN		67
+/*
+ * The CIR Wake FIFO buffer is 67 bytes long, but the stock remote wakes
+ * the system comparing only 65 bytes (fails with this set to 67)
+ */
+#define CIR_WAKE_FIFO_CMP_BYTES		65
 /* CIR Wake byte comparison tolerance */
 #define CIR_WAKE_CMP_TOLERANCE		5
 
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 512a2f4ada0e..5ac1baf45c8e 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -850,7 +850,7 @@ static ssize_t store_protocols(struct device *device,
 			count++;
 		} else {
 			for (i = 0; i < ARRAY_SIZE(proto_names); i++) {
-				if (!strncasecmp(tmp, proto_names[i].name, strlen(proto_names[i].name))) {
+				if (!strcasecmp(tmp, proto_names[i].name)) {
 					tmp += strlen(proto_names[i].name);
 					mask = proto_names[i].type;
 					break;
@@ -966,8 +966,8 @@ struct rc_dev *rc_allocate_device(void)
 		return NULL;
 	}
 
-	dev->input_dev->getkeycode_new = ir_getkeycode;
-	dev->input_dev->setkeycode_new = ir_setkeycode;
+	dev->input_dev->getkeycode = ir_getkeycode;
+	dev->input_dev->setkeycode = ir_setkeycode;
 	input_set_drvdata(dev->input_dev, dev);
 
 	spin_lock_init(&dev->rc_map.lock);
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index aa021600e9df..e2f5a69aa400 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -42,8 +42,30 @@ config VIDEO_TUNER
 
 config V4L2_MEM2MEM_DEV
 	tristate
-	depends on VIDEOBUF_GEN
+	depends on VIDEOBUF2_CORE
 
+config VIDEOBUF2_CORE
+	tristate
+
+config VIDEOBUF2_MEMOPS
+	tristate
+
+config VIDEOBUF2_DMA_CONTIG
+	select VIDEOBUF2_CORE
+	select VIDEOBUF2_MEMOPS
+	tristate
+
+config VIDEOBUF2_VMALLOC
+	select VIDEOBUF2_CORE
+	select VIDEOBUF2_MEMOPS
+	tristate
+
+
+config VIDEOBUF2_DMA_SG
+	#depends on HAS_DMA
+	select VIDEOBUF2_CORE
+	select VIDEOBUF2_MEMOPS
+	tristate
 #
 # Multimedia Video device configuration
 #
@@ -527,7 +549,7 @@ config VIDEO_VIVI
 	depends on VIDEO_DEV && VIDEO_V4L2 && !SPARC32 && !SPARC64
 	depends on FRAMEBUFFER_CONSOLE || STI_CONSOLE
 	select FONT_8x16
-	select VIDEOBUF_VMALLOC
+	select VIDEOBUF2_VMALLOC
 	default n
 	---help---
 	  Enables a virtual video driver. This device shows a color bar
@@ -718,10 +740,17 @@ config VIDEO_VIA_CAMERA
 	   Chrome9 chipsets.  Currently only tested on OLPC xo-1.5 systems
 	   with ov7670 sensors.
 
+config VIDEO_NOON010PC30
+	tristate "NOON010PC30 CIF camera sensor support"
+	depends on I2C && VIDEO_V4L2
+	---help---
+	  This driver supports NOON010PC30 CIF camera from Siliconfile
+
 config SOC_CAMERA
 	tristate "SoC camera support"
 	depends on VIDEO_V4L2 && HAS_DMA && I2C
 	select VIDEOBUF_GEN
+	select VIDEOBUF2_CORE
 	help
 	  SoC Camera is a common API to several cameras, not connecting
 	  over a bus like PCI or USB. For example some i2c camera connected
@@ -809,6 +838,12 @@ config SOC_CAMERA_OV9640
 	help
 	  This is a ov9640 camera driver
 
+config SOC_CAMERA_OV9740
+	tristate "ov9740 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a ov9740 camera driver
+
 config MX1_VIDEO
 	bool
 
@@ -848,7 +883,7 @@ config VIDEO_SH_MOBILE_CSI2
 config VIDEO_SH_MOBILE_CEU
 	tristate "SuperH Mobile CEU Interface driver"
 	depends on VIDEO_DEV && SOC_CAMERA && HAS_DMA && HAVE_CLK
-	select VIDEOBUF_DMA_CONTIG
+	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  This is a v4l2 driver for the SuperH Mobile CEU Interface
 
@@ -967,7 +1002,7 @@ if V4L_MEM2MEM_DRIVERS
 config VIDEO_MEM2MEM_TESTDEV
 	tristate "Virtual test device for mem2mem framework"
 	depends on VIDEO_DEV && VIDEO_V4L2
-	select VIDEOBUF_VMALLOC
+	select VIDEOBUF2_VMALLOC
 	select V4L2_MEM2MEM_DEV
 	default n
 	---help---
@@ -977,7 +1012,7 @@ config VIDEO_MEM2MEM_TESTDEV
 config  VIDEO_SAMSUNG_S5P_FIMC
 	tristate "Samsung S5P FIMC (video postprocessor) driver"
 	depends on VIDEO_DEV && VIDEO_V4L2 && PLAT_S5P
-	select VIDEOBUF_DMA_CONTIG
+	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
 	  This is a v4l2 driver for the S5P camera interface
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index a509d317e258..ac54652396e3 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -67,6 +67,7 @@ obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
 obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
 obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
 obj-$(CONFIG_VIDEO_SR030PC30)	+= sr030pc30.o
+obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
 
 obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
@@ -78,6 +79,7 @@ obj-$(CONFIG_SOC_CAMERA_OV2640)		+= ov2640.o
 obj-$(CONFIG_SOC_CAMERA_OV6650)		+= ov6650.o
 obj-$(CONFIG_SOC_CAMERA_OV772X)		+= ov772x.o
 obj-$(CONFIG_SOC_CAMERA_OV9640)		+= ov9640.o
+obj-$(CONFIG_SOC_CAMERA_OV9740)		+= ov9740.o
 obj-$(CONFIG_SOC_CAMERA_RJ54N1)		+= rj54n1cb0c.o
 obj-$(CONFIG_SOC_CAMERA_TW9910)		+= tw9910.o
 
@@ -111,6 +113,12 @@ obj-$(CONFIG_VIDEOBUF_VMALLOC) += videobuf-vmalloc.o
 obj-$(CONFIG_VIDEOBUF_DVB) += videobuf-dvb.o
 obj-$(CONFIG_VIDEO_BTCX)  += btcx-risc.o
 
+obj-$(CONFIG_VIDEOBUF2_CORE)		+= videobuf2-core.o
+obj-$(CONFIG_VIDEOBUF2_MEMOPS)		+= videobuf2-memops.o
+obj-$(CONFIG_VIDEOBUF2_VMALLOC)		+= videobuf2-vmalloc.o
+obj-$(CONFIG_VIDEOBUF2_DMA_CONTIG)	+= videobuf2-dma-contig.o
+obj-$(CONFIG_VIDEOBUF2_DMA_SG)		+= videobuf2-dma-sg.o
+
 obj-$(CONFIG_V4L2_MEM2MEM_DEV) += v4l2-mem2mem.o
 
 obj-$(CONFIG_VIDEO_M32R_AR_M64278) += arv.o
diff --git a/drivers/media/video/adv7343.c b/drivers/media/video/adv7343.c
index 41b2930d0ce4..021fab23070d 100644
--- a/drivers/media/video/adv7343.c
+++ b/drivers/media/video/adv7343.c
@@ -29,6 +29,7 @@
 #include <media/adv7343.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 #include "adv7343_regs.h"
 
@@ -41,15 +42,13 @@ MODULE_PARM_DESC(debug, "Debug level 0-1");
 
 struct adv7343_state {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	u8 reg00;
 	u8 reg01;
 	u8 reg02;
 	u8 reg35;
 	u8 reg80;
 	u8 reg82;
-	int bright;
-	int hue;
-	int gain;
 	u32 output;
 	v4l2_std_id std;
 };
@@ -59,6 +58,11 @@ static inline struct adv7343_state *to_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct adv7343_state, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7343_state, hdl)->sd;
+}
+
 static inline int adv7343_write(struct v4l2_subdev *sd, u8 reg, u8 value)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -268,111 +272,22 @@ static int adv7343_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int adv7343_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, ADV7343_BRIGHTNESS_MIN,
-						ADV7343_BRIGHTNESS_MAX, 1,
-						ADV7343_BRIGHTNESS_DEF);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, ADV7343_HUE_MIN,
-						ADV7343_HUE_MAX, 1 ,
-						ADV7343_HUE_DEF);
-	case V4L2_CID_GAIN:
-		return v4l2_ctrl_query_fill(qc, ADV7343_GAIN_MIN,
-						ADV7343_GAIN_MAX, 1,
-						ADV7343_GAIN_DEF);
-	default:
-		break;
-	}
-
-	return 0;
-}
-
-static int adv7343_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct adv7343_state *state = to_state(sd);
-	int err = 0;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		if (ctrl->value < ADV7343_BRIGHTNESS_MIN ||
-					ctrl->value > ADV7343_BRIGHTNESS_MAX) {
-			v4l2_dbg(1, debug, sd,
-					"invalid brightness settings %d\n",
-								ctrl->value);
-			return -ERANGE;
-		}
-
-		state->bright = ctrl->value;
-		err = adv7343_write(sd, ADV7343_SD_BRIGHTNESS_WSS,
-					state->bright);
-		break;
-
-	case V4L2_CID_HUE:
-		if (ctrl->value < ADV7343_HUE_MIN ||
-					ctrl->value > ADV7343_HUE_MAX) {
-			v4l2_dbg(1, debug, sd, "invalid hue settings %d\n",
-								ctrl->value);
-			return -ERANGE;
-		}
-
-		state->hue = ctrl->value;
-		err = adv7343_write(sd, ADV7343_SD_HUE_REG, state->hue);
-		break;
-
-	case V4L2_CID_GAIN:
-		if (ctrl->value < ADV7343_GAIN_MIN ||
-					ctrl->value > ADV7343_GAIN_MAX) {
-			v4l2_dbg(1, debug, sd, "invalid gain settings %d\n",
-								ctrl->value);
-			return -ERANGE;
-		}
-
-		if ((ctrl->value > POSITIVE_GAIN_MAX) &&
-			(ctrl->value < NEGATIVE_GAIN_MIN)) {
-			v4l2_dbg(1, debug, sd,
-				"gain settings not within the specified range\n");
-			return -ERANGE;
-		}
-
-		state->gain = ctrl->value;
-		err = adv7343_write(sd, ADV7343_DAC2_OUTPUT_LEVEL, state->gain);
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	if (err < 0)
-		v4l2_err(sd, "Failed to set the encoder controls\n");
-
-	return err;
-}
-
-static int adv7343_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int adv7343_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct adv7343_state *state = to_state(sd);
+	struct v4l2_subdev *sd = to_sd(ctrl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = state->bright;
-		break;
+		return adv7343_write(sd, ADV7343_SD_BRIGHTNESS_WSS,
+					ctrl->val);
 
 	case V4L2_CID_HUE:
-		ctrl->value = state->hue;
-		break;
+		return adv7343_write(sd, ADV7343_SD_HUE_REG, ctrl->val);
 
 	case V4L2_CID_GAIN:
-		ctrl->value = state->gain;
-		break;
-
-	default:
-		return -EINVAL;
+		return adv7343_write(sd, ADV7343_DAC2_OUTPUT_LEVEL, ctrl->val);
 	}
-
-	return 0;
+	return -EINVAL;
 }
 
 static int adv7343_g_chip_ident(struct v4l2_subdev *sd,
@@ -383,12 +298,20 @@ static int adv7343_g_chip_ident(struct v4l2_subdev *sd,
 	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7343, 0);
 }
 
+static const struct v4l2_ctrl_ops adv7343_ctrl_ops = {
+	.s_ctrl = adv7343_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops adv7343_core_ops = {
-	.log_status	= adv7343_log_status,
-	.g_chip_ident	= adv7343_g_chip_ident,
-	.g_ctrl		= adv7343_g_ctrl,
-	.s_ctrl		= adv7343_s_ctrl,
-	.queryctrl	= adv7343_queryctrl,
+	.log_status = adv7343_log_status,
+	.g_chip_ident = adv7343_g_chip_ident,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 };
 
 static int adv7343_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
@@ -468,6 +391,7 @@ static int adv7343_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
 	struct adv7343_state *state;
+	int err;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -ENODEV;
@@ -490,15 +414,46 @@ static int adv7343_probe(struct i2c_client *client,
 	state->std = V4L2_STD_NTSC;
 
 	v4l2_i2c_subdev_init(&state->sd, client, &adv7343_ops);
-	return adv7343_initialize(&state->sd);
+
+	v4l2_ctrl_handler_init(&state->hdl, 2);
+	v4l2_ctrl_new_std(&state->hdl, &adv7343_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, ADV7343_BRIGHTNESS_MIN,
+					     ADV7343_BRIGHTNESS_MAX, 1,
+					     ADV7343_BRIGHTNESS_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7343_ctrl_ops,
+			V4L2_CID_HUE, ADV7343_HUE_MIN,
+				      ADV7343_HUE_MAX, 1,
+				      ADV7343_HUE_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7343_ctrl_ops,
+			V4L2_CID_GAIN, ADV7343_GAIN_MIN,
+				       ADV7343_GAIN_MAX, 1,
+				       ADV7343_GAIN_DEF);
+	state->sd.ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
+
+	err = adv7343_initialize(&state->sd);
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+	}
+	return err;
 }
 
 static int adv7343_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7343_state *state = to_state(sd);
 
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_state(sd));
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
 
 	return 0;
 }
diff --git a/drivers/media/video/adv7343_regs.h b/drivers/media/video/adv7343_regs.h
index 3431045b33da..446606764346 100644
--- a/drivers/media/video/adv7343_regs.h
+++ b/drivers/media/video/adv7343_regs.h
@@ -102,10 +102,6 @@ struct adv7343_std_info {
 
 /* Bit masks for DAC output levels */
 #define DAC_OUTPUT_LEVEL_MASK		(0xFF)
-#define POSITIVE_GAIN_MAX		(0x40)
-#define POSITIVE_GAIN_MIN		(0x00)
-#define NEGATIVE_GAIN_MAX		(0xFF)
-#define NEGATIVE_GAIN_MIN		(0xC0)
 
 /* Bit masks for soft reset register */
 #define SOFT_RESET			(0x02)
@@ -178,8 +174,8 @@ struct adv7343_std_info {
 #define ADV7343_HUE_MAX		(255)
 #define ADV7343_HUE_MIN		(0)
 #define ADV7343_HUE_DEF		(127)
-#define ADV7343_GAIN_MAX	(255)
-#define ADV7343_GAIN_MIN	(0)
+#define ADV7343_GAIN_MAX	(64)
+#define ADV7343_GAIN_MIN	(-64)
 #define ADV7343_GAIN_DEF	(0)
 
 #endif
diff --git a/drivers/media/video/au0828/au0828-cards.c b/drivers/media/video/au0828/au0828-cards.c
index 01be89fa5c78..39fc923fc46b 100644
--- a/drivers/media/video/au0828/au0828-cards.c
+++ b/drivers/media/video/au0828/au0828-cards.c
@@ -185,8 +185,7 @@ void au0828_card_setup(struct au0828_dev *dev)
 	static u8 eeprom[256];
 	struct tuner_setup tun_setup;
 	struct v4l2_subdev *sd;
-	unsigned int mode_mask = T_ANALOG_TV |
-				 T_DIGITAL_TV;
+	unsigned int mode_mask = T_ANALOG_TV;
 
 	dprintk(1, "%s()\n", __func__);
 
diff --git a/drivers/media/video/au0828/au0828-video.c b/drivers/media/video/au0828/au0828-video.c
index e41e4ad5cc40..9c475c600fc9 100644
--- a/drivers/media/video/au0828/au0828-video.c
+++ b/drivers/media/video/au0828/au0828-video.c
@@ -1758,7 +1758,12 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 	if (rc < 0)
 		return rc;
 
-	return videobuf_reqbufs(&fh->vb_vidq, rb);
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		rc = videobuf_reqbufs(&fh->vb_vidq, rb);
+	else if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		rc = videobuf_reqbufs(&fh->vb_vbiq, rb);
+
+	return rc;
 }
 
 static int vidioc_querybuf(struct file *file, void *priv,
@@ -1772,7 +1777,12 @@ static int vidioc_querybuf(struct file *file, void *priv,
 	if (rc < 0)
 		return rc;
 
-	return videobuf_querybuf(&fh->vb_vidq, b);
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		rc = videobuf_querybuf(&fh->vb_vidq, b);
+	else if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		rc = videobuf_querybuf(&fh->vb_vbiq, b);
+
+	return rc;
 }
 
 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *b)
@@ -1785,7 +1795,12 @@ static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *b)
 	if (rc < 0)
 		return rc;
 
-	return videobuf_qbuf(&fh->vb_vidq, b);
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		rc = videobuf_qbuf(&fh->vb_vidq, b);
+	else if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		rc = videobuf_qbuf(&fh->vb_vbiq, b);
+
+	return rc;
 }
 
 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
@@ -1806,7 +1821,12 @@ static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
 		dev->greenscreen_detected = 0;
 	}
 
-	return videobuf_dqbuf(&fh->vb_vidq, b, file->f_flags & O_NONBLOCK);
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		rc = videobuf_dqbuf(&fh->vb_vidq, b, file->f_flags & O_NONBLOCK);
+	else if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE)
+		rc = videobuf_dqbuf(&fh->vb_vbiq, b, file->f_flags & O_NONBLOCK);
+
+	return rc;
 }
 
 static struct v4l2_file_operations au0828_v4l_fops = {
diff --git a/drivers/media/video/bt819.c b/drivers/media/video/bt819.c
index c38300fc0b1d..f87204461cb4 100644
--- a/drivers/media/video/bt819.c
+++ b/drivers/media/video/bt819.c
@@ -37,6 +37,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 #include <media/bt819.h>
 
 MODULE_DESCRIPTION("Brooktree-819 video decoder driver");
@@ -52,16 +53,13 @@ MODULE_PARM_DESC(debug, "Debug level (0-1)");
 
 struct bt819 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	unsigned char reg[32];
 
 	v4l2_std_id norm;
 	int ident;
 	int input;
 	int enable;
-	int bright;
-	int contrast;
-	int hue;
-	int sat;
 };
 
 static inline struct bt819 *to_bt819(struct v4l2_subdev *sd)
@@ -69,6 +67,11 @@ static inline struct bt819 *to_bt819(struct v4l2_subdev *sd)
 	return container_of(sd, struct bt819, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct bt819, hdl)->sd;
+}
+
 struct timing {
 	int hactive;
 	int hdelay;
@@ -333,71 +336,35 @@ static int bt819_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int bt819_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-		break;
-
-	case V4L2_CID_CONTRAST:
-		v4l2_ctrl_query_fill(qc, 0, 511, 1, 256);
-		break;
-
-	case V4L2_CID_SATURATION:
-		v4l2_ctrl_query_fill(qc, 0, 511, 1, 256);
-		break;
-
-	case V4L2_CID_HUE:
-		v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-		break;
-
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int bt819_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int bt819_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct bt819 *decoder = to_bt819(sd);
 	int temp;
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if (decoder->bright == ctrl->value)
-			break;
-		decoder->bright = ctrl->value;
-		bt819_write(decoder, 0x0a, decoder->bright);
+		bt819_write(decoder, 0x0a, ctrl->val);
 		break;
 
 	case V4L2_CID_CONTRAST:
-		if (decoder->contrast == ctrl->value)
-			break;
-		decoder->contrast = ctrl->value;
-		bt819_write(decoder, 0x0c, decoder->contrast & 0xff);
-		bt819_setbit(decoder, 0x0b, 2, ((decoder->contrast >> 8) & 0x01));
+		bt819_write(decoder, 0x0c, ctrl->val & 0xff);
+		bt819_setbit(decoder, 0x0b, 2, ((ctrl->val >> 8) & 0x01));
 		break;
 
 	case V4L2_CID_SATURATION:
-		if (decoder->sat == ctrl->value)
-			break;
-		decoder->sat = ctrl->value;
-		bt819_write(decoder, 0x0d, (decoder->sat >> 7) & 0xff);
-		bt819_setbit(decoder, 0x0b, 1, ((decoder->sat >> 15) & 0x01));
+		bt819_write(decoder, 0x0d, (ctrl->val >> 7) & 0xff);
+		bt819_setbit(decoder, 0x0b, 1, ((ctrl->val >> 15) & 0x01));
 
 		/* Ratio between U gain and V gain must stay the same as
 		   the ratio between the default U and V gain values. */
-		temp = (decoder->sat * 180) / 254;
+		temp = (ctrl->val * 180) / 254;
 		bt819_write(decoder, 0x0e, (temp >> 7) & 0xff);
 		bt819_setbit(decoder, 0x0b, 0, (temp >> 15) & 0x01);
 		break;
 
 	case V4L2_CID_HUE:
-		if (decoder->hue == ctrl->value)
-			break;
-		decoder->hue = ctrl->value;
-		bt819_write(decoder, 0x0f, decoder->hue);
+		bt819_write(decoder, 0x0f, ctrl->val);
 		break;
 
 	default:
@@ -406,29 +373,6 @@ static int bt819_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 	return 0;
 }
 
-static int bt819_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct bt819 *decoder = to_bt819(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = decoder->bright;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = decoder->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = decoder->sat;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = decoder->hue;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
 static int bt819_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
 {
 	struct bt819 *decoder = to_bt819(sd);
@@ -439,11 +383,19 @@ static int bt819_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops bt819_ctrl_ops = {
+	.s_ctrl = bt819_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops bt819_core_ops = {
 	.g_chip_ident = bt819_g_chip_ident,
-	.g_ctrl = bt819_g_ctrl,
-	.s_ctrl = bt819_s_ctrl,
-	.queryctrl = bt819_queryctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 	.s_std = bt819_s_std,
 };
 
@@ -505,23 +457,40 @@ static int bt819_probe(struct i2c_client *client,
 	decoder->norm = V4L2_STD_NTSC;
 	decoder->input = 0;
 	decoder->enable = 1;
-	decoder->bright = 0;
-	decoder->contrast = 0xd8;	/* 100% of original signal */
-	decoder->hue = 0;
-	decoder->sat = 0xfe;		/* 100% of original signal */
 
 	i = bt819_init(sd);
 	if (i < 0)
 		v4l2_dbg(1, debug, sd, "init status %d\n", i);
+
+	v4l2_ctrl_handler_init(&decoder->hdl, 4);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 511, 1, 0xd8);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 511, 1, 0xfe);
+	v4l2_ctrl_new_std(&decoder->hdl, &bt819_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		kfree(decoder);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
 	return 0;
 }
 
 static int bt819_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct bt819 *decoder = to_bt819(sd);
 
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_bt819(sd));
+	v4l2_ctrl_handler_free(&decoder->hdl);
+	kfree(decoder);
 	return 0;
 }
 
diff --git a/drivers/media/video/bt8xx/bttv-cards.c b/drivers/media/video/bt8xx/bttv-cards.c
index 7f58756d72c8..242f0d512238 100644
--- a/drivers/media/video/bt8xx/bttv-cards.c
+++ b/drivers/media/video/bt8xx/bttv-cards.c
@@ -3616,7 +3616,7 @@ void __devinit bttv_init_tuner(struct bttv *btv)
 				&btv->c.i2c_adap, "tuner",
 				0, v4l2_i2c_tuner_addrs(ADDRS_TV_WITH_DEMOD));
 
-		tun_setup.mode_mask = T_ANALOG_TV | T_DIGITAL_TV;
+		tun_setup.mode_mask = T_ANALOG_TV;
 		tun_setup.type = btv->tuner_type;
 		tun_setup.addr = addr;
 
diff --git a/drivers/media/video/cpia2/cpia2_v4l.c b/drivers/media/video/cpia2/cpia2_v4l.c
index 9bad39842936..363fe098b748 100644
--- a/drivers/media/video/cpia2/cpia2_v4l.c
+++ b/drivers/media/video/cpia2/cpia2_v4l.c
@@ -395,10 +395,14 @@ static int sync(struct camera_data *cam, int frame_nr)
  *
  *****************************************************************************/
 
-static int ioctl_set_gpio(void *arg, struct camera_data *cam)
+static long cpia2_default(struct file *file, void *fh, int cmd, void *arg)
 {
+	struct camera_data *cam = video_drvdata(file);
 	__u32 gpio_val;
 
+	if (cmd != CPIA2_CID_GPIO)
+		return -EINVAL;
+
 	gpio_val = *(__u32*) arg;
 
 	if (gpio_val &~ 0xFFU)
@@ -415,11 +419,10 @@ static int ioctl_set_gpio(void *arg, struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_querycap(void *arg, struct camera_data *cam)
+static int cpia2_querycap(struct file *file, void *fh, struct v4l2_capability *vc)
 {
-	struct v4l2_capability *vc = arg;
+	struct camera_data *cam = video_drvdata(file);
 
-	memset(vc, 0, sizeof(*vc));
 	strcpy(vc->driver, "cpia2");
 
 	if (cam->params.pnp_id.product == 0x151)
@@ -479,22 +482,26 @@ static int ioctl_querycap(void *arg, struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_input(unsigned int ioclt_nr,void *arg,struct camera_data *cam)
+static int cpia2_enum_input(struct file *file, void *fh, struct v4l2_input *i)
 {
-	struct v4l2_input *i = arg;
-
-	if(ioclt_nr  != VIDIOC_G_INPUT) {
-		if (i->index != 0)
-		       return -EINVAL;
-	}
-
-	memset(i, 0, sizeof(*i));
+	if (i->index)
+		return -EINVAL;
 	strcpy(i->name, "Camera");
 	i->type = V4L2_INPUT_TYPE_CAMERA;
+	return 0;
+}
 
+static int cpia2_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	*i = 0;
 	return 0;
 }
 
+static int cpia2_s_input(struct file *file, void *fh, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
+
 /******************************************************************************
  *
  *  ioctl_enum_fmt
@@ -503,9 +510,9 @@ static int ioctl_input(unsigned int ioclt_nr,void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_enum_fmt(void *arg,struct camera_data *cam)
+static int cpia2_enum_fmt_vid_cap(struct file *file, void *fh,
+					    struct v4l2_fmtdesc *f)
 {
-	struct v4l2_fmtdesc *f = arg;
 	int index = f->index;
 
 	if (index < 0 || index > 1)
@@ -539,12 +546,10 @@ static int ioctl_enum_fmt(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_try_fmt(void *arg,struct camera_data *cam)
+static int cpia2_try_fmt_vid_cap(struct file *file, void *fh,
+					  struct v4l2_format *f)
 {
-	struct v4l2_format *f = arg;
-
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-	       return -EINVAL;
+	struct camera_data *cam = video_drvdata(file);
 
 	if (f->fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG &&
 	    f->fmt.pix.pixelformat != V4L2_PIX_FMT_JPEG)
@@ -603,12 +608,17 @@ static int ioctl_try_fmt(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_set_fmt(void *arg,struct camera_data *cam, struct cpia2_fh *fh)
+static int cpia2_s_fmt_vid_cap(struct file *file, void *_fh,
+					struct v4l2_format *f)
 {
-	struct v4l2_format *f = arg;
+	struct camera_data *cam = video_drvdata(file);
+	struct cpia2_fh *fh = _fh;
 	int err, frame;
 
-	err = ioctl_try_fmt(arg, cam);
+	err = v4l2_prio_check(&cam->prio, fh->prio);
+	if (err)
+		return err;
+	err = cpia2_try_fmt_vid_cap(file, _fh, f);
 	if(err != 0)
 		return err;
 
@@ -658,12 +668,10 @@ static int ioctl_set_fmt(void *arg,struct camera_data *cam, struct cpia2_fh *fh)
  *
  *****************************************************************************/
 
-static int ioctl_get_fmt(void *arg,struct camera_data *cam)
+static int cpia2_g_fmt_vid_cap(struct file *file, void *fh,
+					struct v4l2_format *f)
 {
-	struct v4l2_format *f = arg;
-
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-	       return -EINVAL;
+	struct camera_data *cam = video_drvdata(file);
 
 	f->fmt.pix.width = cam->width;
 	f->fmt.pix.height = cam->height;
@@ -686,9 +694,9 @@ static int ioctl_get_fmt(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_cropcap(void *arg,struct camera_data *cam)
+static int cpia2_cropcap(struct file *file, void *fh, struct v4l2_cropcap *c)
 {
-	struct v4l2_cropcap *c = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	if (c->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 	       return -EINVAL;
@@ -715,9 +723,9 @@ static int ioctl_cropcap(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_queryctrl(void *arg,struct camera_data *cam)
+static int cpia2_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
 {
-	struct v4l2_queryctrl *c = arg;
+	struct camera_data *cam = video_drvdata(file);
 	int i;
 
 	for(i=0; i<NUM_CONTROLS; ++i) {
@@ -783,12 +791,9 @@ static int ioctl_queryctrl(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_querymenu(void *arg,struct camera_data *cam)
+static int cpia2_querymenu(struct file *file, void *fh, struct v4l2_querymenu *m)
 {
-	struct v4l2_querymenu *m = arg;
-
-	memset(m->name, 0, sizeof(m->name));
-	m->reserved = 0;
+	struct camera_data *cam = video_drvdata(file);
 
 	switch(m->id) {
 	case CPIA2_CID_FLICKER_MODE:
@@ -837,9 +842,9 @@ static int ioctl_querymenu(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_g_ctrl(void *arg,struct camera_data *cam)
+static int cpia2_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
 {
-	struct v4l2_control *c = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	switch(c->id) {
 	case V4L2_CID_BRIGHTNESS:
@@ -955,9 +960,9 @@ static int ioctl_g_ctrl(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_s_ctrl(void *arg,struct camera_data *cam)
+static int cpia2_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
 {
-	struct v4l2_control *c = arg;
+	struct camera_data *cam = video_drvdata(file);
 	int i;
 	int retval = 0;
 
@@ -1031,9 +1036,9 @@ static int ioctl_s_ctrl(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_g_jpegcomp(void *arg,struct camera_data *cam)
+static int cpia2_g_jpegcomp(struct file *file, void *fh, struct v4l2_jpegcompression *parms)
 {
-	struct v4l2_jpegcompression *parms = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	memset(parms, 0, sizeof(*parms));
 
@@ -1072,9 +1077,9 @@ static int ioctl_g_jpegcomp(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_s_jpegcomp(void *arg,struct camera_data *cam)
+static int cpia2_s_jpegcomp(struct file *file, void *fh, struct v4l2_jpegcompression *parms)
 {
-	struct v4l2_jpegcompression *parms = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	DBG("S_JPEGCOMP APP_len:%d COM_len:%d\n",
 	    parms->APP_len, parms->COM_len);
@@ -1121,9 +1126,9 @@ static int ioctl_s_jpegcomp(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_reqbufs(void *arg,struct camera_data *cam)
+static int cpia2_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *req)
 {
-	struct v4l2_requestbuffers *req = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	if(req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
 	   req->memory != V4L2_MEMORY_MMAP)
@@ -1144,9 +1149,9 @@ static int ioctl_reqbufs(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_querybuf(void *arg,struct camera_data *cam)
+static int cpia2_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
-	struct v4l2_buffer *buf = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	if(buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
 	   buf->index > cam->num_frames)
@@ -1192,9 +1197,9 @@ static int ioctl_querybuf(void *arg,struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_qbuf(void *arg,struct camera_data *cam)
+static int cpia2_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
-	struct v4l2_buffer *buf = arg;
+	struct camera_data *cam = video_drvdata(file);
 
 	if(buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
 	   buf->memory != V4L2_MEMORY_MMAP ||
@@ -1248,9 +1253,9 @@ static int find_earliest_filled_buffer(struct camera_data *cam)
  *
  *****************************************************************************/
 
-static int ioctl_dqbuf(void *arg,struct camera_data *cam, struct file *file)
+static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
-	struct v4l2_buffer *buf = arg;
+	struct camera_data *cam = video_drvdata(file);
 	int frame;
 
 	if(buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
@@ -1296,210 +1301,56 @@ static int ioctl_dqbuf(void *arg,struct camera_data *cam, struct file *file)
 	return 0;
 }
 
-/******************************************************************************
- *
- *  cpia2_ioctl
- *
- *****************************************************************************/
-static long cpia2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int cpia2_g_priority(struct file *file, void *_fh, enum v4l2_priority *p)
 {
-	struct camera_data *cam = video_drvdata(file);
-	long retval = 0;
-
-	if (!cam)
-		return -ENOTTY;
-
-	if (!cam->present)
-		return -ENODEV;
-
-	/* Priority check */
-	switch (cmd) {
-	case VIDIOC_S_FMT:
-	{
-		struct cpia2_fh *fh = file->private_data;
-		retval = v4l2_prio_check(&cam->prio, fh->prio);
-		if (retval)
-			return retval;
-		break;
-	}
-	default:
-		break;
-	}
-
-	switch (cmd) {
-	/* CPIA2 extension to Video4Linux API */
-	case CPIA2_IOC_SET_GPIO:
-		retval = ioctl_set_gpio(arg, cam);
-		break;
-	case VIDIOC_QUERYCAP:
-		retval = ioctl_querycap(arg,cam);
-		break;
-
-	case VIDIOC_ENUMINPUT:
-	case VIDIOC_G_INPUT:
-	case VIDIOC_S_INPUT:
-		retval = ioctl_input(cmd, arg, cam);
-		break;
-
-	case VIDIOC_ENUM_FMT:
-		retval = ioctl_enum_fmt(arg,cam);
-		break;
-	case VIDIOC_TRY_FMT:
-		retval = ioctl_try_fmt(arg,cam);
-		break;
-	case VIDIOC_G_FMT:
-		retval = ioctl_get_fmt(arg,cam);
-		break;
-	case VIDIOC_S_FMT:
-		retval = ioctl_set_fmt(arg,cam,file->private_data);
-		break;
+	struct cpia2_fh *fh = _fh;
 
-	case VIDIOC_CROPCAP:
-		retval = ioctl_cropcap(arg,cam);
-		break;
-	case VIDIOC_G_CROP:
-	case VIDIOC_S_CROP:
-		// TODO: I think cropping can be implemented - SJB
-		retval = -EINVAL;
-		break;
-
-	case VIDIOC_QUERYCTRL:
-		retval = ioctl_queryctrl(arg,cam);
-		break;
-	case VIDIOC_QUERYMENU:
-		retval = ioctl_querymenu(arg,cam);
-		break;
-	case VIDIOC_G_CTRL:
-		retval = ioctl_g_ctrl(arg,cam);
-		break;
-	case VIDIOC_S_CTRL:
-		retval = ioctl_s_ctrl(arg,cam);
-		break;
-
-	case VIDIOC_G_JPEGCOMP:
-		retval = ioctl_g_jpegcomp(arg,cam);
-		break;
-	case VIDIOC_S_JPEGCOMP:
-		retval = ioctl_s_jpegcomp(arg,cam);
-		break;
-
-	case VIDIOC_G_PRIORITY:
-	{
-		struct cpia2_fh *fh = file->private_data;
-		*(enum v4l2_priority*)arg = fh->prio;
-		break;
-	}
-	case VIDIOC_S_PRIORITY:
-	{
-		struct cpia2_fh *fh = file->private_data;
-		enum v4l2_priority prio;
-		prio = *(enum v4l2_priority*)arg;
-		if(cam->streaming &&
-		   prio != fh->prio &&
-		   fh->prio == V4L2_PRIORITY_RECORD) {
-			/* Can't drop record priority while streaming */
-			retval = -EBUSY;
-		} else if(prio == V4L2_PRIORITY_RECORD &&
-		   prio != fh->prio &&
-		   v4l2_prio_max(&cam->prio) == V4L2_PRIORITY_RECORD) {
-			/* Only one program can record at a time */
-			retval = -EBUSY;
-		} else {
-			retval = v4l2_prio_change(&cam->prio, &fh->prio, prio);
-		}
-		break;
-	}
-
-	case VIDIOC_REQBUFS:
-		retval = ioctl_reqbufs(arg,cam);
-		break;
-	case VIDIOC_QUERYBUF:
-		retval = ioctl_querybuf(arg,cam);
-		break;
-	case VIDIOC_QBUF:
-		retval = ioctl_qbuf(arg,cam);
-		break;
-	case VIDIOC_DQBUF:
-		retval = ioctl_dqbuf(arg,cam,file);
-		break;
-	case VIDIOC_STREAMON:
-	{
-		int type;
-		DBG("VIDIOC_STREAMON, streaming=%d\n", cam->streaming);
-		type = *(int*)arg;
-		if(!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			retval = -EINVAL;
-
-		if(!cam->streaming) {
-			retval = cpia2_usb_stream_start(cam,
-					  cam->params.camera_state.stream_mode);
-		} else {
-			retval = -EINVAL;
-		}
-
-		break;
-	}
-	case VIDIOC_STREAMOFF:
-	{
-		int type;
-		DBG("VIDIOC_STREAMOFF, streaming=%d\n", cam->streaming);
-		type = *(int*)arg;
-		if(!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			retval = -EINVAL;
-
-		if(cam->streaming) {
-			retval = cpia2_usb_stream_stop(cam);
-		} else {
-			retval = -EINVAL;
-		}
-
-		break;
-	}
-
-	case VIDIOC_ENUMOUTPUT:
-	case VIDIOC_G_OUTPUT:
-	case VIDIOC_S_OUTPUT:
-	case VIDIOC_G_MODULATOR:
-	case VIDIOC_S_MODULATOR:
-
-	case VIDIOC_ENUMAUDIO:
-	case VIDIOC_G_AUDIO:
-	case VIDIOC_S_AUDIO:
+	*p = fh->prio;
+	return 0;
+}
 
-	case VIDIOC_ENUMAUDOUT:
-	case VIDIOC_G_AUDOUT:
-	case VIDIOC_S_AUDOUT:
+static int cpia2_s_priority(struct file *file, void *_fh, enum v4l2_priority prio)
+{
+	struct camera_data *cam = video_drvdata(file);
+	struct cpia2_fh *fh = fh;
 
-	case VIDIOC_ENUMSTD:
-	case VIDIOC_QUERYSTD:
-	case VIDIOC_G_STD:
-	case VIDIOC_S_STD:
+	if (cam->streaming && prio != fh->prio &&
+			fh->prio == V4L2_PRIORITY_RECORD)
+		/* Can't drop record priority while streaming */
+		return -EBUSY;
 
-	case VIDIOC_G_TUNER:
-	case VIDIOC_S_TUNER:
-	case VIDIOC_G_FREQUENCY:
-	case VIDIOC_S_FREQUENCY:
+	if (prio == V4L2_PRIORITY_RECORD && prio != fh->prio &&
+			v4l2_prio_max(&cam->prio) == V4L2_PRIORITY_RECORD)
+		/* Only one program can record at a time */
+		return -EBUSY;
+	return v4l2_prio_change(&cam->prio, &fh->prio, prio);
+}
 
-	case VIDIOC_OVERLAY:
-	case VIDIOC_G_FBUF:
-	case VIDIOC_S_FBUF:
+static int cpia2_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct camera_data *cam = video_drvdata(file);
 
-	case VIDIOC_G_PARM:
-	case VIDIOC_S_PARM:
-		retval = -EINVAL;
-		break;
-	default:
-		retval = -ENOIOCTLCMD;
-		break;
-	}
+	DBG("VIDIOC_STREAMON, streaming=%d\n", cam->streaming);
+	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-	return retval;
+	if (!cam->streaming)
+		return cpia2_usb_stream_start(cam,
+				cam->params.camera_state.stream_mode);
+	return -EINVAL;
 }
 
-static long cpia2_ioctl(struct file *file,
-		       unsigned int cmd, unsigned long arg)
+static int cpia2_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 {
-	return video_usercopy(file, cmd, arg, cpia2_do_ioctl);
+	struct camera_data *cam = video_drvdata(file);
+
+	DBG("VIDIOC_STREAMOFF, streaming=%d\n", cam->streaming);
+	if (!cam->mmapped || type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (cam->streaming)
+		return cpia2_usb_stream_stop(cam);
+	return -EINVAL;
 }
 
 /******************************************************************************
@@ -1550,6 +1401,33 @@ static void reset_camera_struct_v4l(struct camera_data *cam)
 	v4l2_prio_init(&cam->prio);
 }
 
+static const struct v4l2_ioctl_ops cpia2_ioctl_ops = {
+	.vidioc_querycap		    = cpia2_querycap,
+	.vidioc_enum_input		    = cpia2_enum_input,
+	.vidioc_g_input			    = cpia2_g_input,
+	.vidioc_s_input			    = cpia2_s_input,
+	.vidioc_enum_fmt_vid_cap	    = cpia2_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		    = cpia2_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		    = cpia2_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		    = cpia2_try_fmt_vid_cap,
+	.vidioc_queryctrl		    = cpia2_queryctrl,
+	.vidioc_querymenu		    = cpia2_querymenu,
+	.vidioc_g_ctrl			    = cpia2_g_ctrl,
+	.vidioc_s_ctrl			    = cpia2_s_ctrl,
+	.vidioc_g_jpegcomp		    = cpia2_g_jpegcomp,
+	.vidioc_s_jpegcomp		    = cpia2_s_jpegcomp,
+	.vidioc_cropcap			    = cpia2_cropcap,
+	.vidioc_reqbufs			    = cpia2_reqbufs,
+	.vidioc_querybuf		    = cpia2_querybuf,
+	.vidioc_qbuf			    = cpia2_qbuf,
+	.vidioc_dqbuf			    = cpia2_dqbuf,
+	.vidioc_streamon		    = cpia2_streamon,
+	.vidioc_streamoff		    = cpia2_streamoff,
+	.vidioc_g_priority		    = cpia2_g_priority,
+	.vidioc_s_priority		    = cpia2_s_priority,
+	.vidioc_default			    = cpia2_default,
+};
+
 /***
  * The v4l video device structure initialized for this device
  ***/
@@ -1559,7 +1437,7 @@ static const struct v4l2_file_operations cpia2_fops = {
 	.release	= cpia2_close,
 	.read		= cpia2_v4l_read,
 	.poll		= cpia2_v4l_poll,
-	.unlocked_ioctl	= cpia2_ioctl,
+	.unlocked_ioctl	= video_ioctl2,
 	.mmap		= cpia2_mmap,
 };
 
@@ -1567,6 +1445,7 @@ static struct video_device cpia2_template = {
 	/* I could not find any place for the old .initialize initializer?? */
 	.name =		"CPiA2 Camera",
 	.fops =		&cpia2_fops,
+	.ioctl_ops =	&cpia2_ioctl_ops,
 	.release =	video_device_release,
 };
 
diff --git a/drivers/media/video/cs5345.c b/drivers/media/video/cs5345.c
index 9358fe77e562..5909f2557ab4 100644
--- a/drivers/media/video/cs5345.c
+++ b/drivers/media/video/cs5345.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 MODULE_DESCRIPTION("i2c device driver for cs5345 Audio ADC");
 MODULE_AUTHOR("Hans Verkuil");
@@ -36,6 +37,20 @@ module_param(debug, bool, 0644);
 
 MODULE_PARM_DESC(debug, "Debugging messages, 0=Off (default), 1=On");
 
+struct cs5345_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+};
+
+static inline struct cs5345_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct cs5345_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct cs5345_state, hdl)->sd;
+}
 
 /* ----------------------------------------------------------------------- */
 
@@ -65,33 +80,20 @@ static int cs5345_s_routing(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int cs5345_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int cs5345_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	if (ctrl->id == V4L2_CID_AUDIO_MUTE) {
-		ctrl->value = (cs5345_read(sd, 0x04) & 0x08) != 0;
-		return 0;
-	}
-	if (ctrl->id != V4L2_CID_AUDIO_VOLUME)
-		return -EINVAL;
-	ctrl->value = cs5345_read(sd, 0x07) & 0x3f;
-	if (ctrl->value >= 32)
-		ctrl->value = ctrl->value - 64;
-	return 0;
-}
+	struct v4l2_subdev *sd = to_sd(ctrl);
 
-static int cs5345_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	if (ctrl->id == V4L2_CID_AUDIO_MUTE) {
-		cs5345_write(sd, 0x04, ctrl->value ? 0x80 : 0);
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		cs5345_write(sd, 0x04, ctrl->val ? 0x80 : 0);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		cs5345_write(sd, 0x07, ((u8)ctrl->val) & 0x3f);
+		cs5345_write(sd, 0x08, ((u8)ctrl->val) & 0x3f);
 		return 0;
 	}
-	if (ctrl->id != V4L2_CID_AUDIO_VOLUME)
-		return -EINVAL;
-	if (ctrl->value > 24 || ctrl->value < -24)
-		return -EINVAL;
-	cs5345_write(sd, 0x07, ((u8)ctrl->value) & 0x3f);
-	cs5345_write(sd, 0x08, ((u8)ctrl->value) & 0x3f);
-	return 0;
+	return -EINVAL;
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -144,11 +146,20 @@ static int cs5345_log_status(struct v4l2_subdev *sd)
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops cs5345_ctrl_ops = {
+	.s_ctrl = cs5345_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops cs5345_core_ops = {
 	.log_status = cs5345_log_status,
 	.g_chip_ident = cs5345_g_chip_ident,
-	.g_ctrl = cs5345_g_ctrl,
-	.s_ctrl = cs5345_s_ctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = cs5345_g_register,
 	.s_register = cs5345_s_register,
@@ -169,6 +180,7 @@ static const struct v4l2_subdev_ops cs5345_ops = {
 static int cs5345_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
+	struct cs5345_state *state;
 	struct v4l2_subdev *sd;
 
 	/* Check if the adapter supports the needed features */
@@ -178,11 +190,28 @@ static int cs5345_probe(struct i2c_client *client,
 	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)
+	state = kzalloc(sizeof(struct cs5345_state), GFP_KERNEL);
+	if (state == NULL)
 		return -ENOMEM;
+	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &cs5345_ops);
 
+	v4l2_ctrl_handler_init(&state->hdl, 2);
+	v4l2_ctrl_new_std(&state->hdl, &cs5345_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &cs5345_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, -24, 24, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+	/* set volume/mute */
+	v4l2_ctrl_handler_setup(&state->hdl);
+
 	cs5345_write(sd, 0x02, 0x00);
 	cs5345_write(sd, 0x04, 0x01);
 	cs5345_write(sd, 0x09, 0x01);
@@ -194,9 +223,11 @@ static int cs5345_probe(struct i2c_client *client,
 static int cs5345_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct cs5345_state *state = to_state(sd);
 
 	v4l2_device_unregister_subdev(sd);
-	kfree(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
 	return 0;
 }
 
diff --git a/drivers/media/video/cx18/cx18-av-audio.c b/drivers/media/video/cx18/cx18-av-audio.c
index 43d09a24b262..4a24ffb17a7d 100644
--- a/drivers/media/video/cx18/cx18-av-audio.c
+++ b/drivers/media/video/cx18/cx18-av-audio.c
@@ -342,17 +342,6 @@ void cx18_av_audio_set_path(struct cx18 *cx)
 	}
 }
 
-static int get_volume(struct cx18 *cx)
-{
-	/* Volume runs +18dB to -96dB in 1/2dB steps
-	 * change to fit the msp3400 -114dB to +12dB range */
-
-	/* check PATH1_VOLUME */
-	int vol = 228 - cx18_av_read(cx, 0x8d4);
-	vol = (vol / 2) + 23;
-	return vol << 9;
-}
-
 static void set_volume(struct cx18 *cx, int volume)
 {
 	/* First convert the volume to msp3400 values (0-127) */
@@ -369,52 +358,18 @@ static void set_volume(struct cx18 *cx, int volume)
 	cx18_av_write(cx, 0x8d4, 228 - (vol * 2));
 }
 
-static int get_bass(struct cx18 *cx)
-{
-	/* bass is 49 steps +12dB to -12dB */
-
-	/* check PATH1_EQ_BASS_VOL */
-	int bass = cx18_av_read(cx, 0x8d9) & 0x3f;
-	bass = (((48 - bass) * 0xffff) + 47) / 48;
-	return bass;
-}
-
 static void set_bass(struct cx18 *cx, int bass)
 {
 	/* PATH1_EQ_BASS_VOL */
 	cx18_av_and_or(cx, 0x8d9, ~0x3f, 48 - (bass * 48 / 0xffff));
 }
 
-static int get_treble(struct cx18 *cx)
-{
-	/* treble is 49 steps +12dB to -12dB */
-
-	/* check PATH1_EQ_TREBLE_VOL */
-	int treble = cx18_av_read(cx, 0x8db) & 0x3f;
-	treble = (((48 - treble) * 0xffff) + 47) / 48;
-	return treble;
-}
-
 static void set_treble(struct cx18 *cx, int treble)
 {
 	/* PATH1_EQ_TREBLE_VOL */
 	cx18_av_and_or(cx, 0x8db, ~0x3f, 48 - (treble * 48 / 0xffff));
 }
 
-static int get_balance(struct cx18 *cx)
-{
-	/* balance is 7 bit, 0 to -96dB */
-
-	/* check PATH1_BAL_LEVEL */
-	int balance = cx18_av_read(cx, 0x8d5) & 0x7f;
-	/* check PATH1_BAL_LEFT */
-	if ((cx18_av_read(cx, 0x8d5) & 0x80) == 0)
-		balance = 0x80 - balance;
-	else
-		balance = 0x80 + balance;
-	return balance << 8;
-}
-
 static void set_balance(struct cx18 *cx, int balance)
 {
 	int bal = balance >> 8;
@@ -431,12 +386,6 @@ static void set_balance(struct cx18 *cx, int balance)
 	}
 }
 
-static int get_mute(struct cx18 *cx)
-{
-	/* check SRC1_MUTE_EN */
-	return cx18_av_read(cx, 0x8d3) & 0x2 ? 1 : 0;
-}
-
 static void set_mute(struct cx18 *cx, int mute)
 {
 	struct cx18_av_state *state = &cx->av_state;
@@ -490,50 +439,33 @@ int cx18_av_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 	return retval;
 }
 
-int cx18_av_audio_g_ctrl(struct cx18 *cx, struct v4l2_control *ctrl)
+static int cx18_av_audio_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		ctrl->value = get_volume(cx);
-		break;
-	case V4L2_CID_AUDIO_BASS:
-		ctrl->value = get_bass(cx);
-		break;
-	case V4L2_CID_AUDIO_TREBLE:
-		ctrl->value = get_treble(cx);
-		break;
-	case V4L2_CID_AUDIO_BALANCE:
-		ctrl->value = get_balance(cx);
-		break;
-	case V4L2_CID_AUDIO_MUTE:
-		ctrl->value = get_mute(cx);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct cx18 *cx = v4l2_get_subdevdata(sd);
 
-int cx18_av_audio_s_ctrl(struct cx18 *cx, struct v4l2_control *ctrl)
-{
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_VOLUME:
-		set_volume(cx, ctrl->value);
+		set_volume(cx, ctrl->val);
 		break;
 	case V4L2_CID_AUDIO_BASS:
-		set_bass(cx, ctrl->value);
+		set_bass(cx, ctrl->val);
 		break;
 	case V4L2_CID_AUDIO_TREBLE:
-		set_treble(cx, ctrl->value);
+		set_treble(cx, ctrl->val);
 		break;
 	case V4L2_CID_AUDIO_BALANCE:
-		set_balance(cx, ctrl->value);
+		set_balance(cx, ctrl->val);
 		break;
 	case V4L2_CID_AUDIO_MUTE:
-		set_mute(cx, ctrl->value);
+		set_mute(cx, ctrl->val);
 		break;
 	default:
 		return -EINVAL;
 	}
 	return 0;
 }
+
+const struct v4l2_ctrl_ops cx18_av_audio_ctrl_ops = {
+	.s_ctrl = cx18_av_audio_s_ctrl,
+};
diff --git a/drivers/media/video/cx18/cx18-av-core.c b/drivers/media/video/cx18/cx18-av-core.c
index a41951cab276..f164b7f610a5 100644
--- a/drivers/media/video/cx18/cx18-av-core.c
+++ b/drivers/media/video/cx18/cx18-av-core.c
@@ -129,6 +129,7 @@ static void cx18_av_initialize(struct v4l2_subdev *sd)
 {
 	struct cx18_av_state *state = to_cx18_av_state(sd);
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
+	int default_volume;
 	u32 v;
 
 	cx18_av_loadfw(cx);
@@ -247,8 +248,23 @@ static void cx18_av_initialize(struct v4l2_subdev *sd)
 /*      	CxDevWrReg(CXADEC_SRC_COMB_CFG, 0x6628021F); */
 /*    } */
 	cx18_av_write4(cx, CXADEC_SRC_COMB_CFG, 0x6628021F);
-	state->default_volume = 228 - cx18_av_read(cx, 0x8d4);
-	state->default_volume = ((state->default_volume / 2) + 23) << 9;
+	default_volume = cx18_av_read(cx, 0x8d4);
+	/*
+	 * Enforce the legacy volume scale mapping limits to avoid
+	 * -ERANGE errors when initializing the volume control
+	 */
+	if (default_volume > 228) {
+		/* Bottom out at -96 dB, v4l2 vol range 0x2e00-0x2fff */
+		default_volume = 228;
+		cx18_av_write(cx, 0x8d4, 228);
+	} else if (default_volume < 20) {
+		/* Top out at + 8 dB, v4l2 vol range 0xfe00-0xffff */
+		default_volume = 20;
+		cx18_av_write(cx, 0x8d4, 20);
+	}
+	default_volume = (((228 - default_volume) >> 1) + 23) << 9;
+	state->volume->cur.val = state->volume->default_value = default_volume;
+	v4l2_ctrl_handler_setup(&state->hdl);
 }
 
 static int cx18_av_reset(struct v4l2_subdev *sd, u32 val)
@@ -901,126 +917,35 @@ static int cx18_av_s_radio(struct v4l2_subdev *sd)
 	return 0;
 }
 
-static int cx18_av_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int cx18_av_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if (ctrl->value < 0 || ctrl->value > 255) {
-			CX18_ERR_DEV(sd, "invalid brightness setting %d\n",
-				     ctrl->value);
-			return -ERANGE;
-		}
-
-		cx18_av_write(cx, 0x414, ctrl->value - 128);
+		cx18_av_write(cx, 0x414, ctrl->val - 128);
 		break;
 
 	case V4L2_CID_CONTRAST:
-		if (ctrl->value < 0 || ctrl->value > 127) {
-			CX18_ERR_DEV(sd, "invalid contrast setting %d\n",
-				     ctrl->value);
-			return -ERANGE;
-		}
-
-		cx18_av_write(cx, 0x415, ctrl->value << 1);
+		cx18_av_write(cx, 0x415, ctrl->val << 1);
 		break;
 
 	case V4L2_CID_SATURATION:
-		if (ctrl->value < 0 || ctrl->value > 127) {
-			CX18_ERR_DEV(sd, "invalid saturation setting %d\n",
-				     ctrl->value);
-			return -ERANGE;
-		}
-
-		cx18_av_write(cx, 0x420, ctrl->value << 1);
-		cx18_av_write(cx, 0x421, ctrl->value << 1);
+		cx18_av_write(cx, 0x420, ctrl->val << 1);
+		cx18_av_write(cx, 0x421, ctrl->val << 1);
 		break;
 
 	case V4L2_CID_HUE:
-		if (ctrl->value < -128 || ctrl->value > 127) {
-			CX18_ERR_DEV(sd, "invalid hue setting %d\n",
-				     ctrl->value);
-			return -ERANGE;
-		}
-
-		cx18_av_write(cx, 0x422, ctrl->value);
+		cx18_av_write(cx, 0x422, ctrl->val);
 		break;
 
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_MUTE:
-		return cx18_av_audio_s_ctrl(cx, ctrl);
-
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int cx18_av_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct cx18 *cx = v4l2_get_subdevdata(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = (s8)cx18_av_read(cx, 0x414) + 128;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = cx18_av_read(cx, 0x415) >> 1;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = cx18_av_read(cx, 0x420) >> 1;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = (s8)cx18_av_read(cx, 0x422);
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_MUTE:
-		return cx18_av_audio_g_ctrl(cx, ctrl);
 	default:
 		return -EINVAL;
 	}
 	return 0;
 }
 
-static int cx18_av_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	struct cx18_av_state *state = to_cx18_av_state(sd);
-
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 127, 1, 64);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-	default:
-		break;
-	}
-
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 65535,
-			65535 / 100, state->default_volume);
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 0);
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-		return v4l2_ctrl_query_fill(qc, 0, 65535, 65535 / 100, 32768);
-	default:
-		return -EINVAL;
-	}
-	return -EINVAL;
-}
-
 static int cx18_av_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
 	struct cx18_av_state *state = to_cx18_av_state(sd);
@@ -1356,14 +1281,22 @@ static int cx18_av_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
+static const struct v4l2_ctrl_ops cx18_av_ctrl_ops = {
+	.s_ctrl = cx18_av_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops cx18_av_general_ops = {
 	.g_chip_ident = cx18_av_g_chip_ident,
 	.log_status = cx18_av_log_status,
 	.load_fw = cx18_av_load_fw,
 	.reset = cx18_av_reset,
-	.queryctrl = cx18_av_queryctrl,
-	.g_ctrl = cx18_av_g_ctrl,
-	.s_ctrl = cx18_av_s_ctrl,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 	.s_std = cx18_av_s_std,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = cx18_av_g_register,
@@ -1427,8 +1360,42 @@ int cx18_av_probe(struct cx18 *cx)
 	snprintf(sd->name, sizeof(sd->name),
 		 "%s %03x", cx->v4l2_dev.name, (state->rev >> 4));
 	sd->grp_id = CX18_HW_418_AV;
+	v4l2_ctrl_handler_init(&state->hdl, 9);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+
+	state->volume = v4l2_ctrl_new_std(&state->hdl,
+			&cx18_av_audio_ctrl_ops, V4L2_CID_AUDIO_VOLUME,
+			0, 65535, 65535 / 100, 0);
+	v4l2_ctrl_new_std(&state->hdl,
+			&cx18_av_audio_ctrl_ops, V4L2_CID_AUDIO_MUTE,
+			0, 1, 1, 0);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_audio_ctrl_ops,
+			V4L2_CID_AUDIO_BALANCE,
+			0, 65535, 65535 / 100, 32768);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_audio_ctrl_ops,
+			V4L2_CID_AUDIO_BASS,
+			0, 65535, 65535 / 100, 32768);
+	v4l2_ctrl_new_std(&state->hdl, &cx18_av_audio_ctrl_ops,
+			V4L2_CID_AUDIO_TREBLE,
+			0, 65535, 65535 / 100, 32768);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		return err;
+	}
 	err = v4l2_device_register_subdev(&cx->v4l2_dev, sd);
-	if (!err)
+	if (err)
+		v4l2_ctrl_handler_free(&state->hdl);
+	else
 		cx18_av_init(cx);
 	return err;
 }
diff --git a/drivers/media/video/cx18/cx18-av-core.h b/drivers/media/video/cx18/cx18-av-core.h
index 1956991795e3..188c9c3d2db1 100644
--- a/drivers/media/video/cx18/cx18-av-core.h
+++ b/drivers/media/video/cx18/cx18-av-core.h
@@ -26,6 +26,7 @@
 #define _CX18_AV_CORE_H_
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
 struct cx18;
 
@@ -95,13 +96,14 @@ enum cx18_av_audio_input {
 
 struct cx18_av_state {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *volume;
 	int radio;
 	v4l2_std_id std;
 	enum cx18_av_video_input vid_input;
 	enum cx18_av_audio_input aud_input;
 	u32 audclk_freq;
 	int audmode;
-	int default_volume;
 	u32 id;
 	u32 rev;
 	int is_initialized;
@@ -347,6 +349,11 @@ static inline struct cx18_av_state *to_cx18_av_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct cx18_av_state, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct cx18_av_state, hdl)->sd;
+}
+
 /* ----------------------------------------------------------------------- */
 /* cx18_av-core.c 							   */
 int cx18_av_write(struct cx18 *cx, u16 addr, u8 value);
@@ -369,10 +376,9 @@ int cx18_av_loadfw(struct cx18 *cx);
 
 /* ----------------------------------------------------------------------- */
 /* cx18_av-audio.c                                                         */
-int cx18_av_audio_g_ctrl(struct cx18 *cx, struct v4l2_control *ctrl);
-int cx18_av_audio_s_ctrl(struct cx18 *cx, struct v4l2_control *ctrl);
 int cx18_av_s_clock_freq(struct v4l2_subdev *sd, u32 freq);
 void cx18_av_audio_set_path(struct cx18 *cx);
+extern const struct v4l2_ctrl_ops cx18_av_audio_ctrl_ops;
 
 /* ----------------------------------------------------------------------- */
 /* cx18_av-vbi.c                                                           */
diff --git a/drivers/media/video/cx18/cx18-cards.c b/drivers/media/video/cx18/cx18-cards.c
index 87177733cf92..68ad1963f421 100644
--- a/drivers/media/video/cx18/cx18-cards.c
+++ b/drivers/media/video/cx18/cx18-cards.c
@@ -95,6 +95,53 @@ static const struct cx18_card cx18_card_hvr1600_esmt = {
 	.i2c = &cx18_i2c_std,
 };
 
+static const struct cx18_card cx18_card_hvr1600_s5h1411 = {
+	.type = CX18_CARD_HVR_1600_S5H1411,
+	.name = "Hauppauge HVR-1600",
+	.comment = "Simultaneous Digital and Analog TV capture supported\n",
+	.v4l2_capabilities = CX18_CAP_ENCODER,
+	.hw_audio_ctrl = CX18_HW_418_AV,
+	.hw_muxer = CX18_HW_CS5345,
+	.hw_all = CX18_HW_TVEEPROM | CX18_HW_418_AV | CX18_HW_TUNER |
+		  CX18_HW_CS5345 | CX18_HW_DVB | CX18_HW_GPIO_RESET_CTRL |
+		  CX18_HW_Z8F0811_IR_HAUP,
+	.video_inputs = {
+		{ CX18_CARD_INPUT_VID_TUNER,  0, CX18_AV_COMPOSITE7 },
+		{ CX18_CARD_INPUT_SVIDEO1,    1, CX18_AV_SVIDEO1    },
+		{ CX18_CARD_INPUT_COMPOSITE1, 1, CX18_AV_COMPOSITE3 },
+		{ CX18_CARD_INPUT_SVIDEO2,    2, CX18_AV_SVIDEO2    },
+		{ CX18_CARD_INPUT_COMPOSITE2, 2, CX18_AV_COMPOSITE4 },
+	},
+	.audio_inputs = {
+		{ CX18_CARD_INPUT_AUD_TUNER,
+		  CX18_AV_AUDIO8, CS5345_IN_1 | CS5345_MCLK_1_5 },
+		{ CX18_CARD_INPUT_LINE_IN1,
+		  CX18_AV_AUDIO_SERIAL1, CS5345_IN_2 },
+		{ CX18_CARD_INPUT_LINE_IN2,
+		  CX18_AV_AUDIO_SERIAL1, CS5345_IN_3 },
+	},
+	.radio_input = { CX18_CARD_INPUT_AUD_TUNER,
+			 CX18_AV_AUDIO_SERIAL1, CS5345_IN_4 },
+	.ddr = {
+		/* ESMT M13S128324A-5B memory */
+		.chip_config = 0x003,
+		.refresh = 0x30c,
+		.timing1 = 0x44220e82,
+		.timing2 = 0x08,
+		.tune_lane = 0,
+		.initial_emrs = 0,
+	},
+	.gpio_init.initial_value = 0x3001,
+	.gpio_init.direction = 0x3001,
+	.gpio_i2c_slave_reset = {
+		.active_lo_mask = 0x3001,
+		.msecs_asserted = 10,
+		.msecs_recovery = 40,
+		.ir_reset_mask  = 0x0001,
+	},
+	.i2c = &cx18_i2c_std,
+};
+
 static const struct cx18_card cx18_card_hvr1600_samsung = {
 	.type = CX18_CARD_HVR_1600_SAMSUNG,
 	.name = "Hauppauge HVR-1600 (Preproduction)",
@@ -523,7 +570,8 @@ static const struct cx18_card *cx18_card_list[] = {
 	&cx18_card_toshiba_qosmio_dvbt,
 	&cx18_card_leadtek_pvr2100,
 	&cx18_card_leadtek_dvr3100h,
-	&cx18_card_gotview_dvd3
+	&cx18_card_gotview_dvd3,
+	&cx18_card_hvr1600_s5h1411
 };
 
 const struct cx18_card *cx18_get_card(u16 index)
diff --git a/drivers/media/video/cx18/cx18-controls.c b/drivers/media/video/cx18/cx18-controls.c
index 97d7b7e100a3..282a3d29fdaa 100644
--- a/drivers/media/video/cx18/cx18-controls.c
+++ b/drivers/media/video/cx18/cx18-controls.c
@@ -30,152 +30,11 @@
 #include "cx18-mailbox.h"
 #include "cx18-controls.h"
 
-/* Must be sorted from low to high control ID! */
-static const u32 user_ctrls[] = {
-	V4L2_CID_USER_CLASS,
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_AUDIO_VOLUME,
-	V4L2_CID_AUDIO_BALANCE,
-	V4L2_CID_AUDIO_BASS,
-	V4L2_CID_AUDIO_TREBLE,
-	V4L2_CID_AUDIO_MUTE,
-	V4L2_CID_AUDIO_LOUDNESS,
-	0
-};
-
-static const u32 *ctrl_classes[] = {
-	user_ctrls,
-	cx2341x_mpeg_ctrls,
-	NULL
-};
-
-int cx18_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qctrl)
-{
-	struct cx18 *cx = ((struct cx18_open_id *)fh)->cx;
-	const char *name;
-
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (qctrl->id == 0)
-		return -EINVAL;
-
-	switch (qctrl->id) {
-	/* Standard V4L2 controls */
-	case V4L2_CID_USER_CLASS:
-		return v4l2_ctrl_query_fill(qctrl, 0, 0, 0, 0);
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_CONTRAST:
-		if (v4l2_subdev_call(cx->sd_av, core, queryctrl, qctrl))
-			qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
-		return 0;
-
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_MUTE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_LOUDNESS:
-		if (v4l2_subdev_call(cx->sd_av, core, queryctrl, qctrl))
-			qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
-		return 0;
-
-	default:
-		if (cx2341x_ctrl_query(&cx->params, qctrl))
-			qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
-		return 0;
-	}
-	strncpy(qctrl->name, name, sizeof(qctrl->name) - 1);
-	qctrl->name[sizeof(qctrl->name) - 1] = 0;
-	return 0;
-}
-
-int cx18_querymenu(struct file *file, void *fh, struct v4l2_querymenu *qmenu)
+static int cx18_s_stream_vbi_fmt(struct cx2341x_handler *cxhdl, u32 fmt)
 {
-	struct cx18 *cx = ((struct cx18_open_id *)fh)->cx;
-	struct v4l2_queryctrl qctrl;
-
-	qctrl.id = qmenu->id;
-	cx18_queryctrl(file, fh, &qctrl);
-	return v4l2_ctrl_query_menu(qmenu, &qctrl,
-			cx2341x_ctrl_get_menu(&cx->params, qmenu->id));
-}
-
-static int cx18_try_ctrl(struct file *file, void *fh,
-					struct v4l2_ext_control *vctrl)
-{
-	struct v4l2_queryctrl qctrl;
-	const char * const *menu_items = NULL;
-	int err;
-
-	qctrl.id = vctrl->id;
-	err = cx18_queryctrl(file, fh, &qctrl);
-	if (err)
-		return err;
-	if (qctrl.type == V4L2_CTRL_TYPE_MENU)
-		menu_items = v4l2_ctrl_get_menu(qctrl.id);
-	return v4l2_ctrl_check(vctrl, &qctrl, menu_items);
-}
-
-static int cx18_s_ctrl(struct cx18 *cx, struct v4l2_control *vctrl)
-{
-	switch (vctrl->id) {
-		/* Standard V4L2 controls */
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_CONTRAST:
-		return v4l2_subdev_call(cx->sd_av, core, s_ctrl, vctrl);
-
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_MUTE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_LOUDNESS:
-		return v4l2_subdev_call(cx->sd_av, core, s_ctrl, vctrl);
-
-	default:
-		CX18_DEBUG_IOCTL("invalid control 0x%x\n", vctrl->id);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int cx18_g_ctrl(struct cx18 *cx, struct v4l2_control *vctrl)
-{
-	switch (vctrl->id) {
-		/* Standard V4L2 controls */
-	case V4L2_CID_BRIGHTNESS:
-	case V4L2_CID_HUE:
-	case V4L2_CID_SATURATION:
-	case V4L2_CID_CONTRAST:
-		return v4l2_subdev_call(cx->sd_av, core, g_ctrl, vctrl);
-
-	case V4L2_CID_AUDIO_VOLUME:
-	case V4L2_CID_AUDIO_MUTE:
-	case V4L2_CID_AUDIO_BALANCE:
-	case V4L2_CID_AUDIO_BASS:
-	case V4L2_CID_AUDIO_TREBLE:
-	case V4L2_CID_AUDIO_LOUDNESS:
-		return v4l2_subdev_call(cx->sd_av, core, g_ctrl, vctrl);
+	struct cx18 *cx = container_of(cxhdl, struct cx18, cxhdl);
+	int type = cxhdl->stream_type->val;
 
-	default:
-		CX18_DEBUG_IOCTL("invalid control 0x%x\n", vctrl->id);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int cx18_setup_vbi_fmt(struct cx18 *cx,
-			      enum v4l2_mpeg_stream_vbi_fmt fmt,
-			      enum v4l2_mpeg_stream_type type)
-{
-	if (!(cx->v4l2_cap & V4L2_CAP_SLICED_VBI_CAPTURE))
-		return -EINVAL;
 	if (atomic_read(&cx->ana_capturing) > 0)
 		return -EBUSY;
 
@@ -230,121 +89,43 @@ static int cx18_setup_vbi_fmt(struct cx18 *cx,
 	return 0;
 }
 
-int cx18_g_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *c)
+static int cx18_s_video_encoding(struct cx2341x_handler *cxhdl, u32 val)
 {
-	struct cx18 *cx = ((struct cx18_open_id *)fh)->cx;
-	struct v4l2_control ctrl;
-
-	if (c->ctrl_class == V4L2_CTRL_CLASS_USER) {
-		int i;
-		int err = 0;
-
-		for (i = 0; i < c->count; i++) {
-			ctrl.id = c->controls[i].id;
-			ctrl.value = c->controls[i].value;
-			err = cx18_g_ctrl(cx, &ctrl);
-			c->controls[i].value = ctrl.value;
-			if (err) {
-				c->error_idx = i;
-				break;
-			}
-		}
-		return err;
-	}
-	if (c->ctrl_class == V4L2_CTRL_CLASS_MPEG)
-		return cx2341x_ext_ctrls(&cx->params, 0, c, VIDIOC_G_EXT_CTRLS);
-	return -EINVAL;
+	struct cx18 *cx = container_of(cxhdl, struct cx18, cxhdl);
+	int is_mpeg1 = val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
+	struct v4l2_mbus_framefmt fmt;
+
+	/* fix videodecoder resolution */
+	fmt.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
+	fmt.height = cxhdl->height;
+	fmt.code = V4L2_MBUS_FMT_FIXED;
+	v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &fmt);
+	return 0;
 }
 
-int cx18_s_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *c)
+static int cx18_s_audio_sampling_freq(struct cx2341x_handler *cxhdl, u32 idx)
 {
-	struct cx18_open_id *id = fh;
-	struct cx18 *cx = id->cx;
-	int ret;
-	struct v4l2_control ctrl;
-
-	ret = v4l2_prio_check(&cx->prio, id->prio);
-	if (ret)
-		return ret;
-
-	if (c->ctrl_class == V4L2_CTRL_CLASS_USER) {
-		int i;
-		int err = 0;
-
-		for (i = 0; i < c->count; i++) {
-			ctrl.id = c->controls[i].id;
-			ctrl.value = c->controls[i].value;
-			err = cx18_s_ctrl(cx, &ctrl);
-			c->controls[i].value = ctrl.value;
-			if (err) {
-				c->error_idx = i;
-				break;
-			}
-		}
-		return err;
-	}
-	if (c->ctrl_class == V4L2_CTRL_CLASS_MPEG) {
-		static u32 freqs[3] = { 44100, 48000, 32000 };
-		struct cx18_api_func_private priv;
-		struct cx2341x_mpeg_params p = cx->params;
-		int err = cx2341x_ext_ctrls(&p, atomic_read(&cx->ana_capturing),
-						c, VIDIOC_S_EXT_CTRLS);
-		unsigned int idx;
-
-		if (err)
-			return err;
+	static const u32 freqs[3] = { 44100, 48000, 32000 };
+	struct cx18 *cx = container_of(cxhdl, struct cx18, cxhdl);
 
-		if (p.video_encoding != cx->params.video_encoding) {
-			int is_mpeg1 = p.video_encoding ==
-						V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-			struct v4l2_mbus_framefmt fmt;
-
-			/* fix videodecoder resolution */
-			fmt.width = cx->params.width / (is_mpeg1 ? 2 : 1);
-			fmt.height = cx->params.height;
-			fmt.code = V4L2_MBUS_FMT_FIXED;
-			v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &fmt);
-		}
-		priv.cx = cx;
-		priv.s = &cx->streams[id->type];
-		err = cx2341x_update(&priv, cx18_api_func, &cx->params, &p);
-		if (!err &&
-		    (cx->params.stream_vbi_fmt != p.stream_vbi_fmt ||
-		     cx->params.stream_type != p.stream_type))
-			err = cx18_setup_vbi_fmt(cx, p.stream_vbi_fmt,
-						 p.stream_type);
-		cx->params = p;
-		cx->dualwatch_stereo_mode = p.audio_properties & 0x0300;
-		idx = p.audio_properties & 0x03;
-		/* The audio clock of the digitizer must match the codec sample
-		   rate otherwise you get some very strange effects. */
-		if (idx < ARRAY_SIZE(freqs))
-			cx18_call_all(cx, audio, s_clock_freq, freqs[idx]);
-		return err;
-	}
-	return -EINVAL;
+	/* The audio clock of the digitizer must match the codec sample
+	   rate otherwise you get some very strange effects. */
+	if (idx < ARRAY_SIZE(freqs))
+		cx18_call_all(cx, audio, s_clock_freq, freqs[idx]);
+	return 0;
 }
 
-int cx18_try_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *c)
+static int cx18_s_audio_mode(struct cx2341x_handler *cxhdl, u32 val)
 {
-	struct cx18 *cx = ((struct cx18_open_id *)fh)->cx;
+	struct cx18 *cx = container_of(cxhdl, struct cx18, cxhdl);
 
-	if (c->ctrl_class == V4L2_CTRL_CLASS_USER) {
-		int i;
-		int err = 0;
-
-		for (i = 0; i < c->count; i++) {
-			err = cx18_try_ctrl(file, fh, &c->controls[i]);
-			if (err) {
-				c->error_idx = i;
-				break;
-			}
-		}
-		return err;
-	}
-	if (c->ctrl_class == V4L2_CTRL_CLASS_MPEG)
-		return cx2341x_ext_ctrls(&cx->params,
-						atomic_read(&cx->ana_capturing),
-						c, VIDIOC_TRY_EXT_CTRLS);
-	return -EINVAL;
+	cx->dualwatch_stereo_mode = val;
+	return 0;
 }
+
+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,
+	.s_stream_vbi_fmt = cx18_s_stream_vbi_fmt,
+};
diff --git a/drivers/media/video/cx18/cx18-controls.h b/drivers/media/video/cx18/cx18-controls.h
index e46323700b81..cb5dfc7b2054 100644
--- a/drivers/media/video/cx18/cx18-controls.h
+++ b/drivers/media/video/cx18/cx18-controls.h
@@ -21,9 +21,4 @@
  *  02111-1307  USA
  */
 
-int cx18_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *a);
-int cx18_g_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *a);
-int cx18_s_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *a);
-int cx18_try_ext_ctrls(struct file *file, void *fh,
-			struct v4l2_ext_controls *a);
-int cx18_querymenu(struct file *file, void *fh, struct v4l2_querymenu *a);
+extern struct cx2341x_handler_ops cx18_cxhdl_ops;
diff --git a/drivers/media/video/cx18/cx18-driver.c b/drivers/media/video/cx18/cx18-driver.c
index 944af8adbe0c..321c1b79794c 100644
--- a/drivers/media/video/cx18/cx18-driver.c
+++ b/drivers/media/video/cx18/cx18-driver.c
@@ -36,6 +36,7 @@
 #include "cx18-scb.h"
 #include "cx18-mailbox.h"
 #include "cx18-ioctl.h"
+#include "cx18-controls.h"
 #include "tuner-xc2028.h"
 
 #include <media/tveeprom.h>
@@ -157,6 +158,7 @@ MODULE_PARM_DESC(cardtype,
 		 "\t\t\t 7 = Leadtek WinFast PVR2100\n"
 		 "\t\t\t 8 = Leadtek WinFast DVR3100 H\n"
 		 "\t\t\t 9 = GoTView PCI DVD3 Hybrid\n"
+		 "\t\t\t 10 = Hauppauge HVR 1600 (S5H1411)\n"
 		 "\t\t\t 0 = Autodetect (default)\n"
 		 "\t\t\t-1 = Ignore this card\n\t\t");
 MODULE_PARM_DESC(pal, "Set PAL standard: B, G, H, D, K, I, M, N, Nc, 60");
@@ -337,6 +339,7 @@ void cx18_read_eeprom(struct cx18 *cx, struct tveeprom *tv)
 	switch (cx->card->type) {
 	case CX18_CARD_HVR_1600_ESMT:
 	case CX18_CARD_HVR_1600_SAMSUNG:
+	case CX18_CARD_HVR_1600_S5H1411:
 		tveeprom_hauppauge_analog(&c, tv, eedata);
 		break;
 	case CX18_CARD_YUAN_MPC718:
@@ -365,7 +368,25 @@ static void cx18_process_eeprom(struct cx18 *cx)
 	   from the model number. Use the cardtype module option if you
 	   have one of these preproduction models. */
 	switch (tv.model) {
-	case 74000 ... 74999:
+	case 74301: /* Retail models */
+	case 74321:
+	case 74351: /* OEM models */
+	case 74361:
+		/* Digital side is s5h1411/tda18271 */
+		cx->card = cx18_get_card(CX18_CARD_HVR_1600_S5H1411);
+		break;
+	case 74021: /* Retail models */
+	case 74031:
+	case 74041:
+	case 74141:
+	case 74541: /* OEM models */
+	case 74551:
+	case 74591:
+	case 74651:
+	case 74691:
+	case 74751:
+	case 74891:
+		/* Digital side is s5h1409/mxl5005s */
 		cx->card = cx18_get_card(CX18_CARD_HVR_1600_ESMT);
 		break;
 	case 0x718:
@@ -377,7 +398,8 @@ static void cx18_process_eeprom(struct cx18 *cx)
 		CX18_ERR("Invalid EEPROM\n");
 		return;
 	default:
-		CX18_ERR("Unknown model %d, defaulting to HVR-1600\n", tv.model);
+		CX18_ERR("Unknown model %d, defaulting to original HVR-1600 "
+			 "(cardtype=1)\n", tv.model);
 		cx->card = cx18_get_card(CX18_CARD_HVR_1600_ESMT);
 		break;
 	}
@@ -708,15 +730,22 @@ static int __devinit cx18_init_struct1(struct cx18 *cx)
 	cx->open_id = 1;
 
 	/* Initial settings */
-	cx2341x_fill_defaults(&cx->params);
-	cx->temporal_strength = cx->params.video_temporal_filter;
-	cx->spatial_strength = cx->params.video_spatial_filter;
-	cx->filter_mode = cx->params.video_spatial_filter_mode |
-		(cx->params.video_temporal_filter_mode << 1) |
-		(cx->params.video_median_filter_type << 2);
-	cx->params.port = CX2341X_PORT_MEMORY;
-	cx->params.capabilities =
-				CX2341X_CAP_HAS_TS | CX2341X_CAP_HAS_SLICED_VBI;
+	cx->cxhdl.port = CX2341X_PORT_MEMORY;
+	cx->cxhdl.capabilities = CX2341X_CAP_HAS_TS | CX2341X_CAP_HAS_SLICED_VBI;
+	cx->cxhdl.ops = &cx18_cxhdl_ops;
+	cx->cxhdl.func = cx18_api_func;
+	cx->cxhdl.priv = &cx->streams[CX18_ENC_STREAM_TYPE_MPG];
+	ret = cx2341x_handler_init(&cx->cxhdl, 50);
+	if (ret)
+		return ret;
+	cx->v4l2_dev.ctrl_handler = &cx->cxhdl.hdl;
+
+	cx->temporal_strength = cx->cxhdl.video_temporal_filter->cur.val;
+	cx->spatial_strength = cx->cxhdl.video_spatial_filter->cur.val;
+	cx->filter_mode = cx->cxhdl.video_spatial_filter_mode->cur.val |
+		(cx->cxhdl.video_temporal_filter_mode->cur.val << 1) |
+		(cx->cxhdl.video_median_filter_type->cur.val << 2);
+
 	init_waitqueue_head(&cx->cap_w);
 	init_waitqueue_head(&cx->mb_apu_waitq);
 	init_waitqueue_head(&cx->mb_cpu_waitq);
@@ -1028,7 +1057,7 @@ static int __devinit cx18_probe(struct pci_dev *pci_dev,
 	else
 		cx->is_50hz = 1;
 
-	cx->params.video_gop_size = cx->is_60hz ? 15 : 12;
+	cx2341x_handler_set_50hz(&cx->cxhdl, !cx->is_60hz);
 
 	if (cx->options.radio > 0)
 		cx->v4l2_cap |= V4L2_CAP_RADIO;
@@ -1074,7 +1103,6 @@ static int __devinit cx18_probe(struct pci_dev *pci_dev,
 
 	/* Load cx18 submodules (cx18-alsa) */
 	request_modules(cx);
-
 	return 0;
 
 free_streams:
@@ -1257,6 +1285,8 @@ static void cx18_remove(struct pci_dev *pci_dev)
 		for (i = 0; i < CX18_VBI_FRAMES; i++)
 			kfree(cx->vbi.sliced_mpeg_data[i]);
 
+	v4l2_ctrl_handler_free(&cx->av_state.hdl);
+
 	CX18_INFO("Removed %s\n", cx->card_name);
 
 	v4l2_device_unregister(v4l2_dev);
diff --git a/drivers/media/video/cx18/cx18-driver.h b/drivers/media/video/cx18/cx18-driver.h
index 306caac6d3fc..de2457741e26 100644
--- a/drivers/media/video/cx18/cx18-driver.h
+++ b/drivers/media/video/cx18/cx18-driver.h
@@ -85,7 +85,8 @@
 #define CX18_CARD_LEADTEK_PVR2100     6 /* Leadtek WinFast PVR2100 */
 #define CX18_CARD_LEADTEK_DVR3100H    7 /* Leadtek WinFast DVR3100 H */
 #define CX18_CARD_GOTVIEW_PCI_DVD3    8 /* GoTView PCI DVD3 Hybrid */
-#define CX18_CARD_LAST		      8
+#define CX18_CARD_HVR_1600_S5H1411    9 /* Hauppauge HVR 1600 s5h1411/tda18271*/
+#define CX18_CARD_LAST		      9
 
 #define CX18_ENC_STREAM_TYPE_MPG  0
 #define CX18_ENC_STREAM_TYPE_TS   1
@@ -564,7 +565,7 @@ struct cx18 {
 	struct cx18_av_state av_state;
 
 	/* codec settings */
-	struct cx2341x_mpeg_params params;
+	struct cx2341x_handler cxhdl;
 	u32 filter_mode;
 	u32 temporal_strength;
 	u32 spatial_strength;
diff --git a/drivers/media/video/cx18/cx18-dvb.c b/drivers/media/video/cx18/cx18-dvb.c
index f0381d62518d..f41922bd4020 100644
--- a/drivers/media/video/cx18/cx18-dvb.c
+++ b/drivers/media/video/cx18/cx18-dvb.c
@@ -29,6 +29,8 @@
 #include "cx18-gpio.h"
 #include "s5h1409.h"
 #include "mxl5005s.h"
+#include "s5h1411.h"
+#include "tda18271.h"
 #include "zl10353.h"
 
 #include <linux/firmware.h>
@@ -77,6 +79,32 @@ static struct s5h1409_config hauppauge_hvr1600_config = {
 };
 
 /*
+ * CX18_CARD_HVR_1600_S5H1411
+ */
+static struct s5h1411_config hcw_s5h1411_config = {
+	.output_mode   = S5H1411_SERIAL_OUTPUT,
+	.gpio          = S5H1411_GPIO_OFF,
+	.vsb_if        = S5H1411_IF_44000,
+	.qam_if        = S5H1411_IF_4000,
+	.inversion     = S5H1411_INVERSION_ON,
+	.status_mode   = S5H1411_DEMODLOCKING,
+	.mpeg_timing   = S5H1411_MPEGTIMING_CONTINOUS_NONINVERTING_CLOCK,
+};
+
+static struct tda18271_std_map hauppauge_tda18271_std_map = {
+	.atsc_6   = { .if_freq = 5380, .agc_mode = 3, .std = 3,
+		      .if_lvl = 6, .rfagc_top = 0x37 },
+	.qam_6    = { .if_freq = 4000, .agc_mode = 3, .std = 0,
+		      .if_lvl = 6, .rfagc_top = 0x37 },
+};
+
+static struct tda18271_config hauppauge_tda18271_config = {
+	.std_map = &hauppauge_tda18271_std_map,
+	.gate    = TDA18271_GATE_DIGITAL,
+	.output_opt = TDA18271_OUTPUT_LT_OFF,
+};
+
+/*
  * CX18_CARD_LEADTEK_DVR3100H
  */
 /* Information/confirmation of proper config values provided by Terry Wu */
@@ -244,6 +272,7 @@ static int cx18_dvb_start_feed(struct dvb_demux_feed *feed)
 	switch (cx->card->type) {
 	case CX18_CARD_HVR_1600_ESMT:
 	case CX18_CARD_HVR_1600_SAMSUNG:
+	case CX18_CARD_HVR_1600_S5H1411:
 		v = cx18_read_reg(cx, CX18_REG_DMUX_NUM_PORT_0_CONTROL);
 		v |= 0x00400000; /* Serial Mode */
 		v |= 0x00002000; /* Data Length - Byte */
@@ -455,6 +484,15 @@ static int dvb_register(struct cx18_stream *stream)
 			ret = 0;
 		}
 		break;
+	case CX18_CARD_HVR_1600_S5H1411:
+		dvb->fe = dvb_attach(s5h1411_attach,
+				     &hcw_s5h1411_config,
+				     &cx->i2c_adap[0]);
+		if (dvb->fe != NULL)
+			dvb_attach(tda18271_attach, dvb->fe,
+				   0x60, &cx->i2c_adap[0],
+				   &hauppauge_tda18271_config);
+		break;
 	case CX18_CARD_LEADTEK_DVR3100H:
 		dvb->fe = dvb_attach(zl10353_attach,
 				     &leadtek_dvr3100h_demod,
diff --git a/drivers/media/video/cx18/cx18-fileops.c b/drivers/media/video/cx18/cx18-fileops.c
index 9f23b90732f2..98ef33e4326a 100644
--- a/drivers/media/video/cx18/cx18-fileops.c
+++ b/drivers/media/video/cx18/cx18-fileops.c
@@ -160,13 +160,10 @@ EXPORT_SYMBOL(cx18_release_stream);
 static void cx18_dualwatch(struct cx18 *cx)
 {
 	struct v4l2_tuner vt;
-	u32 new_bitmap;
 	u32 new_stereo_mode;
-	const u32 stereo_mask = 0x0300;
 	const u32 dual = 0x0200;
-	u32 h;
 
-	new_stereo_mode = cx->params.audio_properties & stereo_mask;
+	new_stereo_mode = v4l2_ctrl_g_ctrl(cx->cxhdl.audio_mode);
 	memset(&vt, 0, sizeof(vt));
 	cx18_call_all(cx, tuner, g_tuner, &vt);
 	if (vt.audmode == V4L2_TUNER_MODE_LANG1_LANG2 &&
@@ -176,25 +173,10 @@ static void cx18_dualwatch(struct cx18 *cx)
 	if (new_stereo_mode == cx->dualwatch_stereo_mode)
 		return;
 
-	new_bitmap = new_stereo_mode
-			| (cx->params.audio_properties & ~stereo_mask);
-
-	CX18_DEBUG_INFO("dualwatch: change stereo flag from 0x%x to 0x%x. "
-			"new audio_bitmask=0x%ux\n",
-			cx->dualwatch_stereo_mode, new_stereo_mode, new_bitmap);
-
-	h = cx18_find_handle(cx);
-	if (h == CX18_INVALID_TASK_HANDLE) {
-		CX18_DEBUG_INFO("dualwatch: can't find valid task handle\n");
-		return;
-	}
-
-	if (cx18_vapi(cx,
-		      CX18_CPU_SET_AUDIO_PARAMETERS, 2, h, new_bitmap) == 0) {
-		cx->dualwatch_stereo_mode = new_stereo_mode;
-		return;
-	}
-	CX18_DEBUG_INFO("dualwatch: changing stereo flag failed\n");
+	CX18_DEBUG_INFO("dualwatch: change stereo flag from 0x%x to 0x%x.\n",
+			   cx->dualwatch_stereo_mode, new_stereo_mode);
+	if (v4l2_ctrl_s_ctrl(cx->cxhdl.audio_mode, new_stereo_mode))
+		CX18_DEBUG_INFO("dualwatch: changing stereo flag failed\n");
 }
 
 
@@ -724,8 +706,8 @@ int cx18_v4l2_close(struct file *filp)
 		if (atomic_read(&cx->ana_capturing) > 0) {
 			/* Undo video mute */
 			cx18_vapi(cx, CX18_CPU_SET_VIDEO_MUTE, 2, s->handle,
-				cx->params.video_mute |
-					(cx->params.video_mute_yuv << 8));
+			    (v4l2_ctrl_g_ctrl(cx->cxhdl.video_mute) |
+			    (v4l2_ctrl_g_ctrl(cx->cxhdl.video_mute_yuv) << 8)));
 		}
 		/* Done! Unmute and continue. */
 		cx18_unmute(cx);
diff --git a/drivers/media/video/cx18/cx18-ioctl.c b/drivers/media/video/cx18/cx18-ioctl.c
index 7150195740dc..36b018c943e5 100644
--- a/drivers/media/video/cx18/cx18-ioctl.c
+++ b/drivers/media/video/cx18/cx18-ioctl.c
@@ -152,8 +152,8 @@ static int cx18_g_fmt_vid_cap(struct file *file, void *fh,
 	struct cx18 *cx = id->cx;
 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
 
-	pixfmt->width = cx->params.width;
-	pixfmt->height = cx->params.height;
+	pixfmt->width = cx->cxhdl.width;
+	pixfmt->height = cx->cxhdl.height;
 	pixfmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
 	pixfmt->field = V4L2_FIELD_INTERLACED;
 	pixfmt->priv = 0;
@@ -287,14 +287,14 @@ static int cx18_s_fmt_vid_cap(struct file *file, void *fh,
 	w = fmt->fmt.pix.width;
 	h = fmt->fmt.pix.height;
 
-	if (cx->params.width == w && cx->params.height == h)
+	if (cx->cxhdl.width == w && cx->cxhdl.height == h)
 		return 0;
 
 	if (atomic_read(&cx->ana_capturing) > 0)
 		return -EBUSY;
 
-	mbus_fmt.width = cx->params.width = w;
-	mbus_fmt.height = cx->params.height = h;
+	mbus_fmt.width = cx->cxhdl.width = w;
+	mbus_fmt.height = cx->cxhdl.height = h;
 	mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
 	v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &mbus_fmt);
 	return cx18_g_fmt_vid_cap(file, fh, fmt);
@@ -696,9 +696,10 @@ int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std)
 
 	cx->std = *std;
 	cx->is_60hz = (*std & V4L2_STD_525_60) ? 1 : 0;
-	cx->params.is_50hz = cx->is_50hz = !cx->is_60hz;
-	cx->params.width = 720;
-	cx->params.height = cx->is_50hz ? 576 : 480;
+	cx->is_50hz = !cx->is_60hz;
+	cx2341x_handler_set_50hz(&cx->cxhdl, cx->is_50hz);
+	cx->cxhdl.width = 720;
+	cx->cxhdl.height = cx->is_50hz ? 576 : 480;
 	cx->vbi.count = cx->is_50hz ? 18 : 12;
 	cx->vbi.start[0] = cx->is_50hz ? 6 : 10;
 	cx->vbi.start[1] = cx->is_50hz ? 318 : 273;
@@ -1035,7 +1036,7 @@ static int cx18_log_status(struct file *file, void *fh)
 	mutex_unlock(&cx->gpio_lock);
 	CX18_INFO("Tuner: %s\n",
 		test_bit(CX18_F_I_RADIO_USER, &cx->i_flags) ?  "Radio" : "TV");
-	cx2341x_log_status(&cx->params, cx->v4l2_dev.name);
+	v4l2_ctrl_handler_log_status(&cx->cxhdl.hdl, cx->v4l2_dev.name);
 	CX18_INFO("Status flags: 0x%08lx\n", cx->i_flags);
 	for (i = 0; i < CX18_MAX_STREAMS; i++) {
 		struct cx18_stream *s = &cx->streams[i];
@@ -1136,11 +1137,6 @@ static const struct v4l2_ioctl_ops cx18_ioctl_ops = {
 	.vidioc_s_register              = cx18_s_register,
 #endif
 	.vidioc_default                 = cx18_default,
-	.vidioc_queryctrl               = cx18_queryctrl,
-	.vidioc_querymenu               = cx18_querymenu,
-	.vidioc_g_ext_ctrls             = cx18_g_ext_ctrls,
-	.vidioc_s_ext_ctrls             = cx18_s_ext_ctrls,
-	.vidioc_try_ext_ctrls           = cx18_try_ext_ctrls,
 };
 
 void cx18_set_funcs(struct video_device *vdev)
diff --git a/drivers/media/video/cx18/cx18-mailbox.c b/drivers/media/video/cx18/cx18-mailbox.c
index c545f3beef78..9605d54bd083 100644
--- a/drivers/media/video/cx18/cx18-mailbox.c
+++ b/drivers/media/video/cx18/cx18-mailbox.c
@@ -716,9 +716,8 @@ static int cx18_set_filter_param(struct cx18_stream *s)
 int cx18_api_func(void *priv, u32 cmd, int in, int out,
 		u32 data[CX2341X_MBOX_MAX_DATA])
 {
-	struct cx18_api_func_private *api_priv = priv;
-	struct cx18 *cx = api_priv->cx;
-	struct cx18_stream *s = api_priv->s;
+	struct cx18_stream *s = priv;
+	struct cx18 *cx = s->cx;
 
 	switch (cmd) {
 	case CX2341X_ENC_SET_OUTPUT_PORT:
diff --git a/drivers/media/video/cx18/cx18-mailbox.h b/drivers/media/video/cx18/cx18-mailbox.h
index 077952fcbcca..05fe6bdbe062 100644
--- a/drivers/media/video/cx18/cx18-mailbox.h
+++ b/drivers/media/video/cx18/cx18-mailbox.h
@@ -81,11 +81,6 @@ struct cx18_mailbox {
 
 struct cx18_stream;
 
-struct cx18_api_func_private {
-	struct cx18 *cx;
-	struct cx18_stream *s;
-};
-
 int cx18_api(struct cx18 *cx, u32 cmd, int args, u32 data[]);
 int cx18_vapi_result(struct cx18 *cx, u32 data[MAX_MB_ARGUMENTS], u32 cmd,
 		int args, ...);
diff --git a/drivers/media/video/cx18/cx18-streams.c b/drivers/media/video/cx18/cx18-streams.c
index 94f5d7967c5c..2d248560770e 100644
--- a/drivers/media/video/cx18/cx18-streams.c
+++ b/drivers/media/video/cx18/cx18-streams.c
@@ -572,7 +572,7 @@ static void cx18_stream_configure_mdls(struct cx18_stream *s)
 		 * Set the MDL size to the exact size needed for one frame.
 		 * Use enough buffers per MDL to cover the MDL size
 		 */
-		s->mdl_size = 720 * s->cx->params.height * 3 / 2;
+		s->mdl_size = 720 * s->cx->cxhdl.height * 3 / 2;
 		s->bufs_per_mdl = s->mdl_size / s->buf_size;
 		if (s->mdl_size % s->buf_size)
 			s->bufs_per_mdl++;
@@ -607,7 +607,6 @@ int cx18_start_v4l2_encode_stream(struct cx18_stream *s)
 	u32 data[MAX_MB_ARGUMENTS];
 	struct cx18 *cx = s->cx;
 	int captype = 0;
-	struct cx18_api_func_private priv;
 	struct cx18_stream *s_idx;
 
 	if (!cx18_stream_enabled(s))
@@ -620,7 +619,7 @@ int cx18_start_v4l2_encode_stream(struct cx18_stream *s)
 		captype = CAPTURE_CHANNEL_TYPE_MPEG;
 		cx->mpg_data_received = cx->vbi_data_inserted = 0;
 		cx->dualwatch_jiffies = jiffies;
-		cx->dualwatch_stereo_mode = cx->params.audio_properties & 0x300;
+		cx->dualwatch_stereo_mode = v4l2_ctrl_g_ctrl(cx->cxhdl.audio_mode);
 		cx->search_pack_header = 0;
 		break;
 
@@ -710,21 +709,21 @@ int cx18_start_v4l2_encode_stream(struct cx18_stream *s)
 				 s->handle, cx18_stream_enabled(s_idx) ? 7 : 0);
 
 		/* Call out to the common CX2341x API setup for user controls */
-		priv.cx = cx;
-		priv.s = s;
-		cx2341x_update(&priv, cx18_api_func, NULL, &cx->params);
+		cx->cxhdl.priv = s;
+		cx2341x_handler_setup(&cx->cxhdl);
 
 		/*
 		 * When starting a capture and we're set for radio,
 		 * ensure the video is muted, despite the user control.
 		 */
-		if (!cx->params.video_mute &&
+		if (!cx->cxhdl.video_mute &&
 		    test_bit(CX18_F_I_RADIO_USER, &cx->i_flags))
 			cx18_vapi(cx, CX18_CPU_SET_VIDEO_MUTE, 2, s->handle,
-				  (cx->params.video_mute_yuv << 8) | 1);
+			  (v4l2_ctrl_g_ctrl(cx->cxhdl.video_mute_yuv) << 8) | 1);
 	}
 
 	if (atomic_read(&cx->tot_capturing) == 0) {
+		cx2341x_handler_set_busy(&cx->cxhdl, 1);
 		clear_bit(CX18_F_I_EOS, &cx->i_flags);
 		cx18_write_reg(cx, 7, CX18_DSP0_INTERRUPT_MASK);
 	}
@@ -826,6 +825,7 @@ int cx18_stop_v4l2_encode_stream(struct cx18_stream *s, int gop_end)
 	if (atomic_read(&cx->tot_capturing) > 0)
 		return 0;
 
+	cx2341x_handler_set_busy(&cx->cxhdl, 0);
 	cx18_write_reg(cx, 5, CX18_DSP0_INTERRUPT_MASK);
 	wake_up(&s->waitq);
 
diff --git a/drivers/media/video/cx18/cx23418.h b/drivers/media/video/cx18/cx23418.h
index 7e40035028d2..935f557acbd0 100644
--- a/drivers/media/video/cx18/cx23418.h
+++ b/drivers/media/video/cx18/cx23418.h
@@ -477,7 +477,7 @@
 /* The are no buffers ready. Try again soon! */
 #define CXERR_NODATA_AGAIN      0x00001E
 
-/* The stream is stopping. Function not alllowed now! */
+/* The stream is stopping. Function not allowed now! */
 #define CXERR_STOPPING_STATUS   0x00001F
 
 /* Trying to access hardware when the power is turned OFF */
diff --git a/drivers/media/video/cx231xx/cx231xx-417.c b/drivers/media/video/cx231xx/cx231xx-417.c
index fc9526a5b746..f8f0e59cd583 100644
--- a/drivers/media/video/cx231xx/cx231xx-417.c
+++ b/drivers/media/video/cx231xx/cx231xx-417.c
@@ -942,13 +942,13 @@ static int cx231xx_load_firmware(struct cx231xx *dev)
 
 	p_current_fw = vmalloc(1884180 * 4);
 	p_fw = p_current_fw;
-	if (p_current_fw == 0) {
+	if (p_current_fw == NULL) {
 		dprintk(2, "FAIL!!!\n");
 		return -1;
 	}
 
 	p_buffer = vmalloc(4096);
-	if (p_buffer == 0) {
+	if (p_buffer == NULL) {
 		dprintk(2, "FAIL!!!\n");
 		return -1;
 	}
diff --git a/drivers/media/video/cx231xx/cx231xx-avcore.c b/drivers/media/video/cx231xx/cx231xx-avcore.c
index c53e97295a0d..62843d39817c 100644
--- a/drivers/media/video/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/video/cx231xx/cx231xx-avcore.c
@@ -759,11 +759,8 @@ int cx231xx_set_decoder_video_input(struct cx231xx *dev,
 	case CX231XX_VMUX_TELEVISION:
 	case CX231XX_VMUX_CABLE:
 	default:
-		switch (dev->model) {
-		case CX231XX_BOARD_CNXT_CARRAERA:
-		case CX231XX_BOARD_CNXT_RDE_250:
-		case CX231XX_BOARD_CNXT_SHELBY:
-		case CX231XX_BOARD_CNXT_RDU_250:
+		/* TODO: Test if this is also needed for xc2028/xc3028 */
+		if (dev->board.tuner_type == TUNER_XC5000) {
 			/* Disable the use of  DIF   */
 
 			status = vid_blk_read_word(dev, AFE_CTRL, &value);
@@ -820,8 +817,7 @@ int cx231xx_set_decoder_video_input(struct cx231xx *dev,
 				MODE_CTRL, FLD_INPUT_MODE,
 				cx231xx_set_field(FLD_INPUT_MODE,
 						INPUT_MODE_CVBS_0));
-			break;
-		default:
+		} else {
 			/* Enable the DIF for the tuner */
 
 			/* Reinitialize the DIF */
@@ -1275,6 +1271,8 @@ int cx231xx_enable_i2c_port_3(struct cx231xx *dev, bool is_port_3)
 	int status = 0;
 	bool current_is_port_3;
 
+	if (dev->board.dont_use_port_3)
+		is_port_3 = false;
 	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER,
 				       PWR_CTL_EN, value, 4);
 	if (status < 0)
@@ -2550,7 +2548,7 @@ int cx231xx_initialize_stream_xfer(struct cx231xx *dev, u32 media_type)
 		case 4:	/* ts1 */
 			cx231xx_info("%s: set ts1 registers", __func__);
 
-		if (dev->model == CX231XX_BOARD_CNXT_VIDEO_GRABBER) {
+		if (dev->board.has_417) {
 			cx231xx_info(" MPEG\n");
 			value &= 0xFFFFFFFC;
 			value |= 0x3;
diff --git a/drivers/media/video/cx231xx/cx231xx-cards.c b/drivers/media/video/cx231xx/cx231xx-cards.c
index 588f3e8f028b..f49230d170e6 100644
--- a/drivers/media/video/cx231xx/cx231xx-cards.c
+++ b/drivers/media/video/cx231xx/cx231xx-cards.c
@@ -261,6 +261,9 @@ struct cx231xx_board cx231xx_boards[] = {
 		.agc_analog_digital_select_gpio = 0x1c,
 		.gpio_pin_status_mask = 0x4001000,
 		.norm = V4L2_STD_PAL,
+		.no_alt_vanc = 1,
+		.external_av = 1,
+		.has_417 = 1,
 
 		.input = {{
 				.type = CX231XX_VMUX_COMPOSITE1,
@@ -357,19 +360,19 @@ struct cx231xx_board cx231xx_boards[] = {
 			.type = CX231XX_VMUX_TELEVISION,
 			.vmux = CX231XX_VIN_3_1,
 			.amux = CX231XX_AMUX_VIDEO,
-			.gpio = 0,
+			.gpio = NULL,
 		}, {
 			.type = CX231XX_VMUX_COMPOSITE1,
 			.vmux = CX231XX_VIN_2_1,
 			.amux = CX231XX_AMUX_LINE_IN,
-			.gpio = 0,
+			.gpio = NULL,
 		}, {
 			.type = CX231XX_VMUX_SVIDEO,
 			.vmux = CX231XX_VIN_1_1 |
 				(CX231XX_VIN_1_2 << 8) |
 				CX25840_SVIDEO_ON,
 			.amux = CX231XX_AMUX_LINE_IN,
-			.gpio = 0,
+			.gpio = NULL,
 		} },
 	},
 	[CX231XX_BOARD_HAUPPAUGE_USBLIVE2] = {
@@ -382,18 +385,20 @@ struct cx231xx_board cx231xx_boards[] = {
 		.agc_analog_digital_select_gpio = 0x0c,
 		.gpio_pin_status_mask = 0x4001000,
 		.norm = V4L2_STD_NTSC,
+		.no_alt_vanc = 1,
+		.external_av = 1,
 		.input = {{
 			.type = CX231XX_VMUX_COMPOSITE1,
 			.vmux = CX231XX_VIN_2_1,
 			.amux = CX231XX_AMUX_LINE_IN,
-			.gpio = 0,
+			.gpio = NULL,
 		}, {
 			.type = CX231XX_VMUX_SVIDEO,
 			.vmux = CX231XX_VIN_1_1 |
 				(CX231XX_VIN_1_2 << 8) |
 				CX25840_SVIDEO_ON,
 			.amux = CX231XX_AMUX_LINE_IN,
-			.gpio = 0,
+			.gpio = NULL,
 		} },
 	},
 	[CX231XX_BOARD_PV_PLAYTV_USB_HYBRID] = {
@@ -420,21 +425,50 @@ struct cx231xx_board cx231xx_boards[] = {
 			.type = CX231XX_VMUX_TELEVISION,
 			.vmux = CX231XX_VIN_3_1,
 			.amux = CX231XX_AMUX_VIDEO,
-			.gpio = 0,
+			.gpio = NULL,
 		}, {
 			.type = CX231XX_VMUX_COMPOSITE1,
 			.vmux = CX231XX_VIN_2_1,
 			.amux = CX231XX_AMUX_LINE_IN,
-			.gpio = 0,
+			.gpio = NULL,
 		}, {
 			.type = CX231XX_VMUX_SVIDEO,
 			.vmux = CX231XX_VIN_1_1 |
 				(CX231XX_VIN_1_2 << 8) |
 				CX25840_SVIDEO_ON,
 			.amux = CX231XX_AMUX_LINE_IN,
-			.gpio = 0,
+			.gpio = NULL,
 		} },
 	},
+	[CX231XX_BOARD_PV_XCAPTURE_USB] = {
+		.name = "Pixelview Xcapture USB",
+		.tuner_type = TUNER_ABSENT,
+		.decoder = CX231XX_AVDECODER,
+		.output_mode = OUT_MODE_VIP11,
+		.demod_xfer_mode = 0,
+		.ctl_pin_status_mask = 0xFFFFFFC4,
+		.agc_analog_digital_select_gpio = 0x0c,
+		.gpio_pin_status_mask = 0x4001000,
+		.norm = V4L2_STD_NTSC,
+		.no_alt_vanc = 1,
+		.external_av = 1,
+		.dont_use_port_3 = 1,
+
+		.input = {{
+				.type = CX231XX_VMUX_COMPOSITE1,
+				.vmux = CX231XX_VIN_2_1,
+				.amux = CX231XX_AMUX_LINE_IN,
+				.gpio = NULL,
+			}, {
+				.type = CX231XX_VMUX_SVIDEO,
+				.vmux = CX231XX_VIN_1_1 |
+					(CX231XX_VIN_1_2 << 8) |
+					CX25840_SVIDEO_ON,
+				.amux = CX231XX_AMUX_LINE_IN,
+				.gpio = NULL,
+			}
+		},
+	},
 };
 const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
@@ -464,6 +498,8 @@ struct usb_device_id cx231xx_id_table[] = {
 	 .driver_info = CX231XX_BOARD_HAUPPAUGE_USBLIVE2},
 	{USB_DEVICE_VER(USB_VID_PIXELVIEW, USB_PID_PIXELVIEW_SBTVD, 0x4000, 0x4001),
 	 .driver_info = CX231XX_BOARD_PV_PLAYTV_USB_HYBRID},
+	{USB_DEVICE(USB_VID_PIXELVIEW, 0x5014),
+	 .driver_info = CX231XX_BOARD_PV_XCAPTURE_USB},
 	{},
 };
 
@@ -772,7 +808,7 @@ static int cx231xx_init_dev(struct cx231xx **devhandle, struct usb_device *udev,
 	/* Reset other chips required if they are tied up with GPIO pins */
 	cx231xx_add_into_devlist(dev);
 
-	if (dev->model == CX231XX_BOARD_CNXT_VIDEO_GRABBER) {
+	if (dev->board.has_417) {
 		printk(KERN_INFO "attach 417 %d\n", dev->model);
 		if (cx231xx_417_register(dev) < 0) {
 			printk(KERN_ERR
@@ -844,110 +880,110 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 	udev = usb_get_dev(interface_to_usbdev(interface));
 	ifnum = interface->altsetting[0].desc.bInterfaceNumber;
 
-	if (ifnum == 1) {
-		/*
-		 * Interface number 0 - IR interface
-		 */
-		/* Check to see next free device and mark as used */
-		nr = find_first_zero_bit(&cx231xx_devused, CX231XX_MAXBOARDS);
-		cx231xx_devused |= 1 << nr;
-
-		if (nr >= CX231XX_MAXBOARDS) {
-			cx231xx_err(DRIVER_NAME
-		 ": Supports only %i cx231xx boards.\n", CX231XX_MAXBOARDS);
-			cx231xx_devused &= ~(1 << nr);
-			return -ENOMEM;
-		}
-
-		/* allocate memory for our device state and initialize it */
-		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-		if (dev == NULL) {
-			cx231xx_err(DRIVER_NAME ": out of memory!\n");
-			cx231xx_devused &= ~(1 << nr);
-			return -ENOMEM;
-		}
-
-		snprintf(dev->name, 29, "cx231xx #%d", nr);
-		dev->devno = nr;
-		dev->model = id->driver_info;
-		dev->video_mode.alt = -1;
-		dev->interface_count++;
-
-		/* reset gpio dir and value */
-		dev->gpio_dir = 0;
-		dev->gpio_val = 0;
-		dev->xc_fw_load_done = 0;
-		dev->has_alsa_audio = 1;
-		dev->power_mode = -1;
-		atomic_set(&dev->devlist_count, 0);
-
-		/* 0 - vbi ; 1 -sliced cc mode */
-		dev->vbi_or_sliced_cc_mode = 0;
-
-		/* get maximum no.of IAD interfaces */
-		assoc_desc = udev->actconfig->intf_assoc[0];
-		dev->max_iad_interface_count = assoc_desc->bInterfaceCount;
-
-		/* init CIR module TBD */
+	/*
+	 * Interface number 0 - IR interface (handled by mceusb driver)
+	 * Interface number 1 - AV interface (handled by this driver)
+	 */
+	if (ifnum != 1)
+		return -ENODEV;
 
-		/* store the current interface */
-		lif = interface;
+	/* Check to see next free device and mark as used */
+	nr = find_first_zero_bit(&cx231xx_devused, CX231XX_MAXBOARDS);
+	cx231xx_devused |= 1 << nr;
 
-		/*mode_tv: digital=1 or analog=0*/
-		dev->mode_tv = 0;
+	if (nr >= CX231XX_MAXBOARDS) {
+		cx231xx_err(DRIVER_NAME
+		 ": Supports only %i cx231xx boards.\n", CX231XX_MAXBOARDS);
+		cx231xx_devused &= ~(1 << nr);
+		return -ENOMEM;
+	}
 
-		dev->USE_ISO = transfer_mode;
+	/* allocate memory for our device state and initialize it */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		cx231xx_err(DRIVER_NAME ": out of memory!\n");
+		cx231xx_devused &= ~(1 << nr);
+		return -ENOMEM;
+	}
 
-		switch (udev->speed) {
-		case USB_SPEED_LOW:
-			speed = "1.5";
-			break;
-		case USB_SPEED_UNKNOWN:
-		case USB_SPEED_FULL:
-			speed = "12";
-			break;
-		case USB_SPEED_HIGH:
-			speed = "480";
-			break;
-		default:
-			speed = "unknown";
-		}
+	snprintf(dev->name, 29, "cx231xx #%d", nr);
+	dev->devno = nr;
+	dev->model = id->driver_info;
+	dev->video_mode.alt = -1;
+
+	dev->interface_count++;
+	/* reset gpio dir and value */
+	dev->gpio_dir = 0;
+	dev->gpio_val = 0;
+	dev->xc_fw_load_done = 0;
+	dev->has_alsa_audio = 1;
+	dev->power_mode = -1;
+	atomic_set(&dev->devlist_count, 0);
+
+	/* 0 - vbi ; 1 -sliced cc mode */
+	dev->vbi_or_sliced_cc_mode = 0;
+
+	/* get maximum no.of IAD interfaces */
+	assoc_desc = udev->actconfig->intf_assoc[0];
+	dev->max_iad_interface_count = assoc_desc->bInterfaceCount;
+
+	/* init CIR module TBD */
+
+	/* store the current interface */
+	lif = interface;
+
+	/*mode_tv: digital=1 or analog=0*/
+	dev->mode_tv = 0;
+
+	dev->USE_ISO = transfer_mode;
+
+	switch (udev->speed) {
+	case USB_SPEED_LOW:
+		speed = "1.5";
+		break;
+	case USB_SPEED_UNKNOWN:
+	case USB_SPEED_FULL:
+		speed = "12";
+		break;
+	case USB_SPEED_HIGH:
+		speed = "480";
+		break;
+	default:
+		speed = "unknown";
+	}
 
-		if (udev->manufacturer)
-			strlcpy(descr, udev->manufacturer, sizeof(descr));
+	if (udev->manufacturer)
+		strlcpy(descr, udev->manufacturer, sizeof(descr));
 
-		if (udev->product) {
-			if (*descr)
-				strlcat(descr, " ", sizeof(descr));
-			strlcat(descr, udev->product, sizeof(descr));
-		}
+	if (udev->product) {
 		if (*descr)
 			strlcat(descr, " ", sizeof(descr));
-
-		cx231xx_info("New device %s@ %s Mbps "
-		     "(%04x:%04x) with %d interfaces\n",
-		     descr,
-		     speed,
-		     le16_to_cpu(udev->descriptor.idVendor),
-		     le16_to_cpu(udev->descriptor.idProduct),
-		     dev->max_iad_interface_count);
-
-		/* store the interface 0 back */
-		lif = udev->actconfig->interface[0];
-
-		/* increment interface count */
-		dev->interface_count++;
-
-		/* get device number */
-		nr = dev->devno;
-
-		assoc_desc = udev->actconfig->intf_assoc[0];
-		if (assoc_desc->bFirstInterface != ifnum) {
-			cx231xx_err(DRIVER_NAME ": Not found "
-				    "matching IAD interface\n");
-			return -ENODEV;
-		}
-	} else {
+		strlcat(descr, udev->product, sizeof(descr));
+	}
+	if (*descr)
+		strlcat(descr, " ", sizeof(descr));
+
+	cx231xx_info("New device %s@ %s Mbps "
+	     "(%04x:%04x) with %d interfaces\n",
+	     descr,
+	     speed,
+	     le16_to_cpu(udev->descriptor.idVendor),
+	     le16_to_cpu(udev->descriptor.idProduct),
+	     dev->max_iad_interface_count);
+
+	/* store the interface 0 back */
+	lif = udev->actconfig->interface[0];
+
+	/* increment interface count */
+	dev->interface_count++;
+
+	/* get device number */
+	nr = dev->devno;
+
+	assoc_desc = udev->actconfig->intf_assoc[0];
+	if (assoc_desc->bFirstInterface != ifnum) {
+		cx231xx_err(DRIVER_NAME ": Not found "
+			    "matching IAD interface\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/media/video/cx231xx/cx231xx-core.c b/drivers/media/video/cx231xx/cx231xx-core.c
index 7d62d58617f5..abe500feb7dd 100644
--- a/drivers/media/video/cx231xx/cx231xx-core.c
+++ b/drivers/media/video/cx231xx/cx231xx-core.c
@@ -571,6 +571,8 @@ int cx231xx_set_alt_setting(struct cx231xx *dev, u8 index, u8 alt)
 							     alt];
 		break;
 	case INDEX_VANC:
+		if (dev->board.no_alt_vanc)
+			return 0;
 		usb_interface_index =
 		    dev->current_pcb_config.hs_config_info[0].interface_info.
 		    vanc_index + 1;
@@ -600,8 +602,7 @@ int cx231xx_set_alt_setting(struct cx231xx *dev, u8 index, u8 alt)
 		usb_interface_index, alt);
 		/*To workaround error number=-71 on EP0 for videograbber,
 		 need add following codes.*/
-		if (dev->model != CX231XX_BOARD_CNXT_VIDEO_GRABBER &&
-		    dev->model != CX231XX_BOARD_HAUPPAUGE_USBLIVE2)
+		if (dev->board.no_alt_vanc)
 			return -1;
 	}
 
@@ -1301,8 +1302,7 @@ int cx231xx_dev_init(struct cx231xx *dev)
 	/* init hardware */
 	/* Note : with out calling set power mode function,
 	afe can not be set up correctly */
-	if (dev->model == CX231XX_BOARD_CNXT_VIDEO_GRABBER ||
-	    dev->model == CX231XX_BOARD_HAUPPAUGE_USBLIVE2) {
+	if (dev->board.external_av) {
 		errCode = cx231xx_set_power_mode(dev,
 				 POLARIS_AVMODE_ENXTERNAL_AV);
 		if (errCode < 0) {
@@ -1322,11 +1322,9 @@ int cx231xx_dev_init(struct cx231xx *dev)
 		}
 	}
 
-	/* reset the Tuner */
-	if ((dev->model == CX231XX_BOARD_CNXT_CARRAERA) ||
-		(dev->model == CX231XX_BOARD_CNXT_RDE_250) ||
-		(dev->model == CX231XX_BOARD_CNXT_SHELBY) ||
-		(dev->model == CX231XX_BOARD_CNXT_RDU_250))
+	/* reset the Tuner, if it is a Xceive tuner */
+	if ((dev->board.tuner_type == TUNER_XC5000) ||
+	    (dev->board.tuner_type == TUNER_XC2028))
 			cx231xx_gpio_set(dev, dev->board.tuner_gpio);
 
 	/* initialize Colibri block */
diff --git a/drivers/media/video/cx231xx/cx231xx-i2c.c b/drivers/media/video/cx231xx/cx231xx-i2c.c
index 835670623dfb..925f3a04e53c 100644
--- a/drivers/media/video/cx231xx/cx231xx-i2c.c
+++ b/drivers/media/video/cx231xx/cx231xx-i2c.c
@@ -54,6 +54,21 @@ do {							\
       } 						\
 } while (0)
 
+static inline bool is_tuner(struct cx231xx *dev, struct cx231xx_i2c *bus,
+			const struct i2c_msg *msg, int tuner_type)
+{
+	if (bus->nr != dev->board.tuner_i2c_master)
+		return false;
+
+	if (msg->addr != dev->board.tuner_addr)
+		return false;
+
+	if (dev->tuner_type != tuner_type)
+		return false;
+
+	return true;
+}
+
 /*
  * cx231xx_i2c_send_bytes()
  */
@@ -71,9 +86,7 @@ int cx231xx_i2c_send_bytes(struct i2c_adapter *i2c_adap,
 	u16 saddr = 0;
 	u8 need_gpio = 0;
 
-	if ((bus->nr == 1) && (msg->addr == 0x61)
-	    && (dev->tuner_type == TUNER_XC5000)) {
-
+	if (is_tuner(dev, bus, msg, TUNER_XC5000)) {
 		size = msg->len;
 
 		if (size == 2) {	/* register write sub addr */
@@ -180,9 +193,7 @@ static int cx231xx_i2c_recv_bytes(struct i2c_adapter *i2c_adap,
 	u16 saddr = 0;
 	u8 need_gpio = 0;
 
-	if ((bus->nr == 1) && (msg->addr == 0x61)
-	    && dev->tuner_type == TUNER_XC5000) {
-
+	if (is_tuner(dev, bus, msg, TUNER_XC5000)) {
 		if (msg->len == 2)
 			saddr = msg->buf[0] << 8 | msg->buf[1];
 		else if (msg->len == 1)
@@ -274,9 +285,7 @@ static int cx231xx_i2c_recv_bytes_with_saddr(struct i2c_adapter *i2c_adap,
 	else if (msg1->len == 1)
 		saddr = msg1->buf[0];
 
-	if ((bus->nr == 1) && (msg2->addr == 0x61)
-	    && dev->tuner_type == TUNER_XC5000) {
-
+	if (is_tuner(dev, bus, msg2, TUNER_XC5000)) {
 		if ((msg2->len < 16)) {
 
 			dprintk1(1,
@@ -454,8 +463,8 @@ static char *i2c_devs[128] = {
 	[0x32 >> 1] = "GeminiIII",
 	[0x02 >> 1] = "Aquarius",
 	[0xa0 >> 1] = "eeprom",
-	[0xc0 >> 1] = "tuner/XC3028",
-	[0xc2 >> 1] = "tuner/XC5000",
+	[0xc0 >> 1] = "tuner",
+	[0xc2 >> 1] = "tuner",
 };
 
 /*
diff --git a/drivers/media/video/cx231xx/cx231xx-video.c b/drivers/media/video/cx231xx/cx231xx-video.c
index 7e3e8c4f19b7..ffd5af914c44 100644
--- a/drivers/media/video/cx231xx/cx231xx-video.c
+++ b/drivers/media/video/cx231xx/cx231xx-video.c
@@ -2190,8 +2190,7 @@ static int cx231xx_v4l2_open(struct file *filp)
 		dev->height = norm_maxh(dev);
 
 		/* Power up in Analog TV mode */
-		if (dev->model == CX231XX_BOARD_CNXT_VIDEO_GRABBER ||
-		    dev->model == CX231XX_BOARD_HAUPPAUGE_USBLIVE2)
+		if (dev->board.external_av)
 			cx231xx_set_power_mode(dev,
 				 POLARIS_AVMODE_ENXTERNAL_AV);
 		else
@@ -2231,9 +2230,7 @@ static int cx231xx_v4l2_open(struct file *filp)
 	if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
 		/* Set the required alternate setting  VBI interface works in
 		   Bulk mode only */
-		if (dev->model != CX231XX_BOARD_CNXT_VIDEO_GRABBER &&
-		    dev->model != CX231XX_BOARD_HAUPPAUGE_USBLIVE2)
-			cx231xx_set_alt_setting(dev, INDEX_VANC, 0);
+		cx231xx_set_alt_setting(dev, INDEX_VANC, 0);
 
 		videobuf_queue_vmalloc_init(&fh->vb_vidq, &cx231xx_vbi_qops,
 					    NULL, &dev->vbi_mode.slock,
@@ -2275,7 +2272,7 @@ void cx231xx_release_analog_resources(struct cx231xx *dev)
 		cx231xx_info("V4L2 device %s deregistered\n",
 			     video_device_node_name(dev->vdev));
 
-		if (dev->model == CX231XX_BOARD_CNXT_VIDEO_GRABBER)
+		if (dev->board.has_417)
 			cx231xx_417_unregister(dev);
 
 		if (video_is_registered(dev->vdev))
@@ -2302,10 +2299,13 @@ static int cx231xx_v4l2_close(struct file *filp)
 	if (res_check(fh))
 		res_free(fh);
 
-	/*To workaround error number=-71 on EP0 for VideoGrabber,
-		 need exclude following.*/
-	if (dev->model != CX231XX_BOARD_CNXT_VIDEO_GRABBER &&
-	    dev->model != CX231XX_BOARD_HAUPPAUGE_USBLIVE2)
+	/*
+	 * To workaround error number=-71 on EP0 for VideoGrabber,
+	 *	 need exclude following.
+	 * FIXME: It is probably safe to remove most of these, as we're
+	 * now avoiding the alternate setting for INDEX_VANC
+	 */
+	if (!dev->board.no_alt_vanc)
 		if (fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
 			videobuf_stop(&fh->vb_vidq);
 			videobuf_mmap_free(&fh->vb_vidq);
diff --git a/drivers/media/video/cx231xx/cx231xx.h b/drivers/media/video/cx231xx/cx231xx.h
index 72bbea2bcd56..bd4a9cf29577 100644
--- a/drivers/media/video/cx231xx/cx231xx.h
+++ b/drivers/media/video/cx231xx/cx231xx.h
@@ -64,6 +64,7 @@
 #define CX231XX_BOARD_HAUPPAUGE_EXETER  8
 #define CX231XX_BOARD_HAUPPAUGE_USBLIVE2 9
 #define CX231XX_BOARD_PV_PLAYTV_USB_HYBRID 10
+#define CX231XX_BOARD_PV_XCAPTURE_USB 11
 
 /* Limits minimum and default number of buffers */
 #define CX231XX_MIN_BUF                 4
@@ -353,7 +354,11 @@ struct cx231xx_board {
 
 	unsigned int max_range_640_480:1;
 	unsigned int has_dvb:1;
+	unsigned int has_417:1;
 	unsigned int valid:1;
+	unsigned int no_alt_vanc:1;
+	unsigned int external_av:1;
+	unsigned int dont_use_port_3:1;
 
 	unsigned char xclk, i2c_speed;
 
@@ -464,7 +469,7 @@ struct cx231xx_fh {
 #define I2C_STOP                0x0
 /* 1-- do not transmit STOP at end of transaction */
 #define I2C_NOSTOP              0x1
-/* 1--alllow slave to insert clock wait states */
+/* 1--allow slave to insert clock wait states */
 #define I2C_SYNC                0x1
 
 struct cx231xx_i2c {
diff --git a/drivers/media/video/cx23885/Kconfig b/drivers/media/video/cx23885/Kconfig
index 6b4a516addfe..3b6e7f28568e 100644
--- a/drivers/media/video/cx23885/Kconfig
+++ b/drivers/media/video/cx23885/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_CX23885
 	tristate "Conexant cx23885 (2388x successor) support"
-	depends on DVB_CORE && VIDEO_DEV && PCI && I2C && INPUT
+	depends on DVB_CORE && VIDEO_DEV && PCI && I2C && INPUT && SND
+	select SND_PCM
 	select I2C_ALGOBIT
 	select VIDEO_BTCX
 	select VIDEO_TUNER
@@ -33,3 +34,12 @@ config VIDEO_CX23885
 	  To compile this driver as a module, choose M here: the
 	  module will be called cx23885
 
+config MEDIA_ALTERA_CI
+	tristate "Altera FPGA based CI module"
+	depends on VIDEO_CX23885 && DVB_CORE
+	select STAPL_ALTERA
+	---help---
+	  An Altera FPGA CI module for NetUP Dual DVB-T/C RF CI card.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called altera-ci
diff --git a/drivers/media/video/cx23885/Makefile b/drivers/media/video/cx23885/Makefile
index e2ee95f660d8..23293c7b6ac7 100644
--- a/drivers/media/video/cx23885/Makefile
+++ b/drivers/media/video/cx23885/Makefile
@@ -5,6 +5,7 @@ cx23885-objs	:= cx23885-cards.o cx23885-video.o cx23885-vbi.o \
 		    cx23885-f300.o
 
 obj-$(CONFIG_VIDEO_CX23885) += cx23885.o
+obj-$(CONFIG_MEDIA_ALTERA_CI) += altera-ci.o
 
 EXTRA_CFLAGS += -Idrivers/media/video
 EXTRA_CFLAGS += -Idrivers/media/common/tuners
diff --git a/drivers/media/video/cx23885/altera-ci.c b/drivers/media/video/cx23885/altera-ci.c
new file mode 100644
index 000000000000..ad6cc68d4b0c
--- /dev/null
+++ b/drivers/media/video/cx23885/altera-ci.c
@@ -0,0 +1,832 @@
+/*
+ * altera-ci.c
+ *
+ *  CI driver in conjunction with NetUp Dual DVB-T/C RF CI card
+ *
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/*
+ * currently cx23885 GPIO's used.
+ * GPIO-0 ~INT in
+ * GPIO-1 TMS out
+ * GPIO-2 ~reset chips out
+ * GPIO-3 to GPIO-10 data/addr for CA in/out
+ * GPIO-11 ~CS out
+ * GPIO-12 AD_RG out
+ * GPIO-13 ~WR out
+ * GPIO-14 ~RD out
+ * GPIO-15 ~RDY in
+ * GPIO-16 TCK out
+ * GPIO-17 TDO in
+ * GPIO-18 TDI out
+ */
+/*
+ *  Bit definitions for MC417_RWD and MC417_OEN registers
+ * bits 31-16
+ * +-----------+
+ * | Reserved  |
+ * +-----------+
+ *   bit 15  bit 14  bit 13 bit 12  bit 11  bit 10  bit 9   bit 8
+ * +-------+-------+-------+-------+-------+-------+-------+-------+
+ * |  TDI  |  TDO  |  TCK  |  RDY# |  #RD  |  #WR  | AD_RG |  #CS  |
+ * +-------+-------+-------+-------+-------+-------+-------+-------+
+ *  bit 7   bit 6   bit 5   bit 4   bit 3   bit 2   bit 1   bit 0
+ * +-------+-------+-------+-------+-------+-------+-------+-------+
+ * |  DATA7|  DATA6|  DATA5|  DATA4|  DATA3|  DATA2|  DATA1|  DATA0|
+ * +-------+-------+-------+-------+-------+-------+-------+-------+
+ */
+#include <linux/version.h>
+#include <media/videobuf-dma-sg.h>
+#include <media/videobuf-dvb.h>
+#include "altera-ci.h"
+#include "dvb_ca_en50221.h"
+
+/* FPGA regs */
+#define NETUP_CI_INT_CTRL	0x00
+#define NETUP_CI_BUSCTRL2	0x01
+#define NETUP_CI_ADDR0		0x04
+#define NETUP_CI_ADDR1		0x05
+#define NETUP_CI_DATA		0x06
+#define NETUP_CI_BUSCTRL	0x07
+#define NETUP_CI_PID_ADDR0	0x08
+#define NETUP_CI_PID_ADDR1	0x09
+#define NETUP_CI_PID_DATA	0x0a
+#define NETUP_CI_TSA_DIV	0x0c
+#define NETUP_CI_TSB_DIV	0x0d
+#define NETUP_CI_REVISION	0x0f
+
+/* const for ci op */
+#define NETUP_CI_FLG_CTL	1
+#define NETUP_CI_FLG_RD		1
+#define NETUP_CI_FLG_AD		1
+
+static unsigned int ci_dbg;
+module_param(ci_dbg, int, 0644);
+MODULE_PARM_DESC(ci_dbg, "Enable CI debugging");
+
+static unsigned int pid_dbg;
+module_param(pid_dbg, int, 0644);
+MODULE_PARM_DESC(pid_dbg, "Enable PID filtering debugging");
+
+MODULE_DESCRIPTION("altera FPGA CI module");
+MODULE_AUTHOR("Igor M. Liplianin  <liplianin@netup.ru>");
+MODULE_LICENSE("GPL");
+
+#define ci_dbg_print(args...) \
+	do { \
+		if (ci_dbg) \
+			printk(KERN_DEBUG args); \
+	} while (0)
+
+#define pid_dbg_print(args...) \
+	do { \
+		if (pid_dbg) \
+			printk(KERN_DEBUG args); \
+	} while (0)
+
+struct altera_ci_state;
+struct netup_hw_pid_filter;
+
+struct fpga_internal {
+	void *dev;
+	struct mutex fpga_mutex;/* two CI's on the same fpga */
+	struct netup_hw_pid_filter *pid_filt[2];
+	struct altera_ci_state *state[2];
+	struct work_struct work;
+	int (*fpga_rw) (void *dev, int flag, int data, int rw);
+	int cis_used;
+	int filts_used;
+	int strt_wrk;
+};
+
+/* stores all private variables for communication with CI */
+struct altera_ci_state {
+	struct fpga_internal *internal;
+	struct dvb_ca_en50221 ca;
+	int status;
+	int nr;
+};
+
+/* stores all private variables for hardware pid filtering */
+struct netup_hw_pid_filter {
+	struct fpga_internal *internal;
+	struct dvb_demux *demux;
+	/* save old functions */
+	int (*start_feed)(struct dvb_demux_feed *feed);
+	int (*stop_feed)(struct dvb_demux_feed *feed);
+
+	int status;
+	int nr;
+};
+
+/* internal params node */
+struct fpga_inode {
+	/* pointer for internal params, one for each pair of CI's */
+	struct fpga_internal		*internal;
+	struct fpga_inode		*next_inode;
+};
+
+/* first internal params */
+static struct fpga_inode *fpga_first_inode;
+
+/* find chip by dev */
+static struct fpga_inode *find_inode(void *dev)
+{
+	struct fpga_inode *temp_chip = fpga_first_inode;
+
+	if (temp_chip == NULL)
+		return temp_chip;
+
+	/*
+	 Search for the last fpga CI chip or
+	 find it by dev */
+	while ((temp_chip != NULL) &&
+				(temp_chip->internal->dev != dev))
+		temp_chip = temp_chip->next_inode;
+
+	return temp_chip;
+}
+/* check demux */
+static struct fpga_internal *check_filter(struct fpga_internal *temp_int,
+						void *demux_dev, int filt_nr)
+{
+	if (temp_int == NULL)
+		return NULL;
+
+	if ((temp_int->pid_filt[filt_nr]) == NULL)
+		return NULL;
+
+	if (temp_int->pid_filt[filt_nr]->demux == demux_dev)
+		return temp_int;
+
+	return NULL;
+}
+
+/* find chip by demux */
+static struct fpga_inode *find_dinode(void *demux_dev)
+{
+	struct fpga_inode *temp_chip = fpga_first_inode;
+	struct fpga_internal *temp_int;
+
+	/*
+	 * Search of the last fpga CI chip or
+	 * find it by demux
+	 */
+	while (temp_chip != NULL) {
+		if (temp_chip->internal != NULL) {
+			temp_int = temp_chip->internal;
+			if (check_filter(temp_int, demux_dev, 0))
+				break;
+			if (check_filter(temp_int, demux_dev, 1))
+				break;
+		}
+
+		temp_chip = temp_chip->next_inode;
+	}
+
+	return temp_chip;
+}
+
+/* deallocating chip */
+static void remove_inode(struct fpga_internal *internal)
+{
+	struct fpga_inode *prev_node = fpga_first_inode;
+	struct fpga_inode *del_node = find_inode(internal->dev);
+
+	if (del_node != NULL) {
+		if (del_node == fpga_first_inode) {
+			fpga_first_inode = del_node->next_inode;
+		} else {
+			while (prev_node->next_inode != del_node)
+				prev_node = prev_node->next_inode;
+
+			if (del_node->next_inode == NULL)
+				prev_node->next_inode = NULL;
+			else
+				prev_node->next_inode =
+					prev_node->next_inode->next_inode;
+		}
+
+		kfree(del_node);
+	}
+}
+
+/* allocating new chip */
+static struct fpga_inode *append_internal(struct fpga_internal *internal)
+{
+	struct fpga_inode *new_node = fpga_first_inode;
+
+	if (new_node == NULL) {
+		new_node = kmalloc(sizeof(struct fpga_inode), GFP_KERNEL);
+		fpga_first_inode = new_node;
+	} else {
+		while (new_node->next_inode != NULL)
+			new_node = new_node->next_inode;
+
+		new_node->next_inode =
+				kmalloc(sizeof(struct fpga_inode), GFP_KERNEL);
+		if (new_node->next_inode != NULL)
+			new_node = new_node->next_inode;
+		else
+			new_node = NULL;
+	}
+
+	if (new_node != NULL) {
+		new_node->internal = internal;
+		new_node->next_inode = NULL;
+	}
+
+	return new_node;
+}
+
+static int netup_fpga_op_rw(struct fpga_internal *inter, int addr,
+							u8 val, u8 read)
+{
+	inter->fpga_rw(inter->dev, NETUP_CI_FLG_AD, addr, 0);
+	return inter->fpga_rw(inter->dev, 0, val, read);
+}
+
+/* flag - mem/io, read - read/write */
+int altera_ci_op_cam(struct dvb_ca_en50221 *en50221, int slot,
+				u8 flag, u8 read, int addr, u8 val)
+{
+
+	struct altera_ci_state *state = en50221->data;
+	struct fpga_internal *inter = state->internal;
+
+	u8 store;
+	int mem = 0;
+
+	if (0 != slot)
+		return -EINVAL;
+
+	mutex_lock(&inter->fpga_mutex);
+
+	netup_fpga_op_rw(inter, NETUP_CI_ADDR0, ((addr << 1) & 0xfe), 0);
+	netup_fpga_op_rw(inter, NETUP_CI_ADDR1, ((addr >> 7) & 0x7f), 0);
+	store = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL, 0, NETUP_CI_FLG_RD);
+
+	store &= 0x3f;
+	store |= ((state->nr << 7) | (flag << 6));
+
+	netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL, store, 0);
+	mem = netup_fpga_op_rw(inter, NETUP_CI_DATA, val, read);
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	ci_dbg_print("%s: %s: addr=[0x%02x], %s=%x\n", __func__,
+			(read) ? "read" : "write", addr,
+			(flag == NETUP_CI_FLG_CTL) ? "ctl" : "mem",
+			(read) ? mem : val);
+
+	return mem;
+}
+
+int altera_ci_read_attribute_mem(struct dvb_ca_en50221 *en50221,
+						int slot, int addr)
+{
+	return altera_ci_op_cam(en50221, slot, 0, NETUP_CI_FLG_RD, addr, 0);
+}
+
+int altera_ci_write_attribute_mem(struct dvb_ca_en50221 *en50221,
+						int slot, int addr, u8 data)
+{
+	return altera_ci_op_cam(en50221, slot, 0, 0, addr, data);
+}
+
+int altera_ci_read_cam_ctl(struct dvb_ca_en50221 *en50221, int slot, u8 addr)
+{
+	return altera_ci_op_cam(en50221, slot, NETUP_CI_FLG_CTL,
+						NETUP_CI_FLG_RD, addr, 0);
+}
+
+int altera_ci_write_cam_ctl(struct dvb_ca_en50221 *en50221, int slot,
+						u8 addr, u8 data)
+{
+	return altera_ci_op_cam(en50221, slot, NETUP_CI_FLG_CTL, 0, addr, data);
+}
+
+int altera_ci_slot_reset(struct dvb_ca_en50221 *en50221, int slot)
+{
+	struct altera_ci_state *state = en50221->data;
+	struct fpga_internal *inter = state->internal;
+	/* reasonable timeout for CI reset is 10 seconds */
+	unsigned long t_out = jiffies + msecs_to_jiffies(9999);
+	int ret;
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (0 != slot)
+		return -EINVAL;
+
+	mutex_lock(&inter->fpga_mutex);
+
+	ret = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL, 0, NETUP_CI_FLG_RD);
+	netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL,
+				ret | (1 << (5 - state->nr)), 0);
+
+	for (;;) {
+		mdelay(50);
+		ret = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL,
+						0, NETUP_CI_FLG_RD);
+		if ((ret & (1 << (5 - state->nr))) == 0)
+			break;
+		if (time_after(jiffies, t_out))
+			break;
+	}
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	ci_dbg_print("%s: %d msecs\n", __func__,
+		jiffies_to_msecs(jiffies + msecs_to_jiffies(9999) - t_out));
+
+	return 0;
+}
+
+int altera_ci_slot_shutdown(struct dvb_ca_en50221 *en50221, int slot)
+{
+	/* not implemented */
+	return 0;
+}
+
+int altera_ci_slot_ts_ctl(struct dvb_ca_en50221 *en50221, int slot)
+{
+	struct altera_ci_state *state = en50221->data;
+	struct fpga_internal *inter = state->internal;
+	int ret;
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (0 != slot)
+		return -EINVAL;
+
+	mutex_lock(&inter->fpga_mutex);
+
+	ret = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL, 0, NETUP_CI_FLG_RD);
+	netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL,
+				ret | (1 << (3 - state->nr)), 0);
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	return 0;
+}
+
+/* work handler */
+static void netup_read_ci_status(struct work_struct *work)
+{
+	struct fpga_internal *inter =
+			container_of(work, struct fpga_internal, work);
+	int ret;
+
+	ci_dbg_print("%s\n", __func__);
+
+	mutex_lock(&inter->fpga_mutex);
+	/* ack' irq */
+	ret = netup_fpga_op_rw(inter, NETUP_CI_INT_CTRL, 0, NETUP_CI_FLG_RD);
+	ret = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL, 0, NETUP_CI_FLG_RD);
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	if (inter->state[1] != NULL) {
+		inter->state[1]->status =
+				((ret & 1) == 0 ?
+				DVB_CA_EN50221_POLL_CAM_PRESENT |
+				DVB_CA_EN50221_POLL_CAM_READY : 0);
+		ci_dbg_print("%s: setting CI[1] status = 0x%x\n",
+				__func__, inter->state[1]->status);
+	};
+
+	if (inter->state[0] != NULL) {
+		inter->state[0]->status =
+				((ret & 2) == 0 ?
+				DVB_CA_EN50221_POLL_CAM_PRESENT |
+				DVB_CA_EN50221_POLL_CAM_READY : 0);
+		ci_dbg_print("%s: setting CI[0] status = 0x%x\n",
+				__func__, inter->state[0]->status);
+	};
+}
+
+/* CI irq handler */
+int altera_ci_irq(void *dev)
+{
+	struct fpga_inode *temp_int = NULL;
+	struct fpga_internal *inter = NULL;
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (dev != NULL) {
+		temp_int = find_inode(dev);
+		if (temp_int != NULL) {
+			inter = temp_int->internal;
+			schedule_work(&inter->work);
+		}
+	}
+
+	return 1;
+}
+EXPORT_SYMBOL(altera_ci_irq);
+
+int altera_poll_ci_slot_status(struct dvb_ca_en50221 *en50221, int slot,
+								int open)
+{
+	struct altera_ci_state *state = en50221->data;
+
+	if (0 != slot)
+		return -EINVAL;
+
+	return state->status;
+}
+
+void altera_hw_filt_release(void *main_dev, int filt_nr)
+{
+	struct fpga_inode *temp_int = find_inode(main_dev);
+	struct netup_hw_pid_filter *pid_filt = NULL;
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (temp_int != NULL) {
+		pid_filt = temp_int->internal->pid_filt[filt_nr - 1];
+		/* stored old feed controls */
+		pid_filt->demux->start_feed = pid_filt->start_feed;
+		pid_filt->demux->stop_feed = pid_filt->stop_feed;
+
+		if (((--(temp_int->internal->filts_used)) <= 0) &&
+			 ((temp_int->internal->cis_used) <= 0)) {
+
+			ci_dbg_print("%s: Actually removing\n", __func__);
+
+			remove_inode(temp_int->internal);
+			kfree(pid_filt->internal);
+		}
+
+		kfree(pid_filt);
+
+	}
+
+}
+EXPORT_SYMBOL(altera_hw_filt_release);
+
+void altera_ci_release(void *dev, int ci_nr)
+{
+	struct fpga_inode *temp_int = find_inode(dev);
+	struct altera_ci_state *state = NULL;
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (temp_int != NULL) {
+		state = temp_int->internal->state[ci_nr - 1];
+		altera_hw_filt_release(dev, ci_nr);
+
+
+		if (((temp_int->internal->filts_used) <= 0) &&
+				((--(temp_int->internal->cis_used)) <= 0)) {
+
+			ci_dbg_print("%s: Actually removing\n", __func__);
+
+			remove_inode(temp_int->internal);
+			kfree(state->internal);
+		}
+
+		if (state != NULL) {
+			if (state->ca.data != NULL)
+				dvb_ca_en50221_release(&state->ca);
+
+			kfree(state);
+		}
+	}
+
+}
+EXPORT_SYMBOL(altera_ci_release);
+
+static void altera_pid_control(struct netup_hw_pid_filter *pid_filt,
+		u16 pid, int onoff)
+{
+	struct fpga_internal *inter = pid_filt->internal;
+	u8 store = 0;
+
+	/* pid 0-0x1f always enabled, don't touch them */
+	if ((pid == 0x2000) || (pid < 0x20))
+		return;
+
+	mutex_lock(&inter->fpga_mutex);
+
+	netup_fpga_op_rw(inter, NETUP_CI_PID_ADDR0, (pid >> 3) & 0xff, 0);
+	netup_fpga_op_rw(inter, NETUP_CI_PID_ADDR1,
+			((pid >> 11) & 0x03) | (pid_filt->nr << 2), 0);
+
+	store = netup_fpga_op_rw(inter, NETUP_CI_PID_DATA, 0, NETUP_CI_FLG_RD);
+
+	if (onoff)/* 0 - on, 1 - off */
+		store |= (1 << (pid & 7));
+	else
+		store &= ~(1 << (pid & 7));
+
+	netup_fpga_op_rw(inter, NETUP_CI_PID_DATA, store, 0);
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	pid_dbg_print("%s: (%d) set pid: %5d 0x%04x '%s'\n", __func__,
+		pid_filt->nr, pid, pid, onoff ? "off" : "on");
+}
+
+static void altera_toggle_fullts_streaming(struct netup_hw_pid_filter *pid_filt,
+					int filt_nr, int onoff)
+{
+	struct fpga_internal *inter = pid_filt->internal;
+	u8 store = 0;
+	int i;
+
+	pid_dbg_print("%s: pid_filt->nr[%d]  now %s\n", __func__, pid_filt->nr,
+			onoff ? "off" : "on");
+
+	if (onoff)/* 0 - on, 1 - off */
+		store = 0xff;/* ignore pid */
+	else
+		store = 0;/* enable pid */
+
+	mutex_lock(&inter->fpga_mutex);
+
+	for (i = 0; i < 1024; i++) {
+		netup_fpga_op_rw(inter, NETUP_CI_PID_ADDR0, i & 0xff, 0);
+
+		netup_fpga_op_rw(inter, NETUP_CI_PID_ADDR1,
+				((i >> 8) & 0x03) | (pid_filt->nr << 2), 0);
+		/* pid 0-0x1f always enabled */
+		netup_fpga_op_rw(inter, NETUP_CI_PID_DATA,
+				(i > 3 ? store : 0), 0);
+	}
+
+	mutex_unlock(&inter->fpga_mutex);
+}
+
+int altera_pid_feed_control(void *demux_dev, int filt_nr,
+		struct dvb_demux_feed *feed, int onoff)
+{
+	struct fpga_inode *temp_int = find_dinode(demux_dev);
+	struct fpga_internal *inter = temp_int->internal;
+	struct netup_hw_pid_filter *pid_filt = inter->pid_filt[filt_nr - 1];
+
+	altera_pid_control(pid_filt, feed->pid, onoff ? 0 : 1);
+	/* call old feed proc's */
+	if (onoff)
+		pid_filt->start_feed(feed);
+	else
+		pid_filt->stop_feed(feed);
+
+	if (feed->pid == 0x2000)
+		altera_toggle_fullts_streaming(pid_filt, filt_nr,
+						onoff ? 0 : 1);
+
+	return 0;
+}
+EXPORT_SYMBOL(altera_pid_feed_control);
+
+int altera_ci_start_feed(struct dvb_demux_feed *feed, int num)
+{
+	altera_pid_feed_control(feed->demux, num, feed, 1);
+
+	return 0;
+}
+
+int altera_ci_stop_feed(struct dvb_demux_feed *feed, int num)
+{
+	altera_pid_feed_control(feed->demux, num, feed, 0);
+
+	return 0;
+}
+
+int altera_ci_start_feed_1(struct dvb_demux_feed *feed)
+{
+	return altera_ci_start_feed(feed, 1);
+}
+
+int altera_ci_stop_feed_1(struct dvb_demux_feed *feed)
+{
+	return altera_ci_stop_feed(feed, 1);
+}
+
+int altera_ci_start_feed_2(struct dvb_demux_feed *feed)
+{
+	return altera_ci_start_feed(feed, 2);
+}
+
+int altera_ci_stop_feed_2(struct dvb_demux_feed *feed)
+{
+	return altera_ci_stop_feed(feed, 2);
+}
+
+int altera_hw_filt_init(struct altera_ci_config *config, int hw_filt_nr)
+{
+	struct netup_hw_pid_filter *pid_filt = NULL;
+	struct fpga_inode *temp_int = find_inode(config->dev);
+	struct fpga_internal *inter = NULL;
+	int ret = 0;
+
+	pid_filt = kzalloc(sizeof(struct netup_hw_pid_filter), GFP_KERNEL);
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (!pid_filt) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	if (temp_int != NULL) {
+		inter = temp_int->internal;
+		(inter->filts_used)++;
+		ci_dbg_print("%s: Find Internal Structure!\n", __func__);
+	} else {
+		inter = kzalloc(sizeof(struct fpga_internal), GFP_KERNEL);
+		if (!inter) {
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		temp_int = append_internal(inter);
+		inter->filts_used = 1;
+		inter->dev = config->dev;
+		inter->fpga_rw = config->fpga_rw;
+		mutex_init(&inter->fpga_mutex);
+		inter->strt_wrk = 1;
+		ci_dbg_print("%s: Create New Internal Structure!\n", __func__);
+	}
+
+	ci_dbg_print("%s: setting hw pid filter = 0x%x for ci = %d\n", __func__,
+						(int)pid_filt, hw_filt_nr - 1);
+	inter->pid_filt[hw_filt_nr - 1] = pid_filt;
+	pid_filt->demux = config->demux;
+	pid_filt->internal = inter;
+	pid_filt->nr = hw_filt_nr - 1;
+	/* store old feed controls */
+	pid_filt->start_feed = config->demux->start_feed;
+	pid_filt->stop_feed = config->demux->stop_feed;
+	/* replace with new feed controls */
+	if (hw_filt_nr == 1) {
+		pid_filt->demux->start_feed = altera_ci_start_feed_1;
+		pid_filt->demux->stop_feed = altera_ci_stop_feed_1;
+	} else if (hw_filt_nr == 2) {
+		pid_filt->demux->start_feed = altera_ci_start_feed_2;
+		pid_filt->demux->stop_feed = altera_ci_stop_feed_2;
+	}
+
+	altera_toggle_fullts_streaming(pid_filt, 0, 1);
+
+	return 0;
+err:
+	ci_dbg_print("%s: Can't init hardware filter: Error %d\n",
+		     __func__, ret);
+
+	kfree(pid_filt);
+
+	return ret;
+}
+EXPORT_SYMBOL(altera_hw_filt_init);
+
+int altera_ci_init(struct altera_ci_config *config, int ci_nr)
+{
+	struct altera_ci_state *state;
+	struct fpga_inode *temp_int = find_inode(config->dev);
+	struct fpga_internal *inter = NULL;
+	int ret = 0;
+	u8 store = 0;
+
+	state = kzalloc(sizeof(struct altera_ci_state), GFP_KERNEL);
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (!state) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	if (temp_int != NULL) {
+		inter = temp_int->internal;
+		(inter->cis_used)++;
+		ci_dbg_print("%s: Find Internal Structure!\n", __func__);
+	} else {
+		inter = kzalloc(sizeof(struct fpga_internal), GFP_KERNEL);
+		if (!inter) {
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		temp_int = append_internal(inter);
+		inter->cis_used = 1;
+		inter->dev = config->dev;
+		inter->fpga_rw = config->fpga_rw;
+		mutex_init(&inter->fpga_mutex);
+		inter->strt_wrk = 1;
+		ci_dbg_print("%s: Create New Internal Structure!\n", __func__);
+	}
+
+	ci_dbg_print("%s: setting state = 0x%x for ci = %d\n", __func__,
+						(int)state, ci_nr - 1);
+	inter->state[ci_nr - 1] = state;
+	state->internal = inter;
+	state->nr = ci_nr - 1;
+
+	state->ca.owner = THIS_MODULE;
+	state->ca.read_attribute_mem = altera_ci_read_attribute_mem;
+	state->ca.write_attribute_mem = altera_ci_write_attribute_mem;
+	state->ca.read_cam_control = altera_ci_read_cam_ctl;
+	state->ca.write_cam_control = altera_ci_write_cam_ctl;
+	state->ca.slot_reset = altera_ci_slot_reset;
+	state->ca.slot_shutdown = altera_ci_slot_shutdown;
+	state->ca.slot_ts_enable = altera_ci_slot_ts_ctl;
+	state->ca.poll_slot_status = altera_poll_ci_slot_status;
+	state->ca.data = state;
+
+	ret = dvb_ca_en50221_init(config->adapter,
+				   &state->ca,
+				   /* flags */ 0,
+				   /* n_slots */ 1);
+	if (0 != ret)
+		goto err;
+
+	altera_hw_filt_init(config, ci_nr);
+
+	if (inter->strt_wrk) {
+		INIT_WORK(&inter->work, netup_read_ci_status);
+		inter->strt_wrk = 0;
+	}
+
+	ci_dbg_print("%s: CI initialized!\n", __func__);
+
+	mutex_lock(&inter->fpga_mutex);
+
+	/* Enable div */
+	netup_fpga_op_rw(inter, NETUP_CI_TSA_DIV, 0x0, 0);
+	netup_fpga_op_rw(inter, NETUP_CI_TSB_DIV, 0x0, 0);
+
+	/* enable TS out */
+	store = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL2, 0, NETUP_CI_FLG_RD);
+	store |= (3 << 4);
+	netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL2, store, 0);
+
+	ret = netup_fpga_op_rw(inter, NETUP_CI_REVISION, 0, NETUP_CI_FLG_RD);
+	/* enable irq */
+	netup_fpga_op_rw(inter, NETUP_CI_INT_CTRL, 0x44, 0);
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	ci_dbg_print("%s: NetUP CI Revision = 0x%x\n", __func__, ret);
+
+	schedule_work(&inter->work);
+
+	return 0;
+err:
+	ci_dbg_print("%s: Cannot initialize CI: Error %d.\n", __func__, ret);
+
+	kfree(state);
+
+	return ret;
+}
+EXPORT_SYMBOL(altera_ci_init);
+
+int altera_ci_tuner_reset(void *dev, int ci_nr)
+{
+	struct fpga_inode *temp_int = find_inode(dev);
+	struct fpga_internal *inter = NULL;
+	u8 store;
+
+	ci_dbg_print("%s\n", __func__);
+
+	if (temp_int == NULL)
+		return -1;
+
+	if (temp_int->internal == NULL)
+		return -1;
+
+	inter = temp_int->internal;
+
+	mutex_lock(&inter->fpga_mutex);
+
+	store = netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL2, 0, NETUP_CI_FLG_RD);
+	store &= ~(4 << (2 - ci_nr));
+	netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL2, store, 0);
+	msleep(100);
+	store |= (4 << (2 - ci_nr));
+	netup_fpga_op_rw(inter, NETUP_CI_BUSCTRL2, store, 0);
+
+	mutex_unlock(&inter->fpga_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(altera_ci_tuner_reset);
diff --git a/drivers/media/video/cx23885/altera-ci.h b/drivers/media/video/cx23885/altera-ci.h
new file mode 100644
index 000000000000..bf0aa2c9b070
--- /dev/null
+++ b/drivers/media/video/cx23885/altera-ci.h
@@ -0,0 +1,100 @@
+/*
+ * altera-ci.c
+ *
+ *  CI driver in conjunction with NetUp Dual DVB-T/C RF CI card
+ *
+ * Copyright (C) 2010 NetUP Inc.
+ * Copyright (C) 2010 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#ifndef __ALTERA_CI_H
+#define __ALTERA_CI_H
+
+#define ALT_DATA	0x000000ff
+#define ALT_TDI		0x00008000
+#define ALT_TDO		0x00004000
+#define ALT_TCK		0x00002000
+#define ALT_RDY		0x00001000
+#define ALT_RD		0x00000800
+#define ALT_WR		0x00000400
+#define ALT_AD_RG	0x00000200
+#define ALT_CS		0x00000100
+
+struct altera_ci_config {
+	void *dev;/* main dev, for example cx23885_dev */
+	void *adapter;/* for CI to connect to */
+	struct dvb_demux *demux;/* for hardware PID filter to connect to */
+	int (*fpga_rw) (void *dev, int ad_rg, int val, int rw);
+};
+
+#if defined(CONFIG_MEDIA_ALTERA_CI) || (defined(CONFIG_MEDIA_ALTERA_CI_MODULE) \
+							&& defined(MODULE))
+
+extern int altera_ci_init(struct altera_ci_config *config, int ci_nr);
+extern void altera_ci_release(void *dev, int ci_nr);
+extern int altera_ci_irq(void *dev);
+extern int altera_ci_tuner_reset(void *dev, int ci_nr);
+
+#else
+
+static inline int altera_ci_init(struct altera_ci_config *config, int ci_nr)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return 0;
+}
+
+static inline void altera_ci_release(void *dev, int ci_nr)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+}
+
+static inline int altera_ci_irq(void *dev)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return 0;
+}
+
+static int altera_ci_tuner_reset(void *dev, int ci_nr)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return 0;
+}
+
+#endif
+#if 0
+static inline int altera_hw_filt_init(struct altera_ci_config *config,
+							int hw_filt_nr)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return 0;
+}
+
+static inline void altera_hw_filt_release(void *dev, int filt_nr)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+}
+
+static inline int altera_pid_feed_control(void *dev, int filt_nr,
+		struct dvb_demux_feed *dvbdmxfeed, int onoff)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return 0;
+}
+
+#endif /* CONFIG_MEDIA_ALTERA_CI */
+
+#endif /* __ALTERA_CI_H */
diff --git a/drivers/media/video/cx23885/cx23885-cards.c b/drivers/media/video/cx23885/cx23885-cards.c
index b298b730943c..ea88722cb4ab 100644
--- a/drivers/media/video/cx23885/cx23885-cards.c
+++ b/drivers/media/video/cx23885/cx23885-cards.c
@@ -24,10 +24,14 @@
 #include <linux/pci.h>
 #include <linux/delay.h>
 #include <media/cx25840.h>
+#include <linux/firmware.h>
+#include <staging/altera.h>
 
 #include "cx23885.h"
 #include "tuner-xc2028.h"
 #include "netup-init.h"
+#include "altera-ci.h"
+#include "xc5000.h"
 #include "cx23888-ir.h"
 
 static unsigned int enable_885_ir;
@@ -90,6 +94,7 @@ struct cx23885_board cx23885_boards[] = {
 		.portc		= CX23885_MPEG_DVB,
 		.tuner_type	= TUNER_PHILIPS_TDA8290,
 		.tuner_addr	= 0x42, /* 0x84 >> 1 */
+		.tuner_bus	= 1,
 		.input          = {{
 			.type   = CX23885_VMUX_TELEVISION,
 			.vmux   =	CX25840_VIN7_CH3 |
@@ -187,7 +192,7 @@ struct cx23885_board cx23885_boards[] = {
 		.portb		= CX23885_MPEG_DVB,
 	},
 	[CX23885_BOARD_NETUP_DUAL_DVBS2_CI] = {
-		.cimax		= 1,
+		.ci_type	= 1,
 		.name		= "NetUP Dual DVB-S2 CI",
 		.portb		= CX23885_MPEG_DVB,
 		.portc		= CX23885_MPEG_DVB,
@@ -212,6 +217,7 @@ struct cx23885_board cx23885_boards[] = {
 		.name		= "Mygica X8506 DMB-TH",
 		.tuner_type = TUNER_XC5000,
 		.tuner_addr = 0x61,
+		.tuner_bus	= 1,
 		.porta		= CX23885_ANALOG_VIDEO,
 		.portb		= CX23885_MPEG_DVB,
 		.input		= {
@@ -241,6 +247,7 @@ struct cx23885_board cx23885_boards[] = {
 		.name		= "Magic-Pro ProHDTV Extreme 2",
 		.tuner_type = TUNER_XC5000,
 		.tuner_addr = 0x61,
+		.tuner_bus	= 1,
 		.porta		= CX23885_ANALOG_VIDEO,
 		.portb		= CX23885_MPEG_DVB,
 		.input		= {
@@ -289,6 +296,7 @@ struct cx23885_board cx23885_boards[] = {
 		.porta          = CX23885_ANALOG_VIDEO,
 		.tuner_type     = TUNER_XC2028,
 		.tuner_addr     = 0x61,
+		.tuner_bus	= 1,
 		.input          = {{
 			.type   = CX23885_VMUX_TELEVISION,
 			.vmux   = CX25840_VIN2_CH1 |
@@ -313,6 +321,7 @@ struct cx23885_board cx23885_boards[] = {
 		.name		= "GoTView X5 3D Hybrid",
 		.tuner_type	= TUNER_XC5000,
 		.tuner_addr	= 0x64,
+		.tuner_bus	= 1,
 		.porta		= CX23885_ANALOG_VIDEO,
 		.portb		= CX23885_MPEG_DVB,
 		.input          = {{
@@ -329,6 +338,21 @@ struct cx23885_board cx23885_boards[] = {
 				  CX25840_SVIDEO_CHROMA4,
 		} },
 	},
+	[CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF] = {
+		.ci_type	= 2,
+		.name		= "NetUP Dual DVB-T/C-CI RF",
+		.porta		= CX23885_ANALOG_VIDEO,
+		.portb		= CX23885_MPEG_DVB,
+		.portc		= CX23885_MPEG_DVB,
+		.num_fds_portb	= 2,
+		.num_fds_portc	= 2,
+		.tuner_type	= TUNER_XC5000,
+		.tuner_addr	= 0x64,
+		.input          = { {
+				.type   = CX23885_VMUX_TELEVISION,
+				.vmux   = CX25840_COMPOSITE1,
+		} },
+	},
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
 
@@ -520,6 +544,10 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x5654,
 		.subdevice = 0x2390,
 		.card      = CX23885_BOARD_GOTVIEW_X5_3D_HYBRID,
+	}, {
+		.subvendor = 0x1b55,
+		.subdevice = 0xe2e4,
+		.card      = CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF,
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -740,6 +768,9 @@ int cx23885_tuner_callback(void *priv, int component, int command, int arg)
 		/* Tuner Reset Command */
 		bitmask = 0x02;
 		break;
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
+		altera_ci_tuner_reset(dev, port->nr);
+		break;
 	}
 
 	if (bitmask) {
@@ -998,6 +1029,33 @@ void cx23885_gpio_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_GOTVIEW_X5_3D_HYBRID:
 		cx_set(GP0_IO, 0x00010001); /* Bring the part out of reset */
 		break;
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
+		/* GPIO-0 ~INT in
+		   GPIO-1 TMS out
+		   GPIO-2 ~reset chips out
+		   GPIO-3 to GPIO-10 data/addr for CA in/out
+		   GPIO-11 ~CS out
+		   GPIO-12 ADDR out
+		   GPIO-13 ~WR out
+		   GPIO-14 ~RD out
+		   GPIO-15 ~RDY in
+		   GPIO-16 TCK out
+		   GPIO-17 TDO in
+		   GPIO-18 TDI out
+		 */
+		cx_set(GP0_IO, 0x00060000); /* GPIO-1,2 as out */
+		/* GPIO-0 as INT, reset & TMS low */
+		cx_clear(GP0_IO, 0x00010006);
+		mdelay(100);/* reset delay */
+		cx_set(GP0_IO, 0x00000004); /* reset high */
+		cx_write(MC417_CTL, 0x00000037);/* enable GPIO-3..18 pins */
+		/* GPIO-17 is TDO in, GPIO-15 is ~RDY in, rest is out */
+		cx_write(MC417_OEN, 0x00005000);
+		/* ~RD, ~WR high; ADDR low; ~CS high */
+		cx_write(MC417_RWD, 0x00000d00);
+		/* enable irq */
+		cx_write(GPIO_ISM, 0x00000000);/* INTERRUPTS active low*/
+		break;
 	}
 }
 
@@ -1113,6 +1171,31 @@ void cx23885_ir_fini(struct cx23885_dev *dev)
 	}
 }
 
+int netup_jtag_io(void *device, int tms, int tdi, int read_tdo)
+{
+	int data;
+	int tdo = 0;
+	struct cx23885_dev *dev = (struct cx23885_dev *)device;
+	/*TMS*/
+	data = ((cx_read(GP0_IO)) & (~0x00000002));
+	data |= (tms ? 0x00020002 : 0x00020000);
+	cx_write(GP0_IO, data);
+
+	/*TDI*/
+	data = ((cx_read(MC417_RWD)) & (~0x0000a000));
+	data |= (tdi ? 0x00008000 : 0);
+	cx_write(MC417_RWD, data);
+	if (read_tdo)
+		tdo = (data & 0x00004000) ? 1 : 0; /*TDO*/
+
+	cx_write(MC417_RWD, data | 0x00002000);
+	udelay(1);
+	/*TCK*/
+	cx_write(MC417_RWD, data);
+
+	return tdo;
+}
+
 void cx23885_ir_pci_int_enable(struct cx23885_dev *dev)
 {
 	switch (dev->board) {
@@ -1212,6 +1295,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		ts1->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
 		break;
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
 		ts1->gen_ctrl_val  = 0xc; /* Serial bus + punctured clock */
 		ts1->ts_clk_en_val = 0x1; /* Enable TS_CLK */
 		ts1->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
@@ -1271,6 +1355,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_LEADTEK_WINFAST_PXDVR3200_H:
 	case CX23885_BOARD_COMPRO_VIDEOMATE_E650F:
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
 	case CX23885_BOARD_COMPRO_VIDEOMATE_E800:
 	case CX23885_BOARD_HAUPPAUGE_HVR1850:
 	case CX23885_BOARD_MYGICA_X8506:
@@ -1293,6 +1378,29 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
 		netup_initialize(dev);
 		break;
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF: {
+		int ret;
+		const struct firmware *fw;
+		const char *filename = "dvb-netup-altera-01.fw";
+		char *action = "configure";
+		struct altera_config netup_config = {
+			.dev = dev,
+			.action = action,
+			.jtag_io = netup_jtag_io,
+		};
+
+		netup_initialize(dev);
+
+		ret = request_firmware(&fw, filename, &dev->pci->dev);
+		if (ret != 0)
+			printk(KERN_ERR "did not find the firmware file. (%s) "
+			"Please see linux/Documentation/dvb/ for more details "
+			"on firmware-problems.", filename);
+		else
+			altera_init(&netup_config, fw);
+
+		break;
+	}
 	}
 }
 
diff --git a/drivers/media/video/cx23885/cx23885-core.c b/drivers/media/video/cx23885/cx23885-core.c
index 359882419b7f..9933810b4e33 100644
--- a/drivers/media/video/cx23885/cx23885-core.c
+++ b/drivers/media/video/cx23885/cx23885-core.c
@@ -29,9 +29,11 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <asm/div64.h>
+#include <linux/firmware.h>
 
 #include "cx23885.h"
 #include "cimax2.h"
+#include "altera-ci.h"
 #include "cx23888-ir.h"
 #include "cx23885-ir.h"
 #include "cx23885-av.h"
@@ -902,8 +904,6 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	dev->pci_bus  = dev->pci->bus->number;
 	dev->pci_slot = PCI_SLOT(dev->pci->devfn);
 	cx23885_irq_add(dev, 0x001f00);
-	if (cx23885_boards[dev->board].cimax > 0)
-		cx23885_irq_add(dev, 0x01800000); /* for CiMaxes */
 
 	/* External Master 1 Bus */
 	dev->i2c_bus[0].nr = 0;
@@ -970,11 +970,12 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	/* Assume some sensible defaults */
 	dev->tuner_type = cx23885_boards[dev->board].tuner_type;
 	dev->tuner_addr = cx23885_boards[dev->board].tuner_addr;
+	dev->tuner_bus = cx23885_boards[dev->board].tuner_bus;
 	dev->radio_type = cx23885_boards[dev->board].radio_type;
 	dev->radio_addr = cx23885_boards[dev->board].radio_addr;
 
-	dprintk(1, "%s() tuner_type = 0x%x tuner_addr = 0x%x\n",
-		__func__, dev->tuner_type, dev->tuner_addr);
+	dprintk(1, "%s() tuner_type = 0x%x tuner_addr = 0x%x tuner_bus = %d\n",
+		__func__, dev->tuner_type, dev->tuner_addr, dev->tuner_bus);
 	dprintk(1, "%s() radio_type = 0x%x radio_addr = 0x%x\n",
 		__func__, dev->radio_type, dev->radio_addr);
 
@@ -1004,6 +1005,9 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	}
 
 	if (cx23885_boards[dev->board].portb == CX23885_MPEG_DVB) {
+		if (cx23885_boards[dev->board].num_fds_portb)
+			dev->ts1.num_frontends =
+				cx23885_boards[dev->board].num_fds_portb;
 		if (cx23885_dvb_register(&dev->ts1) < 0) {
 			printk(KERN_ERR "%s() Failed to register dvb adapters on VID_B\n",
 			       __func__);
@@ -1018,6 +1022,9 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	}
 
 	if (cx23885_boards[dev->board].portc == CX23885_MPEG_DVB) {
+		if (cx23885_boards[dev->board].num_fds_portc)
+			dev->ts2.num_frontends =
+				cx23885_boards[dev->board].num_fds_portc;
 		if (cx23885_dvb_register(&dev->ts2) < 0) {
 			printk(KERN_ERR
 				"%s() Failed to register dvb on VID_C\n",
@@ -1034,6 +1041,10 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 
 	cx23885_dev_checkrevision(dev);
 
+	/* disable MSI for NetUP cards, otherwise CI is not working */
+	if (cx23885_boards[dev->board].ci_type > 0)
+		cx_clear(RDR_RDRCTL1, 1 << 8);
+
 	return 0;
 }
 
@@ -1822,14 +1833,13 @@ static irqreturn_t cx23885_irq(int irq, void *dev_id)
 				PCI_MSK_IR);
 	}
 
-	if (cx23885_boards[dev->board].cimax > 0 &&
-		((pci_status & PCI_MSK_GPIO0) ||
-			(pci_status & PCI_MSK_GPIO1))) {
-
-		if (cx23885_boards[dev->board].cimax > 0)
-			handled += netup_ci_slot_status(dev, pci_status);
+	if (cx23885_boards[dev->board].ci_type == 1 &&
+			(pci_status & (PCI_MSK_GPIO1 | PCI_MSK_GPIO0)))
+		handled += netup_ci_slot_status(dev, pci_status);
 
-	}
+	if (cx23885_boards[dev->board].ci_type == 2 &&
+			(pci_status & PCI_MSK_GPIO0))
+		handled += altera_ci_irq(dev);
 
 	if (ts1_status) {
 		if (cx23885_boards[dev->board].portb == CX23885_MPEG_DVB)
@@ -2064,7 +2074,10 @@ static int __devinit cx23885_initdev(struct pci_dev *pci_dev,
 
 	switch (dev->board) {
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
-		cx23885_irq_add_enable(dev, 0x01800000); /* for NetUP */
+		cx23885_irq_add_enable(dev, PCI_MSK_GPIO1 | PCI_MSK_GPIO0);
+		break;
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
+		cx23885_irq_add_enable(dev, PCI_MSK_GPIO0);
 		break;
 	}
 
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index 5958cb882e93..cf36b9b4794e 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -58,6 +58,8 @@
 #include "atbm8830.h"
 #include "ds3000.h"
 #include "cx23885-f300.h"
+#include "altera-ci.h"
+#include "stv0367.h"
 
 static unsigned int debug;
 
@@ -108,6 +110,22 @@ static void dvb_buf_release(struct videobuf_queue *q,
 	cx23885_free_buffer(q, (struct cx23885_buffer *)vb);
 }
 
+static void cx23885_dvb_gate_ctrl(struct cx23885_tsport  *port, int open)
+{
+	struct videobuf_dvb_frontends *f;
+	struct videobuf_dvb_frontend *fe;
+
+	f = &port->frontends;
+
+	if (f->gate <= 1) /* undefined or fe0 */
+		fe = videobuf_dvb_get_frontend(f, 1);
+	else
+		fe = videobuf_dvb_get_frontend(f, f->gate);
+
+	if (fe && fe->dvb.frontend && fe->dvb.frontend->ops.i2c_gate_ctrl)
+		fe->dvb.frontend->ops.i2c_gate_ctrl(fe->dvb.frontend, open);
+}
+
 static struct videobuf_queue_ops dvb_qops = {
 	.buf_setup    = dvb_buf_setup,
 	.buf_prepare  = dvb_buf_prepare,
@@ -570,12 +588,84 @@ static struct max2165_config mygic_x8558pro_max2165_cfg2 = {
 	.i2c_address = 0x60,
 	.osc_clk = 20
 };
+static struct stv0367_config netup_stv0367_config[] = {
+	{
+		.demod_address = 0x1c,
+		.xtal = 27000000,
+		.if_khz = 4500,
+		.if_iq_mode = 0,
+		.ts_mode = 1,
+		.clk_pol = 0,
+	}, {
+		.demod_address = 0x1d,
+		.xtal = 27000000,
+		.if_khz = 4500,
+		.if_iq_mode = 0,
+		.ts_mode = 1,
+		.clk_pol = 0,
+	},
+};
+
+static struct xc5000_config netup_xc5000_config[] = {
+	{
+		.i2c_address = 0x61,
+		.if_khz = 4500,
+	}, {
+		.i2c_address = 0x64,
+		.if_khz = 4500,
+	},
+};
+
+int netup_altera_fpga_rw(void *device, int flag, int data, int read)
+{
+	struct cx23885_dev *dev = (struct cx23885_dev *)device;
+	unsigned long timeout = jiffies + msecs_to_jiffies(1);
+	int mem = 0;
+
+	cx_set(MC417_RWD, ALT_RD | ALT_WR | ALT_CS);
+	if (read)
+		cx_set(MC417_OEN, ALT_DATA);
+	else {
+		cx_clear(MC417_OEN, ALT_DATA);/* D0-D7 out */
+		mem = cx_read(MC417_RWD);
+		mem &= ~ALT_DATA;
+		mem |= (data & ALT_DATA);
+		cx_write(MC417_RWD, mem);
+	}
+
+	if (flag)
+		cx_set(MC417_RWD, ALT_AD_RG);/* ADDR */
+	else
+		cx_clear(MC417_RWD, ALT_AD_RG);/* VAL */
+
+	cx_clear(MC417_RWD, ALT_CS);/* ~CS */
+	if (read)
+		cx_clear(MC417_RWD, ALT_RD);
+	else
+		cx_clear(MC417_RWD, ALT_WR);
+
+	for (;;) {
+		mem = cx_read(MC417_RWD);
+		if ((mem & ALT_RDY) == 0)
+			break;
+		if (time_after(jiffies, timeout))
+			break;
+		udelay(1);
+	}
+
+	cx_set(MC417_RWD, ALT_RD | ALT_WR | ALT_CS);
+	if (read)
+		return mem & ALT_DATA;
+
+	return 0;
+};
 
 static int dvb_register(struct cx23885_tsport *port)
 {
 	struct cx23885_dev *dev = port->dev;
 	struct cx23885_i2c *i2c_bus = NULL, *i2c_bus2 = NULL;
-	struct videobuf_dvb_frontend *fe0;
+	struct videobuf_dvb_frontend *fe0, *fe1 = NULL;
+	int mfe_shared = 0; /* bus not shared by default */
 	int ret;
 
 	/* Get the first frontend */
@@ -586,6 +676,12 @@ static int dvb_register(struct cx23885_tsport *port)
 	/* init struct videobuf_dvb */
 	fe0->dvb.name = dev->name;
 
+	/* multi-frontend gate control is undefined or defaults to fe0 */
+	port->frontends.gate = 0;
+
+	/* Sets the gate control callback to be used by i2c command calls */
+	port->gate_ctrl = cx23885_dvb_gate_ctrl;
+
 	/* init frontend */
 	switch (dev->board) {
 	case CX23885_BOARD_HAUPPAUGE_HVR1250:
@@ -966,20 +1062,61 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
-
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
+		i2c_bus = &dev->i2c_bus[0];
+		mfe_shared = 1;/* MFE */
+		port->frontends.gate = 0;/* not clear for me yet */
+		/* ports B, C */
+		/* MFE frontend 1 DVB-T */
+		fe0->dvb.frontend = dvb_attach(stv0367ter_attach,
+					&netup_stv0367_config[port->nr - 1],
+					&i2c_bus->i2c_adap);
+		if (fe0->dvb.frontend != NULL)
+			if (NULL == dvb_attach(xc5000_attach,
+					fe0->dvb.frontend,
+					&i2c_bus->i2c_adap,
+					&netup_xc5000_config[port->nr - 1]))
+				goto frontend_detach;
+		/* MFE frontend 2 */
+		fe1 = videobuf_dvb_get_frontend(&port->frontends, 2);
+		if (fe1 == NULL)
+			goto frontend_detach;
+		/* DVB-C init */
+		fe1->dvb.frontend = dvb_attach(stv0367cab_attach,
+					&netup_stv0367_config[port->nr - 1],
+					&i2c_bus->i2c_adap);
+		if (fe1->dvb.frontend != NULL) {
+			fe1->dvb.frontend->id = 1;
+			if (NULL == dvb_attach(xc5000_attach,
+					fe1->dvb.frontend,
+					&i2c_bus->i2c_adap,
+					&netup_xc5000_config[port->nr - 1]))
+				goto frontend_detach;
+		}
+		break;
 	default:
 		printk(KERN_INFO "%s: The frontend of your DVB/ATSC card "
 			" isn't supported yet\n",
 		       dev->name);
 		break;
 	}
-	if (NULL == fe0->dvb.frontend) {
+
+	if ((NULL == fe0->dvb.frontend) || (fe1 && NULL == fe1->dvb.frontend)) {
 		printk(KERN_ERR "%s: frontend initialization failed\n",
-			dev->name);
-		return -1;
+		       dev->name);
+		goto frontend_detach;
 	}
+
 	/* define general-purpose callback pointer */
 	fe0->dvb.frontend->callback = cx23885_tuner_callback;
+	if (fe1)
+		fe1->dvb.frontend->callback = cx23885_tuner_callback;
+#if 0
+	/* Ensure all frontends negotiate bus access */
+	fe0->dvb.frontend->ops.ts_bus_ctrl = cx23885_dvb_bus_ctrl;
+	if (fe1)
+		fe1->dvb.frontend->ops.ts_bus_ctrl = cx23885_dvb_bus_ctrl;
+#endif
 
 	/* Put the analog decoder in standby to keep it quiet */
 	call_all(dev, core, s_power, 0);
@@ -989,10 +1126,10 @@ static int dvb_register(struct cx23885_tsport *port)
 
 	/* register everything */
 	ret = videobuf_dvb_register_bus(&port->frontends, THIS_MODULE, port,
-					&dev->pci->dev, adapter_nr, 0,
+					&dev->pci->dev, adapter_nr, mfe_shared,
 					cx23885_dvb_fe_ioctl_override);
 	if (ret)
-		return ret;
+		goto frontend_detach;
 
 	/* init CI & MAC */
 	switch (dev->board) {
@@ -1008,6 +1145,17 @@ static int dvb_register(struct cx23885_tsport *port)
 		netup_ci_init(port);
 		break;
 		}
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF: {
+		struct altera_ci_config netup_ci_cfg = {
+			.dev = dev,/* magic number to identify*/
+			.adapter = &port->frontends.adapter,/* for CI */
+			.demux = &fe0->dvb.demux,/* for hw pid filter */
+			.fpga_rw = netup_altera_fpga_rw,
+		};
+
+		altera_ci_init(&netup_ci_cfg, port->nr);
+		break;
+		}
 	case CX23885_BOARD_TEVII_S470: {
 		u8 eeprom[256]; /* 24C02 i2c eeprom */
 
@@ -1024,6 +1172,11 @@ static int dvb_register(struct cx23885_tsport *port)
 	}
 
 	return ret;
+
+frontend_detach:
+	port->gate_ctrl = NULL;
+	videobuf_dvb_dealloc_frontends(&port->frontends);
+	return -EINVAL;
 }
 
 int cx23885_dvb_register(struct cx23885_tsport *port)
@@ -1100,8 +1253,13 @@ int cx23885_dvb_unregister(struct cx23885_tsport *port)
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
 		netup_ci_exit(port);
 		break;
+	case CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF:
+		altera_ci_release(port->dev, port->nr);
+		break;
 	}
 
+	port->gate_ctrl = NULL;
+
 	return 0;
 }
 
diff --git a/drivers/media/video/cx23885/cx23885-i2c.c b/drivers/media/video/cx23885/cx23885-i2c.c
index ed3d8f55029b..307ff543c254 100644
--- a/drivers/media/video/cx23885/cx23885-i2c.c
+++ b/drivers/media/video/cx23885/cx23885-i2c.c
@@ -122,10 +122,6 @@ static int i2c_sendbytes(struct i2c_adapter *i2c_adap,
 
 	if (!i2c_wait_done(i2c_adap))
 		goto eio;
-	if (!i2c_slave_did_ack(i2c_adap)) {
-		retval = -ENXIO;
-		goto err;
-	}
 	if (i2c_debug) {
 		printk(" <W %02x %02x", msg->addr << 1, msg->buf[0]);
 		if (!(ctrl & I2C_NOSTOP))
@@ -158,7 +154,6 @@ static int i2c_sendbytes(struct i2c_adapter *i2c_adap,
 
  eio:
 	retval = -EIO;
- err:
 	if (i2c_debug)
 		printk(KERN_ERR " ERR: %d\n", retval);
 	return retval;
@@ -209,10 +204,6 @@ static int i2c_readbytes(struct i2c_adapter *i2c_adap,
 
 		if (!i2c_wait_done(i2c_adap))
 			goto eio;
-		if (cnt == 0 && !i2c_slave_did_ack(i2c_adap)) {
-			retval = -ENXIO;
-			goto err;
-		}
 		msg->buf[cnt] = cx_read(bus->reg_rdata) & 0xff;
 		if (i2c_debug) {
 			dprintk(1, " %02x", msg->buf[cnt]);
@@ -224,7 +215,6 @@ static int i2c_readbytes(struct i2c_adapter *i2c_adap,
 
  eio:
 	retval = -EIO;
- err:
 	if (i2c_debug)
 		printk(KERN_ERR " ERR: %d\n", retval);
 	return retval;
diff --git a/drivers/media/video/cx23885/cx23885-reg.h b/drivers/media/video/cx23885/cx23885-reg.h
index a28772db11f0..c87ac682ebbe 100644
--- a/drivers/media/video/cx23885/cx23885-reg.h
+++ b/drivers/media/video/cx23885/cx23885-reg.h
@@ -292,6 +292,7 @@ Channel manager Data Structure entry = 20 DWORD
 #define RDR_CFG0	0x00050000
 #define RDR_CFG1	0x00050004
 #define RDR_CFG2	0x00050008
+#define RDR_RDRCTL1	0x0005030c
 #define RDR_TLCTL0	0x00050318
 
 /* APB DMAC Current Buffer Pointer */
diff --git a/drivers/media/video/cx23885/cx23885-video.c b/drivers/media/video/cx23885/cx23885-video.c
index 644fcb808c0b..ee57f6bedbe3 100644
--- a/drivers/media/video/cx23885/cx23885-video.c
+++ b/drivers/media/video/cx23885/cx23885-video.c
@@ -1468,16 +1468,17 @@ int cx23885_video_register(struct cx23885_dev *dev)
 
 	cx23885_irq_add_enable(dev, 0x01);
 
-	if (TUNER_ABSENT != dev->tuner_type) {
+	if ((TUNER_ABSENT != dev->tuner_type) &&
+			((dev->tuner_bus == 0) || (dev->tuner_bus == 1))) {
 		struct v4l2_subdev *sd = NULL;
 
 		if (dev->tuner_addr)
 			sd = v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_bus[1].i2c_adap,
+				&dev->i2c_bus[dev->tuner_bus].i2c_adap,
 				"tuner", dev->tuner_addr, NULL);
 		else
 			sd = v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_bus[1].i2c_adap,
+				&dev->i2c_bus[dev->tuner_bus].i2c_adap,
 				"tuner", 0, v4l2_i2c_tuner_addrs(ADDRS_TV));
 		if (sd) {
 			struct tuner_setup tun_setup;
diff --git a/drivers/media/video/cx23885/cx23885.h b/drivers/media/video/cx23885/cx23885.h
index 62e41ab65810..8db2797bc7c3 100644
--- a/drivers/media/video/cx23885/cx23885.h
+++ b/drivers/media/video/cx23885/cx23885.h
@@ -85,6 +85,7 @@
 #define CX23885_BOARD_MYGICA_X8558PRO          27
 #define CX23885_BOARD_LEADTEK_WINFAST_PXTV1200 28
 #define CX23885_BOARD_GOTVIEW_X5_3D_HYBRID     29
+#define CX23885_BOARD_NETUP_DUAL_DVB_T_C_CI_RF 30
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
@@ -204,10 +205,12 @@ typedef enum {
 struct cx23885_board {
 	char                    *name;
 	port_t			porta, portb, portc;
+	int		num_fds_portb, num_fds_portc;
 	unsigned int		tuner_type;
 	unsigned int		radio_type;
 	unsigned char		tuner_addr;
 	unsigned char		radio_addr;
+	unsigned int		tuner_bus;
 
 	/* Vendors can and do run the PCIe bridge at different
 	 * clock rates, driven physically by crystals on the PCBs.
@@ -220,7 +223,7 @@ struct cx23885_board {
 	 */
 	u32			clk_freq;
 	struct cx23885_input    input[MAX_CX23885_INPUT];
-	int			cimax; /* for NetUP */
+	int			ci_type; /* for NetUP */
 };
 
 struct cx23885_subid {
@@ -303,6 +306,7 @@ struct cx23885_tsport {
 
 	/* Allow a single tsport to have multiple frontends */
 	u32                        num_frontends;
+	void                (*gate_ctrl)(struct cx23885_tsport *port, int open);
 	void                       *port_priv;
 };
 
@@ -362,6 +366,7 @@ struct cx23885_dev {
 	v4l2_std_id                tvnorm;
 	unsigned int               tuner_type;
 	unsigned char              tuner_addr;
+	unsigned int               tuner_bus;
 	unsigned int               radio_type;
 	unsigned char              radio_addr;
 	unsigned int               has_radio;
diff --git a/drivers/media/video/cx25840/cx25840-core.c b/drivers/media/video/cx25840/cx25840-core.c
index 6fc09dd41b9d..35796e035247 100644
--- a/drivers/media/video/cx25840/cx25840-core.c
+++ b/drivers/media/video/cx25840/cx25840-core.c
@@ -2015,7 +2015,8 @@ static int cx25840_probe(struct i2c_client *client,
 		kfree(state);
 		return err;
 	}
-	v4l2_ctrl_cluster(2, &state->volume);
+	if (!is_cx2583x(state))
+		v4l2_ctrl_cluster(2, &state->volume);
 	v4l2_ctrl_handler_setup(&state->hdl);
 
 	if (client->dev.platform_data) {
diff --git a/drivers/media/video/cx88/cx88-alsa.c b/drivers/media/video/cx88/cx88-alsa.c
index 54b7fcd469a8..a2d688ebed90 100644
--- a/drivers/media/video/cx88/cx88-alsa.c
+++ b/drivers/media/video/cx88/cx88-alsa.c
@@ -40,6 +40,7 @@
 #include <sound/control.h>
 #include <sound/initval.h>
 #include <sound/tlv.h>
+#include <media/wm8775.h>
 
 #include "cx88.h"
 #include "cx88-reg.h"
@@ -577,6 +578,35 @@ static int snd_cx88_volume_get(struct snd_kcontrol *kcontrol,
 	return 0;
 }
 
+static void snd_cx88_wm8775_volume_put(struct snd_kcontrol *kcontrol,
+			       struct snd_ctl_elem_value *value)
+{
+	snd_cx88_card_t *chip = snd_kcontrol_chip(kcontrol);
+	struct cx88_core *core = chip->core;
+	struct v4l2_control client_ctl;
+	int left = value->value.integer.value[0];
+	int right = value->value.integer.value[1];
+	int v, b;
+
+	memset(&client_ctl, 0, sizeof(client_ctl));
+
+	/* Pass volume & balance onto any WM8775 */
+	if (left >= right) {
+		v = left << 10;
+		b = left ? (0x8000 * right) / left : 0x8000;
+	} else {
+		v = right << 10;
+		b = right ? 0xffff - (0x8000 * left) / right : 0x8000;
+	}
+	client_ctl.value = v;
+	client_ctl.id = V4L2_CID_AUDIO_VOLUME;
+	call_hw(core, WM8775_GID, core, s_ctrl, &client_ctl);
+
+	client_ctl.value = b;
+	client_ctl.id = V4L2_CID_AUDIO_BALANCE;
+	call_hw(core, WM8775_GID, core, s_ctrl, &client_ctl);
+}
+
 /* OK - TODO: test it */
 static int snd_cx88_volume_put(struct snd_kcontrol *kcontrol,
 			       struct snd_ctl_elem_value *value)
@@ -587,25 +617,28 @@ static int snd_cx88_volume_put(struct snd_kcontrol *kcontrol,
 	int changed = 0;
 	u32 old;
 
+	if (core->board.audio_chip == V4L2_IDENT_WM8775)
+		snd_cx88_wm8775_volume_put(kcontrol, value);
+
 	left = value->value.integer.value[0] & 0x3f;
 	right = value->value.integer.value[1] & 0x3f;
 	b = right - left;
 	if (b < 0) {
-	    v = 0x3f - left;
-	    b = (-b) | 0x40;
+		v = 0x3f - left;
+		b = (-b) | 0x40;
 	} else {
-	    v = 0x3f - right;
+		v = 0x3f - right;
 	}
 	/* Do we really know this will always be called with IRQs on? */
 	spin_lock_irq(&chip->reg_lock);
 	old = cx_read(AUD_VOL_CTL);
 	if (v != (old & 0x3f)) {
-	    cx_write(AUD_VOL_CTL, (old & ~0x3f) | v);
-	    changed = 1;
+		cx_swrite(SHADOW_AUD_VOL_CTL, AUD_VOL_CTL, (old & ~0x3f) | v);
+		changed = 1;
 	}
-	if (cx_read(AUD_BAL_CTL) != b) {
-	    cx_write(AUD_BAL_CTL, b);
-	    changed = 1;
+	if ((cx_read(AUD_BAL_CTL) & 0x7f) != b) {
+		cx_write(AUD_BAL_CTL, b);
+		changed = 1;
 	}
 	spin_unlock_irq(&chip->reg_lock);
 
@@ -618,7 +651,7 @@ static const struct snd_kcontrol_new snd_cx88_volume = {
 	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
 	.access = SNDRV_CTL_ELEM_ACCESS_READWRITE |
 		  SNDRV_CTL_ELEM_ACCESS_TLV_READ,
-	.name = "Playback Volume",
+	.name = "Analog-TV Volume",
 	.info = snd_cx88_volume_info,
 	.get = snd_cx88_volume_get,
 	.put = snd_cx88_volume_put,
@@ -649,7 +682,17 @@ static int snd_cx88_switch_put(struct snd_kcontrol *kcontrol,
 	vol = cx_read(AUD_VOL_CTL);
 	if (value->value.integer.value[0] != !(vol & bit)) {
 		vol ^= bit;
-		cx_write(AUD_VOL_CTL, vol);
+		cx_swrite(SHADOW_AUD_VOL_CTL, AUD_VOL_CTL, vol);
+		/* Pass mute onto any WM8775 */
+		if ((core->board.audio_chip == V4L2_IDENT_WM8775) &&
+		    ((1<<6) == bit)) {
+			struct v4l2_control client_ctl;
+
+			memset(&client_ctl, 0, sizeof(client_ctl));
+			client_ctl.value = 0 != (vol & bit);
+			client_ctl.id = V4L2_CID_AUDIO_MUTE;
+			call_hw(core, WM8775_GID, core, s_ctrl, &client_ctl);
+		}
 		ret = 1;
 	}
 	spin_unlock_irq(&chip->reg_lock);
@@ -658,7 +701,7 @@ static int snd_cx88_switch_put(struct snd_kcontrol *kcontrol,
 
 static const struct snd_kcontrol_new snd_cx88_dac_switch = {
 	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name = "Playback Switch",
+	.name = "Audio-Out Switch",
 	.info = snd_ctl_boolean_mono_info,
 	.get = snd_cx88_switch_get,
 	.put = snd_cx88_switch_put,
@@ -667,13 +710,51 @@ static const struct snd_kcontrol_new snd_cx88_dac_switch = {
 
 static const struct snd_kcontrol_new snd_cx88_source_switch = {
 	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name = "Capture Switch",
+	.name = "Analog-TV Switch",
 	.info = snd_ctl_boolean_mono_info,
 	.get = snd_cx88_switch_get,
 	.put = snd_cx88_switch_put,
 	.private_value = (1<<6),
 };
 
+static int snd_cx88_alc_get(struct snd_kcontrol *kcontrol,
+			       struct snd_ctl_elem_value *value)
+{
+	snd_cx88_card_t *chip = snd_kcontrol_chip(kcontrol);
+	struct cx88_core *core = chip->core;
+	struct v4l2_control client_ctl;
+
+	memset(&client_ctl, 0, sizeof(client_ctl));
+	client_ctl.id = V4L2_CID_AUDIO_LOUDNESS;
+	call_hw(core, WM8775_GID, core, g_ctrl, &client_ctl);
+	value->value.integer.value[0] = client_ctl.value ? 1 : 0;
+
+	return 0;
+}
+
+static int snd_cx88_alc_put(struct snd_kcontrol *kcontrol,
+				       struct snd_ctl_elem_value *value)
+{
+	snd_cx88_card_t *chip = snd_kcontrol_chip(kcontrol);
+	struct cx88_core *core = chip->core;
+	struct v4l2_control client_ctl;
+
+	memset(&client_ctl, 0, sizeof(client_ctl));
+	client_ctl.value = 0 != value->value.integer.value[0];
+	client_ctl.id = V4L2_CID_AUDIO_LOUDNESS;
+	call_hw(core, WM8775_GID, core, s_ctrl, &client_ctl);
+
+	return 0;
+}
+
+static struct snd_kcontrol_new snd_cx88_alc_switch = {
+	.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "Line-In ALC Switch",
+	.info = snd_ctl_boolean_mono_info,
+	.get = snd_cx88_alc_get,
+	.put = snd_cx88_alc_put,
+};
+
 /****************************************************************************
 			Basic Flow for Sound Devices
  ****************************************************************************/
@@ -724,7 +805,8 @@ static void snd_cx88_dev_free(struct snd_card * card)
 static int devno;
 static int __devinit snd_cx88_create(struct snd_card *card,
 				     struct pci_dev *pci,
-				     snd_cx88_card_t **rchip)
+				     snd_cx88_card_t **rchip,
+				     struct cx88_core **core_ptr)
 {
 	snd_cx88_card_t   *chip;
 	struct cx88_core  *core;
@@ -750,7 +832,7 @@ static int __devinit snd_cx88_create(struct snd_card *card,
 	if (!pci_dma_supported(pci,DMA_BIT_MASK(32))) {
 		dprintk(0, "%s/1: Oops: no 32bit PCI DMA ???\n",core->name);
 		err = -EIO;
-		cx88_core_put(core,pci);
+		cx88_core_put(core, pci);
 		return err;
 	}
 
@@ -786,6 +868,7 @@ static int __devinit snd_cx88_create(struct snd_card *card,
 	snd_card_set_dev(card, &pci->dev);
 
 	*rchip = chip;
+	*core_ptr = core;
 
 	return 0;
 }
@@ -795,6 +878,7 @@ static int __devinit cx88_audio_initdev(struct pci_dev *pci,
 {
 	struct snd_card  *card;
 	snd_cx88_card_t  *chip;
+	struct cx88_core *core;
 	int              err;
 
 	if (devno >= SNDRV_CARDS)
@@ -812,7 +896,7 @@ static int __devinit cx88_audio_initdev(struct pci_dev *pci,
 
 	card->private_free = snd_cx88_dev_free;
 
-	err = snd_cx88_create(card, pci, &chip);
+	err = snd_cx88_create(card, pci, &chip, &core);
 	if (err < 0)
 		goto error;
 
@@ -830,6 +914,10 @@ static int __devinit cx88_audio_initdev(struct pci_dev *pci,
 	if (err < 0)
 		goto error;
 
+	/* If there's a wm8775 then add a Line-In ALC switch */
+	if (core->board.audio_chip == V4L2_IDENT_WM8775)
+		snd_ctl_add(card, snd_ctl_new1(&snd_cx88_alc_switch, chip));
+
 	strcpy (card->driver, "CX88x");
 	sprintf(card->shortname, "Conexant CX%x", pci->device);
 	sprintf(card->longname, "%s at %#llx",
diff --git a/drivers/media/video/cx88/cx88-cards.c b/drivers/media/video/cx88/cx88-cards.c
index 4e6ee5584cb3..27222c92b603 100644
--- a/drivers/media/video/cx88/cx88-cards.c
+++ b/drivers/media/video/cx88/cx88-cards.c
@@ -970,7 +970,8 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type	= UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.audio_chip = V4L2_IDENT_WM8775,
+		.audio_chip	= V4L2_IDENT_WM8775,
+		.i2sinputcntl   = 2,
 		.input		= {{
 			.type	= CX88_VMUX_DVB,
 			.vmux	= 0,
@@ -1952,6 +1953,18 @@ static const struct cx88_board cx88_boards[] = {
 		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
+	[CX88_BOARD_TEVII_S464] = {
+		.name           = "TeVii S464 DVB-S/S2",
+		.tuner_type     = UNSET,
+		.radio_type     = UNSET,
+		.tuner_addr     = ADDR_UNSET,
+		.radio_addr     = ADDR_UNSET,
+		.input          = {{
+			.type   = CX88_VMUX_DVB,
+			.vmux   = 0,
+		} },
+		.mpeg           = CX88_MPEG_DVB,
+	},
 	[CX88_BOARD_OMICOM_SS4_PCI] = {
 		.name           = "Omicom SS4 DVB-S/S2 PCI",
 		.tuner_type     = UNSET,
@@ -2528,6 +2541,10 @@ static const struct cx88_subid cx88_subids[] = {
 		.subdevice = 0x9022,
 		.card      = CX88_BOARD_TEVII_S460,
 	}, {
+		.subvendor = 0xd464,
+		.subdevice = 0x9022,
+		.card      = CX88_BOARD_TEVII_S464,
+	}, {
 		.subvendor = 0xA044,
 		.subdevice = 0x2011,
 		.card      = CX88_BOARD_OMICOM_SS4_PCI,
@@ -3165,9 +3182,7 @@ static void cx88_card_setup(struct cx88_core *core)
 {
 	static u8 eeprom[256];
 	struct tuner_setup tun_setup;
-	unsigned int mode_mask = T_RADIO     |
-				 T_ANALOG_TV |
-				 T_DIGITAL_TV;
+	unsigned int mode_mask = T_RADIO | T_ANALOG_TV;
 
 	memset(&tun_setup, 0, sizeof(tun_setup));
 
@@ -3287,6 +3302,7 @@ static void cx88_card_setup(struct cx88_core *core)
 	}
 	case  CX88_BOARD_TEVII_S420:
 	case  CX88_BOARD_TEVII_S460:
+	case  CX88_BOARD_TEVII_S464:
 	case  CX88_BOARD_OMICOM_SS4_PCI:
 	case  CX88_BOARD_TBS_8910:
 	case  CX88_BOARD_TBS_8920:
diff --git a/drivers/media/video/cx88/cx88-dvb.c b/drivers/media/video/cx88/cx88-dvb.c
index 90717ee944ec..7b8c9d3b6efc 100644
--- a/drivers/media/video/cx88/cx88-dvb.c
+++ b/drivers/media/video/cx88/cx88-dvb.c
@@ -57,6 +57,7 @@
 #include "stb6100.h"
 #include "stb6100_proc.h"
 #include "mb86a16.h"
+#include "ds3000.h"
 
 MODULE_DESCRIPTION("driver for cx2388x based DVB cards");
 MODULE_AUTHOR("Chris Pascoe <c.pascoe@itee.uq.edu.au>");
@@ -648,6 +649,20 @@ static const struct cx24116_config tevii_s460_config = {
 	.reset_device  = cx24116_reset_device,
 };
 
+static int ds3000_set_ts_param(struct dvb_frontend *fe,
+	int is_punctured)
+{
+	struct cx8802_dev *dev = fe->dvb->priv;
+	dev->ts_gen_cntrl = 4;
+
+	return 0;
+}
+
+static struct ds3000_config tevii_ds3000_config = {
+	.demod_address = 0x68,
+	.set_ts_params = ds3000_set_ts_param,
+};
+
 static const struct stv0900_config prof_7301_stv0900_config = {
 	.demod_address = 0x6a,
 /*	demod_mode = 0,*/
@@ -1381,6 +1396,14 @@ static int dvb_register(struct cx8802_dev *dev)
 		if (fe0->dvb.frontend != NULL)
 			fe0->dvb.frontend->ops.set_voltage = tevii_dvbs_set_voltage;
 		break;
+	case CX88_BOARD_TEVII_S464:
+		fe0->dvb.frontend = dvb_attach(ds3000_attach,
+						&tevii_ds3000_config,
+						&core->i2c_adap);
+		if (fe0->dvb.frontend != NULL)
+			fe0->dvb.frontend->ops.set_voltage =
+							tevii_dvbs_set_voltage;
+		break;
 	case CX88_BOARD_OMICOM_SS4_PCI:
 	case CX88_BOARD_TBS_8920:
 	case CX88_BOARD_PROF_7300:
diff --git a/drivers/media/video/cx88/cx88-input.c b/drivers/media/video/cx88/cx88-input.c
index 06f7d1d00944..fbfbba5bec5e 100644
--- a/drivers/media/video/cx88/cx88-input.c
+++ b/drivers/media/video/cx88/cx88-input.c
@@ -373,6 +373,7 @@ int cx88_ir_init(struct cx88_core *core, struct pci_dev *pci)
 		ir_codes = RC_MAP_TBS_NEC;
 		ir->sampling = 0xff00; /* address */
 		break;
+	case CX88_BOARD_TEVII_S464:
 	case CX88_BOARD_TEVII_S460:
 	case CX88_BOARD_TEVII_S420:
 		ir_codes = RC_MAP_TEVII_NEC;
diff --git a/drivers/media/video/cx88/cx88-tvaudio.c b/drivers/media/video/cx88/cx88-tvaudio.c
index 08220de3d74d..770ec05b5e9b 100644
--- a/drivers/media/video/cx88/cx88-tvaudio.c
+++ b/drivers/media/video/cx88/cx88-tvaudio.c
@@ -786,8 +786,12 @@ void cx88_set_tvaudio(struct cx88_core *core)
 		break;
 	case WW_I2SADC:
 		set_audio_start(core, 0x01);
-		/* Slave/Philips/Autobaud */
-		cx_write(AUD_I2SINPUTCNTL, 0);
+		/*
+		 * Slave/Philips/Autobaud
+		 * NB on Nova-S bit1 NPhilipsSony appears to be inverted:
+		 *	0= Sony, 1=Philips
+		 */
+		cx_write(AUD_I2SINPUTCNTL, core->board.i2sinputcntl);
 		/* Switch to "I2S ADC mode" */
 		cx_write(AUD_I2SCNTL, 0x1);
 		set_audio_finish(core, EN_I2SIN_ENABLE);
diff --git a/drivers/media/video/cx88/cx88-video.c b/drivers/media/video/cx88/cx88-video.c
index 508dabbed986..287a41ee1c4f 100644
--- a/drivers/media/video/cx88/cx88-video.c
+++ b/drivers/media/video/cx88/cx88-video.c
@@ -40,6 +40,7 @@
 #include "cx88.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/wm8775.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx2388x based TV cards");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
@@ -989,6 +990,32 @@ int cx88_set_control(struct cx88_core *core, struct v4l2_control *ctl)
 		ctl->value = c->v.minimum;
 	if (ctl->value > c->v.maximum)
 		ctl->value = c->v.maximum;
+
+	/* Pass changes onto any WM8775 */
+	if (core->board.audio_chip == V4L2_IDENT_WM8775) {
+		struct v4l2_control client_ctl;
+		memset(&client_ctl, 0, sizeof(client_ctl));
+		client_ctl.id = ctl->id;
+
+		switch (ctl->id) {
+		case V4L2_CID_AUDIO_MUTE:
+			client_ctl.value = ctl->value;
+			break;
+		case V4L2_CID_AUDIO_VOLUME:
+			client_ctl.value = (ctl->value) ?
+				(0x90 + ctl->value) << 8 : 0;
+			break;
+		case V4L2_CID_AUDIO_BALANCE:
+			client_ctl.value = ctl->value << 9;
+			break;
+		default:
+			client_ctl.id = 0;
+			break;
+		}
+		if (client_ctl.id)
+			call_hw(core, WM8775_GID, core, s_ctrl, &client_ctl);
+	}
+
 	mask=c->mask;
 	switch (ctl->id) {
 	case V4L2_CID_AUDIO_BALANCE:
@@ -1526,7 +1553,9 @@ static int radio_queryctrl (struct file *file, void *priv,
 	if (c->id <  V4L2_CID_BASE ||
 		c->id >= V4L2_CID_LASTP1)
 		return -EINVAL;
-	if (c->id == V4L2_CID_AUDIO_MUTE) {
+	if (c->id == V4L2_CID_AUDIO_MUTE ||
+		c->id == V4L2_CID_AUDIO_VOLUME ||
+		c->id == V4L2_CID_AUDIO_BALANCE) {
 		for (i = 0; i < CX8800_CTLS; i++) {
 			if (cx8800_ctls[i].v.id == c->id)
 				break;
@@ -1672,7 +1701,7 @@ static const struct v4l2_file_operations video_fops =
 	.read	       = video_read,
 	.poll          = video_poll,
 	.mmap	       = video_mmap,
-	.ioctl	       = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
@@ -1722,7 +1751,7 @@ static const struct v4l2_file_operations radio_fops =
 	.owner         = THIS_MODULE,
 	.open          = video_open,
 	.release       = video_release,
-	.ioctl         = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
@@ -1856,9 +1885,24 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 
 	/* load and configure helper modules */
 
-	if (core->board.audio_chip == V4L2_IDENT_WM8775)
-		v4l2_i2c_new_subdev(&core->v4l2_dev, &core->i2c_adap,
-				"wm8775", 0x36 >> 1, NULL);
+	if (core->board.audio_chip == V4L2_IDENT_WM8775) {
+		struct i2c_board_info wm8775_info = {
+			.type = "wm8775",
+			.addr = 0x36 >> 1,
+			.platform_data = &core->wm8775_data,
+		};
+		struct v4l2_subdev *sd;
+
+		if (core->boardnr == CX88_BOARD_HAUPPAUGE_NOVASPLUS_S1)
+			core->wm8775_data.is_nova_s = true;
+		else
+			core->wm8775_data.is_nova_s = false;
+
+		sd = v4l2_i2c_new_subdev_board(&core->v4l2_dev, &core->i2c_adap,
+				&wm8775_info, NULL);
+		if (sd != NULL)
+			sd->grp_id = WM8775_GID;
+	}
 
 	if (core->board.audio_chip == V4L2_IDENT_TVAUDIO) {
 		/* This probes for a tda9874 as is used on some
@@ -1882,6 +1926,15 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 		request_module("ir-kbd-i2c");
 	}
 
+	/* Sets device info at pci_dev */
+	pci_set_drvdata(pci_dev, dev);
+
+	/* initial device configuration */
+	mutex_lock(&core->lock);
+	cx88_set_tvnorm(core, core->tvnorm);
+	init_controls(core);
+	cx88_video_mux(core, 0);
+
 	/* register v4l devices */
 	dev->video_dev = cx88_vdev_init(core,dev->pci,
 					&cx8800_video_template,"video");
@@ -1923,16 +1976,6 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 		       core->name, video_device_node_name(dev->radio_dev));
 	}
 
-	/* everything worked */
-	pci_set_drvdata(pci_dev,dev);
-
-	/* initial device configuration */
-	mutex_lock(&core->lock);
-	cx88_set_tvnorm(core,core->tvnorm);
-	init_controls(core);
-	cx88_video_mux(core,0);
-	mutex_unlock(&core->lock);
-
 	/* start tvaudio thread */
 	if (core->board.tuner_type != TUNER_ABSENT) {
 		core->kthread = kthread_run(cx88_audio_thread, core, "cx88 tvaudio");
@@ -1942,11 +1985,14 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 			       core->name, err);
 		}
 	}
+	mutex_unlock(&core->lock);
+
 	return 0;
 
 fail_unreg:
 	cx8800_unregister_video(dev);
 	free_irq(pci_dev->irq, dev);
+	mutex_unlock(&core->lock);
 fail_core:
 	cx88_core_put(core,dev->pci);
 fail_free:
diff --git a/drivers/media/video/cx88/cx88.h b/drivers/media/video/cx88/cx88.h
index c9981e77416a..9b3742a7746c 100644
--- a/drivers/media/video/cx88/cx88.h
+++ b/drivers/media/video/cx88/cx88.h
@@ -33,6 +33,7 @@
 #include <media/cx2341x.h>
 #include <media/videobuf-dvb.h>
 #include <media/ir-kbd-i2c.h>
+#include <media/wm8775.h>
 
 #include "btcx-risc.h"
 #include "cx88-reg.h"
@@ -240,6 +241,7 @@ extern const struct sram_channel const cx88_sram_channels[];
 #define CX88_BOARD_PROF_7301               83
 #define CX88_BOARD_SAMSUNG_SMT_7020        84
 #define CX88_BOARD_TWINHAN_VP1027_DVBS     85
+#define CX88_BOARD_TEVII_S464              86
 
 enum cx88_itype {
 	CX88_VMUX_COMPOSITE1 = 1,
@@ -273,6 +275,9 @@ struct cx88_board {
 	enum cx88_board_type    mpeg;
 	unsigned int            audio_chip;
 	int			num_frontends;
+
+	/* Used for I2S devices */
+	int			i2sinputcntl;
 };
 
 struct cx88_subid {
@@ -379,6 +384,7 @@ struct cx88_core {
 
 	/* I2C remote data */
 	struct IR_i2c_init_data    init_data;
+	struct wm8775_platform_data wm8775_data;
 
 	struct mutex               lock;
 	/* various v4l controls */
@@ -398,17 +404,21 @@ static inline struct cx88_core *to_core(struct v4l2_device *v4l2_dev)
 	return container_of(v4l2_dev, struct cx88_core, v4l2_dev);
 }
 
-#define call_all(core, o, f, args...) 				\
+#define WM8775_GID	(1 << 0)
+
+#define call_hw(core, grpid, o, f, args...) \
 	do {							\
 		if (!core->i2c_rc) {				\
 			if (core->gate_ctrl)			\
 				core->gate_ctrl(core, 1);	\
-			v4l2_device_call_all(&core->v4l2_dev, 0, o, f, ##args); \
+			v4l2_device_call_all(&core->v4l2_dev, grpid, o, f, ##args); \
 			if (core->gate_ctrl)			\
 				core->gate_ctrl(core, 0);	\
 		}						\
 	} while (0)
 
+#define call_all(core, o, f, args...) call_hw(core, 0, o, f, ##args)
+
 struct cx8800_dev;
 struct cx8802_dev;
 
diff --git a/drivers/media/video/em28xx/em28xx-video.c b/drivers/media/video/em28xx/em28xx-video.c
index f34d524ccb09..a83131bd00b2 100644
--- a/drivers/media/video/em28xx/em28xx-video.c
+++ b/drivers/media/video/em28xx/em28xx-video.c
@@ -1387,6 +1387,27 @@ static int vidioc_queryctrl(struct file *file, void *priv,
 		return -EINVAL;
 }
 
+/*
+ * FIXME: This is an indirect way to check if a control exists at a
+ * subdev. Instead of that hack, maybe the better would be to change all
+ * subdevs to return -ENOIOCTLCMD, if an ioctl is not supported.
+ */
+static int check_subdev_ctrl(struct em28xx *dev, int id)
+{
+	struct v4l2_queryctrl qc;
+
+	memset(&qc, 0, sizeof(qc));
+	qc.id = id;
+
+	/* enumerate V4L2 device controls */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, core, queryctrl, &qc);
+
+	if (qc.type)
+		return 0;
+	else
+		return -EINVAL;
+}
+
 static int vidioc_g_ctrl(struct file *file, void *priv,
 				struct v4l2_control *ctrl)
 {
@@ -1399,7 +1420,6 @@ static int vidioc_g_ctrl(struct file *file, void *priv,
 		return rc;
 	rc = 0;
 
-
 	/* Set an AC97 control */
 	if (dev->audio_mode.ac97 != EM28XX_NO_AC97)
 		rc = ac97_get_ctrl(dev, ctrl);
@@ -1408,6 +1428,9 @@ static int vidioc_g_ctrl(struct file *file, void *priv,
 
 	/* It were not an AC97 control. Sends it to the v4l2 dev interface */
 	if (rc == 1) {
+		if (check_subdev_ctrl(dev, ctrl->id))
+			return -EINVAL;
+
 		v4l2_device_call_all(&dev->v4l2_dev, 0, core, g_ctrl, ctrl);
 		rc = 0;
 	}
@@ -1434,8 +1457,10 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 
 	/* It isn't an AC97 control. Sends it to the v4l2 dev interface */
 	if (rc == 1) {
-		rc = v4l2_device_call_until_err(&dev->v4l2_dev, 0, core, s_ctrl, ctrl);
-
+		rc = check_subdev_ctrl(dev, ctrl->id);
+		if (!rc)
+			v4l2_device_call_all(&dev->v4l2_dev, 0,
+					     core, s_ctrl, ctrl);
 		/*
 		 * In the case of non-AC97 volume controls, we still need
 		 * to do some setups at em28xx, in order to mute/unmute
@@ -1452,7 +1477,7 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 			rc = em28xx_audio_analog_set(dev);
 		}
 	}
-	return rc;
+	return (rc < 0) ? rc : 0;
 }
 
 static int vidioc_g_tuner(struct file *file, void *priv,
diff --git a/drivers/media/video/fsl-viu.c b/drivers/media/video/fsl-viu.c
index e4bba88254c7..031af1610154 100644
--- a/drivers/media/video/fsl-viu.c
+++ b/drivers/media/video/fsl-viu.c
@@ -1445,8 +1445,7 @@ static struct video_device viu_template = {
 	.current_norm   = V4L2_STD_NTSC_M,
 };
 
-static int __devinit viu_of_probe(struct platform_device *op,
-				  const struct of_device_id *match)
+static int __devinit viu_of_probe(struct platform_device *op)
 {
 	struct viu_dev *viu_dev;
 	struct video_device *vdev;
@@ -1627,7 +1626,7 @@ static struct of_device_id mpc512x_viu_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mpc512x_viu_of_match);
 
-static struct of_platform_driver viu_of_platform_driver = {
+static struct platform_driver viu_of_platform_driver = {
 	.probe = viu_of_probe,
 	.remove = __devexit_p(viu_of_remove),
 #ifdef CONFIG_PM
@@ -1643,12 +1642,12 @@ static struct of_platform_driver viu_of_platform_driver = {
 
 static int __init viu_init(void)
 {
-	return of_register_platform_driver(&viu_of_platform_driver);
+	return platform_driver_register(&viu_of_platform_driver);
 }
 
 static void __exit viu_exit(void)
 {
-	of_unregister_platform_driver(&viu_of_platform_driver);
+	platform_driver_unregister(&viu_of_platform_driver);
 }
 
 module_init(viu_init);
diff --git a/drivers/media/video/gspca/Kconfig b/drivers/media/video/gspca/Kconfig
index dda56ff834f4..a20f6ae88250 100644
--- a/drivers/media/video/gspca/Kconfig
+++ b/drivers/media/video/gspca/Kconfig
@@ -346,6 +346,16 @@ config USB_GSPCA_VC032X
 	  To compile this driver as a module, choose M here: the
 	  module will be called gspca_vc032x.
 
+config USB_GSPCA_VICAM
+	tristate "ViCam USB Camera Driver"
+	depends on VIDEO_V4L2 && USB_GSPCA
+	help
+	  Say Y here if you want support for the 3com homeconnect camera
+	  (vicam).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gspca_vicam.
+
 config USB_GSPCA_XIRLINK_CIT
 	tristate "Xirlink C-It USB Camera Driver"
 	depends on VIDEO_V4L2 && USB_GSPCA
diff --git a/drivers/media/video/gspca/Makefile b/drivers/media/video/gspca/Makefile
index 24e695b8b077..a0dbcfbab29c 100644
--- a/drivers/media/video/gspca/Makefile
+++ b/drivers/media/video/gspca/Makefile
@@ -34,6 +34,7 @@ obj-$(CONFIG_USB_GSPCA_STV0680)  += gspca_stv0680.o
 obj-$(CONFIG_USB_GSPCA_T613)     += gspca_t613.o
 obj-$(CONFIG_USB_GSPCA_TV8532)   += gspca_tv8532.o
 obj-$(CONFIG_USB_GSPCA_VC032X)   += gspca_vc032x.o
+obj-$(CONFIG_USB_GSPCA_VICAM)    += gspca_vicam.o
 obj-$(CONFIG_USB_GSPCA_XIRLINK_CIT) += gspca_xirlink_cit.o
 obj-$(CONFIG_USB_GSPCA_ZC3XX)    += gspca_zc3xx.o
 
@@ -73,6 +74,7 @@ gspca_sunplus-objs  := sunplus.o
 gspca_t613-objs     := t613.o
 gspca_tv8532-objs   := tv8532.o
 gspca_vc032x-objs   := vc032x.o
+gspca_vicam-objs    := vicam.o
 gspca_xirlink_cit-objs := xirlink_cit.o
 gspca_zc3xx-objs    := zc3xx.o
 
diff --git a/drivers/media/video/gspca/cpia1.c b/drivers/media/video/gspca/cpia1.c
index 4bf2cab98d64..2a4a428f2018 100644
--- a/drivers/media/video/gspca/cpia1.c
+++ b/drivers/media/video/gspca/cpia1.c
@@ -1,7 +1,7 @@
 /*
  * cpia CPiA (1) gspca driver
  *
- * Copyright (C) 2010 Hans de Goede <hdegoede@redhat.com>
+ * Copyright (C) 2010-2011 Hans de Goede <hdegoede@redhat.com>
  *
  * This module is adapted from the in kernel v4l1 cpia driver which is :
  *
@@ -1400,7 +1400,7 @@ static void monitor_exposure(struct gspca_dev *gspca_dev)
 		if ((sd->exposure_status == EXPOSURE_VERY_DARK ||
 		     sd->exposure_status == EXPOSURE_DARK) &&
 		    sd->exposure_count >= DARK_TIME * framerate &&
-		    sd->params.sensorFps.divisor < 3) {
+		    sd->params.sensorFps.divisor < 2) {
 
 			/* dark for too long */
 			++sd->params.sensorFps.divisor;
@@ -1456,7 +1456,7 @@ static void monitor_exposure(struct gspca_dev *gspca_dev)
 		if ((sd->exposure_status == EXPOSURE_VERY_DARK ||
 		     sd->exposure_status == EXPOSURE_DARK) &&
 		    sd->exposure_count >= DARK_TIME * framerate &&
-		    sd->params.sensorFps.divisor < 3) {
+		    sd->params.sensorFps.divisor < 2) {
 
 			/* dark for too long */
 			++sd->params.sensorFps.divisor;
diff --git a/drivers/media/video/gspca/ov519.c b/drivers/media/video/gspca/ov519.c
index 8ab2c452c25e..42670ec7ef3b 100644
--- a/drivers/media/video/gspca/ov519.c
+++ b/drivers/media/video/gspca/ov519.c
@@ -118,6 +118,7 @@ struct sd {
 };
 enum sensors {
 	SEN_OV2610,
+	SEN_OV2610AE,
 	SEN_OV3610,
 	SEN_OV6620,
 	SEN_OV6630,
@@ -239,6 +240,8 @@ static const struct ctrl sd_ctrls[] = {
 static const unsigned ctrl_dis[] = {
 [SEN_OV2610] =		(1 << NCTRL) - 1,	/* no control */
 
+[SEN_OV2610AE] =	(1 << NCTRL) - 1,	/* no control */
+
 [SEN_OV3610] =		(1 << NCTRL) - 1,	/* no control */
 
 [SEN_OV6620] =		(1 << HFLIP) |
@@ -428,6 +431,11 @@ static const struct v4l2_pix_format ovfx2_cif_mode[] = {
 		.priv = 0},
 };
 static const struct v4l2_pix_format ovfx2_ov2610_mode[] = {
+	{800, 600, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
+		.bytesperline = 800,
+		.sizeimage = 800 * 600,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.priv = 1},
 	{1600, 1200, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 1600,
 		.sizeimage = 1600 * 1200,
@@ -544,6 +552,7 @@ static const struct v4l2_pix_format ovfx2_ov3610_mode[] = {
  * buffers, there are some pretty strict real time constraints for
  * isochronous transfer for larger frame sizes).
  */
+/*jfm: this value works well for 1600x1200, but not 800x600 - see isoc_init */
 #define OVFX2_BULK_SIZE (13 * 4096)
 
 /* I2C registers */
@@ -656,6 +665,24 @@ static const struct ov_i2c_regvals norm_2610[] = {
 	{ 0x12, 0x80 },	/* reset */
 };
 
+static const struct ov_i2c_regvals norm_2610ae[] = {
+	{0x12, 0x80},	/* reset */
+	{0x13, 0xcd},
+	{0x09, 0x01},
+	{0x0d, 0x00},
+	{0x11, 0x80},
+	{0x12, 0x20},	/* 1600x1200 */
+	{0x33, 0x0c},
+	{0x35, 0x90},
+	{0x36, 0x37},
+/* ms-win traces */
+	{0x11, 0x83},	/* clock / 3 ? */
+	{0x2d, 0x00},	/* 60 Hz filter */
+	{0x24, 0xb0},	/* normal colors */
+	{0x25, 0x90},
+	{0x10, 0x43},
+};
+
 static const struct ov_i2c_regvals norm_3620b[] = {
 	/*
 	 * From the datasheet: "Note that after writing to register COMH
@@ -2621,6 +2648,9 @@ static void ov_hires_configure(struct sd *sd)
 	if (high == 0x96 && low == 0x40) {
 		PDEBUG(D_PROBE, "Sensor is an OV2610");
 		sd->sensor = SEN_OV2610;
+	} else if (high == 0x96 && low == 0x41) {
+		PDEBUG(D_PROBE, "Sensor is an OV2610AE");
+		sd->sensor = SEN_OV2610AE;
 	} else if (high == 0x36 && (low & 0x0f) == 0x00) {
 		PDEBUG(D_PROBE, "Sensor is an OV3610");
 		sd->sensor = SEN_OV3610;
@@ -3295,15 +3325,22 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		}
 		break;
 	case BRIDGE_OVFX2:
-		if (sd->sensor == SEN_OV2610) {
+		switch (sd->sensor) {
+		case SEN_OV2610:
+		case SEN_OV2610AE:
 			cam->cam_mode = ovfx2_ov2610_mode;
 			cam->nmodes = ARRAY_SIZE(ovfx2_ov2610_mode);
-		} else if (sd->sensor == SEN_OV3610) {
+			break;
+		case SEN_OV3610:
 			cam->cam_mode = ovfx2_ov3610_mode;
 			cam->nmodes = ARRAY_SIZE(ovfx2_ov3610_mode);
-		} else if (sd->sif) {
-			cam->cam_mode = ov519_sif_mode;
-			cam->nmodes = ARRAY_SIZE(ov519_sif_mode);
+			break;
+		default:
+			if (sd->sif) {
+				cam->cam_mode = ov519_sif_mode;
+				cam->nmodes = ARRAY_SIZE(ov519_sif_mode);
+			}
+			break;
 		}
 		break;
 	case BRIDGE_W9968CF:
@@ -3325,6 +3362,12 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		/* Enable autogain, autoexpo, awb, bandfilter */
 		i2c_w_mask(sd, 0x13, 0x27, 0x27);
 		break;
+	case SEN_OV2610AE:
+		write_i2c_regvals(sd, norm_2610ae, ARRAY_SIZE(norm_2610ae));
+
+		/* enable autoexpo */
+		i2c_w_mask(sd, 0x13, 0x05, 0x05);
+		break;
 	case SEN_OV3610:
 		write_i2c_regvals(sd, norm_3620b, ARRAY_SIZE(norm_3620b));
 
@@ -3397,6 +3440,22 @@ error:
 	return -EINVAL;
 }
 
+/* function called at start time before URB creation */
+static int sd_isoc_init(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	switch (sd->bridge) {
+	case BRIDGE_OVFX2:
+		if (gspca_dev->width == 1600)
+			gspca_dev->cam.bulk_size = OVFX2_BULK_SIZE;
+		else
+			gspca_dev->cam.bulk_size = 7 * 4096;
+		break;
+	}
+	return 0;
+}
+
 /* Set up the OV511/OV511+ with the given image parameters.
  *
  * Do not put any sensor-specific code in here (including I2C I/O functions)
@@ -3827,6 +3886,25 @@ static void mode_init_ov_sensor_regs(struct sd *sd)
 		i2c_w_mask(sd, 0x67, qvga ? 0xf0 : 0x90, 0xf0);
 		i2c_w_mask(sd, 0x74, qvga ? 0x20 : 0x00, 0x20);
 		return;
+	case SEN_OV2610AE: {
+		u8 v;
+
+		/* frame rates:
+		 *	10fps / 5 fps for 1600x1200
+		 *	40fps / 20fps for 800x600
+		 */
+		v = 80;
+		if (qvga) {
+			if (sd->frame_rate < 25)
+				v = 0x81;
+		} else {
+			if (sd->frame_rate < 10)
+				v = 0x81;
+		}
+		i2c_w(sd, 0x11, v);
+		i2c_w(sd, 0x12, qvga ? 0x60 : 0x20);
+		return;
+	    }
 	case SEN_OV3610:
 		if (qvga) {
 			xstart = (1040 - gspca_dev->width) / 2 + (0x1f << 4);
@@ -3975,6 +4053,7 @@ static void set_ov_sensor_window(struct sd *sd)
 	/* mode setup is fully handled in mode_init_ov_sensor_regs for these */
 	switch (sd->sensor) {
 	case SEN_OV2610:
+	case SEN_OV2610AE:
 	case SEN_OV3610:
 	case SEN_OV7670:
 		mode_init_ov_sensor_regs(sd);
@@ -4731,6 +4810,7 @@ static const struct sd_desc sd_desc = {
 	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
diff --git a/drivers/media/video/gspca/ov534.c b/drivers/media/video/gspca/ov534.c
index 04da22802736..0c6369b7fe18 100644
--- a/drivers/media/video/gspca/ov534.c
+++ b/drivers/media/video/gspca/ov534.c
@@ -1,5 +1,5 @@
 /*
- * ov534-ov772x gspca driver
+ * ov534-ov7xxx gspca driver
  *
  * Copyright (C) 2008 Antonio Ospite <ospite@studenti.unina.it>
  * Copyright (C) 2008 Jim Paris <jim@jtan.com>
@@ -49,54 +49,59 @@ MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");
 MODULE_DESCRIPTION("GSPCA/OV534 USB Camera Driver");
 MODULE_LICENSE("GPL");
 
+/* controls */
+enum e_ctrl {
+	BRIGHTNESS,
+	CONTRAST,
+	GAIN,
+	EXPOSURE,
+	AGC,
+	AWB,
+	AEC,
+	SHARPNESS,
+	HFLIP,
+	VFLIP,
+	COLORS,
+	LIGHTFREQ,
+	NCTRLS		/* number of controls */
+};
+
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
+
+	struct gspca_ctrl ctrls[NCTRLS];
+
 	__u32 last_pts;
 	u16 last_fid;
 	u8 frame_rate;
 
-	u8 brightness;
-	u8 contrast;
-	u8 gain;
-	u8 exposure;
-	u8 agc;
-	u8 awb;
-	u8 aec;
-	s8 sharpness;
-	u8 hflip;
-	u8 vflip;
-	u8 freqfltr;
+	u8 sensor;
+};
+enum sensors {
+	SENSOR_OV767x,
+	SENSOR_OV772x,
+	NSENSORS
 };
 
 /* V4L2 controls supported by the driver */
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
+static void setbrightness(struct gspca_dev *gspca_dev);
+static void setcontrast(struct gspca_dev *gspca_dev);
+static void setgain(struct gspca_dev *gspca_dev);
+static void setexposure(struct gspca_dev *gspca_dev);
 static int sd_setagc(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getagc(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setsharpness(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getsharpness(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setawb(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getawb(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setaec(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getaec(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setfreqfltr(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getfreqfltr(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-		struct v4l2_querymenu *menu);
+static void setawb(struct gspca_dev *gspca_dev);
+static void setaec(struct gspca_dev *gspca_dev);
+static void setsharpness(struct gspca_dev *gspca_dev);
+static void sethvflip(struct gspca_dev *gspca_dev);
+static void setcolors(struct gspca_dev *gspca_dev);
+static void setlightfreq(struct gspca_dev *gspca_dev);
+
+static int sd_start(struct gspca_dev *gspca_dev);
+static void sd_stopN(struct gspca_dev *gspca_dev);
 
 static const struct ctrl sd_ctrls[] = {
-	{	/* 0 */
+[BRIGHTNESS] = {
 		{
 			.id      = V4L2_CID_BRIGHTNESS,
 			.type    = V4L2_CTRL_TYPE_INTEGER,
@@ -104,13 +109,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 255,
 			.step    = 1,
-#define BRIGHTNESS_DEF 0
-			.default_value = BRIGHTNESS_DEF,
+			.default_value = 0,
 		},
-		.set = sd_setbrightness,
-		.get = sd_getbrightness,
+		.set_control = setbrightness
 	},
-	{	/* 1 */
+[CONTRAST] = {
 		{
 			.id      = V4L2_CID_CONTRAST,
 			.type    = V4L2_CTRL_TYPE_INTEGER,
@@ -118,13 +121,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 255,
 			.step    = 1,
-#define CONTRAST_DEF 32
-			.default_value = CONTRAST_DEF,
+			.default_value = 32,
 		},
-		.set = sd_setcontrast,
-		.get = sd_getcontrast,
+		.set_control = setcontrast
 	},
-	{	/* 2 */
+[GAIN] = {
 		{
 			.id      = V4L2_CID_GAIN,
 			.type    = V4L2_CTRL_TYPE_INTEGER,
@@ -132,13 +133,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 63,
 			.step    = 1,
-#define GAIN_DEF 20
-			.default_value = GAIN_DEF,
+			.default_value = 20,
 		},
-		.set = sd_setgain,
-		.get = sd_getgain,
+		.set_control = setgain
 	},
-	{	/* 3 */
+[EXPOSURE] = {
 		{
 			.id      = V4L2_CID_EXPOSURE,
 			.type    = V4L2_CTRL_TYPE_INTEGER,
@@ -146,13 +145,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 255,
 			.step    = 1,
-#define EXPO_DEF 120
-			.default_value = EXPO_DEF,
+			.default_value = 120,
 		},
-		.set = sd_setexposure,
-		.get = sd_getexposure,
+		.set_control = setexposure
 	},
-	{	/* 4 */
+[AGC] = {
 		{
 			.id      = V4L2_CID_AUTOGAIN,
 			.type    = V4L2_CTRL_TYPE_BOOLEAN,
@@ -160,14 +157,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 1,
 			.step    = 1,
-#define AGC_DEF 1
-			.default_value = AGC_DEF,
+			.default_value = 1,
 		},
-		.set = sd_setagc,
-		.get = sd_getagc,
+		.set = sd_setagc
 	},
-#define AWB_IDX 5
-	{	/* 5 */
+[AWB] = {
 		{
 			.id      = V4L2_CID_AUTO_WHITE_BALANCE,
 			.type    = V4L2_CTRL_TYPE_BOOLEAN,
@@ -175,13 +169,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 1,
 			.step    = 1,
-#define AWB_DEF 1
-			.default_value = AWB_DEF,
+			.default_value = 1,
 		},
-		.set = sd_setawb,
-		.get = sd_getawb,
+		.set_control = setawb
 	},
-	{	/* 6 */
+[AEC] = {
 		{
 			.id      = V4L2_CID_EXPOSURE_AUTO,
 			.type    = V4L2_CTRL_TYPE_BOOLEAN,
@@ -189,13 +181,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 1,
 			.step    = 1,
-#define AEC_DEF 1
-			.default_value = AEC_DEF,
+			.default_value = 1,
 		},
-		.set = sd_setaec,
-		.get = sd_getaec,
+		.set_control = setaec
 	},
-	{	/* 7 */
+[SHARPNESS] = {
 		{
 			.id      = V4L2_CID_SHARPNESS,
 			.type    = V4L2_CTRL_TYPE_INTEGER,
@@ -203,13 +193,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 63,
 			.step    = 1,
-#define SHARPNESS_DEF 0
-			.default_value = SHARPNESS_DEF,
+			.default_value = 0,
 		},
-		.set = sd_setsharpness,
-		.get = sd_getsharpness,
+		.set_control = setsharpness
 	},
-	{	/* 8 */
+[HFLIP] = {
 		{
 			.id      = V4L2_CID_HFLIP,
 			.type    = V4L2_CTRL_TYPE_BOOLEAN,
@@ -217,13 +205,11 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 1,
 			.step    = 1,
-#define HFLIP_DEF 0
-			.default_value = HFLIP_DEF,
+			.default_value = 0,
 		},
-		.set = sd_sethflip,
-		.get = sd_gethflip,
+		.set_control = sethvflip
 	},
-	{	/* 9 */
+[VFLIP] = {
 		{
 			.id      = V4L2_CID_VFLIP,
 			.type    = V4L2_CTRL_TYPE_BOOLEAN,
@@ -231,13 +217,23 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 1,
 			.step    = 1,
-#define VFLIP_DEF 0
-			.default_value = VFLIP_DEF,
+			.default_value = 0,
 		},
-		.set = sd_setvflip,
-		.get = sd_getvflip,
+		.set_control = sethvflip
 	},
-	{	/* 10 */
+[COLORS] = {
+		{
+			.id      = V4L2_CID_SATURATION,
+			.type    = V4L2_CTRL_TYPE_INTEGER,
+			.name    = "Saturation",
+			.minimum = 0,
+			.maximum = 6,
+			.step    = 1,
+			.default_value = 3,
+		},
+		.set_control = setcolors
+	},
+[LIGHTFREQ] = {
 		{
 			.id      = V4L2_CID_POWER_LINE_FREQUENCY,
 			.type    = V4L2_CTRL_TYPE_MENU,
@@ -245,11 +241,9 @@ static const struct ctrl sd_ctrls[] = {
 			.minimum = 0,
 			.maximum = 1,
 			.step    = 1,
-#define FREQFLTR_DEF 0
-			.default_value = FREQFLTR_DEF,
+			.default_value = 0,
 		},
-		.set = sd_setfreqfltr,
-		.get = sd_getfreqfltr,
+		.set_control = setlightfreq
 	},
 };
 
@@ -265,6 +259,16 @@ static const struct v4l2_pix_format ov772x_mode[] = {
 	 .colorspace = V4L2_COLORSPACE_SRGB,
 	 .priv = 0},
 };
+static const struct v4l2_pix_format ov767x_mode[] = {
+	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
+		.bytesperline = 320,
+		.sizeimage = 320 * 240 * 3 / 8 + 590,
+		.colorspace = V4L2_COLORSPACE_JPEG},
+	{640, 480, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
+		.bytesperline = 640,
+		.sizeimage = 640 * 480 * 3 / 8 + 590,
+		.colorspace = V4L2_COLORSPACE_JPEG},
+};
 
 static const u8 qvga_rates[] = {125, 100, 75, 60, 50, 40, 30};
 static const u8 vga_rates[] = {60, 50, 40, 30, 15};
@@ -280,7 +284,288 @@ static const struct framerates ov772x_framerates[] = {
 	},
 };
 
-static const u8 bridge_init[][2] = {
+struct reg_array {
+	const u8 (*val)[2];
+	int len;
+};
+
+static const u8 bridge_init_767x[][2] = {
+/* comments from the ms-win file apollo7670.set */
+/* str1 */
+	{0xf1, 0x42},
+	{0x88, 0xf8},
+	{0x89, 0xff},
+	{0x76, 0x03},
+	{0x92, 0x03},
+	{0x95, 0x10},
+	{0xe2, 0x00},
+	{0xe7, 0x3e},
+	{0x8d, 0x1c},
+	{0x8e, 0x00},
+	{0x8f, 0x00},
+	{0x1f, 0x00},
+	{0xc3, 0xf9},
+	{0x89, 0xff},
+	{0x88, 0xf8},
+	{0x76, 0x03},
+	{0x92, 0x01},
+	{0x93, 0x18},
+	{0x1c, 0x00},
+	{0x1d, 0x48},
+	{0x1d, 0x00},
+	{0x1d, 0xff},
+	{0x1d, 0x02},
+	{0x1d, 0x58},
+	{0x1d, 0x00},
+	{0x1c, 0x0a},
+	{0x1d, 0x0a},
+	{0x1d, 0x0e},
+	{0xc0, 0x50},	/* HSize 640 */
+	{0xc1, 0x3c},	/* VSize 480 */
+	{0x34, 0x05},	/* enable Audio Suspend mode */
+	{0xc2, 0x0c},	/* Input YUV */
+	{0xc3, 0xf9},	/* enable PRE */
+	{0x34, 0x05},	/* enable Audio Suspend mode */
+	{0xe7, 0x2e},	/* this solves failure of "SuspendResumeTest" */
+	{0x31, 0xf9},	/* enable 1.8V Suspend */
+	{0x35, 0x02},	/* turn on JPEG */
+	{0xd9, 0x10},
+	{0x25, 0x42},	/* GPIO[8]:Input */
+	{0x94, 0x11},	/* If the default setting is loaded when
+			 * system boots up, this flag is closed here */
+};
+static const u8 sensor_init_767x[][2] = {
+	{0x12, 0x80},
+	{0x11, 0x03},
+	{0x3a, 0x04},
+	{0x12, 0x00},
+	{0x17, 0x13},
+	{0x18, 0x01},
+	{0x32, 0xb6},
+	{0x19, 0x02},
+	{0x1a, 0x7a},
+	{0x03, 0x0a},
+	{0x0c, 0x00},
+	{0x3e, 0x00},
+	{0x70, 0x3a},
+	{0x71, 0x35},
+	{0x72, 0x11},
+	{0x73, 0xf0},
+	{0xa2, 0x02},
+	{0x7a, 0x2a},	/* set Gamma=1.6 below */
+	{0x7b, 0x12},
+	{0x7c, 0x1d},
+	{0x7d, 0x2d},
+	{0x7e, 0x45},
+	{0x7f, 0x50},
+	{0x80, 0x59},
+	{0x81, 0x62},
+	{0x82, 0x6b},
+	{0x83, 0x73},
+	{0x84, 0x7b},
+	{0x85, 0x8a},
+	{0x86, 0x98},
+	{0x87, 0xb2},
+	{0x88, 0xca},
+	{0x89, 0xe0},
+	{0x13, 0xe0},
+	{0x00, 0x00},
+	{0x10, 0x00},
+	{0x0d, 0x40},
+	{0x14, 0x38},	/* gain max 16x */
+	{0xa5, 0x05},
+	{0xab, 0x07},
+	{0x24, 0x95},
+	{0x25, 0x33},
+	{0x26, 0xe3},
+	{0x9f, 0x78},
+	{0xa0, 0x68},
+	{0xa1, 0x03},
+	{0xa6, 0xd8},
+	{0xa7, 0xd8},
+	{0xa8, 0xf0},
+	{0xa9, 0x90},
+	{0xaa, 0x94},
+	{0x13, 0xe5},
+	{0x0e, 0x61},
+	{0x0f, 0x4b},
+	{0x16, 0x02},
+	{0x21, 0x02},
+	{0x22, 0x91},
+	{0x29, 0x07},
+	{0x33, 0x0b},
+	{0x35, 0x0b},
+	{0x37, 0x1d},
+	{0x38, 0x71},
+	{0x39, 0x2a},
+	{0x3c, 0x78},
+	{0x4d, 0x40},
+	{0x4e, 0x20},
+	{0x69, 0x00},
+	{0x6b, 0x4a},
+	{0x74, 0x10},
+	{0x8d, 0x4f},
+	{0x8e, 0x00},
+	{0x8f, 0x00},
+	{0x90, 0x00},
+	{0x91, 0x00},
+	{0x96, 0x00},
+	{0x9a, 0x80},
+	{0xb0, 0x84},
+	{0xb1, 0x0c},
+	{0xb2, 0x0e},
+	{0xb3, 0x82},
+	{0xb8, 0x0a},
+	{0x43, 0x0a},
+	{0x44, 0xf0},
+	{0x45, 0x34},
+	{0x46, 0x58},
+	{0x47, 0x28},
+	{0x48, 0x3a},
+	{0x59, 0x88},
+	{0x5a, 0x88},
+	{0x5b, 0x44},
+	{0x5c, 0x67},
+	{0x5d, 0x49},
+	{0x5e, 0x0e},
+	{0x6c, 0x0a},
+	{0x6d, 0x55},
+	{0x6e, 0x11},
+	{0x6f, 0x9f},
+	{0x6a, 0x40},
+	{0x01, 0x40},
+	{0x02, 0x40},
+	{0x13, 0xe7},
+	{0x4f, 0x80},
+	{0x50, 0x80},
+	{0x51, 0x00},
+	{0x52, 0x22},
+	{0x53, 0x5e},
+	{0x54, 0x80},
+	{0x58, 0x9e},
+	{0x41, 0x08},
+	{0x3f, 0x00},
+	{0x75, 0x04},
+	{0x76, 0xe1},
+	{0x4c, 0x00},
+	{0x77, 0x01},
+	{0x3d, 0xc2},
+	{0x4b, 0x09},
+	{0xc9, 0x60},
+	{0x41, 0x38},	/* jfm: auto sharpness + auto de-noise  */
+	{0x56, 0x40},
+	{0x34, 0x11},
+	{0x3b, 0xc2},
+	{0xa4, 0x8a},	/* Night mode trigger point */
+	{0x96, 0x00},
+	{0x97, 0x30},
+	{0x98, 0x20},
+	{0x99, 0x20},
+	{0x9a, 0x84},
+	{0x9b, 0x29},
+	{0x9c, 0x03},
+	{0x9d, 0x4c},
+	{0x9e, 0x3f},
+	{0x78, 0x04},
+	{0x79, 0x01},
+	{0xc8, 0xf0},
+	{0x79, 0x0f},
+	{0xc8, 0x00},
+	{0x79, 0x10},
+	{0xc8, 0x7e},
+	{0x79, 0x0a},
+	{0xc8, 0x80},
+	{0x79, 0x0b},
+	{0xc8, 0x01},
+	{0x79, 0x0c},
+	{0xc8, 0x0f},
+	{0x79, 0x0d},
+	{0xc8, 0x20},
+	{0x79, 0x09},
+	{0xc8, 0x80},
+	{0x79, 0x02},
+	{0xc8, 0xc0},
+	{0x79, 0x03},
+	{0xc8, 0x20},
+	{0x79, 0x26},
+};
+static const u8 bridge_start_vga_767x[][2] = {
+/* str59 JPG */
+	{0x94, 0xaa},
+	{0xf1, 0x42},
+	{0xe5, 0x04},
+	{0xc0, 0x50},
+	{0xc1, 0x3c},
+	{0xc2, 0x0c},
+	{0x35, 0x02},	/* turn on JPEG */
+	{0xd9, 0x10},
+	{0xda, 0x00},	/* for higher clock rate(30fps) */
+	{0x34, 0x05},	/* enable Audio Suspend mode */
+	{0xc3, 0xf9},	/* enable PRE */
+	{0x8c, 0x00},	/* CIF VSize LSB[2:0] */
+	{0x8d, 0x1c},	/* output YUV */
+/*	{0x34, 0x05},	 * enable Audio Suspend mode (?) */
+	{0x50, 0x00},	/* H/V divider=0 */
+	{0x51, 0xa0},	/* input H=640/4 */
+	{0x52, 0x3c},	/* input V=480/4 */
+	{0x53, 0x00},	/* offset X=0 */
+	{0x54, 0x00},	/* offset Y=0 */
+	{0x55, 0x00},	/* H/V size[8]=0 */
+	{0x57, 0x00},	/* H-size[9]=0 */
+	{0x5c, 0x00},	/* output size[9:8]=0 */
+	{0x5a, 0xa0},	/* output H=640/4 */
+	{0x5b, 0x78},	/* output V=480/4 */
+	{0x1c, 0x0a},
+	{0x1d, 0x0a},
+	{0x94, 0x11},
+};
+static const u8 sensor_start_vga_767x[][2] = {
+	{0x11, 0x01},
+	{0x1e, 0x04},
+	{0x19, 0x02},
+	{0x1a, 0x7a},
+};
+static const u8 bridge_start_qvga_767x[][2] = {
+/* str86 JPG */
+	{0x94, 0xaa},
+	{0xf1, 0x42},
+	{0xe5, 0x04},
+	{0xc0, 0x80},
+	{0xc1, 0x60},
+	{0xc2, 0x0c},
+	{0x35, 0x02},	/* turn on JPEG */
+	{0xd9, 0x10},
+	{0xc0, 0x50},	/* CIF HSize 640 */
+	{0xc1, 0x3c},	/* CIF VSize 480 */
+	{0x8c, 0x00},	/* CIF VSize LSB[2:0] */
+	{0x8d, 0x1c},	/* output YUV */
+	{0x34, 0x05},	/* enable Audio Suspend mode */
+	{0xc2, 0x4c},	/* output YUV and Enable DCW */
+	{0xc3, 0xf9},	/* enable PRE */
+	{0x1c, 0x00},	/* indirect addressing */
+	{0x1d, 0x48},	/* output YUV422 */
+	{0x50, 0x89},	/* H/V divider=/2; plus DCW AVG */
+	{0x51, 0xa0},	/* DCW input H=640/4 */
+	{0x52, 0x78},	/* DCW input V=480/4 */
+	{0x53, 0x00},	/* offset X=0 */
+	{0x54, 0x00},	/* offset Y=0 */
+	{0x55, 0x00},	/* H/V size[8]=0 */
+	{0x57, 0x00},	/* H-size[9]=0 */
+	{0x5c, 0x00},	/* DCW output size[9:8]=0 */
+	{0x5a, 0x50},	/* DCW output H=320/4 */
+	{0x5b, 0x3c},	/* DCW output V=240/4 */
+	{0x1c, 0x0a},
+	{0x1d, 0x0a},
+	{0x94, 0x11},
+};
+static const u8 sensor_start_qvga_767x[][2] = {
+	{0x11, 0x01},
+	{0x1e, 0x04},
+	{0x19, 0x02},
+	{0x1a, 0x7a},
+};
+
+static const u8 bridge_init_772x[][2] = {
 	{ 0xc2, 0x0c },
 	{ 0x88, 0xf8 },
 	{ 0xc3, 0x69 },
@@ -338,7 +623,7 @@ static const u8 bridge_init[][2] = {
 	{ 0xc1, 0x3c },
 	{ 0xc2, 0x0c },
 };
-static const u8 sensor_init[][2] = {
+static const u8 sensor_init_772x[][2] = {
 	{ 0x12, 0x80 },
 	{ 0x11, 0x01 },
 /*fixme: better have a delay?*/
@@ -431,7 +716,7 @@ static const u8 sensor_init[][2] = {
 	{ 0x8e, 0x00 },		/* De-noise threshold */
 	{ 0x0c, 0xd0 }
 };
-static const u8 bridge_start_vga[][2] = {
+static const u8 bridge_start_vga_772x[][2] = {
 	{0x1c, 0x00},
 	{0x1d, 0x40},
 	{0x1d, 0x02},
@@ -442,7 +727,7 @@ static const u8 bridge_start_vga[][2] = {
 	{0xc0, 0x50},
 	{0xc1, 0x3c},
 };
-static const u8 sensor_start_vga[][2] = {
+static const u8 sensor_start_vga_772x[][2] = {
 	{0x12, 0x00},
 	{0x17, 0x26},
 	{0x18, 0xa0},
@@ -452,7 +737,7 @@ static const u8 sensor_start_vga[][2] = {
 	{0x2c, 0xf0},
 	{0x65, 0x20},
 };
-static const u8 bridge_start_qvga[][2] = {
+static const u8 bridge_start_qvga_772x[][2] = {
 	{0x1c, 0x00},
 	{0x1d, 0x40},
 	{0x1d, 0x02},
@@ -463,7 +748,7 @@ static const u8 bridge_start_qvga[][2] = {
 	{0xc0, 0x28},
 	{0xc1, 0x1e},
 };
-static const u8 sensor_start_qvga[][2] = {
+static const u8 sensor_start_qvga_772x[][2] = {
 	{0x12, 0x40},
 	{0x17, 0x3f},
 	{0x18, 0x50},
@@ -646,6 +931,8 @@ static void set_frame_rate(struct gspca_dev *gspca_dev)
 		{30, 0x04, 0x41, 0x04},
 	};
 
+	if (sd->sensor != SENSOR_OV772x)
+		return;
 	if (gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv == 0) {
 		r = rate_0;
 		i = ARRAY_SIZE(rate_0);
@@ -669,15 +956,28 @@ static void set_frame_rate(struct gspca_dev *gspca_dev)
 static void setbrightness(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	int val;
 
-	sccb_reg_write(gspca_dev, 0x9b, sd->brightness);
+	val = sd->ctrls[BRIGHTNESS].val;
+	if (sd->sensor == SENSOR_OV767x) {
+		if (val < 0)
+			val = 0x80 - val;
+		sccb_reg_write(gspca_dev, 0x55, val);	/* bright */
+	} else {
+		sccb_reg_write(gspca_dev, 0x9b, val);
+	}
 }
 
 static void setcontrast(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 val;
 
-	sccb_reg_write(gspca_dev, 0x9c, sd->contrast);
+	val = sd->ctrls[CONTRAST].val;
+	if (sd->sensor == SENSOR_OV767x)
+		sccb_reg_write(gspca_dev, 0x56, val);	/* contras */
+	else
+		sccb_reg_write(gspca_dev, 0x9c, val);
 }
 
 static void setgain(struct gspca_dev *gspca_dev)
@@ -685,10 +985,10 @@ static void setgain(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
 
-	if (sd->agc)
+	if (sd->ctrls[AGC].val)
 		return;
 
-	val = sd->gain;
+	val = sd->ctrls[GAIN].val;
 	switch (val & 0x30) {
 	case 0x00:
 		val &= 0x0f;
@@ -715,25 +1015,32 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
 
-	if (sd->aec)
+	if (sd->ctrls[AEC].val)
 		return;
 
-	/* 'val' is one byte and represents half of the exposure value we are
-	 * going to set into registers, a two bytes value:
-	 *
-	 *    MSB: ((u16) val << 1) >> 8   == val >> 7
-	 *    LSB: ((u16) val << 1) & 0xff == val << 1
-	 */
-	val = sd->exposure;
-	sccb_reg_write(gspca_dev, 0x08, val >> 7);
-	sccb_reg_write(gspca_dev, 0x10, val << 1);
+	val = sd->ctrls[EXPOSURE].val;
+	if (sd->sensor == SENSOR_OV767x) {
+
+		/* set only aec[9:2] */
+		sccb_reg_write(gspca_dev, 0x10, val);	/* aech */
+	} else {
+
+		/* 'val' is one byte and represents half of the exposure value
+		 * we are going to set into registers, a two bytes value:
+		 *
+		 *    MSB: ((u16) val << 1) >> 8   == val >> 7
+		 *    LSB: ((u16) val << 1) & 0xff == val << 1
+		 */
+		sccb_reg_write(gspca_dev, 0x08, val >> 7);
+		sccb_reg_write(gspca_dev, 0x10, val << 1);
+	}
 }
 
 static void setagc(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->agc) {
+	if (sd->ctrls[AGC].val) {
 		sccb_reg_write(gspca_dev, 0x13,
 				sccb_reg_read(gspca_dev, 0x13) | 0x04);
 		sccb_reg_write(gspca_dev, 0x64,
@@ -752,15 +1059,17 @@ static void setawb(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->awb) {
+	if (sd->ctrls[AWB].val) {
 		sccb_reg_write(gspca_dev, 0x13,
 				sccb_reg_read(gspca_dev, 0x13) | 0x02);
-		sccb_reg_write(gspca_dev, 0x63,
+		if (sd->sensor == SENSOR_OV772x)
+			sccb_reg_write(gspca_dev, 0x63,
 				sccb_reg_read(gspca_dev, 0x63) | 0xc0);
 	} else {
 		sccb_reg_write(gspca_dev, 0x13,
 				sccb_reg_read(gspca_dev, 0x13) & ~0x02);
-		sccb_reg_write(gspca_dev, 0x63,
+		if (sd->sensor == SENSOR_OV772x)
+			sccb_reg_write(gspca_dev, 0x63,
 				sccb_reg_read(gspca_dev, 0x63) & ~0xc0);
 	}
 }
@@ -768,14 +1077,22 @@ static void setawb(struct gspca_dev *gspca_dev)
 static void setaec(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 data;
 
-	if (sd->aec)
+	data = sd->sensor == SENSOR_OV767x ?
+			0x05 :		/* agc + aec */
+			0x01;		/* agc */
+	if (sd->ctrls[AEC].val)
 		sccb_reg_write(gspca_dev, 0x13,
-				sccb_reg_read(gspca_dev, 0x13) | 0x01);
+				sccb_reg_read(gspca_dev, 0x13) | data);
 	else {
 		sccb_reg_write(gspca_dev, 0x13,
-				sccb_reg_read(gspca_dev, 0x13) & ~0x01);
-		setexposure(gspca_dev);
+				sccb_reg_read(gspca_dev, 0x13) & ~data);
+		if (sd->sensor == SENSOR_OV767x)
+			sd->ctrls[EXPOSURE].val =
+				sccb_reg_read(gspca_dev, 10);	/* aech */
+		else
+			setexposure(gspca_dev);
 	}
 }
 
@@ -784,43 +1101,67 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
 
-	val = sd->sharpness;
+	val = sd->ctrls[SHARPNESS].val;
 	sccb_reg_write(gspca_dev, 0x91, val);	/* Auto de-noise threshold */
 	sccb_reg_write(gspca_dev, 0x8e, val);	/* De-noise threshold */
 }
 
-static void sethflip(struct gspca_dev *gspca_dev)
+static void sethvflip(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 val;
 
-	if (sd->hflip == 0)
-		sccb_reg_write(gspca_dev, 0x0c,
-				sccb_reg_read(gspca_dev, 0x0c) | 0x40);
-	else
-		sccb_reg_write(gspca_dev, 0x0c,
-				sccb_reg_read(gspca_dev, 0x0c) & ~0x40);
+	if (sd->sensor == SENSOR_OV767x) {
+		val = sccb_reg_read(gspca_dev, 0x1e);	/* mvfp */
+		val &= ~0x30;
+		if (sd->ctrls[HFLIP].val)
+			val |= 0x20;
+		if (sd->ctrls[VFLIP].val)
+			val |= 0x10;
+		sccb_reg_write(gspca_dev, 0x1e, val);
+	} else {
+		val = sccb_reg_read(gspca_dev, 0x0c);
+		val &= ~0xc0;
+		if (sd->ctrls[HFLIP].val == 0)
+			val |= 0x40;
+		if (sd->ctrls[VFLIP].val == 0)
+			val |= 0x80;
+		sccb_reg_write(gspca_dev, 0x0c, val);
+	}
 }
 
-static void setvflip(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 val;
+	int i;
+	static u8 color_tb[][6] = {
+		{0x42, 0x42, 0x00, 0x11, 0x30, 0x41},
+		{0x52, 0x52, 0x00, 0x16, 0x3c, 0x52},
+		{0x66, 0x66, 0x00, 0x1b, 0x4b, 0x66},
+		{0x80, 0x80, 0x00, 0x22, 0x5e, 0x80},
+		{0x9a, 0x9a, 0x00, 0x29, 0x71, 0x9a},
+		{0xb8, 0xb8, 0x00, 0x31, 0x87, 0xb8},
+		{0xdd, 0xdd, 0x00, 0x3b, 0xa2, 0xdd},
+	};
 
-	if (sd->vflip == 0)
-		sccb_reg_write(gspca_dev, 0x0c,
-				sccb_reg_read(gspca_dev, 0x0c) | 0x80);
-	else
-		sccb_reg_write(gspca_dev, 0x0c,
-				sccb_reg_read(gspca_dev, 0x0c) & ~0x80);
+	val = sd->ctrls[COLORS].val;
+	for (i = 0; i < ARRAY_SIZE(color_tb[0]); i++)
+		sccb_reg_write(gspca_dev, 0x4f + i, color_tb[val][i]);
 }
 
-static void setfreqfltr(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	u8 val;
 
-	if (sd->freqfltr == 0)
-		sccb_reg_write(gspca_dev, 0x2b, 0x00);
-	else
-		sccb_reg_write(gspca_dev, 0x2b, 0x9e);
+	val = sd->ctrls[LIGHTFREQ].val ? 0x9e : 0x00;
+	if (sd->sensor == SENSOR_OV767x) {
+		sccb_reg_write(gspca_dev, 0x2a, 0x00);
+		if (val)
+			val = 0x9d;	/* insert dummy to 25fps for 50Hz */
+	}
+	sccb_reg_write(gspca_dev, 0x2b, val);
 }
 
 
@@ -833,39 +1174,33 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	cam = &gspca_dev->cam;
 
+	cam->ctrls = sd->ctrls;
+
+	/* the auto white balance control works only when auto gain is set */
+	if (sd_ctrls[AGC].qctrl.default_value == 0)
+		gspca_dev->ctrl_inac |= (1 << AWB);
+
 	cam->cam_mode = ov772x_mode;
 	cam->nmodes = ARRAY_SIZE(ov772x_mode);
-	cam->mode_framerates = ov772x_framerates;
-
-	cam->bulk = 1;
-	cam->bulk_size = 16384;
-	cam->bulk_nurbs = 2;
 
 	sd->frame_rate = 30;
 
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->gain = GAIN_DEF;
-	sd->exposure = EXPO_DEF;
-#if AGC_DEF != 0
-	sd->agc = AGC_DEF;
-#else
-	gspca_dev->ctrl_inac |= (1 << AWB_IDX);
-#endif
-	sd->awb = AWB_DEF;
-	sd->aec = AEC_DEF;
-	sd->sharpness = SHARPNESS_DEF;
-	sd->hflip = HFLIP_DEF;
-	sd->vflip = VFLIP_DEF;
-	sd->freqfltr = FREQFLTR_DEF;
-
 	return 0;
 }
 
 /* this function is called at probe and resume time */
 static int sd_init(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	u16 sensor_id;
+	static const struct reg_array bridge_init[NSENSORS] = {
+	[SENSOR_OV767x] = {bridge_init_767x, ARRAY_SIZE(bridge_init_767x)},
+	[SENSOR_OV772x] = {bridge_init_772x, ARRAY_SIZE(bridge_init_772x)},
+	};
+	static const struct reg_array sensor_init[NSENSORS] = {
+	[SENSOR_OV767x] = {sensor_init_767x, ARRAY_SIZE(sensor_init_767x)},
+	[SENSOR_OV772x] = {sensor_init_772x, ARRAY_SIZE(sensor_init_772x)},
+	};
 
 	/* reset bridge */
 	ov534_reg_write(gspca_dev, 0xe7, 0x3a);
@@ -886,48 +1221,100 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	sensor_id |= sccb_reg_read(gspca_dev, 0x0b);
 	PDEBUG(D_PROBE, "Sensor ID: %04x", sensor_id);
 
+	if ((sensor_id & 0xfff0) == 0x7670) {
+		sd->sensor = SENSOR_OV767x;
+		gspca_dev->ctrl_dis = (1 << GAIN) |
+					(1 << AGC) |
+					(1 << SHARPNESS);	/* auto */
+		sd->ctrls[BRIGHTNESS].min = -127;
+		sd->ctrls[BRIGHTNESS].max = 127;
+		sd->ctrls[BRIGHTNESS].def = 0;
+		sd->ctrls[CONTRAST].max = 0x80;
+		sd->ctrls[CONTRAST].def = 0x40;
+		sd->ctrls[EXPOSURE].min = 0x08;
+		sd->ctrls[EXPOSURE].max = 0x60;
+		sd->ctrls[EXPOSURE].def = 0x13;
+		sd->ctrls[SHARPNESS].max = 9;
+		sd->ctrls[SHARPNESS].def = 4;
+		sd->ctrls[HFLIP].def = 1;
+		gspca_dev->cam.cam_mode = ov767x_mode;
+		gspca_dev->cam.nmodes = ARRAY_SIZE(ov767x_mode);
+	} else {
+		sd->sensor = SENSOR_OV772x;
+		gspca_dev->ctrl_dis = (1 << COLORS);
+		gspca_dev->cam.bulk = 1;
+		gspca_dev->cam.bulk_size = 16384;
+		gspca_dev->cam.bulk_nurbs = 2;
+		gspca_dev->cam.mode_framerates = ov772x_framerates;
+	}
+
 	/* initialize */
-	reg_w_array(gspca_dev, bridge_init,
-			ARRAY_SIZE(bridge_init));
+	reg_w_array(gspca_dev, bridge_init[sd->sensor].val,
+			bridge_init[sd->sensor].len);
 	ov534_set_led(gspca_dev, 1);
-	sccb_w_array(gspca_dev, sensor_init,
-			ARRAY_SIZE(sensor_init));
-	ov534_reg_write(gspca_dev, 0xe0, 0x09);
-	ov534_set_led(gspca_dev, 0);
-	set_frame_rate(gspca_dev);
+	sccb_w_array(gspca_dev, sensor_init[sd->sensor].val,
+			sensor_init[sd->sensor].len);
+	if (sd->sensor == SENSOR_OV767x)
+		sd_start(gspca_dev);
+	sd_stopN(gspca_dev);
+/*	set_frame_rate(gspca_dev);	*/
 
 	return gspca_dev->usb_err;
 }
 
 static int sd_start(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	int mode;
+	static const struct reg_array bridge_start[NSENSORS][2] = {
+	[SENSOR_OV767x] = {{bridge_start_qvga_767x,
+					ARRAY_SIZE(bridge_start_qvga_767x)},
+			{bridge_start_vga_767x,
+					ARRAY_SIZE(bridge_start_vga_767x)}},
+	[SENSOR_OV772x] = {{bridge_start_qvga_772x,
+					ARRAY_SIZE(bridge_start_qvga_772x)},
+			{bridge_start_vga_772x,
+					ARRAY_SIZE(bridge_start_vga_772x)}},
+	};
+	static const struct reg_array sensor_start[NSENSORS][2] = {
+	[SENSOR_OV767x] = {{sensor_start_qvga_767x,
+					ARRAY_SIZE(sensor_start_qvga_767x)},
+			{sensor_start_vga_767x,
+					ARRAY_SIZE(sensor_start_vga_767x)}},
+	[SENSOR_OV772x] = {{sensor_start_qvga_772x,
+					ARRAY_SIZE(sensor_start_qvga_772x)},
+			{sensor_start_vga_772x,
+					ARRAY_SIZE(sensor_start_vga_772x)}},
+	};
+
+	/* (from ms-win trace) */
+	if (sd->sensor == SENSOR_OV767x)
+		sccb_reg_write(gspca_dev, 0x1e, 0x04);
+					/* black sun enable ? */
+
+	mode = gspca_dev->curr_mode;	/* 0: 320x240, 1: 640x480 */
+	reg_w_array(gspca_dev, bridge_start[sd->sensor][mode].val,
+				bridge_start[sd->sensor][mode].len);
+	sccb_w_array(gspca_dev, sensor_start[sd->sensor][mode].val,
+				sensor_start[sd->sensor][mode].len);
 
-	mode = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
-	if (mode != 0) {	/* 320x240 */
-		reg_w_array(gspca_dev, bridge_start_qvga,
-				ARRAY_SIZE(bridge_start_qvga));
-		sccb_w_array(gspca_dev, sensor_start_qvga,
-				ARRAY_SIZE(sensor_start_qvga));
-	} else {		/* 640x480 */
-		reg_w_array(gspca_dev, bridge_start_vga,
-				ARRAY_SIZE(bridge_start_vga));
-		sccb_w_array(gspca_dev, sensor_start_vga,
-				ARRAY_SIZE(sensor_start_vga));
-	}
 	set_frame_rate(gspca_dev);
 
-	setagc(gspca_dev);
+	if (!(gspca_dev->ctrl_dis & (1 << AGC)))
+		setagc(gspca_dev);
 	setawb(gspca_dev);
 	setaec(gspca_dev);
-	setgain(gspca_dev);
+	if (!(gspca_dev->ctrl_dis & (1 << GAIN)))
+		setgain(gspca_dev);
 	setexposure(gspca_dev);
 	setbrightness(gspca_dev);
 	setcontrast(gspca_dev);
-	setsharpness(gspca_dev);
-	setvflip(gspca_dev);
-	sethflip(gspca_dev);
-	setfreqfltr(gspca_dev);
+	if (!(gspca_dev->ctrl_dis & (1 << SHARPNESS)))
+		setsharpness(gspca_dev);
+	sethvflip(gspca_dev);
+	if (!(gspca_dev->ctrl_dis & (1 << COLORS)))
+		setcolors(gspca_dev);
+	setlightfreq(gspca_dev);
 
 	ov534_set_led(gspca_dev, 1);
 	ov534_reg_write(gspca_dev, 0xe0, 0x00);
@@ -957,9 +1344,11 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	__u32 this_pts;
 	u16 this_fid;
 	int remaining_len = len;
+	int payload_len;
 
+	payload_len = gspca_dev->cam.bulk ? 2048 : 2040;
 	do {
-		len = min(remaining_len, 2048);
+		len = min(remaining_len, payload_len);
 
 		/* Payloads are prefixed with a UVC-style header.  We
 		   consider a frame to start when the FID toggles, or the PTS
@@ -999,8 +1388,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 		/* If this packet is marked as EOF, end the frame */
 		} else if (data[1] & UVC_STREAM_EOF) {
 			sd->last_pts = 0;
-			if (gspca_dev->image_len + len - 12 !=
-			    gspca_dev->width * gspca_dev->height * 2) {
+			if (gspca_dev->pixfmt == V4L2_PIX_FMT_YUYV
+			 && gspca_dev->image_len + len - 12 !=
+				   gspca_dev->width * gspca_dev->height * 2) {
 				PDEBUG(D_PACK, "wrong sized frame");
 				goto discard;
 			}
@@ -1026,212 +1416,27 @@ scan_next:
 	} while (remaining_len > 0);
 }
 
-/* controls */
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return 0;
-}
-
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
-	return 0;
-}
-
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return 0;
-}
-
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
-	return 0;
-}
-
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->brightness = val;
-	if (gspca_dev->streaming)
-		setbrightness(gspca_dev);
-	return 0;
-}
-
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->brightness;
-	return 0;
-}
-
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->contrast = val;
-	if (gspca_dev->streaming)
-		setcontrast(gspca_dev);
-	return 0;
-}
-
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->contrast;
-	return 0;
-}
-
 static int sd_setagc(struct gspca_dev *gspca_dev, __s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	sd->agc = val;
-
-	if (gspca_dev->streaming) {
+	sd->ctrls[AGC].val = val;
 
-		/* the auto white balance control works only
-		 * when auto gain is set */
-		if (val)
-			gspca_dev->ctrl_inac &= ~(1 << AWB_IDX);
-		else
-			gspca_dev->ctrl_inac |= (1 << AWB_IDX);
-		setagc(gspca_dev);
+	/* the auto white balance control works only
+	 * when auto gain is set */
+	if (val) {
+		gspca_dev->ctrl_inac &= ~(1 << AWB);
+	} else {
+		gspca_dev->ctrl_inac |= (1 << AWB);
+		if (sd->ctrls[AWB].val) {
+			sd->ctrls[AWB].val = 0;
+			if (gspca_dev->streaming)
+				setawb(gspca_dev);
+		}
 	}
-	return 0;
-}
-
-static int sd_getagc(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->agc;
-	return 0;
-}
-
-static int sd_setawb(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->awb = val;
-	if (gspca_dev->streaming)
-		setawb(gspca_dev);
-	return 0;
-}
-
-static int sd_getawb(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->awb;
-	return 0;
-}
-
-static int sd_setaec(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->aec = val;
-	if (gspca_dev->streaming)
-		setaec(gspca_dev);
-	return 0;
-}
-
-static int sd_getaec(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->aec;
-	return 0;
-}
-
-static int sd_setsharpness(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->sharpness = val;
-	if (gspca_dev->streaming)
-		setsharpness(gspca_dev);
-	return 0;
-}
-
-static int sd_getsharpness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->sharpness;
-	return 0;
-}
-
-static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->hflip = val;
-	if (gspca_dev->streaming)
-		sethflip(gspca_dev);
-	return 0;
-}
-
-static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->hflip;
-	return 0;
-}
-
-static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->vflip = val;
-	if (gspca_dev->streaming)
-		setvflip(gspca_dev);
-	return 0;
-}
-
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->vflip;
-	return 0;
-}
-
-static int sd_setfreqfltr(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->freqfltr = val;
 	if (gspca_dev->streaming)
-		setfreqfltr(gspca_dev);
-	return 0;
-}
-
-static int sd_getfreqfltr(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->freqfltr;
-	return 0;
+		setagc(gspca_dev);
+	return gspca_dev->usb_err;
 }
 
 static int sd_querymenu(struct gspca_dev *gspca_dev,
@@ -1302,6 +1507,7 @@ static const struct sd_desc sd_desc = {
 /* -- module initialisation -- */
 static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x1415, 0x2000)},
+	{USB_DEVICE(0x06f8, 0x3002)},
 	{}
 };
 
diff --git a/drivers/media/video/gspca/sn9c20x.c b/drivers/media/video/gspca/sn9c20x.c
index fcf29897b713..c431900cd292 100644
--- a/drivers/media/video/gspca/sn9c20x.c
+++ b/drivers/media/video/gspca/sn9c20x.c
@@ -152,6 +152,13 @@ static const struct dmi_system_id flip_dmi_table[] = {
 		}
 	},
 	{
+		.ident = "MSI MS-1633X",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "MSI"),
+			DMI_MATCH(DMI_BOARD_NAME, "MS-1633X")
+		}
+	},
+	{
 		.ident = "MSI MS-1635X",
 		.matches = {
 			DMI_MATCH(DMI_BOARD_VENDOR, "MSI"),
@@ -369,7 +376,7 @@ static const struct v4l2_pix_format vga_mode[] = {
 		.priv = SCALE_160x120},
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
-		.sizeimage = 320 * 240 * 3 / 8 + 590,
+		.sizeimage = 320 * 240 * 4 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = SCALE_320x240 | MODE_JPEG},
 	{320, 240, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
@@ -384,7 +391,7 @@ static const struct v4l2_pix_format vga_mode[] = {
 		.priv = SCALE_320x240},
 	{640, 480, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 640,
-		.sizeimage = 640 * 480 * 3 / 8 + 590,
+		.sizeimage = 640 * 480 * 4 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = SCALE_640x480 | MODE_JPEG},
 	{640, 480, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
@@ -417,7 +424,7 @@ static const struct v4l2_pix_format sxga_mode[] = {
 		.priv = SCALE_160x120},
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
-		.sizeimage = 320 * 240 * 3 / 8 + 590,
+		.sizeimage = 320 * 240 * 4 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = SCALE_320x240 | MODE_JPEG},
 	{320, 240, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
@@ -432,7 +439,7 @@ static const struct v4l2_pix_format sxga_mode[] = {
 		.priv = SCALE_320x240},
 	{640, 480, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 640,
-		.sizeimage = 640 * 480 * 3 / 8 + 590,
+		.sizeimage = 640 * 480 * 4 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = SCALE_640x480 | MODE_JPEG},
 	{640, 480, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
@@ -884,6 +891,9 @@ static struct i2c_reg_u8 ov7660_init[] = {
 	{0x0e, 0x80}, {0x0d, 0x08}, {0x0f, 0xc3},
 	{0x04, 0xc3}, {0x10, 0x40}, {0x11, 0x40},
 	{0x12, 0x05}, {0x13, 0xba}, {0x14, 0x2a},
+	/* HDG Set hstart and hstop, datasheet default 0x11, 0x61, using
+	   0x10, 0x61 and sd->hstart, vstart = 3, fixes ugly colored borders */
+	{0x17, 0x10}, {0x18, 0x61},
 	{0x37, 0x0f}, {0x38, 0x02}, {0x39, 0x43},
 	{0x3a, 0x00}, {0x69, 0x90}, {0x2d, 0xf6},
 	{0x2e, 0x0b}, {0x01, 0x78}, {0x02, 0x50},
@@ -1332,10 +1342,8 @@ static int ov7660_init_sensor(struct gspca_dev *gspca_dev)
 			return -ENODEV;
 		}
 	}
-	/* disable hflip and vflip */
-	gspca_dev->ctrl_dis = (1 << HFLIP_IDX) | (1 << VFLIP_IDX);
-	sd->hstart = 1;
-	sd->vstart = 1;
+	sd->hstart = 3;
+	sd->vstart = 3;
 	return 0;
 }
 
@@ -1608,6 +1616,18 @@ static int set_hvflip(struct gspca_dev *gspca_dev)
 	}
 
 	switch (sd->sensor) {
+	case SENSOR_OV7660:
+		value = 0x01;
+		if (hflip)
+			value |= 0x20;
+		if (vflip) {
+			value |= 0x10;
+			sd->vstart = 2;
+		} else
+			sd->vstart = 3;
+		reg_w1(gspca_dev, 0x1182, sd->vstart);
+		i2c_w1(gspca_dev, 0x1e, value);
+		break;
 	case SENSOR_OV9650:
 		i2c_r1(gspca_dev, 0x1e, &value);
 		value &= ~0x30;
@@ -2482,7 +2502,7 @@ static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x0c45, 0x6253), SN9C20X(OV9650, 0x30, 0)},
 	{USB_DEVICE(0x0c45, 0x6260), SN9C20X(OV7670, 0x21, 0)},
 	{USB_DEVICE(0x0c45, 0x6270), SN9C20X(MT9VPRB, 0x00, 0)},
-	{USB_DEVICE(0x0c45, 0x627b), SN9C20X(OV7660, 0x21, 0)},
+	{USB_DEVICE(0x0c45, 0x627b), SN9C20X(OV7660, 0x21, FLIP_DETECT)},
 	{USB_DEVICE(0x0c45, 0x627c), SN9C20X(HV7131R, 0x11, 0)},
 	{USB_DEVICE(0x0c45, 0x627f), SN9C20X(OV9650, 0x30, 0)},
 	{USB_DEVICE(0x0c45, 0x6280), SN9C20X(MT9M001, 0x5d, 0)},
@@ -2494,7 +2514,7 @@ static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x0c45, 0x62a0), SN9C20X(OV7670, 0x21, 0)},
 	{USB_DEVICE(0x0c45, 0x62b0), SN9C20X(MT9VPRB, 0x00, 0)},
 	{USB_DEVICE(0x0c45, 0x62b3), SN9C20X(OV9655, 0x30, 0)},
-	{USB_DEVICE(0x0c45, 0x62bb), SN9C20X(OV7660, 0x21, 0)},
+	{USB_DEVICE(0x0c45, 0x62bb), SN9C20X(OV7660, 0x21, LED_REVERSE)},
 	{USB_DEVICE(0x0c45, 0x62bc), SN9C20X(HV7131R, 0x11, 0)},
 	{USB_DEVICE(0x045e, 0x00f4), SN9C20X(OV9650, 0x30, 0)},
 	{USB_DEVICE(0x145f, 0x013d), SN9C20X(OV7660, 0x21, 0)},
diff --git a/drivers/media/video/gspca/sonixj.c b/drivers/media/video/gspca/sonixj.c
index d6f39ce1b7e1..6415aff5cbd1 100644
--- a/drivers/media/video/gspca/sonixj.c
+++ b/drivers/media/video/gspca/sonixj.c
@@ -29,8 +29,6 @@ MODULE_AUTHOR("Jean-François Moine <http://moinejf.free.fr>");
 MODULE_DESCRIPTION("GSPCA/SONIX JPEG USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-static int starcam;
-
 /* controls */
 enum e_ctrl {
 	BRIGHTNESS,
@@ -57,11 +55,18 @@ struct sd {
 	atomic_t avg_lum;
 	u32 exposure;
 
+	struct work_struct work;
+	struct workqueue_struct *work_thread;
+
+	u32 pktsz;			/* (used by pkt_scan) */
+	u16 npkt;
+	u8 nchg;
+	s8 short_mark;
+
 	u8 quality;			/* image quality */
-#define QUALITY_MIN 60
-#define QUALITY_MAX 95
-#define QUALITY_DEF 80
-	u8 jpegqual;			/* webcam quality */
+#define QUALITY_MIN 25
+#define QUALITY_MAX 90
+#define QUALITY_DEF 70
 
 	u8 reg01;
 	u8 reg17;
@@ -99,6 +104,8 @@ enum sensors {
 	SENSOR_SP80708,
 };
 
+static void qual_upd(struct work_struct *work);
+
 /* device flags */
 #define F_PDN_INV	0x01	/* inverse pin S_PWR_DN / sn_xxx tables */
 #define F_ILLUM		0x02	/* presence of illuminator */
@@ -401,7 +408,7 @@ static const u8 sn_hv7131[0x1c] = {
 
 static const u8 sn_mi0360[0x1c] = {
 /*	reg0	reg1	reg2	reg3	reg4	reg5	reg6	reg7 */
-	0x00,	0x61,	0x40,	0x00,	0x1a,	0x20,	0x20,	0x20,
+	0x00,	0x63,	0x40,	0x00,	0x1a,	0x20,	0x20,	0x20,
 /*	reg8	reg9	rega	regb	regc	regd	rege	regf */
 	0x81,	0x5d,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
 /*	reg10	reg11	reg12	reg13	reg14	reg15	reg16	reg17 */
@@ -847,18 +854,8 @@ static const u8 mt9v111_sensor_init[][8] = {
 	{0xb1, 0x5c, 0x01, 0x00, 0x01, 0x00, 0x00, 0x10}, /* IFP select */
 	{0xb1, 0x5c, 0x08, 0x04, 0x80, 0x00, 0x00, 0x10}, /* output fmt ctrl */
 	{0xb1, 0x5c, 0x06, 0x00, 0x00, 0x00, 0x00, 0x10}, /* op mode ctrl */
-	{0xb1, 0x5c, 0x02, 0x00, 0x16, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x03, 0x01, 0xe1, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x04, 0x02, 0x81, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x05, 0x00, 0x04, 0x00, 0x00, 0x10},
 	{0xb1, 0x5c, 0x01, 0x00, 0x04, 0x00, 0x00, 0x10}, /* sensor select */
-	{0xb1, 0x5c, 0x02, 0x00, 0x16, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x03, 0x01, 0xe6, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x04, 0x02, 0x86, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x05, 0x00, 0x04, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x06, 0x00, 0x00, 0x00, 0x00, 0x10},
 	{0xb1, 0x5c, 0x08, 0x00, 0x08, 0x00, 0x00, 0x10}, /* row start */
-	{0xb1, 0x5c, 0x0e, 0x00, 0x08, 0x00, 0x00, 0x10},
 	{0xb1, 0x5c, 0x02, 0x00, 0x16, 0x00, 0x00, 0x10}, /* col start */
 	{0xb1, 0x5c, 0x03, 0x01, 0xe7, 0x00, 0x00, 0x10}, /* window height */
 	{0xb1, 0x5c, 0x04, 0x02, 0x87, 0x00, 0x00, 0x10}, /* window width */
@@ -872,15 +869,10 @@ static const u8 mt9v111_sensor_init[][8] = {
 	{}
 };
 static const u8 mt9v111_sensor_param1[][8] = {
-	{0xb1, 0x5c, 0x20, 0x00, 0x00, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x20, 0x00, 0x00, 0x00, 0x00, 0x10},
-	{0xb1, 0x5c, 0x09, 0x01, 0x2c, 0x00, 0x00, 0x10},
-	{0xd1, 0x5c, 0x2b, 0x00, 0x33, 0x00, 0xa0, 0x10}, /* green1 gain */
-	{0xd1, 0x5c, 0x2d, 0x00, 0xa0, 0x00, 0x33, 0x10}, /* red gain */
-	/*******/
-	{0xb1, 0x5c, 0x06, 0x00, 0x1e, 0x00, 0x00, 0x10}, /* vert blanking */
-	{0xb1, 0x5c, 0x05, 0x00, 0x0a, 0x00, 0x00, 0x10}, /* horiz blanking */
-	{0xd1, 0x5c, 0x2c, 0x00, 0xad, 0x00, 0xad, 0x10}, /* blue gain */
+	{0xd1, 0x5c, 0x2b, 0x00, 0x33, 0x00, 0xad, 0x10}, /* G1 and B gains */
+	{0xd1, 0x5c, 0x2d, 0x00, 0xad, 0x00, 0x33, 0x10}, /* R and G2 gains */
+	{0xb1, 0x5c, 0x06, 0x00, 0x40, 0x00, 0x00, 0x10}, /* vert blanking */
+	{0xb1, 0x5c, 0x05, 0x00, 0x09, 0x00, 0x00, 0x10}, /* horiz blanking */
 	{0xb1, 0x5c, 0x35, 0x01, 0xc0, 0x00, 0x00, 0x10}, /* global gain */
 	{}
 };
@@ -1784,7 +1776,12 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	sd->ag_cnt = -1;
 	sd->quality = QUALITY_DEF;
-	sd->jpegqual = 80;
+
+	/* if USB 1.1, let some bandwidth for the audio device */
+	if (gspca_dev->audio && gspca_dev->dev->speed < USB_SPEED_HIGH)
+		gspca_dev->nbalt--;
+
+	INIT_WORK(&sd->work, qual_upd);
 
 	return 0;
 }
@@ -1794,7 +1791,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	const u8 *sn9c1xx;
-	u8 regGpio[] = { 0x29, 0x74 };		/* with audio */
+	u8 regGpio[] = { 0x29, 0x70 };		/* no audio */
 	u8 regF1;
 
 	/* setup a selector by bridge */
@@ -1806,6 +1803,8 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	if (gspca_dev->usb_err < 0)
 		return gspca_dev->usb_err;
 	PDEBUG(D_PROBE, "Sonix chip id: %02x", regF1);
+	if (gspca_dev->audio)
+		regGpio[1] |= 0x04;		/* with audio */
 	switch (sd->bridge) {
 	case BRIDGE_SN9C102P:
 	case BRIDGE_SN9C105:
@@ -1838,14 +1837,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	case BRIDGE_SN9C102P:
 		reg_w1(gspca_dev, 0x02, regGpio[1]);
 		break;
-	case BRIDGE_SN9C105:
-		reg_w(gspca_dev, 0x01, regGpio, 2);
-		break;
-	case BRIDGE_SN9C110:
-		reg_w1(gspca_dev, 0x02, 0x62);
-		break;
-	case BRIDGE_SN9C120:
-		regGpio[1] = 0x70;		/* no audio */
+	default:
 		reg_w(gspca_dev, 0x01, regGpio, 2);
 		break;
 	}
@@ -1944,10 +1936,10 @@ static u32 setexposure(struct gspca_dev *gspca_dev,
 		u8 expo_c1[] =
 			{ 0xb1, 0x5c, 0x09, 0x00, 0x00, 0x00, 0x00, 0x10 };
 
-		if (expo > 0x0280)
-			expo = 0x0280;
-		else if (expo < 0x0040)
-			expo = 0x0040;
+		if (expo > 0x0390)
+			expo = 0x0390;
+		else if (expo < 0x0060)
+			expo = 0x0060;
 		expo_c1[3] = expo >> 8;
 		expo_c1[4] = expo;
 		i2c_w8(gspca_dev, expo_c1);
@@ -2004,10 +1996,13 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 		sd->exposure = setexposure(gspca_dev, expo);
 		break;
 	case SENSOR_GC0307:
-	case SENSOR_MT9V111:
 		expo = brightness;
 		sd->exposure = setexposure(gspca_dev, expo);
 		return;			/* don't set the Y offset */
+	case SENSOR_MT9V111:
+		expo = brightness << 2;
+		sd->exposure = setexposure(gspca_dev, expo);
+		return;			/* don't set the Y offset */
 	case SENSOR_OM6802:
 		expo = brightness << 2;
 		sd->exposure = setexposure(gspca_dev, expo);
@@ -2199,14 +2194,11 @@ static void setillum(struct gspca_dev *gspca_dev)
 			sd->ctrls[ILLUM].val ? 0x64 : 0x60);
 		break;
 	case SENSOR_MT9V111:
-		if (starcam)
-			reg_w1(gspca_dev, 0x02,
-				sd->ctrls[ILLUM].val ?
-						0x55 : 0x54);	/* 370i */
-		else
-			reg_w1(gspca_dev, 0x02,
-				sd->ctrls[ILLUM].val ?
-						0x66 : 0x64);	/* Clip */
+		reg_w1(gspca_dev, 0x02,
+			sd->ctrls[ILLUM].val ? 0x77 : 0x74);
+/* should have been: */
+/*						0x55 : 0x54);	* 370i */
+/*						0x66 : 0x64);	* Clip */
 		break;
 	}
 }
@@ -2271,18 +2263,12 @@ static void setfreq(struct gspca_dev *gspca_dev)
 static void setjpegqual(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int i, sc;
 
-	if (sd->jpegqual < 50)
-		sc = 5000 / sd->jpegqual;
-	else
-		sc = 200 - sd->jpegqual * 2;
+	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
 #if USB_BUF_SZ < 64
 #error "No room enough in usb_buf for quantization table"
 #endif
-	for (i = 0; i < 64; i++)
-		gspca_dev->usb_buf[i] =
-			(jpeg_head[JPEG_QT0_OFFSET + i] * sc + 50) / 100;
+	memcpy(gspca_dev->usb_buf, &sd->jpeg_hdr[JPEG_QT0_OFFSET], 64);
 	usb_control_msg(gspca_dev->dev,
 			usb_sndctrlpipe(gspca_dev->dev, 0),
 			0x08,
@@ -2290,9 +2276,7 @@ static void setjpegqual(struct gspca_dev *gspca_dev)
 			0x0100, 0,
 			gspca_dev->usb_buf, 64,
 			500);
-	for (i = 0; i < 64; i++)
-		gspca_dev->usb_buf[i] =
-			(jpeg_head[JPEG_QT1_OFFSET + i] * sc + 50) / 100;
+	memcpy(gspca_dev->usb_buf, &sd->jpeg_hdr[JPEG_QT1_OFFSET], 64);
 	usb_control_msg(gspca_dev->dev,
 			usb_sndctrlpipe(gspca_dev->dev, 0),
 			0x08,
@@ -2305,6 +2289,19 @@ static void setjpegqual(struct gspca_dev *gspca_dev)
 	reg_w1(gspca_dev, 0x18, sd->reg18);
 }
 
+/* JPEG quality update */
+/* This function is executed from a work queue. */
+static void qual_upd(struct work_struct *work)
+{
+	struct sd *sd = container_of(work, struct sd, work);
+	struct gspca_dev *gspca_dev = &sd->gspca_dev;
+
+	mutex_lock(&gspca_dev->usb_lock);
+	PDEBUG(D_STREAM, "qual_upd %d%%", sd->quality);
+	setjpegqual(gspca_dev);
+	mutex_unlock(&gspca_dev->usb_lock);
+}
+
 /* -- start the camera -- */
 static int sd_start(struct gspca_dev *gspca_dev)
 {
@@ -2338,7 +2335,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x21);		/* JPEG 422 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
 
 	/* initialize the bridge */
 	sn9c1xx = sn_tb[sd->sensor];
@@ -2619,6 +2615,11 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	setcolors(gspca_dev);
 	setautogain(gspca_dev);
 	setfreq(gspca_dev);
+
+	sd->pktsz = sd->npkt = 0;
+	sd->nchg = sd->short_mark = 0;
+	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
+
 	return gspca_dev->usb_err;
 }
 
@@ -2695,6 +2696,20 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 	/* reg_w1(gspca_dev, 0xf1, 0x01); */
 }
 
+/* called on streamoff with alt==0 and on disconnect */
+/* the usb_lock is held at entry - restore on exit */
+static void sd_stop0(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	if (sd->work_thread != NULL) {
+		mutex_unlock(&gspca_dev->usb_lock);
+		destroy_workqueue(sd->work_thread);
+		mutex_lock(&gspca_dev->usb_lock);
+		sd->work_thread = NULL;
+	}
+}
+
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -2732,6 +2747,7 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 					(unsigned int) (expotimes << 8));
 			break;
 		case SENSOR_OM6802:
+		case SENSOR_MT9V111:
 			expotimes = sd->exposure;
 			expotimes += (luma_mean - delta) >> 2;
 			if (expotimes < 0)
@@ -2744,7 +2760,6 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 /*		case SENSOR_MO4000: */
 /*		case SENSOR_MI0360: */
 /*		case SENSOR_MI0360B: */
-/*		case SENSOR_MT9V111: */
 			expotimes = sd->exposure;
 			expotimes += (luma_mean - delta) >> 6;
 			if (expotimes < 0)
@@ -2757,6 +2772,29 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	}
 }
 
+/* set the average luminosity from an isoc marker */
+static void set_lum(struct sd *sd,
+		    u8 *data)
+{
+	int avg_lum;
+
+	/*	w0 w1 w2
+	 *	w3 w4 w5
+	 *	w6 w7 w8
+	 */
+	avg_lum = (data[27] << 8) + data[28]		/* w3 */
+
+		+ (data[31] << 8) + data[32]		/* w5 */
+
+		+ (data[23] << 8) + data[24]		/* w1 */
+
+		+ (data[35] << 8) + data[36]		/* w7 */
+
+		+ (data[29] << 10) + (data[30] << 2);	/* w4 * 4 */
+	avg_lum >>= 10;
+	atomic_set(&sd->avg_lum, avg_lum);
+}
+
 /* scan the URB packets */
 /* This function is run at interrupt level. */
 static void sd_pkt_scan(struct gspca_dev *gspca_dev,
@@ -2764,70 +2802,141 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 			int len)			/* iso packet length */
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int sof, avg_lum;
-
-	/* the image ends on a 64 bytes block starting with
-	 *	ff d9 ff ff 00 c4 c4 96
-	 * and followed by various information including luminosity */
-	/* this block may be splitted between two packets */
-	/* a new image always starts in a new packet */
-	switch (gspca_dev->last_packet_type) {
-	case DISCARD_PACKET:		/* restart image building */
-		sof = len - 64;
-		if (sof >= 0 && data[sof] == 0xff && data[sof + 1] == 0xd9)
-			gspca_frame_add(gspca_dev, LAST_PACKET, NULL, 0);
-		return;
-	case LAST_PACKET:		/* put the JPEG 422 header */
+	int i, new_qual;
+
+	/*
+	 * A frame ends on the marker
+	 *		ff ff 00 c4 c4 96 ..
+	 * which is 62 bytes long and is followed by various information
+	 * including statuses and luminosity.
+	 *
+	 * A marker may be splitted on two packets.
+	 *
+	 * The 6th byte of a marker contains the bits:
+	 *	0x08: USB full
+	 *	0xc0: frame sequence
+	 * When the bit 'USB full' is set, the frame must be discarded;
+	 * this is also the case when the 2 bytes before the marker are
+	 * not the JPEG end of frame ('ff d9').
+	 */
+
+/*fixme: assumption about the following code:
+ *	- there can be only one marker in a packet
+ */
+
+	/* skip the remaining bytes of a short marker */
+	i = sd->short_mark;
+	if (i != 0) {
+		sd->short_mark = 0;
+		if (i < 0	/* if 'ff' at end of previous packet */
+		 && data[0] == 0xff
+		 && data[1] == 0x00)
+			goto marker_found;
+		if (data[0] == 0xff && data[1] == 0xff) {
+			i = 0;
+			goto marker_found;
+		}
+		len -= i;
+		if (len <= 0)
+			return;
+		data += i;
+	}
+
+	/* count the packets and their size */
+	sd->npkt++;
+	sd->pktsz += len;
+
+	/* search backwards if there is a marker in the packet */
+	for (i = len - 1; --i >= 0; ) {
+		if (data[i] != 0xff) {
+			i--;
+			continue;
+		}
+		if (data[i + 1] == 0xff) {
+
+			/* (there may be 'ff ff' inside a marker) */
+			if (i + 2 >= len || data[i + 2] == 0x00)
+				goto marker_found;
+		}
+	}
+
+	/* no marker found */
+	/* add the JPEG header if first fragment */
+	if (data[len - 1] == 0xff)
+		sd->short_mark = -1;
+	if (gspca_dev->last_packet_type == LAST_PACKET)
 		gspca_frame_add(gspca_dev, FIRST_PACKET,
 				sd->jpeg_hdr, JPEG_HDR_SZ);
-		break;
-	}
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
+	return;
+
+	/* marker found */
+	/* if some error, discard the frame and decrease the quality */
+marker_found:
+	new_qual = 0;
+	if (i > 2) {
+		if (data[i - 2] != 0xff || data[i - 1] != 0xd9) {
+			gspca_dev->last_packet_type = DISCARD_PACKET;
+			new_qual = -3;
+		}
+	} else if (i + 6 < len) {
+		if (data[i + 6] & 0x08) {
+			gspca_dev->last_packet_type = DISCARD_PACKET;
+			new_qual = -5;
+		}
+	}
 
-	data = gspca_dev->image;
-	if (data == NULL)
-		return;
-	sof = gspca_dev->image_len - 64;
-	if (data[sof] != 0xff
-	 || data[sof + 1] != 0xd9)
-		return;
+	gspca_frame_add(gspca_dev, LAST_PACKET, data, i);
 
-	/* end of image found - remove the trailing data */
-	gspca_dev->image_len = sof + 2;
-	gspca_frame_add(gspca_dev, LAST_PACKET, NULL, 0);
-	if (sd->ag_cnt < 0)
-		return;
-/* w1 w2 w3 */
-/* w4 w5 w6 */
-/* w7 w8 */
-/* w4 */
-	avg_lum = ((data[sof + 29] << 8) | data[sof + 30]) >> 6;
-/* w6 */
-	avg_lum += ((data[sof + 33] << 8) | data[sof + 34]) >> 6;
-/* w2 */
-	avg_lum += ((data[sof + 25] << 8) | data[sof + 26]) >> 6;
-/* w8 */
-	avg_lum += ((data[sof + 37] << 8) | data[sof + 38]) >> 6;
-/* w5 */
-	avg_lum += ((data[sof + 31] << 8) | data[sof + 32]) >> 4;
-	avg_lum >>= 4;
-	atomic_set(&sd->avg_lum, avg_lum);
-}
+	/* compute the filling rate and a new JPEG quality */
+	if (new_qual == 0) {
+		int r;
 
-static int sd_set_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+		r = (sd->pktsz * 100) /
+			(sd->npkt *
+				gspca_dev->urb[0]->iso_frame_desc[0].length);
+		if (r >= 85)
+			new_qual = -3;
+		else if (r < 75)
+			new_qual = 2;
+	}
+	if (new_qual != 0) {
+		sd->nchg += new_qual;
+		if (sd->nchg < -6 || sd->nchg >= 12) {
+			sd->nchg = 0;
+			new_qual += sd->quality;
+			if (new_qual < QUALITY_MIN)
+				new_qual = QUALITY_MIN;
+			else if (new_qual > QUALITY_MAX)
+				new_qual = QUALITY_MAX;
+			if (new_qual != sd->quality) {
+				sd->quality = new_qual;
+				queue_work(sd->work_thread, &sd->work);
+			}
+		}
+	} else {
+		sd->nchg = 0;
+	}
+	sd->pktsz = sd->npkt = 0;
 
-	if (jcomp->quality < QUALITY_MIN)
-		sd->quality = QUALITY_MIN;
-	else if (jcomp->quality > QUALITY_MAX)
-		sd->quality = QUALITY_MAX;
-	else
-		sd->quality = jcomp->quality;
-	if (gspca_dev->streaming)
-		jpeg_set_qual(sd->jpeg_hdr, sd->quality);
-	return 0;
+	/* if the marker is smaller than 62 bytes,
+	 * memorize the number of bytes to skip in the next packet */
+	if (i + 62 > len) {			/* no more usable data */
+		sd->short_mark = i + 62 - len;
+		return;
+	}
+	if (sd->ag_cnt >= 0)
+		set_lum(sd, data + i);
+
+	/* if more data, start a new frame */
+	i += 62;
+	if (i < len) {
+		data += i;
+		len -= i;
+		gspca_frame_add(gspca_dev, FIRST_PACKET,
+				sd->jpeg_hdr, JPEG_HDR_SZ);
+		gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
+	}
 }
 
 static int sd_get_jcomp(struct gspca_dev *gspca_dev,
@@ -2891,10 +3000,10 @@ static const struct sd_desc sd_desc = {
 	.init = sd_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
+	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
 	.dq_callback = do_autogain,
 	.get_jcomp = sd_get_jcomp,
-	.set_jcomp = sd_set_jcomp,
 	.querymenu = sd_querymenu,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 	.int_pkt_scan = sd_int_pkt_scan,
@@ -3004,7 +3113,3 @@ static void __exit sd_mod_exit(void)
 
 module_init(sd_mod_init);
 module_exit(sd_mod_exit);
-
-module_param(starcam, int, 0644);
-MODULE_PARM_DESC(starcam,
-	"StarCam model. 0: Clip, 1: 370i");
diff --git a/drivers/media/video/gspca/vicam.c b/drivers/media/video/gspca/vicam.c
new file mode 100644
index 000000000000..84dfbab923b5
--- /dev/null
+++ b/drivers/media/video/gspca/vicam.c
@@ -0,0 +1,381 @@
+/*
+ * gspca ViCam subdriver
+ *
+ * Copyright (C) 2011 Hans de Goede <hdegoede@redhat.com>
+ *
+ * Based on the usbvideo vicam driver, which is:
+ *
+ * Copyright (c) 2002 Joe Burks (jburks@wavicle.org),
+ *                    Christopher L Cheney (ccheney@cheney.cx),
+ *                    Pavel Machek (pavel@ucw.cz),
+ *                    John Tyner (jtyner@cs.ucr.edu),
+ *                    Monroe Williams (monroe@pobox.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
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#define MODULE_NAME "vicam"
+#define HEADER_SIZE 64
+
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+#include <linux/firmware.h>
+#include <linux/ihex.h>
+#include "gspca.h"
+
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_DESCRIPTION("GSPCA ViCam USB Camera Driver");
+MODULE_LICENSE("GPL");
+
+enum e_ctrl {
+	GAIN,
+	EXPOSURE,
+	NCTRL		/* number of controls */
+};
+
+struct sd {
+	struct gspca_dev gspca_dev;	/* !! must be the first item */
+	struct work_struct work_struct;
+	struct workqueue_struct *work_thread;
+	struct gspca_ctrl ctrls[NCTRL];
+};
+
+/* The vicam sensor has a resolution of 512 x 244, with I believe square
+   pixels, but this is forced to a 4:3 ratio by optics. So it has
+   non square pixels :( */
+static struct v4l2_pix_format vicam_mode[] = {
+	{ 256, 122, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
+		.bytesperline = 256,
+		.sizeimage = 256 * 122,
+		.colorspace = V4L2_COLORSPACE_SRGB,},
+	/* 2 modes with somewhat more square pixels */
+	{ 256, 200, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
+		.bytesperline = 256,
+		.sizeimage = 256 * 200,
+		.colorspace = V4L2_COLORSPACE_SRGB,},
+	{ 256, 240, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
+		.bytesperline = 256,
+		.sizeimage = 256 * 240,
+		.colorspace = V4L2_COLORSPACE_SRGB,},
+#if 0   /* This mode has extremely non square pixels, testing use only */
+	{ 512, 122, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
+		.bytesperline = 512,
+		.sizeimage = 512 * 122,
+		.colorspace = V4L2_COLORSPACE_SRGB,},
+#endif
+	{ 512, 244, V4L2_PIX_FMT_SGRBG8, V4L2_FIELD_NONE,
+		.bytesperline = 512,
+		.sizeimage = 512 * 244,
+		.colorspace = V4L2_COLORSPACE_SRGB,},
+};
+
+static const struct ctrl sd_ctrls[] = {
+[GAIN] = {
+	    {
+		.id      = V4L2_CID_GAIN,
+		.type    = V4L2_CTRL_TYPE_INTEGER,
+		.name    = "Gain",
+		.minimum = 0,
+		.maximum = 255,
+		.step    = 1,
+		.default_value = 200,
+	    },
+	},
+[EXPOSURE] = {
+	    {
+		.id      = V4L2_CID_EXPOSURE,
+		.type    = V4L2_CTRL_TYPE_INTEGER,
+		.name    = "Exposure",
+		.minimum = 0,
+		.maximum = 2047,
+		.step    = 1,
+		.default_value = 256,
+	    },
+	},
+};
+
+static int vicam_control_msg(struct gspca_dev *gspca_dev, u8 request,
+	u16 value, u16 index, u8 *data, u16 len)
+{
+	int ret;
+
+	ret = usb_control_msg(gspca_dev->dev,
+			      usb_sndctrlpipe(gspca_dev->dev, 0),
+			      request,
+			      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			      value, index, data, len, 1000);
+	if (ret < 0)
+		err("control msg req %02X error %d", request, ret);
+
+	return ret;
+}
+
+static int vicam_set_camera_power(struct gspca_dev *gspca_dev, int state)
+{
+	int ret;
+
+	ret = vicam_control_msg(gspca_dev, 0x50, state, 0, NULL, 0);
+	if (ret < 0)
+		return ret;
+
+	if (state)
+		ret = vicam_control_msg(gspca_dev, 0x55, 1, 0, NULL, 0);
+
+	return ret;
+}
+
+/*
+ *  request and read a block of data - see warning on vicam_command.
+ */
+static int vicam_read_frame(struct gspca_dev *gspca_dev, u8 *data, int size)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	int ret, unscaled_height, act_len = 0;
+	u8 *req_data = gspca_dev->usb_buf;
+
+	memset(req_data, 0, 16);
+	req_data[0] = sd->ctrls[GAIN].val;
+	if (gspca_dev->width == 256)
+		req_data[1] |= 0x01; /* low nibble x-scale */
+	if (gspca_dev->height <= 122) {
+		req_data[1] |= 0x10; /* high nibble y-scale */
+		unscaled_height = gspca_dev->height * 2;
+	} else
+		unscaled_height = gspca_dev->height;
+	req_data[2] = 0x90; /* unknown, does not seem to do anything */
+	if (unscaled_height <= 200)
+		req_data[3] = 0x06; /* vend? */
+	else if (unscaled_height <= 242) /* Yes 242 not 240 */
+		req_data[3] = 0x07; /* vend? */
+	else /* Up to 244 lines with req_data[3] == 0x08 */
+		req_data[3] = 0x08; /* vend? */
+
+	if (sd->ctrls[EXPOSURE].val < 256) {
+		/* Frame rate maxed out, use partial frame expo time */
+		req_data[4] = 255 - sd->ctrls[EXPOSURE].val;
+		req_data[5] = 0x00;
+		req_data[6] = 0x00;
+		req_data[7] = 0x01;
+	} else {
+		/* Modify frame rate */
+		req_data[4] = 0x00;
+		req_data[5] = 0x00;
+		req_data[6] = sd->ctrls[EXPOSURE].val & 0xFF;
+		req_data[7] = sd->ctrls[EXPOSURE].val >> 8;
+	}
+	req_data[8] = ((244 - unscaled_height) / 2) & ~0x01; /* vstart */
+	/* bytes 9-15 do not seem to affect exposure or image quality */
+
+	mutex_lock(&gspca_dev->usb_lock);
+	ret = vicam_control_msg(gspca_dev, 0x51, 0x80, 0, req_data, 16);
+	mutex_unlock(&gspca_dev->usb_lock);
+	if (ret < 0)
+		return ret;
+
+	ret = usb_bulk_msg(gspca_dev->dev,
+			   usb_rcvbulkpipe(gspca_dev->dev, 0x81),
+			   data, size, &act_len, 10000);
+	/* successful, it returns 0, otherwise  negative */
+	if (ret < 0 || act_len != size) {
+		err("bulk read fail (%d) len %d/%d",
+			ret, act_len, size);
+		return -EIO;
+	}
+	return 0;
+}
+
+/* This function is called as a workqueue function and runs whenever the camera
+ * is streaming data. Because it is a workqueue function it is allowed to sleep
+ * so we can use synchronous USB calls. To avoid possible collisions with other
+ * threads attempting to use the camera's USB interface we take the gspca
+ * usb_lock when performing USB operations. In practice the only thing we need
+ * to protect against is the usb_set_interface call that gspca makes during
+ * stream_off as the camera doesn't provide any controls that the user could try
+ * to change.
+ */
+static void vicam_dostream(struct work_struct *work)
+{
+	struct sd *sd = container_of(work, struct sd, work_struct);
+	struct gspca_dev *gspca_dev = &sd->gspca_dev;
+	int ret, frame_sz;
+	u8 *buffer;
+
+	frame_sz = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].sizeimage +
+		   HEADER_SIZE;
+	buffer = kmalloc(frame_sz, GFP_KERNEL | GFP_DMA);
+	if (!buffer) {
+		err("Couldn't allocate USB buffer");
+		goto exit;
+	}
+
+	while (gspca_dev->present && gspca_dev->streaming) {
+		ret = vicam_read_frame(gspca_dev, buffer, frame_sz);
+		if (ret < 0)
+			break;
+
+		/* Note the frame header contents seem to be completely
+		   constant, they do not change with either image, or
+		   settings. So we simply discard it. The frames have
+		   a very similar 64 byte footer, which we don't even
+		   bother reading from the cam */
+		gspca_frame_add(gspca_dev, FIRST_PACKET,
+				buffer + HEADER_SIZE,
+				frame_sz - HEADER_SIZE);
+		gspca_frame_add(gspca_dev, LAST_PACKET, NULL, 0);
+	}
+exit:
+	kfree(buffer);
+}
+
+/* This function is called at probe time just before sd_init */
+static int sd_config(struct gspca_dev *gspca_dev,
+		const struct usb_device_id *id)
+{
+	struct cam *cam = &gspca_dev->cam;
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	/* We don't use the buffer gspca allocates so make it small. */
+	cam->bulk = 1;
+	cam->bulk_size = 64;
+	cam->cam_mode = vicam_mode;
+	cam->nmodes = ARRAY_SIZE(vicam_mode);
+	cam->ctrls = sd->ctrls;
+
+	INIT_WORK(&sd->work_struct, vicam_dostream);
+
+	return 0;
+}
+
+/* this function is called at probe and resume time */
+static int sd_init(struct gspca_dev *gspca_dev)
+{
+	int ret;
+	const struct ihex_binrec *rec;
+	const struct firmware *uninitialized_var(fw);
+	u8 *firmware_buf;
+
+	ret = request_ihex_firmware(&fw, "vicam/firmware.fw",
+				    &gspca_dev->dev->dev);
+	if (ret) {
+		err("Failed to load \"vicam/firmware.fw\": %d\n", ret);
+		return ret;
+	}
+
+	firmware_buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!firmware_buf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+	for (rec = (void *)fw->data; rec; rec = ihex_next_binrec(rec)) {
+		memcpy(firmware_buf, rec->data, be16_to_cpu(rec->len));
+		ret = vicam_control_msg(gspca_dev, 0xff, 0, 0, firmware_buf,
+					be16_to_cpu(rec->len));
+		if (ret < 0)
+			break;
+	}
+
+	kfree(firmware_buf);
+exit:
+	release_firmware(fw);
+	return ret;
+}
+
+/* Set up for getting frames. */
+static int sd_start(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	int ret;
+
+	ret = vicam_set_camera_power(gspca_dev, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Start the workqueue function to do the streaming */
+	sd->work_thread = create_singlethread_workqueue(MODULE_NAME);
+	queue_work(sd->work_thread, &sd->work_struct);
+
+	return 0;
+}
+
+/* called on streamoff with alt==0 and on disconnect */
+/* the usb_lock is held at entry - restore on exit */
+static void sd_stop0(struct gspca_dev *gspca_dev)
+{
+	struct sd *dev = (struct sd *)gspca_dev;
+
+	/* wait for the work queue to terminate */
+	mutex_unlock(&gspca_dev->usb_lock);
+	/* This waits for vicam_dostream to finish */
+	destroy_workqueue(dev->work_thread);
+	dev->work_thread = NULL;
+	mutex_lock(&gspca_dev->usb_lock);
+
+	vicam_set_camera_power(gspca_dev, 0);
+}
+
+/* Table of supported USB devices */
+static const struct usb_device_id device_table[] = {
+	{USB_DEVICE(0x04c1, 0x009d)},
+	{USB_DEVICE(0x0602, 0x1001)},
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, device_table);
+
+/* sub-driver description */
+static const struct sd_desc sd_desc = {
+	.name   = MODULE_NAME,
+	.ctrls  = sd_ctrls,
+	.nctrls = ARRAY_SIZE(sd_ctrls),
+	.config = sd_config,
+	.init   = sd_init,
+	.start  = sd_start,
+	.stop0  = sd_stop0,
+};
+
+/* -- device connect -- */
+static int sd_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	return gspca_dev_probe(intf, id,
+			&sd_desc,
+			sizeof(struct sd),
+			THIS_MODULE);
+}
+
+static struct usb_driver sd_driver = {
+	.name       = MODULE_NAME,
+	.id_table   = device_table,
+	.probe      = sd_probe,
+	.disconnect = gspca_disconnect,
+#ifdef CONFIG_PM
+	.suspend = gspca_suspend,
+	.resume  = gspca_resume,
+#endif
+};
+
+/* -- module insert / remove -- */
+static int __init sd_mod_init(void)
+{
+	return usb_register(&sd_driver);
+}
+
+static void __exit sd_mod_exit(void)
+{
+	usb_deregister(&sd_driver);
+}
+
+module_init(sd_mod_init);
+module_exit(sd_mod_exit);
diff --git a/drivers/media/video/gspca/zc3xx.c b/drivers/media/video/gspca/zc3xx.c
index 47236a58bf33..b2014915f9c1 100644
--- a/drivers/media/video/gspca/zc3xx.c
+++ b/drivers/media/video/gspca/zc3xx.c
@@ -6414,6 +6414,10 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	gspca_dev->cam.ctrls = sd->ctrls;
 	sd->quality = QUALITY_DEF;
 
+	/* if USB 1.1, let some bandwidth for the audio device */
+	if (gspca_dev->audio && gspca_dev->dev->speed < USB_SPEED_HIGH)
+		gspca_dev->nbalt--;
+
 	return 0;
 }
 
diff --git a/drivers/media/video/ivtv/ivtv-irq.c b/drivers/media/video/ivtv/ivtv-irq.c
index 9b4faf009196..9c29e964d400 100644
--- a/drivers/media/video/ivtv/ivtv-irq.c
+++ b/drivers/media/video/ivtv/ivtv-irq.c
@@ -628,22 +628,66 @@ static void ivtv_irq_enc_pio_complete(struct ivtv *itv)
 static void ivtv_irq_dma_err(struct ivtv *itv)
 {
 	u32 data[CX2341X_MBOX_MAX_DATA];
+	u32 status;
 
 	del_timer(&itv->dma_timer);
+
 	ivtv_api_get_data(&itv->enc_mbox, IVTV_MBOX_DMA_END, 2, data);
+	status = read_reg(IVTV_REG_DMASTATUS);
 	IVTV_DEBUG_WARN("DMA ERROR %08x %08x %08x %d\n", data[0], data[1],
-				read_reg(IVTV_REG_DMASTATUS), itv->cur_dma_stream);
-	write_reg(read_reg(IVTV_REG_DMASTATUS) & 3, IVTV_REG_DMASTATUS);
+				status, itv->cur_dma_stream);
+	/*
+	 * We do *not* write back to the IVTV_REG_DMASTATUS register to
+	 * clear the error status, if either the encoder write (0x02) or
+	 * decoder read (0x01) bus master DMA operation do not indicate
+	 * completed.  We can race with the DMA engine, which may have
+	 * transitioned to completed status *after* we read the register.
+	 * Setting a IVTV_REG_DMASTATUS flag back to "busy" status, after the
+	 * DMA engine has completed, will cause the DMA engine to stop working.
+	 */
+	status &= 0x3;
+	if (status == 0x3)
+		write_reg(status, IVTV_REG_DMASTATUS);
+
 	if (!test_bit(IVTV_F_I_UDMA, &itv->i_flags) &&
 	    itv->cur_dma_stream >= 0 && itv->cur_dma_stream < IVTV_MAX_STREAMS) {
 		struct ivtv_stream *s = &itv->streams[itv->cur_dma_stream];
 
-		/* retry */
-		if (s->type >= IVTV_DEC_STREAM_TYPE_MPG)
+		if (s->type >= IVTV_DEC_STREAM_TYPE_MPG) {
+			/* retry */
+			/*
+			 * FIXME - handle cases of DMA error similar to
+			 * encoder below, except conditioned on status & 0x1
+			 */
 			ivtv_dma_dec_start(s);
-		else
-			ivtv_dma_enc_start(s);
-		return;
+			return;
+		} else {
+			if ((status & 0x2) == 0) {
+				/*
+				 * CX2341x Bus Master DMA write is ongoing.
+				 * Reset the timer and let it complete.
+				 */
+				itv->dma_timer.expires =
+						jiffies + msecs_to_jiffies(600);
+				add_timer(&itv->dma_timer);
+				return;
+			}
+
+			if (itv->dma_retries < 3) {
+				/*
+				 * CX2341x Bus Master DMA write has ended.
+				 * Retry the write, starting with the first
+				 * xfer segment. Just retrying the current
+				 * segment is not sufficient.
+				 */
+				s->sg_processed = 0;
+				itv->dma_retries++;
+				ivtv_dma_enc_start_xfer(s);
+				return;
+			}
+			/* Too many retries, give up on this one */
+		}
+
 	}
 	if (test_bit(IVTV_F_I_UDMA, &itv->i_flags)) {
 		ivtv_udma_start(itv);
diff --git a/drivers/media/video/ivtv/ivtv-udma.c b/drivers/media/video/ivtv/ivtv-udma.c
index 1daf1dd65bf7..69cc8166b20b 100644
--- a/drivers/media/video/ivtv/ivtv-udma.c
+++ b/drivers/media/video/ivtv/ivtv-udma.c
@@ -132,7 +132,12 @@ int ivtv_udma_setup(struct ivtv *itv, unsigned long ivtv_dest_addr,
 	if (user_dma.page_count != err) {
 		IVTV_DEBUG_WARN("failed to map user pages, returned %d instead of %d\n",
 			   err, user_dma.page_count);
-		return -EINVAL;
+		if (err >= 0) {
+			for (i = 0; i < err; i++)
+				put_page(dma->map[i]);
+			return -EINVAL;
+		}
+		return err;
 	}
 
 	dma->page_count = user_dma.page_count;
diff --git a/drivers/media/video/ivtv/ivtv-vbi.c b/drivers/media/video/ivtv/ivtv-vbi.c
index 2dfa957b0fd5..b6eb51ce7735 100644
--- a/drivers/media/video/ivtv/ivtv-vbi.c
+++ b/drivers/media/video/ivtv/ivtv-vbi.c
@@ -174,7 +174,7 @@ ivtv_write_vbi_from_user(struct ivtv *itv,
 			ret = -EFAULT;
 			break;
 		}
-		ivtv_write_vbi_line(itv, sliced + i, &cc, &found_cc);
+		ivtv_write_vbi_line(itv, &d, &cc, &found_cc);
 	}
 
 	if (found_cc)
diff --git a/drivers/media/video/ivtv/ivtv-yuv.c b/drivers/media/video/ivtv/ivtv-yuv.c
index c0875378acc2..dcbab6ad4c26 100644
--- a/drivers/media/video/ivtv/ivtv-yuv.c
+++ b/drivers/media/video/ivtv/ivtv-yuv.c
@@ -77,23 +77,51 @@ static int ivtv_yuv_prep_user_dma(struct ivtv *itv, struct ivtv_user_dma *dma,
 	/* Get user pages for DMA Xfer */
 	down_read(&current->mm->mmap_sem);
 	y_pages = get_user_pages(current, current->mm, y_dma.uaddr, y_dma.page_count, 0, 1, &dma->map[0], NULL);
-	uv_pages = get_user_pages(current, current->mm, uv_dma.uaddr, uv_dma.page_count, 0, 1, &dma->map[y_pages], NULL);
+	uv_pages = 0; /* silence gcc. value is set and consumed only if: */
+	if (y_pages == y_dma.page_count) {
+		uv_pages = get_user_pages(current, current->mm,
+					  uv_dma.uaddr, uv_dma.page_count, 0, 1,
+					  &dma->map[y_pages], NULL);
+	}
 	up_read(&current->mm->mmap_sem);
 
-	dma->page_count = y_dma.page_count + uv_dma.page_count;
-
-	if (y_pages + uv_pages != dma->page_count) {
-		IVTV_DEBUG_WARN
-		    ("failed to map user pages, returned %d instead of %d\n",
-		     y_pages + uv_pages, dma->page_count);
-
-		for (i = 0; i < dma->page_count; i++) {
-			put_page(dma->map[i]);
+	if (y_pages != y_dma.page_count || uv_pages != uv_dma.page_count) {
+		int rc = -EFAULT;
+
+		if (y_pages == y_dma.page_count) {
+			IVTV_DEBUG_WARN
+				("failed to map uv user pages, returned %d "
+				 "expecting %d\n", uv_pages, uv_dma.page_count);
+
+			if (uv_pages >= 0) {
+				for (i = 0; i < uv_pages; i++)
+					put_page(dma->map[y_pages + i]);
+				rc = -EFAULT;
+			} else {
+				rc = uv_pages;
+			}
+		} else {
+			IVTV_DEBUG_WARN
+				("failed to map y user pages, returned %d "
+				 "expecting %d\n", y_pages, y_dma.page_count);
 		}
-		dma->page_count = 0;
-		return -EINVAL;
+		if (y_pages >= 0) {
+			for (i = 0; i < y_pages; i++)
+				put_page(dma->map[i]);
+			/*
+			 * Inherit the -EFAULT from rc's
+			 * initialization, but allow it to be
+			 * overriden by uv_pages above if it was an
+			 * actual errno.
+			 */
+		} else {
+			rc = y_pages;
+		}
+		return rc;
 	}
 
+	dma->page_count = y_pages + uv_pages;
+
 	/* Fill & map SG List */
 	if (ivtv_udma_fill_sg_list (dma, &uv_dma, ivtv_udma_fill_sg_list (dma, &y_dma, 0)) < 0) {
 		IVTV_DEBUG_WARN("could not allocate bounce buffers for highmem userspace buffers\n");
diff --git a/drivers/media/video/mem2mem_testdev.c b/drivers/media/video/mem2mem_testdev.c
index c179041d91f8..e1f96ea45bcb 100644
--- a/drivers/media/video/mem2mem_testdev.c
+++ b/drivers/media/video/mem2mem_testdev.c
@@ -28,7 +28,7 @@
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
-#include <media/videobuf-vmalloc.h>
+#include <media/videobuf2-vmalloc.h>
 
 #define MEM2MEM_TEST_MODULE_NAME "mem2mem-testdev"
 
@@ -201,11 +201,6 @@ struct m2mtest_ctx {
 	struct v4l2_m2m_ctx	*m2m_ctx;
 };
 
-struct m2mtest_buffer {
-	/* vb must be first! */
-	struct videobuf_buffer	vb;
-};
-
 static struct v4l2_queryctrl *get_ctrl(int id)
 {
 	int i;
@@ -219,37 +214,41 @@ static struct v4l2_queryctrl *get_ctrl(int id)
 }
 
 static int device_process(struct m2mtest_ctx *ctx,
-			  struct m2mtest_buffer *in_buf,
-			  struct m2mtest_buffer *out_buf)
+			  struct vb2_buffer *in_vb,
+			  struct vb2_buffer *out_vb)
 {
 	struct m2mtest_dev *dev = ctx->dev;
+	struct m2mtest_q_data *q_data;
 	u8 *p_in, *p_out;
 	int x, y, t, w;
 	int tile_w, bytes_left;
-	struct videobuf_queue *src_q;
-	struct videobuf_queue *dst_q;
+	int width, height, bytesperline;
 
-	src_q = v4l2_m2m_get_src_vq(ctx->m2m_ctx);
-	dst_q = v4l2_m2m_get_dst_vq(ctx->m2m_ctx);
-	p_in = videobuf_queue_to_vaddr(src_q, &in_buf->vb);
-	p_out = videobuf_queue_to_vaddr(dst_q, &out_buf->vb);
+	q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+	width	= q_data->width;
+	height	= q_data->height;
+	bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
+
+	p_in = vb2_plane_vaddr(in_vb, 0);
+	p_out = vb2_plane_vaddr(out_vb, 0);
 	if (!p_in || !p_out) {
 		v4l2_err(&dev->v4l2_dev,
 			 "Acquiring kernel pointers to buffers failed\n");
 		return -EFAULT;
 	}
 
-	if (in_buf->vb.size > out_buf->vb.size) {
+	if (vb2_plane_size(in_vb, 0) > vb2_plane_size(out_vb, 0)) {
 		v4l2_err(&dev->v4l2_dev, "Output buffer is too small\n");
 		return -EINVAL;
 	}
 
-	tile_w = (in_buf->vb.width * (q_data[V4L2_M2M_DST].fmt->depth >> 3))
+	tile_w = (width * (q_data[V4L2_M2M_DST].fmt->depth >> 3))
 		/ MEM2MEM_NUM_TILES;
-	bytes_left = in_buf->vb.bytesperline - tile_w * MEM2MEM_NUM_TILES;
+	bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
 	w = 0;
 
-	for (y = 0; y < in_buf->vb.height; ++y) {
+	for (y = 0; y < height; ++y) {
 		for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 			if (w & 0x1) {
 				for (x = 0; x < tile_w; ++x)
@@ -301,6 +300,21 @@ static void job_abort(void *priv)
 	ctx->aborting = 1;
 }
 
+static void m2mtest_lock(void *priv)
+{
+	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_dev *dev = ctx->dev;
+	mutex_lock(&dev->dev_mutex);
+}
+
+static void m2mtest_unlock(void *priv)
+{
+	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_dev *dev = ctx->dev;
+	mutex_unlock(&dev->dev_mutex);
+}
+
+
 /* device_run() - prepares and starts the device
  *
  * This simulates all the immediate preparations required before starting
@@ -311,7 +325,7 @@ static void device_run(void *priv)
 {
 	struct m2mtest_ctx *ctx = priv;
 	struct m2mtest_dev *dev = ctx->dev;
-	struct m2mtest_buffer *src_buf, *dst_buf;
+	struct vb2_buffer *src_buf, *dst_buf;
 
 	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
@@ -322,12 +336,11 @@ static void device_run(void *priv)
 	schedule_irq(dev, ctx->transtime);
 }
 
-
 static void device_isr(unsigned long priv)
 {
 	struct m2mtest_dev *m2mtest_dev = (struct m2mtest_dev *)priv;
 	struct m2mtest_ctx *curr_ctx;
-	struct m2mtest_buffer *src_buf, *dst_buf;
+	struct vb2_buffer *src_vb, *dst_vb;
 	unsigned long flags;
 
 	curr_ctx = v4l2_m2m_get_curr_priv(m2mtest_dev->m2m_dev);
@@ -338,31 +351,26 @@ static void device_isr(unsigned long priv)
 		return;
 	}
 
-	src_buf = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
-	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
+	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
+
 	curr_ctx->num_processed++;
 
+	spin_lock_irqsave(&m2mtest_dev->irqlock, flags);
+	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+	spin_unlock_irqrestore(&m2mtest_dev->irqlock, flags);
+
 	if (curr_ctx->num_processed == curr_ctx->translen
 	    || curr_ctx->aborting) {
 		dprintk(curr_ctx->dev, "Finishing transaction\n");
 		curr_ctx->num_processed = 0;
-		spin_lock_irqsave(&m2mtest_dev->irqlock, flags);
-		src_buf->vb.state = dst_buf->vb.state = VIDEOBUF_DONE;
-		wake_up(&src_buf->vb.done);
-		wake_up(&dst_buf->vb.done);
-		spin_unlock_irqrestore(&m2mtest_dev->irqlock, flags);
 		v4l2_m2m_job_finish(m2mtest_dev->m2m_dev, curr_ctx->m2m_ctx);
 	} else {
-		spin_lock_irqsave(&m2mtest_dev->irqlock, flags);
-		src_buf->vb.state = dst_buf->vb.state = VIDEOBUF_DONE;
-		wake_up(&src_buf->vb.done);
-		wake_up(&dst_buf->vb.done);
-		spin_unlock_irqrestore(&m2mtest_dev->irqlock, flags);
 		device_run(curr_ctx);
 	}
 }
 
-
 /*
  * video ioctls
  */
@@ -423,7 +431,7 @@ static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 
 static int vidioc_g_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 {
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 	struct m2mtest_q_data *q_data;
 
 	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
@@ -434,7 +442,7 @@ static int vidioc_g_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 
 	f->fmt.pix.width	= q_data->width;
 	f->fmt.pix.height	= q_data->height;
-	f->fmt.pix.field	= vq->field;
+	f->fmt.pix.field	= V4L2_FIELD_NONE;
 	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
 	f->fmt.pix.bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
 	f->fmt.pix.sizeimage	= q_data->sizeimage;
@@ -523,7 +531,7 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 static int vidioc_s_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 {
 	struct m2mtest_q_data *q_data;
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 
 	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 	if (!vq)
@@ -533,7 +541,7 @@ static int vidioc_s_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 	if (!q_data)
 		return -EINVAL;
 
-	if (videobuf_queue_is_busy(vq)) {
+	if (vb2_is_busy(vq)) {
 		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
 		return -EBUSY;
 	}
@@ -543,7 +551,6 @@ static int vidioc_s_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 	q_data->height		= f->fmt.pix.height;
 	q_data->sizeimage	= q_data->width * q_data->height
 				* q_data->fmt->depth >> 3;
-	vq->field		= f->fmt.pix.field;
 
 	dprintk(ctx->dev,
 		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
@@ -733,120 +740,94 @@ static const struct v4l2_ioctl_ops m2mtest_ioctl_ops = {
  * Queue operations
  */
 
-static void m2mtest_buf_release(struct videobuf_queue *vq,
-				struct videobuf_buffer *vb)
-{
-	struct m2mtest_ctx *ctx = vq->priv_data;
-
-	dprintk(ctx->dev, "type: %d, index: %d, state: %d\n",
-		vq->type, vb->i, vb->state);
-
-	videobuf_vmalloc_free(vb);
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
-static int m2mtest_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-			  unsigned int *size)
+static int m2mtest_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+				unsigned int *nplanes, unsigned long sizes[],
+				void *alloc_ctxs[])
 {
-	struct m2mtest_ctx *ctx = vq->priv_data;
+	struct m2mtest_ctx *ctx = vb2_get_drv_priv(vq);
 	struct m2mtest_q_data *q_data;
+	unsigned int size, count = *nbuffers;
 
 	q_data = get_q_data(vq->type);
 
-	*size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
-	dprintk(ctx->dev, "size:%d, w/h %d/%d, depth: %d\n",
-		*size, q_data->width, q_data->height, q_data->fmt->depth);
+	size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
 
-	if (0 == *count)
-		*count = MEM2MEM_DEF_NUM_BUFS;
+	while (size * count > MEM2MEM_VID_MEM_LIMIT)
+		(count)--;
 
-	while (*size * *count > MEM2MEM_VID_MEM_LIMIT)
-		(*count)--;
+	*nplanes = 1;
+	*nbuffers = count;
+	sizes[0] = size;
 
-	v4l2_info(&ctx->dev->v4l2_dev,
-		  "%d buffers of size %d set up.\n", *count, *size);
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
 
 	return 0;
 }
 
-static int m2mtest_buf_prepare(struct videobuf_queue *vq,
-			       struct videobuf_buffer *vb,
-			       enum v4l2_field field)
+static int m2mtest_buf_prepare(struct vb2_buffer *vb)
 {
-	struct m2mtest_ctx *ctx = vq->priv_data;
+	struct m2mtest_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 	struct m2mtest_q_data *q_data;
-	int ret;
 
-	dprintk(ctx->dev, "type: %d, index: %d, state: %d\n",
-		vq->type, vb->i, vb->state);
+	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
 
-	q_data = get_q_data(vq->type);
+	q_data = get_q_data(vb->vb2_queue->type);
 
-	if (vb->baddr) {
-		/* User-provided buffer */
-		if (vb->bsize < q_data->sizeimage) {
-			/* Buffer too small to fit a frame */
-			v4l2_err(&ctx->dev->v4l2_dev,
-				 "User-provided buffer too small\n");
-			return -EINVAL;
-		}
-	} else if (vb->state != VIDEOBUF_NEEDS_INIT
-			&& vb->bsize < q_data->sizeimage) {
-		/* We provide the buffer, but it's already been initialized
-		 * and is too small */
+	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+		dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
+				__func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
 		return -EINVAL;
 	}
 
-	vb->width	= q_data->width;
-	vb->height	= q_data->height;
-	vb->bytesperline = (q_data->width * q_data->fmt->depth) >> 3;
-	vb->size	= q_data->sizeimage;
-	vb->field	= field;
-
-	if (VIDEOBUF_NEEDS_INIT == vb->state) {
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret) {
-			v4l2_err(&ctx->dev->v4l2_dev,
-				 "Iolock failed\n");
-			goto fail;
-		}
-	}
-
-	vb->state = VIDEOBUF_PREPARED;
+	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
 
 	return 0;
-fail:
-	m2mtest_buf_release(vq, vb);
-	return ret;
 }
 
-static void m2mtest_buf_queue(struct videobuf_queue *vq,
-			   struct videobuf_buffer *vb)
+static void m2mtest_buf_queue(struct vb2_buffer *vb)
 {
-	struct m2mtest_ctx *ctx = vq->priv_data;
-
-	v4l2_m2m_buf_queue(ctx->m2m_ctx, vq, vb);
+	struct m2mtest_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
 }
 
-static struct videobuf_queue_ops m2mtest_qops = {
-	.buf_setup	= m2mtest_buf_setup,
-	.buf_prepare	= m2mtest_buf_prepare,
-	.buf_queue	= m2mtest_buf_queue,
-	.buf_release	= m2mtest_buf_release,
+static struct vb2_ops m2mtest_qops = {
+	.queue_setup	 = m2mtest_queue_setup,
+	.buf_prepare	 = m2mtest_buf_prepare,
+	.buf_queue	 = m2mtest_buf_queue,
 };
 
-static void queue_init(void *priv, struct videobuf_queue *vq,
-		       enum v4l2_buf_type type)
+static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
 {
 	struct m2mtest_ctx *ctx = priv;
-	struct m2mtest_dev *dev = ctx->dev;
+	int ret;
 
-	videobuf_queue_vmalloc_init(vq, &m2mtest_qops, dev->v4l2_dev.dev,
-				    &dev->irqlock, type, V4L2_FIELD_NONE,
-				    sizeof(struct m2mtest_buffer), priv,
-				    &dev->dev_mutex);
-}
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &m2mtest_qops;
+	src_vq->mem_ops = &vb2_vmalloc_memops;
 
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &m2mtest_qops;
+	dst_vq->mem_ops = &vb2_vmalloc_memops;
+
+	return vb2_queue_init(dst_vq);
+}
 
 /*
  * File operations
@@ -866,7 +847,8 @@ static int m2mtest_open(struct file *file)
 	ctx->transtime = MEM2MEM_DEF_TRANSTIME;
 	ctx->num_processed = 0;
 
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(ctx, dev->m2m_dev, queue_init);
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+
 	if (IS_ERR(ctx->m2m_ctx)) {
 		int ret = PTR_ERR(ctx->m2m_ctx);
 
@@ -932,6 +914,8 @@ static struct v4l2_m2m_ops m2m_ops = {
 	.device_run	= device_run,
 	.job_ready	= job_ready,
 	.job_abort	= job_abort,
+	.lock		= m2mtest_lock,
+	.unlock		= m2mtest_unlock,
 };
 
 static int m2mtest_probe(struct platform_device *pdev)
@@ -990,6 +974,7 @@ static int m2mtest_probe(struct platform_device *pdev)
 
 	return 0;
 
+	v4l2_m2m_release(dev->m2m_dev);
 err_m2m:
 	video_unregister_device(dev->vfd);
 rel_vdev:
@@ -1011,7 +996,6 @@ static int m2mtest_remove(struct platform_device *pdev)
 	v4l2_m2m_release(dev->m2m_dev);
 	del_timer_sync(&dev->timer);
 	video_unregister_device(dev->vfd);
-	video_device_release(dev->vfd);
 	v4l2_device_unregister(&dev->v4l2_dev);
 	kfree(dev);
 
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index b9cb4a436959..502e2a40964c 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -21,7 +21,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-dma-contig.h>
 #include <media/soc_camera.h>
 #include <media/soc_mediabus.h>
 
@@ -62,10 +62,16 @@
 
 #define MAX_VIDEO_MEM 16
 
+enum csi_buffer_state {
+	CSI_BUF_NEEDS_INIT,
+	CSI_BUF_PREPARED,
+};
+
 struct mx3_camera_buffer {
 	/* common v4l buffer stuff -- must be first */
-	struct videobuf_buffer			vb;
-	enum v4l2_mbus_pixelcode		code;
+	struct vb2_buffer			vb;
+	enum csi_buffer_state			state;
+	struct list_head			queue;
 
 	/* One descriptot per scatterlist (per frame) */
 	struct dma_async_tx_descriptor		*txd;
@@ -108,6 +114,9 @@ struct mx3_camera_dev {
 	struct list_head	capture;
 	spinlock_t		lock;		/* Protects video buffer lists */
 	struct mx3_camera_buffer *active;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	enum v4l2_field		field;
+	int			sequence;
 
 	/* IDMAC / dmaengine interface */
 	struct idmac_channel	*idmac_channel[1];	/* We need one channel */
@@ -130,6 +139,11 @@ static void csi_reg_write(struct mx3_camera_dev *mx3, u32 value, off_t reg)
 	__raw_writel(value, mx3->base + reg);
 }
 
+static struct mx3_camera_buffer *to_mx3_vb(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct mx3_camera_buffer, vb);
+}
+
 /* Called from the IPU IDMAC ISR */
 static void mx3_cam_dma_done(void *arg)
 {
@@ -137,20 +151,20 @@ static void mx3_cam_dma_done(void *arg)
 	struct dma_chan *chan = desc->txd.chan;
 	struct idmac_channel *ichannel = to_idmac_chan(chan);
 	struct mx3_camera_dev *mx3_cam = ichannel->client;
-	struct videobuf_buffer *vb;
 
 	dev_dbg(chan->device->dev, "callback cookie %d, active DMA 0x%08x\n",
 		desc->txd.cookie, mx3_cam->active ? sg_dma_address(&mx3_cam->active->sg) : 0);
 
 	spin_lock(&mx3_cam->lock);
 	if (mx3_cam->active) {
-		vb = &mx3_cam->active->vb;
-
-		list_del_init(&vb->queue);
-		vb->state = VIDEOBUF_DONE;
-		do_gettimeofday(&vb->ts);
-		vb->field_count++;
-		wake_up(&vb->done);
+		struct vb2_buffer *vb = &mx3_cam->active->vb;
+		struct mx3_camera_buffer *buf = to_mx3_vb(vb);
+
+		list_del_init(&buf->queue);
+		do_gettimeofday(&vb->v4l2_buf.timestamp);
+		vb->v4l2_buf.field = mx3_cam->field;
+		vb->v4l2_buf.sequence = mx3_cam->sequence++;
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 	}
 
 	if (list_empty(&mx3_cam->capture)) {
@@ -165,50 +179,22 @@ static void mx3_cam_dma_done(void *arg)
 	}
 
 	mx3_cam->active = list_entry(mx3_cam->capture.next,
-				     struct mx3_camera_buffer, vb.queue);
-	mx3_cam->active->vb.state = VIDEOBUF_ACTIVE;
+				     struct mx3_camera_buffer, queue);
 	spin_unlock(&mx3_cam->lock);
 }
 
-static void free_buffer(struct videobuf_queue *vq, struct mx3_camera_buffer *buf)
-{
-	struct soc_camera_device *icd = vq->priv_data;
-	struct videobuf_buffer *vb = &buf->vb;
-	struct dma_async_tx_descriptor *txd = buf->txd;
-	struct idmac_channel *ichan;
-
-	BUG_ON(in_interrupt());
-
-	dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		vb, vb->baddr, vb->bsize);
-
-	/*
-	 * This waits until this buffer is out of danger, i.e., until it is no
-	 * longer in STATE_QUEUED or STATE_ACTIVE
-	 */
-	videobuf_waiton(vq, vb, 0, 0);
-	if (txd) {
-		ichan = to_idmac_chan(txd->chan);
-		async_tx_ack(txd);
-	}
-	videobuf_dma_contig_free(vq, vb);
-	buf->txd = NULL;
-
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
 /*
  * Videobuf operations
  */
 
 /*
  * Calculate the __buffer__ (not data) size and number of buffers.
- * Called with .vb_lock held
  */
-static int mx3_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
-			      unsigned int *size)
+static int mx3_videobuf_setup(struct vb2_queue *vq,
+			unsigned int *count, unsigned int *num_planes,
+			unsigned long sizes[], void *alloc_ctxs[])
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
 	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
@@ -220,162 +206,133 @@ static int mx3_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 	if (!mx3_cam->idmac_channel[0])
 		return -EINVAL;
 
-	*size = bytes_per_line * icd->user_height;
+	*num_planes = 1;
+
+	mx3_cam->sequence = 0;
+	sizes[0] = bytes_per_line * icd->user_height;
+	alloc_ctxs[0] = mx3_cam->alloc_ctx;
 
 	if (!*count)
 		*count = 32;
 
-	if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
-		*count = MAX_VIDEO_MEM * 1024 * 1024 / *size;
+	if (sizes[0] * *count > MAX_VIDEO_MEM * 1024 * 1024)
+		*count = MAX_VIDEO_MEM * 1024 * 1024 / sizes[0];
 
 	return 0;
 }
 
-/* Called with .vb_lock held */
-static int mx3_videobuf_prepare(struct videobuf_queue *vq,
-		struct videobuf_buffer *vb, enum v4l2_field field)
+static int mx3_videobuf_prepare(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
-	struct mx3_camera_buffer *buf =
-		container_of(vb, struct mx3_camera_buffer, vb);
+	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
+	struct scatterlist *sg;
+	struct mx3_camera_buffer *buf;
 	size_t new_size;
-	int ret;
 	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 						icd->current_fmt->host_fmt);
 
 	if (bytes_per_line < 0)
 		return bytes_per_line;
 
-	new_size = bytes_per_line * icd->user_height;
+	buf = to_mx3_vb(vb);
+	sg = &buf->sg;
 
-	/*
-	 * I think, in buf_prepare you only have to protect global data,
-	 * the actual buffer is yours
-	 */
-
-	if (buf->code	!= icd->current_fmt->code ||
-	    vb->width	!= icd->user_width ||
-	    vb->height	!= icd->user_height ||
-	    vb->field	!= field) {
-		buf->code	= icd->current_fmt->code;
-		vb->width	= icd->user_width;
-		vb->height	= icd->user_height;
-		vb->field	= field;
-		if (vb->state != VIDEOBUF_NEEDS_INIT)
-			free_buffer(vq, buf);
-	}
+	new_size = bytes_per_line * icd->user_height;
 
-	if (vb->baddr && vb->bsize < new_size) {
-		/* User provided buffer, but it is too small */
-		ret = -ENOMEM;
-		goto out;
+	if (vb2_plane_size(vb, 0) < new_size) {
+		dev_err(icd->dev.parent, "Buffer too small (%lu < %zu)\n",
+			vb2_plane_size(vb, 0), new_size);
+		return -ENOBUFS;
 	}
 
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
-		struct scatterlist *sg = &buf->sg;
-
-		/*
-		 * The total size of video-buffers that will be allocated / mapped.
-		 * *size that we calculated in videobuf_setup gets assigned to
-		 * vb->bsize, and now we use the same calculation to get vb->size.
-		 */
-		vb->size = new_size;
-
-		/* This actually (allocates and) maps buffers */
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret)
-			goto fail;
-
-		/*
-		 * We will have to configure the IDMAC channel. It has two slots
-		 * for DMA buffers, we shall enter the first two buffers there,
-		 * and then submit new buffers in DMA-ready interrupts
-		 */
-		sg_init_table(sg, 1);
-		sg_dma_address(sg)	= videobuf_to_dma_contig(vb);
-		sg_dma_len(sg)		= vb->size;
+	if (buf->state == CSI_BUF_NEEDS_INIT) {
+		sg_dma_address(sg)	= vb2_dma_contig_plane_paddr(vb, 0);
+		sg_dma_len(sg)		= new_size;
 
 		buf->txd = ichan->dma_chan.device->device_prep_slave_sg(
 			&ichan->dma_chan, sg, 1, DMA_FROM_DEVICE,
 			DMA_PREP_INTERRUPT);
-		if (!buf->txd) {
-			ret = -EIO;
-			goto fail;
-		}
+		if (!buf->txd)
+			return -EIO;
 
 		buf->txd->callback_param	= buf->txd;
 		buf->txd->callback		= mx3_cam_dma_done;
 
-		vb->state = VIDEOBUF_PREPARED;
+		buf->state = CSI_BUF_PREPARED;
 	}
 
-	return 0;
+	vb2_set_plane_payload(vb, 0, new_size);
 
-fail:
-	free_buffer(vq, buf);
-out:
-	return ret;
+	return 0;
 }
 
 static enum pixel_fmt fourcc_to_ipu_pix(__u32 fourcc)
 {
 	/* Add more formats as need arises and test possibilities appear... */
 	switch (fourcc) {
-	case V4L2_PIX_FMT_RGB565:
-		return IPU_PIX_FMT_RGB565;
 	case V4L2_PIX_FMT_RGB24:
 		return IPU_PIX_FMT_RGB24;
-	case V4L2_PIX_FMT_RGB332:
-		return IPU_PIX_FMT_RGB332;
-	case V4L2_PIX_FMT_YUV422P:
-		return IPU_PIX_FMT_YVU422P;
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_RGB565:
 	default:
 		return IPU_PIX_FMT_GENERIC;
 	}
 }
 
-/*
- * Called with .vb_lock mutex held and
- * under spinlock_irqsave(&mx3_cam->lock, ...)
- */
-static void mx3_videobuf_queue(struct videobuf_queue *vq,
-			       struct videobuf_buffer *vb)
+static void mx3_videobuf_queue(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
-	struct mx3_camera_buffer *buf =
-		container_of(vb, struct mx3_camera_buffer, vb);
+	struct mx3_camera_buffer *buf = to_mx3_vb(vb);
 	struct dma_async_tx_descriptor *txd = buf->txd;
 	struct idmac_channel *ichan = to_idmac_chan(txd->chan);
 	struct idmac_video_param *video = &ichan->params.video;
 	dma_cookie_t cookie;
 	u32 fourcc = icd->current_fmt->host_fmt->fourcc;
-
-	BUG_ON(!irqs_disabled());
+	unsigned long flags;
 
 	/* This is the configuration of one sg-element */
 	video->out_pixel_fmt	= fourcc_to_ipu_pix(fourcc);
-	video->out_width	= icd->user_width;
-	video->out_height	= icd->user_height;
-	video->out_stride	= icd->user_width;
+
+	if (video->out_pixel_fmt == IPU_PIX_FMT_GENERIC) {
+		/*
+		 * If the IPU DMA channel is configured to transport
+		 * generic 8-bit data, we have to set up correctly the
+		 * geometry parameters upon the current pixel format.
+		 * So, since the DMA horizontal parameters are expressed
+		 * in bytes not pixels, convert these in the right unit.
+		 */
+		int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+		BUG_ON(bytes_per_line <= 0);
+
+		video->out_width	= bytes_per_line;
+		video->out_height	= icd->user_height;
+		video->out_stride	= bytes_per_line;
+	} else {
+		/*
+		 * For IPU known formats the pixel unit will be managed
+		 * successfully by the IPU code
+		 */
+		video->out_width	= icd->user_width;
+		video->out_height	= icd->user_height;
+		video->out_stride	= icd->user_width;
+	}
 
 #ifdef DEBUG
 	/* helps to see what DMA actually has written */
-	memset((void *)vb->baddr, 0xaa, vb->bsize);
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
 #endif
 
-	list_add_tail(&vb->queue, &mx3_cam->capture);
+	spin_lock_irqsave(&mx3_cam->lock, flags);
+	list_add_tail(&buf->queue, &mx3_cam->capture);
 
-	if (!mx3_cam->active) {
+	if (!mx3_cam->active)
 		mx3_cam->active = buf;
-		vb->state = VIDEOBUF_ACTIVE;
-	} else {
-		vb->state = VIDEOBUF_QUEUED;
-	}
 
 	spin_unlock_irq(&mx3_cam->lock);
 
@@ -383,67 +340,87 @@ static void mx3_videobuf_queue(struct videobuf_queue *vq,
 	dev_dbg(icd->dev.parent, "Submitted cookie %d DMA 0x%08x\n",
 		cookie, sg_dma_address(&buf->sg));
 
-	spin_lock_irq(&mx3_cam->lock);
-
 	if (cookie >= 0)
 		return;
 
-	/* Submit error */
-	vb->state = VIDEOBUF_PREPARED;
+	spin_lock_irq(&mx3_cam->lock);
 
-	list_del_init(&vb->queue);
+	/* Submit error */
+	list_del_init(&buf->queue);
 
 	if (mx3_cam->active == buf)
 		mx3_cam->active = NULL;
+
+	spin_unlock_irqrestore(&mx3_cam->lock, flags);
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 }
 
-/* Called with .vb_lock held */
-static void mx3_videobuf_release(struct videobuf_queue *vq,
-				 struct videobuf_buffer *vb)
+static void mx3_videobuf_release(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
-	struct mx3_camera_buffer *buf =
-		container_of(vb, struct mx3_camera_buffer, vb);
+	struct mx3_camera_buffer *buf = to_mx3_vb(vb);
+	struct dma_async_tx_descriptor *txd = buf->txd;
 	unsigned long flags;
 
 	dev_dbg(icd->dev.parent,
-		"Release%s DMA 0x%08x (state %d), queue %sempty\n",
+		"Release%s DMA 0x%08x, queue %sempty\n",
 		mx3_cam->active == buf ? " active" : "", sg_dma_address(&buf->sg),
-		vb->state, list_empty(&vb->queue) ? "" : "not ");
+		list_empty(&buf->queue) ? "" : "not ");
+
 	spin_lock_irqsave(&mx3_cam->lock, flags);
-	if ((vb->state == VIDEOBUF_ACTIVE || vb->state == VIDEOBUF_QUEUED) &&
-	    !list_empty(&vb->queue)) {
-		vb->state = VIDEOBUF_ERROR;
 
-		list_del_init(&vb->queue);
-		if (mx3_cam->active == buf)
-			mx3_cam->active = NULL;
+	if (mx3_cam->active == buf)
+		mx3_cam->active = NULL;
+
+	/* Doesn't hurt also if the list is empty */
+	list_del_init(&buf->queue);
+	buf->state = CSI_BUF_NEEDS_INIT;
+
+	if (txd) {
+		buf->txd = NULL;
+		if (mx3_cam->idmac_channel[0])
+			async_tx_ack(txd);
 	}
+
 	spin_unlock_irqrestore(&mx3_cam->lock, flags);
-	free_buffer(vq, buf);
 }
 
-static struct videobuf_queue_ops mx3_videobuf_ops = {
-	.buf_setup      = mx3_videobuf_setup,
-	.buf_prepare    = mx3_videobuf_prepare,
-	.buf_queue      = mx3_videobuf_queue,
-	.buf_release    = mx3_videobuf_release,
+static int mx3_videobuf_init(struct vb2_buffer *vb)
+{
+	struct mx3_camera_buffer *buf = to_mx3_vb(vb);
+	/* This is for locking debugging only */
+	INIT_LIST_HEAD(&buf->queue);
+	sg_init_table(&buf->sg, 1);
+
+	buf->state = CSI_BUF_NEEDS_INIT;
+	buf->txd = NULL;
+
+	return 0;
+}
+
+static struct vb2_ops mx3_videobuf_ops = {
+	.queue_setup	= mx3_videobuf_setup,
+	.buf_prepare	= mx3_videobuf_prepare,
+	.buf_queue	= mx3_videobuf_queue,
+	.buf_cleanup	= mx3_videobuf_release,
+	.buf_init	= mx3_videobuf_init,
+	.wait_prepare	= soc_camera_unlock,
+	.wait_finish	= soc_camera_lock,
 };
 
-static void mx3_camera_init_videobuf(struct videobuf_queue *q,
+static int mx3_camera_init_videobuf(struct vb2_queue *q,
 				     struct soc_camera_device *icd)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
-	struct mx3_camera_dev *mx3_cam = ici->priv;
-
-	videobuf_queue_dma_contig_init(q, &mx3_videobuf_ops, icd->dev.parent,
-				       &mx3_cam->lock,
-				       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-				       V4L2_FIELD_NONE,
-				       sizeof(struct mx3_camera_buffer), icd,
-				       &icd->video_lock);
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &mx3_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct mx3_camera_buffer);
+
+	return vb2_queue_init(q);
 }
 
 /* First part of ipu_csi_init_interface() */
@@ -538,18 +515,6 @@ static void mx3_camera_remove_device(struct soc_camera_device *icd)
 		 icd->devnum);
 }
 
-static bool channel_change_requested(struct soc_camera_device *icd,
-				     struct v4l2_rect *rect)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
-	struct mx3_camera_dev *mx3_cam = ici->priv;
-	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
-
-	/* Do buffers have to be re-allocated or channel re-configured? */
-	return ichan && rect->width * rect->height >
-		icd->user_width * icd->user_height;
-}
-
 static int test_platform_param(struct mx3_camera_dev *mx3_cam,
 			       unsigned char buswidth, unsigned long *flags)
 {
@@ -734,18 +699,36 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	if (xlate) {
 		xlate->host_fmt	= fmt;
 		xlate->code	= code;
+		dev_dbg(dev, "Providing format %c%c%c%c in pass-through mode\n",
+			(fmt->fourcc >> (0*8)) & 0xFF,
+			(fmt->fourcc >> (1*8)) & 0xFF,
+			(fmt->fourcc >> (2*8)) & 0xFF,
+			(fmt->fourcc >> (3*8)) & 0xFF);
 		xlate++;
-		dev_dbg(dev, "Providing format %x in pass-through mode\n",
-			xlate->host_fmt->fourcc);
 	}
 
 	return formats;
 }
 
 static void configure_geometry(struct mx3_camera_dev *mx3_cam,
-			       unsigned int width, unsigned int height)
+			       unsigned int width, unsigned int height,
+			       enum v4l2_mbus_pixelcode code)
 {
 	u32 ctrl, width_field, height_field;
+	const struct soc_mbus_pixelfmt *fmt;
+
+	fmt = soc_mbus_get_fmtdesc(code);
+	BUG_ON(!fmt);
+
+	if (fourcc_to_ipu_pix(fmt->fourcc) == IPU_PIX_FMT_GENERIC) {
+		/*
+		 * As the CSI will be configured to output BAYER, here
+		 * the width parameter count the number of samples to
+		 * capture to complete the whole image width.
+		 */
+		width *= soc_mbus_samples_per_pixel(fmt);
+		BUG_ON(width < 0);
+	}
 
 	/* Setup frame size - this cannot be changed on-the-fly... */
 	width_field = width - 1;
@@ -772,18 +755,6 @@ static int acquire_dma_channel(struct mx3_camera_dev *mx3_cam)
 	struct dma_chan_request rq = {.mx3_cam = mx3_cam,
 				      .id = IDMAC_IC_7};
 
-	if (*ichan) {
-		struct videobuf_buffer *vb, *_vb;
-		dma_release_channel(&(*ichan)->dma_chan);
-		*ichan = NULL;
-		mx3_cam->active = NULL;
-		list_for_each_entry_safe(vb, _vb, &mx3_cam->capture, queue) {
-			list_del_init(&vb->queue);
-			vb->state = VIDEOBUF_ERROR;
-			wake_up(&vb->done);
-		}
-	}
-
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 	dma_cap_set(DMA_PRIVATE, mask);
@@ -843,19 +814,8 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
 			return ret;
 	}
 
-	if (mf.width != icd->user_width || mf.height != icd->user_height) {
-		/*
-		 * We now know pixel formats and can decide upon DMA-channel(s)
-		 * So far only direct camera-to-memory is supported
-		 */
-		if (channel_change_requested(icd, rect)) {
-			ret = acquire_dma_channel(mx3_cam);
-			if (ret < 0)
-				return ret;
-		}
-
-		configure_geometry(mx3_cam, mf.width, mf.height);
-	}
+	if (mf.width != icd->user_width || mf.height != icd->user_height)
+		configure_geometry(mx3_cam, mf.width, mf.height, mf.code);
 
 	dev_dbg(icd->dev.parent, "Sensor cropped %dx%d\n",
 		mf.width, mf.height);
@@ -887,17 +847,13 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 	stride_align(&pix->width);
 	dev_dbg(icd->dev.parent, "Set format %dx%d\n", pix->width, pix->height);
 
-	ret = acquire_dma_channel(mx3_cam);
-	if (ret < 0)
-		return ret;
-
 	/*
 	 * Might have to perform a complete interface initialisation like in
 	 * ipu_csi_init_interface() in mxc_v4l2_s_param(). Also consider
 	 * mxc_v4l2_s_fmt()
 	 */
 
-	configure_geometry(mx3_cam, pix->width, pix->height);
+	configure_geometry(mx3_cam, pix->width, pix->height, xlate->code);
 
 	mf.width	= pix->width;
 	mf.height	= pix->height;
@@ -912,12 +868,25 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 	if (mf.code != xlate->code)
 		return -EINVAL;
 
+	if (!mx3_cam->idmac_channel[0]) {
+		ret = acquire_dma_channel(mx3_cam);
+		if (ret < 0)
+			return ret;
+	}
+
 	pix->width		= mf.width;
 	pix->height		= mf.height;
 	pix->field		= mf.field;
+	mx3_cam->field		= mf.field;
 	pix->colorspace		= mf.colorspace;
 	icd->current_fmt	= xlate;
 
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	if (pix->bytesperline < 0)
+		return pix->bytesperline;
+	pix->sizeimage = pix->height * pix->bytesperline;
+
 	dev_dbg(icd->dev.parent, "Sensor set %dx%d\n", pix->width, pix->height);
 
 	return ret;
@@ -991,7 +960,7 @@ static unsigned int mx3_camera_poll(struct file *file, poll_table *pt)
 {
 	struct soc_camera_device *icd = file->private_data;
 
-	return videobuf_poll_stream(file, &icd->vb_vidq, pt);
+	return vb2_poll(&icd->vb2_vidq, file, pt);
 }
 
 static int mx3_camera_querycap(struct soc_camera_host *ici,
@@ -1165,7 +1134,7 @@ static struct soc_camera_host_ops mx3_soc_camera_host_ops = {
 	.set_fmt	= mx3_camera_set_fmt,
 	.try_fmt	= mx3_camera_try_fmt,
 	.get_formats	= mx3_camera_get_formats,
-	.init_videobuf	= mx3_camera_init_videobuf,
+	.init_videobuf2	= mx3_camera_init_videobuf,
 	.reqbufs	= mx3_camera_reqbufs,
 	.poll		= mx3_camera_poll,
 	.querycap	= mx3_camera_querycap,
@@ -1241,6 +1210,12 @@ static int __devinit mx3_camera_probe(struct platform_device *pdev)
 	soc_host->v4l2_dev.dev	= &pdev->dev;
 	soc_host->nr		= pdev->id;
 
+	mx3_cam->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(mx3_cam->alloc_ctx)) {
+		err = PTR_ERR(mx3_cam->alloc_ctx);
+		goto eallocctx;
+	}
+
 	err = soc_camera_host_register(soc_host);
 	if (err)
 		goto ecamhostreg;
@@ -1251,6 +1226,8 @@ static int __devinit mx3_camera_probe(struct platform_device *pdev)
 	return 0;
 
 ecamhostreg:
+	vb2_dma_contig_cleanup_ctx(mx3_cam->alloc_ctx);
+eallocctx:
 	iounmap(base);
 eioremap:
 	clk_put(mx3_cam->clk);
@@ -1280,6 +1257,8 @@ static int __devexit mx3_camera_remove(struct platform_device *pdev)
 	if (WARN_ON(mx3_cam->idmac_channel[0]))
 		dma_release_channel(&mx3_cam->idmac_channel[0]->dma_chan);
 
+	vb2_dma_contig_cleanup_ctx(mx3_cam->alloc_ctx);
+
 	vfree(mx3_cam);
 
 	dmaengine_put();
diff --git a/drivers/media/video/noon010pc30.c b/drivers/media/video/noon010pc30.c
new file mode 100644
index 000000000000..35f722a88f76
--- /dev/null
+++ b/drivers/media/video/noon010pc30.c
@@ -0,0 +1,792 @@
+/*
+ * Driver for SiliconFile NOON010PC30 CIF (1/11") Image Sensor with ISP
+ *
+ * Copyright (C) 2010 Samsung Electronics
+ * Contact: Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ *
+ * Initial register configuration based on a driver authored by
+ * HeungJun Kim <riverful.kim@samsung.com>.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later vergsion.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <media/noon010pc30.h>
+#include <media/v4l2-chip-ident.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Enable module debug trace. Set to 1 to enable.");
+
+#define MODULE_NAME		"NOON010PC30"
+
+/*
+ * Register offsets within a page
+ * b15..b8 - page id, b7..b0 - register address
+ */
+#define POWER_CTRL_REG		0x0001
+#define PAGEMODE_REG		0x03
+#define DEVICE_ID_REG		0x0004
+#define NOON010PC30_ID		0x86
+#define VDO_CTL_REG(n)		(0x0010 + (n))
+#define SYNC_CTL_REG		0x0012
+/* Window size and position */
+#define WIN_ROWH_REG		0x0013
+#define WIN_ROWL_REG		0x0014
+#define WIN_COLH_REG		0x0015
+#define WIN_COLL_REG		0x0016
+#define WIN_HEIGHTH_REG		0x0017
+#define WIN_HEIGHTL_REG		0x0018
+#define WIN_WIDTHH_REG		0x0019
+#define WIN_WIDTHL_REG		0x001A
+#define HBLANKH_REG		0x001B
+#define HBLANKL_REG		0x001C
+#define VSYNCH_REG		0x001D
+#define VSYNCL_REG		0x001E
+/* VSYNC control */
+#define VS_CTL_REG(n)		(0x00A1 + (n))
+/* page 1 */
+#define ISP_CTL_REG(n)		(0x0110 + (n))
+#define YOFS_REG		0x0119
+#define DARK_YOFS_REG		0x011A
+#define SAT_CTL_REG		0x0120
+#define BSAT_REG		0x0121
+#define RSAT_REG		0x0122
+/* Color correction */
+#define CMC_CTL_REG		0x0130
+#define CMC_OFSGH_REG		0x0133
+#define CMC_OFSGL_REG		0x0135
+#define CMC_SIGN_REG		0x0136
+#define CMC_GOFS_REG		0x0137
+#define CMC_COEF_REG(n)		(0x0138 + (n))
+#define CMC_OFS_REG(n)		(0x0141 + (n))
+/* Gamma correction */
+#define GMA_CTL_REG		0x0160
+#define GMA_COEF_REG(n)		(0x0161 + (n))
+/* Lens Shading */
+#define LENS_CTRL_REG		0x01D0
+#define LENS_XCEN_REG		0x01D1
+#define LENS_YCEN_REG		0x01D2
+#define LENS_RC_REG		0x01D3
+#define LENS_GC_REG		0x01D4
+#define LENS_BC_REG		0x01D5
+#define L_AGON_REG		0x01D6
+#define L_AGOFF_REG		0x01D7
+/* Page 3 - Auto Exposure */
+#define AE_CTL_REG(n)		(0x0310 + (n))
+#define AE_CTL9_REG		0x032C
+#define AE_CTL10_REG		0x032D
+#define AE_YLVL_REG		0x031C
+#define AE_YTH_REG(n)		(0x031D + (n))
+#define AE_WGT_REG		0x0326
+#define EXP_TIMEH_REG		0x0333
+#define EXP_TIMEM_REG		0x0334
+#define EXP_TIMEL_REG		0x0335
+#define EXP_MMINH_REG		0x0336
+#define EXP_MMINL_REG		0x0337
+#define EXP_MMAXH_REG		0x0338
+#define EXP_MMAXM_REG		0x0339
+#define EXP_MMAXL_REG		0x033A
+/* Page 4 - Auto White Balance */
+#define AWB_CTL_REG(n)		(0x0410 + (n))
+#define AWB_ENABE		0x80
+#define AWB_WGHT_REG		0x0419
+#define BGAIN_PAR_REG(n)	(0x044F + (n))
+/* Manual white balance, when AWB_CTL2[0]=1 */
+#define MWB_RGAIN_REG		0x0466
+#define MWB_BGAIN_REG		0x0467
+
+/* The token to mark an array end */
+#define REG_TERM		0xFFFF
+
+struct noon010_format {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace colorspace;
+	u16 ispctl1_reg;
+};
+
+struct noon010_frmsize {
+	u16 width;
+	u16 height;
+	int vid_ctl1;
+};
+
+static const char * const noon010_supply_name[] = {
+	"vdd_core", "vddio", "vdda"
+};
+
+#define NOON010_NUM_SUPPLIES ARRAY_SIZE(noon010_supply_name)
+
+struct noon010_info {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	const struct noon010pc30_platform_data *pdata;
+	const struct noon010_format *curr_fmt;
+	const struct noon010_frmsize *curr_win;
+	unsigned int hflip:1;
+	unsigned int vflip:1;
+	unsigned int power:1;
+	u8 i2c_reg_page;
+	struct regulator_bulk_data supply[NOON010_NUM_SUPPLIES];
+	u32 gpio_nreset;
+	u32 gpio_nstby;
+};
+
+struct i2c_regval {
+	u16 addr;
+	u16 val;
+};
+
+/* Supported resolutions. */
+static const struct noon010_frmsize noon010_sizes[] = {
+	{
+		.width		= 352,
+		.height		= 288,
+		.vid_ctl1	= 0,
+	}, {
+		.width		= 176,
+		.height		= 144,
+		.vid_ctl1	= 0x10,
+	}, {
+		.width		= 88,
+		.height		= 72,
+		.vid_ctl1	= 0x20,
+	},
+};
+
+/* Supported pixel formats. */
+static const struct noon010_format noon010_formats[] = {
+	{
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x03,
+	}, {
+		.code		= V4L2_MBUS_FMT_YVYU8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x02,
+	}, {
+		.code		= V4L2_MBUS_FMT_VYUY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0,
+	}, {
+		.code		= V4L2_MBUS_FMT_UYVY8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x01,
+	}, {
+		.code		= V4L2_MBUS_FMT_RGB565_2X8_BE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.ispctl1_reg	= 0x40,
+	},
+};
+
+static const struct i2c_regval noon010_base_regs[] = {
+	{ WIN_COLL_REG,		0x06 },	{ HBLANKL_REG,		0x7C },
+	/* Color corection and saturation */
+	{ ISP_CTL_REG(0),	0x30 }, { ISP_CTL_REG(2),	0x30 },
+	{ YOFS_REG,		0x80 }, { DARK_YOFS_REG,	0x04 },
+	{ SAT_CTL_REG,		0x1F }, { BSAT_REG,		0x90 },
+	{ CMC_CTL_REG,		0x0F }, { CMC_OFSGH_REG,	0x3C },
+	{ CMC_OFSGL_REG,	0x2C }, { CMC_SIGN_REG,		0x3F },
+	{ CMC_COEF_REG(0),	0x79 }, { CMC_OFS_REG(0),	0x00 },
+	{ CMC_COEF_REG(1),	0x39 }, { CMC_OFS_REG(1),	0x00 },
+	{ CMC_COEF_REG(2),	0x00 }, { CMC_OFS_REG(2),	0x00 },
+	{ CMC_COEF_REG(3),	0x11 }, { CMC_OFS_REG(3),	0x8B },
+	{ CMC_COEF_REG(4),	0x65 }, { CMC_OFS_REG(4),	0x07 },
+	{ CMC_COEF_REG(5),	0x14 }, { CMC_OFS_REG(5),	0x04 },
+	{ CMC_COEF_REG(6),	0x01 }, { CMC_OFS_REG(6),	0x9C },
+	{ CMC_COEF_REG(7),	0x33 }, { CMC_OFS_REG(7),	0x89 },
+	{ CMC_COEF_REG(8),	0x74 }, { CMC_OFS_REG(8),	0x25 },
+	/* Automatic white balance */
+	{ AWB_CTL_REG(0),	0x78 }, { AWB_CTL_REG(1),	0x2E },
+	{ AWB_CTL_REG(2),	0x20 }, { AWB_CTL_REG(3),	0x85 },
+	/* Auto exposure */
+	{ AE_CTL_REG(0),	0xDC }, { AE_CTL_REG(1),	0x81 },
+	{ AE_CTL_REG(2),	0x30 }, { AE_CTL_REG(3),	0xA5 },
+	{ AE_CTL_REG(4),	0x40 }, { AE_CTL_REG(5),	0x51 },
+	{ AE_CTL_REG(6),	0x33 }, { AE_CTL_REG(7),	0x7E },
+	{ AE_CTL9_REG,		0x00 }, { AE_CTL10_REG,		0x02 },
+	{ AE_YLVL_REG,		0x44 },	{ AE_YTH_REG(0),	0x34 },
+	{ AE_YTH_REG(1),	0x30 },	{ AE_WGT_REG,		0xD5 },
+	/* Lens shading compensation */
+	{ LENS_CTRL_REG,	0x01 }, { LENS_XCEN_REG,	0x80 },
+	{ LENS_YCEN_REG,	0x70 }, { LENS_RC_REG,		0x53 },
+	{ LENS_GC_REG,		0x40 }, { LENS_BC_REG,		0x3E },
+	{ REG_TERM,		0 },
+};
+
+static inline struct noon010_info *to_noon010(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct noon010_info, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct noon010_info, hdl)->sd;
+}
+
+static inline int set_i2c_page(struct noon010_info *info,
+			       struct i2c_client *client, unsigned int reg)
+{
+	u32 page = reg >> 8 & 0xFF;
+	int ret = 0;
+
+	if (info->i2c_reg_page != page && (reg & 0xFF) != 0x03) {
+		ret = i2c_smbus_write_byte_data(client, PAGEMODE_REG, page);
+		if (!ret)
+			info->i2c_reg_page = page;
+	}
+	return ret;
+}
+
+static int cam_i2c_read(struct v4l2_subdev *sd, u32 reg_addr)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct noon010_info *info = to_noon010(sd);
+	int ret = set_i2c_page(info, client, reg_addr);
+
+	if (ret)
+		return ret;
+	return i2c_smbus_read_byte_data(client, reg_addr & 0xFF);
+}
+
+static int cam_i2c_write(struct v4l2_subdev *sd, u32 reg_addr, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct noon010_info *info = to_noon010(sd);
+	int ret = set_i2c_page(info, client, reg_addr);
+
+	if (ret)
+		return ret;
+	return i2c_smbus_write_byte_data(client, reg_addr & 0xFF, val);
+}
+
+static inline int noon010_bulk_write_reg(struct v4l2_subdev *sd,
+					 const struct i2c_regval *msg)
+{
+	while (msg->addr != REG_TERM) {
+		int ret = cam_i2c_write(sd, msg->addr, msg->val);
+
+		if (ret)
+			return ret;
+		msg++;
+	}
+	return 0;
+}
+
+/* Device reset and sleep mode control */
+static int noon010_power_ctrl(struct v4l2_subdev *sd, bool reset, bool sleep)
+{
+	struct noon010_info *info = to_noon010(sd);
+	u8 reg = sleep ? 0xF1 : 0xF0;
+	int ret = 0;
+
+	if (reset)
+		ret = cam_i2c_write(sd, POWER_CTRL_REG, reg | 0x02);
+	if (!ret) {
+		ret = cam_i2c_write(sd, POWER_CTRL_REG, reg);
+		if (reset && !ret)
+			info->i2c_reg_page = -1;
+	}
+	return ret;
+}
+
+/* Automatic white balance control */
+static int noon010_enable_autowhitebalance(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	ret = cam_i2c_write(sd, AWB_CTL_REG(1), on ? 0x2E : 0x2F);
+	if (!ret)
+		ret = cam_i2c_write(sd, AWB_CTL_REG(0), on ? 0xFB : 0x7B);
+	return ret;
+}
+
+static int noon010_set_flip(struct v4l2_subdev *sd, int hflip, int vflip)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int reg, ret;
+
+	reg = cam_i2c_read(sd, VDO_CTL_REG(1));
+	if (reg < 0)
+		return reg;
+
+	reg &= 0x7C;
+	if (hflip)
+		reg |= 0x01;
+	if (vflip)
+		reg |= 0x02;
+
+	ret = cam_i2c_write(sd, VDO_CTL_REG(1), reg | 0x80);
+	if (!ret) {
+		info->hflip = hflip;
+		info->vflip = vflip;
+	}
+	return ret;
+}
+
+/* Configure resolution and color format */
+static int noon010_set_params(struct v4l2_subdev *sd)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int ret;
+
+	if (!info->curr_win)
+		return -EINVAL;
+
+	ret = cam_i2c_write(sd, VDO_CTL_REG(0), info->curr_win->vid_ctl1);
+
+	if (!ret && info->curr_fmt)
+		ret = cam_i2c_write(sd, ISP_CTL_REG(0),
+				info->curr_fmt->ispctl1_reg);
+	return ret;
+}
+
+/* Find nearest matching image pixel size. */
+static int noon010_try_frame_size(struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int min_err = ~0;
+	int i = ARRAY_SIZE(noon010_sizes);
+	const struct noon010_frmsize *fsize = &noon010_sizes[0],
+		*match = NULL;
+
+	while (i--) {
+		int err = abs(fsize->width - mf->width)
+				+ abs(fsize->height - mf->height);
+
+		if (err < min_err) {
+			min_err = err;
+			match = fsize;
+		}
+		fsize++;
+	}
+	if (match) {
+		mf->width  = match->width;
+		mf->height = match->height;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int power_enable(struct noon010_info *info)
+{
+	int ret;
+
+	if (info->power) {
+		v4l2_info(&info->sd, "%s: sensor is already on\n", __func__);
+		return 0;
+	}
+
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_set_value(info->gpio_nstby, 0);
+
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_set_value(info->gpio_nreset, 0);
+
+	ret = regulator_bulk_enable(NOON010_NUM_SUPPLIES, info->supply);
+	if (ret)
+		return ret;
+
+	if (gpio_is_valid(info->gpio_nreset)) {
+		msleep(50);
+		gpio_set_value(info->gpio_nreset, 1);
+	}
+	if (gpio_is_valid(info->gpio_nstby)) {
+		udelay(1000);
+		gpio_set_value(info->gpio_nstby, 1);
+	}
+	if (gpio_is_valid(info->gpio_nreset)) {
+		udelay(1000);
+		gpio_set_value(info->gpio_nreset, 0);
+		msleep(100);
+		gpio_set_value(info->gpio_nreset, 1);
+		msleep(20);
+	}
+	info->power = 1;
+
+	v4l2_dbg(1, debug, &info->sd,  "%s: sensor is on\n", __func__);
+	return 0;
+}
+
+static int power_disable(struct noon010_info *info)
+{
+	int ret;
+
+	if (!info->power) {
+		v4l2_info(&info->sd, "%s: sensor is already off\n", __func__);
+		return 0;
+	}
+
+	ret = regulator_bulk_disable(NOON010_NUM_SUPPLIES, info->supply);
+	if (ret)
+		return ret;
+
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_set_value(info->gpio_nstby, 0);
+
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_set_value(info->gpio_nreset, 0);
+
+	info->power = 0;
+
+	v4l2_dbg(1, debug, &info->sd,  "%s: sensor is off\n", __func__);
+
+	return 0;
+}
+
+static int noon010_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	v4l2_dbg(1, debug, sd, "%s: ctrl_id: %d, value: %d\n",
+		 __func__, ctrl->id, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		return noon010_enable_autowhitebalance(sd, ctrl->val);
+	case V4L2_CID_BLUE_BALANCE:
+		return cam_i2c_write(sd, MWB_BGAIN_REG, ctrl->val);
+	case V4L2_CID_RED_BALANCE:
+		return cam_i2c_write(sd, MWB_RGAIN_REG, ctrl->val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int noon010_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			    enum v4l2_mbus_pixelcode *code)
+{
+	if (!code || index >= ARRAY_SIZE(noon010_formats))
+		return -EINVAL;
+
+	*code = noon010_formats[index].code;
+	return 0;
+}
+
+static int noon010_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int ret;
+
+	if (!mf)
+		return -EINVAL;
+
+	if (!info->curr_win || !info->curr_fmt) {
+		ret = noon010_set_params(sd);
+		if (ret)
+			return ret;
+	}
+
+	mf->width	= info->curr_win->width;
+	mf->height	= info->curr_win->height;
+	mf->code	= info->curr_fmt->code;
+	mf->colorspace	= info->curr_fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+/* Return nearest media bus frame format. */
+static const struct noon010_format *try_fmt(struct v4l2_subdev *sd,
+					    struct v4l2_mbus_framefmt *mf)
+{
+	int i = ARRAY_SIZE(noon010_formats);
+
+	noon010_try_frame_size(mf);
+
+	while (i--)
+		if (mf->code == noon010_formats[i].code)
+			break;
+
+	mf->code = noon010_formats[i].code;
+
+	return &noon010_formats[i];
+}
+
+static int noon010_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	if (!sd || !mf)
+		return -EINVAL;
+
+	try_fmt(sd, mf);
+	return 0;
+}
+
+static int noon010_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct noon010_info *info = to_noon010(sd);
+
+	if (!sd || !mf)
+		return -EINVAL;
+
+	info->curr_fmt = try_fmt(sd, mf);
+
+	return noon010_set_params(sd);
+}
+
+static int noon010_base_config(struct v4l2_subdev *sd)
+{
+	struct noon010_info *info = to_noon010(sd);
+	int ret;
+
+	ret = noon010_bulk_write_reg(sd, noon010_base_regs);
+	if (!ret) {
+		info->curr_fmt = &noon010_formats[0];
+		info->curr_win = &noon010_sizes[0];
+		ret = noon010_set_params(sd);
+	}
+	if (!ret)
+		ret = noon010_set_flip(sd, 1, 0);
+	if (!ret)
+		ret = noon010_power_ctrl(sd, false, false);
+
+	/* sync the handler and the registers state */
+	v4l2_ctrl_handler_setup(&to_noon010(sd)->hdl);
+	return ret;
+}
+
+static int noon010_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct noon010_info *info = to_noon010(sd);
+	const struct noon010pc30_platform_data *pdata = info->pdata;
+	int ret = 0;
+
+	if (WARN(pdata == NULL, "No platform data!\n"))
+		return -ENOMEM;
+
+	if (on) {
+		ret = power_enable(info);
+		if (ret)
+			return ret;
+		ret = noon010_base_config(sd);
+	} else {
+		noon010_power_ctrl(sd, false, true);
+		ret = power_disable(info);
+		info->curr_win = NULL;
+		info->curr_fmt = NULL;
+	}
+
+	return ret;
+}
+
+static int noon010_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip,
+					  V4L2_IDENT_NOON010PC30, 0);
+}
+
+static int noon010_log_status(struct v4l2_subdev *sd)
+{
+	struct noon010_info *info = to_noon010(sd);
+
+	v4l2_ctrl_handler_log_status(&info->hdl, sd->name);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops noon010_ctrl_ops = {
+	.s_ctrl = noon010_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops noon010_core_ops = {
+	.g_chip_ident	= noon010_g_chip_ident,
+	.s_power	= noon010_s_power,
+	.g_ctrl		= v4l2_subdev_g_ctrl,
+	.s_ctrl		= v4l2_subdev_s_ctrl,
+	.queryctrl	= v4l2_subdev_queryctrl,
+	.querymenu	= v4l2_subdev_querymenu,
+	.g_ext_ctrls	= v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls	= v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls	= v4l2_subdev_s_ext_ctrls,
+	.log_status	= noon010_log_status,
+};
+
+static const struct v4l2_subdev_video_ops noon010_video_ops = {
+	.g_mbus_fmt	= noon010_g_fmt,
+	.s_mbus_fmt	= noon010_s_fmt,
+	.try_mbus_fmt	= noon010_try_fmt,
+	.enum_mbus_fmt	= noon010_enum_fmt,
+};
+
+static const struct v4l2_subdev_ops noon010_ops = {
+	.core	= &noon010_core_ops,
+	.video	= &noon010_video_ops,
+};
+
+/* Return 0 if NOON010PC30L sensor type was detected or -ENODEV otherwise. */
+static int noon010_detect(struct i2c_client *client, struct noon010_info *info)
+{
+	int ret;
+
+	ret = power_enable(info);
+	if (ret)
+		return ret;
+
+	ret = i2c_smbus_read_byte_data(client, DEVICE_ID_REG);
+	if (ret < 0)
+		dev_err(&client->dev, "I2C read failed: 0x%X\n", ret);
+
+	power_disable(info);
+
+	return ret == NOON010PC30_ID ? 0 : -ENODEV;
+}
+
+static int noon010_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct noon010_info *info;
+	struct v4l2_subdev *sd;
+	const struct noon010pc30_platform_data *pdata
+		= client->dev.platform_data;
+	int ret;
+	int i;
+
+	if (!pdata) {
+		dev_err(&client->dev, "No platform data!\n");
+		return -EIO;
+	}
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	sd = &info->sd;
+	strlcpy(sd->name, MODULE_NAME, sizeof(sd->name));
+	v4l2_i2c_subdev_init(sd, client, &noon010_ops);
+
+	v4l2_ctrl_handler_init(&info->hdl, 3);
+
+	v4l2_ctrl_new_std(&info->hdl, &noon010_ctrl_ops,
+			  V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	v4l2_ctrl_new_std(&info->hdl, &noon010_ctrl_ops,
+			  V4L2_CID_RED_BALANCE, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&info->hdl, &noon010_ctrl_ops,
+			  V4L2_CID_BLUE_BALANCE, 0, 127, 1, 64);
+
+	sd->ctrl_handler = &info->hdl;
+
+	ret = info->hdl.error;
+	if (ret)
+		goto np_err;
+
+	info->pdata		= client->dev.platform_data;
+	info->i2c_reg_page	= -1;
+	info->gpio_nreset	= -EINVAL;
+	info->gpio_nstby	= -EINVAL;
+
+	if (gpio_is_valid(pdata->gpio_nreset)) {
+		ret = gpio_request(pdata->gpio_nreset, "NOON010PC30 NRST");
+		if (ret) {
+			dev_err(&client->dev, "GPIO request error: %d\n", ret);
+			goto np_err;
+		}
+		info->gpio_nreset = pdata->gpio_nreset;
+		gpio_direction_output(info->gpio_nreset, 0);
+		gpio_export(info->gpio_nreset, 0);
+	}
+
+	if (gpio_is_valid(pdata->gpio_nstby)) {
+		ret = gpio_request(pdata->gpio_nstby, "NOON010PC30 NSTBY");
+		if (ret) {
+			dev_err(&client->dev, "GPIO request error: %d\n", ret);
+			goto np_gpio_err;
+		}
+		info->gpio_nstby = pdata->gpio_nstby;
+		gpio_direction_output(info->gpio_nstby, 0);
+		gpio_export(info->gpio_nstby, 0);
+	}
+
+	for (i = 0; i < NOON010_NUM_SUPPLIES; i++)
+		info->supply[i].supply = noon010_supply_name[i];
+
+	ret = regulator_bulk_get(&client->dev, NOON010_NUM_SUPPLIES,
+				 info->supply);
+	if (ret)
+		goto np_reg_err;
+
+	ret = noon010_detect(client, info);
+	if (!ret)
+		return 0;
+
+	/* the sensor detection failed */
+	regulator_bulk_free(NOON010_NUM_SUPPLIES, info->supply);
+np_reg_err:
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_free(info->gpio_nstby);
+np_gpio_err:
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_free(info->gpio_nreset);
+np_err:
+	v4l2_ctrl_handler_free(&info->hdl);
+	v4l2_device_unregister_subdev(sd);
+	kfree(info);
+	return ret;
+}
+
+static int noon010_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct noon010_info *info = to_noon010(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&info->hdl);
+
+	regulator_bulk_free(NOON010_NUM_SUPPLIES, info->supply);
+
+	if (gpio_is_valid(info->gpio_nreset))
+		gpio_free(info->gpio_nreset);
+
+	if (gpio_is_valid(info->gpio_nstby))
+		gpio_free(info->gpio_nstby);
+
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id noon010_id[] = {
+	{ MODULE_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, noon010_id);
+
+
+static struct i2c_driver noon010_i2c_driver = {
+	.driver = {
+		.name = MODULE_NAME
+	},
+	.probe		= noon010_probe,
+	.remove		= noon010_remove,
+	.id_table	= noon010_id,
+};
+
+static int __init noon010_init(void)
+{
+	return i2c_add_driver(&noon010_i2c_driver);
+}
+
+static void __exit noon010_exit(void)
+{
+	i2c_del_driver(&noon010_i2c_driver);
+}
+
+module_init(noon010_init);
+module_exit(noon010_exit);
+
+MODULE_DESCRIPTION("Siliconfile NOON010PC30 camera driver");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/omap1_camera.c b/drivers/media/video/omap1_camera.c
index 0a2fb2bfdbfb..eab31cbd68eb 100644
--- a/drivers/media/video/omap1_camera.c
+++ b/drivers/media/video/omap1_camera.c
@@ -811,8 +811,8 @@ static irqreturn_t cam_isr(int irq, void *data)
 	spin_lock_irqsave(&pcdev->lock, flags);
 
 	if (WARN_ON(!buf)) {
-		dev_warn(dev, "%s: unhandled camera interrupt, status == "
-				"%#x\n", __func__, it_status);
+		dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
+			 __func__, it_status);
 		suspend_capture(pcdev);
 		disable_capture(pcdev);
 		goto out;
@@ -1088,15 +1088,15 @@ static int omap1_cam_get_formats(struct soc_camera_device *icd,
 			xlate->host_fmt	= &omap1_cam_formats[code];
 			xlate->code	= code;
 			xlate++;
-			dev_dbg(dev, "%s: providing format %s "
-					"as byte swapped code #%d\n", __func__,
-					omap1_cam_formats[code].name, code);
+			dev_dbg(dev,
+				"%s: providing format %s as byte swapped code #%d\n",
+				__func__, omap1_cam_formats[code].name, code);
 		}
 	default:
 		if (xlate)
-			dev_dbg(dev, "%s: providing format %s "
-					"in pass-through mode\n", __func__,
-					fmt->name);
+			dev_dbg(dev,
+				"%s: providing format %s in pass-through mode\n",
+				__func__, fmt->name);
 	}
 	formats++;
 	if (xlate) {
@@ -1139,29 +1139,29 @@ static int dma_align(int *width, int *height,
 	return 1;
 }
 
-#define subdev_call_with_sense(pcdev, dev, icd, sd, function, args...)	   \
-({									   \
-	struct soc_camera_sense sense = {				   \
-		.master_clock		= pcdev->camexclk,		   \
-		.pixel_clock_max	= 0,				   \
-	};								   \
-	int __ret;							   \
-									   \
-	if (pcdev->pdata)						   \
-		sense.pixel_clock_max = pcdev->pdata->lclk_khz_max * 1000; \
-	icd->sense = &sense;						   \
-	__ret = v4l2_subdev_call(sd, video, function, ##args);		   \
-	icd->sense = NULL;						   \
-									   \
-	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {			   \
-		if (sense.pixel_clock > sense.pixel_clock_max) {	   \
-			dev_err(dev, "%s: pixel clock %lu "		   \
-					"set by the camera too high!\n",   \
-					__func__, sense.pixel_clock);	   \
-			__ret = -EINVAL;				   \
-		}							   \
-	}								   \
-	__ret;								   \
+#define subdev_call_with_sense(pcdev, dev, icd, sd, function, args...)		     \
+({										     \
+	struct soc_camera_sense sense = {					     \
+		.master_clock		= pcdev->camexclk,			     \
+		.pixel_clock_max	= 0,					     \
+	};									     \
+	int __ret;								     \
+										     \
+	if (pcdev->pdata)							     \
+		sense.pixel_clock_max = pcdev->pdata->lclk_khz_max * 1000;	     \
+	icd->sense = &sense;							     \
+	__ret = v4l2_subdev_call(sd, video, function, ##args);			     \
+	icd->sense = NULL;							     \
+										     \
+	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {				     \
+		if (sense.pixel_clock > sense.pixel_clock_max) {		     \
+			dev_err(dev,						     \
+				"%s: pixel clock %lu set by the camera too high!\n", \
+				__func__, sense.pixel_clock);			     \
+			__ret = -EINVAL;					     \
+		}								     \
+	}									     \
+	__ret;									     \
 })
 
 static int set_mbus_format(struct omap1_cam_dev *pcdev, struct device *dev,
@@ -1664,10 +1664,10 @@ static int __exit omap1_cam_remove(struct platform_device *pdev)
 	res = pcdev->res;
 	release_mem_region(res->start, resource_size(res));
 
-	kfree(pcdev);
-
 	clk_put(pcdev->clk);
 
+	kfree(pcdev);
+
 	dev_info(&pdev->dev, "OMAP1 Camera Interface driver unloaded\n");
 
 	return 0;
diff --git a/drivers/media/video/omap24xxcam.c b/drivers/media/video/omap24xxcam.c
index 017552762902..f6626e87dbc5 100644
--- a/drivers/media/video/omap24xxcam.c
+++ b/drivers/media/video/omap24xxcam.c
@@ -36,6 +36,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/sched.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
diff --git a/drivers/media/video/ov9740.c b/drivers/media/video/ov9740.c
new file mode 100644
index 000000000000..4d4ee4faca69
--- /dev/null
+++ b/drivers/media/video/ov9740.c
@@ -0,0 +1,1009 @@
+/*
+ * OmniVision OV9740 Camera Driver
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * Based on ov9640 camera 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.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+#define to_ov9740(sd)		container_of(sd, struct ov9740_priv, subdev)
+
+/* General Status Registers */
+#define OV9740_MODEL_ID_HI		0x0000
+#define OV9740_MODEL_ID_LO		0x0001
+#define OV9740_REVISION_NUMBER		0x0002
+#define OV9740_MANUFACTURER_ID		0x0003
+#define OV9740_SMIA_VERSION		0x0004
+
+/* General Setup Registers */
+#define OV9740_MODE_SELECT		0x0100
+#define OV9740_IMAGE_ORT		0x0101
+#define OV9740_SOFTWARE_RESET		0x0103
+#define OV9740_GRP_PARAM_HOLD		0x0104
+#define OV9740_MSK_CORRUP_FM		0x0105
+
+/* Timing Setting */
+#define OV9740_FRM_LENGTH_LN_HI		0x0340 /* VTS */
+#define OV9740_FRM_LENGTH_LN_LO		0x0341 /* VTS */
+#define OV9740_LN_LENGTH_PCK_HI		0x0342 /* HTS */
+#define OV9740_LN_LENGTH_PCK_LO		0x0343 /* HTS */
+#define OV9740_X_ADDR_START_HI		0x0344
+#define OV9740_X_ADDR_START_LO		0x0345
+#define OV9740_Y_ADDR_START_HI		0x0346
+#define OV9740_Y_ADDR_START_LO		0x0347
+#define OV9740_X_ADDR_END_HI		0x0348
+#define OV9740_X_ADDR_END_LO		0x0349
+#define OV9740_Y_ADDR_END_HI		0x034A
+#define OV9740_Y_ADDR_END_LO		0x034B
+#define OV9740_X_OUTPUT_SIZE_HI		0x034C
+#define OV9740_X_OUTPUT_SIZE_LO		0x034D
+#define OV9740_Y_OUTPUT_SIZE_HI		0x034E
+#define OV9740_Y_OUTPUT_SIZE_LO		0x034F
+
+/* IO Control Registers */
+#define OV9740_IO_CREL00		0x3002
+#define OV9740_IO_CREL01		0x3004
+#define OV9740_IO_CREL02		0x3005
+#define OV9740_IO_OUTPUT_SEL01		0x3026
+#define OV9740_IO_OUTPUT_SEL02		0x3027
+
+/* AWB Registers */
+#define OV9740_AWB_MANUAL_CTRL		0x3406
+
+/* Analog Control Registers */
+#define OV9740_ANALOG_CTRL01		0x3601
+#define OV9740_ANALOG_CTRL02		0x3602
+#define OV9740_ANALOG_CTRL03		0x3603
+#define OV9740_ANALOG_CTRL04		0x3604
+#define OV9740_ANALOG_CTRL10		0x3610
+#define OV9740_ANALOG_CTRL12		0x3612
+#define OV9740_ANALOG_CTRL20		0x3620
+#define OV9740_ANALOG_CTRL21		0x3621
+#define OV9740_ANALOG_CTRL22		0x3622
+#define OV9740_ANALOG_CTRL30		0x3630
+#define OV9740_ANALOG_CTRL31		0x3631
+#define OV9740_ANALOG_CTRL32		0x3632
+#define OV9740_ANALOG_CTRL33		0x3633
+
+/* Sensor Control */
+#define OV9740_SENSOR_CTRL03		0x3703
+#define OV9740_SENSOR_CTRL04		0x3704
+#define OV9740_SENSOR_CTRL05		0x3705
+#define OV9740_SENSOR_CTRL07		0x3707
+
+/* Timing Control */
+#define OV9740_TIMING_CTRL17		0x3817
+#define OV9740_TIMING_CTRL19		0x3819
+#define OV9740_TIMING_CTRL33		0x3833
+#define OV9740_TIMING_CTRL35		0x3835
+
+/* Banding Filter */
+#define OV9740_AEC_MAXEXPO_60_H		0x3A02
+#define OV9740_AEC_MAXEXPO_60_L		0x3A03
+#define OV9740_AEC_B50_STEP_HI		0x3A08
+#define OV9740_AEC_B50_STEP_LO		0x3A09
+#define OV9740_AEC_B60_STEP_HI		0x3A0A
+#define OV9740_AEC_B60_STEP_LO		0x3A0B
+#define OV9740_AEC_CTRL0D		0x3A0D
+#define OV9740_AEC_CTRL0E		0x3A0E
+#define OV9740_AEC_MAXEXPO_50_H		0x3A14
+#define OV9740_AEC_MAXEXPO_50_L		0x3A15
+
+/* AEC/AGC Control */
+#define OV9740_AEC_ENABLE		0x3503
+#define OV9740_GAIN_CEILING_01		0x3A18
+#define OV9740_GAIN_CEILING_02		0x3A19
+#define OV9740_AEC_HI_THRESHOLD		0x3A11
+#define OV9740_AEC_3A1A			0x3A1A
+#define OV9740_AEC_CTRL1B_WPT2		0x3A1B
+#define OV9740_AEC_CTRL0F_WPT		0x3A0F
+#define OV9740_AEC_CTRL10_BPT		0x3A10
+#define OV9740_AEC_CTRL1E_BPT2		0x3A1E
+#define OV9740_AEC_LO_THRESHOLD		0x3A1F
+
+/* BLC Control */
+#define OV9740_BLC_AUTO_ENABLE		0x4002
+#define OV9740_BLC_MODE			0x4005
+
+/* VFIFO */
+#define OV9740_VFIFO_READ_START_HI	0x4608
+#define OV9740_VFIFO_READ_START_LO	0x4609
+
+/* DVP Control */
+#define OV9740_DVP_VSYNC_CTRL02		0x4702
+#define OV9740_DVP_VSYNC_MODE		0x4704
+#define OV9740_DVP_VSYNC_CTRL06		0x4706
+
+/* PLL Setting */
+#define OV9740_PLL_MODE_CTRL01		0x3104
+#define OV9740_PRE_PLL_CLK_DIV		0x0305
+#define OV9740_PLL_MULTIPLIER		0x0307
+#define OV9740_VT_SYS_CLK_DIV		0x0303
+#define OV9740_VT_PIX_CLK_DIV		0x0301
+#define OV9740_PLL_CTRL3010		0x3010
+#define OV9740_VFIFO_CTRL00		0x460E
+
+/* ISP Control */
+#define OV9740_ISP_CTRL00		0x5000
+#define OV9740_ISP_CTRL01		0x5001
+#define OV9740_ISP_CTRL03		0x5003
+#define OV9740_ISP_CTRL05		0x5005
+#define OV9740_ISP_CTRL12		0x5012
+#define OV9740_ISP_CTRL19		0x5019
+#define OV9740_ISP_CTRL1A		0x501A
+#define OV9740_ISP_CTRL1E		0x501E
+#define OV9740_ISP_CTRL1F		0x501F
+#define OV9740_ISP_CTRL20		0x5020
+#define OV9740_ISP_CTRL21		0x5021
+
+/* AWB */
+#define OV9740_AWB_CTRL00		0x5180
+#define OV9740_AWB_CTRL01		0x5181
+#define OV9740_AWB_CTRL02		0x5182
+#define OV9740_AWB_CTRL03		0x5183
+#define OV9740_AWB_ADV_CTRL01		0x5184
+#define OV9740_AWB_ADV_CTRL02		0x5185
+#define OV9740_AWB_ADV_CTRL03		0x5186
+#define OV9740_AWB_ADV_CTRL04		0x5187
+#define OV9740_AWB_ADV_CTRL05		0x5188
+#define OV9740_AWB_ADV_CTRL06		0x5189
+#define OV9740_AWB_ADV_CTRL07		0x518A
+#define OV9740_AWB_ADV_CTRL08		0x518B
+#define OV9740_AWB_ADV_CTRL09		0x518C
+#define OV9740_AWB_ADV_CTRL10		0x518D
+#define OV9740_AWB_ADV_CTRL11		0x518E
+#define OV9740_AWB_CTRL0F		0x518F
+#define OV9740_AWB_CTRL10		0x5190
+#define OV9740_AWB_CTRL11		0x5191
+#define OV9740_AWB_CTRL12		0x5192
+#define OV9740_AWB_CTRL13		0x5193
+#define OV9740_AWB_CTRL14		0x5194
+
+/* MIPI Control */
+#define OV9740_MIPI_CTRL00		0x4800
+#define OV9740_MIPI_3837		0x3837
+#define OV9740_MIPI_CTRL01		0x4801
+#define OV9740_MIPI_CTRL03		0x4803
+#define OV9740_MIPI_CTRL05		0x4805
+#define OV9740_VFIFO_RD_CTRL		0x4601
+#define OV9740_MIPI_CTRL_3012		0x3012
+#define OV9740_SC_CMMM_MIPI_CTR		0x3014
+
+/* supported resolutions */
+enum {
+	OV9740_VGA,
+	OV9740_720P,
+};
+
+struct ov9740_resolution {
+	unsigned int width;
+	unsigned int height;
+};
+
+static struct ov9740_resolution ov9740_resolutions[] = {
+	[OV9740_VGA] = {
+		.width	= 640,
+		.height	= 480,
+	},
+	[OV9740_720P] = {
+		.width	= 1280,
+		.height	= 720,
+	},
+};
+
+/* Misc. structures */
+struct ov9740_reg {
+	u16				reg;
+	u8				val;
+};
+
+struct ov9740_priv {
+	struct v4l2_subdev		subdev;
+
+	int				ident;
+	u16				model;
+	u8				revision;
+	u8				manid;
+	u8				smiaver;
+
+	bool				flag_vflip;
+	bool				flag_hflip;
+};
+
+static const struct ov9740_reg ov9740_defaults[] = {
+	/* Banding Filter */
+	{ OV9740_AEC_B50_STEP_HI,	0x00 },
+	{ OV9740_AEC_B50_STEP_LO,	0xe8 },
+	{ OV9740_AEC_CTRL0E,		0x03 },
+	{ OV9740_AEC_MAXEXPO_50_H,	0x15 },
+	{ OV9740_AEC_MAXEXPO_50_L,	0xc6 },
+	{ OV9740_AEC_B60_STEP_HI,	0x00 },
+	{ OV9740_AEC_B60_STEP_LO,	0xc0 },
+	{ OV9740_AEC_CTRL0D,		0x04 },
+	{ OV9740_AEC_MAXEXPO_60_H,	0x18 },
+	{ OV9740_AEC_MAXEXPO_60_L,	0x20 },
+
+	/* LC */
+	{ 0x5842, 0x02 }, { 0x5843, 0x5e }, { 0x5844, 0x04 }, { 0x5845, 0x32 },
+	{ 0x5846, 0x03 }, { 0x5847, 0x29 }, { 0x5848, 0x02 }, { 0x5849, 0xcc },
+
+	/* Un-documented OV9740 registers */
+	{ 0x5800, 0x29 }, { 0x5801, 0x25 }, { 0x5802, 0x20 }, { 0x5803, 0x21 },
+	{ 0x5804, 0x26 }, { 0x5805, 0x2e }, { 0x5806, 0x11 }, { 0x5807, 0x0c },
+	{ 0x5808, 0x09 }, { 0x5809, 0x0a }, { 0x580A, 0x0e }, { 0x580B, 0x16 },
+	{ 0x580C, 0x06 }, { 0x580D, 0x02 }, { 0x580E, 0x00 }, { 0x580F, 0x00 },
+	{ 0x5810, 0x04 }, { 0x5811, 0x0a }, { 0x5812, 0x05 }, { 0x5813, 0x02 },
+	{ 0x5814, 0x00 }, { 0x5815, 0x00 }, { 0x5816, 0x03 }, { 0x5817, 0x09 },
+	{ 0x5818, 0x0f }, { 0x5819, 0x0a }, { 0x581A, 0x07 }, { 0x581B, 0x08 },
+	{ 0x581C, 0x0b }, { 0x581D, 0x14 }, { 0x581E, 0x28 }, { 0x581F, 0x23 },
+	{ 0x5820, 0x1d }, { 0x5821, 0x1e }, { 0x5822, 0x24 }, { 0x5823, 0x2a },
+	{ 0x5824, 0x4f }, { 0x5825, 0x6f }, { 0x5826, 0x5f }, { 0x5827, 0x7f },
+	{ 0x5828, 0x9f }, { 0x5829, 0x5f }, { 0x582A, 0x8f }, { 0x582B, 0x9e },
+	{ 0x582C, 0x8f }, { 0x582D, 0x9f }, { 0x582E, 0x4f }, { 0x582F, 0x87 },
+	{ 0x5830, 0x86 }, { 0x5831, 0x97 }, { 0x5832, 0xae }, { 0x5833, 0x3f },
+	{ 0x5834, 0x8e }, { 0x5835, 0x7c }, { 0x5836, 0x7e }, { 0x5837, 0xaf },
+	{ 0x5838, 0x8f }, { 0x5839, 0x8f }, { 0x583A, 0x9f }, { 0x583B, 0x7f },
+	{ 0x583C, 0x5f },
+
+	/* Y Gamma */
+	{ 0x5480, 0x07 }, { 0x5481, 0x18 }, { 0x5482, 0x2c }, { 0x5483, 0x4e },
+	{ 0x5484, 0x5e }, { 0x5485, 0x6b }, { 0x5486, 0x77 }, { 0x5487, 0x82 },
+	{ 0x5488, 0x8c }, { 0x5489, 0x95 }, { 0x548A, 0xa4 }, { 0x548B, 0xb1 },
+	{ 0x548C, 0xc6 }, { 0x548D, 0xd8 }, { 0x548E, 0xe9 },
+
+	/* UV Gamma */
+	{ 0x5490, 0x0f }, { 0x5491, 0xff }, { 0x5492, 0x0d }, { 0x5493, 0x05 },
+	{ 0x5494, 0x07 }, { 0x5495, 0x1a }, { 0x5496, 0x04 }, { 0x5497, 0x01 },
+	{ 0x5498, 0x03 }, { 0x5499, 0x53 }, { 0x549A, 0x02 }, { 0x549B, 0xeb },
+	{ 0x549C, 0x02 }, { 0x549D, 0xa0 }, { 0x549E, 0x02 }, { 0x549F, 0x67 },
+	{ 0x54A0, 0x02 }, { 0x54A1, 0x3b }, { 0x54A2, 0x02 }, { 0x54A3, 0x18 },
+	{ 0x54A4, 0x01 }, { 0x54A5, 0xe7 }, { 0x54A6, 0x01 }, { 0x54A7, 0xc3 },
+	{ 0x54A8, 0x01 }, { 0x54A9, 0x94 }, { 0x54AA, 0x01 }, { 0x54AB, 0x72 },
+	{ 0x54AC, 0x01 }, { 0x54AD, 0x57 },
+
+	/* AWB */
+	{ OV9740_AWB_CTRL00,		0xf0 },
+	{ OV9740_AWB_CTRL01,		0x00 },
+	{ OV9740_AWB_CTRL02,		0x41 },
+	{ OV9740_AWB_CTRL03,		0x42 },
+	{ OV9740_AWB_ADV_CTRL01,	0x8a },
+	{ OV9740_AWB_ADV_CTRL02,	0x61 },
+	{ OV9740_AWB_ADV_CTRL03,	0xce },
+	{ OV9740_AWB_ADV_CTRL04,	0xa8 },
+	{ OV9740_AWB_ADV_CTRL05,	0x17 },
+	{ OV9740_AWB_ADV_CTRL06,	0x1f },
+	{ OV9740_AWB_ADV_CTRL07,	0x27 },
+	{ OV9740_AWB_ADV_CTRL08,	0x41 },
+	{ OV9740_AWB_ADV_CTRL09,	0x34 },
+	{ OV9740_AWB_ADV_CTRL10,	0xf0 },
+	{ OV9740_AWB_ADV_CTRL11,	0x10 },
+	{ OV9740_AWB_CTRL0F,		0xff },
+	{ OV9740_AWB_CTRL10,		0x00 },
+	{ OV9740_AWB_CTRL11,		0xff },
+	{ OV9740_AWB_CTRL12,		0x00 },
+	{ OV9740_AWB_CTRL13,		0xff },
+	{ OV9740_AWB_CTRL14,		0x00 },
+
+	/* CIP */
+	{ 0x530D, 0x12 },
+
+	/* CMX */
+	{ 0x5380, 0x01 }, { 0x5381, 0x00 }, { 0x5382, 0x00 }, { 0x5383, 0x17 },
+	{ 0x5384, 0x00 }, { 0x5385, 0x01 }, { 0x5386, 0x00 }, { 0x5387, 0x00 },
+	{ 0x5388, 0x00 }, { 0x5389, 0xe0 }, { 0x538A, 0x00 }, { 0x538B, 0x20 },
+	{ 0x538C, 0x00 }, { 0x538D, 0x00 }, { 0x538E, 0x00 }, { 0x538F, 0x16 },
+	{ 0x5390, 0x00 }, { 0x5391, 0x9c }, { 0x5392, 0x00 }, { 0x5393, 0xa0 },
+	{ 0x5394, 0x18 },
+
+	/* 50/60 Detection */
+	{ 0x3C0A, 0x9c }, { 0x3C0B, 0x3f },
+
+	/* Output Select */
+	{ OV9740_IO_OUTPUT_SEL01,	0x00 },
+	{ OV9740_IO_OUTPUT_SEL02,	0x00 },
+	{ OV9740_IO_CREL00,		0x00 },
+	{ OV9740_IO_CREL01,		0x00 },
+	{ OV9740_IO_CREL02,		0x00 },
+
+	/* AWB Control */
+	{ OV9740_AWB_MANUAL_CTRL,	0x00 },
+
+	/* Analog Control */
+	{ OV9740_ANALOG_CTRL03,		0xaa },
+	{ OV9740_ANALOG_CTRL32,		0x2f },
+	{ OV9740_ANALOG_CTRL20,		0x66 },
+	{ OV9740_ANALOG_CTRL21,		0xc0 },
+	{ OV9740_ANALOG_CTRL31,		0x52 },
+	{ OV9740_ANALOG_CTRL33,		0x50 },
+	{ OV9740_ANALOG_CTRL30,		0xca },
+	{ OV9740_ANALOG_CTRL04,		0x0c },
+	{ OV9740_ANALOG_CTRL01,		0x40 },
+	{ OV9740_ANALOG_CTRL02,		0x16 },
+	{ OV9740_ANALOG_CTRL10,		0xa1 },
+	{ OV9740_ANALOG_CTRL12,		0x24 },
+	{ OV9740_ANALOG_CTRL22,		0x9f },
+
+	/* Sensor Control */
+	{ OV9740_SENSOR_CTRL03,		0x42 },
+	{ OV9740_SENSOR_CTRL04,		0x10 },
+	{ OV9740_SENSOR_CTRL05,		0x45 },
+	{ OV9740_SENSOR_CTRL07,		0x14 },
+
+	/* Timing Control */
+	{ OV9740_TIMING_CTRL33,		0x04 },
+	{ OV9740_TIMING_CTRL35,		0x02 },
+	{ OV9740_TIMING_CTRL19,		0x6e },
+	{ OV9740_TIMING_CTRL17,		0x94 },
+
+	/* AEC/AGC Control */
+	{ OV9740_AEC_ENABLE,		0x10 },
+	{ OV9740_GAIN_CEILING_01,	0x00 },
+	{ OV9740_GAIN_CEILING_02,	0x7f },
+	{ OV9740_AEC_HI_THRESHOLD,	0xa0 },
+	{ OV9740_AEC_3A1A,		0x05 },
+	{ OV9740_AEC_CTRL1B_WPT2,	0x50 },
+	{ OV9740_AEC_CTRL0F_WPT,	0x50 },
+	{ OV9740_AEC_CTRL10_BPT,	0x4c },
+	{ OV9740_AEC_CTRL1E_BPT2,	0x4c },
+	{ OV9740_AEC_LO_THRESHOLD,	0x26 },
+
+	/* BLC Control */
+	{ OV9740_BLC_AUTO_ENABLE,	0x45 },
+	{ OV9740_BLC_MODE,		0x18 },
+
+	/* DVP Control */
+	{ OV9740_DVP_VSYNC_CTRL02,	0x04 },
+	{ OV9740_DVP_VSYNC_MODE,	0x00 },
+	{ OV9740_DVP_VSYNC_CTRL06,	0x08 },
+
+	/* PLL Setting */
+	{ OV9740_PLL_MODE_CTRL01,	0x20 },
+	{ OV9740_PRE_PLL_CLK_DIV,	0x03 },
+	{ OV9740_PLL_MULTIPLIER,	0x4c },
+	{ OV9740_VT_SYS_CLK_DIV,	0x01 },
+	{ OV9740_VT_PIX_CLK_DIV,	0x08 },
+	{ OV9740_PLL_CTRL3010,		0x01 },
+	{ OV9740_VFIFO_CTRL00,		0x82 },
+
+	/* Timing Setting */
+	/* VTS */
+	{ OV9740_FRM_LENGTH_LN_HI,	0x03 },
+	{ OV9740_FRM_LENGTH_LN_LO,	0x07 },
+	/* HTS */
+	{ OV9740_LN_LENGTH_PCK_HI,	0x06 },
+	{ OV9740_LN_LENGTH_PCK_LO,	0x62 },
+
+	/* MIPI Control */
+	{ OV9740_MIPI_CTRL00,		0x44 },
+	{ OV9740_MIPI_3837,		0x01 },
+	{ OV9740_MIPI_CTRL01,		0x0f },
+	{ OV9740_MIPI_CTRL03,		0x05 },
+	{ OV9740_MIPI_CTRL05,		0x10 },
+	{ OV9740_VFIFO_RD_CTRL,		0x16 },
+	{ OV9740_MIPI_CTRL_3012,	0x70 },
+	{ OV9740_SC_CMMM_MIPI_CTR,	0x01 },
+};
+
+static const struct ov9740_reg ov9740_regs_vga[] = {
+	{ OV9740_X_ADDR_START_HI,	0x00 },
+	{ OV9740_X_ADDR_START_LO,	0xa0 },
+	{ OV9740_Y_ADDR_START_HI,	0x00 },
+	{ OV9740_Y_ADDR_START_LO,	0x00 },
+	{ OV9740_X_ADDR_END_HI,		0x04 },
+	{ OV9740_X_ADDR_END_LO,		0x63 },
+	{ OV9740_Y_ADDR_END_HI,		0x02 },
+	{ OV9740_Y_ADDR_END_LO,		0xd3 },
+	{ OV9740_X_OUTPUT_SIZE_HI,	0x02 },
+	{ OV9740_X_OUTPUT_SIZE_LO,	0x80 },
+	{ OV9740_Y_OUTPUT_SIZE_HI,	0x01 },
+	{ OV9740_Y_OUTPUT_SIZE_LO,	0xe0 },
+	{ OV9740_ISP_CTRL1E,		0x03 },
+	{ OV9740_ISP_CTRL1F,		0xc0 },
+	{ OV9740_ISP_CTRL20,		0x02 },
+	{ OV9740_ISP_CTRL21,		0xd0 },
+	{ OV9740_VFIFO_READ_START_HI,	0x01 },
+	{ OV9740_VFIFO_READ_START_LO,	0x40 },
+	{ OV9740_ISP_CTRL00,		0xff },
+	{ OV9740_ISP_CTRL01,		0xff },
+	{ OV9740_ISP_CTRL03,		0xff },
+};
+
+static const struct ov9740_reg ov9740_regs_720p[] = {
+	{ OV9740_X_ADDR_START_HI,	0x00 },
+	{ OV9740_X_ADDR_START_LO,	0x00 },
+	{ OV9740_Y_ADDR_START_HI,	0x00 },
+	{ OV9740_Y_ADDR_START_LO,	0x00 },
+	{ OV9740_X_ADDR_END_HI,		0x05 },
+	{ OV9740_X_ADDR_END_LO,		0x03 },
+	{ OV9740_Y_ADDR_END_HI,		0x02 },
+	{ OV9740_Y_ADDR_END_LO,		0xd3 },
+	{ OV9740_X_OUTPUT_SIZE_HI,	0x05 },
+	{ OV9740_X_OUTPUT_SIZE_LO,	0x00 },
+	{ OV9740_Y_OUTPUT_SIZE_HI,	0x02 },
+	{ OV9740_Y_OUTPUT_SIZE_LO,	0xd0 },
+	{ OV9740_ISP_CTRL1E,		0x05 },
+	{ OV9740_ISP_CTRL1F,		0x00 },
+	{ OV9740_ISP_CTRL20,		0x02 },
+	{ OV9740_ISP_CTRL21,		0xd0 },
+	{ OV9740_VFIFO_READ_START_HI,	0x02 },
+	{ OV9740_VFIFO_READ_START_LO,	0x30 },
+	{ OV9740_ISP_CTRL00,		0xff },
+	{ OV9740_ISP_CTRL01,		0xef },
+	{ OV9740_ISP_CTRL03,		0xff },
+};
+
+static enum v4l2_mbus_pixelcode ov9740_codes[] = {
+	V4L2_MBUS_FMT_YUYV8_2X8,
+};
+
+static const struct v4l2_queryctrl ov9740_controls[] = {
+	{
+		.id		= V4L2_CID_VFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Flip Vertically",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+	},
+	{
+		.id		= V4L2_CID_HFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Flip Horizontally",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+	},
+};
+
+/* read a register */
+static int ov9740_reg_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+	int ret;
+	struct i2c_msg msg[] = {
+		{
+			.addr	= client->addr,
+			.flags	= 0,
+			.len	= 2,
+			.buf	= (u8 *)&reg,
+		},
+		{
+			.addr	= client->addr,
+			.flags	= I2C_M_RD,
+			.len	= 1,
+			.buf	= val,
+		},
+	};
+
+	reg = swab16(reg);
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed reading register 0x%04x!\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* write a register */
+static int ov9740_reg_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	struct i2c_msg msg;
+	struct {
+		u16 reg;
+		u8 val;
+	} __packed buf;
+	int ret;
+
+	reg = swab16(reg);
+
+	buf.reg = reg;
+	buf.val = val;
+
+	msg.addr	= client->addr;
+	msg.flags	= 0;
+	msg.len		= 3;
+	msg.buf		= (u8 *)&buf;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed writing register 0x%04x!\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+
+/* Read a register, alter its bits, write it back */
+static int ov9740_reg_rmw(struct i2c_client *client, u16 reg, u8 set, u8 unset)
+{
+	u8 val;
+	int ret;
+
+	ret = ov9740_reg_read(client, reg, &val);
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"[Read]-Modify-Write of register %02x failed!\n", reg);
+		return ret;
+	}
+
+	val |= set;
+	val &= ~unset;
+
+	ret = ov9740_reg_write(client, reg, val);
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"Read-Modify-[Write] of register %02x failed!\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov9740_reg_write_array(struct i2c_client *client,
+				  const struct ov9740_reg *regarray,
+				  int regarraylen)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < regarraylen; i++) {
+		ret = ov9740_reg_write(client,
+				       regarray[i].reg, regarray[i].val);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/* Start/Stop streaming from the device */
+static int ov9740_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov9740_priv *priv = to_ov9740(sd);
+	int ret;
+
+	/* Program orientation register. */
+	if (priv->flag_vflip)
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0x2, 0);
+	else
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0, 0x2);
+	if (ret < 0)
+		return ret;
+
+	if (priv->flag_hflip)
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0x1, 0);
+	else
+		ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0, 0x1);
+	if (ret < 0)
+		return ret;
+
+	if (enable) {
+		dev_dbg(&client->dev, "Enabling Streaming\n");
+		/* Start Streaming */
+		ret = ov9740_reg_write(client, OV9740_MODE_SELECT, 0x01);
+
+	} else {
+		dev_dbg(&client->dev, "Disabling Streaming\n");
+		/* Software Reset */
+		ret = ov9740_reg_write(client, OV9740_SOFTWARE_RESET, 0x01);
+		if (!ret)
+			/* Setting Streaming to Standby */
+			ret = ov9740_reg_write(client, OV9740_MODE_SELECT,
+					       0x00);
+	}
+
+	return ret;
+}
+
+/* Alter bus settings on camera side */
+static int ov9740_set_bus_param(struct soc_camera_device *icd,
+				unsigned long flags)
+{
+	return 0;
+}
+
+/* Request bus settings on camera side */
+static unsigned long ov9740_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+/* Get status of additional camera capabilities */
+static int ov9740_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov9740_priv *priv = to_ov9740(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		ctrl->value = priv->flag_vflip;
+		break;
+	case V4L2_CID_HFLIP:
+		ctrl->value = priv->flag_hflip;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Set status of additional camera capabilities */
+static int ov9740_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov9740_priv *priv = to_ov9740(sd);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		priv->flag_vflip = ctrl->value;
+		break;
+	case V4L2_CID_HFLIP:
+		priv->flag_hflip = ctrl->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Get chip identification */
+static int ov9740_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov9740_priv *priv = to_ov9740(sd);
+
+	id->ident = priv->ident;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov9740_get_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 2;
+
+	ret = ov9740_reg_read(client, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov9740_set_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov9740_reg_write(client, reg->reg, reg->val);
+}
+#endif
+
+/* select nearest higher resolution for capture */
+static void ov9740_res_roundup(u32 *width, u32 *height)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ov9740_resolutions); i++)
+		if ((ov9740_resolutions[i].width >= *width) &&
+		    (ov9740_resolutions[i].height >= *height)) {
+			*width = ov9740_resolutions[i].width;
+			*height = ov9740_resolutions[i].height;
+			return;
+		}
+
+	*width = ov9740_resolutions[OV9740_720P].width;
+	*height = ov9740_resolutions[OV9740_720P].height;
+}
+
+/* Setup registers according to resolution and color encoding */
+static int ov9740_set_res(struct i2c_client *client, u32 width)
+{
+	int ret;
+
+	/* select register configuration for given resolution */
+	if (width == ov9740_resolutions[OV9740_VGA].width) {
+		dev_dbg(&client->dev, "Setting image size to 640x480\n");
+		ret = ov9740_reg_write_array(client, ov9740_regs_vga,
+					     ARRAY_SIZE(ov9740_regs_vga));
+	} else if (width == ov9740_resolutions[OV9740_720P].width) {
+		dev_dbg(&client->dev, "Setting image size to 1280x720\n");
+		ret = ov9740_reg_write_array(client, ov9740_regs_720p,
+					     ARRAY_SIZE(ov9740_regs_720p));
+	} else {
+		dev_err(&client->dev, "Failed to select resolution!\n");
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+/* set the format we will capture in */
+static int ov9740_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	enum v4l2_colorspace cspace;
+	enum v4l2_mbus_pixelcode code = mf->code;
+	int ret;
+
+	ov9740_res_roundup(&mf->width, &mf->height);
+
+	switch (code) {
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		cspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = ov9740_reg_write_array(client, ov9740_defaults,
+				     ARRAY_SIZE(ov9740_defaults));
+	if (ret < 0)
+		return ret;
+
+	ret = ov9740_set_res(client, mf->width);
+	if (ret < 0)
+		return ret;
+
+	mf->code	= code;
+	mf->colorspace	= cspace;
+
+	return ret;
+}
+
+static int ov9740_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	ov9740_res_roundup(&mf->width, &mf->height);
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->code = V4L2_MBUS_FMT_YUYV8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+static int ov9740_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov9740_codes))
+		return -EINVAL;
+
+	*code = ov9740_codes[index];
+
+	return 0;
+}
+
+static int ov9740_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left		= 0;
+	a->bounds.top		= 0;
+	a->bounds.width		= ov9740_resolutions[OV9740_720P].width;
+	a->bounds.height	= ov9740_resolutions[OV9740_720P].height;
+	a->defrect		= a->bounds;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov9740_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left		= 0;
+	a->c.top		= 0;
+	a->c.width		= ov9740_resolutions[OV9740_720P].width;
+	a->c.height		= ov9740_resolutions[OV9740_720P].height;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int ov9740_video_probe(struct soc_camera_device *icd,
+			      struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov9740_priv *priv = to_ov9740(sd);
+	u8 modelhi, modello;
+	int ret;
+
+	/*
+	 * We must have a parent by now. And it cannot be a wrong one.
+	 * So this entire test is completely redundant.
+	 */
+	if (!icd->dev.parent ||
+	    to_soc_camera_host(icd->dev.parent)->nr != icd->iface) {
+		dev_err(&client->dev, "Parent missing or invalid!\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	ret = ov9740_reg_read(client, OV9740_MODEL_ID_HI, &modelhi);
+	if (ret < 0)
+		goto err;
+
+	ret = ov9740_reg_read(client, OV9740_MODEL_ID_LO, &modello);
+	if (ret < 0)
+		goto err;
+
+	priv->model = (modelhi << 8) | modello;
+
+	ret = ov9740_reg_read(client, OV9740_REVISION_NUMBER, &priv->revision);
+	if (ret < 0)
+		goto err;
+
+	ret = ov9740_reg_read(client, OV9740_MANUFACTURER_ID, &priv->manid);
+	if (ret < 0)
+		goto err;
+
+	ret = ov9740_reg_read(client, OV9740_SMIA_VERSION, &priv->smiaver);
+	if (ret < 0)
+		goto err;
+
+	if (priv->model != 0x9740) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	priv->ident = V4L2_IDENT_OV9740;
+
+	dev_info(&client->dev, "ov9740 Model ID 0x%04x, Revision 0x%02x, "
+		 "Manufacturer 0x%02x, SMIA Version 0x%02x\n",
+		 priv->model, priv->revision, priv->manid, priv->smiaver);
+
+err:
+	return ret;
+}
+
+static struct soc_camera_ops ov9740_ops = {
+	.set_bus_param		= ov9740_set_bus_param,
+	.query_bus_param	= ov9740_query_bus_param,
+	.controls		= ov9740_controls,
+	.num_controls		= ARRAY_SIZE(ov9740_controls),
+};
+
+static struct v4l2_subdev_core_ops ov9740_core_ops = {
+	.g_ctrl			= ov9740_g_ctrl,
+	.s_ctrl			= ov9740_s_ctrl,
+	.g_chip_ident		= ov9740_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov9740_get_register,
+	.s_register		= ov9740_set_register,
+#endif
+
+};
+
+static struct v4l2_subdev_video_ops ov9740_video_ops = {
+	.s_stream		= ov9740_s_stream,
+	.s_mbus_fmt		= ov9740_s_fmt,
+	.try_mbus_fmt		= ov9740_try_fmt,
+	.enum_mbus_fmt		= ov9740_enum_fmt,
+	.cropcap		= ov9740_cropcap,
+	.g_crop			= ov9740_g_crop,
+};
+
+static struct v4l2_subdev_ops ov9740_subdev_ops = {
+	.core			= &ov9740_core_ops,
+	.video			= &ov9740_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov9740_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov9740_priv *priv;
+	struct soc_camera_device *icd	= client->dev.platform_data;
+	struct soc_camera_link *icl;
+	int ret;
+
+	if (!icd) {
+		dev_err(&client->dev, "Missing soc-camera data!\n");
+		return -EINVAL;
+	}
+
+	icl = to_soc_camera_link(icd);
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	priv = kzalloc(sizeof(struct ov9740_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev, "Failed to allocate private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov9740_subdev_ops);
+
+	icd->ops = &ov9740_ops;
+
+	ret = ov9740_video_probe(icd, client);
+	if (ret < 0) {
+		icd->ops = NULL;
+		kfree(priv);
+	}
+
+	return ret;
+}
+
+static int ov9740_remove(struct i2c_client *client)
+{
+	struct ov9740_priv *priv = i2c_get_clientdata(client);
+
+	kfree(priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov9740_id[] = {
+	{ "ov9740", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov9740_id);
+
+static struct i2c_driver ov9740_i2c_driver = {
+	.driver = {
+		.name = "ov9740",
+	},
+	.probe    = ov9740_probe,
+	.remove   = ov9740_remove,
+	.id_table = ov9740_id,
+};
+
+static int __init ov9740_module_init(void)
+{
+	return i2c_add_driver(&ov9740_i2c_driver);
+}
+
+static void __exit ov9740_module_exit(void)
+{
+	i2c_del_driver(&ov9740_i2c_driver);
+}
+
+module_init(ov9740_module_init);
+module_exit(ov9740_module_exit);
+
+MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV9740");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.c b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
index 66ad516bdfd9..d33dd61de263 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
@@ -499,31 +499,35 @@ static int ctrl_cropt_max_get(struct pvr2_ctrl *cptr, int *top)
 	return 0;
 }
 
-static int ctrl_cropw_max_get(struct pvr2_ctrl *cptr, int *val)
+static int ctrl_cropw_max_get(struct pvr2_ctrl *cptr, int *width)
 {
 	struct v4l2_cropcap *cap = &cptr->hdw->cropcap_info;
-	int stat = pvr2_hdw_check_cropcap(cptr->hdw);
+	int stat, bleftend, cleft;
+
+	stat = pvr2_hdw_check_cropcap(cptr->hdw);
 	if (stat != 0) {
 		return stat;
 	}
-	*val = 0;
-	if (cap->bounds.width > cptr->hdw->cropl_val) {
-		*val = cap->bounds.width - cptr->hdw->cropl_val;
-	}
+	bleftend = cap->bounds.left+cap->bounds.width;
+	cleft = cptr->hdw->cropl_val;
+
+	*width = cleft < bleftend ? bleftend-cleft : 0;
 	return 0;
 }
 
-static int ctrl_croph_max_get(struct pvr2_ctrl *cptr, int *val)
+static int ctrl_croph_max_get(struct pvr2_ctrl *cptr, int *height)
 {
 	struct v4l2_cropcap *cap = &cptr->hdw->cropcap_info;
-	int stat = pvr2_hdw_check_cropcap(cptr->hdw);
+	int stat, btopend, ctop;
+
+	stat = pvr2_hdw_check_cropcap(cptr->hdw);
 	if (stat != 0) {
 		return stat;
 	}
-	*val = 0;
-	if (cap->bounds.height > cptr->hdw->cropt_val) {
-		*val = cap->bounds.height - cptr->hdw->cropt_val;
-	}
+	btopend = cap->bounds.top+cap->bounds.height;
+	ctop = cptr->hdw->cropt_val;
+
+	*height = ctop < btopend ? btopend-ctop : 0;
 	return 0;
 }
 
@@ -1114,6 +1118,7 @@ static const struct pvr2_ctl_info control_defs[] = {
 		.internal_id = PVR2_CID_CROPW,
 		.default_value = 720,
 		DEFREF(cropw),
+		DEFINT(0, 864),
 		.get_max_value = ctrl_cropw_max_get,
 		.get_def_value = ctrl_get_cropcapdw,
 	}, {
@@ -1122,6 +1127,7 @@ static const struct pvr2_ctl_info control_defs[] = {
 		.internal_id = PVR2_CID_CROPH,
 		.default_value = 480,
 		DEFREF(croph),
+		DEFINT(0, 576),
 		.get_max_value = ctrl_croph_max_get,
 		.get_def_value = ctrl_get_cropcapdh,
 	}, {
@@ -2027,6 +2033,8 @@ static void pvr2_hdw_cx25840_vbi_hack(struct pvr2_hdw *hdw)
 		   hdw->decoder_client_id);
 	memset(&fmt, 0, sizeof(fmt));
 	fmt.type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+	fmt.fmt.sliced.service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+	fmt.fmt.sliced.service_lines[1][21] = V4L2_SLICED_CAPTION_525;
 	v4l2_device_call_all(&hdw->v4l2_dev, hdw->decoder_client_id,
 			     vbi, s_sliced_fmt, &fmt.fmt.sliced);
 }
@@ -3165,6 +3173,19 @@ static int pvr2_hdw_commit_execute(struct pvr2_hdw *hdw)
 	struct pvr2_ctrl *cptr;
 	int disruptive_change;
 
+	if (hdw->input_dirty && hdw->state_pathway_ok &&
+	    (((hdw->input_val == PVR2_CVAL_INPUT_DTV) ?
+	      PVR2_PATHWAY_DIGITAL : PVR2_PATHWAY_ANALOG) !=
+	     hdw->pathway_state)) {
+		/* Change of mode being asked for... */
+		hdw->state_pathway_ok = 0;
+		trace_stbit("state_pathway_ok", hdw->state_pathway_ok);
+	}
+	if (!hdw->state_pathway_ok) {
+		/* Can't commit anything until pathway is ok. */
+		return 0;
+	}
+
 	/* Handle some required side effects when the video standard is
 	   changed.... */
 	if (hdw->std_dirty) {
@@ -3199,18 +3220,6 @@ static int pvr2_hdw_commit_execute(struct pvr2_hdw *hdw)
 		}
 	}
 
-	if (hdw->input_dirty && hdw->state_pathway_ok &&
-	    (((hdw->input_val == PVR2_CVAL_INPUT_DTV) ?
-	      PVR2_PATHWAY_DIGITAL : PVR2_PATHWAY_ANALOG) !=
-	     hdw->pathway_state)) {
-		/* Change of mode being asked for... */
-		hdw->state_pathway_ok = 0;
-		trace_stbit("state_pathway_ok",hdw->state_pathway_ok);
-	}
-	if (!hdw->state_pathway_ok) {
-		/* Can't commit anything until pathway is ok. */
-		return 0;
-	}
 	/* The broadcast decoder can only scale down, so if
 	 * res_*_dirty && crop window < output format ==> enlarge crop.
 	 *
@@ -5159,8 +5168,7 @@ void pvr2_hdw_status_poll(struct pvr2_hdw *hdw)
 	   using v4l2-subdev - therefore we can't support that AT ALL right
 	   now.  (Of course, no sub-drivers seem to implement it either.
 	   But now it's a a chicken and egg problem...) */
-	v4l2_device_call_all(&hdw->v4l2_dev, 0, tuner, g_tuner,
-			     &hdw->tuner_signal_info);
+	v4l2_device_call_all(&hdw->v4l2_dev, 0, tuner, g_tuner, vtp);
 	pvr2_trace(PVR2_TRACE_CHIPS, "subdev status poll"
 		   " type=%u strength=%u audio=0x%x cap=0x%x"
 		   " low=%u hi=%u",
diff --git a/drivers/media/video/pvrusb2/pvrusb2-sysfs.c b/drivers/media/video/pvrusb2/pvrusb2-sysfs.c
index 281806b2df62..6ef1335b2858 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-sysfs.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-sysfs.c
@@ -324,36 +324,45 @@ static void pvr2_sysfs_add_control(struct pvr2_sysfs *sfp,int ctl_id)
 	}
 	sfp->item_last = cip;
 
+	sysfs_attr_init(&cip->attr_name.attr);
 	cip->attr_name.attr.name = "name";
 	cip->attr_name.attr.mode = S_IRUGO;
 	cip->attr_name.show = show_name;
 
+	sysfs_attr_init(&cip->attr_type.attr);
 	cip->attr_type.attr.name = "type";
 	cip->attr_type.attr.mode = S_IRUGO;
 	cip->attr_type.show = show_type;
 
+	sysfs_attr_init(&cip->attr_min.attr);
 	cip->attr_min.attr.name = "min_val";
 	cip->attr_min.attr.mode = S_IRUGO;
 	cip->attr_min.show = show_min;
 
+	sysfs_attr_init(&cip->attr_max.attr);
 	cip->attr_max.attr.name = "max_val";
 	cip->attr_max.attr.mode = S_IRUGO;
 	cip->attr_max.show = show_max;
 
+	sysfs_attr_init(&cip->attr_def.attr);
 	cip->attr_def.attr.name = "def_val";
 	cip->attr_def.attr.mode = S_IRUGO;
 	cip->attr_def.show = show_def;
 
+	sysfs_attr_init(&cip->attr_val.attr);
 	cip->attr_val.attr.name = "cur_val";
 	cip->attr_val.attr.mode = S_IRUGO;
 
+	sysfs_attr_init(&cip->attr_custom.attr);
 	cip->attr_custom.attr.name = "custom_val";
 	cip->attr_custom.attr.mode = S_IRUGO;
 
+	sysfs_attr_init(&cip->attr_enum.attr);
 	cip->attr_enum.attr.name = "enum_val";
 	cip->attr_enum.attr.mode = S_IRUGO;
 	cip->attr_enum.show = show_enum;
 
+	sysfs_attr_init(&cip->attr_bits.attr);
 	cip->attr_bits.attr.name = "bit_val";
 	cip->attr_bits.attr.mode = S_IRUGO;
 	cip->attr_bits.show = show_bits;
diff --git a/drivers/media/video/pwc/pwc-if.c b/drivers/media/video/pwc/pwc-if.c
index bd1519a4ecb4..780af5f81642 100644
--- a/drivers/media/video/pwc/pwc-if.c
+++ b/drivers/media/video/pwc/pwc-if.c
@@ -151,8 +151,6 @@ static int pwc_video_close(struct file *file);
 static ssize_t pwc_video_read(struct file *file, char __user *buf,
 			  size_t count, loff_t *ppos);
 static unsigned int pwc_video_poll(struct file *file, poll_table *wait);
-static long  pwc_video_ioctl(struct file *file,
-			    unsigned int ioctlnr, unsigned long arg);
 static int  pwc_video_mmap(struct file *file, struct vm_area_struct *vma);
 
 static const struct v4l2_file_operations pwc_fops = {
@@ -162,7 +160,7 @@ static const struct v4l2_file_operations pwc_fops = {
 	.read =		pwc_video_read,
 	.poll =		pwc_video_poll,
 	.mmap =		pwc_video_mmap,
-	.unlocked_ioctl = pwc_video_ioctl,
+	.unlocked_ioctl = video_ioctl2,
 };
 static struct video_device pwc_template = {
 	.name =		"Philips Webcam",	/* Filled in later */
@@ -1098,7 +1096,6 @@ static int pwc_video_open(struct file *file)
 		return -EBUSY;
 	}
 
-	mutex_lock(&pdev->modlock);
 	pwc_construct(pdev); /* set min/max sizes correct */
 	if (!pdev->usb_init) {
 		PWC_DEBUG_OPEN("Doing first time initialization.\n");
@@ -1130,7 +1127,6 @@ static int pwc_video_open(struct file *file)
 	if (i < 0) {
 		PWC_DEBUG_OPEN("Failed to allocate buffers memory.\n");
 		pwc_free_buffers(pdev);
-		mutex_unlock(&pdev->modlock);
 		return i;
 	}
 
@@ -1171,7 +1167,6 @@ static int pwc_video_open(struct file *file)
 	if (i) {
 		PWC_DEBUG_OPEN("Second attempt at set_video_mode failed.\n");
 		pwc_free_buffers(pdev);
-		mutex_unlock(&pdev->modlock);
 		return i;
 	}
 
@@ -1181,7 +1176,6 @@ static int pwc_video_open(struct file *file)
 
 	pdev->vopen++;
 	file->private_data = vdev;
-	mutex_unlock(&pdev->modlock);
 	PWC_DEBUG_OPEN("<< video_open() returns 0.\n");
 	return 0;
 }
@@ -1210,7 +1204,6 @@ static int pwc_video_close(struct file *file)
 	PWC_DEBUG_OPEN(">> video_close called(vdev = 0x%p).\n", vdev);
 
 	pdev = video_get_drvdata(vdev);
-	mutex_lock(&pdev->modlock);
 	if (pdev->vopen == 0)
 		PWC_DEBUG_MODULE("video_close() called on closed device?\n");
 
@@ -1248,7 +1241,6 @@ static int pwc_video_close(struct file *file)
 			if (device_hint[hint].pdev == pdev)
 				device_hint[hint].pdev = NULL;
 	}
-	mutex_unlock(&pdev->modlock);
 
 	return 0;
 }
@@ -1283,7 +1275,6 @@ static ssize_t pwc_video_read(struct file *file, char __user *buf,
 	if (pdev == NULL)
 		return -EFAULT;
 
-	mutex_lock(&pdev->modlock);
 	if (pdev->error_status) {
 		rv = -pdev->error_status; /* Something happened, report what. */
 		goto err_out;
@@ -1318,8 +1309,10 @@ static ssize_t pwc_video_read(struct file *file, char __user *buf,
 				rv = -ERESTARTSYS;
 				goto err_out;
 			}
+			mutex_unlock(&pdev->modlock);
 			schedule();
 			set_current_state(TASK_INTERRUPTIBLE);
+			mutex_lock(&pdev->modlock);
 		}
 		remove_wait_queue(&pdev->frameq, &wait);
 		set_current_state(TASK_RUNNING);
@@ -1352,10 +1345,8 @@ static ssize_t pwc_video_read(struct file *file, char __user *buf,
 		pdev->image_read_pos = 0;
 		pwc_next_image(pdev);
 	}
-	mutex_unlock(&pdev->modlock);
 	return count;
 err_out:
-	mutex_unlock(&pdev->modlock);
 	return rv;
 }
 
@@ -1372,9 +1363,7 @@ static unsigned int pwc_video_poll(struct file *file, poll_table *wait)
 		return -EFAULT;
 
 	/* Start the stream (if not already started) */
-	mutex_lock(&pdev->modlock);
 	ret = pwc_isoc_init(pdev);
-	mutex_unlock(&pdev->modlock);
 	if (ret)
 		return ret;
 
@@ -1387,25 +1376,6 @@ static unsigned int pwc_video_poll(struct file *file, poll_table *wait)
 	return 0;
 }
 
-static long pwc_video_ioctl(struct file *file,
-			   unsigned int cmd, unsigned long arg)
-{
-	struct video_device *vdev = file->private_data;
-	struct pwc_device *pdev;
-	long r = -ENODEV;
-
-	if (!vdev)
-		goto out;
-	pdev = video_get_drvdata(vdev);
-
-	mutex_lock(&pdev->modlock);
-	if (!pdev->unplugged)
-		r = video_usercopy(file, cmd, arg, pwc_video_do_ioctl);
-	mutex_unlock(&pdev->modlock);
-out:
-	return r;
-}
-
 static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct video_device *vdev = file->private_data;
@@ -1754,6 +1724,8 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	}
 	memcpy(pdev->vdev, &pwc_template, sizeof(pwc_template));
 	pdev->vdev->parent = &intf->dev;
+	pdev->vdev->lock = &pdev->modlock;
+	pdev->vdev->ioctl_ops = &pwc_ioctl_ops;
 	strcpy(pdev->vdev->name, name);
 	video_set_drvdata(pdev->vdev, pdev);
 
diff --git a/drivers/media/video/pwc/pwc-v4l.c b/drivers/media/video/pwc/pwc-v4l.c
index 8ca4d22b4384..68a5313a52d5 100644
--- a/drivers/media/video/pwc/pwc-v4l.c
+++ b/drivers/media/video/pwc/pwc-v4l.c
@@ -341,604 +341,554 @@ static int pwc_vidioc_set_fmt(struct pwc_device *pdev, struct v4l2_format *f)
 
 }
 
-long pwc_video_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int pwc_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 {
 	struct video_device *vdev = video_devdata(file);
-	struct pwc_device *pdev;
-	DECLARE_WAITQUEUE(wait, current);
-
-	if (vdev == NULL)
-		return -EFAULT;
-	pdev = video_get_drvdata(vdev);
-	if (pdev == NULL)
-		return -EFAULT;
+	struct pwc_device *pdev = video_drvdata(file);
+
+	strcpy(cap->driver, PWC_NAME);
+	strlcpy(cap->card, vdev->name, sizeof(cap->card));
+	usb_make_path(pdev->udev, cap->bus_info, sizeof(cap->bus_info));
+	cap->version = PWC_VERSION_CODE;
+	cap->capabilities =
+		V4L2_CAP_VIDEO_CAPTURE	|
+		V4L2_CAP_STREAMING	|
+		V4L2_CAP_READWRITE;
+	return 0;
+}
 
-#ifdef CONFIG_USB_PWC_DEBUG
-	if (PWC_DEBUG_LEVEL_IOCTL & pwc_trace) {
-		v4l_printk_ioctl(cmd);
-		printk("\n");
-	}
-#endif
-
-
-	switch (cmd) {
-		/* V4L2 Layer */
-		case VIDIOC_QUERYCAP:
-		{
-		    struct v4l2_capability *cap = arg;
-
-		    PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYCAP) This application "\
-				       "try to use the v4l2 layer\n");
-		    strcpy(cap->driver,PWC_NAME);
-		    strlcpy(cap->card, vdev->name, sizeof(cap->card));
-		    usb_make_path(pdev->udev,cap->bus_info,sizeof(cap->bus_info));
-		    cap->version = PWC_VERSION_CODE;
-		    cap->capabilities =
-			V4L2_CAP_VIDEO_CAPTURE	|
-			V4L2_CAP_STREAMING	|
-			V4L2_CAP_READWRITE;
-		    return 0;
-		}
+static int pwc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
+{
+	if (i->index)	/* Only one INPUT is supported */
+		return -EINVAL;
 
-		case VIDIOC_ENUMINPUT:
-		{
-		    struct v4l2_input *i = arg;
+	strcpy(i->name, "usb");
+	return 0;
+}
 
-		    if ( i->index )	/* Only one INPUT is supported */
-			  return -EINVAL;
+static int pwc_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
 
-		    memset(i, 0, sizeof(struct v4l2_input));
-		    strcpy(i->name, "usb");
-		    return 0;
-		}
+static int pwc_s_input(struct file *file, void *fh, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
 
-		case VIDIOC_G_INPUT:
-		{
-		    int *i = arg;
-		    *i = 0;	/* Only one INPUT is supported */
-		    return 0;
-		}
-		case VIDIOC_S_INPUT:
-		{
-			int *i = arg;
+static int pwc_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *c)
+{
+	int i;
 
-			if ( *i ) {	/* Only one INPUT is supported */
-				PWC_DEBUG_IOCTL("Only one input source is"\
-					" supported with this webcam.\n");
-				return -EINVAL;
-			}
+	for (i = 0; i < sizeof(pwc_controls) / sizeof(struct v4l2_queryctrl); i++) {
+		if (pwc_controls[i].id == c->id) {
+			PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYCTRL) found\n");
+			memcpy(c, &pwc_controls[i], sizeof(struct v4l2_queryctrl));
 			return 0;
 		}
+	}
+	return -EINVAL;
+}
 
-		/* TODO: */
-		case VIDIOC_QUERYCTRL:
-		{
-			struct v4l2_queryctrl *c = arg;
-			int i;
-
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYCTRL) query id=%d\n", c->id);
-			for (i=0; i<sizeof(pwc_controls)/sizeof(struct v4l2_queryctrl); i++) {
-				if (pwc_controls[i].id == c->id) {
-					PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYCTRL) found\n");
-					memcpy(c,&pwc_controls[i],sizeof(struct v4l2_queryctrl));
-					return 0;
-				}
-			}
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYCTRL) not found\n");
+static int pwc_g_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
+	switch (c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		c->value = pwc_get_brightness(pdev);
+		if (c->value < 0)
 			return -EINVAL;
-		}
-		case VIDIOC_G_CTRL:
-		{
-			struct v4l2_control *c = arg;
-			int ret;
-
-			switch (c->id)
-			{
-				case V4L2_CID_BRIGHTNESS:
-					c->value = pwc_get_brightness(pdev);
-					if (c->value<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_CONTRAST:
-					c->value = pwc_get_contrast(pdev);
-					if (c->value<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_SATURATION:
-					ret = pwc_get_saturation(pdev, &c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_GAMMA:
-					c->value = pwc_get_gamma(pdev);
-					if (c->value<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_RED_BALANCE:
-					ret = pwc_get_red_gain(pdev, &c->value);
-					if (ret<0)
-						return -EINVAL;
-					c->value >>= 8;
-					return 0;
-				case V4L2_CID_BLUE_BALANCE:
-					ret = pwc_get_blue_gain(pdev, &c->value);
-					if (ret<0)
-						return -EINVAL;
-					c->value >>= 8;
-					return 0;
-				case V4L2_CID_AUTO_WHITE_BALANCE:
-					ret = pwc_get_awb(pdev);
-					if (ret<0)
-						return -EINVAL;
-					c->value = (ret == PWC_WB_MANUAL)?0:1;
-					return 0;
-				case V4L2_CID_GAIN:
-					ret = pwc_get_agc(pdev, &c->value);
-					if (ret<0)
-						return -EINVAL;
-					c->value >>= 8;
-					return 0;
-				case V4L2_CID_AUTOGAIN:
-					ret = pwc_get_agc(pdev, &c->value);
-					if (ret<0)
-						return -EINVAL;
-					c->value = (c->value < 0)?1:0;
-					return 0;
-				case V4L2_CID_EXPOSURE:
-					ret = pwc_get_shutter_speed(pdev, &c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_COLOUR_MODE:
-					ret = pwc_get_colour_mode(pdev, &c->value);
-					if (ret < 0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_AUTOCONTOUR:
-					ret = pwc_get_contour(pdev, &c->value);
-					if (ret < 0)
-						return -EINVAL;
-					c->value=(c->value == -1?1:0);
-					return 0;
-				case V4L2_CID_PRIVATE_CONTOUR:
-					ret = pwc_get_contour(pdev, &c->value);
-					if (ret < 0)
-						return -EINVAL;
-					c->value >>= 10;
-					return 0;
-				case V4L2_CID_PRIVATE_BACKLIGHT:
-					ret = pwc_get_backlight(pdev, &c->value);
-					if (ret < 0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_FLICKERLESS:
-					ret = pwc_get_flicker(pdev, &c->value);
-					if (ret < 0)
-						return -EINVAL;
-					c->value=(c->value?1:0);
-					return 0;
-				case V4L2_CID_PRIVATE_NOISE_REDUCTION:
-					ret = pwc_get_dynamic_noise(pdev, &c->value);
-					if (ret < 0)
-						return -EINVAL;
-					return 0;
-
-				case V4L2_CID_PRIVATE_SAVE_USER:
-				case V4L2_CID_PRIVATE_RESTORE_USER:
-				case V4L2_CID_PRIVATE_RESTORE_FACTORY:
-					return -EINVAL;
-			}
+		return 0;
+	case V4L2_CID_CONTRAST:
+		c->value = pwc_get_contrast(pdev);
+		if (c->value < 0)
 			return -EINVAL;
-		}
-		case VIDIOC_S_CTRL:
-		{
-			struct v4l2_control *c = arg;
-			int ret;
-
-			switch (c->id)
-			{
-				case V4L2_CID_BRIGHTNESS:
-					c->value <<= 9;
-					ret = pwc_set_brightness(pdev, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_CONTRAST:
-					c->value <<= 10;
-					ret = pwc_set_contrast(pdev, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_SATURATION:
-					ret = pwc_set_saturation(pdev, c->value);
-					if (ret<0)
-					  return -EINVAL;
-					return 0;
-				case V4L2_CID_GAMMA:
-					c->value <<= 11;
-					ret = pwc_set_gamma(pdev, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_RED_BALANCE:
-					c->value <<= 8;
-					ret = pwc_set_red_gain(pdev, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_BLUE_BALANCE:
-					c->value <<= 8;
-					ret = pwc_set_blue_gain(pdev, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_AUTO_WHITE_BALANCE:
-					c->value = (c->value == 0)?PWC_WB_MANUAL:PWC_WB_AUTO;
-					ret = pwc_set_awb(pdev, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_EXPOSURE:
-					c->value <<= 8;
-					ret = pwc_set_shutter_speed(pdev, c->value?0:1, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_AUTOGAIN:
-					/* autogain off means nothing without a gain */
-					if (c->value == 0)
-						return 0;
-					ret = pwc_set_agc(pdev, c->value, 0);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_GAIN:
-					c->value <<= 8;
-					ret = pwc_set_agc(pdev, 0, c->value);
-					if (ret<0)
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_SAVE_USER:
-					if (pwc_save_user(pdev))
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_RESTORE_USER:
-					if (pwc_restore_user(pdev))
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_RESTORE_FACTORY:
-					if (pwc_restore_factory(pdev))
-						return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_COLOUR_MODE:
-					ret = pwc_set_colour_mode(pdev, c->value);
-					if (ret < 0)
-					  return -EINVAL;
-					return 0;
-				case V4L2_CID_PRIVATE_AUTOCONTOUR:
-				  c->value=(c->value == 1)?-1:0;
-				  ret = pwc_set_contour(pdev, c->value);
-				  if (ret < 0)
-				    return -EINVAL;
-				  return 0;
-				case V4L2_CID_PRIVATE_CONTOUR:
-				  c->value <<= 10;
-				  ret = pwc_set_contour(pdev, c->value);
-				  if (ret < 0)
-				    return -EINVAL;
-				  return 0;
-				case V4L2_CID_PRIVATE_BACKLIGHT:
-				  ret = pwc_set_backlight(pdev, c->value);
-				  if (ret < 0)
-				    return -EINVAL;
-				  return 0;
-				case V4L2_CID_PRIVATE_FLICKERLESS:
-				  ret = pwc_set_flicker(pdev, c->value);
-				  if (ret < 0)
-				    return -EINVAL;
-				case V4L2_CID_PRIVATE_NOISE_REDUCTION:
-				  ret = pwc_set_dynamic_noise(pdev, c->value);
-				  if (ret < 0)
-				    return -EINVAL;
-				  return 0;
-
-			}
+		return 0;
+	case V4L2_CID_SATURATION:
+		ret = pwc_get_saturation(pdev, &c->value);
+		if (ret < 0)
 			return -EINVAL;
-		}
+		return 0;
+	case V4L2_CID_GAMMA:
+		c->value = pwc_get_gamma(pdev);
+		if (c->value < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_RED_BALANCE:
+		ret = pwc_get_red_gain(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value >>= 8;
+		return 0;
+	case V4L2_CID_BLUE_BALANCE:
+		ret = pwc_get_blue_gain(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value >>= 8;
+		return 0;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = pwc_get_awb(pdev);
+		if (ret < 0)
+			return -EINVAL;
+		c->value = (ret == PWC_WB_MANUAL) ? 0 : 1;
+		return 0;
+	case V4L2_CID_GAIN:
+		ret = pwc_get_agc(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value >>= 8;
+		return 0;
+	case V4L2_CID_AUTOGAIN:
+		ret = pwc_get_agc(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value = (c->value < 0) ? 1 : 0;
+		return 0;
+	case V4L2_CID_EXPOSURE:
+		ret = pwc_get_shutter_speed(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_COLOUR_MODE:
+		ret = pwc_get_colour_mode(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_AUTOCONTOUR:
+		ret = pwc_get_contour(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value = (c->value == -1 ? 1 : 0);
+		return 0;
+	case V4L2_CID_PRIVATE_CONTOUR:
+		ret = pwc_get_contour(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value >>= 10;
+		return 0;
+	case V4L2_CID_PRIVATE_BACKLIGHT:
+		ret = pwc_get_backlight(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_FLICKERLESS:
+		ret = pwc_get_flicker(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		c->value = (c->value ? 1 : 0);
+		return 0;
+	case V4L2_CID_PRIVATE_NOISE_REDUCTION:
+		ret = pwc_get_dynamic_noise(pdev, &c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
 
-		case VIDIOC_ENUM_FMT:
-		{
-			struct v4l2_fmtdesc *f = arg;
-			int index;
-
-			if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			      return -EINVAL;
-
-			/* We only support two format: the raw format, and YUV */
-			index = f->index;
-			memset(f,0,sizeof(struct v4l2_fmtdesc));
-			f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-			f->index = index;
-			switch(index)
-			{
-				case 0:
-					/* RAW format */
-					f->pixelformat = pdev->type<=646?V4L2_PIX_FMT_PWC1:V4L2_PIX_FMT_PWC2;
-					f->flags = V4L2_FMT_FLAG_COMPRESSED;
-					strlcpy(f->description,"Raw Philips Webcam",sizeof(f->description));
-					break;
-				case 1:
-					f->pixelformat = V4L2_PIX_FMT_YUV420;
-					strlcpy(f->description,"4:2:0, planar, Y-Cb-Cr",sizeof(f->description));
-					break;
-				default:
-					return -EINVAL;
-			}
-			return 0;
-		}
+	case V4L2_CID_PRIVATE_SAVE_USER:
+	case V4L2_CID_PRIVATE_RESTORE_USER:
+	case V4L2_CID_PRIVATE_RESTORE_FACTORY:
+		return -EINVAL;
+	}
+	return -EINVAL;
+}
 
-		case VIDIOC_G_FMT:
-		{
-			struct v4l2_format *f = arg;
+static int pwc_s_ctrl(struct file *file, void *fh, struct v4l2_control *c)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
+
+	switch (c->id) {
+	case V4L2_CID_BRIGHTNESS:
+		c->value <<= 9;
+		ret = pwc_set_brightness(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_CONTRAST:
+		c->value <<= 10;
+		ret = pwc_set_contrast(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_SATURATION:
+		ret = pwc_set_saturation(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_GAMMA:
+		c->value <<= 11;
+		ret = pwc_set_gamma(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_RED_BALANCE:
+		c->value <<= 8;
+		ret = pwc_set_red_gain(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_BLUE_BALANCE:
+		c->value <<= 8;
+		ret = pwc_set_blue_gain(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		c->value = (c->value == 0) ? PWC_WB_MANUAL : PWC_WB_AUTO;
+		ret = pwc_set_awb(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_EXPOSURE:
+		c->value <<= 8;
+		ret = pwc_set_shutter_speed(pdev, c->value ? 0 : 1, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_AUTOGAIN:
+		/* autogain off means nothing without a gain */
+		if (c->value == 0)
+			return 0;
+		ret = pwc_set_agc(pdev, c->value, 0);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_GAIN:
+		c->value <<= 8;
+		ret = pwc_set_agc(pdev, 0, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_SAVE_USER:
+		if (pwc_save_user(pdev))
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_RESTORE_USER:
+		if (pwc_restore_user(pdev))
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_RESTORE_FACTORY:
+		if (pwc_restore_factory(pdev))
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_COLOUR_MODE:
+		ret = pwc_set_colour_mode(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_AUTOCONTOUR:
+		c->value = (c->value == 1) ? -1 : 0;
+		ret = pwc_set_contour(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_CONTOUR:
+		c->value <<= 10;
+		ret = pwc_set_contour(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_BACKLIGHT:
+		ret = pwc_set_backlight(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
+	case V4L2_CID_PRIVATE_FLICKERLESS:
+		ret = pwc_set_flicker(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+	case V4L2_CID_PRIVATE_NOISE_REDUCTION:
+		ret = pwc_set_dynamic_noise(pdev, c->value);
+		if (ret < 0)
+			return -EINVAL;
+		return 0;
 
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_G_FMT) return size %dx%d\n",pdev->image.x,pdev->image.y);
-			if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			      return -EINVAL;
+	}
+	return -EINVAL;
+}
 
-			pwc_vidioc_fill_fmt(pdev, f);
+static int pwc_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+
+	/* We only support two format: the raw format, and YUV */
+	switch (f->index) {
+	case 0:
+		/* RAW format */
+		f->pixelformat = pdev->type <= 646 ? V4L2_PIX_FMT_PWC1 : V4L2_PIX_FMT_PWC2;
+		f->flags = V4L2_FMT_FLAG_COMPRESSED;
+		strlcpy(f->description, "Raw Philips Webcam", sizeof(f->description));
+		break;
+	case 1:
+		f->pixelformat = V4L2_PIX_FMT_YUV420;
+		strlcpy(f->description, "4:2:0, planar, Y-Cb-Cr", sizeof(f->description));
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
 
-			return 0;
-		}
+static int pwc_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct pwc_device *pdev = video_drvdata(file);
 
-		case VIDIOC_TRY_FMT:
-			return pwc_vidioc_try_fmt(pdev, arg);
+	PWC_DEBUG_IOCTL("ioctl(VIDIOC_G_FMT) return size %dx%d\n",
+			pdev->image.x, pdev->image.y);
+	pwc_vidioc_fill_fmt(pdev, f);
+	return 0;
+}
 
-		case VIDIOC_S_FMT:
-			return pwc_vidioc_set_fmt(pdev, arg);
+static int pwc_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct pwc_device *pdev = video_drvdata(file);
 
-		case VIDIOC_G_STD:
-		{
-			v4l2_std_id *std = arg;
-			*std = V4L2_STD_UNKNOWN;
-			return 0;
-		}
+	return pwc_vidioc_try_fmt(pdev, f);
+}
 
-		case VIDIOC_S_STD:
-		{
-			v4l2_std_id *std = arg;
-			if (*std != V4L2_STD_UNKNOWN)
-				return -EINVAL;
-			return 0;
-		}
+static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct pwc_device *pdev = video_drvdata(file);
 
-		case VIDIOC_ENUMSTD:
-		{
-			struct v4l2_standard *std = arg;
-			if (std->index != 0)
-				return -EINVAL;
-			std->id = V4L2_STD_UNKNOWN;
-			strlcpy(std->name, "webcam", sizeof(std->name));
-			return 0;
-		}
+	return pwc_vidioc_set_fmt(pdev, f);
+}
 
-		case VIDIOC_REQBUFS:
-		{
-			struct v4l2_requestbuffers *rb = arg;
-			int nbuffers;
+static int pwc_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
+{
+	int nbuffers;
 
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_REQBUFS) count=%d\n",rb->count);
-			if (rb->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-				return -EINVAL;
-			if (rb->memory != V4L2_MEMORY_MMAP)
-				return -EINVAL;
+	PWC_DEBUG_IOCTL("ioctl(VIDIOC_REQBUFS) count=%d\n", rb->count);
+	if (rb->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (rb->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
 
-			nbuffers = rb->count;
-			if (nbuffers < 2)
-				nbuffers = 2;
-			else if (nbuffers > pwc_mbufs)
-				nbuffers = pwc_mbufs;
-			/* Force to use our # of buffers */
-			rb->count = pwc_mbufs;
-			return 0;
-		}
+	nbuffers = rb->count;
+	if (nbuffers < 2)
+		nbuffers = 2;
+	else if (nbuffers > pwc_mbufs)
+		nbuffers = pwc_mbufs;
+	/* Force to use our # of buffers */
+	rb->count = pwc_mbufs;
+	return 0;
+}
 
-		case VIDIOC_QUERYBUF:
-		{
-			struct v4l2_buffer *buf = arg;
-			int index;
+static int pwc_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+	int index;
 
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) index=%d\n",buf->index);
-			if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-				PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad type\n");
-				return -EINVAL;
-			}
-			if (buf->memory != V4L2_MEMORY_MMAP) {
-				PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad memory type\n");
-				return -EINVAL;
-			}
-			index = buf->index;
-			if (index < 0 || index >= pwc_mbufs) {
-				PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad index %d\n", buf->index);
-				return -EINVAL;
-			}
+	PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) index=%d\n", buf->index);
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad type\n");
+		return -EINVAL;
+	}
+	index = buf->index;
+	if (index < 0 || index >= pwc_mbufs) {
+		PWC_DEBUG_IOCTL("ioctl(VIDIOC_QUERYBUF) Bad index %d\n", buf->index);
+		return -EINVAL;
+	}
 
-			memset(buf, 0, sizeof(struct v4l2_buffer));
-			buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-			buf->index = index;
-			buf->m.offset = index * pdev->len_per_image;
-			if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
-				buf->bytesused = pdev->frame_size + sizeof(struct pwc_raw_frame);
-			else
-				buf->bytesused = pdev->view.size;
-			buf->field = V4L2_FIELD_NONE;
-			buf->memory = V4L2_MEMORY_MMAP;
-			//buf->flags = V4L2_BUF_FLAG_MAPPED;
-			buf->length = pdev->len_per_image;
-
-			PWC_DEBUG_READ("VIDIOC_QUERYBUF: index=%d\n",buf->index);
-			PWC_DEBUG_READ("VIDIOC_QUERYBUF: m.offset=%d\n",buf->m.offset);
-			PWC_DEBUG_READ("VIDIOC_QUERYBUF: bytesused=%d\n",buf->bytesused);
+	buf->m.offset = index * pdev->len_per_image;
+	if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
+		buf->bytesused = pdev->frame_size + sizeof(struct pwc_raw_frame);
+	else
+		buf->bytesused = pdev->view.size;
+	buf->field = V4L2_FIELD_NONE;
+	buf->memory = V4L2_MEMORY_MMAP;
+	/*buf->flags = V4L2_BUF_FLAG_MAPPED;*/
+	buf->length = pdev->len_per_image;
 
-			return 0;
-		}
+	PWC_DEBUG_READ("VIDIOC_QUERYBUF: index=%d\n", buf->index);
+	PWC_DEBUG_READ("VIDIOC_QUERYBUF: m.offset=%d\n", buf->m.offset);
+	PWC_DEBUG_READ("VIDIOC_QUERYBUF: bytesused=%d\n", buf->bytesused);
 
-		case VIDIOC_QBUF:
-		{
-			struct v4l2_buffer *buf = arg;
+	return 0;
+}
 
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_QBUF) index=%d\n",buf->index);
-			if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-				return -EINVAL;
-			if (buf->memory != V4L2_MEMORY_MMAP)
-				return -EINVAL;
-			if (buf->index >= pwc_mbufs)
-				return -EINVAL;
+static int pwc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	PWC_DEBUG_IOCTL("ioctl(VIDIOC_QBUF) index=%d\n", buf->index);
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (buf->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+	if (buf->index >= pwc_mbufs)
+		return -EINVAL;
 
-			buf->flags |= V4L2_BUF_FLAG_QUEUED;
-			buf->flags &= ~V4L2_BUF_FLAG_DONE;
+	buf->flags |= V4L2_BUF_FLAG_QUEUED;
+	buf->flags &= ~V4L2_BUF_FLAG_DONE;
 
-			return 0;
-		}
+	return 0;
+}
 
-		case VIDIOC_DQBUF:
-		{
-			struct v4l2_buffer *buf = arg;
-			int ret;
+static int pwc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	struct pwc_device *pdev = video_drvdata(file);
+	int ret;
 
-			PWC_DEBUG_IOCTL("ioctl(VIDIOC_DQBUF)\n");
+	PWC_DEBUG_IOCTL("ioctl(VIDIOC_DQBUF)\n");
 
-			if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-				return -EINVAL;
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
 
-			/* Add ourselves to the frame wait-queue.
-
-			   FIXME: needs auditing for safety.
-			   QUESTION: In what respect? I think that using the
-				     frameq is safe now.
-			 */
-			add_wait_queue(&pdev->frameq, &wait);
-			while (pdev->full_frames == NULL) {
-				if (pdev->error_status) {
-					remove_wait_queue(&pdev->frameq, &wait);
-					set_current_state(TASK_RUNNING);
-					return -pdev->error_status;
-				}
+	add_wait_queue(&pdev->frameq, &wait);
+	while (pdev->full_frames == NULL) {
+		if (pdev->error_status) {
+			remove_wait_queue(&pdev->frameq, &wait);
+			set_current_state(TASK_RUNNING);
+			return -pdev->error_status;
+		}
 
-				if (signal_pending(current)) {
-					remove_wait_queue(&pdev->frameq, &wait);
-					set_current_state(TASK_RUNNING);
-					return -ERESTARTSYS;
-				}
-				schedule();
-				set_current_state(TASK_INTERRUPTIBLE);
-			}
+		if (signal_pending(current)) {
 			remove_wait_queue(&pdev->frameq, &wait);
 			set_current_state(TASK_RUNNING);
+			return -ERESTARTSYS;
+		}
+		mutex_unlock(&pdev->modlock);
+		schedule();
+		set_current_state(TASK_INTERRUPTIBLE);
+		mutex_lock(&pdev->modlock);
+	}
+	remove_wait_queue(&pdev->frameq, &wait);
+	set_current_state(TASK_RUNNING);
 
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: frame ready.\n");
-			/* Decompress data in pdev->images[pdev->fill_image] */
-			ret = pwc_handle_frame(pdev);
-			if (ret)
-				return -EFAULT;
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: after pwc_handle_frame\n");
-
-			buf->index = pdev->fill_image;
-			if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
-				buf->bytesused = pdev->frame_size + sizeof(struct pwc_raw_frame);
-			else
-				buf->bytesused = pdev->view.size;
-			buf->flags = V4L2_BUF_FLAG_MAPPED;
-			buf->field = V4L2_FIELD_NONE;
-			do_gettimeofday(&buf->timestamp);
-			buf->sequence = 0;
-			buf->memory = V4L2_MEMORY_MMAP;
-			buf->m.offset = pdev->fill_image * pdev->len_per_image;
-			buf->length = pdev->len_per_image;
-			pwc_next_image(pdev);
-
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: buf->index=%d\n",buf->index);
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: buf->length=%d\n",buf->length);
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: m.offset=%d\n",buf->m.offset);
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: bytesused=%d\n",buf->bytesused);
-			PWC_DEBUG_IOCTL("VIDIOC_DQBUF: leaving\n");
-			return 0;
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: frame ready.\n");
+	/* Decompress data in pdev->images[pdev->fill_image] */
+	ret = pwc_handle_frame(pdev);
+	if (ret)
+		return -EFAULT;
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: after pwc_handle_frame\n");
+
+	buf->index = pdev->fill_image;
+	if (pdev->pixfmt != V4L2_PIX_FMT_YUV420)
+		buf->bytesused = pdev->frame_size + sizeof(struct pwc_raw_frame);
+	else
+		buf->bytesused = pdev->view.size;
+	buf->flags = V4L2_BUF_FLAG_MAPPED;
+	buf->field = V4L2_FIELD_NONE;
+	do_gettimeofday(&buf->timestamp);
+	buf->sequence = 0;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = pdev->fill_image * pdev->len_per_image;
+	buf->length = pdev->len_per_image;
+	pwc_next_image(pdev);
+
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: buf->index=%d\n", buf->index);
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: buf->length=%d\n", buf->length);
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: m.offset=%d\n", buf->m.offset);
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: bytesused=%d\n", buf->bytesused);
+	PWC_DEBUG_IOCTL("VIDIOC_DQBUF: leaving\n");
+	return 0;
 
-		}
+}
 
-		case VIDIOC_STREAMON:
-		{
-			return pwc_isoc_init(pdev);
-		}
+static int pwc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	struct pwc_device *pdev = video_drvdata(file);
 
-		case VIDIOC_STREAMOFF:
-		{
-			pwc_isoc_cleanup(pdev);
-			return 0;
-		}
+	return pwc_isoc_init(pdev);
+}
 
-		case VIDIOC_ENUM_FRAMESIZES:
-		{
-			struct v4l2_frmsizeenum *fsize = arg;
-			unsigned int i = 0, index = fsize->index;
-
-			if (fsize->pixel_format == V4L2_PIX_FMT_YUV420) {
-				for (i = 0; i < PSZ_MAX; i++) {
-					if (pdev->image_mask & (1UL << i)) {
-						if (!index--) {
-							fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-							fsize->discrete.width = pwc_image_sizes[i].x;
-							fsize->discrete.height = pwc_image_sizes[i].y;
-							return 0;
-						}
-					}
-				}
-			} else if (fsize->index == 0 &&
-				   ((fsize->pixel_format == V4L2_PIX_FMT_PWC1 && DEVICE_USE_CODEC1(pdev->type)) ||
-				    (fsize->pixel_format == V4L2_PIX_FMT_PWC2 && DEVICE_USE_CODEC23(pdev->type)))) {
-
-				fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
-				fsize->discrete.width = pdev->abs_max.x;
-				fsize->discrete.height = pdev->abs_max.y;
-				return 0;
-			}
-			return -EINVAL;
-		}
+static int pwc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	struct pwc_device *pdev = video_drvdata(file);
 
-		case VIDIOC_ENUM_FRAMEINTERVALS:
-		{
-			struct v4l2_frmivalenum *fival = arg;
-			int size = -1;
-			unsigned int i;
-
-			for (i = 0; i < PSZ_MAX; i++) {
-				if (pwc_image_sizes[i].x == fival->width &&
-				    pwc_image_sizes[i].y == fival->height) {
-					size = i;
-					break;
+	pwc_isoc_cleanup(pdev);
+	return 0;
+}
+
+static int pwc_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+	unsigned int i = 0, index = fsize->index;
+
+	if (fsize->pixel_format == V4L2_PIX_FMT_YUV420) {
+		for (i = 0; i < PSZ_MAX; i++) {
+			if (pdev->image_mask & (1UL << i)) {
+				if (!index--) {
+					fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+					fsize->discrete.width = pwc_image_sizes[i].x;
+					fsize->discrete.height = pwc_image_sizes[i].y;
+					return 0;
 				}
 			}
+		}
+	} else if (fsize->index == 0 &&
+			((fsize->pixel_format == V4L2_PIX_FMT_PWC1 && DEVICE_USE_CODEC1(pdev->type)) ||
+			 (fsize->pixel_format == V4L2_PIX_FMT_PWC2 && DEVICE_USE_CODEC23(pdev->type)))) {
+
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete.width = pdev->abs_max.x;
+		fsize->discrete.height = pdev->abs_max.y;
+		return 0;
+	}
+	return -EINVAL;
+}
 
-			/* TODO: Support raw format */
-			if (size < 0 || fival->pixel_format != V4L2_PIX_FMT_YUV420) {
-				return -EINVAL;
-			}
+static int pwc_enum_frameintervals(struct file *file, void *fh,
+					   struct v4l2_frmivalenum *fival)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+	int size = -1;
+	unsigned int i;
+
+	for (i = 0; i < PSZ_MAX; i++) {
+		if (pwc_image_sizes[i].x == fival->width &&
+				pwc_image_sizes[i].y == fival->height) {
+			size = i;
+			break;
+		}
+	}
 
-			i = pwc_get_fps(pdev, fival->index, size);
-			if (!i)
-				return -EINVAL;
+	/* TODO: Support raw format */
+	if (size < 0 || fival->pixel_format != V4L2_PIX_FMT_YUV420)
+		return -EINVAL;
 
-			fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
-			fival->discrete.numerator = 1;
-			fival->discrete.denominator = i;
+	i = pwc_get_fps(pdev, fival->index, size);
+	if (!i)
+		return -EINVAL;
 
-			return 0;
-		}
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1;
+	fival->discrete.denominator = i;
 
-		default:
-			return pwc_ioctl(pdev, cmd, arg);
-	} /* ..switch */
 	return 0;
 }
 
+static long pwc_default(struct file *file, void *fh, int cmd, void *arg)
+{
+	struct pwc_device *pdev = video_drvdata(file);
+
+	return pwc_ioctl(pdev, cmd, arg);
+}
+
+const struct v4l2_ioctl_ops pwc_ioctl_ops = {
+	.vidioc_querycap		    = pwc_querycap,
+	.vidioc_enum_input		    = pwc_enum_input,
+	.vidioc_g_input			    = pwc_g_input,
+	.vidioc_s_input			    = pwc_s_input,
+	.vidioc_enum_fmt_vid_cap	    = pwc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		    = pwc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		    = pwc_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		    = pwc_try_fmt_vid_cap,
+	.vidioc_queryctrl		    = pwc_queryctrl,
+	.vidioc_g_ctrl			    = pwc_g_ctrl,
+	.vidioc_s_ctrl			    = pwc_s_ctrl,
+	.vidioc_reqbufs			    = pwc_reqbufs,
+	.vidioc_querybuf		    = pwc_querybuf,
+	.vidioc_qbuf			    = pwc_qbuf,
+	.vidioc_dqbuf			    = pwc_dqbuf,
+	.vidioc_streamon		    = pwc_streamon,
+	.vidioc_streamoff		    = pwc_streamoff,
+	.vidioc_enum_framesizes		    = pwc_enum_framesizes,
+	.vidioc_enum_frameintervals	    = pwc_enum_frameintervals,
+	.vidioc_default		    = pwc_default,
+};
+
+
 /* vim: set cino= formatoptions=croql cindent shiftwidth=8 tabstop=8: */
diff --git a/drivers/media/video/pwc/pwc.h b/drivers/media/video/pwc/pwc.h
index 16bbc6df9b07..e947766337d6 100644
--- a/drivers/media/video/pwc/pwc.h
+++ b/drivers/media/video/pwc/pwc.h
@@ -339,8 +339,7 @@ extern int pwc_camera_power(struct pwc_device *pdev, int power);
 /* Private ioctl()s; see pwc-ioctl.h */
 extern long pwc_ioctl(struct pwc_device *pdev, unsigned int cmd, void *arg);
 
-/** Functions in pwc-v4l.c */
-extern long pwc_video_do_ioctl(struct file *file, unsigned int cmd, void *arg);
+extern const struct v4l2_ioctl_ops pwc_ioctl_ops;
 
 /** pwc-uncompress.c */
 /* Expand frame to image, possibly including decompression. Uses read_frame and fill_image */
diff --git a/drivers/media/video/s2255drv.c b/drivers/media/video/s2255drv.c
index b63f8cafa671..561909b65ce6 100644
--- a/drivers/media/video/s2255drv.c
+++ b/drivers/media/video/s2255drv.c
@@ -57,7 +57,7 @@
 #include <linux/usb.h>
 
 #define S2255_MAJOR_VERSION	1
-#define S2255_MINOR_VERSION	20
+#define S2255_MINOR_VERSION	21
 #define S2255_RELEASE		0
 #define S2255_VERSION		KERNEL_VERSION(S2255_MAJOR_VERSION, \
 					       S2255_MINOR_VERSION, \
@@ -312,9 +312,9 @@ struct s2255_fh {
 };
 
 /* current cypress EEPROM firmware version */
-#define S2255_CUR_USB_FWVER	((3 << 8) | 6)
+#define S2255_CUR_USB_FWVER	((3 << 8) | 11)
 /* current DSP FW version */
-#define S2255_CUR_DSP_FWVER     8
+#define S2255_CUR_DSP_FWVER     10102
 /* Need DSP version 5+ for video status feature */
 #define S2255_MIN_DSP_STATUS      5
 #define S2255_MIN_DSP_COLORFILTER 8
@@ -492,9 +492,11 @@ static void planar422p_to_yuv_packed(const unsigned char *in,
 
 static void s2255_reset_dsppower(struct s2255_dev *dev)
 {
-	s2255_vendor_req(dev, 0x40, 0x0b0b, 0x0b0b, NULL, 0, 1);
+	s2255_vendor_req(dev, 0x40, 0x0b0b, 0x0b01, NULL, 0, 1);
 	msleep(10);
 	s2255_vendor_req(dev, 0x50, 0x0000, 0x0000, NULL, 0, 1);
+	msleep(600);
+	s2255_vendor_req(dev, 0x10, 0x0000, 0x0000, NULL, 0, 1);
 	return;
 }
 
diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 2f500809f53d..5159cc8a0e1c 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -27,13 +27,13 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-mem2mem.h>
-#include <media/videobuf-core.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
 
 #include "fimc-core.h"
 
 static struct v4l2_subdev *fimc_subdev_register(struct fimc_dev *fimc,
-					    struct s3c_fimc_isp_info *isp_info)
+					    struct s5p_fimc_isp_info *isp_info)
 {
 	struct i2c_adapter *i2c_adap;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
@@ -86,8 +86,8 @@ static void fimc_subdev_unregister(struct fimc_dev *fimc)
 static int fimc_subdev_attach(struct fimc_dev *fimc, int index)
 {
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
-	struct s3c_platform_fimc *pdata = fimc->pdata;
-	struct s3c_fimc_isp_info *isp_info;
+	struct s5p_platform_fimc *pdata = fimc->pdata;
+	struct s5p_fimc_isp_info *isp_info;
 	struct v4l2_subdev *sd;
 	int i;
 
@@ -113,26 +113,43 @@ static int fimc_subdev_attach(struct fimc_dev *fimc, int index)
 	return -ENODEV;
 }
 
-static int fimc_isp_subdev_init(struct fimc_dev *fimc, int index)
+static int fimc_isp_subdev_init(struct fimc_dev *fimc, unsigned int index)
 {
-	struct s3c_fimc_isp_info *isp_info;
+	struct s5p_fimc_isp_info *isp_info;
 	int ret;
 
+	if (index >= FIMC_MAX_CAMIF_CLIENTS)
+		return -EINVAL;
+
+	isp_info = fimc->pdata->isp_info[index];
+	if (!isp_info)
+		return -EINVAL;
+
+	if (isp_info->clk_frequency)
+		clk_set_rate(fimc->clock[CLK_CAM], isp_info->clk_frequency);
+
+	ret = clk_enable(fimc->clock[CLK_CAM]);
+	if (ret)
+		return ret;
+
 	ret = fimc_subdev_attach(fimc, index);
 	if (ret)
 		return ret;
 
-	isp_info = fimc->pdata->isp_info[fimc->vid_cap.input_index];
 	ret = fimc_hw_set_camera_polarity(fimc, isp_info);
-	if (!ret) {
-		ret = v4l2_subdev_call(fimc->vid_cap.sd, core,
-				       s_power, 1);
-		if (!ret)
-			return ret;
-	}
+	if (ret)
+		return ret;
 
+	ret = v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 1);
+	if (!ret)
+		return ret;
+
+	/* enabling power failed so unregister subdev */
 	fimc_subdev_unregister(fimc);
-	err("ISP initialization failed: %d", ret);
+
+	v4l2_err(&fimc->vid_cap.v4l2_dev, "ISP initialization failed: %d\n",
+		 ret);
+
 	return ret;
 }
 
@@ -141,7 +158,7 @@ static int fimc_isp_subdev_init(struct fimc_dev *fimc, int index)
  * Locking: The caller holds fimc->slock spinlock.
  */
 int fimc_vid_cap_buf_queue(struct fimc_dev *fimc,
-			     struct fimc_vid_buffer *fimc_vb)
+			   struct fimc_vid_buffer *fimc_vb)
 {
 	struct fimc_vid_cap *cap = &fimc->vid_cap;
 	struct fimc_ctx *ctx = cap->ctx;
@@ -149,7 +166,7 @@ int fimc_vid_cap_buf_queue(struct fimc_dev *fimc,
 
 	BUG_ON(!fimc || !fimc_vb);
 
-	ret = fimc_prepare_addr(ctx, fimc_vb, &ctx->d_frame,
+	ret = fimc_prepare_addr(ctx, &fimc_vb->vb, &ctx->d_frame,
 				&fimc_vb->paddr);
 	if (ret)
 		return ret;
@@ -174,7 +191,7 @@ static int fimc_stop_capture(struct fimc_dev *fimc)
 {
 	unsigned long flags;
 	struct fimc_vid_cap *cap;
-	int ret;
+	struct fimc_vid_buffer *buf;
 
 	cap = &fimc->vid_cap;
 
@@ -187,24 +204,213 @@ static int fimc_stop_capture(struct fimc_dev *fimc)
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	wait_event_timeout(fimc->irq_queue,
-			   test_bit(ST_CAPT_SHUT, &fimc->state),
+			   !test_bit(ST_CAPT_SHUT, &fimc->state),
 			   FIMC_SHUTDOWN_TIMEOUT);
 
-	ret = v4l2_subdev_call(cap->sd, video, s_stream, 0);
-	if (ret)
-		v4l2_err(&fimc->vid_cap.v4l2_dev, "s_stream(0) failed\n");
+	v4l2_subdev_call(cap->sd, video, s_stream, 0);
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc->state &= ~(1 << ST_CAPT_RUN | 1 << ST_CAPT_PEND |
 			1 << ST_CAPT_STREAM);
 
 	fimc->vid_cap.active_buf_cnt = 0;
+
+	/* Release buffers that were enqueued in the driver by videobuf2. */
+	while (!list_empty(&cap->pending_buf_q)) {
+		buf = pending_queue_pop(cap);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&cap->active_buf_q)) {
+		buf = active_queue_pop(cap);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
 	dbg("state: 0x%lx", fimc->state);
 	return 0;
 }
 
+static int start_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct s5p_fimc_isp_info *isp_info;
+	int ret;
+
+	ret = v4l2_subdev_call(fimc->vid_cap.sd, video, s_stream, 1);
+	if (ret && ret != -ENOIOCTLCMD)
+		return ret;
+
+	ret = fimc_prepare_config(ctx, ctx->state);
+	if (ret)
+		return ret;
+
+	isp_info = fimc->pdata->isp_info[fimc->vid_cap.input_index];
+	fimc_hw_set_camera_type(fimc, isp_info);
+	fimc_hw_set_camera_source(fimc, isp_info);
+	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
+
+	if (ctx->state & FIMC_PARAMS) {
+		ret = fimc_set_scaler_info(ctx);
+		if (ret) {
+			err("Scaler setup error");
+			return ret;
+		}
+		fimc_hw_set_input_path(ctx);
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
+		fimc_hw_set_target_format(ctx);
+		fimc_hw_set_rotation(ctx);
+		fimc_hw_set_effect(ctx);
+	}
+
+	fimc_hw_set_output_path(ctx);
+	fimc_hw_set_out_dma(ctx);
+
+	INIT_LIST_HEAD(&fimc->vid_cap.pending_buf_q);
+	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
+	fimc->vid_cap.active_buf_cnt = 0;
+	fimc->vid_cap.frame_count = 0;
+	fimc->vid_cap.buf_index = fimc_hw_get_frame_index(fimc);
+
+	set_bit(ST_CAPT_PEND, &fimc->state);
+
+	return 0;
+}
+
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	if (!fimc_capture_running(fimc) && !fimc_capture_pending(fimc)) {
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		return -EINVAL;
+	}
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	return fimc_stop_capture(fimc);
+}
+
+static unsigned int get_plane_size(struct fimc_frame *fr, unsigned int plane)
+{
+	if (!fr || plane >= fr->fmt->memplanes)
+		return 0;
+
+	dbg("%s: w: %d. h: %d. depth[%d]: %d",
+	    __func__, fr->width, fr->height, plane, fr->fmt->depth[plane]);
+
+	return fr->f_width * fr->f_height * fr->fmt->depth[plane] / 8;
+
+}
+
+static int queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
+		       unsigned int *num_planes, unsigned long sizes[],
+		       void *allocators[])
+{
+	struct fimc_ctx *ctx = vq->drv_priv;
+	struct fimc_fmt *fmt = ctx->d_frame.fmt;
+	int i;
+
+	if (!fmt)
+		return -EINVAL;
+
+	*num_planes = fmt->memplanes;
+
+	dbg("%s, buffer count=%d, plane count=%d",
+	    __func__, *num_buffers, *num_planes);
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		sizes[i] = get_plane_size(&ctx->d_frame, i);
+		dbg("plane: %u, plane_size: %lu", i, sizes[i]);
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int buffer_init(struct vb2_buffer *vb)
+{
+	/* TODO: */
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct fimc_ctx *ctx = vq->drv_priv;
+	struct v4l2_device *v4l2_dev = &ctx->fimc_dev->m2m.v4l2_dev;
+	int i;
+
+	if (!ctx->d_frame.fmt || vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	for (i = 0; i < ctx->d_frame.fmt->memplanes; i++) {
+		unsigned long size = get_plane_size(&ctx->d_frame, i);
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(v4l2_dev, "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_vid_buffer *buf
+		= container_of(vb, struct fimc_vid_buffer, vb);
+	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	fimc_vid_cap_buf_queue(fimc, buf);
+
+	dbg("active_buf_cnt: %d", fimc->vid_cap.active_buf_cnt);
+
+	if (vid_cap->active_buf_cnt >= vid_cap->reqbufs_count ||
+	   vid_cap->active_buf_cnt >= FIMC_MAX_OUT_BUFS) {
+		if (!test_and_set_bit(ST_CAPT_STREAM, &fimc->state)) {
+			fimc_activate_capture(ctx);
+			dbg("");
+		}
+	}
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static void fimc_lock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_lock(&ctx->fimc_dev->lock);
+}
+
+static void fimc_unlock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_unlock(&ctx->fimc_dev->lock);
+}
+
+static struct vb2_ops fimc_capture_qops = {
+	.queue_setup		= queue_setup,
+	.buf_prepare		= buffer_prepare,
+	.buf_queue		= buffer_queue,
+	.buf_init		= buffer_init,
+	.wait_prepare		= fimc_unlock,
+	.wait_finish		= fimc_lock,
+	.start_streaming	= start_streaming,
+	.stop_streaming		= stop_streaming,
+};
+
 static int fimc_capture_open(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
@@ -216,44 +422,36 @@ static int fimc_capture_open(struct file *file)
 	if (fimc_m2m_active(fimc))
 		return -EBUSY;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	if (++fimc->vid_cap.refcnt == 1) {
-		ret = fimc_isp_subdev_init(fimc, -1);
+		ret = fimc_isp_subdev_init(fimc, 0);
 		if (ret) {
 			fimc->vid_cap.refcnt--;
-			ret = -EIO;
+			return -EIO;
 		}
 	}
 
 	file->private_data = fimc->vid_cap.ctx;
 
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return 0;
 }
 
 static int fimc_capture_close(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
 
 	if (--fimc->vid_cap.refcnt == 0) {
 		fimc_stop_capture(fimc);
-
-		videobuf_stop(&fimc->vid_cap.vbq);
-		videobuf_mmap_free(&fimc->vid_cap.vbq);
+		vb2_queue_release(&fimc->vid_cap.vbq);
 
 		v4l2_err(&fimc->vid_cap.v4l2_dev, "releasing ISP\n");
+
 		v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 0);
+		clk_disable(fimc->clock[CLK_CAM]);
 		fimc_subdev_unregister(fimc);
 	}
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -262,32 +460,16 @@ static unsigned int fimc_capture_poll(struct file *file,
 {
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	int ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return POLLERR;
-
-	ret = videobuf_poll_stream(file, &cap->vbq, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
+	return vb2_poll(&fimc->vid_cap.vbq, file, wait);
 }
 
 static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
-	ret = videobuf_mmap_mapper(&cap->vbq, vma);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
+	return vb2_mmap(&fimc->vid_cap.vbq, vma);
 }
 
 /* video device file operations */
@@ -310,7 +492,8 @@ static int fimc_vidioc_querycap_capture(struct file *file, void *priv,
 	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
-	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
+	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE |
+			    V4L2_CAP_VIDEO_CAPTURE_MPLANE;
 
 	return 0;
 }
@@ -351,57 +534,52 @@ static int sync_capture_fmt(struct fimc_ctx *ctx)
 	return 0;
 }
 
-static int fimc_cap_s_fmt(struct file *file, void *priv,
-			     struct v4l2_format *f)
+static int fimc_cap_s_fmt_mplane(struct file *file, void *priv,
+				 struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct fimc_frame *frame;
-	struct v4l2_pix_format *pix;
+	struct v4l2_pix_format_mplane *pix;
 	int ret;
+	int i;
 
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	ret = fimc_vidioc_try_fmt(file, priv, f);
+	ret = fimc_vidioc_try_fmt_mplane(file, priv, f);
 	if (ret)
 		return ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	if (fimc_capture_active(fimc)) {
-		ret = -EBUSY;
-		goto sf_unlock;
-	}
+	if (vb2_is_streaming(&fimc->vid_cap.vbq) || fimc_capture_active(fimc))
+		return -EBUSY;
 
 	frame = &ctx->d_frame;
 
-	pix = &f->fmt.pix;
+	pix = &f->fmt.pix_mp;
 	frame->fmt = find_format(f, FMT_FLAGS_M2M | FMT_FLAGS_CAM);
 	if (!frame->fmt) {
 		err("fimc target format not found\n");
-		ret = -EINVAL;
-		goto sf_unlock;
+		return -EINVAL;
 	}
 
+	for (i = 0; i < frame->fmt->colplanes; i++)
+		frame->payload[i] = pix->plane_fmt[i].bytesperline * pix->height;
+
 	/* Output DMA frame pixel size and offsets. */
-	frame->f_width	= pix->bytesperline * 8 / frame->fmt->depth;
+	frame->f_width = pix->plane_fmt[0].bytesperline * 8
+			/ frame->fmt->depth[0];
 	frame->f_height = pix->height;
 	frame->width	= pix->width;
 	frame->height	= pix->height;
 	frame->o_width	= pix->width;
 	frame->o_height = pix->height;
-	frame->size	= (pix->width * pix->height * frame->fmt->depth) >> 3;
 	frame->offs_h	= 0;
 	frame->offs_v	= 0;
 
-	ret = sync_capture_fmt(ctx);
-
 	ctx->state |= (FIMC_PARAMS | FIMC_DST_FMT);
 
-sf_unlock:
-	mutex_unlock(&fimc->lock);
+	ret = sync_capture_fmt(ctx);
 	return ret;
 }
 
@@ -409,8 +587,8 @@ static int fimc_cap_enum_input(struct file *file, void *priv,
 				     struct v4l2_input *i)
 {
 	struct fimc_ctx *ctx = priv;
-	struct s3c_platform_fimc *pldata = ctx->fimc_dev->pdata;
-	struct s3c_fimc_isp_info *isp_info;
+	struct s5p_platform_fimc *pldata = ctx->fimc_dev->pdata;
+	struct s5p_fimc_isp_info *isp_info;
 
 	if (i->index >= FIMC_MAX_CAMIF_CLIENTS)
 		return -EINVAL;
@@ -429,34 +607,27 @@ static int fimc_cap_s_input(struct file *file, void *priv,
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct s3c_platform_fimc *pdata = fimc->pdata;
-	int ret;
+	struct s5p_platform_fimc *pdata = fimc->pdata;
 
 	if (fimc_capture_active(ctx->fimc_dev))
 		return -EBUSY;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
+	if (i >= FIMC_MAX_CAMIF_CLIENTS || !pdata->isp_info[i])
+		return -EINVAL;
 
-	if (i >= FIMC_MAX_CAMIF_CLIENTS || !pdata->isp_info[i]) {
-		ret = -EINVAL;
-		goto si_unlock;
-	}
 
 	if (fimc->vid_cap.sd) {
-		ret = v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 0);
+		int ret = v4l2_subdev_call(fimc->vid_cap.sd, core, s_power, 0);
 		if (ret)
 			err("s_power failed: %d", ret);
+
+		clk_disable(fimc->clock[CLK_CAM]);
 	}
 
 	/* Release the attached sensor subdevice. */
 	fimc_subdev_unregister(fimc);
 
-	ret = fimc_isp_subdev_init(fimc, i);
-
-si_unlock:
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return fimc_isp_subdev_init(fimc, i);
 }
 
 static int fimc_cap_g_input(struct file *file, void *priv,
@@ -470,66 +641,20 @@ static int fimc_cap_g_input(struct file *file, void *priv,
 }
 
 static int fimc_cap_streamon(struct file *file, void *priv,
-			   enum v4l2_buf_type type)
+			     enum v4l2_buf_type type)
 {
-	struct s3c_fimc_isp_info *isp_info;
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret = -EBUSY;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	if (fimc_capture_active(fimc) || !fimc->vid_cap.sd)
-		goto s_unlock;
+		return -EBUSY;
 
 	if (!(ctx->state & FIMC_DST_FMT)) {
 		v4l2_err(&fimc->vid_cap.v4l2_dev, "Format is not set\n");
-		ret = -EINVAL;
-		goto s_unlock;
-	}
-
-	ret = v4l2_subdev_call(fimc->vid_cap.sd, video, s_stream, 1);
-	if (ret && ret != -ENOIOCTLCMD)
-		goto s_unlock;
-
-	ret = fimc_prepare_config(ctx, ctx->state);
-	if (ret)
-		goto s_unlock;
-
-	isp_info = fimc->pdata->isp_info[fimc->vid_cap.input_index];
-	fimc_hw_set_camera_type(fimc, isp_info);
-	fimc_hw_set_camera_source(fimc, isp_info);
-	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
-
-	if (ctx->state & FIMC_PARAMS) {
-		ret = fimc_set_scaler_info(ctx);
-		if (ret) {
-			err("Scaler setup error");
-			goto s_unlock;
-		}
-		fimc_hw_set_input_path(ctx);
-		fimc_hw_set_scaler(ctx);
-		fimc_hw_set_target_format(ctx);
-		fimc_hw_set_rotation(ctx);
-		fimc_hw_set_effect(ctx);
+		return -EINVAL;
 	}
 
-	fimc_hw_set_output_path(ctx);
-	fimc_hw_set_out_dma(ctx);
-
-	INIT_LIST_HEAD(&fimc->vid_cap.pending_buf_q);
-	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
-	fimc->vid_cap.active_buf_cnt = 0;
-	fimc->vid_cap.frame_count = 0;
-	fimc->vid_cap.buf_index = fimc_hw_get_frame_index(fimc);
-
-	set_bit(ST_CAPT_PEND, &fimc->state);
-	ret = videobuf_streamon(&fimc->vid_cap.vbq);
-
-s_unlock:
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return vb2_streamon(&fimc->vid_cap.vbq, type);
 }
 
 static int fimc_cap_streamoff(struct file *file, void *priv,
@@ -537,46 +662,22 @@ static int fimc_cap_streamoff(struct file *file, void *priv,
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&fimc->slock, flags);
-	if (!fimc_capture_running(fimc) && !fimc_capture_pending(fimc)) {
-		spin_unlock_irqrestore(&fimc->slock, flags);
-		dbg("state: 0x%lx", fimc->state);
-		return -EINVAL;
-	}
-	spin_unlock_irqrestore(&fimc->slock, flags);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	fimc_stop_capture(fimc);
-	ret = videobuf_streamoff(&cap->vbq);
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return vb2_streamoff(&fimc->vid_cap.vbq, type);
 }
 
 static int fimc_cap_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *reqbufs)
+			    struct v4l2_requestbuffers *reqbufs)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_vid_cap *cap = &ctx->fimc_dev->vid_cap;
 	int ret;
 
-	if (fimc_capture_active(ctx->fimc_dev))
-		return -EBUSY;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
-	ret = videobuf_reqbufs(&cap->vbq, reqbufs);
+	ret = vb2_reqbufs(&cap->vbq, reqbufs);
 	if (!ret)
 		cap->reqbufs_count = reqbufs->count;
 
-	mutex_unlock(&fimc->lock);
 	return ret;
 }
 
@@ -586,43 +687,23 @@ static int fimc_cap_querybuf(struct file *file, void *priv,
 	struct fimc_ctx *ctx = priv;
 	struct fimc_vid_cap *cap = &ctx->fimc_dev->vid_cap;
 
-	if (fimc_capture_active(ctx->fimc_dev))
-		return -EBUSY;
-
-	return videobuf_querybuf(&cap->vbq, buf);
+	return vb2_querybuf(&cap->vbq, buf);
 }
 
 static int fimc_cap_qbuf(struct file *file, void *priv,
 			  struct v4l2_buffer *buf)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = videobuf_qbuf(&cap->vbq, buf);
-
-	mutex_unlock(&fimc->lock);
-	return ret;
+	struct fimc_vid_cap *cap = &ctx->fimc_dev->vid_cap;
+	return vb2_qbuf(&cap->vbq, buf);
 }
 
 static int fimc_cap_dqbuf(struct file *file, void *priv,
 			   struct v4l2_buffer *buf)
 {
 	struct fimc_ctx *ctx = priv;
-	int ret;
-
-	if (mutex_lock_interruptible(&ctx->fimc_dev->lock))
-		return -ERESTARTSYS;
-
-	ret = videobuf_dqbuf(&ctx->fimc_dev->vid_cap.vbq, buf,
+	return vb2_dqbuf(&ctx->fimc_dev->vid_cap.vbq, buf,
 		file->f_flags & O_NONBLOCK);
-
-	mutex_unlock(&ctx->fimc_dev->lock);
-	return ret;
 }
 
 static int fimc_cap_s_ctrl(struct file *file, void *priv,
@@ -631,9 +712,6 @@ static int fimc_cap_s_ctrl(struct file *file, void *priv,
 	struct fimc_ctx *ctx = priv;
 	int ret = -EINVAL;
 
-	if (mutex_lock_interruptible(&ctx->fimc_dev->lock))
-		return -ERESTARTSYS;
-
 	/* Allow any controls but 90/270 rotation while streaming */
 	if (!fimc_capture_active(ctx->fimc_dev) ||
 	    ctrl->id != V4L2_CID_ROTATE ||
@@ -648,8 +726,6 @@ static int fimc_cap_s_ctrl(struct file *file, void *priv,
 	if (ret == -EINVAL)
 		ret = v4l2_subdev_call(ctx->fimc_dev->vid_cap.sd,
 				       core, s_ctrl, ctrl);
-
-	mutex_unlock(&ctx->fimc_dev->lock);
 	return ret;
 }
 
@@ -658,22 +734,18 @@ static int fimc_cap_cropcap(struct file *file, void *fh,
 {
 	struct fimc_frame *f;
 	struct fimc_ctx *ctx = fh;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 
-	if (cr->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (cr->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	f = &ctx->s_frame;
+
 	cr->bounds.left		= 0;
 	cr->bounds.top		= 0;
 	cr->bounds.width	= f->o_width;
 	cr->bounds.height	= f->o_height;
 	cr->defrect		= cr->bounds;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -681,19 +753,14 @@ static int fimc_cap_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 {
 	struct fimc_frame *f;
 	struct fimc_ctx *ctx = file->private_data;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	f = &ctx->s_frame;
+
 	cr->c.left	= f->offs_h;
 	cr->c.top	= f->offs_v;
 	cr->c.width	= f->width;
 	cr->c.height	= f->height;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -712,41 +779,38 @@ static int fimc_cap_s_crop(struct file *file, void *fh,
 	if (ret)
 		return ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	if (!(ctx->state & FIMC_DST_FMT)) {
 		v4l2_err(&fimc->vid_cap.v4l2_dev,
 			 "Capture color format not set\n");
-		goto sc_unlock;
+		return -EINVAL; /* TODO: make sure this is the right value */
 	}
 
 	f = &ctx->s_frame;
 	/* Check for the pixel scaling ratio when cropping input image. */
-	ret = fimc_check_scaler_ratio(&cr->c, &ctx->d_frame);
+	ret = fimc_check_scaler_ratio(cr->c.width, cr->c.height,
+				      ctx->d_frame.width, ctx->d_frame.height,
+				      ctx->rotation);
 	if (ret) {
 		v4l2_err(&fimc->vid_cap.v4l2_dev, "Out of the scaler range");
-	} else {
-		ret = 0;
-		f->offs_h = cr->c.left;
-		f->offs_v = cr->c.top;
-		f->width  = cr->c.width;
-		f->height = cr->c.height;
+		return ret;
 	}
 
-sc_unlock:
-	mutex_unlock(&fimc->lock);
-	return ret;
+	f->offs_h = cr->c.left;
+	f->offs_v = cr->c.top;
+	f->width  = cr->c.width;
+	f->height = cr->c.height;
+
+	return 0;
 }
 
 
 static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
 	.vidioc_querycap		= fimc_vidioc_querycap_capture,
 
-	.vidioc_enum_fmt_vid_cap	= fimc_vidioc_enum_fmt,
-	.vidioc_try_fmt_vid_cap		= fimc_vidioc_try_fmt,
-	.vidioc_s_fmt_vid_cap		= fimc_cap_s_fmt,
-	.vidioc_g_fmt_vid_cap		= fimc_vidioc_g_fmt,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_vidioc_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_vidioc_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_cap_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_vidioc_g_fmt_mplane,
 
 	.vidioc_reqbufs			= fimc_cap_reqbufs,
 	.vidioc_querybuf		= fimc_cap_querybuf,
@@ -770,6 +834,7 @@ static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
 	.vidioc_g_input			= fimc_cap_g_input,
 };
 
+/* fimc->lock must be already initialized */
 int fimc_register_capture_device(struct fimc_dev *fimc)
 {
 	struct v4l2_device *v4l2_dev = &fimc->vid_cap.v4l2_dev;
@@ -777,6 +842,8 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	struct fimc_vid_cap *vid_cap;
 	struct fimc_ctx *ctx;
 	struct v4l2_format f;
+	struct fimc_frame *fr;
+	struct vb2_queue *q;
 	int ret;
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
@@ -788,8 +855,12 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	ctx->out_path	 = FIMC_DMA;
 	ctx->state	 = FIMC_CTX_CAP;
 
-	f.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
-	ctx->d_frame.fmt = find_format(&f, FMT_FLAGS_M2M);
+	/* Default format of the output frames */
+	f.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB32;
+	fr = &ctx->d_frame;
+	fr->fmt = find_format(&f, FMT_FLAGS_M2M);
+	fr->width = fr->f_width = fr->o_width = 640;
+	fr->height = fr->f_height = fr->o_height = 480;
 
 	if (!v4l2_dev->name[0])
 		snprintf(v4l2_dev->name, sizeof(v4l2_dev->name),
@@ -812,6 +883,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	vfd->ioctl_ops	= &fimc_capture_ioctl_ops;
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
+	vfd->lock	= &fimc->lock;
 	video_set_drvdata(vfd, fimc);
 
 	vid_cap = &fimc->vid_cap;
@@ -819,7 +891,7 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	vid_cap->active_buf_cnt = 0;
 	vid_cap->reqbufs_count  = 0;
 	vid_cap->refcnt = 0;
-	/* The default color format for image sensor. */
+	/* Default color format for image sensor */
 	vid_cap->fmt.code = V4L2_MBUS_FMT_YUYV8_2X8;
 
 	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
@@ -827,10 +899,16 @@ int fimc_register_capture_device(struct fimc_dev *fimc)
 	spin_lock_init(&ctx->slock);
 	vid_cap->ctx = ctx;
 
-	videobuf_queue_dma_contig_init(&vid_cap->vbq, &fimc_qops,
-		vid_cap->v4l2_dev.dev, &fimc->irqlock,
-		V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
-		sizeof(struct fimc_vid_buffer), (void *)ctx, NULL);
+	q = &fimc->vid_cap.vbq;
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = fimc->vid_cap.ctx;
+	q->ops = &fimc_capture_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct fimc_vid_buffer);
+
+	vb2_queue_init(q);
 
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 	if (ret) {
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index 817aa66627f6..cd8a3003f1aa 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -25,114 +25,141 @@
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <media/v4l2-ioctl.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
 
 #include "fimc-core.h"
 
-static char *fimc_clock_name[NUM_FIMC_CLOCKS] = { "sclk_fimc", "fimc" };
+static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
+	"sclk_fimc", "fimc", "sclk_cam"
+};
 
 static struct fimc_fmt fimc_formats[] = {
 	{
-		.name	= "RGB565",
-		.fourcc	= V4L2_PIX_FMT_RGB565X,
-		.depth	= 16,
-		.color	= S5P_FIMC_RGB565,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_RGB565_2X8_BE,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_RGB565,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_RGB565_2X8_BE,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "BGR666",
+		.fourcc		= V4L2_PIX_FMT_BGR666,
+		.depth		= { 32 },
+		.color		= S5P_FIMC_RGB666,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "XRGB-8-8-8-8, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= { 32 },
+		.color		= S5P_FIMC_RGB888,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "BGR666",
-		.fourcc	= V4L2_PIX_FMT_BGR666,
-		.depth	= 32,
-		.color	= S5P_FIMC_RGB666,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name = "XRGB-8-8-8-8, 32 bpp",
-		.fourcc	= V4L2_PIX_FMT_RGB32,
-		.depth = 32,
-		.color	= S5P_FIMC_RGB888,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_CBYCRY422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_UYVY8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name	= "YUV 4:2:2 packed, YCbYCr",
-		.fourcc	= V4L2_PIX_FMT_YUYV,
-		.depth	= 16,
-		.color	= S5P_FIMC_YCBYCR422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_YUYV8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_CRYCBY422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_VYUY8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name	= "YUV 4:2:2 packed, CbYCrY",
-		.fourcc	= V4L2_PIX_FMT_UYVY,
-		.depth	= 16,
-		.color	= S5P_FIMC_CBYCRY422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_UYVY8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCRYCB422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= V4L2_MBUS_FMT_YVYU8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
 	}, {
-		.name	= "YUV 4:2:2 packed, CrYCbY",
-		.fourcc	= V4L2_PIX_FMT_VYUY,
-		.depth	= 16,
-		.color	= S5P_FIMC_CRYCBY422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_VYUY8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV422P,
+		.depth		= { 12 },
+		.color		= S5P_FIMC_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 packed, YCrYCb",
-		.fourcc	= V4L2_PIX_FMT_YVYU,
-		.depth	= 16,
-		.color	= S5P_FIMC_YCRYCB422,
-		.buff_cnt = 1,
-		.planes_cnt = 1,
-		.mbus_code = V4L2_MBUS_FMT_YVYU8_2X8,
-		.flags = FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+		.name		= "YUV 4:2:2 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 planar, Y/Cb/Cr",
-		.fourcc	= V4L2_PIX_FMT_YUV422P,
-		.depth	= 12,
-		.color	= S5P_FIMC_YCBCR422,
-		.buff_cnt = 1,
-		.planes_cnt = 3,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:2 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV61,
+		.depth		= { 16 },
+		.color		= S5P_FIMC_YCRYCB422,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 planar, Y/CbCr",
-		.fourcc	= V4L2_PIX_FMT_NV16,
-		.depth	= 16,
-		.color	= S5P_FIMC_YCBCR422,
-		.buff_cnt = 1,
-		.planes_cnt = 2,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 planar, YCbCr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= { 12 },
+		.color		= S5P_FIMC_YCBCR420,
+		.memplanes	= 1,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:2 planar, Y/CrCb",
-		.fourcc	= V4L2_PIX_FMT_NV61,
-		.depth	= 16,
-		.color	= S5P_FIMC_RGB565,
-		.buff_cnt = 1,
-		.planes_cnt = 2,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= { 12 },
+		.color		= S5P_FIMC_YCBCR420,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:0 planar, YCbCr",
-		.fourcc	= V4L2_PIX_FMT_YUV420,
-		.depth	= 12,
-		.color	= S5P_FIMC_YCBCR420,
-		.buff_cnt = 1,
-		.planes_cnt = 3,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.color		= S5P_FIMC_YCBCR420,
+		.depth		= { 8, 4 },
+		.memplanes	= 2,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	}, {
-		.name	= "YUV 4:2:0 planar, Y/CbCr",
-		.fourcc	= V4L2_PIX_FMT_NV12,
-		.depth	= 12,
-		.color	= S5P_FIMC_YCBCR420,
-		.buff_cnt = 1,
-		.planes_cnt = 2,
-		.flags = FMT_FLAGS_M2M,
+		.name		= "YUV 4:2:0 non-contiguous 3-planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420M,
+		.color		= S5P_FIMC_YCBCR420,
+		.depth		= { 8, 2, 2 },
+		.memplanes	= 3,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:0 non-contiguous 2-planar, Y/CbCr, tiled",
+		.fourcc		= V4L2_PIX_FMT_NV12MT,
+		.color		= S5P_FIMC_YCBCR420,
+		.depth		= { 8, 4 },
+		.memplanes	= 2,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
 	},
 };
 
@@ -173,24 +200,21 @@ static struct v4l2_queryctrl *get_ctrl(int id)
 	return NULL;
 }
 
-int fimc_check_scaler_ratio(struct v4l2_rect *r, struct fimc_frame *f)
+int fimc_check_scaler_ratio(int sw, int sh, int dw, int dh, int rot)
 {
-	if (r->width > f->width) {
-		if (f->width > (r->width * SCALER_MAX_HRATIO))
-			return -EINVAL;
-	} else {
-		if ((f->width * SCALER_MAX_HRATIO) < r->width)
-			return -EINVAL;
-	}
+	int tx, ty;
 
-	if (r->height > f->height) {
-		if (f->height > (r->height * SCALER_MAX_VRATIO))
-			return -EINVAL;
+	if (rot == 90 || rot == 270) {
+		ty = dw;
+		tx = dh;
 	} else {
-		if ((f->height * SCALER_MAX_VRATIO) < r->height)
-			return -EINVAL;
+		tx = dw;
+		ty = dh;
 	}
 
+	if ((sw >= SCALER_MAX_HRATIO * tx) || (sh >= SCALER_MAX_VRATIO * ty))
+		return -EINVAL;
+
 	return 0;
 }
 
@@ -221,6 +245,7 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	struct fimc_scaler *sc = &ctx->scaler;
 	struct fimc_frame *s_frame = &ctx->s_frame;
 	struct fimc_frame *d_frame = &ctx->d_frame;
+	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
 	int tx, ty, sx, sy;
 	int ret;
 
@@ -259,8 +284,14 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	sc->pre_dst_width = sx / sc->pre_hratio;
 	sc->pre_dst_height = sy / sc->pre_vratio;
 
-	sc->main_hratio = (sx << 8) / (tx << sc->hfactor);
-	sc->main_vratio = (sy << 8) / (ty << sc->vfactor);
+	if (variant->has_mainscaler_ext) {
+		sc->main_hratio = (sx << 14) / (tx << sc->hfactor);
+		sc->main_vratio = (sy << 14) / (ty << sc->vfactor);
+	} else {
+		sc->main_hratio = (sx << 8) / (tx << sc->hfactor);
+		sc->main_vratio = (sy << 8) / (ty << sc->vfactor);
+
+	}
 
 	sc->scaleup_h = (tx >= sx) ? 1 : 0;
 	sc->scaleup_v = (ty >= sy) ? 1 : 0;
@@ -276,6 +307,23 @@ int fimc_set_scaler_info(struct fimc_ctx *ctx)
 	return 0;
 }
 
+static int stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	if (!fimc_m2m_pending(fimc))
+		return 0;
+
+	set_bit(ST_M2M_SHUT, &fimc->state);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_M2M_SHUT, &fimc->state),
+			   FIMC_SHUTDOWN_TIMEOUT);
+
+	return 0;
+}
+
 static void fimc_capture_handler(struct fimc_dev *fimc)
 {
 	struct fimc_vid_cap *cap = &fimc->vid_cap;
@@ -283,7 +331,7 @@ static void fimc_capture_handler(struct fimc_dev *fimc)
 
 	if (!list_empty(&cap->active_buf_q)) {
 		v_buf = active_queue_pop(cap);
-		fimc_buf_finish(fimc, v_buf);
+		vb2_buffer_done(&v_buf->vb, VB2_BUF_STATE_DONE);
 	}
 
 	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
@@ -300,10 +348,6 @@ static void fimc_capture_handler(struct fimc_dev *fimc)
 		dbg("hw ptr: %d, sw ptr: %d",
 		    fimc_hw_get_frame_index(fimc), cap->buf_index);
 
-		spin_lock(&fimc->irqlock);
-		v_buf->vb.state = VIDEOBUF_ACTIVE;
-		spin_unlock(&fimc->irqlock);
-
 		/* Move the buffer to the capture active queue */
 		active_queue_add(cap, v_buf);
 
@@ -324,8 +368,6 @@ static void fimc_capture_handler(struct fimc_dev *fimc)
 
 static irqreturn_t fimc_isr(int irq, void *priv)
 {
-	struct fimc_vid_buffer *src_buf, *dst_buf;
-	struct fimc_ctx *ctx;
 	struct fimc_dev *fimc = priv;
 
 	BUG_ON(!fimc);
@@ -333,18 +375,21 @@ static irqreturn_t fimc_isr(int irq, void *priv)
 
 	spin_lock(&fimc->slock);
 
-	if (test_and_clear_bit(ST_M2M_PEND, &fimc->state)) {
-		ctx = v4l2_m2m_get_curr_priv(fimc->m2m.m2m_dev);
+	if (test_and_clear_bit(ST_M2M_SHUT, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto isr_unlock;
+	} else if (test_and_clear_bit(ST_M2M_PEND, &fimc->state)) {
+		struct vb2_buffer *src_vb, *dst_vb;
+		struct fimc_ctx *ctx = v4l2_m2m_get_curr_priv(fimc->m2m.m2m_dev);
+
 		if (!ctx || !ctx->m2m_ctx)
 			goto isr_unlock;
-		src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-		dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
-		if (src_buf && dst_buf) {
-			spin_lock(&fimc->irqlock);
-			src_buf->vb.state = dst_buf->vb.state = VIDEOBUF_DONE;
-			wake_up(&src_buf->vb.done);
-			wake_up(&dst_buf->vb.done);
-			spin_unlock(&fimc->irqlock);
+
+		src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+		dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+		if (src_vb && dst_vb) {
+			v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+			v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 			v4l2_m2m_job_finish(fimc->m2m.m2m_dev, ctx->m2m_ctx);
 		}
 		goto isr_unlock;
@@ -364,25 +409,25 @@ isr_unlock:
 	return IRQ_HANDLED;
 }
 
-/* The color format (planes_cnt, buff_cnt) must be already configured. */
-int fimc_prepare_addr(struct fimc_ctx *ctx, struct fimc_vid_buffer *buf,
+/* The color format (colplanes, memplanes) must be already configured. */
+int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		      struct fimc_frame *frame, struct fimc_addr *paddr)
 {
 	int ret = 0;
 	u32 pix_size;
 
-	if (buf == NULL || frame == NULL)
+	if (vb == NULL || frame == NULL)
 		return -EINVAL;
 
 	pix_size = frame->width * frame->height;
 
-	dbg("buff_cnt= %d, planes_cnt= %d, frame->size= %d, pix_size= %d",
-		frame->fmt->buff_cnt, frame->fmt->planes_cnt,
-		frame->size, pix_size);
+	dbg("memplanes= %d, colplanes= %d, pix_size= %d",
+		frame->fmt->memplanes, frame->fmt->colplanes, pix_size);
 
-	if (frame->fmt->buff_cnt == 1) {
-		paddr->y = videobuf_to_dma_contig(&buf->vb);
-		switch (frame->fmt->planes_cnt) {
+	paddr->y = vb2_dma_contig_plane_paddr(vb, 0);
+
+	if (frame->fmt->memplanes == 1) {
+		switch (frame->fmt->colplanes) {
 		case 1:
 			paddr->cb = 0;
 			paddr->cr = 0;
@@ -405,6 +450,12 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct fimc_vid_buffer *buf,
 		default:
 			return -EINVAL;
 		}
+	} else {
+		if (frame->fmt->memplanes >= 2)
+			paddr->cb = vb2_dma_contig_plane_paddr(vb, 1);
+
+		if (frame->fmt->memplanes == 3)
+			paddr->cr = vb2_dma_contig_plane_paddr(vb, 2);
 	}
 
 	dbg("PHYS_ADDR: y= 0x%X  cb= 0x%X cr= 0x%X ret= %d",
@@ -423,34 +474,34 @@ static void fimc_set_yuv_order(struct fimc_ctx *ctx)
 	/* Set order for 1 plane input formats. */
 	switch (ctx->s_frame.fmt->color) {
 	case S5P_FIMC_YCRYCB422:
-		ctx->in_order_1p = S5P_FIMC_IN_YCRYCB;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CBYCRY;
 		break;
 	case S5P_FIMC_CBYCRY422:
-		ctx->in_order_1p = S5P_FIMC_IN_CBYCRY;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCRYCB;
 		break;
 	case S5P_FIMC_CRYCBY422:
-		ctx->in_order_1p = S5P_FIMC_IN_CRYCBY;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_YCBYCR;
 		break;
 	case S5P_FIMC_YCBYCR422:
 	default:
-		ctx->in_order_1p = S5P_FIMC_IN_YCBYCR;
+		ctx->in_order_1p = S5P_MSCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
 
 	switch (ctx->d_frame.fmt->color) {
 	case S5P_FIMC_YCRYCB422:
-		ctx->out_order_1p = S5P_FIMC_OUT_YCRYCB;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CBYCRY;
 		break;
 	case S5P_FIMC_CBYCRY422:
-		ctx->out_order_1p = S5P_FIMC_OUT_CBYCRY;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCRYCB;
 		break;
 	case S5P_FIMC_CRYCBY422:
-		ctx->out_order_1p = S5P_FIMC_OUT_CRYCBY;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_YCBYCR;
 		break;
 	case S5P_FIMC_YCBYCR422:
 	default:
-		ctx->out_order_1p = S5P_FIMC_OUT_YCBYCR;
+		ctx->out_order_1p = S5P_CIOCTRL_ORDER422_CRYCBY;
 		break;
 	}
 	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
@@ -459,10 +510,14 @@ static void fimc_set_yuv_order(struct fimc_ctx *ctx)
 static void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 {
 	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
+	u32 i, depth = 0;
+
+	for (i = 0; i < f->fmt->colplanes; i++)
+		depth += f->fmt->depth[i];
 
 	f->dma_offset.y_h = f->offs_h;
 	if (!variant->pix_hoff)
-		f->dma_offset.y_h *= (f->fmt->depth >> 3);
+		f->dma_offset.y_h *= (depth >> 3);
 
 	f->dma_offset.y_v = f->offs_v;
 
@@ -473,7 +528,7 @@ static void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	f->dma_offset.cr_v = f->offs_v;
 
 	if (!variant->pix_hoff) {
-		if (f->fmt->planes_cnt == 3) {
+		if (f->fmt->colplanes == 3) {
 			f->dma_offset.cb_h >>= 1;
 			f->dma_offset.cr_h >>= 1;
 		}
@@ -499,7 +554,7 @@ static void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags)
 {
 	struct fimc_frame *s_frame, *d_frame;
-	struct fimc_vid_buffer *buf = NULL;
+	struct vb2_buffer *vb = NULL;
 	int ret = 0;
 
 	s_frame = &ctx->s_frame;
@@ -522,15 +577,15 @@ int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags)
 	ctx->scaler.enabled = 1;
 
 	if (flags & FIMC_SRC_ADDR) {
-		buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, buf, s_frame, &s_frame->paddr);
+		vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+		ret = fimc_prepare_addr(ctx, vb, s_frame, &s_frame->paddr);
 		if (ret)
 			return ret;
 	}
 
 	if (flags & FIMC_DST_ADDR) {
-		buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
-		ret = fimc_prepare_addr(ctx, buf, d_frame, &d_frame->paddr);
+		vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+		ret = fimc_prepare_addr(ctx, vb, d_frame, &d_frame->paddr);
 	}
 
 	return ret;
@@ -572,7 +627,9 @@ static void fimc_dma_run(void *priv)
 			err("Scaler setup error");
 			goto dma_unlock;
 		}
-		fimc_hw_set_scaler(ctx);
+
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
 		fimc_hw_set_target_format(ctx);
 		fimc_hw_set_rotation(ctx);
 		fimc_hw_set_effect(ctx);
@@ -587,7 +644,8 @@ static void fimc_dma_run(void *priv)
 
 	fimc_activate_capture(ctx);
 
-	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP);
+	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP |
+		       FIMC_SRC_FMT | FIMC_DST_FMT);
 	fimc_hw_activate_input_dma(fimc, true);
 
 dma_unlock:
@@ -596,109 +654,94 @@ dma_unlock:
 
 static void fimc_job_abort(void *priv)
 {
-	/* Nothing done in job_abort. */
-}
+	struct fimc_ctx *ctx = priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
 
-static void fimc_buf_release(struct videobuf_queue *vq,
-				    struct videobuf_buffer *vb)
-{
-	videobuf_dma_contig_free(vq, vb);
-	vb->state = VIDEOBUF_NEEDS_INIT;
+	if (!fimc_m2m_pending(fimc))
+		return;
+
+	set_bit(ST_M2M_SHUT, &fimc->state);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_M2M_SHUT, &fimc->state),
+			   FIMC_SHUTDOWN_TIMEOUT);
 }
 
-static int fimc_buf_setup(struct videobuf_queue *vq, unsigned int *count,
-				unsigned int *size)
+static int fimc_queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
+			    unsigned int *num_planes, unsigned long sizes[],
+			    void *allocators[])
 {
-	struct fimc_ctx *ctx = vq->priv_data;
-	struct fimc_frame *frame;
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	struct fimc_frame *f;
+	int i;
 
-	frame = ctx_get_frame(ctx, vq->type);
-	if (IS_ERR(frame))
-		return PTR_ERR(frame);
+	f = ctx_get_frame(ctx, vq->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+
+	/*
+	 * Return number of non-contigous planes (plane buffers)
+	 * depending on the configured color format.
+	 */
+	if (f->fmt)
+		*num_planes = f->fmt->memplanes;
+
+	for (i = 0; i < f->fmt->memplanes; i++) {
+		sizes[i] = (f->width * f->height * f->fmt->depth[i]) >> 3;
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+
+	if (*num_buffers == 0)
+		*num_buffers = 1;
 
-	*size = (frame->width * frame->height * frame->fmt->depth) >> 3;
-	if (0 == *count)
-		*count = 1;
 	return 0;
 }
 
-static int fimc_buf_prepare(struct videobuf_queue *vq,
-		struct videobuf_buffer *vb, enum v4l2_field field)
+static int fimc_buf_prepare(struct vb2_buffer *vb)
 {
-	struct fimc_ctx *ctx = vq->priv_data;
-	struct v4l2_device *v4l2_dev = &ctx->fimc_dev->m2m.v4l2_dev;
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 	struct fimc_frame *frame;
-	int ret;
+	int i;
 
-	frame = ctx_get_frame(ctx, vq->type);
+	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (vb->baddr) {
-		if (vb->bsize < frame->size) {
-			v4l2_err(v4l2_dev,
-				"User-provided buffer too small (%d < %d)\n",
-				 vb->bsize, frame->size);
-			WARN_ON(1);
-			return -EINVAL;
-		}
-	} else if (vb->state != VIDEOBUF_NEEDS_INIT
-		   && vb->bsize < frame->size) {
-		return -EINVAL;
-	}
-
-	vb->width       = frame->width;
-	vb->height      = frame->height;
-	vb->bytesperline = (frame->width * frame->fmt->depth) >> 3;
-	vb->size        = frame->size;
-	vb->field       = field;
-
-	if (VIDEOBUF_NEEDS_INIT == vb->state) {
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret) {
-			v4l2_err(v4l2_dev, "Iolock failed\n");
-			fimc_buf_release(vq, vb);
-			return ret;
-		}
-	}
-	vb->state = VIDEOBUF_PREPARED;
+	for (i = 0; i < frame->fmt->memplanes; i++)
+		vb2_set_plane_payload(vb, i, frame->payload[i]);
 
 	return 0;
 }
 
-static void fimc_buf_queue(struct videobuf_queue *vq,
-				  struct videobuf_buffer *vb)
+static void fimc_buf_queue(struct vb2_buffer *vb)
 {
-	struct fimc_ctx *ctx = vq->priv_data;
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct fimc_vid_cap *cap = &fimc->vid_cap;
-	unsigned long flags;
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 
 	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
 
-	if ((ctx->state & FIMC_CTX_M2M) && ctx->m2m_ctx) {
-		v4l2_m2m_buf_queue(ctx->m2m_ctx, vq, vb);
-	} else if (ctx->state & FIMC_CTX_CAP) {
-		spin_lock_irqsave(&fimc->slock, flags);
-		fimc_vid_cap_buf_queue(fimc, (struct fimc_vid_buffer *)vb);
+	if (ctx->m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+}
 
-		dbg("fimc->cap.active_buf_cnt: %d",
-		    fimc->vid_cap.active_buf_cnt);
+static void fimc_lock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_lock(&ctx->fimc_dev->lock);
+}
 
-		if (cap->active_buf_cnt >= cap->reqbufs_count ||
-		   cap->active_buf_cnt >= FIMC_MAX_OUT_BUFS) {
-			if (!test_and_set_bit(ST_CAPT_STREAM, &fimc->state))
-				fimc_activate_capture(ctx);
-		}
-		spin_unlock_irqrestore(&fimc->slock, flags);
-	}
+static void fimc_unlock(struct vb2_queue *vq)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	mutex_unlock(&ctx->fimc_dev->lock);
 }
 
-struct videobuf_queue_ops fimc_qops = {
-	.buf_setup	= fimc_buf_setup,
-	.buf_prepare	= fimc_buf_prepare,
-	.buf_queue	= fimc_buf_queue,
-	.buf_release	= fimc_buf_release,
+struct vb2_ops fimc_qops = {
+	.queue_setup	 = fimc_queue_setup,
+	.buf_prepare	 = fimc_buf_prepare,
+	.buf_queue	 = fimc_buf_queue,
+	.wait_prepare	 = fimc_unlock,
+	.wait_finish	 = fimc_lock,
+	.stop_streaming	 = stop_streaming,
 };
 
 static int fimc_m2m_querycap(struct file *file, void *priv,
@@ -712,12 +755,13 @@ static int fimc_m2m_querycap(struct file *file, void *priv,
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
 	cap->capabilities = V4L2_CAP_STREAMING |
-		V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT;
+		V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
+		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
 	return 0;
 }
 
-int fimc_vidioc_enum_fmt(struct file *file, void *priv,
+int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
 				struct v4l2_fmtdesc *f)
 {
 	struct fimc_fmt *fmt;
@@ -732,25 +776,21 @@ int fimc_vidioc_enum_fmt(struct file *file, void *priv,
 	return 0;
 }
 
-int fimc_vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+int fimc_vidioc_g_fmt_mplane(struct file *file, void *priv,
+			     struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct fimc_frame *frame;
 
 	frame = ctx_get_frame(ctx, f->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	f->fmt.pix.width	= frame->width;
 	f->fmt.pix.height	= frame->height;
 	f->fmt.pix.field	= V4L2_FIELD_NONE;
 	f->fmt.pix.pixelformat	= frame->fmt->fourcc;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -785,42 +825,40 @@ struct fimc_fmt *find_mbus_format(struct v4l2_mbus_framefmt *f,
 }
 
 
-int fimc_vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+int fimc_vidioc_try_fmt_mplane(struct file *file, void *priv,
+			       struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct samsung_fimc_variant *variant = fimc->variant;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
 	struct fimc_fmt *fmt;
 	u32 max_width, mod_x, mod_y, mask;
-	int ret = -EINVAL, is_output = 0;
+	int i, is_output = 0;
 
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		if (ctx->state & FIMC_CTX_CAP)
 			return -EINVAL;
 		is_output = 1;
-	} else if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	} else if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		return -EINVAL;
 	}
 
-	dbg("w: %d, h: %d, bpl: %d",
-	    pix->width, pix->height, pix->bytesperline);
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
+	dbg("w: %d, h: %d", pix->width, pix->height);
 
 	mask = is_output ? FMT_FLAGS_M2M : FMT_FLAGS_M2M | FMT_FLAGS_CAM;
 	fmt = find_format(f, mask);
 	if (!fmt) {
 		v4l2_err(&fimc->m2m.v4l2_dev, "Fourcc format (0x%X) invalid.\n",
 			 pix->pixelformat);
-		goto tf_out;
+		return -EINVAL;
 	}
 
 	if (pix->field == V4L2_FIELD_ANY)
 		pix->field = V4L2_FIELD_NONE;
 	else if (V4L2_FIELD_NONE != pix->field)
-		goto tf_out;
+		return -EINVAL;
 
 	if (is_output) {
 		max_width = variant->pix_limit->scaler_dis_w;
@@ -834,7 +872,7 @@ int fimc_vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		mod_x = 6; /* 64 x 32 pixels tile */
 		mod_y = 5;
 	} else {
-		if (fimc->id == 1 && fimc->variant->pix_hoff)
+		if (fimc->id == 1 && variant->pix_hoff)
 			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
 		else
 			mod_y = mod_x;
@@ -845,74 +883,73 @@ int fimc_vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	v4l_bound_align_image(&pix->width, 16, max_width, mod_x,
 		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
 
-	if (pix->bytesperline == 0 ||
-	    (pix->bytesperline * 8 / fmt->depth) > pix->width)
-		pix->bytesperline = (pix->width * fmt->depth) >> 3;
+	pix->num_planes = fmt->memplanes;
 
-	if (pix->sizeimage == 0)
-		pix->sizeimage = pix->height * pix->bytesperline;
+	for (i = 0; i < pix->num_planes; ++i) {
+		int bpl = pix->plane_fmt[i].bytesperline;
 
-	dbg("w: %d, h: %d, bpl: %d, depth: %d",
-	    pix->width, pix->height, pix->bytesperline, fmt->depth);
+		dbg("[%d] bpl: %d, depth: %d, w: %d, h: %d",
+		    i, bpl, fmt->depth[i], pix->width, pix->height);
 
-	ret = 0;
+		if (!bpl || (bpl * 8 / fmt->depth[i]) > pix->width)
+			bpl = (pix->width * fmt->depth[0]) >> 3;
 
-tf_out:
-	mutex_unlock(&fimc->lock);
-	return ret;
+		if (!pix->plane_fmt[i].sizeimage)
+			pix->plane_fmt[i].sizeimage = pix->height * bpl;
+
+		pix->plane_fmt[i].bytesperline = bpl;
+
+		dbg("[%d]: bpl: %d, sizeimage: %d",
+		    i, pix->plane_fmt[i].bytesperline,
+		    pix->plane_fmt[i].sizeimage);
+	}
+
+	return 0;
 }
 
-static int fimc_m2m_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+static int fimc_m2m_s_fmt_mplane(struct file *file, void *priv,
+				 struct v4l2_format *f)
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	struct v4l2_device *v4l2_dev = &fimc->m2m.v4l2_dev;
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 	struct fimc_frame *frame;
-	struct v4l2_pix_format *pix;
+	struct v4l2_pix_format_mplane *pix;
 	unsigned long flags;
-	int ret = 0;
+	int i, ret = 0;
+	u32 tmp;
 
-	ret = fimc_vidioc_try_fmt(file, priv, f);
+	ret = fimc_vidioc_try_fmt_mplane(file, priv, f);
 	if (ret)
 		return ret;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
-	mutex_lock(&vq->vb_lock);
 
-	if (videobuf_queue_is_busy(vq)) {
-		v4l2_err(v4l2_dev, "%s: queue (%d) busy\n", __func__, f->type);
-		ret = -EBUSY;
-		goto sf_out;
+	if (vb2_is_streaming(vq)) {
+		v4l2_err(&fimc->m2m.v4l2_dev, "queue (%d) busy\n", f->type);
+		return -EBUSY;
 	}
 
-	spin_lock_irqsave(&ctx->slock, flags);
-	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		frame = &ctx->s_frame;
-		ctx->state |= FIMC_SRC_FMT;
-	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		frame = &ctx->d_frame;
-		ctx->state |= FIMC_DST_FMT;
 	} else {
-		spin_unlock_irqrestore(&ctx->slock, flags);
-		v4l2_err(&ctx->fimc_dev->m2m.v4l2_dev,
+		v4l2_err(&fimc->m2m.v4l2_dev,
 			 "Wrong buffer/video queue type (%d)\n", f->type);
-		ret = -EINVAL;
-		goto sf_out;
+		return -EINVAL;
 	}
-	spin_unlock_irqrestore(&ctx->slock, flags);
 
-	pix = &f->fmt.pix;
+	pix = &f->fmt.pix_mp;
 	frame->fmt = find_format(f, FMT_FLAGS_M2M);
-	if (!frame->fmt) {
-		ret = -EINVAL;
-		goto sf_out;
-	}
+	if (!frame->fmt)
+		return -EINVAL;
+
+	for (i = 0; i < frame->fmt->colplanes; i++)
+		frame->payload[i] = pix->plane_fmt[i].bytesperline * pix->height;
 
-	frame->f_width	= pix->bytesperline * 8 / frame->fmt->depth;
+	frame->f_width	= pix->plane_fmt[0].bytesperline * 8 /
+		frame->fmt->depth[0];
 	frame->f_height	= pix->height;
 	frame->width	= pix->width;
 	frame->height	= pix->height;
@@ -920,19 +957,15 @@ static int fimc_m2m_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	frame->o_height = pix->height;
 	frame->offs_h	= 0;
 	frame->offs_v	= 0;
-	frame->size	= (pix->width * pix->height * frame->fmt->depth) >> 3;
-	vq->field	= pix->field;
 
 	spin_lock_irqsave(&ctx->slock, flags);
-	ctx->state |= FIMC_PARAMS;
+	tmp = (frame == &ctx->d_frame) ? FIMC_DST_FMT : FIMC_SRC_FMT;
+	ctx->state |= FIMC_PARAMS | tmp;
 	spin_unlock_irqrestore(&ctx->slock, flags);
 
 	dbg("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
 
-sf_out:
-	mutex_unlock(&vq->vb_lock);
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return 0;
 }
 
 static int fimc_m2m_reqbufs(struct file *file, void *priv,
@@ -968,6 +1001,15 @@ static int fimc_m2m_streamon(struct file *file, void *priv,
 			   enum v4l2_buf_type type)
 {
 	struct fimc_ctx *ctx = priv;
+
+	/* The source and target color format need to be set */
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		if (~ctx->state & FIMC_SRC_FMT)
+			return -EINVAL;
+	} else if (~ctx->state & FIMC_DST_FMT) {
+		return -EINVAL;
+	}
+
 	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 }
 
@@ -992,11 +1034,8 @@ int fimc_vidioc_queryctrl(struct file *file, void *priv,
 	}
 
 	if (ctx->state & FIMC_CTX_CAP) {
-		if (mutex_lock_interruptible(&ctx->fimc_dev->lock))
-			return -ERESTARTSYS;
-		ret = v4l2_subdev_call(ctx->fimc_dev->vid_cap.sd,
+		return v4l2_subdev_call(ctx->fimc_dev->vid_cap.sd,
 					core, queryctrl, qc);
-		mutex_unlock(&ctx->fimc_dev->lock);
 	}
 	return ret;
 }
@@ -1006,10 +1045,6 @@ int fimc_vidioc_g_ctrl(struct file *file, void *priv,
 {
 	struct fimc_ctx *ctx = priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret = 0;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	switch (ctrl->id) {
 	case V4L2_CID_HFLIP:
@@ -1023,18 +1058,17 @@ int fimc_vidioc_g_ctrl(struct file *file, void *priv,
 		break;
 	default:
 		if (ctx->state & FIMC_CTX_CAP) {
-			ret = v4l2_subdev_call(fimc->vid_cap.sd, core,
-				       g_ctrl, ctrl);
+			return v4l2_subdev_call(fimc->vid_cap.sd, core,
+						g_ctrl, ctrl);
 		} else {
 			v4l2_err(&fimc->m2m.v4l2_dev,
 				 "Invalid control\n");
-			ret = -EINVAL;
+			return -EINVAL;
 		}
 	}
 	dbg("ctrl->value= %d", ctrl->value);
 
-	mutex_unlock(&fimc->lock);
-	return ret;
+	return 0;
 }
 
 int check_ctrl_val(struct fimc_ctx *ctx,  struct v4l2_control *ctrl)
@@ -1059,13 +1093,7 @@ int fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_control *ctrl)
 	struct samsung_fimc_variant *variant = ctx->fimc_dev->variant;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	unsigned long flags;
-
-	if (ctx->rotation != 0 &&
-	    (ctrl->id == V4L2_CID_HFLIP || ctrl->id == V4L2_CID_VFLIP)) {
-		v4l2_err(&fimc->m2m.v4l2_dev,
-			 "Simultaneous flip and rotation is not supported\n");
-		return -EINVAL;
-	}
+	int ret = 0;
 
 	spin_lock_irqsave(&ctx->slock, flags);
 
@@ -1085,6 +1113,20 @@ int fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_control *ctrl)
 		break;
 
 	case V4L2_CID_ROTATE:
+		if (!(~ctx->state & (FIMC_DST_FMT | FIMC_SRC_FMT))) {
+			ret = fimc_check_scaler_ratio(ctx->s_frame.width,
+						      ctx->s_frame.height,
+						      ctx->d_frame.width,
+						      ctx->d_frame.height,
+						      ctrl->value);
+			if (ret) {
+				v4l2_err(&fimc->m2m.v4l2_dev,
+					 "Out of scaler range");
+				spin_unlock_irqrestore(&ctx->slock, flags);
+				return -EINVAL;
+			}
+		}
+
 		/* Check for the output rotator availability */
 		if ((ctrl->value == 90 || ctrl->value == 270) &&
 		    (ctx->in_path == FIMC_DMA && !variant->has_out_rot)) {
@@ -1107,7 +1149,7 @@ int fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_control *ctrl)
 }
 
 static int fimc_m2m_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
+			   struct v4l2_control *ctrl)
 {
 	struct fimc_ctx *ctx = priv;
 	int ret = 0;
@@ -1125,22 +1167,17 @@ static int fimc_m2m_cropcap(struct file *file, void *fh,
 {
 	struct fimc_frame *frame;
 	struct fimc_ctx *ctx = fh;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 
 	frame = ctx_get_frame(ctx, cr->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	cr->bounds.left		= 0;
 	cr->bounds.top		= 0;
 	cr->bounds.width	= frame->f_width;
 	cr->bounds.height	= frame->f_height;
 	cr->defrect		= cr->bounds;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -1148,21 +1185,16 @@ static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 {
 	struct fimc_frame *frame;
 	struct fimc_ctx *ctx = file->private_data;
-	struct fimc_dev *fimc = ctx->fimc_dev;
 
 	frame = ctx_get_frame(ctx, cr->type);
 	if (IS_ERR(frame))
 		return PTR_ERR(frame);
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	cr->c.left = frame->offs_h;
 	cr->c.top = frame->offs_v;
 	cr->c.width = frame->width;
 	cr->c.height = frame->height;
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -1170,7 +1202,8 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 {
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct fimc_frame *f;
-	u32 min_size, halign;
+	u32 min_size, halign, depth = 0;
+	int i;
 
 	if (cr->c.top < 0 || cr->c.left < 0) {
 		v4l2_err(&fimc->m2m.v4l2_dev,
@@ -1178,9 +1211,9 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 		return -EINVAL;
 	}
 
-	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		f = (ctx->state & FIMC_CTX_CAP) ? &ctx->s_frame : &ctx->d_frame;
-	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
 		 ctx->state & FIMC_CTX_M2M)
 		f = &ctx->s_frame;
 	else
@@ -1200,10 +1233,13 @@ int fimc_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 		halign = 4;
 	}
 
+	for (i = 0; i < f->fmt->colplanes; i++)
+		depth += f->fmt->depth[i];
+
 	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
 			      ffs(min_size) - 1,
 			      &cr->c.height, min_size, f->o_height,
-			      halign, 64/(ALIGN(f->fmt->depth, 8)));
+			      halign, 64/(ALIGN(depth, 8)));
 
 	/* adjust left/top if cropping rectangle is out of bounds */
 	if (cr->c.left + cr->c.width > f->o_width)
@@ -1235,25 +1271,31 @@ static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 	if (ret)
 		return ret;
 
-	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
 		&ctx->s_frame : &ctx->d_frame;
 
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
 	spin_lock_irqsave(&ctx->slock, flags);
-	if (~ctx->state & (FIMC_SRC_FMT | FIMC_DST_FMT)) {
-		/* Check to see if scaling ratio is within supported range */
-		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
-			ret = fimc_check_scaler_ratio(&cr->c, &ctx->d_frame);
-		else
-			ret = fimc_check_scaler_ratio(&cr->c, &ctx->s_frame);
+	/* Check to see if scaling ratio is within supported range */
+	if (!(~ctx->state & (FIMC_DST_FMT | FIMC_SRC_FMT))) {
+		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+			ret = fimc_check_scaler_ratio(cr->c.width, cr->c.height,
+						      ctx->d_frame.width,
+						      ctx->d_frame.height,
+						      ctx->rotation);
+		} else {
+			ret = fimc_check_scaler_ratio(ctx->s_frame.width,
+						      ctx->s_frame.height,
+						      cr->c.width, cr->c.height,
+						      ctx->rotation);
+		}
+
 		if (ret) {
 			v4l2_err(&fimc->m2m.v4l2_dev, "Out of scaler range");
-			ret = -EINVAL;
-			goto scr_unlock;
+			spin_unlock_irqrestore(&ctx->slock, flags);
+			return -EINVAL;
 		}
 	}
+
 	ctx->state |= FIMC_PARAMS;
 
 	f->offs_h = cr->c.left;
@@ -1261,26 +1303,24 @@ static int fimc_m2m_s_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 	f->width  = cr->c.width;
 	f->height = cr->c.height;
 
-scr_unlock:
 	spin_unlock_irqrestore(&ctx->slock, flags);
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
 static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 	.vidioc_querycap		= fimc_m2m_querycap,
 
-	.vidioc_enum_fmt_vid_cap	= fimc_vidioc_enum_fmt,
-	.vidioc_enum_fmt_vid_out	= fimc_vidioc_enum_fmt,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_vidioc_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out_mplane	= fimc_vidioc_enum_fmt_mplane,
 
-	.vidioc_g_fmt_vid_cap		= fimc_vidioc_g_fmt,
-	.vidioc_g_fmt_vid_out		= fimc_vidioc_g_fmt,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_vidioc_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= fimc_vidioc_g_fmt_mplane,
 
-	.vidioc_try_fmt_vid_cap		= fimc_vidioc_try_fmt,
-	.vidioc_try_fmt_vid_out		= fimc_vidioc_try_fmt,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_vidioc_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= fimc_vidioc_try_fmt_mplane,
 
-	.vidioc_s_fmt_vid_cap		= fimc_m2m_s_fmt,
-	.vidioc_s_fmt_vid_out		= fimc_m2m_s_fmt,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
 
 	.vidioc_reqbufs			= fimc_m2m_reqbufs,
 	.vidioc_querybuf		= fimc_m2m_querybuf,
@@ -1301,26 +1341,39 @@ static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 
 };
 
-static void queue_init(void *priv, struct videobuf_queue *vq,
-		       enum v4l2_buf_type type)
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
 {
 	struct fimc_ctx *ctx = priv;
-	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &fimc_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 
-	videobuf_queue_dma_contig_init(vq, &fimc_qops,
-		&fimc->pdev->dev,
-		&fimc->irqlock, type, V4L2_FIELD_NONE,
-		sizeof(struct fimc_vid_buffer), priv, NULL);
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &fimc_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+
+	return vb2_queue_init(dst_vq);
 }
 
 static int fimc_m2m_open(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 	struct fimc_ctx *ctx = NULL;
-	int err = 0;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
 
 	dbg("pid: %d, state: 0x%lx, refcnt: %d",
 		task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
@@ -1329,19 +1382,15 @@ static int fimc_m2m_open(struct file *file)
 	 * Return if the corresponding video capture node
 	 * is already opened.
 	 */
-	if (fimc->vid_cap.refcnt > 0) {
-		err = -EBUSY;
-		goto err_unlock;
-	}
+	if (fimc->vid_cap.refcnt > 0)
+		return -EBUSY;
 
 	fimc->m2m.refcnt++;
 	set_bit(ST_OUTDMA_RUN, &fimc->state);
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
-	if (!ctx) {
-		err = -ENOMEM;
-		goto err_unlock;
-	}
+	if (!ctx)
+		return -ENOMEM;
 
 	file->private_data = ctx;
 	ctx->fimc_dev = fimc;
@@ -1355,15 +1404,14 @@ static int fimc_m2m_open(struct file *file)
 	ctx->out_path = FIMC_DMA;
 	spin_lock_init(&ctx->slock);
 
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(ctx, fimc->m2m.m2m_dev, queue_init);
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
 	if (IS_ERR(ctx->m2m_ctx)) {
-		err = PTR_ERR(ctx->m2m_ctx);
+		int err = PTR_ERR(ctx->m2m_ctx);
 		kfree(ctx);
+		return err;
 	}
 
-err_unlock:
-	mutex_unlock(&fimc->lock);
-	return err;
+	return 0;
 }
 
 static int fimc_m2m_release(struct file *file)
@@ -1371,8 +1419,6 @@ static int fimc_m2m_release(struct file *file)
 	struct fimc_ctx *ctx = file->private_data;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 
-	mutex_lock(&fimc->lock);
-
 	dbg("pid: %d, state: 0x%lx, refcnt= %d",
 		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
 
@@ -1381,7 +1427,6 @@ static int fimc_m2m_release(struct file *file)
 	if (--fimc->m2m.refcnt <= 0)
 		clear_bit(ST_OUTDMA_RUN, &fimc->state);
 
-	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -1415,7 +1460,6 @@ static struct v4l2_m2m_ops m2m_ops = {
 	.job_abort	= fimc_job_abort,
 };
 
-
 static int fimc_register_m2m_device(struct fimc_dev *fimc)
 {
 	struct video_device *vfd;
@@ -1448,6 +1492,7 @@ static int fimc_register_m2m_device(struct fimc_dev *fimc)
 	vfd->ioctl_ops	= &fimc_m2m_ioctl_ops;
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
+	vfd->lock	= &fimc->lock;
 
 	snprintf(vfd->name, sizeof(vfd->name), "%s:m2m", dev_name(&pdev->dev));
 
@@ -1496,7 +1541,7 @@ static void fimc_unregister_m2m_device(struct fimc_dev *fimc)
 static void fimc_clk_release(struct fimc_dev *fimc)
 {
 	int i;
-	for (i = 0; i < NUM_FIMC_CLOCKS; i++) {
+	for (i = 0; i < fimc->num_clocks; i++) {
 		if (fimc->clock[i]) {
 			clk_disable(fimc->clock[i]);
 			clk_put(fimc->clock[i]);
@@ -1507,15 +1552,16 @@ static void fimc_clk_release(struct fimc_dev *fimc)
 static int fimc_clk_get(struct fimc_dev *fimc)
 {
 	int i;
-	for (i = 0; i < NUM_FIMC_CLOCKS; i++) {
-		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clock_name[i]);
-		if (IS_ERR(fimc->clock[i])) {
-			dev_err(&fimc->pdev->dev,
-				"failed to get fimc clock: %s\n",
-				fimc_clock_name[i]);
-			return -ENXIO;
+	for (i = 0; i < fimc->num_clocks; i++) {
+		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clocks[i]);
+
+		if (!IS_ERR_OR_NULL(fimc->clock[i])) {
+			clk_enable(fimc->clock[i]);
+			continue;
 		}
-		clk_enable(fimc->clock[i]);
+		dev_err(&fimc->pdev->dev, "failed to get fimc clock: %s\n",
+			fimc_clocks[i]);
+		return -ENXIO;
 	}
 	return 0;
 }
@@ -1526,6 +1572,7 @@ static int fimc_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct samsung_fimc_driverdata *drv_data;
 	int ret = 0;
+	int cap_input_index = -1;
 
 	dev_dbg(&pdev->dev, "%s():\n", __func__);
 
@@ -1548,7 +1595,6 @@ static int fimc_probe(struct platform_device *pdev)
 	fimc->pdata = pdev->dev.platform_data;
 	fimc->state = ST_IDLE;
 
-	spin_lock_init(&fimc->irqlock);
 	init_waitqueue_head(&fimc->irq_queue);
 	spin_lock_init(&fimc->slock);
 
@@ -1576,10 +1622,26 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_req_region;
 	}
 
+	fimc->num_clocks = MAX_FIMC_CLOCKS - 1;
+	/*
+	 * Check if vide capture node needs to be registered for this device
+	 * instance.
+	 */
+	if (fimc->pdata) {
+		int i;
+		for (i = 0; i < FIMC_MAX_CAMIF_CLIENTS; ++i)
+			if (fimc->pdata->isp_info[i])
+				break;
+		if (i < FIMC_MAX_CAMIF_CLIENTS) {
+			cap_input_index = i;
+			fimc->num_clocks++;
+		}
+	}
+
 	ret = fimc_clk_get(fimc);
 	if (ret)
 		goto err_regs_unmap;
-	clk_set_rate(fimc->clock[0], drv_data->lclk_frequency);
+	clk_set_rate(fimc->clock[CLK_BUS], drv_data->lclk_frequency);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
@@ -1597,24 +1659,24 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
+	/* Initialize contiguous memory allocator */
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(&fimc->pdev->dev);
+	if (IS_ERR(fimc->alloc_ctx)) {
+		ret = PTR_ERR(fimc->alloc_ctx);
+		goto err_irq;
+	}
+
 	ret = fimc_register_m2m_device(fimc);
 	if (ret)
 		goto err_irq;
 
 	/* At least one camera sensor is required to register capture node */
-	if (fimc->pdata) {
-		int i;
-		for (i = 0; i < FIMC_MAX_CAMIF_CLIENTS; ++i)
-			if (fimc->pdata->isp_info[i])
-				break;
-
-		if (i < FIMC_MAX_CAMIF_CLIENTS) {
-			ret = fimc_register_capture_device(fimc);
-			if (ret)
-				goto err_m2m;
-		}
+	if (cap_input_index >= 0) {
+		ret = fimc_register_capture_device(fimc);
+		if (ret)
+			goto err_m2m;
+		clk_disable(fimc->clock[CLK_CAM]);
 	}
-
 	/*
 	 * Exclude the additional output DMA address registers by masking
 	 * them out on HW revisions that provide extended capabilites.
@@ -1656,6 +1718,9 @@ static int __devexit fimc_remove(struct platform_device *pdev)
 	fimc_unregister_capture_device(fimc);
 
 	fimc_clk_release(fimc);
+
+	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
+
 	iounmap(fimc->regs);
 	release_resource(fimc->regs_res);
 	kfree(fimc->regs_res);
@@ -1726,6 +1791,7 @@ static struct samsung_fimc_variant fimc1_variant_s5pv210 = {
 	.pix_hoff	 = 1,
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
+	.has_mainscaler_ext = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
@@ -1747,6 +1813,7 @@ static struct samsung_fimc_variant fimc0_variant_s5pv310 = {
 	.has_inp_rot	 = 1,
 	.has_out_rot	 = 1,
 	.has_cistatus2	 = 1,
+	.has_mainscaler_ext = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
@@ -1757,6 +1824,7 @@ static struct samsung_fimc_variant fimc0_variant_s5pv310 = {
 static struct samsung_fimc_variant fimc2_variant_s5pv310 = {
 	.pix_hoff	 = 1,
 	.has_cistatus2	 = 1,
+	.has_mainscaler_ext = 1,
 	.min_inp_pixsize = 16,
 	.min_out_pixsize = 16,
 	.hor_offs_align	 = 1,
diff --git a/drivers/media/video/s5p-fimc/fimc-core.h b/drivers/media/video/s5p-fimc/fimc-core.h
index 4f047d35f8ad..4829a2515076 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.h
+++ b/drivers/media/video/s5p-fimc/fimc-core.h
@@ -16,11 +16,12 @@
 #include <linux/sched.h>
 #include <linux/types.h>
 #include <linux/videodev2.h>
-#include <media/videobuf-core.h>
+#include <linux/io.h>
+#include <media/videobuf2-core.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s3c_fimc.h>
+#include <media/s5p_fimc.h>
 
 #include "regs-fimc.h"
 
@@ -36,7 +37,7 @@
 
 /* Time to wait for next frame VSYNC interrupt while stopping operation. */
 #define FIMC_SHUTDOWN_TIMEOUT	((100*HZ)/1000)
-#define NUM_FIMC_CLOCKS		2
+#define MAX_FIMC_CLOCKS		3
 #define MODULE_NAME		"s5p-fimc"
 #define FIMC_MAX_DEVS		4
 #define FIMC_MAX_OUT_BUFS	4
@@ -44,12 +45,19 @@
 #define SCALER_MAX_VRATIO	64
 #define DMA_MIN_SIZE		8
 
-/* FIMC device state flags */
+/* indices to the clocks array */
+enum {
+	CLK_BUS,
+	CLK_GATE,
+	CLK_CAM,
+};
+
 enum fimc_dev_flags {
 	/* for m2m node */
 	ST_IDLE,
 	ST_OUTDMA_RUN,
 	ST_M2M_PEND,
+	ST_M2M_SHUT,
 	/* for capture node */
 	ST_CAPT_PEND,
 	ST_CAPT_RUN,
@@ -70,13 +78,6 @@ enum fimc_dev_flags {
 #define fimc_capture_streaming(dev) \
 	test_bit(ST_CAPT_STREAM, &(dev)->state)
 
-#define fimc_buf_finish(dev, vid_buf) do { \
-	spin_lock(&(dev)->irqlock); \
-	(vid_buf)->vb.state = VIDEOBUF_DONE; \
-	spin_unlock(&(dev)->irqlock); \
-	wake_up(&(vid_buf)->vb.done); \
-} while (0)
-
 enum fimc_datapath {
 	FIMC_CAMERA,
 	FIMC_DMA,
@@ -90,7 +91,6 @@ enum fimc_color_fmt {
 	S5P_FIMC_RGB888,
 	S5P_FIMC_RGB30_LOCAL,
 	S5P_FIMC_YCBCR420 = 0x20,
-	S5P_FIMC_YCBCR422,
 	S5P_FIMC_YCBYCR422,
 	S5P_FIMC_YCRYCB422,
 	S5P_FIMC_CBYCRY422,
@@ -100,18 +100,6 @@ enum fimc_color_fmt {
 
 #define fimc_fmt_is_rgb(x) ((x) & 0x10)
 
-/* Y/Cb/Cr components order at DMA output for 1 plane YCbCr 4:2:2 formats. */
-#define	S5P_FIMC_OUT_CRYCBY	S5P_CIOCTRL_ORDER422_CRYCBY
-#define	S5P_FIMC_OUT_CBYCRY	S5P_CIOCTRL_ORDER422_YCRYCB
-#define	S5P_FIMC_OUT_YCRYCB	S5P_CIOCTRL_ORDER422_CBYCRY
-#define	S5P_FIMC_OUT_YCBYCR	S5P_CIOCTRL_ORDER422_YCBYCR
-
-/* Input Y/Cb/Cr components order for 1 plane YCbCr 4:2:2 color formats. */
-#define	S5P_FIMC_IN_CRYCBY	S5P_MSCTRL_ORDER422_CRYCBY
-#define	S5P_FIMC_IN_CBYCRY	S5P_MSCTRL_ORDER422_YCRYCB
-#define	S5P_FIMC_IN_YCRYCB	S5P_MSCTRL_ORDER422_CBYCRY
-#define	S5P_FIMC_IN_YCBYCR	S5P_MSCTRL_ORDER422_YCBYCR
-
 /* Cb/Cr chrominance components order for 2 plane Y/CbCr 4:2:2 formats. */
 #define	S5P_FIMC_LSB_CRCB	S5P_CIOCTRL_ORDER422_2P_LSB_CRCB
 
@@ -157,18 +145,18 @@ enum fimc_color_fmt {
  * @name: format description
  * @fourcc: the fourcc code for this format, 0 if not applicable
  * @color: the corresponding fimc_color_fmt
- * @depth: driver's private 'number of bits per pixel'
- * @buff_cnt: number of physically non-contiguous data planes
- * @planes_cnt: number of physically contiguous data planes
+ * @depth: per plane driver's private 'number of bits per pixel'
+ * @memplanes: number of physically non-contiguous data planes
+ * @colplanes: number of physically contiguous data planes
  */
 struct fimc_fmt {
 	enum v4l2_mbus_pixelcode mbus_code;
 	char	*name;
 	u32	fourcc;
 	u32	color;
-	u16	buff_cnt;
-	u16	planes_cnt;
-	u16	depth;
+	u16	memplanes;
+	u16	colplanes;
+	u8	depth[VIDEO_MAX_PLANES];
 	u16	flags;
 #define FMT_FLAGS_CAM	(1 << 0)
 #define FMT_FLAGS_M2M	(1 << 1)
@@ -260,7 +248,8 @@ struct fimc_addr {
  * @index: buffer index for the output DMA engine
  */
 struct fimc_vid_buffer {
-	struct videobuf_buffer	vb;
+	struct vb2_buffer	vb;
+	struct list_head	list;
 	struct fimc_addr	paddr;
 	int			index;
 };
@@ -277,7 +266,7 @@ struct fimc_vid_buffer {
  * @height:	image pixel weight
  * @paddr:	image frame buffer physical addresses
  * @buf_cnt:	number of buffers depending on a color format
- * @size:	image size in bytes
+ * @payload:	image size in bytes (w x h x bpp)
  * @color:	color format
  * @dma_offset:	DMA offset in bytes
  */
@@ -290,7 +279,7 @@ struct fimc_frame {
 	u32	offs_v;
 	u32	width;
 	u32	height;
-	u32	size;
+	unsigned long		payload[VIDEO_MAX_PLANES];
 	struct fimc_addr	paddr;
 	struct fimc_dma_offset	dma_offset;
 	struct fimc_fmt		*fmt;
@@ -331,13 +320,14 @@ struct fimc_m2m_device {
  */
 struct fimc_vid_cap {
 	struct fimc_ctx			*ctx;
+	struct vb2_alloc_ctx		*alloc_ctx;
 	struct video_device		*vfd;
 	struct v4l2_device		v4l2_dev;
-	struct v4l2_subdev		*sd;
+	struct v4l2_subdev		*sd;;
 	struct v4l2_mbus_framefmt	fmt;
 	struct list_head		pending_buf_q;
 	struct list_head		active_buf_q;
-	struct videobuf_queue		vbq;
+	struct vb2_queue		vbq;
 	int				active_buf_cnt;
 	int				buf_index;
 	unsigned int			frame_count;
@@ -372,6 +362,8 @@ struct fimc_pix_limit {
  * @has_inp_rot: set if has input rotator
  * @has_out_rot: set if has output rotator
  * @has_cistatus2: 1 if CISTATUS2 register is present in this IP revision
+ * @has_mainscaler_ext: 1 if extended mainscaler ratios in CIEXTEN register
+ *			 are present in this IP revision
  * @pix_limit: pixel size constraints for the scaler
  * @min_inp_pixsize: minimum input pixel size
  * @min_out_pixsize: minimum output pixel size
@@ -383,6 +375,7 @@ struct samsung_fimc_variant {
 	unsigned int	has_inp_rot:1;
 	unsigned int	has_out_rot:1;
 	unsigned int	has_cistatus2:1;
+	unsigned int	has_mainscaler_ext:1;
 	struct fimc_pix_limit *pix_limit;
 	u16		min_inp_pixsize;
 	u16		min_out_pixsize;
@@ -412,12 +405,12 @@ struct fimc_ctx;
  * @lock:	the mutex protecting this data structure
  * @pdev:	pointer to the FIMC platform device
  * @pdata:	pointer to the device platform data
- * @id:		FIMC device index (0..2)
+ * @id:		FIMC device index (0..FIMC_MAX_DEVS)
+ * @num_clocks: the number of clocks managed by this device instance
  * @clock[]:	the clocks required for FIMC operation
  * @regs:	the mapped hardware registers
  * @regs_res:	the resource claimed for IO registers
  * @irq:	interrupt number of the FIMC subdevice
- * @irqlock:	spinlock protecting videobuffer queue
  * @irq_queue:
  * @m2m:	memory-to-memory V4L2 device information
  * @vid_cap:	camera capture device information
@@ -427,18 +420,19 @@ struct fimc_dev {
 	spinlock_t			slock;
 	struct mutex			lock;
 	struct platform_device		*pdev;
-	struct s3c_platform_fimc	*pdata;
+	struct s5p_platform_fimc	*pdata;
 	struct samsung_fimc_variant	*variant;
-	int				id;
-	struct clk			*clock[NUM_FIMC_CLOCKS];
+	u16				id;
+	u16				num_clocks;
+	struct clk			*clock[MAX_FIMC_CLOCKS];
 	void __iomem			*regs;
 	struct resource			*regs_res;
 	int				irq;
-	spinlock_t			irqlock;
 	wait_queue_head_t		irq_queue;
 	struct fimc_m2m_device		m2m;
 	struct fimc_vid_cap		vid_cap;
 	unsigned long			state;
+	struct vb2_alloc_ctx		*alloc_ctx;
 };
 
 /**
@@ -482,11 +476,9 @@ struct fimc_ctx {
 	struct v4l2_m2m_ctx	*m2m_ctx;
 };
 
-extern struct videobuf_queue_ops fimc_qops;
-
 static inline int tiled_fmt(struct fimc_fmt *fmt)
 {
-	return 0;
+	return fmt->fourcc == V4L2_PIX_FMT_NV12MT;
 }
 
 static inline void fimc_hw_clear_irq(struct fimc_dev *dev)
@@ -542,12 +534,12 @@ static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
 {
 	struct fimc_frame *frame;
 
-	if (V4L2_BUF_TYPE_VIDEO_OUTPUT == type) {
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == type) {
 		if (ctx->state & FIMC_CTX_M2M)
 			frame = &ctx->s_frame;
 		else
 			return ERR_PTR(-EINVAL);
-	} else if (V4L2_BUF_TYPE_VIDEO_CAPTURE == type) {
+	} else if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
 		frame = &ctx->d_frame;
 	} else {
 		v4l2_err(&ctx->fimc_dev->m2m.v4l2_dev,
@@ -581,7 +573,8 @@ void fimc_hw_set_target_format(struct fimc_ctx *ctx);
 void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
 void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
 void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
-void fimc_hw_set_scaler(struct fimc_ctx *ctx);
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
 void fimc_hw_en_capture(struct fimc_ctx *ctx);
 void fimc_hw_set_effect(struct fimc_ctx *ctx);
 void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
@@ -589,23 +582,23 @@ void fimc_hw_set_input_path(struct fimc_ctx *ctx);
 void fimc_hw_set_output_path(struct fimc_ctx *ctx);
 void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
 void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
-			      int index);
+			     int index);
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
-			      struct s3c_fimc_isp_info *cam);
+			      struct s5p_fimc_isp_info *cam);
 int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
 int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
-				struct s3c_fimc_isp_info *cam);
+				struct s5p_fimc_isp_info *cam);
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
-			    struct s3c_fimc_isp_info *cam);
+			    struct s5p_fimc_isp_info *cam);
 
 /* -----------------------------------------------------*/
 /* fimc-core.c */
-int fimc_vidioc_enum_fmt(struct file *file, void *priv,
-		      struct v4l2_fmtdesc *f);
-int fimc_vidioc_g_fmt(struct file *file, void *priv,
-		      struct v4l2_format *f);
-int fimc_vidioc_try_fmt(struct file *file, void *priv,
-			struct v4l2_format *f);
+int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f);
+int fimc_vidioc_g_fmt_mplane(struct file *file, void *priv,
+			     struct v4l2_format *f);
+int fimc_vidioc_try_fmt_mplane(struct file *file, void *priv,
+			       struct v4l2_format *f);
 int fimc_vidioc_queryctrl(struct file *file, void *priv,
 			  struct v4l2_queryctrl *qc);
 int fimc_vidioc_g_ctrl(struct file *file, void *priv,
@@ -619,10 +612,10 @@ struct fimc_fmt *find_format(struct v4l2_format *f, unsigned int mask);
 struct fimc_fmt *find_mbus_format(struct v4l2_mbus_framefmt *f,
 				  unsigned int mask);
 
-int fimc_check_scaler_ratio(struct v4l2_rect *r, struct fimc_frame *f);
+int fimc_check_scaler_ratio(int sw, int sh, int dw, int dh, int rot);
 int fimc_set_scaler_info(struct fimc_ctx *ctx);
 int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags);
-int fimc_prepare_addr(struct fimc_ctx *ctx, struct fimc_vid_buffer *buf,
+int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		      struct fimc_frame *frame, struct fimc_addr *paddr);
 
 /* -----------------------------------------------------*/
@@ -649,28 +642,27 @@ static inline void fimc_deactivate_capture(struct fimc_dev *fimc)
 }
 
 /*
- * Add video buffer to the active buffers queue.
- * The caller holds irqlock spinlock.
+ * Add buf to the capture active buffers queue.
+ * Locking: Need to be called with fimc_dev::slock held.
  */
 static inline void active_queue_add(struct fimc_vid_cap *vid_cap,
-					 struct fimc_vid_buffer *buf)
+				    struct fimc_vid_buffer *buf)
 {
-	buf->vb.state = VIDEOBUF_ACTIVE;
-	list_add_tail(&buf->vb.queue, &vid_cap->active_buf_q);
+	list_add_tail(&buf->list, &vid_cap->active_buf_q);
 	vid_cap->active_buf_cnt++;
 }
 
 /*
  * Pop a video buffer from the capture active buffers queue
- * Locking: Need to be called with dev->slock held.
+ * Locking: Need to be called with fimc_dev::slock held.
  */
 static inline struct fimc_vid_buffer *
 active_queue_pop(struct fimc_vid_cap *vid_cap)
 {
 	struct fimc_vid_buffer *buf;
 	buf = list_entry(vid_cap->active_buf_q.next,
-			 struct fimc_vid_buffer, vb.queue);
-	list_del(&buf->vb.queue);
+			 struct fimc_vid_buffer, list);
+	list_del(&buf->list);
 	vid_cap->active_buf_cnt--;
 	return buf;
 }
@@ -679,8 +671,7 @@ active_queue_pop(struct fimc_vid_cap *vid_cap)
 static inline void fimc_pending_queue_add(struct fimc_vid_cap *vid_cap,
 					  struct fimc_vid_buffer *buf)
 {
-	buf->vb.state = VIDEOBUF_QUEUED;
-	list_add_tail(&buf->vb.queue, &vid_cap->pending_buf_q);
+	list_add_tail(&buf->list, &vid_cap->pending_buf_q);
 }
 
 /* Add video buffer to the capture pending buffers queue */
@@ -689,10 +680,9 @@ pending_queue_pop(struct fimc_vid_cap *vid_cap)
 {
 	struct fimc_vid_buffer *buf;
 	buf = list_entry(vid_cap->pending_buf_q.next,
-			struct fimc_vid_buffer, vb.queue);
-	list_del(&buf->vb.queue);
+			struct fimc_vid_buffer, list);
+	list_del(&buf->list);
 	return buf;
 }
 
-
 #endif /* FIMC_CORE_H_ */
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.c b/drivers/media/video/s5p-fimc/fimc-reg.c
index 511631a2e5c3..10684aef5b2d 100644
--- a/drivers/media/video/s5p-fimc/fimc-reg.c
+++ b/drivers/media/video/s5p-fimc/fimc-reg.c
@@ -13,7 +13,7 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <mach/map.h>
-#include <media/s3c_fimc.h>
+#include <media/s5p_fimc.h>
 
 #include "fimc-core.h"
 
@@ -37,11 +37,11 @@ void fimc_hw_reset(struct fimc_dev *dev)
 	writel(cfg, dev->regs + S5P_CIGCTRL);
 }
 
-static u32 fimc_hw_get_in_flip(u32 ctx_flip)
+static u32 fimc_hw_get_in_flip(struct fimc_ctx *ctx)
 {
 	u32 flip = S5P_MSCTRL_FLIP_NORMAL;
 
-	switch (ctx_flip) {
+	switch (ctx->flip) {
 	case FLIP_X_AXIS:
 		flip = S5P_MSCTRL_FLIP_X_MIRROR;
 		break;
@@ -51,16 +51,20 @@ static u32 fimc_hw_get_in_flip(u32 ctx_flip)
 	case FLIP_XY_AXIS:
 		flip = S5P_MSCTRL_FLIP_180;
 		break;
+	default:
+		break;
 	}
+	if (ctx->rotation <= 90)
+		return flip;
 
-	return flip;
+	return (flip ^ S5P_MSCTRL_FLIP_180) & S5P_MSCTRL_FLIP_180;
 }
 
-static u32 fimc_hw_get_target_flip(u32 ctx_flip)
+static u32 fimc_hw_get_target_flip(struct fimc_ctx *ctx)
 {
 	u32 flip = S5P_CITRGFMT_FLIP_NORMAL;
 
-	switch (ctx_flip) {
+	switch (ctx->flip) {
 	case FLIP_X_AXIS:
 		flip = S5P_CITRGFMT_FLIP_X_MIRROR;
 		break;
@@ -70,11 +74,13 @@ static u32 fimc_hw_get_target_flip(u32 ctx_flip)
 	case FLIP_XY_AXIS:
 		flip = S5P_CITRGFMT_FLIP_180;
 		break;
-	case FLIP_NONE:
+	default:
 		break;
-
 	}
-	return flip;
+	if (ctx->rotation <= 90)
+		return flip;
+
+	return (flip ^ S5P_CITRGFMT_FLIP_180) & S5P_CITRGFMT_FLIP_180;
 }
 
 void fimc_hw_set_rotation(struct fimc_ctx *ctx)
@@ -84,10 +90,7 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 
 	cfg = readl(dev->regs + S5P_CITRGFMT);
 	cfg &= ~(S5P_CITRGFMT_INROT90 | S5P_CITRGFMT_OUTROT90 |
-		  S5P_CITRGFMT_FLIP_180);
-
-	flip = readl(dev->regs + S5P_MSCTRL);
-	flip &= ~S5P_MSCTRL_FLIP_MASK;
+		 S5P_CITRGFMT_FLIP_180);
 
 	/*
 	 * The input and output rotator cannot work simultaneously.
@@ -95,26 +98,22 @@ void fimc_hw_set_rotation(struct fimc_ctx *ctx)
 	 * in direct fifo output mode.
 	 */
 	if (ctx->rotation == 90 || ctx->rotation == 270) {
-		if (ctx->out_path == FIMC_LCDFIFO) {
-			cfg |= S5P_CITRGFMT_INROT90;
-			if (ctx->rotation == 270)
-				flip |= S5P_MSCTRL_FLIP_180;
-		} else {
-			cfg |= S5P_CITRGFMT_OUTROT90;
-			if (ctx->rotation == 270)
-				cfg |= S5P_CITRGFMT_FLIP_180;
-		}
-	} else if (ctx->rotation == 180) {
 		if (ctx->out_path == FIMC_LCDFIFO)
-			flip |= S5P_MSCTRL_FLIP_180;
+			cfg |= S5P_CITRGFMT_INROT90;
 		else
-			cfg |= S5P_CITRGFMT_FLIP_180;
+			cfg |= S5P_CITRGFMT_OUTROT90;
 	}
-	if (ctx->rotation == 180 || ctx->rotation == 270)
-		writel(flip, dev->regs + S5P_MSCTRL);
 
-	cfg |= fimc_hw_get_target_flip(ctx->flip);
-	writel(cfg, dev->regs + S5P_CITRGFMT);
+	if (ctx->out_path == FIMC_DMA) {
+		cfg |= fimc_hw_get_target_flip(ctx);
+		writel(cfg, dev->regs + S5P_CITRGFMT);
+	} else {
+		/* LCD FIFO path */
+		flip = readl(dev->regs + S5P_MSCTRL);
+		flip &= ~S5P_MSCTRL_FLIP_MASK;
+		flip |= fimc_hw_get_in_flip(ctx);
+		writel(flip, dev->regs + S5P_MSCTRL);
+	}
 }
 
 void fimc_hw_set_target_format(struct fimc_ctx *ctx)
@@ -131,19 +130,14 @@ void fimc_hw_set_target_format(struct fimc_ctx *ctx)
 		  S5P_CITRGFMT_VSIZE_MASK);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB565:
-	case S5P_FIMC_RGB666:
-	case S5P_FIMC_RGB888:
+	case S5P_FIMC_RGB565...S5P_FIMC_RGB888:
 		cfg |= S5P_CITRGFMT_RGB;
 		break;
 	case S5P_FIMC_YCBCR420:
 		cfg |= S5P_CITRGFMT_YCBCR420;
 		break;
-	case S5P_FIMC_YCBYCR422:
-	case S5P_FIMC_YCRYCB422:
-	case S5P_FIMC_CBYCRY422:
-	case S5P_FIMC_CRYCBY422:
-		if (frame->fmt->planes_cnt == 1)
+	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+		if (frame->fmt->colplanes == 1)
 			cfg |= S5P_CITRGFMT_YCBCR422_1P;
 		else
 			cfg |= S5P_CITRGFMT_YCBCR422;
@@ -219,11 +213,11 @@ void fimc_hw_set_out_dma(struct fimc_ctx *ctx)
 	cfg &= ~(S5P_CIOCTRL_ORDER2P_MASK | S5P_CIOCTRL_ORDER422_MASK |
 		 S5P_CIOCTRL_YCBCR_PLANE_MASK);
 
-	if (frame->fmt->planes_cnt == 1)
+	if (frame->fmt->colplanes == 1)
 		cfg |= ctx->out_order_1p;
-	else if (frame->fmt->planes_cnt == 2)
+	else if (frame->fmt->colplanes == 2)
 		cfg |= ctx->out_order_2p | S5P_CIOCTRL_YCBCR_2PLANE;
-	else if (frame->fmt->planes_cnt == 3)
+	else if (frame->fmt->colplanes == 3)
 		cfg |= S5P_CIOCTRL_YCBCR_3PLANE;
 
 	writel(cfg, dev->regs + S5P_CIOCTRL);
@@ -249,7 +243,7 @@ void fimc_hw_en_lastirq(struct fimc_dev *dev, int enable)
 	writel(cfg, dev->regs + S5P_CIOCTRL);
 }
 
-static void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev =  ctx->fimc_dev;
 	struct fimc_scaler *sc = &ctx->scaler;
@@ -267,7 +261,7 @@ static void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
 	writel(cfg, dev->regs + S5P_CISCPREDST);
 }
 
-void fimc_hw_set_scaler(struct fimc_ctx *ctx)
+static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 {
 	struct fimc_dev *dev = ctx->fimc_dev;
 	struct fimc_scaler *sc = &ctx->scaler;
@@ -275,8 +269,6 @@ void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 	struct fimc_frame *dst_frame = &ctx->d_frame;
 	u32 cfg = 0;
 
-	fimc_hw_set_prescaler(ctx);
-
 	if (!(ctx->flags & FIMC_COLOR_RANGE_NARROW))
 		cfg |= (S5P_CISCCTRL_CSCR2Y_WIDE | S5P_CISCCTRL_CSCY2R_WIDE);
 
@@ -316,13 +308,42 @@ void fimc_hw_set_scaler(struct fimc_ctx *ctx)
 			cfg |= S5P_CISCCTRL_INTERLACE;
 	}
 
+	writel(cfg, dev->regs + S5P_CISCCTRL);
+}
+
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct samsung_fimc_variant *variant = dev->variant;
+	struct fimc_scaler *sc = &ctx->scaler;
+	u32 cfg;
+
 	dbg("main_hratio= 0x%X  main_vratio= 0x%X",
 		sc->main_hratio, sc->main_vratio);
 
-	cfg |= S5P_CISCCTRL_SC_HORRATIO(sc->main_hratio);
-	cfg |= S5P_CISCCTRL_SC_VERRATIO(sc->main_vratio);
+	fimc_hw_set_scaler(ctx);
 
-	writel(cfg, dev->regs + S5P_CISCCTRL);
+	cfg = readl(dev->regs + S5P_CISCCTRL);
+
+	if (variant->has_mainscaler_ext) {
+		cfg &= ~(S5P_CISCCTRL_MHRATIO_MASK | S5P_CISCCTRL_MVRATIO_MASK);
+		cfg |= S5P_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
+		cfg |= S5P_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + S5P_CISCCTRL);
+
+		cfg = readl(dev->regs + S5P_CIEXTEN);
+
+		cfg &= ~(S5P_CIEXTEN_MVRATIO_EXT_MASK |
+			 S5P_CIEXTEN_MHRATIO_EXT_MASK);
+		cfg |= S5P_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
+		cfg |= S5P_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + S5P_CIEXTEN);
+	} else {
+		cfg &= ~(S5P_CISCCTRL_MHRATIO_MASK | S5P_CISCCTRL_MVRATIO_MASK);
+		cfg |= S5P_CISCCTRL_MHRATIO(sc->main_hratio);
+		cfg |= S5P_CISCCTRL_MVRATIO(sc->main_vratio);
+		writel(cfg, dev->regs + S5P_CISCCTRL);
+	}
 }
 
 void fimc_hw_en_capture(struct fimc_ctx *ctx)
@@ -410,41 +431,37 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 
 	/* Set the input DMA to process single frame only. */
 	cfg = readl(dev->regs + S5P_MSCTRL);
-	cfg &= ~(S5P_MSCTRL_FLIP_MASK
-		| S5P_MSCTRL_INFORMAT_MASK
+	cfg &= ~(S5P_MSCTRL_INFORMAT_MASK
 		| S5P_MSCTRL_IN_BURST_COUNT_MASK
 		| S5P_MSCTRL_INPUT_MASK
 		| S5P_MSCTRL_C_INT_IN_MASK
 		| S5P_MSCTRL_2P_IN_ORDER_MASK);
 
-	cfg |= (S5P_MSCTRL_FRAME_COUNT(1) | S5P_MSCTRL_INPUT_MEMORY);
+	cfg |= (S5P_MSCTRL_IN_BURST_COUNT(4)
+		| S5P_MSCTRL_INPUT_MEMORY
+		| S5P_MSCTRL_FIFO_CTRL_FULL);
 
 	switch (frame->fmt->color) {
-	case S5P_FIMC_RGB565:
-	case S5P_FIMC_RGB666:
-	case S5P_FIMC_RGB888:
+	case S5P_FIMC_RGB565...S5P_FIMC_RGB888:
 		cfg |= S5P_MSCTRL_INFORMAT_RGB;
 		break;
 	case S5P_FIMC_YCBCR420:
 		cfg |= S5P_MSCTRL_INFORMAT_YCBCR420;
 
-		if (frame->fmt->planes_cnt == 2)
+		if (frame->fmt->colplanes == 2)
 			cfg |= ctx->in_order_2p | S5P_MSCTRL_C_INT_IN_2PLANE;
 		else
 			cfg |= S5P_MSCTRL_C_INT_IN_3PLANE;
 
 		break;
-	case S5P_FIMC_YCBYCR422:
-	case S5P_FIMC_YCRYCB422:
-	case S5P_FIMC_CBYCRY422:
-	case S5P_FIMC_CRYCBY422:
-		if (frame->fmt->planes_cnt == 1) {
+	case S5P_FIMC_YCBYCR422...S5P_FIMC_CRYCBY422:
+		if (frame->fmt->colplanes == 1) {
 			cfg |= ctx->in_order_1p
 				| S5P_MSCTRL_INFORMAT_YCBCR422_1P;
 		} else {
 			cfg |= S5P_MSCTRL_INFORMAT_YCBCR422;
 
-			if (frame->fmt->planes_cnt == 2)
+			if (frame->fmt->colplanes == 2)
 				cfg |= ctx->in_order_2p
 					| S5P_MSCTRL_C_INT_IN_2PLANE;
 			else
@@ -455,13 +472,6 @@ void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
 		break;
 	}
 
-	/*
-	 * Input DMA flip mode (and rotation).
-	 * Do not allow simultaneous rotation and flipping.
-	 */
-	if (!ctx->rotation && ctx->out_path == FIMC_LCDFIFO)
-		cfg |= fimc_hw_get_in_flip(ctx->flip);
-
 	writel(cfg, dev->regs + S5P_MSCTRL);
 
 	/* Input/output DMA linear/tiled mode. */
@@ -532,7 +542,7 @@ void fimc_hw_set_output_addr(struct fimc_dev *dev,
 }
 
 int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
-				struct s3c_fimc_isp_info *cam)
+				struct s5p_fimc_isp_info *cam)
 {
 	u32 cfg = readl(fimc->regs + S5P_CIGCTRL);
 
@@ -557,41 +567,46 @@ int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
 }
 
 int fimc_hw_set_camera_source(struct fimc_dev *fimc,
-			      struct s3c_fimc_isp_info *cam)
+			      struct s5p_fimc_isp_info *cam)
 {
 	struct fimc_frame *f = &fimc->vid_cap.ctx->s_frame;
 	u32 cfg = 0;
+	u32 bus_width;
+	int i;
+
+	static const struct {
+		u32 pixelcode;
+		u32 cisrcfmt;
+		u16 bus_width;
+	} pix_desc[] = {
+		{ V4L2_MBUS_FMT_YUYV8_2X8, S5P_CISRCFMT_ORDER422_YCBYCR, 8 },
+		{ V4L2_MBUS_FMT_YVYU8_2X8, S5P_CISRCFMT_ORDER422_YCRYCB, 8 },
+		{ V4L2_MBUS_FMT_VYUY8_2X8, S5P_CISRCFMT_ORDER422_CRYCBY, 8 },
+		{ V4L2_MBUS_FMT_UYVY8_2X8, S5P_CISRCFMT_ORDER422_CBYCRY, 8 },
+		/* TODO: Add pixel codes for 16-bit bus width */
+	};
 
 	if (cam->bus_type == FIMC_ITU_601 || cam->bus_type == FIMC_ITU_656) {
+		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
+			if (fimc->vid_cap.fmt.code == pix_desc[i].pixelcode) {
+				cfg = pix_desc[i].cisrcfmt;
+				bus_width = pix_desc[i].bus_width;
+				break;
+			}
+		}
 
-		switch (fimc->vid_cap.fmt.code) {
-		case V4L2_MBUS_FMT_YUYV8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_YCBYCR;
-			break;
-		case V4L2_MBUS_FMT_YVYU8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_YCRYCB;
-			break;
-		case V4L2_MBUS_FMT_VYUY8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_CRYCBY;
-			break;
-		case V4L2_MBUS_FMT_UYVY8_2X8:
-			cfg = S5P_CISRCFMT_ORDER422_CBYCRY;
-			break;
-		default:
-			err("camera image format not supported: %d",
-			    fimc->vid_cap.fmt.code);
+		if (i == ARRAY_SIZE(pix_desc)) {
+			v4l2_err(&fimc->vid_cap.v4l2_dev,
+				 "Camera color format not supported: %d\n",
+				 fimc->vid_cap.fmt.code);
 			return -EINVAL;
 		}
 
 		if (cam->bus_type == FIMC_ITU_601) {
-			if (cam->bus_width == 8) {
+			if (bus_width == 8)
 				cfg |= S5P_CISRCFMT_ITU601_8BIT;
-			} else if (cam->bus_width == 16) {
+			else if (bus_width == 16)
 				cfg |= S5P_CISRCFMT_ITU601_16BIT;
-			} else {
-				err("invalid bus width: %d", cam->bus_width);
-				return -EINVAL;
-			}
 		} /* else defaults to ITU-R BT.656 8-bit */
 	}
 
@@ -624,7 +639,7 @@ int fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
 }
 
 int fimc_hw_set_camera_type(struct fimc_dev *fimc,
-			    struct s3c_fimc_isp_info *cam)
+			    struct s5p_fimc_isp_info *cam)
 {
 	u32 cfg, tmp;
 	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
diff --git a/drivers/media/video/s5p-fimc/regs-fimc.h b/drivers/media/video/s5p-fimc/regs-fimc.h
index 57e33f84fcfa..0fea3e635d76 100644
--- a/drivers/media/video/s5p-fimc/regs-fimc.h
+++ b/drivers/media/video/s5p-fimc/regs-fimc.h
@@ -98,8 +98,8 @@
 #define S5P_CIOCTRL			0x4c
 #define S5P_CIOCTRL_ORDER422_MASK	(3 << 0)
 #define S5P_CIOCTRL_ORDER422_CRYCBY	(0 << 0)
-#define S5P_CIOCTRL_ORDER422_YCRYCB	(1 << 0)
-#define S5P_CIOCTRL_ORDER422_CBYCRY	(2 << 0)
+#define S5P_CIOCTRL_ORDER422_CBYCRY	(1 << 0)
+#define S5P_CIOCTRL_ORDER422_YCRYCB	(2 << 0)
 #define S5P_CIOCTRL_ORDER422_YCBYCR	(3 << 0)
 #define S5P_CIOCTRL_LASTIRQ_ENABLE	(1 << 2)
 #define S5P_CIOCTRL_YCBCR_3PLANE	(0 << 3)
@@ -139,8 +139,12 @@
 #define S5P_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
 #define S5P_CISCCTRL_RGB_EXT		(1 << 10)
 #define S5P_CISCCTRL_ONE2ONE		(1 << 9)
-#define S5P_CISCCTRL_SC_HORRATIO(x)	((x) << 16)
-#define S5P_CISCCTRL_SC_VERRATIO(x)	((x) << 0)
+#define S5P_CISCCTRL_MHRATIO(x)		((x) << 16)
+#define S5P_CISCCTRL_MVRATIO(x)		((x) << 0)
+#define S5P_CISCCTRL_MHRATIO_MASK	(0x1ff << 16)
+#define S5P_CISCCTRL_MVRATIO_MASK	(0x1ff << 0)
+#define S5P_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
+#define S5P_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
 
 /* Target area */
 #define S5P_CITAREA			0x5c
@@ -210,7 +214,7 @@
 
 /* Input DMA control */
 #define S5P_MSCTRL			0xfc
-#define S5P_MSCTRL_IN_BURST_COUNT_MASK	(3 << 24)
+#define S5P_MSCTRL_IN_BURST_COUNT_MASK	(0xF << 24)
 #define S5P_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
 #define S5P_MSCTRL_2P_IN_ORDER_SHIFT	16
 #define S5P_MSCTRL_C_INT_IN_3PLANE	(0 << 15)
@@ -222,11 +226,12 @@
 #define S5P_MSCTRL_FLIP_X_MIRROR	(1 << 13)
 #define S5P_MSCTRL_FLIP_Y_MIRROR	(2 << 13)
 #define S5P_MSCTRL_FLIP_180		(3 << 13)
+#define S5P_MSCTRL_FIFO_CTRL_FULL	(1 << 12)
 #define S5P_MSCTRL_ORDER422_SHIFT	4
-#define S5P_MSCTRL_ORDER422_CRYCBY	(0 << 4)
-#define S5P_MSCTRL_ORDER422_YCRYCB	(1 << 4)
-#define S5P_MSCTRL_ORDER422_CBYCRY	(2 << 4)
-#define S5P_MSCTRL_ORDER422_YCBYCR	(3 << 4)
+#define S5P_MSCTRL_ORDER422_YCBYCR	(0 << 4)
+#define S5P_MSCTRL_ORDER422_CBYCRY	(1 << 4)
+#define S5P_MSCTRL_ORDER422_YCRYCB	(2 << 4)
+#define S5P_MSCTRL_ORDER422_CRYCBY	(3 << 4)
 #define S5P_MSCTRL_ORDER422_MASK	(3 << 4)
 #define S5P_MSCTRL_INPUT_EXTCAM		(0 << 3)
 #define S5P_MSCTRL_INPUT_MEMORY		(1 << 3)
@@ -237,7 +242,7 @@
 #define S5P_MSCTRL_INFORMAT_RGB		(3 << 1)
 #define S5P_MSCTRL_INFORMAT_MASK	(3 << 1)
 #define S5P_MSCTRL_ENVID		(1 << 0)
-#define S5P_MSCTRL_FRAME_COUNT(x)	((x) << 24)
+#define S5P_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
 
 /* Output DMA Y/Cb/Cr offset */
 #define S5P_CIOYOFF			0x168
@@ -263,6 +268,10 @@
 
 /* Real output DMA image size (extension register) */
 #define S5P_CIEXTEN			0x188
+#define S5P_CIEXTEN_MHRATIO_EXT(x)	(((x) & 0x3f) << 10)
+#define S5P_CIEXTEN_MVRATIO_EXT(x)	((x) & 0x3f)
+#define S5P_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
+#define S5P_CIEXTEN_MVRATIO_EXT_MASK	0x3f
 
 #define S5P_CIDMAPARAM			0x18c
 #define S5P_CIDMAPARAM_R_LINEAR		(0 << 29)
diff --git a/drivers/media/video/saa7110.c b/drivers/media/video/saa7110.c
index 7913f93979b8..99664205ef4e 100644
--- a/drivers/media/video/saa7110.c
+++ b/drivers/media/video/saa7110.c
@@ -36,6 +36,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 MODULE_DESCRIPTION("Philips SAA7110 video decoder driver");
 MODULE_AUTHOR("Pauline Middelink");
@@ -53,15 +54,12 @@ MODULE_PARM_DESC(debug, "Debug level (0-1)");
 
 struct saa7110 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	u8 reg[SAA7110_NR_REG];
 
 	v4l2_std_id norm;
 	int input;
 	int enable;
-	int bright;
-	int contrast;
-	int hue;
-	int sat;
 
 	wait_queue_head_t wq;
 };
@@ -71,6 +69,11 @@ static inline struct saa7110 *to_saa7110(struct v4l2_subdev *sd)
 	return container_of(sd, struct saa7110, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct saa7110, hdl)->sd;
+}
+
 /* ----------------------------------------------------------------------- */
 /* I2C support functions						   */
 /* ----------------------------------------------------------------------- */
@@ -326,73 +329,22 @@ static int saa7110_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int saa7110_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 127, 1, 64);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int saa7110_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct saa7110 *decoder = to_saa7110(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = decoder->bright;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = decoder->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = decoder->sat;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = decoder->hue;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int saa7110_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int saa7110_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct saa7110 *decoder = to_saa7110(sd);
+	struct v4l2_subdev *sd = to_sd(ctrl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if (decoder->bright != ctrl->value) {
-			decoder->bright = ctrl->value;
-			saa7110_write(sd, 0x19, decoder->bright);
-		}
+		saa7110_write(sd, 0x19, ctrl->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		if (decoder->contrast != ctrl->value) {
-			decoder->contrast = ctrl->value;
-			saa7110_write(sd, 0x13, decoder->contrast);
-		}
+		saa7110_write(sd, 0x13, ctrl->val);
 		break;
 	case V4L2_CID_SATURATION:
-		if (decoder->sat != ctrl->value) {
-			decoder->sat = ctrl->value;
-			saa7110_write(sd, 0x12, decoder->sat);
-		}
+		saa7110_write(sd, 0x12, ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		if (decoder->hue != ctrl->value) {
-			decoder->hue = ctrl->value;
-			saa7110_write(sd, 0x07, decoder->hue);
-		}
+		saa7110_write(sd, 0x07, ctrl->val);
 		break;
 	default:
 		return -EINVAL;
@@ -409,11 +361,19 @@ static int saa7110_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ide
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops saa7110_ctrl_ops = {
+	.s_ctrl = saa7110_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops saa7110_core_ops = {
 	.g_chip_ident = saa7110_g_chip_ident,
-	.g_ctrl = saa7110_g_ctrl,
-	.s_ctrl = saa7110_s_ctrl,
-	.queryctrl = saa7110_queryctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 	.s_std = saa7110_s_std,
 };
 
@@ -454,10 +414,25 @@ static int saa7110_probe(struct i2c_client *client,
 	decoder->norm = V4L2_STD_PAL;
 	decoder->input = 0;
 	decoder->enable = 1;
-	decoder->bright = 32768;
-	decoder->contrast = 32768;
-	decoder->hue = 32768;
-	decoder->sat = 32768;
+	v4l2_ctrl_handler_init(&decoder->hdl, 2);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(&decoder->hdl, &saa7110_ctrl_ops,
+		V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		kfree(decoder);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
 	init_waitqueue_head(&decoder->wq);
 
 	rv = saa7110_write_block(sd, initseq, sizeof(initseq));
@@ -490,9 +465,11 @@ static int saa7110_probe(struct i2c_client *client,
 static int saa7110_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct saa7110 *decoder = to_saa7110(sd);
 
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_saa7110(sd));
+	v4l2_ctrl_handler_free(&decoder->hdl);
+	kfree(decoder);
 	return 0;
 }
 
diff --git a/drivers/media/video/saa7134/saa7134-cards.c b/drivers/media/video/saa7134/saa7134-cards.c
index deb8fcf4aa49..61c6007c8ea6 100644
--- a/drivers/media/video/saa7134/saa7134-cards.c
+++ b/drivers/media/video/saa7134/saa7134-cards.c
@@ -3620,6 +3620,38 @@ struct saa7134_board saa7134_boards[] = {
 			.amux = 0,
 		},
 	},
+	[SAA7134_BOARD_ENCORE_ENLTV_FM3] = {
+		.name           = "Encore ENLTV-FM 3",
+		.audio_clock    = 0x02187de7,
+		.tuner_type     = TUNER_TENA_TNF_5337,
+		.radio_type     = TUNER_TEA5767,
+		.tuner_addr	= 0x61,
+		.radio_addr	= 0x60,
+		.inputs         = { {
+			.name = name_tv,
+			.vmux = 1,
+			.amux = LINE2,
+			.tv   = 1,
+		}, {
+			.name = name_comp1,
+			.vmux = 3,
+			.amux = LINE1,
+		}, {
+			.name = name_svideo,
+			.vmux = 8,
+			.amux = LINE1,
+		} },
+		.radio = {
+			.name = name_radio,
+			.vmux = 1,
+			.amux = LINE1,
+		},
+		.mute = {
+			.name = name_mute,
+			.amux = LINE1,
+			.gpio = 0x43000,
+		},
+	},
 	[SAA7134_BOARD_CINERGY_HT_PCI] = {
 		.name           = "Terratec Cinergy HT PCI",
 		.audio_clock    = 0x00187de7,
@@ -6387,6 +6419,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.driver_data  = SAA7134_BOARD_ENCORE_ENLTV_FM53,
 	}, {
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
+		.subvendor    = 0x1a7f,
+		.subdevice    = 0x2108,
+		.driver_data  = SAA7134_BOARD_ENCORE_ENLTV_FM3,
+	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
 		.subvendor    = 0x153b,
 		.subdevice    = 0x1175,
@@ -7102,6 +7140,7 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 	case SAA7134_BOARD_ENCORE_ENLTV:
 	case SAA7134_BOARD_ENCORE_ENLTV_FM:
 	case SAA7134_BOARD_ENCORE_ENLTV_FM53:
+	case SAA7134_BOARD_ENCORE_ENLTV_FM3:
 	case SAA7134_BOARD_10MOONSTVMASTER3:
 	case SAA7134_BOARD_BEHOLD_401:
 	case SAA7134_BOARD_BEHOLD_403:
@@ -7294,9 +7333,7 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 static void saa7134_tuner_setup(struct saa7134_dev *dev)
 {
 	struct tuner_setup tun_setup;
-	unsigned int mode_mask = T_RADIO     |
-				 T_ANALOG_TV |
-				 T_DIGITAL_TV;
+	unsigned int mode_mask = T_RADIO | T_ANALOG_TV;
 
 	memset(&tun_setup, 0, sizeof(tun_setup));
 	tun_setup.tuner_callback = saa7134_tuner_callback;
diff --git a/drivers/media/video/saa7134/saa7134-core.c b/drivers/media/video/saa7134/saa7134-core.c
index 6abeecff6da7..41f836fc93ec 100644
--- a/drivers/media/video/saa7134/saa7134-core.c
+++ b/drivers/media/video/saa7134/saa7134-core.c
@@ -752,19 +752,28 @@ static int saa7134_hwfini(struct saa7134_dev *dev)
 	return 0;
 }
 
-static void __devinit must_configure_manually(void)
+static void __devinit must_configure_manually(int has_eeprom)
 {
 	unsigned int i,p;
 
-	printk(KERN_WARNING
-	       "saa7134: <rant>\n"
-	       "saa7134:  Congratulations!  Your TV card vendor saved a few\n"
-	       "saa7134:  cents for a eeprom, thus your pci board has no\n"
-	       "saa7134:  subsystem ID and I can't identify it automatically\n"
-	       "saa7134: </rant>\n"
-	       "saa7134: I feel better now.  Ok, here are the good news:\n"
-	       "saa7134: You can use the card=<nr> insmod option to specify\n"
-	       "saa7134: which board do you have.  The list:\n");
+	if (!has_eeprom)
+		printk(KERN_WARNING
+		       "saa7134: <rant>\n"
+		       "saa7134:  Congratulations!  Your TV card vendor saved a few\n"
+		       "saa7134:  cents for a eeprom, thus your pci board has no\n"
+		       "saa7134:  subsystem ID and I can't identify it automatically\n"
+		       "saa7134: </rant>\n"
+		       "saa7134: I feel better now.  Ok, here are the good news:\n"
+		       "saa7134: You can use the card=<nr> insmod option to specify\n"
+		       "saa7134: which board do you have.  The list:\n");
+	else
+		printk(KERN_WARNING
+		       "saa7134: Board is currently unknown. You might try to use the card=<nr>\n"
+		       "saa7134: insmod option to specify which board do you have, but this is\n"
+		       "saa7134: somewhat risky, as might damage your card. It is better to ask\n"
+		       "saa7134: for support at linux-media@vger.kernel.org.\n"
+		       "saa7134: The supported cards are:\n");
+
 	for (i = 0; i < saa7134_bcount; i++) {
 		printk(KERN_WARNING "saa7134:   card=%d -> %-40.40s",
 		       i,saa7134_boards[i].name);
@@ -936,8 +945,10 @@ static int __devinit saa7134_initdev(struct pci_dev *pci_dev,
 	if (card[dev->nr] >= 0 &&
 	    card[dev->nr] < saa7134_bcount)
 		dev->board = card[dev->nr];
-	if (SAA7134_BOARD_NOAUTO == dev->board) {
-		must_configure_manually();
+	if (SAA7134_BOARD_UNKNOWN == dev->board)
+		must_configure_manually(0);
+	else if (SAA7134_BOARD_NOAUTO == dev->board) {
+		must_configure_manually(1);
 		dev->board = SAA7134_BOARD_UNKNOWN;
 	}
 	dev->autodetected = card[dev->nr] != dev->board;
diff --git a/drivers/media/video/saa7134/saa7134-empress.c b/drivers/media/video/saa7134/saa7134-empress.c
index 6b8459c7728e..18294db38a01 100644
--- a/drivers/media/video/saa7134/saa7134-empress.c
+++ b/drivers/media/video/saa7134/saa7134-empress.c
@@ -373,6 +373,10 @@ static int empress_queryctrl(struct file *file, void *priv,
 	static const u32 mpeg_ctrls[] = {
 		V4L2_CID_MPEG_CLASS,
 		V4L2_CID_MPEG_STREAM_TYPE,
+		V4L2_CID_MPEG_STREAM_PID_PMT,
+		V4L2_CID_MPEG_STREAM_PID_AUDIO,
+		V4L2_CID_MPEG_STREAM_PID_VIDEO,
+		V4L2_CID_MPEG_STREAM_PID_PCR,
 		V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ,
 		V4L2_CID_MPEG_AUDIO_ENCODING,
 		V4L2_CID_MPEG_AUDIO_L2_BITRATE,
diff --git a/drivers/media/video/saa7134/saa7134-input.c b/drivers/media/video/saa7134/saa7134-input.c
index dc646e65edb7..c9eff0336aa6 100644
--- a/drivers/media/video/saa7134/saa7134-input.c
+++ b/drivers/media/video/saa7134/saa7134-input.c
@@ -681,6 +681,7 @@ int saa7134_input_init1(struct saa7134_dev *dev)
 		polling      = 50; // ms
 		break;
 	case SAA7134_BOARD_ENCORE_ENLTV_FM53:
+	case SAA7134_BOARD_ENCORE_ENLTV_FM3:
 		ir_codes     = RC_MAP_ENCORE_ENLTV_FM53;
 		mask_keydown = 0x0040000;	/* Enable GPIO18 line on both edges */
 		mask_keyup   = 0x0040000;
diff --git a/drivers/media/video/saa7134/saa7134.h b/drivers/media/video/saa7134/saa7134.h
index 5b0a347b0b8f..f96cd5d761f9 100644
--- a/drivers/media/video/saa7134/saa7134.h
+++ b/drivers/media/video/saa7134/saa7134.h
@@ -327,6 +327,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_TECHNOTREND_BUDGET_T3000 181
 #define SAA7134_BOARD_KWORLD_PCI_SBTVD_FULLSEG 182
 #define SAA7134_BOARD_VIDEOMATE_M1F         183
+#define SAA7134_BOARD_ENCORE_ENLTV_FM3      184
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
diff --git a/drivers/media/video/saa7164/saa7164-api.c b/drivers/media/video/saa7164/saa7164-api.c
index bd86d970f4c2..8a98ab68239e 100644
--- a/drivers/media/video/saa7164/saa7164-api.c
+++ b/drivers/media/video/saa7164/saa7164-api.c
@@ -743,7 +743,7 @@ int saa7164_api_configure_dif(struct saa7164_port *port, u32 std)
 int saa7164_api_initialize_dif(struct saa7164_port *port)
 {
 	struct saa7164_dev *dev = port->dev;
-	struct saa7164_port *p = 0;
+	struct saa7164_port *p = NULL;
 	int ret = -EINVAL;
 	u32 std = 0;
 
@@ -926,9 +926,9 @@ int saa7164_api_configure_port_mpeg2ps(struct saa7164_dev *dev,
 
 int saa7164_api_dump_subdevs(struct saa7164_dev *dev, u8 *buf, int len)
 {
-	struct saa7164_port *tsport = 0;
-	struct saa7164_port *encport = 0;
-	struct saa7164_port *vbiport = 0;
+	struct saa7164_port *tsport = NULL;
+	struct saa7164_port *encport = NULL;
+	struct saa7164_port *vbiport = NULL;
 	u32 idx, next_offset;
 	int i;
 	struct tmComResDescrHeader *hdr, *t;
@@ -1340,7 +1340,7 @@ int saa7164_api_enum_subdevs(struct saa7164_dev *dev)
 
 	/* Allocate enough storage for all of the descs */
 	buf = kzalloc(buflen, GFP_KERNEL);
-	if (buf == NULL)
+	if (!buf)
 		return SAA_ERR_NO_RESOURCES;
 
 	/* Retrieve them */
diff --git a/drivers/media/video/saa7164/saa7164-buffer.c b/drivers/media/video/saa7164/saa7164-buffer.c
index ddd25211c9e8..66696fa8341d 100644
--- a/drivers/media/video/saa7164/saa7164-buffer.c
+++ b/drivers/media/video/saa7164/saa7164-buffer.c
@@ -93,7 +93,7 @@ struct saa7164_buffer *saa7164_buffer_alloc(struct saa7164_port *port,
 	u32 len)
 {
 	struct tmHWStreamParameters *params = &port->hw_streamingparams;
-	struct saa7164_buffer *buf = 0;
+	struct saa7164_buffer *buf = NULL;
 	struct saa7164_dev *dev = port->dev;
 	int i;
 
@@ -103,7 +103,7 @@ struct saa7164_buffer *saa7164_buffer_alloc(struct saa7164_port *port,
 	}
 
 	buf = kzalloc(sizeof(struct saa7164_buffer), GFP_KERNEL);
-	if (buf == NULL) {
+	if (!buf) {
 		log_warn("%s() SAA_ERR_NO_RESOURCES\n", __func__);
 		goto ret;
 	}
@@ -157,7 +157,7 @@ fail2:
 fail1:
 	kfree(buf);
 
-	buf = 0;
+	buf = NULL;
 ret:
 	return buf;
 }
@@ -289,14 +289,14 @@ struct saa7164_user_buffer *saa7164_buffer_alloc_user(struct saa7164_dev *dev,
 	struct saa7164_user_buffer *buf;
 
 	buf = kzalloc(sizeof(struct saa7164_user_buffer), GFP_KERNEL);
-	if (buf == 0)
-		return 0;
+	if (!buf)
+		return NULL;
 
 	buf->data = kzalloc(len, GFP_KERNEL);
 
-	if (buf->data == 0) {
+	if (!buf->data) {
 		kfree(buf);
-		return 0;
+		return NULL;
 	}
 
 	buf->actual_size = len;
@@ -315,7 +315,7 @@ void saa7164_buffer_dealloc_user(struct saa7164_user_buffer *buf)
 		return;
 
 	kfree(buf->data);
-	buf->data = 0;
+	buf->data = NULL;
 
 	kfree(buf);
 }
diff --git a/drivers/media/video/saa7164/saa7164-bus.c b/drivers/media/video/saa7164/saa7164-bus.c
index b2b0d97101d0..466e1b02f91f 100644
--- a/drivers/media/video/saa7164/saa7164-bus.c
+++ b/drivers/media/video/saa7164/saa7164-bus.c
@@ -158,7 +158,7 @@ int saa7164_bus_set(struct saa7164_dev *dev, struct tmComResInfo* msg,
 		return SAA_ERR_BAD_PARAMETER;
 	}
 
-	if ((msg->size > 0) && (buf == 0)) {
+	if ((msg->size > 0) && (buf == NULL)) {
 		printk(KERN_ERR "%s() Missing message buffer\n", __func__);
 		return SAA_ERR_BAD_PARAMETER;
 	}
@@ -315,7 +315,7 @@ int saa7164_bus_get(struct saa7164_dev *dev, struct tmComResInfo* msg,
 
 	saa7164_bus_verify(dev);
 
-	if (msg == 0)
+	if (msg == NULL)
 		return ret;
 
 	if (msg->size > dev->bus.m_wMaxReqSize) {
@@ -324,7 +324,7 @@ int saa7164_bus_get(struct saa7164_dev *dev, struct tmComResInfo* msg,
 		return ret;
 	}
 
-	if ((peekonly == 0) && (msg->size > 0) && (buf == 0)) {
+	if ((peekonly == 0) && (msg->size > 0) && (buf == NULL)) {
 		printk(KERN_ERR
 			"%s() Missing msg buf, size should be %d bytes\n",
 			__func__, msg->size);
@@ -392,7 +392,7 @@ int saa7164_bus_get(struct saa7164_dev *dev, struct tmComResInfo* msg,
 
 		printk(KERN_ERR "%s() Unexpected msg miss-match\n", __func__);
 		saa7164_bus_dumpmsg(dev, msg, buf);
-		saa7164_bus_dumpmsg(dev, &msg_tmp, 0);
+		saa7164_bus_dumpmsg(dev, &msg_tmp, NULL);
 		ret = SAA_ERR_INVALID_COMMAND;
 		goto out;
 	}
diff --git a/drivers/media/video/saa7164/saa7164-cmd.c b/drivers/media/video/saa7164/saa7164-cmd.c
index a97ae17b36c2..6a4c217ed3a7 100644
--- a/drivers/media/video/saa7164/saa7164-cmd.c
+++ b/drivers/media/video/saa7164/saa7164-cmd.c
@@ -84,7 +84,7 @@ int saa7164_irq_dequeue(struct saa7164_dev *dev)
 {
 	int ret = SAA_OK, i = 0;
 	u32 timeout;
-	wait_queue_head_t *q = 0;
+	wait_queue_head_t *q = NULL;
 	u8 tmp[512];
 	dprintk(DBGLVL_CMD, "%s()\n", __func__);
 
@@ -137,7 +137,7 @@ int saa7164_cmd_dequeue(struct saa7164_dev *dev)
 	int loop = 1;
 	int ret;
 	u32 timeout;
-	wait_queue_head_t *q = 0;
+	wait_queue_head_t *q = NULL;
 	u8 tmp[512];
 	dprintk(DBGLVL_CMD, "%s()\n", __func__);
 
@@ -261,7 +261,7 @@ out:
  */
 int saa7164_cmd_wait(struct saa7164_dev *dev, u8 seqno)
 {
-	wait_queue_head_t *q = 0;
+	wait_queue_head_t *q = NULL;
 	int ret = SAA_BUS_TIMEOUT;
 	unsigned long stamp;
 	int r;
@@ -357,7 +357,7 @@ int saa7164_cmd_send(struct saa7164_dev *dev, u8 id, enum tmComResCmd command,
 		"sel = 0x%x)\n", __func__, saa7164_unitid_name(dev, id), id,
 		command, controlselector);
 
-	if ((size == 0) || (buf == 0)) {
+	if ((size == 0) || (buf == NULL)) {
 		printk(KERN_ERR "%s() Invalid param\n", __func__);
 		return SAA_ERR_BAD_PARAMETER;
 	}
@@ -538,7 +538,7 @@ int saa7164_cmd_send(struct saa7164_dev *dev, u8 id, enum tmComResCmd command,
 
 			/* Invalid */
 			dprintk(DBGLVL_CMD, "%s() Invalid\n", __func__);
-			ret = saa7164_bus_get(dev, presponse_t, 0, 0);
+			ret = saa7164_bus_get(dev, presponse_t, NULL, 0);
 			if (ret != SAA_OK) {
 				printk(KERN_ERR "get failed\n");
 				return ret;
diff --git a/drivers/media/video/saa7164/saa7164-core.c b/drivers/media/video/saa7164/saa7164-core.c
index 58af67f2278b..b813aec1e456 100644
--- a/drivers/media/video/saa7164/saa7164-core.c
+++ b/drivers/media/video/saa7164/saa7164-core.c
@@ -277,8 +277,8 @@ static void saa7164_histogram_print(struct saa7164_port *port,
 static void saa7164_work_enchandler_helper(struct saa7164_port *port, int bufnr)
 {
 	struct saa7164_dev *dev = port->dev;
-	struct saa7164_buffer *buf = 0;
-	struct saa7164_user_buffer *ubuf = 0;
+	struct saa7164_buffer *buf = NULL;
+	struct saa7164_user_buffer *ubuf = NULL;
 	struct list_head *c, *n;
 	int i = 0;
 	u8 __iomem *p;
@@ -649,7 +649,7 @@ static irqreturn_t saa7164_irq(int irq, void *dev_id)
 	u32 intid, intstat[INT_SIZE/4];
 	int i, handled = 0, bit;
 
-	if (dev == 0) {
+	if (dev == NULL) {
 		printk(KERN_ERR "%s() No device specified\n", __func__);
 		handled = 0;
 		goto out;
@@ -945,7 +945,7 @@ static int get_resources(struct saa7164_dev *dev)
 
 static int saa7164_port_init(struct saa7164_dev *dev, int portnr)
 {
-	struct saa7164_port *port = 0;
+	struct saa7164_port *port = NULL;
 
 	if ((portnr < 0) || (portnr >= SAA7164_MAX_PORTS))
 		BUG();
diff --git a/drivers/media/video/saa7164/saa7164-dvb.c b/drivers/media/video/saa7164/saa7164-dvb.c
index b305a01b3bde..f65eab63ca87 100644
--- a/drivers/media/video/saa7164/saa7164-dvb.c
+++ b/drivers/media/video/saa7164/saa7164-dvb.c
@@ -309,8 +309,8 @@ static int dvb_register(struct saa7164_port *port)
 
 	port->hw_streamingparams.pitch = 188;
 	port->hw_streamingparams.linethreshold = 0;
-	port->hw_streamingparams.pagetablelistvirt = 0;
-	port->hw_streamingparams.pagetablelistphys = 0;
+	port->hw_streamingparams.pagetablelistvirt = NULL;
+	port->hw_streamingparams.pagetablelistphys = NULL;
 	port->hw_streamingparams.numpagetables = 2 +
 		((SAA7164_TS_NUMBER_OF_LINES * 188) / PAGE_SIZE);
 
diff --git a/drivers/media/video/saa7164/saa7164-encoder.c b/drivers/media/video/saa7164/saa7164-encoder.c
index 1838408cd5cb..f9d594698832 100644
--- a/drivers/media/video/saa7164/saa7164-encoder.c
+++ b/drivers/media/video/saa7164/saa7164-encoder.c
@@ -152,8 +152,8 @@ static int saa7164_encoder_buffers_alloc(struct saa7164_port *port)
 	/* Init and establish defaults */
 	params->bitspersample = 8;
 	params->linethreshold = 0;
-	params->pagetablelistvirt = 0;
-	params->pagetablelistphys = 0;
+	params->pagetablelistvirt = NULL;
+	params->pagetablelistphys = NULL;
 	params->numpagetableentries = port->hwcfg.buffercount;
 
 	/* Allocate the PCI resources, buffers (hard) */
@@ -1108,7 +1108,7 @@ static int fops_release(struct file *file)
 
 struct saa7164_user_buffer *saa7164_enc_next_buf(struct saa7164_port *port)
 {
-	struct saa7164_user_buffer *ubuf = 0;
+	struct saa7164_user_buffer *ubuf = NULL;
 	struct saa7164_dev *dev = port->dev;
 	u32 crc;
 
@@ -1443,7 +1443,7 @@ int saa7164_encoder_register(struct saa7164_port *port)
 	port->v4l_device = saa7164_encoder_alloc(port,
 		dev->pci, &saa7164_mpeg_template, "mpeg");
 
-	if (port->v4l_device == NULL) {
+	if (!port->v4l_device) {
 		printk(KERN_INFO "%s: can't allocate mpeg device\n",
 			dev->name);
 		result = -ENOMEM;
diff --git a/drivers/media/video/saa7164/saa7164-fw.c b/drivers/media/video/saa7164/saa7164-fw.c
index ebed6f786a23..b369300cce06 100644
--- a/drivers/media/video/saa7164/saa7164-fw.c
+++ b/drivers/media/video/saa7164/saa7164-fw.c
@@ -88,7 +88,7 @@ int saa7164_downloadimage(struct saa7164_dev *dev, u8 *src, u32 srcsize,
 		"%s(image=%p, size=%d, flags=0x%x, dst=%p, dstsize=0x%x)\n",
 		__func__, src, srcsize, dlflags, dst, dstsize);
 
-	if ((src == 0) || (dst == 0)) {
+	if ((src == NULL) || (dst == NULL)) {
 		ret = -EIO;
 		goto out;
 	}
diff --git a/drivers/media/video/saa7164/saa7164-vbi.c b/drivers/media/video/saa7164/saa7164-vbi.c
index 8abbe6d661e4..9e5b01c29cf5 100644
--- a/drivers/media/video/saa7164/saa7164-vbi.c
+++ b/drivers/media/video/saa7164/saa7164-vbi.c
@@ -123,8 +123,8 @@ static int saa7164_vbi_buffers_alloc(struct saa7164_port *port)
 		((params->numberoflines * params->pitch) / PAGE_SIZE);
 	params->bitspersample = 8;
 	params->linethreshold = 0;
-	params->pagetablelistvirt = 0;
-	params->pagetablelistphys = 0;
+	params->pagetablelistvirt = NULL;
+	params->pagetablelistphys = NULL;
 	params->numpagetableentries = port->hwcfg.buffercount;
 
 	/* Allocate the PCI resources, buffers (hard) */
@@ -1054,7 +1054,7 @@ static int fops_release(struct file *file)
 
 struct saa7164_user_buffer *saa7164_vbi_next_buf(struct saa7164_port *port)
 {
-	struct saa7164_user_buffer *ubuf = 0;
+	struct saa7164_user_buffer *ubuf = NULL;
 	struct saa7164_dev *dev = port->dev;
 	u32 crc;
 
@@ -1334,7 +1334,7 @@ int saa7164_vbi_register(struct saa7164_port *port)
 	port->v4l_device = saa7164_vbi_alloc(port,
 		dev->pci, &saa7164_vbi_template, "vbi");
 
-	if (port->v4l_device == NULL) {
+	if (!port->v4l_device) {
 		printk(KERN_INFO "%s: can't allocate vbi device\n",
 			dev->name);
 		result = -ENOMEM;
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 954222bc3458..61f37012b4f8 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -38,7 +38,7 @@
 #include <media/v4l2-dev.h>
 #include <media/soc_camera.h>
 #include <media/sh_mobile_ceu.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-mediabus.h>
 #include <media/soc_mediabus.h>
 
@@ -87,7 +87,8 @@
 
 /* per video frame buffer */
 struct sh_mobile_ceu_buffer {
-	struct videobuf_buffer vb; /* v4l buffer must be first */
+	struct vb2_buffer vb; /* v4l buffer must be first */
+	struct list_head queue;
 	enum v4l2_mbus_pixelcode code;
 };
 
@@ -99,16 +100,17 @@ struct sh_mobile_ceu_dev {
 	void __iomem *base;
 	unsigned long video_limit;
 
-	/* lock used to protect videobuf */
-	spinlock_t lock;
+	spinlock_t lock;		/* Protects video buffer lists */
 	struct list_head capture;
-	struct videobuf_buffer *active;
+	struct vb2_buffer *active;
+	struct vb2_alloc_ctx *alloc_ctx;
 
 	struct sh_mobile_ceu_info *pdata;
 
 	u32 cflcr;
 
 	enum v4l2_field field;
+	int sequence;
 
 	unsigned int image_mode:1;
 	unsigned int is_16bit:1;
@@ -133,6 +135,11 @@ struct sh_mobile_ceu_cam {
 	enum v4l2_mbus_pixelcode code;
 };
 
+static struct sh_mobile_ceu_buffer *to_ceu_vb(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct sh_mobile_ceu_buffer, vb);
+}
+
 static unsigned long make_bus_param(struct sh_mobile_ceu_dev *pcdev)
 {
 	unsigned long flags;
@@ -205,11 +212,11 @@ static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
 /*
  *  Videobuf operations
  */
-static int sh_mobile_ceu_videobuf_setup(struct videobuf_queue *vq,
-					unsigned int *count,
-					unsigned int *size)
+static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
+			unsigned int *count, unsigned int *num_planes,
+			unsigned long sizes[], void *alloc_ctxs[])
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = container_of(vq, struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
 	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
@@ -218,39 +225,25 @@ static int sh_mobile_ceu_videobuf_setup(struct videobuf_queue *vq,
 	if (bytes_per_line < 0)
 		return bytes_per_line;
 
-	*size = bytes_per_line * icd->user_height;
+	*num_planes = 1;
+
+	pcdev->sequence = 0;
+	sizes[0] = bytes_per_line * icd->user_height;
+	alloc_ctxs[0] = pcdev->alloc_ctx;
 
-	if (0 == *count)
+	if (!*count)
 		*count = 2;
 
 	if (pcdev->video_limit) {
-		if (PAGE_ALIGN(*size) * *count > pcdev->video_limit)
-			*count = pcdev->video_limit / PAGE_ALIGN(*size);
+		if (PAGE_ALIGN(sizes[0]) * *count > pcdev->video_limit)
+			*count = pcdev->video_limit / PAGE_ALIGN(sizes[0]);
 	}
 
-	dev_dbg(icd->dev.parent, "count=%d, size=%d\n", *count, *size);
+	dev_dbg(icd->dev.parent, "count=%d, size=%lu\n", *count, sizes[0]);
 
 	return 0;
 }
 
-static void free_buffer(struct videobuf_queue *vq,
-			struct sh_mobile_ceu_buffer *buf)
-{
-	struct soc_camera_device *icd = vq->priv_data;
-	struct device *dev = icd->dev.parent;
-
-	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
-		&buf->vb, buf->vb.baddr, buf->vb.bsize);
-
-	if (in_interrupt())
-		BUG();
-
-	videobuf_waiton(vq, &buf->vb, 0, 0);
-	videobuf_dma_contig_free(vq, &buf->vb);
-	dev_dbg(dev, "%s freed\n", __func__);
-	buf->vb.state = VIDEOBUF_NEEDS_INIT;
-}
-
 #define CEU_CETCR_MAGIC 0x0317f313 /* acknowledge magical interrupt sources */
 #define CEU_CETCR_IGRW (1 << 4) /* prohibited register access interrupt bit */
 #define CEU_CEIER_CPEIE (1 << 0) /* one-frame capture end interrupt */
@@ -309,7 +302,10 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 		bottom2	= CDBCR;
 	}
 
-	phys_addr_top = videobuf_to_dma_contig(pcdev->active);
+	/* mem_ops->cookie must not be NULL */
+	phys_addr_top = (dma_addr_t)icd->vb2_vidq.mem_ops->cookie(pcdev->
+						active->planes[0].mem_priv);
+
 	ceu_write(pcdev, top1, phys_addr_top);
 	if (V4L2_FIELD_NONE != pcdev->field) {
 		phys_addr_bottom = phys_addr_top + icd->user_width;
@@ -330,87 +326,67 @@ static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
 		}
 	}
 
-	pcdev->active->state = VIDEOBUF_ACTIVE;
 	ceu_write(pcdev, CAPSR, 0x1); /* start capture */
 
 	return ret;
 }
 
-static int sh_mobile_ceu_videobuf_prepare(struct videobuf_queue *vq,
-					  struct videobuf_buffer *vb,
-					  enum v4l2_field field)
+static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
 	struct sh_mobile_ceu_buffer *buf;
 	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 						icd->current_fmt->host_fmt);
-	int ret;
+	unsigned long size;
 
 	if (bytes_per_line < 0)
 		return bytes_per_line;
 
-	buf = container_of(vb, struct sh_mobile_ceu_buffer, vb);
+	buf = to_ceu_vb(vb);
 
-	dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
-		vb, vb->baddr, vb->bsize);
+	dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
 	/* Added list head initialization on alloc */
-	WARN_ON(!list_empty(&vb->queue));
+	WARN(!list_empty(&buf->queue), "Buffer %p on queue!\n", vb);
 
 #ifdef DEBUG
 	/*
 	 * This can be useful if you want to see if we actually fill
 	 * the buffer with something
 	 */
-	memset((void *)vb->baddr, 0xaa, vb->bsize);
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
 #endif
 
 	BUG_ON(NULL == icd->current_fmt);
 
-	if (buf->code	!= icd->current_fmt->code ||
-	    vb->width	!= icd->user_width ||
-	    vb->height	!= icd->user_height ||
-	    vb->field	!= field) {
-		buf->code	= icd->current_fmt->code;
-		vb->width	= icd->user_width;
-		vb->height	= icd->user_height;
-		vb->field	= field;
-		vb->state	= VIDEOBUF_NEEDS_INIT;
-	}
+	size = icd->user_height * bytes_per_line;
 
-	vb->size = vb->height * bytes_per_line;
-	if (0 != vb->baddr && vb->bsize < vb->size) {
-		ret = -EINVAL;
-		goto out;
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(icd->dev.parent, "Buffer too small (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), size);
+		return -ENOBUFS;
 	}
 
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret)
-			goto fail;
-		vb->state = VIDEOBUF_PREPARED;
-	}
+	vb2_set_plane_payload(vb, 0, size);
 
 	return 0;
-fail:
-	free_buffer(vq, buf);
-out:
-	return ret;
 }
 
-/* Called under spinlock_irqsave(&pcdev->lock, ...) */
-static void sh_mobile_ceu_videobuf_queue(struct videobuf_queue *vq,
-					 struct videobuf_buffer *vb)
+static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
+	unsigned long flags;
 
-	dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%08lx %zd\n", __func__,
-		vb, vb->baddr, vb->bsize);
+	dev_dbg(icd->dev.parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
 
-	vb->state = VIDEOBUF_QUEUED;
-	list_add_tail(&vb->queue, &pcdev->capture);
+	spin_lock_irqsave(&pcdev->lock, flags);
+	list_add_tail(&buf->queue, &pcdev->capture);
 
 	if (!pcdev->active) {
 		/*
@@ -421,13 +397,14 @@ static void sh_mobile_ceu_videobuf_queue(struct videobuf_queue *vq,
 		pcdev->active = vb;
 		sh_mobile_ceu_capture(pcdev);
 	}
+	spin_unlock_irqrestore(&pcdev->lock, flags);
 }
 
-static void sh_mobile_ceu_videobuf_release(struct videobuf_queue *vq,
-					   struct videobuf_buffer *vb)
+static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_device *icd = container_of(vb->vb2_queue, struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vb);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
 	unsigned long flags;
 
@@ -439,53 +416,60 @@ static void sh_mobile_ceu_videobuf_release(struct videobuf_queue *vq,
 		pcdev->active = NULL;
 	}
 
-	if ((vb->state == VIDEOBUF_ACTIVE || vb->state == VIDEOBUF_QUEUED) &&
-	    !list_empty(&vb->queue)) {
-		vb->state = VIDEOBUF_ERROR;
-		list_del_init(&vb->queue);
-	}
+	/* Doesn't hurt also if the list is empty */
+	list_del_init(&buf->queue);
 
 	spin_unlock_irqrestore(&pcdev->lock, flags);
+}
 
-	free_buffer(vq, container_of(vb, struct sh_mobile_ceu_buffer, vb));
+static int sh_mobile_ceu_videobuf_init(struct vb2_buffer *vb)
+{
+	/* This is for locking debugging only */
+	INIT_LIST_HEAD(&to_ceu_vb(vb)->queue);
+	return 0;
 }
 
-static struct videobuf_queue_ops sh_mobile_ceu_videobuf_ops = {
-	.buf_setup      = sh_mobile_ceu_videobuf_setup,
-	.buf_prepare    = sh_mobile_ceu_videobuf_prepare,
-	.buf_queue      = sh_mobile_ceu_videobuf_queue,
-	.buf_release    = sh_mobile_ceu_videobuf_release,
+static struct vb2_ops sh_mobile_ceu_videobuf_ops = {
+	.queue_setup	= sh_mobile_ceu_videobuf_setup,
+	.buf_prepare	= sh_mobile_ceu_videobuf_prepare,
+	.buf_queue	= sh_mobile_ceu_videobuf_queue,
+	.buf_cleanup	= sh_mobile_ceu_videobuf_release,
+	.buf_init	= sh_mobile_ceu_videobuf_init,
+	.wait_prepare	= soc_camera_unlock,
+	.wait_finish	= soc_camera_lock,
 };
 
 static irqreturn_t sh_mobile_ceu_irq(int irq, void *data)
 {
 	struct sh_mobile_ceu_dev *pcdev = data;
-	struct videobuf_buffer *vb;
-	unsigned long flags;
+	struct vb2_buffer *vb;
+	int ret;
 
-	spin_lock_irqsave(&pcdev->lock, flags);
+	spin_lock(&pcdev->lock);
 
 	vb = pcdev->active;
 	if (!vb)
 		/* Stale interrupt from a released buffer */
 		goto out;
 
-	list_del_init(&vb->queue);
+	list_del_init(&to_ceu_vb(vb)->queue);
 
 	if (!list_empty(&pcdev->capture))
-		pcdev->active = list_entry(pcdev->capture.next,
-					   struct videobuf_buffer, queue);
+		pcdev->active = &list_entry(pcdev->capture.next,
+					    struct sh_mobile_ceu_buffer, queue)->vb;
 	else
 		pcdev->active = NULL;
 
-	vb->state = (sh_mobile_ceu_capture(pcdev) < 0) ?
-		VIDEOBUF_ERROR : VIDEOBUF_DONE;
-	do_gettimeofday(&vb->ts);
-	vb->field_count++;
-	wake_up(&vb->done);
+	ret = sh_mobile_ceu_capture(pcdev);
+	do_gettimeofday(&vb->v4l2_buf.timestamp);
+	if (!ret) {
+		vb->v4l2_buf.field = pcdev->field;
+		vb->v4l2_buf.sequence = pcdev->sequence++;
+	}
+	vb2_buffer_done(vb, ret < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 
 out:
-	spin_unlock_irqrestore(&pcdev->lock, flags);
+	spin_unlock(&pcdev->lock);
 
 	return IRQ_HANDLED;
 }
@@ -529,9 +513,8 @@ static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
 	/* make sure active buffer is canceled */
 	spin_lock_irqsave(&pcdev->lock, flags);
 	if (pcdev->active) {
-		list_del(&pcdev->active->queue);
-		pcdev->active->state = VIDEOBUF_ERROR;
-		wake_up_all(&pcdev->active->done);
+		list_del_init(&to_ceu_vb(pcdev->active)->queue);
+		vb2_buffer_done(pcdev->active, VB2_BUF_STATE_ERROR);
 		pcdev->active = NULL;
 	}
 	spin_unlock_irqrestore(&pcdev->lock, flags);
@@ -686,6 +669,7 @@ static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
 		ceu_write(pcdev, CAPSR, capsr);
 }
 
+/* Capture is not running, no interrupts, no locking needed */
 static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd,
 				       __u32 pixfmt)
 {
@@ -1364,7 +1348,7 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	struct device *dev = icd->dev.parent;
 	struct v4l2_mbus_framefmt mf;
 	unsigned int scale_cam_h, scale_cam_v, scale_ceu_h, scale_ceu_v,
-		out_width, out_height, scale_h, scale_v;
+		out_width, out_height;
 	int interm_width, interm_height;
 	u32 capsr, cflcr;
 	int ret;
@@ -1422,10 +1406,6 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	scale_ceu_h	= calc_scale(interm_width, &out_width);
 	scale_ceu_v	= calc_scale(interm_height, &out_height);
 
-	/* Calculate camera scales */
-	scale_h		= calc_generic_scale(cam_rect->width, out_width);
-	scale_v		= calc_generic_scale(cam_rect->height, out_height);
-
 	dev_geo(dev, "5: CEU scales %u:%u\n", scale_ceu_h, scale_ceu_v);
 
 	/* Apply CEU scales. */
@@ -1437,8 +1417,8 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 
 	icd->user_width	 = out_width;
 	icd->user_height = out_height;
-	cam->ceu_left	 = scale_down(rect->left - cam_rect->left, scale_h) & ~1;
-	cam->ceu_top	 = scale_down(rect->top - cam_rect->top, scale_v) & ~1;
+	cam->ceu_left	 = scale_down(rect->left - cam_rect->left, scale_cam_h) & ~1;
+	cam->ceu_top	 = scale_down(rect->top - cam_rect->top, scale_cam_v) & ~1;
 
 	/* 6. Use CEU cropping to crop to the new window. */
 	sh_mobile_ceu_set_rect(icd);
@@ -1449,7 +1429,7 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 		icd->user_width, icd->user_height,
 		cam->ceu_left, cam->ceu_top);
 
-	/* Restore capture */
+	/* Restore capture. The CE bit can be cleared by the hardware */
 	if (pcdev->active)
 		capsr |= 1;
 	capture_restore(pcdev, capsr);
@@ -1726,43 +1706,11 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 	return ret;
 }
 
-static int sh_mobile_ceu_reqbufs(struct soc_camera_device *icd,
-				 struct v4l2_requestbuffers *p)
-{
-	int i;
-
-	/*
-	 * This is for locking debugging only. I removed spinlocks and now I
-	 * check whether .prepare is ever called on a linked buffer, or whether
-	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
-	 * it hadn't triggered
-	 */
-	for (i = 0; i < p->count; i++) {
-		struct sh_mobile_ceu_buffer *buf;
-
-		buf = container_of(icd->vb_vidq.bufs[i],
-				   struct sh_mobile_ceu_buffer, vb);
-		INIT_LIST_HEAD(&buf->vb.queue);
-	}
-
-	return 0;
-}
-
 static unsigned int sh_mobile_ceu_poll(struct file *file, poll_table *pt)
 {
 	struct soc_camera_device *icd = file->private_data;
-	struct sh_mobile_ceu_buffer *buf;
-
-	buf = list_entry(icd->vb_vidq.stream.next,
-			 struct sh_mobile_ceu_buffer, vb.stream);
-
-	poll_wait(file, &buf->vb.done, pt);
 
-	if (buf->vb.state == VIDEOBUF_DONE ||
-	    buf->vb.state == VIDEOBUF_ERROR)
-		return POLLIN|POLLRDNORM;
-
-	return 0;
+	return vb2_poll(&icd->vb2_vidq, file, pt);
 }
 
 static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
@@ -1774,19 +1722,17 @@ static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
 	return 0;
 }
 
-static void sh_mobile_ceu_init_videobuf(struct videobuf_queue *q,
-					struct soc_camera_device *icd)
+static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
+				       struct soc_camera_device *icd)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
-	struct sh_mobile_ceu_dev *pcdev = ici->priv;
-
-	videobuf_queue_dma_contig_init(q,
-				       &sh_mobile_ceu_videobuf_ops,
-				       icd->dev.parent, &pcdev->lock,
-				       V4L2_BUF_TYPE_VIDEO_CAPTURE,
-				       pcdev->field,
-				       sizeof(struct sh_mobile_ceu_buffer),
-				       icd, &icd->video_lock);
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &sh_mobile_ceu_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct sh_mobile_ceu_buffer);
+
+	return vb2_queue_init(q);
 }
 
 static int sh_mobile_ceu_get_ctrl(struct soc_camera_device *icd,
@@ -1850,11 +1796,10 @@ static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
 	.try_fmt	= sh_mobile_ceu_try_fmt,
 	.set_ctrl	= sh_mobile_ceu_set_ctrl,
 	.get_ctrl	= sh_mobile_ceu_get_ctrl,
-	.reqbufs	= sh_mobile_ceu_reqbufs,
 	.poll		= sh_mobile_ceu_poll,
 	.querycap	= sh_mobile_ceu_querycap,
 	.set_bus_param	= sh_mobile_ceu_set_bus_param,
-	.init_videobuf	= sh_mobile_ceu_init_videobuf,
+	.init_videobuf2	= sh_mobile_ceu_init_videobuf,
 	.controls	= sh_mobile_ceu_controls,
 	.num_controls	= ARRAY_SIZE(sh_mobile_ceu_controls),
 };
@@ -2005,12 +1950,20 @@ static int __devinit sh_mobile_ceu_probe(struct platform_device *pdev)
 		}
 	}
 
+	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		err = PTR_ERR(pcdev->alloc_ctx);
+		goto exit_module_put;
+	}
+
 	err = soc_camera_host_register(&pcdev->ici);
 	if (err)
-		goto exit_module_put;
+		goto exit_free_ctx;
 
 	return 0;
 
+exit_free_ctx:
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 exit_module_put:
 	if (csi2 && csi2->driver)
 		module_put(csi2->driver->owner);
@@ -2041,6 +1994,7 @@ static int __devexit sh_mobile_ceu_remove(struct platform_device *pdev)
 	if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
 		dma_release_declared_memory(&pdev->dev);
 	iounmap(pcdev->base);
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
 	if (csi2 && csi2->driver)
 		module_put(csi2->driver->owner);
 	kfree(pcdev);
diff --git a/drivers/media/video/sn9c102/sn9c102_core.c b/drivers/media/video/sn9c102/sn9c102_core.c
index 84984f64b234..ce56a1cdbf0a 100644
--- a/drivers/media/video/sn9c102/sn9c102_core.c
+++ b/drivers/media/video/sn9c102/sn9c102_core.c
@@ -1430,9 +1430,9 @@ static DEVICE_ATTR(i2c_reg, S_IRUGO | S_IWUSR,
 		   sn9c102_show_i2c_reg, sn9c102_store_i2c_reg);
 static DEVICE_ATTR(i2c_val, S_IRUGO | S_IWUSR,
 		   sn9c102_show_i2c_val, sn9c102_store_i2c_val);
-static DEVICE_ATTR(green, S_IWUGO, NULL, sn9c102_store_green);
-static DEVICE_ATTR(blue, S_IWUGO, NULL, sn9c102_store_blue);
-static DEVICE_ATTR(red, S_IWUGO, NULL, sn9c102_store_red);
+static DEVICE_ATTR(green, S_IWUSR, NULL, sn9c102_store_green);
+static DEVICE_ATTR(blue, S_IWUSR, NULL, sn9c102_store_blue);
+static DEVICE_ATTR(red, S_IWUSR, NULL, sn9c102_store_red);
 static DEVICE_ATTR(frame_header, S_IRUGO, sn9c102_show_frame_header, NULL);
 
 
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index a66811b43710..fa80a4a914d4 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -34,6 +34,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-dev.h>
 #include <media/videobuf-core.h>
+#include <media/videobuf2-core.h>
 #include <media/soc_mediabus.h>
 
 /* Default to VGA resolution */
@@ -191,6 +192,15 @@ static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id *a)
 	return v4l2_subdev_call(sd, core, s_std, *a);
 }
 
+static int soc_camera_enum_fsizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+
+	return ici->ops->enum_fsizes(icd, fsize);
+}
+
 static int soc_camera_reqbufs(struct file *file, void *priv,
 			      struct v4l2_requestbuffers *p)
 {
@@ -203,11 +213,16 @@ static int soc_camera_reqbufs(struct file *file, void *priv,
 	if (icd->streamer && icd->streamer != file)
 		return -EBUSY;
 
-	ret = videobuf_reqbufs(&icd->vb_vidq, p);
-	if (ret < 0)
-		return ret;
+	if (ici->ops->init_videobuf) {
+		ret = videobuf_reqbufs(&icd->vb_vidq, p);
+		if (ret < 0)
+			return ret;
+
+		ret = ici->ops->reqbufs(icd, p);
+	} else {
+		ret = vb2_reqbufs(&icd->vb2_vidq, p);
+	}
 
-	ret = ici->ops->reqbufs(icd, p);
 	if (!ret && !icd->streamer)
 		icd->streamer = file;
 
@@ -218,36 +233,48 @@ static int soc_camera_querybuf(struct file *file, void *priv,
 			       struct v4l2_buffer *p)
 {
 	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
-	return videobuf_querybuf(&icd->vb_vidq, p);
+	if (ici->ops->init_videobuf)
+		return videobuf_querybuf(&icd->vb_vidq, p);
+	else
+		return vb2_querybuf(&icd->vb2_vidq, p);
 }
 
 static int soc_camera_qbuf(struct file *file, void *priv,
 			   struct v4l2_buffer *p)
 {
 	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
 	if (icd->streamer != file)
 		return -EBUSY;
 
-	return videobuf_qbuf(&icd->vb_vidq, p);
+	if (ici->ops->init_videobuf)
+		return videobuf_qbuf(&icd->vb_vidq, p);
+	else
+		return vb2_qbuf(&icd->vb2_vidq, p);
 }
 
 static int soc_camera_dqbuf(struct file *file, void *priv,
 			    struct v4l2_buffer *p)
 {
 	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
 	if (icd->streamer != file)
 		return -EBUSY;
 
-	return videobuf_dqbuf(&icd->vb_vidq, p, file->f_flags & O_NONBLOCK);
+	if (ici->ops->init_videobuf)
+		return videobuf_dqbuf(&icd->vb_vidq, p, file->f_flags & O_NONBLOCK);
+	else
+		return vb2_dqbuf(&icd->vb2_vidq, p, file->f_flags & O_NONBLOCK);
 }
 
 /* Always entered with .video_lock held */
@@ -363,8 +390,9 @@ static int soc_camera_set_fmt(struct soc_camera_device *icd,
 	icd->user_width		= pix->width;
 	icd->user_height	= pix->height;
 	icd->colorspace		= pix->colorspace;
-	icd->vb_vidq.field	=
-		icd->field	= pix->field;
+	icd->field		= pix->field;
+	if (ici->ops->init_videobuf)
+		icd->vb_vidq.field = pix->field;
 
 	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		dev_warn(&icd->dev, "Attention! Wrong buf-type %d\n",
@@ -444,7 +472,13 @@ static int soc_camera_open(struct file *file)
 		if (ret < 0)
 			goto esfmt;
 
-		ici->ops->init_videobuf(&icd->vb_vidq, icd);
+		if (ici->ops->init_videobuf) {
+			ici->ops->init_videobuf(&icd->vb_vidq, icd);
+		} else {
+			ret = ici->ops->init_videobuf2(&icd->vb2_vidq, icd);
+			if (ret < 0)
+				goto einitvb;
+		}
 	}
 
 	file->private_data = icd;
@@ -456,6 +490,7 @@ static int soc_camera_open(struct file *file)
 	 * First four errors are entered with the .video_lock held
 	 * and use_count == 1
 	 */
+einitvb:
 esfmt:
 	pm_runtime_disable(&icd->vdev->dev);
 eresume:
@@ -482,6 +517,8 @@ static int soc_camera_close(struct file *file)
 		pm_runtime_disable(&icd->vdev->dev);
 
 		ici->ops->remove(icd);
+		if (ici->ops->init_videobuf2)
+			vb2_queue_release(&icd->vb2_vidq);
 
 		soc_camera_power_set(icd, icl, 0);
 	}
@@ -510,6 +547,7 @@ static ssize_t soc_camera_read(struct file *file, char __user *buf,
 static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	int err;
 
 	dev_dbg(&icd->dev, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
@@ -517,7 +555,10 @@ static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
 	if (icd->streamer != file)
 		return -EBUSY;
 
-	err = videobuf_mmap_mapper(&icd->vb_vidq, vma);
+	if (ici->ops->init_videobuf)
+		err = videobuf_mmap_mapper(&icd->vb_vidq, vma);
+	else
+		err = vb2_mmap(&icd->vb2_vidq, vma);
 
 	dev_dbg(&icd->dev, "vma start=0x%08lx, size=%ld, ret=%d\n",
 		(unsigned long)vma->vm_start,
@@ -535,7 +576,7 @@ static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
 	if (icd->streamer != file)
 		return -EBUSY;
 
-	if (list_empty(&icd->vb_vidq.stream)) {
+	if (ici->ops->init_videobuf && list_empty(&icd->vb_vidq.stream)) {
 		dev_err(&icd->dev, "Trying to poll with no queued buffers!\n");
 		return POLLERR;
 	}
@@ -543,6 +584,20 @@ static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
 	return ici->ops->poll(file, pt);
 }
 
+void soc_camera_lock(struct vb2_queue *vq)
+{
+	struct soc_camera_device *icd = vb2_get_drv_priv(vq);
+	mutex_lock(&icd->video_lock);
+}
+EXPORT_SYMBOL(soc_camera_lock);
+
+void soc_camera_unlock(struct vb2_queue *vq)
+{
+	struct soc_camera_device *icd = vb2_get_drv_priv(vq);
+	mutex_unlock(&icd->video_lock);
+}
+EXPORT_SYMBOL(soc_camera_unlock);
+
 static struct v4l2_file_operations soc_camera_fops = {
 	.owner		= THIS_MODULE,
 	.open		= soc_camera_open,
@@ -606,7 +661,7 @@ static int soc_camera_g_fmt_vid_cap(struct file *file, void *priv,
 
 	pix->width		= icd->user_width;
 	pix->height		= icd->user_height;
-	pix->field		= icd->vb_vidq.field;
+	pix->field		= icd->field;
 	pix->pixelformat	= icd->current_fmt->host_fmt->fourcc;
 	pix->bytesperline	= soc_mbus_bytes_per_line(pix->width,
 						icd->current_fmt->host_fmt);
@@ -635,6 +690,7 @@ static int soc_camera_streamon(struct file *file, void *priv,
 			       enum v4l2_buf_type i)
 {
 	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	int ret;
 
@@ -646,10 +702,14 @@ static int soc_camera_streamon(struct file *file, void *priv,
 	if (icd->streamer != file)
 		return -EBUSY;
 
-	v4l2_subdev_call(sd, video, s_stream, 1);
-
 	/* This calls buf_queue from host driver's videobuf_queue_ops */
-	ret = videobuf_streamon(&icd->vb_vidq);
+	if (ici->ops->init_videobuf)
+		ret = videobuf_streamon(&icd->vb_vidq);
+	else
+		ret = vb2_streamon(&icd->vb2_vidq, i);
+
+	if (!ret)
+		v4l2_subdev_call(sd, video, s_stream, 1);
 
 	return ret;
 }
@@ -659,6 +719,7 @@ static int soc_camera_streamoff(struct file *file, void *priv,
 {
 	struct soc_camera_device *icd = file->private_data;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
 
 	WARN_ON(priv != file->private_data);
 
@@ -672,7 +733,10 @@ static int soc_camera_streamoff(struct file *file, void *priv,
 	 * This calls buf_release from host driver's videobuf_queue_ops for all
 	 * remaining buffers. When the last buffer is freed, stop capture
 	 */
-	videobuf_streamoff(&icd->vb_vidq);
+	if (ici->ops->init_videobuf)
+		videobuf_streamoff(&icd->vb_vidq);
+	else
+		vb2_streamoff(&icd->vb2_vidq, i);
 
 	v4l2_subdev_call(sd, video, s_stream, 0);
 
@@ -1175,6 +1239,31 @@ static int default_s_parm(struct soc_camera_device *icd,
 	return v4l2_subdev_call(sd, video, s_parm, parm);
 }
 
+static int default_enum_fsizes(struct soc_camera_device *icd,
+			  struct v4l2_frmsizeenum *fsize)
+{
+	int ret;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	__u32 pixfmt = fsize->pixel_format;
+	struct v4l2_frmsizeenum fsize_mbus = *fsize;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate)
+		return -EINVAL;
+	/* map xlate-code to pixel_format, sensor only handle xlate-code*/
+	fsize_mbus.pixel_format = xlate->code;
+
+	ret = v4l2_subdev_call(sd, video, enum_mbus_fsizes, &fsize_mbus);
+	if (ret < 0)
+		return ret;
+
+	*fsize = fsize_mbus;
+	fsize->pixel_format = pixfmt;
+
+	return 0;
+}
+
 static void soc_camera_device_init(struct device *dev, void *pdata)
 {
 	dev->platform_data	= pdata;
@@ -1192,8 +1281,9 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 	    !ici->ops->set_fmt ||
 	    !ici->ops->set_bus_param ||
 	    !ici->ops->querycap ||
-	    !ici->ops->init_videobuf ||
-	    !ici->ops->reqbufs ||
+	    ((!ici->ops->init_videobuf ||
+	      !ici->ops->reqbufs) &&
+	     !ici->ops->init_videobuf2) ||
 	    !ici->ops->add ||
 	    !ici->ops->remove ||
 	    !ici->ops->poll ||
@@ -1210,6 +1300,8 @@ int soc_camera_host_register(struct soc_camera_host *ici)
 		ici->ops->set_parm = default_s_parm;
 	if (!ici->ops->get_parm)
 		ici->ops->get_parm = default_g_parm;
+	if (!ici->ops->enum_fsizes)
+		ici->ops->enum_fsizes = default_enum_fsizes;
 
 	mutex_lock(&list_lock);
 	list_for_each_entry(ix, &hosts, list) {
@@ -1317,6 +1409,7 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 	.vidioc_g_input		 = soc_camera_g_input,
 	.vidioc_s_input		 = soc_camera_s_input,
 	.vidioc_s_std		 = soc_camera_s_std,
+	.vidioc_enum_framesizes  = soc_camera_enum_fsizes,
 	.vidioc_reqbufs		 = soc_camera_reqbufs,
 	.vidioc_try_fmt_vid_cap  = soc_camera_try_fmt_vid_cap,
 	.vidioc_querybuf	 = soc_camera_querybuf,
diff --git a/drivers/media/video/soc_mediabus.c b/drivers/media/video/soc_mediabus.c
index 91391214c682..73b4138709e4 100644
--- a/drivers/media/video/soc_mediabus.c
+++ b/drivers/media/video/soc_mediabus.c
@@ -132,6 +132,20 @@ static const struct soc_mbus_pixelfmt mbus_fmt[] = {
 	},
 };
 
+int soc_mbus_samples_per_pixel(const struct soc_mbus_pixelfmt *mf)
+{
+	switch (mf->packing) {
+	case SOC_MBUS_PACKING_NONE:
+	case SOC_MBUS_PACKING_EXTEND16:
+		return 1;
+	case SOC_MBUS_PACKING_2X8_PADHI:
+	case SOC_MBUS_PACKING_2X8_PADLO:
+		return 2;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL(soc_mbus_samples_per_pixel);
+
 s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
 {
 	switch (mf->packing) {
diff --git a/drivers/media/video/timblogiw.c b/drivers/media/video/timblogiw.c
index fc611ebeb82c..84d4c7c83435 100644
--- a/drivers/media/video/timblogiw.c
+++ b/drivers/media/video/timblogiw.c
@@ -24,6 +24,7 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/dmaengine.h>
+#include <linux/mfd/core.h>
 #include <linux/scatterlist.h>
 #include <linux/interrupt.h>
 #include <linux/list.h>
@@ -790,7 +791,7 @@ static int __devinit timblogiw_probe(struct platform_device *pdev)
 {
 	int err;
 	struct timblogiw *lw = NULL;
-	struct timb_video_platform_data *pdata = pdev->dev.platform_data;
+	struct timb_video_platform_data *pdata = mfd_get_data(pdev);
 
 	if (!pdata) {
 		dev_err(&pdev->dev, "No platform data\n");
diff --git a/drivers/media/video/tlv320aic23b.c b/drivers/media/video/tlv320aic23b.c
index dfc4dd7c5097..286ec7e7062a 100644
--- a/drivers/media/video/tlv320aic23b.c
+++ b/drivers/media/video/tlv320aic23b.c
@@ -31,6 +31,7 @@
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 
 MODULE_DESCRIPTION("tlv320aic23b driver");
 MODULE_AUTHOR("Scott Alfter, Ulf Eklund, Hans Verkuil");
@@ -41,7 +42,7 @@ MODULE_LICENSE("GPL");
 
 struct tlv320aic23b_state {
 	struct v4l2_subdev sd;
-	u8 muted;
+	struct v4l2_ctrl_handler hdl;
 };
 
 static inline struct tlv320aic23b_state *to_state(struct v4l2_subdev *sd)
@@ -49,6 +50,11 @@ static inline struct tlv320aic23b_state *to_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct tlv320aic23b_state, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tlv320aic23b_state, hdl)->sd;
+}
+
 static int tlv320aic23b_write(struct v4l2_subdev *sd, int reg, u16 val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -85,44 +91,44 @@ static int tlv320aic23b_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 	return 0;
 }
 
-static int tlv320aic23b_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct tlv320aic23b_state *state = to_state(sd);
-
-	if (ctrl->id != V4L2_CID_AUDIO_MUTE)
-		return -EINVAL;
-	ctrl->value = state->muted;
-	return 0;
-}
-
-static int tlv320aic23b_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tlv320aic23b_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct tlv320aic23b_state *state = to_state(sd);
-
-	if (ctrl->id != V4L2_CID_AUDIO_MUTE)
-		return -EINVAL;
-	state->muted = ctrl->value;
-	tlv320aic23b_write(sd, 0, 0x180); /* mute both channels */
-	/* set gain on both channels to +3.0 dB */
-	if (!state->muted)
-		tlv320aic23b_write(sd, 0, 0x119);
-	return 0;
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		tlv320aic23b_write(sd, 0, 0x180); /* mute both channels */
+		/* set gain on both channels to +3.0 dB */
+		if (!ctrl->val)
+			tlv320aic23b_write(sd, 0, 0x119);
+		return 0;
+	}
+	return -EINVAL;
 }
 
 static int tlv320aic23b_log_status(struct v4l2_subdev *sd)
 {
 	struct tlv320aic23b_state *state = to_state(sd);
 
-	v4l2_info(sd, "Input: %s\n", state->muted ? "muted" : "active");
+	v4l2_ctrl_handler_log_status(&state->hdl, sd->name);
 	return 0;
 }
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops tlv320aic23b_ctrl_ops = {
+	.s_ctrl = tlv320aic23b_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops tlv320aic23b_core_ops = {
 	.log_status = tlv320aic23b_log_status,
-	.g_ctrl = tlv320aic23b_g_ctrl,
-	.s_ctrl = tlv320aic23b_s_ctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 };
 
 static const struct v4l2_subdev_audio_ops tlv320aic23b_audio_ops = {
@@ -161,7 +167,6 @@ static int tlv320aic23b_probe(struct i2c_client *client,
 		return -ENOMEM;
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &tlv320aic23b_ops);
-	state->muted = 0;
 
 	/* Initialize tlv320aic23b */
 
@@ -177,15 +182,30 @@ static int tlv320aic23b_probe(struct i2c_client *client,
 	tlv320aic23b_write(sd, 8, 0x000);
 	/* activate digital interface */
 	tlv320aic23b_write(sd, 9, 0x001);
+
+	v4l2_ctrl_handler_init(&state->hdl, 1);
+	v4l2_ctrl_new_std(&state->hdl, &tlv320aic23b_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	sd->ctrl_handler = &state->hdl;
+	if (state->hdl.error) {
+		int err = state->hdl.error;
+
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->hdl);
 	return 0;
 }
 
 static int tlv320aic23b_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct tlv320aic23b_state *state = to_state(sd);
 
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_state(sd));
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
 	return 0;
 }
 
diff --git a/drivers/media/video/tuner-core.c b/drivers/media/video/tuner-core.c
index 1cec1224913f..9363ed91a4cb 100644
--- a/drivers/media/video/tuner-core.c
+++ b/drivers/media/video/tuner-core.c
@@ -1,7 +1,17 @@
 /*
- *
  * i2c tv tuner chip device driver
  * core core, i.e. kernel interfaces, registering and so on
+ *
+ * Copyright(c) by Ralph Metzler, Gerd Knorr, Gunther Mayer
+ *
+ * Copyright(c) 2005-2011 by Mauro Carvalho Chehab
+ *	- Added support for a separate Radio tuner
+ *	- Major rework and cleanups at the code
+ *
+ * This driver supports many devices and the idea is to let the driver
+ * detect which device is present. So rather than listing all supported
+ * devices here, we pretend to support a single, fake device type that will
+ * handle both radio and analog TV tuning.
  */
 
 #include <linux/module.h>
@@ -32,9 +42,111 @@
 
 #define UNSET (-1U)
 
-#define PREFIX t->i2c->driver->driver.name
+#define PREFIX (t->i2c->driver->driver.name)
+
+/*
+ * Driver modprobe parameters
+ */
+
+/* insmod options used at init time => read/only */
+static unsigned int addr;
+static unsigned int no_autodetect;
+static unsigned int show_i2c;
+
+module_param(addr, int, 0444);
+module_param(no_autodetect, int, 0444);
+module_param(show_i2c, int, 0444);
+
+/* insmod options used at runtime => read/write */
+static int tuner_debug;
+static unsigned int tv_range[2] = { 44, 958 };
+static unsigned int radio_range[2] = { 65, 108 };
+static char pal[] = "--";
+static char secam[] = "--";
+static char ntsc[] = "-";
+
+module_param_named(debug, tuner_debug, int, 0644);
+module_param_array(tv_range, int, NULL, 0644);
+module_param_array(radio_range, int, NULL, 0644);
+module_param_string(pal, pal, sizeof(pal), 0644);
+module_param_string(secam, secam, sizeof(secam), 0644);
+module_param_string(ntsc, ntsc, sizeof(ntsc), 0644);
+
+/*
+ * Static vars
+ */
+
+static LIST_HEAD(tuner_list);
+static const struct v4l2_subdev_ops tuner_ops;
+
+/*
+ * Debug macros
+ */
+
+#define tuner_warn(fmt, arg...) do {			\
+	printk(KERN_WARNING "%s %d-%04x: " fmt, PREFIX, \
+	       i2c_adapter_id(t->i2c->adapter),		\
+	       t->i2c->addr, ##arg);			\
+	 } while (0)
+
+#define tuner_info(fmt, arg...) do {			\
+	printk(KERN_INFO "%s %d-%04x: " fmt, PREFIX,	\
+	       i2c_adapter_id(t->i2c->adapter),		\
+	       t->i2c->addr, ##arg);			\
+	 } while (0)
+
+#define tuner_err(fmt, arg...) do {			\
+	printk(KERN_ERR "%s %d-%04x: " fmt, PREFIX,	\
+	       i2c_adapter_id(t->i2c->adapter),		\
+	       t->i2c->addr, ##arg);			\
+	 } while (0)
 
-/** This macro allows us to probe dynamically, avoiding static links */
+#define tuner_dbg(fmt, arg...) do {				\
+	if (tuner_debug)					\
+		printk(KERN_DEBUG "%s %d-%04x: " fmt, PREFIX,	\
+		       i2c_adapter_id(t->i2c->adapter),		\
+		       t->i2c->addr, ##arg);			\
+	 } while (0)
+
+/*
+ * Internal struct used inside the driver
+ */
+
+struct tuner {
+	/* device */
+	struct dvb_frontend fe;
+	struct i2c_client   *i2c;
+	struct v4l2_subdev  sd;
+	struct list_head    list;
+
+	/* keep track of the current settings */
+	v4l2_std_id         std;
+	unsigned int        tv_freq;
+	unsigned int        radio_freq;
+	unsigned int        audmode;
+
+	enum v4l2_tuner_type mode;
+	unsigned int        mode_mask; /* Combination of allowable modes */
+
+	bool                standby;	/* Standby mode */
+
+	unsigned int        type; /* chip type id */
+	unsigned int        config;
+	const char          *name;
+};
+
+/*
+ * Function prototypes
+ */
+
+static void set_tv_freq(struct i2c_client *c, unsigned int freq);
+static void set_radio_freq(struct i2c_client *c, unsigned int freq);
+
+/*
+ * tuner attach/detach logic
+ */
+
+/* This macro allows us to probe dynamically, avoiding static links */
 #ifdef CONFIG_MEDIA_ATTACH
 #define tuner_symbol_probe(FUNCTION, ARGS...) ({ \
 	int __r = -EINVAL; \
@@ -74,92 +186,15 @@ static void tuner_detach(struct dvb_frontend *fe)
 }
 #endif
 
-struct tuner {
-	/* device */
-	struct dvb_frontend fe;
-	struct i2c_client   *i2c;
-	struct v4l2_subdev  sd;
-	struct list_head    list;
-	unsigned int        using_v4l2:1;
-
-	/* keep track of the current settings */
-	v4l2_std_id         std;
-	unsigned int        tv_freq;
-	unsigned int        radio_freq;
-	unsigned int        audmode;
-
-	unsigned int        mode;
-	unsigned int        mode_mask; /* Combination of allowable modes */
-
-	unsigned int        type; /* chip type id */
-	unsigned int        config;
-	const char          *name;
-};
 
 static inline struct tuner *to_tuner(struct v4l2_subdev *sd)
 {
 	return container_of(sd, struct tuner, sd);
 }
 
-
-/* insmod options used at init time => read/only */
-static unsigned int addr;
-static unsigned int no_autodetect;
-static unsigned int show_i2c;
-
-/* insmod options used at runtime => read/write */
-static int tuner_debug;
-
-#define tuner_warn(fmt, arg...) do {			\
-	printk(KERN_WARNING "%s %d-%04x: " fmt, PREFIX, \
-	       i2c_adapter_id(t->i2c->adapter),		\
-	       t->i2c->addr, ##arg);			\
-	 } while (0)
-
-#define tuner_info(fmt, arg...) do {			\
-	printk(KERN_INFO "%s %d-%04x: " fmt, PREFIX,	\
-	       i2c_adapter_id(t->i2c->adapter),		\
-	       t->i2c->addr, ##arg);			\
-	 } while (0)
-
-#define tuner_err(fmt, arg...) do {			\
-	printk(KERN_ERR "%s %d-%04x: " fmt, PREFIX,	\
-	       i2c_adapter_id(t->i2c->adapter),		\
-	       t->i2c->addr, ##arg);			\
-	 } while (0)
-
-#define tuner_dbg(fmt, arg...) do {				\
-	if (tuner_debug)					\
-		printk(KERN_DEBUG "%s %d-%04x: " fmt, PREFIX,	\
-		       i2c_adapter_id(t->i2c->adapter),		\
-		       t->i2c->addr, ##arg);			\
-	 } while (0)
-
-/* ------------------------------------------------------------------------ */
-
-static unsigned int tv_range[2] = { 44, 958 };
-static unsigned int radio_range[2] = { 65, 108 };
-
-static char pal[] = "--";
-static char secam[] = "--";
-static char ntsc[] = "-";
-
-
-module_param(addr, int, 0444);
-module_param(no_autodetect, int, 0444);
-module_param(show_i2c, int, 0444);
-module_param_named(debug,tuner_debug, int, 0644);
-module_param_string(pal, pal, sizeof(pal), 0644);
-module_param_string(secam, secam, sizeof(secam), 0644);
-module_param_string(ntsc, ntsc, sizeof(ntsc), 0644);
-module_param_array(tv_range, int, NULL, 0644);
-module_param_array(radio_range, int, NULL, 0644);
-
-MODULE_DESCRIPTION("device driver for various TV and TV+FM radio tuners");
-MODULE_AUTHOR("Ralph Metzler, Gerd Knorr, Gunther Mayer");
-MODULE_LICENSE("GPL");
-
-/* ---------------------------------------------------------------------- */
+/*
+ * struct analog_demod_ops callbacks
+ */
 
 static void fe_set_params(struct dvb_frontend *fe,
 			  struct analog_parameters *params)
@@ -215,102 +250,25 @@ static struct analog_demod_ops tuner_analog_ops = {
 	.tuner_status   = tuner_status
 };
 
-/* Set tuner frequency,  freq in Units of 62.5kHz = 1/16MHz */
-static void set_tv_freq(struct i2c_client *c, unsigned int freq)
-{
-	struct tuner *t = to_tuner(i2c_get_clientdata(c));
-	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
-
-	struct analog_parameters params = {
-		.mode      = t->mode,
-		.audmode   = t->audmode,
-		.std       = t->std
-	};
-
-	if (t->type == UNSET) {
-		tuner_warn ("tuner type not set\n");
-		return;
-	}
-	if (NULL == analog_ops->set_params) {
-		tuner_warn ("Tuner has no way to set tv freq\n");
-		return;
-	}
-	if (freq < tv_range[0] * 16 || freq > tv_range[1] * 16) {
-		tuner_dbg ("TV freq (%d.%02d) out of range (%d-%d)\n",
-			   freq / 16, freq % 16 * 100 / 16, tv_range[0],
-			   tv_range[1]);
-		/* V4L2 spec: if the freq is not possible then the closest
-		   possible value should be selected */
-		if (freq < tv_range[0] * 16)
-			freq = tv_range[0] * 16;
-		else
-			freq = tv_range[1] * 16;
-	}
-	params.frequency = freq;
-
-	analog_ops->set_params(&t->fe, &params);
-}
-
-static void set_radio_freq(struct i2c_client *c, unsigned int freq)
-{
-	struct tuner *t = to_tuner(i2c_get_clientdata(c));
-	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
-
-	struct analog_parameters params = {
-		.mode      = t->mode,
-		.audmode   = t->audmode,
-		.std       = t->std
-	};
-
-	if (t->type == UNSET) {
-		tuner_warn ("tuner type not set\n");
-		return;
-	}
-	if (NULL == analog_ops->set_params) {
-		tuner_warn ("tuner has no way to set radio frequency\n");
-		return;
-	}
-	if (freq < radio_range[0] * 16000 || freq > radio_range[1] * 16000) {
-		tuner_dbg ("radio freq (%d.%02d) out of range (%d-%d)\n",
-			   freq / 16000, freq % 16000 * 100 / 16000,
-			   radio_range[0], radio_range[1]);
-		/* V4L2 spec: if the freq is not possible then the closest
-		   possible value should be selected */
-		if (freq < radio_range[0] * 16000)
-			freq = radio_range[0] * 16000;
-		else
-			freq = radio_range[1] * 16000;
-	}
-	params.frequency = freq;
-
-	analog_ops->set_params(&t->fe, &params);
-}
-
-static void set_freq(struct i2c_client *c, unsigned long freq)
-{
-	struct tuner *t = to_tuner(i2c_get_clientdata(c));
-
-	switch (t->mode) {
-	case V4L2_TUNER_RADIO:
-		tuner_dbg("radio freq set to %lu.%02lu\n",
-			  freq / 16000, freq % 16000 * 100 / 16000);
-		set_radio_freq(c, freq);
-		t->radio_freq = freq;
-		break;
-	case V4L2_TUNER_ANALOG_TV:
-	case V4L2_TUNER_DIGITAL_TV:
-		tuner_dbg("tv freq set to %lu.%02lu\n",
-			  freq / 16, freq % 16 * 100 / 16);
-		set_tv_freq(c, freq);
-		t->tv_freq = freq;
-		break;
-	default:
-		tuner_dbg("freq set: unknown mode: 0x%04x!\n",t->mode);
-	}
-}
-
-static struct xc5000_config xc5000_cfg;
+/*
+ * Functions to select between radio and TV and tuner probe/remove functions
+ */
 
+/**
+ * set_type - Sets the tuner type for a given device
+ *
+ * @c:			i2c_client descriptoy
+ * @type:		type of the tuner (e. g. tuner number)
+ * @new_mode_mask:	Indicates if tuner supports TV and/or Radio
+ * @new_config:		an optional parameter ranging from 0-255 used by
+			a few tuners to adjust an internal parameter,
+			like LNA mode
+ * @tuner_callback:	an optional function to be called when switching
+ *			to analog mode
+ *
+ * This function applys the tuner config to tuner specified
+ * by tun_setup structure. It contains several per-tuner initialization "magic"
+ */
 static void set_type(struct i2c_client *c, unsigned int type,
 		     unsigned int new_mode_mask, unsigned int new_config,
 		     int (*tuner_callback) (void *dev, int component, int cmd, int arg))
@@ -322,7 +280,7 @@ static void set_type(struct i2c_client *c, unsigned int type,
 	int tune_now = 1;
 
 	if (type == UNSET || type == TUNER_ABSENT) {
-		tuner_dbg ("tuner 0x%02x: Tuner type absent\n",c->addr);
+		tuner_dbg("tuner 0x%02x: Tuner type absent\n", c->addr);
 		return;
 	}
 
@@ -334,12 +292,6 @@ static void set_type(struct i2c_client *c, unsigned int type,
 		t->fe.callback = tuner_callback;
 	}
 
-	if (t->mode == T_UNINITIALIZED) {
-		tuner_dbg ("tuner 0x%02x: called during i2c_client register by adapter's attach_inform\n", c->addr);
-
-		return;
-	}
-
 	/* discard private data, in case set_type() was previously called */
 	tuner_detach(&t->fe);
 	t->fe.analog_demod_priv = NULL;
@@ -414,9 +366,12 @@ static void set_type(struct i2c_client *c, unsigned int type,
 		break;
 	case TUNER_XC5000:
 	{
-		xc5000_cfg.i2c_address	  = t->i2c->addr;
-		/* if_khz will be set when the digital dvb_attach() occurs */
-		xc5000_cfg.if_khz	  = 0;
+		struct xc5000_config xc5000_cfg = {
+			.i2c_address = t->i2c->addr,
+			/* if_khz will be set at dvb_attach() */
+			.if_khz	  = 0,
+		};
+
 		if (!dvb_attach(xc5000_attach,
 				&t->fe, t->i2c->adapter, &xc5000_cfg))
 			goto attach_failed;
@@ -459,8 +414,7 @@ static void set_type(struct i2c_client *c, unsigned int type,
 
 	tuner_dbg("type set to %s\n", t->name);
 
-	if (t->mode_mask == T_UNINITIALIZED)
-		t->mode_mask = new_mode_mask;
+	t->mode_mask = new_mode_mask;
 
 	/* Some tuners require more initialization setup before use,
 	   such as firmware download or device calibration.
@@ -468,9 +422,12 @@ static void set_type(struct i2c_client *c, unsigned int type,
 	   FIXME: better to move set_freq to the tuner code. This is needed
 	   on analog tuners for PLL to properly work
 	 */
-	if (tune_now)
-		set_freq(c, (V4L2_TUNER_RADIO == t->mode) ?
-			    t->radio_freq : t->tv_freq);
+	if (tune_now) {
+		if (V4L2_TUNER_RADIO == t->mode)
+			set_radio_freq(c, t->radio_freq);
+		else
+			set_tv_freq(c, t->tv_freq);
+	}
 
 	tuner_dbg("%s %s I2C addr 0x%02x with type %d used for 0x%02x\n",
 		  c->adapter->name, c->driver->driver.name, c->addr << 1, type,
@@ -480,86 +437,426 @@ static void set_type(struct i2c_client *c, unsigned int type,
 attach_failed:
 	tuner_dbg("Tuner attach for type = %d failed.\n", t->type);
 	t->type = TUNER_ABSENT;
-	t->mode_mask = T_UNINITIALIZED;
 
 	return;
 }
 
-/*
- * This function apply tuner config to tuner specified
- * by tun_setup structure. I addr is unset, then admin status
- * and tun addr status is more precise then current status,
- * it's applied. Otherwise status and type are applied only to
- * tuner with exactly the same addr.
-*/
-
-static void set_addr(struct i2c_client *c, struct tuner_setup *tun_setup)
+/**
+ * tuner_s_type_addr - Sets the tuner type for a device
+ *
+ * @sd:		subdev descriptor
+ * @tun_setup:	type to be associated to a given tuner i2c address
+ *
+ * This function applys the tuner config to tuner specified
+ * by tun_setup structure.
+ * If tuner I2C address is UNSET, then it will only set the device
+ * if the tuner supports the mode specified in the call.
+ * If the address is specified, the change will be applied only if
+ * tuner I2C address matches.
+ * The call can change the tuner number and the tuner mode.
+ */
+static int tuner_s_type_addr(struct v4l2_subdev *sd,
+			     struct tuner_setup *tun_setup)
 {
-	struct tuner *t = to_tuner(i2c_get_clientdata(c));
+	struct tuner *t = to_tuner(sd);
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
 
-	if ( (t->type == UNSET && ((tun_setup->addr == ADDR_UNSET) &&
-		(t->mode_mask & tun_setup->mode_mask))) ||
-		(tun_setup->addr == c->addr)) {
-			set_type(c, tun_setup->type, tun_setup->mode_mask,
-				 tun_setup->config, tun_setup->tuner_callback);
+	tuner_dbg("Calling set_type_addr for type=%d, addr=0x%02x, mode=0x%02x, config=0x%02x\n",
+			tun_setup->type,
+			tun_setup->addr,
+			tun_setup->mode_mask,
+			tun_setup->config);
+
+	if ((t->type == UNSET && ((tun_setup->addr == ADDR_UNSET) &&
+	    (t->mode_mask & tun_setup->mode_mask))) ||
+	    (tun_setup->addr == c->addr)) {
+		set_type(c, tun_setup->type, tun_setup->mode_mask,
+			 tun_setup->config, tun_setup->tuner_callback);
 	} else
 		tuner_dbg("set addr discarded for type %i, mask %x. "
 			  "Asked to change tuner at addr 0x%02x, with mask %x\n",
 			  t->type, t->mode_mask,
 			  tun_setup->addr, tun_setup->mode_mask);
+
+	return 0;
 }
 
-static inline int check_mode(struct tuner *t, char *cmd)
+/**
+ * tuner_s_config - Sets tuner configuration
+ *
+ * @sd:		subdev descriptor
+ * @cfg:	tuner configuration
+ *
+ * Calls tuner set_config() private function to set some tuner-internal
+ * parameters
+ */
+static int tuner_s_config(struct v4l2_subdev *sd,
+			  const struct v4l2_priv_tun_config *cfg)
 {
-	if ((1 << t->mode & t->mode_mask) == 0) {
-		return -EINVAL;
+	struct tuner *t = to_tuner(sd);
+	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
+
+	if (t->type != cfg->tuner)
+		return 0;
+
+	if (analog_ops->set_config) {
+		analog_ops->set_config(&t->fe, cfg->priv);
+		return 0;
 	}
 
-	switch (t->mode) {
-	case V4L2_TUNER_RADIO:
-		tuner_dbg("Cmd %s accepted for radio\n", cmd);
-		break;
-	case V4L2_TUNER_ANALOG_TV:
-		tuner_dbg("Cmd %s accepted for analog TV\n", cmd);
-		break;
-	case V4L2_TUNER_DIGITAL_TV:
-		tuner_dbg("Cmd %s accepted for digital TV\n", cmd);
-		break;
+	tuner_dbg("Tuner frontend module has no way to set config\n");
+	return 0;
+}
+
+/**
+ * tuner_lookup - Seek for tuner adapters
+ *
+ * @adap:	i2c_adapter struct
+ * @radio:	pointer to be filled if the adapter is radio
+ * @tv:		pointer to be filled if the adapter is TV
+ *
+ * Search for existing radio and/or TV tuners on the given I2C adapter,
+ * discarding demod-only adapters (tda9887).
+ *
+ * Note that when this function is called from tuner_probe you can be
+ * certain no other devices will be added/deleted at the same time, I2C
+ * core protects against that.
+ */
+static void tuner_lookup(struct i2c_adapter *adap,
+		struct tuner **radio, struct tuner **tv)
+{
+	struct tuner *pos;
+
+	*radio = NULL;
+	*tv = NULL;
+
+	list_for_each_entry(pos, &tuner_list, list) {
+		int mode_mask;
+
+		if (pos->i2c->adapter != adap ||
+		    strcmp(pos->i2c->driver->driver.name, "tuner"))
+			continue;
+
+		mode_mask = pos->mode_mask;
+		if (*radio == NULL && mode_mask == T_RADIO)
+			*radio = pos;
+		/* Note: currently TDA9887 is the only demod-only
+		   device. If other devices appear then we need to
+		   make this test more general. */
+		else if (*tv == NULL && pos->type != TUNER_TDA9887 &&
+			 (pos->mode_mask & T_ANALOG_TV))
+			*tv = pos;
+	}
+}
+
+/**
+ *tuner_probe - Probes the existing tuners on an I2C bus
+ *
+ * @client:	i2c_client descriptor
+ * @id:		not used
+ *
+ * This routine probes for tuners at the expected I2C addresses. On most
+ * cases, if a device answers to a given I2C address, it assumes that the
+ * device is a tuner. On a few cases, however, an additional logic is needed
+ * to double check if the device is really a tuner, or to identify the tuner
+ * type, like on tea5767/5761 devices.
+ *
+ * During client attach, set_type is called by adapter's attach_inform callback.
+ * set_type must then be completed by tuner_probe.
+ */
+static int tuner_probe(struct i2c_client *client,
+		       const struct i2c_device_id *id)
+{
+	struct tuner *t;
+	struct tuner *radio;
+	struct tuner *tv;
+
+	t = kzalloc(sizeof(struct tuner), GFP_KERNEL);
+	if (NULL == t)
+		return -ENOMEM;
+	v4l2_i2c_subdev_init(&t->sd, client, &tuner_ops);
+	t->i2c = client;
+	t->name = "(tuner unset)";
+	t->type = UNSET;
+	t->audmode = V4L2_TUNER_MODE_STEREO;
+	t->standby = 1;
+	t->radio_freq = 87.5 * 16000;	/* Initial freq range */
+	t->tv_freq = 400 * 16; /* Sets freq to VHF High - needed for some PLL's to properly start */
+
+	if (show_i2c) {
+		unsigned char buffer[16];
+		int i, rc;
+
+		memset(buffer, 0, sizeof(buffer));
+		rc = i2c_master_recv(client, buffer, sizeof(buffer));
+		tuner_info("I2C RECV = ");
+		for (i = 0; i < rc; i++)
+			printk(KERN_CONT "%02x ", buffer[i]);
+		printk("\n");
+	}
+
+	/* autodetection code based on the i2c addr */
+	if (!no_autodetect) {
+		switch (client->addr) {
+		case 0x10:
+			if (tuner_symbol_probe(tea5761_autodetection,
+					       t->i2c->adapter,
+					       t->i2c->addr) >= 0) {
+				t->type = TUNER_TEA5761;
+				t->mode_mask = T_RADIO;
+				tuner_lookup(t->i2c->adapter, &radio, &tv);
+				if (tv)
+					tv->mode_mask &= ~T_RADIO;
+
+				goto register_client;
+			}
+			kfree(t);
+			return -ENODEV;
+		case 0x42:
+		case 0x43:
+		case 0x4a:
+		case 0x4b:
+			/* If chip is not tda8290, don't register.
+			   since it can be tda9887*/
+			if (tuner_symbol_probe(tda829x_probe, t->i2c->adapter,
+					       t->i2c->addr) >= 0) {
+				tuner_dbg("tda829x detected\n");
+			} else {
+				/* Default is being tda9887 */
+				t->type = TUNER_TDA9887;
+				t->mode_mask = T_RADIO | T_ANALOG_TV;
+				goto register_client;
+			}
+			break;
+		case 0x60:
+			if (tuner_symbol_probe(tea5767_autodetection,
+					       t->i2c->adapter, t->i2c->addr)
+					>= 0) {
+				t->type = TUNER_TEA5767;
+				t->mode_mask = T_RADIO;
+				/* Sets freq to FM range */
+				tuner_lookup(t->i2c->adapter, &radio, &tv);
+				if (tv)
+					tv->mode_mask &= ~T_RADIO;
+
+				goto register_client;
+			}
+			break;
+		}
+	}
+
+	/* Initializes only the first TV tuner on this adapter. Why only the
+	   first? Because there are some devices (notably the ones with TI
+	   tuners) that have more than one i2c address for the *same* device.
+	   Experience shows that, except for just one case, the first
+	   address is the right one. The exception is a Russian tuner
+	   (ACORP_Y878F). So, the desired behavior is just to enable the
+	   first found TV tuner. */
+	tuner_lookup(t->i2c->adapter, &radio, &tv);
+	if (tv == NULL) {
+		t->mode_mask = T_ANALOG_TV;
+		if (radio == NULL)
+			t->mode_mask |= T_RADIO;
+		tuner_dbg("Setting mode_mask to 0x%02x\n", t->mode_mask);
+	}
+
+	/* Should be just before return */
+register_client:
+	/* Sets a default mode */
+	if (t->mode_mask & T_ANALOG_TV)
+		t->mode = V4L2_TUNER_ANALOG_TV;
+	else
+		t->mode = V4L2_TUNER_RADIO;
+	set_type(client, t->type, t->mode_mask, t->config, t->fe.callback);
+	list_add_tail(&t->list, &tuner_list);
+
+	tuner_info("Tuner %d found with type(s)%s%s.\n",
+		   t->type,
+		   t->mode_mask & T_RADIO ? " Radio" : "",
+		   t->mode_mask & T_ANALOG_TV ? " TV" : "");
+	return 0;
+}
+
+/**
+ * tuner_remove - detaches a tuner
+ *
+ * @client:	i2c_client descriptor
+ */
+
+static int tuner_remove(struct i2c_client *client)
+{
+	struct tuner *t = to_tuner(i2c_get_clientdata(client));
+
+	v4l2_device_unregister_subdev(&t->sd);
+	tuner_detach(&t->fe);
+	t->fe.analog_demod_priv = NULL;
+
+	list_del(&t->list);
+	kfree(t);
+	return 0;
+}
+
+/*
+ * Functions to switch between Radio and TV
+ *
+ * A few cards have a separate I2C tuner for radio. Those routines
+ * take care of switching between TV/Radio mode, filtering only the
+ * commands that apply to the Radio or TV tuner.
+ */
+
+/**
+ * check_mode - Verify if tuner supports the requested mode
+ * @t: a pointer to the module's internal struct_tuner
+ *
+ * This function checks if the tuner is capable of tuning analog TV,
+ * digital TV or radio, depending on what the caller wants. If the
+ * tuner can't support that mode, it returns -EINVAL. Otherwise, it
+ * returns 0.
+ * This function is needed for boards that have a separate tuner for
+ * radio (like devices with tea5767).
+ */
+static inline int check_mode(struct tuner *t, enum v4l2_tuner_type mode)
+{
+	if ((1 << mode & t->mode_mask) == 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+/**
+ * set_mode_freq - Switch tuner to other mode.
+ * @client:	struct i2c_client pointer
+ * @t:		a pointer to the module's internal struct_tuner
+ * @mode:	enum v4l2_type (radio or TV)
+ * @freq:	frequency to set (0 means to use the previous one)
+ *
+ * If tuner doesn't support the needed mode (radio or TV), prints a
+ * debug message and returns -EINVAL, changing its state to standby.
+ * Otherwise, changes the state and sets frequency to the last value, if
+ * the tuner can sleep or if it supports both Radio and TV.
+ */
+static int set_mode_freq(struct i2c_client *client, struct tuner *t,
+			 enum v4l2_tuner_type mode, unsigned int freq)
+{
+	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
+
+	if (mode != t->mode) {
+		if (check_mode(t, mode) == -EINVAL) {
+			tuner_dbg("Tuner doesn't support mode %d. "
+				  "Putting tuner to sleep\n", mode);
+			t->standby = true;
+			if (analog_ops->standby)
+				analog_ops->standby(&t->fe);
+			return -EINVAL;
+		}
+		t->mode = mode;
+		tuner_dbg("Changing to mode %d\n", mode);
 	}
+	if (t->mode == V4L2_TUNER_RADIO) {
+		if (freq)
+			t->radio_freq = freq;
+		set_radio_freq(client, t->radio_freq);
+	} else {
+		if (freq)
+			t->tv_freq = freq;
+		set_tv_freq(client, t->tv_freq);
+	}
+
 	return 0;
 }
 
-/* get more precise norm info from insmod option */
+/*
+ * Functions that are specific for TV mode
+ */
+
+/**
+ * set_tv_freq - Set tuner frequency,  freq in Units of 62.5 kHz = 1/16MHz
+ *
+ * @c:	i2c_client descriptor
+ * @freq: frequency
+ */
+static void set_tv_freq(struct i2c_client *c, unsigned int freq)
+{
+	struct tuner *t = to_tuner(i2c_get_clientdata(c));
+	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
+
+	struct analog_parameters params = {
+		.mode      = t->mode,
+		.audmode   = t->audmode,
+		.std       = t->std
+	};
+
+	if (t->type == UNSET) {
+		tuner_warn("tuner type not set\n");
+		return;
+	}
+	if (NULL == analog_ops->set_params) {
+		tuner_warn("Tuner has no way to set tv freq\n");
+		return;
+	}
+	if (freq < tv_range[0] * 16 || freq > tv_range[1] * 16) {
+		tuner_dbg("TV freq (%d.%02d) out of range (%d-%d)\n",
+			   freq / 16, freq % 16 * 100 / 16, tv_range[0],
+			   tv_range[1]);
+		/* V4L2 spec: if the freq is not possible then the closest
+		   possible value should be selected */
+		if (freq < tv_range[0] * 16)
+			freq = tv_range[0] * 16;
+		else
+			freq = tv_range[1] * 16;
+	}
+	params.frequency = freq;
+	tuner_dbg("tv freq set to %d.%02d\n",
+			freq / 16, freq % 16 * 100 / 16);
+	t->tv_freq = freq;
+	t->standby = false;
+
+	analog_ops->set_params(&t->fe, &params);
+}
+
+/**
+ * tuner_fixup_std - force a given video standard variant
+ *
+ * @t:	tuner internal struct
+ *
+ * A few devices or drivers have problem to detect some standard variations.
+ * On other operational systems, the drivers generally have a per-country
+ * code, and some logic to apply per-country hacks. V4L2 API doesn't provide
+ * such hacks. Instead, it relies on a proper video standard selection from
+ * the userspace application. However, as some apps are buggy, not allowing
+ * to distinguish all video standard variations, a modprobe parameter can
+ * be used to force a video standard match.
+ */
 static int tuner_fixup_std(struct tuner *t)
 {
 	if ((t->std & V4L2_STD_PAL) == V4L2_STD_PAL) {
 		switch (pal[0]) {
 		case '6':
-			tuner_dbg ("insmod fixup: PAL => PAL-60\n");
+			tuner_dbg("insmod fixup: PAL => PAL-60\n");
 			t->std = V4L2_STD_PAL_60;
 			break;
 		case 'b':
 		case 'B':
 		case 'g':
 		case 'G':
-			tuner_dbg ("insmod fixup: PAL => PAL-BG\n");
+			tuner_dbg("insmod fixup: PAL => PAL-BG\n");
 			t->std = V4L2_STD_PAL_BG;
 			break;
 		case 'i':
 		case 'I':
-			tuner_dbg ("insmod fixup: PAL => PAL-I\n");
+			tuner_dbg("insmod fixup: PAL => PAL-I\n");
 			t->std = V4L2_STD_PAL_I;
 			break;
 		case 'd':
 		case 'D':
 		case 'k':
 		case 'K':
-			tuner_dbg ("insmod fixup: PAL => PAL-DK\n");
+			tuner_dbg("insmod fixup: PAL => PAL-DK\n");
 			t->std = V4L2_STD_PAL_DK;
 			break;
 		case 'M':
 		case 'm':
-			tuner_dbg ("insmod fixup: PAL => PAL-M\n");
+			tuner_dbg("insmod fixup: PAL => PAL-M\n");
 			t->std = V4L2_STD_PAL_M;
 			break;
 		case 'N':
@@ -568,7 +865,7 @@ static int tuner_fixup_std(struct tuner *t)
 				tuner_dbg("insmod fixup: PAL => PAL-Nc\n");
 				t->std = V4L2_STD_PAL_Nc;
 			} else {
-				tuner_dbg ("insmod fixup: PAL => PAL-N\n");
+				tuner_dbg("insmod fixup: PAL => PAL-N\n");
 				t->std = V4L2_STD_PAL_N;
 			}
 			break;
@@ -576,7 +873,7 @@ static int tuner_fixup_std(struct tuner *t)
 			/* default parameter, do nothing */
 			break;
 		default:
-			tuner_warn ("pal= argument not recognised\n");
+			tuner_warn("pal= argument not recognised\n");
 			break;
 		}
 	}
@@ -589,22 +886,24 @@ static int tuner_fixup_std(struct tuner *t)
 		case 'h':
 		case 'H':
 			tuner_dbg("insmod fixup: SECAM => SECAM-BGH\n");
-			t->std = V4L2_STD_SECAM_B | V4L2_STD_SECAM_G | V4L2_STD_SECAM_H;
+			t->std = V4L2_STD_SECAM_B |
+				 V4L2_STD_SECAM_G |
+				 V4L2_STD_SECAM_H;
 			break;
 		case 'd':
 		case 'D':
 		case 'k':
 		case 'K':
-			tuner_dbg ("insmod fixup: SECAM => SECAM-DK\n");
+			tuner_dbg("insmod fixup: SECAM => SECAM-DK\n");
 			t->std = V4L2_STD_SECAM_DK;
 			break;
 		case 'l':
 		case 'L':
-			if ((secam[1]=='C')||(secam[1]=='c')) {
-				tuner_dbg ("insmod fixup: SECAM => SECAM-L'\n");
+			if ((secam[1] == 'C') || (secam[1] == 'c')) {
+				tuner_dbg("insmod fixup: SECAM => SECAM-L'\n");
 				t->std = V4L2_STD_SECAM_LC;
 			} else {
-				tuner_dbg ("insmod fixup: SECAM => SECAM-L\n");
+				tuner_dbg("insmod fixup: SECAM => SECAM-L\n");
 				t->std = V4L2_STD_SECAM_L;
 			}
 			break;
@@ -612,7 +911,7 @@ static int tuner_fixup_std(struct tuner *t)
 			/* default parameter, do nothing */
 			break;
 		default:
-			tuner_warn ("secam= argument not recognised\n");
+			tuner_warn("secam= argument not recognised\n");
 			break;
 		}
 	}
@@ -645,6 +944,66 @@ static int tuner_fixup_std(struct tuner *t)
 	return 0;
 }
 
+/*
+ * Functions that are specific for Radio mode
+ */
+
+/**
+ * set_radio_freq - Set tuner frequency,  freq in Units of 62.5 Hz  = 1/16kHz
+ *
+ * @c:	i2c_client descriptor
+ * @freq: frequency
+ */
+static void set_radio_freq(struct i2c_client *c, unsigned int freq)
+{
+	struct tuner *t = to_tuner(i2c_get_clientdata(c));
+	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
+
+	struct analog_parameters params = {
+		.mode      = t->mode,
+		.audmode   = t->audmode,
+		.std       = t->std
+	};
+
+	if (t->type == UNSET) {
+		tuner_warn("tuner type not set\n");
+		return;
+	}
+	if (NULL == analog_ops->set_params) {
+		tuner_warn("tuner has no way to set radio frequency\n");
+		return;
+	}
+	if (freq < radio_range[0] * 16000 || freq > radio_range[1] * 16000) {
+		tuner_dbg("radio freq (%d.%02d) out of range (%d-%d)\n",
+			   freq / 16000, freq % 16000 * 100 / 16000,
+			   radio_range[0], radio_range[1]);
+		/* V4L2 spec: if the freq is not possible then the closest
+		   possible value should be selected */
+		if (freq < radio_range[0] * 16000)
+			freq = radio_range[0] * 16000;
+		else
+			freq = radio_range[1] * 16000;
+	}
+	params.frequency = freq;
+	tuner_dbg("radio freq set to %d.%02d\n",
+			freq / 16000, freq % 16000 * 100 / 16000);
+	t->radio_freq = freq;
+	t->standby = false;
+
+	analog_ops->set_params(&t->fe, &params);
+}
+
+/*
+ * Debug function for reporting tuner status to userspace
+ */
+
+/**
+ * tuner_status - Dumps the current tuner status at dmesg
+ * @fe: pointer to struct dvb_frontend
+ *
+ * This callback is used only for driver debug purposes, answering to
+ * VIDIOC_LOG_STATUS. No changes should happen on this call.
+ */
 static void tuner_status(struct dvb_frontend *fe)
 {
 	struct tuner *t = fe->analog_demod_priv;
@@ -654,10 +1013,16 @@ static void tuner_status(struct dvb_frontend *fe)
 	const char *p;
 
 	switch (t->mode) {
-		case V4L2_TUNER_RADIO: 	    p = "radio"; break;
-		case V4L2_TUNER_ANALOG_TV:  p = "analog TV"; break;
-		case V4L2_TUNER_DIGITAL_TV: p = "digital TV"; break;
-		default: p = "undefined"; break;
+	case V4L2_TUNER_RADIO:
+		p = "radio";
+		break;
+	case V4L2_TUNER_DIGITAL_TV:
+		p = "digital TV";
+		break;
+	case V4L2_TUNER_ANALOG_TV:
+	default:
+		p = "analog TV";
+		break;
 	}
 	if (t->mode == V4L2_TUNER_RADIO) {
 		freq = t->radio_freq / 16000;
@@ -666,11 +1031,12 @@ static void tuner_status(struct dvb_frontend *fe)
 		freq = t->tv_freq / 16;
 		freq_fraction = (t->tv_freq % 16) * 100 / 16;
 	}
-	tuner_info("Tuner mode:      %s\n", p);
+	tuner_info("Tuner mode:      %s%s\n", p,
+		   t->standby ? " on standby mode" : "");
 	tuner_info("Frequency:       %lu.%02lu MHz\n", freq, freq_fraction);
 	tuner_info("Standard:        0x%08lx\n", (unsigned long)t->std);
 	if (t->mode != V4L2_TUNER_RADIO)
-	       return;
+		return;
 	if (fe_tuner_ops->get_status) {
 		u32 tuner_status;
 
@@ -683,132 +1049,58 @@ static void tuner_status(struct dvb_frontend *fe)
 	if (analog_ops->has_signal)
 		tuner_info("Signal strength: %d\n",
 			   analog_ops->has_signal(fe));
-	if (analog_ops->is_stereo)
-		tuner_info("Stereo:          %s\n",
-			   analog_ops->is_stereo(fe) ? "yes" : "no");
 }
 
-/* ---------------------------------------------------------------------- */
-
 /*
- * Switch tuner to other mode. If tuner support both tv and radio,
- * set another frequency to some value (This is needed for some pal
- * tuners to avoid locking). Otherwise, just put second tuner in
- * standby mode.
+ * Function to splicitly change mode to radio. Probably not needed anymore
  */
 
-static inline int set_mode(struct i2c_client *client, struct tuner *t, int mode, char *cmd)
-{
-	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
-
-	if (mode == t->mode)
-		return 0;
-
-	t->mode = mode;
-
-	if (check_mode(t, cmd) == -EINVAL) {
-		tuner_dbg("Tuner doesn't support this mode. "
-			  "Putting tuner to sleep\n");
-		t->mode = T_STANDBY;
-		if (analog_ops->standby)
-			analog_ops->standby(&t->fe);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-#define switch_v4l2()	if (!t->using_v4l2) \
-			    tuner_dbg("switching to v4l2\n"); \
-			t->using_v4l2 = 1;
-
-static inline int check_v4l2(struct tuner *t)
-{
-	/* bttv still uses both v4l1 and v4l2 calls to the tuner (v4l2 for
-	   TV, v4l1 for radio), until that is fixed this code is disabled.
-	   Otherwise the radio (v4l1) wouldn't tune after using the TV (v4l2)
-	   first. */
-	return 0;
-}
-
-static int tuner_s_type_addr(struct v4l2_subdev *sd, struct tuner_setup *type)
-{
-	struct tuner *t = to_tuner(sd);
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-
-	tuner_dbg("Calling set_type_addr for type=%d, addr=0x%02x, mode=0x%02x, config=0x%02x\n",
-			type->type,
-			type->addr,
-			type->mode_mask,
-			type->config);
-
-	set_addr(client, type);
-	return 0;
-}
-
 static int tuner_s_radio(struct v4l2_subdev *sd)
 {
 	struct tuner *t = to_tuner(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-	if (set_mode(client, t, V4L2_TUNER_RADIO, "s_radio") == -EINVAL)
+	if (set_mode_freq(client, t, V4L2_TUNER_RADIO, 0) == -EINVAL)
 		return 0;
-	if (t->radio_freq)
-		set_freq(client, t->radio_freq);
 	return 0;
 }
 
+/*
+ * Tuner callbacks to handle userspace ioctl's
+ */
+
+/**
+ * tuner_s_power - controls the power state of the tuner
+ * @sd: pointer to struct v4l2_subdev
+ * @on: a zero value puts the tuner to sleep
+ */
 static int tuner_s_power(struct v4l2_subdev *sd, int on)
 {
 	struct tuner *t = to_tuner(sd);
 	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
 
+	/* FIXME: Why this function don't wake the tuner if on != 0 ? */
 	if (on)
 		return 0;
 
 	tuner_dbg("Putting tuner to sleep\n");
-
-	if (check_mode(t, "s_power") == -EINVAL)
-		return 0;
-	t->mode = T_STANDBY;
+	t->standby = true;
 	if (analog_ops->standby)
 		analog_ops->standby(&t->fe);
 	return 0;
 }
 
-static int tuner_s_config(struct v4l2_subdev *sd, const struct v4l2_priv_tun_config *cfg)
-{
-	struct tuner *t = to_tuner(sd);
-	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
-
-	if (t->type != cfg->tuner)
-		return 0;
-
-	if (analog_ops->set_config) {
-		analog_ops->set_config(&t->fe, cfg->priv);
-		return 0;
-	}
-
-	tuner_dbg("Tuner frontend module has no way to set config\n");
-	return 0;
-}
-
-/* --- v4l ioctls --- */
-/* take care: bttv does userspace copying, we'll get a
-   kernel pointer here... */
 static int tuner_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 {
 	struct tuner *t = to_tuner(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-	if (set_mode(client, t, V4L2_TUNER_ANALOG_TV, "s_std") == -EINVAL)
+	if (set_mode_freq(client, t, V4L2_TUNER_ANALOG_TV, 0) == -EINVAL)
 		return 0;
 
-	switch_v4l2();
-
 	t->std = std;
 	tuner_fixup_std(t);
-	if (t->tv_freq)
-		set_freq(client, t->tv_freq);
+
 	return 0;
 }
 
@@ -817,10 +1109,8 @@ static int tuner_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 	struct tuner *t = to_tuner(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-	if (set_mode(client, t, f->type, "s_frequency") == -EINVAL)
+	if (set_mode_freq(client, t, f->type, f->frequency) == -EINVAL)
 		return 0;
-	switch_v4l2();
-	set_freq(client, f->frequency);
 
 	return 0;
 }
@@ -830,21 +1120,20 @@ static int tuner_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 	struct tuner *t = to_tuner(sd);
 	struct dvb_tuner_ops *fe_tuner_ops = &t->fe.ops.tuner_ops;
 
-	if (check_mode(t, "g_frequency") == -EINVAL)
+	if (check_mode(t, f->type) == -EINVAL)
 		return 0;
-	switch_v4l2();
 	f->type = t->mode;
-	if (fe_tuner_ops->get_frequency) {
+	if (fe_tuner_ops->get_frequency && !t->standby) {
 		u32 abs_freq;
 
 		fe_tuner_ops->get_frequency(&t->fe, &abs_freq);
 		f->frequency = (V4L2_TUNER_RADIO == t->mode) ?
 			DIV_ROUND_CLOSEST(abs_freq * 2, 125) :
 			DIV_ROUND_CLOSEST(abs_freq, 62500);
-		return 0;
+	} else {
+		f->frequency = (V4L2_TUNER_RADIO == t->mode) ?
+			t->radio_freq : t->tv_freq;
 	}
-	f->frequency = (V4L2_TUNER_RADIO == t->mode) ?
-		t->radio_freq : t->tv_freq;
 	return 0;
 }
 
@@ -854,10 +1143,8 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
 	struct dvb_tuner_ops *fe_tuner_ops = &t->fe.ops.tuner_ops;
 
-	if (check_mode(t, "g_tuner") == -EINVAL)
+	if (check_mode(t, vt->type) == -EINVAL)
 		return 0;
-	switch_v4l2();
-
 	vt->type = t->mode;
 	if (analog_ops->get_afc)
 		vt->afc = analog_ops->get_afc(&t->fe);
@@ -870,8 +1157,7 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	}
 
 	/* radio mode */
-	vt->rxsubchans =
-		V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
+	vt->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
 	if (fe_tuner_ops->get_status) {
 		u32 tuner_status;
 
@@ -880,21 +1166,14 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 			(tuner_status & TUNER_STATUS_STEREO) ?
 			V4L2_TUNER_SUB_STEREO :
 			V4L2_TUNER_SUB_MONO;
-	} else {
-		if (analog_ops->is_stereo) {
-			vt->rxsubchans =
-				analog_ops->is_stereo(&t->fe) ?
-				V4L2_TUNER_SUB_STEREO :
-				V4L2_TUNER_SUB_MONO;
-		}
 	}
 	if (analog_ops->has_signal)
 		vt->signal = analog_ops->has_signal(&t->fe);
-	vt->capability |=
-		V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	vt->capability |= V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
 	vt->audmode = t->audmode;
 	vt->rangelow = radio_range[0] * 16000;
 	vt->rangehigh = radio_range[1] * 16000;
+
 	return 0;
 }
 
@@ -903,16 +1182,12 @@ static int tuner_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	struct tuner *t = to_tuner(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-	if (check_mode(t, "s_tuner") == -EINVAL)
+	if (set_mode_freq(client, t, vt->type, 0) == -EINVAL)
 		return 0;
 
-	switch_v4l2();
+	if (t->mode == V4L2_TUNER_RADIO)
+		t->audmode = vt->audmode;
 
-	/* do nothing unless we're a radio tuner */
-	if (t->mode != V4L2_TUNER_RADIO)
-		return 0;
-	t->audmode = vt->audmode;
-	set_radio_freq(client, t->radio_freq);
 	return 0;
 }
 
@@ -929,9 +1204,13 @@ static int tuner_log_status(struct v4l2_subdev *sd)
 static int tuner_suspend(struct i2c_client *c, pm_message_t state)
 {
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
+	struct analog_demod_ops *analog_ops = &t->fe.ops.analog_ops;
 
 	tuner_dbg("suspend\n");
-	/* FIXME: power down ??? */
+
+	if (!t->standby && analog_ops->standby)
+		analog_ops->standby(&t->fe);
+
 	return 0;
 }
 
@@ -940,13 +1219,10 @@ static int tuner_resume(struct i2c_client *c)
 	struct tuner *t = to_tuner(i2c_get_clientdata(c));
 
 	tuner_dbg("resume\n");
-	if (V4L2_TUNER_RADIO == t->mode) {
-		if (t->radio_freq)
-			set_freq(c, t->radio_freq);
-	} else {
-		if (t->tv_freq)
-			set_freq(c, t->tv_freq);
-	}
+
+	if (!t->standby)
+		set_mode_freq(c, t, t->type, 0);
+
 	return 0;
 }
 
@@ -964,7 +1240,9 @@ static int tuner_command(struct i2c_client *client, unsigned cmd, void *arg)
 	return -ENOIOCTLCMD;
 }
 
-/* ----------------------------------------------------------------------- */
+/*
+ * Callback structs
+ */
 
 static const struct v4l2_subdev_core_ops tuner_core_ops = {
 	.log_status = tuner_log_status,
@@ -987,183 +1265,10 @@ static const struct v4l2_subdev_ops tuner_ops = {
 	.tuner = &tuner_tuner_ops,
 };
 
-/* ---------------------------------------------------------------------- */
-
-static LIST_HEAD(tuner_list);
-
-/* Search for existing radio and/or TV tuners on the given I2C adapter.
-   Note that when this function is called from tuner_probe you can be
-   certain no other devices will be added/deleted at the same time, I2C
-   core protects against that. */
-static void tuner_lookup(struct i2c_adapter *adap,
-		struct tuner **radio, struct tuner **tv)
-{
-	struct tuner *pos;
-
-	*radio = NULL;
-	*tv = NULL;
-
-	list_for_each_entry(pos, &tuner_list, list) {
-		int mode_mask;
-
-		if (pos->i2c->adapter != adap ||
-		    strcmp(pos->i2c->driver->driver.name, "tuner"))
-			continue;
-
-		mode_mask = pos->mode_mask & ~T_STANDBY;
-		if (*radio == NULL && mode_mask == T_RADIO)
-			*radio = pos;
-		/* Note: currently TDA9887 is the only demod-only
-		   device. If other devices appear then we need to
-		   make this test more general. */
-		else if (*tv == NULL && pos->type != TUNER_TDA9887 &&
-			 (pos->mode_mask & (T_ANALOG_TV | T_DIGITAL_TV)))
-			*tv = pos;
-	}
-}
-
-/* During client attach, set_type is called by adapter's attach_inform callback.
-   set_type must then be completed by tuner_probe.
+/*
+ * I2C structs and module init functions
  */
-static int tuner_probe(struct i2c_client *client,
-		       const struct i2c_device_id *id)
-{
-	struct tuner *t;
-	struct tuner *radio;
-	struct tuner *tv;
-
-	t = kzalloc(sizeof(struct tuner), GFP_KERNEL);
-	if (NULL == t)
-		return -ENOMEM;
-	v4l2_i2c_subdev_init(&t->sd, client, &tuner_ops);
-	t->i2c = client;
-	t->name = "(tuner unset)";
-	t->type = UNSET;
-	t->audmode = V4L2_TUNER_MODE_STEREO;
-	t->mode_mask = T_UNINITIALIZED;
-
-	if (show_i2c) {
-		unsigned char buffer[16];
-		int i, rc;
-
-		memset(buffer, 0, sizeof(buffer));
-		rc = i2c_master_recv(client, buffer, sizeof(buffer));
-		tuner_info("I2C RECV = ");
-		for (i = 0; i < rc; i++)
-			printk(KERN_CONT "%02x ", buffer[i]);
-		printk("\n");
-	}
 
-	/* autodetection code based on the i2c addr */
-	if (!no_autodetect) {
-		switch (client->addr) {
-		case 0x10:
-			if (tuner_symbol_probe(tea5761_autodetection,
-					       t->i2c->adapter,
-					       t->i2c->addr) >= 0) {
-				t->type = TUNER_TEA5761;
-				t->mode_mask = T_RADIO;
-				t->mode = T_STANDBY;
-				/* Sets freq to FM range */
-				t->radio_freq = 87.5 * 16000;
-				tuner_lookup(t->i2c->adapter, &radio, &tv);
-				if (tv)
-					tv->mode_mask &= ~T_RADIO;
-
-				goto register_client;
-			}
-			kfree(t);
-			return -ENODEV;
-		case 0x42:
-		case 0x43:
-		case 0x4a:
-		case 0x4b:
-			/* If chip is not tda8290, don't register.
-			   since it can be tda9887*/
-			if (tuner_symbol_probe(tda829x_probe, t->i2c->adapter,
-					       t->i2c->addr) >= 0) {
-				tuner_dbg("tda829x detected\n");
-			} else {
-				/* Default is being tda9887 */
-				t->type = TUNER_TDA9887;
-				t->mode_mask = T_RADIO | T_ANALOG_TV |
-					       T_DIGITAL_TV;
-				t->mode = T_STANDBY;
-				goto register_client;
-			}
-			break;
-		case 0x60:
-			if (tuner_symbol_probe(tea5767_autodetection,
-					       t->i2c->adapter, t->i2c->addr)
-					>= 0) {
-				t->type = TUNER_TEA5767;
-				t->mode_mask = T_RADIO;
-				t->mode = T_STANDBY;
-				/* Sets freq to FM range */
-				t->radio_freq = 87.5 * 16000;
-				tuner_lookup(t->i2c->adapter, &radio, &tv);
-				if (tv)
-					tv->mode_mask &= ~T_RADIO;
-
-				goto register_client;
-			}
-			break;
-		}
-	}
-
-	/* Initializes only the first TV tuner on this adapter. Why only the
-	   first? Because there are some devices (notably the ones with TI
-	   tuners) that have more than one i2c address for the *same* device.
-	   Experience shows that, except for just one case, the first
-	   address is the right one. The exception is a Russian tuner
-	   (ACORP_Y878F). So, the desired behavior is just to enable the
-	   first found TV tuner. */
-	tuner_lookup(t->i2c->adapter, &radio, &tv);
-	if (tv == NULL) {
-		t->mode_mask = T_ANALOG_TV | T_DIGITAL_TV;
-		if (radio == NULL)
-			t->mode_mask |= T_RADIO;
-		tuner_dbg("Setting mode_mask to 0x%02x\n", t->mode_mask);
-		t->tv_freq = 400 * 16; /* Sets freq to VHF High */
-		t->radio_freq = 87.5 * 16000; /* Sets freq to FM range */
-	}
-
-	/* Should be just before return */
-register_client:
-	tuner_info("chip found @ 0x%x (%s)\n", client->addr << 1,
-		       client->adapter->name);
-
-	/* Sets a default mode */
-	if (t->mode_mask & T_ANALOG_TV) {
-		t->mode = V4L2_TUNER_ANALOG_TV;
-	} else  if (t->mode_mask & T_RADIO) {
-		t->mode = V4L2_TUNER_RADIO;
-	} else {
-		t->mode = V4L2_TUNER_DIGITAL_TV;
-	}
-	set_type(client, t->type, t->mode_mask, t->config, t->fe.callback);
-	list_add_tail(&t->list, &tuner_list);
-	return 0;
-}
-
-static int tuner_remove(struct i2c_client *client)
-{
-	struct tuner *t = to_tuner(i2c_get_clientdata(client));
-
-	v4l2_device_unregister_subdev(&t->sd);
-	tuner_detach(&t->fe);
-	t->fe.analog_demod_priv = NULL;
-
-	list_del(&t->list);
-	kfree(t);
-	return 0;
-}
-
-/* ----------------------------------------------------------------------- */
-
-/* This driver supports many devices and the idea is to let the driver
-   detect which device is present. So rather than listing all supported
-   devices here, we pretend to support a single, fake device type. */
 static const struct i2c_device_id tuner_id[] = {
 	{ "tuner", }, /* autodetect */
 	{ }
@@ -1196,10 +1301,6 @@ static __exit void exit_tuner(void)
 module_init(init_tuner);
 module_exit(exit_tuner);
 
-/*
- * Overrides for Emacs so that we follow Linus's tabbing style.
- * ---------------------------------------------------------------------------
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
+MODULE_DESCRIPTION("device driver for various TV and TV+FM radio tuners");
+MODULE_AUTHOR("Ralph Metzler, Gerd Knorr, Gunther Mayer");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/tvp514x.c b/drivers/media/video/tvp514x.c
index 45bcf0358a1d..9b3e828b0775 100644
--- a/drivers/media/video/tvp514x.c
+++ b/drivers/media/video/tvp514x.c
@@ -37,6 +37,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 #include <media/tvp514x.h>
 
 #include "tvp514x_regs.h"
@@ -97,6 +98,7 @@ static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable);
  */
 struct tvp514x_decoder {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	struct tvp514x_reg tvp514x_regs[ARRAY_SIZE(tvp514x_reg_list_default)];
 	const struct tvp514x_platform_data *pdata;
 
@@ -238,6 +240,11 @@ static inline struct tvp514x_decoder *to_decoder(struct v4l2_subdev *sd)
 	return container_of(sd, struct tvp514x_decoder, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tvp514x_decoder, hdl)->sd;
+}
+
 
 /**
  * tvp514x_read_reg() - Read a value from a register in an TVP5146/47.
@@ -719,213 +726,54 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
 }
 
 /**
- * tvp514x_queryctrl() - V4L2 decoder interface handler for queryctrl
- * @sd: pointer to standard V4L2 sub-device structure
- * @qctrl: standard V4L2 v4l2_queryctrl structure
- *
- * If the requested control is supported, returns the control information.
- * Otherwise, returns -EINVAL if the control is not supported.
- */
-static int
-tvp514x_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qctrl)
-{
-	int err = -EINVAL;
-
-	if (qctrl == NULL)
-		return err;
-
-	switch (qctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		/* Brightness supported is (0-255), */
-		err = v4l2_ctrl_query_fill(qctrl, 0, 255, 1, 128);
-		break;
-	case V4L2_CID_CONTRAST:
-	case V4L2_CID_SATURATION:
-		/**
-		 * Saturation and Contrast supported is -
-		 *	Contrast: 0 - 255 (Default - 128)
-		 *	Saturation: 0 - 255 (Default - 128)
-		 */
-		err = v4l2_ctrl_query_fill(qctrl, 0, 255, 1, 128);
-		break;
-	case V4L2_CID_HUE:
-		/* Hue Supported is -
-		 *	Hue - -180 - +180 (Default - 0, Step - +180)
-		 */
-		err = v4l2_ctrl_query_fill(qctrl, -180, 180, 180, 0);
-		break;
-	case V4L2_CID_AUTOGAIN:
-		/**
-		 * Auto Gain supported is -
-		 * 	0 - 1 (Default - 1)
-		 */
-		err = v4l2_ctrl_query_fill(qctrl, 0, 1, 1, 1);
-		break;
-	default:
-		v4l2_err(sd, "invalid control id %d\n", qctrl->id);
-		return err;
-	}
-
-	v4l2_dbg(1, debug, sd, "Query Control:%s: Min - %d, Max - %d, Def - %d\n",
-			qctrl->name, qctrl->minimum, qctrl->maximum,
-			qctrl->default_value);
-
-	return err;
-}
-
-/**
- * tvp514x_g_ctrl() - V4L2 decoder interface handler for g_ctrl
- * @sd: pointer to standard V4L2 sub-device structure
- * @ctrl: pointer to v4l2_control structure
- *
- * If the requested control is supported, returns the control's current
- * value from the decoder. Otherwise, returns -EINVAL if the control is not
- * supported.
- */
-static int
-tvp514x_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct tvp514x_decoder *decoder = to_decoder(sd);
-
-	if (ctrl == NULL)
-		return -EINVAL;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = decoder->tvp514x_regs[REG_BRIGHTNESS].val;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = decoder->tvp514x_regs[REG_CONTRAST].val;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = decoder->tvp514x_regs[REG_SATURATION].val;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = decoder->tvp514x_regs[REG_HUE].val;
-		if (ctrl->value == 0x7F)
-			ctrl->value = 180;
-		else if (ctrl->value == 0x80)
-			ctrl->value = -180;
-		else
-			ctrl->value = 0;
-
-		break;
-	case V4L2_CID_AUTOGAIN:
-		ctrl->value = decoder->tvp514x_regs[REG_AFE_GAIN_CTRL].val;
-		if ((ctrl->value & 0x3) == 3)
-			ctrl->value = 1;
-		else
-			ctrl->value = 0;
-
-		break;
-	default:
-		v4l2_err(sd, "invalid control id %d\n", ctrl->id);
-		return -EINVAL;
-	}
-
-	v4l2_dbg(1, debug, sd, "Get Control: ID - %d - %d\n",
-			ctrl->id, ctrl->value);
-	return 0;
-}
-
-/**
  * tvp514x_s_ctrl() - V4L2 decoder interface handler for s_ctrl
- * @sd: pointer to standard V4L2 sub-device structure
- * @ctrl: pointer to v4l2_control structure
+ * @ctrl: pointer to v4l2_ctrl structure
  *
  * If the requested control is supported, sets the control's current
  * value in HW. Otherwise, returns -EINVAL if the control is not supported.
  */
-static int
-tvp514x_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tvp514x_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct tvp514x_decoder *decoder = to_decoder(sd);
 	int err = -EINVAL, value;
 
-	if (ctrl == NULL)
-		return err;
-
-	value = ctrl->value;
+	value = ctrl->val;
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if (ctrl->value < 0 || ctrl->value > 255) {
-			v4l2_err(sd, "invalid brightness setting %d\n",
-					ctrl->value);
-			return -ERANGE;
-		}
-		err = tvp514x_write_reg(sd, REG_BRIGHTNESS,
-				value);
-		if (err)
-			return err;
-
-		decoder->tvp514x_regs[REG_BRIGHTNESS].val = value;
+		err = tvp514x_write_reg(sd, REG_BRIGHTNESS, value);
+		if (!err)
+			decoder->tvp514x_regs[REG_BRIGHTNESS].val = value;
 		break;
 	case V4L2_CID_CONTRAST:
-		if (ctrl->value < 0 || ctrl->value > 255) {
-			v4l2_err(sd, "invalid contrast setting %d\n",
-					ctrl->value);
-			return -ERANGE;
-		}
 		err = tvp514x_write_reg(sd, REG_CONTRAST, value);
-		if (err)
-			return err;
-
-		decoder->tvp514x_regs[REG_CONTRAST].val = value;
+		if (!err)
+			decoder->tvp514x_regs[REG_CONTRAST].val = value;
 		break;
 	case V4L2_CID_SATURATION:
-		if (ctrl->value < 0 || ctrl->value > 255) {
-			v4l2_err(sd, "invalid saturation setting %d\n",
-					ctrl->value);
-			return -ERANGE;
-		}
 		err = tvp514x_write_reg(sd, REG_SATURATION, value);
-		if (err)
-			return err;
-
-		decoder->tvp514x_regs[REG_SATURATION].val = value;
+		if (!err)
+			decoder->tvp514x_regs[REG_SATURATION].val = value;
 		break;
 	case V4L2_CID_HUE:
 		if (value == 180)
 			value = 0x7F;
 		else if (value == -180)
 			value = 0x80;
-		else if (value == 0)
-			value = 0;
-		else {
-			v4l2_err(sd, "invalid hue setting %d\n", ctrl->value);
-			return -ERANGE;
-		}
 		err = tvp514x_write_reg(sd, REG_HUE, value);
-		if (err)
-			return err;
-
-		decoder->tvp514x_regs[REG_HUE].val = value;
+		if (!err)
+			decoder->tvp514x_regs[REG_HUE].val = value;
 		break;
 	case V4L2_CID_AUTOGAIN:
-		if (value == 1)
-			value = 0x0F;
-		else if (value == 0)
-			value = 0x0C;
-		else {
-			v4l2_err(sd, "invalid auto gain setting %d\n",
-					ctrl->value);
-			return -ERANGE;
-		}
-		err = tvp514x_write_reg(sd, REG_AFE_GAIN_CTRL, value);
-		if (err)
-			return err;
-
-		decoder->tvp514x_regs[REG_AFE_GAIN_CTRL].val = value;
+		err = tvp514x_write_reg(sd, REG_AFE_GAIN_CTRL, value ? 0x0f : 0x0c);
+		if (!err)
+			decoder->tvp514x_regs[REG_AFE_GAIN_CTRL].val = value;
 		break;
-	default:
-		v4l2_err(sd, "invalid control id %d\n", ctrl->id);
-		return err;
 	}
 
 	v4l2_dbg(1, debug, sd, "Set Control: ID - %d - %d\n",
-			ctrl->id, ctrl->value);
-
+			ctrl->id, ctrl->val);
 	return err;
 }
 
@@ -1104,10 +952,18 @@ static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable)
 	return err;
 }
 
-static const struct v4l2_subdev_core_ops tvp514x_core_ops = {
-	.queryctrl = tvp514x_queryctrl,
-	.g_ctrl = tvp514x_g_ctrl,
+static const struct v4l2_ctrl_ops tvp514x_ctrl_ops = {
 	.s_ctrl = tvp514x_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops tvp514x_core_ops = {
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 	.s_std = tvp514x_s_std,
 };
 
@@ -1190,6 +1046,27 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	sd = &decoder->sd;
 	v4l2_i2c_subdev_init(sd, client, &tvp514x_ops);
 
+	v4l2_ctrl_handler_init(&decoder->hdl, 5);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_HUE, -180, 180, 180, 0);
+	v4l2_ctrl_new_std(&decoder->hdl, &tvp514x_ctrl_ops,
+		V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		kfree(decoder);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
+
 	v4l2_info(sd, "%s decoder driver registered !!\n", sd->name);
 
 	return 0;
@@ -1209,6 +1086,7 @@ static int tvp514x_remove(struct i2c_client *client)
 	struct tvp514x_decoder *decoder = to_decoder(sd);
 
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
 	kfree(decoder);
 	return 0;
 }
diff --git a/drivers/media/video/tvp5150.c b/drivers/media/video/tvp5150.c
index 58927664d3ea..e927d25e0d35 100644
--- a/drivers/media/video/tvp5150.c
+++ b/drivers/media/video/tvp5150.c
@@ -12,6 +12,7 @@
 #include <media/v4l2-device.h>
 #include <media/tvp5150.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 #include "tvp5150_reg.h"
 
@@ -24,58 +25,14 @@ static int debug;
 module_param(debug, int, 0);
 MODULE_PARM_DESC(debug, "Debug level (0-2)");
 
-/* supported controls */
-static struct v4l2_queryctrl tvp5150_qctrl[] = {
-	{
-		.id = V4L2_CID_BRIGHTNESS,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step = 1,
-		.default_value = 128,
-		.flags = 0,
-	}, {
-		.id = V4L2_CID_CONTRAST,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Contrast",
-		.minimum = 0,
-		.maximum = 255,
-		.step = 0x1,
-		.default_value = 128,
-		.flags = 0,
-	}, {
-		 .id = V4L2_CID_SATURATION,
-		 .type = V4L2_CTRL_TYPE_INTEGER,
-		 .name = "Saturation",
-		 .minimum = 0,
-		 .maximum = 255,
-		 .step = 0x1,
-		 .default_value = 128,
-		 .flags = 0,
-	}, {
-		.id = V4L2_CID_HUE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Hue",
-		.minimum = -128,
-		.maximum = 127,
-		.step = 0x1,
-		.default_value = 0,
-		.flags = 0,
-	}
-};
-
 struct tvp5150 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 
 	v4l2_std_id norm;	/* Current set standard */
 	u32 input;
 	u32 output;
 	int enable;
-	int bright;
-	int contrast;
-	int hue;
-	int sat;
 };
 
 static inline struct tvp5150 *to_tvp5150(struct v4l2_subdev *sd)
@@ -83,6 +40,11 @@ static inline struct tvp5150 *to_tvp5150(struct v4l2_subdev *sd)
 	return container_of(sd, struct tvp5150, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tvp5150, hdl)->sd;
+}
+
 static int tvp5150_read(struct v4l2_subdev *sd, unsigned char addr)
 {
 	struct i2c_client *c = v4l2_get_subdevdata(sd);
@@ -775,27 +737,6 @@ static int tvp5150_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 static int tvp5150_reset(struct v4l2_subdev *sd, u32 val)
 {
 	struct tvp5150 *decoder = to_tvp5150(sd);
-	u8 msb_id, lsb_id, msb_rom, lsb_rom;
-
-	msb_id = tvp5150_read(sd, TVP5150_MSB_DEV_ID);
-	lsb_id = tvp5150_read(sd, TVP5150_LSB_DEV_ID);
-	msb_rom = tvp5150_read(sd, TVP5150_ROM_MAJOR_VER);
-	lsb_rom = tvp5150_read(sd, TVP5150_ROM_MINOR_VER);
-
-	if (msb_rom == 4 && lsb_rom == 0) { /* Is TVP5150AM1 */
-		v4l2_info(sd, "tvp%02x%02xam1 detected.\n", msb_id, lsb_id);
-
-		/* ITU-T BT.656.4 timing */
-		tvp5150_write(sd, TVP5150_REV_SELECT, 0);
-	} else {
-		if (msb_rom == 3 || lsb_rom == 0x21) { /* Is TVP5150A */
-			v4l2_info(sd, "tvp%02x%02xa detected.\n", msb_id, lsb_id);
-		} else {
-			v4l2_info(sd, "*** unknown tvp%02x%02x chip detected.\n",
-					msb_id, lsb_id);
-			v4l2_info(sd, "*** Rom ver is %d.%d\n", msb_rom, lsb_rom);
-		}
-	}
 
 	/* Initializes TVP5150 to its default values */
 	tvp5150_write_inittab(sd, tvp5150_init_default);
@@ -810,64 +751,28 @@ static int tvp5150_reset(struct v4l2_subdev *sd, u32 val)
 	tvp5150_write_inittab(sd, tvp5150_init_enable);
 
 	/* Initialize image preferences */
-	tvp5150_write(sd, TVP5150_BRIGHT_CTL, decoder->bright);
-	tvp5150_write(sd, TVP5150_CONTRAST_CTL, decoder->contrast);
-	tvp5150_write(sd, TVP5150_SATURATION_CTL, decoder->contrast);
-	tvp5150_write(sd, TVP5150_HUE_CTL, decoder->hue);
+	v4l2_ctrl_handler_setup(&decoder->hdl);
 
 	tvp5150_set_std(sd, decoder->norm);
 	return 0;
 };
 
-static int tvp5150_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	v4l2_dbg(1, debug, sd, "g_ctrl called\n");
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = tvp5150_read(sd, TVP5150_BRIGHT_CTL);
-		return 0;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = tvp5150_read(sd, TVP5150_CONTRAST_CTL);
-		return 0;
-	case V4L2_CID_SATURATION:
-		ctrl->value = tvp5150_read(sd, TVP5150_SATURATION_CTL);
-		return 0;
-	case V4L2_CID_HUE:
-		ctrl->value = tvp5150_read(sd, TVP5150_HUE_CTL);
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int tvp5150_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tvp5150_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	u8 i, n;
-	n = ARRAY_SIZE(tvp5150_qctrl);
-
-	for (i = 0; i < n; i++) {
-		if (ctrl->id != tvp5150_qctrl[i].id)
-			continue;
-		if (ctrl->value < tvp5150_qctrl[i].minimum ||
-		    ctrl->value > tvp5150_qctrl[i].maximum)
-			return -ERANGE;
-		v4l2_dbg(1, debug, sd, "s_ctrl: id=%d, value=%d\n",
-					ctrl->id, ctrl->value);
-		break;
-	}
+	struct v4l2_subdev *sd = to_sd(ctrl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		tvp5150_write(sd, TVP5150_BRIGHT_CTL, ctrl->value);
+		tvp5150_write(sd, TVP5150_BRIGHT_CTL, ctrl->val);
 		return 0;
 	case V4L2_CID_CONTRAST:
-		tvp5150_write(sd, TVP5150_CONTRAST_CTL, ctrl->value);
+		tvp5150_write(sd, TVP5150_CONTRAST_CTL, ctrl->val);
 		return 0;
 	case V4L2_CID_SATURATION:
-		tvp5150_write(sd, TVP5150_SATURATION_CTL, ctrl->value);
+		tvp5150_write(sd, TVP5150_SATURATION_CTL, ctrl->val);
 		return 0;
 	case V4L2_CID_HUE:
-		tvp5150_write(sd, TVP5150_HUE_CTL, ctrl->value);
+		tvp5150_write(sd, TVP5150_HUE_CTL, ctrl->val);
 		return 0;
 	}
 	return -EINVAL;
@@ -995,29 +900,21 @@ static int tvp5150_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	return 0;
 }
 
-static int tvp5150_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	int i;
-
-	v4l2_dbg(1, debug, sd, "queryctrl called\n");
-
-	for (i = 0; i < ARRAY_SIZE(tvp5150_qctrl); i++)
-		if (qc->id && qc->id == tvp5150_qctrl[i].id) {
-			memcpy(qc, &(tvp5150_qctrl[i]),
-			       sizeof(*qc));
-			return 0;
-		}
-
-	return -EINVAL;
-}
-
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops tvp5150_ctrl_ops = {
+	.s_ctrl = tvp5150_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops tvp5150_core_ops = {
 	.log_status = tvp5150_log_status,
-	.g_ctrl = tvp5150_g_ctrl,
-	.s_ctrl = tvp5150_s_ctrl,
-	.queryctrl = tvp5150_queryctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 	.s_std = tvp5150_s_std,
 	.reset = tvp5150_reset,
 	.g_chip_ident = tvp5150_g_chip_ident,
@@ -1059,6 +956,7 @@ static int tvp5150_probe(struct i2c_client *c,
 {
 	struct tvp5150 *core;
 	struct v4l2_subdev *sd;
+	u8 msb_id, lsb_id, msb_rom, lsb_rom;
 
 	/* Check if the adapter supports the needed features */
 	if (!i2c_check_functionality(c->adapter,
@@ -1074,13 +972,48 @@ static int tvp5150_probe(struct i2c_client *c,
 	v4l_info(c, "chip found @ 0x%02x (%s)\n",
 		 c->addr << 1, c->adapter->name);
 
+	msb_id = tvp5150_read(sd, TVP5150_MSB_DEV_ID);
+	lsb_id = tvp5150_read(sd, TVP5150_LSB_DEV_ID);
+	msb_rom = tvp5150_read(sd, TVP5150_ROM_MAJOR_VER);
+	lsb_rom = tvp5150_read(sd, TVP5150_ROM_MINOR_VER);
+
+	if (msb_rom == 4 && lsb_rom == 0) { /* Is TVP5150AM1 */
+		v4l2_info(sd, "tvp%02x%02xam1 detected.\n", msb_id, lsb_id);
+
+		/* ITU-T BT.656.4 timing */
+		tvp5150_write(sd, TVP5150_REV_SELECT, 0);
+	} else {
+		if (msb_rom == 3 || lsb_rom == 0x21) { /* Is TVP5150A */
+			v4l2_info(sd, "tvp%02x%02xa detected.\n", msb_id, lsb_id);
+		} else {
+			v4l2_info(sd, "*** unknown tvp%02x%02x chip detected.\n",
+					msb_id, lsb_id);
+			v4l2_info(sd, "*** Rom ver is %d.%d\n", msb_rom, lsb_rom);
+		}
+	}
+
 	core->norm = V4L2_STD_ALL;	/* Default is autodetect */
 	core->input = TVP5150_COMPOSITE1;
 	core->enable = 1;
-	core->bright = 128;
-	core->contrast = 128;
-	core->hue = 0;
-	core->sat = 128;
+
+	v4l2_ctrl_handler_init(&core->hdl, 4);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(&core->hdl, &tvp5150_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	sd->ctrl_handler = &core->hdl;
+	if (core->hdl.error) {
+		int err = core->hdl.error;
+
+		v4l2_ctrl_handler_free(&core->hdl);
+		kfree(core);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&core->hdl);
 
 	if (debug > 1)
 		tvp5150_log_status(sd);
@@ -1090,12 +1023,14 @@ static int tvp5150_probe(struct i2c_client *c,
 static int tvp5150_remove(struct i2c_client *c)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct tvp5150 *decoder = to_tvp5150(sd);
 
 	v4l2_dbg(1, debug, sd,
 		"tvp5150.c: removing tvp5150 adapter on address 0x%x\n",
 		c->addr << 1);
 
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&decoder->hdl);
 	kfree(to_tvp5150(sd));
 	return 0;
 }
diff --git a/drivers/media/video/tvp7002.c b/drivers/media/video/tvp7002.c
index c799e4eb6fcd..b799851bf3d0 100644
--- a/drivers/media/video/tvp7002.c
+++ b/drivers/media/video/tvp7002.c
@@ -32,6 +32,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
 #include "tvp7002_reg.h"
 
 MODULE_DESCRIPTION("TI TVP7002 Video and Graphics Digitizer driver");
@@ -421,13 +422,13 @@ static const struct tvp7002_preset_definition tvp7002_presets[] = {
 /* Device definition */
 struct tvp7002 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	const struct tvp7002_config *pdata;
 
 	int ver;
 	int streaming;
 
 	const struct tvp7002_preset_definition *current_preset;
-	u8 gain;
 };
 
 /*
@@ -441,6 +442,11 @@ static inline struct tvp7002 *to_tvp7002(struct v4l2_subdev *sd)
 	return container_of(sd, struct tvp7002, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct tvp7002, hdl)->sd;
+}
+
 /*
  * tvp7002_read - Read a value from a register in an TVP7002
  * @sd: ptr to v4l2_subdev struct
@@ -606,78 +612,25 @@ static int tvp7002_s_dv_preset(struct v4l2_subdev *sd,
 }
 
 /*
- * tvp7002_g_ctrl() - Get a control
- * @sd: ptr to v4l2_subdev struct
- * @ctrl: ptr to v4l2_control struct
- *
- * Get a control for a TVP7002 decoder device.
- * Returns zero when successful or -EINVAL if register access fails.
- */
-static int tvp7002_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct tvp7002 *device = to_tvp7002(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_GAIN:
-		ctrl->value = device->gain;
-		return 0;
-	default:
-		return -EINVAL;
-	}
-}
-
-/*
  * tvp7002_s_ctrl() - Set a control
- * @sd: ptr to v4l2_subdev struct
- * @ctrl: ptr to v4l2_control struct
+ * @ctrl: ptr to v4l2_ctrl struct
  *
  * Set a control in TVP7002 decoder device.
  * Returns zero when successful or -EINVAL if register access fails.
  */
-static int tvp7002_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int tvp7002_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct tvp7002 *device = to_tvp7002(sd);
+	struct v4l2_subdev *sd = to_sd(ctrl);
 	int error = 0;
 
 	switch (ctrl->id) {
 	case V4L2_CID_GAIN:
-		tvp7002_write_err(sd, TVP7002_R_FINE_GAIN,
-						ctrl->value & 0xff, &error);
-		tvp7002_write_err(sd, TVP7002_G_FINE_GAIN,
-						ctrl->value & 0xff, &error);
-		tvp7002_write_err(sd, TVP7002_B_FINE_GAIN,
-						ctrl->value & 0xff, &error);
-
-		if (error < 0)
-			return error;
-
-		/* Set only after knowing there is no error */
-		device->gain = ctrl->value & 0xff;
-		return 0;
-	default:
-		return -EINVAL;
-	}
-}
-
-/*
- * tvp7002_queryctrl() - Query a control
- * @sd: ptr to v4l2_subdev struct
- * @qc: ptr to v4l2_queryctrl struct
- *
- * Query a control of a TVP7002 decoder device.
- * Returns zero when successful or -EINVAL if register read fails.
- */
-static int tvp7002_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_GAIN:
-		/*
-		 * Gain is supported [0-255, default=0, step=1]
-		 */
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 0);
-	default:
-		return -EINVAL;
+		tvp7002_write_err(sd, TVP7002_R_FINE_GAIN, ctrl->val, &error);
+		tvp7002_write_err(sd, TVP7002_G_FINE_GAIN, ctrl->val, &error);
+		tvp7002_write_err(sd, TVP7002_B_FINE_GAIN, ctrl->val, &error);
+		return error;
 	}
+	return -EINVAL;
 }
 
 /*
@@ -924,7 +877,7 @@ static int tvp7002_log_status(struct v4l2_subdev *sd)
 					device->streaming ? "yes" : "no");
 
 	/* Print the current value of the gain control */
-	v4l2_info(sd, "Gain: %u\n", device->gain);
+	v4l2_ctrl_handler_log_status(&device->hdl, sd->name);
 
 	return 0;
 }
@@ -946,13 +899,21 @@ static int tvp7002_enum_dv_presets(struct v4l2_subdev *sd,
 	return v4l_fill_dv_preset_info(tvp7002_presets[preset->index].preset, preset);
 }
 
+static const struct v4l2_ctrl_ops tvp7002_ctrl_ops = {
+	.s_ctrl = tvp7002_s_ctrl,
+};
+
 /* V4L2 core operation handlers */
 static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
 	.g_chip_ident = tvp7002_g_chip_ident,
 	.log_status = tvp7002_log_status,
-	.g_ctrl = tvp7002_g_ctrl,
-	.s_ctrl = tvp7002_s_ctrl,
-	.queryctrl = tvp7002_queryctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = tvp7002_g_register,
 	.s_register = tvp7002_s_register,
@@ -977,12 +938,6 @@ static const struct v4l2_subdev_ops tvp7002_ops = {
 	.video = &tvp7002_video_ops,
 };
 
-static struct tvp7002 tvp7002_dev = {
-	.streaming = 0,
-	.current_preset = tvp7002_presets,
-	.gain = 0,
-};
-
 /*
  * tvp7002_probe - Probe a TVP7002 device
  * @c: ptr to i2c_client struct
@@ -1013,14 +968,14 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 		return -ENODEV;
 	}
 
-	device = kmalloc(sizeof(struct tvp7002), GFP_KERNEL);
+	device = kzalloc(sizeof(struct tvp7002), GFP_KERNEL);
 
 	if (!device)
 		return -ENOMEM;
 
-	*device = tvp7002_dev;
 	sd = &device->sd;
 	device->pdata = c->dev.platform_data;
+	device->current_preset = tvp7002_presets;
 
 	/* Tell v4l2 the device is ready */
 	v4l2_i2c_subdev_init(sd, c, &tvp7002_ops);
@@ -1060,6 +1015,19 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 	preset.preset = device->current_preset->preset;
 	error = tvp7002_s_dv_preset(sd, &preset);
 
+	v4l2_ctrl_handler_init(&device->hdl, 1);
+	v4l2_ctrl_new_std(&device->hdl, &tvp7002_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 0);
+	sd->ctrl_handler = &device->hdl;
+	if (device->hdl.error) {
+		int err = device->hdl.error;
+
+		v4l2_ctrl_handler_free(&device->hdl);
+		kfree(device);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&device->hdl);
+
 found_error:
 	if (error < 0)
 		kfree(device);
@@ -1083,6 +1051,7 @@ static int tvp7002_remove(struct i2c_client *c)
 				"on address 0x%x\n", c->addr);
 
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&device->hdl);
 	kfree(device);
 	return 0;
 }
diff --git a/drivers/media/video/v4l2-common.c b/drivers/media/video/v4l2-common.c
index 810eef43c216..940b5db3463e 100644
--- a/drivers/media/video/v4l2-common.c
+++ b/drivers/media/video/v4l2-common.c
@@ -59,7 +59,6 @@
 #include <asm/pgtable.h>
 #include <asm/io.h>
 #include <asm/div64.h>
-#define __OLD_VIDIOC_ /* To allow fixing old calls*/
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
diff --git a/drivers/media/video/v4l2-compat-ioctl32.c b/drivers/media/video/v4l2-compat-ioctl32.c
index dc82eb83c1d4..7c2694738b31 100644
--- a/drivers/media/video/v4l2-compat-ioctl32.c
+++ b/drivers/media/video/v4l2-compat-ioctl32.c
@@ -14,7 +14,6 @@
  */
 
 #include <linux/compat.h>
-#define __OLD_VIDIOC_ /* To allow fixing old calls*/
 #include <linux/videodev2.h>
 #include <linux/module.h>
 #include <media/v4l2-ioctl.h>
@@ -97,6 +96,14 @@ static inline int get_v4l2_pix_format(struct v4l2_pix_format *kp, struct v4l2_pi
 	return 0;
 }
 
+static inline int get_v4l2_pix_format_mplane(struct v4l2_pix_format_mplane *kp,
+				struct v4l2_pix_format_mplane __user *up)
+{
+	if (copy_from_user(kp, up, sizeof(struct v4l2_pix_format_mplane)))
+		return -EFAULT;
+	return 0;
+}
+
 static inline int put_v4l2_pix_format(struct v4l2_pix_format *kp, struct v4l2_pix_format __user *up)
 {
 	if (copy_to_user(up, kp, sizeof(struct v4l2_pix_format)))
@@ -104,6 +111,14 @@ static inline int put_v4l2_pix_format(struct v4l2_pix_format *kp, struct v4l2_pi
 	return 0;
 }
 
+static inline int put_v4l2_pix_format_mplane(struct v4l2_pix_format_mplane *kp,
+				struct v4l2_pix_format_mplane __user *up)
+{
+	if (copy_to_user(up, kp, sizeof(struct v4l2_pix_format_mplane)))
+		return -EFAULT;
+	return 0;
+}
+
 static inline int get_v4l2_vbi_format(struct v4l2_vbi_format *kp, struct v4l2_vbi_format __user *up)
 {
 	if (copy_from_user(kp, up, sizeof(struct v4l2_vbi_format)))
@@ -136,6 +151,7 @@ struct v4l2_format32 {
 	enum v4l2_buf_type type;
 	union {
 		struct v4l2_pix_format	pix;
+		struct v4l2_pix_format_mplane	pix_mp;
 		struct v4l2_window32	win;
 		struct v4l2_vbi_format	vbi;
 		struct v4l2_sliced_vbi_format	sliced;
@@ -152,6 +168,10 @@ static int get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 		return get_v4l2_pix_format(&kp->fmt.pix, &up->fmt.pix);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		return get_v4l2_pix_format_mplane(&kp->fmt.pix_mp,
+						  &up->fmt.pix_mp);
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 		return get_v4l2_window32(&kp->fmt.win, &up->fmt.win);
@@ -181,6 +201,10 @@ static int put_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 		return put_v4l2_pix_format(&kp->fmt.pix, &up->fmt.pix);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		return put_v4l2_pix_format_mplane(&kp->fmt.pix_mp,
+						  &up->fmt.pix_mp);
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 		return put_v4l2_window32(&kp->fmt.win, &up->fmt.win);
@@ -232,6 +256,17 @@ static int put_v4l2_standard32(struct v4l2_standard *kp, struct v4l2_standard32
 	return 0;
 }
 
+struct v4l2_plane32 {
+	__u32			bytesused;
+	__u32			length;
+	union {
+		__u32		mem_offset;
+		compat_long_t	userptr;
+	} m;
+	__u32			data_offset;
+	__u32			reserved[11];
+};
+
 struct v4l2_buffer32 {
 	__u32			index;
 	enum v4l2_buf_type      type;
@@ -247,14 +282,64 @@ struct v4l2_buffer32 {
 	union {
 		__u32           offset;
 		compat_long_t   userptr;
+		compat_caddr_t  planes;
 	} m;
 	__u32			length;
 	__u32			input;
 	__u32			reserved;
 };
 
+static int get_v4l2_plane32(struct v4l2_plane *up, struct v4l2_plane32 *up32,
+				enum v4l2_memory memory)
+{
+	void __user *up_pln;
+	compat_long_t p;
+
+	if (copy_in_user(up, up32, 2 * sizeof(__u32)) ||
+		copy_in_user(&up->data_offset, &up32->data_offset,
+				sizeof(__u32)))
+		return -EFAULT;
+
+	if (memory == V4L2_MEMORY_USERPTR) {
+		if (get_user(p, &up32->m.userptr))
+			return -EFAULT;
+		up_pln = compat_ptr(p);
+		if (put_user((unsigned long)up_pln, &up->m.userptr))
+			return -EFAULT;
+	} else {
+		if (copy_in_user(&up->m.mem_offset, &up32->m.mem_offset,
+					sizeof(__u32)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int put_v4l2_plane32(struct v4l2_plane *up, struct v4l2_plane32 *up32,
+				enum v4l2_memory memory)
+{
+	if (copy_in_user(up32, up, 2 * sizeof(__u32)) ||
+		copy_in_user(&up32->data_offset, &up->data_offset,
+				sizeof(__u32)))
+		return -EFAULT;
+
+	/* For MMAP, driver might've set up the offset, so copy it back.
+	 * USERPTR stays the same (was userspace-provided), so no copying. */
+	if (memory == V4L2_MEMORY_MMAP)
+		if (copy_in_user(&up32->m.mem_offset, &up->m.mem_offset,
+					sizeof(__u32)))
+			return -EFAULT;
+
+	return 0;
+}
+
 static int get_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user *up)
 {
+	struct v4l2_plane32 __user *uplane32;
+	struct v4l2_plane __user *uplane;
+	compat_caddr_t p;
+	int num_planes;
+	int ret;
 
 	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_buffer32)) ||
 		get_user(kp->index, &up->index) ||
@@ -263,33 +348,84 @@ static int get_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		get_user(kp->memory, &up->memory) ||
 		get_user(kp->input, &up->input))
 			return -EFAULT;
-	switch (kp->memory) {
-	case V4L2_MEMORY_MMAP:
-		if (get_user(kp->length, &up->length) ||
-			get_user(kp->m.offset, &up->m.offset))
+
+	if (V4L2_TYPE_IS_OUTPUT(kp->type))
+		if (get_user(kp->bytesused, &up->bytesused) ||
+			get_user(kp->field, &up->field) ||
+			get_user(kp->timestamp.tv_sec, &up->timestamp.tv_sec) ||
+			get_user(kp->timestamp.tv_usec,
+					&up->timestamp.tv_usec))
 			return -EFAULT;
-		break;
-	case V4L2_MEMORY_USERPTR:
-		{
-		compat_long_t tmp;
 
-		if (get_user(kp->length, &up->length) ||
-		    get_user(tmp, &up->m.userptr))
+	if (V4L2_TYPE_IS_MULTIPLANAR(kp->type)) {
+		if (get_user(kp->length, &up->length))
 			return -EFAULT;
 
-		kp->m.userptr = (unsigned long)compat_ptr(tmp);
+		num_planes = kp->length;
+		if (num_planes == 0) {
+			kp->m.planes = NULL;
+			/* num_planes == 0 is legal, e.g. when userspace doesn't
+			 * need planes array on DQBUF*/
+			return 0;
 		}
-		break;
-	case V4L2_MEMORY_OVERLAY:
-		if (get_user(kp->m.offset, &up->m.offset))
+
+		if (get_user(p, &up->m.planes))
 			return -EFAULT;
-		break;
+
+		uplane32 = compat_ptr(p);
+		if (!access_ok(VERIFY_READ, uplane32,
+				num_planes * sizeof(struct v4l2_plane32)))
+			return -EFAULT;
+
+		/* We don't really care if userspace decides to kill itself
+		 * by passing a very big num_planes value */
+		uplane = compat_alloc_user_space(num_planes *
+						sizeof(struct v4l2_plane));
+		kp->m.planes = uplane;
+
+		while (--num_planes >= 0) {
+			ret = get_v4l2_plane32(uplane, uplane32, kp->memory);
+			if (ret)
+				return ret;
+			++uplane;
+			++uplane32;
+		}
+	} else {
+		switch (kp->memory) {
+		case V4L2_MEMORY_MMAP:
+			if (get_user(kp->length, &up->length) ||
+				get_user(kp->m.offset, &up->m.offset))
+				return -EFAULT;
+			break;
+		case V4L2_MEMORY_USERPTR:
+			{
+			compat_long_t tmp;
+
+			if (get_user(kp->length, &up->length) ||
+			    get_user(tmp, &up->m.userptr))
+				return -EFAULT;
+
+			kp->m.userptr = (unsigned long)compat_ptr(tmp);
+			}
+			break;
+		case V4L2_MEMORY_OVERLAY:
+			if (get_user(kp->m.offset, &up->m.offset))
+				return -EFAULT;
+			break;
+		}
 	}
+
 	return 0;
 }
 
 static int put_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user *up)
 {
+	struct v4l2_plane32 __user *uplane32;
+	struct v4l2_plane __user *uplane;
+	compat_caddr_t p;
+	int num_planes;
+	int ret;
+
 	if (!access_ok(VERIFY_WRITE, up, sizeof(struct v4l2_buffer32)) ||
 		put_user(kp->index, &up->index) ||
 		put_user(kp->type, &up->type) ||
@@ -297,22 +433,7 @@ static int put_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		put_user(kp->memory, &up->memory) ||
 		put_user(kp->input, &up->input))
 			return -EFAULT;
-	switch (kp->memory) {
-	case V4L2_MEMORY_MMAP:
-		if (put_user(kp->length, &up->length) ||
-			put_user(kp->m.offset, &up->m.offset))
-			return -EFAULT;
-		break;
-	case V4L2_MEMORY_USERPTR:
-		if (put_user(kp->length, &up->length) ||
-			put_user(kp->m.userptr, &up->m.userptr))
-			return -EFAULT;
-		break;
-	case V4L2_MEMORY_OVERLAY:
-		if (put_user(kp->m.offset, &up->m.offset))
-			return -EFAULT;
-		break;
-	}
+
 	if (put_user(kp->bytesused, &up->bytesused) ||
 		put_user(kp->field, &up->field) ||
 		put_user(kp->timestamp.tv_sec, &up->timestamp.tv_sec) ||
@@ -321,6 +442,43 @@ static int put_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		put_user(kp->sequence, &up->sequence) ||
 		put_user(kp->reserved, &up->reserved))
 			return -EFAULT;
+
+	if (V4L2_TYPE_IS_MULTIPLANAR(kp->type)) {
+		num_planes = kp->length;
+		if (num_planes == 0)
+			return 0;
+
+		uplane = kp->m.planes;
+		if (get_user(p, &up->m.planes))
+			return -EFAULT;
+		uplane32 = compat_ptr(p);
+
+		while (--num_planes >= 0) {
+			ret = put_v4l2_plane32(uplane, uplane32, kp->memory);
+			if (ret)
+				return ret;
+			++uplane;
+			++uplane32;
+		}
+	} else {
+		switch (kp->memory) {
+		case V4L2_MEMORY_MMAP:
+			if (put_user(kp->length, &up->length) ||
+				put_user(kp->m.offset, &up->m.offset))
+				return -EFAULT;
+			break;
+		case V4L2_MEMORY_USERPTR:
+			if (put_user(kp->length, &up->length) ||
+				put_user(kp->m.userptr, &up->m.userptr))
+				return -EFAULT;
+			break;
+		case V4L2_MEMORY_OVERLAY:
+			if (put_user(kp->m.offset, &up->m.offset))
+				return -EFAULT;
+			break;
+		}
+	}
+
 	return 0;
 }
 
@@ -442,12 +600,13 @@ static int get_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 	if (get_user(p, &up->controls))
 		return -EFAULT;
 	ucontrols = compat_ptr(p);
-	if (!access_ok(VERIFY_READ, ucontrols, n * sizeof(struct v4l2_ext_control)))
+	if (!access_ok(VERIFY_READ, ucontrols,
+			n * sizeof(struct v4l2_ext_control32)))
 		return -EFAULT;
 	kcontrols = compat_alloc_user_space(n * sizeof(struct v4l2_ext_control));
 	kp->controls = kcontrols;
 	while (--n >= 0) {
-		if (copy_in_user(kcontrols, ucontrols, sizeof(*kcontrols)))
+		if (copy_in_user(kcontrols, ucontrols, sizeof(*ucontrols)))
 			return -EFAULT;
 		if (ctrl_is_pointer(kcontrols->id)) {
 			void __user *s;
@@ -483,7 +642,8 @@ static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 	if (get_user(p, &up->controls))
 		return -EFAULT;
 	ucontrols = compat_ptr(p);
-	if (!access_ok(VERIFY_WRITE, ucontrols, n * sizeof(struct v4l2_ext_control)))
+	if (!access_ok(VERIFY_WRITE, ucontrols,
+			n * sizeof(struct v4l2_ext_control32)))
 		return -EFAULT;
 
 	while (--n >= 0) {
@@ -517,9 +677,6 @@ static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 #define VIDIOC_TRY_EXT_CTRLS32  _IOWR('V', 73, struct v4l2_ext_controls32)
 
 #define VIDIOC_OVERLAY32	_IOW ('V', 14, s32)
-#ifdef __OLD_VIDIOC_
-#define VIDIOC_OVERLAY32_OLD	_IOWR('V', 14, s32)
-#endif
 #define VIDIOC_STREAMON32	_IOW ('V', 18, s32)
 #define VIDIOC_STREAMOFF32	_IOW ('V', 19, s32)
 #define VIDIOC_G_INPUT32	_IOR ('V', 38, s32)
@@ -559,9 +716,6 @@ static long do_video_ioctl(struct file *file, unsigned int cmd, unsigned long ar
 	case VIDIOC_S_EXT_CTRLS32: cmd = VIDIOC_S_EXT_CTRLS; break;
 	case VIDIOC_TRY_EXT_CTRLS32: cmd = VIDIOC_TRY_EXT_CTRLS; break;
 	case VIDIOC_OVERLAY32: cmd = VIDIOC_OVERLAY; break;
-#ifdef __OLD_VIDIOC_
-	case VIDIOC_OVERLAY32_OLD: cmd = VIDIOC_OVERLAY; break;
-#endif
 	case VIDIOC_STREAMON32: cmd = VIDIOC_STREAMON; break;
 	case VIDIOC_STREAMOFF32: cmd = VIDIOC_STREAMOFF; break;
 	case VIDIOC_G_INPUT32: cmd = VIDIOC_G_INPUT; break;
@@ -695,14 +849,6 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
 		return ret;
 
 	switch (cmd) {
-#ifdef __OLD_VIDIOC_
-	case VIDIOC_OVERLAY32_OLD:
-	case VIDIOC_S_PARM_OLD:
-	case VIDIOC_S_CTRL_OLD:
-	case VIDIOC_G_AUDIO_OLD:
-	case VIDIOC_G_AUDOUT_OLD:
-	case VIDIOC_CROPCAP_OLD:
-#endif
 	case VIDIOC_QUERYCAP:
 	case VIDIOC_RESERVED:
 	case VIDIOC_ENUM_FMT:
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index ef66d2af0c57..2412f08527aa 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -1364,6 +1364,8 @@ EXPORT_SYMBOL(v4l2_queryctrl);
 
 int v4l2_subdev_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
 {
+	if (qc->id & V4L2_CTRL_FLAG_NEXT_CTRL)
+		return -EINVAL;
 	return v4l2_queryctrl(sd->ctrl_handler, qc);
 }
 EXPORT_SYMBOL(v4l2_subdev_queryctrl);
diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index f51327ef6757..7a720745c3fa 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -17,7 +17,6 @@
 #include <linux/types.h>
 #include <linux/kernel.h>
 
-#define __OLD_VIDIOC_ /* To allow fixing old calls */
 #include <linux/videodev2.h>
 
 #include <media/v4l2-common.h>
@@ -165,6 +164,8 @@ const char *v4l2_type_names[] = {
 	[V4L2_BUF_TYPE_SLICED_VBI_CAPTURE] = "sliced-vbi-cap",
 	[V4L2_BUF_TYPE_SLICED_VBI_OUTPUT]  = "sliced-vbi-out",
 	[V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY] = "vid-out-overlay",
+	[V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE] = "vid-cap-mplane",
+	[V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE] = "vid-out-mplane",
 };
 EXPORT_SYMBOL(v4l2_type_names);
 
@@ -295,37 +296,6 @@ EXPORT_SYMBOL(v4l_printk_ioctl);
 
 /*
  * helper function -- handles userspace copying for ioctl arguments
- */
-
-#ifdef __OLD_VIDIOC_
-static unsigned int
-video_fix_command(unsigned int cmd)
-{
-	switch (cmd) {
-	case VIDIOC_OVERLAY_OLD:
-		cmd = VIDIOC_OVERLAY;
-		break;
-	case VIDIOC_S_PARM_OLD:
-		cmd = VIDIOC_S_PARM;
-		break;
-	case VIDIOC_S_CTRL_OLD:
-		cmd = VIDIOC_S_CTRL;
-		break;
-	case VIDIOC_G_AUDIO_OLD:
-		cmd = VIDIOC_G_AUDIO;
-		break;
-	case VIDIOC_G_AUDOUT_OLD:
-		cmd = VIDIOC_G_AUDOUT;
-		break;
-	case VIDIOC_CROPCAP_OLD:
-		cmd = VIDIOC_CROPCAP;
-		break;
-	}
-	return cmd;
-}
-#endif
-
-/*
  * Obsolete usercopy function - Should be removed soon
  */
 long
@@ -340,9 +310,6 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 	size_t  ctrls_size = 0;
 	void __user *user_ptr = NULL;
 
-#ifdef __OLD_VIDIOC_
-	cmd = video_fix_command(cmd);
-#endif
 	is_ext_ctrl = (cmd == VIDIOC_S_EXT_CTRLS || cmd == VIDIOC_G_EXT_CTRLS ||
 		       cmd == VIDIOC_TRY_EXT_CTRLS);
 
@@ -426,20 +393,33 @@ static void dbgbuf(unsigned int cmd, struct video_device *vfd,
 					struct v4l2_buffer *p)
 {
 	struct v4l2_timecode *tc = &p->timecode;
+	struct v4l2_plane *plane;
+	int i;
 
 	dbgarg(cmd, "%02ld:%02d:%02d.%08ld index=%d, type=%s, "
-		"bytesused=%d, flags=0x%08d, "
-		"field=%0d, sequence=%d, memory=%s, offset/userptr=0x%08lx, length=%d\n",
+		"flags=0x%08d, field=%0d, sequence=%d, memory=%s\n",
 			p->timestamp.tv_sec / 3600,
 			(int)(p->timestamp.tv_sec / 60) % 60,
 			(int)(p->timestamp.tv_sec % 60),
 			(long)p->timestamp.tv_usec,
 			p->index,
 			prt_names(p->type, v4l2_type_names),
-			p->bytesused, p->flags,
-			p->field, p->sequence,
-			prt_names(p->memory, v4l2_memory_names),
-			p->m.userptr, p->length);
+			p->flags, p->field, p->sequence,
+			prt_names(p->memory, v4l2_memory_names));
+
+	if (V4L2_TYPE_IS_MULTIPLANAR(p->type) && p->m.planes) {
+		for (i = 0; i < p->length; ++i) {
+			plane = &p->m.planes[i];
+			dbgarg2("plane %d: bytesused=%d, data_offset=0x%08x "
+				"offset/userptr=0x%08lx, length=%d\n",
+				i, plane->bytesused, plane->data_offset,
+				plane->m.userptr, plane->length);
+		}
+	} else {
+		dbgarg2("bytesused=%d, offset/userptr=0x%08lx, length=%d\n",
+			p->bytesused, p->m.userptr, p->length);
+	}
+
 	dbgarg2("timecode=%02d:%02d:%02d type=%d, "
 		"flags=0x%08d, frames=%d, userbits=0x%08x\n",
 			tc->hours, tc->minutes, tc->seconds,
@@ -467,6 +447,27 @@ static inline void v4l_print_pix_fmt(struct video_device *vfd,
 		fmt->bytesperline, fmt->sizeimage, fmt->colorspace);
 };
 
+static inline void v4l_print_pix_fmt_mplane(struct video_device *vfd,
+					    struct v4l2_pix_format_mplane *fmt)
+{
+	int i;
+
+	dbgarg2("width=%d, height=%d, format=%c%c%c%c, field=%s, "
+		"colorspace=%d, num_planes=%d\n",
+		fmt->width, fmt->height,
+		(fmt->pixelformat & 0xff),
+		(fmt->pixelformat >>  8) & 0xff,
+		(fmt->pixelformat >> 16) & 0xff,
+		(fmt->pixelformat >> 24) & 0xff,
+		prt_names(fmt->field, v4l2_field_names),
+		fmt->colorspace, fmt->num_planes);
+
+	for (i = 0; i < fmt->num_planes; ++i)
+		dbgarg2("plane %d: bytesperline=%d sizeimage=%d\n", i,
+			fmt->plane_fmt[i].bytesperline,
+			fmt->plane_fmt[i].sizeimage);
+}
+
 static inline void v4l_print_ext_ctrls(unsigned int cmd,
 	struct video_device *vfd, struct v4l2_ext_controls *c, int show_vals)
 {
@@ -520,7 +521,12 @@ static int check_fmt(const struct v4l2_ioctl_ops *ops, enum v4l2_buf_type type)
 
 	switch (type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (ops->vidioc_g_fmt_vid_cap)
+		if (ops->vidioc_g_fmt_vid_cap ||
+				ops->vidioc_g_fmt_vid_cap_mplane)
+			return 0;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		if (ops->vidioc_g_fmt_vid_cap_mplane)
 			return 0;
 		break;
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
@@ -528,7 +534,12 @@ static int check_fmt(const struct v4l2_ioctl_ops *ops, enum v4l2_buf_type type)
 			return 0;
 		break;
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
-		if (ops->vidioc_g_fmt_vid_out)
+		if (ops->vidioc_g_fmt_vid_out ||
+				ops->vidioc_g_fmt_vid_out_mplane)
+			return 0;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (ops->vidioc_g_fmt_vid_out_mplane)
 			return 0;
 		break;
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
@@ -559,12 +570,70 @@ static int check_fmt(const struct v4l2_ioctl_ops *ops, enum v4l2_buf_type type)
 	return -EINVAL;
 }
 
+/**
+ * fmt_sp_to_mp() - Convert a single-plane format to its multi-planar 1-plane
+ * equivalent
+ */
+static int fmt_sp_to_mp(const struct v4l2_format *f_sp,
+			struct v4l2_format *f_mp)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &f_mp->fmt.pix_mp;
+	const struct v4l2_pix_format *pix = &f_sp->fmt.pix;
+
+	if (f_sp->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		f_mp->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	else if (f_sp->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		f_mp->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	else
+		return -EINVAL;
+
+	pix_mp->width = pix->width;
+	pix_mp->height = pix->height;
+	pix_mp->pixelformat = pix->pixelformat;
+	pix_mp->field = pix->field;
+	pix_mp->colorspace = pix->colorspace;
+	pix_mp->num_planes = 1;
+	pix_mp->plane_fmt[0].sizeimage = pix->sizeimage;
+	pix_mp->plane_fmt[0].bytesperline = pix->bytesperline;
+
+	return 0;
+}
+
+/**
+ * fmt_mp_to_sp() - Convert a multi-planar 1-plane format to its single-planar
+ * equivalent
+ */
+static int fmt_mp_to_sp(const struct v4l2_format *f_mp,
+			struct v4l2_format *f_sp)
+{
+	const struct v4l2_pix_format_mplane *pix_mp = &f_mp->fmt.pix_mp;
+	struct v4l2_pix_format *pix = &f_sp->fmt.pix;
+
+	if (f_mp->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		f_sp->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	else if (f_mp->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		f_sp->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	else
+		return -EINVAL;
+
+	pix->width = pix_mp->width;
+	pix->height = pix_mp->height;
+	pix->pixelformat = pix_mp->pixelformat;
+	pix->field = pix_mp->field;
+	pix->colorspace = pix_mp->colorspace;
+	pix->sizeimage = pix_mp->plane_fmt[0].sizeimage;
+	pix->bytesperline = pix_mp->plane_fmt[0].bytesperline;
+
+	return 0;
+}
+
 static long __video_do_ioctl(struct file *file,
 		unsigned int cmd, void *arg)
 {
 	struct video_device *vfd = video_devdata(file);
 	const struct v4l2_ioctl_ops *ops = vfd->ioctl_ops;
 	void *fh = file->private_data;
+	struct v4l2_format f_copy;
 	long ret = -EINVAL;
 
 	if (ops == NULL) {
@@ -633,6 +702,11 @@ static long __video_do_ioctl(struct file *file,
 			if (ops->vidioc_enum_fmt_vid_cap)
 				ret = ops->vidioc_enum_fmt_vid_cap(file, fh, f);
 			break;
+		case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+			if (ops->vidioc_enum_fmt_vid_cap_mplane)
+				ret = ops->vidioc_enum_fmt_vid_cap_mplane(file,
+									fh, f);
+			break;
 		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 			if (ops->vidioc_enum_fmt_vid_overlay)
 				ret = ops->vidioc_enum_fmt_vid_overlay(file,
@@ -642,6 +716,11 @@ static long __video_do_ioctl(struct file *file,
 			if (ops->vidioc_enum_fmt_vid_out)
 				ret = ops->vidioc_enum_fmt_vid_out(file, fh, f);
 			break;
+		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+			if (ops->vidioc_enum_fmt_vid_out_mplane)
+				ret = ops->vidioc_enum_fmt_vid_out_mplane(file,
+									fh, f);
+			break;
 		case V4L2_BUF_TYPE_PRIVATE:
 			if (ops->vidioc_enum_fmt_type_private)
 				ret = ops->vidioc_enum_fmt_type_private(file,
@@ -670,22 +749,90 @@ static long __video_do_ioctl(struct file *file,
 
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-			if (ops->vidioc_g_fmt_vid_cap)
+			if (ops->vidioc_g_fmt_vid_cap) {
 				ret = ops->vidioc_g_fmt_vid_cap(file, fh, f);
+			} else if (ops->vidioc_g_fmt_vid_cap_mplane) {
+				if (fmt_sp_to_mp(f, &f_copy))
+					break;
+				ret = ops->vidioc_g_fmt_vid_cap_mplane(file, fh,
+								       &f_copy);
+				if (ret)
+					break;
+
+				/* Driver is currently in multi-planar format,
+				 * we can't return it in single-planar API*/
+				if (f_copy.fmt.pix_mp.num_planes > 1) {
+					ret = -EBUSY;
+					break;
+				}
+
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
 			if (!ret)
 				v4l_print_pix_fmt(vfd, &f->fmt.pix);
 			break;
+		case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+			if (ops->vidioc_g_fmt_vid_cap_mplane) {
+				ret = ops->vidioc_g_fmt_vid_cap_mplane(file,
+									fh, f);
+			} else if (ops->vidioc_g_fmt_vid_cap) {
+				if (fmt_mp_to_sp(f, &f_copy))
+					break;
+				ret = ops->vidioc_g_fmt_vid_cap(file,
+								fh, &f_copy);
+				if (ret)
+					break;
+
+				ret = fmt_sp_to_mp(&f_copy, f);
+			}
+			if (!ret)
+				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+			break;
 		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 			if (ops->vidioc_g_fmt_vid_overlay)
 				ret = ops->vidioc_g_fmt_vid_overlay(file,
 								    fh, f);
 			break;
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
-			if (ops->vidioc_g_fmt_vid_out)
+			if (ops->vidioc_g_fmt_vid_out) {
 				ret = ops->vidioc_g_fmt_vid_out(file, fh, f);
+			} else if (ops->vidioc_g_fmt_vid_out_mplane) {
+				if (fmt_sp_to_mp(f, &f_copy))
+					break;
+				ret = ops->vidioc_g_fmt_vid_out_mplane(file, fh,
+									&f_copy);
+				if (ret)
+					break;
+
+				/* Driver is currently in multi-planar format,
+				 * we can't return it in single-planar API*/
+				if (f_copy.fmt.pix_mp.num_planes > 1) {
+					ret = -EBUSY;
+					break;
+				}
+
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
 			if (!ret)
 				v4l_print_pix_fmt(vfd, &f->fmt.pix);
 			break;
+		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+			if (ops->vidioc_g_fmt_vid_out_mplane) {
+				ret = ops->vidioc_g_fmt_vid_out_mplane(file,
+									fh, f);
+			} else if (ops->vidioc_g_fmt_vid_out) {
+				if (fmt_mp_to_sp(f, &f_copy))
+					break;
+				ret = ops->vidioc_g_fmt_vid_out(file,
+								fh, &f_copy);
+				if (ret)
+					break;
+
+				ret = fmt_sp_to_mp(&f_copy, f);
+			}
+			if (!ret)
+				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+			break;
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 			if (ops->vidioc_g_fmt_vid_out_overlay)
 				ret = ops->vidioc_g_fmt_vid_out_overlay(file,
@@ -729,8 +876,44 @@ static long __video_do_ioctl(struct file *file,
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			CLEAR_AFTER_FIELD(f, fmt.pix);
 			v4l_print_pix_fmt(vfd, &f->fmt.pix);
-			if (ops->vidioc_s_fmt_vid_cap)
+			if (ops->vidioc_s_fmt_vid_cap) {
 				ret = ops->vidioc_s_fmt_vid_cap(file, fh, f);
+			} else if (ops->vidioc_s_fmt_vid_cap_mplane) {
+				if (fmt_sp_to_mp(f, &f_copy))
+					break;
+				ret = ops->vidioc_s_fmt_vid_cap_mplane(file, fh,
+									&f_copy);
+				if (ret)
+					break;
+
+				if (f_copy.fmt.pix_mp.num_planes > 1) {
+					/* Drivers shouldn't adjust from 1-plane
+					 * to more than 1-plane formats */
+					ret = -EBUSY;
+					WARN_ON(1);
+					break;
+				}
+
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
+			break;
+		case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+			CLEAR_AFTER_FIELD(f, fmt.pix_mp);
+			v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+			if (ops->vidioc_s_fmt_vid_cap_mplane) {
+				ret = ops->vidioc_s_fmt_vid_cap_mplane(file,
+									fh, f);
+			} else if (ops->vidioc_s_fmt_vid_cap &&
+					f->fmt.pix_mp.num_planes == 1) {
+				if (fmt_mp_to_sp(f, &f_copy))
+					break;
+				ret = ops->vidioc_s_fmt_vid_cap(file,
+								fh, &f_copy);
+				if (ret)
+					break;
+
+				ret = fmt_sp_to_mp(&f_copy, f);
+			}
 			break;
 		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 			CLEAR_AFTER_FIELD(f, fmt.win);
@@ -741,8 +924,44 @@ static long __video_do_ioctl(struct file *file,
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 			CLEAR_AFTER_FIELD(f, fmt.pix);
 			v4l_print_pix_fmt(vfd, &f->fmt.pix);
-			if (ops->vidioc_s_fmt_vid_out)
+			if (ops->vidioc_s_fmt_vid_out) {
 				ret = ops->vidioc_s_fmt_vid_out(file, fh, f);
+			} else if (ops->vidioc_s_fmt_vid_out_mplane) {
+				if (fmt_sp_to_mp(f, &f_copy))
+					break;
+				ret = ops->vidioc_s_fmt_vid_out_mplane(file, fh,
+									&f_copy);
+				if (ret)
+					break;
+
+				if (f_copy.fmt.pix_mp.num_planes > 1) {
+					/* Drivers shouldn't adjust from 1-plane
+					 * to more than 1-plane formats */
+					ret = -EBUSY;
+					WARN_ON(1);
+					break;
+				}
+
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
+			break;
+		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+			CLEAR_AFTER_FIELD(f, fmt.pix_mp);
+			v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+			if (ops->vidioc_s_fmt_vid_out_mplane) {
+				ret = ops->vidioc_s_fmt_vid_out_mplane(file,
+									fh, f);
+			} else if (ops->vidioc_s_fmt_vid_out &&
+					f->fmt.pix_mp.num_planes == 1) {
+				if (fmt_mp_to_sp(f, &f_copy))
+					break;
+				ret = ops->vidioc_s_fmt_vid_out(file,
+								fh, &f_copy);
+				if (ret)
+					break;
+
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
 			break;
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 			CLEAR_AFTER_FIELD(f, fmt.win);
@@ -791,11 +1010,47 @@ static long __video_do_ioctl(struct file *file,
 		switch (f->type) {
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 			CLEAR_AFTER_FIELD(f, fmt.pix);
-			if (ops->vidioc_try_fmt_vid_cap)
+			if (ops->vidioc_try_fmt_vid_cap) {
 				ret = ops->vidioc_try_fmt_vid_cap(file, fh, f);
+			} else if (ops->vidioc_try_fmt_vid_cap_mplane) {
+				if (fmt_sp_to_mp(f, &f_copy))
+					break;
+				ret = ops->vidioc_try_fmt_vid_cap_mplane(file,
+								fh, &f_copy);
+				if (ret)
+					break;
+
+				if (f_copy.fmt.pix_mp.num_planes > 1) {
+					/* Drivers shouldn't adjust from 1-plane
+					 * to more than 1-plane formats */
+					ret = -EBUSY;
+					WARN_ON(1);
+					break;
+				}
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
 			if (!ret)
 				v4l_print_pix_fmt(vfd, &f->fmt.pix);
 			break;
+		case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+			CLEAR_AFTER_FIELD(f, fmt.pix_mp);
+			if (ops->vidioc_try_fmt_vid_cap_mplane) {
+				ret = ops->vidioc_try_fmt_vid_cap_mplane(file,
+									 fh, f);
+			} else if (ops->vidioc_try_fmt_vid_cap &&
+					f->fmt.pix_mp.num_planes == 1) {
+				if (fmt_mp_to_sp(f, &f_copy))
+					break;
+				ret = ops->vidioc_try_fmt_vid_cap(file,
+								  fh, &f_copy);
+				if (ret)
+					break;
+
+				ret = fmt_sp_to_mp(&f_copy, f);
+			}
+			if (!ret)
+				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+			break;
 		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 			CLEAR_AFTER_FIELD(f, fmt.win);
 			if (ops->vidioc_try_fmt_vid_overlay)
@@ -804,11 +1059,47 @@ static long __video_do_ioctl(struct file *file,
 			break;
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 			CLEAR_AFTER_FIELD(f, fmt.pix);
-			if (ops->vidioc_try_fmt_vid_out)
+			if (ops->vidioc_try_fmt_vid_out) {
 				ret = ops->vidioc_try_fmt_vid_out(file, fh, f);
+			} else if (ops->vidioc_try_fmt_vid_out_mplane) {
+				if (fmt_sp_to_mp(f, &f_copy))
+					break;
+				ret = ops->vidioc_try_fmt_vid_out_mplane(file,
+								fh, &f_copy);
+				if (ret)
+					break;
+
+				if (f_copy.fmt.pix_mp.num_planes > 1) {
+					/* Drivers shouldn't adjust from 1-plane
+					 * to more than 1-plane formats */
+					ret = -EBUSY;
+					WARN_ON(1);
+					break;
+				}
+				ret = fmt_mp_to_sp(&f_copy, f);
+			}
 			if (!ret)
 				v4l_print_pix_fmt(vfd, &f->fmt.pix);
 			break;
+		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+			CLEAR_AFTER_FIELD(f, fmt.pix_mp);
+			if (ops->vidioc_try_fmt_vid_out_mplane) {
+				ret = ops->vidioc_try_fmt_vid_out_mplane(file,
+									 fh, f);
+			} else if (ops->vidioc_try_fmt_vid_out &&
+					f->fmt.pix_mp.num_planes == 1) {
+				if (fmt_mp_to_sp(f, &f_copy))
+					break;
+				ret = ops->vidioc_try_fmt_vid_out(file,
+								  fh, &f_copy);
+				if (ret)
+					break;
+
+				ret = fmt_sp_to_mp(&f_copy, f);
+			}
+			if (!ret)
+				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+			break;
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 			CLEAR_AFTER_FIELD(f, fmt.win);
 			if (ops->vidioc_try_fmt_vid_out_overlay)
@@ -1973,7 +2264,7 @@ static unsigned long cmd_input_size(unsigned int cmd)
 	switch (cmd) {
 		CMDINSIZE(ENUM_FMT,		fmtdesc,	type);
 		CMDINSIZE(G_FMT,		format,		type);
-		CMDINSIZE(QUERYBUF,		buffer,		type);
+		CMDINSIZE(QUERYBUF,		buffer,		length);
 		CMDINSIZE(G_PARM,		streamparm,	type);
 		CMDINSIZE(ENUMSTD,		standard,	index);
 		CMDINSIZE(ENUMINPUT,		input,		index);
@@ -1998,6 +2289,49 @@ static unsigned long cmd_input_size(unsigned int cmd)
 	}
 }
 
+static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
+			    void * __user *user_ptr, void ***kernel_ptr)
+{
+	int ret = 0;
+
+	switch (cmd) {
+	case VIDIOC_QUERYBUF:
+	case VIDIOC_QBUF:
+	case VIDIOC_DQBUF: {
+		struct v4l2_buffer *buf = parg;
+
+		if (V4L2_TYPE_IS_MULTIPLANAR(buf->type) && buf->length > 0) {
+			if (buf->length > VIDEO_MAX_PLANES) {
+				ret = -EINVAL;
+				break;
+			}
+			*user_ptr = (void __user *)buf->m.planes;
+			*kernel_ptr = (void **)&buf->m.planes;
+			*array_size = sizeof(struct v4l2_plane) * buf->length;
+			ret = 1;
+		}
+		break;
+	}
+
+	case VIDIOC_S_EXT_CTRLS:
+	case VIDIOC_G_EXT_CTRLS:
+	case VIDIOC_TRY_EXT_CTRLS: {
+		struct v4l2_ext_controls *ctrls = parg;
+
+		if (ctrls->count != 0) {
+			*user_ptr = (void __user *)ctrls->controls;
+			*kernel_ptr = (void **)&ctrls->controls;
+			*array_size = sizeof(struct v4l2_ext_control)
+				    * ctrls->count;
+			ret = 1;
+		}
+		break;
+	}
+	}
+
+	return ret;
+}
+
 long video_ioctl2(struct file *file,
 	       unsigned int cmd, unsigned long arg)
 {
@@ -2005,15 +2339,10 @@ long video_ioctl2(struct file *file,
 	void    *mbuf = NULL;
 	void	*parg = (void *)arg;
 	long	err  = -EINVAL;
-	int     is_ext_ctrl;
-	size_t  ctrls_size = 0;
+	bool	has_array_args;
+	size_t  array_size = 0;
 	void __user *user_ptr = NULL;
-
-#ifdef __OLD_VIDIOC_
-	cmd = video_fix_command(cmd);
-#endif
-	is_ext_ctrl = (cmd == VIDIOC_S_EXT_CTRLS || cmd == VIDIOC_G_EXT_CTRLS ||
-		       cmd == VIDIOC_TRY_EXT_CTRLS);
+	void	**kernel_ptr = NULL;
 
 	/*  Copy arguments into temp kernel buffer  */
 	if (_IOC_DIR(cmd) != _IOC_NONE) {
@@ -2043,43 +2372,43 @@ long video_ioctl2(struct file *file,
 		}
 	}
 
-	if (is_ext_ctrl) {
-		struct v4l2_ext_controls *p = parg;
+	err = check_array_args(cmd, parg, &array_size, &user_ptr, &kernel_ptr);
+	if (err < 0)
+		goto out;
+	has_array_args = err;
 
-		/* In case of an error, tell the caller that it wasn't
-		   a specific control that caused it. */
-		p->error_idx = p->count;
-		user_ptr = (void __user *)p->controls;
-		if (p->count) {
-			ctrls_size = sizeof(struct v4l2_ext_control) * p->count;
-			/* Note: v4l2_ext_controls fits in sbuf[] so mbuf is still NULL. */
-			mbuf = kmalloc(ctrls_size, GFP_KERNEL);
-			err = -ENOMEM;
-			if (NULL == mbuf)
-				goto out_ext_ctrl;
-			err = -EFAULT;
-			if (copy_from_user(mbuf, user_ptr, ctrls_size))
-				goto out_ext_ctrl;
-			p->controls = mbuf;
-		}
+	if (has_array_args) {
+		/*
+		 * When adding new types of array args, make sure that the
+		 * parent argument to ioctl (which contains the pointer to the
+		 * array) fits into sbuf (so that mbuf will still remain
+		 * unused up to here).
+		 */
+		mbuf = kmalloc(array_size, GFP_KERNEL);
+		err = -ENOMEM;
+		if (NULL == mbuf)
+			goto out_array_args;
+		err = -EFAULT;
+		if (copy_from_user(mbuf, user_ptr, array_size))
+			goto out_array_args;
+		*kernel_ptr = mbuf;
 	}
 
 	/* Handles IOCTL */
 	err = __video_do_ioctl(file, cmd, parg);
 	if (err == -ENOIOCTLCMD)
 		err = -EINVAL;
-	if (is_ext_ctrl) {
-		struct v4l2_ext_controls *p = parg;
 
-		p->controls = (void *)user_ptr;
-		if (p->count && err == 0 && copy_to_user(user_ptr, mbuf, ctrls_size))
+	if (has_array_args) {
+		*kernel_ptr = user_ptr;
+		if (copy_to_user(user_ptr, mbuf, array_size))
 			err = -EFAULT;
-		goto out_ext_ctrl;
+		goto out_array_args;
 	}
 	if (err < 0)
 		goto out;
 
-out_ext_ctrl:
+out_array_args:
 	/*  Copy results into user buffer  */
 	switch (_IOC_DIR(cmd)) {
 	case _IOC_READ:
diff --git a/drivers/media/video/v4l2-mem2mem.c b/drivers/media/video/v4l2-mem2mem.c
index ac832a28e18e..a78e5c9be1a2 100644
--- a/drivers/media/video/v4l2-mem2mem.c
+++ b/drivers/media/video/v4l2-mem2mem.c
@@ -17,7 +17,7 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 
-#include <media/videobuf-core.h>
+#include <media/videobuf2-core.h>
 #include <media/v4l2-mem2mem.h>
 
 MODULE_DESCRIPTION("Mem to mem device framework for videobuf");
@@ -65,21 +65,16 @@ struct v4l2_m2m_dev {
 static struct v4l2_m2m_queue_ctx *get_queue_ctx(struct v4l2_m2m_ctx *m2m_ctx,
 						enum v4l2_buf_type type)
 {
-	switch (type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return &m2m_ctx->cap_q_ctx;
-	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	if (V4L2_TYPE_IS_OUTPUT(type))
 		return &m2m_ctx->out_q_ctx;
-	default:
-		printk(KERN_ERR "Invalid buffer type\n");
-		return NULL;
-	}
+	else
+		return &m2m_ctx->cap_q_ctx;
 }
 
 /**
- * v4l2_m2m_get_vq() - return videobuf_queue for the given type
+ * v4l2_m2m_get_vq() - return vb2_queue for the given type
  */
-struct videobuf_queue *v4l2_m2m_get_vq(struct v4l2_m2m_ctx *m2m_ctx,
+struct vb2_queue *v4l2_m2m_get_vq(struct v4l2_m2m_ctx *m2m_ctx,
 				       enum v4l2_buf_type type)
 {
 	struct v4l2_m2m_queue_ctx *q_ctx;
@@ -95,27 +90,20 @@ EXPORT_SYMBOL(v4l2_m2m_get_vq);
 /**
  * v4l2_m2m_next_buf() - return next buffer from the list of ready buffers
  */
-void *v4l2_m2m_next_buf(struct v4l2_m2m_ctx *m2m_ctx, enum v4l2_buf_type type)
+void *v4l2_m2m_next_buf(struct v4l2_m2m_queue_ctx *q_ctx)
 {
-	struct v4l2_m2m_queue_ctx *q_ctx;
-	struct videobuf_buffer *vb = NULL;
+	struct v4l2_m2m_buffer *b = NULL;
 	unsigned long flags;
 
-	q_ctx = get_queue_ctx(m2m_ctx, type);
-	if (!q_ctx)
-		return NULL;
-
-	spin_lock_irqsave(q_ctx->q.irqlock, flags);
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
 
 	if (list_empty(&q_ctx->rdy_queue))
 		goto end;
 
-	vb = list_entry(q_ctx->rdy_queue.next, struct videobuf_buffer, queue);
-	vb->state = VIDEOBUF_ACTIVE;
-
+	b = list_entry(q_ctx->rdy_queue.next, struct v4l2_m2m_buffer, list);
 end:
-	spin_unlock_irqrestore(q_ctx->q.irqlock, flags);
-	return vb;
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
+	return &b->vb;
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_next_buf);
 
@@ -123,26 +111,21 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_next_buf);
  * v4l2_m2m_buf_remove() - take off a buffer from the list of ready buffers and
  * return it
  */
-void *v4l2_m2m_buf_remove(struct v4l2_m2m_ctx *m2m_ctx, enum v4l2_buf_type type)
+void *v4l2_m2m_buf_remove(struct v4l2_m2m_queue_ctx *q_ctx)
 {
-	struct v4l2_m2m_queue_ctx *q_ctx;
-	struct videobuf_buffer *vb = NULL;
+	struct v4l2_m2m_buffer *b = NULL;
 	unsigned long flags;
 
-	q_ctx = get_queue_ctx(m2m_ctx, type);
-	if (!q_ctx)
-		return NULL;
-
-	spin_lock_irqsave(q_ctx->q.irqlock, flags);
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
 	if (!list_empty(&q_ctx->rdy_queue)) {
-		vb = list_entry(q_ctx->rdy_queue.next, struct videobuf_buffer,
-				queue);
-		list_del(&vb->queue);
+		b = list_entry(q_ctx->rdy_queue.next, struct v4l2_m2m_buffer,
+				list);
+		list_del(&b->list);
 		q_ctx->num_rdy--;
 	}
-	spin_unlock_irqrestore(q_ctx->q.irqlock, flags);
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
 
-	return vb;
+	return &b->vb;
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_buf_remove);
 
@@ -235,20 +218,20 @@ static void v4l2_m2m_try_schedule(struct v4l2_m2m_ctx *m2m_ctx)
 		return;
 	}
 
-	spin_lock_irqsave(m2m_ctx->out_q_ctx.q.irqlock, flags);
+	spin_lock_irqsave(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 	if (list_empty(&m2m_ctx->out_q_ctx.rdy_queue)) {
-		spin_unlock_irqrestore(m2m_ctx->out_q_ctx.q.irqlock, flags);
+		spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 		spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags_job);
 		dprintk("No input buffers available\n");
 		return;
 	}
 	if (list_empty(&m2m_ctx->cap_q_ctx.rdy_queue)) {
-		spin_unlock_irqrestore(m2m_ctx->out_q_ctx.q.irqlock, flags);
+		spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 		spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags_job);
 		dprintk("No output buffers available\n");
 		return;
 	}
-	spin_unlock_irqrestore(m2m_ctx->out_q_ctx.q.irqlock, flags);
+	spin_unlock_irqrestore(&m2m_ctx->out_q_ctx.rdy_spinlock, flags);
 
 	if (m2m_dev->m2m_ops->job_ready
 		&& (!m2m_dev->m2m_ops->job_ready(m2m_ctx->priv))) {
@@ -291,6 +274,7 @@ void v4l2_m2m_job_finish(struct v4l2_m2m_dev *m2m_dev,
 
 	list_del(&m2m_dev->curr_ctx->queue);
 	m2m_dev->curr_ctx->job_flags &= ~(TRANS_QUEUED | TRANS_RUNNING);
+	wake_up(&m2m_dev->curr_ctx->finished);
 	m2m_dev->curr_ctx = NULL;
 
 	spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags);
@@ -309,10 +293,10 @@ EXPORT_SYMBOL(v4l2_m2m_job_finish);
 int v4l2_m2m_reqbufs(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		     struct v4l2_requestbuffers *reqbufs)
 {
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 
 	vq = v4l2_m2m_get_vq(m2m_ctx, reqbufs->type);
-	return videobuf_reqbufs(vq, reqbufs);
+	return vb2_reqbufs(vq, reqbufs);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_reqbufs);
 
@@ -324,15 +308,22 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_reqbufs);
 int v4l2_m2m_querybuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		      struct v4l2_buffer *buf)
 {
-	struct videobuf_queue *vq;
-	int ret;
+	struct vb2_queue *vq;
+	int ret = 0;
+	unsigned int i;
 
 	vq = v4l2_m2m_get_vq(m2m_ctx, buf->type);
-	ret = videobuf_querybuf(vq, buf);
-
-	if (buf->memory == V4L2_MEMORY_MMAP
-	    && vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		buf->m.offset += DST_QUEUE_OFF_BASE;
+	ret = vb2_querybuf(vq, buf);
+
+	/* Adjust MMAP memory offsets for the CAPTURE queue */
+	if (buf->memory == V4L2_MEMORY_MMAP && !V4L2_TYPE_IS_OUTPUT(vq->type)) {
+		if (V4L2_TYPE_IS_MULTIPLANAR(vq->type)) {
+			for (i = 0; i < buf->length; ++i)
+				buf->m.planes[i].m.mem_offset
+					+= DST_QUEUE_OFF_BASE;
+		} else {
+			buf->m.offset += DST_QUEUE_OFF_BASE;
+		}
 	}
 
 	return ret;
@@ -346,11 +337,11 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_querybuf);
 int v4l2_m2m_qbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		  struct v4l2_buffer *buf)
 {
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 	int ret;
 
 	vq = v4l2_m2m_get_vq(m2m_ctx, buf->type);
-	ret = videobuf_qbuf(vq, buf);
+	ret = vb2_qbuf(vq, buf);
 	if (!ret)
 		v4l2_m2m_try_schedule(m2m_ctx);
 
@@ -365,10 +356,10 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_qbuf);
 int v4l2_m2m_dqbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		   struct v4l2_buffer *buf)
 {
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 
 	vq = v4l2_m2m_get_vq(m2m_ctx, buf->type);
-	return videobuf_dqbuf(vq, buf, file->f_flags & O_NONBLOCK);
+	return vb2_dqbuf(vq, buf, file->f_flags & O_NONBLOCK);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_dqbuf);
 
@@ -378,11 +369,11 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_dqbuf);
 int v4l2_m2m_streamon(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		      enum v4l2_buf_type type)
 {
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 	int ret;
 
 	vq = v4l2_m2m_get_vq(m2m_ctx, type);
-	ret = videobuf_streamon(vq);
+	ret = vb2_streamon(vq, type);
 	if (!ret)
 		v4l2_m2m_try_schedule(m2m_ctx);
 
@@ -396,10 +387,10 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_streamon);
 int v4l2_m2m_streamoff(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		       enum v4l2_buf_type type)
 {
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 
 	vq = v4l2_m2m_get_vq(m2m_ctx, type);
-	return videobuf_streamoff(vq);
+	return vb2_streamoff(vq, type);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_streamoff);
 
@@ -414,44 +405,53 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_streamoff);
 unsigned int v4l2_m2m_poll(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 			   struct poll_table_struct *wait)
 {
-	struct videobuf_queue *src_q, *dst_q;
-	struct videobuf_buffer *src_vb = NULL, *dst_vb = NULL;
+	struct vb2_queue *src_q, *dst_q;
+	struct vb2_buffer *src_vb = NULL, *dst_vb = NULL;
 	unsigned int rc = 0;
+	unsigned long flags;
 
 	src_q = v4l2_m2m_get_src_vq(m2m_ctx);
 	dst_q = v4l2_m2m_get_dst_vq(m2m_ctx);
 
-	videobuf_queue_lock(src_q);
-	videobuf_queue_lock(dst_q);
-
-	if (src_q->streaming && !list_empty(&src_q->stream))
-		src_vb = list_first_entry(&src_q->stream,
-					  struct videobuf_buffer, stream);
-	if (dst_q->streaming && !list_empty(&dst_q->stream))
-		dst_vb = list_first_entry(&dst_q->stream,
-					  struct videobuf_buffer, stream);
-
-	if (!src_vb && !dst_vb) {
+	/*
+	 * There has to be at least one buffer queued on each queued_list, which
+	 * means either in driver already or waiting for driver to claim it
+	 * and start processing.
+	 */
+	if ((!src_q->streaming || list_empty(&src_q->queued_list))
+		&& (!dst_q->streaming || list_empty(&dst_q->queued_list))) {
 		rc = POLLERR;
 		goto end;
 	}
 
-	if (src_vb) {
-		poll_wait(file, &src_vb->done, wait);
-		if (src_vb->state == VIDEOBUF_DONE
-		    || src_vb->state == VIDEOBUF_ERROR)
-			rc |= POLLOUT | POLLWRNORM;
-	}
-	if (dst_vb) {
-		poll_wait(file, &dst_vb->done, wait);
-		if (dst_vb->state == VIDEOBUF_DONE
-		    || dst_vb->state == VIDEOBUF_ERROR)
-			rc |= POLLIN | POLLRDNORM;
-	}
+	if (m2m_ctx->m2m_dev->m2m_ops->unlock)
+		m2m_ctx->m2m_dev->m2m_ops->unlock(m2m_ctx->priv);
+
+	poll_wait(file, &src_q->done_wq, wait);
+	poll_wait(file, &dst_q->done_wq, wait);
+
+	if (m2m_ctx->m2m_dev->m2m_ops->lock)
+		m2m_ctx->m2m_dev->m2m_ops->lock(m2m_ctx->priv);
+
+	spin_lock_irqsave(&src_q->done_lock, flags);
+	if (!list_empty(&src_q->done_list))
+		src_vb = list_first_entry(&src_q->done_list, struct vb2_buffer,
+						done_entry);
+	if (src_vb && (src_vb->state == VB2_BUF_STATE_DONE
+			|| src_vb->state == VB2_BUF_STATE_ERROR))
+		rc |= POLLOUT | POLLWRNORM;
+	spin_unlock_irqrestore(&src_q->done_lock, flags);
+
+	spin_lock_irqsave(&dst_q->done_lock, flags);
+	if (!list_empty(&dst_q->done_list))
+		dst_vb = list_first_entry(&dst_q->done_list, struct vb2_buffer,
+						done_entry);
+	if (dst_vb && (dst_vb->state == VB2_BUF_STATE_DONE
+			|| dst_vb->state == VB2_BUF_STATE_ERROR))
+		rc |= POLLIN | POLLRDNORM;
+	spin_unlock_irqrestore(&dst_q->done_lock, flags);
 
 end:
-	videobuf_queue_unlock(dst_q);
-	videobuf_queue_unlock(src_q);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_poll);
@@ -470,7 +470,7 @@ int v4l2_m2m_mmap(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 			 struct vm_area_struct *vma)
 {
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-	struct videobuf_queue *vq;
+	struct vb2_queue *vq;
 
 	if (offset < DST_QUEUE_OFF_BASE) {
 		vq = v4l2_m2m_get_src_vq(m2m_ctx);
@@ -479,7 +479,7 @@ int v4l2_m2m_mmap(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 		vma->vm_pgoff -= (DST_QUEUE_OFF_BASE >> PAGE_SHIFT);
 	}
 
-	return videobuf_mmap_mapper(vq, vma);
+	return vb2_mmap(vq, vma);
 }
 EXPORT_SYMBOL(v4l2_m2m_mmap);
 
@@ -531,36 +531,41 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_release);
  *
  * Usually called from driver's open() function.
  */
-struct v4l2_m2m_ctx *v4l2_m2m_ctx_init(void *priv, struct v4l2_m2m_dev *m2m_dev,
-			void (*vq_init)(void *priv, struct videobuf_queue *,
-					enum v4l2_buf_type))
+struct v4l2_m2m_ctx *v4l2_m2m_ctx_init(struct v4l2_m2m_dev *m2m_dev,
+		void *drv_priv,
+		int (*queue_init)(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq))
 {
 	struct v4l2_m2m_ctx *m2m_ctx;
 	struct v4l2_m2m_queue_ctx *out_q_ctx, *cap_q_ctx;
-
-	if (!vq_init)
-		return ERR_PTR(-EINVAL);
+	int ret;
 
 	m2m_ctx = kzalloc(sizeof *m2m_ctx, GFP_KERNEL);
 	if (!m2m_ctx)
 		return ERR_PTR(-ENOMEM);
 
-	m2m_ctx->priv = priv;
+	m2m_ctx->priv = drv_priv;
 	m2m_ctx->m2m_dev = m2m_dev;
+	init_waitqueue_head(&m2m_ctx->finished);
 
-	out_q_ctx = get_queue_ctx(m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	cap_q_ctx = get_queue_ctx(m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	out_q_ctx = &m2m_ctx->out_q_ctx;
+	cap_q_ctx = &m2m_ctx->cap_q_ctx;
 
 	INIT_LIST_HEAD(&out_q_ctx->rdy_queue);
 	INIT_LIST_HEAD(&cap_q_ctx->rdy_queue);
+	spin_lock_init(&out_q_ctx->rdy_spinlock);
+	spin_lock_init(&cap_q_ctx->rdy_spinlock);
 
 	INIT_LIST_HEAD(&m2m_ctx->queue);
 
-	vq_init(priv, &out_q_ctx->q, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	vq_init(priv, &cap_q_ctx->q, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-	out_q_ctx->q.priv_data = cap_q_ctx->q.priv_data = priv;
+	ret = queue_init(drv_priv, &out_q_ctx->q, &cap_q_ctx->q);
+
+	if (ret)
+		goto err;
 
 	return m2m_ctx;
+err:
+	kfree(m2m_ctx);
+	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_ctx_init);
 
@@ -572,7 +577,6 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_ctx_init);
 void v4l2_m2m_ctx_release(struct v4l2_m2m_ctx *m2m_ctx)
 {
 	struct v4l2_m2m_dev *m2m_dev;
-	struct videobuf_buffer *vb;
 	unsigned long flags;
 
 	m2m_dev = m2m_ctx->m2m_dev;
@@ -582,10 +586,7 @@ void v4l2_m2m_ctx_release(struct v4l2_m2m_ctx *m2m_ctx)
 		spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags);
 		m2m_dev->m2m_ops->job_abort(m2m_ctx->priv);
 		dprintk("m2m_ctx %p running, will wait to complete", m2m_ctx);
-		vb = v4l2_m2m_next_dst_buf(m2m_ctx);
-		BUG_ON(NULL == vb);
-		wait_event(vb->done, vb->state != VIDEOBUF_ACTIVE
-				     && vb->state != VIDEOBUF_QUEUED);
+		wait_event(m2m_ctx->finished, !(m2m_ctx->job_flags & TRANS_RUNNING));
 	} else if (m2m_ctx->job_flags & TRANS_QUEUED) {
 		list_del(&m2m_ctx->queue);
 		m2m_ctx->job_flags &= ~(TRANS_QUEUED | TRANS_RUNNING);
@@ -597,11 +598,8 @@ void v4l2_m2m_ctx_release(struct v4l2_m2m_ctx *m2m_ctx)
 		spin_unlock_irqrestore(&m2m_dev->job_spinlock, flags);
 	}
 
-	videobuf_stop(&m2m_ctx->cap_q_ctx.q);
-	videobuf_stop(&m2m_ctx->out_q_ctx.q);
-
-	videobuf_mmap_free(&m2m_ctx->cap_q_ctx.q);
-	videobuf_mmap_free(&m2m_ctx->out_q_ctx.q);
+	vb2_queue_release(&m2m_ctx->cap_q_ctx.q);
+	vb2_queue_release(&m2m_ctx->out_q_ctx.q);
 
 	kfree(m2m_ctx);
 }
@@ -611,23 +609,21 @@ EXPORT_SYMBOL_GPL(v4l2_m2m_ctx_release);
  * v4l2_m2m_buf_queue() - add a buffer to the proper ready buffers list.
  *
  * Call from buf_queue(), videobuf_queue_ops callback.
- *
- * Locking: Caller holds q->irqlock (taken by videobuf before calling buf_queue
- * callback in the driver).
  */
-void v4l2_m2m_buf_queue(struct v4l2_m2m_ctx *m2m_ctx, struct videobuf_queue *vq,
-			struct videobuf_buffer *vb)
+void v4l2_m2m_buf_queue(struct v4l2_m2m_ctx *m2m_ctx, struct vb2_buffer *vb)
 {
+	struct v4l2_m2m_buffer *b = container_of(vb, struct v4l2_m2m_buffer, vb);
 	struct v4l2_m2m_queue_ctx *q_ctx;
+	unsigned long flags;
 
-	q_ctx = get_queue_ctx(m2m_ctx, vq->type);
+	q_ctx = get_queue_ctx(m2m_ctx, vb->vb2_queue->type);
 	if (!q_ctx)
 		return;
 
-	list_add_tail(&vb->queue, &q_ctx->rdy_queue);
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
+	list_add_tail(&b->list, &q_ctx->rdy_queue);
 	q_ctx->num_rdy++;
-
-	vb->state = VIDEOBUF_QUEUED;
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_buf_queue);
 
diff --git a/drivers/media/video/via-camera.c b/drivers/media/video/via-camera.c
index 2f973cd56408..3f0146fcf752 100644
--- a/drivers/media/video/via-camera.c
+++ b/drivers/media/video/via-camera.c
@@ -1246,6 +1246,62 @@ static const struct v4l2_ioctl_ops viacam_ioctl_ops = {
 /*
  * Power management.
  */
+#ifdef CONFIG_PM
+
+static int viacam_suspend(void *priv)
+{
+	struct via_camera *cam = priv;
+	enum viacam_opstate state = cam->opstate;
+
+	if (cam->opstate != S_IDLE) {
+		viacam_stop_engine(cam);
+		cam->opstate = state; /* So resume restarts */
+	}
+
+	return 0;
+}
+
+static int viacam_resume(void *priv)
+{
+	struct via_camera *cam = priv;
+	int ret = 0;
+
+	/*
+	 * Get back to a reasonable operating state.
+	 */
+	via_write_reg_mask(VIASR, 0x78, 0, 0x80);
+	via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
+	viacam_int_disable(cam);
+	set_bit(CF_CONFIG_NEEDED, &cam->flags);
+	/*
+	 * Make sure the sensor's power state is correct
+	 */
+	if (cam->users > 0)
+		via_sensor_power_up(cam);
+	else
+		via_sensor_power_down(cam);
+	/*
+	 * If it was operating, try to restart it.
+	 */
+	if (cam->opstate != S_IDLE) {
+		mutex_lock(&cam->lock);
+		ret = viacam_configure_sensor(cam);
+		if (!ret)
+			ret = viacam_config_controller(cam);
+		mutex_unlock(&cam->lock);
+		if (!ret)
+			viacam_start_engine(cam);
+	}
+
+	return ret;
+}
+
+static struct viafb_pm_hooks viacam_pm_hooks = {
+	.suspend = viacam_suspend,
+	.resume = viacam_resume
+};
+
+#endif /* CONFIG_PM */
 
 /*
  * Setup stuff.
@@ -1369,6 +1425,14 @@ static __devinit int viacam_probe(struct platform_device *pdev)
 		goto out_irq;
 	video_set_drvdata(&cam->vdev, cam);
 
+#ifdef CONFIG_PM
+	/*
+	 * Hook into PM events
+	 */
+	viacam_pm_hooks.private = cam;
+	viafb_pm_register(&viacam_pm_hooks);
+#endif
+
 	/* Power the sensor down until somebody opens the device */
 	via_sensor_power_down(cam);
 	return 0;
diff --git a/drivers/media/video/videobuf2-core.c b/drivers/media/video/videobuf2-core.c
new file mode 100644
index 000000000000..cc7ab0a17b68
--- /dev/null
+++ b/drivers/media/video/videobuf2-core.c
@@ -0,0 +1,1804 @@
+/*
+ * videobuf2-core.c - V4L2 driver helper framework
+ *
+ * Copyright (C) 2010 Samsung Electronics
+ *
+ * Author: Pawel Osciak <p.osciak@samsung.com>
+ *	   Marek Szyprowski <m.szyprowski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+
+#include <media/videobuf2-core.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define dprintk(level, fmt, arg...)					\
+	do {								\
+		if (debug >= level)					\
+			printk(KERN_DEBUG "vb2: " fmt, ## arg);		\
+	} while (0)
+
+#define call_memop(q, plane, op, args...)				\
+	(((q)->mem_ops->op) ?						\
+		((q)->mem_ops->op(args)) : 0)
+
+#define call_qop(q, op, args...)					\
+	(((q)->ops->op) ? ((q)->ops->op(args)) : 0)
+
+/**
+ * __vb2_buf_mem_alloc() - allocate video memory for the given buffer
+ */
+static int __vb2_buf_mem_alloc(struct vb2_buffer *vb,
+				unsigned long *plane_sizes)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	void *mem_priv;
+	int plane;
+
+	/* Allocate memory for all planes in this buffer */
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		mem_priv = call_memop(q, plane, alloc, q->alloc_ctx[plane],
+					plane_sizes[plane]);
+		if (!mem_priv)
+			goto free;
+
+		/* Associate allocator private data with this plane */
+		vb->planes[plane].mem_priv = mem_priv;
+		vb->v4l2_planes[plane].length = plane_sizes[plane];
+	}
+
+	return 0;
+free:
+	/* Free already allocated memory if one of the allocations failed */
+	for (; plane > 0; --plane)
+		call_memop(q, plane, put, vb->planes[plane - 1].mem_priv);
+
+	return -ENOMEM;
+}
+
+/**
+ * __vb2_buf_mem_free() - free memory of the given buffer
+ */
+static void __vb2_buf_mem_free(struct vb2_buffer *vb)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	unsigned int plane;
+
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		call_memop(q, plane, put, vb->planes[plane].mem_priv);
+		vb->planes[plane].mem_priv = NULL;
+		dprintk(3, "Freed plane %d of buffer %d\n",
+				plane, vb->v4l2_buf.index);
+	}
+}
+
+/**
+ * __vb2_buf_userptr_put() - release userspace memory associated with
+ * a USERPTR buffer
+ */
+static void __vb2_buf_userptr_put(struct vb2_buffer *vb)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	unsigned int plane;
+
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		void *mem_priv = vb->planes[plane].mem_priv;
+
+		if (mem_priv) {
+			call_memop(q, plane, put_userptr, mem_priv);
+			vb->planes[plane].mem_priv = NULL;
+		}
+	}
+}
+
+/**
+ * __setup_offsets() - setup unique offsets ("cookies") for every plane in
+ * every buffer on the queue
+ */
+static void __setup_offsets(struct vb2_queue *q)
+{
+	unsigned int buffer, plane;
+	struct vb2_buffer *vb;
+	unsigned long off = 0;
+
+	for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+		vb = q->bufs[buffer];
+		if (!vb)
+			continue;
+
+		for (plane = 0; plane < vb->num_planes; ++plane) {
+			vb->v4l2_planes[plane].m.mem_offset = off;
+
+			dprintk(3, "Buffer %d, plane %d offset 0x%08lx\n",
+					buffer, plane, off);
+
+			off += vb->v4l2_planes[plane].length;
+			off = PAGE_ALIGN(off);
+		}
+	}
+}
+
+/**
+ * __vb2_queue_alloc() - allocate videobuf buffer structures and (for MMAP type)
+ * video buffer memory for all buffers/planes on the queue and initializes the
+ * queue
+ *
+ * Returns the number of buffers successfully allocated.
+ */
+static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
+			     unsigned int num_buffers, unsigned int num_planes,
+			     unsigned long plane_sizes[])
+{
+	unsigned int buffer;
+	struct vb2_buffer *vb;
+	int ret;
+
+	for (buffer = 0; buffer < num_buffers; ++buffer) {
+		/* Allocate videobuf buffer structures */
+		vb = kzalloc(q->buf_struct_size, GFP_KERNEL);
+		if (!vb) {
+			dprintk(1, "Memory alloc for buffer struct failed\n");
+			break;
+		}
+
+		/* Length stores number of planes for multiplanar buffers */
+		if (V4L2_TYPE_IS_MULTIPLANAR(q->type))
+			vb->v4l2_buf.length = num_planes;
+
+		vb->state = VB2_BUF_STATE_DEQUEUED;
+		vb->vb2_queue = q;
+		vb->num_planes = num_planes;
+		vb->v4l2_buf.index = buffer;
+		vb->v4l2_buf.type = q->type;
+		vb->v4l2_buf.memory = memory;
+
+		/* Allocate video buffer memory for the MMAP type */
+		if (memory == V4L2_MEMORY_MMAP) {
+			ret = __vb2_buf_mem_alloc(vb, plane_sizes);
+			if (ret) {
+				dprintk(1, "Failed allocating memory for "
+						"buffer %d\n", buffer);
+				kfree(vb);
+				break;
+			}
+			/*
+			 * Call the driver-provided buffer initialization
+			 * callback, if given. An error in initialization
+			 * results in queue setup failure.
+			 */
+			ret = call_qop(q, buf_init, vb);
+			if (ret) {
+				dprintk(1, "Buffer %d %p initialization"
+					" failed\n", buffer, vb);
+				__vb2_buf_mem_free(vb);
+				kfree(vb);
+				break;
+			}
+		}
+
+		q->bufs[buffer] = vb;
+	}
+
+	q->num_buffers = buffer;
+
+	__setup_offsets(q);
+
+	dprintk(1, "Allocated %d buffers, %d plane(s) each\n",
+			q->num_buffers, num_planes);
+
+	return buffer;
+}
+
+/**
+ * __vb2_free_mem() - release all video buffer memory for a given queue
+ */
+static void __vb2_free_mem(struct vb2_queue *q)
+{
+	unsigned int buffer;
+	struct vb2_buffer *vb;
+
+	for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+		vb = q->bufs[buffer];
+		if (!vb)
+			continue;
+
+		/* Free MMAP buffers or release USERPTR buffers */
+		if (q->memory == V4L2_MEMORY_MMAP)
+			__vb2_buf_mem_free(vb);
+		else
+			__vb2_buf_userptr_put(vb);
+	}
+}
+
+/**
+ * __vb2_queue_free() - free the queue - video memory and related information
+ * and return the queue to an uninitialized state. Might be called even if the
+ * queue has already been freed.
+ */
+static int __vb2_queue_free(struct vb2_queue *q)
+{
+	unsigned int buffer;
+
+	/* Call driver-provided cleanup function for each buffer, if provided */
+	if (q->ops->buf_cleanup) {
+		for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+			if (NULL == q->bufs[buffer])
+				continue;
+			q->ops->buf_cleanup(q->bufs[buffer]);
+		}
+	}
+
+	/* Release video buffer memory */
+	__vb2_free_mem(q);
+
+	/* Free videobuf buffers */
+	for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+		kfree(q->bufs[buffer]);
+		q->bufs[buffer] = NULL;
+	}
+
+	q->num_buffers = 0;
+	q->memory = 0;
+
+	return 0;
+}
+
+/**
+ * __verify_planes_array() - verify that the planes array passed in struct
+ * v4l2_buffer from userspace can be safely used
+ */
+static int __verify_planes_array(struct vb2_buffer *vb, struct v4l2_buffer *b)
+{
+	/* Is memory for copying plane information present? */
+	if (NULL == b->m.planes) {
+		dprintk(1, "Multi-planar buffer passed but "
+			   "planes array not provided\n");
+		return -EINVAL;
+	}
+
+	if (b->length < vb->num_planes || b->length > VIDEO_MAX_PLANES) {
+		dprintk(1, "Incorrect planes array length, "
+			   "expected %d, got %d\n", vb->num_planes, b->length);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * __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, struct v4l2_buffer *b)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	int ret = 0;
+
+	/* Copy back data such as timestamp, input, etc. */
+	memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
+	b->input = vb->v4l2_buf.input;
+	b->reserved = vb->v4l2_buf.reserved;
+
+	if (V4L2_TYPE_IS_MULTIPLANAR(q->type)) {
+		ret = __verify_planes_array(vb, b);
+		if (ret)
+			return ret;
+
+		/*
+		 * Fill in plane-related data if userspace provided an array
+		 * for it. The memory and size is verified above.
+		 */
+		memcpy(b->m.planes, vb->v4l2_planes,
+			b->length * sizeof(struct v4l2_plane));
+	} else {
+		/*
+		 * We use length and offset in v4l2_planes array even for
+		 * single-planar buffers, but userspace does not.
+		 */
+		b->length = vb->v4l2_planes[0].length;
+		b->bytesused = vb->v4l2_planes[0].bytesused;
+		if (q->memory == V4L2_MEMORY_MMAP)
+			b->m.offset = vb->v4l2_planes[0].m.mem_offset;
+		else if (q->memory == V4L2_MEMORY_USERPTR)
+			b->m.userptr = vb->v4l2_planes[0].m.userptr;
+	}
+
+	b->flags = 0;
+
+	switch (vb->state) {
+	case VB2_BUF_STATE_QUEUED:
+	case VB2_BUF_STATE_ACTIVE:
+		b->flags |= V4L2_BUF_FLAG_QUEUED;
+		break;
+	case VB2_BUF_STATE_ERROR:
+		b->flags |= V4L2_BUF_FLAG_ERROR;
+		/* fall through */
+	case VB2_BUF_STATE_DONE:
+		b->flags |= V4L2_BUF_FLAG_DONE;
+		break;
+	case VB2_BUF_STATE_DEQUEUED:
+		/* nothing */
+		break;
+	}
+
+	if (vb->num_planes_mapped == vb->num_planes)
+		b->flags |= V4L2_BUF_FLAG_MAPPED;
+
+	return ret;
+}
+
+/**
+ * vb2_querybuf() - query video buffer information
+ * @q:		videobuf queue
+ * @b:		buffer struct passed from userspace to vidioc_querybuf handler
+ *		in driver
+ *
+ * Should be called from vidioc_querybuf ioctl handler in driver.
+ * This function will verify the passed v4l2_buffer structure and fill 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_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
+{
+	struct vb2_buffer *vb;
+
+	if (b->type != q->type) {
+		dprintk(1, "querybuf: wrong buffer type\n");
+		return -EINVAL;
+	}
+
+	if (b->index >= q->num_buffers) {
+		dprintk(1, "querybuf: buffer index out of range\n");
+		return -EINVAL;
+	}
+	vb = q->bufs[b->index];
+
+	return __fill_v4l2_buffer(vb, b);
+}
+EXPORT_SYMBOL(vb2_querybuf);
+
+/**
+ * __verify_userptr_ops() - verify that all memory operations required for
+ * USERPTR queue type have been provided
+ */
+static int __verify_userptr_ops(struct vb2_queue *q)
+{
+	if (!(q->io_modes & VB2_USERPTR) || !q->mem_ops->get_userptr ||
+	    !q->mem_ops->put_userptr)
+		return -EINVAL;
+
+	return 0;
+}
+
+/**
+ * __verify_mmap_ops() - verify that all memory operations required for
+ * MMAP queue type have been provided
+ */
+static int __verify_mmap_ops(struct vb2_queue *q)
+{
+	if (!(q->io_modes & VB2_MMAP) || !q->mem_ops->alloc ||
+	    !q->mem_ops->put || !q->mem_ops->mmap)
+		return -EINVAL;
+
+	return 0;
+}
+
+/**
+ * __buffers_in_use() - return true if any buffers on the queue are in use and
+ * the queue cannot be freed (by the means of REQBUFS(0)) call
+ */
+static bool __buffers_in_use(struct vb2_queue *q)
+{
+	unsigned int buffer, plane;
+	struct vb2_buffer *vb;
+
+	for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+		vb = q->bufs[buffer];
+		for (plane = 0; plane < vb->num_planes; ++plane) {
+			/*
+			 * If num_users() has not been provided, call_memop
+			 * will return 0, apparently nobody cares about this
+			 * case anyway. If num_users() returns more than 1,
+			 * we are not the only user of the plane's memory.
+			 */
+			if (call_memop(q, plane, num_users,
+					vb->planes[plane].mem_priv) > 1)
+				return true;
+		}
+	}
+
+	return false;
+}
+
+/**
+ * vb2_reqbufs() - Initiate streaming
+ * @q:		videobuf2 queue
+ * @req:	struct passed from userspace to vidioc_reqbufs handler in driver
+ *
+ * Should be called from vidioc_reqbufs ioctl handler of a driver.
+ * This function:
+ * 1) verifies streaming parameters passed from the userspace,
+ * 2) sets up the queue,
+ * 3) negotiates number of buffers and planes per buffer with the driver
+ *    to be used during streaming,
+ * 4) allocates internal buffer structures (struct vb2_buffer), according to
+ *    the agreed parameters,
+ * 5) for MMAP memory type, allocates actual video memory, using the
+ *    memory handling/allocation routines provided during queue initialization
+ *
+ * If req->count is 0, all the memory will be freed instead.
+ * If the queue has been allocated previously (by a previous vb2_reqbufs) call
+ * and the queue is not busy, memory will be reallocated.
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_reqbufs handler in driver.
+ */
+int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
+{
+	unsigned int num_buffers, num_planes;
+	unsigned long plane_sizes[VIDEO_MAX_PLANES];
+	int ret = 0;
+
+	if (q->fileio) {
+		dprintk(1, "reqbufs: file io in progress\n");
+		return -EBUSY;
+	}
+
+	if (req->memory != V4L2_MEMORY_MMAP
+			&& req->memory != V4L2_MEMORY_USERPTR) {
+		dprintk(1, "reqbufs: unsupported memory type\n");
+		return -EINVAL;
+	}
+
+	if (req->type != q->type) {
+		dprintk(1, "reqbufs: requested type is incorrect\n");
+		return -EINVAL;
+	}
+
+	if (q->streaming) {
+		dprintk(1, "reqbufs: streaming active\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Make sure all the required memory ops for given memory type
+	 * are available.
+	 */
+	if (req->memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
+		dprintk(1, "reqbufs: MMAP for current setup unsupported\n");
+		return -EINVAL;
+	}
+
+	if (req->memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
+		dprintk(1, "reqbufs: USERPTR for current setup unsupported\n");
+		return -EINVAL;
+	}
+
+	if (req->count == 0 || q->num_buffers != 0) {
+		/*
+		 * We already have buffers allocated, so first check if they
+		 * are not in use and can be freed.
+		 */
+		if (q->memory == V4L2_MEMORY_MMAP && __buffers_in_use(q)) {
+			dprintk(1, "reqbufs: memory in use, cannot free\n");
+			return -EBUSY;
+		}
+
+		ret = __vb2_queue_free(q);
+		if (ret != 0)
+			return ret;
+	}
+
+	/*
+	 * Make sure the requested values and current defaults are sane.
+	 */
+	num_buffers = min_t(unsigned int, req->count, VIDEO_MAX_FRAME);
+	memset(plane_sizes, 0, sizeof(plane_sizes));
+	memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
+
+	/*
+	 * 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, &num_buffers, &num_planes,
+		       plane_sizes, q->alloc_ctx);
+	if (ret)
+		return ret;
+
+	/* Finally, allocate buffers and video memory */
+	ret = __vb2_queue_alloc(q, req->memory, num_buffers, num_planes,
+				plane_sizes);
+	if (ret < 0) {
+		dprintk(1, "Memory allocation failed with error: %d\n", ret);
+		return ret;
+	}
+
+	/*
+	 * Check if driver can handle the allocated number of buffers.
+	 */
+	if (ret < num_buffers) {
+		unsigned int orig_num_buffers;
+
+		orig_num_buffers = num_buffers = ret;
+		ret = call_qop(q, queue_setup, q, &num_buffers, &num_planes,
+			       plane_sizes, q->alloc_ctx);
+		if (ret)
+			goto free_mem;
+
+		if (orig_num_buffers < num_buffers) {
+			ret = -ENOMEM;
+			goto free_mem;
+		}
+
+		/*
+		 * Ok, driver accepted smaller number of buffers.
+		 */
+		ret = num_buffers;
+	}
+
+	q->memory = req->memory;
+
+	/*
+	 * Return the number of successfully allocated buffers
+	 * to the userspace.
+	 */
+	req->count = ret;
+
+	return 0;
+
+free_mem:
+	__vb2_queue_free(q);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vb2_reqbufs);
+
+/**
+ * vb2_plane_vaddr() - Return a kernel virtual address of a given plane
+ * @vb:		vb2_buffer to which the plane in question belongs to
+ * @plane_no:	plane number for which the address is to be returned
+ *
+ * This function returns a kernel virtual address of a given plane if
+ * such a mapping exist, NULL otherwise.
+ */
+void *vb2_plane_vaddr(struct vb2_buffer *vb, unsigned int plane_no)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+
+	if (plane_no > vb->num_planes)
+		return NULL;
+
+	return call_memop(q, plane_no, vaddr, vb->planes[plane_no].mem_priv);
+
+}
+EXPORT_SYMBOL_GPL(vb2_plane_vaddr);
+
+/**
+ * vb2_plane_cookie() - Return allocator specific cookie for the given plane
+ * @vb:		vb2_buffer to which the plane in question belongs to
+ * @plane_no:	plane number for which the cookie is to be returned
+ *
+ * This function returns an allocator specific cookie for a given plane if
+ * available, NULL otherwise. The allocator should provide some simple static
+ * inline function, which would convert this cookie to the allocator specific
+ * type that can be used directly by the driver to access the buffer. This can
+ * be for example physical address, pointer to scatter list or IOMMU mapping.
+ */
+void *vb2_plane_cookie(struct vb2_buffer *vb, unsigned int plane_no)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+
+	if (plane_no > vb->num_planes)
+		return NULL;
+
+	return call_memop(q, plane_no, cookie, vb->planes[plane_no].mem_priv);
+}
+EXPORT_SYMBOL_GPL(vb2_plane_cookie);
+
+/**
+ * vb2_buffer_done() - inform videobuf that an operation on a buffer is finished
+ * @vb:		vb2_buffer returned from the driver
+ * @state:	either VB2_BUF_STATE_DONE if the operation finished successfully
+ *		or VB2_BUF_STATE_ERROR if the operation finished with an error
+ *
+ * This function should be called by the driver after a hardware operation on
+ * a buffer is finished and the buffer may be returned to userspace. The driver
+ * cannot use this buffer anymore until it is queued back to it by videobuf
+ * by the means of buf_queue callback. Only buffers previously queued to the
+ * driver by buf_queue can be passed to this function.
+ */
+void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	unsigned long flags;
+
+	if (vb->state != VB2_BUF_STATE_ACTIVE)
+		return;
+
+	if (state != VB2_BUF_STATE_DONE && state != VB2_BUF_STATE_ERROR)
+		return;
+
+	dprintk(4, "Done processing on buffer %d, state: %d\n",
+			vb->v4l2_buf.index, vb->state);
+
+	/* Add the buffer to the done buffers list */
+	spin_lock_irqsave(&q->done_lock, flags);
+	vb->state = state;
+	list_add_tail(&vb->done_entry, &q->done_list);
+	atomic_dec(&q->queued_count);
+	spin_unlock_irqrestore(&q->done_lock, flags);
+
+	/* Inform any processes that may be waiting for buffers */
+	wake_up(&q->done_wq);
+}
+EXPORT_SYMBOL_GPL(vb2_buffer_done);
+
+/**
+ * __fill_vb2_buffer() - fill a vb2_buffer with information provided in
+ * a v4l2_buffer by the userspace
+ */
+static int __fill_vb2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b,
+				struct v4l2_plane *v4l2_planes)
+{
+	unsigned int plane;
+	int ret;
+
+	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
+		/*
+		 * Verify that the userspace gave us a valid array for
+		 * plane information.
+		 */
+		ret = __verify_planes_array(vb, b);
+		if (ret)
+			return ret;
+
+		/* Fill in driver-provided information for OUTPUT types */
+		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
+			/*
+			 * Will have to go up to b->length when API starts
+			 * accepting variable number of planes.
+			 */
+			for (plane = 0; plane < vb->num_planes; ++plane) {
+				v4l2_planes[plane].bytesused =
+					b->m.planes[plane].bytesused;
+				v4l2_planes[plane].data_offset =
+					b->m.planes[plane].data_offset;
+			}
+		}
+
+		if (b->memory == V4L2_MEMORY_USERPTR) {
+			for (plane = 0; plane < vb->num_planes; ++plane) {
+				v4l2_planes[plane].m.userptr =
+					b->m.planes[plane].m.userptr;
+				v4l2_planes[plane].length =
+					b->m.planes[plane].length;
+			}
+		}
+	} else {
+		/*
+		 * Single-planar buffers do not use planes array,
+		 * so fill in relevant v4l2_buffer struct fields instead.
+		 * In videobuf we use our internal V4l2_planes struct for
+		 * single-planar buffers as well, for simplicity.
+		 */
+		if (V4L2_TYPE_IS_OUTPUT(b->type))
+			v4l2_planes[0].bytesused = b->bytesused;
+
+		if (b->memory == V4L2_MEMORY_USERPTR) {
+			v4l2_planes[0].m.userptr = b->m.userptr;
+			v4l2_planes[0].length = b->length;
+		}
+	}
+
+	vb->v4l2_buf.field = b->field;
+	vb->v4l2_buf.timestamp = b->timestamp;
+
+	return 0;
+}
+
+/**
+ * __qbuf_userptr() - handle qbuf of a USERPTR buffer
+ */
+static int __qbuf_userptr(struct vb2_buffer *vb, struct v4l2_buffer *b)
+{
+	struct v4l2_plane planes[VIDEO_MAX_PLANES];
+	struct vb2_queue *q = vb->vb2_queue;
+	void *mem_priv;
+	unsigned int plane;
+	int ret;
+	int write = !V4L2_TYPE_IS_OUTPUT(q->type);
+
+	/* Verify and copy relevant information provided by the userspace */
+	ret = __fill_vb2_buffer(vb, b, planes);
+	if (ret)
+		return ret;
+
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		/* Skip the plane if already verified */
+		if (vb->v4l2_planes[plane].m.userptr == planes[plane].m.userptr
+		    && vb->v4l2_planes[plane].length == planes[plane].length)
+			continue;
+
+		dprintk(3, "qbuf: userspace address for plane %d changed, "
+				"reacquiring memory\n", plane);
+
+		/* Release previously acquired memory if present */
+		if (vb->planes[plane].mem_priv)
+			call_memop(q, plane, put_userptr,
+					vb->planes[plane].mem_priv);
+
+		vb->planes[plane].mem_priv = NULL;
+
+		/* Acquire each plane's memory */
+		if (q->mem_ops->get_userptr) {
+			mem_priv = q->mem_ops->get_userptr(q->alloc_ctx[plane],
+							planes[plane].m.userptr,
+							planes[plane].length,
+							write);
+			if (IS_ERR(mem_priv)) {
+				dprintk(1, "qbuf: failed acquiring userspace "
+						"memory for plane %d\n", plane);
+				ret = PTR_ERR(mem_priv);
+				goto err;
+			}
+			vb->planes[plane].mem_priv = mem_priv;
+		}
+	}
+
+	/*
+	 * Call driver-specific initialization on the newly acquired buffer,
+	 * if provided.
+	 */
+	ret = call_qop(q, buf_init, vb);
+	if (ret) {
+		dprintk(1, "qbuf: buffer initialization failed\n");
+		goto err;
+	}
+
+	/*
+	 * Now that everything is in order, copy relevant information
+	 * provided by userspace.
+	 */
+	for (plane = 0; plane < vb->num_planes; ++plane)
+		vb->v4l2_planes[plane] = planes[plane];
+
+	return 0;
+err:
+	/* In case of errors, release planes that were already acquired */
+	for (; plane > 0; --plane) {
+		call_memop(q, plane, put_userptr,
+				vb->planes[plane - 1].mem_priv);
+		vb->planes[plane - 1].mem_priv = NULL;
+	}
+
+	return ret;
+}
+
+/**
+ * __qbuf_mmap() - handle qbuf of an MMAP buffer
+ */
+static int __qbuf_mmap(struct vb2_buffer *vb, struct v4l2_buffer *b)
+{
+	return __fill_vb2_buffer(vb, b, vb->v4l2_planes);
+}
+
+/**
+ * __enqueue_in_driver() - enqueue a vb2_buffer in driver for processing
+ */
+static void __enqueue_in_driver(struct vb2_buffer *vb)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+
+	vb->state = VB2_BUF_STATE_ACTIVE;
+	atomic_inc(&q->queued_count);
+	q->ops->buf_queue(vb);
+}
+
+/**
+ * vb2_qbuf() - Queue a buffer from userspace
+ * @q:		videobuf2 queue
+ * @b:		buffer structure passed from userspace to vidioc_qbuf handler
+ *		in driver
+ *
+ * Should be called from vidioc_qbuf ioctl handler of a driver.
+ * This function:
+ * 1) verifies the passed buffer,
+ * 2) calls buf_prepare callback in the driver (if provided), in which
+ *    driver-specific buffer initialization can be performed,
+ * 3) if streaming is on, queues the buffer in driver by the means of buf_queue
+ *    callback for processing.
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_qbuf handler in driver.
+ */
+int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
+{
+	struct vb2_buffer *vb;
+	int ret = 0;
+
+	if (q->fileio) {
+		dprintk(1, "qbuf: file io in progress\n");
+		return -EBUSY;
+	}
+
+	if (b->type != q->type) {
+		dprintk(1, "qbuf: invalid buffer type\n");
+		return -EINVAL;
+	}
+
+	if (b->index >= q->num_buffers) {
+		dprintk(1, "qbuf: buffer index out of range\n");
+		return -EINVAL;
+	}
+
+	vb = q->bufs[b->index];
+	if (NULL == vb) {
+		/* Should never happen */
+		dprintk(1, "qbuf: buffer is NULL\n");
+		return -EINVAL;
+	}
+
+	if (b->memory != q->memory) {
+		dprintk(1, "qbuf: invalid memory type\n");
+		return -EINVAL;
+	}
+
+	if (vb->state != VB2_BUF_STATE_DEQUEUED) {
+		dprintk(1, "qbuf: buffer already in use\n");
+		return -EINVAL;
+	}
+
+	if (q->memory == V4L2_MEMORY_MMAP)
+		ret = __qbuf_mmap(vb, b);
+	else if (q->memory == V4L2_MEMORY_USERPTR)
+		ret = __qbuf_userptr(vb, b);
+	else {
+		WARN(1, "Invalid queue type\n");
+		return -EINVAL;
+	}
+
+	if (ret)
+		return ret;
+
+	ret = call_qop(q, buf_prepare, vb);
+	if (ret) {
+		dprintk(1, "qbuf: buffer preparation failed\n");
+		return ret;
+	}
+
+	/*
+	 * Add to the queued buffers list, a buffer will stay on it until
+	 * dequeued in dqbuf.
+	 */
+	list_add_tail(&vb->queued_entry, &q->queued_list);
+	vb->state = VB2_BUF_STATE_QUEUED;
+
+	/*
+	 * If already streaming, give the buffer to driver for processing.
+	 * If not, the buffer will be given to driver on next streamon.
+	 */
+	if (q->streaming)
+		__enqueue_in_driver(vb);
+
+	dprintk(1, "qbuf of buffer %d succeeded\n", vb->v4l2_buf.index);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_qbuf);
+
+/**
+ * __vb2_wait_for_done_vb() - wait for a buffer to become available
+ * for dequeuing
+ *
+ * Will sleep if required for nonblocking == false.
+ */
+static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
+{
+	/*
+	 * All operations on vb_done_list are performed under done_lock
+	 * spinlock protection. However, buffers may be removed from
+	 * it and returned to userspace only while holding both driver's
+	 * lock and the done_lock spinlock. Thus we can be sure that as
+	 * long as we hold the driver's lock, the list will remain not
+	 * empty if list_empty() check succeeds.
+	 */
+
+	for (;;) {
+		int ret;
+
+		if (!q->streaming) {
+			dprintk(1, "Streaming off, will not wait for buffers\n");
+			return -EINVAL;
+		}
+
+		if (!list_empty(&q->done_list)) {
+			/*
+			 * Found a buffer that we were waiting for.
+			 */
+			break;
+		}
+
+		if (nonblocking) {
+			dprintk(1, "Nonblocking and no buffers to dequeue, "
+								"will not wait\n");
+			return -EAGAIN;
+		}
+
+		/*
+		 * We are streaming and blocking, wait for another buffer to
+		 * become ready or for streamoff. Driver's lock is released to
+		 * allow streamoff or qbuf to be called while waiting.
+		 */
+		call_qop(q, wait_prepare, q);
+
+		/*
+		 * All locks have been released, it is safe to sleep now.
+		 */
+		dprintk(3, "Will sleep waiting for buffers\n");
+		ret = wait_event_interruptible(q->done_wq,
+				!list_empty(&q->done_list) || !q->streaming);
+
+		/*
+		 * We need to reevaluate both conditions again after reacquiring
+		 * the locks or return an error if one occurred.
+		 */
+		call_qop(q, wait_finish, q);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+/**
+ * __vb2_get_done_vb() - get a buffer ready for dequeuing
+ *
+ * Will sleep if required for nonblocking == false.
+ */
+static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
+				int nonblocking)
+{
+	unsigned long flags;
+	int ret;
+
+	/*
+	 * Wait for at least one buffer to become available on the done_list.
+	 */
+	ret = __vb2_wait_for_done_vb(q, nonblocking);
+	if (ret)
+		return ret;
+
+	/*
+	 * Driver's lock has been held since we last verified that done_list
+	 * is not empty, so no need for another list_empty(done_list) check.
+	 */
+	spin_lock_irqsave(&q->done_lock, flags);
+	*vb = list_first_entry(&q->done_list, struct vb2_buffer, done_entry);
+	list_del(&(*vb)->done_entry);
+	spin_unlock_irqrestore(&q->done_lock, flags);
+
+	return 0;
+}
+
+/**
+ * vb2_wait_for_all_buffers() - wait until all buffers are given back to vb2
+ * @q:		videobuf2 queue
+ *
+ * This function will wait until all buffers that have been given to the driver
+ * by buf_queue() are given back to vb2 with vb2_buffer_done(). It doesn't call
+ * wait_prepare, wait_finish pair. It is intended to be called with all locks
+ * taken, for example from stop_streaming() callback.
+ */
+int vb2_wait_for_all_buffers(struct vb2_queue *q)
+{
+	if (!q->streaming) {
+		dprintk(1, "Streaming off, will not wait for buffers\n");
+		return -EINVAL;
+	}
+
+	wait_event(q->done_wq, !atomic_read(&q->queued_count));
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_wait_for_all_buffers);
+
+/**
+ * vb2_dqbuf() - Dequeue a buffer to the userspace
+ * @q:		videobuf2 queue
+ * @b:		buffer structure passed from userspace to vidioc_dqbuf handler
+ *		in driver
+ * @nonblocking: if true, this call will not sleep waiting for a buffer if no
+ *		 buffers ready for dequeuing are present. Normally the driver
+ *		 would be passing (file->f_flags & O_NONBLOCK) here
+ *
+ * Should be called from vidioc_dqbuf ioctl handler of a driver.
+ * This function:
+ * 1) verifies the passed buffer,
+ * 2) calls buf_finish callback in the driver (if provided), in which
+ *    driver can perform any additional operations that may be required before
+ *    returning the buffer to userspace, such as cache sync,
+ * 3) the buffer struct members are filled with relevant information for
+ *    the userspace.
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_dqbuf handler in driver.
+ */
+int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
+{
+	struct vb2_buffer *vb = NULL;
+	int ret;
+
+	if (q->fileio) {
+		dprintk(1, "dqbuf: file io in progress\n");
+		return -EBUSY;
+	}
+
+	if (b->type != q->type) {
+		dprintk(1, "dqbuf: invalid buffer type\n");
+		return -EINVAL;
+	}
+
+	ret = __vb2_get_done_vb(q, &vb, nonblocking);
+	if (ret < 0) {
+		dprintk(1, "dqbuf: error getting next done buffer\n");
+		return ret;
+	}
+
+	ret = call_qop(q, buf_finish, vb);
+	if (ret) {
+		dprintk(1, "dqbuf: buffer finish failed\n");
+		return ret;
+	}
+
+	switch (vb->state) {
+	case VB2_BUF_STATE_DONE:
+		dprintk(3, "dqbuf: Returning done buffer\n");
+		break;
+	case VB2_BUF_STATE_ERROR:
+		dprintk(3, "dqbuf: Returning done buffer with errors\n");
+		break;
+	default:
+		dprintk(1, "dqbuf: Invalid buffer state\n");
+		return -EINVAL;
+	}
+
+	/* Fill buffer information for the userspace */
+	__fill_v4l2_buffer(vb, b);
+	/* Remove from videobuf queue */
+	list_del(&vb->queued_entry);
+
+	dprintk(1, "dqbuf of buffer %d, with state %d\n",
+			vb->v4l2_buf.index, vb->state);
+
+	vb->state = VB2_BUF_STATE_DEQUEUED;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_dqbuf);
+
+/**
+ * vb2_streamon - start streaming
+ * @q:		videobuf2 queue
+ * @type:	type argument passed from userspace to vidioc_streamon handler
+ *
+ * Should be called from vidioc_streamon handler of a driver.
+ * This function:
+ * 1) verifies current state
+ * 2) starts streaming and passes any previously queued buffers to the driver
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_streamon handler in the driver.
+ */
+int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
+{
+	struct vb2_buffer *vb;
+
+	if (q->fileio) {
+		dprintk(1, "streamon: file io in progress\n");
+		return -EBUSY;
+	}
+
+	if (type != q->type) {
+		dprintk(1, "streamon: invalid stream type\n");
+		return -EINVAL;
+	}
+
+	if (q->streaming) {
+		dprintk(1, "streamon: already streaming\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Cannot start streaming on an OUTPUT device if no buffers have
+	 * been queued yet.
+	 */
+	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		if (list_empty(&q->queued_list)) {
+			dprintk(1, "streamon: no output buffers queued\n");
+			return -EINVAL;
+		}
+	}
+
+	q->streaming = 1;
+
+	/*
+	 * Let driver notice that streaming state has been enabled.
+	 */
+	call_qop(q, start_streaming, q);
+
+	/*
+	 * If any buffers were queued before streamon,
+	 * we can now pass them to driver for processing.
+	 */
+	list_for_each_entry(vb, &q->queued_list, queued_entry)
+		__enqueue_in_driver(vb);
+
+	dprintk(3, "Streamon successful\n");
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_streamon);
+
+/**
+ * __vb2_queue_cancel() - cancel and stop (pause) streaming
+ *
+ * Removes all queued buffers from driver's queue and all buffers queued by
+ * userspace from videobuf's queue. Returns to state after reqbufs.
+ */
+static void __vb2_queue_cancel(struct vb2_queue *q)
+{
+	unsigned int i;
+
+	/*
+	 * Tell driver to stop all transactions and release all queued
+	 * buffers.
+	 */
+	if (q->streaming)
+		call_qop(q, stop_streaming, q);
+	q->streaming = 0;
+
+	/*
+	 * Remove all buffers from videobuf's list...
+	 */
+	INIT_LIST_HEAD(&q->queued_list);
+	/*
+	 * ...and done list; userspace will not receive any buffers it
+	 * has not already dequeued before initiating cancel.
+	 */
+	INIT_LIST_HEAD(&q->done_list);
+	wake_up_all(&q->done_wq);
+
+	/*
+	 * Reinitialize all buffers for next use.
+	 */
+	for (i = 0; i < q->num_buffers; ++i)
+		q->bufs[i]->state = VB2_BUF_STATE_DEQUEUED;
+}
+
+/**
+ * vb2_streamoff - stop streaming
+ * @q:		videobuf2 queue
+ * @type:	type argument passed from userspace to vidioc_streamoff handler
+ *
+ * Should be called from vidioc_streamoff handler of a driver.
+ * This function:
+ * 1) verifies current state,
+ * 2) stop streaming and dequeues any queued buffers, including those previously
+ *    passed to the driver (after waiting for the driver to finish).
+ *
+ * This call can be used for pausing playback.
+ * The return values from this function are intended to be directly returned
+ * from vidioc_streamoff handler in the driver
+ */
+int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
+{
+	if (q->fileio) {
+		dprintk(1, "streamoff: file io in progress\n");
+		return -EBUSY;
+	}
+
+	if (type != q->type) {
+		dprintk(1, "streamoff: invalid stream type\n");
+		return -EINVAL;
+	}
+
+	if (!q->streaming) {
+		dprintk(1, "streamoff: not streaming\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Cancel will pause streaming and remove all buffers from the driver
+	 * and videobuf, effectively returning control over them to userspace.
+	 */
+	__vb2_queue_cancel(q);
+
+	dprintk(3, "Streamoff successful\n");
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_streamoff);
+
+/**
+ * __find_plane_by_offset() - find plane associated with the given offset off
+ */
+static int __find_plane_by_offset(struct vb2_queue *q, unsigned long off,
+			unsigned int *_buffer, unsigned int *_plane)
+{
+	struct vb2_buffer *vb;
+	unsigned int buffer, plane;
+
+	/*
+	 * Go over all buffers and their planes, comparing the given offset
+	 * with an offset assigned to each plane. If a match is found,
+	 * return its buffer and plane numbers.
+	 */
+	for (buffer = 0; buffer < q->num_buffers; ++buffer) {
+		vb = q->bufs[buffer];
+
+		for (plane = 0; plane < vb->num_planes; ++plane) {
+			if (vb->v4l2_planes[plane].m.mem_offset == off) {
+				*_buffer = buffer;
+				*_plane = plane;
+				return 0;
+			}
+		}
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * vb2_mmap() - map video buffers into application address space
+ * @q:		videobuf2 queue
+ * @vma:	vma passed to the mmap file operation handler in the driver
+ *
+ * Should be called from mmap file operation handler of a driver.
+ * This function maps one plane of one of the available video buffers to
+ * userspace. To map whole video memory allocated on reqbufs, this function
+ * has to be called once per each plane per each buffer previously allocated.
+ *
+ * When the userspace application calls mmap, it passes to it an offset returned
+ * to it earlier by the means of vidioc_querybuf handler. That offset acts as
+ * a "cookie", which is then used to identify the plane to be mapped.
+ * This function finds a plane with a matching offset and a mapping is performed
+ * by the means of a provided memory operation.
+ *
+ * The return values from this function are intended to be directly returned
+ * from the mmap handler in driver.
+ */
+int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
+{
+	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
+	struct vb2_plane *vb_plane;
+	struct vb2_buffer *vb;
+	unsigned int buffer, plane;
+	int ret;
+
+	if (q->memory != V4L2_MEMORY_MMAP) {
+		dprintk(1, "Queue is not currently set up for mmap\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Check memory area access mode.
+	 */
+	if (!(vma->vm_flags & VM_SHARED)) {
+		dprintk(1, "Invalid vma flags, VM_SHARED needed\n");
+		return -EINVAL;
+	}
+	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		if (!(vma->vm_flags & VM_WRITE)) {
+			dprintk(1, "Invalid vma flags, VM_WRITE needed\n");
+			return -EINVAL;
+		}
+	} else {
+		if (!(vma->vm_flags & VM_READ)) {
+			dprintk(1, "Invalid vma flags, VM_READ needed\n");
+			return -EINVAL;
+		}
+	}
+
+	/*
+	 * Find the plane corresponding to the offset passed by userspace.
+	 */
+	ret = __find_plane_by_offset(q, off, &buffer, &plane);
+	if (ret)
+		return ret;
+
+	vb = q->bufs[buffer];
+	vb_plane = &vb->planes[plane];
+
+	ret = q->mem_ops->mmap(vb_plane->mem_priv, vma);
+	if (ret)
+		return ret;
+
+	vb_plane->mapped = 1;
+	vb->num_planes_mapped++;
+
+	dprintk(3, "Buffer %d, plane %d successfully mapped\n", buffer, plane);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_mmap);
+
+static int __vb2_init_fileio(struct vb2_queue *q, int read);
+static int __vb2_cleanup_fileio(struct vb2_queue *q);
+
+/**
+ * vb2_poll() - implements poll userspace operation
+ * @q:		videobuf2 queue
+ * @file:	file argument passed to the poll file operation handler
+ * @wait:	wait argument passed to the poll file operation handler
+ *
+ * This function implements poll file operation handler for a driver.
+ * For CAPTURE queues, if a buffer is ready to be dequeued, the userspace will
+ * be informed that the file descriptor of a video device is available for
+ * reading.
+ * For OUTPUT queues, if a buffer is ready to be dequeued, the file descriptor
+ * will be reported as available for writing.
+ *
+ * The return values from this function are intended to be directly returned
+ * from poll handler in driver.
+ */
+unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
+{
+	unsigned long flags;
+	unsigned int ret;
+	struct vb2_buffer *vb = NULL;
+
+	/*
+	 * Start file io emulator if streaming api has not been used yet.
+	 */
+	if (q->num_buffers == 0 && q->fileio == NULL) {
+		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ)) {
+			ret = __vb2_init_fileio(q, 1);
+			if (ret)
+				return ret;
+		}
+		if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE)) {
+			ret = __vb2_init_fileio(q, 0);
+			if (ret)
+				return ret;
+			/*
+			 * Write to OUTPUT queue can be done immediately.
+			 */
+			return POLLOUT | POLLWRNORM;
+		}
+	}
+
+	/*
+	 * There is nothing to wait for if no buffers have already been queued.
+	 */
+	if (list_empty(&q->queued_list))
+		return POLLERR;
+
+	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 (V4L2_TYPE_IS_OUTPUT(q->type)) ? POLLOUT | POLLWRNORM :
+			POLLIN | POLLRDNORM;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_poll);
+
+/**
+ * vb2_queue_init() - initialize a videobuf2 queue
+ * @q:		videobuf2 queue; this structure should be allocated in driver
+ *
+ * The vb2_queue structure should be allocated by the driver. The driver is
+ * responsible of clearing it's content and setting initial values for some
+ * required entries before calling this function.
+ * q->ops, q->mem_ops, q->type and q->io_modes are mandatory. Please refer
+ * to the struct vb2_queue description in include/media/videobuf2-core.h
+ * for more information.
+ */
+int vb2_queue_init(struct vb2_queue *q)
+{
+	BUG_ON(!q);
+	BUG_ON(!q->ops);
+	BUG_ON(!q->mem_ops);
+	BUG_ON(!q->type);
+	BUG_ON(!q->io_modes);
+
+	BUG_ON(!q->ops->queue_setup);
+	BUG_ON(!q->ops->buf_queue);
+
+	INIT_LIST_HEAD(&q->queued_list);
+	INIT_LIST_HEAD(&q->done_list);
+	spin_lock_init(&q->done_lock);
+	init_waitqueue_head(&q->done_wq);
+
+	if (q->buf_struct_size == 0)
+		q->buf_struct_size = sizeof(struct vb2_buffer);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_queue_init);
+
+/**
+ * vb2_queue_release() - stop streaming, release the queue and free memory
+ * @q:		videobuf2 queue
+ *
+ * This function stops streaming and performs necessary clean ups, including
+ * freeing video buffer memory. The driver is responsible for freeing
+ * the vb2_queue structure itself.
+ */
+void vb2_queue_release(struct vb2_queue *q)
+{
+	__vb2_cleanup_fileio(q);
+	__vb2_queue_cancel(q);
+	__vb2_queue_free(q);
+}
+EXPORT_SYMBOL_GPL(vb2_queue_release);
+
+/**
+ * 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
+ *
+ * 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_buffer b;
+	struct vb2_fileio_buf bufs[VIDEO_MAX_FRAME];
+	unsigned int index;
+	unsigned int q_count;
+	unsigned int dq_count;
+	unsigned int flags;
+};
+
+/**
+ * __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 ((read && !(q->io_modes & VB2_READ)) ||
+	   (!read && !(q->io_modes & VB2_WRITE)))
+		BUG();
+
+	/*
+	 * 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, flags %08x\n",
+		(read) ? "read" : "write", count, q->io_flags);
+
+	fileio = kzalloc(sizeof(struct vb2_fileio_data), GFP_KERNEL);
+	if (fileio == NULL)
+		return -ENOMEM;
+
+	fileio->flags = q->io_flags;
+
+	/*
+	 * Request buffers and use MMAP type to force driver
+	 * to allocate buffers by itself.
+	 */
+	fileio->req.count = count;
+	fileio->req.memory = V4L2_MEMORY_MMAP;
+	fileio->req.type = q->type;
+	ret = vb2_reqbufs(q, &fileio->req);
+	if (ret)
+		goto err_kfree;
+
+	/*
+	 * Check if plane_count is correct
+	 * (multiplane buffers are not supported).
+	 */
+	if (q->bufs[0]->num_planes != 1) {
+		fileio->req.count = 0;
+		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)
+			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++) {
+			struct v4l2_buffer *b = &fileio->b;
+			memset(b, 0, sizeof(*b));
+			b->type = q->type;
+			b->memory = q->memory;
+			b->index = i;
+			ret = vb2_qbuf(q, b);
+			if (ret)
+				goto err_reqbufs;
+			fileio->bufs[i].queued = 1;
+		}
+
+		/*
+		 * Start streaming.
+		 */
+		ret = vb2_streamon(q, q->type);
+		if (ret)
+			goto err_reqbufs;
+	}
+
+	q->fileio = fileio;
+
+	return ret;
+
+err_reqbufs:
+	vb2_reqbufs(q, &fileio->req);
+
+err_kfree:
+	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) {
+		/*
+		 * Hack fileio context to enable direct calls to vb2 ioctl
+		 * interface.
+		 */
+		q->fileio = NULL;
+
+		vb2_streamoff(q, q->type);
+		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;
+	int ret, index;
+
+	dprintk(3, "file io: 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 (!q->fileio) {
+		ret = __vb2_init_fileio(q, read);
+		dprintk(3, "file io: vb2_init_fileio result: %d\n", ret);
+		if (ret)
+			return ret;
+	}
+	fileio = q->fileio;
+
+	/*
+	 * Hack fileio context to enable direct calls to vb2 ioctl interface.
+	 * The pointer will be restored before returning from this function.
+	 */
+	q->fileio = NULL;
+
+	index = fileio->index;
+	buf = &fileio->bufs[index];
+
+	/*
+	 * Check if we need to dequeue the buffer.
+	 */
+	if (buf->queued) {
+		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;
+		fileio->b.index = index;
+		ret = vb2_dqbuf(q, &fileio->b, nonblock);
+		dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
+		if (ret)
+			goto end;
+		fileio->dq_count += 1;
+
+		/*
+		 * Get number of bytes filled by the driver
+		 */
+		vb = q->bufs[index];
+		buf->size = vb2_get_plane_payload(vb, 0);
+		buf->queued = 0;
+	}
+
+	/*
+	 * 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, "file io: 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, "file io: error copying data\n");
+		ret = -EFAULT;
+		goto end;
+	}
+
+	/*
+	 * Update counters.
+	 */
+	buf->pos += count;
+	*ppos += count;
+
+	/*
+	 * Queue next buffer if required.
+	 */
+	if (buf->pos == buf->size ||
+	   (!read && (fileio->flags & VB2_FILEIO_WRITE_IMMEDIATELY))) {
+		/*
+		 * Check if this is the last buffer to read.
+		 */
+		if (read && (fileio->flags & VB2_FILEIO_READ_ONCE) &&
+		    fileio->dq_count == 1) {
+			dprintk(3, "file io: read limit reached\n");
+			/*
+			 * Restore fileio pointer and release the context.
+			 */
+			q->fileio = fileio;
+			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;
+		ret = vb2_qbuf(q, &fileio->b);
+		dprintk(5, "file io: vb2_dbuf result: %d\n", ret);
+		if (ret)
+			goto end;
+
+		/*
+		 * Buffer has been queued, update the status
+		 */
+		buf->pos = 0;
+		buf->queued = 1;
+		buf->size = q->bufs[0]->v4l2_planes[0].length;
+		fileio->q_count += 1;
+
+		/*
+		 * Switch to the next buffer
+		 */
+		fileio->index = (index + 1) % q->num_buffers;
+
+		/*
+		 * Start streaming if required.
+		 */
+		if (!read && !q->streaming) {
+			ret = vb2_streamon(q, q->type);
+			if (ret)
+				goto end;
+		}
+	}
+
+	/*
+	 * Return proper number of bytes processed.
+	 */
+	if (ret == 0)
+		ret = count;
+end:
+	/*
+	 * Restore the fileio context and block vb2 ioctl interface.
+	 */
+	q->fileio = fileio;
+	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, char __user *data, size_t count,
+		loff_t *ppos, int nonblocking)
+{
+	return __vb2_perform_fileio(q, data, count, ppos, nonblocking, 0);
+}
+EXPORT_SYMBOL_GPL(vb2_write);
+
+MODULE_DESCRIPTION("Driver helper framework for Video for Linux 2");
+MODULE_AUTHOR("Pawel Osciak, Marek Szyprowski");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/videobuf2-dma-contig.c b/drivers/media/video/videobuf2-dma-contig.c
new file mode 100644
index 000000000000..bb6a5602cf94
--- /dev/null
+++ b/drivers/media/video/videobuf2-dma-contig.c
@@ -0,0 +1,185 @@
+/*
+ * videobuf2-dma-contig.c - DMA contig memory allocator for videobuf2
+ *
+ * Copyright (C) 2010 Samsung Electronics
+ *
+ * Author: Pawel Osciak <p.osciak@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-memops.h>
+
+struct vb2_dc_conf {
+	struct device		*dev;
+};
+
+struct vb2_dc_buf {
+	struct vb2_dc_conf		*conf;
+	void				*vaddr;
+	dma_addr_t			paddr;
+	unsigned long			size;
+	struct vm_area_struct		*vma;
+	atomic_t			refcount;
+	struct vb2_vmarea_handler	handler;
+};
+
+static void vb2_dma_contig_put(void *buf_priv);
+
+static void *vb2_dma_contig_alloc(void *alloc_ctx, unsigned long size)
+{
+	struct vb2_dc_conf *conf = alloc_ctx;
+	struct vb2_dc_buf *buf;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	buf->vaddr = dma_alloc_coherent(conf->dev, size, &buf->paddr,
+					GFP_KERNEL);
+	if (!buf->vaddr) {
+		dev_err(conf->dev, "dma_alloc_coherent of size %ld failed\n",
+			buf->size);
+		kfree(buf);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	buf->conf = conf;
+	buf->size = size;
+
+	buf->handler.refcount = &buf->refcount;
+	buf->handler.put = vb2_dma_contig_put;
+	buf->handler.arg = buf;
+
+	atomic_inc(&buf->refcount);
+
+	return buf;
+}
+
+static void vb2_dma_contig_put(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	if (atomic_dec_and_test(&buf->refcount)) {
+		dma_free_coherent(buf->conf->dev, buf->size, buf->vaddr,
+				  buf->paddr);
+		kfree(buf);
+	}
+}
+
+static void *vb2_dma_contig_cookie(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	return (void *)buf->paddr;
+}
+
+static void *vb2_dma_contig_vaddr(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+	if (!buf)
+		return 0;
+
+	return buf->vaddr;
+}
+
+static unsigned int vb2_dma_contig_num_users(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	return atomic_read(&buf->refcount);
+}
+
+static int vb2_dma_contig_mmap(void *buf_priv, struct vm_area_struct *vma)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	if (!buf) {
+		printk(KERN_ERR "No buffer to map\n");
+		return -EINVAL;
+	}
+
+	return vb2_mmap_pfn_range(vma, buf->paddr, buf->size,
+				  &vb2_common_vm_ops, &buf->handler);
+}
+
+static void *vb2_dma_contig_get_userptr(void *alloc_ctx, unsigned long vaddr,
+					unsigned long size, int write)
+{
+	struct vb2_dc_buf *buf;
+	struct vm_area_struct *vma;
+	dma_addr_t paddr = 0;
+	int ret;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	ret = vb2_get_contig_userptr(vaddr, size, &vma, &paddr);
+	if (ret) {
+		printk(KERN_ERR "Failed acquiring VMA for vaddr 0x%08lx\n",
+				vaddr);
+		kfree(buf);
+		return ERR_PTR(ret);
+	}
+
+	buf->size = size;
+	buf->paddr = paddr;
+	buf->vma = vma;
+
+	return buf;
+}
+
+static void vb2_dma_contig_put_userptr(void *mem_priv)
+{
+	struct vb2_dc_buf *buf = mem_priv;
+
+	if (!buf)
+		return;
+
+	vb2_put_vma(buf->vma);
+	kfree(buf);
+}
+
+const struct vb2_mem_ops vb2_dma_contig_memops = {
+	.alloc		= vb2_dma_contig_alloc,
+	.put		= vb2_dma_contig_put,
+	.cookie		= vb2_dma_contig_cookie,
+	.vaddr		= vb2_dma_contig_vaddr,
+	.mmap		= vb2_dma_contig_mmap,
+	.get_userptr	= vb2_dma_contig_get_userptr,
+	.put_userptr	= vb2_dma_contig_put_userptr,
+	.num_users	= vb2_dma_contig_num_users,
+};
+EXPORT_SYMBOL_GPL(vb2_dma_contig_memops);
+
+void *vb2_dma_contig_init_ctx(struct device *dev)
+{
+	struct vb2_dc_conf *conf;
+
+	conf = kzalloc(sizeof *conf, GFP_KERNEL);
+	if (!conf)
+		return ERR_PTR(-ENOMEM);
+
+	conf->dev = dev;
+
+	return conf;
+}
+EXPORT_SYMBOL_GPL(vb2_dma_contig_init_ctx);
+
+void vb2_dma_contig_cleanup_ctx(void *alloc_ctx)
+{
+	kfree(alloc_ctx);
+}
+EXPORT_SYMBOL_GPL(vb2_dma_contig_cleanup_ctx);
+
+MODULE_DESCRIPTION("DMA-contig memory handling routines for videobuf2");
+MODULE_AUTHOR("Pawel Osciak");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/videobuf2-dma-sg.c b/drivers/media/video/videobuf2-dma-sg.c
new file mode 100644
index 000000000000..20b5c5dcc0ef
--- /dev/null
+++ b/drivers/media/video/videobuf2-dma-sg.c
@@ -0,0 +1,292 @@
+/*
+ * videobuf2-dma-sg.c - dma scatter/gather memory allocator for videobuf2
+ *
+ * Copyright (C) 2010 Samsung Electronics
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/scatterlist.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-memops.h>
+#include <media/videobuf2-dma-sg.h>
+
+struct vb2_dma_sg_buf {
+	void				*vaddr;
+	struct page			**pages;
+	int				write;
+	int				offset;
+	struct vb2_dma_sg_desc		sg_desc;
+	atomic_t			refcount;
+	struct vb2_vmarea_handler	handler;
+};
+
+static void vb2_dma_sg_put(void *buf_priv);
+
+static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
+{
+	struct vb2_dma_sg_buf *buf;
+	int i;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return NULL;
+
+	buf->vaddr = NULL;
+	buf->write = 0;
+	buf->offset = 0;
+	buf->sg_desc.size = size;
+	buf->sg_desc.num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+
+	buf->sg_desc.sglist = vmalloc(buf->sg_desc.num_pages *
+				      sizeof(*buf->sg_desc.sglist));
+	if (!buf->sg_desc.sglist)
+		goto fail_sglist_alloc;
+	memset(buf->sg_desc.sglist, 0, buf->sg_desc.num_pages *
+	       sizeof(*buf->sg_desc.sglist));
+	sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
+
+	buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
+			     GFP_KERNEL);
+	if (!buf->pages)
+		goto fail_pages_array_alloc;
+
+	for (i = 0; i < buf->sg_desc.num_pages; ++i) {
+		buf->pages[i] = alloc_page(GFP_KERNEL);
+		if (NULL == buf->pages[i])
+			goto fail_pages_alloc;
+		sg_set_page(&buf->sg_desc.sglist[i],
+			    buf->pages[i], PAGE_SIZE, 0);
+	}
+
+	buf->handler.refcount = &buf->refcount;
+	buf->handler.put = vb2_dma_sg_put;
+	buf->handler.arg = buf;
+
+	atomic_inc(&buf->refcount);
+
+	printk(KERN_DEBUG "%s: Allocated buffer of %d pages\n",
+		__func__, buf->sg_desc.num_pages);
+
+	if (!buf->vaddr)
+		buf->vaddr = vm_map_ram(buf->pages,
+					buf->sg_desc.num_pages,
+					-1,
+					PAGE_KERNEL);
+	return buf;
+
+fail_pages_alloc:
+	while (--i >= 0)
+		__free_page(buf->pages[i]);
+
+fail_pages_array_alloc:
+	vfree(buf->sg_desc.sglist);
+
+fail_sglist_alloc:
+	kfree(buf);
+	return NULL;
+}
+
+static void vb2_dma_sg_put(void *buf_priv)
+{
+	struct vb2_dma_sg_buf *buf = buf_priv;
+	int i = buf->sg_desc.num_pages;
+
+	if (atomic_dec_and_test(&buf->refcount)) {
+		printk(KERN_DEBUG "%s: Freeing buffer of %d pages\n", __func__,
+			buf->sg_desc.num_pages);
+		if (buf->vaddr)
+			vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
+		vfree(buf->sg_desc.sglist);
+		while (--i >= 0)
+			__free_page(buf->pages[i]);
+		kfree(buf->pages);
+		kfree(buf);
+	}
+}
+
+static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
+				    unsigned long size, int write)
+{
+	struct vb2_dma_sg_buf *buf;
+	unsigned long first, last;
+	int num_pages_from_user, i;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return NULL;
+
+	buf->vaddr = NULL;
+	buf->write = write;
+	buf->offset = vaddr & ~PAGE_MASK;
+	buf->sg_desc.size = size;
+
+	first = (vaddr           & PAGE_MASK) >> PAGE_SHIFT;
+	last  = ((vaddr + size - 1) & PAGE_MASK) >> PAGE_SHIFT;
+	buf->sg_desc.num_pages = last - first + 1;
+
+	buf->sg_desc.sglist = vmalloc(
+		buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
+	if (!buf->sg_desc.sglist)
+		goto userptr_fail_sglist_alloc;
+
+	memset(buf->sg_desc.sglist, 0,
+		buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
+	sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
+
+	buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
+			     GFP_KERNEL);
+	if (!buf->pages)
+		goto userptr_fail_pages_array_alloc;
+
+	down_read(&current->mm->mmap_sem);
+	num_pages_from_user = get_user_pages(current, current->mm,
+					     vaddr & PAGE_MASK,
+					     buf->sg_desc.num_pages,
+					     write,
+					     1, /* force */
+					     buf->pages,
+					     NULL);
+	up_read(&current->mm->mmap_sem);
+	if (num_pages_from_user != buf->sg_desc.num_pages)
+		goto userptr_fail_get_user_pages;
+
+	sg_set_page(&buf->sg_desc.sglist[0], buf->pages[0],
+		    PAGE_SIZE - buf->offset, buf->offset);
+	size -= PAGE_SIZE - buf->offset;
+	for (i = 1; i < buf->sg_desc.num_pages; ++i) {
+		sg_set_page(&buf->sg_desc.sglist[i], buf->pages[i],
+			    min_t(size_t, PAGE_SIZE, size), 0);
+		size -= min_t(size_t, PAGE_SIZE, size);
+	}
+	return buf;
+
+userptr_fail_get_user_pages:
+	printk(KERN_DEBUG "get_user_pages requested/got: %d/%d]\n",
+	       num_pages_from_user, buf->sg_desc.num_pages);
+	while (--num_pages_from_user >= 0)
+		put_page(buf->pages[num_pages_from_user]);
+
+userptr_fail_pages_array_alloc:
+	vfree(buf->sg_desc.sglist);
+
+userptr_fail_sglist_alloc:
+	kfree(buf);
+	return NULL;
+}
+
+/*
+ * @put_userptr: inform the allocator that a USERPTR buffer will no longer
+ *		 be used
+ */
+static void vb2_dma_sg_put_userptr(void *buf_priv)
+{
+	struct vb2_dma_sg_buf *buf = buf_priv;
+	int i = buf->sg_desc.num_pages;
+
+	printk(KERN_DEBUG "%s: Releasing userspace buffer of %d pages\n",
+	       __func__, buf->sg_desc.num_pages);
+	if (buf->vaddr)
+		vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
+	while (--i >= 0) {
+		if (buf->write)
+			set_page_dirty_lock(buf->pages[i]);
+		put_page(buf->pages[i]);
+	}
+	vfree(buf->sg_desc.sglist);
+	kfree(buf->pages);
+	kfree(buf);
+}
+
+static void *vb2_dma_sg_vaddr(void *buf_priv)
+{
+	struct vb2_dma_sg_buf *buf = buf_priv;
+
+	BUG_ON(!buf);
+
+	if (!buf->vaddr)
+		buf->vaddr = vm_map_ram(buf->pages,
+					buf->sg_desc.num_pages,
+					-1,
+					PAGE_KERNEL);
+
+	/* add offset in case userptr is not page-aligned */
+	return buf->vaddr + buf->offset;
+}
+
+static unsigned int vb2_dma_sg_num_users(void *buf_priv)
+{
+	struct vb2_dma_sg_buf *buf = buf_priv;
+
+	return atomic_read(&buf->refcount);
+}
+
+static int vb2_dma_sg_mmap(void *buf_priv, struct vm_area_struct *vma)
+{
+	struct vb2_dma_sg_buf *buf = buf_priv;
+	unsigned long uaddr = vma->vm_start;
+	unsigned long usize = vma->vm_end - vma->vm_start;
+	int i = 0;
+
+	if (!buf) {
+		printk(KERN_ERR "No memory to map\n");
+		return -EINVAL;
+	}
+
+	do {
+		int ret;
+
+		ret = vm_insert_page(vma, uaddr, buf->pages[i++]);
+		if (ret) {
+			printk(KERN_ERR "Remapping memory, error: %d\n", ret);
+			return ret;
+		}
+
+		uaddr += PAGE_SIZE;
+		usize -= PAGE_SIZE;
+	} while (usize > 0);
+
+
+	/*
+	 * Use common vm_area operations to track buffer refcount.
+	 */
+	vma->vm_private_data	= &buf->handler;
+	vma->vm_ops		= &vb2_common_vm_ops;
+
+	vma->vm_ops->open(vma);
+
+	return 0;
+}
+
+static void *vb2_dma_sg_cookie(void *buf_priv)
+{
+	struct vb2_dma_sg_buf *buf = buf_priv;
+
+	return &buf->sg_desc;
+}
+
+const struct vb2_mem_ops vb2_dma_sg_memops = {
+	.alloc		= vb2_dma_sg_alloc,
+	.put		= vb2_dma_sg_put,
+	.get_userptr	= vb2_dma_sg_get_userptr,
+	.put_userptr	= vb2_dma_sg_put_userptr,
+	.vaddr		= vb2_dma_sg_vaddr,
+	.mmap		= vb2_dma_sg_mmap,
+	.num_users	= vb2_dma_sg_num_users,
+	.cookie		= vb2_dma_sg_cookie,
+};
+EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);
+
+MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
+MODULE_AUTHOR("Andrzej Pietrasiewicz");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/videobuf2-memops.c b/drivers/media/video/videobuf2-memops.c
new file mode 100644
index 000000000000..a3eb6567dea5
--- /dev/null
+++ b/drivers/media/video/videobuf2-memops.c
@@ -0,0 +1,235 @@
+/*
+ * videobuf2-memops.c - generic memory handling routines for videobuf2
+ *
+ * Copyright (C) 2010 Samsung Electronics
+ *
+ * Author: Pawel Osciak <p.osciak@samsung.com>
+ *	   Marek Szyprowski <m.szyprowski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/dma-mapping.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/file.h>
+#include <linux/slab.h>
+
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-memops.h>
+
+/**
+ * vb2_get_vma() - acquire and lock the virtual memory area
+ * @vma:	given virtual memory area
+ *
+ * This function attempts to acquire an area mapped in the userspace for
+ * the duration of a hardware operation. The area is "locked" by performing
+ * the same set of operation that are done when process calls fork() and
+ * memory areas are duplicated.
+ *
+ * Returns a copy of a virtual memory region on success or NULL.
+ */
+struct vm_area_struct *vb2_get_vma(struct vm_area_struct *vma)
+{
+	struct vm_area_struct *vma_copy;
+
+	vma_copy = kmalloc(sizeof(*vma_copy), GFP_KERNEL);
+	if (vma_copy == NULL)
+		return NULL;
+
+	if (vma->vm_ops && vma->vm_ops->open)
+		vma->vm_ops->open(vma);
+
+	if (vma->vm_file)
+		get_file(vma->vm_file);
+
+	memcpy(vma_copy, vma, sizeof(*vma));
+
+	vma_copy->vm_mm = NULL;
+	vma_copy->vm_next = NULL;
+	vma_copy->vm_prev = NULL;
+
+	return vma_copy;
+}
+
+/**
+ * vb2_put_userptr() - release a userspace virtual memory area
+ * @vma:	virtual memory region associated with the area to be released
+ *
+ * This function releases the previously acquired memory area after a hardware
+ * operation.
+ */
+void vb2_put_vma(struct vm_area_struct *vma)
+{
+	if (!vma)
+		return;
+
+	if (vma->vm_file)
+		fput(vma->vm_file);
+
+	if (vma->vm_ops && vma->vm_ops->close)
+		vma->vm_ops->close(vma);
+
+	kfree(vma);
+}
+EXPORT_SYMBOL_GPL(vb2_put_vma);
+
+/**
+ * vb2_get_contig_userptr() - lock physically contiguous userspace mapped memory
+ * @vaddr:	starting virtual address of the area to be verified
+ * @size:	size of the area
+ * @res_paddr:	will return physical address for the given vaddr
+ * @res_vma:	will return locked copy of struct vm_area for the given area
+ *
+ * This function will go through memory area of size @size mapped at @vaddr and
+ * verify that the underlying physical pages are contiguous. If they are
+ * contiguous the virtual memory area is locked and a @res_vma is filled with
+ * the copy and @res_pa set to the physical address of the buffer.
+ *
+ * Returns 0 on success.
+ */
+int vb2_get_contig_userptr(unsigned long vaddr, unsigned long size,
+			   struct vm_area_struct **res_vma, dma_addr_t *res_pa)
+{
+	struct mm_struct *mm = current->mm;
+	struct vm_area_struct *vma;
+	unsigned long offset, start, end;
+	unsigned long this_pfn, prev_pfn;
+	dma_addr_t pa = 0;
+	int ret = -EFAULT;
+
+	start = vaddr;
+	offset = start & ~PAGE_MASK;
+	end = start + size;
+
+	down_read(&mm->mmap_sem);
+	vma = find_vma(mm, start);
+
+	if (vma == NULL || vma->vm_end < end)
+		goto done;
+
+	for (prev_pfn = 0; start < end; start += PAGE_SIZE) {
+		ret = follow_pfn(vma, start, &this_pfn);
+		if (ret)
+			goto done;
+
+		if (prev_pfn == 0)
+			pa = this_pfn << PAGE_SHIFT;
+		else if (this_pfn != prev_pfn + 1) {
+			ret = -EFAULT;
+			goto done;
+		}
+		prev_pfn = this_pfn;
+	}
+
+	/*
+	 * Memory is contigous, lock vma and return to the caller
+	 */
+	*res_vma = vb2_get_vma(vma);
+	if (*res_vma == NULL) {
+		ret = -ENOMEM;
+		goto done;
+	}
+	*res_pa = pa + offset;
+	ret = 0;
+
+done:
+	up_read(&mm->mmap_sem);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vb2_get_contig_userptr);
+
+/**
+ * vb2_mmap_pfn_range() - map physical pages to userspace
+ * @vma:	virtual memory region for the mapping
+ * @paddr:	starting physical address of the memory to be mapped
+ * @size:	size of the memory to be mapped
+ * @vm_ops:	vm operations to be assigned to the created area
+ * @priv:	private data to be associated with the area
+ *
+ * Returns 0 on success.
+ */
+int vb2_mmap_pfn_range(struct vm_area_struct *vma, unsigned long paddr,
+				unsigned long size,
+				const struct vm_operations_struct *vm_ops,
+				void *priv)
+{
+	int ret;
+
+	size = min_t(unsigned long, vma->vm_end - vma->vm_start, size);
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	ret = remap_pfn_range(vma, vma->vm_start, paddr >> PAGE_SHIFT,
+				size, vma->vm_page_prot);
+	if (ret) {
+		printk(KERN_ERR "Remapping memory failed, error: %d\n", ret);
+		return ret;
+	}
+
+	vma->vm_flags		|= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_private_data	= priv;
+	vma->vm_ops		= vm_ops;
+
+	vma->vm_ops->open(vma);
+
+	printk(KERN_DEBUG "%s: mapped paddr 0x%08lx at 0x%08lx, size %ld\n",
+			__func__, paddr, vma->vm_start, size);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_mmap_pfn_range);
+
+/**
+ * vb2_common_vm_open() - increase refcount of the vma
+ * @vma:	virtual memory region for the mapping
+ *
+ * This function adds another user to the provided vma. It expects
+ * struct vb2_vmarea_handler pointer in vma->vm_private_data.
+ */
+static void vb2_common_vm_open(struct vm_area_struct *vma)
+{
+	struct vb2_vmarea_handler *h = vma->vm_private_data;
+
+	printk(KERN_DEBUG "%s: %p, refcount: %d, vma: %08lx-%08lx\n",
+	       __func__, h, atomic_read(h->refcount), vma->vm_start,
+	       vma->vm_end);
+
+	atomic_inc(h->refcount);
+}
+
+/**
+ * vb2_common_vm_close() - decrease refcount of the vma
+ * @vma:	virtual memory region for the mapping
+ *
+ * This function releases the user from the provided vma. It expects
+ * struct vb2_vmarea_handler pointer in vma->vm_private_data.
+ */
+static void vb2_common_vm_close(struct vm_area_struct *vma)
+{
+	struct vb2_vmarea_handler *h = vma->vm_private_data;
+
+	printk(KERN_DEBUG "%s: %p, refcount: %d, vma: %08lx-%08lx\n",
+	       __func__, h, atomic_read(h->refcount), vma->vm_start,
+	       vma->vm_end);
+
+	h->put(h->arg);
+}
+
+/**
+ * vb2_common_vm_ops - common vm_ops used for tracking refcount of mmaped
+ * video buffers
+ */
+const struct vm_operations_struct vb2_common_vm_ops = {
+	.open = vb2_common_vm_open,
+	.close = vb2_common_vm_close,
+};
+EXPORT_SYMBOL_GPL(vb2_common_vm_ops);
+
+MODULE_DESCRIPTION("common memory handling routines for videobuf2");
+MODULE_AUTHOR("Pawel Osciak");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/videobuf2-vmalloc.c b/drivers/media/video/videobuf2-vmalloc.c
new file mode 100644
index 000000000000..b5e6936fb628
--- /dev/null
+++ b/drivers/media/video/videobuf2-vmalloc.c
@@ -0,0 +1,132 @@
+/*
+ * videobuf2-vmalloc.c - vmalloc memory allocator for videobuf2
+ *
+ * Copyright (C) 2010 Samsung Electronics
+ *
+ * Author: Pawel Osciak <p.osciak@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-memops.h>
+
+struct vb2_vmalloc_buf {
+	void				*vaddr;
+	unsigned long			size;
+	atomic_t			refcount;
+	struct vb2_vmarea_handler	handler;
+};
+
+static void vb2_vmalloc_put(void *buf_priv);
+
+static void *vb2_vmalloc_alloc(void *alloc_ctx, unsigned long size)
+{
+	struct vb2_vmalloc_buf *buf;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return NULL;
+
+	buf->size = size;
+	buf->vaddr = vmalloc_user(buf->size);
+	buf->handler.refcount = &buf->refcount;
+	buf->handler.put = vb2_vmalloc_put;
+	buf->handler.arg = buf;
+
+	if (!buf->vaddr) {
+		printk(KERN_ERR "vmalloc of size %ld failed\n", buf->size);
+		kfree(buf);
+		return NULL;
+	}
+
+	atomic_inc(&buf->refcount);
+	printk(KERN_DEBUG "Allocated vmalloc buffer of size %ld at vaddr=%p\n",
+			buf->size, buf->vaddr);
+
+	return buf;
+}
+
+static void vb2_vmalloc_put(void *buf_priv)
+{
+	struct vb2_vmalloc_buf *buf = buf_priv;
+
+	if (atomic_dec_and_test(&buf->refcount)) {
+		printk(KERN_DEBUG "%s: Freeing vmalloc mem at vaddr=%p\n",
+			__func__, buf->vaddr);
+		vfree(buf->vaddr);
+		kfree(buf);
+	}
+}
+
+static void *vb2_vmalloc_vaddr(void *buf_priv)
+{
+	struct vb2_vmalloc_buf *buf = buf_priv;
+
+	BUG_ON(!buf);
+
+	if (!buf->vaddr) {
+		printk(KERN_ERR "Address of an unallocated plane requested\n");
+		return NULL;
+	}
+
+	return buf->vaddr;
+}
+
+static unsigned int vb2_vmalloc_num_users(void *buf_priv)
+{
+	struct vb2_vmalloc_buf *buf = buf_priv;
+	return atomic_read(&buf->refcount);
+}
+
+static int vb2_vmalloc_mmap(void *buf_priv, struct vm_area_struct *vma)
+{
+	struct vb2_vmalloc_buf *buf = buf_priv;
+	int ret;
+
+	if (!buf) {
+		printk(KERN_ERR "No memory to map\n");
+		return -EINVAL;
+	}
+
+	ret = remap_vmalloc_range(vma, buf->vaddr, 0);
+	if (ret) {
+		printk(KERN_ERR "Remapping vmalloc memory, error: %d\n", ret);
+		return ret;
+	}
+
+	/*
+	 * Make sure that vm_areas for 2 buffers won't be merged together
+	 */
+	vma->vm_flags		|= VM_DONTEXPAND;
+
+	/*
+	 * Use common vm_area operations to track buffer refcount.
+	 */
+	vma->vm_private_data	= &buf->handler;
+	vma->vm_ops		= &vb2_common_vm_ops;
+
+	vma->vm_ops->open(vma);
+
+	return 0;
+}
+
+const struct vb2_mem_ops vb2_vmalloc_memops = {
+	.alloc		= vb2_vmalloc_alloc,
+	.put		= vb2_vmalloc_put,
+	.vaddr		= vb2_vmalloc_vaddr,
+	.mmap		= vb2_vmalloc_mmap,
+	.num_users	= vb2_vmalloc_num_users,
+};
+EXPORT_SYMBOL_GPL(vb2_vmalloc_memops);
+
+MODULE_DESCRIPTION("vmalloc memory handling routines for videobuf2");
+MODULE_AUTHOR("Pawel Osciak");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index c49c39386bd0..bd104d0ad36c 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -7,6 +7,9 @@
  *      John Sokol <sokol--a.t--videotechnology.com>
  *      http://v4l.videotechnology.com/
  *
+ *      Conversion to videobuf2 by Pawel Osciak & Marek Szyprowski
+ *      Copyright (c) 2010 Samsung Electronics
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the BSD Licence, GNU General Public License
  * as published by the Free Software Foundation; either version 2 of the
@@ -23,12 +26,11 @@
 #include <linux/mutex.h>
 #include <linux/videodev2.h>
 #include <linux/kthread.h>
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 20)
 #include <linux/freezer.h>
-#endif
-#include <media/videobuf-vmalloc.h>
+#include <media/videobuf2-vmalloc.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-common.h>
 
 #define VIVI_MODULE_NAME "vivi"
@@ -42,7 +44,7 @@
 #define MAX_HEIGHT 1200
 
 #define VIVI_MAJOR_VERSION 0
-#define VIVI_MINOR_VERSION 7
+#define VIVI_MINOR_VERSION 8
 #define VIVI_RELEASE 0
 #define VIVI_VERSION \
 	KERNEL_VERSION(VIVI_MAJOR_VERSION, VIVI_MINOR_VERSION, VIVI_RELEASE)
@@ -133,16 +135,11 @@ static struct vivi_fmt *get_format(struct v4l2_format *f)
 	return &formats[k];
 }
 
-struct sg_to_addr {
-	int pos;
-	struct scatterlist *sg;
-};
-
 /* buffer for one video frame */
 struct vivi_buffer {
 	/* common v4l buffer stuff -- must be first */
-	struct videobuf_buffer vb;
-
+	struct vb2_buffer	vb;
+	struct list_head	list;
 	struct vivi_fmt        *fmt;
 };
 
@@ -162,13 +159,20 @@ static LIST_HEAD(vivi_devlist);
 struct vivi_dev {
 	struct list_head           vivi_devlist;
 	struct v4l2_device 	   v4l2_dev;
+	struct v4l2_ctrl_handler   ctrl_handler;
 
 	/* controls */
-	int 			   brightness;
-	int 			   contrast;
-	int 			   saturation;
-	int 			   hue;
-	int 			   volume;
+	struct v4l2_ctrl	   *brightness;
+	struct v4l2_ctrl	   *contrast;
+	struct v4l2_ctrl	   *saturation;
+	struct v4l2_ctrl	   *hue;
+	struct v4l2_ctrl	   *volume;
+	struct v4l2_ctrl	   *button;
+	struct v4l2_ctrl	   *boolean;
+	struct v4l2_ctrl	   *int32;
+	struct v4l2_ctrl	   *int64;
+	struct v4l2_ctrl	   *menu;
+	struct v4l2_ctrl	   *string;
 
 	spinlock_t                 slock;
 	struct mutex		   mutex;
@@ -181,6 +185,7 @@ struct vivi_dev {
 	/* Several counters */
 	unsigned 		   ms;
 	unsigned long              jiffies;
+	unsigned		   button_pressed;
 
 	int			   mv_count;	/* Controls bars movement */
 
@@ -190,9 +195,11 @@ struct vivi_dev {
 	/* video capture */
 	struct vivi_fmt            *fmt;
 	unsigned int               width, height;
-	struct videobuf_queue      vb_vidq;
+	struct vb2_queue	   vb_vidq;
+	enum v4l2_field		   field;
+	unsigned int		   field_count;
 
-	unsigned long 		   generating;
+	unsigned int		   open_count;
 	u8 			   bars[9][3];
 	u8 			   line[MAX_WIDTH * 4];
 };
@@ -443,10 +450,10 @@ static void gen_text(struct vivi_dev *dev, char *basep,
 
 static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 {
-	int hmax = buf->vb.height;
-	int wmax = buf->vb.width;
+	int wmax = dev->width;
+	int hmax = dev->height;
 	struct timeval ts;
-	void *vbuf = videobuf_to_vmalloc(&buf->vb);
+	void *vbuf = vb2_plane_vaddr(&buf->vb, 0);
 	unsigned ms;
 	char str[100];
 	int h, line = 1;
@@ -472,22 +479,38 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 			dev->width, dev->height, dev->input);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
 
+	mutex_lock(&dev->ctrl_handler.lock);
 	snprintf(str, sizeof(str), " brightness %3d, contrast %3d, saturation %3d, hue %d ",
-			dev->brightness,
-			dev->contrast,
-			dev->saturation,
-			dev->hue);
+			dev->brightness->cur.val,
+			dev->contrast->cur.val,
+			dev->saturation->cur.val,
+			dev->hue->cur.val);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " volume %3d ", dev->volume);
+	snprintf(str, sizeof(str), " volume %3d ", dev->volume->cur.val);
 	gen_text(dev, vbuf, line++ * 16, 16, str);
+	snprintf(str, sizeof(str), " int32 %d, int64 %lld ",
+			dev->int32->cur.val,
+			dev->int64->cur.val64);
+	gen_text(dev, vbuf, line++ * 16, 16, str);
+	snprintf(str, sizeof(str), " boolean %d, menu %s, string \"%s\" ",
+			dev->boolean->cur.val,
+			dev->menu->qmenu[dev->menu->cur.val],
+			dev->string->cur.string);
+	mutex_unlock(&dev->ctrl_handler.lock);
+	gen_text(dev, vbuf, line++ * 16, 16, str);
+	if (dev->button_pressed) {
+		dev->button_pressed--;
+		snprintf(str, sizeof(str), " button pressed!");
+		gen_text(dev, vbuf, line++ * 16, 16, str);
+	}
 
 	dev->mv_count += 2;
 
-	/* Advice that buffer was filled */
-	buf->vb.field_count++;
+	buf->vb.v4l2_buf.field = dev->field;
+	dev->field_count++;
+	buf->vb.v4l2_buf.sequence = dev->field_count >> 1;
 	do_gettimeofday(&ts);
-	buf->vb.ts = ts;
-	buf->vb.state = VIDEOBUF_DONE;
+	buf->vb.v4l2_buf.timestamp = ts;
 }
 
 static void vivi_thread_tick(struct vivi_dev *dev)
@@ -504,23 +527,17 @@ static void vivi_thread_tick(struct vivi_dev *dev)
 		goto unlock;
 	}
 
-	buf = list_entry(dma_q->active.next,
-			 struct vivi_buffer, vb.queue);
+	buf = list_entry(dma_q->active.next, struct vivi_buffer, list);
+	list_del(&buf->list);
 
-	/* Nobody is waiting on this buffer, return */
-	if (!waitqueue_active(&buf->vb.done))
-		goto unlock;
-
-	list_del(&buf->vb.queue);
-
-	do_gettimeofday(&buf->vb.ts);
+	do_gettimeofday(&buf->vb.v4l2_buf.timestamp);
 
 	/* Fill buffer */
 	vivi_fillbuff(dev, buf);
 	dprintk(dev, 1, "filled buffer %p\n", buf);
 
-	wake_up(&buf->vb.done);
-	dprintk(dev, 2, "[%p/%d] wakeup\n", buf, buf->vb. i);
+	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
+	dprintk(dev, 2, "[%p/%d] done\n", buf, buf->vb.v4l2_buf.index);
 unlock:
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
@@ -571,17 +588,12 @@ static int vivi_thread(void *data)
 	return 0;
 }
 
-static void vivi_start_generating(struct file *file)
+static int vivi_start_generating(struct vivi_dev *dev)
 {
-	struct vivi_dev *dev = video_drvdata(file);
 	struct vivi_dmaqueue *dma_q = &dev->vidq;
 
 	dprintk(dev, 1, "%s\n", __func__);
 
-	if (test_and_set_bit(0, &dev->generating))
-		return;
-	file->private_data = dev;
-
 	/* Resets frame counters */
 	dev->ms = 0;
 	dev->mv_count = 0;
@@ -593,146 +605,200 @@ static void vivi_start_generating(struct file *file)
 
 	if (IS_ERR(dma_q->kthread)) {
 		v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
-		clear_bit(0, &dev->generating);
-		return;
+		return PTR_ERR(dma_q->kthread);
 	}
 	/* Wakes thread */
 	wake_up_interruptible(&dma_q->wq);
 
 	dprintk(dev, 1, "returning from %s\n", __func__);
+	return 0;
 }
 
-static void vivi_stop_generating(struct file *file)
+static void vivi_stop_generating(struct vivi_dev *dev)
 {
-	struct vivi_dev *dev = video_drvdata(file);
 	struct vivi_dmaqueue *dma_q = &dev->vidq;
 
 	dprintk(dev, 1, "%s\n", __func__);
 
-	if (!file->private_data)
-		return;
-	if (!test_and_clear_bit(0, &dev->generating))
-		return;
-
 	/* shutdown control thread */
 	if (dma_q->kthread) {
 		kthread_stop(dma_q->kthread);
 		dma_q->kthread = NULL;
 	}
-	videobuf_stop(&dev->vb_vidq);
-	videobuf_mmap_free(&dev->vb_vidq);
-}
 
-static int vivi_is_generating(struct vivi_dev *dev)
-{
-	return test_bit(0, &dev->generating);
+	/*
+	 * Typical driver might need to wait here until dma engine stops.
+	 * In this case we can abort imiedetly, so it's just a noop.
+	 */
+
+	/* Release all active buffers */
+	while (!list_empty(&dma_q->active)) {
+		struct vivi_buffer *buf;
+		buf = list_entry(dma_q->active.next, struct vivi_buffer, list);
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+		dprintk(dev, 2, "[%p/%d] done\n", buf, buf->vb.v4l2_buf.index);
+	}
 }
-
 /* ------------------------------------------------------------------
 	Videobuf operations
    ------------------------------------------------------------------*/
-static int
-buffer_setup(struct videobuf_queue *vq, unsigned int *count, unsigned int *size)
+static int queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+				unsigned int *nplanes, unsigned long sizes[],
+				void *alloc_ctxs[])
 {
-	struct vivi_dev *dev = vq->priv_data;
+	struct vivi_dev *dev = vb2_get_drv_priv(vq);
+	unsigned long size;
+
+	size = dev->width * dev->height * 2;
+
+	if (0 == *nbuffers)
+		*nbuffers = 32;
+
+	while (size * *nbuffers > vid_limit * 1024 * 1024)
+		(*nbuffers)--;
 
-	*size = dev->width * dev->height * 2;
+	*nplanes = 1;
 
-	if (0 == *count)
-		*count = 32;
+	sizes[0] = size;
 
-	while (*size * *count > vid_limit * 1024 * 1024)
-		(*count)--;
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
 
-	dprintk(dev, 1, "%s, count=%d, size=%d\n", __func__,
-		*count, *size);
+	dprintk(dev, 1, "%s, count=%d, size=%ld\n", __func__,
+		*nbuffers, size);
 
 	return 0;
 }
 
-static void free_buffer(struct videobuf_queue *vq, struct vivi_buffer *buf)
+static int buffer_init(struct vb2_buffer *vb)
 {
-	struct vivi_dev *dev = vq->priv_data;
+	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 
-	dprintk(dev, 1, "%s, state: %i\n", __func__, buf->vb.state);
+	BUG_ON(NULL == dev->fmt);
+
+	/*
+	 * This callback is called once per buffer, after its allocation.
+	 *
+	 * Vivi does not allow changing format during streaming, but it is
+	 * possible to do so when streaming is paused (i.e. in streamoff state).
+	 * Buffers however are not freed when going into streamoff and so
+	 * buffer size verification has to be done in buffer_prepare, on each
+	 * qbuf.
+	 * It would be best to move verification code here to buf_init and
+	 * s_fmt though.
+	 */
 
-	videobuf_vmalloc_free(&buf->vb);
-	dprintk(dev, 1, "free_buffer: freed\n");
-	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+	return 0;
 }
 
-static int
-buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
-						enum v4l2_field field)
+static int buffer_prepare(struct vb2_buffer *vb)
 {
-	struct vivi_dev *dev = vq->priv_data;
+	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 	struct vivi_buffer *buf = container_of(vb, struct vivi_buffer, vb);
-	int rc;
+	unsigned long size;
 
-	dprintk(dev, 1, "%s, field=%d\n", __func__, field);
+	dprintk(dev, 1, "%s, field=%d\n", __func__, vb->v4l2_buf.field);
 
 	BUG_ON(NULL == dev->fmt);
 
+	/*
+	 * Theses properties only change when queue is idle, see s_fmt.
+	 * The below checks should not be performed here, on each
+	 * buffer_prepare (i.e. on each qbuf). Most of the code in this function
+	 * should thus be moved to buffer_init and s_fmt.
+	 */
 	if (dev->width  < 48 || dev->width  > MAX_WIDTH ||
 	    dev->height < 32 || dev->height > MAX_HEIGHT)
 		return -EINVAL;
 
-	buf->vb.size = dev->width * dev->height * 2;
-	if (0 != buf->vb.baddr && buf->vb.bsize < buf->vb.size)
+	size = dev->width * dev->height * 2;
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %lu)\n",
+				__func__, vb2_plane_size(vb, 0), size);
 		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(&buf->vb, 0, size);
 
-	/* These properties only change when queue is idle, see s_fmt */
-	buf->fmt       = dev->fmt;
-	buf->vb.width  = dev->width;
-	buf->vb.height = dev->height;
-	buf->vb.field  = field;
+	buf->fmt = dev->fmt;
 
 	precalculate_bars(dev);
 	precalculate_line(dev);
 
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-		rc = videobuf_iolock(vq, &buf->vb, NULL);
-		if (rc < 0)
-			goto fail;
-	}
+	return 0;
+}
 
-	buf->vb.state = VIDEOBUF_PREPARED;
+static int buffer_finish(struct vb2_buffer *vb)
+{
+	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	dprintk(dev, 1, "%s\n", __func__);
 	return 0;
+}
+
+static void buffer_cleanup(struct vb2_buffer *vb)
+{
+	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	dprintk(dev, 1, "%s\n", __func__);
 
-fail:
-	free_buffer(vq, buf);
-	return rc;
 }
 
-static void
-buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+static void buffer_queue(struct vb2_buffer *vb)
 {
-	struct vivi_dev *dev = vq->priv_data;
+	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 	struct vivi_buffer *buf = container_of(vb, struct vivi_buffer, vb);
 	struct vivi_dmaqueue *vidq = &dev->vidq;
+	unsigned long flags = 0;
 
 	dprintk(dev, 1, "%s\n", __func__);
 
-	buf->vb.state = VIDEOBUF_QUEUED;
-	list_add_tail(&buf->vb.queue, &vidq->active);
+	spin_lock_irqsave(&dev->slock, flags);
+	list_add_tail(&buf->list, &vidq->active);
+	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static void buffer_release(struct videobuf_queue *vq,
-			   struct videobuf_buffer *vb)
+static int start_streaming(struct vb2_queue *vq)
 {
-	struct vivi_dev *dev = vq->priv_data;
-	struct vivi_buffer *buf  = container_of(vb, struct vivi_buffer, vb);
+	struct vivi_dev *dev = vb2_get_drv_priv(vq);
+	dprintk(dev, 1, "%s\n", __func__);
+	return vivi_start_generating(dev);
+}
 
+/* abort streaming and wait for last buffer */
+static int stop_streaming(struct vb2_queue *vq)
+{
+	struct vivi_dev *dev = vb2_get_drv_priv(vq);
 	dprintk(dev, 1, "%s\n", __func__);
+	vivi_stop_generating(dev);
+	return 0;
+}
+
+static void vivi_lock(struct vb2_queue *vq)
+{
+	struct vivi_dev *dev = vb2_get_drv_priv(vq);
+	mutex_lock(&dev->mutex);
+}
 
-	free_buffer(vq, buf);
+static void vivi_unlock(struct vb2_queue *vq)
+{
+	struct vivi_dev *dev = vb2_get_drv_priv(vq);
+	mutex_unlock(&dev->mutex);
 }
 
-static struct videobuf_queue_ops vivi_video_qops = {
-	.buf_setup      = buffer_setup,
-	.buf_prepare    = buffer_prepare,
-	.buf_queue      = buffer_queue,
-	.buf_release    = buffer_release,
+
+static struct vb2_ops vivi_video_qops = {
+	.queue_setup		= queue_setup,
+	.buf_init		= buffer_init,
+	.buf_prepare		= buffer_prepare,
+	.buf_finish		= buffer_finish,
+	.buf_cleanup		= buffer_cleanup,
+	.buf_queue		= buffer_queue,
+	.start_streaming	= start_streaming,
+	.stop_streaming		= stop_streaming,
+	.wait_prepare		= vivi_unlock,
+	.wait_finish		= vivi_lock,
 };
 
 /* ------------------------------------------------------------------
@@ -774,7 +840,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 
 	f->fmt.pix.width        = dev->width;
 	f->fmt.pix.height       = dev->height;
-	f->fmt.pix.field        = dev->vb_vidq.field;
+	f->fmt.pix.field        = dev->field;
 	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
 	f->fmt.pix.bytesperline =
 		(f->fmt.pix.width * dev->fmt->depth) >> 3;
@@ -820,82 +886,60 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 					struct v4l2_format *f)
 {
 	struct vivi_dev *dev = video_drvdata(file);
+	struct vb2_queue *q = &dev->vb_vidq;
 
 	int ret = vidioc_try_fmt_vid_cap(file, priv, f);
 	if (ret < 0)
 		return ret;
 
-	if (vivi_is_generating(dev)) {
+	if (vb2_is_streaming(q)) {
 		dprintk(dev, 1, "%s device busy\n", __func__);
-		ret = -EBUSY;
-		goto out;
+		return -EBUSY;
 	}
 
 	dev->fmt = get_format(f);
 	dev->width = f->fmt.pix.width;
 	dev->height = f->fmt.pix.height;
-	dev->vb_vidq.field = f->fmt.pix.field;
-	ret = 0;
-out:
-	return ret;
+	dev->field = f->fmt.pix.field;
+
+	return 0;
 }
 
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *p)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-
-	return videobuf_reqbufs(&dev->vb_vidq, p);
+	return vb2_reqbufs(&dev->vb_vidq, p);
 }
 
 static int vidioc_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-
-	return videobuf_querybuf(&dev->vb_vidq, p);
+	return vb2_querybuf(&dev->vb_vidq, p);
 }
 
 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-
-	return videobuf_qbuf(&dev->vb_vidq, p);
+	return vb2_qbuf(&dev->vb_vidq, p);
 }
 
 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-
-	return videobuf_dqbuf(&dev->vb_vidq, p,
-				file->f_flags & O_NONBLOCK);
+	return vb2_dqbuf(&dev->vb_vidq, p, file->f_flags & O_NONBLOCK);
 }
 
 static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-	int ret;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	ret = videobuf_streamon(&dev->vb_vidq);
-	if (ret)
-		return ret;
-
-	vivi_start_generating(file);
-	return 0;
+	return vb2_streamon(&dev->vb_vidq, i);
 }
 
 static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-	int ret;
-
-	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	ret = videobuf_streamoff(&dev->vb_vidq);
-	if (!ret)
-		vivi_stop_generating(file);
-	return ret;
+	return vb2_streamoff(&dev->vb_vidq, i);
 }
 
 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *i)
@@ -938,106 +982,47 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 }
 
 /* --- controls ---------------------------------------------- */
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 200);
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 127);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 16);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, 0, 255, 1, 127);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-	}
-	return -EINVAL;
-}
 
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
+static int vivi_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct vivi_dev *dev = video_drvdata(file);
+	struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
 
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		ctrl->value = dev->volume;
-		return 0;
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = dev->brightness;
-		return 0;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = dev->contrast;
-		return 0;
-	case V4L2_CID_SATURATION:
-		ctrl->value = dev->saturation;
-		return 0;
-	case V4L2_CID_HUE:
-		ctrl->value = dev->hue;
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	struct v4l2_queryctrl qc;
-	int err;
-
-	qc.id = ctrl->id;
-	err = vidioc_queryctrl(file, priv, &qc);
-	if (err < 0)
-		return err;
-	if (ctrl->value < qc.minimum || ctrl->value > qc.maximum)
-		return -ERANGE;
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_VOLUME:
-		dev->volume = ctrl->value;
-		return 0;
-	case V4L2_CID_BRIGHTNESS:
-		dev->brightness = ctrl->value;
-		return 0;
-	case V4L2_CID_CONTRAST:
-		dev->contrast = ctrl->value;
-		return 0;
-	case V4L2_CID_SATURATION:
-		dev->saturation = ctrl->value;
-		return 0;
-	case V4L2_CID_HUE:
-		dev->hue = ctrl->value;
-		return 0;
-	}
-	return -EINVAL;
+	if (ctrl == dev->button)
+		dev->button_pressed = 30;
+	return 0;
 }
 
 /* ------------------------------------------------------------------
 	File operations for the device
    ------------------------------------------------------------------*/
 
+static int vivi_open(struct file *file)
+{
+	struct vivi_dev *dev = video_drvdata(file);
+
+	dprintk(dev, 1, "%s, %p\n", __func__, file);
+	dev->open_count++;
+	return 0;
+}
+
 static ssize_t
 vivi_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 {
 	struct vivi_dev *dev = video_drvdata(file);
 
-	vivi_start_generating(file);
-	return videobuf_read_stream(&dev->vb_vidq, data, count, ppos, 0,
-					file->f_flags & O_NONBLOCK);
+	dprintk(dev, 1, "read called\n");
+	return vb2_read(&dev->vb_vidq, data, count, ppos,
+		       file->f_flags & O_NONBLOCK);
 }
 
 static unsigned int
 vivi_poll(struct file *file, struct poll_table_struct *wait)
 {
 	struct vivi_dev *dev = video_drvdata(file);
-	struct videobuf_queue *q = &dev->vb_vidq;
+	struct vb2_queue *q = &dev->vb_vidq;
 
 	dprintk(dev, 1, "%s\n", __func__);
-
-	vivi_start_generating(file);
-	return videobuf_poll_stream(file, q, wait);
+	return vb2_poll(q, file, wait);
 }
 
 static int vivi_close(struct file *file)
@@ -1045,10 +1030,11 @@ static int vivi_close(struct file *file)
 	struct video_device  *vdev = video_devdata(file);
 	struct vivi_dev *dev = video_drvdata(file);
 
-	vivi_stop_generating(file);
+	dprintk(dev, 1, "close called (dev=%s), file %p\n",
+		video_device_node_name(vdev), file);
 
-	dprintk(dev, 1, "close called (dev=%s)\n",
-		video_device_node_name(vdev));
+	if (--dev->open_count == 0)
+		vb2_queue_release(&dev->vb_vidq);
 	return 0;
 }
 
@@ -1059,8 +1045,7 @@ static int vivi_mmap(struct file *file, struct vm_area_struct *vma)
 
 	dprintk(dev, 1, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
 
-	ret = videobuf_mmap_mapper(&dev->vb_vidq, vma);
-
+	ret = vb2_mmap(&dev->vb_vidq, vma);
 	dprintk(dev, 1, "vma start=0x%08lx, size=%ld, ret=%d\n",
 		(unsigned long)vma->vm_start,
 		(unsigned long)vma->vm_end - (unsigned long)vma->vm_start,
@@ -1068,8 +1053,82 @@ static int vivi_mmap(struct file *file, struct vm_area_struct *vma)
 	return ret;
 }
 
+static const struct v4l2_ctrl_ops vivi_ctrl_ops = {
+	.s_ctrl = vivi_s_ctrl,
+};
+
+#define VIVI_CID_CUSTOM_BASE	(V4L2_CID_USER_BASE | 0xf000)
+
+static const struct v4l2_ctrl_config vivi_ctrl_button = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 0,
+	.name = "Button",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_boolean = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 1,
+	.name = "Boolean",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_int32 = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 2,
+	.name = "Integer 32 Bits",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0x80000000,
+	.max = 0x7fffffff,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_int64 = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 3,
+	.name = "Integer 64 Bits",
+	.type = V4L2_CTRL_TYPE_INTEGER64,
+};
+
+static const char * const vivi_ctrl_menu_strings[] = {
+	"Menu Item 0 (Skipped)",
+	"Menu Item 1",
+	"Menu Item 2 (Skipped)",
+	"Menu Item 3",
+	"Menu Item 4",
+	"Menu Item 5 (Skipped)",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_menu = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 4,
+	.name = "Menu",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.max = 4,
+	.def = 3,
+	.menu_skip_mask = 0x04,
+	.qmenu = vivi_ctrl_menu_strings,
+};
+
+static const struct v4l2_ctrl_config vivi_ctrl_string = {
+	.ops = &vivi_ctrl_ops,
+	.id = VIVI_CID_CUSTOM_BASE + 5,
+	.name = "String",
+	.type = V4L2_CTRL_TYPE_STRING,
+	.min = 2,
+	.max = 4,
+	.step = 1,
+};
+
 static const struct v4l2_file_operations vivi_fops = {
 	.owner		= THIS_MODULE,
+	.open		= vivi_open,
 	.release        = vivi_close,
 	.read           = vivi_read,
 	.poll		= vivi_poll,
@@ -1093,9 +1152,6 @@ static const struct v4l2_ioctl_ops vivi_ioctl_ops = {
 	.vidioc_s_input       = vidioc_s_input,
 	.vidioc_streamon      = vidioc_streamon,
 	.vidioc_streamoff     = vidioc_streamoff,
-	.vidioc_queryctrl     = vidioc_queryctrl,
-	.vidioc_g_ctrl        = vidioc_g_ctrl,
-	.vidioc_s_ctrl        = vidioc_s_ctrl,
 };
 
 static struct video_device vivi_template = {
@@ -1126,6 +1182,7 @@ static int vivi_release(void)
 			video_device_node_name(dev->vfd));
 		video_unregister_device(dev->vfd);
 		v4l2_device_unregister(&dev->v4l2_dev);
+		v4l2_ctrl_handler_free(&dev->ctrl_handler);
 		kfree(dev);
 	}
 
@@ -1136,6 +1193,8 @@ static int __init vivi_create_instance(int inst)
 {
 	struct vivi_dev *dev;
 	struct video_device *vfd;
+	struct v4l2_ctrl_handler *hdl;
+	struct vb2_queue *q;
 	int ret;
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
@@ -1151,20 +1210,46 @@ static int __init vivi_create_instance(int inst)
 	dev->fmt = &formats[0];
 	dev->width = 640;
 	dev->height = 480;
-	dev->volume = 200;
-	dev->brightness = 127;
-	dev->contrast = 16;
-	dev->saturation = 127;
-	dev->hue = 0;
+	hdl = &dev->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 11);
+	dev->volume = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 255, 1, 200);
+	dev->brightness = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+	dev->contrast = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 16);
+	dev->saturation = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 127);
+	dev->hue = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	dev->button = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_button, NULL);
+	dev->int32 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int32, NULL);
+	dev->int64 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int64, NULL);
+	dev->boolean = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_boolean, NULL);
+	dev->menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_menu, NULL);
+	dev->string = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_string, NULL);
+	if (hdl->error) {
+		ret = hdl->error;
+		goto unreg_dev;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
 
 	/* initialize locks */
 	spin_lock_init(&dev->slock);
-	mutex_init(&dev->mutex);
 
-	videobuf_queue_vmalloc_init(&dev->vb_vidq, &vivi_video_qops,
-			NULL, &dev->slock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			V4L2_FIELD_INTERLACED,
-			sizeof(struct vivi_buffer), dev, &dev->mutex);
+	/* initialize queue */
+	q = &dev->vb_vidq;
+	memset(q, 0, sizeof(dev->vb_vidq));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	q->drv_priv = dev;
+	q->buf_struct_size = sizeof(struct vivi_buffer);
+	q->ops = &vivi_video_qops;
+	q->mem_ops = &vb2_vmalloc_memops;
+
+	vb2_queue_init(q);
+
+	mutex_init(&dev->mutex);
 
 	/* init video dma queues */
 	INIT_LIST_HEAD(&dev->vidq.active);
@@ -1178,6 +1263,11 @@ static int __init vivi_create_instance(int inst)
 	*vfd = vivi_template;
 	vfd->debug = debug;
 	vfd->v4l2_dev = &dev->v4l2_dev;
+
+	/*
+	 * Provide a mutex to v4l2 core. It will be used to protect
+	 * all fops and v4l2 ioctls.
+	 */
 	vfd->lock = &dev->mutex;
 
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr);
@@ -1200,6 +1290,7 @@ static int __init vivi_create_instance(int inst)
 rel_vdev:
 	video_device_release(vfd);
 unreg_dev:
+	v4l2_ctrl_handler_free(hdl);
 	v4l2_device_unregister(&dev->v4l2_dev);
 free_dev:
 	kfree(dev);
diff --git a/drivers/media/video/vpx3220.c b/drivers/media/video/vpx3220.c
index 91a01b3cdf8c..75301d10a838 100644
--- a/drivers/media/video/vpx3220.c
+++ b/drivers/media/video/vpx3220.c
@@ -28,6 +28,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
 
 MODULE_DESCRIPTION("vpx3220a/vpx3216b/vpx3214c video decoder driver");
 MODULE_AUTHOR("Laurent Pinchart");
@@ -44,16 +45,13 @@ MODULE_PARM_DESC(debug, "Debug level (0-1)");
 
 struct vpx3220 {
 	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
 	unsigned char reg[255];
 
 	v4l2_std_id norm;
 	int ident;
 	int input;
 	int enable;
-	int bright;
-	int contrast;
-	int hue;
-	int sat;
 };
 
 static inline struct vpx3220 *to_vpx3220(struct v4l2_subdev *sd)
@@ -61,6 +59,11 @@ static inline struct vpx3220 *to_vpx3220(struct v4l2_subdev *sd)
 	return container_of(sd, struct vpx3220, sd);
 }
 
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct vpx3220, hdl)->sd;
+}
+
 static char *inputs[] = { "internal", "composite", "svideo" };
 
 /* ----------------------------------------------------------------------- */
@@ -417,88 +420,26 @@ static int vpx3220_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int vpx3220_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		v4l2_ctrl_query_fill(qc, -128, 127, 1, 0);
-		break;
-
-	case V4L2_CID_CONTRAST:
-		v4l2_ctrl_query_fill(qc, 0, 63, 1, 32);
-		break;
-
-	case V4L2_CID_SATURATION:
-		v4l2_ctrl_query_fill(qc, 0, 4095, 1, 2048);
-		break;
-
-	case V4L2_CID_HUE:
-		v4l2_ctrl_query_fill(qc, -512, 511, 1, 0);
-		break;
-
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int vpx3220_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int vpx3220_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct vpx3220 *decoder = to_vpx3220(sd);
+	struct v4l2_subdev *sd = to_sd(ctrl);
 
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = decoder->bright;
-		break;
+		vpx3220_write(sd, 0xe6, ctrl->val);
+		return 0;
 	case V4L2_CID_CONTRAST:
-		ctrl->value = decoder->contrast;
-		break;
+		/* Bit 7 and 8 is for noise shaping */
+		vpx3220_write(sd, 0xe7, ctrl->val + 192);
+		return 0;
 	case V4L2_CID_SATURATION:
-		ctrl->value = decoder->sat;
-		break;
+		vpx3220_fp_write(sd, 0xa0, ctrl->val);
+		return 0;
 	case V4L2_CID_HUE:
-		ctrl->value = decoder->hue;
-		break;
-	default:
-		return -EINVAL;
+		vpx3220_fp_write(sd, 0x1c, ctrl->val);
+		return 0;
 	}
-	return 0;
-}
-
-static int vpx3220_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct vpx3220 *decoder = to_vpx3220(sd);
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		if (decoder->bright != ctrl->value) {
-			decoder->bright = ctrl->value;
-			vpx3220_write(sd, 0xe6, decoder->bright);
-		}
-		break;
-	case V4L2_CID_CONTRAST:
-		if (decoder->contrast != ctrl->value) {
-			/* Bit 7 and 8 is for noise shaping */
-			decoder->contrast = ctrl->value;
-			vpx3220_write(sd, 0xe7, decoder->contrast + 192);
-		}
-		break;
-	case V4L2_CID_SATURATION:
-		if (decoder->sat != ctrl->value) {
-			decoder->sat = ctrl->value;
-			vpx3220_fp_write(sd, 0xa0, decoder->sat);
-		}
-		break;
-	case V4L2_CID_HUE:
-		if (decoder->hue != ctrl->value) {
-			decoder->hue = ctrl->value;
-			vpx3220_fp_write(sd, 0x1c, decoder->hue);
-		}
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
+	return -EINVAL;
 }
 
 static int vpx3220_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *chip)
@@ -511,12 +452,20 @@ static int vpx3220_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ide
 
 /* ----------------------------------------------------------------------- */
 
+static const struct v4l2_ctrl_ops vpx3220_ctrl_ops = {
+	.s_ctrl = vpx3220_s_ctrl,
+};
+
 static const struct v4l2_subdev_core_ops vpx3220_core_ops = {
 	.g_chip_ident = vpx3220_g_chip_ident,
 	.init = vpx3220_init,
-	.g_ctrl = vpx3220_g_ctrl,
-	.s_ctrl = vpx3220_s_ctrl,
-	.queryctrl = vpx3220_queryctrl,
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
 	.s_std = vpx3220_s_std,
 };
 
@@ -558,10 +507,24 @@ static int vpx3220_probe(struct i2c_client *client,
 	decoder->norm = V4L2_STD_PAL;
 	decoder->input = 0;
 	decoder->enable = 1;
-	decoder->bright = 32768;
-	decoder->contrast = 32768;
-	decoder->hue = 32768;
-	decoder->sat = 32768;
+	v4l2_ctrl_handler_init(&decoder->hdl, 4);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_CONTRAST, 0, 63, 1, 32);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_SATURATION, 0, 4095, 1, 2048);
+	v4l2_ctrl_new_std(&decoder->hdl, &vpx3220_ctrl_ops,
+		V4L2_CID_HUE, -512, 511, 1, 0);
+	sd->ctrl_handler = &decoder->hdl;
+	if (decoder->hdl.error) {
+		int err = decoder->hdl.error;
+
+		v4l2_ctrl_handler_free(&decoder->hdl);
+		kfree(decoder);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&decoder->hdl);
 
 	ver = i2c_smbus_read_byte_data(client, 0x00);
 	pn = (i2c_smbus_read_byte_data(client, 0x02) << 8) +
@@ -599,9 +562,11 @@ static int vpx3220_probe(struct i2c_client *client,
 static int vpx3220_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vpx3220 *decoder = to_vpx3220(sd);
 
 	v4l2_device_unregister_subdev(sd);
-	kfree(to_vpx3220(sd));
+	v4l2_ctrl_handler_free(&decoder->hdl);
+	kfree(decoder);
 	return 0;
 }
 
diff --git a/drivers/media/video/wm8775.c b/drivers/media/video/wm8775.c
index fe8ef6419f83..9cedb1e69b58 100644
--- a/drivers/media/video/wm8775.c
+++ b/drivers/media/video/wm8775.c
@@ -35,6 +35,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-ctrls.h>
+#include <media/wm8775.h>
 
 MODULE_DESCRIPTION("wm8775 driver");
 MODULE_AUTHOR("Ulf Eklund, Hans Verkuil");
@@ -50,10 +51,16 @@ enum {
 	TOT_REGS
 };
 
+#define ALC_HOLD 0x85 /* R17: use zero cross detection, ALC hold time 42.6 ms */
+#define ALC_EN 0x100  /* R17: ALC enable */
+
 struct wm8775_state {
 	struct v4l2_subdev sd;
 	struct v4l2_ctrl_handler hdl;
 	struct v4l2_ctrl *mute;
+	struct v4l2_ctrl *vol;
+	struct v4l2_ctrl *bal;
+	struct v4l2_ctrl *loud;
 	u8 input;		/* Last selected input (0-0xf) */
 };
 
@@ -85,6 +92,30 @@ static int wm8775_write(struct v4l2_subdev *sd, int reg, u16 val)
 	return -1;
 }
 
+static void wm8775_set_audio(struct v4l2_subdev *sd, int quietly)
+{
+	struct wm8775_state *state = to_state(sd);
+	u8 vol_l, vol_r;
+	int muted = 0 != state->mute->val;
+	u16 volume = (u16)state->vol->val;
+	u16 balance = (u16)state->bal->val;
+
+	/* normalize ( 65535 to 0 -> 255 to 0 (+24dB to -103dB) ) */
+	vol_l = (min(65536 - balance, 32768) * volume) >> 23;
+	vol_r = (min(balance, (u16)32768) * volume) >> 23;
+
+	/* Mute */
+	if (muted || quietly)
+		wm8775_write(sd, R21, 0x0c0 | state->input);
+
+	wm8775_write(sd, R14, vol_l | 0x100); /* 0x100= Left channel ADC zero cross enable */
+	wm8775_write(sd, R15, vol_r | 0x100); /* 0x100= Right channel ADC zero cross enable */
+
+	/* Un-mute */
+	if (!muted)
+		wm8775_write(sd, R21, state->input);
+}
+
 static int wm8775_s_routing(struct v4l2_subdev *sd,
 			    u32 input, u32 output, u32 config)
 {
@@ -102,25 +133,26 @@ static int wm8775_s_routing(struct v4l2_subdev *sd,
 	state->input = input;
 	if (!v4l2_ctrl_g_ctrl(state->mute))
 		return 0;
-	wm8775_write(sd, R21, 0x0c0);
-	wm8775_write(sd, R14, 0x1d4);
-	wm8775_write(sd, R15, 0x1d4);
-	wm8775_write(sd, R21, 0x100 + state->input);
+	if (!v4l2_ctrl_g_ctrl(state->vol))
+		return 0;
+	if (!v4l2_ctrl_g_ctrl(state->bal))
+		return 0;
+	wm8775_set_audio(sd, 1);
 	return 0;
 }
 
 static int wm8775_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct v4l2_subdev *sd = to_sd(ctrl);
-	struct wm8775_state *state = to_state(sd);
 
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
-		wm8775_write(sd, R21, 0x0c0);
-		wm8775_write(sd, R14, 0x1d4);
-		wm8775_write(sd, R15, 0x1d4);
-		if (!ctrl->val)
-			wm8775_write(sd, R21, 0x100 + state->input);
+	case V4L2_CID_AUDIO_VOLUME:
+	case V4L2_CID_AUDIO_BALANCE:
+		wm8775_set_audio(sd, 0);
+		return 0;
+	case V4L2_CID_AUDIO_LOUDNESS:
+		wm8775_write(sd, R17, (ctrl->val ? ALC_EN : 0) | ALC_HOLD);
 		return 0;
 	}
 	return -EINVAL;
@@ -144,16 +176,7 @@ static int wm8775_log_status(struct v4l2_subdev *sd)
 
 static int wm8775_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *freq)
 {
-	struct wm8775_state *state = to_state(sd);
-
-	/* If I remove this, then it can happen that I have no
-	   sound the first time I tune from static to a valid channel.
-	   It's difficult to reproduce and is almost certainly related
-	   to the zero cross detect circuit. */
-	wm8775_write(sd, R21, 0x0c0);
-	wm8775_write(sd, R14, 0x1d4);
-	wm8775_write(sd, R15, 0x1d4);
-	wm8775_write(sd, R21, 0x100 + state->input);
+	wm8775_set_audio(sd, 0);
 	return 0;
 }
 
@@ -203,6 +226,13 @@ static int wm8775_probe(struct i2c_client *client,
 {
 	struct wm8775_state *state;
 	struct v4l2_subdev *sd;
+	int err;
+	bool is_nova_s = false;
+
+	if (client->dev.platform_data) {
+		struct wm8775_platform_data *data = client->dev.platform_data;
+		is_nova_s = data->is_nova_s;
+	}
 
 	/* Check if the adapter supports the needed features */
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
@@ -218,13 +248,18 @@ static int wm8775_probe(struct i2c_client *client,
 	v4l2_i2c_subdev_init(sd, client, &wm8775_ops);
 	state->input = 2;
 
-	v4l2_ctrl_handler_init(&state->hdl, 1);
+	v4l2_ctrl_handler_init(&state->hdl, 4);
 	state->mute = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
 			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	state->vol = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 65535, (65535+99)/100, 0xCF00); /* 0dB*/
+	state->bal = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_BALANCE, 0, 65535, (65535+99)/100, 32768);
+	state->loud = v4l2_ctrl_new_std(&state->hdl, &wm8775_ctrl_ops,
+			V4L2_CID_AUDIO_LOUDNESS, 0, 1, 1, 1);
 	sd->ctrl_handler = &state->hdl;
-	if (state->hdl.error) {
-		int err = state->hdl.error;
-
+	err = state->hdl.error;
+	if (err) {
 		v4l2_ctrl_handler_free(&state->hdl);
 		kfree(state);
 		return err;
@@ -236,29 +271,44 @@ static int wm8775_probe(struct i2c_client *client,
 	wm8775_write(sd, R23, 0x000);
 	/* Disable zero cross detect timeout */
 	wm8775_write(sd, R7, 0x000);
-	/* Left justified, 24-bit mode */
+	/* HPF enable, left justified, 24-bit (Philips) mode */
 	wm8775_write(sd, R11, 0x021);
 	/* Master mode, clock ratio 256fs */
 	wm8775_write(sd, R12, 0x102);
 	/* Powered up */
 	wm8775_write(sd, R13, 0x000);
-	/* ADC gain +2.5dB, enable zero cross */
-	wm8775_write(sd, R14, 0x1d4);
-	/* ADC gain +2.5dB, enable zero cross */
-	wm8775_write(sd, R15, 0x1d4);
-	/* ALC Stereo, ALC target level -1dB FS max gain +8dB */
-	wm8775_write(sd, R16, 0x1bf);
-	/* Enable gain control, use zero cross detection,
-	   ALC hold time 42.6 ms */
-	wm8775_write(sd, R17, 0x185);
+
+	if (!is_nova_s) {
+		/* ADC gain +2.5dB, enable zero cross */
+		wm8775_write(sd, R14, 0x1d4);
+		/* ADC gain +2.5dB, enable zero cross */
+		wm8775_write(sd, R15, 0x1d4);
+		/* ALC Stereo, ALC target level -1dB FS max gain +8dB */
+		wm8775_write(sd, R16, 0x1bf);
+		/* Enable gain control, use zero cross detection,
+		   ALC hold time 42.6 ms */
+		wm8775_write(sd, R17, 0x185);
+	} else {
+		/* ALC stereo, ALC target level -5dB FS, ALC max gain +8dB */
+		wm8775_write(sd, R16, 0x1bb);
+		/* Set ALC mode and hold time */
+		wm8775_write(sd, R17, (state->loud->val ? ALC_EN : 0) | ALC_HOLD);
+	}
 	/* ALC gain ramp up delay 34 s, ALC gain ramp down delay 33 ms */
 	wm8775_write(sd, R18, 0x0a2);
 	/* Enable noise gate, threshold -72dBfs */
 	wm8775_write(sd, R19, 0x005);
-	/* Transient window 4ms, lower PGA gain limit -1dB */
-	wm8775_write(sd, R20, 0x07a);
-	/* LRBOTH = 1, use input 2. */
-	wm8775_write(sd, R21, 0x102);
+	if (!is_nova_s) {
+		/* Transient window 4ms, lower PGA gain limit -1dB */
+		wm8775_write(sd, R20, 0x07a);
+		/* LRBOTH = 1, use input 2. */
+		wm8775_write(sd, R21, 0x102);
+	} else {
+		/* Transient window 4ms, ALC min gain -5dB  */
+		wm8775_write(sd, R20, 0x0fb);
+
+		wm8775_set_audio(sd, 1);      /* set volume/mute/mux */
+	}
 	return 0;
 }
 
diff --git a/drivers/message/fusion/lsi/mpi_cnfg.h b/drivers/message/fusion/lsi/mpi_cnfg.h
index 013c7d881948..22027e7946f7 100644
--- a/drivers/message/fusion/lsi/mpi_cnfg.h
+++ b/drivers/message/fusion/lsi/mpi_cnfg.h
@@ -2593,6 +2593,7 @@ typedef struct _CONFIG_PAGE_SAS_IO_UNIT_0
 #define MPI_SAS_IOUNIT0_RATE_SATA_OOB_COMPLETE              (0x03)
 #define MPI_SAS_IOUNIT0_RATE_1_5                            (0x08)
 #define MPI_SAS_IOUNIT0_RATE_3_0                            (0x09)
+#define MPI_SAS_IOUNIT0_RATE_6_0                            (0x0A)
 
 /* see mpi_sas.h for values for SAS IO Unit Page 0 ControllerPhyDeviceInfo values */
 
diff --git a/drivers/message/fusion/lsi/mpi_ioc.h b/drivers/message/fusion/lsi/mpi_ioc.h
index 8faa4fab7b89..fd6222882a0e 100644
--- a/drivers/message/fusion/lsi/mpi_ioc.h
+++ b/drivers/message/fusion/lsi/mpi_ioc.h
@@ -841,6 +841,7 @@ typedef struct _EVENT_DATA_SAS_PHY_LINK_STATUS
 #define MPI_EVENT_SAS_PLS_LR_RATE_SATA_OOB_COMPLETE         (0x03)
 #define MPI_EVENT_SAS_PLS_LR_RATE_1_5                       (0x08)
 #define MPI_EVENT_SAS_PLS_LR_RATE_3_0                       (0x09)
+#define MPI_EVENT_SAS_PLS_LR_RATE_6_0                       (0x0A)
 
 /* SAS Discovery Event data */
 
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index 3358c0af3466..ec8080c98081 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -7418,7 +7418,12 @@ mpt_display_event_info(MPT_ADAPTER *ioc, EventNotificationReply_t *pEventReply)
 		case MPI_EVENT_SAS_PLS_LR_RATE_3_0:
 			snprintf(evStr, EVENT_DESCR_STR_SZ,
 			   "SAS PHY Link Status: Phy=%d:"
-			   " Rate 3.0 Gpbs",PhyNumber);
+			   " Rate 3.0 Gbps", PhyNumber);
+			break;
+		case MPI_EVENT_SAS_PLS_LR_RATE_6_0:
+			snprintf(evStr, EVENT_DESCR_STR_SZ,
+			   "SAS PHY Link Status: Phy=%d:"
+			   " Rate 6.0 Gbps", PhyNumber);
 			break;
 		default:
 			snprintf(evStr, EVENT_DESCR_STR_SZ,
diff --git a/drivers/message/fusion/mptctl.c b/drivers/message/fusion/mptctl.c
index e8deb8ed0499..878bda0cce70 100644
--- a/drivers/message/fusion/mptctl.c
+++ b/drivers/message/fusion/mptctl.c
@@ -1314,8 +1314,10 @@ mptctl_getiocinfo (unsigned long arg, unsigned int data_size)
 	else
 		karg->adapterType = MPT_IOCTL_INTERFACE_SCSI;
 
-	if (karg->hdr.port > 1)
+	if (karg->hdr.port > 1) {
+		kfree(karg);
 		return -EINVAL;
+	}
 	port = karg->hdr.port;
 
 	karg->port = port;
diff --git a/drivers/message/fusion/mptsas.c b/drivers/message/fusion/mptsas.c
index 8aefb1829fcd..f5a14afad2cd 100644
--- a/drivers/message/fusion/mptsas.c
+++ b/drivers/message/fusion/mptsas.c
@@ -1973,7 +1973,6 @@ static struct scsi_host_template mptsas_driver_template = {
 	.change_queue_depth 		= mptscsih_change_queue_depth,
 	.eh_abort_handler		= mptscsih_abort,
 	.eh_device_reset_handler	= mptscsih_dev_reset,
-	.eh_bus_reset_handler		= mptscsih_bus_reset,
 	.eh_host_reset_handler		= mptscsih_host_reset,
 	.bios_param			= mptscsih_bios_param,
 	.can_queue			= MPT_SAS_CAN_QUEUE,
@@ -3063,6 +3062,9 @@ static int mptsas_probe_one_phy(struct device *dev,
 	case MPI_SAS_IOUNIT0_RATE_3_0:
 		phy->negotiated_linkrate = SAS_LINK_RATE_3_0_GBPS;
 		break;
+	case MPI_SAS_IOUNIT0_RATE_6_0:
+		phy->negotiated_linkrate = SAS_LINK_RATE_6_0_GBPS;
+		break;
 	case MPI_SAS_IOUNIT0_RATE_SATA_OOB_COMPLETE:
 	case MPI_SAS_IOUNIT0_RATE_UNKNOWN:
 	default:
@@ -3691,7 +3693,8 @@ mptsas_send_link_status_event(struct fw_event_work *fw_event)
 	}
 
 	if (link_rate == MPI_SAS_IOUNIT0_RATE_1_5 ||
-	    link_rate == MPI_SAS_IOUNIT0_RATE_3_0) {
+	    link_rate == MPI_SAS_IOUNIT0_RATE_3_0 ||
+	    link_rate == MPI_SAS_IOUNIT0_RATE_6_0) {
 
 		if (!port_info) {
 			if (ioc->old_sas_discovery_protocal) {
diff --git a/drivers/message/i2o/driver.c b/drivers/message/i2o/driver.c
index a0421efe04ca..8a5b2d8f4daf 100644
--- a/drivers/message/i2o/driver.c
+++ b/drivers/message/i2o/driver.c
@@ -84,7 +84,8 @@ int i2o_driver_register(struct i2o_driver *drv)
 	osm_debug("Register driver %s\n", drv->name);
 
 	if (drv->event) {
-		drv->event_queue = create_workqueue(drv->name);
+		drv->event_queue = alloc_workqueue(drv->name,
+						   WQ_MEM_RECLAIM, 1);
 		if (!drv->event_queue) {
 			osm_err("Could not initialize event queue for driver "
 				"%s\n", drv->name);
diff --git a/drivers/message/i2o/i2o_config.c b/drivers/message/i2o/i2o_config.c
index 7d3cc575c361..098de2b35784 100644
--- a/drivers/message/i2o/i2o_config.c
+++ b/drivers/message/i2o/i2o_config.c
@@ -1044,8 +1044,7 @@ static long i2o_cfg_ioctl(struct file *fp, unsigned int cmd, unsigned long arg)
 
 static int cfg_open(struct inode *inode, struct file *file)
 {
-	struct i2o_cfg_info *tmp =
-	    (struct i2o_cfg_info *)kmalloc(sizeof(struct i2o_cfg_info),
+	struct i2o_cfg_info *tmp = kmalloc(sizeof(struct i2o_cfg_info),
 					   GFP_KERNEL);
 	unsigned long flags;
 
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index fd018366d670..b94ab697638a 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -167,6 +167,16 @@ config TWL4030_CORE
 	  high speed USB OTG transceiver, an audio codec (on most
 	  versions) and many other features.
 
+config TWL4030_MADC
+	tristate "Texas Instruments TWL4030 MADC"
+	depends on TWL4030_CORE
+	help
+	This driver provides support for triton TWL4030-MADC. The
+	driver supports both RT and SW conversion methods.
+
+	This driver can be built as a module. If so it will be
+	named twl4030-madc
+
 config TWL4030_POWER
 	bool "Support power resources on TWL4030 family chips"
 	depends on TWL4030_CORE && ARM
@@ -523,6 +533,13 @@ config AB8500_DEBUG
          Select this option if you want debug information using the debug
          filesystem, debugfs.
 
+config AB8500_GPADC
+	bool "AB8500 GPADC driver"
+	depends on AB8500_CORE && REGULATOR_AB8500
+	default y
+	help
+	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage
+
 config AB3550_CORE
         bool "ST-Ericsson AB3550 Mixed Signal Circuit core functions"
 	select MFD_CORE
@@ -615,7 +632,7 @@ config MFD_VX855
 	  and/or vx855_gpio drivers for this to do anything useful.
 
 config MFD_WL1273_CORE
-	tristate
+	tristate "Support for TI WL1273 FM radio."
 	depends on I2C
 	select MFD_CORE
 	default n
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index a54e2c7c6a1c..7af2db4a4bff 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -37,6 +37,7 @@ obj-$(CONFIG_TPS6507X)		+= tps6507x.o
 obj-$(CONFIG_MENELAUS)		+= menelaus.o
 
 obj-$(CONFIG_TWL4030_CORE)	+= twl-core.o twl4030-irq.o twl6030-irq.o
+obj-$(CONFIG_TWL4030_MADC)      += twl4030-madc.o
 obj-$(CONFIG_TWL4030_POWER)    += twl4030-power.o
 obj-$(CONFIG_TWL4030_CODEC)	+= twl4030-codec.o
 obj-$(CONFIG_TWL6030_PWM)	+= twl6030-pwm.o
@@ -70,9 +71,10 @@ obj-$(CONFIG_ABX500_CORE)	+= abx500-core.o
 obj-$(CONFIG_AB3100_CORE)	+= ab3100-core.o
 obj-$(CONFIG_AB3100_OTP)	+= ab3100-otp.o
 obj-$(CONFIG_AB3550_CORE)	+= ab3550-core.o
-obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o
+obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_AB8500_I2C_CORE)	+= ab8500-i2c.o
 obj-$(CONFIG_AB8500_DEBUG)	+= ab8500-debugfs.o
+obj-$(CONFIG_AB8500_GPADC)	+= ab8500-gpadc.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)	+= adp5520.o
 obj-$(CONFIG_LPC_SCH)		+= lpc_sch.o
diff --git a/drivers/mfd/ab3100-core.c b/drivers/mfd/ab3100-core.c
index 4193af5f2743..a751927047ac 100644
--- a/drivers/mfd/ab3100-core.c
+++ b/drivers/mfd/ab3100-core.c
@@ -613,7 +613,7 @@ static void ab3100_setup_debugfs(struct ab3100 *ab3100)
 	ab3100_get_priv.ab3100 = ab3100;
 	ab3100_get_priv.mode = false;
 	ab3100_get_reg_file = debugfs_create_file("get_reg",
-				S_IWUGO, ab3100_dir, &ab3100_get_priv,
+				S_IWUSR, ab3100_dir, &ab3100_get_priv,
 				&ab3100_get_set_reg_fops);
 	if (!ab3100_get_reg_file) {
 		err = -ENOMEM;
@@ -623,7 +623,7 @@ static void ab3100_setup_debugfs(struct ab3100 *ab3100)
 	ab3100_set_priv.ab3100 = ab3100;
 	ab3100_set_priv.mode = true;
 	ab3100_set_reg_file = debugfs_create_file("set_reg",
-				S_IWUGO, ab3100_dir, &ab3100_set_priv,
+				S_IWUSR, ab3100_dir, &ab3100_set_priv,
 				&ab3100_get_set_reg_fops);
 	if (!ab3100_set_reg_file) {
 		err = -ENOMEM;
@@ -949,10 +949,8 @@ static int __devinit ab3100_probe(struct i2c_client *client,
 		goto exit_no_ops;
 
 	/* Set up and register the platform devices. */
-	for (i = 0; i < ARRAY_SIZE(ab3100_devs); i++) {
-		ab3100_devs[i].platform_data = ab3100_plf_data;
-		ab3100_devs[i].data_size = sizeof(struct ab3100_platform_data);
-	}
+	for (i = 0; i < ARRAY_SIZE(ab3100_devs); i++)
+		ab3100_devs[i].mfd_data = ab3100_plf_data;
 
 	err = mfd_add_devices(&client->dev, 0, ab3100_devs,
 		ARRAY_SIZE(ab3100_devs), NULL, 0);
diff --git a/drivers/mfd/ab3550-core.c b/drivers/mfd/ab3550-core.c
index 5fbca346b998..c12d04285226 100644
--- a/drivers/mfd/ab3550-core.c
+++ b/drivers/mfd/ab3550-core.c
@@ -1053,17 +1053,17 @@ static inline void ab3550_setup_debugfs(struct ab3550 *ab)
 		goto exit_destroy_dir;
 
 	ab3550_bank_file = debugfs_create_file("register-bank",
-		(S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_bank_fops);
+		(S_IRUGO | S_IWUSR), ab3550_dir, ab, &ab3550_bank_fops);
 	if (!ab3550_bank_file)
 		goto exit_destroy_reg;
 
 	ab3550_address_file = debugfs_create_file("register-address",
-		(S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_address_fops);
+		(S_IRUGO | S_IWUSR), ab3550_dir, ab, &ab3550_address_fops);
 	if (!ab3550_address_file)
 		goto exit_destroy_bank;
 
 	ab3550_val_file = debugfs_create_file("register-value",
-		(S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_val_fops);
+		(S_IRUGO | S_IWUSR), ab3550_dir, ab, &ab3550_val_fops);
 	if (!ab3550_val_file)
 		goto exit_destroy_address;
 
@@ -1320,10 +1320,8 @@ static int __init ab3550_probe(struct i2c_client *client,
 		goto exit_no_ops;
 
 	/* Set up and register the platform devices. */
-	for (i = 0; i < AB3550_NUM_DEVICES; i++) {
-		ab3550_devs[i].platform_data = ab3550_plf_data->dev_data[i];
-		ab3550_devs[i].data_size = ab3550_plf_data->dev_data_sz[i];
-	}
+	for (i = 0; i < AB3550_NUM_DEVICES; i++)
+		ab3550_devs[i].mfd_data = ab3550_plf_data->dev_data[i];
 
 	err = mfd_add_devices(&client->dev, 0, ab3550_devs,
 		ARRAY_SIZE(ab3550_devs), NULL,
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index b6887014d687..6e185b272d00 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -4,7 +4,7 @@
  * License Terms: GNU General Public License v2
  * Author: Srinidhi Kasagar <srinidhi.kasagar@stericsson.com>
  * Author: Rabin Vincent <rabin.vincent@stericsson.com>
- * Changes: Mattias Wallin <mattias.wallin@stericsson.com>
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com>
  */
 
 #include <linux/kernel.h>
@@ -90,6 +90,7 @@
 #define AB8500_IT_MASK24_REG		0x57
 
 #define AB8500_REV_REG			0x80
+#define AB8500_SWITCH_OFF_STATUS	0x00
 
 /*
  * Map interrupt numbers to the LATCH and MASK register offsets, Interrupt
@@ -652,10 +653,38 @@ static ssize_t show_chip_id(struct device *dev,
 	return sprintf(buf, "%#x\n", ab8500 ? ab8500->chip_id : -EINVAL);
 }
 
+/*
+ * ab8500 has switched off due to (SWITCH_OFF_STATUS):
+ * 0x01 Swoff bit programming
+ * 0x02 Thermal protection activation
+ * 0x04 Vbat lower then BattOk falling threshold
+ * 0x08 Watchdog expired
+ * 0x10 Non presence of 32kHz clock
+ * 0x20 Battery level lower than power on reset threshold
+ * 0x40 Power on key 1 pressed longer than 10 seconds
+ * 0x80 DB8500 thermal shutdown
+ */
+static ssize_t show_switch_off_status(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int ret;
+	u8 value;
+	struct ab8500 *ab8500;
+
+	ab8500 = dev_get_drvdata(dev);
+	ret = get_register_interruptible(ab8500, AB8500_RTC,
+		AB8500_SWITCH_OFF_STATUS, &value);
+	if (ret < 0)
+		return ret;
+	return sprintf(buf, "%#x\n", value);
+}
+
 static DEVICE_ATTR(chip_id, S_IRUGO, show_chip_id, NULL);
+static DEVICE_ATTR(switch_off_status, S_IRUGO, show_switch_off_status, NULL);
 
 static struct attribute *ab8500_sysfs_entries[] = {
 	&dev_attr_chip_id.attr,
+	&dev_attr_switch_off_status.attr,
 	NULL,
 };
 
@@ -686,9 +715,10 @@ int __devinit ab8500_init(struct ab8500 *ab8500)
 	 * 0x10 - Cut 1.0
 	 * 0x11 - Cut 1.1
 	 * 0x20 - Cut 2.0
+	 * 0x30 - Cut 3.0
 	 */
-	if (value == 0x0 || value == 0x10 || value == 0x11 || value == 0x20) {
-		ab8500->revision = value;
+	if (value == 0x0 || value == 0x10 || value == 0x11 || value == 0x20 ||
+		value == 0x30) {
 		dev_info(ab8500->dev, "detected chip, revision: %#x\n", value);
 	} else {
 		dev_err(ab8500->dev, "unknown chip, revision: %#x\n", value);
@@ -696,6 +726,24 @@ int __devinit ab8500_init(struct ab8500 *ab8500)
 	}
 	ab8500->chip_id = value;
 
+	/*
+	 * ab8500 has switched off due to (SWITCH_OFF_STATUS):
+	 * 0x01 Swoff bit programming
+	 * 0x02 Thermal protection activation
+	 * 0x04 Vbat lower then BattOk falling threshold
+	 * 0x08 Watchdog expired
+	 * 0x10 Non presence of 32kHz clock
+	 * 0x20 Battery level lower than power on reset threshold
+	 * 0x40 Power on key 1 pressed longer than 10 seconds
+	 * 0x80 DB8500 thermal shutdown
+	 */
+
+	ret = get_register_interruptible(ab8500, AB8500_RTC,
+		AB8500_SWITCH_OFF_STATUS, &value);
+	if (ret < 0)
+		return ret;
+	dev_info(ab8500->dev, "switch off status: %#x", value);
+
 	if (plat && plat->init)
 		plat->init(ab8500);
 
@@ -764,6 +812,6 @@ int __devexit ab8500_exit(struct ab8500 *ab8500)
 	return 0;
 }
 
-MODULE_AUTHOR("Srinidhi Kasagar, Rabin Vincent");
+MODULE_AUTHOR("Mattias Wallin, Srinidhi Kasagar, Rabin Vincent");
 MODULE_DESCRIPTION("AB8500 MFD core");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 3c1541ae7223..64748e42ac03 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -585,18 +585,18 @@ static int __devinit ab8500_debug_probe(struct platform_device *plf)
 		goto exit_destroy_dir;
 
 	ab8500_bank_file = debugfs_create_file("register-bank",
-		(S_IRUGO | S_IWUGO), ab8500_dir, &plf->dev, &ab8500_bank_fops);
+		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_bank_fops);
 	if (!ab8500_bank_file)
 		goto exit_destroy_reg;
 
 	ab8500_address_file = debugfs_create_file("register-address",
-		(S_IRUGO | S_IWUGO), ab8500_dir, &plf->dev,
+		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev,
 		&ab8500_address_fops);
 	if (!ab8500_address_file)
 		goto exit_destroy_bank;
 
 	ab8500_val_file = debugfs_create_file("register-value",
-		(S_IRUGO | S_IWUGO), ab8500_dir, &plf->dev, &ab8500_val_fops);
+		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_val_fops);
 	if (!ab8500_val_file)
 		goto exit_destroy_address;
 
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
new file mode 100644
index 000000000000..19339bc439ca
--- /dev/null
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -0,0 +1,296 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500-gpadc.h>
+
+/*
+ * GPADC register offsets
+ * Bank : 0x0A
+ */
+#define AB8500_GPADC_CTRL1_REG		0x00
+#define AB8500_GPADC_CTRL2_REG		0x01
+#define AB8500_GPADC_CTRL3_REG		0x02
+#define AB8500_GPADC_AUTO_TIMER_REG	0x03
+#define AB8500_GPADC_STAT_REG		0x04
+#define AB8500_GPADC_MANDATAL_REG	0x05
+#define AB8500_GPADC_MANDATAH_REG	0x06
+#define AB8500_GPADC_AUTODATAL_REG	0x07
+#define AB8500_GPADC_AUTODATAH_REG	0x08
+#define AB8500_GPADC_MUX_CTRL_REG	0x09
+
+/* gpadc constants */
+#define EN_VINTCORE12			0x04
+#define EN_VTVOUT			0x02
+#define EN_GPADC			0x01
+#define DIS_GPADC			0x00
+#define SW_AVG_16			0x60
+#define ADC_SW_CONV			0x04
+#define EN_BUF				0x40
+#define DIS_ZERO			0x00
+#define GPADC_BUSY			0x01
+
+/**
+ * struct ab8500_gpadc - ab8500 GPADC device information
+ * @dev:			pointer to the struct device
+ * @parent:			pointer to the parent device structure ab8500
+ * @ab8500_gpadc_complete:	pointer to the struct completion, to indicate
+ *				the completion of gpadc conversion
+ * @ab8500_gpadc_lock:		structure of type mutex
+ * @regu:			pointer to the struct regulator
+ * @irq:			interrupt number that is used by gpadc
+ */
+static struct ab8500_gpadc {
+	struct device *dev;
+	struct ab8500 *parent;
+	struct completion ab8500_gpadc_complete;
+	struct mutex ab8500_gpadc_lock;
+	struct regulator *regu;
+	int irq;
+} *di;
+
+/**
+ * ab8500_gpadc_convert() - gpadc conversion
+ * @input:	analog input to be converted to digital data
+ *
+ * This function converts the selected analog i/p to digital
+ * data. Thereafter calibration has to be made to obtain the
+ * data in the required quantity measurement.
+ */
+int ab8500_gpadc_convert(u8 input)
+{
+	int ret;
+	u16 data = 0;
+	int looplimit = 0;
+	u8 val, low_data, high_data;
+
+	if (!di)
+		return -ENODEV;
+
+	mutex_lock(&di->ab8500_gpadc_lock);
+	/* Enable VTVout LDO this is required for GPADC */
+	regulator_enable(di->regu);
+
+	/* Check if ADC is not busy, lock and proceed */
+	do {
+		ret = abx500_get_register_interruptible(di->dev, AB8500_GPADC,
+			AB8500_GPADC_STAT_REG, &val);
+		if (ret < 0)
+			goto out;
+		if (!(val & GPADC_BUSY))
+			break;
+		msleep(10);
+	} while (++looplimit < 10);
+	if (looplimit >= 10 && (val & GPADC_BUSY)) {
+		dev_err(di->dev, "gpadc_conversion: GPADC busy");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Enable GPADC */
+	ret = abx500_mask_and_set_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_CTRL1_REG, EN_GPADC, EN_GPADC);
+	if (ret < 0) {
+		dev_err(di->dev, "gpadc_conversion: enable gpadc failed\n");
+		goto out;
+	}
+	/* Select the input source and set average samples to 16 */
+	ret = abx500_set_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_CTRL2_REG, (input | SW_AVG_16));
+	if (ret < 0) {
+		dev_err(di->dev,
+			"gpadc_conversion: set avg samples failed\n");
+		goto out;
+	}
+	/* Enable ADC, Buffering and select rising edge, start Conversion */
+	ret = abx500_mask_and_set_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_CTRL1_REG, EN_BUF, EN_BUF);
+	if (ret < 0) {
+		dev_err(di->dev,
+			"gpadc_conversion: select falling edge failed\n");
+		goto out;
+	}
+	ret = abx500_mask_and_set_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_CTRL1_REG, ADC_SW_CONV, ADC_SW_CONV);
+	if (ret < 0) {
+		dev_err(di->dev,
+			"gpadc_conversion: start s/w conversion failed\n");
+		goto out;
+	}
+	/* wait for completion of conversion */
+	if (!wait_for_completion_timeout(&di->ab8500_gpadc_complete, 2*HZ)) {
+		dev_err(di->dev,
+			"timeout: didnt recieve GPADC conversion interrupt\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Read the converted RAW data */
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_MANDATAL_REG, &low_data);
+	if (ret < 0) {
+		dev_err(di->dev, "gpadc_conversion: read low data failed\n");
+		goto out;
+	}
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_MANDATAH_REG, &high_data);
+	if (ret < 0) {
+		dev_err(di->dev, "gpadc_conversion: read high data failed\n");
+		goto out;
+	}
+
+	data = (high_data << 8) | low_data;
+	/* Disable GPADC */
+	ret = abx500_set_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_CTRL1_REG, DIS_GPADC);
+	if (ret < 0) {
+		dev_err(di->dev, "gpadc_conversion: disable gpadc failed\n");
+		goto out;
+	}
+	/* Disable VTVout LDO this is required for GPADC */
+	regulator_disable(di->regu);
+	mutex_unlock(&di->ab8500_gpadc_lock);
+	return data;
+
+out:
+	/*
+	 * It has shown to be needed to turn off the GPADC if an error occurs,
+	 * otherwise we might have problem when waiting for the busy bit in the
+	 * GPADC status register to go low. In V1.1 there wait_for_completion
+	 * seems to timeout when waiting for an interrupt.. Not seen in V2.0
+	 */
+	(void) abx500_set_register_interruptible(di->dev, AB8500_GPADC,
+		AB8500_GPADC_CTRL1_REG, DIS_GPADC);
+	regulator_disable(di->regu);
+	mutex_unlock(&di->ab8500_gpadc_lock);
+	dev_err(di->dev, "gpadc_conversion: Failed to AD convert channel %d\n",
+		input);
+	return ret;
+}
+EXPORT_SYMBOL(ab8500_gpadc_convert);
+
+/**
+ * ab8500_bm_gpswadcconvend_handler() - isr for s/w gpadc conversion completion
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for s/w gpadc conversion completion.
+ * Notifies the gpadc completion is completed and the converted raw value
+ * can be read from the registers.
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_bm_gpswadcconvend_handler(int irq, void *_di)
+{
+	struct ab8500_gpadc *gpadc = _di;
+
+	complete(&gpadc->ab8500_gpadc_complete);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = kzalloc(sizeof(struct ab8500_gpadc), GFP_KERNEL);
+	if (!gpadc) {
+		dev_err(&pdev->dev, "Error: No memory\n");
+		return -ENOMEM;
+	}
+
+	gpadc->parent = dev_get_drvdata(pdev->dev.parent);
+	gpadc->irq = platform_get_irq_byname(pdev, "SW_CONV_END");
+	if (gpadc->irq < 0) {
+		dev_err(gpadc->dev, "failed to get platform irq-%d\n", di->irq);
+		ret = gpadc->irq;
+		goto fail;
+	}
+
+	gpadc->dev = &pdev->dev;
+	mutex_init(&di->ab8500_gpadc_lock);
+
+	/* Initialize completion used to notify completion of conversion */
+	init_completion(&gpadc->ab8500_gpadc_complete);
+
+	/* Register interrupt  - SwAdcComplete */
+	ret = request_threaded_irq(gpadc->irq, NULL,
+		ab8500_bm_gpswadcconvend_handler,
+		IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc", gpadc);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "Failed to register interrupt, irq: %d\n",
+			gpadc->irq);
+		goto fail;
+	}
+
+	/* VTVout LDO used to power up ab8500-GPADC */
+	gpadc->regu = regulator_get(&pdev->dev, "vddadc");
+	if (IS_ERR(gpadc->regu)) {
+		ret = PTR_ERR(gpadc->regu);
+		dev_err(gpadc->dev, "failed to get vtvout LDO\n");
+		goto fail;
+	}
+	di = gpadc;
+	dev_dbg(gpadc->dev, "probe success\n");
+	return 0;
+fail:
+	kfree(gpadc);
+	gpadc = NULL;
+	return ret;
+}
+
+static int __devexit ab8500_gpadc_remove(struct platform_device *pdev)
+{
+	struct ab8500_gpadc *gpadc = platform_get_drvdata(pdev);
+
+	/* remove interrupt  - completion of Sw ADC conversion */
+	free_irq(gpadc->irq, di);
+	/* disable VTVout LDO that is being used by GPADC */
+	regulator_put(gpadc->regu);
+	kfree(gpadc);
+	gpadc = NULL;
+	return 0;
+}
+
+static struct platform_driver ab8500_gpadc_driver = {
+	.probe = ab8500_gpadc_probe,
+	.remove = __devexit_p(ab8500_gpadc_remove),
+	.driver = {
+		.name = "ab8500-gpadc",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab8500_gpadc_init(void)
+{
+	return platform_driver_register(&ab8500_gpadc_driver);
+}
+
+static void __exit ab8500_gpadc_exit(void)
+{
+	platform_driver_unregister(&ab8500_gpadc_driver);
+}
+
+subsys_initcall_sync(ab8500_gpadc_init);
+module_exit(ab8500_gpadc_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Arun R Murthy");
+MODULE_ALIAS("platform:ab8500_gpadc");
+MODULE_DESCRIPTION("AB8500 GPADC driver");
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
new file mode 100644
index 000000000000..392185965b39
--- /dev/null
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com> for ST Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/sysctrl.h>
+
+static struct device *sysctrl_dev;
+
+static inline bool valid_bank(u8 bank)
+{
+	return ((bank == AB8500_SYS_CTRL1_BLOCK) ||
+		(bank == AB8500_SYS_CTRL2_BLOCK));
+}
+
+int ab8500_sysctrl_read(u16 reg, u8 *value)
+{
+	u8 bank;
+
+	if (sysctrl_dev == NULL)
+		return -EAGAIN;
+
+	bank = (reg >> 8);
+	if (!valid_bank(bank))
+		return -EINVAL;
+
+	return abx500_get_register_interruptible(sysctrl_dev, bank,
+		(u8)(reg & 0xFF), value);
+}
+
+int ab8500_sysctrl_write(u16 reg, u8 mask, u8 value)
+{
+	u8 bank;
+
+	if (sysctrl_dev == NULL)
+		return -EAGAIN;
+
+	bank = (reg >> 8);
+	if (!valid_bank(bank))
+		return -EINVAL;
+
+	return abx500_mask_and_set_register_interruptible(sysctrl_dev, bank,
+		(u8)(reg & 0xFF), mask, value);
+}
+
+static int __devinit ab8500_sysctrl_probe(struct platform_device *pdev)
+{
+	sysctrl_dev = &pdev->dev;
+	return 0;
+}
+
+static int __devexit ab8500_sysctrl_remove(struct platform_device *pdev)
+{
+	sysctrl_dev = NULL;
+	return 0;
+}
+
+static struct platform_driver ab8500_sysctrl_driver = {
+	.driver = {
+		.name = "ab8500-sysctrl",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab8500_sysctrl_probe,
+	.remove = __devexit_p(ab8500_sysctrl_remove),
+};
+
+static int __init ab8500_sysctrl_init(void)
+{
+	return platform_driver_register(&ab8500_sysctrl_driver);
+}
+subsys_initcall(ab8500_sysctrl_init);
+
+MODULE_AUTHOR("Mattias Nilsson <mattias.i.nilsson@stericsson.com");
+MODULE_DESCRIPTION("AB8500 system control driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/adp5520.c b/drivers/mfd/adp5520.c
index 3122139b4300..f1d88483112c 100644
--- a/drivers/mfd/adp5520.c
+++ b/drivers/mfd/adp5520.c
@@ -321,27 +321,27 @@ static int __devexit adp5520_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
-static int adp5520_suspend(struct i2c_client *client,
-				 pm_message_t state)
+static int adp5520_suspend(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct adp5520_chip *chip = dev_get_drvdata(&client->dev);
 
 	adp5520_clr_bits(chip->dev, ADP5520_MODE_STATUS, ADP5520_nSTNBY);
 	return 0;
 }
 
-static int adp5520_resume(struct i2c_client *client)
+static int adp5520_resume(struct device *dev)
 {
+	struct i2c_client *client = to_i2c_client(dev);
 	struct adp5520_chip *chip = dev_get_drvdata(&client->dev);
 
 	adp5520_set_bits(chip->dev, ADP5520_MODE_STATUS, ADP5520_nSTNBY);
 	return 0;
 }
-#else
-#define adp5520_suspend	NULL
-#define adp5520_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adp5520_pm, adp5520_suspend, adp5520_resume);
+
 static const struct i2c_device_id adp5520_id[] = {
 	{ "pmic-adp5520", ID_ADP5520 },
 	{ "pmic-adp5501", ID_ADP5501 },
@@ -353,11 +353,10 @@ static struct i2c_driver adp5520_driver = {
 	.driver = {
 		.name	= "adp5520",
 		.owner	= THIS_MODULE,
+		.pm	= &adp5520_pm,
 	},
 	.probe		= adp5520_probe,
 	.remove		= __devexit_p(adp5520_remove),
-	.suspend	= adp5520_suspend,
-	.resume		= adp5520_resume,
 	.id_table 	= adp5520_id,
 };
 
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index 6a1f94042612..0241f08fc00d 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -143,9 +143,9 @@ static void asic3_irq_demux(unsigned int irq, struct irq_desc *desc)
 	unsigned long flags;
 	struct asic3 *asic;
 
-	desc->chip->ack(irq);
+	desc->irq_data.chip->irq_ack(&desc->irq_data);
 
-	asic = desc->handler_data;
+	asic = get_irq_data(irq);
 
 	for (iter = 0 ; iter < MAX_ASIC_ISR_LOOPS; iter++) {
 		u32 status;
@@ -682,7 +682,7 @@ static struct mfd_cell asic3_cell_ds1wm = {
 	.name          = "ds1wm",
 	.enable        = ds1wm_enable,
 	.disable       = ds1wm_disable,
-	.driver_data   = &ds1wm_pdata,
+	.mfd_data      = &ds1wm_pdata,
 	.num_resources = ARRAY_SIZE(ds1wm_resources),
 	.resources     = ds1wm_resources,
 };
@@ -783,7 +783,7 @@ static struct mfd_cell asic3_cell_mmc = {
 	.name          = "tmio-mmc",
 	.enable        = asic3_mmc_enable,
 	.disable       = asic3_mmc_disable,
-	.driver_data   = &asic3_mmc_data,
+	.mfd_data      = &asic3_mmc_data,
 	.num_resources = ARRAY_SIZE(asic3_mmc_resources),
 	.resources     = asic3_mmc_resources,
 };
@@ -810,9 +810,6 @@ static int __init asic3_mfd_probe(struct platform_device *pdev,
 	ds1wm_resources[0].start >>= asic->bus_shift;
 	ds1wm_resources[0].end   >>= asic->bus_shift;
 
-	asic3_cell_ds1wm.platform_data = &asic3_cell_ds1wm;
-	asic3_cell_ds1wm.data_size = sizeof(asic3_cell_ds1wm);
-
 	/* MMC */
 	asic->tmio_cnf = ioremap((ASIC3_SD_CONFIG_BASE >> asic->bus_shift) +
 				 mem_sdio->start, 0x400 >> asic->bus_shift);
@@ -824,9 +821,6 @@ static int __init asic3_mfd_probe(struct platform_device *pdev,
 	asic3_mmc_resources[0].start >>= asic->bus_shift;
 	asic3_mmc_resources[0].end   >>= asic->bus_shift;
 
-	asic3_cell_mmc.platform_data = &asic3_cell_mmc;
-	asic3_cell_mmc.data_size = sizeof(asic3_cell_mmc);
-
 	ret = mfd_add_devices(&pdev->dev, pdev->id,
 			&asic3_cell_ds1wm, 1, mem, asic->irq_base);
 	if (ret < 0)
diff --git a/drivers/mfd/cs5535-mfd.c b/drivers/mfd/cs5535-mfd.c
index 59ca6f151e78..886a06871065 100644
--- a/drivers/mfd/cs5535-mfd.c
+++ b/drivers/mfd/cs5535-mfd.c
@@ -39,6 +39,37 @@ enum cs5535_mfd_bars {
 	NR_BARS,
 };
 
+static int cs5535_mfd_res_enable(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");
+		return -EIO;
+	}
+
+	if (!request_region(res->start, resource_size(res), DRV_NAME)) {
+		dev_err(&pdev->dev, "can't request region\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+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");
+		return -EIO;
+	}
+
+	release_region(res->start, resource_size(res));
+	return 0;
+}
+
 static __devinitdata struct resource cs5535_mfd_resources[NR_BARS];
 
 static __devinitdata struct mfd_cell cs5535_mfd_cells[] = {
@@ -65,12 +96,18 @@ static __devinitdata struct mfd_cell cs5535_mfd_cells[] = {
 		.name = "cs5535-pms",
 		.num_resources = 1,
 		.resources = &cs5535_mfd_resources[PMS_BAR],
+
+		.enable = cs5535_mfd_res_enable,
+		.disable = cs5535_mfd_res_disable,
 	},
 	{
 		.id = ACPI_BAR,
 		.name = "cs5535-acpi",
 		.num_resources = 1,
 		.resources = &cs5535_mfd_resources[ACPI_BAR],
+
+		.enable = cs5535_mfd_res_enable,
+		.disable = cs5535_mfd_res_disable,
 	},
 };
 
diff --git a/drivers/mfd/davinci_voicecodec.c b/drivers/mfd/davinci_voicecodec.c
index 33c923d215c7..414783b04849 100644
--- a/drivers/mfd/davinci_voicecodec.c
+++ b/drivers/mfd/davinci_voicecodec.c
@@ -118,13 +118,13 @@ static int __init davinci_vc_probe(struct platform_device *pdev)
 
 	/* Voice codec interface client */
 	cell = &davinci_vc->cells[DAVINCI_VC_VCIF_CELL];
-	cell->name = "davinci_vcif";
-	cell->driver_data = davinci_vc;
+	cell->name = "davinci-vcif";
+	cell->mfd_data = davinci_vc;
 
 	/* Voice codec CQ93VC client */
 	cell = &davinci_vc->cells[DAVINCI_VC_CQ93VC_CELL];
-	cell->name = "cq93vc";
-	cell->driver_data = davinci_vc;
+	cell->name = "cq93vc-codec";
+	cell->mfd_data = davinci_vc;
 
 	ret = mfd_add_devices(&pdev->dev, pdev->id, davinci_vc->cells,
 			      DAVINCI_VC_CELLS, NULL, 0);
diff --git a/drivers/mfd/htc-pasic3.c b/drivers/mfd/htc-pasic3.c
index 7bc752272dc1..fb9770b39a32 100644
--- a/drivers/mfd/htc-pasic3.c
+++ b/drivers/mfd/htc-pasic3.c
@@ -117,7 +117,7 @@ static struct mfd_cell ds1wm_cell __initdata = {
 	.name          = "ds1wm",
 	.enable        = ds1wm_enable,
 	.disable       = ds1wm_disable,
-	.driver_data   = &ds1wm_pdata,
+	.mfd_data      = &ds1wm_pdata,
 	.num_resources = 2,
 	.resources     = ds1wm_resources,
 };
@@ -165,8 +165,6 @@ static int __init pasic3_probe(struct platform_device *pdev)
 		ds1wm_pdata.clock_rate = pdata->clock_rate;
 		/* the first 5 PASIC3 registers control the DS1WM */
 		ds1wm_resources[0].end = (5 << asic->bus_shift) - 1;
-		ds1wm_cell.platform_data = &ds1wm_cell;
-		ds1wm_cell.data_size = sizeof(ds1wm_cell);
 		ret = mfd_add_devices(&pdev->dev, pdev->id,
 				&ds1wm_cell, 1, r, irq);
 		if (ret < 0)
@@ -174,9 +172,6 @@ static int __init pasic3_probe(struct platform_device *pdev)
 	}
 
 	if (pdata && pdata->led_pdata) {
-		led_cell.driver_data = pdata->led_pdata;
-		led_cell.platform_data = &led_cell;
-		led_cell.data_size = sizeof(ds1wm_cell);
 		ret = mfd_add_devices(&pdev->dev, pdev->id, &led_cell, 1, r, 0);
 		if (ret < 0)
 			dev_warn(dev, "failed to register LED device\n");
diff --git a/drivers/mfd/janz-cmodio.c b/drivers/mfd/janz-cmodio.c
index 36a166bcdb08..fc4191137e90 100644
--- a/drivers/mfd/janz-cmodio.c
+++ b/drivers/mfd/janz-cmodio.c
@@ -86,8 +86,7 @@ static int __devinit cmodio_setup_subdevice(struct cmodio_device *priv,
 
 	/* Add platform data */
 	pdata->modno = modno;
-	cell->platform_data = pdata;
-	cell->data_size = sizeof(*pdata);
+	cell->mfd_data = pdata;
 
 	/* MODULbus registers -- PCI BAR3 is big-endian MODULbus access */
 	res->flags = IORESOURCE_MEM;
diff --git a/drivers/mfd/jz4740-adc.c b/drivers/mfd/jz4740-adc.c
index 0cc59795f600..aa518b9beaf5 100644
--- a/drivers/mfd/jz4740-adc.c
+++ b/drivers/mfd/jz4740-adc.c
@@ -232,8 +232,6 @@ const struct mfd_cell jz4740_adc_cells[] = {
 		.name = "jz4740-hwmon",
 		.num_resources = ARRAY_SIZE(jz4740_hwmon_resources),
 		.resources = jz4740_hwmon_resources,
-		.platform_data = (void *)&jz4740_adc_cells[0],
-		.data_size = sizeof(struct mfd_cell),
 
 		.enable = jz4740_adc_cell_enable,
 		.disable = jz4740_adc_cell_disable,
@@ -243,8 +241,6 @@ const struct mfd_cell jz4740_adc_cells[] = {
 		.name = "jz4740-battery",
 		.num_resources = ARRAY_SIZE(jz4740_battery_resources),
 		.resources = jz4740_battery_resources,
-		.platform_data = (void *)&jz4740_adc_cells[1],
-		.data_size = sizeof(struct mfd_cell),
 
 		.enable = jz4740_adc_cell_enable,
 		.disable = jz4740_adc_cell_disable,
diff --git a/drivers/mfd/max8998.c b/drivers/mfd/max8998.c
index bbfe86732602..c00214257da2 100644
--- a/drivers/mfd/max8998.c
+++ b/drivers/mfd/max8998.c
@@ -233,7 +233,7 @@ struct max8998_reg_dump {
 	u8	val;
 };
 #define SAVE_ITEM(x)	{ .addr = (x), .val = 0x0, }
-struct max8998_reg_dump max8998_dump[] = {
+static struct max8998_reg_dump max8998_dump[] = {
 	SAVE_ITEM(MAX8998_REG_IRQM1),
 	SAVE_ITEM(MAX8998_REG_IRQM2),
 	SAVE_ITEM(MAX8998_REG_IRQM3),
@@ -298,7 +298,7 @@ static int max8998_restore(struct device *dev)
 	return 0;
 }
 
-const struct dev_pm_ops max8998_pm = {
+static const struct dev_pm_ops max8998_pm = {
 	.suspend = max8998_suspend,
 	.resume = max8998_resume,
 	.freeze = max8998_freeze,
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index b9fcaf0004da..668634e89e81 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -683,14 +683,13 @@ out:
 EXPORT_SYMBOL_GPL(mc13783_adc_do_conversion);
 
 static int mc13xxx_add_subdevice_pdata(struct mc13xxx *mc13xxx,
-		const char *format, void *pdata, size_t pdata_size)
+		const char *format, void *pdata)
 {
 	char buf[30];
 	const char *name = mc13xxx_get_chipname(mc13xxx);
 
 	struct mfd_cell cell = {
-		.platform_data = pdata,
-		.data_size = pdata_size,
+		.mfd_data = pdata,
 	};
 
 	/* there is no asnprintf in the kernel :-( */
@@ -706,7 +705,7 @@ static int mc13xxx_add_subdevice_pdata(struct mc13xxx *mc13xxx,
 
 static int mc13xxx_add_subdevice(struct mc13xxx *mc13xxx, const char *format)
 {
-	return mc13xxx_add_subdevice_pdata(mc13xxx, format, NULL, 0);
+	return mc13xxx_add_subdevice_pdata(mc13xxx, format, NULL);
 }
 
 static int mc13xxx_probe(struct spi_device *spi)
@@ -764,13 +763,8 @@ err_revision:
 		mc13xxx_add_subdevice(mc13xxx, "%s-codec");
 
 	if (pdata->flags & MC13XXX_USE_REGULATOR) {
-		struct mc13xxx_regulator_platform_data regulator_pdata = {
-			.num_regulators = pdata->num_regulators,
-			.regulators = pdata->regulators,
-		};
-
 		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-regulator",
-				&regulator_pdata, sizeof(regulator_pdata));
+				&pdata->regulators);
 	}
 
 	if (pdata->flags & MC13XXX_USE_RTC)
@@ -779,10 +773,8 @@ err_revision:
 	if (pdata->flags & MC13XXX_USE_TOUCHSCREEN)
 		mc13xxx_add_subdevice(mc13xxx, "%s-ts");
 
-	if (pdata->flags & MC13XXX_USE_LED) {
-		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-led",
-					pdata->leds, sizeof(*pdata->leds));
-	}
+	if (pdata->flags & MC13XXX_USE_LED)
+		mc13xxx_add_subdevice_pdata(mc13xxx, "%s-led", pdata->leds);
 
 	return 0;
 }
@@ -811,6 +803,7 @@ static const struct spi_device_id mc13xxx_device_id[] = {
 		/* sentinel */
 	}
 };
+MODULE_DEVICE_TABLE(spi, mc13xxx_device_id);
 
 static struct spi_driver mc13xxx_driver = {
 	.id_table = mc13xxx_device_id,
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index d83ad0f141af..bb2264cc410b 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -18,6 +18,43 @@
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 
+int mfd_shared_cell_enable(struct platform_device *pdev)
+{
+	const struct mfd_cell *cell = mfd_get_cell(pdev);
+	int err = 0;
+
+	/* only call enable hook if the cell wasn't previously enabled */
+	if (atomic_inc_return(cell->usage_count) == 1)
+		err = cell->enable(pdev);
+
+	/* if the enable hook failed, decrement counter to allow retries */
+	if (err)
+		atomic_dec(cell->usage_count);
+
+	return err;
+}
+EXPORT_SYMBOL(mfd_shared_cell_enable);
+
+int mfd_shared_cell_disable(struct platform_device *pdev)
+{
+	const struct mfd_cell *cell = mfd_get_cell(pdev);
+	int err = 0;
+
+	/* only disable if no other clients are using it */
+	if (atomic_dec_return(cell->usage_count) == 0)
+		err = cell->disable(pdev);
+
+	/* if the disable hook failed, increment to allow retries */
+	if (err)
+		atomic_inc(cell->usage_count);
+
+	/* sanity check; did someone call disable too many times? */
+	WARN_ON(atomic_read(cell->usage_count) < 0);
+
+	return err;
+}
+EXPORT_SYMBOL(mfd_shared_cell_disable);
+
 static int mfd_add_device(struct device *parent, int id,
 			  const struct mfd_cell *cell,
 			  struct resource *mem_base,
@@ -37,14 +74,10 @@ static int mfd_add_device(struct device *parent, int id,
 		goto fail_device;
 
 	pdev->dev.parent = parent;
-	platform_set_drvdata(pdev, cell->driver_data);
 
-	if (cell->data_size) {
-		ret = platform_device_add_data(pdev,
-					cell->platform_data, cell->data_size);
-		if (ret)
-			goto fail_res;
-	}
+	ret = platform_device_add_data(pdev, cell, sizeof(*cell));
+	if (ret)
+		goto fail_res;
 
 	for (r = 0; r < cell->num_resources; r++) {
 		res[r].name = cell->resources[r].name;
@@ -100,14 +133,22 @@ fail_alloc:
 }
 
 int mfd_add_devices(struct device *parent, int id,
-		    const struct mfd_cell *cells, int n_devs,
+		    struct mfd_cell *cells, int n_devs,
 		    struct resource *mem_base,
 		    int irq_base)
 {
 	int i;
 	int ret = 0;
+	atomic_t *cnts;
+
+	/* initialize reference counting for all cells */
+	cnts = kcalloc(sizeof(*cnts), n_devs, GFP_KERNEL);
+	if (!cnts)
+		return -ENOMEM;
 
 	for (i = 0; i < n_devs; i++) {
+		atomic_set(&cnts[i], 0);
+		cells[i].usage_count = &cnts[i];
 		ret = mfd_add_device(parent, id, cells + i, mem_base, irq_base);
 		if (ret)
 			break;
@@ -120,17 +161,89 @@ int mfd_add_devices(struct device *parent, int id,
 }
 EXPORT_SYMBOL(mfd_add_devices);
 
-static int mfd_remove_devices_fn(struct device *dev, void *unused)
+static int mfd_remove_devices_fn(struct device *dev, void *c)
 {
-	platform_device_unregister(to_platform_device(dev));
+	struct platform_device *pdev = to_platform_device(dev);
+	const struct mfd_cell *cell = mfd_get_cell(pdev);
+	atomic_t **usage_count = c;
+
+	/* find the base address of usage_count pointers (for freeing) */
+	if (!*usage_count || (cell->usage_count < *usage_count))
+		*usage_count = cell->usage_count;
+
+	platform_device_unregister(pdev);
 	return 0;
 }
 
 void mfd_remove_devices(struct device *parent)
 {
-	device_for_each_child(parent, NULL, mfd_remove_devices_fn);
+	atomic_t *cnts = NULL;
+
+	device_for_each_child(parent, &cnts, mfd_remove_devices_fn);
+	kfree(cnts);
 }
 EXPORT_SYMBOL(mfd_remove_devices);
 
+static int add_shared_platform_device(const char *cell, const char *name)
+{
+	struct mfd_cell cell_entry;
+	struct device *dev;
+	struct platform_device *pdev;
+	int err;
+
+	/* check if we've already registered a device (don't fail if we have) */
+	if (bus_find_device_by_name(&platform_bus_type, NULL, name))
+		return 0;
+
+	/* fetch the parent cell's device (should already be registered!) */
+	dev = bus_find_device_by_name(&platform_bus_type, NULL, cell);
+	if (!dev) {
+		printk(KERN_ERR "failed to find device for cell %s\n", cell);
+		return -ENODEV;
+	}
+	pdev = to_platform_device(dev);
+	memcpy(&cell_entry, mfd_get_cell(pdev), sizeof(cell_entry));
+
+	WARN_ON(!cell_entry.enable);
+
+	cell_entry.name = name;
+	err = mfd_add_device(pdev->dev.parent, -1, &cell_entry, NULL, 0);
+	if (err)
+		dev_err(dev, "MFD add devices failed: %d\n", err);
+	return err;
+}
+
+int mfd_shared_platform_driver_register(struct platform_driver *drv,
+		const char *cellname)
+{
+	int err;
+
+	err = add_shared_platform_device(cellname, drv->driver.name);
+	if (err)
+		printk(KERN_ERR "failed to add platform device %s\n",
+				drv->driver.name);
+
+	err = platform_driver_register(drv);
+	if (err)
+		printk(KERN_ERR "failed to add platform driver %s\n",
+				drv->driver.name);
+
+	return err;
+}
+EXPORT_SYMBOL(mfd_shared_platform_driver_register);
+
+void mfd_shared_platform_driver_unregister(struct platform_driver *drv)
+{
+	struct device *dev;
+
+	dev = bus_find_device_by_name(&platform_bus_type, NULL,
+			drv->driver.name);
+	if (dev)
+		platform_device_unregister(to_platform_device(dev));
+
+	platform_driver_unregister(drv);
+}
+EXPORT_SYMBOL(mfd_shared_platform_driver_unregister);
+
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ian Molton, Dmitry Baryshkov");
diff --git a/drivers/mfd/pcf50633-core.c b/drivers/mfd/pcf50633-core.c
index 501ce13b693e..c1306ed43e3c 100644
--- a/drivers/mfd/pcf50633-core.c
+++ b/drivers/mfd/pcf50633-core.c
@@ -21,6 +21,7 @@
 #include <linux/workqueue.h>
 #include <linux/platform_device.h>
 #include <linux/i2c.h>
+#include <linux/pm.h>
 #include <linux/slab.h>
 
 #include <linux/mfd/pcf50633/core.h>
@@ -230,27 +231,26 @@ pcf50633_client_dev_register(struct pcf50633 *pcf, const char *name,
 	}
 }
 
-#ifdef CONFIG_PM
-static int pcf50633_suspend(struct i2c_client *client, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int pcf50633_suspend(struct device *dev)
 {
-	struct pcf50633 *pcf;
-	pcf = i2c_get_clientdata(client);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct pcf50633 *pcf = i2c_get_clientdata(client);
 
 	return pcf50633_irq_suspend(pcf);
 }
 
-static int pcf50633_resume(struct i2c_client *client)
+static int pcf50633_resume(struct device *dev)
 {
-	struct pcf50633 *pcf;
-	pcf = i2c_get_clientdata(client);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct pcf50633 *pcf = i2c_get_clientdata(client);
 
 	return pcf50633_irq_resume(pcf);
 }
-#else
-#define pcf50633_suspend NULL
-#define pcf50633_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(pcf50633_pm, pcf50633_suspend, pcf50633_resume);
+
 static int __devinit pcf50633_probe(struct i2c_client *client,
 				const struct i2c_device_id *ids)
 {
@@ -360,16 +360,16 @@ static struct i2c_device_id pcf50633_id_table[] = {
 	{"pcf50633", 0x73},
 	{/* end of list */}
 };
+MODULE_DEVICE_TABLE(i2c, pcf50633_id_table);
 
 static struct i2c_driver pcf50633_driver = {
 	.driver = {
 		.name	= "pcf50633",
+		.pm	= &pcf50633_pm,
 	},
 	.id_table = pcf50633_id_table,
 	.probe = pcf50633_probe,
 	.remove = __devexit_p(pcf50633_remove),
-	.suspend = pcf50633_suspend,
-	.resume	= pcf50633_resume,
 };
 
 static int __init pcf50633_init(void)
diff --git a/drivers/mfd/rdc321x-southbridge.c b/drivers/mfd/rdc321x-southbridge.c
index 50922975bda3..193c940225b5 100644
--- a/drivers/mfd/rdc321x-southbridge.c
+++ b/drivers/mfd/rdc321x-southbridge.c
@@ -61,12 +61,12 @@ static struct mfd_cell rdc321x_sb_cells[] = {
 		.name		= "rdc321x-wdt",
 		.resources	= rdc321x_wdt_resource,
 		.num_resources	= ARRAY_SIZE(rdc321x_wdt_resource),
-		.driver_data	= &rdc321x_wdt_pdata,
+		.mfd_data	= &rdc321x_wdt_pdata,
 	}, {
 		.name		= "rdc321x-gpio",
 		.resources	= rdc321x_gpio_resources,
 		.num_resources	= ARRAY_SIZE(rdc321x_gpio_resources),
-		.driver_data	= &rdc321x_gpio_pdata,
+		.mfd_data	= &rdc321x_gpio_pdata,
 	},
 };
 
diff --git a/drivers/mfd/sh_mobile_sdhi.c b/drivers/mfd/sh_mobile_sdhi.c
index 0a7df44a93c0..53a63024bf11 100644
--- a/drivers/mfd/sh_mobile_sdhi.c
+++ b/drivers/mfd/sh_mobile_sdhi.c
@@ -146,9 +146,7 @@ static int __devinit sh_mobile_sdhi_probe(struct platform_device *pdev)
 	}
 
 	memcpy(&priv->cell_mmc, &sh_mobile_sdhi_cell, sizeof(priv->cell_mmc));
-	priv->cell_mmc.driver_data = mmc_data;
-	priv->cell_mmc.platform_data = &priv->cell_mmc;
-	priv->cell_mmc.data_size = sizeof(priv->cell_mmc);
+	priv->cell_mmc.mfd_data = mmc_data;
 
 	platform_set_drvdata(pdev, priv);
 
diff --git a/drivers/mfd/t7l66xb.c b/drivers/mfd/t7l66xb.c
index 9caeb4ac6ea6..af57fc706a4c 100644
--- a/drivers/mfd/t7l66xb.c
+++ b/drivers/mfd/t7l66xb.c
@@ -170,7 +170,7 @@ static struct mfd_cell t7l66xb_cells[] = {
 		.name = "tmio-mmc",
 		.enable = t7l66xb_mmc_enable,
 		.disable = t7l66xb_mmc_disable,
-		.driver_data = &t7166xb_mmc_data,
+		.mfd_data = &t7166xb_mmc_data,
 		.num_resources = ARRAY_SIZE(t7l66xb_mmc_resources),
 		.resources = t7l66xb_mmc_resources,
 	},
@@ -383,16 +383,7 @@ static int t7l66xb_probe(struct platform_device *dev)
 
 	t7l66xb_attach_irq(dev);
 
-	t7l66xb_cells[T7L66XB_CELL_NAND].driver_data = pdata->nand_data;
-	t7l66xb_cells[T7L66XB_CELL_NAND].platform_data =
-		&t7l66xb_cells[T7L66XB_CELL_NAND];
-	t7l66xb_cells[T7L66XB_CELL_NAND].data_size =
-		sizeof(t7l66xb_cells[T7L66XB_CELL_NAND]);
-
-	t7l66xb_cells[T7L66XB_CELL_MMC].platform_data =
-		&t7l66xb_cells[T7L66XB_CELL_MMC];
-	t7l66xb_cells[T7L66XB_CELL_MMC].data_size =
-		sizeof(t7l66xb_cells[T7L66XB_CELL_MMC]);
+	t7l66xb_cells[T7L66XB_CELL_NAND].mfd_data = pdata->nand_data;
 
 	ret = mfd_add_devices(&dev->dev, dev->id,
 			      t7l66xb_cells, ARRAY_SIZE(t7l66xb_cells),
diff --git a/drivers/mfd/tc6387xb.c b/drivers/mfd/tc6387xb.c
index 6315f63f017d..b006f7cee952 100644
--- a/drivers/mfd/tc6387xb.c
+++ b/drivers/mfd/tc6387xb.c
@@ -131,7 +131,7 @@ static struct mfd_cell tc6387xb_cells[] = {
 		.name = "tmio-mmc",
 		.enable = tc6387xb_mmc_enable,
 		.disable = tc6387xb_mmc_disable,
-		.driver_data = &tc6387xb_mmc_data,
+		.mfd_data = &tc6387xb_mmc_data,
 		.num_resources = ARRAY_SIZE(tc6387xb_mmc_resources),
 		.resources = tc6387xb_mmc_resources,
 	},
@@ -190,11 +190,6 @@ static int __devinit tc6387xb_probe(struct platform_device *dev)
 
 	printk(KERN_INFO "Toshiba tc6387xb initialised\n");
 
-	tc6387xb_cells[TC6387XB_CELL_MMC].platform_data =
-		&tc6387xb_cells[TC6387XB_CELL_MMC];
-	tc6387xb_cells[TC6387XB_CELL_MMC].data_size =
-		sizeof(tc6387xb_cells[TC6387XB_CELL_MMC]);
-
 	ret = mfd_add_devices(&dev->dev, dev->id, tc6387xb_cells,
 			      ARRAY_SIZE(tc6387xb_cells), iomem, irq);
 
diff --git a/drivers/mfd/tc6393xb.c b/drivers/mfd/tc6393xb.c
index 9a238633a54d..3d62ded86a8f 100644
--- a/drivers/mfd/tc6393xb.c
+++ b/drivers/mfd/tc6393xb.c
@@ -393,7 +393,7 @@ static struct mfd_cell __devinitdata tc6393xb_cells[] = {
 		.name = "tmio-mmc",
 		.enable = tc6393xb_mmc_enable,
 		.resume = tc6393xb_mmc_resume,
-		.driver_data = &tc6393xb_mmc_data,
+		.mfd_data = &tc6393xb_mmc_data,
 		.num_resources = ARRAY_SIZE(tc6393xb_mmc_resources),
 		.resources = tc6393xb_mmc_resources,
 	},
@@ -693,27 +693,8 @@ static int __devinit tc6393xb_probe(struct platform_device *dev)
 			goto err_setup;
 	}
 
-	tc6393xb_cells[TC6393XB_CELL_NAND].driver_data = tcpd->nand_data;
-	tc6393xb_cells[TC6393XB_CELL_NAND].platform_data =
-		&tc6393xb_cells[TC6393XB_CELL_NAND];
-	tc6393xb_cells[TC6393XB_CELL_NAND].data_size =
-		sizeof(tc6393xb_cells[TC6393XB_CELL_NAND]);
-
-	tc6393xb_cells[TC6393XB_CELL_MMC].platform_data =
-		&tc6393xb_cells[TC6393XB_CELL_MMC];
-	tc6393xb_cells[TC6393XB_CELL_MMC].data_size =
-		sizeof(tc6393xb_cells[TC6393XB_CELL_MMC]);
-
-	tc6393xb_cells[TC6393XB_CELL_OHCI].platform_data =
-		&tc6393xb_cells[TC6393XB_CELL_OHCI];
-	tc6393xb_cells[TC6393XB_CELL_OHCI].data_size =
-		sizeof(tc6393xb_cells[TC6393XB_CELL_OHCI]);
-
-	tc6393xb_cells[TC6393XB_CELL_FB].driver_data = tcpd->fb_data;
-	tc6393xb_cells[TC6393XB_CELL_FB].platform_data =
-		&tc6393xb_cells[TC6393XB_CELL_FB];
-	tc6393xb_cells[TC6393XB_CELL_FB].data_size =
-		sizeof(tc6393xb_cells[TC6393XB_CELL_FB]);
+	tc6393xb_cells[TC6393XB_CELL_NAND].mfd_data = tcpd->nand_data;
+	tc6393xb_cells[TC6393XB_CELL_FB].mfd_data = tcpd->fb_data;
 
 	ret = mfd_add_devices(&dev->dev, dev->id,
 			tc6393xb_cells, ARRAY_SIZE(tc6393xb_cells),
diff --git a/drivers/mfd/timberdale.c b/drivers/mfd/timberdale.c
index 6ad8a7f8d390..94c6c8afad12 100644
--- a/drivers/mfd/timberdale.c
+++ b/drivers/mfd/timberdale.c
@@ -384,8 +384,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg0[] = {
 		.name = "timb-dma",
 		.num_resources = ARRAY_SIZE(timberdale_dma_resources),
 		.resources = timberdale_dma_resources,
-		.platform_data = &timb_dma_platform_data,
-		.data_size = sizeof(timb_dma_platform_data),
+		.mfd_data = &timb_dma_platform_data,
 	},
 	{
 		.name = "timb-uart",
@@ -396,43 +395,37 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg0[] = {
 		.name = "xiic-i2c",
 		.num_resources = ARRAY_SIZE(timberdale_xiic_resources),
 		.resources = timberdale_xiic_resources,
-		.platform_data = &timberdale_xiic_platform_data,
-		.data_size = sizeof(timberdale_xiic_platform_data),
+		.mfd_data = &timberdale_xiic_platform_data,
 	},
 	{
 		.name = "timb-gpio",
 		.num_resources = ARRAY_SIZE(timberdale_gpio_resources),
 		.resources = timberdale_gpio_resources,
-		.platform_data = &timberdale_gpio_platform_data,
-		.data_size = sizeof(timberdale_gpio_platform_data),
+		.mfd_data = &timberdale_gpio_platform_data,
 	},
 	{
 		.name = "timb-video",
 		.num_resources = ARRAY_SIZE(timberdale_video_resources),
 		.resources = timberdale_video_resources,
-		.platform_data = &timberdale_video_platform_data,
-		.data_size = sizeof(timberdale_video_platform_data),
+		.mfd_data = &timberdale_video_platform_data,
 	},
 	{
 		.name = "timb-radio",
 		.num_resources = ARRAY_SIZE(timberdale_radio_resources),
 		.resources = timberdale_radio_resources,
-		.platform_data = &timberdale_radio_platform_data,
-		.data_size = sizeof(timberdale_radio_platform_data),
+		.mfd_data = &timberdale_radio_platform_data,
 	},
 	{
 		.name = "xilinx_spi",
 		.num_resources = ARRAY_SIZE(timberdale_spi_resources),
 		.resources = timberdale_spi_resources,
-		.platform_data = &timberdale_xspi_platform_data,
-		.data_size = sizeof(timberdale_xspi_platform_data),
+		.mfd_data = &timberdale_xspi_platform_data,
 	},
 	{
 		.name = "ks8842",
 		.num_resources = ARRAY_SIZE(timberdale_eth_resources),
 		.resources = timberdale_eth_resources,
-		.platform_data = &timberdale_ks8842_platform_data,
-		.data_size = sizeof(timberdale_ks8842_platform_data)
+		.mfd_data = &timberdale_ks8842_platform_data,
 	},
 };
 
@@ -441,8 +434,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
 		.name = "timb-dma",
 		.num_resources = ARRAY_SIZE(timberdale_dma_resources),
 		.resources = timberdale_dma_resources,
-		.platform_data = &timb_dma_platform_data,
-		.data_size = sizeof(timb_dma_platform_data),
+		.mfd_data = &timb_dma_platform_data,
 	},
 	{
 		.name = "timb-uart",
@@ -458,15 +450,13 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
 		.name = "xiic-i2c",
 		.num_resources = ARRAY_SIZE(timberdale_xiic_resources),
 		.resources = timberdale_xiic_resources,
-		.platform_data = &timberdale_xiic_platform_data,
-		.data_size = sizeof(timberdale_xiic_platform_data),
+		.mfd_data = &timberdale_xiic_platform_data,
 	},
 	{
 		.name = "timb-gpio",
 		.num_resources = ARRAY_SIZE(timberdale_gpio_resources),
 		.resources = timberdale_gpio_resources,
-		.platform_data = &timberdale_gpio_platform_data,
-		.data_size = sizeof(timberdale_gpio_platform_data),
+		.mfd_data = &timberdale_gpio_platform_data,
 	},
 	{
 		.name = "timb-mlogicore",
@@ -477,29 +467,25 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
 		.name = "timb-video",
 		.num_resources = ARRAY_SIZE(timberdale_video_resources),
 		.resources = timberdale_video_resources,
-		.platform_data = &timberdale_video_platform_data,
-		.data_size = sizeof(timberdale_video_platform_data),
+		.mfd_data = &timberdale_video_platform_data,
 	},
 	{
 		.name = "timb-radio",
 		.num_resources = ARRAY_SIZE(timberdale_radio_resources),
 		.resources = timberdale_radio_resources,
-		.platform_data = &timberdale_radio_platform_data,
-		.data_size = sizeof(timberdale_radio_platform_data),
+		.mfd_data = &timberdale_radio_platform_data,
 	},
 	{
 		.name = "xilinx_spi",
 		.num_resources = ARRAY_SIZE(timberdale_spi_resources),
 		.resources = timberdale_spi_resources,
-		.platform_data = &timberdale_xspi_platform_data,
-		.data_size = sizeof(timberdale_xspi_platform_data),
+		.mfd_data = &timberdale_xspi_platform_data,
 	},
 	{
 		.name = "ks8842",
 		.num_resources = ARRAY_SIZE(timberdale_eth_resources),
 		.resources = timberdale_eth_resources,
-		.platform_data = &timberdale_ks8842_platform_data,
-		.data_size = sizeof(timberdale_ks8842_platform_data)
+		.mfd_data = &timberdale_ks8842_platform_data,
 	},
 };
 
@@ -508,8 +494,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
 		.name = "timb-dma",
 		.num_resources = ARRAY_SIZE(timberdale_dma_resources),
 		.resources = timberdale_dma_resources,
-		.platform_data = &timb_dma_platform_data,
-		.data_size = sizeof(timb_dma_platform_data),
+		.mfd_data = &timb_dma_platform_data,
 	},
 	{
 		.name = "timb-uart",
@@ -520,36 +505,31 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
 		.name = "xiic-i2c",
 		.num_resources = ARRAY_SIZE(timberdale_xiic_resources),
 		.resources = timberdale_xiic_resources,
-		.platform_data = &timberdale_xiic_platform_data,
-		.data_size = sizeof(timberdale_xiic_platform_data),
+		.mfd_data = &timberdale_xiic_platform_data,
 	},
 	{
 		.name = "timb-gpio",
 		.num_resources = ARRAY_SIZE(timberdale_gpio_resources),
 		.resources = timberdale_gpio_resources,
-		.platform_data = &timberdale_gpio_platform_data,
-		.data_size = sizeof(timberdale_gpio_platform_data),
+		.mfd_data = &timberdale_gpio_platform_data,
 	},
 	{
 		.name = "timb-video",
 		.num_resources = ARRAY_SIZE(timberdale_video_resources),
 		.resources = timberdale_video_resources,
-		.platform_data = &timberdale_video_platform_data,
-		.data_size = sizeof(timberdale_video_platform_data),
+		.mfd_data = &timberdale_video_platform_data,
 	},
 	{
 		.name = "timb-radio",
 		.num_resources = ARRAY_SIZE(timberdale_radio_resources),
 		.resources = timberdale_radio_resources,
-		.platform_data = &timberdale_radio_platform_data,
-		.data_size = sizeof(timberdale_radio_platform_data),
+		.mfd_data = &timberdale_radio_platform_data,
 	},
 	{
 		.name = "xilinx_spi",
 		.num_resources = ARRAY_SIZE(timberdale_spi_resources),
 		.resources = timberdale_spi_resources,
-		.platform_data = &timberdale_xspi_platform_data,
-		.data_size = sizeof(timberdale_xspi_platform_data),
+		.mfd_data = &timberdale_xspi_platform_data,
 	},
 };
 
@@ -558,8 +538,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
 		.name = "timb-dma",
 		.num_resources = ARRAY_SIZE(timberdale_dma_resources),
 		.resources = timberdale_dma_resources,
-		.platform_data = &timb_dma_platform_data,
-		.data_size = sizeof(timb_dma_platform_data),
+		.mfd_data = &timb_dma_platform_data,
 	},
 	{
 		.name = "timb-uart",
@@ -570,43 +549,37 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
 		.name = "ocores-i2c",
 		.num_resources = ARRAY_SIZE(timberdale_ocores_resources),
 		.resources = timberdale_ocores_resources,
-		.platform_data = &timberdale_ocores_platform_data,
-		.data_size = sizeof(timberdale_ocores_platform_data),
+		.mfd_data = &timberdale_ocores_platform_data,
 	},
 	{
 		.name = "timb-gpio",
 		.num_resources = ARRAY_SIZE(timberdale_gpio_resources),
 		.resources = timberdale_gpio_resources,
-		.platform_data = &timberdale_gpio_platform_data,
-		.data_size = sizeof(timberdale_gpio_platform_data),
+		.mfd_data = &timberdale_gpio_platform_data,
 	},
 	{
 		.name = "timb-video",
 		.num_resources = ARRAY_SIZE(timberdale_video_resources),
 		.resources = timberdale_video_resources,
-		.platform_data = &timberdale_video_platform_data,
-		.data_size = sizeof(timberdale_video_platform_data),
+		.mfd_data = &timberdale_video_platform_data,
 	},
 	{
 		.name = "timb-radio",
 		.num_resources = ARRAY_SIZE(timberdale_radio_resources),
 		.resources = timberdale_radio_resources,
-		.platform_data = &timberdale_radio_platform_data,
-		.data_size = sizeof(timberdale_radio_platform_data),
+		.mfd_data = &timberdale_radio_platform_data,
 	},
 	{
 		.name = "xilinx_spi",
 		.num_resources = ARRAY_SIZE(timberdale_spi_resources),
 		.resources = timberdale_spi_resources,
-		.platform_data = &timberdale_xspi_platform_data,
-		.data_size = sizeof(timberdale_xspi_platform_data),
+		.mfd_data = &timberdale_xspi_platform_data,
 	},
 	{
 		.name = "ks8842",
 		.num_resources = ARRAY_SIZE(timberdale_eth_resources),
 		.resources = timberdale_eth_resources,
-		.platform_data = &timberdale_ks8842_platform_data,
-		.data_size = sizeof(timberdale_ks8842_platform_data)
+		.mfd_data = &timberdale_ks8842_platform_data,
 	},
 };
 
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index 627cf577b16d..0aa9186aec19 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -150,12 +150,12 @@ static inline int __tps6586x_write(struct i2c_client *client,
 static inline int __tps6586x_writes(struct i2c_client *client, int reg,
 				  int len, uint8_t *val)
 {
-	int ret;
+	int ret, i;
 
-	ret = i2c_smbus_write_i2c_block_data(client, reg, len, val);
-	if (ret < 0) {
-		dev_err(&client->dev, "failed writings to 0x%02x\n", reg);
-		return ret;
+	for (i = 0; i < len; i++) {
+		ret = __tps6586x_write(client, reg + i, *(val + i));
+		if (ret < 0)
+			return ret;
 	}
 
 	return 0;
@@ -288,12 +288,10 @@ static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
 	return tps6586x_update(tps6586x->dev, TPS6586X_GPIOSET1, val, mask);
 }
 
-static void tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
+static int tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
 {
-	int ret;
-
 	if (!gpio_base)
-		return;
+		return 0;
 
 	tps6586x->gpio.owner		= THIS_MODULE;
 	tps6586x->gpio.label		= tps6586x->client->name;
@@ -307,9 +305,7 @@ static void tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
 	tps6586x->gpio.set		= tps6586x_gpio_set;
 	tps6586x->gpio.get		= tps6586x_gpio_get;
 
-	ret = gpiochip_add(&tps6586x->gpio);
-	if (ret)
-		dev_warn(tps6586x->dev, "GPIO registration failed: %d\n", ret);
+	return gpiochip_add(&tps6586x->gpio);
 }
 
 static int __remove_subdev(struct device *dev, void *unused)
@@ -517,17 +513,28 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 		}
 	}
 
+	ret = tps6586x_gpio_init(tps6586x, pdata->gpio_base);
+	if (ret) {
+		dev_err(&client->dev, "GPIO registration failed: %d\n", ret);
+		goto err_gpio_init;
+	}
+
 	ret = tps6586x_add_subdevs(tps6586x, pdata);
 	if (ret) {
 		dev_err(&client->dev, "add devices failed: %d\n", ret);
 		goto err_add_devs;
 	}
 
-	tps6586x_gpio_init(tps6586x, pdata->gpio_base);
-
 	return 0;
 
 err_add_devs:
+	if (pdata->gpio_base) {
+		ret = gpiochip_remove(&tps6586x->gpio);
+		if (ret)
+			dev_err(&client->dev, "Can't remove gpio chip: %d\n",
+				ret);
+	}
+err_gpio_init:
 	if (client->irq)
 		free_irq(client->irq, tps6586x);
 err_irq_init:
@@ -587,4 +594,3 @@ module_exit(tps6586x_exit);
 MODULE_DESCRIPTION("TPS6586X core driver");
 MODULE_AUTHOR("Mike Rapoport <mike@compulab.co.il>");
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index a35fa7dcbf53..960b5bed7f52 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -721,13 +721,13 @@ add_children(struct twl4030_platform_data *pdata, unsigned long features)
 
 	}
 
-	if (twl_has_watchdog()) {
+	if (twl_has_watchdog() && twl_class_is_4030()) {
 		child = add_child(0, "twl4030_wdt", NULL, 0, false, 0, 0);
 		if (IS_ERR(child))
 			return PTR_ERR(child);
 	}
 
-	if (twl_has_pwrbutton()) {
+	if (twl_has_pwrbutton() && twl_class_is_4030()) {
 		child = add_child(1, "twl4030_pwrbutton",
 				NULL, 0, true, pdata->irq_base + 8 + 0, 0);
 		if (IS_ERR(child))
@@ -864,6 +864,10 @@ add_children(struct twl4030_platform_data *pdata, unsigned long features)
 		child = add_regulator(TWL6030_REG_VAUX3_6030, pdata->vaux3);
 		if (IS_ERR(child))
 			return PTR_ERR(child);
+
+		child = add_regulator(TWL6030_REG_CLK32KG, pdata->clk32kg);
+		if (IS_ERR(child))
+			return PTR_ERR(child);
 	}
 
 	if (twl_has_bci() && pdata->bci &&
diff --git a/drivers/mfd/twl4030-codec.c b/drivers/mfd/twl4030-codec.c
index 9a4b196d6deb..c02fded316c9 100644
--- a/drivers/mfd/twl4030-codec.c
+++ b/drivers/mfd/twl4030-codec.c
@@ -208,15 +208,13 @@ static int __devinit twl4030_codec_probe(struct platform_device *pdev)
 	if (pdata->audio) {
 		cell = &codec->cells[childs];
 		cell->name = "twl4030-codec";
-		cell->platform_data = pdata->audio;
-		cell->data_size = sizeof(*pdata->audio);
+		cell->mfd_data = pdata->audio;
 		childs++;
 	}
 	if (pdata->vibra) {
 		cell = &codec->cells[childs];
 		cell->name = "twl4030-vibra";
-		cell->platform_data = pdata->vibra;
-		cell->data_size = sizeof(*pdata->vibra);
+		cell->mfd_data = pdata->vibra;
 		childs++;
 	}
 
diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
new file mode 100644
index 000000000000..e9884e2583bf
--- /dev/null
+++ b/drivers/mfd/twl4030-madc.c
@@ -0,0 +1,802 @@
+/*
+ *
+ * TWL4030 MADC module driver-This driver monitors the real time
+ * conversion of analog signals like battery temperature,
+ * battery type, battery level etc.
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * J Keerthy <j-keerthy@ti.com>
+ *
+ * Based on twl4030-madc.c
+ * Copyright (C) 2008 Nokia Corporation
+ * Mikko Ylinen <mikko.k.ylinen@nokia.com>
+ *
+ * Amit Kucheria <amit.kucheria@canonical.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/i2c/twl.h>
+#include <linux/i2c/twl4030-madc.h>
+#include <linux/module.h>
+#include <linux/stddef.h>
+#include <linux/mutex.h>
+#include <linux/bitops.h>
+#include <linux/jiffies.h>
+#include <linux/types.h>
+#include <linux/gfp.h>
+#include <linux/err.h>
+
+/*
+ * struct twl4030_madc_data - a container for madc info
+ * @dev - pointer to device structure for madc
+ * @lock - mutex protecting this data structure
+ * @requests - Array of request struct corresponding to SW1, SW2 and RT
+ * @imr - Interrupt mask register of MADC
+ * @isr - Interrupt status register of MADC
+ */
+struct twl4030_madc_data {
+	struct device *dev;
+	struct mutex lock;	/* mutex protecting this data structure */
+	struct twl4030_madc_request requests[TWL4030_MADC_NUM_METHODS];
+	int imr;
+	int isr;
+};
+
+static struct twl4030_madc_data *twl4030_madc;
+
+struct twl4030_prescale_divider_ratios {
+	s16 numerator;
+	s16 denominator;
+};
+
+static const struct twl4030_prescale_divider_ratios
+twl4030_divider_ratios[16] = {
+	{1, 1},		/* CHANNEL 0 No Prescaler */
+	{1, 1},		/* CHANNEL 1 No Prescaler */
+	{6, 10},	/* CHANNEL 2 */
+	{6, 10},	/* CHANNEL 3 */
+	{6, 10},	/* CHANNEL 4 */
+	{6, 10},	/* CHANNEL 5 */
+	{6, 10},	/* CHANNEL 6 */
+	{6, 10},	/* CHANNEL 7 */
+	{3, 14},	/* CHANNEL 8 */
+	{1, 3},		/* CHANNEL 9 */
+	{1, 1},		/* CHANNEL 10 No Prescaler */
+	{15, 100},	/* CHANNEL 11 */
+	{1, 4},		/* CHANNEL 12 */
+	{1, 1},		/* CHANNEL 13 Reserved channels */
+	{1, 1},		/* CHANNEL 14 Reseved channels */
+	{5, 11},	/* CHANNEL 15 */
+};
+
+
+/*
+ * Conversion table from -3 to 55 degree Celcius
+ */
+static int therm_tbl[] = {
+30800,	29500,	28300,	27100,
+26000,	24900,	23900,	22900,	22000,	21100,	20300,	19400,	18700,	17900,
+17200,	16500,	15900,	15300,	14700,	14100,	13600,	13100,	12600,	12100,
+11600,	11200,	10800,	10400,	10000,	9630,	9280,	8950,	8620,	8310,
+8020,	7730,	7460,	7200,	6950,	6710,	6470,	6250,	6040,	5830,
+5640,	5450,	5260,	5090,	4920,	4760,	4600,	4450,	4310,	4170,
+4040,	3910,	3790,	3670,	3550
+};
+
+/*
+ * Structure containing the registers
+ * of different conversion methods supported by MADC.
+ * Hardware or RT real time conversion request initiated by external host
+ * processor for RT Signal conversions.
+ * External host processors can also request for non RT conversions
+ * SW1 and SW2 software conversions also called asynchronous or GPC request.
+ */
+static
+const struct twl4030_madc_conversion_method twl4030_conversion_methods[] = {
+	[TWL4030_MADC_RT] = {
+			     .sel = TWL4030_MADC_RTSELECT_LSB,
+			     .avg = TWL4030_MADC_RTAVERAGE_LSB,
+			     .rbase = TWL4030_MADC_RTCH0_LSB,
+			     },
+	[TWL4030_MADC_SW1] = {
+			      .sel = TWL4030_MADC_SW1SELECT_LSB,
+			      .avg = TWL4030_MADC_SW1AVERAGE_LSB,
+			      .rbase = TWL4030_MADC_GPCH0_LSB,
+			      .ctrl = TWL4030_MADC_CTRL_SW1,
+			      },
+	[TWL4030_MADC_SW2] = {
+			      .sel = TWL4030_MADC_SW2SELECT_LSB,
+			      .avg = TWL4030_MADC_SW2AVERAGE_LSB,
+			      .rbase = TWL4030_MADC_GPCH0_LSB,
+			      .ctrl = TWL4030_MADC_CTRL_SW2,
+			      },
+};
+
+/*
+ * Function to read a particular channel value.
+ * @madc - pointer to struct twl4030_madc_data
+ * @reg - lsb of ADC Channel
+ * If the i2c read fails it returns an error else returns 0.
+ */
+static int twl4030_madc_channel_raw_read(struct twl4030_madc_data *madc, u8 reg)
+{
+	u8 msb, lsb;
+	int ret;
+	/*
+	 * For each ADC channel, we have MSB and LSB register pair. MSB address
+	 * is always LSB address+1. reg parameter is the address of LSB register
+	 */
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &msb, reg + 1);
+	if (ret) {
+		dev_err(madc->dev, "unable to read MSB register 0x%X\n",
+			reg + 1);
+		return ret;
+	}
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &lsb, reg);
+	if (ret) {
+		dev_err(madc->dev, "unable to read LSB register 0x%X\n", reg);
+		return ret;
+	}
+
+	return (int)(((msb << 8) | lsb) >> 6);
+}
+
+/*
+ * Return battery temperature
+ * Or < 0 on failure.
+ */
+static int twl4030battery_temperature(int raw_volt)
+{
+	u8 val;
+	int temp, curr, volt, res, ret;
+
+	volt = (raw_volt * TEMP_STEP_SIZE) / TEMP_PSR_R;
+	/* Getting and calculating the supply current in micro ampers */
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE, &val,
+		REG_BCICTL2);
+	if (ret < 0)
+		return ret;
+	curr = ((val & TWL4030_BCI_ITHEN) + 1) * 10;
+	/* Getting and calculating the thermistor resistance in ohms */
+	res = volt * 1000 / curr;
+	/* calculating temperature */
+	for (temp = 58; temp >= 0; temp--) {
+		int actual = therm_tbl[temp];
+
+		if ((actual - res) >= 0)
+			break;
+	}
+
+	return temp + 1;
+}
+
+static int twl4030battery_current(int raw_volt)
+{
+	int ret;
+	u8 val;
+
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE, &val,
+		TWL4030_BCI_BCICTL1);
+	if (ret)
+		return ret;
+	if (val & TWL4030_BCI_CGAIN) /* slope of 0.44 mV/mA */
+		return (raw_volt * CURR_STEP_SIZE) / CURR_PSR_R1;
+	else /* slope of 0.88 mV/mA */
+		return (raw_volt * CURR_STEP_SIZE) / CURR_PSR_R2;
+}
+/*
+ * Function to read channel values
+ * @madc - pointer to twl4030_madc_data struct
+ * @reg_base - Base address of the first channel
+ * @Channels - 16 bit bitmap. If the bit is set, channel value is read
+ * @buf - The channel values are stored here. if read fails error
+ * value is stored
+ * Returns the number of successfully read channels.
+ */
+static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
+				      u8 reg_base, unsigned
+						long channels, int *buf)
+{
+	int count = 0, count_req = 0, i;
+	u8 reg;
+
+	for_each_set_bit(i, &channels, TWL4030_MADC_MAX_CHANNELS) {
+		reg = reg_base + 2 * i;
+		buf[i] = twl4030_madc_channel_raw_read(madc, reg);
+		if (buf[i] < 0) {
+			dev_err(madc->dev,
+				"Unable to read register 0x%X\n", reg);
+			count_req++;
+			continue;
+		}
+		switch (i) {
+		case 10:
+			buf[i] = twl4030battery_current(buf[i]);
+			if (buf[i] < 0) {
+				dev_err(madc->dev, "err reading current\n");
+				count_req++;
+			} else {
+				count++;
+				buf[i] = buf[i] - 750;
+			}
+			break;
+		case 1:
+			buf[i] = twl4030battery_temperature(buf[i]);
+			if (buf[i] < 0) {
+				dev_err(madc->dev, "err reading temperature\n");
+				count_req++;
+			} else {
+				buf[i] -= 3;
+				count++;
+			}
+			break;
+		default:
+			count++;
+			/* Analog Input (V) = conv_result * step_size / R
+			 * conv_result = decimal value of 10-bit conversion
+			 *		 result
+			 * step size = 1.5 / (2 ^ 10 -1)
+			 * R = Prescaler ratio for input channels.
+			 * Result given in mV hence multiplied by 1000.
+			 */
+			buf[i] = (buf[i] * 3 * 1000 *
+				 twl4030_divider_ratios[i].denominator)
+				/ (2 * 1023 *
+				twl4030_divider_ratios[i].numerator);
+		}
+	}
+	if (count_req)
+		dev_err(madc->dev, "%d channel conversion failed\n", count_req);
+
+	return count;
+}
+
+/*
+ * Enables irq.
+ * @madc - pointer to twl4030_madc_data struct
+ * @id - irq number to be enabled
+ * can take one of TWL4030_MADC_RT, TWL4030_MADC_SW1, TWL4030_MADC_SW2
+ * corresponding to RT, SW1, SW2 conversion requests.
+ * If the i2c read fails it returns an error else returns 0.
+ */
+static int twl4030_madc_enable_irq(struct twl4030_madc_data *madc, u8 id)
+{
+	u8 val;
+	int ret;
+
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &val, madc->imr);
+	if (ret) {
+		dev_err(madc->dev, "unable to read imr register 0x%X\n",
+			madc->imr);
+		return ret;
+	}
+	val &= ~(1 << id);
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, val, madc->imr);
+	if (ret) {
+		dev_err(madc->dev,
+			"unable to write imr register 0x%X\n", madc->imr);
+		return ret;
+
+	}
+
+	return 0;
+}
+
+/*
+ * Disables irq.
+ * @madc - pointer to twl4030_madc_data struct
+ * @id - irq number to be disabled
+ * can take one of TWL4030_MADC_RT, TWL4030_MADC_SW1, TWL4030_MADC_SW2
+ * corresponding to RT, SW1, SW2 conversion requests.
+ * Returns error if i2c read/write fails.
+ */
+static int twl4030_madc_disable_irq(struct twl4030_madc_data *madc, u8 id)
+{
+	u8 val;
+	int ret;
+
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &val, madc->imr);
+	if (ret) {
+		dev_err(madc->dev, "unable to read imr register 0x%X\n",
+			madc->imr);
+		return ret;
+	}
+	val |= (1 << id);
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, val, madc->imr);
+	if (ret) {
+		dev_err(madc->dev,
+			"unable to write imr register 0x%X\n", madc->imr);
+		return ret;
+	}
+
+	return 0;
+}
+
+static irqreturn_t twl4030_madc_threaded_irq_handler(int irq, void *_madc)
+{
+	struct twl4030_madc_data *madc = _madc;
+	const struct twl4030_madc_conversion_method *method;
+	u8 isr_val, imr_val;
+	int i, len, ret;
+	struct twl4030_madc_request *r;
+
+	mutex_lock(&madc->lock);
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &isr_val, madc->isr);
+	if (ret) {
+		dev_err(madc->dev, "unable to read isr register 0x%X\n",
+			madc->isr);
+		goto err_i2c;
+	}
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &imr_val, madc->imr);
+	if (ret) {
+		dev_err(madc->dev, "unable to read imr register 0x%X\n",
+			madc->imr);
+		goto err_i2c;
+	}
+	isr_val &= ~imr_val;
+	for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+		if (!(isr_val & (1 << i)))
+			continue;
+		ret = twl4030_madc_disable_irq(madc, i);
+		if (ret < 0)
+			dev_dbg(madc->dev, "Disable interrupt failed%d\n", i);
+		madc->requests[i].result_pending = 1;
+	}
+	for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+		r = &madc->requests[i];
+		/* No pending results for this method, move to next one */
+		if (!r->result_pending)
+			continue;
+		method = &twl4030_conversion_methods[r->method];
+		/* Read results */
+		len = twl4030_madc_read_channels(madc, method->rbase,
+						 r->channels, r->rbuf);
+		/* Return results to caller */
+		if (r->func_cb != NULL) {
+			r->func_cb(len, r->channels, r->rbuf);
+			r->func_cb = NULL;
+		}
+		/* Free request */
+		r->result_pending = 0;
+		r->active = 0;
+	}
+	mutex_unlock(&madc->lock);
+
+	return IRQ_HANDLED;
+
+err_i2c:
+	/*
+	 * In case of error check whichever request is active
+	 * and service the same.
+	 */
+	for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+		r = &madc->requests[i];
+		if (r->active == 0)
+			continue;
+		method = &twl4030_conversion_methods[r->method];
+		/* Read results */
+		len = twl4030_madc_read_channels(madc, method->rbase,
+						 r->channels, r->rbuf);
+		/* Return results to caller */
+		if (r->func_cb != NULL) {
+			r->func_cb(len, r->channels, r->rbuf);
+			r->func_cb = NULL;
+		}
+		/* Free request */
+		r->result_pending = 0;
+		r->active = 0;
+	}
+	mutex_unlock(&madc->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int twl4030_madc_set_irq(struct twl4030_madc_data *madc,
+				struct twl4030_madc_request *req)
+{
+	struct twl4030_madc_request *p;
+	int ret;
+
+	p = &madc->requests[req->method];
+	memcpy(p, req, sizeof(*req));
+	ret = twl4030_madc_enable_irq(madc, req->method);
+	if (ret < 0) {
+		dev_err(madc->dev, "enable irq failed!!\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Function which enables the madc conversion
+ * by writing to the control register.
+ * @madc - pointer to twl4030_madc_data struct
+ * @conv_method - can be TWL4030_MADC_RT, TWL4030_MADC_SW2, TWL4030_MADC_SW1
+ * corresponding to RT SW1 or SW2 conversion methods.
+ * Returns 0 if succeeds else a negative error value
+ */
+static int twl4030_madc_start_conversion(struct twl4030_madc_data *madc,
+					 int conv_method)
+{
+	const struct twl4030_madc_conversion_method *method;
+	int ret = 0;
+	method = &twl4030_conversion_methods[conv_method];
+	switch (conv_method) {
+	case TWL4030_MADC_SW1:
+	case TWL4030_MADC_SW2:
+		ret = twl_i2c_write_u8(TWL4030_MODULE_MADC,
+				       TWL4030_MADC_SW_START, method->ctrl);
+		if (ret) {
+			dev_err(madc->dev,
+				"unable to write ctrl register 0x%X\n",
+				method->ctrl);
+			return ret;
+		}
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Function that waits for conversion to be ready
+ * @madc - pointer to twl4030_madc_data struct
+ * @timeout_ms - timeout value in milliseconds
+ * @status_reg - ctrl register
+ * returns 0 if succeeds else a negative error value
+ */
+static int twl4030_madc_wait_conversion_ready(struct twl4030_madc_data *madc,
+					      unsigned int timeout_ms,
+					      u8 status_reg)
+{
+	unsigned long timeout;
+	int ret;
+
+	timeout = jiffies + msecs_to_jiffies(timeout_ms);
+	do {
+		u8 reg;
+
+		ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &reg, status_reg);
+		if (ret) {
+			dev_err(madc->dev,
+				"unable to read status register 0x%X\n",
+				status_reg);
+			return ret;
+		}
+		if (!(reg & TWL4030_MADC_BUSY) && (reg & TWL4030_MADC_EOC_SW))
+			return 0;
+		usleep_range(500, 2000);
+	} while (!time_after(jiffies, timeout));
+	dev_err(madc->dev, "conversion timeout!\n");
+
+	return -EAGAIN;
+}
+
+/*
+ * An exported function which can be called from other kernel drivers.
+ * @req twl4030_madc_request structure
+ * req->rbuf will be filled with read values of channels based on the
+ * channel index. If a particular channel reading fails there will
+ * be a negative error value in the corresponding array element.
+ * returns 0 if succeeds else error value
+ */
+int twl4030_madc_conversion(struct twl4030_madc_request *req)
+{
+	const struct twl4030_madc_conversion_method *method;
+	u8 ch_msb, ch_lsb;
+	int ret;
+
+	if (!req)
+		return -EINVAL;
+	mutex_lock(&twl4030_madc->lock);
+	if (req->method < TWL4030_MADC_RT || req->method > TWL4030_MADC_SW2) {
+		ret = -EINVAL;
+		goto out;
+	}
+	/* Do we have a conversion request ongoing */
+	if (twl4030_madc->requests[req->method].active) {
+		ret = -EBUSY;
+		goto out;
+	}
+	ch_msb = (req->channels >> 8) & 0xff;
+	ch_lsb = req->channels & 0xff;
+	method = &twl4030_conversion_methods[req->method];
+	/* Select channels to be converted */
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, ch_msb, method->sel + 1);
+	if (ret) {
+		dev_err(twl4030_madc->dev,
+			"unable to write sel register 0x%X\n", method->sel + 1);
+		return ret;
+	}
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, ch_lsb, method->sel);
+	if (ret) {
+		dev_err(twl4030_madc->dev,
+			"unable to write sel register 0x%X\n", method->sel + 1);
+		return ret;
+	}
+	/* Select averaging for all channels if do_avg is set */
+	if (req->do_avg) {
+		ret = twl_i2c_write_u8(TWL4030_MODULE_MADC,
+				       ch_msb, method->avg + 1);
+		if (ret) {
+			dev_err(twl4030_madc->dev,
+				"unable to write avg register 0x%X\n",
+				method->avg + 1);
+			return ret;
+		}
+		ret = twl_i2c_write_u8(TWL4030_MODULE_MADC,
+				       ch_lsb, method->avg);
+		if (ret) {
+			dev_err(twl4030_madc->dev,
+				"unable to write sel reg 0x%X\n",
+				method->sel + 1);
+			return ret;
+		}
+	}
+	if (req->type == TWL4030_MADC_IRQ_ONESHOT && req->func_cb != NULL) {
+		ret = twl4030_madc_set_irq(twl4030_madc, req);
+		if (ret < 0)
+			goto out;
+		ret = twl4030_madc_start_conversion(twl4030_madc, req->method);
+		if (ret < 0)
+			goto out;
+		twl4030_madc->requests[req->method].active = 1;
+		ret = 0;
+		goto out;
+	}
+	/* With RT method we should not be here anymore */
+	if (req->method == TWL4030_MADC_RT) {
+		ret = -EINVAL;
+		goto out;
+	}
+	ret = twl4030_madc_start_conversion(twl4030_madc, req->method);
+	if (ret < 0)
+		goto out;
+	twl4030_madc->requests[req->method].active = 1;
+	/* Wait until conversion is ready (ctrl register returns EOC) */
+	ret = twl4030_madc_wait_conversion_ready(twl4030_madc, 5, method->ctrl);
+	if (ret) {
+		twl4030_madc->requests[req->method].active = 0;
+		goto out;
+	}
+	ret = twl4030_madc_read_channels(twl4030_madc, method->rbase,
+					 req->channels, req->rbuf);
+	twl4030_madc->requests[req->method].active = 0;
+
+out:
+	mutex_unlock(&twl4030_madc->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(twl4030_madc_conversion);
+
+/*
+ * Return channel value
+ * Or < 0 on failure.
+ */
+int twl4030_get_madc_conversion(int channel_no)
+{
+	struct twl4030_madc_request req;
+	int temp = 0;
+	int ret;
+
+	req.channels = (1 << channel_no);
+	req.method = TWL4030_MADC_SW2;
+	req.active = 0;
+	req.func_cb = NULL;
+	ret = twl4030_madc_conversion(&req);
+	if (ret < 0)
+		return ret;
+	if (req.rbuf[channel_no] > 0)
+		temp = req.rbuf[channel_no];
+
+	return temp;
+}
+EXPORT_SYMBOL_GPL(twl4030_get_madc_conversion);
+
+/*
+ * Function to enable or disable bias current for
+ * main battery type reading or temperature sensing
+ * @madc - pointer to twl4030_madc_data struct
+ * @chan - can be one of the two values
+ * TWL4030_BCI_ITHEN - Enables bias current for main battery type reading
+ * TWL4030_BCI_TYPEN - Enables bias current for main battery temperature
+ * sensing
+ * @on - enable or disable chan.
+ */
+static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc,
+					      int chan, int on)
+{
+	int ret;
+	u8 regval;
+
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+			      &regval, TWL4030_BCI_BCICTL1);
+	if (ret) {
+		dev_err(madc->dev, "unable to read BCICTL1 reg 0x%X",
+			TWL4030_BCI_BCICTL1);
+		return ret;
+	}
+	if (on)
+		regval |= chan ? TWL4030_BCI_ITHEN : TWL4030_BCI_TYPEN;
+	else
+		regval &= chan ? ~TWL4030_BCI_ITHEN : ~TWL4030_BCI_TYPEN;
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
+			       regval, TWL4030_BCI_BCICTL1);
+	if (ret) {
+		dev_err(madc->dev, "unable to write BCICTL1 reg 0x%X\n",
+			TWL4030_BCI_BCICTL1);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Function that sets MADC software power on bit to enable MADC
+ * @madc - pointer to twl4030_madc_data struct
+ * @on - Enable or disable MADC software powen on bit.
+ * returns error if i2c read/write fails else 0
+ */
+static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on)
+{
+	u8 regval;
+	int ret;
+
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+			      &regval, TWL4030_MADC_CTRL1);
+	if (ret) {
+		dev_err(madc->dev, "unable to read madc ctrl1 reg 0x%X\n",
+			TWL4030_MADC_CTRL1);
+		return ret;
+	}
+	if (on)
+		regval |= TWL4030_MADC_MADCON;
+	else
+		regval &= ~TWL4030_MADC_MADCON;
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, regval, TWL4030_MADC_CTRL1);
+	if (ret) {
+		dev_err(madc->dev, "unable to write madc ctrl1 reg 0x%X\n",
+			TWL4030_MADC_CTRL1);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Initialize MADC and request for threaded irq
+ */
+static int __devinit twl4030_madc_probe(struct platform_device *pdev)
+{
+	struct twl4030_madc_data *madc;
+	struct twl4030_madc_platform_data *pdata = pdev->dev.platform_data;
+	int ret;
+	u8 regval;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "platform_data not available\n");
+		return -EINVAL;
+	}
+	madc = kzalloc(sizeof(*madc), GFP_KERNEL);
+	if (!madc)
+		return -ENOMEM;
+
+	/*
+	 * Phoenix provides 2 interrupt lines. The first one is connected to
+	 * the OMAP. The other one can be connected to the other processor such
+	 * as modem. Hence two separate ISR and IMR registers.
+	 */
+	madc->imr = (pdata->irq_line == 1) ?
+	    TWL4030_MADC_IMR1 : TWL4030_MADC_IMR2;
+	madc->isr = (pdata->irq_line == 1) ?
+	    TWL4030_MADC_ISR1 : TWL4030_MADC_ISR2;
+	ret = twl4030_madc_set_power(madc, 1);
+	if (ret < 0)
+		goto err_power;
+	ret = twl4030_madc_set_current_generator(madc, 0, 1);
+	if (ret < 0)
+		goto err_current_generator;
+
+	ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+			      &regval, TWL4030_BCI_BCICTL1);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to read reg BCI CTL1 0x%X\n",
+			TWL4030_BCI_BCICTL1);
+		goto err_i2c;
+	}
+	regval |= TWL4030_BCI_MESBAT;
+	ret = twl_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
+			       regval, TWL4030_BCI_BCICTL1);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to write reg BCI Ctl1 0x%X\n",
+			TWL4030_BCI_BCICTL1);
+		goto err_i2c;
+	}
+	platform_set_drvdata(pdev, madc);
+	mutex_init(&madc->lock);
+	ret = request_threaded_irq(platform_get_irq(pdev, 0), NULL,
+				   twl4030_madc_threaded_irq_handler,
+				   IRQF_TRIGGER_RISING, "twl4030_madc", madc);
+	if (ret) {
+		dev_dbg(&pdev->dev, "could not request irq\n");
+		goto err_irq;
+	}
+	twl4030_madc = madc;
+	return 0;
+err_irq:
+	platform_set_drvdata(pdev, NULL);
+err_i2c:
+	twl4030_madc_set_current_generator(madc, 0, 0);
+err_current_generator:
+	twl4030_madc_set_power(madc, 0);
+err_power:
+	kfree(madc);
+
+	return ret;
+}
+
+static int __devexit twl4030_madc_remove(struct platform_device *pdev)
+{
+	struct twl4030_madc_data *madc = platform_get_drvdata(pdev);
+
+	free_irq(platform_get_irq(pdev, 0), madc);
+	platform_set_drvdata(pdev, NULL);
+	twl4030_madc_set_current_generator(madc, 0, 0);
+	twl4030_madc_set_power(madc, 0);
+	kfree(madc);
+
+	return 0;
+}
+
+static struct platform_driver twl4030_madc_driver = {
+	.probe = twl4030_madc_probe,
+	.remove = __exit_p(twl4030_madc_remove),
+	.driver = {
+		   .name = "twl4030_madc",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init twl4030_madc_init(void)
+{
+	return platform_driver_register(&twl4030_madc_driver);
+}
+
+module_init(twl4030_madc_init);
+
+static void __exit twl4030_madc_exit(void)
+{
+	platform_driver_unregister(&twl4030_madc_driver);
+}
+
+module_exit(twl4030_madc_exit);
+
+MODULE_DESCRIPTION("TWL4030 ADC driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("J Keerthy");
+MODULE_ALIAS("twl4030_madc");
diff --git a/drivers/mfd/ucb1x00-ts.c b/drivers/mfd/ucb1x00-ts.c
index 000cb414a78a..38ffbd50a0d2 100644
--- a/drivers/mfd/ucb1x00-ts.c
+++ b/drivers/mfd/ucb1x00-ts.c
@@ -60,6 +60,7 @@ static inline void ucb1x00_ts_evt_add(struct ucb1x00_ts *ts, u16 pressure, u16 x
 	input_report_abs(idev, ABS_X, x);
 	input_report_abs(idev, ABS_Y, y);
 	input_report_abs(idev, ABS_PRESSURE, pressure);
+	input_report_key(idev, BTN_TOUCH, 1);
 	input_sync(idev);
 }
 
@@ -68,6 +69,7 @@ static inline void ucb1x00_ts_event_release(struct ucb1x00_ts *ts)
 	struct input_dev *idev = ts->idev;
 
 	input_report_abs(idev, ABS_PRESSURE, 0);
+	input_report_key(idev, BTN_TOUCH, 0);
 	input_sync(idev);
 }
 
@@ -384,13 +386,20 @@ static int ucb1x00_ts_add(struct ucb1x00_dev *dev)
 	idev->open       = ucb1x00_ts_open;
 	idev->close      = ucb1x00_ts_close;
 
-	__set_bit(EV_ABS, idev->evbit);
-	__set_bit(ABS_X, idev->absbit);
-	__set_bit(ABS_Y, idev->absbit);
-	__set_bit(ABS_PRESSURE, idev->absbit);
+	idev->evbit[0]   = BIT_MASK(EV_ABS) | BIT_MASK(EV_KEY);
+	idev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
 
 	input_set_drvdata(idev, ts);
 
+	ucb1x00_adc_enable(ts->ucb);
+	ts->x_res = ucb1x00_ts_read_xres(ts);
+	ts->y_res = ucb1x00_ts_read_yres(ts);
+	ucb1x00_adc_disable(ts->ucb);
+
+	input_set_abs_params(idev, ABS_X, 0, ts->x_res, 0, 0);
+	input_set_abs_params(idev, ABS_Y, 0, ts->y_res, 0, 0);
+	input_set_abs_params(idev, ABS_PRESSURE, 0, 0, 0, 0);
+
 	err = input_register_device(idev);
 	if (err)
 		goto fail;
diff --git a/drivers/mfd/vx855.c b/drivers/mfd/vx855.c
index 348052aa5dbf..d698703dbd46 100644
--- a/drivers/mfd/vx855.c
+++ b/drivers/mfd/vx855.c
@@ -122,6 +122,7 @@ static struct pci_device_id vx855_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VX855) },
 	{ 0, }
 };
+MODULE_DEVICE_TABLE(pci, vx855_pci_tbl);
 
 static struct pci_driver vx855_pci_driver = {
 	.name		= "vx855",
diff --git a/drivers/mfd/wl1273-core.c b/drivers/mfd/wl1273-core.c
index d2ecc2435736..f76f6c798046 100644
--- a/drivers/mfd/wl1273-core.c
+++ b/drivers/mfd/wl1273-core.c
@@ -1,7 +1,7 @@
 /*
  * MFD driver for wl1273 FM radio and audio codec submodules.
  *
- * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2011 Nokia Corporation
  * Author: Matti Aaltonen <matti.j.aaltonen@nokia.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -31,6 +31,145 @@ static struct i2c_device_id wl1273_driver_id_table[] = {
 };
 MODULE_DEVICE_TABLE(i2c, wl1273_driver_id_table);
 
+static int wl1273_fm_read_reg(struct wl1273_core *core, u8 reg, u16 *value)
+{
+	struct i2c_client *client = core->client;
+	u8 b[2];
+	int r;
+
+	r = i2c_smbus_read_i2c_block_data(client, reg, sizeof(b), b);
+	if (r != 2) {
+		dev_err(&client->dev, "%s: Read: %d fails.\n", __func__, reg);
+		return -EREMOTEIO;
+	}
+
+	*value = (u16)b[0] << 8 | b[1];
+
+	return 0;
+}
+
+static int wl1273_fm_write_cmd(struct wl1273_core *core, u8 cmd, u16 param)
+{
+	struct i2c_client *client = core->client;
+	u8 buf[] = { (param >> 8) & 0xff, param & 0xff };
+	int r;
+
+	r = i2c_smbus_write_i2c_block_data(client, cmd, sizeof(buf), buf);
+	if (r) {
+		dev_err(&client->dev, "%s: Cmd: %d fails.\n", __func__, cmd);
+		return r;
+	}
+
+	return 0;
+}
+
+static int wl1273_fm_write_data(struct wl1273_core *core, u8 *data, u16 len)
+{
+	struct i2c_client *client = core->client;
+	struct i2c_msg msg;
+	int r;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.buf = data;
+	msg.len = len;
+
+	r = i2c_transfer(client->adapter, &msg, 1);
+	if (r != 1) {
+		dev_err(&client->dev, "%s: write error.\n", __func__);
+		return -EREMOTEIO;
+	}
+
+	return 0;
+}
+
+/**
+ * wl1273_fm_set_audio() -	Set audio mode.
+ * @core:			A pointer to the device struct.
+ * @new_mode:			The new audio mode.
+ *
+ * Audio modes are WL1273_AUDIO_DIGITAL and WL1273_AUDIO_ANALOG.
+ */
+static int wl1273_fm_set_audio(struct wl1273_core *core, unsigned int new_mode)
+{
+	int r = 0;
+
+	if (core->mode == WL1273_MODE_OFF ||
+	    core->mode == WL1273_MODE_SUSPENDED)
+		return -EPERM;
+
+	if (core->mode == WL1273_MODE_RX && new_mode == WL1273_AUDIO_DIGITAL) {
+		r = wl1273_fm_write_cmd(core, WL1273_PCM_MODE_SET,
+					WL1273_PCM_DEF_MODE);
+		if (r)
+			goto out;
+
+		r = wl1273_fm_write_cmd(core, WL1273_I2S_MODE_CONFIG_SET,
+					core->i2s_mode);
+		if (r)
+			goto out;
+
+		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_ENABLE,
+					WL1273_AUDIO_ENABLE_I2S);
+		if (r)
+			goto out;
+
+	} else if (core->mode == WL1273_MODE_RX &&
+		   new_mode == WL1273_AUDIO_ANALOG) {
+		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_ENABLE,
+					WL1273_AUDIO_ENABLE_ANALOG);
+		if (r)
+			goto out;
+
+	} else if (core->mode == WL1273_MODE_TX &&
+		   new_mode == WL1273_AUDIO_DIGITAL) {
+		r = wl1273_fm_write_cmd(core, WL1273_I2S_MODE_CONFIG_SET,
+					core->i2s_mode);
+		if (r)
+			goto out;
+
+		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_IO_SET,
+					WL1273_AUDIO_IO_SET_I2S);
+		if (r)
+			goto out;
+
+	} else if (core->mode == WL1273_MODE_TX &&
+		   new_mode == WL1273_AUDIO_ANALOG) {
+		r = wl1273_fm_write_cmd(core, WL1273_AUDIO_IO_SET,
+					WL1273_AUDIO_IO_SET_ANALOG);
+		if (r)
+			goto out;
+	}
+
+	core->audio_mode = new_mode;
+out:
+	return r;
+}
+
+/**
+ * wl1273_fm_set_volume() -	Set volume.
+ * @core:			A pointer to the device struct.
+ * @volume:			The new volume value.
+ */
+static int wl1273_fm_set_volume(struct wl1273_core *core, unsigned int volume)
+{
+	u16 val;
+	int r;
+
+	if (volume > WL1273_MAX_VOLUME)
+		return -EINVAL;
+
+	if (core->volume == volume)
+		return 0;
+
+	r = wl1273_fm_write_cmd(core, WL1273_VOLUME_SET, volume);
+	if (r)
+		return r;
+
+	core->volume = volume;
+	return 0;
+}
+
 static int wl1273_core_remove(struct i2c_client *client)
 {
 	struct wl1273_core *core = i2c_get_clientdata(client);
@@ -38,7 +177,6 @@ static int wl1273_core_remove(struct i2c_client *client)
 	dev_dbg(&client->dev, "%s\n", __func__);
 
 	mfd_remove_devices(&client->dev);
-	i2c_set_clientdata(client, NULL);
 	kfree(core);
 
 	return 0;
@@ -79,17 +217,21 @@ static int __devinit wl1273_core_probe(struct i2c_client *client,
 
 	cell = &core->cells[children];
 	cell->name = "wl1273_fm_radio";
-	cell->platform_data = &core;
-	cell->data_size = sizeof(core);
+	cell->mfd_data = &core;
 	children++;
 
+	core->read = wl1273_fm_read_reg;
+	core->write = wl1273_fm_write_cmd;
+	core->write_data = wl1273_fm_write_data;
+	core->set_audio = wl1273_fm_set_audio;
+	core->set_volume = wl1273_fm_set_volume;
+
 	if (pdata->children & WL1273_CODEC_CHILD) {
 		cell = &core->cells[children];
 
 		dev_dbg(&client->dev, "%s: Have codec.\n", __func__);
 		cell->name = "wl1273-codec";
-		cell->platform_data = &core;
-		cell->data_size = sizeof(core);
+		cell->mfd_data = &core;
 		children++;
 	}
 
@@ -104,7 +246,6 @@ static int __devinit wl1273_core_probe(struct i2c_client *client,
 	return 0;
 
 err:
-	i2c_set_clientdata(client, NULL);
 	pdata->free_resources();
 	kfree(core);
 
diff --git a/drivers/mfd/wm831x-irq.c b/drivers/mfd/wm831x-irq.c
index f7192d438aab..a5cd17e18d09 100644
--- a/drivers/mfd/wm831x-irq.c
+++ b/drivers/mfd/wm831x-irq.c
@@ -26,15 +26,6 @@
 
 #include <linux/delay.h>
 
-/*
- * Since generic IRQs don't currently support interrupt controllers on
- * interrupt driven buses we don't use genirq but instead provide an
- * interface that looks very much like the standard ones.  This leads
- * to some bodges, including storing interrupt handler information in
- * the static irq_data table we use to look up the data for individual
- * interrupts, but hopefully won't last too long.
- */
-
 struct wm831x_irq_data {
 	int primary;
 	int reg;
@@ -361,6 +352,10 @@ static void wm831x_irq_sync_unlock(struct irq_data *data)
 		/* If there's been a change in the mask write it back
 		 * to the hardware. */
 		if (wm831x->irq_masks_cur[i] != wm831x->irq_masks_cache[i]) {
+			dev_dbg(wm831x->dev, "IRQ mask sync: %x = %x\n",
+				WM831X_INTERRUPT_STATUS_1_MASK + i,
+				wm831x->irq_masks_cur[i]);
+
 			wm831x->irq_masks_cache[i] = wm831x->irq_masks_cur[i];
 			wm831x_reg_write(wm831x,
 					 WM831X_INTERRUPT_STATUS_1_MASK + i,
@@ -371,7 +366,7 @@ static void wm831x_irq_sync_unlock(struct irq_data *data)
 	mutex_unlock(&wm831x->irq_lock);
 }
 
-static void wm831x_irq_unmask(struct irq_data *data)
+static void wm831x_irq_enable(struct irq_data *data)
 {
 	struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
 	struct wm831x_irq_data *irq_data = irq_to_wm831x_irq(wm831x,
@@ -380,7 +375,7 @@ static void wm831x_irq_unmask(struct irq_data *data)
 	wm831x->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
 }
 
-static void wm831x_irq_mask(struct irq_data *data)
+static void wm831x_irq_disable(struct irq_data *data)
 {
 	struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
 	struct wm831x_irq_data *irq_data = irq_to_wm831x_irq(wm831x,
@@ -426,8 +421,8 @@ static struct irq_chip wm831x_irq_chip = {
 	.name			= "wm831x",
 	.irq_bus_lock		= wm831x_irq_lock,
 	.irq_bus_sync_unlock	= wm831x_irq_sync_unlock,
-	.irq_mask		= wm831x_irq_mask,
-	.irq_unmask		= wm831x_irq_unmask,
+	.irq_disable		= wm831x_irq_disable,
+	.irq_enable		= wm831x_irq_enable,
 	.irq_set_type		= wm831x_irq_set_type,
 };
 
@@ -449,6 +444,18 @@ static irqreturn_t wm831x_irq_thread(int irq, void *data)
 		goto out;
 	}
 
+	/* The touch interrupts are visible in the primary register as
+	 * an optimisation; open code this to avoid complicating the
+	 * main handling loop and so we can also skip iterating the
+	 * descriptors.
+	 */
+	if (primary & WM831X_TCHPD_INT)
+		handle_nested_irq(wm831x->irq_base + WM831X_IRQ_TCHPD);
+	if (primary & WM831X_TCHDATA_INT)
+		handle_nested_irq(wm831x->irq_base + WM831X_IRQ_TCHDATA);
+	if (primary & (WM831X_TCHDATA_EINT | WM831X_TCHPD_EINT))
+		goto out;
+
 	for (i = 0; i < ARRAY_SIZE(wm831x_irqs); i++) {
 		int offset = wm831x_irqs[i].reg - 1;
 
@@ -481,6 +488,9 @@ static irqreturn_t wm831x_irq_thread(int irq, void *data)
 	}
 
 out:
+	/* Touchscreen interrupts are handled specially in the driver */
+	status_regs[0] &= ~(WM831X_TCHDATA_EINT | WM831X_TCHPD_EINT);
+
 	for (i = 0; i < ARRAY_SIZE(status_regs); i++) {
 		if (status_regs[i])
 			wm831x_reg_write(wm831x, WM831X_INTERRUPT_STATUS_1 + i,
@@ -517,6 +527,14 @@ int wm831x_irq_init(struct wm831x *wm831x, int irq)
 		return 0;
 	}
 
+	if (pdata->irq_cmos)
+		i = 0;
+	else
+		i = WM831X_IRQ_OD;
+
+	wm831x_set_bits(wm831x, WM831X_IRQ_CONFIG,
+			WM831X_IRQ_OD, i);
+
 	/* Try to flag /IRQ as a wake source; there are a number of
 	 * unconditional wake sources in the PMIC so this isn't
 	 * conditional but we don't actually care *too* much if it
diff --git a/drivers/mfd/wm831x-spi.c b/drivers/mfd/wm831x-spi.c
index 0a8f772be88c..eed8e4f7a5a1 100644
--- a/drivers/mfd/wm831x-spi.c
+++ b/drivers/mfd/wm831x-spi.c
@@ -14,6 +14,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/pm.h>
 #include <linux/spi/spi.h>
 
 #include <linux/mfd/wm831x/core.h>
@@ -113,22 +114,27 @@ static int __devexit wm831x_spi_remove(struct spi_device *spi)
 	return 0;
 }
 
-static int wm831x_spi_suspend(struct spi_device *spi, pm_message_t m)
+static int wm831x_spi_suspend(struct device *dev)
 {
-	struct wm831x *wm831x = dev_get_drvdata(&spi->dev);
+	struct wm831x *wm831x = dev_get_drvdata(dev);
 
 	return wm831x_device_suspend(wm831x);
 }
 
+static const struct dev_pm_ops wm831x_spi_pm = {
+	.freeze = wm831x_spi_suspend,
+	.suspend = wm831x_spi_suspend,
+};
+
 static struct spi_driver wm8310_spi_driver = {
 	.driver = {
 		.name	= "wm8310",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8311_spi_driver = {
@@ -136,10 +142,10 @@ static struct spi_driver wm8311_spi_driver = {
 		.name	= "wm8311",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8312_spi_driver = {
@@ -147,10 +153,10 @@ static struct spi_driver wm8312_spi_driver = {
 		.name	= "wm8312",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8320_spi_driver = {
@@ -158,10 +164,10 @@ static struct spi_driver wm8320_spi_driver = {
 		.name	= "wm8320",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8321_spi_driver = {
@@ -169,10 +175,10 @@ static struct spi_driver wm8321_spi_driver = {
 		.name	= "wm8321",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8325_spi_driver = {
@@ -180,10 +186,10 @@ static struct spi_driver wm8325_spi_driver = {
 		.name	= "wm8325",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8326_spi_driver = {
@@ -191,10 +197,10 @@ static struct spi_driver wm8326_spi_driver = {
 		.name	= "wm8326",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
+		.pm	= &wm831x_spi_pm,
 	},
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
-	.suspend	= wm831x_spi_suspend,
 };
 
 static int __init wm831x_spi_init(void)
diff --git a/drivers/mfd/wm8400-core.c b/drivers/mfd/wm8400-core.c
index 1bfef4846b07..3a6e78cb0384 100644
--- a/drivers/mfd/wm8400-core.c
+++ b/drivers/mfd/wm8400-core.c
@@ -245,7 +245,7 @@ static int wm8400_register_codec(struct wm8400 *wm8400)
 {
 	struct mfd_cell cell = {
 		.name = "wm8400-codec",
-		.driver_data = wm8400,
+		.mfd_data = wm8400,
 	};
 
 	return mfd_add_devices(wm8400->dev, -1, &cell, 1, NULL, 0);
diff --git a/drivers/mfd/wm8994-core.c b/drivers/mfd/wm8994-core.c
index 41233c7fa581..e673bda21f5d 100644
--- a/drivers/mfd/wm8994-core.c
+++ b/drivers/mfd/wm8994-core.c
@@ -246,6 +246,16 @@ static int wm8994_suspend(struct device *dev)
 	struct wm8994 *wm8994 = dev_get_drvdata(dev);
 	int ret;
 
+	/* Don't actually go through with the suspend if the CODEC is
+	 * still active (eg, for audio passthrough from CP. */
+	ret = wm8994_reg_read(wm8994, WM8994_POWER_MANAGEMENT_1);
+	if (ret < 0) {
+		dev_err(dev, "Failed to read power status: %d\n", ret);
+	} else if (ret & WM8994_VMID_SEL_MASK) {
+		dev_dbg(dev, "CODEC still active, ignoring suspend\n");
+		return 0;
+	}
+
 	/* GPIO configuration state is saved here since we may be configuring
 	 * the GPIO alternate functions even if we're not using the gpiolib
 	 * driver for them.
@@ -261,6 +271,13 @@ static int wm8994_suspend(struct device *dev)
 	if (ret < 0)
 		dev_err(dev, "Failed to save LDO registers: %d\n", ret);
 
+	/* Explicitly put the device into reset in case regulators
+	 * don't get disabled in order to ensure consistent restart.
+	 */
+	wm8994_reg_write(wm8994, WM8994_SOFTWARE_RESET, 0x8994);
+
+	wm8994->suspended = true;
+
 	ret = regulator_bulk_disable(wm8994->num_supplies,
 				     wm8994->supplies);
 	if (ret != 0) {
@@ -276,6 +293,10 @@ static int wm8994_resume(struct device *dev)
 	struct wm8994 *wm8994 = dev_get_drvdata(dev);
 	int ret;
 
+	/* We may have lied to the PM core about suspending */
+	if (!wm8994->suspended)
+		return 0;
+
 	ret = regulator_bulk_enable(wm8994->num_supplies,
 				    wm8994->supplies);
 	if (ret != 0) {
@@ -298,6 +319,8 @@ static int wm8994_resume(struct device *dev)
 	if (ret < 0)
 		dev_err(dev, "Failed to restore GPIO registers: %d\n", ret);
 
+	wm8994->suspended = false;
+
 	return 0;
 }
 #endif
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index 29e8faf9c01c..f5e439a37dc5 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -182,7 +182,7 @@ static void wm8994_irq_sync_unlock(struct irq_data *data)
 	mutex_unlock(&wm8994->irq_lock);
 }
 
-static void wm8994_irq_unmask(struct irq_data *data)
+static void wm8994_irq_enable(struct irq_data *data)
 {
 	struct wm8994 *wm8994 = irq_data_get_irq_chip_data(data);
 	struct wm8994_irq_data *irq_data = irq_to_wm8994_irq(wm8994,
@@ -191,7 +191,7 @@ static void wm8994_irq_unmask(struct irq_data *data)
 	wm8994->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
 }
 
-static void wm8994_irq_mask(struct irq_data *data)
+static void wm8994_irq_disable(struct irq_data *data)
 {
 	struct wm8994 *wm8994 = irq_data_get_irq_chip_data(data);
 	struct wm8994_irq_data *irq_data = irq_to_wm8994_irq(wm8994,
@@ -204,8 +204,8 @@ static struct irq_chip wm8994_irq_chip = {
 	.name			= "wm8994",
 	.irq_bus_lock		= wm8994_irq_lock,
 	.irq_bus_sync_unlock	= wm8994_irq_sync_unlock,
-	.irq_mask		= wm8994_irq_mask,
-	.irq_unmask		= wm8994_irq_unmask,
+	.irq_disable		= wm8994_irq_disable,
+	.irq_enable		= wm8994_irq_enable,
 };
 
 /* The processing of the primary interrupt occurs in a thread so that
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index cc8e49db45fe..203500d9b848 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -2,6 +2,14 @@
 # Misc strange devices
 #
 
+# This one has to live outside of the MISC_DEVICES conditional,
+# because it may be selected by drivers/platform/x86/hp_accel.
+config SENSORS_LIS3LV02D
+	tristate
+	depends on INPUT
+	select INPUT_POLLDEV
+	default n
+
 menuconfig MISC_DEVICES
 	bool "Misc devices"
 	---help---
@@ -441,7 +449,7 @@ config BMP085
 	  module will be called bmp085.
 
 config PCH_PHUB
-	tristate "PCH Packet Hub of Intel Topcliff"
+	tristate "PCH Packet Hub of Intel Topcliff / OKI SEMICONDUCTOR ML7213"
 	depends on PCI
 	help
 	  This driver is for PCH(Platform controller Hub) PHUB(Packet Hub) of
@@ -449,6 +457,11 @@ config PCH_PHUB
 	  processor. The Topcliff has MAC address and Option ROM data in SROM.
 	  This driver can access MAC address and Option ROM data in SROM.
 
+	  This driver also can be used for OKI SEMICONDUCTOR's ML7213 which is
+	  for IVI(In-Vehicle Infotainment) use.
+	  ML7213 is companion chip for Intel Atom E6xx series.
+	  ML7213 is completely compatible for Intel EG20T PCH.
+
 	  To compile this driver as a module, choose M here: the module will
 	  be called pch_phub.
 
@@ -457,5 +470,6 @@ source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
 source "drivers/misc/iwmc3200top/Kconfig"
 source "drivers/misc/ti-st/Kconfig"
+source "drivers/misc/lis3lv02d/Kconfig"
 
 endif # MISC_DEVICES
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 98009cc20cb9..804f421bc079 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -42,3 +42,4 @@ obj-$(CONFIG_ARM_CHARLCD)	+= arm-charlcd.o
 obj-$(CONFIG_PCH_PHUB)		+= pch_phub.o
 obj-y				+= ti-st/
 obj-$(CONFIG_AB8500_PWM)	+= ab8500-pwm.o
+obj-y				+= lis3lv02d/
diff --git a/drivers/misc/bmp085.c b/drivers/misc/bmp085.c
index 63ee4c1a5315..b6e1c9a6679e 100644
--- a/drivers/misc/bmp085.c
+++ b/drivers/misc/bmp085.c
@@ -449,6 +449,7 @@ static const struct i2c_device_id bmp085_id[] = {
 	{ "bmp085", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, bmp085_id);
 
 static struct i2c_driver bmp085_driver = {
 	.driver = {
diff --git a/drivers/misc/iwmc3200top/iwmc3200top.h b/drivers/misc/iwmc3200top/iwmc3200top.h
index 740ff0738ea8..620973ed8bf9 100644
--- a/drivers/misc/iwmc3200top/iwmc3200top.h
+++ b/drivers/misc/iwmc3200top/iwmc3200top.h
@@ -183,9 +183,7 @@ struct iwmct_priv {
 	u32 barker;
 	struct iwmct_dbg dbg;
 
-	/* drivers work queue */
-	struct workqueue_struct *wq;
-	struct workqueue_struct *bus_rescan_wq;
+	/* drivers work items */
 	struct work_struct bus_rescan_worker;
 	struct work_struct isr_worker;
 
diff --git a/drivers/misc/iwmc3200top/main.c b/drivers/misc/iwmc3200top/main.c
index c73cef2c3c5e..727af07f1fbd 100644
--- a/drivers/misc/iwmc3200top/main.c
+++ b/drivers/misc/iwmc3200top/main.c
@@ -89,7 +89,7 @@ static void op_top_message(struct iwmct_priv *priv, struct top_msg *msg)
 	switch (msg->hdr.opcode) {
 	case OP_OPR_ALIVE:
 		LOG_INFO(priv, FW_MSG, "Got ALIVE from device, wake rescan\n");
-		queue_work(priv->bus_rescan_wq, &priv->bus_rescan_worker);
+		schedule_work(&priv->bus_rescan_worker);
 		break;
 	default:
 		LOG_INFO(priv, FW_MSG, "Received msg opcode 0x%X\n",
@@ -360,7 +360,7 @@ static void iwmct_irq(struct sdio_func *func)
 	/* clear the function's interrupt request bit (write 1 to clear) */
 	sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
 
-	queue_work(priv->wq, &priv->isr_worker);
+	schedule_work(&priv->isr_worker);
 
 	LOG_TRACE(priv, IRQ, "exit iwmct_irq\n");
 
@@ -506,10 +506,6 @@ static int iwmct_probe(struct sdio_func *func,
 	priv->func = func;
 	sdio_set_drvdata(func, priv);
 
-
-	/* create drivers work queue */
-	priv->wq = create_workqueue(DRV_NAME "_wq");
-	priv->bus_rescan_wq = create_workqueue(DRV_NAME "_rescan_wq");
 	INIT_WORK(&priv->bus_rescan_worker, iwmct_rescan_worker);
 	INIT_WORK(&priv->isr_worker, iwmct_irq_read_worker);
 
@@ -604,9 +600,9 @@ static void iwmct_remove(struct sdio_func *func)
 	sdio_release_irq(func);
 	sdio_release_host(func);
 
-	/* Safely destroy osc workqueue */
-	destroy_workqueue(priv->bus_rescan_wq);
-	destroy_workqueue(priv->wq);
+	/* Make sure works are finished */
+	flush_work_sync(&priv->bus_rescan_worker);
+	flush_work_sync(&priv->isr_worker);
 
 	sdio_claim_host(func);
 	sdio_disable_func(func);
diff --git a/drivers/misc/kgdbts.c b/drivers/misc/kgdbts.c
index 59c118c19a91..27dc463097f3 100644
--- a/drivers/misc/kgdbts.c
+++ b/drivers/misc/kgdbts.c
@@ -988,7 +988,7 @@ static void kgdbts_run_tests(void)
 
 static int kgdbts_option_setup(char *opt)
 {
-	if (strlen(opt) > MAX_CONFIG_LEN) {
+	if (strlen(opt) >= MAX_CONFIG_LEN) {
 		printk(KERN_ERR "kgdbts: config string too long\n");
 		return -ENOSPC;
 	}
diff --git a/drivers/misc/lis3lv02d/Kconfig b/drivers/misc/lis3lv02d/Kconfig
new file mode 100644
index 000000000000..8f474e6fc7b4
--- /dev/null
+++ b/drivers/misc/lis3lv02d/Kconfig
@@ -0,0 +1,37 @@
+#
+# STMicroelectonics LIS3LV02D and similar accelerometers
+#
+
+config SENSORS_LIS3_SPI
+	tristate "STMicroeletronics LIS3LV02Dx three-axis digital accelerometer (SPI)"
+	depends on !ACPI && SPI_MASTER && INPUT
+	select SENSORS_LIS3LV02D
+	default n
+	help
+	  This driver provides support for the LIS3LV02Dx accelerometer connected
+	  via SPI. The accelerometer data is readable via
+	  /sys/devices/platform/lis3lv02d.
+
+	  This driver also provides an absolute input class device, allowing
+	  the laptop to act as a pinball machine-esque joystick.
+
+	  This driver can also be built as modules.  If so, the core module
+	  will be called lis3lv02d and a specific module for the SPI transport
+	  is called lis3lv02d_spi.
+
+config SENSORS_LIS3_I2C
+	tristate "STMicroeletronics LIS3LV02Dx three-axis digital accelerometer (I2C)"
+	depends on I2C && INPUT
+	select SENSORS_LIS3LV02D
+	default n
+	help
+	  This driver provides support for the LIS3LV02Dx accelerometer connected
+	  via I2C. The accelerometer data is readable via
+	  /sys/devices/platform/lis3lv02d.
+
+	  This driver also provides an absolute input class device, allowing
+	  the device to act as a pinball machine-esque joystick.
+
+	  This driver can also be built as modules.  If so, the core module
+	  will be called lis3lv02d and a specific module for the I2C transport
+	  is called lis3lv02d_i2c.
diff --git a/drivers/misc/lis3lv02d/Makefile b/drivers/misc/lis3lv02d/Makefile
new file mode 100644
index 000000000000..4bf58b16fcf8
--- /dev/null
+++ b/drivers/misc/lis3lv02d/Makefile
@@ -0,0 +1,7 @@
+#
+# STMicroelectonics LIS3LV02D and similar accelerometers
+#
+
+obj-$(CONFIG_SENSORS_LIS3LV02D) += lis3lv02d.o
+obj-$(CONFIG_SENSORS_LIS3_SPI)	+= lis3lv02d_spi.o
+obj-$(CONFIG_SENSORS_LIS3_I2C)	+= lis3lv02d_i2c.o
diff --git a/drivers/hwmon/lis3lv02d.c b/drivers/misc/lis3lv02d/lis3lv02d.c
index d805e8e57967..b928bc14e97b 100644
--- a/drivers/hwmon/lis3lv02d.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d.c
@@ -38,7 +38,7 @@
 #include <linux/uaccess.h>
 #include <linux/miscdevice.h>
 #include <linux/pm_runtime.h>
-#include <asm/atomic.h>
+#include <linux/atomic.h>
 #include "lis3lv02d.h"
 
 #define DRIVER_NAME     "lis3lv02d"
@@ -88,7 +88,6 @@
 struct lis3lv02d lis3_dev = {
 	.misc_wait   = __WAIT_QUEUE_HEAD_INITIALIZER(lis3_dev.misc_wait),
 };
-
 EXPORT_SYMBOL_GPL(lis3_dev);
 
 /* just like param_set_int() but does sanity-check so that it won't point
diff --git a/drivers/hwmon/lis3lv02d.h b/drivers/misc/lis3lv02d/lis3lv02d.h
index a1939589eb2c..a1939589eb2c 100644
--- a/drivers/hwmon/lis3lv02d.h
+++ b/drivers/misc/lis3lv02d/lis3lv02d.h
diff --git a/drivers/hwmon/lis3lv02d_i2c.c b/drivers/misc/lis3lv02d/lis3lv02d_i2c.c
index 8853afce85ce..b20dfb4522d2 100644
--- a/drivers/hwmon/lis3lv02d_i2c.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d_i2c.c
@@ -33,7 +33,7 @@
 #include <linux/delay.h>
 #include "lis3lv02d.h"
 
-#define DRV_NAME 	"lis3lv02d_i2c"
+#define DRV_NAME	"lis3lv02d_i2c"
 
 static const char reg_vdd[]    = "Vdd";
 static const char reg_vdd_io[] = "Vdd_IO";
diff --git a/drivers/hwmon/lis3lv02d_spi.c b/drivers/misc/lis3lv02d/lis3lv02d_spi.c
index 2549de1de4e2..962c9642cf35 100644
--- a/drivers/hwmon/lis3lv02d_spi.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d_spi.c
@@ -16,10 +16,11 @@
 #include <linux/interrupt.h>
 #include <linux/workqueue.h>
 #include <linux/spi/spi.h>
+#include <linux/pm.h>
 
 #include "lis3lv02d.h"
 
-#define DRV_NAME 	"lis3lv02d_spi"
+#define DRV_NAME	"lis3lv02d_spi"
 #define LIS3_SPI_READ	0x80
 
 static int lis3_spi_read(struct lis3lv02d *lis3, int reg, u8 *v)
@@ -88,9 +89,10 @@ static int __devexit lis302dl_spi_remove(struct spi_device *spi)
 	return lis3lv02d_remove_fs(&lis3_dev);
 }
 
-#ifdef CONFIG_PM
-static int lis3lv02d_spi_suspend(struct spi_device *spi, pm_message_t mesg)
+#ifdef CONFIG_PM_SLEEP
+static int lis3lv02d_spi_suspend(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct lis3lv02d *lis3 = spi_get_drvdata(spi);
 
 	if (!lis3->pdata || !lis3->pdata->wakeup_flags)
@@ -99,8 +101,9 @@ static int lis3lv02d_spi_suspend(struct spi_device *spi, pm_message_t mesg)
 	return 0;
 }
 
-static int lis3lv02d_spi_resume(struct spi_device *spi)
+static int lis3lv02d_spi_resume(struct device *dev)
 {
+	struct spi_device *spi = to_spi_device(dev);
 	struct lis3lv02d *lis3 = spi_get_drvdata(spi);
 
 	if (!lis3->pdata || !lis3->pdata->wakeup_flags)
@@ -108,21 +111,19 @@ static int lis3lv02d_spi_resume(struct spi_device *spi)
 
 	return 0;
 }
-
-#else
-#define lis3lv02d_spi_suspend	NULL
-#define lis3lv02d_spi_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(lis3lv02d_spi_pm, lis3lv02d_spi_suspend,
+			 lis3lv02d_spi_resume);
+
 static struct spi_driver lis302dl_spi_driver = {
 	.driver	 = {
 		.name   = DRV_NAME,
 		.owner  = THIS_MODULE,
+		.pm	= &lis3lv02d_spi_pm,
 	},
 	.probe	= lis302dl_spi_probe,
 	.remove	= __devexit_p(lis302dl_spi_remove),
-	.suspend = lis3lv02d_spi_suspend,
-	.resume  = lis3lv02d_spi_resume,
 };
 
 static int __init lis302dl_init(void)
diff --git a/drivers/misc/pch_phub.c b/drivers/misc/pch_phub.c
index 744b804aca15..98bffc471b17 100644
--- a/drivers/misc/pch_phub.c
+++ b/drivers/misc/pch_phub.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 OKI SEMICONDUCTOR Co., LTD.
+ * Copyright (C) 2010 OKI SEMICONDUCTOR 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
@@ -33,7 +33,12 @@
 #define PHUB_TIMEOUT 0x05		/* Time out value for Status Register */
 #define PCH_PHUB_ROM_WRITE_ENABLE 0x01	/* Enabling for writing ROM */
 #define PCH_PHUB_ROM_WRITE_DISABLE 0x00	/* Disabling for writing ROM */
-#define PCH_PHUB_ROM_START_ADDR 0x14	/* ROM data area start address offset */
+#define PCH_PHUB_MAC_START_ADDR 0x20C  /* MAC data area start address offset */
+#define PCH_PHUB_ROM_START_ADDR_EG20T 0x14 /* ROM data area start address offset
+					      (Intel EG20T PCH)*/
+#define PCH_PHUB_ROM_START_ADDR_ML7213 0x400 /* ROM data area start address
+						offset(OKI SEMICONDUCTOR ML7213)
+					      */
 
 /* MAX number of INT_REDUCE_CONTROL registers */
 #define MAX_NUM_INT_REDUCE_CONTROL_REG 128
@@ -42,6 +47,10 @@
 #define CLKCFG_CAN_50MHZ 0x12000000
 #define CLKCFG_CANCLK_MASK 0xFF000000
 
+/* Macros for ML7213 */
+#define PCI_VENDOR_ID_ROHM			0x10db
+#define PCI_DEVICE_ID_ROHM_ML7213_PHUB		0x801A
+
 /* SROM ACCESS Macro */
 #define PCH_WORD_ADDR_MASK (~((1 << 2) - 1))
 
@@ -298,7 +307,7 @@ static void pch_phub_read_serial_rom_val(struct pch_phub_reg *chip,
 {
 	unsigned int mem_addr;
 
-	mem_addr = PCH_PHUB_ROM_START_ADDR +
+	mem_addr = PCH_PHUB_ROM_START_ADDR_EG20T +
 			pch_phub_mac_offset[offset_address];
 
 	pch_phub_read_serial_rom(chip, mem_addr, data);
@@ -315,7 +324,7 @@ static int pch_phub_write_serial_rom_val(struct pch_phub_reg *chip,
 	int retval;
 	unsigned int mem_addr;
 
-	mem_addr = PCH_PHUB_ROM_START_ADDR +
+	mem_addr = PCH_PHUB_ROM_START_ADDR_EG20T +
 			pch_phub_mac_offset[offset_address];
 
 	retval = pch_phub_write_serial_rom(chip, mem_addr, data);
@@ -594,23 +603,38 @@ static int __devinit pch_phub_probe(struct pci_dev *pdev,
 		"pch_phub_extrom_base_address variable is %p\n", __func__,
 		chip->pch_phub_extrom_base_address);
 
-	pci_set_drvdata(pdev, chip);
-
-	retval = sysfs_create_file(&pdev->dev.kobj, &dev_attr_pch_mac.attr);
-	if (retval)
-		goto err_sysfs_create;
-
-	retval = sysfs_create_bin_file(&pdev->dev.kobj, &pch_bin_attr);
-	if (retval)
-		goto exit_bin_attr;
-
-	pch_phub_read_modify_write_reg(chip, (unsigned int)CLKCFG_REG_OFFSET,
-					CLKCFG_CAN_50MHZ, CLKCFG_CANCLK_MASK);
+	if (id->driver_data == 1) {
+		retval = sysfs_create_file(&pdev->dev.kobj,
+					   &dev_attr_pch_mac.attr);
+		if (retval)
+			goto err_sysfs_create;
 
-	/* set the prefech value */
-	iowrite32(0x000affaa, chip->pch_phub_base_address + 0x14);
-	/* set the interrupt delay value */
-	iowrite32(0x25, chip->pch_phub_base_address + 0x44);
+		retval = sysfs_create_bin_file(&pdev->dev.kobj, &pch_bin_attr);
+		if (retval)
+			goto exit_bin_attr;
+
+		pch_phub_read_modify_write_reg(chip,
+					       (unsigned int)CLKCFG_REG_OFFSET,
+					       CLKCFG_CAN_50MHZ,
+					       CLKCFG_CANCLK_MASK);
+
+		/* set the prefech value */
+		iowrite32(0x000affaa, chip->pch_phub_base_address + 0x14);
+		/* set the interrupt delay value */
+		iowrite32(0x25, chip->pch_phub_base_address + 0x44);
+	} else if (id->driver_data == 2) {
+		retval = sysfs_create_bin_file(&pdev->dev.kobj, &pch_bin_attr);
+		if (retval)
+			goto err_sysfs_create;
+		/* set the prefech value
+		 * Device2(USB OHCI #1/ USB EHCI #1/ USB Device):a
+		 * Device4(SDIO #0,1,2):f
+		 * Device6(SATA 2):f
+		 * Device8(USB OHCI #0/ USB EHCI #0):a
+		 */
+		iowrite32(0x000affa0, chip->pch_phub_base_address + 0x14);
+	}
+	pci_set_drvdata(pdev, chip);
 
 	return 0;
 exit_bin_attr:
@@ -687,8 +711,9 @@ static int pch_phub_resume(struct pci_dev *pdev)
 #endif /* CONFIG_PM */
 
 static struct pci_device_id pch_phub_pcidev_id[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PCH1_PHUB)},
-	{0,}
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_PCH1_PHUB),       1,  },
+	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ROHM_ML7213_PHUB), 2,  },
+	{ }
 };
 
 static struct pci_driver pch_phub_driver = {
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index f9aad06d1ae5..1847c477c0c0 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -25,10 +25,9 @@
 #include <linux/init.h>
 #include <linux/tty.h>
 
-/* understand BT, FM and GPS for now */
-#include <net/bluetooth/bluetooth.h>
-#include <net/bluetooth/hci_core.h>
-#include <net/bluetooth/hci.h>
+#include <linux/seq_file.h>
+#include <linux/skbuff.h>
+
 #include <linux/ti_wilink_st.h>
 
 /* function pointer pointing to either,
@@ -38,21 +37,38 @@
 void (*st_recv) (void*, const unsigned char*, long);
 
 /********************************************************************/
-#if 0
-/* internal misc functions */
-bool is_protocol_list_empty(void)
+static void add_channel_to_table(struct st_data_s *st_gdata,
+		struct st_proto_s *new_proto)
 {
-	unsigned char i = 0;
-	pr_debug(" %s ", __func__);
-	for (i = 0; i < ST_MAX; i++) {
-		if (st_gdata->list[i] != NULL)
-			return ST_NOTEMPTY;
-		/* not empty */
+	pr_info("%s: id %d\n", __func__, new_proto->chnl_id);
+	/* list now has the channel id as index itself */
+	st_gdata->list[new_proto->chnl_id] = new_proto;
+}
+
+static void remove_channel_from_table(struct st_data_s *st_gdata,
+		struct st_proto_s *proto)
+{
+	pr_info("%s: id %d\n", __func__, proto->chnl_id);
+	st_gdata->list[proto->chnl_id] = NULL;
+}
+
+/*
+ * called from KIM during firmware download.
+ *
+ * This is a wrapper function to tty->ops->write_room.
+ * It returns number of free space available in
+ * uart tx buffer.
+ */
+int st_get_uart_wr_room(struct st_data_s *st_gdata)
+{
+	struct tty_struct *tty;
+	if (unlikely(st_gdata == NULL || st_gdata->tty == NULL)) {
+		pr_err("tty unavailable to perform write");
+		return -1;
 	}
-	/* list empty */
-	return ST_EMPTY;
+	tty = st_gdata->tty;
+	return tty->ops->write_room(tty);
 }
-#endif
 
 /* can be called in from
  * -- KIM (during fw download)
@@ -67,7 +83,7 @@ int st_int_write(struct st_data_s *st_gdata,
 	struct tty_struct *tty;
 	if (unlikely(st_gdata == NULL || st_gdata->tty == NULL)) {
 		pr_err("tty unavailable to perform write");
-		return -1;
+		return -EINVAL;
 	}
 	tty = st_gdata->tty;
 #ifdef VERBOSE
@@ -82,15 +98,15 @@ int st_int_write(struct st_data_s *st_gdata,
  * push the skb received to relevant
  * protocol stacks
  */
-void st_send_frame(enum proto_type protoid, struct st_data_s *st_gdata)
+void st_send_frame(unsigned char chnl_id, struct st_data_s *st_gdata)
 {
-	pr_info(" %s(prot:%d) ", __func__, protoid);
+	pr_debug(" %s(prot:%d) ", __func__, chnl_id);
 
 	if (unlikely
 	    (st_gdata == NULL || st_gdata->rx_skb == NULL
-	     || st_gdata->list[protoid] == NULL)) {
-		pr_err("protocol %d not registered, no data to send?",
-			   protoid);
+	     || st_gdata->list[chnl_id] == NULL)) {
+		pr_err("chnl_id %d not registered, no data to send?",
+			   chnl_id);
 		kfree_skb(st_gdata->rx_skb);
 		return;
 	}
@@ -99,17 +115,17 @@ void st_send_frame(enum proto_type protoid, struct st_data_s *st_gdata)
 	 * - should be just skb_queue_tail for the
 	 *   protocol stack driver
 	 */
-	if (likely(st_gdata->list[protoid]->recv != NULL)) {
+	if (likely(st_gdata->list[chnl_id]->recv != NULL)) {
 		if (unlikely
-			(st_gdata->list[protoid]->recv
-			(st_gdata->list[protoid]->priv_data, st_gdata->rx_skb)
+			(st_gdata->list[chnl_id]->recv
+			(st_gdata->list[chnl_id]->priv_data, st_gdata->rx_skb)
 			     != 0)) {
-			pr_err(" proto stack %d's ->recv failed", protoid);
+			pr_err(" proto stack %d's ->recv failed", chnl_id);
 			kfree_skb(st_gdata->rx_skb);
 			return;
 		}
 	} else {
-		pr_err(" proto stack %d's ->recv null", protoid);
+		pr_err(" proto stack %d's ->recv null", chnl_id);
 		kfree_skb(st_gdata->rx_skb);
 	}
 	return;
@@ -124,16 +140,22 @@ void st_reg_complete(struct st_data_s *st_gdata, char err)
 {
 	unsigned char i = 0;
 	pr_info(" %s ", __func__);
-	for (i = 0; i < ST_MAX; i++) {
+	for (i = 0; i < ST_MAX_CHANNELS; i++) {
 		if (likely(st_gdata != NULL && st_gdata->list[i] != NULL &&
-			   st_gdata->list[i]->reg_complete_cb != NULL))
+			   st_gdata->list[i]->reg_complete_cb != NULL)) {
 			st_gdata->list[i]->reg_complete_cb
 				(st_gdata->list[i]->priv_data, err);
+			pr_info("protocol %d's cb sent %d\n", i, err);
+			if (err) { /* cleanup registered protocol */
+				st_gdata->protos_registered--;
+				st_gdata->list[i] = NULL;
+			}
+		}
 	}
 }
 
 static inline int st_check_data_len(struct st_data_s *st_gdata,
-	int protoid, int len)
+	unsigned char chnl_id, int len)
 {
 	int room = skb_tailroom(st_gdata->rx_skb);
 
@@ -144,7 +166,7 @@ static inline int st_check_data_len(struct st_data_s *st_gdata,
 		 * has zero length payload. So, ask ST CORE to
 		 * forward the packet to protocol driver (BT/FM/GPS)
 		 */
-		st_send_frame(protoid, st_gdata);
+		st_send_frame(chnl_id, st_gdata);
 
 	} else if (len > room) {
 		/* Received packet's payload length is larger.
@@ -157,7 +179,7 @@ static inline int st_check_data_len(struct st_data_s *st_gdata,
 		/* Packet header has non-zero payload length and
 		 * we have enough space in created skb. Lets read
 		 * payload data */
-		st_gdata->rx_state = ST_BT_W4_DATA;
+		st_gdata->rx_state = ST_W4_DATA;
 		st_gdata->rx_count = len;
 		return len;
 	}
@@ -167,6 +189,7 @@ static inline int st_check_data_len(struct st_data_s *st_gdata,
 	st_gdata->rx_state = ST_W4_PACKET_TYPE;
 	st_gdata->rx_skb = NULL;
 	st_gdata->rx_count = 0;
+	st_gdata->rx_chnl = 0;
 
 	return 0;
 }
@@ -208,14 +231,12 @@ void st_int_recv(void *disc_data,
 	const unsigned char *data, long count)
 {
 	char *ptr;
-	struct hci_event_hdr *eh;
-	struct hci_acl_hdr *ah;
-	struct hci_sco_hdr *sh;
-	struct fm_event_hdr *fm;
-	struct gps_event_hdr *gps;
-	int len = 0, type = 0, dlen = 0;
-	static enum proto_type protoid = ST_MAX;
+	struct st_proto_s *proto;
+	unsigned short payload_len = 0;
+	int len = 0, type = 0;
+	unsigned char *plen;
 	struct st_data_s *st_gdata = (struct st_data_s *)disc_data;
+	unsigned long flags;
 
 	ptr = (char *)data;
 	/* tty_receive sent null ? */
@@ -224,10 +245,11 @@ void st_int_recv(void *disc_data,
 		return;
 	}
 
-	pr_info("count %ld rx_state %ld"
+	pr_debug("count %ld rx_state %ld"
 		   "rx_count %ld", count, st_gdata->rx_state,
 		   st_gdata->rx_count);
 
+	spin_lock_irqsave(&st_gdata->lock, flags);
 	/* Decode received bytes here */
 	while (count) {
 		if (st_gdata->rx_count) {
@@ -242,64 +264,36 @@ void st_int_recv(void *disc_data,
 
 			/* Check ST RX state machine , where are we? */
 			switch (st_gdata->rx_state) {
-
-				/* Waiting for complete packet ? */
-			case ST_BT_W4_DATA:
+			/* Waiting for complete packet ? */
+			case ST_W4_DATA:
 				pr_debug("Complete pkt received");
-
 				/* Ask ST CORE to forward
 				 * the packet to protocol driver */
-				st_send_frame(protoid, st_gdata);
+				st_send_frame(st_gdata->rx_chnl, st_gdata);
 
 				st_gdata->rx_state = ST_W4_PACKET_TYPE;
 				st_gdata->rx_skb = NULL;
-				protoid = ST_MAX;	/* is this required ? */
-				continue;
-
-				/* Waiting for Bluetooth event header ? */
-			case ST_BT_W4_EVENT_HDR:
-				eh = (struct hci_event_hdr *)st_gdata->rx_skb->
-				    data;
-
-				pr_debug("Event header: evt 0x%2.2x"
-					   "plen %d", eh->evt, eh->plen);
-
-				st_check_data_len(st_gdata, protoid, eh->plen);
-				continue;
-
-				/* Waiting for Bluetooth acl header ? */
-			case ST_BT_W4_ACL_HDR:
-				ah = (struct hci_acl_hdr *)st_gdata->rx_skb->
-				    data;
-				dlen = __le16_to_cpu(ah->dlen);
-
-				pr_info("ACL header: dlen %d", dlen);
-
-				st_check_data_len(st_gdata, protoid, dlen);
-				continue;
-
-				/* Waiting for Bluetooth sco header ? */
-			case ST_BT_W4_SCO_HDR:
-				sh = (struct hci_sco_hdr *)st_gdata->rx_skb->
-				    data;
-
-				pr_info("SCO header: dlen %d", sh->dlen);
-
-				st_check_data_len(st_gdata, protoid, sh->dlen);
-				continue;
-			case ST_FM_W4_EVENT_HDR:
-				fm = (struct fm_event_hdr *)st_gdata->rx_skb->
-				    data;
-				pr_info("FM Header: ");
-				st_check_data_len(st_gdata, ST_FM, fm->plen);
 				continue;
-				/* TODO : Add GPS packet machine logic here */
-			case ST_GPS_W4_EVENT_HDR:
-				/* [0x09 pkt hdr][R/W byte][2 byte len] */
-				gps = (struct gps_event_hdr *)st_gdata->rx_skb->
-				     data;
-				pr_info("GPS Header: ");
-				st_check_data_len(st_gdata, ST_GPS, gps->plen);
+			/* parse the header to know details */
+			case ST_W4_HEADER:
+				proto = st_gdata->list[st_gdata->rx_chnl];
+				plen =
+				&st_gdata->rx_skb->data
+				[proto->offset_len_in_hdr];
+				pr_debug("plen pointing to %x\n", *plen);
+				if (proto->len_size == 1)/* 1 byte len field */
+					payload_len = *(unsigned char *)plen;
+				else if (proto->len_size == 2)
+					payload_len =
+					__le16_to_cpu(*(unsigned short *)plen);
+				else
+					pr_info("%s: invalid length "
+					"for id %d\n",
+					__func__, proto->chnl_id);
+				st_check_data_len(st_gdata, proto->chnl_id,
+						payload_len);
+				pr_debug("off %d, pay len %d\n",
+					proto->offset_len_in_hdr, payload_len);
 				continue;
 			}	/* end of switch rx_state */
 		}
@@ -308,123 +302,56 @@ void st_int_recv(void *disc_data,
 		/* Check first byte of packet and identify module
 		 * owner (BT/FM/GPS) */
 		switch (*ptr) {
-
-			/* Bluetooth event packet? */
-		case HCI_EVENT_PKT:
-			pr_info("Event packet");
-			st_gdata->rx_state = ST_BT_W4_EVENT_HDR;
-			st_gdata->rx_count = HCI_EVENT_HDR_SIZE;
-			type = HCI_EVENT_PKT;
-			protoid = ST_BT;
-			break;
-
-			/* Bluetooth acl packet? */
-		case HCI_ACLDATA_PKT:
-			pr_info("ACL packet");
-			st_gdata->rx_state = ST_BT_W4_ACL_HDR;
-			st_gdata->rx_count = HCI_ACL_HDR_SIZE;
-			type = HCI_ACLDATA_PKT;
-			protoid = ST_BT;
-			break;
-
-			/* Bluetooth sco packet? */
-		case HCI_SCODATA_PKT:
-			pr_info("SCO packet");
-			st_gdata->rx_state = ST_BT_W4_SCO_HDR;
-			st_gdata->rx_count = HCI_SCO_HDR_SIZE;
-			type = HCI_SCODATA_PKT;
-			protoid = ST_BT;
-			break;
-
-			/* Channel 8(FM) packet? */
-		case ST_FM_CH8_PKT:
-			pr_info("FM CH8 packet");
-			type = ST_FM_CH8_PKT;
-			st_gdata->rx_state = ST_FM_W4_EVENT_HDR;
-			st_gdata->rx_count = FM_EVENT_HDR_SIZE;
-			protoid = ST_FM;
-			break;
-
-			/* Channel 9(GPS) packet? */
-		case 0x9:	/*ST_LL_GPS_CH9_PKT */
-			pr_info("GPS CH9 packet");
-			type = 0x9;	/* ST_LL_GPS_CH9_PKT; */
-			protoid = ST_GPS;
-			st_gdata->rx_state = ST_GPS_W4_EVENT_HDR;
-			st_gdata->rx_count = 3;	/* GPS_EVENT_HDR_SIZE -1*/
-			break;
 		case LL_SLEEP_IND:
 		case LL_SLEEP_ACK:
 		case LL_WAKE_UP_IND:
-			pr_info("PM packet");
+			pr_debug("PM packet");
 			/* this takes appropriate action based on
 			 * sleep state received --
 			 */
 			st_ll_sleep_state(st_gdata, *ptr);
+			/* if WAKEUP_IND collides copy from waitq to txq
+			 * and assume chip awake
+			 */
+			spin_unlock_irqrestore(&st_gdata->lock, flags);
+			if (st_ll_getstate(st_gdata) == ST_LL_AWAKE)
+				st_wakeup_ack(st_gdata, LL_WAKE_UP_ACK);
+			spin_lock_irqsave(&st_gdata->lock, flags);
+
 			ptr++;
 			count--;
 			continue;
 		case LL_WAKE_UP_ACK:
-			pr_info("PM packet");
+			pr_debug("PM packet");
+
+			spin_unlock_irqrestore(&st_gdata->lock, flags);
 			/* wake up ack received */
 			st_wakeup_ack(st_gdata, *ptr);
+			spin_lock_irqsave(&st_gdata->lock, flags);
+
 			ptr++;
 			count--;
 			continue;
 			/* Unknow packet? */
 		default:
-			pr_err("Unknown packet type %2.2x", (__u8) *ptr);
-			ptr++;
-			count--;
-			continue;
+			type = *ptr;
+			st_gdata->rx_skb = alloc_skb(
+					st_gdata->list[type]->max_frame_size,
+					GFP_ATOMIC);
+			skb_reserve(st_gdata->rx_skb,
+					st_gdata->list[type]->reserve);
+			/* next 2 required for BT only */
+			st_gdata->rx_skb->cb[0] = type; /*pkt_type*/
+			st_gdata->rx_skb->cb[1] = 0; /*incoming*/
+			st_gdata->rx_chnl = *ptr;
+			st_gdata->rx_state = ST_W4_HEADER;
+			st_gdata->rx_count = st_gdata->list[type]->hdr_len;
+			pr_debug("rx_count %ld\n", st_gdata->rx_count);
 		};
 		ptr++;
 		count--;
-
-		switch (protoid) {
-		case ST_BT:
-			/* Allocate new packet to hold received data */
-			st_gdata->rx_skb =
-			    bt_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC);
-			if (!st_gdata->rx_skb) {
-				pr_err("Can't allocate mem for new packet");
-				st_gdata->rx_state = ST_W4_PACKET_TYPE;
-				st_gdata->rx_count = 0;
-				return;
-			}
-			bt_cb(st_gdata->rx_skb)->pkt_type = type;
-			break;
-		case ST_FM:	/* for FM */
-			st_gdata->rx_skb =
-			    alloc_skb(FM_MAX_FRAME_SIZE, GFP_ATOMIC);
-			if (!st_gdata->rx_skb) {
-				pr_err("Can't allocate mem for new packet");
-				st_gdata->rx_state = ST_W4_PACKET_TYPE;
-				st_gdata->rx_count = 0;
-				return;
-			}
-			/* place holder 0x08 */
-			skb_reserve(st_gdata->rx_skb, 1);
-			st_gdata->rx_skb->cb[0] = ST_FM_CH8_PKT;
-			break;
-		case ST_GPS:
-			/* for GPS */
-			st_gdata->rx_skb =
-			    alloc_skb(100 /*GPS_MAX_FRAME_SIZE */ , GFP_ATOMIC);
-			if (!st_gdata->rx_skb) {
-				pr_err("Can't allocate mem for new packet");
-				st_gdata->rx_state = ST_W4_PACKET_TYPE;
-				st_gdata->rx_count = 0;
-				return;
-			}
-			/* place holder 0x09 */
-			skb_reserve(st_gdata->rx_skb, 1);
-			st_gdata->rx_skb->cb[0] = 0x09;	/*ST_GPS_CH9_PKT; */
-			break;
-		case ST_MAX:
-			break;
-		}
 	}
+	spin_unlock_irqrestore(&st_gdata->lock, flags);
 	pr_debug("done %s", __func__);
 	return;
 }
@@ -466,7 +393,7 @@ void st_int_enqueue(struct st_data_s *st_gdata, struct sk_buff *skb)
 
 	switch (st_ll_getstate(st_gdata)) {
 	case ST_LL_AWAKE:
-		pr_info("ST LL is AWAKE, sending normally");
+		pr_debug("ST LL is AWAKE, sending normally");
 		skb_queue_tail(&st_gdata->txq, skb);
 		break;
 	case ST_LL_ASLEEP_TO_AWAKE:
@@ -506,7 +433,7 @@ void st_tx_wakeup(struct st_data_s *st_data)
 	pr_debug("%s", __func__);
 	/* check for sending & set flag sending here */
 	if (test_and_set_bit(ST_TX_SENDING, &st_data->tx_state)) {
-		pr_info("ST already sending");
+		pr_debug("ST already sending");
 		/* keep sending */
 		set_bit(ST_TX_WAKEUP, &st_data->tx_state);
 		return;
@@ -565,20 +492,20 @@ long st_register(struct st_proto_s *new_proto)
 	unsigned long flags = 0;
 
 	st_kim_ref(&st_gdata, 0);
-	pr_info("%s(%d) ", __func__, new_proto->type);
+	pr_info("%s(%d) ", __func__, new_proto->chnl_id);
 	if (st_gdata == NULL || new_proto == NULL || new_proto->recv == NULL
 	    || new_proto->reg_complete_cb == NULL) {
 		pr_err("gdata/new_proto/recv or reg_complete_cb not ready");
-		return -1;
+		return -EINVAL;
 	}
 
-	if (new_proto->type < ST_BT || new_proto->type >= ST_MAX) {
-		pr_err("protocol %d not supported", new_proto->type);
+	if (new_proto->chnl_id >= ST_MAX_CHANNELS) {
+		pr_err("chnl_id %d not supported", new_proto->chnl_id);
 		return -EPROTONOSUPPORT;
 	}
 
-	if (st_gdata->list[new_proto->type] != NULL) {
-		pr_err("protocol %d already registered", new_proto->type);
+	if (st_gdata->list[new_proto->chnl_id] != NULL) {
+		pr_err("chnl_id %d already registered", new_proto->chnl_id);
 		return -EALREADY;
 	}
 
@@ -586,11 +513,10 @@ long st_register(struct st_proto_s *new_proto)
 	spin_lock_irqsave(&st_gdata->lock, flags);
 
 	if (test_bit(ST_REG_IN_PROGRESS, &st_gdata->st_state)) {
-		pr_info(" ST_REG_IN_PROGRESS:%d ", new_proto->type);
+		pr_info(" ST_REG_IN_PROGRESS:%d ", new_proto->chnl_id);
 		/* fw download in progress */
-		st_kim_chip_toggle(new_proto->type, KIM_GPIO_ACTIVE);
 
-		st_gdata->list[new_proto->type] = new_proto;
+		add_channel_to_table(st_gdata, new_proto);
 		st_gdata->protos_registered++;
 		new_proto->write = st_write;
 
@@ -598,7 +524,7 @@ long st_register(struct st_proto_s *new_proto)
 		spin_unlock_irqrestore(&st_gdata->lock, flags);
 		return -EINPROGRESS;
 	} else if (st_gdata->protos_registered == ST_EMPTY) {
-		pr_info(" protocol list empty :%d ", new_proto->type);
+		pr_info(" chnl_id list empty :%d ", new_proto->chnl_id);
 		set_bit(ST_REG_IN_PROGRESS, &st_gdata->st_state);
 		st_recv = st_kim_recv;
 
@@ -616,16 +542,11 @@ long st_register(struct st_proto_s *new_proto)
 			if ((st_gdata->protos_registered != ST_EMPTY) &&
 			    (test_bit(ST_REG_PENDING, &st_gdata->st_state))) {
 				pr_err(" KIM failure complete callback ");
-				st_reg_complete(st_gdata, -1);
+				st_reg_complete(st_gdata, err);
 			}
-
-			return -1;
+			return -EINVAL;
 		}
 
-		/* the protocol might require other gpios to be toggled
-		 */
-		st_kim_chip_toggle(new_proto->type, KIM_GPIO_ACTIVE);
-
 		clear_bit(ST_REG_IN_PROGRESS, &st_gdata->st_state);
 		st_recv = st_int_recv;
 
@@ -642,14 +563,14 @@ long st_register(struct st_proto_s *new_proto)
 		/* check for already registered once more,
 		 * since the above check is old
 		 */
-		if (st_gdata->list[new_proto->type] != NULL) {
+		if (st_gdata->list[new_proto->chnl_id] != NULL) {
 			pr_err(" proto %d already registered ",
-				   new_proto->type);
+				   new_proto->chnl_id);
 			return -EALREADY;
 		}
 
 		spin_lock_irqsave(&st_gdata->lock, flags);
-		st_gdata->list[new_proto->type] = new_proto;
+		add_channel_to_table(st_gdata, new_proto);
 		st_gdata->protos_registered++;
 		new_proto->write = st_write;
 		spin_unlock_irqrestore(&st_gdata->lock, flags);
@@ -657,22 +578,7 @@ long st_register(struct st_proto_s *new_proto)
 	}
 	/* if fw is already downloaded & new stack registers protocol */
 	else {
-		switch (new_proto->type) {
-		case ST_BT:
-			/* do nothing */
-			break;
-		case ST_FM:
-		case ST_GPS:
-			st_kim_chip_toggle(new_proto->type, KIM_GPIO_ACTIVE);
-			break;
-		case ST_MAX:
-		default:
-			pr_err("%d protocol not supported",
-				   new_proto->type);
-			spin_unlock_irqrestore(&st_gdata->lock, flags);
-			return -EPROTONOSUPPORT;
-		}
-		st_gdata->list[new_proto->type] = new_proto;
+		add_channel_to_table(st_gdata, new_proto);
 		st_gdata->protos_registered++;
 		new_proto->write = st_write;
 
@@ -680,48 +586,42 @@ long st_register(struct st_proto_s *new_proto)
 		spin_unlock_irqrestore(&st_gdata->lock, flags);
 		return err;
 	}
-	pr_debug("done %s(%d) ", __func__, new_proto->type);
+	pr_debug("done %s(%d) ", __func__, new_proto->chnl_id);
 }
 EXPORT_SYMBOL_GPL(st_register);
 
 /* to unregister a protocol -
  * to be called from protocol stack driver
  */
-long st_unregister(enum proto_type type)
+long st_unregister(struct st_proto_s *proto)
 {
 	long err = 0;
 	unsigned long flags = 0;
 	struct st_data_s	*st_gdata;
 
-	pr_debug("%s: %d ", __func__, type);
+	pr_debug("%s: %d ", __func__, proto->chnl_id);
 
 	st_kim_ref(&st_gdata, 0);
-	if (type < ST_BT || type >= ST_MAX) {
-		pr_err(" protocol %d not supported", type);
+	if (proto->chnl_id >= ST_MAX_CHANNELS) {
+		pr_err(" chnl_id %d not supported", proto->chnl_id);
 		return -EPROTONOSUPPORT;
 	}
 
 	spin_lock_irqsave(&st_gdata->lock, flags);
 
-	if (st_gdata->list[type] == NULL) {
-		pr_err(" protocol %d not registered", type);
+	if (st_gdata->list[proto->chnl_id] == NULL) {
+		pr_err(" chnl_id %d not registered", proto->chnl_id);
 		spin_unlock_irqrestore(&st_gdata->lock, flags);
 		return -EPROTONOSUPPORT;
 	}
 
 	st_gdata->protos_registered--;
-	st_gdata->list[type] = NULL;
-
-	/* kim ignores BT in the below function
-	 * and handles the rest, BT is toggled
-	 * only in kim_start and kim_stop
-	 */
-	st_kim_chip_toggle(type, KIM_GPIO_INACTIVE);
+	remove_channel_from_table(st_gdata, proto);
 	spin_unlock_irqrestore(&st_gdata->lock, flags);
 
 	if ((st_gdata->protos_registered == ST_EMPTY) &&
 	    (!test_bit(ST_REG_PENDING, &st_gdata->st_state))) {
-		pr_info(" all protocols unregistered ");
+		pr_info(" all chnl_ids unregistered ");
 
 		/* stop traffic on tty */
 		if (st_gdata->tty) {
@@ -729,7 +629,7 @@ long st_unregister(enum proto_type type)
 			stop_tty(st_gdata->tty);
 		}
 
-		/* all protocols now unregistered */
+		/* all chnl_ids now unregistered */
 		st_kim_stop(st_gdata->kim_data);
 		/* disable ST LL */
 		st_ll_disable(st_gdata);
@@ -745,7 +645,7 @@ long st_write(struct sk_buff *skb)
 {
 	struct st_data_s *st_gdata;
 #ifdef DEBUG
-	enum proto_type protoid = ST_MAX;
+	unsigned char chnl_id = ST_MAX_CHANNELS;
 #endif
 	long len;
 
@@ -753,26 +653,14 @@ long st_write(struct sk_buff *skb)
 	if (unlikely(skb == NULL || st_gdata == NULL
 		|| st_gdata->tty == NULL)) {
 		pr_err("data/tty unavailable to perform write");
-		return -1;
+		return -EINVAL;
 	}
 #ifdef DEBUG			/* open-up skb to read the 1st byte */
-	switch (skb->data[0]) {
-	case HCI_COMMAND_PKT:
-	case HCI_ACLDATA_PKT:
-	case HCI_SCODATA_PKT:
-		protoid = ST_BT;
-		break;
-	case ST_FM_CH8_PKT:
-		protoid = ST_FM;
-		break;
-	case 0x09:
-		protoid = ST_GPS;
-		break;
-	}
-	if (unlikely(st_gdata->list[protoid] == NULL)) {
-		pr_err(" protocol %d not registered, and writing? ",
-			   protoid);
-		return -1;
+	chnl_id = skb->data[0];
+	if (unlikely(st_gdata->list[chnl_id] == NULL)) {
+		pr_err(" chnl_id %d not registered, and writing? ",
+			   chnl_id);
+		return -EINVAL;
 	}
 #endif
 	pr_debug("%d to be written", skb->len);
@@ -824,7 +712,7 @@ static int st_tty_open(struct tty_struct *tty)
 
 static void st_tty_close(struct tty_struct *tty)
 {
-	unsigned char i = ST_MAX;
+	unsigned char i = ST_MAX_CHANNELS;
 	unsigned long flags = 0;
 	struct	st_data_s *st_gdata = tty->disc_data;
 
@@ -835,7 +723,7 @@ static void st_tty_close(struct tty_struct *tty)
 	 * un-installed for some reason - what should be done ?
 	 */
 	spin_lock_irqsave(&st_gdata->lock, flags);
-	for (i = ST_BT; i < ST_MAX; i++) {
+	for (i = ST_BT; i < ST_MAX_CHANNELS; i++) {
 		if (st_gdata->list[i] != NULL)
 			pr_err("%d not un-registered", i);
 		st_gdata->list[i] = NULL;
@@ -869,7 +757,6 @@ static void st_tty_close(struct tty_struct *tty)
 static void st_tty_receive(struct tty_struct *tty, const unsigned char *data,
 			   char *tty_flags, int count)
 {
-
 #ifdef VERBOSE
 	print_hex_dump(KERN_DEBUG, ">in>", DUMP_PREFIX_NONE,
 		16, 1, data, count, 0);
@@ -960,7 +847,7 @@ int st_core_init(struct st_data_s **core_data)
 		err = tty_unregister_ldisc(N_TI_WL);
 		if (err)
 			pr_err("unable to un-register ldisc");
-		return -1;
+		return err;
 	}
 	*core_data = st_gdata;
 	return 0;
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index 73b6c8b0e869..9ee4c788aa69 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -30,50 +30,12 @@
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/sched.h>
-#include <linux/rfkill.h>
-
-/* understand BT events for fw response */
-#include <net/bluetooth/bluetooth.h>
-#include <net/bluetooth/hci_core.h>
-#include <net/bluetooth/hci.h>
+#include <linux/tty.h>
 
+#include <linux/skbuff.h>
 #include <linux/ti_wilink_st.h>
 
 
-static int kim_probe(struct platform_device *pdev);
-static int kim_remove(struct platform_device *pdev);
-
-/* KIM platform device driver structure */
-static struct platform_driver kim_platform_driver = {
-	.probe = kim_probe,
-	.remove = kim_remove,
-	/* TODO: ST driver power management during suspend/resume ?
-	 */
-#if 0
-	.suspend = kim_suspend,
-	.resume = kim_resume,
-#endif
-	.driver = {
-		   .name = "kim",
-		   .owner = THIS_MODULE,
-		   },
-};
-
-static int kim_toggle_radio(void*, bool);
-static const struct rfkill_ops kim_rfkill_ops = {
-	.set_block = kim_toggle_radio,
-};
-
-/* strings to be used for rfkill entries and by
- * ST Core to be used for sysfs debug entry
- */
-#define PROTO_ENTRY(type, name)	name
-const unsigned char *protocol_names[] = {
-	PROTO_ENTRY(ST_BT, "Bluetooth"),
-	PROTO_ENTRY(ST_FM, "FM"),
-	PROTO_ENTRY(ST_GPS, "GPS"),
-};
-
 #define MAX_ST_DEVICES	3	/* Imagine 1 on each UART for now */
 static struct platform_device *st_kim_devices[MAX_ST_DEVICES];
 
@@ -134,7 +96,7 @@ static inline int kim_check_data_len(struct kim_data_s *kim_gdata, int len)
 		/* Packet header has non-zero payload length and
 		 * we have enough space in created skb. Lets read
 		 * payload data */
-		kim_gdata->rx_state = ST_BT_W4_DATA;
+		kim_gdata->rx_state = ST_W4_DATA;
 		kim_gdata->rx_count = len;
 		return len;
 	}
@@ -158,8 +120,8 @@ void kim_int_recv(struct kim_data_s *kim_gdata,
 	const unsigned char *data, long count)
 {
 	const unsigned char *ptr;
-	struct hci_event_hdr *eh;
 	int len = 0, type = 0;
+	unsigned char *plen;
 
 	pr_debug("%s", __func__);
 	/* Decode received bytes here */
@@ -183,29 +145,27 @@ void kim_int_recv(struct kim_data_s *kim_gdata,
 			/* Check ST RX state machine , where are we? */
 			switch (kim_gdata->rx_state) {
 				/* Waiting for complete packet ? */
-			case ST_BT_W4_DATA:
+			case ST_W4_DATA:
 				pr_debug("Complete pkt received");
 				validate_firmware_response(kim_gdata);
 				kim_gdata->rx_state = ST_W4_PACKET_TYPE;
 				kim_gdata->rx_skb = NULL;
 				continue;
 				/* Waiting for Bluetooth event header ? */
-			case ST_BT_W4_EVENT_HDR:
-				eh = (struct hci_event_hdr *)kim_gdata->
-				    rx_skb->data;
-				pr_debug("Event header: evt 0x%2.2x"
-					   "plen %d", eh->evt, eh->plen);
-				kim_check_data_len(kim_gdata, eh->plen);
+			case ST_W4_HEADER:
+				plen =
+				(unsigned char *)&kim_gdata->rx_skb->data[1];
+				pr_debug("event hdr: plen 0x%02x\n", *plen);
+				kim_check_data_len(kim_gdata, *plen);
 				continue;
 			}	/* end of switch */
 		}		/* end of if rx_state */
 		switch (*ptr) {
 			/* Bluetooth event packet? */
-		case HCI_EVENT_PKT:
-			pr_info("Event packet");
-			kim_gdata->rx_state = ST_BT_W4_EVENT_HDR;
-			kim_gdata->rx_count = HCI_EVENT_HDR_SIZE;
-			type = HCI_EVENT_PKT;
+		case 0x04:
+			kim_gdata->rx_state = ST_W4_HEADER;
+			kim_gdata->rx_count = 2;
+			type = *ptr;
 			break;
 		default:
 			pr_info("unknown packet");
@@ -216,16 +176,18 @@ void kim_int_recv(struct kim_data_s *kim_gdata,
 		ptr++;
 		count--;
 		kim_gdata->rx_skb =
-		    bt_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC);
+			alloc_skb(1024+8, GFP_ATOMIC);
 		if (!kim_gdata->rx_skb) {
 			pr_err("can't allocate mem for new packet");
 			kim_gdata->rx_state = ST_W4_PACKET_TYPE;
 			kim_gdata->rx_count = 0;
 			return;
 		}
-		bt_cb(kim_gdata->rx_skb)->pkt_type = type;
+		skb_reserve(kim_gdata->rx_skb, 8);
+		kim_gdata->rx_skb->cb[0] = 4;
+		kim_gdata->rx_skb->cb[1] = 0;
+
 	}
-	pr_info("done %s", __func__);
 	return;
 }
 
@@ -239,13 +201,13 @@ static long read_local_version(struct kim_data_s *kim_gdata, char *bts_scr_name)
 	INIT_COMPLETION(kim_gdata->kim_rcvd);
 	if (4 != st_int_write(kim_gdata->core_data, read_ver_cmd, 4)) {
 		pr_err("kim: couldn't write 4 bytes");
-		return -1;
+		return -EIO;
 	}
 
 	if (!wait_for_completion_timeout
 	    (&kim_gdata->kim_rcvd, msecs_to_jiffies(CMD_RESP_TIME))) {
 		pr_err(" waiting for ver info- timed out ");
-		return -1;
+		return -ETIMEDOUT;
 	}
 
 	version =
@@ -270,6 +232,26 @@ static long read_local_version(struct kim_data_s *kim_gdata, char *bts_scr_name)
 	return 0;
 }
 
+void skip_change_remote_baud(unsigned char **ptr, long *len)
+{
+	unsigned char *nxt_action, *cur_action;
+	cur_action = *ptr;
+
+	nxt_action = cur_action + sizeof(struct bts_action) +
+		((struct bts_action *) cur_action)->size;
+
+	if (((struct bts_action *) nxt_action)->type != ACTION_WAIT_EVENT) {
+		pr_err("invalid action after change remote baud command");
+	} else {
+		*ptr = *ptr + sizeof(struct bts_action) +
+			((struct bts_action *)nxt_action)->size;
+		*len = *len - (sizeof(struct bts_action) +
+				((struct bts_action *)nxt_action)->size);
+		/* warn user on not commenting these in firmware */
+		pr_warn("skipping the wait event of change remote baud");
+	}
+}
+
 /**
  * download_firmware -
  *	internal function which parses through the .bts firmware
@@ -282,6 +264,9 @@ static long download_firmware(struct kim_data_s *kim_gdata)
 	unsigned char *ptr = NULL;
 	unsigned char *action_ptr = NULL;
 	unsigned char bts_scr_name[30] = { 0 };	/* 30 char long bts scr name? */
+	int wr_room_space;
+	int cmd_size;
+	unsigned long timeout;
 
 	err = read_local_version(kim_gdata, bts_scr_name);
 	if (err != 0) {
@@ -295,7 +280,7 @@ static long download_firmware(struct kim_data_s *kim_gdata)
 		     (kim_gdata->fw_entry->size == 0))) {
 		pr_err(" request_firmware failed(errno %ld) for %s", err,
 			   bts_scr_name);
-		return -1;
+		return -EINVAL;
 	}
 	ptr = (void *)kim_gdata->fw_entry->data;
 	len = kim_gdata->fw_entry->size;
@@ -318,29 +303,72 @@ static long download_firmware(struct kim_data_s *kim_gdata)
 			     0xFF36)) {
 				/* ignore remote change
 				 * baud rate HCI VS command */
-				pr_err
-				    (" change remote baud"
+				pr_warn("change remote baud"
 				    " rate command in firmware");
+				skip_change_remote_baud(&ptr, &len);
 				break;
 			}
+			/*
+			 * Make sure we have enough free space in uart
+			 * tx buffer to write current firmware command
+			 */
+			cmd_size = ((struct bts_action *)ptr)->size;
+			timeout = jiffies + msecs_to_jiffies(CMD_WR_TIME);
+			do {
+				wr_room_space =
+					st_get_uart_wr_room(kim_gdata->core_data);
+				if (wr_room_space < 0) {
+					pr_err("Unable to get free "
+							"space info from uart tx buffer");
+					release_firmware(kim_gdata->fw_entry);
+					return wr_room_space;
+				}
+				mdelay(1); /* wait 1ms before checking room */
+			} while ((wr_room_space < cmd_size) &&
+					time_before(jiffies, timeout));
+
+			/* Timeout happened ? */
+			if (time_after_eq(jiffies, timeout)) {
+				pr_err("Timeout while waiting for free "
+						"free space in uart tx buffer");
+				release_firmware(kim_gdata->fw_entry);
+				return -ETIMEDOUT;
+			}
 
-			INIT_COMPLETION(kim_gdata->kim_rcvd);
+			/*
+			 * Free space found in uart buffer, call st_int_write
+			 * to send current firmware command to the uart tx
+			 * buffer.
+			 */
 			err = st_int_write(kim_gdata->core_data,
 			((struct bts_action_send *)action_ptr)->data,
 					   ((struct bts_action *)ptr)->size);
 			if (unlikely(err < 0)) {
 				release_firmware(kim_gdata->fw_entry);
-				return -1;
+				return err;
 			}
+			/*
+			 * Check number of bytes written to the uart tx buffer
+			 * and requested command write size
+			 */
+			if (err != cmd_size) {
+				pr_err("Number of bytes written to uart "
+						"tx buffer are not matching with "
+						"requested cmd write size");
+				release_firmware(kim_gdata->fw_entry);
+				return -EIO;
+			}
+			break;
+		case ACTION_WAIT_EVENT:  /* wait */
 			if (!wait_for_completion_timeout
-			    (&kim_gdata->kim_rcvd,
-			     msecs_to_jiffies(CMD_RESP_TIME))) {
-				pr_err
-				    (" response timeout during fw download ");
+					(&kim_gdata->kim_rcvd,
+					 msecs_to_jiffies(CMD_RESP_TIME))) {
+				pr_err("response timeout during fw download ");
 				/* timed out */
 				release_firmware(kim_gdata->fw_entry);
-				return -1;
+				return -ETIMEDOUT;
 			}
+			INIT_COMPLETION(kim_gdata->kim_rcvd);
 			break;
 		case ACTION_DELAY:	/* sleep */
 			pr_info("sleep command in scr");
@@ -362,50 +390,6 @@ static long download_firmware(struct kim_data_s *kim_gdata)
 
 /**********************************************************************/
 /* functions called from ST core */
-/* function to toggle the GPIO
- * needs to know whether the GPIO is active high or active low
- */
-void st_kim_chip_toggle(enum proto_type type, enum kim_gpio_state state)
-{
-	struct platform_device	*kim_pdev;
-	struct kim_data_s	*kim_gdata;
-	pr_info(" %s ", __func__);
-
-	kim_pdev = st_get_plat_device(0);
-	kim_gdata = dev_get_drvdata(&kim_pdev->dev);
-
-	if (kim_gdata->gpios[type] == -1) {
-		pr_info(" gpio not requested for protocol %s",
-			   protocol_names[type]);
-		return;
-	}
-	switch (type) {
-	case ST_BT:
-		/*Do Nothing */
-		break;
-
-	case ST_FM:
-		if (state == KIM_GPIO_ACTIVE)
-			gpio_set_value(kim_gdata->gpios[ST_FM], GPIO_LOW);
-		else
-			gpio_set_value(kim_gdata->gpios[ST_FM], GPIO_HIGH);
-		break;
-
-	case ST_GPS:
-		if (state == KIM_GPIO_ACTIVE)
-			gpio_set_value(kim_gdata->gpios[ST_GPS], GPIO_HIGH);
-		else
-			gpio_set_value(kim_gdata->gpios[ST_GPS], GPIO_LOW);
-		break;
-
-	case ST_MAX:
-	default:
-		break;
-	}
-
-	return;
-}
-
 /* called from ST Core, when REG_IN_PROGRESS (registration in progress)
  * can be because of
  * 1. response to read local version
@@ -416,7 +400,6 @@ void st_kim_recv(void *disc_data, const unsigned char *data, long count)
 	struct st_data_s	*st_gdata = (struct st_data_s *)disc_data;
 	struct kim_data_s	*kim_gdata = st_gdata->kim_data;
 
-	pr_info(" %s ", __func__);
 	/* copy to local buffer */
 	if (unlikely(data[4] == 0x01 && data[5] == 0x10 && data[0] == 0x04)) {
 		/* must be the read_ver_cmd */
@@ -455,35 +438,28 @@ long st_kim_start(void *kim_data)
 	pr_info(" %s", __func__);
 
 	do {
-		/* TODO: this is only because rfkill sub-system
-		 * doesn't send events to user-space if the state
-		 * isn't changed
-		 */
-		rfkill_set_hw_state(kim_gdata->rfkill[ST_BT], 1);
 		/* Configure BT nShutdown to HIGH state */
-		gpio_set_value(kim_gdata->gpios[ST_BT], GPIO_LOW);
+		gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
 		mdelay(5);	/* FIXME: a proper toggle */
-		gpio_set_value(kim_gdata->gpios[ST_BT], GPIO_HIGH);
+		gpio_set_value(kim_gdata->nshutdown, GPIO_HIGH);
 		mdelay(100);
 		/* re-initialize the completion */
 		INIT_COMPLETION(kim_gdata->ldisc_installed);
-#if 0 /* older way of signalling user-space UIM */
-		/* send signal to UIM */
-		err = kill_pid(find_get_pid(kim_gdata->uim_pid), SIGUSR2, 0);
-		if (err != 0) {
-			pr_info(" sending SIGUSR2 to uim failed %ld", err);
-			err = -1;
-			continue;
-		}
-#endif
-		/* unblock and send event to UIM via /dev/rfkill */
-		rfkill_set_hw_state(kim_gdata->rfkill[ST_BT], 0);
+		/* send notification to UIM */
+		kim_gdata->ldisc_install = 1;
+		pr_info("ldisc_install = 1");
+		sysfs_notify(&kim_gdata->kim_pdev->dev.kobj,
+				NULL, "install");
 		/* wait for ldisc to be installed */
 		err = wait_for_completion_timeout(&kim_gdata->ldisc_installed,
 				msecs_to_jiffies(LDISC_TIME));
 		if (!err) {	/* timeout */
 			pr_err("line disc installation timed out ");
-			err = -1;
+			kim_gdata->ldisc_install = 0;
+			pr_info("ldisc_install = 0");
+			sysfs_notify(&kim_gdata->kim_pdev->dev.kobj,
+					NULL, "install");
+			err = -ETIMEDOUT;
 			continue;
 		} else {
 			/* ldisc installed now */
@@ -491,6 +467,10 @@ long st_kim_start(void *kim_data)
 			err = download_firmware(kim_gdata);
 			if (err != 0) {
 				pr_err("download firmware failed");
+				kim_gdata->ldisc_install = 0;
+				pr_info("ldisc_install = 0");
+				sysfs_notify(&kim_gdata->kim_pdev->dev.kobj,
+						NULL, "install");
 				continue;
 			} else {	/* on success don't retry */
 				break;
@@ -510,31 +490,30 @@ long st_kim_stop(void *kim_data)
 	struct kim_data_s	*kim_gdata = (struct kim_data_s *)kim_data;
 
 	INIT_COMPLETION(kim_gdata->ldisc_installed);
-#if 0 /* older way of signalling user-space UIM */
-	/* send signal to UIM */
-	err = kill_pid(find_get_pid(kim_gdata->uim_pid), SIGUSR2, 1);
-	if (err != 0) {
-		pr_err("sending SIGUSR2 to uim failed %ld", err);
-		return -1;
-	}
-#endif
-	/* set BT rfkill to be blocked */
-	err = rfkill_set_hw_state(kim_gdata->rfkill[ST_BT], 1);
+
+	/* Flush any pending characters in the driver and discipline. */
+	tty_ldisc_flush(kim_gdata->core_data->tty);
+	tty_driver_flush_buffer(kim_gdata->core_data->tty);
+
+	/* send uninstall notification to UIM */
+	pr_info("ldisc_install = 0");
+	kim_gdata->ldisc_install = 0;
+	sysfs_notify(&kim_gdata->kim_pdev->dev.kobj, NULL, "install");
 
 	/* wait for ldisc to be un-installed */
 	err = wait_for_completion_timeout(&kim_gdata->ldisc_installed,
 			msecs_to_jiffies(LDISC_TIME));
 	if (!err) {		/* timeout */
 		pr_err(" timed out waiting for ldisc to be un-installed");
-		return -1;
+		return -ETIMEDOUT;
 	}
 
 	/* By default configure BT nShutdown to LOW state */
-	gpio_set_value(kim_gdata->gpios[ST_BT], GPIO_LOW);
+	gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
 	mdelay(1);
-	gpio_set_value(kim_gdata->gpios[ST_BT], GPIO_HIGH);
+	gpio_set_value(kim_gdata->nshutdown, GPIO_HIGH);
 	mdelay(1);
-	gpio_set_value(kim_gdata->gpios[ST_BT], GPIO_LOW);
+	gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
 	return err;
 }
 
@@ -558,33 +537,59 @@ static int show_list(struct seq_file *s, void *unused)
 	return 0;
 }
 
-/* function called from rfkill subsystem, when someone from
- * user space would write 0/1 on the sysfs entry
- * /sys/class/rfkill/rfkill0,1,3/state
- */
-static int kim_toggle_radio(void *data, bool blocked)
+static ssize_t show_install(struct device *dev,
+		struct device_attribute *attr, char *buf)
 {
-	enum proto_type type = *((enum proto_type *)data);
-	pr_debug(" %s: %d ", __func__, type);
-
-	switch (type) {
-	case ST_BT:
-		/* do nothing */
-	break;
-	case ST_FM:
-	case ST_GPS:
-		if (blocked)
-			st_kim_chip_toggle(type, KIM_GPIO_INACTIVE);
-		else
-			st_kim_chip_toggle(type, KIM_GPIO_ACTIVE);
-	break;
-	case ST_MAX:
-		pr_err(" wrong proto type ");
-	break;
-	}
-	return 0;
+	struct kim_data_s *kim_data = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", kim_data->ldisc_install);
 }
 
+static ssize_t show_dev_name(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kim_data_s *kim_data = dev_get_drvdata(dev);
+	return sprintf(buf, "%s\n", kim_data->dev_name);
+}
+
+static ssize_t show_baud_rate(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kim_data_s *kim_data = dev_get_drvdata(dev);
+	return sprintf(buf, "%ld\n", kim_data->baud_rate);
+}
+
+static ssize_t show_flow_cntrl(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct kim_data_s *kim_data = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", kim_data->flow_cntrl);
+}
+
+/* structures specific for sysfs entries */
+static struct kobj_attribute ldisc_install =
+__ATTR(install, 0444, (void *)show_install, NULL);
+
+static struct kobj_attribute uart_dev_name =
+__ATTR(dev_name, 0444, (void *)show_dev_name, NULL);
+
+static struct kobj_attribute uart_baud_rate =
+__ATTR(baud_rate, 0444, (void *)show_baud_rate, NULL);
+
+static struct kobj_attribute uart_flow_cntrl =
+__ATTR(flow_cntrl, 0444, (void *)show_flow_cntrl, NULL);
+
+static struct attribute *uim_attrs[] = {
+	&ldisc_install.attr,
+	&uart_dev_name.attr,
+	&uart_baud_rate.attr,
+	&uart_flow_cntrl.attr,
+	NULL,
+};
+
+static struct attribute_group uim_attr_grp = {
+	.attrs = uim_attrs,
+};
+
 /**
  * st_kim_ref - reference the core's data
  *	This references the per-ST platform device in the arch/xx/
@@ -637,9 +642,8 @@ struct dentry *kim_debugfs_dir;
 static int kim_probe(struct platform_device *pdev)
 {
 	long status;
-	long proto;
-	long *gpios = pdev->dev.platform_data;
 	struct kim_data_s	*kim_gdata;
+	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
 
 	if ((pdev->id != -1) && (pdev->id < MAX_ST_DEVICES)) {
 		/* multiple devices could exist */
@@ -659,44 +663,24 @@ static int kim_probe(struct platform_device *pdev)
 	status = st_core_init(&kim_gdata->core_data);
 	if (status != 0) {
 		pr_err(" ST core init failed");
-		return -1;
+		return -EIO;
 	}
 	/* refer to itself */
 	kim_gdata->core_data->kim_data = kim_gdata;
 
-	for (proto = 0; proto < ST_MAX; proto++) {
-		kim_gdata->gpios[proto] = gpios[proto];
-		pr_info(" %ld gpio to be requested", gpios[proto]);
+	/* Claim the chip enable nShutdown gpio from the system */
+	kim_gdata->nshutdown = pdata->nshutdown_gpio;
+	status = gpio_request(kim_gdata->nshutdown, "kim");
+	if (unlikely(status)) {
+		pr_err(" gpio %ld request failed ", kim_gdata->nshutdown);
+		return status;
 	}
 
-	for (proto = 0; (proto < ST_MAX) && (gpios[proto] != -1); proto++) {
-		/* Claim the Bluetooth/FM/GPIO
-		 * nShutdown gpio from the system
-		 */
-		status = gpio_request(gpios[proto], "kim");
-		if (unlikely(status)) {
-			pr_err(" gpio %ld request failed ", gpios[proto]);
-			proto -= 1;
-			while (proto >= 0) {
-				if (gpios[proto] != -1)
-					gpio_free(gpios[proto]);
-			}
-			return status;
-		}
-
-		/* Configure nShutdown GPIO as output=0 */
-		status =
-		    gpio_direction_output(gpios[proto], 0);
-		if (unlikely(status)) {
-			pr_err(" unable to configure gpio %ld",
-				   gpios[proto]);
-			proto -= 1;
-			while (proto >= 0) {
-				if (gpios[proto] != -1)
-					gpio_free(gpios[proto]);
-			}
-			return status;
-		}
+	/* Configure nShutdown GPIO as output=0 */
+	status = gpio_direction_output(kim_gdata->nshutdown, 0);
+	if (unlikely(status)) {
+		pr_err(" unable to configure gpio %ld", kim_gdata->nshutdown);
+		return status;
 	}
 	/* get reference of pdev for request_firmware
 	 */
@@ -704,34 +688,23 @@ static int kim_probe(struct platform_device *pdev)
 	init_completion(&kim_gdata->kim_rcvd);
 	init_completion(&kim_gdata->ldisc_installed);
 
-	for (proto = 0; (proto < ST_MAX) && (gpios[proto] != -1); proto++) {
-		/* TODO: should all types be rfkill_type_bt ? */
-		kim_gdata->rf_protos[proto] = proto;
-		kim_gdata->rfkill[proto] = rfkill_alloc(protocol_names[proto],
-			&pdev->dev, RFKILL_TYPE_BLUETOOTH,
-			&kim_rfkill_ops, &kim_gdata->rf_protos[proto]);
-		if (kim_gdata->rfkill[proto] == NULL) {
-			pr_err("cannot create rfkill entry for gpio %ld",
-				   gpios[proto]);
-			continue;
-		}
-		/* block upon creation */
-		rfkill_init_sw_state(kim_gdata->rfkill[proto], 1);
-		status = rfkill_register(kim_gdata->rfkill[proto]);
-		if (unlikely(status)) {
-			pr_err("rfkill registration failed for gpio %ld",
-				   gpios[proto]);
-			rfkill_unregister(kim_gdata->rfkill[proto]);
-			continue;
-		}
-		pr_info("rfkill entry created for %ld", gpios[proto]);
+	status = sysfs_create_group(&pdev->dev.kobj, &uim_attr_grp);
+	if (status) {
+		pr_err("failed to create sysfs entries");
+		return status;
 	}
 
+	/* copying platform data */
+	strncpy(kim_gdata->dev_name, pdata->dev_name, UART_DEV_NAME_LEN);
+	kim_gdata->flow_cntrl = pdata->flow_cntrl;
+	kim_gdata->baud_rate = pdata->baud_rate;
+	pr_info("sysfs entries created\n");
+
 	kim_debugfs_dir = debugfs_create_dir("ti-st", NULL);
 	if (IS_ERR(kim_debugfs_dir)) {
 		pr_err(" debugfs entries creation failed ");
 		kim_debugfs_dir = NULL;
-		return -1;
+		return -EIO;
 	}
 
 	debugfs_create_file("version", S_IRUGO, kim_debugfs_dir,
@@ -744,25 +717,22 @@ static int kim_probe(struct platform_device *pdev)
 
 static int kim_remove(struct platform_device *pdev)
 {
-	/* free the GPIOs requested
-	 */
-	long *gpios = pdev->dev.platform_data;
-	long proto;
+	/* free the GPIOs requested */
+	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
 	struct kim_data_s	*kim_gdata;
 
 	kim_gdata = dev_get_drvdata(&pdev->dev);
 
-	for (proto = 0; (proto < ST_MAX) && (gpios[proto] != -1); proto++) {
-		/* Claim the Bluetooth/FM/GPIO
-		 * nShutdown gpio from the system
-		 */
-		gpio_free(gpios[proto]);
-		rfkill_unregister(kim_gdata->rfkill[proto]);
-		rfkill_destroy(kim_gdata->rfkill[proto]);
-		kim_gdata->rfkill[proto] = NULL;
-	}
-	pr_info("kim: GPIO Freed");
+	/* Free the Bluetooth/FM/GPIO
+	 * nShutdown gpio from the system
+	 */
+	gpio_free(pdata->nshutdown_gpio);
+	pr_info("nshutdown GPIO Freed");
+
 	debugfs_remove_recursive(kim_debugfs_dir);
+	sysfs_remove_group(&pdev->dev.kobj, &uim_attr_grp);
+	pr_info("sysfs entries removed");
+
 	kim_gdata->kim_pdev = NULL;
 	st_core_exit(kim_gdata->core_data);
 
@@ -771,23 +741,46 @@ static int kim_remove(struct platform_device *pdev)
 	return 0;
 }
 
+int kim_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
+
+	if (pdata->suspend)
+		return pdata->suspend(pdev, state);
+
+	return -EOPNOTSUPP;
+}
+
+int kim_resume(struct platform_device *pdev)
+{
+	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
+
+	if (pdata->resume)
+		return pdata->resume(pdev);
+
+	return -EOPNOTSUPP;
+}
+
 /**********************************************************************/
 /* entry point for ST KIM module, called in from ST Core */
+static struct platform_driver kim_platform_driver = {
+	.probe = kim_probe,
+	.remove = kim_remove,
+	.suspend = kim_suspend,
+	.resume = kim_resume,
+	.driver = {
+		.name = "kim",
+		.owner = THIS_MODULE,
+	},
+};
 
 static int __init st_kim_init(void)
 {
-	long ret = 0;
-	ret = platform_driver_register(&kim_platform_driver);
-	if (ret != 0) {
-		pr_err("platform drv registration failed");
-		return -1;
-	}
-	return 0;
+	return platform_driver_register(&kim_platform_driver);
 }
 
 static void __exit st_kim_deinit(void)
 {
-	/* the following returns void */
 	platform_driver_unregister(&kim_platform_driver);
 }
 
diff --git a/drivers/misc/ti-st/st_ll.c b/drivers/misc/ti-st/st_ll.c
index 2bda8dea15b0..3f2495138855 100644
--- a/drivers/misc/ti-st/st_ll.c
+++ b/drivers/misc/ti-st/st_ll.c
@@ -30,7 +30,7 @@ static void send_ll_cmd(struct st_data_s *st_data,
 	unsigned char cmd)
 {
 
-	pr_info("%s: writing %x", __func__, cmd);
+	pr_debug("%s: writing %x", __func__, cmd);
 	st_int_write(st_data, &cmd, 1);
 	return;
 }
@@ -114,23 +114,23 @@ unsigned long st_ll_sleep_state(struct st_data_s *st_data,
 {
 	switch (cmd) {
 	case LL_SLEEP_IND:	/* sleep ind */
-		pr_info("sleep indication recvd");
+		pr_debug("sleep indication recvd");
 		ll_device_want_to_sleep(st_data);
 		break;
 	case LL_SLEEP_ACK:	/* sleep ack */
 		pr_err("sleep ack rcvd: host shouldn't");
 		break;
 	case LL_WAKE_UP_IND:	/* wake ind */
-		pr_info("wake indication recvd");
+		pr_debug("wake indication recvd");
 		ll_device_want_to_wakeup(st_data);
 		break;
 	case LL_WAKE_UP_ACK:	/* wake ack */
-		pr_info("wake ack rcvd");
+		pr_debug("wake ack rcvd");
 		st_data->ll_state = ST_LL_AWAKE;
 		break;
 	default:
 		pr_err(" unknown input/state ");
-		return -1;
+		return -EINVAL;
 	}
 	return 0;
 }
diff --git a/drivers/mmc/card/Kconfig b/drivers/mmc/card/Kconfig
index 2a876c4099cd..3b1f783bf924 100644
--- a/drivers/mmc/card/Kconfig
+++ b/drivers/mmc/card/Kconfig
@@ -58,12 +58,11 @@ config SDIO_UART
 
 config MMC_TEST
 	tristate "MMC host test driver"
-	default n
 	help
 	  Development driver that performs a series of reads and writes
 	  to a memory card in order to expose certain well known bugs
 	  in host controllers. The tests are executed by writing to the
-	  "test" file in sysfs under each card. Note that whatever is
+	  "test" file in debugfs under each card. Note that whatever is
 	  on your card will be overwritten by these tests.
 
 	  This driver is only of interest to those developing or
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index bfc8a8ae55df..61d233a7c118 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -621,6 +621,7 @@ static struct mmc_blk_data *mmc_blk_alloc(struct mmc_card *card)
 	md->disk->private_data = md;
 	md->disk->queue = md->queue.queue;
 	md->disk->driverfs_dev = &card->dev;
+	set_disk_ro(md->disk, md->read_only);
 
 	/*
 	 * As discussed on lkml, GENHD_FL_REMOVABLE should:
diff --git a/drivers/mmc/card/mmc_test.c b/drivers/mmc/card/mmc_test.c
index 21adc27f4132..5ec8eddfcf6e 100644
--- a/drivers/mmc/card/mmc_test.c
+++ b/drivers/mmc/card/mmc_test.c
@@ -88,6 +88,7 @@ struct mmc_test_area {
  * @sectors: amount of sectors to check in one group
  * @ts: time values of transfer
  * @rate: calculated transfer rate
+ * @iops: I/O operations per second (times 100)
  */
 struct mmc_test_transfer_result {
 	struct list_head link;
@@ -95,6 +96,7 @@ struct mmc_test_transfer_result {
 	unsigned int sectors;
 	struct timespec ts;
 	unsigned int rate;
+	unsigned int iops;
 };
 
 /**
@@ -226,9 +228,10 @@ static int mmc_test_wait_busy(struct mmc_test_card *test)
 
 		if (!busy && mmc_test_busy(&cmd)) {
 			busy = 1;
-			printk(KERN_INFO "%s: Warning: Host did not "
-				"wait for busy state to end.\n",
-				mmc_hostname(test->card->host));
+			if (test->card->host->caps & MMC_CAP_WAIT_WHILE_BUSY)
+				printk(KERN_INFO "%s: Warning: Host did not "
+					"wait for busy state to end.\n",
+					mmc_hostname(test->card->host));
 		}
 	} while (mmc_test_busy(&cmd));
 
@@ -494,7 +497,7 @@ static unsigned int mmc_test_rate(uint64_t bytes, struct timespec *ts)
  */
 static void mmc_test_save_transfer_result(struct mmc_test_card *test,
 	unsigned int count, unsigned int sectors, struct timespec ts,
-	unsigned int rate)
+	unsigned int rate, unsigned int iops)
 {
 	struct mmc_test_transfer_result *tr;
 
@@ -509,6 +512,7 @@ static void mmc_test_save_transfer_result(struct mmc_test_card *test,
 	tr->sectors = sectors;
 	tr->ts = ts;
 	tr->rate = rate;
+	tr->iops = iops;
 
 	list_add_tail(&tr->link, &test->gr->tr_lst);
 }
@@ -519,20 +523,22 @@ static void mmc_test_save_transfer_result(struct mmc_test_card *test,
 static void mmc_test_print_rate(struct mmc_test_card *test, uint64_t bytes,
 				struct timespec *ts1, struct timespec *ts2)
 {
-	unsigned int rate, sectors = bytes >> 9;
+	unsigned int rate, iops, sectors = bytes >> 9;
 	struct timespec ts;
 
 	ts = timespec_sub(*ts2, *ts1);
 
 	rate = mmc_test_rate(bytes, &ts);
+	iops = mmc_test_rate(100, &ts); /* I/O ops per sec x 100 */
 
 	printk(KERN_INFO "%s: Transfer of %u sectors (%u%s KiB) took %lu.%09lu "
-			 "seconds (%u kB/s, %u KiB/s)\n",
+			 "seconds (%u kB/s, %u KiB/s, %u.%02u IOPS)\n",
 			 mmc_hostname(test->card->host), sectors, sectors >> 1,
 			 (sectors & 1 ? ".5" : ""), (unsigned long)ts.tv_sec,
-			 (unsigned long)ts.tv_nsec, rate / 1000, rate / 1024);
+			 (unsigned long)ts.tv_nsec, rate / 1000, rate / 1024,
+			 iops / 100, iops % 100);
 
-	mmc_test_save_transfer_result(test, 1, sectors, ts, rate);
+	mmc_test_save_transfer_result(test, 1, sectors, ts, rate, iops);
 }
 
 /*
@@ -542,22 +548,24 @@ static void mmc_test_print_avg_rate(struct mmc_test_card *test, uint64_t bytes,
 				    unsigned int count, struct timespec *ts1,
 				    struct timespec *ts2)
 {
-	unsigned int rate, sectors = bytes >> 9;
+	unsigned int rate, iops, sectors = bytes >> 9;
 	uint64_t tot = bytes * count;
 	struct timespec ts;
 
 	ts = timespec_sub(*ts2, *ts1);
 
 	rate = mmc_test_rate(tot, &ts);
+	iops = mmc_test_rate(count * 100, &ts); /* I/O ops per sec x 100 */
 
 	printk(KERN_INFO "%s: Transfer of %u x %u sectors (%u x %u%s KiB) took "
-			 "%lu.%09lu seconds (%u kB/s, %u KiB/s)\n",
+			 "%lu.%09lu seconds (%u kB/s, %u KiB/s, "
+			 "%u.%02u IOPS)\n",
 			 mmc_hostname(test->card->host), count, sectors, count,
 			 sectors >> 1, (sectors & 1 ? ".5" : ""),
 			 (unsigned long)ts.tv_sec, (unsigned long)ts.tv_nsec,
-			 rate / 1000, rate / 1024);
+			 rate / 1000, rate / 1024, iops / 100, iops % 100);
 
-	mmc_test_save_transfer_result(test, count, sectors, ts, rate);
+	mmc_test_save_transfer_result(test, count, sectors, ts, rate, iops);
 }
 
 /*
@@ -1425,28 +1433,29 @@ static int mmc_test_area_cleanup(struct mmc_test_card *test)
 }
 
 /*
- * Initialize an area for testing large transfers.  The size of the area is the
- * preferred erase size which is a good size for optimal transfer speed.  Note
- * that is typically 4MiB for modern cards.  The test area is set to the middle
- * of the card because cards may have different charateristics at the front
- * (for FAT file system optimization).  Optionally, the area is erased (if the
- * card supports it) which may improve write performance.  Optionally, the area
- * is filled with data for subsequent read tests.
+ * Initialize an area for testing large transfers.  The test area is set to the
+ * middle of the card because cards may have different charateristics at the
+ * front (for FAT file system optimization).  Optionally, the area is erased
+ * (if the card supports it) which may improve write performance.  Optionally,
+ * the area is filled with data for subsequent read tests.
  */
 static int mmc_test_area_init(struct mmc_test_card *test, int erase, int fill)
 {
 	struct mmc_test_area *t = &test->area;
-	unsigned long min_sz = 64 * 1024;
+	unsigned long min_sz = 64 * 1024, sz;
 	int ret;
 
 	ret = mmc_test_set_blksize(test, 512);
 	if (ret)
 		return ret;
 
-	if (test->card->pref_erase > TEST_AREA_MAX_SIZE >> 9)
-		t->max_sz = TEST_AREA_MAX_SIZE;
-	else
-		t->max_sz = (unsigned long)test->card->pref_erase << 9;
+	/* Make the test area size about 4MiB */
+	sz = (unsigned long)test->card->pref_erase << 9;
+	t->max_sz = sz;
+	while (t->max_sz < 4 * 1024 * 1024)
+		t->max_sz += sz;
+	while (t->max_sz > TEST_AREA_MAX_SIZE && t->max_sz > sz)
+		t->max_sz -= sz;
 
 	t->max_segs = test->card->host->max_segs;
 	t->max_seg_sz = test->card->host->max_seg_size;
@@ -1766,6 +1775,188 @@ static int mmc_test_profile_seq_trim_perf(struct mmc_test_card *test)
 	return 0;
 }
 
+static unsigned int rnd_next = 1;
+
+static unsigned int mmc_test_rnd_num(unsigned int rnd_cnt)
+{
+	uint64_t r;
+
+	rnd_next = rnd_next * 1103515245 + 12345;
+	r = (rnd_next >> 16) & 0x7fff;
+	return (r * rnd_cnt) >> 15;
+}
+
+static int mmc_test_rnd_perf(struct mmc_test_card *test, int write, int print,
+			     unsigned long sz)
+{
+	unsigned int dev_addr, cnt, rnd_addr, range1, range2, last_ea = 0, ea;
+	unsigned int ssz;
+	struct timespec ts1, ts2, ts;
+	int ret;
+
+	ssz = sz >> 9;
+
+	rnd_addr = mmc_test_capacity(test->card) / 4;
+	range1 = rnd_addr / test->card->pref_erase;
+	range2 = range1 / ssz;
+
+	getnstimeofday(&ts1);
+	for (cnt = 0; cnt < UINT_MAX; cnt++) {
+		getnstimeofday(&ts2);
+		ts = timespec_sub(ts2, ts1);
+		if (ts.tv_sec >= 10)
+			break;
+		ea = mmc_test_rnd_num(range1);
+		if (ea == last_ea)
+			ea -= 1;
+		last_ea = ea;
+		dev_addr = rnd_addr + test->card->pref_erase * ea +
+			   ssz * mmc_test_rnd_num(range2);
+		ret = mmc_test_area_io(test, sz, dev_addr, write, 0, 0);
+		if (ret)
+			return ret;
+	}
+	if (print)
+		mmc_test_print_avg_rate(test, sz, cnt, &ts1, &ts2);
+	return 0;
+}
+
+static int mmc_test_random_perf(struct mmc_test_card *test, int write)
+{
+	unsigned int next;
+	unsigned long sz;
+	int ret;
+
+	for (sz = 512; sz < test->area.max_tfr; sz <<= 1) {
+		/*
+		 * When writing, try to get more consistent results by running
+		 * the test twice with exactly the same I/O but outputting the
+		 * results only for the 2nd run.
+		 */
+		if (write) {
+			next = rnd_next;
+			ret = mmc_test_rnd_perf(test, write, 0, sz);
+			if (ret)
+				return ret;
+			rnd_next = next;
+		}
+		ret = mmc_test_rnd_perf(test, write, 1, sz);
+		if (ret)
+			return ret;
+	}
+	sz = test->area.max_tfr;
+	if (write) {
+		next = rnd_next;
+		ret = mmc_test_rnd_perf(test, write, 0, sz);
+		if (ret)
+			return ret;
+		rnd_next = next;
+	}
+	return mmc_test_rnd_perf(test, write, 1, sz);
+}
+
+/*
+ * Random read performance by transfer size.
+ */
+static int mmc_test_random_read_perf(struct mmc_test_card *test)
+{
+	return mmc_test_random_perf(test, 0);
+}
+
+/*
+ * Random write performance by transfer size.
+ */
+static int mmc_test_random_write_perf(struct mmc_test_card *test)
+{
+	return mmc_test_random_perf(test, 1);
+}
+
+static int mmc_test_seq_perf(struct mmc_test_card *test, int write,
+			     unsigned int tot_sz, int max_scatter)
+{
+	unsigned int dev_addr, i, cnt, sz, ssz;
+	struct timespec ts1, ts2, ts;
+	int ret;
+
+	sz = test->area.max_tfr;
+	/*
+	 * In the case of a maximally scattered transfer, the maximum transfer
+	 * size is further limited by using PAGE_SIZE segments.
+	 */
+	if (max_scatter) {
+		struct mmc_test_area *t = &test->area;
+		unsigned long max_tfr;
+
+		if (t->max_seg_sz >= PAGE_SIZE)
+			max_tfr = t->max_segs * PAGE_SIZE;
+		else
+			max_tfr = t->max_segs * t->max_seg_sz;
+		if (sz > max_tfr)
+			sz = max_tfr;
+	}
+
+	ssz = sz >> 9;
+	dev_addr = mmc_test_capacity(test->card) / 4;
+	if (tot_sz > dev_addr << 9)
+		tot_sz = dev_addr << 9;
+	cnt = tot_sz / sz;
+	dev_addr &= 0xffff0000; /* Round to 64MiB boundary */
+
+	getnstimeofday(&ts1);
+	for (i = 0; i < cnt; i++) {
+		ret = mmc_test_area_io(test, sz, dev_addr, write,
+				       max_scatter, 0);
+		if (ret)
+			return ret;
+		dev_addr += ssz;
+	}
+	getnstimeofday(&ts2);
+
+	ts = timespec_sub(ts2, ts1);
+	mmc_test_print_avg_rate(test, sz, cnt, &ts1, &ts2);
+
+	return 0;
+}
+
+static int mmc_test_large_seq_perf(struct mmc_test_card *test, int write)
+{
+	int ret, i;
+
+	for (i = 0; i < 10; i++) {
+		ret = mmc_test_seq_perf(test, write, 10 * 1024 * 1024, 1);
+		if (ret)
+			return ret;
+	}
+	for (i = 0; i < 5; i++) {
+		ret = mmc_test_seq_perf(test, write, 100 * 1024 * 1024, 1);
+		if (ret)
+			return ret;
+	}
+	for (i = 0; i < 3; i++) {
+		ret = mmc_test_seq_perf(test, write, 1000 * 1024 * 1024, 1);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+/*
+ * Large sequential read performance.
+ */
+static int mmc_test_large_seq_read_perf(struct mmc_test_card *test)
+{
+	return mmc_test_large_seq_perf(test, 0);
+}
+
+/*
+ * Large sequential write performance.
+ */
+static int mmc_test_large_seq_write_perf(struct mmc_test_card *test)
+{
+	return mmc_test_large_seq_perf(test, 1);
+}
+
 static const struct mmc_test_case mmc_test_cases[] = {
 	{
 		.name = "Basic write (no data verification)",
@@ -2005,6 +2196,34 @@ static const struct mmc_test_case mmc_test_cases[] = {
 		.cleanup = mmc_test_area_cleanup,
 	},
 
+	{
+		.name = "Random read performance by transfer size",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_random_read_perf,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Random write performance by transfer size",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_random_write_perf,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Large sequential read into scattered pages",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_large_seq_read_perf,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
+	{
+		.name = "Large sequential write from scattered pages",
+		.prepare = mmc_test_area_prepare,
+		.run = mmc_test_large_seq_write_perf,
+		.cleanup = mmc_test_area_cleanup,
+	},
+
 };
 
 static DEFINE_MUTEX(mmc_test_lock);
@@ -2148,11 +2367,11 @@ static int mtf_test_show(struct seq_file *sf, void *data)
 		seq_printf(sf, "Test %d: %d\n", gr->testcase + 1, gr->result);
 
 		list_for_each_entry(tr, &gr->tr_lst, link) {
-			seq_printf(sf, "%u %d %lu.%09lu %u\n",
+			seq_printf(sf, "%u %d %lu.%09lu %u %u.%02u\n",
 				tr->count, tr->sectors,
 				(unsigned long)tr->ts.tv_sec,
 				(unsigned long)tr->ts.tv_nsec,
-				tr->rate);
+				tr->rate, tr->iops / 100, tr->iops % 100);
 		}
 	}
 
diff --git a/drivers/mmc/core/Makefile b/drivers/mmc/core/Makefile
index 86b479119332..639501970b41 100644
--- a/drivers/mmc/core/Makefile
+++ b/drivers/mmc/core/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_MMC)		+= mmc_core.o
 mmc_core-y			:= core.o bus.o host.o \
 				   mmc.o mmc_ops.o sd.o sd_ops.o \
 				   sdio.o sdio_ops.o sdio_bus.o \
-				   sdio_cis.o sdio_io.o sdio_irq.o
+				   sdio_cis.o sdio_io.o sdio_irq.o \
+				   quirks.o
 
 mmc_core-$(CONFIG_DEBUG_FS)	+= debugfs.o
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 6625c057be05..c8b0abe5266a 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -167,8 +167,6 @@ mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 
 	WARN_ON(!host->claimed);
 
-	led_trigger_event(host->led, LED_FULL);
-
 	mrq->cmd->error = 0;
 	mrq->cmd->mrq = mrq;
 	if (mrq->data) {
@@ -194,6 +192,7 @@ mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 		}
 	}
 	mmc_host_clk_ungate(host);
+	led_trigger_event(host->led, LED_FULL);
 	host->ops->request(host, mrq);
 }
 
@@ -1495,6 +1494,12 @@ static int mmc_rescan_try_freq(struct mmc_host *host, unsigned freq)
 		mmc_hostname(host), __func__, host->f_init);
 #endif
 	mmc_power_up(host);
+
+	/*
+	 * sdio_reset sends CMD52 to reset card.  Since we do not know
+	 * if the card is being re-initialzed just send it.  CMD52
+	 * should be ignored by SD/eMMC cards.
+	 */
 	sdio_reset(host);
 	mmc_go_idle(host);
 
@@ -1529,7 +1534,7 @@ void mmc_rescan(struct work_struct *work)
 	 * still present
 	 */
 	if (host->bus_ops && host->bus_ops->detect && !host->bus_dead
-	    && mmc_card_is_removable(host))
+	    && !(host->caps & MMC_CAP_NONREMOVABLE))
 		host->bus_ops->detect(host);
 
 	/*
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index ca1fdde29df6..20b1c0831eac 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -61,6 +61,8 @@ int mmc_attach_mmc(struct mmc_host *host);
 int mmc_attach_sd(struct mmc_host *host);
 int mmc_attach_sdio(struct mmc_host *host);
 
+void mmc_fixup_device(struct mmc_card *card);
+
 /* Module parameters */
 extern int use_spi_crc;
 
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index b3ac6c5bc5c6..461e6a17fb90 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -160,10 +160,7 @@ static bool mmc_host_may_gate_card(struct mmc_card *card)
 	 * gate the clock, because there is somebody out there that may still
 	 * be using it.
 	 */
-	if (mmc_card_sdio(card))
-		return false;
-
-	return true;
+	return !(card->quirks & MMC_QUIRK_BROKEN_CLK_GATING);
 }
 
 /**
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 16006ef153fe..14e95f39a7bf 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -302,6 +302,44 @@ static int mmc_read_ext_csd(struct mmc_card *card)
 	}
 
 	if (card->ext_csd.rev >= 4) {
+		/*
+		 * Enhanced area feature support -- check whether the eMMC
+		 * card has the Enhanced area enabled.  If so, export enhanced
+		 * area offset and size to user by adding sysfs interface.
+		 */
+		if ((ext_csd[EXT_CSD_PARTITION_SUPPORT] & 0x2) &&
+				(ext_csd[EXT_CSD_PARTITION_ATTRIBUTE] & 0x1)) {
+			u8 hc_erase_grp_sz =
+				ext_csd[EXT_CSD_HC_ERASE_GRP_SIZE];
+			u8 hc_wp_grp_sz =
+				ext_csd[EXT_CSD_HC_WP_GRP_SIZE];
+
+			card->ext_csd.enhanced_area_en = 1;
+			/*
+			 * calculate the enhanced data area offset, in bytes
+			 */
+			card->ext_csd.enhanced_area_offset =
+				(ext_csd[139] << 24) + (ext_csd[138] << 16) +
+				(ext_csd[137] << 8) + ext_csd[136];
+			if (mmc_card_blockaddr(card))
+				card->ext_csd.enhanced_area_offset <<= 9;
+			/*
+			 * calculate the enhanced data area size, in kilobytes
+			 */
+			card->ext_csd.enhanced_area_size =
+				(ext_csd[142] << 16) + (ext_csd[141] << 8) +
+				ext_csd[140];
+			card->ext_csd.enhanced_area_size *=
+				(size_t)(hc_erase_grp_sz * hc_wp_grp_sz);
+			card->ext_csd.enhanced_area_size <<= 9;
+		} else {
+			/*
+			 * If the enhanced area is not enabled, disable these
+			 * device attributes.
+			 */
+			card->ext_csd.enhanced_area_offset = -EINVAL;
+			card->ext_csd.enhanced_area_size = -EINVAL;
+		}
 		card->ext_csd.sec_trim_mult =
 			ext_csd[EXT_CSD_SEC_TRIM_MULT];
 		card->ext_csd.sec_erase_mult =
@@ -336,6 +374,9 @@ MMC_DEV_ATTR(manfid, "0x%06x\n", card->cid.manfid);
 MMC_DEV_ATTR(name, "%s\n", card->cid.prod_name);
 MMC_DEV_ATTR(oemid, "0x%04x\n", card->cid.oemid);
 MMC_DEV_ATTR(serial, "0x%08x\n", card->cid.serial);
+MMC_DEV_ATTR(enhanced_area_offset, "%llu\n",
+		card->ext_csd.enhanced_area_offset);
+MMC_DEV_ATTR(enhanced_area_size, "%u\n", card->ext_csd.enhanced_area_size);
 
 static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_cid.attr,
@@ -349,6 +390,8 @@ static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_name.attr,
 	&dev_attr_oemid.attr,
 	&dev_attr_serial.attr,
+	&dev_attr_enhanced_area_offset.attr,
+	&dev_attr_enhanced_area_size.attr,
 	NULL,
 };
 
@@ -378,6 +421,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	int err, ddr = 0;
 	u32 cid[4];
 	unsigned int max_dtr;
+	u32 rocr;
 
 	BUG_ON(!host);
 	WARN_ON(!host->claimed);
@@ -391,7 +435,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	mmc_go_idle(host);
 
 	/* The extra bit indicates that we support high capacity */
-	err = mmc_send_op_cond(host, ocr | (1 << 30), NULL);
+	err = mmc_send_op_cond(host, ocr | (1 << 30), &rocr);
 	if (err)
 		goto err;
 
@@ -479,11 +523,51 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 		err = mmc_read_ext_csd(card);
 		if (err)
 			goto free_card;
+
+		/* If doing byte addressing, check if required to do sector
+		 * addressing.  Handle the case of <2GB cards needing sector
+		 * addressing.  See section 8.1 JEDEC Standard JED84-A441;
+		 * ocr register has bit 30 set for sector addressing.
+		 */
+		if (!(mmc_card_blockaddr(card)) && (rocr & (1<<30)))
+			mmc_card_set_blockaddr(card);
+
 		/* Erase size depends on CSD and Extended CSD */
 		mmc_set_erase_size(card);
 	}
 
 	/*
+	 * If enhanced_area_en is TRUE, host needs to enable ERASE_GRP_DEF
+	 * bit.  This bit will be lost everytime after a reset or power off.
+	 */
+	if (card->ext_csd.enhanced_area_en) {
+		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+				EXT_CSD_ERASE_GROUP_DEF, 1);
+
+		if (err && err != -EBADMSG)
+			goto free_card;
+
+		if (err) {
+			err = 0;
+			/*
+			 * Just disable enhanced area off & sz
+			 * will try to enable ERASE_GROUP_DEF
+			 * during next time reinit
+			 */
+			card->ext_csd.enhanced_area_offset = -EINVAL;
+			card->ext_csd.enhanced_area_size = -EINVAL;
+		} else {
+			card->ext_csd.erase_group_def = 1;
+			/*
+			 * enable ERASE_GRP_DEF successfully.
+			 * This will affect the erase size, so
+			 * here need to reset erase size
+			 */
+			mmc_set_erase_size(card);
+		}
+	}
+
+	/*
 	 * Activate high speed (if supported)
 	 */
 	if ((card->ext_csd.hs_max_dtr != 0) &&
diff --git a/drivers/mmc/core/quirks.c b/drivers/mmc/core/quirks.c
new file mode 100644
index 000000000000..4fb16ac007f0
--- /dev/null
+++ b/drivers/mmc/core/quirks.c
@@ -0,0 +1,85 @@
+/*
+ *  This file contains work-arounds for many known sdio hardware
+ *  bugs.
+ *
+ *  Copyright (c) 2011 Pierre Tardy <tardyp@gmail.com>
+ *  Inspired from pci fixup code:
+ *  Copyright (c) 1999 Martin Mares <mj@ucw.cz>
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mmc/card.h>
+#include <linux/mod_devicetable.h>
+
+/*
+ *  The world is not perfect and supplies us with broken mmc/sdio devices.
+ *  For at least a part of these bugs we need a work-around
+ */
+
+struct mmc_fixup {
+	u16 vendor, device;	/* You can use SDIO_ANY_ID here of course */
+	void (*vendor_fixup)(struct mmc_card *card, int data);
+	int data;
+};
+
+/*
+ * This hook just adds a quirk unconditionnally
+ */
+static void __maybe_unused add_quirk(struct mmc_card *card, int data)
+{
+	card->quirks |= data;
+}
+
+/*
+ * This hook just removes a quirk unconditionnally
+ */
+static void __maybe_unused remove_quirk(struct mmc_card *card, int data)
+{
+	card->quirks &= ~data;
+}
+
+/*
+ * This hook just adds a quirk for all sdio devices
+ */
+static void add_quirk_for_sdio_devices(struct mmc_card *card, int data)
+{
+	if (mmc_card_sdio(card))
+		card->quirks |= data;
+}
+
+#ifndef SDIO_VENDOR_ID_TI
+#define SDIO_VENDOR_ID_TI		0x0097
+#endif
+
+#ifndef SDIO_DEVICE_ID_TI_WL1271
+#define SDIO_DEVICE_ID_TI_WL1271	0x4076
+#endif
+
+static const struct mmc_fixup mmc_fixup_methods[] = {
+	/* by default sdio devices are considered CLK_GATING broken */
+	/* good cards will be whitelisted as they are tested */
+	{ SDIO_ANY_ID, SDIO_ANY_ID,
+		add_quirk_for_sdio_devices, MMC_QUIRK_BROKEN_CLK_GATING },
+	{ SDIO_VENDOR_ID_TI, SDIO_DEVICE_ID_TI_WL1271,
+		remove_quirk, MMC_QUIRK_BROKEN_CLK_GATING },
+	{ 0 }
+};
+
+void mmc_fixup_device(struct mmc_card *card)
+{
+	const struct mmc_fixup *f;
+
+	for (f = mmc_fixup_methods; f->vendor_fixup; f++) {
+		if ((f->vendor == card->cis.vendor
+		     || f->vendor == (u16) SDIO_ANY_ID) &&
+		    (f->device == card->cis.device
+		     || f->device == (u16) SDIO_ANY_ID)) {
+			dev_dbg(&card->dev, "calling %pF\n", f->vendor_fixup);
+			f->vendor_fixup(card, f->data);
+		}
+	}
+}
+EXPORT_SYMBOL(mmc_fixup_device);
+
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index d18c32bca99b..6dac89fe0535 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -21,6 +21,7 @@
 #include "core.h"
 #include "bus.h"
 #include "mmc_ops.h"
+#include "sd.h"
 #include "sd_ops.h"
 
 static const unsigned int tran_exp[] = {
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 5c4a54d9b6a4..db0f0b44d684 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -395,6 +395,14 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 		if (err)
 			goto remove;
 
+		/*
+		 * Update oldcard with the new RCA received from the SDIO
+		 * device -- we're doing this so that it's updated in the
+		 * "card" struct when oldcard overwrites that later.
+		 */
+		if (oldcard)
+			oldcard->rca = card->rca;
+
 		mmc_set_bus_mode(host, MMC_BUSMODE_PUSHPULL);
 	}
 
@@ -458,6 +466,7 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 
 		card = oldcard;
 	}
+	mmc_fixup_device(card);
 
 	if (card->type == MMC_TYPE_SD_COMBO) {
 		err = mmc_sd_setup_card(host, card, oldcard != NULL);
@@ -792,7 +801,6 @@ int mmc_attach_sdio(struct mmc_host *host)
 	 */
 	mmc_release_host(host);
 	err = mmc_add_card(host->card);
-	mmc_claim_host(host);
 	if (err)
 		goto remove_added;
 
@@ -805,12 +813,12 @@ int mmc_attach_sdio(struct mmc_host *host)
 			goto remove_added;
 	}
 
+	mmc_claim_host(host);
 	return 0;
 
 
 remove_added:
 	/* Remove without lock if the device has been added. */
-	mmc_release_host(host);
 	mmc_sdio_remove(host);
 	mmc_claim_host(host);
 remove:
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index afe8c6fa166a..1a21c6427a19 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -225,7 +225,7 @@ config MMC_OMAP
 
 config MMC_OMAP_HS
 	tristate "TI OMAP High Speed Multimedia Card Interface support"
-	depends on ARCH_OMAP2430 || ARCH_OMAP3 || ARCH_OMAP4
+	depends on SOC_OMAP2430 || ARCH_OMAP3 || ARCH_OMAP4
 	help
 	  This selects the TI OMAP High Speed Multimedia card Interface.
 	  If you have an OMAP2430 or OMAP3 board or OMAP4 board with a
@@ -311,7 +311,7 @@ config MMC_MSM
 
 config MMC_MXC
 	tristate "Freescale i.MX2/3 Multimedia Card Interface support"
-	depends on ARCH_MXC
+	depends on MACH_MX21 || MACH_MX27 || ARCH_MX31
 	help
 	  This selects the Freescale i.MX2/3 Multimedia card Interface.
 	  If you have a i.MX platform with a Multimedia Card slot,
@@ -319,6 +319,15 @@ config MMC_MXC
 
 	  If unsure, say N.
 
+config MMC_MXS
+	tristate "Freescale MXS Multimedia Card Interface support"
+	depends on ARCH_MXS && MXS_DMA
+	help
+	  This selects the Freescale SSP MMC controller found on MXS based
+	  platforms like mx23/28.
+
+	  If unsure, say N.
+
 config MMC_TIFM_SD
 	tristate "TI Flash Media MMC/SD Interface support  (EXPERIMENTAL)"
 	depends on EXPERIMENTAL && PCI
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index e834fb223e9a..30aa6867745f 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_MMC_ARMMMCI)	+= mmci.o
 obj-$(CONFIG_MMC_PXA)		+= pxamci.o
 obj-$(CONFIG_MMC_IMX)		+= imxmmc.o
 obj-$(CONFIG_MMC_MXC)		+= mxcmmc.o
+obj-$(CONFIG_MMC_MXS)		+= mxs-mmc.o
 obj-$(CONFIG_MMC_SDHCI)		+= sdhci.o
 obj-$(CONFIG_MMC_SDHCI_PCI)	+= sdhci-pci.o
 obj-$(CONFIG_MMC_SDHCI_PXA)	+= sdhci-pxa.o
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index ad2a7a032cdf..80bc9a5c25cc 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -578,7 +578,8 @@ static void atmci_dma_cleanup(struct atmel_mci *host)
 	struct mmc_data			*data = host->data;
 
 	if (data)
-		dma_unmap_sg(&host->pdev->dev, data->sg, data->sg_len,
+		dma_unmap_sg(host->dma.chan->device->dev,
+			     data->sg, data->sg_len,
 			     ((data->flags & MMC_DATA_WRITE)
 			      ? DMA_TO_DEVICE : DMA_FROM_DEVICE));
 }
@@ -588,7 +589,7 @@ static void atmci_stop_dma(struct atmel_mci *host)
 	struct dma_chan *chan = host->data_chan;
 
 	if (chan) {
-	  chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(chan);
 		atmci_dma_cleanup(host);
 	} else {
 		/* Data transfer was stopped by the interrupt handler */
@@ -684,11 +685,11 @@ atmci_prepare_data_dma(struct atmel_mci *host, struct mmc_data *data)
 	else
 		direction = DMA_TO_DEVICE;
 
-	sglen = dma_map_sg(&host->pdev->dev, data->sg, data->sg_len, direction);
-	if (sglen != data->sg_len)
-		goto unmap_exit;
+	sglen = dma_map_sg(chan->device->dev, data->sg,
+			   data->sg_len, direction);
+
 	desc = chan->device->device_prep_slave_sg(chan,
-			data->sg, data->sg_len, direction,
+			data->sg, sglen, direction,
 			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc)
 		goto unmap_exit;
@@ -699,7 +700,7 @@ atmci_prepare_data_dma(struct atmel_mci *host, struct mmc_data *data)
 
 	return 0;
 unmap_exit:
-	dma_unmap_sg(&host->pdev->dev, data->sg, sglen, direction);
+	dma_unmap_sg(chan->device->dev, data->sg, data->sg_len, direction);
 	return -ENOMEM;
 }
 
@@ -709,8 +710,8 @@ static void atmci_submit_data(struct atmel_mci *host)
 	struct dma_async_tx_descriptor	*desc = host->dma.data_desc;
 
 	if (chan) {
-		desc->tx_submit(desc);
-		chan->device->device_issue_pending(chan);
+		dmaengine_submit(desc);
+		dma_async_issue_pending(chan);
 	}
 }
 
diff --git a/drivers/mmc/host/cb710-mmc.c b/drivers/mmc/host/cb710-mmc.c
index 66b4ce587f4b..ce2a47b71dd6 100644
--- a/drivers/mmc/host/cb710-mmc.c
+++ b/drivers/mmc/host/cb710-mmc.c
@@ -205,7 +205,7 @@ static int cb710_wait_while_busy(struct cb710_slot *slot, uint8_t mask)
 			"WAIT12: waited %d loops, mask %02X, entry val %08X, exit val %08X\n",
 			limit, mask, e, x);
 #endif
-	return 0;
+	return err;
 }
 
 static void cb710_mmc_set_transfer_size(struct cb710_slot *slot,
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 2fcc82577c1b..299c1d61b6b3 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -562,7 +562,8 @@ static void dw_mci_setup_bus(struct dw_mci_slot *slot)
 			     SDMMC_CMD_UPD_CLK | SDMMC_CMD_PRV_DAT_WAIT, 0);
 
 		/* enable clock */
-		mci_writel(host, CLKENA, SDMMC_CLKEN_ENABLE);
+		mci_writel(host, CLKENA, SDMMC_CLKEN_ENABLE |
+			   SDMMC_CLKEN_LOW_PWR);
 
 		/* inform CIU */
 		mci_send_cmd(slot,
@@ -661,6 +662,7 @@ static void dw_mci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct dw_mci_slot *slot = mmc_priv(mmc);
+	u32 regs;
 
 	/* set default 1 bit mode */
 	slot->ctype = SDMMC_CTYPE_1BIT;
@@ -672,6 +674,16 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	case MMC_BUS_WIDTH_4:
 		slot->ctype = SDMMC_CTYPE_4BIT;
 		break;
+	case MMC_BUS_WIDTH_8:
+		slot->ctype = SDMMC_CTYPE_8BIT;
+		break;
+	}
+
+	/* DDR mode set */
+	if (ios->ddr) {
+		regs = mci_readl(slot->host, UHS_REG);
+		regs |= (0x1 << slot->id) << 16;
+		mci_writel(slot->host, UHS_REG, regs);
 	}
 
 	if (ios->clock) {
@@ -1019,13 +1031,10 @@ static void dw_mci_read_data_pio(struct dw_mci *host)
 	struct mmc_data	*data = host->data;
 	int shift = host->data_shift;
 	u32 status;
-	unsigned int nbytes = 0, len, old_len, count = 0;
+	unsigned int nbytes = 0, len;
 
 	do {
 		len = SDMMC_GET_FCNT(mci_readl(host, STATUS)) << shift;
-		if (count == 0)
-			old_len = len;
-
 		if (offset + len <= sg->length) {
 			host->pull_data(host, (void *)(buf + offset), len);
 
@@ -1070,7 +1079,6 @@ static void dw_mci_read_data_pio(struct dw_mci *host)
 			tasklet_schedule(&host->tasklet);
 			return;
 		}
-		count++;
 	} while (status & SDMMC_INT_RXDR); /*if the RXDR is ready read again*/
 	len = SDMMC_GET_FCNT(mci_readl(host, STATUS));
 	host->pio_offset = offset;
@@ -1441,6 +1449,12 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	/* Card initially undetected */
 	slot->last_detect_state = 0;
 
+	/*
+	 * Card may have been plugged in prior to boot so we
+	 * need to run the detect tasklet
+	 */
+	tasklet_schedule(&host->card_tasklet);
+
 	return 0;
 }
 
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 5dd55a75233d..23c662af5616 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -43,6 +43,7 @@
 #define SDMMC_USRID		0x068
 #define SDMMC_VERID		0x06c
 #define SDMMC_HCON		0x070
+#define SDMMC_UHS_REG		0x074
 #define SDMMC_BMOD		0x080
 #define SDMMC_PLDMND		0x084
 #define SDMMC_DBADDR		0x088
@@ -51,7 +52,6 @@
 #define SDMMC_DSCADDR		0x094
 #define SDMMC_BUFADDR		0x098
 #define SDMMC_DATA		0x100
-#define SDMMC_DATA_ADR		0x100
 
 /* shift bit field */
 #define _SBF(f, v)		((v) << (f))
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index fd877f633dd2..2f7fc0c5146f 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -1516,21 +1516,17 @@ static int __devexit mmc_spi_remove(struct spi_device *spi)
 	return 0;
 }
 
-#if defined(CONFIG_OF)
 static struct of_device_id mmc_spi_of_match_table[] __devinitdata = {
 	{ .compatible = "mmc-spi-slot", },
 	{},
 };
-#endif
 
 static struct spi_driver mmc_spi_driver = {
 	.driver = {
 		.name =		"mmc_spi",
 		.bus =		&spi_bus_type,
 		.owner =	THIS_MODULE,
-#if defined(CONFIG_OF)
 		.of_match_table = mmc_spi_of_match_table,
-#endif
 	},
 	.probe =	mmc_spi_probe,
 	.remove =	__devexit_p(mmc_spi_remove),
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 2d6de3e03e2d..5bbb87d10251 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -2,7 +2,7 @@
  *  linux/drivers/mmc/host/mmci.c - ARM PrimeCell MMCI PL180/1 driver
  *
  *  Copyright (C) 2003 Deep Blue Solutions, Ltd, All Rights Reserved.
- *  Copyright (C) 2010 ST-Ericsson AB.
+ *  Copyright (C) 2010 ST-Ericsson SA
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -25,8 +25,10 @@
 #include <linux/clk.h>
 #include <linux/scatterlist.h>
 #include <linux/gpio.h>
-#include <linux/amba/mmci.h>
 #include <linux/regulator/consumer.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/amba/mmci.h>
 
 #include <asm/div64.h>
 #include <asm/io.h>
@@ -142,9 +144,6 @@ mmci_request_end(struct mmci_host *host, struct mmc_request *mrq)
 	host->mrq = NULL;
 	host->cmd = NULL;
 
-	if (mrq->data)
-		mrq->data->bytes_xfered = host->data_xfered;
-
 	/*
 	 * Need to drop the host lock here; mmc_request_done may call
 	 * back into the driver...
@@ -189,6 +188,248 @@ static void mmci_init_sg(struct mmci_host *host, struct mmc_data *data)
 	sg_miter_start(&host->sg_miter, data->sg, data->sg_len, flags);
 }
 
+/*
+ * All the DMA operation mode stuff goes inside this ifdef.
+ * This assumes that you have a generic DMA device interface,
+ * no custom DMA interfaces are supported.
+ */
+#ifdef CONFIG_DMA_ENGINE
+static void __devinit mmci_dma_setup(struct mmci_host *host)
+{
+	struct mmci_platform_data *plat = host->plat;
+	const char *rxname, *txname;
+	dma_cap_mask_t mask;
+
+	if (!plat || !plat->dma_filter) {
+		dev_info(mmc_dev(host->mmc), "no DMA platform data\n");
+		return;
+	}
+
+	/* Try to acquire a generic DMA engine slave channel */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	/*
+	 * If only an RX channel is specified, the driver will
+	 * attempt to use it bidirectionally, however if it is
+	 * is specified but cannot be located, DMA will be disabled.
+	 */
+	if (plat->dma_rx_param) {
+		host->dma_rx_channel = dma_request_channel(mask,
+							   plat->dma_filter,
+							   plat->dma_rx_param);
+		/* E.g if no DMA hardware is present */
+		if (!host->dma_rx_channel)
+			dev_err(mmc_dev(host->mmc), "no RX DMA channel\n");
+	}
+
+	if (plat->dma_tx_param) {
+		host->dma_tx_channel = dma_request_channel(mask,
+							   plat->dma_filter,
+							   plat->dma_tx_param);
+		if (!host->dma_tx_channel)
+			dev_warn(mmc_dev(host->mmc), "no TX DMA channel\n");
+	} else {
+		host->dma_tx_channel = host->dma_rx_channel;
+	}
+
+	if (host->dma_rx_channel)
+		rxname = dma_chan_name(host->dma_rx_channel);
+	else
+		rxname = "none";
+
+	if (host->dma_tx_channel)
+		txname = dma_chan_name(host->dma_tx_channel);
+	else
+		txname = "none";
+
+	dev_info(mmc_dev(host->mmc), "DMA channels RX %s, TX %s\n",
+		 rxname, txname);
+
+	/*
+	 * Limit the maximum segment size in any SG entry according to
+	 * the parameters of the DMA engine device.
+	 */
+	if (host->dma_tx_channel) {
+		struct device *dev = host->dma_tx_channel->device->dev;
+		unsigned int max_seg_size = dma_get_max_seg_size(dev);
+
+		if (max_seg_size < host->mmc->max_seg_size)
+			host->mmc->max_seg_size = max_seg_size;
+	}
+	if (host->dma_rx_channel) {
+		struct device *dev = host->dma_rx_channel->device->dev;
+		unsigned int max_seg_size = dma_get_max_seg_size(dev);
+
+		if (max_seg_size < host->mmc->max_seg_size)
+			host->mmc->max_seg_size = max_seg_size;
+	}
+}
+
+/*
+ * This is used in __devinit or __devexit so inline it
+ * so it can be discarded.
+ */
+static inline void mmci_dma_release(struct mmci_host *host)
+{
+	struct mmci_platform_data *plat = host->plat;
+
+	if (host->dma_rx_channel)
+		dma_release_channel(host->dma_rx_channel);
+	if (host->dma_tx_channel && plat->dma_tx_param)
+		dma_release_channel(host->dma_tx_channel);
+	host->dma_rx_channel = host->dma_tx_channel = NULL;
+}
+
+static void mmci_dma_unmap(struct mmci_host *host, struct mmc_data *data)
+{
+	struct dma_chan *chan = host->dma_current;
+	enum dma_data_direction dir;
+	u32 status;
+	int i;
+
+	/* Wait up to 1ms for the DMA to complete */
+	for (i = 0; ; i++) {
+		status = readl(host->base + MMCISTATUS);
+		if (!(status & MCI_RXDATAAVLBLMASK) || i >= 100)
+			break;
+		udelay(10);
+	}
+
+	/*
+	 * Check to see whether we still have some data left in the FIFO -
+	 * this catches DMA controllers which are unable to monitor the
+	 * DMALBREQ and DMALSREQ signals while allowing us to DMA to non-
+	 * contiguous buffers.  On TX, we'll get a FIFO underrun error.
+	 */
+	if (status & MCI_RXDATAAVLBLMASK) {
+		dmaengine_terminate_all(chan);
+		if (!data->error)
+			data->error = -EIO;
+	}
+
+	if (data->flags & MMC_DATA_WRITE) {
+		dir = DMA_TO_DEVICE;
+	} else {
+		dir = DMA_FROM_DEVICE;
+	}
+
+	dma_unmap_sg(chan->device->dev, data->sg, data->sg_len, dir);
+
+	/*
+	 * Use of DMA with scatter-gather is impossible.
+	 * Give up with DMA and switch back to PIO mode.
+	 */
+	if (status & MCI_RXDATAAVLBLMASK) {
+		dev_err(mmc_dev(host->mmc), "buggy DMA detected. Taking evasive action.\n");
+		mmci_dma_release(host);
+	}
+}
+
+static void mmci_dma_data_error(struct mmci_host *host)
+{
+	dev_err(mmc_dev(host->mmc), "error during DMA transfer!\n");
+	dmaengine_terminate_all(host->dma_current);
+}
+
+static int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
+{
+	struct variant_data *variant = host->variant;
+	struct dma_slave_config conf = {
+		.src_addr = host->phybase + MMCIFIFO,
+		.dst_addr = host->phybase + MMCIFIFO,
+		.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.src_maxburst = variant->fifohalfsize >> 2, /* # of words */
+		.dst_maxburst = variant->fifohalfsize >> 2, /* # of words */
+	};
+	struct mmc_data *data = host->data;
+	struct dma_chan *chan;
+	struct dma_device *device;
+	struct dma_async_tx_descriptor *desc;
+	int nr_sg;
+
+	host->dma_current = NULL;
+
+	if (data->flags & MMC_DATA_READ) {
+		conf.direction = DMA_FROM_DEVICE;
+		chan = host->dma_rx_channel;
+	} else {
+		conf.direction = DMA_TO_DEVICE;
+		chan = host->dma_tx_channel;
+	}
+
+	/* If there's no DMA channel, fall back to PIO */
+	if (!chan)
+		return -EINVAL;
+
+	/* If less than or equal to the fifo size, don't bother with DMA */
+	if (host->size <= variant->fifosize)
+		return -EINVAL;
+
+	device = chan->device;
+	nr_sg = dma_map_sg(device->dev, data->sg, data->sg_len, conf.direction);
+	if (nr_sg == 0)
+		return -EINVAL;
+
+	dmaengine_slave_config(chan, &conf);
+	desc = device->device_prep_slave_sg(chan, data->sg, nr_sg,
+					    conf.direction, DMA_CTRL_ACK);
+	if (!desc)
+		goto unmap_exit;
+
+	/* Okay, go for it. */
+	host->dma_current = chan;
+
+	dev_vdbg(mmc_dev(host->mmc),
+		 "Submit MMCI DMA job, sglen %d blksz %04x blks %04x flags %08x\n",
+		 data->sg_len, data->blksz, data->blocks, data->flags);
+	dmaengine_submit(desc);
+	dma_async_issue_pending(chan);
+
+	datactrl |= MCI_DPSM_DMAENABLE;
+
+	/* Trigger the DMA transfer */
+	writel(datactrl, host->base + MMCIDATACTRL);
+
+	/*
+	 * Let the MMCI say when the data is ended and it's time
+	 * to fire next DMA request. When that happens, MMCI will
+	 * call mmci_data_end()
+	 */
+	writel(readl(host->base + MMCIMASK0) | MCI_DATAENDMASK,
+	       host->base + MMCIMASK0);
+	return 0;
+
+unmap_exit:
+	dmaengine_terminate_all(chan);
+	dma_unmap_sg(device->dev, data->sg, data->sg_len, conf.direction);
+	return -ENOMEM;
+}
+#else
+/* Blank functions if the DMA engine is not available */
+static inline void mmci_dma_setup(struct mmci_host *host)
+{
+}
+
+static inline void mmci_dma_release(struct mmci_host *host)
+{
+}
+
+static inline void mmci_dma_unmap(struct mmci_host *host, struct mmc_data *data)
+{
+}
+
+static inline void mmci_dma_data_error(struct mmci_host *host)
+{
+}
+
+static inline int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
+{
+	return -ENOSYS;
+}
+#endif
+
 static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 {
 	struct variant_data *variant = host->variant;
@@ -202,9 +443,7 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 
 	host->data = data;
 	host->size = data->blksz * data->blocks;
-	host->data_xfered = 0;
-
-	mmci_init_sg(host, data);
+	data->bytes_xfered = 0;
 
 	clks = (unsigned long long)data->timeout_ns * host->cclk;
 	do_div(clks, 1000000000UL);
@@ -219,15 +458,29 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	BUG_ON(1 << blksz_bits != data->blksz);
 
 	datactrl = MCI_DPSM_ENABLE | blksz_bits << 4;
-	if (data->flags & MMC_DATA_READ) {
+
+	if (data->flags & MMC_DATA_READ)
 		datactrl |= MCI_DPSM_DIRECTION;
+
+	/*
+	 * Attempt to use DMA operation mode, if this
+	 * should fail, fall back to PIO mode
+	 */
+	if (!mmci_dma_start_data(host, datactrl))
+		return;
+
+	/* IRQ mode, map the SG list for CPU reading/writing */
+	mmci_init_sg(host, data);
+
+	if (data->flags & MMC_DATA_READ) {
 		irqmask = MCI_RXFIFOHALFFULLMASK;
 
 		/*
-		 * If we have less than a FIFOSIZE of bytes to transfer,
-		 * trigger a PIO interrupt as soon as any data is available.
+		 * If we have less than the fifo 'half-full' threshold to
+		 * transfer, trigger a PIO interrupt as soon as any data
+		 * is available.
 		 */
-		if (host->size < variant->fifosize)
+		if (host->size < variant->fifohalfsize)
 			irqmask |= MCI_RXDATAAVLBLMASK;
 	} else {
 		/*
@@ -283,49 +536,51 @@ mmci_data_irq(struct mmci_host *host, struct mmc_data *data,
 	if (status & (MCI_DATACRCFAIL|MCI_DATATIMEOUT|MCI_TXUNDERRUN|MCI_RXOVERRUN)) {
 		u32 remain, success;
 
-		/* Calculate how far we are into the transfer */
+		/* Terminate the DMA transfer */
+		if (dma_inprogress(host))
+			mmci_dma_data_error(host);
+
+		/*
+		 * Calculate how far we are into the transfer.  Note that
+		 * the data counter gives the number of bytes transferred
+		 * on the MMC bus, not on the host side.  On reads, this
+		 * can be as much as a FIFO-worth of data ahead.  This
+		 * matters for FIFO overruns only.
+		 */
 		remain = readl(host->base + MMCIDATACNT);
 		success = data->blksz * data->blocks - remain;
 
-		dev_dbg(mmc_dev(host->mmc), "MCI ERROR IRQ (status %08x)\n", status);
+		dev_dbg(mmc_dev(host->mmc), "MCI ERROR IRQ, status 0x%08x at 0x%08x\n",
+			status, success);
 		if (status & MCI_DATACRCFAIL) {
 			/* Last block was not successful */
-			host->data_xfered = round_down(success - 1, data->blksz);
+			success -= 1;
 			data->error = -EILSEQ;
 		} else if (status & MCI_DATATIMEOUT) {
-			host->data_xfered = round_down(success, data->blksz);
 			data->error = -ETIMEDOUT;
-		} else if (status & (MCI_TXUNDERRUN|MCI_RXOVERRUN)) {
-			host->data_xfered = round_down(success, data->blksz);
+		} else if (status & MCI_TXUNDERRUN) {
+			data->error = -EIO;
+		} else if (status & MCI_RXOVERRUN) {
+			if (success > host->variant->fifosize)
+				success -= host->variant->fifosize;
+			else
+				success = 0;
 			data->error = -EIO;
 		}
-
-		/*
-		 * We hit an error condition.  Ensure that any data
-		 * partially written to a page is properly coherent.
-		 */
-		if (data->flags & MMC_DATA_READ) {
-			struct sg_mapping_iter *sg_miter = &host->sg_miter;
-			unsigned long flags;
-
-			local_irq_save(flags);
-			if (sg_miter_next(sg_miter)) {
-				flush_dcache_page(sg_miter->page);
-				sg_miter_stop(sg_miter);
-			}
-			local_irq_restore(flags);
-		}
+		data->bytes_xfered = round_down(success, data->blksz);
 	}
 
 	if (status & MCI_DATABLOCKEND)
 		dev_err(mmc_dev(host->mmc), "stray MCI_DATABLOCKEND interrupt\n");
 
 	if (status & MCI_DATAEND || data->error) {
+		if (dma_inprogress(host))
+			mmci_dma_unmap(host, data);
 		mmci_stop_data(host);
 
 		if (!data->error)
 			/* The error clause is handled above, success! */
-			host->data_xfered += data->blksz * data->blocks;
+			data->bytes_xfered = data->blksz * data->blocks;
 
 		if (!data->stop) {
 			mmci_request_end(host, data->mrq);
@@ -498,9 +753,6 @@ static irqreturn_t mmci_pio_irq(int irq, void *dev_id)
 		if (remain)
 			break;
 
-		if (status & MCI_RXACTIVE)
-			flush_dcache_page(sg_miter->page);
-
 		status = readl(base + MMCISTATUS);
 	} while (1);
 
@@ -509,10 +761,10 @@ static irqreturn_t mmci_pio_irq(int irq, void *dev_id)
 	local_irq_restore(flags);
 
 	/*
-	 * If we're nearing the end of the read, switch to
-	 * "any data available" mode.
+	 * If we have less than the fifo 'half-full' threshold to transfer,
+	 * trigger a PIO interrupt as soon as any data is available.
 	 */
-	if (status & MCI_RXACTIVE && host->size < variant->fifosize)
+	if (status & MCI_RXACTIVE && host->size < variant->fifohalfsize)
 		mmci_set_mask1(host, MCI_RXDATAAVLBLMASK);
 
 	/*
@@ -713,7 +965,8 @@ static const struct mmc_host_ops mmci_ops = {
 	.get_cd		= mmci_get_cd,
 };
 
-static int __devinit mmci_probe(struct amba_device *dev, struct amba_id *id)
+static int __devinit mmci_probe(struct amba_device *dev,
+	const struct amba_id *id)
 {
 	struct mmci_platform_data *plat = dev->dev.platform_data;
 	struct variant_data *variant = id->data;
@@ -776,6 +1029,7 @@ static int __devinit mmci_probe(struct amba_device *dev, struct amba_id *id)
 		dev_dbg(mmc_dev(mmc), "eventual mclk rate: %u Hz\n",
 			host->mclk);
 	}
+	host->phybase = dev->res.start;
 	host->base = ioremap(dev->res.start, resource_size(&dev->res));
 	if (!host->base) {
 		ret = -ENOMEM;
@@ -903,9 +1157,12 @@ static int __devinit mmci_probe(struct amba_device *dev, struct amba_id *id)
 
 	amba_set_drvdata(dev, mmc);
 
-	dev_info(&dev->dev, "%s: PL%03x rev%u at 0x%08llx irq %d,%d\n",
-		mmc_hostname(mmc), amba_part(dev), amba_rev(dev),
-		(unsigned long long)dev->res.start, dev->irq[0], dev->irq[1]);
+	dev_info(&dev->dev, "%s: PL%03x manf %x rev%u at 0x%08llx irq %d,%d (pio)\n",
+		 mmc_hostname(mmc), amba_part(dev), amba_manf(dev),
+		 amba_rev(dev), (unsigned long long)dev->res.start,
+		 dev->irq[0], dev->irq[1]);
+
+	mmci_dma_setup(host);
 
 	mmc_add_host(mmc);
 
@@ -952,6 +1209,7 @@ static int __devexit mmci_remove(struct amba_device *dev)
 		writel(0, host->base + MMCICOMMAND);
 		writel(0, host->base + MMCIDATACTRL);
 
+		mmci_dma_release(host);
 		free_irq(dev->irq[0], host);
 		if (!host->singleirq)
 			free_irq(dev->irq[1], host);
diff --git a/drivers/mmc/host/mmci.h b/drivers/mmc/host/mmci.h
index c1df7b82d36c..ec9a7bc6d0df 100644
--- a/drivers/mmc/host/mmci.h
+++ b/drivers/mmc/host/mmci.h
@@ -148,8 +148,10 @@
 
 struct clk;
 struct variant_data;
+struct dma_chan;
 
 struct mmci_host {
+	phys_addr_t		phybase;
 	void __iomem		*base;
 	struct mmc_request	*mrq;
 	struct mmc_command	*cmd;
@@ -161,8 +163,6 @@ struct mmci_host {
 	int			gpio_cd_irq;
 	bool			singleirq;
 
-	unsigned int		data_xfered;
-
 	spinlock_t		lock;
 
 	unsigned int		mclk;
@@ -181,5 +181,16 @@ struct mmci_host {
 	struct sg_mapping_iter	sg_miter;
 	unsigned int		size;
 	struct regulator	*vcc;
+
+#ifdef CONFIG_DMA_ENGINE
+	/* DMA stuff */
+	struct dma_chan		*dma_current;
+	struct dma_chan		*dma_rx_channel;
+	struct dma_chan		*dma_tx_channel;
+
+#define dma_inprogress(host)	((host)->dma_current)
+#else
+#define dma_inprogress(host)	(0)
+#endif
 };
 
diff --git a/drivers/mmc/host/msm_sdcc.c b/drivers/mmc/host/msm_sdcc.c
index 153ab977a013..a4c865a5286b 100644
--- a/drivers/mmc/host/msm_sdcc.c
+++ b/drivers/mmc/host/msm_sdcc.c
@@ -36,6 +36,7 @@
 #include <linux/io.h>
 #include <linux/memory.h>
 #include <linux/gfp.h>
+#include <linux/gpio.h>
 
 #include <asm/cacheflush.h>
 #include <asm/div64.h>
@@ -266,14 +267,6 @@ msmsdcc_dma_complete_tlet(unsigned long data)
 	dma_unmap_sg(mmc_dev(host->mmc), host->dma.sg, host->dma.num_ents,
 		     host->dma.dir);
 
-	if (host->curr.user_pages) {
-		struct scatterlist *sg = host->dma.sg;
-		int i;
-
-		for (i = 0; i < host->dma.num_ents; i++)
-			flush_dcache_page(sg_page(sg++));
-	}
-
 	host->dma.sg = NULL;
 	host->dma.busy = 0;
 
@@ -941,6 +934,38 @@ msmsdcc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
+static void msmsdcc_setup_gpio(struct msmsdcc_host *host, bool enable)
+{
+	struct msm_mmc_gpio_data *curr;
+	int i, rc = 0;
+
+	if (!host->plat->gpio_data && host->gpio_config_status == enable)
+		return;
+
+	curr = host->plat->gpio_data;
+	for (i = 0; i < curr->size; i++) {
+		if (enable) {
+			rc = gpio_request(curr->gpio[i].no,
+						curr->gpio[i].name);
+			if (rc) {
+				pr_err("%s: gpio_request(%d, %s) failed %d\n",
+					mmc_hostname(host->mmc),
+					curr->gpio[i].no,
+					curr->gpio[i].name, rc);
+				goto free_gpios;
+			}
+		} else {
+			gpio_free(curr->gpio[i].no);
+		}
+	}
+	host->gpio_config_status = enable;
+	return;
+
+free_gpios:
+	for (; i >= 0; i--)
+		gpio_free(curr->gpio[i].no);
+}
+
 static void
 msmsdcc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
@@ -953,6 +978,8 @@ msmsdcc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	msmsdcc_enable_clocks(host);
 
+	spin_unlock_irqrestore(&host->lock, flags);
+
 	if (ios->clock) {
 		if (ios->clock != host->clk_rate) {
 			rc = clk_set_rate(host->clk, ios->clock);
@@ -979,9 +1006,11 @@ msmsdcc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	switch (ios->power_mode) {
 	case MMC_POWER_OFF:
+		msmsdcc_setup_gpio(host, false);
 		break;
 	case MMC_POWER_UP:
 		pwr |= MCI_PWR_UP;
+		msmsdcc_setup_gpio(host, true);
 		break;
 	case MMC_POWER_ON:
 		pwr |= MCI_PWR_ON;
@@ -998,9 +1027,10 @@ msmsdcc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		msmsdcc_writel(host, pwr, MMCIPOWER);
 	}
 #if BUSCLK_PWRSAVE
+	spin_lock_irqsave(&host->lock, flags);
 	msmsdcc_disable_clocks(host, 1);
-#endif
 	spin_unlock_irqrestore(&host->lock, flags);
+#endif
 }
 
 static void msmsdcc_enable_sdio_irq(struct mmc_host *mmc, int enable)
diff --git a/drivers/mmc/host/msm_sdcc.h b/drivers/mmc/host/msm_sdcc.h
index 939557af266d..42d7bbc977c5 100644
--- a/drivers/mmc/host/msm_sdcc.h
+++ b/drivers/mmc/host/msm_sdcc.h
@@ -243,6 +243,7 @@ struct msmsdcc_host {
 	unsigned int		cmd_datactrl;
 	struct mmc_command	*cmd_cmd;
 	u32			cmd_c;
+	bool			gpio_config_status;
 
 	bool prog_scan;
 	bool prog_enable;
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index 4428594261c5..cc20e0259325 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -32,16 +32,14 @@
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
+#include <linux/dmaengine.h>
 
 #include <asm/dma.h>
 #include <asm/irq.h>
 #include <asm/sizes.h>
 #include <mach/mmc.h>
 
-#ifdef CONFIG_ARCH_MX2
-#include <mach/dma-mx1-mx2.h>
-#define HAS_DMA
-#endif
+#include <mach/dma.h>
 
 #define DRIVER_NAME "mxc-mmc"
 
@@ -118,7 +116,8 @@ struct mxcmci_host {
 	void __iomem		*base;
 	int			irq;
 	int			detect_irq;
-	int			dma;
+	struct dma_chan		*dma;
+	struct dma_async_tx_descriptor *desc;
 	int			do_dma;
 	int			default_irq_mask;
 	int			use_sdio;
@@ -129,7 +128,6 @@ struct mxcmci_host {
 	struct mmc_command	*cmd;
 	struct mmc_data		*data;
 
-	unsigned int		dma_nents;
 	unsigned int		datasize;
 	unsigned int		dma_dir;
 
@@ -144,6 +142,11 @@ struct mxcmci_host {
 	spinlock_t		lock;
 
 	struct regulator	*vcc;
+
+	int			burstlen;
+	int			dmareq;
+	struct dma_slave_config dma_slave_config;
+	struct imx_dma_data	dma_data;
 };
 
 static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios);
@@ -206,17 +209,16 @@ static void mxcmci_softreset(struct mxcmci_host *host)
 
 	writew(0xff, host->base + MMC_REG_RES_TO);
 }
+static int mxcmci_setup_dma(struct mmc_host *mmc);
 
 static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 {
 	unsigned int nob = data->blocks;
 	unsigned int blksz = data->blksz;
 	unsigned int datasize = nob * blksz;
-#ifdef HAS_DMA
 	struct scatterlist *sg;
-	int i;
-	int ret;
-#endif
+	int i, nents;
+
 	if (data->flags & MMC_DATA_STREAM)
 		nob = 0xffff;
 
@@ -227,7 +229,9 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 	writew(blksz, host->base + MMC_REG_BLK_LEN);
 	host->datasize = datasize;
 
-#ifdef HAS_DMA
+	if (!mxcmci_use_dma(host))
+		return 0;
+
 	for_each_sg(data->sg, sg, data->sg_len, i) {
 		if (sg->offset & 3 || sg->length & 3) {
 			host->do_dma = 0;
@@ -235,34 +239,30 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 		}
 	}
 
-	if (data->flags & MMC_DATA_READ) {
+	if (data->flags & MMC_DATA_READ)
 		host->dma_dir = DMA_FROM_DEVICE;
-		host->dma_nents = dma_map_sg(mmc_dev(host->mmc), data->sg,
-					     data->sg_len,  host->dma_dir);
-
-		ret = imx_dma_setup_sg(host->dma, data->sg, host->dma_nents,
-				datasize,
-				host->res->start + MMC_REG_BUFFER_ACCESS,
-				DMA_MODE_READ);
-	} else {
+	else
 		host->dma_dir = DMA_TO_DEVICE;
-		host->dma_nents = dma_map_sg(mmc_dev(host->mmc), data->sg,
-					     data->sg_len,  host->dma_dir);
 
-		ret = imx_dma_setup_sg(host->dma, data->sg, host->dma_nents,
-				datasize,
-				host->res->start + MMC_REG_BUFFER_ACCESS,
-				DMA_MODE_WRITE);
-	}
+	nents = dma_map_sg(host->dma->device->dev, data->sg,
+				     data->sg_len,  host->dma_dir);
+	if (nents != data->sg_len)
+		return -EINVAL;
+
+	host->desc = host->dma->device->device_prep_slave_sg(host->dma,
+		data->sg, data->sg_len, host->dma_dir,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 
-	if (ret) {
-		dev_err(mmc_dev(host->mmc), "failed to setup DMA : %d\n", ret);
-		return ret;
+	if (!host->desc) {
+		dma_unmap_sg(host->dma->device->dev, data->sg, data->sg_len,
+				host->dma_dir);
+		host->do_dma = 0;
+		return 0; /* Fall back to PIO */
 	}
 	wmb();
 
-	imx_dma_enable(host->dma);
-#endif /* HAS_DMA */
+	dmaengine_submit(host->desc);
+
 	return 0;
 }
 
@@ -337,13 +337,11 @@ static int mxcmci_finish_data(struct mxcmci_host *host, unsigned int stat)
 	struct mmc_data *data = host->data;
 	int data_error;
 
-#ifdef HAS_DMA
 	if (mxcmci_use_dma(host)) {
-		imx_dma_disable(host->dma);
-		dma_unmap_sg(mmc_dev(host->mmc), data->sg, host->dma_nents,
+		dmaengine_terminate_all(host->dma);
+		dma_unmap_sg(host->dma->device->dev, data->sg, data->sg_len,
 				host->dma_dir);
 	}
-#endif
 
 	if (stat & STATUS_ERR_MASK) {
 		dev_dbg(mmc_dev(host->mmc), "request failed. status: 0x%08x\n",
@@ -545,7 +543,6 @@ static void mxcmci_datawork(struct work_struct *work)
 	}
 }
 
-#ifdef HAS_DMA
 static void mxcmci_data_done(struct mxcmci_host *host, unsigned int stat)
 {
 	struct mmc_data *data = host->data;
@@ -568,7 +565,6 @@ static void mxcmci_data_done(struct mxcmci_host *host, unsigned int stat)
 		mxcmci_finish_request(host, host->req);
 	}
 }
-#endif /* HAS_DMA */
 
 static void mxcmci_cmd_done(struct mxcmci_host *host, unsigned int stat)
 {
@@ -606,12 +602,10 @@ static irqreturn_t mxcmci_irq(int irq, void *devid)
 	sdio_irq = (stat & STATUS_SDIO_INT_ACTIVE) && host->use_sdio;
 	spin_unlock_irqrestore(&host->lock, flags);
 
-#ifdef HAS_DMA
 	if (mxcmci_use_dma(host) &&
 	    (stat & (STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE)))
 		writel(STATUS_READ_OP_DONE | STATUS_WRITE_OP_DONE,
 			host->base + MMC_REG_STATUS);
-#endif
 
 	if (sdio_irq) {
 		writel(STATUS_SDIO_INT_ACTIVE, host->base + MMC_REG_STATUS);
@@ -621,14 +615,14 @@ static irqreturn_t mxcmci_irq(int irq, void *devid)
 	if (stat & STATUS_END_CMD_RESP)
 		mxcmci_cmd_done(host, stat);
 
-#ifdef HAS_DMA
 	if (mxcmci_use_dma(host) &&
 		  (stat & (STATUS_DATA_TRANS_DONE | STATUS_WRITE_OP_DONE)))
 		mxcmci_data_done(host, stat);
-#endif
+
 	if (host->default_irq_mask &&
 		  (stat & (STATUS_CARD_INSERTION | STATUS_CARD_REMOVAL)))
 		mmc_detect_change(host->mmc, msecs_to_jiffies(200));
+
 	return IRQ_HANDLED;
 }
 
@@ -642,9 +636,10 @@ static void mxcmci_request(struct mmc_host *mmc, struct mmc_request *req)
 
 	host->req = req;
 	host->cmdat &= ~CMD_DAT_CONT_INIT;
-#ifdef HAS_DMA
-	host->do_dma = 1;
-#endif
+
+	if (host->dma)
+		host->do_dma = 1;
+
 	if (req->data) {
 		error = mxcmci_setup_data(host, req->data);
 		if (error) {
@@ -660,6 +655,7 @@ static void mxcmci_request(struct mmc_host *mmc, struct mmc_request *req)
 	}
 
 	error = mxcmci_start_cmd(host, req->cmd, cmdat);
+
 out:
 	if (error)
 		mxcmci_finish_request(host, req);
@@ -698,22 +694,46 @@ static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios)
 			prescaler, divider, clk_in, clk_ios);
 }
 
+static int mxcmci_setup_dma(struct mmc_host *mmc)
+{
+	struct mxcmci_host *host = mmc_priv(mmc);
+	struct dma_slave_config *config = &host->dma_slave_config;
+
+	config->dst_addr = host->res->start + MMC_REG_BUFFER_ACCESS;
+	config->src_addr = host->res->start + MMC_REG_BUFFER_ACCESS;
+	config->dst_addr_width = 4;
+	config->src_addr_width = 4;
+	config->dst_maxburst = host->burstlen;
+	config->src_maxburst = host->burstlen;
+
+	return dmaengine_slave_config(host->dma, config);
+}
+
 static void mxcmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct mxcmci_host *host = mmc_priv(mmc);
-#ifdef HAS_DMA
-	unsigned int blen;
+	int burstlen, ret;
+
 	/*
 	 * use burstlen of 64 in 4 bit mode (--> reg value  0)
 	 * use burstlen of 16 in 1 bit mode (--> reg value 16)
 	 */
 	if (ios->bus_width == MMC_BUS_WIDTH_4)
-		blen = 0;
+		burstlen = 64;
 	else
-		blen = 16;
+		burstlen = 16;
+
+	if (mxcmci_use_dma(host) && burstlen != host->burstlen) {
+		host->burstlen = burstlen;
+		ret = mxcmci_setup_dma(mmc);
+		if (ret) {
+			dev_err(mmc_dev(host->mmc),
+				"failed to config DMA channel. Falling back to PIO\n");
+			dma_release_channel(host->dma);
+			host->do_dma = 0;
+		}
+	}
 
-	imx_dma_config_burstlen(host->dma, blen);
-#endif
 	if (ios->bus_width == MMC_BUS_WIDTH_4)
 		host->cmdat |= CMD_DAT_CONT_BUS_WIDTH_4;
 	else
@@ -794,6 +814,18 @@ static void mxcmci_init_card(struct mmc_host *host, struct mmc_card *card)
 		host->caps |= MMC_CAP_4_BIT_DATA;
 }
 
+static bool filter(struct dma_chan *chan, void *param)
+{
+	struct mxcmci_host *host = param;
+
+	if (!imx_dma_is_general_purpose(chan))
+		return false;
+
+	chan->private = &host->dma_data;
+
+	return true;
+}
+
 static const struct mmc_host_ops mxcmci_ops = {
 	.request		= mxcmci_request,
 	.set_ios		= mxcmci_set_ios,
@@ -808,6 +840,7 @@ static int mxcmci_probe(struct platform_device *pdev)
 	struct mxcmci_host *host = NULL;
 	struct resource *iores, *r;
 	int ret = 0, irq;
+	dma_cap_mask_t mask;
 
 	printk(KERN_INFO "i.MX SDHC driver\n");
 
@@ -883,29 +916,23 @@ static int mxcmci_probe(struct platform_device *pdev)
 
 	writel(host->default_irq_mask, host->base + MMC_REG_INT_CNTR);
 
-#ifdef HAS_DMA
-	host->dma = imx_dma_request_by_prio(DRIVER_NAME, DMA_PRIO_LOW);
-	if (host->dma < 0) {
-		dev_err(mmc_dev(host->mmc), "imx_dma_request_by_prio failed\n");
-		ret = -EBUSY;
-		goto out_clk_put;
-	}
-
 	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
-	if (!r) {
-		ret = -EINVAL;
-		goto out_free_dma;
+	if (r) {
+		host->dmareq = r->start;
+		host->dma_data.peripheral_type = IMX_DMATYPE_SDHC;
+		host->dma_data.priority = DMA_PRIO_LOW;
+		host->dma_data.dma_request = host->dmareq;
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		host->dma = dma_request_channel(mask, filter, host);
+		if (host->dma)
+			mmc->max_seg_size = dma_get_max_seg_size(
+					host->dma->device->dev);
 	}
 
-	ret = imx_dma_config_channel(host->dma,
-				     IMX_DMA_MEMSIZE_32 | IMX_DMA_TYPE_FIFO,
-				     IMX_DMA_MEMSIZE_32 | IMX_DMA_TYPE_LINEAR,
-				     r->start, 0);
-	if (ret) {
-		dev_err(mmc_dev(host->mmc), "failed to config DMA channel\n");
-		goto out_free_dma;
-	}
-#endif
+	if (!host->dma)
+		dev_info(mmc_dev(host->mmc), "dma not available. Using PIO\n");
+
 	INIT_WORK(&host->datawork, mxcmci_datawork);
 
 	ret = request_irq(host->irq, mxcmci_irq, 0, DRIVER_NAME, host);
@@ -928,9 +955,8 @@ static int mxcmci_probe(struct platform_device *pdev)
 out_free_irq:
 	free_irq(host->irq, host);
 out_free_dma:
-#ifdef HAS_DMA
-	imx_dma_free(host->dma);
-#endif
+	if (host->dma)
+		dma_release_channel(host->dma);
 out_clk_put:
 	clk_disable(host->clk);
 	clk_put(host->clk);
@@ -960,9 +986,10 @@ static int mxcmci_remove(struct platform_device *pdev)
 
 	free_irq(host->irq, host);
 	iounmap(host->base);
-#ifdef HAS_DMA
-	imx_dma_free(host->dma);
-#endif
+
+	if (host->dma)
+		dma_release_channel(host->dma);
+
 	clk_disable(host->clk);
 	clk_put(host->clk);
 
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
new file mode 100644
index 000000000000..99d39a6a1032
--- /dev/null
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -0,0 +1,874 @@
+/*
+ * Portions copyright (C) 2003 Russell King, PXA MMCI Driver
+ * Portions copyright (C) 2004-2005 Pierre Ossman, W83L51xD SD/MMC driver
+ *
+ * Copyright 2008 Embedded Alley Solutions, Inc.
+ * Copyright 2009-2011 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/highmem.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/completion.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/sdio.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+
+#include <mach/mxs.h>
+#include <mach/common.h>
+#include <mach/dma.h>
+#include <mach/mmc.h>
+
+#define DRIVER_NAME	"mxs-mmc"
+
+/* card detect polling timeout */
+#define MXS_MMC_DETECT_TIMEOUT			(HZ/2)
+
+#define SSP_VERSION_LATEST	4
+#define ssp_is_old()		(host->version < SSP_VERSION_LATEST)
+
+/* SSP registers */
+#define HW_SSP_CTRL0				0x000
+#define  BM_SSP_CTRL0_RUN			(1 << 29)
+#define  BM_SSP_CTRL0_SDIO_IRQ_CHECK		(1 << 28)
+#define  BM_SSP_CTRL0_IGNORE_CRC		(1 << 26)
+#define  BM_SSP_CTRL0_READ			(1 << 25)
+#define  BM_SSP_CTRL0_DATA_XFER			(1 << 24)
+#define  BP_SSP_CTRL0_BUS_WIDTH			(22)
+#define  BM_SSP_CTRL0_BUS_WIDTH			(0x3 << 22)
+#define  BM_SSP_CTRL0_WAIT_FOR_IRQ		(1 << 21)
+#define  BM_SSP_CTRL0_LONG_RESP			(1 << 19)
+#define  BM_SSP_CTRL0_GET_RESP			(1 << 17)
+#define  BM_SSP_CTRL0_ENABLE			(1 << 16)
+#define  BP_SSP_CTRL0_XFER_COUNT		(0)
+#define  BM_SSP_CTRL0_XFER_COUNT		(0xffff)
+#define HW_SSP_CMD0				0x010
+#define  BM_SSP_CMD0_DBL_DATA_RATE_EN		(1 << 25)
+#define  BM_SSP_CMD0_SLOW_CLKING_EN		(1 << 22)
+#define  BM_SSP_CMD0_CONT_CLKING_EN		(1 << 21)
+#define  BM_SSP_CMD0_APPEND_8CYC		(1 << 20)
+#define  BP_SSP_CMD0_BLOCK_SIZE			(16)
+#define  BM_SSP_CMD0_BLOCK_SIZE			(0xf << 16)
+#define  BP_SSP_CMD0_BLOCK_COUNT		(8)
+#define  BM_SSP_CMD0_BLOCK_COUNT		(0xff << 8)
+#define  BP_SSP_CMD0_CMD			(0)
+#define  BM_SSP_CMD0_CMD			(0xff)
+#define HW_SSP_CMD1				0x020
+#define HW_SSP_XFER_SIZE			0x030
+#define HW_SSP_BLOCK_SIZE			0x040
+#define  BP_SSP_BLOCK_SIZE_BLOCK_COUNT		(4)
+#define  BM_SSP_BLOCK_SIZE_BLOCK_COUNT		(0xffffff << 4)
+#define  BP_SSP_BLOCK_SIZE_BLOCK_SIZE		(0)
+#define  BM_SSP_BLOCK_SIZE_BLOCK_SIZE		(0xf)
+#define HW_SSP_TIMING				(ssp_is_old() ? 0x050 : 0x070)
+#define  BP_SSP_TIMING_TIMEOUT			(16)
+#define  BM_SSP_TIMING_TIMEOUT			(0xffff << 16)
+#define  BP_SSP_TIMING_CLOCK_DIVIDE		(8)
+#define  BM_SSP_TIMING_CLOCK_DIVIDE		(0xff << 8)
+#define  BP_SSP_TIMING_CLOCK_RATE		(0)
+#define  BM_SSP_TIMING_CLOCK_RATE		(0xff)
+#define HW_SSP_CTRL1				(ssp_is_old() ? 0x060 : 0x080)
+#define  BM_SSP_CTRL1_SDIO_IRQ			(1 << 31)
+#define  BM_SSP_CTRL1_SDIO_IRQ_EN		(1 << 30)
+#define  BM_SSP_CTRL1_RESP_ERR_IRQ		(1 << 29)
+#define  BM_SSP_CTRL1_RESP_ERR_IRQ_EN		(1 << 28)
+#define  BM_SSP_CTRL1_RESP_TIMEOUT_IRQ		(1 << 27)
+#define  BM_SSP_CTRL1_RESP_TIMEOUT_IRQ_EN	(1 << 26)
+#define  BM_SSP_CTRL1_DATA_TIMEOUT_IRQ		(1 << 25)
+#define  BM_SSP_CTRL1_DATA_TIMEOUT_IRQ_EN	(1 << 24)
+#define  BM_SSP_CTRL1_DATA_CRC_IRQ		(1 << 23)
+#define  BM_SSP_CTRL1_DATA_CRC_IRQ_EN		(1 << 22)
+#define  BM_SSP_CTRL1_FIFO_UNDERRUN_IRQ		(1 << 21)
+#define  BM_SSP_CTRL1_FIFO_UNDERRUN_IRQ_EN	(1 << 20)
+#define  BM_SSP_CTRL1_RECV_TIMEOUT_IRQ		(1 << 17)
+#define  BM_SSP_CTRL1_RECV_TIMEOUT_IRQ_EN	(1 << 16)
+#define  BM_SSP_CTRL1_FIFO_OVERRUN_IRQ		(1 << 15)
+#define  BM_SSP_CTRL1_FIFO_OVERRUN_IRQ_EN	(1 << 14)
+#define  BM_SSP_CTRL1_DMA_ENABLE		(1 << 13)
+#define  BM_SSP_CTRL1_POLARITY			(1 << 9)
+#define  BP_SSP_CTRL1_WORD_LENGTH		(4)
+#define  BM_SSP_CTRL1_WORD_LENGTH		(0xf << 4)
+#define  BP_SSP_CTRL1_SSP_MODE			(0)
+#define  BM_SSP_CTRL1_SSP_MODE			(0xf)
+#define HW_SSP_SDRESP0				(ssp_is_old() ? 0x080 : 0x0a0)
+#define HW_SSP_SDRESP1				(ssp_is_old() ? 0x090 : 0x0b0)
+#define HW_SSP_SDRESP2				(ssp_is_old() ? 0x0a0 : 0x0c0)
+#define HW_SSP_SDRESP3				(ssp_is_old() ? 0x0b0 : 0x0d0)
+#define HW_SSP_STATUS				(ssp_is_old() ? 0x0c0 : 0x100)
+#define  BM_SSP_STATUS_CARD_DETECT		(1 << 28)
+#define  BM_SSP_STATUS_SDIO_IRQ			(1 << 17)
+#define HW_SSP_VERSION				(cpu_is_mx23() ? 0x110 : 0x130)
+#define  BP_SSP_VERSION_MAJOR			(24)
+
+#define BF_SSP(value, field)	(((value) << BP_SSP_##field) & BM_SSP_##field)
+
+#define MXS_MMC_IRQ_BITS	(BM_SSP_CTRL1_SDIO_IRQ		| \
+				 BM_SSP_CTRL1_RESP_ERR_IRQ	| \
+				 BM_SSP_CTRL1_RESP_TIMEOUT_IRQ	| \
+				 BM_SSP_CTRL1_DATA_TIMEOUT_IRQ	| \
+				 BM_SSP_CTRL1_DATA_CRC_IRQ	| \
+				 BM_SSP_CTRL1_FIFO_UNDERRUN_IRQ	| \
+				 BM_SSP_CTRL1_RECV_TIMEOUT_IRQ  | \
+				 BM_SSP_CTRL1_FIFO_OVERRUN_IRQ)
+
+#define SSP_PIO_NUM	3
+
+struct mxs_mmc_host {
+	struct mmc_host			*mmc;
+	struct mmc_request		*mrq;
+	struct mmc_command		*cmd;
+	struct mmc_data			*data;
+
+	void __iomem			*base;
+	int				irq;
+	struct resource			*res;
+	struct resource			*dma_res;
+	struct clk			*clk;
+	unsigned int			clk_rate;
+
+	struct dma_chan         	*dmach;
+	struct mxs_dma_data		dma_data;
+	unsigned int			dma_dir;
+	u32				ssp_pio_words[SSP_PIO_NUM];
+
+	unsigned int			version;
+	unsigned char			bus_width;
+	spinlock_t			lock;
+	int				sdio_irq_en;
+};
+
+static int mxs_mmc_get_ro(struct mmc_host *mmc)
+{
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+	struct mxs_mmc_platform_data *pdata =
+		mmc_dev(host->mmc)->platform_data;
+
+	if (!pdata)
+		return -EFAULT;
+
+	if (!gpio_is_valid(pdata->wp_gpio))
+		return -EINVAL;
+
+	return gpio_get_value(pdata->wp_gpio);
+}
+
+static int mxs_mmc_get_cd(struct mmc_host *mmc)
+{
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+
+	return !(readl(host->base + HW_SSP_STATUS) &
+		 BM_SSP_STATUS_CARD_DETECT);
+}
+
+static void mxs_mmc_reset(struct mxs_mmc_host *host)
+{
+	u32 ctrl0, ctrl1;
+
+	mxs_reset_block(host->base);
+
+	ctrl0 = BM_SSP_CTRL0_IGNORE_CRC;
+	ctrl1 = BF_SSP(0x3, CTRL1_SSP_MODE) |
+		BF_SSP(0x7, CTRL1_WORD_LENGTH) |
+		BM_SSP_CTRL1_DMA_ENABLE |
+		BM_SSP_CTRL1_POLARITY |
+		BM_SSP_CTRL1_RECV_TIMEOUT_IRQ_EN |
+		BM_SSP_CTRL1_DATA_CRC_IRQ_EN |
+		BM_SSP_CTRL1_DATA_TIMEOUT_IRQ_EN |
+		BM_SSP_CTRL1_RESP_TIMEOUT_IRQ_EN |
+		BM_SSP_CTRL1_RESP_ERR_IRQ_EN;
+
+	writel(BF_SSP(0xffff, TIMING_TIMEOUT) |
+	       BF_SSP(2, TIMING_CLOCK_DIVIDE) |
+	       BF_SSP(0, TIMING_CLOCK_RATE),
+	       host->base + HW_SSP_TIMING);
+
+	if (host->sdio_irq_en) {
+		ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
+		ctrl1 |= BM_SSP_CTRL1_SDIO_IRQ_EN;
+	}
+
+	writel(ctrl0, host->base + HW_SSP_CTRL0);
+	writel(ctrl1, host->base + HW_SSP_CTRL1);
+}
+
+static void mxs_mmc_start_cmd(struct mxs_mmc_host *host,
+			      struct mmc_command *cmd);
+
+static void mxs_mmc_request_done(struct mxs_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct mmc_data *data = host->data;
+	struct mmc_request *mrq = host->mrq;
+
+	if (mmc_resp_type(cmd) & MMC_RSP_PRESENT) {
+		if (mmc_resp_type(cmd) & MMC_RSP_136) {
+			cmd->resp[3] = readl(host->base + HW_SSP_SDRESP0);
+			cmd->resp[2] = readl(host->base + HW_SSP_SDRESP1);
+			cmd->resp[1] = readl(host->base + HW_SSP_SDRESP2);
+			cmd->resp[0] = readl(host->base + HW_SSP_SDRESP3);
+		} else {
+			cmd->resp[0] = readl(host->base + HW_SSP_SDRESP0);
+		}
+	}
+
+	if (data) {
+		dma_unmap_sg(mmc_dev(host->mmc), data->sg,
+			     data->sg_len, host->dma_dir);
+		/*
+		 * If there was an error on any block, we mark all
+		 * data blocks as being in error.
+		 */
+		if (!data->error)
+			data->bytes_xfered = data->blocks * data->blksz;
+		else
+			data->bytes_xfered = 0;
+
+		host->data = NULL;
+		if (mrq->stop) {
+			mxs_mmc_start_cmd(host, mrq->stop);
+			return;
+		}
+	}
+
+	host->mrq = NULL;
+	mmc_request_done(host->mmc, mrq);
+}
+
+static void mxs_mmc_dma_irq_callback(void *param)
+{
+	struct mxs_mmc_host *host = param;
+
+	mxs_mmc_request_done(host);
+}
+
+static irqreturn_t mxs_mmc_irq_handler(int irq, void *dev_id)
+{
+	struct mxs_mmc_host *host = dev_id;
+	struct mmc_command *cmd = host->cmd;
+	struct mmc_data *data = host->data;
+	u32 stat;
+
+	spin_lock(&host->lock);
+
+	stat = readl(host->base + HW_SSP_CTRL1);
+	writel(stat & MXS_MMC_IRQ_BITS,
+	       host->base + HW_SSP_CTRL1 + MXS_CLR_ADDR);
+
+	if ((stat & BM_SSP_CTRL1_SDIO_IRQ) && (stat & BM_SSP_CTRL1_SDIO_IRQ_EN))
+		mmc_signal_sdio_irq(host->mmc);
+
+	spin_unlock(&host->lock);
+
+	if (stat & BM_SSP_CTRL1_RESP_TIMEOUT_IRQ)
+		cmd->error = -ETIMEDOUT;
+	else if (stat & BM_SSP_CTRL1_RESP_ERR_IRQ)
+		cmd->error = -EIO;
+
+	if (data) {
+		if (stat & (BM_SSP_CTRL1_DATA_TIMEOUT_IRQ |
+			    BM_SSP_CTRL1_RECV_TIMEOUT_IRQ))
+			data->error = -ETIMEDOUT;
+		else if (stat & BM_SSP_CTRL1_DATA_CRC_IRQ)
+			data->error = -EILSEQ;
+		else if (stat & (BM_SSP_CTRL1_FIFO_UNDERRUN_IRQ |
+				 BM_SSP_CTRL1_FIFO_OVERRUN_IRQ))
+			data->error = -EIO;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static struct dma_async_tx_descriptor *mxs_mmc_prep_dma(
+	struct mxs_mmc_host *host, unsigned int append)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct mmc_data *data = host->data;
+	struct scatterlist * sgl;
+	unsigned int sg_len;
+
+	if (data) {
+		/* data */
+		dma_map_sg(mmc_dev(host->mmc), data->sg,
+			   data->sg_len, host->dma_dir);
+		sgl = data->sg;
+		sg_len = data->sg_len;
+	} else {
+		/* pio */
+		sgl = (struct scatterlist *) host->ssp_pio_words;
+		sg_len = SSP_PIO_NUM;
+	}
+
+	desc = host->dmach->device->device_prep_slave_sg(host->dmach,
+				sgl, sg_len, host->dma_dir, append);
+	if (desc) {
+		desc->callback = mxs_mmc_dma_irq_callback;
+		desc->callback_param = host;
+	} else {
+		if (data)
+			dma_unmap_sg(mmc_dev(host->mmc), data->sg,
+				     data->sg_len, host->dma_dir);
+	}
+
+	return desc;
+}
+
+static void mxs_mmc_bc(struct mxs_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct dma_async_tx_descriptor *desc;
+	u32 ctrl0, cmd0, cmd1;
+
+	ctrl0 = BM_SSP_CTRL0_ENABLE | BM_SSP_CTRL0_IGNORE_CRC;
+	cmd0 = BF_SSP(cmd->opcode, CMD0_CMD) | BM_SSP_CMD0_APPEND_8CYC;
+	cmd1 = cmd->arg;
+
+	if (host->sdio_irq_en) {
+		ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
+		cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN | BM_SSP_CMD0_SLOW_CLKING_EN;
+	}
+
+	host->ssp_pio_words[0] = ctrl0;
+	host->ssp_pio_words[1] = cmd0;
+	host->ssp_pio_words[2] = cmd1;
+	host->dma_dir = DMA_NONE;
+	desc = mxs_mmc_prep_dma(host, 0);
+	if (!desc)
+		goto out;
+
+	dmaengine_submit(desc);
+	return;
+
+out:
+	dev_warn(mmc_dev(host->mmc),
+		 "%s: failed to prep dma\n", __func__);
+}
+
+static void mxs_mmc_ac(struct mxs_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct dma_async_tx_descriptor *desc;
+	u32 ignore_crc, get_resp, long_resp;
+	u32 ctrl0, cmd0, cmd1;
+
+	ignore_crc = (mmc_resp_type(cmd) & MMC_RSP_CRC) ?
+			0 : BM_SSP_CTRL0_IGNORE_CRC;
+	get_resp = (mmc_resp_type(cmd) & MMC_RSP_PRESENT) ?
+			BM_SSP_CTRL0_GET_RESP : 0;
+	long_resp = (mmc_resp_type(cmd) & MMC_RSP_136) ?
+			BM_SSP_CTRL0_LONG_RESP : 0;
+
+	ctrl0 = BM_SSP_CTRL0_ENABLE | ignore_crc | get_resp | long_resp;
+	cmd0 = BF_SSP(cmd->opcode, CMD0_CMD);
+	cmd1 = cmd->arg;
+
+	if (host->sdio_irq_en) {
+		ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
+		cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN | BM_SSP_CMD0_SLOW_CLKING_EN;
+	}
+
+	host->ssp_pio_words[0] = ctrl0;
+	host->ssp_pio_words[1] = cmd0;
+	host->ssp_pio_words[2] = cmd1;
+	host->dma_dir = DMA_NONE;
+	desc = mxs_mmc_prep_dma(host, 0);
+	if (!desc)
+		goto out;
+
+	dmaengine_submit(desc);
+	return;
+
+out:
+	dev_warn(mmc_dev(host->mmc),
+		 "%s: failed to prep dma\n", __func__);
+}
+
+static unsigned short mxs_ns_to_ssp_ticks(unsigned clock_rate, unsigned ns)
+{
+	const unsigned int ssp_timeout_mul = 4096;
+	/*
+	 * Calculate ticks in ms since ns are large numbers
+	 * and might overflow
+	 */
+	const unsigned int clock_per_ms = clock_rate / 1000;
+	const unsigned int ms = ns / 1000;
+	const unsigned int ticks = ms * clock_per_ms;
+	const unsigned int ssp_ticks = ticks / ssp_timeout_mul;
+
+	WARN_ON(ssp_ticks == 0);
+	return ssp_ticks;
+}
+
+static void mxs_mmc_adtc(struct mxs_mmc_host *host)
+{
+	struct mmc_command *cmd = host->cmd;
+	struct mmc_data *data = cmd->data;
+	struct dma_async_tx_descriptor *desc;
+	struct scatterlist *sgl = data->sg, *sg;
+	unsigned int sg_len = data->sg_len;
+	int i;
+
+	unsigned short dma_data_dir, timeout;
+	unsigned int data_size = 0, log2_blksz;
+	unsigned int blocks = data->blocks;
+
+	u32 ignore_crc, get_resp, long_resp, read;
+	u32 ctrl0, cmd0, cmd1, val;
+
+	ignore_crc = (mmc_resp_type(cmd) & MMC_RSP_CRC) ?
+			0 : BM_SSP_CTRL0_IGNORE_CRC;
+	get_resp = (mmc_resp_type(cmd) & MMC_RSP_PRESENT) ?
+			BM_SSP_CTRL0_GET_RESP : 0;
+	long_resp = (mmc_resp_type(cmd) & MMC_RSP_136) ?
+			BM_SSP_CTRL0_LONG_RESP : 0;
+
+	if (data->flags & MMC_DATA_WRITE) {
+		dma_data_dir = DMA_TO_DEVICE;
+		read = 0;
+	} else {
+		dma_data_dir = DMA_FROM_DEVICE;
+		read = BM_SSP_CTRL0_READ;
+	}
+
+	ctrl0 = BF_SSP(host->bus_width, CTRL0_BUS_WIDTH) |
+		ignore_crc | get_resp | long_resp |
+		BM_SSP_CTRL0_DATA_XFER | read |
+		BM_SSP_CTRL0_WAIT_FOR_IRQ |
+		BM_SSP_CTRL0_ENABLE;
+
+	cmd0 = BF_SSP(cmd->opcode, CMD0_CMD);
+
+	/* get logarithm to base 2 of block size for setting register */
+	log2_blksz = ilog2(data->blksz);
+
+	/*
+	 * take special care of the case that data size from data->sg
+	 * is not equal to blocks x blksz
+	 */
+	for_each_sg(sgl, sg, sg_len, i)
+		data_size += sg->length;
+
+	if (data_size != data->blocks * data->blksz)
+		blocks = 1;
+
+	/* xfer count, block size and count need to be set differently */
+	if (ssp_is_old()) {
+		ctrl0 |= BF_SSP(data_size, CTRL0_XFER_COUNT);
+		cmd0 |= BF_SSP(log2_blksz, CMD0_BLOCK_SIZE) |
+			BF_SSP(blocks - 1, CMD0_BLOCK_COUNT);
+	} else {
+		writel(data_size, host->base + HW_SSP_XFER_SIZE);
+		writel(BF_SSP(log2_blksz, BLOCK_SIZE_BLOCK_SIZE) |
+		       BF_SSP(blocks - 1, BLOCK_SIZE_BLOCK_COUNT),
+		       host->base + HW_SSP_BLOCK_SIZE);
+	}
+
+	if ((cmd->opcode == MMC_STOP_TRANSMISSION) ||
+	    (cmd->opcode == SD_IO_RW_EXTENDED))
+		cmd0 |= BM_SSP_CMD0_APPEND_8CYC;
+
+	cmd1 = cmd->arg;
+
+	if (host->sdio_irq_en) {
+		ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
+		cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN | BM_SSP_CMD0_SLOW_CLKING_EN;
+	}
+
+	/* set the timeout count */
+	timeout = mxs_ns_to_ssp_ticks(host->clk_rate, data->timeout_ns);
+	val = readl(host->base + HW_SSP_TIMING);
+	val &= ~(BM_SSP_TIMING_TIMEOUT);
+	val |= BF_SSP(timeout, TIMING_TIMEOUT);
+	writel(val, host->base + HW_SSP_TIMING);
+
+	/* pio */
+	host->ssp_pio_words[0] = ctrl0;
+	host->ssp_pio_words[1] = cmd0;
+	host->ssp_pio_words[2] = cmd1;
+	host->dma_dir = DMA_NONE;
+	desc = mxs_mmc_prep_dma(host, 0);
+	if (!desc)
+		goto out;
+
+	/* append data sg */
+	WARN_ON(host->data != NULL);
+	host->data = data;
+	host->dma_dir = dma_data_dir;
+	desc = mxs_mmc_prep_dma(host, 1);
+	if (!desc)
+		goto out;
+
+	dmaengine_submit(desc);
+	return;
+out:
+	dev_warn(mmc_dev(host->mmc),
+		 "%s: failed to prep dma\n", __func__);
+}
+
+static void mxs_mmc_start_cmd(struct mxs_mmc_host *host,
+			      struct mmc_command *cmd)
+{
+	host->cmd = cmd;
+
+	switch (mmc_cmd_type(cmd)) {
+	case MMC_CMD_BC:
+		mxs_mmc_bc(host);
+		break;
+	case MMC_CMD_BCR:
+		mxs_mmc_ac(host);
+		break;
+	case MMC_CMD_AC:
+		mxs_mmc_ac(host);
+		break;
+	case MMC_CMD_ADTC:
+		mxs_mmc_adtc(host);
+		break;
+	default:
+		dev_warn(mmc_dev(host->mmc),
+			 "%s: unknown MMC command\n", __func__);
+		break;
+	}
+}
+
+static void mxs_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+
+	WARN_ON(host->mrq != NULL);
+	host->mrq = mrq;
+	mxs_mmc_start_cmd(host, mrq->cmd);
+}
+
+static void mxs_mmc_set_clk_rate(struct mxs_mmc_host *host, unsigned int rate)
+{
+	unsigned int ssp_rate, bit_rate;
+	u32 div1, div2;
+	u32 val;
+
+	ssp_rate = clk_get_rate(host->clk);
+
+	for (div1 = 2; div1 < 254; div1 += 2) {
+		div2 = ssp_rate / rate / div1;
+		if (div2 < 0x100)
+			break;
+	}
+
+	if (div1 >= 254) {
+		dev_err(mmc_dev(host->mmc),
+			"%s: cannot set clock to %d\n", __func__, rate);
+		return;
+	}
+
+	if (div2 == 0)
+		bit_rate = ssp_rate / div1;
+	else
+		bit_rate = ssp_rate / div1 / div2;
+
+	val = readl(host->base + HW_SSP_TIMING);
+	val &= ~(BM_SSP_TIMING_CLOCK_DIVIDE | BM_SSP_TIMING_CLOCK_RATE);
+	val |= BF_SSP(div1, TIMING_CLOCK_DIVIDE);
+	val |= BF_SSP(div2 - 1, TIMING_CLOCK_RATE);
+	writel(val, host->base + HW_SSP_TIMING);
+
+	host->clk_rate = bit_rate;
+
+	dev_dbg(mmc_dev(host->mmc),
+		"%s: div1 %d, div2 %d, ssp %d, bit %d, rate %d\n",
+		__func__, div1, div2, ssp_rate, bit_rate, rate);
+}
+
+static void mxs_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+
+	if (ios->bus_width == MMC_BUS_WIDTH_8)
+		host->bus_width = 2;
+	else if (ios->bus_width == MMC_BUS_WIDTH_4)
+		host->bus_width = 1;
+	else
+		host->bus_width = 0;
+
+	if (ios->clock)
+		mxs_mmc_set_clk_rate(host, ios->clock);
+}
+
+static void mxs_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	host->sdio_irq_en = enable;
+
+	if (enable) {
+		writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
+		       host->base + HW_SSP_CTRL0 + MXS_SET_ADDR);
+		writel(BM_SSP_CTRL1_SDIO_IRQ_EN,
+		       host->base + HW_SSP_CTRL1 + MXS_SET_ADDR);
+
+		if (readl(host->base + HW_SSP_STATUS) & BM_SSP_STATUS_SDIO_IRQ)
+			mmc_signal_sdio_irq(host->mmc);
+
+	} else {
+		writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
+		       host->base + HW_SSP_CTRL0 + MXS_CLR_ADDR);
+		writel(BM_SSP_CTRL1_SDIO_IRQ_EN,
+		       host->base + HW_SSP_CTRL1 + MXS_CLR_ADDR);
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static const struct mmc_host_ops mxs_mmc_ops = {
+	.request = mxs_mmc_request,
+	.get_ro = mxs_mmc_get_ro,
+	.get_cd = mxs_mmc_get_cd,
+	.set_ios = mxs_mmc_set_ios,
+	.enable_sdio_irq = mxs_mmc_enable_sdio_irq,
+};
+
+static bool mxs_mmc_dma_filter(struct dma_chan *chan, void *param)
+{
+	struct mxs_mmc_host *host = param;
+
+	if (!mxs_dma_is_apbh(chan))
+		return false;
+
+	if (chan->chan_id != host->dma_res->start)
+		return false;
+
+	chan->private = &host->dma_data;
+
+	return true;
+}
+
+static int mxs_mmc_probe(struct platform_device *pdev)
+{
+	struct mxs_mmc_host *host;
+	struct mmc_host *mmc;
+	struct resource *iores, *dmares, *r;
+	struct mxs_mmc_platform_data *pdata;
+	int ret = 0, irq_err, irq_dma;
+	dma_cap_mask_t mask;
+
+	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dmares = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+	irq_err = platform_get_irq(pdev, 0);
+	irq_dma = platform_get_irq(pdev, 1);
+	if (!iores || !dmares || irq_err < 0 || irq_dma < 0)
+		return -EINVAL;
+
+	r = request_mem_region(iores->start, resource_size(iores), pdev->name);
+	if (!r)
+		return -EBUSY;
+
+	mmc = mmc_alloc_host(sizeof(struct mxs_mmc_host), &pdev->dev);
+	if (!mmc) {
+		ret = -ENOMEM;
+		goto out_release_mem;
+	}
+
+	host = mmc_priv(mmc);
+	host->base = ioremap(r->start, resource_size(r));
+	if (!host->base) {
+		ret = -ENOMEM;
+		goto out_mmc_free;
+	}
+
+	/* only major verion does matter */
+	host->version = readl(host->base + HW_SSP_VERSION) >>
+			BP_SSP_VERSION_MAJOR;
+
+	host->mmc = mmc;
+	host->res = r;
+	host->dma_res = dmares;
+	host->irq = irq_err;
+	host->sdio_irq_en = 0;
+
+	host->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(host->clk)) {
+		ret = PTR_ERR(host->clk);
+		goto out_iounmap;
+	}
+	clk_enable(host->clk);
+
+	mxs_mmc_reset(host);
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	host->dma_data.chan_irq = irq_dma;
+	host->dmach = dma_request_channel(mask, mxs_mmc_dma_filter, host);
+	if (!host->dmach) {
+		dev_err(mmc_dev(host->mmc),
+			"%s: failed to request dma\n", __func__);
+		goto out_clk_put;
+	}
+
+	/* set mmc core parameters */
+	mmc->ops = &mxs_mmc_ops;
+	mmc->caps = MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED |
+		    MMC_CAP_SDIO_IRQ | MMC_CAP_NEEDS_POLL;
+
+	pdata =	mmc_dev(host->mmc)->platform_data;
+	if (pdata) {
+		if (pdata->flags & SLOTF_8_BIT_CAPABLE)
+			mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA;
+		if (pdata->flags & SLOTF_4_BIT_CAPABLE)
+			mmc->caps |= MMC_CAP_4_BIT_DATA;
+	}
+
+	mmc->f_min = 400000;
+	mmc->f_max = 288000000;
+	mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
+
+	mmc->max_segs = 52;
+	mmc->max_blk_size = 1 << 0xf;
+	mmc->max_blk_count = (ssp_is_old()) ? 0xff : 0xffffff;
+	mmc->max_req_size = (ssp_is_old()) ? 0xffff : 0xffffffff;
+	mmc->max_seg_size = dma_get_max_seg_size(host->dmach->device->dev);
+
+	platform_set_drvdata(pdev, mmc);
+
+	ret = request_irq(host->irq, mxs_mmc_irq_handler, 0, DRIVER_NAME, host);
+	if (ret)
+		goto out_free_dma;
+
+	spin_lock_init(&host->lock);
+
+	ret = mmc_add_host(mmc);
+	if (ret)
+		goto out_free_irq;
+
+	dev_info(mmc_dev(host->mmc), "initialized\n");
+
+	return 0;
+
+out_free_irq:
+	free_irq(host->irq, host);
+out_free_dma:
+	if (host->dmach)
+		dma_release_channel(host->dmach);
+out_clk_put:
+	clk_disable(host->clk);
+	clk_put(host->clk);
+out_iounmap:
+	iounmap(host->base);
+out_mmc_free:
+	mmc_free_host(mmc);
+out_release_mem:
+	release_mem_region(iores->start, resource_size(iores));
+	return ret;
+}
+
+static int mxs_mmc_remove(struct platform_device *pdev)
+{
+	struct mmc_host *mmc = platform_get_drvdata(pdev);
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+	struct resource *res = host->res;
+
+	mmc_remove_host(mmc);
+
+	free_irq(host->irq, host);
+
+	platform_set_drvdata(pdev, NULL);
+
+	if (host->dmach)
+		dma_release_channel(host->dmach);
+
+	clk_disable(host->clk);
+	clk_put(host->clk);
+
+	iounmap(host->base);
+
+	mmc_free_host(mmc);
+
+	release_mem_region(res->start, resource_size(res));
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mxs_mmc_suspend(struct device *dev)
+{
+	struct mmc_host *mmc = dev_get_drvdata(dev);
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+	int ret = 0;
+
+	ret = mmc_suspend_host(mmc);
+
+	clk_disable(host->clk);
+
+	return ret;
+}
+
+static int mxs_mmc_resume(struct device *dev)
+{
+	struct mmc_host *mmc = dev_get_drvdata(dev);
+	struct mxs_mmc_host *host = mmc_priv(mmc);
+	int ret = 0;
+
+	clk_enable(host->clk);
+
+	ret = mmc_resume_host(mmc);
+
+	return ret;
+}
+
+static const struct dev_pm_ops mxs_mmc_pm_ops = {
+	.suspend	= mxs_mmc_suspend,
+	.resume		= mxs_mmc_resume,
+};
+#endif
+
+static struct platform_driver mxs_mmc_driver = {
+	.probe		= mxs_mmc_probe,
+	.remove		= mxs_mmc_remove,
+	.driver		= {
+		.name	= DRIVER_NAME,
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &mxs_mmc_pm_ops,
+#endif
+	},
+};
+
+static int __init mxs_mmc_init(void)
+{
+	return platform_driver_register(&mxs_mmc_driver);
+}
+
+static void __exit mxs_mmc_exit(void)
+{
+	platform_driver_unregister(&mxs_mmc_driver);
+}
+
+module_init(mxs_mmc_init);
+module_exit(mxs_mmc_exit);
+
+MODULE_DESCRIPTION("FREESCALE MXS MMC peripheral");
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mmc/host/of_mmc_spi.c b/drivers/mmc/host/of_mmc_spi.c
index 1247e5de9faa..5530def54e5b 100644
--- a/drivers/mmc/host/of_mmc_spi.c
+++ b/drivers/mmc/host/of_mmc_spi.c
@@ -34,6 +34,7 @@ enum {
 struct of_mmc_spi {
 	int gpios[NUM_GPIOS];
 	bool alow_gpios[NUM_GPIOS];
+	int detect_irq;
 	struct mmc_spi_platform_data pdata;
 };
 
@@ -61,6 +62,22 @@ static int of_mmc_spi_get_ro(struct device *dev)
 	return of_mmc_spi_read_gpio(dev, WP_GPIO);
 }
 
+static int of_mmc_spi_init(struct device *dev,
+			   irqreturn_t (*irqhandler)(int, void *), void *mmc)
+{
+	struct of_mmc_spi *oms = to_of_mmc_spi(dev);
+
+	return request_threaded_irq(oms->detect_irq, NULL, irqhandler, 0,
+				    dev_name(dev), mmc);
+}
+
+static void of_mmc_spi_exit(struct device *dev, void *mmc)
+{
+	struct of_mmc_spi *oms = to_of_mmc_spi(dev);
+
+	free_irq(oms->detect_irq, mmc);
+}
+
 struct mmc_spi_platform_data *mmc_spi_get_pdata(struct spi_device *spi)
 {
 	struct device *dev = &spi->dev;
@@ -121,8 +138,13 @@ struct mmc_spi_platform_data *mmc_spi_get_pdata(struct spi_device *spi)
 	if (gpio_is_valid(oms->gpios[WP_GPIO]))
 		oms->pdata.get_ro = of_mmc_spi_get_ro;
 
-	/* We don't support interrupts yet, let's poll. */
-	oms->pdata.caps |= MMC_CAP_NEEDS_POLL;
+	oms->detect_irq = irq_of_parse_and_map(np, 0);
+	if (oms->detect_irq != NO_IRQ) {
+		oms->pdata.init = of_mmc_spi_init;
+		oms->pdata.exit = of_mmc_spi_exit;
+	} else {
+		oms->pdata.caps |= MMC_CAP_NEEDS_POLL;
+	}
 
 	dev->platform_data = &oms->pdata;
 	return dev->platform_data;
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 078fdf11af03..191332b845cc 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -118,7 +118,7 @@
 
 #define MMC_TIMEOUT_MS		20
 #define OMAP_MMC_MASTER_CLOCK	96000000
-#define DRIVER_NAME		"mmci-omap-hs"
+#define DRIVER_NAME		"omap_hsmmc"
 
 /* Timeouts for entering power saving states on inactivity, msec */
 #define OMAP_MMC_DISABLED_TIMEOUT	100
@@ -260,7 +260,7 @@ static int omap_hsmmc_1_set_power(struct device *dev, int slot, int power_on,
 	return ret;
 }
 
-static int omap_hsmmc_23_set_power(struct device *dev, int slot, int power_on,
+static int omap_hsmmc_235_set_power(struct device *dev, int slot, int power_on,
 				   int vdd)
 {
 	struct omap_hsmmc_host *host =
@@ -316,6 +316,12 @@ static int omap_hsmmc_23_set_power(struct device *dev, int slot, int power_on,
 	return ret;
 }
 
+static int omap_hsmmc_4_set_power(struct device *dev, int slot, int power_on,
+					int vdd)
+{
+	return 0;
+}
+
 static int omap_hsmmc_1_set_sleep(struct device *dev, int slot, int sleep,
 				  int vdd, int cardsleep)
 {
@@ -326,7 +332,7 @@ static int omap_hsmmc_1_set_sleep(struct device *dev, int slot, int sleep,
 	return regulator_set_mode(host->vcc, mode);
 }
 
-static int omap_hsmmc_23_set_sleep(struct device *dev, int slot, int sleep,
+static int omap_hsmmc_235_set_sleep(struct device *dev, int slot, int sleep,
 				   int vdd, int cardsleep)
 {
 	struct omap_hsmmc_host *host =
@@ -365,6 +371,12 @@ static int omap_hsmmc_23_set_sleep(struct device *dev, int slot, int sleep,
 		return regulator_enable(host->vcc_aux);
 }
 
+static int omap_hsmmc_4_set_sleep(struct device *dev, int slot, int sleep,
+					int vdd, int cardsleep)
+{
+	return 0;
+}
+
 static int omap_hsmmc_reg_get(struct omap_hsmmc_host *host)
 {
 	struct regulator *reg;
@@ -379,10 +391,14 @@ static int omap_hsmmc_reg_get(struct omap_hsmmc_host *host)
 		break;
 	case OMAP_MMC2_DEVID:
 	case OMAP_MMC3_DEVID:
+	case OMAP_MMC5_DEVID:
 		/* Off-chip level shifting, or none */
-		mmc_slot(host).set_power = omap_hsmmc_23_set_power;
-		mmc_slot(host).set_sleep = omap_hsmmc_23_set_sleep;
+		mmc_slot(host).set_power = omap_hsmmc_235_set_power;
+		mmc_slot(host).set_sleep = omap_hsmmc_235_set_sleep;
 		break;
+	case OMAP_MMC4_DEVID:
+		mmc_slot(host).set_power = omap_hsmmc_4_set_power;
+		mmc_slot(host).set_sleep = omap_hsmmc_4_set_sleep;
 	default:
 		pr_err("MMC%d configuration not supported!\n", host->id);
 		return -EINVAL;
@@ -1555,7 +1571,7 @@ static void omap_hsmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		break;
 	}
 
-	if (host->id == OMAP_MMC1_DEVID) {
+	if (host->pdata->controller_flags & OMAP_HSMMC_SUPPORTS_DUAL_VOLT) {
 		/* Only MMC1 can interface at 3V without some flavor
 		 * of external transceiver; but they all handle 1.8V.
 		 */
@@ -1647,7 +1663,7 @@ static void omap_hsmmc_conf_bus_power(struct omap_hsmmc_host *host)
 	u32 hctl, capa, value;
 
 	/* Only MMC1 supports 3.0V */
-	if (host->id == OMAP_MMC1_DEVID) {
+	if (host->pdata->controller_flags & OMAP_HSMMC_SUPPORTS_DUAL_VOLT) {
 		hctl = SDVS30;
 		capa = VS30 | VS18;
 	} else {
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 9b82910b9dbb..3b5248567973 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -15,9 +15,11 @@
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/clk.h>
+#include <linux/gpio.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/sdhci-pltfm.h>
 #include <mach/hardware.h>
+#include <mach/esdhc.h>
 #include "sdhci.h"
 #include "sdhci-pltfm.h"
 #include "sdhci-esdhc.h"
@@ -30,6 +32,39 @@ static inline void esdhc_clrset_le(struct sdhci_host *host, u32 mask, u32 val, i
 	writel(((readl(base) & ~(mask << shift)) | (val << shift)), base);
 }
 
+static u32 esdhc_readl_le(struct sdhci_host *host, int reg)
+{
+	/* fake CARD_PRESENT flag on mx25/35 */
+	u32 val = readl(host->ioaddr + reg);
+
+	if (unlikely(reg == SDHCI_PRESENT_STATE)) {
+		struct esdhc_platform_data *boarddata =
+				host->mmc->parent->platform_data;
+
+		if (boarddata && gpio_is_valid(boarddata->cd_gpio)
+				&& gpio_get_value(boarddata->cd_gpio))
+			/* no card, if a valid gpio says so... */
+			val &= SDHCI_CARD_PRESENT;
+		else
+			/* ... in all other cases assume card is present */
+			val |= SDHCI_CARD_PRESENT;
+	}
+
+	return val;
+}
+
+static void esdhc_writel_le(struct sdhci_host *host, u32 val, int reg)
+{
+	if (unlikely(reg == SDHCI_INT_ENABLE || reg == SDHCI_SIGNAL_ENABLE))
+		/*
+		 * these interrupts won't work with a custom card_detect gpio
+		 * (only applied to mx25/35)
+		 */
+		val &= ~(SDHCI_INT_CARD_REMOVE | SDHCI_INT_CARD_INSERT);
+
+	writel(val, host->ioaddr + reg);
+}
+
 static u16 esdhc_readw_le(struct sdhci_host *host, int reg)
 {
 	if (unlikely(reg == SDHCI_HOST_VERSION))
@@ -100,10 +135,39 @@ static unsigned int esdhc_pltfm_get_min_clock(struct sdhci_host *host)
 	return clk_get_rate(pltfm_host->clk) / 256 / 16;
 }
 
+static unsigned int esdhc_pltfm_get_ro(struct sdhci_host *host)
+{
+	struct esdhc_platform_data *boarddata = host->mmc->parent->platform_data;
+
+	if (boarddata && gpio_is_valid(boarddata->wp_gpio))
+		return gpio_get_value(boarddata->wp_gpio);
+	else
+		return -ENOSYS;
+}
+
+static struct sdhci_ops sdhci_esdhc_ops = {
+	.read_w = esdhc_readw_le,
+	.write_w = esdhc_writew_le,
+	.write_b = esdhc_writeb_le,
+	.set_clock = esdhc_set_clock,
+	.get_max_clock = esdhc_pltfm_get_max_clock,
+	.get_min_clock = esdhc_pltfm_get_min_clock,
+};
+
+static irqreturn_t cd_irq(int irq, void *data)
+{
+	struct sdhci_host *sdhost = (struct sdhci_host *)data;
+
+	tasklet_schedule(&sdhost->card_tasklet);
+	return IRQ_HANDLED;
+};
+
 static int esdhc_pltfm_init(struct sdhci_host *host, struct sdhci_pltfm_data *pdata)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct esdhc_platform_data *boarddata = host->mmc->parent->platform_data;
 	struct clk *clk;
+	int err;
 
 	clk = clk_get(mmc_dev(host->mmc), NULL);
 	if (IS_ERR(clk)) {
@@ -116,32 +180,78 @@ static int esdhc_pltfm_init(struct sdhci_host *host, struct sdhci_pltfm_data *pd
 	if (cpu_is_mx35() || cpu_is_mx51())
 		host->quirks |= SDHCI_QUIRK_BROKEN_TIMEOUT_VAL;
 
-	/* Fix errata ENGcm07207 which is present on i.MX25 and i.MX35 */
-	if (cpu_is_mx25() || cpu_is_mx35())
+	if (cpu_is_mx25() || cpu_is_mx35()) {
+		/* Fix errata ENGcm07207 present on i.MX25 and i.MX35 */
 		host->quirks |= SDHCI_QUIRK_NO_MULTIBLOCK;
+		/* write_protect can't be routed to controller, use gpio */
+		sdhci_esdhc_ops.get_ro = esdhc_pltfm_get_ro;
+	}
+
+	if (boarddata) {
+		err = gpio_request_one(boarddata->wp_gpio, GPIOF_IN, "ESDHC_WP");
+		if (err) {
+			dev_warn(mmc_dev(host->mmc),
+				"no write-protect pin available!\n");
+			boarddata->wp_gpio = err;
+		}
+
+		err = gpio_request_one(boarddata->cd_gpio, GPIOF_IN, "ESDHC_CD");
+		if (err) {
+			dev_warn(mmc_dev(host->mmc),
+				"no card-detect pin available!\n");
+			goto no_card_detect_pin;
+		}
+
+		/* i.MX5x has issues to be researched */
+		if (!cpu_is_mx25() && !cpu_is_mx35())
+			goto not_supported;
+
+		err = request_irq(gpio_to_irq(boarddata->cd_gpio), cd_irq,
+				 IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				 mmc_hostname(host->mmc), host);
+		if (err) {
+			dev_warn(mmc_dev(host->mmc), "request irq error\n");
+			goto no_card_detect_irq;
+		}
+
+		sdhci_esdhc_ops.write_l = esdhc_writel_le;
+		sdhci_esdhc_ops.read_l = esdhc_readl_le;
+		/* Now we have a working card_detect again */
+		host->quirks &= ~SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+	}
+
+	return 0;
 
+ no_card_detect_irq:
+	gpio_free(boarddata->cd_gpio);
+ no_card_detect_pin:
+	boarddata->cd_gpio = err;
+ not_supported:
 	return 0;
 }
 
 static void esdhc_pltfm_exit(struct sdhci_host *host)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct esdhc_platform_data *boarddata = host->mmc->parent->platform_data;
+
+	if (boarddata && gpio_is_valid(boarddata->wp_gpio))
+		gpio_free(boarddata->wp_gpio);
+
+	if (boarddata && gpio_is_valid(boarddata->cd_gpio)) {
+		gpio_free(boarddata->cd_gpio);
+
+		if (!(host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION))
+			free_irq(gpio_to_irq(boarddata->cd_gpio), host);
+	}
 
 	clk_disable(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 }
 
-static struct sdhci_ops sdhci_esdhc_ops = {
-	.read_w = esdhc_readw_le,
-	.write_w = esdhc_writew_le,
-	.write_b = esdhc_writeb_le,
-	.set_clock = esdhc_set_clock,
-	.get_max_clock = esdhc_pltfm_get_max_clock,
-	.get_min_clock = esdhc_pltfm_get_min_clock,
-};
-
 struct sdhci_pltfm_data sdhci_esdhc_imx_pdata = {
-	.quirks = ESDHC_DEFAULT_QUIRKS | SDHCI_QUIRK_BROKEN_ADMA,
+	.quirks = ESDHC_DEFAULT_QUIRKS | SDHCI_QUIRK_BROKEN_ADMA
+			| SDHCI_QUIRK_BROKEN_CARD_DETECTION,
 	/* ADMA has issues. Might be fixable */
 	.ops = &sdhci_esdhc_ops,
 	.init = esdhc_pltfm_init,
diff --git a/drivers/mmc/host/sdhci-esdhc.h b/drivers/mmc/host/sdhci-esdhc.h
index afaf1bc4913a..c55aae828aac 100644
--- a/drivers/mmc/host/sdhci-esdhc.h
+++ b/drivers/mmc/host/sdhci-esdhc.h
@@ -19,7 +19,6 @@
  */
 
 #define ESDHC_DEFAULT_QUIRKS	(SDHCI_QUIRK_FORCE_BLK_SZ_2048 | \
-				SDHCI_QUIRK_BROKEN_CARD_DETECTION | \
 				SDHCI_QUIRK_NO_BUSY_IRQ | \
 				SDHCI_QUIRK_NONSTANDARD_CLOCK | \
 				SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK | \
diff --git a/drivers/mmc/host/sdhci-of-core.c b/drivers/mmc/host/sdhci-of-core.c
index dd84124f4209..f9b611fc773e 100644
--- a/drivers/mmc/host/sdhci-of-core.c
+++ b/drivers/mmc/host/sdhci-of-core.c
@@ -124,17 +124,20 @@ static bool __devinit sdhci_of_wp_inverted(struct device_node *np)
 #endif
 }
 
-static int __devinit sdhci_of_probe(struct platform_device *ofdev,
-				 const struct of_device_id *match)
+static int __devinit sdhci_of_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
-	struct sdhci_of_data *sdhci_of_data = match->data;
+	struct sdhci_of_data *sdhci_of_data;
 	struct sdhci_host *host;
 	struct sdhci_of_host *of_host;
 	const __be32 *clk;
 	int size;
 	int ret;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+	sdhci_of_data = ofdev->dev.of_match->data;
+
 	if (!of_device_is_available(np))
 		return -ENODEV;
 
@@ -217,7 +220,7 @@ static const struct of_device_id sdhci_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, sdhci_of_match);
 
-static struct of_platform_driver sdhci_of_driver = {
+static struct platform_driver sdhci_of_driver = {
 	.driver = {
 		.name = "sdhci-of",
 		.owner = THIS_MODULE,
@@ -231,13 +234,13 @@ static struct of_platform_driver sdhci_of_driver = {
 
 static int __init sdhci_of_init(void)
 {
-	return of_register_platform_driver(&sdhci_of_driver);
+	return platform_driver_register(&sdhci_of_driver);
 }
 module_init(sdhci_of_init);
 
 static void __exit sdhci_of_exit(void)
 {
-	of_unregister_platform_driver(&sdhci_of_driver);
+	platform_driver_unregister(&sdhci_of_driver);
 }
 module_exit(sdhci_of_exit);
 
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index fcd0e1fcba44..08161f690ae8 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -73,7 +73,8 @@ static unsigned int esdhc_of_get_min_clock(struct sdhci_host *host)
 }
 
 struct sdhci_of_data sdhci_esdhc = {
-	.quirks = ESDHC_DEFAULT_QUIRKS,
+	/* card detection could be handled via GPIO */
+	.quirks = ESDHC_DEFAULT_QUIRKS | SDHCI_QUIRK_BROKEN_CARD_DETECTION,
 	.ops = {
 		.read_l = sdhci_be32bs_readl,
 		.read_w = esdhc_readw,
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 0dc905b20eee..2f8d46854acd 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -547,6 +547,14 @@ static const struct pci_device_id pci_ids[] __devinitdata = {
 	},
 
 	{
+		.vendor         = PCI_VENDOR_ID_RICOH,
+		.device         = 0xe823,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.driver_data    = (kernel_ulong_t)&sdhci_ricoh_mmc,
+	},
+
+	{
 		.vendor		= PCI_VENDOR_ID_ENE,
 		.device		= PCI_DEVICE_ID_ENE_CB712_SD,
 		.subvendor	= PCI_ANY_ID,
@@ -900,9 +908,6 @@ static struct sdhci_pci_slot * __devinit sdhci_pci_probe_slot(
 {
 	struct sdhci_pci_slot *slot;
 	struct sdhci_host *host;
-
-	resource_size_t addr;
-
 	int ret;
 
 	if (!(pci_resource_flags(pdev, bar) & IORESOURCE_MEM)) {
@@ -949,7 +954,6 @@ static struct sdhci_pci_slot * __devinit sdhci_pci_probe_slot(
 		goto free;
 	}
 
-	addr = pci_resource_start(pdev, bar);
 	host->ioaddr = pci_ioremap_bar(pdev, bar);
 	if (!host->ioaddr) {
 		dev_err(&pdev->dev, "failed to remap registers\n");
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index 5309ab95aada..69e3ee321eb5 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -499,6 +499,9 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 	 * SDHCI block, or a missing configuration that needs to be set. */
 	host->quirks |= SDHCI_QUIRK_NO_BUSY_IRQ;
 
+	/* This host supports the Auto CMD12 */
+	host->quirks |= SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12;
+
 	if (pdata->cd_type == S3C_SDHCI_CD_NONE ||
 	    pdata->cd_type == S3C_SDHCI_CD_PERMANENT)
 		host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index 4823ee94a63f..f7e1f964395f 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -169,7 +169,7 @@ static int tegra_sdhci_pltfm_init(struct sdhci_host *host,
 		if (rc) {
 			dev_err(mmc_dev(host->mmc),
 				"failed to allocate wp gpio\n");
-			goto out_cd;
+			goto out_irq;
 		}
 		tegra_gpio_enable(plat->wp_gpio);
 		gpio_direction_input(plat->wp_gpio);
@@ -195,6 +195,9 @@ out_wp:
 		gpio_free(plat->wp_gpio);
 	}
 
+out_irq:
+	if (gpio_is_valid(plat->cd_gpio))
+		free_irq(gpio_to_irq(plat->cd_gpio), host);
 out_cd:
 	if (gpio_is_valid(plat->cd_gpio)) {
 		tegra_gpio_disable(plat->cd_gpio);
@@ -225,6 +228,7 @@ static void tegra_sdhci_pltfm_exit(struct sdhci_host *host)
 	}
 
 	if (gpio_is_valid(plat->cd_gpio)) {
+		free_irq(gpio_to_irq(plat->cd_gpio), host);
 		tegra_gpio_disable(plat->cd_gpio);
 		gpio_free(plat->cd_gpio);
 	}
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 9e15f41f87be..99c372e2639d 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -1545,9 +1545,27 @@ static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
 		 * boundaries, but as we can't disable the feature
 		 * we need to at least restart the transfer.
 		 */
-		if (intmask & SDHCI_INT_DMA_END)
-			sdhci_writel(host, sdhci_readl(host, SDHCI_DMA_ADDRESS),
-				SDHCI_DMA_ADDRESS);
+		if (intmask & SDHCI_INT_DMA_END) {
+			u32 dmastart, dmanow;
+			dmastart = sg_dma_address(host->data->sg);
+			dmanow = sdhci_readl(host, SDHCI_DMA_ADDRESS);
+			if (dmanow == dmastart) {
+				/*
+				 * HW failed to increase the address.
+				 * Update to the next 512KB block boundary.
+				 */
+				dmanow = (dmanow & ~0x7ffff) + 0x80000;
+				if (dmanow > dmastart + host->data->blksz *
+							host->data->blocks) {
+					WARN_ON(1);
+					dmanow = dmastart;
+				}
+				DBG("%s: next DMA address forced "
+				    "from 0x%08x to 0x%08x\n",
+				    mmc_hostname(host->mmc), dmastart, dmanow);
+			}
+			sdhci_writel(host, dmanow, SDHCI_DMA_ADDRESS);
+		}
 
 		if (intmask & SDHCI_INT_DATA_END) {
 			if (host->cmd) {
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 12884c270171..af97015a2fc7 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -169,7 +169,7 @@ struct sh_mmcif_host {
 	struct dma_chan		*chan_rx;
 	struct dma_chan		*chan_tx;
 	struct completion	dma_complete;
-	unsigned int            dma_sglen;
+	bool			dma_active;
 };
 
 static inline void sh_mmcif_bitset(struct sh_mmcif_host *host,
@@ -194,10 +194,12 @@ static void mmcif_dma_complete(void *arg)
 		return;
 
 	if (host->data->flags & MMC_DATA_READ)
-		dma_unmap_sg(&host->pd->dev, host->data->sg, host->dma_sglen,
+		dma_unmap_sg(host->chan_rx->device->dev,
+			     host->data->sg, host->data->sg_len,
 			     DMA_FROM_DEVICE);
 	else
-		dma_unmap_sg(&host->pd->dev, host->data->sg, host->dma_sglen,
+		dma_unmap_sg(host->chan_tx->device->dev,
+			     host->data->sg, host->data->sg_len,
 			     DMA_TO_DEVICE);
 
 	complete(&host->dma_complete);
@@ -211,9 +213,10 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 	dma_cookie_t cookie = -EINVAL;
 	int ret;
 
-	ret = dma_map_sg(&host->pd->dev, sg, host->data->sg_len, DMA_FROM_DEVICE);
+	ret = dma_map_sg(chan->device->dev, sg, host->data->sg_len,
+			 DMA_FROM_DEVICE);
 	if (ret > 0) {
-		host->dma_sglen = ret;
+		host->dma_active = true;
 		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
 			DMA_FROM_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	}
@@ -221,14 +224,9 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 	if (desc) {
 		desc->callback = mmcif_dma_complete;
 		desc->callback_param = host;
-		cookie = desc->tx_submit(desc);
-		if (cookie < 0) {
-			desc = NULL;
-			ret = cookie;
-		} else {
-			sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAREN);
-			chan->device->device_issue_pending(chan);
-		}
+		cookie = dmaengine_submit(desc);
+		sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAREN);
+		dma_async_issue_pending(chan);
 	}
 	dev_dbg(&host->pd->dev, "%s(): mapped %d -> %d, cookie %d\n",
 		__func__, host->data->sg_len, ret, cookie);
@@ -238,7 +236,7 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 		if (ret >= 0)
 			ret = -EIO;
 		host->chan_rx = NULL;
-		host->dma_sglen = 0;
+		host->dma_active = false;
 		dma_release_channel(chan);
 		/* Free the Tx channel too */
 		chan = host->chan_tx;
@@ -263,9 +261,10 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 	dma_cookie_t cookie = -EINVAL;
 	int ret;
 
-	ret = dma_map_sg(&host->pd->dev, sg, host->data->sg_len, DMA_TO_DEVICE);
+	ret = dma_map_sg(chan->device->dev, sg, host->data->sg_len,
+			 DMA_TO_DEVICE);
 	if (ret > 0) {
-		host->dma_sglen = ret;
+		host->dma_active = true;
 		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
 			DMA_TO_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	}
@@ -273,14 +272,9 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 	if (desc) {
 		desc->callback = mmcif_dma_complete;
 		desc->callback_param = host;
-		cookie = desc->tx_submit(desc);
-		if (cookie < 0) {
-			desc = NULL;
-			ret = cookie;
-		} else {
-			sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAWEN);
-			chan->device->device_issue_pending(chan);
-		}
+		cookie = dmaengine_submit(desc);
+		sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAWEN);
+		dma_async_issue_pending(chan);
 	}
 	dev_dbg(&host->pd->dev, "%s(): mapped %d -> %d, cookie %d\n",
 		__func__, host->data->sg_len, ret, cookie);
@@ -290,7 +284,7 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 		if (ret >= 0)
 			ret = -EIO;
 		host->chan_tx = NULL;
-		host->dma_sglen = 0;
+		host->dma_active = false;
 		dma_release_channel(chan);
 		/* Free the Rx channel too */
 		chan = host->chan_rx;
@@ -317,7 +311,7 @@ static bool sh_mmcif_filter(struct dma_chan *chan, void *arg)
 static void sh_mmcif_request_dma(struct sh_mmcif_host *host,
 				 struct sh_mmcif_plat_data *pdata)
 {
-	host->dma_sglen = 0;
+	host->dma_active = false;
 
 	/* We can only either use DMA for both Tx and Rx or not use it at all */
 	if (pdata->dma) {
@@ -364,7 +358,7 @@ static void sh_mmcif_release_dma(struct sh_mmcif_host *host)
 		dma_release_channel(chan);
 	}
 
-	host->dma_sglen = 0;
+	host->dma_active = false;
 }
 
 static void sh_mmcif_clock_control(struct sh_mmcif_host *host, unsigned int clk)
@@ -753,7 +747,7 @@ static void sh_mmcif_start_cmd(struct sh_mmcif_host *host,
 	}
 	sh_mmcif_get_response(host, cmd);
 	if (host->data) {
-		if (!host->dma_sglen) {
+		if (!host->dma_active) {
 			ret = sh_mmcif_data_trans(host, mrq, cmd->opcode);
 		} else {
 			long time =
@@ -765,7 +759,7 @@ static void sh_mmcif_start_cmd(struct sh_mmcif_host *host,
 				ret = time;
 			sh_mmcif_bitclr(host, MMCIF_CE_BUF_ACC,
 					BUF_ACC_DMAREN | BUF_ACC_DMAWEN);
-			host->dma_sglen = 0;
+			host->dma_active = false;
 		}
 		if (ret < 0)
 			mrq->data->bytes_xfered = 0;
@@ -850,15 +844,15 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	struct sh_mmcif_host *host = mmc_priv(mmc);
 	struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
 
-	if (ios->power_mode == MMC_POWER_OFF) {
+	if (ios->power_mode == MMC_POWER_UP) {
+		if (p->set_pwr)
+			p->set_pwr(host->pd, ios->power_mode);
+	} else if (ios->power_mode == MMC_POWER_OFF || !ios->clock) {
 		/* clock stop */
 		sh_mmcif_clock_control(host, 0);
-		if (p->down_pwr)
+		if (ios->power_mode == MMC_POWER_OFF && p->down_pwr)
 			p->down_pwr(host->pd);
 		return;
-	} else if (ios->power_mode == MMC_POWER_UP) {
-		if (p->set_pwr)
-			p->set_pwr(host->pd, ios->power_mode);
 	}
 
 	if (ios->clock)
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index e3c6ef208391..39918cccab68 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -152,7 +152,6 @@ struct tmio_mmc_host {
 	struct tasklet_struct	dma_complete;
 	struct tasklet_struct	dma_issue;
 #ifdef CONFIG_TMIO_MMC_DMA
-	unsigned int            dma_sglen;
 	u8			bounce_buf[PAGE_CACHE_SIZE] __attribute__((aligned(MAX_ALIGN)));
 	struct scatterlist	bounce_sg;
 #endif
@@ -228,36 +227,40 @@ static void tmio_mmc_kunmap_atomic(void *virt, unsigned long *flags)
 
 #ifdef CONFIG_MMC_DEBUG
 
-#define STATUS_TO_TEXT(a) \
+#define STATUS_TO_TEXT(a, status, i) \
 	do { \
-		if (status & TMIO_STAT_##a) \
+		if (status & TMIO_STAT_##a) { \
+			if (i++) \
+				printk(" | "); \
 			printk(#a); \
+		} \
 	} while (0)
 
 void pr_debug_status(u32 status)
 {
+	int i = 0;
 	printk(KERN_DEBUG "status: %08x = ", status);
-	STATUS_TO_TEXT(CARD_REMOVE);
-	STATUS_TO_TEXT(CARD_INSERT);
-	STATUS_TO_TEXT(SIGSTATE);
-	STATUS_TO_TEXT(WRPROTECT);
-	STATUS_TO_TEXT(CARD_REMOVE_A);
-	STATUS_TO_TEXT(CARD_INSERT_A);
-	STATUS_TO_TEXT(SIGSTATE_A);
-	STATUS_TO_TEXT(CMD_IDX_ERR);
-	STATUS_TO_TEXT(STOPBIT_ERR);
-	STATUS_TO_TEXT(ILL_FUNC);
-	STATUS_TO_TEXT(CMD_BUSY);
-	STATUS_TO_TEXT(CMDRESPEND);
-	STATUS_TO_TEXT(DATAEND);
-	STATUS_TO_TEXT(CRCFAIL);
-	STATUS_TO_TEXT(DATATIMEOUT);
-	STATUS_TO_TEXT(CMDTIMEOUT);
-	STATUS_TO_TEXT(RXOVERFLOW);
-	STATUS_TO_TEXT(TXUNDERRUN);
-	STATUS_TO_TEXT(RXRDY);
-	STATUS_TO_TEXT(TXRQ);
-	STATUS_TO_TEXT(ILL_ACCESS);
+	STATUS_TO_TEXT(CARD_REMOVE, status, i);
+	STATUS_TO_TEXT(CARD_INSERT, status, i);
+	STATUS_TO_TEXT(SIGSTATE, status, i);
+	STATUS_TO_TEXT(WRPROTECT, status, i);
+	STATUS_TO_TEXT(CARD_REMOVE_A, status, i);
+	STATUS_TO_TEXT(CARD_INSERT_A, status, i);
+	STATUS_TO_TEXT(SIGSTATE_A, status, i);
+	STATUS_TO_TEXT(CMD_IDX_ERR, status, i);
+	STATUS_TO_TEXT(STOPBIT_ERR, status, i);
+	STATUS_TO_TEXT(ILL_FUNC, status, i);
+	STATUS_TO_TEXT(CMD_BUSY, status, i);
+	STATUS_TO_TEXT(CMDRESPEND, status, i);
+	STATUS_TO_TEXT(DATAEND, status, i);
+	STATUS_TO_TEXT(CRCFAIL, status, i);
+	STATUS_TO_TEXT(DATATIMEOUT, status, i);
+	STATUS_TO_TEXT(CMDTIMEOUT, status, i);
+	STATUS_TO_TEXT(RXOVERFLOW, status, i);
+	STATUS_TO_TEXT(TXUNDERRUN, status, i);
+	STATUS_TO_TEXT(RXRDY, status, i);
+	STATUS_TO_TEXT(TXRQ, status, i);
+	STATUS_TO_TEXT(ILL_ACCESS, status, i);
 	printk("\n");
 }
 
@@ -300,8 +303,7 @@ static void tmio_mmc_set_clock(struct tmio_mmc_host *host, int new_clock)
 
 static void tmio_mmc_clk_stop(struct tmio_mmc_host *host)
 {
-	struct mfd_cell *cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
 	/*
 	 * Testing on sh-mobile showed that SDIO IRQs are unmasked when
@@ -324,8 +326,7 @@ static void tmio_mmc_clk_stop(struct tmio_mmc_host *host)
 
 static void tmio_mmc_clk_start(struct tmio_mmc_host *host)
 {
-	struct mfd_cell *cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
 	sd_ctrl_write16(host, CTL_SD_CARD_CLK_CTL, 0x0100 |
 		sd_ctrl_read16(host, CTL_SD_CARD_CLK_CTL));
@@ -666,8 +667,7 @@ out:
 static irqreturn_t tmio_mmc_irq(int irq, void *devid)
 {
 	struct tmio_mmc_host *host = devid;
-	struct mfd_cell	*cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 	unsigned int ireg, irq_mask, status;
 	unsigned int sdio_ireg, sdio_irq_mask, sdio_status;
 
@@ -796,8 +796,7 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
 	struct scatterlist *sg = host->sg_ptr, *sg_tmp;
 	struct dma_async_tx_descriptor *desc = NULL;
 	struct dma_chan *chan = host->chan_rx;
-	struct mfd_cell	*cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 	dma_cookie_t cookie;
 	int ret, i;
 	bool aligned = true, multiple = true;
@@ -825,23 +824,16 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
 		sg = host->sg_ptr;
 	}
 
-	ret = dma_map_sg(&host->pdev->dev, sg, host->sg_len, DMA_FROM_DEVICE);
-	if (ret > 0) {
-		host->dma_sglen = ret;
+	ret = dma_map_sg(chan->device->dev, sg, host->sg_len, DMA_FROM_DEVICE);
+	if (ret > 0)
 		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
 			DMA_FROM_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	}
 
 	if (desc) {
 		desc->callback = tmio_dma_complete;
 		desc->callback_param = host;
-		cookie = desc->tx_submit(desc);
-		if (cookie < 0) {
-			desc = NULL;
-			ret = cookie;
-		} else {
-			chan->device->device_issue_pending(chan);
-		}
+		cookie = dmaengine_submit(desc);
+		dma_async_issue_pending(chan);
 	}
 	dev_dbg(&host->pdev->dev, "%s(): mapped %d -> %d, cookie %d, rq %p\n",
 		__func__, host->sg_len, ret, cookie, host->mrq);
@@ -873,8 +865,7 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 	struct scatterlist *sg = host->sg_ptr, *sg_tmp;
 	struct dma_async_tx_descriptor *desc = NULL;
 	struct dma_chan *chan = host->chan_tx;
-	struct mfd_cell	*cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 	dma_cookie_t cookie;
 	int ret, i;
 	bool aligned = true, multiple = true;
@@ -906,21 +897,15 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 		sg = host->sg_ptr;
 	}
 
-	ret = dma_map_sg(&host->pdev->dev, sg, host->sg_len, DMA_TO_DEVICE);
-	if (ret > 0) {
-		host->dma_sglen = ret;
+	ret = dma_map_sg(chan->device->dev, sg, host->sg_len, DMA_TO_DEVICE);
+	if (ret > 0)
 		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
 			DMA_TO_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	}
 
 	if (desc) {
 		desc->callback = tmio_dma_complete;
 		desc->callback_param = host;
-		cookie = desc->tx_submit(desc);
-		if (cookie < 0) {
-			desc = NULL;
-			ret = cookie;
-		}
+		cookie = dmaengine_submit(desc);
 	}
 	dev_dbg(&host->pdev->dev, "%s(): mapped %d -> %d, cookie %d, rq %p\n",
 		__func__, host->sg_len, ret, cookie, host->mrq);
@@ -964,7 +949,7 @@ static void tmio_issue_tasklet_fn(unsigned long priv)
 	struct tmio_mmc_host *host = (struct tmio_mmc_host *)priv;
 	struct dma_chan *chan = host->chan_tx;
 
-	chan->device->device_issue_pending(chan);
+	dma_async_issue_pending(chan);
 }
 
 static void tmio_tasklet_fn(unsigned long arg)
@@ -978,10 +963,12 @@ static void tmio_tasklet_fn(unsigned long arg)
 		goto out;
 
 	if (host->data->flags & MMC_DATA_READ)
-		dma_unmap_sg(&host->pdev->dev, host->sg_ptr, host->dma_sglen,
+		dma_unmap_sg(host->chan_rx->device->dev,
+			     host->sg_ptr, host->sg_len,
 			     DMA_FROM_DEVICE);
 	else
-		dma_unmap_sg(&host->pdev->dev, host->sg_ptr, host->dma_sglen,
+		dma_unmap_sg(host->chan_tx->device->dev,
+			     host->sg_ptr, host->sg_len,
 			     DMA_TO_DEVICE);
 
 	tmio_mmc_do_data_irq(host);
@@ -1071,8 +1058,7 @@ static void tmio_mmc_release_dma(struct tmio_mmc_host *host)
 static int tmio_mmc_start_data(struct tmio_mmc_host *host,
 	struct mmc_data *data)
 {
-	struct mfd_cell *cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
 	pr_debug("setup data transfer: blocksize %08x  nr_blocks %d\n",
 		 data->blksz, data->blocks);
@@ -1177,8 +1163,7 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 static int tmio_mmc_get_ro(struct mmc_host *mmc)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
-	struct mfd_cell	*cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
 	return ((pdata->flags & TMIO_MMC_WRPROTECT_DISABLE) ||
 		(sd_ctrl_read32(host, CTL_STATUS) & TMIO_STAT_WRPROTECT)) ? 0 : 1;
@@ -1187,8 +1172,7 @@ static int tmio_mmc_get_ro(struct mmc_host *mmc)
 static int tmio_mmc_get_cd(struct mmc_host *mmc)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
-	struct mfd_cell	*cell = host->pdev->dev.platform_data;
-	struct tmio_mmc_data *pdata = cell->driver_data;
+	struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
 	if (!pdata->get_cd)
 		return -ENOSYS;
@@ -1207,7 +1191,7 @@ static const struct mmc_host_ops tmio_mmc_ops = {
 #ifdef CONFIG_PM
 static int tmio_mmc_suspend(struct platform_device *dev, pm_message_t state)
 {
-	struct mfd_cell	*cell = (struct mfd_cell *)dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct mmc_host *mmc = platform_get_drvdata(dev);
 	int ret;
 
@@ -1222,7 +1206,7 @@ static int tmio_mmc_suspend(struct platform_device *dev, pm_message_t state)
 
 static int tmio_mmc_resume(struct platform_device *dev)
 {
-	struct mfd_cell	*cell = (struct mfd_cell *)dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct mmc_host *mmc = platform_get_drvdata(dev);
 	int ret = 0;
 
@@ -1245,7 +1229,7 @@ out:
 
 static int __devinit tmio_mmc_probe(struct platform_device *dev)
 {
-	struct mfd_cell	*cell = (struct mfd_cell *)dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct tmio_mmc_data *pdata;
 	struct resource *res_ctl;
 	struct tmio_mmc_host *host;
@@ -1260,7 +1244,7 @@ static int __devinit tmio_mmc_probe(struct platform_device *dev)
 	if (!res_ctl)
 		goto out;
 
-	pdata = cell->driver_data;
+	pdata = mfd_get_data(dev);
 	if (!pdata || !pdata->hclk)
 		goto out;
 
@@ -1360,7 +1344,7 @@ out:
 
 static int __devexit tmio_mmc_remove(struct platform_device *dev)
 {
-	struct mfd_cell	*cell = (struct mfd_cell *)dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct mmc_host *mmc = platform_get_drvdata(dev);
 
 	platform_set_drvdata(dev, NULL);
diff --git a/drivers/mmc/host/via-sdmmc.c b/drivers/mmc/host/via-sdmmc.c
index 9ed84ddb4780..8c5b4881ccd6 100644
--- a/drivers/mmc/host/via-sdmmc.c
+++ b/drivers/mmc/host/via-sdmmc.c
@@ -802,12 +802,9 @@ static const struct mmc_host_ops via_sdc_ops = {
 
 static void via_reset_pcictrl(struct via_crdr_mmc_host *host)
 {
-	void __iomem *addrbase;
 	unsigned long flags;
 	u8 gatt;
 
-	addrbase = host->pcictrl_mmiobase;
-
 	spin_lock_irqsave(&host->lock, flags);
 
 	via_save_pcictrlreg(host);
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 8506578e6a35..3db0cb083d31 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -216,8 +216,7 @@ static void __devinit of_free_probes(const char **probes)
 }
 #endif
 
-static int __devinit of_flash_probe(struct platform_device *dev,
-				    const struct of_device_id *match)
+static int __devinit of_flash_probe(struct platform_device *dev)
 {
 #ifdef CONFIG_MTD_PARTITIONS
 	const char **part_probe_types;
@@ -225,7 +224,7 @@ static int __devinit of_flash_probe(struct platform_device *dev,
 	struct device_node *dp = dev->dev.of_node;
 	struct resource res;
 	struct of_flash *info;
-	const char *probe_type = match->data;
+	const char *probe_type;
 	const __be32 *width;
 	int err;
 	int i;
@@ -235,6 +234,10 @@ static int __devinit of_flash_probe(struct platform_device *dev,
 	struct mtd_info **mtd_list = NULL;
 	resource_size_t res_size;
 
+	if (!dev->dev.of_match)
+		return -EINVAL;
+	probe_type = dev->dev.of_match->data;
+
 	reg_tuple_size = (of_n_addr_cells(dp) + of_n_size_cells(dp)) * sizeof(u32);
 
 	/*
@@ -418,7 +421,7 @@ static struct of_device_id of_flash_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_flash_match);
 
-static struct of_platform_driver of_flash_driver = {
+static struct platform_driver of_flash_driver = {
 	.driver = {
 		.name = "of-flash",
 		.owner = THIS_MODULE,
@@ -430,12 +433,12 @@ static struct of_platform_driver of_flash_driver = {
 
 static int __init of_flash_init(void)
 {
-	return of_register_platform_driver(&of_flash_driver);
+	return platform_driver_register(&of_flash_driver);
 }
 
 static void __exit of_flash_exit(void)
 {
-	of_unregister_platform_driver(&of_flash_driver);
+	platform_driver_unregister(&of_flash_driver);
 }
 
 module_init(of_flash_init);
diff --git a/drivers/mtd/maps/sun_uflash.c b/drivers/mtd/maps/sun_uflash.c
index 3582ba1f9b09..3f1cb328a574 100644
--- a/drivers/mtd/maps/sun_uflash.c
+++ b/drivers/mtd/maps/sun_uflash.c
@@ -108,7 +108,7 @@ int uflash_devinit(struct platform_device *op, struct device_node *dp)
 	return 0;
 }
 
-static int __devinit uflash_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit uflash_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 
@@ -148,7 +148,7 @@ static const struct of_device_id uflash_match[] = {
 
 MODULE_DEVICE_TABLE(of, uflash_match);
 
-static struct of_platform_driver uflash_driver = {
+static struct platform_driver uflash_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -160,12 +160,12 @@ static struct of_platform_driver uflash_driver = {
 
 static int __init uflash_init(void)
 {
-	return of_register_platform_driver(&uflash_driver);
+	return platform_driver_register(&uflash_driver);
 }
 
 static void __exit uflash_exit(void)
 {
-	of_unregister_platform_driver(&uflash_driver);
+	platform_driver_unregister(&uflash_driver);
 }
 
 module_init(uflash_init);
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index c89592239bc7..4f6c06f16328 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -106,23 +106,6 @@ config MTD_NAND_OMAP2
 	help
           Support for NAND flash on Texas Instruments OMAP2 and OMAP3 platforms.
 
-config MTD_NAND_OMAP_PREFETCH
-	bool "GPMC prefetch support for NAND Flash device"
-	depends on MTD_NAND_OMAP2
-	default y
-	help
-	 The NAND device can be accessed for Read/Write using GPMC PREFETCH engine
-	 to improve the performance.
-
-config MTD_NAND_OMAP_PREFETCH_DMA
-	depends on MTD_NAND_OMAP_PREFETCH
-	bool "DMA mode"
-	default n
-	help
-	 The GPMC PREFETCH engine can be configured eigther in MPU interrupt mode
-	 or in DMA interrupt mode.
-	 Say y for DMA mode or MPU mode will be used
-
 config MTD_NAND_IDS
 	tristate
 
@@ -476,7 +459,7 @@ config MTD_NAND_MPC5121_NFC
 
 config MTD_NAND_MXC
 	tristate "MXC NAND support"
-	depends on ARCH_MX2 || ARCH_MX25 || ARCH_MX3 || ARCH_MX51
+	depends on IMX_HAVE_PLATFORM_MXC_NAND
 	help
 	  This enables the driver for the NAND flash controller on the
 	  MXC processors.
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index efdcca94ce55..073ee026a17c 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -217,8 +217,7 @@ err:
 	return ret;
 }
 
-static int __devinit fun_probe(struct platform_device *ofdev,
-			       const struct of_device_id *ofid)
+static int __devinit fun_probe(struct platform_device *ofdev)
 {
 	struct fsl_upm_nand *fun;
 	struct resource io_res;
@@ -360,7 +359,7 @@ static const struct of_device_id of_fun_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_fun_match);
 
-static struct of_platform_driver of_fun_driver = {
+static struct platform_driver of_fun_driver = {
 	.driver = {
 		.name = "fsl,upm-nand",
 		.owner = THIS_MODULE,
@@ -372,13 +371,13 @@ static struct of_platform_driver of_fun_driver = {
 
 static int __init fun_module_init(void)
 {
-	return of_register_platform_driver(&of_fun_driver);
+	return platform_driver_register(&of_fun_driver);
 }
 module_init(fun_module_init);
 
 static void __exit fun_module_exit(void)
 {
-	of_unregister_platform_driver(&of_fun_driver);
+	platform_driver_unregister(&of_fun_driver);
 }
 module_exit(fun_module_exit);
 
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index 469e649c911c..c2f95437e5e9 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -650,8 +650,7 @@ static void mpc5121_nfc_free(struct device *dev, struct mtd_info *mtd)
 		iounmap(prv->csreg);
 }
 
-static int __devinit mpc5121_nfc_probe(struct platform_device *op,
-					const struct of_device_id *match)
+static int __devinit mpc5121_nfc_probe(struct platform_device *op)
 {
 	struct device_node *rootnode, *dn = op->dev.of_node;
 	struct device *dev = &op->dev;
@@ -891,7 +890,7 @@ static struct of_device_id mpc5121_nfc_match[] __devinitdata = {
 	{},
 };
 
-static struct of_platform_driver mpc5121_nfc_driver = {
+static struct platform_driver mpc5121_nfc_driver = {
 	.probe		= mpc5121_nfc_probe,
 	.remove		= __devexit_p(mpc5121_nfc_remove),
 	.driver		= {
@@ -903,14 +902,14 @@ static struct of_platform_driver mpc5121_nfc_driver = {
 
 static int __init mpc5121_nfc_init(void)
 {
-	return of_register_platform_driver(&mpc5121_nfc_driver);
+	return platform_driver_register(&mpc5121_nfc_driver);
 }
 
 module_init(mpc5121_nfc_init);
 
 static void __exit mpc5121_nfc_cleanup(void)
 {
-	of_unregister_platform_driver(&mpc5121_nfc_driver);
+	platform_driver_unregister(&mpc5121_nfc_driver);
 }
 
 module_exit(mpc5121_nfc_cleanup);
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index ef932ba55a0b..5ae1d9ee2cf1 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -722,9 +722,8 @@ static void mxc_do_addr_cycle(struct mtd_info *mtd, int column, int page_addr)
 		/*
 		 * MXC NANDFC can only perform full page+spare or
 		 * spare-only read/write.  When the upper layers
-		 * layers perform a read/write buf operation,
-		 * we will used the saved column address to index into
-		 * the full page.
+		 * perform a read/write buf operation, the saved column
+		  * address is used to index into the full page.
 		 */
 		host->send_addr(host, 0, page_addr == -1);
 		if (mtd->writesize > 512)
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index c9ae0a5023b6..bbe6d451290d 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -225,8 +225,7 @@ err:
 	return ret;
 }
 
-static int __devinit ndfc_probe(struct platform_device *ofdev,
-				const struct of_device_id *match)
+static int __devinit ndfc_probe(struct platform_device *ofdev)
 {
 	struct ndfc_controller *ndfc = &ndfc_ctrl;
 	const __be32 *reg;
@@ -292,7 +291,7 @@ static const struct of_device_id ndfc_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ndfc_match);
 
-static struct of_platform_driver ndfc_driver = {
+static struct platform_driver ndfc_driver = {
 	.driver = {
 		.name = "ndfc",
 		.owner = THIS_MODULE,
@@ -304,12 +303,12 @@ static struct of_platform_driver ndfc_driver = {
 
 static int __init ndfc_nand_init(void)
 {
-	return of_register_platform_driver(&ndfc_driver);
+	return platform_driver_register(&ndfc_driver);
 }
 
 static void __exit ndfc_nand_exit(void)
 {
-	of_unregister_platform_driver(&ndfc_driver);
+	platform_driver_unregister(&ndfc_driver);
 }
 
 module_init(ndfc_nand_init);
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index 15682ec8530e..4e33972ad17a 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -11,6 +11,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/delay.h>
+#include <linux/interrupt.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
 #include <linux/mtd/mtd.h>
@@ -24,6 +25,7 @@
 #include <plat/nand.h>
 
 #define	DRIVER_NAME	"omap2-nand"
+#define	OMAP_NAND_TIMEOUT_MS	5000
 
 #define NAND_Ecc_P1e		(1 << 0)
 #define NAND_Ecc_P2e		(1 << 1)
@@ -96,26 +98,19 @@
 static const char *part_probes[] = { "cmdlinepart", NULL };
 #endif
 
-#ifdef CONFIG_MTD_NAND_OMAP_PREFETCH
-static int use_prefetch = 1;
-
-/* "modprobe ... use_prefetch=0" etc */
-module_param(use_prefetch, bool, 0);
-MODULE_PARM_DESC(use_prefetch, "enable/disable use of PREFETCH");
-
-#ifdef CONFIG_MTD_NAND_OMAP_PREFETCH_DMA
-static int use_dma = 1;
+/* oob info generated runtime depending on ecc algorithm and layout selected */
+static struct nand_ecclayout omap_oobinfo;
+/* Define some generic bad / good block scan pattern which are used
+ * while scanning a device for factory marked good / bad blocks
+ */
+static uint8_t scan_ff_pattern[] = { 0xff };
+static struct nand_bbt_descr bb_descrip_flashbased = {
+	.options = NAND_BBT_SCANEMPTY | NAND_BBT_SCANALLPAGES,
+	.offs = 0,
+	.len = 1,
+	.pattern = scan_ff_pattern,
+};
 
-/* "modprobe ... use_dma=0" etc */
-module_param(use_dma, bool, 0);
-MODULE_PARM_DESC(use_dma, "enable/disable use of DMA");
-#else
-static const int use_dma;
-#endif
-#else
-const int use_prefetch;
-static const int use_dma;
-#endif
 
 struct omap_nand_info {
 	struct nand_hw_control		controller;
@@ -129,6 +124,13 @@ struct omap_nand_info {
 	unsigned long			phys_base;
 	struct completion		comp;
 	int				dma_ch;
+	int				gpmc_irq;
+	enum {
+		OMAP_NAND_IO_READ = 0,	/* read */
+		OMAP_NAND_IO_WRITE,	/* write */
+	} iomode;
+	u_char				*buf;
+	int					buf_len;
 };
 
 /**
@@ -256,7 +258,8 @@ static void omap_read_buf_pref(struct mtd_info *mtd, u_char *buf, int len)
 	}
 
 	/* configure and start prefetch transfer */
-	ret = gpmc_prefetch_enable(info->gpmc_cs, 0x0, len, 0x0);
+	ret = gpmc_prefetch_enable(info->gpmc_cs,
+			PREFETCH_FIFOTHRESHOLD_MAX, 0x0, len, 0x0);
 	if (ret) {
 		/* PFPW engine is busy, use cpu copy method */
 		if (info->nand.options & NAND_BUSWIDTH_16)
@@ -288,9 +291,10 @@ static void omap_write_buf_pref(struct mtd_info *mtd,
 {
 	struct omap_nand_info *info = container_of(mtd,
 						struct omap_nand_info, mtd);
-	uint32_t pref_count = 0, w_count = 0;
+	uint32_t w_count = 0;
 	int i = 0, ret = 0;
 	u16 *p;
+	unsigned long tim, limit;
 
 	/* take care of subpage writes */
 	if (len % 2 != 0) {
@@ -300,7 +304,8 @@ static void omap_write_buf_pref(struct mtd_info *mtd,
 	}
 
 	/*  configure and start prefetch transfer */
-	ret = gpmc_prefetch_enable(info->gpmc_cs, 0x0, len, 0x1);
+	ret = gpmc_prefetch_enable(info->gpmc_cs,
+			PREFETCH_FIFOTHRESHOLD_MAX, 0x0, len, 0x1);
 	if (ret) {
 		/* PFPW engine is busy, use cpu copy method */
 		if (info->nand.options & NAND_BUSWIDTH_16)
@@ -316,15 +321,17 @@ static void omap_write_buf_pref(struct mtd_info *mtd,
 				iowrite16(*p++, info->nand.IO_ADDR_W);
 		}
 		/* wait for data to flushed-out before reset the prefetch */
-		do {
-			pref_count = gpmc_read_status(GPMC_PREFETCH_COUNT);
-		} while (pref_count);
+		tim = 0;
+		limit = (loops_per_jiffy *
+					msecs_to_jiffies(OMAP_NAND_TIMEOUT_MS));
+		while (gpmc_read_status(GPMC_PREFETCH_COUNT) && (tim++ < limit))
+			cpu_relax();
+
 		/* disable and stop the PFPW engine */
 		gpmc_prefetch_reset(info->gpmc_cs);
 	}
 }
 
-#ifdef CONFIG_MTD_NAND_OMAP_PREFETCH_DMA
 /*
  * omap_nand_dma_cb: callback on the completion of dma transfer
  * @lch: logical channel
@@ -348,14 +355,15 @@ static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
 {
 	struct omap_nand_info *info = container_of(mtd,
 					struct omap_nand_info, mtd);
-	uint32_t prefetch_status = 0;
 	enum dma_data_direction dir = is_write ? DMA_TO_DEVICE :
 							DMA_FROM_DEVICE;
 	dma_addr_t dma_addr;
 	int ret;
+	unsigned long tim, limit;
 
-	/* The fifo depth is 64 bytes. We have a sync at each frame and frame
-	 * length is 64 bytes.
+	/* The fifo depth is 64 bytes max.
+	 * But configure the FIFO-threahold to 32 to get a sync at each frame
+	 * and frame length is 32 bytes.
 	 */
 	int buf_len = len >> 6;
 
@@ -396,9 +404,10 @@ static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
 					OMAP24XX_DMA_GPMC, OMAP_DMA_SRC_SYNC);
 	}
 	/*  configure and start prefetch transfer */
-	ret = gpmc_prefetch_enable(info->gpmc_cs, 0x1, len, is_write);
+	ret = gpmc_prefetch_enable(info->gpmc_cs,
+			PREFETCH_FIFOTHRESHOLD_MAX, 0x1, len, is_write);
 	if (ret)
-		/* PFPW engine is busy, use cpu copy methode */
+		/* PFPW engine is busy, use cpu copy method */
 		goto out_copy;
 
 	init_completion(&info->comp);
@@ -407,10 +416,11 @@ static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
 
 	/* setup and start DMA using dma_addr */
 	wait_for_completion(&info->comp);
+	tim = 0;
+	limit = (loops_per_jiffy * msecs_to_jiffies(OMAP_NAND_TIMEOUT_MS));
+	while (gpmc_read_status(GPMC_PREFETCH_COUNT) && (tim++ < limit))
+		cpu_relax();
 
-	do {
-		prefetch_status = gpmc_read_status(GPMC_PREFETCH_COUNT);
-	} while (prefetch_status);
 	/* disable and stop the PFPW engine */
 	gpmc_prefetch_reset(info->gpmc_cs);
 
@@ -426,14 +436,6 @@ out_copy:
 			: omap_write_buf8(mtd, (u_char *) addr, len);
 	return 0;
 }
-#else
-static void omap_nand_dma_cb(int lch, u16 ch_status, void *data) {}
-static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
-					unsigned int len, int is_write)
-{
-	return 0;
-}
-#endif
 
 /**
  * omap_read_buf_dma_pref - read data from NAND controller into buffer
@@ -466,6 +468,157 @@ static void omap_write_buf_dma_pref(struct mtd_info *mtd,
 		omap_nand_dma_transfer(mtd, (u_char *) buf, len, 0x1);
 }
 
+/*
+ * omap_nand_irq - GMPC irq handler
+ * @this_irq: gpmc irq number
+ * @dev: omap_nand_info structure pointer is passed here
+ */
+static irqreturn_t omap_nand_irq(int this_irq, void *dev)
+{
+	struct omap_nand_info *info = (struct omap_nand_info *) dev;
+	u32 bytes;
+	u32 irq_stat;
+
+	irq_stat = gpmc_read_status(GPMC_GET_IRQ_STATUS);
+	bytes = gpmc_read_status(GPMC_PREFETCH_FIFO_CNT);
+	bytes = bytes  & 0xFFFC; /* io in multiple of 4 bytes */
+	if (info->iomode == OMAP_NAND_IO_WRITE) { /* checks for write io */
+		if (irq_stat & 0x2)
+			goto done;
+
+		if (info->buf_len && (info->buf_len < bytes))
+			bytes = info->buf_len;
+		else if (!info->buf_len)
+			bytes = 0;
+		iowrite32_rep(info->nand.IO_ADDR_W,
+						(u32 *)info->buf, bytes >> 2);
+		info->buf = info->buf + bytes;
+		info->buf_len -= bytes;
+
+	} else {
+		ioread32_rep(info->nand.IO_ADDR_R,
+						(u32 *)info->buf, bytes >> 2);
+		info->buf = info->buf + bytes;
+
+		if (irq_stat & 0x2)
+			goto done;
+	}
+	gpmc_cs_configure(info->gpmc_cs, GPMC_SET_IRQ_STATUS, irq_stat);
+
+	return IRQ_HANDLED;
+
+done:
+	complete(&info->comp);
+	/* disable irq */
+	gpmc_cs_configure(info->gpmc_cs, GPMC_ENABLE_IRQ, 0);
+
+	/* clear status */
+	gpmc_cs_configure(info->gpmc_cs, GPMC_SET_IRQ_STATUS, irq_stat);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * omap_read_buf_irq_pref - read data from NAND controller into buffer
+ * @mtd: MTD device structure
+ * @buf: buffer to store date
+ * @len: number of bytes to read
+ */
+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);
+	int ret = 0;
+
+	if (len <= mtd->oobsize) {
+		omap_read_buf_pref(mtd, buf, len);
+		return;
+	}
+
+	info->iomode = OMAP_NAND_IO_READ;
+	info->buf = buf;
+	init_completion(&info->comp);
+
+	/*  configure and start prefetch transfer */
+	ret = gpmc_prefetch_enable(info->gpmc_cs,
+			PREFETCH_FIFOTHRESHOLD_MAX/2, 0x0, len, 0x0);
+	if (ret)
+		/* PFPW engine is busy, use cpu copy method */
+		goto out_copy;
+
+	info->buf_len = len;
+	/* enable irq */
+	gpmc_cs_configure(info->gpmc_cs, GPMC_ENABLE_IRQ,
+		(GPMC_IRQ_FIFOEVENTENABLE | GPMC_IRQ_COUNT_EVENT));
+
+	/* waiting for read to complete */
+	wait_for_completion(&info->comp);
+
+	/* disable and stop the PFPW engine */
+	gpmc_prefetch_reset(info->gpmc_cs);
+	return;
+
+out_copy:
+	if (info->nand.options & NAND_BUSWIDTH_16)
+		omap_read_buf16(mtd, buf, len);
+	else
+		omap_read_buf8(mtd, buf, len);
+}
+
+/*
+ * omap_write_buf_irq_pref - write buffer to NAND controller
+ * @mtd: MTD device structure
+ * @buf: data buffer
+ * @len: number of bytes to write
+ */
+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);
+	int ret = 0;
+	unsigned long tim, limit;
+
+	if (len <= mtd->oobsize) {
+		omap_write_buf_pref(mtd, buf, len);
+		return;
+	}
+
+	info->iomode = OMAP_NAND_IO_WRITE;
+	info->buf = (u_char *) buf;
+	init_completion(&info->comp);
+
+	/* configure and start prefetch transfer : size=24 */
+	ret = gpmc_prefetch_enable(info->gpmc_cs,
+			(PREFETCH_FIFOTHRESHOLD_MAX * 3) / 8, 0x0, len, 0x1);
+	if (ret)
+		/* PFPW engine is busy, use cpu copy method */
+		goto out_copy;
+
+	info->buf_len = len;
+	/* enable irq */
+	gpmc_cs_configure(info->gpmc_cs, GPMC_ENABLE_IRQ,
+			(GPMC_IRQ_FIFOEVENTENABLE | GPMC_IRQ_COUNT_EVENT));
+
+	/* waiting for write to complete */
+	wait_for_completion(&info->comp);
+	/* wait for data to flushed-out before reset the prefetch */
+	tim = 0;
+	limit = (loops_per_jiffy *  msecs_to_jiffies(OMAP_NAND_TIMEOUT_MS));
+	while (gpmc_read_status(GPMC_PREFETCH_COUNT) && (tim++ < limit))
+		cpu_relax();
+
+	/* disable and stop the PFPW engine */
+	gpmc_prefetch_reset(info->gpmc_cs);
+	return;
+
+out_copy:
+	if (info->nand.options & NAND_BUSWIDTH_16)
+		omap_write_buf16(mtd, buf, len);
+	else
+		omap_write_buf8(mtd, buf, len);
+}
+
 /**
  * omap_verify_buf - Verify chip data against buffer
  * @mtd: MTD device structure
@@ -487,8 +640,6 @@ static int omap_verify_buf(struct mtd_info *mtd, const u_char * buf, int len)
 	return 0;
 }
 
-#ifdef CONFIG_MTD_NAND_OMAP_HWECC
-
 /**
  * gen_true_ecc - This function will generate true ECC value
  * @ecc_buf: buffer to store ecc code
@@ -708,8 +859,6 @@ static void omap_enable_hwecc(struct mtd_info *mtd, int mode)
 	gpmc_enable_hwecc(info->gpmc_cs, mode, dev_width, info->nand.ecc.size);
 }
 
-#endif
-
 /**
  * omap_wait - wait until the command is done
  * @mtd: MTD device structure
@@ -779,6 +928,7 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 	struct omap_nand_info		*info;
 	struct omap_nand_platform_data	*pdata;
 	int				err;
+	int				i, offset;
 
 	pdata = pdev->dev.platform_data;
 	if (pdata == NULL) {
@@ -804,7 +954,7 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 	info->mtd.name		= dev_name(&pdev->dev);
 	info->mtd.owner		= THIS_MODULE;
 
-	info->nand.options	|= pdata->devsize ? NAND_BUSWIDTH_16 : 0;
+	info->nand.options	= pdata->devsize;
 	info->nand.options	|= NAND_SKIP_BBTSCAN;
 
 	/* NAND write protect off */
@@ -842,28 +992,13 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 		info->nand.chip_delay = 50;
 	}
 
-	if (use_prefetch) {
-
+	switch (pdata->xfer_type) {
+	case NAND_OMAP_PREFETCH_POLLED:
 		info->nand.read_buf   = omap_read_buf_pref;
 		info->nand.write_buf  = omap_write_buf_pref;
-		if (use_dma) {
-			err = omap_request_dma(OMAP24XX_DMA_GPMC, "NAND",
-				omap_nand_dma_cb, &info->comp, &info->dma_ch);
-			if (err < 0) {
-				info->dma_ch = -1;
-				printk(KERN_WARNING "DMA request failed."
-					" Non-dma data transfer mode\n");
-			} else {
-				omap_set_dma_dest_burst_mode(info->dma_ch,
-						OMAP_DMA_DATA_BURST_16);
-				omap_set_dma_src_burst_mode(info->dma_ch,
-						OMAP_DMA_DATA_BURST_16);
-
-				info->nand.read_buf   = omap_read_buf_dma_pref;
-				info->nand.write_buf  = omap_write_buf_dma_pref;
-			}
-		}
-	} else {
+		break;
+
+	case NAND_OMAP_POLLED:
 		if (info->nand.options & NAND_BUSWIDTH_16) {
 			info->nand.read_buf   = omap_read_buf16;
 			info->nand.write_buf  = omap_write_buf16;
@@ -871,20 +1006,61 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 			info->nand.read_buf   = omap_read_buf8;
 			info->nand.write_buf  = omap_write_buf8;
 		}
+		break;
+
+	case NAND_OMAP_PREFETCH_DMA:
+		err = omap_request_dma(OMAP24XX_DMA_GPMC, "NAND",
+				omap_nand_dma_cb, &info->comp, &info->dma_ch);
+		if (err < 0) {
+			info->dma_ch = -1;
+			dev_err(&pdev->dev, "DMA request failed!\n");
+			goto out_release_mem_region;
+		} else {
+			omap_set_dma_dest_burst_mode(info->dma_ch,
+					OMAP_DMA_DATA_BURST_16);
+			omap_set_dma_src_burst_mode(info->dma_ch,
+					OMAP_DMA_DATA_BURST_16);
+
+			info->nand.read_buf   = omap_read_buf_dma_pref;
+			info->nand.write_buf  = omap_write_buf_dma_pref;
+		}
+		break;
+
+	case NAND_OMAP_PREFETCH_IRQ:
+		err = request_irq(pdata->gpmc_irq,
+				omap_nand_irq, IRQF_SHARED, "gpmc-nand", info);
+		if (err) {
+			dev_err(&pdev->dev, "requesting irq(%d) error:%d",
+							pdata->gpmc_irq, err);
+			goto out_release_mem_region;
+		} else {
+			info->gpmc_irq	     = pdata->gpmc_irq;
+			info->nand.read_buf  = omap_read_buf_irq_pref;
+			info->nand.write_buf = omap_write_buf_irq_pref;
+		}
+		break;
+
+	default:
+		dev_err(&pdev->dev,
+			"xfer_type(%d) not supported!\n", pdata->xfer_type);
+		err = -EINVAL;
+		goto out_release_mem_region;
 	}
-	info->nand.verify_buf = omap_verify_buf;
 
-#ifdef CONFIG_MTD_NAND_OMAP_HWECC
-	info->nand.ecc.bytes		= 3;
-	info->nand.ecc.size		= 512;
-	info->nand.ecc.calculate	= omap_calculate_ecc;
-	info->nand.ecc.hwctl		= omap_enable_hwecc;
-	info->nand.ecc.correct		= omap_correct_data;
-	info->nand.ecc.mode		= NAND_ECC_HW;
+	info->nand.verify_buf = omap_verify_buf;
 
-#else
-	info->nand.ecc.mode = NAND_ECC_SOFT;
-#endif
+	/* selsect the ecc type */
+	if (pdata->ecc_opt == OMAP_ECC_HAMMING_CODE_DEFAULT)
+		info->nand.ecc.mode = NAND_ECC_SOFT;
+	else if ((pdata->ecc_opt == OMAP_ECC_HAMMING_CODE_HW) ||
+		(pdata->ecc_opt == OMAP_ECC_HAMMING_CODE_HW_ROMCODE)) {
+		info->nand.ecc.bytes            = 3;
+		info->nand.ecc.size             = 512;
+		info->nand.ecc.calculate        = omap_calculate_ecc;
+		info->nand.ecc.hwctl            = omap_enable_hwecc;
+		info->nand.ecc.correct          = omap_correct_data;
+		info->nand.ecc.mode             = NAND_ECC_HW;
+	}
 
 	/* DIP switches on some boards change between 8 and 16 bit
 	 * bus widths for flash.  Try the other width if the first try fails.
@@ -897,6 +1073,26 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 		}
 	}
 
+	/* rom code layout */
+	if (pdata->ecc_opt == OMAP_ECC_HAMMING_CODE_HW_ROMCODE) {
+
+		if (info->nand.options & NAND_BUSWIDTH_16)
+			offset = 2;
+		else {
+			offset = 1;
+			info->nand.badblock_pattern = &bb_descrip_flashbased;
+		}
+		omap_oobinfo.eccbytes = 3 * (info->mtd.oobsize/16);
+		for (i = 0; i < omap_oobinfo.eccbytes; i++)
+			omap_oobinfo.eccpos[i] = i+offset;
+
+		omap_oobinfo.oobfree->offset = offset + omap_oobinfo.eccbytes;
+		omap_oobinfo.oobfree->length = info->mtd.oobsize -
+					(offset + omap_oobinfo.eccbytes);
+
+		info->nand.ecc.layout = &omap_oobinfo;
+	}
+
 #ifdef CONFIG_MTD_PARTITIONS
 	err = parse_mtd_partitions(&info->mtd, part_probes, &info->parts, 0);
 	if (err > 0)
@@ -926,9 +1122,12 @@ static int omap_nand_remove(struct platform_device *pdev)
 							mtd);
 
 	platform_set_drvdata(pdev, NULL);
-	if (use_dma)
+	if (info->dma_ch != -1)
 		omap_free_dma(info->dma_ch);
 
+	if (info->gpmc_irq)
+		free_irq(info->gpmc_irq, info);
+
 	/* Release NAND device, its internal structures and partitions */
 	nand_release(&info->mtd);
 	iounmap(info->nand.IO_ADDR_R);
@@ -947,16 +1146,8 @@ static struct platform_driver omap_nand_driver = {
 
 static int __init omap_nand_init(void)
 {
-	printk(KERN_INFO "%s driver initializing\n", DRIVER_NAME);
+	pr_info("%s driver initializing\n", DRIVER_NAME);
 
-	/* This check is required if driver is being
-	 * loaded run time as a module
-	 */
-	if ((1 == use_dma) && (0 == use_prefetch)) {
-		printk(KERN_INFO"Wrong parameters: 'use_dma' can not be 1 "
-				"without use_prefetch'. Prefetch will not be"
-				" used in either mode (mpu or dma)\n");
-	}
 	return platform_driver_register(&omap_nand_driver);
 }
 
diff --git a/drivers/mtd/nand/pasemi_nand.c b/drivers/mtd/nand/pasemi_nand.c
index bb277a54986f..59efa829ef24 100644
--- a/drivers/mtd/nand/pasemi_nand.c
+++ b/drivers/mtd/nand/pasemi_nand.c
@@ -89,8 +89,7 @@ int pasemi_device_ready(struct mtd_info *mtd)
 	return !!(inl(lpcctl) & LBICTRL_LPCCTL_NR);
 }
 
-static int __devinit pasemi_nand_probe(struct platform_device *ofdev,
-				      const struct of_device_id *match)
+static int __devinit pasemi_nand_probe(struct platform_device *ofdev)
 {
 	struct pci_dev *pdev;
 	struct device_node *np = ofdev->dev.of_node;
@@ -219,7 +218,7 @@ static const struct of_device_id pasemi_nand_match[] =
 
 MODULE_DEVICE_TABLE(of, pasemi_nand_match);
 
-static struct of_platform_driver pasemi_nand_driver =
+static struct platform_driver pasemi_nand_driver =
 {
 	.driver = {
 		.name = (char*)driver_name,
@@ -232,13 +231,13 @@ static struct of_platform_driver pasemi_nand_driver =
 
 static int __init pasemi_nand_init(void)
 {
-	return of_register_platform_driver(&pasemi_nand_driver);
+	return platform_driver_register(&pasemi_nand_driver);
 }
 module_init(pasemi_nand_init);
 
 static void __exit pasemi_nand_exit(void)
 {
-	of_unregister_platform_driver(&pasemi_nand_driver);
+	platform_driver_unregister(&pasemi_nand_driver);
 }
 module_exit(pasemi_nand_exit);
 
diff --git a/drivers/mtd/nand/socrates_nand.c b/drivers/mtd/nand/socrates_nand.c
index a8e403eebedb..a853548986f0 100644
--- a/drivers/mtd/nand/socrates_nand.c
+++ b/drivers/mtd/nand/socrates_nand.c
@@ -162,8 +162,7 @@ static const char *part_probes[] = { "cmdlinepart", NULL };
 /*
  * Probe for the NAND device.
  */
-static int __devinit socrates_nand_probe(struct platform_device *ofdev,
-					 const struct of_device_id *ofid)
+static int __devinit socrates_nand_probe(struct platform_device *ofdev)
 {
 	struct socrates_nand_host *host;
 	struct mtd_info *mtd;
@@ -300,7 +299,7 @@ static const struct of_device_id socrates_nand_match[] =
 
 MODULE_DEVICE_TABLE(of, socrates_nand_match);
 
-static struct of_platform_driver socrates_nand_driver = {
+static struct platform_driver socrates_nand_driver = {
 	.driver = {
 		.name = "socrates_nand",
 		.owner = THIS_MODULE,
@@ -312,12 +311,12 @@ static struct of_platform_driver socrates_nand_driver = {
 
 static int __init socrates_nand_init(void)
 {
-	return of_register_platform_driver(&socrates_nand_driver);
+	return platform_driver_register(&socrates_nand_driver);
 }
 
 static void __exit socrates_nand_exit(void)
 {
-	of_unregister_platform_driver(&socrates_nand_driver);
+	platform_driver_unregister(&socrates_nand_driver);
 }
 
 module_init(socrates_nand_init);
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index 3041d1f7ae3f..38fb16771f85 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -319,7 +319,7 @@ static int tmio_nand_correct_data(struct mtd_info *mtd, unsigned char *buf,
 
 static int tmio_hw_init(struct platform_device *dev, struct tmio_nand *tmio)
 {
-	struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	int ret;
 
 	if (cell->enable) {
@@ -363,7 +363,7 @@ static int tmio_hw_init(struct platform_device *dev, struct tmio_nand *tmio)
 
 static void tmio_hw_stop(struct platform_device *dev, struct tmio_nand *tmio)
 {
-	struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 
 	tmio_iowrite8(FCR_MODE_POWER_OFF, tmio->fcr + FCR_MODE);
 	if (cell->disable)
@@ -372,8 +372,7 @@ static void tmio_hw_stop(struct platform_device *dev, struct tmio_nand *tmio)
 
 static int tmio_probe(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev_get_platdata(&dev->dev);
-	struct tmio_nand_data *data = cell->driver_data;
+	struct tmio_nand_data *data = mfd_get_data(dev);
 	struct resource *fcr = platform_get_resource(dev,
 			IORESOURCE_MEM, 0);
 	struct resource *ccr = platform_get_resource(dev,
@@ -516,7 +515,7 @@ static int tmio_remove(struct platform_device *dev)
 #ifdef CONFIG_PM
 static int tmio_suspend(struct platform_device *dev, pm_message_t state)
 {
-	struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 
 	if (cell->suspend)
 		cell->suspend(dev);
@@ -527,7 +526,7 @@ static int tmio_suspend(struct platform_device *dev, pm_message_t state)
 
 static int tmio_resume(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 
 	/* FIXME - is this required or merely another attack of the broken
 	 * SHARP platform? Looks suspicious.
diff --git a/drivers/mtd/onenand/Kconfig b/drivers/mtd/onenand/Kconfig
index 4dbd0f58eebf..4f426195f8db 100644
--- a/drivers/mtd/onenand/Kconfig
+++ b/drivers/mtd/onenand/Kconfig
@@ -32,7 +32,7 @@ config MTD_ONENAND_OMAP2
 
 config MTD_ONENAND_SAMSUNG
         tristate "OneNAND on Samsung SOC controller support"
-        depends on ARCH_S3C64XX || ARCH_S5PC100 || ARCH_S5PV210 || ARCH_S5PV310
+        depends on ARCH_S3C64XX || ARCH_S5PC100 || ARCH_S5PV210 || ARCH_EXYNOS4
         help
           Support for a OneNAND flash device connected to an Samsung SOC.
           S3C64XX/S5PC100 use command mapping method.
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index ac31f461cc1c..ec26399e3cf2 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -63,7 +63,7 @@ struct omap2_onenand {
 	struct completion dma_done;
 	int dma_channel;
 	int freq;
-	int (*setup)(void __iomem *base, int freq);
+	int (*setup)(void __iomem *base, int *freq_ptr);
 	struct regulator *regulator;
 };
 
@@ -148,11 +148,9 @@ static int omap2_onenand_wait(struct mtd_info *mtd, int state)
 			wait_err("controller error", state, ctrl, intr);
 			return -EIO;
 		}
-		if ((intr & intr_flags) != intr_flags) {
-			wait_err("timeout", state, ctrl, intr);
-			return -EIO;
-		}
-		return 0;
+		if ((intr & intr_flags) == intr_flags)
+			return 0;
+		/* Continue in wait for interrupt branch */
 	}
 
 	if (state != FL_READING) {
@@ -581,7 +579,7 @@ static int __adjust_timing(struct device *dev, void *data)
 
 	/* DMA is not in use so this is all that is needed */
 	/* Revisit for OMAP3! */
-	ret = c->setup(c->onenand.base, c->freq);
+	ret = c->setup(c->onenand.base, &c->freq);
 
 	return ret;
 }
@@ -673,7 +671,7 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 	}
 
 	if (pdata->onenand_setup != NULL) {
-		r = pdata->onenand_setup(c->onenand.base, c->freq);
+		r = pdata->onenand_setup(c->onenand.base, &c->freq);
 		if (r < 0) {
 			dev_err(&pdev->dev, "Onenand platform setup failed: "
 				"%d\n", r);
@@ -718,8 +716,8 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 	}
 
 	dev_info(&pdev->dev, "initializing on CS%d, phys base 0x%08lx, virtual "
-		 "base %p\n", c->gpmc_cs, c->phys_base,
-		 c->onenand.base);
+		 "base %p, freq %d MHz\n", c->gpmc_cs, c->phys_base,
+		 c->onenand.base, c->freq);
 
 	c->pdev = pdev;
 	c->mtd.name = dev_name(&pdev->dev);
@@ -754,24 +752,6 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 	if ((r = onenand_scan(&c->mtd, 1)) < 0)
 		goto err_release_regulator;
 
-	switch ((c->onenand.version_id >> 4) & 0xf) {
-	case 0:
-		c->freq = 40;
-		break;
-	case 1:
-		c->freq = 54;
-		break;
-	case 2:
-		c->freq = 66;
-		break;
-	case 3:
-		c->freq = 83;
-		break;
-	case 4:
-		c->freq = 104;
-		break;
-	}
-
 #ifdef CONFIG_MTD_PARTITIONS
 	r = parse_mtd_partitions(&c->mtd, part_probes, &c->parts, 0);
 	if (r > 0)
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 5ebe280225d6..ebaf7ab888a1 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -690,11 +690,25 @@ static int io_init(struct ubi_device *ubi)
 	ubi_assert(ubi->hdrs_min_io_size <= ubi->min_io_size);
 	ubi_assert(ubi->min_io_size % ubi->hdrs_min_io_size == 0);
 
+	ubi->max_write_size = ubi->mtd->writebufsize;
+	/*
+	 * Maximum write size has to be greater or equivalent to min. I/O
+	 * size, and be multiple of min. I/O size.
+	 */
+	if (ubi->max_write_size < ubi->min_io_size ||
+	    ubi->max_write_size % ubi->min_io_size ||
+	    !is_power_of_2(ubi->max_write_size)) {
+		ubi_err("bad write buffer size %d for %d min. I/O unit",
+			ubi->max_write_size, ubi->min_io_size);
+		return -EINVAL;
+	}
+
 	/* Calculate default aligned sizes of EC and VID headers */
 	ubi->ec_hdr_alsize = ALIGN(UBI_EC_HDR_SIZE, ubi->hdrs_min_io_size);
 	ubi->vid_hdr_alsize = ALIGN(UBI_VID_HDR_SIZE, ubi->hdrs_min_io_size);
 
 	dbg_msg("min_io_size      %d", ubi->min_io_size);
+	dbg_msg("max_write_size   %d", ubi->max_write_size);
 	dbg_msg("hdrs_min_io_size %d", ubi->hdrs_min_io_size);
 	dbg_msg("ec_hdr_alsize    %d", ubi->ec_hdr_alsize);
 	dbg_msg("vid_hdr_alsize   %d", ubi->vid_hdr_alsize);
@@ -711,7 +725,7 @@ static int io_init(struct ubi_device *ubi)
 	}
 
 	/* Similar for the data offset */
-	ubi->leb_start = ubi->vid_hdr_offset + UBI_EC_HDR_SIZE;
+	ubi->leb_start = ubi->vid_hdr_offset + UBI_VID_HDR_SIZE;
 	ubi->leb_start = ALIGN(ubi->leb_start, ubi->min_io_size);
 
 	dbg_msg("vid_hdr_offset   %d", ubi->vid_hdr_offset);
@@ -923,6 +937,8 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num, int vid_hdr_offset)
 	spin_lock_init(&ubi->volumes_lock);
 
 	ubi_msg("attaching mtd%d to ubi%d", mtd->index, ubi_num);
+	dbg_msg("sizeof(struct ubi_scan_leb) %zu", sizeof(struct ubi_scan_leb));
+	dbg_msg("sizeof(struct ubi_wl_entry) %zu", sizeof(struct ubi_wl_entry));
 
 	err = io_init(ubi);
 	if (err)
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index 811775aa8ee8..889e25c49323 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -146,6 +146,28 @@ int ubi_io_read(const struct ubi_device *ubi, void *buf, int pnum, int offset,
 	if (err)
 		return err;
 
+	/*
+	 * Deliberately corrupt the buffer to improve robustness. Indeed, if we
+	 * do not do this, the following may happen:
+	 * 1. The buffer contains data from previous operation, e.g., read from
+	 *    another PEB previously. The data looks like expected, e.g., if we
+	 *    just do not read anything and return - the caller would not
+	 *    notice this. E.g., if we are reading a VID header, the buffer may
+	 *    contain a valid VID header from another PEB.
+	 * 2. The driver is buggy and returns us success or -EBADMSG or
+	 *    -EUCLEAN, but it does not actually put any data to the buffer.
+	 *
+	 * This may confuse UBI or upper layers - they may think the buffer
+	 * contains valid data while in fact it is just old data. This is
+	 * especially possible because UBI (and UBIFS) relies on CRC, and
+	 * treats data as correct even in case of ECC errors if the CRC is
+	 * correct.
+	 *
+	 * Try to prevent this situation by changing the first byte of the
+	 * buffer.
+	 */
+	*((uint8_t *)buf) ^= 0xFF;
+
 	addr = (loff_t)pnum * ubi->peb_size + offset;
 retry:
 	err = ubi->mtd->read(ubi->mtd, addr, len, &read, buf);
@@ -166,7 +188,7 @@ retry:
 			return UBI_IO_BITFLIPS;
 		}
 
-		if (read != len && retries++ < UBI_IO_RETRIES) {
+		if (retries++ < UBI_IO_RETRIES) {
 			dbg_io("error %d%s while reading %d bytes from PEB %d:%d,"
 			       " read only %zd bytes, retry",
 			       err, errstr, len, pnum, offset, read);
@@ -480,6 +502,13 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	size_t written;
 	loff_t addr;
 	uint32_t data = 0;
+	/*
+	 * Note, we cannot generally define VID header buffers on stack,
+	 * because of the way we deal with these buffers (see the header
+	 * comment in this file). But we know this is a NOR-specific piece of
+	 * code, so we can do this. But yes, this is error-prone and we should
+	 * (pre-)allocate VID header buffer instead.
+	 */
 	struct ubi_vid_hdr vid_hdr;
 
 	/*
@@ -507,11 +536,13 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	 * PEB.
 	 */
 	err1 = ubi_io_read_vid_hdr(ubi, pnum, &vid_hdr, 0);
-	if (err1 == UBI_IO_BAD_HDR_EBADMSG || err1 == UBI_IO_BAD_HDR) {
+	if (err1 == UBI_IO_BAD_HDR_EBADMSG || err1 == UBI_IO_BAD_HDR ||
+	    err1 == UBI_IO_FF) {
 		struct ubi_ec_hdr ec_hdr;
 
 		err1 = ubi_io_read_ec_hdr(ubi, pnum, &ec_hdr, 0);
-		if (err1 == UBI_IO_BAD_HDR_EBADMSG || err1 == UBI_IO_BAD_HDR)
+		if (err1 == UBI_IO_BAD_HDR_EBADMSG || err1 == UBI_IO_BAD_HDR ||
+		    err1 == UBI_IO_FF)
 			/*
 			 * Both VID and EC headers are corrupted, so we can
 			 * safely erase this PEB and not afraid that it will be
@@ -1294,10 +1325,12 @@ int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
 			int offset, int len)
 {
 	int err, i;
+	size_t read;
+	loff_t addr = (loff_t)pnum * ubi->peb_size + offset;
 
 	mutex_lock(&ubi->dbg_buf_mutex);
-	err = ubi_io_read(ubi, ubi->dbg_peb_buf, pnum, offset, len);
-	if (err)
+	err = ubi->mtd->read(ubi->mtd, addr, len, &read, ubi->dbg_peb_buf);
+	if (err && err != -EUCLEAN)
 		goto out_unlock;
 
 	for (i = 0; i < len; i++) {
diff --git a/drivers/mtd/ubi/kapi.c b/drivers/mtd/ubi/kapi.c
index 69fa4ef03c53..d39716e5b204 100644
--- a/drivers/mtd/ubi/kapi.c
+++ b/drivers/mtd/ubi/kapi.c
@@ -40,7 +40,9 @@ void ubi_do_get_device_info(struct ubi_device *ubi, struct ubi_device_info *di)
 {
 	di->ubi_num = ubi->ubi_num;
 	di->leb_size = ubi->leb_size;
+	di->leb_start = ubi->leb_start;
 	di->min_io_size = ubi->min_io_size;
+	di->max_write_size = ubi->max_write_size;
 	di->ro_mode = ubi->ro_mode;
 	di->cdev = ubi->cdev.dev;
 }
diff --git a/drivers/mtd/ubi/scan.c b/drivers/mtd/ubi/scan.c
index 79ca304fc4db..b65cc088fde5 100644
--- a/drivers/mtd/ubi/scan.c
+++ b/drivers/mtd/ubi/scan.c
@@ -39,32 +39,46 @@
  * eraseblocks are put to the @free list and the physical eraseblock to be
  * erased are put to the @erase list.
  *
+ * About corruptions
+ * ~~~~~~~~~~~~~~~~~
+ *
+ * UBI protects EC and VID headers with CRC-32 checksums, so it can detect
+ * whether the headers are corrupted or not. Sometimes UBI also protects the
+ * data with CRC-32, e.g., when it executes the atomic LEB change operation, or
+ * when it moves the contents of a PEB for wear-leveling purposes.
+ *
  * UBI tries to distinguish between 2 types of corruptions.
- * 1. Corruptions caused by power cuts. These are harmless and expected
- *    corruptions and UBI tries to handle them gracefully, without printing too
- *    many warnings and error messages. The idea is that we do not lose
- *    important data in these case - we may lose only the data which was being
- *    written to the media just before the power cut happened, and the upper
- *    layers (e.g., UBIFS) are supposed to handle these situations. UBI puts
- *    these PEBs to the head of the @erase list and they are scheduled for
- *    erasure.
+ *
+ * 1. Corruptions caused by power cuts. These are expected corruptions and UBI
+ * tries to handle them gracefully, without printing too many warnings and
+ * error messages. The idea is that we do not lose important data in these case
+ * - we may lose only the data which was being written to the media just before
+ * the power cut happened, and the upper layers (e.g., UBIFS) are supposed to
+ * handle such data losses (e.g., by using the FS journal).
+ *
+ * When UBI detects a corruption (CRC-32 mismatch) in a PEB, and it looks like
+ * the reason is a power cut, UBI puts this PEB to the @erase list, and all
+ * PEBs in the @erase list are scheduled for erasure later.
  *
  * 2. Unexpected corruptions which are not caused by power cuts. During
- *    scanning, such PEBs are put to the @corr list and UBI preserves them.
- *    Obviously, this lessens the amount of available PEBs, and if at some
- *    point UBI runs out of free PEBs, it switches to R/O mode. UBI also loudly
- *    informs about such PEBs every time the MTD device is attached.
+ * scanning, such PEBs are put to the @corr list and UBI preserves them.
+ * Obviously, this lessens the amount of available PEBs, and if at some  point
+ * UBI runs out of free PEBs, it switches to R/O mode. UBI also loudly informs
+ * about such PEBs every time the MTD device is attached.
  *
  * However, it is difficult to reliably distinguish between these types of
- * corruptions and UBI's strategy is as follows. UBI assumes (2.) if the VID
- * header is corrupted and the data area does not contain all 0xFFs, and there
- * were not bit-flips or integrity errors while reading the data area. Otherwise
- * UBI assumes (1.). The assumptions are:
- *   o if the data area contains only 0xFFs, there is no data, and it is safe
- *     to just erase this PEB.
- *   o if the data area has bit-flips and data integrity errors (ECC errors on
+ * corruptions and UBI's strategy is as follows. UBI assumes corruption type 2
+ * if the VID header is corrupted and the data area does not contain all 0xFFs,
+ * and there were no bit-flips or integrity errors while reading the data area.
+ * Otherwise UBI assumes corruption type 1. So the decision criteria are as
+ * follows.
+ *   o If the data area contains only 0xFFs, there is no data, and it is safe
+ *     to just erase this PEB - this is corruption type 1.
+ *   o If the data area has bit-flips or data integrity errors (ECC errors on
  *     NAND), it is probably a PEB which was being erased when power cut
- *     happened.
+ *     happened, so this is corruption type 1. However, this is just a guess,
+ *     which might be wrong.
+ *   o Otherwise this it corruption type 2.
  */
 
 #include <linux/err.h>
@@ -115,7 +129,7 @@ static int add_to_list(struct ubi_scan_info *si, int pnum, int ec, int to_head,
 	} else
 		BUG();
 
-	seb = kmalloc(sizeof(struct ubi_scan_leb), GFP_KERNEL);
+	seb = kmem_cache_alloc(si->scan_leb_slab, GFP_KERNEL);
 	if (!seb)
 		return -ENOMEM;
 
@@ -144,7 +158,7 @@ static int add_corrupted(struct ubi_scan_info *si, int pnum, int ec)
 
 	dbg_bld("add to corrupted: PEB %d, EC %d", pnum, ec);
 
-	seb = kmalloc(sizeof(struct ubi_scan_leb), GFP_KERNEL);
+	seb = kmem_cache_alloc(si->scan_leb_slab, GFP_KERNEL);
 	if (!seb)
 		return -ENOMEM;
 
@@ -553,7 +567,7 @@ int ubi_scan_add_used(struct ubi_device *ubi, struct ubi_scan_info *si,
 	if (err)
 		return err;
 
-	seb = kmalloc(sizeof(struct ubi_scan_leb), GFP_KERNEL);
+	seb = kmem_cache_alloc(si->scan_leb_slab, GFP_KERNEL);
 	if (!seb)
 		return -ENOMEM;
 
@@ -1152,9 +1166,15 @@ struct ubi_scan_info *ubi_scan(struct ubi_device *ubi)
 	si->volumes = RB_ROOT;
 
 	err = -ENOMEM;
+	si->scan_leb_slab = kmem_cache_create("ubi_scan_leb_slab",
+					      sizeof(struct ubi_scan_leb),
+					      0, 0, NULL);
+	if (!si->scan_leb_slab)
+		goto out_si;
+
 	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
 	if (!ech)
-		goto out_si;
+		goto out_slab;
 
 	vidh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
 	if (!vidh)
@@ -1215,6 +1235,8 @@ out_vidh:
 	ubi_free_vid_hdr(ubi, vidh);
 out_ech:
 	kfree(ech);
+out_slab:
+	kmem_cache_destroy(si->scan_leb_slab);
 out_si:
 	ubi_scan_destroy_si(si);
 	return ERR_PTR(err);
@@ -1223,11 +1245,12 @@ out_si:
 /**
  * destroy_sv - free the scanning volume information
  * @sv: scanning volume information
+ * @si: scanning information
  *
  * This function destroys the volume RB-tree (@sv->root) and the scanning
  * volume information.
  */
-static void destroy_sv(struct ubi_scan_volume *sv)
+static void destroy_sv(struct ubi_scan_info *si, struct ubi_scan_volume *sv)
 {
 	struct ubi_scan_leb *seb;
 	struct rb_node *this = sv->root.rb_node;
@@ -1247,7 +1270,7 @@ static void destroy_sv(struct ubi_scan_volume *sv)
 					this->rb_right = NULL;
 			}
 
-			kfree(seb);
+			kmem_cache_free(si->scan_leb_slab, seb);
 		}
 	}
 	kfree(sv);
@@ -1265,19 +1288,19 @@ void ubi_scan_destroy_si(struct ubi_scan_info *si)
 
 	list_for_each_entry_safe(seb, seb_tmp, &si->alien, u.list) {
 		list_del(&seb->u.list);
-		kfree(seb);
+		kmem_cache_free(si->scan_leb_slab, seb);
 	}
 	list_for_each_entry_safe(seb, seb_tmp, &si->erase, u.list) {
 		list_del(&seb->u.list);
-		kfree(seb);
+		kmem_cache_free(si->scan_leb_slab, seb);
 	}
 	list_for_each_entry_safe(seb, seb_tmp, &si->corr, u.list) {
 		list_del(&seb->u.list);
-		kfree(seb);
+		kmem_cache_free(si->scan_leb_slab, seb);
 	}
 	list_for_each_entry_safe(seb, seb_tmp, &si->free, u.list) {
 		list_del(&seb->u.list);
-		kfree(seb);
+		kmem_cache_free(si->scan_leb_slab, seb);
 	}
 
 	/* Destroy the volume RB-tree */
@@ -1298,10 +1321,11 @@ void ubi_scan_destroy_si(struct ubi_scan_info *si)
 					rb->rb_right = NULL;
 			}
 
-			destroy_sv(sv);
+			destroy_sv(si, sv);
 		}
 	}
 
+	kmem_cache_destroy(si->scan_leb_slab);
 	kfree(si);
 }
 
diff --git a/drivers/mtd/ubi/scan.h b/drivers/mtd/ubi/scan.h
index a3264f0bef2b..d48aef15ab5d 100644
--- a/drivers/mtd/ubi/scan.h
+++ b/drivers/mtd/ubi/scan.h
@@ -109,6 +109,7 @@ struct ubi_scan_volume {
  * @mean_ec: mean erase counter value
  * @ec_sum: a temporary variable used when calculating @mean_ec
  * @ec_count: a temporary variable used when calculating @mean_ec
+ * @scan_leb_slab: slab cache for &struct ubi_scan_leb objects
  *
  * This data structure contains the result of scanning and may be used by other
  * UBI sub-systems to build final UBI data structures, further error-recovery
@@ -134,6 +135,7 @@ struct ubi_scan_info {
 	int mean_ec;
 	uint64_t ec_sum;
 	int ec_count;
+	struct kmem_cache *scan_leb_slab;
 };
 
 struct ubi_device;
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index 0b0149c41fe3..b78994330ebc 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -381,6 +381,8 @@ struct ubi_wl_entry;
  * @bad_allowed: whether the MTD device admits of bad physical eraseblocks or
  *               not
  * @nor_flash: non-zero if working on top of NOR flash
+ * @max_write_size: maximum amount of bytes the underlying flash can write at a
+ *                  time (MTD write buffer size)
  * @mtd: MTD device descriptor
  *
  * @peb_buf1: a buffer of PEB size used for different purposes
@@ -464,6 +466,7 @@ struct ubi_device {
 	int vid_hdr_shift;
 	unsigned int bad_allowed:1;
 	unsigned int nor_flash:1;
+	int max_write_size;
 	struct mtd_info *mtd;
 
 	void *peb_buf1;
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 03823327db25..6bd5caad9a02 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -238,8 +238,8 @@ source "drivers/net/arm/Kconfig"
 config AX88796
 	tristate "ASIX AX88796 NE2000 clone support"
 	depends on ARM || MIPS || SUPERH
-	select CRC32
-	select MII
+	select PHYLIB
+	select MDIO_BITBANG
 	help
 	  AX88796 driver, using platform bus to provide
 	  chip detection and resources
@@ -1944,7 +1944,8 @@ config 68360_ENET
 config FEC
 	bool "FEC ethernet controller (of ColdFire and some i.MX CPUs)"
 	depends on M523x || M527x || M5272 || M528x || M520x || M532x || \
-		MACH_MX27 || ARCH_MX35 || ARCH_MX25 || ARCH_MX5 || SOC_IMX28
+		IMX_HAVE_PLATFORM_FEC || MXS_HAVE_PLATFORM_FEC
+	default IMX_HAVE_PLATFORM_FEC || MXS_HAVE_PLATFORM_FEC if ARM
 	select PHYLIB
 	help
 	  Say Y here if you want to use the built-in 10/100 Fast ethernet
@@ -2007,6 +2008,15 @@ config BCM63XX_ENET
 	  This driver supports the ethernet MACs in the Broadcom 63xx
 	  MIPS chipset family (BCM63XX).
 
+config FTMAC100
+	tristate "Faraday FTMAC100 10/100 Ethernet support"
+	depends on ARM
+	select MII
+	help
+	  This driver supports the FTMAC100 10/100 Ethernet controller
+	  from Faraday. It is used on Faraday A320, Andes AG101 and some
+	  other ARM/NDS32 SoC's.
+
 source "drivers/net/fs_enet/Kconfig"
 
 source "drivers/net/octeon/Kconfig"
@@ -2235,15 +2245,6 @@ config R8169
 	  To compile this driver as a module, choose M here: the module
 	  will be called r8169.  This is recommended.
 
-config R8169_VLAN
-	bool "VLAN support"
-	depends on R8169 && VLAN_8021Q
-	---help---
-	  Say Y here for the r8169 driver to support the functions required
-	  by the kernel 802.1Q code.
-
-	  If in doubt, say Y.
-
 config SB1250_MAC
 	tristate "SB1250 Gigabit Ethernet support"
 	depends on SIBYTE_SB1xxx_SOC
@@ -2594,14 +2595,9 @@ config CHELSIO_T1_1G
 	  Enables support for Chelsio's gigabit Ethernet PCI cards.  If you
 	  are using only 10G cards say 'N' here.
 
-config CHELSIO_T3_DEPENDS
-	tristate
-	depends on PCI && INET
-	default y
-
 config CHELSIO_T3
 	tristate "Chelsio Communications T3 10Gb Ethernet support"
-	depends on CHELSIO_T3_DEPENDS
+	depends on PCI && INET
 	select FW_LOADER
 	select MDIO
 	help
@@ -2619,14 +2615,9 @@ config CHELSIO_T3
 	  To compile this driver as a module, choose M here: the module
 	  will be called cxgb3.
 
-config CHELSIO_T4_DEPENDS
-	tristate
-	depends on PCI && INET
-	default y
-
 config CHELSIO_T4
 	tristate "Chelsio Communications T4 Ethernet support"
-	depends on CHELSIO_T4_DEPENDS
+	depends on PCI
 	select FW_LOADER
 	select MDIO
 	help
@@ -2644,14 +2635,9 @@ config CHELSIO_T4
 	  To compile this driver as a module choose M here; the module
 	  will be called cxgb4.
 
-config CHELSIO_T4VF_DEPENDS
-	tristate
-	depends on PCI && INET
-	default y
-
 config CHELSIO_T4VF
 	tristate "Chelsio Communications T4 Virtual Function Ethernet support"
-	depends on CHELSIO_T4VF_DEPENDS
+	depends on PCI
 	help
 	  This driver supports Chelsio T4-based gigabit and 10Gb Ethernet
 	  adapters with PCI-E SR-IOV Virtual Functions.
@@ -3422,4 +3408,18 @@ config VMXNET3
 	  To compile this driver as a module, choose M here: the
 	  module will be called vmxnet3.
 
+config VBUS_ENET
+	tristate "VBUS Ethernet Driver"
+	default n
+	depends on VBUS_PROXY
+	help
+	   A virtualized 802.x network device based on the VBUS
+	   "virtual-ethernet" interface.  It can be used with any
+	   hypervisor/kernel that supports the vbus+venet protocol.
+
+config VBUS_ENET_DEBUG
+        bool "Enable Debugging"
+	depends on VBUS_ENET
+	default n
+
 endif # NETDEVICES
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index b90738d13994..9a6bdf228a5c 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -147,6 +147,7 @@ obj-$(CONFIG_FORCEDETH) += forcedeth.o
 obj-$(CONFIG_NE_H8300) += ne-h8300.o 8390.o
 obj-$(CONFIG_AX88796) += ax88796.o
 obj-$(CONFIG_BCM63XX_ENET) += bcm63xx_enet.o
+obj-$(CONFIG_FTMAC100) += ftmac100.o
 
 obj-$(CONFIG_TSI108_ETH) += tsi108_eth.o
 obj-$(CONFIG_MV643XX_ETH) += mv643xx_eth.o
@@ -294,6 +295,7 @@ obj-$(CONFIG_FS_ENET) += fs_enet/
 obj-$(CONFIG_NETXEN_NIC) += netxen/
 obj-$(CONFIG_NIU) += niu.o
 obj-$(CONFIG_VIRTIO_NET) += virtio_net.o
+obj-$(CONFIG_VBUS_ENET) += vbus-enet.o
 obj-$(CONFIG_SFC) += sfc/
 
 obj-$(CONFIG_WIMAX) += wimax/
diff --git a/drivers/net/atl1c/atl1c.h b/drivers/net/atl1c/atl1c.h
index 9ab58097fa2e..7cb375e0e29c 100644
--- a/drivers/net/atl1c/atl1c.h
+++ b/drivers/net/atl1c/atl1c.h
@@ -265,7 +265,7 @@ struct atl1c_recv_ret_status {
 	__le32	word3;
 };
 
-/* RFD desciptor */
+/* RFD descriptor */
 struct atl1c_rx_free_desc {
 	__le64	buffer_addr;
 };
@@ -531,7 +531,7 @@ struct atl1c_rfd_ring {
 	struct atl1c_buffer *buffer_info;
 };
 
-/* receive return desciptor (rrd) ring */
+/* receive return descriptor (rrd) ring */
 struct atl1c_rrd_ring {
 	void *desc;		/* descriptor ring virtual address */
 	dma_addr_t dma;		/* descriptor ring physical address */
diff --git a/drivers/net/atl1c/atl1c_hw.c b/drivers/net/atl1c/atl1c_hw.c
index 1bf672009948..23f2ab0f2fa8 100644
--- a/drivers/net/atl1c/atl1c_hw.c
+++ b/drivers/net/atl1c/atl1c_hw.c
@@ -345,7 +345,7 @@ int atl1c_write_phy_reg(struct atl1c_hw *hw, u32 reg_addr, u16 phy_data)
  */
 static int atl1c_phy_setup_adv(struct atl1c_hw *hw)
 {
-	u16 mii_adv_data = ADVERTISE_DEFAULT_CAP & ~ADVERTISE_SPEED_MASK;
+	u16 mii_adv_data = ADVERTISE_DEFAULT_CAP & ~ADVERTISE_ALL;
 	u16 mii_giga_ctrl_data = GIGA_CR_1000T_DEFAULT_CAP &
 				~GIGA_CR_1000T_SPEED_MASK;
 
@@ -373,7 +373,7 @@ static int atl1c_phy_setup_adv(struct atl1c_hw *hw)
 	}
 
 	if (atl1c_write_phy_reg(hw, MII_ADVERTISE, mii_adv_data) != 0 ||
-	    atl1c_write_phy_reg(hw, MII_GIGA_CR, mii_giga_ctrl_data) != 0)
+	    atl1c_write_phy_reg(hw, MII_CTRL1000, mii_giga_ctrl_data) != 0)
 		return -1;
 	return 0;
 }
@@ -517,19 +517,18 @@ int atl1c_phy_init(struct atl1c_hw *hw)
 					"Error Setting up Auto-Negotiation\n");
 			return ret_val;
 		}
-		mii_bmcr_data |= BMCR_AUTO_NEG_EN | BMCR_RESTART_AUTO_NEG;
+		mii_bmcr_data |= BMCR_ANENABLE | BMCR_ANRESTART;
 		break;
 	case MEDIA_TYPE_100M_FULL:
-		mii_bmcr_data |= BMCR_SPEED_100 | BMCR_FULL_DUPLEX;
+		mii_bmcr_data |= BMCR_SPEED100 | BMCR_FULLDPLX;
 		break;
 	case MEDIA_TYPE_100M_HALF:
-		mii_bmcr_data |= BMCR_SPEED_100;
+		mii_bmcr_data |= BMCR_SPEED100;
 		break;
 	case MEDIA_TYPE_10M_FULL:
-		mii_bmcr_data |= BMCR_SPEED_10 | BMCR_FULL_DUPLEX;
+		mii_bmcr_data |= BMCR_FULLDPLX;
 		break;
 	case MEDIA_TYPE_10M_HALF:
-		mii_bmcr_data |= BMCR_SPEED_10;
 		break;
 	default:
 		if (netif_msg_link(adapter))
@@ -657,7 +656,7 @@ int atl1c_restart_autoneg(struct atl1c_hw *hw)
 	err = atl1c_phy_setup_adv(hw);
 	if (err)
 		return err;
-	mii_bmcr_data |= BMCR_AUTO_NEG_EN | BMCR_RESTART_AUTO_NEG;
+	mii_bmcr_data |= BMCR_ANENABLE | BMCR_ANRESTART;
 
 	return atl1c_write_phy_reg(hw, MII_BMCR, mii_bmcr_data);
 }
diff --git a/drivers/net/atl1c/atl1c_hw.h b/drivers/net/atl1c/atl1c_hw.h
index 3dd675979aa1..655fc6c4a8a4 100644
--- a/drivers/net/atl1c/atl1c_hw.h
+++ b/drivers/net/atl1c/atl1c_hw.h
@@ -736,55 +736,16 @@ int atl1c_phy_power_saving(struct atl1c_hw *hw);
 #define REG_DEBUG_DATA0 		0x1900
 #define REG_DEBUG_DATA1 		0x1904
 
-/* PHY Control Register */
-#define MII_BMCR			0x00
-#define BMCR_SPEED_SELECT_MSB		0x0040  /* bits 6,13: 10=1000, 01=100, 00=10 */
-#define BMCR_COLL_TEST_ENABLE		0x0080  /* Collision test enable */
-#define BMCR_FULL_DUPLEX		0x0100  /* FDX =1, half duplex =0 */
-#define BMCR_RESTART_AUTO_NEG		0x0200  /* Restart auto negotiation */
-#define BMCR_ISOLATE			0x0400  /* Isolate PHY from MII */
-#define BMCR_POWER_DOWN			0x0800  /* Power down */
-#define BMCR_AUTO_NEG_EN		0x1000  /* Auto Neg Enable */
-#define BMCR_SPEED_SELECT_LSB		0x2000  /* bits 6,13: 10=1000, 01=100, 00=10 */
-#define BMCR_LOOPBACK			0x4000  /* 0 = normal, 1 = loopback */
-#define BMCR_RESET			0x8000  /* 0 = normal, 1 = PHY reset */
-#define BMCR_SPEED_MASK			0x2040
-#define BMCR_SPEED_1000			0x0040
-#define BMCR_SPEED_100			0x2000
-#define BMCR_SPEED_10			0x0000
-
-/* PHY Status Register */
-#define MII_BMSR			0x01
-#define BMMSR_EXTENDED_CAPS		0x0001  /* Extended register capabilities */
-#define BMSR_JABBER_DETECT		0x0002  /* Jabber Detected */
-#define BMSR_LINK_STATUS		0x0004  /* Link Status 1 = link */
-#define BMSR_AUTONEG_CAPS		0x0008  /* Auto Neg Capable */
-#define BMSR_REMOTE_FAULT		0x0010  /* Remote Fault Detect */
-#define BMSR_AUTONEG_COMPLETE		0x0020  /* Auto Neg Complete */
-#define BMSR_PREAMBLE_SUPPRESS		0x0040  /* Preamble may be suppressed */
-#define BMSR_EXTENDED_STATUS		0x0100  /* Ext. status info in Reg 0x0F */
-#define BMSR_100T2_HD_CAPS		0x0200  /* 100T2 Half Duplex Capable */
-#define BMSR_100T2_FD_CAPS		0x0400  /* 100T2 Full Duplex Capable */
-#define BMSR_10T_HD_CAPS		0x0800  /* 10T   Half Duplex Capable */
-#define BMSR_10T_FD_CAPS		0x1000  /* 10T   Full Duplex Capable */
-#define BMSR_100X_HD_CAPS		0x2000  /* 100X  Half Duplex Capable */
-#define BMMII_SR_100X_FD_CAPS		0x4000  /* 100X  Full Duplex Capable */
-#define BMMII_SR_100T4_CAPS		0x8000  /* 100T4 Capable */
-
-#define MII_PHYSID1			0x02
-#define MII_PHYSID2			0x03
 #define L1D_MPW_PHYID1			0xD01C  /* V7 */
 #define L1D_MPW_PHYID2			0xD01D  /* V1-V6 */
 #define L1D_MPW_PHYID3			0xD01E  /* V8 */
 
 
 /* Autoneg Advertisement Register */
-#define MII_ADVERTISE			0x04
-#define ADVERTISE_SPEED_MASK		0x01E0
-#define ADVERTISE_DEFAULT_CAP		0x0DE0
+#define ADVERTISE_DEFAULT_CAP \
+	(ADVERTISE_ALL | ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM)
 
 /* 1000BASE-T Control Register */
-#define MII_GIGA_CR			0x09
 #define GIGA_CR_1000T_REPEATER_DTE	0x0400  /* 1=Repeater/switch device port 0=DTE device */
 
 #define GIGA_CR_1000T_MS_VALUE		0x0800  /* 1=Configure PHY as Master 0=Configure PHY as Slave */
diff --git a/drivers/net/atl1c/atl1c_main.c b/drivers/net/atl1c/atl1c_main.c
index 3824382faecc..7d9d5067a65c 100644
--- a/drivers/net/atl1c/atl1c_main.c
+++ b/drivers/net/atl1c/atl1c_main.c
@@ -1102,10 +1102,10 @@ static void atl1c_configure_tx(struct atl1c_adapter *adapter)
 	AT_READ_REG(hw, REG_DEVICE_CTRL, &dev_ctrl_data);
 	max_pay_load  = (dev_ctrl_data >> DEVICE_CTRL_MAX_PAYLOAD_SHIFT) &
 			DEVICE_CTRL_MAX_PAYLOAD_MASK;
-	hw->dmaw_block = min(max_pay_load, hw->dmaw_block);
+	hw->dmaw_block = min_t(u32, max_pay_load, hw->dmaw_block);
 	max_pay_load  = (dev_ctrl_data >> DEVICE_CTRL_MAX_RREQ_SZ_SHIFT) &
 			DEVICE_CTRL_MAX_RREQ_SZ_MASK;
-	hw->dmar_block = min(max_pay_load, hw->dmar_block);
+	hw->dmar_block = min_t(u32, max_pay_load, hw->dmar_block);
 
 	txq_ctrl_data = (hw->tpd_burst & TXQ_NUM_TPD_BURST_MASK) <<
 			TXQ_NUM_TPD_BURST_SHIFT;
@@ -2718,7 +2718,6 @@ static int __devinit atl1c_probe(struct pci_dev *pdev,
 		goto err_reset;
 	}
 
-	device_init_wakeup(&pdev->dev, 1);
 	/* reset the controller to
 	 * put the device in a known good starting state */
 	err = atl1c_phy_init(&adapter->hw);
diff --git a/drivers/net/atl1e/atl1e_ethtool.c b/drivers/net/atl1e/atl1e_ethtool.c
index 6943a6c3b948..1209297433b8 100644
--- a/drivers/net/atl1e/atl1e_ethtool.c
+++ b/drivers/net/atl1e/atl1e_ethtool.c
@@ -95,18 +95,18 @@ static int atl1e_set_settings(struct net_device *netdev,
 		ecmd->advertising = hw->autoneg_advertised |
 				    ADVERTISED_TP | ADVERTISED_Autoneg;
 
-		adv4 = hw->mii_autoneg_adv_reg & ~MII_AR_SPEED_MASK;
+		adv4 = hw->mii_autoneg_adv_reg & ~ADVERTISE_ALL;
 		adv9 = hw->mii_1000t_ctrl_reg & ~MII_AT001_CR_1000T_SPEED_MASK;
 		if (hw->autoneg_advertised & ADVERTISE_10_HALF)
-			adv4 |= MII_AR_10T_HD_CAPS;
+			adv4 |= ADVERTISE_10HALF;
 		if (hw->autoneg_advertised & ADVERTISE_10_FULL)
-			adv4 |= MII_AR_10T_FD_CAPS;
+			adv4 |= ADVERTISE_10FULL;
 		if (hw->autoneg_advertised & ADVERTISE_100_HALF)
-			adv4 |= MII_AR_100TX_HD_CAPS;
+			adv4 |= ADVERTISE_100HALF;
 		if (hw->autoneg_advertised & ADVERTISE_100_FULL)
-			adv4 |= MII_AR_100TX_FD_CAPS;
+			adv4 |= ADVERTISE_100FULL;
 		if (hw->autoneg_advertised & ADVERTISE_1000_FULL)
-			adv9 |= MII_AT001_CR_1000T_FD_CAPS;
+			adv9 |= ADVERTISE_1000FULL;
 
 		if (adv4 != hw->mii_autoneg_adv_reg ||
 				adv9 != hw->mii_1000t_ctrl_reg) {
diff --git a/drivers/net/atl1e/atl1e_hw.c b/drivers/net/atl1e/atl1e_hw.c
index 76cc043def8c..923063d2e5bb 100644
--- a/drivers/net/atl1e/atl1e_hw.c
+++ b/drivers/net/atl1e/atl1e_hw.c
@@ -318,7 +318,7 @@ static int atl1e_phy_setup_autoneg_adv(struct atl1e_hw *hw)
 	 * Advertisement Register (Address 4) and the 1000 mb speed bits in
 	 * the  1000Base-T control Register (Address 9).
 	 */
-	mii_autoneg_adv_reg &= ~MII_AR_SPEED_MASK;
+	mii_autoneg_adv_reg &= ~ADVERTISE_ALL;
 	mii_1000t_ctrl_reg  &= ~MII_AT001_CR_1000T_SPEED_MASK;
 
 	/*
@@ -327,44 +327,37 @@ static int atl1e_phy_setup_autoneg_adv(struct atl1e_hw *hw)
 	 */
 	switch (hw->media_type) {
 	case MEDIA_TYPE_AUTO_SENSOR:
-		mii_autoneg_adv_reg |= (MII_AR_10T_HD_CAPS   |
-					MII_AR_10T_FD_CAPS   |
-					MII_AR_100TX_HD_CAPS |
-					MII_AR_100TX_FD_CAPS);
-		hw->autoneg_advertised = ADVERTISE_10_HALF  |
-					 ADVERTISE_10_FULL  |
-					 ADVERTISE_100_HALF |
-					 ADVERTISE_100_FULL;
+		mii_autoneg_adv_reg |= ADVERTISE_ALL;
+		hw->autoneg_advertised = ADVERTISE_ALL;
 		if (hw->nic_type == athr_l1e) {
-			mii_1000t_ctrl_reg |=
-				MII_AT001_CR_1000T_FD_CAPS;
+			mii_1000t_ctrl_reg |= ADVERTISE_1000FULL;
 			hw->autoneg_advertised |= ADVERTISE_1000_FULL;
 		}
 		break;
 
 	case MEDIA_TYPE_100M_FULL:
-		mii_autoneg_adv_reg   |= MII_AR_100TX_FD_CAPS;
+		mii_autoneg_adv_reg   |= ADVERTISE_100FULL;
 		hw->autoneg_advertised = ADVERTISE_100_FULL;
 		break;
 
 	case MEDIA_TYPE_100M_HALF:
-		mii_autoneg_adv_reg   |= MII_AR_100TX_HD_CAPS;
+		mii_autoneg_adv_reg   |= ADVERTISE_100_HALF;
 		hw->autoneg_advertised = ADVERTISE_100_HALF;
 		break;
 
 	case MEDIA_TYPE_10M_FULL:
-		mii_autoneg_adv_reg   |= MII_AR_10T_FD_CAPS;
+		mii_autoneg_adv_reg   |= ADVERTISE_10_FULL;
 		hw->autoneg_advertised = ADVERTISE_10_FULL;
 		break;
 
 	default:
-		mii_autoneg_adv_reg   |= MII_AR_10T_HD_CAPS;
+		mii_autoneg_adv_reg   |= ADVERTISE_10_HALF;
 		hw->autoneg_advertised = ADVERTISE_10_HALF;
 		break;
 	}
 
 	/* flow control fixed to enable all */
-	mii_autoneg_adv_reg |= (MII_AR_ASM_DIR | MII_AR_PAUSE);
+	mii_autoneg_adv_reg |= (ADVERTISE_PAUSE_ASYM | ADVERTISE_PAUSE_CAP);
 
 	hw->mii_autoneg_adv_reg = mii_autoneg_adv_reg;
 	hw->mii_1000t_ctrl_reg  = mii_1000t_ctrl_reg;
@@ -374,7 +367,7 @@ static int atl1e_phy_setup_autoneg_adv(struct atl1e_hw *hw)
 		return ret_val;
 
 	if (hw->nic_type == athr_l1e || hw->nic_type == athr_l2e_revA) {
-		ret_val = atl1e_write_phy_reg(hw, MII_AT001_CR,
+		ret_val = atl1e_write_phy_reg(hw, MII_CTRL1000,
 					   mii_1000t_ctrl_reg);
 		if (ret_val)
 			return ret_val;
@@ -397,7 +390,7 @@ int atl1e_phy_commit(struct atl1e_hw *hw)
 	int ret_val;
 	u16 phy_data;
 
-	phy_data = MII_CR_RESET | MII_CR_AUTO_NEG_EN | MII_CR_RESTART_AUTO_NEG;
+	phy_data = BMCR_RESET | BMCR_ANENABLE | BMCR_ANRESTART;
 
 	ret_val = atl1e_write_phy_reg(hw, MII_BMCR, phy_data);
 	if (ret_val) {
@@ -645,15 +638,14 @@ int atl1e_restart_autoneg(struct atl1e_hw *hw)
 		return err;
 
 	if (hw->nic_type == athr_l1e || hw->nic_type == athr_l2e_revA) {
-		err = atl1e_write_phy_reg(hw, MII_AT001_CR,
+		err = atl1e_write_phy_reg(hw, MII_CTRL1000,
 				       hw->mii_1000t_ctrl_reg);
 		if (err)
 			return err;
 	}
 
 	err = atl1e_write_phy_reg(hw, MII_BMCR,
-			MII_CR_RESET | MII_CR_AUTO_NEG_EN |
-			MII_CR_RESTART_AUTO_NEG);
+			BMCR_RESET | BMCR_ANENABLE | BMCR_ANRESTART);
 	return err;
 }
 
diff --git a/drivers/net/atl1e/atl1e_hw.h b/drivers/net/atl1e/atl1e_hw.h
index 5ea2f4d86cfa..74df16aef793 100644
--- a/drivers/net/atl1e/atl1e_hw.h
+++ b/drivers/net/atl1e/atl1e_hw.h
@@ -629,127 +629,24 @@ s32 atl1e_restart_autoneg(struct atl1e_hw *hw);
 
 /***************************** MII definition ***************************************/
 /* PHY Common Register */
-#define MII_BMCR                        0x00
-#define MII_BMSR                        0x01
-#define MII_PHYSID1                     0x02
-#define MII_PHYSID2                     0x03
-#define MII_ADVERTISE                   0x04
-#define MII_LPA                         0x05
-#define MII_EXPANSION                   0x06
-#define MII_AT001_CR                    0x09
-#define MII_AT001_SR                    0x0A
-#define MII_AT001_ESR                   0x0F
 #define MII_AT001_PSCR                  0x10
 #define MII_AT001_PSSR                  0x11
 #define MII_INT_CTRL                    0x12
 #define MII_INT_STATUS                  0x13
 #define MII_SMARTSPEED                  0x14
-#define MII_RERRCOUNTER                 0x15
-#define MII_SREVISION                   0x16
-#define MII_RESV1                       0x17
 #define MII_LBRERROR                    0x18
-#define MII_PHYADDR                     0x19
 #define MII_RESV2                       0x1a
-#define MII_TPISTATUS                   0x1b
-#define MII_NCONFIG                     0x1c
 
 #define MII_DBG_ADDR			0x1D
 #define MII_DBG_DATA			0x1E
 
-
-/* PHY Control Register */
-#define MII_CR_SPEED_SELECT_MSB                  0x0040  /* bits 6,13: 10=1000, 01=100, 00=10 */
-#define MII_CR_COLL_TEST_ENABLE                  0x0080  /* Collision test enable */
-#define MII_CR_FULL_DUPLEX                       0x0100  /* FDX =1, half duplex =0 */
-#define MII_CR_RESTART_AUTO_NEG                  0x0200  /* Restart auto negotiation */
-#define MII_CR_ISOLATE                           0x0400  /* Isolate PHY from MII */
-#define MII_CR_POWER_DOWN                        0x0800  /* Power down */
-#define MII_CR_AUTO_NEG_EN                       0x1000  /* Auto Neg Enable */
-#define MII_CR_SPEED_SELECT_LSB                  0x2000  /* bits 6,13: 10=1000, 01=100, 00=10 */
-#define MII_CR_LOOPBACK                          0x4000  /* 0 = normal, 1 = loopback */
-#define MII_CR_RESET                             0x8000  /* 0 = normal, 1 = PHY reset */
-#define MII_CR_SPEED_MASK                        0x2040
-#define MII_CR_SPEED_1000                        0x0040
-#define MII_CR_SPEED_100                         0x2000
-#define MII_CR_SPEED_10                          0x0000
-
-
-/* PHY Status Register */
-#define MII_SR_EXTENDED_CAPS                     0x0001  /* Extended register capabilities */
-#define MII_SR_JABBER_DETECT                     0x0002  /* Jabber Detected */
-#define MII_SR_LINK_STATUS                       0x0004  /* Link Status 1 = link */
-#define MII_SR_AUTONEG_CAPS                      0x0008  /* Auto Neg Capable */
-#define MII_SR_REMOTE_FAULT                      0x0010  /* Remote Fault Detect */
-#define MII_SR_AUTONEG_COMPLETE                  0x0020  /* Auto Neg Complete */
-#define MII_SR_PREAMBLE_SUPPRESS                 0x0040  /* Preamble may be suppressed */
-#define MII_SR_EXTENDED_STATUS                   0x0100  /* Ext. status info in Reg 0x0F */
-#define MII_SR_100T2_HD_CAPS                     0x0200  /* 100T2 Half Duplex Capable */
-#define MII_SR_100T2_FD_CAPS                     0x0400  /* 100T2 Full Duplex Capable */
-#define MII_SR_10T_HD_CAPS                       0x0800  /* 10T   Half Duplex Capable */
-#define MII_SR_10T_FD_CAPS                       0x1000  /* 10T   Full Duplex Capable */
-#define MII_SR_100X_HD_CAPS                      0x2000  /* 100X  Half Duplex Capable */
-#define MII_SR_100X_FD_CAPS                      0x4000  /* 100X  Full Duplex Capable */
-#define MII_SR_100T4_CAPS                        0x8000  /* 100T4 Capable */
-
-/* Link partner ability register. */
-#define MII_LPA_SLCT                             0x001f  /* Same as advertise selector  */
-#define MII_LPA_10HALF                           0x0020  /* Can do 10mbps half-duplex   */
-#define MII_LPA_10FULL                           0x0040  /* Can do 10mbps full-duplex   */
-#define MII_LPA_100HALF                          0x0080  /* Can do 100mbps half-duplex  */
-#define MII_LPA_100FULL                          0x0100  /* Can do 100mbps full-duplex  */
-#define MII_LPA_100BASE4                         0x0200  /* 100BASE-T4  */
-#define MII_LPA_PAUSE                            0x0400  /* PAUSE */
-#define MII_LPA_ASYPAUSE                         0x0800  /* Asymmetrical PAUSE */
-#define MII_LPA_RFAULT                           0x2000  /* Link partner faulted        */
-#define MII_LPA_LPACK                            0x4000  /* Link partner acked us       */
-#define MII_LPA_NPAGE                            0x8000  /* Next page bit               */
-
 /* Autoneg Advertisement Register */
-#define MII_AR_SELECTOR_FIELD                   0x0001  /* indicates IEEE 802.3 CSMA/CD */
-#define MII_AR_10T_HD_CAPS                      0x0020  /* 10T   Half Duplex Capable */
-#define MII_AR_10T_FD_CAPS                      0x0040  /* 10T   Full Duplex Capable */
-#define MII_AR_100TX_HD_CAPS                    0x0080  /* 100TX Half Duplex Capable */
-#define MII_AR_100TX_FD_CAPS                    0x0100  /* 100TX Full Duplex Capable */
-#define MII_AR_100T4_CAPS                       0x0200  /* 100T4 Capable */
-#define MII_AR_PAUSE                            0x0400  /* Pause operation desired */
-#define MII_AR_ASM_DIR                          0x0800  /* Asymmetric Pause Direction bit */
-#define MII_AR_REMOTE_FAULT                     0x2000  /* Remote Fault detected */
-#define MII_AR_NEXT_PAGE                        0x8000  /* Next Page ability supported */
-#define MII_AR_SPEED_MASK                       0x01E0
-#define MII_AR_DEFAULT_CAP_MASK                 0x0DE0
+#define MII_AR_DEFAULT_CAP_MASK                 0
 
 /* 1000BASE-T Control Register */
-#define MII_AT001_CR_1000T_HD_CAPS              0x0100  /* Advertise 1000T HD capability */
-#define MII_AT001_CR_1000T_FD_CAPS              0x0200  /* Advertise 1000T FD capability  */
-#define MII_AT001_CR_1000T_REPEATER_DTE         0x0400  /* 1=Repeater/switch device port */
-/* 0=DTE device */
-#define MII_AT001_CR_1000T_MS_VALUE             0x0800  /* 1=Configure PHY as Master */
-/* 0=Configure PHY as Slave */
-#define MII_AT001_CR_1000T_MS_ENABLE            0x1000  /* 1=Master/Slave manual config value */
-/* 0=Automatic Master/Slave config */
-#define MII_AT001_CR_1000T_TEST_MODE_NORMAL     0x0000  /* Normal Operation */
-#define MII_AT001_CR_1000T_TEST_MODE_1          0x2000  /* Transmit Waveform test */
-#define MII_AT001_CR_1000T_TEST_MODE_2          0x4000  /* Master Transmit Jitter test */
-#define MII_AT001_CR_1000T_TEST_MODE_3          0x6000  /* Slave Transmit Jitter test */
-#define MII_AT001_CR_1000T_TEST_MODE_4          0x8000  /* Transmitter Distortion test */
-#define MII_AT001_CR_1000T_SPEED_MASK           0x0300
-#define MII_AT001_CR_1000T_DEFAULT_CAP_MASK     0x0300
-
-/* 1000BASE-T Status Register */
-#define MII_AT001_SR_1000T_LP_HD_CAPS           0x0400  /* LP is 1000T HD capable */
-#define MII_AT001_SR_1000T_LP_FD_CAPS           0x0800  /* LP is 1000T FD capable */
-#define MII_AT001_SR_1000T_REMOTE_RX_STATUS     0x1000  /* Remote receiver OK */
-#define MII_AT001_SR_1000T_LOCAL_RX_STATUS      0x2000  /* Local receiver OK */
-#define MII_AT001_SR_1000T_MS_CONFIG_RES        0x4000  /* 1=Local TX is Master, 0=Slave */
-#define MII_AT001_SR_1000T_MS_CONFIG_FAULT      0x8000  /* Master/Slave config fault */
-#define MII_AT001_SR_1000T_REMOTE_RX_STATUS_SHIFT   12
-#define MII_AT001_SR_1000T_LOCAL_RX_STATUS_SHIFT    13
-
-/* Extended Status Register */
-#define MII_AT001_ESR_1000T_HD_CAPS             0x1000  /* 1000T HD capable */
-#define MII_AT001_ESR_1000T_FD_CAPS             0x2000  /* 1000T FD capable */
-#define MII_AT001_ESR_1000X_HD_CAPS             0x4000  /* 1000X HD capable */
-#define MII_AT001_ESR_1000X_FD_CAPS             0x8000  /* 1000X FD capable */
+#define MII_AT001_CR_1000T_SPEED_MASK \
+	(ADVERTISE_1000FULL | ADVERTISE_1000HALF)
+#define MII_AT001_CR_1000T_DEFAULT_CAP_MASK	MII_AT001_CR_1000T_SPEED_MASK
 
 /* AT001 PHY Specific Control Register */
 #define MII_AT001_PSCR_JABBER_DISABLE           0x0001  /* 1=Jabber Function disabled */
diff --git a/drivers/net/atl1e/atl1e_main.c b/drivers/net/atl1e/atl1e_main.c
index e28f8baf394e..1ff001a8270c 100644
--- a/drivers/net/atl1e/atl1e_main.c
+++ b/drivers/net/atl1e/atl1e_main.c
@@ -547,8 +547,8 @@ static int __devinit atl1e_sw_init(struct atl1e_adapter *adapter)
 	hw->device_id = pdev->device;
 	hw->subsystem_vendor_id = pdev->subsystem_vendor;
 	hw->subsystem_id = pdev->subsystem_device;
+	hw->revision_id  = pdev->revision;
 
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &hw->revision_id);
 	pci_read_config_word(pdev, PCI_COMMAND, &hw->pci_cmd_word);
 
 	phy_status_data = AT_READ_REG(hw, REG_PHY_STATUS);
@@ -932,11 +932,11 @@ static inline void atl1e_configure_tx(struct atl1e_adapter *adapter)
 	max_pay_load  = ((dev_ctrl_data >> DEVICE_CTRL_MAX_PAYLOAD_SHIFT)) &
 			DEVICE_CTRL_MAX_PAYLOAD_MASK;
 
-	hw->dmaw_block = min(max_pay_load, hw->dmaw_block);
+	hw->dmaw_block = min_t(u32, max_pay_load, hw->dmaw_block);
 
 	max_pay_load  = ((dev_ctrl_data >> DEVICE_CTRL_MAX_RREQ_SZ_SHIFT)) &
 			DEVICE_CTRL_MAX_RREQ_SZ_MASK;
-	hw->dmar_block = min(max_pay_load, hw->dmar_block);
+	hw->dmar_block = min_t(u32, max_pay_load, hw->dmar_block);
 
 	if (hw->nic_type != athr_l2e_revB)
 		AT_WRITE_REGW(hw, REG_TXQ_CTRL + 2,
@@ -2051,9 +2051,9 @@ static int atl1e_suspend(struct pci_dev *pdev, pm_message_t state)
 		atl1e_read_phy_reg(hw, MII_BMSR, (u16 *)&mii_bmsr_data);
 		atl1e_read_phy_reg(hw, MII_BMSR, (u16 *)&mii_bmsr_data);
 
-		mii_advertise_data = MII_AR_10T_HD_CAPS;
+		mii_advertise_data = ADVERTISE_10HALF;
 
-		if ((atl1e_write_phy_reg(hw, MII_AT001_CR, 0) != 0) ||
+		if ((atl1e_write_phy_reg(hw, MII_CTRL1000, 0) != 0) ||
 		    (atl1e_write_phy_reg(hw,
 			   MII_ADVERTISE, mii_advertise_data) != 0) ||
 		    (atl1e_phy_commit(hw)) != 0) {
diff --git a/drivers/net/atlx/atl1.c b/drivers/net/atlx/atl1.c
index 3b527687c28f..67f40b9c16ed 100644
--- a/drivers/net/atlx/atl1.c
+++ b/drivers/net/atlx/atl1.c
@@ -950,6 +950,7 @@ static int __devinit atl1_sw_init(struct atl1_adapter *adapter)
 	hw->min_frame_size = ETH_ZLEN + ETH_FCS_LEN;
 
 	adapter->wol = 0;
+	device_set_wakeup_enable(&adapter->pdev->dev, false);
 	adapter->rx_buffer_len = (hw->max_frame_size + 7) & ~7;
 	adapter->ict = 50000;		/* 100ms */
 	adapter->link_speed = SPEED_0;	/* hardware init */
@@ -2735,15 +2736,15 @@ static int atl1_close(struct net_device *netdev)
 }
 
 #ifdef CONFIG_PM
-static int atl1_suspend(struct pci_dev *pdev, pm_message_t state)
+static int atl1_suspend(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct atl1_adapter *adapter = netdev_priv(netdev);
 	struct atl1_hw *hw = &adapter->hw;
 	u32 ctrl = 0;
 	u32 wufc = adapter->wol;
 	u32 val;
-	int retval;
 	u16 speed;
 	u16 duplex;
 
@@ -2751,17 +2752,15 @@ static int atl1_suspend(struct pci_dev *pdev, pm_message_t state)
 	if (netif_running(netdev))
 		atl1_down(adapter);
 
-	retval = pci_save_state(pdev);
-	if (retval)
-		return retval;
-
 	atl1_read_phy_reg(hw, MII_BMSR, (u16 *) & ctrl);
 	atl1_read_phy_reg(hw, MII_BMSR, (u16 *) & ctrl);
 	val = ctrl & BMSR_LSTATUS;
 	if (val)
 		wufc &= ~ATLX_WUFC_LNKC;
+	if (!wufc)
+		goto disable_wol;
 
-	if (val && wufc) {
+	if (val) {
 		val = atl1_get_speed_and_duplex(hw, &speed, &duplex);
 		if (val) {
 			if (netif_msg_ifdown(adapter))
@@ -2798,23 +2797,18 @@ static int atl1_suspend(struct pci_dev *pdev, pm_message_t state)
 		ctrl |= PCIE_PHYMISC_FORCE_RCV_DET;
 		iowrite32(ctrl, hw->hw_addr + REG_PCIE_PHYMISC);
 		ioread32(hw->hw_addr + REG_PCIE_PHYMISC);
-
-		pci_enable_wake(pdev, pci_choose_state(pdev, state), 1);
-		goto exit;
-	}
-
-	if (!val && wufc) {
+	} else {
 		ctrl |= (WOL_LINK_CHG_EN | WOL_LINK_CHG_PME_EN);
 		iowrite32(ctrl, hw->hw_addr + REG_WOL_CTRL);
 		ioread32(hw->hw_addr + REG_WOL_CTRL);
 		iowrite32(0, hw->hw_addr + REG_MAC_CTRL);
 		ioread32(hw->hw_addr + REG_MAC_CTRL);
 		hw->phy_configured = false;
-		pci_enable_wake(pdev, pci_choose_state(pdev, state), 1);
-		goto exit;
 	}
 
-disable_wol:
+	return 0;
+
+ disable_wol:
 	iowrite32(0, hw->hw_addr + REG_WOL_CTRL);
 	ioread32(hw->hw_addr + REG_WOL_CTRL);
 	ctrl = ioread32(hw->hw_addr + REG_PCIE_PHYMISC);
@@ -2822,37 +2816,17 @@ disable_wol:
 	iowrite32(ctrl, hw->hw_addr + REG_PCIE_PHYMISC);
 	ioread32(hw->hw_addr + REG_PCIE_PHYMISC);
 	hw->phy_configured = false;
-	pci_enable_wake(pdev, pci_choose_state(pdev, state), 0);
-exit:
-	if (netif_running(netdev))
-		pci_disable_msi(adapter->pdev);
-	pci_disable_device(pdev);
-	pci_set_power_state(pdev, pci_choose_state(pdev, state));
 
 	return 0;
 }
 
-static int atl1_resume(struct pci_dev *pdev)
+static int atl1_resume(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct atl1_adapter *adapter = netdev_priv(netdev);
-	u32 err;
 
-	pci_set_power_state(pdev, PCI_D0);
-	pci_restore_state(pdev);
-
-	err = pci_enable_device(pdev);
-	if (err) {
-		if (netif_msg_ifup(adapter))
-			dev_printk(KERN_DEBUG, &pdev->dev,
-				"error enabling pci device\n");
-		return err;
-	}
-
-	pci_set_master(pdev);
 	iowrite32(0, adapter->hw.hw_addr + REG_WOL_CTRL);
-	pci_enable_wake(pdev, PCI_D3hot, 0);
-	pci_enable_wake(pdev, PCI_D3cold, 0);
 
 	atl1_reset_hw(&adapter->hw);
 
@@ -2864,16 +2838,25 @@ static int atl1_resume(struct pci_dev *pdev)
 
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(atl1_pm_ops, atl1_suspend, atl1_resume);
+#define ATL1_PM_OPS	(&atl1_pm_ops)
+
 #else
-#define atl1_suspend NULL
-#define atl1_resume NULL
+
+static int atl1_suspend(struct device *dev) { return 0; }
+
+#define ATL1_PM_OPS	NULL
 #endif
 
 static void atl1_shutdown(struct pci_dev *pdev)
 {
-#ifdef CONFIG_PM
-	atl1_suspend(pdev, PMSG_SUSPEND);
-#endif
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct atl1_adapter *adapter = netdev_priv(netdev);
+
+	atl1_suspend(&pdev->dev);
+	pci_wake_from_d3(pdev, adapter->wol);
+	pci_set_power_state(pdev, PCI_D3hot);
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
@@ -3117,9 +3100,8 @@ static struct pci_driver atl1_driver = {
 	.id_table = atl1_pci_tbl,
 	.probe = atl1_probe,
 	.remove = __devexit_p(atl1_remove),
-	.suspend = atl1_suspend,
-	.resume = atl1_resume,
-	.shutdown = atl1_shutdown
+	.shutdown = atl1_shutdown,
+	.driver.pm = ATL1_PM_OPS,
 };
 
 /*
@@ -3409,6 +3391,9 @@ static int atl1_set_wol(struct net_device *netdev,
 	adapter->wol = 0;
 	if (wol->wolopts & WAKE_MAGIC)
 		adapter->wol |= ATLX_WUFC_MAG;
+
+	device_set_wakeup_enable(&adapter->pdev->dev, adapter->wol);
+
 	return 0;
 }
 
diff --git a/drivers/net/atlx/atl2.c b/drivers/net/atlx/atl2.c
index 4e6f4e95a5a0..e637e9f28fd4 100644
--- a/drivers/net/atlx/atl2.c
+++ b/drivers/net/atlx/atl2.c
@@ -93,8 +93,8 @@ static int __devinit atl2_sw_init(struct atl2_adapter *adapter)
 	hw->device_id = pdev->device;
 	hw->subsystem_vendor_id = pdev->subsystem_vendor;
 	hw->subsystem_id = pdev->subsystem_device;
+	hw->revision_id  = pdev->revision;
 
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &hw->revision_id);
 	pci_read_config_word(pdev, PCI_COMMAND, &hw->pci_cmd_word);
 
 	adapter->wol = 0;
diff --git a/drivers/net/ax88796.c b/drivers/net/ax88796.c
index 4bebff3faeab..e7cb8c8b9776 100644
--- a/drivers/net/ax88796.c
+++ b/drivers/net/ax88796.c
@@ -9,7 +9,7 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
-*/
+ */
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -17,46 +17,45 @@
 #include <linux/isapnp.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/timer.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/ethtool.h>
-#include <linux/mii.h>
+#include <linux/mdio-bitbang.h>
+#include <linux/phy.h>
 #include <linux/eeprom_93cx6.h>
 #include <linux/slab.h>
 
 #include <net/ax88796.h>
 
 #include <asm/system.h>
-#include <asm/io.h>
-
-static int phy_debug = 0;
 
 /* Rename the lib8390.c functions to show that they are in this driver */
-#define __ei_open       ax_ei_open
-#define __ei_close      ax_ei_close
-#define __ei_poll	ax_ei_poll
+#define __ei_open ax_ei_open
+#define __ei_close ax_ei_close
+#define __ei_poll ax_ei_poll
 #define __ei_start_xmit ax_ei_start_xmit
 #define __ei_tx_timeout ax_ei_tx_timeout
-#define __ei_get_stats  ax_ei_get_stats
+#define __ei_get_stats ax_ei_get_stats
 #define __ei_set_multicast_list ax_ei_set_multicast_list
-#define __ei_interrupt  ax_ei_interrupt
+#define __ei_interrupt ax_ei_interrupt
 #define ____alloc_ei_netdev ax__alloc_ei_netdev
-#define __NS8390_init   ax_NS8390_init
+#define __NS8390_init ax_NS8390_init
 
 /* force unsigned long back to 'void __iomem *' */
 #define ax_convert_addr(_a) ((void __force __iomem *)(_a))
 
-#define ei_inb(_a)	readb(ax_convert_addr(_a))
+#define ei_inb(_a) readb(ax_convert_addr(_a))
 #define ei_outb(_v, _a) writeb(_v, ax_convert_addr(_a))
 
-#define ei_inb_p(_a)	ei_inb(_a)
+#define ei_inb_p(_a) ei_inb(_a)
 #define ei_outb_p(_v, _a) ei_outb(_v, _a)
 
 /* define EI_SHIFT() to take into account our register offsets */
-#define EI_SHIFT(x)     (ei_local->reg_offset[(x)])
+#define EI_SHIFT(x) (ei_local->reg_offset[(x)])
 
 /* Ensure we have our RCR base value */
 #define AX88796_PLATFORM
@@ -74,43 +73,46 @@ static unsigned char version[] = "ax88796.c: Copyright 2005,2007 Simtec Electron
 #define NE_DATAPORT	EI_SHIFT(0x10)
 
 #define NE1SM_START_PG	0x20	/* First page of TX buffer */
-#define NE1SM_STOP_PG 	0x40	/* Last page +1 of RX ring */
+#define NE1SM_STOP_PG	0x40	/* Last page +1 of RX ring */
 #define NESM_START_PG	0x40	/* First page of TX buffer */
 #define NESM_STOP_PG	0x80	/* Last page +1 of RX ring */
 
+#define AX_GPOC_PPDSET	BIT(6)
+
 /* device private data */
 
 struct ax_device {
-	struct timer_list	 mii_timer;
-	spinlock_t		 mii_lock;
-	struct mii_if_info	 mii;
-
-	u32			 msg_enable;
-	void __iomem		*map2;
-	struct platform_device	*dev;
-	struct resource		*mem;
-	struct resource		*mem2;
-	struct ax_plat_data	*plat;
-
-	unsigned char		 running;
-	unsigned char		 resume_open;
-	unsigned int		 irqflags;
-
-	u32			 reg_offsets[0x20];
+	struct mii_bus *mii_bus;
+	struct mdiobb_ctrl bb_ctrl;
+	struct phy_device *phy_dev;
+	void __iomem *addr_memr;
+	u8 reg_memr;
+	int link;
+	int speed;
+	int duplex;
+
+	void __iomem *map2;
+	const struct ax_plat_data *plat;
+
+	unsigned char running;
+	unsigned char resume_open;
+	unsigned int irqflags;
+
+	u32 reg_offsets[0x20];
 };
 
 static inline struct ax_device *to_ax_dev(struct net_device *dev)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
-	return (struct ax_device *)(ei_local+1);
+	return (struct ax_device *)(ei_local + 1);
 }
 
-/* ax_initial_check
+/*
+ * ax_initial_check
  *
  * do an initial probe for the card to check wether it exists
  * and is functional
  */
-
 static int ax_initial_check(struct net_device *dev)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
@@ -122,10 +124,10 @@ static int ax_initial_check(struct net_device *dev)
 	if (reg0 == 0xFF)
 		return -ENODEV;
 
-	ei_outb(E8390_NODMA+E8390_PAGE1+E8390_STOP, ioaddr + E8390_CMD);
+	ei_outb(E8390_NODMA + E8390_PAGE1 + E8390_STOP, ioaddr + E8390_CMD);
 	regd = ei_inb(ioaddr + 0x0d);
 	ei_outb(0xff, ioaddr + 0x0d);
-	ei_outb(E8390_NODMA+E8390_PAGE0, ioaddr + E8390_CMD);
+	ei_outb(E8390_NODMA + E8390_PAGE0, ioaddr + E8390_CMD);
 	ei_inb(ioaddr + EN0_COUNTER0); /* Clear the counter by reading. */
 	if (ei_inb(ioaddr + EN0_COUNTER0) != 0) {
 		ei_outb(reg0, ioaddr);
@@ -136,29 +138,28 @@ static int ax_initial_check(struct net_device *dev)
 	return 0;
 }
 
-/* Hard reset the card.  This used to pause for the same period that a
-   8390 reset command required, but that shouldn't be necessary. */
-
+/*
+ * Hard reset the card. This used to pause for the same period that a
+ * 8390 reset command required, but that shouldn't be necessary.
+ */
 static void ax_reset_8390(struct net_device *dev)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
-	struct ax_device  *ax = to_ax_dev(dev);
 	unsigned long reset_start_time = jiffies;
 	void __iomem *addr = (void __iomem *)dev->base_addr;
 
 	if (ei_debug > 1)
-		dev_dbg(&ax->dev->dev, "resetting the 8390 t=%ld\n", jiffies);
+		netdev_dbg(dev, "resetting the 8390 t=%ld\n", jiffies);
 
 	ei_outb(ei_inb(addr + NE_RESET), addr + NE_RESET);
 
-	ei_status.txing = 0;
-	ei_status.dmaing = 0;
+	ei_local->txing = 0;
+	ei_local->dmaing = 0;
 
 	/* This check _should_not_ be necessary, omit eventually. */
 	while ((ei_inb(addr + EN0_ISR) & ENISR_RESET) == 0) {
-		if (jiffies - reset_start_time > 2*HZ/100) {
-			dev_warn(&ax->dev->dev, "%s: %s did not complete.\n",
-			       __func__, dev->name);
+		if (jiffies - reset_start_time > 2 * HZ / 100) {
+			netdev_warn(dev, "%s: did not complete.\n", __func__);
 			break;
 		}
 	}
@@ -171,70 +172,72 @@ static void ax_get_8390_hdr(struct net_device *dev, struct e8390_pkt_hdr *hdr,
 			    int ring_page)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
-	struct ax_device  *ax = to_ax_dev(dev);
 	void __iomem *nic_base = ei_local->mem;
 
 	/* This *shouldn't* happen. If it does, it's the last thing you'll see */
-	if (ei_status.dmaing) {
-		dev_err(&ax->dev->dev, "%s: DMAing conflict in %s "
+	if (ei_local->dmaing) {
+		netdev_err(dev, "DMAing conflict in %s "
 			"[DMAstat:%d][irqlock:%d].\n",
-			dev->name, __func__,
-			ei_status.dmaing, ei_status.irqlock);
+			__func__,
+			ei_local->dmaing, ei_local->irqlock);
 		return;
 	}
 
-	ei_status.dmaing |= 0x01;
-	ei_outb(E8390_NODMA+E8390_PAGE0+E8390_START, nic_base+ NE_CMD);
+	ei_local->dmaing |= 0x01;
+	ei_outb(E8390_NODMA + E8390_PAGE0 + E8390_START, nic_base + NE_CMD);
 	ei_outb(sizeof(struct e8390_pkt_hdr), nic_base + EN0_RCNTLO);
 	ei_outb(0, nic_base + EN0_RCNTHI);
 	ei_outb(0, nic_base + EN0_RSARLO);		/* On page boundary */
 	ei_outb(ring_page, nic_base + EN0_RSARHI);
 	ei_outb(E8390_RREAD+E8390_START, nic_base + NE_CMD);
 
-	if (ei_status.word16)
-		readsw(nic_base + NE_DATAPORT, hdr, sizeof(struct e8390_pkt_hdr)>>1);
+	if (ei_local->word16)
+		readsw(nic_base + NE_DATAPORT, hdr,
+		       sizeof(struct e8390_pkt_hdr) >> 1);
 	else
-		readsb(nic_base + NE_DATAPORT, hdr, sizeof(struct e8390_pkt_hdr));
+		readsb(nic_base + NE_DATAPORT, hdr,
+		       sizeof(struct e8390_pkt_hdr));
 
 	ei_outb(ENISR_RDC, nic_base + EN0_ISR);	/* Ack intr. */
-	ei_status.dmaing &= ~0x01;
+	ei_local->dmaing &= ~0x01;
 
 	le16_to_cpus(&hdr->count);
 }
 
 
-/* Block input and output, similar to the Crynwr packet driver.  If you
-   are porting to a new ethercard, look at the packet driver source for hints.
-   The NEx000 doesn't share the on-board packet memory -- you have to put
-   the packet out through the "remote DMA" dataport using ei_outb. */
-
+/*
+ * Block input and output, similar to the Crynwr packet driver. If
+ * you are porting to a new ethercard, look at the packet driver
+ * source for hints. The NEx000 doesn't share the on-board packet
+ * memory -- you have to put the packet out through the "remote DMA"
+ * dataport using ei_outb.
+ */
 static void ax_block_input(struct net_device *dev, int count,
 			   struct sk_buff *skb, int ring_offset)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
-	struct ax_device  *ax = to_ax_dev(dev);
 	void __iomem *nic_base = ei_local->mem;
 	char *buf = skb->data;
 
-	if (ei_status.dmaing) {
-		dev_err(&ax->dev->dev,
-			"%s: DMAing conflict in %s "
+	if (ei_local->dmaing) {
+		netdev_err(dev,
+			"DMAing conflict in %s "
 			"[DMAstat:%d][irqlock:%d].\n",
-			dev->name, __func__,
-			ei_status.dmaing, ei_status.irqlock);
+			__func__,
+			ei_local->dmaing, ei_local->irqlock);
 		return;
 	}
 
-	ei_status.dmaing |= 0x01;
+	ei_local->dmaing |= 0x01;
 
-	ei_outb(E8390_NODMA+E8390_PAGE0+E8390_START, nic_base+ NE_CMD);
+	ei_outb(E8390_NODMA+E8390_PAGE0+E8390_START, nic_base + NE_CMD);
 	ei_outb(count & 0xff, nic_base + EN0_RCNTLO);
 	ei_outb(count >> 8, nic_base + EN0_RCNTHI);
 	ei_outb(ring_offset & 0xff, nic_base + EN0_RSARLO);
 	ei_outb(ring_offset >> 8, nic_base + EN0_RSARHI);
 	ei_outb(E8390_RREAD+E8390_START, nic_base + NE_CMD);
 
-	if (ei_status.word16) {
+	if (ei_local->word16) {
 		readsw(nic_base + NE_DATAPORT, buf, count >> 1);
 		if (count & 0x01)
 			buf[count-1] = ei_inb(nic_base + NE_DATAPORT);
@@ -243,34 +246,34 @@ static void ax_block_input(struct net_device *dev, int count,
 		readsb(nic_base + NE_DATAPORT, buf, count);
 	}
 
-	ei_status.dmaing &= ~1;
+	ei_local->dmaing &= ~1;
 }
 
 static void ax_block_output(struct net_device *dev, int count,
 			    const unsigned char *buf, const int start_page)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
-	struct ax_device  *ax = to_ax_dev(dev);
 	void __iomem *nic_base = ei_local->mem;
 	unsigned long dma_start;
 
-	/* Round the count up for word writes.  Do we need to do this?
-	   What effect will an odd byte count have on the 8390?
-	   I should check someday. */
-
-	if (ei_status.word16 && (count & 0x01))
+	/*
+	 * Round the count up for word writes. Do we need to do this?
+	 * What effect will an odd byte count have on the 8390?  I
+	 * should check someday.
+	 */
+	if (ei_local->word16 && (count & 0x01))
 		count++;
 
 	/* This *shouldn't* happen. If it does, it's the last thing you'll see */
-	if (ei_status.dmaing) {
-		dev_err(&ax->dev->dev, "%s: DMAing conflict in %s."
+	if (ei_local->dmaing) {
+		netdev_err(dev, "DMAing conflict in %s."
 			"[DMAstat:%d][irqlock:%d]\n",
-			dev->name, __func__,
-		       ei_status.dmaing, ei_status.irqlock);
+			__func__,
+		       ei_local->dmaing, ei_local->irqlock);
 		return;
 	}
 
-	ei_status.dmaing |= 0x01;
+	ei_local->dmaing |= 0x01;
 	/* We should already be in page 0, but to be safe... */
 	ei_outb(E8390_PAGE0+E8390_START+E8390_NODMA, nic_base + NE_CMD);
 
@@ -278,250 +281,170 @@ static void ax_block_output(struct net_device *dev, int count,
 
 	/* Now the normal output. */
 	ei_outb(count & 0xff, nic_base + EN0_RCNTLO);
-	ei_outb(count >> 8,   nic_base + EN0_RCNTHI);
+	ei_outb(count >> 8, nic_base + EN0_RCNTHI);
 	ei_outb(0x00, nic_base + EN0_RSARLO);
 	ei_outb(start_page, nic_base + EN0_RSARHI);
 
 	ei_outb(E8390_RWRITE+E8390_START, nic_base + NE_CMD);
-	if (ei_status.word16) {
-		writesw(nic_base + NE_DATAPORT, buf, count>>1);
-	} else {
+	if (ei_local->word16)
+		writesw(nic_base + NE_DATAPORT, buf, count >> 1);
+	else
 		writesb(nic_base + NE_DATAPORT, buf, count);
-	}
 
 	dma_start = jiffies;
 
 	while ((ei_inb(nic_base + EN0_ISR) & ENISR_RDC) == 0) {
-		if (jiffies - dma_start > 2*HZ/100) {		/* 20ms */
-			dev_warn(&ax->dev->dev,
-				 "%s: timeout waiting for Tx RDC.\n", dev->name);
+		if (jiffies - dma_start > 2 * HZ / 100) {		/* 20ms */
+			netdev_warn(dev, "timeout waiting for Tx RDC.\n");
 			ax_reset_8390(dev);
-			ax_NS8390_init(dev,1);
+			ax_NS8390_init(dev, 1);
 			break;
 		}
 	}
 
 	ei_outb(ENISR_RDC, nic_base + EN0_ISR);	/* Ack intr. */
-	ei_status.dmaing &= ~0x01;
+	ei_local->dmaing &= ~0x01;
 }
 
 /* definitions for accessing MII/EEPROM interface */
 
 #define AX_MEMR			EI_SHIFT(0x14)
-#define AX_MEMR_MDC		(1<<0)
-#define AX_MEMR_MDIR		(1<<1)
-#define AX_MEMR_MDI		(1<<2)
-#define AX_MEMR_MDO		(1<<3)
-#define AX_MEMR_EECS		(1<<4)
-#define AX_MEMR_EEI		(1<<5)
-#define AX_MEMR_EEO		(1<<6)
-#define AX_MEMR_EECLK		(1<<7)
-
-/* ax_mii_ei_outbits
- *
- * write the specified set of bits to the phy
-*/
-
-static void
-ax_mii_ei_outbits(struct net_device *dev, unsigned int bits, int len)
+#define AX_MEMR_MDC		BIT(0)
+#define AX_MEMR_MDIR		BIT(1)
+#define AX_MEMR_MDI		BIT(2)
+#define AX_MEMR_MDO		BIT(3)
+#define AX_MEMR_EECS		BIT(4)
+#define AX_MEMR_EEI		BIT(5)
+#define AX_MEMR_EEO		BIT(6)
+#define AX_MEMR_EECLK		BIT(7)
+
+static void ax_handle_link_change(struct net_device *dev)
 {
-	struct ei_device *ei_local = netdev_priv(dev);
-	void __iomem *memr_addr = (void __iomem *)dev->base_addr + AX_MEMR;
-	unsigned int memr;
-
-	/* clock low, data to output mode */
-	memr = ei_inb(memr_addr);
-	memr &= ~(AX_MEMR_MDC | AX_MEMR_MDIR);
-	ei_outb(memr, memr_addr);
-
-	for (len--; len >= 0; len--) {
-		if (bits & (1 << len))
-			memr |= AX_MEMR_MDO;
-		else
-			memr &= ~AX_MEMR_MDO;
-
-		ei_outb(memr, memr_addr);
-
-		/* clock high */
+	struct ax_device  *ax = to_ax_dev(dev);
+	struct phy_device *phy_dev = ax->phy_dev;
+	int status_change = 0;
 
-		ei_outb(memr | AX_MEMR_MDC, memr_addr);
-		udelay(1);
+	if (phy_dev->link && ((ax->speed != phy_dev->speed) ||
+			     (ax->duplex != phy_dev->duplex))) {
 
-		/* clock low */
-		ei_outb(memr, memr_addr);
+		ax->speed = phy_dev->speed;
+		ax->duplex = phy_dev->duplex;
+		status_change = 1;
 	}
 
-	/* leaves the clock line low, mdir input */
-	memr |= AX_MEMR_MDIR;
-	ei_outb(memr, (void __iomem *)dev->base_addr + AX_MEMR);
-}
-
-/* ax_phy_ei_inbits
- *
- * read a specified number of bits from the phy
-*/
-
-static unsigned int
-ax_phy_ei_inbits(struct net_device *dev, int no)
-{
-	struct ei_device *ei_local = netdev_priv(dev);
-	void __iomem *memr_addr = (void __iomem *)dev->base_addr + AX_MEMR;
-	unsigned int memr;
-	unsigned int result = 0;
-
-	/* clock low, data to input mode */
-	memr = ei_inb(memr_addr);
-	memr &= ~AX_MEMR_MDC;
-	memr |= AX_MEMR_MDIR;
-	ei_outb(memr, memr_addr);
-
-	for (no--; no >= 0; no--) {
-		ei_outb(memr | AX_MEMR_MDC, memr_addr);
-
-		udelay(1);
-
-		if (ei_inb(memr_addr) & AX_MEMR_MDI)
-			result |= (1<<no);
+	if (phy_dev->link != ax->link) {
+		if (!phy_dev->link) {
+			ax->speed = 0;
+			ax->duplex = -1;
+		}
+		ax->link = phy_dev->link;
 
-		ei_outb(memr, memr_addr);
+		status_change = 1;
 	}
 
-	return result;
-}
-
-/* ax_phy_issueaddr
- *
- * use the low level bit shifting routines to send the address
- * and command to the specified phy
-*/
-
-static void
-ax_phy_issueaddr(struct net_device *dev, int phy_addr, int reg, int opc)
-{
-	if (phy_debug)
-		pr_debug("%s: dev %p, %04x, %04x, %d\n",
-			__func__, dev, phy_addr, reg, opc);
-
-	ax_mii_ei_outbits(dev, 0x3f, 6);	/* pre-amble */
-	ax_mii_ei_outbits(dev, 1, 2);		/* frame-start */
-	ax_mii_ei_outbits(dev, opc, 2);		/* op code */
-	ax_mii_ei_outbits(dev, phy_addr, 5);	/* phy address */
-	ax_mii_ei_outbits(dev, reg, 5);		/* reg address */
+	if (status_change)
+		phy_print_status(phy_dev);
 }
 
-static int
-ax_phy_read(struct net_device *dev, int phy_addr, int reg)
+static int ax_mii_probe(struct net_device *dev)
 {
-	struct ei_device *ei_local = netdev_priv(dev);
-	unsigned long flags;
- 	unsigned int result;
+	struct ax_device  *ax = to_ax_dev(dev);
+	struct phy_device *phy_dev = NULL;
+	int ret;
 
-      	spin_lock_irqsave(&ei_local->page_lock, flags);
+	/* find the first phy */
+	phy_dev = phy_find_first(ax->mii_bus);
+	if (!phy_dev) {
+		netdev_err(dev, "no PHY found\n");
+		return -ENODEV;
+	}
 
-	ax_phy_issueaddr(dev, phy_addr, reg, 2);
+	ret = phy_connect_direct(dev, phy_dev, ax_handle_link_change, 0,
+				 PHY_INTERFACE_MODE_MII);
+	if (ret) {
+		netdev_err(dev, "Could not attach to PHY\n");
+		return ret;
+	}
 
-	result = ax_phy_ei_inbits(dev, 17);
-	result &= ~(3<<16);
+	/* mask with MAC supported features */
+	phy_dev->supported &= PHY_BASIC_FEATURES;
+	phy_dev->advertising = phy_dev->supported;
 
-      	spin_unlock_irqrestore(&ei_local->page_lock, flags);
+	ax->phy_dev = phy_dev;
 
-	if (phy_debug)
-		pr_debug("%s: %04x.%04x => read %04x\n", __func__,
-			 phy_addr, reg, result);
+	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);
 
-	return result;
+	return 0;
 }
 
-static void
-ax_phy_write(struct net_device *dev, int phy_addr, int reg, int value)
+static void ax_phy_switch(struct net_device *dev, int on)
 {
-	struct ei_device *ei = netdev_priv(dev);
-	struct ax_device  *ax = to_ax_dev(dev);
-	unsigned long flags;
-
-	dev_dbg(&ax->dev->dev, "%s: %p, %04x, %04x %04x\n",
-		__func__, dev, phy_addr, reg, value);
-
-      	spin_lock_irqsave(&ei->page_lock, flags);
-
-	ax_phy_issueaddr(dev, phy_addr, reg, 1);
-	ax_mii_ei_outbits(dev, 2, 2);		/* send TA */
-	ax_mii_ei_outbits(dev, value, 16);
-
-      	spin_unlock_irqrestore(&ei->page_lock, flags);
-}
+	struct ei_device *ei_local = netdev_priv(dev);
+	struct ax_device *ax = to_ax_dev(dev);
 
-static void ax_mii_expiry(unsigned long data)
-{
-	struct net_device *dev = (struct net_device *)data;
-	struct ax_device  *ax = to_ax_dev(dev);
-	unsigned long flags;
+	u8 reg_gpoc =  ax->plat->gpoc_val;
 
-	spin_lock_irqsave(&ax->mii_lock, flags);
-	mii_check_media(&ax->mii, netif_msg_link(ax), 0);
-	spin_unlock_irqrestore(&ax->mii_lock, flags);
+	if (!!on)
+		reg_gpoc &= ~AX_GPOC_PPDSET;
+	else
+		reg_gpoc |= AX_GPOC_PPDSET;
 
-	if (ax->running) {
-		ax->mii_timer.expires = jiffies + HZ*2;
-		add_timer(&ax->mii_timer);
-	}
+	ei_outb(reg_gpoc, ei_local->mem + EI_SHIFT(0x17));
 }
 
 static int ax_open(struct net_device *dev)
 {
-	struct ax_device  *ax = to_ax_dev(dev);
-	struct ei_device *ei_local = netdev_priv(dev);
+	struct ax_device *ax = to_ax_dev(dev);
 	int ret;
 
-	dev_dbg(&ax->dev->dev, "%s: open\n", dev->name);
+	netdev_dbg(dev, "open\n");
 
 	ret = request_irq(dev->irq, ax_ei_interrupt, ax->irqflags,
 			  dev->name, dev);
 	if (ret)
-		return ret;
-
-	ret = ax_ei_open(dev);
-	if (ret) {
-		free_irq(dev->irq, dev);
-		return ret;
-	}
+		goto failed_request_irq;
 
 	/* turn the phy on (if turned off) */
+	ax_phy_switch(dev, 1);
 
-	ei_outb(ax->plat->gpoc_val, ei_local->mem + EI_SHIFT(0x17));
-	ax->running = 1;
-
-	/* start the MII timer */
-
-	init_timer(&ax->mii_timer);
+	ret = ax_mii_probe(dev);
+	if (ret)
+		goto failed_mii_probe;
+	phy_start(ax->phy_dev);
 
-	ax->mii_timer.expires  = jiffies+1;
-	ax->mii_timer.data     = (unsigned long) dev;
-	ax->mii_timer.function = ax_mii_expiry;
+	ret = ax_ei_open(dev);
+	if (ret)
+		goto failed_ax_ei_open;
 
-	add_timer(&ax->mii_timer);
+	ax->running = 1;
 
 	return 0;
+
+ failed_ax_ei_open:
+	phy_disconnect(ax->phy_dev);
+ failed_mii_probe:
+	ax_phy_switch(dev, 0);
+	free_irq(dev->irq, dev);
+ failed_request_irq:
+	return ret;
 }
 
 static int ax_close(struct net_device *dev)
 {
 	struct ax_device *ax = to_ax_dev(dev);
-	struct ei_device *ei_local = netdev_priv(dev);
 
-	dev_dbg(&ax->dev->dev, "%s: close\n", dev->name);
-
-	/* turn the phy off */
-
-	ei_outb(ax->plat->gpoc_val | (1<<6),
-	       ei_local->mem + EI_SHIFT(0x17));
+	netdev_dbg(dev, "close\n");
 
 	ax->running = 0;
 	wmb();
 
-	del_timer_sync(&ax->mii_timer);
 	ax_ei_close(dev);
 
+	/* turn the phy off */
+	ax_phy_switch(dev, 0);
+	phy_disconnect(ax->phy_dev);
+
 	free_irq(dev->irq, dev);
 	return 0;
 }
@@ -529,17 +452,15 @@ static int ax_close(struct net_device *dev)
 static int ax_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 {
 	struct ax_device *ax = to_ax_dev(dev);
-	unsigned long flags;
-	int rc;
+	struct phy_device *phy_dev = ax->phy_dev;
 
 	if (!netif_running(dev))
 		return -EINVAL;
 
-	spin_lock_irqsave(&ax->mii_lock, flags);
-	rc = generic_mii_ioctl(&ax->mii, if_mii(req), cmd, NULL);
-	spin_unlock_irqrestore(&ax->mii_lock, flags);
+	if (!phy_dev)
+		return -ENODEV;
 
-	return rc;
+	return phy_mii_ioctl(phy_dev, req, cmd);
 }
 
 /* ethtool ops */
@@ -547,56 +468,40 @@ static int ax_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 static void ax_get_drvinfo(struct net_device *dev,
 			   struct ethtool_drvinfo *info)
 {
-	struct ax_device *ax = to_ax_dev(dev);
+	struct platform_device *pdev = to_platform_device(dev->dev.parent);
 
 	strcpy(info->driver, DRV_NAME);
 	strcpy(info->version, DRV_VERSION);
-	strcpy(info->bus_info, ax->dev->name);
+	strcpy(info->bus_info, pdev->name);
 }
 
 static int ax_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
 	struct ax_device *ax = to_ax_dev(dev);
-	unsigned long flags;
+	struct phy_device *phy_dev = ax->phy_dev;
 
-	spin_lock_irqsave(&ax->mii_lock, flags);
-	mii_ethtool_gset(&ax->mii, cmd);
-	spin_unlock_irqrestore(&ax->mii_lock, flags);
+	if (!phy_dev)
+		return -ENODEV;
 
-	return 0;
+	return phy_ethtool_gset(phy_dev, cmd);
 }
 
 static int ax_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
 	struct ax_device *ax = to_ax_dev(dev);
-	unsigned long flags;
-	int rc;
+	struct phy_device *phy_dev = ax->phy_dev;
 
-	spin_lock_irqsave(&ax->mii_lock, flags);
-	rc = mii_ethtool_sset(&ax->mii, cmd);
-	spin_unlock_irqrestore(&ax->mii_lock, flags);
-
-	return rc;
-}
-
-static int ax_nway_reset(struct net_device *dev)
-{
-	struct ax_device *ax = to_ax_dev(dev);
-	return mii_nway_restart(&ax->mii);
-}
+	if (!phy_dev)
+		return -ENODEV;
 
-static u32 ax_get_link(struct net_device *dev)
-{
-	struct ax_device *ax = to_ax_dev(dev);
-	return mii_link_ok(&ax->mii);
+	return phy_ethtool_sset(phy_dev, cmd);
 }
 
 static const struct ethtool_ops ax_ethtool_ops = {
 	.get_drvinfo		= ax_get_drvinfo,
 	.get_settings		= ax_get_settings,
 	.set_settings		= ax_set_settings,
-	.nway_reset		= ax_nway_reset,
-	.get_link		= ax_get_link,
+	.get_link		= ethtool_op_get_link,
 };
 
 #ifdef CONFIG_AX88796_93CX6
@@ -640,37 +545,131 @@ static const struct net_device_ops ax_netdev_ops = {
 	.ndo_get_stats		= ax_ei_get_stats,
 	.ndo_set_multicast_list = ax_ei_set_multicast_list,
 	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_set_mac_address 	= eth_mac_addr,
+	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_change_mtu		= eth_change_mtu,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= ax_ei_poll,
 #endif
 };
 
+static void ax_bb_mdc(struct mdiobb_ctrl *ctrl, int level)
+{
+	struct ax_device *ax = container_of(ctrl, struct ax_device, bb_ctrl);
+
+	if (level)
+		ax->reg_memr |= AX_MEMR_MDC;
+	else
+		ax->reg_memr &= ~AX_MEMR_MDC;
+
+	ei_outb(ax->reg_memr, ax->addr_memr);
+}
+
+static void ax_bb_dir(struct mdiobb_ctrl *ctrl, int output)
+{
+	struct ax_device *ax = container_of(ctrl, struct ax_device, bb_ctrl);
+
+	if (output)
+		ax->reg_memr &= ~AX_MEMR_MDIR;
+	else
+		ax->reg_memr |= AX_MEMR_MDIR;
+
+	ei_outb(ax->reg_memr, ax->addr_memr);
+}
+
+static void ax_bb_set_data(struct mdiobb_ctrl *ctrl, int value)
+{
+	struct ax_device *ax = container_of(ctrl, struct ax_device, bb_ctrl);
+
+	if (value)
+		ax->reg_memr |= AX_MEMR_MDO;
+	else
+		ax->reg_memr &= ~AX_MEMR_MDO;
+
+	ei_outb(ax->reg_memr, ax->addr_memr);
+}
+
+static int ax_bb_get_data(struct mdiobb_ctrl *ctrl)
+{
+	struct ax_device *ax = container_of(ctrl, struct ax_device, bb_ctrl);
+	int reg_memr = ei_inb(ax->addr_memr);
+
+	return reg_memr & AX_MEMR_MDI ? 1 : 0;
+}
+
+static struct mdiobb_ops bb_ops = {
+	.owner = THIS_MODULE,
+	.set_mdc = ax_bb_mdc,
+	.set_mdio_dir = ax_bb_dir,
+	.set_mdio_data = ax_bb_set_data,
+	.get_mdio_data = ax_bb_get_data,
+};
+
 /* setup code */
 
+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;
+
+	ax->bb_ctrl.ops = &bb_ops;
+	ax->addr_memr = ei_local->mem + AX_MEMR;
+	ax->mii_bus = alloc_mdio_bitbang(&ax->bb_ctrl);
+	if (!ax->mii_bus) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	ax->mii_bus->name = "ax88796_mii_bus";
+	ax->mii_bus->parent = dev->dev.parent;
+	snprintf(ax->mii_bus->id, MII_BUS_ID_SIZE, "%x", 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;
+
+	return 0;
+
+ out_free_irq:
+	kfree(ax->mii_bus->irq);
+ out_free_mdio_bitbang:
+	free_mdio_bitbang(ax->mii_bus);
+ out:
+	return err;
+}
+
 static void ax_initial_setup(struct net_device *dev, struct ei_device *ei_local)
 {
 	void __iomem *ioaddr = ei_local->mem;
 	struct ax_device *ax = to_ax_dev(dev);
 
-	/* Select page 0*/
-	ei_outb(E8390_NODMA+E8390_PAGE0+E8390_STOP, ioaddr + E8390_CMD);
+	/* Select page 0 */
+	ei_outb(E8390_NODMA + E8390_PAGE0 + E8390_STOP, ioaddr + E8390_CMD);
 
 	/* set to byte access */
 	ei_outb(ax->plat->dcr_val & ~1, ioaddr + EN0_DCFG);
 	ei_outb(ax->plat->gpoc_val, ioaddr + EI_SHIFT(0x17));
 }
 
-/* ax_init_dev
+/*
+ * ax_init_dev
  *
  * initialise the specified device, taking care to note the MAC
  * address it may already have (if configured), ensure
  * the device is ready to be used by lib8390.c and registerd with
  * the network layer.
  */
-
-static int ax_init_dev(struct net_device *dev, int first_init)
+static int ax_init_dev(struct net_device *dev)
 {
 	struct ei_device *ei_local = netdev_priv(dev);
 	struct ax_device *ax = to_ax_dev(dev);
@@ -690,23 +689,23 @@ static int ax_init_dev(struct net_device *dev, int first_init)
 
 	/* read the mac from the card prom if we need it */
 
-	if (first_init && ax->plat->flags & AXFLG_HAS_EEPROM) {
+	if (ax->plat->flags & AXFLG_HAS_EEPROM) {
 		unsigned char SA_prom[32];
 
-		for(i = 0; i < sizeof(SA_prom); i+=2) {
+		for (i = 0; i < sizeof(SA_prom); i += 2) {
 			SA_prom[i] = ei_inb(ioaddr + NE_DATAPORT);
-			SA_prom[i+1] = ei_inb(ioaddr + NE_DATAPORT);
+			SA_prom[i + 1] = ei_inb(ioaddr + NE_DATAPORT);
 		}
 
 		if (ax->plat->wordlength == 2)
 			for (i = 0; i < 16; i++)
 				SA_prom[i] = SA_prom[i+i];
 
-		memcpy(dev->dev_addr,  SA_prom, 6);
+		memcpy(dev->dev_addr, SA_prom, 6);
 	}
 
 #ifdef CONFIG_AX88796_93CX6
-	if (first_init && ax->plat->flags & AXFLG_HAS_93CX6) {
+	if (ax->plat->flags & AXFLG_HAS_93CX6) {
 		unsigned char mac_addr[6];
 		struct eeprom_93cx6 eeprom;
 
@@ -719,7 +718,7 @@ static int ax_init_dev(struct net_device *dev, int first_init)
 				       (__le16 __force *)mac_addr,
 				       sizeof(mac_addr) >> 1);
 
-		memcpy(dev->dev_addr,  mac_addr, 6);
+		memcpy(dev->dev_addr, mac_addr, 6);
 	}
 #endif
 	if (ax->plat->wordlength == 2) {
@@ -732,67 +731,56 @@ static int ax_init_dev(struct net_device *dev, int first_init)
 		stop_page = NE1SM_STOP_PG;
 	}
 
-	/* load the mac-address from the device if this is the
-	 * first time we've initialised */
-
-	if (first_init) {
-		if (ax->plat->flags & AXFLG_MAC_FROMDEV) {
-			ei_outb(E8390_NODMA + E8390_PAGE1 + E8390_STOP,
-				ei_local->mem + E8390_CMD); /* 0x61 */
-			for (i = 0; i < ETHER_ADDR_LEN; i++)
-				dev->dev_addr[i] =
-					ei_inb(ioaddr + EN1_PHYS_SHIFT(i));
-		}
-
-		if ((ax->plat->flags & AXFLG_MAC_FROMPLATFORM) &&
-		     ax->plat->mac_addr)
-			memcpy(dev->dev_addr, ax->plat->mac_addr,
-				ETHER_ADDR_LEN);
+	/* load the mac-address from the device */
+	if (ax->plat->flags & AXFLG_MAC_FROMDEV) {
+		ei_outb(E8390_NODMA + E8390_PAGE1 + E8390_STOP,
+			ei_local->mem + E8390_CMD); /* 0x61 */
+		for (i = 0; i < ETHER_ADDR_LEN; i++)
+			dev->dev_addr[i] =
+				ei_inb(ioaddr + EN1_PHYS_SHIFT(i));
 	}
 
+	if ((ax->plat->flags & AXFLG_MAC_FROMPLATFORM) &&
+	    ax->plat->mac_addr)
+		memcpy(dev->dev_addr, ax->plat->mac_addr,
+		       ETHER_ADDR_LEN);
+
 	ax_reset_8390(dev);
 
-	ei_status.name = "AX88796";
-	ei_status.tx_start_page = start_page;
-	ei_status.stop_page = stop_page;
-	ei_status.word16 = (ax->plat->wordlength == 2);
-	ei_status.rx_start_page = start_page + TX_PAGES;
+	ei_local->name = "AX88796";
+	ei_local->tx_start_page = start_page;
+	ei_local->stop_page = stop_page;
+	ei_local->word16 = (ax->plat->wordlength == 2);
+	ei_local->rx_start_page = start_page + TX_PAGES;
 
 #ifdef PACKETBUF_MEMSIZE
-	 /* Allow the packet buffer size to be overridden by know-it-alls. */
-	ei_status.stop_page = ei_status.tx_start_page + PACKETBUF_MEMSIZE;
+	/* Allow the packet buffer size to be overridden by know-it-alls. */
+	ei_local->stop_page = ei_local->tx_start_page + PACKETBUF_MEMSIZE;
 #endif
 
-	ei_status.reset_8390	= &ax_reset_8390;
-	ei_status.block_input	= &ax_block_input;
-	ei_status.block_output	= &ax_block_output;
-	ei_status.get_8390_hdr	= &ax_get_8390_hdr;
-	ei_status.priv = 0;
-
-	dev->netdev_ops		= &ax_netdev_ops;
-	dev->ethtool_ops	= &ax_ethtool_ops;
-
-	ax->msg_enable		= NETIF_MSG_LINK;
-	ax->mii.phy_id_mask	= 0x1f;
-	ax->mii.reg_num_mask	= 0x1f;
-	ax->mii.phy_id		= 0x10;		/* onboard phy */
-	ax->mii.force_media	= 0;
-	ax->mii.full_duplex	= 0;
-	ax->mii.mdio_read	= ax_phy_read;
-	ax->mii.mdio_write	= ax_phy_write;
-	ax->mii.dev		= dev;
+	ei_local->reset_8390 = &ax_reset_8390;
+	ei_local->block_input = &ax_block_input;
+	ei_local->block_output = &ax_block_output;
+	ei_local->get_8390_hdr = &ax_get_8390_hdr;
+	ei_local->priv = 0;
 
-	ax_NS8390_init(dev, 0);
+	dev->netdev_ops = &ax_netdev_ops;
+	dev->ethtool_ops = &ax_ethtool_ops;
 
-	if (first_init)
-		dev_info(&ax->dev->dev, "%dbit, irq %d, %lx, MAC: %pM\n",
-			 ei_status.word16 ? 16:8, dev->irq, dev->base_addr,
-			 dev->dev_addr);
+	ret = ax_mii_init(dev);
+	if (ret)
+		goto out_irq;
+
+	ax_NS8390_init(dev, 0);
 
 	ret = register_netdev(dev);
 	if (ret)
 		goto out_irq;
 
+	netdev_info(dev, "%dbit, irq %d, %lx, MAC: %pM\n",
+		    ei_local->word16 ? 16 : 8, dev->irq, dev->base_addr,
+		    dev->dev_addr);
+
 	return 0;
 
  out_irq:
@@ -802,24 +790,24 @@ static int ax_init_dev(struct net_device *dev, int first_init)
 	return ret;
 }
 
-static int ax_remove(struct platform_device *_dev)
+static int ax_remove(struct platform_device *pdev)
 {
-	struct net_device *dev = platform_get_drvdata(_dev);
-	struct ax_device  *ax;
-
-	ax = to_ax_dev(dev);
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct ei_device *ei_local = netdev_priv(dev);
+	struct ax_device *ax = to_ax_dev(dev);
+	struct resource *mem;
 
 	unregister_netdev(dev);
 	free_irq(dev->irq, dev);
 
-	iounmap(ei_status.mem);
-	release_resource(ax->mem);
-	kfree(ax->mem);
+	iounmap(ei_local->mem);
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, resource_size(mem));
 
 	if (ax->map2) {
 		iounmap(ax->map2);
-		release_resource(ax->mem2);
-		kfree(ax->mem2);
+		mem = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		release_mem_region(mem->start, resource_size(mem));
 	}
 
 	free_netdev(dev);
@@ -827,19 +815,20 @@ static int ax_remove(struct platform_device *_dev)
 	return 0;
 }
 
-/* ax_probe
+/*
+ * ax_probe
  *
  * This is the entry point when the platform device system uses to
- * notify us of a new device to attach to. Allocate memory, find
- * the resources and information passed, and map the necessary registers.
-*/
-
+ * notify us of a new device to attach to. Allocate memory, find the
+ * resources and information passed, and map the necessary registers.
+ */
 static int ax_probe(struct platform_device *pdev)
 {
 	struct net_device *dev;
-	struct ax_device  *ax;
-	struct resource   *res;
-	size_t size;
+	struct ei_device *ei_local;
+	struct ax_device *ax;
+	struct resource *irq, *mem, *mem2;
+	resource_size_t mem_size, mem2_size = 0;
 	int ret = 0;
 
 	dev = ax__alloc_ei_netdev(sizeof(struct ax_device));
@@ -847,120 +836,107 @@ static int ax_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	/* ok, let's setup our device */
+	SET_NETDEV_DEV(dev, &pdev->dev);
+	ei_local = netdev_priv(dev);
 	ax = to_ax_dev(dev);
 
-	memset(ax, 0, sizeof(struct ax_device));
-
-	spin_lock_init(&ax->mii_lock);
-
-	ax->dev = pdev;
 	ax->plat = pdev->dev.platform_data;
 	platform_set_drvdata(pdev, dev);
 
-	ei_status.rxcr_base  = ax->plat->rcr_val;
+	ei_local->rxcr_base = ax->plat->rcr_val;
 
 	/* find the platform resources */
-
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!irq) {
 		dev_err(&pdev->dev, "no IRQ specified\n");
 		ret = -ENXIO;
 		goto exit_mem;
 	}
 
-	dev->irq = res->start;
-	ax->irqflags = res->flags & IRQF_TRIGGER_MASK;
+	dev->irq = irq->start;
+	ax->irqflags = irq->flags & IRQF_TRIGGER_MASK;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
 		dev_err(&pdev->dev, "no MEM specified\n");
 		ret = -ENXIO;
 		goto exit_mem;
 	}
 
-	size = (res->end - res->start) + 1;
-
-	/* setup the register offsets from either the platform data
-	 * or by using the size of the resource provided */
+	mem_size = resource_size(mem);
 
+	/*
+	 * setup the register offsets from either the platform data or
+	 * by using the size of the resource provided
+	 */
 	if (ax->plat->reg_offsets)
-		ei_status.reg_offset = ax->plat->reg_offsets;
+		ei_local->reg_offset = ax->plat->reg_offsets;
 	else {
-		ei_status.reg_offset = ax->reg_offsets;
+		ei_local->reg_offset = ax->reg_offsets;
 		for (ret = 0; ret < 0x18; ret++)
-			ax->reg_offsets[ret] = (size / 0x18) * ret;
+			ax->reg_offsets[ret] = (mem_size / 0x18) * ret;
 	}
 
-	ax->mem = request_mem_region(res->start, size, pdev->name);
-	if (ax->mem == NULL) {
+	if (!request_mem_region(mem->start, mem_size, pdev->name)) {
 		dev_err(&pdev->dev, "cannot reserve registers\n");
- 		ret = -ENXIO;
+		ret = -ENXIO;
 		goto exit_mem;
 	}
 
-	ei_status.mem = ioremap(res->start, size);
-	dev->base_addr = (unsigned long)ei_status.mem;
+	ei_local->mem = ioremap(mem->start, mem_size);
+	dev->base_addr = (unsigned long)ei_local->mem;
 
-	if (ei_status.mem == NULL) {
-		dev_err(&pdev->dev, "Cannot ioremap area (%08llx,%08llx)\n",
-			(unsigned long long)res->start,
-			(unsigned long long)res->end);
+	if (ei_local->mem == NULL) {
+		dev_err(&pdev->dev, "Cannot ioremap area %pR\n", mem);
 
- 		ret = -ENXIO;
+		ret = -ENXIO;
 		goto exit_req;
 	}
 
 	/* look for reset area */
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (res == NULL) {
+	mem2 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!mem2) {
 		if (!ax->plat->reg_offsets) {
 			for (ret = 0; ret < 0x20; ret++)
-				ax->reg_offsets[ret] = (size / 0x20) * ret;
+				ax->reg_offsets[ret] = (mem_size / 0x20) * ret;
 		}
-
-		ax->map2 = NULL;
 	} else {
- 		size = (res->end - res->start) + 1;
+		mem2_size = resource_size(mem2);
 
-		ax->mem2 = request_mem_region(res->start, size, pdev->name);
-		if (ax->mem2 == NULL) {
+		if (!request_mem_region(mem2->start, mem2_size, pdev->name)) {
 			dev_err(&pdev->dev, "cannot reserve registers\n");
 			ret = -ENXIO;
 			goto exit_mem1;
 		}
 
-		ax->map2 = ioremap(res->start, size);
-		if (ax->map2 == NULL) {
+		ax->map2 = ioremap(mem2->start, mem2_size);
+		if (!ax->map2) {
 			dev_err(&pdev->dev, "cannot map reset register\n");
 			ret = -ENXIO;
 			goto exit_mem2;
 		}
 
-		ei_status.reg_offset[0x1f] = ax->map2 - ei_status.mem;
+		ei_local->reg_offset[0x1f] = ax->map2 - ei_local->mem;
 	}
 
 	/* got resources, now initialise and register device */
-
-	ret = ax_init_dev(dev, 1);
+	ret = ax_init_dev(dev);
 	if (!ret)
 		return 0;
 
-	if (ax->map2 == NULL)
+	if (!ax->map2)
 		goto exit_mem1;
 
 	iounmap(ax->map2);
 
  exit_mem2:
-	release_resource(ax->mem2);
-	kfree(ax->mem2);
+	release_mem_region(mem2->start, mem2_size);
 
  exit_mem1:
-	iounmap(ei_status.mem);
+	iounmap(ei_local->mem);
 
  exit_req:
-	release_resource(ax->mem);
-	kfree(ax->mem);
+	release_mem_region(mem->start, mem_size);
 
  exit_mem:
 	free_netdev(dev);
@@ -974,7 +950,7 @@ static int ax_probe(struct platform_device *pdev)
 static int ax_suspend(struct platform_device *dev, pm_message_t state)
 {
 	struct net_device *ndev = platform_get_drvdata(dev);
-	struct ax_device  *ax = to_ax_dev(ndev);
+	struct ax_device *ax = to_ax_dev(ndev);
 
 	ax->resume_open = ax->running;
 
@@ -987,7 +963,7 @@ static int ax_suspend(struct platform_device *dev, pm_message_t state)
 static int ax_resume(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
-	struct ax_device  *ax = to_ax_dev(ndev);
+	struct ax_device *ax = to_ax_dev(ndev);
 
 	ax_initial_setup(ndev, netdev_priv(ndev));
 	ax_NS8390_init(ndev, ax->resume_open);
@@ -1001,7 +977,7 @@ static int ax_resume(struct platform_device *pdev)
 
 #else
 #define ax_suspend NULL
-#define ax_resume  NULL
+#define ax_resume NULL
 #endif
 
 static struct platform_driver axdrv = {
diff --git a/drivers/net/benet/be.h b/drivers/net/benet/be.h
index add0b93350dd..4ac0d72660fe 100644
--- a/drivers/net/benet/be.h
+++ b/drivers/net/benet/be.h
@@ -220,9 +220,11 @@ struct be_rx_obj {
 	struct be_rx_stats stats;
 	u8 rss_id;
 	bool rx_post_starved;	/* Zero rx frags have been posted to BE */
-	u16 last_frag_index;
-	u16 rsvd;
-	u32 cache_line_barrier[15];
+	u32 cache_line_barrier[16];
+};
+
+struct be_drv_stats {
+	u8 be_on_die_temperature;
 };
 
 struct be_vf_cfg {
@@ -234,6 +236,7 @@ struct be_vf_cfg {
 };
 
 #define BE_INVALID_PMAC_ID		0xffffffff
+
 struct be_adapter {
 	struct pci_dev *pdev;
 	struct net_device *netdev;
@@ -269,6 +272,7 @@ struct be_adapter {
 	u32 big_page_size;	/* Compounded page size shared by rx wrbs */
 
 	u8 msix_vec_next_idx;
+	struct be_drv_stats drv_stats;
 
 	struct vlan_group *vlan_grp;
 	u16 vlans_added;
@@ -281,6 +285,7 @@ struct be_adapter {
 	struct be_dma_mem stats_cmd;
 	/* Work queue used to perform periodic tasks like getting statistics */
 	struct delayed_work work;
+	u16 work_counter;
 
 	/* Ethtool knobs and info */
 	bool rx_csum; 		/* BE card must perform rx-checksumming */
@@ -298,7 +303,7 @@ struct be_adapter {
 	u32 rx_fc;		/* Rx flow control */
 	u32 tx_fc;		/* Tx flow control */
 	bool ue_detected;
-	bool stats_ioctl_sent;
+	bool stats_cmd_sent;
 	int link_speed;
 	u8 port_type;
 	u8 transceiver;
@@ -311,6 +316,8 @@ struct be_adapter {
 	struct be_vf_cfg vf_cfg[BE_MAX_VF];
 	u8 is_virtfn;
 	u32 sli_family;
+	u8 hba_port_num;
+	u16 pvid;
 };
 
 #define be_physfn(adapter) (!adapter->is_virtfn)
@@ -450,9 +457,8 @@ static inline void be_vf_eth_addr_generate(struct be_adapter *adapter, u8 *mac)
 	mac[5] = (u8)(addr & 0xFF);
 	mac[4] = (u8)((addr >> 8) & 0xFF);
 	mac[3] = (u8)((addr >> 16) & 0xFF);
-	mac[2] = 0xC9;
-	mac[1] = 0x00;
-	mac[0] = 0x00;
+	/* Use the OUI from the current MAC address */
+	memcpy(mac, adapter->netdev->dev_addr, 3);
 }
 
 extern void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm,
diff --git a/drivers/net/benet/be_cmds.c b/drivers/net/benet/be_cmds.c
index a179cc6d79f2..cc3a235475bc 100644
--- a/drivers/net/benet/be_cmds.c
+++ b/drivers/net/benet/be_cmds.c
@@ -18,11 +18,20 @@
 #include "be.h"
 #include "be_cmds.h"
 
+/* Must be a power of 2 or else MODULO will BUG_ON */
+static int be_get_temp_freq = 32;
+
 static void be_mcc_notify(struct be_adapter *adapter)
 {
 	struct be_queue_info *mccq = &adapter->mcc_obj.q;
 	u32 val = 0;
 
+	if (adapter->eeh_err) {
+		dev_info(&adapter->pdev->dev,
+			"Error in Card Detected! Cannot issue commands\n");
+		return;
+	}
+
 	val |= mccq->id & DB_MCCQ_RING_ID_MASK;
 	val |= 1 << DB_MCCQ_NUM_POSTED_SHIFT;
 
@@ -75,7 +84,7 @@ static int be_mcc_compl_process(struct be_adapter *adapter,
 			be_dws_le_to_cpu(&resp->hw_stats,
 						sizeof(resp->hw_stats));
 			netdev_stats_update(adapter);
-			adapter->stats_ioctl_sent = false;
+			adapter->stats_cmd_sent = false;
 		}
 	} else if ((compl_status != MCC_STATUS_NOT_SUPPORTED) &&
 		   (compl->tag0 != OPCODE_COMMON_NTWK_MAC_QUERY)) {
@@ -102,6 +111,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 =
 			evt->reco_default_priority << VLAN_PRIO_SHIFT;
 	}
@@ -117,6 +127,16 @@ static void be_async_grp5_qos_speed_process(struct be_adapter *adapter,
 	}
 }
 
+/*Grp5 PVID evt*/
+static void be_async_grp5_pvid_state_process(struct be_adapter *adapter,
+		struct be_async_event_grp5_pvid_state *evt)
+{
+	if (evt->enabled)
+		adapter->pvid = evt->tag;
+	else
+		adapter->pvid = 0;
+}
+
 static void be_async_grp5_evt_process(struct be_adapter *adapter,
 		u32 trailer, struct be_mcc_compl *evt)
 {
@@ -134,6 +154,10 @@ static void be_async_grp5_evt_process(struct be_adapter *adapter,
 		be_async_grp5_qos_speed_process(adapter,
 		(struct be_async_event_grp5_qos_link_speed *)evt);
 	break;
+	case ASYNC_EVENT_PVID_STATE:
+		be_async_grp5_pvid_state_process(adapter,
+		(struct be_async_event_grp5_pvid_state *)evt);
+	break;
 	default:
 		dev_warn(&adapter->pdev->dev, "Unknown grp5 event!\n");
 		break;
@@ -216,6 +240,9 @@ static int be_mcc_wait_compl(struct be_adapter *adapter)
 	int i, num, status = 0;
 	struct be_mcc_obj *mcc_obj = &adapter->mcc_obj;
 
+	if (adapter->eeh_err)
+		return -EIO;
+
 	for (i = 0; i < mcc_timeout; i++) {
 		num = be_process_mcc(adapter, &status);
 		if (num)
@@ -245,6 +272,12 @@ static int be_mbox_db_ready_wait(struct be_adapter *adapter, void __iomem *db)
 	int msecs = 0;
 	u32 ready;
 
+	if (adapter->eeh_err) {
+		dev_err(&adapter->pdev->dev,
+			"Error detected in card.Cannot issue commands\n");
+		return -EIO;
+	}
+
 	do {
 		ready = ioread32(db);
 		if (ready == 0xffffffff) {
@@ -598,7 +631,7 @@ int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 
 /* Uses synchronous MCCQ */
 int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
-		u32 if_id, u32 *pmac_id)
+		u32 if_id, u32 *pmac_id, u32 domain)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_pmac_add *req;
@@ -619,6 +652,7 @@ int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_PMAC_ADD, sizeof(*req));
 
+	req->hdr.domain = domain;
 	req->if_id = cpu_to_le32(if_id);
 	memcpy(req->mac_address, mac_addr, ETH_ALEN);
 
@@ -634,7 +668,7 @@ err:
 }
 
 /* Uses synchronous MCCQ */
-int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, u32 pmac_id)
+int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, u32 pmac_id, u32 dom)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_pmac_del *req;
@@ -655,6 +689,7 @@ int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, u32 pmac_id)
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_PMAC_DEL, sizeof(*req));
 
+	req->hdr.domain = dom;
 	req->if_id = cpu_to_le32(if_id);
 	req->pmac_id = cpu_to_le32(pmac_id);
 
@@ -691,7 +726,7 @@ int be_cmd_cq_create(struct be_adapter *adapter,
 
 	req->num_pages =  cpu_to_le16(PAGES_4K_SPANNED(q_mem->va, q_mem->size));
 	if (lancer_chip(adapter)) {
-		req->hdr.version = 1;
+		req->hdr.version = 2;
 		req->page_size = 1; /* 1 for 4K */
 		AMAP_SET_BITS(struct amap_cq_context_lancer, coalescwm, ctxt,
 								coalesce_wm);
@@ -827,6 +862,12 @@ int be_cmd_txq_create(struct be_adapter *adapter,
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH, OPCODE_ETH_TX_CREATE,
 		sizeof(*req));
 
+	if (lancer_chip(adapter)) {
+		req->hdr.version = 1;
+		AMAP_SET_BITS(struct amap_tx_context, if_id, ctxt,
+					adapter->if_handle);
+	}
+
 	req->num_pages = PAGES_4K_SPANNED(q_mem->va, q_mem->size);
 	req->ulp_num = BE_ULP1_NUM;
 	req->type = BE_ETH_TX_RING_TYPE_STANDARD;
@@ -995,7 +1036,7 @@ int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags, u32 en_flags,
 }
 
 /* Uses mbox */
-int be_cmd_if_destroy(struct be_adapter *adapter, u32 interface_id)
+int be_cmd_if_destroy(struct be_adapter *adapter, u32 interface_id, u32 domain)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_if_destroy *req;
@@ -1016,6 +1057,7 @@ int be_cmd_if_destroy(struct be_adapter *adapter, u32 interface_id)
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_INTERFACE_DESTROY, sizeof(*req));
 
+	req->hdr.domain = domain;
 	req->interface_id = cpu_to_le32(interface_id);
 
 	status = be_mbox_notify_wait(adapter);
@@ -1036,6 +1078,9 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 	struct be_sge *sge;
 	int status = 0;
 
+	if (MODULO(adapter->work_counter, be_get_temp_freq) == 0)
+		be_cmd_get_die_temperature(adapter);
+
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
@@ -1056,7 +1101,7 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 	sge->len = cpu_to_le32(nonemb_cmd->size);
 
 	be_mcc_notify(adapter);
-	adapter->stats_ioctl_sent = true;
+	adapter->stats_cmd_sent = true;
 
 err:
 	spin_unlock_bh(&adapter->mcc_lock);
@@ -1103,6 +1148,44 @@ err:
 	return status;
 }
 
+/* Uses synchronous mcc */
+int be_cmd_get_die_temperature(struct be_adapter *adapter)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_get_cntl_addnl_attribs *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = embedded_payload(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+		OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES, sizeof(*req));
+
+	status = be_mcc_notify_wait(adapter);
+	if (!status) {
+		struct be_cmd_resp_get_cntl_addnl_attribs *resp =
+						embedded_payload(wrb);
+		adapter->drv_stats.be_on_die_temperature =
+						resp->on_die_temperature;
+	}
+	/* If IOCTL fails once, do not bother issuing it again */
+	else
+		be_get_temp_freq = 0;
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
 /* Uses Mbox */
 int be_cmd_get_fw_ver(struct be_adapter *adapter, char *fw_ver)
 {
@@ -1868,8 +1951,8 @@ int be_cmd_set_qos(struct be_adapter *adapter, u32 bps, u32 domain)
 			OPCODE_COMMON_SET_QOS, sizeof(*req));
 
 	req->hdr.domain = domain;
-	req->valid_bits = BE_QOS_BITS_NIC;
-	req->max_bps_nic = bps;
+	req->valid_bits = cpu_to_le32(BE_QOS_BITS_NIC);
+	req->max_bps_nic = cpu_to_le32(bps);
 
 	status = be_mcc_notify_wait(adapter);
 
@@ -1877,3 +1960,57 @@ err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
+
+int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_cntl_attribs *req;
+	struct be_cmd_resp_cntl_attribs *resp;
+	struct be_sge *sge;
+	int status;
+	int payload_len = max(sizeof(*req), sizeof(*resp));
+	struct mgmt_controller_attrib *attribs;
+	struct be_dma_mem attribs_cmd;
+
+	memset(&attribs_cmd, 0, sizeof(struct be_dma_mem));
+	attribs_cmd.size = sizeof(struct be_cmd_resp_cntl_attribs);
+	attribs_cmd.va = pci_alloc_consistent(adapter->pdev, attribs_cmd.size,
+						&attribs_cmd.dma);
+	if (!attribs_cmd.va) {
+		dev_err(&adapter->pdev->dev,
+				"Memory allocation failure\n");
+		return -ENOMEM;
+	}
+
+	if (mutex_lock_interruptible(&adapter->mbox_lock))
+		return -1;
+
+	wrb = wrb_from_mbox(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = attribs_cmd.va;
+	sge = nonembedded_sgl(wrb);
+
+	be_wrb_hdr_prepare(wrb, payload_len, false, 1,
+			OPCODE_COMMON_GET_CNTL_ATTRIBUTES);
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			 OPCODE_COMMON_GET_CNTL_ATTRIBUTES, payload_len);
+	sge->pa_hi = cpu_to_le32(upper_32_bits(attribs_cmd.dma));
+	sge->pa_lo = cpu_to_le32(attribs_cmd.dma & 0xFFFFFFFF);
+	sge->len = cpu_to_le32(attribs_cmd.size);
+
+	status = be_mbox_notify_wait(adapter);
+	if (!status) {
+		attribs = (struct mgmt_controller_attrib *)( attribs_cmd.va +
+					sizeof(struct be_cmd_resp_hdr));
+		adapter->hba_port_num = attribs->hba_attribs.phy_port;
+	}
+
+err:
+	mutex_unlock(&adapter->mbox_lock);
+	pci_free_consistent(adapter->pdev, attribs_cmd.size, attribs_cmd.va,
+					attribs_cmd.dma);
+	return status;
+}
diff --git a/drivers/net/benet/be_cmds.h b/drivers/net/benet/be_cmds.h
index 83d15c8a9fa3..b4ac3938b298 100644
--- a/drivers/net/benet/be_cmds.h
+++ b/drivers/net/benet/be_cmds.h
@@ -88,6 +88,7 @@ struct be_mcc_compl {
 #define ASYNC_EVENT_CODE_GRP_5		0x5
 #define ASYNC_EVENT_QOS_SPEED		0x1
 #define ASYNC_EVENT_COS_PRIORITY	0x2
+#define ASYNC_EVENT_PVID_STATE		0x3
 struct be_async_event_trailer {
 	u32 code;
 };
@@ -134,6 +135,18 @@ struct be_async_event_grp5_cos_priority {
 	struct be_async_event_trailer trailer;
 } __packed;
 
+/* When the event code of an async trailer is GRP5 and event type is
+ * PVID state, the mcc_compl must be interpreted as follows
+ */
+struct be_async_event_grp5_pvid_state {
+	u8 enabled;
+	u8 rsvd0;
+	u16 tag;
+	u32 event_tag;
+	u32 rsvd1;
+	struct be_async_event_trailer trailer;
+} __packed;
+
 struct be_mcc_mailbox {
 	struct be_mcc_wrb wrb;
 	struct be_mcc_compl compl;
@@ -156,6 +169,7 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_SET_QOS				28
 #define OPCODE_COMMON_MCC_CREATE_EXT			90
 #define OPCODE_COMMON_SEEPROM_READ			30
+#define OPCODE_COMMON_GET_CNTL_ATTRIBUTES               32
 #define OPCODE_COMMON_NTWK_RX_FILTER    		34
 #define OPCODE_COMMON_GET_FW_VERSION			35
 #define OPCODE_COMMON_SET_FLOW_CONTROL			36
@@ -176,6 +190,7 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_GET_BEACON_STATE			70
 #define OPCODE_COMMON_READ_TRANSRECV_DATA		73
 #define OPCODE_COMMON_GET_PHY_DETAILS			102
+#define OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES	121
 
 #define OPCODE_ETH_RSS_CONFIG				1
 #define OPCODE_ETH_ACPI_CONFIG				2
@@ -415,7 +430,7 @@ struct be_cmd_resp_mcc_create {
 /* Pseudo amap definition in which each bit of the actual structure is defined
  * as a byte: used to calculate offset/shift/mask of each field */
 struct amap_tx_context {
-	u8 rsvd0[16];		/* dword 0 */
+	u8 if_id[16];		/* dword 0 */
 	u8 tx_ring_size[4];	/* dword 0 */
 	u8 rsvd1[26];		/* dword 0 */
 	u8 pci_func_id[8];	/* dword 1 */
@@ -503,7 +518,8 @@ enum be_if_flags {
 	BE_IF_FLAGS_VLAN = 0x100,
 	BE_IF_FLAGS_MCAST_PROMISCUOUS = 0x200,
 	BE_IF_FLAGS_PASS_L2_ERRORS = 0x400,
-	BE_IF_FLAGS_PASS_L3L4_ERRORS = 0x800
+	BE_IF_FLAGS_PASS_L3L4_ERRORS = 0x800,
+	BE_IF_FLAGS_MULTICAST = 0x1000
 };
 
 /* An RX interface is an object with one or more MAC addresses and
@@ -619,7 +635,10 @@ struct be_rxf_stats {
 	u32 rx_drops_invalid_ring;	/* dword 145*/
 	u32 forwarded_packets;	/* dword 146*/
 	u32 rx_drops_mtu;	/* dword 147*/
-	u32 rsvd0[15];
+	u32 rsvd0[7];
+	u32 port0_jabber_events;
+	u32 port1_jabber_events;
+	u32 rsvd1[6];
 };
 
 struct be_erx_stats {
@@ -630,11 +649,16 @@ struct be_erx_stats {
 	u32 debug_pmem_pbuf_dealloc;       /* dword 47*/
 };
 
+struct be_pmem_stats {
+	u32 eth_red_drops;
+	u32 rsvd[4];
+};
+
 struct be_hw_stats {
 	struct be_rxf_stats rxf;
 	u32 rsvd[48];
 	struct be_erx_stats erx;
-	u32 rsvd1[6];
+	struct be_pmem_stats pmem;
 };
 
 struct be_cmd_req_get_stats {
@@ -647,6 +671,20 @@ struct be_cmd_resp_get_stats {
 	struct be_hw_stats hw_stats;
 };
 
+struct be_cmd_req_get_cntl_addnl_attribs {
+	struct be_cmd_req_hdr hdr;
+	u8 rsvd[8];
+};
+
+struct be_cmd_resp_get_cntl_addnl_attribs {
+	struct be_cmd_resp_hdr hdr;
+	u16 ipl_file_number;
+	u8 ipl_file_version;
+	u8 rsvd0;
+	u8 on_die_temperature; /* in degrees centigrade*/
+	u8 rsvd1[3];
+};
+
 struct be_cmd_req_vlan_config {
 	struct be_cmd_req_hdr hdr;
 	u8 interface_id;
@@ -994,17 +1032,29 @@ struct be_cmd_resp_set_qos {
 	u32 rsvd;
 };
 
+/*********************** Controller Attributes ***********************/
+struct be_cmd_req_cntl_attribs {
+	struct be_cmd_req_hdr hdr;
+};
+
+struct be_cmd_resp_cntl_attribs {
+	struct be_cmd_resp_hdr hdr;
+	struct mgmt_controller_attrib attribs;
+};
+
 extern int be_pci_fnum_get(struct be_adapter *adapter);
 extern int be_cmd_POST(struct be_adapter *adapter);
 extern int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 			u8 type, bool permanent, u32 if_handle);
 extern int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
-			u32 if_id, u32 *pmac_id);
-extern int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, u32 pmac_id);
+			u32 if_id, u32 *pmac_id, u32 domain);
+extern int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id,
+			u32 pmac_id, u32 domain);
 extern int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags,
 			u32 en_flags, u8 *mac, bool pmac_invalid,
 			u32 *if_handle, u32 *pmac_id, u32 domain);
-extern int be_cmd_if_destroy(struct be_adapter *adapter, u32 if_handle);
+extern int be_cmd_if_destroy(struct be_adapter *adapter, u32 if_handle,
+			u32 domain);
 extern int be_cmd_eq_create(struct be_adapter *adapter,
 			struct be_queue_info *eq, int eq_delay);
 extern int be_cmd_cq_create(struct be_adapter *adapter,
@@ -1076,4 +1126,6 @@ extern int be_cmd_get_phy_info(struct be_adapter *adapter,
 		struct be_dma_mem *cmd);
 extern int be_cmd_set_qos(struct be_adapter *adapter, u32 bps, u32 domain);
 extern void be_detect_dump_ue(struct be_adapter *adapter);
+extern int be_cmd_get_die_temperature(struct be_adapter *adapter);
+extern int be_cmd_get_cntl_attributes(struct be_adapter *adapter);
 
diff --git a/drivers/net/benet/be_ethtool.c b/drivers/net/benet/be_ethtool.c
index b4be0271efe0..6e5e43380c2a 100644
--- a/drivers/net/benet/be_ethtool.c
+++ b/drivers/net/benet/be_ethtool.c
@@ -26,7 +26,8 @@ struct be_ethtool_stat {
 	int offset;
 };
 
-enum {NETSTAT, PORTSTAT, MISCSTAT, DRVSTAT_TX, DRVSTAT_RX, ERXSTAT};
+enum {NETSTAT, PORTSTAT, MISCSTAT, DRVSTAT_TX, DRVSTAT_RX, ERXSTAT,
+			PMEMSTAT, DRVSTAT};
 #define FIELDINFO(_struct, field) FIELD_SIZEOF(_struct, field), \
 					offsetof(_struct, field)
 #define NETSTAT_INFO(field) 	#field, NETSTAT,\
@@ -43,6 +44,11 @@ enum {NETSTAT, PORTSTAT, MISCSTAT, DRVSTAT_TX, DRVSTAT_RX, ERXSTAT};
 						field)
 #define ERXSTAT_INFO(field) 	#field, ERXSTAT,\
 					FIELDINFO(struct be_erx_stats, field)
+#define PMEMSTAT_INFO(field) 	#field, PMEMSTAT,\
+					FIELDINFO(struct be_pmem_stats, field)
+#define	DRVSTAT_INFO(field)	#field, DRVSTAT,\
+					FIELDINFO(struct be_drv_stats, \
+						field)
 
 static const struct be_ethtool_stat et_stats[] = {
 	{NETSTAT_INFO(rx_packets)},
@@ -99,7 +105,11 @@ static const struct be_ethtool_stat et_stats[] = {
 	{MISCSTAT_INFO(rx_drops_too_many_frags)},
 	{MISCSTAT_INFO(rx_drops_invalid_ring)},
 	{MISCSTAT_INFO(forwarded_packets)},
-	{MISCSTAT_INFO(rx_drops_mtu)}
+	{MISCSTAT_INFO(rx_drops_mtu)},
+	{MISCSTAT_INFO(port0_jabber_events)},
+	{MISCSTAT_INFO(port1_jabber_events)},
+	{PMEMSTAT_INFO(eth_red_drops)},
+	{DRVSTAT_INFO(be_on_die_temperature)}
 };
 #define ETHTOOL_STATS_NUM ARRAY_SIZE(et_stats)
 
@@ -121,7 +131,7 @@ static const char et_self_tests[][ETH_GSTRING_LEN] = {
 	"MAC Loopback test",
 	"PHY Loopback test",
 	"External Loopback test",
-	"DDR DMA test"
+	"DDR DMA test",
 	"Link test"
 };
 
@@ -276,6 +286,12 @@ be_get_ethtool_stats(struct net_device *netdev,
 		case MISCSTAT:
 			p = &hw_stats->rxf;
 			break;
+		case PMEMSTAT:
+			p = &hw_stats->pmem;
+			break;
+		case DRVSTAT:
+			p = &adapter->drv_stats;
+			break;
 		}
 
 		p = (u8 *)p + et_stats[i].offset;
@@ -376,8 +392,9 @@ static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 		}
 
 		phy_cmd.size = sizeof(struct be_cmd_req_get_phy_info);
-		phy_cmd.va = pci_alloc_consistent(adapter->pdev, phy_cmd.size,
-					&phy_cmd.dma);
+		phy_cmd.va = dma_alloc_coherent(&adapter->pdev->dev,
+						phy_cmd.size, &phy_cmd.dma,
+						GFP_KERNEL);
 		if (!phy_cmd.va) {
 			dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
 			return -ENOMEM;
@@ -416,8 +433,8 @@ static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 		adapter->port_type = ecmd->port;
 		adapter->transceiver = ecmd->transceiver;
 		adapter->autoneg = ecmd->autoneg;
-		pci_free_consistent(adapter->pdev, phy_cmd.size,
-					phy_cmd.va, phy_cmd.dma);
+		dma_free_coherent(&adapter->pdev->dev, phy_cmd.size, phy_cmd.va,
+				  phy_cmd.dma);
 	} else {
 		ecmd->speed = adapter->link_speed;
 		ecmd->port = adapter->port_type;
@@ -496,7 +513,7 @@ be_phys_id(struct net_device *netdev, u32 data)
 	int status;
 	u32 cur;
 
-	be_cmd_get_beacon_state(adapter, adapter->port_num, &cur);
+	be_cmd_get_beacon_state(adapter, adapter->hba_port_num, &cur);
 
 	if (cur == BEACON_STATE_ENABLED)
 		return 0;
@@ -504,23 +521,34 @@ be_phys_id(struct net_device *netdev, u32 data)
 	if (data < 2)
 		data = 2;
 
-	status = be_cmd_set_beacon_state(adapter, adapter->port_num, 0, 0,
+	status = be_cmd_set_beacon_state(adapter, adapter->hba_port_num, 0, 0,
 			BEACON_STATE_ENABLED);
 	set_current_state(TASK_INTERRUPTIBLE);
 	schedule_timeout(data*HZ);
 
-	status = be_cmd_set_beacon_state(adapter, adapter->port_num, 0, 0,
+	status = be_cmd_set_beacon_state(adapter, adapter->hba_port_num, 0, 0,
 			BEACON_STATE_DISABLED);
 
 	return status;
 }
 
+static bool
+be_is_wol_supported(struct be_adapter *adapter)
+{
+	if (!be_physfn(adapter))
+		return false;
+	else
+		return true;
+}
+
 static void
 be_get_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 
-	wol->supported = WAKE_MAGIC;
+	if (be_is_wol_supported(adapter))
+		wol->supported = WAKE_MAGIC;
+
 	if (adapter->wol)
 		wol->wolopts = WAKE_MAGIC;
 	else
@@ -536,7 +564,7 @@ be_set_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 	if (wol->wolopts & ~WAKE_MAGIC)
 		return -EINVAL;
 
-	if (wol->wolopts & WAKE_MAGIC)
+	if ((wol->wolopts & WAKE_MAGIC) && be_is_wol_supported(adapter))
 		adapter->wol = true;
 	else
 		adapter->wol = false;
@@ -554,8 +582,8 @@ be_test_ddr_dma(struct be_adapter *adapter)
 	};
 
 	ddrdma_cmd.size = sizeof(struct be_cmd_req_ddrdma_test);
-	ddrdma_cmd.va = pci_alloc_consistent(adapter->pdev, ddrdma_cmd.size,
-					&ddrdma_cmd.dma);
+	ddrdma_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, ddrdma_cmd.size,
+					   &ddrdma_cmd.dma, GFP_KERNEL);
 	if (!ddrdma_cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory allocation failure\n");
 		return -ENOMEM;
@@ -569,20 +597,20 @@ be_test_ddr_dma(struct be_adapter *adapter)
 	}
 
 err:
-	pci_free_consistent(adapter->pdev, ddrdma_cmd.size,
-			ddrdma_cmd.va, ddrdma_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, ddrdma_cmd.size, ddrdma_cmd.va,
+			  ddrdma_cmd.dma);
 	return ret;
 }
 
 static u64 be_loopback_test(struct be_adapter *adapter, u8 loopback_type,
 				u64 *status)
 {
-	be_cmd_set_loopback(adapter, adapter->port_num,
+	be_cmd_set_loopback(adapter, adapter->hba_port_num,
 				loopback_type, 1);
-	*status = be_cmd_loopback_test(adapter, adapter->port_num,
+	*status = be_cmd_loopback_test(adapter, adapter->hba_port_num,
 				loopback_type, 1500,
 				2, 0xabc);
-	be_cmd_set_loopback(adapter, adapter->port_num,
+	be_cmd_set_loopback(adapter, adapter->hba_port_num,
 				BE_NO_LOOPBACK, 1);
 	return *status;
 }
@@ -621,7 +649,8 @@ be_self_test(struct net_device *netdev, struct ethtool_test *test, u64 *data)
 				&qos_link_speed) != 0) {
 		test->flags |= ETH_TEST_FL_FAILED;
 		data[4] = -1;
-	} else if (mac_speed) {
+	} else if (!mac_speed) {
+		test->flags |= ETH_TEST_FL_FAILED;
 		data[4] = 1;
 	}
 }
@@ -662,8 +691,8 @@ be_read_eeprom(struct net_device *netdev, struct ethtool_eeprom *eeprom,
 
 	memset(&eeprom_cmd, 0, sizeof(struct be_dma_mem));
 	eeprom_cmd.size = sizeof(struct be_cmd_req_seeprom_read);
-	eeprom_cmd.va = pci_alloc_consistent(adapter->pdev, eeprom_cmd.size,
-				&eeprom_cmd.dma);
+	eeprom_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, eeprom_cmd.size,
+					   &eeprom_cmd.dma, GFP_KERNEL);
 
 	if (!eeprom_cmd.va) {
 		dev_err(&adapter->pdev->dev,
@@ -677,8 +706,8 @@ be_read_eeprom(struct net_device *netdev, struct ethtool_eeprom *eeprom,
 		resp = (struct be_cmd_resp_seeprom_read *) eeprom_cmd.va;
 		memcpy(data, resp->seeprom_data + eeprom->offset, eeprom->len);
 	}
-	pci_free_consistent(adapter->pdev, eeprom_cmd.size, eeprom_cmd.va,
-			eeprom_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, eeprom_cmd.size, eeprom_cmd.va,
+			  eeprom_cmd.dma);
 
 	return status;
 }
diff --git a/drivers/net/benet/be_hw.h b/drivers/net/benet/be_hw.h
index 4096d9778234..dbe67f353e8f 100644
--- a/drivers/net/benet/be_hw.h
+++ b/drivers/net/benet/be_hw.h
@@ -44,6 +44,18 @@
 #define POST_STAGE_BE_RESET		0x3 /* Host wants to reset chip */
 #define POST_STAGE_ARMFW_RDY		0xc000	/* FW is done with POST */
 
+
+/* Lancer SLIPORT_CONTROL SLIPORT_STATUS registers */
+#define SLIPORT_STATUS_OFFSET		0x404
+#define SLIPORT_CONTROL_OFFSET		0x408
+
+#define SLIPORT_STATUS_ERR_MASK		0x80000000
+#define SLIPORT_STATUS_RN_MASK		0x01000000
+#define SLIPORT_STATUS_RDY_MASK		0x00800000
+
+
+#define SLI_PORT_CONTROL_IP_MASK	0x08000000
+
 /********* Memory BAR register ************/
 #define PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET 	0xfc
 /* Host Interrupt Enable, if set interrupts are enabled although "PCI Interrupt
@@ -327,6 +339,53 @@ struct be_eth_rx_compl {
 	u32 dw[4];
 };
 
+struct mgmt_hba_attribs {
+	u8 flashrom_version_string[32];
+	u8 manufacturer_name[32];
+	u32 supported_modes;
+	u32 rsvd0[3];
+	u8 ncsi_ver_string[12];
+	u32 default_extended_timeout;
+	u8 controller_model_number[32];
+	u8 controller_description[64];
+	u8 controller_serial_number[32];
+	u8 ip_version_string[32];
+	u8 firmware_version_string[32];
+	u8 bios_version_string[32];
+	u8 redboot_version_string[32];
+	u8 driver_version_string[32];
+	u8 fw_on_flash_version_string[32];
+	u32 functionalities_supported;
+	u16 max_cdblength;
+	u8 asic_revision;
+	u8 generational_guid[16];
+	u8 hba_port_count;
+	u16 default_link_down_timeout;
+	u8 iscsi_ver_min_max;
+	u8 multifunction_device;
+	u8 cache_valid;
+	u8 hba_status;
+	u8 max_domains_supported;
+	u8 phy_port;
+	u32 firmware_post_status;
+	u32 hba_mtu[8];
+	u32 rsvd1[4];
+};
+
+struct mgmt_controller_attrib {
+	struct mgmt_hba_attribs hba_attribs;
+	u16 pci_vendor_id;
+	u16 pci_device_id;
+	u16 pci_sub_vendor_id;
+	u16 pci_sub_system_id;
+	u8 pci_bus_number;
+	u8 pci_device_number;
+	u8 pci_function_number;
+	u8 interface_type;
+	u64 unique_identifier;
+	u32 rsvd0[5];
+};
+
 struct controller_id {
 	u32 vendor;
 	u32 device;
diff --git a/drivers/net/benet/be_main.c b/drivers/net/benet/be_main.c
index 28a32a6c8bf1..68f107817326 100644
--- a/drivers/net/benet/be_main.c
+++ b/drivers/net/benet/be_main.c
@@ -125,8 +125,8 @@ static void be_queue_free(struct be_adapter *adapter, struct be_queue_info *q)
 {
 	struct be_dma_mem *mem = &q->dma_mem;
 	if (mem->va)
-		pci_free_consistent(adapter->pdev, mem->size,
-			mem->va, mem->dma);
+		dma_free_coherent(&adapter->pdev->dev, mem->size, mem->va,
+				  mem->dma);
 }
 
 static int be_queue_alloc(struct be_adapter *adapter, struct be_queue_info *q,
@@ -138,7 +138,8 @@ static int be_queue_alloc(struct be_adapter *adapter, struct be_queue_info *q,
 	q->len = len;
 	q->entry_size = entry_size;
 	mem->size = len * entry_size;
-	mem->va = pci_alloc_consistent(adapter->pdev, mem->size, &mem->dma);
+	mem->va = dma_alloc_coherent(&adapter->pdev->dev, mem->size, &mem->dma,
+				     GFP_KERNEL);
 	if (!mem->va)
 		return -1;
 	memset(mem->va, 0, mem->size);
@@ -235,12 +236,13 @@ static int be_mac_addr_set(struct net_device *netdev, void *p)
 	if (!be_physfn(adapter))
 		goto netdev_addr;
 
-	status = be_cmd_pmac_del(adapter, adapter->if_handle, adapter->pmac_id);
+	status = be_cmd_pmac_del(adapter, adapter->if_handle,
+				adapter->pmac_id, 0);
 	if (status)
 		return status;
 
 	status = be_cmd_pmac_add(adapter, (u8 *)addr->sa_data,
-			adapter->if_handle, &adapter->pmac_id);
+				adapter->if_handle, &adapter->pmac_id, 0);
 netdev_addr:
 	if (!status)
 		memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
@@ -484,7 +486,7 @@ static void wrb_fill_hdr(struct be_adapter *adapter, struct be_eth_hdr_wrb *hdr,
 	AMAP_SET_BITS(struct amap_eth_hdr_wrb, len, hdr, len);
 }
 
-static void unmap_tx_frag(struct pci_dev *pdev, struct be_eth_wrb *wrb,
+static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
 		bool unmap_single)
 {
 	dma_addr_t dma;
@@ -494,11 +496,10 @@ static void unmap_tx_frag(struct pci_dev *pdev, struct be_eth_wrb *wrb,
 	dma = (u64)wrb->frag_pa_hi << 32 | (u64)wrb->frag_pa_lo;
 	if (wrb->frag_len) {
 		if (unmap_single)
-			pci_unmap_single(pdev, dma, wrb->frag_len,
-				PCI_DMA_TODEVICE);
+			dma_unmap_single(dev, dma, wrb->frag_len,
+					 DMA_TO_DEVICE);
 		else
-			pci_unmap_page(pdev, dma, wrb->frag_len,
-				PCI_DMA_TODEVICE);
+			dma_unmap_page(dev, dma, wrb->frag_len, DMA_TO_DEVICE);
 	}
 }
 
@@ -507,7 +508,7 @@ static int make_tx_wrbs(struct be_adapter *adapter,
 {
 	dma_addr_t busaddr;
 	int i, copied = 0;
-	struct pci_dev *pdev = adapter->pdev;
+	struct device *dev = &adapter->pdev->dev;
 	struct sk_buff *first_skb = skb;
 	struct be_queue_info *txq = &adapter->tx_obj.q;
 	struct be_eth_wrb *wrb;
@@ -521,9 +522,8 @@ static int make_tx_wrbs(struct be_adapter *adapter,
 
 	if (skb->len > skb->data_len) {
 		int len = skb_headlen(skb);
-		busaddr = pci_map_single(pdev, skb->data, len,
-					 PCI_DMA_TODEVICE);
-		if (pci_dma_mapping_error(pdev, busaddr))
+		busaddr = dma_map_single(dev, skb->data, len, DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, busaddr))
 			goto dma_err;
 		map_single = true;
 		wrb = queue_head_node(txq);
@@ -536,10 +536,9 @@ static int make_tx_wrbs(struct be_adapter *adapter,
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		struct skb_frag_struct *frag =
 			&skb_shinfo(skb)->frags[i];
-		busaddr = pci_map_page(pdev, frag->page,
-				       frag->page_offset,
-				       frag->size, PCI_DMA_TODEVICE);
-		if (pci_dma_mapping_error(pdev, busaddr))
+		busaddr = dma_map_page(dev, frag->page, frag->page_offset,
+				       frag->size, DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, busaddr))
 			goto dma_err;
 		wrb = queue_head_node(txq);
 		wrb_fill(wrb, busaddr, frag->size);
@@ -563,7 +562,7 @@ dma_err:
 	txq->head = map_head;
 	while (copied) {
 		wrb = queue_head_node(txq);
-		unmap_tx_frag(pdev, wrb, map_single);
+		unmap_tx_frag(dev, wrb, map_single);
 		map_single = false;
 		copied -= wrb->frag_len;
 		queue_head_inc(txq);
@@ -743,11 +742,11 @@ static int be_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 	if (adapter->vf_cfg[vf].vf_pmac_id != BE_INVALID_PMAC_ID)
 		status = be_cmd_pmac_del(adapter,
 					adapter->vf_cfg[vf].vf_if_handle,
-					adapter->vf_cfg[vf].vf_pmac_id);
+					adapter->vf_cfg[vf].vf_pmac_id, vf + 1);
 
 	status = be_cmd_pmac_add(adapter, mac,
 				adapter->vf_cfg[vf].vf_if_handle,
-				&adapter->vf_cfg[vf].vf_pmac_id);
+				&adapter->vf_cfg[vf].vf_pmac_id, vf + 1);
 
 	if (status)
 		dev_err(&adapter->pdev->dev, "MAC %pM set on VF %d Failed\n",
@@ -822,7 +821,7 @@ static int be_set_vf_tx_rate(struct net_device *netdev,
 		rate = 10000;
 
 	adapter->vf_cfg[vf].vf_tx_rate = rate;
-	status = be_cmd_set_qos(adapter, rate / 10, vf);
+	status = be_cmd_set_qos(adapter, rate / 10, vf + 1);
 
 	if (status)
 		dev_info(&adapter->pdev->dev,
@@ -866,14 +865,17 @@ static void be_rx_stats_update(struct be_rx_obj *rxo,
 
 static inline bool csum_passed(struct be_eth_rx_compl *rxcp)
 {
-	u8 l4_cksm, ipv6, ipcksm;
+	u8 l4_cksm, ipv6, ipcksm, tcpf, udpf;
 
 	l4_cksm = AMAP_GET_BITS(struct amap_eth_rx_compl, l4_cksm, rxcp);
 	ipcksm = AMAP_GET_BITS(struct amap_eth_rx_compl, ipcksm, rxcp);
 	ipv6 = AMAP_GET_BITS(struct amap_eth_rx_compl, ip_version, rxcp);
+	tcpf = AMAP_GET_BITS(struct amap_eth_rx_compl, tcpf, rxcp);
+	udpf = AMAP_GET_BITS(struct amap_eth_rx_compl, udpf, rxcp);
 
-	/* Ignore ipcksm for ipv6 pkts */
-	return l4_cksm && (ipcksm || ipv6);
+	/* L4 checksum is not reliable for non TCP/UDP packets.
+	 * Also ignore ipcksm for ipv6 pkts */
+	return (tcpf || udpf) && l4_cksm && (ipcksm || ipv6);
 }
 
 static struct be_rx_page_info *
@@ -888,8 +890,9 @@ get_rx_page_info(struct be_adapter *adapter,
 	BUG_ON(!rx_page_info->page);
 
 	if (rx_page_info->last_page_user) {
-		pci_unmap_page(adapter->pdev, dma_unmap_addr(rx_page_info, bus),
-			adapter->big_page_size, PCI_DMA_FROMDEVICE);
+		dma_unmap_page(&adapter->pdev->dev,
+			       dma_unmap_addr(rx_page_info, bus),
+			       adapter->big_page_size, DMA_FROM_DEVICE);
 		rx_page_info->last_page_user = false;
 	}
 
@@ -909,17 +912,11 @@ static void be_rx_compl_discard(struct be_adapter *adapter,
 	rxq_idx = AMAP_GET_BITS(struct amap_eth_rx_compl, fragndx, rxcp);
 	num_rcvd = AMAP_GET_BITS(struct amap_eth_rx_compl, numfrags, rxcp);
 
-	 /* Skip out-of-buffer compl(lancer) or flush compl(BE) */
-	if (likely(rxq_idx != rxo->last_frag_index && num_rcvd != 0)) {
-
-		rxo->last_frag_index = rxq_idx;
-
-		for (i = 0; i < num_rcvd; i++) {
-			page_info = get_rx_page_info(adapter, rxo, rxq_idx);
-			put_page(page_info->page);
-			memset(page_info, 0, sizeof(*page_info));
-			index_inc(&rxq_idx, rxq->len);
-		}
+	for (i = 0; i < num_rcvd; i++) {
+		page_info = get_rx_page_info(adapter, rxo, rxq_idx);
+		put_page(page_info->page);
+		memset(page_info, 0, sizeof(*page_info));
+		index_inc(&rxq_idx, rxq->len);
 	}
 }
 
@@ -1047,6 +1044,9 @@ static void be_rx_compl_process(struct be_adapter *adapter,
 	if ((adapter->function_mode & 0x400) && !vtm)
 		vlanf = 0;
 
+	if ((adapter->pvid == vlanf) && !adapter->vlan_tag[vlanf])
+		vlanf = 0;
+
 	if (unlikely(vlanf)) {
 		if (!adapter->vlan_grp || adapter->vlans_added == 0) {
 			kfree_skb(skb);
@@ -1087,6 +1087,9 @@ static void be_rx_compl_process_gro(struct be_adapter *adapter,
 	if ((adapter->function_mode & 0x400) && !vtm)
 		vlanf = 0;
 
+	if ((adapter->pvid == vlanf) && !adapter->vlan_tag[vlanf])
+		vlanf = 0;
+
 	skb = napi_get_frags(&eq_obj->napi);
 	if (!skb) {
 		be_rx_compl_discard(adapter, rxo, rxcp);
@@ -1163,20 +1166,20 @@ static inline void be_rx_compl_reset(struct be_eth_rx_compl *rxcp)
 	rxcp->dw[offsetof(struct amap_eth_rx_compl, valid) / 32] = 0;
 }
 
-static inline struct page *be_alloc_pages(u32 size)
+static inline struct page *be_alloc_pages(u32 size, gfp_t gfp)
 {
-	gfp_t alloc_flags = GFP_ATOMIC;
 	u32 order = get_order(size);
+
 	if (order > 0)
-		alloc_flags |= __GFP_COMP;
-	return  alloc_pages(alloc_flags, order);
+		gfp |= __GFP_COMP;
+	return  alloc_pages(gfp, order);
 }
 
 /*
  * Allocate a page, split it to fragments of size rx_frag_size and post as
  * receive buffers to BE
  */
-static void be_post_rx_frags(struct be_rx_obj *rxo)
+static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp)
 {
 	struct be_adapter *adapter = rxo->adapter;
 	struct be_rx_page_info *page_info_tbl = rxo->page_info_tbl;
@@ -1190,14 +1193,14 @@ static void be_post_rx_frags(struct be_rx_obj *rxo)
 	page_info = &rxo->page_info_tbl[rxq->head];
 	for (posted = 0; posted < MAX_RX_POST && !page_info->page; posted++) {
 		if (!pagep) {
-			pagep = be_alloc_pages(adapter->big_page_size);
+			pagep = be_alloc_pages(adapter->big_page_size, gfp);
 			if (unlikely(!pagep)) {
 				rxo->stats.rx_post_fail++;
 				break;
 			}
-			page_dmaaddr = pci_map_page(adapter->pdev, pagep, 0,
-						adapter->big_page_size,
-						PCI_DMA_FROMDEVICE);
+			page_dmaaddr = dma_map_page(&adapter->pdev->dev, pagep,
+						    0, adapter->big_page_size,
+						    DMA_FROM_DEVICE);
 			page_info->page_offset = 0;
 		} else {
 			get_page(pagep);
@@ -1270,8 +1273,8 @@ static void be_tx_compl_process(struct be_adapter *adapter, u16 last_index)
 	do {
 		cur_index = txq->tail;
 		wrb = queue_tail_node(txq);
-		unmap_tx_frag(adapter->pdev, wrb, (unmap_skb_hdr &&
-					skb_headlen(sent_skb)));
+		unmap_tx_frag(&adapter->pdev->dev, wrb,
+			      (unmap_skb_hdr && skb_headlen(sent_skb)));
 		unmap_skb_hdr = false;
 
 		num_wrbs++;
@@ -1573,9 +1576,6 @@ static int be_rx_queues_create(struct be_adapter *adapter)
 	adapter->big_page_size = (1 << get_order(rx_frag_size)) * PAGE_SIZE;
 	for_all_rx_queues(adapter, rxo, i) {
 		rxo->adapter = adapter;
-		/* Init last_frag_index so that the frag index in the first
-		 * completion will never match */
-		rxo->last_frag_index = 0xffff;
 		rxo->rx_eq.max_eqd = BE_MAX_EQD;
 		rxo->rx_eq.enable_aic = true;
 
@@ -1716,7 +1716,7 @@ static int be_poll_rx(struct napi_struct *napi, int budget)
 	struct be_queue_info *rx_cq = &rxo->cq;
 	struct be_eth_rx_compl *rxcp;
 	u32 work_done;
-	u16 frag_index, num_rcvd;
+	u16 num_rcvd;
 	u8 err;
 
 	rxo->stats.rx_polls++;
@@ -1726,16 +1726,10 @@ static int be_poll_rx(struct napi_struct *napi, int budget)
 			break;
 
 		err = AMAP_GET_BITS(struct amap_eth_rx_compl, err, rxcp);
-		frag_index = AMAP_GET_BITS(struct amap_eth_rx_compl, fragndx,
-								rxcp);
 		num_rcvd = AMAP_GET_BITS(struct amap_eth_rx_compl, numfrags,
 								rxcp);
-
-		/* Skip out-of-buffer compl(lancer) or flush compl(BE) */
-		if (likely(frag_index != rxo->last_frag_index &&
-				num_rcvd != 0)) {
-			rxo->last_frag_index = frag_index;
-
+		/* Ignore flush completions */
+		if (num_rcvd) {
 			if (do_gro(rxo, rxcp, err))
 				be_rx_compl_process_gro(adapter, rxo, rxcp);
 			else
@@ -1747,7 +1741,7 @@ static int be_poll_rx(struct napi_struct *napi, int budget)
 
 	/* Refill the queue */
 	if (atomic_read(&rxo->q.used) < RX_FRAGS_REFILL_WM)
-		be_post_rx_frags(rxo);
+		be_post_rx_frags(rxo, GFP_ATOMIC);
 
 	/* All consumed */
 	if (work_done < budget) {
@@ -1827,6 +1821,7 @@ void be_detect_dump_ue(struct be_adapter *adapter)
 
 	if (ue_status_lo || ue_status_hi) {
 		adapter->ue_detected = true;
+		adapter->eeh_err = true;
 		dev_err(&adapter->pdev->dev, "UE Detected!!\n");
 	}
 
@@ -1865,10 +1860,14 @@ static void be_worker(struct work_struct *work)
 			struct be_mcc_obj *mcc_obj = &adapter->mcc_obj;
 			be_cq_notify(adapter, mcc_obj->cq.id, false, mcc_compl);
 		}
+
+		if (!adapter->ue_detected && !lancer_chip(adapter))
+			be_detect_dump_ue(adapter);
+
 		goto reschedule;
 	}
 
-	if (!adapter->stats_ioctl_sent)
+	if (!adapter->stats_cmd_sent)
 		be_cmd_get_stats(adapter, &adapter->stats_cmd);
 
 	be_tx_rate_update(adapter);
@@ -1879,7 +1878,7 @@ static void be_worker(struct work_struct *work)
 
 		if (rxo->rx_post_starved) {
 			rxo->rx_post_starved = false;
-			be_post_rx_frags(rxo);
+			be_post_rx_frags(rxo, GFP_KERNEL);
 		}
 	}
 	if (!adapter->ue_detected && !lancer_chip(adapter))
@@ -2083,13 +2082,24 @@ static int be_close(struct net_device *netdev)
 
 	be_async_mcc_disable(adapter);
 
-	netif_stop_queue(netdev);
 	netif_carrier_off(netdev);
 	adapter->link_up = false;
 
 	if (!lancer_chip(adapter))
 		be_intr_set(adapter, false);
 
+	for_all_rx_queues(adapter, rxo, i)
+		napi_disable(&rxo->rx_eq.napi);
+
+	napi_disable(&tx_eq->napi);
+
+	if (lancer_chip(adapter)) {
+		be_cq_notify(adapter, adapter->tx_obj.cq.id, false, 0);
+		be_cq_notify(adapter, adapter->mcc_obj.cq.id, false, 0);
+		for_all_rx_queues(adapter, rxo, i)
+			 be_cq_notify(adapter, rxo->cq.id, false, 0);
+	}
+
 	if (adapter->msix_enabled) {
 		vec = be_msix_vec_get(adapter, tx_eq);
 		synchronize_irq(vec);
@@ -2103,11 +2113,6 @@ static int be_close(struct net_device *netdev)
 	}
 	be_irq_unregister(adapter);
 
-	for_all_rx_queues(adapter, rxo, i)
-		napi_disable(&rxo->rx_eq.napi);
-
-	napi_disable(&tx_eq->napi);
-
 	/* Wait for all pending tx completions to arrive so that
 	 * all tx skbs are freed.
 	 */
@@ -2127,7 +2132,7 @@ static int be_open(struct net_device *netdev)
 	u16 link_speed;
 
 	for_all_rx_queues(adapter, rxo, i) {
-		be_post_rx_frags(rxo);
+		be_post_rx_frags(rxo, GFP_KERNEL);
 		napi_enable(&rxo->rx_eq.napi);
 	}
 	napi_enable(&tx_eq->napi);
@@ -2179,7 +2184,8 @@ static int be_setup_wol(struct be_adapter *adapter, bool enable)
 	memset(mac, 0, ETH_ALEN);
 
 	cmd.size = sizeof(struct be_cmd_req_acpi_wol_magic_config);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_alloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				    GFP_KERNEL);
 	if (cmd.va == NULL)
 		return -1;
 	memset(cmd.va, 0, cmd.size);
@@ -2190,8 +2196,8 @@ static int be_setup_wol(struct be_adapter *adapter, bool enable)
 		if (status) {
 			dev_err(&adapter->pdev->dev,
 				"Could not enable Wake-on-lan\n");
-			pci_free_consistent(adapter->pdev, cmd.size, cmd.va,
-					cmd.dma);
+			dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va,
+					  cmd.dma);
 			return status;
 		}
 		status = be_cmd_enable_magic_wol(adapter,
@@ -2204,7 +2210,7 @@ static int be_setup_wol(struct be_adapter *adapter, bool enable)
 		pci_enable_wake(adapter->pdev, PCI_D3cold, 0);
 	}
 
-	pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va, cmd.dma);
 	return status;
 }
 
@@ -2225,7 +2231,8 @@ static inline int be_vf_eth_addr_config(struct be_adapter *adapter)
 	for (vf = 0; vf < num_vfs; vf++) {
 		status = be_cmd_pmac_add(adapter, mac,
 					adapter->vf_cfg[vf].vf_if_handle,
-					&adapter->vf_cfg[vf].vf_pmac_id);
+					&adapter->vf_cfg[vf].vf_pmac_id,
+					vf + 1);
 		if (status)
 			dev_err(&adapter->pdev->dev,
 				"Mac address add failed for VF %d\n", vf);
@@ -2245,7 +2252,7 @@ static inline void be_vf_eth_addr_rem(struct be_adapter *adapter)
 		if (adapter->vf_cfg[vf].vf_pmac_id != BE_INVALID_PMAC_ID)
 			be_cmd_pmac_del(adapter,
 					adapter->vf_cfg[vf].vf_if_handle,
-					adapter->vf_cfg[vf].vf_pmac_id);
+					adapter->vf_cfg[vf].vf_pmac_id, vf + 1);
 	}
 }
 
@@ -2256,7 +2263,9 @@ static int be_setup(struct be_adapter *adapter)
 	int status;
 	u8 mac[ETH_ALEN];
 
-	cap_flags = en_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST;
+	cap_flags = en_flags = BE_IF_FLAGS_UNTAGGED |
+				BE_IF_FLAGS_BROADCAST |
+				BE_IF_FLAGS_MULTICAST;
 
 	if (be_physfn(adapter)) {
 		cap_flags |= BE_IF_FLAGS_MCAST_PROMISCUOUS |
@@ -2277,22 +2286,26 @@ static int be_setup(struct be_adapter *adapter)
 		goto do_none;
 
 	if (be_physfn(adapter)) {
-		while (vf < num_vfs) {
-			cap_flags = en_flags = BE_IF_FLAGS_UNTAGGED
-					| BE_IF_FLAGS_BROADCAST;
-			status = be_cmd_if_create(adapter, cap_flags, en_flags,
-					mac, true,
+		if (adapter->sriov_enabled) {
+			while (vf < num_vfs) {
+				cap_flags = en_flags = BE_IF_FLAGS_UNTAGGED |
+							BE_IF_FLAGS_BROADCAST;
+				status = be_cmd_if_create(adapter, cap_flags,
+					en_flags, mac, true,
 					&adapter->vf_cfg[vf].vf_if_handle,
 					NULL, vf+1);
-			if (status) {
-				dev_err(&adapter->pdev->dev,
-				"Interface Create failed for VF %d\n", vf);
-				goto if_destroy;
+				if (status) {
+					dev_err(&adapter->pdev->dev,
+					"Interface Create failed for VF %d\n",
+					vf);
+					goto if_destroy;
+				}
+				adapter->vf_cfg[vf].vf_pmac_id =
+							BE_INVALID_PMAC_ID;
+				vf++;
 			}
-			adapter->vf_cfg[vf].vf_pmac_id = BE_INVALID_PMAC_ID;
-			vf++;
 		}
-	} else if (!be_physfn(adapter)) {
+	} else {
 		status = be_cmd_mac_addr_query(adapter, mac,
 			MAC_ADDRESS_TYPE_NETWORK, false, adapter->if_handle);
 		if (!status) {
@@ -2313,44 +2326,46 @@ static int be_setup(struct be_adapter *adapter)
 	if (status != 0)
 		goto rx_qs_destroy;
 
-	if (be_physfn(adapter)) {
-		status = be_vf_eth_addr_config(adapter);
-		if (status)
-			goto mcc_q_destroy;
-	}
-
 	adapter->link_speed = -1;
 
 	return 0;
 
-mcc_q_destroy:
-	if (be_physfn(adapter))
-		be_vf_eth_addr_rem(adapter);
 	be_mcc_queues_destroy(adapter);
 rx_qs_destroy:
 	be_rx_queues_destroy(adapter);
 tx_qs_destroy:
 	be_tx_queues_destroy(adapter);
 if_destroy:
-	for (vf = 0; vf < num_vfs; vf++)
-		if (adapter->vf_cfg[vf].vf_if_handle)
-			be_cmd_if_destroy(adapter,
-					adapter->vf_cfg[vf].vf_if_handle);
-	be_cmd_if_destroy(adapter, adapter->if_handle);
+	if (be_physfn(adapter) && adapter->sriov_enabled)
+		for (vf = 0; vf < num_vfs; vf++)
+			if (adapter->vf_cfg[vf].vf_if_handle)
+				be_cmd_if_destroy(adapter,
+					adapter->vf_cfg[vf].vf_if_handle,
+					vf + 1);
+	be_cmd_if_destroy(adapter, adapter->if_handle, 0);
 do_none:
 	return status;
 }
 
 static int be_clear(struct be_adapter *adapter)
 {
-	if (be_physfn(adapter))
+	int vf;
+
+	if (be_physfn(adapter) && adapter->sriov_enabled)
 		be_vf_eth_addr_rem(adapter);
 
 	be_mcc_queues_destroy(adapter);
 	be_rx_queues_destroy(adapter);
 	be_tx_queues_destroy(adapter);
 
-	be_cmd_if_destroy(adapter, adapter->if_handle);
+	if (be_physfn(adapter) && adapter->sriov_enabled)
+		for (vf = 0; vf < num_vfs; vf++)
+			if (adapter->vf_cfg[vf].vf_if_handle)
+				be_cmd_if_destroy(adapter,
+					adapter->vf_cfg[vf].vf_if_handle,
+					vf + 1);
+
+	be_cmd_if_destroy(adapter, adapter->if_handle,  0);
 
 	/* tell fw we're done with firing cmds */
 	be_cmd_fw_clean(adapter);
@@ -2453,8 +2468,8 @@ static int be_flash_data(struct be_adapter *adapter,
 			continue;
 		if ((pflashcomp[i].optype == IMG_TYPE_REDBOOT) &&
 			(!be_flash_redboot(adapter, fw->data,
-			 pflashcomp[i].offset, pflashcomp[i].size,
-			 filehdr_size)))
+			pflashcomp[i].offset, pflashcomp[i].size, filehdr_size +
+			(num_of_images * sizeof(struct image_hdr)))))
 			continue;
 		p = fw->data;
 		p += filehdr_size + pflashcomp[i].offset
@@ -2528,8 +2543,8 @@ int be_load_fw(struct be_adapter *adapter, u8 *func)
 	dev_info(&adapter->pdev->dev, "Flashing firmware file %s\n", fw_file);
 
 	flash_cmd.size = sizeof(struct be_cmd_write_flashrom) + 32*1024;
-	flash_cmd.va = pci_alloc_consistent(adapter->pdev, flash_cmd.size,
-					&flash_cmd.dma);
+	flash_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, flash_cmd.size,
+					  &flash_cmd.dma, GFP_KERNEL);
 	if (!flash_cmd.va) {
 		status = -ENOMEM;
 		dev_err(&adapter->pdev->dev,
@@ -2558,8 +2573,8 @@ int be_load_fw(struct be_adapter *adapter, u8 *func)
 		status = -1;
 	}
 
-	pci_free_consistent(adapter->pdev, flash_cmd.size, flash_cmd.va,
-				flash_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, flash_cmd.size, flash_cmd.va,
+			  flash_cmd.dma);
 	if (status) {
 		dev_err(&adapter->pdev->dev, "Firmware load error\n");
 		goto fw_exit;
@@ -2700,13 +2715,13 @@ static void be_ctrl_cleanup(struct be_adapter *adapter)
 	be_unmap_pci_bars(adapter);
 
 	if (mem->va)
-		pci_free_consistent(adapter->pdev, mem->size,
-			mem->va, mem->dma);
+		dma_free_coherent(&adapter->pdev->dev, mem->size, mem->va,
+				  mem->dma);
 
 	mem = &adapter->mc_cmd_mem;
 	if (mem->va)
-		pci_free_consistent(adapter->pdev, mem->size,
-			mem->va, mem->dma);
+		dma_free_coherent(&adapter->pdev->dev, mem->size, mem->va,
+				  mem->dma);
 }
 
 static int be_ctrl_init(struct be_adapter *adapter)
@@ -2721,8 +2736,10 @@ static int be_ctrl_init(struct be_adapter *adapter)
 		goto done;
 
 	mbox_mem_alloc->size = sizeof(struct be_mcc_mailbox) + 16;
-	mbox_mem_alloc->va = pci_alloc_consistent(adapter->pdev,
-				mbox_mem_alloc->size, &mbox_mem_alloc->dma);
+	mbox_mem_alloc->va = dma_alloc_coherent(&adapter->pdev->dev,
+						mbox_mem_alloc->size,
+						&mbox_mem_alloc->dma,
+						GFP_KERNEL);
 	if (!mbox_mem_alloc->va) {
 		status = -ENOMEM;
 		goto unmap_pci_bars;
@@ -2734,8 +2751,9 @@ static int be_ctrl_init(struct be_adapter *adapter)
 	memset(mbox_mem_align->va, 0, sizeof(struct be_mcc_mailbox));
 
 	mc_cmd_mem->size = sizeof(struct be_cmd_req_mcast_mac_config);
-	mc_cmd_mem->va = pci_alloc_consistent(adapter->pdev, mc_cmd_mem->size,
-			&mc_cmd_mem->dma);
+	mc_cmd_mem->va = dma_alloc_coherent(&adapter->pdev->dev,
+					    mc_cmd_mem->size, &mc_cmd_mem->dma,
+					    GFP_KERNEL);
 	if (mc_cmd_mem->va == NULL) {
 		status = -ENOMEM;
 		goto free_mbox;
@@ -2751,8 +2769,8 @@ static int be_ctrl_init(struct be_adapter *adapter)
 	return 0;
 
 free_mbox:
-	pci_free_consistent(adapter->pdev, mbox_mem_alloc->size,
-		mbox_mem_alloc->va, mbox_mem_alloc->dma);
+	dma_free_coherent(&adapter->pdev->dev, mbox_mem_alloc->size,
+			  mbox_mem_alloc->va, mbox_mem_alloc->dma);
 
 unmap_pci_bars:
 	be_unmap_pci_bars(adapter);
@@ -2766,8 +2784,8 @@ static void be_stats_cleanup(struct be_adapter *adapter)
 	struct be_dma_mem *cmd = &adapter->stats_cmd;
 
 	if (cmd->va)
-		pci_free_consistent(adapter->pdev, cmd->size,
-			cmd->va, cmd->dma);
+		dma_free_coherent(&adapter->pdev->dev, cmd->size,
+				  cmd->va, cmd->dma);
 }
 
 static int be_stats_init(struct be_adapter *adapter)
@@ -2775,7 +2793,8 @@ static int be_stats_init(struct be_adapter *adapter)
 	struct be_dma_mem *cmd = &adapter->stats_cmd;
 
 	cmd->size = sizeof(struct be_cmd_req_get_stats);
-	cmd->va = pci_alloc_consistent(adapter->pdev, cmd->size, &cmd->dma);
+	cmd->va = dma_alloc_coherent(&adapter->pdev->dev, cmd->size, &cmd->dma,
+				     GFP_KERNEL);
 	if (cmd->va == NULL)
 		return -1;
 	memset(cmd->va, 0, cmd->size);
@@ -2845,6 +2864,10 @@ static int be_get_config(struct be_adapter *adapter)
 	else
 		adapter->max_vlans = BE_NUM_VLANS_SUPPORTED;
 
+	status = be_cmd_get_cntl_attributes(adapter);
+	if (status)
+		return status;
+
 	return 0;
 }
 
@@ -2886,6 +2909,54 @@ static int be_dev_family_check(struct be_adapter *adapter)
 	return 0;
 }
 
+static int lancer_wait_ready(struct be_adapter *adapter)
+{
+#define SLIPORT_READY_TIMEOUT 500
+	u32 sliport_status;
+	int status = 0, i;
+
+	for (i = 0; i < SLIPORT_READY_TIMEOUT; i++) {
+		sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
+		if (sliport_status & SLIPORT_STATUS_RDY_MASK)
+			break;
+
+		msleep(20);
+	}
+
+	if (i == SLIPORT_READY_TIMEOUT)
+		status = -1;
+
+	return status;
+}
+
+static int lancer_test_and_set_rdy_state(struct be_adapter *adapter)
+{
+	int status;
+	u32 sliport_status, err, reset_needed;
+	status = lancer_wait_ready(adapter);
+	if (!status) {
+		sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
+		err = sliport_status & SLIPORT_STATUS_ERR_MASK;
+		reset_needed = sliport_status & SLIPORT_STATUS_RN_MASK;
+		if (err && reset_needed) {
+			iowrite32(SLI_PORT_CONTROL_IP_MASK,
+					adapter->db + SLIPORT_CONTROL_OFFSET);
+
+			/* check adapter has corrected the error */
+			status = lancer_wait_ready(adapter);
+			sliport_status = ioread32(adapter->db +
+							SLIPORT_STATUS_OFFSET);
+			sliport_status &= (SLIPORT_STATUS_ERR_MASK |
+						SLIPORT_STATUS_RN_MASK);
+			if (status || sliport_status)
+				status = -1;
+		} else if (err || reset_needed) {
+			status = -1;
+		}
+	}
+	return status;
+}
+
 static int __devinit be_probe(struct pci_dev *pdev,
 			const struct pci_device_id *pdev_id)
 {
@@ -2918,11 +2989,11 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	adapter->netdev = netdev;
 	SET_NETDEV_DEV(netdev, &pdev->dev);
 
-	status = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
+	status = dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
 	if (!status) {
 		netdev->features |= NETIF_F_HIGHDMA;
 	} else {
-		status = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		status = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (status) {
 			dev_err(&pdev->dev, "Could not set PCI DMA Mask\n");
 			goto free_netdev;
@@ -2935,6 +3006,14 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	if (status)
 		goto free_netdev;
 
+	if (lancer_chip(adapter)) {
+		status = lancer_test_and_set_rdy_state(adapter);
+		if (status) {
+			dev_err(&pdev->dev, "Adapter in non recoverable error\n");
+			goto free_netdev;
+		}
+	}
+
 	/* sync up with fw's ready state */
 	if (be_physfn(adapter)) {
 		status = be_cmd_POST(adapter);
@@ -2947,11 +3026,9 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	if (status)
 		goto ctrl_clean;
 
-	if (be_physfn(adapter)) {
-		status = be_cmd_reset_function(adapter);
-		if (status)
-			goto ctrl_clean;
-	}
+	status = be_cmd_reset_function(adapter);
+	if (status)
+		goto ctrl_clean;
 
 	status = be_stats_init(adapter);
 	if (status)
@@ -2975,10 +3052,18 @@ static int __devinit be_probe(struct pci_dev *pdev,
 		goto unsetup;
 	netif_carrier_off(netdev);
 
+	if (be_physfn(adapter) && adapter->sriov_enabled) {
+		status = be_vf_eth_addr_config(adapter);
+		if (status)
+			goto unreg_netdev;
+	}
+
 	dev_info(&pdev->dev, "%s port %d\n", nic_name(pdev), adapter->port_num);
 	schedule_delayed_work(&adapter->work, msecs_to_jiffies(100));
 	return 0;
 
+unreg_netdev:
+	unregister_netdev(netdev);
 unsetup:
 	be_clear(adapter);
 msix_disable:
@@ -3005,6 +3090,7 @@ static int be_suspend(struct pci_dev *pdev, pm_message_t state)
 	struct be_adapter *adapter = pci_get_drvdata(pdev);
 	struct net_device *netdev =  adapter->netdev;
 
+	cancel_delayed_work_sync(&adapter->work);
 	if (adapter->wol)
 		be_setup_wol(adapter, true);
 
@@ -3017,6 +3103,7 @@ static int be_suspend(struct pci_dev *pdev, pm_message_t state)
 	be_cmd_get_flow_control(adapter, &adapter->tx_fc, &adapter->rx_fc);
 	be_clear(adapter);
 
+	be_msix_disable(adapter);
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 	pci_set_power_state(pdev, pci_choose_state(pdev, state));
@@ -3038,6 +3125,7 @@ static int be_resume(struct pci_dev *pdev)
 	pci_set_power_state(pdev, 0);
 	pci_restore_state(pdev);
 
+	be_msix_enable(adapter);
 	/* tell fw we're ready to fire cmds */
 	status = be_cmd_fw_init(adapter);
 	if (status)
@@ -3053,6 +3141,8 @@ static int be_resume(struct pci_dev *pdev)
 
 	if (adapter->wol)
 		be_setup_wol(adapter, false);
+
+	schedule_delayed_work(&adapter->work, msecs_to_jiffies(100));
 	return 0;
 }
 
@@ -3064,6 +3154,9 @@ static void be_shutdown(struct pci_dev *pdev)
 	struct be_adapter *adapter = pci_get_drvdata(pdev);
 	struct net_device *netdev =  adapter->netdev;
 
+	if (netif_running(netdev))
+		cancel_delayed_work_sync(&adapter->work);
+
 	netif_device_detach(netdev);
 
 	be_cmd_reset_function(adapter);
diff --git a/drivers/net/bna/bnad.c b/drivers/net/bna/bnad.c
index fad912656fe4..9f356d5d0f33 100644
--- a/drivers/net/bna/bnad.c
+++ b/drivers/net/bna/bnad.c
@@ -126,22 +126,22 @@ bnad_free_all_txbufs(struct bnad *bnad,
 		}
 		unmap_array[unmap_cons].skb = NULL;
 
-		pci_unmap_single(bnad->pcidev,
-				 pci_unmap_addr(&unmap_array[unmap_cons],
+		dma_unmap_single(&bnad->pcidev->dev,
+				 dma_unmap_addr(&unmap_array[unmap_cons],
 						dma_addr), skb_headlen(skb),
-						PCI_DMA_TODEVICE);
+						DMA_TO_DEVICE);
 
-		pci_unmap_addr_set(&unmap_array[unmap_cons], dma_addr, 0);
+		dma_unmap_addr_set(&unmap_array[unmap_cons], dma_addr, 0);
 		if (++unmap_cons >= unmap_q->q_depth)
 			break;
 
 		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
-			pci_unmap_page(bnad->pcidev,
-				       pci_unmap_addr(&unmap_array[unmap_cons],
+			dma_unmap_page(&bnad->pcidev->dev,
+				       dma_unmap_addr(&unmap_array[unmap_cons],
 						      dma_addr),
 				       skb_shinfo(skb)->frags[i].size,
-				       PCI_DMA_TODEVICE);
-			pci_unmap_addr_set(&unmap_array[unmap_cons], dma_addr,
+				       DMA_TO_DEVICE);
+			dma_unmap_addr_set(&unmap_array[unmap_cons], dma_addr,
 					   0);
 			if (++unmap_cons >= unmap_q->q_depth)
 				break;
@@ -199,23 +199,23 @@ bnad_free_txbufs(struct bnad *bnad,
 		sent_bytes += skb->len;
 		wis -= BNA_TXQ_WI_NEEDED(1 + skb_shinfo(skb)->nr_frags);
 
-		pci_unmap_single(bnad->pcidev,
-				 pci_unmap_addr(&unmap_array[unmap_cons],
+		dma_unmap_single(&bnad->pcidev->dev,
+				 dma_unmap_addr(&unmap_array[unmap_cons],
 						dma_addr), skb_headlen(skb),
-				 PCI_DMA_TODEVICE);
-		pci_unmap_addr_set(&unmap_array[unmap_cons], dma_addr, 0);
+				 DMA_TO_DEVICE);
+		dma_unmap_addr_set(&unmap_array[unmap_cons], dma_addr, 0);
 		BNA_QE_INDX_ADD(unmap_cons, 1, unmap_q->q_depth);
 
 		prefetch(&unmap_array[unmap_cons + 1]);
 		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 			prefetch(&unmap_array[unmap_cons + 1]);
 
-			pci_unmap_page(bnad->pcidev,
-				       pci_unmap_addr(&unmap_array[unmap_cons],
+			dma_unmap_page(&bnad->pcidev->dev,
+				       dma_unmap_addr(&unmap_array[unmap_cons],
 						      dma_addr),
 				       skb_shinfo(skb)->frags[i].size,
-				       PCI_DMA_TODEVICE);
-			pci_unmap_addr_set(&unmap_array[unmap_cons], dma_addr,
+				       DMA_TO_DEVICE);
+			dma_unmap_addr_set(&unmap_array[unmap_cons], dma_addr,
 					   0);
 			BNA_QE_INDX_ADD(unmap_cons, 1, unmap_q->q_depth);
 		}
@@ -340,19 +340,22 @@ static void
 bnad_free_all_rxbufs(struct bnad *bnad, struct bna_rcb *rcb)
 {
 	struct bnad_unmap_q *unmap_q;
+	struct bnad_skb_unmap *unmap_array;
 	struct sk_buff *skb;
 	int unmap_cons;
 
 	unmap_q = rcb->unmap_q;
+	unmap_array = unmap_q->unmap_array;
 	for (unmap_cons = 0; unmap_cons < unmap_q->q_depth; unmap_cons++) {
-		skb = unmap_q->unmap_array[unmap_cons].skb;
+		skb = unmap_array[unmap_cons].skb;
 		if (!skb)
 			continue;
-		unmap_q->unmap_array[unmap_cons].skb = NULL;
-		pci_unmap_single(bnad->pcidev, pci_unmap_addr(&unmap_q->
-					unmap_array[unmap_cons],
-					dma_addr), rcb->rxq->buffer_size,
-					PCI_DMA_FROMDEVICE);
+		unmap_array[unmap_cons].skb = NULL;
+		dma_unmap_single(&bnad->pcidev->dev,
+				 dma_unmap_addr(&unmap_array[unmap_cons],
+						dma_addr),
+				 rcb->rxq->buffer_size,
+				 DMA_FROM_DEVICE);
 		dev_kfree_skb(skb);
 	}
 	bnad_reset_rcb(bnad, rcb);
@@ -391,9 +394,10 @@ bnad_alloc_n_post_rxbufs(struct bnad *bnad, struct bna_rcb *rcb)
 		skb->dev = bnad->netdev;
 		skb_reserve(skb, NET_IP_ALIGN);
 		unmap_array[unmap_prod].skb = skb;
-		dma_addr = pci_map_single(bnad->pcidev, skb->data,
-			rcb->rxq->buffer_size, PCI_DMA_FROMDEVICE);
-		pci_unmap_addr_set(&unmap_array[unmap_prod], dma_addr,
+		dma_addr = dma_map_single(&bnad->pcidev->dev, skb->data,
+					  rcb->rxq->buffer_size,
+					  DMA_FROM_DEVICE);
+		dma_unmap_addr_set(&unmap_array[unmap_prod], dma_addr,
 				   dma_addr);
 		BNA_SET_DMA_ADDR(dma_addr, &rxent->host_addr);
 		BNA_QE_INDX_ADD(unmap_prod, 1, unmap_q->q_depth);
@@ -434,8 +438,9 @@ bnad_poll_cq(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 	struct bna_rcb *rcb = NULL;
 	unsigned int wi_range, packets = 0, wis = 0;
 	struct bnad_unmap_q *unmap_q;
+	struct bnad_skb_unmap *unmap_array;
 	struct sk_buff *skb;
-	u32 flags;
+	u32 flags, unmap_cons;
 	u32 qid0 = ccb->rcb[0]->rxq->rxq_id;
 	struct bna_pkt_rate *pkt_rt = &ccb->pkt_rate;
 
@@ -456,17 +461,17 @@ bnad_poll_cq(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 			rcb = ccb->rcb[1];
 
 		unmap_q = rcb->unmap_q;
+		unmap_array = unmap_q->unmap_array;
+		unmap_cons = unmap_q->consumer_index;
 
-		skb = unmap_q->unmap_array[unmap_q->consumer_index].skb;
+		skb = unmap_array[unmap_cons].skb;
 		BUG_ON(!(skb));
-		unmap_q->unmap_array[unmap_q->consumer_index].skb = NULL;
-		pci_unmap_single(bnad->pcidev,
-				 pci_unmap_addr(&unmap_q->
-						unmap_array[unmap_q->
-							    consumer_index],
+		unmap_array[unmap_cons].skb = NULL;
+		dma_unmap_single(&bnad->pcidev->dev,
+				 dma_unmap_addr(&unmap_array[unmap_cons],
 						dma_addr),
-						rcb->rxq->buffer_size,
-						PCI_DMA_FROMDEVICE);
+				 rcb->rxq->buffer_size,
+				 DMA_FROM_DEVICE);
 		BNA_QE_INDX_ADD(unmap_q->consumer_index, 1, unmap_q->q_depth);
 
 		/* Should be more efficient ? Performance ? */
@@ -1015,9 +1020,9 @@ bnad_mem_free(struct bnad *bnad,
 			if (mem_info->mem_type == BNA_MEM_T_DMA) {
 				BNA_GET_DMA_ADDR(&(mem_info->mdl[i].dma),
 						dma_pa);
-				pci_free_consistent(bnad->pcidev,
-						mem_info->mdl[i].len,
-						mem_info->mdl[i].kva, dma_pa);
+				dma_free_coherent(&bnad->pcidev->dev,
+						  mem_info->mdl[i].len,
+						  mem_info->mdl[i].kva, dma_pa);
 			} else
 				kfree(mem_info->mdl[i].kva);
 		}
@@ -1047,8 +1052,9 @@ bnad_mem_alloc(struct bnad *bnad,
 		for (i = 0; i < mem_info->num; i++) {
 			mem_info->mdl[i].len = mem_info->len;
 			mem_info->mdl[i].kva =
-				pci_alloc_consistent(bnad->pcidev,
-						mem_info->len, &dma_pa);
+				dma_alloc_coherent(&bnad->pcidev->dev,
+						mem_info->len, &dma_pa,
+						GFP_KERNEL);
 
 			if (mem_info->mdl[i].kva == NULL)
 				goto err_return;
@@ -2600,9 +2606,9 @@ bnad_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 	unmap_q->unmap_array[unmap_prod].skb = skb;
 	BUG_ON(!(skb_headlen(skb) <= BFI_TX_MAX_DATA_PER_VECTOR));
 	txqent->vector[vect_id].length = htons(skb_headlen(skb));
-	dma_addr = pci_map_single(bnad->pcidev, skb->data, skb_headlen(skb),
-		PCI_DMA_TODEVICE);
-	pci_unmap_addr_set(&unmap_q->unmap_array[unmap_prod], dma_addr,
+	dma_addr = dma_map_single(&bnad->pcidev->dev, skb->data,
+				  skb_headlen(skb), DMA_TO_DEVICE);
+	dma_unmap_addr_set(&unmap_q->unmap_array[unmap_prod], dma_addr,
 			   dma_addr);
 
 	BNA_SET_DMA_ADDR(dma_addr, &txqent->vector[vect_id].host_addr);
@@ -2630,11 +2636,9 @@ bnad_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 
 		BUG_ON(!(size <= BFI_TX_MAX_DATA_PER_VECTOR));
 		txqent->vector[vect_id].length = htons(size);
-		dma_addr =
-			pci_map_page(bnad->pcidev, frag->page,
-				     frag->page_offset, size,
-				     PCI_DMA_TODEVICE);
-		pci_unmap_addr_set(&unmap_q->unmap_array[unmap_prod], dma_addr,
+		dma_addr = dma_map_page(&bnad->pcidev->dev, frag->page,
+					frag->page_offset, size, DMA_TO_DEVICE);
+		dma_unmap_addr_set(&unmap_q->unmap_array[unmap_prod], dma_addr,
 				   dma_addr);
 		BNA_SET_DMA_ADDR(dma_addr, &txqent->vector[vect_id].host_addr);
 		BNA_QE_INDX_ADD(unmap_prod, 1, unmap_q->q_depth);
@@ -3022,14 +3026,14 @@ bnad_pci_init(struct bnad *bnad,
 	err = pci_request_regions(pdev, BNAD_NAME);
 	if (err)
 		goto disable_device;
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) &&
-	    !pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64))) {
+	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(64)) &&
+	    !dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64))) {
 		*using_dac = 1;
 	} else {
-		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (err) {
-			err = pci_set_consistent_dma_mask(pdev,
-						DMA_BIT_MASK(32));
+			err = dma_set_coherent_mask(&pdev->dev,
+						    DMA_BIT_MASK(32));
 			if (err)
 				goto release_regions;
 		}
diff --git a/drivers/net/bna/bnad.h b/drivers/net/bna/bnad.h
index 8b1d51557def..a89117fa4970 100644
--- a/drivers/net/bna/bnad.h
+++ b/drivers/net/bna/bnad.h
@@ -181,7 +181,7 @@ struct bnad_rx_info {
 /* Unmap queues for Tx / Rx cleanup */
 struct bnad_skb_unmap {
 	struct sk_buff		*skb;
-	DECLARE_PCI_UNMAP_ADDR(dma_addr)
+	DEFINE_DMA_UNMAP_ADDR(dma_addr);
 };
 
 struct bnad_unmap_q {
diff --git a/drivers/net/bnx2.c b/drivers/net/bnx2.c
index 0ba59d5aeb7f..2a961b7f7e17 100644
--- a/drivers/net/bnx2.c
+++ b/drivers/net/bnx2.c
@@ -435,7 +435,8 @@ bnx2_cnic_stop(struct bnx2 *bp)
 	struct cnic_ctl_info info;
 
 	mutex_lock(&bp->cnic_lock);
-	c_ops = bp->cnic_ops;
+	c_ops = rcu_dereference_protected(bp->cnic_ops,
+					  lockdep_is_held(&bp->cnic_lock));
 	if (c_ops) {
 		info.cmd = CNIC_CTL_STOP_CMD;
 		c_ops->cnic_ctl(bp->cnic_data, &info);
@@ -450,7 +451,8 @@ bnx2_cnic_start(struct bnx2 *bp)
 	struct cnic_ctl_info info;
 
 	mutex_lock(&bp->cnic_lock);
-	c_ops = bp->cnic_ops;
+	c_ops = rcu_dereference_protected(bp->cnic_ops,
+					  lockdep_is_held(&bp->cnic_lock));
 	if (c_ops) {
 		if (!(bp->flags & BNX2_FLAG_USING_MSIX)) {
 			struct bnx2_napi *bnapi = &bp->bnx2_napi[0];
@@ -8315,7 +8317,7 @@ static const struct net_device_ops bnx2_netdev_ops = {
 #endif
 };
 
-static void inline vlan_features_add(struct net_device *dev, unsigned long flags)
+static void inline vlan_features_add(struct net_device *dev, u32 flags)
 {
 	dev->vlan_features |= flags;
 }
diff --git a/drivers/net/bnx2.h b/drivers/net/bnx2.h
index f459fb2f9add..7a5e88f831f6 100644
--- a/drivers/net/bnx2.h
+++ b/drivers/net/bnx2.h
@@ -6207,6 +6207,8 @@ struct l2_fhdr {
 
 #define BNX2_CP_SCRATCH					0x001a0000
 
+#define BNX2_FW_MAX_ISCSI_CONN				 0x001a0080
+
 
 /*
  *  mcp_reg definition
@@ -6759,7 +6761,7 @@ struct bnx2 {
 	u32		tx_wake_thresh;
 
 #ifdef BCM_CNIC
-	struct cnic_ops		*cnic_ops;
+	struct cnic_ops	__rcu	*cnic_ops;
 	void			*cnic_data;
 #endif
 
diff --git a/drivers/net/bnx2x/bnx2x.h b/drivers/net/bnx2x/bnx2x.h
index 653c62475cb6..b7ff87b35fbb 100644
--- a/drivers/net/bnx2x/bnx2x.h
+++ b/drivers/net/bnx2x/bnx2x.h
@@ -22,8 +22,8 @@
  * (you will need to reboot afterwards) */
 /* #define BNX2X_STOP_ON_ERROR */
 
-#define DRV_MODULE_VERSION      "1.62.00-5"
-#define DRV_MODULE_RELDATE      "2011/01/30"
+#define DRV_MODULE_VERSION      "1.62.11-0"
+#define DRV_MODULE_RELDATE      "2011/01/31"
 #define BNX2X_BC_VER            0x040200
 
 #define BNX2X_MULTI_QUEUE
@@ -31,7 +31,7 @@
 #define BNX2X_NEW_NAPI
 
 #if defined(CONFIG_DCB)
-#define BCM_DCB
+#define BCM_DCBNL
 #endif
 #if defined(CONFIG_CNIC) || defined(CONFIG_CNIC_MODULE)
 #define BCM_CNIC 1
@@ -129,6 +129,7 @@ void bnx2x_panic_dump(struct bnx2x *bp);
 #endif
 
 #define bnx2x_mc_addr(ha)      ((ha)->addr)
+#define bnx2x_uc_addr(ha)      ((ha)->addr)
 
 #define U64_LO(x)			(u32)(((u64)(x)) & 0xffffffff)
 #define U64_HI(x)			(u32)(((u64)(x)) >> 32)
@@ -341,6 +342,8 @@ struct bnx2x_fastpath {
 	/* chip independed shortcut into rx_prods_offset memory */
 	u32			ustorm_rx_prods_offset;
 
+	u32			rx_buf_size;
+
 	dma_addr_t		status_blk_mapping;
 
 	struct sw_tx_bd		*tx_buf_ring;
@@ -428,6 +431,10 @@ struct bnx2x_fastpath {
 };
 
 #define bnx2x_fp(bp, nr, var)		(bp->fp[nr].var)
+
+/* Use 2500 as a mini-jumbo MTU for FCoE */
+#define BNX2X_FCOE_MINI_JUMBO_MTU	2500
+
 #ifdef BCM_CNIC
 /* FCoE L2 `fastpath' is right after the eth entries */
 #define FCOE_IDX			BNX2X_NUM_ETH_QUEUES(bp)
@@ -810,6 +817,7 @@ struct bnx2x_slowpath {
 	struct eth_stats_query		fw_stats;
 	struct mac_configuration_cmd	mac_config;
 	struct mac_configuration_cmd	mcast_config;
+	struct mac_configuration_cmd	uc_mac_config;
 	struct client_init_ramrod_data	client_init_data;
 
 	/* used by dmae command executer */
@@ -911,7 +919,6 @@ struct bnx2x {
 	int			tx_ring_size;
 
 	u32			rx_csum;
-	u32			rx_buf_size;
 /* L2 header size + 2*VLANs (8 bytes) + LLC SNAP (8 bytes) */
 #define ETH_OVREHEAD		(ETH_HLEN + 8 + 8)
 #define ETH_MIN_PACKET_SIZE		60
@@ -939,7 +946,7 @@ struct bnx2x {
 	struct eth_spe		*spq_prod_bd;
 	struct eth_spe		*spq_last_bd;
 	__le16			*dsb_sp_prod;
-	atomic_t		spq_left; /* serialize spq */
+	atomic_t		cq_spq_left; /* ETH_XXX ramrods credit */
 	/* used to synchronize spq accesses */
 	spinlock_t		spq_lock;
 
@@ -949,6 +956,7 @@ struct bnx2x {
 	u16			eq_prod;
 	u16			eq_cons;
 	__le16			*eq_cons_sb;
+	atomic_t		eq_spq_left; /* COMMON_XXX ramrods credit */
 
 	/* Flags for marking that there is a STAT_QUERY or
 	   SET_MAC ramrod pending */
@@ -976,8 +984,12 @@ struct bnx2x {
 #define MF_FUNC_DIS			0x1000
 #define FCOE_MACS_SET			0x2000
 #define NO_FCOE_FLAG			0x4000
+#define NO_ISCSI_OOO_FLAG		0x8000
+#define NO_ISCSI_FLAG			0x10000
 
 #define NO_FCOE(bp)		((bp)->flags & NO_FCOE_FLAG)
+#define NO_ISCSI(bp)		((bp)->flags & NO_ISCSI_FLAG)
+#define NO_ISCSI_OOO(bp)	((bp)->flags & NO_ISCSI_OOO_FLAG)
 
 	int			pf_num;	/* absolute PF number */
 	int			pfid;	/* per-path PF number */
@@ -1064,6 +1076,7 @@ struct bnx2x {
 	int			num_queues;
 	int			disable_tpa;
 	int			int_mode;
+	u32			*rx_indir_table;
 
 	struct tstorm_eth_mac_filter_config	mac_filters;
 #define BNX2X_ACCEPT_NONE		0x0000
@@ -1110,7 +1123,7 @@ struct bnx2x {
 #define BNX2X_CNIC_FLAG_MAC_SET		1
 	void			*t2;
 	dma_addr_t		t2_mapping;
-	struct cnic_ops		*cnic_ops;
+	struct cnic_ops	__rcu	*cnic_ops;
 	void			*cnic_data;
 	u32			cnic_tag;
 	struct cnic_eth_dev	cnic_eth_dev;
@@ -1125,13 +1138,12 @@ struct bnx2x {
 	u16			cnic_kwq_pending;
 	u16			cnic_spq_pending;
 	struct mutex		cnic_mutex;
-	u8			iscsi_mac[ETH_ALEN];
 	u8			fip_mac[ETH_ALEN];
 #endif
 
 	int			dmae_ready;
 	/* used to synchronize dmae accesses */
-	struct mutex		dmae_mutex;
+	spinlock_t		dmae_lock;
 
 	/* used to protect the FW mail box */
 	struct mutex		fw_mb_mutex;
@@ -1211,6 +1223,7 @@ struct bnx2x {
 	/* DCBX Negotation results */
 	struct dcbx_features			dcbx_local_feat;
 	u32					dcbx_error;
+	u32					pending_max;
 };
 
 /**
@@ -1447,6 +1460,12 @@ u32 bnx2x_fw_command(struct bnx2x *bp, u32 command, u32 param);
 void bnx2x_calc_fc_adv(struct bnx2x *bp);
 int bnx2x_sp_post(struct bnx2x *bp, int command, int cid,
 		  u32 data_hi, u32 data_lo, int common);
+
+/* Clears multicast and unicast list configuration in the chip. */
+void bnx2x_invalidate_e1_mc_list(struct bnx2x *bp);
+void bnx2x_invalidate_e1h_mc_list(struct bnx2x *bp);
+void bnx2x_invalidate_uc_list(struct bnx2x *bp);
+
 void bnx2x_update_coalesce(struct bnx2x *bp);
 int bnx2x_get_link_cfg_idx(struct bnx2x *bp);
 
@@ -1613,19 +1632,23 @@ static inline u32 reg_poll(struct bnx2x *bp, u32 reg, u32 expected, int ms,
 #define BNX2X_BTR			4
 #define MAX_SPQ_PENDING			8
 
-
-/* CMNG constants
-   derived from lab experiments, and not from system spec calculations !!! */
-#define DEF_MIN_RATE			100
-/* resolution of the rate shaping timer - 100 usec */
-#define RS_PERIODIC_TIMEOUT_USEC	100
-/* resolution of fairness algorithm in usecs -
-   coefficient for calculating the actual t fair */
-#define T_FAIR_COEF			10000000
+/* CMNG constants, as derived from system spec calculations */
+/* default MIN rate in case VNIC min rate is configured to zero - 100Mbps */
+#define DEF_MIN_RATE					100
+/* resolution of the rate shaping timer - 400 usec */
+#define RS_PERIODIC_TIMEOUT_USEC			400
 /* number of bytes in single QM arbitration cycle -
-   coefficient for calculating the fairness timer */
-#define QM_ARB_BYTES			40000
-#define FAIR_MEM			2
+ * coefficient for calculating the fairness timer */
+#define QM_ARB_BYTES					160000
+/* resolution of Min algorithm 1:100 */
+#define MIN_RES						100
+/* how many bytes above threshold for the minimal credit of Min algorithm*/
+#define MIN_ABOVE_THRESH				32768
+/* Fairness algorithm integration time coefficient -
+ * for calculating the actual Tfair */
+#define T_FAIR_COEF	((MIN_ABOVE_THRESH +  QM_ARB_BYTES) * 8 * MIN_RES)
+/* Memory of fairness algorithm . 2 cycles */
+#define FAIR_MEM					2
 
 
 #define ATTN_NIG_FOR_FUNC		(1L << 8)
@@ -1782,5 +1805,6 @@ static inline u32 reg_poll(struct bnx2x *bp, u32 reg, u32 expected, int ms,
 BNX2X_EXTERN int load_count[2][3]; /* per path: 0-common, 1-port0, 2-port1 */
 
 extern void bnx2x_set_ethtool_ops(struct net_device *netdev);
+void bnx2x_push_indir_table(struct bnx2x *bp);
 
 #endif /* bnx2x.h */
diff --git a/drivers/net/bnx2x/bnx2x_cmn.c b/drivers/net/bnx2x/bnx2x_cmn.c
index 710ce5d04c53..e83ac6dd6fc0 100644
--- a/drivers/net/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/bnx2x/bnx2x_cmn.c
@@ -232,7 +232,7 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
 	/* move empty skb from pool to prod and map it */
 	prod_rx_buf->skb = fp->tpa_pool[queue].skb;
 	mapping = dma_map_single(&bp->pdev->dev, fp->tpa_pool[queue].skb->data,
-				 bp->rx_buf_size, DMA_FROM_DEVICE);
+				 fp->rx_buf_size, DMA_FROM_DEVICE);
 	dma_unmap_addr_set(prod_rx_buf, mapping, mapping);
 
 	/* move partial skb from cons to pool (don't unmap yet) */
@@ -259,10 +259,44 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
 #endif
 }
 
+/* Timestamp option length allowed for TPA aggregation:
+ *
+ *		nop nop kind length echo val
+ */
+#define TPA_TSTAMP_OPT_LEN	12
+/**
+ * Calculate the approximate value of the MSS for this
+ * aggregation using the first packet of it.
+ *
+ * @param bp
+ * @param parsing_flags Parsing flags from the START CQE
+ * @param len_on_bd Total length of the first packet for the
+ *		     aggregation.
+ */
+static inline u16 bnx2x_set_lro_mss(struct bnx2x *bp, u16 parsing_flags,
+				    u16 len_on_bd)
+{
+	/* TPA arrgregation won't have an IP options and TCP options
+	 * other than timestamp.
+	 */
+	u16 hdrs_len = ETH_HLEN + sizeof(struct iphdr) + sizeof(struct tcphdr);
+
+
+	/* Check if there was a TCP timestamp, if there is it's will
+	 * always be 12 bytes length: nop nop kind length echo val.
+	 *
+	 * Otherwise FW would close the aggregation.
+	 */
+	if (parsing_flags & PARSING_FLAGS_TIME_STAMP_EXIST_FLAG)
+		hdrs_len += TPA_TSTAMP_OPT_LEN;
+
+	return len_on_bd - hdrs_len;
+}
+
 static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 			       struct sk_buff *skb,
 			       struct eth_fast_path_rx_cqe *fp_cqe,
-			       u16 cqe_idx)
+			       u16 cqe_idx, u16 parsing_flags)
 {
 	struct sw_rx_page *rx_pg, old_rx_pg;
 	u16 len_on_bd = le16_to_cpu(fp_cqe->len_on_bd);
@@ -275,8 +309,8 @@ static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 
 	/* This is needed in order to enable forwarding support */
 	if (frag_size)
-		skb_shinfo(skb)->gso_size = min((u32)SGE_PAGE_SIZE,
-					       max(frag_size, (u32)len_on_bd));
+		skb_shinfo(skb)->gso_size = bnx2x_set_lro_mss(bp, parsing_flags,
+							      len_on_bd);
 
 #ifdef BNX2X_STOP_ON_ERROR
 	if (pages > min_t(u32, 8, MAX_SKB_FRAGS)*SGE_PAGE_SIZE*PAGES_PER_SGE) {
@@ -333,26 +367,28 @@ static void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 	struct sw_rx_bd *rx_buf = &fp->tpa_pool[queue];
 	struct sk_buff *skb = rx_buf->skb;
 	/* alloc new skb */
-	struct sk_buff *new_skb = netdev_alloc_skb(bp->dev, bp->rx_buf_size);
+	struct sk_buff *new_skb = netdev_alloc_skb(bp->dev, fp->rx_buf_size);
 
 	/* Unmap skb in the pool anyway, as we are going to change
 	   pool entry status to BNX2X_TPA_STOP even if new skb allocation
 	   fails. */
 	dma_unmap_single(&bp->pdev->dev, dma_unmap_addr(rx_buf, mapping),
-			 bp->rx_buf_size, DMA_FROM_DEVICE);
+			 fp->rx_buf_size, DMA_FROM_DEVICE);
 
 	if (likely(new_skb)) {
 		/* fix ip xsum and give it to the stack */
 		/* (no need to map the new skb) */
+		u16 parsing_flags =
+			le16_to_cpu(cqe->fast_path_cqe.pars_flags.flags);
 
 		prefetch(skb);
 		prefetch(((char *)(skb)) + L1_CACHE_BYTES);
 
 #ifdef BNX2X_STOP_ON_ERROR
-		if (pad + len > bp->rx_buf_size) {
+		if (pad + len > fp->rx_buf_size) {
 			BNX2X_ERR("skb_put is about to fail...  "
 				  "pad %d  len %d  rx_buf_size %d\n",
-				  pad, len, bp->rx_buf_size);
+				  pad, len, fp->rx_buf_size);
 			bnx2x_panic();
 			return;
 		}
@@ -373,9 +409,9 @@ static void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 		}
 
 		if (!bnx2x_fill_frag_skb(bp, fp, skb,
-					 &cqe->fast_path_cqe, cqe_idx)) {
-			if ((le16_to_cpu(cqe->fast_path_cqe.
-			    pars_flags.flags) & PARSING_FLAGS_VLAN))
+					 &cqe->fast_path_cqe, cqe_idx,
+					 parsing_flags)) {
+			if (parsing_flags & PARSING_FLAGS_VLAN)
 				__vlan_hwaccel_put_tag(skb,
 						 le16_to_cpu(cqe->fast_path_cqe.
 							     vlan_tag));
@@ -582,7 +618,7 @@ int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 			if (likely(bnx2x_alloc_rx_skb(bp, fp, bd_prod) == 0)) {
 				dma_unmap_single(&bp->pdev->dev,
 					dma_unmap_addr(rx_buf, mapping),
-						 bp->rx_buf_size,
+						 fp->rx_buf_size,
 						 DMA_FROM_DEVICE);
 				skb_reserve(skb, pad);
 				skb_put(skb, len);
@@ -703,19 +739,20 @@ u16 bnx2x_get_mf_speed(struct bnx2x *bp)
 {
 	u16 line_speed = bp->link_vars.line_speed;
 	if (IS_MF(bp)) {
-		u16 maxCfg = (bp->mf_config[BP_VN(bp)] &
-						FUNC_MF_CFG_MAX_BW_MASK) >>
-						FUNC_MF_CFG_MAX_BW_SHIFT;
-		/* Calculate the current MAX line speed limit for the DCC
-		 * capable devices
+		u16 maxCfg = bnx2x_extract_max_cfg(bp,
+						   bp->mf_config[BP_VN(bp)]);
+
+		/* Calculate the current MAX line speed limit for the MF
+		 * devices
 		 */
-		if (IS_MF_SD(bp)) {
+		if (IS_MF_SI(bp))
+			line_speed = (line_speed * maxCfg) / 100;
+		else { /* SD mode */
 			u16 vn_max_rate = maxCfg * 100;
 
 			if (vn_max_rate < line_speed)
 				line_speed = vn_max_rate;
-		} else /* IS_MF_SI(bp)) */
-			line_speed = (line_speed * maxCfg) / 100;
+		}
 	}
 
 	return line_speed;
@@ -821,19 +858,16 @@ void bnx2x_init_rx_rings(struct bnx2x *bp)
 	u16 ring_prod;
 	int i, j;
 
-	bp->rx_buf_size = bp->dev->mtu + ETH_OVREHEAD + BNX2X_RX_ALIGN +
-		IP_HEADER_ALIGNMENT_PADDING;
-
-	DP(NETIF_MSG_IFUP,
-	   "mtu %d  rx_buf_size %d\n", bp->dev->mtu, bp->rx_buf_size);
-
 	for_each_rx_queue(bp, j) {
 		struct bnx2x_fastpath *fp = &bp->fp[j];
 
+		DP(NETIF_MSG_IFUP,
+		   "mtu %d  rx_buf_size %d\n", bp->dev->mtu, fp->rx_buf_size);
+
 		if (!fp->disable_tpa) {
 			for (i = 0; i < max_agg_queues; i++) {
 				fp->tpa_pool[i].skb =
-				   netdev_alloc_skb(bp->dev, bp->rx_buf_size);
+				   netdev_alloc_skb(bp->dev, fp->rx_buf_size);
 				if (!fp->tpa_pool[i].skb) {
 					BNX2X_ERR("Failed to allocate TPA "
 						  "skb pool for queue[%d] - "
@@ -941,7 +975,7 @@ static void bnx2x_free_rx_skbs(struct bnx2x *bp)
 
 			dma_unmap_single(&bp->pdev->dev,
 					 dma_unmap_addr(rx_buf, mapping),
-					 bp->rx_buf_size, DMA_FROM_DEVICE);
+					 fp->rx_buf_size, DMA_FROM_DEVICE);
 
 			rx_buf->skb = NULL;
 			dev_kfree_skb(skb);
@@ -959,6 +993,23 @@ void bnx2x_free_skbs(struct bnx2x *bp)
 	bnx2x_free_rx_skbs(bp);
 }
 
+void bnx2x_update_max_mf_config(struct bnx2x *bp, u32 value)
+{
+	/* load old values */
+	u32 mf_cfg = bp->mf_config[BP_VN(bp)];
+
+	if (value != bnx2x_extract_max_cfg(bp, mf_cfg)) {
+		/* leave all but MAX value */
+		mf_cfg &= ~FUNC_MF_CFG_MAX_BW_MASK;
+
+		/* set new MAX value */
+		mf_cfg |= (value << FUNC_MF_CFG_MAX_BW_SHIFT)
+				& FUNC_MF_CFG_MAX_BW_MASK;
+
+		bnx2x_fw_command(bp, DRV_MSG_CODE_SET_MF_BW, mf_cfg);
+	}
+}
+
 static void bnx2x_free_msix_irqs(struct bnx2x *bp)
 {
 	int i, offset = 1;
@@ -1249,6 +1300,31 @@ static inline int bnx2x_set_real_num_queues(struct bnx2x *bp)
 	return rc;
 }
 
+static inline void bnx2x_set_rx_buf_size(struct bnx2x *bp)
+{
+	int i;
+
+	for_each_queue(bp, i) {
+		struct bnx2x_fastpath *fp = &bp->fp[i];
+
+		/* Always use a mini-jumbo MTU for the FCoE L2 ring */
+		if (IS_FCOE_IDX(i))
+			/*
+			 * Although there are no IP frames expected to arrive to
+			 * this ring we still want to add an
+			 * IP_HEADER_ALIGNMENT_PADDING to prevent a buffer
+			 * overrun attack.
+			 */
+			fp->rx_buf_size =
+				BNX2X_FCOE_MINI_JUMBO_MTU + ETH_OVREHEAD +
+				BNX2X_RX_ALIGN + IP_HEADER_ALIGNMENT_PADDING;
+		else
+			fp->rx_buf_size =
+				bp->dev->mtu + ETH_OVREHEAD + BNX2X_RX_ALIGN +
+				IP_HEADER_ALIGNMENT_PADDING;
+	}
+}
+
 /* must be called with rtnl_lock */
 int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 {
@@ -1272,6 +1348,9 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 	/* must be called before memory allocation and HW init */
 	bnx2x_ilt_set_info(bp);
 
+	/* Set the receive queues buffer size */
+	bnx2x_set_rx_buf_size(bp);
+
 	if (bnx2x_alloc_mem(bp))
 		return -ENOMEM;
 
@@ -1427,28 +1506,40 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 
 	bnx2x_set_eth_mac(bp, 1);
 
+	/* Clear MC configuration */
+	if (CHIP_IS_E1(bp))
+		bnx2x_invalidate_e1_mc_list(bp);
+	else
+		bnx2x_invalidate_e1h_mc_list(bp);
+
+	/* Clear UC lists configuration */
+	bnx2x_invalidate_uc_list(bp);
+
+	if (bp->pending_max) {
+		bnx2x_update_max_mf_config(bp, bp->pending_max);
+		bp->pending_max = 0;
+	}
+
 	if (bp->port.pmf)
 		bnx2x_initial_phy_init(bp, load_mode);
 
+	/* Initialize Rx filtering */
+	bnx2x_set_rx_mode(bp->dev);
+
 	/* Start fast path */
 	switch (load_mode) {
 	case LOAD_NORMAL:
 		/* Tx queue should be only reenabled */
 		netif_tx_wake_all_queues(bp->dev);
 		/* Initialize the receive filter. */
-		bnx2x_set_rx_mode(bp->dev);
 		break;
 
 	case LOAD_OPEN:
 		netif_tx_start_all_queues(bp->dev);
 		smp_mb__after_clear_bit();
-		/* Initialize the receive filter. */
-		bnx2x_set_rx_mode(bp->dev);
 		break;
 
 	case LOAD_DIAG:
-		/* Initialize the receive filter. */
-		bnx2x_set_rx_mode(bp->dev);
 		bp->state = BNX2X_STATE_DIAG;
 		break;
 
diff --git a/drivers/net/bnx2x/bnx2x_cmn.h b/drivers/net/bnx2x/bnx2x_cmn.h
index 03eb4d68e6bb..ef37b98d6146 100644
--- a/drivers/net/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/bnx2x/bnx2x_cmn.h
@@ -341,6 +341,15 @@ void bnx2x_dcbx_init(struct bnx2x *bp);
  */
 int bnx2x_set_power_state(struct bnx2x *bp, pci_power_t state);
 
+/**
+ * Updates MAX part of MF configuration in HW
+ * (if required)
+ *
+ * @param bp
+ * @param value
+ */
+void bnx2x_update_max_mf_config(struct bnx2x *bp, u32 value);
+
 /* dev_close main block */
 int bnx2x_nic_unload(struct bnx2x *bp, int unload_mode);
 
@@ -822,11 +831,11 @@ static inline int bnx2x_alloc_rx_skb(struct bnx2x *bp,
 	struct eth_rx_bd *rx_bd = &fp->rx_desc_ring[index];
 	dma_addr_t mapping;
 
-	skb = netdev_alloc_skb(bp->dev, bp->rx_buf_size);
+	skb = netdev_alloc_skb(bp->dev, fp->rx_buf_size);
 	if (unlikely(skb == NULL))
 		return -ENOMEM;
 
-	mapping = dma_map_single(&bp->pdev->dev, skb->data, bp->rx_buf_size,
+	mapping = dma_map_single(&bp->pdev->dev, skb->data, fp->rx_buf_size,
 				 DMA_FROM_DEVICE);
 	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
 		dev_kfree_skb(skb);
@@ -892,7 +901,7 @@ static inline void bnx2x_free_tpa_pool(struct bnx2x *bp,
 		if (fp->tpa_state[i] == BNX2X_TPA_START)
 			dma_unmap_single(&bp->pdev->dev,
 					 dma_unmap_addr(rx_buf, mapping),
-					 bp->rx_buf_size, DMA_FROM_DEVICE);
+					 fp->rx_buf_size, DMA_FROM_DEVICE);
 
 		dev_kfree_skb(skb);
 		rx_buf->skb = NULL;
@@ -1044,4 +1053,24 @@ static inline void storm_memset_cmng(struct bnx2x *bp,
 void bnx2x_acquire_phy_lock(struct bnx2x *bp);
 void bnx2x_release_phy_lock(struct bnx2x *bp);
 
+/**
+ * Extracts MAX BW part from MF configuration.
+ *
+ * @param bp
+ * @param mf_cfg
+ *
+ * @return u16
+ */
+static inline u16 bnx2x_extract_max_cfg(struct bnx2x *bp, u32 mf_cfg)
+{
+	u16 max_cfg = (mf_cfg & FUNC_MF_CFG_MAX_BW_MASK) >>
+			      FUNC_MF_CFG_MAX_BW_SHIFT;
+	if (!max_cfg) {
+		BNX2X_ERR("Illegal configuration detected for Max BW - "
+			  "using 100 instead\n");
+		max_cfg = 100;
+	}
+	return max_cfg;
+}
+
 #endif /* BNX2X_CMN_H */
diff --git a/drivers/net/bnx2x/bnx2x_dcb.c b/drivers/net/bnx2x/bnx2x_dcb.c
index fb60021f81fb..9a24d79c71d9 100644
--- a/drivers/net/bnx2x/bnx2x_dcb.c
+++ b/drivers/net/bnx2x/bnx2x_dcb.c
@@ -19,6 +19,9 @@
 #include <linux/netdevice.h>
 #include <linux/types.h>
 #include <linux/errno.h>
+#ifdef BCM_DCBNL
+#include <linux/dcbnl.h>
+#endif
 
 #include "bnx2x.h"
 #include "bnx2x_cmn.h"
@@ -508,13 +511,75 @@ static int bnx2x_dcbx_read_shmem_neg_results(struct bnx2x *bp)
 	return 0;
 }
 
+
+#ifdef BCM_DCBNL
+static inline
+u8 bnx2x_dcbx_dcbnl_app_up(struct dcbx_app_priority_entry *ent)
+{
+	u8 pri;
+
+	/* Choose the highest priority */
+	for (pri = MAX_PFC_PRIORITIES - 1; pri > 0; pri--)
+		if (ent->pri_bitmap & (1 << pri))
+			break;
+	return pri;
+}
+
+static inline
+u8 bnx2x_dcbx_dcbnl_app_idtype(struct dcbx_app_priority_entry *ent)
+{
+	return ((ent->appBitfield & DCBX_APP_ENTRY_SF_MASK) ==
+		DCBX_APP_SF_PORT) ? DCB_APP_IDTYPE_PORTNUM :
+		DCB_APP_IDTYPE_ETHTYPE;
+}
+
+static inline
+void bnx2x_dcbx_invalidate_local_apps(struct bnx2x *bp)
+{
+	int i;
+	for (i = 0; i < DCBX_MAX_APP_PROTOCOL; i++)
+		bp->dcbx_local_feat.app.app_pri_tbl[i].appBitfield &=
+							~DCBX_APP_ENTRY_VALID;
+}
+
+int bnx2x_dcbnl_update_applist(struct bnx2x *bp, bool delall)
+{
+	int i, err = 0;
+
+	for (i = 0; i < DCBX_MAX_APP_PROTOCOL && err == 0; i++) {
+		struct dcbx_app_priority_entry *ent =
+			&bp->dcbx_local_feat.app.app_pri_tbl[i];
+
+		if (ent->appBitfield & DCBX_APP_ENTRY_VALID) {
+			u8 up = bnx2x_dcbx_dcbnl_app_up(ent);
+
+			/* avoid invalid user-priority */
+			if (up) {
+				struct dcb_app app;
+				app.selector = bnx2x_dcbx_dcbnl_app_idtype(ent);
+				app.protocol = ent->app_id;
+				app.priority = delall ? 0 : up;
+				err = dcb_setapp(bp->dev, &app);
+			}
+		}
+	}
+	return err;
+}
+#endif
+
 void bnx2x_dcbx_set_params(struct bnx2x *bp, u32 state)
 {
 	switch (state) {
 	case BNX2X_DCBX_STATE_NEG_RECEIVED:
 		{
 			DP(NETIF_MSG_LINK, "BNX2X_DCBX_STATE_NEG_RECEIVED\n");
-
+#ifdef BCM_DCBNL
+			/**
+			 * Delete app tlvs from dcbnl before reading new
+			 * negotiation results
+			 */
+			bnx2x_dcbnl_update_applist(bp, true);
+#endif
 			/* Read neg results if dcbx is in the FW */
 			if (bnx2x_dcbx_read_shmem_neg_results(bp))
 				return;
@@ -526,10 +591,24 @@ void bnx2x_dcbx_set_params(struct bnx2x *bp, u32 state)
 						 bp->dcbx_error);
 
 			if (bp->state != BNX2X_STATE_OPENING_WAIT4_LOAD) {
+#ifdef BCM_DCBNL
+				/**
+				 * Add new app tlvs to dcbnl
+				 */
+				bnx2x_dcbnl_update_applist(bp, false);
+#endif
 				bnx2x_dcbx_stop_hw_tx(bp);
 				return;
 			}
 			/* fall through */
+#ifdef BCM_DCBNL
+			/**
+			 * Invalidate the local app tlvs if they are not added
+			 * to the dcbnl app list to avoid deleting them from
+			 * the list later on
+			 */
+			bnx2x_dcbx_invalidate_local_apps(bp);
+#endif
 		}
 	case BNX2X_DCBX_STATE_TX_PAUSED:
 		DP(NETIF_MSG_LINK, "BNX2X_DCBX_STATE_TX_PAUSED\n");
@@ -1505,8 +1584,7 @@ static void bnx2x_pfc_fw_struct_e2(struct bnx2x *bp)
 	bnx2x_dcbx_print_cos_params(bp,	pfc_fw_cfg);
 }
 /* DCB netlink */
-#ifdef BCM_DCB
-#include <linux/dcbnl.h>
+#ifdef BCM_DCBNL
 
 #define BNX2X_DCBX_CAPS		(DCB_CAP_DCBX_LLD_MANAGED | \
 				DCB_CAP_DCBX_VER_CEE | DCB_CAP_DCBX_STATIC)
@@ -1816,32 +1894,6 @@ static void bnx2x_dcbnl_set_pfc_state(struct net_device *netdev, u8 state)
 	bp->dcbx_config_params.admin_pfc_enable = (state ? 1 : 0);
 }
 
-static bool bnx2x_app_is_equal(struct dcbx_app_priority_entry *app_ent,
-			       u8 idtype, u16 idval)
-{
-	if (!(app_ent->appBitfield & DCBX_APP_ENTRY_VALID))
-		return false;
-
-	switch (idtype) {
-	case DCB_APP_IDTYPE_ETHTYPE:
-		if ((app_ent->appBitfield & DCBX_APP_ENTRY_SF_MASK) !=
-			DCBX_APP_SF_ETH_TYPE)
-			return false;
-		break;
-	case DCB_APP_IDTYPE_PORTNUM:
-		if ((app_ent->appBitfield & DCBX_APP_ENTRY_SF_MASK) !=
-			DCBX_APP_SF_PORT)
-			return false;
-		break;
-	default:
-		return false;
-	}
-	if (app_ent->app_id != idval)
-		return false;
-
-	return true;
-}
-
 static void bnx2x_admin_app_set_ent(
 	struct bnx2x_admin_priority_app_table *app_ent,
 	u8 idtype, u16 idval, u8 up)
@@ -1943,30 +1995,6 @@ static u8 bnx2x_dcbnl_set_app_up(struct net_device *netdev, u8 idtype,
 	return bnx2x_set_admin_app_up(bp, idtype, idval, up);
 }
 
-static u8 bnx2x_dcbnl_get_app_up(struct net_device *netdev, u8 idtype,
-				 u16 idval)
-{
-	int i;
-	u8 up = 0;
-
-	struct bnx2x *bp = netdev_priv(netdev);
-	DP(NETIF_MSG_LINK, "app_type %d, app_id 0x%x\n", idtype, idval);
-
-	/* iterate over the app entries looking for idtype and idval */
-	for (i = 0; i < DCBX_MAX_APP_PROTOCOL; i++)
-		if (bnx2x_app_is_equal(&bp->dcbx_local_feat.app.app_pri_tbl[i],
-				       idtype, idval))
-			break;
-
-	if (i < DCBX_MAX_APP_PROTOCOL)
-		/* if found return up */
-		up = bp->dcbx_local_feat.app.app_pri_tbl[i].pri_bitmap;
-	else
-		DP(NETIF_MSG_LINK, "app not found\n");
-
-	return up;
-}
-
 static u8 bnx2x_dcbnl_get_dcbx(struct net_device *netdev)
 {
 	struct bnx2x *bp = netdev_priv(netdev);
@@ -2107,7 +2135,6 @@ const struct dcbnl_rtnl_ops bnx2x_dcbnl_ops = {
 	.setnumtcs      = bnx2x_dcbnl_set_numtcs,
 	.getpfcstate    = bnx2x_dcbnl_get_pfc_state,
 	.setpfcstate    = bnx2x_dcbnl_set_pfc_state,
-	.getapp         = bnx2x_dcbnl_get_app_up,
 	.setapp         = bnx2x_dcbnl_set_app_up,
 	.getdcbx        = bnx2x_dcbnl_get_dcbx,
 	.setdcbx        = bnx2x_dcbnl_set_dcbx,
@@ -2115,4 +2142,4 @@ const struct dcbnl_rtnl_ops bnx2x_dcbnl_ops = {
 	.setfeatcfg     = bnx2x_dcbnl_set_featcfg,
 };
 
-#endif /* BCM_DCB */
+#endif /* BCM_DCBNL */
diff --git a/drivers/net/bnx2x/bnx2x_dcb.h b/drivers/net/bnx2x/bnx2x_dcb.h
index f650f98e4092..71b8eda43bd0 100644
--- a/drivers/net/bnx2x/bnx2x_dcb.h
+++ b/drivers/net/bnx2x/bnx2x_dcb.h
@@ -189,8 +189,9 @@ enum {
 void bnx2x_dcbx_set_params(struct bnx2x *bp, u32 state);
 
 /* DCB netlink */
-#ifdef BCM_DCB
+#ifdef BCM_DCBNL
 extern const struct dcbnl_rtnl_ops bnx2x_dcbnl_ops;
-#endif /* BCM_DCB */
+int bnx2x_dcbnl_update_applist(struct bnx2x *bp, bool delall);
+#endif /* BCM_DCBNL */
 
 #endif /* BNX2X_DCB_H */
diff --git a/drivers/net/bnx2x/bnx2x_ethtool.c b/drivers/net/bnx2x/bnx2x_ethtool.c
index 5b44a8b48509..f5050155c6b5 100644
--- a/drivers/net/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/bnx2x/bnx2x_ethtool.c
@@ -238,7 +238,7 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 	speed |= (cmd->speed_hi << 16);
 
 	if (IS_MF_SI(bp)) {
-		u32 param = 0;
+		u32 part;
 		u32 line_speed = bp->link_vars.line_speed;
 
 		/* use 10G if no link detected */
@@ -251,23 +251,22 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 				       REQ_BC_VER_4_SET_MF_BW);
 			return -EINVAL;
 		}
-		if (line_speed < speed) {
-			BNX2X_DEV_INFO("New speed should be less or equal "
-				       "to actual line speed\n");
+
+		part = (speed * 100) / line_speed;
+
+		if (line_speed < speed || !part) {
+			BNX2X_DEV_INFO("Speed setting should be in a range "
+				       "from 1%% to 100%% "
+				       "of actual line speed\n");
 			return -EINVAL;
 		}
-		/* load old values */
-		param = bp->mf_config[BP_VN(bp)];
-
-		/* leave only MIN value */
-		param &= FUNC_MF_CFG_MIN_BW_MASK;
 
-		/* set new MAX value */
-		param |= (((speed * 100) / line_speed)
-				 << FUNC_MF_CFG_MAX_BW_SHIFT)
-				  & FUNC_MF_CFG_MAX_BW_MASK;
+		if (bp->state != BNX2X_STATE_OPEN)
+			/* store value for following "load" */
+			bp->pending_max = part;
+		else
+			bnx2x_update_max_mf_config(bp, part);
 
-		bnx2x_fw_command(bp, DRV_MSG_CODE_SET_MF_BW, param);
 		return 0;
 	}
 
@@ -1618,7 +1617,7 @@ static int bnx2x_run_loopback(struct bnx2x *bp, int loopback_mode, u8 link_up)
 	/* prepare the loopback packet */
 	pkt_size = (((bp->dev->mtu < ETH_MAX_PACKET_SIZE) ?
 		     bp->dev->mtu : ETH_MAX_PACKET_SIZE) + ETH_HLEN);
-	skb = netdev_alloc_skb(bp->dev, bp->rx_buf_size);
+	skb = netdev_alloc_skb(bp->dev, fp_rx->rx_buf_size);
 	if (!skb) {
 		rc = -ENOMEM;
 		goto test_loopback_exit;
@@ -1781,9 +1780,7 @@ static int bnx2x_test_nvram(struct bnx2x *bp)
 		{ 0x100, 0x350 }, /* manuf_info */
 		{ 0x450,  0xf0 }, /* feature_info */
 		{ 0x640,  0x64 }, /* upgrade_key_info */
-		{ 0x6a4,  0x64 },
 		{ 0x708,  0x70 }, /* manuf_key_info */
-		{ 0x778,  0x70 },
 		{     0,     0 }
 	};
 	__be32 buf[0x350 / 4];
@@ -1933,11 +1930,11 @@ static void bnx2x_self_test(struct net_device *dev,
 		buf[4] = 1;
 		etest->flags |= ETH_TEST_FL_FAILED;
 	}
-	if (bp->port.pmf)
-		if (bnx2x_link_test(bp, is_serdes) != 0) {
-			buf[5] = 1;
-			etest->flags |= ETH_TEST_FL_FAILED;
-		}
+
+	if (bnx2x_link_test(bp, is_serdes) != 0) {
+		buf[5] = 1;
+		etest->flags |= ETH_TEST_FL_FAILED;
+	}
 
 #ifdef BNX2X_EXTRA_DEBUG
 	bnx2x_panic_dump(bp);
@@ -2134,6 +2131,59 @@ static int bnx2x_phys_id(struct net_device *dev, u32 data)
 	return 0;
 }
 
+static int bnx2x_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
+			   void *rules __always_unused)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+
+	switch (info->cmd) {
+	case ETHTOOL_GRXRINGS:
+		info->data = BNX2X_NUM_ETH_QUEUES(bp);
+		return 0;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int bnx2x_get_rxfh_indir(struct net_device *dev,
+				struct ethtool_rxfh_indir *indir)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	size_t copy_size =
+		min_t(size_t, indir->size, TSTORM_INDIRECTION_TABLE_SIZE);
+
+	if (bp->multi_mode == ETH_RSS_MODE_DISABLED)
+		return -EOPNOTSUPP;
+
+	indir->size = TSTORM_INDIRECTION_TABLE_SIZE;
+	memcpy(indir->ring_index, bp->rx_indir_table,
+	       copy_size * sizeof(bp->rx_indir_table[0]));
+	return 0;
+}
+
+static int bnx2x_set_rxfh_indir(struct net_device *dev,
+				const struct ethtool_rxfh_indir *indir)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	size_t i;
+
+	if (bp->multi_mode == ETH_RSS_MODE_DISABLED)
+		return -EOPNOTSUPP;
+
+	/* Validate size and indices */
+	if (indir->size != TSTORM_INDIRECTION_TABLE_SIZE)
+		return -EINVAL;
+	for (i = 0; i < TSTORM_INDIRECTION_TABLE_SIZE; i++)
+		if (indir->ring_index[i] >= BNX2X_NUM_ETH_QUEUES(bp))
+			return -EINVAL;
+
+	memcpy(bp->rx_indir_table, indir->ring_index,
+	       indir->size * sizeof(bp->rx_indir_table[0]));
+	bnx2x_push_indir_table(bp);
+	return 0;
+}
+
 static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.get_settings		= bnx2x_get_settings,
 	.set_settings		= bnx2x_set_settings,
@@ -2170,6 +2220,9 @@ static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.get_strings		= bnx2x_get_strings,
 	.phys_id		= bnx2x_phys_id,
 	.get_ethtool_stats	= bnx2x_get_ethtool_stats,
+	.get_rxnfc		= bnx2x_get_rxnfc,
+	.get_rxfh_indir		= bnx2x_get_rxfh_indir,
+	.set_rxfh_indir		= bnx2x_set_rxfh_indir,
 };
 
 void bnx2x_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/bnx2x/bnx2x_hsi.h b/drivers/net/bnx2x/bnx2x_hsi.h
index 548f5631c0dc..be503cc0a50b 100644
--- a/drivers/net/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/bnx2x/bnx2x_hsi.h
@@ -11,20 +11,27 @@
 
 #include "bnx2x_fw_defs.h"
 
+#define FW_ENCODE_32BIT_PATTERN		0x1e1e1e1e
+
 struct license_key {
 	u32 reserved[6];
 
-#if defined(__BIG_ENDIAN)
-	u16 max_iscsi_init_conn;
-	u16 max_iscsi_trgt_conn;
-#elif defined(__LITTLE_ENDIAN)
-	u16 max_iscsi_trgt_conn;
-	u16 max_iscsi_init_conn;
-#endif
+	u32 max_iscsi_conn;
+#define BNX2X_MAX_ISCSI_TRGT_CONN_MASK	0xFFFF
+#define BNX2X_MAX_ISCSI_TRGT_CONN_SHIFT	0
+#define BNX2X_MAX_ISCSI_INIT_CONN_MASK	0xFFFF0000
+#define BNX2X_MAX_ISCSI_INIT_CONN_SHIFT	16
 
-	u32 reserved_a[6];
-};
+	u32 reserved_a;
+
+	u32 max_fcoe_conn;
+#define BNX2X_MAX_FCOE_TRGT_CONN_MASK	0xFFFF
+#define BNX2X_MAX_FCOE_TRGT_CONN_SHIFT	0
+#define BNX2X_MAX_FCOE_INIT_CONN_MASK	0xFFFF0000
+#define BNX2X_MAX_FCOE_INIT_CONN_SHIFT	16
 
+	u32 reserved_b[4];
+};
 
 #define PORT_0				0
 #define PORT_1				1
@@ -237,8 +244,26 @@ struct port_hw_cfg {			    /* port 0: 0x12c  port 1: 0x2bc */
 #define PORT_HW_CFG_SERDES_RX_DRV_EQUALIZER_SHIFT	      16
 
 
-	u32 Reserved0[16];				    /* 0x158 */
-
+	u32 Reserved0[3];				    /* 0x158 */
+	/*	Controls the TX laser of the SFP+ module */
+	u32 sfp_ctrl;					/* 0x164 */
+#define PORT_HW_CFG_TX_LASER_MASK			      0x000000FF
+#define PORT_HW_CFG_TX_LASER_SHIFT			      0
+#define PORT_HW_CFG_TX_LASER_MDIO			      0x00000000
+#define PORT_HW_CFG_TX_LASER_GPIO0			      0x00000001
+#define PORT_HW_CFG_TX_LASER_GPIO1			      0x00000002
+#define PORT_HW_CFG_TX_LASER_GPIO2			      0x00000003
+#define PORT_HW_CFG_TX_LASER_GPIO3			      0x00000004
+
+    /*	Controls the fault module LED of the SFP+ */
+#define PORT_HW_CFG_FAULT_MODULE_LED_MASK		      0x0000FF00
+#define PORT_HW_CFG_FAULT_MODULE_LED_SHIFT		      8
+#define PORT_HW_CFG_FAULT_MODULE_LED_GPIO0		      0x00000000
+#define PORT_HW_CFG_FAULT_MODULE_LED_GPIO1		      0x00000100
+#define PORT_HW_CFG_FAULT_MODULE_LED_GPIO2		      0x00000200
+#define PORT_HW_CFG_FAULT_MODULE_LED_GPIO3		      0x00000300
+#define PORT_HW_CFG_FAULT_MODULE_LED_DISABLED		      0x00000400
+	u32 Reserved01[12];				    /* 0x158 */
 	/*  for external PHY, or forced mode or during AN */
 	u16 xgxs_config_rx[4];				    /* 0x198 */
 
@@ -246,12 +271,78 @@ struct port_hw_cfg {			    /* port 0: 0x12c  port 1: 0x2bc */
 
 	u32 Reserved1[56];				    /* 0x1A8 */
 	u32 default_cfg;				    /* 0x288 */
+#define PORT_HW_CFG_GPIO0_CONFIG_MASK			      0x00000003
+#define PORT_HW_CFG_GPIO0_CONFIG_SHIFT			      0
+#define PORT_HW_CFG_GPIO0_CONFIG_NA			      0x00000000
+#define PORT_HW_CFG_GPIO0_CONFIG_LOW			      0x00000001
+#define PORT_HW_CFG_GPIO0_CONFIG_HIGH			      0x00000002
+#define PORT_HW_CFG_GPIO0_CONFIG_INPUT			      0x00000003
+
+#define PORT_HW_CFG_GPIO1_CONFIG_MASK			      0x0000000C
+#define PORT_HW_CFG_GPIO1_CONFIG_SHIFT			      2
+#define PORT_HW_CFG_GPIO1_CONFIG_NA			      0x00000000
+#define PORT_HW_CFG_GPIO1_CONFIG_LOW			      0x00000004
+#define PORT_HW_CFG_GPIO1_CONFIG_HIGH			      0x00000008
+#define PORT_HW_CFG_GPIO1_CONFIG_INPUT			      0x0000000c
+
+#define PORT_HW_CFG_GPIO2_CONFIG_MASK			      0x00000030
+#define PORT_HW_CFG_GPIO2_CONFIG_SHIFT			      4
+#define PORT_HW_CFG_GPIO2_CONFIG_NA			      0x00000000
+#define PORT_HW_CFG_GPIO2_CONFIG_LOW			      0x00000010
+#define PORT_HW_CFG_GPIO2_CONFIG_HIGH			      0x00000020
+#define PORT_HW_CFG_GPIO2_CONFIG_INPUT			      0x00000030
+
+#define PORT_HW_CFG_GPIO3_CONFIG_MASK			      0x000000C0
+#define PORT_HW_CFG_GPIO3_CONFIG_SHIFT			      6
+#define PORT_HW_CFG_GPIO3_CONFIG_NA			      0x00000000
+#define PORT_HW_CFG_GPIO3_CONFIG_LOW			      0x00000040
+#define PORT_HW_CFG_GPIO3_CONFIG_HIGH			      0x00000080
+#define PORT_HW_CFG_GPIO3_CONFIG_INPUT			      0x000000c0
+
+	/*
+	 * When KR link is required to be set to force which is not
+	 * KR-compliant, this parameter determine what is the trigger for it.
+	 * When GPIO is selected, low input will force the speed. Currently
+	 * default speed is 1G. In the future, it may be widen to select the
+	 * forced speed in with another parameter. Note when force-1G is
+	 * enabled, it override option 56: Link Speed option.
+	 */
+#define PORT_HW_CFG_FORCE_KR_ENABLER_MASK		      0x00000F00
+#define PORT_HW_CFG_FORCE_KR_ENABLER_SHIFT		      8
+#define PORT_HW_CFG_FORCE_KR_ENABLER_NOT_FORCED		      0x00000000
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO0_P0		      0x00000100
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO1_P0		      0x00000200
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO2_P0		      0x00000300
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO3_P0		      0x00000400
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO0_P1		      0x00000500
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO1_P1		      0x00000600
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO2_P1		      0x00000700
+#define PORT_HW_CFG_FORCE_KR_ENABLER_GPIO3_P1		      0x00000800
+#define PORT_HW_CFG_FORCE_KR_ENABLER_FORCED		      0x00000900
+    /*	Enable to determine with which GPIO to reset the external phy */
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_MASK		      0x000F0000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_SHIFT		      16
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_PHY_TYPE		      0x00000000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO0_P0		      0x00010000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO1_P0		      0x00020000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO2_P0		      0x00030000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO3_P0		      0x00040000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO0_P1		      0x00050000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO1_P1		      0x00060000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO2_P1		      0x00070000
+#define PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO3_P1		      0x00080000
 	/*  Enable BAM on KR */
 #define PORT_HW_CFG_ENABLE_BAM_ON_KR_MASK		      0x00100000
 #define PORT_HW_CFG_ENABLE_BAM_ON_KR_SHIFT		      20
 #define PORT_HW_CFG_ENABLE_BAM_ON_KR_DISABLED		      0x00000000
 #define PORT_HW_CFG_ENABLE_BAM_ON_KR_ENABLED		      0x00100000
 
+	/*  Enable Common Mode Sense */
+#define PORT_HW_CFG_ENABLE_CMS_MASK			      0x00200000
+#define PORT_HW_CFG_ENABLE_CMS_SHIFT			      21
+#define PORT_HW_CFG_ENABLE_CMS_DISABLED			      0x00000000
+#define PORT_HW_CFG_ENABLE_CMS_ENABLED			      0x00200000
+
 	u32 speed_capability_mask2;			    /* 0x28C */
 #define PORT_HW_CFG_SPEED_CAPABILITY2_D3_MASK		      0x0000FFFF
 #define PORT_HW_CFG_SPEED_CAPABILITY2_D3_SHIFT		      0
@@ -381,6 +472,7 @@ struct port_hw_cfg {			    /* port 0: 0x12c  port 1: 0x2bc */
 #define PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727	    0x00000900
 #define PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727_NOC   0x00000a00
 #define PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84823	    0x00000b00
+#define PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833	    0x00000d00
 #define PORT_HW_CFG_XGXS_EXT_PHY_TYPE_FAILURE	    0x0000fd00
 #define PORT_HW_CFG_XGXS_EXT_PHY_TYPE_NOT_CONN	    0x0000ff00
 
diff --git a/drivers/net/bnx2x/bnx2x_init.h b/drivers/net/bnx2x/bnx2x_init.h
index 5a268e9a0895..fa6dbe3f2058 100644
--- a/drivers/net/bnx2x/bnx2x_init.h
+++ b/drivers/net/bnx2x/bnx2x_init.h
@@ -241,7 +241,7 @@ static const struct {
 	/* Block IGU, MISC, PXP and PXP2 parity errors as long as we don't
 	 * want to handle "system kill" flow at the moment.
 	 */
-	BLOCK_PRTY_INFO(PXP, 0x3ffffff, 0x3ffffff, 0x3ffffff, 0x3ffffff),
+	BLOCK_PRTY_INFO(PXP, 0x7ffffff, 0x3ffffff, 0x3ffffff, 0x7ffffff),
 	BLOCK_PRTY_INFO_0(PXP2,	0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff),
 	BLOCK_PRTY_INFO_1(PXP2,	0x7ff, 0x7f, 0x7f, 0x7ff),
 	BLOCK_PRTY_INFO(HC, 0x7, 0x7, 0x7, 0),
diff --git a/drivers/net/bnx2x/bnx2x_link.c b/drivers/net/bnx2x/bnx2x_link.c
index dd1210fddfff..f2f367d4e74d 100644
--- a/drivers/net/bnx2x/bnx2x_link.c
+++ b/drivers/net/bnx2x/bnx2x_link.c
@@ -1,4 +1,4 @@
-/* Copyright 2008-2009 Broadcom Corporation
+/* Copyright 2008-2011 Broadcom Corporation
  *
  * Unless you and Broadcom execute a separate written software license
  * agreement governing use of this software, this software is licensed to you
@@ -28,12 +28,13 @@
 
 /********************************************************/
 #define ETH_HLEN			14
-#define ETH_OVREHEAD		(ETH_HLEN + 8 + 8)/* 16 for CRC + VLAN + LLC */
+/* L2 header size + 2*VLANs (8 bytes) + LLC SNAP (8 bytes) */
+#define ETH_OVREHEAD			(ETH_HLEN + 8 + 8)
 #define ETH_MIN_PACKET_SIZE		60
 #define ETH_MAX_PACKET_SIZE		1500
 #define ETH_MAX_JUMBO_PACKET_SIZE	9600
 #define MDIO_ACCESS_TIMEOUT		1000
-#define BMAC_CONTROL_RX_ENABLE	2
+#define BMAC_CONTROL_RX_ENABLE		2
 
 /***********************************************************/
 /*			Shortcut definitions		   */
@@ -79,7 +80,7 @@
 
 #define AUTONEG_CL37		SHARED_HW_CFG_AN_ENABLE_CL37
 #define AUTONEG_CL73		SHARED_HW_CFG_AN_ENABLE_CL73
-#define AUTONEG_BAM 		SHARED_HW_CFG_AN_ENABLE_BAM
+#define AUTONEG_BAM		SHARED_HW_CFG_AN_ENABLE_BAM
 #define AUTONEG_PARALLEL \
 				SHARED_HW_CFG_AN_ENABLE_PARALLEL_DETECTION
 #define AUTONEG_SGMII_FIBER_AUTODET \
@@ -112,10 +113,10 @@
 #define GP_STATUS_10G_KX4 \
 			MDIO_GP_STATUS_TOP_AN_STATUS1_ACTUAL_SPEED_10G_KX4
 
-#define LINK_10THD			LINK_STATUS_SPEED_AND_DUPLEX_10THD
-#define LINK_10TFD			LINK_STATUS_SPEED_AND_DUPLEX_10TFD
+#define LINK_10THD		LINK_STATUS_SPEED_AND_DUPLEX_10THD
+#define LINK_10TFD		LINK_STATUS_SPEED_AND_DUPLEX_10TFD
 #define LINK_100TXHD		LINK_STATUS_SPEED_AND_DUPLEX_100TXHD
-#define LINK_100T4			LINK_STATUS_SPEED_AND_DUPLEX_100T4
+#define LINK_100T4		LINK_STATUS_SPEED_AND_DUPLEX_100T4
 #define LINK_100TXFD		LINK_STATUS_SPEED_AND_DUPLEX_100TXFD
 #define LINK_1000THD		LINK_STATUS_SPEED_AND_DUPLEX_1000THD
 #define LINK_1000TFD		LINK_STATUS_SPEED_AND_DUPLEX_1000TFD
@@ -123,18 +124,18 @@
 #define LINK_2500THD		LINK_STATUS_SPEED_AND_DUPLEX_2500THD
 #define LINK_2500TFD		LINK_STATUS_SPEED_AND_DUPLEX_2500TFD
 #define LINK_2500XFD		LINK_STATUS_SPEED_AND_DUPLEX_2500XFD
-#define LINK_10GTFD			LINK_STATUS_SPEED_AND_DUPLEX_10GTFD
-#define LINK_10GXFD			LINK_STATUS_SPEED_AND_DUPLEX_10GXFD
-#define LINK_12GTFD			LINK_STATUS_SPEED_AND_DUPLEX_12GTFD
-#define LINK_12GXFD			LINK_STATUS_SPEED_AND_DUPLEX_12GXFD
+#define LINK_10GTFD		LINK_STATUS_SPEED_AND_DUPLEX_10GTFD
+#define LINK_10GXFD		LINK_STATUS_SPEED_AND_DUPLEX_10GXFD
+#define LINK_12GTFD		LINK_STATUS_SPEED_AND_DUPLEX_12GTFD
+#define LINK_12GXFD		LINK_STATUS_SPEED_AND_DUPLEX_12GXFD
 #define LINK_12_5GTFD		LINK_STATUS_SPEED_AND_DUPLEX_12_5GTFD
 #define LINK_12_5GXFD		LINK_STATUS_SPEED_AND_DUPLEX_12_5GXFD
-#define LINK_13GTFD			LINK_STATUS_SPEED_AND_DUPLEX_13GTFD
-#define LINK_13GXFD			LINK_STATUS_SPEED_AND_DUPLEX_13GXFD
-#define LINK_15GTFD			LINK_STATUS_SPEED_AND_DUPLEX_15GTFD
-#define LINK_15GXFD			LINK_STATUS_SPEED_AND_DUPLEX_15GXFD
-#define LINK_16GTFD			LINK_STATUS_SPEED_AND_DUPLEX_16GTFD
-#define LINK_16GXFD			LINK_STATUS_SPEED_AND_DUPLEX_16GXFD
+#define LINK_13GTFD		LINK_STATUS_SPEED_AND_DUPLEX_13GTFD
+#define LINK_13GXFD		LINK_STATUS_SPEED_AND_DUPLEX_13GXFD
+#define LINK_15GTFD		LINK_STATUS_SPEED_AND_DUPLEX_15GTFD
+#define LINK_15GXFD		LINK_STATUS_SPEED_AND_DUPLEX_15GXFD
+#define LINK_16GTFD		LINK_STATUS_SPEED_AND_DUPLEX_16GTFD
+#define LINK_16GXFD		LINK_STATUS_SPEED_AND_DUPLEX_16GXFD
 
 #define PHY_XGXS_FLAG			0x1
 #define PHY_SGMII_FLAG			0x2
@@ -142,7 +143,7 @@
 
 /* */
 #define SFP_EEPROM_CON_TYPE_ADDR		0x2
-	#define SFP_EEPROM_CON_TYPE_VAL_LC 		0x7
+	#define SFP_EEPROM_CON_TYPE_VAL_LC	0x7
 	#define SFP_EEPROM_CON_TYPE_VAL_COPPER	0x21
 
 
@@ -153,15 +154,15 @@
 
 #define SFP_EEPROM_FC_TX_TECH_ADDR		0x8
 	#define SFP_EEPROM_FC_TX_TECH_BITMASK_COPPER_PASSIVE 0x4
-	#define SFP_EEPROM_FC_TX_TECH_BITMASK_COPPER_ACTIVE	 0x8
+	#define SFP_EEPROM_FC_TX_TECH_BITMASK_COPPER_ACTIVE  0x8
 
-#define SFP_EEPROM_OPTIONS_ADDR 		0x40
+#define SFP_EEPROM_OPTIONS_ADDR			0x40
 	#define SFP_EEPROM_OPTIONS_LINEAR_RX_OUT_MASK 0x1
-#define SFP_EEPROM_OPTIONS_SIZE 		2
+#define SFP_EEPROM_OPTIONS_SIZE			2
 
-#define EDC_MODE_LINEAR	 			0x0022
-#define EDC_MODE_LIMITING	 			0x0044
-#define EDC_MODE_PASSIVE_DAC 			0x0055
+#define EDC_MODE_LINEAR				0x0022
+#define EDC_MODE_LIMITING				0x0044
+#define EDC_MODE_PASSIVE_DAC			0x0055
 
 
 #define ETS_BW_LIMIT_CREDIT_UPPER_BOUND		(0x5000)
@@ -170,24 +171,18 @@
 /*                     INTERFACE                          */
 /**********************************************************/
 
-#define CL45_WR_OVER_CL22(_bp, _phy, _bank, _addr, _val) \
+#define CL22_WR_OVER_CL45(_bp, _phy, _bank, _addr, _val) \
 	bnx2x_cl45_write(_bp, _phy, \
 		(_phy)->def_md_devad, \
 		(_bank + (_addr & 0xf)), \
 		_val)
 
-#define CL45_RD_OVER_CL22(_bp, _phy, _bank, _addr, _val) \
+#define CL22_RD_OVER_CL45(_bp, _phy, _bank, _addr, _val) \
 	bnx2x_cl45_read(_bp, _phy, \
 		(_phy)->def_md_devad, \
 		(_bank + (_addr & 0xf)), \
 		_val)
 
-static u8 bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
-			  u8 devad, u16 reg, u16 *ret_val);
-
-static u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
-			   u8 devad, u16 reg, u16 val);
-
 static u32 bnx2x_bits_en(struct bnx2x *bp, u32 reg, u32 bits)
 {
 	u32 val = REG_RD(bp, reg);
@@ -216,7 +211,7 @@ void bnx2x_ets_disabled(struct link_params *params)
 
 	DP(NETIF_MSG_LINK, "ETS disabled configuration\n");
 
-	/**
+	/*
 	 * mapping between entry  priority to client number (0,1,2 -debug and
 	 * management clients, 3 - COS0 client, 4 - COS client)(HIGHEST)
 	 * 3bits client num.
@@ -225,7 +220,7 @@ void bnx2x_ets_disabled(struct link_params *params)
 	 */
 
 	REG_WR(bp, NIG_REG_P0_TX_ARB_PRIORITY_CLIENT, 0x4688);
-	/**
+	/*
 	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
 	 * as strict.  Bits 0,1,2 - debug and management entries, 3 -
 	 * COS0 entry, 4 - COS1 entry.
@@ -237,12 +232,12 @@ void bnx2x_ets_disabled(struct link_params *params)
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_STRICT, 0x7);
 	/* defines which entries (clients) are subjected to WFQ arbitration */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_SUBJECT2WFQ, 0);
-	/**
-	* For strict priority entries defines the number of consecutive
-	* slots for the highest priority.
-	*/
+	/*
+	 * For strict priority entries defines the number of consecutive
+	 * slots for the highest priority.
+	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_NUM_STRICT_ARB_SLOTS, 0x100);
-	/**
+	/*
 	 * mapping between the CREDIT_WEIGHT registers and actual client
 	 * numbers
 	 */
@@ -255,7 +250,7 @@ void bnx2x_ets_disabled(struct link_params *params)
 	REG_WR(bp, PBF_REG_HIGH_PRIORITY_COS_NUM, 0);
 	/* ETS mode disable */
 	REG_WR(bp, PBF_REG_ETS_ENABLED, 0);
-	/**
+	/*
 	 * If ETS mode is enabled (there is no strict priority) defines a WFQ
 	 * weight for COS0/COS1.
 	 */
@@ -268,24 +263,24 @@ void bnx2x_ets_disabled(struct link_params *params)
 	REG_WR(bp, PBF_REG_NUM_STRICT_ARB_SLOTS, 0);
 }
 
-void bnx2x_ets_bw_limit_common(const struct link_params *params)
+static void bnx2x_ets_bw_limit_common(const struct link_params *params)
 {
 	/* ETS disabled configuration */
 	struct bnx2x *bp = params->bp;
 	DP(NETIF_MSG_LINK, "ETS enabled BW limit configuration\n");
-	/**
-	* defines which entries (clients) are subjected to WFQ arbitration
-	* COS0 0x8
-	* COS1 0x10
-	*/
+	/*
+	 * defines which entries (clients) are subjected to WFQ arbitration
+	 * COS0 0x8
+	 * COS1 0x10
+	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_SUBJECT2WFQ, 0x18);
-	/**
-	* mapping between the ARB_CREDIT_WEIGHT registers and actual
-	* client numbers (WEIGHT_0 does not actually have to represent
-	* client 0)
-	*    PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
-	*  cos1-001     cos0-000     dbg1-100     dbg0-011     MCP-010
-	*/
+	/*
+	 * mapping between the ARB_CREDIT_WEIGHT registers and actual
+	 * client numbers (WEIGHT_0 does not actually have to represent
+	 * client 0)
+	 *    PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
+	 *  cos1-001     cos0-000     dbg1-100     dbg0-011     MCP-010
+	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_CREDIT_MAP, 0x111A);
 
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CREDIT_UPPER_BOUND_0,
@@ -298,14 +293,14 @@ void bnx2x_ets_bw_limit_common(const struct link_params *params)
 
 	/* Defines the number of consecutive slots for the strict priority */
 	REG_WR(bp, PBF_REG_NUM_STRICT_ARB_SLOTS, 0);
-	/**
-	* Bitmap of 5bits length. Each bit specifies whether the entry behaves
-	* as strict.  Bits 0,1,2 - debug and management entries, 3 - COS0
-	* entry, 4 - COS1 entry.
-	* COS1 | COS0 | DEBUG21 | DEBUG0 | MGMT
-	* bit4   bit3	  bit2     bit1	   bit0
-	* MCP and debug are strict
-	*/
+	/*
+	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
+	 * as strict.  Bits 0,1,2 - debug and management entries, 3 - COS0
+	 * entry, 4 - COS1 entry.
+	 * COS1 | COS0 | DEBUG21 | DEBUG0 | MGMT
+	 * bit4   bit3	  bit2     bit1	   bit0
+	 * MCP and debug are strict
+	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_STRICT, 0x7);
 
 	/* Upper bound that COS0_WEIGHT can reach in the WFQ arbiter.*/
@@ -329,8 +324,7 @@ void bnx2x_ets_bw_limit(const struct link_params *params, const u32 cos0_bw,
 	if ((0 == total_bw) ||
 	    (0 == cos0_bw) ||
 	    (0 == cos1_bw)) {
-		DP(NETIF_MSG_LINK,
-		   "bnx2x_ets_bw_limit: Total BW can't be zero\n");
+		DP(NETIF_MSG_LINK, "Total BW can't be zero\n");
 		return;
 	}
 
@@ -355,7 +349,7 @@ u8 bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 	u32 val	= 0;
 
 	DP(NETIF_MSG_LINK, "ETS enabled strict configuration\n");
-	/**
+	/*
 	 * Bitmap of 5bits length. Each bit specifies whether the entry behaves
 	 * as strict.  Bits 0,1,2 - debug and management entries,
 	 * 3 - COS0 entry, 4 - COS1 entry.
@@ -364,7 +358,7 @@ u8 bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 	 * MCP and debug are strict
 	 */
 	REG_WR(bp, NIG_REG_P0_TX_ARB_CLIENT_IS_STRICT, 0x1F);
-	/**
+	/*
 	 * For strict priority entries defines the number of consecutive slots
 	 * for the highest priority.
 	 */
@@ -377,14 +371,14 @@ u8 bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 	/* Defines the number of consecutive slots for the strict priority */
 	REG_WR(bp, PBF_REG_HIGH_PRIORITY_COS_NUM, strict_cos);
 
-	/**
-	* mapping between entry  priority to client number (0,1,2 -debug and
-	* management clients, 3 - COS0 client, 4 - COS client)(HIGHEST)
-	* 3bits client num.
-	*   PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
-	* dbg0-010     dbg1-001     cos1-100     cos0-011     MCP-000
-	* dbg0-010     dbg1-001     cos0-011     cos1-100     MCP-000
-	*/
+	/*
+	 * mapping between entry  priority to client number (0,1,2 -debug and
+	 * management clients, 3 - COS0 client, 4 - COS client)(HIGHEST)
+	 * 3bits client num.
+	 *   PRI4    |    PRI3    |    PRI2    |    PRI1    |    PRI0
+	 * dbg0-010     dbg1-001     cos1-100     cos0-011     MCP-000
+	 * dbg0-010     dbg1-001     cos0-011     cos1-100     MCP-000
+	 */
 	val = (0 == strict_cos) ? 0x2318 : 0x22E0;
 	REG_WR(bp, NIG_REG_P0_TX_ARB_PRIORITY_CLIENT, val);
 
@@ -471,7 +465,7 @@ void bnx2x_pfc_statistic(struct link_params *params, struct link_vars *vars,
 /*			MAC/PBF section				  */
 /******************************************************************/
 static void bnx2x_emac_init(struct link_params *params,
-			   struct link_vars *vars)
+			    struct link_vars *vars)
 {
 	/* reset and unreset the emac core */
 	struct bnx2x *bp = params->bp;
@@ -481,10 +475,10 @@ static void bnx2x_emac_init(struct link_params *params,
 	u16 timeout;
 
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
-		   (MISC_REGISTERS_RESET_REG_2_RST_EMAC0_HARD_CORE << port));
+	       (MISC_REGISTERS_RESET_REG_2_RST_EMAC0_HARD_CORE << port));
 	udelay(5);
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
-		   (MISC_REGISTERS_RESET_REG_2_RST_EMAC0_HARD_CORE << port));
+	       (MISC_REGISTERS_RESET_REG_2_RST_EMAC0_HARD_CORE << port));
 
 	/* init emac - use read-modify-write */
 	/* self clear reset */
@@ -515,7 +509,7 @@ static void bnx2x_emac_init(struct link_params *params,
 }
 
 static u8 bnx2x_emac_enable(struct link_params *params,
-			  struct link_vars *vars, u8 lb)
+			    struct link_vars *vars, u8 lb)
 {
 	struct bnx2x *bp = params->bp;
 	u8 port = params->port;
@@ -527,55 +521,33 @@ static u8 bnx2x_emac_enable(struct link_params *params,
 	/* enable emac and not bmac */
 	REG_WR(bp, NIG_REG_EGRESS_EMAC0_PORT + port*4, 1);
 
-	/* for paladium */
-	if (CHIP_REV_IS_EMUL(bp)) {
-		/* Use lane 1 (of lanes 0-3) */
-		REG_WR(bp, NIG_REG_XGXS_LANE_SEL_P0 + port*4, 1);
-		REG_WR(bp, NIG_REG_XGXS_SERDES0_MODE_SEL +
-			    port*4, 1);
-	}
-	/* for fpga */
-	else
-
-	if (CHIP_REV_IS_FPGA(bp)) {
-		/* Use lane 1 (of lanes 0-3) */
-		DP(NETIF_MSG_LINK, "bnx2x_emac_enable: Setting FPGA\n");
-
-		REG_WR(bp, NIG_REG_XGXS_LANE_SEL_P0 + port*4, 1);
-		REG_WR(bp, NIG_REG_XGXS_SERDES0_MODE_SEL + port*4,
-			    0);
-	} else
 	/* ASIC */
 	if (vars->phy_flags & PHY_XGXS_FLAG) {
 		u32 ser_lane = ((params->lane_config &
-			    PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
-			    PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
+				 PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
+				PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
 
 		DP(NETIF_MSG_LINK, "XGXS\n");
 		/* select the master lanes (out of 0-3) */
-		REG_WR(bp, NIG_REG_XGXS_LANE_SEL_P0 +
-			   port*4, ser_lane);
+		REG_WR(bp, NIG_REG_XGXS_LANE_SEL_P0 + port*4, ser_lane);
 		/* select XGXS */
-		REG_WR(bp, NIG_REG_XGXS_SERDES0_MODE_SEL +
-			   port*4, 1);
+		REG_WR(bp, NIG_REG_XGXS_SERDES0_MODE_SEL + port*4, 1);
 
 	} else { /* SerDes */
 		DP(NETIF_MSG_LINK, "SerDes\n");
 		/* select SerDes */
-		REG_WR(bp, NIG_REG_XGXS_SERDES0_MODE_SEL +
-			   port*4, 0);
+		REG_WR(bp, NIG_REG_XGXS_SERDES0_MODE_SEL + port*4, 0);
 	}
 
 	bnx2x_bits_en(bp, emac_base + EMAC_REG_EMAC_RX_MODE,
-		    EMAC_RX_MODE_RESET);
+		      EMAC_RX_MODE_RESET);
 	bnx2x_bits_en(bp, emac_base + EMAC_REG_EMAC_TX_MODE,
-		    EMAC_TX_MODE_RESET);
+		      EMAC_TX_MODE_RESET);
 
 	if (CHIP_REV_IS_SLOW(bp)) {
 		/* config GMII mode */
 		val = REG_RD(bp, emac_base + EMAC_REG_EMAC_MODE);
-		EMAC_WR(bp, EMAC_REG_EMAC_MODE,
-			    (val | EMAC_MODE_PORT_GMII));
+		EMAC_WR(bp, EMAC_REG_EMAC_MODE, (val | EMAC_MODE_PORT_GMII));
 	} else { /* ASIC */
 		/* pause enable/disable */
 		bnx2x_bits_dis(bp, emac_base + EMAC_REG_EMAC_RX_MODE,
@@ -605,14 +577,14 @@ static u8 bnx2x_emac_enable(struct link_params *params,
 	val = REG_RD(bp, emac_base + EMAC_REG_EMAC_RX_MODE);
 	val |= EMAC_RX_MODE_KEEP_VLAN_TAG | EMAC_RX_MODE_PROMISCUOUS;
 
-	/**
-	* Setting this bit causes MAC control frames (except for pause
-	* frames) to be passed on for processing. This setting has no
-	* affect on the operation of the pause frames. This bit effects
-	* all packets regardless of RX Parser packet sorting logic.
-	* Turn the PFC off to make sure we are in Xon state before
-	* enabling it.
-	*/
+	/*
+	 * Setting this bit causes MAC control frames (except for pause
+	 * frames) to be passed on for processing. This setting has no
+	 * affect on the operation of the pause frames. This bit effects
+	 * all packets regardless of RX Parser packet sorting logic.
+	 * Turn the PFC off to make sure we are in Xon state before
+	 * enabling it.
+	 */
 	EMAC_WR(bp, EMAC_REG_RX_PFC_MODE, 0);
 	if (params->feature_config_flags & FEATURE_CONFIG_PFC_ENABLED) {
 		DP(NETIF_MSG_LINK, "PFC is enabled\n");
@@ -666,16 +638,7 @@ static u8 bnx2x_emac_enable(struct link_params *params,
 	REG_WR(bp, NIG_REG_EMAC0_PAUSE_OUT_EN + port*4, val);
 	REG_WR(bp, NIG_REG_EGRESS_EMAC0_OUT_EN + port*4, 0x1);
 
-	if (CHIP_REV_IS_EMUL(bp)) {
-		/* take the BigMac out of reset */
-		REG_WR(bp,
-			   GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
-			   (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
-
-		/* enable access for bmac registers */
-		REG_WR(bp, NIG_REG_BMAC0_REGS_OUT_EN + port*4, 0x1);
-	} else
-		REG_WR(bp, NIG_REG_BMAC0_REGS_OUT_EN + port*4, 0x0);
+	REG_WR(bp, NIG_REG_BMAC0_REGS_OUT_EN + port*4, 0x0);
 
 	vars->mac_type = MAC_TYPE_EMAC;
 	return 0;
@@ -731,8 +694,7 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 		val |= (1<<5);
 	wb_data[0] = val;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_RX_CONTROL,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_RX_CONTROL, wb_data, 2);
 	udelay(30);
 
 	/* Tx control */
@@ -768,12 +730,12 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_PFC_CONTROL, wb_data, 2);
 
-	/**
-	* Set Time (based unit is 512 bit time) between automatic
-	* re-sending of PP packets amd enable automatic re-send of
-	* Per-Priroity Packet as long as pp_gen is asserted and
-	* pp_disable is low.
-	*/
+	/*
+	 * Set Time (based unit is 512 bit time) between automatic
+	 * re-sending of PP packets amd enable automatic re-send of
+	 * Per-Priroity Packet as long as pp_gen is asserted and
+	 * pp_disable is low.
+	 */
 	val = 0x8000;
 	if (params->feature_config_flags & FEATURE_CONFIG_PFC_ENABLED)
 		val |= (1<<16); /* enable automatic re-send */
@@ -781,7 +743,7 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 	wb_data[0] = val;
 	wb_data[1] = 0;
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_TX_PAUSE_CONTROL,
-			wb_data, 2);
+		    wb_data, 2);
 
 	/* mac control */
 	val = 0x3; /* Enable RX and TX */
@@ -795,8 +757,7 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 
 	wb_data[0] = val;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_BMAC_CONTROL,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_BMAC_CONTROL, wb_data, 2);
 }
 
 static void bnx2x_update_pfc_brb(struct link_params *params,
@@ -825,17 +786,25 @@ static void bnx2x_update_pfc_brb(struct link_params *params,
 			full_xon_th =
 			  PFC_BRB_MAC_FULL_XON_THRESHOLD_NON_PAUSEABLE;
 		}
-	/* The number of free blocks below which the pause signal to class 0
-	   of MAC #n is asserted. n=0,1 */
+	/*
+	 * The number of free blocks below which the pause signal to class 0
+	 * of MAC #n is asserted. n=0,1
+	 */
 	REG_WR(bp, BRB1_REG_PAUSE_0_XOFF_THRESHOLD_0 , pause_xoff_th);
-	/* The number of free blocks above which the pause signal to class 0
-	   of MAC #n is de-asserted. n=0,1 */
+	/*
+	 * The number of free blocks above which the pause signal to class 0
+	 * of MAC #n is de-asserted. n=0,1
+	 */
 	REG_WR(bp, BRB1_REG_PAUSE_0_XON_THRESHOLD_0 , pause_xon_th);
-	/* The number of free blocks below which the full signal to class 0
-	   of MAC #n is asserted. n=0,1 */
+	/*
+	 * The number of free blocks below which the full signal to class 0
+	 * of MAC #n is asserted. n=0,1
+	 */
 	REG_WR(bp, BRB1_REG_FULL_0_XOFF_THRESHOLD_0 , full_xoff_th);
-	/* The number of free blocks above which the full signal to class 0
-	   of MAC #n is de-asserted. n=0,1 */
+	/*
+	 * The number of free blocks above which the full signal to class 0
+	 * of MAC #n is de-asserted. n=0,1
+	 */
 	REG_WR(bp, BRB1_REG_FULL_0_XON_THRESHOLD_0 , full_xon_th);
 
 	if (set_pfc && pfc_params) {
@@ -859,25 +828,25 @@ static void bnx2x_update_pfc_brb(struct link_params *params,
 			full_xon_th =
 			  PFC_BRB_MAC_FULL_XON_THRESHOLD_NON_PAUSEABLE;
 		}
-		/**
+		/*
 		 * The number of free blocks below which the pause signal to
 		 * class 1 of MAC #n is asserted. n=0,1
-		 **/
+		 */
 		REG_WR(bp, BRB1_REG_PAUSE_1_XOFF_THRESHOLD_0, pause_xoff_th);
-		/**
+		/*
 		 * The number of free blocks above which the pause signal to
 		 * class 1 of MAC #n is de-asserted. n=0,1
-		 **/
+		 */
 		REG_WR(bp, BRB1_REG_PAUSE_1_XON_THRESHOLD_0, pause_xon_th);
-		/**
+		/*
 		 * The number of free blocks below which the full signal to
 		 * class 1 of MAC #n is asserted. n=0,1
-		 **/
+		 */
 		REG_WR(bp, BRB1_REG_FULL_1_XOFF_THRESHOLD_0, full_xoff_th);
-		/**
+		/*
 		 * The number of free blocks above which the full signal to
 		 * class 1 of MAC #n is de-asserted. n=0,1
-		 **/
+		 */
 		REG_WR(bp, BRB1_REG_FULL_1_XON_THRESHOLD_0, full_xon_th);
 	}
 }
@@ -896,7 +865,7 @@ static void bnx2x_update_pfc_nig(struct link_params *params,
 		FEATURE_CONFIG_PFC_ENABLED;
 	DP(NETIF_MSG_LINK, "updating pfc nig parameters\n");
 
-	/**
+	/*
 	 * When NIG_LLH0_XCM_MASK_REG_LLHX_XCM_MASK_BCN bit is set
 	 * MAC control frames (that are not pause packets)
 	 * will be forwarded to the XCM.
@@ -904,7 +873,7 @@ static void bnx2x_update_pfc_nig(struct link_params *params,
 	xcm_mask = REG_RD(bp,
 				port ? NIG_REG_LLH1_XCM_MASK :
 				NIG_REG_LLH0_XCM_MASK);
-	/**
+	/*
 	 * nig params will override non PFC params, since it's possible to
 	 * do transition from PFC to SAFC
 	 */
@@ -994,7 +963,7 @@ void bnx2x_update_pfc(struct link_params *params,
 		      struct link_vars *vars,
 		      struct bnx2x_nig_brb_pfc_port_params *pfc_params)
 {
-	/**
+	/*
 	 * The PFC and pause are orthogonal to one another, meaning when
 	 * PFC is enabled, the pause are disabled, and when PFC is
 	 * disabled, pause are set according to the pause result.
@@ -1035,7 +1004,7 @@ void bnx2x_update_pfc(struct link_params *params,
 
 static u8 bnx2x_bmac1_enable(struct link_params *params,
 			     struct link_vars *vars,
-			  u8 is_lb)
+			     u8 is_lb)
 {
 	struct bnx2x *bp = params->bp;
 	u8 port = params->port;
@@ -1049,9 +1018,8 @@ static u8 bnx2x_bmac1_enable(struct link_params *params,
 	/* XGXS control */
 	wb_data[0] = 0x3c;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr +
-		      BIGMAC_REGISTER_BMAC_XGXS_CONTROL,
-		      wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_BMAC_XGXS_CONTROL,
+		    wb_data, 2);
 
 	/* tx MAC SA */
 	wb_data[0] = ((params->mac_addr[2] << 24) |
@@ -1060,8 +1028,7 @@ static u8 bnx2x_bmac1_enable(struct link_params *params,
 			params->mac_addr[5]);
 	wb_data[1] = ((params->mac_addr[0] << 8) |
 			params->mac_addr[1]);
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_TX_SOURCE_ADDR,
-		    wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_TX_SOURCE_ADDR, wb_data, 2);
 
 	/* mac control */
 	val = 0x3;
@@ -1071,43 +1038,30 @@ static u8 bnx2x_bmac1_enable(struct link_params *params,
 	}
 	wb_data[0] = val;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_BMAC_CONTROL,
-		    wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_BMAC_CONTROL, wb_data, 2);
 
 	/* set rx mtu */
 	wb_data[0] = ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_RX_MAX_SIZE,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_RX_MAX_SIZE, wb_data, 2);
 
 	bnx2x_update_pfc_bmac1(params, vars);
 
 	/* set tx mtu */
 	wb_data[0] = ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_TX_MAX_SIZE,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_TX_MAX_SIZE, wb_data, 2);
 
 	/* set cnt max size */
 	wb_data[0] = ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_CNT_MAX_SIZE,
-		    wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_CNT_MAX_SIZE, wb_data, 2);
 
 	/* configure safc */
 	wb_data[0] = 0x1000200;
 	wb_data[1] = 0;
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_RX_LLFC_MSG_FLDS,
 		    wb_data, 2);
-	/* fix for emulation */
-	if (CHIP_REV_IS_EMUL(bp)) {
-		wb_data[0] = 0xf000;
-		wb_data[1] = 0;
-		REG_WR_DMAE(bp,
-			    bmac_addr + BIGMAC_REGISTER_TX_PAUSE_THRESHOLD,
-			    wb_data, 2);
-	}
-
 
 	return 0;
 }
@@ -1126,16 +1080,14 @@ static u8 bnx2x_bmac2_enable(struct link_params *params,
 
 	wb_data[0] = 0;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_BMAC_CONTROL,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_BMAC_CONTROL, wb_data, 2);
 	udelay(30);
 
 	/* XGXS control: Reset phy HW, MDIO registers, PHY PLL and BMAC */
 	wb_data[0] = 0x3c;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr +
-			BIGMAC2_REGISTER_BMAC_XGXS_CONTROL,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_BMAC_XGXS_CONTROL,
+		    wb_data, 2);
 
 	udelay(30);
 
@@ -1147,7 +1099,7 @@ static u8 bnx2x_bmac2_enable(struct link_params *params,
 	wb_data[1] = ((params->mac_addr[0] << 8) |
 			params->mac_addr[1]);
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_TX_SOURCE_ADDR,
-			wb_data, 2);
+		    wb_data, 2);
 
 	udelay(30);
 
@@ -1155,27 +1107,24 @@ static u8 bnx2x_bmac2_enable(struct link_params *params,
 	wb_data[0] = 0x1000200;
 	wb_data[1] = 0;
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_RX_LLFC_MSG_FLDS,
-			wb_data, 2);
+		    wb_data, 2);
 	udelay(30);
 
 	/* set rx mtu */
 	wb_data[0] = ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_RX_MAX_SIZE,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_RX_MAX_SIZE, wb_data, 2);
 	udelay(30);
 
 	/* set tx mtu */
 	wb_data[0] = ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_TX_MAX_SIZE,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_TX_MAX_SIZE, wb_data, 2);
 	udelay(30);
 	/* set cnt max size */
 	wb_data[0] = ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD - 2;
 	wb_data[1] = 0;
-	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_CNT_MAX_SIZE,
-			wb_data, 2);
+	REG_WR_DMAE(bp, bmac_addr + BIGMAC2_REGISTER_CNT_MAX_SIZE, wb_data, 2);
 	udelay(30);
 	bnx2x_update_pfc_bmac2(params, vars, is_lb);
 
@@ -1191,11 +1140,11 @@ static u8 bnx2x_bmac_enable(struct link_params *params,
 	u32 val;
 	/* reset and unreset the BigMac */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
-		     (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
+	       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
 	msleep(1);
 
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
-		     (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
+	       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
 
 	/* enable access for bmac registers */
 	REG_WR(bp, NIG_REG_BMAC0_REGS_OUT_EN + port*4, 0x1);
@@ -1230,15 +1179,14 @@ static void bnx2x_update_mng(struct link_params *params, u32 link_status)
 	struct bnx2x *bp = params->bp;
 
 	REG_WR(bp, params->shmem_base +
-		   offsetof(struct shmem_region,
-			    port_mb[params->port].link_status),
-			link_status);
+	       offsetof(struct shmem_region,
+			port_mb[params->port].link_status), link_status);
 }
 
 static void bnx2x_bmac_rx_disable(struct bnx2x *bp, u8 port)
 {
 	u32 bmac_addr = port ? NIG_REG_INGRESS_BMAC1_MEM :
-		NIG_REG_INGRESS_BMAC0_MEM;
+			NIG_REG_INGRESS_BMAC0_MEM;
 	u32 wb_data[2];
 	u32 nig_bmac_enable = REG_RD(bp, NIG_REG_BMAC0_REGS_OUT_EN + port*4);
 
@@ -1250,12 +1198,12 @@ static void bnx2x_bmac_rx_disable(struct bnx2x *bp, u8 port)
 		if (CHIP_IS_E2(bp)) {
 			/* Clear Rx Enable bit in BMAC_CONTROL register */
 			REG_RD_DMAE(bp, bmac_addr +
-					BIGMAC2_REGISTER_BMAC_CONTROL,
-					wb_data, 2);
+				    BIGMAC2_REGISTER_BMAC_CONTROL,
+				    wb_data, 2);
 			wb_data[0] &= ~BMAC_CONTROL_RX_ENABLE;
 			REG_WR_DMAE(bp, bmac_addr +
-					BIGMAC2_REGISTER_BMAC_CONTROL,
-					wb_data, 2);
+				    BIGMAC2_REGISTER_BMAC_CONTROL,
+				    wb_data, 2);
 		} else {
 			/* Clear Rx Enable bit in BMAC_CONTROL register */
 			REG_RD_DMAE(bp, bmac_addr +
@@ -1271,7 +1219,7 @@ static void bnx2x_bmac_rx_disable(struct bnx2x *bp, u8 port)
 }
 
 static u8 bnx2x_pbf_update(struct link_params *params, u32 flow_ctrl,
-			 u32 line_speed)
+			   u32 line_speed)
 {
 	struct bnx2x *bp = params->bp;
 	u8 port = params->port;
@@ -1308,7 +1256,7 @@ static u8 bnx2x_pbf_update(struct link_params *params, u32 flow_ctrl,
 		/* update threshold */
 		REG_WR(bp, PBF_REG_P0_ARB_THRSH + port*4, 0);
 		/* update init credit */
-		init_crd = 778; 	/* (800-18-4) */
+		init_crd = 778;		/* (800-18-4) */
 
 	} else {
 		u32 thresh = (ETH_MAX_JUMBO_PACKET_SIZE +
@@ -1353,6 +1301,23 @@ static u8 bnx2x_pbf_update(struct link_params *params, u32 flow_ctrl,
 	return 0;
 }
 
+/*
+ * get_emac_base
+ *
+ * @param cb
+ * @param mdc_mdio_access
+ * @param port
+ *
+ * @return u32
+ *
+ * This function selects the MDC/MDIO access (through emac0 or
+ * emac1) depend on the mdc_mdio_access, port, port swapped. Each
+ * phy has a default access mode, which could also be overridden
+ * by nvram configuration. This parameter, whether this is the
+ * default phy configuration, or the nvram overrun
+ * configuration, is passed here as mdc_mdio_access and selects
+ * the emac_base for the CL45 read/writes operations
+ */
 static u32 bnx2x_get_emac_base(struct bnx2x *bp,
 			       u32 mdc_mdio_access, u8 port)
 {
@@ -1385,13 +1350,16 @@ static u32 bnx2x_get_emac_base(struct bnx2x *bp,
 
 }
 
-u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
-		    u8 devad, u16 reg, u16 val)
+/******************************************************************/
+/*			CL45 access functions			  */
+/******************************************************************/
+static u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
+			   u8 devad, u16 reg, u16 val)
 {
 	u32 tmp, saved_mode;
 	u8 i, rc = 0;
-
-	/* set clause 45 mode, slow down the MDIO clock to 2.5MHz
+	/*
+	 * Set clause 45 mode, slow down the MDIO clock to 2.5MHz
 	 * (a value of 49==0x31) and make sure that the AUTO poll is off
 	 */
 
@@ -1414,8 +1382,7 @@ u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 	for (i = 0; i < 50; i++) {
 		udelay(10);
 
-		tmp = REG_RD(bp, phy->mdio_ctrl +
-				   EMAC_REG_EMAC_MDIO_COMM);
+		tmp = REG_RD(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_COMM);
 		if (!(tmp & EMAC_MDIO_COMM_START_BUSY)) {
 			udelay(5);
 			break;
@@ -1423,6 +1390,7 @@ u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 	}
 	if (tmp & EMAC_MDIO_COMM_START_BUSY) {
 		DP(NETIF_MSG_LINK, "write phy register failed\n");
+		netdev_err(bp->dev,  "MDC/MDIO access timeout\n");
 		rc = -EFAULT;
 	} else {
 		/* data */
@@ -1435,7 +1403,7 @@ u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 			udelay(10);
 
 			tmp = REG_RD(bp, phy->mdio_ctrl +
-					 EMAC_REG_EMAC_MDIO_COMM);
+				     EMAC_REG_EMAC_MDIO_COMM);
 			if (!(tmp & EMAC_MDIO_COMM_START_BUSY)) {
 				udelay(5);
 				break;
@@ -1443,6 +1411,7 @@ u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 		}
 		if (tmp & EMAC_MDIO_COMM_START_BUSY) {
 			DP(NETIF_MSG_LINK, "write phy register failed\n");
+			netdev_err(bp->dev,  "MDC/MDIO access timeout\n");
 			rc = -EFAULT;
 		}
 	}
@@ -1453,20 +1422,20 @@ u8 bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 	return rc;
 }
 
-u8 bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
-		   u8 devad, u16 reg, u16 *ret_val)
+static u8 bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
+			  u8 devad, u16 reg, u16 *ret_val)
 {
 	u32 val, saved_mode;
 	u16 i;
 	u8 rc = 0;
-
-	/* set clause 45 mode, slow down the MDIO clock to 2.5MHz
+	/*
+	 * Set clause 45 mode, slow down the MDIO clock to 2.5MHz
 	 * (a value of 49==0x31) and make sure that the AUTO poll is off
 	 */
 
 	saved_mode = REG_RD(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_MODE);
 	val = saved_mode & ~((EMAC_MDIO_MODE_AUTO_POLL |
-			     EMAC_MDIO_MODE_CLOCK_CNT));
+			      EMAC_MDIO_MODE_CLOCK_CNT));
 	val |= (EMAC_MDIO_MODE_CLAUSE_45 |
 		(49L << EMAC_MDIO_MODE_CLOCK_CNT_BITSHIFT));
 	REG_WR(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_MODE, val);
@@ -1490,7 +1459,7 @@ u8 bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
 	}
 	if (val & EMAC_MDIO_COMM_START_BUSY) {
 		DP(NETIF_MSG_LINK, "read phy register failed\n");
-
+		netdev_err(bp->dev,  "MDC/MDIO access timeout\n");
 		*ret_val = 0;
 		rc = -EFAULT;
 
@@ -1505,7 +1474,7 @@ u8 bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
 			udelay(10);
 
 			val = REG_RD(bp, phy->mdio_ctrl +
-					  EMAC_REG_EMAC_MDIO_COMM);
+				     EMAC_REG_EMAC_MDIO_COMM);
 			if (!(val & EMAC_MDIO_COMM_START_BUSY)) {
 				*ret_val = (u16)(val & EMAC_MDIO_COMM_DATA);
 				break;
@@ -1513,7 +1482,7 @@ u8 bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
 		}
 		if (val & EMAC_MDIO_COMM_START_BUSY) {
 			DP(NETIF_MSG_LINK, "read phy register failed\n");
-
+			netdev_err(bp->dev,  "MDC/MDIO access timeout\n");
 			*ret_val = 0;
 			rc = -EFAULT;
 		}
@@ -1529,7 +1498,7 @@ u8 bnx2x_phy_read(struct link_params *params, u8 phy_addr,
 		  u8 devad, u16 reg, u16 *ret_val)
 {
 	u8 phy_index;
-	/**
+	/*
 	 * Probe for the phy according to the given phy_addr, and execute
 	 * the read request on it
 	 */
@@ -1547,7 +1516,7 @@ u8 bnx2x_phy_write(struct link_params *params, u8 phy_addr,
 		   u8 devad, u16 reg, u16 val)
 {
 	u8 phy_index;
-	/**
+	/*
 	 * Probe for the phy according to the given phy_addr, and execute
 	 * the write request on it
 	 */
@@ -1576,16 +1545,15 @@ static void bnx2x_set_aer_mmd_xgxs(struct link_params *params,
 		aer_val = 0x3800 + offset - 1;
 	else
 		aer_val = 0x3800 + offset;
-	CL45_WR_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_AER_BLOCK,
-				MDIO_AER_BLOCK_AER_REG, aer_val);
+	CL22_WR_OVER_CL45(bp, phy, MDIO_REG_BANK_AER_BLOCK,
+			  MDIO_AER_BLOCK_AER_REG, aer_val);
 }
 static void bnx2x_set_aer_mmd_serdes(struct bnx2x *bp,
 				     struct bnx2x_phy *phy)
 {
-	CL45_WR_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_AER_BLOCK,
-				MDIO_AER_BLOCK_AER_REG, 0x3800);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_AER_BLOCK,
+			  MDIO_AER_BLOCK_AER_REG, 0x3800);
 }
 
 /******************************************************************/
@@ -1621,9 +1589,8 @@ static void bnx2x_serdes_deassert(struct bnx2x *bp, u8 port)
 
 	bnx2x_set_serdes_access(bp, port);
 
-	REG_WR(bp, NIG_REG_SERDES0_CTRL_MD_DEVAD +
-		     port*0x10,
-		     DEFAULT_PHY_DEV_ADDR);
+	REG_WR(bp, NIG_REG_SERDES0_CTRL_MD_DEVAD + port*0x10,
+	       DEFAULT_PHY_DEV_ADDR);
 }
 
 static void bnx2x_xgxs_deassert(struct link_params *params)
@@ -1641,23 +1608,22 @@ static void bnx2x_xgxs_deassert(struct link_params *params)
 	udelay(500);
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_SET, val);
 
-	REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_ST +
-		     port*0x18, 0);
+	REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_ST + port*0x18, 0);
 	REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + port*0x18,
-		     params->phy[INT_PHY].def_md_devad);
+	       params->phy[INT_PHY].def_md_devad);
 }
 
 
 void bnx2x_link_status_update(struct link_params *params,
-			    struct link_vars   *vars)
+			      struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
 	u8 link_10g;
 	u8 port = params->port;
 
 	vars->link_status = REG_RD(bp, params->shmem_base +
-					  offsetof(struct shmem_region,
-					   port_mb[port].link_status));
+				   offsetof(struct shmem_region,
+					    port_mb[port].link_status));
 
 	vars->link_up = (vars->link_status & LINK_STATUS_LINK_UP);
 
@@ -1667,7 +1633,7 @@ void bnx2x_link_status_update(struct link_params *params,
 		vars->phy_link_up = 1;
 		vars->duplex = DUPLEX_FULL;
 		switch (vars->link_status &
-					LINK_STATUS_SPEED_AND_DUPLEX_MASK) {
+			LINK_STATUS_SPEED_AND_DUPLEX_MASK) {
 			case LINK_10THD:
 				vars->duplex = DUPLEX_HALF;
 				/* fall thru */
@@ -1779,20 +1745,20 @@ static void bnx2x_set_master_ln(struct link_params *params,
 {
 	struct bnx2x *bp = params->bp;
 	u16 new_master_ln, ser_lane;
-	ser_lane =  ((params->lane_config &
+	ser_lane = ((params->lane_config &
 		     PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
-		     PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
+		    PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
 
 	/* set the master_ln for AN */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_XGXS_BLOCK2,
-			      MDIO_XGXS_BLOCK2_TEST_MODE_LANE,
-			      &new_master_ln);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_XGXS_BLOCK2,
+			  MDIO_XGXS_BLOCK2_TEST_MODE_LANE,
+			  &new_master_ln);
 
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_XGXS_BLOCK2 ,
-			      MDIO_XGXS_BLOCK2_TEST_MODE_LANE,
-			      (new_master_ln | ser_lane));
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_XGXS_BLOCK2 ,
+			  MDIO_XGXS_BLOCK2_TEST_MODE_LANE,
+			  (new_master_ln | ser_lane));
 }
 
 static u8 bnx2x_reset_unicore(struct link_params *params,
@@ -1802,17 +1768,16 @@ static u8 bnx2x_reset_unicore(struct link_params *params,
 	struct bnx2x *bp = params->bp;
 	u16 mii_control;
 	u16 i;
-
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL, &mii_control);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_MII_CONTROL, &mii_control);
 
 	/* reset the unicore */
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL,
-			      (mii_control |
-			       MDIO_COMBO_IEEO_MII_CONTROL_RESET));
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_MII_CONTROL,
+			  (mii_control |
+			   MDIO_COMBO_IEEO_MII_CONTROL_RESET));
 	if (set_serdes)
 		bnx2x_set_serdes_access(bp, params->port);
 
@@ -1821,10 +1786,10 @@ static u8 bnx2x_reset_unicore(struct link_params *params,
 		udelay(5);
 
 		/* the reset erased the previous bank value */
-		CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL,
-			      &mii_control);
+		CL22_RD_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_COMBO_IEEE0,
+				  MDIO_COMBO_IEEE0_MII_CONTROL,
+				  &mii_control);
 
 		if (!(mii_control & MDIO_COMBO_IEEO_MII_CONTROL_RESET)) {
 			udelay(5);
@@ -1832,6 +1797,9 @@ static u8 bnx2x_reset_unicore(struct link_params *params,
 		}
 	}
 
+	netdev_err(bp->dev,  "Warning: PHY was not initialized,"
+			      " Port %d\n",
+			 params->port);
 	DP(NETIF_MSG_LINK, "BUG! XGXS is still in reset!\n");
 	return -EINVAL;
 
@@ -1841,43 +1809,45 @@ static void bnx2x_set_swap_lanes(struct link_params *params,
 				 struct bnx2x_phy *phy)
 {
 	struct bnx2x *bp = params->bp;
-	/* Each two bits represents a lane number:
-	   No swap is 0123 => 0x1b no need to enable the swap */
+	/*
+	 *  Each two bits represents a lane number:
+	 *  No swap is 0123 => 0x1b no need to enable the swap
+	 */
 	u16 ser_lane, rx_lane_swap, tx_lane_swap;
 
 	ser_lane = ((params->lane_config &
-			 PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
-			PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
+		     PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
+		    PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
 	rx_lane_swap = ((params->lane_config &
-			     PORT_HW_CFG_LANE_SWAP_CFG_RX_MASK) >>
-			    PORT_HW_CFG_LANE_SWAP_CFG_RX_SHIFT);
+			 PORT_HW_CFG_LANE_SWAP_CFG_RX_MASK) >>
+			PORT_HW_CFG_LANE_SWAP_CFG_RX_SHIFT);
 	tx_lane_swap = ((params->lane_config &
-			     PORT_HW_CFG_LANE_SWAP_CFG_TX_MASK) >>
-			    PORT_HW_CFG_LANE_SWAP_CFG_TX_SHIFT);
+			 PORT_HW_CFG_LANE_SWAP_CFG_TX_MASK) >>
+			PORT_HW_CFG_LANE_SWAP_CFG_TX_SHIFT);
 
 	if (rx_lane_swap != 0x1b) {
-		CL45_WR_OVER_CL22(bp, phy,
-				    MDIO_REG_BANK_XGXS_BLOCK2,
-				    MDIO_XGXS_BLOCK2_RX_LN_SWAP,
-				    (rx_lane_swap |
-				    MDIO_XGXS_BLOCK2_RX_LN_SWAP_ENABLE |
-				    MDIO_XGXS_BLOCK2_RX_LN_SWAP_FORCE_ENABLE));
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_XGXS_BLOCK2,
+				  MDIO_XGXS_BLOCK2_RX_LN_SWAP,
+				  (rx_lane_swap |
+				   MDIO_XGXS_BLOCK2_RX_LN_SWAP_ENABLE |
+				   MDIO_XGXS_BLOCK2_RX_LN_SWAP_FORCE_ENABLE));
 	} else {
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_XGXS_BLOCK2,
-				      MDIO_XGXS_BLOCK2_RX_LN_SWAP, 0);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_XGXS_BLOCK2,
+				  MDIO_XGXS_BLOCK2_RX_LN_SWAP, 0);
 	}
 
 	if (tx_lane_swap != 0x1b) {
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_XGXS_BLOCK2,
-				      MDIO_XGXS_BLOCK2_TX_LN_SWAP,
-				      (tx_lane_swap |
-				       MDIO_XGXS_BLOCK2_TX_LN_SWAP_ENABLE));
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_XGXS_BLOCK2,
+				  MDIO_XGXS_BLOCK2_TX_LN_SWAP,
+				  (tx_lane_swap |
+				   MDIO_XGXS_BLOCK2_TX_LN_SWAP_ENABLE));
 	} else {
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_XGXS_BLOCK2,
-				      MDIO_XGXS_BLOCK2_TX_LN_SWAP, 0);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_XGXS_BLOCK2,
+				  MDIO_XGXS_BLOCK2_TX_LN_SWAP, 0);
 	}
 }
 
@@ -1886,66 +1856,66 @@ static void bnx2x_set_parallel_detection(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u16 control2;
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_CONTROL2,
-			      &control2);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL2,
+			  &control2);
 	if (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_1G)
 		control2 |= MDIO_SERDES_DIGITAL_A_1000X_CONTROL2_PRL_DT_EN;
 	else
 		control2 &= ~MDIO_SERDES_DIGITAL_A_1000X_CONTROL2_PRL_DT_EN;
 	DP(NETIF_MSG_LINK, "phy->speed_cap_mask = 0x%x, control2 = 0x%x\n",
 		phy->speed_cap_mask, control2);
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_CONTROL2,
-			      control2);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL2,
+			  control2);
 
 	if ((phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT) &&
 	     (phy->speed_cap_mask &
 		    PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)) {
 		DP(NETIF_MSG_LINK, "XGXS\n");
 
-		CL45_WR_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_10G_PARALLEL_DETECT,
-				MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_LINK,
-				MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_LINK_CNT);
+		CL22_WR_OVER_CL45(bp, phy,
+				 MDIO_REG_BANK_10G_PARALLEL_DETECT,
+				 MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_LINK,
+				 MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_LINK_CNT);
 
-		CL45_RD_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_10G_PARALLEL_DETECT,
-				MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_CONTROL,
-				&control2);
+		CL22_RD_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_10G_PARALLEL_DETECT,
+				  MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_CONTROL,
+				  &control2);
 
 
 		control2 |=
 		    MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_CONTROL_PARDET10G_EN;
 
-		CL45_WR_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_10G_PARALLEL_DETECT,
-				MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_CONTROL,
-				control2);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_10G_PARALLEL_DETECT,
+				  MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_CONTROL,
+				  control2);
 
 		/* Disable parallel detection of HiG */
-		CL45_WR_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_XGXS_BLOCK2,
-				MDIO_XGXS_BLOCK2_UNICORE_MODE_10G,
-				MDIO_XGXS_BLOCK2_UNICORE_MODE_10G_CX4_XGXS |
-				MDIO_XGXS_BLOCK2_UNICORE_MODE_10G_HIGIG_XGXS);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_XGXS_BLOCK2,
+				  MDIO_XGXS_BLOCK2_UNICORE_MODE_10G,
+				  MDIO_XGXS_BLOCK2_UNICORE_MODE_10G_CX4_XGXS |
+				  MDIO_XGXS_BLOCK2_UNICORE_MODE_10G_HIGIG_XGXS);
 	}
 }
 
 static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 			      struct link_params *params,
-			    struct link_vars *vars,
-			    u8 enable_cl73)
+			      struct link_vars *vars,
+			      u8 enable_cl73)
 {
 	struct bnx2x *bp = params->bp;
 	u16 reg_val;
 
 	/* CL37 Autoneg */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL, &reg_val);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_MII_CONTROL, &reg_val);
 
 	/* CL37 Autoneg Enabled */
 	if (vars->line_speed == SPEED_AUTO_NEG)
@@ -1954,15 +1924,15 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 		reg_val &= ~(MDIO_COMBO_IEEO_MII_CONTROL_AN_EN |
 			     MDIO_COMBO_IEEO_MII_CONTROL_RESTART_AN);
 
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL, reg_val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_MII_CONTROL, reg_val);
 
 	/* Enable/Disable Autodetection */
 
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_CONTROL1, &reg_val);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL1, &reg_val);
 	reg_val &= ~(MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_SIGNAL_DETECT_EN |
 		    MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_INVERT_SIGNAL_DETECT);
 	reg_val |= MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_FIBER_MODE;
@@ -1971,14 +1941,14 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 	else
 		reg_val &= ~MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_AUTODET;
 
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_CONTROL1, reg_val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL1, reg_val);
 
 	/* Enable TetonII and BAM autoneg */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_BAM_NEXT_PAGE,
-			      MDIO_BAM_NEXT_PAGE_MP5_NEXT_PAGE_CTRL,
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_BAM_NEXT_PAGE,
+			  MDIO_BAM_NEXT_PAGE_MP5_NEXT_PAGE_CTRL,
 			  &reg_val);
 	if (vars->line_speed == SPEED_AUTO_NEG) {
 		/* Enable BAM aneg Mode and TetonII aneg Mode */
@@ -1989,20 +1959,20 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 		reg_val &= ~(MDIO_BAM_NEXT_PAGE_MP5_NEXT_PAGE_CTRL_BAM_MODE |
 			     MDIO_BAM_NEXT_PAGE_MP5_NEXT_PAGE_CTRL_TETON_AN);
 	}
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_BAM_NEXT_PAGE,
-			      MDIO_BAM_NEXT_PAGE_MP5_NEXT_PAGE_CTRL,
-			      reg_val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_BAM_NEXT_PAGE,
+			  MDIO_BAM_NEXT_PAGE_MP5_NEXT_PAGE_CTRL,
+			  reg_val);
 
 	if (enable_cl73) {
 		/* Enable Cl73 FSM status bits */
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_CL73_USERB0,
-				    MDIO_CL73_USERB0_CL73_UCTRL,
-				      0xe);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_CL73_USERB0,
+				  MDIO_CL73_USERB0_CL73_UCTRL,
+				  0xe);
 
 		/* Enable BAM Station Manager*/
-		CL45_WR_OVER_CL22(bp, phy,
+		CL22_WR_OVER_CL45(bp, phy,
 			MDIO_REG_BANK_CL73_USERB0,
 			MDIO_CL73_USERB0_CL73_BAM_CTRL1,
 			MDIO_CL73_USERB0_CL73_BAM_CTRL1_BAM_EN |
@@ -2010,10 +1980,10 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 			MDIO_CL73_USERB0_CL73_BAM_CTRL1_BAM_NP_AFTER_BP_EN);
 
 		/* Advertise CL73 link speeds */
-		CL45_RD_OVER_CL22(bp, phy,
-					      MDIO_REG_BANK_CL73_IEEEB1,
-					      MDIO_CL73_IEEEB1_AN_ADV2,
-					      &reg_val);
+		CL22_RD_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_CL73_IEEEB1,
+				  MDIO_CL73_IEEEB1_AN_ADV2,
+				  &reg_val);
 		if (phy->speed_cap_mask &
 		    PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)
 			reg_val |= MDIO_CL73_IEEEB1_AN_ADV2_ADVR_10G_KX4;
@@ -2021,10 +1991,10 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 		    PORT_HW_CFG_SPEED_CAPABILITY_D0_1G)
 			reg_val |= MDIO_CL73_IEEEB1_AN_ADV2_ADVR_1000M_KX;
 
-		CL45_WR_OVER_CL22(bp, phy,
-					      MDIO_REG_BANK_CL73_IEEEB1,
-					      MDIO_CL73_IEEEB1_AN_ADV2,
-				      reg_val);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_CL73_IEEEB1,
+				  MDIO_CL73_IEEEB1_AN_ADV2,
+				  reg_val);
 
 		/* CL73 Autoneg Enabled */
 		reg_val = MDIO_CL73_IEEEB0_CL73_AN_CONTROL_AN_EN;
@@ -2032,37 +2002,39 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 	} else /* CL73 Autoneg Disabled */
 		reg_val = 0;
 
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_CL73_IEEEB0,
-			      MDIO_CL73_IEEEB0_CL73_AN_CONTROL, reg_val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_CL73_IEEEB0,
+			  MDIO_CL73_IEEEB0_CL73_AN_CONTROL, reg_val);
 }
 
 /* program SerDes, forced speed */
 static void bnx2x_program_serdes(struct bnx2x_phy *phy,
 				 struct link_params *params,
-			       struct link_vars *vars)
+				 struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
 	u16 reg_val;
 
 	/* program duplex, disable autoneg and sgmii*/
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL, &reg_val);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_MII_CONTROL, &reg_val);
 	reg_val &= ~(MDIO_COMBO_IEEO_MII_CONTROL_FULL_DUPLEX |
 		     MDIO_COMBO_IEEO_MII_CONTROL_AN_EN |
 		     MDIO_COMBO_IEEO_MII_CONTROL_MAN_SGMII_SP_MASK);
 	if (phy->req_duplex == DUPLEX_FULL)
 		reg_val |= MDIO_COMBO_IEEO_MII_CONTROL_FULL_DUPLEX;
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_MII_CONTROL, reg_val);
-
-	/* program speed
-	   - needed only if the speed is greater than 1G (2.5G or 10G) */
-	CL45_RD_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_SERDES_DIGITAL,
-				      MDIO_SERDES_DIGITAL_MISC1, &reg_val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_MII_CONTROL, reg_val);
+
+	/*
+	 * program speed
+	 *  - needed only if the speed is greater than 1G (2.5G or 10G)
+	 */
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_MISC1, &reg_val);
 	/* clearing the speed value before setting the right speed */
 	DP(NETIF_MSG_LINK, "MDIO_REG_BANK_SERDES_DIGITAL = 0x%x\n", reg_val);
 
@@ -2083,9 +2055,9 @@ static void bnx2x_program_serdes(struct bnx2x_phy *phy,
 				MDIO_SERDES_DIGITAL_MISC1_FORCE_SPEED_13G;
 	}
 
-	CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_SERDES_DIGITAL,
-				      MDIO_SERDES_DIGITAL_MISC1, reg_val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_MISC1, reg_val);
 
 }
 
@@ -2102,13 +2074,13 @@ static void bnx2x_set_brcm_cl37_advertisment(struct bnx2x_phy *phy,
 		val |= MDIO_OVER_1G_UP1_2_5G;
 	if (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)
 		val |= MDIO_OVER_1G_UP1_10G;
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_OVER_1G,
-			      MDIO_OVER_1G_UP1, val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_OVER_1G,
+			  MDIO_OVER_1G_UP1, val);
 
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_OVER_1G,
-			      MDIO_OVER_1G_UP3, 0x400);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_OVER_1G,
+			  MDIO_OVER_1G_UP3, 0x400);
 }
 
 static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
@@ -2116,22 +2088,21 @@ static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	*ieee_fc = MDIO_COMBO_IEEE0_AUTO_NEG_ADV_FULL_DUPLEX;
-	/* resolve pause mode and advertisement
-	 * Please refer to Table 28B-3 of the 802.3ab-1999 spec */
+	/*
+	 * Resolve pause mode and advertisement.
+	 * Please refer to Table 28B-3 of the 802.3ab-1999 spec
+	 */
 
 	switch (phy->req_flow_ctrl) {
 	case BNX2X_FLOW_CTRL_AUTO:
-		if (params->req_fc_auto_adv == BNX2X_FLOW_CTRL_BOTH) {
-			*ieee_fc |=
-			     MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH;
-		} else {
+		if (params->req_fc_auto_adv == BNX2X_FLOW_CTRL_BOTH)
+			*ieee_fc |= MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH;
+		else
 			*ieee_fc |=
-		       MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
-		}
+			MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
 		break;
 	case BNX2X_FLOW_CTRL_TX:
-		*ieee_fc |=
-		       MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
+		*ieee_fc |= MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
 		break;
 
 	case BNX2X_FLOW_CTRL_RX:
@@ -2149,23 +2120,23 @@ static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
 
 static void bnx2x_set_ieee_aneg_advertisment(struct bnx2x_phy *phy,
 					     struct link_params *params,
-					   u16 ieee_fc)
+					     u16 ieee_fc)
 {
 	struct bnx2x *bp = params->bp;
 	u16 val;
 	/* for AN, we are always publishing full duplex */
 
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_COMBO_IEEE0,
-			      MDIO_COMBO_IEEE0_AUTO_NEG_ADV, ieee_fc);
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_CL73_IEEEB1,
-			      MDIO_CL73_IEEEB1_AN_ADV1, &val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_COMBO_IEEE0,
+			  MDIO_COMBO_IEEE0_AUTO_NEG_ADV, ieee_fc);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_CL73_IEEEB1,
+			  MDIO_CL73_IEEEB1_AN_ADV1, &val);
 	val &= ~MDIO_CL73_IEEEB1_AN_ADV1_PAUSE_BOTH;
 	val |= ((ieee_fc<<3) & MDIO_CL73_IEEEB1_AN_ADV1_PAUSE_MASK);
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_CL73_IEEEB1,
-			      MDIO_CL73_IEEEB1_AN_ADV1, val);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_CL73_IEEEB1,
+			  MDIO_CL73_IEEEB1_AN_ADV1, val);
 }
 
 static void bnx2x_restart_autoneg(struct bnx2x_phy *phy,
@@ -2179,67 +2150,67 @@ static void bnx2x_restart_autoneg(struct bnx2x_phy *phy,
 	/* Enable and restart BAM/CL37 aneg */
 
 	if (enable_cl73) {
-		CL45_RD_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_CL73_IEEEB0,
-				      MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
-				      &mii_control);
-
-		CL45_WR_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_CL73_IEEEB0,
-				MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
-				(mii_control |
-				MDIO_CL73_IEEEB0_CL73_AN_CONTROL_AN_EN |
-				MDIO_CL73_IEEEB0_CL73_AN_CONTROL_RESTART_AN));
+		CL22_RD_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_CL73_IEEEB0,
+				  MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
+				  &mii_control);
+
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_CL73_IEEEB0,
+				  MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
+				  (mii_control |
+				  MDIO_CL73_IEEEB0_CL73_AN_CONTROL_AN_EN |
+				  MDIO_CL73_IEEEB0_CL73_AN_CONTROL_RESTART_AN));
 	} else {
 
-		CL45_RD_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_COMBO_IEEE0,
-				      MDIO_COMBO_IEEE0_MII_CONTROL,
-				      &mii_control);
+		CL22_RD_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_COMBO_IEEE0,
+				  MDIO_COMBO_IEEE0_MII_CONTROL,
+				  &mii_control);
 		DP(NETIF_MSG_LINK,
 			 "bnx2x_restart_autoneg mii_control before = 0x%x\n",
 			 mii_control);
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_COMBO_IEEE0,
-				      MDIO_COMBO_IEEE0_MII_CONTROL,
-				      (mii_control |
-				       MDIO_COMBO_IEEO_MII_CONTROL_AN_EN |
-				       MDIO_COMBO_IEEO_MII_CONTROL_RESTART_AN));
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_COMBO_IEEE0,
+				  MDIO_COMBO_IEEE0_MII_CONTROL,
+				  (mii_control |
+				   MDIO_COMBO_IEEO_MII_CONTROL_AN_EN |
+				   MDIO_COMBO_IEEO_MII_CONTROL_RESTART_AN));
 	}
 }
 
 static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 					   struct link_params *params,
-					 struct link_vars *vars)
+					   struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
 	u16 control1;
 
 	/* in SGMII mode, the unicore is always slave */
 
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_CONTROL1,
-		      &control1);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL1,
+			  &control1);
 	control1 |= MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_INVERT_SIGNAL_DETECT;
 	/* set sgmii mode (and not fiber) */
 	control1 &= ~(MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_FIBER_MODE |
 		      MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_AUTODET |
 		      MDIO_SERDES_DIGITAL_A_1000X_CONTROL1_MSTR_MODE);
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_CONTROL1,
-			      control1);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL1,
+			  control1);
 
 	/* if forced speed */
 	if (!(vars->line_speed == SPEED_AUTO_NEG)) {
 		/* set speed, disable autoneg */
 		u16 mii_control;
 
-		CL45_RD_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_COMBO_IEEE0,
-				      MDIO_COMBO_IEEE0_MII_CONTROL,
-				      &mii_control);
+		CL22_RD_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_COMBO_IEEE0,
+				  MDIO_COMBO_IEEE0_MII_CONTROL,
+				  &mii_control);
 		mii_control &= ~(MDIO_COMBO_IEEO_MII_CONTROL_AN_EN |
 				 MDIO_COMBO_IEEO_MII_CONTROL_MAN_SGMII_SP_MASK|
 				 MDIO_COMBO_IEEO_MII_CONTROL_FULL_DUPLEX);
@@ -2267,10 +2238,10 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 		if (phy->req_duplex == DUPLEX_FULL)
 			mii_control |=
 				MDIO_COMBO_IEEO_MII_CONTROL_FULL_DUPLEX;
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_COMBO_IEEE0,
-				      MDIO_COMBO_IEEE0_MII_CONTROL,
-				      mii_control);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_COMBO_IEEE0,
+				  MDIO_COMBO_IEEE0_MII_CONTROL,
+				  mii_control);
 
 	} else { /* AN mode */
 		/* enable and restart AN */
@@ -2285,19 +2256,19 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 
 static void bnx2x_pause_resolve(struct link_vars *vars, u32 pause_result)
 {						/*  LD	    LP	 */
-	switch (pause_result) { 		/* ASYM P ASYM P */
-	case 0xb:       			/*   1  0   1  1 */
+	switch (pause_result) {			/* ASYM P ASYM P */
+	case 0xb:				/*   1  0   1  1 */
 		vars->flow_ctrl = BNX2X_FLOW_CTRL_TX;
 		break;
 
-	case 0xe:       			/*   1  1   1  0 */
+	case 0xe:				/*   1  1   1  0 */
 		vars->flow_ctrl = BNX2X_FLOW_CTRL_RX;
 		break;
 
-	case 0x5:       			/*   0  1   0  1 */
-	case 0x7:       			/*   0  1   1  1 */
-	case 0xd:       			/*   1  1   0  1 */
-	case 0xf:       			/*   1  1   1  1 */
+	case 0x5:				/*   0  1   0  1 */
+	case 0x7:				/*   0  1   1  1 */
+	case 0xd:				/*   1  1   0  1 */
+	case 0xf:				/*   1  1   1  1 */
 		vars->flow_ctrl = BNX2X_FLOW_CTRL_BOTH;
 		break;
 
@@ -2317,24 +2288,24 @@ static u8 bnx2x_direct_parallel_detect_used(struct bnx2x_phy *phy,
 	u16 pd_10g, status2_1000x;
 	if (phy->req_line_speed != SPEED_AUTO_NEG)
 		return 0;
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_STATUS2,
-			      &status2_1000x);
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_SERDES_DIGITAL,
-			      MDIO_SERDES_DIGITAL_A_1000X_STATUS2,
-			      &status2_1000x);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_STATUS2,
+			  &status2_1000x);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_SERDES_DIGITAL,
+			  MDIO_SERDES_DIGITAL_A_1000X_STATUS2,
+			  &status2_1000x);
 	if (status2_1000x & MDIO_SERDES_DIGITAL_A_1000X_STATUS2_AN_DISABLED) {
 		DP(NETIF_MSG_LINK, "1G parallel detect link on port %d\n",
 			 params->port);
 		return 1;
 	}
 
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_10G_PARALLEL_DETECT,
-			      MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_STATUS,
-			      &pd_10g);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_10G_PARALLEL_DETECT,
+			  MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_STATUS,
+			  &pd_10g);
 
 	if (pd_10g & MDIO_10G_PARALLEL_DETECT_PAR_DET_10G_STATUS_PD_LINK) {
 		DP(NETIF_MSG_LINK, "10G parallel detect link on port %d\n",
@@ -2373,14 +2344,14 @@ static void bnx2x_flow_ctrl_resolve(struct bnx2x_phy *phy,
 		    (MDIO_GP_STATUS_TOP_AN_STATUS1_CL73_AUTONEG_COMPLETE |
 		     MDIO_GP_STATUS_TOP_AN_STATUS1_CL73_MR_LP_NP_AN_ABLE)) {
 
-			CL45_RD_OVER_CL22(bp, phy,
-					      MDIO_REG_BANK_CL73_IEEEB1,
-					      MDIO_CL73_IEEEB1_AN_ADV1,
-					      &ld_pause);
-			CL45_RD_OVER_CL22(bp, phy,
-					     MDIO_REG_BANK_CL73_IEEEB1,
-					     MDIO_CL73_IEEEB1_AN_LP_ADV1,
-					     &lp_pause);
+			CL22_RD_OVER_CL45(bp, phy,
+					  MDIO_REG_BANK_CL73_IEEEB1,
+					  MDIO_CL73_IEEEB1_AN_ADV1,
+					  &ld_pause);
+			CL22_RD_OVER_CL45(bp, phy,
+					  MDIO_REG_BANK_CL73_IEEEB1,
+					  MDIO_CL73_IEEEB1_AN_LP_ADV1,
+					  &lp_pause);
 			pause_result = (ld_pause &
 					MDIO_CL73_IEEEB1_AN_ADV1_PAUSE_MASK)
 					>> 8;
@@ -2390,18 +2361,18 @@ static void bnx2x_flow_ctrl_resolve(struct bnx2x_phy *phy,
 			DP(NETIF_MSG_LINK, "pause_result CL73 0x%x\n",
 				 pause_result);
 		} else {
-			CL45_RD_OVER_CL22(bp, phy,
-					      MDIO_REG_BANK_COMBO_IEEE0,
-					      MDIO_COMBO_IEEE0_AUTO_NEG_ADV,
-					      &ld_pause);
-			CL45_RD_OVER_CL22(bp, phy,
-			       MDIO_REG_BANK_COMBO_IEEE0,
-			       MDIO_COMBO_IEEE0_AUTO_NEG_LINK_PARTNER_ABILITY1,
-			       &lp_pause);
+			CL22_RD_OVER_CL45(bp, phy,
+					  MDIO_REG_BANK_COMBO_IEEE0,
+					  MDIO_COMBO_IEEE0_AUTO_NEG_ADV,
+					  &ld_pause);
+			CL22_RD_OVER_CL45(bp, phy,
+				MDIO_REG_BANK_COMBO_IEEE0,
+				MDIO_COMBO_IEEE0_AUTO_NEG_LINK_PARTNER_ABILITY1,
+				&lp_pause);
 			pause_result = (ld_pause &
 				MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_MASK)>>5;
 			pause_result |= (lp_pause &
-				 MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_MASK)>>7;
+				MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_MASK)>>7;
 			DP(NETIF_MSG_LINK, "pause_result CL37 0x%x\n",
 				 pause_result);
 		}
@@ -2417,25 +2388,25 @@ static void bnx2x_check_fallback_to_cl37(struct bnx2x_phy *phy,
 	u16 rx_status, ustat_val, cl37_fsm_recieved;
 	DP(NETIF_MSG_LINK, "bnx2x_check_fallback_to_cl37\n");
 	/* Step 1: Make sure signal is detected */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_RX0,
-			      MDIO_RX0_RX_STATUS,
-			      &rx_status);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_RX0,
+			  MDIO_RX0_RX_STATUS,
+			  &rx_status);
 	if ((rx_status & MDIO_RX0_RX_STATUS_SIGDET) !=
 	    (MDIO_RX0_RX_STATUS_SIGDET)) {
 		DP(NETIF_MSG_LINK, "Signal is not detected. Restoring CL73."
 			     "rx_status(0x80b0) = 0x%x\n", rx_status);
-		CL45_WR_OVER_CL22(bp, phy,
-				      MDIO_REG_BANK_CL73_IEEEB0,
-				      MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
-				      MDIO_CL73_IEEEB0_CL73_AN_CONTROL_AN_EN);
+		CL22_WR_OVER_CL45(bp, phy,
+				  MDIO_REG_BANK_CL73_IEEEB0,
+				  MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
+				  MDIO_CL73_IEEEB0_CL73_AN_CONTROL_AN_EN);
 		return;
 	}
 	/* Step 2: Check CL73 state machine */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_CL73_USERB0,
-			      MDIO_CL73_USERB0_CL73_USTAT1,
-			      &ustat_val);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_CL73_USERB0,
+			  MDIO_CL73_USERB0_CL73_USTAT1,
+			  &ustat_val);
 	if ((ustat_val &
 	     (MDIO_CL73_USERB0_CL73_USTAT1_LINK_STATUS_CHECK |
 	      MDIO_CL73_USERB0_CL73_USTAT1_AN_GOOD_CHECK_BAM37)) !=
@@ -2445,12 +2416,14 @@ static void bnx2x_check_fallback_to_cl37(struct bnx2x_phy *phy,
 			     "ustat_val(0x8371) = 0x%x\n", ustat_val);
 		return;
 	}
-	/* Step 3: Check CL37 Message Pages received to indicate LP
-	supports only CL37 */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_REMOTE_PHY,
-			      MDIO_REMOTE_PHY_MISC_RX_STATUS,
-			      &cl37_fsm_recieved);
+	/*
+	 * Step 3: Check CL37 Message Pages received to indicate LP
+	 * supports only CL37
+	 */
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_REMOTE_PHY,
+			  MDIO_REMOTE_PHY_MISC_RX_STATUS,
+			  &cl37_fsm_recieved);
 	if ((cl37_fsm_recieved &
 	     (MDIO_REMOTE_PHY_MISC_RX_STATUS_CL37_FSM_RECEIVED_OVER1G_MSG |
 	     MDIO_REMOTE_PHY_MISC_RX_STATUS_CL37_FSM_RECEIVED_BRCM_OUI_MSG)) !=
@@ -2461,14 +2434,18 @@ static void bnx2x_check_fallback_to_cl37(struct bnx2x_phy *phy,
 			 cl37_fsm_recieved);
 		return;
 	}
-	/* The combined cl37/cl73 fsm state information indicating that we are
-	connected to a device which does not support cl73, but does support
-	cl37 BAM. In this case we disable cl73 and restart cl37 auto-neg */
+	/*
+	 * The combined cl37/cl73 fsm state information indicating that
+	 * we are connected to a device which does not support cl73, but
+	 * does support cl37 BAM. In this case we disable cl73 and
+	 * restart cl37 auto-neg
+	 */
+
 	/* Disable CL73 */
-	CL45_WR_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_CL73_IEEEB0,
-			      MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
-			      0);
+	CL22_WR_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_CL73_IEEEB0,
+			  MDIO_CL73_IEEEB0_CL73_AN_CONTROL,
+			  0);
 	/* Restart CL37 autoneg */
 	bnx2x_restart_autoneg(phy, params, 0);
 	DP(NETIF_MSG_LINK, "Disabling CL73, and restarting CL37 autoneg\n");
@@ -2493,14 +2470,14 @@ static u8 bnx2x_link_settings_status(struct bnx2x_phy *phy,
 				     struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
-	u16 new_line_speed , gp_status;
+	u16 new_line_speed, gp_status;
 	u8 rc = 0;
 
 	/* Read gp_status */
-	CL45_RD_OVER_CL22(bp, phy,
-				MDIO_REG_BANK_GP_STATUS,
-				MDIO_GP_STATUS_TOP_AN_STATUS1,
-				&gp_status);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_GP_STATUS,
+			  MDIO_GP_STATUS_TOP_AN_STATUS1,
+			  &gp_status);
 
 	if (phy->req_line_speed == SPEED_AUTO_NEG)
 		vars->link_status |= LINK_STATUS_AUTO_NEGOTIATE_ENABLED;
@@ -2637,9 +2614,9 @@ static void bnx2x_set_gmii_tx_driver(struct link_params *params)
 	u16 bank;
 
 	/* read precomp */
-	CL45_RD_OVER_CL22(bp, phy,
-			      MDIO_REG_BANK_OVER_1G,
-			      MDIO_OVER_1G_LP_UP2, &lp_up2);
+	CL22_RD_OVER_CL45(bp, phy,
+			  MDIO_REG_BANK_OVER_1G,
+			  MDIO_OVER_1G_LP_UP2, &lp_up2);
 
 	/* bits [10:7] at lp_up2, positioned at [15:12] */
 	lp_up2 = (((lp_up2 & MDIO_OVER_1G_LP_UP2_PREEMPHASIS_MASK) >>
@@ -2651,18 +2628,18 @@ static void bnx2x_set_gmii_tx_driver(struct link_params *params)
 
 	for (bank = MDIO_REG_BANK_TX0; bank <= MDIO_REG_BANK_TX3;
 	      bank += (MDIO_REG_BANK_TX1 - MDIO_REG_BANK_TX0)) {
-		CL45_RD_OVER_CL22(bp, phy,
-				      bank,
-				      MDIO_TX0_TX_DRIVER, &tx_driver);
+		CL22_RD_OVER_CL45(bp, phy,
+				  bank,
+				  MDIO_TX0_TX_DRIVER, &tx_driver);
 
 		/* replace tx_driver bits [15:12] */
 		if (lp_up2 !=
 		    (tx_driver & MDIO_TX0_TX_DRIVER_PREEMPHASIS_MASK)) {
 			tx_driver &= ~MDIO_TX0_TX_DRIVER_PREEMPHASIS_MASK;
 			tx_driver |= lp_up2;
-			CL45_WR_OVER_CL22(bp, phy,
-					      bank,
-					      MDIO_TX0_TX_DRIVER, tx_driver);
+			CL22_WR_OVER_CL45(bp, phy,
+					  bank,
+					  MDIO_TX0_TX_DRIVER, tx_driver);
 		}
 	}
 }
@@ -2676,10 +2653,10 @@ static u8 bnx2x_emac_program(struct link_params *params,
 
 	DP(NETIF_MSG_LINK, "setting link speed & duplex\n");
 	bnx2x_bits_dis(bp, GRCBASE_EMAC0 + port*0x400 +
-		     EMAC_REG_EMAC_MODE,
-		     (EMAC_MODE_25G_MODE |
-		     EMAC_MODE_PORT_MII_10M |
-		     EMAC_MODE_HALF_DUPLEX));
+		       EMAC_REG_EMAC_MODE,
+		       (EMAC_MODE_25G_MODE |
+			EMAC_MODE_PORT_MII_10M |
+			EMAC_MODE_HALF_DUPLEX));
 	switch (vars->line_speed) {
 	case SPEED_10:
 		mode |= EMAC_MODE_PORT_MII_10M;
@@ -2707,8 +2684,8 @@ static u8 bnx2x_emac_program(struct link_params *params,
 	if (vars->duplex == DUPLEX_HALF)
 		mode |= EMAC_MODE_HALF_DUPLEX;
 	bnx2x_bits_en(bp,
-		    GRCBASE_EMAC0 + port*0x400 + EMAC_REG_EMAC_MODE,
-		    mode);
+		      GRCBASE_EMAC0 + port*0x400 + EMAC_REG_EMAC_MODE,
+		      mode);
 
 	bnx2x_set_led(params, vars, LED_MODE_OPER, vars->line_speed);
 	return 0;
@@ -2723,7 +2700,7 @@ static void bnx2x_set_preemphasis(struct bnx2x_phy *phy,
 
 	for (bank = MDIO_REG_BANK_RX0, i = 0; bank <= MDIO_REG_BANK_RX3;
 	      bank += (MDIO_REG_BANK_RX1-MDIO_REG_BANK_RX0), i++) {
-			CL45_WR_OVER_CL22(bp, phy,
+			CL22_WR_OVER_CL45(bp, phy,
 					  bank,
 					  MDIO_RX0_RX_EQ_BOOST,
 					  phy->rx_preemphasis[i]);
@@ -2731,7 +2708,7 @@ static void bnx2x_set_preemphasis(struct bnx2x_phy *phy,
 
 	for (bank = MDIO_REG_BANK_TX0, i = 0; bank <= MDIO_REG_BANK_TX3;
 		      bank += (MDIO_REG_BANK_TX1 - MDIO_REG_BANK_TX0), i++) {
-			CL45_WR_OVER_CL22(bp, phy,
+			CL22_WR_OVER_CL45(bp, phy,
 					  bank,
 					  MDIO_TX0_TX_DRIVER,
 					  phy->tx_preemphasis[i]);
@@ -2754,7 +2731,7 @@ static void bnx2x_init_internal_phy(struct bnx2x_phy *phy,
 		/* forced speed requested? */
 		if (vars->line_speed != SPEED_AUTO_NEG ||
 		    (SINGLE_MEDIA_DIRECT(params) &&
-			  params->loopback_mode == LOOPBACK_EXT)) {
+		     params->loopback_mode == LOOPBACK_EXT)) {
 			DP(NETIF_MSG_LINK, "not SGMII, no AN\n");
 
 			/* disable autoneg */
@@ -2771,7 +2748,7 @@ static void bnx2x_init_internal_phy(struct bnx2x_phy *phy,
 
 			/* program duplex & pause advertisement (for aneg) */
 			bnx2x_set_ieee_aneg_advertisment(phy, params,
-						       vars->ieee_fc);
+							 vars->ieee_fc);
 
 			/* enable autoneg */
 			bnx2x_set_autoneg(phy, params, vars, enable_cl73);
@@ -2842,7 +2819,8 @@ static u8 bnx2x_init_xgxs(struct bnx2x_phy *phy,
 }
 
 static u16 bnx2x_wait_reset_complete(struct bnx2x *bp,
-				     struct bnx2x_phy *phy)
+				     struct bnx2x_phy *phy,
+				     struct link_params *params)
 {
 	u16 cnt, ctrl;
 	/* Wait for soft reset to get cleared upto 1 sec */
@@ -2853,6 +2831,11 @@ static u16 bnx2x_wait_reset_complete(struct bnx2x *bp,
 			break;
 		msleep(1);
 	}
+
+	if (cnt == 1000)
+		netdev_err(bp->dev,  "Warning: PHY was not initialized,"
+				      " Port %d\n",
+			 params->port);
 	DP(NETIF_MSG_LINK, "control reg 0x%x (after %d ms)\n", ctrl, cnt);
 	return cnt;
 }
@@ -2863,9 +2846,7 @@ static void bnx2x_link_int_enable(struct link_params *params)
 	u32 mask;
 	struct bnx2x *bp = params->bp;
 
-	/* setting the status to report on link up
-	   for either XGXS or SerDes */
-
+	/* Setting the status to report on link up for either XGXS or SerDes */
 	if (params->switch_cfg == SWITCH_CFG_10G) {
 		mask = (NIG_MASK_XGXS0_LINK10G |
 			NIG_MASK_XGXS0_LINK_STATUS);
@@ -2908,7 +2889,7 @@ static void bnx2x_rearm_latch_signal(struct bnx2x *bp, u8 port,
 {
 	u32 latch_status = 0;
 
-	/**
+	/*
 	 * Disable the MI INT ( external phy int ) by writing 1 to the
 	 * status register. Link down indication is high-active-signal,
 	 * so in this case we need to write the status to clear the XOR
@@ -2933,27 +2914,30 @@ static void bnx2x_rearm_latch_signal(struct bnx2x *bp, u8 port,
 
 		/* For all latched-signal=up : Re-Arm Latch signals */
 		REG_WR(bp, NIG_REG_LATCH_STATUS_0 + port*8,
-			     (latch_status & 0xfffe) | (latch_status & 1));
+		       (latch_status & 0xfffe) | (latch_status & 1));
 	}
 	/* For all latched-signal=up,Write original_signal to status */
 }
 
 static void bnx2x_link_int_ack(struct link_params *params,
-			     struct link_vars *vars, u8 is_10g)
+			       struct link_vars *vars, u8 is_10g)
 {
 	struct bnx2x *bp = params->bp;
 	u8 port = params->port;
 
-	/* first reset all status
-	 * we assume only one line will be change at a time */
+	/*
+	 * First reset all status we assume only one line will be
+	 * change at a time
+	 */
 	bnx2x_bits_dis(bp, NIG_REG_STATUS_INTERRUPT_PORT0 + port*4,
-		     (NIG_STATUS_XGXS0_LINK10G |
-		      NIG_STATUS_XGXS0_LINK_STATUS |
-		      NIG_STATUS_SERDES0_LINK_STATUS));
+		       (NIG_STATUS_XGXS0_LINK10G |
+			NIG_STATUS_XGXS0_LINK_STATUS |
+			NIG_STATUS_SERDES0_LINK_STATUS));
 	if (vars->phy_link_up) {
 		if (is_10g) {
-			/* Disable the 10G link interrupt
-			 * by writing 1 to the status register
+			/*
+			 * Disable the 10G link interrupt by writing 1 to the
+			 * status register
 			 */
 			DP(NETIF_MSG_LINK, "10G XGXS phy link up\n");
 			bnx2x_bits_en(bp,
@@ -2961,9 +2945,9 @@ static void bnx2x_link_int_ack(struct link_params *params,
 				      NIG_STATUS_XGXS0_LINK10G);
 
 		} else if (params->switch_cfg == SWITCH_CFG_10G) {
-			/* Disable the link interrupt
-			 * by writing 1 to the relevant lane
-			 * in the status register
+			/*
+			 * Disable the link interrupt by writing 1 to the
+			 * relevant lane in the status register
 			 */
 			u32 ser_lane = ((params->lane_config &
 				    PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
@@ -2978,8 +2962,9 @@ static void bnx2x_link_int_ack(struct link_params *params,
 
 		} else { /* SerDes */
 			DP(NETIF_MSG_LINK, "SerDes phy link up\n");
-			/* Disable the link interrupt
-			 * by writing 1 to the status register
+			/*
+			 * Disable the link interrupt by writing 1 to the status
+			 * register
 			 */
 			bnx2x_bits_en(bp,
 				      NIG_REG_STATUS_INTERRUPT_PORT0 + port*4,
@@ -3059,8 +3044,7 @@ u8 bnx2x_get_ext_phy_fw_version(struct link_params *params, u8 driver_loaded,
 	}
 	if ((params->num_phys == MAX_PHYS) &&
 	    (params->phy[EXT_PHY2].ver_addr != 0)) {
-		spirom_ver = REG_RD(bp,
-					  params->phy[EXT_PHY2].ver_addr);
+		spirom_ver = REG_RD(bp, params->phy[EXT_PHY2].ver_addr);
 		if (params->phy[EXT_PHY2].format_fw_ver) {
 			*ver_p = '/';
 			ver_p++;
@@ -3089,29 +3073,27 @@ static void bnx2x_set_xgxs_loopback(struct bnx2x_phy *phy,
 
 		/* change the uni_phy_addr in the nig */
 		md_devad = REG_RD(bp, (NIG_REG_XGXS0_CTRL_MD_DEVAD +
-					  port*0x18));
+				       port*0x18));
 
 		REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + port*0x18, 0x5);
 
 		bnx2x_cl45_write(bp, phy,
-			       5,
-			       (MDIO_REG_BANK_AER_BLOCK +
-				(MDIO_AER_BLOCK_AER_REG & 0xf)),
-			       0x2800);
+				 5,
+				 (MDIO_REG_BANK_AER_BLOCK +
+				  (MDIO_AER_BLOCK_AER_REG & 0xf)),
+				 0x2800);
 
 		bnx2x_cl45_write(bp, phy,
-			       5,
-			       (MDIO_REG_BANK_CL73_IEEEB0 +
-				(MDIO_CL73_IEEEB0_CL73_AN_CONTROL & 0xf)),
-			       0x6041);
+				 5,
+				 (MDIO_REG_BANK_CL73_IEEEB0 +
+				  (MDIO_CL73_IEEEB0_CL73_AN_CONTROL & 0xf)),
+				 0x6041);
 		msleep(200);
 		/* set aer mmd back */
 		bnx2x_set_aer_mmd_xgxs(params, phy);
 
 		/* and md_devad */
-		REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + port*0x18,
-			    md_devad);
-
+		REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + port*0x18, md_devad);
 	} else {
 		u16 mii_ctrl;
 		DP(NETIF_MSG_LINK, "XGXS 1G loopback enable\n");
@@ -3152,26 +3134,26 @@ u8 bnx2x_set_led(struct link_params *params,
 	case LED_MODE_OFF:
 		REG_WR(bp, NIG_REG_LED_10G_P0 + port*4, 0);
 		REG_WR(bp, NIG_REG_LED_MODE_P0 + port*4,
-			   SHARED_HW_CFG_LED_MAC1);
+		       SHARED_HW_CFG_LED_MAC1);
 
 		tmp = EMAC_RD(bp, EMAC_REG_EMAC_LED);
 		EMAC_WR(bp, EMAC_REG_EMAC_LED, (tmp | EMAC_LED_OVERRIDE));
 		break;
 
 	case LED_MODE_OPER:
-		/**
+		/*
 		 * For all other phys, OPER mode is same as ON, so in case
 		 * link is down, do nothing
-		 **/
+		 */
 		if (!vars->link_up)
 			break;
 	case LED_MODE_ON:
 		if (params->phy[EXT_PHY1].type ==
 		    PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727 &&
 		    CHIP_IS_E2(bp) && params->num_phys == 2) {
-			/**
-			* This is a work-around for E2+8727 Configurations
-			*/
+			/*
+			 * This is a work-around for E2+8727 Configurations
+			 */
 			if (mode == LED_MODE_ON ||
 				speed == SPEED_10000){
 				REG_WR(bp, NIG_REG_LED_MODE_P0 + port*4, 0);
@@ -3183,41 +3165,40 @@ u8 bnx2x_set_led(struct link_params *params,
 				return rc;
 			}
 		} else if (SINGLE_MEDIA_DIRECT(params)) {
-			/**
-			* This is a work-around for HW issue found when link
-			* is up in CL73
-			*/
+			/*
+			 * This is a work-around for HW issue found when link
+			 * is up in CL73
+			 */
 			REG_WR(bp, NIG_REG_LED_MODE_P0 + port*4, 0);
 			REG_WR(bp, NIG_REG_LED_10G_P0 + port*4, 1);
 		} else {
-			REG_WR(bp, NIG_REG_LED_MODE_P0 + port*4,
-				   hw_led_mode);
+			REG_WR(bp, NIG_REG_LED_MODE_P0 + port*4, hw_led_mode);
 		}
 
-		REG_WR(bp, NIG_REG_LED_CONTROL_OVERRIDE_TRAFFIC_P0 +
-			   port*4, 0);
+		REG_WR(bp, NIG_REG_LED_CONTROL_OVERRIDE_TRAFFIC_P0 + port*4, 0);
 		/* Set blinking rate to ~15.9Hz */
 		REG_WR(bp, NIG_REG_LED_CONTROL_BLINK_RATE_P0 + port*4,
-			   LED_BLINK_RATE_VAL);
+		       LED_BLINK_RATE_VAL);
 		REG_WR(bp, NIG_REG_LED_CONTROL_BLINK_RATE_ENA_P0 +
-			   port*4, 1);
+		       port*4, 1);
 		tmp = EMAC_RD(bp, EMAC_REG_EMAC_LED);
-		EMAC_WR(bp, EMAC_REG_EMAC_LED,
-			    (tmp & (~EMAC_LED_OVERRIDE)));
+		EMAC_WR(bp, EMAC_REG_EMAC_LED, (tmp & (~EMAC_LED_OVERRIDE)));
 
 		if (CHIP_IS_E1(bp) &&
 		    ((speed == SPEED_2500) ||
 		     (speed == SPEED_1000) ||
 		     (speed == SPEED_100) ||
 		     (speed == SPEED_10))) {
-			/* On Everest 1 Ax chip versions for speeds less than
-			10G LED scheme is different */
+			/*
+			 * On Everest 1 Ax chip versions for speeds less than
+			 * 10G LED scheme is different
+			 */
 			REG_WR(bp, NIG_REG_LED_CONTROL_OVERRIDE_TRAFFIC_P0
-				   + port*4, 1);
+			       + port*4, 1);
 			REG_WR(bp, NIG_REG_LED_CONTROL_TRAFFIC_P0 +
-				   port*4, 0);
+			       port*4, 0);
 			REG_WR(bp, NIG_REG_LED_CONTROL_BLINK_TRAFFIC_P0 +
-				   port*4, 1);
+			       port*4, 1);
 		}
 		break;
 
@@ -3231,7 +3212,7 @@ u8 bnx2x_set_led(struct link_params *params,
 
 }
 
-/**
+/*
  * This function comes to reflect the actual link state read DIRECTLY from the
  * HW
  */
@@ -3243,10 +3224,10 @@ u8 bnx2x_test_link(struct link_params *params, struct link_vars *vars,
 	u8 ext_phy_link_up = 0, serdes_phy_type;
 	struct link_vars temp_vars;
 
-	CL45_RD_OVER_CL22(bp, &params->phy[INT_PHY],
-			      MDIO_REG_BANK_GP_STATUS,
-			      MDIO_GP_STATUS_TOP_AN_STATUS1,
-			      &gp_status);
+	CL22_RD_OVER_CL45(bp, &params->phy[INT_PHY],
+			  MDIO_REG_BANK_GP_STATUS,
+			  MDIO_GP_STATUS_TOP_AN_STATUS1,
+			  &gp_status);
 	/* link is up only if both local phy and external phy are up */
 	if (!(gp_status & MDIO_GP_STATUS_TOP_AN_STATUS1_LINK_STATUS))
 		return -ESRCH;
@@ -3290,15 +3271,15 @@ static u8 bnx2x_link_initialize(struct link_params *params,
 	u8 rc = 0;
 	u8 phy_index, non_ext_phy;
 	struct bnx2x *bp = params->bp;
-	/**
-	* In case of external phy existence, the line speed would be the
-	* line speed linked up by the external phy. In case it is direct
-	* only, then the line_speed during initialization will be
-	* equal to the req_line_speed
-	*/
+	/*
+	 * In case of external phy existence, the line speed would be the
+	 * line speed linked up by the external phy. In case it is direct
+	 * only, then the line_speed during initialization will be
+	 * equal to the req_line_speed
+	 */
 	vars->line_speed = params->phy[INT_PHY].req_line_speed;
 
-	/**
+	/*
 	 * Initialize the internal phy in case this is a direct board
 	 * (no external phys), or this board has external phy which requires
 	 * to first.
@@ -3326,17 +3307,16 @@ static u8 bnx2x_link_initialize(struct link_params *params,
 	if (!non_ext_phy)
 		for (phy_index = EXT_PHY1; phy_index < params->num_phys;
 		      phy_index++) {
-			/**
+			/*
 			 * No need to initialize second phy in case of first
 			 * phy only selection. In case of second phy, we do
 			 * need to initialize the first phy, since they are
 			 * connected.
-			 **/
+			 */
 			if (phy_index == EXT_PHY2 &&
 			    (bnx2x_phy_selection(params) ==
 			     PORT_HW_CFG_PHY_SELECTION_FIRST_PHY)) {
-				DP(NETIF_MSG_LINK, "Not initializing"
-						   "second phy\n");
+				DP(NETIF_MSG_LINK, "Ignoring second phy\n");
 				continue;
 			}
 			params->phy[phy_index].config_init(
@@ -3358,9 +3338,8 @@ static void bnx2x_int_link_reset(struct bnx2x_phy *phy,
 				 struct link_params *params)
 {
 	/* reset the SerDes/XGXS */
-	REG_WR(params->bp, GRCBASE_MISC +
-		     MISC_REGISTERS_RESET_REG_3_CLEAR,
-		     (0x1ff << (params->port*16)));
+	REG_WR(params->bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_CLEAR,
+	       (0x1ff << (params->port*16)));
 }
 
 static void bnx2x_common_ext_link_reset(struct bnx2x_phy *phy,
@@ -3374,11 +3353,11 @@ static void bnx2x_common_ext_link_reset(struct bnx2x_phy *phy,
 	else
 		gpio_port = params->port;
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW,
-			    gpio_port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW,
+		       gpio_port);
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW,
-			    gpio_port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW,
+		       gpio_port);
 	DP(NETIF_MSG_LINK, "reset external PHY\n");
 }
 
@@ -3409,9 +3388,8 @@ static u8 bnx2x_update_link_down(struct link_params *params,
 
 	/* reset BigMac */
 	bnx2x_bmac_rx_disable(bp, params->port);
-	REG_WR(bp, GRCBASE_MISC +
-		   MISC_REGISTERS_RESET_REG_2_CLEAR,
-		   (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
+	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
+	       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
 	return 0;
 }
 
@@ -3462,7 +3440,7 @@ static u8 bnx2x_update_link_up(struct link_params *params,
 	msleep(20);
 	return rc;
 }
-/**
+/*
  * The bnx2x_link_update function should be called upon link
  * interrupt.
  * Link is considered up as follows:
@@ -3501,12 +3479,11 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 		 REG_RD(bp, NIG_REG_STATUS_INTERRUPT_PORT0 + port*4));
 
 	is_mi_int = (u8)(REG_RD(bp, NIG_REG_EMAC0_STATUS_MISC_MI_INT +
-				    port*0x18) > 0);
+				port*0x18) > 0);
 	DP(NETIF_MSG_LINK, "int_mask 0x%x MI_INT %x, SERDES_LINK %x\n",
 		 REG_RD(bp, NIG_REG_MASK_INTERRUPT_PORT0 + port*4),
 		 is_mi_int,
-		 REG_RD(bp,
-			    NIG_REG_SERDES0_STATUS_LINK_STATUS + port*0x3c));
+		 REG_RD(bp, NIG_REG_SERDES0_STATUS_LINK_STATUS + port*0x3c));
 
 	DP(NETIF_MSG_LINK, " 10G %x, XGXS_LINK %x\n",
 	  REG_RD(bp, NIG_REG_XGXS0_STATUS_LINK10G + port*0x68),
@@ -3515,14 +3492,14 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	/* disable emac */
 	REG_WR(bp, NIG_REG_NIG_EMAC0_EN + port*4, 0);
 
-	/**
-	* Step 1:
-	* Check external link change only for external phys, and apply
-	* priority selection between them in case the link on both phys
-	* is up. Note that the instead of the common vars, a temporary
-	* vars argument is used since each phy may have different link/
-	* speed/duplex result
-	*/
+	/*
+	 * Step 1:
+	 * Check external link change only for external phys, and apply
+	 * priority selection between them in case the link on both phys
+	 * is up. Note that the instead of the common vars, a temporary
+	 * vars argument is used since each phy may have different link/
+	 * speed/duplex result
+	 */
 	for (phy_index = EXT_PHY1; phy_index < params->num_phys;
 	      phy_index++) {
 		struct bnx2x_phy *phy = &params->phy[phy_index];
@@ -3547,22 +3524,22 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 			switch (bnx2x_phy_selection(params)) {
 			case PORT_HW_CFG_PHY_SELECTION_HARDWARE_DEFAULT:
 			case PORT_HW_CFG_PHY_SELECTION_FIRST_PHY_PRIORITY:
-			/**
+			/*
 			 * In this option, the first PHY makes sure to pass the
 			 * traffic through itself only.
 			 * Its not clear how to reset the link on the second phy
-			 **/
+			 */
 				active_external_phy = EXT_PHY1;
 				break;
 			case PORT_HW_CFG_PHY_SELECTION_SECOND_PHY_PRIORITY:
-			/**
+			/*
 			 * In this option, the first PHY makes sure to pass the
 			 * traffic through the second PHY.
-			 **/
+			 */
 				active_external_phy = EXT_PHY2;
 				break;
 			default:
-			/**
+			/*
 			 * Link indication on both PHYs with the following cases
 			 * is invalid:
 			 * - FIRST_PHY means that second phy wasn't initialized,
@@ -3570,7 +3547,7 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 			 * - SECOND_PHY means that first phy should not be able
 			 * to link up by itself (using configuration)
 			 * - DEFAULT should be overriden during initialiazation
-			 **/
+			 */
 				DP(NETIF_MSG_LINK, "Invalid link indication"
 					   "mpc=0x%x. DISABLING LINK !!!\n",
 					   params->multi_phy_config);
@@ -3580,18 +3557,18 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 		}
 	}
 	prev_line_speed = vars->line_speed;
-	/**
-	* Step 2:
-	* Read the status of the internal phy. In case of
-	* DIRECT_SINGLE_MEDIA board, this link is the external link,
-	* otherwise this is the link between the 577xx and the first
-	* external phy
-	*/
+	/*
+	 * Step 2:
+	 * Read the status of the internal phy. In case of
+	 * DIRECT_SINGLE_MEDIA board, this link is the external link,
+	 * otherwise this is the link between the 577xx and the first
+	 * external phy
+	 */
 	if (params->phy[INT_PHY].read_status)
 		params->phy[INT_PHY].read_status(
 			&params->phy[INT_PHY],
 			params, vars);
-	/**
+	/*
 	 * The INT_PHY flow control reside in the vars. This include the
 	 * case where the speed or flow control are not set to AUTO.
 	 * Otherwise, the active external phy flow control result is set
@@ -3601,13 +3578,13 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	 */
 	if (active_external_phy > INT_PHY) {
 		vars->flow_ctrl = phy_vars[active_external_phy].flow_ctrl;
-		/**
+		/*
 		 * Link speed is taken from the XGXS. AN and FC result from
 		 * the external phy.
 		 */
 		vars->link_status |= phy_vars[active_external_phy].link_status;
 
-		/**
+		/*
 		 * if active_external_phy is first PHY and link is up - disable
 		 * disable TX on second external PHY
 		 */
@@ -3643,7 +3620,7 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	DP(NETIF_MSG_LINK, "vars->flow_ctrl = 0x%x, vars->link_status = 0x%x,"
 		   " ext_phy_line_speed = %d\n", vars->flow_ctrl,
 		   vars->link_status, ext_phy_line_speed);
-	/**
+	/*
 	 * Upon link speed change set the NIG into drain mode. Comes to
 	 * deals with possible FIFO glitch due to clk change when speed
 	 * is decreased without link down indicator
@@ -3658,8 +3635,8 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 				   ext_phy_line_speed);
 			vars->phy_link_up = 0;
 		} else if (prev_line_speed != vars->line_speed) {
-			REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE
-				     + params->port*4, 0);
+			REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4,
+			       0);
 			msleep(1);
 		}
 	}
@@ -3674,14 +3651,14 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 
 	bnx2x_link_int_ack(params, vars, link_10g);
 
-	/**
-	* In case external phy link is up, and internal link is down
-	* (not initialized yet probably after link initialization, it
-	* needs to be initialized.
-	* Note that after link down-up as result of cable plug, the xgxs
-	* link would probably become up again without the need
-	* initialize it
-	*/
+	/*
+	 * In case external phy link is up, and internal link is down
+	 * (not initialized yet probably after link initialization, it
+	 * needs to be initialized.
+	 * Note that after link down-up as result of cable plug, the xgxs
+	 * link would probably become up again without the need
+	 * initialize it
+	 */
 	if (!(SINGLE_MEDIA_DIRECT(params))) {
 		DP(NETIF_MSG_LINK, "ext_phy_link_up = %d, int_link_up = %d,"
 			   " init_preceding = %d\n", ext_phy_link_up,
@@ -3701,9 +3678,9 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 						vars);
 		}
 	}
-	/**
-	 *  Link is up only if both local phy and external phy (in case of
-	 *  non-direct board) are up
+	/*
+	 * Link is up only if both local phy and external phy (in case of
+	 * non-direct board) are up
 	 */
 	vars->link_up = (vars->phy_link_up &&
 			 (ext_phy_link_up ||
@@ -3724,10 +3701,10 @@ u8 bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 void bnx2x_ext_phy_hw_reset(struct bnx2x *bp, u8 port)
 {
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
 	msleep(1);
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, port);
 }
 
 static void bnx2x_save_spirom_version(struct bnx2x *bp, u8 port,
@@ -3747,9 +3724,9 @@ static void bnx2x_save_bcm_spirom_ver(struct bnx2x *bp,
 	u16 fw_ver1, fw_ver2;
 
 	bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_ROM_VER1, &fw_ver1);
+			MDIO_PMA_REG_ROM_VER1, &fw_ver1);
 	bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_ROM_VER2, &fw_ver2);
+			MDIO_PMA_REG_ROM_VER2, &fw_ver2);
 	bnx2x_save_spirom_version(bp, port, (u32)(fw_ver1<<16 | fw_ver2),
 				  phy->ver_addr);
 }
@@ -3770,7 +3747,7 @@ static void bnx2x_ext_phy_set_pause(struct link_params *params,
 	if ((vars->ieee_fc &
 	    MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC) ==
 	    MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC) {
-		val |=  MDIO_AN_REG_ADV_PAUSE_ASYMMETRIC;
+		val |= MDIO_AN_REG_ADV_PAUSE_ASYMMETRIC;
 	}
 	if ((vars->ieee_fc &
 	    MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH) ==
@@ -3801,11 +3778,11 @@ static u8 bnx2x_ext_phy_resolve_fc(struct bnx2x_phy *phy,
 	else if (vars->link_status & LINK_STATUS_AUTO_NEGOTIATE_COMPLETE) {
 		ret = 1;
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_AN_DEVAD,
-			      MDIO_AN_REG_ADV_PAUSE, &ld_pause);
+				MDIO_AN_DEVAD,
+				MDIO_AN_REG_ADV_PAUSE, &ld_pause);
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_AN_DEVAD,
-			      MDIO_AN_REG_LP_AUTO_NEG, &lp_pause);
+				MDIO_AN_DEVAD,
+				MDIO_AN_REG_LP_AUTO_NEG, &lp_pause);
 		pause_result = (ld_pause &
 				MDIO_AN_REG_ADV_PAUSE_MASK) >> 8;
 		pause_result |= (lp_pause &
@@ -3881,31 +3858,31 @@ static u8 bnx2x_8073_8727_external_rom_boot(struct bnx2x *bp,
 	/* Boot port from external ROM  */
 	/* EDC grst */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_GEN_CTRL,
-		       0x0001);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_GEN_CTRL,
+			 0x0001);
 
 	/* ucode reboot and rst */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_GEN_CTRL,
-		       0x008c);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_GEN_CTRL,
+			 0x008c);
 
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_MISC_CTRL1, 0x0001);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_MISC_CTRL1, 0x0001);
 
 	/* Reset internal microprocessor */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_GEN_CTRL,
-		       MDIO_PMA_REG_GEN_CTRL_ROM_MICRO_RESET);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_GEN_CTRL,
+			 MDIO_PMA_REG_GEN_CTRL_ROM_MICRO_RESET);
 
 	/* Release srst bit */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_GEN_CTRL,
-		       MDIO_PMA_REG_GEN_CTRL_ROM_RESET_INTERNAL_MP);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_GEN_CTRL,
+			 MDIO_PMA_REG_GEN_CTRL_ROM_RESET_INTERNAL_MP);
 
 	/* Delay 100ms per the PHY specifications */
 	msleep(100);
@@ -3936,8 +3913,8 @@ static u8 bnx2x_8073_8727_external_rom_boot(struct bnx2x *bp,
 
 	/* Clear ser_boot_ctl bit */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_MISC_CTRL1, 0x0000);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_MISC_CTRL1, 0x0000);
 	bnx2x_save_bcm_spirom_ver(bp, phy, port);
 
 	DP(NETIF_MSG_LINK,
@@ -3958,8 +3935,8 @@ static u8 bnx2x_8073_is_snr_needed(struct bnx2x *bp, struct bnx2x_phy *phy)
 
 	/* Read 8073 HW revision*/
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_8073_CHIP_REV, &val);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_8073_CHIP_REV, &val);
 
 	if (val != 1) {
 		/* No need to workaround in 8073 A1 */
@@ -3967,8 +3944,8 @@ static u8 bnx2x_8073_is_snr_needed(struct bnx2x *bp, struct bnx2x_phy *phy)
 	}
 
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_ROM_VER2, &val);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_ROM_VER2, &val);
 
 	/* SNR should be applied only for version 0x102 */
 	if (val != 0x102)
@@ -3982,8 +3959,8 @@ static u8 bnx2x_8073_xaui_wa(struct bnx2x *bp, struct bnx2x_phy *phy)
 	u16 val, cnt, cnt1 ;
 
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_8073_CHIP_REV, &val);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_8073_CHIP_REV, &val);
 
 	if (val > 0) {
 		/* No need to workaround in 8073 A1 */
@@ -3991,26 +3968,32 @@ static u8 bnx2x_8073_xaui_wa(struct bnx2x *bp, struct bnx2x_phy *phy)
 	}
 	/* XAUI workaround in 8073 A0: */
 
-	/* After loading the boot ROM and restarting Autoneg,
-	poll Dev1, Reg $C820: */
+	/*
+	 * After loading the boot ROM and restarting Autoneg, poll
+	 * Dev1, Reg $C820:
+	 */
 
 	for (cnt = 0; cnt < 1000; cnt++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_8073_SPEED_LINK_STATUS,
-			      &val);
-		  /* If bit [14] = 0 or bit [13] = 0, continue on with
-		   system initialization (XAUI work-around not required,
-		    as these bits indicate 2.5G or 1G link up). */
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_8073_SPEED_LINK_STATUS,
+				&val);
+		  /*
+		   * If bit [14] = 0 or bit [13] = 0, continue on with
+		   * system initialization (XAUI work-around not required, as
+		   * these bits indicate 2.5G or 1G link up).
+		   */
 		if (!(val & (1<<14)) || !(val & (1<<13))) {
 			DP(NETIF_MSG_LINK, "XAUI work-around not required\n");
 			return 0;
 		} else if (!(val & (1<<15))) {
-			DP(NETIF_MSG_LINK, "clc bit 15 went off\n");
-			 /* If bit 15 is 0, then poll Dev1, Reg $C841 until
-			  it's MSB (bit 15) goes to 1 (indicating that the
-			  XAUI workaround has completed),
-			  then continue on with system initialization.*/
+			DP(NETIF_MSG_LINK, "bit 15 went off\n");
+			/*
+			 * If bit 15 is 0, then poll Dev1, Reg $C841 until it's
+			 * MSB (bit15) goes to 1 (indicating that the XAUI
+			 * workaround has completed), then continue on with
+			 * system initialization.
+			 */
 			for (cnt1 = 0; cnt1 < 1000; cnt1++) {
 				bnx2x_cl45_read(bp, phy,
 					MDIO_PMA_DEVAD,
@@ -4093,10 +4076,10 @@ static u8 bnx2x_8073_config_init(struct bnx2x_phy *phy,
 		gpio_port = params->port;
 	/* Restore normal power mode*/
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, gpio_port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, gpio_port);
 
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, gpio_port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, gpio_port);
 
 	/* enable LASI */
 	bnx2x_cl45_write(bp, phy,
@@ -4114,10 +4097,6 @@ static u8 bnx2x_8073_config_init(struct bnx2x_phy *phy,
 
 	DP(NETIF_MSG_LINK, "Before rom RX_ALARM(port1): 0x%x\n", tmp1);
 
-	/**
-	 * If this is forced speed, set to KR or KX (all other are not
-	 * supported)
-	 */
 	/* Swap polarity if required - Must be done only in non-1G mode */
 	if (params->lane_config & PORT_HW_CFG_SWAP_PHY_POLARITY_ENABLED) {
 		/* Configure the 8073 to swap _P and _N of the KR lines */
@@ -4160,8 +4139,10 @@ static u8 bnx2x_8073_config_init(struct bnx2x_phy *phy,
 			val = (1<<7);
 		} else if (phy->req_line_speed ==  SPEED_2500) {
 			val = (1<<5);
-			/* Note that 2.5G works only
-			when used with 1G advertisment */
+			/*
+			 * Note that 2.5G works only when used with 1G
+			 * advertisment
+			 */
 		} else
 			val = (1<<5);
 	} else {
@@ -4170,8 +4151,7 @@ static u8 bnx2x_8073_config_init(struct bnx2x_phy *phy,
 			PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)
 			val |= (1<<7);
 
-		/* Note that 2.5G works only when
-		used with 1G advertisment */
+		/* Note that 2.5G works only when used with 1G advertisment */
 		if (phy->speed_cap_mask &
 			(PORT_HW_CFG_SPEED_CAPABILITY_D0_1G |
 			 PORT_HW_CFG_SPEED_CAPABILITY_D0_2_5G))
@@ -4211,9 +4191,11 @@ static u8 bnx2x_8073_config_init(struct bnx2x_phy *phy,
 	/* Add support for CL37 (passive mode) III */
 	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x1000);
 
-	/* The SNR will improve about 2db by changing
-	BW and FEE main tap. Rest commands are executed
-	after link is up*/
+	/*
+	 * The SNR will improve about 2db by changing BW and FEE main
+	 * tap. Rest commands are executed after link is up
+	 * Change FFE main cursor to 5 in EDC register
+	 */
 	if (bnx2x_8073_is_snr_needed(bp, phy))
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_PMA_DEVAD, MDIO_PMA_REG_EDC_FFE_MAIN,
@@ -4297,12 +4279,11 @@ static u8 bnx2x_8073_read_status(struct bnx2x_phy *phy,
 
 	link_up = (((val1 & 4) == 4) || (an1000_status & (1<<1)));
 	if (link_up && bnx2x_8073_is_snr_needed(bp, phy)) {
-		/* The SNR will improve about 2dbby
-		changing the BW and FEE main tap.*/
-		/* The 1st write to change FFE main
-		tap is set before restart AN */
-		/* Change PLL Bandwidth in EDC
-		register */
+		/*
+		 * The SNR will improve about 2dbby changing the BW and FEE main
+		 * tap. The 1st write to change FFE main tap is set before
+		 * restart AN. Change PLL Bandwidth in EDC register
+		 */
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_PMA_DEVAD, MDIO_PMA_REG_PLL_BANDWIDTH,
 				 0x26BC);
@@ -4346,10 +4327,10 @@ static u8 bnx2x_8073_read_status(struct bnx2x_phy *phy,
 			bnx2x_cl45_read(bp, phy,
 					MDIO_XS_DEVAD,
 					MDIO_XS_REG_8073_RX_CTRL_PCIE, &val1);
-			/**
-			* Set bit 3 to invert Rx in 1G mode and clear this bit
-			* when it`s in 10G mode.
-			*/
+			/*
+			 * Set bit 3 to invert Rx in 1G mode and clear this bit
+			 * when it`s in 10G mode.
+			 */
 			if (vars->line_speed == SPEED_1000) {
 				DP(NETIF_MSG_LINK, "Swapping 1G polarity for"
 					      "the 8073\n");
@@ -4381,8 +4362,8 @@ static void bnx2x_8073_link_reset(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "Setting 8073 port %d into low power mode\n",
 	   gpio_port);
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW,
-			    gpio_port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW,
+		       gpio_port);
 }
 
 /******************************************************************/
@@ -4396,11 +4377,11 @@ static u8 bnx2x_8705_config_init(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "init 8705\n");
 	/* Restore normal power mode*/
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
 	/* HW reset */
 	bnx2x_ext_phy_hw_reset(bp, params->port);
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 0xa040);
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_MISC_CTRL, 0x8288);
@@ -4451,35 +4432,79 @@ static u8 bnx2x_8705_read_status(struct bnx2x_phy *phy,
 /******************************************************************/
 /*			SFP+ module Section			  */
 /******************************************************************/
-static void bnx2x_sfp_set_transmitter(struct bnx2x *bp,
+static u8 bnx2x_get_gpio_port(struct link_params *params)
+{
+	u8 gpio_port;
+	u32 swap_val, swap_override;
+	struct bnx2x *bp = params->bp;
+	if (CHIP_IS_E2(bp))
+		gpio_port = BP_PATH(bp);
+	else
+		gpio_port = params->port;
+	swap_val = REG_RD(bp, NIG_REG_PORT_SWAP);
+	swap_override = REG_RD(bp, NIG_REG_STRAP_OVERRIDE);
+	return gpio_port ^ (swap_val && swap_override);
+}
+static void bnx2x_sfp_set_transmitter(struct link_params *params,
 				      struct bnx2x_phy *phy,
-				      u8 port,
 				      u8 tx_en)
 {
 	u16 val;
+	u8 port = params->port;
+	struct bnx2x *bp = params->bp;
+	u32 tx_en_mode;
 
-	DP(NETIF_MSG_LINK, "Setting transmitter tx_en=%x for port %x\n",
-		 tx_en, port);
 	/* Disable/Enable transmitter ( TX laser of the SFP+ module.)*/
-	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_PHY_IDENTIFIER,
-		      &val);
+	tx_en_mode = REG_RD(bp, params->shmem_base +
+			    offsetof(struct shmem_region,
+				     dev_info.port_hw_config[port].sfp_ctrl)) &
+		PORT_HW_CFG_TX_LASER_MASK;
+	DP(NETIF_MSG_LINK, "Setting transmitter tx_en=%x for port %x "
+			   "mode = %x\n", tx_en, port, tx_en_mode);
+	switch (tx_en_mode) {
+	case PORT_HW_CFG_TX_LASER_MDIO:
 
-	if (tx_en)
-		val &= ~(1<<15);
-	else
-		val |= (1<<15);
+		bnx2x_cl45_read(bp, phy,
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_PHY_IDENTIFIER,
+				&val);
 
-	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_PHY_IDENTIFIER,
-		       val);
+		if (tx_en)
+			val &= ~(1<<15);
+		else
+			val |= (1<<15);
+
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_PHY_IDENTIFIER,
+				 val);
+	break;
+	case PORT_HW_CFG_TX_LASER_GPIO0:
+	case PORT_HW_CFG_TX_LASER_GPIO1:
+	case PORT_HW_CFG_TX_LASER_GPIO2:
+	case PORT_HW_CFG_TX_LASER_GPIO3:
+	{
+		u16 gpio_pin;
+		u8 gpio_port, gpio_mode;
+		if (tx_en)
+			gpio_mode = MISC_REGISTERS_GPIO_OUTPUT_HIGH;
+		else
+			gpio_mode = MISC_REGISTERS_GPIO_OUTPUT_LOW;
+
+		gpio_pin = tx_en_mode - PORT_HW_CFG_TX_LASER_GPIO0;
+		gpio_port = bnx2x_get_gpio_port(params);
+		bnx2x_set_gpio(bp, gpio_pin, gpio_mode, gpio_port);
+		break;
+	}
+	default:
+		DP(NETIF_MSG_LINK, "Invalid TX_LASER_MDIO 0x%x\n", tx_en_mode);
+		break;
+	}
 }
 
 static u8 bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 					    struct link_params *params,
-					  u16 addr, u8 byte_cnt, u8 *o_buf)
+					    u16 addr, u8 byte_cnt, u8 *o_buf)
 {
 	struct bnx2x *bp = params->bp;
 	u16 val = 0;
@@ -4492,23 +4517,23 @@ static u8 bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	/* Set the read command byte count */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_SFP_TWO_WIRE_BYTE_CNT,
-		       (byte_cnt | 0xa000));
+			 (byte_cnt | 0xa000));
 
 	/* Set the read command address */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_SFP_TWO_WIRE_MEM_ADDR,
-		       addr);
+			 addr);
 
 	/* Activate read command */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_SFP_TWO_WIRE_CTRL,
-		       0x2c0f);
+			 0x2c0f);
 
 	/* Wait up to 500us for command complete status */
 	for (i = 0; i < 100; i++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
 		if ((val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK) ==
 		    MDIO_PMA_REG_SFP_TWO_WIRE_STATUS_COMPLETE)
 			break;
@@ -4526,15 +4551,15 @@ static u8 bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	/* Read the buffer */
 	for (i = 0; i < byte_cnt; i++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_8726_TWO_WIRE_DATA_BUF + i, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_8726_TWO_WIRE_DATA_BUF + i, &val);
 		o_buf[i] = (u8)(val & MDIO_PMA_REG_8726_TWO_WIRE_DATA_MASK);
 	}
 
 	for (i = 0; i < 100; i++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
 		if ((val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK) ==
 		    MDIO_PMA_REG_SFP_TWO_WIRE_STATUS_IDLE)
 			return 0;
@@ -4545,7 +4570,7 @@ static u8 bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 
 static u8 bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 					    struct link_params *params,
-					  u16 addr, u8 byte_cnt, u8 *o_buf)
+					    u16 addr, u8 byte_cnt, u8 *o_buf)
 {
 	struct bnx2x *bp = params->bp;
 	u16 val, i;
@@ -4558,41 +4583,43 @@ static u8 bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 
 	/* Need to read from 1.8000 to clear it */
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_SFP_TWO_WIRE_CTRL,
-		      &val);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_SFP_TWO_WIRE_CTRL,
+			&val);
 
 	/* Set the read command byte count */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_SFP_TWO_WIRE_BYTE_CNT,
-		       ((byte_cnt < 2) ? 2 : byte_cnt));
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_SFP_TWO_WIRE_BYTE_CNT,
+			 ((byte_cnt < 2) ? 2 : byte_cnt));
 
 	/* Set the read command address */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_SFP_TWO_WIRE_MEM_ADDR,
-		       addr);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_SFP_TWO_WIRE_MEM_ADDR,
+			 addr);
 	/* Set the destination address */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       0x8004,
-		       MDIO_PMA_REG_8727_TWO_WIRE_DATA_BUF);
+			 MDIO_PMA_DEVAD,
+			 0x8004,
+			 MDIO_PMA_REG_8727_TWO_WIRE_DATA_BUF);
 
 	/* Activate read command */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_SFP_TWO_WIRE_CTRL,
-		       0x8002);
-	/* Wait appropriate time for two-wire command to finish before
-	polling the status register */
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_SFP_TWO_WIRE_CTRL,
+			 0x8002);
+	/*
+	 * Wait appropriate time for two-wire command to finish before
+	 * polling the status register
+	 */
 	msleep(1);
 
 	/* Wait up to 500us for command complete status */
 	for (i = 0; i < 100; i++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
 		if ((val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK) ==
 		    MDIO_PMA_REG_SFP_TWO_WIRE_STATUS_COMPLETE)
 			break;
@@ -4604,21 +4631,21 @@ static u8 bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK,
 			 "Got bad status 0x%x when reading from SFP+ EEPROM\n",
 			 (val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK));
-		return -EINVAL;
+		return -EFAULT;
 	}
 
 	/* Read the buffer */
 	for (i = 0; i < byte_cnt; i++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_8727_TWO_WIRE_DATA_BUF + i, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_8727_TWO_WIRE_DATA_BUF + i, &val);
 		o_buf[i] = (u8)(val & MDIO_PMA_REG_8727_TWO_WIRE_DATA_MASK);
 	}
 
 	for (i = 0; i < 100; i++) {
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_SFP_TWO_WIRE_CTRL, &val);
 		if ((val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK) ==
 		    MDIO_PMA_REG_SFP_TWO_WIRE_STATUS_IDLE)
 			return 0;
@@ -4628,22 +4655,22 @@ static u8 bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	return -EINVAL;
 }
 
-static u8 bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
-				       struct link_params *params, u16 addr,
-				       u8 byte_cnt, u8 *o_buf)
+u8 bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
+				struct link_params *params, u16 addr,
+				u8 byte_cnt, u8 *o_buf)
 {
 	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8726)
 		return bnx2x_8726_read_sfp_module_eeprom(phy, params, addr,
-						       byte_cnt, o_buf);
+							 byte_cnt, o_buf);
 	else if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727)
 		return bnx2x_8727_read_sfp_module_eeprom(phy, params, addr,
-						       byte_cnt, o_buf);
+							 byte_cnt, o_buf);
 	return -EINVAL;
 }
 
 static u8 bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 			     struct link_params *params,
-				  u16 *edc_mode)
+			     u16 *edc_mode)
 {
 	struct bnx2x *bp = params->bp;
 	u8 val, check_limiting_mode = 0;
@@ -4664,8 +4691,10 @@ static u8 bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 	{
 		u8 copper_module_type;
 
-		/* Check if its active cable( includes SFP+ module)
-		of passive cable*/
+		/*
+		 * Check if its active cable (includes SFP+ module)
+		 * of passive cable
+		 */
 		if (bnx2x_read_sfp_module_eeprom(phy,
 					       params,
 					       SFP_EEPROM_FC_TX_TECH_ADDR,
@@ -4724,8 +4753,10 @@ static u8 bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "EDC mode is set to 0x%x\n", *edc_mode);
 	return 0;
 }
-/* This function read the relevant field from the module ( SFP+ ),
-	and verify it is compliant with this board */
+/*
+ * This function read the relevant field from the module (SFP+), and verify it
+ * is compliant with this board
+ */
 static u8 bnx2x_verify_sfp_module(struct bnx2x_phy *phy,
 				  struct link_params *params)
 {
@@ -4774,24 +4805,24 @@ static u8 bnx2x_verify_sfp_module(struct bnx2x_phy *phy,
 	/* format the warning message */
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
-				       SFP_EEPROM_VENDOR_NAME_ADDR,
-				       SFP_EEPROM_VENDOR_NAME_SIZE,
-				       (u8 *)vendor_name))
+					 SFP_EEPROM_VENDOR_NAME_ADDR,
+					 SFP_EEPROM_VENDOR_NAME_SIZE,
+					 (u8 *)vendor_name))
 		vendor_name[0] = '\0';
 	else
 		vendor_name[SFP_EEPROM_VENDOR_NAME_SIZE] = '\0';
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
-				       SFP_EEPROM_PART_NO_ADDR,
-				       SFP_EEPROM_PART_NO_SIZE,
-				       (u8 *)vendor_pn))
+					 SFP_EEPROM_PART_NO_ADDR,
+					 SFP_EEPROM_PART_NO_SIZE,
+					 (u8 *)vendor_pn))
 		vendor_pn[0] = '\0';
 	else
 		vendor_pn[SFP_EEPROM_PART_NO_SIZE] = '\0';
 
-	netdev_info(bp->dev, "Warning: Unqualified SFP+ module detected,"
-			     " Port %d from %s part number %s\n",
-		    params->port, vendor_name, vendor_pn);
+	netdev_err(bp->dev,  "Warning: Unqualified SFP+ module detected,"
+			      " Port %d from %s part number %s\n",
+			 params->port, vendor_name, vendor_pn);
 	phy->flags |= FLAGS_SFP_NOT_APPROVED;
 	return -EINVAL;
 }
@@ -4803,8 +4834,11 @@ static u8 bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
 	u8 val;
 	struct bnx2x *bp = params->bp;
 	u16 timeout;
-	/* Initialization time after hot-plug may take up to 300ms for some
-	phys type ( e.g. JDSU ) */
+	/*
+	 * Initialization time after hot-plug may take up to 300ms for
+	 * some phys type ( e.g. JDSU )
+	 */
+
 	for (timeout = 0; timeout < 60; timeout++) {
 		if (bnx2x_read_sfp_module_eeprom(phy, params, 1, 1, &val)
 		    == 0) {
@@ -4823,16 +4857,14 @@ static void bnx2x_8727_power_module(struct bnx2x *bp,
 	/* Make sure GPIOs are not using for LED mode */
 	u16 val;
 	/*
-	 * In the GPIO register, bit 4 is use to detemine if the GPIOs are
+	 * In the GPIO register, bit 4 is use to determine if the GPIOs are
 	 * operating as INPUT or as OUTPUT. Bit 1 is for input, and 0 for
 	 * output
 	 * Bits 0-1 determine the gpios value for OUTPUT in case bit 4 val is 0
 	 * Bits 8-9 determine the gpios value for INPUT in case bit 4 val is 1
 	 * where the 1st bit is the over-current(only input), and 2nd bit is
 	 * for power( only output )
-	*/
-
-	/*
+	 *
 	 * In case of NOC feature is disabled and power is up, set GPIO control
 	 *  as input to enable listening of over-current indication
 	 */
@@ -4861,15 +4893,14 @@ static u8 bnx2x_8726_set_limiting_mode(struct bnx2x *bp,
 	u16 cur_limiting_mode;
 
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_ROM_VER2,
-		      &cur_limiting_mode);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_ROM_VER2,
+			&cur_limiting_mode);
 	DP(NETIF_MSG_LINK, "Current Limiting mode is 0x%x\n",
 		 cur_limiting_mode);
 
 	if (edc_mode == EDC_MODE_LIMITING) {
-		DP(NETIF_MSG_LINK,
-			 "Setting LIMITING MODE\n");
+		DP(NETIF_MSG_LINK, "Setting LIMITING MODE\n");
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_PMA_DEVAD,
 				 MDIO_PMA_REG_ROM_VER2,
@@ -4878,62 +4909,63 @@ static u8 bnx2x_8726_set_limiting_mode(struct bnx2x *bp,
 
 		DP(NETIF_MSG_LINK, "Setting LRM MODE\n");
 
-		/* Changing to LRM mode takes quite few seconds.
-		So do it only if current mode is limiting
-		( default is LRM )*/
+		/*
+		 * Changing to LRM mode takes quite few seconds. So do it only
+		 * if current mode is limiting (default is LRM)
+		 */
 		if (cur_limiting_mode != EDC_MODE_LIMITING)
 			return 0;
 
 		bnx2x_cl45_write(bp, phy,
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_LRM_MODE,
-			       0);
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_LRM_MODE,
+				 0);
 		bnx2x_cl45_write(bp, phy,
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_ROM_VER2,
-			       0x128);
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_ROM_VER2,
+				 0x128);
 		bnx2x_cl45_write(bp, phy,
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_MISC_CTRL0,
-			       0x4008);
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_MISC_CTRL0,
+				 0x4008);
 		bnx2x_cl45_write(bp, phy,
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_LRM_MODE,
-			       0xaaaa);
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_LRM_MODE,
+				 0xaaaa);
 	}
 	return 0;
 }
 
 static u8 bnx2x_8727_set_limiting_mode(struct bnx2x *bp,
 				       struct bnx2x_phy *phy,
-					u16 edc_mode)
+				       u16 edc_mode)
 {
 	u16 phy_identifier;
 	u16 rom_ver2_val;
 	bnx2x_cl45_read(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_PHY_IDENTIFIER,
-		       &phy_identifier);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_PHY_IDENTIFIER,
+			&phy_identifier);
 
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_PHY_IDENTIFIER,
-		       (phy_identifier & ~(1<<9)));
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_PHY_IDENTIFIER,
+			 (phy_identifier & ~(1<<9)));
 
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_ROM_VER2,
-		      &rom_ver2_val);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_ROM_VER2,
+			&rom_ver2_val);
 	/* Keep the MSB 8-bits, and set the LSB 8-bits with the edc_mode */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_ROM_VER2,
-		       (rom_ver2_val & 0xff00) | (edc_mode & 0x00ff));
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_ROM_VER2,
+			 (rom_ver2_val & 0xff00) | (edc_mode & 0x00ff));
 
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_PHY_IDENTIFIER,
-		       (phy_identifier | (1<<9)));
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_PHY_IDENTIFIER,
+			 (phy_identifier | (1<<9)));
 
 	return 0;
 }
@@ -4946,11 +4978,11 @@ static void bnx2x_8727_specific_func(struct bnx2x_phy *phy,
 
 	switch (action) {
 	case DISABLE_TX:
-		bnx2x_sfp_set_transmitter(bp, phy, params->port, 0);
+		bnx2x_sfp_set_transmitter(params, phy, 0);
 		break;
 	case ENABLE_TX:
 		if (!(phy->flags & FLAGS_SFP_NOT_APPROVED))
-			bnx2x_sfp_set_transmitter(bp, phy, params->port, 1);
+			bnx2x_sfp_set_transmitter(params, phy, 1);
 		break;
 	default:
 		DP(NETIF_MSG_LINK, "Function 0x%x not supported by 8727\n",
@@ -4959,6 +4991,38 @@ static void bnx2x_8727_specific_func(struct bnx2x_phy *phy,
 	}
 }
 
+static void bnx2x_set_sfp_module_fault_led(struct link_params *params,
+					   u8 gpio_mode)
+{
+	struct bnx2x *bp = params->bp;
+
+	u32 fault_led_gpio = REG_RD(bp, params->shmem_base +
+			    offsetof(struct shmem_region,
+			dev_info.port_hw_config[params->port].sfp_ctrl)) &
+		PORT_HW_CFG_FAULT_MODULE_LED_MASK;
+	switch (fault_led_gpio) {
+	case PORT_HW_CFG_FAULT_MODULE_LED_DISABLED:
+		return;
+	case PORT_HW_CFG_FAULT_MODULE_LED_GPIO0:
+	case PORT_HW_CFG_FAULT_MODULE_LED_GPIO1:
+	case PORT_HW_CFG_FAULT_MODULE_LED_GPIO2:
+	case PORT_HW_CFG_FAULT_MODULE_LED_GPIO3:
+	{
+		u8 gpio_port = bnx2x_get_gpio_port(params);
+		u16 gpio_pin = fault_led_gpio -
+			PORT_HW_CFG_FAULT_MODULE_LED_GPIO0;
+		DP(NETIF_MSG_LINK, "Set fault module-detected led "
+				   "pin %x port %x mode %x\n",
+			       gpio_pin, gpio_port, gpio_mode);
+		bnx2x_set_gpio(bp, gpio_pin, gpio_mode, gpio_port);
+	}
+	break;
+	default:
+		DP(NETIF_MSG_LINK, "Error: Invalid fault led mode 0x%x\n",
+			       fault_led_gpio);
+	}
+}
+
 static u8 bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 				     struct link_params *params)
 {
@@ -4976,15 +5040,14 @@ static u8 bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 	if (bnx2x_get_edc_mode(phy, params, &edc_mode) != 0) {
 		DP(NETIF_MSG_LINK, "Failed to get valid module type\n");
 		return -EINVAL;
-	} else if (bnx2x_verify_sfp_module(phy, params) !=
-		   0) {
+	} else if (bnx2x_verify_sfp_module(phy, params) != 0) {
 		/* check SFP+ module compatibility */
 		DP(NETIF_MSG_LINK, "Module verification failed!!\n");
 		rc = -EINVAL;
 		/* Turn on fault module-detected led */
-		bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_0,
-				  MISC_REGISTERS_GPIO_HIGH,
-				  params->port);
+		bnx2x_set_sfp_module_fault_led(params,
+					       MISC_REGISTERS_GPIO_HIGH);
+
 		if ((phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727) &&
 		    ((val & PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_MASK) ==
 		     PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_POWER_DOWN)) {
@@ -4995,18 +5058,17 @@ static u8 bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 		}
 	} else {
 		/* Turn off fault module-detected led */
-		DP(NETIF_MSG_LINK, "Turn off fault module-detected led\n");
-		bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_0,
-					  MISC_REGISTERS_GPIO_LOW,
-					  params->port);
+		bnx2x_set_sfp_module_fault_led(params, MISC_REGISTERS_GPIO_LOW);
 	}
 
 	/* power up the SFP module */
 	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8727)
 		bnx2x_8727_power_module(bp, phy, 1);
 
-	/* Check and set limiting mode / LRM mode on 8726.
-	On 8727 it is done automatically */
+	/*
+	 * Check and set limiting mode / LRM mode on 8726. On 8727 it
+	 * is done automatically
+	 */
 	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8726)
 		bnx2x_8726_set_limiting_mode(bp, phy, edc_mode);
 	else
@@ -5018,9 +5080,9 @@ static u8 bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 	if (rc == 0 ||
 	    (val & PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_MASK) !=
 	    PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_DISABLE_TX_LASER)
-		bnx2x_sfp_set_transmitter(bp, phy, params->port, 1);
+		bnx2x_sfp_set_transmitter(params, phy, 1);
 	else
-		bnx2x_sfp_set_transmitter(bp, phy, params->port, 0);
+		bnx2x_sfp_set_transmitter(params, phy, 0);
 
 	return rc;
 }
@@ -5033,11 +5095,9 @@ void bnx2x_handle_module_detect_int(struct link_params *params)
 	u8 port = params->port;
 
 	/* Set valid module led off */
-	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_0,
-			  MISC_REGISTERS_GPIO_HIGH,
-			  params->port);
+	bnx2x_set_sfp_module_fault_led(params, MISC_REGISTERS_GPIO_HIGH);
 
-	/* Get current gpio val refelecting module plugged in / out*/
+	/* Get current gpio val reflecting module plugged in / out*/
 	gpio_val = bnx2x_get_gpio(bp, MISC_REGISTERS_GPIO_3, port);
 
 	/* Call the handling function in case module is detected */
@@ -5053,18 +5113,20 @@ void bnx2x_handle_module_detect_int(struct link_params *params)
 			DP(NETIF_MSG_LINK, "SFP+ module is not initialized\n");
 	} else {
 		u32 val = REG_RD(bp, params->shmem_base +
-				     offsetof(struct shmem_region, dev_info.
-					      port_feature_config[params->port].
-					      config));
+				 offsetof(struct shmem_region, dev_info.
+					  port_feature_config[params->port].
+					  config));
 
 		bnx2x_set_gpio_int(bp, MISC_REGISTERS_GPIO_3,
 				   MISC_REGISTERS_GPIO_INT_OUTPUT_SET,
 				   port);
-		/* Module was plugged out. */
-		/* Disable transmit for this module */
+		/*
+		 * Module was plugged out.
+		 * Disable transmit for this module
+		 */
 		if ((val & PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_MASK) ==
 		    PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_DISABLE_TX_LASER)
-			bnx2x_sfp_set_transmitter(bp, phy, params->port, 0);
+			bnx2x_sfp_set_transmitter(params, phy, 0);
 	}
 }
 
@@ -5100,9 +5162,9 @@ static u8 bnx2x_8706_8726_read_status(struct bnx2x_phy *phy,
 
 	DP(NETIF_MSG_LINK, "8706/8726 rx_sd 0x%x pcs_status 0x%x 1Gbps"
 			" link_status 0x%x\n", rx_sd, pcs_status, val2);
-	/* link is up if both bit 0 of pmd_rx_sd and
-	 * bit 0 of pcs_status are set, or if the autoneg bit
-	 * 1 is set
+	/*
+	 * link is up if both bit 0 of pmd_rx_sd and bit 0 of pcs_status
+	 * are set, or if the autoneg bit 1 is set
 	 */
 	link_up = ((rx_sd & pcs_status & 0x1) || (val2 & (1<<1)));
 	if (link_up) {
@@ -5123,14 +5185,15 @@ static u8 bnx2x_8706_config_init(struct bnx2x_phy *phy,
 				 struct link_params *params,
 				 struct link_vars *vars)
 {
-	u16 cnt, val;
+	u32 tx_en_mode;
+	u16 cnt, val, tmp1;
 	struct bnx2x *bp = params->bp;
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
 	/* HW reset */
 	bnx2x_ext_phy_hw_reset(bp, params->port);
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 0xa040);
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 
 	/* Wait until fw is loaded */
 	for (cnt = 0; cnt < 100; cnt++) {
@@ -5197,6 +5260,26 @@ static u8 bnx2x_8706_config_init(struct bnx2x_phy *phy,
 				 0x0004);
 	}
 	bnx2x_save_bcm_spirom_ver(bp, phy, params->port);
+
+	/*
+	 * If TX Laser is controlled by GPIO_0, do not let PHY go into low
+	 * power mode, if TX Laser is disabled
+	 */
+
+	tx_en_mode = REG_RD(bp, params->shmem_base +
+			    offsetof(struct shmem_region,
+				dev_info.port_hw_config[params->port].sfp_ctrl))
+			& PORT_HW_CFG_TX_LASER_MASK;
+
+	if (tx_en_mode == PORT_HW_CFG_TX_LASER_GPIO0) {
+		DP(NETIF_MSG_LINK, "Enabling TXONOFF_PWRDN_DIS\n");
+		bnx2x_cl45_read(bp, phy,
+			MDIO_PMA_DEVAD, MDIO_PMA_REG_DIGITAL_CTRL, &tmp1);
+		tmp1 |= 0x1;
+		bnx2x_cl45_write(bp, phy,
+			MDIO_PMA_DEVAD, MDIO_PMA_REG_DIGITAL_CTRL, tmp1);
+	}
+
 	return 0;
 }
 
@@ -5231,26 +5314,26 @@ static void bnx2x_8726_external_rom_boot(struct bnx2x_phy *phy,
 
 	/* Set soft reset */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_GEN_CTRL,
-		       MDIO_PMA_REG_GEN_CTRL_ROM_MICRO_RESET);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_GEN_CTRL,
+			 MDIO_PMA_REG_GEN_CTRL_ROM_MICRO_RESET);
 
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_MISC_CTRL1, 0x0001);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_MISC_CTRL1, 0x0001);
 
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_GEN_CTRL,
-		       MDIO_PMA_REG_GEN_CTRL_ROM_RESET_INTERNAL_MP);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_GEN_CTRL,
+			 MDIO_PMA_REG_GEN_CTRL_ROM_RESET_INTERNAL_MP);
 
 	/* wait for 150ms for microcode load */
 	msleep(150);
 
 	/* Disable serial boot control, tristates pins SS_N, SCK, MOSI, MISO */
 	bnx2x_cl45_write(bp, phy,
-		       MDIO_PMA_DEVAD,
-		       MDIO_PMA_REG_MISC_CTRL1, 0x0000);
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_MISC_CTRL1, 0x0000);
 
 	msleep(200);
 	bnx2x_save_bcm_spirom_ver(bp, phy, params->port);
@@ -5285,23 +5368,18 @@ static u8 bnx2x_8726_config_init(struct bnx2x_phy *phy,
 	u32 val;
 	u32 swap_val, swap_override, aeu_gpio_mask, offset;
 	DP(NETIF_MSG_LINK, "Initializing BCM8726\n");
-	/* Restore normal power mode*/
-	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
-
-	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
 
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 1<<15);
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 
 	bnx2x_8726_external_rom_boot(phy, params);
 
-	/* Need to call module detected on initialization since
-	the module detection triggered by actual module
-	insertion might occur before driver is loaded, and when
-	driver is loaded, it reset all registers, including the
-	transmitter */
+	/*
+	 * Need to call module detected on initialization since the module
+	 * detection triggered by actual module insertion might occur before
+	 * driver is loaded, and when driver is loaded, it reset all
+	 * registers, including the transmitter
+	 */
 	bnx2x_sfp_module_detection(phy, params);
 
 	if (phy->req_line_speed == SPEED_1000) {
@@ -5334,8 +5412,10 @@ static u8 bnx2x_8726_config_init(struct bnx2x_phy *phy,
 				 MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x1000);
 		bnx2x_cl45_write(bp, phy,
 				MDIO_AN_DEVAD, MDIO_AN_REG_CTRL, 0x1200);
-		/* Enable RX-ALARM control to receive
-		interrupt for 1G speed change */
+		/*
+		 * Enable RX-ALARM control to receive interrupt for 1G speed
+		 * change
+		 */
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_PMA_DEVAD, MDIO_PMA_REG_LASI_CTRL, 0x4);
 		bnx2x_cl45_write(bp, phy,
@@ -5367,7 +5447,7 @@ static u8 bnx2x_8726_config_init(struct bnx2x_phy *phy,
 
 	/* Set GPIO3 to trigger SFP+ module insertion/removal */
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_3,
-			    MISC_REGISTERS_GPIO_INPUT_HI_Z, params->port);
+		       MISC_REGISTERS_GPIO_INPUT_HI_Z, params->port);
 
 	/* The GPIO should be swapped if the swap register is set and active */
 	swap_val = REG_RD(bp, NIG_REG_PORT_SWAP);
@@ -5458,7 +5538,7 @@ static void bnx2x_8727_hw_reset(struct bnx2x_phy *phy,
 				struct link_params *params) {
 	u32 swap_val, swap_override;
 	u8 port;
-	/**
+	/*
 	 * The PHY reset is controlled by GPIO 1. Fake the port number
 	 * to cancel the swap done in set_gpio()
 	 */
@@ -5467,20 +5547,21 @@ static void bnx2x_8727_hw_reset(struct bnx2x_phy *phy,
 	swap_override = REG_RD(bp, NIG_REG_STRAP_OVERRIDE);
 	port = (swap_val && swap_override) ^ 1;
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
 }
 
 static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 				 struct link_params *params,
 				 struct link_vars *vars)
 {
-	u16 tmp1, val, mod_abs;
+	u32 tx_en_mode;
+	u16 tmp1, val, mod_abs, tmp2;
 	u16 rx_alarm_ctrl_val;
 	u16 lasi_ctrl_val;
 	struct bnx2x *bp = params->bp;
 	/* Enable PMD link, MOD_ABS_FLT, and 1G link alarm */
 
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 	rx_alarm_ctrl_val = (1<<2) | (1<<5) ;
 	lasi_ctrl_val = 0x0004;
 
@@ -5493,14 +5574,17 @@ static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_LASI_CTRL, lasi_ctrl_val);
 
-	/* Initially configure  MOD_ABS to interrupt when
-	module is presence( bit 8) */
+	/*
+	 * Initially configure MOD_ABS to interrupt when module is
+	 * presence( bit 8)
+	 */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD, MDIO_PMA_REG_PHY_IDENTIFIER, &mod_abs);
-	/* Set EDC off by setting OPTXLOS signal input to low
-	(bit 9).
-	When the EDC is off it locks onto a reference clock and
-	avoids becoming 'lost'.*/
+	/*
+	 * Set EDC off by setting OPTXLOS signal input to low (bit 9).
+	 * When the EDC is off it locks onto a reference clock and avoids
+	 * becoming 'lost'
+	 */
 	mod_abs &= ~(1<<8);
 	if (!(phy->flags & FLAGS_NOC))
 		mod_abs &= ~(1<<9);
@@ -5515,7 +5599,7 @@ static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 	if (phy->flags & FLAGS_NOC)
 		val |= (3<<5);
 
-	/**
+	/*
 	 * Set 8727 GPIOs to input to allow reading from the 8727 GPIO0
 	 * status which reflect SFP+ module over-current
 	 */
@@ -5542,7 +5626,7 @@ static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 		bnx2x_cl45_read(bp, phy,
 				MDIO_PMA_DEVAD, MDIO_PMA_REG_10G_CTRL2, &tmp1);
 		DP(NETIF_MSG_LINK, "1.7 = 0x%x\n", tmp1);
-		/**
+		/*
 		 * Power down the XAUI until link is up in case of dual-media
 		 * and 1G
 		 */
@@ -5568,7 +5652,7 @@ static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 		bnx2x_cl45_write(bp, phy,
 				 MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x1300);
 	} else {
-		/**
+		/*
 		 * Since the 8727 has only single reset pin, need to set the 10G
 		 * registers although it is default
 		 */
@@ -5584,7 +5668,8 @@ static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 				 0x0008);
 	}
 
-	/* Set 2-wire transfer rate of SFP+ module EEPROM
+	/*
+	 * Set 2-wire transfer rate of SFP+ module EEPROM
 	 * to 100Khz since some DACs(direct attached cables) do
 	 * not work at 400Khz.
 	 */
@@ -5607,6 +5692,26 @@ static u8 bnx2x_8727_config_init(struct bnx2x_phy *phy,
 				 phy->tx_preemphasis[1]);
 	}
 
+	/*
+	 * If TX Laser is controlled by GPIO_0, do not let PHY go into low
+	 * power mode, if TX Laser is disabled
+	 */
+	tx_en_mode = REG_RD(bp, params->shmem_base +
+			    offsetof(struct shmem_region,
+				dev_info.port_hw_config[params->port].sfp_ctrl))
+			& PORT_HW_CFG_TX_LASER_MASK;
+
+	if (tx_en_mode == PORT_HW_CFG_TX_LASER_GPIO0) {
+
+		DP(NETIF_MSG_LINK, "Enabling TXONOFF_PWRDN_DIS\n");
+		bnx2x_cl45_read(bp, phy,
+			MDIO_PMA_DEVAD, MDIO_PMA_REG_8727_OPT_CFG_REG, &tmp2);
+		tmp2 |= 0x1000;
+		tmp2 &= 0xFFEF;
+		bnx2x_cl45_write(bp, phy,
+			MDIO_PMA_DEVAD, MDIO_PMA_REG_8727_OPT_CFG_REG, tmp2);
+	}
+
 	return 0;
 }
 
@@ -5620,46 +5725,49 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 				      port_feature_config[params->port].
 				      config));
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_PHY_IDENTIFIER, &mod_abs);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_PHY_IDENTIFIER, &mod_abs);
 	if (mod_abs & (1<<8)) {
 
 		/* Module is absent */
 		DP(NETIF_MSG_LINK, "MOD_ABS indication "
 			    "show module is absent\n");
 
-		/* 1. Set mod_abs to detect next module
-		presence event
-		   2. Set EDC off by setting OPTXLOS signal input to low
-			(bit 9).
-			When the EDC is off it locks onto a reference clock and
-			avoids becoming 'lost'.*/
+		/*
+		 * 1. Set mod_abs to detect next module
+		 *    presence event
+		 * 2. Set EDC off by setting OPTXLOS signal input to low
+		 *    (bit 9).
+		 *    When the EDC is off it locks onto a reference clock and
+		 *    avoids becoming 'lost'.
+		 */
 		mod_abs &= ~(1<<8);
 		if (!(phy->flags & FLAGS_NOC))
 			mod_abs &= ~(1<<9);
 		bnx2x_cl45_write(bp, phy,
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_PHY_IDENTIFIER, mod_abs);
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_PHY_IDENTIFIER, mod_abs);
 
-		/* Clear RX alarm since it stays up as long as
-		the mod_abs wasn't changed */
+		/*
+		 * Clear RX alarm since it stays up as long as
+		 * the mod_abs wasn't changed
+		 */
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_RX_ALARM, &rx_alarm_status);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_RX_ALARM, &rx_alarm_status);
 
 	} else {
 		/* Module is present */
 		DP(NETIF_MSG_LINK, "MOD_ABS indication "
 			    "show module is present\n");
-		/* First thing, disable transmitter,
-		and if the module is ok, the
-		module_detection will enable it*/
-
-		/* 1. Set mod_abs to detect next module
-		absent event ( bit 8)
-		   2. Restore the default polarity of the OPRXLOS signal and
-		this signal will then correctly indicate the presence or
-		absence of the Rx signal. (bit 9) */
+		/*
+		 * First disable transmitter, and if the module is ok, the
+		 * module_detection will enable it
+		 * 1. Set mod_abs to detect next module absent event ( bit 8)
+		 * 2. Restore the default polarity of the OPRXLOS signal and
+		 * this signal will then correctly indicate the presence or
+		 * absence of the Rx signal. (bit 9)
+		 */
 		mod_abs |= (1<<8);
 		if (!(phy->flags & FLAGS_NOC))
 			mod_abs |= (1<<9);
@@ -5667,10 +5775,12 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 				 MDIO_PMA_DEVAD,
 				 MDIO_PMA_REG_PHY_IDENTIFIER, mod_abs);
 
-		/* Clear RX alarm since it stays up as long as
-		the mod_abs wasn't changed. This is need to be done
-		before calling the module detection, otherwise it will clear
-		the link update alarm */
+		/*
+		 * Clear RX alarm since it stays up as long as the mod_abs
+		 * wasn't changed. This is need to be done before calling the
+		 * module detection, otherwise it will clear* the link update
+		 * alarm
+		 */
 		bnx2x_cl45_read(bp, phy,
 				MDIO_PMA_DEVAD,
 				MDIO_PMA_REG_RX_ALARM, &rx_alarm_status);
@@ -5678,7 +5788,7 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 
 		if ((val & PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_MASK) ==
 		    PORT_FEAT_CFG_OPT_MDL_ENFRCMNT_DISABLE_TX_LASER)
-			bnx2x_sfp_set_transmitter(bp, phy, params->port, 0);
+			bnx2x_sfp_set_transmitter(params, phy, 0);
 
 		if (bnx2x_wait_for_sfp_module_initialized(phy, params) == 0)
 			bnx2x_sfp_module_detection(phy, params);
@@ -5687,9 +5797,8 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 	}
 
 	DP(NETIF_MSG_LINK, "8727 RX_ALARM_STATUS 0x%x\n",
-		 rx_alarm_status);
-	/* No need to check link status in case of
-	module plugged in/out */
+		   rx_alarm_status);
+	/* No need to check link status in case of module plugged in/out */
 }
 
 static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
@@ -5725,7 +5834,7 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD, MDIO_PMA_REG_M8051_MSGOUT_REG, &val1);
 
-	/**
+	/*
 	 * If a module is present and there is need to check
 	 * for over current
 	 */
@@ -5745,12 +5854,8 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 					    " Please remove the SFP+ module and"
 					    " restart the system to clear this"
 					    " error.\n",
-				   params->port);
-
-			/*
-			 * Disable all RX_ALARMs except for
-			 * mod_abs
-			 */
+			 params->port);
+			/* Disable all RX_ALARMs except for mod_abs */
 			bnx2x_cl45_write(bp, phy,
 					 MDIO_PMA_DEVAD,
 					 MDIO_PMA_REG_RX_ALARM_CTRL, (1<<5));
@@ -5793,11 +5898,15 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 			MDIO_PMA_DEVAD,
 			MDIO_PMA_REG_8073_SPEED_LINK_STATUS, &link_status);
 
-	/* Bits 0..2 --> speed detected,
-	   bits 13..15--> link is down */
+	/*
+	 * Bits 0..2 --> speed detected,
+	 * Bits 13..15--> link is down
+	 */
 	if ((link_status & (1<<2)) && (!(link_status & (1<<15)))) {
 		link_up = 1;
 		vars->line_speed = SPEED_10000;
+		DP(NETIF_MSG_LINK, "port %x: External link up in 10G\n",
+			   params->port);
 	} else if ((link_status & (1<<0)) && (!(link_status & (1<<13)))) {
 		link_up = 1;
 		vars->line_speed = SPEED_1000;
@@ -5819,7 +5928,7 @@ static u8 bnx2x_8727_read_status(struct bnx2x_phy *phy,
 		bnx2x_cl45_read(bp, phy,
 				MDIO_PMA_DEVAD,
 				MDIO_PMA_REG_8727_PCS_GP, &val1);
-		/**
+		/*
 		 * In case of dual-media board and 1G, power up the XAUI side,
 		 * otherwise power it down. For 10G it is done automatically
 		 */
@@ -5839,7 +5948,7 @@ static void bnx2x_8727_link_reset(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	/* Disable Transmitter */
-	bnx2x_sfp_set_transmitter(bp, phy, params->port, 0);
+	bnx2x_sfp_set_transmitter(params, phy, 0);
 	/* Clear LASI */
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, MDIO_PMA_REG_LASI_CTRL, 0);
 
@@ -5851,19 +5960,23 @@ static void bnx2x_8727_link_reset(struct bnx2x_phy *phy,
 static void bnx2x_save_848xx_spirom_version(struct bnx2x_phy *phy,
 					   struct link_params *params)
 {
-	u16 val, fw_ver1, fw_ver2, cnt;
+	u16 val, fw_ver1, fw_ver2, cnt, adj;
 	struct bnx2x *bp = params->bp;
 
+	adj = 0;
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833)
+		adj = -1;
+
 	/* For the 32 bits registers in 848xx, access via MDIO2ARM interface.*/
 	/* (1) set register 0xc200_0014(SPI_BRIDGE_CTRL_2) to 0x03000000 */
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA819, 0x0014);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81A, 0xc200);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81B, 0x0000);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81C, 0x0300);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA817, 0x0009);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA819 + adj, 0x0014);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81A + adj, 0xc200);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81B + adj, 0x0000);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81C + adj, 0x0300);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA817 + adj, 0x0009);
 
 	for (cnt = 0; cnt < 100; cnt++) {
-		bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA818, &val);
+		bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA818 + adj, &val);
 		if (val & 1)
 			break;
 		udelay(5);
@@ -5877,11 +5990,11 @@ static void bnx2x_save_848xx_spirom_version(struct bnx2x_phy *phy,
 
 
 	/* 2) read register 0xc200_0000 (SPI_FW_STATUS) */
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA819, 0x0000);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81A, 0xc200);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA817, 0x000A);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA819 + adj, 0x0000);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA81A + adj, 0xc200);
+	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, 0xA817 + adj, 0x000A);
 	for (cnt = 0; cnt < 100; cnt++) {
-		bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA818, &val);
+		bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA818 + adj, &val);
 		if (val & 1)
 			break;
 		udelay(5);
@@ -5894,9 +6007,9 @@ static void bnx2x_save_848xx_spirom_version(struct bnx2x_phy *phy,
 	}
 
 	/* lower 16 bits of the register SPI_FW_STATUS */
-	bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA81B, &fw_ver1);
+	bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA81B + adj, &fw_ver1);
 	/* upper 16 bits of register SPI_FW_STATUS */
-	bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA81C, &fw_ver2);
+	bnx2x_cl45_read(bp, phy, MDIO_PMA_DEVAD, 0xA81C + adj, &fw_ver2);
 
 	bnx2x_save_spirom_version(bp, params->port, (fw_ver2<<16) | fw_ver1,
 				  phy->ver_addr);
@@ -5905,49 +6018,53 @@ 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;
+	u16 val, adj;
+
+	adj = 0;
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833)
+		adj = -1;
 
 	/* PHYC_CTL_LED_CTL */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD,
-			MDIO_PMA_REG_8481_LINK_SIGNAL, &val);
+			MDIO_PMA_REG_8481_LINK_SIGNAL + adj, &val);
 	val &= 0xFE00;
 	val |= 0x0092;
 
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
-			 MDIO_PMA_REG_8481_LINK_SIGNAL, val);
+			 MDIO_PMA_REG_8481_LINK_SIGNAL + adj, val);
 
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
-			 MDIO_PMA_REG_8481_LED1_MASK,
+			 MDIO_PMA_REG_8481_LED1_MASK + adj,
 			 0x80);
 
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
-			 MDIO_PMA_REG_8481_LED2_MASK,
+			 MDIO_PMA_REG_8481_LED2_MASK + adj,
 			 0x18);
 
 	/* Select activity source by Tx and Rx, as suggested by PHY AE */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
-			 MDIO_PMA_REG_8481_LED3_MASK,
+			 MDIO_PMA_REG_8481_LED3_MASK + adj,
 			 0x0006);
 
 	/* Select the closest activity blink rate to that in 10/100/1000 */
 	bnx2x_cl45_write(bp, phy,
 			MDIO_PMA_DEVAD,
-			MDIO_PMA_REG_8481_LED3_BLINK,
+			MDIO_PMA_REG_8481_LED3_BLINK + adj,
 			0);
 
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD,
-			MDIO_PMA_REG_84823_CTL_LED_CTL_1, &val);
+			MDIO_PMA_REG_84823_CTL_LED_CTL_1 + adj, &val);
 	val |= MDIO_PMA_REG_84823_LED3_STRETCH_EN; /* stretch_en for LED3*/
 
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
-			 MDIO_PMA_REG_84823_CTL_LED_CTL_1, val);
+			 MDIO_PMA_REG_84823_CTL_LED_CTL_1 + adj, val);
 
 	/* 'Interrupt Mask' */
 	bnx2x_cl45_write(bp, phy,
@@ -5961,7 +6078,11 @@ static u8 bnx2x_848xx_cmn_config_init(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u16 autoneg_val, an_1000_val, an_10_100_val;
-
+	/*
+	 * This phy uses the NIG latch mechanism since link indication
+	 * arrives through its LED4 and not via its LASI signal, so we
+	 * get steady signal instead of clear on read
+	 */
 	bnx2x_bits_en(bp, NIG_REG_LATCH_BC_0 + params->port*4,
 		      1 << NIG_LATCH_BC_ENABLE_MI_INT);
 
@@ -6086,11 +6207,11 @@ static u8 bnx2x_8481_config_init(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	/* Restore normal power mode*/
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
 
 	/* HW reset */
 	bnx2x_ext_phy_hw_reset(bp, params->port);
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 1<<15);
 	return bnx2x_848xx_cmn_config_init(phy, params, vars);
@@ -6102,12 +6223,15 @@ static u8 bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u8 port, initialize = 1;
-	u16 val;
+	u16 val, adj;
 	u16 temp;
-	u32 actual_phy_selection;
+	u32 actual_phy_selection, cms_enable;
 	u8 rc = 0;
 
 	/* This is just for MDIO_CTL_REG_84823_MEDIA register. */
+	adj = 0;
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833)
+		adj = 3;
 
 	msleep(1);
 	if (CHIP_IS_E2(bp))
@@ -6117,11 +6241,12 @@ static u8 bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_3,
 		       MISC_REGISTERS_GPIO_OUTPUT_HIGH,
 		       port);
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 	/* Wait for GPHY to come out of reset */
 	msleep(50);
-	/* BCM84823 requires that XGXS links up first @ 10G for normal
-	behavior */
+	/*
+	 * BCM84823 requires that XGXS links up first @ 10G for normal behavior
+	 */
 	temp = vars->line_speed;
 	vars->line_speed = SPEED_10000;
 	bnx2x_set_autoneg(&params->phy[INT_PHY], params, vars, 0);
@@ -6131,7 +6256,7 @@ static u8 bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 	/* Set dual-media configuration according to configuration */
 
 	bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
-			MDIO_CTL_REG_84823_MEDIA, &val);
+			MDIO_CTL_REG_84823_MEDIA + adj, &val);
 	val &= ~(MDIO_CTL_REG_84823_MEDIA_MAC_MASK |
 		 MDIO_CTL_REG_84823_MEDIA_LINE_MASK |
 		 MDIO_CTL_REG_84823_MEDIA_COPPER_CORE_DOWN |
@@ -6164,7 +6289,7 @@ static u8 bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 		val |= MDIO_CTL_REG_84823_MEDIA_FIBER_1G;
 
 	bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
-			 MDIO_CTL_REG_84823_MEDIA, val);
+			 MDIO_CTL_REG_84823_MEDIA + adj, val);
 	DP(NETIF_MSG_LINK, "Multi_phy config = 0x%x, Media control = 0x%x\n",
 		   params->multi_phy_config, val);
 
@@ -6172,23 +6297,43 @@ static u8 bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 		rc = bnx2x_848xx_cmn_config_init(phy, params, vars);
 	else
 		bnx2x_save_848xx_spirom_version(phy, params);
+	cms_enable = REG_RD(bp, params->shmem_base +
+			offsetof(struct shmem_region,
+			dev_info.port_hw_config[params->port].default_cfg)) &
+			PORT_HW_CFG_ENABLE_CMS_MASK;
+
+	bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
+		MDIO_CTL_REG_84823_USER_CTRL_REG, &val);
+	if (cms_enable)
+		val |= MDIO_CTL_REG_84823_USER_CTRL_CMS;
+	else
+		val &= ~MDIO_CTL_REG_84823_USER_CTRL_CMS;
+	bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
+		MDIO_CTL_REG_84823_USER_CTRL_REG, val);
+
+
 	return rc;
 }
 
 static u8 bnx2x_848xx_read_status(struct bnx2x_phy *phy,
-				       struct link_params *params,
-				       struct link_vars *vars)
+				  struct link_params *params,
+				  struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
-	u16 val, val1, val2;
+	u16 val, val1, val2, adj;
 	u8 link_up = 0;
 
+	/* Reg offset adjustment for 84833 */
+	adj = 0;
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833)
+		adj = -1;
+
 	/* Check 10G-BaseT link status */
 	/* Check PMD signal ok */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_AN_DEVAD, 0xFFFA, &val1);
 	bnx2x_cl45_read(bp, phy,
-			MDIO_PMA_DEVAD, MDIO_PMA_REG_8481_PMD_SIGNAL,
+			MDIO_PMA_DEVAD, MDIO_PMA_REG_8481_PMD_SIGNAL + adj,
 			&val2);
 	DP(NETIF_MSG_LINK, "BCM848xx: PMD_SIGNAL 1.a811 = 0x%x\n", val2);
 
@@ -6273,9 +6418,9 @@ static void bnx2x_8481_hw_reset(struct bnx2x_phy *phy,
 				struct link_params *params)
 {
 	bnx2x_set_gpio(params->bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW, 0);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW, 0);
 	bnx2x_set_gpio(params->bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW, 1);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW, 1);
 }
 
 static void bnx2x_8481_link_reset(struct bnx2x_phy *phy,
@@ -6297,8 +6442,8 @@ static void bnx2x_848x3_link_reset(struct bnx2x_phy *phy,
 	else
 		port = params->port;
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_3,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW,
-			    port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW,
+		       port);
 }
 
 static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
@@ -6353,24 +6498,24 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 
 			/* Set LED masks */
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED1_MASK,
-					0x0);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED1_MASK,
+					 0x0);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED2_MASK,
-					0x0);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED2_MASK,
+					 0x0);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED3_MASK,
-					0x0);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED3_MASK,
+					 0x0);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED5_MASK,
-					0x20);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED5_MASK,
+					 0x20);
 
 		} else {
 			bnx2x_cl45_write(bp, phy,
@@ -6394,35 +6539,35 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 			val |= 0x2492;
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LINK_SIGNAL,
-					val);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LINK_SIGNAL,
+					 val);
 
 			/* Set LED masks */
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED1_MASK,
-					0x0);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED1_MASK,
+					 0x0);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED2_MASK,
-					0x20);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED2_MASK,
+					 0x20);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED3_MASK,
-					0x20);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED3_MASK,
+					 0x20);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED5_MASK,
-					0x0);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED5_MASK,
+					 0x0);
 		} else {
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED1_MASK,
-					0x20);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED1_MASK,
+					 0x20);
 		}
 		break;
 
@@ -6440,9 +6585,9 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 					&val);
 
 			if (!((val &
-			      MDIO_PMA_REG_8481_LINK_SIGNAL_LED4_ENABLE_MASK)
-			   >> MDIO_PMA_REG_8481_LINK_SIGNAL_LED4_ENABLE_SHIFT)){
-				DP(NETIF_MSG_LINK, "Seting LINK_SIGNAL\n");
+			       MDIO_PMA_REG_8481_LINK_SIGNAL_LED4_ENABLE_MASK)
+			  >> MDIO_PMA_REG_8481_LINK_SIGNAL_LED4_ENABLE_SHIFT)) {
+				DP(NETIF_MSG_LINK, "Setting LINK_SIGNAL\n");
 				bnx2x_cl45_write(bp, phy,
 						 MDIO_PMA_DEVAD,
 						 MDIO_PMA_REG_8481_LINK_SIGNAL,
@@ -6451,24 +6596,24 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 
 			/* Set LED masks */
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED1_MASK,
-					0x10);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED1_MASK,
+					 0x10);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED2_MASK,
-					0x80);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED2_MASK,
+					 0x80);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED3_MASK,
-					0x98);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED3_MASK,
+					 0x98);
 
 			bnx2x_cl45_write(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8481_LED5_MASK,
-					0x40);
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8481_LED5_MASK,
+					 0x40);
 
 		} else {
 			bnx2x_cl45_write(bp, phy,
@@ -6513,10 +6658,10 @@ static u8 bnx2x_7101_config_init(struct bnx2x_phy *phy,
 
 	/* Restore normal power mode*/
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
+		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, params->port);
 	/* HW reset */
 	bnx2x_ext_phy_hw_reset(bp, params->port);
-	bnx2x_wait_reset_complete(bp, phy);
+	bnx2x_wait_reset_complete(bp, phy, params);
 
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_REG_LASI_CTRL, 0x1);
@@ -6563,9 +6708,7 @@ static u8 bnx2x_7101_read_status(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "10G-base-T PMA status 0x%x->0x%x\n",
 		   val2, val1);
 	link_up = ((val1 & 4) == 4);
-	/* if link is up
-	 * print the AN outcome of the SFX7101 PHY
-	 */
+	/* if link is up print the AN outcome of the SFX7101 PHY */
 	if (link_up) {
 		bnx2x_cl45_read(bp, phy,
 				MDIO_AN_DEVAD, MDIO_AN_REG_MASTER_STATUS,
@@ -6599,20 +6742,20 @@ void bnx2x_sfx7101_sp_sw_reset(struct bnx2x *bp, struct bnx2x_phy *phy)
 	u16 val, cnt;
 
 	bnx2x_cl45_read(bp, phy,
-		      MDIO_PMA_DEVAD,
-		      MDIO_PMA_REG_7101_RESET, &val);
+			MDIO_PMA_DEVAD,
+			MDIO_PMA_REG_7101_RESET, &val);
 
 	for (cnt = 0; cnt < 10; cnt++) {
 		msleep(50);
 		/* Writes a self-clearing reset */
 		bnx2x_cl45_write(bp, phy,
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_7101_RESET,
-			       (val | (1<<15)));
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_7101_RESET,
+				 (val | (1<<15)));
 		/* Wait for clear */
 		bnx2x_cl45_read(bp, phy,
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_7101_RESET, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_7101_RESET, &val);
 
 		if ((val & (1<<15)) == 0)
 			break;
@@ -6623,10 +6766,10 @@ static void bnx2x_7101_hw_reset(struct bnx2x_phy *phy,
 				struct link_params *params) {
 	/* Low power mode is controlled by GPIO 2 */
 	bnx2x_set_gpio(params->bp, MISC_REGISTERS_GPIO_2,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW, params->port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW, params->port);
 	/* The PHY reset is controlled by GPIO 1 */
 	bnx2x_set_gpio(params->bp, MISC_REGISTERS_GPIO_1,
-			    MISC_REGISTERS_GPIO_OUTPUT_LOW, params->port);
+		       MISC_REGISTERS_GPIO_OUTPUT_LOW, params->port);
 }
 
 static void bnx2x_7101_set_link_led(struct bnx2x_phy *phy,
@@ -6668,9 +6811,9 @@ static struct bnx2x_phy phy_null = {
 	.supported	= 0,
 	.media_type	= ETH_PHY_NOT_PRESENT,
 	.ver_addr	= 0,
-	.req_flow_ctrl  = 0,
-	.req_line_speed = 0,
-	.speed_cap_mask = 0,
+	.req_flow_ctrl	= 0,
+	.req_line_speed	= 0,
+	.speed_cap_mask	= 0,
 	.req_duplex	= 0,
 	.rsrv		= 0,
 	.config_init	= (config_init_t)NULL,
@@ -6705,8 +6848,8 @@ static struct bnx2x_phy phy_serdes = {
 	.media_type	= ETH_PHY_UNSPECIFIED,
 	.ver_addr	= 0,
 	.req_flow_ctrl	= 0,
-	.req_line_speed = 0,
-	.speed_cap_mask = 0,
+	.req_line_speed	= 0,
+	.speed_cap_mask	= 0,
 	.req_duplex	= 0,
 	.rsrv		= 0,
 	.config_init	= (config_init_t)bnx2x_init_serdes,
@@ -6742,8 +6885,8 @@ static struct bnx2x_phy phy_xgxs = {
 	.media_type	= ETH_PHY_UNSPECIFIED,
 	.ver_addr	= 0,
 	.req_flow_ctrl	= 0,
-	.req_line_speed = 0,
-	.speed_cap_mask = 0,
+	.req_line_speed	= 0,
+	.speed_cap_mask	= 0,
 	.req_duplex	= 0,
 	.rsrv		= 0,
 	.config_init	= (config_init_t)bnx2x_init_xgxs,
@@ -6773,8 +6916,8 @@ static struct bnx2x_phy phy_7101 = {
 	.media_type	= ETH_PHY_BASE_T,
 	.ver_addr	= 0,
 	.req_flow_ctrl	= 0,
-	.req_line_speed = 0,
-	.speed_cap_mask = 0,
+	.req_line_speed	= 0,
+	.speed_cap_mask	= 0,
 	.req_duplex	= 0,
 	.rsrv		= 0,
 	.config_init	= (config_init_t)bnx2x_7101_config_init,
@@ -6804,9 +6947,9 @@ static struct bnx2x_phy phy_8073 = {
 			   SUPPORTED_Asym_Pause),
 	.media_type	= ETH_PHY_UNSPECIFIED,
 	.ver_addr	= 0,
-	.req_flow_ctrl  = 0,
-	.req_line_speed = 0,
-	.speed_cap_mask = 0,
+	.req_flow_ctrl	= 0,
+	.req_line_speed	= 0,
+	.speed_cap_mask	= 0,
 	.req_duplex	= 0,
 	.rsrv		= 0,
 	.config_init	= (config_init_t)bnx2x_8073_config_init,
@@ -7015,6 +7158,43 @@ static struct bnx2x_phy phy_84823 = {
 	.phy_specific_func = (phy_specific_func_t)NULL
 };
 
+static struct bnx2x_phy phy_84833 = {
+	.type		= PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833,
+	.addr		= 0xff,
+	.flags		= FLAGS_FAN_FAILURE_DET_REQ |
+			    FLAGS_REARM_LATCH_SIGNAL,
+	.def_md_devad	= 0,
+	.reserved	= 0,
+	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
+	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
+	.mdio_ctrl	= 0,
+	.supported	= (SUPPORTED_10baseT_Half |
+			   SUPPORTED_10baseT_Full |
+			   SUPPORTED_100baseT_Half |
+			   SUPPORTED_100baseT_Full |
+			   SUPPORTED_1000baseT_Full |
+			   SUPPORTED_10000baseT_Full |
+			   SUPPORTED_TP |
+			   SUPPORTED_Autoneg |
+			   SUPPORTED_Pause |
+			   SUPPORTED_Asym_Pause),
+	.media_type	= ETH_PHY_BASE_T,
+	.ver_addr	= 0,
+	.req_flow_ctrl	= 0,
+	.req_line_speed	= 0,
+	.speed_cap_mask	= 0,
+	.req_duplex	= 0,
+	.rsrv		= 0,
+	.config_init	= (config_init_t)bnx2x_848x3_config_init,
+	.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,
+	.hw_reset	= (hw_reset_t)NULL,
+	.set_link_led	= (set_link_led_t)bnx2x_848xx_set_link_led,
+	.phy_specific_func = (phy_specific_func_t)NULL
+};
+
 /*****************************************************************/
 /*                                                               */
 /* Populate the phy according. Main function: bnx2x_populate_phy   */
@@ -7028,7 +7208,7 @@ static void bnx2x_populate_preemphasis(struct bnx2x *bp, u32 shmem_base,
 	/* Get the 4 lanes xgxs config rx and tx */
 	u32 rx = 0, tx = 0, i;
 	for (i = 0; i < 2; i++) {
-		/**
+		/*
 		 * INT_PHY and EXT_PHY1 share the same value location in the
 		 * shmem. When num_phys is greater than 1, than this value
 		 * applies only to EXT_PHY1
@@ -7036,19 +7216,19 @@ static void bnx2x_populate_preemphasis(struct bnx2x *bp, u32 shmem_base,
 		if (phy_index == INT_PHY || phy_index == EXT_PHY1) {
 			rx = REG_RD(bp, shmem_base +
 				    offsetof(struct shmem_region,
-			   dev_info.port_hw_config[port].xgxs_config_rx[i<<1]));
+			  dev_info.port_hw_config[port].xgxs_config_rx[i<<1]));
 
 			tx = REG_RD(bp, shmem_base +
 				    offsetof(struct shmem_region,
-			   dev_info.port_hw_config[port].xgxs_config_tx[i<<1]));
+			  dev_info.port_hw_config[port].xgxs_config_tx[i<<1]));
 		} else {
 			rx = REG_RD(bp, shmem_base +
 				    offsetof(struct shmem_region,
-			  dev_info.port_hw_config[port].xgxs_config2_rx[i<<1]));
+			 dev_info.port_hw_config[port].xgxs_config2_rx[i<<1]));
 
 			tx = REG_RD(bp, shmem_base +
 				    offsetof(struct shmem_region,
-			  dev_info.port_hw_config[port].xgxs_config2_rx[i<<1]));
+			 dev_info.port_hw_config[port].xgxs_config2_rx[i<<1]));
 		}
 
 		phy->rx_preemphasis[i << 1] = ((rx>>16) & 0xffff);
@@ -7168,6 +7348,9 @@ static u8 bnx2x_populate_ext_phy(struct bnx2x *bp,
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84823:
 		*phy = phy_84823;
 		break;
+	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833:
+		*phy = phy_84833;
+		break;
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_SFX7101:
 		*phy = phy_7101;
 		break;
@@ -7182,21 +7365,21 @@ static u8 bnx2x_populate_ext_phy(struct bnx2x *bp,
 	phy->addr = XGXS_EXT_PHY_ADDR(ext_phy_config);
 	bnx2x_populate_preemphasis(bp, shmem_base, phy, port, phy_index);
 
-	/**
-	* The shmem address of the phy version is located on different
-	* structures. In case this structure is too old, do not set
-	* the address
-	*/
+	/*
+	 * The shmem address of the phy version is located on different
+	 * structures. In case this structure is too old, do not set
+	 * the address
+	 */
 	config2 = REG_RD(bp, shmem_base + offsetof(struct shmem_region,
 					dev_info.shared_hw_config.config2));
 	if (phy_index == EXT_PHY1) {
 		phy->ver_addr = shmem_base + offsetof(struct shmem_region,
 				port_mb[port].ext_phy_fw_version);
 
-	/* Check specific mdc mdio settings */
-	if (config2 & SHARED_HW_CFG_MDC_MDIO_ACCESS1_MASK)
-		mdc_mdio_access = config2 &
-		SHARED_HW_CFG_MDC_MDIO_ACCESS1_MASK;
+		/* Check specific mdc mdio settings */
+		if (config2 & SHARED_HW_CFG_MDC_MDIO_ACCESS1_MASK)
+			mdc_mdio_access = config2 &
+			SHARED_HW_CFG_MDC_MDIO_ACCESS1_MASK;
 	} else {
 		u32 size = REG_RD(bp, shmem2_base);
 
@@ -7215,7 +7398,7 @@ static u8 bnx2x_populate_ext_phy(struct bnx2x *bp,
 	}
 	phy->mdio_ctrl = bnx2x_get_emac_base(bp, mdc_mdio_access, port);
 
-	/**
+	/*
 	 * In case mdc/mdio_access of the external phy is different than the
 	 * mdc/mdio access of the XGXS, a HW lock must be taken in each access
 	 * to prevent one port interfere with another port's CL45 operations.
@@ -7250,18 +7433,20 @@ static void bnx2x_phy_def_cfg(struct link_params *params,
 	/* Populate the default phy configuration for MF mode */
 	if (phy_index == EXT_PHY2) {
 		link_config = REG_RD(bp, params->shmem_base +
-					 offsetof(struct shmem_region, dev_info.
+				     offsetof(struct shmem_region, dev_info.
 			port_feature_config[params->port].link_config2));
 		phy->speed_cap_mask = REG_RD(bp, params->shmem_base +
-					offsetof(struct shmem_region, dev_info.
+					     offsetof(struct shmem_region,
+						      dev_info.
 			port_hw_config[params->port].speed_capability_mask2));
 	} else {
 		link_config = REG_RD(bp, params->shmem_base +
-				offsetof(struct shmem_region, dev_info.
+				     offsetof(struct shmem_region, dev_info.
 				port_feature_config[params->port].link_config));
 		phy->speed_cap_mask = REG_RD(bp, params->shmem_base +
-				offsetof(struct shmem_region, dev_info.
-			   port_hw_config[params->port].speed_capability_mask));
+					     offsetof(struct shmem_region,
+						      dev_info.
+			port_hw_config[params->port].speed_capability_mask));
 	}
 	DP(NETIF_MSG_LINK, "Default config phy idx %x cfg 0x%x speed_cap_mask"
 		       " 0x%x\n", phy_index, link_config, phy->speed_cap_mask);
@@ -7408,7 +7593,7 @@ static void set_phy_vars(struct link_params *params)
 			else if (phy_index == EXT_PHY2)
 				actual_phy_idx = EXT_PHY1;
 		}
-		params->phy[actual_phy_idx].req_flow_ctrl  =
+		params->phy[actual_phy_idx].req_flow_ctrl =
 			params->req_flow_ctrl[link_cfg_idx];
 
 		params->phy[actual_phy_idx].req_line_speed =
@@ -7461,57 +7646,6 @@ u8 bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 	set_phy_vars(params);
 
 	DP(NETIF_MSG_LINK, "Num of phys on board: %d\n", params->num_phys);
-	if (CHIP_REV_IS_FPGA(bp)) {
-
-		vars->link_up = 1;
-		vars->line_speed = SPEED_10000;
-		vars->duplex = DUPLEX_FULL;
-		vars->flow_ctrl = BNX2X_FLOW_CTRL_NONE;
-		vars->link_status = (LINK_STATUS_LINK_UP | LINK_10GTFD);
-		/* enable on E1.5 FPGA */
-		if (CHIP_IS_E1H(bp)) {
-			vars->flow_ctrl |=
-					(BNX2X_FLOW_CTRL_TX |
-					 BNX2X_FLOW_CTRL_RX);
-			vars->link_status |=
-					(LINK_STATUS_TX_FLOW_CONTROL_ENABLED |
-					 LINK_STATUS_RX_FLOW_CONTROL_ENABLED);
-		}
-
-		bnx2x_emac_enable(params, vars, 0);
-		if (!(CHIP_IS_E2(bp)))
-			bnx2x_pbf_update(params, vars->flow_ctrl,
-					 vars->line_speed);
-		/* disable drain */
-		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
-
-		/* update shared memory */
-		bnx2x_update_mng(params, vars->link_status);
-
-		return 0;
-
-	} else
-	if (CHIP_REV_IS_EMUL(bp)) {
-
-		vars->link_up = 1;
-		vars->line_speed = SPEED_10000;
-		vars->duplex = DUPLEX_FULL;
-		vars->flow_ctrl = BNX2X_FLOW_CTRL_NONE;
-		vars->link_status = (LINK_STATUS_LINK_UP | LINK_10GTFD);
-
-		bnx2x_bmac_enable(params, vars, 0);
-
-		bnx2x_pbf_update(params, vars->flow_ctrl, vars->line_speed);
-		/* Disable drain */
-		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE
-				    + params->port*4, 0);
-
-		/* update shared memory */
-		bnx2x_update_mng(params, vars->link_status);
-
-		return 0;
-
-	} else
 	if (params->loopback_mode == LOOPBACK_BMAC) {
 
 		vars->link_up = 1;
@@ -7527,8 +7661,7 @@ u8 bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 		/* set bmac loopback */
 		bnx2x_bmac_enable(params, vars, 1);
 
-		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE +
-		    params->port*4, 0);
+		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
 
 	} else if (params->loopback_mode == LOOPBACK_EMAC) {
 
@@ -7544,8 +7677,7 @@ u8 bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 		/* set bmac loopback */
 		bnx2x_emac_enable(params, vars, 1);
 		bnx2x_emac_program(params, vars);
-		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE +
-		    params->port*4, 0);
+		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
 
 	} else if ((params->loopback_mode == LOOPBACK_XGXS) ||
 		   (params->loopback_mode == LOOPBACK_EXT_PHY)) {
@@ -7568,8 +7700,7 @@ u8 bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 			bnx2x_emac_program(params, vars);
 			bnx2x_emac_enable(params, vars, 0);
 		} else
-		bnx2x_bmac_enable(params, vars, 0);
-
+			bnx2x_bmac_enable(params, vars, 0);
 		if (params->loopback_mode == LOOPBACK_XGXS) {
 			/* set 10G XGXS loopback */
 			params->phy[INT_PHY].config_loopback(
@@ -7587,9 +7718,7 @@ u8 bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 						params);
 			}
 		}
-
-		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE +
-			    params->port*4, 0);
+		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 0);
 
 		bnx2x_set_led(params, vars,
 			      LED_MODE_OPER, vars->line_speed);
@@ -7608,7 +7737,7 @@ u8 bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 	return 0;
 }
 u8 bnx2x_link_reset(struct link_params *params, struct link_vars *vars,
-		  u8 reset_ext_phy)
+		    u8 reset_ext_phy)
 {
 	struct bnx2x *bp = params->bp;
 	u8 phy_index, port = params->port, clear_latch_ind = 0;
@@ -7617,10 +7746,10 @@ u8 bnx2x_link_reset(struct link_params *params, struct link_vars *vars,
 	vars->link_status = 0;
 	bnx2x_update_mng(params, vars->link_status);
 	bnx2x_bits_dis(bp, NIG_REG_MASK_INTERRUPT_PORT0 + port*4,
-		     (NIG_MASK_XGXS0_LINK_STATUS |
-		      NIG_MASK_XGXS0_LINK10G |
-		      NIG_MASK_SERDES0_LINK_STATUS |
-		      NIG_MASK_MI_INT));
+		       (NIG_MASK_XGXS0_LINK_STATUS |
+			NIG_MASK_XGXS0_LINK10G |
+			NIG_MASK_SERDES0_LINK_STATUS |
+			NIG_MASK_MI_INT));
 
 	/* activate nig drain */
 	REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + port*4, 1);
@@ -7719,21 +7848,22 @@ static u8 bnx2x_8073_common_init_phy(struct bnx2x *bp,
 		/* disable attentions */
 		bnx2x_bits_dis(bp, NIG_REG_MASK_INTERRUPT_PORT0 +
 			       port_of_path*4,
-			     (NIG_MASK_XGXS0_LINK_STATUS |
-			      NIG_MASK_XGXS0_LINK10G |
-			      NIG_MASK_SERDES0_LINK_STATUS |
-			      NIG_MASK_MI_INT));
+			       (NIG_MASK_XGXS0_LINK_STATUS |
+				NIG_MASK_XGXS0_LINK10G |
+				NIG_MASK_SERDES0_LINK_STATUS |
+				NIG_MASK_MI_INT));
 
 		/* Need to take the phy out of low power mode in order
 			to write to access its registers */
 		bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-				  MISC_REGISTERS_GPIO_OUTPUT_HIGH, port);
+			       MISC_REGISTERS_GPIO_OUTPUT_HIGH,
+			       port);
 
 		/* Reset the phy */
 		bnx2x_cl45_write(bp, &phy[port],
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_CTRL,
-			       1<<15);
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_CTRL,
+				 1<<15);
 	}
 
 	/* Add delay of 150ms after reset */
@@ -7762,18 +7892,20 @@ static u8 bnx2x_8073_common_init_phy(struct bnx2x *bp,
 
 		/* Only set bit 10 = 1 (Tx power down) */
 		bnx2x_cl45_read(bp, phy_blk[port],
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_TX_POWER_DOWN, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_TX_POWER_DOWN, &val);
 
 		/* Phase1 of TX_POWER_DOWN reset */
 		bnx2x_cl45_write(bp, phy_blk[port],
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_TX_POWER_DOWN,
-			       (val | 1<<10));
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_TX_POWER_DOWN,
+				 (val | 1<<10));
 	}
 
-	/* Toggle Transmitter: Power down and then up with 600ms
-	   delay between */
+	/*
+	 * Toggle Transmitter: Power down and then up with 600ms delay
+	 * between
+	 */
 	msleep(600);
 
 	/* PART3 - complete TX_POWER_DOWN process, and set GPIO2 back to low */
@@ -7781,25 +7913,25 @@ static u8 bnx2x_8073_common_init_phy(struct bnx2x *bp,
 		/* Phase2 of POWER_DOWN_RESET */
 		/* Release bit 10 (Release Tx power down) */
 		bnx2x_cl45_read(bp, phy_blk[port],
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_TX_POWER_DOWN, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_TX_POWER_DOWN, &val);
 
 		bnx2x_cl45_write(bp, phy_blk[port],
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_TX_POWER_DOWN, (val & (~(1<<10))));
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_TX_POWER_DOWN, (val & (~(1<<10))));
 		msleep(15);
 
 		/* Read modify write the SPI-ROM version select register */
 		bnx2x_cl45_read(bp, phy_blk[port],
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_EDC_FFE_MAIN, &val);
+				MDIO_PMA_DEVAD,
+				MDIO_PMA_REG_EDC_FFE_MAIN, &val);
 		bnx2x_cl45_write(bp, phy_blk[port],
-			      MDIO_PMA_DEVAD,
-			      MDIO_PMA_REG_EDC_FFE_MAIN, (val | (1<<12)));
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_EDC_FFE_MAIN, (val | (1<<12)));
 
 		/* set GPIO2 back to LOW */
 		bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_2,
-				  MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
+			       MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
 	}
 	return 0;
 }
@@ -7846,32 +7978,90 @@ static u8 bnx2x_8726_common_init_phy(struct bnx2x *bp,
 
 		/* Set fault module detected LED on */
 		bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_0,
-				  MISC_REGISTERS_GPIO_HIGH,
-				  port);
+			       MISC_REGISTERS_GPIO_HIGH,
+			       port);
 	}
 
 	return 0;
 }
+static void bnx2x_get_ext_phy_reset_gpio(struct bnx2x *bp, u32 shmem_base,
+					 u8 *io_gpio, u8 *io_port)
+{
+
+	u32 phy_gpio_reset = REG_RD(bp, shmem_base +
+					  offsetof(struct shmem_region,
+				dev_info.port_hw_config[PORT_0].default_cfg));
+	switch (phy_gpio_reset) {
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO0_P0:
+		*io_gpio = 0;
+		*io_port = 0;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO1_P0:
+		*io_gpio = 1;
+		*io_port = 0;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO2_P0:
+		*io_gpio = 2;
+		*io_port = 0;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO3_P0:
+		*io_gpio = 3;
+		*io_port = 0;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO0_P1:
+		*io_gpio = 0;
+		*io_port = 1;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO1_P1:
+		*io_gpio = 1;
+		*io_port = 1;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO2_P1:
+		*io_gpio = 2;
+		*io_port = 1;
+		break;
+	case PORT_HW_CFG_EXT_PHY_GPIO_RST_GPIO3_P1:
+		*io_gpio = 3;
+		*io_port = 1;
+		break;
+	default:
+		/* Don't override the io_gpio and io_port */
+		break;
+	}
+}
 static u8 bnx2x_8727_common_init_phy(struct bnx2x *bp,
 				     u32 shmem_base_path[],
 				     u32 shmem2_base_path[], u8 phy_index,
 				     u32 chip_id)
 {
-	s8 port;
+	s8 port, reset_gpio;
 	u32 swap_val, swap_override;
 	struct bnx2x_phy phy[PORT_MAX];
 	struct bnx2x_phy *phy_blk[PORT_MAX];
 	s8 port_of_path;
-	swap_val = REG_RD(bp,  NIG_REG_PORT_SWAP);
-	swap_override = REG_RD(bp,  NIG_REG_STRAP_OVERRIDE);
+	swap_val = REG_RD(bp, NIG_REG_PORT_SWAP);
+	swap_override = REG_RD(bp, NIG_REG_STRAP_OVERRIDE);
 
+	reset_gpio = MISC_REGISTERS_GPIO_1;
 	port = 1;
 
-	bnx2x_ext_phy_hw_reset(bp, port ^ (swap_val && swap_override));
+	/*
+	 * Retrieve the reset gpio/port which control the reset.
+	 * Default is GPIO1, PORT1
+	 */
+	bnx2x_get_ext_phy_reset_gpio(bp, shmem_base_path[0],
+				     (u8 *)&reset_gpio, (u8 *)&port);
 
 	/* Calculate the port based on port swap */
 	port ^= (swap_val && swap_override);
 
+	/* Initiate PHY reset*/
+	bnx2x_set_gpio(bp, reset_gpio, MISC_REGISTERS_GPIO_OUTPUT_LOW,
+		       port);
+	msleep(1);
+	bnx2x_set_gpio(bp, reset_gpio, MISC_REGISTERS_GPIO_OUTPUT_HIGH,
+		       port);
+
 	msleep(5);
 
 	/* PART1 - Reset both phys */
@@ -7907,9 +8097,7 @@ static u8 bnx2x_8727_common_init_phy(struct bnx2x *bp,
 
 		/* Reset the phy */
 		bnx2x_cl45_write(bp, &phy[port],
-			       MDIO_PMA_DEVAD,
-			       MDIO_PMA_REG_CTRL,
-			       1<<15);
+				 MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 1<<15);
 	}
 
 	/* Add delay of 150ms after reset */
@@ -7923,7 +8111,7 @@ static u8 bnx2x_8727_common_init_phy(struct bnx2x *bp,
 	}
 	/* PART2 - Download firmware to both phys */
 	for (port = PORT_MAX - 1; port >= PORT_0; port--) {
-		 if (CHIP_IS_E2(bp))
+		if (CHIP_IS_E2(bp))
 			port_of_path = 0;
 		else
 			port_of_path = port;
@@ -7958,8 +8146,10 @@ static u8 bnx2x_ext_phy_common_init(struct bnx2x *bp, u32 shmem_base_path[],
 		break;
 
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8726:
-		/* GPIO1 affects both ports, so there's need to pull
-		it for single port alone */
+		/*
+		 * GPIO1 affects both ports, so there's need to pull
+		 * it for single port alone
+		 */
 		rc = bnx2x_8726_common_init_phy(bp, shmem_base_path,
 						shmem2_base_path,
 						phy_index, chip_id);
@@ -7969,11 +8159,15 @@ static u8 bnx2x_ext_phy_common_init(struct bnx2x *bp, u32 shmem_base_path[],
 		break;
 	default:
 		DP(NETIF_MSG_LINK,
-			 "bnx2x_common_init_phy: ext_phy 0x%x not required\n",
-			 ext_phy_type);
+			   "ext_phy 0x%x common init not required\n",
+			   ext_phy_type);
 		break;
 	}
 
+	if (rc != 0)
+		netdev_err(bp->dev,  "Warning: PHY was not initialized,"
+				      " Port %d\n",
+			 0);
 	return rc;
 }
 
@@ -7986,9 +8180,6 @@ u8 bnx2x_common_init_phy(struct bnx2x *bp, u32 shmem_base_path[],
 	u32 ext_phy_type, ext_phy_config;
 	DP(NETIF_MSG_LINK, "Begin common phy init\n");
 
-	if (CHIP_REV_IS_EMUL(bp))
-		return 0;
-
 	/* Check if common init was already done */
 	phy_ver = REG_RD(bp, shmem_base_path[0] +
 			 offsetof(struct shmem_region,
diff --git a/drivers/net/bnx2x/bnx2x_link.h b/drivers/net/bnx2x/bnx2x_link.h
index bedab1a942c4..92f36b6950dc 100644
--- a/drivers/net/bnx2x/bnx2x_link.h
+++ b/drivers/net/bnx2x/bnx2x_link.h
@@ -1,4 +1,4 @@
-/* Copyright 2008-2010 Broadcom Corporation
+/* Copyright 2008-2011 Broadcom Corporation
  *
  * Unless you and Broadcom execute a separate written software license
  * agreement governing use of this software, this software is licensed to you
@@ -33,7 +33,7 @@
 #define BNX2X_FLOW_CTRL_BOTH		PORT_FEATURE_FLOW_CONTROL_BOTH
 #define BNX2X_FLOW_CTRL_NONE		PORT_FEATURE_FLOW_CONTROL_NONE
 
-#define SPEED_AUTO_NEG	    0
+#define SPEED_AUTO_NEG		0
 #define SPEED_12000		12000
 #define SPEED_12500		12500
 #define SPEED_13000		13000
@@ -44,8 +44,8 @@
 #define SFP_EEPROM_VENDOR_NAME_SIZE		16
 #define SFP_EEPROM_VENDOR_OUI_ADDR		0x25
 #define SFP_EEPROM_VENDOR_OUI_SIZE		3
-#define SFP_EEPROM_PART_NO_ADDR 		0x28
-#define SFP_EEPROM_PART_NO_SIZE		16
+#define SFP_EEPROM_PART_NO_ADDR			0x28
+#define SFP_EEPROM_PART_NO_SIZE			16
 #define PWR_FLT_ERR_MSG_LEN			250
 
 #define XGXS_EXT_PHY_TYPE(ext_phy_config) \
@@ -62,7 +62,7 @@
 #define SINGLE_MEDIA(params)		(params->num_phys == 2)
 /* Dual Media board contains two external phy with different media */
 #define DUAL_MEDIA(params)		(params->num_phys == 3)
-#define FW_PARAM_MDIO_CTRL_OFFSET 16
+#define FW_PARAM_MDIO_CTRL_OFFSET		16
 #define FW_PARAM_SET(phy_addr, phy_type, mdio_access) \
 	(phy_addr | phy_type | mdio_access << FW_PARAM_MDIO_CTRL_OFFSET)
 
@@ -201,12 +201,14 @@ struct link_params {
 
 	/* Default / User Configuration */
 	u8 loopback_mode;
-#define LOOPBACK_NONE	0
-#define LOOPBACK_EMAC	1
-#define LOOPBACK_BMAC	2
+#define LOOPBACK_NONE		0
+#define LOOPBACK_EMAC		1
+#define LOOPBACK_BMAC		2
 #define LOOPBACK_XGXS		3
 #define LOOPBACK_EXT_PHY	4
-#define LOOPBACK_EXT 	5
+#define LOOPBACK_EXT		5
+#define LOOPBACK_UMAC		6
+#define LOOPBACK_XMAC		7
 
 	/* Device parameters */
 	u8 mac_addr[6];
@@ -230,10 +232,11 @@ struct link_params {
 	/* Phy register parameter */
 	u32 chip_id;
 
+	/* features */
 	u32 feature_config_flags;
-#define FEATURE_CONFIG_OVERRIDE_PREEMPHASIS_ENABLED (1<<0)
-#define FEATURE_CONFIG_PFC_ENABLED		(1<<1)
-#define FEATURE_CONFIG_BC_SUPPORTS_OPT_MDL_VRFY	(1<<2)
+#define FEATURE_CONFIG_OVERRIDE_PREEMPHASIS_ENABLED	(1<<0)
+#define FEATURE_CONFIG_PFC_ENABLED			(1<<1)
+#define FEATURE_CONFIG_BC_SUPPORTS_OPT_MDL_VRFY		(1<<2)
 #define FEATURE_CONFIG_BC_SUPPORTS_DUAL_PHY_OPT_MDL_VRFY	(1<<3)
 	/* Will be populated during common init */
 	struct bnx2x_phy phy[MAX_PHYS];
@@ -334,6 +337,11 @@ void bnx2x_ext_phy_hw_reset(struct bnx2x *bp, u8 port);
 /* Reset the external of SFX7101 */
 void bnx2x_sfx7101_sp_sw_reset(struct bnx2x *bp, struct bnx2x_phy *phy);
 
+/* Read "byte_cnt" bytes from address "addr" from the SFP+ EEPROM */
+u8 bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
+				struct link_params *params, u16 addr,
+				u8 byte_cnt, u8 *o_buf);
+
 void bnx2x_hw_reset_phy(struct link_params *params);
 
 /* Checks if HW lock is required for this phy/board type */
@@ -379,7 +387,7 @@ void bnx2x_ets_disabled(struct link_params *params);
 
 /* Used to configure the ETS to BW limited */
 void bnx2x_ets_bw_limit(const struct link_params *params, const u32 cos0_bw,
-						const u32 cos1_bw);
+			const u32 cos1_bw);
 
 /* Used to configure the ETS to strict */
 u8 bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos);
diff --git a/drivers/net/bnx2x/bnx2x_main.c b/drivers/net/bnx2x/bnx2x_main.c
index d584d32c747d..c907f862e03d 100644
--- a/drivers/net/bnx2x/bnx2x_main.c
+++ b/drivers/net/bnx2x/bnx2x_main.c
@@ -145,13 +145,6 @@ static struct {
 	{ "Broadcom NetXtreme II BCM57712E XGb" }
 };
 
-#ifndef PCI_DEVICE_ID_NX2_57712
-#define PCI_DEVICE_ID_NX2_57712		0x1662
-#endif
-#ifndef PCI_DEVICE_ID_NX2_57712E
-#define PCI_DEVICE_ID_NX2_57712E	0x1663
-#endif
-
 static DEFINE_PCI_DEVICE_TABLE(bnx2x_pci_tbl) = {
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57710), BCM57710 },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57711), BCM57711 },
@@ -586,7 +579,7 @@ static int bnx2x_issue_dmae_with_comp(struct bnx2x *bp,
 	   bp->slowpath->wb_data[2], bp->slowpath->wb_data[3]);
 
 	/* lock the dmae channel */
-	mutex_lock(&bp->dmae_mutex);
+	spin_lock_bh(&bp->dmae_lock);
 
 	/* reset completion */
 	*wb_comp = 0;
@@ -617,7 +610,7 @@ static int bnx2x_issue_dmae_with_comp(struct bnx2x *bp,
 	   bp->slowpath->wb_data[2], bp->slowpath->wb_data[3]);
 
 unlock:
-	mutex_unlock(&bp->dmae_mutex);
+	spin_unlock_bh(&bp->dmae_lock);
 	return rc;
 }
 
@@ -1397,7 +1390,7 @@ void bnx2x_sp_event(struct bnx2x_fastpath *fp,
 	}
 
 	smp_mb__before_atomic_inc();
-	atomic_inc(&bp->spq_left);
+	atomic_inc(&bp->cq_spq_left);
 	/* push the change in fp->state and towards the memory */
 	smp_wmb();
 
@@ -1974,13 +1967,22 @@ static void bnx2x_init_vn_minmax(struct bnx2x *bp, int vn)
 		vn_max_rate = 0;
 
 	} else {
+		u32 maxCfg = bnx2x_extract_max_cfg(bp, vn_cfg);
+
 		vn_min_rate = ((vn_cfg & FUNC_MF_CFG_MIN_BW_MASK) >>
 				FUNC_MF_CFG_MIN_BW_SHIFT) * 100;
-		/* If min rate is zero - set it to 1 */
+		/* If fairness is enabled (not all min rates are zeroes) and
+		   if current min rate is zero - set it to 1.
+		   This is a requirement of the algorithm. */
 		if (bp->vn_weight_sum && (vn_min_rate == 0))
 			vn_min_rate = DEF_MIN_RATE;
-		vn_max_rate = ((vn_cfg & FUNC_MF_CFG_MAX_BW_MASK) >>
-				FUNC_MF_CFG_MAX_BW_SHIFT) * 100;
+
+		if (IS_MF_SI(bp))
+			/* maxCfg in percents of linkspeed */
+			vn_max_rate = (bp->link_vars.line_speed * maxCfg) / 100;
+		else
+			/* maxCfg is absolute in 100Mb units */
+			vn_max_rate = maxCfg * 100;
 	}
 
 	DP(NETIF_MSG_IFUP,
@@ -2006,7 +2008,8 @@ static void bnx2x_init_vn_minmax(struct bnx2x *bp, int vn)
 		m_fair_vn.vn_credit_delta =
 			max_t(u32, (vn_min_rate * (T_FAIR_COEF /
 						   (8 * bp->vn_weight_sum))),
-			      (bp->cmng.fair_vars.fair_threshold * 2));
+			      (bp->cmng.fair_vars.fair_threshold +
+							MIN_ABOVE_THRESH));
 		DP(NETIF_MSG_IFUP, "m_fair_vn.vn_credit_delta %d\n",
 		   m_fair_vn.vn_credit_delta);
 	}
@@ -2082,8 +2085,9 @@ static void bnx2x_cmng_fns_init(struct bnx2x *bp, u8 read_cfg, u8 cmng_type)
 		bnx2x_calc_vn_weight_sum(bp);
 
 		/* calculate and set min-max rate for each vn */
-		for (vn = VN_0; vn < E1HVN_MAX; vn++)
-			bnx2x_init_vn_minmax(bp, vn);
+		if (bp->port.pmf)
+			for (vn = VN_0; vn < E1HVN_MAX; vn++)
+				bnx2x_init_vn_minmax(bp, vn);
 
 		/* always enable rate shaping and fairness */
 		bp->cmng.flags.cmng_enables |=
@@ -2152,13 +2156,6 @@ static void bnx2x_link_attn(struct bnx2x *bp)
 			bnx2x_stats_handle(bp, STATS_EVENT_LINK_UP);
 	}
 
-	/* indicate link status only if link status actually changed */
-	if (prev_link_status != bp->link_vars.link_status)
-		bnx2x_link_report(bp);
-
-	if (IS_MF(bp))
-		bnx2x_link_sync_notify(bp);
-
 	if (bp->link_vars.link_up && bp->link_vars.line_speed) {
 		int cmng_fns = bnx2x_get_cmng_fns_mode(bp);
 
@@ -2170,6 +2167,13 @@ static void bnx2x_link_attn(struct bnx2x *bp)
 			DP(NETIF_MSG_IFUP,
 			   "single function mode without fairness\n");
 	}
+
+	if (IS_MF(bp))
+		bnx2x_link_sync_notify(bp);
+
+	/* indicate link status only if link status actually changed */
+	if (prev_link_status != bp->link_vars.link_status)
+		bnx2x_link_report(bp);
 }
 
 void bnx2x__link_status_update(struct bnx2x *bp)
@@ -2473,8 +2477,14 @@ static void bnx2x_pf_rx_cl_prep(struct bnx2x *bp,
 	rxq_init->sge_map = fp->rx_sge_mapping;
 	rxq_init->rcq_map = fp->rx_comp_mapping;
 	rxq_init->rcq_np_map = fp->rx_comp_mapping + BCM_PAGE_SIZE;
-	rxq_init->mtu = bp->dev->mtu;
-	rxq_init->buf_sz = bp->rx_buf_size;
+
+	/* Always use mini-jumbo MTU for FCoE L2 ring */
+	if (IS_FCOE_FP(fp))
+		rxq_init->mtu = BNX2X_FCOE_MINI_JUMBO_MTU;
+	else
+		rxq_init->mtu = bp->dev->mtu;
+
+	rxq_init->buf_sz = fp->rx_buf_size;
 	rxq_init->cl_qzone_id = fp->cl_qzone_id;
 	rxq_init->cl_id = fp->cl_id;
 	rxq_init->spcl_id = fp->cl_id;
@@ -2726,11 +2736,18 @@ int bnx2x_sp_post(struct bnx2x *bp, int command, int cid,
 
 	spin_lock_bh(&bp->spq_lock);
 
-	if (!atomic_read(&bp->spq_left)) {
-		BNX2X_ERR("BUG! SPQ ring full!\n");
-		spin_unlock_bh(&bp->spq_lock);
-		bnx2x_panic();
-		return -EBUSY;
+	if (common) {
+		if (!atomic_read(&bp->eq_spq_left)) {
+			BNX2X_ERR("BUG! EQ ring full!\n");
+			spin_unlock_bh(&bp->spq_lock);
+			bnx2x_panic();
+			return -EBUSY;
+		}
+	} else if (!atomic_read(&bp->cq_spq_left)) {
+			BNX2X_ERR("BUG! SPQ ring full!\n");
+			spin_unlock_bh(&bp->spq_lock);
+			bnx2x_panic();
+			return -EBUSY;
 	}
 
 	spe = bnx2x_sp_get_next(bp);
@@ -2761,20 +2778,26 @@ int bnx2x_sp_post(struct bnx2x *bp, int command, int cid,
 	spe->data.update_data_addr.lo = cpu_to_le32(data_lo);
 
 	/* stats ramrod has it's own slot on the spq */
-	if (command != RAMROD_CMD_ID_COMMON_STAT_QUERY)
+	if (command != RAMROD_CMD_ID_COMMON_STAT_QUERY) {
 		/* It's ok if the actual decrement is issued towards the memory
 		 * somewhere between the spin_lock and spin_unlock. Thus no
 		 * more explict memory barrier is needed.
 		 */
-		atomic_dec(&bp->spq_left);
+		if (common)
+			atomic_dec(&bp->eq_spq_left);
+		else
+			atomic_dec(&bp->cq_spq_left);
+	}
+
 
 	DP(BNX2X_MSG_SP/*NETIF_MSG_TIMER*/,
 	   "SPQE[%x] (%x:%x)  command %d  hw_cid %x  data (%x:%x) "
-	   "type(0x%x) left %x\n",
+	   "type(0x%x) left (ETH, COMMON) (%x,%x)\n",
 	   bp->spq_prod_idx, (u32)U64_HI(bp->spq_mapping),
 	   (u32)(U64_LO(bp->spq_mapping) +
 	   (void *)bp->spq_prod_bd - (void *)bp->spq), command,
-	   HW_CID(bp, cid), data_hi, data_lo, type, atomic_read(&bp->spq_left));
+	   HW_CID(bp, cid), data_hi, data_lo, type,
+	   atomic_read(&bp->cq_spq_left), atomic_read(&bp->eq_spq_left));
 
 	bnx2x_sp_prod_update(bp);
 	spin_unlock_bh(&bp->spq_lock);
@@ -3686,8 +3709,8 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 	sw_cons = bp->eq_cons;
 	sw_prod = bp->eq_prod;
 
-	DP(BNX2X_MSG_SP, "EQ:  hw_cons %u  sw_cons %u bp->spq_left %u\n",
-			hw_cons, sw_cons, atomic_read(&bp->spq_left));
+	DP(BNX2X_MSG_SP, "EQ:  hw_cons %u  sw_cons %u bp->cq_spq_left %u\n",
+			hw_cons, sw_cons, atomic_read(&bp->eq_spq_left));
 
 	for (; sw_cons != hw_cons;
 	      sw_prod = NEXT_EQ_IDX(sw_prod), sw_cons = NEXT_EQ_IDX(sw_cons)) {
@@ -3752,13 +3775,15 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 		case (EVENT_RING_OPCODE_SET_MAC | BNX2X_STATE_OPEN):
 		case (EVENT_RING_OPCODE_SET_MAC | BNX2X_STATE_DIAG):
 			DP(NETIF_MSG_IFUP, "got set mac ramrod\n");
-			bp->set_mac_pending = 0;
+			if (elem->message.data.set_mac_event.echo)
+				bp->set_mac_pending = 0;
 			break;
 
 		case (EVENT_RING_OPCODE_SET_MAC |
 		      BNX2X_STATE_CLOSING_WAIT4_HALT):
 			DP(NETIF_MSG_IFDOWN, "got (un)set mac ramrod\n");
-			bp->set_mac_pending = 0;
+			if (elem->message.data.set_mac_event.echo)
+				bp->set_mac_pending = 0;
 			break;
 		default:
 			/* unknown event log error and continue */
@@ -3770,7 +3795,7 @@ next_spqe:
 	} /* for */
 
 	smp_mb__before_atomic_inc();
-	atomic_add(spqe_cnt, &bp->spq_left);
+	atomic_add(spqe_cnt, &bp->eq_spq_left);
 
 	bp->eq_cons = sw_cons;
 	bp->eq_prod = sw_prod;
@@ -4203,7 +4228,7 @@ void bnx2x_update_coalesce(struct bnx2x *bp)
 static void bnx2x_init_sp_ring(struct bnx2x *bp)
 {
 	spin_lock_init(&bp->spq_lock);
-	atomic_set(&bp->spq_left, MAX_SPQ_PENDING);
+	atomic_set(&bp->cq_spq_left, MAX_SPQ_PENDING);
 
 	bp->spq_prod_idx = 0;
 	bp->dsb_sp_prod = BNX2X_SP_DSB_INDEX;
@@ -4228,9 +4253,12 @@ static void bnx2x_init_eq_ring(struct bnx2x *bp)
 	bp->eq_cons = 0;
 	bp->eq_prod = NUM_EQ_DESC;
 	bp->eq_cons_sb = BNX2X_EQ_INDEX;
+	/* we want a warning message before it gets rought... */
+	atomic_set(&bp->eq_spq_left,
+		min_t(int, MAX_SP_DESC_CNT - MAX_SPQ_PENDING, NUM_EQ_DESC) - 1);
 }
 
-static void bnx2x_init_ind_table(struct bnx2x *bp)
+void bnx2x_push_indir_table(struct bnx2x *bp)
 {
 	int func = BP_FUNC(bp);
 	int i;
@@ -4238,13 +4266,20 @@ static void bnx2x_init_ind_table(struct bnx2x *bp)
 	if (bp->multi_mode == ETH_RSS_MODE_DISABLED)
 		return;
 
-	DP(NETIF_MSG_IFUP,
-	   "Initializing indirection table  multi_mode %d\n", bp->multi_mode);
 	for (i = 0; i < TSTORM_INDIRECTION_TABLE_SIZE; i++)
 		REG_WR8(bp, BAR_TSTRORM_INTMEM +
 			TSTORM_INDIRECTION_TABLE_OFFSET(func) + i,
-			bp->fp->cl_id + (i % (bp->num_queues -
-				NONE_ETH_CONTEXT_USE)));
+			bp->fp->cl_id + bp->rx_indir_table[i]);
+}
+
+static void bnx2x_init_ind_table(struct bnx2x *bp)
+{
+	int i;
+
+	for (i = 0; i < TSTORM_INDIRECTION_TABLE_SIZE; i++)
+		bp->rx_indir_table[i] = i % BNX2X_NUM_ETH_QUEUES(bp);
+
+	bnx2x_push_indir_table(bp);
 }
 
 void bnx2x_set_storm_rx_mode(struct bnx2x *bp)
@@ -5840,7 +5875,7 @@ int bnx2x_init_hw(struct bnx2x *bp, u32 load_code)
 	   BP_ABS_FUNC(bp), load_code);
 
 	bp->dmae_ready = 0;
-	mutex_init(&bp->dmae_mutex);
+	spin_lock_init(&bp->dmae_lock);
 	rc = bnx2x_gunzip_init(bp);
 	if (rc)
 		return rc;
@@ -5992,6 +6027,8 @@ void bnx2x_free_mem(struct bnx2x *bp)
 	BNX2X_PCI_FREE(bp->eq_ring, bp->eq_mapping,
 		       BCM_PAGE_SIZE * NUM_EQ_PAGES);
 
+	BNX2X_FREE(bp->rx_indir_table);
+
 #undef BNX2X_PCI_FREE
 #undef BNX2X_KFREE
 }
@@ -6122,6 +6159,9 @@ int bnx2x_alloc_mem(struct bnx2x *bp)
 	/* EQ */
 	BNX2X_PCI_ALLOC(bp->eq_ring, &bp->eq_mapping,
 			BCM_PAGE_SIZE * NUM_EQ_PAGES);
+
+	BNX2X_ALLOC(bp->rx_indir_table, sizeof(bp->rx_indir_table[0]) *
+		    TSTORM_INDIRECTION_TABLE_SIZE);
 	return 0;
 
 alloc_mem_err:
@@ -6175,12 +6215,14 @@ static void bnx2x_set_mac_addr_gen(struct bnx2x *bp, int set, const u8 *mac,
 	int ramrod_flags = WAIT_RAMROD_COMMON;
 
 	bp->set_mac_pending = 1;
-	smp_wmb();
 
 	config->hdr.length = 1;
 	config->hdr.offset = cam_offset;
 	config->hdr.client_id = 0xff;
-	config->hdr.reserved1 = 0;
+	/* Mark the single MAC configuration ramrod as opposed to a
+	 * UC/MC list configuration).
+	 */
+	config->hdr.echo = 1;
 
 	/* primary MAC */
 	config->config_table[0].msb_mac_addr =
@@ -6212,6 +6254,8 @@ static void bnx2x_set_mac_addr_gen(struct bnx2x *bp, int set, const u8 *mac,
 	   config->config_table[0].middle_mac_addr,
 	   config->config_table[0].lsb_mac_addr, BP_FUNC(bp), cl_bit_vec);
 
+	mb();
+
 	bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_MAC, 0,
 		      U64_HI(bnx2x_sp_mapping(bp, mac_config)),
 		      U64_LO(bnx2x_sp_mapping(bp, mac_config)), 1);
@@ -6276,20 +6320,15 @@ static u8 bnx2x_e1h_cam_offset(struct bnx2x *bp, u8 rel_offset)
 	if (CHIP_IS_E1H(bp))
 		return E1H_FUNC_MAX * rel_offset + BP_FUNC(bp);
 	else if (CHIP_MODE_IS_4_PORT(bp))
-		return BP_FUNC(bp) * 32  + rel_offset;
+		return E2_FUNC_MAX * rel_offset + BP_FUNC(bp);
 	else
-		return BP_VN(bp) * 32  + rel_offset;
+		return E2_FUNC_MAX * rel_offset + BP_VN(bp);
 }
 
 /**
  *  LLH CAM line allocations: currently only iSCSI and ETH macs are
  *  relevant. In addition, current implementation is tuned for a
  *  single ETH MAC.
- *
- *  When multiple unicast ETH MACs PF configuration in switch
- *  independent mode is required (NetQ, multiple netdev MACs,
- *  etc.), consider better utilisation of 16 per function MAC
- *  entries in the LLH memory.
  */
 enum {
 	LLH_CAM_ISCSI_ETH_LINE = 0,
@@ -6364,14 +6403,37 @@ void bnx2x_set_eth_mac(struct bnx2x *bp, int set)
 		bnx2x_set_mac_addr_gen(bp, set, bcast, 0, cam_offset + 1, 1);
 	}
 }
-static void bnx2x_set_e1_mc_list(struct bnx2x *bp, u8 offset)
+
+static inline u8 bnx2x_e1_cam_mc_offset(struct bnx2x *bp)
+{
+	return CHIP_REV_IS_SLOW(bp) ?
+		(BNX2X_MAX_EMUL_MULTI * (1 + BP_PORT(bp))) :
+		(BNX2X_MAX_MULTICAST * (1 + BP_PORT(bp)));
+}
+
+/* set mc list, do not wait as wait implies sleep and
+ * set_rx_mode can be invoked from non-sleepable context.
+ *
+ * Instead we use the same ramrod data buffer each time we need
+ * to configure a list of addresses, and use the fact that the
+ * list of MACs is changed in an incremental way and that the
+ * function is called under the netif_addr_lock. A temporary
+ * inconsistent CAM configuration (possible in case of a very fast
+ * sequence of add/del/add on the host side) will shortly be
+ * restored by the handler of the last ramrod.
+ */
+static int bnx2x_set_e1_mc_list(struct bnx2x *bp)
 {
 	int i = 0, old;
 	struct net_device *dev = bp->dev;
+	u8 offset = bnx2x_e1_cam_mc_offset(bp);
 	struct netdev_hw_addr *ha;
 	struct mac_configuration_cmd *config_cmd = bnx2x_sp(bp, mcast_config);
 	dma_addr_t config_cmd_map = bnx2x_sp_mapping(bp, mcast_config);
 
+	if (netdev_mc_count(dev) > BNX2X_MAX_MULTICAST)
+		return -EINVAL;
+
 	netdev_for_each_mc_addr(ha, dev) {
 		/* copy mac */
 		config_cmd->config_table[i].msb_mac_addr =
@@ -6412,32 +6474,47 @@ static void bnx2x_set_e1_mc_list(struct bnx2x *bp, u8 offset)
 		}
 	}
 
+	wmb();
+
 	config_cmd->hdr.length = i;
 	config_cmd->hdr.offset = offset;
 	config_cmd->hdr.client_id = 0xff;
-	config_cmd->hdr.reserved1 = 0;
+	/* Mark that this ramrod doesn't use bp->set_mac_pending for
+	 * synchronization.
+	 */
+	config_cmd->hdr.echo = 0;
 
-	bp->set_mac_pending = 1;
-	smp_wmb();
+	mb();
 
-	bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_MAC, 0,
+	return bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_MAC, 0,
 		   U64_HI(config_cmd_map), U64_LO(config_cmd_map), 1);
 }
-static void bnx2x_invlidate_e1_mc_list(struct bnx2x *bp)
+
+void bnx2x_invalidate_e1_mc_list(struct bnx2x *bp)
 {
 	int i;
 	struct mac_configuration_cmd *config_cmd = bnx2x_sp(bp, mcast_config);
 	dma_addr_t config_cmd_map = bnx2x_sp_mapping(bp, mcast_config);
 	int ramrod_flags = WAIT_RAMROD_COMMON;
+	u8 offset = bnx2x_e1_cam_mc_offset(bp);
 
-	bp->set_mac_pending = 1;
-	smp_wmb();
-
-	for (i = 0; i < config_cmd->hdr.length; i++)
+	for (i = 0; i < BNX2X_MAX_MULTICAST; i++)
 		SET_FLAG(config_cmd->config_table[i].flags,
 			MAC_CONFIGURATION_ENTRY_ACTION_TYPE,
 			T_ETH_MAC_COMMAND_INVALIDATE);
 
+	wmb();
+
+	config_cmd->hdr.length = BNX2X_MAX_MULTICAST;
+	config_cmd->hdr.offset = offset;
+	config_cmd->hdr.client_id = 0xff;
+	/* We'll wait for a completion this time... */
+	config_cmd->hdr.echo = 1;
+
+	bp->set_mac_pending = 1;
+
+	mb();
+
 	bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_MAC, 0,
 		      U64_HI(config_cmd_map), U64_LO(config_cmd_map), 1);
 
@@ -6447,6 +6524,44 @@ static void bnx2x_invlidate_e1_mc_list(struct bnx2x *bp)
 
 }
 
+/* Accept one or more multicasts */
+static int bnx2x_set_e1h_mc_list(struct bnx2x *bp)
+{
+	struct net_device *dev = bp->dev;
+	struct netdev_hw_addr *ha;
+	u32 mc_filter[MC_HASH_SIZE];
+	u32 crc, bit, regidx;
+	int i;
+
+	memset(mc_filter, 0, 4 * MC_HASH_SIZE);
+
+	netdev_for_each_mc_addr(ha, dev) {
+		DP(NETIF_MSG_IFUP, "Adding mcast MAC: %pM\n",
+		   bnx2x_mc_addr(ha));
+
+		crc = crc32c_le(0, bnx2x_mc_addr(ha),
+				ETH_ALEN);
+		bit = (crc >> 24) & 0xff;
+		regidx = bit >> 5;
+		bit &= 0x1f;
+		mc_filter[regidx] |= (1 << bit);
+	}
+
+	for (i = 0; i < MC_HASH_SIZE; i++)
+		REG_WR(bp, MC_HASH_OFFSET(bp, i),
+		       mc_filter[i]);
+
+	return 0;
+}
+
+void bnx2x_invalidate_e1h_mc_list(struct bnx2x *bp)
+{
+	int i;
+
+	for (i = 0; i < MC_HASH_SIZE; i++)
+		REG_WR(bp, MC_HASH_OFFSET(bp, i), 0);
+}
+
 #ifdef BCM_CNIC
 /**
  * Set iSCSI MAC(s) at the next enties in the CAM after the ETH
@@ -6465,12 +6580,13 @@ static int bnx2x_set_iscsi_eth_mac_addr(struct bnx2x *bp, int set)
 	u32 iscsi_l2_cl_id = BNX2X_ISCSI_ETH_CL_ID +
 		BP_E1HVN(bp) * NONE_ETH_CONTEXT_USE;
 	u32 cl_bit_vec = (1 << iscsi_l2_cl_id);
+	u8 *iscsi_mac = bp->cnic_eth_dev.iscsi_mac;
 
 	/* Send a SET_MAC ramrod */
-	bnx2x_set_mac_addr_gen(bp, set, bp->iscsi_mac, cl_bit_vec,
+	bnx2x_set_mac_addr_gen(bp, set, iscsi_mac, cl_bit_vec,
 			       cam_offset, 0);
 
-	bnx2x_set_mac_in_nig(bp, set, bp->iscsi_mac, LLH_CAM_ISCSI_ETH_LINE);
+	bnx2x_set_mac_in_nig(bp, set, iscsi_mac, LLH_CAM_ISCSI_ETH_LINE);
 
 	return 0;
 }
@@ -7112,20 +7228,15 @@ void bnx2x_chip_cleanup(struct bnx2x *bp, int unload_mode)
 	/* Give HW time to discard old tx messages */
 	msleep(1);
 
-	if (CHIP_IS_E1(bp)) {
-		/* invalidate mc list,
-		 * wait and poll (interrupts are off)
-		 */
-		bnx2x_invlidate_e1_mc_list(bp);
-		bnx2x_set_eth_mac(bp, 0);
-
-	} else {
-		REG_WR(bp, NIG_REG_LLH0_FUNC_EN + port*8, 0);
+	bnx2x_set_eth_mac(bp, 0);
 
-		bnx2x_set_eth_mac(bp, 0);
+	bnx2x_invalidate_uc_list(bp);
 
-		for (i = 0; i < MC_HASH_SIZE; i++)
-			REG_WR(bp, MC_HASH_OFFSET(bp, i), 0);
+	if (CHIP_IS_E1(bp))
+		bnx2x_invalidate_e1_mc_list(bp);
+	else {
+		bnx2x_invalidate_e1h_mc_list(bp);
+		REG_WR(bp, NIG_REG_LLH0_FUNC_EN + port*8, 0);
 	}
 
 #ifdef BCM_CNIC
@@ -8394,11 +8505,47 @@ static void __devinit bnx2x_get_port_hwinfo(struct bnx2x *bp)
 							bp->common.shmem2_base);
 }
 
+#ifdef BCM_CNIC
+static void __devinit bnx2x_get_cnic_info(struct bnx2x *bp)
+{
+	u32 max_iscsi_conn = FW_ENCODE_32BIT_PATTERN ^ SHMEM_RD(bp,
+				drv_lic_key[BP_PORT(bp)].max_iscsi_conn);
+	u32 max_fcoe_conn = FW_ENCODE_32BIT_PATTERN ^ SHMEM_RD(bp,
+				drv_lic_key[BP_PORT(bp)].max_fcoe_conn);
+
+	/* Get the number of maximum allowed iSCSI and FCoE connections */
+	bp->cnic_eth_dev.max_iscsi_conn =
+		(max_iscsi_conn & BNX2X_MAX_ISCSI_INIT_CONN_MASK) >>
+		BNX2X_MAX_ISCSI_INIT_CONN_SHIFT;
+
+	bp->cnic_eth_dev.max_fcoe_conn =
+		(max_fcoe_conn & BNX2X_MAX_FCOE_INIT_CONN_MASK) >>
+		BNX2X_MAX_FCOE_INIT_CONN_SHIFT;
+
+	BNX2X_DEV_INFO("max_iscsi_conn 0x%x max_fcoe_conn 0x%x\n",
+		       bp->cnic_eth_dev.max_iscsi_conn,
+		       bp->cnic_eth_dev.max_fcoe_conn);
+
+	/* If mamimum allowed number of connections is zero -
+	 * disable the feature.
+	 */
+	if (!bp->cnic_eth_dev.max_iscsi_conn)
+		bp->flags |= NO_ISCSI_OOO_FLAG | NO_ISCSI_FLAG;
+
+	if (!bp->cnic_eth_dev.max_fcoe_conn)
+		bp->flags |= NO_FCOE_FLAG;
+}
+#endif
+
 static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 {
 	u32 val, val2;
 	int func = BP_ABS_FUNC(bp);
 	int port = BP_PORT(bp);
+#ifdef BCM_CNIC
+	u8 *iscsi_mac = bp->cnic_eth_dev.iscsi_mac;
+	u8 *fip_mac = bp->fip_mac;
+#endif
 
 	if (BP_NOMCP(bp)) {
 		BNX2X_ERROR("warning: random MAC workaround active\n");
@@ -8411,7 +8558,9 @@ static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 			bnx2x_set_mac_buf(bp->dev->dev_addr, val, val2);
 
 #ifdef BCM_CNIC
-		/* iSCSI NPAR MAC */
+		/* iSCSI and FCoE NPAR MACs: if there is no either iSCSI or
+		 * FCoE MAC then the appropriate feature should be disabled.
+		 */
 		if (IS_MF_SI(bp)) {
 			u32 cfg = MF_CFG_RD(bp, func_ext_config[func].func_cfg);
 			if (cfg & MACP_FUNC_CFG_FLAGS_ISCSI_OFFLOAD) {
@@ -8419,8 +8568,39 @@ static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 						     iscsi_mac_addr_upper);
 				val = MF_CFG_RD(bp, func_ext_config[func].
 						    iscsi_mac_addr_lower);
-				bnx2x_set_mac_buf(bp->iscsi_mac, val, val2);
-			}
+				BNX2X_DEV_INFO("Read iSCSI MAC: "
+					       "0x%x:0x%04x\n", val2, val);
+				bnx2x_set_mac_buf(iscsi_mac, val, val2);
+
+				/* Disable iSCSI OOO if MAC configuration is
+				 * invalid.
+				 */
+				if (!is_valid_ether_addr(iscsi_mac)) {
+					bp->flags |= NO_ISCSI_OOO_FLAG |
+						     NO_ISCSI_FLAG;
+					memset(iscsi_mac, 0, ETH_ALEN);
+				}
+			} else
+				bp->flags |= NO_ISCSI_OOO_FLAG | NO_ISCSI_FLAG;
+
+			if (cfg & MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD) {
+				val2 = MF_CFG_RD(bp, func_ext_config[func].
+						     fcoe_mac_addr_upper);
+				val = MF_CFG_RD(bp, func_ext_config[func].
+						    fcoe_mac_addr_lower);
+				BNX2X_DEV_INFO("Read FCoE MAC to "
+					       "0x%x:0x%04x\n", val2, val);
+				bnx2x_set_mac_buf(fip_mac, val, val2);
+
+				/* Disable FCoE if MAC configuration is
+				 * invalid.
+				 */
+				if (!is_valid_ether_addr(fip_mac)) {
+					bp->flags |= NO_FCOE_FLAG;
+					memset(bp->fip_mac, 0, ETH_ALEN);
+				}
+			} else
+				bp->flags |= NO_FCOE_FLAG;
 		}
 #endif
 	} else {
@@ -8434,7 +8614,7 @@ static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 				    iscsi_mac_upper);
 		val = SHMEM_RD(bp, dev_info.port_hw_config[port].
 				   iscsi_mac_lower);
-		bnx2x_set_mac_buf(bp->iscsi_mac, val, val2);
+		bnx2x_set_mac_buf(iscsi_mac, val, val2);
 #endif
 	}
 
@@ -8442,14 +8622,12 @@ static void __devinit bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 	memcpy(bp->dev->perm_addr, bp->dev->dev_addr, ETH_ALEN);
 
 #ifdef BCM_CNIC
-	/* Inform the upper layers about FCoE MAC */
+	/* Set the FCoE MAC in modes other then MF_SI */
 	if (!CHIP_IS_E1x(bp)) {
 		if (IS_MF_SD(bp))
-			memcpy(bp->fip_mac, bp->dev->dev_addr,
-			       sizeof(bp->fip_mac));
-		else
-			memcpy(bp->fip_mac, bp->iscsi_mac,
-			       sizeof(bp->fip_mac));
+			memcpy(fip_mac, bp->dev->dev_addr, ETH_ALEN);
+		else if (!IS_MF(bp))
+			memcpy(fip_mac, iscsi_mac, ETH_ALEN);
 	}
 #endif
 }
@@ -8612,6 +8790,10 @@ static int __devinit bnx2x_get_hwinfo(struct bnx2x *bp)
 	/* Get MAC addresses */
 	bnx2x_get_mac_hwinfo(bp);
 
+#ifdef BCM_CNIC
+	bnx2x_get_cnic_info(bp);
+#endif
+
 	return rc;
 }
 
@@ -8826,12 +9008,197 @@ static int bnx2x_close(struct net_device *dev)
 	return 0;
 }
 
+#define E1_MAX_UC_LIST	29
+#define E1H_MAX_UC_LIST	30
+#define E2_MAX_UC_LIST	14
+static inline u8 bnx2x_max_uc_list(struct bnx2x *bp)
+{
+	if (CHIP_IS_E1(bp))
+		return E1_MAX_UC_LIST;
+	else if (CHIP_IS_E1H(bp))
+		return E1H_MAX_UC_LIST;
+	else
+		return E2_MAX_UC_LIST;
+}
+
+
+static inline u8 bnx2x_uc_list_cam_offset(struct bnx2x *bp)
+{
+	if (CHIP_IS_E1(bp))
+		/* CAM Entries for Port0:
+		 *      0 - prim ETH MAC
+		 *      1 - BCAST MAC
+		 *      2 - iSCSI L2 ring ETH MAC
+		 *      3-31 - UC MACs
+		 *
+		 * Port1 entries are allocated the same way starting from
+		 * entry 32.
+		 */
+		return 3 + 32 * BP_PORT(bp);
+	else if (CHIP_IS_E1H(bp)) {
+		/* CAM Entries:
+		 *      0-7  - prim ETH MAC for each function
+		 *      8-15 - iSCSI L2 ring ETH MAC for each function
+		 *      16 till 255 UC MAC lists for each function
+		 *
+		 * Remark: There is no FCoE support for E1H, thus FCoE related
+		 *         MACs are not considered.
+		 */
+		return E1H_FUNC_MAX * (CAM_ISCSI_ETH_LINE + 1) +
+			bnx2x_max_uc_list(bp) * BP_FUNC(bp);
+	} else {
+		/* CAM Entries (there is a separate CAM per engine):
+		 *      0-4  - prim ETH MAC for each function
+		 *      4-7 - iSCSI L2 ring ETH MAC for each function
+		 *      8-11 - FIP ucast L2 MAC for each function
+		 *      12-15 - ALL_ENODE_MACS mcast MAC for each function
+		 *      16 till 71 UC MAC lists for each function
+		 */
+		u8 func_idx =
+			(CHIP_MODE_IS_4_PORT(bp) ? BP_FUNC(bp) : BP_VN(bp));
+
+		return E2_FUNC_MAX * (CAM_MAX_PF_LINE + 1) +
+			bnx2x_max_uc_list(bp) * func_idx;
+	}
+}
+
+/* set uc list, do not wait as wait implies sleep and
+ * set_rx_mode can be invoked from non-sleepable context.
+ *
+ * Instead we use the same ramrod data buffer each time we need
+ * to configure a list of addresses, and use the fact that the
+ * list of MACs is changed in an incremental way and that the
+ * function is called under the netif_addr_lock. A temporary
+ * inconsistent CAM configuration (possible in case of very fast
+ * sequence of add/del/add on the host side) will shortly be
+ * restored by the handler of the last ramrod.
+ */
+static int bnx2x_set_uc_list(struct bnx2x *bp)
+{
+	int i = 0, old;
+	struct net_device *dev = bp->dev;
+	u8 offset = bnx2x_uc_list_cam_offset(bp);
+	struct netdev_hw_addr *ha;
+	struct mac_configuration_cmd *config_cmd = bnx2x_sp(bp, uc_mac_config);
+	dma_addr_t config_cmd_map = bnx2x_sp_mapping(bp, uc_mac_config);
+
+	if (netdev_uc_count(dev) > bnx2x_max_uc_list(bp))
+		return -EINVAL;
+
+	netdev_for_each_uc_addr(ha, dev) {
+		/* copy mac */
+		config_cmd->config_table[i].msb_mac_addr =
+			swab16(*(u16 *)&bnx2x_uc_addr(ha)[0]);
+		config_cmd->config_table[i].middle_mac_addr =
+			swab16(*(u16 *)&bnx2x_uc_addr(ha)[2]);
+		config_cmd->config_table[i].lsb_mac_addr =
+			swab16(*(u16 *)&bnx2x_uc_addr(ha)[4]);
+
+		config_cmd->config_table[i].vlan_id = 0;
+		config_cmd->config_table[i].pf_id = BP_FUNC(bp);
+		config_cmd->config_table[i].clients_bit_vector =
+			cpu_to_le32(1 << BP_L_ID(bp));
+
+		SET_FLAG(config_cmd->config_table[i].flags,
+			MAC_CONFIGURATION_ENTRY_ACTION_TYPE,
+			T_ETH_MAC_COMMAND_SET);
+
+		DP(NETIF_MSG_IFUP,
+		   "setting UCAST[%d] (%04x:%04x:%04x)\n", i,
+		   config_cmd->config_table[i].msb_mac_addr,
+		   config_cmd->config_table[i].middle_mac_addr,
+		   config_cmd->config_table[i].lsb_mac_addr);
+
+		i++;
+
+		/* Set uc MAC in NIG */
+		bnx2x_set_mac_in_nig(bp, 1, bnx2x_uc_addr(ha),
+				     LLH_CAM_ETH_LINE + i);
+	}
+	old = config_cmd->hdr.length;
+	if (old > i) {
+		for (; i < old; i++) {
+			if (CAM_IS_INVALID(config_cmd->
+					   config_table[i])) {
+				/* already invalidated */
+				break;
+			}
+			/* invalidate */
+			SET_FLAG(config_cmd->config_table[i].flags,
+				MAC_CONFIGURATION_ENTRY_ACTION_TYPE,
+				T_ETH_MAC_COMMAND_INVALIDATE);
+		}
+	}
+
+	wmb();
+
+	config_cmd->hdr.length = i;
+	config_cmd->hdr.offset = offset;
+	config_cmd->hdr.client_id = 0xff;
+	/* Mark that this ramrod doesn't use bp->set_mac_pending for
+	 * synchronization.
+	 */
+	config_cmd->hdr.echo = 0;
+
+	mb();
+
+	return bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_MAC, 0,
+		   U64_HI(config_cmd_map), U64_LO(config_cmd_map), 1);
+
+}
+
+void bnx2x_invalidate_uc_list(struct bnx2x *bp)
+{
+	int i;
+	struct mac_configuration_cmd *config_cmd = bnx2x_sp(bp, uc_mac_config);
+	dma_addr_t config_cmd_map = bnx2x_sp_mapping(bp, uc_mac_config);
+	int ramrod_flags = WAIT_RAMROD_COMMON;
+	u8 offset = bnx2x_uc_list_cam_offset(bp);
+	u8 max_list_size = bnx2x_max_uc_list(bp);
+
+	for (i = 0; i < max_list_size; i++) {
+		SET_FLAG(config_cmd->config_table[i].flags,
+			MAC_CONFIGURATION_ENTRY_ACTION_TYPE,
+			T_ETH_MAC_COMMAND_INVALIDATE);
+		bnx2x_set_mac_in_nig(bp, 0, NULL, LLH_CAM_ETH_LINE + 1 + i);
+	}
+
+	wmb();
+
+	config_cmd->hdr.length = max_list_size;
+	config_cmd->hdr.offset = offset;
+	config_cmd->hdr.client_id = 0xff;
+	/* We'll wait for a completion this time... */
+	config_cmd->hdr.echo = 1;
+
+	bp->set_mac_pending = 1;
+
+	mb();
+
+	bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_MAC, 0,
+		      U64_HI(config_cmd_map), U64_LO(config_cmd_map), 1);
+
+	/* Wait for a completion */
+	bnx2x_wait_ramrod(bp, 0, 0, &bp->set_mac_pending,
+				ramrod_flags);
+
+}
+
+static inline int bnx2x_set_mc_list(struct bnx2x *bp)
+{
+	/* some multicasts */
+	if (CHIP_IS_E1(bp)) {
+		return bnx2x_set_e1_mc_list(bp);
+	} else { /* E1H and newer */
+		return bnx2x_set_e1h_mc_list(bp);
+	}
+}
+
 /* called with netif_tx_lock from dev_mcast.c */
 void bnx2x_set_rx_mode(struct net_device *dev)
 {
 	struct bnx2x *bp = netdev_priv(dev);
 	u32 rx_mode = BNX2X_RX_MODE_NORMAL;
-	int port = BP_PORT(bp);
 
 	if (bp->state != BNX2X_STATE_OPEN) {
 		DP(NETIF_MSG_IFUP, "state is %x, returning\n", bp->state);
@@ -8842,47 +9209,16 @@ void bnx2x_set_rx_mode(struct net_device *dev)
 
 	if (dev->flags & IFF_PROMISC)
 		rx_mode = BNX2X_RX_MODE_PROMISC;
-	else if ((dev->flags & IFF_ALLMULTI) ||
-		 ((netdev_mc_count(dev) > BNX2X_MAX_MULTICAST) &&
-		  CHIP_IS_E1(bp)))
+	else if (dev->flags & IFF_ALLMULTI)
 		rx_mode = BNX2X_RX_MODE_ALLMULTI;
-	else { /* some multicasts */
-		if (CHIP_IS_E1(bp)) {
-			/*
-			 * set mc list, do not wait as wait implies sleep
-			 * and set_rx_mode can be invoked from non-sleepable
-			 * context
-			 */
-			u8 offset = (CHIP_REV_IS_SLOW(bp) ?
-				     BNX2X_MAX_EMUL_MULTI*(1 + port) :
-				     BNX2X_MAX_MULTICAST*(1 + port));
-
-			bnx2x_set_e1_mc_list(bp, offset);
-		} else { /* E1H */
-			/* Accept one or more multicasts */
-			struct netdev_hw_addr *ha;
-			u32 mc_filter[MC_HASH_SIZE];
-			u32 crc, bit, regidx;
-			int i;
-
-			memset(mc_filter, 0, 4 * MC_HASH_SIZE);
-
-			netdev_for_each_mc_addr(ha, dev) {
-				DP(NETIF_MSG_IFUP, "Adding mcast MAC: %pM\n",
-				   bnx2x_mc_addr(ha));
-
-				crc = crc32c_le(0, bnx2x_mc_addr(ha),
-						ETH_ALEN);
-				bit = (crc >> 24) & 0xff;
-				regidx = bit >> 5;
-				bit &= 0x1f;
-				mc_filter[regidx] |= (1 << bit);
-			}
+	else {
+		/* some multicasts */
+		if (bnx2x_set_mc_list(bp))
+			rx_mode = BNX2X_RX_MODE_ALLMULTI;
 
-			for (i = 0; i < MC_HASH_SIZE; i++)
-				REG_WR(bp, MC_HASH_OFFSET(bp, i),
-				       mc_filter[i]);
-		}
+		/* some unicasts */
+		if (bnx2x_set_uc_list(bp))
+			rx_mode = BNX2X_RX_MODE_PROMISC;
 	}
 
 	bp->rx_mode = rx_mode;
@@ -8963,7 +9299,7 @@ static const struct net_device_ops bnx2x_netdev_ops = {
 	.ndo_stop		= bnx2x_close,
 	.ndo_start_xmit		= bnx2x_start_xmit,
 	.ndo_select_queue	= bnx2x_select_queue,
-	.ndo_set_multicast_list	= bnx2x_set_rx_mode,
+	.ndo_set_rx_mode	= bnx2x_set_rx_mode,
 	.ndo_set_mac_address	= bnx2x_change_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_do_ioctl		= bnx2x_ioctl,
@@ -9109,7 +9445,7 @@ static int __devinit bnx2x_init_dev(struct pci_dev *pdev,
 	dev->vlan_features |= (NETIF_F_TSO | NETIF_F_TSO_ECN);
 	dev->vlan_features |= NETIF_F_TSO6;
 
-#ifdef BCM_DCB
+#ifdef BCM_DCBNL
 	dev->dcbnl_ops = &bnx2x_dcbnl_ops;
 #endif
 
@@ -9516,6 +9852,11 @@ static void __devexit bnx2x_remove_one(struct pci_dev *pdev)
 	}
 #endif
 
+#ifdef BCM_DCBNL
+	/* Delete app tlvs from dcbnl */
+	bnx2x_dcbnl_update_applist(bp, true);
+#endif
+
 	unregister_netdev(dev);
 
 	/* Delete all NAPI objects */
@@ -9789,15 +10130,21 @@ static void bnx2x_cnic_sp_post(struct bnx2x *bp, int count)
 					HW_CID(bp, BNX2X_ISCSI_ETH_CID));
 		}
 
-		/* There may be not more than 8 L2 and COMMON SPEs and not more
-		 * than 8 L5 SPEs in the air.
+		/* There may be not more than 8 L2 and not more than 8 L5 SPEs
+		 * We also check that the number of outstanding
+		 * COMMON ramrods is not more than the EQ and SPQ can
+		 * accommodate.
 		 */
-		if ((type == NONE_CONNECTION_TYPE) ||
-		    (type == ETH_CONNECTION_TYPE)) {
-			if (!atomic_read(&bp->spq_left))
+		if (type == ETH_CONNECTION_TYPE) {
+			if (!atomic_read(&bp->cq_spq_left))
+				break;
+			else
+				atomic_dec(&bp->cq_spq_left);
+		} else if (type == NONE_CONNECTION_TYPE) {
+			if (!atomic_read(&bp->eq_spq_left))
 				break;
 			else
-				atomic_dec(&bp->spq_left);
+				atomic_dec(&bp->eq_spq_left);
 		} else if ((type == ISCSI_CONNECTION_TYPE) ||
 			   (type == FCOE_CONNECTION_TYPE)) {
 			if (bp->cnic_spq_pending >=
@@ -9875,7 +10222,8 @@ static int bnx2x_cnic_ctl_send(struct bnx2x *bp, struct cnic_ctl_info *ctl)
 	int rc = 0;
 
 	mutex_lock(&bp->cnic_mutex);
-	c_ops = bp->cnic_ops;
+	c_ops = rcu_dereference_protected(bp->cnic_ops,
+					  lockdep_is_held(&bp->cnic_mutex));
 	if (c_ops)
 		rc = c_ops->cnic_ctl(bp->cnic_data, ctl);
 	mutex_unlock(&bp->cnic_mutex);
@@ -9989,7 +10337,7 @@ static int bnx2x_drv_ctl(struct net_device *dev, struct drv_ctl_info *ctl)
 		int count = ctl->data.credit.credit_count;
 
 		smp_mb__before_atomic_inc();
-		atomic_add(count, &bp->spq_left);
+		atomic_add(count, &bp->cq_spq_left);
 		smp_mb__after_atomic_inc();
 		break;
 	}
@@ -10085,6 +10433,13 @@ struct cnic_eth_dev *bnx2x_cnic_probe(struct net_device *dev)
 	struct bnx2x *bp = netdev_priv(dev);
 	struct cnic_eth_dev *cp = &bp->cnic_eth_dev;
 
+	/* If both iSCSI and FCoE are disabled - return NULL in
+	 * order to indicate CNIC that it should not try to work
+	 * with this device.
+	 */
+	if (NO_ISCSI(bp) && NO_FCOE(bp))
+		return NULL;
+
 	cp->drv_owner = THIS_MODULE;
 	cp->chip_id = CHIP_ID(bp);
 	cp->pdev = bp->pdev;
@@ -10105,6 +10460,15 @@ struct cnic_eth_dev *bnx2x_cnic_probe(struct net_device *dev)
 		BP_E1HVN(bp) * NONE_ETH_CONTEXT_USE;
 	cp->iscsi_l2_cid = BNX2X_ISCSI_ETH_CID;
 
+	if (NO_ISCSI_OOO(bp))
+		cp->drv_state |= CNIC_DRV_STATE_NO_ISCSI_OOO;
+
+	if (NO_ISCSI(bp))
+		cp->drv_state |= CNIC_DRV_STATE_NO_ISCSI;
+
+	if (NO_FCOE(bp))
+		cp->drv_state |= CNIC_DRV_STATE_NO_FCOE;
+
 	DP(BNX2X_MSG_SP, "page_size %d, tbl_offset %d, tbl_lines %d, "
 			 "starting cid %d\n",
 	   cp->ctx_blk_size,
diff --git a/drivers/net/bnx2x/bnx2x_reg.h b/drivers/net/bnx2x/bnx2x_reg.h
index e01330bb36c7..1c89f19a4425 100644
--- a/drivers/net/bnx2x/bnx2x_reg.h
+++ b/drivers/net/bnx2x/bnx2x_reg.h
@@ -6083,6 +6083,7 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_PMA_REG_8727_PCS_OPT_CTRL		0xc808
 #define MDIO_PMA_REG_8727_GPIO_CTRL		0xc80e
 #define MDIO_PMA_REG_8727_PCS_GP		0xc842
+#define MDIO_PMA_REG_8727_OPT_CFG_REG		0xc8e4
 
 #define MDIO_AN_REG_8727_MISC_CTRL		0x8309
 
diff --git a/drivers/net/bnx2x/bnx2x_stats.c b/drivers/net/bnx2x/bnx2x_stats.c
index bda60d590fa8..3445ded6674f 100644
--- a/drivers/net/bnx2x/bnx2x_stats.c
+++ b/drivers/net/bnx2x/bnx2x_stats.c
@@ -1239,14 +1239,14 @@ void bnx2x_stats_handle(struct bnx2x *bp, enum bnx2x_stats_event event)
 	if (unlikely(bp->panic))
 		return;
 
+	bnx2x_stats_stm[bp->stats_state][event].action(bp);
+
 	/* Protect a state change flow */
 	spin_lock_bh(&bp->stats_lock);
 	state = bp->stats_state;
 	bp->stats_state = bnx2x_stats_stm[state][event].next_state;
 	spin_unlock_bh(&bp->stats_lock);
 
-	bnx2x_stats_stm[state][event].action(bp);
-
 	if ((event != STATS_EVENT_UPDATE) || netif_msg_timer(bp))
 		DP(BNX2X_MSG_STATS, "state %d -> event %d -> state %d\n",
 		   state, event, bp->stats_state);
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index 1024ae158227..a5d5d0b5b155 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -281,23 +281,23 @@ static inline int __check_agg_selection_timer(struct port *port)
 }
 
 /**
- * __get_rx_machine_lock - lock the port's RX machine
+ * __get_state_machine_lock - lock the port's state machines
  * @port: the port we're looking at
  *
  */
-static inline void __get_rx_machine_lock(struct port *port)
+static inline void __get_state_machine_lock(struct port *port)
 {
-	spin_lock_bh(&(SLAVE_AD_INFO(port->slave).rx_machine_lock));
+	spin_lock_bh(&(SLAVE_AD_INFO(port->slave).state_machine_lock));
 }
 
 /**
- * __release_rx_machine_lock - unlock the port's RX machine
+ * __release_state_machine_lock - unlock the port's state machines
  * @port: the port we're looking at
  *
  */
-static inline void __release_rx_machine_lock(struct port *port)
+static inline void __release_state_machine_lock(struct port *port)
 {
-	spin_unlock_bh(&(SLAVE_AD_INFO(port->slave).rx_machine_lock));
+	spin_unlock_bh(&(SLAVE_AD_INFO(port->slave).state_machine_lock));
 }
 
 /**
@@ -388,14 +388,14 @@ static u8 __get_duplex(struct port *port)
 }
 
 /**
- * __initialize_port_locks - initialize a port's RX machine spinlock
+ * __initialize_port_locks - initialize a port's STATE machine spinlock
  * @port: the port we're looking at
  *
  */
 static inline void __initialize_port_locks(struct port *port)
 {
 	// make sure it isn't called twice
-	spin_lock_init(&(SLAVE_AD_INFO(port->slave).rx_machine_lock));
+	spin_lock_init(&(SLAVE_AD_INFO(port->slave).state_machine_lock));
 }
 
 //conversions
@@ -1025,9 +1025,6 @@ static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port)
 {
 	rx_states_t last_state;
 
-	// Lock to prevent 2 instances of this function to run simultaneously(rx interrupt and periodic machine callback)
-	__get_rx_machine_lock(port);
-
 	// keep current State Machine state to compare later if it was changed
 	last_state = port->sm_rx_state;
 
@@ -1133,7 +1130,6 @@ static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port)
 				pr_err("%s: An illegal loopback occurred on adapter (%s).\n"
 				       "Check the configuration to verify that all adapters are connected to 802.3ad compliant switch ports\n",
 				       port->slave->dev->master->name, port->slave->dev->name);
-				__release_rx_machine_lock(port);
 				return;
 			}
 			__update_selected(lacpdu, port);
@@ -1153,7 +1149,6 @@ static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port)
 			break;
 		}
 	}
-	__release_rx_machine_lock(port);
 }
 
 /**
@@ -2155,6 +2150,12 @@ void bond_3ad_state_machine_handler(struct work_struct *work)
 			goto re_arm;
 		}
 
+		/* Lock around state machines to protect data accessed
+		 * by all (e.g., port->sm_vars).  ad_rx_machine may run
+		 * concurrently due to incoming LACPDU.
+		 */
+		__get_state_machine_lock(port);
+
 		ad_rx_machine(NULL, port);
 		ad_periodic_machine(port);
 		ad_port_selection_logic(port);
@@ -2164,6 +2165,8 @@ void bond_3ad_state_machine_handler(struct work_struct *work)
 		// turn off the BEGIN bit, since we already handled it
 		if (port->sm_vars & AD_PORT_BEGIN)
 			port->sm_vars &= ~AD_PORT_BEGIN;
+
+		__release_state_machine_lock(port);
 	}
 
 re_arm:
@@ -2200,7 +2203,10 @@ static void bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave, u
 		case AD_TYPE_LACPDU:
 			pr_debug("Received LACPDU on port %d\n",
 				 port->actor_port_number);
+			/* Protect against concurrent state machines */
+			__get_state_machine_lock(port);
 			ad_rx_machine(lacpdu, port);
+			__release_state_machine_lock(port);
 			break;
 
 		case AD_TYPE_MARKER:
diff --git a/drivers/net/bonding/bond_3ad.h b/drivers/net/bonding/bond_3ad.h
index 2c46a154f2c6..b28baff70864 100644
--- a/drivers/net/bonding/bond_3ad.h
+++ b/drivers/net/bonding/bond_3ad.h
@@ -264,7 +264,8 @@ struct ad_bond_info {
 struct ad_slave_info {
 	struct aggregator aggregator;	    // 802.3ad aggregator structure
 	struct port port;		    // 802.3ad port structure
-	spinlock_t rx_machine_lock; // To avoid race condition between callback and receive interrupt
+	spinlock_t state_machine_lock; /* mutex state machines vs.
+					  incoming LACPDU */
 	u16 id;
 };
 
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 5c6fba802f2b..9bc5de3e04a8 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -604,7 +604,7 @@ static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bon
 
 	_lock_rx_hashtbl(bond);
 
-	hash_index = _simple_hash((u8 *)&arp->ip_dst, sizeof(arp->ip_src));
+	hash_index = _simple_hash((u8 *)&arp->ip_dst, sizeof(arp->ip_dst));
 	client_info = &(bond_info->rx_hashtbl[hash_index]);
 
 	if (client_info->assigned) {
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 163e0b06eaa5..7b7ca971672f 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -59,7 +59,6 @@
 #include <linux/uaccess.h>
 #include <linux/errno.h>
 #include <linux/netdevice.h>
-#include <linux/netpoll.h>
 #include <linux/inetdevice.h>
 #include <linux/igmp.h>
 #include <linux/etherdevice.h>
@@ -424,15 +423,9 @@ int bond_dev_queue_xmit(struct bonding *bond, struct sk_buff *skb,
 {
 	skb->dev = slave_dev;
 	skb->priority = 1;
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	if (unlikely(bond->dev->priv_flags & IFF_IN_NETPOLL)) {
-		struct netpoll *np = bond->dev->npinfo->netpoll;
-		slave_dev->npinfo = bond->dev->npinfo;
-		slave_dev->priv_flags |= IFF_IN_NETPOLL;
-		netpoll_send_skb_on_dev(np, skb, slave_dev);
-		slave_dev->priv_flags &= ~IFF_IN_NETPOLL;
-	} else
-#endif
+	if (unlikely(netpoll_tx_running(slave_dev)))
+		bond_netpoll_send_skb(bond_get_slave_by_dev(bond, slave_dev), skb);
+	else
 		dev_queue_xmit(skb);
 
 	return 0;
@@ -1288,63 +1281,105 @@ static void bond_detach_slave(struct bonding *bond, struct slave *slave)
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-/*
- * You must hold read lock on bond->lock before calling this.
- */
-static bool slaves_support_netpoll(struct net_device *bond_dev)
+static inline int slave_enable_netpoll(struct slave *slave)
 {
-	struct bonding *bond = netdev_priv(bond_dev);
-	struct slave *slave;
-	int i = 0;
-	bool ret = true;
+	struct netpoll *np;
+	int err = 0;
 
-	bond_for_each_slave(bond, slave, i) {
-		if ((slave->dev->priv_flags & IFF_DISABLE_NETPOLL) ||
-		    !slave->dev->netdev_ops->ndo_poll_controller)
-			ret = false;
+	np = kzalloc(sizeof(*np), GFP_KERNEL);
+	err = -ENOMEM;
+	if (!np)
+		goto out;
+
+	np->dev = slave->dev;
+	err = __netpoll_setup(np);
+	if (err) {
+		kfree(np);
+		goto out;
 	}
-	return i != 0 && ret;
+	slave->np = np;
+out:
+	return err;
+}
+static inline void slave_disable_netpoll(struct slave *slave)
+{
+	struct netpoll *np = slave->np;
+
+	if (!np)
+		return;
+
+	slave->np = NULL;
+	synchronize_rcu_bh();
+	__netpoll_cleanup(np);
+	kfree(np);
+}
+static inline bool slave_dev_support_netpoll(struct net_device *slave_dev)
+{
+	if (slave_dev->priv_flags & IFF_DISABLE_NETPOLL)
+		return false;
+	if (!slave_dev->netdev_ops->ndo_poll_controller)
+		return false;
+	return true;
 }
 
 static void bond_poll_controller(struct net_device *bond_dev)
 {
-	struct bonding *bond = netdev_priv(bond_dev);
+}
+
+static void __bond_netpoll_cleanup(struct bonding *bond)
+{
 	struct slave *slave;
 	int i;
 
-	bond_for_each_slave(bond, slave, i) {
-		if (slave->dev && IS_UP(slave->dev))
-			netpoll_poll_dev(slave->dev);
-	}
+	bond_for_each_slave(bond, slave, i)
+		if (IS_UP(slave->dev))
+			slave_disable_netpoll(slave);
 }
-
 static void bond_netpoll_cleanup(struct net_device *bond_dev)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
+
+	read_lock(&bond->lock);
+	__bond_netpoll_cleanup(bond);
+	read_unlock(&bond->lock);
+}
+
+static int bond_netpoll_setup(struct net_device *dev, struct netpoll_info *ni)
+{
+	struct bonding *bond = netdev_priv(dev);
 	struct slave *slave;
-	const struct net_device_ops *ops;
-	int i;
+	int i, err = 0;
 
 	read_lock(&bond->lock);
-	bond_dev->npinfo = NULL;
 	bond_for_each_slave(bond, slave, i) {
-		if (slave->dev) {
-			ops = slave->dev->netdev_ops;
-			if (ops->ndo_netpoll_cleanup)
-				ops->ndo_netpoll_cleanup(slave->dev);
-			else
-				slave->dev->npinfo = NULL;
+		if (!IS_UP(slave->dev))
+			continue;
+		err = slave_enable_netpoll(slave);
+		if (err) {
+			__bond_netpoll_cleanup(bond);
+			break;
 		}
 	}
 	read_unlock(&bond->lock);
+	return err;
 }
 
-#else
+static struct netpoll_info *bond_netpoll_info(struct bonding *bond)
+{
+	return bond->dev->npinfo;
+}
 
+#else
+static inline int slave_enable_netpoll(struct slave *slave)
+{
+	return 0;
+}
+static inline void slave_disable_netpoll(struct slave *slave)
+{
+}
 static void bond_netpoll_cleanup(struct net_device *bond_dev)
 {
 }
-
 #endif
 
 /*---------------------------------- IOCTL ----------------------------------*/
@@ -1372,8 +1407,8 @@ static int bond_compute_features(struct bonding *bond)
 {
 	struct slave *slave;
 	struct net_device *bond_dev = bond->dev;
-	unsigned long features = bond_dev->features;
-	unsigned long vlan_features = 0;
+	u32 features = bond_dev->features;
+	u32 vlan_features = 0;
 	unsigned short max_hard_header_len = max((u16)ETH_HLEN,
 						bond_dev->hard_header_len);
 	int i;
@@ -1400,8 +1435,8 @@ static int bond_compute_features(struct bonding *bond)
 
 done:
 	features |= (bond_dev->features & BOND_VLAN_FEATURES);
-	bond_dev->features = netdev_fix_features(features, NULL);
-	bond_dev->vlan_features = netdev_fix_features(vlan_features, NULL);
+	bond_dev->features = netdev_fix_features(bond_dev, features);
+	bond_dev->vlan_features = netdev_fix_features(bond_dev, vlan_features);
 	bond_dev->hard_header_len = max_hard_header_len;
 
 	return 0;
@@ -1423,6 +1458,71 @@ static void bond_setup_by_slave(struct net_device *bond_dev,
 	bond->setup_by_slave = 1;
 }
 
+/* On bonding slaves other than the currently active slave, suppress
+ * duplicates except for 802.3ad ETH_P_SLOW, alb non-mcast/bcast, and
+ * ARP on active-backup slaves with arp_validate enabled.
+ */
+static bool bond_should_deliver_exact_match(struct sk_buff *skb,
+					    struct net_device *slave_dev,
+					    struct net_device *bond_dev)
+{
+	if (slave_dev->priv_flags & IFF_SLAVE_INACTIVE) {
+		if (slave_dev->priv_flags & IFF_SLAVE_NEEDARP &&
+		    skb->protocol == __cpu_to_be16(ETH_P_ARP))
+			return false;
+
+		if (bond_dev->priv_flags & IFF_MASTER_ALB &&
+		    skb->pkt_type != PACKET_BROADCAST &&
+		    skb->pkt_type != PACKET_MULTICAST)
+				return false;
+
+		if (bond_dev->priv_flags & IFF_MASTER_8023AD &&
+		    skb->protocol == __cpu_to_be16(ETH_P_SLOW))
+			return false;
+
+		return true;
+	}
+	return false;
+}
+
+static struct sk_buff *bond_handle_frame(struct sk_buff *skb)
+{
+	struct net_device *slave_dev;
+	struct net_device *bond_dev;
+
+	skb = skb_share_check(skb, GFP_ATOMIC);
+	if (unlikely(!skb))
+		return NULL;
+	slave_dev = skb->dev;
+	bond_dev = ACCESS_ONCE(slave_dev->master);
+	if (unlikely(!bond_dev))
+		return skb;
+
+	if (bond_dev->priv_flags & IFF_MASTER_ARPMON)
+		slave_dev->last_rx = jiffies;
+
+	if (bond_should_deliver_exact_match(skb, slave_dev, bond_dev)) {
+		skb->deliver_no_wcard = 1;
+		return skb;
+	}
+
+	skb->dev = bond_dev;
+
+	if (bond_dev->priv_flags & IFF_MASTER_ALB &&
+	    bond_dev->priv_flags & IFF_BRIDGE_PORT &&
+	    skb->pkt_type == PACKET_HOST) {
+
+		if (unlikely(skb_cow_head(skb,
+					  skb->data - skb_mac_header(skb)))) {
+			kfree_skb(skb);
+			return NULL;
+		}
+		memcpy(eth_hdr(skb)->h_dest, bond_dev->dev_addr, ETH_ALEN);
+	}
+
+	return skb;
+}
+
 /* enslave device <slave> to bond device <master> */
 int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 {
@@ -1594,16 +1694,22 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 		}
 	}
 
-	res = netdev_set_master(slave_dev, bond_dev);
+	res = netdev_set_bond_master(slave_dev, bond_dev);
 	if (res) {
-		pr_debug("Error %d calling netdev_set_master\n", res);
+		pr_debug("Error %d calling netdev_set_bond_master\n", res);
 		goto err_restore_mac;
 	}
+	res = netdev_rx_handler_register(slave_dev, bond_handle_frame, NULL);
+	if (res) {
+		pr_debug("Error %d calling netdev_rx_handler_register\n", res);
+		goto err_unset_master;
+	}
+
 	/* open the slave since the application closed it */
 	res = dev_open(slave_dev);
 	if (res) {
 		pr_debug("Opening slave %s failed\n", slave_dev->name);
-		goto err_unset_master;
+		goto err_unreg_rxhandler;
 	}
 
 	new_slave->dev = slave_dev;
@@ -1782,17 +1888,19 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 	bond_set_carrier(bond);
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-	if (slaves_support_netpoll(bond_dev)) {
-		bond_dev->priv_flags &= ~IFF_DISABLE_NETPOLL;
-		if (bond_dev->npinfo)
-			slave_dev->npinfo = bond_dev->npinfo;
-	} else if (!(bond_dev->priv_flags & IFF_DISABLE_NETPOLL)) {
-		bond_dev->priv_flags |= IFF_DISABLE_NETPOLL;
-		pr_info("New slave device %s does not support netpoll\n",
-			slave_dev->name);
-		pr_info("Disabling netpoll support for %s\n", bond_dev->name);
+	slave_dev->npinfo = bond_netpoll_info(bond);
+	if (slave_dev->npinfo) {
+		if (slave_enable_netpoll(new_slave)) {
+			read_unlock(&bond->lock);
+			pr_info("Error, %s: master_dev is using netpoll, "
+				 "but new slave device does not support netpoll.\n",
+				 bond_dev->name);
+			res = -EBUSY;
+			goto err_close;
+		}
 	}
 #endif
+
 	read_unlock(&bond->lock);
 
 	res = bond_create_slave_symlinks(bond_dev, slave_dev);
@@ -1811,8 +1919,11 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 err_close:
 	dev_close(slave_dev);
 
+err_unreg_rxhandler:
+	netdev_rx_handler_unregister(slave_dev);
+
 err_unset_master:
-	netdev_set_master(slave_dev, NULL);
+	netdev_set_bond_master(slave_dev, NULL);
 
 err_restore_mac:
 	if (!bond->params.fail_over_mac) {
@@ -1992,19 +2103,10 @@ int bond_release(struct net_device *bond_dev, struct net_device *slave_dev)
 		netif_addr_unlock_bh(bond_dev);
 	}
 
-	netdev_set_master(slave_dev, NULL);
-
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	read_lock_bh(&bond->lock);
+	netdev_rx_handler_unregister(slave_dev);
+	netdev_set_bond_master(slave_dev, NULL);
 
-	if (slaves_support_netpoll(bond_dev))
-		bond_dev->priv_flags &= ~IFF_DISABLE_NETPOLL;
-	read_unlock_bh(&bond->lock);
-	if (slave_dev->netdev_ops->ndo_netpoll_cleanup)
-		slave_dev->netdev_ops->ndo_netpoll_cleanup(slave_dev);
-	else
-		slave_dev->npinfo = NULL;
-#endif
+	slave_disable_netpoll(slave);
 
 	/* close slave before restoring its mac address */
 	dev_close(slave_dev);
@@ -2039,6 +2141,7 @@ static int  bond_release_and_destroy(struct net_device *bond_dev,
 
 	ret = bond_release(bond_dev, slave_dev);
 	if ((ret == 0) && (bond->slave_cnt == 0)) {
+		bond_dev->priv_flags |= IFF_DISABLE_NETPOLL;
 		pr_info("%s: destroying bond %s.\n",
 			bond_dev->name, bond_dev->name);
 		unregister_netdevice(bond_dev);
@@ -2114,7 +2217,10 @@ static int bond_release_all(struct net_device *bond_dev)
 			netif_addr_unlock_bh(bond_dev);
 		}
 
-		netdev_set_master(slave_dev, NULL);
+		netdev_rx_handler_unregister(slave_dev);
+		netdev_set_bond_master(slave_dev, NULL);
+
+		slave_disable_netpoll(slave);
 
 		/* close slave before restoring its mac address */
 		dev_close(slave_dev);
@@ -2571,7 +2677,7 @@ static void bond_arp_send(struct net_device *slave_dev, int arp_op, __be32 dest_
 
 static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 {
-	int i, vlan_id, rv;
+	int i, vlan_id;
 	__be32 *targets = bond->params.arp_targets;
 	struct vlan_entry *vlan;
 	struct net_device *vlan_dev;
@@ -2598,8 +2704,8 @@ static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 		fl.fl4_dst = targets[i];
 		fl.fl4_tos = RTO_ONLINK;
 
-		rv = ip_route_output_key(dev_net(bond->dev), &rt, &fl);
-		if (rv) {
+		rt = ip_route_output_key(dev_net(bond->dev), &fl);
+		if (IS_ERR(rt)) {
 			if (net_ratelimit()) {
 				pr_warning("%s: no route to arp_ip_target %pI4\n",
 					   bond->dev->name, &fl.fl4_dst);
@@ -4654,9 +4760,12 @@ static const struct net_device_ops bond_netdev_ops = {
 	.ndo_vlan_rx_add_vid 	= bond_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= bond_vlan_rx_kill_vid,
 #ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_netpoll_setup	= bond_netpoll_setup,
 	.ndo_netpoll_cleanup	= bond_netpoll_cleanup,
 	.ndo_poll_controller	= bond_poll_controller,
 #endif
+	.ndo_add_slave		= bond_enslave,
+	.ndo_del_slave		= bond_release,
 };
 
 static void bond_destructor(struct net_device *bond_dev)
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index 8fd0174c5380..72bb0f6cc9bf 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -1198,7 +1198,7 @@ static ssize_t bonding_store_carrier(struct device *d,
 			bond->dev->name, new_value);
 	}
 out:
-	return count;
+	return ret;
 }
 static DEVICE_ATTR(use_carrier, S_IRUGO | S_IWUSR,
 		   bonding_show_carrier, bonding_store_carrier);
@@ -1595,7 +1595,7 @@ static ssize_t bonding_store_slaves_active(struct device *d,
 		}
 	}
 out:
-	return count;
+	return ret;
 }
 static DEVICE_ATTR(all_slaves_active, S_IRUGO | S_IWUSR,
 		   bonding_show_slaves_active, bonding_store_slaves_active);
diff --git a/drivers/net/bonding/bonding.h b/drivers/net/bonding/bonding.h
index 31fe980e4e28..ff4e26980220 100644
--- a/drivers/net/bonding/bonding.h
+++ b/drivers/net/bonding/bonding.h
@@ -20,6 +20,7 @@
 #include <linux/if_bonding.h>
 #include <linux/cpumask.h>
 #include <linux/in6.h>
+#include <linux/netpoll.h>
 #include "bond_3ad.h"
 #include "bond_alb.h"
 
@@ -132,7 +133,7 @@ static inline void unblock_netpoll_tx(void)
 
 static inline int is_netpoll_tx_blocked(struct net_device *dev)
 {
-	if (unlikely(dev->priv_flags & IFF_IN_NETPOLL))
+	if (unlikely(netpoll_tx_running(dev)))
 		return atomic_read(&netpoll_block_tx);
 	return 0;
 }
@@ -198,6 +199,9 @@ struct slave {
 	u16    queue_id;
 	struct ad_slave_info ad_info; /* HUGE - better to dynamically alloc */
 	struct tlb_slave_info tlb_info;
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	struct netpoll *np;
+#endif
 };
 
 /*
@@ -265,7 +269,8 @@ struct bonding {
  *
  * Caller must hold bond lock for read
  */
-static inline struct slave *bond_get_slave_by_dev(struct bonding *bond, struct net_device *slave_dev)
+static inline struct slave *bond_get_slave_by_dev(struct bonding *bond,
+						  struct net_device *slave_dev)
 {
 	struct slave *slave = NULL;
 	int i;
@@ -276,7 +281,7 @@ static inline struct slave *bond_get_slave_by_dev(struct bonding *bond, struct n
 		}
 	}
 
-	return 0;
+	return NULL;
 }
 
 static inline struct bonding *bond_get_bond_by_slave(struct slave *slave)
@@ -323,6 +328,22 @@ static inline unsigned long slave_last_rx(struct bonding *bond,
 	return slave->dev->last_rx;
 }
 
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static inline void bond_netpoll_send_skb(const struct slave *slave,
+					 struct sk_buff *skb)
+{
+	struct netpoll *np = slave->np;
+
+	if (np)
+		netpoll_send_skb(np, skb);
+}
+#else
+static inline void bond_netpoll_send_skb(const struct slave *slave,
+					 struct sk_buff *skb)
+{
+}
+#endif
+
 static inline void bond_set_slave_inactive_flags(struct slave *slave)
 {
 	struct bonding *bond = netdev_priv(slave->dev->master);
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index 5dec456fd4a4..1d699e3df547 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -115,6 +115,8 @@ source "drivers/net/can/mscan/Kconfig"
 
 source "drivers/net/can/sja1000/Kconfig"
 
+source "drivers/net/can/c_can/Kconfig"
+
 source "drivers/net/can/usb/Kconfig"
 
 source "drivers/net/can/softing/Kconfig"
diff --git a/drivers/net/can/Makefile b/drivers/net/can/Makefile
index 53c82a71778e..24ebfe8d758a 100644
--- a/drivers/net/can/Makefile
+++ b/drivers/net/can/Makefile
@@ -13,6 +13,7 @@ obj-y				+= softing/
 
 obj-$(CONFIG_CAN_SJA1000)	+= sja1000/
 obj-$(CONFIG_CAN_MSCAN)		+= mscan/
+obj-$(CONFIG_CAN_C_CAN)		+= c_can/
 obj-$(CONFIG_CAN_AT91)		+= at91_can.o
 obj-$(CONFIG_CAN_TI_HECC)	+= ti_hecc.o
 obj-$(CONFIG_CAN_MCP251X)	+= mcp251x.o
diff --git a/drivers/net/can/c_can/Kconfig b/drivers/net/can/c_can/Kconfig
new file mode 100644
index 000000000000..ffb9773d102d
--- /dev/null
+++ b/drivers/net/can/c_can/Kconfig
@@ -0,0 +1,15 @@
+menuconfig CAN_C_CAN
+	tristate "Bosch C_CAN devices"
+	depends on CAN_DEV && HAS_IOMEM
+
+if CAN_C_CAN
+
+config CAN_C_CAN_PLATFORM
+	tristate "Generic Platform Bus based C_CAN driver"
+	---help---
+	  This driver adds support for the C_CAN chips connected to
+	  the "platform bus" (Linux abstraction for directly to the
+	  processor attached devices) which can be found on various
+	  boards from ST Microelectronics (http://www.st.com)
+	  like the SPEAr1310 and SPEAr320 evaluation boards.
+endif
diff --git a/drivers/net/can/c_can/Makefile b/drivers/net/can/c_can/Makefile
new file mode 100644
index 000000000000..9273f6d5c4b7
--- /dev/null
+++ b/drivers/net/can/c_can/Makefile
@@ -0,0 +1,8 @@
+#
+#  Makefile for the Bosch C_CAN controller drivers.
+#
+
+obj-$(CONFIG_CAN_C_CAN) += c_can.o
+obj-$(CONFIG_CAN_C_CAN_PLATFORM) += c_can_platform.o
+
+ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/c_can/c_can.c b/drivers/net/can/c_can/c_can.c
new file mode 100644
index 000000000000..14050786218a
--- /dev/null
+++ b/drivers/net/can/c_can/c_can.c
@@ -0,0 +1,1158 @@
+/*
+ * CAN bus driver for Bosch C_CAN controller
+ *
+ * Copyright (C) 2010 ST Microelectronics
+ * Bhupesh Sharma <bhupesh.sharma@st.com>
+ *
+ * Borrowed heavily from the C_CAN driver originally written by:
+ * Copyright (C) 2007
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix <s.hauer@pengutronix.de>
+ * - Simon Kallweit, intefo AG <simon.kallweit@intefo.ch>
+ *
+ * TX and RX NAPI implementation has been borrowed from at91 CAN driver
+ * written by:
+ * Copyright
+ * (C) 2007 by Hans J. Koch <hjk@hansjkoch.de>
+ * (C) 2008, 2009 by Marc Kleine-Budde <kernel@pengutronix.de>
+ *
+ * Bosch C_CAN controller is compliant to CAN protocol version 2.0 part A and B.
+ * Bosch C_CAN user manual can be obtained from:
+ * http://www.semiconductors.bosch.de/media/en/pdf/ipmodules_1/c_can/
+ * users_manual_c_can.pdf
+ *
+ * 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/version.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+
+#include "c_can.h"
+
+/* control register */
+#define CONTROL_TEST		BIT(7)
+#define CONTROL_CCE		BIT(6)
+#define CONTROL_DISABLE_AR	BIT(5)
+#define CONTROL_ENABLE_AR	(0 << 5)
+#define CONTROL_EIE		BIT(3)
+#define CONTROL_SIE		BIT(2)
+#define CONTROL_IE		BIT(1)
+#define CONTROL_INIT		BIT(0)
+
+/* test register */
+#define TEST_RX			BIT(7)
+#define TEST_TX1		BIT(6)
+#define TEST_TX2		BIT(5)
+#define TEST_LBACK		BIT(4)
+#define TEST_SILENT		BIT(3)
+#define TEST_BASIC		BIT(2)
+
+/* status register */
+#define STATUS_BOFF		BIT(7)
+#define STATUS_EWARN		BIT(6)
+#define STATUS_EPASS		BIT(5)
+#define STATUS_RXOK		BIT(4)
+#define STATUS_TXOK		BIT(3)
+
+/* error counter register */
+#define ERR_CNT_TEC_MASK	0xff
+#define ERR_CNT_TEC_SHIFT	0
+#define ERR_CNT_REC_SHIFT	8
+#define ERR_CNT_REC_MASK	(0x7f << ERR_CNT_REC_SHIFT)
+#define ERR_CNT_RP_SHIFT	15
+#define ERR_CNT_RP_MASK		(0x1 << ERR_CNT_RP_SHIFT)
+
+/* bit-timing register */
+#define BTR_BRP_MASK		0x3f
+#define BTR_BRP_SHIFT		0
+#define BTR_SJW_SHIFT		6
+#define BTR_SJW_MASK		(0x3 << BTR_SJW_SHIFT)
+#define BTR_TSEG1_SHIFT		8
+#define BTR_TSEG1_MASK		(0xf << BTR_TSEG1_SHIFT)
+#define BTR_TSEG2_SHIFT		12
+#define BTR_TSEG2_MASK		(0x7 << BTR_TSEG2_SHIFT)
+
+/* brp extension register */
+#define BRP_EXT_BRPE_MASK	0x0f
+#define BRP_EXT_BRPE_SHIFT	0
+
+/* IFx command request */
+#define IF_COMR_BUSY		BIT(15)
+
+/* IFx command mask */
+#define IF_COMM_WR		BIT(7)
+#define IF_COMM_MASK		BIT(6)
+#define IF_COMM_ARB		BIT(5)
+#define IF_COMM_CONTROL		BIT(4)
+#define IF_COMM_CLR_INT_PND	BIT(3)
+#define IF_COMM_TXRQST		BIT(2)
+#define IF_COMM_DATAA		BIT(1)
+#define IF_COMM_DATAB		BIT(0)
+#define IF_COMM_ALL		(IF_COMM_MASK | IF_COMM_ARB | \
+				IF_COMM_CONTROL | IF_COMM_TXRQST | \
+				IF_COMM_DATAA | IF_COMM_DATAB)
+
+/* IFx arbitration */
+#define IF_ARB_MSGVAL		BIT(15)
+#define IF_ARB_MSGXTD		BIT(14)
+#define IF_ARB_TRANSMIT		BIT(13)
+
+/* IFx message control */
+#define IF_MCONT_NEWDAT		BIT(15)
+#define IF_MCONT_MSGLST		BIT(14)
+#define IF_MCONT_CLR_MSGLST	(0 << 14)
+#define IF_MCONT_INTPND		BIT(13)
+#define IF_MCONT_UMASK		BIT(12)
+#define IF_MCONT_TXIE		BIT(11)
+#define IF_MCONT_RXIE		BIT(10)
+#define IF_MCONT_RMTEN		BIT(9)
+#define IF_MCONT_TXRQST		BIT(8)
+#define IF_MCONT_EOB		BIT(7)
+#define IF_MCONT_DLC_MASK	0xf
+
+/*
+ * IFx register masks:
+ * allow easy operation on 16-bit registers when the
+ * argument is 32-bit instead
+ */
+#define IFX_WRITE_LOW_16BIT(x)	((x) & 0xFFFF)
+#define IFX_WRITE_HIGH_16BIT(x)	(((x) & 0xFFFF0000) >> 16)
+
+/* message object split */
+#define C_CAN_NO_OF_OBJECTS	32
+#define C_CAN_MSG_OBJ_RX_NUM	16
+#define C_CAN_MSG_OBJ_TX_NUM	16
+
+#define C_CAN_MSG_OBJ_RX_FIRST	1
+#define C_CAN_MSG_OBJ_RX_LAST	(C_CAN_MSG_OBJ_RX_FIRST + \
+				C_CAN_MSG_OBJ_RX_NUM - 1)
+
+#define C_CAN_MSG_OBJ_TX_FIRST	(C_CAN_MSG_OBJ_RX_LAST + 1)
+#define C_CAN_MSG_OBJ_TX_LAST	(C_CAN_MSG_OBJ_TX_FIRST + \
+				C_CAN_MSG_OBJ_TX_NUM - 1)
+
+#define C_CAN_MSG_OBJ_RX_SPLIT	9
+#define C_CAN_MSG_RX_LOW_LAST	(C_CAN_MSG_OBJ_RX_SPLIT - 1)
+
+#define C_CAN_NEXT_MSG_OBJ_MASK	(C_CAN_MSG_OBJ_TX_NUM - 1)
+#define RECEIVE_OBJECT_BITS	0x0000ffff
+
+/* status interrupt */
+#define STATUS_INTERRUPT	0x8000
+
+/* global interrupt masks */
+#define ENABLE_ALL_INTERRUPTS	1
+#define DISABLE_ALL_INTERRUPTS	0
+
+/* minimum timeout for checking BUSY status */
+#define MIN_TIMEOUT_VALUE	6
+
+/* napi related */
+#define C_CAN_NAPI_WEIGHT	C_CAN_MSG_OBJ_RX_NUM
+
+/* c_can lec values */
+enum c_can_lec_type {
+	LEC_NO_ERROR = 0,
+	LEC_STUFF_ERROR,
+	LEC_FORM_ERROR,
+	LEC_ACK_ERROR,
+	LEC_BIT1_ERROR,
+	LEC_BIT0_ERROR,
+	LEC_CRC_ERROR,
+	LEC_UNUSED,
+};
+
+/*
+ * c_can error types:
+ * Bus errors (BUS_OFF, ERROR_WARNING, ERROR_PASSIVE) are supported
+ */
+enum c_can_bus_error_types {
+	C_CAN_NO_ERROR = 0,
+	C_CAN_BUS_OFF,
+	C_CAN_ERROR_WARNING,
+	C_CAN_ERROR_PASSIVE,
+};
+
+static struct can_bittiming_const c_can_bittiming_const = {
+	.name = KBUILD_MODNAME,
+	.tseg1_min = 2,		/* Time segment 1 = prop_seg + phase_seg1 */
+	.tseg1_max = 16,
+	.tseg2_min = 1,		/* Time segment 2 = phase_seg2 */
+	.tseg2_max = 8,
+	.sjw_max = 4,
+	.brp_min = 1,
+	.brp_max = 1024,	/* 6-bit BRP field + 4-bit BRPE field*/
+	.brp_inc = 1,
+};
+
+static inline int get_tx_next_msg_obj(const struct c_can_priv *priv)
+{
+	return (priv->tx_next & C_CAN_NEXT_MSG_OBJ_MASK) +
+			C_CAN_MSG_OBJ_TX_FIRST;
+}
+
+static inline int get_tx_echo_msg_obj(const struct c_can_priv *priv)
+{
+	return (priv->tx_echo & C_CAN_NEXT_MSG_OBJ_MASK) +
+			C_CAN_MSG_OBJ_TX_FIRST;
+}
+
+static u32 c_can_read_reg32(struct c_can_priv *priv, void *reg)
+{
+	u32 val = priv->read_reg(priv, reg);
+	val |= ((u32) priv->read_reg(priv, reg + 2)) << 16;
+	return val;
+}
+
+static void c_can_enable_all_interrupts(struct c_can_priv *priv,
+						int enable)
+{
+	unsigned int cntrl_save = priv->read_reg(priv,
+						&priv->regs->control);
+
+	if (enable)
+		cntrl_save |= (CONTROL_SIE | CONTROL_EIE | CONTROL_IE);
+	else
+		cntrl_save &= ~(CONTROL_EIE | CONTROL_IE | CONTROL_SIE);
+
+	priv->write_reg(priv, &priv->regs->control, cntrl_save);
+}
+
+static inline int c_can_msg_obj_is_busy(struct c_can_priv *priv, int iface)
+{
+	int count = MIN_TIMEOUT_VALUE;
+
+	while (count && priv->read_reg(priv,
+				&priv->regs->ifregs[iface].com_req) &
+				IF_COMR_BUSY) {
+		count--;
+		udelay(1);
+	}
+
+	if (!count)
+		return 1;
+
+	return 0;
+}
+
+static inline void c_can_object_get(struct net_device *dev,
+					int iface, int objno, int mask)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	/*
+	 * As per specs, after writting the message object number in the
+	 * IF command request register the transfer b/w interface
+	 * register and message RAM must be complete in 6 CAN-CLK
+	 * period.
+	 */
+	priv->write_reg(priv, &priv->regs->ifregs[iface].com_mask,
+			IFX_WRITE_LOW_16BIT(mask));
+	priv->write_reg(priv, &priv->regs->ifregs[iface].com_req,
+			IFX_WRITE_LOW_16BIT(objno));
+
+	if (c_can_msg_obj_is_busy(priv, iface))
+		netdev_err(dev, "timed out in object get\n");
+}
+
+static inline void c_can_object_put(struct net_device *dev,
+					int iface, int objno, int mask)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	/*
+	 * As per specs, after writting the message object number in the
+	 * IF command request register the transfer b/w interface
+	 * register and message RAM must be complete in 6 CAN-CLK
+	 * period.
+	 */
+	priv->write_reg(priv, &priv->regs->ifregs[iface].com_mask,
+			(IF_COMM_WR | IFX_WRITE_LOW_16BIT(mask)));
+	priv->write_reg(priv, &priv->regs->ifregs[iface].com_req,
+			IFX_WRITE_LOW_16BIT(objno));
+
+	if (c_can_msg_obj_is_busy(priv, iface))
+		netdev_err(dev, "timed out in object put\n");
+}
+
+static void c_can_write_msg_object(struct net_device *dev,
+			int iface, struct can_frame *frame, int objno)
+{
+	int i;
+	u16 flags = 0;
+	unsigned int id;
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	if (!(frame->can_id & CAN_RTR_FLAG))
+		flags |= IF_ARB_TRANSMIT;
+
+	if (frame->can_id & CAN_EFF_FLAG) {
+		id = frame->can_id & CAN_EFF_MASK;
+		flags |= IF_ARB_MSGXTD;
+	} else
+		id = ((frame->can_id & CAN_SFF_MASK) << 18);
+
+	flags |= IF_ARB_MSGVAL;
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].arb1,
+				IFX_WRITE_LOW_16BIT(id));
+	priv->write_reg(priv, &priv->regs->ifregs[iface].arb2, flags |
+				IFX_WRITE_HIGH_16BIT(id));
+
+	for (i = 0; i < frame->can_dlc; i += 2) {
+		priv->write_reg(priv, &priv->regs->ifregs[iface].data[i / 2],
+				frame->data[i] | (frame->data[i + 1] << 8));
+	}
+
+	/* enable interrupt for this message object */
+	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+			IF_MCONT_TXIE | IF_MCONT_TXRQST | IF_MCONT_EOB |
+			frame->can_dlc);
+	c_can_object_put(dev, iface, objno, IF_COMM_ALL);
+}
+
+static inline void c_can_mark_rx_msg_obj(struct net_device *dev,
+						int iface, int ctrl_mask,
+						int obj)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+			ctrl_mask & ~(IF_MCONT_MSGLST | IF_MCONT_INTPND));
+	c_can_object_put(dev, iface, obj, IF_COMM_CONTROL);
+
+}
+
+static inline void c_can_activate_all_lower_rx_msg_obj(struct net_device *dev,
+						int iface,
+						int ctrl_mask)
+{
+	int i;
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	for (i = C_CAN_MSG_OBJ_RX_FIRST; i <= C_CAN_MSG_RX_LOW_LAST; i++) {
+		priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+				ctrl_mask & ~(IF_MCONT_MSGLST |
+					IF_MCONT_INTPND | IF_MCONT_NEWDAT));
+		c_can_object_put(dev, iface, i, IF_COMM_CONTROL);
+	}
+}
+
+static inline void c_can_activate_rx_msg_obj(struct net_device *dev,
+						int iface, int ctrl_mask,
+						int obj)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+			ctrl_mask & ~(IF_MCONT_MSGLST |
+				IF_MCONT_INTPND | IF_MCONT_NEWDAT));
+	c_can_object_put(dev, iface, obj, IF_COMM_CONTROL);
+}
+
+static void c_can_handle_lost_msg_obj(struct net_device *dev,
+					int iface, int objno)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+
+	netdev_err(dev, "msg lost in buffer %d\n", objno);
+
+	c_can_object_get(dev, iface, objno, IF_COMM_ALL & ~IF_COMM_TXRQST);
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+			IF_MCONT_CLR_MSGLST);
+
+	c_can_object_put(dev, 0, objno, IF_COMM_CONTROL);
+
+	/* create an error msg */
+	skb = alloc_can_err_skb(dev, &frame);
+	if (unlikely(!skb))
+		return;
+
+	frame->can_id |= CAN_ERR_CRTL;
+	frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+	stats->rx_errors++;
+	stats->rx_over_errors++;
+
+	netif_receive_skb(skb);
+}
+
+static int c_can_read_msg_object(struct net_device *dev, int iface, int ctrl)
+{
+	u16 flags, data;
+	int i;
+	unsigned int val;
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+
+	skb = alloc_can_skb(dev, &frame);
+	if (!skb) {
+		stats->rx_dropped++;
+		return -ENOMEM;
+	}
+
+	frame->can_dlc = get_can_dlc(ctrl & 0x0F);
+
+	flags =	priv->read_reg(priv, &priv->regs->ifregs[iface].arb2);
+	val = priv->read_reg(priv, &priv->regs->ifregs[iface].arb1) |
+		(flags << 16);
+
+	if (flags & IF_ARB_MSGXTD)
+		frame->can_id = (val & CAN_EFF_MASK) | CAN_EFF_FLAG;
+	else
+		frame->can_id = (val >> 18) & CAN_SFF_MASK;
+
+	if (flags & IF_ARB_TRANSMIT)
+		frame->can_id |= CAN_RTR_FLAG;
+	else {
+		for (i = 0; i < frame->can_dlc; i += 2) {
+			data = priv->read_reg(priv,
+				&priv->regs->ifregs[iface].data[i / 2]);
+			frame->data[i] = data;
+			frame->data[i + 1] = data >> 8;
+		}
+	}
+
+	netif_receive_skb(skb);
+
+	stats->rx_packets++;
+	stats->rx_bytes += frame->can_dlc;
+
+	return 0;
+}
+
+static void c_can_setup_receive_object(struct net_device *dev, int iface,
+					int objno, unsigned int mask,
+					unsigned int id, unsigned int mcont)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].mask1,
+			IFX_WRITE_LOW_16BIT(mask));
+	priv->write_reg(priv, &priv->regs->ifregs[iface].mask2,
+			IFX_WRITE_HIGH_16BIT(mask));
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].arb1,
+			IFX_WRITE_LOW_16BIT(id));
+	priv->write_reg(priv, &priv->regs->ifregs[iface].arb2,
+			(IF_ARB_MSGVAL | IFX_WRITE_HIGH_16BIT(id)));
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl, mcont);
+	c_can_object_put(dev, iface, objno, IF_COMM_ALL & ~IF_COMM_TXRQST);
+
+	netdev_dbg(dev, "obj no:%d, msgval:0x%08x\n", objno,
+			c_can_read_reg32(priv, &priv->regs->msgval1));
+}
+
+static void c_can_inval_msg_object(struct net_device *dev, int iface, int objno)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	priv->write_reg(priv, &priv->regs->ifregs[iface].arb1, 0);
+	priv->write_reg(priv, &priv->regs->ifregs[iface].arb2, 0);
+	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl, 0);
+
+	c_can_object_put(dev, iface, objno, IF_COMM_ARB | IF_COMM_CONTROL);
+
+	netdev_dbg(dev, "obj no:%d, msgval:0x%08x\n", objno,
+			c_can_read_reg32(priv, &priv->regs->msgval1));
+}
+
+static inline int c_can_is_next_tx_obj_busy(struct c_can_priv *priv, int objno)
+{
+	int val = c_can_read_reg32(priv, &priv->regs->txrqst1);
+
+	/*
+	 * as transmission request register's bit n-1 corresponds to
+	 * message object n, we need to handle the same properly.
+	 */
+	if (val & (1 << (objno - 1)))
+		return 1;
+
+	return 0;
+}
+
+static netdev_tx_t c_can_start_xmit(struct sk_buff *skb,
+					struct net_device *dev)
+{
+	u32 msg_obj_no;
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct can_frame *frame = (struct can_frame *)skb->data;
+
+	if (can_dropped_invalid_skb(dev, skb))
+		return NETDEV_TX_OK;
+
+	msg_obj_no = get_tx_next_msg_obj(priv);
+
+	/* prepare message object for transmission */
+	c_can_write_msg_object(dev, 0, frame, msg_obj_no);
+	can_put_echo_skb(skb, dev, msg_obj_no - C_CAN_MSG_OBJ_TX_FIRST);
+
+	/*
+	 * we have to stop the queue in case of a wrap around or
+	 * if the next TX message object is still in use
+	 */
+	priv->tx_next++;
+	if (c_can_is_next_tx_obj_busy(priv, get_tx_next_msg_obj(priv)) ||
+			(priv->tx_next & C_CAN_NEXT_MSG_OBJ_MASK) == 0)
+		netif_stop_queue(dev);
+
+	return NETDEV_TX_OK;
+}
+
+static int c_can_set_bittiming(struct net_device *dev)
+{
+	unsigned int reg_btr, reg_brpe, ctrl_save;
+	u8 brp, brpe, sjw, tseg1, tseg2;
+	u32 ten_bit_brp;
+	struct c_can_priv *priv = netdev_priv(dev);
+	const struct can_bittiming *bt = &priv->can.bittiming;
+
+	/* c_can provides a 6-bit brp and 4-bit brpe fields */
+	ten_bit_brp = bt->brp - 1;
+	brp = ten_bit_brp & BTR_BRP_MASK;
+	brpe = ten_bit_brp >> 6;
+
+	sjw = bt->sjw - 1;
+	tseg1 = bt->prop_seg + bt->phase_seg1 - 1;
+	tseg2 = bt->phase_seg2 - 1;
+	reg_btr = brp | (sjw << BTR_SJW_SHIFT) | (tseg1 << BTR_TSEG1_SHIFT) |
+			(tseg2 << BTR_TSEG2_SHIFT);
+	reg_brpe = brpe & BRP_EXT_BRPE_MASK;
+
+	netdev_info(dev,
+		"setting BTR=%04x BRPE=%04x\n", reg_btr, reg_brpe);
+
+	ctrl_save = priv->read_reg(priv, &priv->regs->control);
+	priv->write_reg(priv, &priv->regs->control,
+			ctrl_save | CONTROL_CCE | CONTROL_INIT);
+	priv->write_reg(priv, &priv->regs->btr, reg_btr);
+	priv->write_reg(priv, &priv->regs->brp_ext, reg_brpe);
+	priv->write_reg(priv, &priv->regs->control, ctrl_save);
+
+	return 0;
+}
+
+/*
+ * Configure C_CAN message objects for Tx and Rx purposes:
+ * C_CAN provides a total of 32 message objects that can be configured
+ * either for Tx or Rx purposes. Here the first 16 message objects are used as
+ * a reception FIFO. The end of reception FIFO is signified by the EoB bit
+ * being SET. The remaining 16 message objects are kept aside for Tx purposes.
+ * See user guide document for further details on configuring message
+ * objects.
+ */
+static void c_can_configure_msg_objects(struct net_device *dev)
+{
+	int i;
+
+	/* first invalidate all message objects */
+	for (i = C_CAN_MSG_OBJ_RX_FIRST; i <= C_CAN_NO_OF_OBJECTS; i++)
+		c_can_inval_msg_object(dev, 0, i);
+
+	/* setup receive message objects */
+	for (i = C_CAN_MSG_OBJ_RX_FIRST; i < C_CAN_MSG_OBJ_RX_LAST; i++)
+		c_can_setup_receive_object(dev, 0, i, 0, 0,
+			(IF_MCONT_RXIE | IF_MCONT_UMASK) & ~IF_MCONT_EOB);
+
+	c_can_setup_receive_object(dev, 0, C_CAN_MSG_OBJ_RX_LAST, 0, 0,
+			IF_MCONT_EOB | IF_MCONT_RXIE | IF_MCONT_UMASK);
+}
+
+/*
+ * Configure C_CAN chip:
+ * - enable/disable auto-retransmission
+ * - set operating mode
+ * - configure message objects
+ */
+static void c_can_chip_config(struct net_device *dev)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	if (priv->can.ctrlmode & CAN_CTRLMODE_ONE_SHOT)
+		/* disable automatic retransmission */
+		priv->write_reg(priv, &priv->regs->control,
+				CONTROL_DISABLE_AR);
+	else
+		/* enable automatic retransmission */
+		priv->write_reg(priv, &priv->regs->control,
+				CONTROL_ENABLE_AR);
+
+	if (priv->can.ctrlmode & (CAN_CTRLMODE_LISTENONLY &
+					CAN_CTRLMODE_LOOPBACK)) {
+		/* loopback + silent mode : useful for hot self-test */
+		priv->write_reg(priv, &priv->regs->control, CONTROL_EIE |
+				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
+		priv->write_reg(priv, &priv->regs->test,
+				TEST_LBACK | TEST_SILENT);
+	} else if (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK) {
+		/* loopback mode : useful for self-test function */
+		priv->write_reg(priv, &priv->regs->control, CONTROL_EIE |
+				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
+		priv->write_reg(priv, &priv->regs->test, TEST_LBACK);
+	} else if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
+		/* silent mode : bus-monitoring mode */
+		priv->write_reg(priv, &priv->regs->control, CONTROL_EIE |
+				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
+		priv->write_reg(priv, &priv->regs->test, TEST_SILENT);
+	} else
+		/* normal mode*/
+		priv->write_reg(priv, &priv->regs->control,
+				CONTROL_EIE | CONTROL_SIE | CONTROL_IE);
+
+	/* configure message objects */
+	c_can_configure_msg_objects(dev);
+
+	/* set a `lec` value so that we can check for updates later */
+	priv->write_reg(priv, &priv->regs->status, LEC_UNUSED);
+
+	/* set bittiming params */
+	c_can_set_bittiming(dev);
+}
+
+static void c_can_start(struct net_device *dev)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	/* enable status change, error and module interrupts */
+	c_can_enable_all_interrupts(priv, ENABLE_ALL_INTERRUPTS);
+
+	/* basic c_can configuration */
+	c_can_chip_config(dev);
+
+	priv->can.state = CAN_STATE_ERROR_ACTIVE;
+
+	/* reset tx helper pointers */
+	priv->tx_next = priv->tx_echo = 0;
+}
+
+static void c_can_stop(struct net_device *dev)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	/* disable all interrupts */
+	c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
+
+	/* set the state as STOPPED */
+	priv->can.state = CAN_STATE_STOPPED;
+}
+
+static int c_can_set_mode(struct net_device *dev, enum can_mode mode)
+{
+	switch (mode) {
+	case CAN_MODE_START:
+		c_can_start(dev);
+		netif_wake_queue(dev);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static int c_can_get_berr_counter(const struct net_device *dev,
+					struct can_berr_counter *bec)
+{
+	unsigned int reg_err_counter;
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	reg_err_counter = priv->read_reg(priv, &priv->regs->err_cnt);
+	bec->rxerr = (reg_err_counter & ERR_CNT_REC_MASK) >>
+				ERR_CNT_REC_SHIFT;
+	bec->txerr = reg_err_counter & ERR_CNT_TEC_MASK;
+
+	return 0;
+}
+
+/*
+ * theory of operation:
+ *
+ * priv->tx_echo holds the number of the oldest can_frame put for
+ * transmission into the hardware, but not yet ACKed by the CAN tx
+ * complete IRQ.
+ *
+ * We iterate from priv->tx_echo to priv->tx_next and check if the
+ * packet has been transmitted, echo it back to the CAN framework.
+ * If we discover a not yet transmitted package, stop looking for more.
+ */
+static void c_can_do_tx(struct net_device *dev)
+{
+	u32 val;
+	u32 msg_obj_no;
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+
+	for (/* nix */; (priv->tx_next - priv->tx_echo) > 0; priv->tx_echo++) {
+		msg_obj_no = get_tx_echo_msg_obj(priv);
+		c_can_inval_msg_object(dev, 0, msg_obj_no);
+		val = c_can_read_reg32(priv, &priv->regs->txrqst1);
+		if (!(val & (1 << msg_obj_no))) {
+			can_get_echo_skb(dev,
+					msg_obj_no - C_CAN_MSG_OBJ_TX_FIRST);
+			stats->tx_bytes += priv->read_reg(priv,
+					&priv->regs->ifregs[0].msg_cntrl)
+					& IF_MCONT_DLC_MASK;
+			stats->tx_packets++;
+		}
+	}
+
+	/* restart queue if wrap-up or if queue stalled on last pkt */
+	if (((priv->tx_next & C_CAN_NEXT_MSG_OBJ_MASK) != 0) ||
+			((priv->tx_echo & C_CAN_NEXT_MSG_OBJ_MASK) == 0))
+		netif_wake_queue(dev);
+}
+
+/*
+ * theory of operation:
+ *
+ * c_can core saves a received CAN message into the first free message
+ * object it finds free (starting with the lowest). Bits NEWDAT and
+ * INTPND are set for this message object indicating that a new message
+ * has arrived. To work-around this issue, we keep two groups of message
+ * objects whose partitioning is defined by C_CAN_MSG_OBJ_RX_SPLIT.
+ *
+ * To ensure in-order frame reception we use the following
+ * approach while re-activating a message object to receive further
+ * frames:
+ * - if the current message object number is lower than
+ *   C_CAN_MSG_RX_LOW_LAST, do not clear the NEWDAT bit while clearing
+ *   the INTPND bit.
+ * - if the current message object number is equal to
+ *   C_CAN_MSG_RX_LOW_LAST then clear the NEWDAT bit of all lower
+ *   receive message objects.
+ * - if the current message object number is greater than
+ *   C_CAN_MSG_RX_LOW_LAST then clear the NEWDAT bit of
+ *   only this message object.
+ */
+static int c_can_do_rx_poll(struct net_device *dev, int quota)
+{
+	u32 num_rx_pkts = 0;
+	unsigned int msg_obj, msg_ctrl_save;
+	struct c_can_priv *priv = netdev_priv(dev);
+	u32 val = c_can_read_reg32(priv, &priv->regs->intpnd1);
+
+	for (msg_obj = C_CAN_MSG_OBJ_RX_FIRST;
+			msg_obj <= C_CAN_MSG_OBJ_RX_LAST && quota > 0;
+			val = c_can_read_reg32(priv, &priv->regs->intpnd1),
+			msg_obj++) {
+		/*
+		 * as interrupt pending register's bit n-1 corresponds to
+		 * message object n, we need to handle the same properly.
+		 */
+		if (val & (1 << (msg_obj - 1))) {
+			c_can_object_get(dev, 0, msg_obj, IF_COMM_ALL &
+					~IF_COMM_TXRQST);
+			msg_ctrl_save = priv->read_reg(priv,
+					&priv->regs->ifregs[0].msg_cntrl);
+
+			if (msg_ctrl_save & IF_MCONT_EOB)
+				return num_rx_pkts;
+
+			if (msg_ctrl_save & IF_MCONT_MSGLST) {
+				c_can_handle_lost_msg_obj(dev, 0, msg_obj);
+				num_rx_pkts++;
+				quota--;
+				continue;
+			}
+
+			if (!(msg_ctrl_save & IF_MCONT_NEWDAT))
+				continue;
+
+			/* read the data from the message object */
+			c_can_read_msg_object(dev, 0, msg_ctrl_save);
+
+			if (msg_obj < C_CAN_MSG_RX_LOW_LAST)
+				c_can_mark_rx_msg_obj(dev, 0,
+						msg_ctrl_save, msg_obj);
+			else if (msg_obj > C_CAN_MSG_RX_LOW_LAST)
+				/* activate this msg obj */
+				c_can_activate_rx_msg_obj(dev, 0,
+						msg_ctrl_save, msg_obj);
+			else if (msg_obj == C_CAN_MSG_RX_LOW_LAST)
+				/* activate all lower message objects */
+				c_can_activate_all_lower_rx_msg_obj(dev,
+						0, msg_ctrl_save);
+
+			num_rx_pkts++;
+			quota--;
+		}
+	}
+
+	return num_rx_pkts;
+}
+
+static inline int c_can_has_and_handle_berr(struct c_can_priv *priv)
+{
+	return (priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING) &&
+		(priv->current_status & LEC_UNUSED);
+}
+
+static int c_can_handle_state_change(struct net_device *dev,
+				enum c_can_bus_error_types error_type)
+{
+	unsigned int reg_err_counter;
+	unsigned int rx_err_passive;
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf;
+	struct sk_buff *skb;
+	struct can_berr_counter bec;
+
+	/* propogate the error condition to the CAN stack */
+	skb = alloc_can_err_skb(dev, &cf);
+	if (unlikely(!skb))
+		return 0;
+
+	c_can_get_berr_counter(dev, &bec);
+	reg_err_counter = priv->read_reg(priv, &priv->regs->err_cnt);
+	rx_err_passive = (reg_err_counter & ERR_CNT_RP_MASK) >>
+				ERR_CNT_RP_SHIFT;
+
+	switch (error_type) {
+	case C_CAN_ERROR_WARNING:
+		/* error warning state */
+		priv->can.can_stats.error_warning++;
+		priv->can.state = CAN_STATE_ERROR_WARNING;
+		cf->can_id |= CAN_ERR_CRTL;
+		cf->data[1] = (bec.txerr > bec.rxerr) ?
+			CAN_ERR_CRTL_TX_WARNING :
+			CAN_ERR_CRTL_RX_WARNING;
+		cf->data[6] = bec.txerr;
+		cf->data[7] = bec.rxerr;
+
+		break;
+	case C_CAN_ERROR_PASSIVE:
+		/* error passive state */
+		priv->can.can_stats.error_passive++;
+		priv->can.state = CAN_STATE_ERROR_PASSIVE;
+		cf->can_id |= CAN_ERR_CRTL;
+		if (rx_err_passive)
+			cf->data[1] |= CAN_ERR_CRTL_RX_PASSIVE;
+		if (bec.txerr > 127)
+			cf->data[1] |= CAN_ERR_CRTL_TX_PASSIVE;
+
+		cf->data[6] = bec.txerr;
+		cf->data[7] = bec.rxerr;
+		break;
+	case C_CAN_BUS_OFF:
+		/* bus-off state */
+		priv->can.state = CAN_STATE_BUS_OFF;
+		cf->can_id |= CAN_ERR_BUSOFF;
+		/*
+		 * disable all interrupts in bus-off mode to ensure that
+		 * the CPU is not hogged down
+		 */
+		c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
+		can_bus_off(dev);
+		break;
+	default:
+		break;
+	}
+
+	netif_receive_skb(skb);
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+
+	return 1;
+}
+
+static int c_can_handle_bus_err(struct net_device *dev,
+				enum c_can_lec_type lec_type)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf;
+	struct sk_buff *skb;
+
+	/*
+	 * early exit if no lec update or no error.
+	 * no lec update means that no CAN bus event has been detected
+	 * since CPU wrote 0x7 value to status reg.
+	 */
+	if (lec_type == LEC_UNUSED || lec_type == LEC_NO_ERROR)
+		return 0;
+
+	/* propogate the error condition to the CAN stack */
+	skb = alloc_can_err_skb(dev, &cf);
+	if (unlikely(!skb))
+		return 0;
+
+	/*
+	 * check for 'last error code' which tells us the
+	 * type of the last error to occur on the CAN bus
+	 */
+
+	/* common for all type of bus errors */
+	priv->can.can_stats.bus_error++;
+	stats->rx_errors++;
+	cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
+	cf->data[2] |= CAN_ERR_PROT_UNSPEC;
+
+	switch (lec_type) {
+	case LEC_STUFF_ERROR:
+		netdev_dbg(dev, "stuff error\n");
+		cf->data[2] |= CAN_ERR_PROT_STUFF;
+		break;
+	case LEC_FORM_ERROR:
+		netdev_dbg(dev, "form error\n");
+		cf->data[2] |= CAN_ERR_PROT_FORM;
+		break;
+	case LEC_ACK_ERROR:
+		netdev_dbg(dev, "ack error\n");
+		cf->data[2] |= (CAN_ERR_PROT_LOC_ACK |
+				CAN_ERR_PROT_LOC_ACK_DEL);
+		break;
+	case LEC_BIT1_ERROR:
+		netdev_dbg(dev, "bit1 error\n");
+		cf->data[2] |= CAN_ERR_PROT_BIT1;
+		break;
+	case LEC_BIT0_ERROR:
+		netdev_dbg(dev, "bit0 error\n");
+		cf->data[2] |= CAN_ERR_PROT_BIT0;
+		break;
+	case LEC_CRC_ERROR:
+		netdev_dbg(dev, "CRC error\n");
+		cf->data[2] |= (CAN_ERR_PROT_LOC_CRC_SEQ |
+				CAN_ERR_PROT_LOC_CRC_DEL);
+		break;
+	default:
+		break;
+	}
+
+	/* set a `lec` value so that we can check for updates later */
+	priv->write_reg(priv, &priv->regs->status, LEC_UNUSED);
+
+	netif_receive_skb(skb);
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+
+	return 1;
+}
+
+static int c_can_poll(struct napi_struct *napi, int quota)
+{
+	u16 irqstatus;
+	int lec_type = 0;
+	int work_done = 0;
+	struct net_device *dev = napi->dev;
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	irqstatus = priv->read_reg(priv, &priv->regs->interrupt);
+	if (!irqstatus)
+		goto end;
+
+	/* status events have the highest priority */
+	if (irqstatus == STATUS_INTERRUPT) {
+		priv->current_status = priv->read_reg(priv,
+					&priv->regs->status);
+
+		/* handle Tx/Rx events */
+		if (priv->current_status & STATUS_TXOK)
+			priv->write_reg(priv, &priv->regs->status,
+					priv->current_status & ~STATUS_TXOK);
+
+		if (priv->current_status & STATUS_RXOK)
+			priv->write_reg(priv, &priv->regs->status,
+					priv->current_status & ~STATUS_RXOK);
+
+		/* handle state changes */
+		if ((priv->current_status & STATUS_EWARN) &&
+				(!(priv->last_status & STATUS_EWARN))) {
+			netdev_dbg(dev, "entered error warning state\n");
+			work_done += c_can_handle_state_change(dev,
+						C_CAN_ERROR_WARNING);
+		}
+		if ((priv->current_status & STATUS_EPASS) &&
+				(!(priv->last_status & STATUS_EPASS))) {
+			netdev_dbg(dev, "entered error passive state\n");
+			work_done += c_can_handle_state_change(dev,
+						C_CAN_ERROR_PASSIVE);
+		}
+		if ((priv->current_status & STATUS_BOFF) &&
+				(!(priv->last_status & STATUS_BOFF))) {
+			netdev_dbg(dev, "entered bus off state\n");
+			work_done += c_can_handle_state_change(dev,
+						C_CAN_BUS_OFF);
+		}
+
+		/* handle bus recovery events */
+		if ((!(priv->current_status & STATUS_BOFF)) &&
+				(priv->last_status & STATUS_BOFF)) {
+			netdev_dbg(dev, "left bus off state\n");
+			priv->can.state = CAN_STATE_ERROR_ACTIVE;
+		}
+		if ((!(priv->current_status & STATUS_EPASS)) &&
+				(priv->last_status & STATUS_EPASS)) {
+			netdev_dbg(dev, "left error passive state\n");
+			priv->can.state = CAN_STATE_ERROR_ACTIVE;
+		}
+
+		priv->last_status = priv->current_status;
+
+		/* handle lec errors on the bus */
+		lec_type = c_can_has_and_handle_berr(priv);
+		if (lec_type)
+			work_done += c_can_handle_bus_err(dev, lec_type);
+	} else if ((irqstatus >= C_CAN_MSG_OBJ_RX_FIRST) &&
+			(irqstatus <= C_CAN_MSG_OBJ_RX_LAST)) {
+		/* handle events corresponding to receive message objects */
+		work_done += c_can_do_rx_poll(dev, (quota - work_done));
+	} else if ((irqstatus >= C_CAN_MSG_OBJ_TX_FIRST) &&
+			(irqstatus <= C_CAN_MSG_OBJ_TX_LAST)) {
+		/* handle events corresponding to transmit message objects */
+		c_can_do_tx(dev);
+	}
+
+end:
+	if (work_done < quota) {
+		napi_complete(napi);
+		/* enable all IRQs */
+		c_can_enable_all_interrupts(priv, ENABLE_ALL_INTERRUPTS);
+	}
+
+	return work_done;
+}
+
+static irqreturn_t c_can_isr(int irq, void *dev_id)
+{
+	u16 irqstatus;
+	struct net_device *dev = (struct net_device *)dev_id;
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	irqstatus = priv->read_reg(priv, &priv->regs->interrupt);
+	if (!irqstatus)
+		return IRQ_NONE;
+
+	/* disable all interrupts and schedule the NAPI */
+	c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
+	napi_schedule(&priv->napi);
+
+	return IRQ_HANDLED;
+}
+
+static int c_can_open(struct net_device *dev)
+{
+	int err;
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	/* open the can device */
+	err = open_candev(dev);
+	if (err) {
+		netdev_err(dev, "failed to open can device\n");
+		return err;
+	}
+
+	/* register interrupt handler */
+	err = request_irq(dev->irq, &c_can_isr, IRQF_SHARED, dev->name,
+				dev);
+	if (err < 0) {
+		netdev_err(dev, "failed to request interrupt\n");
+		goto exit_irq_fail;
+	}
+
+	/* start the c_can controller */
+	c_can_start(dev);
+
+	napi_enable(&priv->napi);
+	netif_start_queue(dev);
+
+	return 0;
+
+exit_irq_fail:
+	close_candev(dev);
+	return err;
+}
+
+static int c_can_close(struct net_device *dev)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+	napi_disable(&priv->napi);
+	c_can_stop(dev);
+	free_irq(dev->irq, dev);
+	close_candev(dev);
+
+	return 0;
+}
+
+struct net_device *alloc_c_can_dev(void)
+{
+	struct net_device *dev;
+	struct c_can_priv *priv;
+
+	dev = alloc_candev(sizeof(struct c_can_priv), C_CAN_MSG_OBJ_TX_NUM);
+	if (!dev)
+		return NULL;
+
+	priv = netdev_priv(dev);
+	netif_napi_add(dev, &priv->napi, c_can_poll, C_CAN_NAPI_WEIGHT);
+
+	priv->dev = dev;
+	priv->can.bittiming_const = &c_can_bittiming_const;
+	priv->can.do_set_mode = c_can_set_mode;
+	priv->can.do_get_berr_counter = c_can_get_berr_counter;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_ONE_SHOT |
+					CAN_CTRLMODE_LOOPBACK |
+					CAN_CTRLMODE_LISTENONLY |
+					CAN_CTRLMODE_BERR_REPORTING;
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(alloc_c_can_dev);
+
+void free_c_can_dev(struct net_device *dev)
+{
+	free_candev(dev);
+}
+EXPORT_SYMBOL_GPL(free_c_can_dev);
+
+static const struct net_device_ops c_can_netdev_ops = {
+	.ndo_open = c_can_open,
+	.ndo_stop = c_can_close,
+	.ndo_start_xmit = c_can_start_xmit,
+};
+
+int register_c_can_dev(struct net_device *dev)
+{
+	dev->flags |= IFF_ECHO;	/* we support local echo */
+	dev->netdev_ops = &c_can_netdev_ops;
+
+	return register_candev(dev);
+}
+EXPORT_SYMBOL_GPL(register_c_can_dev);
+
+void unregister_c_can_dev(struct net_device *dev)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	/* disable all interrupts */
+	c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
+
+	unregister_candev(dev);
+}
+EXPORT_SYMBOL_GPL(unregister_c_can_dev);
+
+MODULE_AUTHOR("Bhupesh Sharma <bhupesh.sharma@st.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CAN bus driver for Bosch C_CAN controller");
diff --git a/drivers/net/can/c_can/c_can.h b/drivers/net/can/c_can/c_can.h
new file mode 100644
index 000000000000..9b7fbef3d09a
--- /dev/null
+++ b/drivers/net/can/c_can/c_can.h
@@ -0,0 +1,86 @@
+/*
+ * CAN bus driver for Bosch C_CAN controller
+ *
+ * Copyright (C) 2010 ST Microelectronics
+ * Bhupesh Sharma <bhupesh.sharma@st.com>
+ *
+ * Borrowed heavily from the C_CAN driver originally written by:
+ * Copyright (C) 2007
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix <s.hauer@pengutronix.de>
+ * - Simon Kallweit, intefo AG <simon.kallweit@intefo.ch>
+ *
+ * Bosch C_CAN controller is compliant to CAN protocol version 2.0 part A and B.
+ * Bosch C_CAN user manual can be obtained from:
+ * http://www.semiconductors.bosch.de/media/en/pdf/ipmodules_1/c_can/
+ * users_manual_c_can.pdf
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef C_CAN_H
+#define C_CAN_H
+
+/* c_can IF registers */
+struct c_can_if_regs {
+	u16 com_req;
+	u16 com_mask;
+	u16 mask1;
+	u16 mask2;
+	u16 arb1;
+	u16 arb2;
+	u16 msg_cntrl;
+	u16 data[4];
+	u16 _reserved[13];
+};
+
+/* c_can hardware registers */
+struct c_can_regs {
+	u16 control;
+	u16 status;
+	u16 err_cnt;
+	u16 btr;
+	u16 interrupt;
+	u16 test;
+	u16 brp_ext;
+	u16 _reserved1;
+	struct c_can_if_regs ifregs[2]; /* [0] = IF1 and [1] = IF2 */
+	u16 _reserved2[8];
+	u16 txrqst1;
+	u16 txrqst2;
+	u16 _reserved3[6];
+	u16 newdat1;
+	u16 newdat2;
+	u16 _reserved4[6];
+	u16 intpnd1;
+	u16 intpnd2;
+	u16 _reserved5[6];
+	u16 msgval1;
+	u16 msgval2;
+	u16 _reserved6[6];
+};
+
+/* c_can private data structure */
+struct c_can_priv {
+	struct can_priv can;	/* must be the first member */
+	struct napi_struct napi;
+	struct net_device *dev;
+	int tx_object;
+	int current_status;
+	int last_status;
+	u16 (*read_reg) (struct c_can_priv *priv, void *reg);
+	void (*write_reg) (struct c_can_priv *priv, void *reg, u16 val);
+	struct c_can_regs __iomem *regs;
+	unsigned long irq_flags; /* for request_irq() */
+	unsigned int tx_next;
+	unsigned int tx_echo;
+	void *priv;		/* for board-specific data */
+};
+
+struct net_device *alloc_c_can_dev(void);
+void free_c_can_dev(struct net_device *dev);
+int register_c_can_dev(struct net_device *dev);
+void unregister_c_can_dev(struct net_device *dev);
+
+#endif /* C_CAN_H */
diff --git a/drivers/net/can/c_can/c_can_platform.c b/drivers/net/can/c_can/c_can_platform.c
new file mode 100644
index 000000000000..e629b961ae2d
--- /dev/null
+++ b/drivers/net/can/c_can/c_can_platform.c
@@ -0,0 +1,215 @@
+/*
+ * Platform CAN bus driver for Bosch C_CAN controller
+ *
+ * Copyright (C) 2010 ST Microelectronics
+ * Bhupesh Sharma <bhupesh.sharma@st.com>
+ *
+ * Borrowed heavily from the C_CAN driver originally written by:
+ * Copyright (C) 2007
+ * - Sascha Hauer, Marc Kleine-Budde, Pengutronix <s.hauer@pengutronix.de>
+ * - Simon Kallweit, intefo AG <simon.kallweit@intefo.ch>
+ *
+ * Bosch C_CAN controller is compliant to CAN protocol version 2.0 part A and B.
+ * Bosch C_CAN user manual can be obtained from:
+ * http://www.semiconductors.bosch.de/media/en/pdf/ipmodules_1/c_can/
+ * users_manual_c_can.pdf
+ *
+ * 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/version.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <linux/can/dev.h>
+
+#include "c_can.h"
+
+/*
+ * 16-bit c_can registers can be arranged differently in the memory
+ * architecture of different implementations. For example: 16-bit
+ * registers can be aligned to a 16-bit boundary or 32-bit boundary etc.
+ * Handle the same by providing a common read/write interface.
+ */
+static u16 c_can_plat_read_reg_aligned_to_16bit(struct c_can_priv *priv,
+						void *reg)
+{
+	return readw(reg);
+}
+
+static void c_can_plat_write_reg_aligned_to_16bit(struct c_can_priv *priv,
+						void *reg, u16 val)
+{
+	writew(val, reg);
+}
+
+static u16 c_can_plat_read_reg_aligned_to_32bit(struct c_can_priv *priv,
+						void *reg)
+{
+	return readw(reg + (long)reg - (long)priv->regs);
+}
+
+static void c_can_plat_write_reg_aligned_to_32bit(struct c_can_priv *priv,
+						void *reg, u16 val)
+{
+	writew(val, reg + (long)reg - (long)priv->regs);
+}
+
+static int __devinit c_can_plat_probe(struct platform_device *pdev)
+{
+	int ret;
+	void __iomem *addr;
+	struct net_device *dev;
+	struct c_can_priv *priv;
+	struct resource *mem, *irq;
+#ifdef CONFIG_HAVE_CLK
+	struct clk *clk;
+
+	/* get the appropriate clk */
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "no clock defined\n");
+		ret = -ENODEV;
+		goto exit;
+	}
+#endif
+
+	/* get the platform data */
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!mem || (irq <= 0)) {
+		ret = -ENODEV;
+		goto exit_free_clk;
+	}
+
+	if (!request_mem_region(mem->start, resource_size(mem),
+				KBUILD_MODNAME)) {
+		dev_err(&pdev->dev, "resource unavailable\n");
+		ret = -ENODEV;
+		goto exit_free_clk;
+	}
+
+	addr = ioremap(mem->start, resource_size(mem));
+	if (!addr) {
+		dev_err(&pdev->dev, "failed to map can port\n");
+		ret = -ENOMEM;
+		goto exit_release_mem;
+	}
+
+	/* allocate the c_can device */
+	dev = alloc_c_can_dev();
+	if (!dev) {
+		ret = -ENOMEM;
+		goto exit_iounmap;
+	}
+
+	priv = netdev_priv(dev);
+
+	dev->irq = irq->start;
+	priv->regs = addr;
+#ifdef CONFIG_HAVE_CLK
+	priv->can.clock.freq = clk_get_rate(clk);
+	priv->priv = clk;
+#endif
+
+	switch (mem->flags & IORESOURCE_MEM_TYPE_MASK) {
+	case IORESOURCE_MEM_32BIT:
+		priv->read_reg = c_can_plat_read_reg_aligned_to_32bit;
+		priv->write_reg = c_can_plat_write_reg_aligned_to_32bit;
+		break;
+	case IORESOURCE_MEM_16BIT:
+	default:
+		priv->read_reg = c_can_plat_read_reg_aligned_to_16bit;
+		priv->write_reg = c_can_plat_write_reg_aligned_to_16bit;
+		break;
+	}
+
+	platform_set_drvdata(pdev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	ret = register_c_can_dev(dev);
+	if (ret) {
+		dev_err(&pdev->dev, "registering %s failed (err=%d)\n",
+			KBUILD_MODNAME, ret);
+		goto exit_free_device;
+	}
+
+	dev_info(&pdev->dev, "%s device registered (regs=%p, irq=%d)\n",
+		 KBUILD_MODNAME, priv->regs, dev->irq);
+	return 0;
+
+exit_free_device:
+	platform_set_drvdata(pdev, NULL);
+	free_c_can_dev(dev);
+exit_iounmap:
+	iounmap(addr);
+exit_release_mem:
+	release_mem_region(mem->start, resource_size(mem));
+exit_free_clk:
+#ifdef CONFIG_HAVE_CLK
+	clk_put(clk);
+exit:
+#endif
+	dev_err(&pdev->dev, "probe failed\n");
+
+	return ret;
+}
+
+static int __devexit c_can_plat_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct c_can_priv *priv = netdev_priv(dev);
+	struct resource *mem;
+
+	unregister_c_can_dev(dev);
+	platform_set_drvdata(pdev, NULL);
+
+	free_c_can_dev(dev);
+	iounmap(priv->regs);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, resource_size(mem));
+
+#ifdef CONFIG_HAVE_CLK
+	clk_put(priv->priv);
+#endif
+
+	return 0;
+}
+
+static struct platform_driver c_can_plat_driver = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = c_can_plat_probe,
+	.remove = __devexit_p(c_can_plat_remove),
+};
+
+static int __init c_can_plat_init(void)
+{
+	return platform_driver_register(&c_can_plat_driver);
+}
+module_init(c_can_plat_init);
+
+static void __exit c_can_plat_exit(void)
+{
+	platform_driver_unregister(&c_can_plat_driver);
+}
+module_exit(c_can_plat_exit);
+
+MODULE_AUTHOR("Bhupesh Sharma <bhupesh.sharma@st.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Platform CAN bus driver for Bosch C_CAN controller");
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
index 366f5cc050ae..102b16c6cc97 100644
--- a/drivers/net/can/janz-ican3.c
+++ b/drivers/net/can/janz-ican3.c
@@ -15,6 +15,7 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 
 #include <linux/netdevice.h>
 #include <linux/can.h>
@@ -1643,7 +1644,7 @@ static int __devinit ican3_probe(struct platform_device *pdev)
 	struct device *dev;
 	int ret;
 
-	pdata = pdev->dev.platform_data;
+	pdata = mfd_get_data(pdev);
 	if (!pdata)
 		return -ENXIO;
 
diff --git a/drivers/net/can/mscan/mpc5xxx_can.c b/drivers/net/can/mscan/mpc5xxx_can.c
index 312b9c8f4f3b..c0a1bc5b1435 100644
--- a/drivers/net/can/mscan/mpc5xxx_can.c
+++ b/drivers/net/can/mscan/mpc5xxx_can.c
@@ -247,10 +247,9 @@ static u32 __devinit mpc512x_can_get_clock(struct platform_device *ofdev,
 }
 #endif /* CONFIG_PPC_MPC512x */
 
-static int __devinit mpc5xxx_can_probe(struct platform_device *ofdev,
-				       const struct of_device_id *id)
+static int __devinit mpc5xxx_can_probe(struct platform_device *ofdev)
 {
-	struct mpc5xxx_can_data *data = (struct mpc5xxx_can_data *)id->data;
+	struct mpc5xxx_can_data *data;
 	struct device_node *np = ofdev->dev.of_node;
 	struct net_device *dev;
 	struct mscan_priv *priv;
@@ -259,6 +258,10 @@ static int __devinit mpc5xxx_can_probe(struct platform_device *ofdev,
 	int irq, mscan_clksrc = 0;
 	int err = -ENOMEM;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+	data = (struct mpc5xxx_can_data *)of_dev->dev.of_match->data;
+
 	base = of_iomap(np, 0);
 	if (!base) {
 		dev_err(&ofdev->dev, "couldn't ioremap\n");
@@ -391,7 +394,7 @@ static struct of_device_id __devinitdata mpc5xxx_can_table[] = {
 	{},
 };
 
-static struct of_platform_driver mpc5xxx_can_driver = {
+static struct platform_driver mpc5xxx_can_driver = {
 	.driver = {
 		.name = "mpc5xxx_can",
 		.owner = THIS_MODULE,
@@ -407,13 +410,13 @@ static struct of_platform_driver mpc5xxx_can_driver = {
 
 static int __init mpc5xxx_can_init(void)
 {
-	return of_register_platform_driver(&mpc5xxx_can_driver);
+	return platform_driver_register(&mpc5xxx_can_driver);
 }
 module_init(mpc5xxx_can_init);
 
 static void __exit mpc5xxx_can_exit(void)
 {
-	return of_unregister_platform_driver(&mpc5xxx_can_driver);
+	platform_driver_unregister(&mpc5xxx_can_driver);
 };
 module_exit(mpc5xxx_can_exit);
 
diff --git a/drivers/net/can/sja1000/sja1000_of_platform.c b/drivers/net/can/sja1000/sja1000_of_platform.c
index 09c3e9db9316..9793df6e3455 100644
--- a/drivers/net/can/sja1000/sja1000_of_platform.c
+++ b/drivers/net/can/sja1000/sja1000_of_platform.c
@@ -87,8 +87,7 @@ static int __devexit sja1000_ofp_remove(struct platform_device *ofdev)
 	return 0;
 }
 
-static int __devinit sja1000_ofp_probe(struct platform_device *ofdev,
-				       const struct of_device_id *id)
+static int __devinit sja1000_ofp_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct net_device *dev;
@@ -210,7 +209,7 @@ static struct of_device_id __devinitdata sja1000_ofp_table[] = {
 };
 MODULE_DEVICE_TABLE(of, sja1000_ofp_table);
 
-static struct of_platform_driver sja1000_ofp_driver = {
+static struct platform_driver sja1000_ofp_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = DRV_NAME,
@@ -222,12 +221,12 @@ static struct of_platform_driver sja1000_ofp_driver = {
 
 static int __init sja1000_ofp_init(void)
 {
-	return of_register_platform_driver(&sja1000_ofp_driver);
+	return platform_driver_register(&sja1000_ofp_driver);
 }
 module_init(sja1000_ofp_init);
 
 static void __exit sja1000_ofp_exit(void)
 {
-	return of_unregister_platform_driver(&sja1000_ofp_driver);
+	return platform_driver_unregister(&sja1000_ofp_driver);
 };
 module_exit(sja1000_ofp_exit);
diff --git a/drivers/net/can/softing/softing_main.c b/drivers/net/can/softing/softing_main.c
index 5157e15e96eb..aeea9f9ff6e8 100644
--- a/drivers/net/can/softing/softing_main.c
+++ b/drivers/net/can/softing/softing_main.c
@@ -633,6 +633,7 @@ static const struct net_device_ops softing_netdev_ops = {
 };
 
 static const struct can_bittiming_const softing_btr_const = {
+	.name = "softing",
 	.tseg1_min = 1,
 	.tseg1_max = 16,
 	.tseg2_min = 1,
diff --git a/drivers/net/cnic.c b/drivers/net/cnic.c
index 7ff170cbc7dc..271a1f00c224 100644
--- a/drivers/net/cnic.c
+++ b/drivers/net/cnic.c
@@ -65,7 +65,14 @@ static LIST_HEAD(cnic_udev_list);
 static DEFINE_RWLOCK(cnic_dev_lock);
 static DEFINE_MUTEX(cnic_lock);
 
-static struct cnic_ulp_ops *cnic_ulp_tbl[MAX_CNIC_ULP_TYPE];
+static struct cnic_ulp_ops __rcu *cnic_ulp_tbl[MAX_CNIC_ULP_TYPE];
+
+/* helper function, assuming cnic_lock is held */
+static inline struct cnic_ulp_ops *cnic_ulp_tbl_prot(int type)
+{
+	return rcu_dereference_protected(cnic_ulp_tbl[type],
+					 lockdep_is_held(&cnic_lock));
+}
 
 static int cnic_service_bnx2(void *, void *);
 static int cnic_service_bnx2x(void *, void *);
@@ -435,7 +442,7 @@ int cnic_register_driver(int ulp_type, struct cnic_ulp_ops *ulp_ops)
 		return -EINVAL;
 	}
 	mutex_lock(&cnic_lock);
-	if (cnic_ulp_tbl[ulp_type]) {
+	if (cnic_ulp_tbl_prot(ulp_type)) {
 		pr_err("%s: Type %d has already been registered\n",
 		       __func__, ulp_type);
 		mutex_unlock(&cnic_lock);
@@ -478,7 +485,7 @@ int cnic_unregister_driver(int ulp_type)
 		return -EINVAL;
 	}
 	mutex_lock(&cnic_lock);
-	ulp_ops = cnic_ulp_tbl[ulp_type];
+	ulp_ops = cnic_ulp_tbl_prot(ulp_type);
 	if (!ulp_ops) {
 		pr_err("%s: Type %d has not been registered\n",
 		       __func__, ulp_type);
@@ -529,7 +536,7 @@ static int cnic_register_device(struct cnic_dev *dev, int ulp_type,
 		return -EINVAL;
 	}
 	mutex_lock(&cnic_lock);
-	if (cnic_ulp_tbl[ulp_type] == NULL) {
+	if (cnic_ulp_tbl_prot(ulp_type) == NULL) {
 		pr_err("%s: Driver with type %d has not been registered\n",
 		       __func__, ulp_type);
 		mutex_unlock(&cnic_lock);
@@ -544,7 +551,7 @@ static int cnic_register_device(struct cnic_dev *dev, int ulp_type,
 
 	clear_bit(ULP_F_START, &cp->ulp_flags[ulp_type]);
 	cp->ulp_handle[ulp_type] = ulp_ctx;
-	ulp_ops = cnic_ulp_tbl[ulp_type];
+	ulp_ops = cnic_ulp_tbl_prot(ulp_type);
 	rcu_assign_pointer(cp->ulp_ops[ulp_type], ulp_ops);
 	cnic_hold(dev);
 
@@ -2760,6 +2767,8 @@ static u32 cnic_service_bnx2_queues(struct cnic_dev *dev)
 	u32 status_idx = (u16) *cp->kcq1.status_idx_ptr;
 	int kcqe_cnt;
 
+	/* status block index must be read before reading other fields */
+	rmb();
 	cp->kwq_con_idx = *cp->kwq_con_idx_ptr;
 
 	while ((kcqe_cnt = cnic_get_kcqes(dev, &cp->kcq1))) {
@@ -2770,6 +2779,8 @@ static u32 cnic_service_bnx2_queues(struct cnic_dev *dev)
 		barrier();
 		if (status_idx != *cp->kcq1.status_idx_ptr) {
 			status_idx = (u16) *cp->kcq1.status_idx_ptr;
+			/* status block index must be read first */
+			rmb();
 			cp->kwq_con_idx = *cp->kwq_con_idx_ptr;
 		} else
 			break;
@@ -2888,6 +2899,8 @@ static u32 cnic_service_bnx2x_kcq(struct cnic_dev *dev, struct kcq_info *info)
 	u32 last_status = *info->status_idx_ptr;
 	int kcqe_cnt;
 
+	/* status block index must be read before reading the KCQ */
+	rmb();
 	while ((kcqe_cnt = cnic_get_kcqes(dev, info))) {
 
 		service_kcqes(dev, kcqe_cnt);
@@ -2898,6 +2911,8 @@ static u32 cnic_service_bnx2x_kcq(struct cnic_dev *dev, struct kcq_info *info)
 			break;
 
 		last_status = *info->status_idx_ptr;
+		/* status block index must be read before reading the KCQ */
+		rmb();
 	}
 	return last_status;
 }
@@ -2906,26 +2921,35 @@ static void cnic_service_bnx2x_bh(unsigned long data)
 {
 	struct cnic_dev *dev = (struct cnic_dev *) data;
 	struct cnic_local *cp = dev->cnic_priv;
-	u32 status_idx;
+	u32 status_idx, new_status_idx;
 
 	if (unlikely(!test_bit(CNIC_F_CNIC_UP, &dev->flags)))
 		return;
 
-	status_idx = cnic_service_bnx2x_kcq(dev, &cp->kcq1);
+	while (1) {
+		status_idx = cnic_service_bnx2x_kcq(dev, &cp->kcq1);
 
-	CNIC_WR16(dev, cp->kcq1.io_addr, cp->kcq1.sw_prod_idx + MAX_KCQ_IDX);
+		CNIC_WR16(dev, cp->kcq1.io_addr,
+			  cp->kcq1.sw_prod_idx + MAX_KCQ_IDX);
 
-	if (BNX2X_CHIP_IS_E2(cp->chip_id)) {
-		status_idx = cnic_service_bnx2x_kcq(dev, &cp->kcq2);
+		if (!BNX2X_CHIP_IS_E2(cp->chip_id)) {
+			cnic_ack_bnx2x_int(dev, cp->bnx2x_igu_sb_id, USTORM_ID,
+					   status_idx, IGU_INT_ENABLE, 1);
+			break;
+		}
+
+		new_status_idx = cnic_service_bnx2x_kcq(dev, &cp->kcq2);
+
+		if (new_status_idx != status_idx)
+			continue;
 
 		CNIC_WR16(dev, cp->kcq2.io_addr, cp->kcq2.sw_prod_idx +
 			  MAX_KCQ_IDX);
 
 		cnic_ack_igu_sb(dev, cp->bnx2x_igu_sb_id, IGU_SEG_ACCESS_DEF,
 				status_idx, IGU_INT_ENABLE, 1);
-	} else {
-		cnic_ack_bnx2x_int(dev, cp->bnx2x_igu_sb_id, USTORM_ID,
-				   status_idx, IGU_INT_ENABLE, 1);
+
+		break;
 	}
 }
 
@@ -2953,7 +2977,8 @@ static void cnic_ulp_stop(struct cnic_dev *dev)
 		struct cnic_ulp_ops *ulp_ops;
 
 		mutex_lock(&cnic_lock);
-		ulp_ops = cp->ulp_ops[if_type];
+		ulp_ops = rcu_dereference_protected(cp->ulp_ops[if_type],
+						    lockdep_is_held(&cnic_lock));
 		if (!ulp_ops) {
 			mutex_unlock(&cnic_lock);
 			continue;
@@ -2977,7 +3002,8 @@ static void cnic_ulp_start(struct cnic_dev *dev)
 		struct cnic_ulp_ops *ulp_ops;
 
 		mutex_lock(&cnic_lock);
-		ulp_ops = cp->ulp_ops[if_type];
+		ulp_ops = rcu_dereference_protected(cp->ulp_ops[if_type],
+						    lockdep_is_held(&cnic_lock));
 		if (!ulp_ops || !ulp_ops->cnic_start) {
 			mutex_unlock(&cnic_lock);
 			continue;
@@ -3041,7 +3067,7 @@ static void cnic_ulp_init(struct cnic_dev *dev)
 		struct cnic_ulp_ops *ulp_ops;
 
 		mutex_lock(&cnic_lock);
-		ulp_ops = cnic_ulp_tbl[i];
+		ulp_ops = cnic_ulp_tbl_prot(i);
 		if (!ulp_ops || !ulp_ops->cnic_init) {
 			mutex_unlock(&cnic_lock);
 			continue;
@@ -3065,7 +3091,7 @@ static void cnic_ulp_exit(struct cnic_dev *dev)
 		struct cnic_ulp_ops *ulp_ops;
 
 		mutex_lock(&cnic_lock);
-		ulp_ops = cnic_ulp_tbl[i];
+		ulp_ops = cnic_ulp_tbl_prot(i);
 		if (!ulp_ops || !ulp_ops->cnic_exit) {
 			mutex_unlock(&cnic_lock);
 			continue;
@@ -3388,9 +3414,12 @@ static int cnic_get_v4_route(struct sockaddr_in *dst_addr,
 	memset(&fl, 0, sizeof(fl));
 	fl.nl_u.ip4_u.daddr = dst_addr->sin_addr.s_addr;
 
-	err = ip_route_output_key(&init_net, &rt, &fl);
-	if (!err)
+	rt = ip_route_output_key(&init_net, &fl);
+	err = 0;
+	if (!IS_ERR(rt))
 		*dst = &rt->dst;
+	else
+		err = PTR_ERR(rt);
 	return err;
 #else
 	return -ENETUNREACH;
@@ -4170,6 +4199,14 @@ static void cnic_enable_bnx2_int(struct cnic_dev *dev)
 		BNX2_PCICFG_INT_ACK_CMD_INDEX_VALID | cp->last_status_idx);
 }
 
+static void cnic_get_bnx2_iscsi_info(struct cnic_dev *dev)
+{
+	u32 max_conn;
+
+	max_conn = cnic_reg_rd_ind(dev, BNX2_FW_MAX_ISCSI_CONN);
+	dev->max_iscsi_conn = max_conn;
+}
+
 static void cnic_disable_bnx2_int_sync(struct cnic_dev *dev)
 {
 	struct cnic_local *cp = dev->cnic_priv;
@@ -4494,6 +4531,8 @@ static int cnic_start_bnx2_hw(struct cnic_dev *dev)
 		return err;
 	}
 
+	cnic_get_bnx2_iscsi_info(dev);
+
 	return 0;
 }
 
@@ -4705,129 +4744,6 @@ static void cnic_init_bnx2x_rx_ring(struct cnic_dev *dev,
 	cp->rx_cons = *cp->rx_cons_ptr;
 }
 
-static int cnic_read_bnx2x_iscsi_mac(struct cnic_dev *dev, u32 upper_addr,
-				     u32 lower_addr)
-{
-	u32 val;
-	u8 mac[6];
-
-	val = CNIC_RD(dev, upper_addr);
-
-	mac[0] = (u8) (val >> 8);
-	mac[1] = (u8) val;
-
-	val = CNIC_RD(dev, lower_addr);
-
-	mac[2] = (u8) (val >> 24);
-	mac[3] = (u8) (val >> 16);
-	mac[4] = (u8) (val >> 8);
-	mac[5] = (u8) val;
-
-	if (is_valid_ether_addr(mac)) {
-		memcpy(dev->mac_addr, mac, 6);
-		return 0;
-	} else {
-		return -EINVAL;
-	}
-}
-
-static void cnic_get_bnx2x_iscsi_info(struct cnic_dev *dev)
-{
-	struct cnic_local *cp = dev->cnic_priv;
-	u32 base, base2, addr, addr1, val;
-	int port = CNIC_PORT(cp);
-
-	dev->max_iscsi_conn = 0;
-	base = CNIC_RD(dev, MISC_REG_SHARED_MEM_ADDR);
-	if (base == 0)
-		return;
-
-	base2 = CNIC_RD(dev, (CNIC_PATH(cp) ? MISC_REG_GENERIC_CR_1 :
-					      MISC_REG_GENERIC_CR_0));
-	addr = BNX2X_SHMEM_ADDR(base,
-		dev_info.port_hw_config[port].iscsi_mac_upper);
-
-	addr1 = BNX2X_SHMEM_ADDR(base,
-		dev_info.port_hw_config[port].iscsi_mac_lower);
-
-	cnic_read_bnx2x_iscsi_mac(dev, addr, addr1);
-
-	addr = BNX2X_SHMEM_ADDR(base, validity_map[port]);
-	val = CNIC_RD(dev, addr);
-
-	if (!(val & SHR_MEM_VALIDITY_LIC_NO_KEY_IN_EFFECT)) {
-		u16 val16;
-
-		addr = BNX2X_SHMEM_ADDR(base,
-				drv_lic_key[port].max_iscsi_init_conn);
-		val16 = CNIC_RD16(dev, addr);
-
-		if (val16)
-			val16 ^= 0x1e1e;
-		dev->max_iscsi_conn = val16;
-	}
-
-	if (BNX2X_CHIP_IS_E2(cp->chip_id))
-		dev->max_fcoe_conn = BNX2X_FCOE_NUM_CONNECTIONS;
-
-	if (BNX2X_CHIP_IS_E1H(cp->chip_id) || BNX2X_CHIP_IS_E2(cp->chip_id)) {
-		int func = CNIC_FUNC(cp);
-		u32 mf_cfg_addr;
-
-		if (BNX2X_SHMEM2_HAS(base2, mf_cfg_addr))
-			mf_cfg_addr = CNIC_RD(dev, BNX2X_SHMEM2_ADDR(base2,
-					      mf_cfg_addr));
-		else
-			mf_cfg_addr = base + BNX2X_SHMEM_MF_BLK_OFFSET;
-
-		if (BNX2X_CHIP_IS_E2(cp->chip_id)) {
-			/* Must determine if the MF is SD vs SI mode */
-			addr = BNX2X_SHMEM_ADDR(base,
-					dev_info.shared_feature_config.config);
-			val = CNIC_RD(dev, addr);
-			if ((val & SHARED_FEAT_CFG_FORCE_SF_MODE_MASK) ==
-			    SHARED_FEAT_CFG_FORCE_SF_MODE_SWITCH_INDEPT) {
-				int rc;
-
-				/* MULTI_FUNCTION_SI mode */
-				addr = BNX2X_MF_CFG_ADDR(mf_cfg_addr,
-					func_ext_config[func].func_cfg);
-				val = CNIC_RD(dev, addr);
-				if (!(val & MACP_FUNC_CFG_FLAGS_ISCSI_OFFLOAD))
-					dev->max_iscsi_conn = 0;
-
-				if (!(val & MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD))
-					dev->max_fcoe_conn = 0;
-
-				addr = BNX2X_MF_CFG_ADDR(mf_cfg_addr,
-					func_ext_config[func].
-					iscsi_mac_addr_upper);
-				addr1 = BNX2X_MF_CFG_ADDR(mf_cfg_addr,
-					func_ext_config[func].
-					iscsi_mac_addr_lower);
-				rc = cnic_read_bnx2x_iscsi_mac(dev, addr,
-								addr1);
-				if (rc && func > 1)
-					dev->max_iscsi_conn = 0;
-
-				return;
-			}
-		}
-
-		addr = BNX2X_MF_CFG_ADDR(mf_cfg_addr,
-			func_mf_config[func].e1hov_tag);
-
-		val = CNIC_RD(dev, addr);
-		val &= FUNC_MF_CFG_E1HOV_TAG_MASK;
-		if (val != FUNC_MF_CFG_E1HOV_TAG_DEFAULT) {
-			dev->max_fcoe_conn = 0;
-			dev->max_iscsi_conn = 0;
-		}
-	}
-	if (!is_valid_ether_addr(dev->mac_addr))
-		dev->max_iscsi_conn = 0;
-}
-
 static void cnic_init_bnx2x_kcq(struct cnic_dev *dev)
 {
 	struct cnic_local *cp = dev->cnic_priv;
@@ -4909,8 +4825,6 @@ static int cnic_start_bnx2x_hw(struct cnic_dev *dev)
 
 	cnic_init_bnx2x_kcq(dev);
 
-	cnic_get_bnx2x_iscsi_info(dev);
-
 	/* Only 1 EQ */
 	CNIC_WR16(dev, cp->kcq1.io_addr, MAX_KCQ_IDX);
 	CNIC_WR(dev, BAR_CSTRORM_INTMEM +
@@ -5264,15 +5178,11 @@ static struct cnic_dev *init_bnx2_cnic(struct net_device *dev)
 
 	dev_hold(dev);
 	pci_dev_get(pdev);
-	if (pdev->device == PCI_DEVICE_ID_NX2_5709 ||
-	    pdev->device == PCI_DEVICE_ID_NX2_5709S) {
-		u8 rev;
-
-		pci_read_config_byte(pdev, PCI_REVISION_ID, &rev);
-		if (rev < 0x10) {
-			pci_dev_put(pdev);
-			goto cnic_err;
-		}
+	if ((pdev->device == PCI_DEVICE_ID_NX2_5709 ||
+	     pdev->device == PCI_DEVICE_ID_NX2_5709S) &&
+	    (pdev->revision < 0x10)) {
+		pci_dev_put(pdev);
+		goto cnic_err;
 	}
 	pci_dev_put(pdev);
 
@@ -5343,6 +5253,14 @@ static struct cnic_dev *init_bnx2x_cnic(struct net_device *dev)
 	cdev->pcidev = pdev;
 	cp->chip_id = ethdev->chip_id;
 
+	if (!(ethdev->drv_state & CNIC_DRV_STATE_NO_ISCSI))
+		cdev->max_iscsi_conn = ethdev->max_iscsi_conn;
+	if (BNX2X_CHIP_IS_E2(cp->chip_id) &&
+	    !(ethdev->drv_state & CNIC_DRV_STATE_NO_FCOE))
+		cdev->max_fcoe_conn = ethdev->max_fcoe_conn;
+
+	memcpy(cdev->mac_addr, ethdev->iscsi_mac, 6);
+
 	cp->cnic_ops = &cnic_bnx2x_ops;
 	cp->start_hw = cnic_start_bnx2x_hw;
 	cp->stop_hw = cnic_stop_bnx2x_hw;
diff --git a/drivers/net/cnic.h b/drivers/net/cnic.h
index b328f6c924c3..4456260c653c 100644
--- a/drivers/net/cnic.h
+++ b/drivers/net/cnic.h
@@ -220,7 +220,7 @@ struct cnic_local {
 #define ULP_F_INIT	0
 #define ULP_F_START	1
 #define ULP_F_CALL_PENDING	2
-	struct cnic_ulp_ops *ulp_ops[MAX_CNIC_ULP_TYPE];
+	struct cnic_ulp_ops __rcu *ulp_ops[MAX_CNIC_ULP_TYPE];
 
 	unsigned long cnic_local_flags;
 #define	CNIC_LCL_FL_KWQ_INIT		0x0
diff --git a/drivers/net/cnic_if.h b/drivers/net/cnic_if.h
index 9f44e0ffe003..e01b49ee3591 100644
--- a/drivers/net/cnic_if.h
+++ b/drivers/net/cnic_if.h
@@ -12,8 +12,8 @@
 #ifndef CNIC_IF_H
 #define CNIC_IF_H
 
-#define CNIC_MODULE_VERSION	"2.2.12"
-#define CNIC_MODULE_RELDATE	"Jan 03, 2011"
+#define CNIC_MODULE_VERSION	"2.2.13"
+#define CNIC_MODULE_RELDATE	"Jan 31, 2011"
 
 #define CNIC_ULP_RDMA		0
 #define CNIC_ULP_ISCSI		1
@@ -159,6 +159,9 @@ struct cnic_eth_dev {
 	u32		drv_state;
 #define CNIC_DRV_STATE_REGD		0x00000001
 #define CNIC_DRV_STATE_USING_MSIX	0x00000002
+#define CNIC_DRV_STATE_NO_ISCSI_OOO	0x00000004
+#define CNIC_DRV_STATE_NO_ISCSI		0x00000008
+#define CNIC_DRV_STATE_NO_FCOE		0x00000010
 	u32		chip_id;
 	u32		max_kwqe_pending;
 	struct pci_dev	*pdev;
@@ -176,6 +179,7 @@ struct cnic_eth_dev {
 	u32		fcoe_init_cid;
 	u16		iscsi_l2_client_id;
 	u16		iscsi_l2_cid;
+	u8		iscsi_mac[ETH_ALEN];
 
 	int		num_irq;
 	struct cnic_irq	irq_arr[MAX_CNIC_VEC];
diff --git a/drivers/net/cxgb3/cxgb3_offload.c b/drivers/net/cxgb3/cxgb3_offload.c
index ef02aa68c926..862804f32b6e 100644
--- a/drivers/net/cxgb3/cxgb3_offload.c
+++ b/drivers/net/cxgb3/cxgb3_offload.c
@@ -186,9 +186,10 @@ static struct net_device *get_iff_from_mac(struct adapter *adapter,
 				dev = NULL;
 				if (grp)
 					dev = vlan_group_get_device(grp, vlan);
-			} else
+			} else if (netif_is_bond_slave(dev)) {
 				while (dev->master)
 					dev = dev->master;
+			}
 			return dev;
 		}
 	}
@@ -967,8 +968,6 @@ static int nb_callback(struct notifier_block *self, unsigned long event,
 		cxgb_neigh_update((struct neighbour *)ctx);
 		break;
 	}
-	case (NETEVENT_PMTU_UPDATE):
-		break;
 	case (NETEVENT_REDIRECT):{
 		struct netevent_redirect *nr = ctx;
 		cxgb_redirect(nr->old, nr->new);
diff --git a/drivers/net/cxgb4/cxgb4_main.c b/drivers/net/cxgb4/cxgb4_main.c
index ec35d458102c..5352c8a23f4d 100644
--- a/drivers/net/cxgb4/cxgb4_main.c
+++ b/drivers/net/cxgb4/cxgb4_main.c
@@ -2471,7 +2471,6 @@ static int netevent_cb(struct notifier_block *nb, unsigned long event,
 	case NETEVENT_NEIGH_UPDATE:
 		check_neigh_update(data);
 		break;
-	case NETEVENT_PMTU_UPDATE:
 	case NETEVENT_REDIRECT:
 	default:
 		break;
diff --git a/drivers/net/davinci_emac.c b/drivers/net/davinci_emac.c
index 2a628d17d178..7018bfe408a4 100644
--- a/drivers/net/davinci_emac.c
+++ b/drivers/net/davinci_emac.c
@@ -1008,7 +1008,7 @@ static void emac_rx_handler(void *token, int len, int status)
 	int			ret;
 
 	/* free and bail if we are shutting down */
-	if (unlikely(!netif_running(ndev))) {
+	if (unlikely(!netif_running(ndev) || !netif_carrier_ok(ndev))) {
 		dev_kfree_skb_any(skb);
 		return;
 	}
diff --git a/drivers/net/dm9000.c b/drivers/net/dm9000.c
index 2d4c4fc1d900..317708113601 100644
--- a/drivers/net/dm9000.c
+++ b/drivers/net/dm9000.c
@@ -802,10 +802,7 @@ dm9000_init_dm9000(struct net_device *dev)
 	/* Checksum mode */
 	dm9000_set_rx_csum_unlocked(dev, db->rx_csum);
 
-	/* GPIO0 on pre-activate PHY */
-	iow(db, DM9000_GPR, 0);	/* REG_1F bit0 activate phyxcer */
 	iow(db, DM9000_GPCR, GPCR_GEP_CNTL);	/* Let GPIO0 output */
-	iow(db, DM9000_GPR, 0);	/* Enable PHY */
 
 	ncr = (db->flags & DM9000_PLATF_EXT_PHY) ? NCR_EXT_PHY : 0;
 
@@ -852,8 +849,8 @@ static void dm9000_timeout(struct net_device *dev)
 	unsigned long flags;
 
 	/* Save previous register address */
-	reg_save = readb(db->io_addr);
 	spin_lock_irqsave(&db->lock, flags);
+	reg_save = readb(db->io_addr);
 
 	netif_stop_queue(dev);
 	dm9000_reset(db);
@@ -1194,6 +1191,10 @@ dm9000_open(struct net_device *dev)
 	if (request_irq(dev->irq, dm9000_interrupt, irqflags, dev->name, dev))
 		return -EAGAIN;
 
+	/* 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 */
+
 	/* Initialize DM9000 board */
 	dm9000_reset(db);
 	dm9000_init_dm9000(dev);
@@ -1592,10 +1593,15 @@ dm9000_probe(struct platform_device *pdev)
 			ndev->dev_addr[i] = ior(db, i+DM9000_PAR);
 	}
 
-	if (!is_valid_ether_addr(ndev->dev_addr))
+	if (!is_valid_ether_addr(ndev->dev_addr)) {
 		dev_warn(db->dev, "%s: Invalid ethernet MAC address. Please "
 			 "set using ifconfig\n", ndev->name);
 
+		random_ether_addr(ndev->dev_addr);
+		mac_src = "random";
+	}
+
+
 	platform_set_drvdata(pdev, ndev);
 	ret = register_netdev(ndev);
 
diff --git a/drivers/net/dnet.c b/drivers/net/dnet.c
index 9d8a20b72fa9..8318ea06cb6d 100644
--- a/drivers/net/dnet.c
+++ b/drivers/net/dnet.c
@@ -337,8 +337,6 @@ static int dnet_mii_init(struct dnet *bp)
 	for (i = 0; i < PHY_MAX_ADDR; i++)
 		bp->mii_bus->irq[i] = PHY_POLL;
 
-	platform_set_drvdata(bp->dev, bp->mii_bus);
-
 	if (mdiobus_register(bp->mii_bus)) {
 		err = -ENXIO;
 		goto err_out_free_mdio_irq;
@@ -863,6 +861,7 @@ static int __devinit dnet_probe(struct platform_device *pdev)
 	bp = netdev_priv(dev);
 	bp->dev = dev;
 
+	platform_set_drvdata(pdev, dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 	spin_lock_init(&bp->lock);
diff --git a/drivers/net/e1000/e1000_osdep.h b/drivers/net/e1000/e1000_osdep.h
index 55c1711f1688..33e7c45a4fe4 100644
--- a/drivers/net/e1000/e1000_osdep.h
+++ b/drivers/net/e1000/e1000_osdep.h
@@ -42,7 +42,8 @@
 #define GBE_CONFIG_RAM_BASE \
 	((unsigned int)(CONFIG_RAM_BASE + GBE_CONFIG_OFFSET))
 
-#define GBE_CONFIG_BASE_VIRT    phys_to_virt(GBE_CONFIG_RAM_BASE)
+#define GBE_CONFIG_BASE_VIRT \
+	((void __iomem *)phys_to_virt(GBE_CONFIG_RAM_BASE))
 
 #define GBE_CONFIG_FLASH_WRITE(base, offset, count, data) \
 	(iowrite16_rep(base + offset, data, count))
diff --git a/drivers/net/e1000e/e1000.h b/drivers/net/e1000e/e1000.h
index e610e1369053..00bf595ebd67 100644
--- a/drivers/net/e1000e/e1000.h
+++ b/drivers/net/e1000e/e1000.h
@@ -364,6 +364,7 @@ struct e1000_adapter {
 	/* structs defined in e1000_hw.h */
 	struct e1000_hw hw;
 
+	spinlock_t stats64_lock;
 	struct e1000_hw_stats stats;
 	struct e1000_phy_info phy_info;
 	struct e1000_phy_stats phy_stats;
@@ -494,7 +495,9 @@ extern int e1000e_setup_rx_resources(struct e1000_adapter *adapter);
 extern int e1000e_setup_tx_resources(struct e1000_adapter *adapter);
 extern void e1000e_free_rx_resources(struct e1000_adapter *adapter);
 extern void e1000e_free_tx_resources(struct e1000_adapter *adapter);
-extern void e1000e_update_stats(struct e1000_adapter *adapter);
+extern struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
+                                                    struct rtnl_link_stats64
+                                                    *stats);
 extern void e1000e_set_interrupt_capability(struct e1000_adapter *adapter);
 extern void e1000e_reset_interrupt_capability(struct e1000_adapter *adapter);
 extern void e1000e_get_hw_control(struct e1000_adapter *adapter);
diff --git a/drivers/net/e1000e/ethtool.c b/drivers/net/e1000e/ethtool.c
index fa08b6336cfb..d4e51aa231b9 100644
--- a/drivers/net/e1000e/ethtool.c
+++ b/drivers/net/e1000e/ethtool.c
@@ -46,15 +46,15 @@ struct e1000_stats {
 };
 
 #define E1000_STAT(str, m) { \
-			.stat_string = str, \
-			.type = E1000_STATS, \
-			.sizeof_stat = sizeof(((struct e1000_adapter *)0)->m), \
-			.stat_offset = offsetof(struct e1000_adapter, m) }
+		.stat_string = str, \
+		.type = E1000_STATS, \
+		.sizeof_stat = sizeof(((struct e1000_adapter *)0)->m), \
+		.stat_offset = offsetof(struct e1000_adapter, m) }
 #define E1000_NETDEV_STAT(str, m) { \
-			.stat_string = str, \
-			.type = NETDEV_STATS, \
-			.sizeof_stat = sizeof(((struct net_device *)0)->m), \
-			.stat_offset = offsetof(struct net_device, m) }
+		.stat_string = str, \
+		.type = NETDEV_STATS, \
+		.sizeof_stat = sizeof(((struct rtnl_link_stats64 *)0)->m), \
+		.stat_offset = offsetof(struct rtnl_link_stats64, m) }
 
 static const struct e1000_stats e1000_gstrings_stats[] = {
 	E1000_STAT("rx_packets", stats.gprc),
@@ -65,21 +65,21 @@ static const struct e1000_stats e1000_gstrings_stats[] = {
 	E1000_STAT("tx_broadcast", stats.bptc),
 	E1000_STAT("rx_multicast", stats.mprc),
 	E1000_STAT("tx_multicast", stats.mptc),
-	E1000_NETDEV_STAT("rx_errors", stats.rx_errors),
-	E1000_NETDEV_STAT("tx_errors", stats.tx_errors),
-	E1000_NETDEV_STAT("tx_dropped", stats.tx_dropped),
+	E1000_NETDEV_STAT("rx_errors", rx_errors),
+	E1000_NETDEV_STAT("tx_errors", tx_errors),
+	E1000_NETDEV_STAT("tx_dropped", tx_dropped),
 	E1000_STAT("multicast", stats.mprc),
 	E1000_STAT("collisions", stats.colc),
-	E1000_NETDEV_STAT("rx_length_errors", stats.rx_length_errors),
-	E1000_NETDEV_STAT("rx_over_errors", stats.rx_over_errors),
+	E1000_NETDEV_STAT("rx_length_errors", rx_length_errors),
+	E1000_NETDEV_STAT("rx_over_errors", rx_over_errors),
 	E1000_STAT("rx_crc_errors", stats.crcerrs),
-	E1000_NETDEV_STAT("rx_frame_errors", stats.rx_frame_errors),
+	E1000_NETDEV_STAT("rx_frame_errors", rx_frame_errors),
 	E1000_STAT("rx_no_buffer_count", stats.rnbc),
 	E1000_STAT("rx_missed_errors", stats.mpc),
 	E1000_STAT("tx_aborted_errors", stats.ecol),
 	E1000_STAT("tx_carrier_errors", stats.tncrs),
-	E1000_NETDEV_STAT("tx_fifo_errors", stats.tx_fifo_errors),
-	E1000_NETDEV_STAT("tx_heartbeat_errors", stats.tx_heartbeat_errors),
+	E1000_NETDEV_STAT("tx_fifo_errors", tx_fifo_errors),
+	E1000_NETDEV_STAT("tx_heartbeat_errors", tx_heartbeat_errors),
 	E1000_STAT("tx_window_errors", stats.latecol),
 	E1000_STAT("tx_abort_late_coll", stats.latecol),
 	E1000_STAT("tx_deferred_ok", stats.dc),
@@ -433,13 +433,11 @@ static void e1000_get_regs(struct net_device *netdev,
 	struct e1000_hw *hw = &adapter->hw;
 	u32 *regs_buff = p;
 	u16 phy_data;
-	u8 revision_id;
 
 	memset(p, 0, E1000_REGS_LEN * sizeof(u32));
 
-	pci_read_config_byte(adapter->pdev, PCI_REVISION_ID, &revision_id);
-
-	regs->version = (1 << 24) | (revision_id << 16) | adapter->pdev->device;
+	regs->version = (1 << 24) | (adapter->pdev->revision << 16) |
+			adapter->pdev->device;
 
 	regs_buff[0]  = er32(CTRL);
 	regs_buff[1]  = er32(STATUS);
@@ -684,20 +682,13 @@ static int e1000_set_ringparam(struct net_device *netdev,
 	rx_old = adapter->rx_ring;
 
 	err = -ENOMEM;
-	tx_ring = kzalloc(sizeof(struct e1000_ring), GFP_KERNEL);
+	tx_ring = kmemdup(tx_old, sizeof(struct e1000_ring), GFP_KERNEL);
 	if (!tx_ring)
 		goto err_alloc_tx;
-	/*
-	 * use a memcpy to save any previously configured
-	 * items like napi structs from having to be
-	 * reinitialized
-	 */
-	memcpy(tx_ring, tx_old, sizeof(struct e1000_ring));
 
-	rx_ring = kzalloc(sizeof(struct e1000_ring), GFP_KERNEL);
+	rx_ring = kmemdup(rx_old, sizeof(struct e1000_ring), GFP_KERNEL);
 	if (!rx_ring)
 		goto err_alloc_rx;
-	memcpy(rx_ring, rx_old, sizeof(struct e1000_ring));
 
 	adapter->tx_ring = tx_ring;
 	adapter->rx_ring = rx_ring;
@@ -1255,7 +1246,6 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ctrl_reg = 0;
-	u32 stat_reg = 0;
 	u16 phy_reg = 0;
 	s32 ret_val = 0;
 
@@ -1363,8 +1353,7 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 		 * Set the ILOS bit on the fiber Nic if half duplex link is
 		 * detected.
 		 */
-		stat_reg = er32(STATUS);
-		if ((stat_reg & E1000_STATUS_FD) == 0)
+		if ((er32(STATUS) & E1000_STATUS_FD) == 0)
 			ctrl_reg |= (E1000_CTRL_ILOS | E1000_CTRL_SLU);
 	}
 
@@ -1972,8 +1961,15 @@ static int e1000_set_coalesce(struct net_device *netdev,
 static int e1000_nway_reset(struct net_device *netdev)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
-	if (netif_running(netdev))
-		e1000e_reinit_locked(adapter);
+
+	if (!netif_running(netdev))
+		return -EAGAIN;
+
+	if (!adapter->hw.mac.autoneg)
+		return -EINVAL;
+
+	e1000e_reinit_locked(adapter);
+
 	return 0;
 }
 
@@ -1982,14 +1978,15 @@ static void e1000_get_ethtool_stats(struct net_device *netdev,
 				    u64 *data)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
+	struct rtnl_link_stats64 net_stats;
 	int i;
 	char *p = NULL;
 
-	e1000e_update_stats(adapter);
+	e1000e_get_stats64(netdev, &net_stats);
 	for (i = 0; i < E1000_GLOBAL_STATS_LEN; i++) {
 		switch (e1000_gstrings_stats[i].type) {
 		case NETDEV_STATS:
-			p = (char *) netdev +
+			p = (char *) &net_stats +
 					e1000_gstrings_stats[i].stat_offset;
 			break;
 		case E1000_STATS:
@@ -2014,7 +2011,7 @@ static void e1000_get_strings(struct net_device *netdev, u32 stringset,
 
 	switch (stringset) {
 	case ETH_SS_TEST:
-		memcpy(data, *e1000_gstrings_test, sizeof(e1000_gstrings_test));
+		memcpy(data, e1000_gstrings_test, sizeof(e1000_gstrings_test));
 		break;
 	case ETH_SS_STATS:
 		for (i = 0; i < E1000_GLOBAL_STATS_LEN; i++) {
diff --git a/drivers/net/e1000e/ich8lan.c b/drivers/net/e1000e/ich8lan.c
index fb46974cfec1..232b42b7f7ce 100644
--- a/drivers/net/e1000e/ich8lan.c
+++ b/drivers/net/e1000e/ich8lan.c
@@ -2104,7 +2104,6 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 {
 	union ich8_hws_flash_status hsfsts;
 	s32 ret_val = -E1000_ERR_NVM;
-	s32 i = 0;
 
 	hsfsts.regval = er16flash(ICH_FLASH_HSFSTS);
 
@@ -2140,6 +2139,8 @@ static s32 e1000_flash_cycle_init_ich8lan(struct e1000_hw *hw)
 		ew16flash(ICH_FLASH_HSFSTS, hsfsts.regval);
 		ret_val = 0;
 	} else {
+		s32 i = 0;
+
 		/*
 		 * Otherwise poll for sometime so the current
 		 * cycle has a chance to end before giving up.
diff --git a/drivers/net/e1000e/lib.c b/drivers/net/e1000e/lib.c
index 68aa1749bf66..96921de5df2e 100644
--- a/drivers/net/e1000e/lib.c
+++ b/drivers/net/e1000e/lib.c
@@ -1978,15 +1978,15 @@ static s32 e1000_ready_nvm_eeprom(struct e1000_hw *hw)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
 	u32 eecd = er32(EECD);
-	u16 timeout = 0;
 	u8 spi_stat_reg;
 
 	if (nvm->type == e1000_nvm_eeprom_spi) {
+		u16 timeout = NVM_MAX_RETRY_SPI;
+
 		/* Clear SK and CS */
 		eecd &= ~(E1000_EECD_CS | E1000_EECD_SK);
 		ew32(EECD, eecd);
 		udelay(1);
-		timeout = NVM_MAX_RETRY_SPI;
 
 		/*
 		 * Read "Status Register" repeatedly until the LSB is cleared.
diff --git a/drivers/net/e1000e/netdev.c b/drivers/net/e1000e/netdev.c
index 3fa110ddb041..660eeb28ed40 100644
--- a/drivers/net/e1000e/netdev.c
+++ b/drivers/net/e1000e/netdev.c
@@ -900,8 +900,6 @@ next_desc:
 
 	adapter->total_rx_bytes += total_rx_bytes;
 	adapter->total_rx_packets += total_rx_packets;
-	netdev->stats.rx_bytes += total_rx_bytes;
-	netdev->stats.rx_packets += total_rx_packets;
 	return cleaned;
 }
 
@@ -1060,8 +1058,6 @@ static bool e1000_clean_tx_irq(struct e1000_adapter *adapter)
 	}
 	adapter->total_tx_bytes += total_tx_bytes;
 	adapter->total_tx_packets += total_tx_packets;
-	netdev->stats.tx_bytes += total_tx_bytes;
-	netdev->stats.tx_packets += total_tx_packets;
 	return count < tx_ring->count;
 }
 
@@ -1248,8 +1244,6 @@ next_desc:
 
 	adapter->total_rx_bytes += total_rx_bytes;
 	adapter->total_rx_packets += total_rx_packets;
-	netdev->stats.rx_bytes += total_rx_bytes;
-	netdev->stats.rx_packets += total_rx_packets;
 	return cleaned;
 }
 
@@ -1429,8 +1423,6 @@ next_desc:
 
 	adapter->total_rx_bytes += total_rx_bytes;
 	adapter->total_rx_packets += total_rx_packets;
-	netdev->stats.rx_bytes += total_rx_bytes;
-	netdev->stats.rx_packets += total_rx_packets;
 	return cleaned;
 }
 
@@ -1857,7 +1849,9 @@ static int e1000_request_msix(struct e1000_adapter *adapter)
 	int err = 0, vector = 0;
 
 	if (strlen(netdev->name) < (IFNAMSIZ - 5))
-		sprintf(adapter->rx_ring->name, "%s-rx-0", netdev->name);
+		snprintf(adapter->rx_ring->name,
+			 sizeof(adapter->rx_ring->name) - 1,
+			 "%s-rx-0", netdev->name);
 	else
 		memcpy(adapter->rx_ring->name, netdev->name, IFNAMSIZ);
 	err = request_irq(adapter->msix_entries[vector].vector,
@@ -1870,7 +1864,9 @@ static int e1000_request_msix(struct e1000_adapter *adapter)
 	vector++;
 
 	if (strlen(netdev->name) < (IFNAMSIZ - 5))
-		sprintf(adapter->tx_ring->name, "%s-tx-0", netdev->name);
+		snprintf(adapter->tx_ring->name,
+			 sizeof(adapter->tx_ring->name) - 1,
+			 "%s-tx-0", netdev->name);
 	else
 		memcpy(adapter->tx_ring->name, netdev->name, IFNAMSIZ);
 	err = request_irq(adapter->msix_entries[vector].vector,
@@ -2734,7 +2730,6 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 rctl, rfctl;
-	u32 psrctl = 0;
 	u32 pages = 0;
 
 	/* Workaround Si errata on 82579 - configure jumbo frame flow */
@@ -2833,6 +2828,8 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 		adapter->rx_ps_pages = 0;
 
 	if (adapter->rx_ps_pages) {
+		u32 psrctl = 0;
+
 		/* Configure extra packet-split registers */
 		rfctl = er32(RFCTL);
 		rfctl |= E1000_RFCTL_EXTEN;
@@ -3034,7 +3031,6 @@ static void e1000_set_multi(struct net_device *netdev)
 	struct netdev_hw_addr *ha;
 	u8  *mta_list;
 	u32 rctl;
-	int i;
 
 	/* Check for Promiscuous and All Multicast modes */
 
@@ -3057,12 +3053,13 @@ static void e1000_set_multi(struct net_device *netdev)
 	ew32(RCTL, rctl);
 
 	if (!netdev_mc_empty(netdev)) {
+		int i = 0;
+
 		mta_list = kmalloc(netdev_mc_count(netdev) * 6, GFP_ATOMIC);
 		if (!mta_list)
 			return;
 
 		/* prepare a packed array of only addresses. */
-		i = 0;
 		netdev_for_each_mc_addr(ha, netdev)
 			memcpy(mta_list + (i++ * ETH_ALEN), ha->addr, ETH_ALEN);
 
@@ -3359,6 +3356,8 @@ static void e1000e_flush_descriptors(struct e1000_adapter *adapter)
 	e1e_flush();
 }
 
+static void e1000e_update_stats(struct e1000_adapter *adapter);
+
 void e1000e_down(struct e1000_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
@@ -3393,6 +3392,11 @@ void e1000e_down(struct e1000_adapter *adapter)
 	del_timer_sync(&adapter->phy_info_timer);
 
 	netif_carrier_off(netdev);
+
+	spin_lock(&adapter->stats64_lock);
+	e1000e_update_stats(adapter);
+	spin_unlock(&adapter->stats64_lock);
+
 	adapter->link_speed = 0;
 	adapter->link_duplex = 0;
 
@@ -3437,6 +3441,8 @@ static int __devinit e1000_sw_init(struct e1000_adapter *adapter)
 	adapter->max_frame_size = netdev->mtu + ETH_HLEN + ETH_FCS_LEN;
 	adapter->min_frame_size = ETH_ZLEN + ETH_FCS_LEN;
 
+	spin_lock_init(&adapter->stats64_lock);
+
 	e1000e_set_interrupt_capability(adapter);
 
 	if (e1000_alloc_queues(adapter))
@@ -3918,7 +3924,7 @@ release:
  * e1000e_update_stats - Update the board statistics counters
  * @adapter: board private structure
  **/
-void e1000e_update_stats(struct e1000_adapter *adapter)
+static void e1000e_update_stats(struct e1000_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 	struct e1000_hw *hw = &adapter->hw;
@@ -4030,10 +4036,11 @@ static void e1000_phy_read_status(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	struct e1000_phy_regs *phy = &adapter->phy_regs;
-	int ret_val;
 
 	if ((er32(STATUS) & E1000_STATUS_LU) &&
 	    (adapter->hw.phy.media_type == e1000_media_type_copper)) {
+		int ret_val;
+
 		ret_val  = e1e_rphy(hw, PHY_CONTROL, &phy->bmcr);
 		ret_val |= e1e_rphy(hw, PHY_STATUS, &phy->bmsr);
 		ret_val |= e1e_rphy(hw, PHY_AUTONEG_ADV, &phy->advertise);
@@ -4179,7 +4186,6 @@ static void e1000_watchdog_task(struct work_struct *work)
 	struct e1000_ring *tx_ring = adapter->tx_ring;
 	struct e1000_hw *hw = &adapter->hw;
 	u32 link, tctl;
-	int tx_pending = 0;
 
 	if (test_bit(__E1000_DOWN, &adapter->state))
 		return;
@@ -4320,7 +4326,9 @@ static void e1000_watchdog_task(struct work_struct *work)
 	}
 
 link_up:
+	spin_lock(&adapter->stats64_lock);
 	e1000e_update_stats(adapter);
+	spin_unlock(&adapter->stats64_lock);
 
 	mac->tx_packet_delta = adapter->stats.tpt - adapter->tpt_old;
 	adapter->tpt_old = adapter->stats.tpt;
@@ -4334,20 +4342,17 @@ link_up:
 
 	e1000e_update_adaptive(&adapter->hw);
 
-	if (!netif_carrier_ok(netdev)) {
-		tx_pending = (e1000_desc_unused(tx_ring) + 1 <
-			       tx_ring->count);
-		if (tx_pending) {
-			/*
-			 * We've lost link, so the controller stops DMA,
-			 * but we've got queued Tx work that's never going
-			 * to get done, so reset controller to flush Tx.
-			 * (Do the reset outside of interrupt context).
-			 */
-			schedule_work(&adapter->reset_task);
-			/* return immediately since reset is imminent */
-			return;
-		}
+	if (!netif_carrier_ok(netdev) &&
+	    (e1000_desc_unused(tx_ring) + 1 < tx_ring->count)) {
+		/*
+		 * We've lost link, so the controller stops DMA,
+		 * but we've got queued Tx work that's never going
+		 * to get done, so reset controller to flush Tx.
+		 * (Do the reset outside of interrupt context).
+		 */
+		schedule_work(&adapter->reset_task);
+		/* return immediately since reset is imminent */
+		return;
 	}
 
 	/* Simple mode for Interrupt Throttle Rate (ITR) */
@@ -4411,13 +4416,13 @@ static int e1000_tso(struct e1000_adapter *adapter,
 	u32 cmd_length = 0;
 	u16 ipcse = 0, tucse, mss;
 	u8 ipcss, ipcso, tucss, tucso, hdr_len;
-	int err;
 
 	if (!skb_is_gso(skb))
 		return 0;
 
 	if (skb_header_cloned(skb)) {
-		err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
+		int err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
+
 		if (err)
 			return err;
 	}
@@ -4928,16 +4933,55 @@ static void e1000_reset_task(struct work_struct *work)
 }
 
 /**
- * e1000_get_stats - Get System Network Statistics
+ * e1000_get_stats64 - Get System Network Statistics
  * @netdev: network interface device structure
+ * @stats: rtnl_link_stats64 pointer
  *
  * Returns the address of the device statistics structure.
- * The statistics are actually updated from the timer callback.
  **/
-static struct net_device_stats *e1000_get_stats(struct net_device *netdev)
+struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
+                                             struct rtnl_link_stats64 *stats)
 {
-	/* only return the current stats */
-	return &netdev->stats;
+	struct e1000_adapter *adapter = netdev_priv(netdev);
+
+	memset(stats, 0, sizeof(struct rtnl_link_stats64));
+	spin_lock(&adapter->stats64_lock);
+	e1000e_update_stats(adapter);
+	/* Fill out the OS statistics structure */
+	stats->rx_bytes = adapter->stats.gorc;
+	stats->rx_packets = adapter->stats.gprc;
+	stats->tx_bytes = adapter->stats.gotc;
+	stats->tx_packets = adapter->stats.gptc;
+	stats->multicast = adapter->stats.mprc;
+	stats->collisions = adapter->stats.colc;
+
+	/* Rx Errors */
+
+	/*
+	 * RLEC on some newer hardware can be incorrect so build
+	 * our own version based on RUC and ROC
+	 */
+	stats->rx_errors = adapter->stats.rxerrc +
+		adapter->stats.crcerrs + adapter->stats.algnerrc +
+		adapter->stats.ruc + adapter->stats.roc +
+		adapter->stats.cexterr;
+	stats->rx_length_errors = adapter->stats.ruc +
+					      adapter->stats.roc;
+	stats->rx_crc_errors = adapter->stats.crcerrs;
+	stats->rx_frame_errors = adapter->stats.algnerrc;
+	stats->rx_missed_errors = adapter->stats.mpc;
+
+	/* Tx Errors */
+	stats->tx_errors = adapter->stats.ecol +
+				       adapter->stats.latecol;
+	stats->tx_aborted_errors = adapter->stats.ecol;
+	stats->tx_window_errors = adapter->stats.latecol;
+	stats->tx_carrier_errors = adapter->stats.tncrs;
+
+	/* Tx Dropped needs to be maintained elsewhere */
+
+	spin_unlock(&adapter->stats64_lock);
+	return stats;
 }
 
 /**
@@ -5338,7 +5382,7 @@ void e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
 	__e1000e_disable_aspm(pdev, state);
 }
 
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 static bool e1000e_pm_ready(struct e1000_adapter *adapter)
 {
 	return !!adapter->tx_ring->buffer_info;
@@ -5489,7 +5533,7 @@ static int e1000_runtime_resume(struct device *dev)
 	return __e1000_resume(pdev);
 }
 #endif /* CONFIG_PM_RUNTIME */
-#endif /* CONFIG_PM_OPS */
+#endif /* CONFIG_PM */
 
 static void e1000_shutdown(struct pci_dev *pdev)
 {
@@ -5507,9 +5551,10 @@ static irqreturn_t e1000_intr_msix(int irq, void *data)
 {
 	struct net_device *netdev = data;
 	struct e1000_adapter *adapter = netdev_priv(netdev);
-	int vector, msix_irq;
 
 	if (adapter->msix_entries) {
+		int vector, msix_irq;
+
 		vector = 0;
 		msix_irq = adapter->msix_entries[vector].vector;
 		disable_irq(msix_irq);
@@ -5706,7 +5751,7 @@ static const struct net_device_ops e1000e_netdev_ops = {
 	.ndo_open		= e1000_open,
 	.ndo_stop		= e1000_close,
 	.ndo_start_xmit		= e1000_xmit_frame,
-	.ndo_get_stats		= e1000_get_stats,
+	.ndo_get_stats64	= e1000e_get_stats64,
 	.ndo_set_multicast_list	= e1000_set_multi,
 	.ndo_set_mac_address	= e1000_set_mac,
 	.ndo_change_mtu		= e1000_change_mtu,
@@ -5967,7 +6012,8 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 		/* APME bit in EEPROM is mapped to WUC.APME */
 		eeprom_data = er32(WUC);
 		eeprom_apme_mask = E1000_WUC_APME;
-		if (eeprom_data & E1000_WUC_PHY_WAKE)
+		if ((hw->mac.type > e1000_ich10lan) &&
+		    (eeprom_data & E1000_WUC_PHY_WAKE))
 			adapter->flags2 |= FLAG2_HAS_PHY_WAKEUP;
 	} else if (adapter->flags & FLAG_APME_IN_CTRL3) {
 		if (adapter->flags & FLAG_APME_CHECK_PORT_B &&
@@ -6195,7 +6241,7 @@ static DEFINE_PCI_DEVICE_TABLE(e1000_pci_tbl) = {
 };
 MODULE_DEVICE_TABLE(pci, e1000_pci_tbl);
 
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 static const struct dev_pm_ops e1000_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(e1000_suspend, e1000_resume)
 	SET_RUNTIME_PM_OPS(e1000_runtime_suspend,
@@ -6209,7 +6255,7 @@ static struct pci_driver e1000_driver = {
 	.id_table = e1000_pci_tbl,
 	.probe    = e1000_probe,
 	.remove   = __devexit_p(e1000_remove),
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 	.driver.pm = &e1000_pm_ops,
 #endif
 	.shutdown = e1000_shutdown,
diff --git a/drivers/net/e1000e/phy.c b/drivers/net/e1000e/phy.c
index 6bea051b134b..6ae31fcfb629 100644
--- a/drivers/net/e1000e/phy.c
+++ b/drivers/net/e1000e/phy.c
@@ -2409,9 +2409,7 @@ static u32 e1000_get_phy_addr_for_bm_page(u32 page, u32 reg)
 s32 e1000e_write_phy_reg_bm(struct e1000_hw *hw, u32 offset, u16 data)
 {
 	s32 ret_val;
-	u32 page_select = 0;
 	u32 page = offset >> IGP_PAGE_SHIFT;
-	u32 page_shift = 0;
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
@@ -2427,6 +2425,8 @@ s32 e1000e_write_phy_reg_bm(struct e1000_hw *hw, u32 offset, u16 data)
 	hw->phy.addr = e1000_get_phy_addr_for_bm_page(page, offset);
 
 	if (offset > MAX_PHY_MULTI_PAGE_REG) {
+		u32 page_shift, page_select;
+
 		/*
 		 * Page select is register 31 for phy address 1 and 22 for
 		 * phy address 2 and 3. Page select is shifted only for
@@ -2468,9 +2468,7 @@ out:
 s32 e1000e_read_phy_reg_bm(struct e1000_hw *hw, u32 offset, u16 *data)
 {
 	s32 ret_val;
-	u32 page_select = 0;
 	u32 page = offset >> IGP_PAGE_SHIFT;
-	u32 page_shift = 0;
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
@@ -2486,6 +2484,8 @@ s32 e1000e_read_phy_reg_bm(struct e1000_hw *hw, u32 offset, u16 *data)
 	hw->phy.addr = e1000_get_phy_addr_for_bm_page(page, offset);
 
 	if (offset > MAX_PHY_MULTI_PAGE_REG) {
+		u32 page_shift, page_select;
+
 		/*
 		 * Page select is register 31 for phy address 1 and 22 for
 		 * phy address 2 and 3. Page select is shifted only for
diff --git a/drivers/net/enic/Makefile b/drivers/net/enic/Makefile
index e7b6c31880ba..2e573be16c13 100644
--- a/drivers/net/enic/Makefile
+++ b/drivers/net/enic/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_ENIC) := enic.o
 
 enic-y := enic_main.o vnic_cq.o vnic_intr.o vnic_wq.o \
-	enic_res.o vnic_dev.o vnic_rq.o vnic_vic.o
+	enic_res.o enic_dev.o vnic_dev.o vnic_rq.o vnic_vic.o
 
diff --git a/drivers/net/enic/enic.h b/drivers/net/enic/enic.h
index a937f49d9db7..e816bbb9fbf9 100644
--- a/drivers/net/enic/enic.h
+++ b/drivers/net/enic/enic.h
@@ -32,13 +32,13 @@
 
 #define DRV_NAME		"enic"
 #define DRV_DESCRIPTION		"Cisco VIC Ethernet NIC Driver"
-#define DRV_VERSION		"1.4.1.10"
-#define DRV_COPYRIGHT		"Copyright 2008-2010 Cisco Systems, Inc"
+#define DRV_VERSION		"2.1.1.10"
+#define DRV_COPYRIGHT		"Copyright 2008-2011 Cisco Systems, Inc"
 
 #define ENIC_BARS_MAX		6
 
-#define ENIC_WQ_MAX		8
-#define ENIC_RQ_MAX		8
+#define ENIC_WQ_MAX		1
+#define ENIC_RQ_MAX		1
 #define ENIC_CQ_MAX		(ENIC_WQ_MAX + ENIC_RQ_MAX)
 #define ENIC_INTR_MAX		(ENIC_CQ_MAX + 2)
 
@@ -49,7 +49,7 @@ struct enic_msix_entry {
 	void *devid;
 };
 
-#define ENIC_SET_APPLIED		(1 << 0)
+#define ENIC_PORT_REQUEST_APPLIED	(1 << 0)
 #define ENIC_SET_REQUEST		(1 << 1)
 #define ENIC_SET_NAME			(1 << 2)
 #define ENIC_SET_INSTANCE		(1 << 3)
@@ -101,7 +101,6 @@ struct enic {
 	/* receive queue cache line section */
 	____cacheline_aligned struct vnic_rq rq[ENIC_RQ_MAX];
 	unsigned int rq_count;
-	int (*rq_alloc_buf)(struct vnic_rq *rq);
 	u64 rq_truncated_pkts;
 	u64 rq_bad_fcs;
 	struct napi_struct napi[ENIC_RQ_MAX];
diff --git a/drivers/net/enic/enic_dev.c b/drivers/net/enic/enic_dev.c
new file mode 100644
index 000000000000..37ad3a1c82ee
--- /dev/null
+++ b/drivers/net/enic/enic_dev.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2011 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <linux/pci.h>
+#include <linux/etherdevice.h>
+
+#include "vnic_dev.h"
+#include "vnic_vic.h"
+#include "enic_res.h"
+#include "enic.h"
+#include "enic_dev.h"
+
+int enic_dev_fw_info(struct enic *enic, struct vnic_devcmd_fw_info **fw_info)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_fw_info(enic->vdev, fw_info);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_stats_dump(struct enic *enic, struct vnic_stats **vstats)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_stats_dump(enic->vdev, vstats);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_add_station_addr(struct enic *enic)
+{
+	int err;
+
+	if (!is_valid_ether_addr(enic->netdev->dev_addr))
+		return -EADDRNOTAVAIL;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_add_addr(enic->vdev, enic->netdev->dev_addr);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_del_station_addr(struct enic *enic)
+{
+	int err;
+
+	if (!is_valid_ether_addr(enic->netdev->dev_addr))
+		return -EADDRNOTAVAIL;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_del_addr(enic->vdev, enic->netdev->dev_addr);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_packet_filter(struct enic *enic, int directed, int multicast,
+	int broadcast, int promisc, int allmulti)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_packet_filter(enic->vdev, directed,
+		multicast, broadcast, promisc, allmulti);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_add_addr(struct enic *enic, u8 *addr)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_add_addr(enic->vdev, addr);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_del_addr(struct enic *enic, u8 *addr)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_del_addr(enic->vdev, addr);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_notify_unset(struct enic *enic)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_notify_unset(enic->vdev);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_hang_notify(struct enic *enic)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_hang_notify(enic->vdev);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_set_ig_vlan_rewrite_mode(struct enic *enic)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_set_ig_vlan_rewrite_mode(enic->vdev,
+		IG_VLAN_REWRITE_MODE_PRIORITY_TAG_DEFAULT_VLAN);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_enable(struct enic *enic)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_enable_wait(enic->vdev);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_disable(struct enic *enic)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_disable(enic->vdev);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_vnic_dev_deinit(struct enic *enic)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_deinit(enic->vdev);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_init_prov(struct enic *enic, struct vic_provinfo *vp)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_init_prov(enic->vdev,
+		(u8 *)vp, vic_provinfo_size(vp));
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+int enic_dev_init_done(struct enic *enic, int *done, int *error)
+{
+	int err;
+
+	spin_lock(&enic->devcmd_lock);
+	err = vnic_dev_init_done(enic->vdev, done, error);
+	spin_unlock(&enic->devcmd_lock);
+
+	return err;
+}
+
+/* rtnl lock is held */
+void enic_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+{
+	struct enic *enic = netdev_priv(netdev);
+
+	spin_lock(&enic->devcmd_lock);
+	enic_add_vlan(enic, vid);
+	spin_unlock(&enic->devcmd_lock);
+}
+
+/* rtnl lock is held */
+void enic_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+{
+	struct enic *enic = netdev_priv(netdev);
+
+	spin_lock(&enic->devcmd_lock);
+	enic_del_vlan(enic, vid);
+	spin_unlock(&enic->devcmd_lock);
+}
diff --git a/drivers/net/enic/enic_dev.h b/drivers/net/enic/enic_dev.h
new file mode 100644
index 000000000000..495f57fcb887
--- /dev/null
+++ b/drivers/net/enic/enic_dev.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright 2011 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef _ENIC_DEV_H_
+#define _ENIC_DEV_H_
+
+int enic_dev_fw_info(struct enic *enic, struct vnic_devcmd_fw_info **fw_info);
+int enic_dev_stats_dump(struct enic *enic, struct vnic_stats **vstats);
+int enic_dev_add_station_addr(struct enic *enic);
+int enic_dev_del_station_addr(struct enic *enic);
+int enic_dev_packet_filter(struct enic *enic, int directed, int multicast,
+	int broadcast, int promisc, int allmulti);
+int enic_dev_add_addr(struct enic *enic, u8 *addr);
+int enic_dev_del_addr(struct enic *enic, u8 *addr);
+void enic_vlan_rx_add_vid(struct net_device *netdev, u16 vid);
+void enic_vlan_rx_kill_vid(struct net_device *netdev, u16 vid);
+int enic_dev_notify_unset(struct enic *enic);
+int enic_dev_hang_notify(struct enic *enic);
+int enic_dev_set_ig_vlan_rewrite_mode(struct enic *enic);
+int enic_dev_enable(struct enic *enic);
+int enic_dev_disable(struct enic *enic);
+int enic_vnic_dev_deinit(struct enic *enic);
+int enic_dev_init_prov(struct enic *enic, struct vic_provinfo *vp);
+int enic_dev_init_done(struct enic *enic, int *done, int *error);
+
+#endif /* _ENIC_DEV_H_ */
diff --git a/drivers/net/enic/enic_main.c b/drivers/net/enic/enic_main.c
index a0af48c51fb3..8b9cad5e9712 100644
--- a/drivers/net/enic/enic_main.c
+++ b/drivers/net/enic/enic_main.c
@@ -44,6 +44,7 @@
 #include "vnic_vic.h"
 #include "enic_res.h"
 #include "enic.h"
+#include "enic_dev.h"
 
 #define ENIC_NOTIFY_TIMER_PERIOD	(2 * HZ)
 #define WQ_ENET_MAX_DESC_LEN		(1 << WQ_ENET_LEN_BITS)
@@ -190,18 +191,6 @@ static int enic_get_settings(struct net_device *netdev,
 	return 0;
 }
 
-static int enic_dev_fw_info(struct enic *enic,
-	struct vnic_devcmd_fw_info **fw_info)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_fw_info(enic->vdev, fw_info);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
 static void enic_get_drvinfo(struct net_device *netdev,
 	struct ethtool_drvinfo *drvinfo)
 {
@@ -246,17 +235,6 @@ static int enic_get_sset_count(struct net_device *netdev, int sset)
 	}
 }
 
-static int enic_dev_stats_dump(struct enic *enic, struct vnic_stats **vstats)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_stats_dump(enic->vdev, vstats);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
 static void enic_get_ethtool_stats(struct net_device *netdev,
 	struct ethtool_stats *stats, u64 *data)
 {
@@ -896,9 +874,10 @@ static struct net_device_stats *enic_get_stats(struct net_device *netdev)
 	return net_stats;
 }
 
-static void enic_reset_multicast_list(struct enic *enic)
+static void enic_reset_addr_lists(struct enic *enic)
 {
 	enic->mc_count = 0;
+	enic->uc_count = 0;
 	enic->flags = 0;
 }
 
@@ -919,32 +898,6 @@ static int enic_set_mac_addr(struct net_device *netdev, char *addr)
 	return 0;
 }
 
-static int enic_dev_add_station_addr(struct enic *enic)
-{
-	int err = 0;
-
-	if (is_valid_ether_addr(enic->netdev->dev_addr)) {
-		spin_lock(&enic->devcmd_lock);
-		err = vnic_dev_add_addr(enic->vdev, enic->netdev->dev_addr);
-		spin_unlock(&enic->devcmd_lock);
-	}
-
-	return err;
-}
-
-static int enic_dev_del_station_addr(struct enic *enic)
-{
-	int err = 0;
-
-	if (is_valid_ether_addr(enic->netdev->dev_addr)) {
-		spin_lock(&enic->devcmd_lock);
-		err = vnic_dev_del_addr(enic->vdev, enic->netdev->dev_addr);
-		spin_unlock(&enic->devcmd_lock);
-	}
-
-	return err;
-}
-
 static int enic_set_mac_address_dynamic(struct net_device *netdev, void *p)
 {
 	struct enic *enic = netdev_priv(netdev);
@@ -989,42 +942,7 @@ static int enic_set_mac_address(struct net_device *netdev, void *p)
 	return enic_dev_add_station_addr(enic);
 }
 
-static int enic_dev_packet_filter(struct enic *enic, int directed,
-	int multicast, int broadcast, int promisc, int allmulti)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_packet_filter(enic->vdev, directed,
-		multicast, broadcast, promisc, allmulti);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_add_addr(struct enic *enic, u8 *addr)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_add_addr(enic->vdev, addr);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_del_addr(struct enic *enic, u8 *addr)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_del_addr(enic->vdev, addr);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static void enic_add_multicast_addr_list(struct enic *enic)
+static void enic_update_multicast_addr_list(struct enic *enic)
 {
 	struct net_device *netdev = enic->netdev;
 	struct netdev_hw_addr *ha;
@@ -1079,7 +997,7 @@ static void enic_add_multicast_addr_list(struct enic *enic)
 	enic->mc_count = mc_count;
 }
 
-static void enic_add_unicast_addr_list(struct enic *enic)
+static void enic_update_unicast_addr_list(struct enic *enic)
 {
 	struct net_device *netdev = enic->netdev;
 	struct netdev_hw_addr *ha;
@@ -1156,9 +1074,9 @@ static void enic_set_rx_mode(struct net_device *netdev)
 	}
 
 	if (!promisc) {
-		enic_add_unicast_addr_list(enic);
+		enic_update_unicast_addr_list(enic);
 		if (!allmulti)
-			enic_add_multicast_addr_list(enic);
+			enic_update_multicast_addr_list(enic);
 	}
 }
 
@@ -1170,26 +1088,6 @@ static void enic_vlan_rx_register(struct net_device *netdev,
 	enic->vlan_group = vlan_group;
 }
 
-/* rtnl lock is held */
-static void enic_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
-{
-	struct enic *enic = netdev_priv(netdev);
-
-	spin_lock(&enic->devcmd_lock);
-	enic_add_vlan(enic, vid);
-	spin_unlock(&enic->devcmd_lock);
-}
-
-/* rtnl lock is held */
-static void enic_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
-{
-	struct enic *enic = netdev_priv(netdev);
-
-	spin_lock(&enic->devcmd_lock);
-	enic_del_vlan(enic, vid);
-	spin_unlock(&enic->devcmd_lock);
-}
-
 /* netif_tx_lock held, BHs disabled */
 static void enic_tx_timeout(struct net_device *netdev)
 {
@@ -1197,40 +1095,6 @@ static void enic_tx_timeout(struct net_device *netdev)
 	schedule_work(&enic->reset);
 }
 
-static int enic_vnic_dev_deinit(struct enic *enic)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_deinit(enic->vdev);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_init_prov(struct enic *enic, struct vic_provinfo *vp)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_init_prov(enic->vdev,
-		(u8 *)vp, vic_provinfo_size(vp));
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_init_done(struct enic *enic, int *done, int *error)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_init_done(enic->vdev, done, error);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
 static int enic_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 {
 	struct enic *enic = netdev_priv(netdev);
@@ -1262,6 +1126,8 @@ static int enic_set_port_profile(struct enic *enic, u8 *mac)
 	if (err)
 		return err;
 
+	enic_reset_addr_lists(enic);
+
 	switch (enic->pp.request) {
 
 	case PORT_REQUEST_ASSOCIATE:
@@ -1318,18 +1184,20 @@ static int enic_set_port_profile(struct enic *enic, u8 *mac)
 		vic_provinfo_free(vp);
 		if (err)
 			return err;
-
-		enic->pp.set |= ENIC_SET_APPLIED;
 		break;
 
 	case PORT_REQUEST_DISASSOCIATE:
-		enic->pp.set &= ~ENIC_SET_APPLIED;
 		break;
 
 	default:
 		return -EINVAL;
 	}
 
+	/* Set flag to indicate that the port assoc/disassoc
+	 * request has been sent out to fw
+	 */
+	enic->pp.set |= ENIC_PORT_REQUEST_APPLIED;
+
 	return 0;
 }
 
@@ -1379,9 +1247,6 @@ static int enic_set_vf_port(struct net_device *netdev, int vf,
 
 		if (is_zero_ether_addr(netdev->dev_addr))
 			random_ether_addr(netdev->dev_addr);
-	} else if (new_pp.request == PORT_REQUEST_DISASSOCIATE) {
-		if (!is_zero_ether_addr(enic->pp.mac_addr))
-			enic_dev_del_addr(enic, enic->pp.mac_addr);
 	}
 
 	memcpy(&enic->pp, &new_pp, sizeof(struct enic_port_profile));
@@ -1390,9 +1255,6 @@ static int enic_set_vf_port(struct net_device *netdev, int vf,
 	if (err)
 		goto set_port_profile_cleanup;
 
-	if (!is_zero_ether_addr(enic->pp.mac_addr))
-		enic_dev_add_addr(enic, enic->pp.mac_addr);
-
 set_port_profile_cleanup:
 	memset(enic->pp.vf_mac, 0, ETH_ALEN);
 
@@ -1411,7 +1273,7 @@ static int enic_get_vf_port(struct net_device *netdev, int vf,
 	int err, error, done;
 	u16 response = PORT_PROFILE_RESPONSE_SUCCESS;
 
-	if (!(enic->pp.set & ENIC_SET_APPLIED))
+	if (!(enic->pp.set & ENIC_PORT_REQUEST_APPLIED))
 		return -ENODATA;
 
 	err = enic_dev_init_done(enic, &done, &error);
@@ -1489,62 +1351,6 @@ static int enic_rq_alloc_buf(struct vnic_rq *rq)
 	return 0;
 }
 
-static int enic_rq_alloc_buf_a1(struct vnic_rq *rq)
-{
-	struct rq_enet_desc *desc = vnic_rq_next_desc(rq);
-
-	if (vnic_rq_posting_soon(rq)) {
-
-		/* SW workaround for A0 HW erratum: if we're just about
-		 * to write posted_index, insert a dummy desc
-		 * of type resvd
-		 */
-
-		rq_enet_desc_enc(desc, 0, RQ_ENET_TYPE_RESV2, 0);
-		vnic_rq_post(rq, 0, 0, 0, 0);
-	} else {
-		return enic_rq_alloc_buf(rq);
-	}
-
-	return 0;
-}
-
-static int enic_dev_hw_version(struct enic *enic,
-	enum vnic_dev_hw_version *hw_ver)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_hw_version(enic->vdev, hw_ver);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_set_rq_alloc_buf(struct enic *enic)
-{
-	enum vnic_dev_hw_version hw_ver;
-	int err;
-
-	err = enic_dev_hw_version(enic, &hw_ver);
-	if (err)
-		return err;
-
-	switch (hw_ver) {
-	case VNIC_DEV_HW_VER_A1:
-		enic->rq_alloc_buf = enic_rq_alloc_buf_a1;
-		break;
-	case VNIC_DEV_HW_VER_A2:
-	case VNIC_DEV_HW_VER_UNKNOWN:
-		enic->rq_alloc_buf = enic_rq_alloc_buf;
-		break;
-	default:
-		return -ENODEV;
-	}
-
-	return 0;
-}
-
 static void enic_rq_indicate_buf(struct vnic_rq *rq,
 	struct cq_desc *cq_desc, struct vnic_rq_buf *buf,
 	int skipped, void *opaque)
@@ -1681,7 +1487,7 @@ static int enic_poll(struct napi_struct *napi, int budget)
 			0 /* don't unmask intr */,
 			0 /* don't reset intr timer */);
 
-	err = vnic_rq_fill(&enic->rq[0], enic->rq_alloc_buf);
+	err = vnic_rq_fill(&enic->rq[0], enic_rq_alloc_buf);
 
 	/* Buffer allocation failed. Stay in polling
 	 * mode so we can try to fill the ring again.
@@ -1731,7 +1537,7 @@ static int enic_poll_msix(struct napi_struct *napi, int budget)
 			0 /* don't unmask intr */,
 			0 /* don't reset intr timer */);
 
-	err = vnic_rq_fill(&enic->rq[rq], enic->rq_alloc_buf);
+	err = vnic_rq_fill(&enic->rq[rq], enic_rq_alloc_buf);
 
 	/* Buffer allocation failed. Stay in polling mode
 	 * so we can try to fill the ring again.
@@ -1901,39 +1707,6 @@ static int enic_dev_notify_set(struct enic *enic)
 	return err;
 }
 
-static int enic_dev_notify_unset(struct enic *enic)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_notify_unset(enic->vdev);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_enable(struct enic *enic)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_enable_wait(enic->vdev);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_disable(struct enic *enic)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_disable(enic->vdev);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
 static void enic_notify_timer_start(struct enic *enic)
 {
 	switch (vnic_dev_get_intr_mode(enic->vdev)) {
@@ -1967,7 +1740,7 @@ static int enic_open(struct net_device *netdev)
 	}
 
 	for (i = 0; i < enic->rq_count; i++) {
-		vnic_rq_fill(&enic->rq[i], enic->rq_alloc_buf);
+		vnic_rq_fill(&enic->rq[i], enic_rq_alloc_buf);
 		/* Need at least one buffer on ring to get going */
 		if (vnic_rq_desc_used(&enic->rq[i]) == 0) {
 			netdev_err(netdev, "Unable to alloc receive buffers\n");
@@ -2285,29 +2058,6 @@ static int enic_set_rss_nic_cfg(struct enic *enic)
 		rss_hash_bits, rss_base_cpu, rss_enable);
 }
 
-static int enic_dev_hang_notify(struct enic *enic)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_hang_notify(enic->vdev);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
-static int enic_dev_set_ig_vlan_rewrite_mode(struct enic *enic)
-{
-	int err;
-
-	spin_lock(&enic->devcmd_lock);
-	err = vnic_dev_set_ig_vlan_rewrite_mode(enic->vdev,
-		IG_VLAN_REWRITE_MODE_PRIORITY_TAG_DEFAULT_VLAN);
-	spin_unlock(&enic->devcmd_lock);
-
-	return err;
-}
-
 static void enic_reset(struct work_struct *work)
 {
 	struct enic *enic = container_of(work, struct enic, reset);
@@ -2320,7 +2070,7 @@ static void enic_reset(struct work_struct *work)
 	enic_dev_hang_notify(enic);
 	enic_stop(enic->netdev);
 	enic_dev_hang_reset(enic);
-	enic_reset_multicast_list(enic);
+	enic_reset_addr_lists(enic);
 	enic_init_vnic_resources(enic);
 	enic_set_rss_nic_cfg(enic);
 	enic_dev_set_ig_vlan_rewrite_mode(enic);
@@ -2332,7 +2082,7 @@ static void enic_reset(struct work_struct *work)
 static int enic_set_intr_mode(struct enic *enic)
 {
 	unsigned int n = min_t(unsigned int, enic->rq_count, ENIC_RQ_MAX);
-	unsigned int m = 1;
+	unsigned int m = min_t(unsigned int, enic->wq_count, ENIC_WQ_MAX);
 	unsigned int i;
 
 	/* Set interrupt mode (INTx, MSI, MSI-X) depending
@@ -2475,9 +2225,7 @@ static const struct net_device_ops enic_netdev_dynamic_ops = {
 	.ndo_tx_timeout		= enic_tx_timeout,
 	.ndo_set_vf_port	= enic_set_vf_port,
 	.ndo_get_vf_port	= enic_get_vf_port,
-#ifdef IFLA_VF_MAX
 	.ndo_set_vf_mac		= enic_set_vf_mac,
-#endif
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= enic_poll_controller,
 #endif
@@ -2556,25 +2304,12 @@ static int enic_dev_init(struct enic *enic)
 
 	enic_init_vnic_resources(enic);
 
-	err = enic_set_rq_alloc_buf(enic);
-	if (err) {
-		dev_err(dev, "Failed to set RQ buffer allocator, aborting\n");
-		goto err_out_free_vnic_resources;
-	}
-
 	err = enic_set_rss_nic_cfg(enic);
 	if (err) {
 		dev_err(dev, "Failed to config nic, aborting\n");
 		goto err_out_free_vnic_resources;
 	}
 
-	err = enic_dev_set_ig_vlan_rewrite_mode(enic);
-	if (err) {
-		dev_err(dev,
-			"Failed to set ingress vlan rewrite mode, aborting.\n");
-		goto err_out_free_vnic_resources;
-	}
-
 	switch (vnic_dev_get_intr_mode(enic->vdev)) {
 	default:
 		netif_napi_add(netdev, &enic->napi[0], enic_poll, 64);
@@ -2713,6 +2448,22 @@ static int __devinit enic_probe(struct pci_dev *pdev,
 		goto err_out_vnic_unregister;
 	}
 
+	/* Setup devcmd lock
+	 */
+
+	spin_lock_init(&enic->devcmd_lock);
+
+	/*
+	 * Set ingress vlan rewrite mode before vnic initialization
+	 */
+
+	err = enic_dev_set_ig_vlan_rewrite_mode(enic);
+	if (err) {
+		dev_err(dev,
+			"Failed to set ingress vlan rewrite mode, aborting.\n");
+		goto err_out_dev_close;
+	}
+
 	/* Issue device init to initialize the vnic-to-switch link.
 	 * We'll start with carrier off and wait for link UP
 	 * notification later to turn on carrier.  We don't need
@@ -2736,11 +2487,6 @@ static int __devinit enic_probe(struct pci_dev *pdev,
 		}
 	}
 
-	/* Setup devcmd lock
-	 */
-
-	spin_lock_init(&enic->devcmd_lock);
-
 	err = enic_dev_init(enic);
 	if (err) {
 		dev_err(dev, "Device initialization failed, aborting\n");
diff --git a/drivers/net/enic/vnic_dev.c b/drivers/net/enic/vnic_dev.c
index fb35d8b17668..c489e72107de 100644
--- a/drivers/net/enic/vnic_dev.c
+++ b/drivers/net/enic/vnic_dev.c
@@ -419,25 +419,6 @@ int vnic_dev_fw_info(struct vnic_dev *vdev,
 	return err;
 }
 
-int vnic_dev_hw_version(struct vnic_dev *vdev, enum vnic_dev_hw_version *hw_ver)
-{
-	struct vnic_devcmd_fw_info *fw_info;
-	int err;
-
-	err = vnic_dev_fw_info(vdev, &fw_info);
-	if (err)
-		return err;
-
-	if (strncmp(fw_info->hw_version, "A1", sizeof("A1")) == 0)
-		*hw_ver = VNIC_DEV_HW_VER_A1;
-	else if (strncmp(fw_info->hw_version, "A2", sizeof("A2")) == 0)
-		*hw_ver = VNIC_DEV_HW_VER_A2;
-	else
-		*hw_ver = VNIC_DEV_HW_VER_UNKNOWN;
-
-	return 0;
-}
-
 int vnic_dev_spec(struct vnic_dev *vdev, unsigned int offset, unsigned int size,
 	void *value)
 {
diff --git a/drivers/net/enic/vnic_dev.h b/drivers/net/enic/vnic_dev.h
index 05f9a24cd459..e837546213a8 100644
--- a/drivers/net/enic/vnic_dev.h
+++ b/drivers/net/enic/vnic_dev.h
@@ -44,12 +44,6 @@ static inline void writeq(u64 val, void __iomem *reg)
 #undef pr_fmt
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-enum vnic_dev_hw_version {
-	VNIC_DEV_HW_VER_UNKNOWN,
-	VNIC_DEV_HW_VER_A1,
-	VNIC_DEV_HW_VER_A2,
-};
-
 enum vnic_dev_intr_mode {
 	VNIC_DEV_INTR_MODE_UNKNOWN,
 	VNIC_DEV_INTR_MODE_INTX,
@@ -93,8 +87,6 @@ int vnic_dev_cmd(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
 	u64 *a0, u64 *a1, int wait);
 int vnic_dev_fw_info(struct vnic_dev *vdev,
 	struct vnic_devcmd_fw_info **fw_info);
-int vnic_dev_hw_version(struct vnic_dev *vdev,
-	enum vnic_dev_hw_version *hw_ver);
 int vnic_dev_spec(struct vnic_dev *vdev, unsigned int offset, unsigned int size,
 	void *value);
 int vnic_dev_stats_dump(struct vnic_dev *vdev, struct vnic_stats **stats);
diff --git a/drivers/net/enic/vnic_rq.h b/drivers/net/enic/vnic_rq.h
index 37f08de2454a..2056586f4d4b 100644
--- a/drivers/net/enic/vnic_rq.h
+++ b/drivers/net/enic/vnic_rq.h
@@ -141,11 +141,6 @@ static inline void vnic_rq_post(struct vnic_rq *rq,
 	}
 }
 
-static inline int vnic_rq_posting_soon(struct vnic_rq *rq)
-{
-	return (rq->to_use->index & VNIC_RQ_RETURN_RATE) == 0;
-}
-
 static inline void vnic_rq_return_descs(struct vnic_rq *rq, unsigned int count)
 {
 	rq->ring.desc_avail += count;
diff --git a/drivers/net/eql.c b/drivers/net/eql.c
index 0cb1cf9cf4b0..a59cf961a436 100644
--- a/drivers/net/eql.c
+++ b/drivers/net/eql.c
@@ -111,6 +111,8 @@
  * Sorry, I had to rewrite most of this for 2.5.x -DaveM
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/capability.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -162,7 +164,7 @@ static void eql_timer(unsigned long param)
 }
 
 static const char version[] __initconst =
-	"Equalizer2002: Simon Janes (simon@ncm.com) and David S. Miller (davem@redhat.com)\n";
+	"Equalizer2002: Simon Janes (simon@ncm.com) and David S. Miller (davem@redhat.com)";
 
 static const struct net_device_ops eql_netdev_ops = {
 	.ndo_open	= eql_open,
@@ -204,8 +206,8 @@ static int eql_open(struct net_device *dev)
 	equalizer_t *eql = netdev_priv(dev);
 
 	/* XXX We should force this off automatically for the user. */
-	printk(KERN_INFO "%s: remember to turn off Van-Jacobson compression on "
-	       "your slave devices.\n", dev->name);
+	netdev_info(dev,
+		    "remember to turn off Van-Jacobson compression on your slave devices\n");
 
 	BUG_ON(!list_empty(&eql->queue.all_slaves));
 
@@ -591,7 +593,7 @@ static int __init eql_init_module(void)
 {
 	int err;
 
-	printk(version);
+	pr_info("%s\n", version);
 
 	dev_eql = alloc_netdev(sizeof(equalizer_t), "eql", eql_setup);
 	if (!dev_eql)
diff --git a/drivers/net/ethoc.c b/drivers/net/ethoc.c
index b79d7e1555d5..db0290f05bdf 100644
--- a/drivers/net/ethoc.c
+++ b/drivers/net/ethoc.c
@@ -1163,15 +1163,11 @@ static int ethoc_resume(struct platform_device *pdev)
 # define ethoc_resume  NULL
 #endif
 
-#ifdef CONFIG_OF
 static struct of_device_id ethoc_match[] = {
-	{
-		.compatible = "opencores,ethoc",
-	},
+	{ .compatible = "opencores,ethoc", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, ethoc_match);
-#endif
 
 static struct platform_driver ethoc_driver = {
 	.probe   = ethoc_probe,
@@ -1181,9 +1177,7 @@ static struct platform_driver ethoc_driver = {
 	.driver  = {
 		.name = "ethoc",
 		.owner = THIS_MODULE,
-#ifdef CONFIG_OF
 		.of_match_table = ethoc_match,
-#endif
 	},
 };
 
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 2a71373719ae..885d8baff7d5 100644
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -54,7 +54,7 @@
 
 #include "fec.h"
 
-#if defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28)
+#if defined(CONFIG_ARM)
 #define FEC_ALIGNMENT	0xf
 #else
 #define FEC_ALIGNMENT	0x3
@@ -74,7 +74,8 @@ static struct platform_device_id fec_devtype[] = {
 	}, {
 		.name = "imx28-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_SWAP_FRAME,
-	}
+	},
+	{ }
 };
 
 static unsigned char macaddr[ETH_ALEN];
@@ -147,8 +148,7 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
  * account when setting it.
  */
 #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)
 #define	OPT_FRAME_SIZE	(PKT_MAXBUF_SIZE << 16)
 #else
 #define	OPT_FRAME_SIZE	0
@@ -183,7 +183,7 @@ struct fec_enet_private {
 	struct bufdesc	*rx_bd_base;
 	struct bufdesc	*tx_bd_base;
 	/* The next free ring entry */
-	struct bufdesc	*cur_rx, *cur_tx; 
+	struct bufdesc	*cur_rx, *cur_tx;
 	/* The ring entries to be free()ed */
 	struct bufdesc	*dirty_tx;
 
@@ -191,28 +191,21 @@ struct fec_enet_private {
 	/* hold while accessing the HW like ringbuffer for tx/rx but not MAC */
 	spinlock_t hw_lock;
 
-	struct  platform_device *pdev;
+	struct	platform_device *pdev;
 
 	int	opened;
 
 	/* Phylib and MDIO interface */
-	struct  mii_bus *mii_bus;
-	struct  phy_device *phy_dev;
-	int     mii_timeout;
-	uint    phy_speed;
+	struct	mii_bus *mii_bus;
+	struct	phy_device *phy_dev;
+	int	mii_timeout;
+	uint	phy_speed;
 	phy_interface_t	phy_interface;
 	int	link;
 	int	full_duplex;
 	struct	completion mdio_done;
 };
 
-static irqreturn_t fec_enet_interrupt(int irq, void * dev_id);
-static void fec_enet_tx(struct net_device *dev);
-static void fec_enet_rx(struct net_device *dev);
-static int fec_enet_close(struct net_device *dev);
-static void fec_restart(struct net_device *dev, int duplex);
-static void fec_stop(struct net_device *dev);
-
 /* FEC MII MMFR bits definition */
 #define FEC_MMFR_ST		(1 << 30)
 #define FEC_MMFR_OP_READ	(2 << 28)
@@ -239,9 +232,9 @@ static void *swap_buffer(void *bufaddr, int len)
 }
 
 static netdev_tx_t
-fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
+fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
 	struct bufdesc *bdp;
@@ -262,9 +255,9 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (status & BD_ENET_TX_READY) {
 		/* Ooops.  All transmit buffers are full.  Bail out.
-		 * This should not happen, since dev->tbusy should be set.
+		 * This should not happen, since ndev->tbusy should be set.
 		 */
-		printk("%s: tx queue full!.\n", dev->name);
+		printk("%s: tx queue full!.\n", ndev->name);
 		spin_unlock_irqrestore(&fep->hw_lock, flags);
 		return NETDEV_TX_BUSY;
 	}
@@ -284,7 +277,7 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (((unsigned long) bufaddr) & FEC_ALIGNMENT) {
 		unsigned int index;
 		index = bdp - fep->tx_bd_base;
-		memcpy(fep->tx_bounce[index], (void *)skb->data, skb->len);
+		memcpy(fep->tx_bounce[index], skb->data, skb->len);
 		bufaddr = fep->tx_bounce[index];
 	}
 
@@ -299,13 +292,13 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* Save skb pointer */
 	fep->tx_skbuff[fep->skb_cur] = skb;
 
-	dev->stats.tx_bytes += skb->len;
+	ndev->stats.tx_bytes += skb->len;
 	fep->skb_cur = (fep->skb_cur+1) & TX_RING_MOD_MASK;
 
 	/* Push the data cache so the CPM does not get stale memory
 	 * data.
 	 */
-	bdp->cbd_bufaddr = dma_map_single(&dev->dev, bufaddr,
+	bdp->cbd_bufaddr = dma_map_single(&fep->pdev->dev, bufaddr,
 			FEC_ENET_TX_FRSIZE, DMA_TO_DEVICE);
 
 	/* Send it on its way.  Tell FEC it's ready, interrupt when done,
@@ -326,7 +319,7 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (bdp == fep->dirty_tx) {
 		fep->tx_full = 1;
-		netif_stop_queue(dev);
+		netif_stop_queue(ndev);
 	}
 
 	fep->cur_tx = bdp;
@@ -336,62 +329,170 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
+/* This function is called to start or restart the FEC during a link
+ * change.  This only happens when switching between half and full
+ * duplex.
+ */
 static void
-fec_timeout(struct net_device *dev)
+fec_restart(struct net_device *ndev, int duplex)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	const struct platform_device_id *id_entry =
+				platform_get_device_id(fep->pdev);
+	int i;
+	u32 temp_mac[2];
+	u32 rcntl = OPT_FRAME_SIZE | 0x04;
 
-	dev->stats.tx_errors++;
+	/* Whack a reset.  We should wait for this. */
+	writel(1, fep->hwp + FEC_ECNTRL);
+	udelay(10);
 
-	fec_restart(dev, fep->full_duplex);
-	netif_wake_queue(dev);
-}
+	/*
+	 * enet-mac reset will reset mac address registers too,
+	 * so need to reconfigure it.
+	 */
+	if (id_entry->driver_data & 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);
+	}
 
-static irqreturn_t
-fec_enet_interrupt(int irq, void * dev_id)
-{
-	struct	net_device *dev = dev_id;
-	struct fec_enet_private *fep = netdev_priv(dev);
-	uint	int_events;
-	irqreturn_t ret = IRQ_NONE;
+	/* Clear any outstanding interrupt. */
+	writel(0xffc00000, fep->hwp + FEC_IEVENT);
 
-	do {
-		int_events = readl(fep->hwp + FEC_IEVENT);
-		writel(int_events, fep->hwp + FEC_IEVENT);
+	/* Reset all multicast.	*/
+	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_HIGH);
+	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_LOW);
+#ifndef CONFIG_M5272
+	writel(0, fep->hwp + FEC_HASH_TABLE_HIGH);
+	writel(0, fep->hwp + FEC_HASH_TABLE_LOW);
+#endif
 
-		if (int_events & FEC_ENET_RXF) {
-			ret = IRQ_HANDLED;
-			fec_enet_rx(dev);
-		}
+	/* Set maximum receive buffer size. */
+	writel(PKT_MAXBLR_SIZE, fep->hwp + FEC_R_BUFF_SIZE);
 
-		/* Transmit OK, or non-fatal error. Update the buffer
-		 * descriptors. FEC handles all errors, we just discover
-		 * them as part of the transmit process.
-		 */
-		if (int_events & FEC_ENET_TXF) {
-			ret = IRQ_HANDLED;
-			fec_enet_tx(dev);
+	/* Set receive and transmit descriptor base. */
+	writel(fep->bd_dma, fep->hwp + FEC_R_DES_START);
+	writel((unsigned long)fep->bd_dma + sizeof(struct bufdesc) * RX_RING_SIZE,
+			fep->hwp + FEC_X_DES_START);
+
+	fep->dirty_tx = fep->cur_tx = fep->tx_bd_base;
+	fep->cur_rx = fep->rx_bd_base;
+
+	/* Reset SKB transmit buffers. */
+	fep->skb_cur = fep->skb_dirty = 0;
+	for (i = 0; i <= TX_RING_MOD_MASK; i++) {
+		if (fep->tx_skbuff[i]) {
+			dev_kfree_skb_any(fep->tx_skbuff[i]);
+			fep->tx_skbuff[i] = NULL;
 		}
+	}
 
-		if (int_events & FEC_ENET_MII) {
-			ret = IRQ_HANDLED;
-			complete(&fep->mdio_done);
+	/* Enable MII mode */
+	if (duplex) {
+		/* FD enable */
+		writel(0x04, fep->hwp + FEC_X_CNTRL);
+	} else {
+		/* No Rcv on Xmit */
+		rcntl |= 0x02;
+		writel(0x0, fep->hwp + FEC_X_CNTRL);
+	}
+
+	fep->full_duplex = duplex;
+
+	/* Set MII speed */
+	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
+
+	/*
+	 * The phy interface and speed need to get configured
+	 * differently on enet-mac.
+	 */
+	if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) {
+		/* Enable flow control and length check */
+		rcntl |= 0x40000000 | 0x00000020;
+
+		/* MII or RMII */
+		if (fep->phy_interface == PHY_INTERFACE_MODE_RMII)
+			rcntl |= (1 << 8);
+		else
+			rcntl &= ~(1 << 8);
+
+		/* 10M or 100M */
+		if (fep->phy_dev && fep->phy_dev->speed == SPEED_100)
+			rcntl &= ~(1 << 9);
+		else
+			rcntl |= (1 << 9);
+
+	} else {
+#ifdef FEC_MIIGSK_ENR
+		if (fep->phy_interface == PHY_INTERFACE_MODE_RMII) {
+			/* disable the gasket and wait */
+			writel(0, fep->hwp + FEC_MIIGSK_ENR);
+			while (readl(fep->hwp + FEC_MIIGSK_ENR) & 4)
+				udelay(1);
+
+			/*
+			 * configure the gasket:
+			 *   RMII, 50 MHz, no loopback, no echo
+			 */
+			writel(1, fep->hwp + FEC_MIIGSK_CFGR);
+
+			/* re-enable the gasket */
+			writel(2, fep->hwp + FEC_MIIGSK_ENR);
 		}
-	} while (int_events);
+#endif
+	}
+	writel(rcntl, fep->hwp + FEC_R_CNTRL);
 
-	return ret;
+	/* And last, enable the transmit and receive processing */
+	writel(2, fep->hwp + FEC_ECNTRL);
+	writel(0, fep->hwp + FEC_R_DES_ACTIVE);
+
+	/* Enable interrupts we wish to service */
+	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 }
 
+static void
+fec_stop(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+
+	/* We cannot expect a graceful transmit stop without link !!! */
+	if (fep->link) {
+		writel(1, fep->hwp + FEC_X_CNTRL); /* Graceful transmit stop */
+		udelay(10);
+		if (!(readl(fep->hwp + FEC_IEVENT) & FEC_ENET_GRA))
+			printk("fec_stop : Graceful transmit stop did not complete !\n");
+	}
+
+	/* Whack a reset.  We should wait for this. */
+	writel(1, fep->hwp + FEC_ECNTRL);
+	udelay(10);
+	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
+	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
+}
+
+
+static void
+fec_timeout(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+
+	ndev->stats.tx_errors++;
+
+	fec_restart(ndev, fep->full_duplex);
+	netif_wake_queue(ndev);
+}
 
 static void
-fec_enet_tx(struct net_device *dev)
+fec_enet_tx(struct net_device *ndev)
 {
 	struct	fec_enet_private *fep;
 	struct bufdesc *bdp;
 	unsigned short status;
 	struct	sk_buff	*skb;
 
-	fep = netdev_priv(dev);
+	fep = netdev_priv(ndev);
 	spin_lock(&fep->hw_lock);
 	bdp = fep->dirty_tx;
 
@@ -399,7 +500,8 @@ fec_enet_tx(struct net_device *dev)
 		if (bdp == fep->cur_tx && fep->tx_full == 0)
 			break;
 
-		dma_unmap_single(&dev->dev, bdp->cbd_bufaddr, FEC_ENET_TX_FRSIZE, DMA_TO_DEVICE);
+		dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
+				FEC_ENET_TX_FRSIZE, DMA_TO_DEVICE);
 		bdp->cbd_bufaddr = 0;
 
 		skb = fep->tx_skbuff[fep->skb_dirty];
@@ -407,19 +509,19 @@ fec_enet_tx(struct net_device *dev)
 		if (status & (BD_ENET_TX_HB | BD_ENET_TX_LC |
 				   BD_ENET_TX_RL | BD_ENET_TX_UN |
 				   BD_ENET_TX_CSL)) {
-			dev->stats.tx_errors++;
+			ndev->stats.tx_errors++;
 			if (status & BD_ENET_TX_HB)  /* No heartbeat */
-				dev->stats.tx_heartbeat_errors++;
+				ndev->stats.tx_heartbeat_errors++;
 			if (status & BD_ENET_TX_LC)  /* Late collision */
-				dev->stats.tx_window_errors++;
+				ndev->stats.tx_window_errors++;
 			if (status & BD_ENET_TX_RL)  /* Retrans limit */
-				dev->stats.tx_aborted_errors++;
+				ndev->stats.tx_aborted_errors++;
 			if (status & BD_ENET_TX_UN)  /* Underrun */
-				dev->stats.tx_fifo_errors++;
+				ndev->stats.tx_fifo_errors++;
 			if (status & BD_ENET_TX_CSL) /* Carrier lost */
-				dev->stats.tx_carrier_errors++;
+				ndev->stats.tx_carrier_errors++;
 		} else {
-			dev->stats.tx_packets++;
+			ndev->stats.tx_packets++;
 		}
 
 		if (status & BD_ENET_TX_READY)
@@ -429,7 +531,7 @@ fec_enet_tx(struct net_device *dev)
 		 * but we eventually sent the packet OK.
 		 */
 		if (status & BD_ENET_TX_DEF)
-			dev->stats.collisions++;
+			ndev->stats.collisions++;
 
 		/* Free the sk buffer associated with this last transmit */
 		dev_kfree_skb_any(skb);
@@ -446,8 +548,8 @@ fec_enet_tx(struct net_device *dev)
 		 */
 		if (fep->tx_full) {
 			fep->tx_full = 0;
-			if (netif_queue_stopped(dev))
-				netif_wake_queue(dev);
+			if (netif_queue_stopped(ndev))
+				netif_wake_queue(ndev);
 		}
 	}
 	fep->dirty_tx = bdp;
@@ -461,9 +563,9 @@ fec_enet_tx(struct net_device *dev)
  * effectively tossing the packet.
  */
 static void
-fec_enet_rx(struct net_device *dev)
+fec_enet_rx(struct net_device *ndev)
 {
-	struct	fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
 	struct bufdesc *bdp;
@@ -497,17 +599,17 @@ fec_enet_rx(struct net_device *dev)
 		/* Check for errors. */
 		if (status & (BD_ENET_RX_LG | BD_ENET_RX_SH | BD_ENET_RX_NO |
 			   BD_ENET_RX_CR | BD_ENET_RX_OV)) {
-			dev->stats.rx_errors++;
+			ndev->stats.rx_errors++;
 			if (status & (BD_ENET_RX_LG | BD_ENET_RX_SH)) {
 				/* Frame too long or too short. */
-				dev->stats.rx_length_errors++;
+				ndev->stats.rx_length_errors++;
 			}
 			if (status & BD_ENET_RX_NO)	/* Frame alignment */
-				dev->stats.rx_frame_errors++;
+				ndev->stats.rx_frame_errors++;
 			if (status & BD_ENET_RX_CR)	/* CRC Error */
-				dev->stats.rx_crc_errors++;
+				ndev->stats.rx_crc_errors++;
 			if (status & BD_ENET_RX_OV)	/* FIFO overrun */
-				dev->stats.rx_fifo_errors++;
+				ndev->stats.rx_fifo_errors++;
 		}
 
 		/* Report late collisions as a frame error.
@@ -515,19 +617,19 @@ fec_enet_rx(struct net_device *dev)
 		 * have in the buffer.  So, just drop this frame on the floor.
 		 */
 		if (status & BD_ENET_RX_CL) {
-			dev->stats.rx_errors++;
-			dev->stats.rx_frame_errors++;
+			ndev->stats.rx_errors++;
+			ndev->stats.rx_frame_errors++;
 			goto rx_processing_done;
 		}
 
 		/* Process the incoming frame. */
-		dev->stats.rx_packets++;
+		ndev->stats.rx_packets++;
 		pkt_len = bdp->cbd_datlen;
-		dev->stats.rx_bytes += pkt_len;
+		ndev->stats.rx_bytes += pkt_len;
 		data = (__u8*)__va(bdp->cbd_bufaddr);
 
-	        dma_unmap_single(NULL, bdp->cbd_bufaddr, bdp->cbd_datlen,
-        			DMA_FROM_DEVICE);
+		dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
+				FEC_ENET_TX_FRSIZE, DMA_FROM_DEVICE);
 
 		if (id_entry->driver_data & FEC_QUIRK_SWAP_FRAME)
 			swap_buffer(data, pkt_len);
@@ -541,18 +643,18 @@ fec_enet_rx(struct net_device *dev)
 
 		if (unlikely(!skb)) {
 			printk("%s: Memory squeeze, dropping packet.\n",
-					dev->name);
-			dev->stats.rx_dropped++;
+					ndev->name);
+			ndev->stats.rx_dropped++;
 		} else {
 			skb_reserve(skb, NET_IP_ALIGN);
 			skb_put(skb, pkt_len - 4);	/* Make room */
 			skb_copy_to_linear_data(skb, data, pkt_len - 4);
-			skb->protocol = eth_type_trans(skb, dev);
+			skb->protocol = eth_type_trans(skb, ndev);
 			netif_rx(skb);
 		}
 
-        	bdp->cbd_bufaddr = dma_map_single(NULL, data, bdp->cbd_datlen,
-			DMA_FROM_DEVICE);
+		bdp->cbd_bufaddr = dma_map_single(&fep->pdev->dev, data,
+				FEC_ENET_TX_FRSIZE, DMA_FROM_DEVICE);
 rx_processing_done:
 		/* Clear the status flags for this buffer */
 		status &= ~BD_ENET_RX_STATS;
@@ -577,10 +679,47 @@ rx_processing_done:
 	spin_unlock(&fep->hw_lock);
 }
 
+static irqreturn_t
+fec_enet_interrupt(int irq, void *dev_id)
+{
+	struct net_device *ndev = dev_id;
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	uint int_events;
+	irqreturn_t ret = IRQ_NONE;
+
+	do {
+		int_events = readl(fep->hwp + FEC_IEVENT);
+		writel(int_events, fep->hwp + FEC_IEVENT);
+
+		if (int_events & FEC_ENET_RXF) {
+			ret = IRQ_HANDLED;
+			fec_enet_rx(ndev);
+		}
+
+		/* Transmit OK, or non-fatal error. Update the buffer
+		 * descriptors. FEC handles all errors, we just discover
+		 * them as part of the transmit process.
+		 */
+		if (int_events & FEC_ENET_TXF) {
+			ret = IRQ_HANDLED;
+			fec_enet_tx(ndev);
+		}
+
+		if (int_events & FEC_ENET_MII) {
+			ret = IRQ_HANDLED;
+			complete(&fep->mdio_done);
+		}
+	} while (int_events);
+
+	return ret;
+}
+
+
+
 /* ------------------------------------------------------------------------- */
-static void __inline__ fec_get_mac(struct net_device *dev)
+static void __inline__ fec_get_mac(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct fec_platform_data *pdata = fep->pdev->dev.platform_data;
 	unsigned char *iap, tmpaddr[ETH_ALEN];
 
@@ -616,11 +755,11 @@ static void __inline__ fec_get_mac(struct net_device *dev)
 		iap = &tmpaddr[0];
 	}
 
-	memcpy(dev->dev_addr, iap, ETH_ALEN);
+	memcpy(ndev->dev_addr, iap, ETH_ALEN);
 
 	/* Adjust MAC if using macaddr */
 	if (iap == macaddr)
-		 dev->dev_addr[ETH_ALEN-1] = macaddr[ETH_ALEN-1] + fep->pdev->id;
+		 ndev->dev_addr[ETH_ALEN-1] = macaddr[ETH_ALEN-1] + fep->pdev->id;
 }
 
 /* ------------------------------------------------------------------------- */
@@ -628,9 +767,9 @@ static void __inline__ fec_get_mac(struct net_device *dev)
 /*
  * Phy section
  */
-static void fec_enet_adjust_link(struct net_device *dev)
+static void fec_enet_adjust_link(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct phy_device *phy_dev = fep->phy_dev;
 	unsigned long flags;
 
@@ -647,7 +786,7 @@ static void fec_enet_adjust_link(struct net_device *dev)
 	/* Duplex link change */
 	if (phy_dev->link) {
 		if (fep->full_duplex != phy_dev->duplex) {
-			fec_restart(dev, phy_dev->duplex);
+			fec_restart(ndev, phy_dev->duplex);
 			status_change = 1;
 		}
 	}
@@ -656,9 +795,9 @@ static void fec_enet_adjust_link(struct net_device *dev)
 	if (phy_dev->link != fep->link) {
 		fep->link = phy_dev->link;
 		if (phy_dev->link)
-			fec_restart(dev, phy_dev->duplex);
+			fec_restart(ndev, phy_dev->duplex);
 		else
-			fec_stop(dev);
+			fec_stop(ndev);
 		status_change = 1;
 	}
 
@@ -727,9 +866,9 @@ static int fec_enet_mdio_reset(struct mii_bus *bus)
 	return 0;
 }
 
-static int fec_enet_mii_probe(struct net_device *dev)
+static int fec_enet_mii_probe(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct phy_device *phy_dev = NULL;
 	char mdio_bus_id[MII_BUS_ID_SIZE];
 	char phy_name[MII_BUS_ID_SIZE + 3];
@@ -754,16 +893,16 @@ static int fec_enet_mii_probe(struct net_device *dev)
 
 	if (phy_id >= PHY_MAX_ADDR) {
 		printk(KERN_INFO "%s: no PHY, assuming direct connection "
-			"to switch\n", dev->name);
+			"to switch\n", ndev->name);
 		strncpy(mdio_bus_id, "0", MII_BUS_ID_SIZE);
 		phy_id = 0;
 	}
 
 	snprintf(phy_name, MII_BUS_ID_SIZE, PHY_ID_FMT, mdio_bus_id, phy_id);
-	phy_dev = phy_connect(dev, phy_name, &fec_enet_adjust_link, 0,
+	phy_dev = phy_connect(ndev, phy_name, &fec_enet_adjust_link, 0,
 		PHY_INTERFACE_MODE_MII);
 	if (IS_ERR(phy_dev)) {
-		printk(KERN_ERR "%s: could not attach to PHY\n", dev->name);
+		printk(KERN_ERR "%s: could not attach to PHY\n", ndev->name);
 		return PTR_ERR(phy_dev);
 	}
 
@@ -776,7 +915,7 @@ static int fec_enet_mii_probe(struct net_device *dev)
 	fep->full_duplex = 0;
 
 	printk(KERN_INFO "%s: Freescale FEC PHY driver [%s] "
-		"(mii_bus:phy_addr=%s, irq=%d)\n", dev->name,
+		"(mii_bus:phy_addr=%s, irq=%d)\n", ndev->name,
 		fep->phy_dev->drv->name, dev_name(&fep->phy_dev->dev),
 		fep->phy_dev->irq);
 
@@ -786,8 +925,8 @@ static int fec_enet_mii_probe(struct net_device *dev)
 static int fec_enet_mii_init(struct platform_device *pdev)
 {
 	static struct mii_bus *fec0_mii_bus;
-	struct net_device *dev = platform_get_drvdata(pdev);
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
 	int err = -ENXIO, i;
@@ -845,8 +984,6 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	for (i = 0; i < PHY_MAX_ADDR; i++)
 		fep->mii_bus->irq[i] = PHY_POLL;
 
-	platform_set_drvdata(dev, fep->mii_bus);
-
 	if (mdiobus_register(fep->mii_bus))
 		goto err_out_free_mdio_irq;
 
@@ -873,10 +1010,10 @@ static void fec_enet_mii_remove(struct fec_enet_private *fep)
 	mdiobus_free(fep->mii_bus);
 }
 
-static int fec_enet_get_settings(struct net_device *dev,
+static int fec_enet_get_settings(struct net_device *ndev,
 				  struct ethtool_cmd *cmd)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct phy_device *phydev = fep->phy_dev;
 
 	if (!phydev)
@@ -885,10 +1022,10 @@ static int fec_enet_get_settings(struct net_device *dev,
 	return phy_ethtool_gset(phydev, cmd);
 }
 
-static int fec_enet_set_settings(struct net_device *dev,
+static int fec_enet_set_settings(struct net_device *ndev,
 				 struct ethtool_cmd *cmd)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct phy_device *phydev = fep->phy_dev;
 
 	if (!phydev)
@@ -897,14 +1034,14 @@ static int fec_enet_set_settings(struct net_device *dev,
 	return phy_ethtool_sset(phydev, cmd);
 }
 
-static void fec_enet_get_drvinfo(struct net_device *dev,
+static void fec_enet_get_drvinfo(struct net_device *ndev,
 				 struct ethtool_drvinfo *info)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 
 	strcpy(info->driver, fep->pdev->dev.driver->name);
 	strcpy(info->version, "Revision: 1.0");
-	strcpy(info->bus_info, dev_name(&dev->dev));
+	strcpy(info->bus_info, dev_name(&ndev->dev));
 }
 
 static struct ethtool_ops fec_enet_ethtool_ops = {
@@ -914,12 +1051,12 @@ static struct ethtool_ops fec_enet_ethtool_ops = {
 	.get_link		= ethtool_op_get_link,
 };
 
-static int fec_enet_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct phy_device *phydev = fep->phy_dev;
 
-	if (!netif_running(dev))
+	if (!netif_running(ndev))
 		return -EINVAL;
 
 	if (!phydev)
@@ -928,9 +1065,9 @@ static int fec_enet_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	return phy_mii_ioctl(phydev, rq, cmd);
 }
 
-static void fec_enet_free_buffers(struct net_device *dev)
+static void fec_enet_free_buffers(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	int i;
 	struct sk_buff *skb;
 	struct bufdesc	*bdp;
@@ -940,7 +1077,7 @@ static void fec_enet_free_buffers(struct net_device *dev)
 		skb = fep->rx_skbuff[i];
 
 		if (bdp->cbd_bufaddr)
-			dma_unmap_single(&dev->dev, bdp->cbd_bufaddr,
+			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
 					FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE);
 		if (skb)
 			dev_kfree_skb(skb);
@@ -952,9 +1089,9 @@ static void fec_enet_free_buffers(struct net_device *dev)
 		kfree(fep->tx_bounce[i]);
 }
 
-static int fec_enet_alloc_buffers(struct net_device *dev)
+static int fec_enet_alloc_buffers(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	int i;
 	struct sk_buff *skb;
 	struct bufdesc	*bdp;
@@ -963,12 +1100,12 @@ static int fec_enet_alloc_buffers(struct net_device *dev)
 	for (i = 0; i < RX_RING_SIZE; i++) {
 		skb = dev_alloc_skb(FEC_ENET_RX_FRSIZE);
 		if (!skb) {
-			fec_enet_free_buffers(dev);
+			fec_enet_free_buffers(ndev);
 			return -ENOMEM;
 		}
 		fep->rx_skbuff[i] = skb;
 
-		bdp->cbd_bufaddr = dma_map_single(&dev->dev, skb->data,
+		bdp->cbd_bufaddr = dma_map_single(&fep->pdev->dev, skb->data,
 				FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE);
 		bdp->cbd_sc = BD_ENET_RX_EMPTY;
 		bdp++;
@@ -995,45 +1132,47 @@ static int fec_enet_alloc_buffers(struct net_device *dev)
 }
 
 static int
-fec_enet_open(struct net_device *dev)
+fec_enet_open(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	int ret;
 
 	/* I should reset the ring buffers here, but I don't yet know
 	 * a simple way to do that.
 	 */
 
-	ret = fec_enet_alloc_buffers(dev);
+	ret = fec_enet_alloc_buffers(ndev);
 	if (ret)
 		return ret;
 
 	/* Probe and connect to PHY when open the interface */
-	ret = fec_enet_mii_probe(dev);
+	ret = fec_enet_mii_probe(ndev);
 	if (ret) {
-		fec_enet_free_buffers(dev);
+		fec_enet_free_buffers(ndev);
 		return ret;
 	}
 	phy_start(fep->phy_dev);
-	netif_start_queue(dev);
+	netif_start_queue(ndev);
 	fep->opened = 1;
 	return 0;
 }
 
 static int
-fec_enet_close(struct net_device *dev)
+fec_enet_close(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 
 	/* Don't know what to do yet. */
 	fep->opened = 0;
-	netif_stop_queue(dev);
-	fec_stop(dev);
+	netif_stop_queue(ndev);
+	fec_stop(ndev);
 
-	if (fep->phy_dev)
+	if (fep->phy_dev) {
+		phy_stop(fep->phy_dev);
 		phy_disconnect(fep->phy_dev);
+	}
 
-        fec_enet_free_buffers(dev);
+	fec_enet_free_buffers(ndev);
 
 	return 0;
 }
@@ -1051,14 +1190,14 @@ fec_enet_close(struct net_device *dev)
 #define HASH_BITS	6		/* #bits in hash */
 #define CRC32_POLY	0xEDB88320
 
-static void set_multicast_list(struct net_device *dev)
+static void set_multicast_list(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct netdev_hw_addr *ha;
 	unsigned int i, bit, data, crc, tmp;
 	unsigned char hash;
 
-	if (dev->flags & IFF_PROMISC) {
+	if (ndev->flags & IFF_PROMISC) {
 		tmp = readl(fep->hwp + FEC_R_CNTRL);
 		tmp |= 0x8;
 		writel(tmp, fep->hwp + FEC_R_CNTRL);
@@ -1069,7 +1208,7 @@ static void set_multicast_list(struct net_device *dev)
 	tmp &= ~0x8;
 	writel(tmp, fep->hwp + FEC_R_CNTRL);
 
-	if (dev->flags & IFF_ALLMULTI) {
+	if (ndev->flags & IFF_ALLMULTI) {
 		/* Catch all multicast addresses, so set the
 		 * filter to all 1's
 		 */
@@ -1084,7 +1223,7 @@ static void set_multicast_list(struct net_device *dev)
 	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_HIGH);
 	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_LOW);
 
-	netdev_for_each_mc_addr(ha, dev) {
+	netdev_for_each_mc_addr(ha, ndev) {
 		/* Only support group multicast for now */
 		if (!(ha->addr[0] & 1))
 			continue;
@@ -1092,7 +1231,7 @@ static void set_multicast_list(struct net_device *dev)
 		/* calculate crc32 value of mac address */
 		crc = 0xffffffff;
 
-		for (i = 0; i < dev->addr_len; i++) {
+		for (i = 0; i < ndev->addr_len; i++) {
 			data = ha->addr[i];
 			for (bit = 0; bit < 8; bit++, data >>= 1) {
 				crc = (crc >> 1) ^
@@ -1119,20 +1258,20 @@ static void set_multicast_list(struct net_device *dev)
 
 /* Set a MAC change in hardware. */
 static int
-fec_set_mac_address(struct net_device *dev, void *p)
+fec_set_mac_address(struct net_device *ndev, void *p)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct sockaddr *addr = p;
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
-	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
+	memcpy(ndev->dev_addr, addr->sa_data, ndev->addr_len);
 
-	writel(dev->dev_addr[3] | (dev->dev_addr[2] << 8) |
-		(dev->dev_addr[1] << 16) | (dev->dev_addr[0] << 24),
+	writel(ndev->dev_addr[3] | (ndev->dev_addr[2] << 8) |
+		(ndev->dev_addr[1] << 16) | (ndev->dev_addr[0] << 24),
 		fep->hwp + FEC_ADDR_LOW);
-	writel((dev->dev_addr[5] << 16) | (dev->dev_addr[4] << 24),
+	writel((ndev->dev_addr[5] << 16) | (ndev->dev_addr[4] << 24),
 		fep->hwp + FEC_ADDR_HIGH);
 	return 0;
 }
@@ -1146,16 +1285,16 @@ static const struct net_device_ops fec_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_tx_timeout		= fec_timeout,
 	.ndo_set_mac_address	= fec_set_mac_address,
-	.ndo_do_ioctl           = fec_enet_ioctl,
+	.ndo_do_ioctl		= fec_enet_ioctl,
 };
 
  /*
   * XXX:  We need to clean up on failure exits here.
   *
   */
-static int fec_enet_init(struct net_device *dev)
+static int fec_enet_init(struct net_device *ndev)
 {
-	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct bufdesc *cbd_base;
 	struct bufdesc *bdp;
 	int i;
@@ -1170,20 +1309,19 @@ static int fec_enet_init(struct net_device *dev)
 
 	spin_lock_init(&fep->hw_lock);
 
-	fep->hwp = (void __iomem *)dev->base_addr;
-	fep->netdev = dev;
+	fep->netdev = ndev;
 
 	/* Get the Ethernet address */
-	fec_get_mac(dev);
+	fec_get_mac(ndev);
 
 	/* Set receive and transmit descriptor base. */
 	fep->rx_bd_base = cbd_base;
 	fep->tx_bd_base = cbd_base + RX_RING_SIZE;
 
 	/* The FEC Ethernet specific entries in the device structure */
-	dev->watchdog_timeo = TX_TIMEOUT;
-	dev->netdev_ops = &fec_netdev_ops;
-	dev->ethtool_ops = &fec_enet_ethtool_ops;
+	ndev->watchdog_timeo = TX_TIMEOUT;
+	ndev->netdev_ops = &fec_netdev_ops;
+	ndev->ethtool_ops = &fec_enet_ethtool_ops;
 
 	/* Initialize the receive buffer descriptors. */
 	bdp = fep->rx_bd_base;
@@ -1212,152 +1350,11 @@ static int fec_enet_init(struct net_device *dev)
 	bdp--;
 	bdp->cbd_sc |= BD_SC_WRAP;
 
-	fec_restart(dev, 0);
+	fec_restart(ndev, 0);
 
 	return 0;
 }
 
-/* This function is called to start or restart the FEC during a link
- * change.  This only happens when switching between half and full
- * duplex.
- */
-static void
-fec_restart(struct net_device *dev, int duplex)
-{
-	struct fec_enet_private *fep = netdev_priv(dev);
-	const struct platform_device_id *id_entry =
-				platform_get_device_id(fep->pdev);
-	int i;
-	u32 val, temp_mac[2];
-
-	/* Whack a reset.  We should wait for this. */
-	writel(1, fep->hwp + FEC_ECNTRL);
-	udelay(10);
-
-	/*
-	 * enet-mac reset will reset mac address registers too,
-	 * so need to reconfigure it.
-	 */
-	if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) {
-		memcpy(&temp_mac, dev->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);
-	}
-
-	/* Clear any outstanding interrupt. */
-	writel(0xffc00000, fep->hwp + FEC_IEVENT);
-
-	/* Reset all multicast.	*/
-	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_HIGH);
-	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_LOW);
-#ifndef CONFIG_M5272
-	writel(0, fep->hwp + FEC_HASH_TABLE_HIGH);
-	writel(0, fep->hwp + FEC_HASH_TABLE_LOW);
-#endif
-
-	/* Set maximum receive buffer size. */
-	writel(PKT_MAXBLR_SIZE, fep->hwp + FEC_R_BUFF_SIZE);
-
-	/* Set receive and transmit descriptor base. */
-	writel(fep->bd_dma, fep->hwp + FEC_R_DES_START);
-	writel((unsigned long)fep->bd_dma + sizeof(struct bufdesc) * RX_RING_SIZE,
-			fep->hwp + FEC_X_DES_START);
-
-	fep->dirty_tx = fep->cur_tx = fep->tx_bd_base;
-	fep->cur_rx = fep->rx_bd_base;
-
-	/* Reset SKB transmit buffers. */
-	fep->skb_cur = fep->skb_dirty = 0;
-	for (i = 0; i <= TX_RING_MOD_MASK; i++) {
-		if (fep->tx_skbuff[i]) {
-			dev_kfree_skb_any(fep->tx_skbuff[i]);
-			fep->tx_skbuff[i] = NULL;
-		}
-	}
-
-	/* Enable MII mode */
-	if (duplex) {
-		/* MII enable / FD enable */
-		writel(OPT_FRAME_SIZE | 0x04, fep->hwp + FEC_R_CNTRL);
-		writel(0x04, fep->hwp + FEC_X_CNTRL);
-	} else {
-		/* MII enable / No Rcv on Xmit */
-		writel(OPT_FRAME_SIZE | 0x06, fep->hwp + FEC_R_CNTRL);
-		writel(0x0, fep->hwp + FEC_X_CNTRL);
-	}
-	fep->full_duplex = duplex;
-
-	/* Set MII speed */
-	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
-
-	/*
-	 * The phy interface and speed need to get configured
-	 * differently on enet-mac.
-	 */
-	if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) {
-		val = readl(fep->hwp + FEC_R_CNTRL);
-
-		/* MII or RMII */
-		if (fep->phy_interface == PHY_INTERFACE_MODE_RMII)
-			val |= (1 << 8);
-		else
-			val &= ~(1 << 8);
-
-		/* 10M or 100M */
-		if (fep->phy_dev && fep->phy_dev->speed == SPEED_100)
-			val &= ~(1 << 9);
-		else
-			val |= (1 << 9);
-
-		writel(val, fep->hwp + FEC_R_CNTRL);
-	} else {
-#ifdef FEC_MIIGSK_ENR
-		if (fep->phy_interface == PHY_INTERFACE_MODE_RMII) {
-			/* disable the gasket and wait */
-			writel(0, fep->hwp + FEC_MIIGSK_ENR);
-			while (readl(fep->hwp + FEC_MIIGSK_ENR) & 4)
-				udelay(1);
-
-			/*
-			 * configure the gasket:
-			 *   RMII, 50 MHz, no loopback, no echo
-			 */
-			writel(1, fep->hwp + FEC_MIIGSK_CFGR);
-
-			/* re-enable the gasket */
-			writel(2, fep->hwp + FEC_MIIGSK_ENR);
-		}
-#endif
-	}
-
-	/* And last, enable the transmit and receive processing */
-	writel(2, fep->hwp + FEC_ECNTRL);
-	writel(0, fep->hwp + FEC_R_DES_ACTIVE);
-
-	/* Enable interrupts we wish to service */
-	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
-}
-
-static void
-fec_stop(struct net_device *dev)
-{
-	struct fec_enet_private *fep = netdev_priv(dev);
-
-	/* We cannot expect a graceful transmit stop without link !!! */
-	if (fep->link) {
-		writel(1, fep->hwp + FEC_X_CNTRL); /* Graceful transmit stop */
-		udelay(10);
-		if (!(readl(fep->hwp + FEC_IEVENT) & FEC_ENET_GRA))
-			printk("fec_stop : Graceful transmit stop did not complete !\n");
-	}
-
-	/* Whack a reset.  We should wait for this. */
-	writel(1, fep->hwp + FEC_ECNTRL);
-	udelay(10);
-	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
-	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
-}
-
 static int __devinit
 fec_probe(struct platform_device *pdev)
 {
@@ -1377,19 +1374,20 @@ fec_probe(struct platform_device *pdev)
 
 	/* Init network device */
 	ndev = alloc_etherdev(sizeof(struct fec_enet_private));
-	if (!ndev)
-		return -ENOMEM;
+	if (!ndev) {
+		ret = -ENOMEM;
+		goto failed_alloc_etherdev;
+	}
 
 	SET_NETDEV_DEV(ndev, &pdev->dev);
 
 	/* setup board info structure */
 	fep = netdev_priv(ndev);
-	memset(fep, 0, sizeof(*fep));
 
-	ndev->base_addr = (unsigned long)ioremap(r->start, resource_size(r));
+	fep->hwp = ioremap(r->start, resource_size(r));
 	fep->pdev = pdev;
 
-	if (!ndev->base_addr) {
+	if (!fep->hwp) {
 		ret = -ENOMEM;
 		goto failed_ioremap;
 	}
@@ -1407,10 +1405,9 @@ fec_probe(struct platform_device *pdev)
 			break;
 		ret = request_irq(irq, fec_enet_interrupt, IRQF_DISABLED, pdev->name, ndev);
 		if (ret) {
-			while (i >= 0) {
+			while (--i >= 0) {
 				irq = platform_get_irq(pdev, i);
 				free_irq(irq, ndev);
-				i--;
 			}
 			goto failed_irq;
 		}
@@ -1453,9 +1450,11 @@ failed_clk:
 			free_irq(irq, ndev);
 	}
 failed_irq:
-	iounmap((void __iomem *)ndev->base_addr);
+	iounmap(fep->hwp);
 failed_ioremap:
 	free_netdev(ndev);
+failed_alloc_etherdev:
+	release_mem_region(r->start, resource_size(r));
 
 	return ret;
 }
@@ -1465,16 +1464,22 @@ fec_drv_remove(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
-
-	platform_set_drvdata(pdev, NULL);
+	struct resource *r;
 
 	fec_stop(ndev);
 	fec_enet_mii_remove(fep);
 	clk_disable(fep->clk);
 	clk_put(fep->clk);
-	iounmap((void __iomem *)ndev->base_addr);
+	iounmap(fep->hwp);
 	unregister_netdev(ndev);
 	free_netdev(ndev);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	BUG_ON(!r);
+	release_mem_region(r->start, resource_size(r));
+
+	platform_set_drvdata(pdev, NULL);
+
 	return 0;
 }
 
@@ -1483,16 +1488,14 @@ static int
 fec_suspend(struct device *dev)
 {
 	struct net_device *ndev = dev_get_drvdata(dev);
-	struct fec_enet_private *fep;
+	struct fec_enet_private *fep = netdev_priv(ndev);
 
-	if (ndev) {
-		fep = netdev_priv(ndev);
-		if (netif_running(ndev)) {
-			fec_stop(ndev);
-			netif_device_detach(ndev);
-		}
-		clk_disable(fep->clk);
+	if (netif_running(ndev)) {
+		fec_stop(ndev);
+		netif_device_detach(ndev);
 	}
+	clk_disable(fep->clk);
+
 	return 0;
 }
 
@@ -1500,16 +1503,14 @@ static int
 fec_resume(struct device *dev)
 {
 	struct net_device *ndev = dev_get_drvdata(dev);
-	struct fec_enet_private *fep;
+	struct fec_enet_private *fep = netdev_priv(ndev);
 
-	if (ndev) {
-		fep = netdev_priv(ndev);
-		clk_enable(fep->clk);
-		if (netif_running(ndev)) {
-			fec_restart(ndev, fep->full_duplex);
-			netif_device_attach(ndev);
-		}
+	clk_enable(fep->clk);
+	if (netif_running(ndev)) {
+		fec_restart(ndev, fep->full_duplex);
+		netif_device_attach(ndev);
 	}
+
 	return 0;
 }
 
diff --git a/drivers/net/fec_mpc52xx.c b/drivers/net/fec_mpc52xx.c
index 50c1213f61fe..9f81b1ac130e 100644
--- a/drivers/net/fec_mpc52xx.c
+++ b/drivers/net/fec_mpc52xx.c
@@ -840,8 +840,7 @@ static const struct net_device_ops mpc52xx_fec_netdev_ops = {
 /* OF Driver                                                                */
 /* ======================================================================== */
 
-static int __devinit
-mpc52xx_fec_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit mpc52xx_fec_probe(struct platform_device *op)
 {
 	int rv;
 	struct net_device *ndev;
@@ -1049,7 +1048,7 @@ static struct of_device_id mpc52xx_fec_match[] = {
 
 MODULE_DEVICE_TABLE(of, mpc52xx_fec_match);
 
-static struct of_platform_driver mpc52xx_fec_driver = {
+static struct platform_driver mpc52xx_fec_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -1073,21 +1072,21 @@ mpc52xx_fec_init(void)
 {
 #ifdef CONFIG_FEC_MPC52xx_MDIO
 	int ret;
-	ret = of_register_platform_driver(&mpc52xx_fec_mdio_driver);
+	ret = platform_driver_register(&mpc52xx_fec_mdio_driver);
 	if (ret) {
 		printk(KERN_ERR DRIVER_NAME ": failed to register mdio driver\n");
 		return ret;
 	}
 #endif
-	return of_register_platform_driver(&mpc52xx_fec_driver);
+	return platform_driver_register(&mpc52xx_fec_driver);
 }
 
 static void __exit
 mpc52xx_fec_exit(void)
 {
-	of_unregister_platform_driver(&mpc52xx_fec_driver);
+	platform_driver_unregister(&mpc52xx_fec_driver);
 #ifdef CONFIG_FEC_MPC52xx_MDIO
-	of_unregister_platform_driver(&mpc52xx_fec_mdio_driver);
+	platform_driver_unregister(&mpc52xx_fec_mdio_driver);
 #endif
 }
 
diff --git a/drivers/net/fec_mpc52xx.h b/drivers/net/fec_mpc52xx.h
index a227a525bdbb..41d2dffde55b 100644
--- a/drivers/net/fec_mpc52xx.h
+++ b/drivers/net/fec_mpc52xx.h
@@ -289,6 +289,6 @@ struct mpc52xx_fec {
 #define FEC_XMIT_FSM_ENABLE_CRC		0x01000000
 
 
-extern struct of_platform_driver mpc52xx_fec_mdio_driver;
+extern struct platform_driver mpc52xx_fec_mdio_driver;
 
 #endif	/* __DRIVERS_NET_MPC52XX_FEC_H__ */
diff --git a/drivers/net/fec_mpc52xx_phy.c b/drivers/net/fec_mpc52xx_phy.c
index 0b4cb6f15984..360a578c2bb7 100644
--- a/drivers/net/fec_mpc52xx_phy.c
+++ b/drivers/net/fec_mpc52xx_phy.c
@@ -61,8 +61,7 @@ static int mpc52xx_fec_mdio_write(struct mii_bus *bus, int phy_id, int reg,
 		data | FEC_MII_WRITE_FRAME);
 }
 
-static int mpc52xx_fec_mdio_probe(struct platform_device *of,
-		const struct of_device_id *match)
+static int mpc52xx_fec_mdio_probe(struct platform_device *of)
 {
 	struct device *dev = &of->dev;
 	struct device_node *np = of->dev.of_node;
@@ -145,7 +144,7 @@ static struct of_device_id mpc52xx_fec_mdio_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mpc52xx_fec_mdio_match);
 
-struct of_platform_driver mpc52xx_fec_mdio_driver = {
+struct platform_driver mpc52xx_fec_mdio_driver = {
 	.driver = {
 		.name = "mpc5200b-fec-phy",
 		.owner = THIS_MODULE,
diff --git a/drivers/net/fs_enet/fs_enet-main.c b/drivers/net/fs_enet/fs_enet-main.c
index 7a1f3d0ffa78..24cb953900dd 100644
--- a/drivers/net/fs_enet/fs_enet-main.c
+++ b/drivers/net/fs_enet/fs_enet-main.c
@@ -998,8 +998,7 @@ static const struct net_device_ops fs_enet_netdev_ops = {
 #endif
 };
 
-static int __devinit fs_enet_probe(struct platform_device *ofdev,
-                                   const struct of_device_id *match)
+static int __devinit fs_enet_probe(struct platform_device *ofdev)
 {
 	struct net_device *ndev;
 	struct fs_enet_private *fep;
@@ -1008,11 +1007,14 @@ static int __devinit fs_enet_probe(struct platform_device *ofdev,
 	const u8 *mac_addr;
 	int privsize, len, ret = -ENODEV;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+
 	fpi = kzalloc(sizeof(*fpi), GFP_KERNEL);
 	if (!fpi)
 		return -ENOMEM;
 
-	if (!IS_FEC(match)) {
+	if (!IS_FEC(ofdev->dev.of_match)) {
 		data = of_get_property(ofdev->dev.of_node, "fsl,cpm-command", &len);
 		if (!data || len != 4)
 			goto out_free_fpi;
@@ -1047,7 +1049,7 @@ static int __devinit fs_enet_probe(struct platform_device *ofdev,
 	fep->dev = &ofdev->dev;
 	fep->ndev = ndev;
 	fep->fpi = fpi;
-	fep->ops = match->data;
+	fep->ops = ofdev->dev.of_match->data;
 
 	ret = fep->ops->setup_data(ndev);
 	if (ret)
@@ -1156,7 +1158,7 @@ static struct of_device_id fs_enet_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fs_enet_match);
 
-static struct of_platform_driver fs_enet_driver = {
+static struct platform_driver fs_enet_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = "fs_enet",
@@ -1168,12 +1170,12 @@ static struct of_platform_driver fs_enet_driver = {
 
 static int __init fs_init(void)
 {
-	return of_register_platform_driver(&fs_enet_driver);
+	return platform_driver_register(&fs_enet_driver);
 }
 
 static void __exit fs_cleanup(void)
 {
-	of_unregister_platform_driver(&fs_enet_driver);
+	platform_driver_unregister(&fs_enet_driver);
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
diff --git a/drivers/net/fs_enet/mii-bitbang.c b/drivers/net/fs_enet/mii-bitbang.c
index 3cda2b515471..ad2975440719 100644
--- a/drivers/net/fs_enet/mii-bitbang.c
+++ b/drivers/net/fs_enet/mii-bitbang.c
@@ -150,8 +150,7 @@ static int __devinit fs_mii_bitbang_init(struct mii_bus *bus,
 	return 0;
 }
 
-static int __devinit fs_enet_mdio_probe(struct platform_device *ofdev,
-                                        const struct of_device_id *match)
+static int __devinit fs_enet_mdio_probe(struct platform_device *ofdev)
 {
 	struct mii_bus *new_bus;
 	struct bb_info *bitbang;
@@ -223,7 +222,7 @@ static struct of_device_id fs_enet_mdio_bb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fs_enet_mdio_bb_match);
 
-static struct of_platform_driver fs_enet_bb_mdio_driver = {
+static struct platform_driver fs_enet_bb_mdio_driver = {
 	.driver = {
 		.name = "fsl-bb-mdio",
 		.owner = THIS_MODULE,
@@ -235,12 +234,12 @@ static struct of_platform_driver fs_enet_bb_mdio_driver = {
 
 static int fs_enet_mdio_bb_init(void)
 {
-	return of_register_platform_driver(&fs_enet_bb_mdio_driver);
+	return platform_driver_register(&fs_enet_bb_mdio_driver);
 }
 
 static void fs_enet_mdio_bb_exit(void)
 {
-	of_unregister_platform_driver(&fs_enet_bb_mdio_driver);
+	platform_driver_unregister(&fs_enet_bb_mdio_driver);
 }
 
 module_init(fs_enet_mdio_bb_init);
diff --git a/drivers/net/fs_enet/mii-fec.c b/drivers/net/fs_enet/mii-fec.c
index dbb9c48623df..7e840d373ab3 100644
--- a/drivers/net/fs_enet/mii-fec.c
+++ b/drivers/net/fs_enet/mii-fec.c
@@ -101,15 +101,18 @@ static int fs_enet_fec_mii_reset(struct mii_bus *bus)
 	return 0;
 }
 
-static int __devinit fs_enet_mdio_probe(struct platform_device *ofdev,
-                                        const struct of_device_id *match)
+static int __devinit fs_enet_mdio_probe(struct platform_device *ofdev)
 {
 	struct resource res;
 	struct mii_bus *new_bus;
 	struct fec_info *fec;
-	int (*get_bus_freq)(struct device_node *) = match->data;
+	int (*get_bus_freq)(struct device_node *);
 	int ret = -ENOMEM, clock, speed;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+	get_bus_freq = ofdev->dev.of_match->data;
+
 	new_bus = mdiobus_alloc();
 	if (!new_bus)
 		goto out;
@@ -221,7 +224,7 @@ static struct of_device_id fs_enet_mdio_fec_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fs_enet_mdio_fec_match);
 
-static struct of_platform_driver fs_enet_fec_mdio_driver = {
+static struct platform_driver fs_enet_fec_mdio_driver = {
 	.driver = {
 		.name = "fsl-fec-mdio",
 		.owner = THIS_MODULE,
@@ -233,12 +236,12 @@ static struct of_platform_driver fs_enet_fec_mdio_driver = {
 
 static int fs_enet_mdio_fec_init(void)
 {
-	return of_register_platform_driver(&fs_enet_fec_mdio_driver);
+	return platform_driver_register(&fs_enet_fec_mdio_driver);
 }
 
 static void fs_enet_mdio_fec_exit(void)
 {
-	of_unregister_platform_driver(&fs_enet_fec_mdio_driver);
+	platform_driver_unregister(&fs_enet_fec_mdio_driver);
 }
 
 module_init(fs_enet_mdio_fec_init);
diff --git a/drivers/net/fsl_pq_mdio.c b/drivers/net/fsl_pq_mdio.c
index 8d3a2ccbc953..52f4e8ad48e7 100644
--- a/drivers/net/fsl_pq_mdio.c
+++ b/drivers/net/fsl_pq_mdio.c
@@ -265,8 +265,7 @@ static int get_ucc_id_for_range(u64 start, u64 end, u32 *ucc_id)
 #endif
 
 
-static int fsl_pq_mdio_probe(struct platform_device *ofdev,
-		const struct of_device_id *match)
+static int fsl_pq_mdio_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct device_node *tbi;
@@ -471,7 +470,7 @@ static struct of_device_id fsl_pq_mdio_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fsl_pq_mdio_match);
 
-static struct of_platform_driver fsl_pq_mdio_driver = {
+static struct platform_driver fsl_pq_mdio_driver = {
 	.driver = {
 		.name = "fsl-pq_mdio",
 		.owner = THIS_MODULE,
@@ -483,13 +482,13 @@ static struct of_platform_driver fsl_pq_mdio_driver = {
 
 int __init fsl_pq_mdio_init(void)
 {
-	return of_register_platform_driver(&fsl_pq_mdio_driver);
+	return platform_driver_register(&fsl_pq_mdio_driver);
 }
 module_init(fsl_pq_mdio_init);
 
 void fsl_pq_mdio_exit(void)
 {
-	of_unregister_platform_driver(&fsl_pq_mdio_driver);
+	platform_driver_unregister(&fsl_pq_mdio_driver);
 }
 module_exit(fsl_pq_mdio_exit);
 MODULE_LICENSE("GPL");
diff --git a/drivers/net/ftmac100.c b/drivers/net/ftmac100.c
new file mode 100644
index 000000000000..df70368bf317
--- /dev/null
+++ b/drivers/net/ftmac100.c
@@ -0,0 +1,1196 @@
+/*
+ * Faraday FTMAC100 10/100 Ethernet
+ *
+ * (C) Copyright 2009-2011 Faraday Technology
+ * Po-Yu Chuang <ratbert@faraday-tech.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/dma-mapping.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/mii.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/platform_device.h>
+
+#include "ftmac100.h"
+
+#define DRV_NAME	"ftmac100"
+#define DRV_VERSION	"0.2"
+
+#define RX_QUEUE_ENTRIES	128	/* must be power of 2 */
+#define TX_QUEUE_ENTRIES	16	/* must be power of 2 */
+
+#define MAX_PKT_SIZE		1518
+#define RX_BUF_SIZE		2044	/* must be smaller than 0x7ff */
+
+#if MAX_PKT_SIZE > 0x7ff
+#error invalid MAX_PKT_SIZE
+#endif
+
+#if RX_BUF_SIZE > 0x7ff || RX_BUF_SIZE > PAGE_SIZE
+#error invalid RX_BUF_SIZE
+#endif
+
+/******************************************************************************
+ * private data
+ *****************************************************************************/
+struct ftmac100_descs {
+	struct ftmac100_rxdes rxdes[RX_QUEUE_ENTRIES];
+	struct ftmac100_txdes txdes[TX_QUEUE_ENTRIES];
+};
+
+struct ftmac100 {
+	struct resource *res;
+	void __iomem *base;
+	int irq;
+
+	struct ftmac100_descs *descs;
+	dma_addr_t descs_dma_addr;
+
+	unsigned int rx_pointer;
+	unsigned int tx_clean_pointer;
+	unsigned int tx_pointer;
+	unsigned int tx_pending;
+
+	spinlock_t tx_lock;
+
+	struct net_device *netdev;
+	struct device *dev;
+	struct napi_struct napi;
+
+	struct mii_if_info mii;
+};
+
+static int ftmac100_alloc_rx_page(struct ftmac100 *priv, struct ftmac100_rxdes *rxdes);
+
+/******************************************************************************
+ * internal functions (hardware register access)
+ *****************************************************************************/
+#define INT_MASK_ALL_ENABLED	(FTMAC100_INT_RPKT_FINISH	| \
+				 FTMAC100_INT_NORXBUF		| \
+				 FTMAC100_INT_XPKT_OK		| \
+				 FTMAC100_INT_XPKT_LOST		| \
+				 FTMAC100_INT_RPKT_LOST		| \
+				 FTMAC100_INT_AHB_ERR		| \
+				 FTMAC100_INT_PHYSTS_CHG)
+
+#define INT_MASK_ALL_DISABLED	0
+
+static void ftmac100_enable_all_int(struct ftmac100 *priv)
+{
+	iowrite32(INT_MASK_ALL_ENABLED, priv->base + FTMAC100_OFFSET_IMR);
+}
+
+static void ftmac100_disable_all_int(struct ftmac100 *priv)
+{
+	iowrite32(INT_MASK_ALL_DISABLED, priv->base + FTMAC100_OFFSET_IMR);
+}
+
+static void ftmac100_set_rx_ring_base(struct ftmac100 *priv, dma_addr_t addr)
+{
+	iowrite32(addr, priv->base + FTMAC100_OFFSET_RXR_BADR);
+}
+
+static void ftmac100_set_tx_ring_base(struct ftmac100 *priv, dma_addr_t addr)
+{
+	iowrite32(addr, priv->base + FTMAC100_OFFSET_TXR_BADR);
+}
+
+static void ftmac100_txdma_start_polling(struct ftmac100 *priv)
+{
+	iowrite32(1, priv->base + FTMAC100_OFFSET_TXPD);
+}
+
+static int ftmac100_reset(struct ftmac100 *priv)
+{
+	struct net_device *netdev = priv->netdev;
+	int i;
+
+	/* NOTE: reset clears all registers */
+	iowrite32(FTMAC100_MACCR_SW_RST, priv->base + FTMAC100_OFFSET_MACCR);
+
+	for (i = 0; i < 5; i++) {
+		unsigned int maccr;
+
+		maccr = ioread32(priv->base + FTMAC100_OFFSET_MACCR);
+		if (!(maccr & FTMAC100_MACCR_SW_RST)) {
+			/*
+			 * FTMAC100_MACCR_SW_RST cleared does not indicate
+			 * that hardware reset completed (what the f*ck).
+			 * We still need to wait for a while.
+			 */
+			usleep_range(500, 1000);
+			return 0;
+		}
+
+		usleep_range(1000, 10000);
+	}
+
+	netdev_err(netdev, "software reset failed\n");
+	return -EIO;
+}
+
+static void ftmac100_set_mac(struct ftmac100 *priv, const unsigned char *mac)
+{
+	unsigned int maddr = mac[0] << 8 | mac[1];
+	unsigned int laddr = mac[2] << 24 | mac[3] << 16 | mac[4] << 8 | mac[5];
+
+	iowrite32(maddr, priv->base + FTMAC100_OFFSET_MAC_MADR);
+	iowrite32(laddr, priv->base + FTMAC100_OFFSET_MAC_LADR);
+}
+
+#define MACCR_ENABLE_ALL	(FTMAC100_MACCR_XMT_EN	| \
+				 FTMAC100_MACCR_RCV_EN	| \
+				 FTMAC100_MACCR_XDMA_EN	| \
+				 FTMAC100_MACCR_RDMA_EN	| \
+				 FTMAC100_MACCR_CRC_APD	| \
+				 FTMAC100_MACCR_FULLDUP	| \
+				 FTMAC100_MACCR_RX_RUNT	| \
+				 FTMAC100_MACCR_RX_BROADPKT)
+
+static int ftmac100_start_hw(struct ftmac100 *priv)
+{
+	struct net_device *netdev = priv->netdev;
+
+	if (ftmac100_reset(priv))
+		return -EIO;
+
+	/* setup ring buffer base registers */
+	ftmac100_set_rx_ring_base(priv,
+				  priv->descs_dma_addr +
+				  offsetof(struct ftmac100_descs, rxdes));
+	ftmac100_set_tx_ring_base(priv,
+				  priv->descs_dma_addr +
+				  offsetof(struct ftmac100_descs, txdes));
+
+	iowrite32(FTMAC100_APTC_RXPOLL_CNT(1), priv->base + FTMAC100_OFFSET_APTC);
+
+	ftmac100_set_mac(priv, netdev->dev_addr);
+
+	iowrite32(MACCR_ENABLE_ALL, priv->base + FTMAC100_OFFSET_MACCR);
+	return 0;
+}
+
+static void ftmac100_stop_hw(struct ftmac100 *priv)
+{
+	iowrite32(0, priv->base + FTMAC100_OFFSET_MACCR);
+}
+
+/******************************************************************************
+ * internal functions (receive descriptor)
+ *****************************************************************************/
+static bool ftmac100_rxdes_first_segment(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_FRS);
+}
+
+static bool ftmac100_rxdes_last_segment(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_LRS);
+}
+
+static bool ftmac100_rxdes_owned_by_dma(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_RXDMA_OWN);
+}
+
+static void ftmac100_rxdes_set_dma_own(struct ftmac100_rxdes *rxdes)
+{
+	/* clear status bits */
+	rxdes->rxdes0 = cpu_to_le32(FTMAC100_RXDES0_RXDMA_OWN);
+}
+
+static bool ftmac100_rxdes_rx_error(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_RX_ERR);
+}
+
+static bool ftmac100_rxdes_crc_error(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_CRC_ERR);
+}
+
+static bool ftmac100_rxdes_frame_too_long(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_FTL);
+}
+
+static bool ftmac100_rxdes_runt(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_RUNT);
+}
+
+static bool ftmac100_rxdes_odd_nibble(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_RX_ODD_NB);
+}
+
+static unsigned int ftmac100_rxdes_frame_length(struct ftmac100_rxdes *rxdes)
+{
+	return le32_to_cpu(rxdes->rxdes0) & FTMAC100_RXDES0_RFL;
+}
+
+static bool ftmac100_rxdes_multicast(struct ftmac100_rxdes *rxdes)
+{
+	return rxdes->rxdes0 & cpu_to_le32(FTMAC100_RXDES0_MULTICAST);
+}
+
+static void ftmac100_rxdes_set_buffer_size(struct ftmac100_rxdes *rxdes,
+					   unsigned int size)
+{
+	rxdes->rxdes1 &= cpu_to_le32(FTMAC100_RXDES1_EDORR);
+	rxdes->rxdes1 |= cpu_to_le32(FTMAC100_RXDES1_RXBUF_SIZE(size));
+}
+
+static void ftmac100_rxdes_set_end_of_ring(struct ftmac100_rxdes *rxdes)
+{
+	rxdes->rxdes1 |= cpu_to_le32(FTMAC100_RXDES1_EDORR);
+}
+
+static void ftmac100_rxdes_set_dma_addr(struct ftmac100_rxdes *rxdes,
+					dma_addr_t addr)
+{
+	rxdes->rxdes2 = cpu_to_le32(addr);
+}
+
+static dma_addr_t ftmac100_rxdes_get_dma_addr(struct ftmac100_rxdes *rxdes)
+{
+	return le32_to_cpu(rxdes->rxdes2);
+}
+
+/*
+ * rxdes3 is not used by hardware. We use it to keep track of page.
+ * Since hardware does not touch it, we can skip cpu_to_le32()/le32_to_cpu().
+ */
+static void ftmac100_rxdes_set_page(struct ftmac100_rxdes *rxdes, struct page *page)
+{
+	rxdes->rxdes3 = (unsigned int)page;
+}
+
+static struct page *ftmac100_rxdes_get_page(struct ftmac100_rxdes *rxdes)
+{
+	return (struct page *)rxdes->rxdes3;
+}
+
+/******************************************************************************
+ * internal functions (receive)
+ *****************************************************************************/
+static int ftmac100_next_rx_pointer(int pointer)
+{
+	return (pointer + 1) & (RX_QUEUE_ENTRIES - 1);
+}
+
+static void ftmac100_rx_pointer_advance(struct ftmac100 *priv)
+{
+	priv->rx_pointer = ftmac100_next_rx_pointer(priv->rx_pointer);
+}
+
+static struct ftmac100_rxdes *ftmac100_current_rxdes(struct ftmac100 *priv)
+{
+	return &priv->descs->rxdes[priv->rx_pointer];
+}
+
+static struct ftmac100_rxdes *
+ftmac100_rx_locate_first_segment(struct ftmac100 *priv)
+{
+	struct ftmac100_rxdes *rxdes = ftmac100_current_rxdes(priv);
+
+	while (!ftmac100_rxdes_owned_by_dma(rxdes)) {
+		if (ftmac100_rxdes_first_segment(rxdes))
+			return rxdes;
+
+		ftmac100_rxdes_set_dma_own(rxdes);
+		ftmac100_rx_pointer_advance(priv);
+		rxdes = ftmac100_current_rxdes(priv);
+	}
+
+	return NULL;
+}
+
+static bool ftmac100_rx_packet_error(struct ftmac100 *priv,
+				     struct ftmac100_rxdes *rxdes)
+{
+	struct net_device *netdev = priv->netdev;
+	bool error = false;
+
+	if (unlikely(ftmac100_rxdes_rx_error(rxdes))) {
+		if (net_ratelimit())
+			netdev_info(netdev, "rx err\n");
+
+		netdev->stats.rx_errors++;
+		error = true;
+	}
+
+	if (unlikely(ftmac100_rxdes_crc_error(rxdes))) {
+		if (net_ratelimit())
+			netdev_info(netdev, "rx crc err\n");
+
+		netdev->stats.rx_crc_errors++;
+		error = true;
+	}
+
+	if (unlikely(ftmac100_rxdes_frame_too_long(rxdes))) {
+		if (net_ratelimit())
+			netdev_info(netdev, "rx frame too long\n");
+
+		netdev->stats.rx_length_errors++;
+		error = true;
+	} else if (unlikely(ftmac100_rxdes_runt(rxdes))) {
+		if (net_ratelimit())
+			netdev_info(netdev, "rx runt\n");
+
+		netdev->stats.rx_length_errors++;
+		error = true;
+	} else if (unlikely(ftmac100_rxdes_odd_nibble(rxdes))) {
+		if (net_ratelimit())
+			netdev_info(netdev, "rx odd nibble\n");
+
+		netdev->stats.rx_length_errors++;
+		error = true;
+	}
+
+	return error;
+}
+
+static void ftmac100_rx_drop_packet(struct ftmac100 *priv)
+{
+	struct net_device *netdev = priv->netdev;
+	struct ftmac100_rxdes *rxdes = ftmac100_current_rxdes(priv);
+	bool done = false;
+
+	if (net_ratelimit())
+		netdev_dbg(netdev, "drop packet %p\n", rxdes);
+
+	do {
+		if (ftmac100_rxdes_last_segment(rxdes))
+			done = true;
+
+		ftmac100_rxdes_set_dma_own(rxdes);
+		ftmac100_rx_pointer_advance(priv);
+		rxdes = ftmac100_current_rxdes(priv);
+	} while (!done && !ftmac100_rxdes_owned_by_dma(rxdes));
+
+	netdev->stats.rx_dropped++;
+}
+
+static bool ftmac100_rx_packet(struct ftmac100 *priv, int *processed)
+{
+	struct net_device *netdev = priv->netdev;
+	struct ftmac100_rxdes *rxdes;
+	struct sk_buff *skb;
+	struct page *page;
+	dma_addr_t map;
+	int length;
+
+	rxdes = ftmac100_rx_locate_first_segment(priv);
+	if (!rxdes)
+		return false;
+
+	if (unlikely(ftmac100_rx_packet_error(priv, rxdes))) {
+		ftmac100_rx_drop_packet(priv);
+		return true;
+	}
+
+	/*
+	 * It is impossible to get multi-segment packets
+	 * because we always provide big enough receive buffers.
+	 */
+	if (unlikely(!ftmac100_rxdes_last_segment(rxdes)))
+		BUG();
+
+	/* start processing */
+	skb = netdev_alloc_skb_ip_align(netdev, 128);
+	if (unlikely(!skb)) {
+		if (net_ratelimit())
+			netdev_err(netdev, "rx skb alloc failed\n");
+
+		ftmac100_rx_drop_packet(priv);
+		return true;
+	}
+
+	if (unlikely(ftmac100_rxdes_multicast(rxdes)))
+		netdev->stats.multicast++;
+
+	map = ftmac100_rxdes_get_dma_addr(rxdes);
+	dma_unmap_page(priv->dev, map, RX_BUF_SIZE, DMA_FROM_DEVICE);
+
+	length = ftmac100_rxdes_frame_length(rxdes);
+	page = ftmac100_rxdes_get_page(rxdes);
+	skb_fill_page_desc(skb, 0, page, 0, length);
+	skb->len += length;
+	skb->data_len += length;
+	skb->truesize += length;
+	__pskb_pull_tail(skb, min(length, 64));
+
+	ftmac100_alloc_rx_page(priv, rxdes);
+
+	ftmac100_rx_pointer_advance(priv);
+
+	skb->protocol = eth_type_trans(skb, netdev);
+
+	netdev->stats.rx_packets++;
+	netdev->stats.rx_bytes += skb->len;
+
+	/* push packet to protocol stack */
+	netif_receive_skb(skb);
+
+	(*processed)++;
+	return true;
+}
+
+/******************************************************************************
+ * internal functions (transmit descriptor)
+ *****************************************************************************/
+static void ftmac100_txdes_reset(struct ftmac100_txdes *txdes)
+{
+	/* clear all except end of ring bit */
+	txdes->txdes0 = 0;
+	txdes->txdes1 &= cpu_to_le32(FTMAC100_TXDES1_EDOTR);
+	txdes->txdes2 = 0;
+	txdes->txdes3 = 0;
+}
+
+static bool ftmac100_txdes_owned_by_dma(struct ftmac100_txdes *txdes)
+{
+	return txdes->txdes0 & cpu_to_le32(FTMAC100_TXDES0_TXDMA_OWN);
+}
+
+static void ftmac100_txdes_set_dma_own(struct ftmac100_txdes *txdes)
+{
+	/*
+	 * Make sure dma own bit will not be set before any other
+	 * descriptor fields.
+	 */
+	wmb();
+	txdes->txdes0 |= cpu_to_le32(FTMAC100_TXDES0_TXDMA_OWN);
+}
+
+static bool ftmac100_txdes_excessive_collision(struct ftmac100_txdes *txdes)
+{
+	return txdes->txdes0 & cpu_to_le32(FTMAC100_TXDES0_TXPKT_EXSCOL);
+}
+
+static bool ftmac100_txdes_late_collision(struct ftmac100_txdes *txdes)
+{
+	return txdes->txdes0 & cpu_to_le32(FTMAC100_TXDES0_TXPKT_LATECOL);
+}
+
+static void ftmac100_txdes_set_end_of_ring(struct ftmac100_txdes *txdes)
+{
+	txdes->txdes1 |= cpu_to_le32(FTMAC100_TXDES1_EDOTR);
+}
+
+static void ftmac100_txdes_set_first_segment(struct ftmac100_txdes *txdes)
+{
+	txdes->txdes1 |= cpu_to_le32(FTMAC100_TXDES1_FTS);
+}
+
+static void ftmac100_txdes_set_last_segment(struct ftmac100_txdes *txdes)
+{
+	txdes->txdes1 |= cpu_to_le32(FTMAC100_TXDES1_LTS);
+}
+
+static void ftmac100_txdes_set_txint(struct ftmac100_txdes *txdes)
+{
+	txdes->txdes1 |= cpu_to_le32(FTMAC100_TXDES1_TXIC);
+}
+
+static void ftmac100_txdes_set_buffer_size(struct ftmac100_txdes *txdes,
+					   unsigned int len)
+{
+	txdes->txdes1 |= cpu_to_le32(FTMAC100_TXDES1_TXBUF_SIZE(len));
+}
+
+static void ftmac100_txdes_set_dma_addr(struct ftmac100_txdes *txdes,
+					dma_addr_t addr)
+{
+	txdes->txdes2 = cpu_to_le32(addr);
+}
+
+static dma_addr_t ftmac100_txdes_get_dma_addr(struct ftmac100_txdes *txdes)
+{
+	return le32_to_cpu(txdes->txdes2);
+}
+
+/*
+ * txdes3 is not used by hardware. We use it to keep track of socket buffer.
+ * Since hardware does not touch it, we can skip cpu_to_le32()/le32_to_cpu().
+ */
+static void ftmac100_txdes_set_skb(struct ftmac100_txdes *txdes, struct sk_buff *skb)
+{
+	txdes->txdes3 = (unsigned int)skb;
+}
+
+static struct sk_buff *ftmac100_txdes_get_skb(struct ftmac100_txdes *txdes)
+{
+	return (struct sk_buff *)txdes->txdes3;
+}
+
+/******************************************************************************
+ * internal functions (transmit)
+ *****************************************************************************/
+static int ftmac100_next_tx_pointer(int pointer)
+{
+	return (pointer + 1) & (TX_QUEUE_ENTRIES - 1);
+}
+
+static void ftmac100_tx_pointer_advance(struct ftmac100 *priv)
+{
+	priv->tx_pointer = ftmac100_next_tx_pointer(priv->tx_pointer);
+}
+
+static void ftmac100_tx_clean_pointer_advance(struct ftmac100 *priv)
+{
+	priv->tx_clean_pointer = ftmac100_next_tx_pointer(priv->tx_clean_pointer);
+}
+
+static struct ftmac100_txdes *ftmac100_current_txdes(struct ftmac100 *priv)
+{
+	return &priv->descs->txdes[priv->tx_pointer];
+}
+
+static struct ftmac100_txdes *ftmac100_current_clean_txdes(struct ftmac100 *priv)
+{
+	return &priv->descs->txdes[priv->tx_clean_pointer];
+}
+
+static bool ftmac100_tx_complete_packet(struct ftmac100 *priv)
+{
+	struct net_device *netdev = priv->netdev;
+	struct ftmac100_txdes *txdes;
+	struct sk_buff *skb;
+	dma_addr_t map;
+
+	if (priv->tx_pending == 0)
+		return false;
+
+	txdes = ftmac100_current_clean_txdes(priv);
+
+	if (ftmac100_txdes_owned_by_dma(txdes))
+		return false;
+
+	skb = ftmac100_txdes_get_skb(txdes);
+	map = ftmac100_txdes_get_dma_addr(txdes);
+
+	if (unlikely(ftmac100_txdes_excessive_collision(txdes) ||
+		     ftmac100_txdes_late_collision(txdes))) {
+		/*
+		 * packet transmitted to ethernet lost due to late collision
+		 * or excessive collision
+		 */
+		netdev->stats.tx_aborted_errors++;
+	} else {
+		netdev->stats.tx_packets++;
+		netdev->stats.tx_bytes += skb->len;
+	}
+
+	dma_unmap_single(priv->dev, map, skb_headlen(skb), DMA_TO_DEVICE);
+	dev_kfree_skb(skb);
+
+	ftmac100_txdes_reset(txdes);
+
+	ftmac100_tx_clean_pointer_advance(priv);
+
+	spin_lock(&priv->tx_lock);
+	priv->tx_pending--;
+	spin_unlock(&priv->tx_lock);
+	netif_wake_queue(netdev);
+
+	return true;
+}
+
+static void ftmac100_tx_complete(struct ftmac100 *priv)
+{
+	while (ftmac100_tx_complete_packet(priv))
+		;
+}
+
+static int ftmac100_xmit(struct ftmac100 *priv, struct sk_buff *skb,
+			 dma_addr_t map)
+{
+	struct net_device *netdev = priv->netdev;
+	struct ftmac100_txdes *txdes;
+	unsigned int len = (skb->len < ETH_ZLEN) ? ETH_ZLEN : skb->len;
+
+	txdes = ftmac100_current_txdes(priv);
+	ftmac100_tx_pointer_advance(priv);
+
+	/* setup TX descriptor */
+	ftmac100_txdes_set_skb(txdes, skb);
+	ftmac100_txdes_set_dma_addr(txdes, map);
+
+	ftmac100_txdes_set_first_segment(txdes);
+	ftmac100_txdes_set_last_segment(txdes);
+	ftmac100_txdes_set_txint(txdes);
+	ftmac100_txdes_set_buffer_size(txdes, len);
+
+	spin_lock(&priv->tx_lock);
+	priv->tx_pending++;
+	if (priv->tx_pending == TX_QUEUE_ENTRIES)
+		netif_stop_queue(netdev);
+
+	/* start transmit */
+	ftmac100_txdes_set_dma_own(txdes);
+	spin_unlock(&priv->tx_lock);
+
+	ftmac100_txdma_start_polling(priv);
+	return NETDEV_TX_OK;
+}
+
+/******************************************************************************
+ * internal functions (buffer)
+ *****************************************************************************/
+static int ftmac100_alloc_rx_page(struct ftmac100 *priv, struct ftmac100_rxdes *rxdes)
+{
+	struct net_device *netdev = priv->netdev;
+	struct page *page;
+	dma_addr_t map;
+
+	page = alloc_page(GFP_KERNEL);
+	if (!page) {
+		if (net_ratelimit())
+			netdev_err(netdev, "failed to allocate rx page\n");
+		return -ENOMEM;
+	}
+
+	map = dma_map_page(priv->dev, page, 0, RX_BUF_SIZE, DMA_FROM_DEVICE);
+	if (unlikely(dma_mapping_error(priv->dev, map))) {
+		if (net_ratelimit())
+			netdev_err(netdev, "failed to map rx page\n");
+		__free_page(page);
+		return -ENOMEM;
+	}
+
+	ftmac100_rxdes_set_page(rxdes, page);
+	ftmac100_rxdes_set_dma_addr(rxdes, map);
+	ftmac100_rxdes_set_buffer_size(rxdes, RX_BUF_SIZE);
+	ftmac100_rxdes_set_dma_own(rxdes);
+	return 0;
+}
+
+static void ftmac100_free_buffers(struct ftmac100 *priv)
+{
+	int i;
+
+	for (i = 0; i < RX_QUEUE_ENTRIES; i++) {
+		struct ftmac100_rxdes *rxdes = &priv->descs->rxdes[i];
+		struct page *page = ftmac100_rxdes_get_page(rxdes);
+		dma_addr_t map = ftmac100_rxdes_get_dma_addr(rxdes);
+
+		if (!page)
+			continue;
+
+		dma_unmap_page(priv->dev, map, RX_BUF_SIZE, DMA_FROM_DEVICE);
+		__free_page(page);
+	}
+
+	for (i = 0; i < TX_QUEUE_ENTRIES; i++) {
+		struct ftmac100_txdes *txdes = &priv->descs->txdes[i];
+		struct sk_buff *skb = ftmac100_txdes_get_skb(txdes);
+		dma_addr_t map = ftmac100_txdes_get_dma_addr(txdes);
+
+		if (!skb)
+			continue;
+
+		dma_unmap_single(priv->dev, map, skb_headlen(skb), DMA_TO_DEVICE);
+		dev_kfree_skb(skb);
+	}
+
+	dma_free_coherent(priv->dev, sizeof(struct ftmac100_descs),
+			  priv->descs, priv->descs_dma_addr);
+}
+
+static int ftmac100_alloc_buffers(struct ftmac100 *priv)
+{
+	int i;
+
+	priv->descs = dma_alloc_coherent(priv->dev, sizeof(struct ftmac100_descs),
+					 &priv->descs_dma_addr, GFP_KERNEL);
+	if (!priv->descs)
+		return -ENOMEM;
+
+	memset(priv->descs, 0, sizeof(struct ftmac100_descs));
+
+	/* initialize RX ring */
+	ftmac100_rxdes_set_end_of_ring(&priv->descs->rxdes[RX_QUEUE_ENTRIES - 1]);
+
+	for (i = 0; i < RX_QUEUE_ENTRIES; i++) {
+		struct ftmac100_rxdes *rxdes = &priv->descs->rxdes[i];
+
+		if (ftmac100_alloc_rx_page(priv, rxdes))
+			goto err;
+	}
+
+	/* initialize TX ring */
+	ftmac100_txdes_set_end_of_ring(&priv->descs->txdes[TX_QUEUE_ENTRIES - 1]);
+	return 0;
+
+err:
+	ftmac100_free_buffers(priv);
+	return -ENOMEM;
+}
+
+/******************************************************************************
+ * struct mii_if_info functions
+ *****************************************************************************/
+static int ftmac100_mdio_read(struct net_device *netdev, int phy_id, int reg)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	unsigned int phycr;
+	int i;
+
+	phycr = FTMAC100_PHYCR_PHYAD(phy_id) |
+		FTMAC100_PHYCR_REGAD(reg) |
+		FTMAC100_PHYCR_MIIRD;
+
+	iowrite32(phycr, priv->base + FTMAC100_OFFSET_PHYCR);
+
+	for (i = 0; i < 10; i++) {
+		phycr = ioread32(priv->base + FTMAC100_OFFSET_PHYCR);
+
+		if ((phycr & FTMAC100_PHYCR_MIIRD) == 0)
+			return phycr & FTMAC100_PHYCR_MIIRDATA;
+
+		usleep_range(100, 1000);
+	}
+
+	netdev_err(netdev, "mdio read timed out\n");
+	return 0;
+}
+
+static void ftmac100_mdio_write(struct net_device *netdev, int phy_id, int reg,
+				int data)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	unsigned int phycr;
+	int i;
+
+	phycr = FTMAC100_PHYCR_PHYAD(phy_id) |
+		FTMAC100_PHYCR_REGAD(reg) |
+		FTMAC100_PHYCR_MIIWR;
+
+	data = FTMAC100_PHYWDATA_MIIWDATA(data);
+
+	iowrite32(data, priv->base + FTMAC100_OFFSET_PHYWDATA);
+	iowrite32(phycr, priv->base + FTMAC100_OFFSET_PHYCR);
+
+	for (i = 0; i < 10; i++) {
+		phycr = ioread32(priv->base + FTMAC100_OFFSET_PHYCR);
+
+		if ((phycr & FTMAC100_PHYCR_MIIWR) == 0)
+			return;
+
+		usleep_range(100, 1000);
+	}
+
+	netdev_err(netdev, "mdio write timed out\n");
+}
+
+/******************************************************************************
+ * struct ethtool_ops functions
+ *****************************************************************************/
+static void ftmac100_get_drvinfo(struct net_device *netdev,
+				 struct ethtool_drvinfo *info)
+{
+	strcpy(info->driver, DRV_NAME);
+	strcpy(info->version, DRV_VERSION);
+	strcpy(info->bus_info, dev_name(&netdev->dev));
+}
+
+static int ftmac100_get_settings(struct net_device *netdev, struct ethtool_cmd *cmd)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	return mii_ethtool_gset(&priv->mii, cmd);
+}
+
+static int ftmac100_set_settings(struct net_device *netdev, struct ethtool_cmd *cmd)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	return mii_ethtool_sset(&priv->mii, cmd);
+}
+
+static int ftmac100_nway_reset(struct net_device *netdev)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	return mii_nway_restart(&priv->mii);
+}
+
+static u32 ftmac100_get_link(struct net_device *netdev)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	return mii_link_ok(&priv->mii);
+}
+
+static const struct ethtool_ops ftmac100_ethtool_ops = {
+	.set_settings		= ftmac100_set_settings,
+	.get_settings		= ftmac100_get_settings,
+	.get_drvinfo		= ftmac100_get_drvinfo,
+	.nway_reset		= ftmac100_nway_reset,
+	.get_link		= ftmac100_get_link,
+};
+
+/******************************************************************************
+ * interrupt handler
+ *****************************************************************************/
+static irqreturn_t ftmac100_interrupt(int irq, void *dev_id)
+{
+	struct net_device *netdev = dev_id;
+	struct ftmac100 *priv = netdev_priv(netdev);
+
+	if (likely(netif_running(netdev))) {
+		/* Disable interrupts for polling */
+		ftmac100_disable_all_int(priv);
+		napi_schedule(&priv->napi);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/******************************************************************************
+ * struct napi_struct functions
+ *****************************************************************************/
+static int ftmac100_poll(struct napi_struct *napi, int budget)
+{
+	struct ftmac100 *priv = container_of(napi, struct ftmac100, napi);
+	struct net_device *netdev = priv->netdev;
+	unsigned int status;
+	bool completed = true;
+	int rx = 0;
+
+	status = ioread32(priv->base + FTMAC100_OFFSET_ISR);
+
+	if (status & (FTMAC100_INT_RPKT_FINISH | FTMAC100_INT_NORXBUF)) {
+		/*
+		 * FTMAC100_INT_RPKT_FINISH:
+		 *	RX DMA has received packets into RX buffer successfully
+		 *
+		 * FTMAC100_INT_NORXBUF:
+		 *	RX buffer unavailable
+		 */
+		bool retry;
+
+		do {
+			retry = ftmac100_rx_packet(priv, &rx);
+		} while (retry && rx < budget);
+
+		if (retry && rx == budget)
+			completed = false;
+	}
+
+	if (status & (FTMAC100_INT_XPKT_OK | FTMAC100_INT_XPKT_LOST)) {
+		/*
+		 * FTMAC100_INT_XPKT_OK:
+		 *	packet transmitted to ethernet successfully
+		 *
+		 * FTMAC100_INT_XPKT_LOST:
+		 *	packet transmitted to ethernet lost due to late
+		 *	collision or excessive collision
+		 */
+		ftmac100_tx_complete(priv);
+	}
+
+	if (status & (FTMAC100_INT_NORXBUF | FTMAC100_INT_RPKT_LOST |
+		      FTMAC100_INT_AHB_ERR | FTMAC100_INT_PHYSTS_CHG)) {
+		if (net_ratelimit())
+			netdev_info(netdev, "[ISR] = 0x%x: %s%s%s%s\n", status,
+				    status & FTMAC100_INT_NORXBUF ? "NORXBUF " : "",
+				    status & FTMAC100_INT_RPKT_LOST ? "RPKT_LOST " : "",
+				    status & FTMAC100_INT_AHB_ERR ? "AHB_ERR " : "",
+				    status & FTMAC100_INT_PHYSTS_CHG ? "PHYSTS_CHG" : "");
+
+		if (status & FTMAC100_INT_NORXBUF) {
+			/* RX buffer unavailable */
+			netdev->stats.rx_over_errors++;
+		}
+
+		if (status & FTMAC100_INT_RPKT_LOST) {
+			/* received packet lost due to RX FIFO full */
+			netdev->stats.rx_fifo_errors++;
+		}
+
+		if (status & FTMAC100_INT_PHYSTS_CHG) {
+			/* PHY link status change */
+			mii_check_link(&priv->mii);
+		}
+	}
+
+	if (completed) {
+		/* stop polling */
+		napi_complete(napi);
+		ftmac100_enable_all_int(priv);
+	}
+
+	return rx;
+}
+
+/******************************************************************************
+ * struct net_device_ops functions
+ *****************************************************************************/
+static int ftmac100_open(struct net_device *netdev)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	int err;
+
+	err = ftmac100_alloc_buffers(priv);
+	if (err) {
+		netdev_err(netdev, "failed to allocate buffers\n");
+		goto err_alloc;
+	}
+
+	err = request_irq(priv->irq, ftmac100_interrupt, 0, netdev->name, netdev);
+	if (err) {
+		netdev_err(netdev, "failed to request irq %d\n", priv->irq);
+		goto err_irq;
+	}
+
+	priv->rx_pointer = 0;
+	priv->tx_clean_pointer = 0;
+	priv->tx_pointer = 0;
+	priv->tx_pending = 0;
+
+	err = ftmac100_start_hw(priv);
+	if (err)
+		goto err_hw;
+
+	napi_enable(&priv->napi);
+	netif_start_queue(netdev);
+
+	ftmac100_enable_all_int(priv);
+
+	return 0;
+
+err_hw:
+	free_irq(priv->irq, netdev);
+err_irq:
+	ftmac100_free_buffers(priv);
+err_alloc:
+	return err;
+}
+
+static int ftmac100_stop(struct net_device *netdev)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+
+	ftmac100_disable_all_int(priv);
+	netif_stop_queue(netdev);
+	napi_disable(&priv->napi);
+	ftmac100_stop_hw(priv);
+	free_irq(priv->irq, netdev);
+	ftmac100_free_buffers(priv);
+
+	return 0;
+}
+
+static int ftmac100_hard_start_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	dma_addr_t map;
+
+	if (unlikely(skb->len > MAX_PKT_SIZE)) {
+		if (net_ratelimit())
+			netdev_dbg(netdev, "tx packet too big\n");
+
+		netdev->stats.tx_dropped++;
+		dev_kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	map = dma_map_single(priv->dev, skb->data, skb_headlen(skb), DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(priv->dev, map))) {
+		/* drop packet */
+		if (net_ratelimit())
+			netdev_err(netdev, "map socket buffer failed\n");
+
+		netdev->stats.tx_dropped++;
+		dev_kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	return ftmac100_xmit(priv, skb, map);
+}
+
+/* optional */
+static int ftmac100_do_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+	struct mii_ioctl_data *data = if_mii(ifr);
+
+	return generic_mii_ioctl(&priv->mii, data, cmd, NULL);
+}
+
+static const struct net_device_ops ftmac100_netdev_ops = {
+	.ndo_open		= ftmac100_open,
+	.ndo_stop		= ftmac100_stop,
+	.ndo_start_xmit		= ftmac100_hard_start_xmit,
+	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_do_ioctl		= ftmac100_do_ioctl,
+};
+
+/******************************************************************************
+ * struct platform_driver functions
+ *****************************************************************************/
+static int ftmac100_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int irq;
+	struct net_device *netdev;
+	struct ftmac100 *priv;
+	int err;
+
+	if (!pdev)
+		return -ENODEV;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENXIO;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	/* setup net_device */
+	netdev = alloc_etherdev(sizeof(*priv));
+	if (!netdev) {
+		err = -ENOMEM;
+		goto err_alloc_etherdev;
+	}
+
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+	SET_ETHTOOL_OPS(netdev, &ftmac100_ethtool_ops);
+	netdev->netdev_ops = &ftmac100_netdev_ops;
+
+	platform_set_drvdata(pdev, netdev);
+
+	/* setup private data */
+	priv = netdev_priv(netdev);
+	priv->netdev = netdev;
+	priv->dev = &pdev->dev;
+
+	spin_lock_init(&priv->tx_lock);
+
+	/* initialize NAPI */
+	netif_napi_add(netdev, &priv->napi, ftmac100_poll, 64);
+
+	/* map io memory */
+	priv->res = request_mem_region(res->start, resource_size(res),
+				       dev_name(&pdev->dev));
+	if (!priv->res) {
+		dev_err(&pdev->dev, "Could not reserve memory region\n");
+		err = -ENOMEM;
+		goto err_req_mem;
+	}
+
+	priv->base = ioremap(res->start, res->end - res->start);
+	if (!priv->base) {
+		dev_err(&pdev->dev, "Failed to ioremap ethernet registers\n");
+		err = -EIO;
+		goto err_ioremap;
+	}
+
+	priv->irq = irq;
+
+	/* initialize struct mii_if_info */
+	priv->mii.phy_id	= 0;
+	priv->mii.phy_id_mask	= 0x1f;
+	priv->mii.reg_num_mask	= 0x1f;
+	priv->mii.dev		= netdev;
+	priv->mii.mdio_read	= ftmac100_mdio_read;
+	priv->mii.mdio_write	= ftmac100_mdio_write;
+
+	/* register network device */
+	err = register_netdev(netdev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register netdev\n");
+		goto err_register_netdev;
+	}
+
+	netdev_info(netdev, "irq %d, mapped at %p\n", priv->irq, priv->base);
+
+	if (!is_valid_ether_addr(netdev->dev_addr)) {
+		random_ether_addr(netdev->dev_addr);
+		netdev_info(netdev, "generated random MAC address %pM\n",
+			    netdev->dev_addr);
+	}
+
+	return 0;
+
+err_register_netdev:
+	iounmap(priv->base);
+err_ioremap:
+	release_resource(priv->res);
+err_req_mem:
+	netif_napi_del(&priv->napi);
+	platform_set_drvdata(pdev, NULL);
+	free_netdev(netdev);
+err_alloc_etherdev:
+	return err;
+}
+
+static int __exit ftmac100_remove(struct platform_device *pdev)
+{
+	struct net_device *netdev;
+	struct ftmac100 *priv;
+
+	netdev = platform_get_drvdata(pdev);
+	priv = netdev_priv(netdev);
+
+	unregister_netdev(netdev);
+
+	iounmap(priv->base);
+	release_resource(priv->res);
+
+	netif_napi_del(&priv->napi);
+	platform_set_drvdata(pdev, NULL);
+	free_netdev(netdev);
+	return 0;
+}
+
+static struct platform_driver ftmac100_driver = {
+	.probe		= ftmac100_probe,
+	.remove		= __exit_p(ftmac100_remove),
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+/******************************************************************************
+ * initialization / finalization
+ *****************************************************************************/
+static int __init ftmac100_init(void)
+{
+	pr_info("Loading version " DRV_VERSION " ...\n");
+	return platform_driver_register(&ftmac100_driver);
+}
+
+static void __exit ftmac100_exit(void)
+{
+	platform_driver_unregister(&ftmac100_driver);
+}
+
+module_init(ftmac100_init);
+module_exit(ftmac100_exit);
+
+MODULE_AUTHOR("Po-Yu Chuang <ratbert@faraday-tech.com>");
+MODULE_DESCRIPTION("FTMAC100 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ftmac100.h b/drivers/net/ftmac100.h
new file mode 100644
index 000000000000..46a0c47b1ee1
--- /dev/null
+++ b/drivers/net/ftmac100.h
@@ -0,0 +1,180 @@
+/*
+ * Faraday FTMAC100 10/100 Ethernet
+ *
+ * (C) Copyright 2009-2011 Faraday Technology
+ * Po-Yu Chuang <ratbert@faraday-tech.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __FTMAC100_H
+#define __FTMAC100_H
+
+#define	FTMAC100_OFFSET_ISR		0x00
+#define	FTMAC100_OFFSET_IMR		0x04
+#define	FTMAC100_OFFSET_MAC_MADR	0x08
+#define	FTMAC100_OFFSET_MAC_LADR	0x0c
+#define	FTMAC100_OFFSET_MAHT0		0x10
+#define	FTMAC100_OFFSET_MAHT1		0x14
+#define	FTMAC100_OFFSET_TXPD		0x18
+#define	FTMAC100_OFFSET_RXPD		0x1c
+#define	FTMAC100_OFFSET_TXR_BADR	0x20
+#define	FTMAC100_OFFSET_RXR_BADR	0x24
+#define	FTMAC100_OFFSET_ITC		0x28
+#define	FTMAC100_OFFSET_APTC		0x2c
+#define	FTMAC100_OFFSET_DBLAC		0x30
+#define	FTMAC100_OFFSET_MACCR		0x88
+#define	FTMAC100_OFFSET_MACSR		0x8c
+#define	FTMAC100_OFFSET_PHYCR		0x90
+#define	FTMAC100_OFFSET_PHYWDATA	0x94
+#define	FTMAC100_OFFSET_FCR		0x98
+#define	FTMAC100_OFFSET_BPR		0x9c
+#define	FTMAC100_OFFSET_TS		0xc4
+#define	FTMAC100_OFFSET_DMAFIFOS	0xc8
+#define	FTMAC100_OFFSET_TM		0xcc
+#define	FTMAC100_OFFSET_TX_MCOL_SCOL	0xd4
+#define	FTMAC100_OFFSET_RPF_AEP		0xd8
+#define	FTMAC100_OFFSET_XM_PG		0xdc
+#define	FTMAC100_OFFSET_RUNT_TLCC	0xe0
+#define	FTMAC100_OFFSET_CRCER_FTL	0xe4
+#define	FTMAC100_OFFSET_RLC_RCC		0xe8
+#define	FTMAC100_OFFSET_BROC		0xec
+#define	FTMAC100_OFFSET_MULCA		0xf0
+#define	FTMAC100_OFFSET_RP		0xf4
+#define	FTMAC100_OFFSET_XP		0xf8
+
+/*
+ * Interrupt status register & interrupt mask register
+ */
+#define	FTMAC100_INT_RPKT_FINISH	(1 << 0)
+#define	FTMAC100_INT_NORXBUF		(1 << 1)
+#define	FTMAC100_INT_XPKT_FINISH	(1 << 2)
+#define	FTMAC100_INT_NOTXBUF		(1 << 3)
+#define	FTMAC100_INT_XPKT_OK		(1 << 4)
+#define	FTMAC100_INT_XPKT_LOST		(1 << 5)
+#define	FTMAC100_INT_RPKT_SAV		(1 << 6)
+#define	FTMAC100_INT_RPKT_LOST		(1 << 7)
+#define	FTMAC100_INT_AHB_ERR		(1 << 8)
+#define	FTMAC100_INT_PHYSTS_CHG		(1 << 9)
+
+/*
+ * Interrupt timer control register
+ */
+#define FTMAC100_ITC_RXINT_CNT(x)	(((x) & 0xf) << 0)
+#define FTMAC100_ITC_RXINT_THR(x)	(((x) & 0x7) << 4)
+#define FTMAC100_ITC_RXINT_TIME_SEL	(1 << 7)
+#define FTMAC100_ITC_TXINT_CNT(x)	(((x) & 0xf) << 8)
+#define FTMAC100_ITC_TXINT_THR(x)	(((x) & 0x7) << 12)
+#define FTMAC100_ITC_TXINT_TIME_SEL	(1 << 15)
+
+/*
+ * Automatic polling timer control register
+ */
+#define	FTMAC100_APTC_RXPOLL_CNT(x)	(((x) & 0xf) << 0)
+#define	FTMAC100_APTC_RXPOLL_TIME_SEL	(1 << 4)
+#define	FTMAC100_APTC_TXPOLL_CNT(x)	(((x) & 0xf) << 8)
+#define	FTMAC100_APTC_TXPOLL_TIME_SEL	(1 << 12)
+
+/*
+ * DMA burst length and arbitration control register
+ */
+#define FTMAC100_DBLAC_INCR4_EN		(1 << 0)
+#define FTMAC100_DBLAC_INCR8_EN		(1 << 1)
+#define FTMAC100_DBLAC_INCR16_EN	(1 << 2)
+#define FTMAC100_DBLAC_RXFIFO_LTHR(x)	(((x) & 0x7) << 3)
+#define FTMAC100_DBLAC_RXFIFO_HTHR(x)	(((x) & 0x7) << 6)
+#define FTMAC100_DBLAC_RX_THR_EN	(1 << 9)
+
+/*
+ * MAC control register
+ */
+#define	FTMAC100_MACCR_XDMA_EN		(1 << 0)
+#define	FTMAC100_MACCR_RDMA_EN		(1 << 1)
+#define	FTMAC100_MACCR_SW_RST		(1 << 2)
+#define	FTMAC100_MACCR_LOOP_EN		(1 << 3)
+#define	FTMAC100_MACCR_CRC_DIS		(1 << 4)
+#define	FTMAC100_MACCR_XMT_EN		(1 << 5)
+#define	FTMAC100_MACCR_ENRX_IN_HALFTX	(1 << 6)
+#define	FTMAC100_MACCR_RCV_EN		(1 << 8)
+#define	FTMAC100_MACCR_HT_MULTI_EN	(1 << 9)
+#define	FTMAC100_MACCR_RX_RUNT		(1 << 10)
+#define	FTMAC100_MACCR_RX_FTL		(1 << 11)
+#define	FTMAC100_MACCR_RCV_ALL		(1 << 12)
+#define	FTMAC100_MACCR_CRC_APD		(1 << 14)
+#define	FTMAC100_MACCR_FULLDUP		(1 << 15)
+#define	FTMAC100_MACCR_RX_MULTIPKT	(1 << 16)
+#define	FTMAC100_MACCR_RX_BROADPKT	(1 << 17)
+
+/*
+ * PHY control register
+ */
+#define FTMAC100_PHYCR_MIIRDATA		0xffff
+#define FTMAC100_PHYCR_PHYAD(x)		(((x) & 0x1f) << 16)
+#define FTMAC100_PHYCR_REGAD(x)		(((x) & 0x1f) << 21)
+#define FTMAC100_PHYCR_MIIRD		(1 << 26)
+#define FTMAC100_PHYCR_MIIWR		(1 << 27)
+
+/*
+ * PHY write data register
+ */
+#define FTMAC100_PHYWDATA_MIIWDATA(x)	((x) & 0xffff)
+
+/*
+ * Transmit descriptor, aligned to 16 bytes
+ */
+struct ftmac100_txdes {
+	unsigned int	txdes0;
+	unsigned int	txdes1;
+	unsigned int	txdes2;	/* TXBUF_BADR */
+	unsigned int	txdes3;	/* not used by HW */
+} __attribute__ ((aligned(16)));
+
+#define	FTMAC100_TXDES0_TXPKT_LATECOL	(1 << 0)
+#define	FTMAC100_TXDES0_TXPKT_EXSCOL	(1 << 1)
+#define	FTMAC100_TXDES0_TXDMA_OWN	(1 << 31)
+
+#define	FTMAC100_TXDES1_TXBUF_SIZE(x)	((x) & 0x7ff)
+#define	FTMAC100_TXDES1_LTS		(1 << 27)
+#define	FTMAC100_TXDES1_FTS		(1 << 28)
+#define	FTMAC100_TXDES1_TX2FIC		(1 << 29)
+#define	FTMAC100_TXDES1_TXIC		(1 << 30)
+#define	FTMAC100_TXDES1_EDOTR		(1 << 31)
+
+/*
+ * Receive descriptor, aligned to 16 bytes
+ */
+struct ftmac100_rxdes {
+	unsigned int	rxdes0;
+	unsigned int	rxdes1;
+	unsigned int	rxdes2;	/* RXBUF_BADR */
+	unsigned int	rxdes3;	/* not used by HW */
+} __attribute__ ((aligned(16)));
+
+#define	FTMAC100_RXDES0_RFL		0x7ff
+#define	FTMAC100_RXDES0_MULTICAST	(1 << 16)
+#define	FTMAC100_RXDES0_BROADCAST	(1 << 17)
+#define	FTMAC100_RXDES0_RX_ERR		(1 << 18)
+#define	FTMAC100_RXDES0_CRC_ERR		(1 << 19)
+#define	FTMAC100_RXDES0_FTL		(1 << 20)
+#define	FTMAC100_RXDES0_RUNT		(1 << 21)
+#define	FTMAC100_RXDES0_RX_ODD_NB	(1 << 22)
+#define	FTMAC100_RXDES0_LRS		(1 << 28)
+#define	FTMAC100_RXDES0_FRS		(1 << 29)
+#define	FTMAC100_RXDES0_RXDMA_OWN	(1 << 31)
+
+#define	FTMAC100_RXDES1_RXBUF_SIZE(x)	((x) & 0x7ff)
+#define	FTMAC100_RXDES1_EDORR		(1 << 31)
+
+#endif /* __FTMAC100_H */
diff --git a/drivers/net/gianfar.c b/drivers/net/gianfar.c
index 5ed8f9f9419f..ccb231c4d933 100644
--- a/drivers/net/gianfar.c
+++ b/drivers/net/gianfar.c
@@ -123,8 +123,7 @@ static irqreturn_t gfar_interrupt(int irq, void *dev_id);
 static void adjust_link(struct net_device *dev);
 static void init_registers(struct net_device *dev);
 static int init_phy(struct net_device *dev);
-static int gfar_probe(struct platform_device *ofdev,
-		const struct of_device_id *match);
+static int gfar_probe(struct platform_device *ofdev);
 static int gfar_remove(struct platform_device *ofdev);
 static void free_skb_resources(struct gfar_private *priv);
 static void gfar_set_multi(struct net_device *dev);
@@ -957,8 +956,7 @@ static void gfar_detect_errata(struct gfar_private *priv)
 
 /* Set up the ethernet device structure, private data,
  * and anything else we need before we start */
-static int gfar_probe(struct platform_device *ofdev,
-		const struct of_device_id *match)
+static int gfar_probe(struct platform_device *ofdev)
 {
 	u32 tempval;
 	struct net_device *dev = NULL;
@@ -3256,7 +3254,7 @@ static struct of_device_id gfar_match[] =
 MODULE_DEVICE_TABLE(of, gfar_match);
 
 /* Structure for a device driver */
-static struct of_platform_driver gfar_driver = {
+static struct platform_driver gfar_driver = {
 	.driver = {
 		.name = "fsl-gianfar",
 		.owner = THIS_MODULE,
@@ -3269,12 +3267,12 @@ static struct of_platform_driver gfar_driver = {
 
 static int __init gfar_init(void)
 {
-	return of_register_platform_driver(&gfar_driver);
+	return platform_driver_register(&gfar_driver);
 }
 
 static void __exit gfar_exit(void)
 {
-	of_unregister_platform_driver(&gfar_driver);
+	platform_driver_unregister(&gfar_driver);
 }
 
 module_init(gfar_init);
diff --git a/drivers/net/greth.c b/drivers/net/greth.c
index fdb0333f5cb6..396ff7d785d1 100644
--- a/drivers/net/greth.c
+++ b/drivers/net/greth.c
@@ -1411,7 +1411,7 @@ error:
 }
 
 /* Initialize the GRETH MAC */
-static int __devinit greth_of_probe(struct platform_device *ofdev, const struct of_device_id *match)
+static int __devinit greth_of_probe(struct platform_device *ofdev)
 {
 	struct net_device *dev;
 	struct greth_private *greth;
@@ -1646,7 +1646,7 @@ static struct of_device_id greth_of_match[] = {
 
 MODULE_DEVICE_TABLE(of, greth_of_match);
 
-static struct of_platform_driver greth_of_driver = {
+static struct platform_driver greth_of_driver = {
 	.driver = {
 		.name = "grlib-greth",
 		.owner = THIS_MODULE,
@@ -1658,12 +1658,12 @@ static struct of_platform_driver greth_of_driver = {
 
 static int __init greth_init(void)
 {
-	return of_register_platform_driver(&greth_of_driver);
+	return platform_driver_register(&greth_of_driver);
 }
 
 static void __exit greth_cleanup(void)
 {
-	of_unregister_platform_driver(&greth_of_driver);
+	platform_driver_unregister(&greth_of_driver);
 }
 
 module_init(greth_init);
diff --git a/drivers/net/hamradio/bpqether.c b/drivers/net/hamradio/bpqether.c
index ac1d323c5eb5..8931168d3e74 100644
--- a/drivers/net/hamradio/bpqether.c
+++ b/drivers/net/hamradio/bpqether.c
@@ -400,13 +400,14 @@ static void *bpq_seq_start(struct seq_file *seq, loff_t *pos)
 static void *bpq_seq_next(struct seq_file *seq, void *v, loff_t *pos)
 {
 	struct list_head *p;
+	struct bpqdev *bpqdev = v;
 
 	++*pos;
 
 	if (v == SEQ_START_TOKEN)
-		p = rcu_dereference(bpq_devices.next);
+		p = rcu_dereference(list_next_rcu(&bpq_devices));
 	else
-		p = rcu_dereference(((struct bpqdev *)v)->bpq_list.next);
+		p = rcu_dereference(list_next_rcu(&bpqdev->bpq_list));
 
 	return (p == &bpq_devices) ? NULL 
 		: list_entry(p, struct bpqdev, bpq_list);
diff --git a/drivers/net/ibm_newemac/core.c b/drivers/net/ibm_newemac/core.c
index 6d9275c52e05..3bb990b6651a 100644
--- a/drivers/net/ibm_newemac/core.c
+++ b/drivers/net/ibm_newemac/core.c
@@ -2719,8 +2719,7 @@ static const struct net_device_ops emac_gige_netdev_ops = {
 	.ndo_change_mtu		= emac_change_mtu,
 };
 
-static int __devinit emac_probe(struct platform_device *ofdev,
-				const struct of_device_id *match)
+static int __devinit emac_probe(struct platform_device *ofdev)
 {
 	struct net_device *ndev;
 	struct emac_instance *dev;
@@ -2994,7 +2993,7 @@ static struct of_device_id emac_match[] =
 };
 MODULE_DEVICE_TABLE(of, emac_match);
 
-static struct of_platform_driver emac_driver = {
+static struct platform_driver emac_driver = {
 	.driver = {
 		.name = "emac",
 		.owner = THIS_MODULE,
@@ -3069,7 +3068,7 @@ static int __init emac_init(void)
 	rc = tah_init();
 	if (rc)
 		goto err_rgmii;
-	rc = of_register_platform_driver(&emac_driver);
+	rc = platform_driver_register(&emac_driver);
 	if (rc)
 		goto err_tah;
 
@@ -3091,7 +3090,7 @@ static void __exit emac_exit(void)
 {
 	int i;
 
-	of_unregister_platform_driver(&emac_driver);
+	platform_driver_unregister(&emac_driver);
 
 	tah_exit();
 	rgmii_exit();
diff --git a/drivers/net/ibm_newemac/mal.c b/drivers/net/ibm_newemac/mal.c
index d5717e2123e1..d268f404b7b0 100644
--- a/drivers/net/ibm_newemac/mal.c
+++ b/drivers/net/ibm_newemac/mal.c
@@ -517,8 +517,7 @@ void *mal_dump_regs(struct mal_instance *mal, void *buf)
 	return regs + 1;
 }
 
-static int __devinit mal_probe(struct platform_device *ofdev,
-			       const struct of_device_id *match)
+static int __devinit mal_probe(struct platform_device *ofdev)
 {
 	struct mal_instance *mal;
 	int err = 0, i, bd_size;
@@ -789,7 +788,7 @@ static struct of_device_id mal_platform_match[] =
 	{},
 };
 
-static struct of_platform_driver mal_of_driver = {
+static struct platform_driver mal_of_driver = {
 	.driver = {
 		.name = "mcmal",
 		.owner = THIS_MODULE,
@@ -801,10 +800,10 @@ static struct of_platform_driver mal_of_driver = {
 
 int __init mal_init(void)
 {
-	return of_register_platform_driver(&mal_of_driver);
+	return platform_driver_register(&mal_of_driver);
 }
 
 void mal_exit(void)
 {
-	of_unregister_platform_driver(&mal_of_driver);
+	platform_driver_unregister(&mal_of_driver);
 }
diff --git a/drivers/net/ibm_newemac/rgmii.c b/drivers/net/ibm_newemac/rgmii.c
index dd61798897ac..4fa53f3def64 100644
--- a/drivers/net/ibm_newemac/rgmii.c
+++ b/drivers/net/ibm_newemac/rgmii.c
@@ -228,8 +228,7 @@ void *rgmii_dump_regs(struct platform_device *ofdev, void *buf)
 }
 
 
-static int __devinit rgmii_probe(struct platform_device *ofdev,
-				 const struct of_device_id *match)
+static int __devinit rgmii_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct rgmii_instance *dev;
@@ -318,7 +317,7 @@ static struct of_device_id rgmii_match[] =
 	{},
 };
 
-static struct of_platform_driver rgmii_driver = {
+static struct platform_driver rgmii_driver = {
 	.driver = {
 		.name = "emac-rgmii",
 		.owner = THIS_MODULE,
@@ -330,10 +329,10 @@ static struct of_platform_driver rgmii_driver = {
 
 int __init rgmii_init(void)
 {
-	return of_register_platform_driver(&rgmii_driver);
+	return platform_driver_register(&rgmii_driver);
 }
 
 void rgmii_exit(void)
 {
-	of_unregister_platform_driver(&rgmii_driver);
+	platform_driver_unregister(&rgmii_driver);
 }
diff --git a/drivers/net/ibm_newemac/tah.c b/drivers/net/ibm_newemac/tah.c
index 299aa49490c0..8ead6a96abaa 100644
--- a/drivers/net/ibm_newemac/tah.c
+++ b/drivers/net/ibm_newemac/tah.c
@@ -87,8 +87,7 @@ void *tah_dump_regs(struct platform_device *ofdev, void *buf)
 	return regs + 1;
 }
 
-static int __devinit tah_probe(struct platform_device *ofdev,
-			       const struct of_device_id *match)
+static int __devinit tah_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct tah_instance *dev;
@@ -165,7 +164,7 @@ static struct of_device_id tah_match[] =
 	{},
 };
 
-static struct of_platform_driver tah_driver = {
+static struct platform_driver tah_driver = {
 	.driver = {
 		.name = "emac-tah",
 		.owner = THIS_MODULE,
@@ -177,10 +176,10 @@ static struct of_platform_driver tah_driver = {
 
 int __init tah_init(void)
 {
-	return of_register_platform_driver(&tah_driver);
+	return platform_driver_register(&tah_driver);
 }
 
 void tah_exit(void)
 {
-	of_unregister_platform_driver(&tah_driver);
+	platform_driver_unregister(&tah_driver);
 }
diff --git a/drivers/net/ibm_newemac/zmii.c b/drivers/net/ibm_newemac/zmii.c
index 34ed6ee8ca8a..97449e786d61 100644
--- a/drivers/net/ibm_newemac/zmii.c
+++ b/drivers/net/ibm_newemac/zmii.c
@@ -231,8 +231,7 @@ void *zmii_dump_regs(struct platform_device *ofdev, void *buf)
 	return regs + 1;
 }
 
-static int __devinit zmii_probe(struct platform_device *ofdev,
-				const struct of_device_id *match)
+static int __devinit zmii_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct zmii_instance *dev;
@@ -312,7 +311,7 @@ static struct of_device_id zmii_match[] =
 	{},
 };
 
-static struct of_platform_driver zmii_driver = {
+static struct platform_driver zmii_driver = {
 	.driver = {
 		.name = "emac-zmii",
 		.owner = THIS_MODULE,
@@ -324,10 +323,10 @@ static struct of_platform_driver zmii_driver = {
 
 int __init zmii_init(void)
 {
-	return of_register_platform_driver(&zmii_driver);
+	return platform_driver_register(&zmii_driver);
 }
 
 void zmii_exit(void)
 {
-	of_unregister_platform_driver(&zmii_driver);
+	platform_driver_unregister(&zmii_driver);
 }
diff --git a/drivers/net/igb/e1000_82575.c b/drivers/net/igb/e1000_82575.c
index 0a2368fa6bc6..65c1833244f7 100644
--- a/drivers/net/igb/e1000_82575.c
+++ b/drivers/net/igb/e1000_82575.c
@@ -129,6 +129,7 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 		break;
 	case E1000_DEV_ID_82580_COPPER:
 	case E1000_DEV_ID_82580_FIBER:
+	case E1000_DEV_ID_82580_QUAD_FIBER:
 	case E1000_DEV_ID_82580_SERDES:
 	case E1000_DEV_ID_82580_SGMII:
 	case E1000_DEV_ID_82580_COPPER_DUAL:
@@ -237,9 +238,15 @@ static s32 igb_get_invariants_82575(struct e1000_hw *hw)
 		size = 14;
 	nvm->word_size = 1 << size;
 
-	/* if 82576 then initialize mailbox parameters */
-	if (mac->type == e1000_82576)
+	/* if part supports SR-IOV then initialize mailbox parameters */
+	switch (mac->type) {
+	case e1000_82576:
+	case e1000_i350:
 		igb_init_mbx_params_pf(hw);
+		break;
+	default:
+		break;
+	}
 
 	/* setup PHY parameters */
 	if (phy->media_type != e1000_media_type_copper) {
diff --git a/drivers/net/igb/e1000_defines.h b/drivers/net/igb/e1000_defines.h
index 6319ed902bc0..ff46c91520af 100644
--- a/drivers/net/igb/e1000_defines.h
+++ b/drivers/net/igb/e1000_defines.h
@@ -770,4 +770,11 @@
 #define E1000_PCIEMISC_LX_DECISION      0x00000080 /* Lx power decision based
                                                       on DMA coal */
 
+/* Tx Rate-Scheduler Config fields */
+#define E1000_RTTBCNRC_RS_ENA		0x80000000
+#define E1000_RTTBCNRC_RF_DEC_MASK	0x00003FFF
+#define E1000_RTTBCNRC_RF_INT_SHIFT	14
+#define E1000_RTTBCNRC_RF_INT_MASK	\
+	(E1000_RTTBCNRC_RF_DEC_MASK << E1000_RTTBCNRC_RF_INT_SHIFT)
+
 #endif
diff --git a/drivers/net/igb/e1000_hw.h b/drivers/net/igb/e1000_hw.h
index e2638afb8cdc..281324e85980 100644
--- a/drivers/net/igb/e1000_hw.h
+++ b/drivers/net/igb/e1000_hw.h
@@ -54,6 +54,7 @@ struct e1000_hw;
 #define E1000_DEV_ID_82580_SERDES             0x1510
 #define E1000_DEV_ID_82580_SGMII              0x1511
 #define E1000_DEV_ID_82580_COPPER_DUAL        0x1516
+#define E1000_DEV_ID_82580_QUAD_FIBER         0x1527
 #define E1000_DEV_ID_DH89XXCC_SGMII           0x0438
 #define E1000_DEV_ID_DH89XXCC_SERDES          0x043A
 #define E1000_DEV_ID_DH89XXCC_BACKPLANE       0x043C
diff --git a/drivers/net/igb/e1000_mbx.c b/drivers/net/igb/e1000_mbx.c
index c474cdb70047..78d48c7fa859 100644
--- a/drivers/net/igb/e1000_mbx.c
+++ b/drivers/net/igb/e1000_mbx.c
@@ -422,26 +422,24 @@ s32 igb_init_mbx_params_pf(struct e1000_hw *hw)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 
-	if (hw->mac.type == e1000_82576) {
-		mbx->timeout = 0;
-		mbx->usec_delay = 0;
-
-		mbx->size = E1000_VFMAILBOX_SIZE;
-
-		mbx->ops.read = igb_read_mbx_pf;
-		mbx->ops.write = igb_write_mbx_pf;
-		mbx->ops.read_posted = igb_read_posted_mbx;
-		mbx->ops.write_posted = igb_write_posted_mbx;
-		mbx->ops.check_for_msg = igb_check_for_msg_pf;
-		mbx->ops.check_for_ack = igb_check_for_ack_pf;
-		mbx->ops.check_for_rst = igb_check_for_rst_pf;
-
-		mbx->stats.msgs_tx = 0;
-		mbx->stats.msgs_rx = 0;
-		mbx->stats.reqs = 0;
-		mbx->stats.acks = 0;
-		mbx->stats.rsts = 0;
-	}
+	mbx->timeout = 0;
+	mbx->usec_delay = 0;
+
+	mbx->size = E1000_VFMAILBOX_SIZE;
+
+	mbx->ops.read = igb_read_mbx_pf;
+	mbx->ops.write = igb_write_mbx_pf;
+	mbx->ops.read_posted = igb_read_posted_mbx;
+	mbx->ops.write_posted = igb_write_posted_mbx;
+	mbx->ops.check_for_msg = igb_check_for_msg_pf;
+	mbx->ops.check_for_ack = igb_check_for_ack_pf;
+	mbx->ops.check_for_rst = igb_check_for_rst_pf;
+
+	mbx->stats.msgs_tx = 0;
+	mbx->stats.msgs_rx = 0;
+	mbx->stats.reqs = 0;
+	mbx->stats.acks = 0;
+	mbx->stats.rsts = 0;
 
 	return 0;
 }
diff --git a/drivers/net/igb/e1000_regs.h b/drivers/net/igb/e1000_regs.h
index 8ac83c5190d5..3a6f8471aea2 100644
--- a/drivers/net/igb/e1000_regs.h
+++ b/drivers/net/igb/e1000_regs.h
@@ -106,6 +106,10 @@
 
 #define E1000_RQDPC(_n) (0x0C030 + ((_n) * 0x40))
 
+/* TX Rate Limit Registers */
+#define E1000_RTTDQSEL	0x3604	/* Tx Desc Plane Queue Select - WO */
+#define E1000_RTTBCNRC	0x36B0	/* Tx BCN Rate-Scheduler Config - WO */
+
 /* Split and Replication RX Control - RW */
 #define E1000_RXPBS    0x02404  /* Rx Packet Buffer Size - RW */
 /*
diff --git a/drivers/net/igb/igb.h b/drivers/net/igb/igb.h
index 92a4ef09e55c..bbc5ebfe254a 100644
--- a/drivers/net/igb/igb.h
+++ b/drivers/net/igb/igb.h
@@ -77,6 +77,7 @@ struct vf_data_storage {
 	unsigned long last_nack;
 	u16 pf_vlan; /* When set, guest VLAN config not allowed. */
 	u16 pf_qos;
+	u16 tx_rate;
 };
 
 #define IGB_VF_FLAG_CTS            0x00000001 /* VF is clear to send data */
@@ -323,6 +324,7 @@ struct igb_adapter {
 	u16 rx_ring_count;
 	unsigned int vfs_allocated_count;
 	struct vf_data_storage *vf_data;
+	int vf_rate_link_speed;
 	u32 rss_queues;
 	u32 wvbr;
 };
diff --git a/drivers/net/igb/igb_ethtool.c b/drivers/net/igb/igb_ethtool.c
index a70e16bcfa7e..61f7849cb5a7 100644
--- a/drivers/net/igb/igb_ethtool.c
+++ b/drivers/net/igb/igb_ethtool.c
@@ -727,8 +727,9 @@ static void igb_get_drvinfo(struct net_device *netdev,
 	char firmware_version[32];
 	u16 eeprom_data;
 
-	strncpy(drvinfo->driver,  igb_driver_name, 32);
-	strncpy(drvinfo->version, igb_driver_version, 32);
+	strncpy(drvinfo->driver,  igb_driver_name, sizeof(drvinfo->driver) - 1);
+	strncpy(drvinfo->version, igb_driver_version,
+		sizeof(drvinfo->version) - 1);
 
 	/* EEPROM image version # is reported as firmware version # for
 	 * 82575 controllers */
@@ -738,8 +739,10 @@ static void igb_get_drvinfo(struct net_device *netdev,
 		(eeprom_data & 0x0FF0) >> 4,
 		eeprom_data & 0x000F);
 
-	strncpy(drvinfo->fw_version, firmware_version, 32);
-	strncpy(drvinfo->bus_info, pci_name(adapter->pdev), 32);
+	strncpy(drvinfo->fw_version, firmware_version,
+		sizeof(drvinfo->fw_version) - 1);
+	strncpy(drvinfo->bus_info, pci_name(adapter->pdev),
+		sizeof(drvinfo->bus_info) - 1);
 	drvinfo->n_stats = IGB_STATS_LEN;
 	drvinfo->testinfo_len = IGB_TEST_LEN;
 	drvinfo->regdump_len = igb_get_regs_len(netdev);
@@ -1070,7 +1073,7 @@ static bool reg_pattern_test(struct igb_adapter *adapter, u64 *data,
 		{0x5A5A5A5A, 0xA5A5A5A5, 0x00000000, 0xFFFFFFFF};
 	for (pat = 0; pat < ARRAY_SIZE(_test); pat++) {
 		wr32(reg, (_test[pat] & write));
-		val = rd32(reg);
+		val = rd32(reg) & mask;
 		if (val != (_test[pat] & write & mask)) {
 			dev_err(&adapter->pdev->dev, "pattern test reg %04X "
 				"failed: got 0x%08X expected 0x%08X\n",
diff --git a/drivers/net/igb/igb_main.c b/drivers/net/igb/igb_main.c
index 58c665b7513d..eef380af0537 100644
--- a/drivers/net/igb/igb_main.c
+++ b/drivers/net/igb/igb_main.c
@@ -50,12 +50,12 @@
 #endif
 #include "igb.h"
 
-#define DRV_VERSION "2.1.0-k2"
+#define DRV_VERSION "2.4.13-k2"
 char igb_driver_name[] = "igb";
 char igb_driver_version[] = DRV_VERSION;
 static const char igb_driver_string[] =
 				"Intel(R) Gigabit Ethernet Network Driver";
-static const char igb_copyright[] = "Copyright (c) 2007-2009 Intel Corporation.";
+static const char igb_copyright[] = "Copyright (c) 2007-2011 Intel Corporation.";
 
 static const struct e1000_info *igb_info_tbl[] = {
 	[board_82575] = &e1000_82575_info,
@@ -68,6 +68,7 @@ static DEFINE_PCI_DEVICE_TABLE(igb_pci_tbl) = {
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_SGMII), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_COPPER), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_FIBER), board_82575 },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_QUAD_FIBER), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_SERDES), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_SGMII), board_82575 },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_COPPER_DUAL), board_82575 },
@@ -149,6 +150,7 @@ static int igb_ndo_set_vf_vlan(struct net_device *netdev,
 static int igb_ndo_set_vf_bw(struct net_device *netdev, int vf, int tx_rate);
 static int igb_ndo_get_vf_config(struct net_device *netdev, int vf,
 				 struct ifla_vf_info *ivi);
+static void igb_check_vf_rate_limit(struct igb_adapter *);
 
 #ifdef CONFIG_PM
 static int igb_suspend(struct pci_dev *, pm_message_t);
@@ -2286,9 +2288,19 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
 
 	spin_lock_init(&adapter->stats64_lock);
 #ifdef CONFIG_PCI_IOV
-	if (hw->mac.type == e1000_82576)
-		adapter->vfs_allocated_count = (max_vfs > 7) ? 7 : max_vfs;
-
+	switch (hw->mac.type) {
+	case e1000_82576:
+	case e1000_i350:
+		if (max_vfs > 7) {
+			dev_warn(&pdev->dev,
+				 "Maximum of 7 VFs per PF, using max\n");
+			adapter->vfs_allocated_count = 7;
+		} else
+			adapter->vfs_allocated_count = max_vfs;
+		break;
+	default:
+		break;
+	}
 #endif /* CONFIG_PCI_IOV */
 	adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES, num_online_cpus());
 
@@ -3505,6 +3517,7 @@ static void igb_watchdog_task(struct work_struct *work)
 			netif_carrier_on(netdev);
 
 			igb_ping_all_vfs(adapter);
+			igb_check_vf_rate_limit(adapter);
 
 			/* link state has changed, schedule phy info update */
 			if (!test_bit(__IGB_DOWN, &adapter->state))
@@ -6593,9 +6606,91 @@ static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 	return igb_set_vf_mac(adapter, vf, mac);
 }
 
+static int igb_link_mbps(int internal_link_speed)
+{
+	switch (internal_link_speed) {
+	case SPEED_100:
+		return 100;
+	case SPEED_1000:
+		return 1000;
+	default:
+		return 0;
+	}
+}
+
+static void igb_set_vf_rate_limit(struct e1000_hw *hw, int vf, int tx_rate,
+				  int link_speed)
+{
+	int rf_dec, rf_int;
+	u32 bcnrc_val;
+
+	if (tx_rate != 0) {
+		/* Calculate the rate factor values to set */
+		rf_int = link_speed / tx_rate;
+		rf_dec = (link_speed - (rf_int * tx_rate));
+		rf_dec = (rf_dec * (1<<E1000_RTTBCNRC_RF_INT_SHIFT)) / tx_rate;
+
+		bcnrc_val = E1000_RTTBCNRC_RS_ENA;
+		bcnrc_val |= ((rf_int<<E1000_RTTBCNRC_RF_INT_SHIFT) &
+		               E1000_RTTBCNRC_RF_INT_MASK);
+		bcnrc_val |= (rf_dec & E1000_RTTBCNRC_RF_DEC_MASK);
+	} else {
+		bcnrc_val = 0;
+	}
+
+	wr32(E1000_RTTDQSEL, vf); /* vf X uses queue X */
+	wr32(E1000_RTTBCNRC, bcnrc_val);
+}
+
+static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
+{
+	int actual_link_speed, i;
+	bool reset_rate = false;
+
+	/* VF TX rate limit was not set or not supported */
+	if ((adapter->vf_rate_link_speed == 0) ||
+	    (adapter->hw.mac.type != e1000_82576))
+		return;
+
+	actual_link_speed = igb_link_mbps(adapter->link_speed);
+	if (actual_link_speed != adapter->vf_rate_link_speed) {
+		reset_rate = true;
+		adapter->vf_rate_link_speed = 0;
+		dev_info(&adapter->pdev->dev,
+		         "Link speed has been changed. VF Transmit "
+		         "rate is disabled\n");
+	}
+
+	for (i = 0; i < adapter->vfs_allocated_count; i++) {
+		if (reset_rate)
+			adapter->vf_data[i].tx_rate = 0;
+
+		igb_set_vf_rate_limit(&adapter->hw, i,
+		                      adapter->vf_data[i].tx_rate,
+		                      actual_link_speed);
+	}
+}
+
 static int igb_ndo_set_vf_bw(struct net_device *netdev, int vf, int tx_rate)
 {
-	return -EOPNOTSUPP;
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	int actual_link_speed;
+
+	if (hw->mac.type != e1000_82576)
+		return -EOPNOTSUPP;
+
+	actual_link_speed = igb_link_mbps(adapter->link_speed);
+	if ((vf >= adapter->vfs_allocated_count) ||
+	    (!(rd32(E1000_STATUS) & E1000_STATUS_LU)) ||
+	    (tx_rate < 0) || (tx_rate > actual_link_speed))
+		return -EINVAL;
+
+	adapter->vf_rate_link_speed = actual_link_speed;
+	adapter->vf_data[vf].tx_rate = (u16)tx_rate;
+	igb_set_vf_rate_limit(hw, vf, tx_rate, actual_link_speed);
+
+	return 0;
 }
 
 static int igb_ndo_get_vf_config(struct net_device *netdev,
@@ -6606,7 +6701,7 @@ static int igb_ndo_get_vf_config(struct net_device *netdev,
 		return -EINVAL;
 	ivi->vf = vf;
 	memcpy(&ivi->mac, adapter->vf_data[vf].vf_mac_addresses, ETH_ALEN);
-	ivi->tx_rate = 0;
+	ivi->tx_rate = adapter->vf_data[vf].tx_rate;
 	ivi->vlan = adapter->vf_data[vf].pf_vlan;
 	ivi->qos = adapter->vf_data[vf].pf_qos;
 	return 0;
diff --git a/drivers/net/igbvf/ethtool.c b/drivers/net/igbvf/ethtool.c
index ed6e3d910247..1d943aa7c7a6 100644
--- a/drivers/net/igbvf/ethtool.c
+++ b/drivers/net/igbvf/ethtool.c
@@ -201,13 +201,11 @@ static void igbvf_get_regs(struct net_device *netdev,
 	struct igbvf_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 	u32 *regs_buff = p;
-	u8 revision_id;
 
 	memset(p, 0, IGBVF_REGS_LEN * sizeof(u32));
 
-	pci_read_config_byte(adapter->pdev, PCI_REVISION_ID, &revision_id);
-
-	regs->version = (1 << 24) | (revision_id << 16) | adapter->pdev->device;
+	regs->version = (1 << 24) | (adapter->pdev->revision << 16) |
+			adapter->pdev->device;
 
 	regs_buff[0] = er32(CTRL);
 	regs_buff[1] = er32(STATUS);
diff --git a/drivers/net/igbvf/igbvf.h b/drivers/net/igbvf/igbvf.h
index 990c329e6c3b..d5dad5d607d6 100644
--- a/drivers/net/igbvf/igbvf.h
+++ b/drivers/net/igbvf/igbvf.h
@@ -201,9 +201,6 @@ struct igbvf_adapter {
 	unsigned int restart_queue;
 	u32 txd_cmd;
 
-	bool detect_tx_hung;
-	u8 tx_timeout_factor;
-
 	u32 tx_int_delay;
 	u32 tx_abs_int_delay;
 
diff --git a/drivers/net/igbvf/netdev.c b/drivers/net/igbvf/netdev.c
index 6352c8158e6d..6ccc32fd7338 100644
--- a/drivers/net/igbvf/netdev.c
+++ b/drivers/net/igbvf/netdev.c
@@ -396,35 +396,6 @@ static void igbvf_put_txbuf(struct igbvf_adapter *adapter,
 	buffer_info->time_stamp = 0;
 }
 
-static void igbvf_print_tx_hang(struct igbvf_adapter *adapter)
-{
-	struct igbvf_ring *tx_ring = adapter->tx_ring;
-	unsigned int i = tx_ring->next_to_clean;
-	unsigned int eop = tx_ring->buffer_info[i].next_to_watch;
-	union e1000_adv_tx_desc *eop_desc = IGBVF_TX_DESC_ADV(*tx_ring, eop);
-
-	/* detected Tx unit hang */
-	dev_err(&adapter->pdev->dev,
-	        "Detected Tx Unit Hang:\n"
-	        "  TDH                  <%x>\n"
-	        "  TDT                  <%x>\n"
-	        "  next_to_use          <%x>\n"
-	        "  next_to_clean        <%x>\n"
-	        "buffer_info[next_to_clean]:\n"
-	        "  time_stamp           <%lx>\n"
-	        "  next_to_watch        <%x>\n"
-	        "  jiffies              <%lx>\n"
-	        "  next_to_watch.status <%x>\n",
-	        readl(adapter->hw.hw_addr + tx_ring->head),
-	        readl(adapter->hw.hw_addr + tx_ring->tail),
-	        tx_ring->next_to_use,
-	        tx_ring->next_to_clean,
-	        tx_ring->buffer_info[eop].time_stamp,
-	        eop,
-	        jiffies,
-	        eop_desc->wb.status);
-}
-
 /**
  * igbvf_setup_tx_resources - allocate Tx resources (Descriptors)
  * @adapter: board private structure
@@ -771,7 +742,6 @@ static void igbvf_set_itr(struct igbvf_adapter *adapter)
 static bool igbvf_clean_tx_irq(struct igbvf_ring *tx_ring)
 {
 	struct igbvf_adapter *adapter = tx_ring->adapter;
-	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 	struct igbvf_buffer *buffer_info;
 	struct sk_buff *skb;
@@ -832,22 +802,6 @@ static bool igbvf_clean_tx_irq(struct igbvf_ring *tx_ring)
 		}
 	}
 
-	if (adapter->detect_tx_hung) {
-		/* Detect a transmit hang in hardware, this serializes the
-		 * check with the clearing of time_stamp and movement of i */
-		adapter->detect_tx_hung = false;
-		if (tx_ring->buffer_info[i].time_stamp &&
-		    time_after(jiffies, tx_ring->buffer_info[i].time_stamp +
-		               (adapter->tx_timeout_factor * HZ)) &&
-		    !(er32(STATUS) & E1000_STATUS_TXOFF)) {
-
-			tx_desc = IGBVF_TX_DESC_ADV(*tx_ring, i);
-			/* detected Tx unit hang */
-			igbvf_print_tx_hang(adapter);
-
-			netif_stop_queue(netdev);
-		}
-	}
 	adapter->net_stats.tx_bytes += total_bytes;
 	adapter->net_stats.tx_packets += total_packets;
 	return count < tx_ring->count;
@@ -1863,17 +1817,6 @@ static void igbvf_watchdog_task(struct work_struct *work)
 			                          &adapter->link_duplex);
 			igbvf_print_link_info(adapter);
 
-			/* adjust timeout factor according to speed/duplex */
-			adapter->tx_timeout_factor = 1;
-			switch (adapter->link_speed) {
-			case SPEED_10:
-				adapter->tx_timeout_factor = 16;
-				break;
-			case SPEED_100:
-				/* maybe add some timeout factor ? */
-				break;
-			}
-
 			netif_carrier_on(netdev);
 			netif_wake_queue(netdev);
 		}
@@ -1907,9 +1850,6 @@ static void igbvf_watchdog_task(struct work_struct *work)
 	/* Cause software interrupt to ensure Rx ring is cleaned */
 	ew32(EICS, adapter->rx_ring->eims_value);
 
-	/* Force detection of hung controller every watchdog period */
-	adapter->detect_tx_hung = 1;
-
 	/* Reset the timer */
 	if (!test_bit(__IGBVF_DOWN, &adapter->state))
 		mod_timer(&adapter->watchdog_timer,
@@ -2699,8 +2639,7 @@ static int __devinit igbvf_probe(struct pci_dev *pdev,
 	hw->device_id = pdev->device;
 	hw->subsystem_vendor_id = pdev->subsystem_vendor;
 	hw->subsystem_device_id = pdev->subsystem_device;
-
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &hw->revision_id);
+	hw->revision_id = pdev->revision;
 
 	err = -EIO;
 	adapter->hw.hw_addr = ioremap(pci_resource_start(pdev, 0),
diff --git a/drivers/net/igbvf/vf.c b/drivers/net/igbvf/vf.c
index 74486a8b009a..af3822f9ea9a 100644
--- a/drivers/net/igbvf/vf.c
+++ b/drivers/net/igbvf/vf.c
@@ -220,7 +220,7 @@ static u32 e1000_hash_mc_addr_vf(struct e1000_hw *hw, u8 *mc_addr)
  *  The parameter rar_count will usually be hw->mac.rar_entry_count
  *  unless there are workarounds that change this.
  **/
-void e1000_update_mc_addr_list_vf(struct e1000_hw *hw,
+static void e1000_update_mc_addr_list_vf(struct e1000_hw *hw,
                                   u8 *mc_addr_list, u32 mc_addr_count,
                                   u32 rar_used_count, u32 rar_count)
 {
diff --git a/drivers/net/ipg.c b/drivers/net/ipg.c
index aa93655c3aa7..a5b0f0e194bb 100644
--- a/drivers/net/ipg.c
+++ b/drivers/net/ipg.c
@@ -2025,7 +2025,6 @@ static void ipg_init_mii(struct net_device *dev)
 
 	if (phyaddr != 0x1f) {
 		u16 mii_phyctrl, mii_1000cr;
-		u8 revisionid = 0;
 
 		mii_1000cr  = mdio_read(dev, phyaddr, MII_CTRL1000);
 		mii_1000cr |= ADVERTISE_1000FULL | ADVERTISE_1000HALF |
@@ -2035,8 +2034,7 @@ static void ipg_init_mii(struct net_device *dev)
 		mii_phyctrl = mdio_read(dev, phyaddr, MII_BMCR);
 
 		/* Set default phyparam */
-		pci_read_config_byte(sp->pdev, PCI_REVISION_ID, &revisionid);
-		ipg_set_phy_default_param(revisionid, dev, phyaddr);
+		ipg_set_phy_default_param(sp->pdev->revision, dev, phyaddr);
 
 		/* Reset PHY */
 		mii_phyctrl |= BMCR_RESET | BMCR_ANRESTART;
diff --git a/drivers/net/ixgbe/ixgbe.h b/drivers/net/ixgbe/ixgbe.h
index 3b8c92463617..b60b81bc2b15 100644
--- a/drivers/net/ixgbe/ixgbe.h
+++ b/drivers/net/ixgbe/ixgbe.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -334,6 +334,10 @@ struct ixgbe_adapter {
 	u16 bd_number;
 	struct work_struct reset_task;
 	struct ixgbe_q_vector *q_vector[MAX_MSIX_Q_VECTORS];
+
+	/* DCB parameters */
+	struct ieee_pfc *ixgbe_ieee_pfc;
+	struct ieee_ets *ixgbe_ieee_ets;
 	struct ixgbe_dcb_config dcb_cfg;
 	struct ixgbe_dcb_config temp_dcb_cfg;
 	u8 dcb_set_bitmap;
@@ -521,7 +525,6 @@ extern void ixgbe_unmap_and_free_tx_resource(struct ixgbe_ring *,
 extern void ixgbe_alloc_rx_buffers(struct ixgbe_ring *, u16);
 extern void ixgbe_write_eitr(struct ixgbe_q_vector *);
 extern int ethtool_ioctl(struct ifreq *ifr);
-extern u8 ixgbe_dcb_txq_to_tc(struct ixgbe_adapter *adapter, u8 index);
 extern s32 ixgbe_reinit_fdir_tables_82599(struct ixgbe_hw *hw);
 extern s32 ixgbe_init_fdir_signature_82599(struct ixgbe_hw *hw, u32 pballoc);
 extern s32 ixgbe_init_fdir_perfect_82599(struct ixgbe_hw *hw, u32 pballoc);
diff --git a/drivers/net/ixgbe/ixgbe_82598.c b/drivers/net/ixgbe/ixgbe_82598.c
index d0f1d9d2c416..fc41329399be 100644
--- a/drivers/net/ixgbe/ixgbe_82598.c
+++ b/drivers/net/ixgbe/ixgbe_82598.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -627,7 +627,6 @@ out:
 	return 0;
 }
 
-
 /**
  *  ixgbe_setup_mac_link_82598 - Set MAC link speed
  *  @hw: pointer to hardware structure
@@ -698,7 +697,6 @@ static s32 ixgbe_setup_copper_link_82598(struct ixgbe_hw *hw,
 	/* Setup the PHY according to input speed */
 	status = hw->phy.ops.setup_link_speed(hw, speed, autoneg,
 	                                      autoneg_wait_to_complete);
-
 	/* Set up MAC */
 	ixgbe_start_mac_link_82598(hw, autoneg_wait_to_complete);
 
@@ -770,7 +768,6 @@ static s32 ixgbe_reset_hw_82598(struct ixgbe_hw *hw)
 		else if (phy_status == IXGBE_ERR_SFP_NOT_PRESENT)
 			goto no_phy_reset;
 
-
 		hw->phy.ops.reset(hw);
 	}
 
@@ -779,12 +776,9 @@ no_phy_reset:
 	 * Prevent the PCI-E bus from from hanging by disabling PCI-E master
 	 * access and verify no pending requests before reset
 	 */
-	status = ixgbe_disable_pcie_master(hw);
-	if (status != 0) {
-		status = IXGBE_ERR_MASTER_REQUESTS_PENDING;
-		hw_dbg(hw, "PCI-E Master disable polling has failed.\n");
-	}
+	ixgbe_disable_pcie_master(hw);
 
+mac_reset_top:
 	/*
 	 * Issue global reset to the MAC.  This needs to be a SW reset.
 	 * If link reset is used, it might reset the MAC when mng is using it
@@ -805,6 +799,19 @@ no_phy_reset:
 		hw_dbg(hw, "Reset polling failed to complete.\n");
 	}
 
+	/*
+	 * Double resets are required for recovery from certain error
+	 * conditions.  Between resets, it is necessary to stall to allow time
+	 * for any pending HW events to complete.  We use 1usec since that is
+	 * what is needed for ixgbe_disable_pcie_master().  The second reset
+	 * then clears out any effects of those events.
+	 */
+	if (hw->mac.flags & IXGBE_FLAGS_DOUBLE_RESET_REQUIRED) {
+		hw->mac.flags &= ~IXGBE_FLAGS_DOUBLE_RESET_REQUIRED;
+		udelay(1);
+		goto mac_reset_top;
+	}
+
 	msleep(50);
 
 	gheccr = IXGBE_READ_REG(hw, IXGBE_GHECCR);
@@ -824,15 +831,15 @@ no_phy_reset:
 		IXGBE_WRITE_REG(hw, IXGBE_AUTOC, hw->mac.orig_autoc);
 	}
 
+	/* Store the permanent mac address */
+	hw->mac.ops.get_mac_addr(hw, hw->mac.perm_addr);
+
 	/*
 	 * Store MAC address from RAR0, clear receive address registers, and
 	 * clear the multicast table
 	 */
 	hw->mac.ops.init_rx_addrs(hw);
 
-	/* Store the permanent mac address */
-	hw->mac.ops.get_mac_addr(hw, hw->mac.perm_addr);
-
 reset_hw_out:
 	if (phy_status)
 		status = phy_status;
@@ -849,6 +856,13 @@ reset_hw_out:
 static s32 ixgbe_set_vmdq_82598(struct ixgbe_hw *hw, u32 rar, u32 vmdq)
 {
 	u32 rar_high;
+	u32 rar_entries = hw->mac.num_rar_entries;
+
+	/* Make sure we are using a valid rar index range */
+	if (rar >= rar_entries) {
+		hw_dbg(hw, "RAR index %d is out of range.\n", rar);
+		return IXGBE_ERR_INVALID_ARGUMENT;
+	}
 
 	rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(rar));
 	rar_high &= ~IXGBE_RAH_VIND_MASK;
@@ -868,14 +882,17 @@ static s32 ixgbe_clear_vmdq_82598(struct ixgbe_hw *hw, u32 rar, u32 vmdq)
 	u32 rar_high;
 	u32 rar_entries = hw->mac.num_rar_entries;
 
-	if (rar < rar_entries) {
-		rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(rar));
-		if (rar_high & IXGBE_RAH_VIND_MASK) {
-			rar_high &= ~IXGBE_RAH_VIND_MASK;
-			IXGBE_WRITE_REG(hw, IXGBE_RAH(rar), rar_high);
-		}
-	} else {
+
+	/* Make sure we are using a valid rar index range */
+	if (rar >= rar_entries) {
 		hw_dbg(hw, "RAR index %d is out of range.\n", rar);
+		return IXGBE_ERR_INVALID_ARGUMENT;
+	}
+
+	rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(rar));
+	if (rar_high & IXGBE_RAH_VIND_MASK) {
+		rar_high &= ~IXGBE_RAH_VIND_MASK;
+		IXGBE_WRITE_REG(hw, IXGBE_RAH(rar), rar_high);
 	}
 
 	return 0;
@@ -994,13 +1011,12 @@ static s32 ixgbe_write_analog_reg8_82598(struct ixgbe_hw *hw, u32 reg, u8 val)
 }
 
 /**
- *  ixgbe_read_i2c_eeprom_82598 - Read 8 bit EEPROM word of an SFP+ module
- *  over I2C interface through an intermediate phy.
+ *  ixgbe_read_i2c_eeprom_82598 - Reads 8 bit word over I2C interface.
  *  @hw: pointer to hardware structure
  *  @byte_offset: EEPROM byte offset to read
  *  @eeprom_data: value read
  *
- *  Performs byte read operation to SFP module's EEPROM over I2C interface.
+ *  Performs 8 byte read operation to SFP module's EEPROM over I2C interface.
  **/
 static s32 ixgbe_read_i2c_eeprom_82598(struct ixgbe_hw *hw, u8 byte_offset,
 				       u8 *eeprom_data)
@@ -1179,13 +1195,14 @@ static struct ixgbe_mac_operations mac_ops_82598 = {
 	.set_vmdq		= &ixgbe_set_vmdq_82598,
 	.clear_vmdq		= &ixgbe_clear_vmdq_82598,
 	.init_rx_addrs		= &ixgbe_init_rx_addrs_generic,
-	.update_uc_addr_list	= &ixgbe_update_uc_addr_list_generic,
 	.update_mc_addr_list	= &ixgbe_update_mc_addr_list_generic,
 	.enable_mc		= &ixgbe_enable_mc_generic,
 	.disable_mc		= &ixgbe_disable_mc_generic,
 	.clear_vfta		= &ixgbe_clear_vfta_82598,
 	.set_vfta		= &ixgbe_set_vfta_82598,
 	.fc_enable		= &ixgbe_fc_enable_82598,
+	.acquire_swfw_sync      = &ixgbe_acquire_swfw_sync,
+	.release_swfw_sync      = &ixgbe_release_swfw_sync,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_82598 = {
diff --git a/drivers/net/ixgbe/ixgbe_82599.c b/drivers/net/ixgbe/ixgbe_82599.c
index a21f5817685b..5ef968a10d42 100644
--- a/drivers/net/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ixgbe/ixgbe_82599.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -112,7 +112,8 @@ static s32 ixgbe_setup_sfp_modules_82599(struct ixgbe_hw *hw)
 			goto setup_sfp_out;
 
 		/* PHY config will finish before releasing the semaphore */
-		ret_val = ixgbe_acquire_swfw_sync(hw, IXGBE_GSSR_MAC_CSR_SM);
+		ret_val = hw->mac.ops.acquire_swfw_sync(hw,
+		                                        IXGBE_GSSR_MAC_CSR_SM);
 		if (ret_val != 0) {
 			ret_val = IXGBE_ERR_SWFW_SYNC;
 			goto setup_sfp_out;
@@ -329,11 +330,14 @@ static enum ixgbe_media_type ixgbe_get_media_type_82599(struct ixgbe_hw *hw)
 	enum ixgbe_media_type media_type;
 
 	/* Detect if there is a copper PHY attached. */
-	if (hw->phy.type == ixgbe_phy_cu_unknown ||
-	    hw->phy.type == ixgbe_phy_tn ||
-	    hw->phy.type == ixgbe_phy_aq) {
+	switch (hw->phy.type) {
+	case ixgbe_phy_cu_unknown:
+	case ixgbe_phy_tn:
+	case ixgbe_phy_aq:
 		media_type = ixgbe_media_type_copper;
 		goto out;
+	default:
+		break;
 	}
 
 	switch (hw->device_id) {
@@ -354,6 +358,9 @@ static enum ixgbe_media_type ixgbe_get_media_type_82599(struct ixgbe_hw *hw)
 	case IXGBE_DEV_ID_82599_CX4:
 		media_type = ixgbe_media_type_cx4;
 		break;
+	case IXGBE_DEV_ID_82599_T3_LOM:
+		media_type = ixgbe_media_type_copper;
+		break;
 	default:
 		media_type = ixgbe_media_type_unknown;
 		break;
@@ -411,14 +418,14 @@ static s32 ixgbe_start_mac_link_82599(struct ixgbe_hw *hw,
 	return status;
 }
 
- /**
-  *  ixgbe_disable_tx_laser_multispeed_fiber - Disable Tx laser
-  *  @hw: pointer to hardware structure
-  *
-  *  The base drivers may require better control over SFP+ module
-  *  PHY states.  This includes selectively shutting down the Tx
-  *  laser on the PHY, effectively halting physical link.
-  **/
+/**
+ *  ixgbe_disable_tx_laser_multispeed_fiber - Disable Tx laser
+ *  @hw: pointer to hardware structure
+ *
+ *  The base drivers may require better control over SFP+ module
+ *  PHY states.  This includes selectively shutting down the Tx
+ *  laser on the PHY, effectively halting physical link.
+ **/
 static void ixgbe_disable_tx_laser_multispeed_fiber(struct ixgbe_hw *hw)
 {
 	u32 esdp_reg = IXGBE_READ_REG(hw, IXGBE_ESDP);
@@ -536,7 +543,6 @@ s32 ixgbe_setup_mac_link_multispeed_fiber(struct ixgbe_hw *hw,
 		 * Section 73.10.2, we may have to wait up to 500ms if KR is
 		 * attempted.  82599 uses the same timing for 10g SFI.
 		 */
-
 		for (i = 0; i < 5; i++) {
 			/* Wait for the link partner to also set speed */
 			msleep(100);
@@ -761,7 +767,6 @@ static s32 ixgbe_setup_mac_link_82599(struct ixgbe_hw *hw,
 	else
 		orig_autoc = autoc;
 
-
 	if (link_mode == IXGBE_AUTOC_LMS_KX4_KX_KR ||
 	    link_mode == IXGBE_AUTOC_LMS_KX4_KX_KR_1G_AN ||
 	    link_mode == IXGBE_AUTOC_LMS_KX4_KX_KR_SGMII) {
@@ -898,12 +903,9 @@ static s32 ixgbe_reset_hw_82599(struct ixgbe_hw *hw)
 	 * Prevent the PCI-E bus from from hanging by disabling PCI-E master
 	 * access and verify no pending requests before reset
 	 */
-	status = ixgbe_disable_pcie_master(hw);
-	if (status != 0) {
-		status = IXGBE_ERR_MASTER_REQUESTS_PENDING;
-		hw_dbg(hw, "PCI-E Master disable polling has failed.\n");
-	}
+	ixgbe_disable_pcie_master(hw);
 
+mac_reset_top:
 	/*
 	 * Issue global reset to the MAC.  This needs to be a SW reset.
 	 * If link reset is used, it might reset the MAC when mng is using it
@@ -924,6 +926,19 @@ static s32 ixgbe_reset_hw_82599(struct ixgbe_hw *hw)
 		hw_dbg(hw, "Reset polling failed to complete.\n");
 	}
 
+	/*
+	 * Double resets are required for recovery from certain error
+	 * conditions.  Between resets, it is necessary to stall to allow time
+	 * for any pending HW events to complete.  We use 1usec since that is
+	 * what is needed for ixgbe_disable_pcie_master().  The second reset
+	 * then clears out any effects of those events.
+	 */
+	if (hw->mac.flags & IXGBE_FLAGS_DOUBLE_RESET_REQUIRED) {
+		hw->mac.flags &= ~IXGBE_FLAGS_DOUBLE_RESET_REQUIRED;
+		udelay(1);
+		goto mac_reset_top;
+	}
+
 	msleep(50);
 
 	/*
@@ -951,6 +966,9 @@ static s32 ixgbe_reset_hw_82599(struct ixgbe_hw *hw)
 		}
 	}
 
+	/* Store the permanent mac address */
+	hw->mac.ops.get_mac_addr(hw, hw->mac.perm_addr);
+
 	/*
 	 * Store MAC address from RAR0, clear receive address registers, and
 	 * clear the multicast table.  Also reset num_rar_entries to 128,
@@ -959,9 +977,6 @@ static s32 ixgbe_reset_hw_82599(struct ixgbe_hw *hw)
 	hw->mac.num_rar_entries = 128;
 	hw->mac.ops.init_rx_addrs(hw);
 
-	/* Store the permanent mac address */
-	hw->mac.ops.get_mac_addr(hw, hw->mac.perm_addr);
-
 	/* Store the permanent SAN mac address */
 	hw->mac.ops.get_san_mac_addr(hw, hw->mac.san_addr);
 
@@ -1733,13 +1748,34 @@ static s32 ixgbe_start_hw_82599(struct ixgbe_hw *hw)
  *  @hw: pointer to hardware structure
  *
  *  Determines the physical layer module found on the current adapter.
+ *  If PHY already detected, maintains current PHY type in hw struct,
+ *  otherwise executes the PHY detection routine.
  **/
-static s32 ixgbe_identify_phy_82599(struct ixgbe_hw *hw)
+s32 ixgbe_identify_phy_82599(struct ixgbe_hw *hw)
 {
 	s32 status = IXGBE_ERR_PHY_ADDR_INVALID;
+
+	/* Detect PHY if not unknown - returns success if already detected. */
 	status = ixgbe_identify_phy_generic(hw);
-	if (status != 0)
-		status = ixgbe_identify_sfp_module_generic(hw);
+	if (status != 0) {
+		/* 82599 10GBASE-T requires an external PHY */
+		if (hw->mac.ops.get_media_type(hw) == ixgbe_media_type_copper)
+			goto out;
+		else
+			status = ixgbe_identify_sfp_module_generic(hw);
+	}
+
+	/* Set PHY type none if no PHY detected */
+	if (hw->phy.type == ixgbe_phy_unknown) {
+		hw->phy.type = ixgbe_phy_none;
+		status = 0;
+	}
+
+	/* Return error if SFP module has been detected but is not supported */
+	if (hw->phy.type == ixgbe_phy_sfp_unsupported)
+		status = IXGBE_ERR_SFP_NOT_SUPPORTED;
+
+out:
 	return status;
 }
 
@@ -1763,11 +1799,12 @@ static u32 ixgbe_get_supported_physical_layer_82599(struct ixgbe_hw *hw)
 
 	hw->phy.ops.identify(hw);
 
-	if (hw->phy.type == ixgbe_phy_tn ||
-	    hw->phy.type == ixgbe_phy_aq ||
-	    hw->phy.type == ixgbe_phy_cu_unknown) {
+	switch (hw->phy.type) {
+	case ixgbe_phy_tn:
+	case ixgbe_phy_aq:
+	case ixgbe_phy_cu_unknown:
 		hw->phy.ops.read_reg(hw, MDIO_PMA_EXTABLE, MDIO_MMD_PMAPMD,
-				     &ext_ability);
+							 &ext_ability);
 		if (ext_ability & MDIO_PMA_EXTABLE_10GBT)
 			physical_layer |= IXGBE_PHYSICAL_LAYER_10GBASE_T;
 		if (ext_ability & MDIO_PMA_EXTABLE_1000BT)
@@ -1775,6 +1812,8 @@ static u32 ixgbe_get_supported_physical_layer_82599(struct ixgbe_hw *hw)
 		if (ext_ability & MDIO_PMA_EXTABLE_100BTX)
 			physical_layer |= IXGBE_PHYSICAL_LAYER_100BASE_TX;
 		goto out;
+	default:
+		break;
 	}
 
 	switch (autoc & IXGBE_AUTOC_LMS_MASK) {
@@ -1886,6 +1925,7 @@ static s32 ixgbe_enable_rx_dma_82599(struct ixgbe_hw *hw, u32 regval)
 		if (secrxreg & IXGBE_SECRXSTAT_SECRX_RDY)
 			break;
 		else
+			/* Use interrupt-safe sleep just in case */
 			udelay(10);
 	}
 
@@ -1995,7 +2035,6 @@ static struct ixgbe_mac_operations mac_ops_82599 = {
 	.set_vmdq               = &ixgbe_set_vmdq_generic,
 	.clear_vmdq             = &ixgbe_clear_vmdq_generic,
 	.init_rx_addrs          = &ixgbe_init_rx_addrs_generic,
-	.update_uc_addr_list    = &ixgbe_update_uc_addr_list_generic,
 	.update_mc_addr_list    = &ixgbe_update_mc_addr_list_generic,
 	.enable_mc              = &ixgbe_enable_mc_generic,
 	.disable_mc             = &ixgbe_disable_mc_generic,
@@ -2006,6 +2045,9 @@ static struct ixgbe_mac_operations mac_ops_82599 = {
 	.setup_sfp              = &ixgbe_setup_sfp_modules_82599,
 	.set_mac_anti_spoofing  = &ixgbe_set_mac_anti_spoofing,
 	.set_vlan_anti_spoofing = &ixgbe_set_vlan_anti_spoofing,
+	.acquire_swfw_sync      = &ixgbe_acquire_swfw_sync,
+	.release_swfw_sync      = &ixgbe_release_swfw_sync,
+
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_82599 = {
diff --git a/drivers/net/ixgbe/ixgbe_common.c b/drivers/net/ixgbe/ixgbe_common.c
index ebbda7d15254..a7fb2e00f766 100644
--- a/drivers/net/ixgbe/ixgbe_common.c
+++ b/drivers/net/ixgbe/ixgbe_common.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -46,10 +46,7 @@ static void ixgbe_raise_eeprom_clk(struct ixgbe_hw *hw, u32 *eec);
 static void ixgbe_lower_eeprom_clk(struct ixgbe_hw *hw, u32 *eec);
 static void ixgbe_release_eeprom(struct ixgbe_hw *hw);
 
-static void ixgbe_enable_rar(struct ixgbe_hw *hw, u32 index);
-static void ixgbe_disable_rar(struct ixgbe_hw *hw, u32 index);
 static s32 ixgbe_mta_vector(struct ixgbe_hw *hw, u8 *mc_addr);
-static void ixgbe_add_uc_addr(struct ixgbe_hw *hw, u8 *addr, u32 vmdq);
 static s32 ixgbe_setup_fc(struct ixgbe_hw *hw, s32 packetbuf_num);
 
 /**
@@ -454,8 +451,7 @@ s32 ixgbe_stop_adapter_generic(struct ixgbe_hw *hw)
 	 * Prevent the PCI-E bus from from hanging by disabling PCI-E master
 	 * access and verify no pending requests
 	 */
-	if (ixgbe_disable_pcie_master(hw) != 0)
-		hw_dbg(hw, "PCI-E Master disable polling has failed.\n");
+	ixgbe_disable_pcie_master(hw);
 
 	return 0;
 }
@@ -603,7 +599,6 @@ s32 ixgbe_write_eeprom_generic(struct ixgbe_hw *hw, u16 offset, u16 data)
 		ixgbe_shift_out_eeprom_bits(hw, data, 16);
 		ixgbe_standby_eeprom(hw);
 
-		msleep(hw->eeprom.semaphore_delay);
 		/* Done with writing - release the EEPROM */
 		ixgbe_release_eeprom(hw);
 	}
@@ -747,10 +742,10 @@ s32 ixgbe_poll_eerd_eewr_done(struct ixgbe_hw *hw, u32 ee_reg)
 static s32 ixgbe_acquire_eeprom(struct ixgbe_hw *hw)
 {
 	s32 status = 0;
-	u32 eec = 0;
+	u32 eec;
 	u32 i;
 
-	if (ixgbe_acquire_swfw_sync(hw, IXGBE_GSSR_EEP_SM) != 0)
+	if (hw->mac.ops.acquire_swfw_sync(hw, IXGBE_GSSR_EEP_SM) != 0)
 		status = IXGBE_ERR_SWFW_SYNC;
 
 	if (status == 0) {
@@ -773,18 +768,18 @@ static s32 ixgbe_acquire_eeprom(struct ixgbe_hw *hw)
 			IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
 			hw_dbg(hw, "Could not acquire EEPROM grant\n");
 
-			ixgbe_release_swfw_sync(hw, IXGBE_GSSR_EEP_SM);
+			hw->mac.ops.release_swfw_sync(hw, IXGBE_GSSR_EEP_SM);
 			status = IXGBE_ERR_EEPROM;
 		}
-	}
 
-	/* Setup EEPROM for Read/Write */
-	if (status == 0) {
-		/* Clear CS and SK */
-		eec &= ~(IXGBE_EEC_CS | IXGBE_EEC_SK);
-		IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
-		IXGBE_WRITE_FLUSH(hw);
-		udelay(1);
+		/* Setup EEPROM for Read/Write */
+		if (status == 0) {
+			/* Clear CS and SK */
+			eec &= ~(IXGBE_EEC_CS | IXGBE_EEC_SK);
+			IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+			IXGBE_WRITE_FLUSH(hw);
+			udelay(1);
+		}
 	}
 	return status;
 }
@@ -798,13 +793,10 @@ static s32 ixgbe_acquire_eeprom(struct ixgbe_hw *hw)
 static s32 ixgbe_get_eeprom_semaphore(struct ixgbe_hw *hw)
 {
 	s32 status = IXGBE_ERR_EEPROM;
-	u32 timeout;
+	u32 timeout = 2000;
 	u32 i;
 	u32 swsm;
 
-	/* Set timeout value based on size of EEPROM */
-	timeout = hw->eeprom.word_size + 1;
-
 	/* Get SMBI software semaphore between device drivers first */
 	for (i = 0; i < timeout; i++) {
 		/*
@@ -816,7 +808,7 @@ static s32 ixgbe_get_eeprom_semaphore(struct ixgbe_hw *hw)
 			status = 0;
 			break;
 		}
-		msleep(1);
+		udelay(50);
 	}
 
 	/* Now get the semaphore between SW/FW through the SWESMBI bit */
@@ -844,11 +836,14 @@ static s32 ixgbe_get_eeprom_semaphore(struct ixgbe_hw *hw)
 		 * was not granted because we don't have access to the EEPROM
 		 */
 		if (i >= timeout) {
-			hw_dbg(hw, "Driver can't access the Eeprom - Semaphore "
+			hw_dbg(hw, "SWESMBI Software EEPROM semaphore "
 			       "not granted.\n");
 			ixgbe_release_eeprom_semaphore(hw);
 			status = IXGBE_ERR_EEPROM;
 		}
+	} else {
+		hw_dbg(hw, "Software semaphore SMBI between device drivers "
+		       "not granted.\n");
 	}
 
 	return status;
@@ -1081,10 +1076,13 @@ static void ixgbe_release_eeprom(struct ixgbe_hw *hw)
 	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
 
 	ixgbe_release_swfw_sync(hw, IXGBE_GSSR_EEP_SM);
+
+	/* Delay before attempt to obtain semaphore again to allow FW access */
+	msleep(hw->eeprom.semaphore_delay);
 }
 
 /**
- *  ixgbe_calc_eeprom_checksum - Calculates and returns the checksum
+ *  ixgbe_calc_eeprom_checksum_generic - Calculates and returns the checksum
  *  @hw: pointer to hardware structure
  **/
 u16 ixgbe_calc_eeprom_checksum_generic(struct ixgbe_hw *hw)
@@ -1190,7 +1188,7 @@ s32 ixgbe_update_eeprom_checksum_generic(struct ixgbe_hw *hw)
 	if (status == 0) {
 		checksum = hw->eeprom.ops.calc_checksum(hw);
 		status = hw->eeprom.ops.write(hw, IXGBE_EEPROM_CHECKSUM,
-		                            checksum);
+					      checksum);
 	} else {
 		hw_dbg(hw, "EEPROM read failed\n");
 	}
@@ -1238,37 +1236,37 @@ s32 ixgbe_set_rar_generic(struct ixgbe_hw *hw, u32 index, u8 *addr, u32 vmdq,
 	u32 rar_low, rar_high;
 	u32 rar_entries = hw->mac.num_rar_entries;
 
+	/* Make sure we are using a valid rar index range */
+	if (index >= rar_entries) {
+		hw_dbg(hw, "RAR index %d is out of range.\n", index);
+		return IXGBE_ERR_INVALID_ARGUMENT;
+	}
+
 	/* setup VMDq pool selection before this RAR gets enabled */
 	hw->mac.ops.set_vmdq(hw, index, vmdq);
 
-	/* Make sure we are using a valid rar index range */
-	if (index < rar_entries) {
-		/*
-		 * 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));
-		/*
-		 * Some parts put the VMDq setting in the extra RAH bits,
-		 * so save everything except the lower 16 bits that hold part
-		 * of the address and the address valid bit.
-		 */
-		rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(index));
-		rar_high &= ~(0x0000FFFF | IXGBE_RAH_AV);
-		rar_high |= ((u32)addr[4] | ((u32)addr[5] << 8));
+	/*
+	 * 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));
+	/*
+	 * Some parts put the VMDq setting in the extra RAH bits,
+	 * so save everything except the lower 16 bits that hold part
+	 * of the address and the address valid bit.
+	 */
+	rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(index));
+	rar_high &= ~(0x0000FFFF | IXGBE_RAH_AV);
+	rar_high |= ((u32)addr[4] | ((u32)addr[5] << 8));
 
-		if (enable_addr != 0)
-			rar_high |= IXGBE_RAH_AV;
+	if (enable_addr != 0)
+		rar_high |= IXGBE_RAH_AV;
 
-		IXGBE_WRITE_REG(hw, IXGBE_RAL(index), rar_low);
-		IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
-	} else {
-		hw_dbg(hw, "RAR index %d is out of range.\n", index);
-		return IXGBE_ERR_RAR_INDEX;
-	}
+	IXGBE_WRITE_REG(hw, IXGBE_RAL(index), rar_low);
+	IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
 
 	return 0;
 }
@@ -1286,58 +1284,26 @@ s32 ixgbe_clear_rar_generic(struct ixgbe_hw *hw, u32 index)
 	u32 rar_entries = hw->mac.num_rar_entries;
 
 	/* Make sure we are using a valid rar index range */
-	if (index < rar_entries) {
-		/*
-		 * Some parts put the VMDq setting in the extra RAH bits,
-		 * so save everything except the lower 16 bits that hold part
-		 * of the address and the address valid bit.
-		 */
-		rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(index));
-		rar_high &= ~(0x0000FFFF | IXGBE_RAH_AV);
-
-		IXGBE_WRITE_REG(hw, IXGBE_RAL(index), 0);
-		IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
-	} else {
+	if (index >= rar_entries) {
 		hw_dbg(hw, "RAR index %d is out of range.\n", index);
-		return IXGBE_ERR_RAR_INDEX;
+		return IXGBE_ERR_INVALID_ARGUMENT;
 	}
 
-	/* clear VMDq pool/queue selection for this RAR */
-	hw->mac.ops.clear_vmdq(hw, index, IXGBE_CLEAR_VMDQ_ALL);
-
-	return 0;
-}
-
-/**
- *  ixgbe_enable_rar - Enable Rx address register
- *  @hw: pointer to hardware structure
- *  @index: index into the RAR table
- *
- *  Enables the select receive address register.
- **/
-static void ixgbe_enable_rar(struct ixgbe_hw *hw, u32 index)
-{
-	u32 rar_high;
-
+	/*
+	 * Some parts put the VMDq setting in the extra RAH bits,
+	 * so save everything except the lower 16 bits that hold part
+	 * of the address and the address valid bit.
+	 */
 	rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(index));
-	rar_high |= IXGBE_RAH_AV;
+	rar_high &= ~(0x0000FFFF | IXGBE_RAH_AV);
+
+	IXGBE_WRITE_REG(hw, IXGBE_RAL(index), 0);
 	IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
-}
 
-/**
- *  ixgbe_disable_rar - Disable Rx address register
- *  @hw: pointer to hardware structure
- *  @index: index into the RAR table
- *
- *  Disables the select receive address register.
- **/
-static void ixgbe_disable_rar(struct ixgbe_hw *hw, u32 index)
-{
-	u32 rar_high;
+	/* clear VMDq pool/queue selection for this RAR */
+	hw->mac.ops.clear_vmdq(hw, index, IXGBE_CLEAR_VMDQ_ALL);
 
-	rar_high = IXGBE_READ_REG(hw, IXGBE_RAH(index));
-	rar_high &= (~IXGBE_RAH_AV);
-	IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
+	return 0;
 }
 
 /**
@@ -1386,7 +1352,6 @@ s32 ixgbe_init_rx_addrs_generic(struct ixgbe_hw *hw)
 	}
 
 	/* Clear the MTA */
-	hw->addr_ctrl.mc_addr_in_rar_count = 0;
 	hw->addr_ctrl.mta_in_use = 0;
 	IXGBE_WRITE_REG(hw, IXGBE_MCSTCTRL, hw->mac.mc_filter_type);
 
@@ -1401,105 +1366,6 @@ s32 ixgbe_init_rx_addrs_generic(struct ixgbe_hw *hw)
 }
 
 /**
- *  ixgbe_add_uc_addr - Adds a secondary unicast address.
- *  @hw: pointer to hardware structure
- *  @addr: new address
- *
- *  Adds it to unused receive address register or goes into promiscuous mode.
- **/
-static void ixgbe_add_uc_addr(struct ixgbe_hw *hw, u8 *addr, u32 vmdq)
-{
-	u32 rar_entries = hw->mac.num_rar_entries;
-	u32 rar;
-
-	hw_dbg(hw, " UC Addr = %.2X %.2X %.2X %.2X %.2X %.2X\n",
-	          addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
-
-	/*
-	 * Place this address in the RAR if there is room,
-	 * else put the controller into promiscuous mode
-	 */
-	if (hw->addr_ctrl.rar_used_count < rar_entries) {
-		rar = hw->addr_ctrl.rar_used_count -
-		      hw->addr_ctrl.mc_addr_in_rar_count;
-		hw->mac.ops.set_rar(hw, rar, addr, vmdq, IXGBE_RAH_AV);
-		hw_dbg(hw, "Added a secondary address to RAR[%d]\n", rar);
-		hw->addr_ctrl.rar_used_count++;
-	} else {
-		hw->addr_ctrl.overflow_promisc++;
-	}
-
-	hw_dbg(hw, "ixgbe_add_uc_addr Complete\n");
-}
-
-/**
- *  ixgbe_update_uc_addr_list_generic - Updates MAC list of secondary addresses
- *  @hw: pointer to hardware structure
- *  @netdev: pointer to net device structure
- *
- *  The given list replaces any existing list.  Clears the secondary addrs from
- *  receive address registers.  Uses unused receive address registers for the
- *  first secondary addresses, and falls back to promiscuous mode as needed.
- *
- *  Drivers using secondary unicast addresses must set user_set_promisc when
- *  manually putting the device into promiscuous mode.
- **/
-s32 ixgbe_update_uc_addr_list_generic(struct ixgbe_hw *hw,
-				      struct net_device *netdev)
-{
-	u32 i;
-	u32 old_promisc_setting = hw->addr_ctrl.overflow_promisc;
-	u32 uc_addr_in_use;
-	u32 fctrl;
-	struct netdev_hw_addr *ha;
-
-	/*
-	 * Clear accounting of old secondary address list,
-	 * don't count RAR[0]
-	 */
-	uc_addr_in_use = hw->addr_ctrl.rar_used_count - 1;
-	hw->addr_ctrl.rar_used_count -= uc_addr_in_use;
-	hw->addr_ctrl.overflow_promisc = 0;
-
-	/* Zero out the other receive addresses */
-	hw_dbg(hw, "Clearing RAR[1-%d]\n", uc_addr_in_use + 1);
-	for (i = 0; i < uc_addr_in_use; i++) {
-		IXGBE_WRITE_REG(hw, IXGBE_RAL(1+i), 0);
-		IXGBE_WRITE_REG(hw, IXGBE_RAH(1+i), 0);
-	}
-
-	/* Add the new addresses */
-	netdev_for_each_uc_addr(ha, netdev) {
-		hw_dbg(hw, " Adding the secondary addresses:\n");
-		ixgbe_add_uc_addr(hw, ha->addr, 0);
-	}
-
-	if (hw->addr_ctrl.overflow_promisc) {
-		/* enable promisc if not already in overflow or set by user */
-		if (!old_promisc_setting && !hw->addr_ctrl.user_set_promisc) {
-			hw_dbg(hw, " Entering address overflow promisc mode\n");
-			fctrl = IXGBE_READ_REG(hw, IXGBE_FCTRL);
-			fctrl |= IXGBE_FCTRL_UPE;
-			IXGBE_WRITE_REG(hw, IXGBE_FCTRL, fctrl);
-			hw->addr_ctrl.uc_set_promisc = true;
-		}
-	} else {
-		/* only disable if set by overflow, not by user */
-		if ((old_promisc_setting && hw->addr_ctrl.uc_set_promisc) &&
-		   !(hw->addr_ctrl.user_set_promisc)) {
-			hw_dbg(hw, " Leaving address overflow promisc mode\n");
-			fctrl = IXGBE_READ_REG(hw, IXGBE_FCTRL);
-			fctrl &= ~IXGBE_FCTRL_UPE;
-			IXGBE_WRITE_REG(hw, IXGBE_FCTRL, fctrl);
-			hw->addr_ctrl.uc_set_promisc = false;
-		}
-	}
-
-	hw_dbg(hw, "ixgbe_update_uc_addr_list_generic Complete\n");
-	return 0;
-}
-
-/**
  *  ixgbe_mta_vector - Determines bit-vector in multicast table to set
  *  @hw: pointer to hardware structure
  *  @mc_addr: the multicast address
@@ -1550,7 +1416,6 @@ static void ixgbe_set_mta(struct ixgbe_hw *hw, u8 *mc_addr)
 	u32 vector;
 	u32 vector_bit;
 	u32 vector_reg;
-	u32 mta_reg;
 
 	hw->addr_ctrl.mta_in_use++;
 
@@ -1568,9 +1433,7 @@ static void ixgbe_set_mta(struct ixgbe_hw *hw, u8 *mc_addr)
 	 */
 	vector_reg = (vector >> 5) & 0x7F;
 	vector_bit = vector & 0x1F;
-	mta_reg = IXGBE_READ_REG(hw, IXGBE_MTA(vector_reg));
-	mta_reg |= (1 << vector_bit);
-	IXGBE_WRITE_REG(hw, IXGBE_MTA(vector_reg), mta_reg);
+	hw->mac.mta_shadow[vector_reg] |= (1 << vector_bit);
 }
 
 /**
@@ -1596,18 +1459,21 @@ s32 ixgbe_update_mc_addr_list_generic(struct ixgbe_hw *hw,
 	hw->addr_ctrl.num_mc_addrs = netdev_mc_count(netdev);
 	hw->addr_ctrl.mta_in_use = 0;
 
-	/* Clear the MTA */
+	/* Clear mta_shadow */
 	hw_dbg(hw, " Clearing MTA\n");
-	for (i = 0; i < hw->mac.mcft_size; i++)
-		IXGBE_WRITE_REG(hw, IXGBE_MTA(i), 0);
+	memset(&hw->mac.mta_shadow, 0, sizeof(hw->mac.mta_shadow));
 
-	/* Add the new addresses */
+	/* Update mta shadow */
 	netdev_for_each_mc_addr(ha, netdev) {
 		hw_dbg(hw, " Adding the multicast addresses:\n");
 		ixgbe_set_mta(hw, ha->addr);
 	}
 
 	/* Enable mta */
+	for (i = 0; i < hw->mac.mcft_size; i++)
+		IXGBE_WRITE_REG_ARRAY(hw, IXGBE_MTA(0), i,
+				      hw->mac.mta_shadow[i]);
+
 	if (hw->addr_ctrl.mta_in_use > 0)
 		IXGBE_WRITE_REG(hw, IXGBE_MCSTCTRL,
 		                IXGBE_MCSTCTRL_MFE | hw->mac.mc_filter_type);
@@ -1624,15 +1490,8 @@ s32 ixgbe_update_mc_addr_list_generic(struct ixgbe_hw *hw,
  **/
 s32 ixgbe_enable_mc_generic(struct ixgbe_hw *hw)
 {
-	u32 i;
-	u32 rar_entries = hw->mac.num_rar_entries;
 	struct ixgbe_addr_filter_info *a = &hw->addr_ctrl;
 
-	if (a->mc_addr_in_rar_count > 0)
-		for (i = (rar_entries - a->mc_addr_in_rar_count);
-		     i < rar_entries; i++)
-			ixgbe_enable_rar(hw, i);
-
 	if (a->mta_in_use > 0)
 		IXGBE_WRITE_REG(hw, IXGBE_MCSTCTRL, IXGBE_MCSTCTRL_MFE |
 		                hw->mac.mc_filter_type);
@@ -1648,15 +1507,8 @@ s32 ixgbe_enable_mc_generic(struct ixgbe_hw *hw)
  **/
 s32 ixgbe_disable_mc_generic(struct ixgbe_hw *hw)
 {
-	u32 i;
-	u32 rar_entries = hw->mac.num_rar_entries;
 	struct ixgbe_addr_filter_info *a = &hw->addr_ctrl;
 
-	if (a->mc_addr_in_rar_count > 0)
-		for (i = (rar_entries - a->mc_addr_in_rar_count);
-		     i < rar_entries; i++)
-			ixgbe_disable_rar(hw, i);
-
 	if (a->mta_in_use > 0)
 		IXGBE_WRITE_REG(hw, IXGBE_MCSTCTRL, hw->mac.mc_filter_type);
 
@@ -1703,7 +1555,9 @@ s32 ixgbe_fc_enable_generic(struct ixgbe_hw *hw, s32 packetbuf_num)
 	 * 2: Tx flow control is enabled (we can send pause frames but
 	 *    we do not support receiving pause frames).
 	 * 3: Both Rx and Tx flow control (symmetric) are enabled.
+#ifdef CONFIG_DCB
 	 * 4: Priority Flow Control is enabled.
+#endif
 	 * other: Invalid.
 	 */
 	switch (hw->fc.current_mode) {
@@ -2159,10 +2013,16 @@ out:
  **/
 s32 ixgbe_disable_pcie_master(struct ixgbe_hw *hw)
 {
+	struct ixgbe_adapter *adapter = hw->back;
 	u32 i;
 	u32 reg_val;
 	u32 number_of_queues;
-	s32 status = IXGBE_ERR_MASTER_REQUESTS_PENDING;
+	s32 status = 0;
+	u16 dev_status = 0;
+
+	/* Just jump out if bus mastering is already disabled */
+	if (!(IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_GIO))
+		goto out;
 
 	/* Disable the receive unit by stopping each queue */
 	number_of_queues = hw->mac.max_rx_queues;
@@ -2179,13 +2039,43 @@ s32 ixgbe_disable_pcie_master(struct ixgbe_hw *hw)
 	IXGBE_WRITE_REG(hw, IXGBE_CTRL, reg_val);
 
 	for (i = 0; i < IXGBE_PCI_MASTER_DISABLE_TIMEOUT; i++) {
-		if (!(IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_GIO)) {
-			status = 0;
+		if (!(IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_GIO))
+			goto check_device_status;
+		udelay(100);
+	}
+
+	hw_dbg(hw, "GIO Master Disable bit didn't clear - requesting resets\n");
+	status = IXGBE_ERR_MASTER_REQUESTS_PENDING;
+
+	/*
+	 * Before proceeding, make sure that the PCIe block does not have
+	 * transactions pending.
+	 */
+check_device_status:
+	for (i = 0; i < IXGBE_PCI_MASTER_DISABLE_TIMEOUT; i++) {
+		pci_read_config_word(adapter->pdev, IXGBE_PCI_DEVICE_STATUS,
+							 &dev_status);
+		if (!(dev_status & IXGBE_PCI_DEVICE_STATUS_TRANSACTION_PENDING))
 			break;
-		}
 		udelay(100);
 	}
 
+	if (i == IXGBE_PCI_MASTER_DISABLE_TIMEOUT)
+		hw_dbg(hw, "PCIe transaction pending bit also did not clear.\n");
+	else
+		goto out;
+
+	/*
+	 * Two consecutive resets are required via CTRL.RST per datasheet
+	 * 5.2.5.3.2 Master Disable.  We set a flag to inform the reset routine
+	 * of this need.  The first reset prevents new master requests from
+	 * being issued by our device.  We then must wait 1usec for any
+	 * remaining completions from the PCIe bus to trickle in, and then reset
+	 * again to clear out any effects they may have had on our device.
+	 */
+	 hw->mac.flags |= IXGBE_FLAGS_DOUBLE_RESET_REQUIRED;
+
+out:
 	return status;
 }
 
@@ -2206,6 +2096,10 @@ s32 ixgbe_acquire_swfw_sync(struct ixgbe_hw *hw, u16 mask)
 	s32 timeout = 200;
 
 	while (timeout) {
+		/*
+		 * SW EEPROM semaphore bit is used for access to all
+		 * SW_FW_SYNC/GSSR bits (not just EEPROM)
+		 */
 		if (ixgbe_get_eeprom_semaphore(hw))
 			return IXGBE_ERR_SWFW_SYNC;
 
@@ -2223,7 +2117,7 @@ s32 ixgbe_acquire_swfw_sync(struct ixgbe_hw *hw, u16 mask)
 	}
 
 	if (!timeout) {
-		hw_dbg(hw, "Driver can't access resource, GSSR timeout.\n");
+		hw_dbg(hw, "Driver can't access resource, SW_FW_SYNC timeout.\n");
 		return IXGBE_ERR_SWFW_SYNC;
 	}
 
@@ -2427,37 +2321,38 @@ s32 ixgbe_clear_vmdq_generic(struct ixgbe_hw *hw, u32 rar, u32 vmdq)
 	u32 mpsar_lo, mpsar_hi;
 	u32 rar_entries = hw->mac.num_rar_entries;
 
-	if (rar < rar_entries) {
-		mpsar_lo = IXGBE_READ_REG(hw, IXGBE_MPSAR_LO(rar));
-		mpsar_hi = IXGBE_READ_REG(hw, IXGBE_MPSAR_HI(rar));
+	/* Make sure we are using a valid rar index range */
+	if (rar >= rar_entries) {
+		hw_dbg(hw, "RAR index %d is out of range.\n", rar);
+		return IXGBE_ERR_INVALID_ARGUMENT;
+	}
 
-		if (!mpsar_lo && !mpsar_hi)
-			goto done;
+	mpsar_lo = IXGBE_READ_REG(hw, IXGBE_MPSAR_LO(rar));
+	mpsar_hi = IXGBE_READ_REG(hw, IXGBE_MPSAR_HI(rar));
 
-		if (vmdq == IXGBE_CLEAR_VMDQ_ALL) {
-			if (mpsar_lo) {
-				IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), 0);
-				mpsar_lo = 0;
-			}
-			if (mpsar_hi) {
-				IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), 0);
-				mpsar_hi = 0;
-			}
-		} else if (vmdq < 32) {
-			mpsar_lo &= ~(1 << vmdq);
-			IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), mpsar_lo);
-		} else {
-			mpsar_hi &= ~(1 << (vmdq - 32));
-			IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), mpsar_hi);
-		}
+	if (!mpsar_lo && !mpsar_hi)
+		goto done;
 
-		/* was that the last pool using this rar? */
-		if (mpsar_lo == 0 && mpsar_hi == 0 && rar != 0)
-			hw->mac.ops.clear_rar(hw, rar);
+	if (vmdq == IXGBE_CLEAR_VMDQ_ALL) {
+		if (mpsar_lo) {
+			IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), 0);
+			mpsar_lo = 0;
+		}
+		if (mpsar_hi) {
+			IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), 0);
+			mpsar_hi = 0;
+		}
+	} else if (vmdq < 32) {
+		mpsar_lo &= ~(1 << vmdq);
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), mpsar_lo);
 	} else {
-		hw_dbg(hw, "RAR index %d is out of range.\n", rar);
+		mpsar_hi &= ~(1 << (vmdq - 32));
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), mpsar_hi);
 	}
 
+	/* was that the last pool using this rar? */
+	if (mpsar_lo == 0 && mpsar_hi == 0 && rar != 0)
+		hw->mac.ops.clear_rar(hw, rar);
 done:
 	return 0;
 }
@@ -2473,18 +2368,20 @@ s32 ixgbe_set_vmdq_generic(struct ixgbe_hw *hw, u32 rar, u32 vmdq)
 	u32 mpsar;
 	u32 rar_entries = hw->mac.num_rar_entries;
 
-	if (rar < rar_entries) {
-		if (vmdq < 32) {
-			mpsar = IXGBE_READ_REG(hw, IXGBE_MPSAR_LO(rar));
-			mpsar |= 1 << vmdq;
-			IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), mpsar);
-		} else {
-			mpsar = IXGBE_READ_REG(hw, IXGBE_MPSAR_HI(rar));
-			mpsar |= 1 << (vmdq - 32);
-			IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), mpsar);
-		}
-	} else {
+	/* Make sure we are using a valid rar index range */
+	if (rar >= rar_entries) {
 		hw_dbg(hw, "RAR index %d is out of range.\n", rar);
+		return IXGBE_ERR_INVALID_ARGUMENT;
+	}
+
+	if (vmdq < 32) {
+		mpsar = IXGBE_READ_REG(hw, IXGBE_MPSAR_LO(rar));
+		mpsar |= 1 << vmdq;
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), mpsar);
+	} else {
+		mpsar = IXGBE_READ_REG(hw, IXGBE_MPSAR_HI(rar));
+		mpsar |= 1 << (vmdq - 32);
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), mpsar);
 	}
 	return 0;
 }
@@ -2497,7 +2394,6 @@ s32 ixgbe_init_uta_tables_generic(struct ixgbe_hw *hw)
 {
 	int i;
 
-
 	for (i = 0; i < 128; i++)
 		IXGBE_WRITE_REG(hw, IXGBE_UTA(i), 0);
 
@@ -2726,12 +2622,21 @@ s32 ixgbe_clear_vfta_generic(struct ixgbe_hw *hw)
  *  Reads the links register to determine if link is up and the current speed
  **/
 s32 ixgbe_check_mac_link_generic(struct ixgbe_hw *hw, ixgbe_link_speed *speed,
-                               bool *link_up, bool link_up_wait_to_complete)
+				 bool *link_up, bool link_up_wait_to_complete)
 {
-	u32 links_reg;
+	u32 links_reg, links_orig;
 	u32 i;
 
+	/* clear the old state */
+	links_orig = IXGBE_READ_REG(hw, IXGBE_LINKS);
+
 	links_reg = IXGBE_READ_REG(hw, IXGBE_LINKS);
+
+	if (links_orig != links_reg) {
+		hw_dbg(hw, "LINKS changed from %08X to %08X\n",
+		       links_orig, links_reg);
+	}
+
 	if (link_up_wait_to_complete) {
 		for (i = 0; i < IXGBE_LINK_UP_TIME; i++) {
 			if (links_reg & IXGBE_LINKS_UP) {
@@ -2754,10 +2659,13 @@ s32 ixgbe_check_mac_link_generic(struct ixgbe_hw *hw, ixgbe_link_speed *speed,
 	    IXGBE_LINKS_SPEED_10G_82599)
 		*speed = IXGBE_LINK_SPEED_10GB_FULL;
 	else if ((links_reg & IXGBE_LINKS_SPEED_82599) ==
-	         IXGBE_LINKS_SPEED_1G_82599)
+		 IXGBE_LINKS_SPEED_1G_82599)
 		*speed = IXGBE_LINK_SPEED_1GB_FULL;
-	else
+	else if ((links_reg & IXGBE_LINKS_SPEED_82599) ==
+		 IXGBE_LINKS_SPEED_100_82599)
 		*speed = IXGBE_LINK_SPEED_100_FULL;
+	else
+		*speed = IXGBE_LINK_SPEED_UNKNOWN;
 
 	/* if link is down, zero out the current_mode */
 	if (*link_up == false) {
diff --git a/drivers/net/ixgbe/ixgbe_common.h b/drivers/net/ixgbe/ixgbe_common.h
index 66ed045a8cf0..508f635fc2ca 100644
--- a/drivers/net/ixgbe/ixgbe_common.h
+++ b/drivers/net/ixgbe/ixgbe_common.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -29,6 +29,7 @@
 #define _IXGBE_COMMON_H_
 
 #include "ixgbe_type.h"
+#include "ixgbe.h"
 
 u32 ixgbe_get_pcie_msix_count_generic(struct ixgbe_hw *hw);
 s32 ixgbe_init_ops_generic(struct ixgbe_hw *hw);
@@ -62,8 +63,6 @@ s32 ixgbe_clear_rar_generic(struct ixgbe_hw *hw, u32 index);
 s32 ixgbe_init_rx_addrs_generic(struct ixgbe_hw *hw);
 s32 ixgbe_update_mc_addr_list_generic(struct ixgbe_hw *hw,
 				      struct net_device *netdev);
-s32 ixgbe_update_uc_addr_list_generic(struct ixgbe_hw *hw,
-				      struct net_device *netdev);
 s32 ixgbe_enable_mc_generic(struct ixgbe_hw *hw);
 s32 ixgbe_disable_mc_generic(struct ixgbe_hw *hw);
 s32 ixgbe_enable_rx_dma_generic(struct ixgbe_hw *hw, u32 regval);
@@ -110,9 +109,8 @@ void ixgbe_set_vlan_anti_spoofing(struct ixgbe_hw *hw, bool enable, int vf);
 
 #define IXGBE_WRITE_FLUSH(a) IXGBE_READ_REG(a, IXGBE_STATUS)
 
-extern struct net_device *ixgbe_get_hw_dev(struct ixgbe_hw *hw);
 #define hw_dbg(hw, format, arg...) \
-	netdev_dbg(ixgbe_get_hw_dev(hw), format, ##arg)
+	netdev_dbg(((struct ixgbe_adapter *)(hw->back))->netdev, format, ##arg)
 #define e_dev_info(format, arg...) \
 	dev_info(&adapter->pdev->dev, format, ## arg)
 #define e_dev_warn(format, arg...) \
diff --git a/drivers/net/ixgbe/ixgbe_dcb.c b/drivers/net/ixgbe/ixgbe_dcb.c
index d16c260c1f50..c2ee6fcb4e91 100644
--- a/drivers/net/ixgbe/ixgbe_dcb.c
+++ b/drivers/net/ixgbe/ixgbe_dcb.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -34,6 +34,42 @@
 #include "ixgbe_dcb_82599.h"
 
 /**
+ * ixgbe_ieee_credits - This calculates the ieee traffic class
+ * credits from the configured bandwidth percentages. Credits
+ * are the smallest unit programable into the underlying
+ * hardware. The IEEE 802.1Qaz specification do not use bandwidth
+ * groups so this is much simplified from the CEE case.
+ */
+s32 ixgbe_ieee_credits(__u8 *bw, __u16 *refill, __u16 *max, int max_frame)
+{
+	int min_percent = 100;
+	int min_credit, multiplier;
+	int i;
+
+	min_credit = ((max_frame / 2) + DCB_CREDIT_QUANTUM - 1) /
+			DCB_CREDIT_QUANTUM;
+
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		if (bw[i] < min_percent && bw[i])
+			min_percent = bw[i];
+	}
+
+	multiplier = (min_credit / min_percent) + 1;
+
+	/* Find out the hw credits for each TC */
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		int val = min(bw[i] * multiplier, MAX_CREDIT_REFILL);
+
+		if (val < min_credit)
+			val = min_credit;
+		refill[i] = val;
+
+		max[i] = (bw[i] * MAX_CREDIT)/100;
+	}
+	return 0;
+}
+
+/**
  * ixgbe_dcb_calculate_tc_credits - Calculates traffic class credits
  * @ixgbe_dcb_config: Struct containing DCB settings.
  * @direction: Configuring either Tx or Rx.
@@ -141,6 +177,59 @@ out:
 	return ret_val;
 }
 
+void ixgbe_dcb_unpack_pfc(struct ixgbe_dcb_config *cfg, u8 *pfc_en)
+{
+	int i;
+
+	*pfc_en = 0;
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
+		*pfc_en |= (cfg->tc_config[i].dcb_pfc & 0xF) << i;
+}
+
+void ixgbe_dcb_unpack_refill(struct ixgbe_dcb_config *cfg, int direction,
+			     u16 *refill)
+{
+	struct tc_bw_alloc *p;
+	int i;
+
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		p = &cfg->tc_config[i].path[direction];
+		refill[i] = p->data_credits_refill;
+	}
+}
+
+void ixgbe_dcb_unpack_max(struct ixgbe_dcb_config *cfg, u16 *max)
+{
+	int i;
+
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
+		max[i] = cfg->tc_config[i].desc_credits_max;
+}
+
+void ixgbe_dcb_unpack_bwgid(struct ixgbe_dcb_config *cfg, int direction,
+			    u8 *bwgid)
+{
+	struct tc_bw_alloc *p;
+	int i;
+
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		p = &cfg->tc_config[i].path[direction];
+		bwgid[i] = p->bwg_id;
+	}
+}
+
+void ixgbe_dcb_unpack_prio(struct ixgbe_dcb_config *cfg, int direction,
+			    u8 *ptype)
+{
+	struct tc_bw_alloc *p;
+	int i;
+
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		p = &cfg->tc_config[i].path[direction];
+		ptype[i] = p->prio_type;
+	}
+}
+
 /**
  * ixgbe_dcb_hw_config - Config and enable DCB
  * @hw: pointer to hardware structure
@@ -152,13 +241,30 @@ s32 ixgbe_dcb_hw_config(struct ixgbe_hw *hw,
                         struct ixgbe_dcb_config *dcb_config)
 {
 	s32 ret = 0;
+	u8 pfc_en;
+	u8 ptype[MAX_TRAFFIC_CLASS];
+	u8 bwgid[MAX_TRAFFIC_CLASS];
+	u16 refill[MAX_TRAFFIC_CLASS];
+	u16 max[MAX_TRAFFIC_CLASS];
+
+	/* Unpack CEE standard containers */
+	ixgbe_dcb_unpack_pfc(dcb_config, &pfc_en);
+	ixgbe_dcb_unpack_refill(dcb_config, DCB_TX_CONFIG, refill);
+	ixgbe_dcb_unpack_max(dcb_config, max);
+	ixgbe_dcb_unpack_bwgid(dcb_config, DCB_TX_CONFIG, bwgid);
+	ixgbe_dcb_unpack_prio(dcb_config, DCB_TX_CONFIG, ptype);
+
 	switch (hw->mac.type) {
 	case ixgbe_mac_82598EB:
-		ret = ixgbe_dcb_hw_config_82598(hw, dcb_config);
+		ret = ixgbe_dcb_hw_config_82598(hw, dcb_config->rx_pba_cfg,
+						pfc_en, refill, max, bwgid,
+						ptype);
 		break;
 	case ixgbe_mac_82599EB:
 	case ixgbe_mac_X540:
-		ret = ixgbe_dcb_hw_config_82599(hw, dcb_config);
+		ret = ixgbe_dcb_hw_config_82599(hw, dcb_config->rx_pba_cfg,
+						pfc_en, refill, max, bwgid,
+						ptype);
 		break;
 	default:
 		break;
@@ -166,3 +272,70 @@ s32 ixgbe_dcb_hw_config(struct ixgbe_hw *hw,
 	return ret;
 }
 
+/* Helper routines to abstract HW specifics from DCB netlink ops */
+s32 ixgbe_dcb_hw_pfc_config(struct ixgbe_hw *hw, u8 pfc_en)
+{
+	int ret = -EINVAL;
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_82598EB:
+		ret = ixgbe_dcb_config_pfc_82598(hw, pfc_en);
+		break;
+	case ixgbe_mac_82599EB:
+	case ixgbe_mac_X540:
+		ret = ixgbe_dcb_config_pfc_82599(hw, pfc_en);
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+s32 ixgbe_dcb_hw_ets_config(struct ixgbe_hw *hw,
+			    u16 *refill, u16 *max, u8 *bwg_id, u8 *tsa)
+{
+	int i;
+	u8 prio_type[IEEE_8021QAZ_MAX_TCS];
+
+	/* Map TSA onto CEE prio type */
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		switch (tsa[i]) {
+		case IEEE_8021QAZ_TSA_STRICT:
+			prio_type[i] = 2;
+			break;
+		case IEEE_8021QAZ_TSA_ETS:
+			prio_type[i] = 0;
+			break;
+		default:
+			/* Hardware only supports priority strict or
+			 * ETS transmission selection algorithms if
+			 * we receive some other value from dcbnl
+			 * throw an error
+			 */
+			return -EINVAL;
+		}
+	}
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_82598EB:
+		ixgbe_dcb_config_rx_arbiter_82598(hw, refill, max,
+							prio_type);
+		ixgbe_dcb_config_tx_desc_arbiter_82598(hw, refill, max,
+							     bwg_id, prio_type);
+		ixgbe_dcb_config_tx_data_arbiter_82598(hw, refill, max,
+							     bwg_id, prio_type);
+		break;
+	case ixgbe_mac_82599EB:
+	case ixgbe_mac_X540:
+		ixgbe_dcb_config_rx_arbiter_82599(hw, refill, max,
+						  bwg_id, prio_type);
+		ixgbe_dcb_config_tx_desc_arbiter_82599(hw, refill, max,
+						       bwg_id, prio_type);
+		ixgbe_dcb_config_tx_data_arbiter_82599(hw, refill, max,
+						       bwg_id, prio_type);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
diff --git a/drivers/net/ixgbe/ixgbe_dcb.h b/drivers/net/ixgbe/ixgbe_dcb.h
index 1cfe38ee1644..515bc27477f6 100644
--- a/drivers/net/ixgbe/ixgbe_dcb.h
+++ b/drivers/net/ixgbe/ixgbe_dcb.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -139,7 +139,6 @@ struct ixgbe_dcb_config {
 	struct tc_configuration tc_config[MAX_TRAFFIC_CLASS];
 	u8     bw_percentage[2][MAX_BW_GROUP]; /* One each for Tx/Rx */
 	bool   pfc_mode_enable;
-	bool   round_robin_enable;
 
 	enum dcb_rx_pba_cfg rx_pba_cfg;
 
@@ -148,12 +147,21 @@ struct ixgbe_dcb_config {
 };
 
 /* DCB driver APIs */
+void ixgbe_dcb_unpack_pfc(struct ixgbe_dcb_config *cfg, u8 *pfc_en);
+void ixgbe_dcb_unpack_refill(struct ixgbe_dcb_config *, int, u16 *);
+void ixgbe_dcb_unpack_max(struct ixgbe_dcb_config *, u16 *);
+void ixgbe_dcb_unpack_bwgid(struct ixgbe_dcb_config *, int, u8 *);
+void ixgbe_dcb_unpack_prio(struct ixgbe_dcb_config *, int, u8 *);
 
 /* DCB credits calculation */
+s32 ixgbe_ieee_credits(__u8 *bw, __u16 *refill, __u16 *max, int max_frame);
 s32 ixgbe_dcb_calculate_tc_credits(struct ixgbe_hw *,
 				   struct ixgbe_dcb_config *, int, u8);
 
 /* DCB hw initialization */
+s32 ixgbe_dcb_hw_ets_config(struct ixgbe_hw *hw,
+			    u16 *refill, u16 *max, u8 *bwg_id, u8 *prio_type);
+s32 ixgbe_dcb_hw_pfc_config(struct ixgbe_hw *hw, u8 pfc_en);
 s32 ixgbe_dcb_hw_config(struct ixgbe_hw *, struct ixgbe_dcb_config *);
 
 /* DCB definitions for credit calculation */
diff --git a/drivers/net/ixgbe/ixgbe_dcb_82598.c b/drivers/net/ixgbe/ixgbe_dcb_82598.c
index 9a5e89c12e05..c97cf9160dc0 100644
--- a/drivers/net/ixgbe/ixgbe_dcb_82598.c
+++ b/drivers/net/ixgbe/ixgbe_dcb_82598.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,15 +38,14 @@
  *
  * Configure packet buffers for DCB mode.
  */
-static s32 ixgbe_dcb_config_packet_buffers_82598(struct ixgbe_hw *hw,
-						 struct ixgbe_dcb_config *dcb_config)
+static s32 ixgbe_dcb_config_packet_buffers_82598(struct ixgbe_hw *hw, u8 rx_pba)
 {
 	s32 ret_val = 0;
 	u32 value = IXGBE_RXPBSIZE_64KB;
 	u8  i = 0;
 
 	/* Setup Rx packet buffer sizes */
-	switch (dcb_config->rx_pba_cfg) {
+	switch (rx_pba) {
 	case pba_80_48:
 		/* Setup the first four at 80KB */
 		value = IXGBE_RXPBSIZE_80KB;
@@ -78,10 +77,11 @@ static s32 ixgbe_dcb_config_packet_buffers_82598(struct ixgbe_hw *hw,
  *
  * Configure Rx Data Arbiter and credits for each traffic class.
  */
-static s32 ixgbe_dcb_config_rx_arbiter_82598(struct ixgbe_hw *hw,
-                                      struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_rx_arbiter_82598(struct ixgbe_hw *hw,
+					u16 *refill,
+					u16 *max,
+					u8 *prio_type)
 {
-	struct tc_bw_alloc    *p;
 	u32    reg           = 0;
 	u32    credit_refill = 0;
 	u32    credit_max    = 0;
@@ -102,13 +102,12 @@ static s32 ixgbe_dcb_config_rx_arbiter_82598(struct ixgbe_hw *hw,
 
 	/* Configure traffic class credits and priority */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &dcb_config->tc_config[i].path[DCB_RX_CONFIG];
-		credit_refill = p->data_credits_refill;
-		credit_max    = p->data_credits_max;
+		credit_refill = refill[i];
+		credit_max    = max[i];
 
 		reg = credit_refill | (credit_max << IXGBE_RT2CR_MCL_SHIFT);
 
-		if (p->prio_type == prio_link)
+		if (prio_type[i] == prio_link)
 			reg |= IXGBE_RT2CR_LSP;
 
 		IXGBE_WRITE_REG(hw, IXGBE_RT2CR(i), reg);
@@ -135,10 +134,12 @@ static s32 ixgbe_dcb_config_rx_arbiter_82598(struct ixgbe_hw *hw,
  *
  * Configure Tx Descriptor Arbiter and credits for each traffic class.
  */
-static s32 ixgbe_dcb_config_tx_desc_arbiter_82598(struct ixgbe_hw *hw,
-                                           struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_tx_desc_arbiter_82598(struct ixgbe_hw *hw,
+						u16 *refill,
+						u16 *max,
+						u8 *bwg_id,
+						u8 *prio_type)
 {
-	struct tc_bw_alloc *p;
 	u32    reg, max_credits;
 	u8     i;
 
@@ -146,10 +147,8 @@ static s32 ixgbe_dcb_config_tx_desc_arbiter_82598(struct ixgbe_hw *hw,
 
 	/* Enable arbiter */
 	reg &= ~IXGBE_DPMCS_ARBDIS;
-	if (!(dcb_config->round_robin_enable)) {
-		/* Enable DFP and Recycle mode */
-		reg |= (IXGBE_DPMCS_TDPAC | IXGBE_DPMCS_TRM);
-	}
+	/* Enable DFP and Recycle mode */
+	reg |= (IXGBE_DPMCS_TDPAC | IXGBE_DPMCS_TRM);
 	reg |= IXGBE_DPMCS_TSOEF;
 	/* Configure Max TSO packet size 34KB including payload and headers */
 	reg |= (0x4 << IXGBE_DPMCS_MTSOS_SHIFT);
@@ -158,16 +157,15 @@ static s32 ixgbe_dcb_config_tx_desc_arbiter_82598(struct ixgbe_hw *hw,
 
 	/* Configure traffic class credits and priority */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &dcb_config->tc_config[i].path[DCB_TX_CONFIG];
-		max_credits = dcb_config->tc_config[i].desc_credits_max;
+		max_credits = max[i];
 		reg = max_credits << IXGBE_TDTQ2TCCR_MCL_SHIFT;
-		reg |= p->data_credits_refill;
-		reg |= (u32)(p->bwg_id) << IXGBE_TDTQ2TCCR_BWG_SHIFT;
+		reg |= refill[i];
+		reg |= (u32)(bwg_id[i]) << IXGBE_TDTQ2TCCR_BWG_SHIFT;
 
-		if (p->prio_type == prio_group)
+		if (prio_type[i] == prio_group)
 			reg |= IXGBE_TDTQ2TCCR_GSP;
 
-		if (p->prio_type == prio_link)
+		if (prio_type[i] == prio_link)
 			reg |= IXGBE_TDTQ2TCCR_LSP;
 
 		IXGBE_WRITE_REG(hw, IXGBE_TDTQ2TCCR(i), reg);
@@ -183,10 +181,12 @@ static s32 ixgbe_dcb_config_tx_desc_arbiter_82598(struct ixgbe_hw *hw,
  *
  * Configure Tx Data Arbiter and credits for each traffic class.
  */
-static s32 ixgbe_dcb_config_tx_data_arbiter_82598(struct ixgbe_hw *hw,
-                                           struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_tx_data_arbiter_82598(struct ixgbe_hw *hw,
+						u16 *refill,
+						u16 *max,
+						u8 *bwg_id,
+						u8 *prio_type)
 {
-	struct tc_bw_alloc *p;
 	u32 reg;
 	u8 i;
 
@@ -200,15 +200,14 @@ static s32 ixgbe_dcb_config_tx_data_arbiter_82598(struct ixgbe_hw *hw,
 
 	/* Configure traffic class credits and priority */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &dcb_config->tc_config[i].path[DCB_TX_CONFIG];
-		reg = p->data_credits_refill;
-		reg |= (u32)(p->data_credits_max) << IXGBE_TDPT2TCCR_MCL_SHIFT;
-		reg |= (u32)(p->bwg_id) << IXGBE_TDPT2TCCR_BWG_SHIFT;
+		reg = refill[i];
+		reg |= (u32)(max[i]) << IXGBE_TDPT2TCCR_MCL_SHIFT;
+		reg |= (u32)(bwg_id[i]) << IXGBE_TDPT2TCCR_BWG_SHIFT;
 
-		if (p->prio_type == prio_group)
+		if (prio_type[i] == prio_group)
 			reg |= IXGBE_TDPT2TCCR_GSP;
 
-		if (p->prio_type == prio_link)
+		if (prio_type[i] == prio_link)
 			reg |= IXGBE_TDPT2TCCR_LSP;
 
 		IXGBE_WRITE_REG(hw, IXGBE_TDPT2TCCR(i), reg);
@@ -229,13 +228,12 @@ static s32 ixgbe_dcb_config_tx_data_arbiter_82598(struct ixgbe_hw *hw,
  *
  * Configure Priority Flow Control for each traffic class.
  */
-s32 ixgbe_dcb_config_pfc_82598(struct ixgbe_hw *hw,
-                               struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_pfc_82598(struct ixgbe_hw *hw, u8 pfc_en)
 {
 	u32 reg, rx_pba_size;
 	u8  i;
 
-	if (!dcb_config->pfc_mode_enable)
+	if (!pfc_en)
 		goto out;
 
 	/* Enable Transmit Priority Flow Control */
@@ -256,19 +254,20 @@ s32 ixgbe_dcb_config_pfc_82598(struct ixgbe_hw *hw,
 	 * for each traffic class.
 	 */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		int enabled = pfc_en & (1 << i);
 		rx_pba_size = IXGBE_READ_REG(hw, IXGBE_RXPBSIZE(i));
 		rx_pba_size >>= IXGBE_RXPBSIZE_SHIFT;
 		reg = (rx_pba_size - hw->fc.low_water) << 10;
 
-		if (dcb_config->tc_config[i].dcb_pfc == pfc_enabled_tx ||
-		    dcb_config->tc_config[i].dcb_pfc == pfc_enabled_full)
+		if (enabled == pfc_enabled_tx ||
+		    enabled == pfc_enabled_full)
 			reg |= IXGBE_FCRTL_XONE;
 
 		IXGBE_WRITE_REG(hw, IXGBE_FCRTL(i), reg);
 
 		reg = (rx_pba_size - hw->fc.high_water) << 10;
-		if (dcb_config->tc_config[i].dcb_pfc == pfc_enabled_tx ||
-		    dcb_config->tc_config[i].dcb_pfc == pfc_enabled_full)
+		if (enabled == pfc_enabled_tx ||
+		    enabled == pfc_enabled_full)
 			reg |= IXGBE_FCRTH_FCEN;
 
 		IXGBE_WRITE_REG(hw, IXGBE_FCRTH(i), reg);
@@ -292,7 +291,7 @@ out:
  * Configure queue statistics registers, all queues belonging to same traffic
  * class uses a single set of queue statistics counters.
  */
-static s32 ixgbe_dcb_config_tc_stats_82598(struct ixgbe_hw *hw)
+s32 ixgbe_dcb_config_tc_stats_82598(struct ixgbe_hw *hw)
 {
 	u32 reg = 0;
 	u8  i   = 0;
@@ -325,13 +324,16 @@ static s32 ixgbe_dcb_config_tc_stats_82598(struct ixgbe_hw *hw)
  * Configure dcb settings and enable dcb mode.
  */
 s32 ixgbe_dcb_hw_config_82598(struct ixgbe_hw *hw,
-                              struct ixgbe_dcb_config *dcb_config)
+			      u8 rx_pba, u8 pfc_en, u16 *refill,
+			      u16 *max, u8 *bwg_id, u8 *prio_type)
 {
-	ixgbe_dcb_config_packet_buffers_82598(hw, dcb_config);
-	ixgbe_dcb_config_rx_arbiter_82598(hw, dcb_config);
-	ixgbe_dcb_config_tx_desc_arbiter_82598(hw, dcb_config);
-	ixgbe_dcb_config_tx_data_arbiter_82598(hw, dcb_config);
-	ixgbe_dcb_config_pfc_82598(hw, dcb_config);
+	ixgbe_dcb_config_packet_buffers_82598(hw, rx_pba);
+	ixgbe_dcb_config_rx_arbiter_82598(hw, refill, max, prio_type);
+	ixgbe_dcb_config_tx_desc_arbiter_82598(hw, refill, max,
+					       bwg_id, prio_type);
+	ixgbe_dcb_config_tx_data_arbiter_82598(hw, refill, max,
+					       bwg_id, prio_type);
+	ixgbe_dcb_config_pfc_82598(hw, pfc_en);
 	ixgbe_dcb_config_tc_stats_82598(hw);
 
 	return 0;
diff --git a/drivers/net/ixgbe/ixgbe_dcb_82598.h b/drivers/net/ixgbe/ixgbe_dcb_82598.h
index abc03ccfa088..1e9750c2b46b 100644
--- a/drivers/net/ixgbe/ixgbe_dcb_82598.h
+++ b/drivers/net/ixgbe/ixgbe_dcb_82598.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -71,9 +71,28 @@
 /* DCB hardware-specific driver APIs */
 
 /* DCB PFC functions */
-s32 ixgbe_dcb_config_pfc_82598(struct ixgbe_hw *, struct ixgbe_dcb_config *);
+s32 ixgbe_dcb_config_pfc_82598(struct ixgbe_hw *, u8 pfc_en);
 
 /* DCB hw initialization */
-s32 ixgbe_dcb_hw_config_82598(struct ixgbe_hw *, struct ixgbe_dcb_config *);
+s32 ixgbe_dcb_config_rx_arbiter_82598(struct ixgbe_hw *hw,
+					u16 *refill,
+					u16 *max,
+					u8 *prio_type);
+
+s32 ixgbe_dcb_config_tx_desc_arbiter_82598(struct ixgbe_hw *hw,
+						u16 *refill,
+						u16 *max,
+						u8 *bwg_id,
+						u8 *prio_type);
+
+s32 ixgbe_dcb_config_tx_data_arbiter_82598(struct ixgbe_hw *hw,
+						u16 *refill,
+						u16 *max,
+						u8 *bwg_id,
+						u8 *prio_type);
+
+s32 ixgbe_dcb_hw_config_82598(struct ixgbe_hw *hw,
+			      u8 rx_pba, u8 pfc_en, u16 *refill,
+			      u16 *max, u8 *bwg_id, u8 *prio_type);
 
 #endif /* _DCB_82598_CONFIG_H */
diff --git a/drivers/net/ixgbe/ixgbe_dcb_82599.c b/drivers/net/ixgbe/ixgbe_dcb_82599.c
index 374e1f74d0f5..beaa1c1c1e67 100644
--- a/drivers/net/ixgbe/ixgbe_dcb_82599.c
+++ b/drivers/net/ixgbe/ixgbe_dcb_82599.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -33,19 +33,18 @@
 /**
  * ixgbe_dcb_config_packet_buffers_82599 - Configure DCB packet buffers
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
+ * @rx_pba: method to distribute packet buffer
  *
  * Configure packet buffers for DCB mode.
  */
-static s32 ixgbe_dcb_config_packet_buffers_82599(struct ixgbe_hw *hw,
-                                          struct ixgbe_dcb_config *dcb_config)
+static s32 ixgbe_dcb_config_packet_buffers_82599(struct ixgbe_hw *hw, u8 rx_pba)
 {
 	s32 ret_val = 0;
 	u32 value = IXGBE_RXPBSIZE_64KB;
 	u8  i = 0;
 
 	/* Setup Rx packet buffer sizes */
-	switch (dcb_config->rx_pba_cfg) {
+	switch (rx_pba) {
 	case pba_80_48:
 		/* Setup the first four at 80KB */
 		value = IXGBE_RXPBSIZE_80KB;
@@ -75,14 +74,19 @@ static s32 ixgbe_dcb_config_packet_buffers_82599(struct ixgbe_hw *hw,
 /**
  * ixgbe_dcb_config_rx_arbiter_82599 - Config Rx Data arbiter
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
+ * @refill: refill credits index by traffic class
+ * @max: max credits index by traffic class
+ * @bwg_id: bandwidth grouping indexed by traffic class
+ * @prio_type: priority type indexed by traffic class
  *
  * Configure Rx Packet Arbiter and credits for each traffic class.
  */
-static s32 ixgbe_dcb_config_rx_arbiter_82599(struct ixgbe_hw *hw,
-                                      struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_rx_arbiter_82599(struct ixgbe_hw *hw,
+				      u16 *refill,
+				      u16 *max,
+				      u8 *bwg_id,
+				      u8 *prio_type)
 {
-	struct tc_bw_alloc    *p;
 	u32    reg           = 0;
 	u32    credit_refill = 0;
 	u32    credit_max    = 0;
@@ -103,15 +107,13 @@ static s32 ixgbe_dcb_config_rx_arbiter_82599(struct ixgbe_hw *hw,
 
 	/* Configure traffic class credits and priority */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &dcb_config->tc_config[i].path[DCB_RX_CONFIG];
-
-		credit_refill = p->data_credits_refill;
-		credit_max    = p->data_credits_max;
+		credit_refill = refill[i];
+		credit_max    = max[i];
 		reg = credit_refill | (credit_max << IXGBE_RTRPT4C_MCL_SHIFT);
 
-		reg |= (u32)(p->bwg_id) << IXGBE_RTRPT4C_BWG_SHIFT;
+		reg |= (u32)(bwg_id[i]) << IXGBE_RTRPT4C_BWG_SHIFT;
 
-		if (p->prio_type == prio_link)
+		if (prio_type[i] == prio_link)
 			reg |= IXGBE_RTRPT4C_LSP;
 
 		IXGBE_WRITE_REG(hw, IXGBE_RTRPT4C(i), reg);
@@ -130,14 +132,19 @@ static s32 ixgbe_dcb_config_rx_arbiter_82599(struct ixgbe_hw *hw,
 /**
  * ixgbe_dcb_config_tx_desc_arbiter_82599 - Config Tx Desc. arbiter
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
+ * @refill: refill credits index by traffic class
+ * @max: max credits index by traffic class
+ * @bwg_id: bandwidth grouping indexed by traffic class
+ * @prio_type: priority type indexed by traffic class
  *
  * Configure Tx Descriptor Arbiter and credits for each traffic class.
  */
-static s32 ixgbe_dcb_config_tx_desc_arbiter_82599(struct ixgbe_hw *hw,
-                                           struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_tx_desc_arbiter_82599(struct ixgbe_hw *hw,
+					   u16 *refill,
+					   u16 *max,
+					   u8 *bwg_id,
+					   u8 *prio_type)
 {
-	struct tc_bw_alloc *p;
 	u32    reg, max_credits;
 	u8     i;
 
@@ -149,16 +156,15 @@ static s32 ixgbe_dcb_config_tx_desc_arbiter_82599(struct ixgbe_hw *hw,
 
 	/* Configure traffic class credits and priority */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &dcb_config->tc_config[i].path[DCB_TX_CONFIG];
-		max_credits = dcb_config->tc_config[i].desc_credits_max;
+		max_credits = max[i];
 		reg = max_credits << IXGBE_RTTDT2C_MCL_SHIFT;
-		reg |= p->data_credits_refill;
-		reg |= (u32)(p->bwg_id) << IXGBE_RTTDT2C_BWG_SHIFT;
+		reg |= refill[i];
+		reg |= (u32)(bwg_id[i]) << IXGBE_RTTDT2C_BWG_SHIFT;
 
-		if (p->prio_type == prio_group)
+		if (prio_type[i] == prio_group)
 			reg |= IXGBE_RTTDT2C_GSP;
 
-		if (p->prio_type == prio_link)
+		if (prio_type[i] == prio_link)
 			reg |= IXGBE_RTTDT2C_LSP;
 
 		IXGBE_WRITE_REG(hw, IXGBE_RTTDT2C(i), reg);
@@ -177,14 +183,19 @@ static s32 ixgbe_dcb_config_tx_desc_arbiter_82599(struct ixgbe_hw *hw,
 /**
  * ixgbe_dcb_config_tx_data_arbiter_82599 - Config Tx Data arbiter
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
+ * @refill: refill credits index by traffic class
+ * @max: max credits index by traffic class
+ * @bwg_id: bandwidth grouping indexed by traffic class
+ * @prio_type: priority type indexed by traffic class
  *
  * Configure Tx Packet Arbiter and credits for each traffic class.
  */
-static s32 ixgbe_dcb_config_tx_data_arbiter_82599(struct ixgbe_hw *hw,
-                                           struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_tx_data_arbiter_82599(struct ixgbe_hw *hw,
+					   u16 *refill,
+					   u16 *max,
+					   u8 *bwg_id,
+					   u8 *prio_type)
 {
-	struct tc_bw_alloc *p;
 	u32 reg;
 	u8 i;
 
@@ -205,15 +216,14 @@ static s32 ixgbe_dcb_config_tx_data_arbiter_82599(struct ixgbe_hw *hw,
 
 	/* Configure traffic class credits and priority */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &dcb_config->tc_config[i].path[DCB_TX_CONFIG];
-		reg = p->data_credits_refill;
-		reg |= (u32)(p->data_credits_max) << IXGBE_RTTPT2C_MCL_SHIFT;
-		reg |= (u32)(p->bwg_id) << IXGBE_RTTPT2C_BWG_SHIFT;
+		reg = refill[i];
+		reg |= (u32)(max[i]) << IXGBE_RTTPT2C_MCL_SHIFT;
+		reg |= (u32)(bwg_id[i]) << IXGBE_RTTPT2C_BWG_SHIFT;
 
-		if (p->prio_type == prio_group)
+		if (prio_type[i] == prio_group)
 			reg |= IXGBE_RTTPT2C_GSP;
 
-		if (p->prio_type == prio_link)
+		if (prio_type[i] == prio_link)
 			reg |= IXGBE_RTTPT2C_LSP;
 
 		IXGBE_WRITE_REG(hw, IXGBE_RTTPT2C(i), reg);
@@ -233,17 +243,16 @@ static s32 ixgbe_dcb_config_tx_data_arbiter_82599(struct ixgbe_hw *hw,
 /**
  * ixgbe_dcb_config_pfc_82599 - Configure priority flow control
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
+ * @pfc_en: enabled pfc bitmask
  *
  * Configure Priority Flow Control (PFC) for each traffic class.
  */
-s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw,
-                               struct ixgbe_dcb_config *dcb_config)
+s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw, u8 pfc_en)
 {
 	u32 i, reg, rx_pba_size;
 
 	/* If PFC is disabled globally then fall back to LFC. */
-	if (!dcb_config->pfc_mode_enable) {
+	if (!pfc_en) {
 		for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
 			hw->mac.ops.fc_enable(hw, i);
 		goto out;
@@ -251,19 +260,18 @@ s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw,
 
 	/* Configure PFC Tx thresholds per TC */
 	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		int enabled = pfc_en & (1 << i);
 		rx_pba_size = IXGBE_READ_REG(hw, IXGBE_RXPBSIZE(i));
 		rx_pba_size >>= IXGBE_RXPBSIZE_SHIFT;
 
 		reg = (rx_pba_size - hw->fc.low_water) << 10;
 
-		if (dcb_config->tc_config[i].dcb_pfc == pfc_enabled_full ||
-		    dcb_config->tc_config[i].dcb_pfc == pfc_enabled_tx)
+		if (enabled)
 			reg |= IXGBE_FCRTL_XONE;
 		IXGBE_WRITE_REG(hw, IXGBE_FCRTL_82599(i), reg);
 
 		reg = (rx_pba_size - hw->fc.high_water) << 10;
-		if (dcb_config->tc_config[i].dcb_pfc == pfc_enabled_full ||
-		    dcb_config->tc_config[i].dcb_pfc == pfc_enabled_tx)
+		if (enabled)
 			reg |= IXGBE_FCRTH_FCEN;
 		IXGBE_WRITE_REG(hw, IXGBE_FCRTH_82599(i), reg);
 	}
@@ -349,7 +357,6 @@ static s32 ixgbe_dcb_config_tc_stats_82599(struct ixgbe_hw *hw)
 /**
  * ixgbe_dcb_config_82599 - Configure general DCB parameters
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
  *
  * Configure general DCB parameters.
  */
@@ -406,19 +413,27 @@ static s32 ixgbe_dcb_config_82599(struct ixgbe_hw *hw)
 /**
  * ixgbe_dcb_hw_config_82599 - Configure and enable DCB
  * @hw: pointer to hardware structure
- * @dcb_config: pointer to ixgbe_dcb_config structure
+ * @rx_pba: method to distribute packet buffer
+ * @refill: refill credits index by traffic class
+ * @max: max credits index by traffic class
+ * @bwg_id: bandwidth grouping indexed by traffic class
+ * @prio_type: priority type indexed by traffic class
+ * @pfc_en: enabled pfc bitmask
  *
  * Configure dcb settings and enable dcb mode.
  */
 s32 ixgbe_dcb_hw_config_82599(struct ixgbe_hw *hw,
-                              struct ixgbe_dcb_config *dcb_config)
+			      u8 rx_pba, u8 pfc_en, u16 *refill,
+			      u16 *max, u8 *bwg_id, u8 *prio_type)
 {
-	ixgbe_dcb_config_packet_buffers_82599(hw, dcb_config);
+	ixgbe_dcb_config_packet_buffers_82599(hw, rx_pba);
 	ixgbe_dcb_config_82599(hw);
-	ixgbe_dcb_config_rx_arbiter_82599(hw, dcb_config);
-	ixgbe_dcb_config_tx_desc_arbiter_82599(hw, dcb_config);
-	ixgbe_dcb_config_tx_data_arbiter_82599(hw, dcb_config);
-	ixgbe_dcb_config_pfc_82599(hw, dcb_config);
+	ixgbe_dcb_config_rx_arbiter_82599(hw, refill, max, bwg_id, prio_type);
+	ixgbe_dcb_config_tx_desc_arbiter_82599(hw, refill, max,
+					       bwg_id, prio_type);
+	ixgbe_dcb_config_tx_data_arbiter_82599(hw, refill, max,
+					       bwg_id, prio_type);
+	ixgbe_dcb_config_pfc_82599(hw, pfc_en);
 	ixgbe_dcb_config_tc_stats_82599(hw);
 
 	return 0;
diff --git a/drivers/net/ixgbe/ixgbe_dcb_82599.h b/drivers/net/ixgbe/ixgbe_dcb_82599.h
index 3841649fb954..0b39ab4ffc70 100644
--- a/drivers/net/ixgbe/ixgbe_dcb_82599.h
+++ b/drivers/net/ixgbe/ixgbe_dcb_82599.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -102,11 +102,29 @@
 /* DCB hardware-specific driver APIs */
 
 /* DCB PFC functions */
-s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw,
-                               struct ixgbe_dcb_config *dcb_config);
+s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw, u8 pfc_en);
 
 /* DCB hw initialization */
+s32 ixgbe_dcb_config_rx_arbiter_82599(struct ixgbe_hw *hw,
+					u16 *refill,
+					u16 *max,
+					u8 *bwg_id,
+					u8 *prio_type);
+
+s32 ixgbe_dcb_config_tx_desc_arbiter_82599(struct ixgbe_hw *hw,
+						u16 *refill,
+						u16 *max,
+						u8 *bwg_id,
+						u8 *prio_type);
+
+s32 ixgbe_dcb_config_tx_data_arbiter_82599(struct ixgbe_hw *hw,
+						u16 *refill,
+						u16 *max,
+						u8 *bwg_id,
+						u8 *prio_type);
+
 s32 ixgbe_dcb_hw_config_82599(struct ixgbe_hw *hw,
-                              struct ixgbe_dcb_config *config);
+			      u8 rx_pba, u8 pfc_en, u16 *refill,
+			      u16 *max, u8 *bwg_id, u8 *prio_type);
 
 #endif /* _DCB_82599_CONFIG_H */
diff --git a/drivers/net/ixgbe/ixgbe_dcb_nl.c b/drivers/net/ixgbe/ixgbe_dcb_nl.c
index bf566e8a455e..d7f0024014b1 100644
--- a/drivers/net/ixgbe/ixgbe_dcb_nl.c
+++ b/drivers/net/ixgbe/ixgbe_dcb_nl.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -37,7 +37,6 @@
 #define BIT_PG_RX	0x04
 #define BIT_PG_TX	0x08
 #define BIT_APP_UPCHG	0x10
-#define BIT_RESETLINK   0x40
 #define BIT_LINKSPEED   0x80
 
 /* Responses for the DCB_C_SET_ALL command */
@@ -225,10 +224,8 @@ static void ixgbe_dcbnl_set_pg_tc_cfg_tx(struct net_device *netdev, int tc,
 	    (adapter->temp_dcb_cfg.tc_config[tc].path[0].bwg_percent !=
 	     adapter->dcb_cfg.tc_config[tc].path[0].bwg_percent) ||
 	    (adapter->temp_dcb_cfg.tc_config[tc].path[0].up_to_tc_bitmap !=
-	     adapter->dcb_cfg.tc_config[tc].path[0].up_to_tc_bitmap)) {
+	     adapter->dcb_cfg.tc_config[tc].path[0].up_to_tc_bitmap))
 		adapter->dcb_set_bitmap |= BIT_PG_TX;
-		adapter->dcb_set_bitmap |= BIT_RESETLINK;
-	}
 }
 
 static void ixgbe_dcbnl_set_pg_bwg_cfg_tx(struct net_device *netdev, int bwg_id,
@@ -239,10 +236,8 @@ static void ixgbe_dcbnl_set_pg_bwg_cfg_tx(struct net_device *netdev, int bwg_id,
 	adapter->temp_dcb_cfg.bw_percentage[0][bwg_id] = bw_pct;
 
 	if (adapter->temp_dcb_cfg.bw_percentage[0][bwg_id] !=
-	    adapter->dcb_cfg.bw_percentage[0][bwg_id]) {
+	    adapter->dcb_cfg.bw_percentage[0][bwg_id])
 		adapter->dcb_set_bitmap |= BIT_PG_TX;
-		adapter->dcb_set_bitmap |= BIT_RESETLINK;
-	}
 }
 
 static void ixgbe_dcbnl_set_pg_tc_cfg_rx(struct net_device *netdev, int tc,
@@ -269,10 +264,8 @@ static void ixgbe_dcbnl_set_pg_tc_cfg_rx(struct net_device *netdev, int tc,
 	    (adapter->temp_dcb_cfg.tc_config[tc].path[1].bwg_percent !=
 	     adapter->dcb_cfg.tc_config[tc].path[1].bwg_percent) ||
 	    (adapter->temp_dcb_cfg.tc_config[tc].path[1].up_to_tc_bitmap !=
-	     adapter->dcb_cfg.tc_config[tc].path[1].up_to_tc_bitmap)) {
+	     adapter->dcb_cfg.tc_config[tc].path[1].up_to_tc_bitmap))
 		adapter->dcb_set_bitmap |= BIT_PG_RX;
-		adapter->dcb_set_bitmap |= BIT_RESETLINK;
-	}
 }
 
 static void ixgbe_dcbnl_set_pg_bwg_cfg_rx(struct net_device *netdev, int bwg_id,
@@ -283,10 +276,8 @@ static void ixgbe_dcbnl_set_pg_bwg_cfg_rx(struct net_device *netdev, int bwg_id,
 	adapter->temp_dcb_cfg.bw_percentage[1][bwg_id] = bw_pct;
 
 	if (adapter->temp_dcb_cfg.bw_percentage[1][bwg_id] !=
-	    adapter->dcb_cfg.bw_percentage[1][bwg_id]) {
+	    adapter->dcb_cfg.bw_percentage[1][bwg_id])
 		adapter->dcb_set_bitmap |= BIT_PG_RX;
-		adapter->dcb_set_bitmap |= BIT_RESETLINK;
-	}
 }
 
 static void ixgbe_dcbnl_get_pg_tc_cfg_tx(struct net_device *netdev, int tc,
@@ -365,21 +356,17 @@ static u8 ixgbe_dcbnl_set_all(struct net_device *netdev)
 		return DCB_NO_HW_CHG;
 
 	/*
-	 * Only take down the adapter if the configuration change
-	 * requires a reset.
+	 * Only take down the adapter if an app change occured. FCoE
+	 * may shuffle tx rings in this case and this can not be done
+	 * without a reset currently.
 	 */
-	if (adapter->dcb_set_bitmap & BIT_RESETLINK) {
+	if (adapter->dcb_set_bitmap & BIT_APP_UPCHG) {
 		while (test_and_set_bit(__IXGBE_RESETTING, &adapter->state))
 			msleep(1);
 
-		if (adapter->dcb_set_bitmap & BIT_APP_UPCHG) {
-			if (netif_running(netdev))
-				netdev->netdev_ops->ndo_stop(netdev);
-			ixgbe_clear_interrupt_scheme(adapter);
-		} else {
-			if (netif_running(netdev))
-				ixgbe_down(adapter);
-		}
+		if (netif_running(netdev))
+			netdev->netdev_ops->ndo_stop(netdev);
+		ixgbe_clear_interrupt_scheme(adapter);
 	}
 
 	if (adapter->dcb_cfg.pfc_mode_enable) {
@@ -408,29 +395,51 @@ static u8 ixgbe_dcbnl_set_all(struct net_device *netdev)
 		}
 	}
 
-	if (adapter->dcb_set_bitmap & BIT_RESETLINK) {
-		if (adapter->dcb_set_bitmap & BIT_APP_UPCHG) {
-			ixgbe_init_interrupt_scheme(adapter);
-			if (netif_running(netdev))
-				netdev->netdev_ops->ndo_open(netdev);
-		} else {
-			if (netif_running(netdev))
-				ixgbe_up(adapter);
-		}
+	if (adapter->dcb_set_bitmap & BIT_APP_UPCHG) {
+		ixgbe_init_interrupt_scheme(adapter);
+		if (netif_running(netdev))
+			netdev->netdev_ops->ndo_open(netdev);
 		ret = DCB_HW_CHG_RST;
-	} else if (adapter->dcb_set_bitmap & BIT_PFC) {
-		if (adapter->hw.mac.type == ixgbe_mac_82598EB)
-			ixgbe_dcb_config_pfc_82598(&adapter->hw,
-			                           &adapter->dcb_cfg);
-		else if (adapter->hw.mac.type == ixgbe_mac_82599EB)
-			ixgbe_dcb_config_pfc_82599(&adapter->hw,
-			                           &adapter->dcb_cfg);
+	}
+
+	if (adapter->dcb_set_bitmap & BIT_PFC) {
+		u8 pfc_en;
+		ixgbe_dcb_unpack_pfc(&adapter->dcb_cfg, &pfc_en);
+		ixgbe_dcb_hw_pfc_config(&adapter->hw, pfc_en);
 		ret = DCB_HW_CHG;
 	}
+
+	if (adapter->dcb_set_bitmap & (BIT_PG_TX|BIT_PG_RX)) {
+		u16 refill[MAX_TRAFFIC_CLASS], max[MAX_TRAFFIC_CLASS];
+		u8 bwg_id[MAX_TRAFFIC_CLASS], prio_type[MAX_TRAFFIC_CLASS];
+		int max_frame = adapter->netdev->mtu + ETH_HLEN + ETH_FCS_LEN;
+
+#ifdef CONFIG_FCOE
+		if (adapter->netdev->features & NETIF_F_FCOE_MTU)
+			max_frame = max(max_frame, IXGBE_FCOE_JUMBO_FRAME_SIZE);
+#endif
+
+		ixgbe_dcb_calculate_tc_credits(&adapter->hw, &adapter->dcb_cfg,
+					       max_frame, DCB_TX_CONFIG);
+		ixgbe_dcb_calculate_tc_credits(&adapter->hw, &adapter->dcb_cfg,
+					       max_frame, DCB_RX_CONFIG);
+
+		ixgbe_dcb_unpack_refill(&adapter->dcb_cfg,
+					DCB_TX_CONFIG, refill);
+		ixgbe_dcb_unpack_max(&adapter->dcb_cfg, max);
+		ixgbe_dcb_unpack_bwgid(&adapter->dcb_cfg,
+				       DCB_TX_CONFIG, bwg_id);
+		ixgbe_dcb_unpack_prio(&adapter->dcb_cfg,
+				      DCB_TX_CONFIG, prio_type);
+
+		ixgbe_dcb_hw_ets_config(&adapter->hw, refill, max,
+					bwg_id, prio_type);
+	}
+
 	if (adapter->dcb_cfg.pfc_mode_enable)
 		adapter->hw.fc.current_mode = ixgbe_fc_pfc;
 
-	if (adapter->dcb_set_bitmap & BIT_RESETLINK)
+	if (adapter->dcb_set_bitmap & BIT_APP_UPCHG)
 		clear_bit(__IXGBE_RESETTING, &adapter->state);
 	adapter->dcb_set_bitmap = 0x00;
 	return ret;
@@ -568,18 +577,29 @@ static u8 ixgbe_dcbnl_setapp(struct net_device *netdev,
 	case DCB_APP_IDTYPE_ETHTYPE:
 #ifdef IXGBE_FCOE
 		if (id == ETH_P_FCOE) {
-			u8 tc;
-			struct ixgbe_adapter *adapter;
+			u8 old_tc;
+			struct ixgbe_adapter *adapter = netdev_priv(netdev);
 
-			adapter = netdev_priv(netdev);
-			tc = adapter->fcoe.tc;
+			/* Get current programmed tc */
+			old_tc = adapter->fcoe.tc;
 			rval = ixgbe_fcoe_setapp(adapter, up);
-			if ((!rval) && (tc != adapter->fcoe.tc) &&
-			    (adapter->flags & IXGBE_FLAG_DCB_ENABLED) &&
-			    (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)) {
+
+			if (rval ||
+			   !(adapter->flags & IXGBE_FLAG_DCB_ENABLED) ||
+			   !(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
+				break;
+
+			/* The FCoE application priority may be changed multiple
+			 * times in quick sucession with switches that build up
+			 * TLVs. To avoid creating uneeded device resets this
+			 * checks the actual HW configuration and clears
+			 * BIT_APP_UPCHG if a HW configuration change is not
+			 * need
+			 */
+			if (old_tc == adapter->fcoe.tc)
+				adapter->dcb_set_bitmap &= ~BIT_APP_UPCHG;
+			else
 				adapter->dcb_set_bitmap |= BIT_APP_UPCHG;
-				adapter->dcb_set_bitmap |= BIT_RESETLINK;
-			}
 		}
 #endif
 		break;
@@ -591,7 +611,98 @@ static u8 ixgbe_dcbnl_setapp(struct net_device *netdev,
 	return rval;
 }
 
+static int ixgbe_dcbnl_ieee_getets(struct net_device *dev,
+				   struct ieee_ets *ets)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(dev);
+	struct ieee_ets *my_ets = adapter->ixgbe_ieee_ets;
+
+	/* No IEEE PFC settings available */
+	if (!my_ets)
+		return -EINVAL;
+
+	ets->ets_cap = MAX_TRAFFIC_CLASS;
+	ets->cbs = my_ets->cbs;
+	memcpy(ets->tc_tx_bw, my_ets->tc_tx_bw, sizeof(ets->tc_tx_bw));
+	memcpy(ets->tc_rx_bw, my_ets->tc_rx_bw, sizeof(ets->tc_rx_bw));
+	memcpy(ets->tc_tsa, my_ets->tc_tsa, sizeof(ets->tc_tsa));
+	memcpy(ets->prio_tc, my_ets->prio_tc, sizeof(ets->prio_tc));
+	return 0;
+}
+
+static int ixgbe_dcbnl_ieee_setets(struct net_device *dev,
+				   struct ieee_ets *ets)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(dev);
+	__u16 refill[IEEE_8021QAZ_MAX_TCS], max[IEEE_8021QAZ_MAX_TCS];
+	int max_frame = dev->mtu + ETH_HLEN + ETH_FCS_LEN;
+	int err;
+	/* naively give each TC a bwg to map onto CEE hardware */
+	__u8 bwg_id[IEEE_8021QAZ_MAX_TCS] = {0, 1, 2, 3, 4, 5, 6, 7};
+
+	if (!adapter->ixgbe_ieee_ets) {
+		adapter->ixgbe_ieee_ets = kmalloc(sizeof(struct ieee_ets),
+						  GFP_KERNEL);
+		if (!adapter->ixgbe_ieee_ets)
+			return -ENOMEM;
+	}
+
+
+	memcpy(adapter->ixgbe_ieee_ets, ets, sizeof(*adapter->ixgbe_ieee_ets));
+
+	ixgbe_ieee_credits(ets->tc_tx_bw, refill, max, max_frame);
+	err = ixgbe_dcb_hw_ets_config(&adapter->hw, refill, max,
+				      bwg_id, ets->tc_tsa);
+	return err;
+}
+
+static int ixgbe_dcbnl_ieee_getpfc(struct net_device *dev,
+				   struct ieee_pfc *pfc)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(dev);
+	struct ieee_pfc *my_pfc = adapter->ixgbe_ieee_pfc;
+	int i;
+
+	/* No IEEE PFC settings available */
+	if (!my_pfc)
+		return -EINVAL;
+
+	pfc->pfc_cap = MAX_TRAFFIC_CLASS;
+	pfc->pfc_en = my_pfc->pfc_en;
+	pfc->mbc = my_pfc->mbc;
+	pfc->delay = my_pfc->delay;
+
+	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
+		pfc->requests[i] = adapter->stats.pxoffrxc[i];
+		pfc->indications[i] = adapter->stats.pxofftxc[i];
+	}
+
+	return 0;
+}
+
+static int ixgbe_dcbnl_ieee_setpfc(struct net_device *dev,
+				   struct ieee_pfc *pfc)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(dev);
+	int err;
+
+	if (!adapter->ixgbe_ieee_pfc) {
+		adapter->ixgbe_ieee_pfc = kmalloc(sizeof(struct ieee_pfc),
+						  GFP_KERNEL);
+		if (!adapter->ixgbe_ieee_pfc)
+			return -ENOMEM;
+	}
+
+	memcpy(adapter->ixgbe_ieee_pfc, pfc, sizeof(*adapter->ixgbe_ieee_pfc));
+	err = ixgbe_dcb_hw_pfc_config(&adapter->hw, pfc->pfc_en);
+	return err;
+}
+
 const struct dcbnl_rtnl_ops dcbnl_ops = {
+	.ieee_getets	= ixgbe_dcbnl_ieee_getets,
+	.ieee_setets	= ixgbe_dcbnl_ieee_setets,
+	.ieee_getpfc	= ixgbe_dcbnl_ieee_getpfc,
+	.ieee_setpfc	= ixgbe_dcbnl_ieee_setpfc,
 	.getstate	= ixgbe_dcbnl_get_state,
 	.setstate	= ixgbe_dcbnl_set_state,
 	.getpermhwaddr	= ixgbe_dcbnl_get_perm_hw_addr,
diff --git a/drivers/net/ixgbe/ixgbe_ethtool.c b/drivers/net/ixgbe/ixgbe_ethtool.c
index 2002ea88ca2a..83511c022926 100644
--- a/drivers/net/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ixgbe/ixgbe_ethtool.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -152,7 +152,17 @@ static int ixgbe_get_settings(struct net_device *netdev,
 		ecmd->supported |= (SUPPORTED_1000baseT_Full |
 		                    SUPPORTED_Autoneg);
 
+		switch (hw->mac.type) {
+		case ixgbe_mac_X540:
+			ecmd->supported |= SUPPORTED_100baseT_Full;
+			break;
+		default:
+			break;
+		}
+
 		ecmd->advertising = ADVERTISED_Autoneg;
+		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_1GB_FULL)
@@ -167,6 +177,15 @@ static int ixgbe_get_settings(struct net_device *netdev,
 			ecmd->advertising |= (ADVERTISED_10000baseT_Full |
 					      ADVERTISED_1000baseT_Full);
 
+		switch (hw->mac.type) {
+		case ixgbe_mac_X540:
+			if (!(ecmd->advertising & ADVERTISED_100baseT_Full))
+				ecmd->advertising |= (ADVERTISED_100baseT_Full);
+			break;
+		default:
+			break;
+		}
+
 		if (hw->phy.media_type == ixgbe_media_type_copper) {
 			ecmd->supported |= SUPPORTED_TP;
 			ecmd->advertising |= ADVERTISED_TP;
@@ -271,8 +290,19 @@ static int ixgbe_get_settings(struct net_device *netdev,
 
 	hw->mac.ops.check_link(hw, &link_speed, &link_up, false);
 	if (link_up) {
-		ecmd->speed = (link_speed == IXGBE_LINK_SPEED_10GB_FULL) ?
-		               SPEED_10000 : SPEED_1000;
+		switch (link_speed) {
+		case IXGBE_LINK_SPEED_10GB_FULL:
+			ecmd->speed = SPEED_10000;
+			break;
+		case IXGBE_LINK_SPEED_1GB_FULL:
+			ecmd->speed = SPEED_1000;
+			break;
+		case IXGBE_LINK_SPEED_100_FULL:
+			ecmd->speed = SPEED_100;
+			break;
+		default:
+			break;
+		}
 		ecmd->duplex = DUPLEX_FULL;
 	} else {
 		ecmd->speed = -1;
diff --git a/drivers/net/ixgbe/ixgbe_fcoe.c b/drivers/net/ixgbe/ixgbe_fcoe.c
index c54a88274d51..27203c87ea14 100644
--- a/drivers/net/ixgbe/ixgbe_fcoe.c
+++ b/drivers/net/ixgbe/ixgbe_fcoe.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
diff --git a/drivers/net/ixgbe/ixgbe_fcoe.h b/drivers/net/ixgbe/ixgbe_fcoe.h
index 65cc8fb14fe7..02a00d2415d9 100644
--- a/drivers/net/ixgbe/ixgbe_fcoe.h
+++ b/drivers/net/ixgbe/ixgbe_fcoe.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
diff --git a/drivers/net/ixgbe/ixgbe_main.c b/drivers/net/ixgbe/ixgbe_main.c
index 30f9ccfb4f87..32231ffe0717 100644
--- a/drivers/net/ixgbe/ixgbe_main.c
+++ b/drivers/net/ixgbe/ixgbe_main.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -54,7 +54,8 @@ static const char ixgbe_driver_string[] =
 
 #define DRV_VERSION "3.2.9-k2"
 const char ixgbe_driver_version[] = DRV_VERSION;
-static char ixgbe_copyright[] = "Copyright (c) 1999-2010 Intel Corporation.";
+static const char ixgbe_copyright[] =
+				"Copyright (c) 1999-2011 Intel Corporation.";
 
 static const struct ixgbe_info *ixgbe_info_tbl[] = {
 	[board_82598] = &ixgbe_82598_info,
@@ -648,7 +649,7 @@ void ixgbe_unmap_and_free_tx_resource(struct ixgbe_ring *tx_ring,
  *
  * Returns : a tc index for use in range 0-7, or 0-3
  */
-u8 ixgbe_dcb_txq_to_tc(struct ixgbe_adapter *adapter, u8 reg_idx)
+static u8 ixgbe_dcb_txq_to_tc(struct ixgbe_adapter *adapter, u8 reg_idx)
 {
 	int tc = -1;
 	int dcb_i = adapter->ring_feature[RING_F_DCB].indices;
@@ -2597,6 +2598,11 @@ static void ixgbe_free_irq(struct ixgbe_adapter *adapter)
 
 		i--;
 		for (; i >= 0; i--) {
+			/* free only the irqs that were actually requested */
+			if (!adapter->q_vector[i]->rxr_count &&
+			    !adapter->q_vector[i]->txr_count)
+				continue;
+
 			free_irq(adapter->msix_entries[i].vector,
 				 adapter->q_vector[i]);
 		}
@@ -3077,6 +3083,14 @@ void ixgbe_configure_rx_ring(struct ixgbe_adapter *adapter,
 	ixgbe_configure_srrctl(adapter, ring);
 	ixgbe_configure_rscctl(adapter, ring);
 
+	/* If operating in IOV mode set RLPML for X540 */
+	if ((adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) &&
+	    hw->mac.type == ixgbe_mac_X540) {
+		rxdctl &= ~IXGBE_RXDCTL_RLPMLMASK;
+		rxdctl |= ((ring->netdev->mtu + ETH_HLEN +
+			    ETH_FCS_LEN + VLAN_HLEN) | IXGBE_RXDCTL_RLPML_EN);
+	}
+
 	if (hw->mac.type == ixgbe_mac_82598EB) {
 		/*
 		 * enable cache line friendly hardware writes:
@@ -3876,7 +3890,7 @@ static int ixgbe_up_complete(struct ixgbe_adapter *adapter)
 	 * If we're not hot-pluggable SFP+, we just need to configure link
 	 * and bring it up.
 	 */
-	if (hw->phy.type == ixgbe_phy_unknown)
+	if (hw->phy.type == ixgbe_phy_none)
 		schedule_work(&adapter->sfp_config_module_task);
 
 	/* enable transmits */
@@ -5174,7 +5188,6 @@ static int __devinit ixgbe_sw_init(struct ixgbe_adapter *adapter)
 	adapter->dcb_cfg.bw_percentage[DCB_RX_CONFIG][0] = 100;
 	adapter->dcb_cfg.rx_pba_cfg = pba_equal;
 	adapter->dcb_cfg.pfc_mode_enable = false;
-	adapter->dcb_cfg.round_robin_enable = false;
 	adapter->dcb_set_bitmap = 0x00;
 	ixgbe_copy_dcb_cfg(&adapter->dcb_cfg, &adapter->temp_dcb_cfg,
 			   adapter->ring_feature[RING_F_DCB].indices);
@@ -5442,8 +5455,14 @@ static int ixgbe_change_mtu(struct net_device *netdev, int new_mtu)
 	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN;
 
 	/* MTU < 68 is an error and causes problems on some kernels */
-	if ((new_mtu < 68) || (max_frame > IXGBE_MAX_JUMBO_FRAME_SIZE))
-		return -EINVAL;
+	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED &&
+	    hw->mac.type != ixgbe_mac_X540) {
+		if ((new_mtu < 68) || (max_frame > MAXIMUM_ETHERNET_VLAN_SIZE))
+			return -EINVAL;
+	} else {
+		if ((new_mtu < 68) || (max_frame > IXGBE_MAX_JUMBO_FRAME_SIZE))
+			return -EINVAL;
+	}
 
 	e_info(probe, "changing MTU from %d to %d\n", netdev->mtu, new_mtu);
 	/* must set new MTU before calling down or up */
@@ -5611,6 +5630,10 @@ static int __ixgbe_shutdown(struct pci_dev *pdev, bool *enable_wake)
 	}
 
 	ixgbe_clear_interrupt_scheme(adapter);
+#ifdef CONFIG_DCB
+	kfree(adapter->ixgbe_ieee_pfc);
+	kfree(adapter->ixgbe_ieee_ets);
+#endif
 
 #ifdef CONFIG_PM
 	retval = pci_save_state(pdev);
@@ -6101,7 +6124,10 @@ static void ixgbe_watchdog_task(struct work_struct *work)
 			       (link_speed == IXGBE_LINK_SPEED_10GB_FULL ?
 			       "10 Gbps" :
 			       (link_speed == IXGBE_LINK_SPEED_1GB_FULL ?
-			       "1 Gbps" : "unknown speed")),
+			       "1 Gbps" :
+			       (link_speed == IXGBE_LINK_SPEED_100_FULL ?
+			       "100 Mbps" :
+			       "unknown speed"))),
 			       ((flow_rx && flow_tx) ? "RX/TX" :
 			       (flow_rx ? "RX" :
 			       (flow_tx ? "TX" : "None"))));
@@ -7706,16 +7732,6 @@ static int ixgbe_notify_dca(struct notifier_block *nb, unsigned long event,
 
 #endif /* CONFIG_IXGBE_DCA */
 
-/**
- * ixgbe_get_hw_dev return device
- * used by hardware layer to print debugging information
- **/
-struct net_device *ixgbe_get_hw_dev(struct ixgbe_hw *hw)
-{
-	struct ixgbe_adapter *adapter = hw->back;
-	return adapter->netdev;
-}
-
 module_exit(ixgbe_exit_module);
 
 /* ixgbe_main.c */
diff --git a/drivers/net/ixgbe/ixgbe_mbx.c b/drivers/net/ixgbe/ixgbe_mbx.c
index ea82c5a1cd3e..2acacfa5e375 100644
--- a/drivers/net/ixgbe/ixgbe_mbx.c
+++ b/drivers/net/ixgbe/ixgbe_mbx.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -437,6 +437,7 @@ out_no_read:
 	return ret_val;
 }
 
+#ifdef CONFIG_PCI_IOV
 /**
  *  ixgbe_init_mbx_params_pf - set initial values for pf mailbox
  *  @hw: pointer to the HW structure
@@ -465,6 +466,7 @@ void ixgbe_init_mbx_params_pf(struct ixgbe_hw *hw)
 		break;
 	}
 }
+#endif /* CONFIG_PCI_IOV */
 
 struct ixgbe_mbx_operations mbx_ops_generic = {
 	.read                   = ixgbe_read_mbx_pf,
diff --git a/drivers/net/ixgbe/ixgbe_mbx.h b/drivers/net/ixgbe/ixgbe_mbx.h
index 3df9b1590218..fe6ea81dc7f8 100644
--- a/drivers/net/ixgbe/ixgbe_mbx.h
+++ b/drivers/net/ixgbe/ixgbe_mbx.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -86,7 +86,9 @@ s32 ixgbe_write_mbx(struct ixgbe_hw *, u32 *, u16, u16);
 s32 ixgbe_check_for_msg(struct ixgbe_hw *, u16);
 s32 ixgbe_check_for_ack(struct ixgbe_hw *, u16);
 s32 ixgbe_check_for_rst(struct ixgbe_hw *, u16);
+#ifdef CONFIG_PCI_IOV
 void ixgbe_init_mbx_params_pf(struct ixgbe_hw *);
+#endif /* CONFIG_PCI_IOV */
 
 extern struct ixgbe_mbx_operations mbx_ops_generic;
 
diff --git a/drivers/net/ixgbe/ixgbe_phy.c b/drivers/net/ixgbe/ixgbe_phy.c
index 8f7123e8fc0a..197230b2d1ac 100644
--- a/drivers/net/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ixgbe/ixgbe_phy.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -138,17 +138,51 @@ static enum ixgbe_phy_type ixgbe_get_phy_type_from_id(u32 phy_id)
  **/
 s32 ixgbe_reset_phy_generic(struct ixgbe_hw *hw)
 {
+	u32 i;
+	u16 ctrl = 0;
+	s32 status = 0;
+
+	if (hw->phy.type == ixgbe_phy_unknown)
+		status = ixgbe_identify_phy_generic(hw);
+
+	if (status != 0 || hw->phy.type == ixgbe_phy_none)
+		goto out;
+
 	/* Don't reset PHY if it's shut down due to overtemp. */
 	if (!hw->phy.reset_if_overtemp &&
 	    (IXGBE_ERR_OVERTEMP == hw->phy.ops.check_overtemp(hw)))
-		return 0;
+		goto out;
 
 	/*
 	 * Perform soft PHY reset to the PHY_XS.
 	 * This will cause a soft reset to the PHY
 	 */
-	return hw->phy.ops.write_reg(hw, MDIO_CTRL1, MDIO_MMD_PHYXS,
-				     MDIO_CTRL1_RESET);
+	hw->phy.ops.write_reg(hw, MDIO_CTRL1,
+			      MDIO_MMD_PHYXS,
+			      MDIO_CTRL1_RESET);
+
+	/*
+	 * Poll for reset bit to self-clear indicating reset is complete.
+	 * Some PHYs could take up to 3 seconds to complete and need about
+	 * 1.7 usec delay after the reset is complete.
+	 */
+	for (i = 0; i < 30; i++) {
+		msleep(100);
+		hw->phy.ops.read_reg(hw, MDIO_CTRL1,
+				     MDIO_MMD_PHYXS, &ctrl);
+		if (!(ctrl & MDIO_CTRL1_RESET)) {
+			udelay(2);
+			break;
+		}
+	}
+
+	if (ctrl & MDIO_CTRL1_RESET) {
+		status = IXGBE_ERR_RESET_FAILED;
+		hw_dbg(hw, "PHY reset polling failed to complete.\n");
+	}
+
+out:
+	return status;
 }
 
 /**
@@ -171,7 +205,7 @@ s32 ixgbe_read_phy_reg_generic(struct ixgbe_hw *hw, u32 reg_addr,
 	else
 		gssr = IXGBE_GSSR_PHY0_SM;
 
-	if (ixgbe_acquire_swfw_sync(hw, gssr) != 0)
+	if (hw->mac.ops.acquire_swfw_sync(hw, gssr) != 0)
 		status = IXGBE_ERR_SWFW_SYNC;
 
 	if (status == 0) {
@@ -243,7 +277,7 @@ s32 ixgbe_read_phy_reg_generic(struct ixgbe_hw *hw, u32 reg_addr,
 			}
 		}
 
-		ixgbe_release_swfw_sync(hw, gssr);
+		hw->mac.ops.release_swfw_sync(hw, gssr);
 	}
 
 	return status;
@@ -269,7 +303,7 @@ s32 ixgbe_write_phy_reg_generic(struct ixgbe_hw *hw, u32 reg_addr,
 	else
 		gssr = IXGBE_GSSR_PHY0_SM;
 
-	if (ixgbe_acquire_swfw_sync(hw, gssr) != 0)
+	if (hw->mac.ops.acquire_swfw_sync(hw, gssr) != 0)
 		status = IXGBE_ERR_SWFW_SYNC;
 
 	if (status == 0) {
@@ -336,7 +370,7 @@ s32 ixgbe_write_phy_reg_generic(struct ixgbe_hw *hw, u32 reg_addr,
 			}
 		}
 
-		ixgbe_release_swfw_sync(hw, gssr);
+		hw->mac.ops.release_swfw_sync(hw, gssr);
 	}
 
 	return status;
@@ -556,11 +590,10 @@ out:
 }
 
 /**
- *  ixgbe_identify_sfp_module_generic - Identifies SFP module and assigns
- *                                      the PHY type.
+ *  ixgbe_identify_sfp_module_generic - Identifies SFP modules
  *  @hw: pointer to hardware structure
  *
- *  Searches for and indentifies the SFP module.  Assings appropriate PHY type.
+ *  Searches for and identifies the SFP module and assigns appropriate PHY type.
  **/
 s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 {
@@ -581,41 +614,62 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 		goto out;
 	}
 
-	status = hw->phy.ops.read_i2c_eeprom(hw, IXGBE_SFF_IDENTIFIER,
+	status = hw->phy.ops.read_i2c_eeprom(hw,
+					     IXGBE_SFF_IDENTIFIER,
 	                                     &identifier);
 
-	if (status == IXGBE_ERR_SFP_NOT_PRESENT || status == IXGBE_ERR_I2C) {
-		status = IXGBE_ERR_SFP_NOT_PRESENT;
-		hw->phy.sfp_type = ixgbe_sfp_type_not_present;
-		if (hw->phy.type != ixgbe_phy_nl) {
-			hw->phy.id = 0;
-			hw->phy.type = ixgbe_phy_unknown;
-		}
-		goto out;
-	}
+	if (status == IXGBE_ERR_SWFW_SYNC ||
+	    status == IXGBE_ERR_I2C ||
+	    status == IXGBE_ERR_SFP_NOT_PRESENT)
+		goto err_read_i2c_eeprom;
 
-	if (identifier == IXGBE_SFF_IDENTIFIER_SFP) {
-		hw->phy.ops.read_i2c_eeprom(hw, IXGBE_SFF_1GBE_COMP_CODES,
-		                            &comp_codes_1g);
-		hw->phy.ops.read_i2c_eeprom(hw, IXGBE_SFF_10GBE_COMP_CODES,
-		                            &comp_codes_10g);
-		hw->phy.ops.read_i2c_eeprom(hw, IXGBE_SFF_CABLE_TECHNOLOGY,
-		                            &cable_tech);
-
-		/* ID Module
-		 * =========
-		 * 0    SFP_DA_CU
-		 * 1    SFP_SR
-		 * 2    SFP_LR
-		 * 3    SFP_DA_CORE0 - 82599-specific
-		 * 4    SFP_DA_CORE1 - 82599-specific
-		 * 5    SFP_SR/LR_CORE0 - 82599-specific
-		 * 6    SFP_SR/LR_CORE1 - 82599-specific
-		 * 7    SFP_act_lmt_DA_CORE0 - 82599-specific
-		 * 8    SFP_act_lmt_DA_CORE1 - 82599-specific
-		 * 9    SFP_1g_cu_CORE0 - 82599-specific
-		 * 10   SFP_1g_cu_CORE1 - 82599-specific
-		 */
+	/* LAN ID is needed for sfp_type determination */
+	hw->mac.ops.set_lan_id(hw);
+
+	if (identifier != IXGBE_SFF_IDENTIFIER_SFP) {
+		hw->phy.type = ixgbe_phy_sfp_unsupported;
+		status = IXGBE_ERR_SFP_NOT_SUPPORTED;
+	} else {
+		status = hw->phy.ops.read_i2c_eeprom(hw,
+						     IXGBE_SFF_1GBE_COMP_CODES,
+						     &comp_codes_1g);
+
+		if (status == IXGBE_ERR_SWFW_SYNC ||
+		    status == IXGBE_ERR_I2C ||
+		    status == IXGBE_ERR_SFP_NOT_PRESENT)
+			goto err_read_i2c_eeprom;
+
+		status = hw->phy.ops.read_i2c_eeprom(hw,
+						     IXGBE_SFF_10GBE_COMP_CODES,
+						     &comp_codes_10g);
+
+		if (status == IXGBE_ERR_SWFW_SYNC ||
+		    status == IXGBE_ERR_I2C ||
+		    status == IXGBE_ERR_SFP_NOT_PRESENT)
+			goto err_read_i2c_eeprom;
+		status = hw->phy.ops.read_i2c_eeprom(hw,
+						     IXGBE_SFF_CABLE_TECHNOLOGY,
+						     &cable_tech);
+
+		if (status == IXGBE_ERR_SWFW_SYNC ||
+		    status == IXGBE_ERR_I2C ||
+		    status == IXGBE_ERR_SFP_NOT_PRESENT)
+			goto err_read_i2c_eeprom;
+
+		 /* ID Module
+		  * =========
+		  * 0   SFP_DA_CU
+		  * 1   SFP_SR
+		  * 2   SFP_LR
+		  * 3   SFP_DA_CORE0 - 82599-specific
+		  * 4   SFP_DA_CORE1 - 82599-specific
+		  * 5   SFP_SR/LR_CORE0 - 82599-specific
+		  * 6   SFP_SR/LR_CORE1 - 82599-specific
+		  * 7   SFP_act_lmt_DA_CORE0 - 82599-specific
+		  * 8   SFP_act_lmt_DA_CORE1 - 82599-specific
+		  * 9   SFP_1g_cu_CORE0 - 82599-specific
+		  * 10  SFP_1g_cu_CORE1 - 82599-specific
+		  */
 		if (hw->mac.type == ixgbe_mac_82598EB) {
 			if (cable_tech & IXGBE_SFF_DA_PASSIVE_CABLE)
 				hw->phy.sfp_type = ixgbe_sfp_type_da_cu;
@@ -647,31 +701,27 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 						ixgbe_sfp_type_da_act_lmt_core1;
 				} else {
 					hw->phy.sfp_type =
-						ixgbe_sfp_type_unknown;
+							ixgbe_sfp_type_unknown;
 				}
-			} else if (comp_codes_10g & IXGBE_SFF_10GBASESR_CAPABLE)
+			} else if (comp_codes_10g &
+				   (IXGBE_SFF_10GBASESR_CAPABLE |
+				    IXGBE_SFF_10GBASELR_CAPABLE)) {
 				if (hw->bus.lan_id == 0)
 					hw->phy.sfp_type =
 					              ixgbe_sfp_type_srlr_core0;
 				else
 					hw->phy.sfp_type =
 					              ixgbe_sfp_type_srlr_core1;
-			else if (comp_codes_10g & IXGBE_SFF_10GBASELR_CAPABLE)
-				if (hw->bus.lan_id == 0)
-					hw->phy.sfp_type =
-					              ixgbe_sfp_type_srlr_core0;
-				else
-					hw->phy.sfp_type =
-					              ixgbe_sfp_type_srlr_core1;
-			else if (comp_codes_1g & IXGBE_SFF_1GBASET_CAPABLE)
+			} else if (comp_codes_1g & IXGBE_SFF_1GBASET_CAPABLE) {
 				if (hw->bus.lan_id == 0)
 					hw->phy.sfp_type =
 						ixgbe_sfp_type_1g_cu_core0;
 				else
 					hw->phy.sfp_type =
 						ixgbe_sfp_type_1g_cu_core1;
-			else
+			} else {
 				hw->phy.sfp_type = ixgbe_sfp_type_unknown;
+			}
 		}
 
 		if (hw->phy.sfp_type != stored_sfp_type)
@@ -688,16 +738,33 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 		/* Determine PHY vendor */
 		if (hw->phy.type != ixgbe_phy_nl) {
 			hw->phy.id = identifier;
-			hw->phy.ops.read_i2c_eeprom(hw,
+			status = hw->phy.ops.read_i2c_eeprom(hw,
 			                            IXGBE_SFF_VENDOR_OUI_BYTE0,
 			                            &oui_bytes[0]);
-			hw->phy.ops.read_i2c_eeprom(hw,
+
+			if (status == IXGBE_ERR_SWFW_SYNC ||
+			    status == IXGBE_ERR_I2C ||
+			    status == IXGBE_ERR_SFP_NOT_PRESENT)
+				goto err_read_i2c_eeprom;
+
+			status = hw->phy.ops.read_i2c_eeprom(hw,
 			                            IXGBE_SFF_VENDOR_OUI_BYTE1,
 			                            &oui_bytes[1]);
-			hw->phy.ops.read_i2c_eeprom(hw,
+
+			if (status == IXGBE_ERR_SWFW_SYNC ||
+			    status == IXGBE_ERR_I2C ||
+			    status == IXGBE_ERR_SFP_NOT_PRESENT)
+				goto err_read_i2c_eeprom;
+
+			status = hw->phy.ops.read_i2c_eeprom(hw,
 			                            IXGBE_SFF_VENDOR_OUI_BYTE2,
 			                            &oui_bytes[2]);
 
+			if (status == IXGBE_ERR_SWFW_SYNC ||
+			    status == IXGBE_ERR_I2C ||
+			    status == IXGBE_ERR_SFP_NOT_PRESENT)
+				goto err_read_i2c_eeprom;
+
 			vendor_oui =
 			  ((oui_bytes[0] << IXGBE_SFF_VENDOR_OUI_BYTE0_SHIFT) |
 			   (oui_bytes[1] << IXGBE_SFF_VENDOR_OUI_BYTE1_SHIFT) |
@@ -707,7 +774,7 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 			case IXGBE_SFF_VENDOR_OUI_TYCO:
 				if (cable_tech & IXGBE_SFF_DA_PASSIVE_CABLE)
 					hw->phy.type =
-						ixgbe_phy_sfp_passive_tyco;
+						    ixgbe_phy_sfp_passive_tyco;
 				break;
 			case IXGBE_SFF_VENDOR_OUI_FTL:
 				if (cable_tech & IXGBE_SFF_DA_ACTIVE_CABLE)
@@ -724,7 +791,7 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 			default:
 				if (cable_tech & IXGBE_SFF_DA_PASSIVE_CABLE)
 					hw->phy.type =
-						ixgbe_phy_sfp_passive_unknown;
+						 ixgbe_phy_sfp_passive_unknown;
 				else if (cable_tech & IXGBE_SFF_DA_ACTIVE_CABLE)
 					hw->phy.type =
 						ixgbe_phy_sfp_active_unknown;
@@ -734,7 +801,7 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 			}
 		}
 
-		/* All passive DA cables are supported */
+		/* Allow any DA cable vendor */
 		if (cable_tech & (IXGBE_SFF_DA_PASSIVE_CABLE |
 		    IXGBE_SFF_DA_ACTIVE_CABLE)) {
 			status = 0;
@@ -776,15 +843,24 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 
 out:
 	return status;
+
+err_read_i2c_eeprom:
+	hw->phy.sfp_type = ixgbe_sfp_type_not_present;
+	if (hw->phy.type != ixgbe_phy_nl) {
+		hw->phy.id = 0;
+		hw->phy.type = ixgbe_phy_unknown;
+	}
+	return IXGBE_ERR_SFP_NOT_PRESENT;
 }
 
 /**
- *  ixgbe_get_sfp_init_sequence_offsets - Checks the MAC's EEPROM to see
- *  if it supports a given SFP+ module type, if so it returns the offsets to the
- *  phy init sequence block.
+ *  ixgbe_get_sfp_init_sequence_offsets - Provides offset of PHY init sequence
  *  @hw: pointer to hardware structure
  *  @list_offset: offset to the SFP ID list
  *  @data_offset: offset to the SFP data block
+ *
+ *  Checks the MAC's EEPROM to see if it supports a given SFP+ module type, if
+ *  so it returns the offsets to the phy init sequence block.
  **/
 s32 ixgbe_get_sfp_init_sequence_offsets(struct ixgbe_hw *hw,
                                         u16 *list_offset,
@@ -899,11 +975,22 @@ s32 ixgbe_read_i2c_byte_generic(struct ixgbe_hw *hw, u8 byte_offset,
                                 u8 dev_addr, u8 *data)
 {
 	s32 status = 0;
-	u32 max_retry = 1;
+	u32 max_retry = 10;
 	u32 retry = 0;
+	u16 swfw_mask = 0;
 	bool nack = 1;
 
+	if (IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_LAN_ID_1)
+		swfw_mask = IXGBE_GSSR_PHY1_SM;
+	else
+		swfw_mask = IXGBE_GSSR_PHY0_SM;
+
 	do {
+		if (ixgbe_acquire_swfw_sync(hw, swfw_mask) != 0) {
+			status = IXGBE_ERR_SWFW_SYNC;
+			goto read_byte_out;
+		}
+
 		ixgbe_i2c_start(hw);
 
 		/* Device Address and write indication */
@@ -946,6 +1033,8 @@ s32 ixgbe_read_i2c_byte_generic(struct ixgbe_hw *hw, u8 byte_offset,
 		break;
 
 fail:
+		ixgbe_release_swfw_sync(hw, swfw_mask);
+		msleep(100);
 		ixgbe_i2c_bus_clear(hw);
 		retry++;
 		if (retry < max_retry)
@@ -955,6 +1044,9 @@ fail:
 
 	} while (retry < max_retry);
 
+	ixgbe_release_swfw_sync(hw, swfw_mask);
+
+read_byte_out:
 	return status;
 }
 
@@ -973,6 +1065,17 @@ s32 ixgbe_write_i2c_byte_generic(struct ixgbe_hw *hw, u8 byte_offset,
 	s32 status = 0;
 	u32 max_retry = 1;
 	u32 retry = 0;
+	u16 swfw_mask = 0;
+
+	if (IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_LAN_ID_1)
+		swfw_mask = IXGBE_GSSR_PHY1_SM;
+	else
+		swfw_mask = IXGBE_GSSR_PHY0_SM;
+
+	if (ixgbe_acquire_swfw_sync(hw, swfw_mask) != 0) {
+		status = IXGBE_ERR_SWFW_SYNC;
+		goto write_byte_out;
+	}
 
 	do {
 		ixgbe_i2c_start(hw);
@@ -1013,6 +1116,9 @@ fail:
 			hw_dbg(hw, "I2C byte write error.\n");
 	} while (retry < max_retry);
 
+	ixgbe_release_swfw_sync(hw, swfw_mask);
+
+write_byte_out:
 	return status;
 }
 
@@ -1331,6 +1437,8 @@ static void ixgbe_i2c_bus_clear(struct ixgbe_hw *hw)
 	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL);
 	u32 i;
 
+	ixgbe_i2c_start(hw);
+
 	ixgbe_set_i2c_data(hw, &i2cctl, 1);
 
 	for (i = 0; i < 9; i++) {
@@ -1345,6 +1453,8 @@ static void ixgbe_i2c_bus_clear(struct ixgbe_hw *hw)
 		udelay(IXGBE_I2C_T_LOW);
 	}
 
+	ixgbe_i2c_start(hw);
+
 	/* Put the i2c bus back to default state */
 	ixgbe_i2c_stop(hw);
 }
diff --git a/drivers/net/ixgbe/ixgbe_phy.h b/drivers/net/ixgbe/ixgbe_phy.h
index e2c6b7eac641..2327baf04426 100644
--- a/drivers/net/ixgbe/ixgbe_phy.h
+++ b/drivers/net/ixgbe/ixgbe_phy.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
diff --git a/drivers/net/ixgbe/ixgbe_sriov.c b/drivers/net/ixgbe/ixgbe_sriov.c
index 187b3a16ec1f..58c9b45989ff 100644
--- a/drivers/net/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ixgbe/ixgbe_sriov.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -110,6 +110,33 @@ static int ixgbe_set_vf_vlan(struct ixgbe_adapter *adapter, int add, int vid,
 	return adapter->hw.mac.ops.set_vfta(&adapter->hw, vid, vf, (bool)add);
 }
 
+void ixgbe_set_vf_lpe(struct ixgbe_adapter *adapter, u32 *msgbuf)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	int new_mtu = msgbuf[1];
+	u32 max_frs;
+	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN;
+
+	/* Only X540 supports jumbo frames in IOV mode */
+	if (adapter->hw.mac.type != ixgbe_mac_X540)
+		return;
+
+	/* MTU < 68 is an error and causes problems on some kernels */
+	if ((new_mtu < 68) || (max_frame > IXGBE_MAX_JUMBO_FRAME_SIZE)) {
+		e_err(drv, "VF mtu %d out of range\n", new_mtu);
+		return;
+	}
+
+	max_frs = (IXGBE_READ_REG(hw, IXGBE_MAXFRS) &
+		   IXGBE_MHADD_MFS_MASK) >> IXGBE_MHADD_MFS_SHIFT;
+	if (max_frs < new_mtu) {
+		max_frs = new_mtu << IXGBE_MHADD_MFS_SHIFT;
+		IXGBE_WRITE_REG(hw, IXGBE_MAXFRS, max_frs);
+	}
+
+	e_info(hw, "VF requests change max MTU to %d\n", new_mtu);
+}
+
 static void ixgbe_set_vmolr(struct ixgbe_hw *hw, u32 vf, bool aupe)
 {
 	u32 vmolr = IXGBE_READ_REG(hw, IXGBE_VMOLR(vf));
@@ -302,7 +329,7 @@ static int ixgbe_rcv_msg_from_vf(struct ixgbe_adapter *adapter, u32 vf)
 		                                 hash_list, vf);
 		break;
 	case IXGBE_VF_SET_LPE:
-		WARN_ON((msgbuf[0] & 0xFFFF) == IXGBE_VF_SET_LPE);
+		ixgbe_set_vf_lpe(adapter, msgbuf);
 		break;
 	case IXGBE_VF_SET_VLAN:
 		add = (msgbuf[0] & IXGBE_VT_MSGINFO_MASK)
diff --git a/drivers/net/ixgbe/ixgbe_sriov.h b/drivers/net/ixgbe/ixgbe_sriov.h
index 49dc14debef7..e7dd029d576a 100644
--- a/drivers/net/ixgbe/ixgbe_sriov.h
+++ b/drivers/net/ixgbe/ixgbe_sriov.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
diff --git a/drivers/net/ixgbe/ixgbe_type.h b/drivers/net/ixgbe/ixgbe_type.h
index fd3358f54139..013751db5fc0 100644
--- a/drivers/net/ixgbe/ixgbe_type.h
+++ b/drivers/net/ixgbe/ixgbe_type.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -91,7 +91,7 @@
 
 /* General Receive Control */
 #define IXGBE_GRC_MNG  0x00000001 /* Manageability Enable */
-#define IXGBE_GRC_APME 0x00000002 /* Advanced Power Management Enable */
+#define IXGBE_GRC_APME 0x00000002 /* APM enabled in EEPROM */
 
 #define IXGBE_VPDDIAG0  0x10204
 #define IXGBE_VPDDIAG1  0x10208
@@ -342,7 +342,7 @@
 /* Wake Up Control */
 #define IXGBE_WUC_PME_EN     0x00000002 /* PME Enable */
 #define IXGBE_WUC_PME_STATUS 0x00000004 /* PME Status */
-#define IXGBE_WUC_ADVD3WUC   0x00000010 /* D3Cold wake up cap. enable*/
+#define IXGBE_WUC_WKEN       0x00000010 /* Enable PE_WAKE_N pin assertion  */
 
 /* Wake Up Filter Control */
 #define IXGBE_WUFC_LNKC 0x00000001 /* Link Status Change Wakeup Enable */
@@ -1614,6 +1614,8 @@
 #define IXGBE_ALT_SAN_MAC_ADDR_CAPS_ALTWWN  0x1 /* Alt. WWN base exists */
 
 /* PCI Bus Info */
+#define IXGBE_PCI_DEVICE_STATUS   0xAA
+#define IXGBE_PCI_DEVICE_STATUS_TRANSACTION_PENDING   0x0020
 #define IXGBE_PCI_LINK_STATUS     0xB2
 #define IXGBE_PCI_DEVICE_CONTROL2 0xC8
 #define IXGBE_PCI_LINK_WIDTH      0x3F0
@@ -1680,6 +1682,8 @@
 #define IXGBE_RXCTRL_DMBYPS     0x00000002  /* Descriptor Monitor Bypass */
 #define IXGBE_RXDCTL_ENABLE     0x02000000  /* Enable specific Rx Queue */
 #define IXGBE_RXDCTL_VME        0x40000000  /* VLAN mode enable */
+#define IXGBE_RXDCTL_RLPMLMASK  0x00003FFF  /* Only supported on the X540 */
+#define IXGBE_RXDCTL_RLPML_EN   0x00008000
 
 #define IXGBE_FCTRL_SBP 0x00000002 /* Store Bad Packet */
 #define IXGBE_FCTRL_MPE 0x00000100 /* Multicast Promiscuous Ena*/
@@ -2240,6 +2244,7 @@ enum ixgbe_mac_type {
 
 enum ixgbe_phy_type {
 	ixgbe_phy_unknown = 0,
+	ixgbe_phy_none,
 	ixgbe_phy_tn,
 	ixgbe_phy_aq,
 	ixgbe_phy_cu_unknown,
@@ -2328,32 +2333,31 @@ enum ixgbe_bus_type {
 /* PCI bus speeds */
 enum ixgbe_bus_speed {
 	ixgbe_bus_speed_unknown = 0,
-	ixgbe_bus_speed_33,
-	ixgbe_bus_speed_66,
-	ixgbe_bus_speed_100,
-	ixgbe_bus_speed_120,
-	ixgbe_bus_speed_133,
-	ixgbe_bus_speed_2500,
-	ixgbe_bus_speed_5000,
+	ixgbe_bus_speed_33      = 33,
+	ixgbe_bus_speed_66      = 66,
+	ixgbe_bus_speed_100     = 100,
+	ixgbe_bus_speed_120     = 120,
+	ixgbe_bus_speed_133     = 133,
+	ixgbe_bus_speed_2500    = 2500,
+	ixgbe_bus_speed_5000    = 5000,
 	ixgbe_bus_speed_reserved
 };
 
 /* PCI bus widths */
 enum ixgbe_bus_width {
 	ixgbe_bus_width_unknown = 0,
-	ixgbe_bus_width_pcie_x1,
-	ixgbe_bus_width_pcie_x2,
+	ixgbe_bus_width_pcie_x1 = 1,
+	ixgbe_bus_width_pcie_x2 = 2,
 	ixgbe_bus_width_pcie_x4 = 4,
 	ixgbe_bus_width_pcie_x8 = 8,
-	ixgbe_bus_width_32,
-	ixgbe_bus_width_64,
+	ixgbe_bus_width_32      = 32,
+	ixgbe_bus_width_64      = 64,
 	ixgbe_bus_width_reserved
 };
 
 struct ixgbe_addr_filter_info {
 	u32 num_mc_addrs;
 	u32 rar_used_count;
-	u32 mc_addr_in_rar_count;
 	u32 mta_in_use;
 	u32 overflow_promisc;
 	bool uc_set_promisc;
@@ -2491,6 +2495,8 @@ struct ixgbe_mac_operations {
 	s32 (*write_analog_reg8)(struct ixgbe_hw*, u32, u8);
 	s32 (*setup_sfp)(struct ixgbe_hw *);
 	s32 (*enable_rx_dma)(struct ixgbe_hw *, u32);
+	s32 (*acquire_swfw_sync)(struct ixgbe_hw *, u16);
+	void (*release_swfw_sync)(struct ixgbe_hw *, u16);
 
 	/* Link */
 	void (*disable_tx_laser)(struct ixgbe_hw *);
@@ -2513,7 +2519,6 @@ struct ixgbe_mac_operations {
 	s32 (*set_vmdq)(struct ixgbe_hw *, u32, u32);
 	s32 (*clear_vmdq)(struct ixgbe_hw *, u32, u32);
 	s32 (*init_rx_addrs)(struct ixgbe_hw *);
-	s32 (*update_uc_addr_list)(struct ixgbe_hw *, struct net_device *);
 	s32 (*update_mc_addr_list)(struct ixgbe_hw *, struct net_device *);
 	s32 (*enable_mc)(struct ixgbe_hw *);
 	s32 (*disable_mc)(struct ixgbe_hw *);
@@ -2554,6 +2559,7 @@ struct ixgbe_eeprom_info {
 	u16                             address_bits;
 };
 
+#define IXGBE_FLAGS_DOUBLE_RESET_REQUIRED	0x01
 struct ixgbe_mac_info {
 	struct ixgbe_mac_operations     ops;
 	enum ixgbe_mac_type             type;
@@ -2564,6 +2570,8 @@ struct ixgbe_mac_info {
 	u16                             wwnn_prefix;
 	/* prefix for World Wide Port Name (WWPN) */
 	u16                             wwpn_prefix;
+#define IXGBE_MAX_MTA			128
+	u32				mta_shadow[IXGBE_MAX_MTA];
 	s32                             mc_filter_type;
 	u32                             mcft_size;
 	u32                             vft_size;
@@ -2576,6 +2584,7 @@ struct ixgbe_mac_info {
 	u32                             orig_autoc2;
 	bool                            orig_link_settings_stored;
 	bool                            autotry_restart;
+	u8                              flags;
 };
 
 struct ixgbe_phy_info {
@@ -2682,7 +2691,6 @@ struct ixgbe_info {
 #define IXGBE_ERR_EEPROM_VERSION                -24
 #define IXGBE_ERR_NO_SPACE                      -25
 #define IXGBE_ERR_OVERTEMP                      -26
-#define IXGBE_ERR_RAR_INDEX                     -27
 #define IXGBE_ERR_SFP_SETUP_NOT_COMPLETE        -30
 #define IXGBE_ERR_PBA_SECTION                   -31
 #define IXGBE_ERR_INVALID_ARGUMENT              -32
diff --git a/drivers/net/ixgbe/ixgbe_x540.c b/drivers/net/ixgbe/ixgbe_x540.c
index f2518b01067d..2e3a2b4fa8b2 100644
--- a/drivers/net/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ixgbe/ixgbe_x540.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2010 Intel Corporation.
+  Copyright(c) 1999 - 2011 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,
@@ -31,7 +31,6 @@
 
 #include "ixgbe.h"
 #include "ixgbe_phy.h"
-//#include "ixgbe_mbx.h"
 
 #define IXGBE_X540_MAX_TX_QUEUES 128
 #define IXGBE_X540_MAX_RX_QUEUES 128
@@ -110,12 +109,9 @@ static s32 ixgbe_reset_hw_X540(struct ixgbe_hw *hw)
 	 * Prevent the PCI-E bus from from hanging by disabling PCI-E master
 	 * access and verify no pending requests before reset
 	 */
-	status = ixgbe_disable_pcie_master(hw);
-	if (status != 0) {
-		status = IXGBE_ERR_MASTER_REQUESTS_PENDING;
-		hw_dbg(hw, "PCI-E Master disable polling has failed.\n");
-	}
+	ixgbe_disable_pcie_master(hw);
 
+mac_reset_top:
 	/*
 	 * Issue global reset to the MAC.  Needs to be SW reset if link is up.
 	 * If link reset is used when link is up, it might reset the PHY when
@@ -148,6 +144,19 @@ static s32 ixgbe_reset_hw_X540(struct ixgbe_hw *hw)
 		hw_dbg(hw, "Reset polling failed to complete.\n");
 	}
 
+	/*
+	 * Double resets are required for recovery from certain error
+	 * conditions.  Between resets, it is necessary to stall to allow time
+	 * for any pending HW events to complete.  We use 1usec since that is
+	 * what is needed for ixgbe_disable_pcie_master().  The second reset
+	 * then clears out any effects of those events.
+	 */
+	if (hw->mac.flags & IXGBE_FLAGS_DOUBLE_RESET_REQUIRED) {
+		hw->mac.flags &= ~IXGBE_FLAGS_DOUBLE_RESET_REQUIRED;
+		udelay(1);
+		goto mac_reset_top;
+	}
+
 	/* Clear PF Reset Done bit so PF/VF Mail Ops can work */
 	ctrl_ext = IXGBE_READ_REG(hw, IXGBE_CTRL_EXT);
 	ctrl_ext |= IXGBE_CTRL_EXT_PFRSTD;
@@ -191,7 +200,7 @@ static s32 ixgbe_reset_hw_X540(struct ixgbe_hw *hw)
 	 * clear the multicast table.  Also reset num_rar_entries to 128,
 	 * since we modify this value when programming the SAN MAC address.
 	 */
-	hw->mac.num_rar_entries = 128;
+	hw->mac.num_rar_entries = IXGBE_X540_MAX_TX_QUEUES;
 	hw->mac.ops.init_rx_addrs(hw);
 
 	/* Store the permanent mac address */
@@ -278,7 +287,7 @@ static s32 ixgbe_read_eerd_X540(struct ixgbe_hw *hw, u16 offset, u16 *data)
 {
 	s32 status;
 
-	if (ixgbe_acquire_swfw_sync_X540(hw, IXGBE_GSSR_EEP_SM) == 0)
+	if (hw->mac.ops.acquire_swfw_sync(hw, IXGBE_GSSR_EEP_SM) == 0)
 		status = ixgbe_read_eerd_generic(hw, offset, data);
 	else
 		status = IXGBE_ERR_SWFW_SYNC;
@@ -311,7 +320,7 @@ static s32 ixgbe_write_eewr_X540(struct ixgbe_hw *hw, u16 offset, u16 data)
 	       (data << IXGBE_EEPROM_RW_REG_DATA) |
 	       IXGBE_EEPROM_RW_REG_START;
 
-	if (ixgbe_acquire_swfw_sync_X540(hw, IXGBE_GSSR_EEP_SM) == 0) {
+	if (hw->mac.ops.acquire_swfw_sync(hw, IXGBE_GSSR_EEP_SM) == 0) {
 		status = ixgbe_poll_eerd_eewr_done(hw, IXGBE_NVM_POLL_WRITE);
 		if (status != 0) {
 			hw_dbg(hw, "Eeprom write EEWR timed out\n");
@@ -676,7 +685,6 @@ static struct ixgbe_mac_operations mac_ops_X540 = {
 	.set_vmdq               = &ixgbe_set_vmdq_generic,
 	.clear_vmdq             = &ixgbe_clear_vmdq_generic,
 	.init_rx_addrs          = &ixgbe_init_rx_addrs_generic,
-	.update_uc_addr_list    = &ixgbe_update_uc_addr_list_generic,
 	.update_mc_addr_list    = &ixgbe_update_mc_addr_list_generic,
 	.enable_mc              = &ixgbe_enable_mc_generic,
 	.disable_mc             = &ixgbe_disable_mc_generic,
@@ -687,6 +695,8 @@ static struct ixgbe_mac_operations mac_ops_X540 = {
 	.setup_sfp              = NULL,
 	.set_mac_anti_spoofing  = &ixgbe_set_mac_anti_spoofing,
 	.set_vlan_anti_spoofing = &ixgbe_set_vlan_anti_spoofing,
+	.acquire_swfw_sync      = &ixgbe_acquire_swfw_sync_X540,
+	.release_swfw_sync      = &ixgbe_release_swfw_sync_X540,
 };
 
 static struct ixgbe_eeprom_operations eeprom_ops_X540 = {
@@ -702,7 +712,7 @@ static struct ixgbe_phy_operations phy_ops_X540 = {
 	.identify               = &ixgbe_identify_phy_generic,
 	.identify_sfp           = &ixgbe_identify_sfp_module_generic,
 	.init			= NULL,
-	.reset                  = &ixgbe_reset_phy_generic,
+	.reset                  = NULL,
 	.read_reg               = &ixgbe_read_phy_reg_generic,
 	.write_reg              = &ixgbe_write_phy_reg_generic,
 	.setup_link             = &ixgbe_setup_phy_link_generic,
diff --git a/drivers/net/ixgbevf/defines.h b/drivers/net/ixgbevf/defines.h
index de643eb2ada6..78abb6f1a866 100644
--- a/drivers/net/ixgbevf/defines.h
+++ b/drivers/net/ixgbevf/defines.h
@@ -65,6 +65,8 @@ typedef u32 ixgbe_link_speed;
 #define IXGBE_RXCTRL_DMBYPS     0x00000002  /* Descriptor Monitor Bypass */
 #define IXGBE_RXDCTL_ENABLE     0x02000000  /* Enable specific Rx Queue */
 #define IXGBE_RXDCTL_VME        0x40000000  /* VLAN mode enable */
+#define IXGBE_RXDCTL_RLPMLMASK  0x00003FFF  /* Only supported on the X540 */
+#define IXGBE_RXDCTL_RLPML_EN   0x00008000
 
 /* DCA Control */
 #define IXGBE_DCA_TXCTRL_TX_WB_RO_EN (1 << 11) /* Tx Desc writeback RO bit */
diff --git a/drivers/net/ixgbevf/ixgbevf_main.c b/drivers/net/ixgbevf/ixgbevf_main.c
index 464e6c9d3fc2..82768812552d 100644
--- a/drivers/net/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ixgbevf/ixgbevf_main.c
@@ -51,7 +51,7 @@ char ixgbevf_driver_name[] = "ixgbevf";
 static const char ixgbevf_driver_string[] =
 	"Intel(R) 82599 Virtual Function";
 
-#define DRV_VERSION "1.0.19-k0"
+#define DRV_VERSION "1.1.0-k0"
 const char ixgbevf_driver_version[] = DRV_VERSION;
 static char ixgbevf_copyright[] =
 	"Copyright (c) 2009 - 2010 Intel Corporation.";
@@ -107,7 +107,7 @@ static inline void ixgbevf_release_rx_desc(struct ixgbe_hw *hw,
 }
 
 /*
- * ixgbe_set_ivar - set the IVAR registers, mapping interrupt causes to vectors
+ * ixgbevf_set_ivar - set IVAR registers - maps interrupt causes to vectors
  * @adapter: pointer to adapter struct
  * @direction: 0 for Rx, 1 for Tx, -1 for other causes
  * @queue: queue to map the corresponding interrupt to
@@ -178,8 +178,6 @@ static inline bool ixgbevf_check_tx_hang(struct ixgbevf_adapter *adapter,
 	    tx_ring->tx_buffer_info[eop].time_stamp &&
 	    time_after(jiffies, tx_ring->tx_buffer_info[eop].time_stamp + HZ)) {
 		/* detected Tx unit hang */
-		union ixgbe_adv_tx_desc *tx_desc;
-		tx_desc = IXGBE_TX_DESC_ADV(*tx_ring, eop);
 		printk(KERN_ERR "Detected Tx Unit Hang\n"
 		       "  Tx Queue             <%d>\n"
 		       "  TDH, TDT             <%x>, <%x>\n"
@@ -334,7 +332,6 @@ static void ixgbevf_receive_skb(struct ixgbevf_q_vector *q_vector,
 	struct ixgbevf_adapter *adapter = q_vector->adapter;
 	bool is_vlan = (status & IXGBE_RXD_STAT_VP);
 	u16 tag = le16_to_cpu(rx_desc->wb.upper.vlan);
-	int ret;
 
 	if (!(adapter->flags & IXGBE_FLAG_IN_NETPOLL)) {
 		if (adapter->vlgrp && is_vlan)
@@ -345,9 +342,9 @@ static void ixgbevf_receive_skb(struct ixgbevf_q_vector *q_vector,
 			napi_gro_receive(&q_vector->napi, skb);
 	} else {
 		if (adapter->vlgrp && is_vlan)
-			ret = vlan_hwaccel_rx(skb, adapter->vlgrp, tag);
+			vlan_hwaccel_rx(skb, adapter->vlgrp, tag);
 		else
-			ret = netif_rx(skb);
+			netif_rx(skb);
 	}
 }
 
@@ -1017,7 +1014,7 @@ static irqreturn_t ixgbevf_msix_clean_tx(int irq, void *data)
 }
 
 /**
- * ixgbe_msix_clean_rx - single unshared vector rx clean (all queues)
+ * ixgbevf_msix_clean_rx - single unshared vector rx clean (all queues)
  * @irq: unused
  * @data: pointer to our q_vector struct for this interrupt vector
  **/
@@ -1665,6 +1662,11 @@ static int ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 		j = adapter->rx_ring[i].reg_idx;
 		rxdctl = IXGBE_READ_REG(hw, IXGBE_VFRXDCTL(j));
 		rxdctl |= IXGBE_RXDCTL_ENABLE;
+		if (hw->mac.type == ixgbe_mac_X540_vf) {
+			rxdctl &= ~IXGBE_RXDCTL_RLPMLMASK;
+			rxdctl |= ((netdev->mtu + ETH_HLEN + ETH_FCS_LEN) |
+				   IXGBE_RXDCTL_RLPML_EN);
+		}
 		IXGBE_WRITE_REG(hw, IXGBE_VFRXDCTL(j), rxdctl);
 		ixgbevf_rx_desc_queue_enable(adapter, i);
 	}
@@ -1967,7 +1969,7 @@ static void ixgbevf_acquire_msix_vectors(struct ixgbevf_adapter *adapter,
 }
 
 /*
- * ixgbe_set_num_queues: Allocate queues for device, feature dependant
+ * ixgbevf_set_num_queues: Allocate queues for device, feature dependant
  * @adapter: board private structure to initialize
  *
  * This is the top level queue allocation routine.  The order here is very
@@ -2216,7 +2218,7 @@ static int __devinit ixgbevf_sw_init(struct ixgbevf_adapter *adapter)
 
 	hw->vendor_id = pdev->vendor;
 	hw->device_id = pdev->device;
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &hw->revision_id);
+	hw->revision_id = pdev->revision;
 	hw->subsystem_vendor_id = pdev->subsystem_vendor;
 	hw->subsystem_device_id = pdev->subsystem_device;
 
@@ -3217,10 +3219,16 @@ static int ixgbevf_set_mac(struct net_device *netdev, void *p)
 static int ixgbevf_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+	struct ixgbe_hw *hw = &adapter->hw;
 	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN;
+	int max_possible_frame = MAXIMUM_ETHERNET_VLAN_SIZE;
+	u32 msg[2];
+
+	if (adapter->hw.mac.type == ixgbe_mac_X540_vf)
+		max_possible_frame = IXGBE_MAX_JUMBO_FRAME_SIZE;
 
 	/* MTU < 68 is an error and causes problems on some kernels */
-	if ((new_mtu < 68) || (max_frame > MAXIMUM_ETHERNET_VLAN_SIZE))
+	if ((new_mtu < 68) || (max_frame > max_possible_frame))
 		return -EINVAL;
 
 	hw_dbg(&adapter->hw, "changing MTU from %d to %d\n",
@@ -3228,6 +3236,10 @@ static int ixgbevf_change_mtu(struct net_device *netdev, int new_mtu)
 	/* must set new MTU before calling down or up */
 	netdev->mtu = new_mtu;
 
+	msg[0] = IXGBE_VF_SET_LPE;
+	msg[1] = max_frame;
+	hw->mbx.ops.write_posted(hw, msg, 2);
+
 	if (netif_running(netdev))
 		ixgbevf_reinit_locked(adapter);
 
@@ -3272,8 +3284,6 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 
 static void ixgbevf_assign_netdev_ops(struct net_device *dev)
 {
-	struct ixgbevf_adapter *adapter;
-	adapter = netdev_priv(dev);
 	dev->netdev_ops = &ixgbe_netdev_ops;
 	ixgbevf_set_ethtool_ops(dev);
 	dev->watchdog_timeo = 5 * HZ;
@@ -3519,9 +3529,9 @@ static struct pci_driver ixgbevf_driver = {
 };
 
 /**
- * ixgbe_init_module - Driver Registration Routine
+ * ixgbevf_init_module - Driver Registration Routine
  *
- * ixgbe_init_module is the first routine called when the driver is
+ * ixgbevf_init_module is the first routine called when the driver is
  * loaded. All it does is register with the PCI subsystem.
  **/
 static int __init ixgbevf_init_module(void)
@@ -3539,9 +3549,9 @@ static int __init ixgbevf_init_module(void)
 module_init(ixgbevf_init_module);
 
 /**
- * ixgbe_exit_module - Driver Exit Cleanup Routine
+ * ixgbevf_exit_module - Driver Exit Cleanup Routine
  *
- * ixgbe_exit_module is called just before the driver is removed
+ * ixgbevf_exit_module is called just before the driver is removed
  * from memory.
  **/
 static void __exit ixgbevf_exit_module(void)
@@ -3551,7 +3561,7 @@ static void __exit ixgbevf_exit_module(void)
 
 #ifdef DEBUG
 /**
- * ixgbe_get_hw_dev_name - return device name string
+ * ixgbevf_get_hw_dev_name - return device name string
  * used by hardware layer to print debugging information
  **/
 char *ixgbevf_get_hw_dev_name(struct ixgbe_hw *hw)
diff --git a/drivers/net/jme.c b/drivers/net/jme.c
index e97ebef3cf47..f690474f4409 100644
--- a/drivers/net/jme.c
+++ b/drivers/net/jme.c
@@ -161,6 +161,67 @@ jme_setup_wakeup_frame(struct jme_adapter *jme,
 }
 
 static inline void
+jme_mac_rxclk_off(struct jme_adapter *jme)
+{
+	jme->reg_gpreg1 |= GPREG1_RXCLKOFF;
+	jwrite32f(jme, JME_GPREG1, jme->reg_gpreg1);
+}
+
+static inline void
+jme_mac_rxclk_on(struct jme_adapter *jme)
+{
+	jme->reg_gpreg1 &= ~GPREG1_RXCLKOFF;
+	jwrite32f(jme, JME_GPREG1, jme->reg_gpreg1);
+}
+
+static inline void
+jme_mac_txclk_off(struct jme_adapter *jme)
+{
+	jme->reg_ghc &= ~(GHC_TO_CLK_SRC | GHC_TXMAC_CLK_SRC);
+	jwrite32f(jme, JME_GHC, jme->reg_ghc);
+}
+
+static inline void
+jme_mac_txclk_on(struct jme_adapter *jme)
+{
+	u32 speed = jme->reg_ghc & GHC_SPEED;
+	if (speed == GHC_SPEED_1000M)
+		jme->reg_ghc |= GHC_TO_CLK_GPHY | GHC_TXMAC_CLK_GPHY;
+	else
+		jme->reg_ghc |= GHC_TO_CLK_PCIE | GHC_TXMAC_CLK_PCIE;
+	jwrite32f(jme, JME_GHC, jme->reg_ghc);
+}
+
+static inline void
+jme_reset_ghc_speed(struct jme_adapter *jme)
+{
+	jme->reg_ghc &= ~(GHC_SPEED | GHC_DPX);
+	jwrite32f(jme, JME_GHC, jme->reg_ghc);
+}
+
+static inline void
+jme_reset_250A2_workaround(struct jme_adapter *jme)
+{
+	jme->reg_gpreg1 &= ~(GPREG1_HALFMODEPATCH |
+			     GPREG1_RSSPATCH);
+	jwrite32(jme, JME_GPREG1, jme->reg_gpreg1);
+}
+
+static inline void
+jme_assert_ghc_reset(struct jme_adapter *jme)
+{
+	jme->reg_ghc |= GHC_SWRST;
+	jwrite32f(jme, JME_GHC, jme->reg_ghc);
+}
+
+static inline void
+jme_clear_ghc_reset(struct jme_adapter *jme)
+{
+	jme->reg_ghc &= ~GHC_SWRST;
+	jwrite32f(jme, JME_GHC, jme->reg_ghc);
+}
+
+static inline void
 jme_reset_mac_processor(struct jme_adapter *jme)
 {
 	static const u32 mask[WAKEUP_FRAME_MASK_DWNR] = {0, 0, 0, 0};
@@ -168,9 +229,24 @@ jme_reset_mac_processor(struct jme_adapter *jme)
 	u32 gpreg0;
 	int i;
 
-	jwrite32(jme, JME_GHC, jme->reg_ghc | GHC_SWRST);
-	udelay(2);
-	jwrite32(jme, JME_GHC, jme->reg_ghc);
+	jme_reset_ghc_speed(jme);
+	jme_reset_250A2_workaround(jme);
+
+	jme_mac_rxclk_on(jme);
+	jme_mac_txclk_on(jme);
+	udelay(1);
+	jme_assert_ghc_reset(jme);
+	udelay(1);
+	jme_mac_rxclk_off(jme);
+	jme_mac_txclk_off(jme);
+	udelay(1);
+	jme_clear_ghc_reset(jme);
+	udelay(1);
+	jme_mac_rxclk_on(jme);
+	jme_mac_txclk_on(jme);
+	udelay(1);
+	jme_mac_rxclk_off(jme);
+	jme_mac_txclk_off(jme);
 
 	jwrite32(jme, JME_RXDBA_LO, 0x00000000);
 	jwrite32(jme, JME_RXDBA_HI, 0x00000000);
@@ -190,14 +266,6 @@ jme_reset_mac_processor(struct jme_adapter *jme)
 	else
 		gpreg0 = GPREG0_DEFAULT;
 	jwrite32(jme, JME_GPREG0, gpreg0);
-	jwrite32(jme, JME_GPREG1, GPREG1_DEFAULT);
-}
-
-static inline void
-jme_reset_ghc_speed(struct jme_adapter *jme)
-{
-	jme->reg_ghc &= ~(GHC_SPEED_1000M | GHC_DPX);
-	jwrite32(jme, JME_GHC, jme->reg_ghc);
 }
 
 static inline void
@@ -336,13 +404,13 @@ jme_linkstat_from_phy(struct jme_adapter *jme)
 }
 
 static inline void
-jme_set_phyfifoa(struct jme_adapter *jme)
+jme_set_phyfifo_5level(struct jme_adapter *jme)
 {
 	jme_mdio_write(jme->dev, jme->mii_if.phy_id, 27, 0x0004);
 }
 
 static inline void
-jme_set_phyfifob(struct jme_adapter *jme)
+jme_set_phyfifo_8level(struct jme_adapter *jme)
 {
 	jme_mdio_write(jme->dev, jme->mii_if.phy_id, 27, 0x0000);
 }
@@ -351,7 +419,7 @@ static int
 jme_check_link(struct net_device *netdev, int testonly)
 {
 	struct jme_adapter *jme = netdev_priv(netdev);
-	u32 phylink, ghc, cnt = JME_SPDRSV_TIMEOUT, bmcr, gpreg1;
+	u32 phylink, cnt = JME_SPDRSV_TIMEOUT, bmcr;
 	char linkmsg[64];
 	int rc = 0;
 
@@ -414,23 +482,21 @@ jme_check_link(struct net_device *netdev, int testonly)
 
 		jme->phylink = phylink;
 
-		ghc = jme->reg_ghc & ~(GHC_SPEED | GHC_DPX |
-				GHC_TO_CLK_PCIE | GHC_TXMAC_CLK_PCIE |
-				GHC_TO_CLK_GPHY | GHC_TXMAC_CLK_GPHY);
+		/*
+		 * The speed/duplex setting of jme->reg_ghc already cleared
+		 * by jme_reset_mac_processor()
+		 */
 		switch (phylink & PHY_LINK_SPEED_MASK) {
 		case PHY_LINK_SPEED_10M:
-			ghc |= GHC_SPEED_10M |
-				GHC_TO_CLK_PCIE | GHC_TXMAC_CLK_PCIE;
+			jme->reg_ghc |= GHC_SPEED_10M;
 			strcat(linkmsg, "10 Mbps, ");
 			break;
 		case PHY_LINK_SPEED_100M:
-			ghc |= GHC_SPEED_100M |
-				GHC_TO_CLK_PCIE | GHC_TXMAC_CLK_PCIE;
+			jme->reg_ghc |= GHC_SPEED_100M;
 			strcat(linkmsg, "100 Mbps, ");
 			break;
 		case PHY_LINK_SPEED_1000M:
-			ghc |= GHC_SPEED_1000M |
-				GHC_TO_CLK_GPHY | GHC_TXMAC_CLK_GPHY;
+			jme->reg_ghc |= GHC_SPEED_1000M;
 			strcat(linkmsg, "1000 Mbps, ");
 			break;
 		default:
@@ -439,42 +505,40 @@ jme_check_link(struct net_device *netdev, int testonly)
 
 		if (phylink & PHY_LINK_DUPLEX) {
 			jwrite32(jme, JME_TXMCS, TXMCS_DEFAULT);
-			ghc |= GHC_DPX;
+			jwrite32(jme, JME_TXTRHD, TXTRHD_FULLDUPLEX);
+			jme->reg_ghc |= GHC_DPX;
 		} else {
 			jwrite32(jme, JME_TXMCS, TXMCS_DEFAULT |
 						TXMCS_BACKOFF |
 						TXMCS_CARRIERSENSE |
 						TXMCS_COLLISION);
-			jwrite32(jme, JME_TXTRHD, TXTRHD_TXPEN |
-				((0x2000 << TXTRHD_TXP_SHIFT) & TXTRHD_TXP) |
-				TXTRHD_TXREN |
-				((8 << TXTRHD_TXRL_SHIFT) & TXTRHD_TXRL));
+			jwrite32(jme, JME_TXTRHD, TXTRHD_HALFDUPLEX);
 		}
 
-		gpreg1 = GPREG1_DEFAULT;
+		jwrite32(jme, JME_GHC, jme->reg_ghc);
+
 		if (is_buggy250(jme->pdev->device, jme->chiprev)) {
+			jme->reg_gpreg1 &= ~(GPREG1_HALFMODEPATCH |
+					     GPREG1_RSSPATCH);
 			if (!(phylink & PHY_LINK_DUPLEX))
-				gpreg1 |= GPREG1_HALFMODEPATCH;
+				jme->reg_gpreg1 |= GPREG1_HALFMODEPATCH;
 			switch (phylink & PHY_LINK_SPEED_MASK) {
 			case PHY_LINK_SPEED_10M:
-				jme_set_phyfifoa(jme);
-				gpreg1 |= GPREG1_RSSPATCH;
+				jme_set_phyfifo_8level(jme);
+				jme->reg_gpreg1 |= GPREG1_RSSPATCH;
 				break;
 			case PHY_LINK_SPEED_100M:
-				jme_set_phyfifob(jme);
-				gpreg1 |= GPREG1_RSSPATCH;
+				jme_set_phyfifo_5level(jme);
+				jme->reg_gpreg1 |= GPREG1_RSSPATCH;
 				break;
 			case PHY_LINK_SPEED_1000M:
-				jme_set_phyfifoa(jme);
+				jme_set_phyfifo_8level(jme);
 				break;
 			default:
 				break;
 			}
 		}
-
-		jwrite32(jme, JME_GPREG1, gpreg1);
-		jwrite32(jme, JME_GHC, ghc);
-		jme->reg_ghc = ghc;
+		jwrite32(jme, JME_GPREG1, jme->reg_gpreg1);
 
 		strcat(linkmsg, (phylink & PHY_LINK_DUPLEX) ?
 					"Full-Duplex, " :
@@ -613,10 +677,14 @@ jme_enable_tx_engine(struct jme_adapter *jme)
 	 * Enable TX Engine
 	 */
 	wmb();
-	jwrite32(jme, JME_TXCS, jme->reg_txcs |
+	jwrite32f(jme, JME_TXCS, jme->reg_txcs |
 				TXCS_SELECT_QUEUE0 |
 				TXCS_ENABLE);
 
+	/*
+	 * Start clock for TX MAC Processor
+	 */
+	jme_mac_txclk_on(jme);
 }
 
 static inline void
@@ -651,6 +719,11 @@ jme_disable_tx_engine(struct jme_adapter *jme)
 
 	if (!i)
 		pr_err("Disable TX engine timeout\n");
+
+	/*
+	 * Stop clock for TX MAC Processor
+	 */
+	jme_mac_txclk_off(jme);
 }
 
 static void
@@ -825,16 +898,22 @@ jme_enable_rx_engine(struct jme_adapter *jme)
 	/*
 	 * Setup Unicast Filter
 	 */
+	jme_set_unicastaddr(jme->dev);
 	jme_set_multi(jme->dev);
 
 	/*
 	 * Enable RX Engine
 	 */
 	wmb();
-	jwrite32(jme, JME_RXCS, jme->reg_rxcs |
+	jwrite32f(jme, JME_RXCS, jme->reg_rxcs |
 				RXCS_QUEUESEL_Q0 |
 				RXCS_ENABLE |
 				RXCS_QST);
+
+	/*
+	 * Start clock for RX MAC Processor
+	 */
+	jme_mac_rxclk_on(jme);
 }
 
 static inline void
@@ -871,10 +950,40 @@ jme_disable_rx_engine(struct jme_adapter *jme)
 	if (!i)
 		pr_err("Disable RX engine timeout\n");
 
+	/*
+	 * Stop clock for RX MAC Processor
+	 */
+	jme_mac_rxclk_off(jme);
+}
+
+static u16
+jme_udpsum(struct sk_buff *skb)
+{
+	u16 csum = 0xFFFFu;
+
+	if (skb->len < (ETH_HLEN + sizeof(struct iphdr)))
+		return csum;
+	if (skb->protocol != htons(ETH_P_IP))
+		return csum;
+	skb_set_network_header(skb, ETH_HLEN);
+	if ((ip_hdr(skb)->protocol != IPPROTO_UDP) ||
+	    (skb->len < (ETH_HLEN +
+			(ip_hdr(skb)->ihl << 2) +
+			sizeof(struct udphdr)))) {
+		skb_reset_network_header(skb);
+		return csum;
+	}
+	skb_set_transport_header(skb,
+			ETH_HLEN + (ip_hdr(skb)->ihl << 2));
+	csum = udp_hdr(skb)->check;
+	skb_reset_transport_header(skb);
+	skb_reset_network_header(skb);
+
+	return csum;
 }
 
 static int
-jme_rxsum_ok(struct jme_adapter *jme, u16 flags)
+jme_rxsum_ok(struct jme_adapter *jme, u16 flags, struct sk_buff *skb)
 {
 	if (!(flags & (RXWBFLAG_TCPON | RXWBFLAG_UDPON | RXWBFLAG_IPV4)))
 		return false;
@@ -887,7 +996,7 @@ jme_rxsum_ok(struct jme_adapter *jme, u16 flags)
 	}
 
 	if (unlikely((flags & (RXWBFLAG_MF | RXWBFLAG_UDPON | RXWBFLAG_UDPCS))
-			== RXWBFLAG_UDPON)) {
+			== RXWBFLAG_UDPON) && jme_udpsum(skb)) {
 		if (flags & RXWBFLAG_IPV4)
 			netif_err(jme, rx_err, jme->dev, "UDP Checksum error\n");
 		return false;
@@ -935,7 +1044,7 @@ jme_alloc_and_feed_skb(struct jme_adapter *jme, int idx)
 		skb_put(skb, framesize);
 		skb->protocol = eth_type_trans(skb, jme->dev);
 
-		if (jme_rxsum_ok(jme, le16_to_cpu(rxdesc->descwb.flags)))
+		if (jme_rxsum_ok(jme, le16_to_cpu(rxdesc->descwb.flags), skb))
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
 		else
 			skb_checksum_none_assert(skb);
@@ -1207,7 +1316,6 @@ jme_link_change_tasklet(unsigned long arg)
 	tasklet_disable(&jme->rxempty_task);
 
 	if (netif_carrier_ok(netdev)) {
-		jme_reset_ghc_speed(jme);
 		jme_disable_rx_engine(jme);
 		jme_disable_tx_engine(jme);
 		jme_reset_mac_processor(jme);
@@ -1577,6 +1685,38 @@ jme_free_irq(struct jme_adapter *jme)
 }
 
 static inline void
+jme_new_phy_on(struct jme_adapter *jme)
+{
+	u32 reg;
+
+	reg = jread32(jme, JME_PHY_PWR);
+	reg &= ~(PHY_PWR_DWN1SEL | PHY_PWR_DWN1SW |
+		 PHY_PWR_DWN2 | PHY_PWR_CLKSEL);
+	jwrite32(jme, JME_PHY_PWR, reg);
+
+	pci_read_config_dword(jme->pdev, PCI_PRIV_PE1, &reg);
+	reg &= ~PE1_GPREG0_PBG;
+	reg |= PE1_GPREG0_ENBG;
+	pci_write_config_dword(jme->pdev, PCI_PRIV_PE1, reg);
+}
+
+static inline void
+jme_new_phy_off(struct jme_adapter *jme)
+{
+	u32 reg;
+
+	reg = jread32(jme, JME_PHY_PWR);
+	reg |= PHY_PWR_DWN1SEL | PHY_PWR_DWN1SW |
+	       PHY_PWR_DWN2 | PHY_PWR_CLKSEL;
+	jwrite32(jme, JME_PHY_PWR, reg);
+
+	pci_read_config_dword(jme->pdev, PCI_PRIV_PE1, &reg);
+	reg &= ~PE1_GPREG0_PBG;
+	reg |= PE1_GPREG0_PDD3COLD;
+	pci_write_config_dword(jme->pdev, PCI_PRIV_PE1, reg);
+}
+
+static inline void
 jme_phy_on(struct jme_adapter *jme)
 {
 	u32 bmcr;
@@ -1584,6 +1724,22 @@ jme_phy_on(struct jme_adapter *jme)
 	bmcr = jme_mdio_read(jme->dev, jme->mii_if.phy_id, MII_BMCR);
 	bmcr &= ~BMCR_PDOWN;
 	jme_mdio_write(jme->dev, jme->mii_if.phy_id, MII_BMCR, bmcr);
+
+	if (new_phy_power_ctrl(jme->chip_main_rev))
+		jme_new_phy_on(jme);
+}
+
+static inline void
+jme_phy_off(struct jme_adapter *jme)
+{
+	u32 bmcr;
+
+	bmcr = jme_mdio_read(jme->dev, jme->mii_if.phy_id, MII_BMCR);
+	bmcr |= BMCR_PDOWN;
+	jme_mdio_write(jme->dev, jme->mii_if.phy_id, MII_BMCR, bmcr);
+
+	if (new_phy_power_ctrl(jme->chip_main_rev))
+		jme_new_phy_off(jme);
 }
 
 static int
@@ -1606,12 +1762,11 @@ jme_open(struct net_device *netdev)
 
 	jme_start_irq(jme);
 
-	if (test_bit(JME_FLAG_SSET, &jme->flags)) {
-		jme_phy_on(jme);
+	jme_phy_on(jme);
+	if (test_bit(JME_FLAG_SSET, &jme->flags))
 		jme_set_settings(netdev, &jme->old_ecmd);
-	} else {
+	else
 		jme_reset_phy_processor(jme);
-	}
 
 	jme_reset_link(jme);
 
@@ -1657,12 +1812,6 @@ jme_wait_link(struct jme_adapter *jme)
 	}
 }
 
-static inline void
-jme_phy_off(struct jme_adapter *jme)
-{
-	jme_mdio_write(jme->dev, jme->mii_if.phy_id, MII_BMCR, BMCR_PDOWN);
-}
-
 static void
 jme_powersave_phy(struct jme_adapter *jme)
 {
@@ -1696,7 +1845,6 @@ jme_close(struct net_device *netdev)
 	tasklet_disable(&jme->rxclean_task);
 	tasklet_disable(&jme->rxempty_task);
 
-	jme_reset_ghc_speed(jme);
 	jme_disable_rx_engine(jme);
 	jme_disable_tx_engine(jme);
 	jme_reset_mac_processor(jme);
@@ -1993,27 +2141,34 @@ jme_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 	return NETDEV_TX_OK;
 }
 
+static void
+jme_set_unicastaddr(struct net_device *netdev)
+{
+	struct jme_adapter *jme = netdev_priv(netdev);
+	u32 val;
+
+	val = (netdev->dev_addr[3] & 0xff) << 24 |
+	      (netdev->dev_addr[2] & 0xff) << 16 |
+	      (netdev->dev_addr[1] & 0xff) <<  8 |
+	      (netdev->dev_addr[0] & 0xff);
+	jwrite32(jme, JME_RXUMA_LO, val);
+	val = (netdev->dev_addr[5] & 0xff) << 8 |
+	      (netdev->dev_addr[4] & 0xff);
+	jwrite32(jme, JME_RXUMA_HI, val);
+}
+
 static int
 jme_set_macaddr(struct net_device *netdev, void *p)
 {
 	struct jme_adapter *jme = netdev_priv(netdev);
 	struct sockaddr *addr = p;
-	u32 val;
 
 	if (netif_running(netdev))
 		return -EBUSY;
 
 	spin_lock_bh(&jme->macaddr_lock);
 	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
-
-	val = (addr->sa_data[3] & 0xff) << 24 |
-	      (addr->sa_data[2] & 0xff) << 16 |
-	      (addr->sa_data[1] & 0xff) <<  8 |
-	      (addr->sa_data[0] & 0xff);
-	jwrite32(jme, JME_RXUMA_LO, val);
-	val = (addr->sa_data[5] & 0xff) << 8 |
-	      (addr->sa_data[4] & 0xff);
-	jwrite32(jme, JME_RXUMA_HI, val);
+	jme_set_unicastaddr(netdev);
 	spin_unlock_bh(&jme->macaddr_lock);
 
 	return 0;
@@ -2731,6 +2886,8 @@ jme_check_hw_ver(struct jme_adapter *jme)
 
 	jme->fpgaver = (chipmode & CM_FPGAVER_MASK) >> CM_FPGAVER_SHIFT;
 	jme->chiprev = (chipmode & CM_CHIPREV_MASK) >> CM_CHIPREV_SHIFT;
+	jme->chip_main_rev = jme->chiprev & 0xF;
+	jme->chip_sub_rev = (jme->chiprev >> 4) & 0xF;
 }
 
 static const struct net_device_ops jme_netdev_ops = {
@@ -2880,6 +3037,7 @@ jme_init_one(struct pci_dev *pdev,
 	jme->reg_rxmcs = RXMCS_DEFAULT;
 	jme->reg_txpfc = 0;
 	jme->reg_pmcs = PMCS_MFEN;
+	jme->reg_gpreg1 = GPREG1_DEFAULT;
 	set_bit(JME_FLAG_TXCSUM, &jme->flags);
 	set_bit(JME_FLAG_TSO, &jme->flags);
 
@@ -2936,8 +3094,8 @@ jme_init_one(struct pci_dev *pdev,
 	jme->mii_if.mdio_write = jme_mdio_write;
 
 	jme_clear_pm(jme);
-	jme_set_phyfifoa(jme);
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &jme->rev);
+	jme_set_phyfifo_5level(jme);
+	jme->pcirev = pdev->revision;
 	if (!jme->fpgaver)
 		jme_phy_init(jme);
 	jme_phy_off(jme);
@@ -2964,14 +3122,14 @@ jme_init_one(struct pci_dev *pdev,
 		goto err_out_unmap;
 	}
 
-	netif_info(jme, probe, jme->dev, "%s%s ver:%x rev:%x macaddr:%pM\n",
+	netif_info(jme, probe, jme->dev, "%s%s chiprev:%x pcirev:%x macaddr:%pM\n",
 		   (jme->pdev->device == PCI_DEVICE_ID_JMICRON_JMC250) ?
 		   "JMC250 Gigabit Ethernet" :
 		   (jme->pdev->device == PCI_DEVICE_ID_JMICRON_JMC260) ?
 		   "JMC260 Fast Ethernet" : "Unknown",
 		   (jme->fpgaver != 0) ? " (FPGA)" : "",
 		   (jme->fpgaver != 0) ? jme->fpgaver : jme->chiprev,
-		   jme->rev, netdev->dev_addr);
+		   jme->pcirev, netdev->dev_addr);
 
 	return 0;
 
@@ -3035,7 +3193,6 @@ jme_suspend(struct pci_dev *pdev, pm_message_t state)
 			jme_polling_mode(jme);
 
 		jme_stop_pcc_timer(jme);
-		jme_reset_ghc_speed(jme);
 		jme_disable_rx_engine(jme);
 		jme_disable_tx_engine(jme);
 		jme_reset_mac_processor(jme);
@@ -3066,12 +3223,11 @@ jme_resume(struct pci_dev *pdev)
 	jme_clear_pm(jme);
 	pci_restore_state(pdev);
 
-	if (test_bit(JME_FLAG_SSET, &jme->flags)) {
-		jme_phy_on(jme);
+	jme_phy_on(jme);
+	if (test_bit(JME_FLAG_SSET, &jme->flags))
 		jme_set_settings(netdev, &jme->old_ecmd);
-	} else {
+	else
 		jme_reset_phy_processor(jme);
-	}
 
 	jme_start_irq(jme);
 	netif_device_attach(netdev);
diff --git a/drivers/net/jme.h b/drivers/net/jme.h
index eac09264bf2a..8bf30451e821 100644
--- a/drivers/net/jme.h
+++ b/drivers/net/jme.h
@@ -26,7 +26,7 @@
 #define __JME_H_INCLUDED__
 
 #define DRV_NAME	"jme"
-#define DRV_VERSION	"1.0.7"
+#define DRV_VERSION	"1.0.8"
 #define PFX		DRV_NAME ": "
 
 #define PCI_DEVICE_ID_JMICRON_JMC250	0x0250
@@ -103,6 +103,37 @@ enum jme_spi_op_bits {
 #define HALF_US 500	/* 500 ns */
 #define JMESPIIOCTL	SIOCDEVPRIVATE
 
+#define PCI_PRIV_PE1		0xE4
+
+enum pci_priv_pe1_bit_masks {
+	PE1_ASPMSUPRT	= 0x00000003, /*
+				       * RW:
+				       * Aspm_support[1:0]
+				       * (R/W Port of 5C[11:10])
+				       */
+	PE1_MULTIFUN	= 0x00000004, /* RW: Multi_fun_bit */
+	PE1_RDYDMA	= 0x00000008, /* RO: ~link.rdy_for_dma */
+	PE1_ASPMOPTL	= 0x00000030, /* RW: link.rx10s_option[1:0] */
+	PE1_ASPMOPTH	= 0x000000C0, /* RW: 10_req=[3]?HW:[2] */
+	PE1_GPREG0	= 0x0000FF00, /*
+				       * SRW:
+				       * Cfg_gp_reg0
+				       * [7:6] phy_giga BG control
+				       * [5] CREQ_N as CREQ_N1 (CPPE# as CREQ#)
+				       * [4:0] Reserved
+				       */
+	PE1_GPREG0_PBG	= 0x0000C000, /* phy_giga BG control */
+	PE1_GPREG1	= 0x00FF0000, /* RW: Cfg_gp_reg1 */
+	PE1_REVID	= 0xFF000000, /* RO: Rev ID */
+};
+
+enum pci_priv_pe1_values {
+	PE1_GPREG0_ENBG		= 0x00000000, /* en BG */
+	PE1_GPREG0_PDD3COLD	= 0x00004000, /* giga_PD + d3cold */
+	PE1_GPREG0_PDPCIESD	= 0x00008000, /* giga_PD + pcie_shutdown */
+	PE1_GPREG0_PDPCIEIDDQ	= 0x0000C000, /* giga_PD + pcie_iddq */
+};
+
 /*
  * Dynamic(adaptive)/Static PCC values
  */
@@ -403,6 +434,7 @@ struct jme_adapter {
 	u32			reg_rxmcs;
 	u32			reg_ghc;
 	u32			reg_pmcs;
+	u32			reg_gpreg1;
 	u32			phylink;
 	u32			tx_ring_size;
 	u32			tx_ring_mask;
@@ -411,8 +443,10 @@ struct jme_adapter {
 	u32			rx_ring_mask;
 	u8			mrrs;
 	unsigned int		fpgaver;
-	unsigned int		chiprev;
-	u8			rev;
+	u8			chiprev;
+	u8			chip_main_rev;
+	u8			chip_sub_rev;
+	u8			pcirev;
 	u32			msg_enable;
 	struct ethtool_cmd	old_ecmd;
 	unsigned int		old_mtu;
@@ -497,6 +531,7 @@ enum jme_iomap_regs {
 	JME_PMCS	= JME_MAC | 0x60, /* Power Management Control/Stat */
 
 
+	JME_PHY_PWR	= JME_PHY | 0x24, /* New PHY Power Ctrl Register */
 	JME_PHY_CS	= JME_PHY | 0x28, /* PHY Ctrl and Status Register */
 	JME_PHY_LINK	= JME_PHY | 0x30, /* PHY Link Status Register */
 	JME_SMBCSR	= JME_PHY | 0x40, /* SMB Control and Status */
@@ -624,6 +659,14 @@ enum jme_txtrhd_shifts {
 	TXTRHD_TXRL_SHIFT	= 0,
 };
 
+enum jme_txtrhd_values {
+	TXTRHD_FULLDUPLEX	= 0x00000000,
+	TXTRHD_HALFDUPLEX	= TXTRHD_TXPEN |
+				  ((0x2000 << TXTRHD_TXP_SHIFT) & TXTRHD_TXP) |
+				  TXTRHD_TXREN |
+				  ((8 << TXTRHD_TXRL_SHIFT) & TXTRHD_TXRL),
+};
+
 /*
  * RX Control/Status Bits
  */
@@ -779,6 +822,8 @@ static inline u32 smi_phy_addr(int x)
  */
 enum jme_ghc_bit_mask {
 	GHC_SWRST		= 0x40000000,
+	GHC_TO_CLK_SRC		= 0x00C00000,
+	GHC_TXMAC_CLK_SRC	= 0x00300000,
 	GHC_DPX			= 0x00000040,
 	GHC_SPEED		= 0x00000030,
 	GHC_LINK_POLL		= 0x00000001,
@@ -833,6 +878,21 @@ enum jme_pmcs_bit_masks {
 };
 
 /*
+ * New PHY Power Control Register
+ */
+enum jme_phy_pwr_bit_masks {
+	PHY_PWR_DWN1SEL	= 0x01000000, /* Phy_giga.p_PWR_DOWN1_SEL */
+	PHY_PWR_DWN1SW	= 0x02000000, /* Phy_giga.p_PWR_DOWN1_SW */
+	PHY_PWR_DWN2	= 0x04000000, /* Phy_giga.p_PWR_DOWN2 */
+	PHY_PWR_CLKSEL	= 0x08000000, /*
+				       * XTL_OUT Clock select
+				       * (an internal free-running clock)
+				       * 0: xtl_out = phy_giga.A_XTL25_O
+				       * 1: xtl_out = phy_giga.PD_OSC
+				       */
+};
+
+/*
  * Giga PHY Status Registers
  */
 enum jme_phy_link_bit_mask {
@@ -942,18 +1002,17 @@ enum jme_gpreg0_vals {
 
 /*
  * General Purpose REG-1
- * Note: All theses bits defined here are for
- *       Chip mode revision 0x11 only
  */
-enum jme_gpreg1_masks {
+enum jme_gpreg1_bit_masks {
+	GPREG1_RXCLKOFF		= 0x04000000,
+	GPREG1_PCREQN		= 0x00020000,
+	GPREG1_HALFMODEPATCH	= 0x00000040, /* For Chip revision 0x11 only */
+	GPREG1_RSSPATCH		= 0x00000020, /* For Chip revision 0x11 only */
 	GPREG1_INTRDELAYUNIT	= 0x00000018,
 	GPREG1_INTRDELAYENABLE	= 0x00000007,
 };
 
 enum jme_gpreg1_vals {
-	GPREG1_RSSPATCH		= 0x00000040,
-	GPREG1_HALFMODEPATCH	= 0x00000020,
-
 	GPREG1_INTDLYUNIT_16NS	= 0x00000000,
 	GPREG1_INTDLYUNIT_256NS	= 0x00000008,
 	GPREG1_INTDLYUNIT_1US	= 0x00000010,
@@ -967,7 +1026,7 @@ enum jme_gpreg1_vals {
 	GPREG1_INTDLYEN_6U	= 0x00000006,
 	GPREG1_INTDLYEN_7U	= 0x00000007,
 
-	GPREG1_DEFAULT		= 0x00000000,
+	GPREG1_DEFAULT		= GPREG1_PCREQN,
 };
 
 /*
@@ -1184,16 +1243,22 @@ enum jme_phy_reg17_vals {
 /*
  * Workaround
  */
-static inline int is_buggy250(unsigned short device, unsigned int chiprev)
+static inline int is_buggy250(unsigned short device, u8 chiprev)
 {
 	return device == PCI_DEVICE_ID_JMICRON_JMC250 && chiprev == 0x11;
 }
 
+static inline int new_phy_power_ctrl(u8 chip_main_rev)
+{
+	return chip_main_rev >= 5;
+}
+
 /*
  * Function prototypes
  */
 static int jme_set_settings(struct net_device *netdev,
 				struct ethtool_cmd *ecmd);
+static void jme_set_unicastaddr(struct net_device *netdev);
 static void jme_set_multi(struct net_device *netdev);
 
 #endif
diff --git a/drivers/net/ks8842.c b/drivers/net/ks8842.c
index 928b2b83cef5..efd44afeae83 100644
--- a/drivers/net/ks8842.c
+++ b/drivers/net/ks8842.c
@@ -26,6 +26,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/ethtool.h>
@@ -1145,7 +1146,7 @@ static int __devinit ks8842_probe(struct platform_device *pdev)
 	struct resource *iomem;
 	struct net_device *netdev;
 	struct ks8842_adapter *adapter;
-	struct ks8842_platform_data *pdata = pdev->dev.platform_data;
+	struct ks8842_platform_data *pdata = mfd_get_data(pdev);
 	u16 id;
 	unsigned i;
 
diff --git a/drivers/net/ll_temac_main.c b/drivers/net/ll_temac_main.c
index f35554d11441..b7948ccfcf7d 100644
--- a/drivers/net/ll_temac_main.c
+++ b/drivers/net/ll_temac_main.c
@@ -952,8 +952,7 @@ static const struct attribute_group temac_attr_group = {
 	.attrs = temac_device_attrs,
 };
 
-static int __devinit
-temac_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit temac_of_probe(struct platform_device *op)
 {
 	struct device_node *np;
 	struct temac_local *lp;
@@ -1123,7 +1122,7 @@ static struct of_device_id temac_of_match[] __devinitdata = {
 };
 MODULE_DEVICE_TABLE(of, temac_of_match);
 
-static struct of_platform_driver temac_of_driver = {
+static struct platform_driver temac_of_driver = {
 	.probe = temac_of_probe,
 	.remove = __devexit_p(temac_of_remove),
 	.driver = {
@@ -1135,13 +1134,13 @@ static struct of_platform_driver temac_of_driver = {
 
 static int __init temac_init(void)
 {
-	return of_register_platform_driver(&temac_of_driver);
+	return platform_driver_register(&temac_of_driver);
 }
 module_init(temac_init);
 
 static void __exit temac_exit(void)
 {
-	of_unregister_platform_driver(&temac_of_driver);
+	platform_driver_unregister(&temac_of_driver);
 }
 module_exit(temac_exit);
 
diff --git a/drivers/net/loopback.c b/drivers/net/loopback.c
index 2d9663a1c54d..ea0dc451da9c 100644
--- a/drivers/net/loopback.c
+++ b/drivers/net/loopback.c
@@ -129,10 +129,6 @@ static u32 always_on(struct net_device *dev)
 
 static const struct ethtool_ops loopback_ethtool_ops = {
 	.get_link		= always_on,
-	.set_tso		= ethtool_op_set_tso,
-	.get_tx_csum		= always_on,
-	.get_sg			= always_on,
-	.get_rx_csum		= always_on,
 };
 
 static int loopback_dev_init(struct net_device *dev)
@@ -169,9 +165,12 @@ static void loopback_setup(struct net_device *dev)
 	dev->type		= ARPHRD_LOOPBACK;	/* 0x0001*/
 	dev->flags		= IFF_LOOPBACK;
 	dev->priv_flags	       &= ~IFF_XMIT_DST_RELEASE;
+	dev->hw_features	= NETIF_F_ALL_TSO | NETIF_F_UFO;
 	dev->features 		= NETIF_F_SG | NETIF_F_FRAGLIST
-		| NETIF_F_TSO
+		| NETIF_F_ALL_TSO
+		| NETIF_F_UFO
 		| NETIF_F_NO_CSUM
+		| NETIF_F_RXCSUM
 		| NETIF_F_HIGHDMA
 		| NETIF_F_LLTX
 		| NETIF_F_NETNS_LOCAL;
diff --git a/drivers/net/macb.c b/drivers/net/macb.c
index f69e73e2191e..79ccb54ab00c 100644
--- a/drivers/net/macb.c
+++ b/drivers/net/macb.c
@@ -260,7 +260,7 @@ static int macb_mii_init(struct macb *bp)
 	for (i = 0; i < PHY_MAX_ADDR; i++)
 		bp->mii_bus->irq[i] = PHY_POLL;
 
-	platform_set_drvdata(bp->dev, bp->mii_bus);
+	dev_set_drvdata(&bp->dev->dev, bp->mii_bus);
 
 	if (mdiobus_register(bp->mii_bus))
 		goto err_out_free_mdio_irq;
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 5933621ac3ff..6696e56e6320 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -39,7 +39,7 @@ struct macvtap_queue {
 	struct socket sock;
 	struct socket_wq wq;
 	int vnet_hdr_sz;
-	struct macvlan_dev *vlan;
+	struct macvlan_dev __rcu *vlan;
 	struct file *file;
 	unsigned int flags;
 };
@@ -141,7 +141,8 @@ static void macvtap_put_queue(struct macvtap_queue *q)
 	struct macvlan_dev *vlan;
 
 	spin_lock(&macvtap_lock);
-	vlan = rcu_dereference(q->vlan);
+	vlan = rcu_dereference_protected(q->vlan,
+					 lockdep_is_held(&macvtap_lock));
 	if (vlan) {
 		int index = get_slot(vlan, q);
 
@@ -219,7 +220,8 @@ static void macvtap_del_queues(struct net_device *dev)
 	/* macvtap_put_queue can free some slots, so go through all slots */
 	spin_lock(&macvtap_lock);
 	for (i = 0; i < MAX_MACVTAP_QUEUES && vlan->numvtaps; i++) {
-		q = rcu_dereference(vlan->taps[i]);
+		q = rcu_dereference_protected(vlan->taps[i],
+					      lockdep_is_held(&macvtap_lock));
 		if (q) {
 			qlist[j++] = q;
 			rcu_assign_pointer(vlan->taps[i], NULL);
@@ -528,8 +530,9 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q,
 		vnet_hdr_len = q->vnet_hdr_sz;
 
 		err = -EINVAL;
-		if ((len -= vnet_hdr_len) < 0)
+		if (len < vnet_hdr_len)
 			goto err;
+		len -= vnet_hdr_len;
 
 		err = memcpy_fromiovecend((void *)&vnet_hdr, iv, 0,
 					   sizeof(vnet_hdr));
@@ -569,7 +572,7 @@ static ssize_t macvtap_get_user(struct macvtap_queue *q,
 	}
 
 	rcu_read_lock_bh();
-	vlan = rcu_dereference(q->vlan);
+	vlan = rcu_dereference_bh(q->vlan);
 	if (vlan)
 		macvlan_start_xmit(skb, vlan->dev);
 	else
@@ -583,7 +586,7 @@ err_kfree:
 
 err:
 	rcu_read_lock_bh();
-	vlan = rcu_dereference(q->vlan);
+	vlan = rcu_dereference_bh(q->vlan);
 	if (vlan)
 		vlan->dev->stats.tx_dropped++;
 	rcu_read_unlock_bh();
@@ -631,7 +634,7 @@ static ssize_t macvtap_put_user(struct macvtap_queue *q,
 	ret = skb_copy_datagram_const_iovec(skb, 0, iv, vnet_hdr_len, len);
 
 	rcu_read_lock_bh();
-	vlan = rcu_dereference(q->vlan);
+	vlan = rcu_dereference_bh(q->vlan);
 	if (vlan)
 		macvlan_count_rx(vlan, len, ret == 0, 0);
 	rcu_read_unlock_bh();
@@ -727,7 +730,7 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 
 	case TUNGETIFF:
 		rcu_read_lock_bh();
-		vlan = rcu_dereference(q->vlan);
+		vlan = rcu_dereference_bh(q->vlan);
 		if (vlan)
 			dev_hold(vlan->dev);
 		rcu_read_unlock_bh();
@@ -736,7 +739,7 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 			return -ENOLINK;
 
 		ret = 0;
-		if (copy_to_user(&ifr->ifr_name, q->vlan->dev->name, IFNAMSIZ) ||
+		if (copy_to_user(&ifr->ifr_name, vlan->dev->name, IFNAMSIZ) ||
 		    put_user(q->flags, &ifr->ifr_flags))
 			ret = -EFAULT;
 		dev_put(vlan->dev);
diff --git a/drivers/net/mii.c b/drivers/net/mii.c
index 210b2b164b30..0a6c6a2e7550 100644
--- a/drivers/net/mii.c
+++ b/drivers/net/mii.c
@@ -354,7 +354,7 @@ unsigned int mii_check_media (struct mii_if_info *mii,
 	if (!new_carrier) {
 		netif_carrier_off(mii->dev);
 		if (ok_to_print)
-			printk(KERN_INFO "%s: link down\n", mii->dev->name);
+			netdev_info(mii->dev, "link down\n");
 		return 0; /* duplex did not change */
 	}
 
@@ -381,12 +381,12 @@ unsigned int mii_check_media (struct mii_if_info *mii,
 		duplex = 1;
 
 	if (ok_to_print)
-		printk(KERN_INFO "%s: link up, %sMbps, %s-duplex, lpa 0x%04X\n",
-		       mii->dev->name,
-		       lpa2 & (LPA_1000FULL | LPA_1000HALF) ? "1000" :
-		       media & (ADVERTISE_100FULL | ADVERTISE_100HALF) ? "100" : "10",
-		       duplex ? "full" : "half",
-		       lpa);
+		netdev_info(mii->dev, "link up, %uMbps, %s-duplex, lpa 0x%04X\n",
+			    lpa2 & (LPA_1000FULL | LPA_1000HALF) ? 1000 :
+			    media & (ADVERTISE_100FULL | ADVERTISE_100HALF) ?
+			    100 : 10,
+			    duplex ? "full" : "half",
+			    lpa);
 
 	if ((init_media) || (mii->full_duplex != duplex)) {
 		mii->full_duplex = duplex;
diff --git a/drivers/net/mv643xx_eth.c b/drivers/net/mv643xx_eth.c
index 02076e16542a..34425b94452f 100644
--- a/drivers/net/mv643xx_eth.c
+++ b/drivers/net/mv643xx_eth.c
@@ -35,6 +35,8 @@
  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/init.h>
 #include <linux/dma-mapping.h>
 #include <linux/in.h>
@@ -627,9 +629,8 @@ err:
 		if ((cmd_sts & (RX_FIRST_DESC | RX_LAST_DESC)) !=
 			(RX_FIRST_DESC | RX_LAST_DESC)) {
 			if (net_ratelimit())
-				dev_printk(KERN_ERR, &mp->dev->dev,
-					   "received packet spanning "
-					   "multiple descriptors\n");
+				netdev_err(mp->dev,
+					   "received packet spanning multiple descriptors\n");
 		}
 
 		if (cmd_sts & ERROR_SUMMARY)
@@ -868,15 +869,14 @@ static netdev_tx_t mv643xx_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (has_tiny_unaligned_frags(skb) && __skb_linearize(skb)) {
 		txq->tx_dropped++;
-		dev_printk(KERN_DEBUG, &dev->dev,
-			   "failed to linearize skb with tiny "
-			   "unaligned fragment\n");
+		netdev_printk(KERN_DEBUG, dev,
+			      "failed to linearize skb with tiny unaligned fragment\n");
 		return NETDEV_TX_BUSY;
 	}
 
 	if (txq->tx_ring_size - txq->tx_desc_count < MAX_SKB_FRAGS + 1) {
 		if (net_ratelimit())
-			dev_printk(KERN_ERR, &dev->dev, "tx queue full?!\n");
+			netdev_err(dev, "tx queue full?!\n");
 		kfree_skb(skb);
 		return NETDEV_TX_OK;
 	}
@@ -959,7 +959,7 @@ static int txq_reclaim(struct tx_queue *txq, int budget, int force)
 			skb = __skb_dequeue(&txq->tx_skb);
 
 		if (cmd_sts & ERROR_SUMMARY) {
-			dev_printk(KERN_INFO, &mp->dev->dev, "tx error\n");
+			netdev_info(mp->dev, "tx error\n");
 			mp->dev->stats.tx_errors++;
 		}
 
@@ -1122,20 +1122,20 @@ static int smi_bus_read(struct mii_bus *bus, int addr, int reg)
 	int ret;
 
 	if (smi_wait_ready(msp)) {
-		printk(KERN_WARNING "mv643xx_eth: SMI bus busy timeout\n");
+		pr_warn("SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 
 	writel(SMI_OPCODE_READ | (reg << 21) | (addr << 16), smi_reg);
 
 	if (smi_wait_ready(msp)) {
-		printk(KERN_WARNING "mv643xx_eth: SMI bus busy timeout\n");
+		pr_warn("SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 
 	ret = readl(smi_reg);
 	if (!(ret & SMI_READ_VALID)) {
-		printk(KERN_WARNING "mv643xx_eth: SMI bus read not valid\n");
+		pr_warn("SMI bus read not valid\n");
 		return -ENODEV;
 	}
 
@@ -1148,7 +1148,7 @@ static int smi_bus_write(struct mii_bus *bus, int addr, int reg, u16 val)
 	void __iomem *smi_reg = msp->base + SMI_REG;
 
 	if (smi_wait_ready(msp)) {
-		printk(KERN_WARNING "mv643xx_eth: SMI bus busy timeout\n");
+		pr_warn("SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 
@@ -1156,7 +1156,7 @@ static int smi_bus_write(struct mii_bus *bus, int addr, int reg, u16 val)
 		(addr << 16) | (val & 0xffff), smi_reg);
 
 	if (smi_wait_ready(msp)) {
-		printk(KERN_WARNING "mv643xx_eth: SMI bus busy timeout\n");
+		pr_warn("SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 
@@ -1566,9 +1566,8 @@ mv643xx_eth_set_ringparam(struct net_device *dev, struct ethtool_ringparam *er)
 	if (netif_running(dev)) {
 		mv643xx_eth_stop(dev);
 		if (mv643xx_eth_open(dev)) {
-			dev_printk(KERN_ERR, &dev->dev,
-				   "fatal error on re-opening device after "
-				   "ring param change\n");
+			netdev_err(dev,
+				   "fatal error on re-opening device after ring param change\n");
 			return -ENOMEM;
 		}
 	}
@@ -1874,7 +1873,7 @@ static int rxq_init(struct mv643xx_eth_private *mp, int index)
 	}
 
 	if (rxq->rx_desc_area == NULL) {
-		dev_printk(KERN_ERR, &mp->dev->dev,
+		netdev_err(mp->dev,
 			   "can't allocate rx ring (%d bytes)\n", size);
 		goto out;
 	}
@@ -1884,8 +1883,7 @@ static int rxq_init(struct mv643xx_eth_private *mp, int index)
 	rxq->rx_skb = kmalloc(rxq->rx_ring_size * sizeof(*rxq->rx_skb),
 								GFP_KERNEL);
 	if (rxq->rx_skb == NULL) {
-		dev_printk(KERN_ERR, &mp->dev->dev,
-			   "can't allocate rx skb ring\n");
+		netdev_err(mp->dev, "can't allocate rx skb ring\n");
 		goto out_free;
 	}
 
@@ -1944,8 +1942,7 @@ static void rxq_deinit(struct rx_queue *rxq)
 	}
 
 	if (rxq->rx_desc_count) {
-		dev_printk(KERN_ERR, &mp->dev->dev,
-			   "error freeing rx ring -- %d skbs stuck\n",
+		netdev_err(mp->dev, "error freeing rx ring -- %d skbs stuck\n",
 			   rxq->rx_desc_count);
 	}
 
@@ -1987,7 +1984,7 @@ static int txq_init(struct mv643xx_eth_private *mp, int index)
 	}
 
 	if (txq->tx_desc_area == NULL) {
-		dev_printk(KERN_ERR, &mp->dev->dev,
+		netdev_err(mp->dev,
 			   "can't allocate tx ring (%d bytes)\n", size);
 		return -ENOMEM;
 	}
@@ -2093,7 +2090,7 @@ static void handle_link_event(struct mv643xx_eth_private *mp)
 		if (netif_carrier_ok(dev)) {
 			int i;
 
-			printk(KERN_INFO "%s: link down\n", dev->name);
+			netdev_info(dev, "link down\n");
 
 			netif_carrier_off(dev);
 
@@ -2124,10 +2121,8 @@ static void handle_link_event(struct mv643xx_eth_private *mp)
 	duplex = (port_status & FULL_DUPLEX) ? 1 : 0;
 	fc = (port_status & FLOW_CONTROL_ENABLED) ? 1 : 0;
 
-	printk(KERN_INFO "%s: link up, %d Mb/s, %s duplex, "
-			 "flow control %sabled\n", dev->name,
-			 speed, duplex ? "full" : "half",
-			 fc ? "en" : "dis");
+	netdev_info(dev, "link up, %d Mb/s, %s duplex, flow control %sabled\n",
+		    speed, duplex ? "full" : "half", fc ? "en" : "dis");
 
 	if (!netif_carrier_ok(dev))
 		netif_carrier_on(dev);
@@ -2337,7 +2332,7 @@ static int mv643xx_eth_open(struct net_device *dev)
 	err = request_irq(dev->irq, mv643xx_eth_irq,
 			  IRQF_SHARED, dev->name, dev);
 	if (err) {
-		dev_printk(KERN_ERR, &dev->dev, "can't assign irq\n");
+		netdev_err(dev, "can't assign irq\n");
 		return -EAGAIN;
 	}
 
@@ -2483,9 +2478,8 @@ static int mv643xx_eth_change_mtu(struct net_device *dev, int new_mtu)
 	 */
 	mv643xx_eth_stop(dev);
 	if (mv643xx_eth_open(dev)) {
-		dev_printk(KERN_ERR, &dev->dev,
-			   "fatal error on re-opening device after "
-			   "MTU change\n");
+		netdev_err(dev,
+			   "fatal error on re-opening device after MTU change\n");
 	}
 
 	return 0;
@@ -2508,7 +2502,7 @@ static void mv643xx_eth_tx_timeout(struct net_device *dev)
 {
 	struct mv643xx_eth_private *mp = netdev_priv(dev);
 
-	dev_printk(KERN_INFO, &dev->dev, "tx timeout\n");
+	netdev_info(dev, "tx timeout\n");
 
 	schedule_work(&mp->tx_timeout_task);
 }
@@ -2603,8 +2597,8 @@ static int mv643xx_eth_shared_probe(struct platform_device *pdev)
 	int ret;
 
 	if (!mv643xx_eth_version_printed++)
-		printk(KERN_NOTICE "MV-643xx 10/100/1000 ethernet "
-			"driver version %s\n", mv643xx_eth_driver_version);
+		pr_notice("MV-643xx 10/100/1000 ethernet driver version %s\n",
+			  mv643xx_eth_driver_version);
 
 	ret = -EINVAL;
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -2871,14 +2865,12 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 
 	pd = pdev->dev.platform_data;
 	if (pd == NULL) {
-		dev_printk(KERN_ERR, &pdev->dev,
-			   "no mv643xx_eth_platform_data\n");
+		dev_err(&pdev->dev, "no mv643xx_eth_platform_data\n");
 		return -ENODEV;
 	}
 
 	if (pd->shared == NULL) {
-		dev_printk(KERN_ERR, &pdev->dev,
-			   "no mv643xx_eth_platform_data->shared\n");
+		dev_err(&pdev->dev, "no mv643xx_eth_platform_data->shared\n");
 		return -ENODEV;
 	}
 
@@ -2957,11 +2949,11 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 	if (err)
 		goto out;
 
-	dev_printk(KERN_NOTICE, &dev->dev, "port %d with MAC address %pM\n",
-		   mp->port_num, dev->dev_addr);
+	netdev_notice(dev, "port %d with MAC address %pM\n",
+		      mp->port_num, dev->dev_addr);
 
 	if (mp->tx_desc_sram_size > 0)
-		dev_printk(KERN_NOTICE, &dev->dev, "configured with sram\n");
+		netdev_notice(dev, "configured with sram\n");
 
 	return 0;
 
diff --git a/drivers/net/myri10ge/myri10ge.c b/drivers/net/myri10ge/myri10ge.c
index ea5cfe2c3a04..a7f2eed9a08a 100644
--- a/drivers/net/myri10ge/myri10ge.c
+++ b/drivers/net/myri10ge/myri10ge.c
@@ -253,7 +253,7 @@ struct myri10ge_priv {
 	unsigned long serial_number;
 	int vendor_specific_offset;
 	int fw_multicast_support;
-	unsigned long features;
+	u32 features;
 	u32 max_tso6;
 	u32 read_dma;
 	u32 write_dma;
@@ -1776,7 +1776,7 @@ static int myri10ge_set_rx_csum(struct net_device *netdev, u32 csum_enabled)
 static int myri10ge_set_tso(struct net_device *netdev, u32 tso_enabled)
 {
 	struct myri10ge_priv *mgp = netdev_priv(netdev);
-	unsigned long flags = mgp->features & (NETIF_F_TSO6 | NETIF_F_TSO);
+	u32 flags = mgp->features & (NETIF_F_TSO6 | NETIF_F_TSO);
 
 	if (tso_enabled)
 		netdev->features |= flags;
diff --git a/drivers/net/myri_sbus.c b/drivers/net/myri_sbus.c
index 4846e131a04e..a761076b69c3 100644
--- a/drivers/net/myri_sbus.c
+++ b/drivers/net/myri_sbus.c
@@ -926,7 +926,7 @@ static const struct net_device_ops myri_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 };
 
-static int __devinit myri_sbus_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit myri_sbus_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	static unsigned version_printed;
@@ -1160,7 +1160,7 @@ static const struct of_device_id myri_sbus_match[] = {
 
 MODULE_DEVICE_TABLE(of, myri_sbus_match);
 
-static struct of_platform_driver myri_sbus_driver = {
+static struct platform_driver myri_sbus_driver = {
 	.driver = {
 		.name = "myri",
 		.owner = THIS_MODULE,
@@ -1172,12 +1172,12 @@ static struct of_platform_driver myri_sbus_driver = {
 
 static int __init myri_sbus_init(void)
 {
-	return of_register_platform_driver(&myri_sbus_driver);
+	return platform_driver_register(&myri_sbus_driver);
 }
 
 static void __exit myri_sbus_exit(void)
 {
-	of_unregister_platform_driver(&myri_sbus_driver);
+	platform_driver_unregister(&myri_sbus_driver);
 }
 
 module_init(myri_sbus_init);
diff --git a/drivers/net/niu.c b/drivers/net/niu.c
index 9fb59d3f9c92..40fa59e2fd5c 100644
--- a/drivers/net/niu.c
+++ b/drivers/net/niu.c
@@ -10062,8 +10062,7 @@ static const struct niu_ops niu_phys_ops = {
 	.unmap_single	= niu_phys_unmap_single,
 };
 
-static int __devinit niu_of_probe(struct platform_device *op,
-				  const struct of_device_id *match)
+static int __devinit niu_of_probe(struct platform_device *op)
 {
 	union niu_parent_id parent_id;
 	struct net_device *dev;
@@ -10223,7 +10222,7 @@ static const struct of_device_id niu_match[] = {
 };
 MODULE_DEVICE_TABLE(of, niu_match);
 
-static struct of_platform_driver niu_of_driver = {
+static struct platform_driver niu_of_driver = {
 	.driver = {
 		.name = "niu",
 		.owner = THIS_MODULE,
@@ -10244,14 +10243,14 @@ static int __init niu_init(void)
 	niu_debug = netif_msg_init(debug, NIU_MSG_DEFAULT);
 
 #ifdef CONFIG_SPARC64
-	err = of_register_platform_driver(&niu_of_driver);
+	err = platform_driver_register(&niu_of_driver);
 #endif
 
 	if (!err) {
 		err = pci_register_driver(&niu_pci_driver);
 #ifdef CONFIG_SPARC64
 		if (err)
-			of_unregister_platform_driver(&niu_of_driver);
+			platform_driver_unregister(&niu_of_driver);
 #endif
 	}
 
@@ -10262,7 +10261,7 @@ static void __exit niu_exit(void)
 {
 	pci_unregister_driver(&niu_pci_driver);
 #ifdef CONFIG_SPARC64
-	of_unregister_platform_driver(&niu_of_driver);
+	platform_driver_unregister(&niu_of_driver);
 #endif
 }
 
diff --git a/drivers/net/pch_gbe/pch_gbe_main.c b/drivers/net/pch_gbe/pch_gbe_main.c
index b99e90aca37d..8c66e22c3a0a 100644
--- a/drivers/net/pch_gbe/pch_gbe_main.c
+++ b/drivers/net/pch_gbe/pch_gbe_main.c
@@ -2446,7 +2446,7 @@ static struct pci_driver pch_gbe_pcidev = {
 	.id_table = pch_gbe_pcidev_id,
 	.probe = pch_gbe_probe,
 	.remove = pch_gbe_remove,
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 	.driver.pm = &pch_gbe_pm_ops,
 #endif
 	.shutdown = pch_gbe_shutdown,
diff --git a/drivers/net/pcmcia/fmvj18x_cs.c b/drivers/net/pcmcia/fmvj18x_cs.c
index 9226cda4d054..530ab5a10bd3 100644
--- a/drivers/net/pcmcia/fmvj18x_cs.c
+++ b/drivers/net/pcmcia/fmvj18x_cs.c
@@ -691,6 +691,7 @@ static struct pcmcia_device_id fmvj18x_ids[] = {
 	PCMCIA_PFC_DEVICE_MANF_CARD(0, 0x0105, 0x0e0a),
 	PCMCIA_PFC_DEVICE_MANF_CARD(0, 0x0032, 0x0e01),
 	PCMCIA_PFC_DEVICE_MANF_CARD(0, 0x0032, 0x0a05),
+	PCMCIA_PFC_DEVICE_MANF_CARD(0, 0x0032, 0x0b05),
 	PCMCIA_PFC_DEVICE_MANF_CARD(0, 0x0032, 0x1101),
 	PCMCIA_DEVICE_NULL,
 };
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 35fda5ac8120..392a6c4b72e5 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -77,7 +77,6 @@ config NATIONAL_PHY
 	  Currently supports the DP83865 PHY.
 
 config STE10XP
-	depends on PHYLIB
 	tristate "Driver for STMicroelectronics STe10Xp PHYs"
 	---help---
 	  This is the driver for the STe100p and STe101p PHYs.
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index f62c7b717bc8..47c8339a0359 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -188,8 +188,7 @@ static int __devexit mdio_gpio_remove(struct platform_device *pdev)
 
 #ifdef CONFIG_OF_GPIO
 
-static int __devinit mdio_ofgpio_probe(struct platform_device *ofdev,
-                                        const struct of_device_id *match)
+static int __devinit mdio_ofgpio_probe(struct platform_device *ofdev)
 {
 	struct mdio_gpio_platform_data *pdata;
 	struct mii_bus *new_bus;
@@ -240,7 +239,7 @@ static struct of_device_id mdio_ofgpio_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mdio_ofgpio_match);
 
-static struct of_platform_driver mdio_ofgpio_driver = {
+static struct platform_driver mdio_ofgpio_driver = {
 	.driver = {
 		.name = "mdio-gpio",
 		.owner = THIS_MODULE,
@@ -252,12 +251,12 @@ static struct of_platform_driver mdio_ofgpio_driver = {
 
 static inline int __init mdio_ofgpio_init(void)
 {
-	return of_register_platform_driver(&mdio_ofgpio_driver);
+	return platform_driver_register(&mdio_ofgpio_driver);
 }
 
 static inline void __exit mdio_ofgpio_exit(void)
 {
-	of_unregister_platform_driver(&mdio_ofgpio_driver);
+	platform_driver_unregister(&mdio_ofgpio_driver);
 }
 #else
 static inline int __init mdio_ofgpio_init(void) { return 0; }
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 0fd1678bc5a9..590f902deb6b 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -19,13 +19,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/phy.h>
-
-#define	PHY_ID_KSZ9021			0x00221611
-#define	PHY_ID_KS8737			0x00221720
-#define	PHY_ID_KS8041			0x00221510
-#define	PHY_ID_KS8051			0x00221550
-/* both for ks8001 Rev. A/B, and for ks8721 Rev 3. */
-#define	PHY_ID_KS8001			0x0022161A
+#include <linux/micrel_phy.h>
 
 /* general Interrupt control/status reg in vendor specific block. */
 #define MII_KSZPHY_INTCS			0x1B
@@ -46,6 +40,7 @@
 #define KSZPHY_CTRL_INT_ACTIVE_HIGH		(1 << 9)
 #define KSZ9021_CTRL_INT_ACTIVE_HIGH		(1 << 14)
 #define KS8737_CTRL_INT_ACTIVE_HIGH		(1 << 14)
+#define KSZ8051_RMII_50MHZ_CLK			(1 << 7)
 
 static int kszphy_ack_interrupt(struct phy_device *phydev)
 {
@@ -106,6 +101,19 @@ static int kszphy_config_init(struct phy_device *phydev)
 	return 0;
 }
 
+static int ks8051_config_init(struct phy_device *phydev)
+{
+	int regval;
+
+	if (phydev->dev_flags & MICREL_PHY_50MHZ_CLK) {
+		regval = phy_read(phydev, MII_KSZPHY_CTRL);
+		regval |= KSZ8051_RMII_50MHZ_CLK;
+		phy_write(phydev, MII_KSZPHY_CTRL, regval);
+	}
+
+	return 0;
+}
+
 static struct phy_driver ks8737_driver = {
 	.phy_id		= PHY_ID_KS8737,
 	.phy_id_mask	= 0x00fffff0,
@@ -142,7 +150,7 @@ static struct phy_driver ks8051_driver = {
 	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
-	.config_init	= kszphy_config_init,
+	.config_init	= ks8051_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
diff --git a/drivers/net/ppp_generic.c b/drivers/net/ppp_generic.c
index c7a6c4466978..9f6d670748d1 100644
--- a/drivers/net/ppp_generic.c
+++ b/drivers/net/ppp_generic.c
@@ -592,8 +592,8 @@ static long ppp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			ppp_release(NULL, file);
 			err = 0;
 		} else
-			printk(KERN_DEBUG "PPPIOCDETACH file->f_count=%ld\n",
-			       atomic_long_read(&file->f_count));
+			pr_warn("PPPIOCDETACH file->f_count=%ld\n",
+				atomic_long_read(&file->f_count));
 		mutex_unlock(&ppp_mutex);
 		return err;
 	}
@@ -630,7 +630,7 @@ static long ppp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 	if (pf->kind != INTERFACE) {
 		/* can't happen */
-		printk(KERN_ERR "PPP: not interface or channel??\n");
+		pr_err("PPP: not interface or channel??\n");
 		return -EINVAL;
 	}
 
@@ -704,7 +704,8 @@ static long ppp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		}
 		vj = slhc_init(val2+1, val+1);
 		if (!vj) {
-			printk(KERN_ERR "PPP: no memory (VJ compressor)\n");
+			netdev_err(ppp->dev,
+				   "PPP: no memory (VJ compressor)\n");
 			err = -ENOMEM;
 			break;
 		}
@@ -898,17 +899,17 @@ static int __init ppp_init(void)
 {
 	int err;
 
-	printk(KERN_INFO "PPP generic driver version " PPP_VERSION "\n");
+	pr_info("PPP generic driver version " PPP_VERSION "\n");
 
 	err = register_pernet_device(&ppp_net_ops);
 	if (err) {
-		printk(KERN_ERR "failed to register PPP pernet device (%d)\n", err);
+		pr_err("failed to register PPP pernet device (%d)\n", err);
 		goto out;
 	}
 
 	err = register_chrdev(PPP_MAJOR, "ppp", &ppp_device_fops);
 	if (err) {
-		printk(KERN_ERR "failed to register PPP device (%d)\n", err);
+		pr_err("failed to register PPP device (%d)\n", err);
 		goto out_net;
 	}
 
@@ -1078,7 +1079,7 @@ pad_compress_skb(struct ppp *ppp, struct sk_buff *skb)
 	new_skb = alloc_skb(new_skb_size, GFP_ATOMIC);
 	if (!new_skb) {
 		if (net_ratelimit())
-			printk(KERN_ERR "PPP: no memory (comp pkt)\n");
+			netdev_err(ppp->dev, "PPP: no memory (comp pkt)\n");
 		return NULL;
 	}
 	if (ppp->dev->hard_header_len > PPP_HDRLEN)
@@ -1108,7 +1109,7 @@ pad_compress_skb(struct ppp *ppp, struct sk_buff *skb)
 		 * the same number.
 		 */
 		if (net_ratelimit())
-			printk(KERN_ERR "ppp: compressor dropped pkt\n");
+			netdev_err(ppp->dev, "ppp: compressor dropped pkt\n");
 		kfree_skb(skb);
 		kfree_skb(new_skb);
 		new_skb = NULL;
@@ -1138,7 +1139,9 @@ ppp_send_frame(struct ppp *ppp, struct sk_buff *skb)
 		if (ppp->pass_filter &&
 		    sk_run_filter(skb, ppp->pass_filter) == 0) {
 			if (ppp->debug & 1)
-				printk(KERN_DEBUG "PPP: outbound frame not passed\n");
+				netdev_printk(KERN_DEBUG, ppp->dev,
+					      "PPP: outbound frame "
+					      "not passed\n");
 			kfree_skb(skb);
 			return;
 		}
@@ -1164,7 +1167,7 @@ ppp_send_frame(struct ppp *ppp, struct sk_buff *skb)
 		new_skb = alloc_skb(skb->len + ppp->dev->hard_header_len - 2,
 				    GFP_ATOMIC);
 		if (!new_skb) {
-			printk(KERN_ERR "PPP: no memory (VJ comp pkt)\n");
+			netdev_err(ppp->dev, "PPP: no memory (VJ comp pkt)\n");
 			goto drop;
 		}
 		skb_reserve(new_skb, ppp->dev->hard_header_len - 2);
@@ -1202,7 +1205,9 @@ ppp_send_frame(struct ppp *ppp, struct sk_buff *skb)
 	    proto != PPP_LCP && proto != PPP_CCP) {
 		if (!(ppp->flags & SC_CCP_UP) && (ppp->flags & SC_MUST_COMP)) {
 			if (net_ratelimit())
-				printk(KERN_ERR "ppp: compression required but down - pkt dropped.\n");
+				netdev_err(ppp->dev,
+					   "ppp: compression required but "
+					   "down - pkt dropped.\n");
 			goto drop;
 		}
 		skb = pad_compress_skb(ppp, skb);
@@ -1505,7 +1510,7 @@ static int ppp_mp_explode(struct ppp *ppp, struct sk_buff *skb)
  noskb:
 	spin_unlock_bh(&pch->downl);
 	if (ppp->debug & 1)
-		printk(KERN_ERR "PPP: no memory (fragment)\n");
+		netdev_err(ppp->dev, "PPP: no memory (fragment)\n");
 	++ppp->dev->stats.tx_errors;
 	++ppp->nxseq;
 	return 1;	/* abandon the frame */
@@ -1686,7 +1691,8 @@ ppp_receive_nonmp_frame(struct ppp *ppp, struct sk_buff *skb)
 			/* copy to a new sk_buff with more tailroom */
 			ns = dev_alloc_skb(skb->len + 128);
 			if (!ns) {
-				printk(KERN_ERR"PPP: no memory (VJ decomp)\n");
+				netdev_err(ppp->dev, "PPP: no memory "
+					   "(VJ decomp)\n");
 				goto err;
 			}
 			skb_reserve(ns, 2);
@@ -1699,7 +1705,8 @@ ppp_receive_nonmp_frame(struct ppp *ppp, struct sk_buff *skb)
 
 		len = slhc_uncompress(ppp->vj, skb->data + 2, skb->len - 2);
 		if (len <= 0) {
-			printk(KERN_DEBUG "PPP: VJ decompression error\n");
+			netdev_printk(KERN_DEBUG, ppp->dev,
+				      "PPP: VJ decompression error\n");
 			goto err;
 		}
 		len += 2;
@@ -1721,7 +1728,7 @@ ppp_receive_nonmp_frame(struct ppp *ppp, struct sk_buff *skb)
 			goto err;
 
 		if (slhc_remember(ppp->vj, skb->data + 2, skb->len - 2) <= 0) {
-			printk(KERN_ERR "PPP: VJ uncompressed error\n");
+			netdev_err(ppp->dev, "PPP: VJ uncompressed error\n");
 			goto err;
 		}
 		proto = PPP_IP;
@@ -1762,8 +1769,9 @@ ppp_receive_nonmp_frame(struct ppp *ppp, struct sk_buff *skb)
 			if (ppp->pass_filter &&
 			    sk_run_filter(skb, ppp->pass_filter) == 0) {
 				if (ppp->debug & 1)
-					printk(KERN_DEBUG "PPP: inbound frame "
-					       "not passed\n");
+					netdev_printk(KERN_DEBUG, ppp->dev,
+						      "PPP: inbound frame "
+						      "not passed\n");
 				kfree_skb(skb);
 				return;
 			}
@@ -1821,7 +1829,8 @@ ppp_decompress_frame(struct ppp *ppp, struct sk_buff *skb)
 
 		ns = dev_alloc_skb(obuff_size);
 		if (!ns) {
-			printk(KERN_ERR "ppp_decompress_frame: no memory\n");
+			netdev_err(ppp->dev, "ppp_decompress_frame: "
+				   "no memory\n");
 			goto err;
 		}
 		/* the decompressor still expects the A/C bytes in the hdr */
@@ -1989,7 +1998,7 @@ ppp_mp_reconstruct(struct ppp *ppp)
 	u32 seq = ppp->nextseq;
 	u32 minseq = ppp->minseq;
 	struct sk_buff_head *list = &ppp->mrq;
-	struct sk_buff *p, *next;
+	struct sk_buff *p, *tmp;
 	struct sk_buff *head, *tail;
 	struct sk_buff *skb = NULL;
 	int lost = 0, len = 0;
@@ -1998,13 +2007,15 @@ ppp_mp_reconstruct(struct ppp *ppp)
 		return NULL;
 	head = list->next;
 	tail = NULL;
-	for (p = head; p != (struct sk_buff *) list; p = next) {
-		next = p->next;
+	skb_queue_walk_safe(list, p, tmp) {
+	again:
 		if (seq_before(PPP_MP_CB(p)->sequence, seq)) {
 			/* this can't happen, anyway ignore the skb */
-			printk(KERN_ERR "ppp_mp_reconstruct bad seq %u < %u\n",
-			       PPP_MP_CB(p)->sequence, seq);
-			head = next;
+			netdev_err(ppp->dev, "ppp_mp_reconstruct bad "
+				   "seq %u < %u\n",
+				   PPP_MP_CB(p)->sequence, seq);
+			__skb_unlink(p, list);
+			kfree_skb(p);
 			continue;
 		}
 		if (PPP_MP_CB(p)->sequence != seq) {
@@ -2016,8 +2027,7 @@ ppp_mp_reconstruct(struct ppp *ppp)
 			lost = 1;
 			seq = seq_before(minseq, PPP_MP_CB(p)->sequence)?
 				minseq + 1: PPP_MP_CB(p)->sequence;
-			next = p;
-			continue;
+			goto again;
 		}
 
 		/*
@@ -2042,17 +2052,9 @@ ppp_mp_reconstruct(struct ppp *ppp)
 		    (PPP_MP_CB(head)->BEbits & B)) {
 			if (len > ppp->mrru + 2) {
 				++ppp->dev->stats.rx_length_errors;
-				printk(KERN_DEBUG "PPP: reconstructed packet"
-				       " is too long (%d)\n", len);
-			} else if (p == head) {
-				/* fragment is complete packet - reuse skb */
-				tail = p;
-				skb = skb_get(p);
-				break;
-			} else if ((skb = dev_alloc_skb(len)) == NULL) {
-				++ppp->dev->stats.rx_missed_errors;
-				printk(KERN_DEBUG "PPP: no memory for "
-				       "reconstructed packet");
+				netdev_printk(KERN_DEBUG, ppp->dev,
+					      "PPP: reconstructed packet"
+					      " is too long (%d)\n", len);
 			} else {
 				tail = p;
 				break;
@@ -2065,9 +2067,17 @@ ppp_mp_reconstruct(struct ppp *ppp)
 		 * and we haven't found a complete valid packet yet,
 		 * we can discard up to and including this fragment.
 		 */
-		if (PPP_MP_CB(p)->BEbits & E)
-			head = next;
+		if (PPP_MP_CB(p)->BEbits & E) {
+			struct sk_buff *tmp2;
 
+			skb_queue_reverse_walk_from_safe(list, p, tmp2) {
+				__skb_unlink(p, list);
+				kfree_skb(p);
+			}
+			head = skb_peek(list);
+			if (!head)
+				break;
+		}
 		++seq;
 	}
 
@@ -2077,26 +2087,37 @@ ppp_mp_reconstruct(struct ppp *ppp)
 		   signal a receive error. */
 		if (PPP_MP_CB(head)->sequence != ppp->nextseq) {
 			if (ppp->debug & 1)
-				printk(KERN_DEBUG "  missed pkts %u..%u\n",
-				       ppp->nextseq,
-				       PPP_MP_CB(head)->sequence-1);
+				netdev_printk(KERN_DEBUG, ppp->dev,
+					      "  missed pkts %u..%u\n",
+					      ppp->nextseq,
+					      PPP_MP_CB(head)->sequence-1);
 			++ppp->dev->stats.rx_dropped;
 			ppp_receive_error(ppp);
 		}
 
-		if (head != tail)
-			/* copy to a single skb */
-			for (p = head; p != tail->next; p = p->next)
-				skb_copy_bits(p, 0, skb_put(skb, p->len), p->len);
-		ppp->nextseq = PPP_MP_CB(tail)->sequence + 1;
-		head = tail->next;
-	}
+		skb = head;
+		if (head != tail) {
+			struct sk_buff **fragpp = &skb_shinfo(skb)->frag_list;
+			p = skb_queue_next(list, head);
+			__skb_unlink(skb, list);
+			skb_queue_walk_from_safe(list, p, tmp) {
+				__skb_unlink(p, list);
+				*fragpp = p;
+				p->next = NULL;
+				fragpp = &p->next;
+
+				skb->len += p->len;
+				skb->data_len += p->len;
+				skb->truesize += p->len;
+
+				if (p == tail)
+					break;
+			}
+		} else {
+			__skb_unlink(skb, list);
+		}
 
-	/* Discard all the skbuffs that we have copied the data out of
-	   or that we can't use. */
-	while ((p = list->next) != head) {
-		__skb_unlink(p, list);
-		kfree_skb(p);
+		ppp->nextseq = PPP_MP_CB(tail)->sequence + 1;
 	}
 
 	return skb;
@@ -2617,8 +2638,8 @@ ppp_create_interface(struct net *net, int unit, int *retp)
 	ret = register_netdev(dev);
 	if (ret != 0) {
 		unit_put(&pn->units_idr, unit);
-		printk(KERN_ERR "PPP: couldn't register device %s (%d)\n",
-		       dev->name, ret);
+		netdev_err(ppp->dev, "PPP: couldn't register device %s (%d)\n",
+			   dev->name, ret);
 		goto out2;
 	}
 
@@ -2690,9 +2711,9 @@ static void ppp_destroy_interface(struct ppp *ppp)
 
 	if (!ppp->file.dead || ppp->n_channels) {
 		/* "can't happen" */
-		printk(KERN_ERR "ppp: destroying ppp struct %p but dead=%d "
-		       "n_channels=%d !\n", ppp, ppp->file.dead,
-		       ppp->n_channels);
+		netdev_err(ppp->dev, "ppp: destroying ppp struct %p "
+			   "but dead=%d n_channels=%d !\n",
+			   ppp, ppp->file.dead, ppp->n_channels);
 		return;
 	}
 
@@ -2834,8 +2855,7 @@ static void ppp_destroy_channel(struct channel *pch)
 
 	if (!pch->file.dead) {
 		/* "can't happen" */
-		printk(KERN_ERR "ppp: destroying undead channel %p !\n",
-		       pch);
+		pr_err("ppp: destroying undead channel %p !\n", pch);
 		return;
 	}
 	skb_queue_purge(&pch->file.xq);
@@ -2847,7 +2867,7 @@ static void __exit ppp_cleanup(void)
 {
 	/* should never happen */
 	if (atomic_read(&ppp_unit_count) || atomic_read(&channel_count))
-		printk(KERN_ERR "PPP: removing module but units remain!\n");
+		pr_err("PPP: removing module but units remain!\n");
 	unregister_chrdev(PPP_MAJOR, "ppp");
 	device_destroy(ppp_class, MKDEV(PPP_MAJOR, 0));
 	class_destroy(ppp_class);
@@ -2865,7 +2885,7 @@ static int __unit_alloc(struct idr *p, void *ptr, int n)
 
 again:
 	if (!idr_pre_get(p, GFP_KERNEL)) {
-		printk(KERN_ERR "PPP: No free memory for idr\n");
+		pr_err("PPP: No free memory for idr\n");
 		return -ENOMEM;
 	}
 
diff --git a/drivers/net/pptp.c b/drivers/net/pptp.c
index 164cfad6ce79..1af549c89d51 100644
--- a/drivers/net/pptp.c
+++ b/drivers/net/pptp.c
@@ -175,7 +175,6 @@ static int pptp_xmit(struct ppp_channel *chan, struct sk_buff *skb)
 	struct pptp_opt *opt = &po->proto.pptp;
 	struct pptp_gre_header *hdr;
 	unsigned int header_len = sizeof(*hdr);
-	int err = 0;
 	int islcp;
 	int len;
 	unsigned char *data;
@@ -198,8 +197,8 @@ static int pptp_xmit(struct ppp_channel *chan, struct sk_buff *skb)
 					.saddr = opt->src_addr.sin_addr.s_addr,
 					.tos = RT_TOS(0) } },
 			.proto = IPPROTO_GRE };
-		err = ip_route_output_key(&init_net, &rt, &fl);
-		if (err)
+		rt = ip_route_output_key(&init_net, &fl);
+		if (IS_ERR(rt))
 			goto tx_error;
 	}
 	tdev = rt->dst.dev;
@@ -477,7 +476,8 @@ static int pptp_connect(struct socket *sock, struct sockaddr *uservaddr,
 					.tos = RT_CONN_FLAGS(sk) } },
 			.proto = IPPROTO_GRE };
 		security_sk_classify_flow(sk, &fl);
-		if (ip_route_output_key(&init_net, &rt, &fl)) {
+		rt = ip_route_output_key(&init_net, &fl);
+		if (IS_ERR(rt)) {
 			error = -EHOSTUNREACH;
 			goto end;
 		}
diff --git a/drivers/net/qla3xxx.c b/drivers/net/qla3xxx.c
index 1a3584edd79c..348b4f1367c9 100644
--- a/drivers/net/qla3xxx.c
+++ b/drivers/net/qla3xxx.c
@@ -379,7 +379,7 @@ static void fm93c56a_select(struct ql3_adapter *qdev)
 {
 	struct ql3xxx_port_registers __iomem *port_regs =
 			qdev->mem_map_registers;
-	u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
+	__iomem u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
 
 	qdev->eeprom_cmd_data = AUBURN_EEPROM_CS_1;
 	ql_write_nvram_reg(qdev, spir, ISP_NVRAM_MASK | qdev->eeprom_cmd_data);
@@ -398,7 +398,7 @@ static void fm93c56a_cmd(struct ql3_adapter *qdev, u32 cmd, u32 eepromAddr)
 	u32 previousBit;
 	struct ql3xxx_port_registers __iomem *port_regs =
 			qdev->mem_map_registers;
-	u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
+	__iomem u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
 
 	/* Clock in a zero, then do the start bit */
 	ql_write_nvram_reg(qdev, spir,
@@ -467,7 +467,7 @@ static void fm93c56a_deselect(struct ql3_adapter *qdev)
 {
 	struct ql3xxx_port_registers __iomem *port_regs =
 			qdev->mem_map_registers;
-	u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
+	__iomem u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
 
 	qdev->eeprom_cmd_data = AUBURN_EEPROM_CS_0;
 	ql_write_nvram_reg(qdev, spir, ISP_NVRAM_MASK | qdev->eeprom_cmd_data);
@@ -483,7 +483,7 @@ static void fm93c56a_datain(struct ql3_adapter *qdev, unsigned short *value)
 	u32 dataBit;
 	struct ql3xxx_port_registers __iomem *port_regs =
 			qdev->mem_map_registers;
-	u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
+	__iomem u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
 
 	/* Read the data bits */
 	/* The first bit is a dummy.  Clock right over it. */
@@ -2460,7 +2460,7 @@ map_error:
  * The 3032 supports sglists by using the 3 addr/len pairs (ALP)
  * in the IOCB plus a chain of outbound address lists (OAL) that
  * each contain 5 ALPs.  The last ALP of the IOCB (3rd) or OAL (5th)
- * will used to point to an OAL when more ALP entries are required.
+ * will be used to point to an OAL when more ALP entries are required.
  * The IOCB is always the top of the chain followed by one or more
  * OALs (when necessary).
  */
@@ -3011,7 +3011,7 @@ static int ql_adapter_initialize(struct ql3_adapter *qdev)
 	u32 value;
 	struct ql3xxx_port_registers __iomem *port_regs =
 		qdev->mem_map_registers;
-	u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
+	__iomem u32 *spir = &port_regs->CommonRegs.serialPortInterfaceReg;
 	struct ql3xxx_host_memory_registers __iomem *hmem_regs =
 		(void __iomem *)port_regs;
 	u32 delay = 10;
diff --git a/drivers/net/qlcnic/qlcnic.h b/drivers/net/qlcnic/qlcnic.h
index 44e316fd67b8..dc44564ef6f9 100644
--- a/drivers/net/qlcnic/qlcnic.h
+++ b/drivers/net/qlcnic/qlcnic.h
@@ -867,7 +867,6 @@ struct qlcnic_nic_intr_coalesce {
 #define LINKEVENT_LINKSPEED_MBPS	0
 #define LINKEVENT_LINKSPEED_ENCODED	1
 
-#define AUTO_FW_RESET_ENABLED	0x01
 /* firmware response header:
  *	63:58 - message type
  *	57:56 - owner
@@ -1133,14 +1132,10 @@ struct qlcnic_eswitch {
 #define MAX_BW			100	/* % of link speed */
 #define MAX_VLAN_ID		4095
 #define MIN_VLAN_ID		2
-#define MAX_TX_QUEUES		1
-#define MAX_RX_QUEUES		4
 #define DEFAULT_MAC_LEARN	1
 
 #define IS_VALID_VLAN(vlan)	(vlan >= MIN_VLAN_ID && vlan < MAX_VLAN_ID)
 #define IS_VALID_BW(bw)		(bw <= MAX_BW)
-#define IS_VALID_TX_QUEUES(que)	(que > 0 && que <= MAX_TX_QUEUES)
-#define IS_VALID_RX_QUEUES(que)	(que > 0 && que <= MAX_RX_QUEUES)
 
 struct qlcnic_pci_func_cfg {
 	u16	func_type;
diff --git a/drivers/net/qlcnic/qlcnic_main.c b/drivers/net/qlcnic/qlcnic_main.c
index 37c04b4fade3..cd88c7e1bfa9 100644
--- a/drivers/net/qlcnic/qlcnic_main.c
+++ b/drivers/net/qlcnic/qlcnic_main.c
@@ -42,7 +42,7 @@ static int use_msi_x = 1;
 module_param(use_msi_x, int, 0444);
 MODULE_PARM_DESC(use_msi_x, "MSI-X interrupt (0=disabled, 1=enabled");
 
-static int auto_fw_reset = AUTO_FW_RESET_ENABLED;
+static int auto_fw_reset = 1;
 module_param(auto_fw_reset, int, 0644);
 MODULE_PARM_DESC(auto_fw_reset, "Auto firmware reset (0=disabled, 1=enabled");
 
@@ -2959,8 +2959,7 @@ qlcnic_check_health(struct qlcnic_adapter *adapter)
 		if (adapter->need_fw_reset)
 			goto detach;
 
-		if (adapter->reset_context &&
-		    auto_fw_reset == AUTO_FW_RESET_ENABLED) {
+		if (adapter->reset_context && auto_fw_reset) {
 			qlcnic_reset_hw_context(adapter);
 			adapter->netdev->trans_start = jiffies;
 		}
@@ -2973,7 +2972,7 @@ qlcnic_check_health(struct qlcnic_adapter *adapter)
 
 	qlcnic_dev_request_reset(adapter);
 
-	if ((auto_fw_reset == AUTO_FW_RESET_ENABLED))
+	if (auto_fw_reset)
 		clear_bit(__QLCNIC_FW_ATTACHED, &adapter->state);
 
 	dev_info(&netdev->dev, "firmware hang detected\n");
@@ -2982,7 +2981,7 @@ detach:
 	adapter->dev_state = (state == QLCNIC_DEV_NEED_QUISCENT) ? state :
 		QLCNIC_DEV_NEED_RESET;
 
-	if ((auto_fw_reset == AUTO_FW_RESET_ENABLED) &&
+	if (auto_fw_reset &&
 		!test_and_set_bit(__QLCNIC_RESETTING, &adapter->state)) {
 
 		qlcnic_schedule_work(adapter, qlcnic_detach_work, 0);
@@ -3654,10 +3653,8 @@ validate_npar_config(struct qlcnic_adapter *adapter,
 		if (adapter->npars[pci_func].type != QLCNIC_TYPE_NIC)
 			return QL_STATUS_INVALID_PARAM;
 
-		if (!IS_VALID_BW(np_cfg[i].min_bw)
-				|| !IS_VALID_BW(np_cfg[i].max_bw)
-				|| !IS_VALID_RX_QUEUES(np_cfg[i].max_rx_queues)
-				|| !IS_VALID_TX_QUEUES(np_cfg[i].max_tx_queues))
+		if (!IS_VALID_BW(np_cfg[i].min_bw) ||
+		    !IS_VALID_BW(np_cfg[i].max_bw))
 			return QL_STATUS_INVALID_PARAM;
 	}
 	return 0;
diff --git a/drivers/net/r6040.c b/drivers/net/r6040.c
index 27e6f6d43cac..e3ebd90ae651 100644
--- a/drivers/net/r6040.c
+++ b/drivers/net/r6040.c
@@ -49,8 +49,8 @@
 #include <asm/processor.h>
 
 #define DRV_NAME	"r6040"
-#define DRV_VERSION	"0.26"
-#define DRV_RELDATE	"30May2010"
+#define DRV_VERSION	"0.27"
+#define DRV_RELDATE	"23Feb2011"
 
 /* PHY CHIP Address */
 #define PHY1_ADDR	1	/* For MAC1 */
@@ -69,6 +69,8 @@
 
 /* MAC registers */
 #define MCR0		0x00	/* Control register 0 */
+#define  MCR0_PROMISC	0x0020	/* Promiscuous mode */
+#define  MCR0_HASH_EN	0x0100	/* Enable multicast hash table function */
 #define MCR1		0x04	/* Control register 1 */
 #define  MAC_RST	0x0001	/* Reset the MAC */
 #define MBCR		0x08	/* Bus control */
@@ -851,77 +853,92 @@ static void r6040_multicast_list(struct net_device *dev)
 {
 	struct r6040_private *lp = netdev_priv(dev);
 	void __iomem *ioaddr = lp->base;
-	u16 *adrp;
-	u16 reg;
 	unsigned long flags;
 	struct netdev_hw_addr *ha;
 	int i;
+	u16 *adrp;
+	u16 hash_table[4] = { 0 };
+
+	spin_lock_irqsave(&lp->lock, flags);
 
-	/* MAC Address */
+	/* Keep our MAC Address */
 	adrp = (u16 *)dev->dev_addr;
 	iowrite16(adrp[0], ioaddr + MID_0L);
 	iowrite16(adrp[1], ioaddr + MID_0M);
 	iowrite16(adrp[2], ioaddr + MID_0H);
 
-	/* Promiscous Mode */
-	spin_lock_irqsave(&lp->lock, flags);
-
 	/* Clear AMCP & PROM bits */
-	reg = ioread16(ioaddr) & ~0x0120;
-	if (dev->flags & IFF_PROMISC) {
-		reg |= 0x0020;
-		lp->mcr0 |= 0x0020;
-	}
-	/* Too many multicast addresses
-	 * accept all traffic */
-	else if ((netdev_mc_count(dev) > MCAST_MAX) ||
-		 (dev->flags & IFF_ALLMULTI))
-		reg |= 0x0020;
+	lp->mcr0 = ioread16(ioaddr + MCR0) & ~(MCR0_PROMISC | MCR0_HASH_EN);
 
-	iowrite16(reg, ioaddr);
-	spin_unlock_irqrestore(&lp->lock, flags);
+	/* Promiscuous mode */
+	if (dev->flags & IFF_PROMISC)
+		lp->mcr0 |= MCR0_PROMISC;
 
-	/* Build the hash table */
-	if (netdev_mc_count(dev) > MCAST_MAX) {
-		u16 hash_table[4];
-		u32 crc;
+	/* Enable multicast hash table function to
+	 * receive all multicast packets. */
+	else if (dev->flags & IFF_ALLMULTI) {
+		lp->mcr0 |= MCR0_HASH_EN;
 
-		for (i = 0; i < 4; i++)
-			hash_table[i] = 0;
+		for (i = 0; i < MCAST_MAX ; i++) {
+			iowrite16(0, ioaddr + MID_1L + 8 * i);
+			iowrite16(0, ioaddr + MID_1M + 8 * i);
+			iowrite16(0, ioaddr + MID_1H + 8 * i);
+		}
 
+		for (i = 0; i < 4; i++)
+			hash_table[i] = 0xffff;
+	}
+	/* Use internal multicast address registers if the number of
+	 * multicast addresses is not greater than MCAST_MAX. */
+	else if (netdev_mc_count(dev) <= MCAST_MAX) {
+		i = 0;
 		netdev_for_each_mc_addr(ha, dev) {
-			char *addrs = ha->addr;
+			u16 *adrp = (u16 *) ha->addr;
+			iowrite16(adrp[0], ioaddr + MID_1L + 8 * i);
+			iowrite16(adrp[1], ioaddr + MID_1M + 8 * i);
+			iowrite16(adrp[2], ioaddr + MID_1H + 8 * i);
+			i++;
+		}
+		while (i < MCAST_MAX) {
+			iowrite16(0, ioaddr + MID_1L + 8 * i);
+			iowrite16(0, ioaddr + MID_1M + 8 * i);
+			iowrite16(0, ioaddr + MID_1H + 8 * i);
+			i++;
+		}
+	}
+	/* Otherwise, Enable multicast hash table function. */
+	else {
+		u32 crc;
 
-			if (!(*addrs & 1))
-				continue;
+		lp->mcr0 |= MCR0_HASH_EN;
+
+		for (i = 0; i < MCAST_MAX ; i++) {
+			iowrite16(0, ioaddr + MID_1L + 8 * i);
+			iowrite16(0, ioaddr + MID_1M + 8 * i);
+			iowrite16(0, ioaddr + MID_1H + 8 * i);
+		}
 
-			crc = ether_crc_le(6, addrs);
+		/* Build multicast hash table */
+		netdev_for_each_mc_addr(ha, dev) {
+			u8 *addrs = ha->addr;
+
+			crc = ether_crc(ETH_ALEN, addrs);
 			crc >>= 26;
-			hash_table[crc >> 4] |= 1 << (15 - (crc & 0xf));
+			hash_table[crc >> 4] |= 1 << (crc & 0xf);
 		}
-		/* Fill the MAC hash tables with their values */
+	}
+
+	iowrite16(lp->mcr0, ioaddr + MCR0);
+
+	/* Fill the MAC hash tables with their values */
+	if (lp->mcr0 && MCR0_HASH_EN) {
 		iowrite16(hash_table[0], ioaddr + MAR0);
 		iowrite16(hash_table[1], ioaddr + MAR1);
 		iowrite16(hash_table[2], ioaddr + MAR2);
 		iowrite16(hash_table[3], ioaddr + MAR3);
 	}
-	/* Multicast Address 1~4 case */
-	i = 0;
-	netdev_for_each_mc_addr(ha, dev) {
-		if (i >= MCAST_MAX)
-			break;
-		adrp = (u16 *) ha->addr;
-		iowrite16(adrp[0], ioaddr + MID_1L + 8 * i);
-		iowrite16(adrp[1], ioaddr + MID_1M + 8 * i);
-		iowrite16(adrp[2], ioaddr + MID_1H + 8 * i);
-		i++;
-	}
-	while (i < MCAST_MAX) {
-		iowrite16(0xffff, ioaddr + MID_1L + 8 * i);
-		iowrite16(0xffff, ioaddr + MID_1M + 8 * i);
-		iowrite16(0xffff, ioaddr + MID_1H + 8 * i);
-		i++;
-	}
+
+	spin_unlock_irqrestore(&lp->lock, flags);
 }
 
 static void netdev_get_drvinfo(struct net_device *dev,
diff --git a/drivers/net/r8169.c b/drivers/net/r8169.c
index 469ab0b7ce31..5e403511289d 100644
--- a/drivers/net/r8169.c
+++ b/drivers/net/r8169.c
@@ -25,6 +25,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/pm_runtime.h>
 #include <linux/firmware.h>
+#include <linux/pci-aspm.h>
 
 #include <asm/system.h>
 #include <asm/io.h>
@@ -36,6 +37,7 @@
 
 #define FIRMWARE_8168D_1	"rtl_nic/rtl8168d-1.fw"
 #define FIRMWARE_8168D_2	"rtl_nic/rtl8168d-2.fw"
+#define FIRMWARE_8105E_1	"rtl_nic/rtl8105e-1.fw"
 
 #ifdef RTL8169_DEBUG
 #define assert(expr) \
@@ -123,6 +125,8 @@ enum mac_version {
 	RTL_GIGA_MAC_VER_26 = 0x1a, // 8168D
 	RTL_GIGA_MAC_VER_27 = 0x1b, // 8168DP
 	RTL_GIGA_MAC_VER_28 = 0x1c, // 8168DP
+	RTL_GIGA_MAC_VER_29 = 0x1d, // 8105E
+	RTL_GIGA_MAC_VER_30 = 0x1e, // 8105E
 };
 
 #define _R(NAME,MAC,MASK) \
@@ -160,7 +164,9 @@ static const struct {
 	_R("RTL8168d/8111d",	RTL_GIGA_MAC_VER_25, 0xff7e1880), // PCI-E
 	_R("RTL8168d/8111d",	RTL_GIGA_MAC_VER_26, 0xff7e1880), // PCI-E
 	_R("RTL8168dp/8111dp",	RTL_GIGA_MAC_VER_27, 0xff7e1880), // PCI-E
-	_R("RTL8168dp/8111dp",	RTL_GIGA_MAC_VER_28, 0xff7e1880)  // PCI-E
+	_R("RTL8168dp/8111dp",	RTL_GIGA_MAC_VER_28, 0xff7e1880), // PCI-E
+	_R("RTL8105e",		RTL_GIGA_MAC_VER_29, 0xff7e1880), // PCI-E
+	_R("RTL8105e",		RTL_GIGA_MAC_VER_30, 0xff7e1880)  // PCI-E
 };
 #undef _R
 
@@ -267,9 +273,15 @@ enum rtl8168_8101_registers {
 #define	EPHYAR_REG_MASK			0x1f
 #define	EPHYAR_REG_SHIFT		16
 #define	EPHYAR_DATA_MASK		0xffff
+	DLLPR			= 0xd0,
+#define	PM_SWITCH			(1 << 6)
 	DBG_REG			= 0xd1,
 #define	FIX_NAK_1			(1 << 4)
 #define	FIX_NAK_2			(1 << 3)
+	TWSI			= 0xd2,
+	MCU			= 0xd3,
+#define	EN_NDP				(1 << 3)
+#define	EN_OOB_RESET			(1 << 2)
 	EFUSEAR			= 0xdc,
 #define	EFUSEAR_FLAG			0x80000000
 #define	EFUSEAR_WRITE_CMD		0x80000000
@@ -526,9 +538,6 @@ struct rtl8169_private {
 	u16 napi_event;
 	u16 intr_mask;
 	int phy_1000_ctrl_reg;
-#ifdef CONFIG_R8169_VLAN
-	struct vlan_group *vlgrp;
-#endif
 
 	struct mdio_ops {
 		void (*write)(void __iomem *, int, int);
@@ -540,7 +549,7 @@ struct rtl8169_private {
 		void (*up)(struct rtl8169_private *);
 	} pll_power_ops;
 
-	int (*set_speed)(struct net_device *, u8 autoneg, u16 speed, u8 duplex);
+	int (*set_speed)(struct net_device *, u8 aneg, u16 sp, u8 dpx, u32 adv);
 	int (*get_settings)(struct net_device *, struct ethtool_cmd *);
 	void (*phy_reset_enable)(struct rtl8169_private *tp);
 	void (*hw_start)(struct net_device *);
@@ -568,6 +577,7 @@ MODULE_LICENSE("GPL");
 MODULE_VERSION(RTL8169_VERSION);
 MODULE_FIRMWARE(FIRMWARE_8168D_1);
 MODULE_FIRMWARE(FIRMWARE_8168D_2);
+MODULE_FIRMWARE(FIRMWARE_8105E_1);
 
 static int rtl8169_open(struct net_device *dev);
 static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
@@ -617,8 +627,9 @@ static void ocp_write(struct rtl8169_private *tp, u8 mask, u16 reg, u32 data)
 	}
 }
 
-static void rtl8168_oob_notify(void __iomem *ioaddr, u8 cmd)
+static void rtl8168_oob_notify(struct rtl8169_private *tp, u8 cmd)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
 	int i;
 
 	RTL_W8(ERIDR, cmd);
@@ -630,7 +641,7 @@ static void rtl8168_oob_notify(void __iomem *ioaddr, u8 cmd)
 			break;
 	}
 
-	ocp_write(ioaddr, 0x1, 0x30, 0x00000001);
+	ocp_write(tp, 0x1, 0x30, 0x00000001);
 }
 
 #define OOB_CMD_RESET		0x00
@@ -1096,7 +1107,7 @@ static int rtl8169_get_regs_len(struct net_device *dev)
 }
 
 static int rtl8169_set_speed_tbi(struct net_device *dev,
-				 u8 autoneg, u16 speed, u8 duplex)
+				 u8 autoneg, u16 speed, u8 duplex, u32 ignored)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
@@ -1119,17 +1130,30 @@ static int rtl8169_set_speed_tbi(struct net_device *dev,
 }
 
 static int rtl8169_set_speed_xmii(struct net_device *dev,
-				  u8 autoneg, u16 speed, u8 duplex)
+				  u8 autoneg, u16 speed, u8 duplex, u32 adv)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	int giga_ctrl, bmcr;
+	int rc = -EINVAL;
+
+	rtl_writephy(tp, 0x1f, 0x0000);
 
 	if (autoneg == AUTONEG_ENABLE) {
 		int auto_nego;
 
 		auto_nego = rtl_readphy(tp, MII_ADVERTISE);
-		auto_nego |= (ADVERTISE_10HALF | ADVERTISE_10FULL |
-			      ADVERTISE_100HALF | ADVERTISE_100FULL);
+		auto_nego &= ~(ADVERTISE_10HALF | ADVERTISE_10FULL |
+				ADVERTISE_100HALF | ADVERTISE_100FULL);
+
+		if (adv & ADVERTISED_10baseT_Half)
+			auto_nego |= ADVERTISE_10HALF;
+		if (adv & ADVERTISED_10baseT_Full)
+			auto_nego |= ADVERTISE_10FULL;
+		if (adv & ADVERTISED_100baseT_Half)
+			auto_nego |= ADVERTISE_100HALF;
+		if (adv & ADVERTISED_100baseT_Full)
+			auto_nego |= ADVERTISE_100FULL;
+
 		auto_nego |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
 
 		giga_ctrl = rtl_readphy(tp, MII_CTRL1000);
@@ -1143,27 +1167,22 @@ static int rtl8169_set_speed_xmii(struct net_device *dev,
 		    (tp->mac_version != RTL_GIGA_MAC_VER_13) &&
 		    (tp->mac_version != RTL_GIGA_MAC_VER_14) &&
 		    (tp->mac_version != RTL_GIGA_MAC_VER_15) &&
-		    (tp->mac_version != RTL_GIGA_MAC_VER_16)) {
-			giga_ctrl |= ADVERTISE_1000FULL | ADVERTISE_1000HALF;
-		} else {
+		    (tp->mac_version != RTL_GIGA_MAC_VER_16) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_29) &&
+		    (tp->mac_version != RTL_GIGA_MAC_VER_30)) {
+			if (adv & ADVERTISED_1000baseT_Half)
+				giga_ctrl |= ADVERTISE_1000HALF;
+			if (adv & ADVERTISED_1000baseT_Full)
+				giga_ctrl |= ADVERTISE_1000FULL;
+		} else if (adv & (ADVERTISED_1000baseT_Half |
+				  ADVERTISED_1000baseT_Full)) {
 			netif_info(tp, link, dev,
 				   "PHY does not support 1000Mbps\n");
+			goto out;
 		}
 
 		bmcr = BMCR_ANENABLE | BMCR_ANRESTART;
 
-		if ((tp->mac_version == RTL_GIGA_MAC_VER_11) ||
-		    (tp->mac_version == RTL_GIGA_MAC_VER_12) ||
-		    (tp->mac_version >= RTL_GIGA_MAC_VER_17)) {
-			/*
-			 * Wake up the PHY.
-			 * Vendor specific (0x1f) and reserved (0x0e) MII
-			 * registers.
-			 */
-			rtl_writephy(tp, 0x1f, 0x0000);
-			rtl_writephy(tp, 0x0e, 0x0000);
-		}
-
 		rtl_writephy(tp, MII_ADVERTISE, auto_nego);
 		rtl_writephy(tp, MII_CTRL1000, giga_ctrl);
 	} else {
@@ -1174,12 +1193,10 @@ static int rtl8169_set_speed_xmii(struct net_device *dev,
 		else if (speed == SPEED_100)
 			bmcr = BMCR_SPEED100;
 		else
-			return -EINVAL;
+			goto out;
 
 		if (duplex == DUPLEX_FULL)
 			bmcr |= BMCR_FULLDPLX;
-
-		rtl_writephy(tp, 0x1f, 0x0000);
 	}
 
 	tp->phy_1000_ctrl_reg = giga_ctrl;
@@ -1197,16 +1214,18 @@ static int rtl8169_set_speed_xmii(struct net_device *dev,
 		}
 	}
 
-	return 0;
+	rc = 0;
+out:
+	return rc;
 }
 
 static int rtl8169_set_speed(struct net_device *dev,
-			     u8 autoneg, u16 speed, u8 duplex)
+			     u8 autoneg, u16 speed, u8 duplex, u32 advertising)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	int ret;
 
-	ret = tp->set_speed(dev, autoneg, speed, duplex);
+	ret = tp->set_speed(dev, autoneg, speed, duplex, advertising);
 
 	if (netif_running(dev) && (tp->phy_1000_ctrl_reg & ADVERTISE_1000FULL))
 		mod_timer(&tp->timer, jiffies + RTL8169_PHY_TIMEOUT);
@@ -1221,7 +1240,8 @@ static int rtl8169_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 	int ret;
 
 	spin_lock_irqsave(&tp->lock, flags);
-	ret = rtl8169_set_speed(dev, cmd->autoneg, cmd->speed, cmd->duplex);
+	ret = rtl8169_set_speed(dev,
+		cmd->autoneg, cmd->speed, cmd->duplex, cmd->advertising);
 	spin_unlock_irqrestore(&tp->lock, flags);
 
 	return ret;
@@ -1255,8 +1275,6 @@ static int rtl8169_set_rx_csum(struct net_device *dev, u32 data)
 	return 0;
 }
 
-#ifdef CONFIG_R8169_VLAN
-
 static inline u32 rtl8169_tx_vlan_tag(struct rtl8169_private *tp,
 				      struct sk_buff *skb)
 {
@@ -1264,64 +1282,37 @@ static inline u32 rtl8169_tx_vlan_tag(struct rtl8169_private *tp,
 		TxVlanTag | swab16(vlan_tx_tag_get(skb)) : 0x00;
 }
 
-static void rtl8169_vlan_rx_register(struct net_device *dev,
-				     struct vlan_group *grp)
+#define NETIF_F_HW_VLAN_TX_RX	(NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX)
+
+static void rtl8169_vlan_mode(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
 	unsigned long flags;
 
 	spin_lock_irqsave(&tp->lock, flags);
-	tp->vlgrp = grp;
-	/*
-	 * Do not disable RxVlan on 8110SCd.
-	 */
-	if (tp->vlgrp || (tp->mac_version == RTL_GIGA_MAC_VER_05))
+	if (dev->features & NETIF_F_HW_VLAN_RX)
 		tp->cp_cmd |= RxVlan;
 	else
 		tp->cp_cmd &= ~RxVlan;
 	RTL_W16(CPlusCmd, tp->cp_cmd);
+	/* PCI commit */
 	RTL_R16(CPlusCmd);
 	spin_unlock_irqrestore(&tp->lock, flags);
+
+	dev->vlan_features = dev->features &~ NETIF_F_HW_VLAN_TX_RX;
 }
 
-static int rtl8169_rx_vlan_skb(struct rtl8169_private *tp, struct RxDesc *desc,
-			       struct sk_buff *skb, int polling)
+static void rtl8169_rx_vlan_tag(struct RxDesc *desc, struct sk_buff *skb)
 {
 	u32 opts2 = le32_to_cpu(desc->opts2);
-	struct vlan_group *vlgrp = tp->vlgrp;
-	int ret;
 
-	if (vlgrp && (opts2 & RxVlanTag)) {
-		u16 vtag = swab16(opts2 & 0xffff);
+	if (opts2 & RxVlanTag)
+		__vlan_hwaccel_put_tag(skb, swab16(opts2 & 0xffff));
 
-		if (likely(polling))
-			vlan_gro_receive(&tp->napi, vlgrp, vtag, skb);
-		else
-			__vlan_hwaccel_rx(skb, vlgrp, vtag, polling);
-		ret = 0;
-	} else
-		ret = -1;
 	desc->opts2 = 0;
-	return ret;
-}
-
-#else /* !CONFIG_R8169_VLAN */
-
-static inline u32 rtl8169_tx_vlan_tag(struct rtl8169_private *tp,
-				      struct sk_buff *skb)
-{
-	return 0;
-}
-
-static int rtl8169_rx_vlan_skb(struct rtl8169_private *tp, struct RxDesc *desc,
-			       struct sk_buff *skb, int polling)
-{
-	return -1;
 }
 
-#endif
-
 static int rtl8169_gset_tbi(struct net_device *dev, struct ethtool_cmd *cmd)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -1492,6 +1483,28 @@ static void rtl8169_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 	}
 }
 
+static int rtl8169_set_flags(struct net_device *dev, u32 data)
+{
+	struct rtl8169_private *tp = netdev_priv(dev);
+	unsigned long old_feat = dev->features;
+	int rc;
+
+	if ((tp->mac_version == RTL_GIGA_MAC_VER_05) &&
+	    !(data & ETH_FLAG_RXVLAN)) {
+		netif_info(tp, drv, dev, "8110SCd requires hardware Rx VLAN\n");
+		return -EINVAL;
+	}
+
+	rc = ethtool_op_set_flags(dev, data, ETH_FLAG_TXVLAN | ETH_FLAG_RXVLAN);
+	if (rc)
+		return rc;
+
+	if ((old_feat ^ dev->features) & NETIF_F_HW_VLAN_RX)
+		rtl8169_vlan_mode(dev);
+
+	return 0;
+}
+
 static const struct ethtool_ops rtl8169_ethtool_ops = {
 	.get_drvinfo		= rtl8169_get_drvinfo,
 	.get_regs_len		= rtl8169_get_regs_len,
@@ -1511,6 +1524,8 @@ static const struct ethtool_ops rtl8169_ethtool_ops = {
 	.get_strings		= rtl8169_get_strings,
 	.get_sset_count		= rtl8169_get_sset_count,
 	.get_ethtool_stats	= rtl8169_get_ethtool_stats,
+	.set_flags		= rtl8169_set_flags,
+	.get_flags		= ethtool_op_get_flags,
 };
 
 static void rtl8169_get_mac_version(struct rtl8169_private *tp,
@@ -1559,6 +1574,9 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		{ 0x7c800000, 0x30000000,	RTL_GIGA_MAC_VER_11 },
 
 		/* 8101 family. */
+		{ 0x7cf00000, 0x40a00000,	RTL_GIGA_MAC_VER_30 },
+		{ 0x7cf00000, 0x40900000,	RTL_GIGA_MAC_VER_29 },
+		{ 0x7c800000, 0x40800000,	RTL_GIGA_MAC_VER_30 },
 		{ 0x7cf00000, 0x34a00000,	RTL_GIGA_MAC_VER_09 },
 		{ 0x7cf00000, 0x24a00000,	RTL_GIGA_MAC_VER_09 },
 		{ 0x7cf00000, 0x34900000,	RTL_GIGA_MAC_VER_08 },
@@ -2435,6 +2453,33 @@ static void rtl8102e_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
 }
 
+static void rtl8105e_hw_phy_config(struct rtl8169_private *tp)
+{
+	static const struct phy_reg phy_reg_init[] = {
+		{ 0x1f, 0x0005 },
+		{ 0x1a, 0x0000 },
+		{ 0x1f, 0x0000 },
+
+		{ 0x1f, 0x0004 },
+		{ 0x1c, 0x0000 },
+		{ 0x1f, 0x0000 },
+
+		{ 0x1f, 0x0001 },
+		{ 0x15, 0x7701 },
+		{ 0x1f, 0x0000 }
+	};
+
+	/* Disable ALDPS before ram code */
+	rtl_writephy(tp, 0x1f, 0x0000);
+	rtl_writephy(tp, 0x18, 0x0310);
+	msleep(100);
+
+	if (rtl_apply_firmware(tp, FIRMWARE_8105E_1) < 0)
+		netif_warn(tp, probe, tp->dev, "unable to apply firmware patch\n");
+
+	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+}
+
 static void rtl_hw_phy_config(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -2502,6 +2547,10 @@ static void rtl_hw_phy_config(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_28:
 		rtl8168d_4_hw_phy_config(tp);
 		break;
+	case RTL_GIGA_MAC_VER_29:
+	case RTL_GIGA_MAC_VER_30:
+		rtl8105e_hw_phy_config(tp);
+		break;
 
 	default:
 		break;
@@ -2633,11 +2682,12 @@ static void rtl8169_init_phy(struct net_device *dev, struct rtl8169_private *tp)
 
 	rtl8169_phy_reset(dev, tp);
 
-	/*
-	 * rtl8169_set_speed_xmii takes good care of the Fast Ethernet
-	 * only 8101. Don't panic.
-	 */
-	rtl8169_set_speed(dev, AUTONEG_ENABLE, SPEED_1000, DUPLEX_FULL);
+	rtl8169_set_speed(dev, AUTONEG_ENABLE, SPEED_1000, DUPLEX_FULL,
+		ADVERTISED_10baseT_Half | ADVERTISED_10baseT_Full |
+		ADVERTISED_100baseT_Half | ADVERTISED_100baseT_Full |
+		tp->mii.supports_gmii ?
+			ADVERTISED_1000baseT_Half |
+			ADVERTISED_1000baseT_Full : 0);
 
 	if (RTL_R8(PHYstatus) & TBI_Enable)
 		netif_info(tp, link, dev, "TBI auto-negotiating\n");
@@ -2793,9 +2843,6 @@ static const struct net_device_ops rtl8169_netdev_ops = {
 	.ndo_set_mac_address	= rtl_set_mac_address,
 	.ndo_do_ioctl		= rtl8169_ioctl,
 	.ndo_set_multicast_list	= rtl_set_rx_mode,
-#ifdef CONFIG_R8169_VLAN
-	.ndo_vlan_rx_register	= rtl8169_vlan_rx_register,
-#endif
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= rtl8169_netpoll,
 #endif
@@ -2868,8 +2915,11 @@ static void r8168_pll_power_down(struct rtl8169_private *tp)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
 
-	if (tp->mac_version == RTL_GIGA_MAC_VER_27)
+	if (((tp->mac_version == RTL_GIGA_MAC_VER_27) ||
+	     (tp->mac_version == RTL_GIGA_MAC_VER_28)) &&
+	    (ocp_read(tp, 0x0f, 0x0010) & 0x00008000)) {
 		return;
+	}
 
 	if (((tp->mac_version == RTL_GIGA_MAC_VER_23) ||
 	     (tp->mac_version == RTL_GIGA_MAC_VER_24)) &&
@@ -2891,6 +2941,8 @@ static void r8168_pll_power_down(struct rtl8169_private *tp)
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_25:
 	case RTL_GIGA_MAC_VER_26:
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
 		RTL_W8(PMCH, RTL_R8(PMCH) & ~0x80);
 		break;
 	}
@@ -2900,12 +2952,17 @@ static void r8168_pll_power_up(struct rtl8169_private *tp)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
 
-	if (tp->mac_version == RTL_GIGA_MAC_VER_27)
+	if (((tp->mac_version == RTL_GIGA_MAC_VER_27) ||
+	     (tp->mac_version == RTL_GIGA_MAC_VER_28)) &&
+	    (ocp_read(tp, 0x0f, 0x0010) & 0x00008000)) {
 		return;
+	}
 
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_25:
 	case RTL_GIGA_MAC_VER_26:
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
 		RTL_W8(PMCH, RTL_R8(PMCH) | 0x80);
 		break;
 	}
@@ -2940,6 +2997,8 @@ static void __devinit rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_09:
 	case RTL_GIGA_MAC_VER_10:
 	case RTL_GIGA_MAC_VER_16:
+	case RTL_GIGA_MAC_VER_29:
+	case RTL_GIGA_MAC_VER_30:
 		ops->down	= r810x_pll_power_down;
 		ops->up		= r810x_pll_power_up;
 		break;
@@ -3009,6 +3068,11 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	mii->reg_num_mask = 0x1f;
 	mii->supports_gmii = !!(cfg->features & RTL_FEATURE_GMII);
 
+	/* disable ASPM completely as that cause random device stop working
+	 * problems as well as full system hangs for some PCIe devices users */
+	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1 |
+				     PCIE_LINK_STATE_CLKPM);
+
 	/* enable device (incl. PCI PM wakeup and hotplug setup) */
 	rc = pci_enable_device(pdev);
 	if (rc < 0) {
@@ -3042,7 +3106,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_out_mwi_2;
 	}
 
-	tp->cp_cmd = PCIMulRW | RxChkSum;
+	tp->cp_cmd = RxChkSum;
 
 	if ((sizeof(dma_addr_t) > 4) &&
 	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) && use_dac) {
@@ -3087,6 +3151,13 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* Identify chip attached to board */
 	rtl8169_get_mac_version(tp, ioaddr);
 
+	/*
+	 * Pretend we are using VLANs; This bypasses a nasty bug where
+	 * Interrupts stop flowing on high load on 8110SCd controllers.
+	 */
+	if (tp->mac_version == RTL_GIGA_MAC_VER_05)
+		tp->cp_cmd |= RxVlan;
+
 	rtl_init_mdio_ops(tp);
 	rtl_init_pll_power_ops(tp);
 
@@ -3155,10 +3226,7 @@ rtl8169_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	netif_napi_add(dev, &tp->napi, rtl8169_poll, R8169_NAPI_WEIGHT);
 
-#ifdef CONFIG_R8169_VLAN
-	dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
-#endif
-	dev->features |= NETIF_F_GRO;
+	dev->features |= NETIF_F_HW_VLAN_TX_RX | NETIF_F_GRO;
 
 	tp->intr_mask = 0xffff;
 	tp->hw_start = cfg->hw_start;
@@ -3276,12 +3344,7 @@ static int rtl8169_open(struct net_device *dev)
 
 	rtl8169_init_phy(dev, tp);
 
-	/*
-	 * Pretend we are using VLANs; This bypasses a nasty bug where
-	 * Interrupts stop flowing on high load on 8110SCd controllers.
-	 */
-	if (tp->mac_version == RTL_GIGA_MAC_VER_05)
-		RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) | RxVlan);
+	rtl8169_vlan_mode(dev);
 
 	rtl_pll_power_up(tp);
 
@@ -3318,7 +3381,8 @@ static void rtl8169_hw_reset(struct rtl8169_private *tp)
 	/* Disable interrupts */
 	rtl8169_irq_mask_and_ack(ioaddr);
 
-	if (tp->mac_version == RTL_GIGA_MAC_VER_28) {
+	if (tp->mac_version == RTL_GIGA_MAC_VER_27 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_28) {
 		while (RTL_R8(TxPoll) & NPQ)
 			udelay(20);
 
@@ -3847,8 +3911,7 @@ static void rtl_hw_start_8168(struct net_device *dev)
 	Cxpl_dbg_sel | \
 	ASF | \
 	PktCntrDisable | \
-	PCIDAC | \
-	PCIMulRW)
+	Mac_dbgo_sel)
 
 static void rtl_hw_start_8102e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 {
@@ -3878,8 +3941,6 @@ static void rtl_hw_start_8102e_1(void __iomem *ioaddr, struct pci_dev *pdev)
 	if ((cfg1 & LEDS0) && (cfg1 & LEDS1))
 		RTL_W8(Config1, cfg1 & ~LEDS0);
 
-	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R810X_CPCMD_QUIRK_MASK);
-
 	rtl_ephy_init(ioaddr, e_info_8102e_1, ARRAY_SIZE(e_info_8102e_1));
 }
 
@@ -3891,8 +3952,6 @@ static void rtl_hw_start_8102e_2(void __iomem *ioaddr, struct pci_dev *pdev)
 
 	RTL_W8(Config1, MEMMAP | IOMAP | VPD | PMEnable);
 	RTL_W8(Config3, RTL_R8(Config3) & ~Beacon_en);
-
-	RTL_W16(CPlusCmd, RTL_R16(CPlusCmd) & ~R810X_CPCMD_QUIRK_MASK);
 }
 
 static void rtl_hw_start_8102e_3(void __iomem *ioaddr, struct pci_dev *pdev)
@@ -3902,6 +3961,37 @@ static void rtl_hw_start_8102e_3(void __iomem *ioaddr, struct pci_dev *pdev)
 	rtl_ephy_write(ioaddr, 0x03, 0xc2f9);
 }
 
+static void rtl_hw_start_8105e_1(void __iomem *ioaddr, struct pci_dev *pdev)
+{
+	static const struct ephy_info e_info_8105e_1[] = {
+		{ 0x07,	0, 0x4000 },
+		{ 0x19,	0, 0x0200 },
+		{ 0x19,	0, 0x0020 },
+		{ 0x1e,	0, 0x2000 },
+		{ 0x03,	0, 0x0001 },
+		{ 0x19,	0, 0x0100 },
+		{ 0x19,	0, 0x0004 },
+		{ 0x0a,	0, 0x0020 }
+	};
+
+	/* Force LAN exit from ASPM if Rx/Tx are not idel */
+	RTL_W32(FuncEvent, RTL_R32(FuncEvent) | 0x002800);
+
+	/* disable Early Tally Counter */
+	RTL_W32(FuncEvent, RTL_R32(FuncEvent) & ~0x010000);
+
+	RTL_W8(MCU, RTL_R8(MCU) | EN_NDP | EN_OOB_RESET);
+	RTL_W8(DLLPR, RTL_R8(DLLPR) | PM_SWITCH);
+
+	rtl_ephy_init(ioaddr, e_info_8105e_1, ARRAY_SIZE(e_info_8105e_1));
+}
+
+static void rtl_hw_start_8105e_2(void __iomem *ioaddr, struct pci_dev *pdev)
+{
+	rtl_hw_start_8105e_1(ioaddr, pdev);
+	rtl_ephy_write(ioaddr, 0x1e, rtl_ephy_read(ioaddr, 0x1e) | 0x8000);
+}
+
 static void rtl_hw_start_8101(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -3918,6 +4008,8 @@ static void rtl_hw_start_8101(struct net_device *dev)
 		}
 	}
 
+	RTL_W8(Cfg9346, Cfg9346_Unlock);
+
 	switch (tp->mac_version) {
 	case RTL_GIGA_MAC_VER_07:
 		rtl_hw_start_8102e_1(ioaddr, pdev);
@@ -3930,16 +4022,22 @@ static void rtl_hw_start_8101(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_09:
 		rtl_hw_start_8102e_2(ioaddr, pdev);
 		break;
+
+	case RTL_GIGA_MAC_VER_29:
+		rtl_hw_start_8105e_1(ioaddr, pdev);
+		break;
+	case RTL_GIGA_MAC_VER_30:
+		rtl_hw_start_8105e_2(ioaddr, pdev);
+		break;
 	}
 
-	RTL_W8(Cfg9346, Cfg9346_Unlock);
+	RTL_W8(Cfg9346, Cfg9346_Lock);
 
 	RTL_W8(MaxTxPacketSize, TxPacketMax);
 
 	rtl_set_rx_max_size(ioaddr, rx_buf_sz);
 
-	tp->cp_cmd |= rtl_rw_cpluscmd(ioaddr) | PCIMulRW;
-
+	tp->cp_cmd &= ~R810X_CPCMD_QUIRK_MASK;
 	RTL_W16(CPlusCmd, tp->cp_cmd);
 
 	RTL_W16(IntrMitigate, 0x0000);
@@ -3949,14 +4047,10 @@ static void rtl_hw_start_8101(struct net_device *dev)
 	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
 	rtl_set_rx_tx_config_registers(tp);
 
-	RTL_W8(Cfg9346, Cfg9346_Lock);
-
 	RTL_R8(IntrMask);
 
 	rtl_set_rx_mode(dev);
 
-	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
-
 	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xf000);
 
 	RTL_W16(IntrMask, tp->intr_event);
@@ -4593,12 +4687,12 @@ static int rtl8169_rx_interrupt(struct net_device *dev,
 			skb_put(skb, pkt_size);
 			skb->protocol = eth_type_trans(skb, dev);
 
-			if (rtl8169_rx_vlan_skb(tp, desc, skb, polling) < 0) {
-				if (likely(polling))
-					napi_gro_receive(&tp->napi, skb);
-				else
-					netif_rx(skb);
-			}
+			rtl8169_rx_vlan_tag(desc, skb);
+
+			if (likely(polling))
+				napi_gro_receive(&tp->napi, skb);
+			else
+				netif_rx(skb);
 
 			dev->stats.rx_bytes += pkt_size;
 			dev->stats.rx_packets++;
diff --git a/drivers/net/sfc/efx.c b/drivers/net/sfc/efx.c
index 002bac743843..d563049859a8 100644
--- a/drivers/net/sfc/efx.c
+++ b/drivers/net/sfc/efx.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2005-2009 Solarflare Communications Inc.
+ * Copyright 2005-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -21,6 +21,7 @@
 #include <linux/ethtool.h>
 #include <linux/topology.h>
 #include <linux/gfp.h>
+#include <linux/cpu_rmap.h>
 #include "net_driver.h"
 #include "efx.h"
 #include "nic.h"
@@ -307,6 +308,8 @@ static int efx_poll(struct napi_struct *napi, int budget)
 			channel->irq_mod_score = 0;
 		}
 
+		efx_filter_rfs_expire(channel);
+
 		/* There is no race here; although napi_disable() will
 		 * only wait for napi_complete(), this isn't a problem
 		 * since efx_channel_processed() will have no effect if
@@ -673,7 +676,7 @@ static void efx_fini_channels(struct efx_nic *efx)
 
 		efx_for_each_channel_rx_queue(rx_queue, channel)
 			efx_fini_rx_queue(rx_queue);
-		efx_for_each_channel_tx_queue(tx_queue, channel)
+		efx_for_each_possible_channel_tx_queue(tx_queue, channel)
 			efx_fini_tx_queue(tx_queue);
 		efx_fini_eventq(channel);
 	}
@@ -689,7 +692,7 @@ static void efx_remove_channel(struct efx_channel *channel)
 
 	efx_for_each_channel_rx_queue(rx_queue, channel)
 		efx_remove_rx_queue(rx_queue);
-	efx_for_each_channel_tx_queue(tx_queue, channel)
+	efx_for_each_possible_channel_tx_queue(tx_queue, channel)
 		efx_remove_tx_queue(tx_queue);
 	efx_remove_eventq(channel);
 }
@@ -1175,10 +1178,32 @@ static int efx_wanted_channels(void)
 	return count;
 }
 
+static int
+efx_init_rx_cpu_rmap(struct efx_nic *efx, struct msix_entry *xentries)
+{
+#ifdef CONFIG_RFS_ACCEL
+	int i, rc;
+
+	efx->net_dev->rx_cpu_rmap = alloc_irq_cpu_rmap(efx->n_rx_channels);
+	if (!efx->net_dev->rx_cpu_rmap)
+		return -ENOMEM;
+	for (i = 0; i < efx->n_rx_channels; i++) {
+		rc = irq_cpu_rmap_add(efx->net_dev->rx_cpu_rmap,
+				      xentries[i].vector);
+		if (rc) {
+			free_irq_cpu_rmap(efx->net_dev->rx_cpu_rmap);
+			efx->net_dev->rx_cpu_rmap = NULL;
+			return rc;
+		}
+	}
+#endif
+	return 0;
+}
+
 /* Probe the number and type of interrupts we are able to obtain, and
  * the resulting numbers of channels and RX queues.
  */
-static void efx_probe_interrupts(struct efx_nic *efx)
+static int efx_probe_interrupts(struct efx_nic *efx)
 {
 	int max_channels =
 		min_t(int, efx->type->phys_addr_channels, EFX_MAX_CHANNELS);
@@ -1220,6 +1245,11 @@ static void efx_probe_interrupts(struct efx_nic *efx)
 				efx->n_tx_channels = efx->n_channels;
 				efx->n_rx_channels = efx->n_channels;
 			}
+			rc = efx_init_rx_cpu_rmap(efx, xentries);
+			if (rc) {
+				pci_disable_msix(efx->pci_dev);
+				return rc;
+			}
 			for (i = 0; i < n_channels; i++)
 				efx_get_channel(efx, i)->irq =
 					xentries[i].vector;
@@ -1253,6 +1283,8 @@ static void efx_probe_interrupts(struct efx_nic *efx)
 		efx->n_tx_channels = 1;
 		efx->legacy_irq = efx->pci_dev->irq;
 	}
+
+	return 0;
 }
 
 static void efx_remove_interrupts(struct efx_nic *efx)
@@ -1271,21 +1303,8 @@ static void efx_remove_interrupts(struct efx_nic *efx)
 
 static void efx_set_channels(struct efx_nic *efx)
 {
-	struct efx_channel *channel;
-	struct efx_tx_queue *tx_queue;
-
 	efx->tx_channel_offset =
 		separate_tx_channels ? efx->n_channels - efx->n_tx_channels : 0;
-
-	/* Channel pointers were set in efx_init_struct() but we now
-	 * need to clear them for TX queues in any RX-only channels. */
-	efx_for_each_channel(channel, efx) {
-		if (channel->channel - efx->tx_channel_offset >=
-		    efx->n_tx_channels) {
-			efx_for_each_channel_tx_queue(tx_queue, channel)
-				tx_queue->channel = NULL;
-		}
-	}
 }
 
 static int efx_probe_nic(struct efx_nic *efx)
@@ -1302,7 +1321,9 @@ static int efx_probe_nic(struct efx_nic *efx)
 
 	/* Determine the number of channels and queues by trying to hook
 	 * in MSI-X interrupts. */
-	efx_probe_interrupts(efx);
+	rc = efx_probe_interrupts(efx);
+	if (rc)
+		goto fail;
 
 	if (efx->n_channels > 1)
 		get_random_bytes(&efx->rx_hash_key, sizeof(efx->rx_hash_key));
@@ -1317,6 +1338,10 @@ static int efx_probe_nic(struct efx_nic *efx)
 	efx_init_irq_moderation(efx, tx_irq_mod_usec, rx_irq_mod_usec, true);
 
 	return 0;
+
+fail:
+	efx->type->remove(efx);
+	return rc;
 }
 
 static void efx_remove_nic(struct efx_nic *efx)
@@ -1531,9 +1556,9 @@ void efx_init_irq_moderation(struct efx_nic *efx, int tx_usecs, int rx_usecs,
 	efx->irq_rx_adaptive = rx_adaptive;
 	efx->irq_rx_moderation = rx_ticks;
 	efx_for_each_channel(channel, efx) {
-		if (efx_channel_get_rx_queue(channel))
+		if (efx_channel_has_rx_queue(channel))
 			channel->irq_moderation = rx_ticks;
-		else if (efx_channel_get_tx_queue(channel, 0))
+		else if (efx_channel_has_tx_queues(channel))
 			channel->irq_moderation = tx_ticks;
 	}
 }
@@ -1849,6 +1874,10 @@ static const struct net_device_ops efx_netdev_ops = {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = efx_netpoll,
 #endif
+	.ndo_setup_tc		= efx_setup_tc,
+#ifdef CONFIG_RFS_ACCEL
+	.ndo_rx_flow_steer	= efx_filter_rfs,
+#endif
 };
 
 static void efx_update_name(struct efx_nic *efx)
@@ -1910,10 +1939,8 @@ static int efx_register_netdev(struct efx_nic *efx)
 
 	efx_for_each_channel(channel, efx) {
 		struct efx_tx_queue *tx_queue;
-		efx_for_each_channel_tx_queue(tx_queue, channel) {
-			tx_queue->core_txq = netdev_get_tx_queue(
-				efx->net_dev, tx_queue->queue / EFX_TXQ_TYPES);
-		}
+		efx_for_each_channel_tx_queue(tx_queue, channel)
+			efx_init_tx_queue_core_txq(tx_queue);
 	}
 
 	/* Always start with carrier off; PHY events will detect the link */
@@ -2288,6 +2315,10 @@ static void efx_fini_struct(struct efx_nic *efx)
  */
 static void efx_pci_remove_main(struct efx_nic *efx)
 {
+#ifdef CONFIG_RFS_ACCEL
+	free_irq_cpu_rmap(efx->net_dev->rx_cpu_rmap);
+	efx->net_dev->rx_cpu_rmap = NULL;
+#endif
 	efx_nic_fini_interrupt(efx);
 	efx_fini_channels(efx);
 	efx_fini_port(efx);
@@ -2401,7 +2432,8 @@ static int __devinit efx_pci_probe(struct pci_dev *pci_dev,
 	int i, rc;
 
 	/* Allocate and initialise a struct net_device and struct efx_nic */
-	net_dev = alloc_etherdev_mq(sizeof(*efx), EFX_MAX_CORE_TX_QUEUES);
+	net_dev = alloc_etherdev_mqs(sizeof(*efx), EFX_MAX_CORE_TX_QUEUES,
+				     EFX_MAX_RX_QUEUES);
 	if (!net_dev)
 		return -ENOMEM;
 	net_dev->features |= (type->offload_features | NETIF_F_SG |
diff --git a/drivers/net/sfc/efx.h b/drivers/net/sfc/efx.h
index d43a7e5212b1..3d83a1f74fef 100644
--- a/drivers/net/sfc/efx.h
+++ b/drivers/net/sfc/efx.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -29,6 +29,7 @@
 extern int efx_probe_tx_queue(struct efx_tx_queue *tx_queue);
 extern void efx_remove_tx_queue(struct efx_tx_queue *tx_queue);
 extern void efx_init_tx_queue(struct efx_tx_queue *tx_queue);
+extern void efx_init_tx_queue_core_txq(struct efx_tx_queue *tx_queue);
 extern void efx_fini_tx_queue(struct efx_tx_queue *tx_queue);
 extern void efx_release_tx_buffers(struct efx_tx_queue *tx_queue);
 extern netdev_tx_t
@@ -36,6 +37,7 @@ efx_hard_start_xmit(struct sk_buff *skb, struct net_device *net_dev);
 extern netdev_tx_t
 efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb);
 extern void efx_xmit_done(struct efx_tx_queue *tx_queue, unsigned int index);
+extern int efx_setup_tc(struct net_device *net_dev, u8 num_tc);
 
 /* RX */
 extern int efx_probe_rx_queue(struct efx_rx_queue *rx_queue);
@@ -74,6 +76,21 @@ extern int efx_filter_remove_filter(struct efx_nic *efx,
 				    struct efx_filter_spec *spec);
 extern void efx_filter_clear_rx(struct efx_nic *efx,
 				enum efx_filter_priority priority);
+#ifdef CONFIG_RFS_ACCEL
+extern int efx_filter_rfs(struct net_device *net_dev, const struct sk_buff *skb,
+			  u16 rxq_index, u32 flow_id);
+extern bool __efx_filter_rfs_expire(struct efx_nic *efx, unsigned quota);
+static inline void efx_filter_rfs_expire(struct efx_channel *channel)
+{
+	if (channel->rfs_filters_added >= 60 &&
+	    __efx_filter_rfs_expire(channel->efx, 100))
+		channel->rfs_filters_added -= 60;
+}
+#define efx_filter_rfs_enabled() 1
+#else
+static inline void efx_filter_rfs_expire(struct efx_channel *channel) {}
+#define efx_filter_rfs_enabled() 0
+#endif
 
 /* Channels */
 extern void efx_process_channel_now(struct efx_channel *channel);
diff --git a/drivers/net/sfc/ethtool.c b/drivers/net/sfc/ethtool.c
index 0e8bb19ed60d..807178ef65ad 100644
--- a/drivers/net/sfc/ethtool.c
+++ b/drivers/net/sfc/ethtool.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -28,7 +28,8 @@ struct efx_ethtool_stat {
 	enum {
 		EFX_ETHTOOL_STAT_SOURCE_mac_stats,
 		EFX_ETHTOOL_STAT_SOURCE_nic,
-		EFX_ETHTOOL_STAT_SOURCE_channel
+		EFX_ETHTOOL_STAT_SOURCE_channel,
+		EFX_ETHTOOL_STAT_SOURCE_tx_queue
 	} source;
 	unsigned offset;
 	u64(*get_stat) (void *field); /* Reader function */
@@ -86,6 +87,10 @@ static u64 efx_get_atomic_stat(void *field)
 	EFX_ETHTOOL_STAT(field, channel, n_##field,		\
 			 unsigned int, efx_get_uint_stat)
 
+#define EFX_ETHTOOL_UINT_TXQ_STAT(field)			\
+	EFX_ETHTOOL_STAT(tx_##field, tx_queue, field,		\
+			 unsigned int, efx_get_uint_stat)
+
 static struct efx_ethtool_stat efx_ethtool_stats[] = {
 	EFX_ETHTOOL_U64_MAC_STAT(tx_bytes),
 	EFX_ETHTOOL_U64_MAC_STAT(tx_good_bytes),
@@ -116,6 +121,10 @@ static struct efx_ethtool_stat efx_ethtool_stats[] = {
 	EFX_ETHTOOL_ULONG_MAC_STAT(tx_non_tcpudp),
 	EFX_ETHTOOL_ULONG_MAC_STAT(tx_mac_src_error),
 	EFX_ETHTOOL_ULONG_MAC_STAT(tx_ip_src_error),
+	EFX_ETHTOOL_UINT_TXQ_STAT(tso_bursts),
+	EFX_ETHTOOL_UINT_TXQ_STAT(tso_long_headers),
+	EFX_ETHTOOL_UINT_TXQ_STAT(tso_packets),
+	EFX_ETHTOOL_UINT_TXQ_STAT(pushes),
 	EFX_ETHTOOL_U64_MAC_STAT(rx_bytes),
 	EFX_ETHTOOL_U64_MAC_STAT(rx_good_bytes),
 	EFX_ETHTOOL_U64_MAC_STAT(rx_bad_bytes),
@@ -237,8 +246,8 @@ static void efx_ethtool_get_drvinfo(struct net_device *net_dev,
 	strlcpy(info->driver, KBUILD_MODNAME, sizeof(info->driver));
 	strlcpy(info->version, EFX_DRIVER_VERSION, sizeof(info->version));
 	if (efx_nic_rev(efx) >= EFX_REV_SIENA_A0)
-		siena_print_fwver(efx, info->fw_version,
-				  sizeof(info->fw_version));
+		efx_mcdi_print_fwver(efx, info->fw_version,
+				     sizeof(info->fw_version));
 	strlcpy(info->bus_info, pci_name(efx->pci_dev), sizeof(info->bus_info));
 }
 
@@ -470,6 +479,7 @@ static void efx_ethtool_get_stats(struct net_device *net_dev,
 	struct efx_mac_stats *mac_stats = &efx->mac_stats;
 	struct efx_ethtool_stat *stat;
 	struct efx_channel *channel;
+	struct efx_tx_queue *tx_queue;
 	struct rtnl_link_stats64 temp;
 	int i;
 
@@ -495,6 +505,15 @@ static void efx_ethtool_get_stats(struct net_device *net_dev,
 				data[i] += stat->get_stat((void *)channel +
 							  stat->offset);
 			break;
+		case EFX_ETHTOOL_STAT_SOURCE_tx_queue:
+			data[i] = 0;
+			efx_for_each_channel(channel, efx) {
+				efx_for_each_channel_tx_queue(tx_queue, channel)
+					data[i] +=
+						stat->get_stat((void *)tx_queue
+							       + stat->offset);
+			}
+			break;
 		}
 	}
 }
@@ -502,7 +521,7 @@ static void efx_ethtool_get_stats(struct net_device *net_dev,
 static int efx_ethtool_set_tso(struct net_device *net_dev, u32 enable)
 {
 	struct efx_nic *efx __attribute__ ((unused)) = netdev_priv(net_dev);
-	unsigned long features;
+	u32 features;
 
 	features = NETIF_F_TSO;
 	if (efx->type->offload_features & NETIF_F_V6_CSUM)
@@ -519,7 +538,7 @@ static int efx_ethtool_set_tso(struct net_device *net_dev, u32 enable)
 static int efx_ethtool_set_tx_csum(struct net_device *net_dev, u32 enable)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
-	unsigned long features = efx->type->offload_features & NETIF_F_ALL_CSUM;
+	u32 features = efx->type->offload_features & NETIF_F_ALL_CSUM;
 
 	if (enable)
 		net_dev->features |= features;
@@ -569,9 +588,14 @@ static void efx_ethtool_self_test(struct net_device *net_dev,
 				  struct ethtool_test *test, u64 *data)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
-	struct efx_self_tests efx_tests;
+	struct efx_self_tests *efx_tests;
 	int already_up;
-	int rc;
+	int rc = -ENOMEM;
+
+	efx_tests = kzalloc(sizeof(*efx_tests), GFP_KERNEL);
+	if (!efx_tests)
+		goto fail;
+
 
 	ASSERT_RTNL();
 	if (efx->state != STATE_RUNNING) {
@@ -589,13 +613,11 @@ static void efx_ethtool_self_test(struct net_device *net_dev,
 		if (rc) {
 			netif_err(efx, drv, efx->net_dev,
 				  "failed opening device.\n");
-			goto fail2;
+			goto fail1;
 		}
 	}
 
-	memset(&efx_tests, 0, sizeof(efx_tests));
-
-	rc = efx_selftest(efx, &efx_tests, test->flags);
+	rc = efx_selftest(efx, efx_tests, test->flags);
 
 	if (!already_up)
 		dev_close(efx->net_dev);
@@ -604,10 +626,11 @@ static void efx_ethtool_self_test(struct net_device *net_dev,
 		   rc == 0 ? "passed" : "failed",
 		   (test->flags & ETH_TEST_FL_OFFLINE) ? "off" : "on");
 
- fail2:
- fail1:
+fail1:
 	/* Fill ethtool results structures */
-	efx_ethtool_fill_self_tests(efx, &efx_tests, NULL, data);
+	efx_ethtool_fill_self_tests(efx, efx_tests, NULL, data);
+	kfree(efx_tests);
+fail:
 	if (rc)
 		test->flags |= ETH_TEST_FL_FAILED;
 }
@@ -631,7 +654,7 @@ static int efx_ethtool_get_coalesce(struct net_device *net_dev,
 	/* Find lowest IRQ moderation across all used TX queues */
 	coalesce->tx_coalesce_usecs_irq = ~((u32) 0);
 	efx_for_each_channel(channel, efx) {
-		if (!efx_channel_get_tx_queue(channel, 0))
+		if (!efx_channel_has_tx_queues(channel))
 			continue;
 		if (channel->irq_moderation < coalesce->tx_coalesce_usecs_irq) {
 			if (channel->channel < efx->n_rx_channels)
@@ -676,8 +699,8 @@ static int efx_ethtool_set_coalesce(struct net_device *net_dev,
 
 	/* If the channel is shared only allow RX parameters to be set */
 	efx_for_each_channel(channel, efx) {
-		if (efx_channel_get_rx_queue(channel) &&
-		    efx_channel_get_tx_queue(channel, 0) &&
+		if (efx_channel_has_rx_queue(channel) &&
+		    efx_channel_has_tx_queues(channel) &&
 		    tx_usecs) {
 			netif_err(efx, drv, efx->net_dev, "Channel is shared. "
 				  "Only RX coalescing may be set\n");
diff --git a/drivers/net/sfc/falcon.c b/drivers/net/sfc/falcon.c
index 61ddd2c6e750..734fcfb52e85 100644
--- a/drivers/net/sfc/falcon.c
+++ b/drivers/net/sfc/falcon.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -1478,36 +1478,26 @@ static void falcon_init_rx_cfg(struct efx_nic *efx)
 	/* RX control FIFO thresholds (32 entries) */
 	const unsigned ctrl_xon_thr = 20;
 	const unsigned ctrl_xoff_thr = 25;
-	/* RX data FIFO thresholds (256-byte units; size varies) */
-	int data_xon_thr = efx_nic_rx_xon_thresh >> 8;
-	int data_xoff_thr = efx_nic_rx_xoff_thresh >> 8;
 	efx_oword_t reg;
 
 	efx_reado(efx, &reg, FR_AZ_RX_CFG);
 	if (efx_nic_rev(efx) <= EFX_REV_FALCON_A1) {
 		/* Data FIFO size is 5.5K */
-		if (data_xon_thr < 0)
-			data_xon_thr = 512 >> 8;
-		if (data_xoff_thr < 0)
-			data_xoff_thr = 2048 >> 8;
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_DESC_PUSH_EN, 0);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_USR_BUF_SIZE,
 				    huge_buf_size);
-		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XON_MAC_TH, data_xon_thr);
-		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XOFF_MAC_TH, data_xoff_thr);
+		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XON_MAC_TH, 512 >> 8);
+		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XOFF_MAC_TH, 2048 >> 8);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XON_TX_TH, ctrl_xon_thr);
 		EFX_SET_OWORD_FIELD(reg, FRF_AA_RX_XOFF_TX_TH, ctrl_xoff_thr);
 	} else {
 		/* Data FIFO size is 80K; register fields moved */
-		if (data_xon_thr < 0)
-			data_xon_thr = 27648 >> 8; /* ~3*max MTU */
-		if (data_xoff_thr < 0)
-			data_xoff_thr = 54272 >> 8; /* ~80Kb - 3*max MTU */
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_DESC_PUSH_EN, 0);
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_USR_BUF_SIZE,
 				    huge_buf_size);
-		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XON_MAC_TH, data_xon_thr);
-		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XOFF_MAC_TH, data_xoff_thr);
+		/* Send XON and XOFF at ~3 * max MTU away from empty/full */
+		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XON_MAC_TH, 27648 >> 8);
+		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XOFF_MAC_TH, 54272 >> 8);
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XON_TX_TH, ctrl_xon_thr);
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_XOFF_TX_TH, ctrl_xoff_thr);
 		EFX_SET_OWORD_FIELD(reg, FRF_BZ_RX_INGR_EN, 1);
diff --git a/drivers/net/sfc/falcon_boards.c b/drivers/net/sfc/falcon_boards.c
index 2dd16f0b3ced..b9cc846811d6 100644
--- a/drivers/net/sfc/falcon_boards.c
+++ b/drivers/net/sfc/falcon_boards.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2007-2009 Solarflare Communications Inc.
+ * Copyright 2007-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/falcon_xmac.c b/drivers/net/sfc/falcon_xmac.c
index b49e84394641..2c9ee5db3bf7 100644
--- a/drivers/net/sfc/falcon_xmac.c
+++ b/drivers/net/sfc/falcon_xmac.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/filter.c b/drivers/net/sfc/filter.c
index d4722c41c4ce..95a980fd63d5 100644
--- a/drivers/net/sfc/filter.c
+++ b/drivers/net/sfc/filter.c
@@ -8,6 +8,7 @@
  */
 
 #include <linux/in.h>
+#include <net/ip.h>
 #include "efx.h"
 #include "filter.h"
 #include "io.h"
@@ -27,6 +28,10 @@
  */
 #define FILTER_CTL_SRCH_MAX 200
 
+/* Don't try very hard to find space for performance hints, as this is
+ * counter-productive. */
+#define FILTER_CTL_SRCH_HINT_MAX 5
+
 enum efx_filter_table_id {
 	EFX_FILTER_TABLE_RX_IP = 0,
 	EFX_FILTER_TABLE_RX_MAC,
@@ -47,6 +52,10 @@ struct efx_filter_table {
 struct efx_filter_state {
 	spinlock_t	lock;
 	struct efx_filter_table table[EFX_FILTER_TABLE_COUNT];
+#ifdef CONFIG_RFS_ACCEL
+	u32		*rps_flow_id;
+	unsigned	rps_expire_index;
+#endif
 };
 
 /* The filter hash function is LFSR polynomial x^16 + x^3 + 1 of a 32-bit
@@ -325,15 +334,16 @@ static int efx_filter_search(struct efx_filter_table *table,
 			     struct efx_filter_spec *spec, u32 key,
 			     bool for_insert, int *depth_required)
 {
-	unsigned hash, incr, filter_idx, depth;
+	unsigned hash, incr, filter_idx, depth, depth_max;
 	struct efx_filter_spec *cmp;
 
 	hash = efx_filter_hash(key);
 	incr = efx_filter_increment(key);
+	depth_max = (spec->priority <= EFX_FILTER_PRI_HINT ?
+		     FILTER_CTL_SRCH_HINT_MAX : FILTER_CTL_SRCH_MAX);
 
 	for (depth = 1, filter_idx = hash & (table->size - 1);
-	     depth <= FILTER_CTL_SRCH_MAX &&
-		     test_bit(filter_idx, table->used_bitmap);
+	     depth <= depth_max && test_bit(filter_idx, table->used_bitmap);
 	     ++depth) {
 		cmp = &table->spec[filter_idx];
 		if (efx_filter_equal(spec, cmp))
@@ -342,7 +352,7 @@ static int efx_filter_search(struct efx_filter_table *table,
 	}
 	if (!for_insert)
 		return -ENOENT;
-	if (depth > FILTER_CTL_SRCH_MAX)
+	if (depth > depth_max)
 		return -EBUSY;
 found:
 	*depth_required = depth;
@@ -562,6 +572,13 @@ int efx_probe_filters(struct efx_nic *efx)
 	spin_lock_init(&state->lock);
 
 	if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0) {
+#ifdef CONFIG_RFS_ACCEL
+		state->rps_flow_id = kcalloc(FR_BZ_RX_FILTER_TBL0_ROWS,
+					     sizeof(*state->rps_flow_id),
+					     GFP_KERNEL);
+		if (!state->rps_flow_id)
+			goto fail;
+#endif
 		table = &state->table[EFX_FILTER_TABLE_RX_IP];
 		table->id = EFX_FILTER_TABLE_RX_IP;
 		table->offset = FR_BZ_RX_FILTER_TBL0;
@@ -607,5 +624,97 @@ void efx_remove_filters(struct efx_nic *efx)
 		kfree(state->table[table_id].used_bitmap);
 		vfree(state->table[table_id].spec);
 	}
+#ifdef CONFIG_RFS_ACCEL
+	kfree(state->rps_flow_id);
+#endif
 	kfree(state);
 }
+
+#ifdef CONFIG_RFS_ACCEL
+
+int efx_filter_rfs(struct net_device *net_dev, const struct sk_buff *skb,
+		   u16 rxq_index, u32 flow_id)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+	struct efx_channel *channel;
+	struct efx_filter_state *state = efx->filter_state;
+	struct efx_filter_spec spec;
+	const struct iphdr *ip;
+	const __be16 *ports;
+	int nhoff;
+	int rc;
+
+	nhoff = skb_network_offset(skb);
+
+	if (skb->protocol != htons(ETH_P_IP))
+		return -EPROTONOSUPPORT;
+
+	/* RFS must validate the IP header length before calling us */
+	EFX_BUG_ON_PARANOID(!pskb_may_pull(skb, nhoff + sizeof(*ip)));
+	ip = (const struct iphdr *)(skb->data + nhoff);
+	if (ip->frag_off & htons(IP_MF | IP_OFFSET))
+		return -EPROTONOSUPPORT;
+	EFX_BUG_ON_PARANOID(!pskb_may_pull(skb, nhoff + 4 * ip->ihl + 4));
+	ports = (const __be16 *)(skb->data + nhoff + 4 * ip->ihl);
+
+	efx_filter_init_rx(&spec, EFX_FILTER_PRI_HINT, 0, rxq_index);
+	rc = efx_filter_set_ipv4_full(&spec, ip->protocol,
+				      ip->daddr, ports[1], ip->saddr, ports[0]);
+	if (rc)
+		return rc;
+
+	rc = efx_filter_insert_filter(efx, &spec, true);
+	if (rc < 0)
+		return rc;
+
+	/* Remember this so we can check whether to expire the filter later */
+	state->rps_flow_id[rc] = flow_id;
+	channel = efx_get_channel(efx, skb_get_rx_queue(skb));
+	++channel->rfs_filters_added;
+
+	netif_info(efx, rx_status, efx->net_dev,
+		   "steering %s %pI4:%u:%pI4:%u to queue %u [flow %u filter %d]\n",
+		   (ip->protocol == IPPROTO_TCP) ? "TCP" : "UDP",
+		   &ip->saddr, ntohs(ports[0]), &ip->daddr, ntohs(ports[1]),
+		   rxq_index, flow_id, rc);
+
+	return rc;
+}
+
+bool __efx_filter_rfs_expire(struct efx_nic *efx, unsigned quota)
+{
+	struct efx_filter_state *state = efx->filter_state;
+	struct efx_filter_table *table = &state->table[EFX_FILTER_TABLE_RX_IP];
+	unsigned mask = table->size - 1;
+	unsigned index;
+	unsigned stop;
+
+	if (!spin_trylock_bh(&state->lock))
+		return false;
+
+	index = state->rps_expire_index;
+	stop = (index + quota) & mask;
+
+	while (index != stop) {
+		if (test_bit(index, table->used_bitmap) &&
+		    table->spec[index].priority == EFX_FILTER_PRI_HINT &&
+		    rps_may_expire_flow(efx->net_dev,
+					table->spec[index].dmaq_id,
+					state->rps_flow_id[index], index)) {
+			netif_info(efx, rx_status, efx->net_dev,
+				   "expiring filter %d [flow %u]\n",
+				   index, state->rps_flow_id[index]);
+			efx_filter_table_clear_entry(efx, table, index);
+		}
+		index = (index + 1) & mask;
+	}
+
+	state->rps_expire_index = stop;
+	if (table->used == 0)
+		efx_filter_table_reset_search_depth(table);
+
+	spin_unlock_bh(&state->lock);
+	return true;
+}
+
+#endif /* CONFIG_RFS_ACCEL */
diff --git a/drivers/net/sfc/io.h b/drivers/net/sfc/io.h
index 6da4ae20a039..dc45110b2456 100644
--- a/drivers/net/sfc/io.h
+++ b/drivers/net/sfc/io.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/mcdi.c b/drivers/net/sfc/mcdi.c
index b716e827b291..8bba8955f310 100644
--- a/drivers/net/sfc/mcdi.c
+++ b/drivers/net/sfc/mcdi.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2008-2009 Solarflare Communications Inc.
+ * Copyright 2008-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -602,7 +602,7 @@ void efx_mcdi_process_event(struct efx_channel *channel,
  **************************************************************************
  */
 
-int efx_mcdi_fwver(struct efx_nic *efx, u64 *version, u32 *build)
+void efx_mcdi_print_fwver(struct efx_nic *efx, char *buf, size_t len)
 {
 	u8 outbuf[ALIGN(MC_CMD_GET_VERSION_V1_OUT_LEN, 4)];
 	size_t outlength;
@@ -616,29 +616,20 @@ int efx_mcdi_fwver(struct efx_nic *efx, u64 *version, u32 *build)
 	if (rc)
 		goto fail;
 
-	if (outlength == MC_CMD_GET_VERSION_V0_OUT_LEN) {
-		*version = 0;
-		*build = MCDI_DWORD(outbuf, GET_VERSION_OUT_FIRMWARE);
-		return 0;
-	}
-
 	if (outlength < MC_CMD_GET_VERSION_V1_OUT_LEN) {
 		rc = -EIO;
 		goto fail;
 	}
 
 	ver_words = (__le16 *)MCDI_PTR(outbuf, GET_VERSION_OUT_VERSION);
-	*version = (((u64)le16_to_cpu(ver_words[0]) << 48) |
-		    ((u64)le16_to_cpu(ver_words[1]) << 32) |
-		    ((u64)le16_to_cpu(ver_words[2]) << 16) |
-		    le16_to_cpu(ver_words[3]));
-	*build = MCDI_DWORD(outbuf, GET_VERSION_OUT_FIRMWARE);
-
-	return 0;
+	snprintf(buf, len, "%u.%u.%u.%u",
+		 le16_to_cpu(ver_words[0]), le16_to_cpu(ver_words[1]),
+		 le16_to_cpu(ver_words[2]), le16_to_cpu(ver_words[3]));
+	return;
 
 fail:
 	netif_err(efx, probe, efx->net_dev, "%s: failed rc=%d\n", __func__, rc);
-	return rc;
+	buf[0] = 0;
 }
 
 int efx_mcdi_drv_attach(struct efx_nic *efx, bool driver_operating,
diff --git a/drivers/net/sfc/mcdi.h b/drivers/net/sfc/mcdi.h
index c792f1d65e48..aced2a7856fc 100644
--- a/drivers/net/sfc/mcdi.h
+++ b/drivers/net/sfc/mcdi.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2008-2009 Solarflare Communications Inc.
+ * Copyright 2008-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -93,7 +93,7 @@ extern void efx_mcdi_process_event(struct efx_channel *channel,
 #define MCDI_EVENT_FIELD(_ev, _field)			\
 	EFX_QWORD_FIELD(_ev, MCDI_EVENT_ ## _field)
 
-extern int efx_mcdi_fwver(struct efx_nic *efx, u64 *version, u32 *build);
+extern void efx_mcdi_print_fwver(struct efx_nic *efx, char *buf, size_t len);
 extern int efx_mcdi_drv_attach(struct efx_nic *efx, bool driver_operating,
 			       bool *was_attached_out);
 extern int efx_mcdi_get_board_cfg(struct efx_nic *efx, u8 *mac_address,
diff --git a/drivers/net/sfc/mcdi_mac.c b/drivers/net/sfc/mcdi_mac.c
index f88f4bf986ff..33f7294edb47 100644
--- a/drivers/net/sfc/mcdi_mac.c
+++ b/drivers/net/sfc/mcdi_mac.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2009 Solarflare Communications Inc.
+ * Copyright 2009-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/mcdi_pcol.h b/drivers/net/sfc/mcdi_pcol.h
index 90359e644006..b86a15f221ad 100644
--- a/drivers/net/sfc/mcdi_pcol.h
+++ b/drivers/net/sfc/mcdi_pcol.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2009 Solarflare Communications Inc.
+ * Copyright 2009-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/mcdi_phy.c b/drivers/net/sfc/mcdi_phy.c
index 0e97eed663c6..ec3f740f5465 100644
--- a/drivers/net/sfc/mcdi_phy.c
+++ b/drivers/net/sfc/mcdi_phy.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2009 Solarflare Communications Inc.
+ * Copyright 2009-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/mdio_10g.c b/drivers/net/sfc/mdio_10g.c
index 56b0266b441f..19e68c26d103 100644
--- a/drivers/net/sfc/mdio_10g.c
+++ b/drivers/net/sfc/mdio_10g.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -51,13 +51,10 @@ int efx_mdio_reset_mmd(struct efx_nic *port, int mmd,
 	return spins ? spins : -ETIMEDOUT;
 }
 
-static int efx_mdio_check_mmd(struct efx_nic *efx, int mmd, int fault_fatal)
+static int efx_mdio_check_mmd(struct efx_nic *efx, int mmd)
 {
 	int status;
 
-	if (LOOPBACK_INTERNAL(efx))
-		return 0;
-
 	if (mmd != MDIO_MMD_AN) {
 		/* Read MMD STATUS2 to check it is responding. */
 		status = efx_mdio_read(efx, mmd, MDIO_STAT2);
@@ -68,20 +65,6 @@ static int efx_mdio_check_mmd(struct efx_nic *efx, int mmd, int fault_fatal)
 		}
 	}
 
-	/* Read MMD STATUS 1 to check for fault. */
-	status = efx_mdio_read(efx, mmd, MDIO_STAT1);
-	if (status & MDIO_STAT1_FAULT) {
-		if (fault_fatal) {
-			netif_err(efx, hw, efx->net_dev,
-				  "PHY MMD %d reporting fatal"
-				  " fault: status %x\n", mmd, status);
-			return -EIO;
-		} else {
-			netif_dbg(efx, hw, efx->net_dev,
-				  "PHY MMD %d reporting status"
-				  " %x (expected)\n", mmd, status);
-		}
-	}
 	return 0;
 }
 
@@ -130,8 +113,7 @@ int efx_mdio_wait_reset_mmds(struct efx_nic *efx, unsigned int mmd_mask)
 	return rc;
 }
 
-int efx_mdio_check_mmds(struct efx_nic *efx,
-			unsigned int mmd_mask, unsigned int fatal_mask)
+int efx_mdio_check_mmds(struct efx_nic *efx, unsigned int mmd_mask)
 {
 	int mmd = 0, probe_mmd, devs1, devs2;
 	u32 devices;
@@ -161,13 +143,9 @@ int efx_mdio_check_mmds(struct efx_nic *efx,
 
 	/* Check all required MMDs are responding and happy. */
 	while (mmd_mask) {
-		if (mmd_mask & 1) {
-			int fault_fatal = fatal_mask & 1;
-			if (efx_mdio_check_mmd(efx, mmd, fault_fatal))
-				return -EIO;
-		}
+		if ((mmd_mask & 1) && efx_mdio_check_mmd(efx, mmd))
+			return -EIO;
 		mmd_mask = mmd_mask >> 1;
-		fatal_mask = fatal_mask >> 1;
 		mmd++;
 	}
 
@@ -337,7 +315,7 @@ int efx_mdio_test_alive(struct efx_nic *efx)
 			  "no MDIO PHY present with ID %d\n", efx->mdio.prtad);
 		rc = -EINVAL;
 	} else {
-		rc = efx_mdio_check_mmds(efx, efx->mdio.mmds, 0);
+		rc = efx_mdio_check_mmds(efx, efx->mdio.mmds);
 	}
 
 	mutex_unlock(&efx->mac_lock);
diff --git a/drivers/net/sfc/mdio_10g.h b/drivers/net/sfc/mdio_10g.h
index 75791d3d4963..df0703940c83 100644
--- a/drivers/net/sfc/mdio_10g.h
+++ b/drivers/net/sfc/mdio_10g.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -68,8 +68,7 @@ extern int efx_mdio_reset_mmd(struct efx_nic *efx, int mmd,
 			      int spins, int spintime);
 
 /* As efx_mdio_check_mmd but for multiple MMDs */
-int efx_mdio_check_mmds(struct efx_nic *efx,
-			unsigned int mmd_mask, unsigned int fatal_mask);
+int efx_mdio_check_mmds(struct efx_nic *efx, unsigned int mmd_mask);
 
 /* Check the link status of specified mmds in bit mask */
 extern bool efx_mdio_links_ok(struct efx_nic *efx, unsigned int mmd_mask);
diff --git a/drivers/net/sfc/mtd.c b/drivers/net/sfc/mtd.c
index d38627448c22..e646bfce2d84 100644
--- a/drivers/net/sfc/mtd.c
+++ b/drivers/net/sfc/mtd.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/net_driver.h b/drivers/net/sfc/net_driver.h
index 28df8665256a..215d5c51bfa0 100644
--- a/drivers/net/sfc/net_driver.h
+++ b/drivers/net/sfc/net_driver.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2005-2009 Solarflare Communications Inc.
+ * Copyright 2005-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -41,7 +41,7 @@
  *
  **************************************************************************/
 
-#define EFX_DRIVER_VERSION	"3.0"
+#define EFX_DRIVER_VERSION	"3.1"
 
 #ifdef EFX_ENABLE_DEBUG
 #define EFX_BUG_ON_PARANOID(x) BUG_ON(x)
@@ -63,10 +63,12 @@
 /* Checksum generation is a per-queue option in hardware, so each
  * queue visible to the networking core is backed by two hardware TX
  * queues. */
-#define EFX_MAX_CORE_TX_QUEUES	EFX_MAX_CHANNELS
-#define EFX_TXQ_TYPE_OFFLOAD	1
-#define EFX_TXQ_TYPES		2
-#define EFX_MAX_TX_QUEUES	(EFX_TXQ_TYPES * EFX_MAX_CORE_TX_QUEUES)
+#define EFX_MAX_TX_TC		2
+#define EFX_MAX_CORE_TX_QUEUES	(EFX_MAX_TX_TC * EFX_MAX_CHANNELS)
+#define EFX_TXQ_TYPE_OFFLOAD	1	/* flag */
+#define EFX_TXQ_TYPE_HIGHPRI	2	/* flag */
+#define EFX_TXQ_TYPES		4
+#define EFX_MAX_TX_QUEUES	(EFX_TXQ_TYPES * EFX_MAX_CHANNELS)
 
 /**
  * struct efx_special_buffer - An Efx special buffer
@@ -140,6 +142,7 @@ struct efx_tx_buffer {
  * @buffer: The software buffer ring
  * @txd: The hardware descriptor ring
  * @ptr_mask: The size of the ring minus 1.
+ * @initialised: Has hardware queue been initialised?
  * @flushed: Used when handling queue flushing
  * @read_count: Current read pointer.
  *	This is the number of buffers that have been removed from both rings.
@@ -182,6 +185,7 @@ struct efx_tx_queue {
 	struct efx_tx_buffer *buffer;
 	struct efx_special_buffer txd;
 	unsigned int ptr_mask;
+	bool initialised;
 	enum efx_flush_state flushed;
 
 	/* Members used mainly on the completion path */
@@ -210,15 +214,17 @@ struct efx_tx_queue {
  *	If both this and page are %NULL, the buffer slot is currently free.
  * @page: The associated page buffer, if any.
  *	If both this and skb are %NULL, the buffer slot is currently free.
- * @data: Pointer to ethernet header
  * @len: Buffer length, in bytes.
+ * @is_page: Indicates if @page is valid. If false, @skb is valid.
  */
 struct efx_rx_buffer {
 	dma_addr_t dma_addr;
-	struct sk_buff *skb;
-	struct page *page;
-	char *data;
+	union {
+		struct sk_buff *skb;
+		struct page *page;
+	} u;
 	unsigned int len;
+	bool is_page;
 };
 
 /**
@@ -358,6 +364,9 @@ struct efx_channel {
 
 	unsigned int irq_count;
 	unsigned int irq_mod_score;
+#ifdef CONFIG_RFS_ACCEL
+	unsigned int rfs_filters_added;
+#endif
 
 	int rx_alloc_level;
 	int rx_alloc_push_pages;
@@ -377,7 +386,7 @@ struct efx_channel {
 	bool rx_pkt_csummed;
 
 	struct efx_rx_queue rx_queue;
-	struct efx_tx_queue tx_queue[2];
+	struct efx_tx_queue tx_queue[EFX_TXQ_TYPES];
 };
 
 enum efx_led_mode {
@@ -906,7 +915,7 @@ struct efx_nic_type {
 	unsigned int phys_addr_channels;
 	unsigned int tx_dc_base;
 	unsigned int rx_dc_base;
-	unsigned long offload_features;
+	u32 offload_features;
 	u32 reset_world_flags;
 };
 
@@ -938,18 +947,40 @@ efx_get_tx_queue(struct efx_nic *efx, unsigned index, unsigned type)
 	return &efx->channel[efx->tx_channel_offset + index]->tx_queue[type];
 }
 
+static inline bool efx_channel_has_tx_queues(struct efx_channel *channel)
+{
+	return channel->channel - channel->efx->tx_channel_offset <
+		channel->efx->n_tx_channels;
+}
+
 static inline struct efx_tx_queue *
 efx_channel_get_tx_queue(struct efx_channel *channel, unsigned type)
 {
-	struct efx_tx_queue *tx_queue = channel->tx_queue;
-	EFX_BUG_ON_PARANOID(type >= EFX_TXQ_TYPES);
-	return tx_queue->channel ? tx_queue + type : NULL;
+	EFX_BUG_ON_PARANOID(!efx_channel_has_tx_queues(channel) ||
+			    type >= EFX_TXQ_TYPES);
+	return &channel->tx_queue[type];
+}
+
+static inline bool efx_tx_queue_used(struct efx_tx_queue *tx_queue)
+{
+	return !(tx_queue->efx->net_dev->num_tc < 2 &&
+		 tx_queue->queue & EFX_TXQ_TYPE_HIGHPRI);
 }
 
 /* Iterate over all TX queues belonging to a channel */
 #define efx_for_each_channel_tx_queue(_tx_queue, _channel)		\
-	for (_tx_queue = efx_channel_get_tx_queue(channel, 0);		\
-	     _tx_queue && _tx_queue < (_channel)->tx_queue + EFX_TXQ_TYPES; \
+	if (!efx_channel_has_tx_queues(_channel))			\
+		;							\
+	else								\
+		for (_tx_queue = (_channel)->tx_queue;			\
+		     _tx_queue < (_channel)->tx_queue + EFX_TXQ_TYPES && \
+			     efx_tx_queue_used(_tx_queue);		\
+		     _tx_queue++)
+
+/* Iterate over all possible TX queues belonging to a channel */
+#define efx_for_each_possible_channel_tx_queue(_tx_queue, _channel)	\
+	for (_tx_queue = (_channel)->tx_queue;				\
+	     _tx_queue < (_channel)->tx_queue + EFX_TXQ_TYPES;		\
 	     _tx_queue++)
 
 static inline struct efx_rx_queue *
@@ -959,18 +990,26 @@ efx_get_rx_queue(struct efx_nic *efx, unsigned index)
 	return &efx->channel[index]->rx_queue;
 }
 
+static inline bool efx_channel_has_rx_queue(struct efx_channel *channel)
+{
+	return channel->channel < channel->efx->n_rx_channels;
+}
+
 static inline struct efx_rx_queue *
 efx_channel_get_rx_queue(struct efx_channel *channel)
 {
-	return channel->channel < channel->efx->n_rx_channels ?
-		&channel->rx_queue : NULL;
+	EFX_BUG_ON_PARANOID(!efx_channel_has_rx_queue(channel));
+	return &channel->rx_queue;
 }
 
 /* Iterate over all RX queues belonging to a channel */
 #define efx_for_each_channel_rx_queue(_rx_queue, _channel)		\
-	for (_rx_queue = efx_channel_get_rx_queue(channel);		\
-	     _rx_queue;							\
-	     _rx_queue = NULL)
+	if (!efx_channel_has_rx_queue(_channel))			\
+		;							\
+	else								\
+		for (_rx_queue = &(_channel)->rx_queue;			\
+		     _rx_queue;						\
+		     _rx_queue = NULL)
 
 static inline struct efx_channel *
 efx_rx_queue_channel(struct efx_rx_queue *rx_queue)
diff --git a/drivers/net/sfc/nic.c b/drivers/net/sfc/nic.c
index da386599ab68..e8396614daf3 100644
--- a/drivers/net/sfc/nic.c
+++ b/drivers/net/sfc/nic.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -41,26 +41,6 @@
 #define RX_DC_ENTRIES 64
 #define RX_DC_ENTRIES_ORDER 3
 
-/* RX FIFO XOFF watermark
- *
- * When the amount of the RX FIFO increases used increases past this
- * watermark send XOFF. Only used if RX flow control is enabled (ethtool -A)
- * This also has an effect on RX/TX arbitration
- */
-int efx_nic_rx_xoff_thresh = -1;
-module_param_named(rx_xoff_thresh_bytes, efx_nic_rx_xoff_thresh, int, 0644);
-MODULE_PARM_DESC(rx_xoff_thresh_bytes, "RX fifo XOFF threshold");
-
-/* RX FIFO XON watermark
- *
- * When the amount of the RX FIFO used decreases below this
- * watermark send XON. Only used if TX flow control is enabled (ethtool -A)
- * This also has an effect on RX/TX arbitration
- */
-int efx_nic_rx_xon_thresh = -1;
-module_param_named(rx_xon_thresh_bytes, efx_nic_rx_xon_thresh, int, 0644);
-MODULE_PARM_DESC(rx_xon_thresh_bytes, "RX fifo XON threshold");
-
 /* If EFX_MAX_INT_ERRORS internal errors occur within
  * EFX_INT_ERROR_EXPIRE seconds, we consider the NIC broken and
  * disable it.
@@ -445,8 +425,8 @@ int efx_nic_probe_tx(struct efx_tx_queue *tx_queue)
 
 void efx_nic_init_tx(struct efx_tx_queue *tx_queue)
 {
-	efx_oword_t tx_desc_ptr;
 	struct efx_nic *efx = tx_queue->efx;
+	efx_oword_t reg;
 
 	tx_queue->flushed = FLUSH_NONE;
 
@@ -454,7 +434,7 @@ void efx_nic_init_tx(struct efx_tx_queue *tx_queue)
 	efx_init_special_buffer(efx, &tx_queue->txd);
 
 	/* Push TX descriptor ring to card */
-	EFX_POPULATE_OWORD_10(tx_desc_ptr,
+	EFX_POPULATE_OWORD_10(reg,
 			      FRF_AZ_TX_DESCQ_EN, 1,
 			      FRF_AZ_TX_ISCSI_DDIG_EN, 0,
 			      FRF_AZ_TX_ISCSI_HDIG_EN, 0,
@@ -470,17 +450,15 @@ void efx_nic_init_tx(struct efx_tx_queue *tx_queue)
 
 	if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0) {
 		int csum = tx_queue->queue & EFX_TXQ_TYPE_OFFLOAD;
-		EFX_SET_OWORD_FIELD(tx_desc_ptr, FRF_BZ_TX_IP_CHKSM_DIS, !csum);
-		EFX_SET_OWORD_FIELD(tx_desc_ptr, FRF_BZ_TX_TCP_CHKSM_DIS,
+		EFX_SET_OWORD_FIELD(reg, FRF_BZ_TX_IP_CHKSM_DIS, !csum);
+		EFX_SET_OWORD_FIELD(reg, FRF_BZ_TX_TCP_CHKSM_DIS,
 				    !csum);
 	}
 
-	efx_writeo_table(efx, &tx_desc_ptr, efx->type->txd_ptr_tbl_base,
+	efx_writeo_table(efx, &reg, efx->type->txd_ptr_tbl_base,
 			 tx_queue->queue);
 
 	if (efx_nic_rev(efx) < EFX_REV_FALCON_B0) {
-		efx_oword_t reg;
-
 		/* Only 128 bits in this register */
 		BUILD_BUG_ON(EFX_MAX_TX_QUEUES > 128);
 
@@ -491,6 +469,16 @@ void efx_nic_init_tx(struct efx_tx_queue *tx_queue)
 			set_bit_le(tx_queue->queue, (void *)&reg);
 		efx_writeo(efx, &reg, FR_AA_TX_CHKSM_CFG);
 	}
+
+	if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0) {
+		EFX_POPULATE_OWORD_1(reg,
+				     FRF_BZ_TX_PACE,
+				     (tx_queue->queue & EFX_TXQ_TYPE_HIGHPRI) ?
+				     FFE_BZ_TX_PACE_OFF :
+				     FFE_BZ_TX_PACE_RESERVED);
+		efx_writeo_table(efx, &reg, FR_BZ_TX_PACE_TBL,
+				 tx_queue->queue);
+	}
 }
 
 static void efx_flush_tx_queue(struct efx_tx_queue *tx_queue)
@@ -1238,8 +1226,10 @@ int efx_nic_flush_queues(struct efx_nic *efx)
 
 	/* Flush all tx queues in parallel */
 	efx_for_each_channel(channel, efx) {
-		efx_for_each_channel_tx_queue(tx_queue, channel)
-			efx_flush_tx_queue(tx_queue);
+		efx_for_each_possible_channel_tx_queue(tx_queue, channel) {
+			if (tx_queue->initialised)
+				efx_flush_tx_queue(tx_queue);
+		}
 	}
 
 	/* The hardware supports four concurrent rx flushes, each of which may
@@ -1262,8 +1252,9 @@ int efx_nic_flush_queues(struct efx_nic *efx)
 					++rx_pending;
 				}
 			}
-			efx_for_each_channel_tx_queue(tx_queue, channel) {
-				if (tx_queue->flushed != FLUSH_DONE)
+			efx_for_each_possible_channel_tx_queue(tx_queue, channel) {
+				if (tx_queue->initialised &&
+				    tx_queue->flushed != FLUSH_DONE)
 					++tx_pending;
 			}
 		}
@@ -1278,8 +1269,9 @@ int efx_nic_flush_queues(struct efx_nic *efx)
 	/* Mark the queues as all flushed. We're going to return failure
 	 * leading to a reset, or fake up success anyway */
 	efx_for_each_channel(channel, efx) {
-		efx_for_each_channel_tx_queue(tx_queue, channel) {
-			if (tx_queue->flushed != FLUSH_DONE)
+		efx_for_each_possible_channel_tx_queue(tx_queue, channel) {
+			if (tx_queue->initialised &&
+			    tx_queue->flushed != FLUSH_DONE)
 				netif_err(efx, hw, efx->net_dev,
 					  "tx queue %d flush command timed out\n",
 					  tx_queue->queue);
@@ -1682,6 +1674,19 @@ void efx_nic_init_common(struct efx_nic *efx)
 	if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0)
 		EFX_SET_OWORD_FIELD(temp, FRF_BZ_TX_FLUSH_MIN_LEN_EN, 1);
 	efx_writeo(efx, &temp, FR_AZ_TX_RESERVED);
+
+	if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0) {
+		EFX_POPULATE_OWORD_4(temp,
+				     /* Default values */
+				     FRF_BZ_TX_PACE_SB_NOT_AF, 0x15,
+				     FRF_BZ_TX_PACE_SB_AF, 0xb,
+				     FRF_BZ_TX_PACE_FB_BASE, 0,
+				     /* Allow large pace values in the
+				      * fast bin. */
+				     FRF_BZ_TX_PACE_BIN_TH,
+				     FFE_BZ_TX_PACE_RESERVED);
+		efx_writeo(efx, &temp, FR_BZ_TX_PACE);
+	}
 }
 
 /* Register dump */
diff --git a/drivers/net/sfc/nic.h b/drivers/net/sfc/nic.h
index eb0586925b51..d9de1b647d41 100644
--- a/drivers/net/sfc/nic.h
+++ b/drivers/net/sfc/nic.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -142,20 +142,14 @@ static inline struct falcon_board *falcon_board(struct efx_nic *efx)
 
 /**
  * struct siena_nic_data - Siena NIC state
- * @fw_version: Management controller firmware version
- * @fw_build: Firmware build number
  * @mcdi: Management-Controller-to-Driver Interface
  * @wol_filter_id: Wake-on-LAN packet filter id
  */
 struct siena_nic_data {
-	u64 fw_version;
-	u32 fw_build;
 	struct efx_mcdi_iface mcdi;
 	int wol_filter_id;
 };
 
-extern void siena_print_fwver(struct efx_nic *efx, char *buf, size_t len);
-
 extern struct efx_nic_type falcon_a1_nic_type;
 extern struct efx_nic_type falcon_b0_nic_type;
 extern struct efx_nic_type siena_a0_nic_type;
@@ -194,7 +188,6 @@ extern void efx_nic_eventq_read_ack(struct efx_channel *channel);
 /* MAC/PHY */
 extern void falcon_drain_tx_fifo(struct efx_nic *efx);
 extern void falcon_reconfigure_mac_wrapper(struct efx_nic *efx);
-extern int efx_nic_rx_xoff_thresh, efx_nic_rx_xon_thresh;
 
 /* Interrupts and test events */
 extern int efx_nic_init_interrupt(struct efx_nic *efx);
diff --git a/drivers/net/sfc/phy.h b/drivers/net/sfc/phy.h
index 1dab609757fb..b3b79472421e 100644
--- a/drivers/net/sfc/phy.h
+++ b/drivers/net/sfc/phy.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2007-2009 Solarflare Communications Inc.
+ * Copyright 2007-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/qt202x_phy.c b/drivers/net/sfc/qt202x_phy.c
index ea3ae0089315..55f90924247e 100644
--- a/drivers/net/sfc/qt202x_phy.c
+++ b/drivers/net/sfc/qt202x_phy.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/regs.h b/drivers/net/sfc/regs.h
index 96430ed81c36..cc2c86b76a7b 100644
--- a/drivers/net/sfc/regs.h
+++ b/drivers/net/sfc/regs.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -2907,6 +2907,12 @@
 #define FRF_CZ_TMFT_SRC_MAC_HI_LBN 44
 #define FRF_CZ_TMFT_SRC_MAC_HI_WIDTH 16
 
+/* TX_PACE_TBL */
+/* Values >20 are documented as reserved, but will result in a queue going
+ * into the fast bin with a pace value of zero. */
+#define FFE_BZ_TX_PACE_OFF 0
+#define FFE_BZ_TX_PACE_RESERVED 21
+
 /* DRIVER_EV */
 /* Sub-fields of an RX flush completion event */
 #define FSF_AZ_DRIVER_EV_RX_FLUSH_FAIL_LBN 12
diff --git a/drivers/net/sfc/rx.c b/drivers/net/sfc/rx.c
index 3925fd621177..c0fdb59030fb 100644
--- a/drivers/net/sfc/rx.c
+++ b/drivers/net/sfc/rx.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2005-2009 Solarflare Communications Inc.
+ * Copyright 2005-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -89,24 +89,37 @@ static unsigned int rx_refill_limit = 95;
  */
 #define EFX_RXD_HEAD_ROOM 2
 
-static inline unsigned int efx_rx_buf_offset(struct efx_rx_buffer *buf)
+/* Offset of ethernet header within page */
+static inline unsigned int efx_rx_buf_offset(struct efx_nic *efx,
+					     struct efx_rx_buffer *buf)
 {
 	/* Offset is always within one page, so we don't need to consider
 	 * the page order.
 	 */
-	return (__force unsigned long) buf->data & (PAGE_SIZE - 1);
+	return (((__force unsigned long) buf->dma_addr & (PAGE_SIZE - 1)) +
+		efx->type->rx_buffer_hash_size);
 }
 static inline unsigned int efx_rx_buf_size(struct efx_nic *efx)
 {
 	return PAGE_SIZE << efx->rx_buffer_order;
 }
 
-static inline u32 efx_rx_buf_hash(struct efx_rx_buffer *buf)
+static u8 *efx_rx_buf_eh(struct efx_nic *efx, struct efx_rx_buffer *buf)
 {
+	if (buf->is_page)
+		return page_address(buf->u.page) + efx_rx_buf_offset(efx, buf);
+	else
+		return ((u8 *)buf->u.skb->data +
+			efx->type->rx_buffer_hash_size);
+}
+
+static inline u32 efx_rx_buf_hash(const u8 *eh)
+{
+	/* The ethernet header is always directly after any hash. */
 #if defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS) || NET_IP_ALIGN % 4 == 0
-	return __le32_to_cpup((const __le32 *)(buf->data - 4));
+	return __le32_to_cpup((const __le32 *)(eh - 4));
 #else
-	const u8 *data = (const u8 *)(buf->data - 4);
+	const u8 *data = eh - 4;
 	return ((u32)data[0]       |
 		(u32)data[1] << 8  |
 		(u32)data[2] << 16 |
@@ -129,6 +142,7 @@ static int efx_init_rx_buffers_skb(struct efx_rx_queue *rx_queue)
 	struct efx_nic *efx = rx_queue->efx;
 	struct net_device *net_dev = efx->net_dev;
 	struct efx_rx_buffer *rx_buf;
+	struct sk_buff *skb;
 	int skb_len = efx->rx_buffer_len;
 	unsigned index, count;
 
@@ -136,24 +150,23 @@ static int efx_init_rx_buffers_skb(struct efx_rx_queue *rx_queue)
 		index = rx_queue->added_count & rx_queue->ptr_mask;
 		rx_buf = efx_rx_buffer(rx_queue, index);
 
-		rx_buf->skb = netdev_alloc_skb(net_dev, skb_len);
-		if (unlikely(!rx_buf->skb))
+		rx_buf->u.skb = skb = netdev_alloc_skb(net_dev, skb_len);
+		if (unlikely(!skb))
 			return -ENOMEM;
-		rx_buf->page = NULL;
 
 		/* Adjust the SKB for padding and checksum */
-		skb_reserve(rx_buf->skb, NET_IP_ALIGN);
+		skb_reserve(skb, NET_IP_ALIGN);
 		rx_buf->len = skb_len - NET_IP_ALIGN;
-		rx_buf->data = (char *)rx_buf->skb->data;
-		rx_buf->skb->ip_summed = CHECKSUM_UNNECESSARY;
+		rx_buf->is_page = false;
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
 
 		rx_buf->dma_addr = pci_map_single(efx->pci_dev,
-						  rx_buf->data, rx_buf->len,
+						  skb->data, rx_buf->len,
 						  PCI_DMA_FROMDEVICE);
 		if (unlikely(pci_dma_mapping_error(efx->pci_dev,
 						   rx_buf->dma_addr))) {
-			dev_kfree_skb_any(rx_buf->skb);
-			rx_buf->skb = NULL;
+			dev_kfree_skb_any(skb);
+			rx_buf->u.skb = NULL;
 			return -EIO;
 		}
 
@@ -211,10 +224,9 @@ static int efx_init_rx_buffers_page(struct efx_rx_queue *rx_queue)
 		index = rx_queue->added_count & rx_queue->ptr_mask;
 		rx_buf = efx_rx_buffer(rx_queue, index);
 		rx_buf->dma_addr = dma_addr + EFX_PAGE_IP_ALIGN;
-		rx_buf->skb = NULL;
-		rx_buf->page = page;
-		rx_buf->data = page_addr + EFX_PAGE_IP_ALIGN;
+		rx_buf->u.page = page;
 		rx_buf->len = efx->rx_buffer_len - EFX_PAGE_IP_ALIGN;
+		rx_buf->is_page = true;
 		++rx_queue->added_count;
 		++rx_queue->alloc_page_count;
 		++state->refcnt;
@@ -235,19 +247,17 @@ static int efx_init_rx_buffers_page(struct efx_rx_queue *rx_queue)
 static void efx_unmap_rx_buffer(struct efx_nic *efx,
 				struct efx_rx_buffer *rx_buf)
 {
-	if (rx_buf->page) {
+	if (rx_buf->is_page && rx_buf->u.page) {
 		struct efx_rx_page_state *state;
 
-		EFX_BUG_ON_PARANOID(rx_buf->skb);
-
-		state = page_address(rx_buf->page);
+		state = page_address(rx_buf->u.page);
 		if (--state->refcnt == 0) {
 			pci_unmap_page(efx->pci_dev,
 				       state->dma_addr,
 				       efx_rx_buf_size(efx),
 				       PCI_DMA_FROMDEVICE);
 		}
-	} else if (likely(rx_buf->skb)) {
+	} else if (!rx_buf->is_page && rx_buf->u.skb) {
 		pci_unmap_single(efx->pci_dev, rx_buf->dma_addr,
 				 rx_buf->len, PCI_DMA_FROMDEVICE);
 	}
@@ -256,12 +266,12 @@ static void efx_unmap_rx_buffer(struct efx_nic *efx,
 static void efx_free_rx_buffer(struct efx_nic *efx,
 			       struct efx_rx_buffer *rx_buf)
 {
-	if (rx_buf->page) {
-		__free_pages(rx_buf->page, efx->rx_buffer_order);
-		rx_buf->page = NULL;
-	} else if (likely(rx_buf->skb)) {
-		dev_kfree_skb_any(rx_buf->skb);
-		rx_buf->skb = NULL;
+	if (rx_buf->is_page && rx_buf->u.page) {
+		__free_pages(rx_buf->u.page, efx->rx_buffer_order);
+		rx_buf->u.page = NULL;
+	} else if (!rx_buf->is_page && rx_buf->u.skb) {
+		dev_kfree_skb_any(rx_buf->u.skb);
+		rx_buf->u.skb = NULL;
 	}
 }
 
@@ -277,7 +287,7 @@ static void efx_fini_rx_buffer(struct efx_rx_queue *rx_queue,
 static void efx_resurrect_rx_buffer(struct efx_rx_queue *rx_queue,
 				    struct efx_rx_buffer *rx_buf)
 {
-	struct efx_rx_page_state *state = page_address(rx_buf->page);
+	struct efx_rx_page_state *state = page_address(rx_buf->u.page);
 	struct efx_rx_buffer *new_buf;
 	unsigned fill_level, index;
 
@@ -292,16 +302,14 @@ static void efx_resurrect_rx_buffer(struct efx_rx_queue *rx_queue,
 	}
 
 	++state->refcnt;
-	get_page(rx_buf->page);
+	get_page(rx_buf->u.page);
 
 	index = rx_queue->added_count & rx_queue->ptr_mask;
 	new_buf = efx_rx_buffer(rx_queue, index);
 	new_buf->dma_addr = rx_buf->dma_addr ^ (PAGE_SIZE >> 1);
-	new_buf->skb = NULL;
-	new_buf->page = rx_buf->page;
-	new_buf->data = (void *)
-		((__force unsigned long)rx_buf->data ^ (PAGE_SIZE >> 1));
+	new_buf->u.page = rx_buf->u.page;
 	new_buf->len = rx_buf->len;
+	new_buf->is_page = true;
 	++rx_queue->added_count;
 }
 
@@ -315,16 +323,15 @@ static void efx_recycle_rx_buffer(struct efx_channel *channel,
 	struct efx_rx_buffer *new_buf;
 	unsigned index;
 
-	if (rx_buf->page != NULL && efx->rx_buffer_len <= EFX_RX_HALF_PAGE &&
-	    page_count(rx_buf->page) == 1)
+	if (rx_buf->is_page && efx->rx_buffer_len <= EFX_RX_HALF_PAGE &&
+	    page_count(rx_buf->u.page) == 1)
 		efx_resurrect_rx_buffer(rx_queue, rx_buf);
 
 	index = rx_queue->added_count & rx_queue->ptr_mask;
 	new_buf = efx_rx_buffer(rx_queue, index);
 
 	memcpy(new_buf, rx_buf, sizeof(*new_buf));
-	rx_buf->page = NULL;
-	rx_buf->skb = NULL;
+	rx_buf->u.page = NULL;
 	++rx_queue->added_count;
 }
 
@@ -428,7 +435,7 @@ static void efx_rx_packet__check_len(struct efx_rx_queue *rx_queue,
 		 * data at the end of the skb will be trashed. So
 		 * we have no choice but to leak the fragment.
 		 */
-		*leak_packet = (rx_buf->skb != NULL);
+		*leak_packet = !rx_buf->is_page;
 		efx_schedule_reset(efx, RESET_TYPE_RX_RECOVERY);
 	} else {
 		if (net_ratelimit())
@@ -448,19 +455,18 @@ static void efx_rx_packet__check_len(struct efx_rx_queue *rx_queue,
  */
 static void efx_rx_packet_gro(struct efx_channel *channel,
 			      struct efx_rx_buffer *rx_buf,
-			      bool checksummed)
+			      const u8 *eh, bool checksummed)
 {
 	struct napi_struct *napi = &channel->napi_str;
 	gro_result_t gro_result;
 
 	/* Pass the skb/page into the GRO engine */
-	if (rx_buf->page) {
+	if (rx_buf->is_page) {
 		struct efx_nic *efx = channel->efx;
-		struct page *page = rx_buf->page;
+		struct page *page = rx_buf->u.page;
 		struct sk_buff *skb;
 
-		EFX_BUG_ON_PARANOID(rx_buf->skb);
-		rx_buf->page = NULL;
+		rx_buf->u.page = NULL;
 
 		skb = napi_get_frags(napi);
 		if (!skb) {
@@ -469,11 +475,11 @@ static void efx_rx_packet_gro(struct efx_channel *channel,
 		}
 
 		if (efx->net_dev->features & NETIF_F_RXHASH)
-			skb->rxhash = efx_rx_buf_hash(rx_buf);
+			skb->rxhash = efx_rx_buf_hash(eh);
 
 		skb_shinfo(skb)->frags[0].page = page;
 		skb_shinfo(skb)->frags[0].page_offset =
-			efx_rx_buf_offset(rx_buf);
+			efx_rx_buf_offset(efx, rx_buf);
 		skb_shinfo(skb)->frags[0].size = rx_buf->len;
 		skb_shinfo(skb)->nr_frags = 1;
 
@@ -487,11 +493,10 @@ static void efx_rx_packet_gro(struct efx_channel *channel,
 
 		gro_result = napi_gro_frags(napi);
 	} else {
-		struct sk_buff *skb = rx_buf->skb;
+		struct sk_buff *skb = rx_buf->u.skb;
 
-		EFX_BUG_ON_PARANOID(!skb);
 		EFX_BUG_ON_PARANOID(!checksummed);
-		rx_buf->skb = NULL;
+		rx_buf->u.skb = NULL;
 
 		gro_result = napi_gro_receive(napi, skb);
 	}
@@ -513,9 +518,6 @@ void efx_rx_packet(struct efx_rx_queue *rx_queue, unsigned int index,
 	bool leak_packet = false;
 
 	rx_buf = efx_rx_buffer(rx_queue, index);
-	EFX_BUG_ON_PARANOID(!rx_buf->data);
-	EFX_BUG_ON_PARANOID(rx_buf->skb && rx_buf->page);
-	EFX_BUG_ON_PARANOID(!(rx_buf->skb || rx_buf->page));
 
 	/* This allows the refill path to post another buffer.
 	 * EFX_RXD_HEAD_ROOM ensures that the slot we are using
@@ -554,12 +556,12 @@ void efx_rx_packet(struct efx_rx_queue *rx_queue, unsigned int index,
 	/* Prefetch nice and early so data will (hopefully) be in cache by
 	 * the time we look at it.
 	 */
-	prefetch(rx_buf->data);
+	prefetch(efx_rx_buf_eh(efx, rx_buf));
 
 	/* Pipeline receives so that we give time for packet headers to be
 	 * prefetched into cache.
 	 */
-	rx_buf->len = len;
+	rx_buf->len = len - efx->type->rx_buffer_hash_size;
 out:
 	if (channel->rx_pkt)
 		__efx_rx_packet(channel,
@@ -574,45 +576,43 @@ void __efx_rx_packet(struct efx_channel *channel,
 {
 	struct efx_nic *efx = channel->efx;
 	struct sk_buff *skb;
-
-	rx_buf->data += efx->type->rx_buffer_hash_size;
-	rx_buf->len -= efx->type->rx_buffer_hash_size;
+	u8 *eh = efx_rx_buf_eh(efx, rx_buf);
 
 	/* If we're in loopback test, then pass the packet directly to the
 	 * loopback layer, and free the rx_buf here
 	 */
 	if (unlikely(efx->loopback_selftest)) {
-		efx_loopback_rx_packet(efx, rx_buf->data, rx_buf->len);
+		efx_loopback_rx_packet(efx, eh, rx_buf->len);
 		efx_free_rx_buffer(efx, rx_buf);
 		return;
 	}
 
-	if (rx_buf->skb) {
-		prefetch(skb_shinfo(rx_buf->skb));
+	if (!rx_buf->is_page) {
+		skb = rx_buf->u.skb;
 
-		skb_reserve(rx_buf->skb, efx->type->rx_buffer_hash_size);
-		skb_put(rx_buf->skb, rx_buf->len);
+		prefetch(skb_shinfo(skb));
+
+		skb_reserve(skb, efx->type->rx_buffer_hash_size);
+		skb_put(skb, rx_buf->len);
 
 		if (efx->net_dev->features & NETIF_F_RXHASH)
-			rx_buf->skb->rxhash = efx_rx_buf_hash(rx_buf);
+			skb->rxhash = efx_rx_buf_hash(eh);
 
 		/* Move past the ethernet header. rx_buf->data still points
 		 * at the ethernet header */
-		rx_buf->skb->protocol = eth_type_trans(rx_buf->skb,
-						       efx->net_dev);
+		skb->protocol = eth_type_trans(skb, efx->net_dev);
 
-		skb_record_rx_queue(rx_buf->skb, channel->channel);
+		skb_record_rx_queue(skb, channel->channel);
 	}
 
-	if (likely(checksummed || rx_buf->page)) {
-		efx_rx_packet_gro(channel, rx_buf, checksummed);
+	if (likely(checksummed || rx_buf->is_page)) {
+		efx_rx_packet_gro(channel, rx_buf, eh, checksummed);
 		return;
 	}
 
 	/* We now own the SKB */
-	skb = rx_buf->skb;
-	rx_buf->skb = NULL;
-	EFX_BUG_ON_PARANOID(!skb);
+	skb = rx_buf->u.skb;
+	rx_buf->u.skb = NULL;
 
 	/* Set the SKB flags */
 	skb_checksum_none_assert(skb);
diff --git a/drivers/net/sfc/selftest.c b/drivers/net/sfc/selftest.c
index 0ebfb99f1299..a0f49b348d62 100644
--- a/drivers/net/sfc/selftest.c
+++ b/drivers/net/sfc/selftest.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -644,7 +644,7 @@ static int efx_test_loopbacks(struct efx_nic *efx, struct efx_self_tests *tests,
 			goto out;
 		}
 
-		/* Test both types of TX queue */
+		/* Test all enabled types of TX queue */
 		efx_for_each_channel_tx_queue(tx_queue, channel) {
 			state->offload_csum = (tx_queue->queue &
 					       EFX_TXQ_TYPE_OFFLOAD);
diff --git a/drivers/net/sfc/selftest.h b/drivers/net/sfc/selftest.h
index aed495a4dad7..dba5456e70f3 100644
--- a/drivers/net/sfc/selftest.h
+++ b/drivers/net/sfc/selftest.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2008 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/siena.c b/drivers/net/sfc/siena.c
index bf8456176443..e4dd8986b1fe 100644
--- a/drivers/net/sfc/siena.c
+++ b/drivers/net/sfc/siena.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -227,13 +227,6 @@ static int siena_probe_nic(struct efx_nic *efx)
 	if (rc)
 		goto fail1;
 
-	rc = efx_mcdi_fwver(efx, &nic_data->fw_version, &nic_data->fw_build);
-	if (rc) {
-		netif_err(efx, probe, efx->net_dev,
-			  "Failed to read MCPU firmware version - rc %d\n", rc);
-		goto fail1; /* MCPU absent? */
-	}
-
 	/* Let the BMC know that the driver is now in charge of link and
 	 * filter settings. We must do this before we reset the NIC */
 	rc = efx_mcdi_drv_attach(efx, true, &already_attached);
@@ -348,11 +341,6 @@ static int siena_init_nic(struct efx_nic *efx)
 	       FRF_CZ_RX_RSS_IPV6_TKEY_HI_WIDTH / 8);
 	efx_writeo(efx, &temp, FR_CZ_RX_RSS_IPV6_REG3);
 
-	if (efx_nic_rx_xoff_thresh >= 0 || efx_nic_rx_xon_thresh >= 0)
-		/* No MCDI operation has been defined to set thresholds */
-		netif_err(efx, hw, efx->net_dev,
-			  "ignoring RX flow control thresholds\n");
-
 	/* Enable event logging */
 	rc = efx_mcdi_log_ctrl(efx, true, false, 0);
 	if (rc)
@@ -514,16 +502,6 @@ static void siena_stop_nic_stats(struct efx_nic *efx)
 	efx_mcdi_mac_stats(efx, efx->stats_buffer.dma_addr, 0, 0, 0);
 }
 
-void siena_print_fwver(struct efx_nic *efx, char *buf, size_t len)
-{
-	struct siena_nic_data *nic_data = efx->nic_data;
-	snprintf(buf, len, "%u.%u.%u.%u",
-		 (unsigned int)(nic_data->fw_version >> 48),
-		 (unsigned int)(nic_data->fw_version >> 32 & 0xffff),
-		 (unsigned int)(nic_data->fw_version >> 16 & 0xffff),
-		 (unsigned int)(nic_data->fw_version & 0xffff));
-}
-
 /**************************************************************************
  *
  * Wake on LAN
diff --git a/drivers/net/sfc/spi.h b/drivers/net/sfc/spi.h
index 879b7f6bde3d..71f2e3ebe1c7 100644
--- a/drivers/net/sfc/spi.h
+++ b/drivers/net/sfc/spi.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005 Fen Systems Ltd.
- * Copyright 2006 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sfc/tenxpress.c b/drivers/net/sfc/tenxpress.c
index f102912eba91..efdceb35aaae 100644
--- a/drivers/net/sfc/tenxpress.c
+++ b/drivers/net/sfc/tenxpress.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2007-2009 Solarflare Communications Inc.
+ * Copyright 2007-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -196,7 +196,7 @@ static int tenxpress_phy_init(struct efx_nic *efx)
 		if (rc < 0)
 			return rc;
 
-		rc = efx_mdio_check_mmds(efx, TENXPRESS_REQUIRED_DEVS, 0);
+		rc = efx_mdio_check_mmds(efx, TENXPRESS_REQUIRED_DEVS);
 		if (rc < 0)
 			return rc;
 	}
diff --git a/drivers/net/sfc/tx.c b/drivers/net/sfc/tx.c
index 2f5e9da657bf..139801908217 100644
--- a/drivers/net/sfc/tx.c
+++ b/drivers/net/sfc/tx.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
  * Copyright 2005-2006 Fen Systems Ltd.
- * Copyright 2005-2009 Solarflare Communications Inc.
+ * Copyright 2005-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -336,17 +336,91 @@ netdev_tx_t efx_hard_start_xmit(struct sk_buff *skb,
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
 	struct efx_tx_queue *tx_queue;
+	unsigned index, type;
 
 	if (unlikely(efx->port_inhibited))
 		return NETDEV_TX_BUSY;
 
-	tx_queue = efx_get_tx_queue(efx, skb_get_queue_mapping(skb),
-				    skb->ip_summed == CHECKSUM_PARTIAL ?
-				    EFX_TXQ_TYPE_OFFLOAD : 0);
+	index = skb_get_queue_mapping(skb);
+	type = skb->ip_summed == CHECKSUM_PARTIAL ? EFX_TXQ_TYPE_OFFLOAD : 0;
+	if (index >= efx->n_tx_channels) {
+		index -= efx->n_tx_channels;
+		type |= EFX_TXQ_TYPE_HIGHPRI;
+	}
+	tx_queue = efx_get_tx_queue(efx, index, type);
 
 	return efx_enqueue_skb(tx_queue, skb);
 }
 
+void efx_init_tx_queue_core_txq(struct efx_tx_queue *tx_queue)
+{
+	struct efx_nic *efx = tx_queue->efx;
+
+	/* Must be inverse of queue lookup in efx_hard_start_xmit() */
+	tx_queue->core_txq =
+		netdev_get_tx_queue(efx->net_dev,
+				    tx_queue->queue / EFX_TXQ_TYPES +
+				    ((tx_queue->queue & EFX_TXQ_TYPE_HIGHPRI) ?
+				     efx->n_tx_channels : 0));
+}
+
+int efx_setup_tc(struct net_device *net_dev, u8 num_tc)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+	struct efx_channel *channel;
+	struct efx_tx_queue *tx_queue;
+	unsigned tc;
+	int rc;
+
+	if (efx_nic_rev(efx) < EFX_REV_FALCON_B0 || num_tc > EFX_MAX_TX_TC)
+		return -EINVAL;
+
+	if (num_tc == net_dev->num_tc)
+		return 0;
+
+	for (tc = 0; tc < num_tc; tc++) {
+		net_dev->tc_to_txq[tc].offset = tc * efx->n_tx_channels;
+		net_dev->tc_to_txq[tc].count = efx->n_tx_channels;
+	}
+
+	if (num_tc > net_dev->num_tc) {
+		/* Initialise high-priority queues as necessary */
+		efx_for_each_channel(channel, efx) {
+			efx_for_each_possible_channel_tx_queue(tx_queue,
+							       channel) {
+				if (!(tx_queue->queue & EFX_TXQ_TYPE_HIGHPRI))
+					continue;
+				if (!tx_queue->buffer) {
+					rc = efx_probe_tx_queue(tx_queue);
+					if (rc)
+						return rc;
+				}
+				if (!tx_queue->initialised)
+					efx_init_tx_queue(tx_queue);
+				efx_init_tx_queue_core_txq(tx_queue);
+			}
+		}
+	} else {
+		/* Reduce number of classes before number of queues */
+		net_dev->num_tc = num_tc;
+	}
+
+	rc = netif_set_real_num_tx_queues(net_dev,
+					  max_t(int, num_tc, 1) *
+					  efx->n_tx_channels);
+	if (rc)
+		return rc;
+
+	/* Do not destroy high-priority queues when they become
+	 * unused.  We would have to flush them first, and it is
+	 * fairly difficult to flush a subset of TX queues.  Leave
+	 * it to efx_fini_channels().
+	 */
+
+	net_dev->num_tc = num_tc;
+	return 0;
+}
+
 void efx_xmit_done(struct efx_tx_queue *tx_queue, unsigned int index)
 {
 	unsigned fill_level;
@@ -430,6 +504,8 @@ void efx_init_tx_queue(struct efx_tx_queue *tx_queue)
 
 	/* Set up TX descriptor ring */
 	efx_nic_init_tx(tx_queue);
+
+	tx_queue->initialised = true;
 }
 
 void efx_release_tx_buffers(struct efx_tx_queue *tx_queue)
@@ -452,9 +528,14 @@ void efx_release_tx_buffers(struct efx_tx_queue *tx_queue)
 
 void efx_fini_tx_queue(struct efx_tx_queue *tx_queue)
 {
+	if (!tx_queue->initialised)
+		return;
+
 	netif_dbg(tx_queue->efx, drv, tx_queue->efx->net_dev,
 		  "shutting down TX queue %d\n", tx_queue->queue);
 
+	tx_queue->initialised = false;
+
 	/* Flush TX queue, remove descriptor ring */
 	efx_nic_fini_tx(tx_queue);
 
@@ -466,6 +547,9 @@ void efx_fini_tx_queue(struct efx_tx_queue *tx_queue)
 
 void efx_remove_tx_queue(struct efx_tx_queue *tx_queue)
 {
+	if (!tx_queue->buffer)
+		return;
+
 	netif_dbg(tx_queue->efx, drv, tx_queue->efx->net_dev,
 		  "destroying TX queue %d\n", tx_queue->queue);
 	efx_nic_remove_tx(tx_queue);
diff --git a/drivers/net/sfc/txc43128_phy.c b/drivers/net/sfc/txc43128_phy.c
index 351794a79215..d9886addcc99 100644
--- a/drivers/net/sfc/txc43128_phy.c
+++ b/drivers/net/sfc/txc43128_phy.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2006-2010 Solarflare Communications Inc.
+ * Copyright 2006-2011 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
@@ -193,7 +193,7 @@ static int txc_reset_phy(struct efx_nic *efx)
 		goto fail;
 
 	/* Check that all the MMDs we expect are present and responding. */
-	rc = efx_mdio_check_mmds(efx, TXC_REQUIRED_DEVS, 0);
+	rc = efx_mdio_check_mmds(efx, TXC_REQUIRED_DEVS);
 	if (rc < 0)
 		goto fail;
 
diff --git a/drivers/net/sfc/workarounds.h b/drivers/net/sfc/workarounds.h
index e0d63083c3a8..e4dd3a7f304b 100644
--- a/drivers/net/sfc/workarounds.h
+++ b/drivers/net/sfc/workarounds.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  * Driver for Solarflare Solarstorm network controllers and boards
- * Copyright 2006-2009 Solarflare Communications Inc.
+ * Copyright 2006-2010 Solarflare Communications Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published
diff --git a/drivers/net/sh_eth.c b/drivers/net/sh_eth.c
index 819c1750e2ab..095e52580884 100644
--- a/drivers/net/sh_eth.c
+++ b/drivers/net/sh_eth.c
@@ -32,10 +32,17 @@
 #include <linux/io.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
+#include <linux/ethtool.h>
 #include <asm/cacheflush.h>
 
 #include "sh_eth.h"
 
+#define SH_ETH_DEF_MSG_ENABLE \
+		(NETIF_MSG_LINK	| \
+		NETIF_MSG_TIMER	| \
+		NETIF_MSG_RX_ERR| \
+		NETIF_MSG_TX_ERR)
+
 /* There is CPU dependent code */
 #if defined(CONFIG_CPU_SUBTYPE_SH7724)
 #define SH_ETH_RESET_DEFAULT	1
@@ -817,6 +824,20 @@ static int sh_eth_rx(struct net_device *ndev)
 	return 0;
 }
 
+static void sh_eth_rcv_snd_disable(u32 ioaddr)
+{
+	/* disable tx and rx */
+	writel(readl(ioaddr + ECMR) &
+		~(ECMR_RE | ECMR_TE), ioaddr + ECMR);
+}
+
+static void sh_eth_rcv_snd_enable(u32 ioaddr)
+{
+	/* enable tx and rx */
+	writel(readl(ioaddr + ECMR) |
+		(ECMR_RE | ECMR_TE), ioaddr + ECMR);
+}
+
 /* error control function */
 static void sh_eth_error(struct net_device *ndev, int intr_status)
 {
@@ -843,11 +864,9 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 				if (mdp->ether_link_active_low)
 					link_stat = ~link_stat;
 			}
-			if (!(link_stat & PHY_ST_LINK)) {
-				/* Link Down : disable tx and rx */
-				writel(readl(ioaddr + ECMR) &
-					  ~(ECMR_RE | ECMR_TE), ioaddr + ECMR);
-			} else {
+			if (!(link_stat & PHY_ST_LINK))
+				sh_eth_rcv_snd_disable(ioaddr);
+			else {
 				/* Link Up */
 				writel(readl(ioaddr + EESIPR) &
 					  ~DMAC_M_ECI, ioaddr + EESIPR);
@@ -857,8 +876,7 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 				writel(readl(ioaddr + EESIPR) |
 					  DMAC_M_ECI, ioaddr + EESIPR);
 				/* enable tx and rx */
-				writel(readl(ioaddr + ECMR) |
-					  (ECMR_RE | ECMR_TE), ioaddr + ECMR);
+				sh_eth_rcv_snd_enable(ioaddr);
 			}
 		}
 	}
@@ -867,6 +885,8 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 		/* Write buck end. unused write back interrupt */
 		if (intr_status & EESR_TABT)	/* Transmit Abort int */
 			mdp->stats.tx_aborted_errors++;
+			if (netif_msg_tx_err(mdp))
+				dev_err(&ndev->dev, "Transmit Abort\n");
 	}
 
 	if (intr_status & EESR_RABT) {
@@ -874,14 +894,23 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 		if (intr_status & EESR_RFRMER) {
 			/* Receive Frame Overflow int */
 			mdp->stats.rx_frame_errors++;
-			dev_err(&ndev->dev, "Receive Frame Overflow\n");
+			if (netif_msg_rx_err(mdp))
+				dev_err(&ndev->dev, "Receive Abort\n");
 		}
 	}
 
-	if (!mdp->cd->no_ade) {
-		if (intr_status & EESR_ADE && intr_status & EESR_TDE &&
-		    intr_status & EESR_TFE)
-			mdp->stats.tx_fifo_errors++;
+	if (intr_status & EESR_TDE) {
+		/* Transmit Descriptor Empty int */
+		mdp->stats.tx_fifo_errors++;
+		if (netif_msg_tx_err(mdp))
+			dev_err(&ndev->dev, "Transmit Descriptor Empty\n");
+	}
+
+	if (intr_status & EESR_TFE) {
+		/* FIFO under flow */
+		mdp->stats.tx_fifo_errors++;
+		if (netif_msg_tx_err(mdp))
+			dev_err(&ndev->dev, "Transmit FIFO Under flow\n");
 	}
 
 	if (intr_status & EESR_RDE) {
@@ -890,12 +919,22 @@ static void sh_eth_error(struct net_device *ndev, int intr_status)
 
 		if (readl(ioaddr + EDRRR) ^ EDRRR_R)
 			writel(EDRRR_R, ioaddr + EDRRR);
-		dev_err(&ndev->dev, "Receive Descriptor Empty\n");
+		if (netif_msg_rx_err(mdp))
+			dev_err(&ndev->dev, "Receive Descriptor Empty\n");
 	}
+
 	if (intr_status & EESR_RFE) {
 		/* Receive FIFO Overflow int */
 		mdp->stats.rx_fifo_errors++;
-		dev_err(&ndev->dev, "Receive FIFO Overflow\n");
+		if (netif_msg_rx_err(mdp))
+			dev_err(&ndev->dev, "Receive FIFO Overflow\n");
+	}
+
+	if (!mdp->cd->no_ade && (intr_status & EESR_ADE)) {
+		/* Address Error */
+		mdp->stats.tx_fifo_errors++;
+		if (netif_msg_tx_err(mdp))
+			dev_err(&ndev->dev, "Address Error\n");
 	}
 
 	mask = EESR_TWB | EESR_TABT | EESR_ADE | EESR_TDE | EESR_TFE;
@@ -1012,7 +1051,7 @@ static void sh_eth_adjust_link(struct net_device *ndev)
 		mdp->duplex = -1;
 	}
 
-	if (new_state)
+	if (new_state && netif_msg_link(mdp))
 		phy_print_status(phydev);
 }
 
@@ -1063,6 +1102,132 @@ static int sh_eth_phy_start(struct net_device *ndev)
 	return 0;
 }
 
+static int sh_eth_get_settings(struct net_device *ndev,
+			struct ethtool_cmd *ecmd)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&mdp->lock, flags);
+	ret = phy_ethtool_gset(mdp->phydev, ecmd);
+	spin_unlock_irqrestore(&mdp->lock, flags);
+
+	return ret;
+}
+
+static int sh_eth_set_settings(struct net_device *ndev,
+		struct ethtool_cmd *ecmd)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	unsigned long flags;
+	int ret;
+	u32 ioaddr = ndev->base_addr;
+
+	spin_lock_irqsave(&mdp->lock, flags);
+
+	/* disable tx and rx */
+	sh_eth_rcv_snd_disable(ioaddr);
+
+	ret = phy_ethtool_sset(mdp->phydev, ecmd);
+	if (ret)
+		goto error_exit;
+
+	if (ecmd->duplex == DUPLEX_FULL)
+		mdp->duplex = 1;
+	else
+		mdp->duplex = 0;
+
+	if (mdp->cd->set_duplex)
+		mdp->cd->set_duplex(ndev);
+
+error_exit:
+	mdelay(1);
+
+	/* enable tx and rx */
+	sh_eth_rcv_snd_enable(ioaddr);
+
+	spin_unlock_irqrestore(&mdp->lock, flags);
+
+	return ret;
+}
+
+static int sh_eth_nway_reset(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&mdp->lock, flags);
+	ret = phy_start_aneg(mdp->phydev);
+	spin_unlock_irqrestore(&mdp->lock, flags);
+
+	return ret;
+}
+
+static u32 sh_eth_get_msglevel(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	return mdp->msg_enable;
+}
+
+static void sh_eth_set_msglevel(struct net_device *ndev, u32 value)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	mdp->msg_enable = value;
+}
+
+static const char sh_eth_gstrings_stats[][ETH_GSTRING_LEN] = {
+	"rx_current", "tx_current",
+	"rx_dirty", "tx_dirty",
+};
+#define SH_ETH_STATS_LEN  ARRAY_SIZE(sh_eth_gstrings_stats)
+
+static int sh_eth_get_sset_count(struct net_device *netdev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return SH_ETH_STATS_LEN;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void sh_eth_get_ethtool_stats(struct net_device *ndev,
+			struct ethtool_stats *stats, u64 *data)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	int i = 0;
+
+	/* device-specific stats */
+	data[i++] = mdp->cur_rx;
+	data[i++] = mdp->cur_tx;
+	data[i++] = mdp->dirty_rx;
+	data[i++] = mdp->dirty_tx;
+}
+
+static void sh_eth_get_strings(struct net_device *ndev, u32 stringset, u8 *data)
+{
+	switch (stringset) {
+	case ETH_SS_STATS:
+		memcpy(data, *sh_eth_gstrings_stats,
+					sizeof(sh_eth_gstrings_stats));
+		break;
+	}
+}
+
+static struct ethtool_ops sh_eth_ethtool_ops = {
+	.get_settings	= sh_eth_get_settings,
+	.set_settings	= sh_eth_set_settings,
+	.nway_reset		= sh_eth_nway_reset,
+	.get_msglevel	= sh_eth_get_msglevel,
+	.set_msglevel	= sh_eth_set_msglevel,
+	.get_link		= ethtool_op_get_link,
+	.get_strings	= sh_eth_get_strings,
+	.get_ethtool_stats  = sh_eth_get_ethtool_stats,
+	.get_sset_count     = sh_eth_get_sset_count,
+};
+
 /* network device open function */
 static int sh_eth_open(struct net_device *ndev)
 {
@@ -1073,8 +1238,8 @@ static int sh_eth_open(struct net_device *ndev)
 
 	ret = request_irq(ndev->irq, sh_eth_interrupt,
 #if defined(CONFIG_CPU_SUBTYPE_SH7763) || \
-    defined(CONFIG_CPU_SUBTYPE_SH7764) || \
-    defined(CONFIG_CPU_SUBTYPE_SH7757)
+	defined(CONFIG_CPU_SUBTYPE_SH7764) || \
+	defined(CONFIG_CPU_SUBTYPE_SH7757)
 				IRQF_SHARED,
 #else
 				0,
@@ -1123,8 +1288,8 @@ static void sh_eth_tx_timeout(struct net_device *ndev)
 
 	netif_stop_queue(ndev);
 
-	/* worning message out. */
-	printk(KERN_WARNING "%s: transmit timed out, status %8.8x,"
+	if (netif_msg_timer(mdp))
+		dev_err(&ndev->dev, "%s: transmit timed out, status %8.8x,"
 	       " resetting...\n", ndev->name, (int)readl(ioaddr + EESR));
 
 	/* tx_errors count up */
@@ -1167,6 +1332,8 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	spin_lock_irqsave(&mdp->lock, flags);
 	if ((mdp->cur_tx - mdp->dirty_tx) >= (TX_RING_SIZE - 4)) {
 		if (!sh_eth_txfree(ndev)) {
+			if (netif_msg_tx_queued(mdp))
+				dev_warn(&ndev->dev, "TxFD exhausted.\n");
 			netif_stop_queue(ndev);
 			spin_unlock_irqrestore(&mdp->lock, flags);
 			return NETDEV_TX_BUSY;
@@ -1497,8 +1664,11 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 
 	/* set function */
 	ndev->netdev_ops = &sh_eth_netdev_ops;
+	SET_ETHTOOL_OPS(ndev, &sh_eth_ethtool_ops);
 	ndev->watchdog_timeo = TX_TIMEOUT;
 
+	/* debug message level */
+	mdp->msg_enable = SH_ETH_DEF_MSG_ENABLE;
 	mdp->post_rx = POST_RX >> (devno << 1);
 	mdp->post_fw = POST_FW >> (devno << 1);
 
diff --git a/drivers/net/sis900.c b/drivers/net/sis900.c
index 640e368ebeee..84d4167eee9a 100644
--- a/drivers/net/sis900.c
+++ b/drivers/net/sis900.c
@@ -495,7 +495,7 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 	sis_priv->mii_info.reg_num_mask = 0x1f;
 
 	/* Get Mac address according to the chip revision */
-	pci_read_config_byte(pci_dev, PCI_CLASS_REVISION, &(sis_priv->chipset_rev));
+	sis_priv->chipset_rev = pci_dev->revision;
 	if(netif_msg_probe(sis_priv))
 		printk(KERN_DEBUG "%s: detected revision %2.2x, "
 				"trying to get MAC address...\n",
@@ -532,7 +532,7 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 	/* save our host bridge revision */
 	dev = pci_get_device(PCI_VENDOR_ID_SI, PCI_DEVICE_ID_SI_630, NULL);
 	if (dev) {
-		pci_read_config_byte(dev, PCI_CLASS_REVISION, &sis_priv->host_bridge_rev);
+		sis_priv->host_bridge_rev = dev->revision;
 		pci_dev_put(dev);
 	}
 
diff --git a/drivers/net/skge.c b/drivers/net/skge.c
index 42daf98ba736..35b28f42d208 100644
--- a/drivers/net/skge.c
+++ b/drivers/net/skge.c
@@ -3856,9 +3856,6 @@ static struct net_device *skge_devinit(struct skge_hw *hw, int port,
 	memcpy_fromio(dev->dev_addr, hw->regs + B2_MAC_1 + port*8, ETH_ALEN);
 	memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
-	/* device is off until link detection */
-	netif_carrier_off(dev);
-
 	return dev;
 }
 
diff --git a/drivers/net/smc91x.c b/drivers/net/smc91x.c
index 726df611ee17..43654a3bb0ec 100644
--- a/drivers/net/smc91x.c
+++ b/drivers/net/smc91x.c
@@ -81,6 +81,7 @@ static const char version[] =
 #include <linux/ethtool.h>
 #include <linux/mii.h>
 #include <linux/workqueue.h>
+#include <linux/of.h>
 
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
@@ -2394,6 +2395,15 @@ static int smc_drv_resume(struct device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id smc91x_match[] = {
+	{ .compatible = "smsc,lan91c94", },
+	{ .compatible = "smsc,lan91c111", },
+	{},
+}
+MODULE_DEVICE_TABLE(of, smc91x_match);
+#endif
+
 static struct dev_pm_ops smc_drv_pm_ops = {
 	.suspend	= smc_drv_suspend,
 	.resume		= smc_drv_resume,
@@ -2406,6 +2416,9 @@ static struct platform_driver smc_driver = {
 		.name	= CARDNAME,
 		.owner	= THIS_MODULE,
 		.pm	= &smc_drv_pm_ops,
+#ifdef CONFIG_OF
+		.of_match_table = smc91x_match,
+#endif
 	},
 };
 
diff --git a/drivers/net/smc91x.h b/drivers/net/smc91x.h
index ee747919a766..68d48ab6eacf 100644
--- a/drivers/net/smc91x.h
+++ b/drivers/net/smc91x.h
@@ -206,68 +206,6 @@ SMC_outw(u16 val, void __iomem *ioaddr, int reg)
 #define RPC_LSA_DEFAULT		RPC_LED_TX_RX
 #define RPC_LSB_DEFAULT		RPC_LED_100_10
 
-#elif   defined(CONFIG_MACH_LPD79520) ||	\
-	defined(CONFIG_MACH_LPD7A400) ||	\
-	defined(CONFIG_MACH_LPD7A404)
-
-/* The LPD7X_IOBARRIER is necessary to overcome a mismatch between the
- * way that the CPU handles chip selects and the way that the SMC chip
- * expects the chip select to operate.  Refer to
- * Documentation/arm/Sharp-LH/IOBarrier for details.  The read from
- * IOBARRIER is a byte, in order that we read the least-common
- * denominator.  It would be wasteful to read 32 bits from an 8-bit
- * accessible region.
- *
- * There is no explicit protection against interrupts intervening
- * between the writew and the IOBARRIER.  In SMC ISR there is a
- * preamble that performs an IOBARRIER in the extremely unlikely event
- * that the driver interrupts itself between a writew to the chip an
- * the IOBARRIER that follows *and* the cache is large enough that the
- * first off-chip access while handing the interrupt is to the SMC
- * chip.  Other devices in the same address space as the SMC chip must
- * be aware of the potential for trouble and perform a similar
- * IOBARRIER on entry to their ISR.
- */
-
-#include <mach/constants.h>	/* IOBARRIER_VIRT */
-
-#define SMC_CAN_USE_8BIT	0
-#define SMC_CAN_USE_16BIT	1
-#define SMC_CAN_USE_32BIT	0
-#define SMC_NOWAIT		0
-#define LPD7X_IOBARRIER		readb (IOBARRIER_VIRT)
-
-#define SMC_inw(a,r)\
-   ({ unsigned short v = readw ((void*) ((a) + (r))); LPD7X_IOBARRIER; v; })
-#define SMC_outw(v,a,r)	  ({ writew ((v), (a) + (r)); LPD7X_IOBARRIER; })
-
-#define SMC_insw		LPD7_SMC_insw
-static inline void LPD7_SMC_insw (unsigned char* a, int r,
-				  unsigned char* p, int l)
-{
-	unsigned short* ps = (unsigned short*) p;
-	while (l-- > 0) {
-		*ps++ = readw (a + r);
-		LPD7X_IOBARRIER;
-	}
-}
-
-#define SMC_outsw		LPD7_SMC_outsw
-static inline void LPD7_SMC_outsw (unsigned char* a, int r,
-				   unsigned char* p, int l)
-{
-	unsigned short* ps = (unsigned short*) p;
-	while (l-- > 0) {
-		writew (*ps++, a + r);
-		LPD7X_IOBARRIER;
-	}
-}
-
-#define SMC_INTERRUPT_PREAMBLE	LPD7X_IOBARRIER
-
-#define RPC_LSA_DEFAULT		RPC_LED_TX_RX
-#define RPC_LSB_DEFAULT		RPC_LED_100_10
-
 #elif	defined(CONFIG_ARCH_VERSATILE)
 
 #define SMC_CAN_USE_8BIT	1
diff --git a/drivers/net/smsc911x.c b/drivers/net/smsc911x.c
index 64bfdae5956f..d70bde95460b 100644
--- a/drivers/net/smsc911x.c
+++ b/drivers/net/smsc911x.c
@@ -1178,6 +1178,11 @@ static int smsc911x_open(struct net_device *dev)
 	smsc911x_reg_write(pdata, HW_CFG, 0x00050000);
 	smsc911x_reg_write(pdata, AFC_CFG, 0x006E3740);
 
+	/* Increase the legal frame size of VLAN tagged frames to 1522 bytes */
+	spin_lock_irq(&pdata->mac_lock);
+	smsc911x_mac_write(pdata, VLAN1, ETH_P_8021Q);
+	spin_unlock_irq(&pdata->mac_lock);
+
 	/* Make sure EEPROM has finished loading before setting GPIO_CFG */
 	timeout = 50;
 	while ((smsc911x_reg_read(pdata, E2P_CMD) & E2P_CMD_EPC_BUSY_) &&
diff --git a/drivers/net/sunbmac.c b/drivers/net/sunbmac.c
index 0a6a5ced3c1c..aa4765803a4c 100644
--- a/drivers/net/sunbmac.c
+++ b/drivers/net/sunbmac.c
@@ -1242,8 +1242,7 @@ fail_and_cleanup:
 /* QEC can be the parent of either QuadEthernet or a BigMAC.  We want
  * the latter.
  */
-static int __devinit bigmac_sbus_probe(struct platform_device *op,
-				       const struct of_device_id *match)
+static int __devinit bigmac_sbus_probe(struct platform_device *op)
 {
 	struct device *parent = op->dev.parent;
 	struct platform_device *qec_op;
@@ -1289,7 +1288,7 @@ static const struct of_device_id bigmac_sbus_match[] = {
 
 MODULE_DEVICE_TABLE(of, bigmac_sbus_match);
 
-static struct of_platform_driver bigmac_sbus_driver = {
+static struct platform_driver bigmac_sbus_driver = {
 	.driver = {
 		.name = "sunbmac",
 		.owner = THIS_MODULE,
@@ -1301,12 +1300,12 @@ static struct of_platform_driver bigmac_sbus_driver = {
 
 static int __init bigmac_init(void)
 {
-	return of_register_platform_driver(&bigmac_sbus_driver);
+	return platform_driver_register(&bigmac_sbus_driver);
 }
 
 static void __exit bigmac_exit(void)
 {
-	of_unregister_platform_driver(&bigmac_sbus_driver);
+	platform_driver_unregister(&bigmac_sbus_driver);
 }
 
 module_init(bigmac_init);
diff --git a/drivers/net/sungem.c b/drivers/net/sungem.c
index 1c5408f83937..c1a344829b54 100644
--- a/drivers/net/sungem.c
+++ b/drivers/net/sungem.c
@@ -320,28 +320,28 @@ static int gem_txmac_interrupt(struct net_device *dev, struct gem *gp, u32 gem_s
 
 	if (txmac_stat & MAC_TXSTAT_URUN) {
 		netdev_err(dev, "TX MAC xmit underrun\n");
-		gp->net_stats.tx_fifo_errors++;
+		dev->stats.tx_fifo_errors++;
 	}
 
 	if (txmac_stat & MAC_TXSTAT_MPE) {
 		netdev_err(dev, "TX MAC max packet size error\n");
-		gp->net_stats.tx_errors++;
+		dev->stats.tx_errors++;
 	}
 
 	/* The rest are all cases of one of the 16-bit TX
 	 * counters expiring.
 	 */
 	if (txmac_stat & MAC_TXSTAT_NCE)
-		gp->net_stats.collisions += 0x10000;
+		dev->stats.collisions += 0x10000;
 
 	if (txmac_stat & MAC_TXSTAT_ECE) {
-		gp->net_stats.tx_aborted_errors += 0x10000;
-		gp->net_stats.collisions += 0x10000;
+		dev->stats.tx_aborted_errors += 0x10000;
+		dev->stats.collisions += 0x10000;
 	}
 
 	if (txmac_stat & MAC_TXSTAT_LCE) {
-		gp->net_stats.tx_aborted_errors += 0x10000;
-		gp->net_stats.collisions += 0x10000;
+		dev->stats.tx_aborted_errors += 0x10000;
+		dev->stats.collisions += 0x10000;
 	}
 
 	/* We do not keep track of MAC_TXSTAT_FCE and
@@ -469,20 +469,20 @@ static int gem_rxmac_interrupt(struct net_device *dev, struct gem *gp, u32 gem_s
 		u32 smac = readl(gp->regs + MAC_SMACHINE);
 
 		netdev_err(dev, "RX MAC fifo overflow smac[%08x]\n", smac);
-		gp->net_stats.rx_over_errors++;
-		gp->net_stats.rx_fifo_errors++;
+		dev->stats.rx_over_errors++;
+		dev->stats.rx_fifo_errors++;
 
 		ret = gem_rxmac_reset(gp);
 	}
 
 	if (rxmac_stat & MAC_RXSTAT_ACE)
-		gp->net_stats.rx_frame_errors += 0x10000;
+		dev->stats.rx_frame_errors += 0x10000;
 
 	if (rxmac_stat & MAC_RXSTAT_CCE)
-		gp->net_stats.rx_crc_errors += 0x10000;
+		dev->stats.rx_crc_errors += 0x10000;
 
 	if (rxmac_stat & MAC_RXSTAT_LCE)
-		gp->net_stats.rx_length_errors += 0x10000;
+		dev->stats.rx_length_errors += 0x10000;
 
 	/* We do not track MAC_RXSTAT_FCE and MAC_RXSTAT_VCE
 	 * events.
@@ -594,7 +594,7 @@ static int gem_abnormal_irq(struct net_device *dev, struct gem *gp, u32 gem_stat
 		if (netif_msg_rx_err(gp))
 			printk(KERN_DEBUG "%s: no buffer for rx frame\n",
 				gp->dev->name);
-		gp->net_stats.rx_dropped++;
+		dev->stats.rx_dropped++;
 	}
 
 	if (gem_status & GREG_STAT_RXTAGERR) {
@@ -602,7 +602,7 @@ static int gem_abnormal_irq(struct net_device *dev, struct gem *gp, u32 gem_stat
 		if (netif_msg_rx_err(gp))
 			printk(KERN_DEBUG "%s: corrupt rx tag framing\n",
 				gp->dev->name);
-		gp->net_stats.rx_errors++;
+		dev->stats.rx_errors++;
 
 		goto do_reset;
 	}
@@ -684,7 +684,7 @@ static __inline__ void gem_tx(struct net_device *dev, struct gem *gp, u32 gem_st
 				break;
 		}
 		gp->tx_skbs[entry] = NULL;
-		gp->net_stats.tx_bytes += skb->len;
+		dev->stats.tx_bytes += skb->len;
 
 		for (frag = 0; frag <= skb_shinfo(skb)->nr_frags; frag++) {
 			txd = &gp->init_block->txd[entry];
@@ -696,7 +696,7 @@ static __inline__ void gem_tx(struct net_device *dev, struct gem *gp, u32 gem_st
 			entry = NEXT_TX(entry);
 		}
 
-		gp->net_stats.tx_packets++;
+		dev->stats.tx_packets++;
 		dev_kfree_skb_irq(skb);
 	}
 	gp->tx_old = entry;
@@ -738,6 +738,7 @@ static __inline__ void gem_post_rxds(struct gem *gp, int limit)
 
 static int gem_rx(struct gem *gp, int work_to_do)
 {
+	struct net_device *dev = gp->dev;
 	int entry, drops, work_done = 0;
 	u32 done;
 	__sum16 csum;
@@ -782,15 +783,15 @@ static int gem_rx(struct gem *gp, int work_to_do)
 
 		len = (status & RXDCTRL_BUFSZ) >> 16;
 		if ((len < ETH_ZLEN) || (status & RXDCTRL_BAD)) {
-			gp->net_stats.rx_errors++;
+			dev->stats.rx_errors++;
 			if (len < ETH_ZLEN)
-				gp->net_stats.rx_length_errors++;
+				dev->stats.rx_length_errors++;
 			if (len & RXDCTRL_BAD)
-				gp->net_stats.rx_crc_errors++;
+				dev->stats.rx_crc_errors++;
 
 			/* We'll just return it to GEM. */
 		drop_it:
-			gp->net_stats.rx_dropped++;
+			dev->stats.rx_dropped++;
 			goto next;
 		}
 
@@ -843,8 +844,8 @@ static int gem_rx(struct gem *gp, int work_to_do)
 
 		netif_receive_skb(skb);
 
-		gp->net_stats.rx_packets++;
-		gp->net_stats.rx_bytes += len;
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += len;
 
 	next:
 		entry = NEXT_RX(entry);
@@ -2472,7 +2473,6 @@ static int gem_resume(struct pci_dev *pdev)
 static struct net_device_stats *gem_get_stats(struct net_device *dev)
 {
 	struct gem *gp = netdev_priv(dev);
-	struct net_device_stats *stats = &gp->net_stats;
 
 	spin_lock_irq(&gp->lock);
 	spin_lock(&gp->tx_lock);
@@ -2481,17 +2481,17 @@ static struct net_device_stats *gem_get_stats(struct net_device *dev)
 	 * so we shield against this
 	 */
 	if (gp->running) {
-		stats->rx_crc_errors += readl(gp->regs + MAC_FCSERR);
+		dev->stats.rx_crc_errors += readl(gp->regs + MAC_FCSERR);
 		writel(0, gp->regs + MAC_FCSERR);
 
-		stats->rx_frame_errors += readl(gp->regs + MAC_AERR);
+		dev->stats.rx_frame_errors += readl(gp->regs + MAC_AERR);
 		writel(0, gp->regs + MAC_AERR);
 
-		stats->rx_length_errors += readl(gp->regs + MAC_LERR);
+		dev->stats.rx_length_errors += readl(gp->regs + MAC_LERR);
 		writel(0, gp->regs + MAC_LERR);
 
-		stats->tx_aborted_errors += readl(gp->regs + MAC_ECOLL);
-		stats->collisions +=
+		dev->stats.tx_aborted_errors += readl(gp->regs + MAC_ECOLL);
+		dev->stats.collisions +=
 			(readl(gp->regs + MAC_ECOLL) +
 			 readl(gp->regs + MAC_LCOLL));
 		writel(0, gp->regs + MAC_ECOLL);
@@ -2501,7 +2501,7 @@ static struct net_device_stats *gem_get_stats(struct net_device *dev)
 	spin_unlock(&gp->tx_lock);
 	spin_unlock_irq(&gp->lock);
 
-	return &gp->net_stats;
+	return &dev->stats;
 }
 
 static int gem_set_mac_address(struct net_device *dev, void *addr)
diff --git a/drivers/net/sungem.h b/drivers/net/sungem.h
index 19905460def6..d225077964e2 100644
--- a/drivers/net/sungem.h
+++ b/drivers/net/sungem.h
@@ -843,7 +843,7 @@ struct gem_txd {
 
 /* GEM requires that RX descriptors are provided four at a time,
  * aligned.  Also, the RX ring may not wrap around.  This means that
- * there will be at least 4 unused desciptor entries in the middle
+ * there will be at least 4 unused descriptor entries in the middle
  * of the RX ring at all times.
  *
  * Similar to HME, GEM assumes that it can write garbage bytes before
@@ -994,7 +994,6 @@ struct gem {
 	u32			status;
 
 	struct napi_struct	napi;
-	struct net_device_stats net_stats;
 
 	int			tx_fifo_sz;
 	int			rx_fifo_sz;
diff --git a/drivers/net/sunhme.c b/drivers/net/sunhme.c
index 55bbb9c15d96..eb4f59fb01e9 100644
--- a/drivers/net/sunhme.c
+++ b/drivers/net/sunhme.c
@@ -3237,11 +3237,15 @@ static void happy_meal_pci_exit(void)
 #endif
 
 #ifdef CONFIG_SBUS
-static int __devinit hme_sbus_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit hme_sbus_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	const char *model = of_get_property(dp, "model", NULL);
-	int is_qfe = (match->data != NULL);
+	int is_qfe;
+
+	if (!op->dev.of_match)
+		return -EINVAL;
+	is_qfe = (op->dev.of_match->data != NULL);
 
 	if (!is_qfe && model && !strcmp(model, "SUNW,sbus-qfe"))
 		is_qfe = 1;
@@ -3292,7 +3296,7 @@ static const struct of_device_id hme_sbus_match[] = {
 
 MODULE_DEVICE_TABLE(of, hme_sbus_match);
 
-static struct of_platform_driver hme_sbus_driver = {
+static struct platform_driver hme_sbus_driver = {
 	.driver = {
 		.name = "hme",
 		.owner = THIS_MODULE,
@@ -3306,7 +3310,7 @@ static int __init happy_meal_sbus_init(void)
 {
 	int err;
 
-	err = of_register_platform_driver(&hme_sbus_driver);
+	err = platform_driver_register(&hme_sbus_driver);
 	if (!err)
 		err = quattro_sbus_register_irqs();
 
@@ -3315,7 +3319,7 @@ static int __init happy_meal_sbus_init(void)
 
 static void happy_meal_sbus_exit(void)
 {
-	of_unregister_platform_driver(&hme_sbus_driver);
+	platform_driver_unregister(&hme_sbus_driver);
 	quattro_sbus_free_irqs();
 
 	while (qfe_sbus_list) {
diff --git a/drivers/net/sunlance.c b/drivers/net/sunlance.c
index 767e1e2b210d..32a5c7f63c43 100644
--- a/drivers/net/sunlance.c
+++ b/drivers/net/sunlance.c
@@ -1495,7 +1495,7 @@ fail:
 	return -ENODEV;
 }
 
-static int __devinit sunlance_sbus_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit sunlance_sbus_probe(struct platform_device *op)
 {
 	struct platform_device *parent = to_platform_device(op->dev.parent);
 	struct device_node *parent_dp = parent->dev.of_node;
@@ -1536,7 +1536,7 @@ static const struct of_device_id sunlance_sbus_match[] = {
 
 MODULE_DEVICE_TABLE(of, sunlance_sbus_match);
 
-static struct of_platform_driver sunlance_sbus_driver = {
+static struct platform_driver sunlance_sbus_driver = {
 	.driver = {
 		.name = "sunlance",
 		.owner = THIS_MODULE,
@@ -1550,12 +1550,12 @@ static struct of_platform_driver sunlance_sbus_driver = {
 /* Find all the lance cards on the system and initialize them */
 static int __init sparc_lance_init(void)
 {
-	return of_register_platform_driver(&sunlance_sbus_driver);
+	return platform_driver_register(&sunlance_sbus_driver);
 }
 
 static void __exit sparc_lance_exit(void)
 {
-	of_unregister_platform_driver(&sunlance_sbus_driver);
+	platform_driver_unregister(&sunlance_sbus_driver);
 }
 
 module_init(sparc_lance_init);
diff --git a/drivers/net/sunqe.c b/drivers/net/sunqe.c
index 9536b2f010be..18ecdc303751 100644
--- a/drivers/net/sunqe.c
+++ b/drivers/net/sunqe.c
@@ -941,7 +941,7 @@ fail:
 	return res;
 }
 
-static int __devinit qec_sbus_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit qec_sbus_probe(struct platform_device *op)
 {
 	return qec_ether_init(op);
 }
@@ -976,7 +976,7 @@ static const struct of_device_id qec_sbus_match[] = {
 
 MODULE_DEVICE_TABLE(of, qec_sbus_match);
 
-static struct of_platform_driver qec_sbus_driver = {
+static struct platform_driver qec_sbus_driver = {
 	.driver = {
 		.name = "qec",
 		.owner = THIS_MODULE,
@@ -988,12 +988,12 @@ static struct of_platform_driver qec_sbus_driver = {
 
 static int __init qec_init(void)
 {
-	return of_register_platform_driver(&qec_sbus_driver);
+	return platform_driver_register(&qec_sbus_driver);
 }
 
 static void __exit qec_exit(void)
 {
-	of_unregister_platform_driver(&qec_sbus_driver);
+	platform_driver_unregister(&qec_sbus_driver);
 
 	while (root_qec_dev) {
 		struct sunqec *next = root_qec_dev->next_module;
diff --git a/drivers/net/tg3.c b/drivers/net/tg3.c
index 06c0e5033656..6be418591df9 100644
--- a/drivers/net/tg3.c
+++ b/drivers/net/tg3.c
@@ -4,7 +4,7 @@
  * Copyright (C) 2001, 2002, 2003, 2004 David S. Miller (davem@redhat.com)
  * Copyright (C) 2001, 2002, 2003 Jeff Garzik (jgarzik@pobox.com)
  * Copyright (C) 2004 Sun Microsystems Inc.
- * Copyright (C) 2005-2010 Broadcom Corporation.
+ * Copyright (C) 2005-2011 Broadcom Corporation.
  *
  * Firmware is:
  *	Derived from proprietary unpublished source code,
@@ -64,10 +64,10 @@
 
 #define DRV_MODULE_NAME		"tg3"
 #define TG3_MAJ_NUM			3
-#define TG3_MIN_NUM			116
+#define TG3_MIN_NUM			117
 #define DRV_MODULE_VERSION	\
 	__stringify(TG3_MAJ_NUM) "." __stringify(TG3_MIN_NUM)
-#define DRV_MODULE_RELDATE	"December 3, 2010"
+#define DRV_MODULE_RELDATE	"January 25, 2011"
 
 #define TG3_DEF_MAC_MODE	0
 #define TG3_DEF_RX_MODE		0
@@ -1776,9 +1776,29 @@ static void tg3_phy_eee_adjust(struct tg3 *tp, u32 current_link_up)
 		tg3_phy_cl45_read(tp, MDIO_MMD_AN,
 				  TG3_CL45_D7_EEERES_STAT, &val);
 
-		if (val == TG3_CL45_D7_EEERES_STAT_LP_1000T ||
-		    val == TG3_CL45_D7_EEERES_STAT_LP_100TX)
+		switch (val) {
+		case TG3_CL45_D7_EEERES_STAT_LP_1000T:
+			switch (GET_ASIC_REV(tp->pci_chip_rev_id)) {
+			case ASIC_REV_5717:
+			case ASIC_REV_5719:
+			case ASIC_REV_57765:
+				/* Enable SM_DSP clock and tx 6dB coding. */
+				val = MII_TG3_AUXCTL_SHDWSEL_AUXCTL |
+				      MII_TG3_AUXCTL_ACTL_SMDSP_ENA |
+				      MII_TG3_AUXCTL_ACTL_TX_6DB;
+				tg3_writephy(tp, MII_TG3_AUX_CTRL, val);
+
+				tg3_phydsp_write(tp, MII_TG3_DSP_TAP26, 0x0000);
+
+				/* Turn off SM_DSP clock. */
+				val = MII_TG3_AUXCTL_SHDWSEL_AUXCTL |
+				      MII_TG3_AUXCTL_ACTL_TX_6DB;
+				tg3_writephy(tp, MII_TG3_AUX_CTRL, val);
+			}
+			/* Fallthrough */
+		case TG3_CL45_D7_EEERES_STAT_LP_100TX:
 			tp->setlpicnt = 2;
+		}
 	}
 
 	if (!tp->setlpicnt) {
@@ -2968,11 +2988,19 @@ static void tg3_phy_copper_begin(struct tg3 *tp)
 		      MII_TG3_AUXCTL_ACTL_TX_6DB;
 		tg3_writephy(tp, MII_TG3_AUX_CTRL, val);
 
-		if ((GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5717 ||
-		     GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_57765) &&
-		    !tg3_phydsp_read(tp, MII_TG3_DSP_CH34TP2, &val))
-			tg3_phydsp_write(tp, MII_TG3_DSP_CH34TP2,
-					 val | MII_TG3_DSP_CH34TP2_HIBW01);
+		switch (GET_ASIC_REV(tp->pci_chip_rev_id)) {
+		case ASIC_REV_5717:
+		case ASIC_REV_57765:
+			if (!tg3_phydsp_read(tp, MII_TG3_DSP_CH34TP2, &val))
+				tg3_phydsp_write(tp, MII_TG3_DSP_CH34TP2, val |
+						 MII_TG3_DSP_CH34TP2_HIBW01);
+			/* Fall through */
+		case ASIC_REV_5719:
+			val = MII_TG3_DSP_TAP26_ALNOKO |
+			      MII_TG3_DSP_TAP26_RMRXSTO |
+			      MII_TG3_DSP_TAP26_OPCSINPT;
+			tg3_phydsp_write(tp, MII_TG3_DSP_TAP26, val);
+		}
 
 		val = 0;
 		if (tp->link_config.autoneg == AUTONEG_ENABLE) {
@@ -7801,7 +7829,7 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 		       TG3_CPMU_DBTMR1_LNKIDLE_2047US);
 
 		tw32_f(TG3_CPMU_EEE_DBTMR2,
-		       TG3_CPMU_DBTMR1_APE_TX_2047US |
+		       TG3_CPMU_DBTMR2_APE_TX_2047US |
 		       TG3_CPMU_DBTMR2_TXIDXEQ_2047US);
 	}
 
@@ -8075,8 +8103,9 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 	/* Program the jumbo buffer descriptor ring control
 	 * blocks on those devices that have them.
 	 */
-	if ((tp->tg3_flags & TG3_FLAG_JUMBO_CAPABLE) &&
-	    !(tp->tg3_flags2 & TG3_FLG2_5780_CLASS)) {
+	if (tp->pci_chip_rev_id == CHIPREV_ID_5719_A0 ||
+	    ((tp->tg3_flags & TG3_FLAG_JUMBO_CAPABLE) &&
+	    !(tp->tg3_flags2 & TG3_FLG2_5780_CLASS))) {
 		/* Setup replenish threshold. */
 		tw32(RCVBDI_JUMBO_THRESH, tp->rx_jumbo_pending / 8);
 
@@ -8194,8 +8223,12 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 	    (tp->tg3_flags3 & TG3_FLG3_5717_PLUS)) {
 		val = tr32(TG3_RDMA_RSRVCTRL_REG);
 		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5719) {
-			val &= ~TG3_RDMA_RSRVCTRL_TXMRGN_MASK;
-			val |= TG3_RDMA_RSRVCTRL_TXMRGN_320B;
+			val &= ~(TG3_RDMA_RSRVCTRL_TXMRGN_MASK |
+				 TG3_RDMA_RSRVCTRL_FIFO_LWM_MASK |
+				 TG3_RDMA_RSRVCTRL_FIFO_HWM_MASK);
+			val |= TG3_RDMA_RSRVCTRL_TXMRGN_320B |
+			       TG3_RDMA_RSRVCTRL_FIFO_LWM_1_5K |
+			       TG3_RDMA_RSRVCTRL_FIFO_HWM_1_5K;
 		}
 		tw32(TG3_RDMA_RSRVCTRL_REG,
 		     val | TG3_RDMA_RSRVCTRL_FIFO_OFLW_FIX);
@@ -8317,7 +8350,8 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 	tw32_f(GRC_LOCAL_CTRL, tp->grc_local_ctrl);
 	udelay(100);
 
-	if (tp->tg3_flags2 & TG3_FLG2_USING_MSIX) {
+	if ((tp->tg3_flags2 & TG3_FLG2_USING_MSIX) &&
+		tp->irq_cnt > 1) {
 		val = tr32(MSGINT_MODE);
 		val |= MSGINT_MODE_MULTIVEC_EN | MSGINT_MODE_ENABLE;
 		tw32(MSGINT_MODE, val);
@@ -9057,7 +9091,8 @@ static void tg3_ints_init(struct tg3 *tp)
 
 	if (tp->tg3_flags2 & TG3_FLG2_USING_MSI_OR_MSIX) {
 		u32 msi_mode = tr32(MSGINT_MODE);
-		if (tp->tg3_flags2 & TG3_FLG2_USING_MSIX)
+		if ((tp->tg3_flags2 & TG3_FLG2_USING_MSIX) &&
+		    tp->irq_cnt > 1)
 			msi_mode |= MSGINT_MODE_MULTIVEC_EN;
 		tw32(MSGINT_MODE, msi_mode | MSGINT_MODE_ENABLE);
 	}
@@ -10833,13 +10868,16 @@ static int tg3_run_loopback(struct tg3 *tp, int loopback_mode)
 	if (loopback_mode == TG3_MAC_LOOPBACK) {
 		/* HW errata - mac loopback fails in some cases on 5780.
 		 * Normal traffic and PHY loopback are not affected by
-		 * errata.
+		 * errata.  Also, the MAC loopback test is deprecated for
+		 * all newer ASIC revisions.
 		 */
-		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5780)
+		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5780 ||
+		    (tp->tg3_flags & TG3_FLAG_CPMU_PRESENT))
 			return 0;
 
-		mac_mode = (tp->mac_mode & ~MAC_MODE_PORT_MODE_MASK) |
-			   MAC_MODE_PORT_INT_LPBACK;
+		mac_mode = tp->mac_mode &
+			   ~(MAC_MODE_PORT_MODE_MASK | MAC_MODE_HALF_DUPLEX);
+		mac_mode |= MAC_MODE_PORT_INT_LPBACK;
 		if (!(tp->tg3_flags2 & TG3_FLG2_5705_PLUS))
 			mac_mode |= MAC_MODE_LINK_POLARITY;
 		if (tp->phy_flags & TG3_PHYFLG_10_100_ONLY)
@@ -10861,7 +10899,8 @@ static int tg3_run_loopback(struct tg3 *tp, int loopback_mode)
 		tg3_writephy(tp, MII_BMCR, val);
 		udelay(40);
 
-		mac_mode = tp->mac_mode & ~MAC_MODE_PORT_MODE_MASK;
+		mac_mode = tp->mac_mode &
+			   ~(MAC_MODE_PORT_MODE_MASK | MAC_MODE_HALF_DUPLEX);
 		if (tp->phy_flags & TG3_PHYFLG_IS_FET) {
 			tg3_writephy(tp, MII_TG3_FET_PTEST,
 				     MII_TG3_FET_PTEST_FRC_TX_LINK |
@@ -10889,6 +10928,13 @@ static int tg3_run_loopback(struct tg3 *tp, int loopback_mode)
 				     MII_TG3_EXT_CTRL_LNK3_LED_MODE);
 		}
 		tw32(MAC_MODE, mac_mode);
+
+		/* Wait for link */
+		for (i = 0; i < 100; i++) {
+			if (tr32(MAC_TX_STATUS) & TX_STATUS_LINK_UP)
+				break;
+			mdelay(1);
+		}
 	} else {
 		return -EINVAL;
 	}
@@ -10995,14 +11041,19 @@ out:
 static int tg3_test_loopback(struct tg3 *tp)
 {
 	int err = 0;
-	u32 cpmuctrl = 0;
+	u32 eee_cap, cpmuctrl = 0;
 
 	if (!netif_running(tp->dev))
 		return TG3_LOOPBACK_FAILED;
 
+	eee_cap = tp->phy_flags & TG3_PHYFLG_EEE_CAP;
+	tp->phy_flags &= ~TG3_PHYFLG_EEE_CAP;
+
 	err = tg3_reset_hw(tp, 1);
-	if (err)
-		return TG3_LOOPBACK_FAILED;
+	if (err) {
+		err = TG3_LOOPBACK_FAILED;
+		goto done;
+	}
 
 	/* Turn off gphy autopowerdown. */
 	if (tp->phy_flags & TG3_PHYFLG_ENABLE_APD)
@@ -11022,8 +11073,10 @@ static int tg3_test_loopback(struct tg3 *tp)
 			udelay(10);
 		}
 
-		if (status != CPMU_MUTEX_GNT_DRIVER)
-			return TG3_LOOPBACK_FAILED;
+		if (status != CPMU_MUTEX_GNT_DRIVER) {
+			err = TG3_LOOPBACK_FAILED;
+			goto done;
+		}
 
 		/* Turn off link-based power management. */
 		cpmuctrl = tr32(TG3_CPMU_CTRL);
@@ -11052,6 +11105,9 @@ static int tg3_test_loopback(struct tg3 *tp)
 	if (tp->phy_flags & TG3_PHYFLG_ENABLE_APD)
 		tg3_phy_toggle_apd(tp, true);
 
+done:
+	tp->phy_flags |= eee_cap;
+
 	return err;
 }
 
@@ -12407,9 +12463,11 @@ static void __devinit tg3_get_eeprom_hw_cfg(struct tg3 *tp)
 			tp->tg3_flags3 |= TG3_FLG3_RGMII_EXT_IBND_TX_EN;
 	}
 done:
-	device_init_wakeup(&tp->pdev->dev, tp->tg3_flags & TG3_FLAG_WOL_CAP);
-	device_set_wakeup_enable(&tp->pdev->dev,
+	if (tp->tg3_flags & TG3_FLAG_WOL_CAP)
+		device_set_wakeup_enable(&tp->pdev->dev,
 				 tp->tg3_flags & TG3_FLAG_WOL_ENABLE);
+	else
+		device_set_wakeup_capable(&tp->pdev->dev, false);
 }
 
 static int __devinit tg3_issue_otp_command(struct tg3 *tp, u32 cmd)
@@ -13262,7 +13320,9 @@ static int __devinit tg3_get_invariants(struct tg3 *tp)
 	}
 
 	/* Determine TSO capabilities */
-	if (tp->tg3_flags3 & TG3_FLG3_5717_PLUS)
+	if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5719)
+		; /* Do nothing. HW bug. */
+	else if (tp->tg3_flags3 & TG3_FLG3_5717_PLUS)
 		tp->tg3_flags2 |= TG3_FLG2_HW_TSO_3;
 	else if ((tp->tg3_flags3 & TG3_FLG3_5755_PLUS) ||
 		 GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5906)
@@ -13313,7 +13373,8 @@ static int __devinit tg3_get_invariants(struct tg3 *tp)
 		tp->tg3_flags3 |= TG3_FLG3_40BIT_DMA_LIMIT_BUG;
 	}
 
-	if (tp->tg3_flags3 & TG3_FLG3_5717_PLUS)
+	if ((tp->tg3_flags3 & TG3_FLG3_5717_PLUS) &&
+	    GET_ASIC_REV(tp->pci_chip_rev_id) != ASIC_REV_5719)
 		tp->tg3_flags3 |= TG3_FLG3_USE_JUMBO_BDFLAG;
 
 	if (!(tp->tg3_flags2 & TG3_FLG2_5705_PLUS) ||
@@ -13331,42 +13392,8 @@ static int __devinit tg3_get_invariants(struct tg3 *tp)
 		tp->tg3_flags2 |= TG3_FLG2_PCI_EXPRESS;
 
 		tp->pcie_readrq = 4096;
-		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5719) {
-			u16 word;
-
-			pci_read_config_word(tp->pdev,
-					     tp->pcie_cap + PCI_EXP_LNKSTA,
-					     &word);
-			switch (word & PCI_EXP_LNKSTA_CLS) {
-			case PCI_EXP_LNKSTA_CLS_2_5GB:
-				word &= PCI_EXP_LNKSTA_NLW;
-				word >>= PCI_EXP_LNKSTA_NLW_SHIFT;
-				switch (word) {
-				case 2:
-					tp->pcie_readrq = 2048;
-					break;
-				case 4:
-					tp->pcie_readrq = 1024;
-					break;
-				}
-				break;
-
-			case PCI_EXP_LNKSTA_CLS_5_0GB:
-				word &= PCI_EXP_LNKSTA_NLW;
-				word >>= PCI_EXP_LNKSTA_NLW_SHIFT;
-				switch (word) {
-				case 1:
-					tp->pcie_readrq = 2048;
-					break;
-				case 2:
-					tp->pcie_readrq = 1024;
-					break;
-				case 4:
-					tp->pcie_readrq = 512;
-					break;
-				}
-			}
-		}
+		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5719)
+			tp->pcie_readrq = 2048;
 
 		pcie_set_readrq(tp->pdev, tp->pcie_readrq);
 
diff --git a/drivers/net/tg3.h b/drivers/net/tg3.h
index f528243e1a4f..73884b69b749 100644
--- a/drivers/net/tg3.h
+++ b/drivers/net/tg3.h
@@ -4,7 +4,7 @@
  * Copyright (C) 2001, 2002, 2003, 2004 David S. Miller (davem@redhat.com)
  * Copyright (C) 2001 Jeff Garzik (jgarzik@pobox.com)
  * Copyright (C) 2004 Sun Microsystems Inc.
- * Copyright (C) 2007-2010 Broadcom Corporation.
+ * Copyright (C) 2007-2011 Broadcom Corporation.
  */
 
 #ifndef _T3_H
@@ -141,6 +141,7 @@
 #define  CHIPREV_ID_57780_A1		 0x57780001
 #define  CHIPREV_ID_5717_A0		 0x05717000
 #define  CHIPREV_ID_57765_A0		 0x57785000
+#define  CHIPREV_ID_5719_A0		 0x05719000
 #define  GET_ASIC_REV(CHIP_REV_ID)	((CHIP_REV_ID) >> 12)
 #define   ASIC_REV_5700			 0x07
 #define   ASIC_REV_5701			 0x00
@@ -1105,7 +1106,7 @@
 #define  TG3_CPMU_DBTMR1_PCIEXIT_2047US	 0x07ff0000
 #define  TG3_CPMU_DBTMR1_LNKIDLE_2047US	 0x000070ff
 #define TG3_CPMU_EEE_DBTMR2		0x000036b8
-#define  TG3_CPMU_DBTMR1_APE_TX_2047US	 0x07ff0000
+#define  TG3_CPMU_DBTMR2_APE_TX_2047US	 0x07ff0000
 #define  TG3_CPMU_DBTMR2_TXIDXEQ_2047US	 0x000070ff
 #define TG3_CPMU_EEE_LNKIDL_CTRL	0x000036bc
 #define  TG3_CPMU_EEE_LNKIDL_PCIE_NL0	 0x01000000
@@ -1333,6 +1334,10 @@
 
 #define TG3_RDMA_RSRVCTRL_REG		0x00004900
 #define TG3_RDMA_RSRVCTRL_FIFO_OFLW_FIX	 0x00000004
+#define TG3_RDMA_RSRVCTRL_FIFO_LWM_1_5K	 0x00000c00
+#define TG3_RDMA_RSRVCTRL_FIFO_LWM_MASK	 0x00000ff0
+#define TG3_RDMA_RSRVCTRL_FIFO_HWM_1_5K	 0x000c0000
+#define TG3_RDMA_RSRVCTRL_FIFO_HWM_MASK	 0x000ff000
 #define TG3_RDMA_RSRVCTRL_TXMRGN_320B	 0x28000000
 #define TG3_RDMA_RSRVCTRL_TXMRGN_MASK	 0xffe00000
 /* 0x4904 --> 0x4910 unused */
@@ -2108,6 +2113,10 @@
 
 #define MII_TG3_DSP_TAP1		0x0001
 #define  MII_TG3_DSP_TAP1_AGCTGT_DFLT	0x0007
+#define MII_TG3_DSP_TAP26		0x001a
+#define  MII_TG3_DSP_TAP26_ALNOKO	0x0001
+#define  MII_TG3_DSP_TAP26_RMRXSTO	0x0002
+#define  MII_TG3_DSP_TAP26_OPCSINPT	0x0004
 #define MII_TG3_DSP_AADJ1CH0		0x001f
 #define MII_TG3_DSP_CH34TP2		0x4022
 #define MII_TG3_DSP_CH34TP2_HIBW01	0x0010
diff --git a/drivers/net/tlan.c b/drivers/net/tlan.c
index f8e463cd8ecc..ace6404e2fac 100644
--- a/drivers/net/tlan.c
+++ b/drivers/net/tlan.c
@@ -25,150 +25,9 @@
  *		Microchip Technology, 24C01A/02A/04A Data Sheet
  *			available in PDF format from www.microchip.com
  *
- * Change History
- *
- *	Tigran Aivazian <tigran@sco.com>:	TLan_PciProbe() now uses
- *						new PCI BIOS interface.
- *	Alan Cox	<alan@lxorguk.ukuu.org.uk>:
- *						Fixed the out of memory
- *						handling.
- *
- *	Torben Mathiasen <torben.mathiasen@compaq.com> New Maintainer!
- *
- *	v1.1 Dec 20, 1999    - Removed linux version checking
- *			       Patch from Tigran Aivazian.
- *			     - v1.1 includes Alan's SMP updates.
- *			     - We still have problems on SMP though,
- *			       but I'm looking into that.
- *
- *	v1.2 Jan 02, 2000    - Hopefully fixed the SMP deadlock.
- *			     - Removed dependency of HZ being 100.
- *			     - We now allow higher priority timers to
- *			       overwrite timers like TLAN_TIMER_ACTIVITY
- *			       Patch from John Cagle <john.cagle@compaq.com>.
- *			     - Fixed a few compiler warnings.
- *
- *	v1.3 Feb 04, 2000    - Fixed the remaining HZ issues.
- *			     - Removed call to pci_present().
- *			     - Removed SA_INTERRUPT flag from irq handler.
- *			     - Added __init and __initdata to reduce resisdent
- *			       code size.
- *			     - Driver now uses module_init/module_exit.
- *			     - Rewrote init_module and tlan_probe to
- *			       share a lot more code. We now use tlan_probe
- *			       with builtin and module driver.
- *			     - Driver ported to new net API.
- *			     - tlan.txt has been reworked to reflect current
- *			       driver (almost)
- *			     - Other minor stuff
- *
- *	v1.4 Feb 10, 2000    - Updated with more changes required after Dave's
- *	                       network cleanup in 2.3.43pre7 (Tigran & myself)
- *	                     - Minor stuff.
- *
- *	v1.5 March 22, 2000  - Fixed another timer bug that would hang the driver
- *			       if no cable/link were present.
- *			     - Cosmetic changes.
- *			     - TODO: Port completely to new PCI/DMA API
- *			     	     Auto-Neg fallback.
- *
- * 	v1.6 April 04, 2000  - Fixed driver support for kernel-parameters. Haven't
- * 			       tested it though, as the kernel support is currently
- * 			       broken (2.3.99p4p3).
- * 			     - Updated tlan.txt accordingly.
- * 			     - Adjusted minimum/maximum frame length.
- * 			     - There is now a TLAN website up at
- * 			       http://hp.sourceforge.net/ 
- *
- * 	v1.7 April 07, 2000  - Started to implement custom ioctls. Driver now
- * 			       reports PHY information when used with Donald
- * 			       Beckers userspace MII diagnostics utility.
- *
- * 	v1.8 April 23, 2000  - Fixed support for forced speed/duplex settings.
- * 			     - Added link information to Auto-Neg and forced
- * 			       modes. When NIC operates with auto-neg the driver
- * 			       will report Link speed & duplex modes as well as
- * 			       link partner abilities. When forced link is used,
- * 			       the driver will report status of the established
- * 			       link.
- * 			       Please read tlan.txt for additional information.
- * 			     - Removed call to check_region(), and used
- * 			       return value of request_region() instead.
- *
- *	v1.8a May 28, 2000   - Minor updates.
- *
- *	v1.9 July 25, 2000   - Fixed a few remaining Full-Duplex issues.
- *	                     - Updated with timer fixes from Andrew Morton.
- *	                     - Fixed module race in TLan_Open.
- *	                     - Added routine to monitor PHY status.
- *	                     - Added activity led support for Proliant devices.
- *
- *	v1.10 Aug 30, 2000   - Added support for EISA based tlan controllers
- *			       like the Compaq NetFlex3/E.
- *			     - Rewrote tlan_probe to better handle multiple
- *			       bus probes. Probing and device setup is now
- *			       done through TLan_Probe and TLan_init_one. Actual
- *			       hardware probe is done with kernel API and
- *			       TLan_EisaProbe.
- *			     - Adjusted debug information for probing.
- *			     - Fixed bug that would cause general debug information
- *			       to be printed after driver removal.
- *			     - Added transmit timeout handling.
- *			     - Fixed OOM return values in tlan_probe.
- *			     - Fixed possible mem leak in tlan_exit
- *			       (now tlan_remove_one).
- *			     - Fixed timer bug in TLan_phyMonitor.
- *			     - This driver version is alpha quality, please
- *			       send me any bug issues you may encounter.
- *
- *	v1.11 Aug 31, 2000   - Do not try to register irq 0 if no irq line was
- *			       set for EISA cards.
- *			     - Added support for NetFlex3/E with nibble-rate
- *			       10Base-T PHY. This is untestet as I haven't got
- *			       one of these cards.
- *			     - Fixed timer being added twice.
- *			     - Disabled PhyMonitoring by default as this is
- *			       work in progress. Define MONITOR to enable it.
- *			     - Now we don't display link info with PHYs that
- *			       doesn't support it (level1).
- *			     - Incresed tx_timeout beacuse of auto-neg.
- *			     - Adjusted timers for forced speeds.
- *
- *	v1.12 Oct 12, 2000   - Minor fixes (memleak, init, etc.)
- *
- * 	v1.13 Nov 28, 2000   - Stop flooding console with auto-neg issues
- * 			       when link can't be established.
- *			     - Added the bbuf option as a kernel parameter.
- *			     - Fixed ioaddr probe bug.
- *			     - Fixed stupid deadlock with MII interrupts.
- *			     - Added support for speed/duplex selection with
- *			       multiple nics.
- *			     - Added partly fix for TX Channel lockup with
- *			       TLAN v1.0 silicon. This needs to be investigated
- *			       further.
- *
- * 	v1.14 Dec 16, 2000   - Added support for servicing multiple frames per.
- * 			       interrupt. Thanks goes to
- * 			       Adam Keys <adam@ti.com>
- * 			       Denis Beaudoin <dbeaudoin@ti.com>
- * 			       for providing the patch.
- * 			     - Fixed auto-neg output when using multiple
- * 			       adapters.
- * 			     - Converted to use new taskq interface.
- *
- * 	v1.14a Jan 6, 2001   - Minor adjustments (spinlocks, etc.)
- *
- *	Samuel Chessman <chessman@tux.org> New Maintainer!
- *
- *	v1.15 Apr 4, 2002    - Correct operation when aui=1 to be
- *	                       10T half duplex no loopback
- *	                       Thanks to Gunnar Eikman
- *
- *	Sakari Ailus <sakari.ailus@iki.fi>:
- *
- *	v1.15a Dec 15 2008   - Remove bbuf support, it doesn't work anyway.
- *
- *******************************************************************************/
+ ******************************************************************************/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
 #include <linux/init.h>
@@ -185,13 +44,11 @@
 
 #include "tlan.h"
 
-typedef u32 (TLanIntVectorFunc)( struct net_device *, u16 );
-
 
 /* For removing EISA devices */
-static	struct net_device	*TLan_Eisa_Devices;
+static	struct net_device	*tlan_eisa_devices;
 
-static	int		TLanDevicesInstalled;
+static	int		tlan_devices_installed;
 
 /* Set speed, duplex and aui settings */
 static  int aui[MAX_TLAN_BOARDS];
@@ -202,8 +59,9 @@ module_param_array(aui, int, NULL, 0);
 module_param_array(duplex, int, NULL, 0);
 module_param_array(speed, int, NULL, 0);
 MODULE_PARM_DESC(aui, "ThunderLAN use AUI port(s) (0-1)");
-MODULE_PARM_DESC(duplex, "ThunderLAN duplex setting(s) (0-default, 1-half, 2-full)");
-MODULE_PARM_DESC(speed, "ThunderLAN port speen setting(s) (0,10,100)");
+MODULE_PARM_DESC(duplex,
+		 "ThunderLAN duplex setting(s) (0-default, 1-half, 2-full)");
+MODULE_PARM_DESC(speed, "ThunderLAN port speed setting(s) (0,10,100)");
 
 MODULE_AUTHOR("Maintainer: Samuel Chessman <chessman@tux.org>");
 MODULE_DESCRIPTION("Driver for TI ThunderLAN based ethernet PCI adapters");
@@ -218,139 +76,144 @@ static  int		debug;
 module_param(debug, int, 0);
 MODULE_PARM_DESC(debug, "ThunderLAN debug mask");
 
-static	const char TLanSignature[] = "TLAN";
-static  const char tlan_banner[] = "ThunderLAN driver v1.15a\n";
+static	const char tlan_signature[] = "TLAN";
+static  const char tlan_banner[] = "ThunderLAN driver v1.17\n";
 static  int tlan_have_pci;
 static  int tlan_have_eisa;
 
-static const char *media[] = {
-	"10BaseT-HD ", "10BaseT-FD ","100baseTx-HD ",
-	"100baseTx-FD", "100baseT4", NULL
+static const char * const media[] = {
+	"10BaseT-HD", "10BaseT-FD", "100baseTx-HD",
+	"100BaseTx-FD", "100BaseT4", NULL
 };
 
 static struct board {
-	const char	*deviceLabel;
-	u32	   	flags;
-	u16	   	addrOfs;
+	const char	*device_label;
+	u32		flags;
+	u16		addr_ofs;
 } board_info[] = {
 	{ "Compaq Netelligent 10 T PCI UTP", TLAN_ADAPTER_ACTIVITY_LED, 0x83 },
-	{ "Compaq Netelligent 10/100 TX PCI UTP", TLAN_ADAPTER_ACTIVITY_LED, 0x83 },
+	{ "Compaq Netelligent 10/100 TX PCI UTP",
+	  TLAN_ADAPTER_ACTIVITY_LED, 0x83 },
 	{ "Compaq Integrated NetFlex-3/P", TLAN_ADAPTER_NONE, 0x83 },
 	{ "Compaq NetFlex-3/P",
 	  TLAN_ADAPTER_UNMANAGED_PHY | TLAN_ADAPTER_BIT_RATE_PHY, 0x83 },
 	{ "Compaq NetFlex-3/P", TLAN_ADAPTER_NONE, 0x83 },
 	{ "Compaq Netelligent Integrated 10/100 TX UTP",
 	  TLAN_ADAPTER_ACTIVITY_LED, 0x83 },
-	{ "Compaq Netelligent Dual 10/100 TX PCI UTP", TLAN_ADAPTER_NONE, 0x83 },
-	{ "Compaq Netelligent 10/100 TX Embedded UTP", TLAN_ADAPTER_NONE, 0x83 },
+	{ "Compaq Netelligent Dual 10/100 TX PCI UTP",
+	  TLAN_ADAPTER_NONE, 0x83 },
+	{ "Compaq Netelligent 10/100 TX Embedded UTP",
+	  TLAN_ADAPTER_NONE, 0x83 },
 	{ "Olicom OC-2183/2185", TLAN_ADAPTER_USE_INTERN_10, 0x83 },
-	{ "Olicom OC-2325", TLAN_ADAPTER_UNMANAGED_PHY, 0xF8 },
-	{ "Olicom OC-2326", TLAN_ADAPTER_USE_INTERN_10, 0xF8 },
+	{ "Olicom OC-2325", TLAN_ADAPTER_UNMANAGED_PHY, 0xf8 },
+	{ "Olicom OC-2326", TLAN_ADAPTER_USE_INTERN_10, 0xf8 },
 	{ "Compaq Netelligent 10/100 TX UTP", TLAN_ADAPTER_ACTIVITY_LED, 0x83 },
-	{ "Compaq Netelligent 10 T/2 PCI UTP/Coax", TLAN_ADAPTER_NONE, 0x83 },
+	{ "Compaq Netelligent 10 T/2 PCI UTP/coax", TLAN_ADAPTER_NONE, 0x83 },
 	{ "Compaq NetFlex-3/E",
-	  TLAN_ADAPTER_ACTIVITY_LED | 	/* EISA card */
+	  TLAN_ADAPTER_ACTIVITY_LED |	/* EISA card */
 	  TLAN_ADAPTER_UNMANAGED_PHY | TLAN_ADAPTER_BIT_RATE_PHY, 0x83 },
-	{ "Compaq NetFlex-3/E", TLAN_ADAPTER_ACTIVITY_LED, 0x83 }, /* EISA card */
+	{ "Compaq NetFlex-3/E",
+	  TLAN_ADAPTER_ACTIVITY_LED, 0x83 }, /* EISA card */
 };
 
 static DEFINE_PCI_DEVICE_TABLE(tlan_pci_tbl) = {
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETEL10,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETEL100,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 1 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 1 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETFLEX3I,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 2 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 2 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_THUNDER,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 3 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 3 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETFLEX3B,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 4 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 4 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETEL100PI,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 5 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 5 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETEL100D,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 6 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 6 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_COMPAQ_NETEL100I,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 7 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 7 },
 	{ PCI_VENDOR_ID_OLICOM, PCI_DEVICE_ID_OLICOM_OC2183,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 8 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 8 },
 	{ PCI_VENDOR_ID_OLICOM, PCI_DEVICE_ID_OLICOM_OC2325,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 9 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 9 },
 	{ PCI_VENDOR_ID_OLICOM, PCI_DEVICE_ID_OLICOM_OC2326,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 10 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 10 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_NETELLIGENT_10_100_WS_5100,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 11 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 11 },
 	{ PCI_VENDOR_ID_COMPAQ, PCI_DEVICE_ID_NETELLIGENT_10_T2,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 12 },
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 12 },
 	{ 0,}
 };
 MODULE_DEVICE_TABLE(pci, tlan_pci_tbl);
 
-static void	TLan_EisaProbe( void );
-static void	TLan_Eisa_Cleanup( void );
-static int      TLan_Init( struct net_device * );
-static int	TLan_Open( struct net_device *dev );
-static netdev_tx_t TLan_StartTx( struct sk_buff *, struct net_device *);
-static irqreturn_t TLan_HandleInterrupt( int, void *);
-static int	TLan_Close( struct net_device *);
-static struct	net_device_stats *TLan_GetStats( struct net_device *);
-static void	TLan_SetMulticastList( struct net_device *);
-static int	TLan_ioctl( struct net_device *dev, struct ifreq *rq, int cmd);
-static int      TLan_probe1( struct pci_dev *pdev, long ioaddr,
-			     int irq, int rev, const struct pci_device_id *ent);
-static void	TLan_tx_timeout( struct net_device *dev);
-static void	TLan_tx_timeout_work(struct work_struct *work);
-static int 	tlan_init_one( struct pci_dev *pdev, const struct pci_device_id *ent);
-
-static u32	TLan_HandleTxEOF( struct net_device *, u16 );
-static u32	TLan_HandleStatOverflow( struct net_device *, u16 );
-static u32	TLan_HandleRxEOF( struct net_device *, u16 );
-static u32	TLan_HandleDummy( struct net_device *, u16 );
-static u32	TLan_HandleTxEOC( struct net_device *, u16 );
-static u32	TLan_HandleStatusCheck( struct net_device *, u16 );
-static u32	TLan_HandleRxEOC( struct net_device *, u16 );
-
-static void	TLan_Timer( unsigned long );
-
-static void	TLan_ResetLists( struct net_device * );
-static void	TLan_FreeLists( struct net_device * );
-static void	TLan_PrintDio( u16 );
-static void	TLan_PrintList( TLanList *, char *, int );
-static void	TLan_ReadAndClearStats( struct net_device *, int );
-static void	TLan_ResetAdapter( struct net_device * );
-static void	TLan_FinishReset( struct net_device * );
-static void	TLan_SetMac( struct net_device *, int areg, char *mac );
-
-static void	TLan_PhyPrint( struct net_device * );
-static void	TLan_PhyDetect( struct net_device * );
-static void	TLan_PhyPowerDown( struct net_device * );
-static void	TLan_PhyPowerUp( struct net_device * );
-static void	TLan_PhyReset( struct net_device * );
-static void	TLan_PhyStartLink( struct net_device * );
-static void	TLan_PhyFinishAutoNeg( struct net_device * );
+static void	tlan_eisa_probe(void);
+static void	tlan_eisa_cleanup(void);
+static int      tlan_init(struct net_device *);
+static int	tlan_open(struct net_device *dev);
+static netdev_tx_t tlan_start_tx(struct sk_buff *, struct net_device *);
+static irqreturn_t tlan_handle_interrupt(int, void *);
+static int	tlan_close(struct net_device *);
+static struct	net_device_stats *tlan_get_stats(struct net_device *);
+static void	tlan_set_multicast_list(struct net_device *);
+static int	tlan_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+static int      tlan_probe1(struct pci_dev *pdev, long ioaddr,
+			    int irq, int rev, const struct pci_device_id *ent);
+static void	tlan_tx_timeout(struct net_device *dev);
+static void	tlan_tx_timeout_work(struct work_struct *work);
+static int	tlan_init_one(struct pci_dev *pdev,
+			      const struct pci_device_id *ent);
+
+static u32	tlan_handle_tx_eof(struct net_device *, u16);
+static u32	tlan_handle_stat_overflow(struct net_device *, u16);
+static u32	tlan_handle_rx_eof(struct net_device *, u16);
+static u32	tlan_handle_dummy(struct net_device *, u16);
+static u32	tlan_handle_tx_eoc(struct net_device *, u16);
+static u32	tlan_handle_status_check(struct net_device *, u16);
+static u32	tlan_handle_rx_eoc(struct net_device *, u16);
+
+static void	tlan_timer(unsigned long);
+
+static void	tlan_reset_lists(struct net_device *);
+static void	tlan_free_lists(struct net_device *);
+static void	tlan_print_dio(u16);
+static void	tlan_print_list(struct tlan_list *, char *, int);
+static void	tlan_read_and_clear_stats(struct net_device *, int);
+static void	tlan_reset_adapter(struct net_device *);
+static void	tlan_finish_reset(struct net_device *);
+static void	tlan_set_mac(struct net_device *, int areg, char *mac);
+
+static void	tlan_phy_print(struct net_device *);
+static void	tlan_phy_detect(struct net_device *);
+static void	tlan_phy_power_down(struct net_device *);
+static void	tlan_phy_power_up(struct net_device *);
+static void	tlan_phy_reset(struct net_device *);
+static void	tlan_phy_start_link(struct net_device *);
+static void	tlan_phy_finish_auto_neg(struct net_device *);
 #ifdef MONITOR
-static void     TLan_PhyMonitor( struct net_device * );
+static void     tlan_phy_monitor(struct net_device *);
 #endif
 
 /*
-static int	TLan_PhyNop( struct net_device * );
-static int	TLan_PhyInternalCheck( struct net_device * );
-static int	TLan_PhyInternalService( struct net_device * );
-static int	TLan_PhyDp83840aCheck( struct net_device * );
+  static int	tlan_phy_nop(struct net_device *);
+  static int	tlan_phy_internal_check(struct net_device *);
+  static int	tlan_phy_internal_service(struct net_device *);
+  static int	tlan_phy_dp83840a_check(struct net_device *);
 */
 
-static bool	TLan_MiiReadReg( struct net_device *, u16, u16, u16 * );
-static void	TLan_MiiSendData( u16, u32, unsigned );
-static void	TLan_MiiSync( u16 );
-static void	TLan_MiiWriteReg( struct net_device *, u16, u16, u16 );
+static bool	tlan_mii_read_reg(struct net_device *, u16, u16, u16 *);
+static void	tlan_mii_send_data(u16, u32, unsigned);
+static void	tlan_mii_sync(u16);
+static void	tlan_mii_write_reg(struct net_device *, u16, u16, u16);
 
-static void	TLan_EeSendStart( u16 );
-static int	TLan_EeSendByte( u16, u8, int );
-static void	TLan_EeReceiveByte( u16, u8 *, int );
-static int	TLan_EeReadByte( struct net_device *, u8, u8 * );
+static void	tlan_ee_send_start(u16);
+static int	tlan_ee_send_byte(u16, u8, int);
+static void	tlan_ee_receive_byte(u16, u8 *, int);
+static int	tlan_ee_read_byte(struct net_device *, u8, u8 *);
 
 
 static inline void
-TLan_StoreSKB( struct tlan_list_tag *tag, struct sk_buff *skb)
+tlan_store_skb(struct tlan_list *tag, struct sk_buff *skb)
 {
 	unsigned long addr = (unsigned long)skb;
 	tag->buffer[9].address = addr;
@@ -358,7 +221,7 @@ TLan_StoreSKB( struct tlan_list_tag *tag, struct sk_buff *skb)
 }
 
 static inline struct sk_buff *
-TLan_GetSKB( const struct tlan_list_tag *tag)
+tlan_get_skb(const struct tlan_list *tag)
 {
 	unsigned long addr;
 
@@ -367,50 +230,50 @@ TLan_GetSKB( const struct tlan_list_tag *tag)
 	return (struct sk_buff *) addr;
 }
 
-
-static TLanIntVectorFunc *TLanIntVector[TLAN_INT_NUMBER_OF_INTS] = {
+static u32
+(*tlan_int_vector[TLAN_INT_NUMBER_OF_INTS])(struct net_device *, u16) = {
 	NULL,
-	TLan_HandleTxEOF,
-	TLan_HandleStatOverflow,
-	TLan_HandleRxEOF,
-	TLan_HandleDummy,
-	TLan_HandleTxEOC,
-	TLan_HandleStatusCheck,
-	TLan_HandleRxEOC
+	tlan_handle_tx_eof,
+	tlan_handle_stat_overflow,
+	tlan_handle_rx_eof,
+	tlan_handle_dummy,
+	tlan_handle_tx_eoc,
+	tlan_handle_status_check,
+	tlan_handle_rx_eoc
 };
 
 static inline void
-TLan_SetTimer( struct net_device *dev, u32 ticks, u32 type )
+tlan_set_timer(struct net_device *dev, u32 ticks, u32 type)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	unsigned long flags = 0;
 
 	if (!in_irq())
 		spin_lock_irqsave(&priv->lock, flags);
-	if ( priv->timer.function != NULL &&
-		priv->timerType != TLAN_TIMER_ACTIVITY ) {
+	if (priv->timer.function != NULL &&
+	    priv->timer_type != TLAN_TIMER_ACTIVITY) {
 		if (!in_irq())
 			spin_unlock_irqrestore(&priv->lock, flags);
 		return;
 	}
-	priv->timer.function = TLan_Timer;
+	priv->timer.function = tlan_timer;
 	if (!in_irq())
 		spin_unlock_irqrestore(&priv->lock, flags);
 
 	priv->timer.data = (unsigned long) dev;
-	priv->timerSetAt = jiffies;
-	priv->timerType = type;
+	priv->timer_set_at = jiffies;
+	priv->timer_type = type;
 	mod_timer(&priv->timer, jiffies + ticks);
 
-} /* TLan_SetTimer */
+}
 
 
 /*****************************************************************************
 ******************************************************************************
 
-	ThunderLAN Driver Primary Functions
+ThunderLAN driver primary functions
 
-	These functions are more or less common to all Linux network drivers.
+these functions are more or less common to all linux network drivers.
 
 ******************************************************************************
 *****************************************************************************/
@@ -419,56 +282,124 @@ TLan_SetTimer( struct net_device *dev, u32 ticks, u32 type )
 
 
 
-	/***************************************************************
-	 *	tlan_remove_one
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		None
-	 *
-	 *	Goes through the TLanDevices list and frees the device
-	 *	structs and memory associated with each device (lists
-	 *	and buffers).  It also ureserves the IO port regions
-	 *	associated with this device.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_remove_one
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		None
+ *
+ *	Goes through the TLanDevices list and frees the device
+ *	structs and memory associated with each device (lists
+ *	and buffers).  It also ureserves the IO port regions
+ *	associated with this device.
+ *
+ **************************************************************/
 
 
-static void __devexit tlan_remove_one( struct pci_dev *pdev)
+static void __devexit tlan_remove_one(struct pci_dev *pdev)
 {
-	struct net_device *dev = pci_get_drvdata( pdev );
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 
-	unregister_netdev( dev );
+	unregister_netdev(dev);
 
-	if ( priv->dmaStorage ) {
-		pci_free_consistent(priv->pciDev,
-				    priv->dmaSize, priv->dmaStorage,
-				    priv->dmaStorageDMA );
+	if (priv->dma_storage) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->dma_size, priv->dma_storage,
+				    priv->dma_storage_dma);
 	}
 
 #ifdef CONFIG_PCI
 	pci_release_regions(pdev);
 #endif
 
-	free_netdev( dev );
+	free_netdev(dev);
 
-	pci_set_drvdata( pdev, NULL );
+	pci_set_drvdata(pdev, NULL);
 }
 
+static void tlan_start(struct net_device *dev)
+{
+	tlan_reset_lists(dev);
+	/* NOTE: It might not be necessary to read the stats before a
+	   reset if you don't care what the values are.
+	*/
+	tlan_read_and_clear_stats(dev, TLAN_IGNORE);
+	tlan_reset_adapter(dev);
+	netif_wake_queue(dev);
+}
+
+static void tlan_stop(struct net_device *dev)
+{
+	struct tlan_priv *priv = netdev_priv(dev);
+
+	tlan_read_and_clear_stats(dev, TLAN_RECORD);
+	outl(TLAN_HC_AD_RST, dev->base_addr + TLAN_HOST_CMD);
+	/* Reset and power down phy */
+	tlan_reset_adapter(dev);
+	if (priv->timer.function != NULL) {
+		del_timer_sync(&priv->timer);
+		priv->timer.function = NULL;
+	}
+}
+
+#ifdef CONFIG_PM
+
+static int tlan_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+
+	if (netif_running(dev))
+		tlan_stop(dev);
+
+	netif_device_detach(dev);
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_wake_from_d3(pdev, false);
+	pci_set_power_state(pdev, PCI_D3hot);
+
+	return 0;
+}
+
+static int tlan_resume(struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+	pci_enable_wake(pdev, 0, 0);
+	netif_device_attach(dev);
+
+	if (netif_running(dev))
+		tlan_start(dev);
+
+	return 0;
+}
+
+#else /* CONFIG_PM */
+
+#define tlan_suspend   NULL
+#define tlan_resume    NULL
+
+#endif /* CONFIG_PM */
+
+
 static struct pci_driver tlan_driver = {
 	.name		= "tlan",
 	.id_table	= tlan_pci_tbl,
 	.probe		= tlan_init_one,
 	.remove		= __devexit_p(tlan_remove_one),
+	.suspend	= tlan_suspend,
+	.resume		= tlan_resume,
 };
 
 static int __init tlan_probe(void)
 {
 	int rc = -ENODEV;
 
-	printk(KERN_INFO "%s", tlan_banner);
+	pr_info("%s", tlan_banner);
 
 	TLAN_DBG(TLAN_DEBUG_PROBE, "Starting PCI Probe....\n");
 
@@ -477,18 +408,18 @@ static int __init tlan_probe(void)
 	rc = pci_register_driver(&tlan_driver);
 
 	if (rc != 0) {
-		printk(KERN_ERR "TLAN: Could not register pci driver.\n");
+		pr_err("Could not register pci driver\n");
 		goto err_out_pci_free;
 	}
 
 	TLAN_DBG(TLAN_DEBUG_PROBE, "Starting EISA Probe....\n");
-	TLan_EisaProbe();
+	tlan_eisa_probe();
 
-	printk(KERN_INFO "TLAN: %d device%s installed, PCI: %d  EISA: %d\n",
-		 TLanDevicesInstalled, TLanDevicesInstalled == 1 ? "" : "s",
-		 tlan_have_pci, tlan_have_eisa);
+	pr_info("%d device%s installed, PCI: %d  EISA: %d\n",
+		tlan_devices_installed, tlan_devices_installed == 1 ? "" : "s",
+		tlan_have_pci, tlan_have_eisa);
 
-	if (TLanDevicesInstalled == 0) {
+	if (tlan_devices_installed == 0) {
 		rc = -ENODEV;
 		goto  err_out_pci_unreg;
 	}
@@ -501,39 +432,39 @@ err_out_pci_free:
 }
 
 
-static int __devinit tlan_init_one( struct pci_dev *pdev,
-				    const struct pci_device_id *ent)
+static int __devinit tlan_init_one(struct pci_dev *pdev,
+				   const struct pci_device_id *ent)
 {
-	return TLan_probe1( pdev, -1, -1, 0, ent);
+	return tlan_probe1(pdev, -1, -1, 0, ent);
 }
 
 
 /*
-	***************************************************************
-	 *	tlan_probe1
-	 *
-	 *	Returns:
-	 *		0 on success, error code on error
-	 *	Parms:
-	 *		none
-	 *
-	 *	The name is lower case to fit in with all the rest of
-	 *	the netcard_probe names.  This function looks for
-	 *	another TLan based adapter, setting it up with the
-	 *	allocated device struct if one is found.
-	 *	tlan_probe has been ported to the new net API and
-	 *	now allocates its own device structure. This function
-	 *	is also used by modules.
-	 *
-	 **************************************************************/
-
-static int __devinit TLan_probe1(struct pci_dev *pdev,
+***************************************************************
+*	tlan_probe1
+*
+*	Returns:
+*		0 on success, error code on error
+*	Parms:
+*		none
+*
+*	The name is lower case to fit in with all the rest of
+*	the netcard_probe names.  This function looks for
+*	another TLan based adapter, setting it up with the
+*	allocated device struct if one is found.
+*	tlan_probe has been ported to the new net API and
+*	now allocates its own device structure. This function
+*	is also used by modules.
+*
+**************************************************************/
+
+static int __devinit tlan_probe1(struct pci_dev *pdev,
 				 long ioaddr, int irq, int rev,
-				 const struct pci_device_id *ent )
+				 const struct pci_device_id *ent)
 {
 
 	struct net_device  *dev;
-	TLanPrivateInfo    *priv;
+	struct tlan_priv  *priv;
 	u16		   device_id;
 	int		   reg, rc = -ENODEV;
 
@@ -543,17 +474,17 @@ static int __devinit TLan_probe1(struct pci_dev *pdev,
 		if (rc)
 			return rc;
 
-		rc = pci_request_regions(pdev, TLanSignature);
+		rc = pci_request_regions(pdev, tlan_signature);
 		if (rc) {
-			printk(KERN_ERR "TLAN: Could not reserve IO regions\n");
+			pr_err("Could not reserve IO regions\n");
 			goto err_out;
 		}
 	}
 #endif  /*  CONFIG_PCI  */
 
-	dev = alloc_etherdev(sizeof(TLanPrivateInfo));
+	dev = alloc_etherdev(sizeof(struct tlan_priv));
 	if (dev == NULL) {
-		printk(KERN_ERR "TLAN: Could not allocate memory for device.\n");
+		pr_err("Could not allocate memory for device\n");
 		rc = -ENOMEM;
 		goto err_out_regions;
 	}
@@ -561,38 +492,39 @@ static int __devinit TLan_probe1(struct pci_dev *pdev,
 
 	priv = netdev_priv(dev);
 
-	priv->pciDev = pdev;
+	priv->pci_dev = pdev;
 	priv->dev = dev;
 
 	/* Is this a PCI device? */
 	if (pdev) {
-		u32 		   pci_io_base = 0;
+		u32		   pci_io_base = 0;
 
 		priv->adapter = &board_info[ent->driver_data];
 
 		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (rc) {
-			printk(KERN_ERR "TLAN: No suitable PCI mapping available.\n");
+			pr_err("No suitable PCI mapping available\n");
 			goto err_out_free_dev;
 		}
 
-		for ( reg= 0; reg <= 5; reg ++ ) {
+		for (reg = 0; reg <= 5; reg++) {
 			if (pci_resource_flags(pdev, reg) & IORESOURCE_IO) {
 				pci_io_base = pci_resource_start(pdev, reg);
-				TLAN_DBG( TLAN_DEBUG_GNRL, "IO mapping is available at %x.\n",
-						pci_io_base);
+				TLAN_DBG(TLAN_DEBUG_GNRL,
+					 "IO mapping is available at %x.\n",
+					 pci_io_base);
 				break;
 			}
 		}
 		if (!pci_io_base) {
-			printk(KERN_ERR "TLAN: No IO mappings available\n");
+			pr_err("No IO mappings available\n");
 			rc = -EIO;
 			goto err_out_free_dev;
 		}
 
 		dev->base_addr = pci_io_base;
 		dev->irq = pdev->irq;
-		priv->adapterRev = pdev->revision;
+		priv->adapter_rev = pdev->revision;
 		pci_set_master(pdev);
 		pci_set_drvdata(pdev, dev);
 
@@ -602,11 +534,11 @@ static int __devinit TLan_probe1(struct pci_dev *pdev,
 		device_id = inw(ioaddr + EISA_ID2);
 		priv->is_eisa = 1;
 		if (device_id == 0x20F1) {
-			priv->adapter = &board_info[13]; 	/* NetFlex-3/E */
-			priv->adapterRev = 23;			/* TLAN 2.3 */
+			priv->adapter = &board_info[13]; /* NetFlex-3/E */
+			priv->adapter_rev = 23;		/* TLAN 2.3 */
 		} else {
 			priv->adapter = &board_info[14];
-			priv->adapterRev = 10;			/* TLAN 1.0 */
+			priv->adapter_rev = 10;		/* TLAN 1.0 */
 		}
 		dev->base_addr = ioaddr;
 		dev->irq = irq;
@@ -620,11 +552,11 @@ static int __devinit TLan_probe1(struct pci_dev *pdev,
 		priv->speed  = ((dev->mem_start & 0x18) == 0x18) ? 0
 			: (dev->mem_start & 0x18) >> 3;
 
-		if (priv->speed == 0x1) {
+		if (priv->speed == 0x1)
 			priv->speed = TLAN_SPEED_10;
-		} else if (priv->speed == 0x2) {
+		else if (priv->speed == 0x2)
 			priv->speed = TLAN_SPEED_100;
-		}
+
 		debug = priv->debug = dev->mem_end;
 	} else {
 		priv->aui    = aui[boards_found];
@@ -635,46 +567,45 @@ static int __devinit TLan_probe1(struct pci_dev *pdev,
 
 	/* This will be used when we get an adapter error from
 	 * within our irq handler */
-	INIT_WORK(&priv->tlan_tqueue, TLan_tx_timeout_work);
+	INIT_WORK(&priv->tlan_tqueue, tlan_tx_timeout_work);
 
 	spin_lock_init(&priv->lock);
 
-	rc = TLan_Init(dev);
+	rc = tlan_init(dev);
 	if (rc) {
-		printk(KERN_ERR "TLAN: Could not set up device.\n");
+		pr_err("Could not set up device\n");
 		goto err_out_free_dev;
 	}
 
 	rc = register_netdev(dev);
 	if (rc) {
-		printk(KERN_ERR "TLAN: Could not register device.\n");
+		pr_err("Could not register device\n");
 		goto err_out_uninit;
 	}
 
 
-	TLanDevicesInstalled++;
+	tlan_devices_installed++;
 	boards_found++;
 
 	/* pdev is NULL if this is an EISA device */
 	if (pdev)
 		tlan_have_pci++;
 	else {
-		priv->nextDevice = TLan_Eisa_Devices;
-		TLan_Eisa_Devices = dev;
+		priv->next_device = tlan_eisa_devices;
+		tlan_eisa_devices = dev;
 		tlan_have_eisa++;
 	}
 
-	printk(KERN_INFO "TLAN: %s irq=%2d, io=%04x, %s, Rev. %d\n",
-			dev->name,
-			(int) dev->irq,
-			(int) dev->base_addr,
-			priv->adapter->deviceLabel,
-			priv->adapterRev);
+	netdev_info(dev, "irq=%2d, io=%04x, %s, Rev. %d\n",
+		    (int)dev->irq,
+		    (int)dev->base_addr,
+		    priv->adapter->device_label,
+		    priv->adapter_rev);
 	return 0;
 
 err_out_uninit:
-	pci_free_consistent(priv->pciDev, priv->dmaSize, priv->dmaStorage,
-			    priv->dmaStorageDMA );
+	pci_free_consistent(priv->pci_dev, priv->dma_size, priv->dma_storage,
+			    priv->dma_storage_dma);
 err_out_free_dev:
 	free_netdev(dev);
 err_out_regions:
@@ -689,22 +620,23 @@ err_out:
 }
 
 
-static void TLan_Eisa_Cleanup(void)
+static void tlan_eisa_cleanup(void)
 {
 	struct net_device *dev;
-	TLanPrivateInfo *priv;
+	struct tlan_priv *priv;
 
-	while( tlan_have_eisa ) {
-		dev = TLan_Eisa_Devices;
+	while (tlan_have_eisa) {
+		dev = tlan_eisa_devices;
 		priv = netdev_priv(dev);
-		if (priv->dmaStorage) {
-			pci_free_consistent(priv->pciDev, priv->dmaSize,
-					    priv->dmaStorage, priv->dmaStorageDMA );
+		if (priv->dma_storage) {
+			pci_free_consistent(priv->pci_dev, priv->dma_size,
+					    priv->dma_storage,
+					    priv->dma_storage_dma);
 		}
-		release_region( dev->base_addr, 0x10);
-		unregister_netdev( dev );
-		TLan_Eisa_Devices = priv->nextDevice;
-		free_netdev( dev );
+		release_region(dev->base_addr, 0x10);
+		unregister_netdev(dev);
+		tlan_eisa_devices = priv->next_device;
+		free_netdev(dev);
 		tlan_have_eisa--;
 	}
 }
@@ -715,7 +647,7 @@ static void __exit tlan_exit(void)
 	pci_unregister_driver(&tlan_driver);
 
 	if (tlan_have_eisa)
-		TLan_Eisa_Cleanup();
+		tlan_eisa_cleanup();
 
 }
 
@@ -726,24 +658,24 @@ module_exit(tlan_exit);
 
 
 
-	/**************************************************************
-	 * 	TLan_EisaProbe
-	 *
-	 *  	Returns: 0 on success, 1 otherwise
-	 *
-	 *  	Parms:	 None
-	 *
-	 *
-	 *  	This functions probes for EISA devices and calls
-	 *  	TLan_probe1 when one is found.
-	 *
-	 *************************************************************/
+/**************************************************************
+ *	tlan_eisa_probe
+ *
+ *	Returns: 0 on success, 1 otherwise
+ *
+ *	Parms:	 None
+ *
+ *
+ *	This functions probes for EISA devices and calls
+ *	TLan_probe1 when one is found.
+ *
+ *************************************************************/
 
-static void  __init TLan_EisaProbe (void)
+static void  __init tlan_eisa_probe(void)
 {
-	long 	ioaddr;
-	int 	rc = -ENODEV;
-	int 	irq;
+	long	ioaddr;
+	int	rc = -ENODEV;
+	int	irq;
 	u16	device_id;
 
 	if (!EISA_bus) {
@@ -754,15 +686,16 @@ static void  __init TLan_EisaProbe (void)
 	/* Loop through all slots of the EISA bus */
 	for (ioaddr = 0x1000; ioaddr < 0x9000; ioaddr += 0x1000) {
 
-	TLAN_DBG(TLAN_DEBUG_PROBE,"EISA_ID 0x%4x: 0x%4x\n",
-		 (int) ioaddr + 0xC80, inw(ioaddr + EISA_ID));
-	TLAN_DBG(TLAN_DEBUG_PROBE,"EISA_ID 0x%4x: 0x%4x\n",
-		 (int) ioaddr + 0xC82, inw(ioaddr + EISA_ID2));
+		TLAN_DBG(TLAN_DEBUG_PROBE, "EISA_ID 0x%4x: 0x%4x\n",
+			 (int) ioaddr + 0xc80, inw(ioaddr + EISA_ID));
+		TLAN_DBG(TLAN_DEBUG_PROBE, "EISA_ID 0x%4x: 0x%4x\n",
+			 (int) ioaddr + 0xc82, inw(ioaddr + EISA_ID2));
 
 
-		TLAN_DBG(TLAN_DEBUG_PROBE, "Probing for EISA adapter at IO: 0x%4x : ",
-				   	(int) ioaddr);
-		if (request_region(ioaddr, 0x10, TLanSignature) == NULL)
+		TLAN_DBG(TLAN_DEBUG_PROBE,
+			 "Probing for EISA adapter at IO: 0x%4x : ",
+			 (int) ioaddr);
+		if (request_region(ioaddr, 0x10, tlan_signature) == NULL)
 			goto out;
 
 		if (inw(ioaddr + EISA_ID) != 0x110E) {
@@ -772,326 +705,324 @@ static void  __init TLan_EisaProbe (void)
 
 		device_id = inw(ioaddr + EISA_ID2);
 		if (device_id !=  0x20F1 && device_id != 0x40F1) {
-			release_region (ioaddr, 0x10);
+			release_region(ioaddr, 0x10);
 			goto out;
 		}
 
-	 	if (inb(ioaddr + EISA_CR) != 0x1) { 	/* Check if adapter is enabled */
-			release_region (ioaddr, 0x10);
+		/* check if adapter is enabled */
+		if (inb(ioaddr + EISA_CR) != 0x1) {
+			release_region(ioaddr, 0x10);
 			goto out2;
 		}
 
 		if (debug == 0x10)
-			printk("Found one\n");
+			pr_info("Found one\n");
 
 
 		/* Get irq from board */
-		switch (inb(ioaddr + 0xCC0)) {
-			case(0x10):
-				irq=5;
-				break;
-			case(0x20):
-				irq=9;
-				break;
-			case(0x40):
-				irq=10;
-				break;
-			case(0x80):
-				irq=11;
-				break;
-			default:
-				goto out;
+		switch (inb(ioaddr + 0xcc0)) {
+		case(0x10):
+			irq = 5;
+			break;
+		case(0x20):
+			irq = 9;
+			break;
+		case(0x40):
+			irq = 10;
+			break;
+		case(0x80):
+			irq = 11;
+			break;
+		default:
+			goto out;
 		}
 
 
 		/* Setup the newly found eisa adapter */
-		rc = TLan_probe1( NULL, ioaddr, irq,
-					12, NULL);
+		rc = tlan_probe1(NULL, ioaddr, irq,
+				 12, NULL);
 		continue;
 
-		out:
-			if (debug == 0x10)
-				printk("None found\n");
-			continue;
+out:
+		if (debug == 0x10)
+			pr_info("None found\n");
+		continue;
 
-		out2:	if (debug == 0x10)
-				printk("Card found but it is not enabled, skipping\n");
-			continue;
+out2:
+		if (debug == 0x10)
+			pr_info("Card found but it is not enabled, skipping\n");
+		continue;
 
 	}
 
-} /* TLan_EisaProbe */
+}
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-static void TLan_Poll(struct net_device *dev)
+static void tlan_poll(struct net_device *dev)
 {
 	disable_irq(dev->irq);
-	TLan_HandleInterrupt(dev->irq, dev);
+	tlan_handle_interrupt(dev->irq, dev);
 	enable_irq(dev->irq);
 }
 #endif
 
-static const struct net_device_ops TLan_netdev_ops = {
-	.ndo_open 		= TLan_Open,
-	.ndo_stop		= TLan_Close,
-	.ndo_start_xmit		= TLan_StartTx,
-	.ndo_tx_timeout		= TLan_tx_timeout,
-	.ndo_get_stats		= TLan_GetStats,
-	.ndo_set_multicast_list = TLan_SetMulticastList,
-	.ndo_do_ioctl		= TLan_ioctl,
+static const struct net_device_ops tlan_netdev_ops = {
+	.ndo_open		= tlan_open,
+	.ndo_stop		= tlan_close,
+	.ndo_start_xmit		= tlan_start_tx,
+	.ndo_tx_timeout		= tlan_tx_timeout,
+	.ndo_get_stats		= tlan_get_stats,
+	.ndo_set_multicast_list = tlan_set_multicast_list,
+	.ndo_do_ioctl		= tlan_ioctl,
 	.ndo_change_mtu		= eth_change_mtu,
-	.ndo_set_mac_address 	= eth_mac_addr,
+	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
 #ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller	 = TLan_Poll,
+	.ndo_poll_controller	 = tlan_poll,
 #endif
 };
 
 
 
-	/***************************************************************
-	 *	TLan_Init
-	 *
-	 *	Returns:
-	 *		0 on success, error code otherwise.
-	 *	Parms:
-	 *		dev	The structure of the device to be
-	 *			init'ed.
-	 *
-	 *	This function completes the initialization of the
-	 *	device structure and driver.  It reserves the IO
-	 *	addresses, allocates memory for the lists and bounce
-	 *	buffers, retrieves the MAC address from the eeprom
-	 *	and assignes the device's methods.
-	 *
-	 **************************************************************/
-
-static int TLan_Init( struct net_device *dev )
+/***************************************************************
+ *	tlan_init
+ *
+ *	Returns:
+ *		0 on success, error code otherwise.
+ *	Parms:
+ *		dev	The structure of the device to be
+ *			init'ed.
+ *
+ *	This function completes the initialization of the
+ *	device structure and driver.  It reserves the IO
+ *	addresses, allocates memory for the lists and bounce
+ *	buffers, retrieves the MAC address from the eeprom
+ *	and assignes the device's methods.
+ *
+ **************************************************************/
+
+static int tlan_init(struct net_device *dev)
 {
 	int		dma_size;
-	int 		err;
+	int		err;
 	int		i;
-	TLanPrivateInfo	*priv;
+	struct tlan_priv	*priv;
 
 	priv = netdev_priv(dev);
 
-	dma_size = ( TLAN_NUM_RX_LISTS + TLAN_NUM_TX_LISTS )
-		* ( sizeof(TLanList) );
-	priv->dmaStorage = pci_alloc_consistent(priv->pciDev,
-						dma_size, &priv->dmaStorageDMA);
-	priv->dmaSize = dma_size;
+	dma_size = (TLAN_NUM_RX_LISTS + TLAN_NUM_TX_LISTS)
+		* (sizeof(struct tlan_list));
+	priv->dma_storage = pci_alloc_consistent(priv->pci_dev,
+						 dma_size,
+						 &priv->dma_storage_dma);
+	priv->dma_size = dma_size;
 
-	if ( priv->dmaStorage == NULL ) {
-		printk(KERN_ERR "TLAN:  Could not allocate lists and buffers for %s.\n",
-			dev->name );
+	if (priv->dma_storage == NULL) {
+		pr_err("Could not allocate lists and buffers for %s\n",
+		       dev->name);
 		return -ENOMEM;
 	}
-	memset( priv->dmaStorage, 0, dma_size );
-	priv->rxList = (TLanList *) ALIGN((unsigned long)priv->dmaStorage, 8);
-	priv->rxListDMA = ALIGN(priv->dmaStorageDMA, 8);
-	priv->txList = priv->rxList + TLAN_NUM_RX_LISTS;
-	priv->txListDMA = priv->rxListDMA + sizeof(TLanList) * TLAN_NUM_RX_LISTS;
+	memset(priv->dma_storage, 0, dma_size);
+	priv->rx_list = (struct tlan_list *)
+		ALIGN((unsigned long)priv->dma_storage, 8);
+	priv->rx_list_dma = ALIGN(priv->dma_storage_dma, 8);
+	priv->tx_list = priv->rx_list + TLAN_NUM_RX_LISTS;
+	priv->tx_list_dma =
+		priv->rx_list_dma + sizeof(struct tlan_list)*TLAN_NUM_RX_LISTS;
 
 	err = 0;
-	for ( i = 0;  i < 6 ; i++ )
-		err |= TLan_EeReadByte( dev,
-					(u8) priv->adapter->addrOfs + i,
-					(u8 *) &dev->dev_addr[i] );
-	if ( err ) {
-		printk(KERN_ERR "TLAN: %s: Error reading MAC from eeprom: %d\n",
-			dev->name,
-			err );
+	for (i = 0;  i < 6 ; i++)
+		err |= tlan_ee_read_byte(dev,
+					 (u8) priv->adapter->addr_ofs + i,
+					 (u8 *) &dev->dev_addr[i]);
+	if (err) {
+		pr_err("%s: Error reading MAC from eeprom: %d\n",
+		       dev->name, err);
 	}
 	dev->addr_len = 6;
 
 	netif_carrier_off(dev);
 
 	/* Device methods */
-	dev->netdev_ops = &TLan_netdev_ops;
+	dev->netdev_ops = &tlan_netdev_ops;
 	dev->watchdog_timeo = TX_TIMEOUT;
 
 	return 0;
 
-} /* TLan_Init */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_Open
-	 *
-	 *	Returns:
-	 *		0 on success, error code otherwise.
-	 *	Parms:
-	 *		dev	Structure of device to be opened.
-	 *
-	 *	This routine puts the driver and TLAN adapter in a
-	 *	state where it is ready to send and receive packets.
-	 *	It allocates the IRQ, resets and brings the adapter
-	 *	out of reset, and allows interrupts.  It also delays
-	 *	the startup for autonegotiation or sends a Rx GO
-	 *	command to the adapter, as appropriate.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_open
+ *
+ *	Returns:
+ *		0 on success, error code otherwise.
+ *	Parms:
+ *		dev	Structure of device to be opened.
+ *
+ *	This routine puts the driver and TLAN adapter in a
+ *	state where it is ready to send and receive packets.
+ *	It allocates the IRQ, resets and brings the adapter
+ *	out of reset, and allows interrupts.  It also delays
+ *	the startup for autonegotiation or sends a Rx GO
+ *	command to the adapter, as appropriate.
+ *
+ **************************************************************/
 
-static int TLan_Open( struct net_device *dev )
+static int tlan_open(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	int		err;
 
-	priv->tlanRev = TLan_DioRead8( dev->base_addr, TLAN_DEF_REVISION );
-	err = request_irq( dev->irq, TLan_HandleInterrupt, IRQF_SHARED,
-			   dev->name, dev );
+	priv->tlan_rev = tlan_dio_read8(dev->base_addr, TLAN_DEF_REVISION);
+	err = request_irq(dev->irq, tlan_handle_interrupt, IRQF_SHARED,
+			  dev->name, dev);
 
-	if ( err ) {
-		pr_err("TLAN:  Cannot open %s because IRQ %d is already in use.\n",
-		       dev->name, dev->irq );
+	if (err) {
+		netdev_err(dev, "Cannot open because IRQ %d is already in use\n",
+			   dev->irq);
 		return err;
 	}
 
 	init_timer(&priv->timer);
-	netif_start_queue(dev);
 
-	/* NOTE: It might not be necessary to read the stats before a
-			 reset if you don't care what the values are.
-	*/
-	TLan_ResetLists( dev );
-	TLan_ReadAndClearStats( dev, TLAN_IGNORE );
-	TLan_ResetAdapter( dev );
+	tlan_start(dev);
 
-	TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Opened.  TLAN Chip Rev: %x\n",
-		  dev->name, priv->tlanRev );
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Opened.  TLAN Chip Rev: %x\n",
+		 dev->name, priv->tlan_rev);
 
 	return 0;
 
-} /* TLan_Open */
+}
 
 
 
-	/**************************************************************
-	 *	TLan_ioctl
-	 *
-	 *	Returns:
-	 *		0 on success, error code otherwise
-	 *	Params:
-	 *		dev	structure of device to receive ioctl.
-	 *
-	 *		rq	ifreq structure to hold userspace data.
-	 *
-	 *		cmd	ioctl command.
-	 *
-	 *
-	 *************************************************************/
+/**************************************************************
+ *	tlan_ioctl
+ *
+ *	Returns:
+ *		0 on success, error code otherwise
+ *	Params:
+ *		dev	structure of device to receive ioctl.
+ *
+ *		rq	ifreq structure to hold userspace data.
+ *
+ *		cmd	ioctl command.
+ *
+ *
+ *************************************************************/
 
-static int TLan_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+static int tlan_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	struct mii_ioctl_data *data = if_mii(rq);
-	u32 phy   = priv->phy[priv->phyNum];
+	u32 phy   = priv->phy[priv->phy_num];
 
-	if (!priv->phyOnline)
+	if (!priv->phy_online)
 		return -EAGAIN;
 
-	switch(cmd) {
-	case SIOCGMIIPHY:		/* Get address of MII PHY in use. */
-			data->phy_id = phy;
+	switch (cmd) {
+	case SIOCGMIIPHY:		/* get address of MII PHY in use. */
+		data->phy_id = phy;
 
 
-	case SIOCGMIIREG:		/* Read MII PHY register. */
-			TLan_MiiReadReg(dev, data->phy_id & 0x1f,
-					data->reg_num & 0x1f, &data->val_out);
-			return 0;
+	case SIOCGMIIREG:		/* read MII PHY register. */
+		tlan_mii_read_reg(dev, data->phy_id & 0x1f,
+				  data->reg_num & 0x1f, &data->val_out);
+		return 0;
 
 
-	case SIOCSMIIREG:		/* Write MII PHY register. */
-			TLan_MiiWriteReg(dev, data->phy_id & 0x1f,
-					 data->reg_num & 0x1f, data->val_in);
-			return 0;
-		default:
-			return -EOPNOTSUPP;
+	case SIOCSMIIREG:		/* write MII PHY register. */
+		tlan_mii_write_reg(dev, data->phy_id & 0x1f,
+				   data->reg_num & 0x1f, data->val_in);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
 	}
-} /* tlan_ioctl */
+}
 
 
-	/***************************************************************
-	 * 	TLan_tx_timeout
-	 *
-	 * 	Returns: nothing
-	 *
-	 * 	Params:
-	 * 		dev	structure of device which timed out
-	 * 			during transmit.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_tx_timeout
+ *
+ *	Returns: nothing
+ *
+ *	Params:
+ *		dev	structure of device which timed out
+ *			during transmit.
+ *
+ **************************************************************/
 
-static void TLan_tx_timeout(struct net_device *dev)
+static void tlan_tx_timeout(struct net_device *dev)
 {
 
-	TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Transmit timed out.\n", dev->name);
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Transmit timed out.\n", dev->name);
 
 	/* Ok so we timed out, lets see what we can do about it...*/
-	TLan_FreeLists( dev );
-	TLan_ResetLists( dev );
-	TLan_ReadAndClearStats( dev, TLAN_IGNORE );
-	TLan_ResetAdapter( dev );
+	tlan_free_lists(dev);
+	tlan_reset_lists(dev);
+	tlan_read_and_clear_stats(dev, TLAN_IGNORE);
+	tlan_reset_adapter(dev);
 	dev->trans_start = jiffies; /* prevent tx timeout */
-	netif_wake_queue( dev );
+	netif_wake_queue(dev);
 
 }
 
 
-	/***************************************************************
-	 * 	TLan_tx_timeout_work
-	 *
-	 * 	Returns: nothing
-	 *
-	 * 	Params:
-	 * 		work	work item of device which timed out
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_tx_timeout_work
+ *
+ *	Returns: nothing
+ *
+ *	Params:
+ *		work	work item of device which timed out
+ *
+ **************************************************************/
 
-static void TLan_tx_timeout_work(struct work_struct *work)
+static void tlan_tx_timeout_work(struct work_struct *work)
 {
-	TLanPrivateInfo	*priv =
-		container_of(work, TLanPrivateInfo, tlan_tqueue);
+	struct tlan_priv	*priv =
+		container_of(work, struct tlan_priv, tlan_tqueue);
 
-	TLan_tx_timeout(priv->dev);
+	tlan_tx_timeout(priv->dev);
 }
 
 
 
-	/***************************************************************
-	 *	TLan_StartTx
-	 *
-	 *	Returns:
-	 *		0 on success, non-zero on failure.
-	 *	Parms:
-	 *		skb	A pointer to the sk_buff containing the
-	 *			frame to be sent.
-	 *		dev	The device to send the data on.
-	 *
-	 *	This function adds a frame to the Tx list to be sent
-	 *	ASAP.  First it	verifies that the adapter is ready and
-	 *	there is room in the queue.  Then it sets up the next
-	 *	available list, copies the frame to the	corresponding
-	 *	buffer.  If the adapter Tx channel is idle, it gives
-	 *	the adapter a Tx Go command on the list, otherwise it
-	 *	sets the forward address of the previous list to point
-	 *	to this one.  Then it frees the sk_buff.
-	 *
-	 **************************************************************/
-
-static netdev_tx_t TLan_StartTx( struct sk_buff *skb, struct net_device *dev )
+/***************************************************************
+ *	tlan_start_tx
+ *
+ *	Returns:
+ *		0 on success, non-zero on failure.
+ *	Parms:
+ *		skb	A pointer to the sk_buff containing the
+ *			frame to be sent.
+ *		dev	The device to send the data on.
+ *
+ *	This function adds a frame to the Tx list to be sent
+ *	ASAP.  First it	verifies that the adapter is ready and
+ *	there is room in the queue.  Then it sets up the next
+ *	available list, copies the frame to the	corresponding
+ *	buffer.  If the adapter Tx channel is idle, it gives
+ *	the adapter a Tx Go command on the list, otherwise it
+ *	sets the forward address of the previous list to point
+ *	to this one.  Then it frees the sk_buff.
+ *
+ **************************************************************/
+
+static netdev_tx_t tlan_start_tx(struct sk_buff *skb, struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	dma_addr_t	tail_list_phys;
-	TLanList	*tail_list;
+	struct tlan_list	*tail_list;
 	unsigned long	flags;
 	unsigned int    txlen;
 
-	if ( ! priv->phyOnline ) {
-		TLAN_DBG( TLAN_DEBUG_TX, "TRANSMIT:  %s PHY is not ready\n",
-			  dev->name );
+	if (!priv->phy_online) {
+		TLAN_DBG(TLAN_DEBUG_TX, "TRANSMIT:  %s PHY is not ready\n",
+			 dev->name);
 		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
 	}
@@ -1100,218 +1031,214 @@ static netdev_tx_t TLan_StartTx( struct sk_buff *skb, struct net_device *dev )
 		return NETDEV_TX_OK;
 	txlen = max(skb->len, (unsigned int)TLAN_MIN_FRAME_SIZE);
 
-	tail_list = priv->txList + priv->txTail;
-	tail_list_phys = priv->txListDMA + sizeof(TLanList) * priv->txTail;
+	tail_list = priv->tx_list + priv->tx_tail;
+	tail_list_phys =
+		priv->tx_list_dma + sizeof(struct tlan_list)*priv->tx_tail;
 
-	if ( tail_list->cStat != TLAN_CSTAT_UNUSED ) {
-		TLAN_DBG( TLAN_DEBUG_TX,
-			  "TRANSMIT:  %s is busy (Head=%d Tail=%d)\n",
-			  dev->name, priv->txHead, priv->txTail );
+	if (tail_list->c_stat != TLAN_CSTAT_UNUSED) {
+		TLAN_DBG(TLAN_DEBUG_TX,
+			 "TRANSMIT:  %s is busy (Head=%d Tail=%d)\n",
+			 dev->name, priv->tx_head, priv->tx_tail);
 		netif_stop_queue(dev);
-		priv->txBusyCount++;
+		priv->tx_busy_count++;
 		return NETDEV_TX_BUSY;
 	}
 
 	tail_list->forward = 0;
 
-	tail_list->buffer[0].address = pci_map_single(priv->pciDev,
+	tail_list->buffer[0].address = pci_map_single(priv->pci_dev,
 						      skb->data, txlen,
 						      PCI_DMA_TODEVICE);
-	TLan_StoreSKB(tail_list, skb);
+	tlan_store_skb(tail_list, skb);
 
-	tail_list->frameSize = (u16) txlen;
+	tail_list->frame_size = (u16) txlen;
 	tail_list->buffer[0].count = TLAN_LAST_BUFFER | (u32) txlen;
 	tail_list->buffer[1].count = 0;
 	tail_list->buffer[1].address = 0;
 
 	spin_lock_irqsave(&priv->lock, flags);
-	tail_list->cStat = TLAN_CSTAT_READY;
-	if ( ! priv->txInProgress ) {
-		priv->txInProgress = 1;
-		TLAN_DBG( TLAN_DEBUG_TX,
-			  "TRANSMIT:  Starting TX on buffer %d\n", priv->txTail );
-		outl( tail_list_phys, dev->base_addr + TLAN_CH_PARM );
-		outl( TLAN_HC_GO, dev->base_addr + TLAN_HOST_CMD );
+	tail_list->c_stat = TLAN_CSTAT_READY;
+	if (!priv->tx_in_progress) {
+		priv->tx_in_progress = 1;
+		TLAN_DBG(TLAN_DEBUG_TX,
+			 "TRANSMIT:  Starting TX on buffer %d\n",
+			 priv->tx_tail);
+		outl(tail_list_phys, dev->base_addr + TLAN_CH_PARM);
+		outl(TLAN_HC_GO, dev->base_addr + TLAN_HOST_CMD);
 	} else {
-		TLAN_DBG( TLAN_DEBUG_TX, "TRANSMIT:  Adding buffer %d to TX channel\n",
-			  priv->txTail );
-		if ( priv->txTail == 0 ) {
-			( priv->txList + ( TLAN_NUM_TX_LISTS - 1 ) )->forward
+		TLAN_DBG(TLAN_DEBUG_TX,
+			 "TRANSMIT:  Adding buffer %d to TX channel\n",
+			 priv->tx_tail);
+		if (priv->tx_tail == 0) {
+			(priv->tx_list + (TLAN_NUM_TX_LISTS - 1))->forward
 				= tail_list_phys;
 		} else {
-			( priv->txList + ( priv->txTail - 1 ) )->forward
+			(priv->tx_list + (priv->tx_tail - 1))->forward
 				= tail_list_phys;
 		}
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	CIRC_INC( priv->txTail, TLAN_NUM_TX_LISTS );
+	CIRC_INC(priv->tx_tail, TLAN_NUM_TX_LISTS);
 
 	return NETDEV_TX_OK;
 
-} /* TLan_StartTx */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_HandleInterrupt
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		irq	The line on which the interrupt
-	 *			occurred.
-	 *		dev_id	A pointer to the device assigned to
-	 *			this irq line.
-	 *
-	 *	This function handles an interrupt generated by its
-	 *	assigned TLAN adapter.  The function deactivates
-	 *	interrupts on its adapter, records the type of
-	 *	interrupt, executes the appropriate subhandler, and
-	 *	acknowdges the interrupt to the adapter (thus
-	 *	re-enabling adapter interrupts.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_handle_interrupt
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		irq	The line on which the interrupt
+ *			occurred.
+ *		dev_id	A pointer to the device assigned to
+ *			this irq line.
+ *
+ *	This function handles an interrupt generated by its
+ *	assigned TLAN adapter.  The function deactivates
+ *	interrupts on its adapter, records the type of
+ *	interrupt, executes the appropriate subhandler, and
+ *	acknowdges the interrupt to the adapter (thus
+ *	re-enabling adapter interrupts.
+ *
+ **************************************************************/
 
-static irqreturn_t TLan_HandleInterrupt(int irq, void *dev_id)
+static irqreturn_t tlan_handle_interrupt(int irq, void *dev_id)
 {
 	struct net_device	*dev = dev_id;
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	u16		host_int;
 	u16		type;
 
 	spin_lock(&priv->lock);
 
-	host_int = inw( dev->base_addr + TLAN_HOST_INT );
-	type = ( host_int & TLAN_HI_IT_MASK ) >> 2;
-	if ( type ) {
+	host_int = inw(dev->base_addr + TLAN_HOST_INT);
+	type = (host_int & TLAN_HI_IT_MASK) >> 2;
+	if (type) {
 		u32	ack;
 		u32	host_cmd;
 
-		outw( host_int, dev->base_addr + TLAN_HOST_INT );
-		ack = TLanIntVector[type]( dev, host_int );
+		outw(host_int, dev->base_addr + TLAN_HOST_INT);
+		ack = tlan_int_vector[type](dev, host_int);
 
-		if ( ack ) {
-			host_cmd = TLAN_HC_ACK | ack | ( type << 18 );
-			outl( host_cmd, dev->base_addr + TLAN_HOST_CMD );
+		if (ack) {
+			host_cmd = TLAN_HC_ACK | ack | (type << 18);
+			outl(host_cmd, dev->base_addr + TLAN_HOST_CMD);
 		}
 	}
 
 	spin_unlock(&priv->lock);
 
 	return IRQ_RETVAL(type);
-} /* TLan_HandleInterrupts */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_Close
-	 *
-	 * 	Returns:
-	 *		An error code.
-	 *	Parms:
-	 *		dev	The device structure of the device to
-	 *			close.
-	 *
-	 *	This function shuts down the adapter.  It records any
-	 *	stats, puts the adapter into reset state, deactivates
-	 *	its time as needed, and	frees the irq it is using.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_close
+ *
+ *	Returns:
+ *		An error code.
+ *	Parms:
+ *		dev	The device structure of the device to
+ *			close.
+ *
+ *	This function shuts down the adapter.  It records any
+ *	stats, puts the adapter into reset state, deactivates
+ *	its time as needed, and	frees the irq it is using.
+ *
+ **************************************************************/
 
-static int TLan_Close(struct net_device *dev)
+static int tlan_close(struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 
-	netif_stop_queue(dev);
 	priv->neg_be_verbose = 0;
+	tlan_stop(dev);
 
-	TLan_ReadAndClearStats( dev, TLAN_RECORD );
-	outl( TLAN_HC_AD_RST, dev->base_addr + TLAN_HOST_CMD );
-	if ( priv->timer.function != NULL ) {
-		del_timer_sync( &priv->timer );
-		priv->timer.function = NULL;
-	}
-
-	free_irq( dev->irq, dev );
-	TLan_FreeLists( dev );
-	TLAN_DBG( TLAN_DEBUG_GNRL, "Device %s closed.\n", dev->name );
+	free_irq(dev->irq, dev);
+	tlan_free_lists(dev);
+	TLAN_DBG(TLAN_DEBUG_GNRL, "Device %s closed.\n", dev->name);
 
 	return 0;
 
-} /* TLan_Close */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_GetStats
-	 *
-	 *	Returns:
-	 *		A pointer to the device's statistics structure.
-	 *	Parms:
-	 *		dev	The device structure to return the
-	 *			stats for.
-	 *
-	 *	This function updates the devices statistics by reading
-	 *	the TLAN chip's onboard registers.  Then it returns the
-	 *	address of the statistics structure.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_get_stats
+ *
+ *	Returns:
+ *		A pointer to the device's statistics structure.
+ *	Parms:
+ *		dev	The device structure to return the
+ *			stats for.
+ *
+ *	This function updates the devices statistics by reading
+ *	the TLAN chip's onboard registers.  Then it returns the
+ *	address of the statistics structure.
+ *
+ **************************************************************/
 
-static struct net_device_stats *TLan_GetStats( struct net_device *dev )
+static struct net_device_stats *tlan_get_stats(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	int i;
 
 	/* Should only read stats if open ? */
-	TLan_ReadAndClearStats( dev, TLAN_RECORD );
+	tlan_read_and_clear_stats(dev, TLAN_RECORD);
 
-	TLAN_DBG( TLAN_DEBUG_RX, "RECEIVE:  %s EOC count = %d\n", dev->name,
-		  priv->rxEocCount );
-	TLAN_DBG( TLAN_DEBUG_TX, "TRANSMIT:  %s Busy count = %d\n", dev->name,
-		  priv->txBusyCount );
-	if ( debug & TLAN_DEBUG_GNRL ) {
-		TLan_PrintDio( dev->base_addr );
-		TLan_PhyPrint( dev );
+	TLAN_DBG(TLAN_DEBUG_RX, "RECEIVE:  %s EOC count = %d\n", dev->name,
+		 priv->rx_eoc_count);
+	TLAN_DBG(TLAN_DEBUG_TX, "TRANSMIT:  %s Busy count = %d\n", dev->name,
+		 priv->tx_busy_count);
+	if (debug & TLAN_DEBUG_GNRL) {
+		tlan_print_dio(dev->base_addr);
+		tlan_phy_print(dev);
 	}
-	if ( debug & TLAN_DEBUG_LIST ) {
-		for ( i = 0; i < TLAN_NUM_RX_LISTS; i++ )
-			TLan_PrintList( priv->rxList + i, "RX", i );
-		for ( i = 0; i < TLAN_NUM_TX_LISTS; i++ )
-			TLan_PrintList( priv->txList + i, "TX", i );
+	if (debug & TLAN_DEBUG_LIST) {
+		for (i = 0; i < TLAN_NUM_RX_LISTS; i++)
+			tlan_print_list(priv->rx_list + i, "RX", i);
+		for (i = 0; i < TLAN_NUM_TX_LISTS; i++)
+			tlan_print_list(priv->tx_list + i, "TX", i);
 	}
 
 	return &dev->stats;
 
-} /* TLan_GetStats */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_SetMulticastList
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev	The device structure to set the
-	 *			multicast list for.
-	 *
-	 *	This function sets the TLAN adaptor to various receive
-	 *	modes.  If the IFF_PROMISC flag is set, promiscuous
-	 *	mode is acitviated.  Otherwise,	promiscuous mode is
-	 *	turned off.  If the IFF_ALLMULTI flag is set, then
-	 *	the hash table is set to receive all group addresses.
-	 *	Otherwise, the first three multicast addresses are
-	 *	stored in AREG_1-3, and the rest are selected via the
-	 *	hash table, as necessary.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_set_multicast_list
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev	The device structure to set the
+ *			multicast list for.
+ *
+ *	This function sets the TLAN adaptor to various receive
+ *	modes.  If the IFF_PROMISC flag is set, promiscuous
+ *	mode is acitviated.  Otherwise,	promiscuous mode is
+ *	turned off.  If the IFF_ALLMULTI flag is set, then
+ *	the hash table is set to receive all group addresses.
+ *	Otherwise, the first three multicast addresses are
+ *	stored in AREG_1-3, and the rest are selected via the
+ *	hash table, as necessary.
+ *
+ **************************************************************/
 
-static void TLan_SetMulticastList( struct net_device *dev )
+static void tlan_set_multicast_list(struct net_device *dev)
 {
 	struct netdev_hw_addr *ha;
 	u32			hash1 = 0;
@@ -1320,53 +1247,56 @@ static void TLan_SetMulticastList( struct net_device *dev )
 	u32			offset;
 	u8			tmp;
 
-	if ( dev->flags & IFF_PROMISC ) {
-		tmp = TLan_DioRead8( dev->base_addr, TLAN_NET_CMD );
-		TLan_DioWrite8( dev->base_addr,
-				TLAN_NET_CMD, tmp | TLAN_NET_CMD_CAF );
+	if (dev->flags & IFF_PROMISC) {
+		tmp = tlan_dio_read8(dev->base_addr, TLAN_NET_CMD);
+		tlan_dio_write8(dev->base_addr,
+				TLAN_NET_CMD, tmp | TLAN_NET_CMD_CAF);
 	} else {
-		tmp = TLan_DioRead8( dev->base_addr, TLAN_NET_CMD );
-		TLan_DioWrite8( dev->base_addr,
-				TLAN_NET_CMD, tmp & ~TLAN_NET_CMD_CAF );
-		if ( dev->flags & IFF_ALLMULTI ) {
-			for ( i = 0; i < 3; i++ )
-				TLan_SetMac( dev, i + 1, NULL );
-			TLan_DioWrite32( dev->base_addr, TLAN_HASH_1, 0xFFFFFFFF );
-			TLan_DioWrite32( dev->base_addr, TLAN_HASH_2, 0xFFFFFFFF );
+		tmp = tlan_dio_read8(dev->base_addr, TLAN_NET_CMD);
+		tlan_dio_write8(dev->base_addr,
+				TLAN_NET_CMD, tmp & ~TLAN_NET_CMD_CAF);
+		if (dev->flags & IFF_ALLMULTI) {
+			for (i = 0; i < 3; i++)
+				tlan_set_mac(dev, i + 1, NULL);
+			tlan_dio_write32(dev->base_addr, TLAN_HASH_1,
+					 0xffffffff);
+			tlan_dio_write32(dev->base_addr, TLAN_HASH_2,
+					 0xffffffff);
 		} else {
 			i = 0;
 			netdev_for_each_mc_addr(ha, dev) {
-				if ( i < 3 ) {
-					TLan_SetMac( dev, i + 1,
+				if (i < 3) {
+					tlan_set_mac(dev, i + 1,
 						     (char *) &ha->addr);
 				} else {
-					offset = TLan_HashFunc((u8 *)&ha->addr);
-					if ( offset < 32 )
-						hash1 |= ( 1 << offset );
+					offset =
+						tlan_hash_func((u8 *)&ha->addr);
+					if (offset < 32)
+						hash1 |= (1 << offset);
 					else
-						hash2 |= ( 1 << ( offset - 32 ) );
+						hash2 |= (1 << (offset - 32));
 				}
 				i++;
 			}
-			for ( ; i < 3; i++ )
-				TLan_SetMac( dev, i + 1, NULL );
-			TLan_DioWrite32( dev->base_addr, TLAN_HASH_1, hash1 );
-			TLan_DioWrite32( dev->base_addr, TLAN_HASH_2, hash2 );
+			for ( ; i < 3; i++)
+				tlan_set_mac(dev, i + 1, NULL);
+			tlan_dio_write32(dev->base_addr, TLAN_HASH_1, hash1);
+			tlan_dio_write32(dev->base_addr, TLAN_HASH_2, hash2);
 		}
 	}
 
-} /* TLan_SetMulticastList */
+}
 
 
 
 /*****************************************************************************
 ******************************************************************************
 
-        ThunderLAN Driver Interrupt Vectors and Table
+ThunderLAN driver interrupt vectors and table
 
-	Please see Chap. 4, "Interrupt Handling" of the "ThunderLAN
-	Programmer's Guide" for more informations on handling interrupts
-	generated by TLAN based adapters.
+please see chap. 4, "Interrupt Handling" of the "ThunderLAN
+Programmer's Guide" for more informations on handling interrupts
+generated by TLAN based adapters.
 
 ******************************************************************************
 *****************************************************************************/
@@ -1374,46 +1304,48 @@ static void TLan_SetMulticastList( struct net_device *dev )
 
 
 
-	/***************************************************************
-	 *	TLan_HandleTxEOF
-	 *
-	 *	Returns:
-	 *		1
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This function handles Tx EOF interrupts which are raised
-	 *	by the adapter when it has completed sending the
-	 *	contents of a buffer.  If detemines which list/buffer
-	 *	was completed and resets it.  If the buffer was the last
-	 *	in the channel (EOC), then the function checks to see if
-	 *	another buffer is ready to send, and if so, sends a Tx
-	 *	Go command.  Finally, the driver activates/continues the
-	 *	activity LED.
-	 *
-	 **************************************************************/
-
-static u32 TLan_HandleTxEOF( struct net_device *dev, u16 host_int )
+/***************************************************************
+ *	tlan_handle_tx_eof
+ *
+ *	Returns:
+ *		1
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This function handles Tx EOF interrupts which are raised
+ *	by the adapter when it has completed sending the
+ *	contents of a buffer.  If detemines which list/buffer
+ *	was completed and resets it.  If the buffer was the last
+ *	in the channel (EOC), then the function checks to see if
+ *	another buffer is ready to send, and if so, sends a Tx
+ *	Go command.  Finally, the driver activates/continues the
+ *	activity LED.
+ *
+ **************************************************************/
+
+static u32 tlan_handle_tx_eof(struct net_device *dev, u16 host_int)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	int		eoc = 0;
-	TLanList	*head_list;
+	struct tlan_list	*head_list;
 	dma_addr_t	head_list_phys;
 	u32		ack = 0;
-	u16		tmpCStat;
+	u16		tmp_c_stat;
 
-	TLAN_DBG( TLAN_DEBUG_TX, "TRANSMIT:  Handling TX EOF (Head=%d Tail=%d)\n",
-		  priv->txHead, priv->txTail );
-	head_list = priv->txList + priv->txHead;
+	TLAN_DBG(TLAN_DEBUG_TX,
+		 "TRANSMIT:  Handling TX EOF (Head=%d Tail=%d)\n",
+		 priv->tx_head, priv->tx_tail);
+	head_list = priv->tx_list + priv->tx_head;
 
-	while (((tmpCStat = head_list->cStat ) & TLAN_CSTAT_FRM_CMP) && (ack < 255)) {
-		struct sk_buff *skb = TLan_GetSKB(head_list);
+	while (((tmp_c_stat = head_list->c_stat) & TLAN_CSTAT_FRM_CMP)
+	       && (ack < 255)) {
+		struct sk_buff *skb = tlan_get_skb(head_list);
 
 		ack++;
-		pci_unmap_single(priv->pciDev, head_list->buffer[0].address,
+		pci_unmap_single(priv->pci_dev, head_list->buffer[0].address,
 				 max(skb->len,
 				     (unsigned int)TLAN_MIN_FRAME_SIZE),
 				 PCI_DMA_TODEVICE);
@@ -1421,304 +1353,313 @@ static u32 TLan_HandleTxEOF( struct net_device *dev, u16 host_int )
 		head_list->buffer[8].address = 0;
 		head_list->buffer[9].address = 0;
 
-		if ( tmpCStat & TLAN_CSTAT_EOC )
+		if (tmp_c_stat & TLAN_CSTAT_EOC)
 			eoc = 1;
 
-		dev->stats.tx_bytes += head_list->frameSize;
+		dev->stats.tx_bytes += head_list->frame_size;
 
-		head_list->cStat = TLAN_CSTAT_UNUSED;
+		head_list->c_stat = TLAN_CSTAT_UNUSED;
 		netif_start_queue(dev);
-		CIRC_INC( priv->txHead, TLAN_NUM_TX_LISTS );
-		head_list = priv->txList + priv->txHead;
+		CIRC_INC(priv->tx_head, TLAN_NUM_TX_LISTS);
+		head_list = priv->tx_list + priv->tx_head;
 	}
 
 	if (!ack)
-		printk(KERN_INFO "TLAN: Received interrupt for uncompleted TX frame.\n");
-
-	if ( eoc ) {
-		TLAN_DBG( TLAN_DEBUG_TX,
-			  "TRANSMIT:  Handling TX EOC (Head=%d Tail=%d)\n",
-			  priv->txHead, priv->txTail );
-		head_list = priv->txList + priv->txHead;
-		head_list_phys = priv->txListDMA + sizeof(TLanList) * priv->txHead;
-		if ( ( head_list->cStat & TLAN_CSTAT_READY ) == TLAN_CSTAT_READY ) {
-			outl(head_list_phys, dev->base_addr + TLAN_CH_PARM );
+		netdev_info(dev,
+			    "Received interrupt for uncompleted TX frame\n");
+
+	if (eoc) {
+		TLAN_DBG(TLAN_DEBUG_TX,
+			 "TRANSMIT:  handling TX EOC (Head=%d Tail=%d)\n",
+			 priv->tx_head, priv->tx_tail);
+		head_list = priv->tx_list + priv->tx_head;
+		head_list_phys = priv->tx_list_dma
+			+ sizeof(struct tlan_list)*priv->tx_head;
+		if ((head_list->c_stat & TLAN_CSTAT_READY)
+		    == TLAN_CSTAT_READY) {
+			outl(head_list_phys, dev->base_addr + TLAN_CH_PARM);
 			ack |= TLAN_HC_GO;
 		} else {
-			priv->txInProgress = 0;
+			priv->tx_in_progress = 0;
 		}
 	}
 
-	if ( priv->adapter->flags & TLAN_ADAPTER_ACTIVITY_LED ) {
-		TLan_DioWrite8( dev->base_addr,
-				TLAN_LED_REG, TLAN_LED_LINK | TLAN_LED_ACT );
-		if ( priv->timer.function == NULL ) {
-			 priv->timer.function = TLan_Timer;
-			 priv->timer.data = (unsigned long) dev;
-			 priv->timer.expires = jiffies + TLAN_TIMER_ACT_DELAY;
-			 priv->timerSetAt = jiffies;
-			 priv->timerType = TLAN_TIMER_ACTIVITY;
-			 add_timer(&priv->timer);
-		} else if ( priv->timerType == TLAN_TIMER_ACTIVITY ) {
-			priv->timerSetAt = jiffies;
+	if (priv->adapter->flags & TLAN_ADAPTER_ACTIVITY_LED) {
+		tlan_dio_write8(dev->base_addr,
+				TLAN_LED_REG, TLAN_LED_LINK | TLAN_LED_ACT);
+		if (priv->timer.function == NULL) {
+			priv->timer.function = tlan_timer;
+			priv->timer.data = (unsigned long) dev;
+			priv->timer.expires = jiffies + TLAN_TIMER_ACT_DELAY;
+			priv->timer_set_at = jiffies;
+			priv->timer_type = TLAN_TIMER_ACTIVITY;
+			add_timer(&priv->timer);
+		} else if (priv->timer_type == TLAN_TIMER_ACTIVITY) {
+			priv->timer_set_at = jiffies;
 		}
 	}
 
 	return ack;
 
-} /* TLan_HandleTxEOF */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_HandleStatOverflow
-	 *
-	 *	Returns:
-	 *		1
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This function handles the Statistics Overflow interrupt
-	 *	which means that one or more of the TLAN statistics
-	 *	registers has reached 1/2 capacity and needs to be read.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	TLan_HandleStatOverflow
+ *
+ *	Returns:
+ *		1
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This function handles the Statistics Overflow interrupt
+ *	which means that one or more of the TLAN statistics
+ *	registers has reached 1/2 capacity and needs to be read.
+ *
+ **************************************************************/
 
-static u32 TLan_HandleStatOverflow( struct net_device *dev, u16 host_int )
+static u32 tlan_handle_stat_overflow(struct net_device *dev, u16 host_int)
 {
-	TLan_ReadAndClearStats( dev, TLAN_RECORD );
+	tlan_read_and_clear_stats(dev, TLAN_RECORD);
 
 	return 1;
 
-} /* TLan_HandleStatOverflow */
-
-
-
-
-	/***************************************************************
-	 *	TLan_HandleRxEOF
-	 *
-	 *	Returns:
-	 *		1
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This function handles the Rx EOF interrupt which
-	 *	indicates a frame has been received by the adapter from
-	 *	the net and the frame has been transferred to memory.
-	 *	The function determines the bounce buffer the frame has
-	 *	been loaded into, creates a new sk_buff big enough to
-	 *	hold the frame, and sends it to protocol stack.  It
-	 *	then resets the used buffer and appends it to the end
-	 *	of the list.  If the frame was the last in the Rx
-	 *	channel (EOC), the function restarts the receive channel
-	 *	by sending an Rx Go command to the adapter.  Then it
-	 *	activates/continues the activity LED.
-	 *
-	 **************************************************************/
-
-static u32 TLan_HandleRxEOF( struct net_device *dev, u16 host_int )
+}
+
+
+
+
+/***************************************************************
+ *	TLan_HandleRxEOF
+ *
+ *	Returns:
+ *		1
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This function handles the Rx EOF interrupt which
+ *	indicates a frame has been received by the adapter from
+ *	the net and the frame has been transferred to memory.
+ *	The function determines the bounce buffer the frame has
+ *	been loaded into, creates a new sk_buff big enough to
+ *	hold the frame, and sends it to protocol stack.  It
+ *	then resets the used buffer and appends it to the end
+ *	of the list.  If the frame was the last in the Rx
+ *	channel (EOC), the function restarts the receive channel
+ *	by sending an Rx Go command to the adapter.  Then it
+ *	activates/continues the activity LED.
+ *
+ **************************************************************/
+
+static u32 tlan_handle_rx_eof(struct net_device *dev, u16 host_int)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u32		ack = 0;
 	int		eoc = 0;
-	TLanList	*head_list;
+	struct tlan_list	*head_list;
 	struct sk_buff	*skb;
-	TLanList	*tail_list;
-	u16		tmpCStat;
+	struct tlan_list	*tail_list;
+	u16		tmp_c_stat;
 	dma_addr_t	head_list_phys;
 
-	TLAN_DBG( TLAN_DEBUG_RX, "RECEIVE:  Handling RX EOF (Head=%d Tail=%d)\n",
-		  priv->rxHead, priv->rxTail );
-	head_list = priv->rxList + priv->rxHead;
-	head_list_phys = priv->rxListDMA + sizeof(TLanList) * priv->rxHead;
+	TLAN_DBG(TLAN_DEBUG_RX, "RECEIVE:  handling RX EOF (Head=%d Tail=%d)\n",
+		 priv->rx_head, priv->rx_tail);
+	head_list = priv->rx_list + priv->rx_head;
+	head_list_phys =
+		priv->rx_list_dma + sizeof(struct tlan_list)*priv->rx_head;
 
-	while (((tmpCStat = head_list->cStat) & TLAN_CSTAT_FRM_CMP) && (ack < 255)) {
-		dma_addr_t frameDma = head_list->buffer[0].address;
-		u32 frameSize = head_list->frameSize;
+	while (((tmp_c_stat = head_list->c_stat) & TLAN_CSTAT_FRM_CMP)
+	       && (ack < 255)) {
+		dma_addr_t frame_dma = head_list->buffer[0].address;
+		u32 frame_size = head_list->frame_size;
 		struct sk_buff *new_skb;
 
 		ack++;
-		if (tmpCStat & TLAN_CSTAT_EOC)
+		if (tmp_c_stat & TLAN_CSTAT_EOC)
 			eoc = 1;
 
 		new_skb = netdev_alloc_skb_ip_align(dev,
 						    TLAN_MAX_FRAME_SIZE + 5);
-		if ( !new_skb )
+		if (!new_skb)
 			goto drop_and_reuse;
 
-		skb = TLan_GetSKB(head_list);
-		pci_unmap_single(priv->pciDev, frameDma,
+		skb = tlan_get_skb(head_list);
+		pci_unmap_single(priv->pci_dev, frame_dma,
 				 TLAN_MAX_FRAME_SIZE, PCI_DMA_FROMDEVICE);
-		skb_put( skb, frameSize );
+		skb_put(skb, frame_size);
 
-		dev->stats.rx_bytes += frameSize;
+		dev->stats.rx_bytes += frame_size;
 
-		skb->protocol = eth_type_trans( skb, dev );
-		netif_rx( skb );
+		skb->protocol = eth_type_trans(skb, dev);
+		netif_rx(skb);
 
-		head_list->buffer[0].address = pci_map_single(priv->pciDev,
-							      new_skb->data,
-							      TLAN_MAX_FRAME_SIZE,
-							      PCI_DMA_FROMDEVICE);
+		head_list->buffer[0].address =
+			pci_map_single(priv->pci_dev, new_skb->data,
+				       TLAN_MAX_FRAME_SIZE, PCI_DMA_FROMDEVICE);
 
-		TLan_StoreSKB(head_list, new_skb);
+		tlan_store_skb(head_list, new_skb);
 drop_and_reuse:
 		head_list->forward = 0;
-		head_list->cStat = 0;
-		tail_list = priv->rxList + priv->rxTail;
+		head_list->c_stat = 0;
+		tail_list = priv->rx_list + priv->rx_tail;
 		tail_list->forward = head_list_phys;
 
-		CIRC_INC( priv->rxHead, TLAN_NUM_RX_LISTS );
-		CIRC_INC( priv->rxTail, TLAN_NUM_RX_LISTS );
-		head_list = priv->rxList + priv->rxHead;
-		head_list_phys = priv->rxListDMA + sizeof(TLanList) * priv->rxHead;
+		CIRC_INC(priv->rx_head, TLAN_NUM_RX_LISTS);
+		CIRC_INC(priv->rx_tail, TLAN_NUM_RX_LISTS);
+		head_list = priv->rx_list + priv->rx_head;
+		head_list_phys = priv->rx_list_dma
+			+ sizeof(struct tlan_list)*priv->rx_head;
 	}
 
 	if (!ack)
-		printk(KERN_INFO "TLAN: Received interrupt for uncompleted RX frame.\n");
-
-
-	if ( eoc ) {
-		TLAN_DBG( TLAN_DEBUG_RX,
-			  "RECEIVE:  Handling RX EOC (Head=%d Tail=%d)\n",
-			  priv->rxHead, priv->rxTail );
-		head_list = priv->rxList + priv->rxHead;
-		head_list_phys = priv->rxListDMA + sizeof(TLanList) * priv->rxHead;
-		outl(head_list_phys, dev->base_addr + TLAN_CH_PARM );
+		netdev_info(dev,
+			    "Received interrupt for uncompleted RX frame\n");
+
+
+	if (eoc) {
+		TLAN_DBG(TLAN_DEBUG_RX,
+			 "RECEIVE:  handling RX EOC (Head=%d Tail=%d)\n",
+			 priv->rx_head, priv->rx_tail);
+		head_list = priv->rx_list + priv->rx_head;
+		head_list_phys = priv->rx_list_dma
+			+ sizeof(struct tlan_list)*priv->rx_head;
+		outl(head_list_phys, dev->base_addr + TLAN_CH_PARM);
 		ack |= TLAN_HC_GO | TLAN_HC_RT;
-		priv->rxEocCount++;
+		priv->rx_eoc_count++;
 	}
 
-	if ( priv->adapter->flags & TLAN_ADAPTER_ACTIVITY_LED ) {
-		TLan_DioWrite8( dev->base_addr,
-				TLAN_LED_REG, TLAN_LED_LINK | TLAN_LED_ACT );
-		if ( priv->timer.function == NULL )  {
-			priv->timer.function = TLan_Timer;
+	if (priv->adapter->flags & TLAN_ADAPTER_ACTIVITY_LED) {
+		tlan_dio_write8(dev->base_addr,
+				TLAN_LED_REG, TLAN_LED_LINK | TLAN_LED_ACT);
+		if (priv->timer.function == NULL)  {
+			priv->timer.function = tlan_timer;
 			priv->timer.data = (unsigned long) dev;
 			priv->timer.expires = jiffies + TLAN_TIMER_ACT_DELAY;
-			priv->timerSetAt = jiffies;
-			priv->timerType = TLAN_TIMER_ACTIVITY;
+			priv->timer_set_at = jiffies;
+			priv->timer_type = TLAN_TIMER_ACTIVITY;
 			add_timer(&priv->timer);
-		} else if ( priv->timerType == TLAN_TIMER_ACTIVITY ) {
-			priv->timerSetAt = jiffies;
+		} else if (priv->timer_type == TLAN_TIMER_ACTIVITY) {
+			priv->timer_set_at = jiffies;
 		}
 	}
 
 	return ack;
 
-} /* TLan_HandleRxEOF */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_HandleDummy
-	 *
-	 *	Returns:
-	 *		1
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This function handles the Dummy interrupt, which is
-	 *	raised whenever a test interrupt is generated by setting
-	 *	the Req_Int bit of HOST_CMD to 1.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_handle_dummy
+ *
+ *	Returns:
+ *		1
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This function handles the Dummy interrupt, which is
+ *	raised whenever a test interrupt is generated by setting
+ *	the Req_Int bit of HOST_CMD to 1.
+ *
+ **************************************************************/
 
-static u32 TLan_HandleDummy( struct net_device *dev, u16 host_int )
+static u32 tlan_handle_dummy(struct net_device *dev, u16 host_int)
 {
-	printk( "TLAN:  Test interrupt on %s.\n", dev->name );
+	netdev_info(dev, "Test interrupt\n");
 	return 1;
 
-} /* TLan_HandleDummy */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_HandleTxEOC
-	 *
-	 *	Returns:
-	 *		1
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This driver is structured to determine EOC occurrences by
-	 *	reading the CSTAT member of the list structure.  Tx EOC
-	 *	interrupts are disabled via the DIO INTDIS register.
-	 *	However, TLAN chips before revision 3.0 didn't have this
-	 *	functionality, so process EOC events if this is the
-	 *	case.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_handle_tx_eoc
+ *
+ *	Returns:
+ *		1
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This driver is structured to determine EOC occurrences by
+ *	reading the CSTAT member of the list structure.  Tx EOC
+ *	interrupts are disabled via the DIO INTDIS register.
+ *	However, TLAN chips before revision 3.0 didn't have this
+ *	functionality, so process EOC events if this is the
+ *	case.
+ *
+ **************************************************************/
 
-static u32 TLan_HandleTxEOC( struct net_device *dev, u16 host_int )
+static u32 tlan_handle_tx_eoc(struct net_device *dev, u16 host_int)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
-	TLanList		*head_list;
+	struct tlan_priv	*priv = netdev_priv(dev);
+	struct tlan_list		*head_list;
 	dma_addr_t		head_list_phys;
 	u32			ack = 1;
 
 	host_int = 0;
-	if ( priv->tlanRev < 0x30 ) {
-		TLAN_DBG( TLAN_DEBUG_TX,
-			  "TRANSMIT:  Handling TX EOC (Head=%d Tail=%d) -- IRQ\n",
-			  priv->txHead, priv->txTail );
-		head_list = priv->txList + priv->txHead;
-		head_list_phys = priv->txListDMA + sizeof(TLanList) * priv->txHead;
-		if ( ( head_list->cStat & TLAN_CSTAT_READY ) == TLAN_CSTAT_READY ) {
+	if (priv->tlan_rev < 0x30) {
+		TLAN_DBG(TLAN_DEBUG_TX,
+			 "TRANSMIT:  handling TX EOC (Head=%d Tail=%d) -- IRQ\n",
+			 priv->tx_head, priv->tx_tail);
+		head_list = priv->tx_list + priv->tx_head;
+		head_list_phys = priv->tx_list_dma
+			+ sizeof(struct tlan_list)*priv->tx_head;
+		if ((head_list->c_stat & TLAN_CSTAT_READY)
+		    == TLAN_CSTAT_READY) {
 			netif_stop_queue(dev);
-			outl( head_list_phys, dev->base_addr + TLAN_CH_PARM );
+			outl(head_list_phys, dev->base_addr + TLAN_CH_PARM);
 			ack |= TLAN_HC_GO;
 		} else {
-			priv->txInProgress = 0;
+			priv->tx_in_progress = 0;
 		}
 	}
 
 	return ack;
 
-} /* TLan_HandleTxEOC */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_HandleStatusCheck
-	 *
-	 *	Returns:
-	 *		0 if Adapter check, 1 if Network Status check.
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This function handles Adapter Check/Network Status
-	 *	interrupts generated by the adapter.  It checks the
-	 *	vector in the HOST_INT register to determine if it is
-	 *	an Adapter Check interrupt.  If so, it resets the
-	 *	adapter.  Otherwise it clears the status registers
-	 *	and services the PHY.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_handle_status_check
+ *
+ *	Returns:
+ *		0 if Adapter check, 1 if Network Status check.
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This function handles Adapter Check/Network Status
+ *	interrupts generated by the adapter.  It checks the
+ *	vector in the HOST_INT register to determine if it is
+ *	an Adapter Check interrupt.  If so, it resets the
+ *	adapter.  Otherwise it clears the status registers
+ *	and services the PHY.
+ *
+ **************************************************************/
 
-static u32 TLan_HandleStatusCheck( struct net_device *dev, u16 host_int )
+static u32 tlan_handle_status_check(struct net_device *dev, u16 host_int)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u32		ack;
 	u32		error;
 	u8		net_sts;
@@ -1727,92 +1668,94 @@ static u32 TLan_HandleStatusCheck( struct net_device *dev, u16 host_int )
 	u16		tlphy_sts;
 
 	ack = 1;
-	if ( host_int & TLAN_HI_IV_MASK ) {
-		netif_stop_queue( dev );
-		error = inl( dev->base_addr + TLAN_CH_PARM );
-		printk( "TLAN:  %s: Adaptor Error = 0x%x\n", dev->name, error );
-		TLan_ReadAndClearStats( dev, TLAN_RECORD );
-		outl( TLAN_HC_AD_RST, dev->base_addr + TLAN_HOST_CMD );
+	if (host_int & TLAN_HI_IV_MASK) {
+		netif_stop_queue(dev);
+		error = inl(dev->base_addr + TLAN_CH_PARM);
+		netdev_info(dev, "Adaptor Error = 0x%x\n", error);
+		tlan_read_and_clear_stats(dev, TLAN_RECORD);
+		outl(TLAN_HC_AD_RST, dev->base_addr + TLAN_HOST_CMD);
 
 		schedule_work(&priv->tlan_tqueue);
 
 		netif_wake_queue(dev);
 		ack = 0;
 	} else {
-		TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Status Check\n", dev->name );
-		phy = priv->phy[priv->phyNum];
-
-		net_sts = TLan_DioRead8( dev->base_addr, TLAN_NET_STS );
-		if ( net_sts ) {
-			TLan_DioWrite8( dev->base_addr, TLAN_NET_STS, net_sts );
-			TLAN_DBG( TLAN_DEBUG_GNRL, "%s:    Net_Sts = %x\n",
-				  dev->name, (unsigned) net_sts );
+		TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Status Check\n", dev->name);
+		phy = priv->phy[priv->phy_num];
+
+		net_sts = tlan_dio_read8(dev->base_addr, TLAN_NET_STS);
+		if (net_sts) {
+			tlan_dio_write8(dev->base_addr, TLAN_NET_STS, net_sts);
+			TLAN_DBG(TLAN_DEBUG_GNRL, "%s:    Net_Sts = %x\n",
+				 dev->name, (unsigned) net_sts);
 		}
-		if ( ( net_sts & TLAN_NET_STS_MIRQ ) &&  ( priv->phyNum == 0 ) ) {
-			TLan_MiiReadReg( dev, phy, TLAN_TLPHY_STS, &tlphy_sts );
-			TLan_MiiReadReg( dev, phy, TLAN_TLPHY_CTL, &tlphy_ctl );
-        		if ( ! ( tlphy_sts & TLAN_TS_POLOK ) &&
-			     ! ( tlphy_ctl & TLAN_TC_SWAPOL ) ) {
-                		tlphy_ctl |= TLAN_TC_SWAPOL;
-                		TLan_MiiWriteReg( dev, phy, TLAN_TLPHY_CTL, tlphy_ctl);
-			} else if ( ( tlphy_sts & TLAN_TS_POLOK ) &&
-				    ( tlphy_ctl & TLAN_TC_SWAPOL ) ) {
-                		tlphy_ctl &= ~TLAN_TC_SWAPOL;
-                		TLan_MiiWriteReg( dev, phy, TLAN_TLPHY_CTL, tlphy_ctl);
-        		}
-
-			if (debug) {
-				TLan_PhyPrint( dev );
+		if ((net_sts & TLAN_NET_STS_MIRQ) &&  (priv->phy_num == 0)) {
+			tlan_mii_read_reg(dev, phy, TLAN_TLPHY_STS, &tlphy_sts);
+			tlan_mii_read_reg(dev, phy, TLAN_TLPHY_CTL, &tlphy_ctl);
+			if (!(tlphy_sts & TLAN_TS_POLOK) &&
+			    !(tlphy_ctl & TLAN_TC_SWAPOL)) {
+				tlphy_ctl |= TLAN_TC_SWAPOL;
+				tlan_mii_write_reg(dev, phy, TLAN_TLPHY_CTL,
+						   tlphy_ctl);
+			} else if ((tlphy_sts & TLAN_TS_POLOK) &&
+				   (tlphy_ctl & TLAN_TC_SWAPOL)) {
+				tlphy_ctl &= ~TLAN_TC_SWAPOL;
+				tlan_mii_write_reg(dev, phy, TLAN_TLPHY_CTL,
+						   tlphy_ctl);
 			}
+
+			if (debug)
+				tlan_phy_print(dev);
 		}
 	}
 
 	return ack;
 
-} /* TLan_HandleStatusCheck */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_HandleRxEOC
-	 *
-	 *	Returns:
-	 *		1
-	 *	Parms:
-	 *		dev		Device assigned the IRQ that was
-	 *				raised.
-	 *		host_int	The contents of the HOST_INT
-	 *				port.
-	 *
-	 *	This driver is structured to determine EOC occurrences by
-	 *	reading the CSTAT member of the list structure.  Rx EOC
-	 *	interrupts are disabled via the DIO INTDIS register.
-	 *	However, TLAN chips before revision 3.0 didn't have this
-	 *	CSTAT member or a INTDIS register, so if this chip is
-	 *	pre-3.0, process EOC interrupts normally.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_handle_rx_eoc
+ *
+ *	Returns:
+ *		1
+ *	Parms:
+ *		dev		Device assigned the IRQ that was
+ *				raised.
+ *		host_int	The contents of the HOST_INT
+ *				port.
+ *
+ *	This driver is structured to determine EOC occurrences by
+ *	reading the CSTAT member of the list structure.  Rx EOC
+ *	interrupts are disabled via the DIO INTDIS register.
+ *	However, TLAN chips before revision 3.0 didn't have this
+ *	CSTAT member or a INTDIS register, so if this chip is
+ *	pre-3.0, process EOC interrupts normally.
+ *
+ **************************************************************/
 
-static u32 TLan_HandleRxEOC( struct net_device *dev, u16 host_int )
+static u32 tlan_handle_rx_eoc(struct net_device *dev, u16 host_int)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	dma_addr_t	head_list_phys;
 	u32		ack = 1;
 
-	if (  priv->tlanRev < 0x30 ) {
-		TLAN_DBG( TLAN_DEBUG_RX,
-			  "RECEIVE:  Handling RX EOC (Head=%d Tail=%d) -- IRQ\n",
-			  priv->rxHead, priv->rxTail );
-		head_list_phys = priv->rxListDMA + sizeof(TLanList) * priv->rxHead;
-		outl( head_list_phys, dev->base_addr + TLAN_CH_PARM );
+	if (priv->tlan_rev < 0x30) {
+		TLAN_DBG(TLAN_DEBUG_RX,
+			 "RECEIVE:  Handling RX EOC (head=%d tail=%d) -- IRQ\n",
+			 priv->rx_head, priv->rx_tail);
+		head_list_phys = priv->rx_list_dma
+			+ sizeof(struct tlan_list)*priv->rx_head;
+		outl(head_list_phys, dev->base_addr + TLAN_CH_PARM);
 		ack |= TLAN_HC_GO | TLAN_HC_RT;
-		priv->rxEocCount++;
+		priv->rx_eoc_count++;
 	}
 
 	return ack;
 
-} /* TLan_HandleRxEOC */
+}
 
 
 
@@ -1820,98 +1763,98 @@ static u32 TLan_HandleRxEOC( struct net_device *dev, u16 host_int )
 /*****************************************************************************
 ******************************************************************************
 
-	ThunderLAN Driver Timer Function
+ThunderLAN driver timer function
 
 ******************************************************************************
 *****************************************************************************/
 
 
-	/***************************************************************
-	 *	TLan_Timer
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		data	A value given to add timer when
-	 *			add_timer was called.
-	 *
-	 *	This function handles timed functionality for the
-	 *	TLAN driver.  The two current timer uses are for
-	 *	delaying for autonegotionation and driving the ACT LED.
-	 *	-	Autonegotiation requires being allowed about
-	 *		2 1/2 seconds before attempting to transmit a
-	 *		packet.  It would be a very bad thing to hang
-	 *		the kernel this long, so the driver doesn't
-	 *		allow transmission 'til after this time, for
-	 *		certain PHYs.  It would be much nicer if all
-	 *		PHYs were interrupt-capable like the internal
-	 *		PHY.
-	 *	-	The ACT LED, which shows adapter activity, is
-	 *		driven by the driver, and so must be left on
-	 *		for a short period to power up the LED so it
-	 *		can be seen.  This delay can be changed by
-	 *		changing the TLAN_TIMER_ACT_DELAY in tlan.h,
-	 *		if desired.  100 ms  produces a slightly
-	 *		sluggish response.
-	 *
-	 **************************************************************/
-
-static void TLan_Timer( unsigned long data )
+/***************************************************************
+ *	tlan_timer
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		data	A value given to add timer when
+ *			add_timer was called.
+ *
+ *	This function handles timed functionality for the
+ *	TLAN driver.  The two current timer uses are for
+ *	delaying for autonegotionation and driving the ACT LED.
+ *	-	Autonegotiation requires being allowed about
+ *		2 1/2 seconds before attempting to transmit a
+ *		packet.  It would be a very bad thing to hang
+ *		the kernel this long, so the driver doesn't
+ *		allow transmission 'til after this time, for
+ *		certain PHYs.  It would be much nicer if all
+ *		PHYs were interrupt-capable like the internal
+ *		PHY.
+ *	-	The ACT LED, which shows adapter activity, is
+ *		driven by the driver, and so must be left on
+ *		for a short period to power up the LED so it
+ *		can be seen.  This delay can be changed by
+ *		changing the TLAN_TIMER_ACT_DELAY in tlan.h,
+ *		if desired.  100 ms  produces a slightly
+ *		sluggish response.
+ *
+ **************************************************************/
+
+static void tlan_timer(unsigned long data)
 {
 	struct net_device	*dev = (struct net_device *) data;
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u32		elapsed;
 	unsigned long	flags = 0;
 
 	priv->timer.function = NULL;
 
-	switch ( priv->timerType ) {
+	switch (priv->timer_type) {
 #ifdef MONITOR
-		case TLAN_TIMER_LINK_BEAT:
-			TLan_PhyMonitor( dev );
-			break;
+	case TLAN_TIMER_LINK_BEAT:
+		tlan_phy_monitor(dev);
+		break;
 #endif
-		case TLAN_TIMER_PHY_PDOWN:
-			TLan_PhyPowerDown( dev );
-			break;
-		case TLAN_TIMER_PHY_PUP:
-			TLan_PhyPowerUp( dev );
-			break;
-		case TLAN_TIMER_PHY_RESET:
-			TLan_PhyReset( dev );
-			break;
-		case TLAN_TIMER_PHY_START_LINK:
-			TLan_PhyStartLink( dev );
-			break;
-		case TLAN_TIMER_PHY_FINISH_AN:
-			TLan_PhyFinishAutoNeg( dev );
-			break;
-		case TLAN_TIMER_FINISH_RESET:
-			TLan_FinishReset( dev );
-			break;
-		case TLAN_TIMER_ACTIVITY:
-			spin_lock_irqsave(&priv->lock, flags);
-			if ( priv->timer.function == NULL ) {
-				elapsed = jiffies - priv->timerSetAt;
-				if ( elapsed >= TLAN_TIMER_ACT_DELAY ) {
-					TLan_DioWrite8( dev->base_addr,
-							TLAN_LED_REG, TLAN_LED_LINK );
-				} else  {
-					priv->timer.function = TLan_Timer;
-					priv->timer.expires = priv->timerSetAt
-						+ TLAN_TIMER_ACT_DELAY;
-					spin_unlock_irqrestore(&priv->lock, flags);
-					add_timer( &priv->timer );
-					break;
-				}
+	case TLAN_TIMER_PHY_PDOWN:
+		tlan_phy_power_down(dev);
+		break;
+	case TLAN_TIMER_PHY_PUP:
+		tlan_phy_power_up(dev);
+		break;
+	case TLAN_TIMER_PHY_RESET:
+		tlan_phy_reset(dev);
+		break;
+	case TLAN_TIMER_PHY_START_LINK:
+		tlan_phy_start_link(dev);
+		break;
+	case TLAN_TIMER_PHY_FINISH_AN:
+		tlan_phy_finish_auto_neg(dev);
+		break;
+	case TLAN_TIMER_FINISH_RESET:
+		tlan_finish_reset(dev);
+		break;
+	case TLAN_TIMER_ACTIVITY:
+		spin_lock_irqsave(&priv->lock, flags);
+		if (priv->timer.function == NULL) {
+			elapsed = jiffies - priv->timer_set_at;
+			if (elapsed >= TLAN_TIMER_ACT_DELAY) {
+				tlan_dio_write8(dev->base_addr,
+						TLAN_LED_REG, TLAN_LED_LINK);
+			} else  {
+				priv->timer.function = tlan_timer;
+				priv->timer.expires = priv->timer_set_at
+					+ TLAN_TIMER_ACT_DELAY;
+				spin_unlock_irqrestore(&priv->lock, flags);
+				add_timer(&priv->timer);
+				break;
 			}
-			spin_unlock_irqrestore(&priv->lock, flags);
-			break;
-		default:
-			break;
+		}
+		spin_unlock_irqrestore(&priv->lock, flags);
+		break;
+	default:
+		break;
 	}
 
-} /* TLan_Timer */
+}
 
 
 
@@ -1919,39 +1862,39 @@ static void TLan_Timer( unsigned long data )
 /*****************************************************************************
 ******************************************************************************
 
-	ThunderLAN Driver Adapter Related Routines
+ThunderLAN driver adapter related routines
 
 ******************************************************************************
 *****************************************************************************/
 
 
-	/***************************************************************
-	 *	TLan_ResetLists
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev	The device structure with the list
-	 *			stuctures to be reset.
-	 *
-	 *	This routine sets the variables associated with managing
-	 *	the TLAN lists to their initial values.
-	 *
-	 **************************************************************/
-
-static void TLan_ResetLists( struct net_device *dev )
+/***************************************************************
+ *	tlan_reset_lists
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev	The device structure with the list
+ *			stuctures to be reset.
+ *
+ *	This routine sets the variables associated with managing
+ *	the TLAN lists to their initial values.
+ *
+ **************************************************************/
+
+static void tlan_reset_lists(struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	int		i;
-	TLanList	*list;
+	struct tlan_list	*list;
 	dma_addr_t	list_phys;
 	struct sk_buff	*skb;
 
-	priv->txHead = 0;
-	priv->txTail = 0;
-	for ( i = 0; i < TLAN_NUM_TX_LISTS; i++ ) {
-		list = priv->txList + i;
-		list->cStat = TLAN_CSTAT_UNUSED;
+	priv->tx_head = 0;
+	priv->tx_tail = 0;
+	for (i = 0; i < TLAN_NUM_TX_LISTS; i++) {
+		list = priv->tx_list + i;
+		list->c_stat = TLAN_CSTAT_UNUSED;
 		list->buffer[0].address = 0;
 		list->buffer[2].count = 0;
 		list->buffer[2].address = 0;
@@ -1959,169 +1902,169 @@ static void TLan_ResetLists( struct net_device *dev )
 		list->buffer[9].address = 0;
 	}
 
-	priv->rxHead = 0;
-	priv->rxTail = TLAN_NUM_RX_LISTS - 1;
-	for ( i = 0; i < TLAN_NUM_RX_LISTS; i++ ) {
-		list = priv->rxList + i;
-		list_phys = priv->rxListDMA + sizeof(TLanList) * i;
-		list->cStat = TLAN_CSTAT_READY;
-		list->frameSize = TLAN_MAX_FRAME_SIZE;
+	priv->rx_head = 0;
+	priv->rx_tail = TLAN_NUM_RX_LISTS - 1;
+	for (i = 0; i < TLAN_NUM_RX_LISTS; i++) {
+		list = priv->rx_list + i;
+		list_phys = priv->rx_list_dma + sizeof(struct tlan_list)*i;
+		list->c_stat = TLAN_CSTAT_READY;
+		list->frame_size = TLAN_MAX_FRAME_SIZE;
 		list->buffer[0].count = TLAN_MAX_FRAME_SIZE | TLAN_LAST_BUFFER;
 		skb = netdev_alloc_skb_ip_align(dev, TLAN_MAX_FRAME_SIZE + 5);
-		if ( !skb ) {
-			pr_err("TLAN: out of memory for received data.\n" );
+		if (!skb) {
+			netdev_err(dev, "Out of memory for received data\n");
 			break;
 		}
 
-		list->buffer[0].address = pci_map_single(priv->pciDev,
+		list->buffer[0].address = pci_map_single(priv->pci_dev,
 							 skb->data,
 							 TLAN_MAX_FRAME_SIZE,
 							 PCI_DMA_FROMDEVICE);
-		TLan_StoreSKB(list, skb);
+		tlan_store_skb(list, skb);
 		list->buffer[1].count = 0;
 		list->buffer[1].address = 0;
-		list->forward = list_phys + sizeof(TLanList);
+		list->forward = list_phys + sizeof(struct tlan_list);
 	}
 
 	/* in case ran out of memory early, clear bits */
 	while (i < TLAN_NUM_RX_LISTS) {
-		TLan_StoreSKB(priv->rxList + i, NULL);
+		tlan_store_skb(priv->rx_list + i, NULL);
 		++i;
 	}
 	list->forward = 0;
 
-} /* TLan_ResetLists */
+}
 
 
-static void TLan_FreeLists( struct net_device *dev )
+static void tlan_free_lists(struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	int		i;
-	TLanList	*list;
+	struct tlan_list	*list;
 	struct sk_buff	*skb;
 
-	for ( i = 0; i < TLAN_NUM_TX_LISTS; i++ ) {
-		list = priv->txList + i;
-		skb = TLan_GetSKB(list);
-		if ( skb ) {
+	for (i = 0; i < TLAN_NUM_TX_LISTS; i++) {
+		list = priv->tx_list + i;
+		skb = tlan_get_skb(list);
+		if (skb) {
 			pci_unmap_single(
-				priv->pciDev,
+				priv->pci_dev,
 				list->buffer[0].address,
 				max(skb->len,
 				    (unsigned int)TLAN_MIN_FRAME_SIZE),
 				PCI_DMA_TODEVICE);
-			dev_kfree_skb_any( skb );
+			dev_kfree_skb_any(skb);
 			list->buffer[8].address = 0;
 			list->buffer[9].address = 0;
 		}
 	}
 
-	for ( i = 0; i < TLAN_NUM_RX_LISTS; i++ ) {
-		list = priv->rxList + i;
-		skb = TLan_GetSKB(list);
-		if ( skb ) {
-			pci_unmap_single(priv->pciDev,
+	for (i = 0; i < TLAN_NUM_RX_LISTS; i++) {
+		list = priv->rx_list + i;
+		skb = tlan_get_skb(list);
+		if (skb) {
+			pci_unmap_single(priv->pci_dev,
 					 list->buffer[0].address,
 					 TLAN_MAX_FRAME_SIZE,
 					 PCI_DMA_FROMDEVICE);
-			dev_kfree_skb_any( skb );
+			dev_kfree_skb_any(skb);
 			list->buffer[8].address = 0;
 			list->buffer[9].address = 0;
 		}
 	}
-} /* TLan_FreeLists */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_PrintDio
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		io_base		Base IO port of the device of
-	 *				which to print DIO registers.
-	 *
-	 *	This function prints out all the internal (DIO)
-	 *	registers of a TLAN chip.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_print_dio
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		io_base		Base IO port of the device of
+ *				which to print DIO registers.
+ *
+ *	This function prints out all the internal (DIO)
+ *	registers of a TLAN chip.
+ *
+ **************************************************************/
 
-static void TLan_PrintDio( u16 io_base )
+static void tlan_print_dio(u16 io_base)
 {
 	u32 data0, data1;
 	int	i;
 
-	printk( "TLAN:   Contents of internal registers for io base 0x%04hx.\n",
-		io_base );
-	printk( "TLAN:      Off.  +0         +4\n" );
-	for ( i = 0; i < 0x4C; i+= 8 ) {
-		data0 = TLan_DioRead32( io_base, i );
-		data1 = TLan_DioRead32( io_base, i + 0x4 );
-		printk( "TLAN:      0x%02x  0x%08x 0x%08x\n", i, data0, data1 );
+	pr_info("Contents of internal registers for io base 0x%04hx\n",
+		io_base);
+	pr_info("Off.  +0        +4\n");
+	for (i = 0; i < 0x4C; i += 8) {
+		data0 = tlan_dio_read32(io_base, i);
+		data1 = tlan_dio_read32(io_base, i + 0x4);
+		pr_info("0x%02x  0x%08x 0x%08x\n", i, data0, data1);
 	}
 
-} /* TLan_PrintDio */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_PrintList
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		list	A pointer to the TLanList structure to
-	 *			be printed.
-	 *		type	A string to designate type of list,
-	 *			"Rx" or "Tx".
-	 *		num	The index of the list.
-	 *
-	 *	This function prints out the contents of the list
-	 *	pointed to by the list parameter.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	TLan_PrintList
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		list	A pointer to the struct tlan_list structure to
+ *			be printed.
+ *		type	A string to designate type of list,
+ *			"Rx" or "Tx".
+ *		num	The index of the list.
+ *
+ *	This function prints out the contents of the list
+ *	pointed to by the list parameter.
+ *
+ **************************************************************/
 
-static void TLan_PrintList( TLanList *list, char *type, int num)
+static void tlan_print_list(struct tlan_list *list, char *type, int num)
 {
 	int i;
 
-	printk( "TLAN:   %s List %d at %p\n", type, num, list );
-	printk( "TLAN:      Forward    = 0x%08x\n",  list->forward );
-	printk( "TLAN:      CSTAT      = 0x%04hx\n", list->cStat );
-	printk( "TLAN:      Frame Size = 0x%04hx\n", list->frameSize );
-	/* for ( i = 0; i < 10; i++ ) { */
-	for ( i = 0; i < 2; i++ ) {
-		printk( "TLAN:      Buffer[%d].count, addr = 0x%08x, 0x%08x\n",
-			i, list->buffer[i].count, list->buffer[i].address );
+	pr_info("%s List %d at %p\n", type, num, list);
+	pr_info("   Forward    = 0x%08x\n",  list->forward);
+	pr_info("   CSTAT      = 0x%04hx\n", list->c_stat);
+	pr_info("   Frame Size = 0x%04hx\n", list->frame_size);
+	/* for (i = 0; i < 10; i++) { */
+	for (i = 0; i < 2; i++) {
+		pr_info("   Buffer[%d].count, addr = 0x%08x, 0x%08x\n",
+			i, list->buffer[i].count, list->buffer[i].address);
 	}
 
-} /* TLan_PrintList */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_ReadAndClearStats
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev	Pointer to device structure of adapter
-	 *			to which to read stats.
-	 *		record	Flag indicating whether to add
-	 *
-	 *	This functions reads all the internal status registers
-	 *	of the TLAN chip, which clears them as a side effect.
-	 *	It then either adds the values to the device's status
-	 *	struct, or discards them, depending on whether record
-	 *	is TLAN_RECORD (!=0)  or TLAN_IGNORE (==0).
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_read_and_clear_stats
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev	Pointer to device structure of adapter
+ *			to which to read stats.
+ *		record	Flag indicating whether to add
+ *
+ *	This functions reads all the internal status registers
+ *	of the TLAN chip, which clears them as a side effect.
+ *	It then either adds the values to the device's status
+ *	struct, or discards them, depending on whether record
+ *	is TLAN_RECORD (!=0)  or TLAN_IGNORE (==0).
+ *
+ **************************************************************/
 
-static void TLan_ReadAndClearStats( struct net_device *dev, int record )
+static void tlan_read_and_clear_stats(struct net_device *dev, int record)
 {
 	u32		tx_good, tx_under;
 	u32		rx_good, rx_over;
@@ -2129,41 +2072,42 @@ static void TLan_ReadAndClearStats( struct net_device *dev, int record )
 	u32		multi_col, single_col;
 	u32		excess_col, late_col, loss;
 
-	outw( TLAN_GOOD_TX_FRMS, dev->base_addr + TLAN_DIO_ADR );
-	tx_good  = inb( dev->base_addr + TLAN_DIO_DATA );
-	tx_good += inb( dev->base_addr + TLAN_DIO_DATA + 1 ) << 8;
-	tx_good += inb( dev->base_addr + TLAN_DIO_DATA + 2 ) << 16;
-	tx_under = inb( dev->base_addr + TLAN_DIO_DATA + 3 );
-
-	outw( TLAN_GOOD_RX_FRMS, dev->base_addr + TLAN_DIO_ADR );
-	rx_good  = inb( dev->base_addr + TLAN_DIO_DATA );
-	rx_good += inb( dev->base_addr + TLAN_DIO_DATA + 1 ) << 8;
-	rx_good += inb( dev->base_addr + TLAN_DIO_DATA + 2 ) << 16;
-	rx_over  = inb( dev->base_addr + TLAN_DIO_DATA + 3 );
-
-	outw( TLAN_DEFERRED_TX, dev->base_addr + TLAN_DIO_ADR );
-	def_tx  = inb( dev->base_addr + TLAN_DIO_DATA );
-	def_tx += inb( dev->base_addr + TLAN_DIO_DATA + 1 ) << 8;
-	crc     = inb( dev->base_addr + TLAN_DIO_DATA + 2 );
-	code    = inb( dev->base_addr + TLAN_DIO_DATA + 3 );
-
-	outw( TLAN_MULTICOL_FRMS, dev->base_addr + TLAN_DIO_ADR );
-	multi_col   = inb( dev->base_addr + TLAN_DIO_DATA );
-	multi_col  += inb( dev->base_addr + TLAN_DIO_DATA + 1 ) << 8;
-	single_col  = inb( dev->base_addr + TLAN_DIO_DATA + 2 );
-	single_col += inb( dev->base_addr + TLAN_DIO_DATA + 3 ) << 8;
-
-	outw( TLAN_EXCESSCOL_FRMS, dev->base_addr + TLAN_DIO_ADR );
-	excess_col = inb( dev->base_addr + TLAN_DIO_DATA );
-	late_col   = inb( dev->base_addr + TLAN_DIO_DATA + 1 );
-	loss       = inb( dev->base_addr + TLAN_DIO_DATA + 2 );
-
-	if ( record ) {
+	outw(TLAN_GOOD_TX_FRMS, dev->base_addr + TLAN_DIO_ADR);
+	tx_good  = inb(dev->base_addr + TLAN_DIO_DATA);
+	tx_good += inb(dev->base_addr + TLAN_DIO_DATA + 1) << 8;
+	tx_good += inb(dev->base_addr + TLAN_DIO_DATA + 2) << 16;
+	tx_under = inb(dev->base_addr + TLAN_DIO_DATA + 3);
+
+	outw(TLAN_GOOD_RX_FRMS, dev->base_addr + TLAN_DIO_ADR);
+	rx_good  = inb(dev->base_addr + TLAN_DIO_DATA);
+	rx_good += inb(dev->base_addr + TLAN_DIO_DATA + 1) << 8;
+	rx_good += inb(dev->base_addr + TLAN_DIO_DATA + 2) << 16;
+	rx_over  = inb(dev->base_addr + TLAN_DIO_DATA + 3);
+
+	outw(TLAN_DEFERRED_TX, dev->base_addr + TLAN_DIO_ADR);
+	def_tx  = inb(dev->base_addr + TLAN_DIO_DATA);
+	def_tx += inb(dev->base_addr + TLAN_DIO_DATA + 1) << 8;
+	crc     = inb(dev->base_addr + TLAN_DIO_DATA + 2);
+	code    = inb(dev->base_addr + TLAN_DIO_DATA + 3);
+
+	outw(TLAN_MULTICOL_FRMS, dev->base_addr + TLAN_DIO_ADR);
+	multi_col   = inb(dev->base_addr + TLAN_DIO_DATA);
+	multi_col  += inb(dev->base_addr + TLAN_DIO_DATA + 1) << 8;
+	single_col  = inb(dev->base_addr + TLAN_DIO_DATA + 2);
+	single_col += inb(dev->base_addr + TLAN_DIO_DATA + 3) << 8;
+
+	outw(TLAN_EXCESSCOL_FRMS, dev->base_addr + TLAN_DIO_ADR);
+	excess_col = inb(dev->base_addr + TLAN_DIO_DATA);
+	late_col   = inb(dev->base_addr + TLAN_DIO_DATA + 1);
+	loss       = inb(dev->base_addr + TLAN_DIO_DATA + 2);
+
+	if (record) {
 		dev->stats.rx_packets += rx_good;
 		dev->stats.rx_errors  += rx_over + crc + code;
 		dev->stats.tx_packets += tx_good;
 		dev->stats.tx_errors  += tx_under + loss;
-		dev->stats.collisions += multi_col + single_col + excess_col + late_col;
+		dev->stats.collisions += multi_col
+			+ single_col + excess_col + late_col;
 
 		dev->stats.rx_over_errors    += rx_over;
 		dev->stats.rx_crc_errors     += crc;
@@ -2173,39 +2117,39 @@ static void TLan_ReadAndClearStats( struct net_device *dev, int record )
 		dev->stats.tx_carrier_errors += loss;
 	}
 
-} /* TLan_ReadAndClearStats */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_Reset
-	 *
-	 *	Returns:
-	 *		0
-	 *	Parms:
-	 *		dev	Pointer to device structure of adapter
-	 *			to be reset.
-	 *
-	 *	This function resets the adapter and it's physical
-	 *	device.  See Chap. 3, pp. 9-10 of the "ThunderLAN
-	 *	Programmer's Guide" for details.  The routine tries to
-	 *	implement what is detailed there, though adjustments
-	 *	have been made.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	TLan_Reset
+ *
+ *	Returns:
+ *		0
+ *	Parms:
+ *		dev	Pointer to device structure of adapter
+ *			to be reset.
+ *
+ *	This function resets the adapter and it's physical
+ *	device.  See Chap. 3, pp. 9-10 of the "ThunderLAN
+ *	Programmer's Guide" for details.  The routine tries to
+ *	implement what is detailed there, though adjustments
+ *	have been made.
+ *
+ **************************************************************/
 
 static void
-TLan_ResetAdapter( struct net_device *dev )
+tlan_reset_adapter(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	int		i;
 	u32		addr;
 	u32		data;
 	u8		data8;
 
-	priv->tlanFullDuplex = false;
-	priv->phyOnline=0;
+	priv->tlan_full_duplex = false;
+	priv->phy_online = 0;
 	netif_carrier_off(dev);
 
 /*  1.	Assert reset bit. */
@@ -2216,7 +2160,7 @@ TLan_ResetAdapter( struct net_device *dev )
 
 	udelay(1000);
 
-/*  2.	Turn off interrupts. ( Probably isn't necessary ) */
+/*  2.	Turn off interrupts. (Probably isn't necessary) */
 
 	data = inl(dev->base_addr + TLAN_HOST_CMD);
 	data |= TLAN_HC_INT_OFF;
@@ -2224,207 +2168,204 @@ TLan_ResetAdapter( struct net_device *dev )
 
 /*  3.	Clear AREGs and HASHs. */
 
- 	for ( i = TLAN_AREG_0; i <= TLAN_HASH_2; i += 4 ) {
-		TLan_DioWrite32( dev->base_addr, (u16) i, 0 );
-	}
+	for (i = TLAN_AREG_0; i <= TLAN_HASH_2; i += 4)
+		tlan_dio_write32(dev->base_addr, (u16) i, 0);
 
 /*  4.	Setup NetConfig register. */
 
 	data = TLAN_NET_CFG_1FRAG | TLAN_NET_CFG_1CHAN | TLAN_NET_CFG_PHY_EN;
-	TLan_DioWrite16( dev->base_addr, TLAN_NET_CONFIG, (u16) data );
+	tlan_dio_write16(dev->base_addr, TLAN_NET_CONFIG, (u16) data);
 
 /*  5.	Load Ld_Tmr and Ld_Thr in HOST_CMD. */
 
- 	outl( TLAN_HC_LD_TMR | 0x3f, dev->base_addr + TLAN_HOST_CMD );
- 	outl( TLAN_HC_LD_THR | 0x9, dev->base_addr + TLAN_HOST_CMD );
+	outl(TLAN_HC_LD_TMR | 0x3f, dev->base_addr + TLAN_HOST_CMD);
+	outl(TLAN_HC_LD_THR | 0x9, dev->base_addr + TLAN_HOST_CMD);
 
 /*  6.	Unreset the MII by setting NMRST (in NetSio) to 1. */
 
-	outw( TLAN_NET_SIO, dev->base_addr + TLAN_DIO_ADR );
+	outw(TLAN_NET_SIO, dev->base_addr + TLAN_DIO_ADR);
 	addr = dev->base_addr + TLAN_DIO_DATA + TLAN_NET_SIO;
-	TLan_SetBit( TLAN_NET_SIO_NMRST, addr );
+	tlan_set_bit(TLAN_NET_SIO_NMRST, addr);
 
 /*  7.	Setup the remaining registers. */
 
-	if ( priv->tlanRev >= 0x30 ) {
+	if (priv->tlan_rev >= 0x30) {
 		data8 = TLAN_ID_TX_EOC | TLAN_ID_RX_EOC;
-		TLan_DioWrite8( dev->base_addr, TLAN_INT_DIS, data8 );
+		tlan_dio_write8(dev->base_addr, TLAN_INT_DIS, data8);
 	}
-	TLan_PhyDetect( dev );
+	tlan_phy_detect(dev);
 	data = TLAN_NET_CFG_1FRAG | TLAN_NET_CFG_1CHAN;
 
-	if ( priv->adapter->flags & TLAN_ADAPTER_BIT_RATE_PHY ) {
+	if (priv->adapter->flags & TLAN_ADAPTER_BIT_RATE_PHY) {
 		data |= TLAN_NET_CFG_BIT;
-		if ( priv->aui == 1 ) {
-			TLan_DioWrite8( dev->base_addr, TLAN_ACOMMIT, 0x0a );
-		} else if ( priv->duplex == TLAN_DUPLEX_FULL ) {
-			TLan_DioWrite8( dev->base_addr, TLAN_ACOMMIT, 0x00 );
-			priv->tlanFullDuplex = true;
+		if (priv->aui == 1) {
+			tlan_dio_write8(dev->base_addr, TLAN_ACOMMIT, 0x0a);
+		} else if (priv->duplex == TLAN_DUPLEX_FULL) {
+			tlan_dio_write8(dev->base_addr, TLAN_ACOMMIT, 0x00);
+			priv->tlan_full_duplex = true;
 		} else {
-			TLan_DioWrite8( dev->base_addr, TLAN_ACOMMIT, 0x08 );
+			tlan_dio_write8(dev->base_addr, TLAN_ACOMMIT, 0x08);
 		}
 	}
 
-	if ( priv->phyNum == 0 ) {
+	if (priv->phy_num == 0)
 		data |= TLAN_NET_CFG_PHY_EN;
-	}
-	TLan_DioWrite16( dev->base_addr, TLAN_NET_CONFIG, (u16) data );
+	tlan_dio_write16(dev->base_addr, TLAN_NET_CONFIG, (u16) data);
 
-	if ( priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY ) {
-		TLan_FinishReset( dev );
-	} else {
-		TLan_PhyPowerDown( dev );
-	}
+	if (priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY)
+		tlan_finish_reset(dev);
+	else
+		tlan_phy_power_down(dev);
 
-} /* TLan_ResetAdapter */
+}
 
 
 
 
 static void
-TLan_FinishReset( struct net_device *dev )
+tlan_finish_reset(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u8		data;
 	u32		phy;
 	u8		sio;
 	u16		status;
 	u16		partner;
 	u16		tlphy_ctl;
-	u16 		tlphy_par;
+	u16		tlphy_par;
 	u16		tlphy_id1, tlphy_id2;
-	int 		i;
+	int		i;
 
-	phy = priv->phy[priv->phyNum];
+	phy = priv->phy[priv->phy_num];
 
 	data = TLAN_NET_CMD_NRESET | TLAN_NET_CMD_NWRAP;
-	if ( priv->tlanFullDuplex ) {
+	if (priv->tlan_full_duplex)
 		data |= TLAN_NET_CMD_DUPLEX;
-	}
-	TLan_DioWrite8( dev->base_addr, TLAN_NET_CMD, data );
+	tlan_dio_write8(dev->base_addr, TLAN_NET_CMD, data);
 	data = TLAN_NET_MASK_MASK4 | TLAN_NET_MASK_MASK5;
-	if ( priv->phyNum == 0 ) {
+	if (priv->phy_num == 0)
 		data |= TLAN_NET_MASK_MASK7;
-	}
-	TLan_DioWrite8( dev->base_addr, TLAN_NET_MASK, data );
-	TLan_DioWrite16( dev->base_addr, TLAN_MAX_RX, ((1536)+7)&~7 );
-	TLan_MiiReadReg( dev, phy, MII_GEN_ID_HI, &tlphy_id1 );
-	TLan_MiiReadReg( dev, phy, MII_GEN_ID_LO, &tlphy_id2 );
+	tlan_dio_write8(dev->base_addr, TLAN_NET_MASK, data);
+	tlan_dio_write16(dev->base_addr, TLAN_MAX_RX, ((1536)+7)&~7);
+	tlan_mii_read_reg(dev, phy, MII_GEN_ID_HI, &tlphy_id1);
+	tlan_mii_read_reg(dev, phy, MII_GEN_ID_LO, &tlphy_id2);
 
-	if ( ( priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY ) ||
-	     ( priv->aui ) ) {
+	if ((priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY) ||
+	    (priv->aui)) {
 		status = MII_GS_LINK;
-		printk( "TLAN:  %s: Link forced.\n", dev->name );
+		netdev_info(dev, "Link forced\n");
 	} else {
-		TLan_MiiReadReg( dev, phy, MII_GEN_STS, &status );
-		udelay( 1000 );
-		TLan_MiiReadReg( dev, phy, MII_GEN_STS, &status );
-		if ( (status & MII_GS_LINK) &&
-		     /* We only support link info on Nat.Sem. PHY's */
-			(tlphy_id1 == NAT_SEM_ID1) &&
-			(tlphy_id2 == NAT_SEM_ID2) ) {
-			TLan_MiiReadReg( dev, phy, MII_AN_LPA, &partner );
-			TLan_MiiReadReg( dev, phy, TLAN_TLPHY_PAR, &tlphy_par );
-
-			printk( "TLAN: %s: Link active with ", dev->name );
-			if (!(tlphy_par & TLAN_PHY_AN_EN_STAT)) {
-			      	 printk( "forced 10%sMbps %s-Duplex\n",
-					 tlphy_par & TLAN_PHY_SPEED_100 ? "" : "0",
-					 tlphy_par & TLAN_PHY_DUPLEX_FULL ? "Full" : "Half");
-			} else {
-				printk( "AutoNegotiation enabled, at 10%sMbps %s-Duplex\n",
-					tlphy_par & TLAN_PHY_SPEED_100 ? "" : "0",
-					tlphy_par & TLAN_PHY_DUPLEX_FULL ? "Full" : "Half");
-				printk("TLAN: Partner capability: ");
-					for (i = 5; i <= 10; i++)
-						if (partner & (1<<i))
-							printk("%s",media[i-5]);
-				printk("\n");
+		tlan_mii_read_reg(dev, phy, MII_GEN_STS, &status);
+		udelay(1000);
+		tlan_mii_read_reg(dev, phy, MII_GEN_STS, &status);
+		if ((status & MII_GS_LINK) &&
+		    /* We only support link info on Nat.Sem. PHY's */
+		    (tlphy_id1 == NAT_SEM_ID1) &&
+		    (tlphy_id2 == NAT_SEM_ID2)) {
+			tlan_mii_read_reg(dev, phy, MII_AN_LPA, &partner);
+			tlan_mii_read_reg(dev, phy, TLAN_TLPHY_PAR, &tlphy_par);
+
+			netdev_info(dev,
+				    "Link active with %s %uMbps %s-Duplex\n",
+				    !(tlphy_par & TLAN_PHY_AN_EN_STAT)
+				    ? "forced" : "Autonegotiation enabled,",
+				    tlphy_par & TLAN_PHY_SPEED_100
+				    ? 100 : 10,
+				    tlphy_par & TLAN_PHY_DUPLEX_FULL
+				    ? "Full" : "Half");
+
+			if (tlphy_par & TLAN_PHY_AN_EN_STAT) {
+				netdev_info(dev, "Partner capability:");
+				for (i = 5; i < 10; i++)
+					if (partner & (1 << i))
+						pr_cont(" %s", media[i-5]);
+				pr_cont("\n");
 			}
 
-			TLan_DioWrite8( dev->base_addr, TLAN_LED_REG, TLAN_LED_LINK );
+			tlan_dio_write8(dev->base_addr, TLAN_LED_REG,
+					TLAN_LED_LINK);
 #ifdef MONITOR
 			/* We have link beat..for now anyway */
-	        	priv->link = 1;
-	        	/*Enabling link beat monitoring */
-			TLan_SetTimer( dev, (10*HZ), TLAN_TIMER_LINK_BEAT );
+			priv->link = 1;
+			/*Enabling link beat monitoring */
+			tlan_set_timer(dev, (10*HZ), TLAN_TIMER_LINK_BEAT);
 #endif
 		} else if (status & MII_GS_LINK)  {
-			printk( "TLAN: %s: Link active\n", dev->name );
-			TLan_DioWrite8( dev->base_addr, TLAN_LED_REG, TLAN_LED_LINK );
+			netdev_info(dev, "Link active\n");
+			tlan_dio_write8(dev->base_addr, TLAN_LED_REG,
+					TLAN_LED_LINK);
 		}
 	}
 
-	if ( priv->phyNum == 0 ) {
-        	TLan_MiiReadReg( dev, phy, TLAN_TLPHY_CTL, &tlphy_ctl );
-        	tlphy_ctl |= TLAN_TC_INTEN;
-        	TLan_MiiWriteReg( dev, phy, TLAN_TLPHY_CTL, tlphy_ctl );
-        	sio = TLan_DioRead8( dev->base_addr, TLAN_NET_SIO );
-        	sio |= TLAN_NET_SIO_MINTEN;
-        	TLan_DioWrite8( dev->base_addr, TLAN_NET_SIO, sio );
-	}
-
-	if ( status & MII_GS_LINK ) {
-		TLan_SetMac( dev, 0, dev->dev_addr );
-		priv->phyOnline = 1;
-		outb( ( TLAN_HC_INT_ON >> 8 ), dev->base_addr + TLAN_HOST_CMD + 1 );
-		if ( debug >= 1 && debug != TLAN_DEBUG_PROBE ) {
-			outb( ( TLAN_HC_REQ_INT >> 8 ), dev->base_addr + TLAN_HOST_CMD + 1 );
-		}
-		outl( priv->rxListDMA, dev->base_addr + TLAN_CH_PARM );
-		outl( TLAN_HC_GO | TLAN_HC_RT, dev->base_addr + TLAN_HOST_CMD );
+	if (priv->phy_num == 0) {
+		tlan_mii_read_reg(dev, phy, TLAN_TLPHY_CTL, &tlphy_ctl);
+		tlphy_ctl |= TLAN_TC_INTEN;
+		tlan_mii_write_reg(dev, phy, TLAN_TLPHY_CTL, tlphy_ctl);
+		sio = tlan_dio_read8(dev->base_addr, TLAN_NET_SIO);
+		sio |= TLAN_NET_SIO_MINTEN;
+		tlan_dio_write8(dev->base_addr, TLAN_NET_SIO, sio);
+	}
+
+	if (status & MII_GS_LINK) {
+		tlan_set_mac(dev, 0, dev->dev_addr);
+		priv->phy_online = 1;
+		outb((TLAN_HC_INT_ON >> 8), dev->base_addr + TLAN_HOST_CMD + 1);
+		if (debug >= 1 && debug != TLAN_DEBUG_PROBE)
+			outb((TLAN_HC_REQ_INT >> 8),
+			     dev->base_addr + TLAN_HOST_CMD + 1);
+		outl(priv->rx_list_dma, dev->base_addr + TLAN_CH_PARM);
+		outl(TLAN_HC_GO | TLAN_HC_RT, dev->base_addr + TLAN_HOST_CMD);
 		netif_carrier_on(dev);
 	} else {
-		printk( "TLAN: %s: Link inactive, will retry in 10 secs...\n",
-			dev->name );
-		TLan_SetTimer( dev, (10*HZ), TLAN_TIMER_FINISH_RESET );
+		netdev_info(dev, "Link inactive, will retry in 10 secs...\n");
+		tlan_set_timer(dev, (10*HZ), TLAN_TIMER_FINISH_RESET);
 		return;
 	}
-	TLan_SetMulticastList(dev);
+	tlan_set_multicast_list(dev);
 
-} /* TLan_FinishReset */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_SetMac
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev	Pointer to device structure of adapter
-	 *			on which to change the AREG.
-	 *		areg	The AREG to set the address in (0 - 3).
-	 *		mac	A pointer to an array of chars.  Each
-	 *			element stores one byte of the address.
-	 *			IE, it isn't in ascii.
-	 *
-	 *	This function transfers a MAC address to one of the
-	 *	TLAN AREGs (address registers).  The TLAN chip locks
-	 *	the register on writing to offset 0 and unlocks the
-	 *	register after writing to offset 5.  If NULL is passed
-	 *	in mac, then the AREG is filled with 0's.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_set_mac
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev	Pointer to device structure of adapter
+ *			on which to change the AREG.
+ *		areg	The AREG to set the address in (0 - 3).
+ *		mac	A pointer to an array of chars.  Each
+ *			element stores one byte of the address.
+ *			IE, it isn't in ascii.
+ *
+ *	This function transfers a MAC address to one of the
+ *	TLAN AREGs (address registers).  The TLAN chip locks
+ *	the register on writing to offset 0 and unlocks the
+ *	register after writing to offset 5.  If NULL is passed
+ *	in mac, then the AREG is filled with 0's.
+ *
+ **************************************************************/
 
-static void TLan_SetMac( struct net_device *dev, int areg, char *mac )
+static void tlan_set_mac(struct net_device *dev, int areg, char *mac)
 {
 	int i;
 
 	areg *= 6;
 
-	if ( mac != NULL ) {
-		for ( i = 0; i < 6; i++ )
-			TLan_DioWrite8( dev->base_addr,
-					TLAN_AREG_0 + areg + i, mac[i] );
+	if (mac != NULL) {
+		for (i = 0; i < 6; i++)
+			tlan_dio_write8(dev->base_addr,
+					TLAN_AREG_0 + areg + i, mac[i]);
 	} else {
-		for ( i = 0; i < 6; i++ )
-			TLan_DioWrite8( dev->base_addr,
-					TLAN_AREG_0 + areg + i, 0 );
+		for (i = 0; i < 6; i++)
+			tlan_dio_write8(dev->base_addr,
+					TLAN_AREG_0 + areg + i, 0);
 	}
 
-} /* TLan_SetMac */
+}
 
 
 
@@ -2432,205 +2373,199 @@ static void TLan_SetMac( struct net_device *dev, int areg, char *mac )
 /*****************************************************************************
 ******************************************************************************
 
-	ThunderLAN Driver PHY Layer Routines
+ThunderLAN driver PHY layer routines
 
 ******************************************************************************
 *****************************************************************************/
 
 
 
-	/*********************************************************************
-	 *	TLan_PhyPrint
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev	A pointer to the device structure of the
-	 *			TLAN device having the PHYs to be detailed.
-	 *
-	 *	This function prints the registers a PHY (aka transceiver).
-	 *
-	 ********************************************************************/
+/*********************************************************************
+ *	tlan_phy_print
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev	A pointer to the device structure of the
+ *			TLAN device having the PHYs to be detailed.
+ *
+ *	This function prints the registers a PHY (aka transceiver).
+ *
+ ********************************************************************/
 
-static void TLan_PhyPrint( struct net_device *dev )
+static void tlan_phy_print(struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	u16 i, data0, data1, data2, data3, phy;
 
-	phy = priv->phy[priv->phyNum];
-
-	if ( priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY ) {
-		printk( "TLAN:   Device %s, Unmanaged PHY.\n", dev->name );
-	} else if ( phy <= TLAN_PHY_MAX_ADDR ) {
-		printk( "TLAN:   Device %s, PHY 0x%02x.\n", dev->name, phy );
-		printk( "TLAN:      Off.  +0     +1     +2     +3\n" );
-                for ( i = 0; i < 0x20; i+= 4 ) {
-			printk( "TLAN:      0x%02x", i );
-			TLan_MiiReadReg( dev, phy, i, &data0 );
-			printk( " 0x%04hx", data0 );
-			TLan_MiiReadReg( dev, phy, i + 1, &data1 );
-			printk( " 0x%04hx", data1 );
-			TLan_MiiReadReg( dev, phy, i + 2, &data2 );
-			printk( " 0x%04hx", data2 );
-			TLan_MiiReadReg( dev, phy, i + 3, &data3 );
-			printk( " 0x%04hx\n", data3 );
+	phy = priv->phy[priv->phy_num];
+
+	if (priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY) {
+		netdev_info(dev, "Unmanaged PHY\n");
+	} else if (phy <= TLAN_PHY_MAX_ADDR) {
+		netdev_info(dev, "PHY 0x%02x\n", phy);
+		pr_info("   Off.  +0     +1     +2     +3\n");
+		for (i = 0; i < 0x20; i += 4) {
+			tlan_mii_read_reg(dev, phy, i, &data0);
+			tlan_mii_read_reg(dev, phy, i + 1, &data1);
+			tlan_mii_read_reg(dev, phy, i + 2, &data2);
+			tlan_mii_read_reg(dev, phy, i + 3, &data3);
+			pr_info("   0x%02x 0x%04hx 0x%04hx 0x%04hx 0x%04hx\n",
+				i, data0, data1, data2, data3);
 		}
 	} else {
-		printk( "TLAN:   Device %s, Invalid PHY.\n", dev->name );
+		netdev_info(dev, "Invalid PHY\n");
 	}
 
-} /* TLan_PhyPrint */
+}
 
 
 
 
-	/*********************************************************************
-	 *	TLan_PhyDetect
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev	A pointer to the device structure of the adapter
-	 *			for which the PHY needs determined.
-	 *
-	 *	So far I've found that adapters which have external PHYs
-	 *	may also use the internal PHY for part of the functionality.
-	 *	(eg, AUI/Thinnet).  This function finds out if this TLAN
-	 *	chip has an internal PHY, and then finds the first external
-	 *	PHY (starting from address 0) if it exists).
-	 *
-	 ********************************************************************/
+/*********************************************************************
+ *	tlan_phy_detect
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev	A pointer to the device structure of the adapter
+ *			for which the PHY needs determined.
+ *
+ *	So far I've found that adapters which have external PHYs
+ *	may also use the internal PHY for part of the functionality.
+ *	(eg, AUI/Thinnet).  This function finds out if this TLAN
+ *	chip has an internal PHY, and then finds the first external
+ *	PHY (starting from address 0) if it exists).
+ *
+ ********************************************************************/
 
-static void TLan_PhyDetect( struct net_device *dev )
+static void tlan_phy_detect(struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	u16		control;
 	u16		hi;
 	u16		lo;
 	u32		phy;
 
-	if ( priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY ) {
-		priv->phyNum = 0xFFFF;
+	if (priv->adapter->flags & TLAN_ADAPTER_UNMANAGED_PHY) {
+		priv->phy_num = 0xffff;
 		return;
 	}
 
-	TLan_MiiReadReg( dev, TLAN_PHY_MAX_ADDR, MII_GEN_ID_HI, &hi );
+	tlan_mii_read_reg(dev, TLAN_PHY_MAX_ADDR, MII_GEN_ID_HI, &hi);
 
-	if ( hi != 0xFFFF ) {
+	if (hi != 0xffff)
 		priv->phy[0] = TLAN_PHY_MAX_ADDR;
-	} else {
+	else
 		priv->phy[0] = TLAN_PHY_NONE;
-	}
 
 	priv->phy[1] = TLAN_PHY_NONE;
-	for ( phy = 0; phy <= TLAN_PHY_MAX_ADDR; phy++ ) {
-		TLan_MiiReadReg( dev, phy, MII_GEN_CTL, &control );
-		TLan_MiiReadReg( dev, phy, MII_GEN_ID_HI, &hi );
-		TLan_MiiReadReg( dev, phy, MII_GEN_ID_LO, &lo );
-		if ( ( control != 0xFFFF ) ||
-		     ( hi != 0xFFFF ) || ( lo != 0xFFFF ) ) {
-			TLAN_DBG( TLAN_DEBUG_GNRL,
-				  "PHY found at %02x %04x %04x %04x\n",
-				  phy, control, hi, lo );
-			if ( ( priv->phy[1] == TLAN_PHY_NONE ) &&
-			     ( phy != TLAN_PHY_MAX_ADDR ) ) {
+	for (phy = 0; phy <= TLAN_PHY_MAX_ADDR; phy++) {
+		tlan_mii_read_reg(dev, phy, MII_GEN_CTL, &control);
+		tlan_mii_read_reg(dev, phy, MII_GEN_ID_HI, &hi);
+		tlan_mii_read_reg(dev, phy, MII_GEN_ID_LO, &lo);
+		if ((control != 0xffff) ||
+		    (hi != 0xffff) || (lo != 0xffff)) {
+			TLAN_DBG(TLAN_DEBUG_GNRL,
+				 "PHY found at %02x %04x %04x %04x\n",
+				 phy, control, hi, lo);
+			if ((priv->phy[1] == TLAN_PHY_NONE) &&
+			    (phy != TLAN_PHY_MAX_ADDR)) {
 				priv->phy[1] = phy;
 			}
 		}
 	}
 
-	if ( priv->phy[1] != TLAN_PHY_NONE ) {
-		priv->phyNum = 1;
-	} else if ( priv->phy[0] != TLAN_PHY_NONE ) {
-		priv->phyNum = 0;
-	} else {
-		printk( "TLAN:  Cannot initialize device, no PHY was found!\n" );
-	}
+	if (priv->phy[1] != TLAN_PHY_NONE)
+		priv->phy_num = 1;
+	else if (priv->phy[0] != TLAN_PHY_NONE)
+		priv->phy_num = 0;
+	else
+		netdev_info(dev, "Cannot initialize device, no PHY was found!\n");
 
-} /* TLan_PhyDetect */
+}
 
 
 
 
-static void TLan_PhyPowerDown( struct net_device *dev )
+static void tlan_phy_power_down(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u16		value;
 
-	TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Powering down PHY(s).\n", dev->name );
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Powering down PHY(s).\n", dev->name);
 	value = MII_GC_PDOWN | MII_GC_LOOPBK | MII_GC_ISOLATE;
-	TLan_MiiSync( dev->base_addr );
-	TLan_MiiWriteReg( dev, priv->phy[priv->phyNum], MII_GEN_CTL, value );
-	if ( ( priv->phyNum == 0 ) &&
-	     ( priv->phy[1] != TLAN_PHY_NONE ) &&
-	     ( ! ( priv->adapter->flags & TLAN_ADAPTER_USE_INTERN_10 ) ) ) {
-		TLan_MiiSync( dev->base_addr );
-		TLan_MiiWriteReg( dev, priv->phy[1], MII_GEN_CTL, value );
+	tlan_mii_sync(dev->base_addr);
+	tlan_mii_write_reg(dev, priv->phy[priv->phy_num], MII_GEN_CTL, value);
+	if ((priv->phy_num == 0) &&
+	    (priv->phy[1] != TLAN_PHY_NONE) &&
+	    (!(priv->adapter->flags & TLAN_ADAPTER_USE_INTERN_10))) {
+		tlan_mii_sync(dev->base_addr);
+		tlan_mii_write_reg(dev, priv->phy[1], MII_GEN_CTL, value);
 	}
 
 	/* Wait for 50 ms and powerup
 	 * This is abitrary.  It is intended to make sure the
 	 * transceiver settles.
 	 */
-	TLan_SetTimer( dev, (HZ/20), TLAN_TIMER_PHY_PUP );
+	tlan_set_timer(dev, (HZ/20), TLAN_TIMER_PHY_PUP);
 
-} /* TLan_PhyPowerDown */
+}
 
 
 
 
-static void TLan_PhyPowerUp( struct net_device *dev )
+static void tlan_phy_power_up(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u16		value;
 
-	TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Powering up PHY.\n", dev->name );
-	TLan_MiiSync( dev->base_addr );
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Powering up PHY.\n", dev->name);
+	tlan_mii_sync(dev->base_addr);
 	value = MII_GC_LOOPBK;
-	TLan_MiiWriteReg( dev, priv->phy[priv->phyNum], MII_GEN_CTL, value );
-	TLan_MiiSync(dev->base_addr);
+	tlan_mii_write_reg(dev, priv->phy[priv->phy_num], MII_GEN_CTL, value);
+	tlan_mii_sync(dev->base_addr);
 	/* Wait for 500 ms and reset the
 	 * transceiver.  The TLAN docs say both 50 ms and
 	 * 500 ms, so do the longer, just in case.
 	 */
-	TLan_SetTimer( dev, (HZ/20), TLAN_TIMER_PHY_RESET );
+	tlan_set_timer(dev, (HZ/20), TLAN_TIMER_PHY_RESET);
 
-} /* TLan_PhyPowerUp */
+}
 
 
 
 
-static void TLan_PhyReset( struct net_device *dev )
+static void tlan_phy_reset(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u16		phy;
 	u16		value;
 
-	phy = priv->phy[priv->phyNum];
+	phy = priv->phy[priv->phy_num];
 
-	TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Reseting PHY.\n", dev->name );
-	TLan_MiiSync( dev->base_addr );
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Reseting PHY.\n", dev->name);
+	tlan_mii_sync(dev->base_addr);
 	value = MII_GC_LOOPBK | MII_GC_RESET;
-	TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, value );
-	TLan_MiiReadReg( dev, phy, MII_GEN_CTL, &value );
-	while ( value & MII_GC_RESET ) {
-		TLan_MiiReadReg( dev, phy, MII_GEN_CTL, &value );
-	}
+	tlan_mii_write_reg(dev, phy, MII_GEN_CTL, value);
+	tlan_mii_read_reg(dev, phy, MII_GEN_CTL, &value);
+	while (value & MII_GC_RESET)
+		tlan_mii_read_reg(dev, phy, MII_GEN_CTL, &value);
 
 	/* Wait for 500 ms and initialize.
 	 * I don't remember why I wait this long.
 	 * I've changed this to 50ms, as it seems long enough.
 	 */
-	TLan_SetTimer( dev, (HZ/20), TLAN_TIMER_PHY_START_LINK );
+	tlan_set_timer(dev, (HZ/20), TLAN_TIMER_PHY_START_LINK);
 
-} /* TLan_PhyReset */
+}
 
 
 
 
-static void TLan_PhyStartLink( struct net_device *dev )
+static void tlan_phy_start_link(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u16		ability;
 	u16		control;
 	u16		data;
@@ -2638,86 +2573,87 @@ static void TLan_PhyStartLink( struct net_device *dev )
 	u16		status;
 	u16		tctl;
 
-	phy = priv->phy[priv->phyNum];
-	TLAN_DBG( TLAN_DEBUG_GNRL, "%s: Trying to activate link.\n", dev->name );
-	TLan_MiiReadReg( dev, phy, MII_GEN_STS, &status );
-	TLan_MiiReadReg( dev, phy, MII_GEN_STS, &ability );
+	phy = priv->phy[priv->phy_num];
+	TLAN_DBG(TLAN_DEBUG_GNRL, "%s: Trying to activate link.\n", dev->name);
+	tlan_mii_read_reg(dev, phy, MII_GEN_STS, &status);
+	tlan_mii_read_reg(dev, phy, MII_GEN_STS, &ability);
 
-	if ( ( status & MII_GS_AUTONEG ) &&
-	     ( ! priv->aui ) ) {
+	if ((status & MII_GS_AUTONEG) &&
+	    (!priv->aui)) {
 		ability = status >> 11;
-		if ( priv->speed  == TLAN_SPEED_10 &&
-		     priv->duplex == TLAN_DUPLEX_HALF) {
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, 0x0000);
-		} else if ( priv->speed == TLAN_SPEED_10 &&
-			    priv->duplex == TLAN_DUPLEX_FULL) {
-			priv->tlanFullDuplex = true;
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, 0x0100);
-		} else if ( priv->speed == TLAN_SPEED_100 &&
-			    priv->duplex == TLAN_DUPLEX_HALF) {
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, 0x2000);
-		} else if ( priv->speed == TLAN_SPEED_100 &&
-			    priv->duplex == TLAN_DUPLEX_FULL) {
-			priv->tlanFullDuplex = true;
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, 0x2100);
+		if (priv->speed  == TLAN_SPEED_10 &&
+		    priv->duplex == TLAN_DUPLEX_HALF) {
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL, 0x0000);
+		} else if (priv->speed == TLAN_SPEED_10 &&
+			   priv->duplex == TLAN_DUPLEX_FULL) {
+			priv->tlan_full_duplex = true;
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL, 0x0100);
+		} else if (priv->speed == TLAN_SPEED_100 &&
+			   priv->duplex == TLAN_DUPLEX_HALF) {
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL, 0x2000);
+		} else if (priv->speed == TLAN_SPEED_100 &&
+			   priv->duplex == TLAN_DUPLEX_FULL) {
+			priv->tlan_full_duplex = true;
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL, 0x2100);
 		} else {
 
 			/* Set Auto-Neg advertisement */
-			TLan_MiiWriteReg( dev, phy, MII_AN_ADV, (ability << 5) | 1);
+			tlan_mii_write_reg(dev, phy, MII_AN_ADV,
+					   (ability << 5) | 1);
 			/* Enablee Auto-Neg */
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, 0x1000 );
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL, 0x1000);
 			/* Restart Auto-Neg */
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, 0x1200 );
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL, 0x1200);
 			/* Wait for 4 sec for autonegotiation
-		 	* to complete.  The max spec time is less than this
-		 	* but the card need additional time to start AN.
-		 	* .5 sec should be plenty extra.
-		 	*/
-			printk( "TLAN: %s: Starting autonegotiation.\n", dev->name );
-			TLan_SetTimer( dev, (2*HZ), TLAN_TIMER_PHY_FINISH_AN );
+			 * to complete.  The max spec time is less than this
+			 * but the card need additional time to start AN.
+			 * .5 sec should be plenty extra.
+			 */
+			netdev_info(dev, "Starting autonegotiation\n");
+			tlan_set_timer(dev, (2*HZ), TLAN_TIMER_PHY_FINISH_AN);
 			return;
 		}
 
 	}
 
-	if ( ( priv->aui ) && ( priv->phyNum != 0 ) ) {
-		priv->phyNum = 0;
-		data = TLAN_NET_CFG_1FRAG | TLAN_NET_CFG_1CHAN | TLAN_NET_CFG_PHY_EN;
-		TLan_DioWrite16( dev->base_addr, TLAN_NET_CONFIG, data );
-		TLan_SetTimer( dev, (40*HZ/1000), TLAN_TIMER_PHY_PDOWN );
+	if ((priv->aui) && (priv->phy_num != 0)) {
+		priv->phy_num = 0;
+		data = TLAN_NET_CFG_1FRAG | TLAN_NET_CFG_1CHAN
+			| TLAN_NET_CFG_PHY_EN;
+		tlan_dio_write16(dev->base_addr, TLAN_NET_CONFIG, data);
+		tlan_set_timer(dev, (40*HZ/1000), TLAN_TIMER_PHY_PDOWN);
 		return;
-	}  else if ( priv->phyNum == 0 ) {
+	} else if (priv->phy_num == 0) {
 		control = 0;
-        	TLan_MiiReadReg( dev, phy, TLAN_TLPHY_CTL, &tctl );
-		if ( priv->aui ) {
-                	tctl |= TLAN_TC_AUISEL;
+		tlan_mii_read_reg(dev, phy, TLAN_TLPHY_CTL, &tctl);
+		if (priv->aui) {
+			tctl |= TLAN_TC_AUISEL;
 		} else {
-                	tctl &= ~TLAN_TC_AUISEL;
-			if ( priv->duplex == TLAN_DUPLEX_FULL ) {
+			tctl &= ~TLAN_TC_AUISEL;
+			if (priv->duplex == TLAN_DUPLEX_FULL) {
 				control |= MII_GC_DUPLEX;
-				priv->tlanFullDuplex = true;
+				priv->tlan_full_duplex = true;
 			}
-			if ( priv->speed == TLAN_SPEED_100 ) {
+			if (priv->speed == TLAN_SPEED_100)
 				control |= MII_GC_SPEEDSEL;
-			}
 		}
-		TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, control );
-        	TLan_MiiWriteReg( dev, phy, TLAN_TLPHY_CTL, tctl );
+		tlan_mii_write_reg(dev, phy, MII_GEN_CTL, control);
+		tlan_mii_write_reg(dev, phy, TLAN_TLPHY_CTL, tctl);
 	}
 
 	/* Wait for 2 sec to give the transceiver time
 	 * to establish link.
 	 */
-	TLan_SetTimer( dev, (4*HZ), TLAN_TIMER_FINISH_RESET );
+	tlan_set_timer(dev, (4*HZ), TLAN_TIMER_FINISH_RESET);
 
-} /* TLan_PhyStartLink */
+}
 
 
 
 
-static void TLan_PhyFinishAutoNeg( struct net_device *dev )
+static void tlan_phy_finish_auto_neg(struct net_device *dev)
 {
-	TLanPrivateInfo	*priv = netdev_priv(dev);
+	struct tlan_priv	*priv = netdev_priv(dev);
 	u16		an_adv;
 	u16		an_lpa;
 	u16		data;
@@ -2725,115 +2661,118 @@ static void TLan_PhyFinishAutoNeg( struct net_device *dev )
 	u16		phy;
 	u16		status;
 
-	phy = priv->phy[priv->phyNum];
+	phy = priv->phy[priv->phy_num];
 
-	TLan_MiiReadReg( dev, phy, MII_GEN_STS, &status );
-	udelay( 1000 );
-	TLan_MiiReadReg( dev, phy, MII_GEN_STS, &status );
+	tlan_mii_read_reg(dev, phy, MII_GEN_STS, &status);
+	udelay(1000);
+	tlan_mii_read_reg(dev, phy, MII_GEN_STS, &status);
 
-	if ( ! ( status & MII_GS_AUTOCMPLT ) ) {
+	if (!(status & MII_GS_AUTOCMPLT)) {
 		/* Wait for 8 sec to give the process
 		 * more time.  Perhaps we should fail after a while.
 		 */
-		 if (!priv->neg_be_verbose++) {
-			 pr_info("TLAN:  Giving autonegotiation more time.\n");
-		 	 pr_info("TLAN:  Please check that your adapter has\n");
-		 	 pr_info("TLAN:  been properly connected to a HUB or Switch.\n");
-			 pr_info("TLAN:  Trying to establish link in the background...\n");
-		 }
-		TLan_SetTimer( dev, (8*HZ), TLAN_TIMER_PHY_FINISH_AN );
+		if (!priv->neg_be_verbose++) {
+			pr_info("Giving autonegotiation more time.\n");
+			pr_info("Please check that your adapter has\n");
+			pr_info("been properly connected to a HUB or Switch.\n");
+			pr_info("Trying to establish link in the background...\n");
+		}
+		tlan_set_timer(dev, (8*HZ), TLAN_TIMER_PHY_FINISH_AN);
 		return;
 	}
 
-	printk( "TLAN: %s: Autonegotiation complete.\n", dev->name );
-	TLan_MiiReadReg( dev, phy, MII_AN_ADV, &an_adv );
-	TLan_MiiReadReg( dev, phy, MII_AN_LPA, &an_lpa );
+	netdev_info(dev, "Autonegotiation complete\n");
+	tlan_mii_read_reg(dev, phy, MII_AN_ADV, &an_adv);
+	tlan_mii_read_reg(dev, phy, MII_AN_LPA, &an_lpa);
 	mode = an_adv & an_lpa & 0x03E0;
-	if ( mode & 0x0100 ) {
-		priv->tlanFullDuplex = true;
-	} else if ( ! ( mode & 0x0080 ) && ( mode & 0x0040 ) ) {
-		priv->tlanFullDuplex = true;
-	}
-
-	if ( ( ! ( mode & 0x0180 ) ) &&
-	     ( priv->adapter->flags & TLAN_ADAPTER_USE_INTERN_10 ) &&
-	     ( priv->phyNum != 0 ) ) {
-		priv->phyNum = 0;
-		data = TLAN_NET_CFG_1FRAG | TLAN_NET_CFG_1CHAN | TLAN_NET_CFG_PHY_EN;
-		TLan_DioWrite16( dev->base_addr, TLAN_NET_CONFIG, data );
-		TLan_SetTimer( dev, (400*HZ/1000), TLAN_TIMER_PHY_PDOWN );
+	if (mode & 0x0100)
+		priv->tlan_full_duplex = true;
+	else if (!(mode & 0x0080) && (mode & 0x0040))
+		priv->tlan_full_duplex = true;
+
+	if ((!(mode & 0x0180)) &&
+	    (priv->adapter->flags & TLAN_ADAPTER_USE_INTERN_10) &&
+	    (priv->phy_num != 0)) {
+		priv->phy_num = 0;
+		data = TLAN_NET_CFG_1FRAG | TLAN_NET_CFG_1CHAN
+			| TLAN_NET_CFG_PHY_EN;
+		tlan_dio_write16(dev->base_addr, TLAN_NET_CONFIG, data);
+		tlan_set_timer(dev, (400*HZ/1000), TLAN_TIMER_PHY_PDOWN);
 		return;
 	}
 
-	if ( priv->phyNum == 0 ) {
-		if ( ( priv->duplex == TLAN_DUPLEX_FULL ) ||
-		     ( an_adv & an_lpa & 0x0040 ) ) {
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL,
-					  MII_GC_AUTOENB | MII_GC_DUPLEX );
-			pr_info("TLAN:  Starting internal PHY with FULL-DUPLEX\n" );
+	if (priv->phy_num == 0) {
+		if ((priv->duplex == TLAN_DUPLEX_FULL) ||
+		    (an_adv & an_lpa & 0x0040)) {
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL,
+					   MII_GC_AUTOENB | MII_GC_DUPLEX);
+			netdev_info(dev, "Starting internal PHY with FULL-DUPLEX\n");
 		} else {
-			TLan_MiiWriteReg( dev, phy, MII_GEN_CTL, MII_GC_AUTOENB );
-			pr_info( "TLAN:  Starting internal PHY with HALF-DUPLEX\n" );
+			tlan_mii_write_reg(dev, phy, MII_GEN_CTL,
+					   MII_GC_AUTOENB);
+			netdev_info(dev, "Starting internal PHY with HALF-DUPLEX\n");
 		}
 	}
 
 	/* Wait for 100 ms.  No reason in partiticular.
 	 */
-	TLan_SetTimer( dev, (HZ/10), TLAN_TIMER_FINISH_RESET );
+	tlan_set_timer(dev, (HZ/10), TLAN_TIMER_FINISH_RESET);
 
-} /* TLan_PhyFinishAutoNeg */
+}
 
 #ifdef MONITOR
 
-        /*********************************************************************
-        *
-        *      TLan_phyMonitor
-        *
-        *      Returns:
-        *              None
-        *
-        *      Params:
-        *              dev             The device structure of this device.
-        *
-        *
-        *      This function monitors PHY condition by reading the status
-        *      register via the MII bus. This can be used to give info
-        *      about link changes (up/down), and possible switch to alternate
-        *      media.
-        *
-        * ******************************************************************/
-
-void TLan_PhyMonitor( struct net_device *dev )
+/*********************************************************************
+ *
+ *     tlan_phy_monitor
+ *
+ *     Returns:
+ *	      None
+ *
+ *     Params:
+ *	      dev	     The device structure of this device.
+ *
+ *
+ *     This function monitors PHY condition by reading the status
+ *     register via the MII bus. This can be used to give info
+ *     about link changes (up/down), and possible switch to alternate
+ *     media.
+ *
+ *******************************************************************/
+
+void tlan_phy_monitor(struct net_device *dev)
 {
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	u16     phy;
 	u16     phy_status;
 
-	phy = priv->phy[priv->phyNum];
+	phy = priv->phy[priv->phy_num];
 
-        /* Get PHY status register */
-        TLan_MiiReadReg( dev, phy, MII_GEN_STS, &phy_status );
+	/* Get PHY status register */
+	tlan_mii_read_reg(dev, phy, MII_GEN_STS, &phy_status);
 
-        /* Check if link has been lost */
-        if (!(phy_status & MII_GS_LINK)) {
- 	       if (priv->link) {
-		      priv->link = 0;
-	              printk(KERN_DEBUG "TLAN: %s has lost link\n", dev->name);
-		      netif_carrier_off(dev);
-		      TLan_SetTimer( dev, (2*HZ), TLAN_TIMER_LINK_BEAT );
-		      return;
+	/* Check if link has been lost */
+	if (!(phy_status & MII_GS_LINK)) {
+		if (priv->link) {
+			priv->link = 0;
+			printk(KERN_DEBUG "TLAN: %s has lost link\n",
+			       dev->name);
+			netif_carrier_off(dev);
+			tlan_set_timer(dev, (2*HZ), TLAN_TIMER_LINK_BEAT);
+			return;
 		}
 	}
 
-        /* Link restablished? */
-        if ((phy_status & MII_GS_LINK) && !priv->link) {
- 		priv->link = 1;
-        	printk(KERN_DEBUG "TLAN: %s has reestablished link\n", dev->name);
+	/* Link restablished? */
+	if ((phy_status & MII_GS_LINK) && !priv->link) {
+		priv->link = 1;
+		printk(KERN_DEBUG "TLAN: %s has reestablished link\n",
+		       dev->name);
 		netif_carrier_on(dev);
-        }
+	}
 
 	/* Setup a new monitor */
-	TLan_SetTimer( dev, (2*HZ), TLAN_TIMER_LINK_BEAT );
+	tlan_set_timer(dev, (2*HZ), TLAN_TIMER_LINK_BEAT);
 }
 
 #endif /* MONITOR */
@@ -2842,47 +2781,48 @@ void TLan_PhyMonitor( struct net_device *dev )
 /*****************************************************************************
 ******************************************************************************
 
-	ThunderLAN Driver MII Routines
+ThunderLAN driver MII routines
 
-	These routines are based on the information in Chap. 2 of the
-	"ThunderLAN Programmer's Guide", pp. 15-24.
+these routines are based on the information in chap. 2 of the
+"ThunderLAN Programmer's Guide", pp. 15-24.
 
 ******************************************************************************
 *****************************************************************************/
 
 
-	/***************************************************************
-	 *	TLan_MiiReadReg
-	 *
-	 *	Returns:
-	 *		false	if ack received ok
-	 *		true	if no ack received or other error
-	 *
-	 *	Parms:
-	 *		dev		The device structure containing
-	 *				The io address and interrupt count
-	 *				for this device.
-	 *		phy		The address of the PHY to be queried.
-	 *		reg		The register whose contents are to be
-	 *				retrieved.
-	 *		val		A pointer to a variable to store the
-	 *				retrieved value.
-	 *
-	 *	This function uses the TLAN's MII bus to retrieve the contents
-	 *	of a given register on a PHY.  It sends the appropriate info
-	 *	and then reads the 16-bit register value from the MII bus via
-	 *	the TLAN SIO register.
-	 *
-	 **************************************************************/
-
-static bool TLan_MiiReadReg( struct net_device *dev, u16 phy, u16 reg, u16 *val )
+/***************************************************************
+ *	tlan_mii_read_reg
+ *
+ *	Returns:
+ *		false	if ack received ok
+ *		true	if no ack received or other error
+ *
+ *	Parms:
+ *		dev		The device structure containing
+ *				The io address and interrupt count
+ *				for this device.
+ *		phy		The address of the PHY to be queried.
+ *		reg		The register whose contents are to be
+ *				retrieved.
+ *		val		A pointer to a variable to store the
+ *				retrieved value.
+ *
+ *	This function uses the TLAN's MII bus to retrieve the contents
+ *	of a given register on a PHY.  It sends the appropriate info
+ *	and then reads the 16-bit register value from the MII bus via
+ *	the TLAN SIO register.
+ *
+ **************************************************************/
+
+static bool
+tlan_mii_read_reg(struct net_device *dev, u16 phy, u16 reg, u16 *val)
 {
 	u8	nack;
 	u16	sio, tmp;
- 	u32	i;
+	u32	i;
 	bool	err;
 	int	minten;
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	unsigned long flags = 0;
 
 	err = false;
@@ -2892,48 +2832,48 @@ static bool TLan_MiiReadReg( struct net_device *dev, u16 phy, u16 reg, u16 *val
 	if (!in_irq())
 		spin_lock_irqsave(&priv->lock, flags);
 
-	TLan_MiiSync(dev->base_addr);
+	tlan_mii_sync(dev->base_addr);
 
-	minten = TLan_GetBit( TLAN_NET_SIO_MINTEN, sio );
-	if ( minten )
-		TLan_ClearBit(TLAN_NET_SIO_MINTEN, sio);
+	minten = tlan_get_bit(TLAN_NET_SIO_MINTEN, sio);
+	if (minten)
+		tlan_clear_bit(TLAN_NET_SIO_MINTEN, sio);
 
-	TLan_MiiSendData( dev->base_addr, 0x1, 2 );	/* Start ( 01b ) */
-	TLan_MiiSendData( dev->base_addr, 0x2, 2 );	/* Read  ( 10b ) */
-	TLan_MiiSendData( dev->base_addr, phy, 5 );	/* Device #      */
-	TLan_MiiSendData( dev->base_addr, reg, 5 );	/* Register #    */
+	tlan_mii_send_data(dev->base_addr, 0x1, 2);	/* start (01b) */
+	tlan_mii_send_data(dev->base_addr, 0x2, 2);	/* read  (10b) */
+	tlan_mii_send_data(dev->base_addr, phy, 5);	/* device #      */
+	tlan_mii_send_data(dev->base_addr, reg, 5);	/* register #    */
 
 
-	TLan_ClearBit(TLAN_NET_SIO_MTXEN, sio);		/* Change direction */
+	tlan_clear_bit(TLAN_NET_SIO_MTXEN, sio);	/* change direction */
 
-	TLan_ClearBit(TLAN_NET_SIO_MCLK, sio);		/* Clock Idle bit */
-	TLan_SetBit(TLAN_NET_SIO_MCLK, sio);
-	TLan_ClearBit(TLAN_NET_SIO_MCLK, sio);		/* Wait 300ns */
+	tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);		/* clock idle bit */
+	tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
+	tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);		/* wait 300ns */
 
-	nack = TLan_GetBit(TLAN_NET_SIO_MDATA, sio);	/* Check for ACK */
-	TLan_SetBit(TLAN_NET_SIO_MCLK, sio);		/* Finish ACK */
-	if (nack) {					/* No ACK, so fake it */
+	nack = tlan_get_bit(TLAN_NET_SIO_MDATA, sio);	/* check for ACK */
+	tlan_set_bit(TLAN_NET_SIO_MCLK, sio);		/* finish ACK */
+	if (nack) {					/* no ACK, so fake it */
 		for (i = 0; i < 16; i++) {
-			TLan_ClearBit(TLAN_NET_SIO_MCLK, sio);
-			TLan_SetBit(TLAN_NET_SIO_MCLK, sio);
+			tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);
+			tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
 		}
 		tmp = 0xffff;
 		err = true;
 	} else {					/* ACK, so read data */
 		for (tmp = 0, i = 0x8000; i; i >>= 1) {
-			TLan_ClearBit(TLAN_NET_SIO_MCLK, sio);
-			if (TLan_GetBit(TLAN_NET_SIO_MDATA, sio))
+			tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);
+			if (tlan_get_bit(TLAN_NET_SIO_MDATA, sio))
 				tmp |= i;
-			TLan_SetBit(TLAN_NET_SIO_MCLK, sio);
+			tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
 		}
 	}
 
 
-	TLan_ClearBit(TLAN_NET_SIO_MCLK, sio);		/* Idle cycle */
-	TLan_SetBit(TLAN_NET_SIO_MCLK, sio);
+	tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);		/* idle cycle */
+	tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
 
-	if ( minten )
-		TLan_SetBit(TLAN_NET_SIO_MINTEN, sio);
+	if (minten)
+		tlan_set_bit(TLAN_NET_SIO_MINTEN, sio);
 
 	*val = tmp;
 
@@ -2942,116 +2882,117 @@ static bool TLan_MiiReadReg( struct net_device *dev, u16 phy, u16 reg, u16 *val
 
 	return err;
 
-} /* TLan_MiiReadReg */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_MiiSendData
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		base_port	The base IO port of the adapter	in
-	 *				question.
-	 *		dev		The address of the PHY to be queried.
-	 *		data		The value to be placed on the MII bus.
-	 *		num_bits	The number of bits in data that are to
-	 *				be placed on the MII bus.
-	 *
-	 *	This function sends on sequence of bits on the MII
-	 *	configuration bus.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_mii_send_data
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		base_port	The base IO port of the adapter	in
+ *				question.
+ *		dev		The address of the PHY to be queried.
+ *		data		The value to be placed on the MII bus.
+ *		num_bits	The number of bits in data that are to
+ *				be placed on the MII bus.
+ *
+ *	This function sends on sequence of bits on the MII
+ *	configuration bus.
+ *
+ **************************************************************/
 
-static void TLan_MiiSendData( u16 base_port, u32 data, unsigned num_bits )
+static void tlan_mii_send_data(u16 base_port, u32 data, unsigned num_bits)
 {
 	u16 sio;
 	u32 i;
 
-	if ( num_bits == 0 )
+	if (num_bits == 0)
 		return;
 
-	outw( TLAN_NET_SIO, base_port + TLAN_DIO_ADR );
+	outw(TLAN_NET_SIO, base_port + TLAN_DIO_ADR);
 	sio = base_port + TLAN_DIO_DATA + TLAN_NET_SIO;
-	TLan_SetBit( TLAN_NET_SIO_MTXEN, sio );
+	tlan_set_bit(TLAN_NET_SIO_MTXEN, sio);
 
-	for ( i = ( 0x1 << ( num_bits - 1 ) ); i; i >>= 1 ) {
-		TLan_ClearBit( TLAN_NET_SIO_MCLK, sio );
-		(void) TLan_GetBit( TLAN_NET_SIO_MCLK, sio );
-		if ( data & i )
-			TLan_SetBit( TLAN_NET_SIO_MDATA, sio );
+	for (i = (0x1 << (num_bits - 1)); i; i >>= 1) {
+		tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);
+		(void) tlan_get_bit(TLAN_NET_SIO_MCLK, sio);
+		if (data & i)
+			tlan_set_bit(TLAN_NET_SIO_MDATA, sio);
 		else
-			TLan_ClearBit( TLAN_NET_SIO_MDATA, sio );
-		TLan_SetBit( TLAN_NET_SIO_MCLK, sio );
-		(void) TLan_GetBit( TLAN_NET_SIO_MCLK, sio );
+			tlan_clear_bit(TLAN_NET_SIO_MDATA, sio);
+		tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
+		(void) tlan_get_bit(TLAN_NET_SIO_MCLK, sio);
 	}
 
-} /* TLan_MiiSendData */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_MiiSync
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		base_port	The base IO port of the adapter in
-	 *				question.
-	 *
-	 *	This functions syncs all PHYs in terms of the MII configuration
-	 *	bus.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	TLan_MiiSync
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		base_port	The base IO port of the adapter in
+ *				question.
+ *
+ *	This functions syncs all PHYs in terms of the MII configuration
+ *	bus.
+ *
+ **************************************************************/
 
-static void TLan_MiiSync( u16 base_port )
+static void tlan_mii_sync(u16 base_port)
 {
 	int i;
 	u16 sio;
 
-	outw( TLAN_NET_SIO, base_port + TLAN_DIO_ADR );
+	outw(TLAN_NET_SIO, base_port + TLAN_DIO_ADR);
 	sio = base_port + TLAN_DIO_DATA + TLAN_NET_SIO;
 
-	TLan_ClearBit( TLAN_NET_SIO_MTXEN, sio );
-	for ( i = 0; i < 32; i++ ) {
-		TLan_ClearBit( TLAN_NET_SIO_MCLK, sio );
-		TLan_SetBit( TLAN_NET_SIO_MCLK, sio );
+	tlan_clear_bit(TLAN_NET_SIO_MTXEN, sio);
+	for (i = 0; i < 32; i++) {
+		tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);
+		tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
 	}
 
-} /* TLan_MiiSync */
+}
 
 
 
 
-	/***************************************************************
-	 *	TLan_MiiWriteReg
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		dev		The device structure for the device
-	 *				to write to.
-	 *		phy		The address of the PHY to be written to.
-	 *		reg		The register whose contents are to be
-	 *				written.
-	 *		val		The value to be written to the register.
-	 *
-	 *	This function uses the TLAN's MII bus to write the contents of a
-	 *	given register on a PHY.  It sends the appropriate info and then
-	 *	writes the 16-bit register value from the MII configuration bus
-	 *	via the TLAN SIO register.
-	 *
-	 **************************************************************/
+/***************************************************************
+ *	tlan_mii_write_reg
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		dev		The device structure for the device
+ *				to write to.
+ *		phy		The address of the PHY to be written to.
+ *		reg		The register whose contents are to be
+ *				written.
+ *		val		The value to be written to the register.
+ *
+ *	This function uses the TLAN's MII bus to write the contents of a
+ *	given register on a PHY.  It sends the appropriate info and then
+ *	writes the 16-bit register value from the MII configuration bus
+ *	via the TLAN SIO register.
+ *
+ **************************************************************/
 
-static void TLan_MiiWriteReg( struct net_device *dev, u16 phy, u16 reg, u16 val )
+static void
+tlan_mii_write_reg(struct net_device *dev, u16 phy, u16 reg, u16 val)
 {
 	u16	sio;
 	int	minten;
 	unsigned long flags = 0;
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 
 	outw(TLAN_NET_SIO, dev->base_addr + TLAN_DIO_ADR);
 	sio = dev->base_addr + TLAN_DIO_DATA + TLAN_NET_SIO;
@@ -3059,30 +3000,30 @@ static void TLan_MiiWriteReg( struct net_device *dev, u16 phy, u16 reg, u16 val
 	if (!in_irq())
 		spin_lock_irqsave(&priv->lock, flags);
 
-	TLan_MiiSync( dev->base_addr );
+	tlan_mii_sync(dev->base_addr);
 
-	minten = TLan_GetBit( TLAN_NET_SIO_MINTEN, sio );
-	if ( minten )
-		TLan_ClearBit( TLAN_NET_SIO_MINTEN, sio );
+	minten = tlan_get_bit(TLAN_NET_SIO_MINTEN, sio);
+	if (minten)
+		tlan_clear_bit(TLAN_NET_SIO_MINTEN, sio);
 
-	TLan_MiiSendData( dev->base_addr, 0x1, 2 );	/* Start ( 01b ) */
-	TLan_MiiSendData( dev->base_addr, 0x1, 2 );	/* Write ( 01b ) */
-	TLan_MiiSendData( dev->base_addr, phy, 5 );	/* Device #      */
-	TLan_MiiSendData( dev->base_addr, reg, 5 );	/* Register #    */
+	tlan_mii_send_data(dev->base_addr, 0x1, 2);	/* start (01b) */
+	tlan_mii_send_data(dev->base_addr, 0x1, 2);	/* write (01b) */
+	tlan_mii_send_data(dev->base_addr, phy, 5);	/* device #      */
+	tlan_mii_send_data(dev->base_addr, reg, 5);	/* register #    */
 
-	TLan_MiiSendData( dev->base_addr, 0x2, 2 );	/* Send ACK */
-	TLan_MiiSendData( dev->base_addr, val, 16 );	/* Send Data */
+	tlan_mii_send_data(dev->base_addr, 0x2, 2);	/* send ACK */
+	tlan_mii_send_data(dev->base_addr, val, 16);	/* send data */
 
-	TLan_ClearBit( TLAN_NET_SIO_MCLK, sio );	/* Idle cycle */
-	TLan_SetBit( TLAN_NET_SIO_MCLK, sio );
+	tlan_clear_bit(TLAN_NET_SIO_MCLK, sio);	/* idle cycle */
+	tlan_set_bit(TLAN_NET_SIO_MCLK, sio);
 
-	if ( minten )
-		TLan_SetBit( TLAN_NET_SIO_MINTEN, sio );
+	if (minten)
+		tlan_set_bit(TLAN_NET_SIO_MINTEN, sio);
 
 	if (!in_irq())
 		spin_unlock_irqrestore(&priv->lock, flags);
 
-} /* TLan_MiiWriteReg */
+}
 
 
 
@@ -3090,229 +3031,226 @@ static void TLan_MiiWriteReg( struct net_device *dev, u16 phy, u16 reg, u16 val
 /*****************************************************************************
 ******************************************************************************
 
-	ThunderLAN Driver Eeprom routines
+ThunderLAN driver eeprom routines
 
-	The Compaq Netelligent 10 and 10/100 cards use a Microchip 24C02A
-	EEPROM.  These functions are based on information in Microchip's
-	data sheet.  I don't know how well this functions will work with
-	other EEPROMs.
+the Compaq netelligent 10 and 10/100 cards use a microchip 24C02A
+EEPROM.  these functions are based on information in microchip's
+data sheet.  I don't know how well this functions will work with
+other Eeproms.
 
 ******************************************************************************
 *****************************************************************************/
 
 
-	/***************************************************************
-	 *	TLan_EeSendStart
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		io_base		The IO port base address for the
-	 *				TLAN device with the EEPROM to
-	 *				use.
-	 *
-	 *	This function sends a start cycle to an EEPROM attached
-	 *	to a TLAN chip.
-	 *
-	 **************************************************************/
-
-static void TLan_EeSendStart( u16 io_base )
+/***************************************************************
+ *	tlan_ee_send_start
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		io_base		The IO port base address for the
+ *				TLAN device with the EEPROM to
+ *				use.
+ *
+ *	This function sends a start cycle to an EEPROM attached
+ *	to a TLAN chip.
+ *
+ **************************************************************/
+
+static void tlan_ee_send_start(u16 io_base)
 {
 	u16	sio;
 
-	outw( TLAN_NET_SIO, io_base + TLAN_DIO_ADR );
+	outw(TLAN_NET_SIO, io_base + TLAN_DIO_ADR);
 	sio = io_base + TLAN_DIO_DATA + TLAN_NET_SIO;
 
-	TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-	TLan_SetBit( TLAN_NET_SIO_EDATA, sio );
-	TLan_SetBit( TLAN_NET_SIO_ETXEN, sio );
-	TLan_ClearBit( TLAN_NET_SIO_EDATA, sio );
-	TLan_ClearBit( TLAN_NET_SIO_ECLOK, sio );
-
-} /* TLan_EeSendStart */
-
-
-
-
-	/***************************************************************
-	 *	TLan_EeSendByte
-	 *
-	 *	Returns:
-	 *		If the correct ack was received, 0, otherwise 1
-	 *	Parms:	io_base		The IO port base address for the
-	 *				TLAN device with the EEPROM to
-	 *				use.
-	 *		data		The 8 bits of information to
-	 *				send to the EEPROM.
-	 *		stop		If TLAN_EEPROM_STOP is passed, a
-	 *				stop cycle is sent after the
-	 *				byte is sent after the ack is
-	 *				read.
-	 *
-	 *	This function sends a byte on the serial EEPROM line,
-	 *	driving the clock to send each bit. The function then
-	 *	reverses transmission direction and reads an acknowledge
-	 *	bit.
-	 *
-	 **************************************************************/
-
-static int TLan_EeSendByte( u16 io_base, u8 data, int stop )
+	tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+	tlan_set_bit(TLAN_NET_SIO_EDATA, sio);
+	tlan_set_bit(TLAN_NET_SIO_ETXEN, sio);
+	tlan_clear_bit(TLAN_NET_SIO_EDATA, sio);
+	tlan_clear_bit(TLAN_NET_SIO_ECLOK, sio);
+
+}
+
+
+
+
+/***************************************************************
+ *	tlan_ee_send_byte
+ *
+ *	Returns:
+ *		If the correct ack was received, 0, otherwise 1
+ *	Parms:	io_base		The IO port base address for the
+ *				TLAN device with the EEPROM to
+ *				use.
+ *		data		The 8 bits of information to
+ *				send to the EEPROM.
+ *		stop		If TLAN_EEPROM_STOP is passed, a
+ *				stop cycle is sent after the
+ *				byte is sent after the ack is
+ *				read.
+ *
+ *	This function sends a byte on the serial EEPROM line,
+ *	driving the clock to send each bit. The function then
+ *	reverses transmission direction and reads an acknowledge
+ *	bit.
+ *
+ **************************************************************/
+
+static int tlan_ee_send_byte(u16 io_base, u8 data, int stop)
 {
 	int	err;
 	u8	place;
 	u16	sio;
 
-	outw( TLAN_NET_SIO, io_base + TLAN_DIO_ADR );
+	outw(TLAN_NET_SIO, io_base + TLAN_DIO_ADR);
 	sio = io_base + TLAN_DIO_DATA + TLAN_NET_SIO;
 
 	/* Assume clock is low, tx is enabled; */
-	for ( place = 0x80; place != 0; place >>= 1 ) {
-		if ( place & data )
-			TLan_SetBit( TLAN_NET_SIO_EDATA, sio );
+	for (place = 0x80; place != 0; place >>= 1) {
+		if (place & data)
+			tlan_set_bit(TLAN_NET_SIO_EDATA, sio);
 		else
-			TLan_ClearBit( TLAN_NET_SIO_EDATA, sio );
-		TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-		TLan_ClearBit( TLAN_NET_SIO_ECLOK, sio );
+			tlan_clear_bit(TLAN_NET_SIO_EDATA, sio);
+		tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+		tlan_clear_bit(TLAN_NET_SIO_ECLOK, sio);
 	}
-	TLan_ClearBit( TLAN_NET_SIO_ETXEN, sio );
-	TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-	err = TLan_GetBit( TLAN_NET_SIO_EDATA, sio );
-	TLan_ClearBit( TLAN_NET_SIO_ECLOK, sio );
-	TLan_SetBit( TLAN_NET_SIO_ETXEN, sio );
+	tlan_clear_bit(TLAN_NET_SIO_ETXEN, sio);
+	tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+	err = tlan_get_bit(TLAN_NET_SIO_EDATA, sio);
+	tlan_clear_bit(TLAN_NET_SIO_ECLOK, sio);
+	tlan_set_bit(TLAN_NET_SIO_ETXEN, sio);
 
-	if ( ( ! err ) && stop ) {
+	if ((!err) && stop) {
 		/* STOP, raise data while clock is high */
-		TLan_ClearBit( TLAN_NET_SIO_EDATA, sio );
-		TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-		TLan_SetBit( TLAN_NET_SIO_EDATA, sio );
+		tlan_clear_bit(TLAN_NET_SIO_EDATA, sio);
+		tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+		tlan_set_bit(TLAN_NET_SIO_EDATA, sio);
 	}
 
 	return err;
 
-} /* TLan_EeSendByte */
-
-
-
-
-	/***************************************************************
-	 *	TLan_EeReceiveByte
-	 *
-	 *	Returns:
-	 *		Nothing
-	 *	Parms:
-	 *		io_base		The IO port base address for the
-	 *				TLAN device with the EEPROM to
-	 *				use.
-	 *		data		An address to a char to hold the
-	 *				data sent from the EEPROM.
-	 *		stop		If TLAN_EEPROM_STOP is passed, a
-	 *				stop cycle is sent after the
-	 *				byte is received, and no ack is
-	 *				sent.
-	 *
-	 *	This function receives 8 bits of data from the EEPROM
-	 *	over the serial link.  It then sends and ack bit, or no
-	 *	ack and a stop bit.  This function is used to retrieve
-	 *	data after the address of a byte in the EEPROM has been
-	 *	sent.
-	 *
-	 **************************************************************/
-
-static void TLan_EeReceiveByte( u16 io_base, u8 *data, int stop )
+}
+
+
+
+
+/***************************************************************
+ *	tlan_ee_receive_byte
+ *
+ *	Returns:
+ *		Nothing
+ *	Parms:
+ *		io_base		The IO port base address for the
+ *				TLAN device with the EEPROM to
+ *				use.
+ *		data		An address to a char to hold the
+ *				data sent from the EEPROM.
+ *		stop		If TLAN_EEPROM_STOP is passed, a
+ *				stop cycle is sent after the
+ *				byte is received, and no ack is
+ *				sent.
+ *
+ *	This function receives 8 bits of data from the EEPROM
+ *	over the serial link.  It then sends and ack bit, or no
+ *	ack and a stop bit.  This function is used to retrieve
+ *	data after the address of a byte in the EEPROM has been
+ *	sent.
+ *
+ **************************************************************/
+
+static void tlan_ee_receive_byte(u16 io_base, u8 *data, int stop)
 {
 	u8  place;
 	u16 sio;
 
-	outw( TLAN_NET_SIO, io_base + TLAN_DIO_ADR );
+	outw(TLAN_NET_SIO, io_base + TLAN_DIO_ADR);
 	sio = io_base + TLAN_DIO_DATA + TLAN_NET_SIO;
 	*data = 0;
 
 	/* Assume clock is low, tx is enabled; */
-	TLan_ClearBit( TLAN_NET_SIO_ETXEN, sio );
-	for ( place = 0x80; place; place >>= 1 ) {
-		TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-		if ( TLan_GetBit( TLAN_NET_SIO_EDATA, sio ) )
+	tlan_clear_bit(TLAN_NET_SIO_ETXEN, sio);
+	for (place = 0x80; place; place >>= 1) {
+		tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+		if (tlan_get_bit(TLAN_NET_SIO_EDATA, sio))
 			*data |= place;
-		TLan_ClearBit( TLAN_NET_SIO_ECLOK, sio );
+		tlan_clear_bit(TLAN_NET_SIO_ECLOK, sio);
 	}
 
-	TLan_SetBit( TLAN_NET_SIO_ETXEN, sio );
-	if ( ! stop ) {
-		TLan_ClearBit( TLAN_NET_SIO_EDATA, sio );	/* Ack = 0 */
-		TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-		TLan_ClearBit( TLAN_NET_SIO_ECLOK, sio );
+	tlan_set_bit(TLAN_NET_SIO_ETXEN, sio);
+	if (!stop) {
+		tlan_clear_bit(TLAN_NET_SIO_EDATA, sio); /* ack = 0 */
+		tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+		tlan_clear_bit(TLAN_NET_SIO_ECLOK, sio);
 	} else {
-		TLan_SetBit( TLAN_NET_SIO_EDATA, sio );		/* No ack = 1 (?) */
-		TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-		TLan_ClearBit( TLAN_NET_SIO_ECLOK, sio );
+		tlan_set_bit(TLAN_NET_SIO_EDATA, sio);	/* no ack = 1 (?) */
+		tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+		tlan_clear_bit(TLAN_NET_SIO_ECLOK, sio);
 		/* STOP, raise data while clock is high */
-		TLan_ClearBit( TLAN_NET_SIO_EDATA, sio );
-		TLan_SetBit( TLAN_NET_SIO_ECLOK, sio );
-		TLan_SetBit( TLAN_NET_SIO_EDATA, sio );
-	}
-
-} /* TLan_EeReceiveByte */
-
-
-
-
-	/***************************************************************
-	 *	TLan_EeReadByte
-	 *
-	 *	Returns:
-	 *		No error = 0, else, the stage at which the error
-	 *		occurred.
-	 *	Parms:
-	 *		io_base		The IO port base address for the
-	 *				TLAN device with the EEPROM to
-	 *				use.
-	 *		ee_addr		The address of the byte in the
-	 *				EEPROM whose contents are to be
-	 *				retrieved.
-	 *		data		An address to a char to hold the
-	 *				data obtained from the EEPROM.
-	 *
-	 *	This function reads a byte of information from an byte
-	 *	cell in the EEPROM.
-	 *
-	 **************************************************************/
-
-static int TLan_EeReadByte( struct net_device *dev, u8 ee_addr, u8 *data )
+		tlan_clear_bit(TLAN_NET_SIO_EDATA, sio);
+		tlan_set_bit(TLAN_NET_SIO_ECLOK, sio);
+		tlan_set_bit(TLAN_NET_SIO_EDATA, sio);
+	}
+
+}
+
+
+
+
+/***************************************************************
+ *	tlan_ee_read_byte
+ *
+ *	Returns:
+ *		No error = 0, else, the stage at which the error
+ *		occurred.
+ *	Parms:
+ *		io_base		The IO port base address for the
+ *				TLAN device with the EEPROM to
+ *				use.
+ *		ee_addr		The address of the byte in the
+ *				EEPROM whose contents are to be
+ *				retrieved.
+ *		data		An address to a char to hold the
+ *				data obtained from the EEPROM.
+ *
+ *	This function reads a byte of information from an byte
+ *	cell in the EEPROM.
+ *
+ **************************************************************/
+
+static int tlan_ee_read_byte(struct net_device *dev, u8 ee_addr, u8 *data)
 {
 	int err;
-	TLanPrivateInfo *priv = netdev_priv(dev);
+	struct tlan_priv *priv = netdev_priv(dev);
 	unsigned long flags = 0;
-	int ret=0;
+	int ret = 0;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	TLan_EeSendStart( dev->base_addr );
-	err = TLan_EeSendByte( dev->base_addr, 0xA0, TLAN_EEPROM_ACK );
-	if (err)
-	{
-		ret=1;
+	tlan_ee_send_start(dev->base_addr);
+	err = tlan_ee_send_byte(dev->base_addr, 0xa0, TLAN_EEPROM_ACK);
+	if (err) {
+		ret = 1;
 		goto fail;
 	}
-	err = TLan_EeSendByte( dev->base_addr, ee_addr, TLAN_EEPROM_ACK );
-	if (err)
-	{
-		ret=2;
+	err = tlan_ee_send_byte(dev->base_addr, ee_addr, TLAN_EEPROM_ACK);
+	if (err) {
+		ret = 2;
 		goto fail;
 	}
-	TLan_EeSendStart( dev->base_addr );
-	err = TLan_EeSendByte( dev->base_addr, 0xA1, TLAN_EEPROM_ACK );
-	if (err)
-	{
-		ret=3;
+	tlan_ee_send_start(dev->base_addr);
+	err = tlan_ee_send_byte(dev->base_addr, 0xa1, TLAN_EEPROM_ACK);
+	if (err) {
+		ret = 3;
 		goto fail;
 	}
-	TLan_EeReceiveByte( dev->base_addr, data, TLAN_EEPROM_STOP );
+	tlan_ee_receive_byte(dev->base_addr, data, TLAN_EEPROM_STOP);
 fail:
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return ret;
 
-} /* TLan_EeReadByte */
+}
 
 
 
diff --git a/drivers/net/tlan.h b/drivers/net/tlan.h
index 3315ced774e2..5fc98a8e4889 100644
--- a/drivers/net/tlan.h
+++ b/drivers/net/tlan.h
@@ -20,8 +20,8 @@
  ********************************************************************/
 
 
-#include <asm/io.h>
-#include <asm/types.h>
+#include <linux/io.h>
+#include <linux/types.h>
 #include <linux/netdevice.h>
 
 
@@ -40,8 +40,11 @@
 #define TLAN_IGNORE		0
 #define TLAN_RECORD		1
 
-#define TLAN_DBG(lvl, format, args...)	\
-	do { if (debug&lvl) printk(KERN_DEBUG "TLAN: " format, ##args ); } while(0)
+#define TLAN_DBG(lvl, format, args...)					\
+	do {								\
+		if (debug&lvl)						\
+			printk(KERN_DEBUG "TLAN: " format, ##args);	\
+	} while (0)
 
 #define TLAN_DEBUG_GNRL		0x0001
 #define TLAN_DEBUG_TX		0x0002
@@ -50,7 +53,8 @@
 #define TLAN_DEBUG_PROBE	0x0010
 
 #define TX_TIMEOUT		(10*HZ)	 /* We need time for auto-neg */
-#define MAX_TLAN_BOARDS		8	 /* Max number of boards installed at a time */
+#define MAX_TLAN_BOARDS		8	 /* Max number of boards installed
+					    at a time */
 
 
 	/*****************************************************************
@@ -70,13 +74,13 @@
 #define PCI_DEVICE_ID_OLICOM_OC2326			0x0014
 #endif
 
-typedef struct tlan_adapter_entry {
-	u16	vendorId;
-	u16	deviceId;
-	char	*deviceLabel;
+struct tlan_adapter_entry {
+	u16	vendor_id;
+	u16	device_id;
+	char	*device_label;
 	u32	flags;
-	u16	addrOfs;
-} TLanAdapterEntry;
+	u16	addr_ofs;
+};
 
 #define TLAN_ADAPTER_NONE		0x00000000
 #define TLAN_ADAPTER_UNMANAGED_PHY	0x00000001
@@ -129,18 +133,18 @@ typedef struct tlan_adapter_entry {
 #define TLAN_CSTAT_DP_PR	0x0100
 
 
-typedef struct tlan_buffer_ref_tag {
+struct tlan_buffer {
 	u32	count;
 	u32	address;
-} TLanBufferRef;
+};
 
 
-typedef struct tlan_list_tag {
+struct tlan_list {
 	u32		forward;
-	u16		cStat;
-	u16		frameSize;
-	TLanBufferRef	buffer[TLAN_BUFFERS_PER_LIST];
-} TLanList;
+	u16		c_stat;
+	u16		frame_size;
+	struct tlan_buffer buffer[TLAN_BUFFERS_PER_LIST];
+};
 
 
 typedef u8 TLanBuffer[TLAN_MAX_FRAME_SIZE];
@@ -164,49 +168,49 @@ typedef u8 TLanBuffer[TLAN_MAX_FRAME_SIZE];
 	 *
 	 ****************************************************************/
 
-typedef struct tlan_private_tag {
-	struct net_device       *nextDevice;
-	struct pci_dev		*pciDev;
+struct tlan_priv {
+	struct net_device       *next_device;
+	struct pci_dev		*pci_dev;
 	struct net_device       *dev;
-	void			*dmaStorage;
-	dma_addr_t		dmaStorageDMA;
-	unsigned int		dmaSize;
-	u8			*padBuffer;
-	TLanList                *rxList;
-	dma_addr_t		rxListDMA;
-	u8			*rxBuffer;
-	dma_addr_t		rxBufferDMA;
-	u32                     rxHead;
-	u32                     rxTail;
-	u32			rxEocCount;
-	TLanList                *txList;
-	dma_addr_t		txListDMA;
-	u8			*txBuffer;
-	dma_addr_t		txBufferDMA;
-	u32                     txHead;
-	u32                     txInProgress;
-	u32                     txTail;
-	u32			txBusyCount;
-	u32                     phyOnline;
-	u32			timerSetAt;
-	u32			timerType;
+	void			*dma_storage;
+	dma_addr_t		dma_storage_dma;
+	unsigned int		dma_size;
+	u8			*pad_buffer;
+	struct tlan_list	*rx_list;
+	dma_addr_t		rx_list_dma;
+	u8			*rx_buffer;
+	dma_addr_t		rx_buffer_dma;
+	u32			rx_head;
+	u32			rx_tail;
+	u32			rx_eoc_count;
+	struct tlan_list	*tx_list;
+	dma_addr_t		tx_list_dma;
+	u8			*tx_buffer;
+	dma_addr_t		tx_buffer_dma;
+	u32			tx_head;
+	u32			tx_in_progress;
+	u32			tx_tail;
+	u32			tx_busy_count;
+	u32			phy_online;
+	u32			timer_set_at;
+	u32			timer_type;
 	struct timer_list	timer;
 	struct board		*adapter;
-	u32			adapterRev;
+	u32			adapter_rev;
 	u32			aui;
 	u32			debug;
 	u32			duplex;
 	u32			phy[2];
-	u32			phyNum;
+	u32			phy_num;
 	u32			speed;
-	u8			tlanRev;
-	u8			tlanFullDuplex;
+	u8			tlan_rev;
+	u8			tlan_full_duplex;
 	spinlock_t		lock;
 	u8			link;
 	u8			is_eisa;
 	struct work_struct			tlan_tqueue;
 	u8			neg_be_verbose;
-} TLanPrivateInfo;
+};
 
 
 
@@ -247,7 +251,7 @@ typedef struct tlan_private_tag {
 	 ****************************************************************/
 
 #define TLAN_HOST_CMD			0x00
-#define 	TLAN_HC_GO		0x80000000
+#define	TLAN_HC_GO		0x80000000
 #define		TLAN_HC_STOP		0x40000000
 #define		TLAN_HC_ACK		0x20000000
 #define		TLAN_HC_CS_MASK		0x1FE00000
@@ -283,7 +287,7 @@ typedef struct tlan_private_tag {
 #define		TLAN_NET_CMD_TRFRAM	0x02
 #define		TLAN_NET_CMD_TXPACE	0x01
 #define TLAN_NET_SIO			0x01
-#define 	TLAN_NET_SIO_MINTEN	0x80
+#define	TLAN_NET_SIO_MINTEN	0x80
 #define		TLAN_NET_SIO_ECLOK	0x40
 #define		TLAN_NET_SIO_ETXEN	0x20
 #define		TLAN_NET_SIO_EDATA	0x10
@@ -304,7 +308,7 @@ typedef struct tlan_private_tag {
 #define		TLAN_NET_MASK_MASK4	0x10
 #define		TLAN_NET_MASK_RSRVD	0x0F
 #define TLAN_NET_CONFIG			0x04
-#define 	TLAN_NET_CFG_RCLK	0x8000
+#define	TLAN_NET_CFG_RCLK	0x8000
 #define		TLAN_NET_CFG_TCLK	0x4000
 #define		TLAN_NET_CFG_BIT	0x2000
 #define		TLAN_NET_CFG_RXCRC	0x1000
@@ -372,7 +376,7 @@ typedef struct tlan_private_tag {
 /* Generic MII/PHY Registers */
 
 #define MII_GEN_CTL			0x00
-#define 	MII_GC_RESET		0x8000
+#define	MII_GC_RESET		0x8000
 #define		MII_GC_LOOPBK		0x4000
 #define		MII_GC_SPEEDSEL		0x2000
 #define		MII_GC_AUTOENB		0x1000
@@ -397,9 +401,9 @@ typedef struct tlan_private_tag {
 #define		MII_GS_EXTCAP		0x0001
 #define MII_GEN_ID_HI			0x02
 #define MII_GEN_ID_LO			0x03
-#define 	MII_GIL_OUI		0xFC00
-#define 	MII_GIL_MODEL		0x03F0
-#define 	MII_GIL_REVISION	0x000F
+#define	MII_GIL_OUI		0xFC00
+#define	MII_GIL_MODEL		0x03F0
+#define	MII_GIL_REVISION	0x000F
 #define MII_AN_ADV			0x04
 #define MII_AN_LPA			0x05
 #define MII_AN_EXP			0x06
@@ -408,7 +412,7 @@ typedef struct tlan_private_tag {
 
 #define TLAN_TLPHY_ID			0x10
 #define TLAN_TLPHY_CTL			0x11
-#define 	TLAN_TC_IGLINK		0x8000
+#define	TLAN_TC_IGLINK		0x8000
 #define		TLAN_TC_SWAPOL		0x4000
 #define		TLAN_TC_AUISEL		0x2000
 #define		TLAN_TC_SQEEN		0x1000
@@ -435,41 +439,41 @@ typedef struct tlan_private_tag {
 #define LEVEL1_ID1			0x7810
 #define LEVEL1_ID2			0x0000
 
-#define CIRC_INC( a, b ) if ( ++a >= b ) a = 0
+#define CIRC_INC(a, b) if (++a >= b) a = 0
 
 /* Routines to access internal registers. */
 
-static inline u8 TLan_DioRead8(u16 base_addr, u16 internal_addr)
+static inline u8 tlan_dio_read8(u16 base_addr, u16 internal_addr)
 {
 	outw(internal_addr, base_addr + TLAN_DIO_ADR);
 	return inb((base_addr + TLAN_DIO_DATA) + (internal_addr & 0x3));
 
-} /* TLan_DioRead8 */
+}
 
 
 
 
-static inline u16 TLan_DioRead16(u16 base_addr, u16 internal_addr)
+static inline u16 tlan_dio_read16(u16 base_addr, u16 internal_addr)
 {
 	outw(internal_addr, base_addr + TLAN_DIO_ADR);
 	return inw((base_addr + TLAN_DIO_DATA) + (internal_addr & 0x2));
 
-} /* TLan_DioRead16 */
+}
 
 
 
 
-static inline u32 TLan_DioRead32(u16 base_addr, u16 internal_addr)
+static inline u32 tlan_dio_read32(u16 base_addr, u16 internal_addr)
 {
 	outw(internal_addr, base_addr + TLAN_DIO_ADR);
 	return inl(base_addr + TLAN_DIO_DATA);
 
-} /* TLan_DioRead32 */
+}
 
 
 
 
-static inline void TLan_DioWrite8(u16 base_addr, u16 internal_addr, u8 data)
+static inline void tlan_dio_write8(u16 base_addr, u16 internal_addr, u8 data)
 {
 	outw(internal_addr, base_addr + TLAN_DIO_ADR);
 	outb(data, base_addr + TLAN_DIO_DATA + (internal_addr & 0x3));
@@ -479,7 +483,7 @@ static inline void TLan_DioWrite8(u16 base_addr, u16 internal_addr, u8 data)
 
 
 
-static inline void TLan_DioWrite16(u16 base_addr, u16 internal_addr, u16 data)
+static inline void tlan_dio_write16(u16 base_addr, u16 internal_addr, u16 data)
 {
 	outw(internal_addr, base_addr + TLAN_DIO_ADR);
 	outw(data, base_addr + TLAN_DIO_DATA + (internal_addr & 0x2));
@@ -489,16 +493,16 @@ static inline void TLan_DioWrite16(u16 base_addr, u16 internal_addr, u16 data)
 
 
 
-static inline void TLan_DioWrite32(u16 base_addr, u16 internal_addr, u32 data)
+static inline void tlan_dio_write32(u16 base_addr, u16 internal_addr, u32 data)
 {
 	outw(internal_addr, base_addr + TLAN_DIO_ADR);
 	outl(data, base_addr + TLAN_DIO_DATA + (internal_addr & 0x2));
 
 }
 
-#define TLan_ClearBit( bit, port )	outb_p(inb_p(port) & ~bit, port)
-#define TLan_GetBit( bit, port )	((int) (inb_p(port) & bit))
-#define TLan_SetBit( bit, port )	outb_p(inb_p(port) | bit, port)
+#define tlan_clear_bit(bit, port)	outb_p(inb_p(port) & ~bit, port)
+#define tlan_get_bit(bit, port)	((int) (inb_p(port) & bit))
+#define tlan_set_bit(bit, port)	outb_p(inb_p(port) | bit, port)
 
 /*
  * given 6 bytes, view them as 8 6-bit numbers and return the XOR of those
@@ -506,37 +510,37 @@ static inline void TLan_DioWrite32(u16 base_addr, u16 internal_addr, u32 data)
  *
  * The original code was:
  *
- * u32	xor( u32 a, u32 b ) {	return ( ( a && ! b ) || ( ! a && b ) ); }
+ * u32	xor(u32 a, u32 b) {	return ((a && !b ) || (! a && b )); }
  *
- * #define XOR8( a, b, c, d, e, f, g, h )	\
- * 	xor( a, xor( b, xor( c, xor( d, xor( e, xor( f, xor( g, h ) ) ) ) ) ) )
- * #define DA( a, bit )		( ( (u8) a[bit/8] ) & ( (u8) ( 1 << bit%8 ) ) )
+ * #define XOR8(a, b, c, d, e, f, g, h)	\
+ *	xor(a, xor(b, xor(c, xor(d, xor(e, xor(f, xor(g, h)) ) ) ) ) )
+ * #define DA(a, bit)		(( (u8) a[bit/8] ) & ( (u8) (1 << bit%8)) )
  *
- * 	hash  = XOR8( DA(a,0), DA(a, 6), DA(a,12), DA(a,18), DA(a,24),
- * 	              DA(a,30), DA(a,36), DA(a,42) );
- * 	hash |= XOR8( DA(a,1), DA(a, 7), DA(a,13), DA(a,19), DA(a,25),
- * 		      DA(a,31), DA(a,37), DA(a,43) ) << 1;
- * 	hash |= XOR8( DA(a,2), DA(a, 8), DA(a,14), DA(a,20), DA(a,26),
- * 		      DA(a,32), DA(a,38), DA(a,44) ) << 2;
- * 	hash |= XOR8( DA(a,3), DA(a, 9), DA(a,15), DA(a,21), DA(a,27),
- * 		      DA(a,33), DA(a,39), DA(a,45) ) << 3;
- * 	hash |= XOR8( DA(a,4), DA(a,10), DA(a,16), DA(a,22), DA(a,28),
- * 	              DA(a,34), DA(a,40), DA(a,46) ) << 4;
- * 	hash |= XOR8( DA(a,5), DA(a,11), DA(a,17), DA(a,23), DA(a,29),
- * 	              DA(a,35), DA(a,41), DA(a,47) ) << 5;
+ *	hash  = XOR8(DA(a,0), DA(a, 6), DA(a,12), DA(a,18), DA(a,24),
+ *		      DA(a,30), DA(a,36), DA(a,42));
+ *	hash |= XOR8(DA(a,1), DA(a, 7), DA(a,13), DA(a,19), DA(a,25),
+ *		      DA(a,31), DA(a,37), DA(a,43)) << 1;
+ *	hash |= XOR8(DA(a,2), DA(a, 8), DA(a,14), DA(a,20), DA(a,26),
+ *		      DA(a,32), DA(a,38), DA(a,44)) << 2;
+ *	hash |= XOR8(DA(a,3), DA(a, 9), DA(a,15), DA(a,21), DA(a,27),
+ *		      DA(a,33), DA(a,39), DA(a,45)) << 3;
+ *	hash |= XOR8(DA(a,4), DA(a,10), DA(a,16), DA(a,22), DA(a,28),
+ *		      DA(a,34), DA(a,40), DA(a,46)) << 4;
+ *	hash |= XOR8(DA(a,5), DA(a,11), DA(a,17), DA(a,23), DA(a,29),
+ *		      DA(a,35), DA(a,41), DA(a,47)) << 5;
  *
  */
-static inline u32 TLan_HashFunc( const u8 *a )
+static inline u32 tlan_hash_func(const u8 *a)
 {
-        u8     hash;
+	u8     hash;
 
-        hash = (a[0]^a[3]);             /* & 077 */
-        hash ^= ((a[0]^a[3])>>6);       /* & 003 */
-        hash ^= ((a[1]^a[4])<<2);       /* & 074 */
-        hash ^= ((a[1]^a[4])>>4);       /* & 017 */
-        hash ^= ((a[2]^a[5])<<4);       /* & 060 */
-        hash ^= ((a[2]^a[5])>>2);       /* & 077 */
+	hash = (a[0]^a[3]);		/* & 077 */
+	hash ^= ((a[0]^a[3])>>6);	/* & 003 */
+	hash ^= ((a[1]^a[4])<<2);	/* & 074 */
+	hash ^= ((a[1]^a[4])>>4);	/* & 017 */
+	hash ^= ((a[2]^a[5])<<4);	/* & 060 */
+	hash ^= ((a[2]^a[5])>>2);	/* & 077 */
 
-        return hash & 077;
+	return hash & 077;
 }
 #endif
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index b100bd50a0d7..f5e9ac00a07b 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -34,6 +34,8 @@
  *    Modifications for 2.3.99-pre5 kernel.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #define DRV_NAME	"tun"
 #define DRV_VERSION	"1.6"
 #define DRV_DESCRIPTION	"Universal TUN/TAP device driver"
@@ -76,11 +78,27 @@
 #ifdef TUN_DEBUG
 static int debug;
 
-#define DBG  if(tun->debug)printk
-#define DBG1 if(debug==2)printk
+#define tun_debug(level, tun, fmt, args...)			\
+do {								\
+	if (tun->debug)						\
+		netdev_printk(level, tun->dev, fmt, ##args);	\
+} while (0)
+#define DBG1(level, fmt, args...)				\
+do {								\
+	if (debug == 2)						\
+		printk(level fmt, ##args);			\
+} while (0)
 #else
-#define DBG( a... )
-#define DBG1( a... )
+#define tun_debug(level, tun, fmt, args...)			\
+do {								\
+	if (0)							\
+		netdev_printk(level, tun->dev, fmt, ##args);	\
+} while (0)
+#define DBG1(level, fmt, args...)				\
+do {								\
+	if (0)							\
+		printk(level fmt, ##args);			\
+} while (0)
 #endif
 
 #define FLT_EXACT_COUNT 8
@@ -205,7 +223,7 @@ static void tun_put(struct tun_struct *tun)
 		tun_detach(tfile->tun);
 }
 
-/* TAP filterting */
+/* TAP filtering */
 static void addr_hash_set(u32 *mask, const u8 *addr)
 {
 	int n = ether_crc(ETH_ALEN, addr) >> 26;
@@ -360,7 +378,7 @@ static netdev_tx_t tun_net_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct tun_struct *tun = netdev_priv(dev);
 
-	DBG(KERN_INFO "%s: tun_net_xmit %d\n", tun->dev->name, skb->len);
+	tun_debug(KERN_INFO, tun, "tun_net_xmit %d\n", skb->len);
 
 	/* Drop packet if interface is not attached */
 	if (!tun->tfile)
@@ -499,7 +517,7 @@ static unsigned int tun_chr_poll(struct file *file, poll_table * wait)
 
 	sk = tun->socket.sk;
 
-	DBG(KERN_INFO "%s: tun_chr_poll\n", tun->dev->name);
+	tun_debug(KERN_INFO, tun, "tun_chr_poll\n");
 
 	poll_wait(file, &tun->wq.wait, wait);
 
@@ -690,7 +708,7 @@ static ssize_t tun_chr_aio_write(struct kiocb *iocb, const struct iovec *iv,
 	if (!tun)
 		return -EBADFD;
 
-	DBG(KERN_INFO "%s: tun_chr_write %ld\n", tun->dev->name, count);
+	tun_debug(KERN_INFO, tun, "tun_chr_write %ld\n", count);
 
 	result = tun_get_user(tun, iv, iov_length(iv, count),
 			      file->f_flags & O_NONBLOCK);
@@ -739,7 +757,7 @@ static __inline__ ssize_t tun_put_user(struct tun_struct *tun,
 			else if (sinfo->gso_type & SKB_GSO_UDP)
 				gso.gso_type = VIRTIO_NET_HDR_GSO_UDP;
 			else {
-				printk(KERN_ERR "tun: unexpected GSO type: "
+				pr_err("unexpected GSO type: "
 				       "0x%x, gso_size %d, hdr_len %d\n",
 				       sinfo->gso_type, gso.gso_size,
 				       gso.hdr_len);
@@ -786,7 +804,7 @@ static ssize_t tun_do_read(struct tun_struct *tun,
 	struct sk_buff *skb;
 	ssize_t ret = 0;
 
-	DBG(KERN_INFO "%s: tun_chr_read\n", tun->dev->name);
+	tun_debug(KERN_INFO, tun, "tun_chr_read\n");
 
 	add_wait_queue(&tun->wq.wait, &wait);
 	while (len) {
@@ -1083,7 +1101,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 		if (device_create_file(&tun->dev->dev, &dev_attr_tun_flags) ||
 		    device_create_file(&tun->dev->dev, &dev_attr_owner) ||
 		    device_create_file(&tun->dev->dev, &dev_attr_group))
-			printk(KERN_ERR "Failed to create tun sysfs files\n");
+			pr_err("Failed to create tun sysfs files\n");
 
 		sk->sk_destruct = tun_sock_destruct;
 
@@ -1092,7 +1110,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 			goto failed;
 	}
 
-	DBG(KERN_INFO "%s: tun_set_iff\n", tun->dev->name);
+	tun_debug(KERN_INFO, tun, "tun_set_iff\n");
 
 	if (ifr->ifr_flags & IFF_NO_PI)
 		tun->flags |= TUN_NO_PI;
@@ -1129,7 +1147,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 static int tun_get_iff(struct net *net, struct tun_struct *tun,
 		       struct ifreq *ifr)
 {
-	DBG(KERN_INFO "%s: tun_get_iff\n", tun->dev->name);
+	tun_debug(KERN_INFO, tun, "tun_get_iff\n");
 
 	strcpy(ifr->ifr_name, tun->dev->name);
 
@@ -1142,7 +1160,7 @@ static int tun_get_iff(struct net *net, struct tun_struct *tun,
  * privs required. */
 static int set_offload(struct net_device *dev, unsigned long arg)
 {
-	unsigned int old_features, features;
+	u32 old_features, features;
 
 	old_features = dev->features;
 	/* Unset features, set them as we chew on the arg. */
@@ -1229,7 +1247,7 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 	if (!tun)
 		goto unlock;
 
-	DBG(KERN_INFO "%s: tun_chr_ioctl cmd %d\n", tun->dev->name, cmd);
+	tun_debug(KERN_INFO, tun, "tun_chr_ioctl cmd %d\n", cmd);
 
 	ret = 0;
 	switch (cmd) {
@@ -1249,8 +1267,8 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 		else
 			tun->flags &= ~TUN_NOCHECKSUM;
 
-		DBG(KERN_INFO "%s: checksum %s\n",
-		    tun->dev->name, arg ? "disabled" : "enabled");
+		tun_debug(KERN_INFO, tun, "checksum %s\n",
+			  arg ? "disabled" : "enabled");
 		break;
 
 	case TUNSETPERSIST:
@@ -1260,33 +1278,34 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 		else
 			tun->flags &= ~TUN_PERSIST;
 
-		DBG(KERN_INFO "%s: persist %s\n",
-		    tun->dev->name, arg ? "enabled" : "disabled");
+		tun_debug(KERN_INFO, tun, "persist %s\n",
+			  arg ? "enabled" : "disabled");
 		break;
 
 	case TUNSETOWNER:
 		/* Set owner of the device */
 		tun->owner = (uid_t) arg;
 
-		DBG(KERN_INFO "%s: owner set to %d\n", tun->dev->name, tun->owner);
+		tun_debug(KERN_INFO, tun, "owner set to %d\n", tun->owner);
 		break;
 
 	case TUNSETGROUP:
 		/* Set group of the device */
 		tun->group= (gid_t) arg;
 
-		DBG(KERN_INFO "%s: group set to %d\n", tun->dev->name, tun->group);
+		tun_debug(KERN_INFO, tun, "group set to %d\n", tun->group);
 		break;
 
 	case TUNSETLINK:
 		/* Only allow setting the type when the interface is down */
 		if (tun->dev->flags & IFF_UP) {
-			DBG(KERN_INFO "%s: Linktype set failed because interface is up\n",
-				tun->dev->name);
+			tun_debug(KERN_INFO, tun,
+				  "Linktype set failed because interface is up\n");
 			ret = -EBUSY;
 		} else {
 			tun->dev->type = (int) arg;
-			DBG(KERN_INFO "%s: linktype set to %d\n", tun->dev->name, tun->dev->type);
+			tun_debug(KERN_INFO, tun, "linktype set to %d\n",
+				  tun->dev->type);
 			ret = 0;
 		}
 		break;
@@ -1318,8 +1337,8 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 
 	case SIOCSIFHWADDR:
 		/* Set hw address */
-		DBG(KERN_DEBUG "%s: set hw address: %pM\n",
-			tun->dev->name, ifr.ifr_hwaddr.sa_data);
+		tun_debug(KERN_DEBUG, tun, "set hw address: %pM\n",
+			  ifr.ifr_hwaddr.sa_data);
 
 		ret = dev_set_mac_address(tun->dev, &ifr.ifr_hwaddr);
 		break;
@@ -1433,7 +1452,7 @@ static int tun_chr_fasync(int fd, struct file *file, int on)
 	if (!tun)
 		return -EBADFD;
 
-	DBG(KERN_INFO "%s: tun_chr_fasync %d\n", tun->dev->name, on);
+	tun_debug(KERN_INFO, tun, "tun_chr_fasync %d\n", on);
 
 	if ((ret = fasync_helper(fd, file, on, &tun->fasync)) < 0)
 		goto out;
@@ -1455,7 +1474,7 @@ static int tun_chr_open(struct inode *inode, struct file * file)
 {
 	struct tun_file *tfile;
 
-	DBG1(KERN_INFO "tunX: tun_chr_open\n");
+	DBG1(KERN_INFO, "tunX: tun_chr_open\n");
 
 	tfile = kmalloc(sizeof(*tfile), GFP_KERNEL);
 	if (!tfile)
@@ -1476,7 +1495,7 @@ static int tun_chr_close(struct inode *inode, struct file *file)
 	if (tun) {
 		struct net_device *dev = tun->dev;
 
-		DBG(KERN_INFO "%s: tun_chr_close\n", dev->name);
+		tun_debug(KERN_INFO, tun, "tun_chr_close\n");
 
 		__tun_detach(tun);
 
@@ -1607,18 +1626,18 @@ static int __init tun_init(void)
 {
 	int ret = 0;
 
-	printk(KERN_INFO "tun: %s, %s\n", DRV_DESCRIPTION, DRV_VERSION);
-	printk(KERN_INFO "tun: %s\n", DRV_COPYRIGHT);
+	pr_info("%s, %s\n", DRV_DESCRIPTION, DRV_VERSION);
+	pr_info("%s\n", DRV_COPYRIGHT);
 
 	ret = rtnl_link_register(&tun_link_ops);
 	if (ret) {
-		printk(KERN_ERR "tun: Can't register link_ops\n");
+		pr_err("Can't register link_ops\n");
 		goto err_linkops;
 	}
 
 	ret = misc_register(&tun_miscdev);
 	if (ret) {
-		printk(KERN_ERR "tun: Can't register misc device %d\n", TUN_MINOR);
+		pr_err("Can't register misc device %d\n", TUN_MINOR);
 		goto err_misc;
 	}
 	return  0;
diff --git a/drivers/net/typhoon.c b/drivers/net/typhoon.c
index a3c46f6a15e7..7fa5ec2de942 100644
--- a/drivers/net/typhoon.c
+++ b/drivers/net/typhoon.c
@@ -123,12 +123,11 @@ static const int multicast_filter_limit = 32;
 #include <linux/in6.h>
 #include <linux/dma-mapping.h>
 #include <linux/firmware.h>
-#include <generated/utsrelease.h>
 
 #include "typhoon.h"
 
 MODULE_AUTHOR("David Dillow <dave@thedillows.org>");
-MODULE_VERSION(UTS_RELEASE);
+MODULE_VERSION("1.0");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(FIRMWARE_NAME);
 MODULE_DESCRIPTION("3Com Typhoon Family (3C990, 3CR990, and variants)");
diff --git a/drivers/net/ucc_geth.c b/drivers/net/ucc_geth.c
index 715e7b47e7e9..ef041057d9d3 100644
--- a/drivers/net/ucc_geth.c
+++ b/drivers/net/ucc_geth.c
@@ -3740,7 +3740,7 @@ static const struct net_device_ops ucc_geth_netdev_ops = {
 #endif
 };
 
-static int ucc_geth_probe(struct platform_device* ofdev, const struct of_device_id *match)
+static int ucc_geth_probe(struct platform_device* ofdev)
 {
 	struct device *device = &ofdev->dev;
 	struct device_node *np = ofdev->dev.of_node;
@@ -3986,7 +3986,7 @@ static struct of_device_id ucc_geth_match[] = {
 
 MODULE_DEVICE_TABLE(of, ucc_geth_match);
 
-static struct of_platform_driver ucc_geth_driver = {
+static struct platform_driver ucc_geth_driver = {
 	.driver = {
 		.name = DRV_NAME,
 		.owner = THIS_MODULE,
@@ -4008,14 +4008,14 @@ static int __init ucc_geth_init(void)
 		memcpy(&(ugeth_info[i]), &ugeth_primary_info,
 		       sizeof(ugeth_primary_info));
 
-	ret = of_register_platform_driver(&ucc_geth_driver);
+	ret = platform_driver_register(&ucc_geth_driver);
 
 	return ret;
 }
 
 static void __exit ucc_geth_exit(void)
 {
-	of_unregister_platform_driver(&ucc_geth_driver);
+	platform_driver_unregister(&ucc_geth_driver);
 }
 
 module_init(ucc_geth_init);
diff --git a/drivers/net/usb/dm9601.c b/drivers/net/usb/dm9601.c
index 02b622e3b9fb..5002f5be47be 100644
--- a/drivers/net/usb/dm9601.c
+++ b/drivers/net/usb/dm9601.c
@@ -651,6 +651,10 @@ static const struct usb_device_id products[] = {
 	.driver_info = (unsigned long)&dm9601_info,
 	 },
 	{
+	 USB_DEVICE(0x0fe6, 0x9700),	/* DM9601 USB to Fast Ethernet Adapter */
+	 .driver_info = (unsigned long)&dm9601_info,
+	 },
+	{
 	 USB_DEVICE(0x0a46, 0x9000),	/* DM9000E */
 	 .driver_info = (unsigned long)&dm9601_info,
 	 },
diff --git a/drivers/net/vbus-enet.c b/drivers/net/vbus-enet.c
new file mode 100644
index 000000000000..94b86d482cee
--- /dev/null
+++ b/drivers/net/vbus-enet.c
@@ -0,0 +1,1560 @@
+/*
+ * vbus_enet - A virtualized 802.x network device based on the VBUS interface
+ *
+ * Copyright (C) 2009 Novell, Gregory Haskins <ghaskins@novell.com>
+ *
+ * Derived from the SNULL example from the book "Linux Device Drivers" by
+ * Alessandro Rubini, Jonathan Corbet, and Greg Kroah-Hartman, published
+ * by O'Reilly & Associates.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+
+#include <linux/sched.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+
+#include <linux/in.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/ip.h>
+#include <linux/tcp.h>
+#include <linux/skbuff.h>
+#include <linux/ioq.h>
+#include <linux/vbus_driver.h>
+
+#include <linux/in6.h>
+#include <asm/checksum.h>
+
+#include <linux/venet.h>
+
+MODULE_AUTHOR("Gregory Haskins");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("virtual-ethernet");
+MODULE_VERSION("1");
+
+static int rx_ringlen = 256;
+module_param(rx_ringlen, int, 0444);
+static int tx_ringlen = 256;
+module_param(tx_ringlen, int, 0444);
+static int sg_enabled = 1;
+module_param(sg_enabled, int, 0444);
+
+#define PDEBUG(_dev, fmt, args...) dev_dbg(&(_dev)->dev, fmt, ## args)
+
+#define SG_DESC_SIZE VSG_DESC_SIZE(MAX_SKB_FRAGS)
+
+struct vbus_enet_queue {
+	struct ioq              *queue;
+	struct ioq_notifier      notifier;
+	unsigned long            count;
+};
+
+struct vbus_enet_priv {
+	spinlock_t                 lock;
+	struct net_device         *dev;
+	struct vbus_device_proxy  *vdev;
+	struct napi_struct         napi;
+	struct vbus_enet_queue     rxq;
+	struct {
+		struct vbus_enet_queue veq;
+		struct tasklet_struct  task;
+		struct sk_buff_head    outstanding;
+	} tx;
+	bool                       sg;
+	struct {
+		bool               enabled;
+		char              *pool;
+	} pmtd; /* pre-mapped transmit descriptors */
+	struct {
+		bool                   enabled;
+		bool                   linkstate;
+		bool                   txc;
+		unsigned long          evsize;
+		struct vbus_enet_queue veq;
+		struct tasklet_struct  task;
+		char                  *pool;
+	} evq;
+	struct {
+		bool                   available;
+		char                  *pool;
+		struct vbus_enet_queue pageq;
+	} l4ro;
+
+	struct sk_buff *(*import)(struct vbus_enet_priv *priv,
+				  struct ioq_ring_desc *desc);
+};
+
+static void vbus_enet_tx_reap(struct vbus_enet_priv *priv);
+
+static struct vbus_enet_priv *
+napi_to_priv(struct napi_struct *napi)
+{
+	return container_of(napi, struct vbus_enet_priv, napi);
+}
+
+static int
+queue_init(struct vbus_enet_priv *priv,
+	   struct vbus_enet_queue *q,
+	   const char *name,
+	   int qid,
+	   size_t ringsize,
+	   void (*func)(struct ioq_notifier *))
+{
+	struct vbus_device_proxy *dev = priv->vdev;
+	int ret;
+	char _name[64];
+
+	if (name)
+		snprintf(_name, sizeof(_name), "%s-%s", priv->dev->name, name);
+
+	ret = vbus_driver_ioq_alloc(dev, name ? _name : NULL, qid, 0,
+				    ringsize, &q->queue);
+	if (ret < 0)
+		panic("ioq_alloc failed: %d\n", ret);
+
+	if (func) {
+		q->notifier.signal = func;
+		q->queue->notifier = &q->notifier;
+	}
+
+	q->count = ringsize;
+
+	return 0;
+}
+
+static int
+devcall(struct vbus_enet_priv *priv, u32 func, void *data, size_t len)
+{
+	struct vbus_device_proxy *dev = priv->vdev;
+
+	return dev->ops->call(dev, func, data, len, 0);
+}
+
+/*
+ * ---------------
+ * rx descriptors
+ * ---------------
+ */
+
+static void
+rxdesc_alloc(struct vbus_enet_priv *priv, struct ioq_ring_desc *desc, size_t len)
+{
+	struct net_device *dev = priv->dev;
+	struct sk_buff *skb;
+
+	len += ETH_HLEN;
+
+	skb = netdev_alloc_skb(dev, len + NET_IP_ALIGN);
+	BUG_ON(!skb);
+
+	skb_reserve(skb, NET_IP_ALIGN); /* align IP on 16B boundary */
+
+	if (priv->l4ro.available) {
+		/*
+		 * We will populate an SG descriptor initially with one
+		 * IOV filled with an MTU SKB.  If the packet needs to be
+		 * larger than MTU, the host will grab pages out of the
+		 * page-queue and populate additional IOVs
+		 */
+		struct venet_sg *vsg = (struct venet_sg *)(unsigned long)desc->cookie;
+		struct venet_iov *iov = &vsg->iov[0];
+
+		memset(vsg, 0, SG_DESC_SIZE);
+
+		vsg->cookie  = (u64)(unsigned long)skb;
+		vsg->count   = 1;
+
+		iov->ptr     = (u64)__pa(skb->data);
+		iov->len     = len;
+	} else {
+		desc->cookie = (u64)(unsigned long)skb;
+		desc->ptr    = cpu_to_le64(__pa(skb->data));
+		desc->len    = cpu_to_le64(len); /* total length  */
+	}
+
+	desc->valid  = 1;
+}
+
+static void
+rx_pageq_refill(struct vbus_enet_priv *priv, gfp_t gfp_mask)
+{
+	struct ioq *ioq = priv->l4ro.pageq.queue;
+	struct ioq_iterator iter;
+	int ret, added = 0;
+
+	if (ioq_full(ioq, ioq_idxtype_inuse))
+		/* nothing to do if the pageq is already fully populated */
+		return;
+
+	ret = ioq_iter_init(ioq, &iter, ioq_idxtype_inuse, 0);
+	BUG_ON(ret < 0); /* will never fail unless seriously broken */
+
+	ret = ioq_iter_seek(&iter, ioq_seek_tail, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * Now populate each descriptor with an empty page
+	 */
+	while (!iter.desc->sown) {
+		struct page *page = NULL;
+
+		page = alloc_page(gfp_mask);
+
+		if (!page)
+			break;
+
+		added = 1;
+		iter.desc->cookie = (u64)(unsigned long)page;
+		iter.desc->ptr    = cpu_to_le64(__pa(page_address(page)));
+		iter.desc->len    = cpu_to_le64(PAGE_SIZE);
+
+		ret = ioq_iter_push(&iter, 0);
+		BUG_ON(ret < 0);
+	}
+
+	if (added)
+		ioq_signal(ioq, 0);
+}
+
+static void
+rx_setup(struct vbus_enet_priv *priv)
+{
+	struct ioq *ioq = priv->rxq.queue;
+	struct ioq_iterator iter;
+	int ret;
+	int i = 0;
+
+	/*
+	 * We want to iterate on the "valid" index.  By default the iterator
+	 * will not "autoupdate" which means it will not hypercall the host
+	 * with our changes.  This is good, because we are really just
+	 * initializing stuff here anyway.  Note that you can always manually
+	 * signal the host with ioq_signal() if the autoupdate feature is not
+	 * used.
+	 */
+	ret = ioq_iter_init(ioq, &iter, ioq_idxtype_valid, 0);
+	BUG_ON(ret < 0); /* will never fail unless seriously broken */
+
+	/*
+	 * Seek to the tail of the valid index (which should be our first
+	 * item, since the queue is brand-new)
+	 */
+	ret = ioq_iter_seek(&iter, ioq_seek_tail, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * Now populate each descriptor with an empty buffer and mark it valid
+	 */
+	while (!iter.desc->valid) {
+		if (priv->l4ro.available) {
+			size_t offset = (i * SG_DESC_SIZE);
+			void *addr = &priv->l4ro.pool[offset];
+
+			iter.desc->ptr    = cpu_to_le64(offset);
+			iter.desc->cookie = (u64)(unsigned long)addr;
+			iter.desc->len    = cpu_to_le64(SG_DESC_SIZE);
+		}
+
+		rxdesc_alloc(priv, iter.desc, priv->dev->mtu);
+
+		/*
+		 * This push operation will simultaneously advance the
+		 * valid-head index and increment our position in the queue
+		 * by one.
+		 */
+		ret = ioq_iter_push(&iter, 0);
+		BUG_ON(ret < 0);
+
+		i++;
+	}
+
+	if (priv->l4ro.available)
+		rx_pageq_refill(priv, GFP_KERNEL);
+}
+
+static void
+rx_rxq_teardown(struct vbus_enet_priv *priv)
+{
+	struct ioq *ioq = priv->rxq.queue;
+	struct ioq_iterator iter;
+	int ret;
+
+	ret = ioq_iter_init(ioq, &iter, ioq_idxtype_valid, 0);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_head, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * free each valid descriptor
+	 */
+	while (iter.desc->valid) {
+		struct sk_buff *skb;
+
+		if (priv->l4ro.available) {
+			struct venet_sg *vsg;
+			int i;
+
+			vsg = (struct venet_sg *)(unsigned long)iter.desc->cookie;
+
+			/* skip i=0, since that is the skb->data IOV */
+			for (i = 1; i < vsg->count; i++) {
+				struct venet_iov *iov = &vsg->iov[i];
+				struct page *page = (struct page *)(unsigned long)iov->ptr;
+
+				put_page(page);
+			}
+
+			skb = (struct sk_buff *)(unsigned long)vsg->cookie;
+		} else
+			skb = (struct sk_buff *)(unsigned long)iter.desc->cookie;
+
+		iter.desc->valid = 0;
+		wmb();
+
+		iter.desc->ptr = 0;
+		iter.desc->cookie = 0;
+
+		ret = ioq_iter_pop(&iter, 0);
+		BUG_ON(ret < 0);
+
+		dev_kfree_skb(skb);
+	}
+}
+
+static void
+rx_l4ro_teardown(struct vbus_enet_priv *priv)
+{
+	struct ioq *ioq = priv->l4ro.pageq.queue;
+	struct ioq_iterator iter;
+	int ret;
+
+	ret = ioq_iter_init(ioq, &iter, ioq_idxtype_inuse, 0);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_head, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * free each valid descriptor
+	 */
+	while (iter.desc->sown) {
+		struct page *page = (struct page *)(unsigned long)iter.desc->cookie;
+
+		iter.desc->valid = 0;
+		wmb();
+
+		iter.desc->ptr = 0;
+		iter.desc->cookie = 0;
+
+		ret = ioq_iter_pop(&iter, 0);
+		BUG_ON(ret < 0);
+
+		put_page(page);
+	}
+
+	ioq_put(ioq);
+	kfree(priv->l4ro.pool);
+}
+
+static void
+rx_teardown(struct vbus_enet_priv *priv)
+{
+	rx_rxq_teardown(priv);
+
+	if (priv->l4ro.available)
+		rx_l4ro_teardown(priv);
+}
+
+static int
+tx_setup(struct vbus_enet_priv *priv)
+{
+	struct ioq *ioq    = priv->tx.veq.queue;
+	struct ioq_iterator iter;
+	int i;
+	int ret;
+
+	if (!priv->sg)
+		/*
+		 * There is nothing to do for a ring that is not using
+		 * scatter-gather
+		 */
+		return 0;
+
+	/* pre-allocate our descriptor pool if pmtd is enabled */
+	if (priv->pmtd.enabled) {
+		struct vbus_device_proxy *dev = priv->vdev;
+		size_t poollen = SG_DESC_SIZE * priv->tx.veq.count;
+		char *pool;
+		int shmid;
+
+		/* pmtdquery will return the shm-id to use for the pool */
+		ret = devcall(priv, VENET_FUNC_PMTDQUERY, NULL, 0);
+		BUG_ON(ret < 0);
+
+		shmid = ret;
+
+		pool = kzalloc(poollen, GFP_KERNEL | GFP_DMA);
+		if (!pool)
+			return -ENOMEM;
+
+		priv->pmtd.pool = pool;
+
+		ret = dev->ops->shm(dev, NULL, shmid, 0, pool, poollen,
+				    NULL, NULL, 0);
+		BUG_ON(ret < 0);
+	}
+
+	ret = ioq_iter_init(ioq, &iter, ioq_idxtype_valid, 0);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_set, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * Now populate each descriptor with an empty SG descriptor
+	 */
+	for (i = 0; i < priv->tx.veq.count; i++) {
+		struct venet_sg *vsg;
+
+		if (priv->pmtd.enabled) {
+			size_t offset = (i * SG_DESC_SIZE);
+
+			vsg = (struct venet_sg *)&priv->pmtd.pool[offset];
+			iter.desc->ptr = cpu_to_le64(offset);
+		} else {
+			vsg = kzalloc(SG_DESC_SIZE, GFP_KERNEL);
+			if (!vsg)
+				return -ENOMEM;
+
+			iter.desc->ptr = cpu_to_le64(__pa(vsg));
+		}
+
+		iter.desc->cookie = (u64)(unsigned long)vsg;
+		iter.desc->len    = cpu_to_le64(SG_DESC_SIZE);
+
+		ret = ioq_iter_seek(&iter, ioq_seek_next, 0, 0);
+		BUG_ON(ret < 0);
+	}
+
+	return 0;
+}
+
+static void
+tx_teardown(struct vbus_enet_priv *priv)
+{
+	struct ioq *ioq = priv->tx.veq.queue;
+	struct ioq_iterator iter;
+	struct sk_buff *skb;
+	int ret;
+
+	/* forcefully free all outstanding transmissions */
+	while ((skb = __skb_dequeue(&priv->tx.outstanding)))
+		dev_kfree_skb(skb);
+
+	if (!priv->sg)
+		/*
+		 * There is nothing else to do for a ring that is not using
+		 * scatter-gather
+		 */
+		return;
+
+	if (priv->pmtd.enabled) {
+		/*
+		 * PMTD mode means we only need to free the pool
+		 */
+		kfree(priv->pmtd.pool);
+		return;
+	}
+
+	ret = ioq_iter_init(ioq, &iter, ioq_idxtype_valid, 0);
+	BUG_ON(ret < 0);
+
+	/* seek to position 0 */
+	ret = ioq_iter_seek(&iter, ioq_seek_set, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * free each valid descriptor
+	 */
+	while (iter.desc->cookie) {
+		struct venet_sg *vsg = (struct venet_sg *)(unsigned long)iter.desc->cookie;
+
+		iter.desc->valid = 0;
+		wmb();
+
+		iter.desc->ptr = 0;
+		iter.desc->cookie = 0;
+
+		ret = ioq_iter_seek(&iter, ioq_seek_next, 0, 0);
+		BUG_ON(ret < 0);
+
+		kfree(vsg);
+	}
+}
+
+static void
+evq_teardown(struct vbus_enet_priv *priv)
+{
+	if (!priv->evq.enabled)
+		return;
+
+	ioq_put(priv->evq.veq.queue);
+	kfree(priv->evq.pool);
+}
+
+/*
+ * Open and close
+ */
+
+static int
+vbus_enet_open(struct net_device *dev)
+{
+	struct vbus_enet_priv *priv = netdev_priv(dev);
+	int ret;
+
+	ret = devcall(priv, VENET_FUNC_LINKUP, NULL, 0);
+	BUG_ON(ret < 0);
+
+	napi_enable(&priv->napi);
+
+	return 0;
+}
+
+static int
+vbus_enet_stop(struct net_device *dev)
+{
+	struct vbus_enet_priv *priv = netdev_priv(dev);
+	int ret;
+
+	napi_disable(&priv->napi);
+
+	ret = devcall(priv, VENET_FUNC_LINKDOWN, NULL, 0);
+	BUG_ON(ret < 0);
+
+	return 0;
+}
+
+/*
+ * Configuration changes (passed on by ifconfig)
+ */
+static int
+vbus_enet_config(struct net_device *dev, struct ifmap *map)
+{
+	if (dev->flags & IFF_UP) /* can't act on a running interface */
+		return -EBUSY;
+
+	/* Don't allow changing the I/O address */
+	if (map->base_addr != dev->base_addr) {
+		dev_warn(&dev->dev, "Can't change I/O address\n");
+		return -EOPNOTSUPP;
+	}
+
+	/* ignore other fields */
+	return 0;
+}
+
+static void
+vbus_enet_schedule_rx(struct vbus_enet_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (napi_schedule_prep(&priv->napi)) {
+		/* Disable further interrupts */
+		ioq_notify_disable(priv->rxq.queue, 0);
+		__napi_schedule(&priv->napi);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int
+vbus_enet_change_mtu(struct net_device *dev, int new_mtu)
+{
+	struct vbus_enet_priv *priv = netdev_priv(dev);
+	int ret;
+
+	dev->mtu = new_mtu;
+
+	/*
+	 * FLUSHRX will cause the device to flush any outstanding
+	 * RX buffers.  They will appear to come in as 0 length
+	 * packets which we can simply discard and replace with new_mtu
+	 * buffers for the future.
+	 */
+	ret = devcall(priv, VENET_FUNC_FLUSHRX, NULL, 0);
+	BUG_ON(ret < 0);
+
+	vbus_enet_schedule_rx(priv);
+
+	return 0;
+}
+
+static struct sk_buff *
+vbus_enet_l4ro_import(struct vbus_enet_priv *priv, struct ioq_ring_desc *desc)
+{
+	struct venet_sg *vsg = (struct venet_sg *)(unsigned long)desc->cookie;
+	struct sk_buff *skb = (struct sk_buff *)(unsigned long)vsg->cookie;
+	struct skb_shared_info *sinfo = skb_shinfo(skb);
+	int i;
+
+	rx_pageq_refill(priv, GFP_ATOMIC);
+
+	if (!vsg->len)
+		/*
+		 * the device may send a zero-length packet when its
+		 * flushing references on the ring.  We can just drop
+		 * these on the floor
+		 */
+		goto fail;
+
+	/* advance only by the linear portion in IOV[0] */
+	skb_put(skb, vsg->iov[0].len);
+
+	/* skip i=0, since that is the skb->data IOV */
+	for (i = 1; i < vsg->count; i++) {
+		struct venet_iov *iov = &vsg->iov[i];
+		struct page *page = (struct page *)(unsigned long)iov->ptr;
+		skb_frag_t *f = &sinfo->frags[i-1];
+
+		f->page        = page;
+		f->page_offset = 0;
+		f->size        = iov->len;
+
+		PDEBUG(priv->dev, "SG: Importing %d byte page[%i]\n",
+		       f->size, i);
+
+		skb->data_len += f->size;
+		skb->len      += f->size;
+		skb->truesize += f->size;
+		sinfo->nr_frags++;
+	}
+
+	if (vsg->flags & VENET_SG_FLAG_NEEDS_CSUM
+	    && !skb_partial_csum_set(skb, vsg->csum.start,
+				     vsg->csum.offset)) {
+		priv->dev->stats.rx_frame_errors++;
+		goto fail;
+	}
+
+	if (vsg->flags & VENET_SG_FLAG_GSO) {
+		PDEBUG(priv->dev, "L4RO packet detected\n");
+
+		switch (vsg->gso.type) {
+		case VENET_GSO_TYPE_TCPV4:
+			sinfo->gso_type = SKB_GSO_TCPV4;
+			break;
+		case VENET_GSO_TYPE_TCPV6:
+			sinfo->gso_type = SKB_GSO_TCPV6;
+			break;
+		case VENET_GSO_TYPE_UDP:
+			sinfo->gso_type = SKB_GSO_UDP;
+			break;
+		default:
+			PDEBUG(priv->dev, "Illegal L4RO type: %d\n",
+			       vsg->gso.type);
+			priv->dev->stats.rx_frame_errors++;
+			goto fail;
+		}
+
+		if (vsg->flags & VENET_SG_FLAG_ECN)
+			sinfo->gso_type |= SKB_GSO_TCP_ECN;
+
+		sinfo->gso_size = vsg->gso.size;
+		if (sinfo->gso_size == 0) {
+			PDEBUG(priv->dev, "Illegal L4RO size: %d\n",
+			       vsg->gso.size);
+			priv->dev->stats.rx_frame_errors++;
+			goto fail;
+		}
+
+		/*
+		 * Header must be checked, and gso_segs
+		 * computed.
+		 */
+		sinfo->gso_type |= SKB_GSO_DODGY;
+		sinfo->gso_segs = 0;
+	}
+
+	return skb;
+
+fail:
+	dev_kfree_skb(skb);
+
+	return NULL;
+}
+
+static struct sk_buff *
+vbus_enet_flat_import(struct vbus_enet_priv *priv, struct ioq_ring_desc *desc)
+{
+	struct sk_buff *skb = (struct sk_buff *)(unsigned long)desc->cookie;
+
+	if (!desc->len) {
+		/*
+		 * the device may send a zero-length packet when its
+		 * flushing references on the ring.  We can just drop
+		 * these on the floor
+		 */
+		dev_kfree_skb(skb);
+		return NULL;
+	}
+
+	skb_put(skb, le64_to_cpu(desc->len));
+
+	return skb;
+}
+
+/*
+ * The poll implementation.
+ */
+static int
+vbus_enet_poll(struct napi_struct *napi, int budget)
+{
+	struct vbus_enet_priv *priv = napi_to_priv(napi);
+	int npackets = 0;
+	struct ioq_iterator iter;
+	int ret;
+
+	PDEBUG(priv->dev, "polling...\n");
+
+	/* We want to iterate on the head of the in-use index */
+	ret = ioq_iter_init(priv->rxq.queue, &iter, ioq_idxtype_inuse,
+			    IOQ_ITER_AUTOUPDATE);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_head, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * We stop if we have met the quota or there are no more packets.
+	 * The EOM is indicated by finding a packet that is still owned by
+	 * the south side
+	 */
+	while ((npackets < budget) && (!iter.desc->sown)) {
+		struct sk_buff *skb;
+
+		skb = priv->import(priv, iter.desc);
+		if (skb) {
+			/* Maintain stats */
+			npackets++;
+			priv->dev->stats.rx_packets++;
+			priv->dev->stats.rx_bytes += skb->len;
+
+			/* Pass the buffer up to the stack */
+			skb->dev      = priv->dev;
+			skb->protocol = eth_type_trans(skb, priv->dev);
+			netif_receive_skb(skb);
+
+			mb();
+		}
+
+		/* Grab a new buffer to put in the ring */
+		rxdesc_alloc(priv, iter.desc, priv->dev->mtu);
+
+		/* Advance the in-use tail */
+		ret = ioq_iter_pop(&iter, 0);
+		BUG_ON(ret < 0);
+	}
+
+	PDEBUG(priv->dev, "%d packets received\n", npackets);
+
+	/*
+	 * If we processed all packets, we're done; tell the kernel and
+	 * reenable ints
+	 */
+	if (ioq_empty(priv->rxq.queue, ioq_idxtype_inuse)) {
+		napi_complete(napi);
+		ioq_notify_enable(priv->rxq.queue, 0);
+		ret = 0;
+	} else
+		/* We couldn't process everything. */
+		ret = 1;
+
+	return ret;
+}
+
+/*
+ * Transmit a packet (called by the kernel)
+ */
+static int
+vbus_enet_tx_start(struct sk_buff *skb, struct net_device *dev)
+{
+	struct vbus_enet_priv *priv = netdev_priv(dev);
+	struct ioq_iterator    iter;
+	int ret;
+	unsigned long flags;
+
+	PDEBUG(priv->dev, "sending %d bytes\n", skb->len);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (ioq_full(priv->tx.veq.queue, ioq_idxtype_valid)) {
+		/*
+		 * We must flow-control the kernel by disabling the
+		 * queue
+		 */
+		spin_unlock_irqrestore(&priv->lock, flags);
+		netif_stop_queue(dev);
+		dev_err(&priv->dev->dev, "tx on full queue bug\n");
+		return 1;
+	}
+
+	/*
+	 * We want to iterate on the tail of both the "inuse" and "valid" index
+	 * so we specify the "both" index
+	 */
+	ret = ioq_iter_init(priv->tx.veq.queue, &iter, ioq_idxtype_both,
+			    IOQ_ITER_AUTOUPDATE);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_tail, 0, 0);
+	BUG_ON(ret < 0);
+	BUG_ON(iter.desc->sown);
+
+	if (priv->sg) {
+		struct venet_sg *vsg = (struct venet_sg *)(unsigned long)iter.desc->cookie;
+		struct scatterlist sgl[MAX_SKB_FRAGS+1];
+		struct scatterlist *sg;
+		int count, maxcount = ARRAY_SIZE(sgl);
+
+		sg_init_table(sgl, maxcount);
+
+		memset(vsg, 0, sizeof(*vsg));
+
+		vsg->cookie = (u64)(unsigned long)skb;
+		vsg->len    = skb->len;
+
+		if (skb->ip_summed == CHECKSUM_PARTIAL) {
+			vsg->flags      |= VENET_SG_FLAG_NEEDS_CSUM;
+			vsg->csum.start  = skb->csum_start - skb_headroom(skb);
+			vsg->csum.offset = skb->csum_offset;
+		}
+
+		if (skb_is_gso(skb)) {
+			struct skb_shared_info *sinfo = skb_shinfo(skb);
+
+			vsg->flags |= VENET_SG_FLAG_GSO;
+
+			vsg->gso.hdrlen = skb_headlen(skb);
+			vsg->gso.size = sinfo->gso_size;
+			if (sinfo->gso_type & SKB_GSO_TCPV4)
+				vsg->gso.type = VENET_GSO_TYPE_TCPV4;
+			else if (sinfo->gso_type & SKB_GSO_TCPV6)
+				vsg->gso.type = VENET_GSO_TYPE_TCPV6;
+			else if (sinfo->gso_type & SKB_GSO_UDP)
+				vsg->gso.type = VENET_GSO_TYPE_UDP;
+			else
+				panic("Virtual-Ethernet: unknown GSO type " \
+				      "0x%x\n", sinfo->gso_type);
+
+			if (sinfo->gso_type & SKB_GSO_TCP_ECN)
+				vsg->flags |= VENET_SG_FLAG_ECN;
+		}
+
+		count = skb_to_sgvec(skb, sgl, 0, skb->len);
+
+		BUG_ON(count > maxcount);
+
+		for (sg = &sgl[0]; sg; sg = sg_next(sg)) {
+			struct venet_iov *iov = &vsg->iov[vsg->count++];
+
+			iov->len = sg->length;
+			iov->ptr = (u64)sg_phys(sg);
+		}
+
+		iter.desc->len = cpu_to_le64(VSG_DESC_SIZE(vsg->count));
+
+	} else {
+		/*
+		 * non scatter-gather mode: simply put the skb right onto the
+		 * ring.
+		 */
+		iter.desc->cookie = (u64)(unsigned long)skb;
+		iter.desc->len = cpu_to_le64(skb->len);
+		iter.desc->ptr = cpu_to_le64(__pa(skb->data));
+	}
+
+	iter.desc->valid  = 1;
+
+	priv->dev->stats.tx_packets++;
+	priv->dev->stats.tx_bytes += skb->len;
+
+	skb_queue_tail(&priv->tx.outstanding, skb);
+
+	/*
+	 * This advances both indexes together implicitly, and then
+	 * signals the south side to consume the packet
+	 */
+	ret = ioq_iter_push(&iter, 0);
+	BUG_ON(ret < 0);
+
+	dev->trans_start = jiffies; /* save the timestamp */
+
+	if (ioq_full(priv->tx.veq.queue, ioq_idxtype_valid)) {
+		/*
+		 * If the queue is congested, we must flow-control the kernel
+		 */
+		PDEBUG(priv->dev, "backpressure tx queue\n");
+		netif_stop_queue(dev);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/* assumes priv->lock held */
+static void
+vbus_enet_skb_complete(struct vbus_enet_priv *priv, struct sk_buff *skb)
+{
+	PDEBUG(priv->dev, "completed sending %d bytes\n",
+	       skb->len);
+
+	skb_unlink(skb, &priv->tx.outstanding);
+	dev_kfree_skb(skb);
+}
+
+/*
+ * reclaim any outstanding completed tx packets
+ *
+ * assumes priv->lock held
+ */
+static struct sk_buff *
+vbus_enet_tx_reap_one(struct vbus_enet_priv *priv)
+{
+	struct sk_buff *skb = NULL;
+	struct ioq_iterator iter;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/*
+	 * We want to iterate on the head of the valid index, but we
+	 * do not want the iter_pop (below) to flip the ownership, so
+	 * we set the NOFLIPOWNER option
+	 */
+	ret = ioq_iter_init(priv->tx.veq.queue, &iter, ioq_idxtype_valid,
+			    IOQ_ITER_NOFLIPOWNER);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_head, 0, 0);
+	BUG_ON(ret < 0);
+
+	if (iter.desc->valid && !iter.desc->sown) {
+
+		if (priv->sg) {
+			struct venet_sg *vsg;
+
+			vsg = (struct venet_sg *)(unsigned long)iter.desc->cookie;
+			skb = (struct sk_buff *)(unsigned long)vsg->cookie;
+		} else
+			skb = (struct sk_buff *)(unsigned long)iter.desc->cookie;
+
+		/* Reset the descriptor */
+		iter.desc->valid  = 0;
+
+		/* Advance the valid-index head */
+		ret = ioq_iter_pop(&iter, 0);
+		BUG_ON(ret < 0);
+	}
+
+	/*
+	 * If we were previously stopped due to flow control, restart the
+	 * processing
+	 */
+	if (netif_queue_stopped(priv->dev)
+	    && !ioq_full(priv->tx.veq.queue, ioq_idxtype_valid)) {
+		PDEBUG(priv->dev, "re-enabling tx queue\n");
+		netif_wake_queue(priv->dev);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return skb;
+}
+
+static void
+vbus_enet_tx_reap(struct vbus_enet_priv *priv)
+{
+	struct sk_buff *skb;
+
+	while ((skb = vbus_enet_tx_reap_one(priv))) {
+		if (!priv->evq.txc)
+			/*
+			 * We are responsible for freeing the packet upon
+			 * reap if TXC is not enabled
+			 */
+			vbus_enet_skb_complete(priv, skb);
+	}
+}
+
+static void
+vbus_enet_timeout(struct net_device *dev)
+{
+	struct vbus_enet_priv *priv = netdev_priv(dev);
+
+	dev_dbg(&dev->dev, "Transmit timeout\n");
+
+	vbus_enet_tx_reap(priv);
+}
+
+static void
+rx_isr(struct ioq_notifier *notifier)
+{
+	struct vbus_enet_priv *priv;
+	struct net_device  *dev;
+
+	priv = container_of(notifier, struct vbus_enet_priv, rxq.notifier);
+	dev = priv->dev;
+
+	if (!ioq_empty(priv->rxq.queue, ioq_idxtype_inuse))
+		vbus_enet_schedule_rx(priv);
+}
+
+static void
+deferred_tx_isr(unsigned long data)
+{
+	struct vbus_enet_priv *priv = (struct vbus_enet_priv *)data;
+
+	PDEBUG(priv->dev, "deferred_tx_isr\n");
+
+	vbus_enet_tx_reap(priv);
+
+	ioq_notify_enable(priv->tx.veq.queue, 0);
+}
+
+static void
+tx_isr(struct ioq_notifier *notifier)
+{
+       struct vbus_enet_priv *priv;
+
+       priv = container_of(notifier, struct vbus_enet_priv, tx.veq.notifier);
+
+       PDEBUG(priv->dev, "tx_isr\n");
+
+       ioq_notify_disable(priv->tx.veq.queue, 0);
+       tasklet_schedule(&priv->tx.task);
+}
+
+static void
+evq_linkstate_event(struct vbus_enet_priv *priv,
+		    struct venet_event_header *header)
+{
+	struct venet_event_linkstate *event =
+		(struct venet_event_linkstate *)header;
+
+	switch (event->state) {
+	case 0:
+		netif_carrier_off(priv->dev);
+		break;
+	case 1:
+		netif_carrier_on(priv->dev);
+		break;
+	default:
+		break;
+	}
+}
+
+static void
+evq_txc_event(struct vbus_enet_priv *priv,
+	      struct venet_event_header *header)
+{
+	struct venet_event_txc *event =
+		(struct venet_event_txc *)header;
+
+	vbus_enet_tx_reap(priv);
+
+	vbus_enet_skb_complete(priv, (struct sk_buff *)(unsigned long)event->cookie);
+}
+
+static void
+deferred_evq_isr(unsigned long data)
+{
+	struct vbus_enet_priv *priv = (struct vbus_enet_priv *)data;
+	int nevents = 0;
+	struct ioq_iterator iter;
+	int ret;
+
+	PDEBUG(priv->dev, "evq: polling...\n");
+
+	/* We want to iterate on the head of the in-use index */
+	ret = ioq_iter_init(priv->evq.veq.queue, &iter, ioq_idxtype_inuse,
+			    IOQ_ITER_AUTOUPDATE);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_head, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * The EOM is indicated by finding a packet that is still owned by
+	 * the south side
+	 */
+	while (!iter.desc->sown) {
+		struct venet_event_header *header;
+
+		header = (struct venet_event_header *)(unsigned long)iter.desc->cookie;
+
+		switch (header->id) {
+		case VENET_EVENT_LINKSTATE:
+			evq_linkstate_event(priv, header);
+			break;
+		case VENET_EVENT_TXC:
+			evq_txc_event(priv, header);
+			break;
+		default:
+			panic("venet: unexpected event id:%d of size %d\n",
+			      header->id, header->size);
+			break;
+		}
+
+		memset((void *)(unsigned long)iter.desc->cookie, 0, priv->evq.evsize);
+
+		/* Advance the in-use tail */
+		ret = ioq_iter_pop(&iter, 0);
+		BUG_ON(ret < 0);
+
+		nevents++;
+	}
+
+	PDEBUG(priv->dev, "%d events received\n", nevents);
+
+	ioq_notify_enable(priv->evq.veq.queue, 0);
+}
+
+static void
+evq_isr(struct ioq_notifier *notifier)
+{
+       struct vbus_enet_priv *priv;
+
+       priv = container_of(notifier, struct vbus_enet_priv, evq.veq.notifier);
+
+       PDEBUG(priv->dev, "evq_isr\n");
+
+       ioq_notify_disable(priv->evq.veq.queue, 0);
+       tasklet_schedule(&priv->evq.task);
+}
+
+static int
+vbus_enet_sg_negcap(struct vbus_enet_priv *priv)
+{
+	struct net_device *dev = priv->dev;
+	struct venet_capabilities caps;
+	int ret;
+
+	memset(&caps, 0, sizeof(caps));
+
+	if (sg_enabled) {
+		caps.gid = VENET_CAP_GROUP_SG;
+		caps.bits |= (VENET_CAP_SG|VENET_CAP_TSO4|VENET_CAP_TSO6
+			      |VENET_CAP_ECN|VENET_CAP_PMTD);
+		/* note: exclude UFO for now due to stack bug */
+	}
+
+	ret = devcall(priv, VENET_FUNC_NEGCAP, &caps, sizeof(caps));
+	if (ret < 0)
+		return ret;
+
+	if (caps.bits & VENET_CAP_SG) {
+		priv->sg = true;
+
+		dev->features |= NETIF_F_SG|NETIF_F_HW_CSUM|NETIF_F_FRAGLIST;
+
+		if (caps.bits & VENET_CAP_TSO4)
+			dev->features |= NETIF_F_TSO;
+		if (caps.bits & VENET_CAP_UFO)
+			dev->features |= NETIF_F_UFO;
+		if (caps.bits & VENET_CAP_TSO6)
+			dev->features |= NETIF_F_TSO6;
+		if (caps.bits & VENET_CAP_ECN)
+			dev->features |= NETIF_F_TSO_ECN;
+
+		if (caps.bits & VENET_CAP_PMTD)
+			priv->pmtd.enabled = true;
+	}
+
+	return 0;
+}
+
+static int
+vbus_enet_evq_negcap(struct vbus_enet_priv *priv, unsigned long count)
+{
+	struct venet_capabilities caps;
+	int ret;
+
+	memset(&caps, 0, sizeof(caps));
+
+	caps.gid = VENET_CAP_GROUP_EVENTQ;
+	caps.bits |= VENET_CAP_EVQ_LINKSTATE;
+	caps.bits |= VENET_CAP_EVQ_TXC;
+
+	ret = devcall(priv, VENET_FUNC_NEGCAP, &caps, sizeof(caps));
+	if (ret < 0)
+		return ret;
+
+	if (caps.bits) {
+		struct vbus_device_proxy *dev = priv->vdev;
+		struct venet_eventq_query query;
+		size_t                    poollen;
+		struct ioq_iterator       iter;
+		char                     *pool;
+		int                       i;
+
+		priv->evq.enabled = true;
+
+		if (caps.bits & VENET_CAP_EVQ_LINKSTATE) {
+			/*
+			 * We will assume there is no carrier until we get
+			 * an event telling us otherwise
+			 */
+			netif_carrier_off(priv->dev);
+			priv->evq.linkstate = true;
+		}
+
+		if (caps.bits & VENET_CAP_EVQ_TXC)
+			priv->evq.txc = true;
+
+		memset(&query, 0, sizeof(query));
+
+		ret = devcall(priv, VENET_FUNC_EVQQUERY, &query, sizeof(query));
+		if (ret < 0)
+			return ret;
+
+		priv->evq.evsize = query.evsize;
+		poollen = query.evsize * count;
+
+		pool = kzalloc(poollen, GFP_KERNEL | GFP_DMA);
+		if (!pool)
+			return -ENOMEM;
+
+		priv->evq.pool = pool;
+
+		ret = dev->ops->shm(dev, NULL, query.dpid, 0,
+				    pool, poollen, NULL, NULL, 0);
+		if (ret < 0)
+			return ret;
+
+		queue_init(priv, &priv->evq.veq, "evq",
+			   query.qid, count, evq_isr);
+
+		ret = ioq_iter_init(priv->evq.veq.queue,
+				    &iter, ioq_idxtype_valid, 0);
+		BUG_ON(ret < 0);
+
+		ret = ioq_iter_seek(&iter, ioq_seek_set, 0, 0);
+		BUG_ON(ret < 0);
+
+		/* Now populate each descriptor with an empty event */
+		for (i = 0; i < count; i++) {
+			size_t offset = (i * query.evsize);
+			void *addr = &priv->evq.pool[offset];
+
+			iter.desc->ptr    = cpu_to_le64(offset);
+			iter.desc->cookie = (u64)(unsigned long)addr;
+			iter.desc->len    = cpu_to_le64(query.evsize);
+
+			ret = ioq_iter_push(&iter, 0);
+			BUG_ON(ret < 0);
+		}
+
+		/* Finally, enable interrupts */
+		tasklet_init(&priv->evq.task, deferred_evq_isr,
+			     (unsigned long)priv);
+		ioq_notify_enable(priv->evq.veq.queue, 0);
+	}
+
+	return 0;
+}
+
+static int
+vbus_enet_l4ro_negcap(struct vbus_enet_priv *priv, unsigned long count)
+{
+	struct venet_capabilities caps;
+	int ret;
+
+	memset(&caps, 0, sizeof(caps));
+
+	caps.gid = VENET_CAP_GROUP_L4RO;
+	caps.bits |= (VENET_CAP_SG|VENET_CAP_TSO4|VENET_CAP_TSO6
+		      |VENET_CAP_ECN);
+
+	ret = devcall(priv, VENET_FUNC_NEGCAP, &caps, sizeof(caps));
+	if (ret < 0) {
+		printk(KERN_ERR "Error negotiating L4RO: %d\n", ret);
+		return ret;
+	}
+
+	if (caps.bits & VENET_CAP_SG) {
+		struct vbus_device_proxy *dev = priv->vdev;
+		size_t                    poollen = SG_DESC_SIZE * count;
+		struct venet_l4ro_query    query;
+		char                     *pool;
+
+		memset(&query, 0, sizeof(query));
+
+		ret = devcall(priv, VENET_FUNC_L4ROQUERY, &query, sizeof(query));
+		if (ret < 0) {
+			printk(KERN_ERR "Error querying L4RO: %d\n", ret);
+			return ret;
+		}
+
+		pool = kzalloc(poollen, GFP_KERNEL | GFP_DMA);
+		if (!pool)
+			return -ENOMEM;
+
+		/*
+		 * pre-mapped descriptor pool
+		 */
+		ret = dev->ops->shm(dev, NULL, query.dpid, 0,
+				    pool, poollen, NULL, NULL, 0);
+		if (ret < 0) {
+			printk(KERN_ERR "Error registering L4RO pool: %d\n",
+			       ret);
+			kfree(pool);
+			return ret;
+		}
+
+		/*
+		 * page-queue: contains a ring of arbitrary pages for
+		 * consumption by the host for when the SG::IOV count exceeds
+		 * one MTU frame.  All we need to do is keep it populated
+		 * with free pages.
+		 */
+		queue_init(priv, &priv->l4ro.pageq, "pageq", query.pqid,
+			   count, NULL);
+
+		priv->l4ro.pool      = pool;
+		priv->l4ro.available = true;
+	}
+
+	return 0;
+}
+
+static int
+vbus_enet_negcap(struct vbus_enet_priv *priv)
+{
+	int ret;
+
+	ret = vbus_enet_sg_negcap(priv);
+	if (ret < 0)
+		return ret;
+
+	ret = vbus_enet_evq_negcap(priv, tx_ringlen);
+	if (ret < 0)
+		return ret;
+
+	ret = vbus_enet_l4ro_negcap(priv, rx_ringlen);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int vbus_enet_set_tx_csum(struct net_device *dev, u32 data)
+{
+	struct vbus_enet_priv *priv = netdev_priv(dev);
+
+	if (data && !priv->sg)
+		return -ENOSYS;
+
+	return ethtool_op_set_tx_hw_csum(dev, data);
+}
+
+static struct ethtool_ops vbus_enet_ethtool_ops = {
+	.set_tx_csum = vbus_enet_set_tx_csum,
+	.set_sg      = ethtool_op_set_sg,
+	.set_tso     = ethtool_op_set_tso,
+	.get_link    = ethtool_op_get_link,
+};
+
+static const struct net_device_ops vbus_enet_netdev_ops = {
+	.ndo_open            = vbus_enet_open,
+	.ndo_stop            = vbus_enet_stop,
+	.ndo_set_config      = vbus_enet_config,
+	.ndo_start_xmit      = vbus_enet_tx_start,
+	.ndo_change_mtu	     = vbus_enet_change_mtu,
+	.ndo_tx_timeout      = vbus_enet_timeout,
+	.ndo_set_mac_address = eth_mac_addr,
+	.ndo_validate_addr   = eth_validate_addr,
+};
+
+/*
+ * This is called whenever a new vbus_device_proxy is added to the vbus
+ * with the matching VENET_ID
+ */
+static int
+vbus_enet_probe(struct vbus_device_proxy *vdev)
+{
+	struct net_device  *dev;
+	struct vbus_enet_priv *priv;
+	int ret;
+
+	printk(KERN_INFO "VENET: Found new device at %lld\n", vdev->id);
+
+	ret = vdev->ops->open(vdev, VENET_VERSION, 0);
+	if (ret < 0)
+		return ret;
+
+	dev = alloc_etherdev(sizeof(struct vbus_enet_priv));
+	if (!dev)
+		return -ENOMEM;
+
+	/*
+	 * establish our device-name early so we can incorporate it into
+	 * the signal-path names, etc
+	 */
+	rtnl_lock();
+
+	ret = dev_alloc_name(dev, dev->name);
+	if (ret < 0)
+		goto out_free;
+
+	priv = netdev_priv(dev);
+
+	spin_lock_init(&priv->lock);
+	priv->dev  = dev;
+	priv->vdev = vdev;
+
+	ret = vbus_enet_negcap(priv);
+	if (ret < 0) {
+		printk(KERN_INFO "VENET: Error negotiating capabilities for " \
+		       "%lld\n",
+		       priv->vdev->id);
+		goto out_free;
+	}
+
+	if (priv->l4ro.available)
+		priv->import = &vbus_enet_l4ro_import;
+	else
+		priv->import = &vbus_enet_flat_import;
+
+	skb_queue_head_init(&priv->tx.outstanding);
+
+	queue_init(priv, &priv->rxq, "rx", VENET_QUEUE_RX, rx_ringlen,
+		   rx_isr);
+	queue_init(priv, &priv->tx.veq, "tx", VENET_QUEUE_TX, tx_ringlen,
+		   tx_isr);
+
+	rx_setup(priv);
+	tx_setup(priv);
+
+	ioq_notify_enable(priv->rxq.queue, 0);  /* enable rx interrupts */
+
+	if (!priv->evq.txc) {
+		/*
+		 * If the TXC feature is present, we will recieve our
+		 * tx-complete notification via the event-channel.  Therefore,
+		 * we only enable txq interrupts if the TXC feature is not
+		 * present.
+		 */
+		tasklet_init(&priv->tx.task, deferred_tx_isr,
+			     (unsigned long)priv);
+		ioq_notify_enable(priv->tx.veq.queue, 0);
+	}
+
+	dev->netdev_ops     = &vbus_enet_netdev_ops;
+	dev->watchdog_timeo = 5 * HZ;
+	SET_ETHTOOL_OPS(dev, &vbus_enet_ethtool_ops);
+	SET_NETDEV_DEV(dev, &vdev->dev);
+
+	netif_napi_add(dev, &priv->napi, vbus_enet_poll, 128);
+
+	ret = devcall(priv, VENET_FUNC_MACQUERY, priv->dev->dev_addr, ETH_ALEN);
+	if (ret < 0) {
+		printk(KERN_INFO "VENET: Error obtaining MAC address for " \
+		       "%lld\n",
+		       priv->vdev->id);
+		goto out_free;
+	}
+
+	dev->features |= NETIF_F_HIGHDMA;
+
+	ret = register_netdevice(dev);
+	if (ret < 0) {
+		printk(KERN_INFO "VENET: error %i registering device \"%s\"\n",
+		       ret, dev->name);
+		goto out_free;
+	}
+
+	rtnl_unlock();
+
+	vdev->priv = priv;
+
+	return 0;
+
+ out_free:
+	rtnl_unlock();
+
+	free_netdev(dev);
+
+	return ret;
+}
+
+static int
+vbus_enet_remove(struct vbus_device_proxy *vdev)
+{
+	struct vbus_enet_priv *priv = (struct vbus_enet_priv *)vdev->priv;
+	struct vbus_device_proxy *dev = priv->vdev;
+
+	unregister_netdev(priv->dev);
+	napi_disable(&priv->napi);
+
+	rx_teardown(priv);
+	ioq_put(priv->rxq.queue);
+
+	tx_teardown(priv);
+	ioq_put(priv->tx.veq.queue);
+
+	if (priv->evq.enabled)
+		evq_teardown(priv);
+
+	dev->ops->close(dev, 0);
+
+	free_netdev(priv->dev);
+
+	return 0;
+}
+
+/*
+ * Finally, the module stuff
+ */
+
+static struct vbus_driver_ops vbus_enet_driver_ops = {
+	.probe  = vbus_enet_probe,
+	.remove = vbus_enet_remove,
+};
+
+static struct vbus_driver vbus_enet_driver = {
+	.type   = VENET_TYPE,
+	.owner  = THIS_MODULE,
+	.ops    = &vbus_enet_driver_ops,
+};
+
+static __init int
+vbus_enet_init_module(void)
+{
+	printk(KERN_INFO "Virtual Ethernet: Copyright (C) 2009 Novell, Gregory Haskins\n");
+	printk(KERN_DEBUG "VENET: Using %d/%d queue depth\n",
+	       rx_ringlen, tx_ringlen);
+	return vbus_driver_register(&vbus_enet_driver);
+}
+
+static __exit void
+vbus_enet_cleanup(void)
+{
+	vbus_driver_unregister(&vbus_enet_driver);
+}
+
+module_init(vbus_enet_init_module);
+module_exit(vbus_enet_cleanup);
+
+VBUS_DRIVER_AUTOPROBE(VENET_TYPE);
diff --git a/drivers/net/veth.c b/drivers/net/veth.c
index cc83fa71c3ff..105d7f0630cc 100644
--- a/drivers/net/veth.c
+++ b/drivers/net/veth.c
@@ -403,17 +403,6 @@ static int veth_newlink(struct net *src_net, struct net_device *dev,
 	if (tb[IFLA_ADDRESS] == NULL)
 		random_ether_addr(dev->dev_addr);
 
-	if (tb[IFLA_IFNAME])
-		nla_strlcpy(dev->name, tb[IFLA_IFNAME], IFNAMSIZ);
-	else
-		snprintf(dev->name, IFNAMSIZ, DRV_NAME "%%d");
-
-	if (strchr(dev->name, '%')) {
-		err = dev_alloc_name(dev, dev->name);
-		if (err < 0)
-			goto err_alloc_name;
-	}
-
 	err = register_netdevice(dev);
 	if (err < 0)
 		goto err_register_dev;
@@ -433,7 +422,6 @@ static int veth_newlink(struct net *src_net, struct net_device *dev,
 
 err_register_dev:
 	/* nothing to do */
-err_alloc_name:
 err_configure_peer:
 	unregister_netdevice(peer);
 	return err;
diff --git a/drivers/net/via-velocity.c b/drivers/net/via-velocity.c
index 09cac704fdd7..0d6fec6b7d93 100644
--- a/drivers/net/via-velocity.c
+++ b/drivers/net/via-velocity.c
@@ -2923,6 +2923,7 @@ static u16 wol_calc_crc(int size, u8 *pattern, u8 *mask_pattern)
 static int velocity_set_wol(struct velocity_info *vptr)
 {
 	struct mac_regs __iomem *regs = vptr->mac_regs;
+	enum speed_opt spd_dpx = vptr->options.spd_dpx;
 	static u8 buf[256];
 	int i;
 
@@ -2968,6 +2969,12 @@ static int velocity_set_wol(struct velocity_info *vptr)
 
 	writew(0x0FFF, &regs->WOLSRClr);
 
+	if (spd_dpx == SPD_DPX_1000_FULL)
+		goto mac_done;
+
+	if (spd_dpx != SPD_DPX_AUTO)
+		goto advertise_done;
+
 	if (vptr->mii_status & VELOCITY_AUTONEG_ENABLE) {
 		if (PHYID_GET_PHY_ID(vptr->phy_id) == PHYID_CICADA_CS8201)
 			MII_REG_BITS_ON(AUXCR_MDPPS, MII_NCONFIG, vptr->mac_regs);
@@ -2978,6 +2985,7 @@ static int velocity_set_wol(struct velocity_info *vptr)
 	if (vptr->mii_status & VELOCITY_SPEED_1000)
 		MII_REG_BITS_ON(BMCR_ANRESTART, MII_BMCR, vptr->mac_regs);
 
+advertise_done:
 	BYTE_REG_BITS_ON(CHIPGCR_FCMODE, &regs->CHIPGCR);
 
 	{
@@ -2987,6 +2995,7 @@ static int velocity_set_wol(struct velocity_info *vptr)
 		writeb(GCR, &regs->CHIPGCR);
 	}
 
+mac_done:
 	BYTE_REG_BITS_OFF(ISR_PWEI, &regs->ISR);
 	/* Turn on SWPTAG just before entering power mode */
 	BYTE_REG_BITS_ON(STICKHW_SWPTAG, &regs->STICKHW);
diff --git a/drivers/net/via-velocity.h b/drivers/net/via-velocity.h
index aa2e69b9ff61..d7227539484e 100644
--- a/drivers/net/via-velocity.h
+++ b/drivers/net/via-velocity.h
@@ -361,7 +361,7 @@ enum  velocity_owner {
 #define MAC_REG_CHIPGSR     0x9C
 #define MAC_REG_TESTCFG     0x9D
 #define MAC_REG_DEBUG       0x9E
-#define MAC_REG_CHIPGCR     0x9F
+#define MAC_REG_CHIPGCR     0x9F	/* Chip Operation and Diagnostic Control */
 #define MAC_REG_WOLCR0_SET  0xA0
 #define MAC_REG_WOLCR1_SET  0xA1
 #define MAC_REG_PWCFG_SET   0xA2
@@ -848,10 +848,10 @@ enum  velocity_owner {
  *	Bits in CHIPGCR register
  */
 
-#define CHIPGCR_FCGMII      0x80	/* enable GMII mode */
-#define CHIPGCR_FCFDX       0x40
+#define CHIPGCR_FCGMII      0x80	/* force GMII (else MII only) */
+#define CHIPGCR_FCFDX       0x40	/* force full duplex */
 #define CHIPGCR_FCRESV      0x20
-#define CHIPGCR_FCMODE      0x10
+#define CHIPGCR_FCMODE      0x10	/* enable MAC forced mode */
 #define CHIPGCR_LPSOPT      0x08
 #define CHIPGCR_TM1US       0x04
 #define CHIPGCR_TM0US       0x02
diff --git a/drivers/net/vxge/vxge-config.c b/drivers/net/vxge/vxge-config.c
index 228d4f7a58af..e74e4b42592d 100644
--- a/drivers/net/vxge/vxge-config.c
+++ b/drivers/net/vxge/vxge-config.c
@@ -387,8 +387,8 @@ vxge_hw_vpath_eprom_img_ver_get(struct __vxge_hw_device *hldev,
 		data1 = steer_ctrl = 0;
 
 		status = vxge_hw_vpath_fw_api(vpath,
-			VXGE_HW_RTS_ACCESS_STEER_CTRL_DATA_STRUCT_SEL_FW_MEMO,
 			VXGE_HW_FW_API_GET_EPROM_REV,
+			VXGE_HW_RTS_ACCESS_STEER_CTRL_DATA_STRUCT_SEL_FW_MEMO,
 			0, &data0, &data1, &steer_ctrl);
 		if (status != VXGE_HW_OK)
 			break;
@@ -2868,6 +2868,8 @@ __vxge_hw_ring_create(struct __vxge_hw_vpath_handle *vp,
 	ring->rxd_init = attr->rxd_init;
 	ring->rxd_term = attr->rxd_term;
 	ring->buffer_mode = config->buffer_mode;
+	ring->tim_rti_cfg1_saved = vp->vpath->tim_rti_cfg1_saved;
+	ring->tim_rti_cfg3_saved = vp->vpath->tim_rti_cfg3_saved;
 	ring->rxds_limit = config->rxds_limit;
 
 	ring->rxd_size = vxge_hw_ring_rxd_size_get(config->buffer_mode);
@@ -3511,6 +3513,8 @@ __vxge_hw_fifo_create(struct __vxge_hw_vpath_handle *vp,
 
 	/* apply "interrupts per txdl" attribute */
 	fifo->interrupt_type = VXGE_HW_FIFO_TXD_INT_TYPE_UTILZ;
+	fifo->tim_tti_cfg1_saved = vpath->tim_tti_cfg1_saved;
+	fifo->tim_tti_cfg3_saved = vpath->tim_tti_cfg3_saved;
 
 	if (fifo->config->intr)
 		fifo->interrupt_type = VXGE_HW_FIFO_TXD_INT_TYPE_PER_LIST;
@@ -4377,6 +4381,8 @@ __vxge_hw_vpath_tim_configure(struct __vxge_hw_device *hldev, u32 vp_id)
 		}
 
 		writeq(val64, &vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_TX]);
+		vpath->tim_tti_cfg1_saved = val64;
+
 		val64 = readq(&vp_reg->tim_cfg2_int_num[VXGE_HW_VPATH_INTR_TX]);
 
 		if (config->tti.uec_a != VXGE_HW_USE_FLASH_DEFAULT) {
@@ -4433,6 +4439,7 @@ __vxge_hw_vpath_tim_configure(struct __vxge_hw_device *hldev, u32 vp_id)
 		}
 
 		writeq(val64, &vp_reg->tim_cfg3_int_num[VXGE_HW_VPATH_INTR_TX]);
+		vpath->tim_tti_cfg3_saved = val64;
 	}
 
 	if (config->ring.enable == VXGE_HW_RING_ENABLE) {
@@ -4481,6 +4488,8 @@ __vxge_hw_vpath_tim_configure(struct __vxge_hw_device *hldev, u32 vp_id)
 		}
 
 		writeq(val64, &vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_RX]);
+		vpath->tim_rti_cfg1_saved = val64;
+
 		val64 = readq(&vp_reg->tim_cfg2_int_num[VXGE_HW_VPATH_INTR_RX]);
 
 		if (config->rti.uec_a != VXGE_HW_USE_FLASH_DEFAULT) {
@@ -4537,6 +4546,7 @@ __vxge_hw_vpath_tim_configure(struct __vxge_hw_device *hldev, u32 vp_id)
 		}
 
 		writeq(val64, &vp_reg->tim_cfg3_int_num[VXGE_HW_VPATH_INTR_RX]);
+		vpath->tim_rti_cfg3_saved = val64;
 	}
 
 	val64 = 0;
@@ -4555,26 +4565,6 @@ __vxge_hw_vpath_tim_configure(struct __vxge_hw_device *hldev, u32 vp_id)
 	return status;
 }
 
-void vxge_hw_vpath_tti_ci_set(struct __vxge_hw_device *hldev, u32 vp_id)
-{
-	struct __vxge_hw_virtualpath *vpath;
-	struct vxge_hw_vpath_reg __iomem *vp_reg;
-	struct vxge_hw_vp_config *config;
-	u64 val64;
-
-	vpath = &hldev->virtual_paths[vp_id];
-	vp_reg = vpath->vp_reg;
-	config = vpath->vp_config;
-
-	if (config->fifo.enable == VXGE_HW_FIFO_ENABLE &&
-	    config->tti.timer_ci_en != VXGE_HW_TIM_TIMER_CI_ENABLE) {
-		config->tti.timer_ci_en = VXGE_HW_TIM_TIMER_CI_ENABLE;
-		val64 = readq(&vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_TX]);
-		val64 |= VXGE_HW_TIM_CFG1_INT_NUM_TIMER_CI;
-		writeq(val64, &vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_TX]);
-	}
-}
-
 /*
  * __vxge_hw_vpath_initialize
  * This routine is the final phase of init which initializes the
diff --git a/drivers/net/vxge/vxge-config.h b/drivers/net/vxge/vxge-config.h
index e249e288d160..3c53aa732c9d 100644
--- a/drivers/net/vxge/vxge-config.h
+++ b/drivers/net/vxge/vxge-config.h
@@ -682,6 +682,10 @@ struct __vxge_hw_virtualpath {
 	u32				vsport_number;
 	u32				max_kdfc_db;
 	u32				max_nofl_db;
+	u64				tim_tti_cfg1_saved;
+	u64				tim_tti_cfg3_saved;
+	u64				tim_rti_cfg1_saved;
+	u64				tim_rti_cfg3_saved;
 
 	struct __vxge_hw_ring *____cacheline_aligned ringh;
 	struct __vxge_hw_fifo *____cacheline_aligned fifoh;
@@ -921,6 +925,9 @@ struct __vxge_hw_ring {
 	u32					doorbell_cnt;
 	u32					total_db_cnt;
 	u64					rxds_limit;
+	u32					rtimer;
+	u64					tim_rti_cfg1_saved;
+	u64					tim_rti_cfg3_saved;
 
 	enum vxge_hw_status (*callback)(
 			struct __vxge_hw_ring *ringh,
@@ -1000,6 +1007,9 @@ struct __vxge_hw_fifo {
 	u32					per_txdl_space;
 	u32					vp_id;
 	u32					tx_intr_num;
+	u32					rtimer;
+	u64					tim_tti_cfg1_saved;
+	u64					tim_tti_cfg3_saved;
 
 	enum vxge_hw_status (*callback)(
 			struct __vxge_hw_fifo *fifo_handle,
diff --git a/drivers/net/vxge/vxge-main.c b/drivers/net/vxge/vxge-main.c
index c81a6512c683..395423aeec00 100644
--- a/drivers/net/vxge/vxge-main.c
+++ b/drivers/net/vxge/vxge-main.c
@@ -371,9 +371,6 @@ vxge_rx_1b_compl(struct __vxge_hw_ring *ringh, void *dtr,
 	struct vxge_hw_ring_rxd_info ext_info;
 	vxge_debug_entryexit(VXGE_TRACE, "%s: %s:%d",
 		ring->ndev->name, __func__, __LINE__);
-	ring->pkts_processed = 0;
-
-	vxge_hw_ring_replenish(ringh);
 
 	do {
 		prefetch((char *)dtr + L1_CACHE_BYTES);
@@ -1588,6 +1585,36 @@ static int vxge_reset_vpath(struct vxgedev *vdev, int vp_id)
 	return ret;
 }
 
+/* Configure CI */
+static void vxge_config_ci_for_tti_rti(struct vxgedev *vdev)
+{
+	int i = 0;
+
+	/* Enable CI for RTI */
+	if (vdev->config.intr_type == MSI_X) {
+		for (i = 0; i < vdev->no_of_vpath; i++) {
+			struct __vxge_hw_ring *hw_ring;
+
+			hw_ring = vdev->vpaths[i].ring.handle;
+			vxge_hw_vpath_dynamic_rti_ci_set(hw_ring);
+		}
+	}
+
+	/* Enable CI for TTI */
+	for (i = 0; i < vdev->no_of_vpath; i++) {
+		struct __vxge_hw_fifo *hw_fifo = vdev->vpaths[i].fifo.handle;
+		vxge_hw_vpath_tti_ci_set(hw_fifo);
+		/*
+		 * For Inta (with or without napi), Set CI ON for only one
+		 * vpath. (Have only one free running timer).
+		 */
+		if ((vdev->config.intr_type == INTA) && (i == 0))
+			break;
+	}
+
+	return;
+}
+
 static int do_vxge_reset(struct vxgedev *vdev, int event)
 {
 	enum vxge_hw_status status;
@@ -1753,6 +1780,9 @@ static int do_vxge_reset(struct vxgedev *vdev, int event)
 		netif_tx_wake_all_queues(vdev->ndev);
 	}
 
+	/* configure CI */
+	vxge_config_ci_for_tti_rti(vdev);
+
 out:
 	vxge_debug_entryexit(VXGE_TRACE,
 		"%s:%d  Exiting...", __func__, __LINE__);
@@ -1793,22 +1823,29 @@ static void vxge_reset(struct work_struct *work)
  */
 static int vxge_poll_msix(struct napi_struct *napi, int budget)
 {
-	struct vxge_ring *ring =
-		container_of(napi, struct vxge_ring, napi);
+	struct vxge_ring *ring = container_of(napi, struct vxge_ring, napi);
+	int pkts_processed;
 	int budget_org = budget;
-	ring->budget = budget;
 
+	ring->budget = budget;
+	ring->pkts_processed = 0;
 	vxge_hw_vpath_poll_rx(ring->handle);
+	pkts_processed = ring->pkts_processed;
 
 	if (ring->pkts_processed < budget_org) {
 		napi_complete(napi);
+
 		/* Re enable the Rx interrupts for the vpath */
 		vxge_hw_channel_msix_unmask(
 				(struct __vxge_hw_channel *)ring->handle,
 				ring->rx_vector_no);
+		mmiowb();
 	}
 
-	return ring->pkts_processed;
+	/* We are copying and returning the local variable, in case if after
+	 * clearing the msix interrupt above, if the interrupt fires right
+	 * away which can preempt this NAPI thread */
+	return pkts_processed;
 }
 
 static int vxge_poll_inta(struct napi_struct *napi, int budget)
@@ -1824,6 +1861,7 @@ static int vxge_poll_inta(struct napi_struct *napi, int budget)
 	for (i = 0; i < vdev->no_of_vpath; i++) {
 		ring = &vdev->vpaths[i].ring;
 		ring->budget = budget;
+		ring->pkts_processed = 0;
 		vxge_hw_vpath_poll_rx(ring->handle);
 		pkts_processed += ring->pkts_processed;
 		budget -= ring->pkts_processed;
@@ -2054,6 +2092,7 @@ static int vxge_open_vpaths(struct vxgedev *vdev)
 					netdev_get_tx_queue(vdev->ndev, 0);
 			vpath->fifo.indicate_max_pkts =
 				vdev->config.fifo_indicate_max_pkts;
+			vpath->fifo.tx_vector_no = 0;
 			vpath->ring.rx_vector_no = 0;
 			vpath->ring.rx_csum = vdev->rx_csum;
 			vpath->ring.rx_hwts = vdev->rx_hwts;
@@ -2079,6 +2118,61 @@ static int vxge_open_vpaths(struct vxgedev *vdev)
 	return VXGE_HW_OK;
 }
 
+/**
+ *  adaptive_coalesce_tx_interrupts - Changes the interrupt coalescing
+ *  if the interrupts are not within a range
+ *  @fifo: pointer to transmit fifo structure
+ *  Description: The function changes boundary timer and restriction timer
+ *  value depends on the traffic
+ *  Return Value: None
+ */
+static void adaptive_coalesce_tx_interrupts(struct vxge_fifo *fifo)
+{
+	fifo->interrupt_count++;
+	if (jiffies > fifo->jiffies + HZ / 100) {
+		struct __vxge_hw_fifo *hw_fifo = fifo->handle;
+
+		fifo->jiffies = jiffies;
+		if (fifo->interrupt_count > VXGE_T1A_MAX_TX_INTERRUPT_COUNT &&
+		    hw_fifo->rtimer != VXGE_TTI_RTIMER_ADAPT_VAL) {
+			hw_fifo->rtimer = VXGE_TTI_RTIMER_ADAPT_VAL;
+			vxge_hw_vpath_dynamic_tti_rtimer_set(hw_fifo);
+		} else if (hw_fifo->rtimer != 0) {
+			hw_fifo->rtimer = 0;
+			vxge_hw_vpath_dynamic_tti_rtimer_set(hw_fifo);
+		}
+		fifo->interrupt_count = 0;
+	}
+}
+
+/**
+ *  adaptive_coalesce_rx_interrupts - Changes the interrupt coalescing
+ *  if the interrupts are not within a range
+ *  @ring: pointer to receive ring structure
+ *  Description: The function increases of decreases the packet counts within
+ *  the ranges of traffic utilization, if the interrupts due to this ring are
+ *  not within a fixed range.
+ *  Return Value: Nothing
+ */
+static void adaptive_coalesce_rx_interrupts(struct vxge_ring *ring)
+{
+	ring->interrupt_count++;
+	if (jiffies > ring->jiffies + HZ / 100) {
+		struct __vxge_hw_ring *hw_ring = ring->handle;
+
+		ring->jiffies = jiffies;
+		if (ring->interrupt_count > VXGE_T1A_MAX_INTERRUPT_COUNT &&
+		    hw_ring->rtimer != VXGE_RTI_RTIMER_ADAPT_VAL) {
+			hw_ring->rtimer = VXGE_RTI_RTIMER_ADAPT_VAL;
+			vxge_hw_vpath_dynamic_rti_rtimer_set(hw_ring);
+		} else if (hw_ring->rtimer != 0) {
+			hw_ring->rtimer = 0;
+			vxge_hw_vpath_dynamic_rti_rtimer_set(hw_ring);
+		}
+		ring->interrupt_count = 0;
+	}
+}
+
 /*
  *  vxge_isr_napi
  *  @irq: the irq of the device.
@@ -2139,24 +2233,39 @@ static irqreturn_t vxge_isr_napi(int irq, void *dev_id)
 
 #ifdef CONFIG_PCI_MSI
 
-static irqreturn_t
-vxge_tx_msix_handle(int irq, void *dev_id)
+static irqreturn_t vxge_tx_msix_handle(int irq, void *dev_id)
 {
 	struct vxge_fifo *fifo = (struct vxge_fifo *)dev_id;
 
+	adaptive_coalesce_tx_interrupts(fifo);
+
+	vxge_hw_channel_msix_mask((struct __vxge_hw_channel *)fifo->handle,
+				  fifo->tx_vector_no);
+
+	vxge_hw_channel_msix_clear((struct __vxge_hw_channel *)fifo->handle,
+				   fifo->tx_vector_no);
+
 	VXGE_COMPLETE_VPATH_TX(fifo);
 
+	vxge_hw_channel_msix_unmask((struct __vxge_hw_channel *)fifo->handle,
+				    fifo->tx_vector_no);
+
+	mmiowb();
+
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t
-vxge_rx_msix_napi_handle(int irq, void *dev_id)
+static irqreturn_t vxge_rx_msix_napi_handle(int irq, void *dev_id)
 {
 	struct vxge_ring *ring = (struct vxge_ring *)dev_id;
 
-	/* MSIX_IDX for Rx is 1 */
+	adaptive_coalesce_rx_interrupts(ring);
+
 	vxge_hw_channel_msix_mask((struct __vxge_hw_channel *)ring->handle,
-					ring->rx_vector_no);
+				  ring->rx_vector_no);
+
+	vxge_hw_channel_msix_clear((struct __vxge_hw_channel *)ring->handle,
+				   ring->rx_vector_no);
 
 	napi_schedule(&ring->napi);
 	return IRQ_HANDLED;
@@ -2173,14 +2282,20 @@ vxge_alarm_msix_handle(int irq, void *dev_id)
 		VXGE_HW_VPATH_MSIX_ACTIVE) + VXGE_ALARM_MSIX_ID;
 
 	for (i = 0; i < vdev->no_of_vpath; i++) {
+		/* Reduce the chance of loosing alarm interrupts by masking
+		 * the vector. A pending bit will be set if an alarm is
+		 * generated and on unmask the interrupt will be fired.
+		 */
 		vxge_hw_vpath_msix_mask(vdev->vpaths[i].handle, msix_id);
+		vxge_hw_vpath_msix_clear(vdev->vpaths[i].handle, msix_id);
+		mmiowb();
 
 		status = vxge_hw_vpath_alarm_process(vdev->vpaths[i].handle,
 			vdev->exec_mode);
 		if (status == VXGE_HW_OK) {
-
 			vxge_hw_vpath_msix_unmask(vdev->vpaths[i].handle,
-					msix_id);
+						  msix_id);
+			mmiowb();
 			continue;
 		}
 		vxge_debug_intr(VXGE_ERR,
@@ -2299,6 +2414,9 @@ static int vxge_enable_msix(struct vxgedev *vdev)
 			vpath->ring.rx_vector_no = (vpath->device_id *
 						VXGE_HW_VPATH_MSIX_ACTIVE) + 1;
 
+			vpath->fifo.tx_vector_no = (vpath->device_id *
+						VXGE_HW_VPATH_MSIX_ACTIVE);
+
 			vxge_hw_vpath_msix_set(vpath->handle, tim_msix_id,
 					       VXGE_ALARM_MSIX_ID);
 		}
@@ -2474,8 +2592,9 @@ INTA_MODE:
 			"%s:vxge:INTA", vdev->ndev->name);
 		vxge_hw_device_set_intr_type(vdev->devh,
 			VXGE_HW_INTR_MODE_IRQLINE);
-		vxge_hw_vpath_tti_ci_set(vdev->devh,
-			vdev->vpaths[0].device_id);
+
+		vxge_hw_vpath_tti_ci_set(vdev->vpaths[0].fifo.handle);
+
 		ret = request_irq((int) vdev->pdev->irq,
 			vxge_isr_napi,
 			IRQF_SHARED, vdev->desc[0], vdev);
@@ -2745,6 +2864,10 @@ static int vxge_open(struct net_device *dev)
 	}
 
 	netif_tx_start_all_queues(vdev->ndev);
+
+	/* configure CI */
+	vxge_config_ci_for_tti_rti(vdev);
+
 	goto out0;
 
 out2:
@@ -3264,19 +3387,6 @@ static const struct net_device_ops vxge_netdev_ops = {
 #endif
 };
 
-static int __devinit vxge_device_revision(struct vxgedev *vdev)
-{
-	int ret;
-	u8 revision;
-
-	ret = pci_read_config_byte(vdev->pdev, PCI_REVISION_ID, &revision);
-	if (ret)
-		return -EIO;
-
-	vdev->titan1 = (revision == VXGE_HW_TITAN1_PCI_REVISION);
-	return 0;
-}
-
 static int __devinit vxge_device_register(struct __vxge_hw_device *hldev,
 					  struct vxge_config *config,
 					  int high_dma, int no_of_vpath,
@@ -3316,10 +3426,7 @@ static int __devinit vxge_device_register(struct __vxge_hw_device *hldev,
 	memcpy(&vdev->config, config, sizeof(struct vxge_config));
 	vdev->rx_csum = 1;	/* Enable Rx CSUM by default. */
 	vdev->rx_hwts = 0;
-
-	ret = vxge_device_revision(vdev);
-	if (ret < 0)
-		goto _out1;
+	vdev->titan1 = (vdev->pdev->revision == VXGE_HW_TITAN1_PCI_REVISION);
 
 	SET_NETDEV_DEV(ndev, &vdev->pdev->dev);
 
@@ -3348,7 +3455,7 @@ static int __devinit vxge_device_register(struct __vxge_hw_device *hldev,
 		vxge_debug_init(VXGE_ERR,
 			"%s: vpath memory allocation failed",
 			vdev->ndev->name);
-		ret = -ENODEV;
+		ret = -ENOMEM;
 		goto _out1;
 	}
 
@@ -3369,11 +3476,11 @@ static int __devinit vxge_device_register(struct __vxge_hw_device *hldev,
 	if (vdev->config.gro_enable)
 		ndev->features |= NETIF_F_GRO;
 
-	if (register_netdev(ndev)) {
+	ret = register_netdev(ndev);
+	if (ret) {
 		vxge_debug_init(vxge_hw_device_trace_level_get(hldev),
 			"%s: %s : device registration failed!",
 			ndev->name, __func__);
-		ret = -ENODEV;
 		goto _out2;
 	}
 
@@ -3444,6 +3551,11 @@ static void vxge_device_unregister(struct __vxge_hw_device *hldev)
 	/* in 2.6 will call stop() if device is up */
 	unregister_netdev(dev);
 
+	kfree(vdev->vpaths);
+
+	/* we are safe to free it now */
+	free_netdev(dev);
+
 	vxge_debug_init(vdev->level_trace, "%s: ethernet device unregistered",
 			buf);
 	vxge_debug_entryexit(vdev->level_trace,	"%s: %s:%d  Exiting...", buf,
@@ -3799,7 +3911,7 @@ static void __devinit vxge_device_config_init(
 		break;
 
 	case MSI_X:
-		device_config->intr_mode = VXGE_HW_INTR_MODE_MSIX;
+		device_config->intr_mode = VXGE_HW_INTR_MODE_MSIX_ONE_SHOT;
 		break;
 	}
 
@@ -4335,10 +4447,10 @@ vxge_probe(struct pci_dev *pdev, const struct pci_device_id *pre)
 		goto _exit1;
 	}
 
-	if (pci_request_region(pdev, 0, VXGE_DRIVER_NAME)) {
+	ret = pci_request_region(pdev, 0, VXGE_DRIVER_NAME);
+	if (ret) {
 		vxge_debug_init(VXGE_ERR,
 			"%s : request regions failed", __func__);
-		ret = -ENODEV;
 		goto _exit1;
 	}
 
@@ -4446,7 +4558,7 @@ vxge_probe(struct pci_dev *pdev, const struct pci_device_id *pre)
 			if (!img[i].is_valid)
 				break;
 			vxge_debug_init(VXGE_TRACE, "%s: EPROM %d, version "
-					"%d.%d.%d.%d\n", VXGE_DRIVER_NAME, i,
+					"%d.%d.%d.%d", VXGE_DRIVER_NAME, i,
 					VXGE_EPROM_IMG_MAJOR(img[i].version),
 					VXGE_EPROM_IMG_MINOR(img[i].version),
 					VXGE_EPROM_IMG_FIX(img[i].version),
@@ -4643,8 +4755,9 @@ _exit6:
 _exit5:
 	vxge_device_unregister(hldev);
 _exit4:
-	pci_disable_sriov(pdev);
+	pci_set_drvdata(pdev, NULL);
 	vxge_hw_device_terminate(hldev);
+	pci_disable_sriov(pdev);
 _exit3:
 	iounmap(attr.bar0);
 _exit2:
@@ -4655,7 +4768,7 @@ _exit0:
 	kfree(ll_config);
 	kfree(device_config);
 	driver_config->config_dev_cnt--;
-	pci_set_drvdata(pdev, NULL);
+	driver_config->total_dev_cnt--;
 	return ret;
 }
 
@@ -4668,45 +4781,34 @@ _exit0:
 static void __devexit vxge_remove(struct pci_dev *pdev)
 {
 	struct __vxge_hw_device *hldev;
-	struct vxgedev *vdev = NULL;
-	struct net_device *dev;
-	int i = 0;
+	struct vxgedev *vdev;
+	int i;
 
 	hldev = pci_get_drvdata(pdev);
-
 	if (hldev == NULL)
 		return;
 
-	dev = hldev->ndev;
-	vdev = netdev_priv(dev);
+	vdev = netdev_priv(hldev->ndev);
 
 	vxge_debug_entryexit(vdev->level_trace,	"%s:%d", __func__, __LINE__);
-
 	vxge_debug_init(vdev->level_trace, "%s : removing PCI device...",
 			__func__);
-	vxge_device_unregister(hldev);
 
-	for (i = 0; i < vdev->no_of_vpath; i++) {
+	for (i = 0; i < vdev->no_of_vpath; i++)
 		vxge_free_mac_add_list(&vdev->vpaths[i]);
-		vdev->vpaths[i].mcast_addr_cnt = 0;
-		vdev->vpaths[i].mac_addr_cnt = 0;
-	}
-
-	kfree(vdev->vpaths);
 
+	vxge_device_unregister(hldev);
+	pci_set_drvdata(pdev, NULL);
+	/* Do not call pci_disable_sriov here, as it will break child devices */
+	vxge_hw_device_terminate(hldev);
 	iounmap(vdev->bar0);
-
-	/* we are safe to free it now */
-	free_netdev(dev);
+	pci_release_region(pdev, 0);
+	pci_disable_device(pdev);
+	driver_config->config_dev_cnt--;
+	driver_config->total_dev_cnt--;
 
 	vxge_debug_init(vdev->level_trace, "%s:%d Device unregistered",
 			__func__, __LINE__);
-
-	vxge_hw_device_terminate(hldev);
-
-	pci_disable_device(pdev);
-	pci_release_region(pdev, 0);
-	pci_set_drvdata(pdev, NULL);
 	vxge_debug_entryexit(vdev->level_trace,	"%s:%d  Exiting...", __func__,
 			     __LINE__);
 }
diff --git a/drivers/net/vxge/vxge-main.h b/drivers/net/vxge/vxge-main.h
index 5746fedc356f..40474f0da576 100644
--- a/drivers/net/vxge/vxge-main.h
+++ b/drivers/net/vxge/vxge-main.h
@@ -59,11 +59,13 @@
 #define VXGE_TTI_LTIMER_VAL	1000
 #define VXGE_T1A_TTI_LTIMER_VAL	80
 #define VXGE_TTI_RTIMER_VAL	0
+#define VXGE_TTI_RTIMER_ADAPT_VAL	10
 #define VXGE_T1A_TTI_RTIMER_VAL	400
 #define VXGE_RTI_BTIMER_VAL	250
 #define VXGE_RTI_LTIMER_VAL	100
 #define VXGE_RTI_RTIMER_VAL	0
-#define VXGE_FIFO_INDICATE_MAX_PKTS VXGE_DEF_FIFO_LENGTH
+#define VXGE_RTI_RTIMER_ADAPT_VAL	15
+#define VXGE_FIFO_INDICATE_MAX_PKTS	VXGE_DEF_FIFO_LENGTH
 #define VXGE_ISR_POLLING_CNT 	8
 #define VXGE_MAX_CONFIG_DEV	0xFF
 #define VXGE_EXEC_MODE_DISABLE	0
@@ -107,6 +109,14 @@
 #define RTI_T1A_RX_UFC_C	50
 #define RTI_T1A_RX_UFC_D	60
 
+/*
+ * The interrupt rate is maintained at 3k per second with the moderation
+ * parameters for most traffic but not all. This is the maximum interrupt
+ * count allowed per function with INTA or per vector in the case of
+ * MSI-X in a 10 millisecond time period. Enabled only for Titan 1A.
+ */
+#define VXGE_T1A_MAX_INTERRUPT_COUNT	100
+#define VXGE_T1A_MAX_TX_INTERRUPT_COUNT	200
 
 /* Milli secs timer period */
 #define VXGE_TIMER_DELAY		10000
@@ -247,6 +257,11 @@ struct vxge_fifo {
 	int tx_steering_type;
 	int indicate_max_pkts;
 
+	/* Adaptive interrupt moderation parameters used in T1A */
+	unsigned long interrupt_count;
+	unsigned long jiffies;
+
+	u32 tx_vector_no;
 	/* Tx stats */
 	struct vxge_fifo_stats stats;
 } ____cacheline_aligned;
@@ -271,6 +286,10 @@ struct vxge_ring {
 	 */
 	int driver_id;
 
+	/* Adaptive interrupt moderation parameters used in T1A */
+	unsigned long interrupt_count;
+	unsigned long jiffies;
+
 	/* copy of the flag indicating whether rx_csum is to be used */
 	u32 rx_csum:1,
 	    rx_hwts:1;
@@ -286,7 +305,7 @@ struct vxge_ring {
 
 	int vlan_tag_strip;
 	struct vlan_group *vlgrp;
-	int rx_vector_no;
+	u32 rx_vector_no;
 	enum vxge_hw_status last_status;
 
 	/* Rx stats */
diff --git a/drivers/net/vxge/vxge-traffic.c b/drivers/net/vxge/vxge-traffic.c
index 4c10d6c4075f..8674f331311c 100644
--- a/drivers/net/vxge/vxge-traffic.c
+++ b/drivers/net/vxge/vxge-traffic.c
@@ -218,6 +218,68 @@ exit:
 	return status;
 }
 
+void vxge_hw_vpath_tti_ci_set(struct __vxge_hw_fifo *fifo)
+{
+	struct vxge_hw_vpath_reg __iomem *vp_reg;
+	struct vxge_hw_vp_config *config;
+	u64 val64;
+
+	if (fifo->config->enable != VXGE_HW_FIFO_ENABLE)
+		return;
+
+	vp_reg = fifo->vp_reg;
+	config = container_of(fifo->config, struct vxge_hw_vp_config, fifo);
+
+	if (config->tti.timer_ci_en != VXGE_HW_TIM_TIMER_CI_ENABLE) {
+		config->tti.timer_ci_en = VXGE_HW_TIM_TIMER_CI_ENABLE;
+		val64 = readq(&vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_TX]);
+		val64 |= VXGE_HW_TIM_CFG1_INT_NUM_TIMER_CI;
+		fifo->tim_tti_cfg1_saved = val64;
+		writeq(val64, &vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_TX]);
+	}
+}
+
+void vxge_hw_vpath_dynamic_rti_ci_set(struct __vxge_hw_ring *ring)
+{
+	u64 val64 = ring->tim_rti_cfg1_saved;
+
+	val64 |= VXGE_HW_TIM_CFG1_INT_NUM_TIMER_CI;
+	ring->tim_rti_cfg1_saved = val64;
+	writeq(val64, &ring->vp_reg->tim_cfg1_int_num[VXGE_HW_VPATH_INTR_RX]);
+}
+
+void vxge_hw_vpath_dynamic_tti_rtimer_set(struct __vxge_hw_fifo *fifo)
+{
+	u64 val64 = fifo->tim_tti_cfg3_saved;
+	u64 timer = (fifo->rtimer * 1000) / 272;
+
+	val64 &= ~VXGE_HW_TIM_CFG3_INT_NUM_RTIMER_VAL(0x3ffffff);
+	if (timer)
+		val64 |= VXGE_HW_TIM_CFG3_INT_NUM_RTIMER_VAL(timer) |
+			VXGE_HW_TIM_CFG3_INT_NUM_RTIMER_EVENT_SF(5);
+
+	writeq(val64, &fifo->vp_reg->tim_cfg3_int_num[VXGE_HW_VPATH_INTR_TX]);
+	/* tti_cfg3_saved is not updated again because it is
+	 * initialized at one place only - init time.
+	 */
+}
+
+void vxge_hw_vpath_dynamic_rti_rtimer_set(struct __vxge_hw_ring *ring)
+{
+	u64 val64 = ring->tim_rti_cfg3_saved;
+	u64 timer = (ring->rtimer * 1000) / 272;
+
+	val64 &= ~VXGE_HW_TIM_CFG3_INT_NUM_RTIMER_VAL(0x3ffffff);
+	if (timer)
+		val64 |= VXGE_HW_TIM_CFG3_INT_NUM_RTIMER_VAL(timer) |
+			VXGE_HW_TIM_CFG3_INT_NUM_RTIMER_EVENT_SF(4);
+
+	writeq(val64, &ring->vp_reg->tim_cfg3_int_num[VXGE_HW_VPATH_INTR_RX]);
+	/* rti_cfg3_saved is not updated again because it is
+	 * initialized at one place only - init time.
+	 */
+}
+
 /**
  * vxge_hw_channel_msix_mask - Mask MSIX Vector.
  * @channeh: Channel for rx or tx handle
@@ -254,6 +316,23 @@ vxge_hw_channel_msix_unmask(struct __vxge_hw_channel *channel, int msix_id)
 }
 
 /**
+ * vxge_hw_channel_msix_clear - Unmask the MSIX Vector.
+ * @channel: Channel for rx or tx handle
+ * @msix_id:  MSI ID
+ *
+ * The function unmasks the msix interrupt for the given msix_id
+ * if configured in MSIX oneshot mode
+ *
+ * Returns: 0
+ */
+void vxge_hw_channel_msix_clear(struct __vxge_hw_channel *channel, int msix_id)
+{
+	__vxge_hw_pio_mem_write32_upper(
+		(u32) vxge_bVALn(vxge_mBIT(msix_id >> 2), 0, 32),
+		&channel->common_reg->clr_msix_one_shot_vec[msix_id % 4]);
+}
+
+/**
  * vxge_hw_device_set_intr_type - Updates the configuration
  *		with new interrupt type.
  * @hldev: HW device handle.
@@ -2191,19 +2270,14 @@ vxge_hw_vpath_msix_set(struct __vxge_hw_vpath_handle *vp, int *tim_msix_id,
 	if (vpath->hldev->config.intr_mode ==
 					VXGE_HW_INTR_MODE_MSIX_ONE_SHOT) {
 		__vxge_hw_pio_mem_write32_upper((u32)vxge_bVALn(
+				VXGE_HW_ONE_SHOT_VECT0_EN_ONE_SHOT_VECT0_EN,
+				0, 32), &vp_reg->one_shot_vect0_en);
+		__vxge_hw_pio_mem_write32_upper((u32)vxge_bVALn(
 				VXGE_HW_ONE_SHOT_VECT1_EN_ONE_SHOT_VECT1_EN,
 				0, 32), &vp_reg->one_shot_vect1_en);
-	}
-
-	if (vpath->hldev->config.intr_mode ==
-		VXGE_HW_INTR_MODE_MSIX_ONE_SHOT) {
 		__vxge_hw_pio_mem_write32_upper((u32)vxge_bVALn(
 				VXGE_HW_ONE_SHOT_VECT2_EN_ONE_SHOT_VECT2_EN,
 				0, 32), &vp_reg->one_shot_vect2_en);
-
-		__vxge_hw_pio_mem_write32_upper((u32)vxge_bVALn(
-				VXGE_HW_ONE_SHOT_VECT3_EN_ONE_SHOT_VECT3_EN,
-				0, 32), &vp_reg->one_shot_vect3_en);
 	}
 }
 
@@ -2229,6 +2303,32 @@ vxge_hw_vpath_msix_mask(struct __vxge_hw_vpath_handle *vp, int msix_id)
 }
 
 /**
+ * vxge_hw_vpath_msix_clear - Clear MSIX Vector.
+ * @vp: Virtual Path handle.
+ * @msix_id:  MSI ID
+ *
+ * The function clears the msix interrupt for the given msix_id
+ *
+ * Returns: 0,
+ * Otherwise, VXGE_HW_ERR_WRONG_IRQ if the msix index is out of range
+ * status.
+ * See also:
+ */
+void vxge_hw_vpath_msix_clear(struct __vxge_hw_vpath_handle *vp, int msix_id)
+{
+	struct __vxge_hw_device *hldev = vp->vpath->hldev;
+
+	if ((hldev->config.intr_mode == VXGE_HW_INTR_MODE_MSIX_ONE_SHOT))
+		__vxge_hw_pio_mem_write32_upper(
+			(u32) vxge_bVALn(vxge_mBIT((msix_id >> 2)), 0, 32),
+			&hldev->common_reg->clr_msix_one_shot_vec[msix_id % 4]);
+	else
+		__vxge_hw_pio_mem_write32_upper(
+			(u32) vxge_bVALn(vxge_mBIT((msix_id >> 2)), 0, 32),
+			&hldev->common_reg->clear_msix_mask_vect[msix_id % 4]);
+}
+
+/**
  * vxge_hw_vpath_msix_unmask - Unmask the MSIX Vector.
  * @vp: Virtual Path handle.
  * @msix_id:  MSI ID
diff --git a/drivers/net/vxge/vxge-traffic.h b/drivers/net/vxge/vxge-traffic.h
index d48486d6afa1..9d9dfda4c7ab 100644
--- a/drivers/net/vxge/vxge-traffic.h
+++ b/drivers/net/vxge/vxge-traffic.h
@@ -2142,6 +2142,10 @@ void vxge_hw_device_clear_tx_rx(
  *  Virtual Paths
  */
 
+void vxge_hw_vpath_dynamic_rti_rtimer_set(struct __vxge_hw_ring *ring);
+
+void vxge_hw_vpath_dynamic_tti_rtimer_set(struct __vxge_hw_fifo *fifo);
+
 u32 vxge_hw_vpath_id(
 	struct __vxge_hw_vpath_handle *vpath_handle);
 
@@ -2245,6 +2249,8 @@ void
 vxge_hw_vpath_msix_mask(struct __vxge_hw_vpath_handle *vpath_handle,
 			int msix_id);
 
+void vxge_hw_vpath_msix_clear(struct __vxge_hw_vpath_handle *vp, int msix_id);
+
 void vxge_hw_device_flush_io(struct __vxge_hw_device *devh);
 
 void
@@ -2270,6 +2276,9 @@ void
 vxge_hw_channel_msix_unmask(struct __vxge_hw_channel *channelh, int msix_id);
 
 void
+vxge_hw_channel_msix_clear(struct __vxge_hw_channel *channelh, int msix_id);
+
+void
 vxge_hw_channel_dtr_try_complete(struct __vxge_hw_channel *channel,
 				 void **dtrh);
 
@@ -2282,7 +2291,8 @@ vxge_hw_channel_dtr_free(struct __vxge_hw_channel *channel, void *dtrh);
 int
 vxge_hw_channel_dtr_count(struct __vxge_hw_channel *channel);
 
-void
-vxge_hw_vpath_tti_ci_set(struct __vxge_hw_device *hldev, u32 vp_id);
+void vxge_hw_vpath_tti_ci_set(struct __vxge_hw_fifo *fifo);
+
+void vxge_hw_vpath_dynamic_rti_ci_set(struct __vxge_hw_ring *ring);
 
 #endif
diff --git a/drivers/net/vxge/vxge-version.h b/drivers/net/vxge/vxge-version.h
index ad2f99b9bcf3..581e21525e85 100644
--- a/drivers/net/vxge/vxge-version.h
+++ b/drivers/net/vxge/vxge-version.h
@@ -16,8 +16,8 @@
 
 #define VXGE_VERSION_MAJOR	"2"
 #define VXGE_VERSION_MINOR	"5"
-#define VXGE_VERSION_FIX	"1"
-#define VXGE_VERSION_BUILD	"22082"
+#define VXGE_VERSION_FIX	"2"
+#define VXGE_VERSION_BUILD	"22259"
 #define VXGE_VERSION_FOR	"k"
 
 #define VXGE_FW_VER(maj, min, bld) (((maj) << 16) + ((min) << 8) + (bld))
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index b4338f389394..7aeb113cbb90 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -274,6 +274,7 @@ source "drivers/net/wireless/b43legacy/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/iwmc3200wifi/Kconfig"
 source "drivers/net/wireless/libertas/Kconfig"
 source "drivers/net/wireless/orinoco/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 9760561a27a5..ddd3fb6ba1d3 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -24,7 +24,7 @@ obj-$(CONFIG_B43LEGACY)		+= b43legacy/
 obj-$(CONFIG_ZD1211RW)		+= zd1211rw/
 obj-$(CONFIG_RTL8180)		+= rtl818x/
 obj-$(CONFIG_RTL8187)		+= rtl818x/
-obj-$(CONFIG_RTL8192CE)		+= rtlwifi/
+obj-$(CONFIG_RTLWIFI)		+= rtlwifi/
 
 # 16-bit wireless PCMCIA client drivers
 obj-$(CONFIG_PCMCIA_RAYCS)	+= ray_cs.o
@@ -41,7 +41,8 @@ obj-$(CONFIG_ADM8211)	+= adm8211.o
 
 obj-$(CONFIG_MWL8K)	+= mwl8k.o
 
-obj-$(CONFIG_IWLWIFI)	+= iwlwifi/
+obj-$(CONFIG_IWLAGN)	+= iwlwifi/
+obj-$(CONFIG_IWLWIFI_LEGACY)	+= iwlegacy/
 obj-$(CONFIG_RT2X00)	+= rt2x00/
 
 obj-$(CONFIG_P54_COMMON)	+= p54/
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index f9aa1bc0a947..afe2cbc6cb24 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -1658,7 +1658,7 @@ static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
 }
 
 /* Put adm8211_tx_hdr on skb and transmit */
-static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+static void adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct adm8211_tx_hdr *txhdr;
 	size_t payload_len, hdrlen;
@@ -1707,8 +1707,6 @@ static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	txhdr->retry_limit = info->control.rates[0].count;
 
 	adm8211_tx_raw(dev, skb, plcp_signal, hdrlen);
-
-	return NETDEV_TX_OK;
 }
 
 static int adm8211_alloc_rings(struct ieee80211_hw *dev)
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/at76c50x-usb.c
index 1476314afa8a..298601436ee2 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/at76c50x-usb.c
@@ -1728,7 +1728,7 @@ static void at76_mac80211_tx_callback(struct urb *urb)
 	ieee80211_wake_queues(priv->hw);
 }
 
-static int at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct at76_priv *priv = hw->priv;
 	struct at76_tx_buffer *tx_buffer = priv->bulk_out_buffer;
@@ -1741,7 +1741,8 @@ static int at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	if (priv->tx_urb->status == -EINPROGRESS) {
 		wiphy_err(priv->hw->wiphy,
 			  "%s called while tx urb is pending\n", __func__);
-		return NETDEV_TX_BUSY;
+		dev_kfree_skb_any(skb);
+		return;
 	}
 
 	/* The following code lines are important when the device is going to
@@ -1755,7 +1756,8 @@ static int at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		if (compare_ether_addr(priv->bssid, mgmt->bssid)) {
 			memcpy(priv->bssid, mgmt->bssid, ETH_ALEN);
 			ieee80211_queue_work(hw, &priv->work_join_bssid);
-			return NETDEV_TX_BUSY;
+			dev_kfree_skb_any(skb);
+			return;
 		}
 	}
 
@@ -1795,8 +1797,6 @@ static int at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 				  priv->tx_urb,
 				  priv->tx_urb->hcpriv, priv->tx_urb->complete);
 	}
-
-	return 0;
 }
 
 static int at76_mac80211_start(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/ath/ar9170/Kconfig b/drivers/net/wireless/ath/ar9170/Kconfig
index d7a4799d20fb..7b9672b0d090 100644
--- a/drivers/net/wireless/ath/ar9170/Kconfig
+++ b/drivers/net/wireless/ath/ar9170/Kconfig
@@ -1,8 +1,10 @@
 config AR9170_USB
-	tristate "Atheros AR9170 802.11n USB support"
+	tristate "Atheros AR9170 802.11n USB support (OBSOLETE)"
 	depends on USB && MAC80211
 	select FW_LOADER
 	help
+	  This driver is going to get replaced by carl9170.
+
 	  This is a driver for the Atheros "otus" 802.11n USB devices.
 
 	  These devices require additional firmware (2 files).
diff --git a/drivers/net/wireless/ath/ar9170/ar9170.h b/drivers/net/wireless/ath/ar9170/ar9170.h
index 4f845f80c098..371e4ce49528 100644
--- a/drivers/net/wireless/ath/ar9170/ar9170.h
+++ b/drivers/net/wireless/ath/ar9170/ar9170.h
@@ -224,7 +224,7 @@ void ar9170_handle_command_response(struct ar9170 *ar, void *buf, u32 len);
 int ar9170_nag_limiter(struct ar9170 *ar);
 
 /* MAC */
-int ar9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
+void ar9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 int ar9170_init_mac(struct ar9170 *ar);
 int ar9170_set_qos(struct ar9170 *ar);
 int ar9170_update_multicast(struct ar9170 *ar, const u64 mc_hast);
diff --git a/drivers/net/wireless/ath/ar9170/main.c b/drivers/net/wireless/ath/ar9170/main.c
index 32bf79e6a320..b761fec0d721 100644
--- a/drivers/net/wireless/ath/ar9170/main.c
+++ b/drivers/net/wireless/ath/ar9170/main.c
@@ -1475,7 +1475,7 @@ static void ar9170_tx(struct ar9170 *ar)
 				     msecs_to_jiffies(AR9170_JANITOR_DELAY));
 }
 
-int ar9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+void ar9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ar9170 *ar = hw->priv;
 	struct ieee80211_tx_info *info;
@@ -1493,11 +1493,10 @@ int ar9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	skb_queue_tail(&ar->tx_pending[queue], skb);
 
 	ar9170_tx(ar);
-	return NETDEV_TX_OK;
+	return;
 
 err_free:
 	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
 }
 
 static int ar9170_op_add_interface(struct ieee80211_hw *hw,
@@ -1945,7 +1944,8 @@ static int ar9170_conf_tx(struct ieee80211_hw *hw, u16 queue,
 static int ar9170_ampdu_action(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif,
 			       enum ieee80211_ampdu_mlme_action action,
-			       struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+			       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			       u8 buf_size)
 {
 	switch (action) {
 	case IEEE80211_AMPDU_RX_START:
diff --git a/drivers/net/wireless/ath/ath.h b/drivers/net/wireless/ath/ath.h
index e43210c8585c..a6c6a466000f 100644
--- a/drivers/net/wireless/ath/ath.h
+++ b/drivers/net/wireless/ath/ath.h
@@ -108,12 +108,14 @@ enum ath_cipher {
  * struct ath_ops - Register read/write operations
  *
  * @read: Register read
+ * @multi_read: Multiple register read
  * @write: Register write
  * @enable_write_buffer: Enable multiple register writes
  * @write_flush: flush buffered register writes and disable buffering
  */
 struct ath_ops {
 	unsigned int (*read)(void *, u32 reg_offset);
+	void (*multi_read)(void *, u32 *addr, u32 *val, u16 count);
 	void (*write)(void *, u32 val, u32 reg_offset);
 	void (*enable_write_buffer)(void *);
 	void (*write_flush) (void *);
diff --git a/drivers/net/wireless/ath/ath5k/Kconfig b/drivers/net/wireless/ath/ath5k/Kconfig
index e0793319389d..e18a9aa7b6ca 100644
--- a/drivers/net/wireless/ath/ath5k/Kconfig
+++ b/drivers/net/wireless/ath/ath5k/Kconfig
@@ -40,6 +40,17 @@ config ATH5K_DEBUG
 
 	  modprobe ath5k debug=0x00000400
 
+config ATH5K_TRACER
+	bool "Atheros 5xxx tracer"
+	depends on ATH5K
+	depends on EVENT_TRACING
+	---help---
+	  Say Y here to enable tracepoints for the ath5k driver
+	  using the kernel tracing infrastructure.  Select this
+	  option if you are interested in debugging the driver.
+
+	  If unsure, say N.
+
 config ATH5K_AHB
 	bool "Atheros 5xxx AHB bus support"
 	depends on (ATHEROS_AR231X && !PCI)
diff --git a/drivers/net/wireless/ath/ath5k/ahb.c b/drivers/net/wireless/ath/ath5k/ahb.c
index 707cde149248..82324e98efef 100644
--- a/drivers/net/wireless/ath/ath5k/ahb.c
+++ b/drivers/net/wireless/ath/ath5k/ahb.c
@@ -31,7 +31,8 @@ static void ath5k_ahb_read_cachesize(struct ath_common *common, int *csz)
 	*csz = L1_CACHE_BYTES >> 2;
 }
 
-bool ath5k_ahb_eeprom_read(struct ath_common *common, u32 off, u16 *data)
+static bool
+ath5k_ahb_eeprom_read(struct ath_common *common, u32 off, u16 *data)
 {
 	struct ath5k_softc *sc = common->priv;
 	struct platform_device *pdev = to_platform_device(sc->dev);
@@ -46,10 +47,10 @@ bool ath5k_ahb_eeprom_read(struct ath_common *common, u32 off, u16 *data)
 
 	eeprom += off;
 	if (eeprom > eeprom_end)
-		return -EINVAL;
+		return false;
 
 	*data = *eeprom;
-	return 0;
+	return true;
 }
 
 int ath5k_hw_read_srev(struct ath5k_hw *ah)
@@ -92,7 +93,7 @@ static int ath_ahb_probe(struct platform_device *pdev)
 		goto err_out;
 	}
 
-	mem = ioremap_nocache(res->start, res->end - res->start + 1);
+	mem = ioremap_nocache(res->start, resource_size(res));
 	if (mem == NULL) {
 		dev_err(&pdev->dev, "ioremap failed\n");
 		ret = -ENOMEM;
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index 407e39c2b10b..0ee54eb333de 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -210,14 +210,9 @@
 /* Initial values */
 #define	AR5K_INIT_CYCRSSI_THR1			2
 
-/* Tx retry limits */
-#define AR5K_INIT_SH_RETRY			10
-#define AR5K_INIT_LG_RETRY			AR5K_INIT_SH_RETRY
-/* For station mode */
-#define AR5K_INIT_SSH_RETRY			32
-#define AR5K_INIT_SLG_RETRY			AR5K_INIT_SSH_RETRY
-#define AR5K_INIT_TX_RETRY			10
-
+/* Tx retry limit defaults from standard */
+#define AR5K_INIT_RETRY_SHORT			7
+#define AR5K_INIT_RETRY_LONG			4
 
 /* Slot time */
 #define AR5K_INIT_SLOT_TIME_TURBO		6
@@ -1057,7 +1052,9 @@ struct ath5k_hw {
 #define ah_modes		ah_capabilities.cap_mode
 #define ah_ee_version		ah_capabilities.cap_eeprom.ee_version
 
-	u32			ah_limit_tx_retries;
+	u8			ah_retry_long;
+	u8			ah_retry_short;
+
 	u8			ah_coverage_class;
 	bool			ah_ack_bitrate_high;
 	u8			ah_bwmode;
@@ -1067,7 +1064,6 @@ struct ath5k_hw {
 	u8			ah_ant_mode;
 	u8			ah_tx_ant;
 	u8			ah_def_ant;
-	bool			ah_software_retry;
 
 	struct ath5k_capabilities ah_capabilities;
 
@@ -1162,6 +1158,26 @@ void ath5k_hw_deinit(struct ath5k_hw *ah);
 int ath5k_sysfs_register(struct ath5k_softc *sc);
 void ath5k_sysfs_unregister(struct ath5k_softc *sc);
 
+/* base.c */
+struct ath5k_buf;
+struct ath5k_txq;
+
+void set_beacon_filter(struct ieee80211_hw *hw, bool enable);
+bool ath_any_vif_assoc(struct ath5k_softc *sc);
+void ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
+		    struct ath5k_txq *txq);
+int ath5k_init_hw(struct ath5k_softc *sc);
+int ath5k_stop_hw(struct ath5k_softc *sc);
+void ath5k_mode_setup(struct ath5k_softc *sc, struct ieee80211_vif *vif);
+void ath5k_update_bssid_mask_and_opmode(struct ath5k_softc *sc,
+					struct ieee80211_vif *vif);
+int ath5k_chan_set(struct ath5k_softc *sc, struct ieee80211_channel *chan);
+void ath5k_beacon_update_timers(struct ath5k_softc *sc, u64 bc_tsf);
+int ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
+void ath5k_beacon_config(struct ath5k_softc *sc);
+void ath5k_txbuf_free_skb(struct ath5k_softc *sc, struct ath5k_buf *bf);
+void ath5k_rxbuf_free_skb(struct ath5k_softc *sc, struct ath5k_buf *bf);
+
 /*Chip id helper functions */
 const char *ath5k_chip_name(enum ath5k_srev_type type, u_int16_t val);
 int ath5k_hw_read_srev(struct ath5k_hw *ah);
@@ -1250,6 +1266,8 @@ int ath5k_hw_set_tx_queueprops(struct ath5k_hw *ah, int queue,
 int ath5k_hw_setup_tx_queue(struct ath5k_hw *ah,
 			    enum ath5k_tx_queue queue_type,
 			    struct ath5k_txq_info *queue_info);
+void ath5k_hw_set_tx_retry_limits(struct ath5k_hw *ah,
+				  unsigned int queue);
 u32 ath5k_hw_num_tx_pending(struct ath5k_hw *ah, unsigned int queue);
 void ath5k_hw_release_tx_queue(struct ath5k_hw *ah, unsigned int queue);
 int ath5k_hw_reset_tx_queue(struct ath5k_hw *ah, unsigned int queue);
diff --git a/drivers/net/wireless/ath/ath5k/attach.c b/drivers/net/wireless/ath/ath5k/attach.c
index cdac5cff0177..bc8240560488 100644
--- a/drivers/net/wireless/ath/ath5k/attach.c
+++ b/drivers/net/wireless/ath/ath5k/attach.c
@@ -118,8 +118,8 @@ int ath5k_hw_init(struct ath5k_softc *sc)
 	ah->ah_bwmode = AR5K_BWMODE_DEFAULT;
 	ah->ah_txpower.txp_tpc = AR5K_TUNE_TPC_TXPOWER;
 	ah->ah_imr = 0;
-	ah->ah_limit_tx_retries = AR5K_INIT_TX_RETRY;
-	ah->ah_software_retry = false;
+	ah->ah_retry_short = AR5K_INIT_RETRY_SHORT;
+	ah->ah_retry_long = AR5K_INIT_RETRY_LONG;
 	ah->ah_ant_mode = AR5K_ANTMODE_DEFAULT;
 	ah->ah_noise_floor = -95;	/* until first NF calibration is run */
 	sc->ani_state.ani_mode = ATH5K_ANI_MODE_AUTO;
@@ -220,7 +220,8 @@ int ath5k_hw_init(struct ath5k_softc *sc)
 			ah->ah_radio = AR5K_RF5112;
 			ah->ah_single_chip = false;
 			ah->ah_radio_5ghz_revision = AR5K_SREV_RAD_5112B;
-		} else if (ah->ah_mac_version == (AR5K_SREV_AR2415 >> 4)) {
+		} else if (ah->ah_mac_version == (AR5K_SREV_AR2415 >> 4) ||
+			ah->ah_mac_version == (AR5K_SREV_AR2315_R6 >> 4)) {
 			ah->ah_radio = AR5K_RF2316;
 			ah->ah_single_chip = true;
 			ah->ah_radio_5ghz_revision = AR5K_SREV_RAD_2316;
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 09ae4ef0fd51..e6ff62e60a79 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -61,6 +61,9 @@
 #include "debug.h"
 #include "ani.h"
 
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+
 int ath5k_modparam_nohwcrypt;
 module_param_named(nohwcrypt, ath5k_modparam_nohwcrypt, bool, S_IRUGO);
 MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
@@ -242,73 +245,68 @@ static int ath5k_reg_notifier(struct wiphy *wiphy, struct regulatory_request *re
 \********************/
 
 /*
- * Convert IEEE channel number to MHz frequency.
- */
-static inline short
-ath5k_ieee2mhz(short chan)
-{
-	if (chan <= 14 || chan >= 27)
-		return ieee80211chan2mhz(chan);
-	else
-		return 2212 + chan * 20;
-}
-
-/*
  * Returns true for the channel numbers used without all_channels modparam.
  */
-static bool ath5k_is_standard_channel(short chan)
+static bool ath5k_is_standard_channel(short chan, enum ieee80211_band band)
 {
-	return ((chan <= 14) ||
-		/* UNII 1,2 */
-		((chan & 3) == 0 && chan >= 36 && chan <= 64) ||
+	if (band == IEEE80211_BAND_2GHZ && chan <= 14)
+		return true;
+
+	return	/* UNII 1,2 */
+		(((chan & 3) == 0 && chan >= 36 && chan <= 64) ||
 		/* midband */
 		((chan & 3) == 0 && chan >= 100 && chan <= 140) ||
 		/* UNII-3 */
-		((chan & 3) == 1 && chan >= 149 && chan <= 165));
+		((chan & 3) == 1 && chan >= 149 && chan <= 165) ||
+		/* 802.11j 5.030-5.080 GHz (20MHz) */
+		(chan == 8 || chan == 12 || chan == 16) ||
+		/* 802.11j 4.9GHz (20MHz) */
+		(chan == 184 || chan == 188 || chan == 192 || chan == 196));
 }
 
 static unsigned int
-ath5k_copy_channels(struct ath5k_hw *ah,
-		struct ieee80211_channel *channels,
-		unsigned int mode,
-		unsigned int max)
+ath5k_setup_channels(struct ath5k_hw *ah, struct ieee80211_channel *channels,
+		unsigned int mode, unsigned int max)
 {
-	unsigned int i, count, size, chfreq, freq, ch;
-
-	if (!test_bit(mode, ah->ah_modes))
-		return 0;
+	unsigned int count, size, chfreq, freq, ch;
+	enum ieee80211_band band;
 
 	switch (mode) {
 	case AR5K_MODE_11A:
 		/* 1..220, but 2GHz frequencies are filtered by check_channel */
-		size = 220 ;
+		size = 220;
 		chfreq = CHANNEL_5GHZ;
+		band = IEEE80211_BAND_5GHZ;
 		break;
 	case AR5K_MODE_11B:
 	case AR5K_MODE_11G:
 		size = 26;
 		chfreq = CHANNEL_2GHZ;
+		band = IEEE80211_BAND_2GHZ;
 		break;
 	default:
 		ATH5K_WARN(ah->ah_sc, "bad mode, not copying channels\n");
 		return 0;
 	}
 
-	for (i = 0, count = 0; i < size && max > 0; i++) {
-		ch = i + 1 ;
-		freq = ath5k_ieee2mhz(ch);
+	count = 0;
+	for (ch = 1; ch <= size && count < max; ch++) {
+		freq = ieee80211_channel_to_frequency(ch, band);
+
+		if (freq == 0) /* mapping failed - not a standard channel */
+			continue;
 
 		/* Check if channel is supported by the chipset */
 		if (!ath5k_channel_ok(ah, freq, chfreq))
 			continue;
 
-		if (!modparam_all_channels && !ath5k_is_standard_channel(ch))
+		if (!modparam_all_channels &&
+		    !ath5k_is_standard_channel(ch, band))
 			continue;
 
 		/* Write channel info and increment counter */
 		channels[count].center_freq = freq;
-		channels[count].band = (chfreq == CHANNEL_2GHZ) ?
-			IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+		channels[count].band = band;
 		switch (mode) {
 		case AR5K_MODE_11A:
 		case AR5K_MODE_11G:
@@ -319,7 +317,6 @@ ath5k_copy_channels(struct ath5k_hw *ah,
 		}
 
 		count++;
-		max--;
 	}
 
 	return count;
@@ -364,7 +361,7 @@ ath5k_setup_bands(struct ieee80211_hw *hw)
 		sband->n_bitrates = 12;
 
 		sband->channels = sc->channels;
-		sband->n_channels = ath5k_copy_channels(ah, sband->channels,
+		sband->n_channels = ath5k_setup_channels(ah, sband->channels,
 					AR5K_MODE_11G, max_c);
 
 		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
@@ -390,7 +387,7 @@ ath5k_setup_bands(struct ieee80211_hw *hw)
 		}
 
 		sband->channels = sc->channels;
-		sband->n_channels = ath5k_copy_channels(ah, sband->channels,
+		sband->n_channels = ath5k_setup_channels(ah, sband->channels,
 					AR5K_MODE_11B, max_c);
 
 		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
@@ -410,7 +407,7 @@ ath5k_setup_bands(struct ieee80211_hw *hw)
 		sband->n_bitrates = 8;
 
 		sband->channels = &sc->channels[count_c];
-		sband->n_channels = ath5k_copy_channels(ah, sband->channels,
+		sband->n_channels = ath5k_setup_channels(ah, sband->channels,
 					AR5K_MODE_11A, max_c);
 
 		hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
@@ -445,31 +442,9 @@ ath5k_chan_set(struct ath5k_softc *sc, struct ieee80211_channel *chan)
 	return ath5k_reset(sc, chan, true);
 }
 
-static void
-ath5k_setcurmode(struct ath5k_softc *sc, unsigned int mode)
-{
-	sc->curmode = mode;
-
-	if (mode == AR5K_MODE_11A) {
-		sc->curband = &sc->sbands[IEEE80211_BAND_5GHZ];
-	} else {
-		sc->curband = &sc->sbands[IEEE80211_BAND_2GHZ];
-	}
-}
-
-struct ath_vif_iter_data {
-	const u8	*hw_macaddr;
-	u8		mask[ETH_ALEN];
-	u8		active_mac[ETH_ALEN]; /* first active MAC */
-	bool		need_set_hw_addr;
-	bool		found_active;
-	bool		any_assoc;
-	enum nl80211_iftype opmode;
-};
-
-static void ath_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+void ath5k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 {
-	struct ath_vif_iter_data *iter_data = data;
+	struct ath5k_vif_iter_data *iter_data = data;
 	int i;
 	struct ath5k_vif *avf = (void *)vif->drv_priv;
 
@@ -499,9 +474,12 @@ static void ath_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 	 */
 	if (avf->opmode == NL80211_IFTYPE_AP)
 		iter_data->opmode = NL80211_IFTYPE_AP;
-	else
+	else {
+		if (avf->opmode == NL80211_IFTYPE_STATION)
+			iter_data->n_stas++;
 		if (iter_data->opmode == NL80211_IFTYPE_UNSPECIFIED)
 			iter_data->opmode = avf->opmode;
+	}
 }
 
 void
@@ -509,7 +487,8 @@ ath5k_update_bssid_mask_and_opmode(struct ath5k_softc *sc,
 				   struct ieee80211_vif *vif)
 {
 	struct ath_common *common = ath5k_hw_common(sc->ah);
-	struct ath_vif_iter_data iter_data;
+	struct ath5k_vif_iter_data iter_data;
+	u32 rfilt;
 
 	/*
 	 * Use the hardware MAC address as reference, the hardware uses it
@@ -520,12 +499,13 @@ ath5k_update_bssid_mask_and_opmode(struct ath5k_softc *sc,
 	iter_data.found_active = false;
 	iter_data.need_set_hw_addr = true;
 	iter_data.opmode = NL80211_IFTYPE_UNSPECIFIED;
+	iter_data.n_stas = 0;
 
 	if (vif)
-		ath_vif_iter(&iter_data, vif->addr, vif);
+		ath5k_vif_iter(&iter_data, vif->addr, vif);
 
 	/* Get list of all active MAC addresses */
-	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath_vif_iter,
+	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath5k_vif_iter,
 						   &iter_data);
 	memcpy(sc->bssidmask, iter_data.mask, ETH_ALEN);
 
@@ -543,20 +523,19 @@ ath5k_update_bssid_mask_and_opmode(struct ath5k_softc *sc,
 
 	if (ath5k_hw_hasbssidmask(sc->ah))
 		ath5k_hw_set_bssid_mask(sc->ah, sc->bssidmask);
-}
 
-void
-ath5k_mode_setup(struct ath5k_softc *sc, struct ieee80211_vif *vif)
-{
-	struct ath5k_hw *ah = sc->ah;
-	u32 rfilt;
+	/* Set up RX Filter */
+	if (iter_data.n_stas > 1) {
+		/* If you have multiple STA interfaces connected to
+		 * different APs, ARPs are not received (most of the time?)
+		 * Enabling PROMISC appears to fix that probem.
+		 */
+		sc->filter_flags |= AR5K_RX_FILTER_PROM;
+	}
 
-	/* configure rx filter */
 	rfilt = sc->filter_flags;
-	ath5k_hw_set_rx_filter(ah, rfilt);
+	ath5k_hw_set_rx_filter(sc->ah, rfilt);
 	ATH5K_DBG(sc, ATH5K_DEBUG_MODE, "RX filter 0x%x\n", rfilt);
-
-	ath5k_update_bssid_mask_and_opmode(sc, vif);
 }
 
 static inline int
@@ -569,7 +548,7 @@ ath5k_hw_to_driver_rix(struct ath5k_softc *sc, int hw_rix)
 			"hw_rix out of bounds: %x\n", hw_rix))
 		return 0;
 
-	rix = sc->rate_idx[sc->curband->band][hw_rix];
+	rix = sc->rate_idx[sc->curchan->band][hw_rix];
 	if (WARN(rix < 0, "invalid hw_rix: %x\n", hw_rix))
 		rix = 0;
 
@@ -1132,7 +1111,7 @@ ath5k_rx_start(struct ath5k_softc *sc)
 	spin_unlock_bh(&sc->rxbuflock);
 
 	ath5k_hw_start_rx_dma(ah);	/* enable recv descriptors */
-	ath5k_mode_setup(sc, NULL);		/* set filters, etc. */
+	ath5k_update_bssid_mask_and_opmode(sc, NULL); /* set filters, etc. */
 	ath5k_hw_start_rx_pcu(ah);	/* re-enable PCU/DMA engine */
 
 	return 0;
@@ -1376,10 +1355,10 @@ ath5k_receive_frame(struct ath5k_softc *sc, struct sk_buff *skb,
 	 * right now, so it's not too bad...
 	 */
 	rxs->mactime = ath5k_extend_tsf(sc->ah, rs->rs_tstamp);
-	rxs->flag |= RX_FLAG_TSFT;
+	rxs->flag |= RX_FLAG_MACTIME_MPDU;
 
 	rxs->freq = sc->curchan->center_freq;
-	rxs->band = sc->curband->band;
+	rxs->band = sc->curchan->band;
 
 	rxs->signal = sc->ah->ah_noise_floor + rs->rs_rssi;
 
@@ -1394,10 +1373,10 @@ ath5k_receive_frame(struct ath5k_softc *sc, struct sk_buff *skb,
 	rxs->flag |= ath5k_rx_decrypted(sc, skb, rs);
 
 	if (rxs->rate_idx >= 0 && rs->rs_rate ==
-	    sc->curband->bitrates[rxs->rate_idx].hw_value_short)
+	    sc->sbands[sc->curchan->band].bitrates[rxs->rate_idx].hw_value_short)
 		rxs->flag |= RX_FLAG_SHORTPRE;
 
-	ath5k_debug_dump_skb(sc, skb, "RX  ", 0);
+	trace_ath5k_rx(sc, skb);
 
 	ath5k_update_beacon_rssi(sc, skb, rs->rs_rssi);
 
@@ -1533,7 +1512,7 @@ unlock:
 * TX Handling *
 \*************/
 
-int
+void
 ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
 	       struct ath5k_txq *txq)
 {
@@ -1542,7 +1521,7 @@ ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
 	unsigned long flags;
 	int padsize;
 
-	ath5k_debug_dump_skb(sc, skb, "TX  ", 1);
+	trace_ath5k_tx(sc, skb, txq);
 
 	/*
 	 * The hardware expects the header padded to 4 byte boundaries.
@@ -1582,16 +1561,15 @@ ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
 		spin_unlock_irqrestore(&sc->txbuflock, flags);
 		goto drop_packet;
 	}
-	return NETDEV_TX_OK;
+	return;
 
 drop_packet:
 	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
 }
 
 static void
 ath5k_tx_frame_completed(struct ath5k_softc *sc, struct sk_buff *skb,
-			 struct ath5k_tx_status *ts)
+			 struct ath5k_txq *txq, struct ath5k_tx_status *ts)
 {
 	struct ieee80211_tx_info *info;
 	int i;
@@ -1643,6 +1621,7 @@ ath5k_tx_frame_completed(struct ath5k_softc *sc, struct sk_buff *skb,
 	else
 		sc->stats.antenna_tx[0]++; /* invalid */
 
+	trace_ath5k_tx_complete(sc, skb, txq, ts);
 	ieee80211_tx_status(sc->hw, skb);
 }
 
@@ -1679,7 +1658,7 @@ ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 
 			dma_unmap_single(sc->dev, bf->skbaddr, skb->len,
 					DMA_TO_DEVICE);
-			ath5k_tx_frame_completed(sc, skb, &ts);
+			ath5k_tx_frame_completed(sc, skb, txq, &ts);
 		}
 
 		/*
@@ -1821,8 +1800,6 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 		goto out;
 	}
 
-	ath5k_debug_dump_skb(sc, skb, "BC  ", 1);
-
 	ath5k_txbuf_free_skb(sc, avf->bbuf);
 	avf->bbuf->skb = skb;
 	ret = ath5k_beacon_setup(sc, avf->bbuf);
@@ -1917,6 +1894,8 @@ ath5k_beacon_send(struct ath5k_softc *sc)
 			sc->opmode == NL80211_IFTYPE_MESH_POINT)
 		ath5k_beacon_update(sc->hw, vif);
 
+	trace_ath5k_tx(sc, bf->skb, &sc->txqs[sc->bhalq]);
+
 	ath5k_hw_set_txdp(ah, sc->bhalq, bf->daddr);
 	ath5k_hw_start_tx_dma(ah, sc->bhalq);
 	ATH5K_DBG(sc, ATH5K_DEBUG_BEACON, "TXDP[%u] = %llx (%p)\n",
@@ -2417,7 +2396,8 @@ ath5k_init_softc(struct ath5k_softc *sc, const struct ath_bus_ops *bus_ops)
 	/* set up multi-rate retry capabilities */
 	if (sc->ah->ah_version == AR5K_AR5212) {
 		hw->max_rates = 4;
-		hw->max_rate_tries = 11;
+		hw->max_rate_tries = max(AR5K_INIT_RETRY_SHORT,
+					 AR5K_INIT_RETRY_LONG);
 	}
 
 	hw->vif_data_size = sizeof(struct ath5k_vif);
@@ -2554,7 +2534,6 @@ ath5k_init_hw(struct ath5k_softc *sc)
 	 * and then setup of the interrupt mask.
 	 */
 	sc->curchan = sc->hw->conf.channel;
-	sc->curband = &sc->sbands[sc->curchan->band];
 	sc->imask = AR5K_INT_RXOK | AR5K_INT_RXERR | AR5K_INT_RXEOL |
 		AR5K_INT_RXORN | AR5K_INT_TXDESC | AR5K_INT_TXEOL |
 		AR5K_INT_FATAL | AR5K_INT_GLOBAL | AR5K_INT_MIB;
@@ -2681,10 +2660,8 @@ ath5k_reset(struct ath5k_softc *sc, struct ieee80211_channel *chan,
 	 * so we should also free any remaining
 	 * tx buffers */
 	ath5k_drain_tx_buffs(sc);
-	if (chan) {
+	if (chan)
 		sc->curchan = chan;
-		sc->curband = &sc->sbands[chan->band];
-	}
 	ret = ath5k_hw_reset(ah, sc->opmode, sc->curchan, chan != NULL,
 								skip_pcu);
 	if (ret) {
@@ -2782,12 +2759,6 @@ ath5k_init(struct ieee80211_hw *hw)
 		goto err;
 	}
 
-	/* NB: setup here so ath5k_rate_update is happy */
-	if (test_bit(AR5K_MODE_11A, ah->ah_modes))
-		ath5k_setcurmode(sc, AR5K_MODE_11A);
-	else
-		ath5k_setcurmode(sc, AR5K_MODE_11B);
-
 	/*
 	 * Allocate tx+rx descriptors and populate the lists.
 	 */
@@ -2946,13 +2917,13 @@ ath5k_deinit_softc(struct ath5k_softc *sc)
 bool
 ath_any_vif_assoc(struct ath5k_softc *sc)
 {
-	struct ath_vif_iter_data iter_data;
+	struct ath5k_vif_iter_data iter_data;
 	iter_data.hw_macaddr = NULL;
 	iter_data.any_assoc = false;
 	iter_data.need_set_hw_addr = false;
 	iter_data.found_active = true;
 
-	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath_vif_iter,
+	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath5k_vif_iter,
 						   &iter_data);
 	return iter_data.any_assoc;
 }
diff --git a/drivers/net/wireless/ath/ath5k/base.h b/drivers/net/wireless/ath/ath5k/base.h
index 6d511476e4d2..8d1df1fa2351 100644
--- a/drivers/net/wireless/ath/ath5k/base.h
+++ b/drivers/net/wireless/ath/ath5k/base.h
@@ -183,8 +183,6 @@ struct ath5k_softc {
 	enum nl80211_iftype	opmode;
 	struct ath5k_hw		*ah;		/* Atheros HW */
 
-	struct ieee80211_supported_band		*curband;
-
 #ifdef CONFIG_ATH5K_DEBUG
 	struct ath5k_dbg_info	debug;		/* debug info */
 #endif /* CONFIG_ATH5K_DEBUG */
@@ -202,7 +200,6 @@ struct ath5k_softc {
 #define ATH_STAT_STARTED	4		/* opened & irqs enabled */
 
 	unsigned int		filter_flags;	/* HW flags, AR5K_RX_FILTER_* */
-	unsigned int		curmode;	/* current phy mode */
 	struct ieee80211_channel *curchan;	/* current h/w channel */
 
 	u16			nvifs;
@@ -262,6 +259,19 @@ struct ath5k_softc {
 	struct survey_info	survey;		/* collected survey info */
 };
 
+struct ath5k_vif_iter_data {
+	const u8	*hw_macaddr;
+	u8		mask[ETH_ALEN];
+	u8		active_mac[ETH_ALEN]; /* first active MAC */
+	bool		need_set_hw_addr;
+	bool		found_active;
+	bool		any_assoc;
+	enum nl80211_iftype opmode;
+	int n_stas;
+};
+void ath5k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif);
+
+
 #define ath5k_hw_hasbssidmask(_ah) \
 	(ath5k_hw_get_capability(_ah, AR5K_CAP_BSSIDMASK, 0, NULL) == 0)
 #define ath5k_hw_hasveol(_ah) \
diff --git a/drivers/net/wireless/ath/ath5k/caps.c b/drivers/net/wireless/ath/ath5k/caps.c
index 31cad80e9b01..f77e8a703c5c 100644
--- a/drivers/net/wireless/ath/ath5k/caps.c
+++ b/drivers/net/wireless/ath/ath5k/caps.c
@@ -32,23 +32,24 @@
  */
 int ath5k_hw_set_capabilities(struct ath5k_hw *ah)
 {
+	struct ath5k_capabilities *caps = &ah->ah_capabilities;
 	u16 ee_header;
 
 	/* Capabilities stored in the EEPROM */
-	ee_header = ah->ah_capabilities.cap_eeprom.ee_header;
+	ee_header = caps->cap_eeprom.ee_header;
 
 	if (ah->ah_version == AR5K_AR5210) {
 		/*
 		 * Set radio capabilities
 		 * (The AR5110 only supports the middle 5GHz band)
 		 */
-		ah->ah_capabilities.cap_range.range_5ghz_min = 5120;
-		ah->ah_capabilities.cap_range.range_5ghz_max = 5430;
-		ah->ah_capabilities.cap_range.range_2ghz_min = 0;
-		ah->ah_capabilities.cap_range.range_2ghz_max = 0;
+		caps->cap_range.range_5ghz_min = 5120;
+		caps->cap_range.range_5ghz_max = 5430;
+		caps->cap_range.range_2ghz_min = 0;
+		caps->cap_range.range_2ghz_max = 0;
 
 		/* Set supported modes */
-		__set_bit(AR5K_MODE_11A, ah->ah_capabilities.cap_mode);
+		__set_bit(AR5K_MODE_11A, caps->cap_mode);
 	} else {
 		/*
 		 * XXX The tranceiver supports frequencies from 4920 to 6100GHz
@@ -56,9 +57,8 @@ int ath5k_hw_set_capabilities(struct ath5k_hw *ah)
 		 * XXX current ieee80211 implementation because the IEEE
 		 * XXX channel mapping does not support negative channel
 		 * XXX numbers (2312MHz is channel -19). Of course, this
-		 * XXX doesn't matter because these channels are out of range
-		 * XXX but some regulation domains like MKK (Japan) will
-		 * XXX support frequencies somewhere around 4.8GHz.
+		 * XXX doesn't matter because these channels are out of the
+		 * XXX legal range.
 		 */
 
 		/*
@@ -66,13 +66,14 @@ int ath5k_hw_set_capabilities(struct ath5k_hw *ah)
 		 */
 
 		if (AR5K_EEPROM_HDR_11A(ee_header)) {
-			/* 4920 */
-			ah->ah_capabilities.cap_range.range_5ghz_min = 5005;
-			ah->ah_capabilities.cap_range.range_5ghz_max = 6100;
+			if (ath_is_49ghz_allowed(caps->cap_eeprom.ee_regdomain))
+				caps->cap_range.range_5ghz_min = 4920;
+			else
+				caps->cap_range.range_5ghz_min = 5005;
+			caps->cap_range.range_5ghz_max = 6100;
 
 			/* Set supported modes */
-			__set_bit(AR5K_MODE_11A,
-					ah->ah_capabilities.cap_mode);
+			__set_bit(AR5K_MODE_11A, caps->cap_mode);
 		}
 
 		/* Enable  802.11b if a 2GHz capable radio (2111/5112) is
@@ -81,32 +82,29 @@ int ath5k_hw_set_capabilities(struct ath5k_hw *ah)
 		    (AR5K_EEPROM_HDR_11G(ee_header) &&
 		     ah->ah_version != AR5K_AR5211)) {
 			/* 2312 */
-			ah->ah_capabilities.cap_range.range_2ghz_min = 2412;
-			ah->ah_capabilities.cap_range.range_2ghz_max = 2732;
+			caps->cap_range.range_2ghz_min = 2412;
+			caps->cap_range.range_2ghz_max = 2732;
 
 			if (AR5K_EEPROM_HDR_11B(ee_header))
-				__set_bit(AR5K_MODE_11B,
-						ah->ah_capabilities.cap_mode);
+				__set_bit(AR5K_MODE_11B, caps->cap_mode);
 
 			if (AR5K_EEPROM_HDR_11G(ee_header) &&
 			    ah->ah_version != AR5K_AR5211)
-				__set_bit(AR5K_MODE_11G,
-						ah->ah_capabilities.cap_mode);
+				__set_bit(AR5K_MODE_11G, caps->cap_mode);
 		}
 	}
 
 	/* Set number of supported TX queues */
 	if (ah->ah_version == AR5K_AR5210)
-		ah->ah_capabilities.cap_queues.q_tx_num =
-			AR5K_NUM_TX_QUEUES_NOQCU;
+		caps->cap_queues.q_tx_num = AR5K_NUM_TX_QUEUES_NOQCU;
 	else
-		ah->ah_capabilities.cap_queues.q_tx_num = AR5K_NUM_TX_QUEUES;
+		caps->cap_queues.q_tx_num = AR5K_NUM_TX_QUEUES;
 
 	/* newer hardware has PHY error counters */
 	if (ah->ah_mac_srev >= AR5K_SREV_AR5213A)
-		ah->ah_capabilities.cap_has_phyerr_counters = true;
+		caps->cap_has_phyerr_counters = true;
 	else
-		ah->ah_capabilities.cap_has_phyerr_counters = false;
+		caps->cap_has_phyerr_counters = false;
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/ath5k/debug.c b/drivers/net/wireless/ath/ath5k/debug.c
index d2f84d76bb07..0230f30e9e9a 100644
--- a/drivers/net/wireless/ath/ath5k/debug.c
+++ b/drivers/net/wireless/ath/ath5k/debug.c
@@ -308,8 +308,6 @@ static const struct {
 	{ ATH5K_DEBUG_CALIBRATE, "calib",	"periodic calibration" },
 	{ ATH5K_DEBUG_TXPOWER,	"txpower",	"transmit power setting" },
 	{ ATH5K_DEBUG_LED,	"led",		"LED management" },
-	{ ATH5K_DEBUG_DUMP_RX,	"dumprx",	"print received skb content" },
-	{ ATH5K_DEBUG_DUMP_TX,	"dumptx",	"print transmit skb content" },
 	{ ATH5K_DEBUG_DUMPBANDS, "dumpbands",	"dump bands" },
 	{ ATH5K_DEBUG_DMA,	"dma",		"dma start/stop" },
 	{ ATH5K_DEBUG_ANI,	"ani",		"adaptive noise immunity" },
@@ -1036,24 +1034,6 @@ ath5k_debug_printrxbuffs(struct ath5k_softc *sc, struct ath5k_hw *ah)
 }
 
 void
-ath5k_debug_dump_skb(struct ath5k_softc *sc,
-			struct sk_buff *skb, const char *prefix, int tx)
-{
-	char buf[16];
-
-	if (likely(!((tx && (sc->debug.level & ATH5K_DEBUG_DUMP_TX)) ||
-		     (!tx && (sc->debug.level & ATH5K_DEBUG_DUMP_RX)))))
-		return;
-
-	snprintf(buf, sizeof(buf), "%s %s", wiphy_name(sc->hw->wiphy), prefix);
-
-	print_hex_dump_bytes(buf, DUMP_PREFIX_NONE, skb->data,
-		min(200U, skb->len));
-
-	printk(KERN_DEBUG "\n");
-}
-
-void
 ath5k_debug_printtxbuf(struct ath5k_softc *sc, struct ath5k_buf *bf)
 {
 	struct ath5k_desc *ds = bf->desc;
diff --git a/drivers/net/wireless/ath/ath5k/debug.h b/drivers/net/wireless/ath/ath5k/debug.h
index 3e34428d5126..b0355aef68d3 100644
--- a/drivers/net/wireless/ath/ath5k/debug.h
+++ b/drivers/net/wireless/ath/ath5k/debug.h
@@ -116,8 +116,6 @@ enum ath5k_debug_level {
 	ATH5K_DEBUG_CALIBRATE	= 0x00000020,
 	ATH5K_DEBUG_TXPOWER	= 0x00000040,
 	ATH5K_DEBUG_LED		= 0x00000080,
-	ATH5K_DEBUG_DUMP_RX	= 0x00000100,
-	ATH5K_DEBUG_DUMP_TX	= 0x00000200,
 	ATH5K_DEBUG_DUMPBANDS	= 0x00000400,
 	ATH5K_DEBUG_DMA		= 0x00000800,
 	ATH5K_DEBUG_ANI		= 0x00002000,
@@ -152,10 +150,6 @@ void
 ath5k_debug_dump_bands(struct ath5k_softc *sc);
 
 void
-ath5k_debug_dump_skb(struct ath5k_softc *sc,
-			struct sk_buff *skb, const char *prefix, int tx);
-
-void
 ath5k_debug_printtxbuf(struct ath5k_softc *sc, struct ath5k_buf *bf);
 
 #else /* no debugging */
@@ -182,10 +176,6 @@ static inline void
 ath5k_debug_dump_bands(struct ath5k_softc *sc) {}
 
 static inline void
-ath5k_debug_dump_skb(struct ath5k_softc *sc,
-			struct sk_buff *skb, const char *prefix, int tx) {}
-
-static inline void
 ath5k_debug_printtxbuf(struct ath5k_softc *sc, struct ath5k_buf *bf) {}
 
 #endif /* ifdef CONFIG_ATH5K_DEBUG */
diff --git a/drivers/net/wireless/ath/ath5k/eeprom.c b/drivers/net/wireless/ath/ath5k/eeprom.c
index 80e625608bac..b6561f785c6e 100644
--- a/drivers/net/wireless/ath/ath5k/eeprom.c
+++ b/drivers/net/wireless/ath/ath5k/eeprom.c
@@ -72,7 +72,6 @@ static int
 ath5k_eeprom_init_header(struct ath5k_hw *ah)
 {
 	struct ath5k_eeprom_info *ee = &ah->ah_capabilities.cap_eeprom;
-	int ret;
 	u16 val;
 	u32 cksum, offset, eep_max = AR5K_EEPROM_INFO_MAX;
 
@@ -192,7 +191,7 @@ static int ath5k_eeprom_read_ants(struct ath5k_hw *ah, u32 *offset,
 	struct ath5k_eeprom_info *ee = &ah->ah_capabilities.cap_eeprom;
 	u32 o = *offset;
 	u16 val;
-	int ret, i = 0;
+	int i = 0;
 
 	AR5K_EEPROM_READ(o++, val);
 	ee->ee_switch_settling[mode]	= (val >> 8) & 0x7f;
@@ -252,7 +251,6 @@ static int ath5k_eeprom_read_modes(struct ath5k_hw *ah, u32 *offset,
 	struct ath5k_eeprom_info *ee = &ah->ah_capabilities.cap_eeprom;
 	u32 o = *offset;
 	u16 val;
-	int ret;
 
 	ee->ee_n_piers[mode] = 0;
 	AR5K_EEPROM_READ(o++, val);
@@ -515,7 +513,6 @@ ath5k_eeprom_read_freq_list(struct ath5k_hw *ah, int *offset, int max,
 	int o = *offset;
 	int i = 0;
 	u8 freq1, freq2;
-	int ret;
 	u16 val;
 
 	ee->ee_n_piers[mode] = 0;
@@ -551,7 +548,7 @@ ath5k_eeprom_init_11a_pcal_freq(struct ath5k_hw *ah, int offset)
 {
 	struct ath5k_eeprom_info *ee = &ah->ah_capabilities.cap_eeprom;
 	struct ath5k_chan_pcal_info *pcal = ee->ee_pwr_cal_a;
-	int i, ret;
+	int i;
 	u16 val;
 	u8 mask;
 
@@ -970,7 +967,6 @@ ath5k_eeprom_read_pcal_info_5112(struct ath5k_hw *ah, int mode)
 	u32 offset;
 	u8 i, c;
 	u16 val;
-	int ret;
 	u8 pd_gains = 0;
 
 	/* Count how many curves we have and
@@ -1228,7 +1224,7 @@ ath5k_eeprom_read_pcal_info_2413(struct ath5k_hw *ah, int mode)
 	struct ath5k_chan_pcal_info *chinfo;
 	u8 *pdgain_idx = ee->ee_pdc_to_idx[mode];
 	u32 offset;
-	int idx, i, ret;
+	int idx, i;
 	u16 val;
 	u8 pd_gains = 0;
 
@@ -1419,7 +1415,7 @@ ath5k_eeprom_read_target_rate_pwr_info(struct ath5k_hw *ah, unsigned int mode)
 	u8 *rate_target_pwr_num;
 	u32 offset;
 	u16 val;
-	int ret, i;
+	int i;
 
 	offset = AR5K_EEPROM_TARGET_PWRSTART(ee->ee_misc1);
 	rate_target_pwr_num = &ee->ee_rate_target_pwr_num[mode];
@@ -1593,7 +1589,7 @@ ath5k_eeprom_read_ctl_info(struct ath5k_hw *ah)
 	struct ath5k_edge_power *rep;
 	unsigned int fmask, pmask;
 	unsigned int ctl_mode;
-	int ret, i, j;
+	int i, j;
 	u32 offset;
 	u16 val;
 
@@ -1733,16 +1729,12 @@ int ath5k_eeprom_read_mac(struct ath5k_hw *ah, u8 *mac)
 	u8 mac_d[ETH_ALEN] = {};
 	u32 total, offset;
 	u16 data;
-	int octet, ret;
+	int octet;
 
-	ret = ath5k_hw_nvram_read(ah, 0x20, &data);
-	if (ret)
-		return ret;
+	AR5K_EEPROM_READ(0x20, data);
 
 	for (offset = 0x1f, octet = 0, total = 0; offset >= 0x1d; offset--) {
-		ret = ath5k_hw_nvram_read(ah, offset, &data);
-		if (ret)
-			return ret;
+		AR5K_EEPROM_READ(offset, data);
 
 		total += data;
 		mac_d[octet + 1] = data & 0xff;
diff --git a/drivers/net/wireless/ath/ath5k/eeprom.h b/drivers/net/wireless/ath/ath5k/eeprom.h
index 7c09e150dbdc..6511c27d938e 100644
--- a/drivers/net/wireless/ath/ath5k/eeprom.h
+++ b/drivers/net/wireless/ath/ath5k/eeprom.h
@@ -241,9 +241,8 @@ enum ath5k_eeprom_freq_bands{
 #define	AR5K_SPUR_SYMBOL_WIDTH_TURBO_100Hz	6250
 
 #define AR5K_EEPROM_READ(_o, _v) do {			\
-	ret = ath5k_hw_nvram_read(ah, (_o), &(_v));	\
-	if (ret)					\
-		return ret;				\
+	if (!ath5k_hw_nvram_read(ah, (_o), &(_v)))	\
+		return -EIO;				\
 } while (0)
 
 #define AR5K_EEPROM_READ_HDR(_o, _v)					\
@@ -269,29 +268,6 @@ enum ath5k_ctl_mode {
 	AR5K_CTL_MODE_M = 15,
 };
 
-/* Default CTL ids for the 3 main reg domains.
- * Atheros only uses these by default but vendors
- * can have up to 32 different CTLs for different
- * scenarios. Note that theese values are ORed with
- * the mode id (above) so we can have up to 24 CTL
- * datasets out of these 3 main regdomains. That leaves
- * 8 ids that can be used by vendors and since 0x20 is
- * missing from HAL sources i guess this is the set of
- * custom CTLs vendors can use. */
-#define	AR5K_CTL_FCC	0x10
-#define	AR5K_CTL_CUSTOM	0x20
-#define	AR5K_CTL_ETSI	0x30
-#define	AR5K_CTL_MKK	0x40
-
-/* Indicates a CTL with only mode set and
- * no reg domain mapping, such CTLs are used
- * for world roaming domains or simply when
- * a reg domain is not set */
-#define	AR5K_CTL_NO_REGDOMAIN	0xf0
-
-/* Indicates an empty (invalid) CTL */
-#define AR5K_CTL_NO_CTL		0xff
-
 /* Per channel calibration data, used for power table setup */
 struct ath5k_chan_pcal_info_rf5111 {
 	/* Power levels in half dbm units
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index d76d68c99f72..c9b0b676adda 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -48,28 +48,11 @@
 
 extern int ath5k_modparam_nohwcrypt;
 
-/* functions used from base.c */
-void set_beacon_filter(struct ieee80211_hw *hw, bool enable);
-bool ath_any_vif_assoc(struct ath5k_softc *sc);
-int ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
-		   struct ath5k_txq *txq);
-int ath5k_init_hw(struct ath5k_softc *sc);
-int ath5k_stop_hw(struct ath5k_softc *sc);
-void ath5k_mode_setup(struct ath5k_softc *sc, struct ieee80211_vif *vif);
-void ath5k_update_bssid_mask_and_opmode(struct ath5k_softc *sc,
-					struct ieee80211_vif *vif);
-int ath5k_chan_set(struct ath5k_softc *sc, struct ieee80211_channel *chan);
-void ath5k_beacon_update_timers(struct ath5k_softc *sc, u64 bc_tsf);
-int ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
-void ath5k_beacon_config(struct ath5k_softc *sc);
-void ath5k_txbuf_free_skb(struct ath5k_softc *sc, struct ath5k_buf *bf);
-void ath5k_rxbuf_free_skb(struct ath5k_softc *sc, struct ath5k_buf *bf);
-
 /********************\
 * Mac80211 functions *
 \********************/
 
-static int
+static void
 ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ath5k_softc *sc = hw->priv;
@@ -77,10 +60,10 @@ ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	if (WARN_ON(qnum >= sc->ah->ah_capabilities.cap_queues.q_tx_num)) {
 		dev_kfree_skb_any(skb);
-		return 0;
+		return;
 	}
 
-	return ath5k_tx_queue(hw, skb, &sc->txqs[qnum]);
+	ath5k_tx_queue(hw, skb, &sc->txqs[qnum]);
 }
 
 
@@ -175,8 +158,7 @@ ath5k_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
 	memcpy(&avf->lladdr, vif->addr, ETH_ALEN);
 
-	ath5k_mode_setup(sc, vif);
-
+	ath5k_update_bssid_mask_and_opmode(sc, vif);
 	ret = 0;
 end:
 	mutex_unlock(&sc->lock);
@@ -226,6 +208,7 @@ ath5k_config(struct ieee80211_hw *hw, u32 changed)
 	struct ath5k_hw *ah = sc->ah;
 	struct ieee80211_conf *conf = &hw->conf;
 	int ret = 0;
+	int i;
 
 	mutex_lock(&sc->lock);
 
@@ -243,6 +226,14 @@ ath5k_config(struct ieee80211_hw *hw, u32 changed)
 		ath5k_hw_set_txpower_limit(ah, (conf->power_level * 2));
 	}
 
+	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
+		ah->ah_retry_long = conf->long_frame_max_tx_count;
+		ah->ah_retry_short = conf->short_frame_max_tx_count;
+
+		for (i = 0; i < ah->ah_capabilities.cap_queues.q_tx_num; i++)
+			ath5k_hw_set_tx_retry_limits(ah, i);
+	}
+
 	/* TODO:
 	 * 1) Move this on config_interface and handle each case
 	 * separately eg. when we have only one STA vif, use
@@ -389,6 +380,7 @@ ath5k_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	struct ath5k_softc *sc = hw->priv;
 	struct ath5k_hw *ah = sc->ah;
 	u32 mfilt[2], rfilt;
+	struct ath5k_vif_iter_data iter_data; /* to count STA interfaces */
 
 	mutex_lock(&sc->lock);
 
@@ -462,6 +454,21 @@ ath5k_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 		break;
 	}
 
+	iter_data.hw_macaddr = NULL;
+	iter_data.n_stas = 0;
+	iter_data.need_set_hw_addr = false;
+	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath5k_vif_iter,
+						   &iter_data);
+
+	/* Set up RX Filter */
+	if (iter_data.n_stas > 1) {
+		/* If you have multiple STA interfaces connected to
+		 * different APs, ARPs are not received (most of the time?)
+		 * Enabling PROMISC appears to fix that probem.
+		 */
+		rfilt |= AR5K_RX_FILTER_PROM;
+	}
+
 	/* Set filters */
 	ath5k_hw_set_rx_filter(ah, rfilt);
 
diff --git a/drivers/net/wireless/ath/ath5k/pci.c b/drivers/net/wireless/ath/ath5k/pci.c
index 7f8c5b0e9d2a..66598a0d1df0 100644
--- a/drivers/net/wireless/ath/ath5k/pci.c
+++ b/drivers/net/wireless/ath/ath5k/pci.c
@@ -69,7 +69,8 @@ static void ath5k_pci_read_cachesize(struct ath_common *common, int *csz)
 /*
  * Read from eeprom
  */
-bool ath5k_pci_eeprom_read(struct ath_common *common, u32 offset, u16 *data)
+static bool
+ath5k_pci_eeprom_read(struct ath_common *common, u32 offset, u16 *data)
 {
 	struct ath5k_hw *ah = (struct ath5k_hw *) common->ah;
 	u32 status, timeout;
@@ -90,15 +91,15 @@ bool ath5k_pci_eeprom_read(struct ath_common *common, u32 offset, u16 *data)
 		status = ath5k_hw_reg_read(ah, AR5K_EEPROM_STATUS);
 		if (status & AR5K_EEPROM_STAT_RDDONE) {
 			if (status & AR5K_EEPROM_STAT_RDERR)
-				return -EIO;
+				return false;
 			*data = (u16)(ath5k_hw_reg_read(ah, AR5K_EEPROM_DATA) &
 					0xffff);
-			return 0;
+			return true;
 		}
 		udelay(15);
 	}
 
-	return -ETIMEDOUT;
+	return false;
 }
 
 int ath5k_hw_read_srev(struct ath5k_hw *ah)
diff --git a/drivers/net/wireless/ath/ath5k/phy.c b/drivers/net/wireless/ath/ath5k/phy.c
index 78c26fdccad1..62ce2f4e8605 100644
--- a/drivers/net/wireless/ath/ath5k/phy.c
+++ b/drivers/net/wireless/ath/ath5k/phy.c
@@ -282,6 +282,34 @@ int ath5k_hw_phy_disable(struct ath5k_hw *ah)
 	return 0;
 }
 
+/*
+ * Wait for synth to settle
+ */
+static void ath5k_hw_wait_for_synth(struct ath5k_hw *ah,
+			struct ieee80211_channel *channel)
+{
+	/*
+	 * On 5211+ read activation -> rx delay
+	 * and use it (100ns steps).
+	 */
+	if (ah->ah_version != AR5K_AR5210) {
+		u32 delay;
+		delay = ath5k_hw_reg_read(ah, AR5K_PHY_RX_DELAY) &
+			AR5K_PHY_RX_DELAY_M;
+		delay = (channel->hw_value & CHANNEL_CCK) ?
+			((delay << 2) / 22) : (delay / 10);
+		if (ah->ah_bwmode == AR5K_BWMODE_10MHZ)
+			delay = delay << 1;
+		if (ah->ah_bwmode == AR5K_BWMODE_5MHZ)
+			delay = delay << 2;
+		/* XXX: /2 on turbo ? Let's be safe
+		 * for now */
+		udelay(100 + delay);
+	} else {
+		mdelay(1);
+	}
+}
+
 
 /**********************\
 * RF Gain optimization *
@@ -1253,6 +1281,7 @@ static int ath5k_hw_channel(struct ath5k_hw *ah,
 	case AR5K_RF5111:
 		ret = ath5k_hw_rf5111_channel(ah, channel);
 		break;
+	case AR5K_RF2317:
 	case AR5K_RF2425:
 		ret = ath5k_hw_rf2425_channel(ah, channel);
 		break;
@@ -3237,6 +3266,13 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
 		/* Failed */
 		if (i >= 100)
 			return -EIO;
+
+		/* Set channel and wait for synth */
+		ret = ath5k_hw_channel(ah, channel);
+		if (ret)
+			return ret;
+
+		ath5k_hw_wait_for_synth(ah, channel);
 	}
 
 	/*
@@ -3251,13 +3287,53 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
 	if (ret)
 		return ret;
 
+	/* Write OFDM timings on 5212*/
+	if (ah->ah_version == AR5K_AR5212 &&
+		channel->hw_value & CHANNEL_OFDM) {
+
+		ret = ath5k_hw_write_ofdm_timings(ah, channel);
+		if (ret)
+			return ret;
+
+		/* Spur info is available only from EEPROM versions
+		 * greater than 5.3, but the EEPROM routines will use
+		 * static values for older versions */
+		if (ah->ah_mac_srev >= AR5K_SREV_AR5424)
+			ath5k_hw_set_spur_mitigation_filter(ah,
+							    channel);
+	}
+
+	/* If we used fast channel switching
+	 * we are done, release RF bus and
+	 * fire up NF calibration.
+	 *
+	 * Note: Only NF calibration due to
+	 * channel change, not AGC calibration
+	 * since AGC is still running !
+	 */
+	if (fast) {
+		/*
+		 * Release RF Bus grant
+		 */
+		AR5K_REG_DISABLE_BITS(ah, AR5K_PHY_RFBUS_REQ,
+				    AR5K_PHY_RFBUS_REQ_REQUEST);
+
+		/*
+		 * Start NF calibration
+		 */
+		AR5K_REG_ENABLE_BITS(ah, AR5K_PHY_AGCCTL,
+					AR5K_PHY_AGCCTL_NF);
+
+		return ret;
+	}
+
 	/*
 	 * For 5210 we do all initialization using
 	 * initvals, so we don't have to modify
 	 * any settings (5210 also only supports
 	 * a/aturbo modes)
 	 */
-	if ((ah->ah_version != AR5K_AR5210) && !fast) {
+	if (ah->ah_version != AR5K_AR5210) {
 
 		/*
 		 * Write initial RF gain settings
@@ -3276,22 +3352,6 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
 		if (ret)
 			return ret;
 
-		/* Write OFDM timings on 5212*/
-		if (ah->ah_version == AR5K_AR5212 &&
-			channel->hw_value & CHANNEL_OFDM) {
-
-			ret = ath5k_hw_write_ofdm_timings(ah, channel);
-			if (ret)
-				return ret;
-
-			/* Spur info is available only from EEPROM versions
-			 * greater than 5.3, but the EEPROM routines will use
-			 * static values for older versions */
-			if (ah->ah_mac_srev >= AR5K_SREV_AR5424)
-				ath5k_hw_set_spur_mitigation_filter(ah,
-								    channel);
-		}
-
 		/*Enable/disable 802.11b mode on 5111
 		(enable 2111 frequency converter + CCK)*/
 		if (ah->ah_radio == AR5K_RF5111) {
@@ -3322,47 +3382,20 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
 	 */
 	ath5k_hw_reg_write(ah, AR5K_PHY_ACT_ENABLE, AR5K_PHY_ACT);
 
+	ath5k_hw_wait_for_synth(ah, channel);
+
 	/*
-	 * On 5211+ read activation -> rx delay
-	 * and use it.
+	 * Perform ADC test to see if baseband is ready
+	 * Set tx hold and check adc test register
 	 */
-	if (ah->ah_version != AR5K_AR5210) {
-		u32 delay;
-		delay = ath5k_hw_reg_read(ah, AR5K_PHY_RX_DELAY) &
-			AR5K_PHY_RX_DELAY_M;
-		delay = (channel->hw_value & CHANNEL_CCK) ?
-			((delay << 2) / 22) : (delay / 10);
-		if (ah->ah_bwmode == AR5K_BWMODE_10MHZ)
-			delay = delay << 1;
-		if (ah->ah_bwmode == AR5K_BWMODE_5MHZ)
-			delay = delay << 2;
-		/* XXX: /2 on turbo ? Let's be safe
-		 * for now */
-		udelay(100 + delay);
-	} else {
-		mdelay(1);
-	}
-
-	if (fast)
-		/*
-		 * Release RF Bus grant
-		 */
-		AR5K_REG_DISABLE_BITS(ah, AR5K_PHY_RFBUS_REQ,
-				    AR5K_PHY_RFBUS_REQ_REQUEST);
-	else {
-		/*
-		 * Perform ADC test to see if baseband is ready
-		 * Set tx hold and check adc test register
-		 */
-		phy_tst1 = ath5k_hw_reg_read(ah, AR5K_PHY_TST1);
-		ath5k_hw_reg_write(ah, AR5K_PHY_TST1_TXHOLD, AR5K_PHY_TST1);
-		for (i = 0; i <= 20; i++) {
-			if (!(ath5k_hw_reg_read(ah, AR5K_PHY_ADC_TEST) & 0x10))
-				break;
-			udelay(200);
-		}
-		ath5k_hw_reg_write(ah, phy_tst1, AR5K_PHY_TST1);
+	phy_tst1 = ath5k_hw_reg_read(ah, AR5K_PHY_TST1);
+	ath5k_hw_reg_write(ah, AR5K_PHY_TST1_TXHOLD, AR5K_PHY_TST1);
+	for (i = 0; i <= 20; i++) {
+		if (!(ath5k_hw_reg_read(ah, AR5K_PHY_ADC_TEST) & 0x10))
+			break;
+		udelay(200);
 	}
+	ath5k_hw_reg_write(ah, phy_tst1, AR5K_PHY_TST1);
 
 	/*
 	 * Start automatic gain control calibration
diff --git a/drivers/net/wireless/ath/ath5k/qcu.c b/drivers/net/wireless/ath/ath5k/qcu.c
index 2c9c9e793d4e..3343fb9e4940 100644
--- a/drivers/net/wireless/ath/ath5k/qcu.c
+++ b/drivers/net/wireless/ath/ath5k/qcu.c
@@ -228,24 +228,9 @@ int ath5k_hw_setup_tx_queue(struct ath5k_hw *ah, enum ath5k_tx_queue queue_type,
 /*
  * Set tx retry limits on DCU
  */
-static void ath5k_hw_set_tx_retry_limits(struct ath5k_hw *ah,
-					unsigned int queue)
+void ath5k_hw_set_tx_retry_limits(struct ath5k_hw *ah,
+				  unsigned int queue)
 {
-	u32 retry_lg, retry_sh;
-
-	/*
-	 * Calculate and set retry limits
-	 */
-	if (ah->ah_software_retry) {
-		/* XXX Need to test this */
-		retry_lg = ah->ah_limit_tx_retries;
-		retry_sh = retry_lg = retry_lg > AR5K_DCU_RETRY_LMT_SH_RETRY ?
-			AR5K_DCU_RETRY_LMT_SH_RETRY : retry_lg;
-	} else {
-		retry_lg = AR5K_INIT_LG_RETRY;
-		retry_sh = AR5K_INIT_SH_RETRY;
-	}
-
 	/* Single data queue on AR5210 */
 	if (ah->ah_version == AR5K_AR5210) {
 		struct ath5k_txq_info *tq = &ah->ah_txq[queue];
@@ -255,25 +240,26 @@ static void ath5k_hw_set_tx_retry_limits(struct ath5k_hw *ah,
 
 		ath5k_hw_reg_write(ah,
 			(tq->tqi_cw_min << AR5K_NODCU_RETRY_LMT_CW_MIN_S)
-			| AR5K_REG_SM(AR5K_INIT_SLG_RETRY,
-				AR5K_NODCU_RETRY_LMT_SLG_RETRY)
-			| AR5K_REG_SM(AR5K_INIT_SSH_RETRY,
-				AR5K_NODCU_RETRY_LMT_SSH_RETRY)
-			| AR5K_REG_SM(retry_lg, AR5K_NODCU_RETRY_LMT_LG_RETRY)
-			| AR5K_REG_SM(retry_sh, AR5K_NODCU_RETRY_LMT_SH_RETRY),
+			| AR5K_REG_SM(ah->ah_retry_long,
+				      AR5K_NODCU_RETRY_LMT_SLG_RETRY)
+			| AR5K_REG_SM(ah->ah_retry_short,
+				      AR5K_NODCU_RETRY_LMT_SSH_RETRY)
+			| AR5K_REG_SM(ah->ah_retry_long,
+				      AR5K_NODCU_RETRY_LMT_LG_RETRY)
+			| AR5K_REG_SM(ah->ah_retry_short,
+				      AR5K_NODCU_RETRY_LMT_SH_RETRY),
 			AR5K_NODCU_RETRY_LMT);
 	/* DCU on AR5211+ */
 	} else {
 		ath5k_hw_reg_write(ah,
-			AR5K_REG_SM(AR5K_INIT_SLG_RETRY,
-				AR5K_DCU_RETRY_LMT_SLG_RETRY) |
-			AR5K_REG_SM(AR5K_INIT_SSH_RETRY,
-				AR5K_DCU_RETRY_LMT_SSH_RETRY) |
-			AR5K_REG_SM(retry_lg, AR5K_DCU_RETRY_LMT_LG_RETRY) |
-			AR5K_REG_SM(retry_sh, AR5K_DCU_RETRY_LMT_SH_RETRY),
+			AR5K_REG_SM(ah->ah_retry_long,
+				    AR5K_DCU_RETRY_LMT_RTS)
+			| AR5K_REG_SM(ah->ah_retry_long,
+				      AR5K_DCU_RETRY_LMT_STA_RTS)
+			| AR5K_REG_SM(max(ah->ah_retry_long, ah->ah_retry_short),
+				      AR5K_DCU_RETRY_LMT_STA_DATA),
 			AR5K_QUEUE_DFS_RETRY_LIMIT(queue));
 	}
-	return;
 }
 
 /**
diff --git a/drivers/net/wireless/ath/ath5k/reg.h b/drivers/net/wireless/ath/ath5k/reg.h
index fd14b9103951..e1c9abd8c879 100644
--- a/drivers/net/wireless/ath/ath5k/reg.h
+++ b/drivers/net/wireless/ath/ath5k/reg.h
@@ -686,16 +686,15 @@
 
 /*
  * DCU retry limit registers
+ * all these fields don't allow zero values
  */
 #define AR5K_DCU_RETRY_LMT_BASE		0x1080			/* Register Address -Queue0 DCU_RETRY_LMT */
-#define AR5K_DCU_RETRY_LMT_SH_RETRY	0x0000000f	/* Short retry limit mask */
-#define AR5K_DCU_RETRY_LMT_SH_RETRY_S	0
-#define AR5K_DCU_RETRY_LMT_LG_RETRY	0x000000f0	/* Long retry limit mask */
-#define AR5K_DCU_RETRY_LMT_LG_RETRY_S	4
-#define AR5K_DCU_RETRY_LMT_SSH_RETRY	0x00003f00	/* Station short retry limit mask (?) */
-#define AR5K_DCU_RETRY_LMT_SSH_RETRY_S	8
-#define AR5K_DCU_RETRY_LMT_SLG_RETRY	0x000fc000	/* Station long retry limit mask (?) */
-#define AR5K_DCU_RETRY_LMT_SLG_RETRY_S	14
+#define AR5K_DCU_RETRY_LMT_RTS		0x0000000f	/* RTS failure limit. Transmission fails if no CTS is received for this number of times */
+#define AR5K_DCU_RETRY_LMT_RTS_S	0
+#define AR5K_DCU_RETRY_LMT_STA_RTS	0x00003f00	/* STA RTS failure limit. If exceeded CW reset */
+#define AR5K_DCU_RETRY_LMT_STA_RTS_S	8
+#define AR5K_DCU_RETRY_LMT_STA_DATA	0x000fc000	/* STA data failure limit. If exceeded CW reset. */
+#define AR5K_DCU_RETRY_LMT_STA_DATA_S	14
 #define	AR5K_QUEUE_DFS_RETRY_LIMIT(_q)	AR5K_QUEUE_REG(AR5K_DCU_RETRY_LMT_BASE, _q)
 
 /*
diff --git a/drivers/net/wireless/ath/ath5k/trace.h b/drivers/net/wireless/ath/ath5k/trace.h
new file mode 100644
index 000000000000..2de68adb6240
--- /dev/null
+++ b/drivers/net/wireless/ath/ath5k/trace.h
@@ -0,0 +1,107 @@
+#if !defined(__TRACE_ATH5K_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __TRACE_ATH5K_H
+
+#include <linux/tracepoint.h>
+#include "base.h"
+
+#ifndef CONFIG_ATH5K_TRACER
+#undef TRACE_EVENT
+#define TRACE_EVENT(name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+#endif
+
+struct sk_buff;
+
+#define PRIV_ENTRY  __field(struct ath5k_softc *, priv)
+#define PRIV_ASSIGN __entry->priv = priv
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM ath5k
+
+TRACE_EVENT(ath5k_rx,
+	TP_PROTO(struct ath5k_softc *priv, struct sk_buff *skb),
+	TP_ARGS(priv, skb),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(unsigned long, skbaddr)
+		__dynamic_array(u8, frame, skb->len)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->skbaddr = (unsigned long) skb;
+		memcpy(__get_dynamic_array(frame), skb->data, skb->len);
+	),
+	TP_printk(
+		"[%p] RX skb=%lx", __entry->priv, __entry->skbaddr
+	)
+);
+
+TRACE_EVENT(ath5k_tx,
+	TP_PROTO(struct ath5k_softc *priv, struct sk_buff *skb,
+		 struct ath5k_txq *q),
+
+	TP_ARGS(priv, skb, q),
+
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(unsigned long, skbaddr)
+		__field(u8, qnum)
+		__dynamic_array(u8, frame, skb->len)
+	),
+
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->skbaddr = (unsigned long) skb;
+		__entry->qnum = (u8) q->qnum;
+		memcpy(__get_dynamic_array(frame), skb->data, skb->len);
+	),
+
+	TP_printk(
+		"[%p] TX skb=%lx q=%d", __entry->priv, __entry->skbaddr,
+		__entry->qnum
+	)
+);
+
+TRACE_EVENT(ath5k_tx_complete,
+	TP_PROTO(struct ath5k_softc *priv, struct sk_buff *skb,
+		 struct ath5k_txq *q, struct ath5k_tx_status *ts),
+
+	TP_ARGS(priv, skb, q, ts),
+
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(unsigned long, skbaddr)
+		__field(u8, qnum)
+		__field(u8, ts_status)
+		__field(s8, ts_rssi)
+		__field(u8, ts_antenna)
+	),
+
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->skbaddr = (unsigned long) skb;
+		__entry->qnum = (u8) q->qnum;
+		__entry->ts_status = ts->ts_status;
+		__entry->ts_rssi =  ts->ts_rssi;
+		__entry->ts_antenna = ts->ts_antenna;
+	),
+
+	TP_printk(
+		"[%p] TX end skb=%lx q=%d stat=%x rssi=%d ant=%x",
+		__entry->priv, __entry->skbaddr, __entry->qnum,
+		__entry->ts_status, __entry->ts_rssi, __entry->ts_antenna
+	)
+);
+
+#endif /* __TRACE_ATH5K_H */
+
+#ifdef CONFIG_ATH5K_TRACER
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH ../../drivers/net/wireless/ath/ath5k
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+#include <trace/define_trace.h>
+
+#endif
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index aca01621c205..4d66ca8042eb 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -4,7 +4,6 @@ ath9k-y +=	beacon.o \
 		main.o \
 		recv.o \
 		xmit.o \
-		virtual.o \
 
 ath9k-$(CONFIG_ATH9K_RATE_CONTROL) += rc.o
 ath9k-$(CONFIG_PCI) += pci.o
diff --git a/drivers/net/wireless/ath/ath9k/ahb.c b/drivers/net/wireless/ath/ath9k/ahb.c
index 25a6e4417cdb..9cb0efa9b4c0 100644
--- a/drivers/net/wireless/ath/ath9k/ahb.c
+++ b/drivers/net/wireless/ath/ath9k/ahb.c
@@ -54,7 +54,6 @@ static struct ath_bus_ops ath_ahb_bus_ops  = {
 static int ath_ahb_probe(struct platform_device *pdev)
 {
 	void __iomem *mem;
-	struct ath_wiphy *aphy;
 	struct ath_softc *sc;
 	struct ieee80211_hw *hw;
 	struct resource *res;
@@ -76,7 +75,7 @@ static int ath_ahb_probe(struct platform_device *pdev)
 		goto err_out;
 	}
 
-	mem = ioremap_nocache(res->start, res->end - res->start + 1);
+	mem = ioremap_nocache(res->start, resource_size(res));
 	if (mem == NULL) {
 		dev_err(&pdev->dev, "ioremap failed\n");
 		ret = -ENOMEM;
@@ -92,8 +91,7 @@ static int ath_ahb_probe(struct platform_device *pdev)
 
 	irq = res->start;
 
-	hw = ieee80211_alloc_hw(sizeof(struct ath_wiphy) +
-				sizeof(struct ath_softc), &ath9k_ops);
+	hw = ieee80211_alloc_hw(sizeof(struct ath_softc), &ath9k_ops);
 	if (hw == NULL) {
 		dev_err(&pdev->dev, "no memory for ieee80211_hw\n");
 		ret = -ENOMEM;
@@ -103,11 +101,7 @@ static int ath_ahb_probe(struct platform_device *pdev)
 	SET_IEEE80211_DEV(hw, &pdev->dev);
 	platform_set_drvdata(pdev, hw);
 
-	aphy = hw->priv;
-	sc = (struct ath_softc *) (aphy + 1);
-	aphy->sc = sc;
-	aphy->hw = hw;
-	sc->pri_wiphy = aphy;
+	sc = hw->priv;
 	sc->hw = hw;
 	sc->dev = &pdev->dev;
 	sc->mem = mem;
@@ -151,8 +145,7 @@ static int ath_ahb_remove(struct platform_device *pdev)
 	struct ieee80211_hw *hw = platform_get_drvdata(pdev);
 
 	if (hw) {
-		struct ath_wiphy *aphy = hw->priv;
-		struct ath_softc *sc = aphy->sc;
+		struct ath_softc *sc = hw->priv;
 		void __iomem *mem = sc->mem;
 
 		ath9k_deinit_device(sc);
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_calib.c b/drivers/net/wireless/ath/ath9k/ar9002_calib.c
index 5e300bd3d264..76388c6d6692 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_calib.c
@@ -805,7 +805,10 @@ static bool ar9002_hw_init_cal(struct ath_hw *ah, struct ath9k_channel *chan)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
 
-	if (AR_SREV_9271(ah) || AR_SREV_9285_12_OR_LATER(ah)) {
+	if (AR_SREV_9271(ah)) {
+		if (!ar9285_hw_cl_cal(ah, chan))
+			return false;
+	} else if (AR_SREV_9285_12_OR_LATER(ah)) {
 		if (!ar9285_hw_clc(ah, chan))
 			return false;
 	} else {
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 4819747fa4c3..4a9271802991 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -3673,7 +3673,7 @@ static void ar9003_hw_internal_regulator_apply(struct ath_hw *ah)
 				return;
 
 			reg_pmu_set = (5 << 1) | (7 << 4) | (1 << 8) |
-				      (7 << 14) | (6 << 17) | (1 << 20) |
+				      (2 << 14) | (6 << 17) | (1 << 20) |
 				      (3 << 24) | (1 << 28);
 
 			REG_WRITE(ah, AR_PHY_PMU1, reg_pmu_set);
@@ -3959,19 +3959,19 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 {
 #define POW_SM(_r, _s)     (((_r) & 0x3f) << (_s))
 	/* make sure forced gain is not set */
-	REG_WRITE(ah, 0xa458, 0);
+	REG_WRITE(ah, AR_PHY_TX_FORCED_GAIN, 0);
 
 	/* Write the OFDM power per rate set */
 
 	/* 6 (LSB), 9, 12, 18 (MSB) */
-	REG_WRITE(ah, 0xa3c0,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(0),
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_6_24], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_6_24], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_6_24], 8) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_6_24], 0));
 
 	/* 24 (LSB), 36, 48, 54 (MSB) */
-	REG_WRITE(ah, 0xa3c4,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(1),
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_54], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_48], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_36], 8) |
@@ -3980,14 +3980,14 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	/* Write the CCK power per rate set */
 
 	/* 1L (LSB), reserved, 2L, 2S (MSB) */
-	REG_WRITE(ah, 0xa3c8,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(2),
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_1L_5L], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_1L_5L], 16) |
 		  /* POW_SM(txPowerTimes2,  8) | this is reserved for AR9003 */
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_1L_5L], 0));
 
 	/* 5.5L (LSB), 5.5S, 11L, 11S (MSB) */
-	REG_WRITE(ah, 0xa3cc,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(3),
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_11S], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_11L], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_LEGACY_5S], 8) |
@@ -3997,7 +3997,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	/* Write the HT20 power per rate set */
 
 	/* 0/8/16 (LSB), 1-3/9-11/17-19, 4, 5 (MSB) */
-	REG_WRITE(ah, 0xa3d0,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(4),
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_5], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_4], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_1_3_9_11_17_19], 8) |
@@ -4005,7 +4005,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	    );
 
 	/* 6 (LSB), 7, 12, 13 (MSB) */
-	REG_WRITE(ah, 0xa3d4,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(5),
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_13], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_12], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_7], 8) |
@@ -4013,7 +4013,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	    );
 
 	/* 14 (LSB), 15, 20, 21 */
-	REG_WRITE(ah, 0xa3e4,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(9),
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_21], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_20], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_15], 8) |
@@ -4023,7 +4023,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	/* Mixed HT20 and HT40 rates */
 
 	/* HT20 22 (LSB), HT20 23, HT40 22, HT40 23 (MSB) */
-	REG_WRITE(ah, 0xa3e8,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(10),
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_23], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_22], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT20_23], 8) |
@@ -4035,7 +4035,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	 * correct PAR difference between HT40 and HT20/LEGACY
 	 * 0/8/16 (LSB), 1-3/9-11/17-19, 4, 5 (MSB)
 	 */
-	REG_WRITE(ah, 0xa3d8,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(6),
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_5], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_4], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_1_3_9_11_17_19], 8) |
@@ -4043,7 +4043,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	    );
 
 	/* 6 (LSB), 7, 12, 13 (MSB) */
-	REG_WRITE(ah, 0xa3dc,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(7),
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_13], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_12], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_7], 8) |
@@ -4051,7 +4051,7 @@ static int ar9003_hw_tx_power_regwrite(struct ath_hw *ah, u8 * pPwrArray)
 	    );
 
 	/* 14 (LSB), 15, 20, 21 */
-	REG_WRITE(ah, 0xa3ec,
+	REG_WRITE(ah, AR_PHY_POWER_TX_RATE(11),
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_21], 24) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_20], 16) |
 		  POW_SM(pPwrArray[ALL_TARGET_HT40_15], 8) |
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_hw.c b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
index 06fb2c850535..7f5de6e4448b 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
@@ -28,7 +28,67 @@
  */
 static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 {
-	if (AR_SREV_9485(ah)) {
+	if (AR_SREV_9485_11(ah)) {
+		/* mac */
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
+				ar9485_1_1_mac_core,
+				ARRAY_SIZE(ar9485_1_1_mac_core), 2);
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
+				ar9485_1_1_mac_postamble,
+				ARRAY_SIZE(ar9485_1_1_mac_postamble), 5);
+
+		/* bb */
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], ar9485_1_1,
+				ARRAY_SIZE(ar9485_1_1), 2);
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
+				ar9485_1_1_baseband_core,
+				ARRAY_SIZE(ar9485_1_1_baseband_core), 2);
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
+				ar9485_1_1_baseband_postamble,
+				ARRAY_SIZE(ar9485_1_1_baseband_postamble), 5);
+
+		/* radio */
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
+				ar9485_1_1_radio_core,
+				ARRAY_SIZE(ar9485_1_1_radio_core), 2);
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
+				ar9485_1_1_radio_postamble,
+				ARRAY_SIZE(ar9485_1_1_radio_postamble), 2);
+
+		/* soc */
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
+				ar9485_1_1_soc_preamble,
+				ARRAY_SIZE(ar9485_1_1_soc_preamble), 2);
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST], NULL, 0, 0);
+
+		/* rx/tx gain */
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+				ar9485_common_rx_gain_1_1,
+				ARRAY_SIZE(ar9485_common_rx_gain_1_1), 2);
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+				ar9485_modes_lowest_ob_db_tx_gain_1_1,
+				ARRAY_SIZE(ar9485_modes_lowest_ob_db_tx_gain_1_1),
+				5);
+
+		/* Load PCIE SERDES settings from INI */
+
+		/* Awake Setting */
+
+		INIT_INI_ARRAY(&ah->iniPcieSerdes,
+				ar9485_1_1_pcie_phy_clkreq_disable_L1,
+				ARRAY_SIZE(ar9485_1_1_pcie_phy_clkreq_disable_L1),
+				2);
+
+		/* Sleep Setting */
+
+		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
+				ar9485_1_1_pcie_phy_clkreq_disable_L1,
+				ARRAY_SIZE(ar9485_1_1_pcie_phy_clkreq_disable_L1),
+				2);
+	} else if (AR_SREV_9485(ah)) {
 		/* mac */
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
@@ -85,8 +145,8 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 		/* Sleep Setting */
 
 		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-				ar9485_1_0_pcie_phy_pll_on_clkreq_enable_L1,
-				ARRAY_SIZE(ar9485_1_0_pcie_phy_pll_on_clkreq_enable_L1),
+				ar9485_1_0_pcie_phy_pll_on_clkreq_disable_L1,
+				ARRAY_SIZE(ar9485_1_0_pcie_phy_pll_on_clkreq_disable_L1),
 				2);
 	} else {
 		/* mac */
@@ -163,7 +223,12 @@ static void ar9003_tx_gain_table_apply(struct ath_hw *ah)
 	switch (ar9003_hw_get_tx_gain_idx(ah)) {
 	case 0:
 	default:
-		if (AR_SREV_9485(ah))
+		if (AR_SREV_9485_11(ah))
+			INIT_INI_ARRAY(&ah->iniModesTxGain,
+				       ar9485_modes_lowest_ob_db_tx_gain_1_1,
+				       ARRAY_SIZE(ar9485_modes_lowest_ob_db_tx_gain_1_1),
+				       5);
+		else if (AR_SREV_9485(ah))
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
 				       ar9485Modes_lowest_ob_db_tx_gain_1_0,
 				       ARRAY_SIZE(ar9485Modes_lowest_ob_db_tx_gain_1_0),
@@ -175,10 +240,15 @@ static void ar9003_tx_gain_table_apply(struct ath_hw *ah)
 				       5);
 		break;
 	case 1:
-		if (AR_SREV_9485(ah))
+		if (AR_SREV_9485_11(ah))
+			INIT_INI_ARRAY(&ah->iniModesTxGain,
+				       ar9485Modes_high_ob_db_tx_gain_1_1,
+				       ARRAY_SIZE(ar9485Modes_high_ob_db_tx_gain_1_1),
+				       5);
+		else if (AR_SREV_9485(ah))
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
 				       ar9485Modes_high_ob_db_tx_gain_1_0,
-				       ARRAY_SIZE(ar9485Modes_lowest_ob_db_tx_gain_1_0),
+				       ARRAY_SIZE(ar9485Modes_high_ob_db_tx_gain_1_0),
 				       5);
 		else
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
@@ -187,10 +257,15 @@ static void ar9003_tx_gain_table_apply(struct ath_hw *ah)
 				       5);
 		break;
 	case 2:
-		if (AR_SREV_9485(ah))
+		if (AR_SREV_9485_11(ah))
+			INIT_INI_ARRAY(&ah->iniModesTxGain,
+				       ar9485Modes_low_ob_db_tx_gain_1_1,
+				       ARRAY_SIZE(ar9485Modes_low_ob_db_tx_gain_1_1),
+				       5);
+		else if (AR_SREV_9485(ah))
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
 				       ar9485Modes_low_ob_db_tx_gain_1_0,
-				       ARRAY_SIZE(ar9485Modes_lowest_ob_db_tx_gain_1_0),
+				       ARRAY_SIZE(ar9485Modes_low_ob_db_tx_gain_1_0),
 				       5);
 		else
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
@@ -199,7 +274,12 @@ static void ar9003_tx_gain_table_apply(struct ath_hw *ah)
 				       5);
 		break;
 	case 3:
-		if (AR_SREV_9485(ah))
+		if (AR_SREV_9485_11(ah))
+			INIT_INI_ARRAY(&ah->iniModesTxGain,
+				       ar9485Modes_high_power_tx_gain_1_1,
+				       ARRAY_SIZE(ar9485Modes_high_power_tx_gain_1_1),
+				       5);
+		else if (AR_SREV_9485(ah))
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
 				       ar9485Modes_high_power_tx_gain_1_0,
 				       ARRAY_SIZE(ar9485Modes_high_power_tx_gain_1_0),
@@ -218,7 +298,12 @@ static void ar9003_rx_gain_table_apply(struct ath_hw *ah)
 	switch (ar9003_hw_get_rx_gain_idx(ah)) {
 	case 0:
 	default:
-		if (AR_SREV_9485(ah))
+		if (AR_SREV_9485_11(ah))
+			INIT_INI_ARRAY(&ah->iniModesRxGain,
+				       ar9485_common_rx_gain_1_1,
+				       ARRAY_SIZE(ar9485_common_rx_gain_1_1),
+				       2);
+		else if (AR_SREV_9485(ah))
 			INIT_INI_ARRAY(&ah->iniModesRxGain,
 				       ar9485Common_rx_gain_1_0,
 				       ARRAY_SIZE(ar9485Common_rx_gain_1_0),
@@ -230,7 +315,12 @@ static void ar9003_rx_gain_table_apply(struct ath_hw *ah)
 				       2);
 		break;
 	case 1:
-		if (AR_SREV_9485(ah))
+		if (AR_SREV_9485_11(ah))
+			INIT_INI_ARRAY(&ah->iniModesRxGain,
+				       ar9485Common_wo_xlna_rx_gain_1_1,
+				       ARRAY_SIZE(ar9485Common_wo_xlna_rx_gain_1_1),
+				       2);
+		else if (AR_SREV_9485(ah))
 			INIT_INI_ARRAY(&ah->iniModesRxGain,
 				       ar9485Common_wo_xlna_rx_gain_1_0,
 				       ARRAY_SIZE(ar9485Common_wo_xlna_rx_gain_1_0),
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index 4ceddbbdfcee..038a0cbfc6e7 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
@@ -615,7 +615,7 @@ int ath9k_hw_process_rxdesc_edma(struct ath_hw *ah, struct ath_rx_status *rxs,
 		 */
 		if (rxsp->status11 & AR_CRCErr)
 			rxs->rs_status |= ATH9K_RXERR_CRC;
-		if (rxsp->status11 & AR_PHYErr) {
+		else if (rxsp->status11 & AR_PHYErr) {
 			phyerr = MS(rxsp->status11, AR_PHYErrCode);
 			/*
 			 * If we reach a point here where AR_PostDelimCRCErr is
@@ -638,11 +638,11 @@ int ath9k_hw_process_rxdesc_edma(struct ath_hw *ah, struct ath_rx_status *rxs,
 				rxs->rs_phyerr = phyerr;
 			}
 
-		}
-		if (rxsp->status11 & AR_DecryptCRCErr)
+		} else if (rxsp->status11 & AR_DecryptCRCErr)
 			rxs->rs_status |= ATH9K_RXERR_DECRYPT;
-		if (rxsp->status11 & AR_MichaelErr)
+		else if (rxsp->status11 & AR_MichaelErr)
 			rxs->rs_status |= ATH9K_RXERR_MIC;
+
 		if (rxsp->status11 & AR_KeyMiss)
 			rxs->rs_status |= ATH9K_RXERR_DECRYPT;
 	}
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index 8d60f4f09acc..eb250d6b8038 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -1020,28 +1020,29 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 static void ar9003_hw_do_getnf(struct ath_hw *ah,
 			      int16_t nfarray[NUM_NF_READINGS])
 {
-	int16_t nf;
-
-	nf = MS(REG_READ(ah, AR_PHY_CCA_0), AR_PHY_MINCCA_PWR);
-	nfarray[0] = sign_extend32(nf, 8);
-
-	nf = MS(REG_READ(ah, AR_PHY_CCA_1), AR_PHY_CH1_MINCCA_PWR);
-	nfarray[1] = sign_extend32(nf, 8);
+#define AR_PHY_CH_MINCCA_PWR	0x1FF00000
+#define AR_PHY_CH_MINCCA_PWR_S	20
+#define AR_PHY_CH_EXT_MINCCA_PWR 0x01FF0000
+#define AR_PHY_CH_EXT_MINCCA_PWR_S 16
 
-	nf = MS(REG_READ(ah, AR_PHY_CCA_2), AR_PHY_CH2_MINCCA_PWR);
-	nfarray[2] = sign_extend32(nf, 8);
-
-	if (!IS_CHAN_HT40(ah->curchan))
-		return;
+	int16_t nf;
+	int i;
 
-	nf = MS(REG_READ(ah, AR_PHY_EXT_CCA), AR_PHY_EXT_MINCCA_PWR);
-	nfarray[3] = sign_extend32(nf, 8);
+	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
+		if (ah->rxchainmask & BIT(i)) {
+			nf = MS(REG_READ(ah, ah->nf_regs[i]),
+					 AR_PHY_CH_MINCCA_PWR);
+			nfarray[i] = sign_extend32(nf, 8);
 
-	nf = MS(REG_READ(ah, AR_PHY_EXT_CCA_1), AR_PHY_CH1_EXT_MINCCA_PWR);
-	nfarray[4] = sign_extend32(nf, 8);
+			if (IS_CHAN_HT40(ah->curchan)) {
+				u8 ext_idx = AR9300_MAX_CHAINS + i;
 
-	nf = MS(REG_READ(ah, AR_PHY_EXT_CCA_2), AR_PHY_CH2_EXT_MINCCA_PWR);
-	nfarray[5] = sign_extend32(nf, 8);
+				nf = MS(REG_READ(ah, ah->nf_regs[ext_idx]),
+						 AR_PHY_CH_EXT_MINCCA_PWR);
+				nfarray[ext_idx] = sign_extend32(nf, 8);
+			}
+		}
+	}
 }
 
 static void ar9003_hw_set_nf_limits(struct ath_hw *ah)
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.h b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
index 59bab6bd8a74..8bdda2cf9dd7 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -486,6 +486,8 @@
 #define AR_PHY_HEAVYCLIP_40      (AR_SM_BASE + 0x1ac)
 #define AR_PHY_ILLEGAL_TXRATE    (AR_SM_BASE + 0x1b0)
 
+#define AR_PHY_POWER_TX_RATE(_d) (AR_SM_BASE + 0x1c0 + ((_d) << 2))
+
 #define AR_PHY_PWRTX_MAX         (AR_SM_BASE + 0x1f0)
 #define AR_PHY_POWER_TX_SUB      (AR_SM_BASE + 0x1f4)
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9485_initvals.h b/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
index 70de3d89a7b5..eac4d8526fc1 100644
--- a/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
@@ -940,4 +940,1145 @@ static const u32 ar9485_1_0_mac_core[][2] = {
 	{0x000083cc, 0x00000200},
 	{0x000083d0, 0x000301ff},
 };
+
+static const u32 ar9485_1_1_mac_core[][2] = {
+	/*  Addr       allmodes */
+	{0x00000008, 0x00000000},
+	{0x00000030, 0x00020085},
+	{0x00000034, 0x00000005},
+	{0x00000040, 0x00000000},
+	{0x00000044, 0x00000000},
+	{0x00000048, 0x00000008},
+	{0x0000004c, 0x00000010},
+	{0x00000050, 0x00000000},
+	{0x00001040, 0x002ffc0f},
+	{0x00001044, 0x002ffc0f},
+	{0x00001048, 0x002ffc0f},
+	{0x0000104c, 0x002ffc0f},
+	{0x00001050, 0x002ffc0f},
+	{0x00001054, 0x002ffc0f},
+	{0x00001058, 0x002ffc0f},
+	{0x0000105c, 0x002ffc0f},
+	{0x00001060, 0x002ffc0f},
+	{0x00001064, 0x002ffc0f},
+	{0x000010f0, 0x00000100},
+	{0x00001270, 0x00000000},
+	{0x000012b0, 0x00000000},
+	{0x000012f0, 0x00000000},
+	{0x0000143c, 0x00000000},
+	{0x0000147c, 0x00000000},
+	{0x00008000, 0x00000000},
+	{0x00008004, 0x00000000},
+	{0x00008008, 0x00000000},
+	{0x0000800c, 0x00000000},
+	{0x00008018, 0x00000000},
+	{0x00008020, 0x00000000},
+	{0x00008038, 0x00000000},
+	{0x0000803c, 0x00000000},
+	{0x00008040, 0x00000000},
+	{0x00008044, 0x00000000},
+	{0x00008048, 0x00000000},
+	{0x0000804c, 0xffffffff},
+	{0x00008054, 0x00000000},
+	{0x00008058, 0x00000000},
+	{0x0000805c, 0x000fc78f},
+	{0x00008060, 0x0000000f},
+	{0x00008064, 0x00000000},
+	{0x00008070, 0x00000310},
+	{0x00008074, 0x00000020},
+	{0x00008078, 0x00000000},
+	{0x0000809c, 0x0000000f},
+	{0x000080a0, 0x00000000},
+	{0x000080a4, 0x02ff0000},
+	{0x000080a8, 0x0e070605},
+	{0x000080ac, 0x0000000d},
+	{0x000080b0, 0x00000000},
+	{0x000080b4, 0x00000000},
+	{0x000080b8, 0x00000000},
+	{0x000080bc, 0x00000000},
+	{0x000080c0, 0x2a800000},
+	{0x000080c4, 0x06900168},
+	{0x000080c8, 0x13881c22},
+	{0x000080cc, 0x01f40000},
+	{0x000080d0, 0x00252500},
+	{0x000080d4, 0x00a00000},
+	{0x000080d8, 0x00400000},
+	{0x000080dc, 0x00000000},
+	{0x000080e0, 0xffffffff},
+	{0x000080e4, 0x0000ffff},
+	{0x000080e8, 0x3f3f3f3f},
+	{0x000080ec, 0x00000000},
+	{0x000080f0, 0x00000000},
+	{0x000080f4, 0x00000000},
+	{0x000080fc, 0x00020000},
+	{0x00008100, 0x00000000},
+	{0x00008108, 0x00000052},
+	{0x0000810c, 0x00000000},
+	{0x00008110, 0x00000000},
+	{0x00008114, 0x000007ff},
+	{0x00008118, 0x000000aa},
+	{0x0000811c, 0x00003210},
+	{0x00008124, 0x00000000},
+	{0x00008128, 0x00000000},
+	{0x0000812c, 0x00000000},
+	{0x00008130, 0x00000000},
+	{0x00008134, 0x00000000},
+	{0x00008138, 0x00000000},
+	{0x0000813c, 0x0000ffff},
+	{0x00008144, 0xffffffff},
+	{0x00008168, 0x00000000},
+	{0x0000816c, 0x00000000},
+	{0x00008170, 0x18486200},
+	{0x00008174, 0x33332210},
+	{0x00008178, 0x00000000},
+	{0x0000817c, 0x00020000},
+	{0x000081c0, 0x00000000},
+	{0x000081c4, 0x33332210},
+	{0x000081d4, 0x00000000},
+	{0x000081ec, 0x00000000},
+	{0x000081f0, 0x00000000},
+	{0x000081f4, 0x00000000},
+	{0x000081f8, 0x00000000},
+	{0x000081fc, 0x00000000},
+	{0x00008240, 0x00100000},
+	{0x00008244, 0x0010f400},
+	{0x00008248, 0x00000800},
+	{0x0000824c, 0x0001e800},
+	{0x00008250, 0x00000000},
+	{0x00008254, 0x00000000},
+	{0x00008258, 0x00000000},
+	{0x0000825c, 0x40000000},
+	{0x00008260, 0x00080922},
+	{0x00008264, 0x9ca00010},
+	{0x00008268, 0xffffffff},
+	{0x0000826c, 0x0000ffff},
+	{0x00008270, 0x00000000},
+	{0x00008274, 0x40000000},
+	{0x00008278, 0x003e4180},
+	{0x0000827c, 0x00000004},
+	{0x00008284, 0x0000002c},
+	{0x00008288, 0x0000002c},
+	{0x0000828c, 0x000000ff},
+	{0x00008294, 0x00000000},
+	{0x00008298, 0x00000000},
+	{0x0000829c, 0x00000000},
+	{0x00008300, 0x00000140},
+	{0x00008314, 0x00000000},
+	{0x0000831c, 0x0000010d},
+	{0x00008328, 0x00000000},
+	{0x0000832c, 0x00000007},
+	{0x00008330, 0x00000302},
+	{0x00008334, 0x00000700},
+	{0x00008338, 0x00ff0000},
+	{0x0000833c, 0x02400000},
+	{0x00008340, 0x000107ff},
+	{0x00008344, 0xa248105b},
+	{0x00008348, 0x008f0000},
+	{0x0000835c, 0x00000000},
+	{0x00008360, 0xffffffff},
+	{0x00008364, 0xffffffff},
+	{0x00008368, 0x00000000},
+	{0x00008370, 0x00000000},
+	{0x00008374, 0x000000ff},
+	{0x00008378, 0x00000000},
+	{0x0000837c, 0x00000000},
+	{0x00008380, 0xffffffff},
+	{0x00008384, 0xffffffff},
+	{0x00008390, 0xffffffff},
+	{0x00008394, 0xffffffff},
+	{0x00008398, 0x00000000},
+	{0x0000839c, 0x00000000},
+	{0x000083a0, 0x00000000},
+	{0x000083a4, 0x0000fa14},
+	{0x000083a8, 0x000f0c00},
+	{0x000083ac, 0x33332210},
+	{0x000083b0, 0x33332210},
+	{0x000083b4, 0x33332210},
+	{0x000083b8, 0x33332210},
+	{0x000083bc, 0x00000000},
+	{0x000083c0, 0x00000000},
+	{0x000083c4, 0x00000000},
+	{0x000083c8, 0x00000000},
+	{0x000083cc, 0x00000200},
+	{0x000083d0, 0x000301ff},
+};
+
+static const u32 ar9485_1_1_baseband_core[][2] = {
+	/* Addr       allmodes */
+	{0x00009800, 0xafe68e30},
+	{0x00009804, 0xfd14e000},
+	{0x00009808, 0x9c0a8f6b},
+	{0x0000980c, 0x04800000},
+	{0x00009814, 0x9280c00a},
+	{0x00009818, 0x00000000},
+	{0x0000981c, 0x00020028},
+	{0x00009834, 0x5f3ca3de},
+	{0x00009838, 0x0108ecff},
+	{0x0000983c, 0x14750600},
+	{0x00009880, 0x201fff00},
+	{0x00009884, 0x00001042},
+	{0x000098a4, 0x00200400},
+	{0x000098b0, 0x52440bbe},
+	{0x000098d0, 0x004b6a8e},
+	{0x000098d4, 0x00000820},
+	{0x000098dc, 0x00000000},
+	{0x000098f0, 0x00000000},
+	{0x000098f4, 0x00000000},
+	{0x00009c04, 0x00000000},
+	{0x00009c08, 0x03200000},
+	{0x00009c0c, 0x00000000},
+	{0x00009c10, 0x00000000},
+	{0x00009c14, 0x00046384},
+	{0x00009c18, 0x05b6b440},
+	{0x00009c1c, 0x00b6b440},
+	{0x00009d00, 0xc080a333},
+	{0x00009d04, 0x40206c10},
+	{0x00009d08, 0x009c4060},
+	{0x00009d0c, 0x1883800a},
+	{0x00009d10, 0x01834061},
+	{0x00009d14, 0x00c00400},
+	{0x00009d18, 0x00000000},
+	{0x00009d1c, 0x00000000},
+	{0x00009e08, 0x0038233c},
+	{0x00009e24, 0x9927b515},
+	{0x00009e28, 0x12ef0200},
+	{0x00009e30, 0x06336f77},
+	{0x00009e34, 0x6af6532f},
+	{0x00009e38, 0x0cc80c00},
+	{0x00009e40, 0x0d261820},
+	{0x00009e4c, 0x00001004},
+	{0x00009e50, 0x00ff03f1},
+	{0x00009fc0, 0x80be4788},
+	{0x00009fc4, 0x0001efb5},
+	{0x00009fcc, 0x40000014},
+	{0x0000a20c, 0x00000000},
+	{0x0000a210, 0x00000000},
+	{0x0000a220, 0x00000000},
+	{0x0000a224, 0x00000000},
+	{0x0000a228, 0x10002310},
+	{0x0000a23c, 0x00000000},
+	{0x0000a244, 0x0c000000},
+	{0x0000a2a0, 0x00000001},
+	{0x0000a2c0, 0x00000001},
+	{0x0000a2c8, 0x00000000},
+	{0x0000a2cc, 0x18c43433},
+	{0x0000a2d4, 0x00000000},
+	{0x0000a2dc, 0x00000000},
+	{0x0000a2e0, 0x00000000},
+	{0x0000a2e4, 0x00000000},
+	{0x0000a2e8, 0x00000000},
+	{0x0000a2ec, 0x00000000},
+	{0x0000a2f0, 0x00000000},
+	{0x0000a2f4, 0x00000000},
+	{0x0000a2f8, 0x00000000},
+	{0x0000a344, 0x00000000},
+	{0x0000a34c, 0x00000000},
+	{0x0000a350, 0x0000a000},
+	{0x0000a364, 0x00000000},
+	{0x0000a370, 0x00000000},
+	{0x0000a390, 0x00000001},
+	{0x0000a394, 0x00000444},
+	{0x0000a398, 0x001f0e0f},
+	{0x0000a39c, 0x0075393f},
+	{0x0000a3a0, 0xb79f6427},
+	{0x0000a3a4, 0x000000ff},
+	{0x0000a3a8, 0x3b3b3b3b},
+	{0x0000a3ac, 0x2f2f2f2f},
+	{0x0000a3c0, 0x20202020},
+	{0x0000a3c4, 0x22222220},
+	{0x0000a3c8, 0x20200020},
+	{0x0000a3cc, 0x20202020},
+	{0x0000a3d0, 0x20202020},
+	{0x0000a3d4, 0x20202020},
+	{0x0000a3d8, 0x20202020},
+	{0x0000a3dc, 0x20202020},
+	{0x0000a3e0, 0x20202020},
+	{0x0000a3e4, 0x20202020},
+	{0x0000a3e8, 0x20202020},
+	{0x0000a3ec, 0x20202020},
+	{0x0000a3f0, 0x00000000},
+	{0x0000a3f4, 0x00000006},
+	{0x0000a3f8, 0x0cdbd380},
+	{0x0000a3fc, 0x000f0f01},
+	{0x0000a400, 0x8fa91f01},
+	{0x0000a404, 0x00000000},
+	{0x0000a408, 0x0e79e5c6},
+	{0x0000a40c, 0x00820820},
+	{0x0000a414, 0x1ce739cf},
+	{0x0000a418, 0x2d0019ce},
+	{0x0000a41c, 0x1ce739ce},
+	{0x0000a420, 0x000001ce},
+	{0x0000a424, 0x1ce739ce},
+	{0x0000a428, 0x000001ce},
+	{0x0000a42c, 0x1ce739ce},
+	{0x0000a430, 0x1ce739ce},
+	{0x0000a434, 0x00000000},
+	{0x0000a438, 0x00001801},
+	{0x0000a43c, 0x00000000},
+	{0x0000a440, 0x00000000},
+	{0x0000a444, 0x00000000},
+	{0x0000a448, 0x04000000},
+	{0x0000a44c, 0x00000001},
+	{0x0000a450, 0x00010000},
+	{0x0000a5c4, 0xbfad9d74},
+	{0x0000a5c8, 0x0048060a},
+	{0x0000a5cc, 0x00000637},
+	{0x0000a760, 0x03020100},
+	{0x0000a764, 0x09080504},
+	{0x0000a768, 0x0d0c0b0a},
+	{0x0000a76c, 0x13121110},
+	{0x0000a770, 0x31301514},
+	{0x0000a774, 0x35343332},
+	{0x0000a778, 0x00000036},
+	{0x0000a780, 0x00000838},
+	{0x0000a7c0, 0x00000000},
+	{0x0000a7c4, 0xfffffffc},
+	{0x0000a7c8, 0x00000000},
+	{0x0000a7cc, 0x00000000},
+	{0x0000a7d0, 0x00000000},
+	{0x0000a7d4, 0x00000004},
+	{0x0000a7dc, 0x00000000},
+};
+
+static const u32 ar9485Common_1_1[][2] = {
+	/*  Addr      allmodes */
+	{0x00007010, 0x00000022},
+	{0x00007020, 0x00000000},
+	{0x00007034, 0x00000002},
+	{0x00007038, 0x000004c2},
+};
+
+static const u32 ar9485_1_1_baseband_postamble[][5] = {
+	/* Addr       5G_HT20        5G_HT40       2G_HT40       2G_HT20 */
+	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8005, 0xd00a8005},
+	{0x00009820, 0x206a002e, 0x206a002e, 0x206a002e, 0x206a002e},
+	{0x00009824, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0},
+	{0x00009828, 0x06903081, 0x06903081, 0x06903881, 0x06903881},
+	{0x0000982c, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4},
+	{0x00009830, 0x0000059c, 0x0000059c, 0x0000059c, 0x0000059c},
+	{0x00009c00, 0x00000044, 0x00000044, 0x00000044, 0x00000044},
+	{0x00009e00, 0x0372161e, 0x0372161e, 0x037216a0, 0x037216a0},
+	{0x00009e04, 0x00182020, 0x00182020, 0x00182020, 0x00182020},
+	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000e2, 0x6c4000e2},
+	{0x00009e10, 0x7ec88d2e, 0x7ec88d2e, 0x7ec80d2e, 0x7ec80d2e},
+	{0x00009e14, 0x31395d5e, 0x3139605e, 0x3139605e, 0x31395d5e},
+	{0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
+	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003ce, 0x000003ce},
+	{0x00009e2c, 0x0000001c, 0x0000001c, 0x00000021, 0x00000021},
+	{0x00009e3c, 0xcf946220, 0xcf946220, 0xcf946222, 0xcf946222},
+	{0x00009e44, 0x02321e27, 0x02321e27, 0x02282324, 0x02282324},
+	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302010, 0x50302010},
+	{0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
+	{0x0000a204, 0x01303fc0, 0x01303fc4, 0x01303fc4, 0x01303fc0},
+	{0x0000a208, 0x00000104, 0x00000104, 0x00000004, 0x00000004},
+	{0x0000a230, 0x0000400a, 0x00004014, 0x00004016, 0x0000400b},
+	{0x0000a234, 0x10000fff, 0x10000fff, 0x10000fff, 0x10000fff},
+	{0x0000a238, 0xffb81018, 0xffb81018, 0xffb81018, 0xffb81018},
+	{0x0000a250, 0x00000000, 0x00000000, 0x00000210, 0x00000108},
+	{0x0000a254, 0x000007d0, 0x00000fa0, 0x00001130, 0x00000898},
+	{0x0000a258, 0x02020002, 0x02020002, 0x02020002, 0x02020002},
+	{0x0000a25c, 0x01000e0e, 0x01000e0e, 0x01000e0e, 0x01000e0e},
+	{0x0000a260, 0x3a021501, 0x3a021501, 0x3a021501, 0x3a021501},
+	{0x0000a264, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0x00000e0e},
+	{0x0000a280, 0x00000007, 0x00000007, 0x0000000b, 0x0000000b},
+	{0x0000a284, 0x00000000, 0x00000000, 0x000002a0, 0x000002a0},
+	{0x0000a288, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a28c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a2c4, 0x00158d18, 0x00158d18, 0x00158d18, 0x00158d18},
+	{0x0000a2d0, 0x00071981, 0x00071981, 0x00071982, 0x00071982},
+	{0x0000a2d8, 0xf999a83a, 0xf999a83a, 0xf999a83a, 0xf999a83a},
+	{0x0000a358, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000be04, 0x00802020, 0x00802020, 0x00802020, 0x00802020},
+	{0x0000be18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+};
+
+static const u32 ar9485Modes_high_ob_db_tx_gain_1_1[][5] = {
+	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20 */
+	{0x000098bc, 0x00000002, 0x00000002, 0x00000002, 0x00000002},
+	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d8, 0x000050d8},
+	{0x0000a458, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a500, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
+	{0x0000a504, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
+	{0x0000a508, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
+	{0x0000a50c, 0x11062202, 0x11062202, 0x0d000200, 0x0d000200},
+	{0x0000a510, 0x17022e00, 0x17022e00, 0x11000202, 0x11000202},
+	{0x0000a514, 0x1d000ec2, 0x1d000ec2, 0x15000400, 0x15000400},
+	{0x0000a518, 0x25020ec0, 0x25020ec0, 0x19000402, 0x19000402},
+	{0x0000a51c, 0x2b020ec3, 0x2b020ec3, 0x1d000404, 0x1d000404},
+	{0x0000a520, 0x2f001f04, 0x2f001f04, 0x21000603, 0x21000603},
+	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x25000605, 0x25000605},
+	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2a000a03, 0x2a000a03},
+	{0x0000a52c, 0x41023e85, 0x41023e85, 0x2c000a04, 0x2c000a04},
+	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x34000e20, 0x34000e20},
+	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x35000e21, 0x35000e21},
+	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x43000e62, 0x43000e62},
+	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x45000e63, 0x45000e63},
+	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x49000e65, 0x49000e65},
+	{0x0000a544, 0x6502feca, 0x6502feca, 0x4b000e66, 0x4b000e66},
+	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x4d001645, 0x4d001645},
+	{0x0000a54c, 0x7203feca, 0x7203feca, 0x51001865, 0x51001865},
+	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x55001a86, 0x55001a86},
+	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x57001ce9, 0x57001ce9},
+	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5a001eeb, 0x5a001eeb},
+	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000b500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b504, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b508, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b50c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b510, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b514, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b518, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b51c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b520, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b524, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b528, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b52c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b530, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b534, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b538, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b53c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b540, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b544, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b548, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b54c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b550, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b554, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b558, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b55c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b560, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b564, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b568, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b56c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b570, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b574, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b578, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b57c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00016044, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db},
+	{0x00016048, 0x6c924260, 0x6c924260, 0x6c924260, 0x6c924260},
+};
+
+static const u32 ar9485_modes_lowest_ob_db_tx_gain_1_1[][5] = {
+	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20  */
+	{0x000098bc, 0x00000002, 0x00000002, 0x00000002, 0x00000002},
+	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d8, 0x000050d8},
+	{0x0000a458, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a500, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
+	{0x0000a504, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
+	{0x0000a508, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
+	{0x0000a50c, 0x11062202, 0x11062202, 0x0d000200, 0x0d000200},
+	{0x0000a510, 0x17022e00, 0x17022e00, 0x11000202, 0x11000202},
+	{0x0000a514, 0x1d000ec2, 0x1d000ec2, 0x15000400, 0x15000400},
+	{0x0000a518, 0x25020ec0, 0x25020ec0, 0x19000402, 0x19000402},
+	{0x0000a51c, 0x2b020ec3, 0x2b020ec3, 0x1d000404, 0x1d000404},
+	{0x0000a520, 0x2f001f04, 0x2f001f04, 0x21000603, 0x21000603},
+	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x25000605, 0x25000605},
+	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2a000a03, 0x2a000a03},
+	{0x0000a52c, 0x41023e85, 0x41023e85, 0x2c000a04, 0x2c000a04},
+	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x34000e20, 0x34000e20},
+	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x35000e21, 0x35000e21},
+	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x43000e62, 0x43000e62},
+	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x45000e63, 0x45000e63},
+	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x49000e65, 0x49000e65},
+	{0x0000a544, 0x6502feca, 0x6502feca, 0x4b000e66, 0x4b000e66},
+	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x4d001645, 0x4d001645},
+	{0x0000a54c, 0x7203feca, 0x7203feca, 0x51001865, 0x51001865},
+	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x55001a86, 0x55001a86},
+	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x57001ce9, 0x57001ce9},
+	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5a001eeb, 0x5a001eeb},
+	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000b500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b504, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b508, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b50c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b510, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b514, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b518, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b51c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b520, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b524, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b528, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b52c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b530, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b534, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b538, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b53c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b540, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b544, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b548, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b54c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b550, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b554, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b558, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b55c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b560, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b564, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b568, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b56c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b570, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b574, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b578, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b57c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00016044, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db},
+	{0x00016048, 0x6c924260, 0x6c924260, 0x6c924260, 0x6c924260},
+};
+
+static const u32 ar9485_1_1_radio_postamble[][2] = {
+	/* Addr        allmodes */
+	{0x0001609c, 0x0b283f31},
+	{0x000160ac, 0x24611800},
+	{0x000160b0, 0x03284f3e},
+	{0x0001610c, 0x00170000},
+	{0x00016140, 0x10804008},
+};
+
+static const u32 ar9485_1_1_mac_postamble[][5] = {
+	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20 */
+	{0x00001030, 0x00000230, 0x00000460, 0x000002c0, 0x00000160},
+	{0x00001070, 0x00000168, 0x000002d0, 0x00000318, 0x0000018c},
+	{0x000010b0, 0x00000e60, 0x00001cc0, 0x00007c70, 0x00003e38},
+	{0x00008014, 0x03e803e8, 0x07d007d0, 0x10801600, 0x08400b00},
+	{0x0000801c, 0x128d8027, 0x128d804f, 0x12e00057, 0x12e0002b},
+	{0x00008120, 0x08f04800, 0x08f04800, 0x08f04810, 0x08f04810},
+	{0x000081d0, 0x00003210, 0x00003210, 0x0000320a, 0x0000320a},
+	{0x00008318, 0x00003e80, 0x00007d00, 0x00006880, 0x00003440},
+};
+
+static const u32 ar9485_1_1_radio_core[][2] = {
+	/* Addr        allmodes */
+	{0x00016000, 0x36db6db6},
+	{0x00016004, 0x6db6db40},
+	{0x00016008, 0x73800000},
+	{0x0001600c, 0x00000000},
+	{0x00016040, 0x7f80fff8},
+	{0x0001604c, 0x000f0278},
+	{0x00016050, 0x4db6db8c},
+	{0x00016054, 0x6db60000},
+	{0x00016080, 0x00080000},
+	{0x00016084, 0x0e48048c},
+	{0x00016088, 0x14214514},
+	{0x0001608c, 0x119f081e},
+	{0x00016090, 0x24926490},
+	{0x00016098, 0xd28b3330},
+	{0x000160a0, 0xc2108ffe},
+	{0x000160a4, 0x812fc370},
+	{0x000160a8, 0x423c8000},
+	{0x000160b4, 0x92480040},
+	{0x000160c0, 0x006db6db},
+	{0x000160c4, 0x0186db60},
+	{0x000160c8, 0x6db6db6c},
+	{0x000160cc, 0x6de6fbe0},
+	{0x000160d0, 0xf7dfcf3c},
+	{0x00016100, 0x04cb0001},
+	{0x00016104, 0xfff80015},
+	{0x00016108, 0x00080010},
+	{0x00016144, 0x01884080},
+	{0x00016148, 0x00008040},
+	{0x00016240, 0x08400000},
+	{0x00016244, 0x1bf90f00},
+	{0x00016248, 0x00000000},
+	{0x0001624c, 0x00000000},
+	{0x00016280, 0x01000015},
+	{0x00016284, 0x00d30000},
+	{0x00016288, 0x00318000},
+	{0x0001628c, 0x50000000},
+	{0x00016290, 0x4b96210f},
+	{0x00016380, 0x00000000},
+	{0x00016384, 0x00000000},
+	{0x00016388, 0x00800700},
+	{0x0001638c, 0x00800700},
+	{0x00016390, 0x00800700},
+	{0x00016394, 0x00000000},
+	{0x00016398, 0x00000000},
+	{0x0001639c, 0x00000000},
+	{0x000163a0, 0x00000001},
+	{0x000163a4, 0x00000001},
+	{0x000163a8, 0x00000000},
+	{0x000163ac, 0x00000000},
+	{0x000163b0, 0x00000000},
+	{0x000163b4, 0x00000000},
+	{0x000163b8, 0x00000000},
+	{0x000163bc, 0x00000000},
+	{0x000163c0, 0x000000a0},
+	{0x000163c4, 0x000c0000},
+	{0x000163c8, 0x14021402},
+	{0x000163cc, 0x00001402},
+	{0x000163d0, 0x00000000},
+	{0x000163d4, 0x00000000},
+	{0x00016c40, 0x13188278},
+	{0x00016c44, 0x12000000},
+};
+
+static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_enable_L1[][2] = {
+	/* Addr        allmodes */
+	{0x00018c00, 0x10052e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
+};
+
+static const u32 ar9485Modes_high_power_tx_gain_1_1[][5] = {
+	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20 */
+	{0x000098bc, 0x00000002, 0x00000002, 0x00000002, 0x00000002},
+	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d8, 0x000050d8},
+	{0x0000a458, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a500, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
+	{0x0000a504, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
+	{0x0000a508, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
+	{0x0000a50c, 0x11062202, 0x11062202, 0x0d000200, 0x0d000200},
+	{0x0000a510, 0x17022e00, 0x17022e00, 0x11000202, 0x11000202},
+	{0x0000a514, 0x1d000ec2, 0x1d000ec2, 0x15000400, 0x15000400},
+	{0x0000a518, 0x25020ec0, 0x25020ec0, 0x19000402, 0x19000402},
+	{0x0000a51c, 0x2b020ec3, 0x2b020ec3, 0x1d000404, 0x1d000404},
+	{0x0000a520, 0x2f001f04, 0x2f001f04, 0x21000603, 0x21000603},
+	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x25000605, 0x25000605},
+	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2a000a03, 0x2a000a03},
+	{0x0000a52c, 0x41023e85, 0x41023e85, 0x2c000a04, 0x2c000a04},
+	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x34000e20, 0x34000e20},
+	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x35000e21, 0x35000e21},
+	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x43000e62, 0x43000e62},
+	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x45000e63, 0x45000e63},
+	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x49000e65, 0x49000e65},
+	{0x0000a544, 0x6502feca, 0x6502feca, 0x4b000e66, 0x4b000e66},
+	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x4d001645, 0x4d001645},
+	{0x0000a54c, 0x7203feca, 0x7203feca, 0x51001865, 0x51001865},
+	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x55001a86, 0x55001a86},
+	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x57001ce9, 0x57001ce9},
+	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5a001eeb, 0x5a001eeb},
+	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000b500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b504, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b508, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b50c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b510, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b514, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b518, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b51c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b520, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b524, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b528, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b52c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b530, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b534, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b538, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b53c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b540, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b544, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b548, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b54c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b550, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b554, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b558, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b55c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b560, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b564, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b568, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b56c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b570, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b574, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b578, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b57c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00016044, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db},
+	{0x00016048, 0x6c924260, 0x6c924260, 0x6c924260, 0x6c924260},
+};
+
+static const u32 ar9485_1_1[][2] = {
+	/* Addr        allmodes */
+	{0x0000a580, 0x00000000},
+	{0x0000a584, 0x00000000},
+	{0x0000a588, 0x00000000},
+	{0x0000a58c, 0x00000000},
+	{0x0000a590, 0x00000000},
+	{0x0000a594, 0x00000000},
+	{0x0000a598, 0x00000000},
+	{0x0000a59c, 0x00000000},
+	{0x0000a5a0, 0x00000000},
+	{0x0000a5a4, 0x00000000},
+	{0x0000a5a8, 0x00000000},
+	{0x0000a5ac, 0x00000000},
+	{0x0000a5b0, 0x00000000},
+	{0x0000a5b4, 0x00000000},
+	{0x0000a5b8, 0x00000000},
+	{0x0000a5bc, 0x00000000},
+};
+
+static const u32 ar9485_modes_green_ob_db_tx_gain_1_1[][5] = {
+	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20 */
+	{0x000098bc, 0x00000003, 0x00000003, 0x00000003, 0x00000003},
+	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d8, 0x000050d8},
+	{0x0000a458, 0x80000000, 0x80000000, 0x80000000, 0x80000000},
+	{0x0000a500, 0x00022200, 0x00022200, 0x00000006, 0x00000006},
+	{0x0000a504, 0x05062002, 0x05062002, 0x03000201, 0x03000201},
+	{0x0000a508, 0x0c002e00, 0x0c002e00, 0x06000203, 0x06000203},
+	{0x0000a50c, 0x11062202, 0x11062202, 0x0a000401, 0x0a000401},
+	{0x0000a510, 0x17022e00, 0x17022e00, 0x0e000403, 0x0e000403},
+	{0x0000a514, 0x1d000ec2, 0x1d000ec2, 0x12000405, 0x12000405},
+	{0x0000a518, 0x25020ec0, 0x25020ec0, 0x15000604, 0x15000604},
+	{0x0000a51c, 0x2b020ec3, 0x2b020ec3, 0x18000605, 0x18000605},
+	{0x0000a520, 0x2f001f04, 0x2f001f04, 0x1c000a04, 0x1c000a04},
+	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x21000a06, 0x21000a06},
+	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x29000a24, 0x29000a24},
+	{0x0000a52c, 0x41023e85, 0x41023e85, 0x2f000e21, 0x2f000e21},
+	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x31000e20, 0x31000e20},
+	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x33000e20, 0x33000e20},
+	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x43000e62, 0x43000e62},
+	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x45000e63, 0x45000e63},
+	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x49000e65, 0x49000e65},
+	{0x0000a544, 0x6502feca, 0x6502feca, 0x4b000e66, 0x4b000e66},
+	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x4d001645, 0x4d001645},
+	{0x0000a54c, 0x7203feca, 0x7203feca, 0x51001865, 0x51001865},
+	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x55001a86, 0x55001a86},
+	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x57001ce9, 0x57001ce9},
+	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5a001eeb, 0x5a001eeb},
+	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000b500, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b504, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b508, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b50c, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b510, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b514, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b518, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b51c, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b520, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b524, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b528, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a},
+	{0x0000b52c, 0x0000002a, 0x0000002a, 0x0000002a, 0x0000002a},
+	{0x0000b530, 0x0000003a, 0x0000003a, 0x0000003a, 0x0000003a},
+	{0x0000b534, 0x0000004a, 0x0000004a, 0x0000004a, 0x0000004a},
+	{0x0000b538, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b53c, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b540, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b544, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b548, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b54c, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b550, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b554, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b558, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b55c, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b560, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b564, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b568, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b56c, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b570, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b574, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b578, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x0000b57c, 0x0000005b, 0x0000005b, 0x0000005b, 0x0000005b},
+	{0x00016044, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db},
+	{0x00016048, 0x6c924260, 0x6c924260, 0x6c924260, 0x6c924260},
+};
+
+static const u32 ar9485_1_1_pcie_phy_clkreq_disable_L1[][2] = {
+	/* Addr        allmodes */
+	{0x00018c00, 0x10013e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
+};
+
+static const u32 ar9485_1_1_soc_preamble[][2] = {
+	/* Addr        allmodes */
+	{0x00004014, 0xba280400},
+	{0x000040a4, 0x00a0c9c9},
+	{0x00007010, 0x00000022},
+	{0x00007020, 0x00000000},
+	{0x00007034, 0x00000002},
+	{0x00007038, 0x000004c2},
+	{0x00007048, 0x00000002},
+};
+
+static const u32 ar9485_1_1_baseband_core_txfir_coeff_japan_2484[][2] = {
+	/* Addr        allmodes */
+	{0x0000a398, 0x00000000},
+	{0x0000a39c, 0x6f7f0301},
+	{0x0000a3a0, 0xca9228ee},
+};
+
+static const u32 ar9485Modes_low_ob_db_tx_gain_1_1[][5] = {
+	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20  */
+	{0x000098bc, 0x00000002, 0x00000002, 0x00000002, 0x00000002},
+	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d8, 0x000050d8},
+	{0x0000a458, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a500, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
+	{0x0000a504, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
+	{0x0000a508, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
+	{0x0000a50c, 0x11062202, 0x11062202, 0x0d000200, 0x0d000200},
+	{0x0000a510, 0x17022e00, 0x17022e00, 0x11000202, 0x11000202},
+	{0x0000a514, 0x1d000ec2, 0x1d000ec2, 0x15000400, 0x15000400},
+	{0x0000a518, 0x25020ec0, 0x25020ec0, 0x19000402, 0x19000402},
+	{0x0000a51c, 0x2b020ec3, 0x2b020ec3, 0x1d000404, 0x1d000404},
+	{0x0000a520, 0x2f001f04, 0x2f001f04, 0x21000603, 0x21000603},
+	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x25000605, 0x25000605},
+	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2a000a03, 0x2a000a03},
+	{0x0000a52c, 0x41023e85, 0x41023e85, 0x2c000a04, 0x2c000a04},
+	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x34000e20, 0x34000e20},
+	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x35000e21, 0x35000e21},
+	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x43000e62, 0x43000e62},
+	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x45000e63, 0x45000e63},
+	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x49000e65, 0x49000e65},
+	{0x0000a544, 0x6502feca, 0x6502feca, 0x4b000e66, 0x4b000e66},
+	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x4d001645, 0x4d001645},
+	{0x0000a54c, 0x7203feca, 0x7203feca, 0x51001865, 0x51001865},
+	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x55001a86, 0x55001a86},
+	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x57001ce9, 0x57001ce9},
+	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5a001eeb, 0x5a001eeb},
+	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x5e001eeb, 0x5e001eeb},
+	{0x0000b500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b504, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b508, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b50c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b510, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b514, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b518, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b51c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b520, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b524, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b528, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b52c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b530, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b534, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b538, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b53c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b540, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b544, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b548, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b54c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b550, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b554, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b558, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b55c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b560, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b564, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b568, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b56c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b570, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b574, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b578, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000b57c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00016044, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db, 0x05d6b2db},
+	{0x00016048, 0x6c924260, 0x6c924260, 0x6c924260, 0x6c924260},
+};
+
+static const u32 ar9485_fast_clock_1_1_baseband_postamble[][3] = {
+	/* Addr        5G_HT2        5G_HT40  */
+	{0x00009e00, 0x03721821, 0x03721821},
+	{0x0000a230, 0x0000400b, 0x00004016},
+	{0x0000a254, 0x00000898, 0x00001130},
+};
+
+static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_disable_L1[][2] = {
+	/* Addr        allmodes  */
+	{0x00018c00, 0x10012e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
+};
+
+static const u32 ar9485_common_rx_gain_1_1[][2] = {
+	/* Addr        allmodes */
+	{0x0000a000, 0x00010000},
+	{0x0000a004, 0x00030002},
+	{0x0000a008, 0x00050004},
+	{0x0000a00c, 0x00810080},
+	{0x0000a010, 0x01800082},
+	{0x0000a014, 0x01820181},
+	{0x0000a018, 0x01840183},
+	{0x0000a01c, 0x01880185},
+	{0x0000a020, 0x018a0189},
+	{0x0000a024, 0x02850284},
+	{0x0000a028, 0x02890288},
+	{0x0000a02c, 0x03850384},
+	{0x0000a030, 0x03890388},
+	{0x0000a034, 0x038b038a},
+	{0x0000a038, 0x038d038c},
+	{0x0000a03c, 0x03910390},
+	{0x0000a040, 0x03930392},
+	{0x0000a044, 0x03950394},
+	{0x0000a048, 0x00000396},
+	{0x0000a04c, 0x00000000},
+	{0x0000a050, 0x00000000},
+	{0x0000a054, 0x00000000},
+	{0x0000a058, 0x00000000},
+	{0x0000a05c, 0x00000000},
+	{0x0000a060, 0x00000000},
+	{0x0000a064, 0x00000000},
+	{0x0000a068, 0x00000000},
+	{0x0000a06c, 0x00000000},
+	{0x0000a070, 0x00000000},
+	{0x0000a074, 0x00000000},
+	{0x0000a078, 0x00000000},
+	{0x0000a07c, 0x00000000},
+	{0x0000a080, 0x28282828},
+	{0x0000a084, 0x28282828},
+	{0x0000a088, 0x28282828},
+	{0x0000a08c, 0x28282828},
+	{0x0000a090, 0x28282828},
+	{0x0000a094, 0x21212128},
+	{0x0000a098, 0x171c1c1c},
+	{0x0000a09c, 0x02020212},
+	{0x0000a0a0, 0x00000202},
+	{0x0000a0a4, 0x00000000},
+	{0x0000a0a8, 0x00000000},
+	{0x0000a0ac, 0x00000000},
+	{0x0000a0b0, 0x00000000},
+	{0x0000a0b4, 0x00000000},
+	{0x0000a0b8, 0x00000000},
+	{0x0000a0bc, 0x00000000},
+	{0x0000a0c0, 0x001f0000},
+	{0x0000a0c4, 0x111f1100},
+	{0x0000a0c8, 0x111d111e},
+	{0x0000a0cc, 0x111b111c},
+	{0x0000a0d0, 0x22032204},
+	{0x0000a0d4, 0x22012202},
+	{0x0000a0d8, 0x221f2200},
+	{0x0000a0dc, 0x221d221e},
+	{0x0000a0e0, 0x33013302},
+	{0x0000a0e4, 0x331f3300},
+	{0x0000a0e8, 0x4402331e},
+	{0x0000a0ec, 0x44004401},
+	{0x0000a0f0, 0x441e441f},
+	{0x0000a0f4, 0x55015502},
+	{0x0000a0f8, 0x551f5500},
+	{0x0000a0fc, 0x6602551e},
+	{0x0000a100, 0x66006601},
+	{0x0000a104, 0x661e661f},
+	{0x0000a108, 0x7703661d},
+	{0x0000a10c, 0x77017702},
+	{0x0000a110, 0x00007700},
+	{0x0000a114, 0x00000000},
+	{0x0000a118, 0x00000000},
+	{0x0000a11c, 0x00000000},
+	{0x0000a120, 0x00000000},
+	{0x0000a124, 0x00000000},
+	{0x0000a128, 0x00000000},
+	{0x0000a12c, 0x00000000},
+	{0x0000a130, 0x00000000},
+	{0x0000a134, 0x00000000},
+	{0x0000a138, 0x00000000},
+	{0x0000a13c, 0x00000000},
+	{0x0000a140, 0x001f0000},
+	{0x0000a144, 0x111f1100},
+	{0x0000a148, 0x111d111e},
+	{0x0000a14c, 0x111b111c},
+	{0x0000a150, 0x22032204},
+	{0x0000a154, 0x22012202},
+	{0x0000a158, 0x221f2200},
+	{0x0000a15c, 0x221d221e},
+	{0x0000a160, 0x33013302},
+	{0x0000a164, 0x331f3300},
+	{0x0000a168, 0x4402331e},
+	{0x0000a16c, 0x44004401},
+	{0x0000a170, 0x441e441f},
+	{0x0000a174, 0x55015502},
+	{0x0000a178, 0x551f5500},
+	{0x0000a17c, 0x6602551e},
+	{0x0000a180, 0x66006601},
+	{0x0000a184, 0x661e661f},
+	{0x0000a188, 0x7703661d},
+	{0x0000a18c, 0x77017702},
+	{0x0000a190, 0x00007700},
+	{0x0000a194, 0x00000000},
+	{0x0000a198, 0x00000000},
+	{0x0000a19c, 0x00000000},
+	{0x0000a1a0, 0x00000000},
+	{0x0000a1a4, 0x00000000},
+	{0x0000a1a8, 0x00000000},
+	{0x0000a1ac, 0x00000000},
+	{0x0000a1b0, 0x00000000},
+	{0x0000a1b4, 0x00000000},
+	{0x0000a1b8, 0x00000000},
+	{0x0000a1bc, 0x00000000},
+	{0x0000a1c0, 0x00000000},
+	{0x0000a1c4, 0x00000000},
+	{0x0000a1c8, 0x00000000},
+	{0x0000a1cc, 0x00000000},
+	{0x0000a1d0, 0x00000000},
+	{0x0000a1d4, 0x00000000},
+	{0x0000a1d8, 0x00000000},
+	{0x0000a1dc, 0x00000000},
+	{0x0000a1e0, 0x00000000},
+	{0x0000a1e4, 0x00000000},
+	{0x0000a1e8, 0x00000000},
+	{0x0000a1ec, 0x00000000},
+	{0x0000a1f0, 0x00000396},
+	{0x0000a1f4, 0x00000396},
+	{0x0000a1f8, 0x00000396},
+	{0x0000a1fc, 0x00000296},
+};
+
+static const u32 ar9485_1_1_pcie_phy_clkreq_enable_L1[][2] = {
+	/* Addr        allmodes */
+	{0x00018c00, 0x10053e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
+};
+
+static const u32 ar9485Common_wo_xlna_rx_gain_1_1[][2] = {
+	/* Addr        allmodes */
+	{0x0000a000, 0x00060005},
+	{0x0000a004, 0x00810080},
+	{0x0000a008, 0x00830082},
+	{0x0000a00c, 0x00850084},
+	{0x0000a010, 0x01820181},
+	{0x0000a014, 0x01840183},
+	{0x0000a018, 0x01880185},
+	{0x0000a01c, 0x018a0189},
+	{0x0000a020, 0x02850284},
+	{0x0000a024, 0x02890288},
+	{0x0000a028, 0x028b028a},
+	{0x0000a02c, 0x03850384},
+	{0x0000a030, 0x03890388},
+	{0x0000a034, 0x038b038a},
+	{0x0000a038, 0x038d038c},
+	{0x0000a03c, 0x03910390},
+	{0x0000a040, 0x03930392},
+	{0x0000a044, 0x03950394},
+	{0x0000a048, 0x00000396},
+	{0x0000a04c, 0x00000000},
+	{0x0000a050, 0x00000000},
+	{0x0000a054, 0x00000000},
+	{0x0000a058, 0x00000000},
+	{0x0000a05c, 0x00000000},
+	{0x0000a060, 0x00000000},
+	{0x0000a064, 0x00000000},
+	{0x0000a068, 0x00000000},
+	{0x0000a06c, 0x00000000},
+	{0x0000a070, 0x00000000},
+	{0x0000a074, 0x00000000},
+	{0x0000a078, 0x00000000},
+	{0x0000a07c, 0x00000000},
+	{0x0000a080, 0x28282828},
+	{0x0000a084, 0x28282828},
+	{0x0000a088, 0x28282828},
+	{0x0000a08c, 0x28282828},
+	{0x0000a090, 0x28282828},
+	{0x0000a094, 0x24242428},
+	{0x0000a098, 0x171e1e1e},
+	{0x0000a09c, 0x02020b0b},
+	{0x0000a0a0, 0x02020202},
+	{0x0000a0a4, 0x00000000},
+	{0x0000a0a8, 0x00000000},
+	{0x0000a0ac, 0x00000000},
+	{0x0000a0b0, 0x00000000},
+	{0x0000a0b4, 0x00000000},
+	{0x0000a0b8, 0x00000000},
+	{0x0000a0bc, 0x00000000},
+	{0x0000a0c0, 0x22072208},
+	{0x0000a0c4, 0x22052206},
+	{0x0000a0c8, 0x22032204},
+	{0x0000a0cc, 0x22012202},
+	{0x0000a0d0, 0x221f2200},
+	{0x0000a0d4, 0x221d221e},
+	{0x0000a0d8, 0x33023303},
+	{0x0000a0dc, 0x33003301},
+	{0x0000a0e0, 0x331e331f},
+	{0x0000a0e4, 0x4402331d},
+	{0x0000a0e8, 0x44004401},
+	{0x0000a0ec, 0x441e441f},
+	{0x0000a0f0, 0x55025503},
+	{0x0000a0f4, 0x55005501},
+	{0x0000a0f8, 0x551e551f},
+	{0x0000a0fc, 0x6602551d},
+	{0x0000a100, 0x66006601},
+	{0x0000a104, 0x661e661f},
+	{0x0000a108, 0x7703661d},
+	{0x0000a10c, 0x77017702},
+	{0x0000a110, 0x00007700},
+	{0x0000a114, 0x00000000},
+	{0x0000a118, 0x00000000},
+	{0x0000a11c, 0x00000000},
+	{0x0000a120, 0x00000000},
+	{0x0000a124, 0x00000000},
+	{0x0000a128, 0x00000000},
+	{0x0000a12c, 0x00000000},
+	{0x0000a130, 0x00000000},
+	{0x0000a134, 0x00000000},
+	{0x0000a138, 0x00000000},
+	{0x0000a13c, 0x00000000},
+	{0x0000a140, 0x001f0000},
+	{0x0000a144, 0x111f1100},
+	{0x0000a148, 0x111d111e},
+	{0x0000a14c, 0x111b111c},
+	{0x0000a150, 0x22032204},
+	{0x0000a154, 0x22012202},
+	{0x0000a158, 0x221f2200},
+	{0x0000a15c, 0x221d221e},
+	{0x0000a160, 0x33013302},
+	{0x0000a164, 0x331f3300},
+	{0x0000a168, 0x4402331e},
+	{0x0000a16c, 0x44004401},
+	{0x0000a170, 0x441e441f},
+	{0x0000a174, 0x55015502},
+	{0x0000a178, 0x551f5500},
+	{0x0000a17c, 0x6602551e},
+	{0x0000a180, 0x66006601},
+	{0x0000a184, 0x661e661f},
+	{0x0000a188, 0x7703661d},
+	{0x0000a18c, 0x77017702},
+	{0x0000a190, 0x00007700},
+	{0x0000a194, 0x00000000},
+	{0x0000a198, 0x00000000},
+	{0x0000a19c, 0x00000000},
+	{0x0000a1a0, 0x00000000},
+	{0x0000a1a4, 0x00000000},
+	{0x0000a1a8, 0x00000000},
+	{0x0000a1ac, 0x00000000},
+	{0x0000a1b0, 0x00000000},
+	{0x0000a1b4, 0x00000000},
+	{0x0000a1b8, 0x00000000},
+	{0x0000a1bc, 0x00000000},
+	{0x0000a1c0, 0x00000000},
+	{0x0000a1c4, 0x00000000},
+	{0x0000a1c8, 0x00000000},
+	{0x0000a1cc, 0x00000000},
+	{0x0000a1d0, 0x00000000},
+	{0x0000a1d4, 0x00000000},
+	{0x0000a1d8, 0x00000000},
+	{0x0000a1dc, 0x00000000},
+	{0x0000a1e0, 0x00000000},
+	{0x0000a1e4, 0x00000000},
+	{0x0000a1e8, 0x00000000},
+	{0x0000a1ec, 0x00000000},
+	{0x0000a1f0, 0x00000396},
+	{0x0000a1f4, 0x00000396},
+	{0x0000a1f8, 0x00000396},
+	{0x0000a1fc, 0x00000296},
+};
+
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 23838e37d45f..c718ab512a97 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -21,7 +21,6 @@
 #include <linux/device.h>
 #include <linux/leds.h>
 #include <linux/completion.h>
-#include <linux/pm_qos_params.h>
 
 #include "debug.h"
 #include "common.h"
@@ -57,8 +56,6 @@ struct ath_node;
 
 #define A_MAX(a, b) ((a) > (b) ? (a) : (b))
 
-#define ATH9K_PM_QOS_DEFAULT_VALUE	55
-
 #define TSF_TO_TU(_h,_l) \
 	((((u32)(_h)) << 22) | (((u32)(_l)) >> 10))
 
@@ -95,9 +92,9 @@ struct ath_config {
  * @BUF_XRETRY: To denote excessive retries of the buffer
  */
 enum buffer_type {
-	BUF_AMPDU		= BIT(2),
-	BUF_AGGR		= BIT(3),
-	BUF_XRETRY		= BIT(5),
+	BUF_AMPDU		= BIT(0),
+	BUF_AGGR		= BIT(1),
+	BUF_XRETRY		= BIT(2),
 };
 
 #define bf_isampdu(bf)		(bf->bf_state.bf_type & BUF_AMPDU)
@@ -137,7 +134,6 @@ void ath_descdma_cleanup(struct ath_softc *sc, struct ath_descdma *dd,
 	 (((_tid) == 4) || ((_tid) == 5)) ? WME_AC_VI :	\
 	 WME_AC_VO)
 
-#define ADDBA_EXCHANGE_ATTEMPTS    10
 #define ATH_AGGR_DELIM_SZ          4
 #define ATH_AGGR_MINPLEN           256 /* in bytes, minimum packet length */
 /* number of delimiters for encryption padding */
@@ -184,7 +180,8 @@ enum ATH_AGGR_STATUS {
 
 #define ATH_TXFIFO_DEPTH 8
 struct ath_txq {
-	u32 axq_qnum;
+	int mac80211_qnum; /* mac80211 queue number, -1 means not mac80211 Q */
+	u32 axq_qnum; /* ath9k hardware queue number */
 	u32 *axq_link;
 	struct list_head axq_q;
 	spinlock_t axq_lock;
@@ -192,6 +189,7 @@ struct ath_txq {
 	u32 axq_ampdu_depth;
 	bool stopped;
 	bool axq_tx_inprogress;
+	bool txq_flush_inprogress;
 	struct list_head axq_acq;
 	struct list_head txq_fifo[ATH_TXFIFO_DEPTH];
 	struct list_head txq_fifo_pending;
@@ -234,7 +232,6 @@ struct ath_buf {
 	bool bf_stale;
 	u16 bf_flags;
 	struct ath_buf_state bf_state;
-	struct ath_wiphy *aphy;
 };
 
 struct ath_atx_tid {
@@ -255,7 +252,10 @@ struct ath_atx_tid {
 };
 
 struct ath_node {
-	struct ath_common *common;
+#ifdef CONFIG_ATH9K_DEBUGFS
+	struct list_head list; /* for sc->nodes */
+	struct ieee80211_sta *sta; /* station struct we're part of */
+#endif
 	struct ath_atx_tid tid[WME_NUM_TID];
 	struct ath_atx_ac ac[WME_NUM_AC];
 	u16 maxampdu;
@@ -278,6 +278,11 @@ struct ath_tx_control {
 #define ATH_TX_XRETRY       0x02
 #define ATH_TX_BAR          0x04
 
+/**
+ * @txq_map:  Index is mac80211 queue number.  This is
+ *  not necessarily the same as the hardware queue number
+ *  (axq_qnum).
+ */
 struct ath_tx {
 	u16 seq_no;
 	u32 txqsetup;
@@ -304,6 +309,8 @@ struct ath_rx {
 	struct ath_descdma rxdma;
 	struct ath_buf *rx_bufptr;
 	struct ath_rx_edma rx_edma[ATH9K_RX_QUEUE_MAX];
+
+	struct sk_buff *frag;
 };
 
 int ath_startrecv(struct ath_softc *sc);
@@ -340,10 +347,10 @@ void ath_tx_aggr_resume(struct ath_softc *sc, struct ieee80211_sta *sta, u16 tid
 
 struct ath_vif {
 	int av_bslot;
+	bool is_bslot_active;
 	__le64 tsf_adjust; /* TSF adjustment for staggered beacons */
 	enum nl80211_iftype av_opmode;
 	struct ath_buf *av_bcbuf;
-	struct ath_tx_control av_btxctl;
 	u8 bssid[ETH_ALEN]; /* current BSSID from config_interface */
 };
 
@@ -363,7 +370,7 @@ struct ath_vif {
 #define IEEE80211_MS_TO_TU(x)           (((x) * 1000) / 1024)
 
 struct ath_beacon_config {
-	u16 beacon_interval;
+	int beacon_interval;
 	u16 listen_interval;
 	u16 dtim_period;
 	u16 bmiss_timeout;
@@ -382,7 +389,6 @@ struct ath_beacon {
 	u32 ast_be_xmit;
 	u64 bc_tstamp;
 	struct ieee80211_vif *bslot[ATH_BCBUF];
-	struct ath_wiphy *bslot_aphy[ATH_BCBUF];
 	int slottime;
 	int slotupdate;
 	struct ath9k_tx_queue_info beacon_qi;
@@ -393,9 +399,10 @@ struct ath_beacon {
 
 void ath_beacon_tasklet(unsigned long data);
 void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif);
-int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif);
+int ath_beacon_alloc(struct ath_softc *sc, struct ieee80211_vif *vif);
 void ath_beacon_return(struct ath_softc *sc, struct ath_vif *avp);
 int ath_beaconq_config(struct ath_softc *sc);
+void ath9k_set_beaconing_status(struct ath_softc *sc, bool status);
 
 /*******/
 /* ANI */
@@ -442,26 +449,21 @@ void ath9k_btcoex_timer_pause(struct ath_softc *sc);
 
 #define ATH_LED_PIN_DEF 		1
 #define ATH_LED_PIN_9287		8
-#define ATH_LED_ON_DURATION_IDLE	350	/* in msecs */
-#define ATH_LED_OFF_DURATION_IDLE	250	/* in msecs */
-
-enum ath_led_type {
-	ATH_LED_RADIO,
-	ATH_LED_ASSOC,
-	ATH_LED_TX,
-	ATH_LED_RX
-};
-
-struct ath_led {
-	struct ath_softc *sc;
-	struct led_classdev led_cdev;
-	enum ath_led_type led_type;
-	char name[32];
-	bool registered;
-};
+#define ATH_LED_PIN_9485		6
 
+#ifdef CONFIG_MAC80211_LEDS
 void ath_init_leds(struct ath_softc *sc);
 void ath_deinit_leds(struct ath_softc *sc);
+#else
+static inline void ath_init_leds(struct ath_softc *sc)
+{
+}
+
+static inline void ath_deinit_leds(struct ath_softc *sc)
+{
+}
+#endif
+
 
 /* Antenna diversity/combining */
 #define ATH_ANT_RX_CURRENT_SHIFT 4
@@ -530,7 +532,6 @@ struct ath_ant_comb {
 #define ATH_CABQ_READY_TIME     80      /* % of beacon interval */
 #define ATH_MAX_SW_RETRIES      10
 #define ATH_CHAN_MAX            255
-#define IEEE80211_WEP_NKID      4       /* number of key ids */
 
 #define ATH_TXPOWER_MAX         100     /* .5 dBm units */
 #define ATH_RATE_DUMMY_MARKER   0
@@ -558,27 +559,28 @@ struct ath_ant_comb {
 #define PS_WAIT_FOR_TX_ACK        BIT(3)
 #define PS_BEACON_SYNC            BIT(4)
 
-struct ath_wiphy;
 struct ath_rate_table;
 
+struct ath9k_vif_iter_data {
+	const u8 *hw_macaddr; /* phy's hardware address, set
+			       * before starting iteration for
+			       * valid bssid mask.
+			       */
+	u8 mask[ETH_ALEN]; /* bssid mask */
+	int naps;      /* number of AP vifs */
+	int nmeshes;   /* number of mesh vifs */
+	int nstations; /* number of station vifs */
+	int nwds;      /* number of nwd vifs */
+	int nadhocs;   /* number of adhoc vifs */
+	int nothers;   /* number of vifs not specified above. */
+};
+
 struct ath_softc {
 	struct ieee80211_hw *hw;
 	struct device *dev;
 
-	spinlock_t wiphy_lock; /* spinlock to protect ath_wiphy data */
-	struct ath_wiphy *pri_wiphy;
-	struct ath_wiphy **sec_wiphy; /* secondary wiphys (virtual radios); may
-				       * have NULL entries */
-	int num_sec_wiphy; /* number of sec_wiphy pointers in the array */
 	int chan_idx;
 	int chan_is_ht;
-	struct ath_wiphy *next_wiphy;
-	struct work_struct chan_work;
-	int wiphy_select_failures;
-	unsigned long wiphy_select_first_fail;
-	struct delayed_work wiphy_work;
-	unsigned long wiphy_scheduler_int;
-	int wiphy_scheduler_index;
 	struct survey_info *cur_survey;
 	struct survey_info survey[ATH9K_NUM_CHANNELS];
 
@@ -595,14 +597,16 @@ struct ath_softc {
 	struct work_struct hw_check_work;
 	struct completion paprd_complete;
 
+	unsigned int hw_busy_count;
+
 	u32 intrstatus;
 	u32 sc_flags; /* SC_OP_* */
 	u16 ps_flags; /* PS_* */
 	u16 curtxpow;
-	u8 nbcnvifs;
-	u16 nvifs;
 	bool ps_enabled;
 	bool ps_idle;
+	short nbcnvifs;
+	short nvifs;
 	unsigned long ps_usecount;
 
 	struct ath_config config;
@@ -611,47 +615,29 @@ struct ath_softc {
 	struct ath_beacon beacon;
 	struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
 
-	struct ath_led radio_led;
-	struct ath_led assoc_led;
-	struct ath_led tx_led;
-	struct ath_led rx_led;
-	struct delayed_work ath_led_blink_work;
-	int led_on_duration;
-	int led_off_duration;
-	int led_on_cnt;
-	int led_off_cnt;
+#ifdef CONFIG_MAC80211_LEDS
+	bool led_registered;
+	char led_name[32];
+	struct led_classdev led_cdev;
+#endif
 
-	int beacon_interval;
+	struct ath9k_hw_cal_data caldata;
+	int last_rssi;
 
 #ifdef CONFIG_ATH9K_DEBUGFS
 	struct ath9k_debug debug;
+	spinlock_t nodes_lock;
+	struct list_head nodes; /* basically, stations */
+	unsigned int tx_complete_poll_work_seen;
 #endif
 	struct ath_beacon_config cur_beacon_conf;
 	struct delayed_work tx_complete_work;
+	struct delayed_work hw_pll_work;
 	struct ath_btcoex btcoex;
 
 	struct ath_descdma txsdma;
 
 	struct ath_ant_comb ant_comb;
-
-	struct pm_qos_request_list pm_qos_req;
-};
-
-struct ath_wiphy {
-	struct ath_softc *sc; /* shared for all virtual wiphys */
-	struct ieee80211_hw *hw;
-	struct ath9k_hw_cal_data caldata;
-	enum ath_wiphy_state {
-		ATH_WIPHY_INACTIVE,
-		ATH_WIPHY_ACTIVE,
-		ATH_WIPHY_PAUSING,
-		ATH_WIPHY_PAUSED,
-		ATH_WIPHY_SCAN,
-	} state;
-	bool idle;
-	int chan_idx;
-	int chan_is_ht;
-	int last_rssi;
 };
 
 void ath9k_tasklet(unsigned long data);
@@ -666,7 +652,6 @@ static inline void ath_read_cachesize(struct ath_common *common, int *csz)
 extern struct ieee80211_ops ath9k_ops;
 extern int ath9k_modparam_nohwcrypt;
 extern int led_blink;
-extern int ath9k_pm_qos_value;
 extern bool is_ath9k_unloaded;
 
 irqreturn_t ath_isr(int irq, void *dev);
@@ -675,14 +660,13 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc, u16 subsysid,
 		    const struct ath_bus_ops *bus_ops);
 void ath9k_deinit_device(struct ath_softc *sc);
 void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw);
-void ath9k_update_ichannel(struct ath_softc *sc, struct ieee80211_hw *hw,
-			   struct ath9k_channel *ichan);
 int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 		    struct ath9k_channel *hchan);
 
 void ath_radio_enable(struct ath_softc *sc, struct ieee80211_hw *hw);
 void ath_radio_disable(struct ath_softc *sc, struct ieee80211_hw *hw);
 bool ath9k_setpower(struct ath_softc *sc, enum ath9k_power_mode mode);
+bool ath9k_uses_beacons(int type);
 
 #ifdef CONFIG_PCI
 int ath_pci_init(void);
@@ -706,26 +690,12 @@ void ath9k_ps_restore(struct ath_softc *sc);
 u8 ath_txchainmask_reduction(struct ath_softc *sc, u8 chainmask, u32 rate);
 
 void ath9k_set_bssid_mask(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
-int ath9k_wiphy_add(struct ath_softc *sc);
-int ath9k_wiphy_del(struct ath_wiphy *aphy);
-void ath9k_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb, int ftype);
-int ath9k_wiphy_pause(struct ath_wiphy *aphy);
-int ath9k_wiphy_unpause(struct ath_wiphy *aphy);
-int ath9k_wiphy_select(struct ath_wiphy *aphy);
-void ath9k_wiphy_set_scheduler(struct ath_softc *sc, unsigned int msec_int);
-void ath9k_wiphy_chan_work(struct work_struct *work);
-bool ath9k_wiphy_started(struct ath_softc *sc);
-void ath9k_wiphy_pause_all_forced(struct ath_softc *sc,
-				  struct ath_wiphy *selected);
-bool ath9k_wiphy_scanning(struct ath_softc *sc);
-void ath9k_wiphy_work(struct work_struct *work);
-bool ath9k_all_wiphys_idle(struct ath_softc *sc);
-void ath9k_set_wiphy_idle(struct ath_wiphy *aphy, bool idle);
-
-void ath_mac80211_stop_queue(struct ath_softc *sc, u16 skb_queue);
-bool ath_mac80211_start_queue(struct ath_softc *sc, u16 skb_queue);
 
 void ath_start_rfkill_poll(struct ath_softc *sc);
 extern void ath9k_rfkill_poll_state(struct ieee80211_hw *hw);
+void ath9k_calculate_iter_data(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct ath9k_vif_iter_data *iter_data);
+
 
 #endif /* ATH9K_H */
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 385ba03134ba..a4bdfdb043ef 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -112,8 +112,7 @@ static void ath_beacon_setup(struct ath_softc *sc, struct ath_vif *avp,
 
 static void ath_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_tx_control txctl;
 
@@ -132,8 +131,7 @@ static void ath_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
 static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
 					   struct ieee80211_vif *vif)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_buf *bf;
 	struct ath_vif *avp;
@@ -142,13 +140,10 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
 	struct ieee80211_tx_info *info;
 	int cabq_depth;
 
-	if (aphy->state != ATH_WIPHY_ACTIVE)
-		return NULL;
-
 	avp = (void *)vif->drv_priv;
 	cabq = sc->beacon.cabq;
 
-	if (avp->av_bcbuf == NULL)
+	if ((avp->av_bcbuf == NULL) || !avp->is_bslot_active)
 		return NULL;
 
 	/* Release the old beacon first */
@@ -225,13 +220,13 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
 	return bf;
 }
 
-int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
+int ath_beacon_alloc(struct ath_softc *sc, struct ieee80211_vif *vif)
 {
-	struct ath_softc *sc = aphy->sc;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_vif *avp;
 	struct ath_buf *bf;
 	struct sk_buff *skb;
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 	__le64 tstamp;
 
 	avp = (void *)vif->drv_priv;
@@ -244,9 +239,7 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
 						 struct ath_buf, list);
 		list_del(&avp->av_bcbuf->list);
 
-		if (sc->sc_ah->opmode == NL80211_IFTYPE_AP ||
-		    sc->sc_ah->opmode == NL80211_IFTYPE_ADHOC ||
-		    sc->sc_ah->opmode == NL80211_IFTYPE_MESH_POINT) {
+		if (ath9k_uses_beacons(vif->type)) {
 			int slot;
 			/*
 			 * Assign the vif to a beacon xmit slot. As
@@ -256,6 +249,7 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
 			for (slot = 0; slot < ATH_BCBUF; slot++)
 				if (sc->beacon.bslot[slot] == NULL) {
 					avp->av_bslot = slot;
+					avp->is_bslot_active = false;
 
 					/* NB: keep looking for a double slot */
 					if (slot == 0 || !sc->beacon.bslot[slot-1])
@@ -263,7 +257,6 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
 				}
 			BUG_ON(sc->beacon.bslot[avp->av_bslot] != NULL);
 			sc->beacon.bslot[avp->av_bslot] = vif;
-			sc->beacon.bslot_aphy[avp->av_bslot] = aphy;
 			sc->nbcnvifs++;
 		}
 	}
@@ -281,10 +274,8 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
 
 	/* NB: the beacon data buffer must be 32-bit aligned. */
 	skb = ieee80211_beacon_get(sc->hw, vif);
-	if (skb == NULL) {
-		ath_dbg(common, ATH_DBG_BEACON, "cannot get skb\n");
+	if (skb == NULL)
 		return -ENOMEM;
-	}
 
 	tstamp = ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp;
 	sc->beacon.bc_tstamp = le64_to_cpu(tstamp);
@@ -293,7 +284,7 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
 		u64 tsfadjust;
 		int intval;
 
-		intval = sc->beacon_interval ? : ATH_DEFAULT_BINTVAL;
+		intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
 
 		/*
 		 * Calculate the TSF offset for this beacon slot, i.e., the
@@ -325,6 +316,7 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
 		ath_err(common, "dma_mapping_error on beacon alloc\n");
 		return -ENOMEM;
 	}
+	avp->is_bslot_active = true;
 
 	return 0;
 }
@@ -336,7 +328,6 @@ void ath_beacon_return(struct ath_softc *sc, struct ath_vif *avp)
 
 		if (avp->av_bslot != -1) {
 			sc->beacon.bslot[avp->av_bslot] = NULL;
-			sc->beacon.bslot_aphy[avp->av_bslot] = NULL;
 			sc->nbcnvifs--;
 		}
 
@@ -358,11 +349,11 @@ void ath_beacon_return(struct ath_softc *sc, struct ath_vif *avp)
 void ath_beacon_tasklet(unsigned long data)
 {
 	struct ath_softc *sc = (struct ath_softc *)data;
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_buf *bf = NULL;
 	struct ieee80211_vif *vif;
-	struct ath_wiphy *aphy;
 	int slot;
 	u32 bfaddr, bc = 0, tsftu;
 	u64 tsf;
@@ -406,7 +397,7 @@ void ath_beacon_tasklet(unsigned long data)
 	 * on the tsf to safeguard against missing an swba.
 	 */
 
-	intval = sc->beacon_interval ? : ATH_DEFAULT_BINTVAL;
+	intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
 
 	tsf = ath9k_hw_gettsf64(ah);
 	tsftu = TSF_TO_TU(tsf>>32, tsf);
@@ -420,7 +411,6 @@ void ath_beacon_tasklet(unsigned long data)
 	 */
 	slot = ATH_BCBUF - slot - 1;
 	vif = sc->beacon.bslot[slot];
-	aphy = sc->beacon.bslot_aphy[slot];
 
 	ath_dbg(common, ATH_DBG_BEACON,
 		"slot %d [tsf %llu tsftu %u intval %u] vif %p\n",
@@ -428,7 +418,7 @@ void ath_beacon_tasklet(unsigned long data)
 
 	bfaddr = 0;
 	if (vif) {
-		bf = ath_beacon_generate(aphy->hw, vif);
+		bf = ath_beacon_generate(sc->hw, vif);
 		if (bf != NULL) {
 			bfaddr = bf->bf_daddr;
 			bc = 1;
@@ -720,10 +710,10 @@ void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
 		iftype = sc->sc_ah->opmode;
 	}
 
-		cur_conf->listen_interval = 1;
-		cur_conf->dtim_count = 1;
-		cur_conf->bmiss_timeout =
-			ATH_DEFAULT_BMISS_LIMIT * cur_conf->beacon_interval;
+	cur_conf->listen_interval = 1;
+	cur_conf->dtim_count = 1;
+	cur_conf->bmiss_timeout =
+		ATH_DEFAULT_BMISS_LIMIT * cur_conf->beacon_interval;
 
 	/*
 	 * It looks like mac80211 may end up using beacon interval of zero in
@@ -735,8 +725,9 @@ void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
 		cur_conf->beacon_interval = 100;
 
 	/*
-	 * Some times we dont parse dtim period from mac80211, in that case
-	 * use a default value
+	 * We don't parse dtim period from mac80211 during the driver
+	 * initialization as it breaks association with hidden-ssid
+	 * AP and it causes latency in roaming
 	 */
 	if (cur_conf->dtim_period == 0)
 		cur_conf->dtim_period = 1;
@@ -760,3 +751,36 @@ void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
 
 	sc->sc_flags |= SC_OP_BEACONS;
 }
+
+void ath9k_set_beaconing_status(struct ath_softc *sc, bool status)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_vif *avp;
+	int slot;
+	bool found = false;
+
+	ath9k_ps_wakeup(sc);
+	if (status) {
+		for (slot = 0; slot < ATH_BCBUF; slot++) {
+			if (sc->beacon.bslot[slot]) {
+				avp = (void *)sc->beacon.bslot[slot]->drv_priv;
+				if (avp->is_bslot_active) {
+					found = true;
+					break;
+				}
+			}
+		}
+		if (found) {
+			/* Re-enable beaconing */
+			ah->imask |= ATH9K_INT_SWBA;
+			ath9k_hw_set_interrupts(ah, ah->imask);
+		}
+	} else {
+		/* Disable SWBA interrupt */
+		ah->imask &= ~ATH9K_INT_SWBA;
+		ath9k_hw_set_interrupts(ah, ah->imask);
+		tasklet_kill(&sc->bcon_tasklet);
+		ath9k_hw_stoptxdma(ah, sc->beacon.beaconq);
+	}
+	ath9k_ps_restore(sc);
+}
diff --git a/drivers/net/wireless/ath/ath9k/calib.c b/drivers/net/wireless/ath/ath9k/calib.c
index b68a1acbddd0..b4a92a4313f6 100644
--- a/drivers/net/wireless/ath/ath9k/calib.c
+++ b/drivers/net/wireless/ath/ath9k/calib.c
@@ -382,9 +382,8 @@ void ath9k_init_nfcal_hist_buffer(struct ath_hw *ah,
 	s16 default_nf;
 	int i, j;
 
-	if (!ah->caldata)
-		return;
-
+	ah->caldata->channel = chan->channel;
+	ah->caldata->channelFlags = chan->channelFlags & ~CHANNEL_CW_INT;
 	h = ah->caldata->nfCalHist;
 	default_nf = ath9k_hw_get_default_nf(ah, chan);
 	for (i = 0; i < NUM_NF_READINGS; i++) {
diff --git a/drivers/net/wireless/ath/ath9k/common.c b/drivers/net/wireless/ath/ath9k/common.c
index df1998d48253..615e68276e72 100644
--- a/drivers/net/wireless/ath/ath9k/common.c
+++ b/drivers/net/wireless/ath/ath9k/common.c
@@ -189,6 +189,17 @@ void ath9k_cmn_btcoex_bt_stomp(struct ath_common *common,
 }
 EXPORT_SYMBOL(ath9k_cmn_btcoex_bt_stomp);
 
+void ath9k_cmn_update_txpow(struct ath_hw *ah, u16 cur_txpow,
+			    u16 new_txpow, u16 *txpower)
+{
+	if (cur_txpow != new_txpow) {
+		ath9k_hw_set_txpowerlimit(ah, new_txpow, false);
+		/* read back in case value is clamped */
+		*txpower = ath9k_hw_regulatory(ah)->power_limit;
+	}
+}
+EXPORT_SYMBOL(ath9k_cmn_update_txpow);
+
 static int __init ath9k_cmn_init(void)
 {
 	return 0;
diff --git a/drivers/net/wireless/ath/ath9k/common.h b/drivers/net/wireless/ath/ath9k/common.h
index a126bddebb0a..b2f7b5f89097 100644
--- a/drivers/net/wireless/ath/ath9k/common.h
+++ b/drivers/net/wireless/ath/ath9k/common.h
@@ -23,8 +23,6 @@
 
 /* Common header for Atheros 802.11n base driver cores */
 
-#define IEEE80211_WEP_NKID 4
-
 #define WME_NUM_TID             16
 #define WME_BA_BMP_SIZE         64
 #define WME_MAX_BA              WME_BA_BMP_SIZE
@@ -70,3 +68,5 @@ struct ath9k_channel *ath9k_cmn_get_curchannel(struct ieee80211_hw *hw,
 int ath9k_cmn_count_streams(unsigned int chainmask, int max);
 void ath9k_cmn_btcoex_bt_stomp(struct ath_common *common,
 				  enum ath_stomp_type stomp_type);
+void ath9k_cmn_update_txpow(struct ath_hw *ah, u16 cur_txpow,
+			    u16 new_txpow, u16 *txpower);
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 3586c43077a7..8df5a92a20f1 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/slab.h>
+#include <linux/vmalloc.h>
 #include <asm/unaligned.h>
 
 #include "ath9k.h"
@@ -30,6 +31,19 @@ static int ath9k_debugfs_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
+static ssize_t ath9k_debugfs_read_buf(struct file *file, char __user *user_buf,
+				      size_t count, loff_t *ppos)
+{
+	u8 *buf = file->private_data;
+	return simple_read_from_buffer(user_buf, count, ppos, buf, strlen(buf));
+}
+
+static int ath9k_debugfs_release_buf(struct inode *inode, struct file *file)
+{
+	vfree(file->private_data);
+	return 0;
+}
+
 #ifdef CONFIG_ATH_DEBUG
 
 static ssize_t read_file_debug(struct file *file, char __user *user_buf,
@@ -381,41 +395,40 @@ static const struct file_operations fops_interrupt = {
 	.llseek = default_llseek,
 };
 
-static const char * ath_wiphy_state_str(enum ath_wiphy_state state)
+static const char *channel_type_str(enum nl80211_channel_type t)
 {
-	switch (state) {
-	case ATH_WIPHY_INACTIVE:
-		return "INACTIVE";
-	case ATH_WIPHY_ACTIVE:
-		return "ACTIVE";
-	case ATH_WIPHY_PAUSING:
-		return "PAUSING";
-	case ATH_WIPHY_PAUSED:
-		return "PAUSED";
-	case ATH_WIPHY_SCAN:
-		return "SCAN";
+	switch (t) {
+	case NL80211_CHAN_NO_HT:
+		return "no ht";
+	case NL80211_CHAN_HT20:
+		return "ht20";
+	case NL80211_CHAN_HT40MINUS:
+		return "ht40-";
+	case NL80211_CHAN_HT40PLUS:
+		return "ht40+";
+	default:
+		return "???";
 	}
-	return "?";
 }
 
 static ssize_t read_file_wiphy(struct file *file, char __user *user_buf,
 			       size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
-	struct ath_wiphy *aphy = sc->pri_wiphy;
-	struct ieee80211_channel *chan = aphy->hw->conf.channel;
+	struct ieee80211_channel *chan = sc->hw->conf.channel;
+	struct ieee80211_conf *conf = &(sc->hw->conf);
 	char buf[512];
 	unsigned int len = 0;
-	int i;
 	u8 addr[ETH_ALEN];
 	u32 tmp;
 
 	len += snprintf(buf + len, sizeof(buf) - len,
-			"primary: %s (%s chan=%d ht=%d)\n",
-			wiphy_name(sc->pri_wiphy->hw->wiphy),
-			ath_wiphy_state_str(sc->pri_wiphy->state),
+			"%s (chan=%d  center-freq: %d MHz  channel-type: %d (%s))\n",
+			wiphy_name(sc->hw->wiphy),
 			ieee80211_frequency_to_channel(chan->center_freq),
-			aphy->chan_is_ht);
+			chan->center_freq,
+			conf->channel_type,
+			channel_type_str(conf->channel_type));
 
 	put_unaligned_le32(REG_READ_D(sc->sc_ah, AR_STA_ID0), addr);
 	put_unaligned_le16(REG_READ_D(sc->sc_ah, AR_STA_ID1) & 0xffff, addr + 4);
@@ -457,156 +470,82 @@ static ssize_t read_file_wiphy(struct file *file, char __user *user_buf,
 	else
 		len += snprintf(buf + len, sizeof(buf) - len, "\n");
 
-	/* Put variable-length stuff down here, and check for overflows. */
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy_tmp = sc->sec_wiphy[i];
-		if (aphy_tmp == NULL)
-			continue;
-		chan = aphy_tmp->hw->conf.channel;
-		len += snprintf(buf + len, sizeof(buf) - len,
-			"secondary: %s (%s chan=%d ht=%d)\n",
-			wiphy_name(aphy_tmp->hw->wiphy),
-			ath_wiphy_state_str(aphy_tmp->state),
-			ieee80211_frequency_to_channel(chan->center_freq),
-						       aphy_tmp->chan_is_ht);
-	}
 	if (len > sizeof(buf))
 		len = sizeof(buf);
 
 	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
 }
 
-static struct ath_wiphy * get_wiphy(struct ath_softc *sc, const char *name)
-{
-	int i;
-	if (strcmp(name, wiphy_name(sc->pri_wiphy->hw->wiphy)) == 0)
-		return sc->pri_wiphy;
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy = sc->sec_wiphy[i];
-		if (aphy && strcmp(name, wiphy_name(aphy->hw->wiphy)) == 0)
-			return aphy;
-	}
-	return NULL;
-}
-
-static int del_wiphy(struct ath_softc *sc, const char *name)
-{
-	struct ath_wiphy *aphy = get_wiphy(sc, name);
-	if (!aphy)
-		return -ENOENT;
-	return ath9k_wiphy_del(aphy);
-}
-
-static int pause_wiphy(struct ath_softc *sc, const char *name)
-{
-	struct ath_wiphy *aphy = get_wiphy(sc, name);
-	if (!aphy)
-		return -ENOENT;
-	return ath9k_wiphy_pause(aphy);
-}
-
-static int unpause_wiphy(struct ath_softc *sc, const char *name)
-{
-	struct ath_wiphy *aphy = get_wiphy(sc, name);
-	if (!aphy)
-		return -ENOENT;
-	return ath9k_wiphy_unpause(aphy);
-}
-
-static int select_wiphy(struct ath_softc *sc, const char *name)
-{
-	struct ath_wiphy *aphy = get_wiphy(sc, name);
-	if (!aphy)
-		return -ENOENT;
-	return ath9k_wiphy_select(aphy);
-}
-
-static int schedule_wiphy(struct ath_softc *sc, const char *msec)
-{
-	ath9k_wiphy_set_scheduler(sc, simple_strtoul(msec, NULL, 0));
-	return 0;
-}
-
-static ssize_t write_file_wiphy(struct file *file, const char __user *user_buf,
-				size_t count, loff_t *ppos)
-{
-	struct ath_softc *sc = file->private_data;
-	char buf[50];
-	size_t len;
-
-	len = min(count, sizeof(buf) - 1);
-	if (copy_from_user(buf, user_buf, len))
-		return -EFAULT;
-	buf[len] = '\0';
-	if (len > 0 && buf[len - 1] == '\n')
-		buf[len - 1] = '\0';
-
-	if (strncmp(buf, "add", 3) == 0) {
-		int res = ath9k_wiphy_add(sc);
-		if (res < 0)
-			return res;
-	} else if (strncmp(buf, "del=", 4) == 0) {
-		int res = del_wiphy(sc, buf + 4);
-		if (res < 0)
-			return res;
-	} else if (strncmp(buf, "pause=", 6) == 0) {
-		int res = pause_wiphy(sc, buf + 6);
-		if (res < 0)
-			return res;
-	} else if (strncmp(buf, "unpause=", 8) == 0) {
-		int res = unpause_wiphy(sc, buf + 8);
-		if (res < 0)
-			return res;
-	} else if (strncmp(buf, "select=", 7) == 0) {
-		int res = select_wiphy(sc, buf + 7);
-		if (res < 0)
-			return res;
-	} else if (strncmp(buf, "schedule=", 9) == 0) {
-		int res = schedule_wiphy(sc, buf + 9);
-		if (res < 0)
-			return res;
-	} else
-		return -EOPNOTSUPP;
-
-	return count;
-}
-
 static const struct file_operations fops_wiphy = {
 	.read = read_file_wiphy,
-	.write = write_file_wiphy,
 	.open = ath9k_debugfs_open,
 	.owner = THIS_MODULE,
 	.llseek = default_llseek,
 };
 
+#define PR_QNUM(_n) sc->tx.txq_map[_n]->axq_qnum
 #define PR(str, elem)							\
 	do {								\
 		len += snprintf(buf + len, size - len,			\
 				"%s%13u%11u%10u%10u\n", str,		\
-		sc->debug.stats.txstats[WME_AC_BE].elem, \
-		sc->debug.stats.txstats[WME_AC_BK].elem, \
-		sc->debug.stats.txstats[WME_AC_VI].elem, \
-		sc->debug.stats.txstats[WME_AC_VO].elem); \
+		sc->debug.stats.txstats[PR_QNUM(WME_AC_BE)].elem, \
+		sc->debug.stats.txstats[PR_QNUM(WME_AC_BK)].elem, \
+		sc->debug.stats.txstats[PR_QNUM(WME_AC_VI)].elem, \
+		sc->debug.stats.txstats[PR_QNUM(WME_AC_VO)].elem); \
+		if (len >= size)			  \
+			goto done;			  \
+} while(0)
+
+#define PRX(str, elem)							\
+do {									\
+	len += snprintf(buf + len, size - len,				\
+			"%s%13u%11u%10u%10u\n", str,			\
+			(unsigned int)(sc->tx.txq_map[WME_AC_BE]->elem),	\
+			(unsigned int)(sc->tx.txq_map[WME_AC_BK]->elem),	\
+			(unsigned int)(sc->tx.txq_map[WME_AC_VI]->elem),	\
+			(unsigned int)(sc->tx.txq_map[WME_AC_VO]->elem));	\
+	if (len >= size)						\
+		goto done;						\
 } while(0)
 
+#define PRQLE(str, elem)						\
+do {									\
+	len += snprintf(buf + len, size - len,				\
+			"%s%13i%11i%10i%10i\n", str,			\
+			list_empty(&sc->tx.txq_map[WME_AC_BE]->elem),	\
+			list_empty(&sc->tx.txq_map[WME_AC_BK]->elem),	\
+			list_empty(&sc->tx.txq_map[WME_AC_VI]->elem),	\
+			list_empty(&sc->tx.txq_map[WME_AC_VO]->elem));	\
+	if (len >= size)						\
+		goto done;						\
+} while (0)
+
 static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
 			      size_t count, loff_t *ppos)
 {
 	struct ath_softc *sc = file->private_data;
 	char *buf;
-	unsigned int len = 0, size = 2048;
+	unsigned int len = 0, size = 8000;
+	int i;
 	ssize_t retval = 0;
+	char tmp[32];
 
 	buf = kzalloc(size, GFP_KERNEL);
 	if (buf == NULL)
 		return -ENOMEM;
 
-	len += sprintf(buf, "%30s %10s%10s%10s\n\n", "BE", "BK", "VI", "VO");
+	len += sprintf(buf, "Num-Tx-Queues: %i  tx-queues-setup: 0x%x"
+		       " poll-work-seen: %u\n"
+		       "%30s %10s%10s%10s\n\n",
+		       ATH9K_NUM_TX_QUEUES, sc->tx.txqsetup,
+		       sc->tx_complete_poll_work_seen,
+		       "BE", "BK", "VI", "VO");
 
 	PR("MPDUs Queued:    ", queued);
 	PR("MPDUs Completed: ", completed);
 	PR("Aggregates:      ", a_aggr);
-	PR("AMPDUs Queued:   ", a_queued);
+	PR("AMPDUs Queued HW:", a_queued_hw);
+	PR("AMPDUs Queued SW:", a_queued_sw);
 	PR("AMPDUs Completed:", a_completed);
 	PR("AMPDUs Retried:  ", a_retries);
 	PR("AMPDUs XRetried: ", a_xretries);
@@ -618,6 +557,223 @@ static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
 	PR("DELIM Underrun:  ", delim_underrun);
 	PR("TX-Pkts-All:     ", tx_pkts_all);
 	PR("TX-Bytes-All:    ", tx_bytes_all);
+	PR("hw-put-tx-buf:   ", puttxbuf);
+	PR("hw-tx-start:     ", txstart);
+	PR("hw-tx-proc-desc: ", txprocdesc);
+	len += snprintf(buf + len, size - len,
+			"%s%11p%11p%10p%10p\n", "txq-memory-address:",
+			sc->tx.txq_map[WME_AC_BE],
+			sc->tx.txq_map[WME_AC_BK],
+			sc->tx.txq_map[WME_AC_VI],
+			sc->tx.txq_map[WME_AC_VO]);
+	if (len >= size)
+		goto done;
+
+	PRX("axq-qnum:        ", axq_qnum);
+	PRX("axq-depth:       ", axq_depth);
+	PRX("axq-ampdu_depth: ", axq_ampdu_depth);
+	PRX("axq-stopped      ", stopped);
+	PRX("tx-in-progress   ", axq_tx_inprogress);
+	PRX("pending-frames   ", pending_frames);
+	PRX("txq_headidx:     ", txq_headidx);
+	PRX("txq_tailidx:     ", txq_headidx);
+
+	PRQLE("axq_q empty:       ", axq_q);
+	PRQLE("axq_acq empty:     ", axq_acq);
+	PRQLE("txq_fifo_pending:  ", txq_fifo_pending);
+	for (i = 0; i < ATH_TXFIFO_DEPTH; i++) {
+		snprintf(tmp, sizeof(tmp) - 1, "txq_fifo[%i] empty: ", i);
+		PRQLE(tmp, txq_fifo[i]);
+	}
+
+	/* Print out more detailed queue-info */
+	for (i = 0; i <= WME_AC_BK; i++) {
+		struct ath_txq *txq = &(sc->tx.txq[i]);
+		struct ath_atx_ac *ac;
+		struct ath_atx_tid *tid;
+		if (len >= size)
+			goto done;
+		spin_lock_bh(&txq->axq_lock);
+		if (!list_empty(&txq->axq_acq)) {
+			ac = list_first_entry(&txq->axq_acq, struct ath_atx_ac,
+					      list);
+			len += snprintf(buf + len, size - len,
+					"txq[%i] first-ac: %p sched: %i\n",
+					i, ac, ac->sched);
+			if (list_empty(&ac->tid_q) || (len >= size))
+				goto done_for;
+			tid = list_first_entry(&ac->tid_q, struct ath_atx_tid,
+					       list);
+			len += snprintf(buf + len, size - len,
+					" first-tid: %p sched: %i paused: %i\n",
+					tid, tid->sched, tid->paused);
+		}
+	done_for:
+		spin_unlock_bh(&txq->axq_lock);
+	}
+
+done:
+	if (len > size)
+		len = size;
+
+	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	kfree(buf);
+
+	return retval;
+}
+
+static ssize_t read_file_stations(struct file *file, char __user *user_buf,
+				  size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+	char *buf;
+	unsigned int len = 0, size = 64000;
+	struct ath_node *an = NULL;
+	ssize_t retval = 0;
+	int q;
+
+	buf = kzalloc(size, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
+	len += snprintf(buf + len, size - len,
+			"Stations:\n"
+			" tid: addr sched paused buf_q-empty an ac\n"
+			" ac: addr sched tid_q-empty txq\n");
+
+	spin_lock(&sc->nodes_lock);
+	list_for_each_entry(an, &sc->nodes, list) {
+		len += snprintf(buf + len, size - len,
+				"%pM\n", an->sta->addr);
+		if (len >= size)
+			goto done;
+
+		for (q = 0; q < WME_NUM_TID; q++) {
+			struct ath_atx_tid *tid = &(an->tid[q]);
+			len += snprintf(buf + len, size - len,
+					" tid: %p %s %s %i %p %p\n",
+					tid, tid->sched ? "sched" : "idle",
+					tid->paused ? "paused" : "running",
+					list_empty(&tid->buf_q),
+					tid->an, tid->ac);
+			if (len >= size)
+				goto done;
+		}
+
+		for (q = 0; q < WME_NUM_AC; q++) {
+			struct ath_atx_ac *ac = &(an->ac[q]);
+			len += snprintf(buf + len, size - len,
+					" ac: %p %s %i %p\n",
+					ac, ac->sched ? "sched" : "idle",
+					list_empty(&ac->tid_q), ac->txq);
+			if (len >= size)
+				goto done;
+		}
+	}
+
+done:
+	spin_unlock(&sc->nodes_lock);
+	if (len > size)
+		len = size;
+
+	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	kfree(buf);
+
+	return retval;
+}
+
+static ssize_t read_file_misc(struct file *file, char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_hw *ah = sc->sc_ah;
+	struct ieee80211_hw *hw = sc->hw;
+	char *buf;
+	unsigned int len = 0, size = 8000;
+	ssize_t retval = 0;
+	const char *tmp;
+	unsigned int reg;
+	struct ath9k_vif_iter_data iter_data;
+
+	ath9k_calculate_iter_data(hw, NULL, &iter_data);
+	
+	buf = kzalloc(size, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
+	switch (sc->sc_ah->opmode) {
+	case  NL80211_IFTYPE_ADHOC:
+		tmp = "ADHOC";
+		break;
+	case  NL80211_IFTYPE_MESH_POINT:
+		tmp = "MESH";
+		break;
+	case  NL80211_IFTYPE_AP:
+		tmp = "AP";
+		break;
+	case  NL80211_IFTYPE_STATION:
+		tmp = "STATION";
+		break;
+	default:
+		tmp = "???";
+		break;
+	}
+
+	len += snprintf(buf + len, size - len,
+			"curbssid: %pM\n"
+			"OP-Mode: %s(%i)\n"
+			"Beacon-Timer-Register: 0x%x\n",
+			common->curbssid,
+			tmp, (int)(sc->sc_ah->opmode),
+			REG_READ(ah, AR_BEACON_PERIOD));
+
+	reg = REG_READ(ah, AR_TIMER_MODE);
+	len += snprintf(buf + len, size - len, "Timer-Mode-Register: 0x%x (",
+			reg);
+	if (reg & AR_TBTT_TIMER_EN)
+		len += snprintf(buf + len, size - len, "TBTT ");
+	if (reg & AR_DBA_TIMER_EN)
+		len += snprintf(buf + len, size - len, "DBA ");
+	if (reg & AR_SWBA_TIMER_EN)
+		len += snprintf(buf + len, size - len, "SWBA ");
+	if (reg & AR_HCF_TIMER_EN)
+		len += snprintf(buf + len, size - len, "HCF ");
+	if (reg & AR_TIM_TIMER_EN)
+		len += snprintf(buf + len, size - len, "TIM ");
+	if (reg & AR_DTIM_TIMER_EN)
+		len += snprintf(buf + len, size - len, "DTIM ");
+	len += snprintf(buf + len, size - len, ")\n");
+
+	reg = sc->sc_ah->imask;
+	len += snprintf(buf + len, size - len, "imask: 0x%x (", reg);
+	if (reg & ATH9K_INT_SWBA)
+		len += snprintf(buf + len, size - len, "SWBA ");
+	if (reg & ATH9K_INT_BMISS)
+		len += snprintf(buf + len, size - len, "BMISS ");
+	if (reg & ATH9K_INT_CST)
+		len += snprintf(buf + len, size - len, "CST ");
+	if (reg & ATH9K_INT_RX)
+		len += snprintf(buf + len, size - len, "RX ");
+	if (reg & ATH9K_INT_RXHP)
+		len += snprintf(buf + len, size - len, "RXHP ");
+	if (reg & ATH9K_INT_RXLP)
+		len += snprintf(buf + len, size - len, "RXLP ");
+	if (reg & ATH9K_INT_BB_WATCHDOG)
+		len += snprintf(buf + len, size - len, "BB_WATCHDOG ");
+	/* there are other IRQs if one wanted to add them. */
+	len += snprintf(buf + len, size - len, ")\n");
+
+	len += snprintf(buf + len, size - len,
+			"VIF Counts: AP: %i STA: %i MESH: %i WDS: %i"
+			" ADHOC: %i OTHER: %i nvifs: %hi beacon-vifs: %hi\n",
+			iter_data.naps, iter_data.nstations, iter_data.nmeshes,
+			iter_data.nwds, iter_data.nadhocs, iter_data.nothers,
+			sc->nvifs, sc->nbcnvifs);
+
+	len += snprintf(buf + len, size - len,
+			"Calculated-BSSID-Mask: %pM\n",
+			iter_data.mask);
 
 	if (len > size)
 		len = size;
@@ -629,9 +785,9 @@ static ssize_t read_file_xmit(struct file *file, char __user *user_buf,
 }
 
 void ath_debug_stat_tx(struct ath_softc *sc, struct ath_buf *bf,
-		       struct ath_tx_status *ts)
+		       struct ath_tx_status *ts, struct ath_txq *txq)
 {
-	int qnum = skb_get_queue_mapping(bf->bf_mpdu);
+	int qnum = txq->axq_qnum;
 
 	TX_STAT_INC(qnum, tx_pkts_all);
 	sc->debug.stats.txstats[qnum].tx_bytes_all += bf->bf_mpdu->len;
@@ -666,6 +822,20 @@ static const struct file_operations fops_xmit = {
 	.llseek = default_llseek,
 };
 
+static const struct file_operations fops_stations = {
+	.read = read_file_stations,
+	.open = ath9k_debugfs_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
+static const struct file_operations fops_misc = {
+	.read = read_file_misc,
+	.open = ath9k_debugfs_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 static ssize_t read_file_recv(struct file *file, char __user *user_buf,
 			      size_t count, loff_t *ppos)
 {
@@ -871,6 +1041,42 @@ static const struct file_operations fops_regval = {
 	.llseek = default_llseek,
 };
 
+#define REGDUMP_LINE_SIZE	20
+
+static int open_file_regdump(struct inode *inode, struct file *file)
+{
+	struct ath_softc *sc = inode->i_private;
+	unsigned int len = 0;
+	u8 *buf;
+	int i;
+	unsigned long num_regs, regdump_len, max_reg_offset;
+
+	max_reg_offset = AR_SREV_9300_20_OR_LATER(sc->sc_ah) ? 0x16bd4 : 0xb500;
+	num_regs = max_reg_offset / 4 + 1;
+	regdump_len = num_regs * REGDUMP_LINE_SIZE + 1;
+	buf = vmalloc(regdump_len);
+	if (!buf)
+		return -ENOMEM;
+
+	ath9k_ps_wakeup(sc);
+	for (i = 0; i < num_regs; i++)
+		len += scnprintf(buf + len, regdump_len - len,
+			"0x%06x 0x%08x\n", i << 2, REG_READ(sc->sc_ah, i << 2));
+	ath9k_ps_restore(sc);
+
+	file->private_data = buf;
+
+	return 0;
+}
+
+static const struct file_operations fops_regdump = {
+	.open = open_file_regdump,
+	.read = ath9k_debugfs_read_buf,
+	.release = ath9k_debugfs_release_buf,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,/* read accesses f_pos */
+};
+
 int ath9k_init_debug(struct ath_hw *ah)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -903,6 +1109,14 @@ int ath9k_init_debug(struct ath_hw *ah)
 			sc, &fops_xmit))
 		goto err;
 
+	if (!debugfs_create_file("stations", S_IRUSR, sc->debug.debugfs_phy,
+			sc, &fops_stations))
+		goto err;
+
+	if (!debugfs_create_file("misc", S_IRUSR, sc->debug.debugfs_phy,
+			sc, &fops_misc))
+		goto err;
+
 	if (!debugfs_create_file("recv", S_IRUSR, sc->debug.debugfs_phy,
 			sc, &fops_recv))
 		goto err;
@@ -927,6 +1141,10 @@ int ath9k_init_debug(struct ath_hw *ah)
 			sc->debug.debugfs_phy, &ah->config.cwm_ignore_extcca))
 		goto err;
 
+	if (!debugfs_create_file("regdump", S_IRUSR, sc->debug.debugfs_phy,
+			sc, &fops_regdump))
+		goto err;
+
 	sc->debug.regidx = 0;
 	return 0;
 err:
diff --git a/drivers/net/wireless/ath/ath9k/debug.h b/drivers/net/wireless/ath/ath9k/debug.h
index 1e5078bd0344..59338de0ce19 100644
--- a/drivers/net/wireless/ath/ath9k/debug.h
+++ b/drivers/net/wireless/ath/ath9k/debug.h
@@ -89,7 +89,8 @@ struct ath_interrupt_stats {
  * @queued: Total MPDUs (non-aggr) queued
  * @completed: Total MPDUs (non-aggr) completed
  * @a_aggr: Total no. of aggregates queued
- * @a_queued: Total AMPDUs queued
+ * @a_queued_hw: Total AMPDUs queued to hardware
+ * @a_queued_sw: Total AMPDUs queued to software queues
  * @a_completed: Total AMPDUs completed
  * @a_retries: No. of AMPDUs retried (SW)
  * @a_xretries: No. of AMPDUs dropped due to xretries
@@ -102,6 +103,9 @@ struct ath_interrupt_stats {
  * @desc_cfg_err: Descriptor configuration errors
  * @data_urn: TX data underrun errors
  * @delim_urn: TX delimiter underrun errors
+ * @puttxbuf: Number of times hardware was given txbuf to write.
+ * @txstart:  Number of times hardware was told to start tx.
+ * @txprocdesc:  Number of times tx descriptor was processed
  */
 struct ath_tx_stats {
 	u32 tx_pkts_all;
@@ -109,7 +113,8 @@ struct ath_tx_stats {
 	u32 queued;
 	u32 completed;
 	u32 a_aggr;
-	u32 a_queued;
+	u32 a_queued_hw;
+	u32 a_queued_sw;
 	u32 a_completed;
 	u32 a_retries;
 	u32 a_xretries;
@@ -119,6 +124,9 @@ struct ath_tx_stats {
 	u32 desc_cfg_err;
 	u32 data_underrun;
 	u32 delim_underrun;
+	u32 puttxbuf;
+	u32 txstart;
+	u32 txprocdesc;
 };
 
 /**
@@ -167,7 +175,7 @@ int ath9k_init_debug(struct ath_hw *ah);
 
 void ath_debug_stat_interrupt(struct ath_softc *sc, enum ath9k_int status);
 void ath_debug_stat_tx(struct ath_softc *sc, struct ath_buf *bf,
-		       struct ath_tx_status *ts);
+		       struct ath_tx_status *ts, struct ath_txq *txq);
 void ath_debug_stat_rx(struct ath_softc *sc, struct ath_rx_status *rs);
 
 #else
@@ -184,7 +192,8 @@ static inline void ath_debug_stat_interrupt(struct ath_softc *sc,
 
 static inline void ath_debug_stat_tx(struct ath_softc *sc,
 				     struct ath_buf *bf,
-				     struct ath_tx_status *ts)
+				     struct ath_tx_status *ts,
+				     struct ath_txq *txq)
 {
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.c b/drivers/net/wireless/ath/ath9k/eeprom.c
index d05163159572..8c18bed3a558 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom.c
@@ -89,6 +89,38 @@ bool ath9k_hw_get_lower_upper_index(u8 target, u8 *pList, u16 listSize,
 	return false;
 }
 
+void ath9k_hw_usb_gen_fill_eeprom(struct ath_hw *ah, u16 *eep_data,
+				  int eep_start_loc, int size)
+{
+	int i = 0, j, addr;
+	u32 addrdata[8];
+	u32 data[8];
+
+	for (addr = 0; addr < size; addr++) {
+		addrdata[i] = AR5416_EEPROM_OFFSET +
+			((addr + eep_start_loc) << AR5416_EEPROM_S);
+		i++;
+		if (i == 8) {
+			REG_READ_MULTI(ah, addrdata, data, i);
+
+			for (j = 0; j < i; j++) {
+				*eep_data = data[j];
+				eep_data++;
+			}
+			i = 0;
+		}
+	}
+
+	if (i != 0) {
+		REG_READ_MULTI(ah, addrdata, data, i);
+
+		for (j = 0; j < i; j++) {
+			*eep_data = data[j];
+			eep_data++;
+		}
+	}
+}
+
 bool ath9k_hw_nvram_read(struct ath_common *common, u32 off, u16 *data)
 {
 	return common->bus_ops->eeprom_read(common, off, data);
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.h b/drivers/net/wireless/ath/ath9k/eeprom.h
index 58e2ddc927a9..bd82447f5b78 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.h
+++ b/drivers/net/wireless/ath/ath9k/eeprom.h
@@ -665,6 +665,8 @@ 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_common *common, u32 off, u16 *data);
+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,
 			     u8 *pVpdList, u16 numIntercepts,
 			     u8 *pRetVpdList);
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_4k.c b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
index fbdff7e47952..bc77a308c901 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_4k.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
@@ -27,19 +27,13 @@ static int ath9k_hw_4k_get_eeprom_rev(struct ath_hw *ah)
 	return ((ah->eeprom.map4k.baseEepHeader.version) & 0xFFF);
 }
 
-static bool ath9k_hw_4k_fill_eeprom(struct ath_hw *ah)
-{
 #define SIZE_EEPROM_4K (sizeof(struct ar5416_eeprom_4k) / sizeof(u16))
+
+static bool __ath9k_hw_4k_fill_eeprom(struct ath_hw *ah)
+{
 	struct ath_common *common = ath9k_hw_common(ah);
 	u16 *eep_data = (u16 *)&ah->eeprom.map4k;
-	int addr, eep_start_loc = 0;
-
-	eep_start_loc = 64;
-
-	if (!ath9k_hw_use_flash(ah)) {
-		ath_dbg(common, ATH_DBG_EEPROM,
-			"Reading from EEPROM, not flash\n");
-	}
+	int addr, eep_start_loc = 64;
 
 	for (addr = 0; addr < SIZE_EEPROM_4K; addr++) {
 		if (!ath9k_hw_nvram_read(common, addr + eep_start_loc, eep_data)) {
@@ -51,9 +45,34 @@ static bool ath9k_hw_4k_fill_eeprom(struct ath_hw *ah)
 	}
 
 	return true;
-#undef SIZE_EEPROM_4K
 }
 
+static bool __ath9k_hw_usb_4k_fill_eeprom(struct ath_hw *ah)
+{
+	u16 *eep_data = (u16 *)&ah->eeprom.map4k;
+
+	ath9k_hw_usb_gen_fill_eeprom(ah, eep_data, 64, SIZE_EEPROM_4K);
+
+	return true;
+}
+
+static bool ath9k_hw_4k_fill_eeprom(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (!ath9k_hw_use_flash(ah)) {
+		ath_dbg(common, ATH_DBG_EEPROM,
+			"Reading from EEPROM, not flash\n");
+	}
+
+	if (common->bus_ops->ath_bus_type == ATH_USB)
+		return __ath9k_hw_usb_4k_fill_eeprom(ah);
+	else
+		return __ath9k_hw_4k_fill_eeprom(ah);
+}
+
+#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))
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_9287.c b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
index 9b6bc8a953bc..8cd8333cc086 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_9287.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
@@ -17,7 +17,7 @@
 #include "hw.h"
 #include "ar9002_phy.h"
 
-#define NUM_EEP_WORDS (sizeof(struct ar9287_eeprom) / sizeof(u16))
+#define SIZE_EEPROM_AR9287 (sizeof(struct ar9287_eeprom) / sizeof(u16))
 
 static int ath9k_hw_ar9287_get_eeprom_ver(struct ath_hw *ah)
 {
@@ -29,25 +29,15 @@ static int ath9k_hw_ar9287_get_eeprom_rev(struct ath_hw *ah)
 	return (ah->eeprom.map9287.baseEepHeader.version) & 0xFFF;
 }
 
-static bool ath9k_hw_ar9287_fill_eeprom(struct ath_hw *ah)
+static bool __ath9k_hw_ar9287_fill_eeprom(struct ath_hw *ah)
 {
 	struct ar9287_eeprom *eep = &ah->eeprom.map9287;
 	struct ath_common *common = ath9k_hw_common(ah);
 	u16 *eep_data;
-	int addr, eep_start_loc;
+	int addr, eep_start_loc = AR9287_EEP_START_LOC;
 	eep_data = (u16 *)eep;
 
-	if (common->bus_ops->ath_bus_type == ATH_USB)
-		eep_start_loc = AR9287_HTC_EEP_START_LOC;
-	else
-		eep_start_loc = AR9287_EEP_START_LOC;
-
-	if (!ath9k_hw_use_flash(ah)) {
-		ath_dbg(common, ATH_DBG_EEPROM,
-			"Reading from EEPROM, not flash\n");
-	}
-
-	for (addr = 0; addr < NUM_EEP_WORDS; addr++) {
+	for (addr = 0; addr < SIZE_EEPROM_AR9287; addr++) {
 		if (!ath9k_hw_nvram_read(common, addr + eep_start_loc,
 					 eep_data)) {
 			ath_dbg(common, ATH_DBG_EEPROM,
@@ -60,6 +50,31 @@ static bool ath9k_hw_ar9287_fill_eeprom(struct ath_hw *ah)
 	return true;
 }
 
+static bool __ath9k_hw_usb_ar9287_fill_eeprom(struct ath_hw *ah)
+{
+	u16 *eep_data = (u16 *)&ah->eeprom.map9287;
+
+	ath9k_hw_usb_gen_fill_eeprom(ah, eep_data,
+				     AR9287_HTC_EEP_START_LOC,
+				     SIZE_EEPROM_AR9287);
+	return true;
+}
+
+static bool ath9k_hw_ar9287_fill_eeprom(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (!ath9k_hw_use_flash(ah)) {
+		ath_dbg(common, ATH_DBG_EEPROM,
+			"Reading from EEPROM, not flash\n");
+	}
+
+	if (common->bus_ops->ath_bus_type == ATH_USB)
+		return __ath9k_hw_usb_ar9287_fill_eeprom(ah);
+	else
+		return __ath9k_hw_ar9287_fill_eeprom(ah);
+}
+
 static int ath9k_hw_ar9287_check_eeprom(struct ath_hw *ah)
 {
 	u32 sum = 0, el, integer;
@@ -86,7 +101,7 @@ static int ath9k_hw_ar9287_check_eeprom(struct ath_hw *ah)
 				need_swap = true;
 				eepdata = (u16 *)(&ah->eeprom);
 
-				for (addr = 0; addr < NUM_EEP_WORDS; addr++) {
+				for (addr = 0; addr < SIZE_EEPROM_AR9287; addr++) {
 					temp = swab16(*eepdata);
 					*eepdata = temp;
 					eepdata++;
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_def.c b/drivers/net/wireless/ath/ath9k/eeprom_def.c
index 749a93608664..fccd87df7300 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_def.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_def.c
@@ -86,9 +86,10 @@ static int ath9k_hw_def_get_eeprom_rev(struct ath_hw *ah)
 	return ((ah->eeprom.def.baseEepHeader.version) & 0xFFF);
 }
 
-static bool ath9k_hw_def_fill_eeprom(struct ath_hw *ah)
-{
 #define SIZE_EEPROM_DEF (sizeof(struct ar5416_eeprom_def) / sizeof(u16))
+
+static bool __ath9k_hw_def_fill_eeprom(struct ath_hw *ah)
+{
 	struct ath_common *common = ath9k_hw_common(ah);
 	u16 *eep_data = (u16 *)&ah->eeprom.def;
 	int addr, ar5416_eep_start_loc = 0x100;
@@ -103,9 +104,34 @@ static bool ath9k_hw_def_fill_eeprom(struct ath_hw *ah)
 		eep_data++;
 	}
 	return true;
-#undef SIZE_EEPROM_DEF
 }
 
+static bool __ath9k_hw_usb_def_fill_eeprom(struct ath_hw *ah)
+{
+	u16 *eep_data = (u16 *)&ah->eeprom.def;
+
+	ath9k_hw_usb_gen_fill_eeprom(ah, eep_data,
+				     0x100, SIZE_EEPROM_DEF);
+	return true;
+}
+
+static bool ath9k_hw_def_fill_eeprom(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (!ath9k_hw_use_flash(ah)) {
+		ath_dbg(common, ATH_DBG_EEPROM,
+			"Reading from EEPROM, not flash\n");
+	}
+
+	if (common->bus_ops->ath_bus_type == ATH_USB)
+		return __ath9k_hw_usb_def_fill_eeprom(ah);
+	else
+		return __ath9k_hw_def_fill_eeprom(ah);
+}
+
+#undef SIZE_EEPROM_DEF
+
 static int ath9k_hw_def_check_eeprom(struct ath_hw *ah)
 {
 	struct ar5416_eeprom_def *eep =
@@ -221,9 +247,9 @@ static int ath9k_hw_def_check_eeprom(struct ath_hw *ah)
 	}
 
 	/* Enable fixup for AR_AN_TOP2 if necessary */
-	if (AR_SREV_9280_20_OR_LATER(ah) &&
-	    (eep->baseEepHeader.version & 0xff) > 0x0a &&
-	    eep->baseEepHeader.pwdclkind == 0)
+	if ((ah->hw_version.devid == AR9280_DEVID_PCI) &&
+	    ((eep->baseEepHeader.version & 0xff) > 0x0a) &&
+	    (eep->baseEepHeader.pwdclkind == 0))
 		ah->need_an_top2_fixup = 1;
 
 	if ((common->bus_ops->ath_bus_type == ATH_USB) &&
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index 133764069246..0fb8f8ac275a 100644
--- a/drivers/net/wireless/ath/ath9k/gpio.c
+++ b/drivers/net/wireless/ath/ath9k/gpio.c
@@ -20,121 +20,31 @@
 /*	 LED functions		*/
 /********************************/
 
-static void ath_led_blink_work(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc,
-					    ath_led_blink_work.work);
-
-	if (!(sc->sc_flags & SC_OP_LED_ASSOCIATED))
-		return;
-
-	if ((sc->led_on_duration == ATH_LED_ON_DURATION_IDLE) ||
-	    (sc->led_off_duration == ATH_LED_OFF_DURATION_IDLE))
-		ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin, 0);
-	else
-		ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin,
-				  (sc->sc_flags & SC_OP_LED_ON) ? 1 : 0);
-
-	ieee80211_queue_delayed_work(sc->hw,
-				     &sc->ath_led_blink_work,
-				     (sc->sc_flags & SC_OP_LED_ON) ?
-					msecs_to_jiffies(sc->led_off_duration) :
-					msecs_to_jiffies(sc->led_on_duration));
-
-	sc->led_on_duration = sc->led_on_cnt ?
-			max((ATH_LED_ON_DURATION_IDLE - sc->led_on_cnt), 25) :
-			ATH_LED_ON_DURATION_IDLE;
-	sc->led_off_duration = sc->led_off_cnt ?
-			max((ATH_LED_OFF_DURATION_IDLE - sc->led_off_cnt), 10) :
-			ATH_LED_OFF_DURATION_IDLE;
-	sc->led_on_cnt = sc->led_off_cnt = 0;
-	if (sc->sc_flags & SC_OP_LED_ON)
-		sc->sc_flags &= ~SC_OP_LED_ON;
-	else
-		sc->sc_flags |= SC_OP_LED_ON;
-}
-
+#ifdef CONFIG_MAC80211_LEDS
 static void ath_led_brightness(struct led_classdev *led_cdev,
 			       enum led_brightness brightness)
 {
-	struct ath_led *led = container_of(led_cdev, struct ath_led, led_cdev);
-	struct ath_softc *sc = led->sc;
-
-	switch (brightness) {
-	case LED_OFF:
-		if (led->led_type == ATH_LED_ASSOC ||
-		    led->led_type == ATH_LED_RADIO) {
-			ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin,
-				(led->led_type == ATH_LED_RADIO));
-			sc->sc_flags &= ~SC_OP_LED_ASSOCIATED;
-			if (led->led_type == ATH_LED_RADIO)
-				sc->sc_flags &= ~SC_OP_LED_ON;
-		} else {
-			sc->led_off_cnt++;
-		}
-		break;
-	case LED_FULL:
-		if (led->led_type == ATH_LED_ASSOC) {
-			sc->sc_flags |= SC_OP_LED_ASSOCIATED;
-			if (led_blink)
-				ieee80211_queue_delayed_work(sc->hw,
-						     &sc->ath_led_blink_work, 0);
-		} else if (led->led_type == ATH_LED_RADIO) {
-			ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin, 0);
-			sc->sc_flags |= SC_OP_LED_ON;
-		} else {
-			sc->led_on_cnt++;
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static int ath_register_led(struct ath_softc *sc, struct ath_led *led,
-			    char *trigger)
-{
-	int ret;
-
-	led->sc = sc;
-	led->led_cdev.name = led->name;
-	led->led_cdev.default_trigger = trigger;
-	led->led_cdev.brightness_set = ath_led_brightness;
-
-	ret = led_classdev_register(wiphy_dev(sc->hw->wiphy), &led->led_cdev);
-	if (ret)
-		ath_err(ath9k_hw_common(sc->sc_ah),
-			"Failed to register led:%s", led->name);
-	else
-		led->registered = 1;
-	return ret;
-}
-
-static void ath_unregister_led(struct ath_led *led)
-{
-	if (led->registered) {
-		led_classdev_unregister(&led->led_cdev);
-		led->registered = 0;
-	}
+	struct ath_softc *sc = container_of(led_cdev, struct ath_softc, led_cdev);
+	ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin, (brightness == LED_OFF));
 }
 
 void ath_deinit_leds(struct ath_softc *sc)
 {
-	ath_unregister_led(&sc->assoc_led);
-	sc->sc_flags &= ~SC_OP_LED_ASSOCIATED;
-	ath_unregister_led(&sc->tx_led);
-	ath_unregister_led(&sc->rx_led);
-	ath_unregister_led(&sc->radio_led);
-	ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin, 1);
+	if (!sc->led_registered)
+		return;
+
+	ath_led_brightness(&sc->led_cdev, LED_OFF);
+	led_classdev_unregister(&sc->led_cdev);
 }
 
 void ath_init_leds(struct ath_softc *sc)
 {
-	char *trigger;
 	int ret;
 
 	if (AR_SREV_9287(sc->sc_ah))
 		sc->sc_ah->led_pin = ATH_LED_PIN_9287;
+	else if (AR_SREV_9485(sc->sc_ah))
+		sc->sc_ah->led_pin = ATH_LED_PIN_9485;
 	else
 		sc->sc_ah->led_pin = ATH_LED_PIN_DEF;
 
@@ -144,48 +54,22 @@ void ath_init_leds(struct ath_softc *sc)
 	/* LED off, active low */
 	ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin, 1);
 
-	if (led_blink)
-		INIT_DELAYED_WORK(&sc->ath_led_blink_work, ath_led_blink_work);
-
-	trigger = ieee80211_get_radio_led_name(sc->hw);
-	snprintf(sc->radio_led.name, sizeof(sc->radio_led.name),
-		"ath9k-%s::radio", wiphy_name(sc->hw->wiphy));
-	ret = ath_register_led(sc, &sc->radio_led, trigger);
-	sc->radio_led.led_type = ATH_LED_RADIO;
-	if (ret)
-		goto fail;
-
-	trigger = ieee80211_get_assoc_led_name(sc->hw);
-	snprintf(sc->assoc_led.name, sizeof(sc->assoc_led.name),
-		"ath9k-%s::assoc", wiphy_name(sc->hw->wiphy));
-	ret = ath_register_led(sc, &sc->assoc_led, trigger);
-	sc->assoc_led.led_type = ATH_LED_ASSOC;
-	if (ret)
-		goto fail;
-
-	trigger = ieee80211_get_tx_led_name(sc->hw);
-	snprintf(sc->tx_led.name, sizeof(sc->tx_led.name),
-		"ath9k-%s::tx", wiphy_name(sc->hw->wiphy));
-	ret = ath_register_led(sc, &sc->tx_led, trigger);
-	sc->tx_led.led_type = ATH_LED_TX;
-	if (ret)
-		goto fail;
-
-	trigger = ieee80211_get_rx_led_name(sc->hw);
-	snprintf(sc->rx_led.name, sizeof(sc->rx_led.name),
-		"ath9k-%s::rx", wiphy_name(sc->hw->wiphy));
-	ret = ath_register_led(sc, &sc->rx_led, trigger);
-	sc->rx_led.led_type = ATH_LED_RX;
-	if (ret)
-		goto fail;
-
-	return;
-
-fail:
-	if (led_blink)
-		cancel_delayed_work_sync(&sc->ath_led_blink_work);
-	ath_deinit_leds(sc);
+	if (!led_blink)
+		sc->led_cdev.default_trigger =
+			ieee80211_get_radio_led_name(sc->hw);
+
+	snprintf(sc->led_name, sizeof(sc->led_name),
+		"ath9k-%s", wiphy_name(sc->hw->wiphy));
+	sc->led_cdev.name = sc->led_name;
+	sc->led_cdev.brightness_set = ath_led_brightness;
+
+	ret = led_classdev_register(wiphy_dev(sc->hw->wiphy), &sc->led_cdev);
+	if (ret < 0)
+		return;
+
+	sc->led_registered = true;
 }
+#endif
 
 /*******************/
 /*	Rfkill	   */
@@ -201,8 +85,7 @@ static bool ath_is_rfkill_set(struct ath_softc *sc)
 
 void ath9k_rfkill_poll_state(struct ieee80211_hw *hw)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	bool blocked = !!ath_is_rfkill_set(sc);
 
 	wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 5ab3084eb9cb..f1b8af64569c 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -52,6 +52,9 @@ static struct usb_device_id ath9k_hif_usb_ids[] = {
 	{ USB_DEVICE(0x083A, 0xA704),
 	  .driver_info = AR9280_USB },  /* SMC Networks */
 
+	{ USB_DEVICE(0x0cf3, 0x20ff),
+	  .driver_info = STORAGE_DEVICE },
+
 	{ },
 };
 
@@ -219,8 +222,9 @@ static int __hif_usb_tx(struct hif_device_usb *hif_dev)
 	struct tx_buf *tx_buf = NULL;
 	struct sk_buff *nskb = NULL;
 	int ret = 0, i;
-	u16 *hdr, tx_skb_cnt = 0;
+	u16 tx_skb_cnt = 0;
 	u8 *buf;
+	__le16 *hdr;
 
 	if (hif_dev->tx.tx_skb_cnt == 0)
 		return 0;
@@ -245,9 +249,9 @@ static int __hif_usb_tx(struct hif_device_usb *hif_dev)
 
 		buf = tx_buf->buf;
 		buf += tx_buf->offset;
-		hdr = (u16 *)buf;
-		*hdr++ = nskb->len;
-		*hdr++ = ATH_USB_TX_STREAM_MODE_TAG;
+		hdr = (__le16 *)buf;
+		*hdr++ = cpu_to_le16(nskb->len);
+		*hdr++ = cpu_to_le16(ATH_USB_TX_STREAM_MODE_TAG);
 		buf += 4;
 		memcpy(buf, nskb->data, nskb->len);
 		tx_buf->len = nskb->len + 4;
@@ -913,13 +917,11 @@ static int ath9k_hif_usb_dev_init(struct hif_device_usb *hif_dev, u32 drv_info)
 	if (ret) {
 		dev_err(&hif_dev->udev->dev,
 			"ath9k_htc: Unable to allocate URBs\n");
-		goto err_urb;
+		goto err_fw_download;
 	}
 
 	return 0;
 
-err_urb:
-	ath9k_hif_usb_dealloc_urbs(hif_dev);
 err_fw_download:
 	release_firmware(hif_dev->firmware);
 err_fw_req:
@@ -934,6 +936,61 @@ static void ath9k_hif_usb_dev_deinit(struct hif_device_usb *hif_dev)
 		release_firmware(hif_dev->firmware);
 }
 
+/*
+ * An exact copy of the function from zd1211rw.
+ */
+static int send_eject_command(struct usb_interface *interface)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+	struct usb_host_interface *iface_desc = &interface->altsetting[0];
+	struct usb_endpoint_descriptor *endpoint;
+	unsigned char *cmd;
+	u8 bulk_out_ep;
+	int r;
+
+	/* Find bulk out endpoint */
+	for (r = 1; r >= 0; r--) {
+		endpoint = &iface_desc->endpoint[r].desc;
+		if (usb_endpoint_dir_out(endpoint) &&
+		    usb_endpoint_xfer_bulk(endpoint)) {
+			bulk_out_ep = endpoint->bEndpointAddress;
+			break;
+		}
+	}
+	if (r == -1) {
+		dev_err(&udev->dev,
+			"ath9k_htc: Could not find bulk out endpoint\n");
+		return -ENODEV;
+	}
+
+	cmd = kzalloc(31, GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENODEV;
+
+	/* USB bulk command block */
+	cmd[0] = 0x55;	/* bulk command signature */
+	cmd[1] = 0x53;	/* bulk command signature */
+	cmd[2] = 0x42;	/* bulk command signature */
+	cmd[3] = 0x43;	/* bulk command signature */
+	cmd[14] = 6;	/* command length */
+
+	cmd[15] = 0x1b;	/* SCSI command: START STOP UNIT */
+	cmd[19] = 0x2;	/* eject disc */
+
+	dev_info(&udev->dev, "Ejecting storage device...\n");
+	r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, bulk_out_ep),
+		cmd, 31, NULL, 2000);
+	kfree(cmd);
+	if (r)
+		return r;
+
+	/* At this point, the device disconnects and reconnects with the real
+	 * ID numbers. */
+
+	usb_set_intfdata(interface, NULL);
+	return 0;
+}
+
 static int ath9k_hif_usb_probe(struct usb_interface *interface,
 			       const struct usb_device_id *id)
 {
@@ -941,6 +998,9 @@ static int ath9k_hif_usb_probe(struct usb_interface *interface,
 	struct hif_device_usb *hif_dev;
 	int ret = 0;
 
+	if (id->driver_info == STORAGE_DEVICE)
+		return send_eject_command(interface);
+
 	hif_dev = kzalloc(sizeof(struct hif_device_usb), GFP_KERNEL);
 	if (!hif_dev) {
 		ret = -ENOMEM;
@@ -1027,12 +1087,13 @@ static void ath9k_hif_usb_disconnect(struct usb_interface *interface)
 	struct hif_device_usb *hif_dev = usb_get_intfdata(interface);
 	bool unplugged = (udev->state == USB_STATE_NOTATTACHED) ? true : false;
 
-	if (hif_dev) {
-		ath9k_htc_hw_deinit(hif_dev->htc_handle, unplugged);
-		ath9k_htc_hw_free(hif_dev->htc_handle);
-		ath9k_hif_usb_dev_deinit(hif_dev);
-		usb_set_intfdata(interface, NULL);
-	}
+	if (!hif_dev)
+		return;
+
+	ath9k_htc_hw_deinit(hif_dev->htc_handle, unplugged);
+	ath9k_htc_hw_free(hif_dev->htc_handle);
+	ath9k_hif_usb_dev_deinit(hif_dev);
+	usb_set_intfdata(interface, NULL);
 
 	if (!unplugged && (hif_dev->flags & HIF_USB_START))
 		ath9k_hif_usb_reboot(udev);
diff --git a/drivers/net/wireless/ath/ath9k/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index 780ac5eac501..753a245c5ad1 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -32,6 +32,7 @@
 #include "wmi.h"
 
 #define ATH_STA_SHORT_CALINTERVAL 1000    /* 1 second */
+#define ATH_AP_SHORT_CALINTERVAL  100     /* 100 ms */
 #define ATH_ANI_POLLINTERVAL      100     /* 100 ms */
 #define ATH_LONG_CALINTERVAL      30000   /* 30 seconds */
 #define ATH_RESTART_CALINTERVAL   1200000 /* 20 minutes */
@@ -204,8 +205,50 @@ struct ath9k_htc_target_stats {
 	__be32 ht_tx_xretries;
 } __packed;
 
+#define ATH9K_HTC_MAX_VIF 2
+#define ATH9K_HTC_MAX_BCN_VIF 2
+
+#define INC_VIF(_priv, _type) do {		\
+		switch (_type) {		\
+		case NL80211_IFTYPE_STATION:	\
+			_priv->num_sta_vif++;	\
+			break;			\
+		case NL80211_IFTYPE_ADHOC:	\
+			_priv->num_ibss_vif++;	\
+			break;			\
+		case NL80211_IFTYPE_AP:		\
+			_priv->num_ap_vif++;	\
+			break;			\
+		default:			\
+			break;			\
+		}				\
+	} while (0)
+
+#define DEC_VIF(_priv, _type) do {		\
+		switch (_type) {		\
+		case NL80211_IFTYPE_STATION:	\
+			_priv->num_sta_vif--;	\
+			break;			\
+		case NL80211_IFTYPE_ADHOC:	\
+			_priv->num_ibss_vif--;	\
+			break;			\
+		case NL80211_IFTYPE_AP:		\
+			_priv->num_ap_vif--;	\
+			break;			\
+		default:			\
+			break;			\
+		}				\
+	} while (0)
+
 struct ath9k_htc_vif {
 	u8 index;
+	u16 seq_no;
+	bool beacon_configured;
+};
+
+struct ath9k_vif_iter_data {
+	const u8 *hw_macaddr;
+	u8 mask[ETH_ALEN];
 };
 
 #define ATH9K_HTC_MAX_STA 8
@@ -310,10 +353,8 @@ struct ath_led {
 
 struct htc_beacon_config {
 	u16 beacon_interval;
-	u16 listen_interval;
 	u16 dtim_period;
 	u16 bmiss_timeout;
-	u8 dtim_count;
 };
 
 struct ath_btcoex {
@@ -333,13 +374,12 @@ void ath_htc_cancel_btcoex_work(struct ath9k_htc_priv *priv);
 #define OP_SCANNING		   BIT(1)
 #define OP_LED_ASSOCIATED	   BIT(2)
 #define OP_LED_ON		   BIT(3)
-#define OP_PREAMBLE_SHORT	   BIT(4)
-#define OP_PROTECT_ENABLE	   BIT(5)
-#define OP_ASSOCIATED		   BIT(6)
-#define OP_ENABLE_BEACON	   BIT(7)
-#define OP_LED_DEINIT		   BIT(8)
-#define OP_BT_PRIORITY_DETECTED    BIT(9)
-#define OP_BT_SCAN                 BIT(10)
+#define OP_ENABLE_BEACON	   BIT(4)
+#define OP_LED_DEINIT		   BIT(5)
+#define OP_BT_PRIORITY_DETECTED    BIT(6)
+#define OP_BT_SCAN                 BIT(7)
+#define OP_ANI_RUNNING             BIT(8)
+#define OP_TSF_RESET               BIT(9)
 
 struct ath9k_htc_priv {
 	struct device *dev;
@@ -358,15 +398,24 @@ struct ath9k_htc_priv {
 	enum htc_endpoint_id data_vi_ep;
 	enum htc_endpoint_id data_vo_ep;
 
+	u8 vif_slot;
+	u8 mon_vif_idx;
+	u8 sta_slot;
+	u8 vif_sta_pos[ATH9K_HTC_MAX_VIF];
+	u8 num_ibss_vif;
+	u8 num_sta_vif;
+	u8 num_ap_vif;
+
 	u16 op_flags;
 	u16 curtxpow;
 	u16 txpowlimit;
 	u16 nvifs;
 	u16 nstations;
-	u16 seq_no;
 	u32 bmiss_cnt;
+	bool rearm_ani;
+	bool reconfig_beacon;
 
-	struct ath9k_hw_cal_data caldata[ATH9K_NUM_CHANNELS];
+	struct ath9k_hw_cal_data caldata;
 
 	spinlock_t beacon_lock;
 
@@ -382,7 +431,7 @@ struct ath9k_htc_priv {
 	struct ath9k_htc_rx rx;
 	struct tasklet_struct tx_tasklet;
 	struct sk_buff_head tx_queue;
-	struct delayed_work ath9k_ani_work;
+	struct delayed_work ani_work;
 	struct work_struct ps_work;
 	struct work_struct fatal_work;
 
@@ -424,6 +473,7 @@ void ath9k_htc_reset(struct ath9k_htc_priv *priv);
 void ath9k_htc_beaconq_config(struct ath9k_htc_priv *priv);
 void ath9k_htc_beacon_config(struct ath9k_htc_priv *priv,
 			     struct ieee80211_vif *vif);
+void ath9k_htc_beacon_reconfig(struct ath9k_htc_priv *priv);
 void ath9k_htc_swba(struct ath9k_htc_priv *priv, u8 beacon_pending);
 
 void ath9k_htc_rxep(void *priv, struct sk_buff *skb,
@@ -436,8 +486,9 @@ void ath9k_htc_beaconep(void *drv_priv, struct sk_buff *skb,
 int ath9k_htc_update_cap_target(struct ath9k_htc_priv *priv);
 void ath9k_htc_station_work(struct work_struct *work);
 void ath9k_htc_aggr_work(struct work_struct *work);
-void ath9k_ani_work(struct work_struct *work);;
-void ath_start_ani(struct ath9k_htc_priv *priv);
+void ath9k_htc_ani_work(struct work_struct *work);
+void ath9k_htc_start_ani(struct ath9k_htc_priv *priv);
+void ath9k_htc_stop_ani(struct ath9k_htc_priv *priv);
 
 int ath9k_tx_init(struct ath9k_htc_priv *priv);
 void ath9k_tx_tasklet(unsigned long data);
@@ -460,7 +511,6 @@ void ath9k_htc_ps_restore(struct ath9k_htc_priv *priv);
 void ath9k_ps_work(struct work_struct *work);
 bool ath9k_htc_setpower(struct ath9k_htc_priv *priv,
 			enum ath9k_power_mode mode);
-void ath_update_txpow(struct ath9k_htc_priv *priv);
 
 void ath9k_start_rfkill_poll(struct ath9k_htc_priv *priv);
 void ath9k_htc_rfkill_poll_state(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
index 87cc65a78a3f..8d1d8792436d 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
@@ -123,8 +123,9 @@ static void ath9k_htc_beacon_config_sta(struct ath9k_htc_priv *priv,
 	/* TSF out of range threshold fixed at 1 second */
 	bs.bs_tsfoor_threshold = ATH9K_TSFOOR_THRESHOLD;
 
-	ath_dbg(common, ATH_DBG_BEACON, "tsf: %llu tsftu: %u\n", tsf, tsftu);
-	ath_dbg(common, ATH_DBG_BEACON,
+	ath_dbg(common, ATH_DBG_CONFIG, "intval: %u tsf: %llu tsftu: %u\n",
+		intval, tsf, tsftu);
+	ath_dbg(common, ATH_DBG_CONFIG,
 		"bmiss: %u sleep: %u cfp-period: %u maxdur: %u next: %u\n",
 		bs.bs_bmissthreshold, bs.bs_sleepduration,
 		bs.bs_cfpperiod, bs.bs_cfpmaxduration, bs.bs_cfpnext);
@@ -138,25 +139,81 @@ static void ath9k_htc_beacon_config_sta(struct ath9k_htc_priv *priv,
 	WMI_CMD_BUF(WMI_ENABLE_INTR_CMDID, &htc_imask);
 }
 
+static void ath9k_htc_beacon_config_ap(struct ath9k_htc_priv *priv,
+				       struct htc_beacon_config *bss_conf)
+{
+	struct ath_common *common = ath9k_hw_common(priv->ah);
+	enum ath9k_int imask = 0;
+	u32 nexttbtt, intval, tsftu;
+	__be32 htc_imask = 0;
+	int ret;
+	u8 cmd_rsp;
+	u64 tsf;
+
+	intval = bss_conf->beacon_interval & ATH9K_BEACON_PERIOD;
+	intval /= ATH9K_HTC_MAX_BCN_VIF;
+	nexttbtt = intval;
+
+	if (priv->op_flags & OP_TSF_RESET) {
+		intval |= ATH9K_BEACON_RESET_TSF;
+		priv->op_flags &= ~OP_TSF_RESET;
+	} else {
+		/*
+		 * Pull nexttbtt forward to reflect the current TSF.
+		 */
+		tsf = ath9k_hw_gettsf64(priv->ah);
+		tsftu = TSF_TO_TU(tsf >> 32, tsf) + FUDGE;
+		do {
+			nexttbtt += intval;
+		} while (nexttbtt < tsftu);
+	}
+
+	intval |= ATH9K_BEACON_ENA;
+
+	if (priv->op_flags & OP_ENABLE_BEACON)
+		imask |= ATH9K_INT_SWBA;
+
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"AP Beacon config, intval: %d, nexttbtt: %u imask: 0x%x\n",
+		bss_conf->beacon_interval, nexttbtt, imask);
+
+	WMI_CMD(WMI_DISABLE_INTR_CMDID);
+	ath9k_hw_beaconinit(priv->ah, nexttbtt, intval);
+	priv->bmiss_cnt = 0;
+	htc_imask = cpu_to_be32(imask);
+	WMI_CMD_BUF(WMI_ENABLE_INTR_CMDID, &htc_imask);
+}
+
 static void ath9k_htc_beacon_config_adhoc(struct ath9k_htc_priv *priv,
 					  struct htc_beacon_config *bss_conf)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	enum ath9k_int imask = 0;
-	u32 nexttbtt, intval;
+	u32 nexttbtt, intval, tsftu;
 	__be32 htc_imask = 0;
 	int ret;
 	u8 cmd_rsp;
+	u64 tsf;
 
 	intval = bss_conf->beacon_interval & ATH9K_BEACON_PERIOD;
 	nexttbtt = intval;
+
+	/*
+	 * Pull nexttbtt forward to reflect the current TSF.
+	 */
+	tsf = ath9k_hw_gettsf64(priv->ah);
+	tsftu = TSF_TO_TU(tsf >> 32, tsf) + FUDGE;
+	do {
+		nexttbtt += intval;
+	} while (nexttbtt < tsftu);
+
 	intval |= ATH9K_BEACON_ENA;
 	if (priv->op_flags & OP_ENABLE_BEACON)
 		imask |= ATH9K_INT_SWBA;
 
-	ath_dbg(common, ATH_DBG_BEACON,
-		"IBSS Beacon config, intval: %d, imask: 0x%x\n",
-		bss_conf->beacon_interval, imask);
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"IBSS Beacon config, intval: %d, nexttbtt: %u, imask: 0x%x\n",
+		bss_conf->beacon_interval, nexttbtt, imask);
 
 	WMI_CMD(WMI_DISABLE_INTR_CMDID);
 	ath9k_hw_beaconinit(priv->ah, nexttbtt, intval);
@@ -207,9 +264,9 @@ void ath9k_htc_swba(struct ath9k_htc_priv *priv, u8 beacon_pending)
 	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
 		struct ieee80211_hdr *hdr =
 			(struct ieee80211_hdr *) beacon->data;
-		priv->seq_no += 0x10;
+		avp->seq_no += 0x10;
 		hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
-		hdr->seq_ctrl |= cpu_to_le16(priv->seq_no);
+		hdr->seq_ctrl |= cpu_to_le16(avp->seq_no);
 	}
 
 	tx_ctl.type = ATH9K_HTC_NORMAL;
@@ -253,30 +310,123 @@ void ath9k_htc_beaconq_config(struct ath9k_htc_priv *priv)
 	}
 }
 
+static void ath9k_htc_beacon_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	bool *beacon_configured = (bool *)data;
+	struct ath9k_htc_vif *avp = (struct ath9k_htc_vif *) vif->drv_priv;
+
+	if (vif->type == NL80211_IFTYPE_STATION &&
+	    avp->beacon_configured)
+		*beacon_configured = true;
+}
+
+static bool ath9k_htc_check_beacon_config(struct ath9k_htc_priv *priv,
+					  struct ieee80211_vif *vif)
+{
+	struct ath_common *common = ath9k_hw_common(priv->ah);
+	struct htc_beacon_config *cur_conf = &priv->cur_beacon_conf;
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	bool beacon_configured;
+
+	/*
+	 * Changing the beacon interval when multiple AP interfaces
+	 * are configured will affect beacon transmission of all
+	 * of them.
+	 */
+	if ((priv->ah->opmode == NL80211_IFTYPE_AP) &&
+	    (priv->num_ap_vif > 1) &&
+	    (vif->type == NL80211_IFTYPE_AP) &&
+	    (cur_conf->beacon_interval != bss_conf->beacon_int)) {
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Changing beacon interval of multiple AP interfaces !\n");
+		return false;
+	}
+
+	/*
+	 * If the HW is operating in AP mode, any new station interfaces that
+	 * are added cannot change the beacon parameters.
+	 */
+	if (priv->num_ap_vif &&
+	    (vif->type != NL80211_IFTYPE_AP)) {
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"HW in AP mode, cannot set STA beacon parameters\n");
+		return false;
+	}
+
+	/*
+	 * The beacon parameters are configured only for the first
+	 * station interface.
+	 */
+	if ((priv->ah->opmode == NL80211_IFTYPE_STATION) &&
+	    (priv->num_sta_vif > 1) &&
+	    (vif->type == NL80211_IFTYPE_STATION)) {
+		beacon_configured = false;
+		ieee80211_iterate_active_interfaces_atomic(priv->hw,
+							   ath9k_htc_beacon_iter,
+							   &beacon_configured);
+
+		if (beacon_configured) {
+			ath_dbg(common, ATH_DBG_CONFIG,
+				"Beacon already configured for a station interface\n");
+			return false;
+		}
+	}
+
+	return true;
+}
+
 void ath9k_htc_beacon_config(struct ath9k_htc_priv *priv,
 			     struct ieee80211_vif *vif)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct htc_beacon_config *cur_conf = &priv->cur_beacon_conf;
 	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	struct ath9k_htc_vif *avp = (struct ath9k_htc_vif *) vif->drv_priv;
+
+	if (!ath9k_htc_check_beacon_config(priv, vif))
+		return;
 
 	cur_conf->beacon_interval = bss_conf->beacon_int;
 	if (cur_conf->beacon_interval == 0)
 		cur_conf->beacon_interval = 100;
 
 	cur_conf->dtim_period = bss_conf->dtim_period;
-	cur_conf->listen_interval = 1;
-	cur_conf->dtim_count = 1;
 	cur_conf->bmiss_timeout =
 		ATH_DEFAULT_BMISS_LIMIT * cur_conf->beacon_interval;
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_STATION:
 		ath9k_htc_beacon_config_sta(priv, cur_conf);
+		avp->beacon_configured = true;
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		ath9k_htc_beacon_config_adhoc(priv, cur_conf);
+		break;
+	case NL80211_IFTYPE_AP:
+		ath9k_htc_beacon_config_ap(priv, cur_conf);
+		break;
+	default:
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Unsupported beaconing mode\n");
+		return;
+	}
+}
+
+void ath9k_htc_beacon_reconfig(struct ath9k_htc_priv *priv)
+{
+	struct ath_common *common = ath9k_hw_common(priv->ah);
+	struct htc_beacon_config *cur_conf = &priv->cur_beacon_conf;
+
+	switch (priv->ah->opmode) {
+	case NL80211_IFTYPE_STATION:
+		ath9k_htc_beacon_config_sta(priv, cur_conf);
 		break;
 	case NL80211_IFTYPE_ADHOC:
 		ath9k_htc_beacon_config_adhoc(priv, cur_conf);
 		break;
+	case NL80211_IFTYPE_AP:
+		ath9k_htc_beacon_config_ap(priv, cur_conf);
+		break;
 	default:
 		ath_dbg(common, ATH_DBG_CONFIG,
 			"Unsupported beaconing mode\n");
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
index fe70f67aa088..7e630a81b453 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
@@ -389,7 +389,8 @@ void ath9k_htc_radio_enable(struct ieee80211_hw *hw)
 			ret, ah->curchan->channel);
 	}
 
-	ath_update_txpow(priv);
+	ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+			       &priv->curtxpow);
 
 	/* Start RX */
 	WMI_CMD(WMI_START_RECV_CMDID);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index 0352f0994caa..fc67c937e172 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -294,6 +294,34 @@ static unsigned int ath9k_regread(void *hw_priv, u32 reg_offset)
 	return be32_to_cpu(val);
 }
 
+static void ath9k_multi_regread(void *hw_priv, u32 *addr,
+				u32 *val, u16 count)
+{
+	struct ath_hw *ah = (struct ath_hw *) hw_priv;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+	__be32 tmpaddr[8];
+	__be32 tmpval[8];
+	int i, ret;
+
+       for (i = 0; i < count; i++) {
+	       tmpaddr[i] = cpu_to_be32(addr[i]);
+       }
+
+       ret = ath9k_wmi_cmd(priv->wmi, WMI_REG_READ_CMDID,
+			   (u8 *)tmpaddr , sizeof(u32) * count,
+			   (u8 *)tmpval, sizeof(u32) * count,
+			   100);
+	if (unlikely(ret)) {
+		ath_dbg(common, ATH_DBG_WMI,
+			"Multiple REGISTER READ FAILED (count: %d)\n", count);
+	}
+
+       for (i = 0; i < count; i++) {
+	       val[i] = be32_to_cpu(tmpval[i]);
+       }
+}
+
 static void ath9k_regwrite_single(void *hw_priv, u32 val, u32 reg_offset)
 {
 	struct ath_hw *ah = (struct ath_hw *) hw_priv;
@@ -404,6 +432,7 @@ static void ath9k_regwrite_flush(void *hw_priv)
 
 static const struct ath_ops ath9k_common_ops = {
 	.read = ath9k_regread,
+	.multi_read = ath9k_multi_regread,
 	.write = ath9k_regwrite,
 	.enable_write_buffer = ath9k_enable_regwrite_buffer,
 	.write_flush = ath9k_regwrite_flush,
@@ -650,7 +679,7 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
 		     (unsigned long)priv);
 	tasklet_init(&priv->tx_tasklet, ath9k_tx_tasklet,
 		     (unsigned long)priv);
-	INIT_DELAYED_WORK(&priv->ath9k_ani_work, ath9k_ani_work);
+	INIT_DELAYED_WORK(&priv->ani_work, ath9k_htc_ani_work);
 	INIT_WORK(&priv->ps_work, ath9k_ps_work);
 	INIT_WORK(&priv->fatal_work, ath9k_fatal_work);
 
@@ -758,6 +787,7 @@ static int ath9k_init_device(struct ath9k_htc_priv *priv,
 	struct ath_hw *ah;
 	int error = 0;
 	struct ath_regulatory *reg;
+	char hw_name[64];
 
 	/* Bring up device */
 	error = ath9k_init_priv(priv, devid, product, drv_info);
@@ -798,6 +828,22 @@ static int ath9k_init_device(struct ath9k_htc_priv *priv,
 			goto err_world;
 	}
 
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"WMI:%d, BCN:%d, CAB:%d, UAPSD:%d, MGMT:%d, "
+		"BE:%d, BK:%d, VI:%d, VO:%d\n",
+		priv->wmi_cmd_ep,
+		priv->beacon_ep,
+		priv->cab_ep,
+		priv->uapsd_ep,
+		priv->mgmt_ep,
+		priv->data_be_ep,
+		priv->data_bk_ep,
+		priv->data_vi_ep,
+		priv->data_vo_ep);
+
+	ath9k_hw_name(priv->ah, hw_name, sizeof(hw_name));
+	wiphy_info(hw->wiphy, "%s\n", hw_name);
+
 	ath9k_init_leds(priv);
 	ath9k_start_rfkill_poll(priv);
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index 6bb59958f71e..db8c0c044e9e 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -24,17 +24,6 @@ static struct dentry *ath9k_debugfs_root;
 /* Utilities */
 /*************/
 
-void ath_update_txpow(struct ath9k_htc_priv *priv)
-{
-	struct ath_hw *ah = priv->ah;
-
-	if (priv->curtxpow != priv->txpowlimit) {
-		ath9k_hw_set_txpowerlimit(ah, priv->txpowlimit, false);
-		/* read back in case value is clamped */
-		priv->curtxpow = ath9k_hw_regulatory(ah)->power_limit;
-	}
-}
-
 /* HACK Alert: Use 11NG for 2.4, use 11NA for 5 */
 static enum htc_phymode ath9k_htc_get_curmode(struct ath9k_htc_priv *priv,
 					      struct ath9k_channel *ichan)
@@ -116,12 +105,88 @@ void ath9k_ps_work(struct work_struct *work)
 	ath9k_htc_setpower(priv, ATH9K_PM_NETWORK_SLEEP);
 }
 
+static void ath9k_htc_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct ath9k_htc_priv *priv = data;
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+
+	if ((vif->type == NL80211_IFTYPE_AP) && bss_conf->enable_beacon)
+		priv->reconfig_beacon = true;
+
+	if (bss_conf->assoc) {
+		priv->rearm_ani = true;
+		priv->reconfig_beacon = true;
+	}
+}
+
+static void ath9k_htc_vif_reconfig(struct ath9k_htc_priv *priv)
+{
+	priv->rearm_ani = false;
+	priv->reconfig_beacon = false;
+
+	ieee80211_iterate_active_interfaces_atomic(priv->hw,
+						   ath9k_htc_vif_iter, priv);
+	if (priv->rearm_ani)
+		ath9k_htc_start_ani(priv);
+
+	if (priv->reconfig_beacon) {
+		ath9k_htc_ps_wakeup(priv);
+		ath9k_htc_beacon_reconfig(priv);
+		ath9k_htc_ps_restore(priv);
+	}
+}
+
+static void ath9k_htc_bssid_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct ath9k_vif_iter_data *iter_data = data;
+	int i;
+
+	for (i = 0; i < ETH_ALEN; i++)
+		iter_data->mask[i] &= ~(iter_data->hw_macaddr[i] ^ mac[i]);
+}
+
+static void ath9k_htc_set_bssid_mask(struct ath9k_htc_priv *priv,
+				     struct ieee80211_vif *vif)
+{
+	struct ath_common *common = ath9k_hw_common(priv->ah);
+	struct ath9k_vif_iter_data iter_data;
+
+	/*
+	 * Use the hardware MAC address as reference, the hardware uses it
+	 * together with the BSSID mask when matching addresses.
+	 */
+	iter_data.hw_macaddr = common->macaddr;
+	memset(&iter_data.mask, 0xff, ETH_ALEN);
+
+	if (vif)
+		ath9k_htc_bssid_iter(&iter_data, vif->addr, vif);
+
+	/* Get list of all active MAC addresses */
+	ieee80211_iterate_active_interfaces_atomic(priv->hw, ath9k_htc_bssid_iter,
+						   &iter_data);
+
+	memcpy(common->bssidmask, iter_data.mask, ETH_ALEN);
+	ath_hw_setbssidmask(common);
+}
+
+static void ath9k_htc_set_opmode(struct ath9k_htc_priv *priv)
+{
+	if (priv->num_ibss_vif)
+		priv->ah->opmode = NL80211_IFTYPE_ADHOC;
+	else if (priv->num_ap_vif)
+		priv->ah->opmode = NL80211_IFTYPE_AP;
+	else
+		priv->ah->opmode = NL80211_IFTYPE_STATION;
+
+	ath9k_hw_setopmode(priv->ah);
+}
+
 void ath9k_htc_reset(struct ath9k_htc_priv *priv)
 {
 	struct ath_hw *ah = priv->ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_channel *channel = priv->hw->conf.channel;
-	struct ath9k_hw_cal_data *caldata;
+	struct ath9k_hw_cal_data *caldata = NULL;
 	enum htc_phymode mode;
 	__be16 htc_mode;
 	u8 cmd_rsp;
@@ -130,16 +195,14 @@ void ath9k_htc_reset(struct ath9k_htc_priv *priv)
 	mutex_lock(&priv->mutex);
 	ath9k_htc_ps_wakeup(priv);
 
-	if (priv->op_flags & OP_ASSOCIATED)
-		cancel_delayed_work_sync(&priv->ath9k_ani_work);
-
+	ath9k_htc_stop_ani(priv);
 	ieee80211_stop_queues(priv->hw);
 	htc_stop(priv->htc);
 	WMI_CMD(WMI_DISABLE_INTR_CMDID);
 	WMI_CMD(WMI_DRAIN_TXQ_ALL_CMDID);
 	WMI_CMD(WMI_STOP_RECV_CMDID);
 
-	caldata = &priv->caldata[channel->hw_value];
+	caldata = &priv->caldata;
 	ret = ath9k_hw_reset(ah, ah->curchan, caldata, false);
 	if (ret) {
 		ath_err(common,
@@ -147,7 +210,8 @@ void ath9k_htc_reset(struct ath9k_htc_priv *priv)
 			channel->center_freq, ret);
 	}
 
-	ath_update_txpow(priv);
+	ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+			       &priv->curtxpow);
 
 	WMI_CMD(WMI_START_RECV_CMDID);
 	ath9k_host_rx_init(priv);
@@ -158,12 +222,7 @@ void ath9k_htc_reset(struct ath9k_htc_priv *priv)
 
 	WMI_CMD(WMI_ENABLE_INTR_CMDID);
 	htc_start(priv->htc);
-
-	if (priv->op_flags & OP_ASSOCIATED) {
-		ath9k_htc_beacon_config(priv, priv->vif);
-		ath_start_ani(priv);
-	}
-
+	ath9k_htc_vif_reconfig(priv);
 	ieee80211_wake_queues(priv->hw);
 
 	ath9k_htc_ps_restore(priv);
@@ -179,7 +238,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 	struct ieee80211_conf *conf = &common->hw->conf;
 	bool fastcc;
 	struct ieee80211_channel *channel = hw->conf.channel;
-	struct ath9k_hw_cal_data *caldata;
+	struct ath9k_hw_cal_data *caldata = NULL;
 	enum htc_phymode mode;
 	__be16 htc_mode;
 	u8 cmd_rsp;
@@ -202,7 +261,8 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 		channel->center_freq, conf_is_ht(conf), conf_is_ht40(conf),
 		fastcc);
 
-	caldata = &priv->caldata[channel->hw_value];
+	if (!fastcc)
+		caldata = &priv->caldata;
 	ret = ath9k_hw_reset(ah, hchan, caldata, fastcc);
 	if (ret) {
 		ath_err(common,
@@ -211,7 +271,8 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 		goto err;
 	}
 
-	ath_update_txpow(priv);
+	ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+			       &priv->curtxpow);
 
 	WMI_CMD(WMI_START_RECV_CMDID);
 	if (ret)
@@ -230,11 +291,23 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 		goto err;
 
 	htc_start(priv->htc);
+
+	if (!(priv->op_flags & OP_SCANNING) &&
+	    !(hw->conf.flags & IEEE80211_CONF_OFFCHANNEL))
+		ath9k_htc_vif_reconfig(priv);
+
 err:
 	ath9k_htc_ps_restore(priv);
 	return ret;
 }
 
+/*
+ * Monitor mode handling is a tad complicated because the firmware requires
+ * an interface to be created exclusively, while mac80211 doesn't associate
+ * an interface with the mode.
+ *
+ * So, for now, only one monitor interface can be configured.
+ */
 static void __ath9k_htc_remove_monitor_interface(struct ath9k_htc_priv *priv)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
@@ -244,9 +317,10 @@ static void __ath9k_htc_remove_monitor_interface(struct ath9k_htc_priv *priv)
 
 	memset(&hvif, 0, sizeof(struct ath9k_htc_target_vif));
 	memcpy(&hvif.myaddr, common->macaddr, ETH_ALEN);
-	hvif.index = 0; /* Should do for now */
+	hvif.index = priv->mon_vif_idx;
 	WMI_CMD_BUF(WMI_VAP_REMOVE_CMDID, &hvif);
 	priv->nvifs--;
+	priv->vif_slot &= ~(1 << priv->mon_vif_idx);
 }
 
 static int ath9k_htc_add_monitor_interface(struct ath9k_htc_priv *priv)
@@ -254,70 +328,87 @@ static int ath9k_htc_add_monitor_interface(struct ath9k_htc_priv *priv)
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct ath9k_htc_target_vif hvif;
 	struct ath9k_htc_target_sta tsta;
-	int ret = 0;
+	int ret = 0, sta_idx;
 	u8 cmd_rsp;
 
-	if (priv->nvifs > 0)
-		return -ENOBUFS;
+	if ((priv->nvifs >= ATH9K_HTC_MAX_VIF) ||
+	    (priv->nstations >= ATH9K_HTC_MAX_STA)) {
+		ret = -ENOBUFS;
+		goto err_vif;
+	}
 
-	if (priv->nstations >= ATH9K_HTC_MAX_STA)
-		return -ENOBUFS;
+	sta_idx = ffz(priv->sta_slot);
+	if ((sta_idx < 0) || (sta_idx > ATH9K_HTC_MAX_STA)) {
+		ret = -ENOBUFS;
+		goto err_vif;
+	}
 
 	/*
 	 * Add an interface.
 	 */
-
 	memset(&hvif, 0, sizeof(struct ath9k_htc_target_vif));
 	memcpy(&hvif.myaddr, common->macaddr, ETH_ALEN);
 
 	hvif.opmode = cpu_to_be32(HTC_M_MONITOR);
-	priv->ah->opmode = NL80211_IFTYPE_MONITOR;
-	hvif.index = priv->nvifs;
+	hvif.index = ffz(priv->vif_slot);
 
 	WMI_CMD_BUF(WMI_VAP_CREATE_CMDID, &hvif);
 	if (ret)
-		return ret;
+		goto err_vif;
+
+	/*
+	 * Assign the monitor interface index as a special case here.
+	 * This is needed when the interface is brought down.
+	 */
+	priv->mon_vif_idx = hvif.index;
+	priv->vif_slot |= (1 << hvif.index);
+
+	/*
+	 * Set the hardware mode to monitor only if there are no
+	 * other interfaces.
+	 */
+	if (!priv->nvifs)
+		priv->ah->opmode = NL80211_IFTYPE_MONITOR;
 
 	priv->nvifs++;
 
 	/*
 	 * Associate a station with the interface for packet injection.
 	 */
-
 	memset(&tsta, 0, sizeof(struct ath9k_htc_target_sta));
 
 	memcpy(&tsta.macaddr, common->macaddr, ETH_ALEN);
 
 	tsta.is_vif_sta = 1;
-	tsta.sta_index = priv->nstations;
+	tsta.sta_index = sta_idx;
 	tsta.vif_index = hvif.index;
 	tsta.maxampdu = 0xffff;
 
 	WMI_CMD_BUF(WMI_NODE_CREATE_CMDID, &tsta);
 	if (ret) {
 		ath_err(common, "Unable to add station entry for monitor mode\n");
-		goto err_vif;
+		goto err_sta;
 	}
 
+	priv->sta_slot |= (1 << sta_idx);
 	priv->nstations++;
-
-	/*
-	 * Set chainmask etc. on the target.
-	 */
-	ret = ath9k_htc_update_cap_target(priv);
-	if (ret)
-		ath_dbg(common, ATH_DBG_CONFIG,
-			"Failed to update capability in target\n");
-
+	priv->vif_sta_pos[priv->mon_vif_idx] = sta_idx;
 	priv->ah->is_monitoring = true;
 
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"Attached a monitor interface at idx: %d, sta idx: %d\n",
+		priv->mon_vif_idx, sta_idx);
+
 	return 0;
 
-err_vif:
+err_sta:
 	/*
 	 * Remove the interface from the target.
 	 */
 	__ath9k_htc_remove_monitor_interface(priv);
+err_vif:
+	ath_dbg(common, ATH_DBG_FATAL, "Unable to attach a monitor interface\n");
+
 	return ret;
 }
 
@@ -329,7 +420,7 @@ static int ath9k_htc_remove_monitor_interface(struct ath9k_htc_priv *priv)
 
 	__ath9k_htc_remove_monitor_interface(priv);
 
-	sta_idx = 0; /* Only single interface, for now */
+	sta_idx = priv->vif_sta_pos[priv->mon_vif_idx];
 
 	WMI_CMD_BUF(WMI_NODE_REMOVE_CMDID, &sta_idx);
 	if (ret) {
@@ -337,9 +428,14 @@ static int ath9k_htc_remove_monitor_interface(struct ath9k_htc_priv *priv)
 		return ret;
 	}
 
+	priv->sta_slot &= ~(1 << sta_idx);
 	priv->nstations--;
 	priv->ah->is_monitoring = false;
 
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"Removed a monitor interface at idx: %d, sta idx: %d\n",
+		priv->mon_vif_idx, sta_idx);
+
 	return 0;
 }
 
@@ -351,12 +447,16 @@ static int ath9k_htc_add_station(struct ath9k_htc_priv *priv,
 	struct ath9k_htc_target_sta tsta;
 	struct ath9k_htc_vif *avp = (struct ath9k_htc_vif *) vif->drv_priv;
 	struct ath9k_htc_sta *ista;
-	int ret;
+	int ret, sta_idx;
 	u8 cmd_rsp;
 
 	if (priv->nstations >= ATH9K_HTC_MAX_STA)
 		return -ENOBUFS;
 
+	sta_idx = ffz(priv->sta_slot);
+	if ((sta_idx < 0) || (sta_idx > ATH9K_HTC_MAX_STA))
+		return -ENOBUFS;
+
 	memset(&tsta, 0, sizeof(struct ath9k_htc_target_sta));
 
 	if (sta) {
@@ -366,13 +466,13 @@ static int ath9k_htc_add_station(struct ath9k_htc_priv *priv,
 		tsta.associd = common->curaid;
 		tsta.is_vif_sta = 0;
 		tsta.valid = true;
-		ista->index = priv->nstations;
+		ista->index = sta_idx;
 	} else {
 		memcpy(&tsta.macaddr, vif->addr, ETH_ALEN);
 		tsta.is_vif_sta = 1;
 	}
 
-	tsta.sta_index = priv->nstations;
+	tsta.sta_index = sta_idx;
 	tsta.vif_index = avp->index;
 	tsta.maxampdu = 0xffff;
 	if (sta && sta->ht_cap.ht_supported)
@@ -387,12 +487,21 @@ static int ath9k_htc_add_station(struct ath9k_htc_priv *priv,
 		return ret;
 	}
 
-	if (sta)
+	if (sta) {
 		ath_dbg(common, ATH_DBG_CONFIG,
 			"Added a station entry for: %pM (idx: %d)\n",
 			sta->addr, tsta.sta_index);
+	} else {
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Added a station entry for VIF %d (idx: %d)\n",
+			avp->index, tsta.sta_index);
+	}
 
+	priv->sta_slot |= (1 << sta_idx);
 	priv->nstations++;
+	if (!sta)
+		priv->vif_sta_pos[avp->index] = sta_idx;
+
 	return 0;
 }
 
@@ -401,6 +510,7 @@ static int ath9k_htc_remove_station(struct ath9k_htc_priv *priv,
 				    struct ieee80211_sta *sta)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
+	struct ath9k_htc_vif *avp = (struct ath9k_htc_vif *) vif->drv_priv;
 	struct ath9k_htc_sta *ista;
 	int ret;
 	u8 cmd_rsp, sta_idx;
@@ -409,7 +519,7 @@ static int ath9k_htc_remove_station(struct ath9k_htc_priv *priv,
 		ista = (struct ath9k_htc_sta *) sta->drv_priv;
 		sta_idx = ista->index;
 	} else {
-		sta_idx = 0;
+		sta_idx = priv->vif_sta_pos[avp->index];
 	}
 
 	WMI_CMD_BUF(WMI_NODE_REMOVE_CMDID, &sta_idx);
@@ -421,12 +531,19 @@ static int ath9k_htc_remove_station(struct ath9k_htc_priv *priv,
 		return ret;
 	}
 
-	if (sta)
+	if (sta) {
 		ath_dbg(common, ATH_DBG_CONFIG,
 			"Removed a station entry for: %pM (idx: %d)\n",
 			sta->addr, sta_idx);
+	} else {
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Removed a station entry for VIF %d (idx: %d)\n",
+			avp->index, sta_idx);
+	}
 
+	priv->sta_slot &= ~(1 << sta_idx);
 	priv->nstations--;
+
 	return 0;
 }
 
@@ -808,7 +925,7 @@ void ath9k_htc_debug_remove_root(void)
 /* ANI */
 /*******/
 
-void ath_start_ani(struct ath9k_htc_priv *priv)
+void ath9k_htc_start_ani(struct ath9k_htc_priv *priv)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	unsigned long timestamp = jiffies_to_msecs(jiffies);
@@ -817,15 +934,22 @@ void ath_start_ani(struct ath9k_htc_priv *priv)
 	common->ani.shortcal_timer = timestamp;
 	common->ani.checkani_timer = timestamp;
 
-	ieee80211_queue_delayed_work(common->hw, &priv->ath9k_ani_work,
+	priv->op_flags |= OP_ANI_RUNNING;
+
+	ieee80211_queue_delayed_work(common->hw, &priv->ani_work,
 				     msecs_to_jiffies(ATH_ANI_POLLINTERVAL));
 }
 
-void ath9k_ani_work(struct work_struct *work)
+void ath9k_htc_stop_ani(struct ath9k_htc_priv *priv)
+{
+	cancel_delayed_work_sync(&priv->ani_work);
+	priv->op_flags &= ~OP_ANI_RUNNING;
+}
+
+void ath9k_htc_ani_work(struct work_struct *work)
 {
 	struct ath9k_htc_priv *priv =
-		container_of(work, struct ath9k_htc_priv,
-			     ath9k_ani_work.work);
+		container_of(work, struct ath9k_htc_priv, ani_work.work);
 	struct ath_hw *ah = priv->ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	bool longcal = false;
@@ -834,7 +958,8 @@ void ath9k_ani_work(struct work_struct *work)
 	unsigned int timestamp = jiffies_to_msecs(jiffies);
 	u32 cal_interval, short_cal_interval;
 
-	short_cal_interval = ATH_STA_SHORT_CALINTERVAL;
+	short_cal_interval = (ah->opmode == NL80211_IFTYPE_AP) ?
+		ATH_AP_SHORT_CALINTERVAL : ATH_STA_SHORT_CALINTERVAL;
 
 	/* Only calibrate if awake */
 	if (ah->power_mode != ATH9K_PM_AWAKE)
@@ -903,7 +1028,7 @@ set_timer:
 	if (!common->ani.caldone)
 		cal_interval = min(cal_interval, (u32)short_cal_interval);
 
-	ieee80211_queue_delayed_work(common->hw, &priv->ath9k_ani_work,
+	ieee80211_queue_delayed_work(common->hw, &priv->ani_work,
 				     msecs_to_jiffies(cal_interval));
 }
 
@@ -911,7 +1036,7 @@ set_timer:
 /* mac80211 Callbacks */
 /**********************/
 
-static int ath9k_htc_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void ath9k_htc_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ieee80211_hdr *hdr;
 	struct ath9k_htc_priv *priv = hw->priv;
@@ -924,7 +1049,7 @@ static int ath9k_htc_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	padsize = padpos & 3;
 	if (padsize && skb->len > padpos) {
 		if (skb_headroom(skb) < padsize)
-			return -1;
+			goto fail_tx;
 		skb_push(skb, padsize);
 		memmove(skb->data, skb->data + padsize, padpos);
 	}
@@ -945,11 +1070,10 @@ static int ath9k_htc_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		goto fail_tx;
 	}
 
-	return 0;
+	return;
 
 fail_tx:
 	dev_kfree_skb_any(skb);
-	return 0;
 }
 
 static int ath9k_htc_start(struct ieee80211_hw *hw)
@@ -987,7 +1111,8 @@ static int ath9k_htc_start(struct ieee80211_hw *hw)
 		return ret;
 	}
 
-	ath_update_txpow(priv);
+	ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+			       &priv->curtxpow);
 
 	mode = ath9k_htc_get_curmode(priv, init_channel);
 	htc_mode = cpu_to_be16(mode);
@@ -997,6 +1122,11 @@ static int ath9k_htc_start(struct ieee80211_hw *hw)
 
 	ath9k_host_rx_init(priv);
 
+	ret = ath9k_htc_update_cap_target(priv);
+	if (ret)
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Failed to update capability in target\n");
+
 	priv->op_flags &= ~OP_INVALID;
 	htc_start(priv->htc);
 
@@ -1051,25 +1181,21 @@ static void ath9k_htc_stop(struct ieee80211_hw *hw)
 	cancel_work_sync(&priv->fatal_work);
 	cancel_work_sync(&priv->ps_work);
 	cancel_delayed_work_sync(&priv->ath9k_led_blink_work);
+	ath9k_htc_stop_ani(priv);
 	ath9k_led_stop_brightness(priv);
 
 	mutex_lock(&priv->mutex);
 
-	/* Remove monitor interface here */
-	if (ah->opmode == NL80211_IFTYPE_MONITOR) {
-		if (ath9k_htc_remove_monitor_interface(priv))
-			ath_err(common, "Unable to remove monitor interface\n");
-		else
-			ath_dbg(common, ATH_DBG_CONFIG,
-				"Monitor interface removed\n");
-	}
-
 	if (ah->btcoex_hw.enabled) {
 		ath9k_hw_btcoex_disable(ah);
 		if (ah->btcoex_hw.scheme == ATH_BTCOEX_CFG_3WIRE)
 			ath_htc_cancel_btcoex_work(priv);
 	}
 
+	/* Remove a monitor interface if it's present. */
+	if (priv->ah->is_monitoring)
+		ath9k_htc_remove_monitor_interface(priv);
+
 	ath9k_hw_phy_disable(ah);
 	ath9k_hw_disable(ah);
 	ath9k_htc_ps_restore(priv);
@@ -1093,10 +1219,24 @@ static int ath9k_htc_add_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&priv->mutex);
 
-	/* Only one interface for now */
-	if (priv->nvifs > 0) {
-		ret = -ENOBUFS;
-		goto out;
+	if (priv->nvifs >= ATH9K_HTC_MAX_VIF) {
+		mutex_unlock(&priv->mutex);
+		return -ENOBUFS;
+	}
+
+	if (priv->num_ibss_vif ||
+	    (priv->nvifs && vif->type == NL80211_IFTYPE_ADHOC)) {
+		ath_err(common, "IBSS coexistence with other modes is not allowed\n");
+		mutex_unlock(&priv->mutex);
+		return -ENOBUFS;
+	}
+
+	if (((vif->type == NL80211_IFTYPE_AP) ||
+	     (vif->type == NL80211_IFTYPE_ADHOC)) &&
+	    ((priv->num_ap_vif + priv->num_ibss_vif) >= ATH9K_HTC_MAX_BCN_VIF)) {
+		ath_err(common, "Max. number of beaconing interfaces reached\n");
+		mutex_unlock(&priv->mutex);
+		return -ENOBUFS;
 	}
 
 	ath9k_htc_ps_wakeup(priv);
@@ -1110,6 +1250,9 @@ static int ath9k_htc_add_interface(struct ieee80211_hw *hw,
 	case NL80211_IFTYPE_ADHOC:
 		hvif.opmode = cpu_to_be32(HTC_M_IBSS);
 		break;
+	case NL80211_IFTYPE_AP:
+		hvif.opmode = cpu_to_be32(HTC_M_HOSTAP);
+		break;
 	default:
 		ath_err(common,
 			"Interface type %d not yet supported\n", vif->type);
@@ -1117,34 +1260,39 @@ static int ath9k_htc_add_interface(struct ieee80211_hw *hw,
 		goto out;
 	}
 
-	ath_dbg(common, ATH_DBG_CONFIG,
-		"Attach a VIF of type: %d\n", vif->type);
-
-	priv->ah->opmode = vif->type;
-
 	/* Index starts from zero on the target */
-	avp->index = hvif.index = priv->nvifs;
+	avp->index = hvif.index = ffz(priv->vif_slot);
 	hvif.rtsthreshold = cpu_to_be16(2304);
 	WMI_CMD_BUF(WMI_VAP_CREATE_CMDID, &hvif);
 	if (ret)
 		goto out;
 
-	priv->nvifs++;
-
 	/*
 	 * We need a node in target to tx mgmt frames
 	 * before association.
 	 */
 	ret = ath9k_htc_add_station(priv, vif, NULL);
-	if (ret)
+	if (ret) {
+		WMI_CMD_BUF(WMI_VAP_REMOVE_CMDID, &hvif);
 		goto out;
+	}
 
-	ret = ath9k_htc_update_cap_target(priv);
-	if (ret)
-		ath_dbg(common, ATH_DBG_CONFIG,
-			"Failed to update capability in target\n");
+	ath9k_htc_set_bssid_mask(priv, vif);
 
+	priv->vif_slot |= (1 << avp->index);
+	priv->nvifs++;
 	priv->vif = vif;
+
+	INC_VIF(priv, vif->type);
+	ath9k_htc_set_opmode(priv);
+
+	if ((priv->ah->opmode == NL80211_IFTYPE_AP) &&
+	    !(priv->op_flags & OP_ANI_RUNNING))
+		ath9k_htc_start_ani(priv);
+
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"Attach a VIF of type: %d at idx: %d\n", vif->type, avp->index);
+
 out:
 	ath9k_htc_ps_restore(priv);
 	mutex_unlock(&priv->mutex);
@@ -1162,8 +1310,6 @@ static void ath9k_htc_remove_interface(struct ieee80211_hw *hw,
 	int ret = 0;
 	u8 cmd_rsp;
 
-	ath_dbg(common, ATH_DBG_CONFIG, "Detach Interface\n");
-
 	mutex_lock(&priv->mutex);
 	ath9k_htc_ps_wakeup(priv);
 
@@ -1172,10 +1318,27 @@ static void ath9k_htc_remove_interface(struct ieee80211_hw *hw,
 	hvif.index = avp->index;
 	WMI_CMD_BUF(WMI_VAP_REMOVE_CMDID, &hvif);
 	priv->nvifs--;
+	priv->vif_slot &= ~(1 << avp->index);
 
 	ath9k_htc_remove_station(priv, vif, NULL);
 	priv->vif = NULL;
 
+	DEC_VIF(priv, vif->type);
+	ath9k_htc_set_opmode(priv);
+
+	/*
+	 * Stop ANI only if there are no associated station interfaces.
+	 */
+	if ((vif->type == NL80211_IFTYPE_AP) && (priv->num_ap_vif == 0)) {
+		priv->rearm_ani = false;
+		ieee80211_iterate_active_interfaces_atomic(priv->hw,
+						   ath9k_htc_vif_iter, priv);
+		if (!priv->rearm_ani)
+			ath9k_htc_stop_ani(priv);
+	}
+
+	ath_dbg(common, ATH_DBG_CONFIG, "Detach Interface at idx: %d\n", avp->index);
+
 	ath9k_htc_ps_restore(priv);
 	mutex_unlock(&priv->mutex);
 }
@@ -1211,13 +1374,11 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 	 * IEEE80211_CONF_CHANGE_CHANNEL is handled.
 	 */
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
-		if (conf->flags & IEEE80211_CONF_MONITOR) {
-			if (ath9k_htc_add_monitor_interface(priv))
-				ath_err(common, "Failed to set monitor mode\n");
-			else
-				ath_dbg(common, ATH_DBG_CONFIG,
-					"HW opmode set to Monitor mode\n");
-		}
+		if ((conf->flags & IEEE80211_CONF_MONITOR) &&
+		    !priv->ah->is_monitoring)
+			ath9k_htc_add_monitor_interface(priv);
+		else if (priv->ah->is_monitoring)
+			ath9k_htc_remove_monitor_interface(priv);
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
@@ -1252,7 +1413,8 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
 		priv->txpowlimit = 2 * conf->power_level;
-		ath_update_txpow(priv);
+		ath9k_cmn_update_txpow(priv->ah, priv->curtxpow,
+				       priv->txpowlimit, &priv->curtxpow);
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
@@ -1439,66 +1601,81 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
 	struct ath9k_htc_priv *priv = hw->priv;
 	struct ath_hw *ah = priv->ah;
 	struct ath_common *common = ath9k_hw_common(ah);
+	bool set_assoc;
 
 	mutex_lock(&priv->mutex);
 	ath9k_htc_ps_wakeup(priv);
 
+	/*
+	 * Set the HW AID/BSSID only for the first station interface
+	 * or in IBSS mode.
+	 */
+	set_assoc = !!((priv->ah->opmode == NL80211_IFTYPE_ADHOC) ||
+		       ((priv->ah->opmode == NL80211_IFTYPE_STATION) &&
+			(priv->num_sta_vif == 1)));
+
+
 	if (changed & BSS_CHANGED_ASSOC) {
-		common->curaid = bss_conf->assoc ?
-				 bss_conf->aid : 0;
-		ath_dbg(common, ATH_DBG_CONFIG, "BSS Changed ASSOC %d\n",
-			bss_conf->assoc);
-
-		if (bss_conf->assoc) {
-			priv->op_flags |= OP_ASSOCIATED;
-			ath_start_ani(priv);
-		} else {
-			priv->op_flags &= ~OP_ASSOCIATED;
-			cancel_delayed_work_sync(&priv->ath9k_ani_work);
+		if (set_assoc) {
+			ath_dbg(common, ATH_DBG_CONFIG, "BSS Changed ASSOC %d\n",
+				bss_conf->assoc);
+
+			common->curaid = bss_conf->assoc ?
+				bss_conf->aid : 0;
+
+			if (bss_conf->assoc)
+				ath9k_htc_start_ani(priv);
+			else
+				ath9k_htc_stop_ani(priv);
 		}
 	}
 
 	if (changed & BSS_CHANGED_BSSID) {
-		/* Set BSSID */
-		memcpy(common->curbssid, bss_conf->bssid, ETH_ALEN);
-		ath9k_hw_write_associd(ah);
+		if (set_assoc) {
+			memcpy(common->curbssid, bss_conf->bssid, ETH_ALEN);
+			ath9k_hw_write_associd(ah);
 
-		ath_dbg(common, ATH_DBG_CONFIG,
-			"BSSID: %pM aid: 0x%x\n",
-			common->curbssid, common->curaid);
+			ath_dbg(common, ATH_DBG_CONFIG,
+				"BSSID: %pM aid: 0x%x\n",
+				common->curbssid, common->curaid);
+		}
 	}
 
-	if ((changed & BSS_CHANGED_BEACON_INT) ||
-	    (changed & BSS_CHANGED_BEACON) ||
-	    ((changed & BSS_CHANGED_BEACON_ENABLED) &&
-	    bss_conf->enable_beacon)) {
+	if ((changed & BSS_CHANGED_BEACON_ENABLED) && bss_conf->enable_beacon) {
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Beacon enabled for BSS: %pM\n", bss_conf->bssid);
 		priv->op_flags |= OP_ENABLE_BEACON;
 		ath9k_htc_beacon_config(priv, vif);
 	}
 
-	if ((changed & BSS_CHANGED_BEACON_ENABLED) &&
-	    !bss_conf->enable_beacon) {
-		priv->op_flags &= ~OP_ENABLE_BEACON;
-		ath9k_htc_beacon_config(priv, vif);
-	}
-
-	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		ath_dbg(common, ATH_DBG_CONFIG, "BSS Changed PREAMBLE %d\n",
-			bss_conf->use_short_preamble);
-		if (bss_conf->use_short_preamble)
-			priv->op_flags |= OP_PREAMBLE_SHORT;
-		else
-			priv->op_flags &= ~OP_PREAMBLE_SHORT;
+	if ((changed & BSS_CHANGED_BEACON_ENABLED) && !bss_conf->enable_beacon) {
+		/*
+		 * Disable SWBA interrupt only if there are no
+		 * AP/IBSS interfaces.
+		 */
+		if ((priv->num_ap_vif <= 1) || priv->num_ibss_vif) {
+			ath_dbg(common, ATH_DBG_CONFIG,
+				"Beacon disabled for BSS: %pM\n",
+				bss_conf->bssid);
+			priv->op_flags &= ~OP_ENABLE_BEACON;
+			ath9k_htc_beacon_config(priv, vif);
+		}
 	}
 
-	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
-		ath_dbg(common, ATH_DBG_CONFIG, "BSS Changed CTS PROT %d\n",
-			bss_conf->use_cts_prot);
-		if (bss_conf->use_cts_prot &&
-		    hw->conf.channel->band != IEEE80211_BAND_5GHZ)
-			priv->op_flags |= OP_PROTECT_ENABLE;
-		else
-			priv->op_flags &= ~OP_PROTECT_ENABLE;
+	if (changed & BSS_CHANGED_BEACON_INT) {
+		/*
+		 * Reset the HW TSF for the first AP interface.
+		 */
+		if ((priv->ah->opmode == NL80211_IFTYPE_AP) &&
+		    (priv->nvifs == 1) &&
+		    (priv->num_ap_vif == 1) &&
+		    (vif->type == NL80211_IFTYPE_AP)) {
+			priv->op_flags |= OP_TSF_RESET;
+		}
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Beacon interval changed for BSS: %pM\n",
+			bss_conf->bssid);
+		ath9k_htc_beacon_config(priv, vif);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -1557,12 +1734,14 @@ static int ath9k_htc_ampdu_action(struct ieee80211_hw *hw,
 				  struct ieee80211_vif *vif,
 				  enum ieee80211_ampdu_mlme_action action,
 				  struct ieee80211_sta *sta,
-				  u16 tid, u16 *ssn)
+				  u16 tid, u16 *ssn, u8 buf_size)
 {
 	struct ath9k_htc_priv *priv = hw->priv;
 	struct ath9k_htc_sta *ista;
 	int ret = 0;
 
+	mutex_lock(&priv->mutex);
+
 	switch (action) {
 	case IEEE80211_AMPDU_RX_START:
 		break;
@@ -1587,6 +1766,8 @@ static int ath9k_htc_ampdu_action(struct ieee80211_hw *hw,
 		ath_err(ath9k_hw_common(priv->ah), "Unknown AMPDU action\n");
 	}
 
+	mutex_unlock(&priv->mutex);
+
 	return ret;
 }
 
@@ -1599,8 +1780,7 @@ static void ath9k_htc_sw_scan_start(struct ieee80211_hw *hw)
 	priv->op_flags |= OP_SCANNING;
 	spin_unlock_bh(&priv->beacon_lock);
 	cancel_work_sync(&priv->ps_work);
-	if (priv->op_flags & OP_ASSOCIATED)
-		cancel_delayed_work_sync(&priv->ath9k_ani_work);
+	ath9k_htc_stop_ani(priv);
 	mutex_unlock(&priv->mutex);
 }
 
@@ -1609,14 +1789,11 @@ static void ath9k_htc_sw_scan_complete(struct ieee80211_hw *hw)
 	struct ath9k_htc_priv *priv = hw->priv;
 
 	mutex_lock(&priv->mutex);
-	ath9k_htc_ps_wakeup(priv);
 	spin_lock_bh(&priv->beacon_lock);
 	priv->op_flags &= ~OP_SCANNING;
 	spin_unlock_bh(&priv->beacon_lock);
-	if (priv->op_flags & OP_ASSOCIATED) {
-		ath9k_htc_beacon_config(priv, priv->vif);
-		ath_start_ani(priv);
-	}
+	ath9k_htc_ps_wakeup(priv);
+	ath9k_htc_vif_reconfig(priv);
 	ath9k_htc_ps_restore(priv);
 	mutex_unlock(&priv->mutex);
 }
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index 7a5ffca21958..4a4f27ba96af 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -84,7 +84,9 @@ int ath9k_htc_tx_start(struct ath9k_htc_priv *priv, struct sk_buff *skb)
 	struct ieee80211_hdr *hdr;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_sta *sta = tx_info->control.sta;
+	struct ieee80211_vif *vif = tx_info->control.vif;
 	struct ath9k_htc_sta *ista;
+	struct ath9k_htc_vif *avp;
 	struct ath9k_htc_tx_ctl tx_ctl;
 	enum htc_endpoint_id epid;
 	u16 qnum;
@@ -95,18 +97,31 @@ int ath9k_htc_tx_start(struct ath9k_htc_priv *priv, struct sk_buff *skb)
 	hdr = (struct ieee80211_hdr *) skb->data;
 	fc = hdr->frame_control;
 
-	if (tx_info->control.vif &&
-			(struct ath9k_htc_vif *) tx_info->control.vif->drv_priv)
-		vif_idx = ((struct ath9k_htc_vif *)
-				tx_info->control.vif->drv_priv)->index;
-	else
-		vif_idx = priv->nvifs;
+	/*
+	 * Find out on which interface this packet has to be
+	 * sent out.
+	 */
+	if (vif) {
+		avp = (struct ath9k_htc_vif *) vif->drv_priv;
+		vif_idx = avp->index;
+	} else {
+		if (!priv->ah->is_monitoring) {
+			ath_dbg(ath9k_hw_common(priv->ah), ATH_DBG_XMIT,
+				"VIF is null, but no monitor interface !\n");
+			return -EINVAL;
+		}
 
+		vif_idx = priv->mon_vif_idx;
+	}
+
+	/*
+	 * Find out which station this packet is destined for.
+	 */
 	if (sta) {
 		ista = (struct ath9k_htc_sta *) sta->drv_priv;
 		sta_idx = ista->index;
 	} else {
-		sta_idx = 0;
+		sta_idx = priv->vif_sta_pos[vif_idx];
 	}
 
 	memset(&tx_ctl, 0, sizeof(struct ath9k_htc_tx_ctl));
@@ -141,7 +156,7 @@ int ath9k_htc_tx_start(struct ath9k_htc_priv *priv, struct sk_buff *skb)
 
 		/* CTS-to-self */
 		if (!(flags & ATH9K_HTC_TX_RTSCTS) &&
-		    (priv->op_flags & OP_PROTECT_ENABLE))
+		    (vif && vif->bss_conf.use_cts_prot))
 			flags |= ATH9K_HTC_TX_CTSONLY;
 
 		tx_hdr.flags = cpu_to_be32(flags);
@@ -217,6 +232,7 @@ static bool ath9k_htc_check_tx_aggr(struct ath9k_htc_priv *priv,
 void ath9k_tx_tasklet(unsigned long data)
 {
 	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *)data;
+	struct ieee80211_vif *vif;
 	struct ieee80211_sta *sta;
 	struct ieee80211_hdr *hdr;
 	struct ieee80211_tx_info *tx_info;
@@ -228,12 +244,16 @@ void ath9k_tx_tasklet(unsigned long data)
 		hdr = (struct ieee80211_hdr *) skb->data;
 		fc = hdr->frame_control;
 		tx_info = IEEE80211_SKB_CB(skb);
+		vif = tx_info->control.vif;
 
 		memset(&tx_info->status, 0, sizeof(tx_info->status));
 
+		if (!vif)
+			goto send_mac80211;
+
 		rcu_read_lock();
 
-		sta = ieee80211_find_sta(priv->vif, hdr->addr1);
+		sta = ieee80211_find_sta(vif, hdr->addr1);
 		if (!sta) {
 			rcu_read_unlock();
 			ieee80211_tx_status(priv->hw, skb);
@@ -263,6 +283,7 @@ void ath9k_tx_tasklet(unsigned long data)
 
 		rcu_read_unlock();
 
+	send_mac80211:
 		/* Send status to mac80211 */
 		ieee80211_tx_status(priv->hw, skb);
 	}
@@ -386,7 +407,7 @@ u32 ath9k_htc_calcrxfilter(struct ath9k_htc_priv *priv)
 	 */
 	if (((ah->opmode != NL80211_IFTYPE_AP) &&
 	     (priv->rxfilter & FIF_PROMISC_IN_BSS)) ||
-	    (ah->opmode == NL80211_IFTYPE_MONITOR))
+	    ah->is_monitoring)
 		rfilt |= ATH9K_RX_FILTER_PROM;
 
 	if (priv->rxfilter & FIF_CONTROL)
@@ -398,8 +419,13 @@ u32 ath9k_htc_calcrxfilter(struct ath9k_htc_priv *priv)
 	else
 		rfilt |= ATH9K_RX_FILTER_BEACON;
 
-	if (conf_is_ht(&priv->hw->conf))
+	if (conf_is_ht(&priv->hw->conf)) {
 		rfilt |= ATH9K_RX_FILTER_COMP_BAR;
+		rfilt |= ATH9K_RX_FILTER_UNCOMP_BA_BAR;
+	}
+
+	if (priv->rxfilter & FIF_PSPOLL)
+		rfilt |= ATH9K_RX_FILTER_PSPOLL;
 
 	return rfilt;
 
@@ -412,20 +438,12 @@ u32 ath9k_htc_calcrxfilter(struct ath9k_htc_priv *priv)
 static void ath9k_htc_opmode_init(struct ath9k_htc_priv *priv)
 {
 	struct ath_hw *ah = priv->ah;
-	struct ath_common *common = ath9k_hw_common(ah);
-
 	u32 rfilt, mfilt[2];
 
 	/* configure rx filter */
 	rfilt = ath9k_htc_calcrxfilter(priv);
 	ath9k_hw_setrxfilter(ah, rfilt);
 
-	/* configure bssid mask */
-	ath_hw_setbssidmask(common);
-
-	/* configure operational mode */
-	ath9k_hw_setopmode(ah);
-
 	/* calculate and install multicast filter */
 	mfilt[0] = mfilt[1] = ~0;
 	ath9k_hw_setmcastfilter(ah, mfilt[0], mfilt[1]);
@@ -576,31 +594,29 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	ath9k_process_rate(hw, rx_status, rxbuf->rxstatus.rs_rate,
 			   rxbuf->rxstatus.rs_flags);
 
-	if (priv->op_flags & OP_ASSOCIATED) {
-		if (rxbuf->rxstatus.rs_rssi != ATH9K_RSSI_BAD &&
-		    !rxbuf->rxstatus.rs_moreaggr)
-			ATH_RSSI_LPF(priv->rx.last_rssi,
-				     rxbuf->rxstatus.rs_rssi);
+	if (rxbuf->rxstatus.rs_rssi != ATH9K_RSSI_BAD &&
+	    !rxbuf->rxstatus.rs_moreaggr)
+		ATH_RSSI_LPF(priv->rx.last_rssi,
+			     rxbuf->rxstatus.rs_rssi);
 
-		last_rssi = priv->rx.last_rssi;
+	last_rssi = priv->rx.last_rssi;
 
-		if (likely(last_rssi != ATH_RSSI_DUMMY_MARKER))
-			rxbuf->rxstatus.rs_rssi = ATH_EP_RND(last_rssi,
-							     ATH_RSSI_EP_MULTIPLIER);
+	if (likely(last_rssi != ATH_RSSI_DUMMY_MARKER))
+		rxbuf->rxstatus.rs_rssi = ATH_EP_RND(last_rssi,
+						     ATH_RSSI_EP_MULTIPLIER);
 
-		if (rxbuf->rxstatus.rs_rssi < 0)
-			rxbuf->rxstatus.rs_rssi = 0;
+	if (rxbuf->rxstatus.rs_rssi < 0)
+		rxbuf->rxstatus.rs_rssi = 0;
 
-		if (ieee80211_is_beacon(fc))
-			priv->ah->stats.avgbrssi = rxbuf->rxstatus.rs_rssi;
-	}
+	if (ieee80211_is_beacon(fc))
+		priv->ah->stats.avgbrssi = rxbuf->rxstatus.rs_rssi;
 
 	rx_status->mactime = be64_to_cpu(rxbuf->rxstatus.rs_tstamp);
 	rx_status->band = hw->conf.channel->band;
 	rx_status->freq = hw->conf.channel->center_freq;
 	rx_status->signal =  rxbuf->rxstatus.rs_rssi + ATH_DEFAULT_NOISE_FLOOR;
 	rx_status->antenna = rxbuf->rxstatus.rs_antenna;
-	rx_status->flag |= RX_FLAG_TSFT;
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
 
 	return true;
 
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 9f01e50d5cda..9a3438174f86 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -495,6 +495,17 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 	if (ah->hw_version.devid == AR5416_AR9100_DEVID)
 		ah->hw_version.macVersion = AR_SREV_VERSION_9100;
 
+	ath9k_hw_read_revisions(ah);
+
+	/*
+	 * Read back AR_WA into a permanent copy and set bits 14 and 17.
+	 * We need to do this to avoid RMW of this register. We cannot
+	 * read the reg when chip is asleep.
+	 */
+	ah->WARegVal = REG_READ(ah, AR_WA);
+	ah->WARegVal |= (AR_WA_D3_L1_DISABLE |
+			 AR_WA_ASPM_TIMER_BASED_DISABLE);
+
 	if (!ath9k_hw_set_reset_reg(ah, ATH9K_RESET_POWER_ON)) {
 		ath_err(common, "Couldn't reset chip\n");
 		return -EIO;
@@ -563,14 +574,6 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 
 	ath9k_hw_init_mode_regs(ah);
 
-	/*
-	 * Read back AR_WA into a permanent copy and set bits 14 and 17.
-	 * We need to do this to avoid RMW of this register. We cannot
-	 * read the reg when chip is asleep.
-	 */
-	ah->WARegVal = REG_READ(ah, AR_WA);
-	ah->WARegVal |= (AR_WA_D3_L1_DISABLE |
-			 AR_WA_ASPM_TIMER_BASED_DISABLE);
 
 	if (ah->is_pciexpress)
 		ath9k_hw_configpcipowersave(ah, 0, 0);
@@ -668,14 +671,51 @@ static void ath9k_hw_init_qos(struct ath_hw *ah)
 	REGWRITE_BUFFER_FLUSH(ah);
 }
 
+unsigned long ar9003_get_pll_sqsum_dvc(struct ath_hw *ah)
+{
+		REG_WRITE(ah, PLL3, (REG_READ(ah, PLL3) & ~(PLL3_DO_MEAS_MASK)));
+		udelay(100);
+		REG_WRITE(ah, PLL3, (REG_READ(ah, PLL3) | PLL3_DO_MEAS_MASK));
+
+		while ((REG_READ(ah, PLL4) & PLL4_MEAS_DONE) == 0)
+			udelay(100);
+
+		return (REG_READ(ah, PLL3) & SQSUM_DVC_MASK) >> 3;
+}
+EXPORT_SYMBOL(ar9003_get_pll_sqsum_dvc);
+
+#define DPLL2_KD_VAL            0x3D
+#define DPLL2_KI_VAL            0x06
+#define DPLL3_PHASE_SHIFT_VAL   0x1
+
 static void ath9k_hw_init_pll(struct ath_hw *ah,
 			      struct ath9k_channel *chan)
 {
 	u32 pll;
 
-	if (AR_SREV_9485(ah))
+	if (AR_SREV_9485(ah)) {
+		REG_WRITE(ah, AR_RTC_PLL_CONTROL2, 0x886666);
+		REG_WRITE(ah, AR_CH0_DDR_DPLL2, 0x19e82f01);
+
+		REG_RMW_FIELD(ah, AR_CH0_DDR_DPLL3,
+			      AR_CH0_DPLL3_PHASE_SHIFT, DPLL3_PHASE_SHIFT_VAL);
+
+		REG_WRITE(ah, AR_RTC_PLL_CONTROL, 0x1142c);
+		udelay(100);
+
 		REG_WRITE(ah, AR_RTC_PLL_CONTROL2, 0x886666);
 
+		REG_RMW_FIELD(ah, AR_CH0_BB_DPLL2,
+			      AR_CH0_DPLL2_KD, DPLL2_KD_VAL);
+		REG_RMW_FIELD(ah, AR_CH0_BB_DPLL2,
+			      AR_CH0_DPLL2_KI, DPLL2_KI_VAL);
+
+		REG_RMW_FIELD(ah, AR_CH0_BB_DPLL3,
+			      AR_CH0_DPLL3_PHASE_SHIFT, DPLL3_PHASE_SHIFT_VAL);
+		REG_WRITE(ah, AR_RTC_PLL_CONTROL, 0x142c);
+		udelay(110);
+	}
+
 	pll = ath9k_hw_compute_pll_control(ah, chan);
 
 	REG_WRITE(ah, AR_RTC_PLL_CONTROL, pll);
@@ -1060,7 +1100,6 @@ static bool ath9k_hw_set_reset_power_on(struct ath_hw *ah)
 		REG_WRITE(ah, AR_RC, AR_RC_AHB);
 
 	REG_WRITE(ah, AR_RTC_RESET, 0);
-	udelay(2);
 
 	REGWRITE_BUFFER_FLUSH(ah);
 
@@ -1082,8 +1121,6 @@ static bool ath9k_hw_set_reset_power_on(struct ath_hw *ah)
 		return false;
 	}
 
-	ath9k_hw_read_revisions(ah);
-
 	return ath9k_hw_set_reset(ah, ATH9K_RESET_WARM);
 }
 
@@ -1348,8 +1385,6 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	ath9k_hw_spur_mitigate_freq(ah, chan);
 	ah->eep_ops->set_board_values(ah, chan);
 
-	ath9k_hw_set_operating_mode(ah, ah->opmode);
-
 	ENABLE_REGWRITE_BUFFER(ah);
 
 	REG_WRITE(ah, AR_STA_ID0, get_unaligned_le32(common->macaddr));
@@ -1367,6 +1402,8 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
 	REGWRITE_BUFFER_FLUSH(ah);
 
+	ath9k_hw_set_operating_mode(ah, ah->opmode);
+
 	r = ath9k_hw_rf_set_freq(ah, chan);
 	if (r)
 		return r;
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index ea9fde670646..ef79f4c876ca 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -70,6 +70,9 @@
 #define REG_READ(_ah, _reg) \
 	ath9k_hw_common(_ah)->ops->read((_ah), (_reg))
 
+#define REG_READ_MULTI(_ah, _addr, _val, _cnt)		\
+	ath9k_hw_common(_ah)->ops->multi_read((_ah), (_addr), (_val), (_cnt))
+
 #define ENABLE_REGWRITE_BUFFER(_ah)					\
 	do {								\
 		if (ath9k_hw_common(_ah)->ops->enable_write_buffer)	\
@@ -926,6 +929,7 @@ void ath9k_hw_settsf64(struct ath_hw *ah, u64 tsf64);
 void ath9k_hw_reset_tsf(struct ath_hw *ah);
 void ath9k_hw_set_tsfadjust(struct ath_hw *ah, u32 setting);
 void ath9k_hw_init_global_settings(struct ath_hw *ah);
+unsigned long ar9003_get_pll_sqsum_dvc(struct ath_hw *ah);
 void ath9k_hw_set11nmac2040(struct ath_hw *ah);
 void ath9k_hw_beaconinit(struct ath_hw *ah, u32 next_beacon, u32 beacon_period);
 void ath9k_hw_set_sta_beacon_timers(struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index 087a6a95edd5..79aec983279f 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -41,10 +41,6 @@ static int ath9k_btcoex_enable;
 module_param_named(btcoex_enable, ath9k_btcoex_enable, int, 0444);
 MODULE_PARM_DESC(btcoex_enable, "Enable wifi-BT coexistence");
 
-int ath9k_pm_qos_value = ATH9K_PM_QOS_DEFAULT_VALUE;
-module_param_named(pmqos, ath9k_pm_qos_value, int, S_IRUSR | S_IRGRP | S_IROTH);
-MODULE_PARM_DESC(pmqos, "User specified PM-QOS value");
-
 bool is_ath9k_unloaded;
 /* We use the hw_value as an index into our private channel structure */
 
@@ -144,6 +140,21 @@ static struct ieee80211_rate ath9k_legacy_rates[] = {
 	RATE(540, 0x0c, 0),
 };
 
+#ifdef CONFIG_MAC80211_LEDS
+static const struct ieee80211_tpt_blink ath9k_tpt_blink[] = {
+	{ .throughput = 0 * 1024, .blink_time = 334 },
+	{ .throughput = 1 * 1024, .blink_time = 260 },
+	{ .throughput = 5 * 1024, .blink_time = 220 },
+	{ .throughput = 10 * 1024, .blink_time = 190 },
+	{ .throughput = 20 * 1024, .blink_time = 170 },
+	{ .throughput = 50 * 1024, .blink_time = 150 },
+	{ .throughput = 70 * 1024, .blink_time = 130 },
+	{ .throughput = 100 * 1024, .blink_time = 110 },
+	{ .throughput = 200 * 1024, .blink_time = 80 },
+	{ .throughput = 300 * 1024, .blink_time = 50 },
+};
+#endif
+
 static void ath9k_deinit_softc(struct ath_softc *sc);
 
 /*
@@ -254,8 +265,7 @@ static int ath9k_reg_notifier(struct wiphy *wiphy,
 			      struct regulatory_request *request)
 {
 	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_regulatory *reg = ath9k_hw_regulatory(sc->sc_ah);
 
 	return ath_reg_notifier_apply(wiphy, request, reg);
@@ -442,9 +452,10 @@ static int ath9k_init_queues(struct ath_softc *sc)
 	sc->config.cabqReadytime = ATH_CABQ_READY_TIME;
 	ath_cabq_update(sc);
 
-	for (i = 0; i < WME_NUM_AC; i++)
+	for (i = 0; i < WME_NUM_AC; i++) {
 		sc->tx.txq_map[i] = ath_txq_setup(sc, ATH9K_TX_QUEUE_DATA, i);
-
+		sc->tx.txq_map[i]->mac80211_qnum = i;
+	}
 	return 0;
 }
 
@@ -516,10 +527,8 @@ static void ath9k_init_misc(struct ath_softc *sc)
 
 	sc->beacon.slottime = ATH9K_SLOT_TIME_9;
 
-	for (i = 0; i < ARRAY_SIZE(sc->beacon.bslot); i++) {
+	for (i = 0; i < ARRAY_SIZE(sc->beacon.bslot); i++)
 		sc->beacon.bslot[i] = NULL;
-		sc->beacon.bslot_aphy[i] = NULL;
-	}
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_ANT_DIV_COMB)
 		sc->ant_comb.count = ATH_ANT_DIV_COMB_INIT_COUNT;
@@ -537,6 +546,7 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc, u16 subsysid,
 	if (!ah)
 		return -ENOMEM;
 
+	ah->hw = sc->hw;
 	ah->hw_version.devid = devid;
 	ah->hw_version.subsysid = subsysid;
 	sc->sc_ah = ah;
@@ -554,10 +564,13 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc, u16 subsysid,
 	common->btcoex_enabled = ath9k_btcoex_enable == 1;
 	spin_lock_init(&common->cc_lock);
 
-	spin_lock_init(&sc->wiphy_lock);
 	spin_lock_init(&sc->sc_serial_rw);
 	spin_lock_init(&sc->sc_pm_lock);
 	mutex_init(&sc->mutex);
+#ifdef CONFIG_ATH9K_DEBUGFS
+	spin_lock_init(&sc->nodes_lock);
+	INIT_LIST_HEAD(&sc->nodes);
+#endif
 	tasklet_init(&sc->intr_tq, ath9k_tasklet, (unsigned long)sc);
 	tasklet_init(&sc->bcon_tasklet, ath_beacon_tasklet,
 		     (unsigned long)sc);
@@ -699,7 +712,6 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc, u16 subsysid,
 		    const struct ath_bus_ops *bus_ops)
 {
 	struct ieee80211_hw *hw = sc->hw;
-	struct ath_wiphy *aphy = hw->priv;
 	struct ath_common *common;
 	struct ath_hw *ah;
 	int error = 0;
@@ -734,6 +746,13 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc, u16 subsysid,
 
 	ath9k_init_txpower_limits(sc);
 
+#ifdef CONFIG_MAC80211_LEDS
+	/* must be initialized before ieee80211_register_hw */
+	sc->led_cdev.default_trigger = ieee80211_create_tpt_led_trigger(sc->hw,
+		IEEE80211_TPT_LEDTRIG_FL_RADIO, ath9k_tpt_blink,
+		ARRAY_SIZE(ath9k_tpt_blink));
+#endif
+
 	/* Register with mac80211 */
 	error = ieee80211_register_hw(hw);
 	if (error)
@@ -754,17 +773,11 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc, u16 subsysid,
 
 	INIT_WORK(&sc->hw_check_work, ath_hw_check);
 	INIT_WORK(&sc->paprd_work, ath_paprd_calibrate);
-	INIT_WORK(&sc->chan_work, ath9k_wiphy_chan_work);
-	INIT_DELAYED_WORK(&sc->wiphy_work, ath9k_wiphy_work);
-	sc->wiphy_scheduler_int = msecs_to_jiffies(500);
-	aphy->last_rssi = ATH_RSSI_DUMMY_MARKER;
+	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
 
 	ath_init_leds(sc);
 	ath_start_rfkill_poll(sc);
 
-	pm_qos_add_request(&sc->pm_qos_req, PM_QOS_CPU_DMA_LATENCY,
-			   PM_QOS_DEFAULT_VALUE);
-
 	return 0;
 
 error_world:
@@ -812,7 +825,6 @@ static void ath9k_deinit_softc(struct ath_softc *sc)
 void ath9k_deinit_device(struct ath_softc *sc)
 {
 	struct ieee80211_hw *hw = sc->hw;
-	int i = 0;
 
 	ath9k_ps_wakeup(sc);
 
@@ -821,21 +833,10 @@ void ath9k_deinit_device(struct ath_softc *sc)
 
 	ath9k_ps_restore(sc);
 
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy = sc->sec_wiphy[i];
-		if (aphy == NULL)
-			continue;
-		sc->sec_wiphy[i] = NULL;
-		ieee80211_unregister_hw(aphy->hw);
-		ieee80211_free_hw(aphy->hw);
-	}
-
 	ieee80211_unregister_hw(hw);
-	pm_qos_remove_request(&sc->pm_qos_req);
 	ath_rx_cleanup(sc);
 	ath_tx_cleanup(sc);
 	ath9k_deinit_softc(sc);
-	kfree(sc->sec_wiphy);
 }
 
 void ath_descdma_cleanup(struct ath_softc *sc,
diff --git a/drivers/net/wireless/ath/ath9k/mac.c b/drivers/net/wireless/ath/ath9k/mac.c
index 180170d3ce25..5efc869d65ff 100644
--- a/drivers/net/wireless/ath/ath9k/mac.c
+++ b/drivers/net/wireless/ath/ath9k/mac.c
@@ -690,17 +690,23 @@ int ath9k_hw_rxprocdesc(struct ath_hw *ah, struct ath_desc *ds,
 		rs->rs_flags |= ATH9K_RX_DECRYPT_BUSY;
 
 	if ((ads.ds_rxstatus8 & AR_RxFrameOK) == 0) {
+		/*
+		 * Treat these errors as mutually exclusive to avoid spurious
+		 * extra error reports from the hardware. If a CRC error is
+		 * reported, then decryption and MIC errors are irrelevant,
+		 * the frame is going to be dropped either way
+		 */
 		if (ads.ds_rxstatus8 & AR_CRCErr)
 			rs->rs_status |= ATH9K_RXERR_CRC;
-		if (ads.ds_rxstatus8 & AR_PHYErr) {
+		else if (ads.ds_rxstatus8 & AR_PHYErr) {
 			rs->rs_status |= ATH9K_RXERR_PHY;
 			phyerr = MS(ads.ds_rxstatus8, AR_PHYErrCode);
 			rs->rs_phyerr = phyerr;
-		}
-		if (ads.ds_rxstatus8 & AR_DecryptCRCErr)
+		} else if (ads.ds_rxstatus8 & AR_DecryptCRCErr)
 			rs->rs_status |= ATH9K_RXERR_DECRYPT;
-		if (ads.ds_rxstatus8 & AR_MichaelErr)
+		else if (ads.ds_rxstatus8 & AR_MichaelErr)
 			rs->rs_status |= ATH9K_RXERR_MIC;
+
 		if (ads.ds_rxstatus8 & AR_KeyMiss)
 			rs->rs_status |= ATH9K_RXERR_DECRYPT;
 	}
@@ -885,7 +891,7 @@ void ath9k_hw_set_interrupts(struct ath_hw *ah, enum ath9k_int ints)
 	struct ath_common *common = ath9k_hw_common(ah);
 
 	if (!(ints & ATH9K_INT_GLOBAL))
-		ath9k_hw_enable_interrupts(ah);
+		ath9k_hw_disable_interrupts(ah);
 
 	ath_dbg(common, ATH_DBG_INTERRUPT, "0x%x => 0x%x\n", omask, ints);
 
@@ -963,7 +969,8 @@ void ath9k_hw_set_interrupts(struct ath_hw *ah, enum ath9k_int ints)
 			REG_CLR_BIT(ah, AR_IMR_S5, AR_IMR_S5_TIM_TIMER);
 	}
 
-	ath9k_hw_enable_interrupts(ah);
+	if (ints & ATH9K_INT_GLOBAL)
+		ath9k_hw_enable_interrupts(ah);
 
 	return;
 }
diff --git a/drivers/net/wireless/ath/ath9k/mac.h b/drivers/net/wireless/ath/ath9k/mac.h
index 7512f97e8f49..04d58ae923bb 100644
--- a/drivers/net/wireless/ath/ath9k/mac.h
+++ b/drivers/net/wireless/ath/ath9k/mac.h
@@ -639,6 +639,8 @@ enum ath9k_rx_filter {
 	ATH9K_RX_FILTER_PHYERR = 0x00000100,
 	ATH9K_RX_FILTER_MYBEACON = 0x00000200,
 	ATH9K_RX_FILTER_COMP_BAR = 0x00000400,
+	ATH9K_RX_FILTER_COMP_BA = 0x00000800,
+	ATH9K_RX_FILTER_UNCOMP_BA_BAR = 0x00001000,
 	ATH9K_RX_FILTER_PSPOLL = 0x00004000,
 	ATH9K_RX_FILTER_PHYRADAR = 0x00002000,
 	ATH9K_RX_FILTER_MCAST_BCAST_ALL = 0x00008000,
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index da5c64597c1f..2e228aada1a9 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -15,20 +15,10 @@
  */
 
 #include <linux/nl80211.h>
+#include <linux/delay.h>
 #include "ath9k.h"
 #include "btcoex.h"
 
-static void ath_update_txpow(struct ath_softc *sc)
-{
-	struct ath_hw *ah = sc->sc_ah;
-
-	if (sc->curtxpow != sc->config.txpowlimit) {
-		ath9k_hw_set_txpowerlimit(ah, sc->config.txpowlimit, false);
-		/* read back in case value is clamped */
-		sc->curtxpow = ath9k_hw_regulatory(ah)->power_limit;
-	}
-}
-
 static u8 parse_mpdudensity(u8 mpdudensity)
 {
 	/*
@@ -64,17 +54,19 @@ static u8 parse_mpdudensity(u8 mpdudensity)
 	}
 }
 
-static struct ath9k_channel *ath_get_curchannel(struct ath_softc *sc,
-						struct ieee80211_hw *hw)
+static bool ath9k_has_pending_frames(struct ath_softc *sc, struct ath_txq *txq)
 {
-	struct ieee80211_channel *curchan = hw->conf.channel;
-	struct ath9k_channel *channel;
-	u8 chan_idx;
+	bool pending = false;
+
+	spin_lock_bh(&txq->axq_lock);
 
-	chan_idx = curchan->hw_value;
-	channel = &sc->sc_ah->channels[chan_idx];
-	ath9k_update_ichannel(sc, hw, channel);
-	return channel;
+	if (txq->axq_depth || !list_empty(&txq->axq_acq))
+		pending = true;
+	else if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA)
+		pending = !list_empty(&txq->txq_fifo_pending);
+
+	spin_unlock_bh(&txq->axq_lock);
+	return pending;
 }
 
 bool ath9k_setpower(struct ath_softc *sc, enum ath9k_power_mode mode)
@@ -177,7 +169,12 @@ static void ath_update_survey_nf(struct ath_softc *sc, int channel)
 	}
 }
 
-static void ath_update_survey_stats(struct ath_softc *sc)
+/*
+ * Updates the survey statistics and returns the busy time since last
+ * update in %, if the measurement duration was long enough for the
+ * result to be useful, -1 otherwise.
+ */
+static int ath_update_survey_stats(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -185,9 +182,10 @@ static void ath_update_survey_stats(struct ath_softc *sc)
 	struct survey_info *survey = &sc->survey[pos];
 	struct ath_cycle_counters *cc = &common->cc_survey;
 	unsigned int div = common->clockrate * 1000;
+	int ret = 0;
 
 	if (!ah->curchan)
-		return;
+		return -1;
 
 	if (ah->power_mode == ATH9K_PM_AWAKE)
 		ath_hw_cycle_counters_update(common);
@@ -202,9 +200,18 @@ static void ath_update_survey_stats(struct ath_softc *sc)
 		survey->channel_time_rx += cc->rx_frame / div;
 		survey->channel_time_tx += cc->tx_frame / div;
 	}
+
+	if (cc->cycles < div)
+		return -1;
+
+	if (cc->cycles > 0)
+		ret = cc->rx_busy * 100 / cc->cycles;
+
 	memset(cc, 0, sizeof(*cc));
 
 	ath_update_survey_nf(sc, pos);
+
+	return ret;
 }
 
 /*
@@ -215,7 +222,6 @@ static void ath_update_survey_stats(struct ath_softc *sc)
 int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 		    struct ath9k_channel *hchan)
 {
-	struct ath_wiphy *aphy = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_conf *conf = &common->hw->conf;
@@ -227,10 +233,13 @@ int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 	if (sc->sc_flags & SC_OP_INVALID)
 		return -EIO;
 
+	sc->hw_busy_count = 0;
+
 	del_timer_sync(&common->ani.timer);
 	cancel_work_sync(&sc->paprd_work);
 	cancel_work_sync(&sc->hw_check_work);
 	cancel_delayed_work_sync(&sc->tx_complete_work);
+	cancel_delayed_work_sync(&sc->hw_pll_work);
 
 	ath9k_ps_wakeup(sc);
 
@@ -251,6 +260,9 @@ int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 	if (!ath_stoprecv(sc))
 		stopped = false;
 
+	if (!ath9k_hw_check_alive(ah))
+		stopped = false;
+
 	/* XXX: do not flush receive queue here. We don't want
 	 * to flush data frames already in queue because of
 	 * changing channel. */
@@ -259,7 +271,7 @@ int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 		fastcc = false;
 
 	if (!(sc->sc_flags & SC_OP_OFFCHANNEL))
-		caldata = &aphy->caldata;
+		caldata = &sc->caldata;
 
 	ath_dbg(common, ATH_DBG_CONFIG,
 		"(%u MHz) -> (%u MHz), conf_is_ht40: %d fastcc: %d\n",
@@ -281,17 +293,21 @@ int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 		goto ps_restore;
 	}
 
-	ath_update_txpow(sc);
+	ath9k_cmn_update_txpow(ah, sc->curtxpow,
+			       sc->config.txpowlimit, &sc->curtxpow);
 	ath9k_hw_set_interrupts(ah, ah->imask);
 
 	if (!(sc->sc_flags & (SC_OP_OFFCHANNEL))) {
 		if (sc->sc_flags & SC_OP_BEACONS)
 			ath_beacon_config(sc, NULL);
 		ieee80211_queue_delayed_work(sc->hw, &sc->tx_complete_work, 0);
+		ieee80211_queue_delayed_work(sc->hw, &sc->hw_pll_work, HZ/2);
 		ath_start_ani(common);
 	}
 
  ps_restore:
+	ieee80211_wake_queues(hw);
+
 	spin_unlock_bh(&sc->sc_pcu_lock);
 
 	ath9k_ps_restore(sc);
@@ -549,6 +565,12 @@ static void ath_node_attach(struct ath_softc *sc, struct ieee80211_sta *sta)
 	struct ath_hw *ah = sc->sc_ah;
 	an = (struct ath_node *)sta->drv_priv;
 
+#ifdef CONFIG_ATH9K_DEBUGFS
+	spin_lock(&sc->nodes_lock);
+	list_add(&an->list, &sc->nodes);
+	spin_unlock(&sc->nodes_lock);
+	an->sta = sta;
+#endif
 	if ((ah->caps.hw_caps) & ATH9K_HW_CAP_APM)
 		sc->sc_flags |= SC_OP_ENABLE_APM;
 
@@ -564,6 +586,13 @@ static void ath_node_detach(struct ath_softc *sc, struct ieee80211_sta *sta)
 {
 	struct ath_node *an = (struct ath_node *)sta->drv_priv;
 
+#ifdef CONFIG_ATH9K_DEBUGFS
+	spin_lock(&sc->nodes_lock);
+	list_del(&an->list);
+	spin_unlock(&sc->nodes_lock);
+	an->sta = NULL;
+#endif
+
 	if (sc->sc_flags & SC_OP_TXAGGR)
 		ath_tx_node_cleanup(sc, an);
 }
@@ -571,17 +600,25 @@ static void ath_node_detach(struct ath_softc *sc, struct ieee80211_sta *sta)
 void ath_hw_check(struct work_struct *work)
 {
 	struct ath_softc *sc = container_of(work, struct ath_softc, hw_check_work);
-	int i;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	unsigned long flags;
+	int busy;
 
 	ath9k_ps_wakeup(sc);
+	if (ath9k_hw_check_alive(sc->sc_ah))
+		goto out;
 
-	for (i = 0; i < 3; i++) {
-		if (ath9k_hw_check_alive(sc->sc_ah))
-			goto out;
+	spin_lock_irqsave(&common->cc_lock, flags);
+	busy = ath_update_survey_stats(sc);
+	spin_unlock_irqrestore(&common->cc_lock, flags);
 
-		msleep(1);
-	}
-	ath_reset(sc, true);
+	ath_dbg(common, ATH_DBG_RESET, "Possible baseband hang, "
+		"busy=%d (try %d)\n", busy, sc->hw_busy_count + 1);
+	if (busy >= 99) {
+		if (++sc->hw_busy_count >= 3)
+			ath_reset(sc, true);
+	} else if (busy >= 0)
+		sc->hw_busy_count = 0;
 
 out:
 	ath9k_ps_restore(sc);
@@ -604,7 +641,15 @@ void ath9k_tasklet(unsigned long data)
 	ath9k_ps_wakeup(sc);
 	spin_lock(&sc->sc_pcu_lock);
 
-	if (!ath9k_hw_check_alive(ah))
+	/*
+	 * Only run the baseband hang check if beacons stop working in AP or
+	 * IBSS mode, because it has a high false positive rate. For station
+	 * mode it should not be necessary, since the upper layers will detect
+	 * this through a beacon miss automatically and the following channel
+	 * change will trigger a hardware reset anyway
+	 */
+	if (ath9k_hw_numtxpending(ah, sc->beacon.beaconq) != 0 &&
+	    !ath9k_hw_check_alive(ah))
 		ieee80211_queue_work(sc->hw, &sc->hw_check_work);
 
 	if (ah->caps.hw_caps & ATH9K_HW_CAP_EDMA)
@@ -783,54 +828,11 @@ chip_reset:
 #undef SCHED_INTR
 }
 
-static u32 ath_get_extchanmode(struct ath_softc *sc,
-			       struct ieee80211_channel *chan,
-			       enum nl80211_channel_type channel_type)
-{
-	u32 chanmode = 0;
-
-	switch (chan->band) {
-	case IEEE80211_BAND_2GHZ:
-		switch(channel_type) {
-		case NL80211_CHAN_NO_HT:
-		case NL80211_CHAN_HT20:
-			chanmode = CHANNEL_G_HT20;
-			break;
-		case NL80211_CHAN_HT40PLUS:
-			chanmode = CHANNEL_G_HT40PLUS;
-			break;
-		case NL80211_CHAN_HT40MINUS:
-			chanmode = CHANNEL_G_HT40MINUS;
-			break;
-		}
-		break;
-	case IEEE80211_BAND_5GHZ:
-		switch(channel_type) {
-		case NL80211_CHAN_NO_HT:
-		case NL80211_CHAN_HT20:
-			chanmode = CHANNEL_A_HT20;
-			break;
-		case NL80211_CHAN_HT40PLUS:
-			chanmode = CHANNEL_A_HT40PLUS;
-			break;
-		case NL80211_CHAN_HT40MINUS:
-			chanmode = CHANNEL_A_HT40MINUS;
-			break;
-		}
-		break;
-	default:
-		break;
-	}
-
-	return chanmode;
-}
-
 static void ath9k_bss_assoc_info(struct ath_softc *sc,
 				 struct ieee80211_hw *hw,
 				 struct ieee80211_vif *vif,
 				 struct ieee80211_bss_conf *bss_conf)
 {
-	struct ath_wiphy *aphy = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 
@@ -854,7 +856,7 @@ static void ath9k_bss_assoc_info(struct ath_softc *sc,
 		ath_beacon_config(sc, vif);
 
 		/* Reset rssi stats */
-		aphy->last_rssi = ATH_RSSI_DUMMY_MARKER;
+		sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
 		sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
 
 		sc->sc_flags |= SC_OP_ANI_RUN;
@@ -881,7 +883,7 @@ void ath_radio_enable(struct ath_softc *sc, struct ieee80211_hw *hw)
 	ath9k_hw_configpcipowersave(ah, 0, 0);
 
 	if (!ah->curchan)
-		ah->curchan = ath_get_curchannel(sc, sc->hw);
+		ah->curchan = ath9k_cmn_get_curchannel(sc->hw, ah);
 
 	r = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
 	if (r) {
@@ -890,7 +892,8 @@ void ath_radio_enable(struct ath_softc *sc, struct ieee80211_hw *hw)
 			channel->center_freq, r);
 	}
 
-	ath_update_txpow(sc);
+	ath9k_cmn_update_txpow(ah, sc->curtxpow,
+			       sc->config.txpowlimit, &sc->curtxpow);
 	if (ath_startrecv(sc) != 0) {
 		ath_err(common, "Unable to restart recv logic\n");
 		goto out;
@@ -907,6 +910,8 @@ void ath_radio_enable(struct ath_softc *sc, struct ieee80211_hw *hw)
 	ath9k_hw_set_gpio(ah, ah->led_pin, 0);
 
 	ieee80211_wake_queues(hw);
+	ieee80211_queue_delayed_work(hw, &sc->hw_pll_work, HZ/2);
+
 out:
 	spin_unlock_bh(&sc->sc_pcu_lock);
 
@@ -920,6 +925,8 @@ void ath_radio_disable(struct ath_softc *sc, struct ieee80211_hw *hw)
 	int r;
 
 	ath9k_ps_wakeup(sc);
+	cancel_delayed_work_sync(&sc->hw_pll_work);
+
 	spin_lock_bh(&sc->sc_pcu_lock);
 
 	ieee80211_stop_queues(hw);
@@ -942,7 +949,7 @@ void ath_radio_disable(struct ath_softc *sc, struct ieee80211_hw *hw)
 	ath_flushrecv(sc);		/* flush recv queue */
 
 	if (!ah->curchan)
-		ah->curchan = ath_get_curchannel(sc, hw);
+		ah->curchan = ath9k_cmn_get_curchannel(hw, ah);
 
 	r = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
 	if (r) {
@@ -966,6 +973,8 @@ int ath_reset(struct ath_softc *sc, bool retry_tx)
 	struct ieee80211_hw *hw = sc->hw;
 	int r;
 
+	sc->hw_busy_count = 0;
+
 	/* Stop ANI */
 	del_timer_sync(&common->ani.timer);
 
@@ -993,7 +1002,8 @@ int ath_reset(struct ath_softc *sc, bool retry_tx)
 	 * that changes the channel so update any state that
 	 * might change as a result.
 	 */
-	ath_update_txpow(sc);
+	ath9k_cmn_update_txpow(ah, sc->curtxpow,
+			       sc->config.txpowlimit, &sc->curtxpow);
 
 	if ((sc->sc_flags & SC_OP_BEACONS) || !(sc->sc_flags & (SC_OP_OFFCHANNEL)))
 		ath_beacon_config(sc, NULL);	/* restart beacons */
@@ -1021,38 +1031,13 @@ int ath_reset(struct ath_softc *sc, bool retry_tx)
 	return r;
 }
 
-/* XXX: Remove me once we don't depend on ath9k_channel for all
- * this redundant data */
-void ath9k_update_ichannel(struct ath_softc *sc, struct ieee80211_hw *hw,
-			   struct ath9k_channel *ichan)
-{
-	struct ieee80211_channel *chan = hw->conf.channel;
-	struct ieee80211_conf *conf = &hw->conf;
-
-	ichan->channel = chan->center_freq;
-	ichan->chan = chan;
-
-	if (chan->band == IEEE80211_BAND_2GHZ) {
-		ichan->chanmode = CHANNEL_G;
-		ichan->channelFlags = CHANNEL_2GHZ | CHANNEL_OFDM | CHANNEL_G;
-	} else {
-		ichan->chanmode = CHANNEL_A;
-		ichan->channelFlags = CHANNEL_5GHZ | CHANNEL_OFDM;
-	}
-
-	if (conf_is_ht(conf))
-		ichan->chanmode = ath_get_extchanmode(sc, chan,
-					    conf->channel_type);
-}
-
 /**********************/
 /* mac80211 callbacks */
 /**********************/
 
 static int ath9k_start(struct ieee80211_hw *hw)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_channel *curchan = hw->conf.channel;
@@ -1065,32 +1050,10 @@ static int ath9k_start(struct ieee80211_hw *hw)
 
 	mutex_lock(&sc->mutex);
 
-	if (ath9k_wiphy_started(sc)) {
-		if (sc->chan_idx == curchan->hw_value) {
-			/*
-			 * Already on the operational channel, the new wiphy
-			 * can be marked active.
-			 */
-			aphy->state = ATH_WIPHY_ACTIVE;
-			ieee80211_wake_queues(hw);
-		} else {
-			/*
-			 * Another wiphy is on another channel, start the new
-			 * wiphy in paused state.
-			 */
-			aphy->state = ATH_WIPHY_PAUSED;
-			ieee80211_stop_queues(hw);
-		}
-		mutex_unlock(&sc->mutex);
-		return 0;
-	}
-	aphy->state = ATH_WIPHY_ACTIVE;
-
 	/* setup initial channel */
-
 	sc->chan_idx = curchan->hw_value;
 
-	init_channel = ath_get_curchannel(sc, hw);
+	init_channel = ath9k_cmn_get_curchannel(hw, ah);
 
 	/* Reset SERDES registers */
 	ath9k_hw_configpcipowersave(ah, 0, 0);
@@ -1116,7 +1079,8 @@ static int ath9k_start(struct ieee80211_hw *hw)
 	 * This is needed only to setup initial state
 	 * but it's best done after a reset.
 	 */
-	ath_update_txpow(sc);
+	ath9k_cmn_update_txpow(ah, sc->curtxpow,
+			sc->config.txpowlimit, &sc->curtxpow);
 
 	/*
 	 * Setup the hardware after reset:
@@ -1173,12 +1137,6 @@ static int ath9k_start(struct ieee80211_hw *hw)
 			ath9k_btcoex_timer_resume(sc);
 	}
 
-	/* User has the option to provide pm-qos value as a module
-	 * parameter rather than using the default value of
-	 * 'ATH9K_PM_QOS_DEFAULT_VALUE'.
-	 */
-	pm_qos_update_request(&sc->pm_qos_req, ath9k_pm_qos_value);
-
 	if (ah->caps.pcie_lcr_extsync_en && common->bus_ops->extn_synch_en)
 		common->bus_ops->extn_synch_en(common);
 
@@ -1188,22 +1146,13 @@ mutex_unlock:
 	return r;
 }
 
-static int ath9k_tx(struct ieee80211_hw *hw,
-		    struct sk_buff *skb)
+static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_tx_control txctl;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 
-	if (aphy->state != ATH_WIPHY_ACTIVE && aphy->state != ATH_WIPHY_SCAN) {
-		ath_dbg(common, ATH_DBG_XMIT,
-			"ath9k: %s: TX in unexpected wiphy state %d\n",
-			wiphy_name(hw->wiphy), aphy->state);
-		goto exit;
-	}
-
 	if (sc->ps_enabled) {
 		/*
 		 * mac80211 does not set PM field for normal data frames, so we
@@ -1254,52 +1203,30 @@ static int ath9k_tx(struct ieee80211_hw *hw,
 		goto exit;
 	}
 
-	return 0;
+	return;
 exit:
 	dev_kfree_skb_any(skb);
-	return 0;
 }
 
 static void ath9k_stop(struct ieee80211_hw *hw)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
-	int i;
 
 	mutex_lock(&sc->mutex);
 
-	aphy->state = ATH_WIPHY_INACTIVE;
-
-	if (led_blink)
-		cancel_delayed_work_sync(&sc->ath_led_blink_work);
-
 	cancel_delayed_work_sync(&sc->tx_complete_work);
+	cancel_delayed_work_sync(&sc->hw_pll_work);
 	cancel_work_sync(&sc->paprd_work);
 	cancel_work_sync(&sc->hw_check_work);
 
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i])
-			break;
-	}
-
-	if (i == sc->num_sec_wiphy) {
-		cancel_delayed_work_sync(&sc->wiphy_work);
-		cancel_work_sync(&sc->chan_work);
-	}
-
 	if (sc->sc_flags & SC_OP_INVALID) {
 		ath_dbg(common, ATH_DBG_ANY, "Device not present\n");
 		mutex_unlock(&sc->mutex);
 		return;
 	}
 
-	if (ath9k_wiphy_started(sc)) {
-		mutex_unlock(&sc->mutex);
-		return; /* another wiphy still in use */
-	}
-
 	/* Ensure HW is awake when we try to shut it down. */
 	ath9k_ps_wakeup(sc);
 
@@ -1325,6 +1252,11 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 	} else
 		sc->rx.rxlink = NULL;
 
+	if (sc->rx.frag) {
+		dev_kfree_skb_any(sc->rx.frag);
+		sc->rx.frag = NULL;
+	}
+
 	/* disable HAL and put h/w to sleep */
 	ath9k_hw_disable(ah);
 	ath9k_hw_configpcipowersave(ah, 1, 1);
@@ -1340,124 +1272,234 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 	ath9k_ps_restore(sc);
 
 	sc->ps_idle = true;
-	ath9k_set_wiphy_idle(aphy, true);
 	ath_radio_disable(sc, hw);
 
 	sc->sc_flags |= SC_OP_INVALID;
 
-	pm_qos_update_request(&sc->pm_qos_req, PM_QOS_DEFAULT_VALUE);
-
 	mutex_unlock(&sc->mutex);
 
 	ath_dbg(common, ATH_DBG_CONFIG, "Driver halt\n");
 }
 
-static int ath9k_add_interface(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif)
+bool ath9k_uses_beacons(int type)
+{
+	switch (type) {
+	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void ath9k_reclaim_beacon(struct ath_softc *sc,
+				 struct ieee80211_vif *vif)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
-	struct ath_hw *ah = sc->sc_ah;
-	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
-	enum nl80211_iftype ic_opmode = NL80211_IFTYPE_UNSPECIFIED;
-	int ret = 0;
 
-	mutex_lock(&sc->mutex);
+	ath9k_set_beaconing_status(sc, false);
+	ath_beacon_return(sc, avp);
+	ath9k_set_beaconing_status(sc, true);
+	sc->sc_flags &= ~SC_OP_BEACONS;
+}
+
+static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct ath9k_vif_iter_data *iter_data = data;
+	int i;
+
+	if (iter_data->hw_macaddr)
+		for (i = 0; i < ETH_ALEN; i++)
+			iter_data->mask[i] &=
+				~(iter_data->hw_macaddr[i] ^ mac[i]);
 
 	switch (vif->type) {
-	case NL80211_IFTYPE_STATION:
-		ic_opmode = NL80211_IFTYPE_STATION;
+	case NL80211_IFTYPE_AP:
+		iter_data->naps++;
 		break;
-	case NL80211_IFTYPE_WDS:
-		ic_opmode = NL80211_IFTYPE_WDS;
+	case NL80211_IFTYPE_STATION:
+		iter_data->nstations++;
 		break;
 	case NL80211_IFTYPE_ADHOC:
-	case NL80211_IFTYPE_AP:
+		iter_data->nadhocs++;
+		break;
 	case NL80211_IFTYPE_MESH_POINT:
-		if (sc->nbcnvifs >= ATH_BCBUF) {
-			ret = -ENOBUFS;
-			goto out;
-		}
-		ic_opmode = vif->type;
+		iter_data->nmeshes++;
+		break;
+	case NL80211_IFTYPE_WDS:
+		iter_data->nwds++;
 		break;
 	default:
-		ath_err(common, "Interface type %d not yet supported\n",
-			vif->type);
-		ret = -EOPNOTSUPP;
-		goto out;
+		iter_data->nothers++;
+		break;
 	}
+}
 
-	ath_dbg(common, ATH_DBG_CONFIG,
-		"Attach a VIF of type: %d\n", ic_opmode);
+/* Called with sc->mutex held. */
+void ath9k_calculate_iter_data(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct ath9k_vif_iter_data *iter_data)
+{
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 
-	/* Set the VIF opmode */
-	avp->av_opmode = ic_opmode;
-	avp->av_bslot = -1;
+	/*
+	 * Use the hardware MAC address as reference, the hardware uses it
+	 * together with the BSSID mask when matching addresses.
+	 */
+	memset(iter_data, 0, sizeof(*iter_data));
+	iter_data->hw_macaddr = common->macaddr;
+	memset(&iter_data->mask, 0xff, ETH_ALEN);
 
-	sc->nvifs++;
+	if (vif)
+		ath9k_vif_iter(iter_data, vif->addr, vif);
 
-	ath9k_set_bssid_mask(hw, vif);
+	/* Get list of all active MAC addresses */
+	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath9k_vif_iter,
+						   iter_data);
+}
+
+/* Called with sc->mutex held. */
+static void ath9k_calculate_summary_state(struct ieee80211_hw *hw,
+					  struct ieee80211_vif *vif)
+{
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_vif_iter_data iter_data;
 
-	if (sc->nvifs > 1)
-		goto out; /* skip global settings for secondary vif */
+	ath9k_calculate_iter_data(hw, vif, &iter_data);
 
-	if (ic_opmode == NL80211_IFTYPE_AP) {
+	ath9k_ps_wakeup(sc);
+	/* Set BSSID mask. */
+	memcpy(common->bssidmask, iter_data.mask, ETH_ALEN);
+	ath_hw_setbssidmask(common);
+
+	/* Set op-mode & TSF */
+	if (iter_data.naps > 0) {
 		ath9k_hw_set_tsfadjust(ah, 1);
 		sc->sc_flags |= SC_OP_TSF_RESET;
-	}
+		ah->opmode = NL80211_IFTYPE_AP;
+	} else {
+		ath9k_hw_set_tsfadjust(ah, 0);
+		sc->sc_flags &= ~SC_OP_TSF_RESET;
 
-	/* Set the device opmode */
-	ah->opmode = ic_opmode;
+		if (iter_data.nwds + iter_data.nmeshes)
+			ah->opmode = NL80211_IFTYPE_AP;
+		else if (iter_data.nadhocs)
+			ah->opmode = NL80211_IFTYPE_ADHOC;
+		else
+			ah->opmode = NL80211_IFTYPE_STATION;
+	}
 
 	/*
 	 * Enable MIB interrupts when there are hardware phy counters.
-	 * Note we only do this (at the moment) for station mode.
 	 */
-	if ((vif->type == NL80211_IFTYPE_STATION) ||
-	    (vif->type == NL80211_IFTYPE_ADHOC) ||
-	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
+	if ((iter_data.nstations + iter_data.nadhocs + iter_data.nmeshes) > 0) {
 		if (ah->config.enable_ani)
 			ah->imask |= ATH9K_INT_MIB;
 		ah->imask |= ATH9K_INT_TSFOOR;
+	} else {
+		ah->imask &= ~ATH9K_INT_MIB;
+		ah->imask &= ~ATH9K_INT_TSFOOR;
 	}
 
 	ath9k_hw_set_interrupts(ah, ah->imask);
+	ath9k_ps_restore(sc);
 
-	if (vif->type == NL80211_IFTYPE_AP    ||
-	    vif->type == NL80211_IFTYPE_ADHOC) {
+	/* Set up ANI */
+	if ((iter_data.naps + iter_data.nadhocs) > 0) {
 		sc->sc_flags |= SC_OP_ANI_RUN;
 		ath_start_ani(common);
+	} else {
+		sc->sc_flags &= ~SC_OP_ANI_RUN;
+		del_timer_sync(&common->ani.timer);
 	}
+}
 
-out:
-	mutex_unlock(&sc->mutex);
-	return ret;
+/* Called with sc->mutex held, vif counts set up properly. */
+static void ath9k_do_vif_add_setup(struct ieee80211_hw *hw,
+				   struct ieee80211_vif *vif)
+{
+	struct ath_softc *sc = hw->priv;
+
+	ath9k_calculate_summary_state(hw, vif);
+
+	if (ath9k_uses_beacons(vif->type)) {
+		int error;
+		/* This may fail because upper levels do not have beacons
+		 * properly configured yet.  That's OK, we assume it
+		 * will be properly configured and then we will be notified
+		 * in the info_changed method and set up beacons properly
+		 * there.
+		 */
+		ath9k_set_beaconing_status(sc, false);
+		error = ath_beacon_alloc(sc, vif);
+		if (!error)
+			ath_beacon_config(sc, vif);
+		ath9k_set_beaconing_status(sc, true);
+	}
 }
 
-static void ath9k_reclaim_beacon(struct ath_softc *sc,
-				 struct ieee80211_vif *vif)
+
+static int ath9k_add_interface(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif)
 {
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
+	int ret = 0;
 
-	/* Disable SWBA interrupt */
-	sc->sc_ah->imask &= ~ATH9K_INT_SWBA;
-	ath9k_ps_wakeup(sc);
-	ath9k_hw_set_interrupts(sc->sc_ah, sc->sc_ah->imask);
-	ath9k_hw_stoptxdma(sc->sc_ah, sc->beacon.beaconq);
-	tasklet_kill(&sc->bcon_tasklet);
-	ath9k_ps_restore(sc);
+	mutex_lock(&sc->mutex);
 
-	ath_beacon_return(sc, avp);
-	sc->sc_flags &= ~SC_OP_BEACONS;
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_WDS:
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_MESH_POINT:
+		break;
+	default:
+		ath_err(common, "Interface type %d not yet supported\n",
+			vif->type);
+		ret = -EOPNOTSUPP;
+		goto out;
+	}
 
-	if (sc->nbcnvifs > 0) {
-		/* Re-enable beaconing */
-		sc->sc_ah->imask |= ATH9K_INT_SWBA;
-		ath9k_ps_wakeup(sc);
-		ath9k_hw_set_interrupts(sc->sc_ah, sc->sc_ah->imask);
-		ath9k_ps_restore(sc);
+	if (ath9k_uses_beacons(vif->type)) {
+		if (sc->nbcnvifs >= ATH_BCBUF) {
+			ath_err(common, "Not enough beacon buffers when adding"
+				" new interface of type: %i\n",
+				vif->type);
+			ret = -ENOBUFS;
+			goto out;
+		}
+	}
+
+	if ((vif->type == NL80211_IFTYPE_ADHOC) &&
+	    sc->nvifs > 0) {
+		ath_err(common, "Cannot create ADHOC interface when other"
+			" interfaces already exist.\n");
+		ret = -EINVAL;
+		goto out;
 	}
+
+	ath_dbg(common, ATH_DBG_CONFIG,
+		"Attach a VIF of type: %d\n", vif->type);
+
+	/* Set the VIF opmode */
+	avp->av_opmode = vif->type;
+	avp->av_bslot = -1;
+
+	sc->nvifs++;
+
+	ath9k_do_vif_add_setup(hw, vif);
+out:
+	mutex_unlock(&sc->mutex);
+	return ret;
 }
 
 static int ath9k_change_interface(struct ieee80211_hw *hw,
@@ -1465,40 +1507,40 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
 				  enum nl80211_iftype new_type,
 				  bool p2p)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	int ret = 0;
 
 	ath_dbg(common, ATH_DBG_CONFIG, "Change Interface\n");
 	mutex_lock(&sc->mutex);
 
-	switch (new_type) {
-	case NL80211_IFTYPE_AP:
-	case NL80211_IFTYPE_ADHOC:
+	/* See if new interface type is valid. */
+	if ((new_type == NL80211_IFTYPE_ADHOC) &&
+	    (sc->nvifs > 1)) {
+		ath_err(common, "When using ADHOC, it must be the only"
+			" interface.\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (ath9k_uses_beacons(new_type) &&
+	    !ath9k_uses_beacons(vif->type)) {
 		if (sc->nbcnvifs >= ATH_BCBUF) {
 			ath_err(common, "No beacon slot available\n");
 			ret = -ENOBUFS;
 			goto out;
 		}
-		break;
-	case NL80211_IFTYPE_STATION:
-		/* Stop ANI */
-		sc->sc_flags &= ~SC_OP_ANI_RUN;
-		del_timer_sync(&common->ani.timer);
-		if ((vif->type == NL80211_IFTYPE_AP) ||
-		    (vif->type == NL80211_IFTYPE_ADHOC))
-			ath9k_reclaim_beacon(sc, vif);
-		break;
-	default:
-		ath_err(common, "Interface type %d not yet supported\n",
-				vif->type);
-		ret = -ENOTSUPP;
-		goto out;
 	}
+
+	/* Clean up old vif stuff */
+	if (ath9k_uses_beacons(vif->type))
+		ath9k_reclaim_beacon(sc, vif);
+
+	/* Add new settings */
 	vif->type = new_type;
 	vif->p2p = p2p;
 
+	ath9k_do_vif_add_setup(hw, vif);
 out:
 	mutex_unlock(&sc->mutex);
 	return ret;
@@ -1507,25 +1549,20 @@ out:
 static void ath9k_remove_interface(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 
 	ath_dbg(common, ATH_DBG_CONFIG, "Detach Interface\n");
 
 	mutex_lock(&sc->mutex);
 
-	/* Stop ANI */
-	sc->sc_flags &= ~SC_OP_ANI_RUN;
-	del_timer_sync(&common->ani.timer);
+	sc->nvifs--;
 
 	/* Reclaim beacon resources */
-	if ((sc->sc_ah->opmode == NL80211_IFTYPE_AP) ||
-	    (sc->sc_ah->opmode == NL80211_IFTYPE_ADHOC) ||
-	    (sc->sc_ah->opmode == NL80211_IFTYPE_MESH_POINT))
+	if (ath9k_uses_beacons(vif->type))
 		ath9k_reclaim_beacon(sc, vif);
 
-	sc->nvifs--;
+	ath9k_calculate_summary_state(hw, NULL);
 
 	mutex_unlock(&sc->mutex);
 }
@@ -1566,12 +1603,11 @@ static void ath9k_disable_ps(struct ath_softc *sc)
 
 static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_conf *conf = &hw->conf;
-	bool disable_radio;
+	bool disable_radio = false;
 
 	mutex_lock(&sc->mutex);
 
@@ -1582,29 +1618,13 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 	 * the end.
 	 */
 	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-		bool enable_radio;
-		bool all_wiphys_idle;
-		bool idle = !!(conf->flags & IEEE80211_CONF_IDLE);
-
-		spin_lock_bh(&sc->wiphy_lock);
-		all_wiphys_idle =  ath9k_all_wiphys_idle(sc);
-		ath9k_set_wiphy_idle(aphy, idle);
-
-		enable_radio = (!idle && all_wiphys_idle);
-
-		/*
-		 * After we unlock here its possible another wiphy
-		 * can be re-renabled so to account for that we will
-		 * only disable the radio toward the end of this routine
-		 * if by then all wiphys are still idle.
-		 */
-		spin_unlock_bh(&sc->wiphy_lock);
-
-		if (enable_radio) {
-			sc->ps_idle = false;
+		sc->ps_idle = !!(conf->flags & IEEE80211_CONF_IDLE);
+		if (!sc->ps_idle) {
 			ath_radio_enable(sc, hw);
 			ath_dbg(common, ATH_DBG_CONFIG,
 				"not-idle: enabling radio\n");
+		} else {
+			disable_radio = true;
 		}
 	}
 
@@ -1645,29 +1665,17 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 		if (ah->curchan)
 			old_pos = ah->curchan - &ah->channels[0];
 
-		aphy->chan_idx = pos;
-		aphy->chan_is_ht = conf_is_ht(conf);
 		if (hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)
 			sc->sc_flags |= SC_OP_OFFCHANNEL;
 		else
 			sc->sc_flags &= ~SC_OP_OFFCHANNEL;
 
-		if (aphy->state == ATH_WIPHY_SCAN ||
-		    aphy->state == ATH_WIPHY_ACTIVE)
-			ath9k_wiphy_pause_all_forced(sc, aphy);
-		else {
-			/*
-			 * Do not change operational channel based on a paused
-			 * wiphy changes.
-			 */
-			goto skip_chan_change;
-		}
-
-		ath_dbg(common, ATH_DBG_CONFIG, "Set channel: %d MHz\n",
-			curchan->center_freq);
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Set channel: %d MHz type: %d\n",
+			curchan->center_freq, conf->channel_type);
 
-		/* XXX: remove me eventualy */
-		ath9k_update_ichannel(sc, hw, &sc->sc_ah->channels[pos]);
+		ath9k_cmn_update_ichannel(&sc->sc_ah->channels[pos],
+					  curchan, conf->channel_type);
 
 		/* update survey stats for the old channel before switching */
 		spin_lock_irqsave(&common->cc_lock, flags);
@@ -1709,21 +1717,18 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 			ath_update_survey_nf(sc, old_pos);
 	}
 
-skip_chan_change:
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		ath_dbg(common, ATH_DBG_CONFIG,
+			"Set power: %d\n", conf->power_level);
 		sc->config.txpowlimit = 2 * conf->power_level;
 		ath9k_ps_wakeup(sc);
-		ath_update_txpow(sc);
+		ath9k_cmn_update_txpow(ah, sc->curtxpow,
+				       sc->config.txpowlimit, &sc->curtxpow);
 		ath9k_ps_restore(sc);
 	}
 
-	spin_lock_bh(&sc->wiphy_lock);
-	disable_radio = ath9k_all_wiphys_idle(sc);
-	spin_unlock_bh(&sc->wiphy_lock);
-
 	if (disable_radio) {
 		ath_dbg(common, ATH_DBG_CONFIG, "idle: disabling radio\n");
-		sc->ps_idle = true;
 		ath_radio_disable(sc, hw);
 	}
 
@@ -1748,8 +1753,7 @@ static void ath9k_configure_filter(struct ieee80211_hw *hw,
 				   unsigned int *total_flags,
 				   u64 multicast)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	u32 rfilt;
 
 	changed_flags &= SUPPORTED_FILTERS;
@@ -1769,8 +1773,7 @@ static int ath9k_sta_add(struct ieee80211_hw *hw,
 			 struct ieee80211_vif *vif,
 			 struct ieee80211_sta *sta)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 
 	ath_node_attach(sc, sta);
 
@@ -1781,8 +1784,7 @@ static int ath9k_sta_remove(struct ieee80211_hw *hw,
 			    struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 
 	ath_node_detach(sc, sta);
 
@@ -1792,8 +1794,7 @@ static int ath9k_sta_remove(struct ieee80211_hw *hw,
 static int ath9k_conf_tx(struct ieee80211_hw *hw, u16 queue,
 			 const struct ieee80211_tx_queue_params *params)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_txq *txq;
 	struct ath9k_tx_queue_info qi;
@@ -1837,8 +1838,7 @@ static int ath9k_set_key(struct ieee80211_hw *hw,
 			 struct ieee80211_sta *sta,
 			 struct ieee80211_key_conf *key)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	int ret = 0;
 
@@ -1882,8 +1882,8 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 				   struct ieee80211_bss_conf *bss_conf,
 				   u32 changed)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
@@ -1912,10 +1912,11 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	/* Enable transmission of beacons (AP, IBSS, MESH) */
 	if ((changed & BSS_CHANGED_BEACON) ||
 	    ((changed & BSS_CHANGED_BEACON_ENABLED) && bss_conf->enable_beacon)) {
-		ath9k_hw_stoptxdma(sc->sc_ah, sc->beacon.beaconq);
-		error = ath_beacon_alloc(aphy, vif);
+		ath9k_set_beaconing_status(sc, false);
+		error = ath_beacon_alloc(sc, vif);
 		if (!error)
 			ath_beacon_config(sc, vif);
+		ath9k_set_beaconing_status(sc, true);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -1938,21 +1939,26 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	}
 
 	/* Disable transmission of beacons */
-	if ((changed & BSS_CHANGED_BEACON_ENABLED) && !bss_conf->enable_beacon)
-		ath9k_hw_stoptxdma(sc->sc_ah, sc->beacon.beaconq);
+	if ((changed & BSS_CHANGED_BEACON_ENABLED) &&
+	    !bss_conf->enable_beacon) {
+		ath9k_set_beaconing_status(sc, false);
+		avp->is_bslot_active = false;
+		ath9k_set_beaconing_status(sc, true);
+	}
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		sc->beacon_interval = bss_conf->beacon_int;
+		cur_conf->beacon_interval = bss_conf->beacon_int;
 		/*
 		 * In case of AP mode, the HW TSF has to be reset
 		 * when the beacon interval changes.
 		 */
 		if (vif->type == NL80211_IFTYPE_AP) {
 			sc->sc_flags |= SC_OP_TSF_RESET;
-			ath9k_hw_stoptxdma(sc->sc_ah, sc->beacon.beaconq);
-			error = ath_beacon_alloc(aphy, vif);
+			ath9k_set_beaconing_status(sc, false);
+			error = ath_beacon_alloc(sc, vif);
 			if (!error)
 				ath_beacon_config(sc, vif);
+			ath9k_set_beaconing_status(sc, true);
 		} else {
 			ath_beacon_config(sc, vif);
 		}
@@ -1988,9 +1994,8 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 
 static u64 ath9k_get_tsf(struct ieee80211_hw *hw)
 {
+	struct ath_softc *sc = hw->priv;
 	u64 tsf;
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
 
 	mutex_lock(&sc->mutex);
 	ath9k_ps_wakeup(sc);
@@ -2003,8 +2008,7 @@ static u64 ath9k_get_tsf(struct ieee80211_hw *hw)
 
 static void ath9k_set_tsf(struct ieee80211_hw *hw, u64 tsf)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 
 	mutex_lock(&sc->mutex);
 	ath9k_ps_wakeup(sc);
@@ -2015,8 +2019,7 @@ static void ath9k_set_tsf(struct ieee80211_hw *hw, u64 tsf)
 
 static void ath9k_reset_tsf(struct ieee80211_hw *hw)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 
 	mutex_lock(&sc->mutex);
 
@@ -2031,10 +2034,9 @@ static int ath9k_ampdu_action(struct ieee80211_hw *hw,
 			      struct ieee80211_vif *vif,
 			      enum ieee80211_ampdu_mlme_action action,
 			      struct ieee80211_sta *sta,
-			      u16 tid, u16 *ssn)
+			      u16 tid, u16 *ssn, u8 buf_size)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	int ret = 0;
 
 	local_bh_disable();
@@ -2079,8 +2081,7 @@ static int ath9k_ampdu_action(struct ieee80211_hw *hw,
 static int ath9k_get_survey(struct ieee80211_hw *hw, int idx,
 			     struct survey_info *survey)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_channel *chan;
@@ -2114,52 +2115,68 @@ static int ath9k_get_survey(struct ieee80211_hw *hw, int idx,
 	return 0;
 }
 
-static void ath9k_sw_scan_start(struct ieee80211_hw *hw)
+static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
 
 	mutex_lock(&sc->mutex);
-	if (ath9k_wiphy_scanning(sc)) {
-		/*
-		 * There is a race here in mac80211 but fixing it requires
-		 * we revisit how we handle the scan complete callback.
-		 * After mac80211 fixes we will not have configured hardware
-		 * to the home channel nor would we have configured the RX
-		 * filter yet.
-		 */
-		mutex_unlock(&sc->mutex);
-		return;
-	}
-
-	aphy->state = ATH_WIPHY_SCAN;
-	ath9k_wiphy_pause_all_forced(sc, aphy);
+	ah->coverage_class = coverage_class;
+	ath9k_hw_init_global_settings(ah);
 	mutex_unlock(&sc->mutex);
 }
 
-/*
- * XXX: this requires a revisit after the driver
- * scan_complete gets moved to another place/removed in mac80211.
- */
-static void ath9k_sw_scan_complete(struct ieee80211_hw *hw)
+static void ath9k_flush(struct ieee80211_hw *hw, bool drop)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+#define ATH_FLUSH_TIMEOUT	60 /* ms */
+	struct ath_softc *sc = hw->priv;
+	struct ath_txq *txq = NULL;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	int i, j, npend = 0;
 
 	mutex_lock(&sc->mutex);
-	aphy->state = ATH_WIPHY_ACTIVE;
-	mutex_unlock(&sc->mutex);
-}
 
-static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
-{
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
-	struct ath_hw *ah = sc->sc_ah;
+	cancel_delayed_work_sync(&sc->tx_complete_work);
 
-	mutex_lock(&sc->mutex);
-	ah->coverage_class = coverage_class;
-	ath9k_hw_init_global_settings(ah);
+	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
+		if (!ATH_TXQ_SETUP(sc, i))
+			continue;
+		txq = &sc->tx.txq[i];
+
+		if (!drop) {
+			for (j = 0; j < ATH_FLUSH_TIMEOUT; j++) {
+				if (!ath9k_has_pending_frames(sc, txq))
+					break;
+				usleep_range(1000, 2000);
+			}
+		}
+
+		if (drop || ath9k_has_pending_frames(sc, txq)) {
+			ath_dbg(common, ATH_DBG_QUEUE, "Drop frames from hw queue:%d\n",
+				txq->axq_qnum);
+			spin_lock_bh(&txq->axq_lock);
+			txq->txq_flush_inprogress = true;
+			spin_unlock_bh(&txq->axq_lock);
+
+			ath9k_ps_wakeup(sc);
+			ath9k_hw_stoptxdma(ah, txq->axq_qnum);
+			npend = ath9k_hw_numtxpending(ah, txq->axq_qnum);
+			ath9k_ps_restore(sc);
+			if (npend)
+				break;
+
+			ath_draintxq(sc, txq, false);
+			txq->txq_flush_inprogress = false;
+		}
+	}
+
+	if (npend) {
+		ath_reset(sc, false);
+		txq->txq_flush_inprogress = false;
+	}
+
+	ieee80211_queue_delayed_work(hw, &sc->tx_complete_work, 0);
 	mutex_unlock(&sc->mutex);
 }
 
@@ -2182,8 +2199,7 @@ struct ieee80211_ops ath9k_ops = {
 	.reset_tsf 	    = ath9k_reset_tsf,
 	.ampdu_action       = ath9k_ampdu_action,
 	.get_survey	    = ath9k_get_survey,
-	.sw_scan_start      = ath9k_sw_scan_start,
-	.sw_scan_complete   = ath9k_sw_scan_complete,
 	.rfkill_poll        = ath9k_rfkill_poll_state,
 	.set_coverage_class = ath9k_set_coverage_class,
+	.flush		    = ath9k_flush,
 };
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index 78ef1f13386f..e83128c50f7b 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -126,7 +126,6 @@ static const struct ath_bus_ops ath_pci_bus_ops = {
 static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	void __iomem *mem;
-	struct ath_wiphy *aphy;
 	struct ath_softc *sc;
 	struct ieee80211_hw *hw;
 	u8 csz;
@@ -198,8 +197,7 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto err_iomap;
 	}
 
-	hw = ieee80211_alloc_hw(sizeof(struct ath_wiphy) +
-				sizeof(struct ath_softc), &ath9k_ops);
+	hw = ieee80211_alloc_hw(sizeof(struct ath_softc), &ath9k_ops);
 	if (!hw) {
 		dev_err(&pdev->dev, "No memory for ieee80211_hw\n");
 		ret = -ENOMEM;
@@ -209,11 +207,7 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	SET_IEEE80211_DEV(hw, &pdev->dev);
 	pci_set_drvdata(pdev, hw);
 
-	aphy = hw->priv;
-	sc = (struct ath_softc *) (aphy + 1);
-	aphy->sc = sc;
-	aphy->hw = hw;
-	sc->pri_wiphy = aphy;
+	sc = hw->priv;
 	sc->hw = hw;
 	sc->dev = &pdev->dev;
 	sc->mem = mem;
@@ -260,8 +254,7 @@ err_dma:
 static void ath_pci_remove(struct pci_dev *pdev)
 {
 	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	void __iomem *mem = sc->mem;
 
 	if (!is_ath9k_unloaded)
@@ -281,8 +274,7 @@ static int ath_pci_suspend(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 
 	ath9k_hw_set_gpio(sc->sc_ah, sc->sc_ah->led_pin, 1);
 
@@ -293,8 +285,7 @@ static int ath_pci_resume(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	u32 val;
 
 	/*
@@ -320,7 +311,6 @@ static int ath_pci_resume(struct device *device)
 	ath9k_ps_restore(sc);
 
 	sc->ps_idle = true;
-	ath9k_set_wiphy_idle(aphy, true);
 	ath_radio_disable(sc, hw);
 
 	return 0;
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
index e45147820eae..960d717ca7c2 100644
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ b/drivers/net/wireless/ath/ath9k/rc.c
@@ -1560,8 +1560,7 @@ static void ath_rate_add_sta_debugfs(void *priv, void *priv_sta,
 
 static void *ath_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	return aphy->sc;
+	return hw->priv;
 }
 
 static void ath_rate_free(void *priv)
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index b2497b8601e5..cb559e345b86 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -34,27 +34,6 @@ static inline bool ath9k_check_auto_sleep(struct ath_softc *sc)
 	       (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP);
 }
 
-static struct ieee80211_hw * ath_get_virt_hw(struct ath_softc *sc,
-					     struct ieee80211_hdr *hdr)
-{
-	struct ieee80211_hw *hw = sc->pri_wiphy->hw;
-	int i;
-
-	spin_lock_bh(&sc->wiphy_lock);
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy = sc->sec_wiphy[i];
-		if (aphy == NULL)
-			continue;
-		if (compare_ether_addr(hdr->addr1, aphy->hw->wiphy->perm_addr)
-		    == 0) {
-			hw = aphy->hw;
-			break;
-		}
-	}
-	spin_unlock_bh(&sc->wiphy_lock);
-	return hw;
-}
-
 /*
  * Setup and link descriptors.
  *
@@ -230,11 +209,6 @@ static int ath_rx_edma_init(struct ath_softc *sc, int nbufs)
 	int error = 0, i;
 	u32 size;
 
-
-	common->rx_bufsize = roundup(IEEE80211_MAX_MPDU_LEN +
-				     ah->caps.rx_status_len,
-				     min(common->cachelsz, (u16)64));
-
 	ath9k_hw_set_rx_bufsize(ah, common->rx_bufsize -
 				    ah->caps.rx_status_len);
 
@@ -321,12 +295,12 @@ int ath_rx_init(struct ath_softc *sc, int nbufs)
 	sc->sc_flags &= ~SC_OP_RXFLUSH;
 	spin_lock_init(&sc->rx.rxbuflock);
 
+	common->rx_bufsize = IEEE80211_MAX_MPDU_LEN / 2 +
+			     sc->sc_ah->caps.rx_status_len;
+
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
 		return ath_rx_edma_init(sc, nbufs);
 	} else {
-		common->rx_bufsize = roundup(IEEE80211_MAX_MPDU_LEN,
-				min(common->cachelsz, (u16)64));
-
 		ath_dbg(common, ATH_DBG_CONFIG, "cachelsz %u rxbufsize %u\n",
 			common->cachelsz, common->rx_bufsize);
 
@@ -463,8 +437,7 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	if (conf_is_ht(&sc->hw->conf))
 		rfilt |= ATH9K_RX_FILTER_COMP_BAR;
 
-	if (sc->sec_wiphy || (sc->nvifs > 1) ||
-	    (sc->rx.rxfilter & FIF_OTHER_BSS)) {
+	if (sc->nvifs > 1 || (sc->rx.rxfilter & FIF_OTHER_BSS)) {
 		/* The following may also be needed for other older chips */
 		if (sc->sc_ah->hw_version.macVersion == AR_SREV_VERSION_9160)
 			rfilt |= ATH9K_RX_FILTER_PROM;
@@ -588,8 +561,14 @@ static void ath_rx_ps_beacon(struct ath_softc *sc, struct sk_buff *skb)
 		return;
 
 	mgmt = (struct ieee80211_mgmt *)skb->data;
-	if (memcmp(common->curbssid, mgmt->bssid, ETH_ALEN) != 0)
+	if (memcmp(common->curbssid, mgmt->bssid, ETH_ALEN) != 0) {
+		/* TODO:  This doesn't work well if you have stations
+		 * associated to two different APs because curbssid
+		 * is just the last AP that any of the stations associated
+		 * with.
+		 */
 		return; /* not from our current AP */
+	}
 
 	sc->ps_flags &= ~PS_WAIT_FOR_BEACON;
 
@@ -662,37 +641,6 @@ static void ath_rx_ps(struct ath_softc *sc, struct sk_buff *skb)
 	}
 }
 
-static void ath_rx_send_to_mac80211(struct ieee80211_hw *hw,
-				    struct ath_softc *sc, struct sk_buff *skb)
-{
-	struct ieee80211_hdr *hdr;
-
-	hdr = (struct ieee80211_hdr *)skb->data;
-
-	/* Send the frame to mac80211 */
-	if (is_multicast_ether_addr(hdr->addr1)) {
-		int i;
-		/*
-		 * Deliver broadcast/multicast frames to all suitable
-		 * virtual wiphys.
-		 */
-		/* TODO: filter based on channel configuration */
-		for (i = 0; i < sc->num_sec_wiphy; i++) {
-			struct ath_wiphy *aphy = sc->sec_wiphy[i];
-			struct sk_buff *nskb;
-			if (aphy == NULL)
-				continue;
-			nskb = skb_copy(skb, GFP_ATOMIC);
-			if (!nskb)
-				continue;
-			ieee80211_rx(aphy->hw, nskb);
-		}
-		ieee80211_rx(sc->hw, skb);
-	} else
-		/* Deliver unicast frames based on receiver address */
-		ieee80211_rx(hw, skb);
-}
-
 static bool ath_edma_get_buffers(struct ath_softc *sc,
 				 enum ath9k_rx_qtype qtype)
 {
@@ -862,15 +810,9 @@ static bool ath9k_rx_accept(struct ath_common *common,
 	if (rx_stats->rs_datalen > (common->rx_bufsize - rx_status_len))
 		return false;
 
-	/*
-	 * rs_more indicates chained descriptors which can be used
-	 * to link buffers together for a sort of scatter-gather
-	 * operation.
-	 * reject the frame, we don't support scatter-gather yet and
-	 * the frame is probably corrupt anyway
-	 */
+	/* Only use error bits from the last fragment */
 	if (rx_stats->rs_more)
-		return false;
+		return true;
 
 	/*
 	 * The rx_stats->rs_status will not be set until the end of the
@@ -974,7 +916,7 @@ static void ath9k_process_rssi(struct ath_common *common,
 			       struct ieee80211_hdr *hdr,
 			       struct ath_rx_status *rx_stats)
 {
-	struct ath_wiphy *aphy = hw->priv;
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = common->ah;
 	int last_rssi;
 	__le16 fc;
@@ -984,13 +926,19 @@ static void ath9k_process_rssi(struct ath_common *common,
 
 	fc = hdr->frame_control;
 	if (!ieee80211_is_beacon(fc) ||
-	    compare_ether_addr(hdr->addr3, common->curbssid))
+	    compare_ether_addr(hdr->addr3, common->curbssid)) {
+		/* TODO:  This doesn't work well if you have stations
+		 * associated to two different APs because curbssid
+		 * is just the last AP that any of the stations associated
+		 * with.
+		 */
 		return;
+	}
 
 	if (rx_stats->rs_rssi != ATH9K_RSSI_BAD && !rx_stats->rs_moreaggr)
-		ATH_RSSI_LPF(aphy->last_rssi, rx_stats->rs_rssi);
+		ATH_RSSI_LPF(sc->last_rssi, rx_stats->rs_rssi);
 
-	last_rssi = aphy->last_rssi;
+	last_rssi = sc->last_rssi;
 	if (likely(last_rssi != ATH_RSSI_DUMMY_MARKER))
 		rx_stats->rs_rssi = ATH_EP_RND(last_rssi,
 					      ATH_RSSI_EP_MULTIPLIER);
@@ -1022,6 +970,10 @@ static int ath9k_rx_skb_preprocess(struct ath_common *common,
 	if (!ath9k_rx_accept(common, hdr, rx_status, rx_stats, decrypt_error))
 		return -EINVAL;
 
+	/* Only use status info from the last fragment */
+	if (rx_stats->rs_more)
+		return 0;
+
 	ath9k_process_rssi(common, hw, hdr, rx_stats);
 
 	if (ath9k_process_rate(common, hw, rx_stats, rx_status))
@@ -1031,7 +983,7 @@ static int ath9k_rx_skb_preprocess(struct ath_common *common,
 	rx_status->freq = hw->conf.channel->center_freq;
 	rx_status->signal = ATH_DEFAULT_NOISE_FLOOR + rx_stats->rs_rssi;
 	rx_status->antenna = rx_stats->rs_antenna;
-	rx_status->flag |= RX_FLAG_TSFT;
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
 
 	return 0;
 }
@@ -1623,7 +1575,7 @@ div_comb_done:
 int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 {
 	struct ath_buf *bf;
-	struct sk_buff *skb = NULL, *requeue_skb;
+	struct sk_buff *skb = NULL, *requeue_skb, *hdr_skb;
 	struct ieee80211_rx_status *rxs;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -1632,7 +1584,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 	 * virtual wiphy so to account for that we iterate over the active
 	 * wiphys and find the appropriate wiphy and therefore hw.
 	 */
-	struct ieee80211_hw *hw = NULL;
+	struct ieee80211_hw *hw = sc->hw;
 	struct ieee80211_hdr *hdr;
 	int retval;
 	bool decrypt_error = false;
@@ -1674,10 +1626,17 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		if (!skb)
 			continue;
 
-		hdr = (struct ieee80211_hdr *) (skb->data + rx_status_len);
-		rxs =  IEEE80211_SKB_RXCB(skb);
+		/*
+		 * Take frame header from the first fragment and RX status from
+		 * the last one.
+		 */
+		if (sc->rx.frag)
+			hdr_skb = sc->rx.frag;
+		else
+			hdr_skb = skb;
 
-		hw = ath_get_virt_hw(sc, hdr);
+		hdr = (struct ieee80211_hdr *) (hdr_skb->data + rx_status_len);
+		rxs = IEEE80211_SKB_RXCB(hdr_skb);
 
 		ath_debug_stat_rx(sc, &rs);
 
@@ -1686,12 +1645,12 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		 * chain it back at the queue without processing it.
 		 */
 		if (flush)
-			goto requeue;
+			goto requeue_drop_frag;
 
 		retval = ath9k_rx_skb_preprocess(common, hw, hdr, &rs,
 						 rxs, &decrypt_error);
 		if (retval)
-			goto requeue;
+			goto requeue_drop_frag;
 
 		rxs->mactime = (tsf & ~0xffffffffULL) | rs.rs_tstamp;
 		if (rs.rs_tstamp > tsf_lower &&
@@ -1711,7 +1670,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		 * skb and put it at the tail of the sc->rx.rxbuf list for
 		 * processing. */
 		if (!requeue_skb)
-			goto requeue;
+			goto requeue_drop_frag;
 
 		/* Unmap the frame */
 		dma_unmap_single(sc->dev, bf->bf_buf_addr,
@@ -1722,8 +1681,9 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		if (ah->caps.rx_status_len)
 			skb_pull(skb, ah->caps.rx_status_len);
 
-		ath9k_rx_skb_postprocess(common, skb, &rs,
-					 rxs, decrypt_error);
+		if (!rs.rs_more)
+			ath9k_rx_skb_postprocess(common, hdr_skb, &rs,
+						 rxs, decrypt_error);
 
 		/* We will now give hardware our shiny new allocated skb */
 		bf->bf_mpdu = requeue_skb;
@@ -1736,10 +1696,42 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			bf->bf_mpdu = NULL;
 			bf->bf_buf_addr = 0;
 			ath_err(common, "dma_mapping_error() on RX\n");
-			ath_rx_send_to_mac80211(hw, sc, skb);
+			ieee80211_rx(hw, skb);
 			break;
 		}
 
+		if (rs.rs_more) {
+			/*
+			 * rs_more indicates chained descriptors which can be
+			 * used to link buffers together for a sort of
+			 * scatter-gather operation.
+			 */
+			if (sc->rx.frag) {
+				/* too many fragments - cannot handle frame */
+				dev_kfree_skb_any(sc->rx.frag);
+				dev_kfree_skb_any(skb);
+				skb = NULL;
+			}
+			sc->rx.frag = skb;
+			goto requeue;
+		}
+
+		if (sc->rx.frag) {
+			int space = skb->len - skb_tailroom(hdr_skb);
+
+			sc->rx.frag = NULL;
+
+			if (pskb_expand_head(hdr_skb, 0, space, GFP_ATOMIC) < 0) {
+				dev_kfree_skb(skb);
+				goto requeue_drop_frag;
+			}
+
+			skb_copy_from_linear_data(skb, skb_put(hdr_skb, skb->len),
+						  skb->len);
+			dev_kfree_skb_any(skb);
+			skb = hdr_skb;
+		}
+
 		/*
 		 * change the default rx antenna if rx diversity chooses the
 		 * other antenna 3 times in a row.
@@ -1763,8 +1755,13 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		if (ah->caps.hw_caps & ATH9K_HW_CAP_ANT_DIV_COMB)
 			ath_ant_comb_scan(sc, &rs);
 
-		ath_rx_send_to_mac80211(hw, sc, skb);
+		ieee80211_rx(hw, skb);
 
+requeue_drop_frag:
+		if (sc->rx.frag) {
+			dev_kfree_skb_any(sc->rx.frag);
+			sc->rx.frag = NULL;
+		}
 requeue:
 		if (edma) {
 			list_add_tail(&bf->list, &sc->rx.rxbuf);
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index 4df5659c6c16..8fa8acfde62e 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -789,6 +789,7 @@
 #define AR_SREV_REVISION_9300_20	2 /* 2.0 and 2.1 */
 #define AR_SREV_VERSION_9485		0x240
 #define AR_SREV_REVISION_9485_10	0
+#define AR_SREV_REVISION_9485_11        1
 
 #define AR_SREV_5416(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_5416_PCI) || \
@@ -866,6 +867,9 @@
 #define AR_SREV_9485_10(_ah) \
 	(AR_SREV_9485(_ah) && \
 	 ((_ah)->hw_version.macRev == AR_SREV_REVISION_9485_10))
+#define AR_SREV_9485_11(_ah) \
+	(AR_SREV_9485(_ah) && \
+	 ((_ah)->hw_version.macRev == AR_SREV_REVISION_9485_11))
 
 #define AR_SREV_9285E_20(_ah) \
     (AR_SREV_9285_12_OR_LATER(_ah) && \
@@ -874,6 +878,7 @@
 enum ath_usb_dev {
 	AR9280_USB = 1, /* AR7010 + AR9280, UB94 */
 	AR9287_USB = 2, /* AR7010 + AR9287, UB95 */
+	STORAGE_DEVICE = 3,
 };
 
 #define AR_DEVID_7010(_ah) \
@@ -1083,6 +1088,17 @@ enum {
 #define AR_ENT_OTP		  0x40d8
 #define AR_ENT_OTP_CHAIN2_DISABLE               0x00020000
 #define AR_ENT_OTP_MPSD		0x00800000
+#define AR_CH0_BB_DPLL2          0x16184
+#define AR_CH0_BB_DPLL3          0x16188
+#define AR_CH0_DDR_DPLL2         0x16244
+#define AR_CH0_DDR_DPLL3         0x16248
+#define AR_CH0_DPLL2_KD              0x03F80000
+#define AR_CH0_DPLL2_KD_S            19
+#define AR_CH0_DPLL2_KI              0x3C000000
+#define AR_CH0_DPLL2_KI_S            26
+#define AR_CH0_DPLL3_PHASE_SHIFT     0x3F800000
+#define AR_CH0_DPLL3_PHASE_SHIFT_S   23
+#define AR_PHY_CCA_NOM_VAL_2GHZ      -118
 
 #define AR_RTC_9300_PLL_DIV          0x000003ff
 #define AR_RTC_9300_PLL_DIV_S        0
@@ -1129,6 +1145,12 @@ enum {
 #define AR_RTC_PLL_CLKSEL       0x00000300
 #define AR_RTC_PLL_CLKSEL_S     8
 
+#define PLL3 0x16188
+#define PLL3_DO_MEAS_MASK 0x40000000
+#define PLL4 0x1618c
+#define PLL4_MEAS_DONE    0x8
+#define SQSUM_DVC_MASK 0x007ffff8
+
 #define AR_RTC_RESET \
 	((AR_SREV_9100(ah)) ? (AR_RTC_BASE + 0x0040) : 0x7040)
 #define AR_RTC_RESET_EN		(0x00000001)
diff --git a/drivers/net/wireless/ath/ath9k/virtual.c b/drivers/net/wireless/ath/ath9k/virtual.c
deleted file mode 100644
index 2dc7095e56d1..000000000000
--- a/drivers/net/wireless/ath/ath9k/virtual.c
+++ /dev/null
@@ -1,717 +0,0 @@
-/*
- * Copyright (c) 2008-2009 Atheros Communications Inc.
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
- * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
- * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
- * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <linux/slab.h>
-
-#include "ath9k.h"
-
-struct ath9k_vif_iter_data {
-	const u8 *hw_macaddr;
-	u8 mask[ETH_ALEN];
-};
-
-static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
-{
-	struct ath9k_vif_iter_data *iter_data = data;
-	int i;
-
-	for (i = 0; i < ETH_ALEN; i++)
-		iter_data->mask[i] &= ~(iter_data->hw_macaddr[i] ^ mac[i]);
-}
-
-void ath9k_set_bssid_mask(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-{
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath9k_vif_iter_data iter_data;
-	int i;
-
-	/*
-	 * Use the hardware MAC address as reference, the hardware uses it
-	 * together with the BSSID mask when matching addresses.
-	 */
-	iter_data.hw_macaddr = common->macaddr;
-	memset(&iter_data.mask, 0xff, ETH_ALEN);
-
-	if (vif)
-		ath9k_vif_iter(&iter_data, vif->addr, vif);
-
-	/* Get list of all active MAC addresses */
-	spin_lock_bh(&sc->wiphy_lock);
-	ieee80211_iterate_active_interfaces_atomic(sc->hw, ath9k_vif_iter,
-						   &iter_data);
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] == NULL)
-			continue;
-		ieee80211_iterate_active_interfaces_atomic(
-			sc->sec_wiphy[i]->hw, ath9k_vif_iter, &iter_data);
-	}
-	spin_unlock_bh(&sc->wiphy_lock);
-
-	memcpy(common->bssidmask, iter_data.mask, ETH_ALEN);
-	ath_hw_setbssidmask(common);
-}
-
-int ath9k_wiphy_add(struct ath_softc *sc)
-{
-	int i, error;
-	struct ath_wiphy *aphy;
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ieee80211_hw *hw;
-	u8 addr[ETH_ALEN];
-
-	hw = ieee80211_alloc_hw(sizeof(struct ath_wiphy), &ath9k_ops);
-	if (hw == NULL)
-		return -ENOMEM;
-
-	spin_lock_bh(&sc->wiphy_lock);
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] == NULL)
-			break;
-	}
-
-	if (i == sc->num_sec_wiphy) {
-		/* No empty slot available; increase array length */
-		struct ath_wiphy **n;
-		n = krealloc(sc->sec_wiphy,
-			     (sc->num_sec_wiphy + 1) *
-			     sizeof(struct ath_wiphy *),
-			     GFP_ATOMIC);
-		if (n == NULL) {
-			spin_unlock_bh(&sc->wiphy_lock);
-			ieee80211_free_hw(hw);
-			return -ENOMEM;
-		}
-		n[i] = NULL;
-		sc->sec_wiphy = n;
-		sc->num_sec_wiphy++;
-	}
-
-	SET_IEEE80211_DEV(hw, sc->dev);
-
-	aphy = hw->priv;
-	aphy->sc = sc;
-	aphy->hw = hw;
-	sc->sec_wiphy[i] = aphy;
-	aphy->last_rssi = ATH_RSSI_DUMMY_MARKER;
-	spin_unlock_bh(&sc->wiphy_lock);
-
-	memcpy(addr, common->macaddr, ETH_ALEN);
-	addr[0] |= 0x02; /* Locally managed address */
-	/*
-	 * XOR virtual wiphy index into the least significant bits to generate
-	 * a different MAC address for each virtual wiphy.
-	 */
-	addr[5] ^= i & 0xff;
-	addr[4] ^= (i & 0xff00) >> 8;
-	addr[3] ^= (i & 0xff0000) >> 16;
-
-	SET_IEEE80211_PERM_ADDR(hw, addr);
-
-	ath9k_set_hw_capab(sc, hw);
-
-	error = ieee80211_register_hw(hw);
-
-	if (error == 0) {
-		/* Make sure wiphy scheduler is started (if enabled) */
-		ath9k_wiphy_set_scheduler(sc, sc->wiphy_scheduler_int);
-	}
-
-	return error;
-}
-
-int ath9k_wiphy_del(struct ath_wiphy *aphy)
-{
-	struct ath_softc *sc = aphy->sc;
-	int i;
-
-	spin_lock_bh(&sc->wiphy_lock);
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (aphy == sc->sec_wiphy[i]) {
-			sc->sec_wiphy[i] = NULL;
-			spin_unlock_bh(&sc->wiphy_lock);
-			ieee80211_unregister_hw(aphy->hw);
-			ieee80211_free_hw(aphy->hw);
-			return 0;
-		}
-	}
-	spin_unlock_bh(&sc->wiphy_lock);
-	return -ENOENT;
-}
-
-static int ath9k_send_nullfunc(struct ath_wiphy *aphy,
-			       struct ieee80211_vif *vif, const u8 *bssid,
-			       int ps)
-{
-	struct ath_softc *sc = aphy->sc;
-	struct ath_tx_control txctl;
-	struct sk_buff *skb;
-	struct ieee80211_hdr *hdr;
-	__le16 fc;
-	struct ieee80211_tx_info *info;
-
-	skb = dev_alloc_skb(24);
-	if (skb == NULL)
-		return -ENOMEM;
-	hdr = (struct ieee80211_hdr *) skb_put(skb, 24);
-	memset(hdr, 0, 24);
-	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC |
-			 IEEE80211_FCTL_TODS);
-	if (ps)
-		fc |= cpu_to_le16(IEEE80211_FCTL_PM);
-	hdr->frame_control = fc;
-	memcpy(hdr->addr1, bssid, ETH_ALEN);
-	memcpy(hdr->addr2, aphy->hw->wiphy->perm_addr, ETH_ALEN);
-	memcpy(hdr->addr3, bssid, ETH_ALEN);
-
-	info = IEEE80211_SKB_CB(skb);
-	memset(info, 0, sizeof(*info));
-	info->flags = IEEE80211_TX_CTL_REQ_TX_STATUS;
-	info->control.vif = vif;
-	info->control.rates[0].idx = 0;
-	info->control.rates[0].count = 4;
-	info->control.rates[1].idx = -1;
-
-	memset(&txctl, 0, sizeof(struct ath_tx_control));
-	txctl.txq = sc->tx.txq_map[WME_AC_VO];
-	txctl.frame_type = ps ? ATH9K_IFT_PAUSE : ATH9K_IFT_UNPAUSE;
-
-	if (ath_tx_start(aphy->hw, skb, &txctl) != 0)
-		goto exit;
-
-	return 0;
-exit:
-	dev_kfree_skb_any(skb);
-	return -1;
-}
-
-static bool __ath9k_wiphy_pausing(struct ath_softc *sc)
-{
-	int i;
-	if (sc->pri_wiphy->state == ATH_WIPHY_PAUSING)
-		return true;
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] &&
-		    sc->sec_wiphy[i]->state == ATH_WIPHY_PAUSING)
-			return true;
-	}
-	return false;
-}
-
-static bool ath9k_wiphy_pausing(struct ath_softc *sc)
-{
-	bool ret;
-	spin_lock_bh(&sc->wiphy_lock);
-	ret = __ath9k_wiphy_pausing(sc);
-	spin_unlock_bh(&sc->wiphy_lock);
-	return ret;
-}
-
-static bool __ath9k_wiphy_scanning(struct ath_softc *sc)
-{
-	int i;
-	if (sc->pri_wiphy->state == ATH_WIPHY_SCAN)
-		return true;
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] &&
-		    sc->sec_wiphy[i]->state == ATH_WIPHY_SCAN)
-			return true;
-	}
-	return false;
-}
-
-bool ath9k_wiphy_scanning(struct ath_softc *sc)
-{
-	bool ret;
-	spin_lock_bh(&sc->wiphy_lock);
-	ret = __ath9k_wiphy_scanning(sc);
-	spin_unlock_bh(&sc->wiphy_lock);
-	return ret;
-}
-
-static int __ath9k_wiphy_unpause(struct ath_wiphy *aphy);
-
-/* caller must hold wiphy_lock */
-static void __ath9k_wiphy_unpause_ch(struct ath_wiphy *aphy)
-{
-	if (aphy == NULL)
-		return;
-	if (aphy->chan_idx != aphy->sc->chan_idx)
-		return; /* wiphy not on the selected channel */
-	__ath9k_wiphy_unpause(aphy);
-}
-
-static void ath9k_wiphy_unpause_channel(struct ath_softc *sc)
-{
-	int i;
-	spin_lock_bh(&sc->wiphy_lock);
-	__ath9k_wiphy_unpause_ch(sc->pri_wiphy);
-	for (i = 0; i < sc->num_sec_wiphy; i++)
-		__ath9k_wiphy_unpause_ch(sc->sec_wiphy[i]);
-	spin_unlock_bh(&sc->wiphy_lock);
-}
-
-void ath9k_wiphy_chan_work(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc, chan_work);
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath_wiphy *aphy = sc->next_wiphy;
-
-	if (aphy == NULL)
-		return;
-
-	/*
-	 * All pending interfaces paused; ready to change
-	 * channels.
-	 */
-
-	/* Change channels */
-	mutex_lock(&sc->mutex);
-	/* XXX: remove me eventually */
-	ath9k_update_ichannel(sc, aphy->hw,
-			      &sc->sc_ah->channels[sc->chan_idx]);
-
-	/* sync hw configuration for hw code */
-	common->hw = aphy->hw;
-
-	if (ath_set_channel(sc, aphy->hw,
-			    &sc->sc_ah->channels[sc->chan_idx]) < 0) {
-		printk(KERN_DEBUG "ath9k: Failed to set channel for new "
-		       "virtual wiphy\n");
-		mutex_unlock(&sc->mutex);
-		return;
-	}
-	mutex_unlock(&sc->mutex);
-
-	ath9k_wiphy_unpause_channel(sc);
-}
-
-/*
- * ath9k version of ieee80211_tx_status() for TX frames that are generated
- * internally in the driver.
- */
-void ath9k_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb, int ftype)
-{
-	struct ath_wiphy *aphy = hw->priv;
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-
-	if (ftype == ATH9K_IFT_PAUSE && aphy->state == ATH_WIPHY_PAUSING) {
-		if (!(tx_info->flags & IEEE80211_TX_STAT_ACK)) {
-			printk(KERN_DEBUG "ath9k: %s: no ACK for pause "
-			       "frame\n", wiphy_name(hw->wiphy));
-			/*
-			 * The AP did not reply; ignore this to allow us to
-			 * continue.
-			 */
-		}
-		aphy->state = ATH_WIPHY_PAUSED;
-		if (!ath9k_wiphy_pausing(aphy->sc)) {
-			/*
-			 * Drop from tasklet to work to allow mutex for channel
-			 * change.
-			 */
-			ieee80211_queue_work(aphy->sc->hw,
-				   &aphy->sc->chan_work);
-		}
-	}
-
-	dev_kfree_skb(skb);
-}
-
-static void ath9k_mark_paused(struct ath_wiphy *aphy)
-{
-	struct ath_softc *sc = aphy->sc;
-	aphy->state = ATH_WIPHY_PAUSED;
-	if (!__ath9k_wiphy_pausing(sc))
-		ieee80211_queue_work(sc->hw, &sc->chan_work);
-}
-
-static void ath9k_pause_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
-{
-	struct ath_wiphy *aphy = data;
-	struct ath_vif *avp = (void *) vif->drv_priv;
-
-	switch (vif->type) {
-	case NL80211_IFTYPE_STATION:
-		if (!vif->bss_conf.assoc) {
-			ath9k_mark_paused(aphy);
-			break;
-		}
-		/* TODO: could avoid this if already in PS mode */
-		if (ath9k_send_nullfunc(aphy, vif, avp->bssid, 1)) {
-			printk(KERN_DEBUG "%s: failed to send PS nullfunc\n",
-			       __func__);
-			ath9k_mark_paused(aphy);
-		}
-		break;
-	case NL80211_IFTYPE_AP:
-		/* Beacon transmission is paused by aphy->state change */
-		ath9k_mark_paused(aphy);
-		break;
-	default:
-		break;
-	}
-}
-
-/* caller must hold wiphy_lock */
-static int __ath9k_wiphy_pause(struct ath_wiphy *aphy)
-{
-	ieee80211_stop_queues(aphy->hw);
-	aphy->state = ATH_WIPHY_PAUSING;
-	/*
-	 * TODO: handle PAUSING->PAUSED for the case where there are multiple
-	 * active vifs (now we do it on the first vif getting ready; should be
-	 * on the last)
-	 */
-	ieee80211_iterate_active_interfaces_atomic(aphy->hw, ath9k_pause_iter,
-						   aphy);
-	return 0;
-}
-
-int ath9k_wiphy_pause(struct ath_wiphy *aphy)
-{
-	int ret;
-	spin_lock_bh(&aphy->sc->wiphy_lock);
-	ret = __ath9k_wiphy_pause(aphy);
-	spin_unlock_bh(&aphy->sc->wiphy_lock);
-	return ret;
-}
-
-static void ath9k_unpause_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
-{
-	struct ath_wiphy *aphy = data;
-	struct ath_vif *avp = (void *) vif->drv_priv;
-
-	switch (vif->type) {
-	case NL80211_IFTYPE_STATION:
-		if (!vif->bss_conf.assoc)
-			break;
-		ath9k_send_nullfunc(aphy, vif, avp->bssid, 0);
-		break;
-	case NL80211_IFTYPE_AP:
-		/* Beacon transmission is re-enabled by aphy->state change */
-		break;
-	default:
-		break;
-	}
-}
-
-/* caller must hold wiphy_lock */
-static int __ath9k_wiphy_unpause(struct ath_wiphy *aphy)
-{
-	ieee80211_iterate_active_interfaces_atomic(aphy->hw,
-						   ath9k_unpause_iter, aphy);
-	aphy->state = ATH_WIPHY_ACTIVE;
-	ieee80211_wake_queues(aphy->hw);
-	return 0;
-}
-
-int ath9k_wiphy_unpause(struct ath_wiphy *aphy)
-{
-	int ret;
-	spin_lock_bh(&aphy->sc->wiphy_lock);
-	ret = __ath9k_wiphy_unpause(aphy);
-	spin_unlock_bh(&aphy->sc->wiphy_lock);
-	return ret;
-}
-
-static void __ath9k_wiphy_mark_all_paused(struct ath_softc *sc)
-{
-	int i;
-	if (sc->pri_wiphy->state != ATH_WIPHY_INACTIVE)
-		sc->pri_wiphy->state = ATH_WIPHY_PAUSED;
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] &&
-		    sc->sec_wiphy[i]->state != ATH_WIPHY_INACTIVE)
-			sc->sec_wiphy[i]->state = ATH_WIPHY_PAUSED;
-	}
-}
-
-/* caller must hold wiphy_lock */
-static void __ath9k_wiphy_pause_all(struct ath_softc *sc)
-{
-	int i;
-	if (sc->pri_wiphy->state == ATH_WIPHY_ACTIVE)
-		__ath9k_wiphy_pause(sc->pri_wiphy);
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] &&
-		    sc->sec_wiphy[i]->state == ATH_WIPHY_ACTIVE)
-			__ath9k_wiphy_pause(sc->sec_wiphy[i]);
-	}
-}
-
-int ath9k_wiphy_select(struct ath_wiphy *aphy)
-{
-	struct ath_softc *sc = aphy->sc;
-	bool now;
-
-	spin_lock_bh(&sc->wiphy_lock);
-	if (__ath9k_wiphy_scanning(sc)) {
-		/*
-		 * For now, we are using mac80211 sw scan and it expects to
-		 * have full control over channel changes, so avoid wiphy
-		 * scheduling during a scan. This could be optimized if the
-		 * scanning control were moved into the driver.
-		 */
-		spin_unlock_bh(&sc->wiphy_lock);
-		return -EBUSY;
-	}
-	if (__ath9k_wiphy_pausing(sc)) {
-		if (sc->wiphy_select_failures == 0)
-			sc->wiphy_select_first_fail = jiffies;
-		sc->wiphy_select_failures++;
-		if (time_after(jiffies, sc->wiphy_select_first_fail + HZ / 2))
-		{
-			printk(KERN_DEBUG "ath9k: Previous wiphy select timed "
-			       "out; disable/enable hw to recover\n");
-			__ath9k_wiphy_mark_all_paused(sc);
-			/*
-			 * TODO: this workaround to fix hardware is unlikely to
-			 * be specific to virtual wiphy changes. It can happen
-			 * on normal channel change, too, and as such, this
-			 * should really be made more generic. For example,
-			 * tricker radio disable/enable on GTT interrupt burst
-			 * (say, 10 GTT interrupts received without any TX
-			 * frame being completed)
-			 */
-			spin_unlock_bh(&sc->wiphy_lock);
-			ath_radio_disable(sc, aphy->hw);
-			ath_radio_enable(sc, aphy->hw);
-			/* Only the primary wiphy hw is used for queuing work */
-			ieee80211_queue_work(aphy->sc->hw,
-				   &aphy->sc->chan_work);
-			return -EBUSY; /* previous select still in progress */
-		}
-		spin_unlock_bh(&sc->wiphy_lock);
-		return -EBUSY; /* previous select still in progress */
-	}
-	sc->wiphy_select_failures = 0;
-
-	/* Store the new channel */
-	sc->chan_idx = aphy->chan_idx;
-	sc->chan_is_ht = aphy->chan_is_ht;
-	sc->next_wiphy = aphy;
-
-	__ath9k_wiphy_pause_all(sc);
-	now = !__ath9k_wiphy_pausing(aphy->sc);
-	spin_unlock_bh(&sc->wiphy_lock);
-
-	if (now) {
-		/* Ready to request channel change immediately */
-		ieee80211_queue_work(aphy->sc->hw, &aphy->sc->chan_work);
-	}
-
-	/*
-	 * wiphys will be unpaused in ath9k_tx_status() once channel has been
-	 * changed if any wiphy needs time to become paused.
-	 */
-
-	return 0;
-}
-
-bool ath9k_wiphy_started(struct ath_softc *sc)
-{
-	int i;
-	spin_lock_bh(&sc->wiphy_lock);
-	if (sc->pri_wiphy->state != ATH_WIPHY_INACTIVE) {
-		spin_unlock_bh(&sc->wiphy_lock);
-		return true;
-	}
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] &&
-		    sc->sec_wiphy[i]->state != ATH_WIPHY_INACTIVE) {
-			spin_unlock_bh(&sc->wiphy_lock);
-			return true;
-		}
-	}
-	spin_unlock_bh(&sc->wiphy_lock);
-	return false;
-}
-
-static void ath9k_wiphy_pause_chan(struct ath_wiphy *aphy,
-				   struct ath_wiphy *selected)
-{
-	if (selected->state == ATH_WIPHY_SCAN) {
-		if (aphy == selected)
-			return;
-		/*
-		 * Pause all other wiphys for the duration of the scan even if
-		 * they are on the current channel now.
-		 */
-	} else if (aphy->chan_idx == selected->chan_idx)
-		return;
-	aphy->state = ATH_WIPHY_PAUSED;
-	ieee80211_stop_queues(aphy->hw);
-}
-
-void ath9k_wiphy_pause_all_forced(struct ath_softc *sc,
-				  struct ath_wiphy *selected)
-{
-	int i;
-	spin_lock_bh(&sc->wiphy_lock);
-	if (sc->pri_wiphy->state == ATH_WIPHY_ACTIVE)
-		ath9k_wiphy_pause_chan(sc->pri_wiphy, selected);
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		if (sc->sec_wiphy[i] &&
-		    sc->sec_wiphy[i]->state == ATH_WIPHY_ACTIVE)
-			ath9k_wiphy_pause_chan(sc->sec_wiphy[i], selected);
-	}
-	spin_unlock_bh(&sc->wiphy_lock);
-}
-
-void ath9k_wiphy_work(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc,
-					    wiphy_work.work);
-	struct ath_wiphy *aphy = NULL;
-	bool first = true;
-
-	spin_lock_bh(&sc->wiphy_lock);
-
-	if (sc->wiphy_scheduler_int == 0) {
-		/* wiphy scheduler is disabled */
-		spin_unlock_bh(&sc->wiphy_lock);
-		return;
-	}
-
-try_again:
-	sc->wiphy_scheduler_index++;
-	while (sc->wiphy_scheduler_index <= sc->num_sec_wiphy) {
-		aphy = sc->sec_wiphy[sc->wiphy_scheduler_index - 1];
-		if (aphy && aphy->state != ATH_WIPHY_INACTIVE)
-			break;
-
-		sc->wiphy_scheduler_index++;
-		aphy = NULL;
-	}
-	if (aphy == NULL) {
-		sc->wiphy_scheduler_index = 0;
-		if (sc->pri_wiphy->state == ATH_WIPHY_INACTIVE) {
-			if (first) {
-				first = false;
-				goto try_again;
-			}
-			/* No wiphy is ready to be scheduled */
-		} else
-			aphy = sc->pri_wiphy;
-	}
-
-	spin_unlock_bh(&sc->wiphy_lock);
-
-	if (aphy &&
-	    aphy->state != ATH_WIPHY_ACTIVE && aphy->state != ATH_WIPHY_SCAN &&
-	    ath9k_wiphy_select(aphy)) {
-		printk(KERN_DEBUG "ath9k: Failed to schedule virtual wiphy "
-		       "change\n");
-	}
-
-	ieee80211_queue_delayed_work(sc->hw,
-				     &sc->wiphy_work,
-				     sc->wiphy_scheduler_int);
-}
-
-void ath9k_wiphy_set_scheduler(struct ath_softc *sc, unsigned int msec_int)
-{
-	cancel_delayed_work_sync(&sc->wiphy_work);
-	sc->wiphy_scheduler_int = msecs_to_jiffies(msec_int);
-	if (sc->wiphy_scheduler_int)
-		ieee80211_queue_delayed_work(sc->hw, &sc->wiphy_work,
-					     sc->wiphy_scheduler_int);
-}
-
-/* caller must hold wiphy_lock */
-bool ath9k_all_wiphys_idle(struct ath_softc *sc)
-{
-	unsigned int i;
-	if (!sc->pri_wiphy->idle)
-		return false;
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy = sc->sec_wiphy[i];
-		if (!aphy)
-			continue;
-		if (!aphy->idle)
-			return false;
-	}
-	return true;
-}
-
-/* caller must hold wiphy_lock */
-void ath9k_set_wiphy_idle(struct ath_wiphy *aphy, bool idle)
-{
-	struct ath_softc *sc = aphy->sc;
-
-	aphy->idle = idle;
-	ath_dbg(ath9k_hw_common(sc->sc_ah), ATH_DBG_CONFIG,
-		"Marking %s as %sidle\n",
-		wiphy_name(aphy->hw->wiphy), idle ? "" : "not-");
-}
-/* Only bother starting a queue on an active virtual wiphy */
-bool ath_mac80211_start_queue(struct ath_softc *sc, u16 skb_queue)
-{
-	struct ieee80211_hw *hw = sc->pri_wiphy->hw;
-	unsigned int i;
-	bool txq_started = false;
-
-	spin_lock_bh(&sc->wiphy_lock);
-
-	/* Start the primary wiphy */
-	if (sc->pri_wiphy->state == ATH_WIPHY_ACTIVE) {
-		ieee80211_wake_queue(hw, skb_queue);
-		txq_started = true;
-		goto unlock;
-	}
-
-	/* Now start the secondary wiphy queues */
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy = sc->sec_wiphy[i];
-		if (!aphy)
-			continue;
-		if (aphy->state != ATH_WIPHY_ACTIVE)
-			continue;
-
-		hw = aphy->hw;
-		ieee80211_wake_queue(hw, skb_queue);
-		txq_started = true;
-		break;
-	}
-
-unlock:
-	spin_unlock_bh(&sc->wiphy_lock);
-	return txq_started;
-}
-
-/* Go ahead and propagate information to all virtual wiphys, it won't hurt */
-void ath_mac80211_stop_queue(struct ath_softc *sc, u16 skb_queue)
-{
-	struct ieee80211_hw *hw = sc->pri_wiphy->hw;
-	unsigned int i;
-
-	spin_lock_bh(&sc->wiphy_lock);
-
-	/* Stop the primary wiphy */
-	ieee80211_stop_queue(hw, skb_queue);
-
-	/* Now stop the secondary wiphy queues */
-	for (i = 0; i < sc->num_sec_wiphy; i++) {
-		struct ath_wiphy *aphy = sc->sec_wiphy[i];
-		if (!aphy)
-			continue;
-		hw = aphy->hw;
-		ieee80211_stop_queue(hw, skb_queue);
-	}
-	spin_unlock_bh(&sc->wiphy_lock);
-}
diff --git a/drivers/net/wireless/ath/ath9k/wmi.c b/drivers/net/wireless/ath/ath9k/wmi.c
index dc862f5e1162..d3d24904f62f 100644
--- a/drivers/net/wireless/ath/ath9k/wmi.c
+++ b/drivers/net/wireless/ath/ath9k/wmi.c
@@ -123,12 +123,8 @@ void ath9k_deinit_wmi(struct ath9k_htc_priv *priv)
 void ath9k_swba_tasklet(unsigned long data)
 {
 	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *)data;
-	struct ath_common *common = ath9k_hw_common(priv->ah);
-
-	ath_dbg(common, ATH_DBG_WMI, "SWBA Event received\n");
 
 	ath9k_htc_swba(priv, priv->wmi->beacon_pending);
-
 }
 
 void ath9k_fatal_work(struct work_struct *work)
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 07b7804aec5b..e16136d61799 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -19,7 +19,6 @@
 
 #define BITS_PER_BYTE           8
 #define OFDM_PLCP_BITS          22
-#define HT_RC_2_MCS(_rc)        ((_rc) & 0x1f)
 #define HT_RC_2_STREAMS(_rc)    ((((_rc) & 0x78) >> 3) + 1)
 #define L_STF                   8
 #define L_LTF                   8
@@ -32,7 +31,6 @@
 #define NUM_SYMBOLS_PER_USEC(_usec) (_usec >> 2)
 #define NUM_SYMBOLS_PER_USEC_HALFGI(_usec) (((_usec*5)-4)/18)
 
-#define OFDM_SIFS_TIME    	    16
 
 static u16 bits_per_symbol[][2] = {
 	/* 20MHz 40MHz */
@@ -57,8 +55,9 @@ static void ath_tx_complete_buf(struct ath_softc *sc, struct ath_buf *bf,
 static void ath_tx_txqaddbuf(struct ath_softc *sc, struct ath_txq *txq,
 			     struct list_head *head);
 static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf, int len);
-static void ath_tx_rc_status(struct ath_buf *bf, struct ath_tx_status *ts,
-			     int nframes, int nbad, int txok, bool update_rc);
+static void ath_tx_rc_status(struct ath_softc *sc, struct ath_buf *bf,
+			     struct ath_tx_status *ts, int nframes, int nbad,
+			     int txok, bool update_rc);
 static void ath_tx_update_baw(struct ath_softc *sc, struct ath_atx_tid *tid,
 			      int seqno);
 
@@ -169,7 +168,7 @@ static void ath_tx_flush_tid(struct ath_softc *sc, struct ath_atx_tid *tid)
 			ath_tx_update_baw(sc, tid, fi->seqno);
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, &ts, 0, 0);
 		} else {
-			ath_tx_send_normal(sc, txq, tid, &bf_head);
+			ath_tx_send_normal(sc, txq, NULL, &bf_head);
 		}
 		spin_lock_bh(&txq->axq_lock);
 	}
@@ -297,7 +296,6 @@ static struct ath_buf* ath_clone_txbuf(struct ath_softc *sc, struct ath_buf *bf)
 
 	ATH_TXBUF_RESET(tbf);
 
-	tbf->aphy = bf->aphy;
 	tbf->bf_mpdu = bf->bf_mpdu;
 	tbf->bf_buf_addr = bf->bf_buf_addr;
 	memcpy(tbf->bf_desc, bf->bf_desc, sc->sc_ah->caps.tx_desc_len);
@@ -345,7 +343,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 	struct ath_node *an = NULL;
 	struct sk_buff *skb;
 	struct ieee80211_sta *sta;
-	struct ieee80211_hw *hw;
+	struct ieee80211_hw *hw = sc->hw;
 	struct ieee80211_hdr *hdr;
 	struct ieee80211_tx_info *tx_info;
 	struct ath_atx_tid *tid = NULL;
@@ -364,7 +362,6 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 	hdr = (struct ieee80211_hdr *)skb->data;
 
 	tx_info = IEEE80211_SKB_CB(skb);
-	hw = bf->aphy->hw;
 
 	memcpy(rates, tx_info->control.rates, sizeof(rates));
 
@@ -383,7 +380,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 			    !bf->bf_stale || bf_next != NULL)
 				list_move_tail(&bf->list, &bf_head);
 
-			ath_tx_rc_status(bf, ts, 1, 1, 0, false);
+			ath_tx_rc_status(sc, bf, ts, 1, 1, 0, false);
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, ts,
 				0, 0);
 
@@ -429,7 +426,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 
 	ath_tx_count_frames(sc, bf, ts, txok, &nframes, &nbad);
 	while (bf) {
-		txfail = txpending = 0;
+		txfail = txpending = sendbar = 0;
 		bf_next = bf->bf_next;
 
 		skb = bf->bf_mpdu;
@@ -489,10 +486,10 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 
 			if (rc_update && (acked_cnt == 1 || txfail_cnt == 1)) {
 				memcpy(tx_info->control.rates, rates, sizeof(rates));
-				ath_tx_rc_status(bf, ts, nframes, nbad, txok, true);
+				ath_tx_rc_status(sc, bf, ts, nframes, nbad, txok, true);
 				rc_update = false;
 			} else {
-				ath_tx_rc_status(bf, ts, nframes, nbad, txok, false);
+				ath_tx_rc_status(sc, bf, ts, nframes, nbad, txok, false);
 			}
 
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, ts,
@@ -516,7 +513,7 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 
 						bf->bf_state.bf_type |=
 							BUF_XRETRY;
-						ath_tx_rc_status(bf, ts, nframes,
+						ath_tx_rc_status(sc, bf, ts, nframes,
 								nbad, 0, false);
 						ath_tx_complete_buf(sc, bf, txq,
 								    &bf_head,
@@ -566,8 +563,11 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 
 	rcu_read_unlock();
 
-	if (needreset)
+	if (needreset) {
+		spin_unlock_bh(&sc->sc_pcu_lock);
 		ath_reset(sc, false);
+		spin_lock_bh(&sc->sc_pcu_lock);
+	}
 }
 
 static u32 ath_lookup_rate(struct ath_softc *sc, struct ath_buf *bf,
@@ -856,7 +856,10 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 
 	txtid->state |= AGGR_ADDBA_PROGRESS;
 	txtid->paused = true;
-	*ssn = txtid->seq_start;
+	*ssn = txtid->seq_start = txtid->seq_next;
+
+	memset(txtid->tx_buf, 0, sizeof(txtid->tx_buf));
+	txtid->baw_head = txtid->baw_tail = 0;
 
 	return 0;
 }
@@ -942,7 +945,7 @@ struct ath_txq *ath_txq_setup(struct ath_softc *sc, int qtype, int subtype)
 		[WME_AC_VI] = ATH_TXQ_AC_VI,
 		[WME_AC_VO] = ATH_TXQ_AC_VO,
 	};
-	int qnum, i;
+	int axq_qnum, i;
 
 	memset(&qi, 0, sizeof(qi));
 	qi.tqi_subtype = subtype_txq_to_hwq[subtype];
@@ -976,24 +979,25 @@ struct ath_txq *ath_txq_setup(struct ath_softc *sc, int qtype, int subtype)
 			qi.tqi_qflags = TXQ_FLAG_TXEOLINT_ENABLE |
 					TXQ_FLAG_TXDESCINT_ENABLE;
 	}
-	qnum = ath9k_hw_setuptxqueue(ah, qtype, &qi);
-	if (qnum == -1) {
+	axq_qnum = ath9k_hw_setuptxqueue(ah, qtype, &qi);
+	if (axq_qnum == -1) {
 		/*
 		 * NB: don't print a message, this happens
 		 * normally on parts with too few tx queues
 		 */
 		return NULL;
 	}
-	if (qnum >= ARRAY_SIZE(sc->tx.txq)) {
+	if (axq_qnum >= ARRAY_SIZE(sc->tx.txq)) {
 		ath_err(common, "qnum %u out of range, max %zu!\n",
-			qnum, ARRAY_SIZE(sc->tx.txq));
-		ath9k_hw_releasetxqueue(ah, qnum);
+			axq_qnum, ARRAY_SIZE(sc->tx.txq));
+		ath9k_hw_releasetxqueue(ah, axq_qnum);
 		return NULL;
 	}
-	if (!ATH_TXQ_SETUP(sc, qnum)) {
-		struct ath_txq *txq = &sc->tx.txq[qnum];
+	if (!ATH_TXQ_SETUP(sc, axq_qnum)) {
+		struct ath_txq *txq = &sc->tx.txq[axq_qnum];
 
-		txq->axq_qnum = qnum;
+		txq->axq_qnum = axq_qnum;
+		txq->mac80211_qnum = -1;
 		txq->axq_link = NULL;
 		INIT_LIST_HEAD(&txq->axq_q);
 		INIT_LIST_HEAD(&txq->axq_acq);
@@ -1001,14 +1005,14 @@ struct ath_txq *ath_txq_setup(struct ath_softc *sc, int qtype, int subtype)
 		txq->axq_depth = 0;
 		txq->axq_ampdu_depth = 0;
 		txq->axq_tx_inprogress = false;
-		sc->tx.txqsetup |= 1<<qnum;
+		sc->tx.txqsetup |= 1<<axq_qnum;
 
 		txq->txq_headidx = txq->txq_tailidx = 0;
 		for (i = 0; i < ATH_TXFIFO_DEPTH; i++)
 			INIT_LIST_HEAD(&txq->txq_fifo[i]);
 		INIT_LIST_HEAD(&txq->txq_fifo_pending);
 	}
-	return &sc->tx.txq[qnum];
+	return &sc->tx.txq[axq_qnum];
 }
 
 int ath_txq_update(struct ath_softc *sc, int qnum,
@@ -1051,6 +1055,7 @@ int ath_txq_update(struct ath_softc *sc, int qnum,
 int ath_cabq_update(struct ath_softc *sc)
 {
 	struct ath9k_tx_queue_info qi;
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 	int qnum = sc->beacon.cabq->axq_qnum;
 
 	ath9k_hw_get_txq_props(sc->sc_ah, qnum, &qi);
@@ -1062,7 +1067,7 @@ int ath_cabq_update(struct ath_softc *sc)
 	else if (sc->config.cabqReadytime > ATH9K_READY_TIME_HI_BOUND)
 		sc->config.cabqReadytime = ATH9K_READY_TIME_HI_BOUND;
 
-	qi.tqi_readyTime = (sc->beacon_interval *
+	qi.tqi_readyTime = (cur_conf->beacon_interval *
 			    sc->config.cabqReadytime) / 100;
 	ath_txq_update(sc, qnum, &qi);
 
@@ -1205,8 +1210,17 @@ bool ath_drain_all_txq(struct ath_softc *sc, bool retry_tx)
 		ath_err(common, "Failed to stop TX DMA!\n");
 
 	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
-		if (ATH_TXQ_SETUP(sc, i))
-			ath_draintxq(sc, &sc->tx.txq[i], retry_tx);
+		if (!ATH_TXQ_SETUP(sc, i))
+			continue;
+
+		/*
+		 * The caller will resume queues with ieee80211_wake_queues.
+		 * Mark the queue as not stopped to prevent ath_tx_complete
+		 * from waking the queue too early.
+		 */
+		txq = &sc->tx.txq[i];
+		txq->stopped = false;
+		ath_draintxq(sc, txq, retry_tx);
 	}
 
 	return !npend;
@@ -1218,46 +1232,59 @@ void ath_tx_cleanupq(struct ath_softc *sc, struct ath_txq *txq)
 	sc->tx.txqsetup &= ~(1<<txq->axq_qnum);
 }
 
+/* For each axq_acq entry, for each tid, try to schedule packets
+ * for transmit until ampdu_depth has reached min Q depth.
+ */
 void ath_txq_schedule(struct ath_softc *sc, struct ath_txq *txq)
 {
-	struct ath_atx_ac *ac;
-	struct ath_atx_tid *tid;
+	struct ath_atx_ac *ac, *ac_tmp, *last_ac;
+	struct ath_atx_tid *tid, *last_tid;
 
-	if (list_empty(&txq->axq_acq))
+	if (list_empty(&txq->axq_acq) ||
+	    txq->axq_ampdu_depth >= ATH_AGGR_MIN_QDEPTH)
 		return;
 
 	ac = list_first_entry(&txq->axq_acq, struct ath_atx_ac, list);
-	list_del(&ac->list);
-	ac->sched = false;
+	last_ac = list_entry(txq->axq_acq.prev, struct ath_atx_ac, list);
 
-	do {
-		if (list_empty(&ac->tid_q))
-			return;
+	list_for_each_entry_safe(ac, ac_tmp, &txq->axq_acq, list) {
+		last_tid = list_entry(ac->tid_q.prev, struct ath_atx_tid, list);
+		list_del(&ac->list);
+		ac->sched = false;
 
-		tid = list_first_entry(&ac->tid_q, struct ath_atx_tid, list);
-		list_del(&tid->list);
-		tid->sched = false;
+		while (!list_empty(&ac->tid_q)) {
+			tid = list_first_entry(&ac->tid_q, struct ath_atx_tid,
+					       list);
+			list_del(&tid->list);
+			tid->sched = false;
 
-		if (tid->paused)
-			continue;
+			if (tid->paused)
+				continue;
 
-		ath_tx_sched_aggr(sc, txq, tid);
+			ath_tx_sched_aggr(sc, txq, tid);
 
-		/*
-		 * add tid to round-robin queue if more frames
-		 * are pending for the tid
-		 */
-		if (!list_empty(&tid->buf_q))
-			ath_tx_queue_tid(txq, tid);
+			/*
+			 * add tid to round-robin queue if more frames
+			 * are pending for the tid
+			 */
+			if (!list_empty(&tid->buf_q))
+				ath_tx_queue_tid(txq, tid);
 
-		break;
-	} while (!list_empty(&ac->tid_q));
+			if (tid == last_tid ||
+			    txq->axq_ampdu_depth >= ATH_AGGR_MIN_QDEPTH)
+				break;
+		}
 
-	if (!list_empty(&ac->tid_q)) {
-		if (!ac->sched) {
-			ac->sched = true;
-			list_add_tail(&ac->list, &txq->axq_acq);
+		if (!list_empty(&ac->tid_q)) {
+			if (!ac->sched) {
+				ac->sched = true;
+				list_add_tail(&ac->list, &txq->axq_acq);
+			}
 		}
+
+		if (ac == last_ac ||
+		    txq->axq_ampdu_depth >= ATH_AGGR_MIN_QDEPTH)
+			return;
 	}
 }
 
@@ -1301,6 +1328,7 @@ static void ath_tx_txqaddbuf(struct ath_softc *sc, struct ath_txq *txq,
 		INIT_LIST_HEAD(&txq->txq_fifo[txq->txq_headidx]);
 		list_splice_init(head, &txq->txq_fifo[txq->txq_headidx]);
 		INCR(txq->txq_headidx, ATH_TXFIFO_DEPTH);
+		TX_STAT_INC(txq->axq_qnum, puttxbuf);
 		ath9k_hw_puttxbuf(ah, txq->axq_qnum, bf->bf_daddr);
 		ath_dbg(common, ATH_DBG_XMIT, "TXDP[%u] = %llx (%p)\n",
 			txq->axq_qnum, ito64(bf->bf_daddr), bf->bf_desc);
@@ -1308,6 +1336,7 @@ static void ath_tx_txqaddbuf(struct ath_softc *sc, struct ath_txq *txq,
 		list_splice_tail_init(head, &txq->axq_q);
 
 		if (txq->axq_link == NULL) {
+			TX_STAT_INC(txq->axq_qnum, puttxbuf);
 			ath9k_hw_puttxbuf(ah, txq->axq_qnum, bf->bf_daddr);
 			ath_dbg(common, ATH_DBG_XMIT, "TXDP[%u] = %llx (%p)\n",
 				txq->axq_qnum, ito64(bf->bf_daddr),
@@ -1321,6 +1350,7 @@ static void ath_tx_txqaddbuf(struct ath_softc *sc, struct ath_txq *txq,
 		}
 		ath9k_hw_get_desc_link(ah, bf->bf_lastbf->bf_desc,
 				       &txq->axq_link);
+		TX_STAT_INC(txq->axq_qnum, txstart);
 		ath9k_hw_txstart(ah, txq->axq_qnum);
 	}
 	txq->axq_depth++;
@@ -1335,7 +1365,6 @@ static void ath_tx_send_ampdu(struct ath_softc *sc, struct ath_atx_tid *tid,
 	struct list_head bf_head;
 
 	bf->bf_state.bf_type |= BUF_AMPDU;
-	TX_STAT_INC(txctl->txq->axq_qnum, a_queued);
 
 	/*
 	 * Do not queue to h/w when any of the following conditions is true:
@@ -1351,6 +1380,7 @@ static void ath_tx_send_ampdu(struct ath_softc *sc, struct ath_atx_tid *tid,
 		 * Add this frame to software queue for scheduling later
 		 * for aggregation.
 		 */
+		TX_STAT_INC(txctl->txq->axq_qnum, a_queued_sw);
 		list_add_tail(&bf->list, &tid->buf_q);
 		ath_tx_queue_tid(txctl->txq, tid);
 		return;
@@ -1364,6 +1394,7 @@ static void ath_tx_send_ampdu(struct ath_softc *sc, struct ath_atx_tid *tid,
 		ath_tx_addto_baw(sc, tid, fi->seqno);
 
 	/* Queue to h/w without aggregation */
+	TX_STAT_INC(txctl->txq->axq_qnum, a_queued_hw);
 	bf->bf_lastbf = bf;
 	ath_buf_set_rate(sc, bf, fi->framelen);
 	ath_tx_txqaddbuf(sc, txctl->txq, &bf_head);
@@ -1416,8 +1447,7 @@ static enum ath9k_pkt_type get_hw_packet_type(struct sk_buff *skb)
 static void setup_frame_info(struct ieee80211_hw *hw, struct sk_buff *skb,
 			     int framelen)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_sta *sta = tx_info->control.sta;
 	struct ieee80211_key_conf *hw_key = tx_info->control.hw_key;
@@ -1635,8 +1665,7 @@ static struct ath_buf *ath_tx_setup_buffer(struct ieee80211_hw *hw,
 					   struct ath_txq *txq,
 					   struct sk_buff *skb)
 {
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_frame_info *fi = get_frame_info(skb);
@@ -1652,7 +1681,6 @@ static struct ath_buf *ath_tx_setup_buffer(struct ieee80211_hw *hw,
 
 	ATH_TXBUF_RESET(bf);
 
-	bf->aphy = aphy;
 	bf->bf_flags = setup_tx_flags(skb);
 	bf->bf_mpdu = skb;
 
@@ -1741,8 +1769,7 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_sta *sta = info->control.sta;
-	struct ath_wiphy *aphy = hw->priv;
-	struct ath_softc *sc = aphy->sc;
+	struct ath_softc *sc = hw->priv;
 	struct ath_txq *txq = txctl->txq;
 	struct ath_buf *bf;
 	int padpos, padsize;
@@ -1794,7 +1821,7 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	spin_lock_bh(&txq->axq_lock);
 	if (txq == sc->tx.txq_map[q] &&
 	    ++txq->pending_frames > ATH_MAX_QDEPTH && !txq->stopped) {
-		ath_mac80211_stop_queue(sc, q);
+		ieee80211_stop_queue(sc->hw, q);
 		txq->stopped = 1;
 	}
 	spin_unlock_bh(&txq->axq_lock);
@@ -1809,8 +1836,7 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 /*****************/
 
 static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
-			    struct ath_wiphy *aphy, int tx_flags, int ftype,
-			    struct ath_txq *txq)
+			    int tx_flags, int ftype, struct ath_txq *txq)
 {
 	struct ieee80211_hw *hw = sc->hw;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
@@ -1820,9 +1846,6 @@ static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 
 	ath_dbg(common, ATH_DBG_XMIT, "TX complete: skb: %p\n", skb);
 
-	if (aphy)
-		hw = aphy->hw;
-
 	if (tx_flags & ATH_TX_BAR)
 		tx_info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
 
@@ -1852,19 +1875,20 @@ static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 					PS_WAIT_FOR_TX_ACK));
 	}
 
-	if (unlikely(ftype))
-		ath9k_tx_status(hw, skb, ftype);
-	else {
-		q = skb_get_queue_mapping(skb);
-		if (txq == sc->tx.txq_map[q]) {
-			spin_lock_bh(&txq->axq_lock);
-			if (WARN_ON(--txq->pending_frames < 0))
-				txq->pending_frames = 0;
-			spin_unlock_bh(&txq->axq_lock);
-		}
+	q = skb_get_queue_mapping(skb);
+	if (txq == sc->tx.txq_map[q]) {
+		spin_lock_bh(&txq->axq_lock);
+		if (WARN_ON(--txq->pending_frames < 0))
+			txq->pending_frames = 0;
 
-		ieee80211_tx_status(hw, skb);
+		if (txq->stopped && txq->pending_frames < ATH_MAX_QDEPTH) {
+			ieee80211_wake_queue(sc->hw, q);
+			txq->stopped = 0;
+		}
+		spin_unlock_bh(&txq->axq_lock);
 	}
+
+	ieee80211_tx_status(hw, skb);
 }
 
 static void ath_tx_complete_buf(struct ath_softc *sc, struct ath_buf *bf,
@@ -1896,8 +1920,8 @@ static void ath_tx_complete_buf(struct ath_softc *sc, struct ath_buf *bf,
 		else
 			complete(&sc->paprd_complete);
 	} else {
-		ath_debug_stat_tx(sc, bf, ts);
-		ath_tx_complete(sc, skb, bf->aphy, tx_flags,
+		ath_debug_stat_tx(sc, bf, ts, txq);
+		ath_tx_complete(sc, skb, tx_flags,
 				bf->bf_state.bfs_ftype, txq);
 	}
 	/* At this point, skb (bf->bf_mpdu) is consumed...make sure we don't
@@ -1913,14 +1937,14 @@ static void ath_tx_complete_buf(struct ath_softc *sc, struct ath_buf *bf,
 	spin_unlock_irqrestore(&sc->tx.txbuflock, flags);
 }
 
-static void ath_tx_rc_status(struct ath_buf *bf, struct ath_tx_status *ts,
-			     int nframes, int nbad, int txok, bool update_rc)
+static void ath_tx_rc_status(struct ath_softc *sc, struct ath_buf *bf,
+			     struct ath_tx_status *ts, int nframes, int nbad,
+			     int txok, bool update_rc)
 {
 	struct sk_buff *skb = bf->bf_mpdu;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_hw *hw = bf->aphy->hw;
-	struct ath_softc *sc = bf->aphy->sc;
+	struct ieee80211_hw *hw = sc->hw;
 	struct ath_hw *ah = sc->sc_ah;
 	u8 i, tx_rateindex;
 
@@ -1971,19 +1995,6 @@ static void ath_tx_rc_status(struct ath_buf *bf, struct ath_tx_status *ts,
 	tx_info->status.rates[tx_rateindex].count = ts->ts_longretry + 1;
 }
 
-static void ath_wake_mac80211_queue(struct ath_softc *sc, int qnum)
-{
-	struct ath_txq *txq;
-
-	txq = sc->tx.txq_map[qnum];
-	spin_lock_bh(&txq->axq_lock);
-	if (txq->stopped && txq->pending_frames < ATH_MAX_QDEPTH) {
-		if (ath_mac80211_start_queue(sc, qnum))
-			txq->stopped = 0;
-	}
-	spin_unlock_bh(&txq->axq_lock);
-}
-
 static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 {
 	struct ath_hw *ah = sc->sc_ah;
@@ -1994,7 +2005,6 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 	struct ath_tx_status ts;
 	int txok;
 	int status;
-	int qnum;
 
 	ath_dbg(common, ATH_DBG_QUEUE, "tx queue %d (%x), link %p\n",
 		txq->axq_qnum, ath9k_hw_gettxbuf(sc->sc_ah, txq->axq_qnum),
@@ -2004,6 +2014,9 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 		spin_lock_bh(&txq->axq_lock);
 		if (list_empty(&txq->axq_q)) {
 			txq->axq_link = NULL;
+			if (sc->sc_flags & SC_OP_TXAGGR &&
+			    !txq->txq_flush_inprogress)
+				ath_txq_schedule(sc, txq);
 			spin_unlock_bh(&txq->axq_lock);
 			break;
 		}
@@ -2038,6 +2051,7 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 			spin_unlock_bh(&txq->axq_lock);
 			break;
 		}
+		TX_STAT_INC(txq->axq_qnum, txprocdesc);
 
 		/*
 		 * Remove ath_buf's of the same transmit unit from txq,
@@ -2058,6 +2072,7 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 
 		if (bf_is_ampdu_not_probing(bf))
 			txq->axq_ampdu_depth--;
+
 		spin_unlock_bh(&txq->axq_lock);
 
 		if (bf_held)
@@ -2070,27 +2085,45 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 			 */
 			if (ts.ts_status & ATH9K_TXERR_XRETRY)
 				bf->bf_state.bf_type |= BUF_XRETRY;
-			ath_tx_rc_status(bf, &ts, 1, txok ? 0 : 1, txok, true);
+			ath_tx_rc_status(sc, bf, &ts, 1, txok ? 0 : 1, txok, true);
 		}
 
-		qnum = skb_get_queue_mapping(bf->bf_mpdu);
-
 		if (bf_isampdu(bf))
 			ath_tx_complete_aggr(sc, txq, bf, &bf_head, &ts, txok,
 					     true);
 		else
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, &ts, txok, 0);
 
-		if (txq == sc->tx.txq_map[qnum])
-			ath_wake_mac80211_queue(sc, qnum);
-
 		spin_lock_bh(&txq->axq_lock);
-		if (sc->sc_flags & SC_OP_TXAGGR)
+
+		if (sc->sc_flags & SC_OP_TXAGGR && !txq->txq_flush_inprogress)
 			ath_txq_schedule(sc, txq);
 		spin_unlock_bh(&txq->axq_lock);
 	}
 }
 
+static void ath_hw_pll_work(struct work_struct *work)
+{
+	struct ath_softc *sc = container_of(work, struct ath_softc,
+					    hw_pll_work.work);
+	static int count;
+
+	if (AR_SREV_9485(sc->sc_ah)) {
+		if (ar9003_get_pll_sqsum_dvc(sc->sc_ah) >= 0x40000) {
+			count++;
+
+			if (count == 3) {
+				/* Rx is hung for more than 500ms. Reset it */
+				ath_reset(sc, true);
+				count = 0;
+			}
+		} else
+			count = 0;
+
+		ieee80211_queue_delayed_work(sc->hw, &sc->hw_pll_work, HZ/5);
+	}
+}
+
 static void ath_tx_complete_poll_work(struct work_struct *work)
 {
 	struct ath_softc *sc = container_of(work, struct ath_softc,
@@ -2098,6 +2131,9 @@ static void ath_tx_complete_poll_work(struct work_struct *work)
 	struct ath_txq *txq;
 	int i;
 	bool needreset = false;
+#ifdef CONFIG_ATH9K_DEBUGFS
+	sc->tx_complete_poll_work_seen++;
+#endif
 
 	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++)
 		if (ATH_TXQ_SETUP(sc, i)) {
@@ -2111,6 +2147,33 @@ static void ath_tx_complete_poll_work(struct work_struct *work)
 				} else {
 					txq->axq_tx_inprogress = true;
 				}
+			} else {
+				/* If the queue has pending buffers, then it
+				 * should be doing tx work (and have axq_depth).
+				 * Shouldn't get to this state I think..but
+				 * we do.
+				 */
+				if (!(sc->sc_flags & (SC_OP_OFFCHANNEL)) &&
+				    (txq->pending_frames > 0 ||
+				     !list_empty(&txq->axq_acq) ||
+				     txq->stopped)) {
+					ath_err(ath9k_hw_common(sc->sc_ah),
+						"txq: %p axq_qnum: %u,"
+						" mac80211_qnum: %i"
+						" axq_link: %p"
+						" pending frames: %i"
+						" axq_acq empty: %i"
+						" stopped: %i"
+						" axq_depth: 0  Attempting to"
+						" restart tx logic.\n",
+						txq, txq->axq_qnum,
+						txq->mac80211_qnum,
+						txq->axq_link,
+						txq->pending_frames,
+						list_empty(&txq->axq_acq),
+						txq->stopped);
+					ath_txq_schedule(sc, txq);
+				}
 			}
 			spin_unlock_bh(&txq->axq_lock);
 		}
@@ -2150,7 +2213,6 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 	struct list_head bf_head;
 	int status;
 	int txok;
-	int qnum;
 
 	for (;;) {
 		status = ath9k_hw_txprocdesc(ah, NULL, (void *)&txs);
@@ -2193,11 +2255,9 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 		if (!bf_isampdu(bf)) {
 			if (txs.ts_status & ATH9K_TXERR_XRETRY)
 				bf->bf_state.bf_type |= BUF_XRETRY;
-			ath_tx_rc_status(bf, &txs, 1, txok ? 0 : 1, txok, true);
+			ath_tx_rc_status(sc, bf, &txs, 1, txok ? 0 : 1, txok, true);
 		}
 
-		qnum = skb_get_queue_mapping(bf->bf_mpdu);
-
 		if (bf_isampdu(bf))
 			ath_tx_complete_aggr(sc, txq, bf, &bf_head, &txs,
 					     txok, true);
@@ -2205,19 +2265,20 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 			ath_tx_complete_buf(sc, bf, txq, &bf_head,
 					    &txs, txok, 0);
 
-		if (txq == sc->tx.txq_map[qnum])
-			ath_wake_mac80211_queue(sc, qnum);
-
 		spin_lock_bh(&txq->axq_lock);
-		if (!list_empty(&txq->txq_fifo_pending)) {
-			INIT_LIST_HEAD(&bf_head);
-			bf = list_first_entry(&txq->txq_fifo_pending,
-				struct ath_buf, list);
-			list_cut_position(&bf_head, &txq->txq_fifo_pending,
-				&bf->bf_lastbf->list);
-			ath_tx_txqaddbuf(sc, txq, &bf_head);
-		} else if (sc->sc_flags & SC_OP_TXAGGR)
-			ath_txq_schedule(sc, txq);
+
+		if (!txq->txq_flush_inprogress) {
+			if (!list_empty(&txq->txq_fifo_pending)) {
+				INIT_LIST_HEAD(&bf_head);
+				bf = list_first_entry(&txq->txq_fifo_pending,
+						      struct ath_buf, list);
+				list_cut_position(&bf_head,
+						  &txq->txq_fifo_pending,
+						  &bf->bf_lastbf->list);
+				ath_tx_txqaddbuf(sc, txq, &bf_head);
+			} else if (sc->sc_flags & SC_OP_TXAGGR)
+				ath_txq_schedule(sc, txq);
+		}
 		spin_unlock_bh(&txq->axq_lock);
 	}
 }
@@ -2285,6 +2346,7 @@ int ath_tx_init(struct ath_softc *sc, int nbufs)
 	}
 
 	INIT_DELAYED_WORK(&sc->tx_complete_work, ath_tx_complete_poll_work);
+	INIT_DELAYED_WORK(&sc->hw_pll_work, ath_hw_pll_work);
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
 		error = ath_tx_edma_init(sc);
diff --git a/drivers/net/wireless/ath/carl9170/carl9170.h b/drivers/net/wireless/ath/carl9170/carl9170.h
index d07ff7f2fd92..c6a5fae634a0 100644
--- a/drivers/net/wireless/ath/carl9170/carl9170.h
+++ b/drivers/net/wireless/ath/carl9170/carl9170.h
@@ -283,6 +283,7 @@ struct ar9170 {
 		unsigned int mem_blocks;
 		unsigned int mem_block_size;
 		unsigned int rx_size;
+		unsigned int tx_seq_table;
 	} fw;
 
 	/* reset / stuck frames/queue detection */
@@ -533,7 +534,7 @@ void carl9170_rx(struct ar9170 *ar, void *buf, unsigned int len);
 void carl9170_handle_command_response(struct ar9170 *ar, void *buf, u32 len);
 
 /* TX */
-int carl9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
+void carl9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 void carl9170_tx_janitor(struct work_struct *work);
 void carl9170_tx_process_status(struct ar9170 *ar,
 				const struct carl9170_rsp *cmd);
diff --git a/drivers/net/wireless/ath/carl9170/fw.c b/drivers/net/wireless/ath/carl9170/fw.c
index 546b4e4ec5ea..9517ede9e2df 100644
--- a/drivers/net/wireless/ath/carl9170/fw.c
+++ b/drivers/net/wireless/ath/carl9170/fw.c
@@ -150,6 +150,7 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 	const struct carl9170fw_otus_desc *otus_desc;
 	const struct carl9170fw_chk_desc *chk_desc;
 	const struct carl9170fw_last_desc *last_desc;
+	const struct carl9170fw_txsq_desc *txsq_desc;
 
 	last_desc = carl9170_fw_find_desc(ar, LAST_MAGIC,
 		sizeof(*last_desc), CARL9170FW_LAST_DESC_CUR_VER);
@@ -264,6 +265,9 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 			FIF_PROMISC_IN_BSS;
 	}
 
+	if (SUPP(CARL9170FW_WOL))
+		device_set_wakeup_enable(&ar->udev->dev, true);
+
 	ar->fw.vif_num = otus_desc->vif_num;
 	ar->fw.cmd_bufs = otus_desc->cmd_bufs;
 	ar->fw.address = le32_to_cpu(otus_desc->fw_address);
@@ -296,6 +300,17 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 		}
 	}
 
+	txsq_desc = carl9170_fw_find_desc(ar, TXSQ_MAGIC,
+		sizeof(*txsq_desc), CARL9170FW_TXSQ_DESC_CUR_VER);
+
+	if (txsq_desc) {
+		ar->fw.tx_seq_table = le32_to_cpu(txsq_desc->seq_table_addr);
+		if (!valid_cpu_addr(ar->fw.tx_seq_table))
+			return -EINVAL;
+	} else {
+		ar->fw.tx_seq_table = 0;
+	}
+
 #undef SUPPORTED
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/carl9170/fwcmd.h b/drivers/net/wireless/ath/carl9170/fwcmd.h
index 3680dfc70f46..30449d21b762 100644
--- a/drivers/net/wireless/ath/carl9170/fwcmd.h
+++ b/drivers/net/wireless/ath/carl9170/fwcmd.h
@@ -167,6 +167,7 @@ struct carl9170_rx_filter_cmd {
 #define CARL9170_RX_FILTER_CTL_BACKR	0x20
 #define CARL9170_RX_FILTER_MGMT		0x40
 #define CARL9170_RX_FILTER_DATA		0x80
+#define CARL9170_RX_FILTER_EVERYTHING	(~0)
 
 struct carl9170_bcn_ctrl_cmd {
 	__le32		vif_id;
diff --git a/drivers/net/wireless/ath/carl9170/fwdesc.h b/drivers/net/wireless/ath/carl9170/fwdesc.h
index 71f3821f6058..921066822dd5 100644
--- a/drivers/net/wireless/ath/carl9170/fwdesc.h
+++ b/drivers/net/wireless/ath/carl9170/fwdesc.h
@@ -69,6 +69,9 @@ enum carl9170fw_feature_list {
 	/* Firmware RX filter | CARL9170_CMD_RX_FILTER */
 	CARL9170FW_RX_FILTER,
 
+	/* Wake up on WLAN */
+	CARL9170FW_WOL,
+
 	/* KEEP LAST */
 	__CARL9170FW_FEATURE_NUM
 };
@@ -78,6 +81,7 @@ enum carl9170fw_feature_list {
 #define FIX_MAGIC	"FIX\0"
 #define DBG_MAGIC	"DBG\0"
 #define CHK_MAGIC	"CHK\0"
+#define TXSQ_MAGIC	"TXSQ"
 #define LAST_MAGIC	"LAST"
 
 #define CARL9170FW_SET_DAY(d) (((d) - 1) % 31)
@@ -88,8 +92,10 @@ enum carl9170fw_feature_list {
 #define CARL9170FW_GET_MONTH(m) ((((m) / 31) % 12) + 1)
 #define CARL9170FW_GET_YEAR(y) ((y) / 372 + 10)
 
+#define CARL9170FW_MAGIC_SIZE			4
+
 struct carl9170fw_desc_head {
-	u8	magic[4];
+	u8	magic[CARL9170FW_MAGIC_SIZE];
 	__le16 length;
 	u8 min_ver;
 	u8 cur_ver;
@@ -170,6 +176,16 @@ struct carl9170fw_chk_desc {
 #define CARL9170FW_CHK_DESC_SIZE			\
 	(sizeof(struct carl9170fw_chk_desc))
 
+#define CARL9170FW_TXSQ_DESC_MIN_VER			1
+#define CARL9170FW_TXSQ_DESC_CUR_VER			1
+struct carl9170fw_txsq_desc {
+	struct carl9170fw_desc_head head;
+
+	__le32 seq_table_addr;
+} __packed;
+#define CARL9170FW_TXSQ_DESC_SIZE			\
+	(sizeof(struct carl9170fw_txsq_desc))
+
 #define CARL9170FW_LAST_DESC_MIN_VER			1
 #define CARL9170FW_LAST_DESC_CUR_VER			2
 struct carl9170fw_last_desc {
@@ -189,8 +205,8 @@ struct carl9170fw_last_desc {
 	}
 
 static inline void carl9170fw_fill_desc(struct carl9170fw_desc_head *head,
-					 u8 magic[4], __le16 length,
-					 u8 min_ver, u8 cur_ver)
+					 u8 magic[CARL9170FW_MAGIC_SIZE],
+					 __le16 length, u8 min_ver, u8 cur_ver)
 {
 	head->magic[0] = magic[0];
 	head->magic[1] = magic[1];
@@ -204,7 +220,7 @@ static inline void carl9170fw_fill_desc(struct carl9170fw_desc_head *head,
 
 #define carl9170fw_for_each_hdr(desc, fw_desc)				\
 	for (desc = fw_desc;						\
-	     memcmp(desc->magic, LAST_MAGIC, 4) &&			\
+	     memcmp(desc->magic, LAST_MAGIC, CARL9170FW_MAGIC_SIZE) &&	\
 	     le16_to_cpu(desc->length) >= CARL9170FW_DESC_HEAD_SIZE &&	\
 	     le16_to_cpu(desc->length) < CARL9170FW_DESC_MAX_LENGTH;	\
 	     desc = (void *)((unsigned long)desc + le16_to_cpu(desc->length)))
@@ -218,8 +234,8 @@ static inline bool carl9170fw_supports(__le32 list, u8 feature)
 }
 
 static inline bool carl9170fw_desc_cmp(const struct carl9170fw_desc_head *head,
-				       const u8 descid[4], u16 min_len,
-				       u8 compatible_revision)
+				       const u8 descid[CARL9170FW_MAGIC_SIZE],
+				       u16 min_len, u8 compatible_revision)
 {
 	if (descid[0] == head->magic[0] && descid[1] == head->magic[1] &&
 	    descid[2] == head->magic[2] && descid[3] == head->magic[3] &&
diff --git a/drivers/net/wireless/ath/carl9170/hw.h b/drivers/net/wireless/ath/carl9170/hw.h
index e85df6edfed3..4e30762dd903 100644
--- a/drivers/net/wireless/ath/carl9170/hw.h
+++ b/drivers/net/wireless/ath/carl9170/hw.h
@@ -463,6 +463,8 @@
 
 #define	AR9170_PWR_REG_CHIP_REVISION		(AR9170_PWR_REG_BASE + 0x010)
 #define AR9170_PWR_REG_PLL_ADDAC		(AR9170_PWR_REG_BASE + 0x014)
+#define		AR9170_PWR_PLL_ADDAC_DIV_S		2
+#define		AR9170_PWR_PLL_ADDAC_DIV		0xffc
 #define	AR9170_PWR_REG_WATCH_DOG_MAGIC		(AR9170_PWR_REG_BASE + 0x020)
 
 /* Faraday USB Controller */
@@ -471,6 +473,9 @@
 #define	AR9170_USB_REG_MAIN_CTRL		(AR9170_USB_REG_BASE + 0x000)
 #define		AR9170_USB_MAIN_CTRL_REMOTE_WAKEUP	BIT(0)
 #define		AR9170_USB_MAIN_CTRL_ENABLE_GLOBAL_INT	BIT(2)
+#define		AR9170_USB_MAIN_CTRL_GO_TO_SUSPEND	BIT(3)
+#define		AR9170_USB_MAIN_CTRL_RESET		BIT(4)
+#define		AR9170_USB_MAIN_CTRL_CHIP_ENABLE	BIT(5)
 #define		AR9170_USB_MAIN_CTRL_HIGHSPEED		BIT(6)
 
 #define	AR9170_USB_REG_DEVICE_ADDRESS		(AR9170_USB_REG_BASE + 0x001)
@@ -499,6 +504,13 @@
 #define	AR9170_USB_REG_INTR_GROUP		(AR9170_USB_REG_BASE + 0x020)
 
 #define	AR9170_USB_REG_INTR_SOURCE_0		(AR9170_USB_REG_BASE + 0x021)
+#define		AR9170_USB_INTR_SRC0_SETUP		BIT(0)
+#define		AR9170_USB_INTR_SRC0_IN			BIT(1)
+#define		AR9170_USB_INTR_SRC0_OUT		BIT(2)
+#define		AR9170_USB_INTR_SRC0_FAIL		BIT(3) /* ??? */
+#define		AR9170_USB_INTR_SRC0_END		BIT(4) /* ??? */
+#define		AR9170_USB_INTR_SRC0_ABORT		BIT(7)
+
 #define	AR9170_USB_REG_INTR_SOURCE_1		(AR9170_USB_REG_BASE + 0x022)
 #define	AR9170_USB_REG_INTR_SOURCE_2		(AR9170_USB_REG_BASE + 0x023)
 #define	AR9170_USB_REG_INTR_SOURCE_3		(AR9170_USB_REG_BASE + 0x024)
@@ -506,6 +518,15 @@
 #define	AR9170_USB_REG_INTR_SOURCE_5		(AR9170_USB_REG_BASE + 0x026)
 #define	AR9170_USB_REG_INTR_SOURCE_6		(AR9170_USB_REG_BASE + 0x027)
 #define	AR9170_USB_REG_INTR_SOURCE_7		(AR9170_USB_REG_BASE + 0x028)
+#define		AR9170_USB_INTR_SRC7_USB_RESET		BIT(1)
+#define		AR9170_USB_INTR_SRC7_USB_SUSPEND	BIT(2)
+#define		AR9170_USB_INTR_SRC7_USB_RESUME		BIT(3)
+#define		AR9170_USB_INTR_SRC7_ISO_SEQ_ERR	BIT(4)
+#define		AR9170_USB_INTR_SRC7_ISO_SEQ_ABORT	BIT(5)
+#define		AR9170_USB_INTR_SRC7_TX0BYTE		BIT(6)
+#define		AR9170_USB_INTR_SRC7_RX0BYTE		BIT(7)
+
+#define	AR9170_USB_REG_IDLE_COUNT		(AR9170_USB_REG_BASE + 0x02f)
 
 #define	AR9170_USB_REG_EP_MAP			(AR9170_USB_REG_BASE + 0x030)
 #define	AR9170_USB_REG_EP1_MAP			(AR9170_USB_REG_BASE + 0x030)
@@ -581,6 +602,10 @@
 
 #define	AR9170_USB_REG_MAX_AGG_UPLOAD		(AR9170_USB_REG_BASE + 0x110)
 #define	AR9170_USB_REG_UPLOAD_TIME_CTL		(AR9170_USB_REG_BASE + 0x114)
+
+#define AR9170_USB_REG_WAKE_UP			(AR9170_USB_REG_BASE + 0x120)
+#define		AR9170_USB_WAKE_UP_WAKE			BIT(0)
+
 #define	AR9170_USB_REG_CBUS_CTRL		(AR9170_USB_REG_BASE + 0x1f0)
 #define		AR9170_USB_CBUS_CTRL_BUFFER_END		(BIT(1))
 
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index 870df8c42622..ede3d7e5a048 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -662,6 +662,13 @@ init:
 			goto unlock;
 	}
 
+	if (ar->fw.tx_seq_table) {
+		err = carl9170_write_reg(ar, ar->fw.tx_seq_table + vif_id * 4,
+					 0);
+		if (err)
+			goto unlock;
+	}
+
 unlock:
 	if (err && (vif_id >= 0)) {
 		vif_priv->active = false;
@@ -1279,7 +1286,7 @@ static int carl9170_op_ampdu_action(struct ieee80211_hw *hw,
 				    struct ieee80211_vif *vif,
 				    enum ieee80211_ampdu_mlme_action action,
 				    struct ieee80211_sta *sta,
-				    u16 tid, u16 *ssn)
+				    u16 tid, u16 *ssn, u8 buf_size)
 {
 	struct ar9170 *ar = hw->priv;
 	struct carl9170_sta_info *sta_info = (void *) sta->drv_priv;
diff --git a/drivers/net/wireless/ath/carl9170/tx.c b/drivers/net/wireless/ath/carl9170/tx.c
index 6cc58e052d10..0ef70b6fc512 100644
--- a/drivers/net/wireless/ath/carl9170/tx.c
+++ b/drivers/net/wireless/ath/carl9170/tx.c
@@ -862,6 +862,9 @@ static int carl9170_tx_prepare(struct ar9170 *ar, struct sk_buff *skb)
 	if (unlikely(info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM))
 		txc->s.misc |= CARL9170_TX_SUPER_MISC_CAB;
 
+	if (unlikely(info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ))
+		txc->s.misc |= CARL9170_TX_SUPER_MISC_ASSIGN_SEQ;
+
 	if (unlikely(ieee80211_is_probe_resp(hdr->frame_control)))
 		txc->s.misc |= CARL9170_TX_SUPER_MISC_FILL_IN_TSF;
 
@@ -1336,7 +1339,7 @@ err_unlock_rcu:
 	return false;
 }
 
-int carl9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+void carl9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ar9170 *ar = hw->priv;
 	struct ieee80211_tx_info *info;
@@ -1370,12 +1373,11 @@ int carl9170_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	}
 
 	carl9170_tx(ar);
-	return NETDEV_TX_OK;
+	return;
 
 err_free:
 	ar->tx_dropped++;
 	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
 }
 
 void carl9170_tx_scheduler(struct ar9170 *ar)
diff --git a/drivers/net/wireless/ath/carl9170/usb.c b/drivers/net/wireless/ath/carl9170/usb.c
index 537732e5964f..f82c400be288 100644
--- a/drivers/net/wireless/ath/carl9170/usb.c
+++ b/drivers/net/wireless/ath/carl9170/usb.c
@@ -118,6 +118,8 @@ static struct usb_device_id carl9170_usb_ids[] = {
 	{ USB_DEVICE(0x057c, 0x8402) },
 	/* Qwest/Actiontec 802AIN Wireless N USB Network Adapter */
 	{ USB_DEVICE(0x1668, 0x1200) },
+	/* Airlive X.USB a/b/g/n */
+	{ USB_DEVICE(0x1b75, 0x9170) },
 
 	/* terminate */
 	{}
diff --git a/drivers/net/wireless/ath/carl9170/version.h b/drivers/net/wireless/ath/carl9170/version.h
index ee0f84f2a2f6..15095c035169 100644
--- a/drivers/net/wireless/ath/carl9170/version.h
+++ b/drivers/net/wireless/ath/carl9170/version.h
@@ -1,7 +1,7 @@
 #ifndef __CARL9170_SHARED_VERSION_H
 #define __CARL9170_SHARED_VERSION_H
-#define CARL9170FW_VERSION_YEAR 10
-#define CARL9170FW_VERSION_MONTH 10
-#define CARL9170FW_VERSION_DAY 29
-#define CARL9170FW_VERSION_GIT "1.9.0"
+#define CARL9170FW_VERSION_YEAR 11
+#define CARL9170FW_VERSION_MONTH 1
+#define CARL9170FW_VERSION_DAY 22
+#define CARL9170FW_VERSION_GIT "1.9.2"
 #endif /* __CARL9170_SHARED_VERSION_H */
diff --git a/drivers/net/wireless/ath/carl9170/wlan.h b/drivers/net/wireless/ath/carl9170/wlan.h
index 24d63b583b6b..9e1324b67e08 100644
--- a/drivers/net/wireless/ath/carl9170/wlan.h
+++ b/drivers/net/wireless/ath/carl9170/wlan.h
@@ -251,7 +251,7 @@ struct carl9170_tx_superdesc {
 	u8 ampdu_commit_factor:1;
 	u8 ampdu_unused_bit:1;
 	u8 queue:2;
-	u8 reserved:1;
+	u8 assign_seq:1;
 	u8 vif_id:3;
 	u8 fill_in_tsf:1;
 	u8 cab:1;
@@ -299,6 +299,7 @@ struct _ar9170_tx_hwdesc {
 
 #define CARL9170_TX_SUPER_MISC_QUEUE			0x3
 #define CARL9170_TX_SUPER_MISC_QUEUE_S			0
+#define CARL9170_TX_SUPER_MISC_ASSIGN_SEQ		0x4
 #define	CARL9170_TX_SUPER_MISC_VIF_ID			0x38
 #define	CARL9170_TX_SUPER_MISC_VIF_ID_S			3
 #define	CARL9170_TX_SUPER_MISC_FILL_IN_TSF		0x40
@@ -413,6 +414,23 @@ enum ar9170_txq {
 	__AR9170_NUM_TXQ,
 };
 
+/*
+ * This is an workaround for several undocumented bugs.
+ * Don't mess with the QoS/AC <-> HW Queue map, if you don't
+ * know what you are doing.
+ *
+ * Known problems [hardware]:
+ *  * The MAC does not aggregate frames on anything other
+ *    than the first HW queue.
+ *  * when an AMPDU is placed [in the first hw queue] and
+ *    additional frames are already queued on a different
+ *    hw queue, the MAC will ALWAYS freeze.
+ *
+ * In a nutshell: The hardware can either do QoS or
+ * Aggregation but not both at the same time. As a
+ * result, this makes the device pretty much useless
+ * for any serious 802.11n setup.
+ */
 static const u8 ar9170_qmap[__AR9170_NUM_TXQ] = { 2, 1, 0, 3 };
 
 #define	AR9170_TXQ_DEPTH			32
diff --git a/drivers/net/wireless/ath/key.c b/drivers/net/wireless/ath/key.c
index 5d465e5fcf24..37b8e115375a 100644
--- a/drivers/net/wireless/ath/key.c
+++ b/drivers/net/wireless/ath/key.c
@@ -58,8 +58,11 @@ bool ath_hw_keyreset(struct ath_common *common, u16 entry)
 		REG_WRITE(ah, AR_KEYTABLE_KEY1(micentry), 0);
 		REG_WRITE(ah, AR_KEYTABLE_KEY2(micentry), 0);
 		REG_WRITE(ah, AR_KEYTABLE_KEY3(micentry), 0);
-		if (common->crypt_caps & ATH_CRYPT_CAP_MIC_COMBINED)
+		if (common->crypt_caps & ATH_CRYPT_CAP_MIC_COMBINED) {
 			REG_WRITE(ah, AR_KEYTABLE_KEY4(micentry), 0);
+			REG_WRITE(ah, AR_KEYTABLE_TYPE(micentry),
+				  AR_KEYTABLE_TYPE_CLR);
+		}
 
 	}
 
diff --git a/drivers/net/wireless/ath/regd.c b/drivers/net/wireless/ath/regd.c
index 2b14775e6bc6..f828f294ba89 100644
--- a/drivers/net/wireless/ath/regd.c
+++ b/drivers/net/wireless/ath/regd.c
@@ -158,6 +158,13 @@ ieee80211_regdomain *ath_world_regdomain(struct ath_regulatory *reg)
 	}
 }
 
+bool ath_is_49ghz_allowed(u16 regdomain)
+{
+	/* possibly more */
+	return regdomain == MKK9_MKKC;
+}
+EXPORT_SYMBOL(ath_is_49ghz_allowed);
+
 /* Frequency is one where radar detection is required */
 static bool ath_is_radar_freq(u16 center_freq)
 {
diff --git a/drivers/net/wireless/ath/regd.h b/drivers/net/wireless/ath/regd.h
index 345dd9721b41..172f63f671cf 100644
--- a/drivers/net/wireless/ath/regd.h
+++ b/drivers/net/wireless/ath/regd.h
@@ -250,6 +250,7 @@ enum CountryCode {
 };
 
 bool ath_is_world_regd(struct ath_regulatory *reg);
+bool ath_is_49ghz_allowed(u16 redomain);
 int ath_regd_init(struct ath_regulatory *reg, struct wiphy *wiphy,
 		  int (*reg_notifier)(struct wiphy *wiphy,
 		  struct regulatory_request *request));
diff --git a/drivers/net/wireless/b43/Kconfig b/drivers/net/wireless/b43/Kconfig
index 47033f6a1c2b..480595f04411 100644
--- a/drivers/net/wireless/b43/Kconfig
+++ b/drivers/net/wireless/b43/Kconfig
@@ -92,7 +92,7 @@ config B43_PHY_N
 	---help---
 	  Support for the N-PHY.
 
-	  This enables support for devices with N-PHY revision up to 2.
+	  This enables support for devices with N-PHY.
 
 	  Say N if you expect high stability and performance. Saying Y will not
 	  affect other devices support and may provide support for basic needs.
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 22bc9f17f634..57eb5b649730 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -3203,7 +3203,7 @@ static void b43_tx_work(struct work_struct *work)
 	mutex_unlock(&wl->mutex);
 }
 
-static int b43_op_tx(struct ieee80211_hw *hw,
+static void b43_op_tx(struct ieee80211_hw *hw,
 		     struct sk_buff *skb)
 {
 	struct b43_wl *wl = hw_to_b43_wl(hw);
@@ -3211,14 +3211,12 @@ static int b43_op_tx(struct ieee80211_hw *hw,
 	if (unlikely(skb->len < 2 + 2 + 6)) {
 		/* Too short, this can't be a valid frame. */
 		dev_kfree_skb_any(skb);
-		return NETDEV_TX_OK;
+		return;
 	}
 	B43_WARN_ON(skb_shinfo(skb)->nr_frags);
 
 	skb_queue_tail(&wl->tx_queue, skb);
 	ieee80211_queue_work(wl->hw, &wl->tx_work);
-
-	return NETDEV_TX_OK;
 }
 
 static void b43_qos_params_upload(struct b43_wldev *dev,
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index ab81ed8b19d7..8a00f9a95dbb 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -430,9 +430,9 @@ static void b43_radio_init2055_post(struct b43_wldev *dev)
 	bool workaround = false;
 
 	if (sprom->revision < 4)
-		workaround = (binfo->vendor != PCI_VENDOR_ID_BROADCOM ||
-				binfo->type != 0x46D ||
-				binfo->rev < 0x41);
+		workaround = (binfo->vendor != PCI_VENDOR_ID_BROADCOM &&
+				binfo->type == 0x46D &&
+				binfo->rev >= 0x41);
 	else
 		workaround =
 			!(sprom->boardflags2_lo & B43_BFL2_RXBB_INT_REG_DIS);
@@ -1168,23 +1168,98 @@ static void b43_nphy_adjust_lna_gain_table(struct b43_wldev *dev)
 static void b43_nphy_gain_ctrl_workarounds(struct b43_wldev *dev)
 {
 	struct b43_phy_n *nphy = dev->phy.n;
+	struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
+
+	/* PHY rev 0, 1, 2 */
 	u8 i, j;
 	u8 code;
 	u16 tmp;
+	u8 rfseq_events[3] = { 6, 8, 7 };
+	u8 rfseq_delays[3] = { 10, 30, 1 };
 
-	/* TODO: for PHY >= 3
-	s8 *lna1_gain, *lna2_gain;
-	u8 *gain_db, *gain_bits;
-	u16 *rfseq_init;
+	/* PHY rev >= 3 */
+	bool ghz5;
+	bool ext_lna;
+	u16 rssi_gain;
+	struct nphy_gain_ctl_workaround_entry *e;
 	u8 lpf_gain[6] = { 0x00, 0x06, 0x0C, 0x12, 0x12, 0x12 };
 	u8 lpf_bits[6] = { 0, 1, 2, 3, 3, 3 };
-	*/
-
-	u8 rfseq_events[3] = { 6, 8, 7 };
-	u8 rfseq_delays[3] = { 10, 30, 1 };
 
 	if (dev->phy.rev >= 3) {
-		/* TODO */
+		/* Prepare values */
+		ghz5 = b43_phy_read(dev, B43_NPHY_BANDCTL)
+			& B43_NPHY_BANDCTL_5GHZ;
+		ext_lna = sprom->boardflags_lo & B43_BFL_EXTLNA;
+		e = b43_nphy_get_gain_ctl_workaround_ent(dev, ghz5, ext_lna);
+		if (ghz5 && dev->phy.rev >= 5)
+			rssi_gain = 0x90;
+		else
+			rssi_gain = 0x50;
+
+		b43_phy_set(dev, B43_NPHY_RXCTL, 0x0040);
+
+		/* Set Clip 2 detect */
+		b43_phy_set(dev, B43_NPHY_C1_CGAINI,
+				B43_NPHY_C1_CGAINI_CL2DETECT);
+		b43_phy_set(dev, B43_NPHY_C2_CGAINI,
+				B43_NPHY_C2_CGAINI_CL2DETECT);
+
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_BIASPOLE_LNAG1_IDAC,
+				0x17);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_BIASPOLE_LNAG1_IDAC,
+				0x17);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_LNAG2_IDAC, 0xF0);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_LNAG2_IDAC, 0xF0);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_RSSI_POLE, 0x00);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_RSSI_POLE, 0x00);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_RSSI_GAIN,
+				rssi_gain);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_RSSI_GAIN,
+				rssi_gain);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_BIASPOLE_LNAA1_IDAC,
+				0x17);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_BIASPOLE_LNAA1_IDAC,
+				0x17);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_LNAA2_IDAC, 0xFF);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_LNAA2_IDAC, 0xFF);
+
+		b43_ntab_write_bulk(dev, B43_NTAB8(0, 8), 4, e->lna1_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB8(1, 8), 4, e->lna1_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB8(0, 16), 4, e->lna2_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB8(1, 16), 4, e->lna2_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB8(0, 32), 10, e->gain_db);
+		b43_ntab_write_bulk(dev, B43_NTAB8(1, 32), 10, e->gain_db);
+		b43_ntab_write_bulk(dev, B43_NTAB8(2, 32), 10, e->gain_bits);
+		b43_ntab_write_bulk(dev, B43_NTAB8(3, 32), 10, e->gain_bits);
+		b43_ntab_write_bulk(dev, B43_NTAB8(0, 0x40), 6, lpf_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB8(1, 0x40), 6, lpf_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB8(2, 0x40), 6, lpf_bits);
+		b43_ntab_write_bulk(dev, B43_NTAB8(3, 0x40), 6, lpf_bits);
+
+		b43_phy_write(dev, B43_NPHY_C1_INITGAIN, e->init_gain);
+		b43_phy_write(dev, 0x2A7, e->init_gain);
+		b43_ntab_write_bulk(dev, B43_NTAB16(7, 0x106), 2,
+					e->rfseq_init);
+		b43_phy_write(dev, B43_NPHY_C1_INITGAIN, e->init_gain);
+
+		/* TODO: check defines. Do not match variables names */
+		b43_phy_write(dev, B43_NPHY_C1_CLIP1_MEDGAIN, e->cliphi_gain);
+		b43_phy_write(dev, 0x2A9, e->cliphi_gain);
+		b43_phy_write(dev, B43_NPHY_C1_CLIP2_GAIN, e->clipmd_gain);
+		b43_phy_write(dev, 0x2AB, e->clipmd_gain);
+		b43_phy_write(dev, B43_NPHY_C2_CLIP1_HIGAIN, e->cliplo_gain);
+		b43_phy_write(dev, 0x2AD, e->cliplo_gain);
+
+		b43_phy_maskset(dev, 0x27D, 0xFF00, e->crsmin);
+		b43_phy_maskset(dev, 0x280, 0xFF00, e->crsminl);
+		b43_phy_maskset(dev, 0x283, 0xFF00, e->crsminu);
+		b43_phy_write(dev, B43_NPHY_C1_NBCLIPTHRES, e->nbclip);
+		b43_phy_write(dev, B43_NPHY_C2_NBCLIPTHRES, e->nbclip);
+		b43_phy_maskset(dev, B43_NPHY_C1_CLIPWBTHRES,
+				~B43_NPHY_C1_CLIPWBTHRES_CLIP2, e->wlclip);
+		b43_phy_maskset(dev, B43_NPHY_C2_CLIPWBTHRES,
+				~B43_NPHY_C2_CLIPWBTHRES_CLIP2, e->wlclip);
+		b43_phy_write(dev, B43_NPHY_CCK_SHIFTB_REF, 0x809C);
 	} else {
 		/* Set Clip 2 detect */
 		b43_phy_set(dev, B43_NPHY_C1_CGAINI,
@@ -1281,17 +1356,17 @@ static void b43_nphy_gain_ctrl_workarounds(struct b43_wldev *dev)
 						B43_NPHY_TABLE_DATALO, tmp);
 				}
 			}
+		}
 
-			b43_nphy_set_rf_sequence(dev, 5,
-					rfseq_events, rfseq_delays, 3);
-			b43_phy_maskset(dev, B43_NPHY_OVER_DGAIN1,
-				~B43_NPHY_OVER_DGAIN_CCKDGECV & 0xFFFF,
-				0x5A << B43_NPHY_OVER_DGAIN_CCKDGECV_SHIFT);
+		b43_nphy_set_rf_sequence(dev, 5,
+				rfseq_events, rfseq_delays, 3);
+		b43_phy_maskset(dev, B43_NPHY_OVER_DGAIN1,
+			~B43_NPHY_OVER_DGAIN_CCKDGECV & 0xFFFF,
+			0x5A << B43_NPHY_OVER_DGAIN_CCKDGECV_SHIFT);
 
-			if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
-				b43_phy_maskset(dev, B43_PHY_N(0xC5D),
-						0xFF80, 4);
-		}
+		if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
+			b43_phy_maskset(dev, B43_PHY_N(0xC5D),
+					0xFF80, 4);
 	}
 }
 
@@ -1308,6 +1383,9 @@ static void b43_nphy_workarounds(struct b43_wldev *dev)
 	u8 events2[7] = { 0x0, 0x3, 0x5, 0x4, 0x2, 0x1, 0x8 };
 	u8 delays2[7] = { 0x8, 0x6, 0x2, 0x4, 0x4, 0x6, 0x1 };
 
+	u16 tmp16;
+	u32 tmp32;
+
 	if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
 		b43_nphy_classifier(dev, 1, 0);
 	else
@@ -1320,7 +1398,82 @@ static void b43_nphy_workarounds(struct b43_wldev *dev)
 		    B43_NPHY_IQFLIP_ADC1 | B43_NPHY_IQFLIP_ADC2);
 
 	if (dev->phy.rev >= 3) {
+		tmp32 = b43_ntab_read(dev, B43_NTAB32(30, 0));
+		tmp32 &= 0xffffff;
+		b43_ntab_write(dev, B43_NTAB32(30, 0), tmp32);
+
+		b43_phy_write(dev, B43_NPHY_PHASETR_A0, 0x0125);
+		b43_phy_write(dev, B43_NPHY_PHASETR_A1, 0x01B3);
+		b43_phy_write(dev, B43_NPHY_PHASETR_A2, 0x0105);
+		b43_phy_write(dev, B43_NPHY_PHASETR_B0, 0x016E);
+		b43_phy_write(dev, B43_NPHY_PHASETR_B1, 0x00CD);
+		b43_phy_write(dev, B43_NPHY_PHASETR_B2, 0x0020);
+
+		b43_phy_write(dev, B43_NPHY_C2_CLIP1_MEDGAIN, 0x000C);
+		b43_phy_write(dev, 0x2AE, 0x000C);
+
 		/* TODO */
+
+		tmp16 = (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) ?
+			0x2 : 0x9C40;
+		b43_phy_write(dev, B43_NPHY_ENDROP_TLEN, tmp16);
+
+		b43_phy_maskset(dev, 0x294, 0xF0FF, 0x0700);
+
+		b43_ntab_write(dev, B43_NTAB32(16, 3), 0x18D);
+		b43_ntab_write(dev, B43_NTAB32(16, 127), 0x18D);
+
+		b43_nphy_gain_ctrl_workarounds(dev);
+
+		b43_ntab_write(dev, B43_NTAB32(8, 0), 2);
+		b43_ntab_write(dev, B43_NTAB32(8, 16), 2);
+
+		/* TODO */
+
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_MAST_BIAS, 0x00);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_MAST_BIAS, 0x00);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_BIAS_MAIN, 0x06);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_BIAS_MAIN, 0x06);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_BIAS_AUX, 0x07);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_BIAS_AUX, 0x07);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXA_LOB_BIAS, 0x88);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXA_LOB_BIAS, 0x88);
+		b43_radio_write(dev, B2056_RX0 | B2056_RX_MIXG_CMFB_IDAC, 0x00);
+		b43_radio_write(dev, B2056_RX1 | B2056_RX_MIXG_CMFB_IDAC, 0x00);
+
+		/* N PHY WAR TX Chain Update with hw_phytxchain as argument */
+
+		if ((bus->sprom.boardflags2_lo & B43_BFL2_APLL_WAR &&
+		    b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ||
+		    (bus->sprom.boardflags2_lo & B43_BFL2_GPLL_WAR &&
+		    b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ))
+			tmp32 = 0x00088888;
+		else
+			tmp32 = 0x88888888;
+		b43_ntab_write(dev, B43_NTAB32(30, 1), tmp32);
+		b43_ntab_write(dev, B43_NTAB32(30, 2), tmp32);
+		b43_ntab_write(dev, B43_NTAB32(30, 3), tmp32);
+
+		if (dev->phy.rev == 4 &&
+		    b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
+			b43_radio_write(dev, B2056_TX0 | B2056_TX_GMBB_IDAC,
+					0x70);
+			b43_radio_write(dev, B2056_TX1 | B2056_TX_GMBB_IDAC,
+					0x70);
+		}
+
+		b43_phy_write(dev, 0x224, 0x039C);
+		b43_phy_write(dev, 0x225, 0x0357);
+		b43_phy_write(dev, 0x226, 0x0317);
+		b43_phy_write(dev, 0x227, 0x02D7);
+		b43_phy_write(dev, 0x228, 0x039C);
+		b43_phy_write(dev, 0x229, 0x0357);
+		b43_phy_write(dev, 0x22A, 0x0317);
+		b43_phy_write(dev, 0x22B, 0x02D7);
+		b43_phy_write(dev, 0x22C, 0x039C);
+		b43_phy_write(dev, 0x22D, 0x0357);
+		b43_phy_write(dev, 0x22E, 0x0317);
+		b43_phy_write(dev, 0x22F, 0x02D7);
 	} else {
 		if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ &&
 		    nphy->band5g_pwrgain) {
@@ -2128,7 +2281,7 @@ static int b43_nphy_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
 		save_regs_phy[5] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
 		save_regs_phy[6] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B1S0);
 		save_regs_phy[7] = b43_phy_read(dev, B43_NPHY_TXF_40CO_B32S1);
-	} else if (dev->phy.rev == 2) {
+	} else {
 		save_regs_phy[0] = b43_phy_read(dev, B43_NPHY_AFECTL_C1);
 		save_regs_phy[1] = b43_phy_read(dev, B43_NPHY_AFECTL_C2);
 		save_regs_phy[2] = b43_phy_read(dev, B43_NPHY_AFECTL_OVER);
@@ -2179,7 +2332,7 @@ static int b43_nphy_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
 		b43_phy_write(dev, B43_NPHY_AFECTL_OVER, save_regs_phy[5]);
 		b43_phy_write(dev, B43_NPHY_TXF_40CO_B1S0, save_regs_phy[6]);
 		b43_phy_write(dev, B43_NPHY_TXF_40CO_B32S1, save_regs_phy[7]);
-	} else if (dev->phy.rev == 2) {
+	} else {
 		b43_phy_write(dev, B43_NPHY_AFECTL_C1, save_regs_phy[0]);
 		b43_phy_write(dev, B43_NPHY_AFECTL_C2, save_regs_phy[1]);
 		b43_phy_write(dev, B43_NPHY_AFECTL_OVER, save_regs_phy[2]);
@@ -3878,10 +4031,14 @@ static void b43_nphy_op_software_rfkill(struct b43_wldev *dev,
 	}
 }
 
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/Anacore */
 static void b43_nphy_op_switch_analog(struct b43_wldev *dev, bool on)
 {
-	b43_phy_write(dev, B43_NPHY_AFECTL_OVER,
-		      on ? 0 : 0x7FFF);
+	u16 val = on ? 0 : 0x7FFF;
+
+	if (dev->phy.rev >= 3)
+		b43_phy_write(dev, B43_NPHY_AFECTL_OVER1, val);
+	b43_phy_write(dev, B43_NPHY_AFECTL_OVER, val);
 }
 
 static int b43_nphy_op_switch_channel(struct b43_wldev *dev,
diff --git a/drivers/net/wireless/b43/tables_nphy.c b/drivers/net/wireless/b43/tables_nphy.c
index dc8ef09a8552..2de483b3d3ba 100644
--- a/drivers/net/wireless/b43/tables_nphy.c
+++ b/drivers/net/wireless/b43/tables_nphy.c
@@ -1097,6 +1097,1080 @@ static const u32 b43_ntab_tmap[] = {
 	0x00000000, 0x00000000, 0x00000000, 0x00000000,
 };
 
+/* static tables, PHY revision >= 3 */
+static const u32 b43_ntab_framestruct_r3[] = {
+	0x08004a04, 0x00100000, 0x01000a05, 0x00100020,
+	0x09804506, 0x00100030, 0x09804507, 0x00100030,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x08004a0c, 0x00100004, 0x01000a0d, 0x00100024,
+	0x0980450e, 0x00100034, 0x0980450f, 0x00100034,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000a04, 0x00100000, 0x11008a05, 0x00100020,
+	0x1980c506, 0x00100030, 0x21810506, 0x00100030,
+	0x21810506, 0x00100030, 0x01800504, 0x00100030,
+	0x11808505, 0x00100030, 0x29814507, 0x01100030,
+	0x00000a04, 0x00100000, 0x11008a05, 0x00100020,
+	0x21810506, 0x00100030, 0x21810506, 0x00100030,
+	0x29814507, 0x01100030, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000a0c, 0x00100008, 0x11008a0d, 0x00100028,
+	0x1980c50e, 0x00100038, 0x2181050e, 0x00100038,
+	0x2181050e, 0x00100038, 0x0180050c, 0x00100038,
+	0x1180850d, 0x00100038, 0x2981450f, 0x01100038,
+	0x00000a0c, 0x00100008, 0x11008a0d, 0x00100028,
+	0x2181050e, 0x00100038, 0x2181050e, 0x00100038,
+	0x2981450f, 0x01100038, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x08004a04, 0x00100000, 0x01000a05, 0x00100020,
+	0x1980c506, 0x00100030, 0x1980c506, 0x00100030,
+	0x11808504, 0x00100030, 0x3981ca05, 0x00100030,
+	0x29814507, 0x01100030, 0x00000000, 0x00000000,
+	0x10008a04, 0x00100000, 0x3981ca05, 0x00100030,
+	0x1980c506, 0x00100030, 0x29814507, 0x01100030,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x08004a0c, 0x00100008, 0x01000a0d, 0x00100028,
+	0x1980c50e, 0x00100038, 0x1980c50e, 0x00100038,
+	0x1180850c, 0x00100038, 0x3981ca0d, 0x00100038,
+	0x2981450f, 0x01100038, 0x00000000, 0x00000000,
+	0x10008a0c, 0x00100008, 0x3981ca0d, 0x00100038,
+	0x1980c50e, 0x00100038, 0x2981450f, 0x01100038,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x40021404, 0x00100000, 0x02001405, 0x00100040,
+	0x0b004a06, 0x01900060, 0x13008a06, 0x01900060,
+	0x13008a06, 0x01900060, 0x43020a04, 0x00100060,
+	0x1b00ca05, 0x00100060, 0x23010a07, 0x01500060,
+	0x40021404, 0x00100000, 0x1a00d405, 0x00100040,
+	0x13008a06, 0x01900060, 0x13008a06, 0x01900060,
+	0x23010a07, 0x01500060, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x4002140c, 0x00100010, 0x0200140d, 0x00100050,
+	0x0b004a0e, 0x01900070, 0x13008a0e, 0x01900070,
+	0x13008a0e, 0x01900070, 0x43020a0c, 0x00100070,
+	0x1b00ca0d, 0x00100070, 0x23010a0f, 0x01500070,
+	0x4002140c, 0x00100010, 0x1a00d40d, 0x00100050,
+	0x13008a0e, 0x01900070, 0x13008a0e, 0x01900070,
+	0x23010a0f, 0x01500070, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x50029404, 0x00100000, 0x32019405, 0x00100040,
+	0x0b004a06, 0x01900060, 0x0b004a06, 0x01900060,
+	0x5b02ca04, 0x00100060, 0x3b01d405, 0x00100060,
+	0x23010a07, 0x01500060, 0x00000000, 0x00000000,
+	0x5802d404, 0x00100000, 0x3b01d405, 0x00100060,
+	0x0b004a06, 0x01900060, 0x23010a07, 0x01500060,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x5002940c, 0x00100010, 0x3201940d, 0x00100050,
+	0x0b004a0e, 0x01900070, 0x0b004a0e, 0x01900070,
+	0x5b02ca0c, 0x00100070, 0x3b01d40d, 0x00100070,
+	0x23010a0f, 0x01500070, 0x00000000, 0x00000000,
+	0x5802d40c, 0x00100010, 0x3b01d40d, 0x00100070,
+	0x0b004a0e, 0x01900070, 0x23010a0f, 0x01500070,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x40021404, 0x000f4800, 0x62031405, 0x00100040,
+	0x53028a06, 0x01900060, 0x53028a07, 0x01900060,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x4002140c, 0x000f4808, 0x6203140d, 0x00100048,
+	0x53028a0e, 0x01900068, 0x53028a0f, 0x01900068,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000a0c, 0x00100004, 0x11008a0d, 0x00100024,
+	0x1980c50e, 0x00100034, 0x2181050e, 0x00100034,
+	0x2181050e, 0x00100034, 0x0180050c, 0x00100038,
+	0x1180850d, 0x00100038, 0x1181850d, 0x00100038,
+	0x2981450f, 0x01100038, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000a0c, 0x00100008, 0x11008a0d, 0x00100028,
+	0x2181050e, 0x00100038, 0x2181050e, 0x00100038,
+	0x1181850d, 0x00100038, 0x2981450f, 0x01100038,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x08004a04, 0x00100000, 0x01000a05, 0x00100020,
+	0x0180c506, 0x00100030, 0x0180c506, 0x00100030,
+	0x2180c50c, 0x00100030, 0x49820a0d, 0x0016a130,
+	0x41824a0d, 0x0016a130, 0x2981450f, 0x01100030,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x2000ca0c, 0x00100000, 0x49820a0d, 0x0016a130,
+	0x1980c50e, 0x00100030, 0x41824a0d, 0x0016a130,
+	0x2981450f, 0x01100030, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x4002140c, 0x00100008, 0x0200140d, 0x00100048,
+	0x0b004a0e, 0x01900068, 0x13008a0e, 0x01900068,
+	0x13008a0e, 0x01900068, 0x43020a0c, 0x00100070,
+	0x1b00ca0d, 0x00100070, 0x1b014a0d, 0x00100070,
+	0x23010a0f, 0x01500070, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x4002140c, 0x00100010, 0x1a00d40d, 0x00100050,
+	0x13008a0e, 0x01900070, 0x13008a0e, 0x01900070,
+	0x1b014a0d, 0x00100070, 0x23010a0f, 0x01500070,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x50029404, 0x00100000, 0x32019405, 0x00100040,
+	0x03004a06, 0x01900060, 0x03004a06, 0x01900060,
+	0x6b030a0c, 0x00100060, 0x4b02140d, 0x0016a160,
+	0x4302540d, 0x0016a160, 0x23010a0f, 0x01500060,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x6b03140c, 0x00100060, 0x4b02140d, 0x0016a160,
+	0x0b004a0e, 0x01900060, 0x4302540d, 0x0016a160,
+	0x23010a0f, 0x01500060, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x40021404, 0x00100000, 0x1a00d405, 0x00100040,
+	0x53028a06, 0x01900060, 0x5b02ca06, 0x01900060,
+	0x5b02ca06, 0x01900060, 0x43020a04, 0x00100060,
+	0x1b00ca05, 0x00100060, 0x53028a07, 0x0190c060,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x4002140c, 0x00100010, 0x1a00d40d, 0x00100050,
+	0x53028a0e, 0x01900070, 0x5b02ca0e, 0x01900070,
+	0x5b02ca0e, 0x01900070, 0x43020a0c, 0x00100070,
+	0x1b00ca0d, 0x00100070, 0x53028a0f, 0x0190c070,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x40021404, 0x00100000, 0x1a00d405, 0x00100040,
+	0x5b02ca06, 0x01900060, 0x5b02ca06, 0x01900060,
+	0x53028a07, 0x0190c060, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x4002140c, 0x00100010, 0x1a00d40d, 0x00100050,
+	0x5b02ca0e, 0x01900070, 0x5b02ca0e, 0x01900070,
+	0x53028a0f, 0x0190c070, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+static const u16 b43_ntab_pilot_r3[] = {
+	0xff08, 0xff08, 0xff08, 0xff08, 0xff08, 0xff08,
+	0xff08, 0xff08, 0x80d5, 0x80d5, 0x80d5, 0x80d5,
+	0x80d5, 0x80d5, 0x80d5, 0x80d5, 0xff0a, 0xff82,
+	0xffa0, 0xff28, 0xffff, 0xffff, 0xffff, 0xffff,
+	0xff82, 0xffa0, 0xff28, 0xff0a, 0xffff, 0xffff,
+	0xffff, 0xffff, 0xf83f, 0xfa1f, 0xfa97, 0xfab5,
+	0xf2bd, 0xf0bf, 0xffff, 0xffff, 0xf017, 0xf815,
+	0xf215, 0xf095, 0xf035, 0xf01d, 0xffff, 0xffff,
+	0xff08, 0xff02, 0xff80, 0xff20, 0xff08, 0xff02,
+	0xff80, 0xff20, 0xf01f, 0xf817, 0xfa15, 0xf295,
+	0xf0b5, 0xf03d, 0xffff, 0xffff, 0xf82a, 0xfa0a,
+	0xfa82, 0xfaa0, 0xf2a8, 0xf0aa, 0xffff, 0xffff,
+	0xf002, 0xf800, 0xf200, 0xf080, 0xf020, 0xf008,
+	0xffff, 0xffff, 0xf00a, 0xf802, 0xfa00, 0xf280,
+	0xf0a0, 0xf028, 0xffff, 0xffff,
+};
+
+static const u32 b43_ntab_tmap_r3[] = {
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0xf1111110, 0x11111111, 0x11f11111, 0x00000111,
+	0x11000000, 0x1111f111, 0x11111111, 0x111111f1,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x000aa888,
+	0x88880000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0xa1111110, 0x11111111, 0x11c11111, 0x00000111,
+	0x11000000, 0x1111a111, 0x11111111, 0x111111a1,
+	0xa2222220, 0x22222222, 0x22c22222, 0x00000222,
+	0x22000000, 0x2222a222, 0x22222222, 0x222222a2,
+	0xf1111110, 0x11111111, 0x11f11111, 0x00011111,
+	0x11110000, 0x1111f111, 0x11111111, 0x111111f1,
+	0xa8aa88a0, 0xa88888a8, 0xa8a8a88a, 0x00088aaa,
+	0xaaaa0000, 0xa8a8aa88, 0xa88aaaaa, 0xaaaa8a8a,
+	0xaaa8aaa0, 0x8aaa8aaa, 0xaa8a8a8a, 0x000aaa88,
+	0x8aaa0000, 0xaaa8a888, 0x8aa88a8a, 0x8a88a888,
+	0x08080a00, 0x0a08080a, 0x080a0a08, 0x00080808,
+	0x080a0000, 0x080a0808, 0x080a0808, 0x0a0a0a08,
+	0xa0a0a0a0, 0x80a0a080, 0x8080a0a0, 0x00008080,
+	0x80a00000, 0x80a080a0, 0xa080a0a0, 0x8080a0a0,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x99999000, 0x9b9b99bb, 0x9bb99999, 0x9999b9b9,
+	0x9b99bb90, 0x9bbbbb9b, 0x9b9b9bb9, 0x00000999,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00aaa888,
+	0x22000000, 0x2222b222, 0x22222222, 0x222222b2,
+	0xb2222220, 0x22222222, 0x22d22222, 0x00000222,
+	0x11000000, 0x1111a111, 0x11111111, 0x111111a1,
+	0xa1111110, 0x11111111, 0x11c11111, 0x00000111,
+	0x33000000, 0x3333b333, 0x33333333, 0x333333b3,
+	0xb3333330, 0x33333333, 0x33d33333, 0x00000333,
+	0x22000000, 0x2222a222, 0x22222222, 0x222222a2,
+	0xa2222220, 0x22222222, 0x22c22222, 0x00000222,
+	0x99b99b00, 0x9b9b99bb, 0x9bb99999, 0x9999b9b9,
+	0x9b99bb99, 0x9bbbbb9b, 0x9b9b9bb9, 0x00000999,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa88, 0x8aaaaa8a, 0x8a8a8aa8, 0x08aaa888,
+	0x22222200, 0x2222f222, 0x22222222, 0x222222f2,
+	0x22222222, 0x22222222, 0x22f22222, 0x00000222,
+	0x11000000, 0x1111f111, 0x11111111, 0x11111111,
+	0xf1111111, 0x11111111, 0x11f11111, 0x01111111,
+	0xbb9bb900, 0xb9b9bb99, 0xb99bbbbb, 0xbbbb9b9b,
+	0xb9bb99bb, 0xb99999b9, 0xb9b9b99b, 0x00000bbb,
+	0xaa000000, 0xa8a8aa88, 0xa88aaaaa, 0xaaaa8a8a,
+	0xa8aa88aa, 0xa88888a8, 0xa8a8a88a, 0x0a888aaa,
+	0xaa000000, 0xa8a8aa88, 0xa88aaaaa, 0xaaaa8a8a,
+	0xa8aa88a0, 0xa88888a8, 0xa8a8a88a, 0x00000aaa,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0xbbbbbb00, 0x999bbbbb, 0x9bb99b9b, 0xb9b9b9bb,
+	0xb9b99bbb, 0xb9b9b9bb, 0xb9bb9b99, 0x00000999,
+	0x8a000000, 0xaa88a888, 0xa88888aa, 0xa88a8a88,
+	0xa88aa88a, 0x88a8aaaa, 0xa8aa8aaa, 0x0888a88a,
+	0x0b0b0b00, 0x090b0b0b, 0x0b090b0b, 0x0909090b,
+	0x09090b0b, 0x09090b0b, 0x09090b09, 0x00000909,
+	0x0a000000, 0x0a080808, 0x080a080a, 0x080a0a08,
+	0x080a080a, 0x0808080a, 0x0a0a0a08, 0x0808080a,
+	0xb0b0b000, 0x9090b0b0, 0x90b09090, 0xb0b0b090,
+	0xb0b090b0, 0x90b0b0b0, 0xb0b09090, 0x00000090,
+	0x80000000, 0xa080a080, 0xa08080a0, 0xa0808080,
+	0xa080a080, 0x80a0a0a0, 0xa0a080a0, 0x00a0a0a0,
+	0x22000000, 0x2222f222, 0x22222222, 0x222222f2,
+	0xf2222220, 0x22222222, 0x22f22222, 0x00000222,
+	0x11000000, 0x1111f111, 0x11111111, 0x111111f1,
+	0xf1111110, 0x11111111, 0x11f11111, 0x00000111,
+	0x33000000, 0x3333f333, 0x33333333, 0x333333f3,
+	0xf3333330, 0x33333333, 0x33f33333, 0x00000333,
+	0x22000000, 0x2222f222, 0x22222222, 0x222222f2,
+	0xf2222220, 0x22222222, 0x22f22222, 0x00000222,
+	0x99000000, 0x9b9b99bb, 0x9bb99999, 0x9999b9b9,
+	0x9b99bb90, 0x9bbbbb9b, 0x9b9b9bb9, 0x00000999,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0x88888000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00aaa888,
+	0x88a88a00, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa88, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa88, 0x8aaaaa8a, 0x8a8a8aa8, 0x08aaa888,
+	0x11000000, 0x1111a111, 0x11111111, 0x111111a1,
+	0xa1111110, 0x11111111, 0x11c11111, 0x00000111,
+	0x11000000, 0x1111a111, 0x11111111, 0x111111a1,
+	0xa1111110, 0x11111111, 0x11c11111, 0x00000111,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0x88000000, 0x8a8a88aa, 0x8aa88888, 0x8888a8a8,
+	0x8a88aa80, 0x8aaaaa8a, 0x8a8a8aa8, 0x00000888,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+static const u32 b43_ntab_intlevel_r3[] = {
+	0x00802070, 0x0671188d, 0x0a60192c, 0x0a300e46,
+	0x00c1188d, 0x080024d2, 0x00000070,
+};
+
+static const u32 b43_ntab_tdtrn_r3[] = {
+	0x061c061c, 0x0050ee68, 0xf592fe36, 0xfe5212f6,
+	0x00000c38, 0xfe5212f6, 0xf592fe36, 0x0050ee68,
+	0x061c061c, 0xee680050, 0xfe36f592, 0x12f6fe52,
+	0x0c380000, 0x12f6fe52, 0xfe36f592, 0xee680050,
+	0x061c061c, 0x0050ee68, 0xf592fe36, 0xfe5212f6,
+	0x00000c38, 0xfe5212f6, 0xf592fe36, 0x0050ee68,
+	0x061c061c, 0xee680050, 0xfe36f592, 0x12f6fe52,
+	0x0c380000, 0x12f6fe52, 0xfe36f592, 0xee680050,
+	0x05e305e3, 0x004def0c, 0xf5f3fe47, 0xfe611246,
+	0x00000bc7, 0xfe611246, 0xf5f3fe47, 0x004def0c,
+	0x05e305e3, 0xef0c004d, 0xfe47f5f3, 0x1246fe61,
+	0x0bc70000, 0x1246fe61, 0xfe47f5f3, 0xef0c004d,
+	0x05e305e3, 0x004def0c, 0xf5f3fe47, 0xfe611246,
+	0x00000bc7, 0xfe611246, 0xf5f3fe47, 0x004def0c,
+	0x05e305e3, 0xef0c004d, 0xfe47f5f3, 0x1246fe61,
+	0x0bc70000, 0x1246fe61, 0xfe47f5f3, 0xef0c004d,
+	0xfa58fa58, 0xf895043b, 0xff4c09c0, 0xfbc6ffa8,
+	0xfb84f384, 0x0798f6f9, 0x05760122, 0x058409f6,
+	0x0b500000, 0x05b7f542, 0x08860432, 0x06ddfee7,
+	0xfb84f384, 0xf9d90664, 0xf7e8025c, 0x00fff7bd,
+	0x05a805a8, 0xf7bd00ff, 0x025cf7e8, 0x0664f9d9,
+	0xf384fb84, 0xfee706dd, 0x04320886, 0xf54205b7,
+	0x00000b50, 0x09f60584, 0x01220576, 0xf6f90798,
+	0xf384fb84, 0xffa8fbc6, 0x09c0ff4c, 0x043bf895,
+	0x02d402d4, 0x07de0270, 0xfc96079c, 0xf90afe94,
+	0xfe00ff2c, 0x02d4065d, 0x092a0096, 0x0014fbb8,
+	0xfd2cfd2c, 0x076afb3c, 0x0096f752, 0xf991fd87,
+	0xfb2c0200, 0xfeb8f960, 0x08e0fc96, 0x049802a8,
+	0xfd2cfd2c, 0x02a80498, 0xfc9608e0, 0xf960feb8,
+	0x0200fb2c, 0xfd87f991, 0xf7520096, 0xfb3c076a,
+	0xfd2cfd2c, 0xfbb80014, 0x0096092a, 0x065d02d4,
+	0xff2cfe00, 0xfe94f90a, 0x079cfc96, 0x027007de,
+	0x02d402d4, 0x027007de, 0x079cfc96, 0xfe94f90a,
+	0xff2cfe00, 0x065d02d4, 0x0096092a, 0xfbb80014,
+	0xfd2cfd2c, 0xfb3c076a, 0xf7520096, 0xfd87f991,
+	0x0200fb2c, 0xf960feb8, 0xfc9608e0, 0x02a80498,
+	0xfd2cfd2c, 0x049802a8, 0x08e0fc96, 0xfeb8f960,
+	0xfb2c0200, 0xf991fd87, 0x0096f752, 0x076afb3c,
+	0xfd2cfd2c, 0x0014fbb8, 0x092a0096, 0x02d4065d,
+	0xfe00ff2c, 0xf90afe94, 0xfc96079c, 0x07de0270,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x062a0000, 0xfefa0759, 0x08b80908, 0xf396fc2d,
+	0xf9d6045c, 0xfc4ef608, 0xf748f596, 0x07b207bf,
+	0x062a062a, 0xf84ef841, 0xf748f596, 0x03b209f8,
+	0xf9d6045c, 0x0c6a03d3, 0x08b80908, 0x0106f8a7,
+	0x062a0000, 0xfefaf8a7, 0x08b8f6f8, 0xf39603d3,
+	0xf9d6fba4, 0xfc4e09f8, 0xf7480a6a, 0x07b2f841,
+	0x062af9d6, 0xf84e07bf, 0xf7480a6a, 0x03b2f608,
+	0xf9d6fba4, 0x0c6afc2d, 0x08b8f6f8, 0x01060759,
+	0x062a0000, 0xfefa0759, 0x08b80908, 0xf396fc2d,
+	0xf9d6045c, 0xfc4ef608, 0xf748f596, 0x07b207bf,
+	0x062a062a, 0xf84ef841, 0xf748f596, 0x03b209f8,
+	0xf9d6045c, 0x0c6a03d3, 0x08b80908, 0x0106f8a7,
+	0x062a0000, 0xfefaf8a7, 0x08b8f6f8, 0xf39603d3,
+	0xf9d6fba4, 0xfc4e09f8, 0xf7480a6a, 0x07b2f841,
+	0x062af9d6, 0xf84e07bf, 0xf7480a6a, 0x03b2f608,
+	0xf9d6fba4, 0x0c6afc2d, 0x08b8f6f8, 0x01060759,
+	0x061c061c, 0xff30009d, 0xffb21141, 0xfd87fb54,
+	0xf65dfe59, 0x02eef99e, 0x0166f03c, 0xfff809b6,
+	0x000008a4, 0x000af42b, 0x00eff577, 0xfa840bf2,
+	0xfc02ff51, 0x08260f67, 0xfff0036f, 0x0842f9c3,
+	0x00000000, 0x063df7be, 0xfc910010, 0xf099f7da,
+	0x00af03fe, 0xf40e057c, 0x0a89ff11, 0x0bd5fff6,
+	0xf75c0000, 0xf64a0008, 0x0fc4fe9a, 0x0662fd12,
+	0x01a709a3, 0x04ac0279, 0xeebf004e, 0xff6300d0,
+	0xf9e4f9e4, 0x00d0ff63, 0x004eeebf, 0x027904ac,
+	0x09a301a7, 0xfd120662, 0xfe9a0fc4, 0x0008f64a,
+	0x0000f75c, 0xfff60bd5, 0xff110a89, 0x057cf40e,
+	0x03fe00af, 0xf7daf099, 0x0010fc91, 0xf7be063d,
+	0x00000000, 0xf9c30842, 0x036ffff0, 0x0f670826,
+	0xff51fc02, 0x0bf2fa84, 0xf57700ef, 0xf42b000a,
+	0x08a40000, 0x09b6fff8, 0xf03c0166, 0xf99e02ee,
+	0xfe59f65d, 0xfb54fd87, 0x1141ffb2, 0x009dff30,
+	0x05e30000, 0xff060705, 0x085408a0, 0xf425fc59,
+	0xfa1d042a, 0xfc78f67a, 0xf7acf60e, 0x075a0766,
+	0x05e305e3, 0xf8a6f89a, 0xf7acf60e, 0x03880986,
+	0xfa1d042a, 0x0bdb03a7, 0x085408a0, 0x00faf8fb,
+	0x05e30000, 0xff06f8fb, 0x0854f760, 0xf42503a7,
+	0xfa1dfbd6, 0xfc780986, 0xf7ac09f2, 0x075af89a,
+	0x05e3fa1d, 0xf8a60766, 0xf7ac09f2, 0x0388f67a,
+	0xfa1dfbd6, 0x0bdbfc59, 0x0854f760, 0x00fa0705,
+	0x05e30000, 0xff060705, 0x085408a0, 0xf425fc59,
+	0xfa1d042a, 0xfc78f67a, 0xf7acf60e, 0x075a0766,
+	0x05e305e3, 0xf8a6f89a, 0xf7acf60e, 0x03880986,
+	0xfa1d042a, 0x0bdb03a7, 0x085408a0, 0x00faf8fb,
+	0x05e30000, 0xff06f8fb, 0x0854f760, 0xf42503a7,
+	0xfa1dfbd6, 0xfc780986, 0xf7ac09f2, 0x075af89a,
+	0x05e3fa1d, 0xf8a60766, 0xf7ac09f2, 0x0388f67a,
+	0xfa1dfbd6, 0x0bdbfc59, 0x0854f760, 0x00fa0705,
+	0xfa58fa58, 0xf8f0fe00, 0x0448073d, 0xfdc9fe46,
+	0xf9910258, 0x089d0407, 0xfd5cf71a, 0x02affde0,
+	0x083e0496, 0xff5a0740, 0xff7afd97, 0x00fe01f1,
+	0x0009082e, 0xfa94ff75, 0xfecdf8ea, 0xffb0f693,
+	0xfd2cfa58, 0x0433ff16, 0xfba405dd, 0xfa610341,
+	0x06a606cb, 0x0039fd2d, 0x0677fa97, 0x01fa05e0,
+	0xf896003e, 0x075a068b, 0x012cfc3e, 0xfa23f98d,
+	0xfc7cfd43, 0xff90fc0d, 0x01c10982, 0x00c601d6,
+	0xfd2cfd2c, 0x01d600c6, 0x098201c1, 0xfc0dff90,
+	0xfd43fc7c, 0xf98dfa23, 0xfc3e012c, 0x068b075a,
+	0x003ef896, 0x05e001fa, 0xfa970677, 0xfd2d0039,
+	0x06cb06a6, 0x0341fa61, 0x05ddfba4, 0xff160433,
+	0xfa58fd2c, 0xf693ffb0, 0xf8eafecd, 0xff75fa94,
+	0x082e0009, 0x01f100fe, 0xfd97ff7a, 0x0740ff5a,
+	0x0496083e, 0xfde002af, 0xf71afd5c, 0x0407089d,
+	0x0258f991, 0xfe46fdc9, 0x073d0448, 0xfe00f8f0,
+	0xfd2cfd2c, 0xfce00500, 0xfc09fddc, 0xfe680157,
+	0x04c70571, 0xfc3aff21, 0xfcd70228, 0x056d0277,
+	0x0200fe00, 0x0022f927, 0xfe3c032b, 0xfc44ff3c,
+	0x03e9fbdb, 0x04570313, 0x04c9ff5c, 0x000d03b8,
+	0xfa580000, 0xfbe900d2, 0xf9d0fe0b, 0x0125fdf9,
+	0x042501bf, 0x0328fa2b, 0xffa902f0, 0xfa250157,
+	0x0200fe00, 0x03740438, 0xff0405fd, 0x030cfe52,
+	0x0037fb39, 0xff6904c5, 0x04f8fd23, 0xfd31fc1b,
+	0xfd2cfd2c, 0xfc1bfd31, 0xfd2304f8, 0x04c5ff69,
+	0xfb390037, 0xfe52030c, 0x05fdff04, 0x04380374,
+	0xfe000200, 0x0157fa25, 0x02f0ffa9, 0xfa2b0328,
+	0x01bf0425, 0xfdf90125, 0xfe0bf9d0, 0x00d2fbe9,
+	0x0000fa58, 0x03b8000d, 0xff5c04c9, 0x03130457,
+	0xfbdb03e9, 0xff3cfc44, 0x032bfe3c, 0xf9270022,
+	0xfe000200, 0x0277056d, 0x0228fcd7, 0xff21fc3a,
+	0x057104c7, 0x0157fe68, 0xfddcfc09, 0x0500fce0,
+	0xfd2cfd2c, 0x0500fce0, 0xfddcfc09, 0x0157fe68,
+	0x057104c7, 0xff21fc3a, 0x0228fcd7, 0x0277056d,
+	0xfe000200, 0xf9270022, 0x032bfe3c, 0xff3cfc44,
+	0xfbdb03e9, 0x03130457, 0xff5c04c9, 0x03b8000d,
+	0x0000fa58, 0x00d2fbe9, 0xfe0bf9d0, 0xfdf90125,
+	0x01bf0425, 0xfa2b0328, 0x02f0ffa9, 0x0157fa25,
+	0xfe000200, 0x04380374, 0x05fdff04, 0xfe52030c,
+	0xfb390037, 0x04c5ff69, 0xfd2304f8, 0xfc1bfd31,
+	0xfd2cfd2c, 0xfd31fc1b, 0x04f8fd23, 0xff6904c5,
+	0x0037fb39, 0x030cfe52, 0xff0405fd, 0x03740438,
+	0x0200fe00, 0xfa250157, 0xffa902f0, 0x0328fa2b,
+	0x042501bf, 0x0125fdf9, 0xf9d0fe0b, 0xfbe900d2,
+	0xfa580000, 0x000d03b8, 0x04c9ff5c, 0x04570313,
+	0x03e9fbdb, 0xfc44ff3c, 0xfe3c032b, 0x0022f927,
+	0x0200fe00, 0x056d0277, 0xfcd70228, 0xfc3aff21,
+	0x04c70571, 0xfe680157, 0xfc09fddc, 0xfce00500,
+	0x05a80000, 0xff1006be, 0x0800084a, 0xf49cfc7e,
+	0xfa580400, 0xfc9cf6da, 0xf800f672, 0x0710071c,
+	0x05a805a8, 0xf8f0f8e4, 0xf800f672, 0x03640926,
+	0xfa580400, 0x0b640382, 0x0800084a, 0x00f0f942,
+	0x05a80000, 0xff10f942, 0x0800f7b6, 0xf49c0382,
+	0xfa58fc00, 0xfc9c0926, 0xf800098e, 0x0710f8e4,
+	0x05a8fa58, 0xf8f0071c, 0xf800098e, 0x0364f6da,
+	0xfa58fc00, 0x0b64fc7e, 0x0800f7b6, 0x00f006be,
+	0x05a80000, 0xff1006be, 0x0800084a, 0xf49cfc7e,
+	0xfa580400, 0xfc9cf6da, 0xf800f672, 0x0710071c,
+	0x05a805a8, 0xf8f0f8e4, 0xf800f672, 0x03640926,
+	0xfa580400, 0x0b640382, 0x0800084a, 0x00f0f942,
+	0x05a80000, 0xff10f942, 0x0800f7b6, 0xf49c0382,
+	0xfa58fc00, 0xfc9c0926, 0xf800098e, 0x0710f8e4,
+	0x05a8fa58, 0xf8f0071c, 0xf800098e, 0x0364f6da,
+	0xfa58fc00, 0x0b64fc7e, 0x0800f7b6, 0x00f006be,
+};
+
+static const u32 b43_ntab_noisevar0_r3[] = {
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+};
+
+static const u32 b43_ntab_noisevar1_r3[] = {
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+	0x02110211, 0x0000014d, 0x02110211, 0x0000014d,
+};
+
+static const u16 b43_ntab_mcs_r3[] = {
+	0x0000, 0x0008, 0x000a, 0x0010, 0x0012, 0x0019,
+	0x001a, 0x001c, 0x0080, 0x0088, 0x008a, 0x0090,
+	0x0092, 0x0099, 0x009a, 0x009c, 0x0100, 0x0108,
+	0x010a, 0x0110, 0x0112, 0x0119, 0x011a, 0x011c,
+	0x0180, 0x0188, 0x018a, 0x0190, 0x0192, 0x0199,
+	0x019a, 0x019c, 0x0000, 0x0098, 0x00a0, 0x00a8,
+	0x009a, 0x00a2, 0x00aa, 0x0120, 0x0128, 0x0128,
+	0x0130, 0x0138, 0x0138, 0x0140, 0x0122, 0x012a,
+	0x012a, 0x0132, 0x013a, 0x013a, 0x0142, 0x01a8,
+	0x01b0, 0x01b8, 0x01b0, 0x01b8, 0x01c0, 0x01c8,
+	0x01c0, 0x01c8, 0x01d0, 0x01d0, 0x01d8, 0x01aa,
+	0x01b2, 0x01ba, 0x01b2, 0x01ba, 0x01c2, 0x01ca,
+	0x01c2, 0x01ca, 0x01d2, 0x01d2, 0x01da, 0x0001,
+	0x0002, 0x0004, 0x0009, 0x000c, 0x0011, 0x0014,
+	0x0018, 0x0020, 0x0021, 0x0022, 0x0024, 0x0081,
+	0x0082, 0x0084, 0x0089, 0x008c, 0x0091, 0x0094,
+	0x0098, 0x00a0, 0x00a1, 0x00a2, 0x00a4, 0x0007,
+	0x0007, 0x0007, 0x0007, 0x0007, 0x0007, 0x0007,
+	0x0007, 0x0007, 0x0007, 0x0007, 0x0007, 0x0007,
+	0x0007, 0x0007, 0x0007, 0x0007, 0x0007, 0x0007,
+	0x0007, 0x0007, 0x0007, 0x0007, 0x0007, 0x0007,
+	0x0007, 0x0007,
+};
+
+static const u32 b43_ntab_tdi20a0_r3[] = {
+	0x00091226, 0x000a1429, 0x000b56ad, 0x000c58b0,
+	0x000d5ab3, 0x000e9cb6, 0x000f9eba, 0x0000c13d,
+	0x00020301, 0x00030504, 0x00040708, 0x0005090b,
+	0x00064b8e, 0x00095291, 0x000a5494, 0x000b9718,
+	0x000c9927, 0x000d9b2a, 0x000edd2e, 0x000fdf31,
+	0x000101b4, 0x000243b7, 0x000345bb, 0x000447be,
+	0x00058982, 0x00068c05, 0x00099309, 0x000a950c,
+	0x000bd78f, 0x000cd992, 0x000ddb96, 0x000f1d99,
+	0x00005fa8, 0x0001422c, 0x0002842f, 0x00038632,
+	0x00048835, 0x0005ca38, 0x0006ccbc, 0x0009d3bf,
+	0x000b1603, 0x000c1806, 0x000d1a0a, 0x000e1c0d,
+	0x000f5e10, 0x00008093, 0x00018297, 0x0002c49a,
+	0x0003c680, 0x0004c880, 0x00060b00, 0x00070d00,
+	0x00000000, 0x00000000, 0x00000000,
+};
+
+static const u32 b43_ntab_tdi20a1_r3[] = {
+	0x00014b26, 0x00028d29, 0x000393ad, 0x00049630,
+	0x0005d833, 0x0006da36, 0x00099c3a, 0x000a9e3d,
+	0x000bc081, 0x000cc284, 0x000dc488, 0x000f068b,
+	0x0000488e, 0x00018b91, 0x0002d214, 0x0003d418,
+	0x0004d6a7, 0x000618aa, 0x00071aae, 0x0009dcb1,
+	0x000b1eb4, 0x000c0137, 0x000d033b, 0x000e053e,
+	0x000f4702, 0x00008905, 0x00020c09, 0x0003128c,
+	0x0004148f, 0x00051712, 0x00065916, 0x00091b19,
+	0x000a1d28, 0x000b5f2c, 0x000c41af, 0x000d43b2,
+	0x000e85b5, 0x000f87b8, 0x0000c9bc, 0x00024cbf,
+	0x00035303, 0x00045506, 0x0005978a, 0x0006998d,
+	0x00095b90, 0x000a5d93, 0x000b9f97, 0x000c821a,
+	0x000d8400, 0x000ec600, 0x000fc800, 0x00010a00,
+	0x00000000, 0x00000000, 0x00000000,
+};
+
+static const u32 b43_ntab_tdi40a0_r3[] = {
+	0x0011a346, 0x00136ccf, 0x0014f5d9, 0x001641e2,
+	0x0017cb6b, 0x00195475, 0x001b2383, 0x001cad0c,
+	0x001e7616, 0x0000821f, 0x00020ba8, 0x0003d4b2,
+	0x00056447, 0x00072dd0, 0x0008b6da, 0x000a02e3,
+	0x000b8c6c, 0x000d15f6, 0x0011e484, 0x0013ae0d,
+	0x00153717, 0x00168320, 0x00180ca9, 0x00199633,
+	0x001b6548, 0x001ceed1, 0x001eb7db, 0x0000c3e4,
+	0x00024d6d, 0x000416f7, 0x0005a585, 0x00076f0f,
+	0x0008f818, 0x000a4421, 0x000bcdab, 0x000d9734,
+	0x00122649, 0x0013efd2, 0x001578dc, 0x0016c4e5,
+	0x00184e6e, 0x001a17f8, 0x001ba686, 0x001d3010,
+	0x001ef999, 0x00010522, 0x00028eac, 0x00045835,
+	0x0005e74a, 0x0007b0d3, 0x00093a5d, 0x000a85e6,
+	0x000c0f6f, 0x000dd8f9, 0x00126787, 0x00143111,
+	0x0015ba9a, 0x00170623, 0x00188fad, 0x001a5936,
+	0x001be84b, 0x001db1d4, 0x001f3b5e, 0x000146e7,
+	0x00031070, 0x000499fa, 0x00062888, 0x0007f212,
+	0x00097b9b, 0x000ac7a4, 0x000c50ae, 0x000e1a37,
+	0x0012a94c, 0x001472d5, 0x0015fc5f, 0x00174868,
+	0x0018d171, 0x001a9afb, 0x001c2989, 0x001df313,
+	0x001f7c9c, 0x000188a5, 0x000351af, 0x0004db38,
+	0x0006aa4d, 0x000833d7, 0x0009bd60, 0x000b0969,
+	0x000c9273, 0x000e5bfc, 0x00132a8a, 0x0014b414,
+	0x00163d9d, 0x001789a6, 0x001912b0, 0x001adc39,
+	0x001c6bce, 0x001e34d8, 0x001fbe61, 0x0001ca6a,
+	0x00039374, 0x00051cfd, 0x0006ec0b, 0x00087515,
+	0x0009fe9e, 0x000b4aa7, 0x000cd3b1, 0x000e9d3a,
+	0x00000000, 0x00000000,
+};
+
+static const u32 b43_ntab_tdi40a1_r3[] = {
+	0x001edb36, 0x000129ca, 0x0002b353, 0x00047cdd,
+	0x0005c8e6, 0x000791ef, 0x00091bf9, 0x000aaa07,
+	0x000c3391, 0x000dfd1a, 0x00120923, 0x0013d22d,
+	0x00155c37, 0x0016eacb, 0x00187454, 0x001a3dde,
+	0x001b89e7, 0x001d12f0, 0x001f1cfa, 0x00016b88,
+	0x00033492, 0x0004be1b, 0x00060a24, 0x0007d32e,
+	0x00095d38, 0x000aec4c, 0x000c7555, 0x000e3edf,
+	0x00124ae8, 0x001413f1, 0x0015a37b, 0x00172c89,
+	0x0018b593, 0x001a419c, 0x001bcb25, 0x001d942f,
+	0x001f63b9, 0x0001ad4d, 0x00037657, 0x0004c260,
+	0x00068be9, 0x000814f3, 0x0009a47c, 0x000b2d8a,
+	0x000cb694, 0x000e429d, 0x00128c26, 0x001455b0,
+	0x0015e4ba, 0x00176e4e, 0x0018f758, 0x001a8361,
+	0x001c0cea, 0x001dd674, 0x001fa57d, 0x0001ee8b,
+	0x0003b795, 0x0005039e, 0x0006cd27, 0x000856b1,
+	0x0009e5c6, 0x000b6f4f, 0x000cf859, 0x000e8462,
+	0x00130deb, 0x00149775, 0x00162603, 0x0017af8c,
+	0x00193896, 0x001ac49f, 0x001c4e28, 0x001e17b2,
+	0x0000a6c7, 0x00023050, 0x0003f9da, 0x00054563,
+	0x00070eec, 0x00089876, 0x000a2704, 0x000bb08d,
+	0x000d3a17, 0x001185a0, 0x00134f29, 0x0014d8b3,
+	0x001667c8, 0x0017f151, 0x00197adb, 0x001b0664,
+	0x001c8fed, 0x001e5977, 0x0000e805, 0x0002718f,
+	0x00043b18, 0x000586a1, 0x0007502b, 0x0008d9b4,
+	0x000a68c9, 0x000bf252, 0x000dbbdc, 0x0011c7e5,
+	0x001390ee, 0x00151a78, 0x0016a906, 0x00183290,
+	0x0019bc19, 0x001b4822, 0x001cd12c, 0x001e9ab5,
+	0x00000000, 0x00000000,
+};
+
+static const u32 b43_ntab_pilotlt_r3[] = {
+	0x76540213, 0x62407351, 0x76543210, 0x76540213,
+	0x76540213, 0x76430521,
+};
+
+static const u32 b43_ntab_channelest_r3[] = {
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x44444444, 0x44444444, 0x44444444, 0x44444444,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+	0x10101010, 0x10101010, 0x10101010, 0x10101010,
+};
+
+static const u8 b43_ntab_framelookup_r3[] = {
+	0x02, 0x04, 0x14, 0x14, 0x03, 0x05, 0x16, 0x16,
+	0x0a, 0x0c, 0x1c, 0x1c, 0x0b, 0x0d, 0x1e, 0x1e,
+	0x06, 0x08, 0x18, 0x18, 0x07, 0x09, 0x1a, 0x1a,
+	0x0e, 0x10, 0x20, 0x28, 0x0f, 0x11, 0x22, 0x2a,
+};
+
+static const u8 b43_ntab_estimatepowerlt0_r3[] = {
+	0x55, 0x54, 0x54, 0x53, 0x52, 0x52, 0x51, 0x51,
+	0x50, 0x4f, 0x4f, 0x4e, 0x4e, 0x4d, 0x4c, 0x4c,
+	0x4b, 0x4a, 0x49, 0x49, 0x48, 0x47, 0x46, 0x46,
+	0x45, 0x44, 0x43, 0x42, 0x41, 0x40, 0x40, 0x3f,
+	0x3e, 0x3d, 0x3c, 0x3a, 0x39, 0x38, 0x37, 0x36,
+	0x35, 0x33, 0x32, 0x31, 0x2f, 0x2e, 0x2c, 0x2b,
+	0x29, 0x27, 0x25, 0x23, 0x21, 0x1f, 0x1d, 0x1a,
+	0x18, 0x15, 0x12, 0x0e, 0x0b, 0x07, 0x02, 0xfd,
+};
+
+static const u8 b43_ntab_estimatepowerlt1_r3[] = {
+	0x55, 0x54, 0x54, 0x53, 0x52, 0x52, 0x51, 0x51,
+	0x50, 0x4f, 0x4f, 0x4e, 0x4e, 0x4d, 0x4c, 0x4c,
+	0x4b, 0x4a, 0x49, 0x49, 0x48, 0x47, 0x46, 0x46,
+	0x45, 0x44, 0x43, 0x42, 0x41, 0x40, 0x40, 0x3f,
+	0x3e, 0x3d, 0x3c, 0x3a, 0x39, 0x38, 0x37, 0x36,
+	0x35, 0x33, 0x32, 0x31, 0x2f, 0x2e, 0x2c, 0x2b,
+	0x29, 0x27, 0x25, 0x23, 0x21, 0x1f, 0x1d, 0x1a,
+	0x18, 0x15, 0x12, 0x0e, 0x0b, 0x07, 0x02, 0xfd,
+};
+
+static const u8 b43_ntab_adjustpower0_r3[] = {
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const u8 b43_ntab_adjustpower1_r3[] = {
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const u32 b43_ntab_gainctl0_r3[] = {
+	0x5bf70044, 0x5bf70042, 0x5bf70040, 0x5bf7003e,
+	0x5bf7003c, 0x5bf7003b, 0x5bf70039, 0x5bf70037,
+	0x5bf70036, 0x5bf70034, 0x5bf70033, 0x5bf70031,
+	0x5bf70030, 0x5ba70044, 0x5ba70042, 0x5ba70040,
+	0x5ba7003e, 0x5ba7003c, 0x5ba7003b, 0x5ba70039,
+	0x5ba70037, 0x5ba70036, 0x5ba70034, 0x5ba70033,
+	0x5b770044, 0x5b770042, 0x5b770040, 0x5b77003e,
+	0x5b77003c, 0x5b77003b, 0x5b770039, 0x5b770037,
+	0x5b770036, 0x5b770034, 0x5b770033, 0x5b770031,
+	0x5b770030, 0x5b77002f, 0x5b77002d, 0x5b77002c,
+	0x5b470044, 0x5b470042, 0x5b470040, 0x5b47003e,
+	0x5b47003c, 0x5b47003b, 0x5b470039, 0x5b470037,
+	0x5b470036, 0x5b470034, 0x5b470033, 0x5b470031,
+	0x5b470030, 0x5b47002f, 0x5b47002d, 0x5b47002c,
+	0x5b47002b, 0x5b47002a, 0x5b270044, 0x5b270042,
+	0x5b270040, 0x5b27003e, 0x5b27003c, 0x5b27003b,
+	0x5b270039, 0x5b270037, 0x5b270036, 0x5b270034,
+	0x5b270033, 0x5b270031, 0x5b270030, 0x5b27002f,
+	0x5b170044, 0x5b170042, 0x5b170040, 0x5b17003e,
+	0x5b17003c, 0x5b17003b, 0x5b170039, 0x5b170037,
+	0x5b170036, 0x5b170034, 0x5b170033, 0x5b170031,
+	0x5b170030, 0x5b17002f, 0x5b17002d, 0x5b17002c,
+	0x5b17002b, 0x5b17002a, 0x5b170028, 0x5b170027,
+	0x5b170026, 0x5b170025, 0x5b170024, 0x5b170023,
+	0x5b070044, 0x5b070042, 0x5b070040, 0x5b07003e,
+	0x5b07003c, 0x5b07003b, 0x5b070039, 0x5b070037,
+	0x5b070036, 0x5b070034, 0x5b070033, 0x5b070031,
+	0x5b070030, 0x5b07002f, 0x5b07002d, 0x5b07002c,
+	0x5b07002b, 0x5b07002a, 0x5b070028, 0x5b070027,
+	0x5b070026, 0x5b070025, 0x5b070024, 0x5b070023,
+	0x5b070022, 0x5b070021, 0x5b070020, 0x5b07001f,
+	0x5b07001e, 0x5b07001d, 0x5b07001d, 0x5b07001c,
+};
+
+static const u32 b43_ntab_gainctl1_r3[] = {
+	0x5bf70044, 0x5bf70042, 0x5bf70040, 0x5bf7003e,
+	0x5bf7003c, 0x5bf7003b, 0x5bf70039, 0x5bf70037,
+	0x5bf70036, 0x5bf70034, 0x5bf70033, 0x5bf70031,
+	0x5bf70030, 0x5ba70044, 0x5ba70042, 0x5ba70040,
+	0x5ba7003e, 0x5ba7003c, 0x5ba7003b, 0x5ba70039,
+	0x5ba70037, 0x5ba70036, 0x5ba70034, 0x5ba70033,
+	0x5b770044, 0x5b770042, 0x5b770040, 0x5b77003e,
+	0x5b77003c, 0x5b77003b, 0x5b770039, 0x5b770037,
+	0x5b770036, 0x5b770034, 0x5b770033, 0x5b770031,
+	0x5b770030, 0x5b77002f, 0x5b77002d, 0x5b77002c,
+	0x5b470044, 0x5b470042, 0x5b470040, 0x5b47003e,
+	0x5b47003c, 0x5b47003b, 0x5b470039, 0x5b470037,
+	0x5b470036, 0x5b470034, 0x5b470033, 0x5b470031,
+	0x5b470030, 0x5b47002f, 0x5b47002d, 0x5b47002c,
+	0x5b47002b, 0x5b47002a, 0x5b270044, 0x5b270042,
+	0x5b270040, 0x5b27003e, 0x5b27003c, 0x5b27003b,
+	0x5b270039, 0x5b270037, 0x5b270036, 0x5b270034,
+	0x5b270033, 0x5b270031, 0x5b270030, 0x5b27002f,
+	0x5b170044, 0x5b170042, 0x5b170040, 0x5b17003e,
+	0x5b17003c, 0x5b17003b, 0x5b170039, 0x5b170037,
+	0x5b170036, 0x5b170034, 0x5b170033, 0x5b170031,
+	0x5b170030, 0x5b17002f, 0x5b17002d, 0x5b17002c,
+	0x5b17002b, 0x5b17002a, 0x5b170028, 0x5b170027,
+	0x5b170026, 0x5b170025, 0x5b170024, 0x5b170023,
+	0x5b070044, 0x5b070042, 0x5b070040, 0x5b07003e,
+	0x5b07003c, 0x5b07003b, 0x5b070039, 0x5b070037,
+	0x5b070036, 0x5b070034, 0x5b070033, 0x5b070031,
+	0x5b070030, 0x5b07002f, 0x5b07002d, 0x5b07002c,
+	0x5b07002b, 0x5b07002a, 0x5b070028, 0x5b070027,
+	0x5b070026, 0x5b070025, 0x5b070024, 0x5b070023,
+	0x5b070022, 0x5b070021, 0x5b070020, 0x5b07001f,
+	0x5b07001e, 0x5b07001d, 0x5b07001d, 0x5b07001c,
+};
+
+static const u32 b43_ntab_iqlt0_r3[] = {
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+static const u32 b43_ntab_iqlt1_r3[] = {
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+static const u16 b43_ntab_loftlt0_r3[] = {
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000,
+};
+
+static const u16 b43_ntab_loftlt1_r3[] = {
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
+	0x0000, 0x0000,
+};
+
+/* TX gain tables */
 const u32 b43_ntab_tx_gain_rev0_1_2[] = {
 	0x03cc2b44, 0x03cc2b42, 0x03cc2a44, 0x03cc2a42,
 	0x03cc2944, 0x03c82b44, 0x03c82b42, 0x03c82a44,
@@ -1635,6 +2709,79 @@ const struct nphy_rf_control_override_rev3 tbl_rf_control_override_rev3[] = {
 	{ 0x00C0,  6, 0xE7, 0xF9, 0xEC, 0xFB }  /* field == 0x4000 (fls 15) */
 };
 
+struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_workaround[2][3] = {
+	{ /* 2GHz */
+		{ /* PHY rev 3 */
+			{ 7, 11, 16, 23 },
+			{ -5, 6, 10, 14 },
+			{ 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA },
+			{ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
+			0x627E,
+			{ 0x613F, 0x613F, 0x613F, 0x613F },
+			0x107E, 0x0066, 0x0074,
+			0x18, 0x18, 0x18,
+			0x020D, 0x5,
+		},
+		{ /* PHY rev 4 */
+			{ 8, 12, 17, 25 },
+			{ -5, 6, 10, 14 },
+			{ 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA },
+			{ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
+			0x527E,
+			{ 0x513F, 0x513F, 0x513F, 0x513F },
+			0x007E, 0x0066, 0x0074,
+			0x18, 0x18, 0x18,
+			0x01A1, 0x5,
+		},
+		{ /* PHY rev 5+ */
+			{ 9, 13, 18, 26 },
+			{ -3, 7, 11, 16 },
+			{ 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA, 0xA },
+			{ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
+			0x427E, /* invalid for external LNA! */
+			{ 0x413F, 0x413F, 0x413F, 0x413F }, /* invalid for external LNA! */
+			0x1076, 0x0066, 0x106A,
+			0xC, 0xC, 0xC,
+			0x01D0, 0x5,
+		},
+	},
+	{ /* 5GHz */
+		{ /* PHY rev 3 */
+			{ 7, 11, 17, 23 },
+			{ -6, 2, 6, 10 },
+			{ 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13 },
+			{ 6, 6, 6, 6, 6, 6, 6, 6, 6, 6 },
+			0x52DE,
+			{ 0x516F, 0x516F, 0x516F, 0x516F },
+			0x00DE, 0x00CA, 0x00CC,
+			0x1E, 0x1E, 0x1E,
+			0x01A1, 25,
+		},
+		{ /* PHY rev 4 */
+			{ 8, 12, 18, 23 },
+			{ -5, 2, 6, 10 },
+			{ 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD },
+			{ 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 },
+			0x629E,
+			{ 0x614F, 0x614F, 0x614F, 0x614F },
+			0x029E, 0x1084, 0x0086,
+			0x24, 0x24, 0x24,
+			0x0107, 25,
+		},
+		{ /* PHY rev 5+ */
+			{ 6, 10, 16, 21 },
+			{ -7, 0, 4, 8 },
+			{ 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD, 0xD },
+			{ 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 },
+			0x729E,
+			{ 0x714F, 0x714F, 0x714F, 0x714F },
+			0x029E, 0x2084, 0x2086,
+			0x24, 0x24, 0x24,
+			0x00A9, 25,
+		},
+	},
+};
+
 static inline void assert_ntab_array_sizes(void)
 {
 #undef check
@@ -1813,7 +2960,6 @@ void b43_ntab_write_bulk(struct b43_wldev *dev, u32 offset,
 #define ntab_upload(dev, offset, data) do { \
 		b43_ntab_write_bulk(dev, offset, offset##_SIZE, data);	\
 	} while (0)
-
 void b43_nphy_rev0_1_2_tables_init(struct b43_wldev *dev)
 {
 	/* Static tables */
@@ -1847,11 +2993,70 @@ void b43_nphy_rev0_1_2_tables_init(struct b43_wldev *dev)
 	ntab_upload(dev, B43_NTAB_C1_LOFEEDTH, b43_ntab_loftlt1);
 }
 
+#define ntab_upload_r3(dev, offset, data) do { \
+		b43_ntab_write_bulk(dev, offset, ARRAY_SIZE(data), data); \
+	} while (0)
 void b43_nphy_rev3plus_tables_init(struct b43_wldev *dev)
 {
 	/* Static tables */
-	/* TODO */
+	ntab_upload_r3(dev, B43_NTAB_FRAMESTRUCT_R3, b43_ntab_framestruct_r3);
+	ntab_upload_r3(dev, B43_NTAB_PILOT_R3, b43_ntab_pilot_r3);
+	ntab_upload_r3(dev, B43_NTAB_TMAP_R3, b43_ntab_tmap_r3);
+	ntab_upload_r3(dev, B43_NTAB_INTLEVEL_R3, b43_ntab_intlevel_r3);
+	ntab_upload_r3(dev, B43_NTAB_TDTRN_R3, b43_ntab_tdtrn_r3);
+	ntab_upload_r3(dev, B43_NTAB_NOISEVAR0_R3, b43_ntab_noisevar0_r3);
+	ntab_upload_r3(dev, B43_NTAB_NOISEVAR1_R3, b43_ntab_noisevar1_r3);
+	ntab_upload_r3(dev, B43_NTAB_MCS_R3, b43_ntab_mcs_r3);
+	ntab_upload_r3(dev, B43_NTAB_TDI20A0_R3, b43_ntab_tdi20a0_r3);
+	ntab_upload_r3(dev, B43_NTAB_TDI20A1_R3, b43_ntab_tdi20a1_r3);
+	ntab_upload_r3(dev, B43_NTAB_TDI40A0_R3, b43_ntab_tdi40a0_r3);
+	ntab_upload_r3(dev, B43_NTAB_TDI40A1_R3, b43_ntab_tdi40a1_r3);
+	ntab_upload_r3(dev, B43_NTAB_PILOTLT_R3, b43_ntab_pilotlt_r3);
+	ntab_upload_r3(dev, B43_NTAB_CHANEST_R3, b43_ntab_channelest_r3);
+	ntab_upload_r3(dev, B43_NTAB_FRAMELT_R3, b43_ntab_framelookup_r3);
+	ntab_upload_r3(dev, B43_NTAB_C0_ESTPLT_R3,
+		       b43_ntab_estimatepowerlt0_r3);
+	ntab_upload_r3(dev, B43_NTAB_C1_ESTPLT_R3,
+		       b43_ntab_estimatepowerlt1_r3);
+	ntab_upload_r3(dev, B43_NTAB_C0_ADJPLT_R3, b43_ntab_adjustpower0_r3);
+	ntab_upload_r3(dev, B43_NTAB_C1_ADJPLT_R3, b43_ntab_adjustpower1_r3);
+	ntab_upload_r3(dev, B43_NTAB_C0_GAINCTL_R3, b43_ntab_gainctl0_r3);
+	ntab_upload_r3(dev, B43_NTAB_C1_GAINCTL_R3, b43_ntab_gainctl1_r3);
+	ntab_upload_r3(dev, B43_NTAB_C0_IQLT_R3, b43_ntab_iqlt0_r3);
+	ntab_upload_r3(dev, B43_NTAB_C1_IQLT_R3, b43_ntab_iqlt1_r3);
+	ntab_upload_r3(dev, B43_NTAB_C0_LOFEEDTH_R3, b43_ntab_loftlt0_r3);
+	ntab_upload_r3(dev, B43_NTAB_C1_LOFEEDTH_R3, b43_ntab_loftlt1_r3);
 
 	/* Volatile tables */
 	/* TODO */
 }
+
+struct nphy_gain_ctl_workaround_entry *b43_nphy_get_gain_ctl_workaround_ent(
+	struct b43_wldev *dev, bool ghz5, bool ext_lna)
+{
+	struct nphy_gain_ctl_workaround_entry *e;
+	u8 phy_idx;
+
+	B43_WARN_ON(dev->phy.rev < 3);
+	if (dev->phy.rev >= 5)
+		phy_idx = 2;
+	else if (dev->phy.rev == 4)
+		phy_idx = 1;
+	else
+		phy_idx = 0;
+
+	e = &nphy_gain_ctl_workaround[ghz5][phy_idx];
+
+	/* Only one entry differs for external LNA, so instead making whole
+	 * table 2 times bigger, hack is here
+	 */
+	if (!ghz5 && dev->phy.rev >= 5 && ext_lna) {
+		e->rfseq_init[0] &= 0x0FFF;
+		e->rfseq_init[1] &= 0x0FFF;
+		e->rfseq_init[2] &= 0x0FFF;
+		e->rfseq_init[3] &= 0x0FFF;
+		e->init_gain &= 0x0FFF;
+	}
+
+	return e;
+}
diff --git a/drivers/net/wireless/b43/tables_nphy.h b/drivers/net/wireless/b43/tables_nphy.h
index 4ec593ba3eef..18569367ce43 100644
--- a/drivers/net/wireless/b43/tables_nphy.h
+++ b/drivers/net/wireless/b43/tables_nphy.h
@@ -35,6 +35,31 @@ struct nphy_rf_control_override_rev3 {
 	u8 val_addr1;
 };
 
+struct nphy_gain_ctl_workaround_entry {
+	s8 lna1_gain[4];
+	s8 lna2_gain[4];
+	u8 gain_db[10];
+	u8 gain_bits[10];
+
+	u16 init_gain;
+	u16 rfseq_init[4];
+
+	u16 cliphi_gain;
+	u16 clipmd_gain;
+	u16 cliplo_gain;
+
+	u16 crsmin;
+	u16 crsminl;
+	u16 crsminu;
+
+	u16 nbclip;
+	u16 wlclip;
+};
+
+/* Get entry with workaround values for gain ctl. Does not return NULL. */
+struct nphy_gain_ctl_workaround_entry *b43_nphy_get_gain_ctl_workaround_ent(
+	struct b43_wldev *dev, bool ghz5, bool ext_lna);
+
 /* Get the NPHY Channel Switch Table entry for a channel.
  * Returns NULL on failure to find an entry. */
 const struct b43_nphy_channeltab_entry_rev2 *
@@ -109,6 +134,33 @@ b43_nphy_get_chantabent_rev3(struct b43_wldev *dev, u16 freq);
 #define B43_NTAB_C1_LOFEEDTH		B43_NTAB16(0x1B, 0x1C0) /* Local Oscillator Feed Through Lookup Table Core 1 */
 #define B43_NTAB_C1_LOFEEDTH_SIZE	128
 
+/* Static N-PHY tables, PHY revision >= 3 */
+#define B43_NTAB_FRAMESTRUCT_R3		B43_NTAB32(10, 000) /* frame struct  */
+#define B43_NTAB_PILOT_R3		B43_NTAB16(11, 000) /* pilot  */
+#define B43_NTAB_TMAP_R3		B43_NTAB32(12, 000) /* TM AP  */
+#define B43_NTAB_INTLEVEL_R3		B43_NTAB32(13, 000) /* INT LV  */
+#define B43_NTAB_TDTRN_R3		B43_NTAB32(14, 000) /* TD TRN  */
+#define B43_NTAB_NOISEVAR0_R3		B43_NTAB32(16, 000) /* noise variance 0  */
+#define B43_NTAB_NOISEVAR1_R3		B43_NTAB32(16, 128) /* noise variance 1  */
+#define B43_NTAB_MCS_R3			B43_NTAB16(18, 000) /* MCS  */
+#define B43_NTAB_TDI20A0_R3		B43_NTAB32(19, 128) /* TDI 20/0  */
+#define B43_NTAB_TDI20A1_R3		B43_NTAB32(19, 256) /* TDI 20/1  */
+#define B43_NTAB_TDI40A0_R3		B43_NTAB32(19, 640) /* TDI 40/0  */
+#define B43_NTAB_TDI40A1_R3		B43_NTAB32(19, 768) /* TDI 40/1  */
+#define B43_NTAB_PILOTLT_R3		B43_NTAB32(20, 000) /* PLT lookup  */
+#define B43_NTAB_CHANEST_R3		B43_NTAB32(22, 000) /* channel estimate  */
+#define B43_NTAB_FRAMELT_R3		B43_NTAB8 (24, 000) /* frame lookup  */
+#define B43_NTAB_C0_ESTPLT_R3		B43_NTAB8 (26, 000) /* estimated power lookup 0  */
+#define B43_NTAB_C1_ESTPLT_R3		B43_NTAB8 (27, 000) /* estimated power lookup 1  */
+#define B43_NTAB_C0_ADJPLT_R3		B43_NTAB8 (26, 064) /* adjusted power lookup 0  */
+#define B43_NTAB_C1_ADJPLT_R3		B43_NTAB8 (27, 064) /* adjusted power lookup 1  */
+#define B43_NTAB_C0_GAINCTL_R3		B43_NTAB32(26, 192) /* gain control lookup 0  */
+#define B43_NTAB_C1_GAINCTL_R3		B43_NTAB32(27, 192) /* gain control lookup 1  */
+#define B43_NTAB_C0_IQLT_R3		B43_NTAB32(26, 320) /* I/Q lookup 0  */
+#define B43_NTAB_C1_IQLT_R3		B43_NTAB32(27, 320) /* I/Q lookup 1  */
+#define B43_NTAB_C0_LOFEEDTH_R3		B43_NTAB16(26, 448) /* Local Oscillator Feed Through lookup 0  */
+#define B43_NTAB_C1_LOFEEDTH_R3		B43_NTAB16(27, 448) /* Local Oscillator Feed Through lookup 1 */
+
 #define B43_NTAB_TX_IQLO_CAL_LOFT_LADDER_40_SIZE	18
 #define B43_NTAB_TX_IQLO_CAL_LOFT_LADDER_20_SIZE	18
 #define B43_NTAB_TX_IQLO_CAL_IQIMB_LADDER_40_SIZE	18
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index e6b0528f3b52..e5be381c17bc 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -32,6 +32,36 @@
 #include "dma.h"
 #include "pio.h"
 
+static const struct b43_tx_legacy_rate_phy_ctl_entry b43_tx_legacy_rate_phy_ctl[] = {
+	{ B43_CCK_RATE_1MB,	0x0,			0x0 },
+	{ B43_CCK_RATE_2MB,	0x0,			0x1 },
+	{ B43_CCK_RATE_5MB,	0x0,			0x2 },
+	{ B43_CCK_RATE_11MB,	0x0,			0x3 },
+	{ B43_OFDM_RATE_6MB,	B43_TXH_PHY1_CRATE_1_2,	B43_TXH_PHY1_MODUL_BPSK },
+	{ B43_OFDM_RATE_9MB,	B43_TXH_PHY1_CRATE_3_4,	B43_TXH_PHY1_MODUL_BPSK },
+	{ B43_OFDM_RATE_12MB,	B43_TXH_PHY1_CRATE_1_2,	B43_TXH_PHY1_MODUL_QPSK },
+	{ B43_OFDM_RATE_18MB,	B43_TXH_PHY1_CRATE_3_4,	B43_TXH_PHY1_MODUL_QPSK },
+	{ B43_OFDM_RATE_24MB,	B43_TXH_PHY1_CRATE_1_2,	B43_TXH_PHY1_MODUL_QAM16 },
+	{ B43_OFDM_RATE_36MB,	B43_TXH_PHY1_CRATE_3_4,	B43_TXH_PHY1_MODUL_QAM16 },
+	{ B43_OFDM_RATE_48MB,	B43_TXH_PHY1_CRATE_2_3,	B43_TXH_PHY1_MODUL_QAM64 },
+	{ B43_OFDM_RATE_54MB,	B43_TXH_PHY1_CRATE_3_4,	B43_TXH_PHY1_MODUL_QAM64 },
+};
+
+static const struct b43_tx_legacy_rate_phy_ctl_entry *
+b43_tx_legacy_rate_phy_ctl_ent(u8 bitrate)
+{
+	const struct b43_tx_legacy_rate_phy_ctl_entry *e;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(b43_tx_legacy_rate_phy_ctl); i++) {
+		e = &(b43_tx_legacy_rate_phy_ctl[i]);
+		if (e->bitrate == bitrate)
+			return e;
+	}
+
+	B43_WARN_ON(1);
+	return NULL;
+}
 
 /* Extract the bitrate index out of a CCK PLCP header. */
 static int b43_plcp_get_bitrate_idx_cck(struct b43_plcp_hdr6 *plcp)
@@ -145,6 +175,34 @@ void b43_generate_plcp_hdr(struct b43_plcp_hdr4 *plcp,
 	}
 }
 
+static u16 b43_generate_tx_phy_ctl1(struct b43_wldev *dev, u8 bitrate)
+{
+	const struct b43_phy *phy = &dev->phy;
+	const struct b43_tx_legacy_rate_phy_ctl_entry *e;
+	u16 control = 0;
+	u16 bw;
+
+	if (phy->type == B43_PHYTYPE_LP)
+		bw = B43_TXH_PHY1_BW_20;
+	else /* FIXME */
+		bw = B43_TXH_PHY1_BW_20;
+
+	if (0) { /* FIXME: MIMO */
+	} else if (b43_is_cck_rate(bitrate) && phy->type != B43_PHYTYPE_LP) {
+		control = bw;
+	} else {
+		control = bw;
+		e = b43_tx_legacy_rate_phy_ctl_ent(bitrate);
+		if (e) {
+			control |= e->coding_rate;
+			control |= e->modulation;
+		}
+		control |= B43_TXH_PHY1_MODE_SISO;
+	}
+
+	return control;
+}
+
 static u8 b43_calc_fallback_rate(u8 bitrate)
 {
 	switch (bitrate) {
@@ -437,6 +495,14 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 			extra_ft |= B43_TXH_EFT_RTSFB_OFDM;
 		else
 			extra_ft |= B43_TXH_EFT_RTSFB_CCK;
+
+		if (rates[0].flags & IEEE80211_TX_RC_USE_RTS_CTS &&
+		    phy->type == B43_PHYTYPE_N) {
+			txhdr->phy_ctl1_rts = cpu_to_le16(
+				b43_generate_tx_phy_ctl1(dev, rts_rate));
+			txhdr->phy_ctl1_rts_fb = cpu_to_le16(
+				b43_generate_tx_phy_ctl1(dev, rts_rate_fb));
+		}
 	}
 
 	/* Magic cookie */
@@ -445,6 +511,13 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 	else
 		txhdr->new_format.cookie = cpu_to_le16(cookie);
 
+	if (phy->type == B43_PHYTYPE_N) {
+		txhdr->phy_ctl1 =
+			cpu_to_le16(b43_generate_tx_phy_ctl1(dev, rate));
+		txhdr->phy_ctl1_fb =
+			cpu_to_le16(b43_generate_tx_phy_ctl1(dev, rate_fb));
+	}
+
 	/* Apply the bitfields */
 	txhdr->mac_ctl = cpu_to_le32(mac_ctl);
 	txhdr->phy_ctl = cpu_to_le16(phy_ctl);
@@ -652,7 +725,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 		status.mactime += mactime;
 		if (low_mactime_now <= mactime)
 			status.mactime -= 0x10000;
-		status.flag |= RX_FLAG_TSFT;
+		status.flag |= RX_FLAG_MACTIME_MPDU;
 	}
 
 	chanid = (chanstat & B43_RX_CHAN_ID) >> B43_RX_CHAN_ID_SHIFT;
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/b43/xmit.h
index d4cf9b390af3..42debb5cd6fa 100644
--- a/drivers/net/wireless/b43/xmit.h
+++ b/drivers/net/wireless/b43/xmit.h
@@ -73,6 +73,12 @@ struct b43_txhdr {
 	} __packed;
 } __packed;
 
+struct b43_tx_legacy_rate_phy_ctl_entry {
+	u8 bitrate;
+	u16 coding_rate;
+	u16 modulation;
+};
+
 /* MAC TX control */
 #define B43_TXH_MAC_USEFBR		0x10000000 /* Use fallback rate for this AMPDU */
 #define B43_TXH_MAC_KEYIDX		0x0FF00000 /* Security key index */
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 1f11e1670bf0..c7fd73e3ad76 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -2442,8 +2442,8 @@ static int b43legacy_rng_init(struct b43legacy_wl *wl)
 	return err;
 }
 
-static int b43legacy_op_tx(struct ieee80211_hw *hw,
-			   struct sk_buff *skb)
+static void b43legacy_op_tx(struct ieee80211_hw *hw,
+			    struct sk_buff *skb)
 {
 	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
 	struct b43legacy_wldev *dev = wl->current_dev;
@@ -2466,7 +2466,6 @@ out:
 		/* Drop the packet. */
 		dev_kfree_skb_any(skb);
 	}
-	return NETDEV_TX_OK;
 }
 
 static int b43legacy_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
index 7d177d97f1f7..3a95541708a6 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -572,7 +572,7 @@ void b43legacy_rx(struct b43legacy_wldev *dev,
 		status.mactime += mactime;
 		if (low_mactime_now <= mactime)
 			status.mactime -= 0x10000;
-		status.flag |= RX_FLAG_TSFT;
+		status.flag |= RX_FLAG_MACTIME_MPDU;
 	}
 
 	chanid = (chanstat & B43legacy_RX_CHAN_ID) >>
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index 61915f371416..4b97f918daff 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -706,11 +706,10 @@ static void schedule_reset(struct ipw2100_priv *priv)
 		netif_stop_queue(priv->net_dev);
 		priv->status |= STATUS_RESET_PENDING;
 		if (priv->reset_backoff)
-			queue_delayed_work(priv->workqueue, &priv->reset_work,
-					   priv->reset_backoff * HZ);
+			schedule_delayed_work(&priv->reset_work,
+					      priv->reset_backoff * HZ);
 		else
-			queue_delayed_work(priv->workqueue, &priv->reset_work,
-					   0);
+			schedule_delayed_work(&priv->reset_work, 0);
 
 		if (priv->reset_backoff < MAX_RESET_BACKOFF)
 			priv->reset_backoff++;
@@ -1397,7 +1396,7 @@ static int ipw2100_power_cycle_adapter(struct ipw2100_priv *priv)
 }
 
 /*
- * Send the CARD_DISABLE_PHY_OFF comamnd to the card to disable it
+ * Send the CARD_DISABLE_PHY_OFF command to the card to disable it
  *
  * After disabling, if the card was associated, a STATUS_ASSN_LOST will be sent.
  *
@@ -1474,7 +1473,7 @@ static int ipw2100_enable_adapter(struct ipw2100_priv *priv)
 
 	if (priv->stop_hang_check) {
 		priv->stop_hang_check = 0;
-		queue_delayed_work(priv->workqueue, &priv->hang_check, HZ / 2);
+		schedule_delayed_work(&priv->hang_check, HZ / 2);
 	}
 
       fail_up:
@@ -1808,8 +1807,8 @@ static int ipw2100_up(struct ipw2100_priv *priv, int deferred)
 
 		if (priv->stop_rf_kill) {
 			priv->stop_rf_kill = 0;
-			queue_delayed_work(priv->workqueue, &priv->rf_kill,
-					   round_jiffies_relative(HZ));
+			schedule_delayed_work(&priv->rf_kill,
+					      round_jiffies_relative(HZ));
 		}
 
 		deferred = 1;
@@ -2086,7 +2085,7 @@ static void isr_indicate_associated(struct ipw2100_priv *priv, u32 status)
 	priv->status |= STATUS_ASSOCIATING;
 	priv->connect_start = get_seconds();
 
-	queue_delayed_work(priv->workqueue, &priv->wx_event_work, HZ / 10);
+	schedule_delayed_work(&priv->wx_event_work, HZ / 10);
 }
 
 static int ipw2100_set_essid(struct ipw2100_priv *priv, char *essid,
@@ -2166,9 +2165,9 @@ static void isr_indicate_association_lost(struct ipw2100_priv *priv, u32 status)
 		return;
 
 	if (priv->status & STATUS_SECURITY_UPDATED)
-		queue_delayed_work(priv->workqueue, &priv->security_work, 0);
+		schedule_delayed_work(&priv->security_work, 0);
 
-	queue_delayed_work(priv->workqueue, &priv->wx_event_work, 0);
+	schedule_delayed_work(&priv->wx_event_work, 0);
 }
 
 static void isr_indicate_rf_kill(struct ipw2100_priv *priv, u32 status)
@@ -2183,8 +2182,7 @@ static void isr_indicate_rf_kill(struct ipw2100_priv *priv, u32 status)
 	/* Make sure the RF Kill check timer is running */
 	priv->stop_rf_kill = 0;
 	cancel_delayed_work(&priv->rf_kill);
-	queue_delayed_work(priv->workqueue, &priv->rf_kill,
-			   round_jiffies_relative(HZ));
+	schedule_delayed_work(&priv->rf_kill, round_jiffies_relative(HZ));
 }
 
 static void send_scan_event(void *data)
@@ -2219,13 +2217,12 @@ static void isr_scan_complete(struct ipw2100_priv *priv, u32 status)
 	/* Only userspace-requested scan completion events go out immediately */
 	if (!priv->user_requested_scan) {
 		if (!delayed_work_pending(&priv->scan_event_later))
-			queue_delayed_work(priv->workqueue,
-					&priv->scan_event_later,
-					round_jiffies_relative(msecs_to_jiffies(4000)));
+			schedule_delayed_work(&priv->scan_event_later,
+					      round_jiffies_relative(msecs_to_jiffies(4000)));
 	} else {
 		priv->user_requested_scan = 0;
 		cancel_delayed_work(&priv->scan_event_later);
-		queue_work(priv->workqueue, &priv->scan_event_now);
+		schedule_work(&priv->scan_event_now);
 	}
 }
 
@@ -4329,8 +4326,8 @@ static int ipw_radio_kill_sw(struct ipw2100_priv *priv, int disable_radio)
 			/* Make sure the RF_KILL check timer is running */
 			priv->stop_rf_kill = 0;
 			cancel_delayed_work(&priv->rf_kill);
-			queue_delayed_work(priv->workqueue, &priv->rf_kill,
-					   round_jiffies_relative(HZ));
+			schedule_delayed_work(&priv->rf_kill,
+					      round_jiffies_relative(HZ));
 		} else
 			schedule_reset(priv);
 	}
@@ -4461,20 +4458,17 @@ static void bd_queue_initialize(struct ipw2100_priv *priv,
 	IPW_DEBUG_INFO("exit\n");
 }
 
-static void ipw2100_kill_workqueue(struct ipw2100_priv *priv)
+static void ipw2100_kill_works(struct ipw2100_priv *priv)
 {
-	if (priv->workqueue) {
-		priv->stop_rf_kill = 1;
-		priv->stop_hang_check = 1;
-		cancel_delayed_work(&priv->reset_work);
-		cancel_delayed_work(&priv->security_work);
-		cancel_delayed_work(&priv->wx_event_work);
-		cancel_delayed_work(&priv->hang_check);
-		cancel_delayed_work(&priv->rf_kill);
-		cancel_delayed_work(&priv->scan_event_later);
-		destroy_workqueue(priv->workqueue);
-		priv->workqueue = NULL;
-	}
+	priv->stop_rf_kill = 1;
+	priv->stop_hang_check = 1;
+	cancel_delayed_work_sync(&priv->reset_work);
+	cancel_delayed_work_sync(&priv->security_work);
+	cancel_delayed_work_sync(&priv->wx_event_work);
+	cancel_delayed_work_sync(&priv->hang_check);
+	cancel_delayed_work_sync(&priv->rf_kill);
+	cancel_work_sync(&priv->scan_event_now);
+	cancel_delayed_work_sync(&priv->scan_event_later);
 }
 
 static int ipw2100_tx_allocate(struct ipw2100_priv *priv)
@@ -6046,7 +6040,7 @@ static void ipw2100_hang_check(struct work_struct *work)
 	priv->last_rtc = rtc;
 
 	if (!priv->stop_hang_check)
-		queue_delayed_work(priv->workqueue, &priv->hang_check, HZ / 2);
+		schedule_delayed_work(&priv->hang_check, HZ / 2);
 
 	spin_unlock_irqrestore(&priv->low_lock, flags);
 }
@@ -6062,8 +6056,8 @@ static void ipw2100_rf_kill(struct work_struct *work)
 	if (rf_kill_active(priv)) {
 		IPW_DEBUG_RF_KILL("RF Kill active, rescheduling GPIO check\n");
 		if (!priv->stop_rf_kill)
-			queue_delayed_work(priv->workqueue, &priv->rf_kill,
-					   round_jiffies_relative(HZ));
+			schedule_delayed_work(&priv->rf_kill,
+					      round_jiffies_relative(HZ));
 		goto exit_unlock;
 	}
 
@@ -6209,8 +6203,6 @@ static struct net_device *ipw2100_alloc_device(struct pci_dev *pci_dev,
 	INIT_LIST_HEAD(&priv->fw_pend_list);
 	INIT_STAT(&priv->fw_pend_stat);
 
-	priv->workqueue = create_workqueue(DRV_NAME);
-
 	INIT_DELAYED_WORK(&priv->reset_work, ipw2100_reset_adapter);
 	INIT_DELAYED_WORK(&priv->security_work, ipw2100_security_work);
 	INIT_DELAYED_WORK(&priv->wx_event_work, ipw2100_wx_event_work);
@@ -6410,7 +6402,7 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 		if (dev->irq)
 			free_irq(dev->irq, priv);
 
-		ipw2100_kill_workqueue(priv);
+		ipw2100_kill_works(priv);
 
 		/* These are safe to call even if they weren't allocated */
 		ipw2100_queues_free(priv);
@@ -6460,9 +6452,7 @@ static void __devexit ipw2100_pci_remove_one(struct pci_dev *pci_dev)
 		 * first, then close() will crash. */
 		unregister_netdev(dev);
 
-		/* ipw2100_down will ensure that there is no more pending work
-		 * in the workqueue's, so we can safely remove them now. */
-		ipw2100_kill_workqueue(priv);
+		ipw2100_kill_works(priv);
 
 		ipw2100_queues_free(priv);
 
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.h b/drivers/net/wireless/ipw2x00/ipw2100.h
index 838002b4881e..99cba968aa58 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.h
+++ b/drivers/net/wireless/ipw2x00/ipw2100.h
@@ -580,7 +580,6 @@ struct ipw2100_priv {
 
 	struct tasklet_struct irq_tasklet;
 
-	struct workqueue_struct *workqueue;
 	struct delayed_work reset_work;
 	struct delayed_work security_work;
 	struct delayed_work wx_event_work;
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index ae438ed80c2f..160881f234cc 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -894,9 +894,8 @@ static void ipw_led_link_on(struct ipw_priv *priv)
 
 		/* If we aren't associated, schedule turning the LED off */
 		if (!(priv->status & STATUS_ASSOCIATED))
-			queue_delayed_work(priv->workqueue,
-					   &priv->led_link_off,
-					   LD_TIME_LINK_ON);
+			schedule_delayed_work(&priv->led_link_off,
+					      LD_TIME_LINK_ON);
 	}
 
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -939,8 +938,8 @@ static void ipw_led_link_off(struct ipw_priv *priv)
 		 * turning the LED on (blink while unassociated) */
 		if (!(priv->status & STATUS_RF_KILL_MASK) &&
 		    !(priv->status & STATUS_ASSOCIATED))
-			queue_delayed_work(priv->workqueue, &priv->led_link_on,
-					   LD_TIME_LINK_OFF);
+			schedule_delayed_work(&priv->led_link_on,
+					      LD_TIME_LINK_OFF);
 
 	}
 
@@ -980,13 +979,11 @@ static void __ipw_led_activity_on(struct ipw_priv *priv)
 		priv->status |= STATUS_LED_ACT_ON;
 
 		cancel_delayed_work(&priv->led_act_off);
-		queue_delayed_work(priv->workqueue, &priv->led_act_off,
-				   LD_TIME_ACT_ON);
+		schedule_delayed_work(&priv->led_act_off, LD_TIME_ACT_ON);
 	} else {
 		/* Reschedule LED off for full time period */
 		cancel_delayed_work(&priv->led_act_off);
-		queue_delayed_work(priv->workqueue, &priv->led_act_off,
-				   LD_TIME_ACT_ON);
+		schedule_delayed_work(&priv->led_act_off, LD_TIME_ACT_ON);
 	}
 }
 
@@ -1795,13 +1792,11 @@ static int ipw_radio_kill_sw(struct ipw_priv *priv, int disable_radio)
 	if (disable_radio) {
 		priv->status |= STATUS_RF_KILL_SW;
 
-		if (priv->workqueue) {
-			cancel_delayed_work(&priv->request_scan);
-			cancel_delayed_work(&priv->request_direct_scan);
-			cancel_delayed_work(&priv->request_passive_scan);
-			cancel_delayed_work(&priv->scan_event);
-		}
-		queue_work(priv->workqueue, &priv->down);
+		cancel_delayed_work(&priv->request_scan);
+		cancel_delayed_work(&priv->request_direct_scan);
+		cancel_delayed_work(&priv->request_passive_scan);
+		cancel_delayed_work(&priv->scan_event);
+		schedule_work(&priv->down);
 	} else {
 		priv->status &= ~STATUS_RF_KILL_SW;
 		if (rf_kill_active(priv)) {
@@ -1809,10 +1804,10 @@ static int ipw_radio_kill_sw(struct ipw_priv *priv, int disable_radio)
 					  "disabled by HW switch\n");
 			/* Make sure the RF_KILL check timer is running */
 			cancel_delayed_work(&priv->rf_kill);
-			queue_delayed_work(priv->workqueue, &priv->rf_kill,
-					   round_jiffies_relative(2 * HZ));
+			schedule_delayed_work(&priv->rf_kill,
+					      round_jiffies_relative(2 * HZ));
 		} else
-			queue_work(priv->workqueue, &priv->up);
+			schedule_work(&priv->up);
 	}
 
 	return 1;
@@ -2063,7 +2058,7 @@ static void ipw_irq_tasklet(struct ipw_priv *priv)
 		cancel_delayed_work(&priv->request_passive_scan);
 		cancel_delayed_work(&priv->scan_event);
 		schedule_work(&priv->link_down);
-		queue_delayed_work(priv->workqueue, &priv->rf_kill, 2 * HZ);
+		schedule_delayed_work(&priv->rf_kill, 2 * HZ);
 		handled |= IPW_INTA_BIT_RF_KILL_DONE;
 	}
 
@@ -2103,7 +2098,7 @@ static void ipw_irq_tasklet(struct ipw_priv *priv)
 		priv->status &= ~STATUS_HCMD_ACTIVE;
 		wake_up_interruptible(&priv->wait_command_queue);
 
-		queue_work(priv->workqueue, &priv->adapter_restart);
+		schedule_work(&priv->adapter_restart);
 		handled |= IPW_INTA_BIT_FATAL_ERROR;
 	}
 
@@ -2323,11 +2318,6 @@ static int ipw_send_adapter_address(struct ipw_priv *priv, u8 * mac)
 	return ipw_send_cmd_pdu(priv, IPW_CMD_ADAPTER_ADDRESS, ETH_ALEN, mac);
 }
 
-/*
- * NOTE: This must be executed from our workqueue as it results in udelay
- * being called which may corrupt the keyboard if executed on default
- * workqueue
- */
 static void ipw_adapter_restart(void *adapter)
 {
 	struct ipw_priv *priv = adapter;
@@ -2368,13 +2358,13 @@ static void ipw_scan_check(void *data)
 		IPW_DEBUG_SCAN("Scan completion watchdog resetting "
 			       "adapter after (%dms).\n",
 			       jiffies_to_msecs(IPW_SCAN_CHECK_WATCHDOG));
-		queue_work(priv->workqueue, &priv->adapter_restart);
+		schedule_work(&priv->adapter_restart);
 	} else if (priv->status & STATUS_SCANNING) {
 		IPW_DEBUG_SCAN("Scan completion watchdog aborting scan "
 			       "after (%dms).\n",
 			       jiffies_to_msecs(IPW_SCAN_CHECK_WATCHDOG));
 		ipw_abort_scan(priv);
-		queue_delayed_work(priv->workqueue, &priv->scan_check, HZ);
+		schedule_delayed_work(&priv->scan_check, HZ);
 	}
 }
 
@@ -3943,7 +3933,7 @@ static void ipw_send_disassociate(struct ipw_priv *priv, int quiet)
 
 	if (priv->status & STATUS_ASSOCIATING) {
 		IPW_DEBUG_ASSOC("Disassociating while associating.\n");
-		queue_work(priv->workqueue, &priv->disassociate);
+		schedule_work(&priv->disassociate);
 		return;
 	}
 
@@ -4360,8 +4350,7 @@ static void ipw_gather_stats(struct ipw_priv *priv)
 
 	priv->quality = quality;
 
-	queue_delayed_work(priv->workqueue, &priv->gather_stats,
-			   IPW_STATS_INTERVAL);
+	schedule_delayed_work(&priv->gather_stats, IPW_STATS_INTERVAL);
 }
 
 static void ipw_bg_gather_stats(struct work_struct *work)
@@ -4396,10 +4385,10 @@ static void ipw_handle_missed_beacon(struct ipw_priv *priv,
 			IPW_DEBUG(IPW_DL_INFO | IPW_DL_NOTIF |
 				  IPW_DL_STATE,
 				  "Aborting scan with missed beacon.\n");
-			queue_work(priv->workqueue, &priv->abort_scan);
+			schedule_work(&priv->abort_scan);
 		}
 
-		queue_work(priv->workqueue, &priv->disassociate);
+		schedule_work(&priv->disassociate);
 		return;
 	}
 
@@ -4425,8 +4414,7 @@ static void ipw_handle_missed_beacon(struct ipw_priv *priv,
 		if (!(priv->status & STATUS_ROAMING)) {
 			priv->status |= STATUS_ROAMING;
 			if (!(priv->status & STATUS_SCANNING))
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_scan, 0);
+				schedule_delayed_work(&priv->request_scan, 0);
 		}
 		return;
 	}
@@ -4439,7 +4427,7 @@ static void ipw_handle_missed_beacon(struct ipw_priv *priv,
 		 * channels..) */
 		IPW_DEBUG(IPW_DL_INFO | IPW_DL_NOTIF | IPW_DL_STATE,
 			  "Aborting scan with missed beacon.\n");
-		queue_work(priv->workqueue, &priv->abort_scan);
+		schedule_work(&priv->abort_scan);
 	}
 
 	IPW_DEBUG_NOTIF("Missed beacon: %d\n", missed_count);
@@ -4462,8 +4450,8 @@ static void handle_scan_event(struct ipw_priv *priv)
 	/* Only userspace-requested scan completion events go out immediately */
 	if (!priv->user_requested_scan) {
 		if (!delayed_work_pending(&priv->scan_event))
-			queue_delayed_work(priv->workqueue, &priv->scan_event,
-					 round_jiffies_relative(msecs_to_jiffies(4000)));
+			schedule_delayed_work(&priv->scan_event,
+					      round_jiffies_relative(msecs_to_jiffies(4000)));
 	} else {
 		union iwreq_data wrqu;
 
@@ -4516,20 +4504,17 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 
 						IPW_DEBUG_ASSOC
 						    ("queueing adhoc check\n");
-						queue_delayed_work(priv->
-								   workqueue,
-								   &priv->
-								   adhoc_check,
-								   le16_to_cpu(priv->
-								   assoc_request.
-								   beacon_interval));
+						schedule_delayed_work(
+							&priv->adhoc_check,
+							le16_to_cpu(priv->
+							assoc_request.
+							beacon_interval));
 						break;
 					}
 
 					priv->status &= ~STATUS_ASSOCIATING;
 					priv->status |= STATUS_ASSOCIATED;
-					queue_work(priv->workqueue,
-						   &priv->system_config);
+					schedule_work(&priv->system_config);
 
 #ifdef CONFIG_IPW2200_QOS
 #define IPW_GET_PACKET_STYPE(x) WLAN_FC_GET_STYPE( \
@@ -4792,43 +4777,37 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 #ifdef CONFIG_IPW2200_MONITOR
 			if (priv->ieee->iw_mode == IW_MODE_MONITOR) {
 				priv->status |= STATUS_SCAN_FORCED;
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_scan, 0);
+				schedule_delayed_work(&priv->request_scan, 0);
 				break;
 			}
 			priv->status &= ~STATUS_SCAN_FORCED;
 #endif				/* CONFIG_IPW2200_MONITOR */
 
 			/* Do queued direct scans first */
-			if (priv->status & STATUS_DIRECT_SCAN_PENDING) {
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_direct_scan, 0);
-			}
+			if (priv->status & STATUS_DIRECT_SCAN_PENDING)
+				schedule_delayed_work(&priv->request_direct_scan, 0);
 
 			if (!(priv->status & (STATUS_ASSOCIATED |
 					      STATUS_ASSOCIATING |
 					      STATUS_ROAMING |
 					      STATUS_DISASSOCIATING)))
-				queue_work(priv->workqueue, &priv->associate);
+				schedule_work(&priv->associate);
 			else if (priv->status & STATUS_ROAMING) {
 				if (x->status == SCAN_COMPLETED_STATUS_COMPLETE)
 					/* If a scan completed and we are in roam mode, then
 					 * the scan that completed was the one requested as a
 					 * result of entering roam... so, schedule the
 					 * roam work */
-					queue_work(priv->workqueue,
-						   &priv->roam);
+					schedule_work(&priv->roam);
 				else
 					/* Don't schedule if we aborted the scan */
 					priv->status &= ~STATUS_ROAMING;
 			} else if (priv->status & STATUS_SCAN_PENDING)
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_scan, 0);
+				schedule_delayed_work(&priv->request_scan, 0);
 			else if (priv->config & CFG_BACKGROUND_SCAN
 				 && priv->status & STATUS_ASSOCIATED)
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_scan,
-						   round_jiffies_relative(HZ));
+				schedule_delayed_work(&priv->request_scan,
+						      round_jiffies_relative(HZ));
 
 			/* Send an empty event to user space.
 			 * We don't send the received data on the event because
@@ -5192,7 +5171,7 @@ static void ipw_rx_queue_restock(struct ipw_priv *priv)
 	/* If the pre-allocated buffer pool is dropping low, schedule to
 	 * refill it */
 	if (rxq->free_count <= RX_LOW_WATERMARK)
-		queue_work(priv->workqueue, &priv->rx_replenish);
+		schedule_work(&priv->rx_replenish);
 
 	/* If we've added more space for the firmware to place data, tell it */
 	if (write != rxq->write)
@@ -6133,8 +6112,8 @@ static void ipw_adhoc_check(void *data)
 		return;
 	}
 
-	queue_delayed_work(priv->workqueue, &priv->adhoc_check,
-			   le16_to_cpu(priv->assoc_request.beacon_interval));
+	schedule_delayed_work(&priv->adhoc_check,
+			      le16_to_cpu(priv->assoc_request.beacon_interval));
 }
 
 static void ipw_bg_adhoc_check(struct work_struct *work)
@@ -6523,8 +6502,7 @@ send_request:
 	} else
 		priv->status &= ~STATUS_SCAN_PENDING;
 
-	queue_delayed_work(priv->workqueue, &priv->scan_check,
-			   IPW_SCAN_CHECK_WATCHDOG);
+	schedule_delayed_work(&priv->scan_check, IPW_SCAN_CHECK_WATCHDOG);
 done:
 	mutex_unlock(&priv->mutex);
 	return err;
@@ -6994,8 +6972,7 @@ static int ipw_qos_handle_probe_response(struct ipw_priv *priv,
 				    !memcmp(network->ssid,
 					    priv->assoc_network->ssid,
 					    network->ssid_len)) {
-					queue_work(priv->workqueue,
-						   &priv->merge_networks);
+					schedule_work(&priv->merge_networks);
 				}
 	}
 
@@ -7663,7 +7640,7 @@ static int ipw_associate(void *data)
 	if (priv->status & STATUS_DISASSOCIATING) {
 		IPW_DEBUG_ASSOC("Not attempting association (in "
 				"disassociating)\n ");
-		queue_work(priv->workqueue, &priv->associate);
+		schedule_work(&priv->associate);
 		return 0;
 	}
 
@@ -7731,12 +7708,10 @@ static int ipw_associate(void *data)
 
 		if (!(priv->status & STATUS_SCANNING)) {
 			if (!(priv->config & CFG_SPEED_SCAN))
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_scan,
-						   SCAN_INTERVAL);
+				schedule_delayed_work(&priv->request_scan,
+						      SCAN_INTERVAL);
 			else
-				queue_delayed_work(priv->workqueue,
-						   &priv->request_scan, 0);
+				schedule_delayed_work(&priv->request_scan, 0);
 		}
 
 		return 0;
@@ -8899,7 +8874,7 @@ static int ipw_wx_set_mode(struct net_device *dev,
 
 	priv->ieee->iw_mode = wrqu->mode;
 
-	queue_work(priv->workqueue, &priv->adapter_restart);
+	schedule_work(&priv->adapter_restart);
 	mutex_unlock(&priv->mutex);
 	return err;
 }
@@ -9598,7 +9573,7 @@ static int ipw_wx_set_scan(struct net_device *dev,
 
 	IPW_DEBUG_WX("Start scan\n");
 
-	queue_delayed_work(priv->workqueue, work, 0);
+	schedule_delayed_work(work, 0);
 
 	return 0;
 }
@@ -9937,7 +9912,7 @@ static int ipw_wx_set_monitor(struct net_device *dev,
 #else
 			priv->net_dev->type = ARPHRD_IEEE80211;
 #endif
-			queue_work(priv->workqueue, &priv->adapter_restart);
+			schedule_work(&priv->adapter_restart);
 		}
 
 		ipw_set_channel(priv, parms[1]);
@@ -9947,7 +9922,7 @@ static int ipw_wx_set_monitor(struct net_device *dev,
 			return 0;
 		}
 		priv->net_dev->type = ARPHRD_ETHER;
-		queue_work(priv->workqueue, &priv->adapter_restart);
+		schedule_work(&priv->adapter_restart);
 	}
 	mutex_unlock(&priv->mutex);
 	return 0;
@@ -9961,7 +9936,7 @@ static int ipw_wx_reset(struct net_device *dev,
 {
 	struct ipw_priv *priv = libipw_priv(dev);
 	IPW_DEBUG_WX("RESET\n");
-	queue_work(priv->workqueue, &priv->adapter_restart);
+	schedule_work(&priv->adapter_restart);
 	return 0;
 }
 
@@ -10551,7 +10526,7 @@ static int ipw_net_set_mac_address(struct net_device *dev, void *p)
 	memcpy(priv->mac_addr, addr->sa_data, ETH_ALEN);
 	printk(KERN_INFO "%s: Setting MAC to %pM\n",
 	       priv->net_dev->name, priv->mac_addr);
-	queue_work(priv->workqueue, &priv->adapter_restart);
+	schedule_work(&priv->adapter_restart);
 	mutex_unlock(&priv->mutex);
 	return 0;
 }
@@ -10684,9 +10659,7 @@ static void ipw_rf_kill(void *adapter)
 
 	if (rf_kill_active(priv)) {
 		IPW_DEBUG_RF_KILL("RF Kill active, rescheduling GPIO check\n");
-		if (priv->workqueue)
-			queue_delayed_work(priv->workqueue,
-					   &priv->rf_kill, 2 * HZ);
+		schedule_delayed_work(&priv->rf_kill, 2 * HZ);
 		goto exit_unlock;
 	}
 
@@ -10697,7 +10670,7 @@ static void ipw_rf_kill(void *adapter)
 				  "device\n");
 
 		/* we can not do an adapter restart while inside an irq lock */
-		queue_work(priv->workqueue, &priv->adapter_restart);
+		schedule_work(&priv->adapter_restart);
 	} else
 		IPW_DEBUG_RF_KILL("HW RF Kill deactivated.  SW RF Kill still "
 				  "enabled\n");
@@ -10735,7 +10708,7 @@ static void ipw_link_up(struct ipw_priv *priv)
 	notify_wx_assoc_event(priv);
 
 	if (priv->config & CFG_BACKGROUND_SCAN)
-		queue_delayed_work(priv->workqueue, &priv->request_scan, HZ);
+		schedule_delayed_work(&priv->request_scan, HZ);
 }
 
 static void ipw_bg_link_up(struct work_struct *work)
@@ -10764,7 +10737,7 @@ static void ipw_link_down(struct ipw_priv *priv)
 
 	if (!(priv->status & STATUS_EXIT_PENDING)) {
 		/* Queue up another scan... */
-		queue_delayed_work(priv->workqueue, &priv->request_scan, 0);
+		schedule_delayed_work(&priv->request_scan, 0);
 	} else
 		cancel_delayed_work(&priv->scan_event);
 }
@@ -10782,7 +10755,6 @@ static int __devinit ipw_setup_deferred_work(struct ipw_priv *priv)
 {
 	int ret = 0;
 
-	priv->workqueue = create_workqueue(DRV_NAME);
 	init_waitqueue_head(&priv->wait_command_queue);
 	init_waitqueue_head(&priv->wait_state);
 
@@ -11339,8 +11311,7 @@ static int ipw_up(struct ipw_priv *priv)
 			IPW_WARNING("Radio Frequency Kill Switch is On:\n"
 				    "Kill switch must be turned off for "
 				    "wireless networking to work.\n");
-			queue_delayed_work(priv->workqueue, &priv->rf_kill,
-					   2 * HZ);
+			schedule_delayed_work(&priv->rf_kill, 2 * HZ);
 			return 0;
 		}
 
@@ -11350,8 +11321,7 @@ static int ipw_up(struct ipw_priv *priv)
 
 			/* If configure to try and auto-associate, kick
 			 * off a scan. */
-			queue_delayed_work(priv->workqueue,
-					   &priv->request_scan, 0);
+			schedule_delayed_work(&priv->request_scan, 0);
 
 			return 0;
 		}
@@ -11817,7 +11787,7 @@ static int __devinit ipw_pci_probe(struct pci_dev *pdev,
 	err = request_irq(pdev->irq, ipw_isr, IRQF_SHARED, DRV_NAME, priv);
 	if (err) {
 		IPW_ERROR("Error allocating IRQ %d\n", pdev->irq);
-		goto out_destroy_workqueue;
+		goto out_iounmap;
 	}
 
 	SET_NETDEV_DEV(net_dev, &pdev->dev);
@@ -11885,9 +11855,6 @@ static int __devinit ipw_pci_probe(struct pci_dev *pdev,
 	sysfs_remove_group(&pdev->dev.kobj, &ipw_attribute_group);
       out_release_irq:
 	free_irq(pdev->irq, priv);
-      out_destroy_workqueue:
-	destroy_workqueue(priv->workqueue);
-	priv->workqueue = NULL;
       out_iounmap:
 	iounmap(priv->hw_base);
       out_pci_release_regions:
@@ -11930,18 +11897,31 @@ static void __devexit ipw_pci_remove(struct pci_dev *pdev)
 		kfree(priv->cmdlog);
 		priv->cmdlog = NULL;
 	}
-	/* ipw_down will ensure that there is no more pending work
-	 * in the workqueue's, so we can safely remove them now. */
-	cancel_delayed_work(&priv->adhoc_check);
-	cancel_delayed_work(&priv->gather_stats);
-	cancel_delayed_work(&priv->request_scan);
-	cancel_delayed_work(&priv->request_direct_scan);
-	cancel_delayed_work(&priv->request_passive_scan);
-	cancel_delayed_work(&priv->scan_event);
-	cancel_delayed_work(&priv->rf_kill);
-	cancel_delayed_work(&priv->scan_check);
-	destroy_workqueue(priv->workqueue);
-	priv->workqueue = NULL;
+
+	/* make sure all works are inactive */
+	cancel_delayed_work_sync(&priv->adhoc_check);
+	cancel_work_sync(&priv->associate);
+	cancel_work_sync(&priv->disassociate);
+	cancel_work_sync(&priv->system_config);
+	cancel_work_sync(&priv->rx_replenish);
+	cancel_work_sync(&priv->adapter_restart);
+	cancel_delayed_work_sync(&priv->rf_kill);
+	cancel_work_sync(&priv->up);
+	cancel_work_sync(&priv->down);
+	cancel_delayed_work_sync(&priv->request_scan);
+	cancel_delayed_work_sync(&priv->request_direct_scan);
+	cancel_delayed_work_sync(&priv->request_passive_scan);
+	cancel_delayed_work_sync(&priv->scan_event);
+	cancel_delayed_work_sync(&priv->gather_stats);
+	cancel_work_sync(&priv->abort_scan);
+	cancel_work_sync(&priv->roam);
+	cancel_delayed_work_sync(&priv->scan_check);
+	cancel_work_sync(&priv->link_up);
+	cancel_work_sync(&priv->link_down);
+	cancel_delayed_work_sync(&priv->led_link_on);
+	cancel_delayed_work_sync(&priv->led_link_off);
+	cancel_delayed_work_sync(&priv->led_act_off);
+	cancel_work_sync(&priv->merge_networks);
 
 	/* Free MAC hash list for ADHOC */
 	for (i = 0; i < IPW_IBSS_MAC_HASH_SIZE; i++) {
@@ -12029,7 +12009,7 @@ static int ipw_pci_resume(struct pci_dev *pdev)
 	priv->suspend_time = get_seconds() - priv->suspend_at;
 
 	/* Bring the device back up */
-	queue_work(priv->workqueue, &priv->up);
+	schedule_work(&priv->up);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.h b/drivers/net/wireless/ipw2x00/ipw2200.h
index d7d049c7a4fa..0441445b8bfa 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.h
+++ b/drivers/net/wireless/ipw2x00/ipw2200.h
@@ -1299,8 +1299,6 @@ struct ipw_priv {
 	u8 direct_scan_ssid[IW_ESSID_MAX_SIZE];
 	u8 direct_scan_ssid_len;
 
-	struct workqueue_struct *workqueue;
-
 	struct delayed_work adhoc_check;
 	struct work_struct associate;
 	struct work_struct disassociate;
diff --git a/drivers/net/wireless/iwlegacy/Kconfig b/drivers/net/wireless/iwlegacy/Kconfig
new file mode 100644
index 000000000000..2a45dd44cc12
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/Kconfig
@@ -0,0 +1,116 @@
+config IWLWIFI_LEGACY
+	tristate "Intel Wireless Wifi legacy devices"
+	depends on PCI && MAC80211
+	select FW_LOADER
+	select NEW_LEDS
+	select LEDS_CLASS
+	select LEDS_TRIGGERS
+	select MAC80211_LEDS
+
+menu "Debugging Options"
+	depends on IWLWIFI_LEGACY
+
+config IWLWIFI_LEGACY_DEBUG
+	bool "Enable full debugging output in 4965 and 3945 drivers"
+	depends on IWLWIFI_LEGACY
+	---help---
+	  This option will enable debug tracing output for the iwlwifilegacy
+	  drivers.
+
+	  This will result in the kernel module being ~100k larger.  You can
+	  control which debug output is sent to the kernel log by setting the
+	  value in
+
+		/sys/class/net/wlan0/device/debug_level
+
+	  This entry will only exist if this option is enabled.
+
+	  To set a value, simply echo an 8-byte hex value to the same file:
+
+		  % echo 0x43fff > /sys/class/net/wlan0/device/debug_level
+
+	  You can find the list of debug mask values in:
+		  drivers/net/wireless/iwlwifilegacy/iwl-debug.h
+
+	  If this is your first time using this driver, you should say Y here
+	  as the debug information can assist others in helping you resolve
+	  any problems you may encounter.
+
+config IWLWIFI_LEGACY_DEBUGFS
+        bool "4965 and 3945 debugfs support"
+        depends on IWLWIFI_LEGACY && MAC80211_DEBUGFS
+        ---help---
+	  Enable creation of debugfs files for the iwlwifilegacy drivers. This
+	  is a low-impact option that allows getting insight into the
+	  driver's state at runtime.
+
+config IWLWIFI_LEGACY_DEVICE_TRACING
+	bool "iwlwifilegacy legacy device access tracing"
+	depends on IWLWIFI_LEGACY
+	depends on EVENT_TRACING
+	help
+	  Say Y here to trace all commands, including TX frames and IO
+	  accesses, sent to the device. If you say yes, iwlwifilegacy will
+	  register with the ftrace framework for event tracing and dump
+	  all this information to the ringbuffer, you may need to
+	  increase the ringbuffer size. See the ftrace documentation
+	  for more information.
+
+	  When tracing is not enabled, this option still has some
+	  (though rather small) overhead.
+
+	  If unsure, say Y so we can help you better when problems
+	  occur.
+endmenu
+
+config IWL4965
+	tristate "Intel Wireless WiFi 4965AGN (iwl4965)"
+	depends on IWLWIFI_LEGACY
+	---help---
+	  This option enables support for
+
+	  Select to build the driver supporting the:
+
+	  Intel Wireless WiFi Link 4965AGN
+
+	  This driver uses the kernel's mac80211 subsystem.
+
+	  In order to use this driver, you will need a microcode (uCode)
+	  image for it. You can obtain the microcode from:
+
+	          <http://intellinuxwireless.org/>.
+
+	  The microcode is typically installed in /lib/firmware. You can
+	  look in the hotplug script /etc/hotplug/firmware.agent to
+	  determine which directory FIRMWARE_DIR is set to when the script
+	  runs.
+
+	  If you want to compile the driver as a module ( = code which can be
+	  inserted in and removed from the running kernel whenever you want),
+	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
+	  module will be called iwl4965.
+
+config IWL3945
+	tristate "Intel PRO/Wireless 3945ABG/BG Network Connection (iwl3945)"
+	depends on IWLWIFI_LEGACY
+	---help---
+	  Select to build the driver supporting the:
+
+	  Intel PRO/Wireless 3945ABG/BG Network Connection
+
+	  This driver uses the kernel's mac80211 subsystem.
+
+	  In order to use this driver, you will need a microcode (uCode)
+	  image for it. You can obtain the microcode from:
+
+	          <http://intellinuxwireless.org/>.
+
+	  The microcode is typically installed in /lib/firmware. You can
+	  look in the hotplug script /etc/hotplug/firmware.agent to
+	  determine which directory FIRMWARE_DIR is set to when the script
+	  runs.
+
+	  If you want to compile the driver as a module ( = code which can be
+	  inserted in and removed from the running kernel whenever you want),
+	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
+	  module will be called iwl3945.
diff --git a/drivers/net/wireless/iwlegacy/Makefile b/drivers/net/wireless/iwlegacy/Makefile
new file mode 100644
index 000000000000..d56aeb38c211
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/Makefile
@@ -0,0 +1,25 @@
+obj-$(CONFIG_IWLWIFI_LEGACY)	+= iwl-legacy.o
+iwl-legacy-objs 		:= iwl-core.o iwl-eeprom.o iwl-hcmd.o iwl-power.o
+iwl-legacy-objs 		+= iwl-rx.o iwl-tx.o iwl-sta.o
+iwl-legacy-objs 		+= iwl-scan.o iwl-led.o
+iwl-legacy-$(CONFIG_IWLWIFI_LEGACY_DEBUGFS) += iwl-debugfs.o
+iwl-legacy-$(CONFIG_IWLWIFI_LEGACY_DEVICE_TRACING) += iwl-devtrace.o
+
+iwl-legacy-objs += $(iwl-legacy-m)
+
+CFLAGS_iwl-devtrace.o := -I$(src)
+
+# 4965
+obj-$(CONFIG_IWL4965)	+= iwl4965.o
+iwl4965-objs		:= iwl-4965.o iwl4965-base.o iwl-4965-rs.o iwl-4965-led.o
+iwl4965-objs		+= iwl-4965-ucode.o iwl-4965-tx.o
+iwl4965-objs		+= iwl-4965-lib.o iwl-4965-rx.o iwl-4965-calib.o
+iwl4965-objs		+= iwl-4965-sta.o iwl-4965-eeprom.o
+iwl4965-$(CONFIG_IWLWIFI_LEGACY_DEBUGFS) += iwl-4965-debugfs.o
+
+# 3945
+obj-$(CONFIG_IWL3945)	+= iwl3945.o
+iwl3945-objs		:= iwl3945-base.o iwl-3945.o iwl-3945-rs.o iwl-3945-led.o
+iwl3945-$(CONFIG_IWLWIFI_LEGACY_DEBUGFS) += iwl-3945-debugfs.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-debugfs.c b/drivers/net/wireless/iwlegacy/iwl-3945-debugfs.c
index ef0835b01b6b..cfabb38793ab 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-debugfs.c
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-debugfs.c
@@ -2,7 +2,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2008 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2008 - 2011 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
@@ -60,12 +60,13 @@ ssize_t iwl3945_ucode_rx_stats_read(struct file *file,
 	int bufsz = sizeof(struct iwl39_statistics_rx_phy) * 40 +
 		    sizeof(struct iwl39_statistics_rx_non_phy) * 40 + 400;
 	ssize_t ret;
-	struct iwl39_statistics_rx_phy *ofdm, *accum_ofdm, *delta_ofdm, *max_ofdm;
+	struct iwl39_statistics_rx_phy *ofdm, *accum_ofdm, *delta_ofdm,
+					*max_ofdm;
 	struct iwl39_statistics_rx_phy *cck, *accum_cck, *delta_cck, *max_cck;
 	struct iwl39_statistics_rx_non_phy *general, *accum_general;
 	struct iwl39_statistics_rx_non_phy *delta_general, *max_general;
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
@@ -335,7 +336,7 @@ ssize_t iwl3945_ucode_tx_stats_read(struct file *file,
 	ssize_t ret;
 	struct iwl39_statistics_tx *tx, *accum_tx, *delta_tx, *max_tx;
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
@@ -434,7 +435,7 @@ ssize_t iwl3945_ucode_general_stats_read(struct file *file,
 	struct statistics_dbg *dbg, *accum_dbg, *delta_dbg, *max_dbg;
 	struct iwl39_statistics_div *div, *accum_div, *delta_div, *max_div;
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EAGAIN;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-debugfs.h b/drivers/net/wireless/iwlegacy/iwl-3945-debugfs.h
index 70809c53c215..8fef4b32b447 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-debugfs.h
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-debugfs.h
@@ -2,7 +2,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2008 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2008 - 2011 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
@@ -30,7 +30,7 @@
 #include "iwl-core.h"
 #include "iwl-debug.h"
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
 ssize_t iwl3945_ucode_rx_stats_read(struct file *file, char __user *user_buf,
 				    size_t count, loff_t *ppos);
 ssize_t iwl3945_ucode_tx_stats_read(struct file *file, char __user *user_buf,
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-fh.h b/drivers/net/wireless/iwlegacy/iwl-3945-fh.h
index 2c9ed2b502a3..836c9919f82e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-fh.h
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-fh.h
@@ -5,7 +5,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 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
@@ -30,7 +30,7 @@
  *
  * BSD LICENSE
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -185,4 +185,3 @@ struct iwl3945_tfd {
 
 
 #endif /* __iwl_3945_fh_h__ */
-
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-hw.h b/drivers/net/wireless/iwlegacy/iwl-3945-hw.h
index 65b5834da28c..779d3cb86e2c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-hw.h
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-hw.h
@@ -5,7 +5,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 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
@@ -30,7 +30,7 @@
  *
  * BSD LICENSE
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -164,12 +164,11 @@ struct iwl3945_eeprom {
 /*
  * Per-channel regulatory data.
  *
- * Each channel that *might* be supported by 3945 or 4965 has a fixed location
+ * Each channel that *might* be supported by 3945 has a fixed location
  * in EEPROM containing EEPROM_CHANNEL_* usage flags (LSB) and max regulatory
  * txpower (MSB).
  *
- * Entries immediately below are for 20 MHz channel width.  HT40 (40 MHz)
- * channels (only for 4965, not supported by 3945) appear later in the EEPROM.
+ * Entries immediately below are for 20 MHz channel width.
  *
  * 2.4 GHz channels 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
  */
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-led.c b/drivers/net/wireless/iwlegacy/iwl-3945-led.c
index abe2b739c4dc..abd923558d48 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-led.c
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-led.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2003 - 2011 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
@@ -56,36 +56,9 @@ static int iwl3945_send_led_cmd(struct iwl_priv *priv,
 		.callback = NULL,
 	};
 
-	return iwl_send_cmd(priv, &cmd);
-}
-
-/* Set led on command */
-static int iwl3945_led_on(struct iwl_priv *priv)
-{
-	struct iwl_led_cmd led_cmd = {
-		.id = IWL_LED_LINK,
-		.on = IWL_LED_SOLID,
-		.off = 0,
-		.interval = IWL_DEF_LED_INTRVL
-	};
-	return iwl3945_send_led_cmd(priv, &led_cmd);
-}
-
-/* Set led off command */
-static int iwl3945_led_off(struct iwl_priv *priv)
-{
-	struct iwl_led_cmd led_cmd = {
-		.id = IWL_LED_LINK,
-		.on = 0,
-		.off = 0,
-		.interval = IWL_DEF_LED_INTRVL
-	};
-	IWL_DEBUG_LED(priv, "led off\n");
-	return iwl3945_send_led_cmd(priv, &led_cmd);
+	return iwl_legacy_send_cmd(priv, &cmd);
 }
 
 const struct iwl_led_ops iwl3945_led_ops = {
 	.cmd = iwl3945_send_led_cmd,
-	.on = iwl3945_led_on,
-	.off = iwl3945_led_off,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-led.h b/drivers/net/wireless/iwlegacy/iwl-3945-led.h
index ce990adc51e7..96716276eb0d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-led.h
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-led.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2003 - 2011 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
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c b/drivers/net/wireless/iwlegacy/iwl-3945-rs.c
index 1f3e7e34fbc7..977bd2477c6a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
+++ b/drivers/net/wireless/iwlegacy/iwl-3945-rs.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 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
@@ -89,7 +89,7 @@ static struct iwl3945_tpt_entry iwl3945_tpt_table_g[] = {
 };
 
 #define IWL_RATE_MAX_WINDOW          62
-#define IWL_RATE_FLUSH       	 (3*HZ)
+#define IWL_RATE_FLUSH		(3*HZ)
 #define IWL_RATE_WIN_FLUSH       (HZ/2)
 #define IWL39_RATE_HIGH_TH          11520
 #define IWL_SUCCESS_UP_TH	   8960
@@ -394,18 +394,18 @@ out:
 	IWL_DEBUG_INFO(priv, "leave\n");
 }
 
-static void *rs_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
+static void *iwl3945_rs_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
 {
 	return hw->priv;
 }
 
 /* rate scale requires free function to be implemented */
-static void rs_free(void *priv)
+static void iwl3945_rs_free(void *priv)
 {
 	return;
 }
 
-static void *rs_alloc_sta(void *iwl_priv, struct ieee80211_sta *sta, gfp_t gfp)
+static void *iwl3945_rs_alloc_sta(void *iwl_priv, struct ieee80211_sta *sta, gfp_t gfp)
 {
 	struct iwl3945_rs_sta *rs_sta;
 	struct iwl3945_sta_priv *psta = (void *) sta->drv_priv;
@@ -423,7 +423,7 @@ static void *rs_alloc_sta(void *iwl_priv, struct ieee80211_sta *sta, gfp_t gfp)
 	return rs_sta;
 }
 
-static void rs_free_sta(void *iwl_priv, struct ieee80211_sta *sta,
+static void iwl3945_rs_free_sta(void *iwl_priv, struct ieee80211_sta *sta,
 			void *priv_sta)
 {
 	struct iwl3945_rs_sta *rs_sta = priv_sta;
@@ -438,12 +438,12 @@ static void rs_free_sta(void *iwl_priv, struct ieee80211_sta *sta,
 
 
 /**
- * rs_tx_status - Update rate control values based on Tx results
+ * iwl3945_rs_tx_status - Update rate control values based on Tx results
  *
  * NOTE: Uses iwl_priv->retry_rate for the # of retries attempted by
  * the hardware for each rate.
  */
-static void rs_tx_status(void *priv_rate, struct ieee80211_supported_band *sband,
+static void iwl3945_rs_tx_status(void *priv_rate, struct ieee80211_supported_band *sband,
 			 struct ieee80211_sta *sta, void *priv_sta,
 			 struct sk_buff *skb)
 {
@@ -612,7 +612,7 @@ static u16 iwl3945_get_adjacent_rate(struct iwl3945_rs_sta *rs_sta,
 }
 
 /**
- * rs_get_rate - find the rate for the requested packet
+ * iwl3945_rs_get_rate - find the rate for the requested packet
  *
  * Returns the ieee80211_rate structure allocated by the driver.
  *
@@ -627,7 +627,7 @@ static u16 iwl3945_get_adjacent_rate(struct iwl3945_rs_sta *rs_sta,
  * rate table and must reference the driver allocated rate table
  *
  */
-static void rs_get_rate(void *priv_r, struct ieee80211_sta *sta,
+static void iwl3945_rs_get_rate(void *priv_r, struct ieee80211_sta *sta,
 			void *priv_sta,	struct ieee80211_tx_rate_control *txrc)
 {
 	struct ieee80211_supported_band *sband = txrc->sband;
@@ -644,7 +644,7 @@ static void rs_get_rate(void *priv_r, struct ieee80211_sta *sta,
 	u32 fail_count;
 	s8 scale_action = 0;
 	unsigned long flags;
-	u16 rate_mask = sta ? sta->supp_rates[sband->band] : 0;
+	u16 rate_mask;
 	s8 max_rate_idx = -1;
 	struct iwl_priv *priv __maybe_unused = (struct iwl_priv *)priv_r;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -899,7 +899,8 @@ static void iwl3945_remove_debugfs(void *priv, void *priv_sta)
  * the station is added. Since mac80211 calls this function before a
  * station is added we ignore it.
  */
-static void rs_rate_init_stub(void *priv_r, struct ieee80211_supported_band *sband,
+static void iwl3945_rs_rate_init_stub(void *priv_r,
+				struct ieee80211_supported_band *sband,
 			      struct ieee80211_sta *sta, void *priv_sta)
 {
 }
@@ -907,13 +908,13 @@ static void rs_rate_init_stub(void *priv_r, struct ieee80211_supported_band *sba
 static struct rate_control_ops rs_ops = {
 	.module = NULL,
 	.name = RS_NAME,
-	.tx_status = rs_tx_status,
-	.get_rate = rs_get_rate,
-	.rate_init = rs_rate_init_stub,
-	.alloc = rs_alloc,
-	.free = rs_free,
-	.alloc_sta = rs_alloc_sta,
-	.free_sta = rs_free_sta,
+	.tx_status = iwl3945_rs_tx_status,
+	.get_rate = iwl3945_rs_get_rate,
+	.rate_init = iwl3945_rs_rate_init_stub,
+	.alloc = iwl3945_rs_alloc,
+	.free = iwl3945_rs_free,
+	.alloc_sta = iwl3945_rs_alloc_sta,
+	.free_sta = iwl3945_rs_free_sta,
 #ifdef CONFIG_MAC80211_DEBUGFS
 	.add_sta_debugfs = iwl3945_add_debugfs,
 	.remove_sta_debugfs = iwl3945_remove_debugfs,
@@ -991,5 +992,3 @@ void iwl3945_rate_control_unregister(void)
 {
 	ieee80211_rate_control_unregister(&rs_ops);
 }
-
-
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlegacy/iwl-3945.c
index 39b6f16c87fa..d096dc28204d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.c
+++ b/drivers/net/wireless/iwlegacy/iwl-3945.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2003 - 2011 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
@@ -51,7 +51,6 @@
 #include "iwl-led.h"
 #include "iwl-3945-led.h"
 #include "iwl-3945-debugfs.h"
-#include "iwl-legacy.h"
 
 #define IWL_DECLARE_RATE_INFO(r, ip, in, rp, rn, pp, np)    \
 	[IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,   \
@@ -172,14 +171,14 @@ void iwl3945_disable_events(struct iwl_priv *priv)
 		return;
 	}
 
-	disable_ptr = iwl_read_targ_mem(priv, base + (4 * sizeof(u32)));
-	array_size = iwl_read_targ_mem(priv, base + (5 * sizeof(u32)));
+	disable_ptr = iwl_legacy_read_targ_mem(priv, base + (4 * sizeof(u32)));
+	array_size = iwl_legacy_read_targ_mem(priv, base + (5 * sizeof(u32)));
 
 	if (IWL_EVT_DISABLE && (array_size == IWL_EVT_DISABLE_SIZE)) {
 		IWL_DEBUG_INFO(priv, "Disabling selected uCode log events at 0x%x\n",
 			       disable_ptr);
 		for (i = 0; i < IWL_EVT_DISABLE_SIZE; i++)
-			iwl_write_targ_mem(priv,
+			iwl_legacy_write_targ_mem(priv,
 					   disable_ptr + (i * sizeof(u32)),
 					   evt_disable[i]);
 
@@ -202,7 +201,7 @@ static int iwl3945_hwrate_to_plcp_idx(u8 plcp)
 	return -1;
 }
 
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 #define TX_STATUS_ENTRY(x) case TX_3945_STATUS_FAIL_ ## x: return #x
 
 static const char *iwl3945_get_tx_fail_reason(u32 status)
@@ -255,7 +254,7 @@ int iwl3945_rs_next_rate(struct iwl_priv *priv, int rate)
 		break;
 	case IEEE80211_BAND_2GHZ:
 		if (!(priv->_3945.sta_supp_rates & IWL_OFDM_RATES_MASK) &&
-		    iwl_is_associated(priv, IWL_RXON_CTX_BSS)) {
+		    iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS)) {
 			if (rate == IWL_RATE_11M_INDEX)
 				next_rate = IWL_RATE_5M_INDEX;
 		}
@@ -285,8 +284,9 @@ static void iwl3945_tx_queue_reclaim(struct iwl_priv *priv,
 
 	BUG_ON(txq_id == IWL39_CMD_QUEUE_NUM);
 
-	for (index = iwl_queue_inc_wrap(index, q->n_bd); q->read_ptr != index;
-		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+	for (index = iwl_legacy_queue_inc_wrap(index, q->n_bd);
+		q->read_ptr != index;
+		q->read_ptr = iwl_legacy_queue_inc_wrap(q->read_ptr, q->n_bd)) {
 
 		tx_info = &txq->txb[txq->q.read_ptr];
 		ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb);
@@ -294,10 +294,10 @@ static void iwl3945_tx_queue_reclaim(struct iwl_priv *priv,
 		priv->cfg->ops->lib->txq_free_tfd(priv, txq);
 	}
 
-	if (iwl_queue_space(q) > q->low_mark && (txq_id >= 0) &&
+	if (iwl_legacy_queue_space(q) > q->low_mark && (txq_id >= 0) &&
 			(txq_id != IWL39_CMD_QUEUE_NUM) &&
 			priv->mac80211_registered)
-		iwl_wake_queue(priv, txq);
+		iwl_legacy_wake_queue(priv, txq);
 }
 
 /**
@@ -317,7 +317,7 @@ static void iwl3945_rx_reply_tx(struct iwl_priv *priv,
 	int rate_idx;
 	int fail;
 
-	if ((index >= txq->q.n_bd) || (iwl_queue_used(&txq->q, index) == 0)) {
+	if ((index >= txq->q.n_bd) || (iwl_legacy_queue_used(&txq->q, index) == 0)) {
 		IWL_ERR(priv, "Read index for DMA queue txq_id (%d) index %d "
 			  "is out of range [0-%d] %d %d\n", txq_id,
 			  index, txq->q.n_bd, txq->q.write_ptr,
@@ -363,12 +363,7 @@ static void iwl3945_rx_reply_tx(struct iwl_priv *priv,
  *  RX handler implementations
  *
  *****************************************************************************/
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-/*
- *  based on the assumption of all statistics counter are in DWORD
- *  FIXME: This function is for debugging, do not deal with
- *  the case of counters roll-over.
- */
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
 static void iwl3945_accumulative_statistics(struct iwl_priv *priv,
 					    __le32 *stats)
 {
@@ -410,10 +405,10 @@ void iwl3945_hw_rx_statistics(struct iwl_priv *priv,
 	IWL_DEBUG_RX(priv, "Statistics notification received (%d vs %d).\n",
 		     (int)sizeof(struct iwl3945_notif_statistics),
 		     le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK);
-#ifdef CONFIG_IWLWIFI_DEBUGFS
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
 	iwl3945_accumulative_statistics(priv, (__le32 *)&pkt->u.raw);
 #endif
-	iwl_recover_from_statistics(priv, pkt);
+	iwl_legacy_recover_from_statistics(priv, pkt);
 
 	memcpy(&priv->_3945.statistics, pkt->u.raw, sizeof(priv->_3945.statistics));
 }
@@ -425,7 +420,7 @@ void iwl3945_reply_statistics(struct iwl_priv *priv,
 	__le32 *flag = (__le32 *)&pkt->u.raw;
 
 	if (le32_to_cpu(*flag) & UCODE_STATISTICS_CLEAR_MSK) {
-#ifdef CONFIG_IWLWIFI_DEBUGFS
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
 		memset(&priv->_3945.accum_statistics, 0,
 			sizeof(struct iwl3945_notif_statistics));
 		memset(&priv->_3945.delta_statistics, 0,
@@ -496,14 +491,14 @@ static void iwl3945_pass_packet_to_mac80211(struct iwl_priv *priv,
 	}
 
 	if (!iwl3945_mod_params.sw_crypto)
-		iwl_set_decrypted_flag(priv,
+		iwl_legacy_set_decrypted_flag(priv,
 				       (struct ieee80211_hdr *)rxb_addr(rxb),
 				       le32_to_cpu(rx_end->status), stats);
 
 	skb_add_rx_frag(skb, 0, rxb->page,
 			(void *)rx_hdr->payload - (void *)pkt, len);
 
-	iwl_update_stats(priv, false, fc, len);
+	iwl_legacy_update_stats(priv, false, fc, len);
 	memcpy(IEEE80211_SKB_RXCB(skb), stats, sizeof(*stats));
 
 	ieee80211_rx(priv->hw, skb);
@@ -528,10 +523,11 @@ static void iwl3945_rx_reply_rx(struct iwl_priv *priv,
 
 	rx_status.flag = 0;
 	rx_status.mactime = le64_to_cpu(rx_end->timestamp);
-	rx_status.freq =
-		ieee80211_channel_to_frequency(le16_to_cpu(rx_hdr->channel));
 	rx_status.band = (rx_hdr->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
 				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+	rx_status.freq =
+		ieee80211_channel_to_frequency(le16_to_cpu(rx_hdr->channel),
+					       rx_status.band);
 
 	rx_status.rate_idx = iwl3945_hwrate_to_plcp_idx(rx_hdr->rate);
 	if (rx_status.band == IEEE80211_BAND_5GHZ)
@@ -575,7 +571,8 @@ static void iwl3945_rx_reply_rx(struct iwl_priv *priv,
 			      rx_status.signal, rx_status.signal,
 			      rx_status.rate_idx);
 
-	iwl_dbg_log_rx_data_frame(priv, le16_to_cpu(rx_hdr->len), header);
+	iwl_legacy_dbg_log_rx_data_frame(priv, le16_to_cpu(rx_hdr->len),
+						header);
 
 	if (network_packet) {
 		priv->_3945.last_beacon_time =
@@ -695,8 +692,7 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl_priv *priv,
 
 	/* We need to figure out how to get the sta->supp_rates while
 	 * in this running context */
-	rate_mask = IWL_RATES_MASK;
-
+	rate_mask = IWL_RATES_MASK_3945;
 
 	/* Set retry limit on DATA packets and Probe Responses*/
 	if (ieee80211_is_probe_resp(fc))
@@ -744,7 +740,7 @@ static u8 iwl3945_sync_sta(struct iwl_priv *priv, int sta_id, u16 tx_rate)
 	station->sta.sta.modify_mask = STA_MODIFY_TX_RATE_MSK;
 	station->sta.rate_n_flags = cpu_to_le16(tx_rate);
 	station->sta.mode = STA_CONTROL_MODIFY_MSK;
-	iwl_send_add_sta(priv, &station->sta, CMD_ASYNC);
+	iwl_legacy_send_add_sta(priv, &station->sta, CMD_ASYNC);
 	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
 
 	IWL_DEBUG_RATE(priv, "SCALE sync station %d to rate %d\n",
@@ -759,7 +755,7 @@ static void iwl3945_set_pwr_vmain(struct iwl_priv *priv)
  * to set power to V_AUX, do
 
 		if (pci_pme_capable(priv->pci_dev, PCI_D3cold)) {
-			iwl_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
+			iwl_legacy_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
 					APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
 					~APMG_PS_CTRL_MSK_PWR_SRC);
 
@@ -769,7 +765,7 @@ static void iwl3945_set_pwr_vmain(struct iwl_priv *priv)
 		}
  */
 
-	iwl_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
+	iwl_legacy_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
 			APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
 			~APMG_PS_CTRL_MSK_PWR_SRC);
 
@@ -779,10 +775,11 @@ static void iwl3945_set_pwr_vmain(struct iwl_priv *priv)
 
 static int iwl3945_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
 {
-	iwl_write_direct32(priv, FH39_RCSR_RBD_BASE(0), rxq->bd_dma);
-	iwl_write_direct32(priv, FH39_RCSR_RPTR_ADDR(0), rxq->rb_stts_dma);
-	iwl_write_direct32(priv, FH39_RCSR_WPTR(0), 0);
-	iwl_write_direct32(priv, FH39_RCSR_CONFIG(0),
+	iwl_legacy_write_direct32(priv, FH39_RCSR_RBD_BASE(0), rxq->bd_dma);
+	iwl_legacy_write_direct32(priv, FH39_RCSR_RPTR_ADDR(0),
+					rxq->rb_stts_dma);
+	iwl_legacy_write_direct32(priv, FH39_RCSR_WPTR(0), 0);
+	iwl_legacy_write_direct32(priv, FH39_RCSR_CONFIG(0),
 		FH39_RCSR_RX_CONFIG_REG_VAL_DMA_CHNL_EN_ENABLE |
 		FH39_RCSR_RX_CONFIG_REG_VAL_RDRBD_EN_ENABLE |
 		FH39_RCSR_RX_CONFIG_REG_BIT_WR_STTS_EN |
@@ -793,7 +790,7 @@ static int iwl3945_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
 		FH39_RCSR_RX_CONFIG_REG_VAL_MSG_MODE_FH);
 
 	/* fake read to flush all prev I/O */
-	iwl_read_direct32(priv, FH39_RSSR_CTRL);
+	iwl_legacy_read_direct32(priv, FH39_RSSR_CTRL);
 
 	return 0;
 }
@@ -802,23 +799,23 @@ static int iwl3945_tx_reset(struct iwl_priv *priv)
 {
 
 	/* bypass mode */
-	iwl_write_prph(priv, ALM_SCD_MODE_REG, 0x2);
+	iwl_legacy_write_prph(priv, ALM_SCD_MODE_REG, 0x2);
 
 	/* RA 0 is active */
-	iwl_write_prph(priv, ALM_SCD_ARASTAT_REG, 0x01);
+	iwl_legacy_write_prph(priv, ALM_SCD_ARASTAT_REG, 0x01);
 
 	/* all 6 fifo are active */
-	iwl_write_prph(priv, ALM_SCD_TXFACT_REG, 0x3f);
+	iwl_legacy_write_prph(priv, ALM_SCD_TXFACT_REG, 0x3f);
 
-	iwl_write_prph(priv, ALM_SCD_SBYP_MODE_1_REG, 0x010000);
-	iwl_write_prph(priv, ALM_SCD_SBYP_MODE_2_REG, 0x030002);
-	iwl_write_prph(priv, ALM_SCD_TXF4MF_REG, 0x000004);
-	iwl_write_prph(priv, ALM_SCD_TXF5MF_REG, 0x000005);
+	iwl_legacy_write_prph(priv, ALM_SCD_SBYP_MODE_1_REG, 0x010000);
+	iwl_legacy_write_prph(priv, ALM_SCD_SBYP_MODE_2_REG, 0x030002);
+	iwl_legacy_write_prph(priv, ALM_SCD_TXF4MF_REG, 0x000004);
+	iwl_legacy_write_prph(priv, ALM_SCD_TXF5MF_REG, 0x000005);
 
-	iwl_write_direct32(priv, FH39_TSSR_CBB_BASE,
+	iwl_legacy_write_direct32(priv, FH39_TSSR_CBB_BASE,
 			     priv->_3945.shared_phys);
 
-	iwl_write_direct32(priv, FH39_TSSR_MSG_CONFIG,
+	iwl_legacy_write_direct32(priv, FH39_TSSR_MSG_CONFIG,
 		FH39_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TXPD_ON |
 		FH39_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_TXPD_ON |
 		FH39_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_128B |
@@ -844,7 +841,7 @@ static int iwl3945_txq_ctx_reset(struct iwl_priv *priv)
 	iwl3945_hw_txq_ctx_free(priv);
 
 	/* allocate tx queue structure */
-	rc = iwl_alloc_txq_mem(priv);
+	rc = iwl_legacy_alloc_txq_mem(priv);
 	if (rc)
 		return rc;
 
@@ -857,8 +854,8 @@ static int iwl3945_txq_ctx_reset(struct iwl_priv *priv)
 	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
 		slots_num = (txq_id == IWL39_CMD_QUEUE_NUM) ?
 				TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
-		rc = iwl_tx_queue_init(priv, &priv->txq[txq_id], slots_num,
-				       txq_id);
+		rc = iwl_legacy_tx_queue_init(priv, &priv->txq[txq_id],
+						slots_num, txq_id);
 		if (rc) {
 			IWL_ERR(priv, "Tx %d queue init failed\n", txq_id);
 			goto error;
@@ -875,21 +872,23 @@ static int iwl3945_txq_ctx_reset(struct iwl_priv *priv)
 
 /*
  * Start up 3945's basic functionality after it has been reset
- * (e.g. after platform boot, or shutdown via iwl_apm_stop())
+ * (e.g. after platform boot, or shutdown via iwl_legacy_apm_stop())
  * NOTE:  This does not load uCode nor start the embedded processor
  */
 static int iwl3945_apm_init(struct iwl_priv *priv)
 {
-	int ret = iwl_apm_init(priv);
+	int ret = iwl_legacy_apm_init(priv);
 
 	/* Clear APMG (NIC's internal power management) interrupts */
-	iwl_write_prph(priv, APMG_RTC_INT_MSK_REG, 0x0);
-	iwl_write_prph(priv, APMG_RTC_INT_STT_REG, 0xFFFFFFFF);
+	iwl_legacy_write_prph(priv, APMG_RTC_INT_MSK_REG, 0x0);
+	iwl_legacy_write_prph(priv, APMG_RTC_INT_STT_REG, 0xFFFFFFFF);
 
 	/* Reset radio chip */
-	iwl_set_bits_prph(priv, APMG_PS_CTRL_REG, APMG_PS_CTRL_VAL_RESET_REQ);
+	iwl_legacy_set_bits_prph(priv, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_VAL_RESET_REQ);
 	udelay(5);
-	iwl_clear_bits_prph(priv, APMG_PS_CTRL_REG, APMG_PS_CTRL_VAL_RESET_REQ);
+	iwl_legacy_clear_bits_prph(priv, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_VAL_RESET_REQ);
 
 	return ret;
 }
@@ -898,30 +897,28 @@ static void iwl3945_nic_config(struct iwl_priv *priv)
 {
 	struct iwl3945_eeprom *eeprom = (struct iwl3945_eeprom *)priv->eeprom;
 	unsigned long flags;
-	u8 rev_id = 0;
+	u8 rev_id = priv->pci_dev->revision;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
 	/* Determine HW type */
-	pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &rev_id);
-
 	IWL_DEBUG_INFO(priv, "HW Revision ID = 0x%X\n", rev_id);
 
 	if (rev_id & PCI_CFG_REV_ID_BIT_RTP)
 		IWL_DEBUG_INFO(priv, "RTP type\n");
 	else if (rev_id & PCI_CFG_REV_ID_BIT_BASIC_SKU) {
 		IWL_DEBUG_INFO(priv, "3945 RADIO-MB type\n");
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    CSR39_HW_IF_CONFIG_REG_BIT_3945_MB);
 	} else {
 		IWL_DEBUG_INFO(priv, "3945 RADIO-MM type\n");
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    CSR39_HW_IF_CONFIG_REG_BIT_3945_MM);
 	}
 
 	if (EEPROM_SKU_CAP_OP_MODE_MRC == eeprom->sku_cap) {
 		IWL_DEBUG_INFO(priv, "SKU OP mode is mrc\n");
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    CSR39_HW_IF_CONFIG_REG_BIT_SKU_MRC);
 	} else
 		IWL_DEBUG_INFO(priv, "SKU OP mode is basic\n");
@@ -929,24 +926,24 @@ static void iwl3945_nic_config(struct iwl_priv *priv)
 	if ((eeprom->board_revision & 0xF0) == 0xD0) {
 		IWL_DEBUG_INFO(priv, "3945ABG revision is 0x%X\n",
 			       eeprom->board_revision);
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    CSR39_HW_IF_CONFIG_REG_BIT_BOARD_TYPE);
 	} else {
 		IWL_DEBUG_INFO(priv, "3945ABG revision is 0x%X\n",
 			       eeprom->board_revision);
-		iwl_clear_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_clear_bit(priv, CSR_HW_IF_CONFIG_REG,
 			      CSR39_HW_IF_CONFIG_REG_BIT_BOARD_TYPE);
 	}
 
 	if (eeprom->almgor_m_version <= 1) {
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    CSR39_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_A);
 		IWL_DEBUG_INFO(priv, "Card M type A version is 0x%X\n",
 			       eeprom->almgor_m_version);
 	} else {
 		IWL_DEBUG_INFO(priv, "Card M type B version is 0x%X\n",
 			       eeprom->almgor_m_version);
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    CSR39_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_B);
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -974,7 +971,7 @@ int iwl3945_hw_nic_init(struct iwl_priv *priv)
 
 	/* Allocate the RX queue, or reset if it is already allocated */
 	if (!rxq->bd) {
-		rc = iwl_rx_queue_alloc(priv);
+		rc = iwl_legacy_rx_queue_alloc(priv);
 		if (rc) {
 			IWL_ERR(priv, "Unable to initialize Rx queue\n");
 			return -ENOMEM;
@@ -989,10 +986,10 @@ int iwl3945_hw_nic_init(struct iwl_priv *priv)
 
 	/* Look at using this instead:
 	rxq->need_update = 1;
-	iwl_rx_queue_update_write_ptr(priv, rxq);
+	iwl_legacy_rx_queue_update_write_ptr(priv, rxq);
 	*/
 
-	iwl_write_direct32(priv, FH39_RCSR_WPTR(0), rxq->write & ~7);
+	iwl_legacy_write_direct32(priv, FH39_RCSR_WPTR(0), rxq->write & ~7);
 
 	rc = iwl3945_txq_ctx_reset(priv);
 	if (rc)
@@ -1017,12 +1014,12 @@ void iwl3945_hw_txq_ctx_free(struct iwl_priv *priv)
 		for (txq_id = 0; txq_id < priv->hw_params.max_txq_num;
 		     txq_id++)
 			if (txq_id == IWL39_CMD_QUEUE_NUM)
-				iwl_cmd_queue_free(priv);
+				iwl_legacy_cmd_queue_free(priv);
 			else
-				iwl_tx_queue_free(priv, txq_id);
+				iwl_legacy_tx_queue_free(priv, txq_id);
 
 	/* free tx queue structure */
-	iwl_free_txq_mem(priv);
+	iwl_legacy_txq_mem(priv);
 }
 
 void iwl3945_hw_txq_ctx_stop(struct iwl_priv *priv)
@@ -1030,12 +1027,12 @@ void iwl3945_hw_txq_ctx_stop(struct iwl_priv *priv)
 	int txq_id;
 
 	/* stop SCD */
-	iwl_write_prph(priv, ALM_SCD_MODE_REG, 0);
-	iwl_write_prph(priv, ALM_SCD_TXFACT_REG, 0);
+	iwl_legacy_write_prph(priv, ALM_SCD_MODE_REG, 0);
+	iwl_legacy_write_prph(priv, ALM_SCD_TXFACT_REG, 0);
 
 	/* reset TFD queues */
 	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
-		iwl_write_direct32(priv, FH39_TCSR_CONFIG(txq_id), 0x0);
+		iwl_legacy_write_direct32(priv, FH39_TCSR_CONFIG(txq_id), 0x0);
 		iwl_poll_direct_bit(priv, FH39_TSSR_TX_STATUS,
 				FH39_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(txq_id),
 				1000);
@@ -1102,12 +1099,12 @@ static int iwl3945_hw_reg_txpower_get_temperature(struct iwl_priv *priv)
 #define IWL_TEMPERATURE_LIMIT_TIMER   6
 
 /**
- * is_temp_calib_needed - determines if new calibration is needed
+ * iwl3945_is_temp_calib_needed - determines if new calibration is needed
  *
  * records new temperature in tx_mgr->temperature.
  * replaces tx_mgr->last_temperature *only* if calib needed
  *    (assumes caller will actually do the calibration!). */
-static int is_temp_calib_needed(struct iwl_priv *priv)
+static int iwl3945_is_temp_calib_needed(struct iwl_priv *priv)
 {
 	int temp_diff;
 
@@ -1338,9 +1335,6 @@ static void iwl3945_hw_reg_set_scan_power(struct iwl_priv *priv, u32 scan_tbl_in
 	 *   based on eeprom channel data) for this channel.  */
 	power = min(ch_info->scan_power, clip_pwrs[IWL_RATE_6M_INDEX_TABLE]);
 
-	/* further limit to user's max power preference.
-	 * FIXME:  Other spectrum management power limitations do not
-	 *   seem to apply?? */
 	power = min(power, priv->tx_power_user_lmt);
 	scan_power_info->requested_power = power;
 
@@ -1394,7 +1388,7 @@ static int iwl3945_send_tx_power(struct iwl_priv *priv)
 	chan = le16_to_cpu(priv->contexts[IWL_RXON_CTX_BSS].active.channel);
 
 	txpower.band = (priv->band == IEEE80211_BAND_5GHZ) ? 0 : 1;
-	ch_info = iwl_get_channel_info(priv, priv->band, chan);
+	ch_info = iwl_legacy_get_channel_info(priv, priv->band, chan);
 	if (!ch_info) {
 		IWL_ERR(priv,
 			"Failed to get channel info for channel %d [%d]\n",
@@ -1402,7 +1396,7 @@ static int iwl3945_send_tx_power(struct iwl_priv *priv)
 		return -EINVAL;
 	}
 
-	if (!is_channel_valid(ch_info)) {
+	if (!iwl_legacy_is_channel_valid(ch_info)) {
 		IWL_DEBUG_POWER(priv, "Not calling TX_PWR_TABLE_CMD on "
 				"non-Tx channel.\n");
 		return 0;
@@ -1437,7 +1431,7 @@ static int iwl3945_send_tx_power(struct iwl_priv *priv)
 				txpower.power[i].rate);
 	}
 
-	return iwl_send_cmd_pdu(priv, REPLY_TX_PWR_TABLE_CMD,
+	return iwl_legacy_send_cmd_pdu(priv, REPLY_TX_PWR_TABLE_CMD,
 				sizeof(struct iwl3945_txpowertable_cmd),
 				&txpower);
 
@@ -1571,7 +1565,7 @@ static int iwl3945_hw_reg_comp_txpower_temp(struct iwl_priv *priv)
 	/* set up new Tx power info for each and every channel, 2.4 and 5.x */
 	for (i = 0; i < priv->channel_count; i++) {
 		ch_info = &priv->channel_info[i];
-		a_band = is_channel_a_band(ch_info);
+		a_band = iwl_legacy_is_channel_a_band(ch_info);
 
 		/* Get this chnlgrp's factory calibration temperature */
 		ref_temp = (s16)eeprom->groups[ch_info->group_index].
@@ -1583,7 +1577,7 @@ static int iwl3945_hw_reg_comp_txpower_temp(struct iwl_priv *priv)
 							      ref_temp);
 
 		/* set tx power value for all rates, OFDM and CCK */
-		for (rate_index = 0; rate_index < IWL_RATE_COUNT;
+		for (rate_index = 0; rate_index < IWL_RATE_COUNT_3945;
 		     rate_index++) {
 			int power_idx =
 			    ch_info->power_info[rate_index].base_power_index;
@@ -1637,7 +1631,7 @@ int iwl3945_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
 
 	for (i = 0; i < priv->channel_count; i++) {
 		ch_info = &priv->channel_info[i];
-		a_band = is_channel_a_band(ch_info);
+		a_band = iwl_legacy_is_channel_a_band(ch_info);
 
 		/* find minimum power of all user and regulatory constraints
 		 *    (does not consider h/w clipping limitations) */
@@ -1653,7 +1647,7 @@ int iwl3945_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
 
 	/* update txpower settings for all channels,
 	 *   send to NIC if associated. */
-	is_temp_calib_needed(priv);
+	iwl3945_is_temp_calib_needed(priv);
 	iwl3945_hw_reg_comp_txpower_temp(priv);
 
 	return 0;
@@ -1671,8 +1665,8 @@ static int iwl3945_send_rxon_assoc(struct iwl_priv *priv,
 		.flags = CMD_WANT_SKB,
 		.data = &rxon_assoc,
 	};
-	const struct iwl_rxon_cmd *rxon1 = &ctx->staging;
-	const struct iwl_rxon_cmd *rxon2 = &ctx->active;
+	const struct iwl_legacy_rxon_cmd *rxon1 = &ctx->staging;
+	const struct iwl_legacy_rxon_cmd *rxon2 = &ctx->active;
 
 	if ((rxon1->flags == rxon2->flags) &&
 	    (rxon1->filter_flags == rxon2->filter_flags) &&
@@ -1688,7 +1682,7 @@ static int iwl3945_send_rxon_assoc(struct iwl_priv *priv,
 	rxon_assoc.cck_basic_rates = ctx->staging.cck_basic_rates;
 	rxon_assoc.reserved = 0;
 
-	rc = iwl_send_cmd_sync(priv, &cmd);
+	rc = iwl_legacy_send_cmd_sync(priv, &cmd);
 	if (rc)
 		return rc;
 
@@ -1698,7 +1692,7 @@ static int iwl3945_send_rxon_assoc(struct iwl_priv *priv,
 		rc = -EIO;
 	}
 
-	iwl_free_pages(priv, cmd.reply_page);
+	iwl_legacy_free_pages(priv, cmd.reply_page);
 
 	return rc;
 }
@@ -1722,7 +1716,7 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return -EINVAL;
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -1;
 
 	/* always get timestamp with Rx frame */
@@ -1733,7 +1727,7 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	    ~(RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_SEL_MSK);
 	staging_rxon->flags |= iwl3945_get_antenna_flags(priv);
 
-	rc = iwl_check_rxon_cmd(priv, ctx);
+	rc = iwl_legacy_check_rxon_cmd(priv, ctx);
 	if (rc) {
 		IWL_ERR(priv, "Invalid RXON configuration.  Not committing.\n");
 		return -EINVAL;
@@ -1742,8 +1736,9 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	/* If we don't need to send a full RXON, we can use
 	 * iwl3945_rxon_assoc_cmd which is used to reconfigure filter
 	 * and other flags for the current radio configuration. */
-	if (!iwl_full_rxon_required(priv, &priv->contexts[IWL_RXON_CTX_BSS])) {
-		rc = iwl_send_rxon_assoc(priv,
+	if (!iwl_legacy_full_rxon_required(priv,
+			&priv->contexts[IWL_RXON_CTX_BSS])) {
+		rc = iwl_legacy_send_rxon_assoc(priv,
 					 &priv->contexts[IWL_RXON_CTX_BSS]);
 		if (rc) {
 			IWL_ERR(priv, "Error setting RXON_ASSOC "
@@ -1760,7 +1755,7 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	 * an RXON_ASSOC and the new config wants the associated mask enabled,
 	 * we must clear the associated from the active configuration
 	 * before we apply the new config */
-	if (iwl_is_associated(priv, IWL_RXON_CTX_BSS) && new_assoc) {
+	if (iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS) && new_assoc) {
 		IWL_DEBUG_INFO(priv, "Toggling associated bit on current RXON\n");
 		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 
@@ -1770,7 +1765,7 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 		 */
 		active_rxon->reserved4 = 0;
 		active_rxon->reserved5 = 0;
-		rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
+		rc = iwl_legacy_send_cmd_pdu(priv, REPLY_RXON,
 				      sizeof(struct iwl3945_rxon_cmd),
 				      &priv->contexts[IWL_RXON_CTX_BSS].active);
 
@@ -1782,9 +1777,10 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 				  "configuration (%d).\n", rc);
 			return rc;
 		}
-		iwl_clear_ucode_stations(priv,
+		iwl_legacy_clear_ucode_stations(priv,
+					 &priv->contexts[IWL_RXON_CTX_BSS]);
+		iwl_legacy_restore_stations(priv,
 					 &priv->contexts[IWL_RXON_CTX_BSS]);
-		iwl_restore_stations(priv, &priv->contexts[IWL_RXON_CTX_BSS]);
 	}
 
 	IWL_DEBUG_INFO(priv, "Sending RXON\n"
@@ -1802,10 +1798,10 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	staging_rxon->reserved4 = 0;
 	staging_rxon->reserved5 = 0;
 
-	iwl_set_rxon_hwcrypto(priv, ctx, !iwl3945_mod_params.sw_crypto);
+	iwl_legacy_set_rxon_hwcrypto(priv, ctx, !iwl3945_mod_params.sw_crypto);
 
 	/* Apply the new configuration */
-	rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
+	rc = iwl_legacy_send_cmd_pdu(priv, REPLY_RXON,
 			      sizeof(struct iwl3945_rxon_cmd),
 			      staging_rxon);
 	if (rc) {
@@ -1816,14 +1812,15 @@ int iwl3945_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	memcpy(active_rxon, staging_rxon, sizeof(*active_rxon));
 
 	if (!new_assoc) {
-		iwl_clear_ucode_stations(priv,
+		iwl_legacy_clear_ucode_stations(priv,
 					 &priv->contexts[IWL_RXON_CTX_BSS]);
-		iwl_restore_stations(priv, &priv->contexts[IWL_RXON_CTX_BSS]);
+		iwl_legacy_restore_stations(priv,
+					&priv->contexts[IWL_RXON_CTX_BSS]);
 	}
 
 	/* If we issue a new RXON command which required a tune then we must
 	 * send a new TXPOWER command or we won't be able to Tx any frames */
-	rc = priv->cfg->ops->lib->send_tx_power(priv);
+	rc = iwl_legacy_set_tx_power(priv, priv->tx_power_next, true);
 	if (rc) {
 		IWL_ERR(priv, "Error setting Tx power (%d).\n", rc);
 		return rc;
@@ -1853,7 +1850,7 @@ void iwl3945_reg_txpower_periodic(struct iwl_priv *priv)
 {
 	/* This will kick in the "brute force"
 	 * iwl3945_hw_reg_comp_txpower_temp() below */
-	if (!is_temp_calib_needed(priv))
+	if (!iwl3945_is_temp_calib_needed(priv))
 		goto reschedule;
 
 	/* Set up a new set of temp-adjusted TxPowers, send to NIC.
@@ -1900,7 +1897,7 @@ static u16 iwl3945_hw_reg_get_ch_grp_index(struct iwl_priv *priv,
 	u8 grp_channel;
 
 	/* Find the group index for the channel ... don't use index 1(?) */
-	if (is_channel_a_band(ch_info)) {
+	if (iwl_legacy_is_channel_a_band(ch_info)) {
 		for (group = 1; group < 5; group++) {
 			grp_channel = ch_grp[group].group_channel;
 			if (ch_info->channel <= grp_channel) {
@@ -2080,8 +2077,8 @@ int iwl3945_txpower_set_from_eeprom(struct iwl_priv *priv)
 	/* initialize Tx power info for each and every channel, 2.4 and 5.x */
 	for (i = 0, ch_info = priv->channel_info; i < priv->channel_count;
 	     i++, ch_info++) {
-		a_band = is_channel_a_band(ch_info);
-		if (!is_channel_valid(ch_info))
+		a_band = iwl_legacy_is_channel_a_band(ch_info);
+		if (!iwl_legacy_is_channel_valid(ch_info))
 			continue;
 
 		/* find this channel's channel group (*not* "band") index */
@@ -2184,7 +2181,7 @@ int iwl3945_hw_rxq_stop(struct iwl_priv *priv)
 {
 	int rc;
 
-	iwl_write_direct32(priv, FH39_RCSR_CONFIG(0), 0);
+	iwl_legacy_write_direct32(priv, FH39_RCSR_CONFIG(0), 0);
 	rc = iwl_poll_direct_bit(priv, FH39_RSSR_STATUS,
 			FH39_RSSR_CHNL0_RX_STATUS_CHNL_IDLE, 1000);
 	if (rc < 0)
@@ -2201,10 +2198,10 @@ int iwl3945_hw_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq)
 
 	shared_data->tx_base_ptr[txq_id] = cpu_to_le32((u32)txq->q.dma_addr);
 
-	iwl_write_direct32(priv, FH39_CBCC_CTRL(txq_id), 0);
-	iwl_write_direct32(priv, FH39_CBCC_BASE(txq_id), 0);
+	iwl_legacy_write_direct32(priv, FH39_CBCC_CTRL(txq_id), 0);
+	iwl_legacy_write_direct32(priv, FH39_CBCC_BASE(txq_id), 0);
 
-	iwl_write_direct32(priv, FH39_TCSR_CONFIG(txq_id),
+	iwl_legacy_write_direct32(priv, FH39_TCSR_CONFIG(txq_id),
 		FH39_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_NOINT |
 		FH39_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_TXF |
 		FH39_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_IFTFD |
@@ -2233,7 +2230,8 @@ static u16 iwl3945_get_hcmd_size(u8 cmd_id, u16 len)
 }
 
 
-static u16 iwl3945_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
+static u16 iwl3945_build_addsta_hcmd(const struct iwl_legacy_addsta_cmd *cmd,
+								u8 *data)
 {
 	struct iwl3945_addsta_cmd *addsta = (struct iwl3945_addsta_cmd *)data;
 	addsta->mode = cmd->mode;
@@ -2261,7 +2259,7 @@ static int iwl3945_add_bssid_station(struct iwl_priv *priv,
 	if (sta_id_r)
 		*sta_id_r = IWL_INVALID_STATION;
 
-	ret = iwl_add_station_common(priv, ctx, addr, 0, NULL, &sta_id);
+	ret = iwl_legacy_add_station_common(priv, ctx, addr, 0, NULL, &sta_id);
 	if (ret) {
 		IWL_ERR(priv, "Unable to add station %pM\n", addr);
 		return ret;
@@ -2296,7 +2294,7 @@ static int iwl3945_manage_ibss_station(struct iwl_priv *priv,
 		return 0;
 	}
 
-	return iwl_remove_station(priv, vif_priv->ibss_bssid_sta_id,
+	return iwl_legacy_remove_station(priv, vif_priv->ibss_bssid_sta_id,
 				  vif->bss_conf.bssid);
 }
 
@@ -2347,7 +2345,7 @@ int iwl3945_init_hw_rate_table(struct iwl_priv *priv)
 		 * 1M CCK rates */
 
 		if (!(priv->_3945.sta_supp_rates & IWL_OFDM_RATES_MASK) &&
-		    iwl_is_associated(priv, IWL_RXON_CTX_BSS)) {
+		    iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS)) {
 
 			index = IWL_FIRST_CCK_RATE;
 			for (i = IWL_RATE_6M_INDEX_TABLE;
@@ -2368,14 +2366,14 @@ int iwl3945_init_hw_rate_table(struct iwl_priv *priv)
 
 	/* Update the rate scaling for control frame Tx */
 	rate_cmd.table_id = 0;
-	rc = iwl_send_cmd_pdu(priv, REPLY_RATE_SCALE, sizeof(rate_cmd),
+	rc = iwl_legacy_send_cmd_pdu(priv, REPLY_RATE_SCALE, sizeof(rate_cmd),
 			      &rate_cmd);
 	if (rc)
 		return rc;
 
 	/* Update the rate scaling for data frame Tx */
 	rate_cmd.table_id = 1;
-	return iwl_send_cmd_pdu(priv, REPLY_RATE_SCALE, sizeof(rate_cmd),
+	return iwl_legacy_send_cmd_pdu(priv, REPLY_RATE_SCALE, sizeof(rate_cmd),
 				&rate_cmd);
 }
 
@@ -2475,11 +2473,11 @@ static int iwl3945_verify_bsm(struct iwl_priv *priv)
 	IWL_DEBUG_INFO(priv, "Begin verify bsm\n");
 
 	/* verify BSM SRAM contents */
-	val = iwl_read_prph(priv, BSM_WR_DWCOUNT_REG);
+	val = iwl_legacy_read_prph(priv, BSM_WR_DWCOUNT_REG);
 	for (reg = BSM_SRAM_LOWER_BOUND;
 	     reg < BSM_SRAM_LOWER_BOUND + len;
 	     reg += sizeof(u32), image++) {
-		val = iwl_read_prph(priv, reg);
+		val = iwl_legacy_read_prph(priv, reg);
 		if (val != le32_to_cpu(*image)) {
 			IWL_ERR(priv, "BSM uCode verification failed at "
 				  "addr 0x%08X+%u (of %u), is 0x%x, s/b 0x%x\n",
@@ -2512,7 +2510,7 @@ static int iwl3945_verify_bsm(struct iwl_priv *priv)
  */
 static int iwl3945_eeprom_acquire_semaphore(struct iwl_priv *priv)
 {
-	_iwl_clear_bit(priv, CSR_EEPROM_GP, CSR_EEPROM_GP_IF_OWNER_MSK);
+	_iwl_legacy_clear_bit(priv, CSR_EEPROM_GP, CSR_EEPROM_GP_IF_OWNER_MSK);
 	return 0;
 }
 
@@ -2583,16 +2581,16 @@ static int iwl3945_load_bsm(struct iwl_priv *priv)
 	inst_len = priv->ucode_init.len;
 	data_len = priv->ucode_init_data.len;
 
-	iwl_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
-	iwl_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
-	iwl_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG, inst_len);
-	iwl_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG, data_len);
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG, inst_len);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG, data_len);
 
 	/* Fill BSM memory with bootstrap instructions */
 	for (reg_offset = BSM_SRAM_LOWER_BOUND;
 	     reg_offset < BSM_SRAM_LOWER_BOUND + len;
 	     reg_offset += sizeof(u32), image++)
-		_iwl_write_prph(priv, reg_offset,
+		_iwl_legacy_write_prph(priv, reg_offset,
 					  le32_to_cpu(*image));
 
 	rc = iwl3945_verify_bsm(priv);
@@ -2600,19 +2598,19 @@ static int iwl3945_load_bsm(struct iwl_priv *priv)
 		return rc;
 
 	/* Tell BSM to copy from BSM SRAM into instruction SRAM, when asked */
-	iwl_write_prph(priv, BSM_WR_MEM_SRC_REG, 0x0);
-	iwl_write_prph(priv, BSM_WR_MEM_DST_REG,
+	iwl_legacy_write_prph(priv, BSM_WR_MEM_SRC_REG, 0x0);
+	iwl_legacy_write_prph(priv, BSM_WR_MEM_DST_REG,
 				 IWL39_RTC_INST_LOWER_BOUND);
-	iwl_write_prph(priv, BSM_WR_DWCOUNT_REG, len / sizeof(u32));
+	iwl_legacy_write_prph(priv, BSM_WR_DWCOUNT_REG, len / sizeof(u32));
 
 	/* Load bootstrap code into instruction SRAM now,
 	 *   to prepare to load "initialize" uCode */
-	iwl_write_prph(priv, BSM_WR_CTRL_REG,
+	iwl_legacy_write_prph(priv, BSM_WR_CTRL_REG,
 		BSM_WR_CTRL_REG_BIT_START);
 
 	/* Wait for load of bootstrap uCode to finish */
 	for (i = 0; i < 100; i++) {
-		done = iwl_read_prph(priv, BSM_WR_CTRL_REG);
+		done = iwl_legacy_read_prph(priv, BSM_WR_CTRL_REG);
 		if (!(done & BSM_WR_CTRL_REG_BIT_START))
 			break;
 		udelay(10);
@@ -2626,7 +2624,7 @@ static int iwl3945_load_bsm(struct iwl_priv *priv)
 
 	/* Enable future boot loads whenever power management unit triggers it
 	 *   (e.g. when powering back up after power-save shutdown) */
-	iwl_write_prph(priv, BSM_WR_CTRL_REG,
+	iwl_legacy_write_prph(priv, BSM_WR_CTRL_REG,
 		BSM_WR_CTRL_REG_BIT_START_EN);
 
 	return 0;
@@ -2635,7 +2633,6 @@ static int iwl3945_load_bsm(struct iwl_priv *priv)
 static struct iwl_hcmd_ops iwl3945_hcmd = {
 	.rxon_assoc = iwl3945_send_rxon_assoc,
 	.commit_rxon = iwl3945_commit_rxon,
-	.send_bt_config = iwl_send_bt_config,
 };
 
 static struct iwl_lib_ops iwl3945_lib = {
@@ -2661,13 +2658,9 @@ static struct iwl_lib_ops iwl3945_lib = {
 		},
 		.acquire_semaphore = iwl3945_eeprom_acquire_semaphore,
 		.release_semaphore = iwl3945_eeprom_release_semaphore,
-		.query_addr = iwlcore_eeprom_query_addr,
 	},
 	.send_tx_power	= iwl3945_send_tx_power,
 	.is_valid_rtc_data_addr = iwl3945_hw_valid_rtc_data_addr,
-	.isr_ops = {
-		.isr = iwl_isr_legacy,
-	},
 
 	.debugfs_ops = {
 		.rx_stats_read = iwl3945_ucode_rx_stats_read,
@@ -2685,7 +2678,6 @@ static const struct iwl_legacy_ops iwl3945_legacy_ops = {
 static struct iwl_hcmd_utils_ops iwl3945_hcmd_utils = {
 	.get_hcmd_size = iwl3945_get_hcmd_size,
 	.build_addsta_hcmd = iwl3945_build_addsta_hcmd,
-	.tx_cmd_protection = iwl_legacy_tx_cmd_protection,
 	.request_scan = iwl3945_request_scan,
 	.post_scan = iwl3945_post_scan,
 };
@@ -2705,13 +2697,10 @@ static struct iwl_base_params iwl3945_base_params = {
 	.pll_cfg_val = CSR39_ANA_PLL_CFG_VAL,
 	.set_l0s = false,
 	.use_bsm = true,
-	.use_isr_legacy = true,
 	.led_compensation = 64,
-	.broken_powersave = true,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF,
 	.wd_timeout = IWL_DEF_WD_TIMEOUT,
 	.max_event_log_size = 512,
-	.tx_power_by_driver = true,
 };
 
 static struct iwl_cfg iwl3945_bg_cfg = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.h b/drivers/net/wireless/iwlegacy/iwl-3945.h
index 3eef1eb74a78..b118b59b71de 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.h
+++ b/drivers/net/wireless/iwlegacy/iwl-3945.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2003 - 2011 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
@@ -108,7 +108,7 @@ struct iwl3945_rs_sta {
 
 /*
  * The common struct MUST be first because it is shared between
- * 3945 and agn!
+ * 3945 and 4965!
  */
 struct iwl3945_sta_priv {
 	struct iwl_station_priv_common common;
@@ -201,7 +201,7 @@ struct iwl3945_ibss_seq {
 
 /******************************************************************************
  *
- * Functions implemented in iwl-base.c which are forward declared here
+ * Functions implemented in iwl3945-base.c which are forward declared here
  * for use by iwl-*.c
  *
  *****************************************************************************/
@@ -209,7 +209,7 @@ extern int iwl3945_calc_db_from_ratio(int sig_ratio);
 extern void iwl3945_rx_replenish(void *data);
 extern void iwl3945_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
 extern unsigned int iwl3945_fill_beacon_frame(struct iwl_priv *priv,
-					struct ieee80211_hdr *hdr,int left);
+					struct ieee80211_hdr *hdr, int left);
 extern int iwl3945_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
 				       char **buf, bool display);
 extern void iwl3945_dump_nic_error_log(struct iwl_priv *priv);
@@ -217,7 +217,7 @@ extern void iwl3945_dump_nic_error_log(struct iwl_priv *priv);
 /******************************************************************************
  *
  * Functions implemented in iwl-[34]*.c which are forward declared here
- * for use by iwl-base.c
+ * for use by iwl3945-base.c
  *
  * NOTE:  The implementation of these functions are hardware specific
  * which is why they are in the hardware specific files (vs. iwl-base.c)
@@ -283,7 +283,7 @@ extern u8 iwl3945_hw_find_station(struct iwl_priv *priv, const u8 *bssid);
 extern struct ieee80211_ops iwl3945_hw_ops;
 
 /*
- * Forward declare iwl-3945.c functions for iwl-base.c
+ * Forward declare iwl-3945.c functions for iwl3945-base.c
  */
 extern __le32 iwl3945_get_antenna_flags(const struct iwl_priv *priv);
 extern int iwl3945_init_hw_rate_table(struct iwl_priv *priv);
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-calib.c b/drivers/net/wireless/iwlegacy/iwl-4965-calib.c
new file mode 100644
index 000000000000..81d6a25eb04f
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-calib.c
@@ -0,0 +1,967 @@
+/******************************************************************************
+ *
+ * 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 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#include <linux/slab.h>
+#include <net/mac80211.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-4965-calib.h"
+
+/*****************************************************************************
+ * INIT calibrations framework
+ *****************************************************************************/
+
+struct statistics_general_data {
+	u32 beacon_silence_rssi_a;
+	u32 beacon_silence_rssi_b;
+	u32 beacon_silence_rssi_c;
+	u32 beacon_energy_a;
+	u32 beacon_energy_b;
+	u32 beacon_energy_c;
+};
+
+void iwl4965_calib_free_results(struct iwl_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < IWL_CALIB_MAX; i++) {
+		kfree(priv->calib_results[i].buf);
+		priv->calib_results[i].buf = NULL;
+		priv->calib_results[i].buf_len = 0;
+	}
+}
+
+/*****************************************************************************
+ * RUNTIME calibrations framework
+ *****************************************************************************/
+
+/* "false alarms" are signals that our DSP tries to lock onto,
+ *   but then determines that they are either noise, or transmissions
+ *   from a distant wireless network (also "noise", really) that get
+ *   "stepped on" by stronger transmissions within our own network.
+ * This algorithm attempts to set a sensitivity level that is high
+ *   enough to receive all of our own network traffic, but not so
+ *   high that our DSP gets too busy trying to lock onto non-network
+ *   activity/noise. */
+static int iwl4965_sens_energy_cck(struct iwl_priv *priv,
+				   u32 norm_fa,
+				   u32 rx_enable_time,
+				   struct statistics_general_data *rx_info)
+{
+	u32 max_nrg_cck = 0;
+	int i = 0;
+	u8 max_silence_rssi = 0;
+	u32 silence_ref = 0;
+	u8 silence_rssi_a = 0;
+	u8 silence_rssi_b = 0;
+	u8 silence_rssi_c = 0;
+	u32 val;
+
+	/* "false_alarms" values below are cross-multiplications to assess the
+	 *   numbers of false alarms within the measured period of actual Rx
+	 *   (Rx is off when we're txing), vs the min/max expected false alarms
+	 *   (some should be expected if rx is sensitive enough) in a
+	 *   hypothetical listening period of 200 time units (TU), 204.8 msec:
+	 *
+	 * MIN_FA/fixed-time < false_alarms/actual-rx-time < MAX_FA/beacon-time
+	 *
+	 * */
+	u32 false_alarms = norm_fa * 200 * 1024;
+	u32 max_false_alarms = MAX_FA_CCK * rx_enable_time;
+	u32 min_false_alarms = MIN_FA_CCK * rx_enable_time;
+	struct iwl_sensitivity_data *data = NULL;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
+
+	data = &(priv->sensitivity_data);
+
+	data->nrg_auto_corr_silence_diff = 0;
+
+	/* Find max silence rssi among all 3 receivers.
+	 * This is background noise, which may include transmissions from other
+	 *    networks, measured during silence before our network's beacon */
+	silence_rssi_a = (u8)((rx_info->beacon_silence_rssi_a &
+			    ALL_BAND_FILTER) >> 8);
+	silence_rssi_b = (u8)((rx_info->beacon_silence_rssi_b &
+			    ALL_BAND_FILTER) >> 8);
+	silence_rssi_c = (u8)((rx_info->beacon_silence_rssi_c &
+			    ALL_BAND_FILTER) >> 8);
+
+	val = max(silence_rssi_b, silence_rssi_c);
+	max_silence_rssi = max(silence_rssi_a, (u8) val);
+
+	/* Store silence rssi in 20-beacon history table */
+	data->nrg_silence_rssi[data->nrg_silence_idx] = max_silence_rssi;
+	data->nrg_silence_idx++;
+	if (data->nrg_silence_idx >= NRG_NUM_PREV_STAT_L)
+		data->nrg_silence_idx = 0;
+
+	/* Find max silence rssi across 20 beacon history */
+	for (i = 0; i < NRG_NUM_PREV_STAT_L; i++) {
+		val = data->nrg_silence_rssi[i];
+		silence_ref = max(silence_ref, val);
+	}
+	IWL_DEBUG_CALIB(priv, "silence a %u, b %u, c %u, 20-bcn max %u\n",
+			silence_rssi_a, silence_rssi_b, silence_rssi_c,
+			silence_ref);
+
+	/* Find max rx energy (min value!) among all 3 receivers,
+	 *   measured during beacon frame.
+	 * Save it in 10-beacon history table. */
+	i = data->nrg_energy_idx;
+	val = min(rx_info->beacon_energy_b, rx_info->beacon_energy_c);
+	data->nrg_value[i] = min(rx_info->beacon_energy_a, val);
+
+	data->nrg_energy_idx++;
+	if (data->nrg_energy_idx >= 10)
+		data->nrg_energy_idx = 0;
+
+	/* Find min rx energy (max value) across 10 beacon history.
+	 * This is the minimum signal level that we want to receive well.
+	 * Add backoff (margin so we don't miss slightly lower energy frames).
+	 * This establishes an upper bound (min value) for energy threshold. */
+	max_nrg_cck = data->nrg_value[0];
+	for (i = 1; i < 10; i++)
+		max_nrg_cck = (u32) max(max_nrg_cck, (data->nrg_value[i]));
+	max_nrg_cck += 6;
+
+	IWL_DEBUG_CALIB(priv, "rx energy a %u, b %u, c %u, 10-bcn max/min %u\n",
+			rx_info->beacon_energy_a, rx_info->beacon_energy_b,
+			rx_info->beacon_energy_c, max_nrg_cck - 6);
+
+	/* Count number of consecutive beacons with fewer-than-desired
+	 *   false alarms. */
+	if (false_alarms < min_false_alarms)
+		data->num_in_cck_no_fa++;
+	else
+		data->num_in_cck_no_fa = 0;
+	IWL_DEBUG_CALIB(priv, "consecutive bcns with few false alarms = %u\n",
+			data->num_in_cck_no_fa);
+
+	/* If we got too many false alarms this time, reduce sensitivity */
+	if ((false_alarms > max_false_alarms) &&
+		(data->auto_corr_cck > AUTO_CORR_MAX_TH_CCK)) {
+		IWL_DEBUG_CALIB(priv, "norm FA %u > max FA %u\n",
+		     false_alarms, max_false_alarms);
+		IWL_DEBUG_CALIB(priv, "... reducing sensitivity\n");
+		data->nrg_curr_state = IWL_FA_TOO_MANY;
+		/* Store for "fewer than desired" on later beacon */
+		data->nrg_silence_ref = silence_ref;
+
+		/* increase energy threshold (reduce nrg value)
+		 *   to decrease sensitivity */
+		data->nrg_th_cck = data->nrg_th_cck - NRG_STEP_CCK;
+	/* Else if we got fewer than desired, increase sensitivity */
+	} else if (false_alarms < min_false_alarms) {
+		data->nrg_curr_state = IWL_FA_TOO_FEW;
+
+		/* Compare silence level with silence level for most recent
+		 *   healthy number or too many false alarms */
+		data->nrg_auto_corr_silence_diff = (s32)data->nrg_silence_ref -
+						   (s32)silence_ref;
+
+		IWL_DEBUG_CALIB(priv,
+			 "norm FA %u < min FA %u, silence diff %d\n",
+			 false_alarms, min_false_alarms,
+			 data->nrg_auto_corr_silence_diff);
+
+		/* Increase value to increase sensitivity, but only if:
+		 * 1a) previous beacon did *not* have *too many* false alarms
+		 * 1b) AND there's a significant difference in Rx levels
+		 *      from a previous beacon with too many, or healthy # FAs
+		 * OR 2) We've seen a lot of beacons (100) with too few
+		 *       false alarms */
+		if ((data->nrg_prev_state != IWL_FA_TOO_MANY) &&
+			((data->nrg_auto_corr_silence_diff > NRG_DIFF) ||
+			(data->num_in_cck_no_fa > MAX_NUMBER_CCK_NO_FA))) {
+
+			IWL_DEBUG_CALIB(priv, "... increasing sensitivity\n");
+			/* Increase nrg value to increase sensitivity */
+			val = data->nrg_th_cck + NRG_STEP_CCK;
+			data->nrg_th_cck = min((u32)ranges->min_nrg_cck, val);
+		} else {
+			IWL_DEBUG_CALIB(priv,
+					 "... but not changing sensitivity\n");
+		}
+
+	/* Else we got a healthy number of false alarms, keep status quo */
+	} else {
+		IWL_DEBUG_CALIB(priv, " FA in safe zone\n");
+		data->nrg_curr_state = IWL_FA_GOOD_RANGE;
+
+		/* Store for use in "fewer than desired" with later beacon */
+		data->nrg_silence_ref = silence_ref;
+
+		/* 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) {
+			IWL_DEBUG_CALIB(priv, "... increasing margin\n");
+			if (data->nrg_th_cck > (max_nrg_cck + NRG_MARGIN))
+				data->nrg_th_cck -= NRG_MARGIN;
+			else
+				data->nrg_th_cck = max_nrg_cck;
+		}
+	}
+
+	/* Make sure the energy threshold does not go above the measured
+	 * energy of the desired Rx signals (reduced by backoff margin),
+	 * or else we might start missing Rx frames.
+	 * Lower value is higher energy, so we use max()!
+	 */
+	data->nrg_th_cck = max(max_nrg_cck, data->nrg_th_cck);
+	IWL_DEBUG_CALIB(priv, "new nrg_th_cck %u\n", data->nrg_th_cck);
+
+	data->nrg_prev_state = data->nrg_curr_state;
+
+	/* Auto-correlation CCK algorithm */
+	if (false_alarms > min_false_alarms) {
+
+		/* increase auto_corr values to decrease sensitivity
+		 * so the DSP won't be disturbed by the noise
+		 */
+		if (data->auto_corr_cck < AUTO_CORR_MAX_TH_CCK)
+			data->auto_corr_cck = AUTO_CORR_MAX_TH_CCK + 1;
+		else {
+			val = data->auto_corr_cck + AUTO_CORR_STEP_CCK;
+			data->auto_corr_cck =
+				min((u32)ranges->auto_corr_max_cck, val);
+		}
+		val = data->auto_corr_cck_mrc + AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck_mrc =
+			min((u32)ranges->auto_corr_max_cck_mrc, val);
+	} else if ((false_alarms < min_false_alarms) &&
+	   ((data->nrg_auto_corr_silence_diff > NRG_DIFF) ||
+	   (data->num_in_cck_no_fa > MAX_NUMBER_CCK_NO_FA))) {
+
+		/* Decrease auto_corr values to increase sensitivity */
+		val = data->auto_corr_cck - AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck =
+			max((u32)ranges->auto_corr_min_cck, val);
+		val = data->auto_corr_cck_mrc - AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck_mrc =
+			max((u32)ranges->auto_corr_min_cck_mrc, val);
+	}
+
+	return 0;
+}
+
+
+static int iwl4965_sens_auto_corr_ofdm(struct iwl_priv *priv,
+				       u32 norm_fa,
+				       u32 rx_enable_time)
+{
+	u32 val;
+	u32 false_alarms = norm_fa * 200 * 1024;
+	u32 max_false_alarms = MAX_FA_OFDM * rx_enable_time;
+	u32 min_false_alarms = MIN_FA_OFDM * rx_enable_time;
+	struct iwl_sensitivity_data *data = NULL;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
+
+	data = &(priv->sensitivity_data);
+
+	/* If we got too many false alarms this time, reduce sensitivity */
+	if (false_alarms > max_false_alarms) {
+
+		IWL_DEBUG_CALIB(priv, "norm FA %u > max FA %u)\n",
+			     false_alarms, max_false_alarms);
+
+		val = data->auto_corr_ofdm + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm =
+			min((u32)ranges->auto_corr_max_ofdm, val);
+
+		val = data->auto_corr_ofdm_mrc + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc =
+			min((u32)ranges->auto_corr_max_ofdm_mrc, val);
+
+		val = data->auto_corr_ofdm_x1 + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_x1 =
+			min((u32)ranges->auto_corr_max_ofdm_x1, val);
+
+		val = data->auto_corr_ofdm_mrc_x1 + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc_x1 =
+			min((u32)ranges->auto_corr_max_ofdm_mrc_x1, val);
+	}
+
+	/* Else if we got fewer than desired, increase sensitivity */
+	else if (false_alarms < min_false_alarms) {
+
+		IWL_DEBUG_CALIB(priv, "norm FA %u < min FA %u\n",
+			     false_alarms, min_false_alarms);
+
+		val = data->auto_corr_ofdm - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm =
+			max((u32)ranges->auto_corr_min_ofdm, val);
+
+		val = data->auto_corr_ofdm_mrc - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc =
+			max((u32)ranges->auto_corr_min_ofdm_mrc, val);
+
+		val = data->auto_corr_ofdm_x1 - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_x1 =
+			max((u32)ranges->auto_corr_min_ofdm_x1, val);
+
+		val = data->auto_corr_ofdm_mrc_x1 - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc_x1 =
+			max((u32)ranges->auto_corr_min_ofdm_mrc_x1, val);
+	} else {
+		IWL_DEBUG_CALIB(priv, "min FA %u < norm FA %u < max FA %u OK\n",
+			 min_false_alarms, false_alarms, max_false_alarms);
+	}
+	return 0;
+}
+
+static void iwl4965_prepare_legacy_sensitivity_tbl(struct iwl_priv *priv,
+				struct iwl_sensitivity_data *data,
+				__le16 *tbl)
+{
+	tbl[HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm);
+	tbl[HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm_mrc);
+	tbl[HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm_x1);
+	tbl[HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm_mrc_x1);
+
+	tbl[HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_cck);
+	tbl[HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_cck_mrc);
+
+	tbl[HD_MIN_ENERGY_CCK_DET_INDEX] =
+				cpu_to_le16((u16)data->nrg_th_cck);
+	tbl[HD_MIN_ENERGY_OFDM_DET_INDEX] =
+				cpu_to_le16((u16)data->nrg_th_ofdm);
+
+	tbl[HD_BARKER_CORR_TH_ADD_MIN_INDEX] =
+				cpu_to_le16(data->barker_corr_th_min);
+	tbl[HD_BARKER_CORR_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16(data->barker_corr_th_min_mrc);
+	tbl[HD_OFDM_ENERGY_TH_IN_INDEX] =
+				cpu_to_le16(data->nrg_th_cca);
+
+	IWL_DEBUG_CALIB(priv, "ofdm: ac %u mrc %u x1 %u mrc_x1 %u thresh %u\n",
+			data->auto_corr_ofdm, data->auto_corr_ofdm_mrc,
+			data->auto_corr_ofdm_x1, data->auto_corr_ofdm_mrc_x1,
+			data->nrg_th_ofdm);
+
+	IWL_DEBUG_CALIB(priv, "cck: ac %u mrc %u thresh %u\n",
+			data->auto_corr_cck, data->auto_corr_cck_mrc,
+			data->nrg_th_cck);
+}
+
+/* Prepare a SENSITIVITY_CMD, send to uCode if values have changed */
+static int iwl4965_sensitivity_write(struct iwl_priv *priv)
+{
+	struct iwl_sensitivity_cmd cmd;
+	struct iwl_sensitivity_data *data = NULL;
+	struct iwl_host_cmd cmd_out = {
+		.id = SENSITIVITY_CMD,
+		.len = sizeof(struct iwl_sensitivity_cmd),
+		.flags = CMD_ASYNC,
+		.data = &cmd,
+	};
+
+	data = &(priv->sensitivity_data);
+
+	memset(&cmd, 0, sizeof(cmd));
+
+	iwl4965_prepare_legacy_sensitivity_tbl(priv, data, &cmd.table[0]);
+
+	/* Update uCode's "work" table, and copy it to DSP */
+	cmd.control = SENSITIVITY_CMD_CONTROL_WORK_TABLE;
+
+	/* Don't send command to uCode if nothing has changed */
+	if (!memcmp(&cmd.table[0], &(priv->sensitivity_tbl[0]),
+		    sizeof(u16)*HD_TABLE_SIZE)) {
+		IWL_DEBUG_CALIB(priv, "No change in SENSITIVITY_CMD\n");
+		return 0;
+	}
+
+	/* Copy table for comparison next time */
+	memcpy(&(priv->sensitivity_tbl[0]), &(cmd.table[0]),
+	       sizeof(u16)*HD_TABLE_SIZE);
+
+	return iwl_legacy_send_cmd(priv, &cmd_out);
+}
+
+void iwl4965_init_sensitivity(struct iwl_priv *priv)
+{
+	int ret = 0;
+	int i;
+	struct iwl_sensitivity_data *data = NULL;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
+
+	if (priv->disable_sens_cal)
+		return;
+
+	IWL_DEBUG_CALIB(priv, "Start iwl4965_init_sensitivity\n");
+
+	/* Clear driver's sensitivity algo data */
+	data = &(priv->sensitivity_data);
+
+	if (ranges == NULL)
+		return;
+
+	memset(data, 0, sizeof(struct iwl_sensitivity_data));
+
+	data->num_in_cck_no_fa = 0;
+	data->nrg_curr_state = IWL_FA_TOO_MANY;
+	data->nrg_prev_state = IWL_FA_TOO_MANY;
+	data->nrg_silence_ref = 0;
+	data->nrg_silence_idx = 0;
+	data->nrg_energy_idx = 0;
+
+	for (i = 0; i < 10; i++)
+		data->nrg_value[i] = 0;
+
+	for (i = 0; i < NRG_NUM_PREV_STAT_L; i++)
+		data->nrg_silence_rssi[i] = 0;
+
+	data->auto_corr_ofdm =  ranges->auto_corr_min_ofdm;
+	data->auto_corr_ofdm_mrc = ranges->auto_corr_min_ofdm_mrc;
+	data->auto_corr_ofdm_x1  = ranges->auto_corr_min_ofdm_x1;
+	data->auto_corr_ofdm_mrc_x1 = ranges->auto_corr_min_ofdm_mrc_x1;
+	data->auto_corr_cck = AUTO_CORR_CCK_MIN_VAL_DEF;
+	data->auto_corr_cck_mrc = ranges->auto_corr_min_cck_mrc;
+	data->nrg_th_cck = ranges->nrg_th_cck;
+	data->nrg_th_ofdm = ranges->nrg_th_ofdm;
+	data->barker_corr_th_min = ranges->barker_corr_th_min;
+	data->barker_corr_th_min_mrc = ranges->barker_corr_th_min_mrc;
+	data->nrg_th_cca = ranges->nrg_th_cca;
+
+	data->last_bad_plcp_cnt_ofdm = 0;
+	data->last_fa_cnt_ofdm = 0;
+	data->last_bad_plcp_cnt_cck = 0;
+	data->last_fa_cnt_cck = 0;
+
+	ret |= iwl4965_sensitivity_write(priv);
+	IWL_DEBUG_CALIB(priv, "<<return 0x%X\n", ret);
+}
+
+void iwl4965_sensitivity_calibration(struct iwl_priv *priv, void *resp)
+{
+	u32 rx_enable_time;
+	u32 fa_cck;
+	u32 fa_ofdm;
+	u32 bad_plcp_cck;
+	u32 bad_plcp_ofdm;
+	u32 norm_fa_ofdm;
+	u32 norm_fa_cck;
+	struct iwl_sensitivity_data *data = NULL;
+	struct statistics_rx_non_phy *rx_info;
+	struct statistics_rx_phy *ofdm, *cck;
+	unsigned long flags;
+	struct statistics_general_data statis;
+
+	if (priv->disable_sens_cal)
+		return;
+
+	data = &(priv->sensitivity_data);
+
+	if (!iwl_legacy_is_any_associated(priv)) {
+		IWL_DEBUG_CALIB(priv, "<< - not associated\n");
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	rx_info = &(((struct iwl_notif_statistics *)resp)->rx.general);
+	ofdm = &(((struct iwl_notif_statistics *)resp)->rx.ofdm);
+	cck = &(((struct iwl_notif_statistics *)resp)->rx.cck);
+
+	if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
+		IWL_DEBUG_CALIB(priv, "<< invalid data.\n");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	/* Extract Statistics: */
+	rx_enable_time = le32_to_cpu(rx_info->channel_load);
+	fa_cck = le32_to_cpu(cck->false_alarm_cnt);
+	fa_ofdm = le32_to_cpu(ofdm->false_alarm_cnt);
+	bad_plcp_cck = le32_to_cpu(cck->plcp_err);
+	bad_plcp_ofdm = le32_to_cpu(ofdm->plcp_err);
+
+	statis.beacon_silence_rssi_a =
+			le32_to_cpu(rx_info->beacon_silence_rssi_a);
+	statis.beacon_silence_rssi_b =
+			le32_to_cpu(rx_info->beacon_silence_rssi_b);
+	statis.beacon_silence_rssi_c =
+			le32_to_cpu(rx_info->beacon_silence_rssi_c);
+	statis.beacon_energy_a =
+			le32_to_cpu(rx_info->beacon_energy_a);
+	statis.beacon_energy_b =
+			le32_to_cpu(rx_info->beacon_energy_b);
+	statis.beacon_energy_c =
+			le32_to_cpu(rx_info->beacon_energy_c);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_CALIB(priv, "rx_enable_time = %u usecs\n", rx_enable_time);
+
+	if (!rx_enable_time) {
+		IWL_DEBUG_CALIB(priv, "<< RX Enable Time == 0!\n");
+		return;
+	}
+
+	/* These statistics increase monotonically, and do not reset
+	 *   at each beacon.  Calculate difference from last value, or just
+	 *   use the new statistics value if it has reset or wrapped around. */
+	if (data->last_bad_plcp_cnt_cck > bad_plcp_cck)
+		data->last_bad_plcp_cnt_cck = bad_plcp_cck;
+	else {
+		bad_plcp_cck -= data->last_bad_plcp_cnt_cck;
+		data->last_bad_plcp_cnt_cck += bad_plcp_cck;
+	}
+
+	if (data->last_bad_plcp_cnt_ofdm > bad_plcp_ofdm)
+		data->last_bad_plcp_cnt_ofdm = bad_plcp_ofdm;
+	else {
+		bad_plcp_ofdm -= data->last_bad_plcp_cnt_ofdm;
+		data->last_bad_plcp_cnt_ofdm += bad_plcp_ofdm;
+	}
+
+	if (data->last_fa_cnt_ofdm > fa_ofdm)
+		data->last_fa_cnt_ofdm = fa_ofdm;
+	else {
+		fa_ofdm -= data->last_fa_cnt_ofdm;
+		data->last_fa_cnt_ofdm += fa_ofdm;
+	}
+
+	if (data->last_fa_cnt_cck > fa_cck)
+		data->last_fa_cnt_cck = fa_cck;
+	else {
+		fa_cck -= data->last_fa_cnt_cck;
+		data->last_fa_cnt_cck += fa_cck;
+	}
+
+	/* Total aborted signal locks */
+	norm_fa_ofdm = fa_ofdm + bad_plcp_ofdm;
+	norm_fa_cck = fa_cck + bad_plcp_cck;
+
+	IWL_DEBUG_CALIB(priv,
+			 "cck: fa %u badp %u  ofdm: fa %u badp %u\n", fa_cck,
+			bad_plcp_cck, fa_ofdm, bad_plcp_ofdm);
+
+	iwl4965_sens_auto_corr_ofdm(priv, norm_fa_ofdm, rx_enable_time);
+	iwl4965_sens_energy_cck(priv, norm_fa_cck, rx_enable_time, &statis);
+
+	iwl4965_sensitivity_write(priv);
+}
+
+static inline u8 iwl4965_find_first_chain(u8 mask)
+{
+	if (mask & ANT_A)
+		return CHAIN_A;
+	if (mask & ANT_B)
+		return CHAIN_B;
+	return CHAIN_C;
+}
+
+/**
+ * Run disconnected antenna algorithm to find out which antennas are
+ * disconnected.
+ */
+static void
+iwl4965_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
+				     struct iwl_chain_noise_data *data)
+{
+	u32 active_chains = 0;
+	u32 max_average_sig;
+	u16 max_average_sig_antenna_i;
+	u8 num_tx_chains;
+	u8 first_chain;
+	u16 i = 0;
+
+	average_sig[0] = data->chain_signal_a /
+			 priv->cfg->base_params->chain_noise_num_beacons;
+	average_sig[1] = data->chain_signal_b /
+			 priv->cfg->base_params->chain_noise_num_beacons;
+	average_sig[2] = data->chain_signal_c /
+			 priv->cfg->base_params->chain_noise_num_beacons;
+
+	if (average_sig[0] >= average_sig[1]) {
+		max_average_sig = average_sig[0];
+		max_average_sig_antenna_i = 0;
+		active_chains = (1 << max_average_sig_antenna_i);
+	} else {
+		max_average_sig = average_sig[1];
+		max_average_sig_antenna_i = 1;
+		active_chains = (1 << max_average_sig_antenna_i);
+	}
+
+	if (average_sig[2] >= max_average_sig) {
+		max_average_sig = average_sig[2];
+		max_average_sig_antenna_i = 2;
+		active_chains = (1 << max_average_sig_antenna_i);
+	}
+
+	IWL_DEBUG_CALIB(priv, "average_sig: a %d b %d c %d\n",
+		     average_sig[0], average_sig[1], average_sig[2]);
+	IWL_DEBUG_CALIB(priv, "max_average_sig = %d, antenna %d\n",
+		     max_average_sig, max_average_sig_antenna_i);
+
+	/* Compare signal strengths for all 3 receivers. */
+	for (i = 0; i < NUM_RX_CHAINS; i++) {
+		if (i != max_average_sig_antenna_i) {
+			s32 rssi_delta = (max_average_sig - average_sig[i]);
+
+			/* If signal is very weak, compared with
+			 * strongest, mark it as disconnected. */
+			if (rssi_delta > MAXIMUM_ALLOWED_PATHLOSS)
+				data->disconn_array[i] = 1;
+			else
+				active_chains |= (1 << i);
+			IWL_DEBUG_CALIB(priv, "i = %d  rssiDelta = %d  "
+			     "disconn_array[i] = %d\n",
+			     i, rssi_delta, data->disconn_array[i]);
+		}
+	}
+
+	/*
+	 * The above algorithm sometimes fails when the ucode
+	 * reports 0 for all chains. It's not clear why that
+	 * happens to start with, but it is then causing trouble
+	 * because this can make us enable more chains than the
+	 * hardware really has.
+	 *
+	 * To be safe, simply mask out any chains that we know
+	 * are not on the device.
+	 */
+	active_chains &= priv->hw_params.valid_rx_ant;
+
+	num_tx_chains = 0;
+	for (i = 0; i < NUM_RX_CHAINS; i++) {
+		/* loops on all the bits of
+		 * priv->hw_setting.valid_tx_ant */
+		u8 ant_msk = (1 << i);
+		if (!(priv->hw_params.valid_tx_ant & ant_msk))
+			continue;
+
+		num_tx_chains++;
+		if (data->disconn_array[i] == 0)
+			/* there is a Tx antenna connected */
+			break;
+		if (num_tx_chains == priv->hw_params.tx_chains_num &&
+		    data->disconn_array[i]) {
+			/*
+			 * If all chains are disconnected
+			 * connect the first valid tx chain
+			 */
+			first_chain =
+			iwl4965_find_first_chain(priv->cfg->valid_tx_ant);
+			data->disconn_array[first_chain] = 0;
+			active_chains |= BIT(first_chain);
+			IWL_DEBUG_CALIB(priv, "All Tx chains are disconnected \
+					W/A - declare %d as connected\n",
+					first_chain);
+			break;
+		}
+	}
+
+	if (active_chains != priv->hw_params.valid_rx_ant &&
+	    active_chains != priv->chain_noise_data.active_chains)
+		IWL_DEBUG_CALIB(priv,
+				"Detected that not all antennas are connected! "
+				"Connected: %#x, valid: %#x.\n",
+				active_chains, priv->hw_params.valid_rx_ant);
+
+	/* Save for use within RXON, TX, SCAN commands, etc. */
+	data->active_chains = active_chains;
+	IWL_DEBUG_CALIB(priv, "active_chains (bitwise) = 0x%x\n",
+			active_chains);
+}
+
+static void iwl4965_gain_computation(struct iwl_priv *priv,
+		u32 *average_noise,
+		u16 min_average_noise_antenna_i,
+		u32 min_average_noise,
+		u8 default_chain)
+{
+	int i, ret;
+	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
+
+	data->delta_gain_code[min_average_noise_antenna_i] = 0;
+
+	for (i = default_chain; i < NUM_RX_CHAINS; i++) {
+		s32 delta_g = 0;
+
+		if (!(data->disconn_array[i]) &&
+		    (data->delta_gain_code[i] ==
+			     CHAIN_NOISE_DELTA_GAIN_INIT_VAL)) {
+			delta_g = average_noise[i] - min_average_noise;
+			data->delta_gain_code[i] = (u8)((delta_g * 10) / 15);
+			data->delta_gain_code[i] =
+				min(data->delta_gain_code[i],
+				(u8) CHAIN_NOISE_MAX_DELTA_GAIN_CODE);
+
+			data->delta_gain_code[i] =
+				(data->delta_gain_code[i] | (1 << 2));
+		} else {
+			data->delta_gain_code[i] = 0;
+		}
+	}
+	IWL_DEBUG_CALIB(priv, "delta_gain_codes: a %d b %d c %d\n",
+		     data->delta_gain_code[0],
+		     data->delta_gain_code[1],
+		     data->delta_gain_code[2]);
+
+	/* Differential gain gets sent to uCode only once */
+	if (!data->radio_write) {
+		struct iwl_calib_diff_gain_cmd cmd;
+		data->radio_write = 1;
+
+		memset(&cmd, 0, sizeof(cmd));
+		cmd.hdr.op_code = IWL_PHY_CALIBRATE_DIFF_GAIN_CMD;
+		cmd.diff_gain_a = data->delta_gain_code[0];
+		cmd.diff_gain_b = data->delta_gain_code[1];
+		cmd.diff_gain_c = data->delta_gain_code[2];
+		ret = iwl_legacy_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+				      sizeof(cmd), &cmd);
+		if (ret)
+			IWL_DEBUG_CALIB(priv, "fail sending cmd "
+				     "REPLY_PHY_CALIBRATION_CMD\n");
+
+		/* TODO we might want recalculate
+		 * rx_chain in rxon cmd */
+
+		/* Mark so we run this algo only once! */
+		data->state = IWL_CHAIN_NOISE_CALIBRATED;
+	}
+}
+
+
+
+/*
+ * Accumulate 16 beacons of signal and noise statistics for each of
+ *   3 receivers/antennas/rx-chains, then figure out:
+ * 1)  Which antennas are connected.
+ * 2)  Differential rx gain settings to balance the 3 receivers.
+ */
+void iwl4965_chain_noise_calibration(struct iwl_priv *priv, void *stat_resp)
+{
+	struct iwl_chain_noise_data *data = NULL;
+
+	u32 chain_noise_a;
+	u32 chain_noise_b;
+	u32 chain_noise_c;
+	u32 chain_sig_a;
+	u32 chain_sig_b;
+	u32 chain_sig_c;
+	u32 average_sig[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
+	u32 average_noise[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
+	u32 min_average_noise = MIN_AVERAGE_NOISE_MAX_VALUE;
+	u16 min_average_noise_antenna_i = INITIALIZATION_VALUE;
+	u16 i = 0;
+	u16 rxon_chnum = INITIALIZATION_VALUE;
+	u16 stat_chnum = INITIALIZATION_VALUE;
+	u8 rxon_band24;
+	u8 stat_band24;
+	unsigned long flags;
+	struct statistics_rx_non_phy *rx_info;
+
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	if (priv->disable_chain_noise_cal)
+		return;
+
+	data = &(priv->chain_noise_data);
+
+	/*
+	 * Accumulate just the first "chain_noise_num_beacons" after
+	 * the first association, then we're done forever.
+	 */
+	if (data->state != IWL_CHAIN_NOISE_ACCUMULATE) {
+		if (data->state == IWL_CHAIN_NOISE_ALIVE)
+			IWL_DEBUG_CALIB(priv, "Wait for noise calib reset\n");
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	rx_info = &(((struct iwl_notif_statistics *)stat_resp)->
+		      rx.general);
+
+	if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
+		IWL_DEBUG_CALIB(priv, " << Interference data unavailable\n");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	rxon_band24 = !!(ctx->staging.flags & RXON_FLG_BAND_24G_MSK);
+	rxon_chnum = le16_to_cpu(ctx->staging.channel);
+
+	stat_band24 = !!(((struct iwl_notif_statistics *)
+			 stat_resp)->flag &
+			 STATISTICS_REPLY_FLG_BAND_24G_MSK);
+	stat_chnum = le32_to_cpu(((struct iwl_notif_statistics *)
+				 stat_resp)->flag) >> 16;
+
+	/* Make sure we accumulate data for just the associated channel
+	 *   (even if scanning). */
+	if ((rxon_chnum != stat_chnum) || (rxon_band24 != stat_band24)) {
+		IWL_DEBUG_CALIB(priv, "Stats not from chan=%d, band24=%d\n",
+				rxon_chnum, rxon_band24);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	/*
+	 *  Accumulate beacon statistics values across
+	 * "chain_noise_num_beacons"
+	 */
+	chain_noise_a = le32_to_cpu(rx_info->beacon_silence_rssi_a) &
+				IN_BAND_FILTER;
+	chain_noise_b = le32_to_cpu(rx_info->beacon_silence_rssi_b) &
+				IN_BAND_FILTER;
+	chain_noise_c = le32_to_cpu(rx_info->beacon_silence_rssi_c) &
+				IN_BAND_FILTER;
+
+	chain_sig_a = le32_to_cpu(rx_info->beacon_rssi_a) & IN_BAND_FILTER;
+	chain_sig_b = le32_to_cpu(rx_info->beacon_rssi_b) & IN_BAND_FILTER;
+	chain_sig_c = le32_to_cpu(rx_info->beacon_rssi_c) & IN_BAND_FILTER;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	data->beacon_count++;
+
+	data->chain_noise_a = (chain_noise_a + data->chain_noise_a);
+	data->chain_noise_b = (chain_noise_b + data->chain_noise_b);
+	data->chain_noise_c = (chain_noise_c + data->chain_noise_c);
+
+	data->chain_signal_a = (chain_sig_a + data->chain_signal_a);
+	data->chain_signal_b = (chain_sig_b + data->chain_signal_b);
+	data->chain_signal_c = (chain_sig_c + data->chain_signal_c);
+
+	IWL_DEBUG_CALIB(priv, "chan=%d, band24=%d, beacon=%d\n",
+			rxon_chnum, rxon_band24, data->beacon_count);
+	IWL_DEBUG_CALIB(priv, "chain_sig: a %d b %d c %d\n",
+			chain_sig_a, chain_sig_b, chain_sig_c);
+	IWL_DEBUG_CALIB(priv, "chain_noise: a %d b %d c %d\n",
+			chain_noise_a, chain_noise_b, chain_noise_c);
+
+	/* If this is the "chain_noise_num_beacons", determine:
+	 * 1)  Disconnected antennas (using signal strengths)
+	 * 2)  Differential gain (using silence noise) to balance receivers */
+	if (data->beacon_count !=
+		priv->cfg->base_params->chain_noise_num_beacons)
+		return;
+
+	/* Analyze signal for disconnected antenna */
+	iwl4965_find_disconn_antenna(priv, average_sig, data);
+
+	/* Analyze noise for rx balance */
+	average_noise[0] = data->chain_noise_a /
+			   priv->cfg->base_params->chain_noise_num_beacons;
+	average_noise[1] = data->chain_noise_b /
+			   priv->cfg->base_params->chain_noise_num_beacons;
+	average_noise[2] = data->chain_noise_c /
+			   priv->cfg->base_params->chain_noise_num_beacons;
+
+	for (i = 0; i < NUM_RX_CHAINS; i++) {
+		if (!(data->disconn_array[i]) &&
+		   (average_noise[i] <= min_average_noise)) {
+			/* This means that chain i is active and has
+			 * lower noise values so far: */
+			min_average_noise = average_noise[i];
+			min_average_noise_antenna_i = i;
+		}
+	}
+
+	IWL_DEBUG_CALIB(priv, "average_noise: a %d b %d c %d\n",
+			average_noise[0], average_noise[1],
+			average_noise[2]);
+
+	IWL_DEBUG_CALIB(priv, "min_average_noise = %d, antenna %d\n",
+			min_average_noise, min_average_noise_antenna_i);
+
+	iwl4965_gain_computation(priv, average_noise,
+			min_average_noise_antenna_i, min_average_noise,
+			iwl4965_find_first_chain(priv->cfg->valid_rx_ant));
+
+	/* Some power changes may have been made during the calibration.
+	 * Update and commit the RXON
+	 */
+	if (priv->cfg->ops->lib->update_chain_flags)
+		priv->cfg->ops->lib->update_chain_flags(priv);
+
+	data->state = IWL_CHAIN_NOISE_DONE;
+	iwl_legacy_power_update_mode(priv, false);
+}
+
+void iwl4965_reset_run_time_calib(struct iwl_priv *priv)
+{
+	int i;
+	memset(&(priv->sensitivity_data), 0,
+	       sizeof(struct iwl_sensitivity_data));
+	memset(&(priv->chain_noise_data), 0,
+	       sizeof(struct iwl_chain_noise_data));
+	for (i = 0; i < NUM_RX_CHAINS; i++)
+		priv->chain_noise_data.delta_gain_code[i] =
+				CHAIN_NOISE_DELTA_GAIN_INIT_VAL;
+
+	/* Ask for statistics now, the uCode will send notification
+	 * periodically after association */
+	iwl_legacy_send_statistics_request(priv, CMD_ASYNC, true);
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-legacy.h b/drivers/net/wireless/iwlegacy/iwl-4965-calib.h
index 9f7b2f935964..f46c80e6e005 100644
--- a/drivers/net/wireless/iwlwifi/iwl-legacy.h
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-calib.h
@@ -5,7 +5,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2008 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2008 - 2011 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
@@ -30,7 +30,7 @@
  *
  * BSD LICENSE
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -59,21 +59,17 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *****************************************************************************/
+#ifndef __iwl_4965_calib_h__
+#define __iwl_4965_calib_h__
 
-#ifndef __iwl_legacy_h__
-#define __iwl_legacy_h__
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-commands.h"
 
-/* mac80211 handlers */
-int iwl_legacy_mac_config(struct ieee80211_hw *hw, u32 changed);
-void iwl_legacy_mac_reset_tsf(struct ieee80211_hw *hw);
-void iwl_legacy_mac_bss_info_changed(struct ieee80211_hw *hw,
-				     struct ieee80211_vif *vif,
-				     struct ieee80211_bss_conf *bss_conf,
-				     u32 changes);
-void iwl_legacy_tx_cmd_protection(struct iwl_priv *priv,
-				struct ieee80211_tx_info *info,
-				__le16 fc, __le32 *tx_flags);
+void iwl4965_chain_noise_calibration(struct iwl_priv *priv, void *stat_resp);
+void iwl4965_sensitivity_calibration(struct iwl_priv *priv, void *resp);
+void iwl4965_init_sensitivity(struct iwl_priv *priv);
+void iwl4965_reset_run_time_calib(struct iwl_priv *priv);
+void iwl4965_calib_free_results(struct iwl_priv *priv);
 
-irqreturn_t iwl_isr_legacy(int irq, void *data);
-
-#endif /* __iwl_legacy_h__ */
+#endif /* __iwl_4965_calib_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-debugfs.c b/drivers/net/wireless/iwlegacy/iwl-4965-debugfs.c
new file mode 100644
index 000000000000..1c93665766e4
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-debugfs.c
@@ -0,0 +1,774 @@
+/******************************************************************************
+*
+* GPL LICENSE SUMMARY
+*
+* Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+* General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program; if not, write to the Free Software
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+* USA
+*
+* The full GNU General Public License is included in this distribution
+* in the file called LICENSE.GPL.
+*
+* Contact Information:
+*  Intel Linux Wireless <ilw@linux.intel.com>
+* Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+*****************************************************************************/
+#include "iwl-4965.h"
+#include "iwl-4965-debugfs.h"
+
+static const char *fmt_value = "  %-30s %10u\n";
+static const char *fmt_table = "  %-30s %10u  %10u  %10u  %10u\n";
+static const char *fmt_header =
+	"%-32s    current  cumulative       delta         max\n";
+
+static int iwl4965_statistics_flag(struct iwl_priv *priv, char *buf, int bufsz)
+{
+	int p = 0;
+	u32 flag;
+
+	flag = le32_to_cpu(priv->_4965.statistics.flag);
+
+	p += scnprintf(buf + p, bufsz - p, "Statistics Flag(0x%X):\n", flag);
+	if (flag & UCODE_STATISTICS_CLEAR_MSK)
+		p += scnprintf(buf + p, bufsz - p,
+		"\tStatistics have been cleared\n");
+	p += scnprintf(buf + p, bufsz - p, "\tOperational Frequency: %s\n",
+		(flag & UCODE_STATISTICS_FREQUENCY_MSK)
+		? "2.4 GHz" : "5.2 GHz");
+	p += scnprintf(buf + p, bufsz - p, "\tTGj Narrow Band: %s\n",
+		(flag & UCODE_STATISTICS_NARROW_BAND_MSK)
+		 ? "enabled" : "disabled");
+
+	return p;
+}
+
+ssize_t iwl4965_ucode_rx_stats_read(struct file *file, char __user *user_buf,
+				size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	char *buf;
+	int bufsz = sizeof(struct statistics_rx_phy) * 40 +
+		    sizeof(struct statistics_rx_non_phy) * 40 +
+		    sizeof(struct statistics_rx_ht_phy) * 40 + 400;
+	ssize_t ret;
+	struct statistics_rx_phy *ofdm, *accum_ofdm, *delta_ofdm, *max_ofdm;
+	struct statistics_rx_phy *cck, *accum_cck, *delta_cck, *max_cck;
+	struct statistics_rx_non_phy *general, *accum_general;
+	struct statistics_rx_non_phy *delta_general, *max_general;
+	struct statistics_rx_ht_phy *ht, *accum_ht, *delta_ht, *max_ht;
+
+	if (!iwl_legacy_is_alive(priv))
+		return -EAGAIN;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * the statistic information display here is based on
+	 * the last statistics notification from uCode
+	 * might not reflect the current uCode activity
+	 */
+	ofdm = &priv->_4965.statistics.rx.ofdm;
+	cck = &priv->_4965.statistics.rx.cck;
+	general = &priv->_4965.statistics.rx.general;
+	ht = &priv->_4965.statistics.rx.ofdm_ht;
+	accum_ofdm = &priv->_4965.accum_statistics.rx.ofdm;
+	accum_cck = &priv->_4965.accum_statistics.rx.cck;
+	accum_general = &priv->_4965.accum_statistics.rx.general;
+	accum_ht = &priv->_4965.accum_statistics.rx.ofdm_ht;
+	delta_ofdm = &priv->_4965.delta_statistics.rx.ofdm;
+	delta_cck = &priv->_4965.delta_statistics.rx.cck;
+	delta_general = &priv->_4965.delta_statistics.rx.general;
+	delta_ht = &priv->_4965.delta_statistics.rx.ofdm_ht;
+	max_ofdm = &priv->_4965.max_delta.rx.ofdm;
+	max_cck = &priv->_4965.max_delta.rx.cck;
+	max_general = &priv->_4965.max_delta.rx.general;
+	max_ht = &priv->_4965.max_delta.rx.ofdm_ht;
+
+	pos += iwl4965_statistics_flag(priv, buf, bufsz);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_header, "Statistics_Rx - OFDM:");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "ina_cnt:",
+			 le32_to_cpu(ofdm->ina_cnt),
+			 accum_ofdm->ina_cnt,
+			 delta_ofdm->ina_cnt, max_ofdm->ina_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "fina_cnt:",
+			 le32_to_cpu(ofdm->fina_cnt), accum_ofdm->fina_cnt,
+			 delta_ofdm->fina_cnt, max_ofdm->fina_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "plcp_err:",
+			 le32_to_cpu(ofdm->plcp_err), accum_ofdm->plcp_err,
+			 delta_ofdm->plcp_err, max_ofdm->plcp_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "crc32_err:",
+			 le32_to_cpu(ofdm->crc32_err), accum_ofdm->crc32_err,
+			 delta_ofdm->crc32_err, max_ofdm->crc32_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "overrun_err:",
+			 le32_to_cpu(ofdm->overrun_err),
+			 accum_ofdm->overrun_err, delta_ofdm->overrun_err,
+			 max_ofdm->overrun_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "early_overrun_err:",
+			 le32_to_cpu(ofdm->early_overrun_err),
+			 accum_ofdm->early_overrun_err,
+			 delta_ofdm->early_overrun_err,
+			 max_ofdm->early_overrun_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "crc32_good:",
+			 le32_to_cpu(ofdm->crc32_good),
+			 accum_ofdm->crc32_good, delta_ofdm->crc32_good,
+			 max_ofdm->crc32_good);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "false_alarm_cnt:",
+			 le32_to_cpu(ofdm->false_alarm_cnt),
+			 accum_ofdm->false_alarm_cnt,
+			 delta_ofdm->false_alarm_cnt,
+			 max_ofdm->false_alarm_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "fina_sync_err_cnt:",
+			 le32_to_cpu(ofdm->fina_sync_err_cnt),
+			 accum_ofdm->fina_sync_err_cnt,
+			 delta_ofdm->fina_sync_err_cnt,
+			 max_ofdm->fina_sync_err_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sfd_timeout:",
+			 le32_to_cpu(ofdm->sfd_timeout),
+			 accum_ofdm->sfd_timeout, delta_ofdm->sfd_timeout,
+			 max_ofdm->sfd_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "fina_timeout:",
+			 le32_to_cpu(ofdm->fina_timeout),
+			 accum_ofdm->fina_timeout, delta_ofdm->fina_timeout,
+			 max_ofdm->fina_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "unresponded_rts:",
+			 le32_to_cpu(ofdm->unresponded_rts),
+			 accum_ofdm->unresponded_rts,
+			 delta_ofdm->unresponded_rts,
+			 max_ofdm->unresponded_rts);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "rxe_frame_lmt_ovrun:",
+			 le32_to_cpu(ofdm->rxe_frame_limit_overrun),
+			 accum_ofdm->rxe_frame_limit_overrun,
+			 delta_ofdm->rxe_frame_limit_overrun,
+			 max_ofdm->rxe_frame_limit_overrun);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sent_ack_cnt:",
+			 le32_to_cpu(ofdm->sent_ack_cnt),
+			 accum_ofdm->sent_ack_cnt, delta_ofdm->sent_ack_cnt,
+			 max_ofdm->sent_ack_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sent_cts_cnt:",
+			 le32_to_cpu(ofdm->sent_cts_cnt),
+			 accum_ofdm->sent_cts_cnt, delta_ofdm->sent_cts_cnt,
+			 max_ofdm->sent_cts_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sent_ba_rsp_cnt:",
+			 le32_to_cpu(ofdm->sent_ba_rsp_cnt),
+			 accum_ofdm->sent_ba_rsp_cnt,
+			 delta_ofdm->sent_ba_rsp_cnt,
+			 max_ofdm->sent_ba_rsp_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "dsp_self_kill:",
+			 le32_to_cpu(ofdm->dsp_self_kill),
+			 accum_ofdm->dsp_self_kill,
+			 delta_ofdm->dsp_self_kill,
+			 max_ofdm->dsp_self_kill);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "mh_format_err:",
+			 le32_to_cpu(ofdm->mh_format_err),
+			 accum_ofdm->mh_format_err,
+			 delta_ofdm->mh_format_err,
+			 max_ofdm->mh_format_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "re_acq_main_rssi_sum:",
+			 le32_to_cpu(ofdm->re_acq_main_rssi_sum),
+			 accum_ofdm->re_acq_main_rssi_sum,
+			 delta_ofdm->re_acq_main_rssi_sum,
+			 max_ofdm->re_acq_main_rssi_sum);
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_header, "Statistics_Rx - CCK:");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "ina_cnt:",
+			 le32_to_cpu(cck->ina_cnt), accum_cck->ina_cnt,
+			 delta_cck->ina_cnt, max_cck->ina_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "fina_cnt:",
+			 le32_to_cpu(cck->fina_cnt), accum_cck->fina_cnt,
+			 delta_cck->fina_cnt, max_cck->fina_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "plcp_err:",
+			 le32_to_cpu(cck->plcp_err), accum_cck->plcp_err,
+			 delta_cck->plcp_err, max_cck->plcp_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "crc32_err:",
+			 le32_to_cpu(cck->crc32_err), accum_cck->crc32_err,
+			 delta_cck->crc32_err, max_cck->crc32_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "overrun_err:",
+			 le32_to_cpu(cck->overrun_err),
+			 accum_cck->overrun_err, delta_cck->overrun_err,
+			 max_cck->overrun_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "early_overrun_err:",
+			 le32_to_cpu(cck->early_overrun_err),
+			 accum_cck->early_overrun_err,
+			 delta_cck->early_overrun_err,
+			 max_cck->early_overrun_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "crc32_good:",
+			 le32_to_cpu(cck->crc32_good), accum_cck->crc32_good,
+			 delta_cck->crc32_good, max_cck->crc32_good);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "false_alarm_cnt:",
+			 le32_to_cpu(cck->false_alarm_cnt),
+			 accum_cck->false_alarm_cnt,
+			 delta_cck->false_alarm_cnt, max_cck->false_alarm_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "fina_sync_err_cnt:",
+			 le32_to_cpu(cck->fina_sync_err_cnt),
+			 accum_cck->fina_sync_err_cnt,
+			 delta_cck->fina_sync_err_cnt,
+			 max_cck->fina_sync_err_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sfd_timeout:",
+			 le32_to_cpu(cck->sfd_timeout),
+			 accum_cck->sfd_timeout, delta_cck->sfd_timeout,
+			 max_cck->sfd_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "fina_timeout:",
+			 le32_to_cpu(cck->fina_timeout),
+			 accum_cck->fina_timeout, delta_cck->fina_timeout,
+			 max_cck->fina_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "unresponded_rts:",
+			 le32_to_cpu(cck->unresponded_rts),
+			 accum_cck->unresponded_rts, delta_cck->unresponded_rts,
+			 max_cck->unresponded_rts);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "rxe_frame_lmt_ovrun:",
+			 le32_to_cpu(cck->rxe_frame_limit_overrun),
+			 accum_cck->rxe_frame_limit_overrun,
+			 delta_cck->rxe_frame_limit_overrun,
+			 max_cck->rxe_frame_limit_overrun);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sent_ack_cnt:",
+			 le32_to_cpu(cck->sent_ack_cnt),
+			 accum_cck->sent_ack_cnt, delta_cck->sent_ack_cnt,
+			 max_cck->sent_ack_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sent_cts_cnt:",
+			 le32_to_cpu(cck->sent_cts_cnt),
+			 accum_cck->sent_cts_cnt, delta_cck->sent_cts_cnt,
+			 max_cck->sent_cts_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sent_ba_rsp_cnt:",
+			 le32_to_cpu(cck->sent_ba_rsp_cnt),
+			 accum_cck->sent_ba_rsp_cnt,
+			 delta_cck->sent_ba_rsp_cnt,
+			 max_cck->sent_ba_rsp_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "dsp_self_kill:",
+			 le32_to_cpu(cck->dsp_self_kill),
+			 accum_cck->dsp_self_kill, delta_cck->dsp_self_kill,
+			 max_cck->dsp_self_kill);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "mh_format_err:",
+			 le32_to_cpu(cck->mh_format_err),
+			 accum_cck->mh_format_err, delta_cck->mh_format_err,
+			 max_cck->mh_format_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "re_acq_main_rssi_sum:",
+			 le32_to_cpu(cck->re_acq_main_rssi_sum),
+			 accum_cck->re_acq_main_rssi_sum,
+			 delta_cck->re_acq_main_rssi_sum,
+			 max_cck->re_acq_main_rssi_sum);
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_header, "Statistics_Rx - GENERAL:");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "bogus_cts:",
+			 le32_to_cpu(general->bogus_cts),
+			 accum_general->bogus_cts, delta_general->bogus_cts,
+			 max_general->bogus_cts);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "bogus_ack:",
+			 le32_to_cpu(general->bogus_ack),
+			 accum_general->bogus_ack, delta_general->bogus_ack,
+			 max_general->bogus_ack);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "non_bssid_frames:",
+			 le32_to_cpu(general->non_bssid_frames),
+			 accum_general->non_bssid_frames,
+			 delta_general->non_bssid_frames,
+			 max_general->non_bssid_frames);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "filtered_frames:",
+			 le32_to_cpu(general->filtered_frames),
+			 accum_general->filtered_frames,
+			 delta_general->filtered_frames,
+			 max_general->filtered_frames);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "non_channel_beacons:",
+			 le32_to_cpu(general->non_channel_beacons),
+			 accum_general->non_channel_beacons,
+			 delta_general->non_channel_beacons,
+			 max_general->non_channel_beacons);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "channel_beacons:",
+			 le32_to_cpu(general->channel_beacons),
+			 accum_general->channel_beacons,
+			 delta_general->channel_beacons,
+			 max_general->channel_beacons);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "num_missed_bcon:",
+			 le32_to_cpu(general->num_missed_bcon),
+			 accum_general->num_missed_bcon,
+			 delta_general->num_missed_bcon,
+			 max_general->num_missed_bcon);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "adc_rx_saturation_time:",
+			 le32_to_cpu(general->adc_rx_saturation_time),
+			 accum_general->adc_rx_saturation_time,
+			 delta_general->adc_rx_saturation_time,
+			 max_general->adc_rx_saturation_time);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "ina_detect_search_tm:",
+			 le32_to_cpu(general->ina_detection_search_time),
+			 accum_general->ina_detection_search_time,
+			 delta_general->ina_detection_search_time,
+			 max_general->ina_detection_search_time);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_silence_rssi_a:",
+			 le32_to_cpu(general->beacon_silence_rssi_a),
+			 accum_general->beacon_silence_rssi_a,
+			 delta_general->beacon_silence_rssi_a,
+			 max_general->beacon_silence_rssi_a);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_silence_rssi_b:",
+			 le32_to_cpu(general->beacon_silence_rssi_b),
+			 accum_general->beacon_silence_rssi_b,
+			 delta_general->beacon_silence_rssi_b,
+			 max_general->beacon_silence_rssi_b);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_silence_rssi_c:",
+			 le32_to_cpu(general->beacon_silence_rssi_c),
+			 accum_general->beacon_silence_rssi_c,
+			 delta_general->beacon_silence_rssi_c,
+			 max_general->beacon_silence_rssi_c);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "interference_data_flag:",
+			 le32_to_cpu(general->interference_data_flag),
+			 accum_general->interference_data_flag,
+			 delta_general->interference_data_flag,
+			 max_general->interference_data_flag);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "channel_load:",
+			 le32_to_cpu(general->channel_load),
+			 accum_general->channel_load,
+			 delta_general->channel_load,
+			 max_general->channel_load);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "dsp_false_alarms:",
+			 le32_to_cpu(general->dsp_false_alarms),
+			 accum_general->dsp_false_alarms,
+			 delta_general->dsp_false_alarms,
+			 max_general->dsp_false_alarms);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_rssi_a:",
+			 le32_to_cpu(general->beacon_rssi_a),
+			 accum_general->beacon_rssi_a,
+			 delta_general->beacon_rssi_a,
+			 max_general->beacon_rssi_a);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_rssi_b:",
+			 le32_to_cpu(general->beacon_rssi_b),
+			 accum_general->beacon_rssi_b,
+			 delta_general->beacon_rssi_b,
+			 max_general->beacon_rssi_b);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_rssi_c:",
+			 le32_to_cpu(general->beacon_rssi_c),
+			 accum_general->beacon_rssi_c,
+			 delta_general->beacon_rssi_c,
+			 max_general->beacon_rssi_c);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_energy_a:",
+			 le32_to_cpu(general->beacon_energy_a),
+			 accum_general->beacon_energy_a,
+			 delta_general->beacon_energy_a,
+			 max_general->beacon_energy_a);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_energy_b:",
+			 le32_to_cpu(general->beacon_energy_b),
+			 accum_general->beacon_energy_b,
+			 delta_general->beacon_energy_b,
+			 max_general->beacon_energy_b);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "beacon_energy_c:",
+			 le32_to_cpu(general->beacon_energy_c),
+			 accum_general->beacon_energy_c,
+			 delta_general->beacon_energy_c,
+			 max_general->beacon_energy_c);
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_header, "Statistics_Rx - OFDM_HT:");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "plcp_err:",
+			 le32_to_cpu(ht->plcp_err), accum_ht->plcp_err,
+			 delta_ht->plcp_err, max_ht->plcp_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "overrun_err:",
+			 le32_to_cpu(ht->overrun_err), accum_ht->overrun_err,
+			 delta_ht->overrun_err, max_ht->overrun_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "early_overrun_err:",
+			 le32_to_cpu(ht->early_overrun_err),
+			 accum_ht->early_overrun_err,
+			 delta_ht->early_overrun_err,
+			 max_ht->early_overrun_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "crc32_good:",
+			 le32_to_cpu(ht->crc32_good), accum_ht->crc32_good,
+			 delta_ht->crc32_good, max_ht->crc32_good);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "crc32_err:",
+			 le32_to_cpu(ht->crc32_err), accum_ht->crc32_err,
+			 delta_ht->crc32_err, max_ht->crc32_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "mh_format_err:",
+			 le32_to_cpu(ht->mh_format_err),
+			 accum_ht->mh_format_err,
+			 delta_ht->mh_format_err, max_ht->mh_format_err);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg_crc32_good:",
+			 le32_to_cpu(ht->agg_crc32_good),
+			 accum_ht->agg_crc32_good,
+			 delta_ht->agg_crc32_good, max_ht->agg_crc32_good);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg_mpdu_cnt:",
+			 le32_to_cpu(ht->agg_mpdu_cnt),
+			 accum_ht->agg_mpdu_cnt,
+			 delta_ht->agg_mpdu_cnt, max_ht->agg_mpdu_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg_cnt:",
+			 le32_to_cpu(ht->agg_cnt), accum_ht->agg_cnt,
+			 delta_ht->agg_cnt, max_ht->agg_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "unsupport_mcs:",
+			 le32_to_cpu(ht->unsupport_mcs),
+			 accum_ht->unsupport_mcs,
+			 delta_ht->unsupport_mcs, max_ht->unsupport_mcs);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+ssize_t iwl4965_ucode_tx_stats_read(struct file *file,
+				char __user *user_buf,
+				size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	char *buf;
+	int bufsz = (sizeof(struct statistics_tx) * 48) + 250;
+	ssize_t ret;
+	struct statistics_tx *tx, *accum_tx, *delta_tx, *max_tx;
+
+	if (!iwl_legacy_is_alive(priv))
+		return -EAGAIN;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	/* the statistic information display here is based on
+	  * the last statistics notification from uCode
+	  * might not reflect the current uCode activity
+	  */
+	tx = &priv->_4965.statistics.tx;
+	accum_tx = &priv->_4965.accum_statistics.tx;
+	delta_tx = &priv->_4965.delta_statistics.tx;
+	max_tx = &priv->_4965.max_delta.tx;
+
+	pos += iwl4965_statistics_flag(priv, buf, bufsz);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_header, "Statistics_Tx:");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "preamble:",
+			 le32_to_cpu(tx->preamble_cnt),
+			 accum_tx->preamble_cnt,
+			 delta_tx->preamble_cnt, max_tx->preamble_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "rx_detected_cnt:",
+			 le32_to_cpu(tx->rx_detected_cnt),
+			 accum_tx->rx_detected_cnt,
+			 delta_tx->rx_detected_cnt, max_tx->rx_detected_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "bt_prio_defer_cnt:",
+			 le32_to_cpu(tx->bt_prio_defer_cnt),
+			 accum_tx->bt_prio_defer_cnt,
+			 delta_tx->bt_prio_defer_cnt,
+			 max_tx->bt_prio_defer_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "bt_prio_kill_cnt:",
+			 le32_to_cpu(tx->bt_prio_kill_cnt),
+			 accum_tx->bt_prio_kill_cnt,
+			 delta_tx->bt_prio_kill_cnt,
+			 max_tx->bt_prio_kill_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "few_bytes_cnt:",
+			 le32_to_cpu(tx->few_bytes_cnt),
+			 accum_tx->few_bytes_cnt,
+			 delta_tx->few_bytes_cnt, max_tx->few_bytes_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "cts_timeout:",
+			 le32_to_cpu(tx->cts_timeout), accum_tx->cts_timeout,
+			 delta_tx->cts_timeout, max_tx->cts_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "ack_timeout:",
+			 le32_to_cpu(tx->ack_timeout),
+			 accum_tx->ack_timeout,
+			 delta_tx->ack_timeout, max_tx->ack_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "expected_ack_cnt:",
+			 le32_to_cpu(tx->expected_ack_cnt),
+			 accum_tx->expected_ack_cnt,
+			 delta_tx->expected_ack_cnt,
+			 max_tx->expected_ack_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "actual_ack_cnt:",
+			 le32_to_cpu(tx->actual_ack_cnt),
+			 accum_tx->actual_ack_cnt,
+			 delta_tx->actual_ack_cnt,
+			 max_tx->actual_ack_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "dump_msdu_cnt:",
+			 le32_to_cpu(tx->dump_msdu_cnt),
+			 accum_tx->dump_msdu_cnt,
+			 delta_tx->dump_msdu_cnt,
+			 max_tx->dump_msdu_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "abort_nxt_frame_mismatch:",
+			 le32_to_cpu(tx->burst_abort_next_frame_mismatch_cnt),
+			 accum_tx->burst_abort_next_frame_mismatch_cnt,
+			 delta_tx->burst_abort_next_frame_mismatch_cnt,
+			 max_tx->burst_abort_next_frame_mismatch_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "abort_missing_nxt_frame:",
+			 le32_to_cpu(tx->burst_abort_missing_next_frame_cnt),
+			 accum_tx->burst_abort_missing_next_frame_cnt,
+			 delta_tx->burst_abort_missing_next_frame_cnt,
+			 max_tx->burst_abort_missing_next_frame_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "cts_timeout_collision:",
+			 le32_to_cpu(tx->cts_timeout_collision),
+			 accum_tx->cts_timeout_collision,
+			 delta_tx->cts_timeout_collision,
+			 max_tx->cts_timeout_collision);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "ack_ba_timeout_collision:",
+			 le32_to_cpu(tx->ack_or_ba_timeout_collision),
+			 accum_tx->ack_or_ba_timeout_collision,
+			 delta_tx->ack_or_ba_timeout_collision,
+			 max_tx->ack_or_ba_timeout_collision);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg ba_timeout:",
+			 le32_to_cpu(tx->agg.ba_timeout),
+			 accum_tx->agg.ba_timeout,
+			 delta_tx->agg.ba_timeout,
+			 max_tx->agg.ba_timeout);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg ba_resched_frames:",
+			 le32_to_cpu(tx->agg.ba_reschedule_frames),
+			 accum_tx->agg.ba_reschedule_frames,
+			 delta_tx->agg.ba_reschedule_frames,
+			 max_tx->agg.ba_reschedule_frames);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg scd_query_agg_frame:",
+			 le32_to_cpu(tx->agg.scd_query_agg_frame_cnt),
+			 accum_tx->agg.scd_query_agg_frame_cnt,
+			 delta_tx->agg.scd_query_agg_frame_cnt,
+			 max_tx->agg.scd_query_agg_frame_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg scd_query_no_agg:",
+			 le32_to_cpu(tx->agg.scd_query_no_agg),
+			 accum_tx->agg.scd_query_no_agg,
+			 delta_tx->agg.scd_query_no_agg,
+			 max_tx->agg.scd_query_no_agg);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg scd_query_agg:",
+			 le32_to_cpu(tx->agg.scd_query_agg),
+			 accum_tx->agg.scd_query_agg,
+			 delta_tx->agg.scd_query_agg,
+			 max_tx->agg.scd_query_agg);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg scd_query_mismatch:",
+			 le32_to_cpu(tx->agg.scd_query_mismatch),
+			 accum_tx->agg.scd_query_mismatch,
+			 delta_tx->agg.scd_query_mismatch,
+			 max_tx->agg.scd_query_mismatch);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg frame_not_ready:",
+			 le32_to_cpu(tx->agg.frame_not_ready),
+			 accum_tx->agg.frame_not_ready,
+			 delta_tx->agg.frame_not_ready,
+			 max_tx->agg.frame_not_ready);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg underrun:",
+			 le32_to_cpu(tx->agg.underrun),
+			 accum_tx->agg.underrun,
+			 delta_tx->agg.underrun, max_tx->agg.underrun);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg bt_prio_kill:",
+			 le32_to_cpu(tx->agg.bt_prio_kill),
+			 accum_tx->agg.bt_prio_kill,
+			 delta_tx->agg.bt_prio_kill,
+			 max_tx->agg.bt_prio_kill);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "agg rx_ba_rsp_cnt:",
+			 le32_to_cpu(tx->agg.rx_ba_rsp_cnt),
+			 accum_tx->agg.rx_ba_rsp_cnt,
+			 delta_tx->agg.rx_ba_rsp_cnt,
+			 max_tx->agg.rx_ba_rsp_cnt);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+ssize_t
+iwl4965_ucode_general_stats_read(struct file *file, char __user *user_buf,
+				     size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	char *buf;
+	int bufsz = sizeof(struct statistics_general) * 10 + 300;
+	ssize_t ret;
+	struct statistics_general_common *general, *accum_general;
+	struct statistics_general_common *delta_general, *max_general;
+	struct statistics_dbg *dbg, *accum_dbg, *delta_dbg, *max_dbg;
+	struct statistics_div *div, *accum_div, *delta_div, *max_div;
+
+	if (!iwl_legacy_is_alive(priv))
+		return -EAGAIN;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	/* the statistic information display here is based on
+	  * the last statistics notification from uCode
+	  * might not reflect the current uCode activity
+	  */
+	general = &priv->_4965.statistics.general.common;
+	dbg = &priv->_4965.statistics.general.common.dbg;
+	div = &priv->_4965.statistics.general.common.div;
+	accum_general = &priv->_4965.accum_statistics.general.common;
+	accum_dbg = &priv->_4965.accum_statistics.general.common.dbg;
+	accum_div = &priv->_4965.accum_statistics.general.common.div;
+	delta_general = &priv->_4965.delta_statistics.general.common;
+	max_general = &priv->_4965.max_delta.general.common;
+	delta_dbg = &priv->_4965.delta_statistics.general.common.dbg;
+	max_dbg = &priv->_4965.max_delta.general.common.dbg;
+	delta_div = &priv->_4965.delta_statistics.general.common.div;
+	max_div = &priv->_4965.max_delta.general.common.div;
+
+	pos += iwl4965_statistics_flag(priv, buf, bufsz);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_header, "Statistics_General:");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_value, "temperature:",
+			 le32_to_cpu(general->temperature));
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_value, "ttl_timestamp:",
+			 le32_to_cpu(general->ttl_timestamp));
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "burst_check:",
+			 le32_to_cpu(dbg->burst_check),
+			 accum_dbg->burst_check,
+			 delta_dbg->burst_check, max_dbg->burst_check);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "burst_count:",
+			 le32_to_cpu(dbg->burst_count),
+			 accum_dbg->burst_count,
+			 delta_dbg->burst_count, max_dbg->burst_count);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "wait_for_silence_timeout_count:",
+			 le32_to_cpu(dbg->wait_for_silence_timeout_cnt),
+			 accum_dbg->wait_for_silence_timeout_cnt,
+			 delta_dbg->wait_for_silence_timeout_cnt,
+			 max_dbg->wait_for_silence_timeout_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "sleep_time:",
+			 le32_to_cpu(general->sleep_time),
+			 accum_general->sleep_time,
+			 delta_general->sleep_time, max_general->sleep_time);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "slots_out:",
+			 le32_to_cpu(general->slots_out),
+			 accum_general->slots_out,
+			 delta_general->slots_out, max_general->slots_out);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "slots_idle:",
+			 le32_to_cpu(general->slots_idle),
+			 accum_general->slots_idle,
+			 delta_general->slots_idle, max_general->slots_idle);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "tx_on_a:",
+			 le32_to_cpu(div->tx_on_a), accum_div->tx_on_a,
+			 delta_div->tx_on_a, max_div->tx_on_a);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "tx_on_b:",
+			 le32_to_cpu(div->tx_on_b), accum_div->tx_on_b,
+			 delta_div->tx_on_b, max_div->tx_on_b);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "exec_time:",
+			 le32_to_cpu(div->exec_time), accum_div->exec_time,
+			 delta_div->exec_time, max_div->exec_time);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "probe_time:",
+			 le32_to_cpu(div->probe_time), accum_div->probe_time,
+			 delta_div->probe_time, max_div->probe_time);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "rx_enable_counter:",
+			 le32_to_cpu(general->rx_enable_counter),
+			 accum_general->rx_enable_counter,
+			 delta_general->rx_enable_counter,
+			 max_general->rx_enable_counter);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			 fmt_table, "num_of_sos_states:",
+			 le32_to_cpu(general->num_of_sos_states),
+			 accum_general->num_of_sos_states,
+			 delta_general->num_of_sos_states,
+			 max_general->num_of_sos_states);
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-debugfs.h b/drivers/net/wireless/iwlegacy/iwl-4965-debugfs.h
new file mode 100644
index 000000000000..6c8e35361a9e
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-debugfs.h
@@ -0,0 +1,59 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-debug.h"
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+ssize_t iwl4965_ucode_rx_stats_read(struct file *file, char __user *user_buf,
+				size_t count, loff_t *ppos);
+ssize_t iwl4965_ucode_tx_stats_read(struct file *file, char __user *user_buf,
+				size_t count, loff_t *ppos);
+ssize_t iwl4965_ucode_general_stats_read(struct file *file,
+			char __user *user_buf, size_t count, loff_t *ppos);
+#else
+static ssize_t
+iwl4965_ucode_rx_stats_read(struct file *file, char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	return 0;
+}
+static ssize_t
+iwl4965_ucode_tx_stats_read(struct file *file, char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	return 0;
+}
+static ssize_t
+iwl4965_ucode_general_stats_read(struct file *file, char __user *user_buf,
+					    size_t count, loff_t *ppos)
+{
+	return 0;
+}
+#endif
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-eeprom.c b/drivers/net/wireless/iwlegacy/iwl-4965-eeprom.c
new file mode 100644
index 000000000000..cb9baab1ff7d
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-eeprom.c
@@ -0,0 +1,154 @@
+/******************************************************************************
+ *
+ * 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 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+
+#include <net/mac80211.h>
+
+#include "iwl-commands.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-debug.h"
+#include "iwl-4965.h"
+#include "iwl-io.h"
+
+/******************************************************************************
+ *
+ * EEPROM related functions
+ *
+******************************************************************************/
+
+/*
+ * The device's EEPROM semaphore prevents conflicts between driver and uCode
+ * when accessing the EEPROM; each access is a series of pulses to/from the
+ * EEPROM chip, not a single event, so even reads could conflict if they
+ * weren't arbitrated by the semaphore.
+ */
+int iwl4965_eeprom_acquire_semaphore(struct iwl_priv *priv)
+{
+	u16 count;
+	int ret;
+
+	for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
+		/* Request semaphore */
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+
+		/* See if we got it */
+		ret = iwl_poll_bit(priv, CSR_HW_IF_CONFIG_REG,
+				CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
+				CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
+				EEPROM_SEM_TIMEOUT);
+		if (ret >= 0) {
+			IWL_DEBUG_IO(priv,
+				"Acquired semaphore after %d tries.\n",
+				count+1);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+void iwl4965_eeprom_release_semaphore(struct iwl_priv *priv)
+{
+	iwl_legacy_clear_bit(priv, CSR_HW_IF_CONFIG_REG,
+		CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+
+}
+
+int iwl4965_eeprom_check_version(struct iwl_priv *priv)
+{
+	u16 eeprom_ver;
+	u16 calib_ver;
+
+	eeprom_ver = iwl_legacy_eeprom_query16(priv, EEPROM_VERSION);
+	calib_ver = iwl_legacy_eeprom_query16(priv,
+			EEPROM_4965_CALIB_VERSION_OFFSET);
+
+	if (eeprom_ver < priv->cfg->eeprom_ver ||
+	    calib_ver < priv->cfg->eeprom_calib_ver)
+		goto err;
+
+	IWL_INFO(priv, "device EEPROM VER=0x%x, CALIB=0x%x\n",
+		 eeprom_ver, calib_ver);
+
+	return 0;
+err:
+	IWL_ERR(priv, "Unsupported (too old) EEPROM VER=0x%x < 0x%x "
+		  "CALIB=0x%x < 0x%x\n",
+		  eeprom_ver, priv->cfg->eeprom_ver,
+		  calib_ver,  priv->cfg->eeprom_calib_ver);
+	return -EINVAL;
+
+}
+
+void iwl4965_eeprom_get_mac(const struct iwl_priv *priv, u8 *mac)
+{
+	const u8 *addr = iwl_legacy_eeprom_query_addr(priv,
+					EEPROM_MAC_ADDRESS);
+	memcpy(mac, addr, ETH_ALEN);
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h b/drivers/net/wireless/iwlegacy/iwl-4965-hw.h
index 9166794eda0d..08b189c8472d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-hw.h
@@ -5,7 +5,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 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
@@ -30,7 +30,7 @@
  *
  * BSD LICENSE
  *
- * Copyright(c) 2005 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -789,4 +789,26 @@ struct iwl4965_scd_bc_tbl {
 	u8 pad[1024 - (TFD_QUEUE_BC_SIZE) * sizeof(__le16)];
 } __packed;
 
+
+#define IWL4965_RTC_INST_LOWER_BOUND		(0x000000)
+
+/* RSSI to dBm */
+#define IWL4965_RSSI_OFFSET	44
+
+/* PCI registers */
+#define PCI_CFG_RETRY_TIMEOUT	0x041
+
+/* PCI register values */
+#define PCI_CFG_LINK_CTRL_VAL_L0S_EN	0x01
+#define PCI_CFG_LINK_CTRL_VAL_L1_EN	0x02
+
+#define IWL4965_DEFAULT_TX_RETRY  15
+
+/* Limit range of txpower output target to be between these values */
+#define IWL4965_TX_POWER_TARGET_POWER_MIN	(0)	/* 0 dBm: 1 milliwatt */
+
+/* EEPROM */
+#define IWL4965_FIRST_AMPDU_QUEUE	10
+
+
 #endif /* !__iwl_4965_hw_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-led.c b/drivers/net/wireless/iwlegacy/iwl-4965-led.c
new file mode 100644
index 000000000000..26d324e30692
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-led.c
@@ -0,0 +1,74 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+#include <asm/unaligned.h>
+
+#include "iwl-commands.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-4965-led.h"
+
+/* Send led command */
+static int
+iwl4965_send_led_cmd(struct iwl_priv *priv, struct iwl_led_cmd *led_cmd)
+{
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_LEDS_CMD,
+		.len = sizeof(struct iwl_led_cmd),
+		.data = led_cmd,
+		.flags = CMD_ASYNC,
+		.callback = NULL,
+	};
+	u32 reg;
+
+	reg = iwl_read32(priv, CSR_LED_REG);
+	if (reg != (reg & CSR_LED_BSM_CTRL_MSK))
+		iwl_write32(priv, CSR_LED_REG, reg & CSR_LED_BSM_CTRL_MSK);
+
+	return iwl_legacy_send_cmd(priv, &cmd);
+}
+
+/* Set led register off */
+void iwl4965_led_enable(struct iwl_priv *priv)
+{
+	iwl_write32(priv, CSR_LED_REG, CSR_LED_REG_TRUN_ON);
+}
+
+const struct iwl_led_ops iwl4965_led_ops = {
+	.cmd = iwl4965_send_led_cmd,
+};
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-led.h b/drivers/net/wireless/iwlegacy/iwl-4965-led.h
new file mode 100644
index 000000000000..5ed3615fc338
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-led.h
@@ -0,0 +1,33 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_4965_led_h__
+#define __iwl_4965_led_h__
+
+extern const struct iwl_led_ops iwl4965_led_ops;
+void iwl4965_led_enable(struct iwl_priv *priv);
+
+#endif /* __iwl_4965_led_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-lib.c b/drivers/net/wireless/iwlegacy/iwl-4965-lib.c
new file mode 100644
index 000000000000..5a8a3cce27bc
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-lib.c
@@ -0,0 +1,1260 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+#include <linux/etherdevice.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+#include "iwl-4965-hw.h"
+#include "iwl-4965.h"
+#include "iwl-sta.h"
+
+void iwl4965_check_abort_status(struct iwl_priv *priv,
+			    u8 frame_count, u32 status)
+{
+	if (frame_count == 1 && status == TX_STATUS_FAIL_RFKILL_FLUSH) {
+		IWL_ERR(priv, "Tx flush command to flush out all frames\n");
+		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+			queue_work(priv->workqueue, &priv->tx_flush);
+	}
+}
+
+/*
+ * EEPROM
+ */
+struct iwl_mod_params iwl4965_mod_params = {
+	.amsdu_size_8K = 1,
+	.restart_fw = 1,
+	/* the rest are 0 by default */
+};
+
+void iwl4965_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	unsigned long flags;
+	int i;
+	spin_lock_irqsave(&rxq->lock, flags);
+	INIT_LIST_HEAD(&rxq->rx_free);
+	INIT_LIST_HEAD(&rxq->rx_used);
+	/* Fill the rx_used queue with _all_ of the Rx buffers */
+	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++) {
+		/* In the reset function, these buffers may have been allocated
+		 * to an SKB, so we need to unmap and free potential storage */
+		if (rxq->pool[i].page != NULL) {
+			pci_unmap_page(priv->pci_dev, rxq->pool[i].page_dma,
+				PAGE_SIZE << priv->hw_params.rx_page_order,
+				PCI_DMA_FROMDEVICE);
+			__iwl_legacy_free_pages(priv, rxq->pool[i].page);
+			rxq->pool[i].page = NULL;
+		}
+		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
+	}
+
+	for (i = 0; i < RX_QUEUE_SIZE; i++)
+		rxq->queue[i] = NULL;
+
+	/* Set us so that we have processed and used all buffers, but have
+	 * not restocked the Rx queue with fresh buffers */
+	rxq->read = rxq->write = 0;
+	rxq->write_actual = 0;
+	rxq->free_count = 0;
+	spin_unlock_irqrestore(&rxq->lock, flags);
+}
+
+int iwl4965_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	u32 rb_size;
+	const u32 rfdnlog = RX_QUEUE_SIZE_LOG; /* 256 RBDs */
+	u32 rb_timeout = 0;
+
+	if (priv->cfg->mod_params->amsdu_size_8K)
+		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K;
+	else
+		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K;
+
+	/* Stop Rx DMA */
+	iwl_legacy_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+
+	/* Reset driver's Rx queue write index */
+	iwl_legacy_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_WPTR_REG, 0);
+
+	/* Tell device where to find RBD circular buffer in DRAM */
+	iwl_legacy_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_BASE_REG,
+			   (u32)(rxq->bd_dma >> 8));
+
+	/* Tell device where in DRAM to update its Rx status */
+	iwl_legacy_write_direct32(priv, FH_RSCSR_CHNL0_STTS_WPTR_REG,
+			   rxq->rb_stts_dma >> 4);
+
+	/* Enable Rx DMA
+	 * Direct rx interrupts to hosts
+	 * Rx buffer size 4 or 8k
+	 * RB timeout 0x10
+	 * 256 RBDs
+	 */
+	iwl_legacy_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG,
+			   FH_RCSR_RX_CONFIG_CHNL_EN_ENABLE_VAL |
+			   FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL |
+			   FH_RCSR_CHNL0_RX_CONFIG_SINGLE_FRAME_MSK |
+			   rb_size|
+			   (rb_timeout << FH_RCSR_RX_CONFIG_REG_IRQ_RBTH_POS)|
+			   (rfdnlog << FH_RCSR_RX_CONFIG_RBDCB_SIZE_POS));
+
+	/* Set interrupt coalescing timer to default (2048 usecs) */
+	iwl_write8(priv, CSR_INT_COALESCING, IWL_HOST_INT_TIMEOUT_DEF);
+
+	return 0;
+}
+
+static void iwl4965_set_pwr_vmain(struct iwl_priv *priv)
+{
+/*
+ * (for documentation purposes)
+ * to set power to V_AUX, do:
+
+		if (pci_pme_capable(priv->pci_dev, PCI_D3cold))
+			iwl_legacy_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
+					       APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
+					       ~APMG_PS_CTRL_MSK_PWR_SRC);
+ */
+
+	iwl_legacy_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
+			       APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
+			       ~APMG_PS_CTRL_MSK_PWR_SRC);
+}
+
+int iwl4965_hw_nic_init(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	int ret;
+
+	/* nic_init */
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->cfg->ops->lib->apm_ops.init(priv);
+
+	/* Set interrupt coalescing calibration timer to default (512 usecs) */
+	iwl_write8(priv, CSR_INT_COALESCING, IWL_HOST_INT_CALIB_TIMEOUT_DEF);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl4965_set_pwr_vmain(priv);
+
+	priv->cfg->ops->lib->apm_ops.config(priv);
+
+	/* Allocate the RX queue, or reset if it is already allocated */
+	if (!rxq->bd) {
+		ret = iwl_legacy_rx_queue_alloc(priv);
+		if (ret) {
+			IWL_ERR(priv, "Unable to initialize Rx queue\n");
+			return -ENOMEM;
+		}
+	} else
+		iwl4965_rx_queue_reset(priv, rxq);
+
+	iwl4965_rx_replenish(priv);
+
+	iwl4965_rx_init(priv, rxq);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	rxq->need_update = 1;
+	iwl_legacy_rx_queue_update_write_ptr(priv, rxq);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Allocate or reset and init all Tx and Command queues */
+	if (!priv->txq) {
+		ret = iwl4965_txq_ctx_alloc(priv);
+		if (ret)
+			return ret;
+	} else
+		iwl4965_txq_ctx_reset(priv);
+
+	set_bit(STATUS_INIT, &priv->status);
+
+	return 0;
+}
+
+/**
+ * iwl4965_dma_addr2rbd_ptr - convert a DMA address to a uCode read buffer ptr
+ */
+static inline __le32 iwl4965_dma_addr2rbd_ptr(struct iwl_priv *priv,
+					  dma_addr_t dma_addr)
+{
+	return cpu_to_le32((u32)(dma_addr >> 8));
+}
+
+/**
+ * iwl4965_rx_queue_restock - refill RX queue from pre-allocated pool
+ *
+ * If there are slots in the RX queue that need to be restocked,
+ * and we have free pre-allocated buffers, fill the ranks as much
+ * as we can, pulling from rx_free.
+ *
+ * This moves the 'write' index forward to catch up with 'processed', and
+ * also updates the memory address in the firmware to reference the new
+ * target buffer.
+ */
+void iwl4965_rx_queue_restock(struct iwl_priv *priv)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct list_head *element;
+	struct iwl_rx_mem_buffer *rxb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rxq->lock, flags);
+	while ((iwl_legacy_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
+		/* The overwritten rxb must be a used one */
+		rxb = rxq->queue[rxq->write];
+		BUG_ON(rxb && rxb->page);
+
+		/* Get next free Rx buffer, remove from free list */
+		element = rxq->rx_free.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		list_del(element);
+
+		/* Point to Rx buffer via next RBD in circular buffer */
+		rxq->bd[rxq->write] = iwl4965_dma_addr2rbd_ptr(priv,
+							      rxb->page_dma);
+		rxq->queue[rxq->write] = rxb;
+		rxq->write = (rxq->write + 1) & RX_QUEUE_MASK;
+		rxq->free_count--;
+	}
+	spin_unlock_irqrestore(&rxq->lock, flags);
+	/* If the pre-allocated buffer pool is dropping low, schedule to
+	 * refill it */
+	if (rxq->free_count <= RX_LOW_WATERMARK)
+		queue_work(priv->workqueue, &priv->rx_replenish);
+
+
+	/* If we've added more space for the firmware to place data, tell it.
+	 * Increment device's write pointer in multiples of 8. */
+	if (rxq->write_actual != (rxq->write & ~0x7)) {
+		spin_lock_irqsave(&rxq->lock, flags);
+		rxq->need_update = 1;
+		spin_unlock_irqrestore(&rxq->lock, flags);
+		iwl_legacy_rx_queue_update_write_ptr(priv, rxq);
+	}
+}
+
+/**
+ * iwl4965_rx_replenish - Move all used packet from rx_used to rx_free
+ *
+ * When moving to rx_free an SKB is allocated for the slot.
+ *
+ * Also restock the Rx queue via iwl_rx_queue_restock.
+ * This is called as a scheduled work item (except for during initialization)
+ */
+static void iwl4965_rx_allocate(struct iwl_priv *priv, gfp_t priority)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct list_head *element;
+	struct iwl_rx_mem_buffer *rxb;
+	struct page *page;
+	unsigned long flags;
+	gfp_t gfp_mask = priority;
+
+	while (1) {
+		spin_lock_irqsave(&rxq->lock, flags);
+		if (list_empty(&rxq->rx_used)) {
+			spin_unlock_irqrestore(&rxq->lock, flags);
+			return;
+		}
+		spin_unlock_irqrestore(&rxq->lock, flags);
+
+		if (rxq->free_count > RX_LOW_WATERMARK)
+			gfp_mask |= __GFP_NOWARN;
+
+		if (priv->hw_params.rx_page_order > 0)
+			gfp_mask |= __GFP_COMP;
+
+		/* Alloc a new receive buffer */
+		page = alloc_pages(gfp_mask, priv->hw_params.rx_page_order);
+		if (!page) {
+			if (net_ratelimit())
+				IWL_DEBUG_INFO(priv, "alloc_pages failed, "
+					       "order: %d\n",
+					       priv->hw_params.rx_page_order);
+
+			if ((rxq->free_count <= RX_LOW_WATERMARK) &&
+			    net_ratelimit())
+				IWL_CRIT(priv,
+					"Failed to alloc_pages with %s. "
+					"Only %u free buffers remaining.\n",
+					 priority == GFP_ATOMIC ?
+						 "GFP_ATOMIC" : "GFP_KERNEL",
+					 rxq->free_count);
+			/* We don't reschedule replenish work here -- we will
+			 * call the restock method and if it still needs
+			 * more buffers it will schedule replenish */
+			return;
+		}
+
+		spin_lock_irqsave(&rxq->lock, flags);
+
+		if (list_empty(&rxq->rx_used)) {
+			spin_unlock_irqrestore(&rxq->lock, flags);
+			__free_pages(page, priv->hw_params.rx_page_order);
+			return;
+		}
+		element = rxq->rx_used.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		list_del(element);
+
+		spin_unlock_irqrestore(&rxq->lock, flags);
+
+		BUG_ON(rxb->page);
+		rxb->page = page;
+		/* Get physical address of the RB */
+		rxb->page_dma = pci_map_page(priv->pci_dev, page, 0,
+				PAGE_SIZE << priv->hw_params.rx_page_order,
+				PCI_DMA_FROMDEVICE);
+		/* dma address must be no more than 36 bits */
+		BUG_ON(rxb->page_dma & ~DMA_BIT_MASK(36));
+		/* and also 256 byte aligned! */
+		BUG_ON(rxb->page_dma & DMA_BIT_MASK(8));
+
+		spin_lock_irqsave(&rxq->lock, flags);
+
+		list_add_tail(&rxb->list, &rxq->rx_free);
+		rxq->free_count++;
+		priv->alloc_rxb_page++;
+
+		spin_unlock_irqrestore(&rxq->lock, flags);
+	}
+}
+
+void iwl4965_rx_replenish(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	iwl4965_rx_allocate(priv, GFP_KERNEL);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl4965_rx_queue_restock(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+void iwl4965_rx_replenish_now(struct iwl_priv *priv)
+{
+	iwl4965_rx_allocate(priv, GFP_ATOMIC);
+
+	iwl4965_rx_queue_restock(priv);
+}
+
+/* Assumes that the skb field of the buffers in 'pool' is kept accurate.
+ * If an SKB has been detached, the POOL needs to have its SKB set to NULL
+ * This free routine walks the list of POOL entries and if SKB is set to
+ * non NULL it is unmapped and freed
+ */
+void iwl4965_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	int i;
+	for (i = 0; i < RX_QUEUE_SIZE + RX_FREE_BUFFERS; i++) {
+		if (rxq->pool[i].page != NULL) {
+			pci_unmap_page(priv->pci_dev, rxq->pool[i].page_dma,
+				PAGE_SIZE << priv->hw_params.rx_page_order,
+				PCI_DMA_FROMDEVICE);
+			__iwl_legacy_free_pages(priv, rxq->pool[i].page);
+			rxq->pool[i].page = NULL;
+		}
+	}
+
+	dma_free_coherent(&priv->pci_dev->dev, 4 * RX_QUEUE_SIZE, rxq->bd,
+			  rxq->bd_dma);
+	dma_free_coherent(&priv->pci_dev->dev, sizeof(struct iwl_rb_status),
+			  rxq->rb_stts, rxq->rb_stts_dma);
+	rxq->bd = NULL;
+	rxq->rb_stts  = NULL;
+}
+
+int iwl4965_rxq_stop(struct iwl_priv *priv)
+{
+
+	/* stop Rx DMA */
+	iwl_legacy_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+	iwl_poll_direct_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
+			    FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE, 1000);
+
+	return 0;
+}
+
+int iwl4965_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band)
+{
+	int idx = 0;
+	int band_offset = 0;
+
+	/* HT rate format: mac80211 wants an MCS number, which is just LSB */
+	if (rate_n_flags & RATE_MCS_HT_MSK) {
+		idx = (rate_n_flags & 0xff);
+		return idx;
+	/* Legacy rate format, search for match in table */
+	} else {
+		if (band == IEEE80211_BAND_5GHZ)
+			band_offset = IWL_FIRST_OFDM_RATE;
+		for (idx = band_offset; idx < IWL_RATE_COUNT_LEGACY; idx++)
+			if (iwlegacy_rates[idx].plcp == (rate_n_flags & 0xFF))
+				return idx - band_offset;
+	}
+
+	return -1;
+}
+
+static int iwl4965_calc_rssi(struct iwl_priv *priv,
+			     struct iwl_rx_phy_res *rx_resp)
+{
+	/* data from PHY/DSP regarding signal strength, etc.,
+	 *   contents are always there, not configurable by host.  */
+	struct iwl4965_rx_non_cfg_phy *ncphy =
+	    (struct iwl4965_rx_non_cfg_phy *)rx_resp->non_cfg_phy_buf;
+	u32 agc = (le16_to_cpu(ncphy->agc_info) & IWL49_AGC_DB_MASK)
+			>> IWL49_AGC_DB_POS;
+
+	u32 valid_antennae =
+	    (le16_to_cpu(rx_resp->phy_flags) & IWL49_RX_PHY_FLAGS_ANTENNAE_MASK)
+			>> IWL49_RX_PHY_FLAGS_ANTENNAE_OFFSET;
+	u8 max_rssi = 0;
+	u32 i;
+
+	/* Find max rssi among 3 possible receivers.
+	 * These values are measured by the digital signal processor (DSP).
+	 * They should stay fairly constant even as the signal strength varies,
+	 *   if the radio's automatic gain control (AGC) is working right.
+	 * AGC value (see below) will provide the "interesting" info. */
+	for (i = 0; i < 3; i++)
+		if (valid_antennae & (1 << i))
+			max_rssi = max(ncphy->rssi_info[i << 1], max_rssi);
+
+	IWL_DEBUG_STATS(priv, "Rssi In A %d B %d C %d Max %d AGC dB %d\n",
+		ncphy->rssi_info[0], ncphy->rssi_info[2], ncphy->rssi_info[4],
+		max_rssi, agc);
+
+	/* dBm = max_rssi dB - agc dB - constant.
+	 * Higher AGC (higher radio gain) means lower signal. */
+	return max_rssi - agc - IWL4965_RSSI_OFFSET;
+}
+
+
+static u32 iwl4965_translate_rx_status(struct iwl_priv *priv, u32 decrypt_in)
+{
+	u32 decrypt_out = 0;
+
+	if ((decrypt_in & RX_RES_STATUS_STATION_FOUND) ==
+					RX_RES_STATUS_STATION_FOUND)
+		decrypt_out |= (RX_RES_STATUS_STATION_FOUND |
+				RX_RES_STATUS_NO_STATION_INFO_MISMATCH);
+
+	decrypt_out |= (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK);
+
+	/* packet was not encrypted */
+	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
+					RX_RES_STATUS_SEC_TYPE_NONE)
+		return decrypt_out;
+
+	/* packet was encrypted with unknown alg */
+	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
+					RX_RES_STATUS_SEC_TYPE_ERR)
+		return decrypt_out;
+
+	/* decryption was not done in HW */
+	if ((decrypt_in & RX_MPDU_RES_STATUS_DEC_DONE_MSK) !=
+					RX_MPDU_RES_STATUS_DEC_DONE_MSK)
+		return decrypt_out;
+
+	switch (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) {
+
+	case RX_RES_STATUS_SEC_TYPE_CCMP:
+		/* alg is CCM: check MIC only */
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_MIC_OK))
+			/* Bad MIC */
+			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
+		else
+			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
+
+		break;
+
+	case RX_RES_STATUS_SEC_TYPE_TKIP:
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_TTAK_OK)) {
+			/* Bad TTAK */
+			decrypt_out |= RX_RES_STATUS_BAD_KEY_TTAK;
+			break;
+		}
+		/* fall through if TTAK OK */
+	default:
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_ICV_OK))
+			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
+		else
+			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
+		break;
+	}
+
+	IWL_DEBUG_RX(priv, "decrypt_in:0x%x  decrypt_out = 0x%x\n",
+					decrypt_in, decrypt_out);
+
+	return decrypt_out;
+}
+
+static void iwl4965_pass_packet_to_mac80211(struct iwl_priv *priv,
+					struct ieee80211_hdr *hdr,
+					u16 len,
+					u32 ampdu_status,
+					struct iwl_rx_mem_buffer *rxb,
+					struct ieee80211_rx_status *stats)
+{
+	struct sk_buff *skb;
+	__le16 fc = hdr->frame_control;
+
+	/* We only process data packets if the interface is open */
+	if (unlikely(!priv->is_open)) {
+		IWL_DEBUG_DROP_LIMIT(priv,
+		    "Dropping packet while interface is not open.\n");
+		return;
+	}
+
+	/* In case of HW accelerated crypto and bad decryption, drop */
+	if (!priv->cfg->mod_params->sw_crypto &&
+	    iwl_legacy_set_decrypted_flag(priv, hdr, ampdu_status, stats))
+		return;
+
+	skb = dev_alloc_skb(128);
+	if (!skb) {
+		IWL_ERR(priv, "dev_alloc_skb failed\n");
+		return;
+	}
+
+	skb_add_rx_frag(skb, 0, rxb->page, (void *)hdr - rxb_addr(rxb), len);
+
+	iwl_legacy_update_stats(priv, false, fc, len);
+	memcpy(IEEE80211_SKB_RXCB(skb), stats, sizeof(*stats));
+
+	ieee80211_rx(priv->hw, skb);
+	priv->alloc_rxb_page--;
+	rxb->page = NULL;
+}
+
+/* Called for REPLY_RX (legacy ABG frames), or
+ * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
+void iwl4965_rx_reply_rx(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct ieee80211_hdr *header;
+	struct ieee80211_rx_status rx_status;
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_rx_phy_res *phy_res;
+	__le32 rx_pkt_status;
+	struct iwl_rx_mpdu_res_start *amsdu;
+	u32 len;
+	u32 ampdu_status;
+	u32 rate_n_flags;
+
+	/**
+	 * REPLY_RX and REPLY_RX_MPDU_CMD are handled differently.
+	 *	REPLY_RX: physical layer info is in this buffer
+	 *	REPLY_RX_MPDU_CMD: physical layer info was sent in separate
+	 *		command and cached in priv->last_phy_res
+	 *
+	 * Here we set up local variables depending on which command is
+	 * received.
+	 */
+	if (pkt->hdr.cmd == REPLY_RX) {
+		phy_res = (struct iwl_rx_phy_res *)pkt->u.raw;
+		header = (struct ieee80211_hdr *)(pkt->u.raw + sizeof(*phy_res)
+				+ phy_res->cfg_phy_cnt);
+
+		len = le16_to_cpu(phy_res->byte_count);
+		rx_pkt_status = *(__le32 *)(pkt->u.raw + sizeof(*phy_res) +
+				phy_res->cfg_phy_cnt + len);
+		ampdu_status = le32_to_cpu(rx_pkt_status);
+	} else {
+		if (!priv->_4965.last_phy_res_valid) {
+			IWL_ERR(priv, "MPDU frame without cached PHY data\n");
+			return;
+		}
+		phy_res = &priv->_4965.last_phy_res;
+		amsdu = (struct iwl_rx_mpdu_res_start *)pkt->u.raw;
+		header = (struct ieee80211_hdr *)(pkt->u.raw + sizeof(*amsdu));
+		len = le16_to_cpu(amsdu->byte_count);
+		rx_pkt_status = *(__le32 *)(pkt->u.raw + sizeof(*amsdu) + len);
+		ampdu_status = iwl4965_translate_rx_status(priv,
+				le32_to_cpu(rx_pkt_status));
+	}
+
+	if ((unlikely(phy_res->cfg_phy_cnt > 20))) {
+		IWL_DEBUG_DROP(priv, "dsp size out of range [0,20]: %d/n",
+				phy_res->cfg_phy_cnt);
+		return;
+	}
+
+	if (!(rx_pkt_status & RX_RES_STATUS_NO_CRC32_ERROR) ||
+	    !(rx_pkt_status & RX_RES_STATUS_NO_RXE_OVERFLOW)) {
+		IWL_DEBUG_RX(priv, "Bad CRC or FIFO: 0x%08X.\n",
+				le32_to_cpu(rx_pkt_status));
+		return;
+	}
+
+	/* This will be used in several places later */
+	rate_n_flags = le32_to_cpu(phy_res->rate_n_flags);
+
+	/* rx_status carries information about the packet to mac80211 */
+	rx_status.mactime = le64_to_cpu(phy_res->timestamp);
+	rx_status.freq =
+		ieee80211_channel_to_frequency(le16_to_cpu(phy_res->channel),
+							rx_status.band);
+	rx_status.band = (phy_res->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
+				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+	rx_status.rate_idx =
+		iwl4965_hwrate_to_mac80211_idx(rate_n_flags, rx_status.band);
+	rx_status.flag = 0;
+
+	/* TSF isn't reliable. In order to allow smooth user experience,
+	 * this W/A doesn't propagate it to the mac80211 */
+	/*rx_status.flag |= RX_FLAG_MACTIME_MPDU;*/
+
+	priv->ucode_beacon_time = le32_to_cpu(phy_res->beacon_time_stamp);
+
+	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
+	rx_status.signal = iwl4965_calc_rssi(priv, phy_res);
+
+	iwl_legacy_dbg_log_rx_data_frame(priv, len, header);
+	IWL_DEBUG_STATS_LIMIT(priv, "Rssi %d, TSF %llu\n",
+		rx_status.signal, (unsigned long long)rx_status.mactime);
+
+	/*
+	 * "antenna number"
+	 *
+	 * It seems that the antenna field in the phy flags value
+	 * is actually a bit field. This is undefined by radiotap,
+	 * it wants an actual antenna number but I always get "7"
+	 * for most legacy frames I receive indicating that the
+	 * same frame was received on all three RX chains.
+	 *
+	 * I think this field should be removed in favor of a
+	 * new 802.11n radiotap field "RX chains" that is defined
+	 * as a bitmask.
+	 */
+	rx_status.antenna =
+		(le16_to_cpu(phy_res->phy_flags) & RX_RES_PHY_FLAGS_ANTENNA_MSK)
+		>> RX_RES_PHY_FLAGS_ANTENNA_POS;
+
+	/* set the preamble flag if appropriate */
+	if (phy_res->phy_flags & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
+		rx_status.flag |= RX_FLAG_SHORTPRE;
+
+	/* Set up the HT phy flags */
+	if (rate_n_flags & RATE_MCS_HT_MSK)
+		rx_status.flag |= RX_FLAG_HT;
+	if (rate_n_flags & RATE_MCS_HT40_MSK)
+		rx_status.flag |= RX_FLAG_40MHZ;
+	if (rate_n_flags & RATE_MCS_SGI_MSK)
+		rx_status.flag |= RX_FLAG_SHORT_GI;
+
+	iwl4965_pass_packet_to_mac80211(priv, header, len, ampdu_status,
+				    rxb, &rx_status);
+}
+
+/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
+ * This will be used later in iwl_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
+void iwl4965_rx_reply_rx_phy(struct iwl_priv *priv,
+			    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	priv->_4965.last_phy_res_valid = true;
+	memcpy(&priv->_4965.last_phy_res, pkt->u.raw,
+	       sizeof(struct iwl_rx_phy_res));
+}
+
+static int iwl4965_get_single_channel_for_scan(struct iwl_priv *priv,
+					   struct ieee80211_vif *vif,
+					   enum ieee80211_band band,
+					   struct iwl_scan_channel *scan_ch)
+{
+	const struct ieee80211_supported_band *sband;
+	u16 passive_dwell = 0;
+	u16 active_dwell = 0;
+	int added = 0;
+	u16 channel = 0;
+
+	sband = iwl_get_hw_mode(priv, band);
+	if (!sband) {
+		IWL_ERR(priv, "invalid band\n");
+		return added;
+	}
+
+	active_dwell = iwl_legacy_get_active_dwell_time(priv, band, 0);
+	passive_dwell = iwl_legacy_get_passive_dwell_time(priv, band, vif);
+
+	if (passive_dwell <= active_dwell)
+		passive_dwell = active_dwell + 1;
+
+	channel = iwl_legacy_get_single_channel_number(priv, band);
+	if (channel) {
+		scan_ch->channel = cpu_to_le16(channel);
+		scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
+		scan_ch->active_dwell = cpu_to_le16(active_dwell);
+		scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+		/* Set txpower levels to defaults */
+		scan_ch->dsp_atten = 110;
+		if (band == IEEE80211_BAND_5GHZ)
+			scan_ch->tx_gain = ((1 << 5) | (3 << 3)) | 3;
+		else
+			scan_ch->tx_gain = ((1 << 5) | (5 << 3));
+		added++;
+	} else
+		IWL_ERR(priv, "no valid channel found\n");
+	return added;
+}
+
+static int iwl4965_get_channels_for_scan(struct iwl_priv *priv,
+				     struct ieee80211_vif *vif,
+				     enum ieee80211_band band,
+				     u8 is_active, u8 n_probes,
+				     struct iwl_scan_channel *scan_ch)
+{
+	struct ieee80211_channel *chan;
+	const struct ieee80211_supported_band *sband;
+	const struct iwl_channel_info *ch_info;
+	u16 passive_dwell = 0;
+	u16 active_dwell = 0;
+	int added, i;
+	u16 channel;
+
+	sband = iwl_get_hw_mode(priv, band);
+	if (!sband)
+		return 0;
+
+	active_dwell = iwl_legacy_get_active_dwell_time(priv, band, n_probes);
+	passive_dwell = iwl_legacy_get_passive_dwell_time(priv, band, vif);
+
+	if (passive_dwell <= active_dwell)
+		passive_dwell = active_dwell + 1;
+
+	for (i = 0, added = 0; i < priv->scan_request->n_channels; i++) {
+		chan = priv->scan_request->channels[i];
+
+		if (chan->band != band)
+			continue;
+
+		channel = chan->hw_value;
+		scan_ch->channel = cpu_to_le16(channel);
+
+		ch_info = iwl_legacy_get_channel_info(priv, band, channel);
+		if (!iwl_legacy_is_channel_valid(ch_info)) {
+			IWL_DEBUG_SCAN(priv,
+				 "Channel %d is INVALID for this band.\n",
+					channel);
+			continue;
+		}
+
+		if (!is_active || iwl_legacy_is_channel_passive(ch_info) ||
+		    (chan->flags & IEEE80211_CHAN_PASSIVE_SCAN))
+			scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
+		else
+			scan_ch->type = SCAN_CHANNEL_TYPE_ACTIVE;
+
+		if (n_probes)
+			scan_ch->type |= IWL_SCAN_PROBE_MASK(n_probes);
+
+		scan_ch->active_dwell = cpu_to_le16(active_dwell);
+		scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+
+		/* Set txpower levels to defaults */
+		scan_ch->dsp_atten = 110;
+
+		/* NOTE: if we were doing 6Mb OFDM for scans we'd use
+		 * power level:
+		 * scan_ch->tx_gain = ((1 << 5) | (2 << 3)) | 3;
+		 */
+		if (band == IEEE80211_BAND_5GHZ)
+			scan_ch->tx_gain = ((1 << 5) | (3 << 3)) | 3;
+		else
+			scan_ch->tx_gain = ((1 << 5) | (5 << 3));
+
+		IWL_DEBUG_SCAN(priv, "Scanning ch=%d prob=0x%X [%s %d]\n",
+			       channel, le32_to_cpu(scan_ch->type),
+			       (scan_ch->type & SCAN_CHANNEL_TYPE_ACTIVE) ?
+				"ACTIVE" : "PASSIVE",
+			       (scan_ch->type & SCAN_CHANNEL_TYPE_ACTIVE) ?
+			       active_dwell : passive_dwell);
+
+		scan_ch++;
+		added++;
+	}
+
+	IWL_DEBUG_SCAN(priv, "total channels to scan %d\n", added);
+	return added;
+}
+
+int iwl4965_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
+{
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SCAN_CMD,
+		.len = sizeof(struct iwl_scan_cmd),
+		.flags = CMD_SIZE_HUGE,
+	};
+	struct iwl_scan_cmd *scan;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	u32 rate_flags = 0;
+	u16 cmd_len;
+	u16 rx_chain = 0;
+	enum ieee80211_band band;
+	u8 n_probes = 0;
+	u8 rx_ant = priv->hw_params.valid_rx_ant;
+	u8 rate;
+	bool is_active = false;
+	int  chan_mod;
+	u8 active_chains;
+	u8 scan_tx_antennas = priv->hw_params.valid_tx_ant;
+	int ret;
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (vif)
+		ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+
+	if (!priv->scan_cmd) {
+		priv->scan_cmd = kmalloc(sizeof(struct iwl_scan_cmd) +
+					 IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+		if (!priv->scan_cmd) {
+			IWL_DEBUG_SCAN(priv,
+				       "fail to allocate memory for scan\n");
+			return -ENOMEM;
+		}
+	}
+	scan = priv->scan_cmd;
+	memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+
+	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
+	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
+
+	if (iwl_legacy_is_any_associated(priv)) {
+		u16 interval = 0;
+		u32 extra;
+		u32 suspend_time = 100;
+		u32 scan_suspend_time = 100;
+
+		IWL_DEBUG_INFO(priv, "Scanning while associated...\n");
+		if (priv->is_internal_short_scan)
+			interval = 0;
+		else
+			interval = vif->bss_conf.beacon_int;
+
+		scan->suspend_time = 0;
+		scan->max_out_time = cpu_to_le32(200 * 1024);
+		if (!interval)
+			interval = suspend_time;
+
+		extra = (suspend_time / interval) << 22;
+		scan_suspend_time = (extra |
+		    ((suspend_time % interval) * 1024));
+		scan->suspend_time = cpu_to_le32(scan_suspend_time);
+		IWL_DEBUG_SCAN(priv, "suspend_time 0x%X beacon interval %d\n",
+			       scan_suspend_time, interval);
+	}
+
+	if (priv->is_internal_short_scan) {
+		IWL_DEBUG_SCAN(priv, "Start internal passive scan.\n");
+	} else if (priv->scan_request->n_ssids) {
+		int i, p = 0;
+		IWL_DEBUG_SCAN(priv, "Kicking off active scan\n");
+		for (i = 0; i < priv->scan_request->n_ssids; i++) {
+			/* always does wildcard anyway */
+			if (!priv->scan_request->ssids[i].ssid_len)
+				continue;
+			scan->direct_scan[p].id = WLAN_EID_SSID;
+			scan->direct_scan[p].len =
+				priv->scan_request->ssids[i].ssid_len;
+			memcpy(scan->direct_scan[p].ssid,
+			       priv->scan_request->ssids[i].ssid,
+			       priv->scan_request->ssids[i].ssid_len);
+			n_probes++;
+			p++;
+		}
+		is_active = true;
+	} else
+		IWL_DEBUG_SCAN(priv, "Start passive scan.\n");
+
+	scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
+	scan->tx_cmd.sta_id = ctx->bcast_sta_id;
+	scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+
+	switch (priv->scan_band) {
+	case IEEE80211_BAND_2GHZ:
+		scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
+		chan_mod = le32_to_cpu(
+			priv->contexts[IWL_RXON_CTX_BSS].active.flags &
+						RXON_FLG_CHANNEL_MODE_MSK)
+				       >> RXON_FLG_CHANNEL_MODE_POS;
+		if (chan_mod == CHANNEL_MODE_PURE_40) {
+			rate = IWL_RATE_6M_PLCP;
+		} else {
+			rate = IWL_RATE_1M_PLCP;
+			rate_flags = RATE_MCS_CCK_MSK;
+		}
+		break;
+	case IEEE80211_BAND_5GHZ:
+		rate = IWL_RATE_6M_PLCP;
+		break;
+	default:
+		IWL_WARN(priv, "Invalid scan band\n");
+		return -EIO;
+	}
+
+	/*
+	 * If active scanning is requested but a certain channel is
+	 * marked passive, we can do active scanning if we detect
+	 * transmissions.
+	 *
+	 * There is an issue with some firmware versions that triggers
+	 * a sysassert on a "good CRC threshold" of zero (== disabled),
+	 * on a radar channel even though this means that we should NOT
+	 * send probes.
+	 *
+	 * The "good CRC threshold" is the number of frames that we
+	 * need to receive during our dwell time on a channel before
+	 * sending out probes -- setting this to a huge value will
+	 * mean we never reach it, but at the same time work around
+	 * the aforementioned issue. Thus use IWL_GOOD_CRC_TH_NEVER
+	 * here instead of IWL_GOOD_CRC_TH_DISABLED.
+	 */
+	scan->good_CRC_th = is_active ? IWL_GOOD_CRC_TH_DEFAULT :
+					IWL_GOOD_CRC_TH_NEVER;
+
+	band = priv->scan_band;
+
+	if (priv->cfg->scan_rx_antennas[band])
+		rx_ant = priv->cfg->scan_rx_antennas[band];
+
+	if (priv->cfg->scan_tx_antennas[band])
+		scan_tx_antennas = priv->cfg->scan_tx_antennas[band];
+
+	priv->scan_tx_ant[band] = iwl4965_toggle_tx_ant(priv,
+						priv->scan_tx_ant[band],
+						    scan_tx_antennas);
+	rate_flags |= iwl4965_ant_idx_to_flags(priv->scan_tx_ant[band]);
+	scan->tx_cmd.rate_n_flags = iwl4965_hw_set_rate_n_flags(rate, rate_flags);
+
+	/* In power save mode use one chain, otherwise use all chains */
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		/* rx_ant has been set to all valid chains previously */
+		active_chains = rx_ant &
+				((u8)(priv->chain_noise_data.active_chains));
+		if (!active_chains)
+			active_chains = rx_ant;
+
+		IWL_DEBUG_SCAN(priv, "chain_noise_data.active_chains: %u\n",
+				priv->chain_noise_data.active_chains);
+
+		rx_ant = iwl4965_first_antenna(active_chains);
+	}
+
+	/* MIMO is not used here, but value is required */
+	rx_chain |= priv->hw_params.valid_rx_ant << RXON_RX_CHAIN_VALID_POS;
+	rx_chain |= rx_ant << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS;
+	rx_chain |= rx_ant << RXON_RX_CHAIN_FORCE_SEL_POS;
+	rx_chain |= 0x1 << RXON_RX_CHAIN_DRIVER_FORCE_POS;
+	scan->rx_chain = cpu_to_le16(rx_chain);
+	if (!priv->is_internal_short_scan) {
+		cmd_len = iwl_legacy_fill_probe_req(priv,
+					(struct ieee80211_mgmt *)scan->data,
+					vif->addr,
+					priv->scan_request->ie,
+					priv->scan_request->ie_len,
+					IWL_MAX_SCAN_SIZE - sizeof(*scan));
+	} else {
+		/* use bcast addr, will not be transmitted but must be valid */
+		cmd_len = iwl_legacy_fill_probe_req(priv,
+					(struct ieee80211_mgmt *)scan->data,
+					iwlegacy_bcast_addr, NULL, 0,
+					IWL_MAX_SCAN_SIZE - sizeof(*scan));
+
+	}
+	scan->tx_cmd.len = cpu_to_le16(cmd_len);
+
+	scan->filter_flags |= (RXON_FILTER_ACCEPT_GRP_MSK |
+			       RXON_FILTER_BCON_AWARE_MSK);
+
+	if (priv->is_internal_short_scan) {
+		scan->channel_count =
+			iwl4965_get_single_channel_for_scan(priv, vif, band,
+				(void *)&scan->data[le16_to_cpu(
+				scan->tx_cmd.len)]);
+	} else {
+		scan->channel_count =
+			iwl4965_get_channels_for_scan(priv, vif, band,
+				is_active, n_probes,
+				(void *)&scan->data[le16_to_cpu(
+				scan->tx_cmd.len)]);
+	}
+	if (scan->channel_count == 0) {
+		IWL_DEBUG_SCAN(priv, "channel count %d\n", scan->channel_count);
+		return -EIO;
+	}
+
+	cmd.len += le16_to_cpu(scan->tx_cmd.len) +
+	    scan->channel_count * sizeof(struct iwl_scan_channel);
+	cmd.data = scan;
+	scan->len = cpu_to_le16(cmd.len);
+
+	set_bit(STATUS_SCAN_HW, &priv->status);
+
+	ret = iwl_legacy_send_cmd_sync(priv, &cmd);
+	if (ret)
+		clear_bit(STATUS_SCAN_HW, &priv->status);
+
+	return ret;
+}
+
+int iwl4965_manage_ibss_station(struct iwl_priv *priv,
+			       struct ieee80211_vif *vif, bool add)
+{
+	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
+
+	if (add)
+		return iwl4965_add_bssid_station(priv, vif_priv->ctx,
+						vif->bss_conf.bssid,
+						&vif_priv->ibss_bssid_sta_id);
+	return iwl_legacy_remove_station(priv, vif_priv->ibss_bssid_sta_id,
+				  vif->bss_conf.bssid);
+}
+
+void iwl4965_free_tfds_in_queue(struct iwl_priv *priv,
+			    int sta_id, int tid, int freed)
+{
+	lockdep_assert_held(&priv->sta_lock);
+
+	if (priv->stations[sta_id].tid[tid].tfds_in_queue >= freed)
+		priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+	else {
+		IWL_DEBUG_TX(priv, "free more than tfds_in_queue (%u:%d)\n",
+			priv->stations[sta_id].tid[tid].tfds_in_queue,
+			freed);
+		priv->stations[sta_id].tid[tid].tfds_in_queue = 0;
+	}
+}
+
+#define IWL_TX_QUEUE_MSK	0xfffff
+
+static bool iwl4965_is_single_rx_stream(struct iwl_priv *priv)
+{
+	return priv->current_ht_config.smps == IEEE80211_SMPS_STATIC ||
+	       priv->current_ht_config.single_chain_sufficient;
+}
+
+#define IWL_NUM_RX_CHAINS_MULTIPLE	3
+#define IWL_NUM_RX_CHAINS_SINGLE	2
+#define IWL_NUM_IDLE_CHAINS_DUAL	2
+#define IWL_NUM_IDLE_CHAINS_SINGLE	1
+
+/*
+ * Determine how many receiver/antenna chains to use.
+ *
+ * More provides better reception via diversity.  Fewer saves power
+ * at the expense of throughput, but only when not in powersave to
+ * start with.
+ *
+ * MIMO (dual stream) requires at least 2, but works better with 3.
+ * This does not determine *which* chains to use, just how many.
+ */
+static int iwl4965_get_active_rx_chain_count(struct iwl_priv *priv)
+{
+	/* # of Rx chains to use when expecting MIMO. */
+	if (iwl4965_is_single_rx_stream(priv))
+		return IWL_NUM_RX_CHAINS_SINGLE;
+	else
+		return IWL_NUM_RX_CHAINS_MULTIPLE;
+}
+
+/*
+ * When we are in power saving mode, unless device support spatial
+ * multiplexing power save, use the active count for rx chain count.
+ */
+static int
+iwl4965_get_idle_rx_chain_count(struct iwl_priv *priv, int active_cnt)
+{
+	/* # Rx chains when idling, depending on SMPS mode */
+	switch (priv->current_ht_config.smps) {
+	case IEEE80211_SMPS_STATIC:
+	case IEEE80211_SMPS_DYNAMIC:
+		return IWL_NUM_IDLE_CHAINS_SINGLE;
+	case IEEE80211_SMPS_OFF:
+		return active_cnt;
+	default:
+		WARN(1, "invalid SMPS mode %d",
+		     priv->current_ht_config.smps);
+		return active_cnt;
+	}
+}
+
+/* up to 4 chains */
+static u8 iwl4965_count_chain_bitmap(u32 chain_bitmap)
+{
+	u8 res;
+	res = (chain_bitmap & BIT(0)) >> 0;
+	res += (chain_bitmap & BIT(1)) >> 1;
+	res += (chain_bitmap & BIT(2)) >> 2;
+	res += (chain_bitmap & BIT(3)) >> 3;
+	return res;
+}
+
+/**
+ * iwl4965_set_rxon_chain - Set up Rx chain usage in "staging" RXON image
+ *
+ * Selects how many and which Rx receivers/antennas/chains to use.
+ * This should not be used for scan command ... it puts data in wrong place.
+ */
+void iwl4965_set_rxon_chain(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+{
+	bool is_single = iwl4965_is_single_rx_stream(priv);
+	bool is_cam = !test_bit(STATUS_POWER_PMI, &priv->status);
+	u8 idle_rx_cnt, active_rx_cnt, valid_rx_cnt;
+	u32 active_chains;
+	u16 rx_chain;
+
+	/* Tell uCode which antennas are actually connected.
+	 * Before first association, we assume all antennas are connected.
+	 * Just after first association, iwl4965_chain_noise_calibration()
+	 *    checks which antennas actually *are* connected. */
+	if (priv->chain_noise_data.active_chains)
+		active_chains = priv->chain_noise_data.active_chains;
+	else
+		active_chains = priv->hw_params.valid_rx_ant;
+
+	rx_chain = active_chains << RXON_RX_CHAIN_VALID_POS;
+
+	/* How many receivers should we use? */
+	active_rx_cnt = iwl4965_get_active_rx_chain_count(priv);
+	idle_rx_cnt = iwl4965_get_idle_rx_chain_count(priv, active_rx_cnt);
+
+
+	/* correct rx chain count according hw settings
+	 * and chain noise calibration
+	 */
+	valid_rx_cnt = iwl4965_count_chain_bitmap(active_chains);
+	if (valid_rx_cnt < active_rx_cnt)
+		active_rx_cnt = valid_rx_cnt;
+
+	if (valid_rx_cnt < idle_rx_cnt)
+		idle_rx_cnt = valid_rx_cnt;
+
+	rx_chain |= active_rx_cnt << RXON_RX_CHAIN_MIMO_CNT_POS;
+	rx_chain |= idle_rx_cnt  << RXON_RX_CHAIN_CNT_POS;
+
+	ctx->staging.rx_chain = cpu_to_le16(rx_chain);
+
+	if (!is_single && (active_rx_cnt >= IWL_NUM_RX_CHAINS_SINGLE) && is_cam)
+		ctx->staging.rx_chain |= RXON_RX_CHAIN_MIMO_FORCE_MSK;
+	else
+		ctx->staging.rx_chain &= ~RXON_RX_CHAIN_MIMO_FORCE_MSK;
+
+	IWL_DEBUG_ASSOC(priv, "rx_chain=0x%X active=%d idle=%d\n",
+			ctx->staging.rx_chain,
+			active_rx_cnt, idle_rx_cnt);
+
+	WARN_ON(active_rx_cnt == 0 || idle_rx_cnt == 0 ||
+		active_rx_cnt < idle_rx_cnt);
+}
+
+u8 iwl4965_toggle_tx_ant(struct iwl_priv *priv, u8 ant, u8 valid)
+{
+	int i;
+	u8 ind = ant;
+
+	for (i = 0; i < RATE_ANT_NUM - 1; i++) {
+		ind = (ind + 1) < RATE_ANT_NUM ?  ind + 1 : 0;
+		if (valid & BIT(ind))
+			return ind;
+	}
+	return ant;
+}
+
+static const char *iwl4965_get_fh_string(int cmd)
+{
+	switch (cmd) {
+	IWL_CMD(FH_RSCSR_CHNL0_STTS_WPTR_REG);
+	IWL_CMD(FH_RSCSR_CHNL0_RBDCB_BASE_REG);
+	IWL_CMD(FH_RSCSR_CHNL0_WPTR);
+	IWL_CMD(FH_MEM_RCSR_CHNL0_CONFIG_REG);
+	IWL_CMD(FH_MEM_RSSR_SHARED_CTRL_REG);
+	IWL_CMD(FH_MEM_RSSR_RX_STATUS_REG);
+	IWL_CMD(FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV);
+	IWL_CMD(FH_TSSR_TX_STATUS_REG);
+	IWL_CMD(FH_TSSR_TX_ERROR_REG);
+	default:
+		return "UNKNOWN";
+	}
+}
+
+int iwl4965_dump_fh(struct iwl_priv *priv, char **buf, bool display)
+{
+	int i;
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	int pos = 0;
+	size_t bufsz = 0;
+#endif
+	static const u32 fh_tbl[] = {
+		FH_RSCSR_CHNL0_STTS_WPTR_REG,
+		FH_RSCSR_CHNL0_RBDCB_BASE_REG,
+		FH_RSCSR_CHNL0_WPTR,
+		FH_MEM_RCSR_CHNL0_CONFIG_REG,
+		FH_MEM_RSSR_SHARED_CTRL_REG,
+		FH_MEM_RSSR_RX_STATUS_REG,
+		FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV,
+		FH_TSSR_TX_STATUS_REG,
+		FH_TSSR_TX_ERROR_REG
+	};
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (display) {
+		bufsz = ARRAY_SIZE(fh_tbl) * 48 + 40;
+		*buf = kmalloc(bufsz, GFP_KERNEL);
+		if (!*buf)
+			return -ENOMEM;
+		pos += scnprintf(*buf + pos, bufsz - pos,
+				"FH register values:\n");
+		for (i = 0; i < ARRAY_SIZE(fh_tbl); i++) {
+			pos += scnprintf(*buf + pos, bufsz - pos,
+				"  %34s: 0X%08x\n",
+				iwl4965_get_fh_string(fh_tbl[i]),
+				iwl_legacy_read_direct32(priv, fh_tbl[i]));
+		}
+		return pos;
+	}
+#endif
+	IWL_ERR(priv, "FH register values:\n");
+	for (i = 0; i <  ARRAY_SIZE(fh_tbl); i++) {
+		IWL_ERR(priv, "  %34s: 0X%08x\n",
+			iwl4965_get_fh_string(fh_tbl[i]),
+			iwl_legacy_read_direct32(priv, fh_tbl[i]));
+	}
+	return 0;
+}
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-rs.c b/drivers/net/wireless/iwlegacy/iwl-4965-rs.c
new file mode 100644
index 000000000000..31ac672b64e1
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-rs.c
@@ -0,0 +1,2870 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+
+#include <linux/workqueue.h>
+
+#include "iwl-dev.h"
+#include "iwl-sta.h"
+#include "iwl-core.h"
+#include "iwl-4965.h"
+
+#define IWL4965_RS_NAME "iwl-4965-rs"
+
+#define NUM_TRY_BEFORE_ANT_TOGGLE 1
+#define IWL_NUMBER_TRY      1
+#define IWL_HT_NUMBER_TRY   3
+
+#define IWL_RATE_MAX_WINDOW		62	/* # tx in history window */
+#define IWL_RATE_MIN_FAILURE_TH		6	/* min failures to calc tpt */
+#define IWL_RATE_MIN_SUCCESS_TH		8	/* min successes to calc tpt */
+
+/* max allowed rate miss before sync LQ cmd */
+#define IWL_MISSED_RATE_MAX		15
+/* max time to accum history 2 seconds */
+#define IWL_RATE_SCALE_FLUSH_INTVL   (3*HZ)
+
+static u8 rs_ht_to_legacy[] = {
+	IWL_RATE_6M_INDEX, IWL_RATE_6M_INDEX,
+	IWL_RATE_6M_INDEX, IWL_RATE_6M_INDEX,
+	IWL_RATE_6M_INDEX,
+	IWL_RATE_6M_INDEX, IWL_RATE_9M_INDEX,
+	IWL_RATE_12M_INDEX, IWL_RATE_18M_INDEX,
+	IWL_RATE_24M_INDEX, IWL_RATE_36M_INDEX,
+	IWL_RATE_48M_INDEX, IWL_RATE_54M_INDEX
+};
+
+static const u8 ant_toggle_lookup[] = {
+	/*ANT_NONE -> */ ANT_NONE,
+	/*ANT_A    -> */ ANT_B,
+	/*ANT_B    -> */ ANT_C,
+	/*ANT_AB   -> */ ANT_BC,
+	/*ANT_C    -> */ ANT_A,
+	/*ANT_AC   -> */ ANT_AB,
+	/*ANT_BC   -> */ ANT_AC,
+	/*ANT_ABC  -> */ ANT_ABC,
+};
+
+#define IWL_DECLARE_RATE_INFO(r, s, ip, in, rp, rn, pp, np)    \
+	[IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,      \
+				    IWL_RATE_SISO_##s##M_PLCP, \
+				    IWL_RATE_MIMO2_##s##M_PLCP,\
+				    IWL_RATE_##r##M_IEEE,      \
+				    IWL_RATE_##ip##M_INDEX,    \
+				    IWL_RATE_##in##M_INDEX,    \
+				    IWL_RATE_##rp##M_INDEX,    \
+				    IWL_RATE_##rn##M_INDEX,    \
+				    IWL_RATE_##pp##M_INDEX,    \
+				    IWL_RATE_##np##M_INDEX }
+
+/*
+ * Parameter order:
+ *   rate, ht rate, prev rate, next rate, prev tgg rate, next tgg rate
+ *
+ * If there isn't a valid next or previous rate then INV is used which
+ * maps to IWL_RATE_INVALID
+ *
+ */
+const struct iwl_rate_info iwlegacy_rates[IWL_RATE_COUNT] = {
+	IWL_DECLARE_RATE_INFO(1, INV, INV, 2, INV, 2, INV, 2),    /*  1mbps */
+	IWL_DECLARE_RATE_INFO(2, INV, 1, 5, 1, 5, 1, 5),          /*  2mbps */
+	IWL_DECLARE_RATE_INFO(5, INV, 2, 6, 2, 11, 2, 11),        /*5.5mbps */
+	IWL_DECLARE_RATE_INFO(11, INV, 9, 12, 9, 12, 5, 18),      /* 11mbps */
+	IWL_DECLARE_RATE_INFO(6, 6, 5, 9, 5, 11, 5, 11),        /*  6mbps */
+	IWL_DECLARE_RATE_INFO(9, 6, 6, 11, 6, 11, 5, 11),       /*  9mbps */
+	IWL_DECLARE_RATE_INFO(12, 12, 11, 18, 11, 18, 11, 18),   /* 12mbps */
+	IWL_DECLARE_RATE_INFO(18, 18, 12, 24, 12, 24, 11, 24),   /* 18mbps */
+	IWL_DECLARE_RATE_INFO(24, 24, 18, 36, 18, 36, 18, 36),   /* 24mbps */
+	IWL_DECLARE_RATE_INFO(36, 36, 24, 48, 24, 48, 24, 48),   /* 36mbps */
+	IWL_DECLARE_RATE_INFO(48, 48, 36, 54, 36, 54, 36, 54),   /* 48mbps */
+	IWL_DECLARE_RATE_INFO(54, 54, 48, INV, 48, INV, 48, INV),/* 54mbps */
+	IWL_DECLARE_RATE_INFO(60, 60, 48, INV, 48, INV, 48, INV),/* 60mbps */
+};
+
+static int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags)
+{
+	int idx = 0;
+
+	/* HT rate format */
+	if (rate_n_flags & RATE_MCS_HT_MSK) {
+		idx = (rate_n_flags & 0xff);
+
+		if (idx >= IWL_RATE_MIMO2_6M_PLCP)
+			idx = idx - IWL_RATE_MIMO2_6M_PLCP;
+
+		idx += IWL_FIRST_OFDM_RATE;
+		/* skip 9M not supported in ht*/
+		if (idx >= IWL_RATE_9M_INDEX)
+			idx += 1;
+		if ((idx >= IWL_FIRST_OFDM_RATE) && (idx <= IWL_LAST_OFDM_RATE))
+			return idx;
+
+	/* legacy rate format, search for match in table */
+	} else {
+		for (idx = 0; idx < ARRAY_SIZE(iwlegacy_rates); idx++)
+			if (iwlegacy_rates[idx].plcp == (rate_n_flags & 0xFF))
+				return idx;
+	}
+
+	return -1;
+}
+
+static void iwl4965_rs_rate_scale_perform(struct iwl_priv *priv,
+				   struct sk_buff *skb,
+				   struct ieee80211_sta *sta,
+				   struct iwl_lq_sta *lq_sta);
+static void iwl4965_rs_fill_link_cmd(struct iwl_priv *priv,
+			     struct iwl_lq_sta *lq_sta, u32 rate_n_flags);
+static void iwl4965_rs_stay_in_table(struct iwl_lq_sta *lq_sta,
+					bool force_search);
+
+#ifdef CONFIG_MAC80211_DEBUGFS
+static void iwl4965_rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
+			     u32 *rate_n_flags, int index);
+#else
+static void iwl4965_rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
+			     u32 *rate_n_flags, int index)
+{}
+#endif
+
+/**
+ * The following tables contain the expected throughput metrics for all rates
+ *
+ *	1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 60 MBits
+ *
+ * where invalid entries are zeros.
+ *
+ * CCK rates are only valid in legacy table and will only be used in G
+ * (2.4 GHz) band.
+ */
+
+static s32 expected_tpt_legacy[IWL_RATE_COUNT] = {
+	7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 0
+};
+
+static s32 expected_tpt_siso20MHz[4][IWL_RATE_COUNT] = {
+	{0, 0, 0, 0, 42, 0,  76, 102, 124, 158, 183, 193, 202}, /* Norm */
+	{0, 0, 0, 0, 46, 0,  82, 110, 132, 167, 192, 202, 210}, /* SGI */
+	{0, 0, 0, 0, 48, 0,  93, 135, 176, 251, 319, 351, 381}, /* AGG */
+	{0, 0, 0, 0, 53, 0, 102, 149, 193, 275, 348, 381, 413}, /* AGG+SGI */
+};
+
+static s32 expected_tpt_siso40MHz[4][IWL_RATE_COUNT] = {
+	{0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250, 257}, /* Norm */
+	{0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257, 264}, /* SGI */
+	{0, 0, 0, 0,  96, 0, 182, 259, 328, 451, 553, 598, 640}, /* AGG */
+	{0, 0, 0, 0, 106, 0, 199, 282, 357, 487, 593, 640, 683}, /* AGG+SGI */
+};
+
+static s32 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
+	{0, 0, 0, 0,  74, 0, 123, 155, 179, 213, 235, 243, 250}, /* Norm */
+	{0, 0, 0, 0,  81, 0, 131, 164, 187, 221, 242, 250, 256}, /* SGI */
+	{0, 0, 0, 0,  92, 0, 175, 250, 317, 436, 534, 578, 619}, /* AGG */
+	{0, 0, 0, 0, 102, 0, 192, 273, 344, 470, 573, 619, 660}, /* AGG+SGI*/
+};
+
+static s32 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
+	{0, 0, 0, 0, 123, 0, 182, 214, 235, 264, 279, 285, 289}, /* Norm */
+	{0, 0, 0, 0, 131, 0, 191, 222, 242, 270, 284, 289, 293}, /* SGI */
+	{0, 0, 0, 0, 180, 0, 327, 446, 545, 708, 828, 878, 922}, /* AGG */
+	{0, 0, 0, 0, 197, 0, 355, 481, 584, 752, 872, 922, 966}, /* AGG+SGI */
+};
+
+/* mbps, mcs */
+static const struct iwl_rate_mcs_info iwl_rate_mcs[IWL_RATE_COUNT] = {
+	{  "1", "BPSK DSSS"},
+	{  "2", "QPSK DSSS"},
+	{"5.5", "BPSK CCK"},
+	{ "11", "QPSK CCK"},
+	{  "6", "BPSK 1/2"},
+	{  "9", "BPSK 1/2"},
+	{ "12", "QPSK 1/2"},
+	{ "18", "QPSK 3/4"},
+	{ "24", "16QAM 1/2"},
+	{ "36", "16QAM 3/4"},
+	{ "48", "64QAM 2/3"},
+	{ "54", "64QAM 3/4"},
+	{ "60", "64QAM 5/6"},
+};
+
+#define MCS_INDEX_PER_STREAM	(8)
+
+static inline u8 iwl4965_rs_extract_rate(u32 rate_n_flags)
+{
+	return (u8)(rate_n_flags & 0xFF);
+}
+
+static void
+iwl4965_rs_rate_scale_clear_window(struct iwl_rate_scale_data *window)
+{
+	window->data = 0;
+	window->success_counter = 0;
+	window->success_ratio = IWL_INVALID_VALUE;
+	window->counter = 0;
+	window->average_tpt = IWL_INVALID_VALUE;
+	window->stamp = 0;
+}
+
+static inline u8 iwl4965_rs_is_valid_ant(u8 valid_antenna, u8 ant_type)
+{
+	return (ant_type & valid_antenna) == ant_type;
+}
+
+/*
+ *	removes the old data from the statistics. All data that is older than
+ *	TID_MAX_TIME_DIFF, will be deleted.
+ */
+static void
+iwl4965_rs_tl_rm_old_stats(struct iwl_traffic_load *tl, u32 curr_time)
+{
+	/* The oldest age we want to keep */
+	u32 oldest_time = curr_time - TID_MAX_TIME_DIFF;
+
+	while (tl->queue_count &&
+	       (tl->time_stamp < oldest_time)) {
+		tl->total -= tl->packet_count[tl->head];
+		tl->packet_count[tl->head] = 0;
+		tl->time_stamp += TID_QUEUE_CELL_SPACING;
+		tl->queue_count--;
+		tl->head++;
+		if (tl->head >= TID_QUEUE_MAX_SIZE)
+			tl->head = 0;
+	}
+}
+
+/*
+ *	increment traffic load value for tid and also remove
+ *	any old values if passed the certain time period
+ */
+static u8 iwl4965_rs_tl_add_packet(struct iwl_lq_sta *lq_data,
+			   struct ieee80211_hdr *hdr)
+{
+	u32 curr_time = jiffies_to_msecs(jiffies);
+	u32 time_diff;
+	s32 index;
+	struct iwl_traffic_load *tl = NULL;
+	u8 tid;
+
+	if (ieee80211_is_data_qos(hdr->frame_control)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 0xf;
+	} else
+		return MAX_TID_COUNT;
+
+	if (unlikely(tid >= TID_MAX_LOAD_COUNT))
+		return MAX_TID_COUNT;
+
+	tl = &lq_data->load[tid];
+
+	curr_time -= curr_time % TID_ROUND_VALUE;
+
+	/* Happens only for the first packet. Initialize the data */
+	if (!(tl->queue_count)) {
+		tl->total = 1;
+		tl->time_stamp = curr_time;
+		tl->queue_count = 1;
+		tl->head = 0;
+		tl->packet_count[0] = 1;
+		return MAX_TID_COUNT;
+	}
+
+	time_diff = TIME_WRAP_AROUND(tl->time_stamp, curr_time);
+	index = time_diff / TID_QUEUE_CELL_SPACING;
+
+	/* The history is too long: remove data that is older than */
+	/* TID_MAX_TIME_DIFF */
+	if (index >= TID_QUEUE_MAX_SIZE)
+		iwl4965_rs_tl_rm_old_stats(tl, curr_time);
+
+	index = (tl->head + index) % TID_QUEUE_MAX_SIZE;
+	tl->packet_count[index] = tl->packet_count[index] + 1;
+	tl->total = tl->total + 1;
+
+	if ((index + 1) > tl->queue_count)
+		tl->queue_count = index + 1;
+
+	return tid;
+}
+
+/*
+	get the traffic load value for tid
+*/
+static u32 iwl4965_rs_tl_get_load(struct iwl_lq_sta *lq_data, u8 tid)
+{
+	u32 curr_time = jiffies_to_msecs(jiffies);
+	u32 time_diff;
+	s32 index;
+	struct iwl_traffic_load *tl = NULL;
+
+	if (tid >= TID_MAX_LOAD_COUNT)
+		return 0;
+
+	tl = &(lq_data->load[tid]);
+
+	curr_time -= curr_time % TID_ROUND_VALUE;
+
+	if (!(tl->queue_count))
+		return 0;
+
+	time_diff = TIME_WRAP_AROUND(tl->time_stamp, curr_time);
+	index = time_diff / TID_QUEUE_CELL_SPACING;
+
+	/* The history is too long: remove data that is older than */
+	/* TID_MAX_TIME_DIFF */
+	if (index >= TID_QUEUE_MAX_SIZE)
+		iwl4965_rs_tl_rm_old_stats(tl, curr_time);
+
+	return tl->total;
+}
+
+static int iwl4965_rs_tl_turn_on_agg_for_tid(struct iwl_priv *priv,
+				      struct iwl_lq_sta *lq_data, u8 tid,
+				      struct ieee80211_sta *sta)
+{
+	int ret = -EAGAIN;
+	u32 load;
+
+	load = iwl4965_rs_tl_get_load(lq_data, tid);
+
+	if (load > IWL_AGG_LOAD_THRESHOLD) {
+		IWL_DEBUG_HT(priv, "Starting Tx agg: STA: %pM tid: %d\n",
+				sta->addr, tid);
+		ret = ieee80211_start_tx_ba_session(sta, tid, 5000);
+		if (ret == -EAGAIN) {
+			/*
+			 * driver and mac80211 is out of sync
+			 * this might be cause by reloading firmware
+			 * stop the tx ba session here
+			 */
+			IWL_ERR(priv, "Fail start Tx agg on tid: %d\n",
+				tid);
+			ieee80211_stop_tx_ba_session(sta, tid);
+		}
+	} else {
+		IWL_ERR(priv, "Aggregation not enabled for tid %d "
+			"because load = %u\n", tid, load);
+	}
+	return ret;
+}
+
+static void iwl4965_rs_tl_turn_on_agg(struct iwl_priv *priv, u8 tid,
+			      struct iwl_lq_sta *lq_data,
+			      struct ieee80211_sta *sta)
+{
+	if (tid < TID_MAX_LOAD_COUNT)
+		iwl4965_rs_tl_turn_on_agg_for_tid(priv, lq_data, tid, sta);
+	else
+		IWL_ERR(priv, "tid exceeds max load count: %d/%d\n",
+			tid, TID_MAX_LOAD_COUNT);
+}
+
+static inline int iwl4965_get_iwl4965_num_of_ant_from_rate(u32 rate_n_flags)
+{
+	return !!(rate_n_flags & RATE_MCS_ANT_A_MSK) +
+	       !!(rate_n_flags & RATE_MCS_ANT_B_MSK) +
+	       !!(rate_n_flags & RATE_MCS_ANT_C_MSK);
+}
+
+/*
+ * Static function to get the expected throughput from an iwl_scale_tbl_info
+ * that wraps a NULL pointer check
+ */
+static s32
+iwl4965_get_expected_tpt(struct iwl_scale_tbl_info *tbl, int rs_index)
+{
+	if (tbl->expected_tpt)
+		return tbl->expected_tpt[rs_index];
+	return 0;
+}
+
+/**
+ * iwl4965_rs_collect_tx_data - Update the success/failure sliding window
+ *
+ * We keep a sliding window of the last 62 packets transmitted
+ * at this rate.  window->data contains the bitmask of successful
+ * packets.
+ */
+static int iwl4965_rs_collect_tx_data(struct iwl_scale_tbl_info *tbl,
+			      int scale_index, int attempts, int successes)
+{
+	struct iwl_rate_scale_data *window = NULL;
+	static const u64 mask = (((u64)1) << (IWL_RATE_MAX_WINDOW - 1));
+	s32 fail_count, tpt;
+
+	if (scale_index < 0 || scale_index >= IWL_RATE_COUNT)
+		return -EINVAL;
+
+	/* Select window for current tx bit rate */
+	window = &(tbl->win[scale_index]);
+
+	/* Get expected throughput */
+	tpt = iwl4965_get_expected_tpt(tbl, scale_index);
+
+	/*
+	 * Keep track of only the latest 62 tx frame attempts in this rate's
+	 * history window; anything older isn't really relevant any more.
+	 * If we have filled up the sliding window, drop the oldest attempt;
+	 * if the oldest attempt (highest bit in bitmap) shows "success",
+	 * subtract "1" from the success counter (this is the main reason
+	 * we keep these bitmaps!).
+	 */
+	while (attempts > 0) {
+		if (window->counter >= IWL_RATE_MAX_WINDOW) {
+
+			/* remove earliest */
+			window->counter = IWL_RATE_MAX_WINDOW - 1;
+
+			if (window->data & mask) {
+				window->data &= ~mask;
+				window->success_counter--;
+			}
+		}
+
+		/* Increment frames-attempted counter */
+		window->counter++;
+
+		/* Shift bitmap by one frame to throw away oldest history */
+		window->data <<= 1;
+
+		/* Mark the most recent #successes attempts as successful */
+		if (successes > 0) {
+			window->success_counter++;
+			window->data |= 0x1;
+			successes--;
+		}
+
+		attempts--;
+	}
+
+	/* Calculate current success ratio, avoid divide-by-0! */
+	if (window->counter > 0)
+		window->success_ratio = 128 * (100 * window->success_counter)
+					/ window->counter;
+	else
+		window->success_ratio = IWL_INVALID_VALUE;
+
+	fail_count = window->counter - window->success_counter;
+
+	/* Calculate average throughput, if we have enough history. */
+	if ((fail_count >= IWL_RATE_MIN_FAILURE_TH) ||
+	    (window->success_counter >= IWL_RATE_MIN_SUCCESS_TH))
+		window->average_tpt = (window->success_ratio * tpt + 64) / 128;
+	else
+		window->average_tpt = IWL_INVALID_VALUE;
+
+	/* Tag this window as having been updated */
+	window->stamp = jiffies;
+
+	return 0;
+}
+
+/*
+ * Fill uCode API rate_n_flags field, based on "search" or "active" table.
+ */
+static u32 iwl4965_rate_n_flags_from_tbl(struct iwl_priv *priv,
+				 struct iwl_scale_tbl_info *tbl,
+				 int index, u8 use_green)
+{
+	u32 rate_n_flags = 0;
+
+	if (is_legacy(tbl->lq_type)) {
+		rate_n_flags = iwlegacy_rates[index].plcp;
+		if (index >= IWL_FIRST_CCK_RATE && index <= IWL_LAST_CCK_RATE)
+			rate_n_flags |= RATE_MCS_CCK_MSK;
+
+	} else if (is_Ht(tbl->lq_type)) {
+		if (index > IWL_LAST_OFDM_RATE) {
+			IWL_ERR(priv, "Invalid HT rate index %d\n", index);
+			index = IWL_LAST_OFDM_RATE;
+		}
+		rate_n_flags = RATE_MCS_HT_MSK;
+
+		if (is_siso(tbl->lq_type))
+			rate_n_flags |=	iwlegacy_rates[index].plcp_siso;
+		else
+			rate_n_flags |=	iwlegacy_rates[index].plcp_mimo2;
+	} else {
+		IWL_ERR(priv, "Invalid tbl->lq_type %d\n", tbl->lq_type);
+	}
+
+	rate_n_flags |= ((tbl->ant_type << RATE_MCS_ANT_POS) &
+						     RATE_MCS_ANT_ABC_MSK);
+
+	if (is_Ht(tbl->lq_type)) {
+		if (tbl->is_ht40) {
+			if (tbl->is_dup)
+				rate_n_flags |= RATE_MCS_DUP_MSK;
+			else
+				rate_n_flags |= RATE_MCS_HT40_MSK;
+		}
+		if (tbl->is_SGI)
+			rate_n_flags |= RATE_MCS_SGI_MSK;
+
+		if (use_green) {
+			rate_n_flags |= RATE_MCS_GF_MSK;
+			if (is_siso(tbl->lq_type) && tbl->is_SGI) {
+				rate_n_flags &= ~RATE_MCS_SGI_MSK;
+				IWL_ERR(priv, "GF was set with SGI:SISO\n");
+			}
+		}
+	}
+	return rate_n_flags;
+}
+
+/*
+ * Interpret uCode API's rate_n_flags format,
+ * fill "search" or "active" tx mode table.
+ */
+static int iwl4965_rs_get_tbl_info_from_mcs(const u32 rate_n_flags,
+				    enum ieee80211_band band,
+				    struct iwl_scale_tbl_info *tbl,
+				    int *rate_idx)
+{
+	u32 ant_msk = (rate_n_flags & RATE_MCS_ANT_ABC_MSK);
+	u8 iwl4965_num_of_ant = iwl4965_get_iwl4965_num_of_ant_from_rate(rate_n_flags);
+	u8 mcs;
+
+	memset(tbl, 0, sizeof(struct iwl_scale_tbl_info));
+	*rate_idx = iwl4965_hwrate_to_plcp_idx(rate_n_flags);
+
+	if (*rate_idx  == IWL_RATE_INVALID) {
+		*rate_idx = -1;
+		return -EINVAL;
+	}
+	tbl->is_SGI = 0;	/* default legacy setup */
+	tbl->is_ht40 = 0;
+	tbl->is_dup = 0;
+	tbl->ant_type = (ant_msk >> RATE_MCS_ANT_POS);
+	tbl->lq_type = LQ_NONE;
+	tbl->max_search = IWL_MAX_SEARCH;
+
+	/* legacy rate format */
+	if (!(rate_n_flags & RATE_MCS_HT_MSK)) {
+		if (iwl4965_num_of_ant == 1) {
+			if (band == IEEE80211_BAND_5GHZ)
+				tbl->lq_type = LQ_A;
+			else
+				tbl->lq_type = LQ_G;
+		}
+	/* HT rate format */
+	} else {
+		if (rate_n_flags & RATE_MCS_SGI_MSK)
+			tbl->is_SGI = 1;
+
+		if ((rate_n_flags & RATE_MCS_HT40_MSK) ||
+		    (rate_n_flags & RATE_MCS_DUP_MSK))
+			tbl->is_ht40 = 1;
+
+		if (rate_n_flags & RATE_MCS_DUP_MSK)
+			tbl->is_dup = 1;
+
+		mcs = iwl4965_rs_extract_rate(rate_n_flags);
+
+		/* SISO */
+		if (mcs <= IWL_RATE_SISO_60M_PLCP) {
+			if (iwl4965_num_of_ant == 1)
+				tbl->lq_type = LQ_SISO; /*else NONE*/
+		/* MIMO2 */
+		} else {
+			if (iwl4965_num_of_ant == 2)
+				tbl->lq_type = LQ_MIMO2;
+		}
+	}
+	return 0;
+}
+
+/* switch to another antenna/antennas and return 1 */
+/* if no other valid antenna found, return 0 */
+static int iwl4965_rs_toggle_antenna(u32 valid_ant, u32 *rate_n_flags,
+			     struct iwl_scale_tbl_info *tbl)
+{
+	u8 new_ant_type;
+
+	if (!tbl->ant_type || tbl->ant_type > ANT_ABC)
+		return 0;
+
+	if (!iwl4965_rs_is_valid_ant(valid_ant, tbl->ant_type))
+		return 0;
+
+	new_ant_type = ant_toggle_lookup[tbl->ant_type];
+
+	while ((new_ant_type != tbl->ant_type) &&
+	       !iwl4965_rs_is_valid_ant(valid_ant, new_ant_type))
+		new_ant_type = ant_toggle_lookup[new_ant_type];
+
+	if (new_ant_type == tbl->ant_type)
+		return 0;
+
+	tbl->ant_type = new_ant_type;
+	*rate_n_flags &= ~RATE_MCS_ANT_ABC_MSK;
+	*rate_n_flags |= new_ant_type << RATE_MCS_ANT_POS;
+	return 1;
+}
+
+/**
+ * Green-field mode is valid if the station supports it and
+ * there are no non-GF stations present in the BSS.
+ */
+static bool iwl4965_rs_use_green(struct ieee80211_sta *sta)
+{
+	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
+	struct iwl_rxon_context *ctx = sta_priv->common.ctx;
+
+	return (sta->ht_cap.cap & IEEE80211_HT_CAP_GRN_FLD) &&
+		!(ctx->ht.non_gf_sta_present);
+}
+
+/**
+ * iwl4965_rs_get_supported_rates - get the available rates
+ *
+ * if management frame or broadcast frame only return
+ * basic available rates.
+ *
+ */
+static u16 iwl4965_rs_get_supported_rates(struct iwl_lq_sta *lq_sta,
+				  struct ieee80211_hdr *hdr,
+				  enum iwl_table_type rate_type)
+{
+	if (is_legacy(rate_type)) {
+		return lq_sta->active_legacy_rate;
+	} else {
+		if (is_siso(rate_type))
+			return lq_sta->active_siso_rate;
+		else
+			return lq_sta->active_mimo2_rate;
+	}
+}
+
+static u16
+iwl4965_rs_get_adjacent_rate(struct iwl_priv *priv, u8 index, u16 rate_mask,
+				int rate_type)
+{
+	u8 high = IWL_RATE_INVALID;
+	u8 low = IWL_RATE_INVALID;
+
+	/* 802.11A or ht walks to the next literal adjacent rate in
+	 * the rate table */
+	if (is_a_band(rate_type) || !is_legacy(rate_type)) {
+		int i;
+		u32 mask;
+
+		/* Find the previous rate that is in the rate mask */
+		i = index - 1;
+		for (mask = (1 << i); i >= 0; i--, mask >>= 1) {
+			if (rate_mask & mask) {
+				low = i;
+				break;
+			}
+		}
+
+		/* Find the next rate that is in the rate mask */
+		i = index + 1;
+		for (mask = (1 << i); i < IWL_RATE_COUNT; i++, mask <<= 1) {
+			if (rate_mask & mask) {
+				high = i;
+				break;
+			}
+		}
+
+		return (high << 8) | low;
+	}
+
+	low = index;
+	while (low != IWL_RATE_INVALID) {
+		low = iwlegacy_rates[low].prev_rs;
+		if (low == IWL_RATE_INVALID)
+			break;
+		if (rate_mask & (1 << low))
+			break;
+		IWL_DEBUG_RATE(priv, "Skipping masked lower rate: %d\n", low);
+	}
+
+	high = index;
+	while (high != IWL_RATE_INVALID) {
+		high = iwlegacy_rates[high].next_rs;
+		if (high == IWL_RATE_INVALID)
+			break;
+		if (rate_mask & (1 << high))
+			break;
+		IWL_DEBUG_RATE(priv, "Skipping masked higher rate: %d\n", high);
+	}
+
+	return (high << 8) | low;
+}
+
+static u32 iwl4965_rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
+			     struct iwl_scale_tbl_info *tbl,
+			     u8 scale_index, u8 ht_possible)
+{
+	s32 low;
+	u16 rate_mask;
+	u16 high_low;
+	u8 switch_to_legacy = 0;
+	u8 is_green = lq_sta->is_green;
+	struct iwl_priv *priv = lq_sta->drv;
+
+	/* check if we need to switch from HT to legacy rates.
+	 * assumption is that mandatory rates (1Mbps or 6Mbps)
+	 * are always supported (spec demand) */
+	if (!is_legacy(tbl->lq_type) && (!ht_possible || !scale_index)) {
+		switch_to_legacy = 1;
+		scale_index = rs_ht_to_legacy[scale_index];
+		if (lq_sta->band == IEEE80211_BAND_5GHZ)
+			tbl->lq_type = LQ_A;
+		else
+			tbl->lq_type = LQ_G;
+
+		if (iwl4965_num_of_ant(tbl->ant_type) > 1)
+			tbl->ant_type =
+				iwl4965_first_antenna(priv->hw_params.valid_tx_ant);
+
+		tbl->is_ht40 = 0;
+		tbl->is_SGI = 0;
+		tbl->max_search = IWL_MAX_SEARCH;
+	}
+
+	rate_mask = iwl4965_rs_get_supported_rates(lq_sta, NULL, tbl->lq_type);
+
+	/* Mask with station rate restriction */
+	if (is_legacy(tbl->lq_type)) {
+		/* supp_rates has no CCK bits in A mode */
+		if (lq_sta->band == IEEE80211_BAND_5GHZ)
+			rate_mask  = (u16)(rate_mask &
+			   (lq_sta->supp_rates << IWL_FIRST_OFDM_RATE));
+		else
+			rate_mask = (u16)(rate_mask & lq_sta->supp_rates);
+	}
+
+	/* If we switched from HT to legacy, check current rate */
+	if (switch_to_legacy && (rate_mask & (1 << scale_index))) {
+		low = scale_index;
+		goto out;
+	}
+
+	high_low = iwl4965_rs_get_adjacent_rate(lq_sta->drv,
+					scale_index, rate_mask,
+					tbl->lq_type);
+	low = high_low & 0xff;
+
+	if (low == IWL_RATE_INVALID)
+		low = scale_index;
+
+out:
+	return iwl4965_rate_n_flags_from_tbl(lq_sta->drv, tbl, low, is_green);
+}
+
+/*
+ * Simple function to compare two rate scale table types
+ */
+static bool iwl4965_table_type_matches(struct iwl_scale_tbl_info *a,
+			       struct iwl_scale_tbl_info *b)
+{
+	return (a->lq_type == b->lq_type) && (a->ant_type == b->ant_type) &&
+		(a->is_SGI == b->is_SGI);
+}
+
+/*
+ * mac80211 sends us Tx status
+ */
+static void
+iwl4965_rs_tx_status(void *priv_r, struct ieee80211_supported_band *sband,
+			 struct ieee80211_sta *sta, void *priv_sta,
+			 struct sk_buff *skb)
+{
+	int legacy_success;
+	int retries;
+	int rs_index, mac_index, i;
+	struct iwl_lq_sta *lq_sta = priv_sta;
+	struct iwl_link_quality_cmd *table;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct iwl_priv *priv = (struct iwl_priv *)priv_r;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	enum mac80211_rate_control_flags mac_flags;
+	u32 tx_rate;
+	struct iwl_scale_tbl_info tbl_type;
+	struct iwl_scale_tbl_info *curr_tbl, *other_tbl, *tmp_tbl;
+	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
+	struct iwl_rxon_context *ctx = sta_priv->common.ctx;
+
+	IWL_DEBUG_RATE_LIMIT(priv,
+		"get frame ack response, update rate scale window\n");
+
+	/* Treat uninitialized rate scaling data same as non-existing. */
+	if (!lq_sta) {
+		IWL_DEBUG_RATE(priv, "Station rate scaling not created yet.\n");
+		return;
+	} else if (!lq_sta->drv) {
+		IWL_DEBUG_RATE(priv, "Rate scaling not initialized yet.\n");
+		return;
+	}
+
+	if (!ieee80211_is_data(hdr->frame_control) ||
+	    info->flags & IEEE80211_TX_CTL_NO_ACK)
+		return;
+
+	/* This packet was aggregated but doesn't carry status info */
+	if ((info->flags & IEEE80211_TX_CTL_AMPDU) &&
+	    !(info->flags & IEEE80211_TX_STAT_AMPDU))
+		return;
+
+	/*
+	 * Ignore this Tx frame response if its initial rate doesn't match
+	 * that of latest Link Quality command.  There may be stragglers
+	 * from a previous Link Quality command, but we're no longer interested
+	 * in those; they're either from the "active" mode while we're trying
+	 * to check "search" mode, or a prior "search" mode after we've moved
+	 * to a new "search" mode (which might become the new "active" mode).
+	 */
+	table = &lq_sta->lq;
+	tx_rate = le32_to_cpu(table->rs_table[0].rate_n_flags);
+	iwl4965_rs_get_tbl_info_from_mcs(tx_rate,
+			 priv->band, &tbl_type, &rs_index);
+	if (priv->band == IEEE80211_BAND_5GHZ)
+		rs_index -= IWL_FIRST_OFDM_RATE;
+	mac_flags = info->status.rates[0].flags;
+	mac_index = info->status.rates[0].idx;
+	/* For HT packets, map MCS to PLCP */
+	if (mac_flags & IEEE80211_TX_RC_MCS) {
+		mac_index &= RATE_MCS_CODE_MSK;	/* Remove # of streams */
+		if (mac_index >= (IWL_RATE_9M_INDEX - IWL_FIRST_OFDM_RATE))
+			mac_index++;
+		/*
+		 * mac80211 HT index is always zero-indexed; we need to move
+		 * HT OFDM rates after CCK rates in 2.4 GHz band
+		 */
+		if (priv->band == IEEE80211_BAND_2GHZ)
+			mac_index += IWL_FIRST_OFDM_RATE;
+	}
+	/* Here we actually compare this rate to the latest LQ command */
+	if ((mac_index < 0) ||
+	    (tbl_type.is_SGI !=
+			!!(mac_flags & IEEE80211_TX_RC_SHORT_GI)) ||
+	    (tbl_type.is_ht40 !=
+			!!(mac_flags & IEEE80211_TX_RC_40_MHZ_WIDTH)) ||
+	    (tbl_type.is_dup !=
+			!!(mac_flags & IEEE80211_TX_RC_DUP_DATA)) ||
+	    (tbl_type.ant_type != info->antenna_sel_tx) ||
+	    (!!(tx_rate & RATE_MCS_HT_MSK) !=
+			!!(mac_flags & IEEE80211_TX_RC_MCS)) ||
+	    (!!(tx_rate & RATE_MCS_GF_MSK) !=
+			!!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
+	    (rs_index != mac_index)) {
+		IWL_DEBUG_RATE(priv,
+		"initial rate %d does not match %d (0x%x)\n",
+			 mac_index, rs_index, tx_rate);
+		/*
+		 * Since rates mis-match, the last LQ command may have failed.
+		 * After IWL_MISSED_RATE_MAX mis-matches, resync the uCode with
+		 * ... driver.
+		 */
+		lq_sta->missed_rate_counter++;
+		if (lq_sta->missed_rate_counter > IWL_MISSED_RATE_MAX) {
+			lq_sta->missed_rate_counter = 0;
+			iwl_legacy_send_lq_cmd(priv, ctx, &lq_sta->lq,
+							CMD_ASYNC, false);
+		}
+		/* Regardless, ignore this status info for outdated rate */
+		return;
+	} else
+		/* Rate did match, so reset the missed_rate_counter */
+		lq_sta->missed_rate_counter = 0;
+
+	/* Figure out if rate scale algorithm is in active or search table */
+	if (iwl4965_table_type_matches(&tbl_type,
+				&(lq_sta->lq_info[lq_sta->active_tbl]))) {
+		curr_tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+		other_tbl = &(lq_sta->lq_info[1 - lq_sta->active_tbl]);
+	} else if (iwl4965_table_type_matches(&tbl_type,
+				&lq_sta->lq_info[1 - lq_sta->active_tbl])) {
+		curr_tbl = &(lq_sta->lq_info[1 - lq_sta->active_tbl]);
+		other_tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+	} else {
+		IWL_DEBUG_RATE(priv,
+			"Neither active nor search matches tx rate\n");
+		tmp_tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+		IWL_DEBUG_RATE(priv, "active- lq:%x, ant:%x, SGI:%d\n",
+			tmp_tbl->lq_type, tmp_tbl->ant_type, tmp_tbl->is_SGI);
+		tmp_tbl = &(lq_sta->lq_info[1 - lq_sta->active_tbl]);
+		IWL_DEBUG_RATE(priv, "search- lq:%x, ant:%x, SGI:%d\n",
+			tmp_tbl->lq_type, tmp_tbl->ant_type, tmp_tbl->is_SGI);
+		IWL_DEBUG_RATE(priv, "actual- lq:%x, ant:%x, SGI:%d\n",
+			tbl_type.lq_type, tbl_type.ant_type, tbl_type.is_SGI);
+		/*
+		 * no matching table found, let's by-pass the data collection
+		 * and continue to perform rate scale to find the rate table
+		 */
+		iwl4965_rs_stay_in_table(lq_sta, true);
+		goto done;
+	}
+
+	/*
+	 * Updating the frame history depends on whether packets were
+	 * aggregated.
+	 *
+	 * For aggregation, all packets were transmitted at the same rate, the
+	 * first index into rate scale table.
+	 */
+	if (info->flags & IEEE80211_TX_STAT_AMPDU) {
+		tx_rate = le32_to_cpu(table->rs_table[0].rate_n_flags);
+		iwl4965_rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type,
+				&rs_index);
+		iwl4965_rs_collect_tx_data(curr_tbl, rs_index,
+				   info->status.ampdu_len,
+				   info->status.ampdu_ack_len);
+
+		/* Update success/fail counts if not searching for new mode */
+		if (lq_sta->stay_in_tbl) {
+			lq_sta->total_success += info->status.ampdu_ack_len;
+			lq_sta->total_failed += (info->status.ampdu_len -
+					info->status.ampdu_ack_len);
+		}
+	} else {
+	/*
+	 * For legacy, update frame history with for each Tx retry.
+	 */
+		retries = info->status.rates[0].count - 1;
+		/* HW doesn't send more than 15 retries */
+		retries = min(retries, 15);
+
+		/* The last transmission may have been successful */
+		legacy_success = !!(info->flags & IEEE80211_TX_STAT_ACK);
+		/* Collect data for each rate used during failed TX attempts */
+		for (i = 0; i <= retries; ++i) {
+			tx_rate = le32_to_cpu(table->rs_table[i].rate_n_flags);
+			iwl4965_rs_get_tbl_info_from_mcs(tx_rate, priv->band,
+					&tbl_type, &rs_index);
+			/*
+			 * Only collect stats if retried rate is in the same RS
+			 * table as active/search.
+			 */
+			if (iwl4965_table_type_matches(&tbl_type, curr_tbl))
+				tmp_tbl = curr_tbl;
+			else if (iwl4965_table_type_matches(&tbl_type,
+								 other_tbl))
+				tmp_tbl = other_tbl;
+			else
+				continue;
+			iwl4965_rs_collect_tx_data(tmp_tbl, rs_index, 1,
+					   i < retries ? 0 : legacy_success);
+		}
+
+		/* Update success/fail counts if not searching for new mode */
+		if (lq_sta->stay_in_tbl) {
+			lq_sta->total_success += legacy_success;
+			lq_sta->total_failed += retries + (1 - legacy_success);
+		}
+	}
+	/* The last TX rate is cached in lq_sta; it's set in if/else above */
+	lq_sta->last_rate_n_flags = tx_rate;
+done:
+	/* See if there's a better rate or modulation mode to try. */
+	if (sta && sta->supp_rates[sband->band])
+		iwl4965_rs_rate_scale_perform(priv, skb, sta, lq_sta);
+}
+
+/*
+ * Begin a period of staying with a selected modulation mode.
+ * Set "stay_in_tbl" flag to prevent any mode switches.
+ * Set frame tx success limits according to legacy vs. high-throughput,
+ * and reset overall (spanning all rates) tx success history statistics.
+ * These control how long we stay using same modulation mode before
+ * searching for a new mode.
+ */
+static void iwl4965_rs_set_stay_in_table(struct iwl_priv *priv, u8 is_legacy,
+				 struct iwl_lq_sta *lq_sta)
+{
+	IWL_DEBUG_RATE(priv, "we are staying in the same table\n");
+	lq_sta->stay_in_tbl = 1;	/* only place this gets set */
+	if (is_legacy) {
+		lq_sta->table_count_limit = IWL_LEGACY_TABLE_COUNT;
+		lq_sta->max_failure_limit = IWL_LEGACY_FAILURE_LIMIT;
+		lq_sta->max_success_limit = IWL_LEGACY_SUCCESS_LIMIT;
+	} else {
+		lq_sta->table_count_limit = IWL_NONE_LEGACY_TABLE_COUNT;
+		lq_sta->max_failure_limit = IWL_NONE_LEGACY_FAILURE_LIMIT;
+		lq_sta->max_success_limit = IWL_NONE_LEGACY_SUCCESS_LIMIT;
+	}
+	lq_sta->table_count = 0;
+	lq_sta->total_failed = 0;
+	lq_sta->total_success = 0;
+	lq_sta->flush_timer = jiffies;
+	lq_sta->action_counter = 0;
+}
+
+/*
+ * Find correct throughput table for given mode of modulation
+ */
+static void iwl4965_rs_set_expected_tpt_table(struct iwl_lq_sta *lq_sta,
+				      struct iwl_scale_tbl_info *tbl)
+{
+	/* Used to choose among HT tables */
+	s32 (*ht_tbl_pointer)[IWL_RATE_COUNT];
+
+	/* Check for invalid LQ type */
+	if (WARN_ON_ONCE(!is_legacy(tbl->lq_type) && !is_Ht(tbl->lq_type))) {
+		tbl->expected_tpt = expected_tpt_legacy;
+		return;
+	}
+
+	/* Legacy rates have only one table */
+	if (is_legacy(tbl->lq_type)) {
+		tbl->expected_tpt = expected_tpt_legacy;
+		return;
+	}
+
+	/* Choose among many HT tables depending on number of streams
+	 * (SISO/MIMO2), channel width (20/40), SGI, and aggregation
+	 * status */
+	if (is_siso(tbl->lq_type) && (!tbl->is_ht40 || lq_sta->is_dup))
+		ht_tbl_pointer = expected_tpt_siso20MHz;
+	else if (is_siso(tbl->lq_type))
+		ht_tbl_pointer = expected_tpt_siso40MHz;
+	else if (is_mimo2(tbl->lq_type) && (!tbl->is_ht40 || lq_sta->is_dup))
+		ht_tbl_pointer = expected_tpt_mimo2_20MHz;
+	else /* if (is_mimo2(tbl->lq_type)) <-- must be true */
+		ht_tbl_pointer = expected_tpt_mimo2_40MHz;
+
+	if (!tbl->is_SGI && !lq_sta->is_agg)		/* Normal */
+		tbl->expected_tpt = ht_tbl_pointer[0];
+	else if (tbl->is_SGI && !lq_sta->is_agg)	/* SGI */
+		tbl->expected_tpt = ht_tbl_pointer[1];
+	else if (!tbl->is_SGI && lq_sta->is_agg)	/* AGG */
+		tbl->expected_tpt = ht_tbl_pointer[2];
+	else						/* AGG+SGI */
+		tbl->expected_tpt = ht_tbl_pointer[3];
+}
+
+/*
+ * Find starting rate for new "search" high-throughput mode of modulation.
+ * Goal is to find lowest expected rate (under perfect conditions) that is
+ * above the current measured throughput of "active" mode, to give new mode
+ * a fair chance to prove itself without too many challenges.
+ *
+ * This gets called when transitioning to more aggressive modulation
+ * (i.e. legacy to SISO or MIMO, or SISO to MIMO), as well as less aggressive
+ * (i.e. MIMO to SISO).  When moving to MIMO, bit rate will typically need
+ * to decrease to match "active" throughput.  When moving from MIMO to SISO,
+ * bit rate will typically need to increase, but not if performance was bad.
+ */
+static s32 iwl4965_rs_get_best_rate(struct iwl_priv *priv,
+			    struct iwl_lq_sta *lq_sta,
+			    struct iwl_scale_tbl_info *tbl,	/* "search" */
+			    u16 rate_mask, s8 index)
+{
+	/* "active" values */
+	struct iwl_scale_tbl_info *active_tbl =
+	    &(lq_sta->lq_info[lq_sta->active_tbl]);
+	s32 active_sr = active_tbl->win[index].success_ratio;
+	s32 active_tpt = active_tbl->expected_tpt[index];
+
+	/* expected "search" throughput */
+	s32 *tpt_tbl = tbl->expected_tpt;
+
+	s32 new_rate, high, low, start_hi;
+	u16 high_low;
+	s8 rate = index;
+
+	new_rate = high = low = start_hi = IWL_RATE_INVALID;
+
+	for (; ;) {
+		high_low = iwl4965_rs_get_adjacent_rate(priv, rate, rate_mask,
+						tbl->lq_type);
+
+		low = high_low & 0xff;
+		high = (high_low >> 8) & 0xff;
+
+		/*
+		 * Lower the "search" bit rate, to give new "search" mode
+		 * approximately the same throughput as "active" if:
+		 *
+		 * 1) "Active" mode has been working modestly well (but not
+		 *    great), and expected "search" throughput (under perfect
+		 *    conditions) at candidate rate is above the actual
+		 *    measured "active" throughput (but less than expected
+		 *    "active" throughput under perfect conditions).
+		 * OR
+		 * 2) "Active" mode has been working perfectly or very well
+		 *    and expected "search" throughput (under perfect
+		 *    conditions) at candidate rate is above expected
+		 *    "active" throughput (under perfect conditions).
+		 */
+		if ((((100 * tpt_tbl[rate]) > lq_sta->last_tpt) &&
+		     ((active_sr > IWL_RATE_DECREASE_TH) &&
+		      (active_sr <= IWL_RATE_HIGH_TH) &&
+		      (tpt_tbl[rate] <= active_tpt))) ||
+		    ((active_sr >= IWL_RATE_SCALE_SWITCH) &&
+		     (tpt_tbl[rate] > active_tpt))) {
+
+			/* (2nd or later pass)
+			 * If we've already tried to raise the rate, and are
+			 * now trying to lower it, use the higher rate. */
+			if (start_hi != IWL_RATE_INVALID) {
+				new_rate = start_hi;
+				break;
+			}
+
+			new_rate = rate;
+
+			/* Loop again with lower rate */
+			if (low != IWL_RATE_INVALID)
+				rate = low;
+
+			/* Lower rate not available, use the original */
+			else
+				break;
+
+		/* Else try to raise the "search" rate to match "active" */
+		} else {
+			/* (2nd or later pass)
+			 * If we've already tried to lower the rate, and are
+			 * now trying to raise it, use the lower rate. */
+			if (new_rate != IWL_RATE_INVALID)
+				break;
+
+			/* Loop again with higher rate */
+			else if (high != IWL_RATE_INVALID) {
+				start_hi = high;
+				rate = high;
+
+			/* Higher rate not available, use the original */
+			} else {
+				new_rate = rate;
+				break;
+			}
+		}
+	}
+
+	return new_rate;
+}
+
+/*
+ * Set up search table for MIMO2
+ */
+static int iwl4965_rs_switch_to_mimo2(struct iwl_priv *priv,
+			     struct iwl_lq_sta *lq_sta,
+			     struct ieee80211_conf *conf,
+			     struct ieee80211_sta *sta,
+			     struct iwl_scale_tbl_info *tbl, int index)
+{
+	u16 rate_mask;
+	s32 rate;
+	s8 is_green = lq_sta->is_green;
+	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
+	struct iwl_rxon_context *ctx = sta_priv->common.ctx;
+
+	if (!conf_is_ht(conf) || !sta->ht_cap.ht_supported)
+		return -1;
+
+	if (((sta->ht_cap.cap & IEEE80211_HT_CAP_SM_PS) >> 2)
+						== WLAN_HT_CAP_SM_PS_STATIC)
+		return -1;
+
+	/* Need both Tx chains/antennas to support MIMO */
+	if (priv->hw_params.tx_chains_num < 2)
+		return -1;
+
+	IWL_DEBUG_RATE(priv, "LQ: try to switch to MIMO2\n");
+
+	tbl->lq_type = LQ_MIMO2;
+	tbl->is_dup = lq_sta->is_dup;
+	tbl->action = 0;
+	tbl->max_search = IWL_MAX_SEARCH;
+	rate_mask = lq_sta->active_mimo2_rate;
+
+	if (iwl_legacy_is_ht40_tx_allowed(priv, ctx, &sta->ht_cap))
+		tbl->is_ht40 = 1;
+	else
+		tbl->is_ht40 = 0;
+
+	iwl4965_rs_set_expected_tpt_table(lq_sta, tbl);
+
+	rate = iwl4965_rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index);
+
+	IWL_DEBUG_RATE(priv, "LQ: MIMO2 best rate %d mask %X\n",
+				rate, rate_mask);
+	if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
+		IWL_DEBUG_RATE(priv,
+				"Can't switch with index %d rate mask %x\n",
+						rate, rate_mask);
+		return -1;
+	}
+	tbl->current_rate = iwl4965_rate_n_flags_from_tbl(priv,
+						 tbl, rate, is_green);
+
+	IWL_DEBUG_RATE(priv, "LQ: Switch to new mcs %X index is green %X\n",
+		     tbl->current_rate, is_green);
+	return 0;
+}
+
+/*
+ * Set up search table for SISO
+ */
+static int iwl4965_rs_switch_to_siso(struct iwl_priv *priv,
+			     struct iwl_lq_sta *lq_sta,
+			     struct ieee80211_conf *conf,
+			     struct ieee80211_sta *sta,
+			     struct iwl_scale_tbl_info *tbl, int index)
+{
+	u16 rate_mask;
+	u8 is_green = lq_sta->is_green;
+	s32 rate;
+	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
+	struct iwl_rxon_context *ctx = sta_priv->common.ctx;
+
+	if (!conf_is_ht(conf) || !sta->ht_cap.ht_supported)
+		return -1;
+
+	IWL_DEBUG_RATE(priv, "LQ: try to switch to SISO\n");
+
+	tbl->is_dup = lq_sta->is_dup;
+	tbl->lq_type = LQ_SISO;
+	tbl->action = 0;
+	tbl->max_search = IWL_MAX_SEARCH;
+	rate_mask = lq_sta->active_siso_rate;
+
+	if (iwl_legacy_is_ht40_tx_allowed(priv, ctx, &sta->ht_cap))
+		tbl->is_ht40 = 1;
+	else
+		tbl->is_ht40 = 0;
+
+	if (is_green)
+		tbl->is_SGI = 0; /*11n spec: no SGI in SISO+Greenfield*/
+
+	iwl4965_rs_set_expected_tpt_table(lq_sta, tbl);
+	rate = iwl4965_rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index);
+
+	IWL_DEBUG_RATE(priv, "LQ: get best rate %d mask %X\n", rate, rate_mask);
+	if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
+		IWL_DEBUG_RATE(priv,
+			"can not switch with index %d rate mask %x\n",
+			     rate, rate_mask);
+		return -1;
+	}
+	tbl->current_rate = iwl4965_rate_n_flags_from_tbl(priv,
+						tbl, rate, is_green);
+	IWL_DEBUG_RATE(priv, "LQ: Switch to new mcs %X index is green %X\n",
+		     tbl->current_rate, is_green);
+	return 0;
+}
+
+/*
+ * Try to switch to new modulation mode from legacy
+ */
+static int iwl4965_rs_move_legacy_other(struct iwl_priv *priv,
+				struct iwl_lq_sta *lq_sta,
+				struct ieee80211_conf *conf,
+				struct ieee80211_sta *sta,
+				int index)
+{
+	struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+	struct iwl_scale_tbl_info *search_tbl =
+				&(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
+	struct iwl_rate_scale_data *window = &(tbl->win[index]);
+	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
+		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
+	u8 start_action;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
+	int ret = 0;
+	u8 update_search_tbl_counter = 0;
+
+	tbl->action = IWL_LEGACY_SWITCH_SISO;
+
+	start_action = tbl->action;
+	for (; ;) {
+		lq_sta->action_counter++;
+		switch (tbl->action) {
+		case IWL_LEGACY_SWITCH_ANTENNA1:
+		case IWL_LEGACY_SWITCH_ANTENNA2:
+			IWL_DEBUG_RATE(priv, "LQ: Legacy toggle Antenna\n");
+
+			if ((tbl->action == IWL_LEGACY_SWITCH_ANTENNA1 &&
+							tx_chains_num <= 1) ||
+			    (tbl->action == IWL_LEGACY_SWITCH_ANTENNA2 &&
+							tx_chains_num <= 2))
+				break;
+
+			/* Don't change antenna if success has been great */
+			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
+				break;
+
+			/* Set up search table to try other antenna */
+			memcpy(search_tbl, tbl, sz);
+
+			if (iwl4965_rs_toggle_antenna(valid_tx_ant,
+				&search_tbl->current_rate, search_tbl)) {
+				update_search_tbl_counter = 1;
+				iwl4965_rs_set_expected_tpt_table(lq_sta,
+								search_tbl);
+				goto out;
+			}
+			break;
+		case IWL_LEGACY_SWITCH_SISO:
+			IWL_DEBUG_RATE(priv, "LQ: Legacy switch to SISO\n");
+
+			/* Set up search table to try SISO */
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->is_SGI = 0;
+			ret = iwl4965_rs_switch_to_siso(priv, lq_sta, conf, sta,
+						 search_tbl, index);
+			if (!ret) {
+				lq_sta->action_counter = 0;
+				goto out;
+			}
+
+			break;
+		case IWL_LEGACY_SWITCH_MIMO2_AB:
+		case IWL_LEGACY_SWITCH_MIMO2_AC:
+		case IWL_LEGACY_SWITCH_MIMO2_BC:
+			IWL_DEBUG_RATE(priv, "LQ: Legacy switch to MIMO2\n");
+
+			/* Set up search table to try MIMO */
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->is_SGI = 0;
+
+			if (tbl->action == IWL_LEGACY_SWITCH_MIMO2_AB)
+				search_tbl->ant_type = ANT_AB;
+			else if (tbl->action == IWL_LEGACY_SWITCH_MIMO2_AC)
+				search_tbl->ant_type = ANT_AC;
+			else
+				search_tbl->ant_type = ANT_BC;
+
+			if (!iwl4965_rs_is_valid_ant(valid_tx_ant,
+						search_tbl->ant_type))
+				break;
+
+			ret = iwl4965_rs_switch_to_mimo2(priv, lq_sta,
+						conf, sta,
+						 search_tbl, index);
+			if (!ret) {
+				lq_sta->action_counter = 0;
+				goto out;
+			}
+			break;
+		}
+		tbl->action++;
+		if (tbl->action > IWL_LEGACY_SWITCH_MIMO2_BC)
+			tbl->action = IWL_LEGACY_SWITCH_ANTENNA1;
+
+		if (tbl->action == start_action)
+			break;
+
+	}
+	search_tbl->lq_type = LQ_NONE;
+	return 0;
+
+out:
+	lq_sta->search_better_tbl = 1;
+	tbl->action++;
+	if (tbl->action > IWL_LEGACY_SWITCH_MIMO2_BC)
+		tbl->action = IWL_LEGACY_SWITCH_ANTENNA1;
+	if (update_search_tbl_counter)
+		search_tbl->action = tbl->action;
+	return 0;
+
+}
+
+/*
+ * Try to switch to new modulation mode from SISO
+ */
+static int iwl4965_rs_move_siso_to_other(struct iwl_priv *priv,
+				 struct iwl_lq_sta *lq_sta,
+				 struct ieee80211_conf *conf,
+				 struct ieee80211_sta *sta, int index)
+{
+	u8 is_green = lq_sta->is_green;
+	struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+	struct iwl_scale_tbl_info *search_tbl =
+				&(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
+	struct iwl_rate_scale_data *window = &(tbl->win[index]);
+	struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
+	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
+		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
+	u8 start_action;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
+	u8 update_search_tbl_counter = 0;
+	int ret;
+
+	start_action = tbl->action;
+
+	for (;;) {
+		lq_sta->action_counter++;
+		switch (tbl->action) {
+		case IWL_SISO_SWITCH_ANTENNA1:
+		case IWL_SISO_SWITCH_ANTENNA2:
+			IWL_DEBUG_RATE(priv, "LQ: SISO toggle Antenna\n");
+			if ((tbl->action == IWL_SISO_SWITCH_ANTENNA1 &&
+						tx_chains_num <= 1) ||
+			    (tbl->action == IWL_SISO_SWITCH_ANTENNA2 &&
+						tx_chains_num <= 2))
+				break;
+
+			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
+				break;
+
+			memcpy(search_tbl, tbl, sz);
+			if (iwl4965_rs_toggle_antenna(valid_tx_ant,
+				       &search_tbl->current_rate, search_tbl)) {
+				update_search_tbl_counter = 1;
+				goto out;
+			}
+			break;
+		case IWL_SISO_SWITCH_MIMO2_AB:
+		case IWL_SISO_SWITCH_MIMO2_AC:
+		case IWL_SISO_SWITCH_MIMO2_BC:
+			IWL_DEBUG_RATE(priv, "LQ: SISO switch to MIMO2\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->is_SGI = 0;
+
+			if (tbl->action == IWL_SISO_SWITCH_MIMO2_AB)
+				search_tbl->ant_type = ANT_AB;
+			else if (tbl->action == IWL_SISO_SWITCH_MIMO2_AC)
+				search_tbl->ant_type = ANT_AC;
+			else
+				search_tbl->ant_type = ANT_BC;
+
+			if (!iwl4965_rs_is_valid_ant(valid_tx_ant,
+						 search_tbl->ant_type))
+				break;
+
+			ret = iwl4965_rs_switch_to_mimo2(priv, lq_sta,
+						conf, sta,
+						 search_tbl, index);
+			if (!ret)
+				goto out;
+			break;
+		case IWL_SISO_SWITCH_GI:
+			if (!tbl->is_ht40 && !(ht_cap->cap &
+						IEEE80211_HT_CAP_SGI_20))
+				break;
+			if (tbl->is_ht40 && !(ht_cap->cap &
+						IEEE80211_HT_CAP_SGI_40))
+				break;
+
+			IWL_DEBUG_RATE(priv, "LQ: SISO toggle SGI/NGI\n");
+
+			memcpy(search_tbl, tbl, sz);
+			if (is_green) {
+				if (!tbl->is_SGI)
+					break;
+				else
+					IWL_ERR(priv,
+						"SGI was set in GF+SISO\n");
+			}
+			search_tbl->is_SGI = !tbl->is_SGI;
+			iwl4965_rs_set_expected_tpt_table(lq_sta, search_tbl);
+			if (tbl->is_SGI) {
+				s32 tpt = lq_sta->last_tpt / 100;
+				if (tpt >= search_tbl->expected_tpt[index])
+					break;
+			}
+			search_tbl->current_rate =
+				iwl4965_rate_n_flags_from_tbl(priv, search_tbl,
+						      index, is_green);
+			update_search_tbl_counter = 1;
+			goto out;
+		}
+		tbl->action++;
+		if (tbl->action > IWL_SISO_SWITCH_GI)
+			tbl->action = IWL_SISO_SWITCH_ANTENNA1;
+
+		if (tbl->action == start_action)
+			break;
+	}
+	search_tbl->lq_type = LQ_NONE;
+	return 0;
+
+ out:
+	lq_sta->search_better_tbl = 1;
+	tbl->action++;
+	if (tbl->action > IWL_SISO_SWITCH_GI)
+		tbl->action = IWL_SISO_SWITCH_ANTENNA1;
+	if (update_search_tbl_counter)
+		search_tbl->action = tbl->action;
+
+	return 0;
+}
+
+/*
+ * Try to switch to new modulation mode from MIMO2
+ */
+static int iwl4965_rs_move_mimo2_to_other(struct iwl_priv *priv,
+				 struct iwl_lq_sta *lq_sta,
+				 struct ieee80211_conf *conf,
+				 struct ieee80211_sta *sta, int index)
+{
+	s8 is_green = lq_sta->is_green;
+	struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+	struct iwl_scale_tbl_info *search_tbl =
+				&(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
+	struct iwl_rate_scale_data *window = &(tbl->win[index]);
+	struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
+	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
+		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
+	u8 start_action;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
+	u8 update_search_tbl_counter = 0;
+	int ret;
+
+	start_action = tbl->action;
+	for (;;) {
+		lq_sta->action_counter++;
+		switch (tbl->action) {
+		case IWL_MIMO2_SWITCH_ANTENNA1:
+		case IWL_MIMO2_SWITCH_ANTENNA2:
+			IWL_DEBUG_RATE(priv, "LQ: MIMO2 toggle Antennas\n");
+
+			if (tx_chains_num <= 2)
+				break;
+
+			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
+				break;
+
+			memcpy(search_tbl, tbl, sz);
+			if (iwl4965_rs_toggle_antenna(valid_tx_ant,
+				       &search_tbl->current_rate, search_tbl)) {
+				update_search_tbl_counter = 1;
+				goto out;
+			}
+			break;
+		case IWL_MIMO2_SWITCH_SISO_A:
+		case IWL_MIMO2_SWITCH_SISO_B:
+		case IWL_MIMO2_SWITCH_SISO_C:
+			IWL_DEBUG_RATE(priv, "LQ: MIMO2 switch to SISO\n");
+
+			/* Set up new search table for SISO */
+			memcpy(search_tbl, tbl, sz);
+
+			if (tbl->action == IWL_MIMO2_SWITCH_SISO_A)
+				search_tbl->ant_type = ANT_A;
+			else if (tbl->action == IWL_MIMO2_SWITCH_SISO_B)
+				search_tbl->ant_type = ANT_B;
+			else
+				search_tbl->ant_type = ANT_C;
+
+			if (!iwl4965_rs_is_valid_ant(valid_tx_ant,
+						search_tbl->ant_type))
+				break;
+
+			ret = iwl4965_rs_switch_to_siso(priv, lq_sta,
+						conf, sta,
+						 search_tbl, index);
+			if (!ret)
+				goto out;
+
+			break;
+
+		case IWL_MIMO2_SWITCH_GI:
+			if (!tbl->is_ht40 && !(ht_cap->cap &
+						IEEE80211_HT_CAP_SGI_20))
+				break;
+			if (tbl->is_ht40 && !(ht_cap->cap &
+						IEEE80211_HT_CAP_SGI_40))
+				break;
+
+			IWL_DEBUG_RATE(priv, "LQ: MIMO2 toggle SGI/NGI\n");
+
+			/* Set up new search table for MIMO2 */
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->is_SGI = !tbl->is_SGI;
+			iwl4965_rs_set_expected_tpt_table(lq_sta, search_tbl);
+			/*
+			 * If active table already uses the fastest possible
+			 * modulation (dual stream with short guard interval),
+			 * and it's working well, there's no need to look
+			 * for a better type of modulation!
+			 */
+			if (tbl->is_SGI) {
+				s32 tpt = lq_sta->last_tpt / 100;
+				if (tpt >= search_tbl->expected_tpt[index])
+					break;
+			}
+			search_tbl->current_rate =
+				iwl4965_rate_n_flags_from_tbl(priv, search_tbl,
+						      index, is_green);
+			update_search_tbl_counter = 1;
+			goto out;
+
+		}
+		tbl->action++;
+		if (tbl->action > IWL_MIMO2_SWITCH_GI)
+			tbl->action = IWL_MIMO2_SWITCH_ANTENNA1;
+
+		if (tbl->action == start_action)
+			break;
+	}
+	search_tbl->lq_type = LQ_NONE;
+	return 0;
+ out:
+	lq_sta->search_better_tbl = 1;
+	tbl->action++;
+	if (tbl->action > IWL_MIMO2_SWITCH_GI)
+		tbl->action = IWL_MIMO2_SWITCH_ANTENNA1;
+	if (update_search_tbl_counter)
+		search_tbl->action = tbl->action;
+
+	return 0;
+
+}
+
+/*
+ * Check whether we should continue using same modulation mode, or
+ * begin search for a new mode, based on:
+ * 1) # tx successes or failures while using this mode
+ * 2) # times calling this function
+ * 3) elapsed time in this mode (not used, for now)
+ */
+static void
+iwl4965_rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
+{
+	struct iwl_scale_tbl_info *tbl;
+	int i;
+	int active_tbl;
+	int flush_interval_passed = 0;
+	struct iwl_priv *priv;
+
+	priv = lq_sta->drv;
+	active_tbl = lq_sta->active_tbl;
+
+	tbl = &(lq_sta->lq_info[active_tbl]);
+
+	/* If we've been disallowing search, see if we should now allow it */
+	if (lq_sta->stay_in_tbl) {
+
+		/* Elapsed time using current modulation mode */
+		if (lq_sta->flush_timer)
+			flush_interval_passed =
+			time_after(jiffies,
+					(unsigned long)(lq_sta->flush_timer +
+					IWL_RATE_SCALE_FLUSH_INTVL));
+
+		/*
+		 * Check if we should allow search for new modulation mode.
+		 * If many frames have failed or succeeded, or we've used
+		 * this same modulation for a long time, allow search, and
+		 * reset history stats that keep track of whether we should
+		 * allow a new search.  Also (below) reset all bitmaps and
+		 * stats in active history.
+		 */
+		if (force_search ||
+		    (lq_sta->total_failed > lq_sta->max_failure_limit) ||
+		    (lq_sta->total_success > lq_sta->max_success_limit) ||
+		    ((!lq_sta->search_better_tbl) && (lq_sta->flush_timer)
+		     && (flush_interval_passed))) {
+			IWL_DEBUG_RATE(priv, "LQ: stay is expired %d %d %d\n:",
+				     lq_sta->total_failed,
+				     lq_sta->total_success,
+				     flush_interval_passed);
+
+			/* Allow search for new mode */
+			lq_sta->stay_in_tbl = 0;	/* only place reset */
+			lq_sta->total_failed = 0;
+			lq_sta->total_success = 0;
+			lq_sta->flush_timer = 0;
+
+		/*
+		 * Else if we've used this modulation mode enough repetitions
+		 * (regardless of elapsed time or success/failure), reset
+		 * history bitmaps and rate-specific stats for all rates in
+		 * active table.
+		 */
+		} else {
+			lq_sta->table_count++;
+			if (lq_sta->table_count >=
+			    lq_sta->table_count_limit) {
+				lq_sta->table_count = 0;
+
+				IWL_DEBUG_RATE(priv,
+					"LQ: stay in table clear win\n");
+				for (i = 0; i < IWL_RATE_COUNT; i++)
+					iwl4965_rs_rate_scale_clear_window(
+						&(tbl->win[i]));
+			}
+		}
+
+		/* If transitioning to allow "search", reset all history
+		 * bitmaps and stats in active table (this will become the new
+		 * "search" table). */
+		if (!lq_sta->stay_in_tbl) {
+			for (i = 0; i < IWL_RATE_COUNT; i++)
+				iwl4965_rs_rate_scale_clear_window(
+							&(tbl->win[i]));
+		}
+	}
+}
+
+/*
+ * setup rate table in uCode
+ * return rate_n_flags as used in the table
+ */
+static u32 iwl4965_rs_update_rate_tbl(struct iwl_priv *priv,
+			      struct iwl_rxon_context *ctx,
+				struct iwl_lq_sta *lq_sta,
+				struct iwl_scale_tbl_info *tbl,
+				int index, u8 is_green)
+{
+	u32 rate;
+
+	/* Update uCode's rate table. */
+	rate = iwl4965_rate_n_flags_from_tbl(priv, tbl, index, is_green);
+	iwl4965_rs_fill_link_cmd(priv, lq_sta, rate);
+	iwl_legacy_send_lq_cmd(priv, ctx, &lq_sta->lq, CMD_ASYNC, false);
+
+	return rate;
+}
+
+/*
+ * Do rate scaling and search for new modulation mode.
+ */
+static void iwl4965_rs_rate_scale_perform(struct iwl_priv *priv,
+				  struct sk_buff *skb,
+				  struct ieee80211_sta *sta,
+				  struct iwl_lq_sta *lq_sta)
+{
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_conf *conf = &hw->conf;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	int low = IWL_RATE_INVALID;
+	int high = IWL_RATE_INVALID;
+	int index;
+	int i;
+	struct iwl_rate_scale_data *window = NULL;
+	int current_tpt = IWL_INVALID_VALUE;
+	int low_tpt = IWL_INVALID_VALUE;
+	int high_tpt = IWL_INVALID_VALUE;
+	u32 fail_count;
+	s8 scale_action = 0;
+	u16 rate_mask;
+	u8 update_lq = 0;
+	struct iwl_scale_tbl_info *tbl, *tbl1;
+	u16 rate_scale_index_msk = 0;
+	u32 rate;
+	u8 is_green = 0;
+	u8 active_tbl = 0;
+	u8 done_search = 0;
+	u16 high_low;
+	s32 sr;
+	u8 tid = MAX_TID_COUNT;
+	struct iwl_tid_data *tid_data;
+	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
+	struct iwl_rxon_context *ctx = sta_priv->common.ctx;
+
+	IWL_DEBUG_RATE(priv, "rate scale calculate new rate for skb\n");
+
+	/* Send management frames and NO_ACK data using lowest rate. */
+	/* TODO: this could probably be improved.. */
+	if (!ieee80211_is_data(hdr->frame_control) ||
+	    info->flags & IEEE80211_TX_CTL_NO_ACK)
+		return;
+
+	if (!sta || !lq_sta)
+		return;
+
+	lq_sta->supp_rates = sta->supp_rates[lq_sta->band];
+
+	tid = iwl4965_rs_tl_add_packet(lq_sta, hdr);
+	if ((tid != MAX_TID_COUNT) && (lq_sta->tx_agg_tid_en & (1 << tid))) {
+		tid_data = &priv->stations[lq_sta->lq.sta_id].tid[tid];
+		if (tid_data->agg.state == IWL_AGG_OFF)
+			lq_sta->is_agg = 0;
+		else
+			lq_sta->is_agg = 1;
+	} else
+		lq_sta->is_agg = 0;
+
+	/*
+	 * Select rate-scale / modulation-mode table to work with in
+	 * the rest of this function:  "search" if searching for better
+	 * modulation mode, or "active" if doing rate scaling within a mode.
+	 */
+	if (!lq_sta->search_better_tbl)
+		active_tbl = lq_sta->active_tbl;
+	else
+		active_tbl = 1 - lq_sta->active_tbl;
+
+	tbl = &(lq_sta->lq_info[active_tbl]);
+	if (is_legacy(tbl->lq_type))
+		lq_sta->is_green = 0;
+	else
+		lq_sta->is_green = iwl4965_rs_use_green(sta);
+	is_green = lq_sta->is_green;
+
+	/* current tx rate */
+	index = lq_sta->last_txrate_idx;
+
+	IWL_DEBUG_RATE(priv, "Rate scale index %d for type %d\n", index,
+		       tbl->lq_type);
+
+	/* rates available for this association, and for modulation mode */
+	rate_mask = iwl4965_rs_get_supported_rates(lq_sta, hdr, tbl->lq_type);
+
+	IWL_DEBUG_RATE(priv, "mask 0x%04X\n", rate_mask);
+
+	/* mask with station rate restriction */
+	if (is_legacy(tbl->lq_type)) {
+		if (lq_sta->band == IEEE80211_BAND_5GHZ)
+			/* supp_rates has no CCK bits in A mode */
+			rate_scale_index_msk = (u16) (rate_mask &
+				(lq_sta->supp_rates << IWL_FIRST_OFDM_RATE));
+		else
+			rate_scale_index_msk = (u16) (rate_mask &
+						      lq_sta->supp_rates);
+
+	} else
+		rate_scale_index_msk = rate_mask;
+
+	if (!rate_scale_index_msk)
+		rate_scale_index_msk = rate_mask;
+
+	if (!((1 << index) & rate_scale_index_msk)) {
+		IWL_ERR(priv, "Current Rate is not valid\n");
+		if (lq_sta->search_better_tbl) {
+			/* revert to active table if search table is not valid*/
+			tbl->lq_type = LQ_NONE;
+			lq_sta->search_better_tbl = 0;
+			tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+			/* get "active" rate info */
+			index = iwl4965_hwrate_to_plcp_idx(tbl->current_rate);
+			rate = iwl4965_rs_update_rate_tbl(priv, ctx, lq_sta,
+						  tbl, index, is_green);
+		}
+		return;
+	}
+
+	/* Get expected throughput table and history window for current rate */
+	if (!tbl->expected_tpt) {
+		IWL_ERR(priv, "tbl->expected_tpt is NULL\n");
+		return;
+	}
+
+	/* force user max rate if set by user */
+	if ((lq_sta->max_rate_idx != -1) &&
+	    (lq_sta->max_rate_idx < index)) {
+		index = lq_sta->max_rate_idx;
+		update_lq = 1;
+		window = &(tbl->win[index]);
+		goto lq_update;
+	}
+
+	window = &(tbl->win[index]);
+
+	/*
+	 * If there is not enough history to calculate actual average
+	 * throughput, keep analyzing results of more tx frames, without
+	 * changing rate or mode (bypass most of the rest of this function).
+	 * Set up new rate table in uCode only if old rate is not supported
+	 * in current association (use new rate found above).
+	 */
+	fail_count = window->counter - window->success_counter;
+	if ((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
+			(window->success_counter < IWL_RATE_MIN_SUCCESS_TH)) {
+		IWL_DEBUG_RATE(priv, "LQ: still below TH. succ=%d total=%d "
+			       "for index %d\n",
+			       window->success_counter, window->counter, index);
+
+		/* Can't calculate this yet; not enough history */
+		window->average_tpt = IWL_INVALID_VALUE;
+
+		/* Should we stay with this modulation mode,
+		 * or search for a new one? */
+		iwl4965_rs_stay_in_table(lq_sta, false);
+
+		goto out;
+	}
+	/* Else we have enough samples; calculate estimate of
+	 * actual average throughput */
+	if (window->average_tpt != ((window->success_ratio *
+			tbl->expected_tpt[index] + 64) / 128)) {
+		IWL_ERR(priv,
+			 "expected_tpt should have been calculated by now\n");
+		window->average_tpt = ((window->success_ratio *
+					tbl->expected_tpt[index] + 64) / 128);
+	}
+
+	/* If we are searching for better modulation mode, check success. */
+	if (lq_sta->search_better_tbl) {
+		/* If good success, continue using the "search" mode;
+		 * no need to send new link quality command, since we're
+		 * continuing to use the setup that we've been trying. */
+		if (window->average_tpt > lq_sta->last_tpt) {
+
+			IWL_DEBUG_RATE(priv, "LQ: SWITCHING TO NEW TABLE "
+					"suc=%d cur-tpt=%d old-tpt=%d\n",
+					window->success_ratio,
+					window->average_tpt,
+					lq_sta->last_tpt);
+
+			if (!is_legacy(tbl->lq_type))
+				lq_sta->enable_counter = 1;
+
+			/* Swap tables; "search" becomes "active" */
+			lq_sta->active_tbl = active_tbl;
+			current_tpt = window->average_tpt;
+
+		/* Else poor success; go back to mode in "active" table */
+		} else {
+
+			IWL_DEBUG_RATE(priv, "LQ: GOING BACK TO THE OLD TABLE "
+					"suc=%d cur-tpt=%d old-tpt=%d\n",
+					window->success_ratio,
+					window->average_tpt,
+					lq_sta->last_tpt);
+
+			/* Nullify "search" table */
+			tbl->lq_type = LQ_NONE;
+
+			/* Revert to "active" table */
+			active_tbl = lq_sta->active_tbl;
+			tbl = &(lq_sta->lq_info[active_tbl]);
+
+			/* Revert to "active" rate and throughput info */
+			index = iwl4965_hwrate_to_plcp_idx(tbl->current_rate);
+			current_tpt = lq_sta->last_tpt;
+
+			/* Need to set up a new rate table in uCode */
+			update_lq = 1;
+		}
+
+		/* Either way, we've made a decision; modulation mode
+		 * search is done, allow rate adjustment next time. */
+		lq_sta->search_better_tbl = 0;
+		done_search = 1;	/* Don't switch modes below! */
+		goto lq_update;
+	}
+
+	/* (Else) not in search of better modulation mode, try for better
+	 * starting rate, while staying in this mode. */
+	high_low = iwl4965_rs_get_adjacent_rate(priv, index,
+					rate_scale_index_msk,
+					tbl->lq_type);
+	low = high_low & 0xff;
+	high = (high_low >> 8) & 0xff;
+
+	/* If user set max rate, dont allow higher than user constrain */
+	if ((lq_sta->max_rate_idx != -1) &&
+	    (lq_sta->max_rate_idx < high))
+		high = IWL_RATE_INVALID;
+
+	sr = window->success_ratio;
+
+	/* Collect measured throughputs for current and adjacent rates */
+	current_tpt = window->average_tpt;
+	if (low != IWL_RATE_INVALID)
+		low_tpt = tbl->win[low].average_tpt;
+	if (high != IWL_RATE_INVALID)
+		high_tpt = tbl->win[high].average_tpt;
+
+	scale_action = 0;
+
+	/* Too many failures, decrease rate */
+	if ((sr <= IWL_RATE_DECREASE_TH) || (current_tpt == 0)) {
+		IWL_DEBUG_RATE(priv,
+			"decrease rate because of low success_ratio\n");
+		scale_action = -1;
+
+	/* No throughput measured yet for adjacent rates; try increase. */
+	} else if ((low_tpt == IWL_INVALID_VALUE) &&
+		   (high_tpt == IWL_INVALID_VALUE)) {
+
+		if (high != IWL_RATE_INVALID && sr >= IWL_RATE_INCREASE_TH)
+			scale_action = 1;
+		else if (low != IWL_RATE_INVALID)
+			scale_action = 0;
+	}
+
+	/* Both adjacent throughputs are measured, but neither one has better
+	 * throughput; we're using the best rate, don't change it! */
+	else if ((low_tpt != IWL_INVALID_VALUE) &&
+		 (high_tpt != IWL_INVALID_VALUE) &&
+		 (low_tpt < current_tpt) &&
+		 (high_tpt < current_tpt))
+		scale_action = 0;
+
+	/* At least one adjacent rate's throughput is measured,
+	 * and may have better performance. */
+	else {
+		/* Higher adjacent rate's throughput is measured */
+		if (high_tpt != IWL_INVALID_VALUE) {
+			/* Higher rate has better throughput */
+			if (high_tpt > current_tpt &&
+					sr >= IWL_RATE_INCREASE_TH) {
+				scale_action = 1;
+			} else {
+				scale_action = 0;
+			}
+
+		/* Lower adjacent rate's throughput is measured */
+		} else if (low_tpt != IWL_INVALID_VALUE) {
+			/* Lower rate has better throughput */
+			if (low_tpt > current_tpt) {
+				IWL_DEBUG_RATE(priv,
+				    "decrease rate because of low tpt\n");
+				scale_action = -1;
+			} else if (sr >= IWL_RATE_INCREASE_TH) {
+				scale_action = 1;
+			}
+		}
+	}
+
+	/* Sanity check; asked for decrease, but success rate or throughput
+	 * has been good at old rate.  Don't change it. */
+	if ((scale_action == -1) && (low != IWL_RATE_INVALID) &&
+		    ((sr > IWL_RATE_HIGH_TH) ||
+		     (current_tpt > (100 * tbl->expected_tpt[low]))))
+		scale_action = 0;
+
+	switch (scale_action) {
+	case -1:
+		/* Decrease starting rate, update uCode's rate table */
+		if (low != IWL_RATE_INVALID) {
+			update_lq = 1;
+			index = low;
+		}
+
+		break;
+	case 1:
+		/* Increase starting rate, update uCode's rate table */
+		if (high != IWL_RATE_INVALID) {
+			update_lq = 1;
+			index = high;
+		}
+
+		break;
+	case 0:
+		/* No change */
+	default:
+		break;
+	}
+
+	IWL_DEBUG_RATE(priv, "choose rate scale index %d action %d low %d "
+		    "high %d type %d\n",
+		     index, scale_action, low, high, tbl->lq_type);
+
+lq_update:
+	/* Replace uCode's rate table for the destination station. */
+	if (update_lq)
+		rate = iwl4965_rs_update_rate_tbl(priv, ctx, lq_sta,
+					  tbl, index, is_green);
+
+	/* Should we stay with this modulation mode,
+	 * or search for a new one? */
+	 iwl4965_rs_stay_in_table(lq_sta, false);
+
+	/*
+	 * Search for new modulation mode if we're:
+	 * 1)  Not changing rates right now
+	 * 2)  Not just finishing up a search
+	 * 3)  Allowing a new search
+	 */
+	if (!update_lq && !done_search &&
+		!lq_sta->stay_in_tbl && window->counter) {
+		/* Save current throughput to compare with "search" throughput*/
+		lq_sta->last_tpt = current_tpt;
+
+		/* Select a new "search" modulation mode to try.
+		 * If one is found, set up the new "search" table. */
+		if (is_legacy(tbl->lq_type))
+			iwl4965_rs_move_legacy_other(priv, lq_sta,
+							conf, sta, index);
+		else if (is_siso(tbl->lq_type))
+			iwl4965_rs_move_siso_to_other(priv, lq_sta,
+							conf, sta, index);
+		else /* (is_mimo2(tbl->lq_type)) */
+			iwl4965_rs_move_mimo2_to_other(priv, lq_sta,
+							conf, sta, index);
+
+		/* If new "search" mode was selected, set up in uCode table */
+		if (lq_sta->search_better_tbl) {
+			/* Access the "search" table, clear its history. */
+			tbl = &(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
+			for (i = 0; i < IWL_RATE_COUNT; i++)
+				iwl4965_rs_rate_scale_clear_window(
+							&(tbl->win[i]));
+
+			/* Use new "search" start rate */
+			index = iwl4965_hwrate_to_plcp_idx(tbl->current_rate);
+
+			IWL_DEBUG_RATE(priv,
+				"Switch current  mcs: %X index: %d\n",
+				     tbl->current_rate, index);
+			iwl4965_rs_fill_link_cmd(priv, lq_sta,
+						tbl->current_rate);
+			iwl_legacy_send_lq_cmd(priv, ctx,
+						&lq_sta->lq, CMD_ASYNC, false);
+		} else
+			done_search = 1;
+	}
+
+	if (done_search && !lq_sta->stay_in_tbl) {
+		/* If the "active" (non-search) mode was legacy,
+		 * and we've tried switching antennas,
+		 * but we haven't been able to try HT modes (not available),
+		 * stay with best antenna legacy modulation for a while
+		 * before next round of mode comparisons. */
+		tbl1 = &(lq_sta->lq_info[lq_sta->active_tbl]);
+		if (is_legacy(tbl1->lq_type) && !conf_is_ht(conf) &&
+		    lq_sta->action_counter > tbl1->max_search) {
+			IWL_DEBUG_RATE(priv, "LQ: STAY in legacy table\n");
+			iwl4965_rs_set_stay_in_table(priv, 1, lq_sta);
+		}
+
+		/* If we're in an HT mode, and all 3 mode switch actions
+		 * have been tried and compared, stay in this best modulation
+		 * mode for a while before next round of mode comparisons. */
+		if (lq_sta->enable_counter &&
+		    (lq_sta->action_counter >= tbl1->max_search)) {
+			if ((lq_sta->last_tpt > IWL_AGG_TPT_THREHOLD) &&
+			    (lq_sta->tx_agg_tid_en & (1 << tid)) &&
+			    (tid != MAX_TID_COUNT)) {
+				tid_data =
+				   &priv->stations[lq_sta->lq.sta_id].tid[tid];
+				if (tid_data->agg.state == IWL_AGG_OFF) {
+					IWL_DEBUG_RATE(priv,
+						       "try to aggregate tid %d\n",
+						       tid);
+					iwl4965_rs_tl_turn_on_agg(priv, tid,
+							  lq_sta, sta);
+				}
+			}
+			iwl4965_rs_set_stay_in_table(priv, 0, lq_sta);
+		}
+	}
+
+out:
+	tbl->current_rate = iwl4965_rate_n_flags_from_tbl(priv, tbl,
+							index, is_green);
+	i = index;
+	lq_sta->last_txrate_idx = i;
+}
+
+/**
+ * iwl4965_rs_initialize_lq - Initialize a station's hardware rate table
+ *
+ * The uCode's station table contains a table of fallback rates
+ * for automatic fallback during transmission.
+ *
+ * NOTE: This sets up a default set of values.  These will be replaced later
+ *       if the driver's iwl-4965-rs rate scaling algorithm is used, instead of
+ *       rc80211_simple.
+ *
+ * NOTE: Run REPLY_ADD_STA command to set up station table entry, before
+ *       calling this function (which runs REPLY_TX_LINK_QUALITY_CMD,
+ *       which requires station table entry to exist).
+ */
+static void iwl4965_rs_initialize_lq(struct iwl_priv *priv,
+			     struct ieee80211_conf *conf,
+			     struct ieee80211_sta *sta,
+			     struct iwl_lq_sta *lq_sta)
+{
+	struct iwl_scale_tbl_info *tbl;
+	int rate_idx;
+	int i;
+	u32 rate;
+	u8 use_green = iwl4965_rs_use_green(sta);
+	u8 active_tbl = 0;
+	u8 valid_tx_ant;
+	struct iwl_station_priv *sta_priv;
+	struct iwl_rxon_context *ctx;
+
+	if (!sta || !lq_sta)
+		return;
+
+	sta_priv = (void *)sta->drv_priv;
+	ctx = sta_priv->common.ctx;
+
+	i = lq_sta->last_txrate_idx;
+
+	valid_tx_ant = priv->hw_params.valid_tx_ant;
+
+	if (!lq_sta->search_better_tbl)
+		active_tbl = lq_sta->active_tbl;
+	else
+		active_tbl = 1 - lq_sta->active_tbl;
+
+	tbl = &(lq_sta->lq_info[active_tbl]);
+
+	if ((i < 0) || (i >= IWL_RATE_COUNT))
+		i = 0;
+
+	rate = iwlegacy_rates[i].plcp;
+	tbl->ant_type = iwl4965_first_antenna(valid_tx_ant);
+	rate |= tbl->ant_type << RATE_MCS_ANT_POS;
+
+	if (i >= IWL_FIRST_CCK_RATE && i <= IWL_LAST_CCK_RATE)
+		rate |= RATE_MCS_CCK_MSK;
+
+	iwl4965_rs_get_tbl_info_from_mcs(rate, priv->band, tbl, &rate_idx);
+	if (!iwl4965_rs_is_valid_ant(valid_tx_ant, tbl->ant_type))
+		iwl4965_rs_toggle_antenna(valid_tx_ant, &rate, tbl);
+
+	rate = iwl4965_rate_n_flags_from_tbl(priv, tbl, rate_idx, use_green);
+	tbl->current_rate = rate;
+	iwl4965_rs_set_expected_tpt_table(lq_sta, tbl);
+	iwl4965_rs_fill_link_cmd(NULL, lq_sta, rate);
+	priv->stations[lq_sta->lq.sta_id].lq = &lq_sta->lq;
+	iwl_legacy_send_lq_cmd(priv, ctx, &lq_sta->lq, CMD_SYNC, true);
+}
+
+static void
+iwl4965_rs_get_rate(void *priv_r, struct ieee80211_sta *sta, void *priv_sta,
+			struct ieee80211_tx_rate_control *txrc)
+{
+
+	struct sk_buff *skb = txrc->skb;
+	struct ieee80211_supported_band *sband = txrc->sband;
+	struct iwl_priv *priv __maybe_unused = (struct iwl_priv *)priv_r;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct iwl_lq_sta *lq_sta = priv_sta;
+	int rate_idx;
+
+	IWL_DEBUG_RATE_LIMIT(priv, "rate scale calculate new rate for skb\n");
+
+	/* Get max rate if user set max rate */
+	if (lq_sta) {
+		lq_sta->max_rate_idx = txrc->max_rate_idx;
+		if ((sband->band == IEEE80211_BAND_5GHZ) &&
+		    (lq_sta->max_rate_idx != -1))
+			lq_sta->max_rate_idx += IWL_FIRST_OFDM_RATE;
+		if ((lq_sta->max_rate_idx < 0) ||
+		    (lq_sta->max_rate_idx >= IWL_RATE_COUNT))
+			lq_sta->max_rate_idx = -1;
+	}
+
+	/* Treat uninitialized rate scaling data same as non-existing. */
+	if (lq_sta && !lq_sta->drv) {
+		IWL_DEBUG_RATE(priv, "Rate scaling not initialized yet.\n");
+		priv_sta = NULL;
+	}
+
+	/* Send management frames and NO_ACK data using lowest rate. */
+	if (rate_control_send_low(sta, priv_sta, txrc))
+		return;
+
+	rate_idx  = lq_sta->last_txrate_idx;
+
+	if (lq_sta->last_rate_n_flags & RATE_MCS_HT_MSK) {
+		rate_idx -= IWL_FIRST_OFDM_RATE;
+		/* 6M and 9M shared same MCS index */
+		rate_idx = (rate_idx > 0) ? (rate_idx - 1) : 0;
+		if (iwl4965_rs_extract_rate(lq_sta->last_rate_n_flags) >=
+			 IWL_RATE_MIMO2_6M_PLCP)
+			rate_idx = rate_idx + MCS_INDEX_PER_STREAM;
+		info->control.rates[0].flags = IEEE80211_TX_RC_MCS;
+		if (lq_sta->last_rate_n_flags & RATE_MCS_SGI_MSK)
+			info->control.rates[0].flags |=
+					IEEE80211_TX_RC_SHORT_GI;
+		if (lq_sta->last_rate_n_flags & RATE_MCS_DUP_MSK)
+			info->control.rates[0].flags |=
+					IEEE80211_TX_RC_DUP_DATA;
+		if (lq_sta->last_rate_n_flags & RATE_MCS_HT40_MSK)
+			info->control.rates[0].flags |=
+					IEEE80211_TX_RC_40_MHZ_WIDTH;
+		if (lq_sta->last_rate_n_flags & RATE_MCS_GF_MSK)
+			info->control.rates[0].flags |=
+					IEEE80211_TX_RC_GREEN_FIELD;
+	} else {
+		/* Check for invalid rates */
+		if ((rate_idx < 0) || (rate_idx >= IWL_RATE_COUNT_LEGACY) ||
+				((sband->band == IEEE80211_BAND_5GHZ) &&
+				 (rate_idx < IWL_FIRST_OFDM_RATE)))
+			rate_idx = rate_lowest_index(sband, sta);
+		/* On valid 5 GHz rate, adjust index */
+		else if (sband->band == IEEE80211_BAND_5GHZ)
+			rate_idx -= IWL_FIRST_OFDM_RATE;
+		info->control.rates[0].flags = 0;
+	}
+	info->control.rates[0].idx = rate_idx;
+
+}
+
+static void *iwl4965_rs_alloc_sta(void *priv_rate, struct ieee80211_sta *sta,
+			  gfp_t gfp)
+{
+	struct iwl_lq_sta *lq_sta;
+	struct iwl_station_priv *sta_priv =
+				(struct iwl_station_priv *) sta->drv_priv;
+	struct iwl_priv *priv;
+
+	priv = (struct iwl_priv *)priv_rate;
+	IWL_DEBUG_RATE(priv, "create station rate scale window\n");
+
+	lq_sta = &sta_priv->lq_sta;
+
+	return lq_sta;
+}
+
+/*
+ * Called after adding a new station to initialize rate scaling
+ */
+void
+iwl4965_rs_rate_init(struct iwl_priv *priv,
+			struct ieee80211_sta *sta,
+			u8 sta_id)
+{
+	int i, j;
+	struct ieee80211_hw *hw = priv->hw;
+	struct ieee80211_conf *conf = &priv->hw->conf;
+	struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
+	struct iwl_station_priv *sta_priv;
+	struct iwl_lq_sta *lq_sta;
+	struct ieee80211_supported_band *sband;
+
+	sta_priv = (struct iwl_station_priv *) sta->drv_priv;
+	lq_sta = &sta_priv->lq_sta;
+	sband = hw->wiphy->bands[conf->channel->band];
+
+
+	lq_sta->lq.sta_id = sta_id;
+
+	for (j = 0; j < LQ_SIZE; j++)
+		for (i = 0; i < IWL_RATE_COUNT; i++)
+			iwl4965_rs_rate_scale_clear_window(
+					&lq_sta->lq_info[j].win[i]);
+
+	lq_sta->flush_timer = 0;
+	lq_sta->supp_rates = sta->supp_rates[sband->band];
+	for (j = 0; j < LQ_SIZE; j++)
+		for (i = 0; i < IWL_RATE_COUNT; i++)
+			iwl4965_rs_rate_scale_clear_window(
+					&lq_sta->lq_info[j].win[i]);
+
+	IWL_DEBUG_RATE(priv, "LQ:"
+			"*** rate scale station global init for station %d ***\n",
+		       sta_id);
+	/* TODO: what is a good starting rate for STA? About middle? Maybe not
+	 * the lowest or the highest rate.. Could consider using RSSI from
+	 * previous packets? Need to have IEEE 802.1X auth succeed immediately
+	 * after assoc.. */
+
+	lq_sta->is_dup = 0;
+	lq_sta->max_rate_idx = -1;
+	lq_sta->missed_rate_counter = IWL_MISSED_RATE_MAX;
+	lq_sta->is_green = iwl4965_rs_use_green(sta);
+	lq_sta->active_legacy_rate = priv->active_rate & ~(0x1000);
+	lq_sta->band = priv->band;
+	/*
+	 * active_siso_rate mask includes 9 MBits (bit 5), and CCK (bits 0-3),
+	 * supp_rates[] does not; shift to convert format, force 9 MBits off.
+	 */
+	lq_sta->active_siso_rate = ht_cap->mcs.rx_mask[0] << 1;
+	lq_sta->active_siso_rate |= ht_cap->mcs.rx_mask[0] & 0x1;
+	lq_sta->active_siso_rate &= ~((u16)0x2);
+	lq_sta->active_siso_rate <<= IWL_FIRST_OFDM_RATE;
+
+	/* Same here */
+	lq_sta->active_mimo2_rate = ht_cap->mcs.rx_mask[1] << 1;
+	lq_sta->active_mimo2_rate |= ht_cap->mcs.rx_mask[1] & 0x1;
+	lq_sta->active_mimo2_rate &= ~((u16)0x2);
+	lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
+
+	/* These values will be overridden later */
+	lq_sta->lq.general_params.single_stream_ant_msk =
+		iwl4965_first_antenna(priv->hw_params.valid_tx_ant);
+	lq_sta->lq.general_params.dual_stream_ant_msk =
+		priv->hw_params.valid_tx_ant &
+		~iwl4965_first_antenna(priv->hw_params.valid_tx_ant);
+	if (!lq_sta->lq.general_params.dual_stream_ant_msk) {
+		lq_sta->lq.general_params.dual_stream_ant_msk = ANT_AB;
+	} else if (iwl4965_num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+		lq_sta->lq.general_params.dual_stream_ant_msk =
+			priv->hw_params.valid_tx_ant;
+	}
+
+	/* as default allow aggregation for all tids */
+	lq_sta->tx_agg_tid_en = IWL_AGG_ALL_TID;
+	lq_sta->drv = priv;
+
+	/* Set last_txrate_idx to lowest rate */
+	lq_sta->last_txrate_idx = rate_lowest_index(sband, sta);
+	if (sband->band == IEEE80211_BAND_5GHZ)
+		lq_sta->last_txrate_idx += IWL_FIRST_OFDM_RATE;
+	lq_sta->is_agg = 0;
+
+#ifdef CONFIG_MAC80211_DEBUGFS
+	lq_sta->dbg_fixed_rate = 0;
+#endif
+
+	iwl4965_rs_initialize_lq(priv, conf, sta, lq_sta);
+}
+
+static void iwl4965_rs_fill_link_cmd(struct iwl_priv *priv,
+			     struct iwl_lq_sta *lq_sta, u32 new_rate)
+{
+	struct iwl_scale_tbl_info tbl_type;
+	int index = 0;
+	int rate_idx;
+	int repeat_rate = 0;
+	u8 ant_toggle_cnt = 0;
+	u8 use_ht_possible = 1;
+	u8 valid_tx_ant = 0;
+	struct iwl_link_quality_cmd *lq_cmd = &lq_sta->lq;
+
+	/* Override starting rate (index 0) if needed for debug purposes */
+	iwl4965_rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
+
+	/* Interpret new_rate (rate_n_flags) */
+	iwl4965_rs_get_tbl_info_from_mcs(new_rate, lq_sta->band,
+				  &tbl_type, &rate_idx);
+
+	/* How many times should we repeat the initial rate? */
+	if (is_legacy(tbl_type.lq_type)) {
+		ant_toggle_cnt = 1;
+		repeat_rate = IWL_NUMBER_TRY;
+	} else {
+		repeat_rate = IWL_HT_NUMBER_TRY;
+	}
+
+	lq_cmd->general_params.mimo_delimiter =
+			is_mimo(tbl_type.lq_type) ? 1 : 0;
+
+	/* Fill 1st table entry (index 0) */
+	lq_cmd->rs_table[index].rate_n_flags = cpu_to_le32(new_rate);
+
+	if (iwl4965_num_of_ant(tbl_type.ant_type) == 1) {
+		lq_cmd->general_params.single_stream_ant_msk =
+						tbl_type.ant_type;
+	} else if (iwl4965_num_of_ant(tbl_type.ant_type) == 2) {
+		lq_cmd->general_params.dual_stream_ant_msk =
+						tbl_type.ant_type;
+	} /* otherwise we don't modify the existing value */
+
+	index++;
+	repeat_rate--;
+	if (priv)
+		valid_tx_ant = priv->hw_params.valid_tx_ant;
+
+	/* Fill rest of rate table */
+	while (index < LINK_QUAL_MAX_RETRY_NUM) {
+		/* Repeat initial/next rate.
+		 * For legacy IWL_NUMBER_TRY == 1, this loop will not execute.
+		 * For HT IWL_HT_NUMBER_TRY == 3, this executes twice. */
+		while (repeat_rate > 0 && (index < LINK_QUAL_MAX_RETRY_NUM)) {
+			if (is_legacy(tbl_type.lq_type)) {
+				if (ant_toggle_cnt < NUM_TRY_BEFORE_ANT_TOGGLE)
+					ant_toggle_cnt++;
+				else if (priv &&
+					 iwl4965_rs_toggle_antenna(valid_tx_ant,
+							&new_rate, &tbl_type))
+					ant_toggle_cnt = 1;
+			}
+
+			/* Override next rate if needed for debug purposes */
+			iwl4965_rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
+
+			/* Fill next table entry */
+			lq_cmd->rs_table[index].rate_n_flags =
+					cpu_to_le32(new_rate);
+			repeat_rate--;
+			index++;
+		}
+
+		iwl4965_rs_get_tbl_info_from_mcs(new_rate,
+						lq_sta->band, &tbl_type,
+						&rate_idx);
+
+		/* Indicate to uCode which entries might be MIMO.
+		 * If initial rate was MIMO, this will finally end up
+		 * as (IWL_HT_NUMBER_TRY * 2), after 2nd pass, otherwise 0. */
+		if (is_mimo(tbl_type.lq_type))
+			lq_cmd->general_params.mimo_delimiter = index;
+
+		/* Get next rate */
+		new_rate = iwl4965_rs_get_lower_rate(lq_sta,
+					&tbl_type, rate_idx,
+					     use_ht_possible);
+
+		/* How many times should we repeat the next rate? */
+		if (is_legacy(tbl_type.lq_type)) {
+			if (ant_toggle_cnt < NUM_TRY_BEFORE_ANT_TOGGLE)
+				ant_toggle_cnt++;
+			else if (priv &&
+				 iwl4965_rs_toggle_antenna(valid_tx_ant,
+						   &new_rate, &tbl_type))
+				ant_toggle_cnt = 1;
+
+			repeat_rate = IWL_NUMBER_TRY;
+		} else {
+			repeat_rate = IWL_HT_NUMBER_TRY;
+		}
+
+		/* Don't allow HT rates after next pass.
+		 * iwl4965_rs_get_lower_rate() will change type to LQ_A or LQ_G. */
+		use_ht_possible = 0;
+
+		/* Override next rate if needed for debug purposes */
+		iwl4965_rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
+
+		/* Fill next table entry */
+		lq_cmd->rs_table[index].rate_n_flags = cpu_to_le32(new_rate);
+
+		index++;
+		repeat_rate--;
+	}
+
+	lq_cmd->agg_params.agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+	lq_cmd->agg_params.agg_dis_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
+
+	lq_cmd->agg_params.agg_time_limit =
+		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
+}
+
+static void
+*iwl4965_rs_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
+{
+	return hw->priv;
+}
+/* rate scale requires free function to be implemented */
+static void iwl4965_rs_free(void *priv_rate)
+{
+	return;
+}
+
+static void iwl4965_rs_free_sta(void *priv_r, struct ieee80211_sta *sta,
+			void *priv_sta)
+{
+	struct iwl_priv *priv __maybe_unused = priv_r;
+
+	IWL_DEBUG_RATE(priv, "enter\n");
+	IWL_DEBUG_RATE(priv, "leave\n");
+}
+
+
+#ifdef CONFIG_MAC80211_DEBUGFS
+static int iwl4965_open_file_generic(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+static void iwl4965_rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
+			     u32 *rate_n_flags, int index)
+{
+	struct iwl_priv *priv;
+	u8 valid_tx_ant;
+	u8 ant_sel_tx;
+
+	priv = lq_sta->drv;
+	valid_tx_ant = priv->hw_params.valid_tx_ant;
+	if (lq_sta->dbg_fixed_rate) {
+		ant_sel_tx =
+		  ((lq_sta->dbg_fixed_rate & RATE_MCS_ANT_ABC_MSK)
+		  >> RATE_MCS_ANT_POS);
+		if ((valid_tx_ant & ant_sel_tx) == ant_sel_tx) {
+			*rate_n_flags = lq_sta->dbg_fixed_rate;
+			IWL_DEBUG_RATE(priv, "Fixed rate ON\n");
+		} else {
+			lq_sta->dbg_fixed_rate = 0;
+			IWL_ERR(priv,
+			    "Invalid antenna selection 0x%X, Valid is 0x%X\n",
+			    ant_sel_tx, valid_tx_ant);
+			IWL_DEBUG_RATE(priv, "Fixed rate OFF\n");
+		}
+	} else {
+		IWL_DEBUG_RATE(priv, "Fixed rate OFF\n");
+	}
+}
+
+static ssize_t iwl4965_rs_sta_dbgfs_scale_table_write(struct file *file,
+			const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct iwl_lq_sta *lq_sta = file->private_data;
+	struct iwl_priv *priv;
+	char buf[64];
+	int buf_size;
+	u32 parsed_rate;
+	struct iwl_station_priv *sta_priv =
+		container_of(lq_sta, struct iwl_station_priv, lq_sta);
+	struct iwl_rxon_context *ctx = sta_priv->common.ctx;
+
+	priv = lq_sta->drv;
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+
+	if (sscanf(buf, "%x", &parsed_rate) == 1)
+		lq_sta->dbg_fixed_rate = parsed_rate;
+	else
+		lq_sta->dbg_fixed_rate = 0;
+
+	lq_sta->active_legacy_rate = 0x0FFF;	/* 1 - 54 MBits, includes CCK */
+	lq_sta->active_siso_rate   = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
+	lq_sta->active_mimo2_rate  = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
+
+	IWL_DEBUG_RATE(priv, "sta_id %d rate 0x%X\n",
+		lq_sta->lq.sta_id, lq_sta->dbg_fixed_rate);
+
+	if (lq_sta->dbg_fixed_rate) {
+		iwl4965_rs_fill_link_cmd(NULL, lq_sta, lq_sta->dbg_fixed_rate);
+		iwl_legacy_send_lq_cmd(lq_sta->drv, ctx, &lq_sta->lq, CMD_ASYNC,
+				false);
+	}
+
+	return count;
+}
+
+static ssize_t iwl4965_rs_sta_dbgfs_scale_table_read(struct file *file,
+			char __user *user_buf, size_t count, loff_t *ppos)
+{
+	char *buff;
+	int desc = 0;
+	int i = 0;
+	int index = 0;
+	ssize_t ret;
+
+	struct iwl_lq_sta *lq_sta = file->private_data;
+	struct iwl_priv *priv;
+	struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
+
+	priv = lq_sta->drv;
+	buff = kmalloc(1024, GFP_KERNEL);
+	if (!buff)
+		return -ENOMEM;
+
+	desc += sprintf(buff+desc, "sta_id %d\n", lq_sta->lq.sta_id);
+	desc += sprintf(buff+desc, "failed=%d success=%d rate=0%X\n",
+			lq_sta->total_failed, lq_sta->total_success,
+			lq_sta->active_legacy_rate);
+	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
+			lq_sta->dbg_fixed_rate);
+	desc += sprintf(buff+desc, "valid_tx_ant %s%s%s\n",
+	    (priv->hw_params.valid_tx_ant & ANT_A) ? "ANT_A," : "",
+	    (priv->hw_params.valid_tx_ant & ANT_B) ? "ANT_B," : "",
+	    (priv->hw_params.valid_tx_ant & ANT_C) ? "ANT_C" : "");
+	desc += sprintf(buff+desc, "lq type %s\n",
+	   (is_legacy(tbl->lq_type)) ? "legacy" : "HT");
+	if (is_Ht(tbl->lq_type)) {
+		desc += sprintf(buff+desc, " %s",
+		   (is_siso(tbl->lq_type)) ? "SISO" : "MIMO2");
+		   desc += sprintf(buff+desc, " %s",
+		   (tbl->is_ht40) ? "40MHz" : "20MHz");
+		   desc += sprintf(buff+desc, " %s %s %s\n",
+			(tbl->is_SGI) ? "SGI" : "",
+		   (lq_sta->is_green) ? "GF enabled" : "",
+		   (lq_sta->is_agg) ? "AGG on" : "");
+	}
+	desc += sprintf(buff+desc, "last tx rate=0x%X\n",
+		lq_sta->last_rate_n_flags);
+	desc += sprintf(buff+desc, "general:"
+		"flags=0x%X mimo-d=%d s-ant0x%x d-ant=0x%x\n",
+		lq_sta->lq.general_params.flags,
+		lq_sta->lq.general_params.mimo_delimiter,
+		lq_sta->lq.general_params.single_stream_ant_msk,
+		lq_sta->lq.general_params.dual_stream_ant_msk);
+
+	desc += sprintf(buff+desc, "agg:"
+			"time_limit=%d dist_start_th=%d frame_cnt_limit=%d\n",
+			le16_to_cpu(lq_sta->lq.agg_params.agg_time_limit),
+			lq_sta->lq.agg_params.agg_dis_start_th,
+			lq_sta->lq.agg_params.agg_frame_cnt_limit);
+
+	desc += sprintf(buff+desc,
+			"Start idx [0]=0x%x [1]=0x%x [2]=0x%x [3]=0x%x\n",
+			lq_sta->lq.general_params.start_rate_index[0],
+			lq_sta->lq.general_params.start_rate_index[1],
+			lq_sta->lq.general_params.start_rate_index[2],
+			lq_sta->lq.general_params.start_rate_index[3]);
+
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++) {
+		index = iwl4965_hwrate_to_plcp_idx(
+			le32_to_cpu(lq_sta->lq.rs_table[i].rate_n_flags));
+		if (is_legacy(tbl->lq_type)) {
+			desc += sprintf(buff+desc, " rate[%d] 0x%X %smbps\n",
+			i,
+			le32_to_cpu(lq_sta->lq.rs_table[i].rate_n_flags),
+			iwl_rate_mcs[index].mbps);
+		} else {
+			desc += sprintf(buff+desc,
+			" rate[%d] 0x%X %smbps (%s)\n",
+			i,
+			le32_to_cpu(lq_sta->lq.rs_table[i].rate_n_flags),
+			iwl_rate_mcs[index].mbps, iwl_rate_mcs[index].mcs);
+		}
+	}
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buff, desc);
+	kfree(buff);
+	return ret;
+}
+
+static const struct file_operations rs_sta_dbgfs_scale_table_ops = {
+	.write = iwl4965_rs_sta_dbgfs_scale_table_write,
+	.read = iwl4965_rs_sta_dbgfs_scale_table_read,
+	.open = iwl4965_open_file_generic,
+	.llseek = default_llseek,
+};
+static ssize_t iwl4965_rs_sta_dbgfs_stats_table_read(struct file *file,
+			char __user *user_buf, size_t count, loff_t *ppos)
+{
+	char *buff;
+	int desc = 0;
+	int i, j;
+	ssize_t ret;
+
+	struct iwl_lq_sta *lq_sta = file->private_data;
+
+	buff = kmalloc(1024, GFP_KERNEL);
+	if (!buff)
+		return -ENOMEM;
+
+	for (i = 0; i < LQ_SIZE; i++) {
+		desc += sprintf(buff+desc,
+				"%s type=%d SGI=%d HT40=%d DUP=%d GF=%d\n"
+				"rate=0x%X\n",
+				lq_sta->active_tbl == i ? "*" : "x",
+				lq_sta->lq_info[i].lq_type,
+				lq_sta->lq_info[i].is_SGI,
+				lq_sta->lq_info[i].is_ht40,
+				lq_sta->lq_info[i].is_dup,
+				lq_sta->is_green,
+				lq_sta->lq_info[i].current_rate);
+		for (j = 0; j < IWL_RATE_COUNT; j++) {
+			desc += sprintf(buff+desc,
+				"counter=%d success=%d %%=%d\n",
+				lq_sta->lq_info[i].win[j].counter,
+				lq_sta->lq_info[i].win[j].success_counter,
+				lq_sta->lq_info[i].win[j].success_ratio);
+		}
+	}
+	ret = simple_read_from_buffer(user_buf, count, ppos, buff, desc);
+	kfree(buff);
+	return ret;
+}
+
+static const struct file_operations rs_sta_dbgfs_stats_table_ops = {
+	.read = iwl4965_rs_sta_dbgfs_stats_table_read,
+	.open = iwl4965_open_file_generic,
+	.llseek = default_llseek,
+};
+
+static ssize_t iwl4965_rs_sta_dbgfs_rate_scale_data_read(struct file *file,
+			char __user *user_buf, size_t count, loff_t *ppos)
+{
+	char buff[120];
+	int desc = 0;
+	ssize_t ret;
+
+	struct iwl_lq_sta *lq_sta = file->private_data;
+	struct iwl_priv *priv;
+	struct iwl_scale_tbl_info *tbl = &lq_sta->lq_info[lq_sta->active_tbl];
+
+	priv = lq_sta->drv;
+
+	if (is_Ht(tbl->lq_type))
+		desc += sprintf(buff+desc,
+				"Bit Rate= %d Mb/s\n",
+				tbl->expected_tpt[lq_sta->last_txrate_idx]);
+	else
+		desc += sprintf(buff+desc,
+				"Bit Rate= %d Mb/s\n",
+				iwlegacy_rates[lq_sta->last_txrate_idx].ieee >> 1);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buff, desc);
+	return ret;
+}
+
+static const struct file_operations rs_sta_dbgfs_rate_scale_data_ops = {
+	.read = iwl4965_rs_sta_dbgfs_rate_scale_data_read,
+	.open = iwl4965_open_file_generic,
+	.llseek = default_llseek,
+};
+
+static void iwl4965_rs_add_debugfs(void *priv, void *priv_sta,
+					struct dentry *dir)
+{
+	struct iwl_lq_sta *lq_sta = priv_sta;
+	lq_sta->rs_sta_dbgfs_scale_table_file =
+		debugfs_create_file("rate_scale_table", S_IRUSR | S_IWUSR, dir,
+				lq_sta, &rs_sta_dbgfs_scale_table_ops);
+	lq_sta->rs_sta_dbgfs_stats_table_file =
+		debugfs_create_file("rate_stats_table", S_IRUSR, dir,
+			lq_sta, &rs_sta_dbgfs_stats_table_ops);
+	lq_sta->rs_sta_dbgfs_rate_scale_data_file =
+		debugfs_create_file("rate_scale_data", S_IRUSR, dir,
+			lq_sta, &rs_sta_dbgfs_rate_scale_data_ops);
+	lq_sta->rs_sta_dbgfs_tx_agg_tid_en_file =
+		debugfs_create_u8("tx_agg_tid_enable", S_IRUSR | S_IWUSR, dir,
+		&lq_sta->tx_agg_tid_en);
+
+}
+
+static void iwl4965_rs_remove_debugfs(void *priv, void *priv_sta)
+{
+	struct iwl_lq_sta *lq_sta = priv_sta;
+	debugfs_remove(lq_sta->rs_sta_dbgfs_scale_table_file);
+	debugfs_remove(lq_sta->rs_sta_dbgfs_stats_table_file);
+	debugfs_remove(lq_sta->rs_sta_dbgfs_rate_scale_data_file);
+	debugfs_remove(lq_sta->rs_sta_dbgfs_tx_agg_tid_en_file);
+}
+#endif
+
+/*
+ * Initialization of rate scaling information is done by driver after
+ * the station is added. Since mac80211 calls this function before a
+ * station is added we ignore it.
+ */
+static void
+iwl4965_rs_rate_init_stub(void *priv_r, struct ieee80211_supported_band *sband,
+			 struct ieee80211_sta *sta, void *priv_sta)
+{
+}
+static struct rate_control_ops rs_4965_ops = {
+	.module = NULL,
+	.name = IWL4965_RS_NAME,
+	.tx_status = iwl4965_rs_tx_status,
+	.get_rate = iwl4965_rs_get_rate,
+	.rate_init = iwl4965_rs_rate_init_stub,
+	.alloc = iwl4965_rs_alloc,
+	.free = iwl4965_rs_free,
+	.alloc_sta = iwl4965_rs_alloc_sta,
+	.free_sta = iwl4965_rs_free_sta,
+#ifdef CONFIG_MAC80211_DEBUGFS
+	.add_sta_debugfs = iwl4965_rs_add_debugfs,
+	.remove_sta_debugfs = iwl4965_rs_remove_debugfs,
+#endif
+};
+
+int iwl4965_rate_control_register(void)
+{
+	pr_err("Registering 4965 rate control operations\n");
+	return ieee80211_rate_control_register(&rs_4965_ops);
+}
+
+void iwl4965_rate_control_unregister(void)
+{
+	ieee80211_rate_control_unregister(&rs_4965_ops);
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c b/drivers/net/wireless/iwlegacy/iwl-4965-rx.c
index bbd40b7dd597..b9fa2f6411a7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-rx.c
@@ -2,7 +2,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2008 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2008 - 2011 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
@@ -34,14 +34,14 @@
 
 #include "iwl-dev.h"
 #include "iwl-core.h"
-#include "iwl-agn-calib.h"
+#include "iwl-4965-calib.h"
 #include "iwl-sta.h"
 #include "iwl-io.h"
 #include "iwl-helpers.h"
-#include "iwl-agn-hw.h"
-#include "iwl-agn.h"
+#include "iwl-4965-hw.h"
+#include "iwl-4965.h"
 
-void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
+void iwl4965_rx_missed_beacon_notif(struct iwl_priv *priv,
 				struct iwl_rx_mem_buffer *rxb)
 
 {
@@ -58,14 +58,14 @@ void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
 		    le32_to_cpu(missed_beacon->num_recvd_beacons),
 		    le32_to_cpu(missed_beacon->num_expected_beacons));
 		if (!test_bit(STATUS_SCANNING, &priv->status))
-			iwl_init_sensitivity(priv);
+			iwl4965_init_sensitivity(priv);
 	}
 }
 
 /* Calculate noise level, based on measurements during network silence just
  *   before arriving beacon.  This measurement can be done only if we know
  *   exactly when to expect beacons, therefore only when we're associated. */
-static void iwl_rx_calc_noise(struct iwl_priv *priv)
+static void iwl4965_rx_calc_noise(struct iwl_priv *priv)
 {
 	struct statistics_rx_non_phy *rx_info;
 	int num_active_rx = 0;
@@ -73,11 +73,7 @@ static void iwl_rx_calc_noise(struct iwl_priv *priv)
 	int bcn_silence_a, bcn_silence_b, bcn_silence_c;
 	int last_rx_noise;
 
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics)
-		rx_info = &(priv->_agn.statistics_bt.rx.general.common);
-	else
-		rx_info = &(priv->_agn.statistics.rx.general);
+	rx_info = &(priv->_4965.statistics.rx.general);
 	bcn_silence_a =
 		le32_to_cpu(rx_info->beacon_silence_rssi_a) & IN_BAND_FILTER;
 	bcn_silence_b =
@@ -109,13 +105,13 @@ static void iwl_rx_calc_noise(struct iwl_priv *priv)
 			last_rx_noise);
 }
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
 /*
  *  based on the assumption of all statistics counter are in DWORD
  *  FIXME: This function is for debugging, do not deal with
  *  the case of counters roll-over.
  */
-static void iwl_accumulative_statistics(struct iwl_priv *priv,
+static void iwl4965_accumulative_statistics(struct iwl_priv *priv,
 					__le32 *stats)
 {
 	int i, size;
@@ -125,28 +121,16 @@ static void iwl_accumulative_statistics(struct iwl_priv *priv,
 	struct statistics_general_common *general, *accum_general;
 	struct statistics_tx *tx, *accum_tx;
 
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
-		prev_stats = (__le32 *)&priv->_agn.statistics_bt;
-		accum_stats = (u32 *)&priv->_agn.accum_statistics_bt;
-		size = sizeof(struct iwl_bt_notif_statistics);
-		general = &priv->_agn.statistics_bt.general.common;
-		accum_general = &priv->_agn.accum_statistics_bt.general.common;
-		tx = &priv->_agn.statistics_bt.tx;
-		accum_tx = &priv->_agn.accum_statistics_bt.tx;
-		delta = (u32 *)&priv->_agn.delta_statistics_bt;
-		max_delta = (u32 *)&priv->_agn.max_delta_bt;
-	} else {
-		prev_stats = (__le32 *)&priv->_agn.statistics;
-		accum_stats = (u32 *)&priv->_agn.accum_statistics;
-		size = sizeof(struct iwl_notif_statistics);
-		general = &priv->_agn.statistics.general.common;
-		accum_general = &priv->_agn.accum_statistics.general.common;
-		tx = &priv->_agn.statistics.tx;
-		accum_tx = &priv->_agn.accum_statistics.tx;
-		delta = (u32 *)&priv->_agn.delta_statistics;
-		max_delta = (u32 *)&priv->_agn.max_delta;
-	}
+	prev_stats = (__le32 *)&priv->_4965.statistics;
+	accum_stats = (u32 *)&priv->_4965.accum_statistics;
+	size = sizeof(struct iwl_notif_statistics);
+	general = &priv->_4965.statistics.general.common;
+	accum_general = &priv->_4965.accum_statistics.general.common;
+	tx = &priv->_4965.statistics.tx;
+	accum_tx = &priv->_4965.accum_statistics.tx;
+	delta = (u32 *)&priv->_4965.delta_statistics;
+	max_delta = (u32 *)&priv->_4965.max_delta;
+
 	for (i = sizeof(__le32); i < size;
 	     i += sizeof(__le32), stats++, prev_stats++, delta++,
 	     max_delta++, accum_stats++) {
@@ -161,23 +145,19 @@ static void iwl_accumulative_statistics(struct iwl_priv *priv,
 
 	/* reset accumulative statistics for "no-counter" type statistics */
 	accum_general->temperature = general->temperature;
-	accum_general->temperature_m = general->temperature_m;
 	accum_general->ttl_timestamp = general->ttl_timestamp;
-	accum_tx->tx_power.ant_a = tx->tx_power.ant_a;
-	accum_tx->tx_power.ant_b = tx->tx_power.ant_b;
-	accum_tx->tx_power.ant_c = tx->tx_power.ant_c;
 }
 #endif
 
 #define REG_RECALIB_PERIOD (60)
 
 /**
- * iwl_good_plcp_health - checks for plcp error.
+ * iwl4965_good_plcp_health - checks for plcp error.
  *
  * When the plcp error is exceeding the thresholds, reset the radio
  * to improve the throughput.
  */
-bool iwl_good_plcp_health(struct iwl_priv *priv,
+bool iwl4965_good_plcp_health(struct iwl_priv *priv,
 				struct iwl_rx_packet *pkt)
 {
 	bool rc = true;
@@ -207,28 +187,15 @@ bool iwl_good_plcp_health(struct iwl_priv *priv,
 		struct statistics_rx_phy *ofdm;
 		struct statistics_rx_ht_phy *ofdm_ht;
 
-		if (priv->cfg->bt_params &&
-		    priv->cfg->bt_params->bt_statistics) {
-			ofdm = &pkt->u.stats_bt.rx.ofdm;
-			ofdm_ht = &pkt->u.stats_bt.rx.ofdm_ht;
-			combined_plcp_delta =
-			   (le32_to_cpu(ofdm->plcp_err) -
-			   le32_to_cpu(priv->_agn.statistics_bt.
-				       rx.ofdm.plcp_err)) +
-			   (le32_to_cpu(ofdm_ht->plcp_err) -
-			   le32_to_cpu(priv->_agn.statistics_bt.
-				       rx.ofdm_ht.plcp_err));
-		} else {
-			ofdm = &pkt->u.stats.rx.ofdm;
-			ofdm_ht = &pkt->u.stats.rx.ofdm_ht;
-			combined_plcp_delta =
-			    (le32_to_cpu(ofdm->plcp_err) -
-			    le32_to_cpu(priv->_agn.statistics.
-					rx.ofdm.plcp_err)) +
-			    (le32_to_cpu(ofdm_ht->plcp_err) -
-			    le32_to_cpu(priv->_agn.statistics.
-					rx.ofdm_ht.plcp_err));
-		}
+		ofdm = &pkt->u.stats.rx.ofdm;
+		ofdm_ht = &pkt->u.stats.rx.ofdm_ht;
+		combined_plcp_delta =
+		    (le32_to_cpu(ofdm->plcp_err) -
+		    le32_to_cpu(priv->_4965.statistics.
+				rx.ofdm.plcp_err)) +
+		    (le32_to_cpu(ofdm_ht->plcp_err) -
+		    le32_to_cpu(priv->_4965.statistics.
+				rx.ofdm_ht.plcp_err));
 
 		if ((combined_plcp_delta > 0) &&
 		    ((combined_plcp_delta * 100) / plcp_msec) >
@@ -259,58 +226,32 @@ bool iwl_good_plcp_health(struct iwl_priv *priv,
 	return rc;
 }
 
-void iwl_rx_statistics(struct iwl_priv *priv,
+void iwl4965_rx_statistics(struct iwl_priv *priv,
 			      struct iwl_rx_mem_buffer *rxb)
 {
 	int change;
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
-		IWL_DEBUG_RX(priv,
-			     "Statistics notification received (%d vs %d).\n",
-			     (int)sizeof(struct iwl_bt_notif_statistics),
-			     le32_to_cpu(pkt->len_n_flags) &
-			     FH_RSCSR_FRAME_SIZE_MSK);
-
-		change = ((priv->_agn.statistics_bt.general.common.temperature !=
-			   pkt->u.stats_bt.general.common.temperature) ||
-			   ((priv->_agn.statistics_bt.flag &
-			   STATISTICS_REPLY_FLG_HT40_MODE_MSK) !=
-			   (pkt->u.stats_bt.flag &
-			   STATISTICS_REPLY_FLG_HT40_MODE_MSK)));
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-		iwl_accumulative_statistics(priv, (__le32 *)&pkt->u.stats_bt);
+	IWL_DEBUG_RX(priv,
+		     "Statistics notification received (%d vs %d).\n",
+		     (int)sizeof(struct iwl_notif_statistics),
+		     le32_to_cpu(pkt->len_n_flags) &
+		     FH_RSCSR_FRAME_SIZE_MSK);
+
+	change = ((priv->_4965.statistics.general.common.temperature !=
+		   pkt->u.stats.general.common.temperature) ||
+		   ((priv->_4965.statistics.flag &
+		   STATISTICS_REPLY_FLG_HT40_MODE_MSK) !=
+		   (pkt->u.stats.flag &
+		   STATISTICS_REPLY_FLG_HT40_MODE_MSK)));
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+	iwl4965_accumulative_statistics(priv, (__le32 *)&pkt->u.stats);
 #endif
 
-	} else {
-		IWL_DEBUG_RX(priv,
-			     "Statistics notification received (%d vs %d).\n",
-			     (int)sizeof(struct iwl_notif_statistics),
-			     le32_to_cpu(pkt->len_n_flags) &
-			     FH_RSCSR_FRAME_SIZE_MSK);
+	iwl_legacy_recover_from_statistics(priv, pkt);
 
-		change = ((priv->_agn.statistics.general.common.temperature !=
-			   pkt->u.stats.general.common.temperature) ||
-			   ((priv->_agn.statistics.flag &
-			   STATISTICS_REPLY_FLG_HT40_MODE_MSK) !=
-			   (pkt->u.stats.flag &
-			   STATISTICS_REPLY_FLG_HT40_MODE_MSK)));
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-		iwl_accumulative_statistics(priv, (__le32 *)&pkt->u.stats);
-#endif
-
-	}
-
-	iwl_recover_from_statistics(priv, pkt);
-
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics)
-		memcpy(&priv->_agn.statistics_bt, &pkt->u.stats_bt,
-			sizeof(priv->_agn.statistics_bt));
-	else
-		memcpy(&priv->_agn.statistics, &pkt->u.stats,
-			sizeof(priv->_agn.statistics));
+	memcpy(&priv->_4965.statistics, &pkt->u.stats,
+		sizeof(priv->_4965.statistics));
 
 	set_bit(STATUS_STATISTICS, &priv->status);
 
@@ -323,34 +264,28 @@ void iwl_rx_statistics(struct iwl_priv *priv,
 
 	if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
 	    (pkt->hdr.cmd == STATISTICS_NOTIFICATION)) {
-		iwl_rx_calc_noise(priv);
+		iwl4965_rx_calc_noise(priv);
 		queue_work(priv->workqueue, &priv->run_time_calib_work);
 	}
 	if (priv->cfg->ops->lib->temp_ops.temperature && change)
 		priv->cfg->ops->lib->temp_ops.temperature(priv);
 }
 
-void iwl_reply_statistics(struct iwl_priv *priv,
+void iwl4965_reply_statistics(struct iwl_priv *priv,
 			      struct iwl_rx_mem_buffer *rxb)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 
 	if (le32_to_cpu(pkt->u.stats.flag) & UCODE_STATISTICS_CLEAR_MSK) {
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-		memset(&priv->_agn.accum_statistics, 0,
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+		memset(&priv->_4965.accum_statistics, 0,
 			sizeof(struct iwl_notif_statistics));
-		memset(&priv->_agn.delta_statistics, 0,
+		memset(&priv->_4965.delta_statistics, 0,
 			sizeof(struct iwl_notif_statistics));
-		memset(&priv->_agn.max_delta, 0,
+		memset(&priv->_4965.max_delta, 0,
 			sizeof(struct iwl_notif_statistics));
-		memset(&priv->_agn.accum_statistics_bt, 0,
-			sizeof(struct iwl_bt_notif_statistics));
-		memset(&priv->_agn.delta_statistics_bt, 0,
-			sizeof(struct iwl_bt_notif_statistics));
-		memset(&priv->_agn.max_delta_bt, 0,
-			sizeof(struct iwl_bt_notif_statistics));
 #endif
 		IWL_DEBUG_RX(priv, "Statistics have been cleared\n");
 	}
-	iwl_rx_statistics(priv, rxb);
+	iwl4965_rx_statistics(priv, rxb);
 }
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-sta.c b/drivers/net/wireless/iwlegacy/iwl-4965-sta.c
new file mode 100644
index 000000000000..a262c23553d2
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-sta.c
@@ -0,0 +1,721 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <net/mac80211.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-4965.h"
+
+static struct iwl_link_quality_cmd *
+iwl4965_sta_alloc_lq(struct iwl_priv *priv, u8 sta_id)
+{
+	int i, r;
+	struct iwl_link_quality_cmd *link_cmd;
+	u32 rate_flags = 0;
+	__le32 rate_n_flags;
+
+	link_cmd = kzalloc(sizeof(struct iwl_link_quality_cmd), GFP_KERNEL);
+	if (!link_cmd) {
+		IWL_ERR(priv, "Unable to allocate memory for LQ cmd.\n");
+		return NULL;
+	}
+	/* Set up the rate scaling to start at selected rate, fall back
+	 * all the way down to 1M in IEEE order, and then spin on 1M */
+	if (priv->band == IEEE80211_BAND_5GHZ)
+		r = IWL_RATE_6M_INDEX;
+	else
+		r = IWL_RATE_1M_INDEX;
+
+	if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
+		rate_flags |= RATE_MCS_CCK_MSK;
+
+	rate_flags |= iwl4965_first_antenna(priv->hw_params.valid_tx_ant) <<
+				RATE_MCS_ANT_POS;
+	rate_n_flags = iwl4965_hw_set_rate_n_flags(iwlegacy_rates[r].plcp,
+						   rate_flags);
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
+		link_cmd->rs_table[i].rate_n_flags = rate_n_flags;
+
+	link_cmd->general_params.single_stream_ant_msk =
+				iwl4965_first_antenna(priv->hw_params.valid_tx_ant);
+
+	link_cmd->general_params.dual_stream_ant_msk =
+		priv->hw_params.valid_tx_ant &
+		~iwl4965_first_antenna(priv->hw_params.valid_tx_ant);
+	if (!link_cmd->general_params.dual_stream_ant_msk) {
+		link_cmd->general_params.dual_stream_ant_msk = ANT_AB;
+	} else if (iwl4965_num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+		link_cmd->general_params.dual_stream_ant_msk =
+			priv->hw_params.valid_tx_ant;
+	}
+
+	link_cmd->agg_params.agg_dis_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
+	link_cmd->agg_params.agg_time_limit =
+		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
+
+	link_cmd->sta_id = sta_id;
+
+	return link_cmd;
+}
+
+/*
+ * iwl4965_add_bssid_station - Add the special IBSS BSSID station
+ *
+ * Function sleeps.
+ */
+int
+iwl4965_add_bssid_station(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
+			     const u8 *addr, u8 *sta_id_r)
+{
+	int ret;
+	u8 sta_id;
+	struct iwl_link_quality_cmd *link_cmd;
+	unsigned long flags;
+
+	if (sta_id_r)
+		*sta_id_r = IWL_INVALID_STATION;
+
+	ret = iwl_legacy_add_station_common(priv, ctx, addr, 0, NULL, &sta_id);
+	if (ret) {
+		IWL_ERR(priv, "Unable to add station %pM\n", addr);
+		return ret;
+	}
+
+	if (sta_id_r)
+		*sta_id_r = sta_id;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].used |= IWL_STA_LOCAL;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	/* Set up default rate scaling table in device's station table */
+	link_cmd = iwl4965_sta_alloc_lq(priv, sta_id);
+	if (!link_cmd) {
+		IWL_ERR(priv,
+			"Unable to initialize rate scaling for station %pM.\n",
+			addr);
+		return -ENOMEM;
+	}
+
+	ret = iwl_legacy_send_lq_cmd(priv, ctx, link_cmd, CMD_SYNC, true);
+	if (ret)
+		IWL_ERR(priv, "Link quality command failed (%d)\n", ret);
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].lq = link_cmd;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return 0;
+}
+
+static int iwl4965_static_wepkey_cmd(struct iwl_priv *priv,
+				      struct iwl_rxon_context *ctx,
+				      bool send_if_empty)
+{
+	int i, not_empty = 0;
+	u8 buff[sizeof(struct iwl_wep_cmd) +
+		sizeof(struct iwl_wep_key) * WEP_KEYS_MAX];
+	struct iwl_wep_cmd *wep_cmd = (struct iwl_wep_cmd *)buff;
+	size_t cmd_size  = sizeof(struct iwl_wep_cmd);
+	struct iwl_host_cmd cmd = {
+		.id = ctx->wep_key_cmd,
+		.data = wep_cmd,
+		.flags = CMD_SYNC,
+	};
+
+	might_sleep();
+
+	memset(wep_cmd, 0, cmd_size +
+			(sizeof(struct iwl_wep_key) * WEP_KEYS_MAX));
+
+	for (i = 0; i < WEP_KEYS_MAX ; i++) {
+		wep_cmd->key[i].key_index = i;
+		if (ctx->wep_keys[i].key_size) {
+			wep_cmd->key[i].key_offset = i;
+			not_empty = 1;
+		} else {
+			wep_cmd->key[i].key_offset = WEP_INVALID_OFFSET;
+		}
+
+		wep_cmd->key[i].key_size = ctx->wep_keys[i].key_size;
+		memcpy(&wep_cmd->key[i].key[3], ctx->wep_keys[i].key,
+				ctx->wep_keys[i].key_size);
+	}
+
+	wep_cmd->global_key_type = WEP_KEY_WEP_TYPE;
+	wep_cmd->num_keys = WEP_KEYS_MAX;
+
+	cmd_size += sizeof(struct iwl_wep_key) * WEP_KEYS_MAX;
+
+	cmd.len = cmd_size;
+
+	if (not_empty || send_if_empty)
+		return iwl_legacy_send_cmd(priv, &cmd);
+	else
+		return 0;
+}
+
+int iwl4965_restore_default_wep_keys(struct iwl_priv *priv,
+				 struct iwl_rxon_context *ctx)
+{
+	lockdep_assert_held(&priv->mutex);
+
+	return iwl4965_static_wepkey_cmd(priv, ctx, false);
+}
+
+int iwl4965_remove_default_wep_key(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx,
+			       struct ieee80211_key_conf *keyconf)
+{
+	int ret;
+
+	lockdep_assert_held(&priv->mutex);
+
+	IWL_DEBUG_WEP(priv, "Removing default WEP key: idx=%d\n",
+		      keyconf->keyidx);
+
+	memset(&ctx->wep_keys[keyconf->keyidx], 0, sizeof(ctx->wep_keys[0]));
+	if (iwl_legacy_is_rfkill(priv)) {
+		IWL_DEBUG_WEP(priv,
+		"Not sending REPLY_WEPKEY command due to RFKILL.\n");
+		/* but keys in device are clear anyway so return success */
+		return 0;
+	}
+	ret = iwl4965_static_wepkey_cmd(priv, ctx, 1);
+	IWL_DEBUG_WEP(priv, "Remove default WEP key: idx=%d ret=%d\n",
+		      keyconf->keyidx, ret);
+
+	return ret;
+}
+
+int iwl4965_set_default_wep_key(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    struct ieee80211_key_conf *keyconf)
+{
+	int ret;
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (keyconf->keylen != WEP_KEY_LEN_128 &&
+	    keyconf->keylen != WEP_KEY_LEN_64) {
+		IWL_DEBUG_WEP(priv, "Bad WEP key length %d\n", keyconf->keylen);
+		return -EINVAL;
+	}
+
+	keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
+	keyconf->hw_key_idx = HW_KEY_DEFAULT;
+	priv->stations[ctx->ap_sta_id].keyinfo.cipher = keyconf->cipher;
+
+	ctx->wep_keys[keyconf->keyidx].key_size = keyconf->keylen;
+	memcpy(&ctx->wep_keys[keyconf->keyidx].key, &keyconf->key,
+							keyconf->keylen);
+
+	ret = iwl4965_static_wepkey_cmd(priv, ctx, false);
+	IWL_DEBUG_WEP(priv, "Set default WEP key: len=%d idx=%d ret=%d\n",
+		keyconf->keylen, keyconf->keyidx, ret);
+
+	return ret;
+}
+
+static int iwl4965_set_wep_dynamic_key_info(struct iwl_priv *priv,
+					struct iwl_rxon_context *ctx,
+					struct ieee80211_key_conf *keyconf,
+					u8 sta_id)
+{
+	unsigned long flags;
+	__le16 key_flags = 0;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	lockdep_assert_held(&priv->mutex);
+
+	keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
+
+	key_flags |= (STA_KEY_FLG_WEP | STA_KEY_FLG_MAP_KEY_MSK);
+	key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
+	key_flags &= ~STA_KEY_FLG_INVALID;
+
+	if (keyconf->keylen == WEP_KEY_LEN_128)
+		key_flags |= STA_KEY_FLG_KEY_SIZE_MSK;
+
+	if (sta_id == ctx->bcast_sta_id)
+		key_flags |= STA_KEY_MULTICAST_MSK;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	priv->stations[sta_id].keyinfo.cipher = keyconf->cipher;
+	priv->stations[sta_id].keyinfo.keylen = keyconf->keylen;
+	priv->stations[sta_id].keyinfo.keyidx = keyconf->keyidx;
+
+	memcpy(priv->stations[sta_id].keyinfo.key,
+				keyconf->key, keyconf->keylen);
+
+	memcpy(&priv->stations[sta_id].sta.key.key[3],
+				keyconf->key, keyconf->keylen);
+
+	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
+			== STA_KEY_FLG_NO_ENC)
+		priv->stations[sta_id].sta.key.key_offset =
+				 iwl_legacy_get_free_ucode_key_index(priv);
+	/* else, we are overriding an existing key => no need to allocated room
+	 * in uCode. */
+
+	WARN(priv->stations[sta_id].sta.key.key_offset == WEP_INVALID_OFFSET,
+		"no space for a new key");
+
+	priv->stations[sta_id].sta.key.key_flags = key_flags;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+			sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+}
+
+static int iwl4965_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
+					 struct iwl_rxon_context *ctx,
+					 struct ieee80211_key_conf *keyconf,
+					 u8 sta_id)
+{
+	unsigned long flags;
+	__le16 key_flags = 0;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	lockdep_assert_held(&priv->mutex);
+
+	key_flags |= (STA_KEY_FLG_CCMP | STA_KEY_FLG_MAP_KEY_MSK);
+	key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
+	key_flags &= ~STA_KEY_FLG_INVALID;
+
+	if (sta_id == ctx->bcast_sta_id)
+		key_flags |= STA_KEY_MULTICAST_MSK;
+
+	keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].keyinfo.cipher = keyconf->cipher;
+	priv->stations[sta_id].keyinfo.keylen = keyconf->keylen;
+
+	memcpy(priv->stations[sta_id].keyinfo.key, keyconf->key,
+	       keyconf->keylen);
+
+	memcpy(priv->stations[sta_id].sta.key.key, keyconf->key,
+	       keyconf->keylen);
+
+	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
+			== STA_KEY_FLG_NO_ENC)
+		priv->stations[sta_id].sta.key.key_offset =
+				 iwl_legacy_get_free_ucode_key_index(priv);
+	/* else, we are overriding an existing key => no need to allocated room
+	 * in uCode. */
+
+	WARN(priv->stations[sta_id].sta.key.key_offset == WEP_INVALID_OFFSET,
+		"no space for a new key");
+
+	priv->stations[sta_id].sta.key.key_flags = key_flags;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+			 sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+}
+
+static int iwl4965_set_tkip_dynamic_key_info(struct iwl_priv *priv,
+					 struct iwl_rxon_context *ctx,
+					 struct ieee80211_key_conf *keyconf,
+					 u8 sta_id)
+{
+	unsigned long flags;
+	int ret = 0;
+	__le16 key_flags = 0;
+
+	key_flags |= (STA_KEY_FLG_TKIP | STA_KEY_FLG_MAP_KEY_MSK);
+	key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
+	key_flags &= ~STA_KEY_FLG_INVALID;
+
+	if (sta_id == ctx->bcast_sta_id)
+		key_flags |= STA_KEY_MULTICAST_MSK;
+
+	keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+	keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	priv->stations[sta_id].keyinfo.cipher = keyconf->cipher;
+	priv->stations[sta_id].keyinfo.keylen = 16;
+
+	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
+			== STA_KEY_FLG_NO_ENC)
+		priv->stations[sta_id].sta.key.key_offset =
+				 iwl_legacy_get_free_ucode_key_index(priv);
+	/* else, we are overriding an existing key => no need to allocated room
+	 * in uCode. */
+
+	WARN(priv->stations[sta_id].sta.key.key_offset == WEP_INVALID_OFFSET,
+		"no space for a new key");
+
+	priv->stations[sta_id].sta.key.key_flags = key_flags;
+
+
+	/* This copy is acutally not needed: we get the key with each TX */
+	memcpy(priv->stations[sta_id].keyinfo.key, keyconf->key, 16);
+
+	memcpy(priv->stations[sta_id].sta.key.key, keyconf->key, 16);
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return ret;
+}
+
+void iwl4965_update_tkip_key(struct iwl_priv *priv,
+			 struct iwl_rxon_context *ctx,
+			 struct ieee80211_key_conf *keyconf,
+			 struct ieee80211_sta *sta, u32 iv32, u16 *phase1key)
+{
+	u8 sta_id;
+	unsigned long flags;
+	int i;
+
+	if (iwl_legacy_scan_cancel(priv)) {
+		/* cancel scan failed, just live w/ bad key and rely
+		   briefly on SW decryption */
+		return;
+	}
+
+	sta_id = iwl_legacy_sta_id_or_broadcast(priv, ctx, sta);
+	if (sta_id == IWL_INVALID_STATION)
+		return;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	priv->stations[sta_id].sta.key.tkip_rx_tsc_byte2 = (u8) iv32;
+
+	for (i = 0; i < 5; i++)
+		priv->stations[sta_id].sta.key.tkip_rx_ttak[i] =
+			cpu_to_le16(phase1key[i]);
+
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	iwl_legacy_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+}
+
+int iwl4965_remove_dynamic_key(struct iwl_priv *priv,
+			   struct iwl_rxon_context *ctx,
+			   struct ieee80211_key_conf *keyconf,
+			   u8 sta_id)
+{
+	unsigned long flags;
+	u16 key_flags;
+	u8 keyidx;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	lockdep_assert_held(&priv->mutex);
+
+	ctx->key_mapping_keys--;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	key_flags = le16_to_cpu(priv->stations[sta_id].sta.key.key_flags);
+	keyidx = (key_flags >> STA_KEY_FLG_KEYID_POS) & 0x3;
+
+	IWL_DEBUG_WEP(priv, "Remove dynamic key: idx=%d sta=%d\n",
+		      keyconf->keyidx, sta_id);
+
+	if (keyconf->keyidx != keyidx) {
+		/* We need to remove a key with index different that the one
+		 * in the uCode. This means that the key we need to remove has
+		 * been replaced by another one with different index.
+		 * Don't do anything and return ok
+		 */
+		spin_unlock_irqrestore(&priv->sta_lock, flags);
+		return 0;
+	}
+
+	if (priv->stations[sta_id].sta.key.key_offset == WEP_INVALID_OFFSET) {
+		IWL_WARN(priv, "Removing wrong key %d 0x%x\n",
+			    keyconf->keyidx, key_flags);
+		spin_unlock_irqrestore(&priv->sta_lock, flags);
+		return 0;
+	}
+
+	if (!test_and_clear_bit(priv->stations[sta_id].sta.key.key_offset,
+		&priv->ucode_key_table))
+		IWL_ERR(priv, "index %d not used in uCode key table.\n",
+			priv->stations[sta_id].sta.key.key_offset);
+	memset(&priv->stations[sta_id].keyinfo, 0,
+					sizeof(struct iwl_hw_key));
+	memset(&priv->stations[sta_id].sta.key, 0,
+					sizeof(struct iwl4965_keyinfo));
+	priv->stations[sta_id].sta.key.key_flags =
+			STA_KEY_FLG_NO_ENC | STA_KEY_FLG_INVALID;
+	priv->stations[sta_id].sta.key.key_offset = WEP_INVALID_OFFSET;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	if (iwl_legacy_is_rfkill(priv)) {
+		IWL_DEBUG_WEP(priv,
+		 "Not sending REPLY_ADD_STA command because RFKILL enabled.\n");
+		spin_unlock_irqrestore(&priv->sta_lock, flags);
+		return 0;
+	}
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+			sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+}
+
+int iwl4965_set_dynamic_key(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
+			struct ieee80211_key_conf *keyconf, u8 sta_id)
+{
+	int ret;
+
+	lockdep_assert_held(&priv->mutex);
+
+	ctx->key_mapping_keys++;
+	keyconf->hw_key_idx = HW_KEY_DYNAMIC;
+
+	switch (keyconf->cipher) {
+	case WLAN_CIPHER_SUITE_CCMP:
+		ret = iwl4965_set_ccmp_dynamic_key_info(priv, ctx,
+							keyconf, sta_id);
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		ret = iwl4965_set_tkip_dynamic_key_info(priv, ctx,
+							keyconf, sta_id);
+		break;
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		ret = iwl4965_set_wep_dynamic_key_info(priv, ctx,
+							keyconf, sta_id);
+		break;
+	default:
+		IWL_ERR(priv,
+			"Unknown alg: %s cipher = %x\n", __func__,
+			keyconf->cipher);
+		ret = -EINVAL;
+	}
+
+	IWL_DEBUG_WEP(priv,
+		"Set dynamic key: cipher=%x len=%d idx=%d sta=%d ret=%d\n",
+		      keyconf->cipher, keyconf->keylen, keyconf->keyidx,
+		      sta_id, ret);
+
+	return ret;
+}
+
+/**
+ * iwl4965_alloc_bcast_station - add broadcast station into driver's station table.
+ *
+ * This adds the broadcast station into the driver's station table
+ * and marks it driver active, so that it will be restored to the
+ * device at the next best time.
+ */
+int iwl4965_alloc_bcast_station(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx)
+{
+	struct iwl_link_quality_cmd *link_cmd;
+	unsigned long flags;
+	u8 sta_id;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	sta_id = iwl_legacy_prep_station(priv, ctx, iwlegacy_bcast_addr,
+								false, NULL);
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_ERR(priv, "Unable to prepare broadcast station\n");
+		spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+		return -EINVAL;
+	}
+
+	priv->stations[sta_id].used |= IWL_STA_DRIVER_ACTIVE;
+	priv->stations[sta_id].used |= IWL_STA_BCAST;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	link_cmd = iwl4965_sta_alloc_lq(priv, sta_id);
+	if (!link_cmd) {
+		IWL_ERR(priv,
+			"Unable to initialize rate scaling for bcast station.\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].lq = link_cmd;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return 0;
+}
+
+/**
+ * iwl4965_update_bcast_station - update broadcast station's LQ command
+ *
+ * Only used by iwl4965. Placed here to have all bcast station management
+ * code together.
+ */
+static int iwl4965_update_bcast_station(struct iwl_priv *priv,
+				    struct iwl_rxon_context *ctx)
+{
+	unsigned long flags;
+	struct iwl_link_quality_cmd *link_cmd;
+	u8 sta_id = ctx->bcast_sta_id;
+
+	link_cmd = iwl4965_sta_alloc_lq(priv, sta_id);
+	if (!link_cmd) {
+		IWL_ERR(priv,
+		"Unable to initialize rate scaling for bcast station.\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	if (priv->stations[sta_id].lq)
+		kfree(priv->stations[sta_id].lq);
+	else
+		IWL_DEBUG_INFO(priv,
+		"Bcast station rate scaling has not been initialized yet.\n");
+	priv->stations[sta_id].lq = link_cmd;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return 0;
+}
+
+int iwl4965_update_bcast_stations(struct iwl_priv *priv)
+{
+	struct iwl_rxon_context *ctx;
+	int ret = 0;
+
+	for_each_context(priv, ctx) {
+		ret = iwl4965_update_bcast_station(priv, ctx);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+/**
+ * iwl4965_sta_tx_modify_enable_tid - Enable Tx for this TID in station table
+ */
+int iwl4965_sta_tx_modify_enable_tid(struct iwl_priv *priv, int sta_id, int tid)
+{
+	unsigned long flags;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	lockdep_assert_held(&priv->mutex);
+
+	/* Remove "disable" flag, to enable Tx for this TID */
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_TID_DISABLE_TX;
+	priv->stations[sta_id].sta.tid_disable_tx &= cpu_to_le16(~(1 << tid));
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+					sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+}
+
+int iwl4965_sta_rx_agg_start(struct iwl_priv *priv, struct ieee80211_sta *sta,
+			 int tid, u16 ssn)
+{
+	unsigned long flags;
+	int sta_id;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	lockdep_assert_held(&priv->mutex);
+
+	sta_id = iwl_legacy_sta_id(sta);
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.station_flags_msk = 0;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_ADDBA_TID_MSK;
+	priv->stations[sta_id].sta.add_immediate_ba_tid = (u8)tid;
+	priv->stations[sta_id].sta.add_immediate_ba_ssn = cpu_to_le16(ssn);
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+					sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+}
+
+int iwl4965_sta_rx_agg_stop(struct iwl_priv *priv, struct ieee80211_sta *sta,
+			int tid)
+{
+	unsigned long flags;
+	int sta_id;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	lockdep_assert_held(&priv->mutex);
+
+	sta_id = iwl_legacy_sta_id(sta);
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_ERR(priv, "Invalid station for AGG tid %d\n", tid);
+		return -ENXIO;
+	}
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.station_flags_msk = 0;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_DELBA_TID_MSK;
+	priv->stations[sta_id].sta.remove_immediate_ba_tid = (u8)tid;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+				sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+}
+
+void
+iwl4965_sta_modify_sleep_tx_count(struct iwl_priv *priv, int sta_id, int cnt)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.station_flags |= STA_FLG_PWR_SAVE_MSK;
+	priv->stations[sta_id].sta.station_flags_msk = STA_FLG_PWR_SAVE_MSK;
+	priv->stations[sta_id].sta.sta.modify_mask =
+					STA_MODIFY_SLEEP_TX_COUNT_MSK;
+	priv->stations[sta_id].sta.sleep_tx_count = cpu_to_le16(cnt);
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	iwl_legacy_send_add_sta(priv,
+				&priv->stations[sta_id].sta, CMD_ASYNC);
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+}
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-tx.c b/drivers/net/wireless/iwlegacy/iwl-4965-tx.c
new file mode 100644
index 000000000000..5c40502f869a
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-tx.c
@@ -0,0 +1,1369 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+#include "iwl-4965-hw.h"
+#include "iwl-4965.h"
+
+/*
+ * mac80211 queues, ACs, hardware queues, FIFOs.
+ *
+ * Cf. http://wireless.kernel.org/en/developers/Documentation/mac80211/queues
+ *
+ * Mac80211 uses the following numbers, which we get as from it
+ * by way of skb_get_queue_mapping(skb):
+ *
+ *	VO	0
+ *	VI	1
+ *	BE	2
+ *	BK	3
+ *
+ *
+ * Regular (not A-MPDU) frames are put into hardware queues corresponding
+ * to the FIFOs, see comments in iwl-prph.h. Aggregated frames get their
+ * own queue per aggregation session (RA/TID combination), such queues are
+ * set up to map into FIFOs too, for which we need an AC->FIFO mapping. In
+ * order to map frames to the right queue, we also need an AC->hw queue
+ * mapping. This is implemented here.
+ *
+ * Due to the way hw queues are set up (by the hw specific modules like
+ * iwl-4965.c), the AC->hw queue mapping is the identity
+ * mapping.
+ */
+
+static const u8 tid_to_ac[] = {
+	IEEE80211_AC_BE,
+	IEEE80211_AC_BK,
+	IEEE80211_AC_BK,
+	IEEE80211_AC_BE,
+	IEEE80211_AC_VI,
+	IEEE80211_AC_VI,
+	IEEE80211_AC_VO,
+	IEEE80211_AC_VO
+};
+
+static inline int iwl4965_get_ac_from_tid(u16 tid)
+{
+	if (likely(tid < ARRAY_SIZE(tid_to_ac)))
+		return tid_to_ac[tid];
+
+	/* no support for TIDs 8-15 yet */
+	return -EINVAL;
+}
+
+static inline int
+iwl4965_get_fifo_from_tid(struct iwl_rxon_context *ctx, u16 tid)
+{
+	if (likely(tid < ARRAY_SIZE(tid_to_ac)))
+		return ctx->ac_to_fifo[tid_to_ac[tid]];
+
+	/* no support for TIDs 8-15 yet */
+	return -EINVAL;
+}
+
+/*
+ * handle build REPLY_TX command notification.
+ */
+static void iwl4965_tx_cmd_build_basic(struct iwl_priv *priv,
+					struct sk_buff *skb,
+					struct iwl_tx_cmd *tx_cmd,
+					struct ieee80211_tx_info *info,
+					struct ieee80211_hdr *hdr,
+					u8 std_id)
+{
+	__le16 fc = hdr->frame_control;
+	__le32 tx_flags = tx_cmd->tx_flags;
+
+	tx_cmd->stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+		tx_flags |= TX_CMD_FLG_ACK_MSK;
+		if (ieee80211_is_mgmt(fc))
+			tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+		if (ieee80211_is_probe_resp(fc) &&
+		    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
+			tx_flags |= TX_CMD_FLG_TSF_MSK;
+	} else {
+		tx_flags &= (~TX_CMD_FLG_ACK_MSK);
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+	}
+
+	if (ieee80211_is_back_req(fc))
+		tx_flags |= TX_CMD_FLG_ACK_MSK | TX_CMD_FLG_IMM_BA_RSP_MASK;
+
+	tx_cmd->sta_id = std_id;
+	if (ieee80211_has_morefrags(fc))
+		tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
+
+	if (ieee80211_is_data_qos(fc)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		tx_cmd->tid_tspec = qc[0] & 0xf;
+		tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
+	} else {
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+	}
+
+	iwl_legacy_tx_cmd_protection(priv, info, fc, &tx_flags);
+
+	tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
+	if (ieee80211_is_mgmt(fc)) {
+		if (ieee80211_is_assoc_req(fc) || ieee80211_is_reassoc_req(fc))
+			tx_cmd->timeout.pm_frame_timeout = cpu_to_le16(3);
+		else
+			tx_cmd->timeout.pm_frame_timeout = cpu_to_le16(2);
+	} else {
+		tx_cmd->timeout.pm_frame_timeout = 0;
+	}
+
+	tx_cmd->driver_txop = 0;
+	tx_cmd->tx_flags = tx_flags;
+	tx_cmd->next_frame_len = 0;
+}
+
+#define RTS_DFAULT_RETRY_LIMIT		60
+
+static void iwl4965_tx_cmd_build_rate(struct iwl_priv *priv,
+			      struct iwl_tx_cmd *tx_cmd,
+			      struct ieee80211_tx_info *info,
+			      __le16 fc)
+{
+	u32 rate_flags;
+	int rate_idx;
+	u8 rts_retry_limit;
+	u8 data_retry_limit;
+	u8 rate_plcp;
+
+	/* Set retry limit on DATA packets and Probe Responses*/
+	if (ieee80211_is_probe_resp(fc))
+		data_retry_limit = 3;
+	else
+		data_retry_limit = IWL4965_DEFAULT_TX_RETRY;
+	tx_cmd->data_retry_limit = data_retry_limit;
+
+	/* Set retry limit on RTS packets */
+	rts_retry_limit = RTS_DFAULT_RETRY_LIMIT;
+	if (data_retry_limit < rts_retry_limit)
+		rts_retry_limit = data_retry_limit;
+	tx_cmd->rts_retry_limit = rts_retry_limit;
+
+	/* DATA packets will use the uCode station table for rate/antenna
+	 * selection */
+	if (ieee80211_is_data(fc)) {
+		tx_cmd->initial_rate_index = 0;
+		tx_cmd->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
+		return;
+	}
+
+	/**
+	 * If the current TX rate stored in mac80211 has the MCS bit set, it's
+	 * not really a TX rate.  Thus, we use the lowest supported rate for
+	 * this band.  Also use the lowest supported rate if the stored rate
+	 * index is invalid.
+	 */
+	rate_idx = info->control.rates[0].idx;
+	if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS ||
+			(rate_idx < 0) || (rate_idx > IWL_RATE_COUNT_LEGACY))
+		rate_idx = rate_lowest_index(&priv->bands[info->band],
+				info->control.sta);
+	/* For 5 GHZ band, remap mac80211 rate indices into driver indices */
+	if (info->band == IEEE80211_BAND_5GHZ)
+		rate_idx += IWL_FIRST_OFDM_RATE;
+	/* Get PLCP rate for tx_cmd->rate_n_flags */
+	rate_plcp = iwlegacy_rates[rate_idx].plcp;
+	/* Zero out flags for this packet */
+	rate_flags = 0;
+
+	/* Set CCK flag as needed */
+	if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
+		rate_flags |= RATE_MCS_CCK_MSK;
+
+	/* Set up antennas */
+	priv->mgmt_tx_ant = iwl4965_toggle_tx_ant(priv, priv->mgmt_tx_ant,
+				      priv->hw_params.valid_tx_ant);
+
+	rate_flags |= iwl4965_ant_idx_to_flags(priv->mgmt_tx_ant);
+
+	/* Set the rate in the TX cmd */
+	tx_cmd->rate_n_flags = iwl4965_hw_set_rate_n_flags(rate_plcp, rate_flags);
+}
+
+static void iwl4965_tx_cmd_build_hwcrypto(struct iwl_priv *priv,
+				      struct ieee80211_tx_info *info,
+				      struct iwl_tx_cmd *tx_cmd,
+				      struct sk_buff *skb_frag,
+				      int sta_id)
+{
+	struct ieee80211_key_conf *keyconf = info->control.hw_key;
+
+	switch (keyconf->cipher) {
+	case WLAN_CIPHER_SUITE_CCMP:
+		tx_cmd->sec_ctl = TX_CMD_SEC_CCM;
+		memcpy(tx_cmd->key, keyconf->key, keyconf->keylen);
+		if (info->flags & IEEE80211_TX_CTL_AMPDU)
+			tx_cmd->tx_flags |= TX_CMD_FLG_AGG_CCMP_MSK;
+		IWL_DEBUG_TX(priv, "tx_cmd with AES hwcrypto\n");
+		break;
+
+	case WLAN_CIPHER_SUITE_TKIP:
+		tx_cmd->sec_ctl = TX_CMD_SEC_TKIP;
+		ieee80211_get_tkip_key(keyconf, skb_frag,
+			IEEE80211_TKIP_P2_KEY, tx_cmd->key);
+		IWL_DEBUG_TX(priv, "tx_cmd with tkip hwcrypto\n");
+		break;
+
+	case WLAN_CIPHER_SUITE_WEP104:
+		tx_cmd->sec_ctl |= TX_CMD_SEC_KEY128;
+		/* fall through */
+	case WLAN_CIPHER_SUITE_WEP40:
+		tx_cmd->sec_ctl |= (TX_CMD_SEC_WEP |
+			(keyconf->keyidx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT);
+
+		memcpy(&tx_cmd->key[3], keyconf->key, keyconf->keylen);
+
+		IWL_DEBUG_TX(priv, "Configuring packet for WEP encryption "
+			     "with key %d\n", keyconf->keyidx);
+		break;
+
+	default:
+		IWL_ERR(priv, "Unknown encode cipher %x\n", keyconf->cipher);
+		break;
+	}
+}
+
+/*
+ * start REPLY_TX command process
+ */
+int iwl4965_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_sta *sta = info->control.sta;
+	struct iwl_station_priv *sta_priv = NULL;
+	struct iwl_tx_queue *txq;
+	struct iwl_queue *q;
+	struct iwl_device_cmd *out_cmd;
+	struct iwl_cmd_meta *out_meta;
+	struct iwl_tx_cmd *tx_cmd;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	int txq_id;
+	dma_addr_t phys_addr;
+	dma_addr_t txcmd_phys;
+	dma_addr_t scratch_phys;
+	u16 len, firstlen, secondlen;
+	u16 seq_number = 0;
+	__le16 fc;
+	u8 hdr_len;
+	u8 sta_id;
+	u8 wait_write_ptr = 0;
+	u8 tid = 0;
+	u8 *qc = NULL;
+	unsigned long flags;
+	bool is_agg = false;
+
+	if (info->control.vif)
+		ctx = iwl_legacy_rxon_ctx_from_vif(info->control.vif);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (iwl_legacy_is_rfkill(priv)) {
+		IWL_DEBUG_DROP(priv, "Dropping - RF KILL\n");
+		goto drop_unlock;
+	}
+
+	fc = hdr->frame_control;
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (ieee80211_is_auth(fc))
+		IWL_DEBUG_TX(priv, "Sending AUTH frame\n");
+	else if (ieee80211_is_assoc_req(fc))
+		IWL_DEBUG_TX(priv, "Sending ASSOC frame\n");
+	else if (ieee80211_is_reassoc_req(fc))
+		IWL_DEBUG_TX(priv, "Sending REASSOC frame\n");
+#endif
+
+	hdr_len = ieee80211_hdrlen(fc);
+
+	/* Find index into station table for destination station */
+	sta_id = iwl_legacy_sta_id_or_broadcast(priv, ctx, info->control.sta);
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_DEBUG_DROP(priv, "Dropping - INVALID STATION: %pM\n",
+			       hdr->addr1);
+		goto drop_unlock;
+	}
+
+	IWL_DEBUG_TX(priv, "station Id %d\n", sta_id);
+
+	if (sta)
+		sta_priv = (void *)sta->drv_priv;
+
+	if (sta_priv && sta_priv->asleep &&
+	    (info->flags & IEEE80211_TX_CTL_PSPOLL_RESPONSE)) {
+		/*
+		 * This sends an asynchronous command to the device,
+		 * but we can rely on it being processed before the
+		 * next frame is processed -- and the next frame to
+		 * this station is the one that will consume this
+		 * counter.
+		 * For now set the counter to just 1 since we do not
+		 * support uAPSD yet.
+		 */
+		iwl4965_sta_modify_sleep_tx_count(priv, sta_id, 1);
+	}
+
+	/*
+	 * Send this frame after DTIM -- there's a special queue
+	 * reserved for this for contexts that support AP mode.
+	 */
+	if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
+		txq_id = ctx->mcast_queue;
+		/*
+		 * The microcode will clear the more data
+		 * bit in the last frame it transmits.
+		 */
+		hdr->frame_control |=
+			cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+	} else
+		txq_id = ctx->ac_to_queue[skb_get_queue_mapping(skb)];
+
+	/* irqs already disabled/saved above when locking priv->lock */
+	spin_lock(&priv->sta_lock);
+
+	if (ieee80211_is_data_qos(fc)) {
+		qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & IEEE80211_QOS_CTL_TID_MASK;
+		if (WARN_ON_ONCE(tid >= MAX_TID_COUNT)) {
+			spin_unlock(&priv->sta_lock);
+			goto drop_unlock;
+		}
+		seq_number = priv->stations[sta_id].tid[tid].seq_number;
+		seq_number &= IEEE80211_SCTL_SEQ;
+		hdr->seq_ctrl = hdr->seq_ctrl &
+				cpu_to_le16(IEEE80211_SCTL_FRAG);
+		hdr->seq_ctrl |= cpu_to_le16(seq_number);
+		seq_number += 0x10;
+		/* aggregation is on for this <sta,tid> */
+		if (info->flags & IEEE80211_TX_CTL_AMPDU &&
+		    priv->stations[sta_id].tid[tid].agg.state == IWL_AGG_ON) {
+			txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
+			is_agg = true;
+		}
+	}
+
+	txq = &priv->txq[txq_id];
+	q = &txq->q;
+
+	if (unlikely(iwl_legacy_queue_space(q) < q->high_mark)) {
+		spin_unlock(&priv->sta_lock);
+		goto drop_unlock;
+	}
+
+	if (ieee80211_is_data_qos(fc)) {
+		priv->stations[sta_id].tid[tid].tfds_in_queue++;
+		if (!ieee80211_has_morefrags(fc))
+			priv->stations[sta_id].tid[tid].seq_number = seq_number;
+	}
+
+	spin_unlock(&priv->sta_lock);
+
+	/* Set up driver data for this TFD */
+	memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl_tx_info));
+	txq->txb[q->write_ptr].skb = skb;
+	txq->txb[q->write_ptr].ctx = ctx;
+
+	/* Set up first empty entry in queue's array of Tx/cmd buffers */
+	out_cmd = txq->cmd[q->write_ptr];
+	out_meta = &txq->meta[q->write_ptr];
+	tx_cmd = &out_cmd->cmd.tx;
+	memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
+	memset(tx_cmd, 0, sizeof(struct iwl_tx_cmd));
+
+	/*
+	 * Set up the Tx-command (not MAC!) header.
+	 * Store the chosen Tx queue and TFD index within the sequence field;
+	 * after Tx, uCode's Tx response will return this value so driver can
+	 * locate the frame within the tx queue and do post-tx processing.
+	 */
+	out_cmd->hdr.cmd = REPLY_TX;
+	out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
+				INDEX_TO_SEQ(q->write_ptr)));
+
+	/* Copy MAC header from skb into command buffer */
+	memcpy(tx_cmd->hdr, hdr, hdr_len);
+
+
+	/* Total # bytes to be transmitted */
+	len = (u16)skb->len;
+	tx_cmd->len = cpu_to_le16(len);
+
+	if (info->control.hw_key)
+		iwl4965_tx_cmd_build_hwcrypto(priv, info, tx_cmd, skb, sta_id);
+
+	/* TODO need this for burst mode later on */
+	iwl4965_tx_cmd_build_basic(priv, skb, tx_cmd, info, hdr, sta_id);
+	iwl_legacy_dbg_log_tx_data_frame(priv, len, hdr);
+
+	iwl4965_tx_cmd_build_rate(priv, tx_cmd, info, fc);
+
+	iwl_legacy_update_stats(priv, true, fc, len);
+	/*
+	 * Use the first empty entry in this queue's command buffer array
+	 * to contain the Tx command and MAC header concatenated together
+	 * (payload data will be in another buffer).
+	 * Size of this varies, due to varying MAC header length.
+	 * If end is not dword aligned, we'll have 2 extra bytes at the end
+	 * of the MAC header (device reads on dword boundaries).
+	 * We'll tell device about this padding later.
+	 */
+	len = sizeof(struct iwl_tx_cmd) +
+		sizeof(struct iwl_cmd_header) + hdr_len;
+	firstlen = (len + 3) & ~3;
+
+	/* Tell NIC about any 2-byte padding after MAC header */
+	if (firstlen != len)
+		tx_cmd->tx_flags |= TX_CMD_FLG_MH_PAD_MSK;
+
+	/* Physical address of this Tx command's header (not MAC header!),
+	 * within command buffer array. */
+	txcmd_phys = pci_map_single(priv->pci_dev,
+				    &out_cmd->hdr, firstlen,
+				    PCI_DMA_BIDIRECTIONAL);
+	dma_unmap_addr_set(out_meta, mapping, txcmd_phys);
+	dma_unmap_len_set(out_meta, len, firstlen);
+	/* Add buffer containing Tx command and MAC(!) header to TFD's
+	 * first entry */
+	priv->cfg->ops->lib->txq_attach_buf_to_tfd(priv, txq,
+						   txcmd_phys, firstlen, 1, 0);
+
+	if (!ieee80211_has_morefrags(hdr->frame_control)) {
+		txq->need_update = 1;
+	} else {
+		wait_write_ptr = 1;
+		txq->need_update = 0;
+	}
+
+	/* Set up TFD's 2nd entry to point directly to remainder of skb,
+	 * if any (802.11 null frames have no payload). */
+	secondlen = skb->len - hdr_len;
+	if (secondlen > 0) {
+		phys_addr = pci_map_single(priv->pci_dev, skb->data + hdr_len,
+					   secondlen, PCI_DMA_TODEVICE);
+		priv->cfg->ops->lib->txq_attach_buf_to_tfd(priv, txq,
+							   phys_addr, secondlen,
+							   0, 0);
+	}
+
+	scratch_phys = txcmd_phys + sizeof(struct iwl_cmd_header) +
+				offsetof(struct iwl_tx_cmd, scratch);
+
+	/* take back ownership of DMA buffer to enable update */
+	pci_dma_sync_single_for_cpu(priv->pci_dev, txcmd_phys,
+				    firstlen, PCI_DMA_BIDIRECTIONAL);
+	tx_cmd->dram_lsb_ptr = cpu_to_le32(scratch_phys);
+	tx_cmd->dram_msb_ptr = iwl_legacy_get_dma_hi_addr(scratch_phys);
+
+	IWL_DEBUG_TX(priv, "sequence nr = 0X%x\n",
+		     le16_to_cpu(out_cmd->hdr.sequence));
+	IWL_DEBUG_TX(priv, "tx_flags = 0X%x\n", le32_to_cpu(tx_cmd->tx_flags));
+	iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd, sizeof(*tx_cmd));
+	iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd->hdr, hdr_len);
+
+	/* Set up entry for this TFD in Tx byte-count array */
+	if (info->flags & IEEE80211_TX_CTL_AMPDU)
+		priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq,
+						     le16_to_cpu(tx_cmd->len));
+
+	pci_dma_sync_single_for_device(priv->pci_dev, txcmd_phys,
+				       firstlen, PCI_DMA_BIDIRECTIONAL);
+
+	trace_iwlwifi_legacy_dev_tx(priv,
+			     &((struct iwl_tfd *)txq->tfds)[txq->q.write_ptr],
+			     sizeof(struct iwl_tfd),
+			     &out_cmd->hdr, firstlen,
+			     skb->data + hdr_len, secondlen);
+
+	/* Tell device the write index *just past* this latest filled TFD */
+	q->write_ptr = iwl_legacy_queue_inc_wrap(q->write_ptr, q->n_bd);
+	iwl_legacy_txq_update_write_ptr(priv, txq);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/*
+	 * At this point the frame is "transmitted" successfully
+	 * and we will get a TX status notification eventually,
+	 * regardless of the value of ret. "ret" only indicates
+	 * whether or not we should update the write pointer.
+	 */
+
+	/*
+	 * Avoid atomic ops if it isn't an associated client.
+	 * Also, if this is a packet for aggregation, don't
+	 * increase the counter because the ucode will stop
+	 * aggregation queues when their respective station
+	 * goes to sleep.
+	 */
+	if (sta_priv && sta_priv->client && !is_agg)
+		atomic_inc(&sta_priv->pending_frames);
+
+	if ((iwl_legacy_queue_space(q) < q->high_mark) &&
+			priv->mac80211_registered) {
+		if (wait_write_ptr) {
+			spin_lock_irqsave(&priv->lock, flags);
+			txq->need_update = 1;
+			iwl_legacy_txq_update_write_ptr(priv, txq);
+			spin_unlock_irqrestore(&priv->lock, flags);
+		} else {
+			iwl_legacy_stop_queue(priv, txq);
+		}
+	}
+
+	return 0;
+
+drop_unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return -1;
+}
+
+static inline int iwl4965_alloc_dma_ptr(struct iwl_priv *priv,
+				    struct iwl_dma_ptr *ptr, size_t size)
+{
+	ptr->addr = dma_alloc_coherent(&priv->pci_dev->dev, size, &ptr->dma,
+				       GFP_KERNEL);
+	if (!ptr->addr)
+		return -ENOMEM;
+	ptr->size = size;
+	return 0;
+}
+
+static inline void iwl4965_free_dma_ptr(struct iwl_priv *priv,
+				    struct iwl_dma_ptr *ptr)
+{
+	if (unlikely(!ptr->addr))
+		return;
+
+	dma_free_coherent(&priv->pci_dev->dev, ptr->size, ptr->addr, ptr->dma);
+	memset(ptr, 0, sizeof(*ptr));
+}
+
+/**
+ * iwl4965_hw_txq_ctx_free - Free TXQ Context
+ *
+ * Destroy all TX DMA queues and structures
+ */
+void iwl4965_hw_txq_ctx_free(struct iwl_priv *priv)
+{
+	int txq_id;
+
+	/* Tx queues */
+	if (priv->txq) {
+		for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+			if (txq_id == priv->cmd_queue)
+				iwl_legacy_cmd_queue_free(priv);
+			else
+				iwl_legacy_tx_queue_free(priv, txq_id);
+	}
+	iwl4965_free_dma_ptr(priv, &priv->kw);
+
+	iwl4965_free_dma_ptr(priv, &priv->scd_bc_tbls);
+
+	/* free tx queue structure */
+	iwl_legacy_txq_mem(priv);
+}
+
+/**
+ * iwl4965_txq_ctx_alloc - allocate TX queue context
+ * Allocate all Tx DMA structures and initialize them
+ *
+ * @param priv
+ * @return error code
+ */
+int iwl4965_txq_ctx_alloc(struct iwl_priv *priv)
+{
+	int ret;
+	int txq_id, slots_num;
+	unsigned long flags;
+
+	/* Free all tx/cmd queues and keep-warm buffer */
+	iwl4965_hw_txq_ctx_free(priv);
+
+	ret = iwl4965_alloc_dma_ptr(priv, &priv->scd_bc_tbls,
+				priv->hw_params.scd_bc_tbls_size);
+	if (ret) {
+		IWL_ERR(priv, "Scheduler BC Table allocation failed\n");
+		goto error_bc_tbls;
+	}
+	/* Alloc keep-warm buffer */
+	ret = iwl4965_alloc_dma_ptr(priv, &priv->kw, IWL_KW_SIZE);
+	if (ret) {
+		IWL_ERR(priv, "Keep Warm allocation failed\n");
+		goto error_kw;
+	}
+
+	/* allocate tx queue structure */
+	ret = iwl_legacy_alloc_txq_mem(priv);
+	if (ret)
+		goto error;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Turn off all Tx DMA fifos */
+	iwl4965_txq_set_sched(priv, 0);
+
+	/* Tell NIC where to find the "keep warm" buffer */
+	iwl_legacy_write_direct32(priv, FH_KW_MEM_ADDR_REG, priv->kw.dma >> 4);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Alloc and init all Tx queues, including the command queue (#4/#9) */
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
+		slots_num = (txq_id == priv->cmd_queue) ?
+					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
+		ret = iwl_legacy_tx_queue_init(priv,
+					&priv->txq[txq_id], slots_num,
+				       txq_id);
+		if (ret) {
+			IWL_ERR(priv, "Tx %d queue init failed\n", txq_id);
+			goto error;
+		}
+	}
+
+	return ret;
+
+ error:
+	iwl4965_hw_txq_ctx_free(priv);
+	iwl4965_free_dma_ptr(priv, &priv->kw);
+ error_kw:
+	iwl4965_free_dma_ptr(priv, &priv->scd_bc_tbls);
+ error_bc_tbls:
+	return ret;
+}
+
+void iwl4965_txq_ctx_reset(struct iwl_priv *priv)
+{
+	int txq_id, slots_num;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Turn off all Tx DMA fifos */
+	iwl4965_txq_set_sched(priv, 0);
+
+	/* Tell NIC where to find the "keep warm" buffer */
+	iwl_legacy_write_direct32(priv, FH_KW_MEM_ADDR_REG, priv->kw.dma >> 4);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Alloc and init all Tx queues, including the command queue (#4) */
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
+		slots_num = txq_id == priv->cmd_queue ?
+			    TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
+		iwl_legacy_tx_queue_reset(priv, &priv->txq[txq_id],
+						slots_num, txq_id);
+	}
+}
+
+/**
+ * iwl4965_txq_ctx_stop - Stop all Tx DMA channels
+ */
+void iwl4965_txq_ctx_stop(struct iwl_priv *priv)
+{
+	int ch, txq_id;
+	unsigned long flags;
+
+	/* Turn off all Tx DMA fifos */
+	spin_lock_irqsave(&priv->lock, flags);
+
+	iwl4965_txq_set_sched(priv, 0);
+
+	/* Stop each Tx DMA channel, and wait for it to be idle */
+	for (ch = 0; ch < priv->hw_params.dma_chnl_num; ch++) {
+		iwl_legacy_write_direct32(priv,
+				FH_TCSR_CHNL_TX_CONFIG_REG(ch), 0x0);
+		if (iwl_poll_direct_bit(priv, FH_TSSR_TX_STATUS_REG,
+				    FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(ch),
+				    1000))
+			IWL_ERR(priv, "Failing on timeout while stopping"
+			    " DMA channel %d [0x%08x]", ch,
+			    iwl_legacy_read_direct32(priv,
+					FH_TSSR_TX_STATUS_REG));
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (!priv->txq)
+		return;
+
+	/* Unmap DMA from host system and free skb's */
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+		if (txq_id == priv->cmd_queue)
+			iwl_legacy_cmd_queue_unmap(priv);
+		else
+			iwl_legacy_tx_queue_unmap(priv, txq_id);
+}
+
+/*
+ * Find first available (lowest unused) Tx Queue, mark it "active".
+ * Called only when finding queue for aggregation.
+ * Should never return anything < 7, because they should already
+ * be in use as EDCA AC (0-3), Command (4), reserved (5, 6)
+ */
+static int iwl4965_txq_ctx_activate_free(struct iwl_priv *priv)
+{
+	int txq_id;
+
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+		if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
+			return txq_id;
+	return -1;
+}
+
+/**
+ * iwl4965_tx_queue_stop_scheduler - Stop queue, but keep configuration
+ */
+static void iwl4965_tx_queue_stop_scheduler(struct iwl_priv *priv,
+					    u16 txq_id)
+{
+	/* Simply stop the queue, but don't change any configuration;
+	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
+	iwl_legacy_write_prph(priv,
+		IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
+		(0 << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+		(1 << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+}
+
+/**
+ * iwl4965_tx_queue_set_q2ratid - Map unique receiver/tid combination to a queue
+ */
+static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
+					u16 txq_id)
+{
+	u32 tbl_dw_addr;
+	u32 tbl_dw;
+	u16 scd_q2ratid;
+
+	scd_q2ratid = ra_tid & IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
+
+	tbl_dw_addr = priv->scd_base_addr +
+			IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
+
+	tbl_dw = iwl_legacy_read_targ_mem(priv, tbl_dw_addr);
+
+	if (txq_id & 0x1)
+		tbl_dw = (scd_q2ratid << 16) | (tbl_dw & 0x0000FFFF);
+	else
+		tbl_dw = scd_q2ratid | (tbl_dw & 0xFFFF0000);
+
+	iwl_legacy_write_targ_mem(priv, tbl_dw_addr, tbl_dw);
+
+	return 0;
+}
+
+/**
+ * iwl4965_tx_queue_agg_enable - Set up & enable aggregation for selected queue
+ *
+ * NOTE:  txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE,
+ *        i.e. it must be one of the higher queues used for aggregation
+ */
+static int iwl4965_txq_agg_enable(struct iwl_priv *priv, int txq_id,
+				  int tx_fifo, int sta_id, int tid, u16 ssn_idx)
+{
+	unsigned long flags;
+	u16 ra_tid;
+	int ret;
+
+	if ((IWL49_FIRST_AMPDU_QUEUE > txq_id) ||
+	    (IWL49_FIRST_AMPDU_QUEUE +
+		priv->cfg->base_params->num_of_ampdu_queues <= txq_id)) {
+		IWL_WARN(priv,
+			"queue number out of range: %d, must be %d to %d\n",
+			txq_id, IWL49_FIRST_AMPDU_QUEUE,
+			IWL49_FIRST_AMPDU_QUEUE +
+			priv->cfg->base_params->num_of_ampdu_queues - 1);
+		return -EINVAL;
+	}
+
+	ra_tid = BUILD_RAxTID(sta_id, tid);
+
+	/* Modify device's station table to Tx this TID */
+	ret = iwl4965_sta_tx_modify_enable_tid(priv, sta_id, tid);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Stop this Tx queue before configuring it */
+	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
+
+	/* Map receiver-address / traffic-ID to this queue */
+	iwl4965_tx_queue_set_q2ratid(priv, ra_tid, txq_id);
+
+	/* Set this queue as a chain-building queue */
+	iwl_legacy_set_bits_prph(priv, IWL49_SCD_QUEUECHAIN_SEL, (1 << txq_id));
+
+	/* Place first TFD at index corresponding to start sequence number.
+	 * Assumes that ssn_idx is valid (!= 0xFFF) */
+	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	/* Set up Tx window size and frame limit for this queue */
+	iwl_legacy_write_targ_mem(priv,
+		priv->scd_base_addr + IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id),
+		(SCD_WIN_SIZE << IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
+		IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
+
+	iwl_legacy_write_targ_mem(priv, priv->scd_base_addr +
+		IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
+		(SCD_FRAME_LIMIT << IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
+		& IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
+
+	iwl_legacy_set_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
+
+	/* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
+	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 1);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+
+int iwl4965_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+{
+	int sta_id;
+	int tx_fifo;
+	int txq_id;
+	int ret;
+	unsigned long flags;
+	struct iwl_tid_data *tid_data;
+
+	tx_fifo = iwl4965_get_fifo_from_tid(iwl_legacy_rxon_ctx_from_vif(vif), tid);
+	if (unlikely(tx_fifo < 0))
+		return tx_fifo;
+
+	IWL_WARN(priv, "%s on ra = %pM tid = %d\n",
+			__func__, sta->addr, tid);
+
+	sta_id = iwl_legacy_sta_id(sta);
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_ERR(priv, "Start AGG on invalid station\n");
+		return -ENXIO;
+	}
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_OFF) {
+		IWL_ERR(priv, "Start AGG when state is not IWL_AGG_OFF !\n");
+		return -ENXIO;
+	}
+
+	txq_id = iwl4965_txq_ctx_activate_free(priv);
+	if (txq_id == -1) {
+		IWL_ERR(priv, "No free aggregation queue available\n");
+		return -ENXIO;
+	}
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	tid_data = &priv->stations[sta_id].tid[tid];
+	*ssn = SEQ_TO_SN(tid_data->seq_number);
+	tid_data->agg.txq_id = txq_id;
+	iwl_legacy_set_swq_id(&priv->txq[txq_id],
+				iwl4965_get_ac_from_tid(tid), txq_id);
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	ret = iwl4965_txq_agg_enable(priv, txq_id, tx_fifo,
+						  sta_id, tid, *ssn);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	tid_data = &priv->stations[sta_id].tid[tid];
+	if (tid_data->tfds_in_queue == 0) {
+		IWL_DEBUG_HT(priv, "HW queue is empty\n");
+		tid_data->agg.state = IWL_AGG_ON;
+		ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+	} else {
+		IWL_DEBUG_HT(priv,
+			"HW queue is NOT empty: %d packets in HW queue\n",
+			     tid_data->tfds_in_queue);
+		tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
+	}
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return ret;
+}
+
+/**
+ * txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE
+ * priv->lock must be held by the caller
+ */
+static int iwl4965_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
+				   u16 ssn_idx, u8 tx_fifo)
+{
+	if ((IWL49_FIRST_AMPDU_QUEUE > txq_id) ||
+	    (IWL49_FIRST_AMPDU_QUEUE +
+		priv->cfg->base_params->num_of_ampdu_queues <= txq_id)) {
+		IWL_WARN(priv,
+			"queue number out of range: %d, must be %d to %d\n",
+			txq_id, IWL49_FIRST_AMPDU_QUEUE,
+			IWL49_FIRST_AMPDU_QUEUE +
+			priv->cfg->base_params->num_of_ampdu_queues - 1);
+		return -EINVAL;
+	}
+
+	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
+
+	iwl_legacy_clear_bits_prph(priv,
+			IWL49_SCD_QUEUECHAIN_SEL, (1 << txq_id));
+
+	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+	/* supposes that ssn_idx is valid (!= 0xFFF) */
+	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	iwl_legacy_clear_bits_prph(priv,
+			 IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
+	iwl_txq_ctx_deactivate(priv, txq_id);
+	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
+
+	return 0;
+}
+
+int iwl4965_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta, u16 tid)
+{
+	int tx_fifo_id, txq_id, sta_id, ssn;
+	struct iwl_tid_data *tid_data;
+	int write_ptr, read_ptr;
+	unsigned long flags;
+
+	tx_fifo_id = iwl4965_get_fifo_from_tid(iwl_legacy_rxon_ctx_from_vif(vif), tid);
+	if (unlikely(tx_fifo_id < 0))
+		return tx_fifo_id;
+
+	sta_id = iwl_legacy_sta_id(sta);
+
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_ERR(priv, "Invalid station for AGG tid %d\n", tid);
+		return -ENXIO;
+	}
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	tid_data = &priv->stations[sta_id].tid[tid];
+	ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
+	txq_id = tid_data->agg.txq_id;
+
+	switch (priv->stations[sta_id].tid[tid].agg.state) {
+	case IWL_EMPTYING_HW_QUEUE_ADDBA:
+		/*
+		 * This can happen if the peer stops aggregation
+		 * again before we've had a chance to drain the
+		 * queue we selected previously, i.e. before the
+		 * session was really started completely.
+		 */
+		IWL_DEBUG_HT(priv, "AGG stop before setup done\n");
+		goto turn_off;
+	case IWL_AGG_ON:
+		break;
+	default:
+		IWL_WARN(priv, "Stopping AGG while state not ON or starting\n");
+	}
+
+	write_ptr = priv->txq[txq_id].q.write_ptr;
+	read_ptr = priv->txq[txq_id].q.read_ptr;
+
+	/* The queue is not empty */
+	if (write_ptr != read_ptr) {
+		IWL_DEBUG_HT(priv, "Stopping a non empty AGG HW QUEUE\n");
+		priv->stations[sta_id].tid[tid].agg.state =
+				IWL_EMPTYING_HW_QUEUE_DELBA;
+		spin_unlock_irqrestore(&priv->sta_lock, flags);
+		return 0;
+	}
+
+	IWL_DEBUG_HT(priv, "HW queue is empty\n");
+ turn_off:
+	priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
+
+	/* do not restore/save irqs */
+	spin_unlock(&priv->sta_lock);
+	spin_lock(&priv->lock);
+
+	/*
+	 * the only reason this call can fail is queue number out of range,
+	 * which can happen if uCode is reloaded and all the station
+	 * information are lost. if it is outside the range, there is no need
+	 * to deactivate the uCode queue, just return "success" to allow
+	 *  mac80211 to clean up it own data.
+	 */
+	iwl4965_txq_agg_disable(priv, txq_id, ssn, tx_fifo_id);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+
+	return 0;
+}
+
+int iwl4965_txq_check_empty(struct iwl_priv *priv,
+			   int sta_id, u8 tid, int txq_id)
+{
+	struct iwl_queue *q = &priv->txq[txq_id].q;
+	u8 *addr = priv->stations[sta_id].sta.sta.addr;
+	struct iwl_tid_data *tid_data = &priv->stations[sta_id].tid[tid];
+	struct iwl_rxon_context *ctx;
+
+	ctx = &priv->contexts[priv->stations[sta_id].ctxid];
+
+	lockdep_assert_held(&priv->sta_lock);
+
+	switch (priv->stations[sta_id].tid[tid].agg.state) {
+	case IWL_EMPTYING_HW_QUEUE_DELBA:
+		/* We are reclaiming the last packet of the */
+		/* aggregated HW queue */
+		if ((txq_id  == tid_data->agg.txq_id) &&
+		    (q->read_ptr == q->write_ptr)) {
+			u16 ssn = SEQ_TO_SN(tid_data->seq_number);
+			int tx_fifo = iwl4965_get_fifo_from_tid(ctx, tid);
+			IWL_DEBUG_HT(priv,
+				"HW queue empty: continue DELBA flow\n");
+			iwl4965_txq_agg_disable(priv, txq_id, ssn, tx_fifo);
+			tid_data->agg.state = IWL_AGG_OFF;
+			ieee80211_stop_tx_ba_cb_irqsafe(ctx->vif, addr, tid);
+		}
+		break;
+	case IWL_EMPTYING_HW_QUEUE_ADDBA:
+		/* We are reclaiming the last packet of the queue */
+		if (tid_data->tfds_in_queue == 0) {
+			IWL_DEBUG_HT(priv,
+				"HW queue empty: continue ADDBA flow\n");
+			tid_data->agg.state = IWL_AGG_ON;
+			ieee80211_start_tx_ba_cb_irqsafe(ctx->vif, addr, tid);
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static void iwl4965_non_agg_tx_status(struct iwl_priv *priv,
+				     struct iwl_rxon_context *ctx,
+				     const u8 *addr1)
+{
+	struct ieee80211_sta *sta;
+	struct iwl_station_priv *sta_priv;
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(ctx->vif, addr1);
+	if (sta) {
+		sta_priv = (void *)sta->drv_priv;
+		/* avoid atomic ops if this isn't a client */
+		if (sta_priv->client &&
+		    atomic_dec_return(&sta_priv->pending_frames) == 0)
+			ieee80211_sta_block_awake(priv->hw, sta, false);
+	}
+	rcu_read_unlock();
+}
+
+static void
+iwl4965_tx_status(struct iwl_priv *priv, struct iwl_tx_info *tx_info,
+			     bool is_agg)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) tx_info->skb->data;
+
+	if (!is_agg)
+		iwl4965_non_agg_tx_status(priv, tx_info->ctx, hdr->addr1);
+
+	ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb);
+}
+
+int iwl4965_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct iwl_queue *q = &txq->q;
+	struct iwl_tx_info *tx_info;
+	int nfreed = 0;
+	struct ieee80211_hdr *hdr;
+
+	if ((index >= q->n_bd) || (iwl_legacy_queue_used(q, index) == 0)) {
+		IWL_ERR(priv, "Read index for DMA queue txq id (%d), index %d, "
+			  "is out of range [0-%d] %d %d.\n", txq_id,
+			  index, q->n_bd, q->write_ptr, q->read_ptr);
+		return 0;
+	}
+
+	for (index = iwl_legacy_queue_inc_wrap(index, q->n_bd);
+	     q->read_ptr != index;
+	     q->read_ptr = iwl_legacy_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+
+		tx_info = &txq->txb[txq->q.read_ptr];
+		iwl4965_tx_status(priv, tx_info,
+				 txq_id >= IWL4965_FIRST_AMPDU_QUEUE);
+
+		hdr = (struct ieee80211_hdr *)tx_info->skb->data;
+		if (hdr && ieee80211_is_data_qos(hdr->frame_control))
+			nfreed++;
+		tx_info->skb = NULL;
+
+		priv->cfg->ops->lib->txq_free_tfd(priv, txq);
+	}
+	return nfreed;
+}
+
+/**
+ * iwl4965_tx_status_reply_compressed_ba - Update tx status from block-ack
+ *
+ * Go through block-ack's bitmap of ACK'd frames, update driver's record of
+ * ACK vs. not.  This gets sent to mac80211, then to rate scaling algo.
+ */
+static int iwl4965_tx_status_reply_compressed_ba(struct iwl_priv *priv,
+				 struct iwl_ht_agg *agg,
+				 struct iwl_compressed_ba_resp *ba_resp)
+
+{
+	int i, sh, ack;
+	u16 seq_ctl = le16_to_cpu(ba_resp->seq_ctl);
+	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
+	int successes = 0;
+	struct ieee80211_tx_info *info;
+	u64 bitmap, sent_bitmap;
+
+	if (unlikely(!agg->wait_for_ba))  {
+		if (unlikely(ba_resp->bitmap))
+			IWL_ERR(priv, "Received BA when not expected\n");
+		return -EINVAL;
+	}
+
+	/* Mark that the expected block-ack response arrived */
+	agg->wait_for_ba = 0;
+	IWL_DEBUG_TX_REPLY(priv, "BA %d %d\n", agg->start_idx,
+							ba_resp->seq_ctl);
+
+	/* Calculate shift to align block-ack bits with our Tx window bits */
+	sh = agg->start_idx - SEQ_TO_INDEX(seq_ctl >> 4);
+	if (sh < 0) /* tbw something is wrong with indices */
+		sh += 0x100;
+
+	if (agg->frame_count > (64 - sh)) {
+		IWL_DEBUG_TX_REPLY(priv, "more frames than bitmap size");
+		return -1;
+	}
+
+	/* don't use 64-bit values for now */
+	bitmap = le64_to_cpu(ba_resp->bitmap) >> sh;
+
+	/* check for success or failure according to the
+	 * transmitted bitmap and block-ack bitmap */
+	sent_bitmap = bitmap & agg->bitmap;
+
+	/* For each frame attempted in aggregation,
+	 * update driver's record of tx frame's status. */
+	i = 0;
+	while (sent_bitmap) {
+		ack = sent_bitmap & 1ULL;
+		successes += ack;
+		IWL_DEBUG_TX_REPLY(priv, "%s ON i=%d idx=%d raw=%d\n",
+			ack ? "ACK" : "NACK", i,
+			(agg->start_idx + i) & 0xff,
+			agg->start_idx + i);
+		sent_bitmap >>= 1;
+		++i;
+	}
+
+	IWL_DEBUG_TX_REPLY(priv, "Bitmap %llx\n",
+				   (unsigned long long)bitmap);
+
+	info = IEEE80211_SKB_CB(priv->txq[scd_flow].txb[agg->start_idx].skb);
+	memset(&info->status, 0, sizeof(info->status));
+	info->flags |= IEEE80211_TX_STAT_ACK;
+	info->flags |= IEEE80211_TX_STAT_AMPDU;
+	info->status.ampdu_ack_len = successes;
+	info->status.ampdu_len = agg->frame_count;
+	iwl4965_hwrate_to_tx_control(priv, agg->rate_n_flags, info);
+
+	return 0;
+}
+
+/**
+ * translate ucode response to mac80211 tx status control values
+ */
+void iwl4965_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
+				  struct ieee80211_tx_info *info)
+{
+	struct ieee80211_tx_rate *r = &info->control.rates[0];
+
+	info->antenna_sel_tx =
+		((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
+	if (rate_n_flags & RATE_MCS_HT_MSK)
+		r->flags |= IEEE80211_TX_RC_MCS;
+	if (rate_n_flags & RATE_MCS_GF_MSK)
+		r->flags |= IEEE80211_TX_RC_GREEN_FIELD;
+	if (rate_n_flags & RATE_MCS_HT40_MSK)
+		r->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+	if (rate_n_flags & RATE_MCS_DUP_MSK)
+		r->flags |= IEEE80211_TX_RC_DUP_DATA;
+	if (rate_n_flags & RATE_MCS_SGI_MSK)
+		r->flags |= IEEE80211_TX_RC_SHORT_GI;
+	r->idx = iwl4965_hwrate_to_mac80211_idx(rate_n_flags, info->band);
+}
+
+/**
+ * iwl4965_rx_reply_compressed_ba - Handler for REPLY_COMPRESSED_BA
+ *
+ * Handles block-acknowledge notification from device, which reports success
+ * of frames sent via aggregation.
+ */
+void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
+					   struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_compressed_ba_resp *ba_resp = &pkt->u.compressed_ba;
+	struct iwl_tx_queue *txq = NULL;
+	struct iwl_ht_agg *agg;
+	int index;
+	int sta_id;
+	int tid;
+	unsigned long flags;
+
+	/* "flow" corresponds to Tx queue */
+	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
+
+	/* "ssn" is start of block-ack Tx window, corresponds to index
+	 * (in Tx queue's circular buffer) of first TFD/frame in window */
+	u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
+
+	if (scd_flow >= priv->hw_params.max_txq_num) {
+		IWL_ERR(priv,
+			"BUG_ON scd_flow is bigger than number of queues\n");
+		return;
+	}
+
+	txq = &priv->txq[scd_flow];
+	sta_id = ba_resp->sta_id;
+	tid = ba_resp->tid;
+	agg = &priv->stations[sta_id].tid[tid].agg;
+	if (unlikely(agg->txq_id != scd_flow)) {
+		/*
+		 * FIXME: this is a uCode bug which need to be addressed,
+		 * log the information and return for now!
+		 * since it is possible happen very often and in order
+		 * not to fill the syslog, don't enable the logging by default
+		 */
+		IWL_DEBUG_TX_REPLY(priv,
+			"BA scd_flow %d does not match txq_id %d\n",
+			scd_flow, agg->txq_id);
+		return;
+	}
+
+	/* Find index just before block-ack window */
+	index = iwl_legacy_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	IWL_DEBUG_TX_REPLY(priv, "REPLY_COMPRESSED_BA [%d] Received from %pM, "
+			   "sta_id = %d\n",
+			   agg->wait_for_ba,
+			   (u8 *) &ba_resp->sta_addr_lo32,
+			   ba_resp->sta_id);
+	IWL_DEBUG_TX_REPLY(priv, "TID = %d, SeqCtl = %d, bitmap = 0x%llx,"
+			"scd_flow = "
+			   "%d, scd_ssn = %d\n",
+			   ba_resp->tid,
+			   ba_resp->seq_ctl,
+			   (unsigned long long)le64_to_cpu(ba_resp->bitmap),
+			   ba_resp->scd_flow,
+			   ba_resp->scd_ssn);
+	IWL_DEBUG_TX_REPLY(priv, "DAT start_idx = %d, bitmap = 0x%llx\n",
+			   agg->start_idx,
+			   (unsigned long long)agg->bitmap);
+
+	/* Update driver's record of ACK vs. not for each frame in window */
+	iwl4965_tx_status_reply_compressed_ba(priv, agg, ba_resp);
+
+	/* Release all TFDs before the SSN, i.e. all TFDs in front of
+	 * block-ack window (we assume that they've been successfully
+	 * transmitted ... if not, it's too late anyway). */
+	if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff)) {
+		/* calculate mac80211 ampdu sw queue to wake */
+		int freed = iwl4965_tx_queue_reclaim(priv, scd_flow, index);
+		iwl4965_free_tfds_in_queue(priv, sta_id, tid, freed);
+
+		if ((iwl_legacy_queue_space(&txq->q) > txq->q.low_mark) &&
+		    priv->mac80211_registered &&
+		    (agg->state != IWL_EMPTYING_HW_QUEUE_DELBA))
+			iwl_legacy_wake_queue(priv, txq);
+
+		iwl4965_txq_check_empty(priv, sta_id, tid, scd_flow);
+	}
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+const char *iwl4965_get_tx_fail_reason(u32 status)
+{
+#define TX_STATUS_FAIL(x) case TX_STATUS_FAIL_ ## x: return #x
+#define TX_STATUS_POSTPONE(x) case TX_STATUS_POSTPONE_ ## x: return #x
+
+	switch (status & TX_STATUS_MSK) {
+	case TX_STATUS_SUCCESS:
+		return "SUCCESS";
+	TX_STATUS_POSTPONE(DELAY);
+	TX_STATUS_POSTPONE(FEW_BYTES);
+	TX_STATUS_POSTPONE(QUIET_PERIOD);
+	TX_STATUS_POSTPONE(CALC_TTAK);
+	TX_STATUS_FAIL(INTERNAL_CROSSED_RETRY);
+	TX_STATUS_FAIL(SHORT_LIMIT);
+	TX_STATUS_FAIL(LONG_LIMIT);
+	TX_STATUS_FAIL(FIFO_UNDERRUN);
+	TX_STATUS_FAIL(DRAIN_FLOW);
+	TX_STATUS_FAIL(RFKILL_FLUSH);
+	TX_STATUS_FAIL(LIFE_EXPIRE);
+	TX_STATUS_FAIL(DEST_PS);
+	TX_STATUS_FAIL(HOST_ABORTED);
+	TX_STATUS_FAIL(BT_RETRY);
+	TX_STATUS_FAIL(STA_INVALID);
+	TX_STATUS_FAIL(FRAG_DROPPED);
+	TX_STATUS_FAIL(TID_DISABLE);
+	TX_STATUS_FAIL(FIFO_FLUSHED);
+	TX_STATUS_FAIL(INSUFFICIENT_CF_POLL);
+	TX_STATUS_FAIL(PASSIVE_NO_RX);
+	TX_STATUS_FAIL(NO_BEACON_ON_RADAR);
+	}
+
+	return "UNKNOWN";
+
+#undef TX_STATUS_FAIL
+#undef TX_STATUS_POSTPONE
+}
+#endif /* CONFIG_IWLWIFI_LEGACY_DEBUG */
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965-ucode.c b/drivers/net/wireless/iwlegacy/iwl-4965-ucode.c
new file mode 100644
index 000000000000..001d148feb94
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965-ucode.c
@@ -0,0 +1,166 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+#include "iwl-4965-hw.h"
+#include "iwl-4965.h"
+#include "iwl-4965-calib.h"
+
+#define IWL_AC_UNSET -1
+
+/**
+ * iwl_verify_inst_sparse - verify runtime uCode image in card vs. host,
+ *   using sample data 100 bytes apart.  If these sample points are good,
+ *   it's a pretty good bet that everything between them is good, too.
+ */
+static int
+iwl4965_verify_inst_sparse(struct iwl_priv *priv, __le32 *image, u32 len)
+{
+	u32 val;
+	int ret = 0;
+	u32 errcnt = 0;
+	u32 i;
+
+	IWL_DEBUG_INFO(priv, "ucode inst image size is %u\n", len);
+
+	for (i = 0; i < len; i += 100, image += 100/sizeof(u32)) {
+		/* read data comes through single port, auto-incr addr */
+		/* NOTE: Use the debugless read so we don't flood kernel log
+		 * if IWL_DL_IO is set */
+		iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR,
+			i + IWL4965_RTC_INST_LOWER_BOUND);
+		val = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+			ret = -EIO;
+			errcnt++;
+			if (errcnt >= 3)
+				break;
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * iwl4965_verify_inst_full - verify runtime uCode image in card vs. host,
+ *     looking at all data.
+ */
+static int iwl4965_verify_inst_full(struct iwl_priv *priv, __le32 *image,
+				 u32 len)
+{
+	u32 val;
+	u32 save_len = len;
+	int ret = 0;
+	u32 errcnt;
+
+	IWL_DEBUG_INFO(priv, "ucode inst image size is %u\n", len);
+
+	iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR,
+			   IWL4965_RTC_INST_LOWER_BOUND);
+
+	errcnt = 0;
+	for (; len > 0; len -= sizeof(u32), image++) {
+		/* read data comes through single port, auto-incr addr */
+		/* NOTE: Use the debugless read so we don't flood kernel log
+		 * if IWL_DL_IO is set */
+		val = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+			IWL_ERR(priv, "uCode INST section is invalid at "
+				  "offset 0x%x, is 0x%x, s/b 0x%x\n",
+				  save_len - len, val, le32_to_cpu(*image));
+			ret = -EIO;
+			errcnt++;
+			if (errcnt >= 20)
+				break;
+		}
+	}
+
+	if (!errcnt)
+		IWL_DEBUG_INFO(priv,
+		    "ucode image in INSTRUCTION memory is good\n");
+
+	return ret;
+}
+
+/**
+ * iwl4965_verify_ucode - determine which instruction image is in SRAM,
+ *    and verify its contents
+ */
+int iwl4965_verify_ucode(struct iwl_priv *priv)
+{
+	__le32 *image;
+	u32 len;
+	int ret;
+
+	/* Try bootstrap */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	ret = iwl4965_verify_inst_sparse(priv, image, len);
+	if (!ret) {
+		IWL_DEBUG_INFO(priv, "Bootstrap uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	/* Try initialize */
+	image = (__le32 *)priv->ucode_init.v_addr;
+	len = priv->ucode_init.len;
+	ret = iwl4965_verify_inst_sparse(priv, image, len);
+	if (!ret) {
+		IWL_DEBUG_INFO(priv, "Initialize uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	/* Try runtime/protocol */
+	image = (__le32 *)priv->ucode_code.v_addr;
+	len = priv->ucode_code.len;
+	ret = iwl4965_verify_inst_sparse(priv, image, len);
+	if (!ret) {
+		IWL_DEBUG_INFO(priv, "Runtime uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	IWL_ERR(priv, "NO VALID UCODE IMAGE IN INSTRUCTION SRAM!!\n");
+
+	/* Since nothing seems to match, show first several data entries in
+	 * instruction SRAM, so maybe visual inspection will give a clue.
+	 * Selection of bootstrap image (vs. other images) is arbitrary. */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	ret = iwl4965_verify_inst_full(priv, image, len);
+
+	return ret;
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlegacy/iwl-4965.c
index 91a9f5253469..f5433c74b845 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlegacy/iwl-4965.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2003 - 2011 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
@@ -43,12 +43,11 @@
 #include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-helpers.h"
-#include "iwl-agn-calib.h"
+#include "iwl-4965-calib.h"
 #include "iwl-sta.h"
-#include "iwl-agn-led.h"
-#include "iwl-agn.h"
-#include "iwl-agn-debugfs.h"
-#include "iwl-legacy.h"
+#include "iwl-4965-led.h"
+#include "iwl-4965.h"
+#include "iwl-4965-debugfs.h"
 
 static int iwl4965_send_tx_power(struct iwl_priv *priv);
 static int iwl4965_hw_get_temperature(struct iwl_priv *priv);
@@ -74,11 +73,11 @@ static int iwl4965_verify_bsm(struct iwl_priv *priv)
 	IWL_DEBUG_INFO(priv, "Begin verify bsm\n");
 
 	/* verify BSM SRAM contents */
-	val = iwl_read_prph(priv, BSM_WR_DWCOUNT_REG);
+	val = iwl_legacy_read_prph(priv, BSM_WR_DWCOUNT_REG);
 	for (reg = BSM_SRAM_LOWER_BOUND;
 	     reg < BSM_SRAM_LOWER_BOUND + len;
 	     reg += sizeof(u32), image++) {
-		val = iwl_read_prph(priv, reg);
+		val = iwl_legacy_read_prph(priv, reg);
 		if (val != le32_to_cpu(*image)) {
 			IWL_ERR(priv, "BSM uCode verification failed at "
 				  "addr 0x%08X+%u (of %u), is 0x%x, s/b 0x%x\n",
@@ -158,33 +157,34 @@ static int iwl4965_load_bsm(struct iwl_priv *priv)
 	inst_len = priv->ucode_init.len;
 	data_len = priv->ucode_init_data.len;
 
-	iwl_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
-	iwl_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
-	iwl_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG, inst_len);
-	iwl_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG, data_len);
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG, inst_len);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG, data_len);
 
 	/* Fill BSM memory with bootstrap instructions */
 	for (reg_offset = BSM_SRAM_LOWER_BOUND;
 	     reg_offset < BSM_SRAM_LOWER_BOUND + len;
 	     reg_offset += sizeof(u32), image++)
-		_iwl_write_prph(priv, reg_offset, le32_to_cpu(*image));
+		_iwl_legacy_write_prph(priv, reg_offset, le32_to_cpu(*image));
 
 	ret = iwl4965_verify_bsm(priv);
 	if (ret)
 		return ret;
 
 	/* Tell BSM to copy from BSM SRAM into instruction SRAM, when asked */
-	iwl_write_prph(priv, BSM_WR_MEM_SRC_REG, 0x0);
-	iwl_write_prph(priv, BSM_WR_MEM_DST_REG, IWL49_RTC_INST_LOWER_BOUND);
-	iwl_write_prph(priv, BSM_WR_DWCOUNT_REG, len / sizeof(u32));
+	iwl_legacy_write_prph(priv, BSM_WR_MEM_SRC_REG, 0x0);
+	iwl_legacy_write_prph(priv,
+			BSM_WR_MEM_DST_REG, IWL49_RTC_INST_LOWER_BOUND);
+	iwl_legacy_write_prph(priv, BSM_WR_DWCOUNT_REG, len / sizeof(u32));
 
 	/* Load bootstrap code into instruction SRAM now,
 	 *   to prepare to load "initialize" uCode */
-	iwl_write_prph(priv, BSM_WR_CTRL_REG, BSM_WR_CTRL_REG_BIT_START);
+	iwl_legacy_write_prph(priv, BSM_WR_CTRL_REG, BSM_WR_CTRL_REG_BIT_START);
 
 	/* Wait for load of bootstrap uCode to finish */
 	for (i = 0; i < 100; i++) {
-		done = iwl_read_prph(priv, BSM_WR_CTRL_REG);
+		done = iwl_legacy_read_prph(priv, BSM_WR_CTRL_REG);
 		if (!(done & BSM_WR_CTRL_REG_BIT_START))
 			break;
 		udelay(10);
@@ -198,7 +198,8 @@ static int iwl4965_load_bsm(struct iwl_priv *priv)
 
 	/* Enable future boot loads whenever power management unit triggers it
 	 *   (e.g. when powering back up after power-save shutdown) */
-	iwl_write_prph(priv, BSM_WR_CTRL_REG, BSM_WR_CTRL_REG_BIT_START_EN);
+	iwl_legacy_write_prph(priv,
+			BSM_WR_CTRL_REG, BSM_WR_CTRL_REG_BIT_START_EN);
 
 
 	return 0;
@@ -224,14 +225,14 @@ static int iwl4965_set_ucode_ptrs(struct iwl_priv *priv)
 	pdata = priv->ucode_data_backup.p_addr >> 4;
 
 	/* Tell bootstrap uCode where to find image to load */
-	iwl_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
-	iwl_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
-	iwl_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
 				 priv->ucode_data.len);
 
 	/* Inst byte count must be last to set up, bit 31 signals uCode
 	 *   that all new ptr/size info is in place */
-	iwl_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG,
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG,
 				 priv->ucode_code.len | BSM_DRAM_INST_LOAD);
 	IWL_DEBUG_INFO(priv, "Runtime uCode pointers are set.\n");
 
@@ -251,18 +252,10 @@ static int iwl4965_set_ucode_ptrs(struct iwl_priv *priv)
 */
 static void iwl4965_init_alive_start(struct iwl_priv *priv)
 {
-	/* Check alive response for "valid" sign from uCode */
-	if (priv->card_alive_init.is_valid != UCODE_VALID_OK) {
-		/* We had an error bringing up the hardware, so take it
-		 * all the way back down so we can try again */
-		IWL_DEBUG_INFO(priv, "Initialize Alive failed.\n");
-		goto restart;
-	}
-
 	/* Bootstrap uCode has loaded initialize uCode ... verify inst image.
 	 * This is a paranoid check, because we would not have gotten the
 	 * "initialize" alive if code weren't properly loaded.  */
-	if (iwl_verify_ucode(priv)) {
+	if (iwl4965_verify_ucode(priv)) {
 		/* Runtime instruction load was bad;
 		 * take it all the way back down so we can try again */
 		IWL_DEBUG_INFO(priv, "Bad \"initialize\" uCode load.\n");
@@ -288,7 +281,7 @@ restart:
 	queue_work(priv->workqueue, &priv->restart);
 }
 
-static bool is_ht40_channel(__le32 rxon_flags)
+static bool iw4965_is_ht40_channel(__le32 rxon_flags)
 {
 	int chan_mod = le32_to_cpu(rxon_flags & RXON_FLG_CHANNEL_MODE_MSK)
 				    >> RXON_FLG_CHANNEL_MODE_POS;
@@ -296,23 +289,6 @@ static bool is_ht40_channel(__le32 rxon_flags)
 		  (chan_mod == CHANNEL_MODE_MIXED));
 }
 
-/*
- * EEPROM handlers
- */
-static u16 iwl4965_eeprom_calib_version(struct iwl_priv *priv)
-{
-	return iwl_eeprom_query16(priv, EEPROM_4965_CALIB_VERSION_OFFSET);
-}
-
-/*
- * Activate/Deactivate Tx DMA/FIFO channels according tx fifos mask
- * must be called under priv->lock and mac access
- */
-static void iwl4965_txq_set_sched(struct iwl_priv *priv, u32 mask)
-{
-	iwl_write_prph(priv, IWL49_SCD_TXFACT, mask);
-}
-
 static void iwl4965_nic_config(struct iwl_priv *priv)
 {
 	unsigned long flags;
@@ -320,22 +296,23 @@ static void iwl4965_nic_config(struct iwl_priv *priv)
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
+	radio_cfg = iwl_legacy_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
 
 	/* write radio config values to register */
 	if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) == EEPROM_4965_RF_CFG_TYPE_MAX)
-		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 			    EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
 			    EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
 			    EEPROM_RF_CFG_DASH_MSK(radio_cfg));
 
 	/* set CSR_HW_CONFIG_REG for uCode use */
-	iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+	iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
 		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
 		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
 
 	priv->calib_info = (struct iwl_eeprom_calib_info *)
-		iwl_eeprom_query_addr(priv, EEPROM_4965_CALIB_TXPOWER_OFFSET);
+		iwl_legacy_eeprom_query_addr(priv,
+				EEPROM_4965_CALIB_TXPOWER_OFFSET);
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 }
@@ -348,7 +325,7 @@ static void iwl4965_chain_noise_reset(struct iwl_priv *priv)
 	struct iwl_chain_noise_data *data = &(priv->chain_noise_data);
 
 	if ((data->state == IWL_CHAIN_NOISE_ALIVE) &&
-	    iwl_is_any_associated(priv)) {
+	    iwl_legacy_is_any_associated(priv)) {
 		struct iwl_calib_diff_gain_cmd cmd;
 
 		/* clear data for chain noise calibration algorithm */
@@ -365,7 +342,7 @@ static void iwl4965_chain_noise_reset(struct iwl_priv *priv)
 		cmd.diff_gain_a = 0;
 		cmd.diff_gain_b = 0;
 		cmd.diff_gain_c = 0;
-		if (iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+		if (iwl_legacy_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
 				 sizeof(cmd), &cmd))
 			IWL_ERR(priv,
 				"Could not send REPLY_PHY_CALIBRATION_CMD\n");
@@ -374,237 +351,6 @@ static void iwl4965_chain_noise_reset(struct iwl_priv *priv)
 	}
 }
 
-static void iwl4965_gain_computation(struct iwl_priv *priv,
-		u32 *average_noise,
-		u16 min_average_noise_antenna_i,
-		u32 min_average_noise,
-		u8 default_chain)
-{
-	int i, ret;
-	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
-
-	data->delta_gain_code[min_average_noise_antenna_i] = 0;
-
-	for (i = default_chain; i < NUM_RX_CHAINS; i++) {
-		s32 delta_g = 0;
-
-		if (!(data->disconn_array[i]) &&
-		    (data->delta_gain_code[i] ==
-			     CHAIN_NOISE_DELTA_GAIN_INIT_VAL)) {
-			delta_g = average_noise[i] - min_average_noise;
-			data->delta_gain_code[i] = (u8)((delta_g * 10) / 15);
-			data->delta_gain_code[i] =
-				min(data->delta_gain_code[i],
-				(u8) CHAIN_NOISE_MAX_DELTA_GAIN_CODE);
-
-			data->delta_gain_code[i] =
-				(data->delta_gain_code[i] | (1 << 2));
-		} else {
-			data->delta_gain_code[i] = 0;
-		}
-	}
-	IWL_DEBUG_CALIB(priv, "delta_gain_codes: a %d b %d c %d\n",
-		     data->delta_gain_code[0],
-		     data->delta_gain_code[1],
-		     data->delta_gain_code[2]);
-
-	/* Differential gain gets sent to uCode only once */
-	if (!data->radio_write) {
-		struct iwl_calib_diff_gain_cmd cmd;
-		data->radio_write = 1;
-
-		memset(&cmd, 0, sizeof(cmd));
-		cmd.hdr.op_code = IWL_PHY_CALIBRATE_DIFF_GAIN_CMD;
-		cmd.diff_gain_a = data->delta_gain_code[0];
-		cmd.diff_gain_b = data->delta_gain_code[1];
-		cmd.diff_gain_c = data->delta_gain_code[2];
-		ret = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
-				      sizeof(cmd), &cmd);
-		if (ret)
-			IWL_DEBUG_CALIB(priv, "fail sending cmd "
-				     "REPLY_PHY_CALIBRATION_CMD\n");
-
-		/* TODO we might want recalculate
-		 * rx_chain in rxon cmd */
-
-		/* Mark so we run this algo only once! */
-		data->state = IWL_CHAIN_NOISE_CALIBRATED;
-	}
-}
-
-static void iwl4965_bg_txpower_work(struct work_struct *work)
-{
-	struct iwl_priv *priv = container_of(work, struct iwl_priv,
-			txpower_work);
-
-	/* If a scan happened to start before we got here
-	 * then just return; the statistics notification will
-	 * kick off another scheduled work to compensate for
-	 * any temperature delta we missed here. */
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
-	    test_bit(STATUS_SCANNING, &priv->status))
-		return;
-
-	mutex_lock(&priv->mutex);
-
-	/* Regardless of if we are associated, we must reconfigure the
-	 * TX power since frames can be sent on non-radar channels while
-	 * not associated */
-	iwl4965_send_tx_power(priv);
-
-	/* Update last_temperature to keep is_calib_needed from running
-	 * when it isn't needed... */
-	priv->last_temperature = priv->temperature;
-
-	mutex_unlock(&priv->mutex);
-}
-
-/*
- * Acquire priv->lock before calling this function !
- */
-static void iwl4965_set_wr_ptrs(struct iwl_priv *priv, int txq_id, u32 index)
-{
-	iwl_write_direct32(priv, HBUS_TARG_WRPTR,
-			     (index & 0xff) | (txq_id << 8));
-	iwl_write_prph(priv, IWL49_SCD_QUEUE_RDPTR(txq_id), index);
-}
-
-/**
- * iwl4965_tx_queue_set_status - (optionally) start Tx/Cmd queue
- * @tx_fifo_id: Tx DMA/FIFO channel (range 0-7) that the queue will feed
- * @scd_retry: (1) Indicates queue will be used in aggregation mode
- *
- * NOTE:  Acquire priv->lock before calling this function !
- */
-static void iwl4965_tx_queue_set_status(struct iwl_priv *priv,
-					struct iwl_tx_queue *txq,
-					int tx_fifo_id, int scd_retry)
-{
-	int txq_id = txq->q.id;
-
-	/* Find out whether to activate Tx queue */
-	int active = test_bit(txq_id, &priv->txq_ctx_active_msk) ? 1 : 0;
-
-	/* Set up and activate */
-	iwl_write_prph(priv, IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
-			 (active << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE) |
-			 (tx_fifo_id << IWL49_SCD_QUEUE_STTS_REG_POS_TXF) |
-			 (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_WSL) |
-			 (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
-			 IWL49_SCD_QUEUE_STTS_REG_MSK);
-
-	txq->sched_retry = scd_retry;
-
-	IWL_DEBUG_INFO(priv, "%s %s Queue %d on AC %d\n",
-		       active ? "Activate" : "Deactivate",
-		       scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
-}
-
-static const s8 default_queue_to_tx_fifo[] = {
-	IWL_TX_FIFO_VO,
-	IWL_TX_FIFO_VI,
-	IWL_TX_FIFO_BE,
-	IWL_TX_FIFO_BK,
-	IWL49_CMD_FIFO_NUM,
-	IWL_TX_FIFO_UNUSED,
-	IWL_TX_FIFO_UNUSED,
-};
-
-static int iwl4965_alive_notify(struct iwl_priv *priv)
-{
-	u32 a;
-	unsigned long flags;
-	int i, chan;
-	u32 reg_val;
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	/* Clear 4965's internal Tx Scheduler data base */
-	priv->scd_base_addr = iwl_read_prph(priv, IWL49_SCD_SRAM_BASE_ADDR);
-	a = priv->scd_base_addr + IWL49_SCD_CONTEXT_DATA_OFFSET;
-	for (; a < priv->scd_base_addr + IWL49_SCD_TX_STTS_BITMAP_OFFSET; a += 4)
-		iwl_write_targ_mem(priv, a, 0);
-	for (; a < priv->scd_base_addr + IWL49_SCD_TRANSLATE_TBL_OFFSET; a += 4)
-		iwl_write_targ_mem(priv, a, 0);
-	for (; a < priv->scd_base_addr +
-	       IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(priv->hw_params.max_txq_num); a += 4)
-		iwl_write_targ_mem(priv, a, 0);
-
-	/* Tel 4965 where to find Tx byte count tables */
-	iwl_write_prph(priv, IWL49_SCD_DRAM_BASE_ADDR,
-			priv->scd_bc_tbls.dma >> 10);
-
-	/* Enable DMA channel */
-	for (chan = 0; chan < FH49_TCSR_CHNL_NUM ; chan++)
-		iwl_write_direct32(priv, FH_TCSR_CHNL_TX_CONFIG_REG(chan),
-				FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
-				FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE);
-
-	/* Update FH chicken bits */
-	reg_val = iwl_read_direct32(priv, FH_TX_CHICKEN_BITS_REG);
-	iwl_write_direct32(priv, FH_TX_CHICKEN_BITS_REG,
-			   reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
-
-	/* Disable chain mode for all queues */
-	iwl_write_prph(priv, IWL49_SCD_QUEUECHAIN_SEL, 0);
-
-	/* Initialize each Tx queue (including the command queue) */
-	for (i = 0; i < priv->hw_params.max_txq_num; i++) {
-
-		/* TFD circular buffer read/write indexes */
-		iwl_write_prph(priv, IWL49_SCD_QUEUE_RDPTR(i), 0);
-		iwl_write_direct32(priv, HBUS_TARG_WRPTR, 0 | (i << 8));
-
-		/* Max Tx Window size for Scheduler-ACK mode */
-		iwl_write_targ_mem(priv, priv->scd_base_addr +
-				IWL49_SCD_CONTEXT_QUEUE_OFFSET(i),
-				(SCD_WIN_SIZE <<
-				IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
-				IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
-
-		/* Frame limit */
-		iwl_write_targ_mem(priv, priv->scd_base_addr +
-				IWL49_SCD_CONTEXT_QUEUE_OFFSET(i) +
-				sizeof(u32),
-				(SCD_FRAME_LIMIT <<
-				IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
-				IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
-
-	}
-	iwl_write_prph(priv, IWL49_SCD_INTERRUPT_MASK,
-				 (1 << priv->hw_params.max_txq_num) - 1);
-
-	/* Activate all Tx DMA/FIFO channels */
-	priv->cfg->ops->lib->txq_set_sched(priv, IWL_MASK(0, 6));
-
-	iwl4965_set_wr_ptrs(priv, IWL_DEFAULT_CMD_QUEUE_NUM, 0);
-
-	/* make sure all queue are not stopped */
-	memset(&priv->queue_stopped[0], 0, sizeof(priv->queue_stopped));
-	for (i = 0; i < 4; i++)
-		atomic_set(&priv->queue_stop_count[i], 0);
-
-	/* reset to 0 to enable all the queue first */
-	priv->txq_ctx_active_msk = 0;
-	/* Map each Tx/cmd queue to its corresponding fifo */
-	BUILD_BUG_ON(ARRAY_SIZE(default_queue_to_tx_fifo) != 7);
-
-	for (i = 0; i < ARRAY_SIZE(default_queue_to_tx_fifo); i++) {
-		int ac = default_queue_to_tx_fifo[i];
-
-		iwl_txq_ctx_activate(priv, i);
-
-		if (ac == IWL_TX_FIFO_UNUSED)
-			continue;
-
-		iwl4965_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
-	}
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
-}
-
 static struct iwl_sensitivity_ranges iwl4965_sensitivity = {
 	.min_nrg_cck = 97,
 	.max_nrg_cck = 0, /* not used, set to 0 */
@@ -666,15 +412,15 @@ static int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
 
 	priv->hw_params.rx_wrt_ptr_reg = FH_RSCSR_CHNL0_WPTR;
 
-	priv->hw_params.tx_chains_num = num_of_ant(priv->cfg->valid_tx_ant);
-	priv->hw_params.rx_chains_num = num_of_ant(priv->cfg->valid_rx_ant);
+	priv->hw_params.tx_chains_num = iwl4965_num_of_ant(priv->cfg->valid_tx_ant);
+	priv->hw_params.rx_chains_num = iwl4965_num_of_ant(priv->cfg->valid_rx_ant);
 	priv->hw_params.valid_tx_ant = priv->cfg->valid_tx_ant;
 	priv->hw_params.valid_rx_ant = priv->cfg->valid_rx_ant;
 
 	iwl4965_set_ct_threshold(priv);
 
 	priv->hw_params.sens = &iwl4965_sensitivity;
-	priv->hw_params.beacon_time_tsf_bits = IWLAGN_EXT_BEACON_TIME_POS;
+	priv->hw_params.beacon_time_tsf_bits = IWL4965_EXT_BEACON_TIME_POS;
 
 	return 0;
 }
@@ -1158,9 +904,9 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 	IWL_DEBUG_TXPOWER(priv, "chan %d band %d is_ht40 %d\n", channel, band,
 			  is_ht40);
 
-	ch_info = iwl_get_channel_info(priv, priv->band, channel);
+	ch_info = iwl_legacy_get_channel_info(priv, priv->band, channel);
 
-	if (!is_channel_valid(ch_info))
+	if (!iwl_legacy_is_channel_valid(ch_info))
 		return -EINVAL;
 
 	/* get txatten group, used to select 1) thermal txpower adjustment
@@ -1384,7 +1130,7 @@ static int iwl4965_send_tx_power(struct iwl_priv *priv)
 
 	band = priv->band == IEEE80211_BAND_2GHZ;
 
-	is_ht40 = is_ht40_channel(ctx->active.flags);
+	is_ht40 = iw4965_is_ht40_channel(ctx->active.flags);
 
 	if (is_ht40 && (ctx->active.flags & RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK))
 		ctrl_chan_high = 1;
@@ -1398,7 +1144,8 @@ static int iwl4965_send_tx_power(struct iwl_priv *priv)
 	if (ret)
 		goto out;
 
-	ret = iwl_send_cmd_pdu(priv, REPLY_TX_PWR_TABLE_CMD, sizeof(cmd), &cmd);
+	ret = iwl_legacy_send_cmd_pdu(priv,
+			 REPLY_TX_PWR_TABLE_CMD, sizeof(cmd), &cmd);
 
 out:
 	return ret;
@@ -1409,8 +1156,8 @@ static int iwl4965_send_rxon_assoc(struct iwl_priv *priv,
 {
 	int ret = 0;
 	struct iwl4965_rxon_assoc_cmd rxon_assoc;
-	const struct iwl_rxon_cmd *rxon1 = &ctx->staging;
-	const struct iwl_rxon_cmd *rxon2 = &ctx->active;
+	const struct iwl_legacy_rxon_cmd *rxon1 = &ctx->staging;
+	const struct iwl_legacy_rxon_cmd *rxon2 = &ctx->active;
 
 	if ((rxon1->flags == rxon2->flags) &&
 	    (rxon1->filter_flags == rxon2->filter_flags) &&
@@ -1436,7 +1183,7 @@ static int iwl4965_send_rxon_assoc(struct iwl_priv *priv,
 	    ctx->staging.ofdm_ht_dual_stream_basic_rates;
 	rxon_assoc.rx_chain_select_flags = ctx->staging.rx_chain;
 
-	ret = iwl_send_cmd_pdu_async(priv, REPLY_RXON_ASSOC,
+	ret = iwl_legacy_send_cmd_pdu_async(priv, REPLY_RXON_ASSOC,
 				     sizeof(rxon_assoc), &rxon_assoc, NULL);
 	if (ret)
 		return ret;
@@ -1447,12 +1194,12 @@ static int iwl4965_send_rxon_assoc(struct iwl_priv *priv,
 static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
 	/* cast away the const for active_rxon in this function */
-	struct iwl_rxon_cmd *active_rxon = (void *)&ctx->active;
+	struct iwl_legacy_rxon_cmd *active_rxon = (void *)&ctx->active;
 	int ret;
 	bool new_assoc =
 		!!(ctx->staging.filter_flags & RXON_FILTER_ASSOC_MSK);
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EBUSY;
 
 	if (!ctx->is_active)
@@ -1461,7 +1208,7 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *c
 	/* always get timestamp with Rx frame */
 	ctx->staging.flags |= RXON_FLG_TSF2HOST_MSK;
 
-	ret = iwl_check_rxon_cmd(priv, ctx);
+	ret = iwl_legacy_check_rxon_cmd(priv, ctx);
 	if (ret) {
 		IWL_ERR(priv, "Invalid RXON configuration.  Not committing.\n");
 		return -EINVAL;
@@ -1475,21 +1222,21 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *c
 	    (priv->switch_rxon.channel != ctx->staging.channel)) {
 		IWL_DEBUG_11H(priv, "abort channel switch on %d\n",
 		      le16_to_cpu(priv->switch_rxon.channel));
-		iwl_chswitch_done(priv, false);
+		iwl_legacy_chswitch_done(priv, false);
 	}
 
 	/* If we don't need to send a full RXON, we can use
 	 * iwl_rxon_assoc_cmd which is used to reconfigure filter
 	 * and other flags for the current radio configuration. */
-	if (!iwl_full_rxon_required(priv, ctx)) {
-		ret = iwl_send_rxon_assoc(priv, ctx);
+	if (!iwl_legacy_full_rxon_required(priv, ctx)) {
+		ret = iwl_legacy_send_rxon_assoc(priv, ctx);
 		if (ret) {
 			IWL_ERR(priv, "Error setting RXON_ASSOC (%d)\n", ret);
 			return ret;
 		}
 
 		memcpy(active_rxon, &ctx->staging, sizeof(*active_rxon));
-		iwl_print_rx_config_cmd(priv, ctx);
+		iwl_legacy_print_rx_config_cmd(priv, ctx);
 		return 0;
 	}
 
@@ -1497,12 +1244,12 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *c
 	 * an RXON_ASSOC and the new config wants the associated mask enabled,
 	 * we must clear the associated from the active configuration
 	 * before we apply the new config */
-	if (iwl_is_associated_ctx(ctx) && new_assoc) {
+	if (iwl_legacy_is_associated_ctx(ctx) && new_assoc) {
 		IWL_DEBUG_INFO(priv, "Toggling associated bit on current RXON\n");
 		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 
-		ret = iwl_send_cmd_pdu(priv, ctx->rxon_cmd,
-				       sizeof(struct iwl_rxon_cmd),
+		ret = iwl_legacy_send_cmd_pdu(priv, ctx->rxon_cmd,
+				       sizeof(struct iwl_legacy_rxon_cmd),
 				       active_rxon);
 
 		/* If the mask clearing failed then we set
@@ -1512,9 +1259,9 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *c
 			IWL_ERR(priv, "Error clearing ASSOC_MSK (%d)\n", ret);
 			return ret;
 		}
-		iwl_clear_ucode_stations(priv, ctx);
-		iwl_restore_stations(priv, ctx);
-		ret = iwl_restore_default_wep_keys(priv, ctx);
+		iwl_legacy_clear_ucode_stations(priv, ctx);
+		iwl_legacy_restore_stations(priv, ctx);
+		ret = iwl4965_restore_default_wep_keys(priv, ctx);
 		if (ret) {
 			IWL_ERR(priv, "Failed to restore WEP keys (%d)\n", ret);
 			return ret;
@@ -1529,24 +1276,25 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *c
 		       le16_to_cpu(ctx->staging.channel),
 		       ctx->staging.bssid_addr);
 
-	iwl_set_rxon_hwcrypto(priv, ctx, !priv->cfg->mod_params->sw_crypto);
+	iwl_legacy_set_rxon_hwcrypto(priv, ctx,
+				!priv->cfg->mod_params->sw_crypto);
 
 	/* Apply the new configuration
 	 * RXON unassoc clears the station table in uCode so restoration of
 	 * stations is needed after it (the RXON command) completes
 	 */
 	if (!new_assoc) {
-		ret = iwl_send_cmd_pdu(priv, ctx->rxon_cmd,
-			      sizeof(struct iwl_rxon_cmd), &ctx->staging);
+		ret = iwl_legacy_send_cmd_pdu(priv, ctx->rxon_cmd,
+			      sizeof(struct iwl_legacy_rxon_cmd), &ctx->staging);
 		if (ret) {
 			IWL_ERR(priv, "Error setting new RXON (%d)\n", ret);
 			return ret;
 		}
 		IWL_DEBUG_INFO(priv, "Return from !new_assoc RXON.\n");
 		memcpy(active_rxon, &ctx->staging, sizeof(*active_rxon));
-		iwl_clear_ucode_stations(priv, ctx);
-		iwl_restore_stations(priv, ctx);
-		ret = iwl_restore_default_wep_keys(priv, ctx);
+		iwl_legacy_clear_ucode_stations(priv, ctx);
+		iwl_legacy_restore_stations(priv, ctx);
+		ret = iwl4965_restore_default_wep_keys(priv, ctx);
 		if (ret) {
 			IWL_ERR(priv, "Failed to restore WEP keys (%d)\n", ret);
 			return ret;
@@ -1557,21 +1305,21 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *c
 		/* Apply the new configuration
 		 * RXON assoc doesn't clear the station table in uCode,
 		 */
-		ret = iwl_send_cmd_pdu(priv, ctx->rxon_cmd,
-			      sizeof(struct iwl_rxon_cmd), &ctx->staging);
+		ret = iwl_legacy_send_cmd_pdu(priv, ctx->rxon_cmd,
+			      sizeof(struct iwl_legacy_rxon_cmd), &ctx->staging);
 		if (ret) {
 			IWL_ERR(priv, "Error setting new RXON (%d)\n", ret);
 			return ret;
 		}
 		memcpy(active_rxon, &ctx->staging, sizeof(*active_rxon));
 	}
-	iwl_print_rx_config_cmd(priv, ctx);
+	iwl_legacy_print_rx_config_cmd(priv, ctx);
 
-	iwl_init_sensitivity(priv);
+	iwl4965_init_sensitivity(priv);
 
 	/* If we issue a new RXON command which required a tune then we must
 	 * send a new TXPOWER command or we won't be able to Tx any frames */
-	ret = iwl_set_tx_power(priv, priv->tx_power_user_lmt, true);
+	ret = iwl_legacy_set_tx_power(priv, priv->tx_power_next, true);
 	if (ret) {
 		IWL_ERR(priv, "Error sending TX power (%d)\n", ret);
 		return ret;
@@ -1598,7 +1346,7 @@ static int iwl4965_hw_channel_switch(struct iwl_priv *priv,
 	struct ieee80211_vif *vif = ctx->vif;
 	band = priv->band == IEEE80211_BAND_2GHZ;
 
-	is_ht40 = is_ht40_channel(ctx->staging.flags);
+	is_ht40 = iw4965_is_ht40_channel(ctx->staging.flags);
 
 	if (is_ht40 &&
 	    (ctx->staging.flags & RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK))
@@ -1629,19 +1377,19 @@ static int iwl4965_hw_channel_switch(struct iwl_priv *priv,
 	else {
 		switch_time_in_usec =
 			vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
-		ucode_switch_time = iwl_usecs_to_beacons(priv,
+		ucode_switch_time = iwl_legacy_usecs_to_beacons(priv,
 							 switch_time_in_usec,
 							 beacon_interval);
-		cmd.switch_time = iwl_add_beacon_time(priv,
+		cmd.switch_time = iwl_legacy_add_beacon_time(priv,
 						      priv->ucode_beacon_time,
 						      ucode_switch_time,
 						      beacon_interval);
 	}
 	IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
 		      cmd.switch_time);
-	ch_info = iwl_get_channel_info(priv, priv->band, ch);
+	ch_info = iwl_legacy_get_channel_info(priv, priv->band, ch);
 	if (ch_info)
-		cmd.expect_beacon = is_channel_radar(ch_info);
+		cmd.expect_beacon = iwl_legacy_is_channel_radar(ch_info);
 	else {
 		IWL_ERR(priv, "invalid channel switch from %u to %u\n",
 			ctx->active.channel, ch);
@@ -1658,7 +1406,8 @@ static int iwl4965_hw_channel_switch(struct iwl_priv *priv,
 	priv->switch_rxon.channel = cmd.channel;
 	priv->switch_rxon.switch_in_progress = true;
 
-	return iwl_send_cmd_pdu(priv, REPLY_CHANNEL_SWITCH, sizeof(cmd), &cmd);
+	return iwl_legacy_send_cmd_pdu(priv,
+			 REPLY_CHANNEL_SWITCH, sizeof(cmd), &cmd);
 }
 
 /**
@@ -1700,7 +1449,7 @@ static int iwl4965_hw_get_temperature(struct iwl_priv *priv)
 	u32 R4;
 
 	if (test_bit(STATUS_TEMPERATURE, &priv->status) &&
-	    (priv->_agn.statistics.flag &
+	    (priv->_4965.statistics.flag &
 			STATISTICS_REPLY_FLG_HT40_MODE_MSK)) {
 		IWL_DEBUG_TEMP(priv, "Running HT40 temperature calibration\n");
 		R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[1]);
@@ -1725,7 +1474,7 @@ static int iwl4965_hw_get_temperature(struct iwl_priv *priv)
 	if (!test_bit(STATUS_TEMPERATURE, &priv->status))
 		vt = sign_extend32(R4, 23);
 	else
-		vt = sign_extend32(le32_to_cpu(priv->_agn.statistics.
+		vt = sign_extend32(le32_to_cpu(priv->_4965.statistics.
 				 general.common.temperature), 23);
 
 	IWL_DEBUG_TEMP(priv, "Calib values R[1-3]: %d %d %d R4: %d\n", R1, R2, R3, vt);
@@ -1810,7 +1559,6 @@ static void iwl4965_temperature_calib(struct iwl_priv *priv)
 	}
 
 	priv->temperature = temp;
-	iwl_tt_handler(priv);
 	set_bit(STATUS_TEMPERATURE, &priv->status);
 
 	if (!priv->disable_tx_power_cal &&
@@ -1819,152 +1567,6 @@ static void iwl4965_temperature_calib(struct iwl_priv *priv)
 		queue_work(priv->workqueue, &priv->txpower_work);
 }
 
-/**
- * iwl4965_tx_queue_stop_scheduler - Stop queue, but keep configuration
- */
-static void iwl4965_tx_queue_stop_scheduler(struct iwl_priv *priv,
-					    u16 txq_id)
-{
-	/* Simply stop the queue, but don't change any configuration;
-	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
-	iwl_write_prph(priv,
-		IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
-		(0 << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE)|
-		(1 << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
-}
-
-/**
- * txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE
- * priv->lock must be held by the caller
- */
-static int iwl4965_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
-				   u16 ssn_idx, u8 tx_fifo)
-{
-	if ((IWL49_FIRST_AMPDU_QUEUE > txq_id) ||
-	    (IWL49_FIRST_AMPDU_QUEUE +
-		priv->cfg->base_params->num_of_ampdu_queues <= txq_id)) {
-		IWL_WARN(priv,
-			"queue number out of range: %d, must be %d to %d\n",
-			txq_id, IWL49_FIRST_AMPDU_QUEUE,
-			IWL49_FIRST_AMPDU_QUEUE +
-			priv->cfg->base_params->num_of_ampdu_queues - 1);
-		return -EINVAL;
-	}
-
-	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
-
-	iwl_clear_bits_prph(priv, IWL49_SCD_QUEUECHAIN_SEL, (1 << txq_id));
-
-	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
-	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
-	/* supposes that ssn_idx is valid (!= 0xFFF) */
-	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
-
-	iwl_clear_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
-	iwl_txq_ctx_deactivate(priv, txq_id);
-	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
-
-	return 0;
-}
-
-/**
- * iwl4965_tx_queue_set_q2ratid - Map unique receiver/tid combination to a queue
- */
-static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
-					u16 txq_id)
-{
-	u32 tbl_dw_addr;
-	u32 tbl_dw;
-	u16 scd_q2ratid;
-
-	scd_q2ratid = ra_tid & IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
-
-	tbl_dw_addr = priv->scd_base_addr +
-			IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
-
-	tbl_dw = iwl_read_targ_mem(priv, tbl_dw_addr);
-
-	if (txq_id & 0x1)
-		tbl_dw = (scd_q2ratid << 16) | (tbl_dw & 0x0000FFFF);
-	else
-		tbl_dw = scd_q2ratid | (tbl_dw & 0xFFFF0000);
-
-	iwl_write_targ_mem(priv, tbl_dw_addr, tbl_dw);
-
-	return 0;
-}
-
-
-/**
- * iwl4965_tx_queue_agg_enable - Set up & enable aggregation for selected queue
- *
- * NOTE:  txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE,
- *        i.e. it must be one of the higher queues used for aggregation
- */
-static int iwl4965_txq_agg_enable(struct iwl_priv *priv, int txq_id,
-				  int tx_fifo, int sta_id, int tid, u16 ssn_idx)
-{
-	unsigned long flags;
-	u16 ra_tid;
-	int ret;
-
-	if ((IWL49_FIRST_AMPDU_QUEUE > txq_id) ||
-	    (IWL49_FIRST_AMPDU_QUEUE +
-		priv->cfg->base_params->num_of_ampdu_queues <= txq_id)) {
-		IWL_WARN(priv,
-			"queue number out of range: %d, must be %d to %d\n",
-			txq_id, IWL49_FIRST_AMPDU_QUEUE,
-			IWL49_FIRST_AMPDU_QUEUE +
-			priv->cfg->base_params->num_of_ampdu_queues - 1);
-		return -EINVAL;
-	}
-
-	ra_tid = BUILD_RAxTID(sta_id, tid);
-
-	/* Modify device's station table to Tx this TID */
-	ret = iwl_sta_tx_modify_enable_tid(priv, sta_id, tid);
-	if (ret)
-		return ret;
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	/* Stop this Tx queue before configuring it */
-	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
-
-	/* Map receiver-address / traffic-ID to this queue */
-	iwl4965_tx_queue_set_q2ratid(priv, ra_tid, txq_id);
-
-	/* Set this queue as a chain-building queue */
-	iwl_set_bits_prph(priv, IWL49_SCD_QUEUECHAIN_SEL, (1 << txq_id));
-
-	/* Place first TFD at index corresponding to start sequence number.
-	 * Assumes that ssn_idx is valid (!= 0xFFF) */
-	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
-	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
-	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
-
-	/* Set up Tx window size and frame limit for this queue */
-	iwl_write_targ_mem(priv,
-		priv->scd_base_addr + IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id),
-		(SCD_WIN_SIZE << IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
-		IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
-
-	iwl_write_targ_mem(priv, priv->scd_base_addr +
-		IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
-		(SCD_FRAME_LIMIT << IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
-		& IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
-
-	iwl_set_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
-
-	/* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
-	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 1);
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
-}
-
-
 static u16 iwl4965_get_hcmd_size(u8 cmd_id, u16 len)
 {
 	switch (cmd_id) {
@@ -1975,7 +1577,8 @@ static u16 iwl4965_get_hcmd_size(u8 cmd_id, u16 len)
 	}
 }
 
-static u16 iwl4965_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
+static u16 iwl4965_build_addsta_hcmd(const struct iwl_legacy_addsta_cmd *cmd,
+								u8 *data)
 {
 	struct iwl4965_addsta_cmd *addsta = (struct iwl4965_addsta_cmd *)data;
 	addsta->mode = cmd->mode;
@@ -2028,16 +1631,14 @@ static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
 		status = le16_to_cpu(frame_status[0].status);
 		idx = start_idx;
 
-		/* FIXME: code repetition */
 		IWL_DEBUG_TX_REPLY(priv, "FrameCnt = %d, StartIdx=%d idx=%d\n",
 				   agg->frame_count, agg->start_idx, idx);
 
 		info = IEEE80211_SKB_CB(priv->txq[txq_id].txb[idx].skb);
 		info->status.rates[0].count = tx_resp->failure_frame + 1;
 		info->flags &= ~IEEE80211_TX_CTL_AMPDU;
-		info->flags |= iwl_tx_status_to_mac80211(status);
-		iwlagn_hwrate_to_tx_control(priv, rate_n_flags, info);
-		/* FIXME: code repetition end */
+		info->flags |= iwl4965_tx_status_to_mac80211(status);
+		iwl4965_hwrate_to_tx_control(priv, rate_n_flags, info);
 
 		IWL_DEBUG_TX_REPLY(priv, "1 Frame 0x%x failure :%d\n",
 				    status & 0xff, tx_resp->failure_frame);
@@ -2064,7 +1665,7 @@ static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
 			IWL_DEBUG_TX_REPLY(priv, "FrameCnt = %d, txq_id=%d idx=%d\n",
 					   agg->frame_count, txq_id, idx);
 
-			hdr = iwl_tx_queue_get_hdr(priv, txq_id, idx);
+			hdr = iwl_legacy_tx_queue_get_hdr(priv, txq_id, idx);
 			if (!hdr) {
 				IWL_ERR(priv,
 					"BUG_ON idx doesn't point to valid skb"
@@ -2115,15 +1716,14 @@ static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
 	return 0;
 }
 
-static u8 iwl_find_station(struct iwl_priv *priv, const u8 *addr)
+static u8 iwl4965_find_station(struct iwl_priv *priv, const u8 *addr)
 {
 	int i;
 	int start = 0;
 	int ret = IWL_INVALID_STATION;
 	unsigned long flags;
 
-	if ((priv->iw_mode == NL80211_IFTYPE_ADHOC) ||
-	    (priv->iw_mode == NL80211_IFTYPE_AP))
+	if ((priv->iw_mode == NL80211_IFTYPE_ADHOC))
 		start = IWL_STA_ID;
 
 	if (is_broadcast_ether_addr(addr))
@@ -2159,13 +1759,13 @@ static u8 iwl_find_station(struct iwl_priv *priv, const u8 *addr)
 	return ret;
 }
 
-static int iwl_get_ra_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr)
+static int iwl4965_get_ra_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr)
 {
 	if (priv->iw_mode == NL80211_IFTYPE_STATION) {
 		return IWL_AP_ID;
 	} else {
 		u8 *da = ieee80211_get_DA(hdr);
-		return iwl_find_station(priv, da);
+		return iwl4965_find_station(priv, da);
 	}
 }
 
@@ -2190,7 +1790,7 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
 	u8 *qc = NULL;
 	unsigned long flags;
 
-	if ((index >= txq->q.n_bd) || (iwl_queue_used(&txq->q, index) == 0)) {
+	if ((index >= txq->q.n_bd) || (iwl_legacy_queue_used(&txq->q, index) == 0)) {
 		IWL_ERR(priv, "Read index for DMA queue txq_id (%d) index %d "
 			  "is out of range [0-%d] %d %d\n", txq_id,
 			  index, txq->q.n_bd, txq->q.write_ptr,
@@ -2202,13 +1802,13 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
 	info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb);
 	memset(&info->status, 0, sizeof(info->status));
 
-	hdr = iwl_tx_queue_get_hdr(priv, txq_id, index);
+	hdr = iwl_legacy_tx_queue_get_hdr(priv, txq_id, index);
 	if (ieee80211_is_data_qos(hdr->frame_control)) {
 		qc = ieee80211_get_qos_ctl(hdr);
 		tid = qc[0] & 0xf;
 	}
 
-	sta_id = iwl_get_ra_sta_id(priv, hdr);
+	sta_id = iwl4965_get_ra_sta_id(priv, hdr);
 	if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
 		IWL_ERR(priv, "Station not known\n");
 		return;
@@ -2225,114 +1825,89 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
 		iwl4965_tx_status_reply_tx(priv, agg, tx_resp, txq_id, index);
 
 		/* check if BAR is needed */
-		if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status))
+		if ((tx_resp->frame_count == 1) && !iwl4965_is_tx_success(status))
 			info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
 
 		if (txq->q.read_ptr != (scd_ssn & 0xff)) {
-			index = iwl_queue_dec_wrap(scd_ssn & 0xff, txq->q.n_bd);
+			index = iwl_legacy_queue_dec_wrap(scd_ssn & 0xff,
+								txq->q.n_bd);
 			IWL_DEBUG_TX_REPLY(priv, "Retry scheduler reclaim scd_ssn "
 					   "%d index %d\n", scd_ssn , index);
-			freed = iwlagn_tx_queue_reclaim(priv, txq_id, index);
+			freed = iwl4965_tx_queue_reclaim(priv, txq_id, index);
 			if (qc)
-				iwl_free_tfds_in_queue(priv, sta_id,
+				iwl4965_free_tfds_in_queue(priv, sta_id,
 						       tid, freed);
 
 			if (priv->mac80211_registered &&
-			    (iwl_queue_space(&txq->q) > txq->q.low_mark) &&
-			    (agg->state != IWL_EMPTYING_HW_QUEUE_DELBA))
-				iwl_wake_queue(priv, txq);
+			    (iwl_legacy_queue_space(&txq->q) > txq->q.low_mark)
+				 && (agg->state != IWL_EMPTYING_HW_QUEUE_DELBA))
+				iwl_legacy_wake_queue(priv, txq);
 		}
 	} else {
 		info->status.rates[0].count = tx_resp->failure_frame + 1;
-		info->flags |= iwl_tx_status_to_mac80211(status);
-		iwlagn_hwrate_to_tx_control(priv,
+		info->flags |= iwl4965_tx_status_to_mac80211(status);
+		iwl4965_hwrate_to_tx_control(priv,
 					le32_to_cpu(tx_resp->rate_n_flags),
 					info);
 
 		IWL_DEBUG_TX_REPLY(priv, "TXQ %d status %s (0x%08x) "
 				   "rate_n_flags 0x%x retries %d\n",
 				   txq_id,
-				   iwl_get_tx_fail_reason(status), status,
+				   iwl4965_get_tx_fail_reason(status), status,
 				   le32_to_cpu(tx_resp->rate_n_flags),
 				   tx_resp->failure_frame);
 
-		freed = iwlagn_tx_queue_reclaim(priv, txq_id, index);
+		freed = iwl4965_tx_queue_reclaim(priv, txq_id, index);
 		if (qc && likely(sta_id != IWL_INVALID_STATION))
-			iwl_free_tfds_in_queue(priv, sta_id, tid, freed);
+			iwl4965_free_tfds_in_queue(priv, sta_id, tid, freed);
 		else if (sta_id == IWL_INVALID_STATION)
 			IWL_DEBUG_TX_REPLY(priv, "Station not known\n");
 
 		if (priv->mac80211_registered &&
-		    (iwl_queue_space(&txq->q) > txq->q.low_mark))
-			iwl_wake_queue(priv, txq);
+		    (iwl_legacy_queue_space(&txq->q) > txq->q.low_mark))
+			iwl_legacy_wake_queue(priv, txq);
 	}
 	if (qc && likely(sta_id != IWL_INVALID_STATION))
-		iwlagn_txq_check_empty(priv, sta_id, tid, txq_id);
+		iwl4965_txq_check_empty(priv, sta_id, tid, txq_id);
 
-	iwl_check_abort_status(priv, tx_resp->frame_count, status);
+	iwl4965_check_abort_status(priv, tx_resp->frame_count, status);
 
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 }
 
-static int iwl4965_calc_rssi(struct iwl_priv *priv,
-			     struct iwl_rx_phy_res *rx_resp)
+static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
 {
-	/* data from PHY/DSP regarding signal strength, etc.,
-	 *   contents are always there, not configurable by host.  */
-	struct iwl4965_rx_non_cfg_phy *ncphy =
-	    (struct iwl4965_rx_non_cfg_phy *)rx_resp->non_cfg_phy_buf;
-	u32 agc = (le16_to_cpu(ncphy->agc_info) & IWL49_AGC_DB_MASK)
-			>> IWL49_AGC_DB_POS;
-
-	u32 valid_antennae =
-	    (le16_to_cpu(rx_resp->phy_flags) & IWL49_RX_PHY_FLAGS_ANTENNAE_MASK)
-			>> IWL49_RX_PHY_FLAGS_ANTENNAE_OFFSET;
-	u8 max_rssi = 0;
-	u32 i;
-
-	/* Find max rssi among 3 possible receivers.
-	 * These values are measured by the digital signal processor (DSP).
-	 * They should stay fairly constant even as the signal strength varies,
-	 *   if the radio's automatic gain control (AGC) is working right.
-	 * AGC value (see below) will provide the "interesting" info. */
-	for (i = 0; i < 3; i++)
-		if (valid_antennae & (1 << i))
-			max_rssi = max(ncphy->rssi_info[i << 1], max_rssi);
-
-	IWL_DEBUG_STATS(priv, "Rssi In A %d B %d C %d Max %d AGC dB %d\n",
-		ncphy->rssi_info[0], ncphy->rssi_info[2], ncphy->rssi_info[4],
-		max_rssi, agc);
-
-	/* dBm = max_rssi dB - agc dB - constant.
-	 * Higher AGC (higher radio gain) means lower signal. */
-	return max_rssi - agc - IWLAGN_RSSI_OFFSET;
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl4965_beacon_notif *beacon = (void *)pkt->u.raw;
+	u8 rate __maybe_unused =
+		iwl4965_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+
+	IWL_DEBUG_RX(priv, "beacon status %#x, retries:%d ibssmgr:%d "
+		"tsf:0x%.8x%.8x rate:%d\n",
+		le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
+		beacon->beacon_notify_hdr.failure_frame,
+		le32_to_cpu(beacon->ibss_mgr_status),
+		le32_to_cpu(beacon->high_tsf),
+		le32_to_cpu(beacon->low_tsf), rate);
+
+	priv->ibss_manager = le32_to_cpu(beacon->ibss_mgr_status);
 }
 
-
 /* Set up 4965-specific Rx frame reply handlers */
 static void iwl4965_rx_handler_setup(struct iwl_priv *priv)
 {
 	/* Legacy Rx frames */
-	priv->rx_handlers[REPLY_RX] = iwlagn_rx_reply_rx;
+	priv->rx_handlers[REPLY_RX] = iwl4965_rx_reply_rx;
 	/* Tx response */
 	priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
-}
-
-static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
-{
-	INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
-}
-
-static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
-{
-	cancel_work_sync(&priv->txpower_work);
+	priv->rx_handlers[BEACON_NOTIFICATION] = iwl4965_rx_beacon_notif;
 }
 
 static struct iwl_hcmd_ops iwl4965_hcmd = {
 	.rxon_assoc = iwl4965_send_rxon_assoc,
 	.commit_rxon = iwl4965_commit_rxon,
-	.set_rxon_chain = iwlagn_set_rxon_chain,
-	.send_bt_config = iwl_send_bt_config,
+	.set_rxon_chain = iwl4965_set_rxon_chain,
 };
 
 static void iwl4965_post_scan(struct iwl_priv *priv)
@@ -2344,7 +1919,7 @@ static void iwl4965_post_scan(struct iwl_priv *priv)
 	 * performing the scan, fire one off if needed
 	 */
 	if (memcmp(&ctx->staging, &ctx->active, sizeof(ctx->staging)))
-		iwlcore_commit_rxon(priv, ctx);
+		iwl_legacy_commit_rxon(priv, ctx);
 }
 
 static void iwl4965_post_associate(struct iwl_priv *priv)
@@ -2357,29 +1932,24 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 	if (!vif || !priv->is_open)
 		return;
 
-	if (vif->type == NL80211_IFTYPE_AP) {
-		IWL_ERR(priv, "%s Should not be called in AP mode\n", __func__);
-		return;
-	}
-
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
 
-	iwl_scan_cancel_timeout(priv, 200);
+	iwl_legacy_scan_cancel_timeout(priv, 200);
 
-	conf = ieee80211_get_hw_conf(priv->hw);
+	conf = iwl_legacy_ieee80211_get_hw_conf(priv->hw);
 
 	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
-	iwlcore_commit_rxon(priv, ctx);
+	iwl_legacy_commit_rxon(priv, ctx);
 
-	ret = iwl_send_rxon_timing(priv, ctx);
+	ret = iwl_legacy_send_rxon_timing(priv, ctx);
 	if (ret)
 		IWL_WARN(priv, "RXON timing - "
 			    "Attempting to continue.\n");
 
 	ctx->staging.filter_flags |= RXON_FILTER_ASSOC_MSK;
 
-	iwl_set_rxon_ht(priv, &priv->current_ht_config);
+	iwl_legacy_set_rxon_ht(priv, &priv->current_ht_config);
 
 	if (priv->cfg->ops->hcmd->set_rxon_chain)
 		priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
@@ -2401,7 +1971,7 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 			ctx->staging.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
 	}
 
-	iwlcore_commit_rxon(priv, ctx);
+	iwl_legacy_commit_rxon(priv, ctx);
 
 	IWL_DEBUG_ASSOC(priv, "Associated as %d to: %pM\n",
 			vif->bss_conf.aid, ctx->active.bssid_addr);
@@ -2410,7 +1980,7 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 	case NL80211_IFTYPE_STATION:
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		iwlagn_send_beacon_cmd(priv);
+		iwl4965_send_beacon_cmd(priv);
 		break;
 	default:
 		IWL_ERR(priv, "%s Should not be called in %d mode\n",
@@ -2422,10 +1992,10 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 	 * If chain noise has already been run, then we need to enable
 	 * power management here */
 	if (priv->chain_noise_data.state == IWL_CHAIN_NOISE_DONE)
-		iwl_power_update_mode(priv, false);
+		iwl_legacy_power_update_mode(priv, false);
 
 	/* Enable Rx differential gain and sensitivity calibrations */
-	iwl_chain_noise_reset(priv);
+	iwl4965_chain_noise_reset(priv);
 	priv->start_calib = 1;
 }
 
@@ -2441,14 +2011,14 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
 		return;
 
 	/* The following should be done only at AP bring up */
-	if (!iwl_is_associated_ctx(ctx)) {
+	if (!iwl_legacy_is_associated_ctx(ctx)) {
 
 		/* RXON - unassoc (to set timing command) */
 		ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
-		iwlcore_commit_rxon(priv, ctx);
+		iwl_legacy_commit_rxon(priv, ctx);
 
 		/* RXON Timing */
-		ret = iwl_send_rxon_timing(priv, ctx);
+		ret = iwl_legacy_send_rxon_timing(priv, ctx);
 		if (ret)
 			IWL_WARN(priv, "RXON timing failed - "
 					"Attempting to continue.\n");
@@ -2456,7 +2026,7 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
 		/* AP has all antennas */
 		priv->chain_noise_data.active_chains =
 			priv->hw_params.valid_rx_ant;
-		iwl_set_rxon_ht(priv, &priv->current_ht_config);
+		iwl_legacy_set_rxon_ht(priv, &priv->current_ht_config);
 		if (priv->cfg->ops->hcmd->set_rxon_chain)
 			priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
 
@@ -2478,51 +2048,37 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
 					~RXON_FLG_SHORT_SLOT_MSK;
 		}
 		/* need to send beacon cmd before committing assoc RXON! */
-		iwlagn_send_beacon_cmd(priv);
+		iwl4965_send_beacon_cmd(priv);
 		/* restore RXON assoc */
 		ctx->staging.filter_flags |= RXON_FILTER_ASSOC_MSK;
-		iwlcore_commit_rxon(priv, ctx);
+		iwl_legacy_commit_rxon(priv, ctx);
 	}
-	iwlagn_send_beacon_cmd(priv);
-
-	/* FIXME - we need to add code here to detect a totally new
-	 * configuration, reset the AP, unassoc, rxon timing, assoc,
-	 * clear sta table, add BCAST sta... */
+	iwl4965_send_beacon_cmd(priv);
 }
 
 static struct iwl_hcmd_utils_ops iwl4965_hcmd_utils = {
 	.get_hcmd_size = iwl4965_get_hcmd_size,
 	.build_addsta_hcmd = iwl4965_build_addsta_hcmd,
-	.chain_noise_reset = iwl4965_chain_noise_reset,
-	.gain_computation = iwl4965_gain_computation,
-	.tx_cmd_protection = iwl_legacy_tx_cmd_protection,
-	.calc_rssi = iwl4965_calc_rssi,
-	.request_scan = iwlagn_request_scan,
+	.request_scan = iwl4965_request_scan,
 	.post_scan = iwl4965_post_scan,
 };
 
 static struct iwl_lib_ops iwl4965_lib = {
 	.set_hw_params = iwl4965_hw_set_hw_params,
 	.txq_update_byte_cnt_tbl = iwl4965_txq_update_byte_cnt_tbl,
-	.txq_set_sched = iwl4965_txq_set_sched,
-	.txq_agg_enable = iwl4965_txq_agg_enable,
-	.txq_agg_disable = iwl4965_txq_agg_disable,
-	.txq_attach_buf_to_tfd = iwl_hw_txq_attach_buf_to_tfd,
-	.txq_free_tfd = iwl_hw_txq_free_tfd,
-	.txq_init = iwl_hw_tx_queue_init,
+	.txq_attach_buf_to_tfd = iwl4965_hw_txq_attach_buf_to_tfd,
+	.txq_free_tfd = iwl4965_hw_txq_free_tfd,
+	.txq_init = iwl4965_hw_tx_queue_init,
 	.rx_handler_setup = iwl4965_rx_handler_setup,
-	.setup_deferred_work = iwl4965_setup_deferred_work,
-	.cancel_deferred_work = iwl4965_cancel_deferred_work,
 	.is_valid_rtc_data_addr = iwl4965_hw_valid_rtc_data_addr,
-	.alive_notify = iwl4965_alive_notify,
 	.init_alive_start = iwl4965_init_alive_start,
 	.load_ucode = iwl4965_load_bsm,
-	.dump_nic_event_log = iwl_dump_nic_event_log,
-	.dump_nic_error_log = iwl_dump_nic_error_log,
-	.dump_fh = iwl_dump_fh,
+	.dump_nic_event_log = iwl4965_dump_nic_event_log,
+	.dump_nic_error_log = iwl4965_dump_nic_error_log,
+	.dump_fh = iwl4965_dump_fh,
 	.set_channel_switch = iwl4965_hw_channel_switch,
 	.apm_ops = {
-		.init = iwl_apm_init,
+		.init = iwl_legacy_apm_init,
 		.config = iwl4965_nic_config,
 	},
 	.eeprom_ops = {
@@ -2535,64 +2091,56 @@ static struct iwl_lib_ops iwl4965_lib = {
 			EEPROM_4965_REGULATORY_BAND_24_HT40_CHANNELS,
 			EEPROM_4965_REGULATORY_BAND_52_HT40_CHANNELS
 		},
-		.acquire_semaphore = iwlcore_eeprom_acquire_semaphore,
-		.release_semaphore = iwlcore_eeprom_release_semaphore,
-		.calib_version = iwl4965_eeprom_calib_version,
-		.query_addr = iwlcore_eeprom_query_addr,
+		.acquire_semaphore = iwl4965_eeprom_acquire_semaphore,
+		.release_semaphore = iwl4965_eeprom_release_semaphore,
 	},
 	.send_tx_power	= iwl4965_send_tx_power,
-	.update_chain_flags = iwl_update_chain_flags,
-	.isr_ops = {
-		.isr = iwl_isr_legacy,
-	},
+	.update_chain_flags = iwl4965_update_chain_flags,
 	.temp_ops = {
 		.temperature = iwl4965_temperature_calib,
 	},
 	.debugfs_ops = {
-		.rx_stats_read = iwl_ucode_rx_stats_read,
-		.tx_stats_read = iwl_ucode_tx_stats_read,
-		.general_stats_read = iwl_ucode_general_stats_read,
-		.bt_stats_read = iwl_ucode_bt_stats_read,
-		.reply_tx_error = iwl_reply_tx_error_read,
+		.rx_stats_read = iwl4965_ucode_rx_stats_read,
+		.tx_stats_read = iwl4965_ucode_tx_stats_read,
+		.general_stats_read = iwl4965_ucode_general_stats_read,
 	},
-	.check_plcp_health = iwl_good_plcp_health,
+	.check_plcp_health = iwl4965_good_plcp_health,
 };
 
 static const struct iwl_legacy_ops iwl4965_legacy_ops = {
 	.post_associate = iwl4965_post_associate,
 	.config_ap = iwl4965_config_ap,
-	.manage_ibss_station = iwlagn_manage_ibss_station,
-	.update_bcast_stations = iwl_update_bcast_stations,
+	.manage_ibss_station = iwl4965_manage_ibss_station,
+	.update_bcast_stations = iwl4965_update_bcast_stations,
 };
 
 struct ieee80211_ops iwl4965_hw_ops = {
-	.tx = iwlagn_mac_tx,
-	.start = iwlagn_mac_start,
-	.stop = iwlagn_mac_stop,
-	.add_interface = iwl_mac_add_interface,
-	.remove_interface = iwl_mac_remove_interface,
-	.change_interface = iwl_mac_change_interface,
+	.tx = iwl4965_mac_tx,
+	.start = iwl4965_mac_start,
+	.stop = iwl4965_mac_stop,
+	.add_interface = iwl_legacy_mac_add_interface,
+	.remove_interface = iwl_legacy_mac_remove_interface,
+	.change_interface = iwl_legacy_mac_change_interface,
 	.config = iwl_legacy_mac_config,
-	.configure_filter = iwlagn_configure_filter,
-	.set_key = iwlagn_mac_set_key,
-	.update_tkip_key = iwlagn_mac_update_tkip_key,
-	.conf_tx = iwl_mac_conf_tx,
+	.configure_filter = iwl4965_configure_filter,
+	.set_key = iwl4965_mac_set_key,
+	.update_tkip_key = iwl4965_mac_update_tkip_key,
+	.conf_tx = iwl_legacy_mac_conf_tx,
 	.reset_tsf = iwl_legacy_mac_reset_tsf,
 	.bss_info_changed = iwl_legacy_mac_bss_info_changed,
-	.ampdu_action = iwlagn_mac_ampdu_action,
-	.hw_scan = iwl_mac_hw_scan,
-	.sta_add = iwlagn_mac_sta_add,
-	.sta_remove = iwl_mac_sta_remove,
-	.channel_switch = iwlagn_mac_channel_switch,
-	.flush = iwlagn_mac_flush,
-	.tx_last_beacon = iwl_mac_tx_last_beacon,
+	.ampdu_action = iwl4965_mac_ampdu_action,
+	.hw_scan = iwl_legacy_mac_hw_scan,
+	.sta_add = iwl4965_mac_sta_add,
+	.sta_remove = iwl_legacy_mac_sta_remove,
+	.channel_switch = iwl4965_mac_channel_switch,
+	.tx_last_beacon = iwl_legacy_mac_tx_last_beacon,
 };
 
 static const struct iwl_ops iwl4965_ops = {
 	.lib = &iwl4965_lib,
 	.hcmd = &iwl4965_hcmd,
 	.utils = &iwl4965_hcmd_utils,
-	.led = &iwlagn_led_ops,
+	.led = &iwl4965_led_ops,
 	.legacy = &iwl4965_legacy_ops,
 	.ieee80211_ops = &iwl4965_hw_ops,
 };
@@ -2604,22 +2152,18 @@ static struct iwl_base_params iwl4965_base_params = {
 	.pll_cfg_val = 0,
 	.set_l0s = true,
 	.use_bsm = true,
-	.use_isr_legacy = true,
-	.broken_powersave = true,
 	.led_compensation = 61,
 	.chain_noise_num_beacons = IWL4965_CAL_NUM_BEACONS,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
 	.wd_timeout = IWL_DEF_WD_TIMEOUT,
 	.temperature_kelvin = true,
 	.max_event_log_size = 512,
-	.tx_power_by_driver = true,
 	.ucode_tracing = true,
 	.sensitivity_calib_by_driver = true,
 	.chain_noise_calib_by_driver = true,
-	.no_agg_framecnt_info = true,
 };
 
-struct iwl_cfg iwl4965_agn_cfg = {
+struct iwl_cfg iwl4965_cfg = {
 	.name = "Intel(R) Wireless WiFi Link 4965AGN",
 	.fw_name_pre = IWL4965_FW_PRE,
 	.ucode_api_max = IWL4965_UCODE_API_MAX,
@@ -2630,7 +2174,7 @@ struct iwl_cfg iwl4965_agn_cfg = {
 	.eeprom_ver = EEPROM_4965_EEPROM_VERSION,
 	.eeprom_calib_ver = EEPROM_4965_TX_POWER_VERSION,
 	.ops = &iwl4965_ops,
-	.mod_params = &iwlagn_mod_params,
+	.mod_params = &iwl4965_mod_params,
 	.base_params = &iwl4965_base_params,
 	.led_mode = IWL_LED_BLINK,
 	/*
@@ -2642,4 +2186,3 @@ struct iwl_cfg iwl4965_agn_cfg = {
 
 /* Module firmware */
 MODULE_FIRMWARE(IWL4965_MODULE_FIRMWARE(IWL4965_UCODE_API_MAX));
-
diff --git a/drivers/net/wireless/iwlegacy/iwl-4965.h b/drivers/net/wireless/iwlegacy/iwl-4965.h
new file mode 100644
index 000000000000..01f8163daf16
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-4965.h
@@ -0,0 +1,282 @@
+/******************************************************************************
+ *
+ * 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 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef __iwl_4965_h__
+#define __iwl_4965_h__
+
+#include "iwl-dev.h"
+
+/* configuration for the _4965 devices */
+extern struct iwl_cfg iwl4965_cfg;
+
+extern struct iwl_mod_params iwl4965_mod_params;
+
+extern struct ieee80211_ops iwl4965_hw_ops;
+
+/* tx queue */
+void iwl4965_free_tfds_in_queue(struct iwl_priv *priv,
+			    int sta_id, int tid, int freed);
+
+/* RXON */
+void iwl4965_set_rxon_chain(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx);
+
+/* uCode */
+int iwl4965_verify_ucode(struct iwl_priv *priv);
+
+/* lib */
+void iwl4965_check_abort_status(struct iwl_priv *priv,
+			    u8 frame_count, u32 status);
+
+void iwl4965_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+int iwl4965_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+int iwl4965_hw_nic_init(struct iwl_priv *priv);
+int iwl4965_dump_fh(struct iwl_priv *priv, char **buf, bool display);
+
+/* rx */
+void iwl4965_rx_queue_restock(struct iwl_priv *priv);
+void iwl4965_rx_replenish(struct iwl_priv *priv);
+void iwl4965_rx_replenish_now(struct iwl_priv *priv);
+void iwl4965_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+int iwl4965_rxq_stop(struct iwl_priv *priv);
+int iwl4965_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band);
+void iwl4965_rx_reply_rx(struct iwl_priv *priv,
+		     struct iwl_rx_mem_buffer *rxb);
+void iwl4965_rx_reply_rx_phy(struct iwl_priv *priv,
+			 struct iwl_rx_mem_buffer *rxb);
+void iwl4965_rx_handle(struct iwl_priv *priv);
+
+/* tx */
+void iwl4965_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq);
+int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv,
+				 struct iwl_tx_queue *txq,
+				 dma_addr_t addr, u16 len, u8 reset, u8 pad);
+int iwl4965_hw_tx_queue_init(struct iwl_priv *priv,
+			 struct iwl_tx_queue *txq);
+void iwl4965_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
+			      struct ieee80211_tx_info *info);
+int iwl4965_tx_skb(struct iwl_priv *priv, struct sk_buff *skb);
+int iwl4965_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn);
+int iwl4965_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta, u16 tid);
+int iwl4965_txq_check_empty(struct iwl_priv *priv,
+			   int sta_id, u8 tid, int txq_id);
+void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb);
+int iwl4965_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index);
+void iwl4965_hw_txq_ctx_free(struct iwl_priv *priv);
+int iwl4965_txq_ctx_alloc(struct iwl_priv *priv);
+void iwl4965_txq_ctx_reset(struct iwl_priv *priv);
+void iwl4965_txq_ctx_stop(struct iwl_priv *priv);
+void iwl4965_txq_set_sched(struct iwl_priv *priv, u32 mask);
+
+/*
+ * Acquire priv->lock before calling this function !
+ */
+void iwl4965_set_wr_ptrs(struct iwl_priv *priv, int txq_id, u32 index);
+/**
+ * iwl4965_tx_queue_set_status - (optionally) start Tx/Cmd queue
+ * @tx_fifo_id: Tx DMA/FIFO channel (range 0-7) that the queue will feed
+ * @scd_retry: (1) Indicates queue will be used in aggregation mode
+ *
+ * NOTE:  Acquire priv->lock before calling this function !
+ */
+void iwl4965_tx_queue_set_status(struct iwl_priv *priv,
+					struct iwl_tx_queue *txq,
+					int tx_fifo_id, int scd_retry);
+
+static inline u32 iwl4965_tx_status_to_mac80211(u32 status)
+{
+	status &= TX_STATUS_MSK;
+
+	switch (status) {
+	case TX_STATUS_SUCCESS:
+	case TX_STATUS_DIRECT_DONE:
+		return IEEE80211_TX_STAT_ACK;
+	case TX_STATUS_FAIL_DEST_PS:
+		return IEEE80211_TX_STAT_TX_FILTERED;
+	default:
+		return 0;
+	}
+}
+
+static inline bool iwl4965_is_tx_success(u32 status)
+{
+	status &= TX_STATUS_MSK;
+	return (status == TX_STATUS_SUCCESS) ||
+	       (status == TX_STATUS_DIRECT_DONE);
+}
+
+u8 iwl4965_toggle_tx_ant(struct iwl_priv *priv, u8 ant_idx, u8 valid);
+
+/* rx */
+void iwl4965_rx_missed_beacon_notif(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb);
+bool iwl4965_good_plcp_health(struct iwl_priv *priv,
+			  struct iwl_rx_packet *pkt);
+void iwl4965_rx_statistics(struct iwl_priv *priv,
+		       struct iwl_rx_mem_buffer *rxb);
+void iwl4965_reply_statistics(struct iwl_priv *priv,
+			  struct iwl_rx_mem_buffer *rxb);
+
+/* scan */
+int iwl4965_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif);
+
+/* station mgmt */
+int iwl4965_manage_ibss_station(struct iwl_priv *priv,
+			       struct ieee80211_vif *vif, bool add);
+
+/* hcmd */
+int iwl4965_send_beacon_cmd(struct iwl_priv *priv);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+const char *iwl4965_get_tx_fail_reason(u32 status);
+#else
+static inline const char *
+iwl4965_get_tx_fail_reason(u32 status) { return ""; }
+#endif
+
+/* station management */
+int iwl4965_alloc_bcast_station(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx);
+int iwl4965_add_bssid_station(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx,
+			     const u8 *addr, u8 *sta_id_r);
+int iwl4965_remove_default_wep_key(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx,
+			       struct ieee80211_key_conf *key);
+int iwl4965_set_default_wep_key(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    struct ieee80211_key_conf *key);
+int iwl4965_restore_default_wep_keys(struct iwl_priv *priv,
+				 struct iwl_rxon_context *ctx);
+int iwl4965_set_dynamic_key(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			struct ieee80211_key_conf *key, u8 sta_id);
+int iwl4965_remove_dynamic_key(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			struct ieee80211_key_conf *key, u8 sta_id);
+void iwl4965_update_tkip_key(struct iwl_priv *priv,
+			 struct iwl_rxon_context *ctx,
+			 struct ieee80211_key_conf *keyconf,
+			 struct ieee80211_sta *sta, u32 iv32, u16 *phase1key);
+int iwl4965_sta_tx_modify_enable_tid(struct iwl_priv *priv,
+			int sta_id, int tid);
+int iwl4965_sta_rx_agg_start(struct iwl_priv *priv, struct ieee80211_sta *sta,
+			 int tid, u16 ssn);
+int iwl4965_sta_rx_agg_stop(struct iwl_priv *priv, struct ieee80211_sta *sta,
+			int tid);
+void iwl4965_sta_modify_sleep_tx_count(struct iwl_priv *priv,
+			int sta_id, int cnt);
+int iwl4965_update_bcast_stations(struct iwl_priv *priv);
+
+/* rate */
+static inline u32 iwl4965_ant_idx_to_flags(u8 ant_idx)
+{
+	return BIT(ant_idx) << RATE_MCS_ANT_POS;
+}
+
+static inline u8 iwl4965_hw_get_rate(__le32 rate_n_flags)
+{
+	return le32_to_cpu(rate_n_flags) & 0xFF;
+}
+
+static inline __le32 iwl4965_hw_set_rate_n_flags(u8 rate, u32 flags)
+{
+	return cpu_to_le32(flags|(u32)rate);
+}
+
+/* eeprom */
+void iwl4965_eeprom_get_mac(const struct iwl_priv *priv, u8 *mac);
+int iwl4965_eeprom_acquire_semaphore(struct iwl_priv *priv);
+void iwl4965_eeprom_release_semaphore(struct iwl_priv *priv);
+int  iwl4965_eeprom_check_version(struct iwl_priv *priv);
+
+/* mac80211 handlers (for 4965) */
+void iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
+int iwl4965_mac_start(struct ieee80211_hw *hw);
+void iwl4965_mac_stop(struct ieee80211_hw *hw);
+void iwl4965_configure_filter(struct ieee80211_hw *hw,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast);
+int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key);
+void iwl4965_mac_update_tkip_key(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_key_conf *keyconf,
+				struct ieee80211_sta *sta,
+				u32 iv32, u16 *phase1key);
+int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
+			    struct ieee80211_vif *vif,
+			    enum ieee80211_ampdu_mlme_action action,
+			    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			    u8 buf_size);
+int iwl4965_mac_sta_add(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta);
+void iwl4965_mac_channel_switch(struct ieee80211_hw *hw,
+			       struct ieee80211_channel_switch *ch_switch);
+
+#endif /* __iwl_4965_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-commands.h b/drivers/net/wireless/iwlegacy/iwl-commands.h
new file mode 100644
index 000000000000..17a1d504348e
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-commands.h
@@ -0,0 +1,3405 @@
+/******************************************************************************
+ *
+ * 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) 2005 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+/*
+ * Please use this file (iwl-commands.h) only for uCode API definitions.
+ * Please use iwl-xxxx-hw.h for hardware-related definitions.
+ * Please use iwl-dev.h for driver implementation definitions.
+ */
+
+#ifndef __iwl_legacy_commands_h__
+#define __iwl_legacy_commands_h__
+
+struct iwl_priv;
+
+/* uCode version contains 4 values: Major/Minor/API/Serial */
+#define IWL_UCODE_MAJOR(ver)	(((ver) & 0xFF000000) >> 24)
+#define IWL_UCODE_MINOR(ver)	(((ver) & 0x00FF0000) >> 16)
+#define IWL_UCODE_API(ver)	(((ver) & 0x0000FF00) >> 8)
+#define IWL_UCODE_SERIAL(ver)	((ver) & 0x000000FF)
+
+
+/* Tx rates */
+#define IWL_CCK_RATES	4
+#define IWL_OFDM_RATES	8
+#define IWL_MAX_RATES	(IWL_CCK_RATES + IWL_OFDM_RATES)
+
+enum {
+	REPLY_ALIVE = 0x1,
+	REPLY_ERROR = 0x2,
+
+	/* RXON and QOS commands */
+	REPLY_RXON = 0x10,
+	REPLY_RXON_ASSOC = 0x11,
+	REPLY_QOS_PARAM = 0x13,
+	REPLY_RXON_TIMING = 0x14,
+
+	/* Multi-Station support */
+	REPLY_ADD_STA = 0x18,
+	REPLY_REMOVE_STA = 0x19,
+
+	/* Security */
+	REPLY_WEPKEY = 0x20,
+
+	/* RX, TX, LEDs */
+	REPLY_3945_RX = 0x1b,           /* 3945 only */
+	REPLY_TX = 0x1c,
+	REPLY_RATE_SCALE = 0x47,	/* 3945 only */
+	REPLY_LEDS_CMD = 0x48,
+	REPLY_TX_LINK_QUALITY_CMD = 0x4e, /* for 4965 and up */
+
+	/* 802.11h related */
+	REPLY_CHANNEL_SWITCH = 0x72,
+	CHANNEL_SWITCH_NOTIFICATION = 0x73,
+	REPLY_SPECTRUM_MEASUREMENT_CMD = 0x74,
+	SPECTRUM_MEASURE_NOTIFICATION = 0x75,
+
+	/* Power Management */
+	POWER_TABLE_CMD = 0x77,
+	PM_SLEEP_NOTIFICATION = 0x7A,
+	PM_DEBUG_STATISTIC_NOTIFIC = 0x7B,
+
+	/* Scan commands and notifications */
+	REPLY_SCAN_CMD = 0x80,
+	REPLY_SCAN_ABORT_CMD = 0x81,
+	SCAN_START_NOTIFICATION = 0x82,
+	SCAN_RESULTS_NOTIFICATION = 0x83,
+	SCAN_COMPLETE_NOTIFICATION = 0x84,
+
+	/* IBSS/AP commands */
+	BEACON_NOTIFICATION = 0x90,
+	REPLY_TX_BEACON = 0x91,
+
+	/* Miscellaneous commands */
+	REPLY_TX_PWR_TABLE_CMD = 0x97,
+
+	/* Bluetooth device coexistence config command */
+	REPLY_BT_CONFIG = 0x9b,
+
+	/* Statistics */
+	REPLY_STATISTICS_CMD = 0x9c,
+	STATISTICS_NOTIFICATION = 0x9d,
+
+	/* RF-KILL commands and notifications */
+	CARD_STATE_NOTIFICATION = 0xa1,
+
+	/* Missed beacons notification */
+	MISSED_BEACONS_NOTIFICATION = 0xa2,
+
+	REPLY_CT_KILL_CONFIG_CMD = 0xa4,
+	SENSITIVITY_CMD = 0xa8,
+	REPLY_PHY_CALIBRATION_CMD = 0xb0,
+	REPLY_RX_PHY_CMD = 0xc0,
+	REPLY_RX_MPDU_CMD = 0xc1,
+	REPLY_RX = 0xc3,
+	REPLY_COMPRESSED_BA = 0xc5,
+
+	REPLY_MAX = 0xff
+};
+
+/******************************************************************************
+ * (0)
+ * Commonly used structures and definitions:
+ * Command header, rate_n_flags, txpower
+ *
+ *****************************************************************************/
+
+/* iwl_cmd_header flags value */
+#define IWL_CMD_FAILED_MSK 0x40
+
+#define SEQ_TO_QUEUE(s)	(((s) >> 8) & 0x1f)
+#define QUEUE_TO_SEQ(q)	(((q) & 0x1f) << 8)
+#define SEQ_TO_INDEX(s)	((s) & 0xff)
+#define INDEX_TO_SEQ(i)	((i) & 0xff)
+#define SEQ_HUGE_FRAME	cpu_to_le16(0x4000)
+#define SEQ_RX_FRAME	cpu_to_le16(0x8000)
+
+/**
+ * struct iwl_cmd_header
+ *
+ * This header format appears in the beginning of each command sent from the
+ * driver, and each response/notification received from uCode.
+ */
+struct iwl_cmd_header {
+	u8 cmd;		/* Command ID:  REPLY_RXON, etc. */
+	u8 flags;	/* 0:5 reserved, 6 abort, 7 internal */
+	/*
+	 * The driver sets up the sequence number to values of its choosing.
+	 * uCode does not use this value, but passes it back to the driver
+	 * when sending the response to each driver-originated command, so
+	 * the driver can match the response to the command.  Since the values
+	 * don't get used by uCode, the driver may set up an arbitrary format.
+	 *
+	 * There is one exception:  uCode sets bit 15 when it originates
+	 * the response/notification, i.e. when the response/notification
+	 * is not a direct response to a command sent by the driver.  For
+	 * example, uCode issues REPLY_3945_RX when it sends a received frame
+	 * to the driver; it is not a direct response to any driver command.
+	 *
+	 * The Linux driver uses the following format:
+	 *
+	 *  0:7		tfd index - position within TX queue
+	 *  8:12	TX queue id
+	 *  13		reserved
+	 *  14		huge - driver sets this to indicate command is in the
+	 *  		'huge' storage at the end of the command buffers
+	 *  15		unsolicited RX or uCode-originated notification
+	*/
+	__le16 sequence;
+
+	/* command or response/notification data follows immediately */
+	u8 data[0];
+} __packed;
+
+
+/**
+ * struct iwl3945_tx_power
+ *
+ * Used in REPLY_TX_PWR_TABLE_CMD, REPLY_SCAN_CMD, REPLY_CHANNEL_SWITCH
+ *
+ * Each entry contains two values:
+ * 1)  DSP gain (or sometimes called DSP attenuation).  This is a fine-grained
+ *     linear value that multiplies the output of the digital signal processor,
+ *     before being sent to the analog radio.
+ * 2)  Radio gain.  This sets the analog gain of the radio Tx path.
+ *     It is a coarser setting, and behaves in a logarithmic (dB) fashion.
+ *
+ * Driver obtains values from struct iwl3945_tx_power power_gain_table[][].
+ */
+struct iwl3945_tx_power {
+	u8 tx_gain;		/* gain for analog radio */
+	u8 dsp_atten;		/* gain for DSP */
+} __packed;
+
+/**
+ * struct iwl3945_power_per_rate
+ *
+ * Used in REPLY_TX_PWR_TABLE_CMD, REPLY_CHANNEL_SWITCH
+ */
+struct iwl3945_power_per_rate {
+	u8 rate;		/* plcp */
+	struct iwl3945_tx_power tpc;
+	u8 reserved;
+} __packed;
+
+/**
+ * iwl4965 rate_n_flags bit fields
+ *
+ * rate_n_flags format is used in following iwl4965 commands:
+ *  REPLY_RX (response only)
+ *  REPLY_RX_MPDU (response only)
+ *  REPLY_TX (both command and response)
+ *  REPLY_TX_LINK_QUALITY_CMD
+ *
+ * High-throughput (HT) rate format for bits 7:0 (bit 8 must be "1"):
+ *  2-0:  0)   6 Mbps
+ *        1)  12 Mbps
+ *        2)  18 Mbps
+ *        3)  24 Mbps
+ *        4)  36 Mbps
+ *        5)  48 Mbps
+ *        6)  54 Mbps
+ *        7)  60 Mbps
+ *
+ *  4-3:  0)  Single stream (SISO)
+ *        1)  Dual stream (MIMO)
+ *        2)  Triple stream (MIMO)
+ *
+ *    5:  Value of 0x20 in bits 7:0 indicates 6 Mbps HT40 duplicate data
+ *
+ * Legacy OFDM rate format for bits 7:0 (bit 8 must be "0", bit 9 "0"):
+ *  3-0:  0xD)   6 Mbps
+ *        0xF)   9 Mbps
+ *        0x5)  12 Mbps
+ *        0x7)  18 Mbps
+ *        0x9)  24 Mbps
+ *        0xB)  36 Mbps
+ *        0x1)  48 Mbps
+ *        0x3)  54 Mbps
+ *
+ * Legacy CCK rate format for bits 7:0 (bit 8 must be "0", bit 9 "1"):
+ *  6-0:   10)  1 Mbps
+ *         20)  2 Mbps
+ *         55)  5.5 Mbps
+ *        110)  11 Mbps
+ */
+#define RATE_MCS_CODE_MSK 0x7
+#define RATE_MCS_SPATIAL_POS 3
+#define RATE_MCS_SPATIAL_MSK 0x18
+#define RATE_MCS_HT_DUP_POS 5
+#define RATE_MCS_HT_DUP_MSK 0x20
+
+/* Bit 8: (1) HT format, (0) legacy format in bits 7:0 */
+#define RATE_MCS_FLAGS_POS 8
+#define RATE_MCS_HT_POS 8
+#define RATE_MCS_HT_MSK 0x100
+
+/* Bit 9: (1) CCK, (0) OFDM.  HT (bit 8) must be "0" for this bit to be valid */
+#define RATE_MCS_CCK_POS 9
+#define RATE_MCS_CCK_MSK 0x200
+
+/* Bit 10: (1) Use Green Field preamble */
+#define RATE_MCS_GF_POS 10
+#define RATE_MCS_GF_MSK 0x400
+
+/* Bit 11: (1) Use 40Mhz HT40 chnl width, (0) use 20 MHz legacy chnl width */
+#define RATE_MCS_HT40_POS 11
+#define RATE_MCS_HT40_MSK 0x800
+
+/* Bit 12: (1) Duplicate data on both 20MHz chnls. HT40 (bit 11) must be set. */
+#define RATE_MCS_DUP_POS 12
+#define RATE_MCS_DUP_MSK 0x1000
+
+/* Bit 13: (1) Short guard interval (0.4 usec), (0) normal GI (0.8 usec) */
+#define RATE_MCS_SGI_POS 13
+#define RATE_MCS_SGI_MSK 0x2000
+
+/**
+ * rate_n_flags Tx antenna masks
+ * 4965 has 2 transmitters
+ * bit14:16
+ */
+#define RATE_MCS_ANT_POS	14
+#define RATE_MCS_ANT_A_MSK	0x04000
+#define RATE_MCS_ANT_B_MSK	0x08000
+#define RATE_MCS_ANT_C_MSK	0x10000
+#define RATE_MCS_ANT_AB_MSK	(RATE_MCS_ANT_A_MSK | RATE_MCS_ANT_B_MSK)
+#define RATE_MCS_ANT_ABC_MSK	(RATE_MCS_ANT_AB_MSK | RATE_MCS_ANT_C_MSK)
+#define RATE_ANT_NUM 3
+
+#define POWER_TABLE_NUM_ENTRIES			33
+#define POWER_TABLE_NUM_HT_OFDM_ENTRIES		32
+#define POWER_TABLE_CCK_ENTRY			32
+
+#define IWL_PWR_NUM_HT_OFDM_ENTRIES		24
+#define IWL_PWR_CCK_ENTRIES			2
+
+/**
+ * union iwl4965_tx_power_dual_stream
+ *
+ * Host format used for REPLY_TX_PWR_TABLE_CMD, REPLY_CHANNEL_SWITCH
+ * Use __le32 version (struct tx_power_dual_stream) when building command.
+ *
+ * Driver provides radio gain and DSP attenuation settings to device in pairs,
+ * one value for each transmitter chain.  The first value is for transmitter A,
+ * second for transmitter B.
+ *
+ * For SISO bit rates, both values in a pair should be identical.
+ * For MIMO rates, one value may be different from the other,
+ * in order to balance the Tx output between the two transmitters.
+ *
+ * See more details in doc for TXPOWER in iwl-4965-hw.h.
+ */
+union iwl4965_tx_power_dual_stream {
+	struct {
+		u8 radio_tx_gain[2];
+		u8 dsp_predis_atten[2];
+	} s;
+	u32 dw;
+};
+
+/**
+ * struct tx_power_dual_stream
+ *
+ * Table entries in REPLY_TX_PWR_TABLE_CMD, REPLY_CHANNEL_SWITCH
+ *
+ * Same format as iwl_tx_power_dual_stream, but __le32
+ */
+struct tx_power_dual_stream {
+	__le32 dw;
+} __packed;
+
+/**
+ * struct iwl4965_tx_power_db
+ *
+ * Entire table within REPLY_TX_PWR_TABLE_CMD, REPLY_CHANNEL_SWITCH
+ */
+struct iwl4965_tx_power_db {
+	struct tx_power_dual_stream power_tbl[POWER_TABLE_NUM_ENTRIES];
+} __packed;
+
+/******************************************************************************
+ * (0a)
+ * Alive and Error Commands & Responses:
+ *
+ *****************************************************************************/
+
+#define UCODE_VALID_OK	cpu_to_le32(0x1)
+#define INITIALIZE_SUBTYPE    (9)
+
+/*
+ * ("Initialize") REPLY_ALIVE = 0x1 (response only, not a command)
+ *
+ * uCode issues this "initialize alive" notification once the initialization
+ * uCode image has completed its work, and is ready to load the runtime image.
+ * This is the *first* "alive" notification that the driver will receive after
+ * rebooting uCode; the "initialize" alive is indicated by subtype field == 9.
+ *
+ * See comments documenting "BSM" (bootstrap state machine).
+ *
+ * For 4965, this notification contains important calibration data for
+ * calculating txpower settings:
+ *
+ * 1)  Power supply voltage indication.  The voltage sensor outputs higher
+ *     values for lower voltage, and vice verse.
+ *
+ * 2)  Temperature measurement parameters, for each of two channel widths
+ *     (20 MHz and 40 MHz) supported by the radios.  Temperature sensing
+ *     is done via one of the receiver chains, and channel width influences
+ *     the results.
+ *
+ * 3)  Tx gain compensation to balance 4965's 2 Tx chains for MIMO operation,
+ *     for each of 5 frequency ranges.
+ */
+struct iwl_init_alive_resp {
+	u8 ucode_minor;
+	u8 ucode_major;
+	__le16 reserved1;
+	u8 sw_rev[8];
+	u8 ver_type;
+	u8 ver_subtype;		/* "9" for initialize alive */
+	__le16 reserved2;
+	__le32 log_event_table_ptr;
+	__le32 error_event_table_ptr;
+	__le32 timestamp;
+	__le32 is_valid;
+
+	/* calibration values from "initialize" uCode */
+	__le32 voltage;		/* signed, higher value is lower voltage */
+	__le32 therm_r1[2];	/* signed, 1st for normal, 2nd for HT40 */
+	__le32 therm_r2[2];	/* signed */
+	__le32 therm_r3[2];	/* signed */
+	__le32 therm_r4[2];	/* signed */
+	__le32 tx_atten[5][2];	/* signed MIMO gain comp, 5 freq groups,
+				 * 2 Tx chains */
+} __packed;
+
+
+/**
+ * REPLY_ALIVE = 0x1 (response only, not a command)
+ *
+ * uCode issues this "alive" notification once the runtime image is ready
+ * to receive commands from the driver.  This is the *second* "alive"
+ * notification that the driver will receive after rebooting uCode;
+ * this "alive" is indicated by subtype field != 9.
+ *
+ * See comments documenting "BSM" (bootstrap state machine).
+ *
+ * This response includes two pointers to structures within the device's
+ * data SRAM (access via HBUS_TARG_MEM_* regs) that are useful for debugging:
+ *
+ * 1)  log_event_table_ptr indicates base of the event log.  This traces
+ *     a 256-entry history of uCode execution within a circular buffer.
+ *     Its header format is:
+ *
+ *	__le32 log_size;     log capacity (in number of entries)
+ *	__le32 type;         (1) timestamp with each entry, (0) no timestamp
+ *	__le32 wraps;        # times uCode has wrapped to top of circular buffer
+ *      __le32 write_index;  next circular buffer entry that uCode would fill
+ *
+ *     The header is followed by the circular buffer of log entries.  Entries
+ *     with timestamps have the following format:
+ *
+ *	__le32 event_id;     range 0 - 1500
+ *	__le32 timestamp;    low 32 bits of TSF (of network, if associated)
+ *	__le32 data;         event_id-specific data value
+ *
+ *     Entries without timestamps contain only event_id and data.
+ *
+ *
+ * 2)  error_event_table_ptr indicates base of the error log.  This contains
+ *     information about any uCode error that occurs.  For 4965, the format
+ *     of the error log is:
+ *
+ *	__le32 valid;        (nonzero) valid, (0) log is empty
+ *	__le32 error_id;     type of error
+ *	__le32 pc;           program counter
+ *	__le32 blink1;       branch link
+ *	__le32 blink2;       branch link
+ *	__le32 ilink1;       interrupt link
+ *	__le32 ilink2;       interrupt link
+ *	__le32 data1;        error-specific data
+ *	__le32 data2;        error-specific data
+ *	__le32 line;         source code line of error
+ *	__le32 bcon_time;    beacon timer
+ *	__le32 tsf_low;      network timestamp function timer
+ *	__le32 tsf_hi;       network timestamp function timer
+ *	__le32 gp1;          GP1 timer register
+ *	__le32 gp2;          GP2 timer register
+ *	__le32 gp3;          GP3 timer register
+ *	__le32 ucode_ver;    uCode version
+ *	__le32 hw_ver;       HW Silicon version
+ *	__le32 brd_ver;      HW board version
+ *	__le32 log_pc;       log program counter
+ *	__le32 frame_ptr;    frame pointer
+ *	__le32 stack_ptr;    stack pointer
+ *	__le32 hcmd;         last host command
+ *	__le32 isr0;         isr status register LMPM_NIC_ISR0: rxtx_flag
+ *	__le32 isr1;         isr status register LMPM_NIC_ISR1: host_flag
+ *	__le32 isr2;         isr status register LMPM_NIC_ISR2: enc_flag
+ *	__le32 isr3;         isr status register LMPM_NIC_ISR3: time_flag
+ *	__le32 isr4;         isr status register LMPM_NIC_ISR4: wico interrupt
+ *	__le32 isr_pref;     isr status register LMPM_NIC_PREF_STAT
+ *	__le32 wait_event;   wait event() caller address
+ *	__le32 l2p_control;  L2pControlField
+ *	__le32 l2p_duration; L2pDurationField
+ *	__le32 l2p_mhvalid;  L2pMhValidBits
+ *	__le32 l2p_addr_match; L2pAddrMatchStat
+ *	__le32 lmpm_pmg_sel; indicate which clocks are turned on (LMPM_PMG_SEL)
+ *	__le32 u_timestamp;  indicate when the date and time of the compilation
+ *	__le32 reserved;
+ *
+ * The Linux driver can print both logs to the system log when a uCode error
+ * occurs.
+ */
+struct iwl_alive_resp {
+	u8 ucode_minor;
+	u8 ucode_major;
+	__le16 reserved1;
+	u8 sw_rev[8];
+	u8 ver_type;
+	u8 ver_subtype;			/* not "9" for runtime alive */
+	__le16 reserved2;
+	__le32 log_event_table_ptr;	/* SRAM address for event log */
+	__le32 error_event_table_ptr;	/* SRAM address for error log */
+	__le32 timestamp;
+	__le32 is_valid;
+} __packed;
+
+/*
+ * REPLY_ERROR = 0x2 (response only, not a command)
+ */
+struct iwl_error_resp {
+	__le32 error_type;
+	u8 cmd_id;
+	u8 reserved1;
+	__le16 bad_cmd_seq_num;
+	__le32 error_info;
+	__le64 timestamp;
+} __packed;
+
+/******************************************************************************
+ * (1)
+ * RXON Commands & Responses:
+ *
+ *****************************************************************************/
+
+/*
+ * Rx config defines & structure
+ */
+/* rx_config device types  */
+enum {
+	RXON_DEV_TYPE_AP = 1,
+	RXON_DEV_TYPE_ESS = 3,
+	RXON_DEV_TYPE_IBSS = 4,
+	RXON_DEV_TYPE_SNIFFER = 6,
+};
+
+
+#define RXON_RX_CHAIN_DRIVER_FORCE_MSK		cpu_to_le16(0x1 << 0)
+#define RXON_RX_CHAIN_DRIVER_FORCE_POS		(0)
+#define RXON_RX_CHAIN_VALID_MSK			cpu_to_le16(0x7 << 1)
+#define RXON_RX_CHAIN_VALID_POS			(1)
+#define RXON_RX_CHAIN_FORCE_SEL_MSK		cpu_to_le16(0x7 << 4)
+#define RXON_RX_CHAIN_FORCE_SEL_POS		(4)
+#define RXON_RX_CHAIN_FORCE_MIMO_SEL_MSK	cpu_to_le16(0x7 << 7)
+#define RXON_RX_CHAIN_FORCE_MIMO_SEL_POS	(7)
+#define RXON_RX_CHAIN_CNT_MSK			cpu_to_le16(0x3 << 10)
+#define RXON_RX_CHAIN_CNT_POS			(10)
+#define RXON_RX_CHAIN_MIMO_CNT_MSK		cpu_to_le16(0x3 << 12)
+#define RXON_RX_CHAIN_MIMO_CNT_POS		(12)
+#define RXON_RX_CHAIN_MIMO_FORCE_MSK		cpu_to_le16(0x1 << 14)
+#define RXON_RX_CHAIN_MIMO_FORCE_POS		(14)
+
+/* rx_config flags */
+/* band & modulation selection */
+#define RXON_FLG_BAND_24G_MSK           cpu_to_le32(1 << 0)
+#define RXON_FLG_CCK_MSK                cpu_to_le32(1 << 1)
+/* auto detection enable */
+#define RXON_FLG_AUTO_DETECT_MSK        cpu_to_le32(1 << 2)
+/* TGg protection when tx */
+#define RXON_FLG_TGG_PROTECT_MSK        cpu_to_le32(1 << 3)
+/* cck short slot & preamble */
+#define RXON_FLG_SHORT_SLOT_MSK          cpu_to_le32(1 << 4)
+#define RXON_FLG_SHORT_PREAMBLE_MSK     cpu_to_le32(1 << 5)
+/* antenna selection */
+#define RXON_FLG_DIS_DIV_MSK            cpu_to_le32(1 << 7)
+#define RXON_FLG_ANT_SEL_MSK            cpu_to_le32(0x0f00)
+#define RXON_FLG_ANT_A_MSK              cpu_to_le32(1 << 8)
+#define RXON_FLG_ANT_B_MSK              cpu_to_le32(1 << 9)
+/* radar detection enable */
+#define RXON_FLG_RADAR_DETECT_MSK       cpu_to_le32(1 << 12)
+#define RXON_FLG_TGJ_NARROW_BAND_MSK    cpu_to_le32(1 << 13)
+/* rx response to host with 8-byte TSF
+* (according to ON_AIR deassertion) */
+#define RXON_FLG_TSF2HOST_MSK           cpu_to_le32(1 << 15)
+
+
+/* HT flags */
+#define RXON_FLG_CTRL_CHANNEL_LOC_POS		(22)
+#define RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK	cpu_to_le32(0x1 << 22)
+
+#define RXON_FLG_HT_OPERATING_MODE_POS		(23)
+
+#define RXON_FLG_HT_PROT_MSK			cpu_to_le32(0x1 << 23)
+#define RXON_FLG_HT40_PROT_MSK			cpu_to_le32(0x2 << 23)
+
+#define RXON_FLG_CHANNEL_MODE_POS		(25)
+#define RXON_FLG_CHANNEL_MODE_MSK		cpu_to_le32(0x3 << 25)
+
+/* channel mode */
+enum {
+	CHANNEL_MODE_LEGACY = 0,
+	CHANNEL_MODE_PURE_40 = 1,
+	CHANNEL_MODE_MIXED = 2,
+	CHANNEL_MODE_RESERVED = 3,
+};
+#define RXON_FLG_CHANNEL_MODE_LEGACY			\
+	cpu_to_le32(CHANNEL_MODE_LEGACY << RXON_FLG_CHANNEL_MODE_POS)
+#define RXON_FLG_CHANNEL_MODE_PURE_40			\
+	cpu_to_le32(CHANNEL_MODE_PURE_40 << RXON_FLG_CHANNEL_MODE_POS)
+#define RXON_FLG_CHANNEL_MODE_MIXED			\
+	cpu_to_le32(CHANNEL_MODE_MIXED << RXON_FLG_CHANNEL_MODE_POS)
+
+/* CTS to self (if spec allows) flag */
+#define RXON_FLG_SELF_CTS_EN			cpu_to_le32(0x1<<30)
+
+/* rx_config filter flags */
+/* accept all data frames */
+#define RXON_FILTER_PROMISC_MSK         cpu_to_le32(1 << 0)
+/* pass control & management to host */
+#define RXON_FILTER_CTL2HOST_MSK        cpu_to_le32(1 << 1)
+/* accept multi-cast */
+#define RXON_FILTER_ACCEPT_GRP_MSK      cpu_to_le32(1 << 2)
+/* don't decrypt uni-cast frames */
+#define RXON_FILTER_DIS_DECRYPT_MSK     cpu_to_le32(1 << 3)
+/* don't decrypt multi-cast frames */
+#define RXON_FILTER_DIS_GRP_DECRYPT_MSK cpu_to_le32(1 << 4)
+/* STA is associated */
+#define RXON_FILTER_ASSOC_MSK           cpu_to_le32(1 << 5)
+/* transfer to host non bssid beacons in associated state */
+#define RXON_FILTER_BCON_AWARE_MSK      cpu_to_le32(1 << 6)
+
+/**
+ * REPLY_RXON = 0x10 (command, has simple generic response)
+ *
+ * RXON tunes the radio tuner to a service channel, and sets up a number
+ * of parameters that are used primarily for Rx, but also for Tx operations.
+ *
+ * NOTE:  When tuning to a new channel, driver must set the
+ *        RXON_FILTER_ASSOC_MSK to 0.  This will clear station-dependent
+ *        info within the device, including the station tables, tx retry
+ *        rate tables, and txpower tables.  Driver must build a new station
+ *        table and txpower table before transmitting anything on the RXON
+ *        channel.
+ *
+ * NOTE:  All RXONs wipe clean the internal txpower table.  Driver must
+ *        issue a new REPLY_TX_PWR_TABLE_CMD after each REPLY_RXON (0x10),
+ *        regardless of whether RXON_FILTER_ASSOC_MSK is set.
+ */
+
+struct iwl3945_rxon_cmd {
+	u8 node_addr[6];
+	__le16 reserved1;
+	u8 bssid_addr[6];
+	__le16 reserved2;
+	u8 wlap_bssid_addr[6];
+	__le16 reserved3;
+	u8 dev_type;
+	u8 air_propagation;
+	__le16 reserved4;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 assoc_id;
+	__le32 flags;
+	__le32 filter_flags;
+	__le16 channel;
+	__le16 reserved5;
+} __packed;
+
+struct iwl4965_rxon_cmd {
+	u8 node_addr[6];
+	__le16 reserved1;
+	u8 bssid_addr[6];
+	__le16 reserved2;
+	u8 wlap_bssid_addr[6];
+	__le16 reserved3;
+	u8 dev_type;
+	u8 air_propagation;
+	__le16 rx_chain;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 assoc_id;
+	__le32 flags;
+	__le32 filter_flags;
+	__le16 channel;
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+} __packed;
+
+/* Create a common rxon cmd which will be typecast into the 3945 or 4965
+ * specific rxon cmd, depending on where it is called from.
+ */
+struct iwl_legacy_rxon_cmd {
+	u8 node_addr[6];
+	__le16 reserved1;
+	u8 bssid_addr[6];
+	__le16 reserved2;
+	u8 wlap_bssid_addr[6];
+	__le16 reserved3;
+	u8 dev_type;
+	u8 air_propagation;
+	__le16 rx_chain;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 assoc_id;
+	__le32 flags;
+	__le32 filter_flags;
+	__le16 channel;
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+	u8 reserved4;
+	u8 reserved5;
+} __packed;
+
+
+/*
+ * REPLY_RXON_ASSOC = 0x11 (command, has simple generic response)
+ */
+struct iwl3945_rxon_assoc_cmd {
+	__le32 flags;
+	__le32 filter_flags;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 reserved;
+} __packed;
+
+struct iwl4965_rxon_assoc_cmd {
+	__le32 flags;
+	__le32 filter_flags;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+	__le16 rx_chain_select_flags;
+	__le16 reserved;
+} __packed;
+
+#define IWL_CONN_MAX_LISTEN_INTERVAL	10
+#define IWL_MAX_UCODE_BEACON_INTERVAL	4 /* 4096 */
+#define IWL39_MAX_UCODE_BEACON_INTERVAL	1 /* 1024 */
+
+/*
+ * REPLY_RXON_TIMING = 0x14 (command, has simple generic response)
+ */
+struct iwl_rxon_time_cmd {
+	__le64 timestamp;
+	__le16 beacon_interval;
+	__le16 atim_window;
+	__le32 beacon_init_val;
+	__le16 listen_interval;
+	u8 dtim_period;
+	u8 delta_cp_bss_tbtts;
+} __packed;
+
+/*
+ * REPLY_CHANNEL_SWITCH = 0x72 (command, has simple generic response)
+ */
+struct iwl3945_channel_switch_cmd {
+	u8 band;
+	u8 expect_beacon;
+	__le16 channel;
+	__le32 rxon_flags;
+	__le32 rxon_filter_flags;
+	__le32 switch_time;
+	struct iwl3945_power_per_rate power[IWL_MAX_RATES];
+} __packed;
+
+struct iwl4965_channel_switch_cmd {
+	u8 band;
+	u8 expect_beacon;
+	__le16 channel;
+	__le32 rxon_flags;
+	__le32 rxon_filter_flags;
+	__le32 switch_time;
+	struct iwl4965_tx_power_db tx_power;
+} __packed;
+
+/*
+ * CHANNEL_SWITCH_NOTIFICATION = 0x73 (notification only, not a command)
+ */
+struct iwl_csa_notification {
+	__le16 band;
+	__le16 channel;
+	__le32 status;		/* 0 - OK, 1 - fail */
+} __packed;
+
+/******************************************************************************
+ * (2)
+ * Quality-of-Service (QOS) Commands & Responses:
+ *
+ *****************************************************************************/
+
+/**
+ * struct iwl_ac_qos -- QOS timing params for REPLY_QOS_PARAM
+ * One for each of 4 EDCA access categories in struct iwl_qosparam_cmd
+ *
+ * @cw_min: Contention window, start value in numbers of slots.
+ *          Should be a power-of-2, minus 1.  Device's default is 0x0f.
+ * @cw_max: Contention window, max value in numbers of slots.
+ *          Should be a power-of-2, minus 1.  Device's default is 0x3f.
+ * @aifsn:  Number of slots in Arbitration Interframe Space (before
+ *          performing random backoff timing prior to Tx).  Device default 1.
+ * @edca_txop:  Length of Tx opportunity, in uSecs.  Device default is 0.
+ *
+ * Device will automatically increase contention window by (2*CW) + 1 for each
+ * transmission retry.  Device uses cw_max as a bit mask, ANDed with new CW
+ * value, to cap the CW value.
+ */
+struct iwl_ac_qos {
+	__le16 cw_min;
+	__le16 cw_max;
+	u8 aifsn;
+	u8 reserved1;
+	__le16 edca_txop;
+} __packed;
+
+/* QoS flags defines */
+#define QOS_PARAM_FLG_UPDATE_EDCA_MSK	cpu_to_le32(0x01)
+#define QOS_PARAM_FLG_TGN_MSK		cpu_to_le32(0x02)
+#define QOS_PARAM_FLG_TXOP_TYPE_MSK	cpu_to_le32(0x10)
+
+/* Number of Access Categories (AC) (EDCA), queues 0..3 */
+#define AC_NUM                4
+
+/*
+ * REPLY_QOS_PARAM = 0x13 (command, has simple generic response)
+ *
+ * This command sets up timings for each of the 4 prioritized EDCA Tx FIFOs
+ * 0: Background, 1: Best Effort, 2: Video, 3: Voice.
+ */
+struct iwl_qosparam_cmd {
+	__le32 qos_flags;
+	struct iwl_ac_qos ac[AC_NUM];
+} __packed;
+
+/******************************************************************************
+ * (3)
+ * Add/Modify Stations Commands & Responses:
+ *
+ *****************************************************************************/
+/*
+ * Multi station support
+ */
+
+/* Special, dedicated locations within device's station table */
+#define	IWL_AP_ID		0
+#define	IWL_STA_ID		2
+#define	IWL3945_BROADCAST_ID	24
+#define IWL3945_STATION_COUNT	25
+#define IWL4965_BROADCAST_ID	31
+#define	IWL4965_STATION_COUNT	32
+
+#define	IWL_STATION_COUNT	32 	/* MAX(3945,4965)*/
+#define	IWL_INVALID_STATION	255
+
+#define STA_FLG_TX_RATE_MSK		cpu_to_le32(1 << 2)
+#define STA_FLG_PWR_SAVE_MSK		cpu_to_le32(1 << 8)
+#define STA_FLG_RTS_MIMO_PROT_MSK	cpu_to_le32(1 << 17)
+#define STA_FLG_AGG_MPDU_8US_MSK	cpu_to_le32(1 << 18)
+#define STA_FLG_MAX_AGG_SIZE_POS	(19)
+#define STA_FLG_MAX_AGG_SIZE_MSK	cpu_to_le32(3 << 19)
+#define STA_FLG_HT40_EN_MSK		cpu_to_le32(1 << 21)
+#define STA_FLG_MIMO_DIS_MSK		cpu_to_le32(1 << 22)
+#define STA_FLG_AGG_MPDU_DENSITY_POS	(23)
+#define STA_FLG_AGG_MPDU_DENSITY_MSK	cpu_to_le32(7 << 23)
+
+/* Use in mode field.  1: modify existing entry, 0: add new station entry */
+#define STA_CONTROL_MODIFY_MSK		0x01
+
+/* key flags __le16*/
+#define STA_KEY_FLG_ENCRYPT_MSK	cpu_to_le16(0x0007)
+#define STA_KEY_FLG_NO_ENC	cpu_to_le16(0x0000)
+#define STA_KEY_FLG_WEP		cpu_to_le16(0x0001)
+#define STA_KEY_FLG_CCMP	cpu_to_le16(0x0002)
+#define STA_KEY_FLG_TKIP	cpu_to_le16(0x0003)
+
+#define STA_KEY_FLG_KEYID_POS	8
+#define STA_KEY_FLG_INVALID	cpu_to_le16(0x0800)
+/* wep key is either from global key (0) or from station info array (1) */
+#define STA_KEY_FLG_MAP_KEY_MSK	cpu_to_le16(0x0008)
+
+/* wep key in STA: 5-bytes (0) or 13-bytes (1) */
+#define STA_KEY_FLG_KEY_SIZE_MSK	cpu_to_le16(0x1000)
+#define STA_KEY_MULTICAST_MSK		cpu_to_le16(0x4000)
+#define STA_KEY_MAX_NUM		8
+
+/* Flags indicate whether to modify vs. don't change various station params */
+#define	STA_MODIFY_KEY_MASK		0x01
+#define	STA_MODIFY_TID_DISABLE_TX	0x02
+#define	STA_MODIFY_TX_RATE_MSK		0x04
+#define STA_MODIFY_ADDBA_TID_MSK	0x08
+#define STA_MODIFY_DELBA_TID_MSK	0x10
+#define STA_MODIFY_SLEEP_TX_COUNT_MSK	0x20
+
+/* Receiver address (actually, Rx station's index into station table),
+ * combined with Traffic ID (QOS priority), in format used by Tx Scheduler */
+#define BUILD_RAxTID(sta_id, tid)	(((sta_id) << 4) + (tid))
+
+struct iwl4965_keyinfo {
+	__le16 key_flags;
+	u8 tkip_rx_tsc_byte2;	/* TSC[2] for key mix ph1 detection */
+	u8 reserved1;
+	__le16 tkip_rx_ttak[5];	/* 10-byte unicast TKIP TTAK */
+	u8 key_offset;
+	u8 reserved2;
+	u8 key[16];		/* 16-byte unicast decryption key */
+} __packed;
+
+/**
+ * struct sta_id_modify
+ * @addr[ETH_ALEN]: station's MAC address
+ * @sta_id: index of station in uCode's station table
+ * @modify_mask: STA_MODIFY_*, 1: modify, 0: don't change
+ *
+ * Driver selects unused table index when adding new station,
+ * or the index to a pre-existing station entry when modifying that station.
+ * Some indexes have special purposes (IWL_AP_ID, index 0, is for AP).
+ *
+ * modify_mask flags select which parameters to modify vs. leave alone.
+ */
+struct sta_id_modify {
+	u8 addr[ETH_ALEN];
+	__le16 reserved1;
+	u8 sta_id;
+	u8 modify_mask;
+	__le16 reserved2;
+} __packed;
+
+/*
+ * REPLY_ADD_STA = 0x18 (command)
+ *
+ * The device contains an internal table of per-station information,
+ * with info on security keys, aggregation parameters, and Tx rates for
+ * initial Tx attempt and any retries (4965 devices uses
+ * REPLY_TX_LINK_QUALITY_CMD,
+ * 3945 uses REPLY_RATE_SCALE to set up rate tables).
+ *
+ * REPLY_ADD_STA sets up the table entry for one station, either creating
+ * a new entry, or modifying a pre-existing one.
+ *
+ * NOTE:  RXON command (without "associated" bit set) wipes the station table
+ *        clean.  Moving into RF_KILL state does this also.  Driver must set up
+ *        new station table before transmitting anything on the RXON channel
+ *        (except active scans or active measurements; those commands carry
+ *        their own txpower/rate setup data).
+ *
+ *        When getting started on a new channel, driver must set up the
+ *        IWL_BROADCAST_ID entry (last entry in the table).  For a client
+ *        station in a BSS, once an AP is selected, driver sets up the AP STA
+ *        in the IWL_AP_ID entry (1st entry in the table).  BROADCAST and AP
+ *        are all that are needed for a BSS client station.  If the device is
+ *        used as AP, or in an IBSS network, driver must set up station table
+ *        entries for all STAs in network, starting with index IWL_STA_ID.
+ */
+
+struct iwl3945_addsta_cmd {
+	u8 mode;		/* 1: modify existing, 0: add new station */
+	u8 reserved[3];
+	struct sta_id_modify sta;
+	struct iwl4965_keyinfo key;
+	__le32 station_flags;		/* STA_FLG_* */
+	__le32 station_flags_msk;	/* STA_FLG_* */
+
+	/* bit field to disable (1) or enable (0) Tx for Traffic ID (TID)
+	 * corresponding to bit (e.g. bit 5 controls TID 5).
+	 * Set modify_mask bit STA_MODIFY_TID_DISABLE_TX to use this field. */
+	__le16 tid_disable_tx;
+
+	__le16 rate_n_flags;
+
+	/* TID for which to add block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	u8 add_immediate_ba_tid;
+
+	/* TID for which to remove block-ack support.
+	 * Set modify_mask bit STA_MODIFY_DELBA_TID_MSK to use this field. */
+	u8 remove_immediate_ba_tid;
+
+	/* Starting Sequence Number for added block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	__le16 add_immediate_ba_ssn;
+} __packed;
+
+struct iwl4965_addsta_cmd {
+	u8 mode;		/* 1: modify existing, 0: add new station */
+	u8 reserved[3];
+	struct sta_id_modify sta;
+	struct iwl4965_keyinfo key;
+	__le32 station_flags;		/* STA_FLG_* */
+	__le32 station_flags_msk;	/* STA_FLG_* */
+
+	/* bit field to disable (1) or enable (0) Tx for Traffic ID (TID)
+	 * corresponding to bit (e.g. bit 5 controls TID 5).
+	 * Set modify_mask bit STA_MODIFY_TID_DISABLE_TX to use this field. */
+	__le16 tid_disable_tx;
+
+	__le16	reserved1;
+
+	/* TID for which to add block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	u8 add_immediate_ba_tid;
+
+	/* TID for which to remove block-ack support.
+	 * Set modify_mask bit STA_MODIFY_DELBA_TID_MSK to use this field. */
+	u8 remove_immediate_ba_tid;
+
+	/* Starting Sequence Number for added block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	__le16 add_immediate_ba_ssn;
+
+	/*
+	 * Number of packets OK to transmit to station even though
+	 * it is asleep -- used to synchronise PS-poll and u-APSD
+	 * responses while ucode keeps track of STA sleep state.
+	 */
+	__le16 sleep_tx_count;
+
+	__le16 reserved2;
+} __packed;
+
+/* Wrapper struct for 3945 and 4965 addsta_cmd structures */
+struct iwl_legacy_addsta_cmd {
+	u8 mode;		/* 1: modify existing, 0: add new station */
+	u8 reserved[3];
+	struct sta_id_modify sta;
+	struct iwl4965_keyinfo key;
+	__le32 station_flags;		/* STA_FLG_* */
+	__le32 station_flags_msk;	/* STA_FLG_* */
+
+	/* bit field to disable (1) or enable (0) Tx for Traffic ID (TID)
+	 * corresponding to bit (e.g. bit 5 controls TID 5).
+	 * Set modify_mask bit STA_MODIFY_TID_DISABLE_TX to use this field. */
+	__le16 tid_disable_tx;
+
+	__le16	rate_n_flags;		/* 3945 only */
+
+	/* TID for which to add block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	u8 add_immediate_ba_tid;
+
+	/* TID for which to remove block-ack support.
+	 * Set modify_mask bit STA_MODIFY_DELBA_TID_MSK to use this field. */
+	u8 remove_immediate_ba_tid;
+
+	/* Starting Sequence Number for added block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	__le16 add_immediate_ba_ssn;
+
+	/*
+	 * Number of packets OK to transmit to station even though
+	 * it is asleep -- used to synchronise PS-poll and u-APSD
+	 * responses while ucode keeps track of STA sleep state.
+	 */
+	__le16 sleep_tx_count;
+
+	__le16 reserved2;
+} __packed;
+
+
+#define ADD_STA_SUCCESS_MSK		0x1
+#define ADD_STA_NO_ROOM_IN_TABLE	0x2
+#define ADD_STA_NO_BLOCK_ACK_RESOURCE	0x4
+#define ADD_STA_MODIFY_NON_EXIST_STA	0x8
+/*
+ * REPLY_ADD_STA = 0x18 (response)
+ */
+struct iwl_add_sta_resp {
+	u8 status;	/* ADD_STA_* */
+} __packed;
+
+#define REM_STA_SUCCESS_MSK              0x1
+/*
+ *  REPLY_REM_STA = 0x19 (response)
+ */
+struct iwl_rem_sta_resp {
+	u8 status;
+} __packed;
+
+/*
+ *  REPLY_REM_STA = 0x19 (command)
+ */
+struct iwl_rem_sta_cmd {
+	u8 num_sta;     /* number of removed stations */
+	u8 reserved[3];
+	u8 addr[ETH_ALEN]; /* MAC addr of the first station */
+	u8 reserved2[2];
+} __packed;
+
+#define IWL_TX_FIFO_BK_MSK		cpu_to_le32(BIT(0))
+#define IWL_TX_FIFO_BE_MSK		cpu_to_le32(BIT(1))
+#define IWL_TX_FIFO_VI_MSK		cpu_to_le32(BIT(2))
+#define IWL_TX_FIFO_VO_MSK		cpu_to_le32(BIT(3))
+#define IWL_AGG_TX_QUEUE_MSK		cpu_to_le32(0xffc00)
+
+#define IWL_DROP_SINGLE		0
+#define IWL_DROP_SELECTED	1
+#define IWL_DROP_ALL		2
+
+/*
+ * REPLY_WEP_KEY = 0x20
+ */
+struct iwl_wep_key {
+	u8 key_index;
+	u8 key_offset;
+	u8 reserved1[2];
+	u8 key_size;
+	u8 reserved2[3];
+	u8 key[16];
+} __packed;
+
+struct iwl_wep_cmd {
+	u8 num_keys;
+	u8 global_key_type;
+	u8 flags;
+	u8 reserved;
+	struct iwl_wep_key key[0];
+} __packed;
+
+#define WEP_KEY_WEP_TYPE 1
+#define WEP_KEYS_MAX 4
+#define WEP_INVALID_OFFSET 0xff
+#define WEP_KEY_LEN_64 5
+#define WEP_KEY_LEN_128 13
+
+/******************************************************************************
+ * (4)
+ * Rx Responses:
+ *
+ *****************************************************************************/
+
+#define RX_RES_STATUS_NO_CRC32_ERROR	cpu_to_le32(1 << 0)
+#define RX_RES_STATUS_NO_RXE_OVERFLOW	cpu_to_le32(1 << 1)
+
+#define RX_RES_PHY_FLAGS_BAND_24_MSK	cpu_to_le16(1 << 0)
+#define RX_RES_PHY_FLAGS_MOD_CCK_MSK		cpu_to_le16(1 << 1)
+#define RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK	cpu_to_le16(1 << 2)
+#define RX_RES_PHY_FLAGS_NARROW_BAND_MSK	cpu_to_le16(1 << 3)
+#define RX_RES_PHY_FLAGS_ANTENNA_MSK		0xf0
+#define RX_RES_PHY_FLAGS_ANTENNA_POS		4
+
+#define RX_RES_STATUS_SEC_TYPE_MSK	(0x7 << 8)
+#define RX_RES_STATUS_SEC_TYPE_NONE	(0x0 << 8)
+#define RX_RES_STATUS_SEC_TYPE_WEP	(0x1 << 8)
+#define RX_RES_STATUS_SEC_TYPE_CCMP	(0x2 << 8)
+#define RX_RES_STATUS_SEC_TYPE_TKIP	(0x3 << 8)
+#define	RX_RES_STATUS_SEC_TYPE_ERR	(0x7 << 8)
+
+#define RX_RES_STATUS_STATION_FOUND	(1<<6)
+#define RX_RES_STATUS_NO_STATION_INFO_MISMATCH	(1<<7)
+
+#define RX_RES_STATUS_DECRYPT_TYPE_MSK	(0x3 << 11)
+#define RX_RES_STATUS_NOT_DECRYPT	(0x0 << 11)
+#define RX_RES_STATUS_DECRYPT_OK	(0x3 << 11)
+#define RX_RES_STATUS_BAD_ICV_MIC	(0x1 << 11)
+#define RX_RES_STATUS_BAD_KEY_TTAK	(0x2 << 11)
+
+#define RX_MPDU_RES_STATUS_ICV_OK	(0x20)
+#define RX_MPDU_RES_STATUS_MIC_OK	(0x40)
+#define RX_MPDU_RES_STATUS_TTAK_OK	(1 << 7)
+#define RX_MPDU_RES_STATUS_DEC_DONE_MSK	(0x800)
+
+
+struct iwl3945_rx_frame_stats {
+	u8 phy_count;
+	u8 id;
+	u8 rssi;
+	u8 agc;
+	__le16 sig_avg;
+	__le16 noise_diff;
+	u8 payload[0];
+} __packed;
+
+struct iwl3945_rx_frame_hdr {
+	__le16 channel;
+	__le16 phy_flags;
+	u8 reserved1;
+	u8 rate;
+	__le16 len;
+	u8 payload[0];
+} __packed;
+
+struct iwl3945_rx_frame_end {
+	__le32 status;
+	__le64 timestamp;
+	__le32 beacon_timestamp;
+} __packed;
+
+/*
+ * REPLY_3945_RX = 0x1b (response only, not a command)
+ *
+ * NOTE:  DO NOT dereference from casts to this structure
+ * It is provided only for calculating minimum data set size.
+ * The actual offsets of the hdr and end are dynamic based on
+ * stats.phy_count
+ */
+struct iwl3945_rx_frame {
+	struct iwl3945_rx_frame_stats stats;
+	struct iwl3945_rx_frame_hdr hdr;
+	struct iwl3945_rx_frame_end end;
+} __packed;
+
+#define IWL39_RX_FRAME_SIZE	(4 + sizeof(struct iwl3945_rx_frame))
+
+/* Fixed (non-configurable) rx data from phy */
+
+#define IWL49_RX_RES_PHY_CNT 14
+#define IWL49_RX_PHY_FLAGS_ANTENNAE_OFFSET	(4)
+#define IWL49_RX_PHY_FLAGS_ANTENNAE_MASK	(0x70)
+#define IWL49_AGC_DB_MASK			(0x3f80)	/* MASK(7,13) */
+#define IWL49_AGC_DB_POS			(7)
+struct iwl4965_rx_non_cfg_phy {
+	__le16 ant_selection;	/* ant A bit 4, ant B bit 5, ant C bit 6 */
+	__le16 agc_info;	/* agc code 0:6, agc dB 7:13, reserved 14:15 */
+	u8 rssi_info[6];	/* we use even entries, 0/2/4 for A/B/C rssi */
+	u8 pad[0];
+} __packed;
+
+
+/*
+ * REPLY_RX = 0xc3 (response only, not a command)
+ * Used only for legacy (non 11n) frames.
+ */
+struct iwl_rx_phy_res {
+	u8 non_cfg_phy_cnt;     /* non configurable DSP phy data byte count */
+	u8 cfg_phy_cnt;		/* configurable DSP phy data byte count */
+	u8 stat_id;		/* configurable DSP phy data set ID */
+	u8 reserved1;
+	__le64 timestamp;	/* TSF at on air rise */
+	__le32 beacon_time_stamp; /* beacon at on-air rise */
+	__le16 phy_flags;	/* general phy flags: band, modulation, ... */
+	__le16 channel;		/* channel number */
+	u8 non_cfg_phy_buf[32]; /* for various implementations of non_cfg_phy */
+	__le32 rate_n_flags;	/* RATE_MCS_* */
+	__le16 byte_count;	/* frame's byte-count */
+	__le16 frame_time;	/* frame's time on the air */
+} __packed;
+
+struct iwl_rx_mpdu_res_start {
+	__le16 byte_count;
+	__le16 reserved;
+} __packed;
+
+
+/******************************************************************************
+ * (5)
+ * Tx Commands & Responses:
+ *
+ * Driver must place each REPLY_TX command into one of the prioritized Tx
+ * queues in host DRAM, shared between driver and device (see comments for
+ * SCD registers and Tx/Rx Queues).  When the device's Tx scheduler and uCode
+ * are preparing to transmit, the device pulls the Tx command over the PCI
+ * bus via one of the device's Tx DMA channels, to fill an internal FIFO
+ * from which data will be transmitted.
+ *
+ * uCode handles all timing and protocol related to control frames
+ * (RTS/CTS/ACK), based on flags in the Tx command.  uCode and Tx scheduler
+ * handle reception of block-acks; uCode updates the host driver via
+ * REPLY_COMPRESSED_BA.
+ *
+ * uCode handles retrying Tx when an ACK is expected but not received.
+ * This includes trying lower data rates than the one requested in the Tx
+ * command, as set up by the REPLY_RATE_SCALE (for 3945) or
+ * REPLY_TX_LINK_QUALITY_CMD (4965).
+ *
+ * Driver sets up transmit power for various rates via REPLY_TX_PWR_TABLE_CMD.
+ * This command must be executed after every RXON command, before Tx can occur.
+ *****************************************************************************/
+
+/* REPLY_TX Tx flags field */
+
+/*
+ * 1: Use Request-To-Send protocol before this frame.
+ * Mutually exclusive vs. TX_CMD_FLG_CTS_MSK.
+ */
+#define TX_CMD_FLG_RTS_MSK cpu_to_le32(1 << 1)
+
+/*
+ * 1: Transmit Clear-To-Send to self before this frame.
+ * Driver should set this for AUTH/DEAUTH/ASSOC-REQ/REASSOC mgmnt frames.
+ * Mutually exclusive vs. TX_CMD_FLG_RTS_MSK.
+ */
+#define TX_CMD_FLG_CTS_MSK cpu_to_le32(1 << 2)
+
+/* 1: Expect ACK from receiving station
+ * 0: Don't expect ACK (MAC header's duration field s/b 0)
+ * Set this for unicast frames, but not broadcast/multicast. */
+#define TX_CMD_FLG_ACK_MSK cpu_to_le32(1 << 3)
+
+/* For 4965 devices:
+ * 1: Use rate scale table (see REPLY_TX_LINK_QUALITY_CMD).
+ *    Tx command's initial_rate_index indicates first rate to try;
+ *    uCode walks through table for additional Tx attempts.
+ * 0: Use Tx rate/MCS from Tx command's rate_n_flags field.
+ *    This rate will be used for all Tx attempts; it will not be scaled. */
+#define TX_CMD_FLG_STA_RATE_MSK cpu_to_le32(1 << 4)
+
+/* 1: Expect immediate block-ack.
+ * Set when Txing a block-ack request frame.  Also set TX_CMD_FLG_ACK_MSK. */
+#define TX_CMD_FLG_IMM_BA_RSP_MASK  cpu_to_le32(1 << 6)
+
+/*
+ * 1: Frame requires full Tx-Op protection.
+ * Set this if either RTS or CTS Tx Flag gets set.
+ */
+#define TX_CMD_FLG_FULL_TXOP_PROT_MSK cpu_to_le32(1 << 7)
+
+/* Tx antenna selection field; used only for 3945, reserved (0) for 4965 devices.
+ * Set field to "0" to allow 3945 uCode to select antenna (normal usage). */
+#define TX_CMD_FLG_ANT_SEL_MSK cpu_to_le32(0xf00)
+#define TX_CMD_FLG_ANT_A_MSK cpu_to_le32(1 << 8)
+#define TX_CMD_FLG_ANT_B_MSK cpu_to_le32(1 << 9)
+
+/* 1: uCode overrides sequence control field in MAC header.
+ * 0: Driver provides sequence control field in MAC header.
+ * Set this for management frames, non-QOS data frames, non-unicast frames,
+ * and also in Tx command embedded in REPLY_SCAN_CMD for active scans. */
+#define TX_CMD_FLG_SEQ_CTL_MSK cpu_to_le32(1 << 13)
+
+/* 1: This frame is non-last MPDU; more fragments are coming.
+ * 0: Last fragment, or not using fragmentation. */
+#define TX_CMD_FLG_MORE_FRAG_MSK cpu_to_le32(1 << 14)
+
+/* 1: uCode calculates and inserts Timestamp Function (TSF) in outgoing frame.
+ * 0: No TSF required in outgoing frame.
+ * Set this for transmitting beacons and probe responses. */
+#define TX_CMD_FLG_TSF_MSK cpu_to_le32(1 << 16)
+
+/* 1: Driver inserted 2 bytes pad after the MAC header, for (required) dword
+ *    alignment of frame's payload data field.
+ * 0: No pad
+ * Set this for MAC headers with 26 or 30 bytes, i.e. those with QOS or ADDR4
+ * field (but not both).  Driver must align frame data (i.e. data following
+ * MAC header) to DWORD boundary. */
+#define TX_CMD_FLG_MH_PAD_MSK cpu_to_le32(1 << 20)
+
+/* accelerate aggregation support
+ * 0 - no CCMP encryption; 1 - CCMP encryption */
+#define TX_CMD_FLG_AGG_CCMP_MSK cpu_to_le32(1 << 22)
+
+/* HCCA-AP - disable duration overwriting. */
+#define TX_CMD_FLG_DUR_MSK cpu_to_le32(1 << 25)
+
+
+/*
+ * TX command security control
+ */
+#define TX_CMD_SEC_WEP		0x01
+#define TX_CMD_SEC_CCM		0x02
+#define TX_CMD_SEC_TKIP		0x03
+#define TX_CMD_SEC_MSK		0x03
+#define TX_CMD_SEC_SHIFT	6
+#define TX_CMD_SEC_KEY128	0x08
+
+/*
+ * security overhead sizes
+ */
+#define WEP_IV_LEN 4
+#define WEP_ICV_LEN 4
+#define CCMP_MIC_LEN 8
+#define TKIP_ICV_LEN 4
+
+/*
+ * REPLY_TX = 0x1c (command)
+ */
+
+struct iwl3945_tx_cmd {
+	/*
+	 * MPDU byte count:
+	 * MAC header (24/26/30/32 bytes) + 2 bytes pad if 26/30 header size,
+	 * + 8 byte IV for CCM or TKIP (not used for WEP)
+	 * + Data payload
+	 * + 8-byte MIC (not used for CCM/WEP)
+	 * NOTE:  Does not include Tx command bytes, post-MAC pad bytes,
+	 *        MIC (CCM) 8 bytes, ICV (WEP/TKIP/CKIP) 4 bytes, CRC 4 bytes.i
+	 * Range: 14-2342 bytes.
+	 */
+	__le16 len;
+
+	/*
+	 * MPDU or MSDU byte count for next frame.
+	 * Used for fragmentation and bursting, but not 11n aggregation.
+	 * Same as "len", but for next frame.  Set to 0 if not applicable.
+	 */
+	__le16 next_frame_len;
+
+	__le32 tx_flags;	/* TX_CMD_FLG_* */
+
+	u8 rate;
+
+	/* Index of recipient station in uCode's station table */
+	u8 sta_id;
+	u8 tid_tspec;
+	u8 sec_ctl;
+	u8 key[16];
+	union {
+		u8 byte[8];
+		__le16 word[4];
+		__le32 dw[2];
+	} tkip_mic;
+	__le32 next_frame_info;
+	union {
+		__le32 life_time;
+		__le32 attempt;
+	} stop_time;
+	u8 supp_rates[2];
+	u8 rts_retry_limit;	/*byte 50 */
+	u8 data_retry_limit;	/*byte 51 */
+	union {
+		__le16 pm_frame_timeout;
+		__le16 attempt_duration;
+	} timeout;
+
+	/*
+	 * Duration of EDCA burst Tx Opportunity, in 32-usec units.
+	 * Set this if txop time is not specified by HCCA protocol (e.g. by AP).
+	 */
+	__le16 driver_txop;
+
+	/*
+	 * MAC header goes here, followed by 2 bytes padding if MAC header
+	 * length is 26 or 30 bytes, followed by payload data
+	 */
+	u8 payload[0];
+	struct ieee80211_hdr hdr[0];
+} __packed;
+
+/*
+ * REPLY_TX = 0x1c (response)
+ */
+struct iwl3945_tx_resp {
+	u8 failure_rts;
+	u8 failure_frame;
+	u8 bt_kill_count;
+	u8 rate;
+	__le32 wireless_media_time;
+	__le32 status;		/* TX status */
+} __packed;
+
+
+/*
+ * 4965 uCode updates these Tx attempt count values in host DRAM.
+ * Used for managing Tx retries when expecting block-acks.
+ * Driver should set these fields to 0.
+ */
+struct iwl_dram_scratch {
+	u8 try_cnt;		/* Tx attempts */
+	u8 bt_kill_cnt;		/* Tx attempts blocked by Bluetooth device */
+	__le16 reserved;
+} __packed;
+
+struct iwl_tx_cmd {
+	/*
+	 * MPDU byte count:
+	 * MAC header (24/26/30/32 bytes) + 2 bytes pad if 26/30 header size,
+	 * + 8 byte IV for CCM or TKIP (not used for WEP)
+	 * + Data payload
+	 * + 8-byte MIC (not used for CCM/WEP)
+	 * NOTE:  Does not include Tx command bytes, post-MAC pad bytes,
+	 *        MIC (CCM) 8 bytes, ICV (WEP/TKIP/CKIP) 4 bytes, CRC 4 bytes.i
+	 * Range: 14-2342 bytes.
+	 */
+	__le16 len;
+
+	/*
+	 * MPDU or MSDU byte count for next frame.
+	 * Used for fragmentation and bursting, but not 11n aggregation.
+	 * Same as "len", but for next frame.  Set to 0 if not applicable.
+	 */
+	__le16 next_frame_len;
+
+	__le32 tx_flags;	/* TX_CMD_FLG_* */
+
+	/* uCode may modify this field of the Tx command (in host DRAM!).
+	 * Driver must also set dram_lsb_ptr and dram_msb_ptr in this cmd. */
+	struct iwl_dram_scratch scratch;
+
+	/* Rate for *all* Tx attempts, if TX_CMD_FLG_STA_RATE_MSK is cleared. */
+	__le32 rate_n_flags;	/* RATE_MCS_* */
+
+	/* Index of destination station in uCode's station table */
+	u8 sta_id;
+
+	/* Type of security encryption:  CCM or TKIP */
+	u8 sec_ctl;		/* TX_CMD_SEC_* */
+
+	/*
+	 * Index into rate table (see REPLY_TX_LINK_QUALITY_CMD) for initial
+	 * Tx attempt, if TX_CMD_FLG_STA_RATE_MSK is set.  Normally "0" for
+	 * data frames, this field may be used to selectively reduce initial
+	 * rate (via non-0 value) for special frames (e.g. management), while
+	 * still supporting rate scaling for all frames.
+	 */
+	u8 initial_rate_index;
+	u8 reserved;
+	u8 key[16];
+	__le16 next_frame_flags;
+	__le16 reserved2;
+	union {
+		__le32 life_time;
+		__le32 attempt;
+	} stop_time;
+
+	/* Host DRAM physical address pointer to "scratch" in this command.
+	 * Must be dword aligned.  "0" in dram_lsb_ptr disables usage. */
+	__le32 dram_lsb_ptr;
+	u8 dram_msb_ptr;
+
+	u8 rts_retry_limit;	/*byte 50 */
+	u8 data_retry_limit;	/*byte 51 */
+	u8 tid_tspec;
+	union {
+		__le16 pm_frame_timeout;
+		__le16 attempt_duration;
+	} timeout;
+
+	/*
+	 * Duration of EDCA burst Tx Opportunity, in 32-usec units.
+	 * Set this if txop time is not specified by HCCA protocol (e.g. by AP).
+	 */
+	__le16 driver_txop;
+
+	/*
+	 * MAC header goes here, followed by 2 bytes padding if MAC header
+	 * length is 26 or 30 bytes, followed by payload data
+	 */
+	u8 payload[0];
+	struct ieee80211_hdr hdr[0];
+} __packed;
+
+/* TX command response is sent after *3945* transmission attempts.
+ *
+ * NOTES:
+ *
+ * TX_STATUS_FAIL_NEXT_FRAG
+ *
+ * If the fragment flag in the MAC header for the frame being transmitted
+ * is set and there is insufficient time to transmit the next frame, the
+ * TX status will be returned with 'TX_STATUS_FAIL_NEXT_FRAG'.
+ *
+ * TX_STATUS_FIFO_UNDERRUN
+ *
+ * Indicates the host did not provide bytes to the FIFO fast enough while
+ * a TX was in progress.
+ *
+ * TX_STATUS_FAIL_MGMNT_ABORT
+ *
+ * This status is only possible if the ABORT ON MGMT RX parameter was
+ * set to true with the TX command.
+ *
+ * If the MSB of the status parameter is set then an abort sequence is
+ * required.  This sequence consists of the host activating the TX Abort
+ * control line, and then waiting for the TX Abort command response.  This
+ * indicates that a the device is no longer in a transmit state, and that the
+ * command FIFO has been cleared.  The host must then deactivate the TX Abort
+ * control line.  Receiving is still allowed in this case.
+ */
+enum {
+	TX_3945_STATUS_SUCCESS = 0x01,
+	TX_3945_STATUS_DIRECT_DONE = 0x02,
+	TX_3945_STATUS_FAIL_SHORT_LIMIT = 0x82,
+	TX_3945_STATUS_FAIL_LONG_LIMIT = 0x83,
+	TX_3945_STATUS_FAIL_FIFO_UNDERRUN = 0x84,
+	TX_3945_STATUS_FAIL_MGMNT_ABORT = 0x85,
+	TX_3945_STATUS_FAIL_NEXT_FRAG = 0x86,
+	TX_3945_STATUS_FAIL_LIFE_EXPIRE = 0x87,
+	TX_3945_STATUS_FAIL_DEST_PS = 0x88,
+	TX_3945_STATUS_FAIL_ABORTED = 0x89,
+	TX_3945_STATUS_FAIL_BT_RETRY = 0x8a,
+	TX_3945_STATUS_FAIL_STA_INVALID = 0x8b,
+	TX_3945_STATUS_FAIL_FRAG_DROPPED = 0x8c,
+	TX_3945_STATUS_FAIL_TID_DISABLE = 0x8d,
+	TX_3945_STATUS_FAIL_FRAME_FLUSHED = 0x8e,
+	TX_3945_STATUS_FAIL_INSUFFICIENT_CF_POLL = 0x8f,
+	TX_3945_STATUS_FAIL_TX_LOCKED = 0x90,
+	TX_3945_STATUS_FAIL_NO_BEACON_ON_RADAR = 0x91,
+};
+
+/*
+ * TX command response is sent after *4965* transmission attempts.
+ *
+ * both postpone and abort status are expected behavior from uCode. there is
+ * no special operation required from driver; except for RFKILL_FLUSH,
+ * which required tx flush host command to flush all the tx frames in queues
+ */
+enum {
+	TX_STATUS_SUCCESS = 0x01,
+	TX_STATUS_DIRECT_DONE = 0x02,
+	/* postpone TX */
+	TX_STATUS_POSTPONE_DELAY = 0x40,
+	TX_STATUS_POSTPONE_FEW_BYTES = 0x41,
+	TX_STATUS_POSTPONE_QUIET_PERIOD = 0x43,
+	TX_STATUS_POSTPONE_CALC_TTAK = 0x44,
+	/* abort TX */
+	TX_STATUS_FAIL_INTERNAL_CROSSED_RETRY = 0x81,
+	TX_STATUS_FAIL_SHORT_LIMIT = 0x82,
+	TX_STATUS_FAIL_LONG_LIMIT = 0x83,
+	TX_STATUS_FAIL_FIFO_UNDERRUN = 0x84,
+	TX_STATUS_FAIL_DRAIN_FLOW = 0x85,
+	TX_STATUS_FAIL_RFKILL_FLUSH = 0x86,
+	TX_STATUS_FAIL_LIFE_EXPIRE = 0x87,
+	TX_STATUS_FAIL_DEST_PS = 0x88,
+	TX_STATUS_FAIL_HOST_ABORTED = 0x89,
+	TX_STATUS_FAIL_BT_RETRY = 0x8a,
+	TX_STATUS_FAIL_STA_INVALID = 0x8b,
+	TX_STATUS_FAIL_FRAG_DROPPED = 0x8c,
+	TX_STATUS_FAIL_TID_DISABLE = 0x8d,
+	TX_STATUS_FAIL_FIFO_FLUSHED = 0x8e,
+	TX_STATUS_FAIL_INSUFFICIENT_CF_POLL = 0x8f,
+	TX_STATUS_FAIL_PASSIVE_NO_RX = 0x90,
+	TX_STATUS_FAIL_NO_BEACON_ON_RADAR = 0x91,
+};
+
+#define	TX_PACKET_MODE_REGULAR		0x0000
+#define	TX_PACKET_MODE_BURST_SEQ	0x0100
+#define	TX_PACKET_MODE_BURST_FIRST	0x0200
+
+enum {
+	TX_POWER_PA_NOT_ACTIVE = 0x0,
+};
+
+enum {
+	TX_STATUS_MSK = 0x000000ff,		/* bits 0:7 */
+	TX_STATUS_DELAY_MSK = 0x00000040,
+	TX_STATUS_ABORT_MSK = 0x00000080,
+	TX_PACKET_MODE_MSK = 0x0000ff00,	/* bits 8:15 */
+	TX_FIFO_NUMBER_MSK = 0x00070000,	/* bits 16:18 */
+	TX_RESERVED = 0x00780000,		/* bits 19:22 */
+	TX_POWER_PA_DETECT_MSK = 0x7f800000,	/* bits 23:30 */
+	TX_ABORT_REQUIRED_MSK = 0x80000000,	/* bits 31:31 */
+};
+
+/* *******************************
+ * TX aggregation status
+ ******************************* */
+
+enum {
+	AGG_TX_STATE_TRANSMITTED = 0x00,
+	AGG_TX_STATE_UNDERRUN_MSK = 0x01,
+	AGG_TX_STATE_FEW_BYTES_MSK = 0x04,
+	AGG_TX_STATE_ABORT_MSK = 0x08,
+	AGG_TX_STATE_LAST_SENT_TTL_MSK = 0x10,
+	AGG_TX_STATE_LAST_SENT_TRY_CNT_MSK = 0x20,
+	AGG_TX_STATE_SCD_QUERY_MSK = 0x80,
+	AGG_TX_STATE_TEST_BAD_CRC32_MSK = 0x100,
+	AGG_TX_STATE_RESPONSE_MSK = 0x1ff,
+	AGG_TX_STATE_DUMP_TX_MSK = 0x200,
+	AGG_TX_STATE_DELAY_TX_MSK = 0x400
+};
+
+#define AGG_TX_STATUS_MSK	0x00000fff	/* bits 0:11 */
+#define AGG_TX_TRY_MSK		0x0000f000	/* bits 12:15 */
+
+#define AGG_TX_STATE_LAST_SENT_MSK  (AGG_TX_STATE_LAST_SENT_TTL_MSK | \
+				     AGG_TX_STATE_LAST_SENT_TRY_CNT_MSK)
+
+/* # tx attempts for first frame in aggregation */
+#define AGG_TX_STATE_TRY_CNT_POS 12
+#define AGG_TX_STATE_TRY_CNT_MSK 0xf000
+
+/* Command ID and sequence number of Tx command for this frame */
+#define AGG_TX_STATE_SEQ_NUM_POS 16
+#define AGG_TX_STATE_SEQ_NUM_MSK 0xffff0000
+
+/*
+ * REPLY_TX = 0x1c (response)
+ *
+ * This response may be in one of two slightly different formats, indicated
+ * by the frame_count field:
+ *
+ * 1)  No aggregation (frame_count == 1).  This reports Tx results for
+ *     a single frame.  Multiple attempts, at various bit rates, may have
+ *     been made for this frame.
+ *
+ * 2)  Aggregation (frame_count > 1).  This reports Tx results for
+ *     2 or more frames that used block-acknowledge.  All frames were
+ *     transmitted at same rate.  Rate scaling may have been used if first
+ *     frame in this new agg block failed in previous agg block(s).
+ *
+ *     Note that, for aggregation, ACK (block-ack) status is not delivered here;
+ *     block-ack has not been received by the time the 4965 device records
+ *     this status.
+ *     This status relates to reasons the tx might have been blocked or aborted
+ *     within the sending station (this 4965 device), rather than whether it was
+ *     received successfully by the destination station.
+ */
+struct agg_tx_status {
+	__le16 status;
+	__le16 sequence;
+} __packed;
+
+struct iwl4965_tx_resp {
+	u8 frame_count;		/* 1 no aggregation, >1 aggregation */
+	u8 bt_kill_count;	/* # blocked by bluetooth (unused for agg) */
+	u8 failure_rts;		/* # failures due to unsuccessful RTS */
+	u8 failure_frame;	/* # failures due to no ACK (unused for agg) */
+
+	/* For non-agg:  Rate at which frame was successful.
+	 * For agg:  Rate at which all frames were transmitted. */
+	__le32 rate_n_flags;	/* RATE_MCS_*  */
+
+	/* For non-agg:  RTS + CTS + frame tx attempts time + ACK.
+	 * For agg:  RTS + CTS + aggregation tx time + block-ack time. */
+	__le16 wireless_media_time;	/* uSecs */
+
+	__le16 reserved;
+	__le32 pa_power1;	/* RF power amplifier measurement (not used) */
+	__le32 pa_power2;
+
+	/*
+	 * For non-agg:  frame status TX_STATUS_*
+	 * For agg:  status of 1st frame, AGG_TX_STATE_*; other frame status
+	 *           fields follow this one, up to frame_count.
+	 *           Bit fields:
+	 *           11- 0:  AGG_TX_STATE_* status code
+	 *           15-12:  Retry count for 1st frame in aggregation (retries
+	 *                   occur if tx failed for this frame when it was a
+	 *                   member of a previous aggregation block).  If rate
+	 *                   scaling is used, retry count indicates the rate
+	 *                   table entry used for all frames in the new agg.
+	 *           31-16:  Sequence # for this frame's Tx cmd (not SSN!)
+	 */
+	union {
+		__le32 status;
+		struct agg_tx_status agg_status[0]; /* for each agg frame */
+	} u;
+} __packed;
+
+/*
+ * REPLY_COMPRESSED_BA = 0xc5 (response only, not a command)
+ *
+ * Reports Block-Acknowledge from recipient station
+ */
+struct iwl_compressed_ba_resp {
+	__le32 sta_addr_lo32;
+	__le16 sta_addr_hi16;
+	__le16 reserved;
+
+	/* Index of recipient (BA-sending) station in uCode's station table */
+	u8 sta_id;
+	u8 tid;
+	__le16 seq_ctl;
+	__le64 bitmap;
+	__le16 scd_flow;
+	__le16 scd_ssn;
+} __packed;
+
+/*
+ * REPLY_TX_PWR_TABLE_CMD = 0x97 (command, has simple generic response)
+ *
+ * See details under "TXPOWER" in iwl-4965-hw.h.
+ */
+
+struct iwl3945_txpowertable_cmd {
+	u8 band;		/* 0: 5 GHz, 1: 2.4 GHz */
+	u8 reserved;
+	__le16 channel;
+	struct iwl3945_power_per_rate power[IWL_MAX_RATES];
+} __packed;
+
+struct iwl4965_txpowertable_cmd {
+	u8 band;		/* 0: 5 GHz, 1: 2.4 GHz */
+	u8 reserved;
+	__le16 channel;
+	struct iwl4965_tx_power_db tx_power;
+} __packed;
+
+
+/**
+ * struct iwl3945_rate_scaling_cmd - Rate Scaling Command & Response
+ *
+ * REPLY_RATE_SCALE = 0x47 (command, has simple generic response)
+ *
+ * NOTE: The table of rates passed to the uCode via the
+ * RATE_SCALE command sets up the corresponding order of
+ * rates used for all related commands, including rate
+ * masks, etc.
+ *
+ * For example, if you set 9MB (PLCP 0x0f) as the first
+ * rate in the rate table, the bit mask for that rate
+ * when passed through ofdm_basic_rates on the REPLY_RXON
+ * command would be bit 0 (1 << 0)
+ */
+struct iwl3945_rate_scaling_info {
+	__le16 rate_n_flags;
+	u8 try_cnt;
+	u8 next_rate_index;
+} __packed;
+
+struct iwl3945_rate_scaling_cmd {
+	u8 table_id;
+	u8 reserved[3];
+	struct iwl3945_rate_scaling_info table[IWL_MAX_RATES];
+} __packed;
+
+
+/*RS_NEW_API: only TLC_RTS remains and moved to bit 0 */
+#define  LINK_QUAL_FLAGS_SET_STA_TLC_RTS_MSK	(1 << 0)
+
+/* # of EDCA prioritized tx fifos */
+#define  LINK_QUAL_AC_NUM AC_NUM
+
+/* # entries in rate scale table to support Tx retries */
+#define  LINK_QUAL_MAX_RETRY_NUM 16
+
+/* Tx antenna selection values */
+#define  LINK_QUAL_ANT_A_MSK (1 << 0)
+#define  LINK_QUAL_ANT_B_MSK (1 << 1)
+#define  LINK_QUAL_ANT_MSK   (LINK_QUAL_ANT_A_MSK|LINK_QUAL_ANT_B_MSK)
+
+
+/**
+ * struct iwl_link_qual_general_params
+ *
+ * Used in REPLY_TX_LINK_QUALITY_CMD
+ */
+struct iwl_link_qual_general_params {
+	u8 flags;
+
+	/* No entries at or above this (driver chosen) index contain MIMO */
+	u8 mimo_delimiter;
+
+	/* Best single antenna to use for single stream (legacy, SISO). */
+	u8 single_stream_ant_msk;	/* LINK_QUAL_ANT_* */
+
+	/* Best antennas to use for MIMO (unused for 4965, assumes both). */
+	u8 dual_stream_ant_msk;		/* LINK_QUAL_ANT_* */
+
+	/*
+	 * If driver needs to use different initial rates for different
+	 * EDCA QOS access categories (as implemented by tx fifos 0-3),
+	 * this table will set that up, by indicating the indexes in the
+	 * rs_table[LINK_QUAL_MAX_RETRY_NUM] rate table at which to start.
+	 * Otherwise, driver should set all entries to 0.
+	 *
+	 * Entry usage:
+	 * 0 = Background, 1 = Best Effort (normal), 2 = Video, 3 = Voice
+	 * TX FIFOs above 3 use same value (typically 0) as TX FIFO 3.
+	 */
+	u8 start_rate_index[LINK_QUAL_AC_NUM];
+} __packed;
+
+#define LINK_QUAL_AGG_TIME_LIMIT_DEF	(4000) /* 4 milliseconds */
+#define LINK_QUAL_AGG_TIME_LIMIT_MAX	(8000)
+#define LINK_QUAL_AGG_TIME_LIMIT_MIN	(100)
+
+#define LINK_QUAL_AGG_DISABLE_START_DEF	(3)
+#define LINK_QUAL_AGG_DISABLE_START_MAX	(255)
+#define LINK_QUAL_AGG_DISABLE_START_MIN	(0)
+
+#define LINK_QUAL_AGG_FRAME_LIMIT_DEF	(31)
+#define LINK_QUAL_AGG_FRAME_LIMIT_MAX	(63)
+#define LINK_QUAL_AGG_FRAME_LIMIT_MIN	(0)
+
+/**
+ * struct iwl_link_qual_agg_params
+ *
+ * Used in REPLY_TX_LINK_QUALITY_CMD
+ */
+struct iwl_link_qual_agg_params {
+
+	/*
+	 *Maximum number of uSec in aggregation.
+	 * default set to 4000 (4 milliseconds) if not configured in .cfg
+	 */
+	__le16 agg_time_limit;
+
+	/*
+	 * Number of Tx retries allowed for a frame, before that frame will
+	 * no longer be considered for the start of an aggregation sequence
+	 * (scheduler will then try to tx it as single frame).
+	 * Driver should set this to 3.
+	 */
+	u8 agg_dis_start_th;
+
+	/*
+	 * Maximum number of frames in aggregation.
+	 * 0 = no limit (default).  1 = no aggregation.
+	 * Other values = max # frames in aggregation.
+	 */
+	u8 agg_frame_cnt_limit;
+
+	__le32 reserved;
+} __packed;
+
+/*
+ * REPLY_TX_LINK_QUALITY_CMD = 0x4e (command, has simple generic response)
+ *
+ * For 4965 devices only; 3945 uses REPLY_RATE_SCALE.
+ *
+ * Each station in the 4965 device's internal station table has its own table
+ * of 16
+ * Tx rates and modulation modes (e.g. legacy/SISO/MIMO) for retrying Tx when
+ * an ACK is not received.  This command replaces the entire table for
+ * one station.
+ *
+ * NOTE:  Station must already be in 4965 device's station table.
+ *	  Use REPLY_ADD_STA.
+ *
+ * The rate scaling procedures described below work well.  Of course, other
+ * procedures are possible, and may work better for particular environments.
+ *
+ *
+ * FILLING THE RATE TABLE
+ *
+ * Given a particular initial rate and mode, as determined by the rate
+ * scaling algorithm described below, the Linux driver uses the following
+ * formula to fill the rs_table[LINK_QUAL_MAX_RETRY_NUM] rate table in the
+ * Link Quality command:
+ *
+ *
+ * 1)  If using High-throughput (HT) (SISO or MIMO) initial rate:
+ *     a) Use this same initial rate for first 3 entries.
+ *     b) Find next lower available rate using same mode (SISO or MIMO),
+ *        use for next 3 entries.  If no lower rate available, switch to
+ *        legacy mode (no HT40 channel, no MIMO, no short guard interval).
+ *     c) If using MIMO, set command's mimo_delimiter to number of entries
+ *        using MIMO (3 or 6).
+ *     d) After trying 2 HT rates, switch to legacy mode (no HT40 channel,
+ *        no MIMO, no short guard interval), at the next lower bit rate
+ *        (e.g. if second HT bit rate was 54, try 48 legacy), and follow
+ *        legacy procedure for remaining table entries.
+ *
+ * 2)  If using legacy initial rate:
+ *     a) Use the initial rate for only one entry.
+ *     b) For each following entry, reduce the rate to next lower available
+ *        rate, until reaching the lowest available rate.
+ *     c) When reducing rate, also switch antenna selection.
+ *     d) Once lowest available rate is reached, repeat this rate until
+ *        rate table is filled (16 entries), switching antenna each entry.
+ *
+ *
+ * ACCUMULATING HISTORY
+ *
+ * The rate scaling algorithm for 4965 devices, as implemented in Linux driver,
+ * uses two sets of frame Tx success history:  One for the current/active
+ * modulation mode, and one for a speculative/search mode that is being
+ * attempted. If the speculative mode turns out to be more effective (i.e.
+ * actual transfer rate is better), then the driver continues to use the
+ * speculative mode as the new current active mode.
+ *
+ * Each history set contains, separately for each possible rate, data for a
+ * sliding window of the 62 most recent tx attempts at that rate.  The data
+ * includes a shifting bitmap of success(1)/failure(0), and sums of successful
+ * and attempted frames, from which the driver can additionally calculate a
+ * success ratio (success / attempted) and number of failures
+ * (attempted - success), and control the size of the window (attempted).
+ * The driver uses the bit map to remove successes from the success sum, as
+ * the oldest tx attempts fall out of the window.
+ *
+ * When the 4965 device makes multiple tx attempts for a given frame, each
+ * attempt might be at a different rate, and have different modulation
+ * characteristics (e.g. antenna, fat channel, short guard interval), as set
+ * up in the rate scaling table in the Link Quality command.  The driver must
+ * determine which rate table entry was used for each tx attempt, to determine
+ * which rate-specific history to update, and record only those attempts that
+ * match the modulation characteristics of the history set.
+ *
+ * When using block-ack (aggregation), all frames are transmitted at the same
+ * rate, since there is no per-attempt acknowledgment from the destination
+ * station.  The Tx response struct iwl_tx_resp indicates the Tx rate in
+ * rate_n_flags field.  After receiving a block-ack, the driver can update
+ * history for the entire block all at once.
+ *
+ *
+ * FINDING BEST STARTING RATE:
+ *
+ * When working with a selected initial modulation mode (see below), the
+ * driver attempts to find a best initial rate.  The initial rate is the
+ * first entry in the Link Quality command's rate table.
+ *
+ * 1)  Calculate actual throughput (success ratio * expected throughput, see
+ *     table below) for current initial rate.  Do this only if enough frames
+ *     have been attempted to make the value meaningful:  at least 6 failed
+ *     tx attempts, or at least 8 successes.  If not enough, don't try rate
+ *     scaling yet.
+ *
+ * 2)  Find available rates adjacent to current initial rate.  Available means:
+ *     a)  supported by hardware &&
+ *     b)  supported by association &&
+ *     c)  within any constraints selected by user
+ *
+ * 3)  Gather measured throughputs for adjacent rates.  These might not have
+ *     enough history to calculate a throughput.  That's okay, we might try
+ *     using one of them anyway!
+ *
+ * 4)  Try decreasing rate if, for current rate:
+ *     a)  success ratio is < 15% ||
+ *     b)  lower adjacent rate has better measured throughput ||
+ *     c)  higher adjacent rate has worse throughput, and lower is unmeasured
+ *
+ *     As a sanity check, if decrease was determined above, leave rate
+ *     unchanged if:
+ *     a)  lower rate unavailable
+ *     b)  success ratio at current rate > 85% (very good)
+ *     c)  current measured throughput is better than expected throughput
+ *         of lower rate (under perfect 100% tx conditions, see table below)
+ *
+ * 5)  Try increasing rate if, for current rate:
+ *     a)  success ratio is < 15% ||
+ *     b)  both adjacent rates' throughputs are unmeasured (try it!) ||
+ *     b)  higher adjacent rate has better measured throughput ||
+ *     c)  lower adjacent rate has worse throughput, and higher is unmeasured
+ *
+ *     As a sanity check, if increase was determined above, leave rate
+ *     unchanged if:
+ *     a)  success ratio at current rate < 70%.  This is not particularly
+ *         good performance; higher rate is sure to have poorer success.
+ *
+ * 6)  Re-evaluate the rate after each tx frame.  If working with block-
+ *     acknowledge, history and statistics may be calculated for the entire
+ *     block (including prior history that fits within the history windows),
+ *     before re-evaluation.
+ *
+ * FINDING BEST STARTING MODULATION MODE:
+ *
+ * After working with a modulation mode for a "while" (and doing rate scaling),
+ * the driver searches for a new initial mode in an attempt to improve
+ * throughput.  The "while" is measured by numbers of attempted frames:
+ *
+ * For legacy mode, search for new mode after:
+ *   480 successful frames, or 160 failed frames
+ * For high-throughput modes (SISO or MIMO), search for new mode after:
+ *   4500 successful frames, or 400 failed frames
+ *
+ * Mode switch possibilities are (3 for each mode):
+ *
+ * For legacy:
+ *   Change antenna, try SISO (if HT association), try MIMO (if HT association)
+ * For SISO:
+ *   Change antenna, try MIMO, try shortened guard interval (SGI)
+ * For MIMO:
+ *   Try SISO antenna A, SISO antenna B, try shortened guard interval (SGI)
+ *
+ * When trying a new mode, use the same bit rate as the old/current mode when
+ * trying antenna switches and shortened guard interval.  When switching to
+ * SISO from MIMO or legacy, or to MIMO from SISO or legacy, use a rate
+ * for which the expected throughput (under perfect conditions) is about the
+ * same or slightly better than the actual measured throughput delivered by
+ * the old/current mode.
+ *
+ * Actual throughput can be estimated by multiplying the expected throughput
+ * by the success ratio (successful / attempted tx frames).  Frame size is
+ * not considered in this calculation; it assumes that frame size will average
+ * out to be fairly consistent over several samples.  The following are
+ * metric values for expected throughput assuming 100% success ratio.
+ * Only G band has support for CCK rates:
+ *
+ *           RATE:  1    2    5   11    6   9   12   18   24   36   48   54   60
+ *
+ *              G:  7   13   35   58   40  57   72   98  121  154  177  186  186
+ *              A:  0    0    0    0   40  57   72   98  121  154  177  186  186
+ *     SISO 20MHz:  0    0    0    0   42  42   76  102  124  159  183  193  202
+ * SGI SISO 20MHz:  0    0    0    0   46  46   82  110  132  168  192  202  211
+ *     MIMO 20MHz:  0    0    0    0   74  74  123  155  179  214  236  244  251
+ * SGI MIMO 20MHz:  0    0    0    0   81  81  131  164  188  222  243  251  257
+ *     SISO 40MHz:  0    0    0    0   77  77  127  160  184  220  242  250  257
+ * SGI SISO 40MHz:  0    0    0    0   83  83  135  169  193  229  250  257  264
+ *     MIMO 40MHz:  0    0    0    0  123 123  182  214  235  264  279  285  289
+ * SGI MIMO 40MHz:  0    0    0    0  131 131  191  222  242  270  284  289  293
+ *
+ * After the new mode has been tried for a short while (minimum of 6 failed
+ * frames or 8 successful frames), compare success ratio and actual throughput
+ * estimate of the new mode with the old.  If either is better with the new
+ * mode, continue to use the new mode.
+ *
+ * Continue comparing modes until all 3 possibilities have been tried.
+ * If moving from legacy to HT, try all 3 possibilities from the new HT
+ * mode.  After trying all 3, a best mode is found.  Continue to use this mode
+ * for the longer "while" described above (e.g. 480 successful frames for
+ * legacy), and then repeat the search process.
+ *
+ */
+struct iwl_link_quality_cmd {
+
+	/* Index of destination/recipient station in uCode's station table */
+	u8 sta_id;
+	u8 reserved1;
+	__le16 control;		/* not used */
+	struct iwl_link_qual_general_params general_params;
+	struct iwl_link_qual_agg_params agg_params;
+
+	/*
+	 * Rate info; when using rate-scaling, Tx command's initial_rate_index
+	 * specifies 1st Tx rate attempted, via index into this table.
+	 * 4965 devices works its way through table when retrying Tx.
+	 */
+	struct {
+		__le32 rate_n_flags;	/* RATE_MCS_*, IWL_RATE_* */
+	} rs_table[LINK_QUAL_MAX_RETRY_NUM];
+	__le32 reserved2;
+} __packed;
+
+/*
+ * BT configuration enable flags:
+ *   bit 0 - 1: BT channel announcement enabled
+ *           0: disable
+ *   bit 1 - 1: priority of BT device enabled
+ *           0: disable
+ */
+#define BT_COEX_DISABLE (0x0)
+#define BT_ENABLE_CHANNEL_ANNOUNCE BIT(0)
+#define BT_ENABLE_PRIORITY	   BIT(1)
+
+#define BT_COEX_ENABLE  (BT_ENABLE_CHANNEL_ANNOUNCE | BT_ENABLE_PRIORITY)
+
+#define BT_LEAD_TIME_DEF (0x1E)
+
+#define BT_MAX_KILL_DEF (0x5)
+
+/*
+ * REPLY_BT_CONFIG = 0x9b (command, has simple generic response)
+ *
+ * 3945 and 4965 devices support hardware handshake with Bluetooth device on
+ * same platform.  Bluetooth device alerts wireless device when it will Tx;
+ * wireless device can delay or kill its own Tx to accommodate.
+ */
+struct iwl_bt_cmd {
+	u8 flags;
+	u8 lead_time;
+	u8 max_kill;
+	u8 reserved;
+	__le32 kill_ack_mask;
+	__le32 kill_cts_mask;
+} __packed;
+
+
+/******************************************************************************
+ * (6)
+ * Spectrum Management (802.11h) Commands, Responses, Notifications:
+ *
+ *****************************************************************************/
+
+/*
+ * Spectrum Management
+ */
+#define MEASUREMENT_FILTER_FLAG (RXON_FILTER_PROMISC_MSK         | \
+				 RXON_FILTER_CTL2HOST_MSK        | \
+				 RXON_FILTER_ACCEPT_GRP_MSK      | \
+				 RXON_FILTER_DIS_DECRYPT_MSK     | \
+				 RXON_FILTER_DIS_GRP_DECRYPT_MSK | \
+				 RXON_FILTER_ASSOC_MSK           | \
+				 RXON_FILTER_BCON_AWARE_MSK)
+
+struct iwl_measure_channel {
+	__le32 duration;	/* measurement duration in extended beacon
+				 * format */
+	u8 channel;		/* channel to measure */
+	u8 type;		/* see enum iwl_measure_type */
+	__le16 reserved;
+} __packed;
+
+/*
+ * REPLY_SPECTRUM_MEASUREMENT_CMD = 0x74 (command)
+ */
+struct iwl_spectrum_cmd {
+	__le16 len;		/* number of bytes starting from token */
+	u8 token;		/* token id */
+	u8 id;			/* measurement id -- 0 or 1 */
+	u8 origin;		/* 0 = TGh, 1 = other, 2 = TGk */
+	u8 periodic;		/* 1 = periodic */
+	__le16 path_loss_timeout;
+	__le32 start_time;	/* start time in extended beacon format */
+	__le32 reserved2;
+	__le32 flags;		/* rxon flags */
+	__le32 filter_flags;	/* rxon filter flags */
+	__le16 channel_count;	/* minimum 1, maximum 10 */
+	__le16 reserved3;
+	struct iwl_measure_channel channels[10];
+} __packed;
+
+/*
+ * REPLY_SPECTRUM_MEASUREMENT_CMD = 0x74 (response)
+ */
+struct iwl_spectrum_resp {
+	u8 token;
+	u8 id;			/* id of the prior command replaced, or 0xff */
+	__le16 status;		/* 0 - command will be handled
+				 * 1 - cannot handle (conflicts with another
+				 *     measurement) */
+} __packed;
+
+enum iwl_measurement_state {
+	IWL_MEASUREMENT_START = 0,
+	IWL_MEASUREMENT_STOP = 1,
+};
+
+enum iwl_measurement_status {
+	IWL_MEASUREMENT_OK = 0,
+	IWL_MEASUREMENT_CONCURRENT = 1,
+	IWL_MEASUREMENT_CSA_CONFLICT = 2,
+	IWL_MEASUREMENT_TGH_CONFLICT = 3,
+	/* 4-5 reserved */
+	IWL_MEASUREMENT_STOPPED = 6,
+	IWL_MEASUREMENT_TIMEOUT = 7,
+	IWL_MEASUREMENT_PERIODIC_FAILED = 8,
+};
+
+#define NUM_ELEMENTS_IN_HISTOGRAM 8
+
+struct iwl_measurement_histogram {
+	__le32 ofdm[NUM_ELEMENTS_IN_HISTOGRAM];	/* in 0.8usec counts */
+	__le32 cck[NUM_ELEMENTS_IN_HISTOGRAM];	/* in 1usec counts */
+} __packed;
+
+/* clear channel availability counters */
+struct iwl_measurement_cca_counters {
+	__le32 ofdm;
+	__le32 cck;
+} __packed;
+
+enum iwl_measure_type {
+	IWL_MEASURE_BASIC = (1 << 0),
+	IWL_MEASURE_CHANNEL_LOAD = (1 << 1),
+	IWL_MEASURE_HISTOGRAM_RPI = (1 << 2),
+	IWL_MEASURE_HISTOGRAM_NOISE = (1 << 3),
+	IWL_MEASURE_FRAME = (1 << 4),
+	/* bits 5:6 are reserved */
+	IWL_MEASURE_IDLE = (1 << 7),
+};
+
+/*
+ * SPECTRUM_MEASURE_NOTIFICATION = 0x75 (notification only, not a command)
+ */
+struct iwl_spectrum_notification {
+	u8 id;			/* measurement id -- 0 or 1 */
+	u8 token;
+	u8 channel_index;	/* index in measurement channel list */
+	u8 state;		/* 0 - start, 1 - stop */
+	__le32 start_time;	/* lower 32-bits of TSF */
+	u8 band;		/* 0 - 5.2GHz, 1 - 2.4GHz */
+	u8 channel;
+	u8 type;		/* see enum iwl_measurement_type */
+	u8 reserved1;
+	/* NOTE:  cca_ofdm, cca_cck, basic_type, and histogram are only only
+	 * valid if applicable for measurement type requested. */
+	__le32 cca_ofdm;	/* cca fraction time in 40Mhz clock periods */
+	__le32 cca_cck;		/* cca fraction time in 44Mhz clock periods */
+	__le32 cca_time;	/* channel load time in usecs */
+	u8 basic_type;		/* 0 - bss, 1 - ofdm preamble, 2 -
+				 * unidentified */
+	u8 reserved2[3];
+	struct iwl_measurement_histogram histogram;
+	__le32 stop_time;	/* lower 32-bits of TSF */
+	__le32 status;		/* see iwl_measurement_status */
+} __packed;
+
+/******************************************************************************
+ * (7)
+ * Power Management Commands, Responses, Notifications:
+ *
+ *****************************************************************************/
+
+/**
+ * struct iwl_powertable_cmd - Power Table Command
+ * @flags: See below:
+ *
+ * POWER_TABLE_CMD = 0x77 (command, has simple generic response)
+ *
+ * PM allow:
+ *   bit 0 - '0' Driver not allow power management
+ *           '1' Driver allow PM (use rest of parameters)
+ *
+ * uCode send sleep notifications:
+ *   bit 1 - '0' Don't send sleep notification
+ *           '1' send sleep notification (SEND_PM_NOTIFICATION)
+ *
+ * Sleep over DTIM
+ *   bit 2 - '0' PM have to walk up every DTIM
+ *           '1' PM could sleep over DTIM till listen Interval.
+ *
+ * PCI power managed
+ *   bit 3 - '0' (PCI_CFG_LINK_CTRL & 0x1)
+ *           '1' !(PCI_CFG_LINK_CTRL & 0x1)
+ *
+ * Fast PD
+ *   bit 4 - '1' Put radio to sleep when receiving frame for others
+ *
+ * Force sleep Modes
+ *   bit 31/30- '00' use both mac/xtal sleeps
+ *              '01' force Mac sleep
+ *              '10' force xtal sleep
+ *              '11' Illegal set
+ *
+ * NOTE: if sleep_interval[SLEEP_INTRVL_TABLE_SIZE-1] > DTIM period then
+ * ucode assume sleep over DTIM is allowed and we don't need to wake up
+ * for every DTIM.
+ */
+#define IWL_POWER_VEC_SIZE 5
+
+#define IWL_POWER_DRIVER_ALLOW_SLEEP_MSK	cpu_to_le16(BIT(0))
+#define IWL_POWER_POWER_SAVE_ENA_MSK		cpu_to_le16(BIT(0))
+#define IWL_POWER_POWER_MANAGEMENT_ENA_MSK	cpu_to_le16(BIT(1))
+#define IWL_POWER_SLEEP_OVER_DTIM_MSK		cpu_to_le16(BIT(2))
+#define IWL_POWER_PCI_PM_MSK			cpu_to_le16(BIT(3))
+#define IWL_POWER_FAST_PD			cpu_to_le16(BIT(4))
+#define IWL_POWER_BEACON_FILTERING		cpu_to_le16(BIT(5))
+#define IWL_POWER_SHADOW_REG_ENA		cpu_to_le16(BIT(6))
+#define IWL_POWER_CT_KILL_SET			cpu_to_le16(BIT(7))
+
+struct iwl3945_powertable_cmd {
+	__le16 flags;
+	u8 reserved[2];
+	__le32 rx_data_timeout;
+	__le32 tx_data_timeout;
+	__le32 sleep_interval[IWL_POWER_VEC_SIZE];
+} __packed;
+
+struct iwl_powertable_cmd {
+	__le16 flags;
+	u8 keep_alive_seconds;		/* 3945 reserved */
+	u8 debug_flags;			/* 3945 reserved */
+	__le32 rx_data_timeout;
+	__le32 tx_data_timeout;
+	__le32 sleep_interval[IWL_POWER_VEC_SIZE];
+	__le32 keep_alive_beacons;
+} __packed;
+
+/*
+ * PM_SLEEP_NOTIFICATION = 0x7A (notification only, not a command)
+ * all devices identical.
+ */
+struct iwl_sleep_notification {
+	u8 pm_sleep_mode;
+	u8 pm_wakeup_src;
+	__le16 reserved;
+	__le32 sleep_time;
+	__le32 tsf_low;
+	__le32 bcon_timer;
+} __packed;
+
+/* Sleep states.  all devices identical. */
+enum {
+	IWL_PM_NO_SLEEP = 0,
+	IWL_PM_SLP_MAC = 1,
+	IWL_PM_SLP_FULL_MAC_UNASSOCIATE = 2,
+	IWL_PM_SLP_FULL_MAC_CARD_STATE = 3,
+	IWL_PM_SLP_PHY = 4,
+	IWL_PM_SLP_REPENT = 5,
+	IWL_PM_WAKEUP_BY_TIMER = 6,
+	IWL_PM_WAKEUP_BY_DRIVER = 7,
+	IWL_PM_WAKEUP_BY_RFKILL = 8,
+	/* 3 reserved */
+	IWL_PM_NUM_OF_MODES = 12,
+};
+
+/*
+ * CARD_STATE_NOTIFICATION = 0xa1 (notification only, not a command)
+ */
+struct iwl_card_state_notif {
+	__le32 flags;
+} __packed;
+
+#define HW_CARD_DISABLED   0x01
+#define SW_CARD_DISABLED   0x02
+#define CT_CARD_DISABLED   0x04
+#define RXON_CARD_DISABLED 0x10
+
+struct iwl_ct_kill_config {
+	__le32   reserved;
+	__le32   critical_temperature_M;
+	__le32   critical_temperature_R;
+}  __packed;
+
+/******************************************************************************
+ * (8)
+ * Scan Commands, Responses, Notifications:
+ *
+ *****************************************************************************/
+
+#define SCAN_CHANNEL_TYPE_PASSIVE cpu_to_le32(0)
+#define SCAN_CHANNEL_TYPE_ACTIVE  cpu_to_le32(1)
+
+/**
+ * struct iwl_scan_channel - entry in REPLY_SCAN_CMD channel table
+ *
+ * One for each channel in the scan list.
+ * Each channel can independently select:
+ * 1)  SSID for directed active scans
+ * 2)  Txpower setting (for rate specified within Tx command)
+ * 3)  How long to stay on-channel (behavior may be modified by quiet_time,
+ *     quiet_plcp_th, good_CRC_th)
+ *
+ * To avoid uCode errors, make sure the following are true (see comments
+ * under struct iwl_scan_cmd about max_out_time and quiet_time):
+ * 1)  If using passive_dwell (i.e. passive_dwell != 0):
+ *     active_dwell <= passive_dwell (< max_out_time if max_out_time != 0)
+ * 2)  quiet_time <= active_dwell
+ * 3)  If restricting off-channel time (i.e. max_out_time !=0):
+ *     passive_dwell < max_out_time
+ *     active_dwell < max_out_time
+ */
+struct iwl3945_scan_channel {
+	/*
+	 * type is defined as:
+	 * 0:0 1 = active, 0 = passive
+	 * 1:4 SSID direct bit map; if a bit is set, then corresponding
+	 *     SSID IE is transmitted in probe request.
+	 * 5:7 reserved
+	 */
+	u8 type;
+	u8 channel;	/* band is selected by iwl3945_scan_cmd "flags" field */
+	struct iwl3945_tx_power tpc;
+	__le16 active_dwell;	/* in 1024-uSec TU (time units), typ 5-50 */
+	__le16 passive_dwell;	/* in 1024-uSec TU (time units), typ 20-500 */
+} __packed;
+
+/* set number of direct probes u8 type */
+#define IWL39_SCAN_PROBE_MASK(n) ((BIT(n) | (BIT(n) - BIT(1))))
+
+struct iwl_scan_channel {
+	/*
+	 * type is defined as:
+	 * 0:0 1 = active, 0 = passive
+	 * 1:20 SSID direct bit map; if a bit is set, then corresponding
+	 *     SSID IE is transmitted in probe request.
+	 * 21:31 reserved
+	 */
+	__le32 type;
+	__le16 channel;	/* band is selected by iwl_scan_cmd "flags" field */
+	u8 tx_gain;		/* gain for analog radio */
+	u8 dsp_atten;		/* gain for DSP */
+	__le16 active_dwell;	/* in 1024-uSec TU (time units), typ 5-50 */
+	__le16 passive_dwell;	/* in 1024-uSec TU (time units), typ 20-500 */
+} __packed;
+
+/* set number of direct probes __le32 type */
+#define IWL_SCAN_PROBE_MASK(n)	cpu_to_le32((BIT(n) | (BIT(n) - BIT(1))))
+
+/**
+ * struct iwl_ssid_ie - directed scan network information element
+ *
+ * Up to 20 of these may appear in REPLY_SCAN_CMD (Note: Only 4 are in
+ * 3945 SCAN api), selected by "type" bit field in struct iwl_scan_channel;
+ * each channel may select different ssids from among the 20 (4) entries.
+ * SSID IEs get transmitted in reverse order of entry.
+ */
+struct iwl_ssid_ie {
+	u8 id;
+	u8 len;
+	u8 ssid[32];
+} __packed;
+
+#define PROBE_OPTION_MAX_3945		4
+#define PROBE_OPTION_MAX		20
+#define TX_CMD_LIFE_TIME_INFINITE	cpu_to_le32(0xFFFFFFFF)
+#define IWL_GOOD_CRC_TH_DISABLED	0
+#define IWL_GOOD_CRC_TH_DEFAULT		cpu_to_le16(1)
+#define IWL_GOOD_CRC_TH_NEVER		cpu_to_le16(0xffff)
+#define IWL_MAX_SCAN_SIZE 1024
+#define IWL_MAX_CMD_SIZE 4096
+
+/*
+ * REPLY_SCAN_CMD = 0x80 (command)
+ *
+ * The hardware scan command is very powerful; the driver can set it up to
+ * maintain (relatively) normal network traffic while doing a scan in the
+ * background.  The max_out_time and suspend_time control the ratio of how
+ * long the device stays on an associated network channel ("service channel")
+ * vs. how long it's away from the service channel, i.e. tuned to other channels
+ * for scanning.
+ *
+ * max_out_time is the max time off-channel (in usec), and suspend_time
+ * is how long (in "extended beacon" format) that the scan is "suspended"
+ * after returning to the service channel.  That is, suspend_time is the
+ * time that we stay on the service channel, doing normal work, between
+ * scan segments.  The driver may set these parameters differently to support
+ * scanning when associated vs. not associated, and light vs. heavy traffic
+ * loads when associated.
+ *
+ * After receiving this command, the device's scan engine does the following;
+ *
+ * 1)  Sends SCAN_START notification to driver
+ * 2)  Checks to see if it has time to do scan for one channel
+ * 3)  Sends NULL packet, with power-save (PS) bit set to 1,
+ *     to tell AP that we're going off-channel
+ * 4)  Tunes to first channel in scan list, does active or passive scan
+ * 5)  Sends SCAN_RESULT notification to driver
+ * 6)  Checks to see if it has time to do scan on *next* channel in list
+ * 7)  Repeats 4-6 until it no longer has time to scan the next channel
+ *     before max_out_time expires
+ * 8)  Returns to service channel
+ * 9)  Sends NULL packet with PS=0 to tell AP that we're back
+ * 10) Stays on service channel until suspend_time expires
+ * 11) Repeats entire process 2-10 until list is complete
+ * 12) Sends SCAN_COMPLETE notification
+ *
+ * For fast, efficient scans, the scan command also has support for staying on
+ * a channel for just a short time, if doing active scanning and getting no
+ * responses to the transmitted probe request.  This time is controlled by
+ * quiet_time, and the number of received packets below which a channel is
+ * considered "quiet" is controlled by quiet_plcp_threshold.
+ *
+ * For active scanning on channels that have regulatory restrictions against
+ * blindly transmitting, the scan can listen before transmitting, to make sure
+ * that there is already legitimate activity on the channel.  If enough
+ * packets are cleanly received on the channel (controlled by good_CRC_th,
+ * typical value 1), the scan engine starts transmitting probe requests.
+ *
+ * Driver must use separate scan commands for 2.4 vs. 5 GHz bands.
+ *
+ * To avoid uCode errors, see timing restrictions described under
+ * struct iwl_scan_channel.
+ */
+
+struct iwl3945_scan_cmd {
+	__le16 len;
+	u8 reserved0;
+	u8 channel_count;	/* # channels in channel list */
+	__le16 quiet_time;	/* dwell only this # millisecs on quiet channel
+				 * (only for active scan) */
+	__le16 quiet_plcp_th;	/* quiet chnl is < this # pkts (typ. 1) */
+	__le16 good_CRC_th;	/* passive -> active promotion threshold */
+	__le16 reserved1;
+	__le32 max_out_time;	/* max usec to be away from associated (service)
+				 * channel */
+	__le32 suspend_time;	/* pause scan this long (in "extended beacon
+				 * format") when returning to service channel:
+				 * 3945; 31:24 # beacons, 19:0 additional usec,
+				 * 4965; 31:22 # beacons, 21:0 additional usec.
+				 */
+	__le32 flags;		/* RXON_FLG_* */
+	__le32 filter_flags;	/* RXON_FILTER_* */
+
+	/* For active scans (set to all-0s for passive scans).
+	 * Does not include payload.  Must specify Tx rate; no rate scaling. */
+	struct iwl3945_tx_cmd tx_cmd;
+
+	/* For directed active scans (set to all-0s otherwise) */
+	struct iwl_ssid_ie direct_scan[PROBE_OPTION_MAX_3945];
+
+	/*
+	 * Probe request frame, followed by channel list.
+	 *
+	 * Size of probe request frame is specified by byte count in tx_cmd.
+	 * Channel list follows immediately after probe request frame.
+	 * Number of channels in list is specified by channel_count.
+	 * Each channel in list is of type:
+	 *
+	 * struct iwl3945_scan_channel channels[0];
+	 *
+	 * NOTE:  Only one band of channels can be scanned per pass.  You
+	 * must not mix 2.4GHz channels and 5.2GHz channels, and you must wait
+	 * for one scan to complete (i.e. receive SCAN_COMPLETE_NOTIFICATION)
+	 * before requesting another scan.
+	 */
+	u8 data[0];
+} __packed;
+
+struct iwl_scan_cmd {
+	__le16 len;
+	u8 reserved0;
+	u8 channel_count;	/* # channels in channel list */
+	__le16 quiet_time;	/* dwell only this # millisecs on quiet channel
+				 * (only for active scan) */
+	__le16 quiet_plcp_th;	/* quiet chnl is < this # pkts (typ. 1) */
+	__le16 good_CRC_th;	/* passive -> active promotion threshold */
+	__le16 rx_chain;	/* RXON_RX_CHAIN_* */
+	__le32 max_out_time;	/* max usec to be away from associated (service)
+				 * channel */
+	__le32 suspend_time;	/* pause scan this long (in "extended beacon
+				 * format") when returning to service chnl:
+				 * 3945; 31:24 # beacons, 19:0 additional usec,
+				 * 4965; 31:22 # beacons, 21:0 additional usec.
+				 */
+	__le32 flags;		/* RXON_FLG_* */
+	__le32 filter_flags;	/* RXON_FILTER_* */
+
+	/* For active scans (set to all-0s for passive scans).
+	 * Does not include payload.  Must specify Tx rate; no rate scaling. */
+	struct iwl_tx_cmd tx_cmd;
+
+	/* For directed active scans (set to all-0s otherwise) */
+	struct iwl_ssid_ie direct_scan[PROBE_OPTION_MAX];
+
+	/*
+	 * Probe request frame, followed by channel list.
+	 *
+	 * Size of probe request frame is specified by byte count in tx_cmd.
+	 * Channel list follows immediately after probe request frame.
+	 * Number of channels in list is specified by channel_count.
+	 * Each channel in list is of type:
+	 *
+	 * struct iwl_scan_channel channels[0];
+	 *
+	 * NOTE:  Only one band of channels can be scanned per pass.  You
+	 * must not mix 2.4GHz channels and 5.2GHz channels, and you must wait
+	 * for one scan to complete (i.e. receive SCAN_COMPLETE_NOTIFICATION)
+	 * before requesting another scan.
+	 */
+	u8 data[0];
+} __packed;
+
+/* Can abort will notify by complete notification with abort status. */
+#define CAN_ABORT_STATUS	cpu_to_le32(0x1)
+/* complete notification statuses */
+#define ABORT_STATUS            0x2
+
+/*
+ * REPLY_SCAN_CMD = 0x80 (response)
+ */
+struct iwl_scanreq_notification {
+	__le32 status;		/* 1: okay, 2: cannot fulfill request */
+} __packed;
+
+/*
+ * SCAN_START_NOTIFICATION = 0x82 (notification only, not a command)
+ */
+struct iwl_scanstart_notification {
+	__le32 tsf_low;
+	__le32 tsf_high;
+	__le32 beacon_timer;
+	u8 channel;
+	u8 band;
+	u8 reserved[2];
+	__le32 status;
+} __packed;
+
+#define  SCAN_OWNER_STATUS 0x1;
+#define  MEASURE_OWNER_STATUS 0x2;
+
+#define IWL_PROBE_STATUS_OK		0
+#define IWL_PROBE_STATUS_TX_FAILED	BIT(0)
+/* error statuses combined with TX_FAILED */
+#define IWL_PROBE_STATUS_FAIL_TTL	BIT(1)
+#define IWL_PROBE_STATUS_FAIL_BT	BIT(2)
+
+#define NUMBER_OF_STATISTICS 1	/* first __le32 is good CRC */
+/*
+ * SCAN_RESULTS_NOTIFICATION = 0x83 (notification only, not a command)
+ */
+struct iwl_scanresults_notification {
+	u8 channel;
+	u8 band;
+	u8 probe_status;
+	u8 num_probe_not_sent; /* not enough time to send */
+	__le32 tsf_low;
+	__le32 tsf_high;
+	__le32 statistics[NUMBER_OF_STATISTICS];
+} __packed;
+
+/*
+ * SCAN_COMPLETE_NOTIFICATION = 0x84 (notification only, not a command)
+ */
+struct iwl_scancomplete_notification {
+	u8 scanned_channels;
+	u8 status;
+	u8 last_channel;
+	__le32 tsf_low;
+	__le32 tsf_high;
+} __packed;
+
+
+/******************************************************************************
+ * (9)
+ * IBSS/AP Commands and Notifications:
+ *
+ *****************************************************************************/
+
+enum iwl_ibss_manager {
+	IWL_NOT_IBSS_MANAGER = 0,
+	IWL_IBSS_MANAGER = 1,
+};
+
+/*
+ * BEACON_NOTIFICATION = 0x90 (notification only, not a command)
+ */
+
+struct iwl3945_beacon_notif {
+	struct iwl3945_tx_resp beacon_notify_hdr;
+	__le32 low_tsf;
+	__le32 high_tsf;
+	__le32 ibss_mgr_status;
+} __packed;
+
+struct iwl4965_beacon_notif {
+	struct iwl4965_tx_resp beacon_notify_hdr;
+	__le32 low_tsf;
+	__le32 high_tsf;
+	__le32 ibss_mgr_status;
+} __packed;
+
+/*
+ * REPLY_TX_BEACON = 0x91 (command, has simple generic response)
+ */
+
+struct iwl3945_tx_beacon_cmd {
+	struct iwl3945_tx_cmd tx;
+	__le16 tim_idx;
+	u8 tim_size;
+	u8 reserved1;
+	struct ieee80211_hdr frame[0];	/* beacon frame */
+} __packed;
+
+struct iwl_tx_beacon_cmd {
+	struct iwl_tx_cmd tx;
+	__le16 tim_idx;
+	u8 tim_size;
+	u8 reserved1;
+	struct ieee80211_hdr frame[0];	/* beacon frame */
+} __packed;
+
+/******************************************************************************
+ * (10)
+ * Statistics Commands and Notifications:
+ *
+ *****************************************************************************/
+
+#define IWL_TEMP_CONVERT 260
+
+#define SUP_RATE_11A_MAX_NUM_CHANNELS  8
+#define SUP_RATE_11B_MAX_NUM_CHANNELS  4
+#define SUP_RATE_11G_MAX_NUM_CHANNELS  12
+
+/* Used for passing to driver number of successes and failures per rate */
+struct rate_histogram {
+	union {
+		__le32 a[SUP_RATE_11A_MAX_NUM_CHANNELS];
+		__le32 b[SUP_RATE_11B_MAX_NUM_CHANNELS];
+		__le32 g[SUP_RATE_11G_MAX_NUM_CHANNELS];
+	} success;
+	union {
+		__le32 a[SUP_RATE_11A_MAX_NUM_CHANNELS];
+		__le32 b[SUP_RATE_11B_MAX_NUM_CHANNELS];
+		__le32 g[SUP_RATE_11G_MAX_NUM_CHANNELS];
+	} failed;
+} __packed;
+
+/* statistics command response */
+
+struct iwl39_statistics_rx_phy {
+	__le32 ina_cnt;
+	__le32 fina_cnt;
+	__le32 plcp_err;
+	__le32 crc32_err;
+	__le32 overrun_err;
+	__le32 early_overrun_err;
+	__le32 crc32_good;
+	__le32 false_alarm_cnt;
+	__le32 fina_sync_err_cnt;
+	__le32 sfd_timeout;
+	__le32 fina_timeout;
+	__le32 unresponded_rts;
+	__le32 rxe_frame_limit_overrun;
+	__le32 sent_ack_cnt;
+	__le32 sent_cts_cnt;
+} __packed;
+
+struct iwl39_statistics_rx_non_phy {
+	__le32 bogus_cts;	/* CTS received when not expecting CTS */
+	__le32 bogus_ack;	/* ACK received when not expecting ACK */
+	__le32 non_bssid_frames;	/* number of frames with BSSID that
+					 * doesn't belong to the STA BSSID */
+	__le32 filtered_frames;	/* count frames that were dumped in the
+				 * filtering process */
+	__le32 non_channel_beacons;	/* beacons with our bss id but not on
+					 * our serving channel */
+} __packed;
+
+struct iwl39_statistics_rx {
+	struct iwl39_statistics_rx_phy ofdm;
+	struct iwl39_statistics_rx_phy cck;
+	struct iwl39_statistics_rx_non_phy general;
+} __packed;
+
+struct iwl39_statistics_tx {
+	__le32 preamble_cnt;
+	__le32 rx_detected_cnt;
+	__le32 bt_prio_defer_cnt;
+	__le32 bt_prio_kill_cnt;
+	__le32 few_bytes_cnt;
+	__le32 cts_timeout;
+	__le32 ack_timeout;
+	__le32 expected_ack_cnt;
+	__le32 actual_ack_cnt;
+} __packed;
+
+struct statistics_dbg {
+	__le32 burst_check;
+	__le32 burst_count;
+	__le32 wait_for_silence_timeout_cnt;
+	__le32 reserved[3];
+} __packed;
+
+struct iwl39_statistics_div {
+	__le32 tx_on_a;
+	__le32 tx_on_b;
+	__le32 exec_time;
+	__le32 probe_time;
+} __packed;
+
+struct iwl39_statistics_general {
+	__le32 temperature;
+	struct statistics_dbg dbg;
+	__le32 sleep_time;
+	__le32 slots_out;
+	__le32 slots_idle;
+	__le32 ttl_timestamp;
+	struct iwl39_statistics_div div;
+} __packed;
+
+struct statistics_rx_phy {
+	__le32 ina_cnt;
+	__le32 fina_cnt;
+	__le32 plcp_err;
+	__le32 crc32_err;
+	__le32 overrun_err;
+	__le32 early_overrun_err;
+	__le32 crc32_good;
+	__le32 false_alarm_cnt;
+	__le32 fina_sync_err_cnt;
+	__le32 sfd_timeout;
+	__le32 fina_timeout;
+	__le32 unresponded_rts;
+	__le32 rxe_frame_limit_overrun;
+	__le32 sent_ack_cnt;
+	__le32 sent_cts_cnt;
+	__le32 sent_ba_rsp_cnt;
+	__le32 dsp_self_kill;
+	__le32 mh_format_err;
+	__le32 re_acq_main_rssi_sum;
+	__le32 reserved3;
+} __packed;
+
+struct statistics_rx_ht_phy {
+	__le32 plcp_err;
+	__le32 overrun_err;
+	__le32 early_overrun_err;
+	__le32 crc32_good;
+	__le32 crc32_err;
+	__le32 mh_format_err;
+	__le32 agg_crc32_good;
+	__le32 agg_mpdu_cnt;
+	__le32 agg_cnt;
+	__le32 unsupport_mcs;
+} __packed;
+
+#define INTERFERENCE_DATA_AVAILABLE      cpu_to_le32(1)
+
+struct statistics_rx_non_phy {
+	__le32 bogus_cts;	/* CTS received when not expecting CTS */
+	__le32 bogus_ack;	/* ACK received when not expecting ACK */
+	__le32 non_bssid_frames;	/* number of frames with BSSID that
+					 * doesn't belong to the STA BSSID */
+	__le32 filtered_frames;	/* count frames that were dumped in the
+				 * filtering process */
+	__le32 non_channel_beacons;	/* beacons with our bss id but not on
+					 * our serving channel */
+	__le32 channel_beacons;	/* beacons with our bss id and in our
+				 * serving channel */
+	__le32 num_missed_bcon;	/* number of missed beacons */
+	__le32 adc_rx_saturation_time;	/* count in 0.8us units the time the
+					 * ADC was in saturation */
+	__le32 ina_detection_search_time;/* total time (in 0.8us) searched
+					  * for INA */
+	__le32 beacon_silence_rssi_a;	/* RSSI silence after beacon frame */
+	__le32 beacon_silence_rssi_b;	/* RSSI silence after beacon frame */
+	__le32 beacon_silence_rssi_c;	/* RSSI silence after beacon frame */
+	__le32 interference_data_flag;	/* flag for interference data
+					 * availability. 1 when data is
+					 * available. */
+	__le32 channel_load;		/* counts RX Enable time in uSec */
+	__le32 dsp_false_alarms;	/* DSP false alarm (both OFDM
+					 * and CCK) counter */
+	__le32 beacon_rssi_a;
+	__le32 beacon_rssi_b;
+	__le32 beacon_rssi_c;
+	__le32 beacon_energy_a;
+	__le32 beacon_energy_b;
+	__le32 beacon_energy_c;
+} __packed;
+
+struct statistics_rx {
+	struct statistics_rx_phy ofdm;
+	struct statistics_rx_phy cck;
+	struct statistics_rx_non_phy general;
+	struct statistics_rx_ht_phy ofdm_ht;
+} __packed;
+
+/**
+ * struct statistics_tx_power - current tx power
+ *
+ * @ant_a: current tx power on chain a in 1/2 dB step
+ * @ant_b: current tx power on chain b in 1/2 dB step
+ * @ant_c: current tx power on chain c in 1/2 dB step
+ */
+struct statistics_tx_power {
+	u8 ant_a;
+	u8 ant_b;
+	u8 ant_c;
+	u8 reserved;
+} __packed;
+
+struct statistics_tx_non_phy_agg {
+	__le32 ba_timeout;
+	__le32 ba_reschedule_frames;
+	__le32 scd_query_agg_frame_cnt;
+	__le32 scd_query_no_agg;
+	__le32 scd_query_agg;
+	__le32 scd_query_mismatch;
+	__le32 frame_not_ready;
+	__le32 underrun;
+	__le32 bt_prio_kill;
+	__le32 rx_ba_rsp_cnt;
+} __packed;
+
+struct statistics_tx {
+	__le32 preamble_cnt;
+	__le32 rx_detected_cnt;
+	__le32 bt_prio_defer_cnt;
+	__le32 bt_prio_kill_cnt;
+	__le32 few_bytes_cnt;
+	__le32 cts_timeout;
+	__le32 ack_timeout;
+	__le32 expected_ack_cnt;
+	__le32 actual_ack_cnt;
+	__le32 dump_msdu_cnt;
+	__le32 burst_abort_next_frame_mismatch_cnt;
+	__le32 burst_abort_missing_next_frame_cnt;
+	__le32 cts_timeout_collision;
+	__le32 ack_or_ba_timeout_collision;
+	struct statistics_tx_non_phy_agg agg;
+
+	__le32 reserved1;
+} __packed;
+
+
+struct statistics_div {
+	__le32 tx_on_a;
+	__le32 tx_on_b;
+	__le32 exec_time;
+	__le32 probe_time;
+	__le32 reserved1;
+	__le32 reserved2;
+} __packed;
+
+struct statistics_general_common {
+	__le32 temperature;   /* radio temperature */
+	struct statistics_dbg dbg;
+	__le32 sleep_time;
+	__le32 slots_out;
+	__le32 slots_idle;
+	__le32 ttl_timestamp;
+	struct statistics_div div;
+	__le32 rx_enable_counter;
+	/*
+	 * num_of_sos_states:
+	 *  count the number of times we have to re-tune
+	 *  in order to get out of bad PHY status
+	 */
+	__le32 num_of_sos_states;
+} __packed;
+
+struct statistics_general {
+	struct statistics_general_common common;
+	__le32 reserved2;
+	__le32 reserved3;
+} __packed;
+
+#define UCODE_STATISTICS_CLEAR_MSK		(0x1 << 0)
+#define UCODE_STATISTICS_FREQUENCY_MSK		(0x1 << 1)
+#define UCODE_STATISTICS_NARROW_BAND_MSK	(0x1 << 2)
+
+/*
+ * REPLY_STATISTICS_CMD = 0x9c,
+ * all devices identical.
+ *
+ * This command triggers an immediate response containing uCode statistics.
+ * The response is in the same format as STATISTICS_NOTIFICATION 0x9d, below.
+ *
+ * If the CLEAR_STATS configuration flag is set, uCode will clear its
+ * internal copy of the statistics (counters) after issuing the response.
+ * This flag does not affect STATISTICS_NOTIFICATIONs after beacons (see below).
+ *
+ * If the DISABLE_NOTIF configuration flag is set, uCode will not issue
+ * STATISTICS_NOTIFICATIONs after received beacons (see below).  This flag
+ * does not affect the response to the REPLY_STATISTICS_CMD 0x9c itself.
+ */
+#define IWL_STATS_CONF_CLEAR_STATS cpu_to_le32(0x1)	/* see above */
+#define IWL_STATS_CONF_DISABLE_NOTIF cpu_to_le32(0x2)/* see above */
+struct iwl_statistics_cmd {
+	__le32 configuration_flags;	/* IWL_STATS_CONF_* */
+} __packed;
+
+/*
+ * STATISTICS_NOTIFICATION = 0x9d (notification only, not a command)
+ *
+ * By default, uCode issues this notification after receiving a beacon
+ * while associated.  To disable this behavior, set DISABLE_NOTIF flag in the
+ * REPLY_STATISTICS_CMD 0x9c, above.
+ *
+ * Statistics counters continue to increment beacon after beacon, but are
+ * cleared when changing channels or when driver issues REPLY_STATISTICS_CMD
+ * 0x9c with CLEAR_STATS bit set (see above).
+ *
+ * uCode also issues this notification during scans.  uCode clears statistics
+ * appropriately so that each notification contains statistics for only the
+ * one channel that has just been scanned.
+ */
+#define STATISTICS_REPLY_FLG_BAND_24G_MSK         cpu_to_le32(0x2)
+#define STATISTICS_REPLY_FLG_HT40_MODE_MSK        cpu_to_le32(0x8)
+
+struct iwl3945_notif_statistics {
+	__le32 flag;
+	struct iwl39_statistics_rx rx;
+	struct iwl39_statistics_tx tx;
+	struct iwl39_statistics_general general;
+} __packed;
+
+struct iwl_notif_statistics {
+	__le32 flag;
+	struct statistics_rx rx;
+	struct statistics_tx tx;
+	struct statistics_general general;
+} __packed;
+
+/*
+ * MISSED_BEACONS_NOTIFICATION = 0xa2 (notification only, not a command)
+ *
+ * uCode send MISSED_BEACONS_NOTIFICATION to driver when detect beacon missed
+ * in regardless of how many missed beacons, which mean when driver receive the
+ * notification, inside the command, it can find all the beacons information
+ * which include number of total missed beacons, number of consecutive missed
+ * beacons, number of beacons received and number of beacons expected to
+ * receive.
+ *
+ * If uCode detected consecutive_missed_beacons > 5, it will reset the radio
+ * in order to bring the radio/PHY back to working state; which has no relation
+ * to when driver will perform sensitivity calibration.
+ *
+ * Driver should set it own missed_beacon_threshold to decide when to perform
+ * sensitivity calibration based on number of consecutive missed beacons in
+ * order to improve overall performance, especially in noisy environment.
+ *
+ */
+
+#define IWL_MISSED_BEACON_THRESHOLD_MIN	(1)
+#define IWL_MISSED_BEACON_THRESHOLD_DEF	(5)
+#define IWL_MISSED_BEACON_THRESHOLD_MAX	IWL_MISSED_BEACON_THRESHOLD_DEF
+
+struct iwl_missed_beacon_notif {
+	__le32 consecutive_missed_beacons;
+	__le32 total_missed_becons;
+	__le32 num_expected_beacons;
+	__le32 num_recvd_beacons;
+} __packed;
+
+
+/******************************************************************************
+ * (11)
+ * Rx Calibration Commands:
+ *
+ * With the uCode used for open source drivers, most Tx calibration (except
+ * for Tx Power) and most Rx calibration is done by uCode during the
+ * "initialize" phase of uCode boot.  Driver must calibrate only:
+ *
+ * 1)  Tx power (depends on temperature), described elsewhere
+ * 2)  Receiver gain balance (optimize MIMO, and detect disconnected antennas)
+ * 3)  Receiver sensitivity (to optimize signal detection)
+ *
+ *****************************************************************************/
+
+/**
+ * SENSITIVITY_CMD = 0xa8 (command, has simple generic response)
+ *
+ * This command sets up the Rx signal detector for a sensitivity level that
+ * is high enough to lock onto all signals within the associated network,
+ * but low enough to ignore signals that are below a certain threshold, so as
+ * not to have too many "false alarms".  False alarms are signals that the
+ * Rx DSP tries to lock onto, but then discards after determining that they
+ * are noise.
+ *
+ * The optimum number of false alarms is between 5 and 50 per 200 TUs
+ * (200 * 1024 uSecs, i.e. 204.8 milliseconds) of actual Rx time (i.e.
+ * time listening, not transmitting).  Driver must adjust sensitivity so that
+ * the ratio of actual false alarms to actual Rx time falls within this range.
+ *
+ * While associated, uCode delivers STATISTICS_NOTIFICATIONs after each
+ * received beacon.  These provide information to the driver to analyze the
+ * sensitivity.  Don't analyze statistics that come in from scanning, or any
+ * other non-associated-network source.  Pertinent statistics include:
+ *
+ * From "general" statistics (struct statistics_rx_non_phy):
+ *
+ * (beacon_energy_[abc] & 0x0FF00) >> 8 (unsigned, higher value is lower level)
+ *   Measure of energy of desired signal.  Used for establishing a level
+ *   below which the device does not detect signals.
+ *
+ * (beacon_silence_rssi_[abc] & 0x0FF00) >> 8 (unsigned, units in dB)
+ *   Measure of background noise in silent period after beacon.
+ *
+ * channel_load
+ *   uSecs of actual Rx time during beacon period (varies according to
+ *   how much time was spent transmitting).
+ *
+ * From "cck" and "ofdm" statistics (struct statistics_rx_phy), separately:
+ *
+ * false_alarm_cnt
+ *   Signal locks abandoned early (before phy-level header).
+ *
+ * plcp_err
+ *   Signal locks abandoned late (during phy-level header).
+ *
+ * NOTE:  Both false_alarm_cnt and plcp_err increment monotonically from
+ *        beacon to beacon, i.e. each value is an accumulation of all errors
+ *        before and including the latest beacon.  Values will wrap around to 0
+ *        after counting up to 2^32 - 1.  Driver must differentiate vs.
+ *        previous beacon's values to determine # false alarms in the current
+ *        beacon period.
+ *
+ * Total number of false alarms = false_alarms + plcp_errs
+ *
+ * For OFDM, adjust the following table entries in struct iwl_sensitivity_cmd
+ * (notice that the start points for OFDM are at or close to settings for
+ * maximum sensitivity):
+ *
+ *                                             START  /  MIN  /  MAX
+ *   HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX          90   /   85  /  120
+ *   HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX     170   /  170  /  210
+ *   HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX         105   /  105  /  140
+ *   HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX     220   /  220  /  270
+ *
+ *   If actual rate of OFDM false alarms (+ plcp_errors) is too high
+ *   (greater than 50 for each 204.8 msecs listening), reduce sensitivity
+ *   by *adding* 1 to all 4 of the table entries above, up to the max for
+ *   each entry.  Conversely, if false alarm rate is too low (less than 5
+ *   for each 204.8 msecs listening), *subtract* 1 from each entry to
+ *   increase sensitivity.
+ *
+ * For CCK sensitivity, keep track of the following:
+ *
+ *   1).  20-beacon history of maximum background noise, indicated by
+ *        (beacon_silence_rssi_[abc] & 0x0FF00), units in dB, across the
+ *        3 receivers.  For any given beacon, the "silence reference" is
+ *        the maximum of last 60 samples (20 beacons * 3 receivers).
+ *
+ *   2).  10-beacon history of strongest signal level, as indicated
+ *        by (beacon_energy_[abc] & 0x0FF00) >> 8, across the 3 receivers,
+ *        i.e. the strength of the signal through the best receiver at the
+ *        moment.  These measurements are "upside down", with lower values
+ *        for stronger signals, so max energy will be *minimum* value.
+ *
+ *        Then for any given beacon, the driver must determine the *weakest*
+ *        of the strongest signals; this is the minimum level that needs to be
+ *        successfully detected, when using the best receiver at the moment.
+ *        "Max cck energy" is the maximum (higher value means lower energy!)
+ *        of the last 10 minima.  Once this is determined, driver must add
+ *        a little margin by adding "6" to it.
+ *
+ *   3).  Number of consecutive beacon periods with too few false alarms.
+ *        Reset this to 0 at the first beacon period that falls within the
+ *        "good" range (5 to 50 false alarms per 204.8 milliseconds rx).
+ *
+ * Then, adjust the following CCK table entries in struct iwl_sensitivity_cmd
+ * (notice that the start points for CCK are at maximum sensitivity):
+ *
+ *                                             START  /  MIN  /  MAX
+ *   HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX         125   /  125  /  200
+ *   HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX     200   /  200  /  400
+ *   HD_MIN_ENERGY_CCK_DET_INDEX                100   /    0  /  100
+ *
+ *   If actual rate of CCK false alarms (+ plcp_errors) is too high
+ *   (greater than 50 for each 204.8 msecs listening), method for reducing
+ *   sensitivity is:
+ *
+ *   1)  *Add* 3 to value in HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX,
+ *       up to max 400.
+ *
+ *   2)  If current value in HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX is < 160,
+ *       sensitivity has been reduced a significant amount; bring it up to
+ *       a moderate 161.  Otherwise, *add* 3, up to max 200.
+ *
+ *   3)  a)  If current value in HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX is > 160,
+ *       sensitivity has been reduced only a moderate or small amount;
+ *       *subtract* 2 from value in HD_MIN_ENERGY_CCK_DET_INDEX,
+ *       down to min 0.  Otherwise (if gain has been significantly reduced),
+ *       don't change the HD_MIN_ENERGY_CCK_DET_INDEX value.
+ *
+ *       b)  Save a snapshot of the "silence reference".
+ *
+ *   If actual rate of CCK false alarms (+ plcp_errors) is too low
+ *   (less than 5 for each 204.8 msecs listening), method for increasing
+ *   sensitivity is used only if:
+ *
+ *   1a)  Previous beacon did not have too many false alarms
+ *   1b)  AND difference between previous "silence reference" and current
+ *        "silence reference" (prev - current) is 2 or more,
+ *   OR 2)  100 or more consecutive beacon periods have had rate of
+ *          less than 5 false alarms per 204.8 milliseconds rx time.
+ *
+ *   Method for increasing sensitivity:
+ *
+ *   1)  *Subtract* 3 from value in HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX,
+ *       down to min 125.
+ *
+ *   2)  *Subtract* 3 from value in HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX,
+ *       down to min 200.
+ *
+ *   3)  *Add* 2 to value in HD_MIN_ENERGY_CCK_DET_INDEX, up to max 100.
+ *
+ *   If actual rate of CCK false alarms (+ plcp_errors) is within good range
+ *   (between 5 and 50 for each 204.8 msecs listening):
+ *
+ *   1)  Save a snapshot of the silence reference.
+ *
+ *   2)  If previous beacon had too many CCK false alarms (+ plcp_errors),
+ *       give some extra margin to energy threshold by *subtracting* 8
+ *       from value in HD_MIN_ENERGY_CCK_DET_INDEX.
+ *
+ *   For all cases (too few, too many, good range), make sure that the CCK
+ *   detection threshold (energy) is below the energy level for robust
+ *   detection over the past 10 beacon periods, the "Max cck energy".
+ *   Lower values mean higher energy; this means making sure that the value
+ *   in HD_MIN_ENERGY_CCK_DET_INDEX is at or *above* "Max cck energy".
+ *
+ */
+
+/*
+ * Table entries in SENSITIVITY_CMD (struct iwl_sensitivity_cmd)
+ */
+#define HD_TABLE_SIZE  (11)	/* number of entries */
+#define HD_MIN_ENERGY_CCK_DET_INDEX                 (0)	/* table indexes */
+#define HD_MIN_ENERGY_OFDM_DET_INDEX                (1)
+#define HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX          (2)
+#define HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX      (3)
+#define HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX      (4)
+#define HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX          (5)
+#define HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX      (6)
+#define HD_BARKER_CORR_TH_ADD_MIN_INDEX             (7)
+#define HD_BARKER_CORR_TH_ADD_MIN_MRC_INDEX         (8)
+#define HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX          (9)
+#define HD_OFDM_ENERGY_TH_IN_INDEX                  (10)
+
+/* Control field in struct iwl_sensitivity_cmd */
+#define SENSITIVITY_CMD_CONTROL_DEFAULT_TABLE	cpu_to_le16(0)
+#define SENSITIVITY_CMD_CONTROL_WORK_TABLE	cpu_to_le16(1)
+
+/**
+ * struct iwl_sensitivity_cmd
+ * @control:  (1) updates working table, (0) updates default table
+ * @table:  energy threshold values, use HD_* as index into table
+ *
+ * Always use "1" in "control" to update uCode's working table and DSP.
+ */
+struct iwl_sensitivity_cmd {
+	__le16 control;			/* always use "1" */
+	__le16 table[HD_TABLE_SIZE];	/* use HD_* as index */
+} __packed;
+
+
+/**
+ * REPLY_PHY_CALIBRATION_CMD = 0xb0 (command, has simple generic response)
+ *
+ * This command sets the relative gains of 4965 device's 3 radio receiver chains.
+ *
+ * After the first association, driver should accumulate signal and noise
+ * statistics from the STATISTICS_NOTIFICATIONs that follow the first 20
+ * beacons from the associated network (don't collect statistics that come
+ * in from scanning, or any other non-network source).
+ *
+ * DISCONNECTED ANTENNA:
+ *
+ * Driver should determine which antennas are actually connected, by comparing
+ * average beacon signal levels for the 3 Rx chains.  Accumulate (add) the
+ * following values over 20 beacons, one accumulator for each of the chains
+ * a/b/c, from struct statistics_rx_non_phy:
+ *
+ * beacon_rssi_[abc] & 0x0FF (unsigned, units in dB)
+ *
+ * Find the strongest signal from among a/b/c.  Compare the other two to the
+ * strongest.  If any signal is more than 15 dB (times 20, unless you
+ * divide the accumulated values by 20) below the strongest, the driver
+ * considers that antenna to be disconnected, and should not try to use that
+ * antenna/chain for Rx or Tx.  If both A and B seem to be disconnected,
+ * driver should declare the stronger one as connected, and attempt to use it
+ * (A and B are the only 2 Tx chains!).
+ *
+ *
+ * RX BALANCE:
+ *
+ * Driver should balance the 3 receivers (but just the ones that are connected
+ * to antennas, see above) for gain, by comparing the average signal levels
+ * detected during the silence after each beacon (background noise).
+ * Accumulate (add) the following values over 20 beacons, one accumulator for
+ * each of the chains a/b/c, from struct statistics_rx_non_phy:
+ *
+ * beacon_silence_rssi_[abc] & 0x0FF (unsigned, units in dB)
+ *
+ * Find the weakest background noise level from among a/b/c.  This Rx chain
+ * will be the reference, with 0 gain adjustment.  Attenuate other channels by
+ * finding noise difference:
+ *
+ * (accum_noise[i] - accum_noise[reference]) / 30
+ *
+ * The "30" adjusts the dB in the 20 accumulated samples to units of 1.5 dB.
+ * For use in diff_gain_[abc] fields of struct iwl_calibration_cmd, the
+ * driver should limit the difference results to a range of 0-3 (0-4.5 dB),
+ * and set bit 2 to indicate "reduce gain".  The value for the reference
+ * (weakest) chain should be "0".
+ *
+ * diff_gain_[abc] bit fields:
+ *   2: (1) reduce gain, (0) increase gain
+ * 1-0: amount of gain, units of 1.5 dB
+ */
+
+/* Phy calibration command for series */
+/* The default calibrate table size if not specified by firmware */
+#define IWL_DEFAULT_STANDARD_PHY_CALIBRATE_TBL_SIZE	18
+enum {
+	IWL_PHY_CALIBRATE_DIFF_GAIN_CMD		= 7,
+	IWL_MAX_STANDARD_PHY_CALIBRATE_TBL_SIZE	= 19,
+};
+
+#define IWL_MAX_PHY_CALIBRATE_TBL_SIZE		(253)
+
+struct iwl_calib_hdr {
+	u8 op_code;
+	u8 first_group;
+	u8 groups_num;
+	u8 data_valid;
+} __packed;
+
+/* IWL_PHY_CALIBRATE_DIFF_GAIN_CMD (7) */
+struct iwl_calib_diff_gain_cmd {
+	struct iwl_calib_hdr hdr;
+	s8 diff_gain_a;		/* see above */
+	s8 diff_gain_b;
+	s8 diff_gain_c;
+	u8 reserved1;
+} __packed;
+
+/******************************************************************************
+ * (12)
+ * Miscellaneous Commands:
+ *
+ *****************************************************************************/
+
+/*
+ * LEDs Command & Response
+ * REPLY_LEDS_CMD = 0x48 (command, has simple generic response)
+ *
+ * For each of 3 possible LEDs (Activity/Link/Tech, selected by "id" field),
+ * this command turns it on or off, or sets up a periodic blinking cycle.
+ */
+struct iwl_led_cmd {
+	__le32 interval;	/* "interval" in uSec */
+	u8 id;			/* 1: Activity, 2: Link, 3: Tech */
+	u8 off;			/* # intervals off while blinking;
+				 * "0", with >0 "on" value, turns LED on */
+	u8 on;			/* # intervals on while blinking;
+				 * "0", regardless of "off", turns LED off */
+	u8 reserved;
+} __packed;
+
+
+/******************************************************************************
+ * (13)
+ * Union of all expected notifications/responses:
+ *
+ *****************************************************************************/
+
+struct iwl_rx_packet {
+	/*
+	 * The first 4 bytes of the RX frame header contain both the RX frame
+	 * size and some flags.
+	 * Bit fields:
+	 * 31:    flag flush RB request
+	 * 30:    flag ignore TC (terminal counter) request
+	 * 29:    flag fast IRQ request
+	 * 28-14: Reserved
+	 * 13-00: RX frame size
+	 */
+	__le32 len_n_flags;
+	struct iwl_cmd_header hdr;
+	union {
+		struct iwl3945_rx_frame rx_frame;
+		struct iwl3945_tx_resp tx_resp;
+		struct iwl3945_beacon_notif beacon_status;
+
+		struct iwl_alive_resp alive_frame;
+		struct iwl_spectrum_notification spectrum_notif;
+		struct iwl_csa_notification csa_notif;
+		struct iwl_error_resp err_resp;
+		struct iwl_card_state_notif card_state_notif;
+		struct iwl_add_sta_resp add_sta;
+		struct iwl_rem_sta_resp rem_sta;
+		struct iwl_sleep_notification sleep_notif;
+		struct iwl_spectrum_resp spectrum;
+		struct iwl_notif_statistics stats;
+		struct iwl_compressed_ba_resp compressed_ba;
+		struct iwl_missed_beacon_notif missed_beacon;
+		__le32 status;
+		u8 raw[0];
+	} u;
+} __packed;
+
+#endif				/* __iwl_legacy_commands_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-core.c b/drivers/net/wireless/iwlegacy/iwl-core.c
new file mode 100644
index 000000000000..d418b647be80
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-core.c
@@ -0,0 +1,2674 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <net/mac80211.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-debug.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-power.h"
+#include "iwl-sta.h"
+#include "iwl-helpers.h"
+
+
+MODULE_DESCRIPTION("iwl-legacy: common functions for 3945 and 4965");
+MODULE_VERSION(IWLWIFI_VERSION);
+MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
+MODULE_LICENSE("GPL");
+
+/*
+ * set bt_coex_active to true, uCode will do kill/defer
+ * every time the priority line is asserted (BT is sending signals on the
+ * priority line in the PCIx).
+ * set bt_coex_active to false, uCode will ignore the BT activity and
+ * perform the normal operation
+ *
+ * User might experience transmit issue on some platform due to WiFi/BT
+ * co-exist problem. The possible behaviors are:
+ *   Able to scan and finding all the available AP
+ *   Not able to associate with any AP
+ * On those platforms, WiFi communication can be restored by set
+ * "bt_coex_active" module parameter to "false"
+ *
+ * default: bt_coex_active = true (BT_COEX_ENABLE)
+ */
+static bool bt_coex_active = true;
+module_param(bt_coex_active, bool, S_IRUGO);
+MODULE_PARM_DESC(bt_coex_active, "enable wifi/bluetooth co-exist");
+
+u32 iwlegacy_debug_level;
+EXPORT_SYMBOL(iwlegacy_debug_level);
+
+const u8 iwlegacy_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+EXPORT_SYMBOL(iwlegacy_bcast_addr);
+
+
+/* This function both allocates and initializes hw and priv. */
+struct ieee80211_hw *iwl_legacy_alloc_all(struct iwl_cfg *cfg)
+{
+	struct iwl_priv *priv;
+	/* mac80211 allocates memory for this device instance, including
+	 *   space for this driver's private structure */
+	struct ieee80211_hw *hw;
+
+	hw = ieee80211_alloc_hw(sizeof(struct iwl_priv),
+				cfg->ops->ieee80211_ops);
+	if (hw == NULL) {
+		pr_err("%s: Can not allocate network device\n",
+		       cfg->name);
+		goto out;
+	}
+
+	priv = hw->priv;
+	priv->hw = hw;
+
+out:
+	return hw;
+}
+EXPORT_SYMBOL(iwl_legacy_alloc_all);
+
+#define MAX_BIT_RATE_40_MHZ 150 /* Mbps */
+#define MAX_BIT_RATE_20_MHZ 72 /* Mbps */
+static void iwl_legacy_init_ht_hw_capab(const struct iwl_priv *priv,
+			      struct ieee80211_sta_ht_cap *ht_info,
+			      enum ieee80211_band band)
+{
+	u16 max_bit_rate = 0;
+	u8 rx_chains_num = priv->hw_params.rx_chains_num;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
+
+	ht_info->cap = 0;
+	memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
+
+	ht_info->ht_supported = true;
+
+	ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+	max_bit_rate = MAX_BIT_RATE_20_MHZ;
+	if (priv->hw_params.ht40_channel & BIT(band)) {
+		ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+		ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
+		ht_info->mcs.rx_mask[4] = 0x01;
+		max_bit_rate = MAX_BIT_RATE_40_MHZ;
+	}
+
+	if (priv->cfg->mod_params->amsdu_size_8K)
+		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
+
+	ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
+	ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
+
+	ht_info->mcs.rx_mask[0] = 0xFF;
+	if (rx_chains_num >= 2)
+		ht_info->mcs.rx_mask[1] = 0xFF;
+	if (rx_chains_num >= 3)
+		ht_info->mcs.rx_mask[2] = 0xFF;
+
+	/* Highest supported Rx data rate */
+	max_bit_rate *= rx_chains_num;
+	WARN_ON(max_bit_rate & ~IEEE80211_HT_MCS_RX_HIGHEST_MASK);
+	ht_info->mcs.rx_highest = cpu_to_le16(max_bit_rate);
+
+	/* Tx MCS capabilities */
+	ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+	if (tx_chains_num != rx_chains_num) {
+		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
+		ht_info->mcs.tx_params |= ((tx_chains_num - 1) <<
+				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
+	}
+}
+
+/**
+ * iwl_legacy_init_geos - Initialize mac80211's geo/channel info based from eeprom
+ */
+int iwl_legacy_init_geos(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *channels;
+	struct ieee80211_channel *geo_ch;
+	struct ieee80211_rate *rates;
+	int i = 0;
+
+	if (priv->bands[IEEE80211_BAND_2GHZ].n_bitrates ||
+	    priv->bands[IEEE80211_BAND_5GHZ].n_bitrates) {
+		IWL_DEBUG_INFO(priv, "Geography modes already initialized.\n");
+		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+		return 0;
+	}
+
+	channels = kzalloc(sizeof(struct ieee80211_channel) *
+			   priv->channel_count, GFP_KERNEL);
+	if (!channels)
+		return -ENOMEM;
+
+	rates = kzalloc((sizeof(struct ieee80211_rate) * IWL_RATE_COUNT_LEGACY),
+			GFP_KERNEL);
+	if (!rates) {
+		kfree(channels);
+		return -ENOMEM;
+	}
+
+	/* 5.2GHz channels start after the 2.4GHz channels */
+	sband = &priv->bands[IEEE80211_BAND_5GHZ];
+	sband->channels = &channels[ARRAY_SIZE(iwlegacy_eeprom_band_1)];
+	/* just OFDM */
+	sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
+	sband->n_bitrates = IWL_RATE_COUNT_LEGACY - IWL_FIRST_OFDM_RATE;
+
+	if (priv->cfg->sku & IWL_SKU_N)
+		iwl_legacy_init_ht_hw_capab(priv, &sband->ht_cap,
+					 IEEE80211_BAND_5GHZ);
+
+	sband = &priv->bands[IEEE80211_BAND_2GHZ];
+	sband->channels = channels;
+	/* OFDM & CCK */
+	sband->bitrates = rates;
+	sband->n_bitrates = IWL_RATE_COUNT_LEGACY;
+
+	if (priv->cfg->sku & IWL_SKU_N)
+		iwl_legacy_init_ht_hw_capab(priv, &sband->ht_cap,
+					 IEEE80211_BAND_2GHZ);
+
+	priv->ieee_channels = channels;
+	priv->ieee_rates = rates;
+
+	for (i = 0;  i < priv->channel_count; i++) {
+		ch = &priv->channel_info[i];
+
+		if (!iwl_legacy_is_channel_valid(ch))
+			continue;
+
+		if (iwl_legacy_is_channel_a_band(ch))
+			sband =  &priv->bands[IEEE80211_BAND_5GHZ];
+		else
+			sband =  &priv->bands[IEEE80211_BAND_2GHZ];
+
+		geo_ch = &sband->channels[sband->n_channels++];
+
+		geo_ch->center_freq =
+			ieee80211_channel_to_frequency(ch->channel, ch->band);
+		geo_ch->max_power = ch->max_power_avg;
+		geo_ch->max_antenna_gain = 0xff;
+		geo_ch->hw_value = ch->channel;
+
+		if (iwl_legacy_is_channel_valid(ch)) {
+			if (!(ch->flags & EEPROM_CHANNEL_IBSS))
+				geo_ch->flags |= IEEE80211_CHAN_NO_IBSS;
+
+			if (!(ch->flags & EEPROM_CHANNEL_ACTIVE))
+				geo_ch->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
+
+			if (ch->flags & EEPROM_CHANNEL_RADAR)
+				geo_ch->flags |= IEEE80211_CHAN_RADAR;
+
+			geo_ch->flags |= ch->ht40_extension_channel;
+
+			if (ch->max_power_avg > priv->tx_power_device_lmt)
+				priv->tx_power_device_lmt = ch->max_power_avg;
+		} else {
+			geo_ch->flags |= IEEE80211_CHAN_DISABLED;
+		}
+
+		IWL_DEBUG_INFO(priv, "Channel %d Freq=%d[%sGHz] %s flag=0x%X\n",
+				ch->channel, geo_ch->center_freq,
+				iwl_legacy_is_channel_a_band(ch) ?  "5.2" : "2.4",
+				geo_ch->flags & IEEE80211_CHAN_DISABLED ?
+				"restricted" : "valid",
+				 geo_ch->flags);
+	}
+
+	if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
+	     priv->cfg->sku & IWL_SKU_A) {
+		IWL_INFO(priv, "Incorrectly detected BG card as ABG. "
+			"Please send your PCI ID 0x%04X:0x%04X to maintainer.\n",
+			   priv->pci_dev->device,
+			   priv->pci_dev->subsystem_device);
+		priv->cfg->sku &= ~IWL_SKU_A;
+	}
+
+	IWL_INFO(priv, "Tunable channels: %d 802.11bg, %d 802.11a channels\n",
+		   priv->bands[IEEE80211_BAND_2GHZ].n_channels,
+		   priv->bands[IEEE80211_BAND_5GHZ].n_channels);
+
+	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_init_geos);
+
+/*
+ * iwl_legacy_free_geos - undo allocations in iwl_legacy_init_geos
+ */
+void iwl_legacy_free_geos(struct iwl_priv *priv)
+{
+	kfree(priv->ieee_channels);
+	kfree(priv->ieee_rates);
+	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
+}
+EXPORT_SYMBOL(iwl_legacy_free_geos);
+
+static bool iwl_legacy_is_channel_extension(struct iwl_priv *priv,
+				     enum ieee80211_band band,
+				     u16 channel, u8 extension_chan_offset)
+{
+	const struct iwl_channel_info *ch_info;
+
+	ch_info = iwl_legacy_get_channel_info(priv, band, channel);
+	if (!iwl_legacy_is_channel_valid(ch_info))
+		return false;
+
+	if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_ABOVE)
+		return !(ch_info->ht40_extension_channel &
+					IEEE80211_CHAN_NO_HT40PLUS);
+	else if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_BELOW)
+		return !(ch_info->ht40_extension_channel &
+					IEEE80211_CHAN_NO_HT40MINUS);
+
+	return false;
+}
+
+bool iwl_legacy_is_ht40_tx_allowed(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    struct ieee80211_sta_ht_cap *ht_cap)
+{
+	if (!ctx->ht.enabled || !ctx->ht.is_40mhz)
+		return false;
+
+	/*
+	 * We do not check for IEEE80211_HT_CAP_SUP_WIDTH_20_40
+	 * the bit will not set if it is pure 40MHz case
+	 */
+	if (ht_cap && !ht_cap->ht_supported)
+		return false;
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+	if (priv->disable_ht40)
+		return false;
+#endif
+
+	return iwl_legacy_is_channel_extension(priv, priv->band,
+			le16_to_cpu(ctx->staging.channel),
+			ctx->ht.extension_chan_offset);
+}
+EXPORT_SYMBOL(iwl_legacy_is_ht40_tx_allowed);
+
+static u16 iwl_legacy_adjust_beacon_interval(u16 beacon_val, u16 max_beacon_val)
+{
+	u16 new_val;
+	u16 beacon_factor;
+
+	/*
+	 * If mac80211 hasn't given us a beacon interval, program
+	 * the default into the device.
+	 */
+	if (!beacon_val)
+		return DEFAULT_BEACON_INTERVAL;
+
+	/*
+	 * If the beacon interval we obtained from the peer
+	 * is too large, we'll have to wake up more often
+	 * (and in IBSS case, we'll beacon too much)
+	 *
+	 * For example, if max_beacon_val is 4096, and the
+	 * requested beacon interval is 7000, we'll have to
+	 * use 3500 to be able to wake up on the beacons.
+	 *
+	 * This could badly influence beacon detection stats.
+	 */
+
+	beacon_factor = (beacon_val + max_beacon_val) / max_beacon_val;
+	new_val = beacon_val / beacon_factor;
+
+	if (!new_val)
+		new_val = max_beacon_val;
+
+	return new_val;
+}
+
+int
+iwl_legacy_send_rxon_timing(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+{
+	u64 tsf;
+	s32 interval_tm, rem;
+	struct ieee80211_conf *conf = NULL;
+	u16 beacon_int;
+	struct ieee80211_vif *vif = ctx->vif;
+
+	conf = iwl_legacy_ieee80211_get_hw_conf(priv->hw);
+
+	lockdep_assert_held(&priv->mutex);
+
+	memset(&ctx->timing, 0, sizeof(struct iwl_rxon_time_cmd));
+
+	ctx->timing.timestamp = cpu_to_le64(priv->timestamp);
+	ctx->timing.listen_interval = cpu_to_le16(conf->listen_interval);
+
+	beacon_int = vif ? vif->bss_conf.beacon_int : 0;
+
+	/*
+	 * TODO: For IBSS we need to get atim_window from mac80211,
+	 *	 for now just always use 0
+	 */
+	ctx->timing.atim_window = 0;
+
+	beacon_int = iwl_legacy_adjust_beacon_interval(beacon_int,
+			priv->hw_params.max_beacon_itrvl * TIME_UNIT);
+	ctx->timing.beacon_interval = cpu_to_le16(beacon_int);
+
+	tsf = priv->timestamp; /* tsf is modifed by do_div: copy it */
+	interval_tm = beacon_int * TIME_UNIT;
+	rem = do_div(tsf, interval_tm);
+	ctx->timing.beacon_init_val = cpu_to_le32(interval_tm - rem);
+
+	ctx->timing.dtim_period = vif ? (vif->bss_conf.dtim_period ?: 1) : 1;
+
+	IWL_DEBUG_ASSOC(priv,
+			"beacon interval %d beacon timer %d beacon tim %d\n",
+			le16_to_cpu(ctx->timing.beacon_interval),
+			le32_to_cpu(ctx->timing.beacon_init_val),
+			le16_to_cpu(ctx->timing.atim_window));
+
+	return iwl_legacy_send_cmd_pdu(priv, ctx->rxon_timing_cmd,
+				sizeof(ctx->timing), &ctx->timing);
+}
+EXPORT_SYMBOL(iwl_legacy_send_rxon_timing);
+
+void
+iwl_legacy_set_rxon_hwcrypto(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx,
+				int hw_decrypt)
+{
+	struct iwl_legacy_rxon_cmd *rxon = &ctx->staging;
+
+	if (hw_decrypt)
+		rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
+	else
+		rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;
+
+}
+EXPORT_SYMBOL(iwl_legacy_set_rxon_hwcrypto);
+
+/* validate RXON structure is valid */
+int
+iwl_legacy_check_rxon_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+{
+	struct iwl_legacy_rxon_cmd *rxon = &ctx->staging;
+	bool error = false;
+
+	if (rxon->flags & RXON_FLG_BAND_24G_MSK) {
+		if (rxon->flags & RXON_FLG_TGJ_NARROW_BAND_MSK) {
+			IWL_WARN(priv, "check 2.4G: wrong narrow\n");
+			error = true;
+		}
+		if (rxon->flags & RXON_FLG_RADAR_DETECT_MSK) {
+			IWL_WARN(priv, "check 2.4G: wrong radar\n");
+			error = true;
+		}
+	} else {
+		if (!(rxon->flags & RXON_FLG_SHORT_SLOT_MSK)) {
+			IWL_WARN(priv, "check 5.2G: not short slot!\n");
+			error = true;
+		}
+		if (rxon->flags & RXON_FLG_CCK_MSK) {
+			IWL_WARN(priv, "check 5.2G: CCK!\n");
+			error = true;
+		}
+	}
+	if ((rxon->node_addr[0] | rxon->bssid_addr[0]) & 0x1) {
+		IWL_WARN(priv, "mac/bssid mcast!\n");
+		error = true;
+	}
+
+	/* make sure basic rates 6Mbps and 1Mbps are supported */
+	if ((rxon->ofdm_basic_rates & IWL_RATE_6M_MASK) == 0 &&
+	    (rxon->cck_basic_rates & IWL_RATE_1M_MASK) == 0) {
+		IWL_WARN(priv, "neither 1 nor 6 are basic\n");
+		error = true;
+	}
+
+	if (le16_to_cpu(rxon->assoc_id) > 2007) {
+		IWL_WARN(priv, "aid > 2007\n");
+		error = true;
+	}
+
+	if ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK)) {
+		IWL_WARN(priv, "CCK and short slot\n");
+		error = true;
+	}
+
+	if ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK)) {
+		IWL_WARN(priv, "CCK and auto detect");
+		error = true;
+	}
+
+	if ((rxon->flags & (RXON_FLG_AUTO_DETECT_MSK |
+			    RXON_FLG_TGG_PROTECT_MSK)) ==
+			    RXON_FLG_TGG_PROTECT_MSK) {
+		IWL_WARN(priv, "TGg but no auto-detect\n");
+		error = true;
+	}
+
+	if (error)
+		IWL_WARN(priv, "Tuning to channel %d\n",
+			    le16_to_cpu(rxon->channel));
+
+	if (error) {
+		IWL_ERR(priv, "Invalid RXON\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_check_rxon_cmd);
+
+/**
+ * iwl_legacy_full_rxon_required - check if full RXON (vs RXON_ASSOC) cmd is needed
+ * @priv: staging_rxon is compared to active_rxon
+ *
+ * If the RXON structure is changing enough to require a new tune,
+ * or is clearing the RXON_FILTER_ASSOC_MSK, then return 1 to indicate that
+ * a new tune (full RXON command, rather than RXON_ASSOC cmd) is required.
+ */
+int iwl_legacy_full_rxon_required(struct iwl_priv *priv,
+			   struct iwl_rxon_context *ctx)
+{
+	const struct iwl_legacy_rxon_cmd *staging = &ctx->staging;
+	const struct iwl_legacy_rxon_cmd *active = &ctx->active;
+
+#define CHK(cond)							\
+	if ((cond)) {							\
+		IWL_DEBUG_INFO(priv, "need full RXON - " #cond "\n");	\
+		return 1;						\
+	}
+
+#define CHK_NEQ(c1, c2)						\
+	if ((c1) != (c2)) {					\
+		IWL_DEBUG_INFO(priv, "need full RXON - "	\
+			       #c1 " != " #c2 " - %d != %d\n",	\
+			       (c1), (c2));			\
+		return 1;					\
+	}
+
+	/* These items are only settable from the full RXON command */
+	CHK(!iwl_legacy_is_associated_ctx(ctx));
+	CHK(compare_ether_addr(staging->bssid_addr, active->bssid_addr));
+	CHK(compare_ether_addr(staging->node_addr, active->node_addr));
+	CHK(compare_ether_addr(staging->wlap_bssid_addr,
+				active->wlap_bssid_addr));
+	CHK_NEQ(staging->dev_type, active->dev_type);
+	CHK_NEQ(staging->channel, active->channel);
+	CHK_NEQ(staging->air_propagation, active->air_propagation);
+	CHK_NEQ(staging->ofdm_ht_single_stream_basic_rates,
+		active->ofdm_ht_single_stream_basic_rates);
+	CHK_NEQ(staging->ofdm_ht_dual_stream_basic_rates,
+		active->ofdm_ht_dual_stream_basic_rates);
+	CHK_NEQ(staging->assoc_id, active->assoc_id);
+
+	/* flags, filter_flags, ofdm_basic_rates, and cck_basic_rates can
+	 * be updated with the RXON_ASSOC command -- however only some
+	 * flag transitions are allowed using RXON_ASSOC */
+
+	/* Check if we are not switching bands */
+	CHK_NEQ(staging->flags & RXON_FLG_BAND_24G_MSK,
+		active->flags & RXON_FLG_BAND_24G_MSK);
+
+	/* Check if we are switching association toggle */
+	CHK_NEQ(staging->filter_flags & RXON_FILTER_ASSOC_MSK,
+		active->filter_flags & RXON_FILTER_ASSOC_MSK);
+
+#undef CHK
+#undef CHK_NEQ
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_full_rxon_required);
+
+u8 iwl_legacy_get_lowest_plcp(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx)
+{
+	/*
+	 * Assign the lowest rate -- should really get this from
+	 * the beacon skb from mac80211.
+	 */
+	if (ctx->staging.flags & RXON_FLG_BAND_24G_MSK)
+		return IWL_RATE_1M_PLCP;
+	else
+		return IWL_RATE_6M_PLCP;
+}
+EXPORT_SYMBOL(iwl_legacy_get_lowest_plcp);
+
+static void _iwl_legacy_set_rxon_ht(struct iwl_priv *priv,
+			     struct iwl_ht_config *ht_conf,
+			     struct iwl_rxon_context *ctx)
+{
+	struct iwl_legacy_rxon_cmd *rxon = &ctx->staging;
+
+	if (!ctx->ht.enabled) {
+		rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MSK |
+			RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK |
+			RXON_FLG_HT40_PROT_MSK |
+			RXON_FLG_HT_PROT_MSK);
+		return;
+	}
+
+	rxon->flags |= cpu_to_le32(ctx->ht.protection <<
+					RXON_FLG_HT_OPERATING_MODE_POS);
+
+	/* Set up channel bandwidth:
+	 * 20 MHz only, 20/40 mixed or pure 40 if ht40 ok */
+	/* clear the HT channel mode before set the mode */
+	rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MSK |
+			 RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
+	if (iwl_legacy_is_ht40_tx_allowed(priv, ctx, NULL)) {
+		/* pure ht40 */
+		if (ctx->ht.protection ==
+				IEEE80211_HT_OP_MODE_PROTECTION_20MHZ) {
+			rxon->flags |= RXON_FLG_CHANNEL_MODE_PURE_40;
+			/* Note: control channel is opposite of extension channel */
+			switch (ctx->ht.extension_chan_offset) {
+			case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+				rxon->flags &=
+					~RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+				break;
+			case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+				rxon->flags |=
+					RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+				break;
+			}
+		} else {
+			/* Note: control channel is opposite of extension channel */
+			switch (ctx->ht.extension_chan_offset) {
+			case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+				rxon->flags &=
+					~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
+				rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED;
+				break;
+			case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+				rxon->flags |=
+					RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+				rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED;
+				break;
+			case IEEE80211_HT_PARAM_CHA_SEC_NONE:
+			default:
+				/* channel location only valid if in Mixed mode */
+				IWL_ERR(priv,
+					"invalid extension channel offset\n");
+				break;
+			}
+		}
+	} else {
+		rxon->flags |= RXON_FLG_CHANNEL_MODE_LEGACY;
+	}
+
+	if (priv->cfg->ops->hcmd->set_rxon_chain)
+		priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
+
+	IWL_DEBUG_ASSOC(priv, "rxon flags 0x%X operation mode :0x%X "
+			"extension channel offset 0x%x\n",
+			le32_to_cpu(rxon->flags), ctx->ht.protection,
+			ctx->ht.extension_chan_offset);
+}
+
+void iwl_legacy_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf)
+{
+	struct iwl_rxon_context *ctx;
+
+	for_each_context(priv, ctx)
+		_iwl_legacy_set_rxon_ht(priv, ht_conf, ctx);
+}
+EXPORT_SYMBOL(iwl_legacy_set_rxon_ht);
+
+/* Return valid, unused, channel for a passive scan to reset the RF */
+u8 iwl_legacy_get_single_channel_number(struct iwl_priv *priv,
+				 enum ieee80211_band band)
+{
+	const struct iwl_channel_info *ch_info;
+	int i;
+	u8 channel = 0;
+	u8 min, max;
+	struct iwl_rxon_context *ctx;
+
+	if (band == IEEE80211_BAND_5GHZ) {
+		min = 14;
+		max = priv->channel_count;
+	} else {
+		min = 0;
+		max = 14;
+	}
+
+	for (i = min; i < max; i++) {
+		bool busy = false;
+
+		for_each_context(priv, ctx) {
+			busy = priv->channel_info[i].channel ==
+				le16_to_cpu(ctx->staging.channel);
+			if (busy)
+				break;
+		}
+
+		if (busy)
+			continue;
+
+		channel = priv->channel_info[i].channel;
+		ch_info = iwl_legacy_get_channel_info(priv, band, channel);
+		if (iwl_legacy_is_channel_valid(ch_info))
+			break;
+	}
+
+	return channel;
+}
+EXPORT_SYMBOL(iwl_legacy_get_single_channel_number);
+
+/**
+ * iwl_legacy_set_rxon_channel - Set the band and channel values in staging RXON
+ * @ch: requested channel as a pointer to struct ieee80211_channel
+
+ * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
+ * in the staging RXON flag structure based on the ch->band
+ */
+int
+iwl_legacy_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch,
+			 struct iwl_rxon_context *ctx)
+{
+	enum ieee80211_band band = ch->band;
+	u16 channel = ch->hw_value;
+
+	if ((le16_to_cpu(ctx->staging.channel) == channel) &&
+	    (priv->band == band))
+		return 0;
+
+	ctx->staging.channel = cpu_to_le16(channel);
+	if (band == IEEE80211_BAND_5GHZ)
+		ctx->staging.flags &= ~RXON_FLG_BAND_24G_MSK;
+	else
+		ctx->staging.flags |= RXON_FLG_BAND_24G_MSK;
+
+	priv->band = band;
+
+	IWL_DEBUG_INFO(priv, "Staging channel set to %d [%d]\n", channel, band);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_set_rxon_channel);
+
+void iwl_legacy_set_flags_for_band(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    enum ieee80211_band band,
+			    struct ieee80211_vif *vif)
+{
+	if (band == IEEE80211_BAND_5GHZ) {
+		ctx->staging.flags &=
+		    ~(RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK
+		      | RXON_FLG_CCK_MSK);
+		ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
+	} else {
+		/* Copied from iwl_post_associate() */
+		if (vif && vif->bss_conf.use_short_slot)
+			ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
+		else
+			ctx->staging.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		ctx->staging.flags |= RXON_FLG_BAND_24G_MSK;
+		ctx->staging.flags |= RXON_FLG_AUTO_DETECT_MSK;
+		ctx->staging.flags &= ~RXON_FLG_CCK_MSK;
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_set_flags_for_band);
+
+/*
+ * initialize rxon structure with default values from eeprom
+ */
+void iwl_legacy_connection_init_rx_config(struct iwl_priv *priv,
+				   struct iwl_rxon_context *ctx)
+{
+	const struct iwl_channel_info *ch_info;
+
+	memset(&ctx->staging, 0, sizeof(ctx->staging));
+
+	if (!ctx->vif) {
+		ctx->staging.dev_type = ctx->unused_devtype;
+	} else
+	switch (ctx->vif->type) {
+
+	case NL80211_IFTYPE_STATION:
+		ctx->staging.dev_type = ctx->station_devtype;
+		ctx->staging.filter_flags = RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case NL80211_IFTYPE_ADHOC:
+		ctx->staging.dev_type = ctx->ibss_devtype;
+		ctx->staging.flags = RXON_FLG_SHORT_PREAMBLE_MSK;
+		ctx->staging.filter_flags = RXON_FILTER_BCON_AWARE_MSK |
+						  RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	default:
+		IWL_ERR(priv, "Unsupported interface type %d\n",
+			ctx->vif->type);
+		break;
+	}
+
+#if 0
+	/* TODO:  Figure out when short_preamble would be set and cache from
+	 * that */
+	if (!hw_to_local(priv->hw)->short_preamble)
+		ctx->staging.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+	else
+		ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+#endif
+
+	ch_info = iwl_legacy_get_channel_info(priv, priv->band,
+				       le16_to_cpu(ctx->active.channel));
+
+	if (!ch_info)
+		ch_info = &priv->channel_info[0];
+
+	ctx->staging.channel = cpu_to_le16(ch_info->channel);
+	priv->band = ch_info->band;
+
+	iwl_legacy_set_flags_for_band(priv, ctx, priv->band, ctx->vif);
+
+	ctx->staging.ofdm_basic_rates =
+	    (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
+	ctx->staging.cck_basic_rates =
+	    (IWL_CCK_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
+
+	/* clear both MIX and PURE40 mode flag */
+	ctx->staging.flags &= ~(RXON_FLG_CHANNEL_MODE_MIXED |
+					RXON_FLG_CHANNEL_MODE_PURE_40);
+	if (ctx->vif)
+		memcpy(ctx->staging.node_addr, ctx->vif->addr, ETH_ALEN);
+
+	ctx->staging.ofdm_ht_single_stream_basic_rates = 0xff;
+	ctx->staging.ofdm_ht_dual_stream_basic_rates = 0xff;
+}
+EXPORT_SYMBOL(iwl_legacy_connection_init_rx_config);
+
+void iwl_legacy_set_rate(struct iwl_priv *priv)
+{
+	const struct ieee80211_supported_band *hw = NULL;
+	struct ieee80211_rate *rate;
+	struct iwl_rxon_context *ctx;
+	int i;
+
+	hw = iwl_get_hw_mode(priv, priv->band);
+	if (!hw) {
+		IWL_ERR(priv, "Failed to set rate: unable to get hw mode\n");
+		return;
+	}
+
+	priv->active_rate = 0;
+
+	for (i = 0; i < hw->n_bitrates; i++) {
+		rate = &(hw->bitrates[i]);
+		if (rate->hw_value < IWL_RATE_COUNT_LEGACY)
+			priv->active_rate |= (1 << rate->hw_value);
+	}
+
+	IWL_DEBUG_RATE(priv, "Set active_rate = %0x\n", priv->active_rate);
+
+	for_each_context(priv, ctx) {
+		ctx->staging.cck_basic_rates =
+		    (IWL_CCK_BASIC_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
+
+		ctx->staging.ofdm_basic_rates =
+		   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_set_rate);
+
+void iwl_legacy_chswitch_done(struct iwl_priv *priv, bool is_success)
+{
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (priv->switch_rxon.switch_in_progress) {
+		ieee80211_chswitch_done(ctx->vif, is_success);
+		mutex_lock(&priv->mutex);
+		priv->switch_rxon.switch_in_progress = false;
+		mutex_unlock(&priv->mutex);
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_chswitch_done);
+
+void iwl_legacy_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_csa_notification *csa = &(pkt->u.csa_notif);
+
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	struct iwl_legacy_rxon_cmd *rxon = (void *)&ctx->active;
+
+	if (priv->switch_rxon.switch_in_progress) {
+		if (!le32_to_cpu(csa->status) &&
+		    (csa->channel == priv->switch_rxon.channel)) {
+			rxon->channel = csa->channel;
+			ctx->staging.channel = csa->channel;
+			IWL_DEBUG_11H(priv, "CSA notif: channel %d\n",
+			      le16_to_cpu(csa->channel));
+			iwl_legacy_chswitch_done(priv, true);
+		} else {
+			IWL_ERR(priv, "CSA notif (fail) : channel %d\n",
+			      le16_to_cpu(csa->channel));
+			iwl_legacy_chswitch_done(priv, false);
+		}
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_rx_csa);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+void iwl_legacy_print_rx_config_cmd(struct iwl_priv *priv,
+			     struct iwl_rxon_context *ctx)
+{
+	struct iwl_legacy_rxon_cmd *rxon = &ctx->staging;
+
+	IWL_DEBUG_RADIO(priv, "RX CONFIG:\n");
+	iwl_print_hex_dump(priv, IWL_DL_RADIO, (u8 *) rxon, sizeof(*rxon));
+	IWL_DEBUG_RADIO(priv, "u16 channel: 0x%x\n",
+				le16_to_cpu(rxon->channel));
+	IWL_DEBUG_RADIO(priv, "u32 flags: 0x%08X\n", le32_to_cpu(rxon->flags));
+	IWL_DEBUG_RADIO(priv, "u32 filter_flags: 0x%08x\n",
+				le32_to_cpu(rxon->filter_flags));
+	IWL_DEBUG_RADIO(priv, "u8 dev_type: 0x%x\n", rxon->dev_type);
+	IWL_DEBUG_RADIO(priv, "u8 ofdm_basic_rates: 0x%02x\n",
+			rxon->ofdm_basic_rates);
+	IWL_DEBUG_RADIO(priv, "u8 cck_basic_rates: 0x%02x\n",
+				rxon->cck_basic_rates);
+	IWL_DEBUG_RADIO(priv, "u8[6] node_addr: %pM\n", rxon->node_addr);
+	IWL_DEBUG_RADIO(priv, "u8[6] bssid_addr: %pM\n", rxon->bssid_addr);
+	IWL_DEBUG_RADIO(priv, "u16 assoc_id: 0x%x\n",
+				le16_to_cpu(rxon->assoc_id));
+}
+EXPORT_SYMBOL(iwl_legacy_print_rx_config_cmd);
+#endif
+/**
+ * iwl_legacy_irq_handle_error - called for HW or SW error interrupt from card
+ */
+void iwl_legacy_irq_handle_error(struct iwl_priv *priv)
+{
+	/* Set the FW error flag -- cleared on iwl_down */
+	set_bit(STATUS_FW_ERROR, &priv->status);
+
+	/* Cancel currently queued command. */
+	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+
+	IWL_ERR(priv, "Loaded firmware version: %s\n",
+		priv->hw->wiphy->fw_version);
+
+	priv->cfg->ops->lib->dump_nic_error_log(priv);
+	if (priv->cfg->ops->lib->dump_fh)
+		priv->cfg->ops->lib->dump_fh(priv, NULL, false);
+	priv->cfg->ops->lib->dump_nic_event_log(priv, false, NULL, false);
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & IWL_DL_FW_ERRORS)
+		iwl_legacy_print_rx_config_cmd(priv,
+					&priv->contexts[IWL_RXON_CTX_BSS]);
+#endif
+
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	/* Keep the restart process from trying to send host
+	 * commands by clearing the INIT status bit */
+	clear_bit(STATUS_READY, &priv->status);
+
+	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_DEBUG(priv, IWL_DL_FW_ERRORS,
+			  "Restarting adapter due to uCode error.\n");
+
+		if (priv->cfg->mod_params->restart_fw)
+			queue_work(priv->workqueue, &priv->restart);
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_irq_handle_error);
+
+static int iwl_legacy_apm_stop_master(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	/* stop device's busmaster DMA activity */
+	iwl_legacy_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
+
+	ret = iwl_poll_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_MASTER_DISABLED,
+			CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
+	if (ret)
+		IWL_WARN(priv, "Master Disable Timed Out, 100 usec\n");
+
+	IWL_DEBUG_INFO(priv, "stop master\n");
+
+	return ret;
+}
+
+void iwl_legacy_apm_stop(struct iwl_priv *priv)
+{
+	IWL_DEBUG_INFO(priv, "Stop card, put in low power state\n");
+
+	/* Stop device's DMA activity */
+	iwl_legacy_apm_stop_master(priv);
+
+	/* Reset the entire device */
+	iwl_legacy_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+
+	udelay(10);
+
+	/*
+	 * Clear "initialization complete" bit to move adapter from
+	 * D0A* (powered-up Active) --> D0U* (Uninitialized) state.
+	 */
+	iwl_legacy_clear_bit(priv, CSR_GP_CNTRL,
+				CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+}
+EXPORT_SYMBOL(iwl_legacy_apm_stop);
+
+
+/*
+ * Start up NIC's basic functionality after it has been reset
+ * (e.g. after platform boot, or shutdown via iwl_legacy_apm_stop())
+ * NOTE:  This does not load uCode nor start the embedded processor
+ */
+int iwl_legacy_apm_init(struct iwl_priv *priv)
+{
+	int ret = 0;
+	u16 lctl;
+
+	IWL_DEBUG_INFO(priv, "Init card's basic functions\n");
+
+	/*
+	 * Use "set_bit" below rather than "write", to preserve any hardware
+	 * bits already set by default after reset.
+	 */
+
+	/* Disable L0S exit timer (platform NMI Work/Around) */
+	iwl_legacy_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+			  CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
+
+	/*
+	 * Disable L0s without affecting L1;
+	 *  don't wait for ICH L0s (ICH bug W/A)
+	 */
+	iwl_legacy_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+			  CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
+
+	/* Set FH wait threshold to maximum (HW error during stress W/A) */
+	iwl_legacy_set_bit(priv, CSR_DBG_HPET_MEM_REG,
+					CSR_DBG_HPET_MEM_REG_VAL);
+
+	/*
+	 * Enable HAP INTA (interrupt from management bus) to
+	 * wake device's PCI Express link L1a -> L0s
+	 * NOTE:  This is no-op for 3945 (non-existant bit)
+	 */
+	iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+				    CSR_HW_IF_CONFIG_REG_BIT_HAP_WAKE_L1A);
+
+	/*
+	 * HW bug W/A for instability in PCIe bus L0->L0S->L1 transition.
+	 * Check if BIOS (or OS) enabled L1-ASPM on this device.
+	 * If so (likely), disable L0S, so device moves directly L0->L1;
+	 *    costs negligible amount of power savings.
+	 * If not (unlikely), enable L0S, so there is at least some
+	 *    power savings, even without L1.
+	 */
+	if (priv->cfg->base_params->set_l0s) {
+		lctl = iwl_legacy_pcie_link_ctl(priv);
+		if ((lctl & PCI_CFG_LINK_CTRL_VAL_L1_EN) ==
+					PCI_CFG_LINK_CTRL_VAL_L1_EN) {
+			/* L1-ASPM enabled; disable(!) L0S  */
+			iwl_legacy_set_bit(priv, CSR_GIO_REG,
+					CSR_GIO_REG_VAL_L0S_ENABLED);
+			IWL_DEBUG_POWER(priv, "L1 Enabled; Disabling L0S\n");
+		} else {
+			/* L1-ASPM disabled; enable(!) L0S */
+			iwl_legacy_clear_bit(priv, CSR_GIO_REG,
+					CSR_GIO_REG_VAL_L0S_ENABLED);
+			IWL_DEBUG_POWER(priv, "L1 Disabled; Enabling L0S\n");
+		}
+	}
+
+	/* Configure analog phase-lock-loop before activating to D0A */
+	if (priv->cfg->base_params->pll_cfg_val)
+		iwl_legacy_set_bit(priv, CSR_ANA_PLL_CFG,
+			    priv->cfg->base_params->pll_cfg_val);
+
+	/*
+	 * Set "initialization complete" bit to move adapter from
+	 * D0U* --> D0A* (powered-up active) state.
+	 */
+	iwl_legacy_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+	/*
+	 * Wait for clock stabilization; once stabilized, access to
+	 * device-internal resources is supported, e.g. iwl_legacy_write_prph()
+	 * and accesses to uCode SRAM.
+	 */
+	ret = iwl_poll_bit(priv, CSR_GP_CNTRL,
+			CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+	if (ret < 0) {
+		IWL_DEBUG_INFO(priv, "Failed to init the card\n");
+		goto out;
+	}
+
+	/*
+	 * Enable DMA and BSM (if used) clocks, wait for them to stabilize.
+	 * BSM (Boostrap State Machine) is only in 3945 and 4965.
+	 *
+	 * Write to "CLK_EN_REG"; "1" bits enable clocks, while "0" bits
+	 * do not disable clocks.  This preserves any hardware bits already
+	 * set by default in "CLK_CTRL_REG" after reset.
+	 */
+	if (priv->cfg->base_params->use_bsm)
+		iwl_legacy_write_prph(priv, APMG_CLK_EN_REG,
+			APMG_CLK_VAL_DMA_CLK_RQT | APMG_CLK_VAL_BSM_CLK_RQT);
+	else
+		iwl_legacy_write_prph(priv, APMG_CLK_EN_REG,
+			APMG_CLK_VAL_DMA_CLK_RQT);
+	udelay(20);
+
+	/* Disable L1-Active */
+	iwl_legacy_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
+			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+
+out:
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_apm_init);
+
+
+int iwl_legacy_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
+{
+	int ret;
+	s8 prev_tx_power;
+	bool defer;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (priv->tx_power_user_lmt == tx_power && !force)
+		return 0;
+
+	if (!priv->cfg->ops->lib->send_tx_power)
+		return -EOPNOTSUPP;
+
+	if (tx_power < IWL4965_TX_POWER_TARGET_POWER_MIN) {
+		IWL_WARN(priv,
+			 "Requested user TXPOWER %d below lower limit %d.\n",
+			 tx_power,
+			 IWL4965_TX_POWER_TARGET_POWER_MIN);
+		return -EINVAL;
+	}
+
+	if (tx_power > priv->tx_power_device_lmt) {
+		IWL_WARN(priv,
+			"Requested user TXPOWER %d above upper limit %d.\n",
+			 tx_power, priv->tx_power_device_lmt);
+		return -EINVAL;
+	}
+
+	if (!iwl_legacy_is_ready_rf(priv))
+		return -EIO;
+
+	/* scan complete and commit_rxon use tx_power_next value,
+	 * it always need to be updated for newest request */
+	priv->tx_power_next = tx_power;
+
+	/* do not set tx power when scanning or channel changing */
+	defer = test_bit(STATUS_SCANNING, &priv->status) ||
+		memcmp(&ctx->active, &ctx->staging, sizeof(ctx->staging));
+	if (defer && !force) {
+		IWL_DEBUG_INFO(priv, "Deferring tx power set\n");
+		return 0;
+	}
+
+	prev_tx_power = priv->tx_power_user_lmt;
+	priv->tx_power_user_lmt = tx_power;
+
+	ret = priv->cfg->ops->lib->send_tx_power(priv);
+
+	/* if fail to set tx_power, restore the orig. tx power */
+	if (ret) {
+		priv->tx_power_user_lmt = prev_tx_power;
+		priv->tx_power_next = prev_tx_power;
+	}
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_set_tx_power);
+
+void iwl_legacy_send_bt_config(struct iwl_priv *priv)
+{
+	struct iwl_bt_cmd bt_cmd = {
+		.lead_time = BT_LEAD_TIME_DEF,
+		.max_kill = BT_MAX_KILL_DEF,
+		.kill_ack_mask = 0,
+		.kill_cts_mask = 0,
+	};
+
+	if (!bt_coex_active)
+		bt_cmd.flags = BT_COEX_DISABLE;
+	else
+		bt_cmd.flags = BT_COEX_ENABLE;
+
+	IWL_DEBUG_INFO(priv, "BT coex %s\n",
+		(bt_cmd.flags == BT_COEX_DISABLE) ? "disable" : "active");
+
+	if (iwl_legacy_send_cmd_pdu(priv, REPLY_BT_CONFIG,
+			     sizeof(struct iwl_bt_cmd), &bt_cmd))
+		IWL_ERR(priv, "failed to send BT Coex Config\n");
+}
+EXPORT_SYMBOL(iwl_legacy_send_bt_config);
+
+int iwl_legacy_send_statistics_request(struct iwl_priv *priv, u8 flags, bool clear)
+{
+	struct iwl_statistics_cmd statistics_cmd = {
+		.configuration_flags =
+			clear ? IWL_STATS_CONF_CLEAR_STATS : 0,
+	};
+
+	if (flags & CMD_ASYNC)
+		return iwl_legacy_send_cmd_pdu_async(priv, REPLY_STATISTICS_CMD,
+					sizeof(struct iwl_statistics_cmd),
+					&statistics_cmd, NULL);
+	else
+		return iwl_legacy_send_cmd_pdu(priv, REPLY_STATISTICS_CMD,
+					sizeof(struct iwl_statistics_cmd),
+					&statistics_cmd);
+}
+EXPORT_SYMBOL(iwl_legacy_send_statistics_request);
+
+void iwl_legacy_rx_pm_sleep_notif(struct iwl_priv *priv,
+			   struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_sleep_notification *sleep = &(pkt->u.sleep_notif);
+	IWL_DEBUG_RX(priv, "sleep mode: %d, src: %d\n",
+		     sleep->pm_sleep_mode, sleep->pm_wakeup_src);
+#endif
+}
+EXPORT_SYMBOL(iwl_legacy_rx_pm_sleep_notif);
+
+void iwl_legacy_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
+				      struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	u32 len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+	IWL_DEBUG_RADIO(priv, "Dumping %d bytes of unhandled "
+			"notification for %s:\n", len,
+			iwl_legacy_get_cmd_string(pkt->hdr.cmd));
+	iwl_print_hex_dump(priv, IWL_DL_RADIO, pkt->u.raw, len);
+}
+EXPORT_SYMBOL(iwl_legacy_rx_pm_debug_statistics_notif);
+
+void iwl_legacy_rx_reply_error(struct iwl_priv *priv,
+			struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+
+	IWL_ERR(priv, "Error Reply type 0x%08X cmd %s (0x%02X) "
+		"seq 0x%04X ser 0x%08X\n",
+		le32_to_cpu(pkt->u.err_resp.error_type),
+		iwl_legacy_get_cmd_string(pkt->u.err_resp.cmd_id),
+		pkt->u.err_resp.cmd_id,
+		le16_to_cpu(pkt->u.err_resp.bad_cmd_seq_num),
+		le32_to_cpu(pkt->u.err_resp.error_info));
+}
+EXPORT_SYMBOL(iwl_legacy_rx_reply_error);
+
+void iwl_legacy_clear_isr_stats(struct iwl_priv *priv)
+{
+	memset(&priv->isr_stats, 0, sizeof(priv->isr_stats));
+}
+
+int iwl_legacy_mac_conf_tx(struct ieee80211_hw *hw, u16 queue,
+			   const struct ieee80211_tx_queue_params *params)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_rxon_context *ctx;
+	unsigned long flags;
+	int q;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	if (!iwl_legacy_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211(priv, "leave - RF not ready\n");
+		return -EIO;
+	}
+
+	if (queue >= AC_NUM) {
+		IWL_DEBUG_MAC80211(priv, "leave - queue >= AC_NUM %d\n", queue);
+		return 0;
+	}
+
+	q = AC_NUM - 1 - queue;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	for_each_context(priv, ctx) {
+		ctx->qos_data.def_qos_parm.ac[q].cw_min =
+			cpu_to_le16(params->cw_min);
+		ctx->qos_data.def_qos_parm.ac[q].cw_max =
+			cpu_to_le16(params->cw_max);
+		ctx->qos_data.def_qos_parm.ac[q].aifsn = params->aifs;
+		ctx->qos_data.def_qos_parm.ac[q].edca_txop =
+				cpu_to_le16((params->txop * 32));
+
+		ctx->qos_data.def_qos_parm.ac[q].reserved1 = 0;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_mac_conf_tx);
+
+int iwl_legacy_mac_tx_last_beacon(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	return priv->ibss_manager == IWL_IBSS_MANAGER;
+}
+EXPORT_SYMBOL_GPL(iwl_legacy_mac_tx_last_beacon);
+
+static int
+iwl_legacy_set_mode(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+{
+	iwl_legacy_connection_init_rx_config(priv, ctx);
+
+	if (priv->cfg->ops->hcmd->set_rxon_chain)
+		priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
+
+	return iwl_legacy_commit_rxon(priv, ctx);
+}
+
+static int iwl_legacy_setup_interface(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx)
+{
+	struct ieee80211_vif *vif = ctx->vif;
+	int err;
+
+	lockdep_assert_held(&priv->mutex);
+
+	/*
+	 * This variable will be correct only when there's just
+	 * a single context, but all code using it is for hardware
+	 * that supports only one context.
+	 */
+	priv->iw_mode = vif->type;
+
+	ctx->is_active = true;
+
+	err = iwl_legacy_set_mode(priv, ctx);
+	if (err) {
+		if (!ctx->always_active)
+			ctx->is_active = false;
+		return err;
+	}
+
+	return 0;
+}
+
+int
+iwl_legacy_mac_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
+	struct iwl_rxon_context *tmp, *ctx = NULL;
+	int err;
+
+	IWL_DEBUG_MAC80211(priv, "enter: type %d, addr %pM\n",
+			   vif->type, vif->addr);
+
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_legacy_is_ready_rf(priv)) {
+		IWL_WARN(priv, "Try to add interface when device not ready\n");
+		err = -EINVAL;
+		goto out;
+	}
+
+	for_each_context(priv, tmp) {
+		u32 possible_modes =
+			tmp->interface_modes | tmp->exclusive_interface_modes;
+
+		if (tmp->vif) {
+			/* check if this busy context is exclusive */
+			if (tmp->exclusive_interface_modes &
+						BIT(tmp->vif->type)) {
+				err = -EINVAL;
+				goto out;
+			}
+			continue;
+		}
+
+		if (!(possible_modes & BIT(vif->type)))
+			continue;
+
+		/* have maybe usable context w/o interface */
+		ctx = tmp;
+		break;
+	}
+
+	if (!ctx) {
+		err = -EOPNOTSUPP;
+		goto out;
+	}
+
+	vif_priv->ctx = ctx;
+	ctx->vif = vif;
+
+	err = iwl_legacy_setup_interface(priv, ctx);
+	if (!err)
+		goto out;
+
+	ctx->vif = NULL;
+	priv->iw_mode = NL80211_IFTYPE_STATION;
+ out:
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+	return err;
+}
+EXPORT_SYMBOL(iwl_legacy_mac_add_interface);
+
+static void iwl_legacy_teardown_interface(struct iwl_priv *priv,
+				   struct ieee80211_vif *vif,
+				   bool mode_change)
+{
+	struct iwl_rxon_context *ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (priv->scan_vif == vif) {
+		iwl_legacy_scan_cancel_timeout(priv, 200);
+		iwl_legacy_force_scan_end(priv);
+	}
+
+	if (!mode_change) {
+		iwl_legacy_set_mode(priv, ctx);
+		if (!ctx->always_active)
+			ctx->is_active = false;
+	}
+}
+
+void iwl_legacy_mac_remove_interface(struct ieee80211_hw *hw,
+			      struct ieee80211_vif *vif)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_rxon_context *ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	mutex_lock(&priv->mutex);
+
+	WARN_ON(ctx->vif != vif);
+	ctx->vif = NULL;
+
+	iwl_legacy_teardown_interface(priv, vif, false);
+
+	memset(priv->bssid, 0, ETH_ALEN);
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+
+}
+EXPORT_SYMBOL(iwl_legacy_mac_remove_interface);
+
+int iwl_legacy_alloc_txq_mem(struct iwl_priv *priv)
+{
+	if (!priv->txq)
+		priv->txq = kzalloc(
+			sizeof(struct iwl_tx_queue) *
+				priv->cfg->base_params->num_of_queues,
+			GFP_KERNEL);
+	if (!priv->txq) {
+		IWL_ERR(priv, "Not enough memory for txq\n");
+		return -ENOMEM;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_alloc_txq_mem);
+
+void iwl_legacy_txq_mem(struct iwl_priv *priv)
+{
+	kfree(priv->txq);
+	priv->txq = NULL;
+}
+EXPORT_SYMBOL(iwl_legacy_txq_mem);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+
+#define IWL_TRAFFIC_DUMP_SIZE	(IWL_TRAFFIC_ENTRY_SIZE * IWL_TRAFFIC_ENTRIES)
+
+void iwl_legacy_reset_traffic_log(struct iwl_priv *priv)
+{
+	priv->tx_traffic_idx = 0;
+	priv->rx_traffic_idx = 0;
+	if (priv->tx_traffic)
+		memset(priv->tx_traffic, 0, IWL_TRAFFIC_DUMP_SIZE);
+	if (priv->rx_traffic)
+		memset(priv->rx_traffic, 0, IWL_TRAFFIC_DUMP_SIZE);
+}
+
+int iwl_legacy_alloc_traffic_mem(struct iwl_priv *priv)
+{
+	u32 traffic_size = IWL_TRAFFIC_DUMP_SIZE;
+
+	if (iwlegacy_debug_level & IWL_DL_TX) {
+		if (!priv->tx_traffic) {
+			priv->tx_traffic =
+				kzalloc(traffic_size, GFP_KERNEL);
+			if (!priv->tx_traffic)
+				return -ENOMEM;
+		}
+	}
+	if (iwlegacy_debug_level & IWL_DL_RX) {
+		if (!priv->rx_traffic) {
+			priv->rx_traffic =
+				kzalloc(traffic_size, GFP_KERNEL);
+			if (!priv->rx_traffic)
+				return -ENOMEM;
+		}
+	}
+	iwl_legacy_reset_traffic_log(priv);
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_alloc_traffic_mem);
+
+void iwl_legacy_free_traffic_mem(struct iwl_priv *priv)
+{
+	kfree(priv->tx_traffic);
+	priv->tx_traffic = NULL;
+
+	kfree(priv->rx_traffic);
+	priv->rx_traffic = NULL;
+}
+EXPORT_SYMBOL(iwl_legacy_free_traffic_mem);
+
+void iwl_legacy_dbg_log_tx_data_frame(struct iwl_priv *priv,
+		      u16 length, struct ieee80211_hdr *header)
+{
+	__le16 fc;
+	u16 len;
+
+	if (likely(!(iwlegacy_debug_level & IWL_DL_TX)))
+		return;
+
+	if (!priv->tx_traffic)
+		return;
+
+	fc = header->frame_control;
+	if (ieee80211_is_data(fc)) {
+		len = (length > IWL_TRAFFIC_ENTRY_SIZE)
+		       ? IWL_TRAFFIC_ENTRY_SIZE : length;
+		memcpy((priv->tx_traffic +
+		       (priv->tx_traffic_idx * IWL_TRAFFIC_ENTRY_SIZE)),
+		       header, len);
+		priv->tx_traffic_idx =
+			(priv->tx_traffic_idx + 1) % IWL_TRAFFIC_ENTRIES;
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_dbg_log_tx_data_frame);
+
+void iwl_legacy_dbg_log_rx_data_frame(struct iwl_priv *priv,
+		      u16 length, struct ieee80211_hdr *header)
+{
+	__le16 fc;
+	u16 len;
+
+	if (likely(!(iwlegacy_debug_level & IWL_DL_RX)))
+		return;
+
+	if (!priv->rx_traffic)
+		return;
+
+	fc = header->frame_control;
+	if (ieee80211_is_data(fc)) {
+		len = (length > IWL_TRAFFIC_ENTRY_SIZE)
+		       ? IWL_TRAFFIC_ENTRY_SIZE : length;
+		memcpy((priv->rx_traffic +
+		       (priv->rx_traffic_idx * IWL_TRAFFIC_ENTRY_SIZE)),
+		       header, len);
+		priv->rx_traffic_idx =
+			(priv->rx_traffic_idx + 1) % IWL_TRAFFIC_ENTRIES;
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_dbg_log_rx_data_frame);
+
+const char *iwl_legacy_get_mgmt_string(int cmd)
+{
+	switch (cmd) {
+		IWL_CMD(MANAGEMENT_ASSOC_REQ);
+		IWL_CMD(MANAGEMENT_ASSOC_RESP);
+		IWL_CMD(MANAGEMENT_REASSOC_REQ);
+		IWL_CMD(MANAGEMENT_REASSOC_RESP);
+		IWL_CMD(MANAGEMENT_PROBE_REQ);
+		IWL_CMD(MANAGEMENT_PROBE_RESP);
+		IWL_CMD(MANAGEMENT_BEACON);
+		IWL_CMD(MANAGEMENT_ATIM);
+		IWL_CMD(MANAGEMENT_DISASSOC);
+		IWL_CMD(MANAGEMENT_AUTH);
+		IWL_CMD(MANAGEMENT_DEAUTH);
+		IWL_CMD(MANAGEMENT_ACTION);
+	default:
+		return "UNKNOWN";
+
+	}
+}
+
+const char *iwl_legacy_get_ctrl_string(int cmd)
+{
+	switch (cmd) {
+		IWL_CMD(CONTROL_BACK_REQ);
+		IWL_CMD(CONTROL_BACK);
+		IWL_CMD(CONTROL_PSPOLL);
+		IWL_CMD(CONTROL_RTS);
+		IWL_CMD(CONTROL_CTS);
+		IWL_CMD(CONTROL_ACK);
+		IWL_CMD(CONTROL_CFEND);
+		IWL_CMD(CONTROL_CFENDACK);
+	default:
+		return "UNKNOWN";
+
+	}
+}
+
+void iwl_legacy_clear_traffic_stats(struct iwl_priv *priv)
+{
+	memset(&priv->tx_stats, 0, sizeof(struct traffic_stats));
+	memset(&priv->rx_stats, 0, sizeof(struct traffic_stats));
+}
+
+/*
+ * if CONFIG_IWLWIFI_LEGACY_DEBUGFS defined,
+ * iwl_legacy_update_stats function will
+ * record all the MGMT, CTRL and DATA pkt for both TX and Rx pass
+ * Use debugFs to display the rx/rx_statistics
+ * if CONFIG_IWLWIFI_LEGACY_DEBUGFS not being defined, then no MGMT and CTRL
+ * information will be recorded, but DATA pkt still will be recorded
+ * for the reason of iwl_led.c need to control the led blinking based on
+ * number of tx and rx data.
+ *
+ */
+void
+iwl_legacy_update_stats(struct iwl_priv *priv, bool is_tx, __le16 fc, u16 len)
+{
+	struct traffic_stats	*stats;
+
+	if (is_tx)
+		stats = &priv->tx_stats;
+	else
+		stats = &priv->rx_stats;
+
+	if (ieee80211_is_mgmt(fc)) {
+		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
+		case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
+			stats->mgmt[MANAGEMENT_ASSOC_REQ]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP):
+			stats->mgmt[MANAGEMENT_ASSOC_RESP]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
+			stats->mgmt[MANAGEMENT_REASSOC_REQ]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_REASSOC_RESP):
+			stats->mgmt[MANAGEMENT_REASSOC_RESP]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_PROBE_REQ):
+			stats->mgmt[MANAGEMENT_PROBE_REQ]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_PROBE_RESP):
+			stats->mgmt[MANAGEMENT_PROBE_RESP]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_BEACON):
+			stats->mgmt[MANAGEMENT_BEACON]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_ATIM):
+			stats->mgmt[MANAGEMENT_ATIM]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_DISASSOC):
+			stats->mgmt[MANAGEMENT_DISASSOC]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_AUTH):
+			stats->mgmt[MANAGEMENT_AUTH]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
+			stats->mgmt[MANAGEMENT_DEAUTH]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_ACTION):
+			stats->mgmt[MANAGEMENT_ACTION]++;
+			break;
+		}
+	} else if (ieee80211_is_ctl(fc)) {
+		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
+		case cpu_to_le16(IEEE80211_STYPE_BACK_REQ):
+			stats->ctrl[CONTROL_BACK_REQ]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_BACK):
+			stats->ctrl[CONTROL_BACK]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_PSPOLL):
+			stats->ctrl[CONTROL_PSPOLL]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_RTS):
+			stats->ctrl[CONTROL_RTS]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_CTS):
+			stats->ctrl[CONTROL_CTS]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_ACK):
+			stats->ctrl[CONTROL_ACK]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_CFEND):
+			stats->ctrl[CONTROL_CFEND]++;
+			break;
+		case cpu_to_le16(IEEE80211_STYPE_CFENDACK):
+			stats->ctrl[CONTROL_CFENDACK]++;
+			break;
+		}
+	} else {
+		/* data */
+		stats->data_cnt++;
+		stats->data_bytes += len;
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_update_stats);
+#endif
+
+static void _iwl_legacy_force_rf_reset(struct iwl_priv *priv)
+{
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (!iwl_legacy_is_any_associated(priv)) {
+		IWL_DEBUG_SCAN(priv, "force reset rejected: not associated\n");
+		return;
+	}
+	/*
+	 * There is no easy and better way to force reset the radio,
+	 * the only known method is switching channel which will force to
+	 * reset and tune the radio.
+	 * Use internal short scan (single channel) operation to should
+	 * achieve this objective.
+	 * Driver should reset the radio when number of consecutive missed
+	 * beacon, or any other uCode error condition detected.
+	 */
+	IWL_DEBUG_INFO(priv, "perform radio reset.\n");
+	iwl_legacy_internal_short_hw_scan(priv);
+}
+
+
+int iwl_legacy_force_reset(struct iwl_priv *priv, int mode, bool external)
+{
+	struct iwl_force_reset *force_reset;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return -EINVAL;
+
+	if (mode >= IWL_MAX_FORCE_RESET) {
+		IWL_DEBUG_INFO(priv, "invalid reset request.\n");
+		return -EINVAL;
+	}
+	force_reset = &priv->force_reset[mode];
+	force_reset->reset_request_count++;
+	if (!external) {
+		if (force_reset->last_force_reset_jiffies &&
+		    time_after(force_reset->last_force_reset_jiffies +
+		    force_reset->reset_duration, jiffies)) {
+			IWL_DEBUG_INFO(priv, "force reset rejected\n");
+			force_reset->reset_reject_count++;
+			return -EAGAIN;
+		}
+	}
+	force_reset->reset_success_count++;
+	force_reset->last_force_reset_jiffies = jiffies;
+	IWL_DEBUG_INFO(priv, "perform force reset (%d)\n", mode);
+	switch (mode) {
+	case IWL_RF_RESET:
+		_iwl_legacy_force_rf_reset(priv);
+		break;
+	case IWL_FW_RESET:
+		/*
+		 * if the request is from external(ex: debugfs),
+		 * then always perform the request in regardless the module
+		 * parameter setting
+		 * if the request is from internal (uCode error or driver
+		 * detect failure), then fw_restart module parameter
+		 * need to be check before performing firmware reload
+		 */
+		if (!external && !priv->cfg->mod_params->restart_fw) {
+			IWL_DEBUG_INFO(priv, "Cancel firmware reload based on "
+				       "module parameter setting\n");
+			break;
+		}
+		IWL_ERR(priv, "On demand firmware reload\n");
+		/* Set the FW error flag -- cleared on iwl_down */
+		set_bit(STATUS_FW_ERROR, &priv->status);
+		wake_up_interruptible(&priv->wait_command_queue);
+		/*
+		 * Keep the restart process from trying to send host
+		 * commands by clearing the INIT status bit
+		 */
+		clear_bit(STATUS_READY, &priv->status);
+		queue_work(priv->workqueue, &priv->restart);
+		break;
+	}
+	return 0;
+}
+
+int
+iwl_legacy_mac_change_interface(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			enum nl80211_iftype newtype, bool newp2p)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_rxon_context *ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+	struct iwl_rxon_context *tmp;
+	u32 interface_modes;
+	int err;
+
+	newtype = ieee80211_iftype_p2p(newtype, newp2p);
+
+	mutex_lock(&priv->mutex);
+
+	interface_modes = ctx->interface_modes | ctx->exclusive_interface_modes;
+
+	if (!(interface_modes & BIT(newtype))) {
+		err = -EBUSY;
+		goto out;
+	}
+
+	if (ctx->exclusive_interface_modes & BIT(newtype)) {
+		for_each_context(priv, tmp) {
+			if (ctx == tmp)
+				continue;
+
+			if (!tmp->vif)
+				continue;
+
+			/*
+			 * The current mode switch would be exclusive, but
+			 * another context is active ... refuse the switch.
+			 */
+			err = -EBUSY;
+			goto out;
+		}
+	}
+
+	/* success */
+	iwl_legacy_teardown_interface(priv, vif, true);
+	vif->type = newtype;
+	err = iwl_legacy_setup_interface(priv, ctx);
+	WARN_ON(err);
+	/*
+	 * We've switched internally, but submitting to the
+	 * device may have failed for some reason. Mask this
+	 * error, because otherwise mac80211 will not switch
+	 * (and set the interface type back) and we'll be
+	 * out of sync with it.
+	 */
+	err = 0;
+
+ out:
+	mutex_unlock(&priv->mutex);
+	return err;
+}
+EXPORT_SYMBOL(iwl_legacy_mac_change_interface);
+
+/*
+ * On every watchdog tick we check (latest) time stamp. If it does not
+ * change during timeout period and queue is not empty we reset firmware.
+ */
+static int iwl_legacy_check_stuck_queue(struct iwl_priv *priv, int cnt)
+{
+	struct iwl_tx_queue *txq = &priv->txq[cnt];
+	struct iwl_queue *q = &txq->q;
+	unsigned long timeout;
+	int ret;
+
+	if (q->read_ptr == q->write_ptr) {
+		txq->time_stamp = jiffies;
+		return 0;
+	}
+
+	timeout = txq->time_stamp +
+		  msecs_to_jiffies(priv->cfg->base_params->wd_timeout);
+
+	if (time_after(jiffies, timeout)) {
+		IWL_ERR(priv, "Queue %d stuck for %u ms.\n",
+				q->id, priv->cfg->base_params->wd_timeout);
+		ret = iwl_legacy_force_reset(priv, IWL_FW_RESET, false);
+		return (ret == -EAGAIN) ? 0 : 1;
+	}
+
+	return 0;
+}
+
+/*
+ * Making watchdog tick be a quarter of timeout assure we will
+ * discover the queue hung between timeout and 1.25*timeout
+ */
+#define IWL_WD_TICK(timeout) ((timeout) / 4)
+
+/*
+ * Watchdog timer callback, we check each tx queue for stuck, if if hung
+ * we reset the firmware. If everything is fine just rearm the timer.
+ */
+void iwl_legacy_bg_watchdog(unsigned long data)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)data;
+	int cnt;
+	unsigned long timeout;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	timeout = priv->cfg->base_params->wd_timeout;
+	if (timeout == 0)
+		return;
+
+	/* monitor and check for stuck cmd queue */
+	if (iwl_legacy_check_stuck_queue(priv, priv->cmd_queue))
+		return;
+
+	/* monitor and check for other stuck queues */
+	if (iwl_legacy_is_any_associated(priv)) {
+		for (cnt = 0; cnt < priv->hw_params.max_txq_num; cnt++) {
+			/* skip as we already checked the command queue */
+			if (cnt == priv->cmd_queue)
+				continue;
+			if (iwl_legacy_check_stuck_queue(priv, cnt))
+				return;
+		}
+	}
+
+	mod_timer(&priv->watchdog, jiffies +
+		  msecs_to_jiffies(IWL_WD_TICK(timeout)));
+}
+EXPORT_SYMBOL(iwl_legacy_bg_watchdog);
+
+void iwl_legacy_setup_watchdog(struct iwl_priv *priv)
+{
+	unsigned int timeout = priv->cfg->base_params->wd_timeout;
+
+	if (timeout)
+		mod_timer(&priv->watchdog,
+			  jiffies + msecs_to_jiffies(IWL_WD_TICK(timeout)));
+	else
+		del_timer(&priv->watchdog);
+}
+EXPORT_SYMBOL(iwl_legacy_setup_watchdog);
+
+/*
+ * extended beacon time format
+ * time in usec will be changed into a 32-bit value in extended:internal format
+ * the extended part is the beacon counts
+ * the internal part is the time in usec within one beacon interval
+ */
+u32
+iwl_legacy_usecs_to_beacons(struct iwl_priv *priv,
+					u32 usec, u32 beacon_interval)
+{
+	u32 quot;
+	u32 rem;
+	u32 interval = beacon_interval * TIME_UNIT;
+
+	if (!interval || !usec)
+		return 0;
+
+	quot = (usec / interval) &
+		(iwl_legacy_beacon_time_mask_high(priv,
+		priv->hw_params.beacon_time_tsf_bits) >>
+		priv->hw_params.beacon_time_tsf_bits);
+	rem = (usec % interval) & iwl_legacy_beacon_time_mask_low(priv,
+				   priv->hw_params.beacon_time_tsf_bits);
+
+	return (quot << priv->hw_params.beacon_time_tsf_bits) + rem;
+}
+EXPORT_SYMBOL(iwl_legacy_usecs_to_beacons);
+
+/* base is usually what we get from ucode with each received frame,
+ * the same as HW timer counter counting down
+ */
+__le32 iwl_legacy_add_beacon_time(struct iwl_priv *priv, u32 base,
+			   u32 addon, u32 beacon_interval)
+{
+	u32 base_low = base & iwl_legacy_beacon_time_mask_low(priv,
+					priv->hw_params.beacon_time_tsf_bits);
+	u32 addon_low = addon & iwl_legacy_beacon_time_mask_low(priv,
+					priv->hw_params.beacon_time_tsf_bits);
+	u32 interval = beacon_interval * TIME_UNIT;
+	u32 res = (base & iwl_legacy_beacon_time_mask_high(priv,
+				priv->hw_params.beacon_time_tsf_bits)) +
+				(addon & iwl_legacy_beacon_time_mask_high(priv,
+				priv->hw_params.beacon_time_tsf_bits));
+
+	if (base_low > addon_low)
+		res += base_low - addon_low;
+	else if (base_low < addon_low) {
+		res += interval + base_low - addon_low;
+		res += (1 << priv->hw_params.beacon_time_tsf_bits);
+	} else
+		res += (1 << priv->hw_params.beacon_time_tsf_bits);
+
+	return cpu_to_le32(res);
+}
+EXPORT_SYMBOL(iwl_legacy_add_beacon_time);
+
+#ifdef CONFIG_PM
+
+int iwl_legacy_pci_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+
+	/*
+	 * This function is called when system goes into suspend state
+	 * mac80211 will call iwl_mac_stop() from the mac80211 suspend function
+	 * first but since iwl_mac_stop() has no knowledge of who the caller is,
+	 * it will not call apm_ops.stop() to stop the DMA operation.
+	 * Calling apm_ops.stop here to make sure we stop the DMA.
+	 */
+	iwl_legacy_apm_stop(priv);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_pci_suspend);
+
+int iwl_legacy_pci_resume(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+	bool hw_rfkill = false;
+
+	/*
+	 * We disable the RETRY_TIMEOUT register (0x41) to keep
+	 * PCI Tx retries from interfering with C3 CPU state.
+	 */
+	pci_write_config_byte(pdev, PCI_CFG_RETRY_TIMEOUT, 0x00);
+
+	iwl_legacy_enable_interrupts(priv);
+
+	if (!(iwl_read32(priv, CSR_GP_CNTRL) &
+				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW))
+		hw_rfkill = true;
+
+	if (hw_rfkill)
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+
+	wiphy_rfkill_set_hw_state(priv->hw->wiphy, hw_rfkill);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_pci_resume);
+
+const struct dev_pm_ops iwl_legacy_pm_ops = {
+	.suspend = iwl_legacy_pci_suspend,
+	.resume = iwl_legacy_pci_resume,
+	.freeze = iwl_legacy_pci_suspend,
+	.thaw = iwl_legacy_pci_resume,
+	.poweroff = iwl_legacy_pci_suspend,
+	.restore = iwl_legacy_pci_resume,
+};
+EXPORT_SYMBOL(iwl_legacy_pm_ops);
+
+#endif /* CONFIG_PM */
+
+static void
+iwl_legacy_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+{
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (!ctx->is_active)
+		return;
+
+	ctx->qos_data.def_qos_parm.qos_flags = 0;
+
+	if (ctx->qos_data.qos_active)
+		ctx->qos_data.def_qos_parm.qos_flags |=
+			QOS_PARAM_FLG_UPDATE_EDCA_MSK;
+
+	if (ctx->ht.enabled)
+		ctx->qos_data.def_qos_parm.qos_flags |= QOS_PARAM_FLG_TGN_MSK;
+
+	IWL_DEBUG_QOS(priv, "send QoS cmd with Qos active=%d FLAGS=0x%X\n",
+		      ctx->qos_data.qos_active,
+		      ctx->qos_data.def_qos_parm.qos_flags);
+
+	iwl_legacy_send_cmd_pdu_async(priv, ctx->qos_cmd,
+			       sizeof(struct iwl_qosparam_cmd),
+			       &ctx->qos_data.def_qos_parm, NULL);
+}
+
+/**
+ * iwl_legacy_mac_config - mac80211 config callback
+ */
+int iwl_legacy_mac_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct iwl_priv *priv = hw->priv;
+	const struct iwl_channel_info *ch_info;
+	struct ieee80211_conf *conf = &hw->conf;
+	struct ieee80211_channel *channel = conf->channel;
+	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
+	struct iwl_rxon_context *ctx;
+	unsigned long flags = 0;
+	int ret = 0;
+	u16 ch;
+	int scan_active = 0;
+	bool ht_changed[NUM_IWL_RXON_CTX] = {};
+
+	if (WARN_ON(!priv->cfg->ops->legacy))
+		return -EOPNOTSUPP;
+
+	mutex_lock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211(priv, "enter to channel %d changed 0x%X\n",
+					channel->hw_value, changed);
+
+	if (unlikely(!priv->cfg->mod_params->disable_hw_scan &&
+			test_bit(STATUS_SCANNING, &priv->status))) {
+		scan_active = 1;
+		IWL_DEBUG_MAC80211(priv, "leave - scanning\n");
+	}
+
+	if (changed & (IEEE80211_CONF_CHANGE_SMPS |
+		       IEEE80211_CONF_CHANGE_CHANNEL)) {
+		/* mac80211 uses static for non-HT which is what we want */
+		priv->current_ht_config.smps = conf->smps_mode;
+
+		/*
+		 * Recalculate chain counts.
+		 *
+		 * If monitor mode is enabled then mac80211 will
+		 * set up the SM PS mode to OFF if an HT channel is
+		 * configured.
+		 */
+		if (priv->cfg->ops->hcmd->set_rxon_chain)
+			for_each_context(priv, ctx)
+				priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
+	}
+
+	/* during scanning mac80211 will delay channel setting until
+	 * scan finish with changed = 0
+	 */
+	if (!changed || (changed & IEEE80211_CONF_CHANGE_CHANNEL)) {
+		if (scan_active)
+			goto set_ch_out;
+
+		ch = channel->hw_value;
+		ch_info = iwl_legacy_get_channel_info(priv, channel->band, ch);
+		if (!iwl_legacy_is_channel_valid(ch_info)) {
+			IWL_DEBUG_MAC80211(priv, "leave - invalid channel\n");
+			ret = -EINVAL;
+			goto set_ch_out;
+		}
+
+		spin_lock_irqsave(&priv->lock, flags);
+
+		for_each_context(priv, ctx) {
+			/* Configure HT40 channels */
+			if (ctx->ht.enabled != conf_is_ht(conf)) {
+				ctx->ht.enabled = conf_is_ht(conf);
+				ht_changed[ctx->ctxid] = true;
+			}
+			if (ctx->ht.enabled) {
+				if (conf_is_ht40_minus(conf)) {
+					ctx->ht.extension_chan_offset =
+					IEEE80211_HT_PARAM_CHA_SEC_BELOW;
+					ctx->ht.is_40mhz = true;
+				} else if (conf_is_ht40_plus(conf)) {
+					ctx->ht.extension_chan_offset =
+					IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
+					ctx->ht.is_40mhz = true;
+				} else {
+					ctx->ht.extension_chan_offset =
+					IEEE80211_HT_PARAM_CHA_SEC_NONE;
+					ctx->ht.is_40mhz = false;
+				}
+			} else
+				ctx->ht.is_40mhz = false;
+
+			/*
+			 * Default to no protection. Protection mode will
+			 * later be set from BSS config in iwl_ht_conf
+			 */
+			ctx->ht.protection =
+					IEEE80211_HT_OP_MODE_PROTECTION_NONE;
+
+			/* if we are switching from ht to 2.4 clear flags
+			 * from any ht related info since 2.4 does not
+			 * support ht */
+			if ((le16_to_cpu(ctx->staging.channel) != ch))
+				ctx->staging.flags = 0;
+
+			iwl_legacy_set_rxon_channel(priv, channel, ctx);
+			iwl_legacy_set_rxon_ht(priv, ht_conf);
+
+			iwl_legacy_set_flags_for_band(priv, ctx, channel->band,
+					       ctx->vif);
+		}
+
+		spin_unlock_irqrestore(&priv->lock, flags);
+
+		if (priv->cfg->ops->legacy->update_bcast_stations)
+			ret =
+			priv->cfg->ops->legacy->update_bcast_stations(priv);
+
+ set_ch_out:
+		/* The list of supported rates and rate mask can be different
+		 * for each band; since the band may have changed, reset
+		 * the rate mask to what mac80211 lists */
+		iwl_legacy_set_rate(priv);
+	}
+
+	if (changed & (IEEE80211_CONF_CHANGE_PS |
+			IEEE80211_CONF_CHANGE_IDLE)) {
+		ret = iwl_legacy_power_update_mode(priv, false);
+		if (ret)
+			IWL_DEBUG_MAC80211(priv, "Error setting sleep level\n");
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		IWL_DEBUG_MAC80211(priv, "TX Power old=%d new=%d\n",
+			priv->tx_power_user_lmt, conf->power_level);
+
+		iwl_legacy_set_tx_power(priv, conf->power_level, false);
+	}
+
+	if (!iwl_legacy_is_ready(priv)) {
+		IWL_DEBUG_MAC80211(priv, "leave - not ready\n");
+		goto out;
+	}
+
+	if (scan_active)
+		goto out;
+
+	for_each_context(priv, ctx) {
+		if (memcmp(&ctx->active, &ctx->staging, sizeof(ctx->staging)))
+			iwl_legacy_commit_rxon(priv, ctx);
+		else
+			IWL_DEBUG_INFO(priv,
+				"Not re-sending same RXON configuration.\n");
+		if (ht_changed[ctx->ctxid])
+			iwl_legacy_update_qos(priv, ctx);
+	}
+
+out:
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+	mutex_unlock(&priv->mutex);
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_mac_config);
+
+void iwl_legacy_mac_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+	/* IBSS can only be the IWL_RXON_CTX_BSS context */
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	if (WARN_ON(!priv->cfg->ops->legacy))
+		return;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	spin_lock_irqsave(&priv->lock, flags);
+	memset(&priv->current_ht_config, 0, sizeof(struct iwl_ht_config));
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* new association get rid of ibss beacon skb */
+	if (priv->beacon_skb)
+		dev_kfree_skb(priv->beacon_skb);
+
+	priv->beacon_skb = NULL;
+
+	priv->timestamp = 0;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl_legacy_scan_cancel_timeout(priv, 100);
+	if (!iwl_legacy_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211(priv, "leave - not ready\n");
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	/* we are restarting association process
+	 * clear RXON_FILTER_ASSOC_MSK bit
+	 */
+	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	iwl_legacy_commit_rxon(priv, ctx);
+
+	iwl_legacy_set_rate(priv);
+
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+}
+EXPORT_SYMBOL(iwl_legacy_mac_reset_tsf);
+
+static void iwl_legacy_ht_conf(struct iwl_priv *priv,
+			struct ieee80211_vif *vif)
+{
+	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
+	struct ieee80211_sta *sta;
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	struct iwl_rxon_context *ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+
+	IWL_DEBUG_ASSOC(priv, "enter:\n");
+
+	if (!ctx->ht.enabled)
+		return;
+
+	ctx->ht.protection =
+		bss_conf->ht_operation_mode & IEEE80211_HT_OP_MODE_PROTECTION;
+	ctx->ht.non_gf_sta_present =
+		!!(bss_conf->ht_operation_mode &
+				IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
+
+	ht_conf->single_chain_sufficient = false;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		rcu_read_lock();
+		sta = ieee80211_find_sta(vif, bss_conf->bssid);
+		if (sta) {
+			struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
+			int maxstreams;
+
+			maxstreams = (ht_cap->mcs.tx_params &
+			      IEEE80211_HT_MCS_TX_MAX_STREAMS_MASK)
+				>> IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT;
+			maxstreams += 1;
+
+			if ((ht_cap->mcs.rx_mask[1] == 0) &&
+			    (ht_cap->mcs.rx_mask[2] == 0))
+				ht_conf->single_chain_sufficient = true;
+			if (maxstreams <= 1)
+				ht_conf->single_chain_sufficient = true;
+		} else {
+			/*
+			 * If at all, this can only happen through a race
+			 * when the AP disconnects us while we're still
+			 * setting up the connection, in that case mac80211
+			 * will soon tell us about that.
+			 */
+			ht_conf->single_chain_sufficient = true;
+		}
+		rcu_read_unlock();
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		ht_conf->single_chain_sufficient = true;
+		break;
+	default:
+		break;
+	}
+
+	IWL_DEBUG_ASSOC(priv, "leave\n");
+}
+
+static inline void iwl_legacy_set_no_assoc(struct iwl_priv *priv,
+				    struct ieee80211_vif *vif)
+{
+	struct iwl_rxon_context *ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+
+	/*
+	 * inform the ucode that there is no longer an
+	 * association and that no more packets should be
+	 * sent
+	 */
+	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	ctx->staging.assoc_id = 0;
+	iwl_legacy_commit_rxon(priv, ctx);
+}
+
+static void iwl_legacy_beacon_update(struct ieee80211_hw *hw,
+				  struct ieee80211_vif *vif)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+	__le64 timestamp;
+	struct sk_buff *skb = ieee80211_beacon_get(hw, vif);
+
+	if (!skb)
+		return;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (!priv->beacon_ctx) {
+		IWL_ERR(priv, "update beacon but no beacon context!\n");
+		dev_kfree_skb(skb);
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (priv->beacon_skb)
+		dev_kfree_skb(priv->beacon_skb);
+
+	priv->beacon_skb = skb;
+
+	timestamp = ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp;
+	priv->timestamp = le64_to_cpu(timestamp);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (!iwl_legacy_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211(priv, "leave - RF not ready\n");
+		return;
+	}
+
+	priv->cfg->ops->legacy->post_associate(priv);
+}
+
+void iwl_legacy_mac_bss_info_changed(struct ieee80211_hw *hw,
+				     struct ieee80211_vif *vif,
+				     struct ieee80211_bss_conf *bss_conf,
+				     u32 changes)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_rxon_context *ctx = iwl_legacy_rxon_ctx_from_vif(vif);
+	int ret;
+
+	if (WARN_ON(!priv->cfg->ops->legacy))
+		return;
+
+	IWL_DEBUG_MAC80211(priv, "changes = 0x%X\n", changes);
+
+	if (!iwl_legacy_is_alive(priv))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	if (changes & BSS_CHANGED_QOS) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&priv->lock, flags);
+		ctx->qos_data.qos_active = bss_conf->qos;
+		iwl_legacy_update_qos(priv, ctx);
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	if (changes & BSS_CHANGED_BEACON_ENABLED) {
+		/*
+		 * the add_interface code must make sure we only ever
+		 * have a single interface that could be beaconing at
+		 * any time.
+		 */
+		if (vif->bss_conf.enable_beacon)
+			priv->beacon_ctx = ctx;
+		else
+			priv->beacon_ctx = NULL;
+	}
+
+	if (changes & BSS_CHANGED_BSSID) {
+		IWL_DEBUG_MAC80211(priv, "BSSID %pM\n", bss_conf->bssid);
+
+		/*
+		 * If there is currently a HW scan going on in the
+		 * background then we need to cancel it else the RXON
+		 * below/in post_associate will fail.
+		 */
+		if (iwl_legacy_scan_cancel_timeout(priv, 100)) {
+			IWL_WARN(priv,
+				"Aborted scan still in progress after 100ms\n");
+			IWL_DEBUG_MAC80211(priv,
+				"leaving - scan abort failed.\n");
+			mutex_unlock(&priv->mutex);
+			return;
+		}
+
+		/* mac80211 only sets assoc when in STATION mode */
+		if (vif->type == NL80211_IFTYPE_ADHOC || bss_conf->assoc) {
+			memcpy(ctx->staging.bssid_addr,
+			       bss_conf->bssid, ETH_ALEN);
+
+			/* currently needed in a few places */
+			memcpy(priv->bssid, bss_conf->bssid, ETH_ALEN);
+		} else {
+			ctx->staging.filter_flags &=
+				~RXON_FILTER_ASSOC_MSK;
+		}
+
+	}
+
+	/*
+	 * This needs to be after setting the BSSID in case
+	 * mac80211 decides to do both changes at once because
+	 * it will invoke post_associate.
+	 */
+	if (vif->type == NL80211_IFTYPE_ADHOC && changes & BSS_CHANGED_BEACON)
+		iwl_legacy_beacon_update(hw, vif);
+
+	if (changes & BSS_CHANGED_ERP_PREAMBLE) {
+		IWL_DEBUG_MAC80211(priv, "ERP_PREAMBLE %d\n",
+				   bss_conf->use_short_preamble);
+		if (bss_conf->use_short_preamble)
+			ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+		else
+			ctx->staging.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+	}
+
+	if (changes & BSS_CHANGED_ERP_CTS_PROT) {
+		IWL_DEBUG_MAC80211(priv,
+			"ERP_CTS %d\n", bss_conf->use_cts_prot);
+		if (bss_conf->use_cts_prot &&
+			(priv->band != IEEE80211_BAND_5GHZ))
+			ctx->staging.flags |= RXON_FLG_TGG_PROTECT_MSK;
+		else
+			ctx->staging.flags &= ~RXON_FLG_TGG_PROTECT_MSK;
+		if (bss_conf->use_cts_prot)
+			ctx->staging.flags |= RXON_FLG_SELF_CTS_EN;
+		else
+			ctx->staging.flags &= ~RXON_FLG_SELF_CTS_EN;
+	}
+
+	if (changes & BSS_CHANGED_BASIC_RATES) {
+		/* XXX use this information
+		 *
+		 * To do that, remove code from iwl_legacy_set_rate() and put something
+		 * like this here:
+		 *
+		if (A-band)
+			ctx->staging.ofdm_basic_rates =
+				bss_conf->basic_rates;
+		else
+			ctx->staging.ofdm_basic_rates =
+				bss_conf->basic_rates >> 4;
+			ctx->staging.cck_basic_rates =
+				bss_conf->basic_rates & 0xF;
+		 */
+	}
+
+	if (changes & BSS_CHANGED_HT) {
+		iwl_legacy_ht_conf(priv, vif);
+
+		if (priv->cfg->ops->hcmd->set_rxon_chain)
+			priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
+	}
+
+	if (changes & BSS_CHANGED_ASSOC) {
+		IWL_DEBUG_MAC80211(priv, "ASSOC %d\n", bss_conf->assoc);
+		if (bss_conf->assoc) {
+			priv->timestamp = bss_conf->timestamp;
+
+			if (!iwl_legacy_is_rfkill(priv))
+				priv->cfg->ops->legacy->post_associate(priv);
+		} else
+			iwl_legacy_set_no_assoc(priv, vif);
+	}
+
+	if (changes && iwl_legacy_is_associated_ctx(ctx) && bss_conf->aid) {
+		IWL_DEBUG_MAC80211(priv, "Changes (%#x) while associated\n",
+				   changes);
+		ret = iwl_legacy_send_rxon_assoc(priv, ctx);
+		if (!ret) {
+			/* Sync active_rxon with latest change. */
+			memcpy((void *)&ctx->active,
+				&ctx->staging,
+				sizeof(struct iwl_legacy_rxon_cmd));
+		}
+	}
+
+	if (changes & BSS_CHANGED_BEACON_ENABLED) {
+		if (vif->bss_conf.enable_beacon) {
+			memcpy(ctx->staging.bssid_addr,
+			       bss_conf->bssid, ETH_ALEN);
+			memcpy(priv->bssid, bss_conf->bssid, ETH_ALEN);
+			priv->cfg->ops->legacy->config_ap(priv);
+		} else
+			iwl_legacy_set_no_assoc(priv, vif);
+	}
+
+	if (changes & BSS_CHANGED_IBSS) {
+		ret = priv->cfg->ops->legacy->manage_ibss_station(priv, vif,
+							bss_conf->ibss_joined);
+		if (ret)
+			IWL_ERR(priv, "failed to %s IBSS station %pM\n",
+				bss_conf->ibss_joined ? "add" : "remove",
+				bss_conf->bssid);
+	}
+
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+}
+EXPORT_SYMBOL(iwl_legacy_mac_bss_info_changed);
+
+irqreturn_t iwl_legacy_isr(int irq, void *data)
+{
+	struct iwl_priv *priv = data;
+	u32 inta, inta_mask;
+	u32 inta_fh;
+	unsigned long flags;
+	if (!priv)
+		return IRQ_NONE;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Disable (but don't clear!) interrupts here to avoid
+	 *    back-to-back ISRs and sporadic interrupts from our NIC.
+	 * If we have something to service, the tasklet will re-enable ints.
+	 * If we *don't* have something, we'll re-enable before leaving here. */
+	inta_mask = iwl_read32(priv, CSR_INT_MASK);  /* just for debug */
+	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
+
+	/* Discover which interrupts are active/pending */
+	inta = iwl_read32(priv, CSR_INT);
+	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+
+	/* Ignore interrupt if there's nothing in NIC to service.
+	 * This may be due to IRQ shared with another device,
+	 * or due to sporadic interrupts thrown from our NIC. */
+	if (!inta && !inta_fh) {
+		IWL_DEBUG_ISR(priv,
+			"Ignore interrupt, inta == 0, inta_fh == 0\n");
+		goto none;
+	}
+
+	if ((inta == 0xFFFFFFFF) || ((inta & 0xFFFFFFF0) == 0xa5a5a5a0)) {
+		/* Hardware disappeared. It might have already raised
+		 * an interrupt */
+		IWL_WARN(priv, "HARDWARE GONE?? INTA == 0x%08x\n", inta);
+		goto unplugged;
+	}
+
+	IWL_DEBUG_ISR(priv, "ISR inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
+		      inta, inta_mask, inta_fh);
+
+	inta &= ~CSR_INT_BIT_SCD;
+
+	/* iwl_irq_tasklet() will service interrupts and re-enable them */
+	if (likely(inta || inta_fh))
+		tasklet_schedule(&priv->irq_tasklet);
+
+unplugged:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return IRQ_HANDLED;
+
+none:
+	/* re-enable interrupts here since we don't have anything to service. */
+	/* only Re-enable if diabled by irq */
+	if (test_bit(STATUS_INT_ENABLED, &priv->status))
+		iwl_legacy_enable_interrupts(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return IRQ_NONE;
+}
+EXPORT_SYMBOL(iwl_legacy_isr);
+
+/*
+ *  iwl_legacy_tx_cmd_protection: Set rts/cts. 3945 and 4965 only share this
+ *  function.
+ */
+void iwl_legacy_tx_cmd_protection(struct iwl_priv *priv,
+			       struct ieee80211_tx_info *info,
+			       __le16 fc, __le32 *tx_flags)
+{
+	if (info->control.rates[0].flags & IEEE80211_TX_RC_USE_RTS_CTS) {
+		*tx_flags |= TX_CMD_FLG_RTS_MSK;
+		*tx_flags &= ~TX_CMD_FLG_CTS_MSK;
+		*tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
+
+		if (!ieee80211_is_mgmt(fc))
+			return;
+
+		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
+		case cpu_to_le16(IEEE80211_STYPE_AUTH):
+		case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
+		case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
+		case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
+			*tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+			*tx_flags |= TX_CMD_FLG_CTS_MSK;
+			break;
+		}
+	} else if (info->control.rates[0].flags &
+		   IEEE80211_TX_RC_USE_CTS_PROTECT) {
+		*tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+		*tx_flags |= TX_CMD_FLG_CTS_MSK;
+		*tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_tx_cmd_protection);
diff --git a/drivers/net/wireless/iwlegacy/iwl-core.h b/drivers/net/wireless/iwlegacy/iwl-core.h
new file mode 100644
index 000000000000..f03b463e4378
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-core.h
@@ -0,0 +1,646 @@
+/******************************************************************************
+ *
+ * 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 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_core_h__
+#define __iwl_legacy_core_h__
+
+/************************
+ * forward declarations *
+ ************************/
+struct iwl_host_cmd;
+struct iwl_cmd;
+
+
+#define IWLWIFI_VERSION "in-tree:"
+#define DRV_COPYRIGHT	"Copyright(c) 2003-2011 Intel Corporation"
+#define DRV_AUTHOR     "<ilw@linux.intel.com>"
+
+#define IWL_PCI_DEVICE(dev, subdev, cfg) \
+	.vendor = PCI_VENDOR_ID_INTEL,  .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = (subdev), \
+	.driver_data = (kernel_ulong_t)&(cfg)
+
+#define TIME_UNIT		1024
+
+#define IWL_SKU_G       0x1
+#define IWL_SKU_A       0x2
+#define IWL_SKU_N       0x8
+
+#define IWL_CMD(x) case x: return #x
+
+struct iwl_hcmd_ops {
+	int (*rxon_assoc)(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
+	int (*commit_rxon)(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
+	void (*set_rxon_chain)(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx);
+};
+
+struct iwl_hcmd_utils_ops {
+	u16 (*get_hcmd_size)(u8 cmd_id, u16 len);
+	u16 (*build_addsta_hcmd)(const struct iwl_legacy_addsta_cmd *cmd,
+								u8 *data);
+	int (*request_scan)(struct iwl_priv *priv, struct ieee80211_vif *vif);
+	void (*post_scan)(struct iwl_priv *priv);
+};
+
+struct iwl_apm_ops {
+	int (*init)(struct iwl_priv *priv);
+	void (*config)(struct iwl_priv *priv);
+};
+
+struct iwl_debugfs_ops {
+	ssize_t (*rx_stats_read)(struct file *file, char __user *user_buf,
+				 size_t count, loff_t *ppos);
+	ssize_t (*tx_stats_read)(struct file *file, char __user *user_buf,
+				 size_t count, loff_t *ppos);
+	ssize_t (*general_stats_read)(struct file *file, char __user *user_buf,
+				      size_t count, loff_t *ppos);
+};
+
+struct iwl_temp_ops {
+	void (*temperature)(struct iwl_priv *priv);
+};
+
+struct iwl_lib_ops {
+	/* set hw dependent parameters */
+	int (*set_hw_params)(struct iwl_priv *priv);
+	/* Handling TX */
+	void (*txq_update_byte_cnt_tbl)(struct iwl_priv *priv,
+					struct iwl_tx_queue *txq,
+					u16 byte_cnt);
+	int (*txq_attach_buf_to_tfd)(struct iwl_priv *priv,
+				     struct iwl_tx_queue *txq,
+				     dma_addr_t addr,
+				     u16 len, u8 reset, u8 pad);
+	void (*txq_free_tfd)(struct iwl_priv *priv,
+			     struct iwl_tx_queue *txq);
+	int (*txq_init)(struct iwl_priv *priv,
+			struct iwl_tx_queue *txq);
+	/* setup Rx handler */
+	void (*rx_handler_setup)(struct iwl_priv *priv);
+	/* alive notification after init uCode load */
+	void (*init_alive_start)(struct iwl_priv *priv);
+	/* check validity of rtc data address */
+	int (*is_valid_rtc_data_addr)(u32 addr);
+	/* 1st ucode load */
+	int (*load_ucode)(struct iwl_priv *priv);
+	int (*dump_nic_event_log)(struct iwl_priv *priv,
+				  bool full_log, char **buf, bool display);
+	void (*dump_nic_error_log)(struct iwl_priv *priv);
+	int (*dump_fh)(struct iwl_priv *priv, char **buf, bool display);
+	int (*set_channel_switch)(struct iwl_priv *priv,
+				  struct ieee80211_channel_switch *ch_switch);
+	/* power management */
+	struct iwl_apm_ops apm_ops;
+
+	/* power */
+	int (*send_tx_power) (struct iwl_priv *priv);
+	void (*update_chain_flags)(struct iwl_priv *priv);
+
+	/* eeprom operations (as defined in iwl-eeprom.h) */
+	struct iwl_eeprom_ops eeprom_ops;
+
+	/* temperature */
+	struct iwl_temp_ops temp_ops;
+	/* check for plcp health */
+	bool (*check_plcp_health)(struct iwl_priv *priv,
+					struct iwl_rx_packet *pkt);
+
+	struct iwl_debugfs_ops debugfs_ops;
+
+};
+
+struct iwl_led_ops {
+	int (*cmd)(struct iwl_priv *priv, struct iwl_led_cmd *led_cmd);
+};
+
+struct iwl_legacy_ops {
+	void (*post_associate)(struct iwl_priv *priv);
+	void (*config_ap)(struct iwl_priv *priv);
+	/* station management */
+	int (*update_bcast_stations)(struct iwl_priv *priv);
+	int (*manage_ibss_station)(struct iwl_priv *priv,
+				   struct ieee80211_vif *vif, bool add);
+};
+
+struct iwl_ops {
+	const struct iwl_lib_ops *lib;
+	const struct iwl_hcmd_ops *hcmd;
+	const struct iwl_hcmd_utils_ops *utils;
+	const struct iwl_led_ops *led;
+	const struct iwl_nic_ops *nic;
+	const struct iwl_legacy_ops *legacy;
+	const struct ieee80211_ops *ieee80211_ops;
+};
+
+struct iwl_mod_params {
+	int sw_crypto;		/* def: 0 = using hardware encryption */
+	int disable_hw_scan;	/* def: 0 = use h/w scan */
+	int num_of_queues;	/* def: HW dependent */
+	int disable_11n;	/* def: 0 = 11n capabilities enabled */
+	int amsdu_size_8K;	/* def: 1 = enable 8K amsdu size */
+	int antenna;  		/* def: 0 = both antennas (use diversity) */
+	int restart_fw;		/* def: 1 = restart firmware */
+};
+
+/*
+ * @led_compensation: compensate on the led on/off time per HW according
+ *	to the deviation to achieve the desired led frequency.
+ *	The detail algorithm is described in iwl-led.c
+ * @chain_noise_num_beacons: number of beacons used to compute chain noise
+ * @plcp_delta_threshold: plcp error rate threshold used to trigger
+ *	radio tuning when there is a high receiving plcp error rate
+ * @wd_timeout: TX queues watchdog timeout
+ * @temperature_kelvin: temperature report by uCode in kelvin
+ * @max_event_log_size: size of event log buffer size for ucode event logging
+ * @ucode_tracing: support ucode continuous tracing
+ * @sensitivity_calib_by_driver: driver has the capability to perform
+ *	sensitivity calibration operation
+ * @chain_noise_calib_by_driver: driver has the capability to perform
+ *	chain noise calibration operation
+ */
+struct iwl_base_params {
+	int eeprom_size;
+	int num_of_queues;	/* def: HW dependent */
+	int num_of_ampdu_queues;/* def: HW dependent */
+	/* for iwl_legacy_apm_init() */
+	u32 pll_cfg_val;
+	bool set_l0s;
+	bool use_bsm;
+
+	u16 led_compensation;
+	int chain_noise_num_beacons;
+	u8 plcp_delta_threshold;
+	unsigned int wd_timeout;
+	bool temperature_kelvin;
+	u32 max_event_log_size;
+	const bool ucode_tracing;
+	const bool sensitivity_calib_by_driver;
+	const bool chain_noise_calib_by_driver;
+};
+
+/**
+ * struct iwl_cfg
+ * @fw_name_pre: Firmware filename prefix. The api version and extension
+ *	(.ucode) will be added to filename before loading from disk. The
+ *	filename is constructed as fw_name_pre<api>.ucode.
+ * @ucode_api_max: Highest version of uCode API supported by driver.
+ * @ucode_api_min: Lowest version of uCode API supported by driver.
+ * @scan_antennas: available antenna for scan operation
+ * @led_mode: 0=blinking, 1=On(RF On)/Off(RF Off)
+ *
+ * We enable the driver to be backward compatible wrt API version. The
+ * driver specifies which APIs it supports (with @ucode_api_max being the
+ * highest and @ucode_api_min the lowest). Firmware will only be loaded if
+ * it has a supported API version. The firmware's API version will be
+ * stored in @iwl_priv, enabling the driver to make runtime changes based
+ * on firmware version used.
+ *
+ * For example,
+ * if (IWL_UCODE_API(priv->ucode_ver) >= 2) {
+ *	Driver interacts with Firmware API version >= 2.
+ * } else {
+ *	Driver interacts with Firmware API version 1.
+ * }
+ *
+ * The ideal usage of this infrastructure is to treat a new ucode API
+ * release as a new hardware revision. That is, through utilizing the
+ * iwl_hcmd_utils_ops etc. we accommodate different command structures
+ * and flows between hardware versions as well as their API
+ * versions.
+ *
+ */
+struct iwl_cfg {
+	/* params specific to an individual device within a device family */
+	const char *name;
+	const char *fw_name_pre;
+	const unsigned int ucode_api_max;
+	const unsigned int ucode_api_min;
+	u8   valid_tx_ant;
+	u8   valid_rx_ant;
+	unsigned int sku;
+	u16  eeprom_ver;
+	u16  eeprom_calib_ver;
+	const struct iwl_ops *ops;
+	/* module based parameters which can be set from modprobe cmd */
+	const struct iwl_mod_params *mod_params;
+	/* params not likely to change within a device family */
+	struct iwl_base_params *base_params;
+	/* params likely to change within a device family */
+	u8 scan_rx_antennas[IEEE80211_NUM_BANDS];
+	u8 scan_tx_antennas[IEEE80211_NUM_BANDS];
+	enum iwl_led_mode led_mode;
+};
+
+/***************************
+ *   L i b                 *
+ ***************************/
+
+struct ieee80211_hw *iwl_legacy_alloc_all(struct iwl_cfg *cfg);
+int iwl_legacy_mac_conf_tx(struct ieee80211_hw *hw, u16 queue,
+		    const struct ieee80211_tx_queue_params *params);
+int iwl_legacy_mac_tx_last_beacon(struct ieee80211_hw *hw);
+void iwl_legacy_set_rxon_hwcrypto(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			int hw_decrypt);
+int iwl_legacy_check_rxon_cmd(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx);
+int iwl_legacy_full_rxon_required(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx);
+int iwl_legacy_set_rxon_channel(struct iwl_priv *priv,
+			struct ieee80211_channel *ch,
+			struct iwl_rxon_context *ctx);
+void iwl_legacy_set_flags_for_band(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    enum ieee80211_band band,
+			    struct ieee80211_vif *vif);
+u8 iwl_legacy_get_single_channel_number(struct iwl_priv *priv,
+				  enum ieee80211_band band);
+void iwl_legacy_set_rxon_ht(struct iwl_priv *priv,
+			struct iwl_ht_config *ht_conf);
+bool iwl_legacy_is_ht40_tx_allowed(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx,
+			    struct ieee80211_sta_ht_cap *ht_cap);
+void iwl_legacy_connection_init_rx_config(struct iwl_priv *priv,
+				   struct iwl_rxon_context *ctx);
+void iwl_legacy_set_rate(struct iwl_priv *priv);
+int iwl_legacy_set_decrypted_flag(struct iwl_priv *priv,
+			   struct ieee80211_hdr *hdr,
+			   u32 decrypt_res,
+			   struct ieee80211_rx_status *stats);
+void iwl_legacy_irq_handle_error(struct iwl_priv *priv);
+int iwl_legacy_mac_add_interface(struct ieee80211_hw *hw,
+			  struct ieee80211_vif *vif);
+void iwl_legacy_mac_remove_interface(struct ieee80211_hw *hw,
+			      struct ieee80211_vif *vif);
+int iwl_legacy_mac_change_interface(struct ieee80211_hw *hw,
+			     struct ieee80211_vif *vif,
+			     enum nl80211_iftype newtype, bool newp2p);
+int iwl_legacy_alloc_txq_mem(struct iwl_priv *priv);
+void iwl_legacy_txq_mem(struct iwl_priv *priv);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+int iwl_legacy_alloc_traffic_mem(struct iwl_priv *priv);
+void iwl_legacy_free_traffic_mem(struct iwl_priv *priv);
+void iwl_legacy_reset_traffic_log(struct iwl_priv *priv);
+void iwl_legacy_dbg_log_tx_data_frame(struct iwl_priv *priv,
+				u16 length, struct ieee80211_hdr *header);
+void iwl_legacy_dbg_log_rx_data_frame(struct iwl_priv *priv,
+				u16 length, struct ieee80211_hdr *header);
+const char *iwl_legacy_get_mgmt_string(int cmd);
+const char *iwl_legacy_get_ctrl_string(int cmd);
+void iwl_legacy_clear_traffic_stats(struct iwl_priv *priv);
+void iwl_legacy_update_stats(struct iwl_priv *priv, bool is_tx, __le16 fc,
+		      u16 len);
+#else
+static inline int iwl_legacy_alloc_traffic_mem(struct iwl_priv *priv)
+{
+	return 0;
+}
+static inline void iwl_legacy_free_traffic_mem(struct iwl_priv *priv)
+{
+}
+static inline void iwl_legacy_reset_traffic_log(struct iwl_priv *priv)
+{
+}
+static inline void iwl_legacy_dbg_log_tx_data_frame(struct iwl_priv *priv,
+		      u16 length, struct ieee80211_hdr *header)
+{
+}
+static inline void iwl_legacy_dbg_log_rx_data_frame(struct iwl_priv *priv,
+		      u16 length, struct ieee80211_hdr *header)
+{
+}
+static inline void iwl_legacy_update_stats(struct iwl_priv *priv, bool is_tx,
+				    __le16 fc, u16 len)
+{
+}
+#endif
+/*****************************************************
+ * RX handlers.
+ * **************************************************/
+void iwl_legacy_rx_pm_sleep_notif(struct iwl_priv *priv,
+			   struct iwl_rx_mem_buffer *rxb);
+void iwl_legacy_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
+				      struct iwl_rx_mem_buffer *rxb);
+void iwl_legacy_rx_reply_error(struct iwl_priv *priv,
+			struct iwl_rx_mem_buffer *rxb);
+
+/*****************************************************
+* RX
+******************************************************/
+void iwl_legacy_cmd_queue_unmap(struct iwl_priv *priv);
+void iwl_legacy_cmd_queue_free(struct iwl_priv *priv);
+int iwl_legacy_rx_queue_alloc(struct iwl_priv *priv);
+void iwl_legacy_rx_queue_update_write_ptr(struct iwl_priv *priv,
+				  struct iwl_rx_queue *q);
+int iwl_legacy_rx_queue_space(const struct iwl_rx_queue *q);
+void iwl_legacy_tx_cmd_complete(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb);
+/* Handlers */
+void iwl_legacy_rx_spectrum_measure_notif(struct iwl_priv *priv,
+					  struct iwl_rx_mem_buffer *rxb);
+void iwl_legacy_recover_from_statistics(struct iwl_priv *priv,
+				struct iwl_rx_packet *pkt);
+void iwl_legacy_chswitch_done(struct iwl_priv *priv, bool is_success);
+void iwl_legacy_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb);
+
+/* TX helpers */
+
+/*****************************************************
+* TX
+******************************************************/
+void iwl_legacy_txq_update_write_ptr(struct iwl_priv *priv,
+					struct iwl_tx_queue *txq);
+int iwl_legacy_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq,
+		      int slots_num, u32 txq_id);
+void iwl_legacy_tx_queue_reset(struct iwl_priv *priv,
+			struct iwl_tx_queue *txq,
+			int slots_num, u32 txq_id);
+void iwl_legacy_tx_queue_unmap(struct iwl_priv *priv, int txq_id);
+void iwl_legacy_tx_queue_free(struct iwl_priv *priv, int txq_id);
+void iwl_legacy_setup_watchdog(struct iwl_priv *priv);
+/*****************************************************
+ * TX power
+ ****************************************************/
+int iwl_legacy_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force);
+
+/*******************************************************************************
+ * Rate
+ ******************************************************************************/
+
+u8 iwl_legacy_get_lowest_plcp(struct iwl_priv *priv,
+			    struct iwl_rxon_context *ctx);
+
+/*******************************************************************************
+ * Scanning
+ ******************************************************************************/
+void iwl_legacy_init_scan_params(struct iwl_priv *priv);
+int iwl_legacy_scan_cancel(struct iwl_priv *priv);
+int iwl_legacy_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms);
+void iwl_legacy_force_scan_end(struct iwl_priv *priv);
+int iwl_legacy_mac_hw_scan(struct ieee80211_hw *hw,
+		    struct ieee80211_vif *vif,
+		    struct cfg80211_scan_request *req);
+void iwl_legacy_internal_short_hw_scan(struct iwl_priv *priv);
+int iwl_legacy_force_reset(struct iwl_priv *priv, int mode, bool external);
+u16 iwl_legacy_fill_probe_req(struct iwl_priv *priv,
+			struct ieee80211_mgmt *frame,
+		       const u8 *ta, const u8 *ie, int ie_len, int left);
+void iwl_legacy_setup_rx_scan_handlers(struct iwl_priv *priv);
+u16 iwl_legacy_get_active_dwell_time(struct iwl_priv *priv,
+			      enum ieee80211_band band,
+			      u8 n_probes);
+u16 iwl_legacy_get_passive_dwell_time(struct iwl_priv *priv,
+			       enum ieee80211_band band,
+			       struct ieee80211_vif *vif);
+void iwl_legacy_setup_scan_deferred_work(struct iwl_priv *priv);
+void iwl_legacy_cancel_scan_deferred_work(struct iwl_priv *priv);
+
+/* For faster active scanning, scan will move to the next channel if fewer than
+ * PLCP_QUIET_THRESH packets are heard on this channel within
+ * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
+ * time if it's a quiet channel (nothing responded to our probe, and there's
+ * no other traffic).
+ * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
+#define IWL_ACTIVE_QUIET_TIME       cpu_to_le16(10)  /* msec */
+#define IWL_PLCP_QUIET_THRESH       cpu_to_le16(1)  /* packets */
+
+#define IWL_SCAN_CHECK_WATCHDOG		(HZ * 7)
+
+/*****************************************************
+ *   S e n d i n g     H o s t     C o m m a n d s   *
+ *****************************************************/
+
+const char *iwl_legacy_get_cmd_string(u8 cmd);
+int __must_check iwl_legacy_send_cmd_sync(struct iwl_priv *priv,
+				   struct iwl_host_cmd *cmd);
+int iwl_legacy_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+int __must_check iwl_legacy_send_cmd_pdu(struct iwl_priv *priv, u8 id,
+				  u16 len, const void *data);
+int iwl_legacy_send_cmd_pdu_async(struct iwl_priv *priv, u8 id, u16 len,
+			   const void *data,
+			   void (*callback)(struct iwl_priv *priv,
+					    struct iwl_device_cmd *cmd,
+					    struct iwl_rx_packet *pkt));
+
+int iwl_legacy_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+
+
+/*****************************************************
+ * PCI						     *
+ *****************************************************/
+
+static inline u16 iwl_legacy_pcie_link_ctl(struct iwl_priv *priv)
+{
+	int pos;
+	u16 pci_lnk_ctl;
+	pos = pci_find_capability(priv->pci_dev, PCI_CAP_ID_EXP);
+	pci_read_config_word(priv->pci_dev, pos + PCI_EXP_LNKCTL, &pci_lnk_ctl);
+	return pci_lnk_ctl;
+}
+
+void iwl_legacy_bg_watchdog(unsigned long data);
+u32 iwl_legacy_usecs_to_beacons(struct iwl_priv *priv,
+					u32 usec, u32 beacon_interval);
+__le32 iwl_legacy_add_beacon_time(struct iwl_priv *priv, u32 base,
+			   u32 addon, u32 beacon_interval);
+
+#ifdef CONFIG_PM
+int iwl_legacy_pci_suspend(struct device *device);
+int iwl_legacy_pci_resume(struct device *device);
+extern const struct dev_pm_ops iwl_legacy_pm_ops;
+
+#define IWL_LEGACY_PM_OPS	(&iwl_legacy_pm_ops)
+
+#else /* !CONFIG_PM */
+
+#define IWL_LEGACY_PM_OPS	NULL
+
+#endif /* !CONFIG_PM */
+
+/*****************************************************
+*  Error Handling Debugging
+******************************************************/
+void iwl4965_dump_nic_error_log(struct iwl_priv *priv);
+int iwl4965_dump_nic_event_log(struct iwl_priv *priv,
+			   bool full_log, char **buf, bool display);
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+void iwl_legacy_print_rx_config_cmd(struct iwl_priv *priv,
+			     struct iwl_rxon_context *ctx);
+#else
+static inline void iwl_legacy_print_rx_config_cmd(struct iwl_priv *priv,
+					   struct iwl_rxon_context *ctx)
+{
+}
+#endif
+
+void iwl_legacy_clear_isr_stats(struct iwl_priv *priv);
+
+/*****************************************************
+*  GEOS
+******************************************************/
+int iwl_legacy_init_geos(struct iwl_priv *priv);
+void iwl_legacy_free_geos(struct iwl_priv *priv);
+
+/*************** DRIVER STATUS FUNCTIONS   *****/
+
+#define STATUS_HCMD_ACTIVE	0	/* host command in progress */
+/* 1 is unused (used to be STATUS_HCMD_SYNC_ACTIVE) */
+#define STATUS_INT_ENABLED	2
+#define STATUS_RF_KILL_HW	3
+#define STATUS_CT_KILL		4
+#define STATUS_INIT		5
+#define STATUS_ALIVE		6
+#define STATUS_READY		7
+#define STATUS_TEMPERATURE	8
+#define STATUS_GEO_CONFIGURED	9
+#define STATUS_EXIT_PENDING	10
+#define STATUS_STATISTICS	12
+#define STATUS_SCANNING		13
+#define STATUS_SCAN_ABORTING	14
+#define STATUS_SCAN_HW		15
+#define STATUS_POWER_PMI	16
+#define STATUS_FW_ERROR		17
+
+
+static inline int iwl_legacy_is_ready(struct iwl_priv *priv)
+{
+	/* The adapter is 'ready' if READY and GEO_CONFIGURED bits are
+	 * set but EXIT_PENDING is not */
+	return test_bit(STATUS_READY, &priv->status) &&
+	       test_bit(STATUS_GEO_CONFIGURED, &priv->status) &&
+	       !test_bit(STATUS_EXIT_PENDING, &priv->status);
+}
+
+static inline int iwl_legacy_is_alive(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_ALIVE, &priv->status);
+}
+
+static inline int iwl_legacy_is_init(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_INIT, &priv->status);
+}
+
+static inline int iwl_legacy_is_rfkill_hw(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_HW, &priv->status);
+}
+
+static inline int iwl_legacy_is_rfkill(struct iwl_priv *priv)
+{
+	return iwl_legacy_is_rfkill_hw(priv);
+}
+
+static inline int iwl_legacy_is_ctkill(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_CT_KILL, &priv->status);
+}
+
+static inline int iwl_legacy_is_ready_rf(struct iwl_priv *priv)
+{
+
+	if (iwl_legacy_is_rfkill(priv))
+		return 0;
+
+	return iwl_legacy_is_ready(priv);
+}
+
+extern void iwl_legacy_send_bt_config(struct iwl_priv *priv);
+extern int iwl_legacy_send_statistics_request(struct iwl_priv *priv,
+				       u8 flags, bool clear);
+void iwl_legacy_apm_stop(struct iwl_priv *priv);
+int iwl_legacy_apm_init(struct iwl_priv *priv);
+
+int iwl_legacy_send_rxon_timing(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx);
+static inline int iwl_legacy_send_rxon_assoc(struct iwl_priv *priv,
+				      struct iwl_rxon_context *ctx)
+{
+	return priv->cfg->ops->hcmd->rxon_assoc(priv, ctx);
+}
+static inline int iwl_legacy_commit_rxon(struct iwl_priv *priv,
+				      struct iwl_rxon_context *ctx)
+{
+	return priv->cfg->ops->hcmd->commit_rxon(priv, ctx);
+}
+static inline const struct ieee80211_supported_band *iwl_get_hw_mode(
+			struct iwl_priv *priv, enum ieee80211_band band)
+{
+	return priv->hw->wiphy->bands[band];
+}
+
+/* mac80211 handlers */
+int iwl_legacy_mac_config(struct ieee80211_hw *hw, u32 changed);
+void iwl_legacy_mac_reset_tsf(struct ieee80211_hw *hw);
+void iwl_legacy_mac_bss_info_changed(struct ieee80211_hw *hw,
+				     struct ieee80211_vif *vif,
+				     struct ieee80211_bss_conf *bss_conf,
+				     u32 changes);
+void iwl_legacy_tx_cmd_protection(struct iwl_priv *priv,
+				struct ieee80211_tx_info *info,
+				__le16 fc, __le32 *tx_flags);
+
+irqreturn_t iwl_legacy_isr(int irq, void *data);
+
+#endif /* __iwl_legacy_core_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-csr.h b/drivers/net/wireless/iwlegacy/iwl-csr.h
new file mode 100644
index 000000000000..668a9616c269
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-csr.h
@@ -0,0 +1,422 @@
+/******************************************************************************
+ *
+ * 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) 2005 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef __iwl_legacy_csr_h__
+#define __iwl_legacy_csr_h__
+/*
+ * CSR (control and status registers)
+ *
+ * CSR registers are mapped directly into PCI bus space, and are accessible
+ * whenever platform supplies power to device, even when device is in
+ * low power states due to driver-invoked device resets
+ * (e.g. CSR_RESET_REG_FLAG_SW_RESET) or uCode-driven power-saving modes.
+ *
+ * Use iwl_write32() and iwl_read32() family to access these registers;
+ * these provide simple PCI bus access, without waking up the MAC.
+ * Do not use iwl_legacy_write_direct32() family for these registers;
+ * no need to "grab nic access" via CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ.
+ * The MAC (uCode processor, etc.) does not need to be powered up for accessing
+ * the CSR registers.
+ *
+ * NOTE:  Device does need to be awake in order to read this memory
+ *        via CSR_EEPROM register
+ */
+#define CSR_BASE    (0x000)
+
+#define CSR_HW_IF_CONFIG_REG    (CSR_BASE+0x000) /* hardware interface config */
+#define CSR_INT_COALESCING      (CSR_BASE+0x004) /* accum ints, 32-usec units */
+#define CSR_INT                 (CSR_BASE+0x008) /* host interrupt status/ack */
+#define CSR_INT_MASK            (CSR_BASE+0x00c) /* host interrupt enable */
+#define CSR_FH_INT_STATUS       (CSR_BASE+0x010) /* busmaster int status/ack*/
+#define CSR_GPIO_IN             (CSR_BASE+0x018) /* read external chip pins */
+#define CSR_RESET               (CSR_BASE+0x020) /* busmaster enable, NMI, etc*/
+#define CSR_GP_CNTRL            (CSR_BASE+0x024)
+
+/* 2nd byte of CSR_INT_COALESCING, not accessible via iwl_write32()! */
+#define CSR_INT_PERIODIC_REG	(CSR_BASE+0x005)
+
+/*
+ * Hardware revision info
+ * Bit fields:
+ * 31-8:  Reserved
+ *  7-4:  Type of device:  see CSR_HW_REV_TYPE_xxx definitions
+ *  3-2:  Revision step:  0 = A, 1 = B, 2 = C, 3 = D
+ *  1-0:  "Dash" (-) value, as in A-1, etc.
+ *
+ * NOTE:  Revision step affects calculation of CCK txpower for 4965.
+ * NOTE:  See also CSR_HW_REV_WA_REG (work-around for bug in 4965).
+ */
+#define CSR_HW_REV              (CSR_BASE+0x028)
+
+/*
+ * EEPROM memory reads
+ *
+ * NOTE:  Device must be awake, initialized via apm_ops.init(),
+ *        in order to read.
+ */
+#define CSR_EEPROM_REG          (CSR_BASE+0x02c)
+#define CSR_EEPROM_GP           (CSR_BASE+0x030)
+
+#define CSR_GIO_REG		(CSR_BASE+0x03C)
+#define CSR_GP_UCODE_REG	(CSR_BASE+0x048)
+#define CSR_GP_DRIVER_REG	(CSR_BASE+0x050)
+
+/*
+ * UCODE-DRIVER GP (general purpose) mailbox registers.
+ * SET/CLR registers set/clear bit(s) if "1" is written.
+ */
+#define CSR_UCODE_DRV_GP1       (CSR_BASE+0x054)
+#define CSR_UCODE_DRV_GP1_SET   (CSR_BASE+0x058)
+#define CSR_UCODE_DRV_GP1_CLR   (CSR_BASE+0x05c)
+#define CSR_UCODE_DRV_GP2       (CSR_BASE+0x060)
+
+#define CSR_LED_REG             (CSR_BASE+0x094)
+#define CSR_DRAM_INT_TBL_REG	(CSR_BASE+0x0A0)
+
+/* GIO Chicken Bits (PCI Express bus link power management) */
+#define CSR_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
+
+/* Analog phase-lock-loop configuration  */
+#define CSR_ANA_PLL_CFG         (CSR_BASE+0x20c)
+
+/*
+ * CSR Hardware Revision Workaround Register.  Indicates hardware rev;
+ * "step" determines CCK backoff for txpower calculation.  Used for 4965 only.
+ * See also CSR_HW_REV register.
+ * Bit fields:
+ *  3-2:  0 = A, 1 = B, 2 = C, 3 = D step
+ *  1-0:  "Dash" (-) value, as in C-1, etc.
+ */
+#define CSR_HW_REV_WA_REG		(CSR_BASE+0x22C)
+
+#define CSR_DBG_HPET_MEM_REG		(CSR_BASE+0x240)
+#define CSR_DBG_LINK_PWR_MGMT_REG	(CSR_BASE+0x250)
+
+/* Bits for CSR_HW_IF_CONFIG_REG */
+#define CSR49_HW_IF_CONFIG_REG_BIT_4965_R	(0x00000010)
+#define CSR_HW_IF_CONFIG_REG_MSK_BOARD_VER	(0x00000C00)
+#define CSR_HW_IF_CONFIG_REG_BIT_MAC_SI 	(0x00000100)
+#define CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI	(0x00000200)
+
+#define CSR39_HW_IF_CONFIG_REG_BIT_3945_MB         (0x00000100)
+#define CSR39_HW_IF_CONFIG_REG_BIT_3945_MM         (0x00000200)
+#define CSR39_HW_IF_CONFIG_REG_BIT_SKU_MRC            (0x00000400)
+#define CSR39_HW_IF_CONFIG_REG_BIT_BOARD_TYPE         (0x00000800)
+#define CSR39_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_A    (0x00000000)
+#define CSR39_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_B    (0x00001000)
+
+#define CSR_HW_IF_CONFIG_REG_BIT_HAP_WAKE_L1A	(0x00080000)
+#define CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM	(0x00200000)
+#define CSR_HW_IF_CONFIG_REG_BIT_NIC_READY	(0x00400000) /* PCI_OWN_SEM */
+#define CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE (0x02000000) /* ME_OWN */
+#define CSR_HW_IF_CONFIG_REG_PREPARE		  (0x08000000) /* WAKE_ME */
+
+#define CSR_INT_PERIODIC_DIS			(0x00) /* disable periodic int*/
+#define CSR_INT_PERIODIC_ENA			(0xFF) /* 255*32 usec ~ 8 msec*/
+
+/* interrupt flags in INTA, set by uCode or hardware (e.g. dma),
+ * acknowledged (reset) by host writing "1" to flagged bits. */
+#define CSR_INT_BIT_FH_RX        (1 << 31) /* Rx DMA, cmd responses, FH_INT[17:16] */
+#define CSR_INT_BIT_HW_ERR       (1 << 29) /* DMA hardware error FH_INT[31] */
+#define CSR_INT_BIT_RX_PERIODIC	 (1 << 28) /* Rx periodic */
+#define CSR_INT_BIT_FH_TX        (1 << 27) /* Tx DMA FH_INT[1:0] */
+#define CSR_INT_BIT_SCD          (1 << 26) /* TXQ pointer advanced */
+#define CSR_INT_BIT_SW_ERR       (1 << 25) /* uCode error */
+#define CSR_INT_BIT_RF_KILL      (1 << 7)  /* HW RFKILL switch GP_CNTRL[27] toggled */
+#define CSR_INT_BIT_CT_KILL      (1 << 6)  /* Critical temp (chip too hot) rfkill */
+#define CSR_INT_BIT_SW_RX        (1 << 3)  /* Rx, command responses, 3945 */
+#define CSR_INT_BIT_WAKEUP       (1 << 1)  /* NIC controller waking up (pwr mgmt) */
+#define CSR_INT_BIT_ALIVE        (1 << 0)  /* uCode interrupts once it initializes */
+
+#define CSR_INI_SET_MASK	(CSR_INT_BIT_FH_RX   | \
+				 CSR_INT_BIT_HW_ERR  | \
+				 CSR_INT_BIT_FH_TX   | \
+				 CSR_INT_BIT_SW_ERR  | \
+				 CSR_INT_BIT_RF_KILL | \
+				 CSR_INT_BIT_SW_RX   | \
+				 CSR_INT_BIT_WAKEUP  | \
+				 CSR_INT_BIT_ALIVE)
+
+/* interrupt flags in FH (flow handler) (PCI busmaster DMA) */
+#define CSR_FH_INT_BIT_ERR       (1 << 31) /* Error */
+#define CSR_FH_INT_BIT_HI_PRIOR  (1 << 30) /* High priority Rx, bypass coalescing */
+#define CSR39_FH_INT_BIT_RX_CHNL2  (1 << 18) /* Rx channel 2 (3945 only) */
+#define CSR_FH_INT_BIT_RX_CHNL1  (1 << 17) /* Rx channel 1 */
+#define CSR_FH_INT_BIT_RX_CHNL0  (1 << 16) /* Rx channel 0 */
+#define CSR39_FH_INT_BIT_TX_CHNL6  (1 << 6)  /* Tx channel 6 (3945 only) */
+#define CSR_FH_INT_BIT_TX_CHNL1  (1 << 1)  /* Tx channel 1 */
+#define CSR_FH_INT_BIT_TX_CHNL0  (1 << 0)  /* Tx channel 0 */
+
+#define CSR39_FH_INT_RX_MASK	(CSR_FH_INT_BIT_HI_PRIOR | \
+				 CSR39_FH_INT_BIT_RX_CHNL2 | \
+				 CSR_FH_INT_BIT_RX_CHNL1 | \
+				 CSR_FH_INT_BIT_RX_CHNL0)
+
+
+#define CSR39_FH_INT_TX_MASK	(CSR39_FH_INT_BIT_TX_CHNL6 | \
+				 CSR_FH_INT_BIT_TX_CHNL1 | \
+				 CSR_FH_INT_BIT_TX_CHNL0)
+
+#define CSR49_FH_INT_RX_MASK	(CSR_FH_INT_BIT_HI_PRIOR | \
+				 CSR_FH_INT_BIT_RX_CHNL1 | \
+				 CSR_FH_INT_BIT_RX_CHNL0)
+
+#define CSR49_FH_INT_TX_MASK	(CSR_FH_INT_BIT_TX_CHNL1 | \
+				 CSR_FH_INT_BIT_TX_CHNL0)
+
+/* GPIO */
+#define CSR_GPIO_IN_BIT_AUX_POWER                   (0x00000200)
+#define CSR_GPIO_IN_VAL_VAUX_PWR_SRC                (0x00000000)
+#define CSR_GPIO_IN_VAL_VMAIN_PWR_SRC               (0x00000200)
+
+/* RESET */
+#define CSR_RESET_REG_FLAG_NEVO_RESET                (0x00000001)
+#define CSR_RESET_REG_FLAG_FORCE_NMI                 (0x00000002)
+#define CSR_RESET_REG_FLAG_SW_RESET                  (0x00000080)
+#define CSR_RESET_REG_FLAG_MASTER_DISABLED           (0x00000100)
+#define CSR_RESET_REG_FLAG_STOP_MASTER               (0x00000200)
+#define CSR_RESET_LINK_PWR_MGMT_DISABLED             (0x80000000)
+
+/*
+ * GP (general purpose) CONTROL REGISTER
+ * Bit fields:
+ *    27:  HW_RF_KILL_SW
+ *         Indicates state of (platform's) hardware RF-Kill switch
+ * 26-24:  POWER_SAVE_TYPE
+ *         Indicates current power-saving mode:
+ *         000 -- No power saving
+ *         001 -- MAC power-down
+ *         010 -- PHY (radio) power-down
+ *         011 -- Error
+ *   9-6:  SYS_CONFIG
+ *         Indicates current system configuration, reflecting pins on chip
+ *         as forced high/low by device circuit board.
+ *     4:  GOING_TO_SLEEP
+ *         Indicates MAC is entering a power-saving sleep power-down.
+ *         Not a good time to access device-internal resources.
+ *     3:  MAC_ACCESS_REQ
+ *         Host sets this to request and maintain MAC wakeup, to allow host
+ *         access to device-internal resources.  Host must wait for
+ *         MAC_CLOCK_READY (and !GOING_TO_SLEEP) before accessing non-CSR
+ *         device registers.
+ *     2:  INIT_DONE
+ *         Host sets this to put device into fully operational D0 power mode.
+ *         Host resets this after SW_RESET to put device into low power mode.
+ *     0:  MAC_CLOCK_READY
+ *         Indicates MAC (ucode processor, etc.) is powered up and can run.
+ *         Internal resources are accessible.
+ *         NOTE:  This does not indicate that the processor is actually running.
+ *         NOTE:  This does not indicate that 4965 or 3945 has completed
+ *                init or post-power-down restore of internal SRAM memory.
+ *                Use CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP as indication that
+ *                SRAM is restored and uCode is in normal operation mode.
+ *                Later devices (5xxx/6xxx/1xxx) use non-volatile SRAM, and
+ *                do not need to save/restore it.
+ *         NOTE:  After device reset, this bit remains "0" until host sets
+ *                INIT_DONE
+ */
+#define CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY        (0x00000001)
+#define CSR_GP_CNTRL_REG_FLAG_INIT_DONE              (0x00000004)
+#define CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ         (0x00000008)
+#define CSR_GP_CNTRL_REG_FLAG_GOING_TO_SLEEP         (0x00000010)
+
+#define CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN           (0x00000001)
+
+#define CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE         (0x07000000)
+#define CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE         (0x04000000)
+#define CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW          (0x08000000)
+
+
+/* EEPROM REG */
+#define CSR_EEPROM_REG_READ_VALID_MSK	(0x00000001)
+#define CSR_EEPROM_REG_BIT_CMD		(0x00000002)
+#define CSR_EEPROM_REG_MSK_ADDR		(0x0000FFFC)
+#define CSR_EEPROM_REG_MSK_DATA		(0xFFFF0000)
+
+/* EEPROM GP */
+#define CSR_EEPROM_GP_VALID_MSK		(0x00000007) /* signature */
+#define CSR_EEPROM_GP_IF_OWNER_MSK	(0x00000180)
+#define CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K		(0x00000002)
+#define CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K		(0x00000004)
+
+/* GP REG */
+#define CSR_GP_REG_POWER_SAVE_STATUS_MSK            (0x03000000) /* bit 24/25 */
+#define CSR_GP_REG_NO_POWER_SAVE            (0x00000000)
+#define CSR_GP_REG_MAC_POWER_SAVE           (0x01000000)
+#define CSR_GP_REG_PHY_POWER_SAVE           (0x02000000)
+#define CSR_GP_REG_POWER_SAVE_ERROR         (0x03000000)
+
+
+/* CSR GIO */
+#define CSR_GIO_REG_VAL_L0S_ENABLED	(0x00000002)
+
+/*
+ * UCODE-DRIVER GP (general purpose) mailbox register 1
+ * Host driver and uCode write and/or read this register to communicate with
+ * each other.
+ * Bit fields:
+ *     4:  UCODE_DISABLE
+ *         Host sets this to request permanent halt of uCode, same as
+ *         sending CARD_STATE command with "halt" bit set.
+ *     3:  CT_KILL_EXIT
+ *         Host sets this to request exit from CT_KILL state, i.e. host thinks
+ *         device temperature is low enough to continue normal operation.
+ *     2:  CMD_BLOCKED
+ *         Host sets this during RF KILL power-down sequence (HW, SW, CT KILL)
+ *         to release uCode to clear all Tx and command queues, enter
+ *         unassociated mode, and power down.
+ *         NOTE:  Some devices also use HBUS_TARG_MBX_C register for this bit.
+ *     1:  SW_BIT_RFKILL
+ *         Host sets this when issuing CARD_STATE command to request
+ *         device sleep.
+ *     0:  MAC_SLEEP
+ *         uCode sets this when preparing a power-saving power-down.
+ *         uCode resets this when power-up is complete and SRAM is sane.
+ *         NOTE:  3945/4965 saves internal SRAM data to host when powering down,
+ *                and must restore this data after powering back up.
+ *                MAC_SLEEP is the best indication that restore is complete.
+ *                Later devices (5xxx/6xxx/1xxx) use non-volatile SRAM, and
+ *                do not need to save/restore it.
+ */
+#define CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP             (0x00000001)
+#define CSR_UCODE_SW_BIT_RFKILL                     (0x00000002)
+#define CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED           (0x00000004)
+#define CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT      (0x00000008)
+
+/* GIO Chicken Bits (PCI Express bus link power management) */
+#define CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX  (0x00800000)
+#define CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER  (0x20000000)
+
+/* LED */
+#define CSR_LED_BSM_CTRL_MSK (0xFFFFFFDF)
+#define CSR_LED_REG_TRUN_ON (0x78)
+#define CSR_LED_REG_TRUN_OFF (0x38)
+
+/* ANA_PLL */
+#define CSR39_ANA_PLL_CFG_VAL        (0x01000000)
+
+/* HPET MEM debug */
+#define CSR_DBG_HPET_MEM_REG_VAL	(0xFFFF0000)
+
+/* DRAM INT TABLE */
+#define CSR_DRAM_INT_TBL_ENABLE		(1 << 31)
+#define CSR_DRAM_INIT_TBL_WRAP_CHECK	(1 << 27)
+
+/*
+ * HBUS (Host-side Bus)
+ *
+ * HBUS registers are mapped directly into PCI bus space, but are used
+ * to indirectly access device's internal memory or registers that
+ * may be powered-down.
+ *
+ * Use iwl_legacy_write_direct32()/iwl_legacy_read_direct32() family
+ * for these registers;
+ * host must "grab nic access" via CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ
+ * to make sure the MAC (uCode processor, etc.) is powered up for accessing
+ * internal resources.
+ *
+ * Do not use iwl_write32()/iwl_read32() family to access these registers;
+ * these provide only simple PCI bus access, without waking up the MAC.
+ */
+#define HBUS_BASE	(0x400)
+
+/*
+ * Registers for accessing device's internal SRAM memory (e.g. SCD SRAM
+ * structures, error log, event log, verifying uCode load).
+ * First write to address register, then read from or write to data register
+ * to complete the job.  Once the address register is set up, accesses to
+ * data registers auto-increment the address by one dword.
+ * Bit usage for address registers (read or write):
+ *  0-31:  memory address within device
+ */
+#define HBUS_TARG_MEM_RADDR     (HBUS_BASE+0x00c)
+#define HBUS_TARG_MEM_WADDR     (HBUS_BASE+0x010)
+#define HBUS_TARG_MEM_WDAT      (HBUS_BASE+0x018)
+#define HBUS_TARG_MEM_RDAT      (HBUS_BASE+0x01c)
+
+/* Mailbox C, used as workaround alternative to CSR_UCODE_DRV_GP1 mailbox */
+#define HBUS_TARG_MBX_C         (HBUS_BASE+0x030)
+#define HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED         (0x00000004)
+
+/*
+ * Registers for accessing device's internal peripheral registers
+ * (e.g. SCD, BSM, etc.).  First write to address register,
+ * then read from or write to data register to complete the job.
+ * Bit usage for address registers (read or write):
+ *  0-15:  register address (offset) within device
+ * 24-25:  (# bytes - 1) to read or write (e.g. 3 for dword)
+ */
+#define HBUS_TARG_PRPH_WADDR    (HBUS_BASE+0x044)
+#define HBUS_TARG_PRPH_RADDR    (HBUS_BASE+0x048)
+#define HBUS_TARG_PRPH_WDAT     (HBUS_BASE+0x04c)
+#define HBUS_TARG_PRPH_RDAT     (HBUS_BASE+0x050)
+
+/*
+ * Per-Tx-queue write pointer (index, really!)
+ * Indicates index to next TFD that driver will fill (1 past latest filled).
+ * Bit usage:
+ *  0-7:  queue write index
+ * 11-8:  queue selector
+ */
+#define HBUS_TARG_WRPTR         (HBUS_BASE+0x060)
+
+#endif /* !__iwl_legacy_csr_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-debug.h b/drivers/net/wireless/iwlegacy/iwl-debug.h
new file mode 100644
index 000000000000..ae13112701bf
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-debug.h
@@ -0,0 +1,198 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_debug_h__
+#define __iwl_legacy_debug_h__
+
+struct iwl_priv;
+extern u32 iwlegacy_debug_level;
+
+#define IWL_ERR(p, f, a...) dev_err(&((p)->pci_dev->dev), f, ## a)
+#define IWL_WARN(p, f, a...) dev_warn(&((p)->pci_dev->dev), f, ## a)
+#define IWL_INFO(p, f, a...) dev_info(&((p)->pci_dev->dev), f, ## a)
+#define IWL_CRIT(p, f, a...) dev_crit(&((p)->pci_dev->dev), f, ## a)
+
+#define iwl_print_hex_error(priv, p, len)				 \
+do {									\
+	print_hex_dump(KERN_ERR, "iwl data: ",				\
+		       DUMP_PREFIX_OFFSET, 16, 1, p, len, 1);		\
+} while (0)
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+#define IWL_DEBUG(__priv, level, fmt, args...)				\
+do {									\
+	if (iwl_legacy_get_debug_level(__priv) & (level))			\
+		dev_printk(KERN_ERR, &(__priv->hw->wiphy->dev),		\
+			 "%c %s " fmt, in_interrupt() ? 'I' : 'U',	\
+			__func__ , ## args);				\
+} while (0)
+
+#define IWL_DEBUG_LIMIT(__priv, level, fmt, args...)			\
+do {									\
+	if ((iwl_legacy_get_debug_level(__priv) & (level)) && net_ratelimit())	\
+		dev_printk(KERN_ERR, &(__priv->hw->wiphy->dev),		\
+			"%c %s " fmt, in_interrupt() ? 'I' : 'U',	\
+			 __func__ , ## args);				\
+} while (0)
+
+#define iwl_print_hex_dump(priv, level, p, len) 			\
+do {									\
+	if (iwl_legacy_get_debug_level(priv) & level)				\
+		print_hex_dump(KERN_DEBUG, "iwl data: ",		\
+			       DUMP_PREFIX_OFFSET, 16, 1, p, len, 1);	\
+} while (0)
+
+#else
+#define IWL_DEBUG(__priv, level, fmt, args...)
+#define IWL_DEBUG_LIMIT(__priv, level, fmt, args...)
+static inline void iwl_print_hex_dump(struct iwl_priv *priv, int level,
+				      const void *p, u32 len)
+{}
+#endif				/* CONFIG_IWLWIFI_LEGACY_DEBUG */
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+int iwl_legacy_dbgfs_register(struct iwl_priv *priv, const char *name);
+void iwl_legacy_dbgfs_unregister(struct iwl_priv *priv);
+#else
+static inline int
+iwl_legacy_dbgfs_register(struct iwl_priv *priv, const char *name)
+{
+	return 0;
+}
+static inline void iwl_legacy_dbgfs_unregister(struct iwl_priv *priv)
+{
+}
+#endif				/* CONFIG_IWLWIFI_LEGACY_DEBUGFS */
+
+/*
+ * To use the debug system:
+ *
+ * If you are defining a new debug classification, simply add it to the #define
+ * list here in the form of
+ *
+ * #define IWL_DL_xxxx VALUE
+ *
+ * where xxxx should be the name of the classification (for example, WEP).
+ *
+ * You then need to either add a IWL_xxxx_DEBUG() macro definition for your
+ * classification, or use IWL_DEBUG(IWL_DL_xxxx, ...) whenever you want
+ * to send output to that classification.
+ *
+ * The active debug levels can be accessed via files
+ *
+ * 	/sys/module/iwl4965/parameters/debug{50}
+ *	/sys/module/iwl3945/parameters/debug
+ * 	/sys/class/net/wlan0/device/debug_level
+ *
+ * when CONFIG_IWLWIFI_LEGACY_DEBUG=y.
+ */
+
+/* 0x0000000F - 0x00000001 */
+#define IWL_DL_INFO		(1 << 0)
+#define IWL_DL_MAC80211		(1 << 1)
+#define IWL_DL_HCMD		(1 << 2)
+#define IWL_DL_STATE		(1 << 3)
+/* 0x000000F0 - 0x00000010 */
+#define IWL_DL_MACDUMP		(1 << 4)
+#define IWL_DL_HCMD_DUMP	(1 << 5)
+#define IWL_DL_EEPROM		(1 << 6)
+#define IWL_DL_RADIO		(1 << 7)
+/* 0x00000F00 - 0x00000100 */
+#define IWL_DL_POWER		(1 << 8)
+#define IWL_DL_TEMP		(1 << 9)
+#define IWL_DL_NOTIF		(1 << 10)
+#define IWL_DL_SCAN		(1 << 11)
+/* 0x0000F000 - 0x00001000 */
+#define IWL_DL_ASSOC		(1 << 12)
+#define IWL_DL_DROP		(1 << 13)
+#define IWL_DL_TXPOWER		(1 << 14)
+#define IWL_DL_AP		(1 << 15)
+/* 0x000F0000 - 0x00010000 */
+#define IWL_DL_FW		(1 << 16)
+#define IWL_DL_RF_KILL		(1 << 17)
+#define IWL_DL_FW_ERRORS	(1 << 18)
+#define IWL_DL_LED		(1 << 19)
+/* 0x00F00000 - 0x00100000 */
+#define IWL_DL_RATE		(1 << 20)
+#define IWL_DL_CALIB		(1 << 21)
+#define IWL_DL_WEP		(1 << 22)
+#define IWL_DL_TX		(1 << 23)
+/* 0x0F000000 - 0x01000000 */
+#define IWL_DL_RX		(1 << 24)
+#define IWL_DL_ISR		(1 << 25)
+#define IWL_DL_HT		(1 << 26)
+#define IWL_DL_IO		(1 << 27)
+/* 0xF0000000 - 0x10000000 */
+#define IWL_DL_11H		(1 << 28)
+#define IWL_DL_STATS		(1 << 29)
+#define IWL_DL_TX_REPLY		(1 << 30)
+#define IWL_DL_QOS		(1 << 31)
+
+#define IWL_DEBUG_INFO(p, f, a...)	IWL_DEBUG(p, IWL_DL_INFO, f, ## a)
+#define IWL_DEBUG_MAC80211(p, f, a...)	IWL_DEBUG(p, IWL_DL_MAC80211, f, ## a)
+#define IWL_DEBUG_MACDUMP(p, f, a...)	IWL_DEBUG(p, IWL_DL_MACDUMP, f, ## a)
+#define IWL_DEBUG_TEMP(p, f, a...)	IWL_DEBUG(p, IWL_DL_TEMP, f, ## a)
+#define IWL_DEBUG_SCAN(p, f, a...)	IWL_DEBUG(p, IWL_DL_SCAN, f, ## a)
+#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_HC_DUMP(p, f, a...)	IWL_DEBUG(p, IWL_DL_HCMD_DUMP, f, ## a)
+#define IWL_DEBUG_EEPROM(p, f, a...)	IWL_DEBUG(p, IWL_DL_EEPROM, f, ## a)
+#define IWL_DEBUG_CALIB(p, f, a...)	IWL_DEBUG(p, IWL_DL_CALIB, f, ## a)
+#define IWL_DEBUG_FW(p, f, a...)	IWL_DEBUG(p, IWL_DL_FW, f, ## a)
+#define IWL_DEBUG_RF_KILL(p, f, a...)	IWL_DEBUG(p, IWL_DL_RF_KILL, f, ## a)
+#define IWL_DEBUG_DROP(p, f, a...)	IWL_DEBUG(p, IWL_DL_DROP, f, ## a)
+#define IWL_DEBUG_DROP_LIMIT(p, f, a...)	\
+		IWL_DEBUG_LIMIT(p, IWL_DL_DROP, f, ## a)
+#define IWL_DEBUG_AP(p, f, a...)	IWL_DEBUG(p, IWL_DL_AP, f, ## a)
+#define IWL_DEBUG_TXPOWER(p, f, a...)	IWL_DEBUG(p, IWL_DL_TXPOWER, f, ## a)
+#define IWL_DEBUG_IO(p, f, a...)	IWL_DEBUG(p, IWL_DL_IO, f, ## a)
+#define IWL_DEBUG_RATE(p, f, a...)	IWL_DEBUG(p, IWL_DL_RATE, f, ## a)
+#define IWL_DEBUG_RATE_LIMIT(p, f, a...)	\
+		IWL_DEBUG_LIMIT(p, IWL_DL_RATE, f, ## a)
+#define IWL_DEBUG_NOTIF(p, f, a...)	IWL_DEBUG(p, IWL_DL_NOTIF, f, ## a)
+#define IWL_DEBUG_ASSOC(p, f, a...)	\
+		IWL_DEBUG(p, IWL_DL_ASSOC | IWL_DL_INFO, f, ## a)
+#define IWL_DEBUG_ASSOC_LIMIT(p, f, a...)	\
+		IWL_DEBUG_LIMIT(p, IWL_DL_ASSOC | IWL_DL_INFO, f, ## a)
+#define IWL_DEBUG_HT(p, f, a...)	IWL_DEBUG(p, IWL_DL_HT, f, ## a)
+#define IWL_DEBUG_STATS(p, f, a...)	IWL_DEBUG(p, IWL_DL_STATS, f, ## a)
+#define IWL_DEBUG_STATS_LIMIT(p, f, a...)	\
+		IWL_DEBUG_LIMIT(p, IWL_DL_STATS, f, ## a)
+#define IWL_DEBUG_TX_REPLY(p, f, a...)	IWL_DEBUG(p, IWL_DL_TX_REPLY, f, ## a)
+#define IWL_DEBUG_TX_REPLY_LIMIT(p, f, a...) \
+		IWL_DEBUG_LIMIT(p, IWL_DL_TX_REPLY, f, ## a)
+#define IWL_DEBUG_QOS(p, f, a...)	IWL_DEBUG(p, IWL_DL_QOS, f, ## a)
+#define IWL_DEBUG_RADIO(p, f, a...)	IWL_DEBUG(p, IWL_DL_RADIO, f, ## a)
+#define IWL_DEBUG_POWER(p, f, a...)	IWL_DEBUG(p, IWL_DL_POWER, f, ## a)
+#define IWL_DEBUG_11H(p, f, a...)	IWL_DEBUG(p, IWL_DL_11H, f, ## a)
+
+#endif
diff --git a/drivers/net/wireless/iwlegacy/iwl-debugfs.c b/drivers/net/wireless/iwlegacy/iwl-debugfs.c
new file mode 100644
index 000000000000..2d32438b4cb8
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-debugfs.c
@@ -0,0 +1,1467 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+#include <linux/ieee80211.h>
+#include <net/mac80211.h>
+
+
+#include "iwl-dev.h"
+#include "iwl-debug.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+
+/* create and remove of files */
+#define DEBUGFS_ADD_FILE(name, parent, mode) do {			\
+	if (!debugfs_create_file(#name, mode, parent, priv,		\
+			 &iwl_legacy_dbgfs_##name##_ops))		\
+		goto err;						\
+} while (0)
+
+#define DEBUGFS_ADD_BOOL(name, parent, ptr) do {			\
+	struct dentry *__tmp;						\
+	__tmp = debugfs_create_bool(#name, S_IWUSR | S_IRUSR,		\
+				    parent, ptr);			\
+	if (IS_ERR(__tmp) || !__tmp)					\
+		goto err;						\
+} while (0)
+
+#define DEBUGFS_ADD_X32(name, parent, ptr) do {				\
+	struct dentry *__tmp;						\
+	__tmp = debugfs_create_x32(#name, S_IWUSR | S_IRUSR,		\
+				   parent, ptr);			\
+	if (IS_ERR(__tmp) || !__tmp)					\
+		goto err;						\
+} while (0)
+
+/* file operation */
+#define DEBUGFS_READ_FUNC(name)                                         \
+static ssize_t iwl_legacy_dbgfs_##name##_read(struct file *file,               \
+					char __user *user_buf,          \
+					size_t count, loff_t *ppos);
+
+#define DEBUGFS_WRITE_FUNC(name)                                        \
+static ssize_t iwl_legacy_dbgfs_##name##_write(struct file *file,              \
+					const char __user *user_buf,    \
+					size_t count, loff_t *ppos);
+
+
+static int
+iwl_legacy_dbgfs_open_file_generic(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+#define DEBUGFS_READ_FILE_OPS(name) 				\
+	DEBUGFS_READ_FUNC(name);                                        \
+static const struct file_operations iwl_legacy_dbgfs_##name##_ops = {	\
+	.read = iwl_legacy_dbgfs_##name##_read,				\
+	.open = iwl_legacy_dbgfs_open_file_generic,                    	\
+	.llseek = generic_file_llseek,					\
+};
+
+#define DEBUGFS_WRITE_FILE_OPS(name) 				\
+	DEBUGFS_WRITE_FUNC(name);                                       \
+static const struct file_operations iwl_legacy_dbgfs_##name##_ops = {	\
+	.write = iwl_legacy_dbgfs_##name##_write,			\
+	.open = iwl_legacy_dbgfs_open_file_generic,                    	\
+	.llseek = generic_file_llseek,					\
+};
+
+#define DEBUGFS_READ_WRITE_FILE_OPS(name)                           \
+	DEBUGFS_READ_FUNC(name);                                        \
+	DEBUGFS_WRITE_FUNC(name);                                       \
+static const struct file_operations iwl_legacy_dbgfs_##name##_ops = {	\
+	.write = iwl_legacy_dbgfs_##name##_write,			\
+	.read = iwl_legacy_dbgfs_##name##_read,				\
+	.open = iwl_legacy_dbgfs_open_file_generic,			\
+	.llseek = generic_file_llseek,					\
+};
+
+static ssize_t iwl_legacy_dbgfs_tx_statistics_read(struct file *file,
+						char __user *user_buf,
+						size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	char *buf;
+	int pos = 0;
+
+	int cnt;
+	ssize_t ret;
+	const size_t bufsz = 100 +
+		sizeof(char) * 50 * (MANAGEMENT_MAX + CONTROL_MAX);
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	pos += scnprintf(buf + pos, bufsz - pos, "Management:\n");
+	for (cnt = 0; cnt < MANAGEMENT_MAX; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "\t%25s\t\t: %u\n",
+				 iwl_legacy_get_mgmt_string(cnt),
+				 priv->tx_stats.mgmt[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "Control\n");
+	for (cnt = 0; cnt < CONTROL_MAX; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "\t%25s\t\t: %u\n",
+				 iwl_legacy_get_ctrl_string(cnt),
+				 priv->tx_stats.ctrl[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "Data:\n");
+	pos += scnprintf(buf + pos, bufsz - pos, "\tcnt: %u\n",
+			 priv->tx_stats.data_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos, "\tbytes: %llu\n",
+			 priv->tx_stats.data_bytes);
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t
+iwl_legacy_dbgfs_clear_traffic_statistics_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	u32 clear_flag;
+	char buf[8];
+	int buf_size;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%x", &clear_flag) != 1)
+		return -EFAULT;
+	iwl_legacy_clear_traffic_stats(priv);
+
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_rx_statistics_read(struct file *file,
+						char __user *user_buf,
+						size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	char *buf;
+	int pos = 0;
+	int cnt;
+	ssize_t ret;
+	const size_t bufsz = 100 +
+		sizeof(char) * 50 * (MANAGEMENT_MAX + CONTROL_MAX);
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Management:\n");
+	for (cnt = 0; cnt < MANAGEMENT_MAX; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "\t%25s\t\t: %u\n",
+				 iwl_legacy_get_mgmt_string(cnt),
+				 priv->rx_stats.mgmt[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "Control:\n");
+	for (cnt = 0; cnt < CONTROL_MAX; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "\t%25s\t\t: %u\n",
+				 iwl_legacy_get_ctrl_string(cnt),
+				 priv->rx_stats.ctrl[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "Data:\n");
+	pos += scnprintf(buf + pos, bufsz - pos, "\tcnt: %u\n",
+			 priv->rx_stats.data_cnt);
+	pos += scnprintf(buf + pos, bufsz - pos, "\tbytes: %llu\n",
+			 priv->rx_stats.data_bytes);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+#define BYTE1_MASK 0x000000ff;
+#define BYTE2_MASK 0x0000ffff;
+#define BYTE3_MASK 0x00ffffff;
+static ssize_t iwl_legacy_dbgfs_sram_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	u32 val;
+	char *buf;
+	ssize_t ret;
+	int i;
+	int pos = 0;
+	struct iwl_priv *priv = file->private_data;
+	size_t bufsz;
+
+	/* default is to dump the entire data segment */
+	if (!priv->dbgfs_sram_offset && !priv->dbgfs_sram_len) {
+		priv->dbgfs_sram_offset = 0x800000;
+		if (priv->ucode_type == UCODE_INIT)
+			priv->dbgfs_sram_len = priv->ucode_init_data.len;
+		else
+			priv->dbgfs_sram_len = priv->ucode_data.len;
+	}
+	bufsz =  30 + priv->dbgfs_sram_len * sizeof(char) * 10;
+	buf = kmalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	pos += scnprintf(buf + pos, bufsz - pos, "sram_len: 0x%x\n",
+			priv->dbgfs_sram_len);
+	pos += scnprintf(buf + pos, bufsz - pos, "sram_offset: 0x%x\n",
+			priv->dbgfs_sram_offset);
+	for (i = priv->dbgfs_sram_len; i > 0; i -= 4) {
+		val = iwl_legacy_read_targ_mem(priv, priv->dbgfs_sram_offset + \
+					priv->dbgfs_sram_len - i);
+		if (i < 4) {
+			switch (i) {
+			case 1:
+				val &= BYTE1_MASK;
+				break;
+			case 2:
+				val &= BYTE2_MASK;
+				break;
+			case 3:
+				val &= BYTE3_MASK;
+				break;
+			}
+		}
+		if (!(i % 16))
+			pos += scnprintf(buf + pos, bufsz - pos, "\n");
+		pos += scnprintf(buf + pos, bufsz - pos, "0x%08x ", val);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "\n");
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_sram_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[64];
+	int buf_size;
+	u32 offset, len;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+
+	if (sscanf(buf, "%x,%x", &offset, &len) == 2) {
+		priv->dbgfs_sram_offset = offset;
+		priv->dbgfs_sram_len = len;
+	} else {
+		priv->dbgfs_sram_offset = 0;
+		priv->dbgfs_sram_len = 0;
+	}
+
+	return count;
+}
+
+static ssize_t
+iwl_legacy_dbgfs_stations_read(struct file *file, char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	struct iwl_station_entry *station;
+	int max_sta = priv->hw_params.max_stations;
+	char *buf;
+	int i, j, pos = 0;
+	ssize_t ret;
+	/* Add 30 for initial string */
+	const size_t bufsz = 30 + sizeof(char) * 500 * (priv->num_stations);
+
+	buf = kmalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	pos += scnprintf(buf + pos, bufsz - pos, "num of stations: %d\n\n",
+			priv->num_stations);
+
+	for (i = 0; i < max_sta; i++) {
+		station = &priv->stations[i];
+		if (!station->used)
+			continue;
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "station %d - addr: %pM, flags: %#x\n",
+				 i, station->sta.sta.addr,
+				 station->sta.station_flags_msk);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"TID\tseq_num\ttxq_id\tframes\ttfds\t");
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"start_idx\tbitmap\t\t\trate_n_flags\n");
+
+		for (j = 0; j < MAX_TID_COUNT; j++) {
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"%d:\t%#x\t%#x\t%u\t%u\t%u\t\t%#.16llx\t%#x",
+				j, station->tid[j].seq_number,
+				station->tid[j].agg.txq_id,
+				station->tid[j].agg.frame_count,
+				station->tid[j].tfds_in_queue,
+				station->tid[j].agg.start_idx,
+				station->tid[j].agg.bitmap,
+				station->tid[j].agg.rate_n_flags);
+
+			if (station->tid[j].agg.wait_for_ba)
+				pos += scnprintf(buf + pos, bufsz - pos,
+						 " - waitforba");
+			pos += scnprintf(buf + pos, bufsz - pos, "\n");
+		}
+
+		pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	}
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_nvm_read(struct file *file,
+				       char __user *user_buf,
+				       size_t count,
+				       loff_t *ppos)
+{
+	ssize_t ret;
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0, ofs = 0, buf_size = 0;
+	const u8 *ptr;
+	char *buf;
+	u16 eeprom_ver;
+	size_t eeprom_len = priv->cfg->base_params->eeprom_size;
+	buf_size = 4 * eeprom_len + 256;
+
+	if (eeprom_len % 16) {
+		IWL_ERR(priv, "NVM size is not multiple of 16.\n");
+		return -ENODATA;
+	}
+
+	ptr = priv->eeprom;
+	if (!ptr) {
+		IWL_ERR(priv, "Invalid EEPROM memory\n");
+		return -ENOMEM;
+	}
+
+	/* 4 characters for byte 0xYY */
+	buf = kzalloc(buf_size, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+	eeprom_ver = iwl_legacy_eeprom_query16(priv, EEPROM_VERSION);
+	pos += scnprintf(buf + pos, buf_size - pos, "EEPROM "
+			"version: 0x%x\n", eeprom_ver);
+	for (ofs = 0 ; ofs < eeprom_len ; ofs += 16) {
+		pos += scnprintf(buf + pos, buf_size - pos, "0x%.4x ", ofs);
+		hex_dump_to_buffer(ptr + ofs, 16 , 16, 2, buf + pos,
+				   buf_size - pos, 0);
+		pos += strlen(buf + pos);
+		if (buf_size - pos > 0)
+			buf[pos++] = '\n';
+	}
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_log_event_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char *buf;
+	int pos = 0;
+	ssize_t ret = -ENOMEM;
+
+	ret = pos = priv->cfg->ops->lib->dump_nic_event_log(
+					priv, true, &buf, true);
+	if (buf) {
+		ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+		kfree(buf);
+	}
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_log_event_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	u32 event_log_flag;
+	char buf[8];
+	int buf_size;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &event_log_flag) != 1)
+		return -EFAULT;
+	if (event_log_flag == 1)
+		priv->cfg->ops->lib->dump_nic_event_log(priv, true,
+							NULL, false);
+
+	return count;
+}
+
+
+
+static ssize_t
+iwl_legacy_dbgfs_channels_read(struct file *file, char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_supported_band *supp_band = NULL;
+	int pos = 0, i, bufsz = PAGE_SIZE;
+	char *buf;
+	ssize_t ret;
+
+	if (!test_bit(STATUS_GEO_CONFIGURED, &priv->status))
+		return -EAGAIN;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_2GHZ);
+	if (supp_band) {
+		channels = supp_band->channels;
+
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"Displaying %d channels in 2.4GHz band 802.11bg):\n",
+				supp_band->n_channels);
+
+		for (i = 0; i < supp_band->n_channels; i++)
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"%d: %ddBm: BSS%s%s, %s.\n",
+				channels[i].hw_value,
+				channels[i].max_power,
+				channels[i].flags & IEEE80211_CHAN_RADAR ?
+				" (IEEE 802.11h required)" : "",
+				((channels[i].flags & IEEE80211_CHAN_NO_IBSS)
+				|| (channels[i].flags &
+				IEEE80211_CHAN_RADAR)) ? "" :
+				", IBSS",
+				channels[i].flags &
+				IEEE80211_CHAN_PASSIVE_SCAN ?
+				"passive only" : "active/passive");
+	}
+	supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_5GHZ);
+	if (supp_band) {
+		channels = supp_band->channels;
+
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"Displaying %d channels in 5.2GHz band (802.11a)\n",
+				supp_band->n_channels);
+
+		for (i = 0; i < supp_band->n_channels; i++)
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"%d: %ddBm: BSS%s%s, %s.\n",
+				channels[i].hw_value,
+				channels[i].max_power,
+				channels[i].flags & IEEE80211_CHAN_RADAR ?
+				" (IEEE 802.11h required)" : "",
+				((channels[i].flags & IEEE80211_CHAN_NO_IBSS)
+				|| (channels[i].flags &
+				IEEE80211_CHAN_RADAR)) ? "" :
+				", IBSS",
+				channels[i].flags &
+				IEEE80211_CHAN_PASSIVE_SCAN ?
+				"passive only" : "active/passive");
+	}
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_status_read(struct file *file,
+						char __user *user_buf,
+						size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	char buf[512];
+	int pos = 0;
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_HCMD_ACTIVE:\t %d\n",
+		test_bit(STATUS_HCMD_ACTIVE, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_INT_ENABLED:\t %d\n",
+		test_bit(STATUS_INT_ENABLED, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_RF_KILL_HW:\t %d\n",
+		test_bit(STATUS_RF_KILL_HW, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_CT_KILL:\t\t %d\n",
+		test_bit(STATUS_CT_KILL, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_INIT:\t\t %d\n",
+		test_bit(STATUS_INIT, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_ALIVE:\t\t %d\n",
+		test_bit(STATUS_ALIVE, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_READY:\t\t %d\n",
+		test_bit(STATUS_READY, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_TEMPERATURE:\t %d\n",
+		test_bit(STATUS_TEMPERATURE, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_GEO_CONFIGURED:\t %d\n",
+		test_bit(STATUS_GEO_CONFIGURED, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_EXIT_PENDING:\t %d\n",
+		test_bit(STATUS_EXIT_PENDING, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_STATISTICS:\t %d\n",
+		test_bit(STATUS_STATISTICS, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_SCANNING:\t %d\n",
+		test_bit(STATUS_SCANNING, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_SCAN_ABORTING:\t %d\n",
+		test_bit(STATUS_SCAN_ABORTING, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_SCAN_HW:\t\t %d\n",
+		test_bit(STATUS_SCAN_HW, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_POWER_PMI:\t %d\n",
+		test_bit(STATUS_POWER_PMI, &priv->status));
+	pos += scnprintf(buf + pos, bufsz - pos, "STATUS_FW_ERROR:\t %d\n",
+		test_bit(STATUS_FW_ERROR, &priv->status));
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_interrupt_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	int cnt = 0;
+	char *buf;
+	int bufsz = 24 * 64; /* 24 items * 64 char per item */
+	ssize_t ret;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"Interrupt Statistics Report:\n");
+
+	pos += scnprintf(buf + pos, bufsz - pos, "HW Error:\t\t\t %u\n",
+		priv->isr_stats.hw);
+	pos += scnprintf(buf + pos, bufsz - pos, "SW Error:\t\t\t %u\n",
+		priv->isr_stats.sw);
+	if (priv->isr_stats.sw || priv->isr_stats.hw) {
+		pos += scnprintf(buf + pos, bufsz - pos,
+			"\tLast Restarting Code:  0x%X\n",
+			priv->isr_stats.err_code);
+	}
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	pos += scnprintf(buf + pos, bufsz - pos, "Frame transmitted:\t\t %u\n",
+		priv->isr_stats.sch);
+	pos += scnprintf(buf + pos, bufsz - pos, "Alive interrupt:\t\t %u\n",
+		priv->isr_stats.alive);
+#endif
+	pos += scnprintf(buf + pos, bufsz - pos,
+		"HW RF KILL switch toggled:\t %u\n",
+		priv->isr_stats.rfkill);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "CT KILL:\t\t\t %u\n",
+		priv->isr_stats.ctkill);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Wakeup Interrupt:\t\t %u\n",
+		priv->isr_stats.wakeup);
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+		"Rx command responses:\t\t %u\n",
+		priv->isr_stats.rx);
+	for (cnt = 0; cnt < REPLY_MAX; cnt++) {
+		if (priv->isr_stats.rx_handlers[cnt] > 0)
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"\tRx handler[%36s]:\t\t %u\n",
+				iwl_legacy_get_cmd_string(cnt),
+				priv->isr_stats.rx_handlers[cnt]);
+	}
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Tx/FH interrupt:\t\t %u\n",
+		priv->isr_stats.tx);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Unexpected INTA:\t\t %u\n",
+		priv->isr_stats.unhandled);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_interrupt_write(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	u32 reset_flag;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%x", &reset_flag) != 1)
+		return -EFAULT;
+	if (reset_flag == 0)
+		iwl_legacy_clear_isr_stats(priv);
+
+	return count;
+}
+
+static ssize_t
+iwl_legacy_dbgfs_qos_read(struct file *file, char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	struct iwl_rxon_context *ctx;
+	int pos = 0, i;
+	char buf[256 * NUM_IWL_RXON_CTX];
+	const size_t bufsz = sizeof(buf);
+
+	for_each_context(priv, ctx) {
+		pos += scnprintf(buf + pos, bufsz - pos, "context %d:\n",
+				 ctx->ctxid);
+		for (i = 0; i < AC_NUM; i++) {
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"\tcw_min\tcw_max\taifsn\ttxop\n");
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"AC[%d]\t%u\t%u\t%u\t%u\n", i,
+				ctx->qos_data.def_qos_parm.ac[i].cw_min,
+				ctx->qos_data.def_qos_parm.ac[i].cw_max,
+				ctx->qos_data.def_qos_parm.ac[i].aifsn,
+				ctx->qos_data.def_qos_parm.ac[i].edca_txop);
+		}
+		pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	}
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_disable_ht40_write(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int ht40;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &ht40) != 1)
+		return -EFAULT;
+	if (!iwl_legacy_is_any_associated(priv))
+		priv->disable_ht40 = ht40 ? true : false;
+	else {
+		IWL_ERR(priv, "Sta associated with AP - "
+			"Change to 40MHz channel support is not allowed\n");
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_disable_ht40_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[100];
+	int pos = 0;
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"11n 40MHz Mode: %s\n",
+			priv->disable_ht40 ? "Disabled" : "Enabled");
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+DEBUGFS_READ_WRITE_FILE_OPS(sram);
+DEBUGFS_READ_WRITE_FILE_OPS(log_event);
+DEBUGFS_READ_FILE_OPS(nvm);
+DEBUGFS_READ_FILE_OPS(stations);
+DEBUGFS_READ_FILE_OPS(channels);
+DEBUGFS_READ_FILE_OPS(status);
+DEBUGFS_READ_WRITE_FILE_OPS(interrupt);
+DEBUGFS_READ_FILE_OPS(qos);
+DEBUGFS_READ_WRITE_FILE_OPS(disable_ht40);
+
+static ssize_t iwl_legacy_dbgfs_traffic_log_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0, ofs = 0;
+	int cnt = 0, entry;
+	struct iwl_tx_queue *txq;
+	struct iwl_queue *q;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	char *buf;
+	int bufsz = ((IWL_TRAFFIC_ENTRIES * IWL_TRAFFIC_ENTRY_SIZE * 64) * 2) +
+		(priv->cfg->base_params->num_of_queues * 32 * 8) + 400;
+	const u8 *ptr;
+	ssize_t ret;
+
+	if (!priv->txq) {
+		IWL_ERR(priv, "txq not ready\n");
+		return -EAGAIN;
+	}
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate buffer\n");
+		return -ENOMEM;
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "Tx Queue\n");
+	for (cnt = 0; cnt < priv->hw_params.max_txq_num; cnt++) {
+		txq = &priv->txq[cnt];
+		q = &txq->q;
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"q[%d]: read_ptr: %u, write_ptr: %u\n",
+				cnt, q->read_ptr, q->write_ptr);
+	}
+	if (priv->tx_traffic && (iwlegacy_debug_level & IWL_DL_TX)) {
+		ptr = priv->tx_traffic;
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"Tx Traffic idx: %u\n",	priv->tx_traffic_idx);
+		for (cnt = 0, ofs = 0; cnt < IWL_TRAFFIC_ENTRIES; cnt++) {
+			for (entry = 0; entry < IWL_TRAFFIC_ENTRY_SIZE / 16;
+			     entry++,  ofs += 16) {
+				pos += scnprintf(buf + pos, bufsz - pos,
+						"0x%.4x ", ofs);
+				hex_dump_to_buffer(ptr + ofs, 16, 16, 2,
+						   buf + pos, bufsz - pos, 0);
+				pos += strlen(buf + pos);
+				if (bufsz - pos > 0)
+					buf[pos++] = '\n';
+			}
+		}
+	}
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Rx Queue\n");
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"read: %u, write: %u\n",
+			 rxq->read, rxq->write);
+
+	if (priv->rx_traffic && (iwlegacy_debug_level & IWL_DL_RX)) {
+		ptr = priv->rx_traffic;
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"Rx Traffic idx: %u\n",	priv->rx_traffic_idx);
+		for (cnt = 0, ofs = 0; cnt < IWL_TRAFFIC_ENTRIES; cnt++) {
+			for (entry = 0; entry < IWL_TRAFFIC_ENTRY_SIZE / 16;
+			     entry++,  ofs += 16) {
+				pos += scnprintf(buf + pos, bufsz - pos,
+						"0x%.4x ", ofs);
+				hex_dump_to_buffer(ptr + ofs, 16, 16, 2,
+						   buf + pos, bufsz - pos, 0);
+				pos += strlen(buf + pos);
+				if (bufsz - pos > 0)
+					buf[pos++] = '\n';
+			}
+		}
+	}
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_traffic_log_write(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int traffic_log;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &traffic_log) != 1)
+		return -EFAULT;
+	if (traffic_log == 0)
+		iwl_legacy_reset_traffic_log(priv);
+
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_tx_queue_read(struct file *file,
+						char __user *user_buf,
+						size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	struct iwl_tx_queue *txq;
+	struct iwl_queue *q;
+	char *buf;
+	int pos = 0;
+	int cnt;
+	int ret;
+	const size_t bufsz = sizeof(char) * 64 *
+				priv->cfg->base_params->num_of_queues;
+
+	if (!priv->txq) {
+		IWL_ERR(priv, "txq not ready\n");
+		return -EAGAIN;
+	}
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	for (cnt = 0; cnt < priv->hw_params.max_txq_num; cnt++) {
+		txq = &priv->txq[cnt];
+		q = &txq->q;
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"hwq %.2d: read=%u write=%u stop=%d"
+				" swq_id=%#.2x (ac %d/hwq %d)\n",
+				cnt, q->read_ptr, q->write_ptr,
+				!!test_bit(cnt, priv->queue_stopped),
+				txq->swq_id, txq->swq_id & 3,
+				(txq->swq_id >> 2) & 0x1f);
+		if (cnt >= 4)
+			continue;
+		/* for the ACs, display the stop count too */
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"        stop-count: %d\n",
+				atomic_read(&priv->queue_stop_count[cnt]));
+	}
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_rx_queue_read(struct file *file,
+						char __user *user_buf,
+						size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	char buf[256];
+	int pos = 0;
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "read: %u\n",
+						rxq->read);
+	pos += scnprintf(buf + pos, bufsz - pos, "write: %u\n",
+						rxq->write);
+	pos += scnprintf(buf + pos, bufsz - pos, "free_count: %u\n",
+						rxq->free_count);
+	if (rxq->rb_stts) {
+		pos += scnprintf(buf + pos, bufsz - pos, "closed_rb_num: %u\n",
+			 le16_to_cpu(rxq->rb_stts->closed_rb_num) &  0x0FFF);
+	} else {
+		pos += scnprintf(buf + pos, bufsz - pos,
+					"closed_rb_num: Not Allocated\n");
+	}
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_ucode_rx_stats_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	return priv->cfg->ops->lib->debugfs_ops.rx_stats_read(file,
+			user_buf, count, ppos);
+}
+
+static ssize_t iwl_legacy_dbgfs_ucode_tx_stats_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	return priv->cfg->ops->lib->debugfs_ops.tx_stats_read(file,
+			user_buf, count, ppos);
+}
+
+static ssize_t iwl_legacy_dbgfs_ucode_general_stats_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	return priv->cfg->ops->lib->debugfs_ops.general_stats_read(file,
+			user_buf, count, ppos);
+}
+
+static ssize_t iwl_legacy_dbgfs_sensitivity_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	int cnt = 0;
+	char *buf;
+	int bufsz = sizeof(struct iwl_sensitivity_data) * 4 + 100;
+	ssize_t ret;
+	struct iwl_sensitivity_data *data;
+
+	data = &priv->sensitivity_data;
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	pos += scnprintf(buf + pos, bufsz - pos, "auto_corr_ofdm:\t\t\t %u\n",
+			data->auto_corr_ofdm);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"auto_corr_ofdm_mrc:\t\t %u\n",
+			data->auto_corr_ofdm_mrc);
+	pos += scnprintf(buf + pos, bufsz - pos, "auto_corr_ofdm_x1:\t\t %u\n",
+			data->auto_corr_ofdm_x1);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"auto_corr_ofdm_mrc_x1:\t\t %u\n",
+			data->auto_corr_ofdm_mrc_x1);
+	pos += scnprintf(buf + pos, bufsz - pos, "auto_corr_cck:\t\t\t %u\n",
+			data->auto_corr_cck);
+	pos += scnprintf(buf + pos, bufsz - pos, "auto_corr_cck_mrc:\t\t %u\n",
+			data->auto_corr_cck_mrc);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"last_bad_plcp_cnt_ofdm:\t\t %u\n",
+			data->last_bad_plcp_cnt_ofdm);
+	pos += scnprintf(buf + pos, bufsz - pos, "last_fa_cnt_ofdm:\t\t %u\n",
+			data->last_fa_cnt_ofdm);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"last_bad_plcp_cnt_cck:\t\t %u\n",
+			data->last_bad_plcp_cnt_cck);
+	pos += scnprintf(buf + pos, bufsz - pos, "last_fa_cnt_cck:\t\t %u\n",
+			data->last_fa_cnt_cck);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_curr_state:\t\t\t %u\n",
+			data->nrg_curr_state);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_prev_state:\t\t\t %u\n",
+			data->nrg_prev_state);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_value:\t\t\t");
+	for (cnt = 0; cnt < 10; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos, " %u",
+				data->nrg_value[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_silence_rssi:\t\t");
+	for (cnt = 0; cnt < NRG_NUM_PREV_STAT_L; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos, " %u",
+				data->nrg_silence_rssi[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_silence_ref:\t\t %u\n",
+			data->nrg_silence_ref);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_energy_idx:\t\t\t %u\n",
+			data->nrg_energy_idx);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_silence_idx:\t\t %u\n",
+			data->nrg_silence_idx);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_th_cck:\t\t\t %u\n",
+			data->nrg_th_cck);
+	pos += scnprintf(buf + pos, bufsz - pos,
+			"nrg_auto_corr_silence_diff:\t %u\n",
+			data->nrg_auto_corr_silence_diff);
+	pos += scnprintf(buf + pos, bufsz - pos, "num_in_cck_no_fa:\t\t %u\n",
+			data->num_in_cck_no_fa);
+	pos += scnprintf(buf + pos, bufsz - pos, "nrg_th_ofdm:\t\t\t %u\n",
+			data->nrg_th_ofdm);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+
+static ssize_t iwl_legacy_dbgfs_chain_noise_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	int cnt = 0;
+	char *buf;
+	int bufsz = sizeof(struct iwl_chain_noise_data) * 4 + 100;
+	ssize_t ret;
+	struct iwl_chain_noise_data *data;
+
+	data = &priv->chain_noise_data;
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERR(priv, "Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	pos += scnprintf(buf + pos, bufsz - pos, "active_chains:\t\t\t %u\n",
+			data->active_chains);
+	pos += scnprintf(buf + pos, bufsz - pos, "chain_noise_a:\t\t\t %u\n",
+			data->chain_noise_a);
+	pos += scnprintf(buf + pos, bufsz - pos, "chain_noise_b:\t\t\t %u\n",
+			data->chain_noise_b);
+	pos += scnprintf(buf + pos, bufsz - pos, "chain_noise_c:\t\t\t %u\n",
+			data->chain_noise_c);
+	pos += scnprintf(buf + pos, bufsz - pos, "chain_signal_a:\t\t\t %u\n",
+			data->chain_signal_a);
+	pos += scnprintf(buf + pos, bufsz - pos, "chain_signal_b:\t\t\t %u\n",
+			data->chain_signal_b);
+	pos += scnprintf(buf + pos, bufsz - pos, "chain_signal_c:\t\t\t %u\n",
+			data->chain_signal_c);
+	pos += scnprintf(buf + pos, bufsz - pos, "beacon_count:\t\t\t %u\n",
+			data->beacon_count);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "disconn_array:\t\t\t");
+	for (cnt = 0; cnt < NUM_RX_CHAINS; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos, " %u",
+				data->disconn_array[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	pos += scnprintf(buf + pos, bufsz - pos, "delta_gain_code:\t\t");
+	for (cnt = 0; cnt < NUM_RX_CHAINS; cnt++) {
+		pos += scnprintf(buf + pos, bufsz - pos, " %u",
+				data->delta_gain_code[cnt]);
+	}
+	pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	pos += scnprintf(buf + pos, bufsz - pos, "radio_write:\t\t\t %u\n",
+			data->radio_write);
+	pos += scnprintf(buf + pos, bufsz - pos, "state:\t\t\t\t %u\n",
+			data->state);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_power_save_status_read(struct file *file,
+						    char __user *user_buf,
+						    size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[60];
+	int pos = 0;
+	const size_t bufsz = sizeof(buf);
+	u32 pwrsave_status;
+
+	pwrsave_status = iwl_read32(priv, CSR_GP_CNTRL) &
+			CSR_GP_REG_POWER_SAVE_STATUS_MSK;
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Power Save Status: ");
+	pos += scnprintf(buf + pos, bufsz - pos, "%s\n",
+		(pwrsave_status == CSR_GP_REG_NO_POWER_SAVE) ? "none" :
+		(pwrsave_status == CSR_GP_REG_MAC_POWER_SAVE) ? "MAC" :
+		(pwrsave_status == CSR_GP_REG_PHY_POWER_SAVE) ? "PHY" :
+		"error");
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_clear_ucode_statistics_write(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int clear;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &clear) != 1)
+		return -EFAULT;
+
+	/* make request to uCode to retrieve statistics information */
+	mutex_lock(&priv->mutex);
+	iwl_legacy_send_statistics_request(priv, CMD_SYNC, true);
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_ucode_tracing_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	char buf[128];
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "ucode trace timer is %s\n",
+			priv->event_log.ucode_trace ? "On" : "Off");
+	pos += scnprintf(buf + pos, bufsz - pos, "non_wraps_count:\t\t %u\n",
+			priv->event_log.non_wraps_count);
+	pos += scnprintf(buf + pos, bufsz - pos, "wraps_once_count:\t\t %u\n",
+			priv->event_log.wraps_once_count);
+	pos += scnprintf(buf + pos, bufsz - pos, "wraps_more_count:\t\t %u\n",
+			priv->event_log.wraps_more_count);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_ucode_tracing_write(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int trace;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &trace) != 1)
+		return -EFAULT;
+
+	if (trace) {
+		priv->event_log.ucode_trace = true;
+		/* schedule the ucode timer to occur in UCODE_TRACE_PERIOD */
+		mod_timer(&priv->ucode_trace,
+			jiffies + msecs_to_jiffies(UCODE_TRACE_PERIOD));
+	} else {
+		priv->event_log.ucode_trace = false;
+		del_timer_sync(&priv->ucode_trace);
+	}
+
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_rxon_flags_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int len = 0;
+	char buf[20];
+
+	len = sprintf(buf, "0x%04X\n",
+		le32_to_cpu(priv->contexts[IWL_RXON_CTX_BSS].active.flags));
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t iwl_legacy_dbgfs_rxon_filter_flags_read(struct file *file,
+						char __user *user_buf,
+						size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int len = 0;
+	char buf[20];
+
+	len = sprintf(buf, "0x%04X\n",
+	le32_to_cpu(priv->contexts[IWL_RXON_CTX_BSS].active.filter_flags));
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t iwl_legacy_dbgfs_fh_reg_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char *buf;
+	int pos = 0;
+	ssize_t ret = -EFAULT;
+
+	if (priv->cfg->ops->lib->dump_fh) {
+		ret = pos = priv->cfg->ops->lib->dump_fh(priv, &buf, true);
+		if (buf) {
+			ret = simple_read_from_buffer(user_buf,
+						      count, ppos, buf, pos);
+			kfree(buf);
+		}
+	}
+
+	return ret;
+}
+
+static ssize_t iwl_legacy_dbgfs_missed_beacon_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	char buf[12];
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "%d\n",
+			priv->missed_beacon_threshold);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_missed_beacon_write(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int missed;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &missed) != 1)
+		return -EINVAL;
+
+	if (missed < IWL_MISSED_BEACON_THRESHOLD_MIN ||
+	    missed > IWL_MISSED_BEACON_THRESHOLD_MAX)
+		priv->missed_beacon_threshold =
+			IWL_MISSED_BEACON_THRESHOLD_DEF;
+	else
+		priv->missed_beacon_threshold = missed;
+
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_plcp_delta_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int pos = 0;
+	char buf[12];
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "%u\n",
+			priv->cfg->base_params->plcp_delta_threshold);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_plcp_delta_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int plcp;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &plcp) != 1)
+		return -EINVAL;
+	if ((plcp < IWL_MAX_PLCP_ERR_THRESHOLD_MIN) ||
+		(plcp > IWL_MAX_PLCP_ERR_THRESHOLD_MAX))
+		priv->cfg->base_params->plcp_delta_threshold =
+			IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE;
+	else
+		priv->cfg->base_params->plcp_delta_threshold = plcp;
+	return count;
+}
+
+static ssize_t iwl_legacy_dbgfs_force_reset_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	int i, pos = 0;
+	char buf[300];
+	const size_t bufsz = sizeof(buf);
+	struct iwl_force_reset *force_reset;
+
+	for (i = 0; i < IWL_MAX_FORCE_RESET; i++) {
+		force_reset = &priv->force_reset[i];
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"Force reset method %d\n", i);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"\tnumber of reset request: %d\n",
+				force_reset->reset_request_count);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"\tnumber of reset request success: %d\n",
+				force_reset->reset_success_count);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"\tnumber of reset request reject: %d\n",
+				force_reset->reset_reject_count);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				"\treset duration: %lu\n",
+				force_reset->reset_duration);
+	}
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_legacy_dbgfs_force_reset_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int reset, ret;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &reset) != 1)
+		return -EINVAL;
+	switch (reset) {
+	case IWL_RF_RESET:
+	case IWL_FW_RESET:
+		ret = iwl_legacy_force_reset(priv, reset, true);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return ret ? ret : count;
+}
+
+static ssize_t iwl_legacy_dbgfs_wd_timeout_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos) {
+
+	struct iwl_priv *priv = file->private_data;
+	char buf[8];
+	int buf_size;
+	int timeout;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	if (sscanf(buf, "%d", &timeout) != 1)
+		return -EINVAL;
+	if (timeout < 0 || timeout > IWL_MAX_WD_TIMEOUT)
+		timeout = IWL_DEF_WD_TIMEOUT;
+
+	priv->cfg->base_params->wd_timeout = timeout;
+	iwl_legacy_setup_watchdog(priv);
+	return count;
+}
+
+DEBUGFS_READ_FILE_OPS(rx_statistics);
+DEBUGFS_READ_FILE_OPS(tx_statistics);
+DEBUGFS_READ_WRITE_FILE_OPS(traffic_log);
+DEBUGFS_READ_FILE_OPS(rx_queue);
+DEBUGFS_READ_FILE_OPS(tx_queue);
+DEBUGFS_READ_FILE_OPS(ucode_rx_stats);
+DEBUGFS_READ_FILE_OPS(ucode_tx_stats);
+DEBUGFS_READ_FILE_OPS(ucode_general_stats);
+DEBUGFS_READ_FILE_OPS(sensitivity);
+DEBUGFS_READ_FILE_OPS(chain_noise);
+DEBUGFS_READ_FILE_OPS(power_save_status);
+DEBUGFS_WRITE_FILE_OPS(clear_ucode_statistics);
+DEBUGFS_WRITE_FILE_OPS(clear_traffic_statistics);
+DEBUGFS_READ_WRITE_FILE_OPS(ucode_tracing);
+DEBUGFS_READ_FILE_OPS(fh_reg);
+DEBUGFS_READ_WRITE_FILE_OPS(missed_beacon);
+DEBUGFS_READ_WRITE_FILE_OPS(plcp_delta);
+DEBUGFS_READ_WRITE_FILE_OPS(force_reset);
+DEBUGFS_READ_FILE_OPS(rxon_flags);
+DEBUGFS_READ_FILE_OPS(rxon_filter_flags);
+DEBUGFS_WRITE_FILE_OPS(wd_timeout);
+
+/*
+ * Create the debugfs files and directories
+ *
+ */
+int iwl_legacy_dbgfs_register(struct iwl_priv *priv, const char *name)
+{
+	struct dentry *phyd = priv->hw->wiphy->debugfsdir;
+	struct dentry *dir_drv, *dir_data, *dir_rf, *dir_debug;
+
+	dir_drv = debugfs_create_dir(name, phyd);
+	if (!dir_drv)
+		return -ENOMEM;
+
+	priv->debugfs_dir = dir_drv;
+
+	dir_data = debugfs_create_dir("data", dir_drv);
+	if (!dir_data)
+		goto err;
+	dir_rf = debugfs_create_dir("rf", dir_drv);
+	if (!dir_rf)
+		goto err;
+	dir_debug = debugfs_create_dir("debug", dir_drv);
+	if (!dir_debug)
+		goto err;
+
+	DEBUGFS_ADD_FILE(nvm, dir_data, S_IRUSR);
+	DEBUGFS_ADD_FILE(sram, dir_data, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(log_event, dir_data, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(stations, dir_data, S_IRUSR);
+	DEBUGFS_ADD_FILE(channels, dir_data, S_IRUSR);
+	DEBUGFS_ADD_FILE(status, dir_data, S_IRUSR);
+	DEBUGFS_ADD_FILE(interrupt, dir_data, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(qos, dir_data, S_IRUSR);
+	DEBUGFS_ADD_FILE(disable_ht40, dir_data, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(rx_statistics, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(tx_statistics, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(traffic_log, dir_debug, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(rx_queue, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(tx_queue, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(power_save_status, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(clear_ucode_statistics, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(clear_traffic_statistics, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(fh_reg, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(missed_beacon, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(plcp_delta, dir_debug, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(force_reset, dir_debug, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(ucode_rx_stats, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(ucode_tx_stats, dir_debug, S_IRUSR);
+	DEBUGFS_ADD_FILE(ucode_general_stats, dir_debug, S_IRUSR);
+
+	if (priv->cfg->base_params->sensitivity_calib_by_driver)
+		DEBUGFS_ADD_FILE(sensitivity, dir_debug, S_IRUSR);
+	if (priv->cfg->base_params->chain_noise_calib_by_driver)
+		DEBUGFS_ADD_FILE(chain_noise, dir_debug, S_IRUSR);
+	if (priv->cfg->base_params->ucode_tracing)
+		DEBUGFS_ADD_FILE(ucode_tracing, dir_debug, S_IWUSR | S_IRUSR);
+	DEBUGFS_ADD_FILE(rxon_flags, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(rxon_filter_flags, dir_debug, S_IWUSR);
+	DEBUGFS_ADD_FILE(wd_timeout, dir_debug, S_IWUSR);
+	if (priv->cfg->base_params->sensitivity_calib_by_driver)
+		DEBUGFS_ADD_BOOL(disable_sensitivity, dir_rf,
+				 &priv->disable_sens_cal);
+	if (priv->cfg->base_params->chain_noise_calib_by_driver)
+		DEBUGFS_ADD_BOOL(disable_chain_noise, dir_rf,
+				 &priv->disable_chain_noise_cal);
+	DEBUGFS_ADD_BOOL(disable_tx_power, dir_rf,
+				&priv->disable_tx_power_cal);
+	return 0;
+
+err:
+	IWL_ERR(priv, "Can't create the debugfs directory\n");
+	iwl_legacy_dbgfs_unregister(priv);
+	return -ENOMEM;
+}
+EXPORT_SYMBOL(iwl_legacy_dbgfs_register);
+
+/**
+ * Remove the debugfs files and directories
+ *
+ */
+void iwl_legacy_dbgfs_unregister(struct iwl_priv *priv)
+{
+	if (!priv->debugfs_dir)
+		return;
+
+	debugfs_remove_recursive(priv->debugfs_dir);
+	priv->debugfs_dir = NULL;
+}
+EXPORT_SYMBOL(iwl_legacy_dbgfs_unregister);
diff --git a/drivers/net/wireless/iwlegacy/iwl-dev.h b/drivers/net/wireless/iwlegacy/iwl-dev.h
new file mode 100644
index 000000000000..9ee849d669f3
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-dev.h
@@ -0,0 +1,1426 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+/*
+ * Please use this file (iwl-dev.h) for driver implementation definitions.
+ * Please use iwl-commands.h for uCode API definitions.
+ * Please use iwl-4965-hw.h for hardware-related definitions.
+ */
+
+#ifndef __iwl_legacy_dev_h__
+#define __iwl_legacy_dev_h__
+
+#include <linux/pci.h> /* for struct pci_device_id */
+#include <linux/kernel.h>
+#include <linux/leds.h>
+#include <linux/wait.h>
+#include <net/ieee80211_radiotap.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-csr.h"
+#include "iwl-prph.h"
+#include "iwl-fh.h"
+#include "iwl-debug.h"
+#include "iwl-4965-hw.h"
+#include "iwl-3945-hw.h"
+#include "iwl-led.h"
+#include "iwl-power.h"
+#include "iwl-legacy-rs.h"
+
+struct iwl_tx_queue;
+
+/* CT-KILL constants */
+#define CT_KILL_THRESHOLD_LEGACY   110 /* in Celsius */
+
+/* Default noise level to report when noise measurement is not available.
+ *   This may be because we're:
+ *   1)  Not associated (4965, no beacon statistics being sent to driver)
+ *   2)  Scanning (noise measurement does not apply to associated channel)
+ *   3)  Receiving CCK (3945 delivers noise info only for OFDM frames)
+ * Use default noise value of -127 ... this is below the range of measurable
+ *   Rx dBm for either 3945 or 4965, so it can indicate "unmeasurable" to user.
+ *   Also, -127 works better than 0 when averaging frames with/without
+ *   noise info (e.g. averaging might be done in app); measured dBm values are
+ *   always negative ... using a negative value as the default keeps all
+ *   averages within an s8's (used in some apps) range of negative values. */
+#define IWL_NOISE_MEAS_NOT_AVAILABLE (-127)
+
+/*
+ * RTS threshold here is total size [2347] minus 4 FCS bytes
+ * Per spec:
+ *   a value of 0 means RTS on all data/management packets
+ *   a value > max MSDU size means no RTS
+ * else RTS for data/management frames where MPDU is larger
+ *   than RTS value.
+ */
+#define DEFAULT_RTS_THRESHOLD     2347U
+#define MIN_RTS_THRESHOLD         0U
+#define MAX_RTS_THRESHOLD         2347U
+#define MAX_MSDU_SIZE		  2304U
+#define MAX_MPDU_SIZE		  2346U
+#define DEFAULT_BEACON_INTERVAL   100U
+#define	DEFAULT_SHORT_RETRY_LIMIT 7U
+#define	DEFAULT_LONG_RETRY_LIMIT  4U
+
+struct iwl_rx_mem_buffer {
+	dma_addr_t page_dma;
+	struct page *page;
+	struct list_head list;
+};
+
+#define rxb_addr(r) page_address(r->page)
+
+/* defined below */
+struct iwl_device_cmd;
+
+struct iwl_cmd_meta {
+	/* only for SYNC commands, iff the reply skb is wanted */
+	struct iwl_host_cmd *source;
+	/*
+	 * only for ASYNC commands
+	 * (which is somewhat stupid -- look at iwl-sta.c for instance
+	 * which duplicates a bunch of code because the callback isn't
+	 * invoked for SYNC commands, if it were and its result passed
+	 * through it would be simpler...)
+	 */
+	void (*callback)(struct iwl_priv *priv,
+			 struct iwl_device_cmd *cmd,
+			 struct iwl_rx_packet *pkt);
+
+	/* The CMD_SIZE_HUGE flag bit indicates that the command
+	 * structure is stored at the end of the shared queue memory. */
+	u32 flags;
+
+	DEFINE_DMA_UNMAP_ADDR(mapping);
+	DEFINE_DMA_UNMAP_LEN(len);
+};
+
+/*
+ * Generic queue structure
+ *
+ * Contains common data for Rx and Tx queues
+ */
+struct iwl_queue {
+	int n_bd;              /* number of BDs in this queue */
+	int write_ptr;       /* 1-st empty entry (index) host_w*/
+	int read_ptr;         /* last used entry (index) host_r*/
+	/* use for monitoring and recovering the stuck queue */
+	dma_addr_t dma_addr;   /* physical addr for BD's */
+	int n_window;	       /* safe queue window */
+	u32 id;
+	int low_mark;	       /* low watermark, resume queue if free
+				* space more than this */
+	int high_mark;         /* high watermark, stop queue if free
+				* space less than this */
+} __packed;
+
+/* One for each TFD */
+struct iwl_tx_info {
+	struct sk_buff *skb;
+	struct iwl_rxon_context *ctx;
+};
+
+/**
+ * struct iwl_tx_queue - Tx Queue for DMA
+ * @q: generic Rx/Tx queue descriptor
+ * @bd: base of circular buffer of TFDs
+ * @cmd: array of command/TX buffer pointers
+ * @meta: array of meta data for each command/tx buffer
+ * @dma_addr_cmd: physical address of cmd/tx buffer array
+ * @txb: array of per-TFD driver data
+ * @time_stamp: time (in jiffies) of last read_ptr change
+ * @need_update: indicates need to update read/write index
+ * @sched_retry: indicates queue is high-throughput aggregation (HT AGG) enabled
+ *
+ * A Tx queue consists of circular buffer of BDs (a.k.a. TFDs, transmit frame
+ * descriptors) and required locking structures.
+ */
+#define TFD_TX_CMD_SLOTS 256
+#define TFD_CMD_SLOTS 32
+
+struct iwl_tx_queue {
+	struct iwl_queue q;
+	void *tfds;
+	struct iwl_device_cmd **cmd;
+	struct iwl_cmd_meta *meta;
+	struct iwl_tx_info *txb;
+	unsigned long time_stamp;
+	u8 need_update;
+	u8 sched_retry;
+	u8 active;
+	u8 swq_id;
+};
+
+#define IWL_NUM_SCAN_RATES         (2)
+
+struct iwl4965_channel_tgd_info {
+	u8 type;
+	s8 max_power;
+};
+
+struct iwl4965_channel_tgh_info {
+	s64 last_radar_time;
+};
+
+#define IWL4965_MAX_RATE (33)
+
+struct iwl3945_clip_group {
+	/* maximum power level to prevent clipping for each rate, derived by
+	 *   us from this band's saturation power in EEPROM */
+	const s8 clip_powers[IWL_MAX_RATES];
+};
+
+/* current Tx power values to use, one for each rate for each channel.
+ * requested power is limited by:
+ * -- regulatory EEPROM limits for this channel
+ * -- hardware capabilities (clip-powers)
+ * -- spectrum management
+ * -- user preference (e.g. iwconfig)
+ * when requested power is set, base power index must also be set. */
+struct iwl3945_channel_power_info {
+	struct iwl3945_tx_power tpc;	/* actual radio and DSP gain settings */
+	s8 power_table_index;	/* actual (compenst'd) index into gain table */
+	s8 base_power_index;	/* gain index for power at factory temp. */
+	s8 requested_power;	/* power (dBm) requested for this chnl/rate */
+};
+
+/* current scan Tx power values to use, one for each scan rate for each
+ * channel. */
+struct iwl3945_scan_power_info {
+	struct iwl3945_tx_power tpc;	/* actual radio and DSP gain settings */
+	s8 power_table_index;	/* actual (compenst'd) index into gain table */
+	s8 requested_power;	/* scan pwr (dBm) requested for chnl/rate */
+};
+
+/*
+ * One for each channel, holds all channel setup data
+ * Some of the fields (e.g. eeprom and flags/max_power_avg) are redundant
+ *     with one another!
+ */
+struct iwl_channel_info {
+	struct iwl4965_channel_tgd_info tgd;
+	struct iwl4965_channel_tgh_info tgh;
+	struct iwl_eeprom_channel eeprom;	/* EEPROM regulatory limit */
+	struct iwl_eeprom_channel ht40_eeprom;	/* EEPROM regulatory limit for
+						 * HT40 channel */
+
+	u8 channel;	  /* channel number */
+	u8 flags;	  /* flags copied from EEPROM */
+	s8 max_power_avg; /* (dBm) regul. eeprom, normal Tx, any rate */
+	s8 curr_txpow;	  /* (dBm) regulatory/spectrum/user (not h/w) limit */
+	s8 min_power;	  /* always 0 */
+	s8 scan_power;	  /* (dBm) regul. eeprom, direct scans, any rate */
+
+	u8 group_index;	  /* 0-4, maps channel to group1/2/3/4/5 */
+	u8 band_index;	  /* 0-4, maps channel to band1/2/3/4/5 */
+	enum ieee80211_band band;
+
+	/* HT40 channel info */
+	s8 ht40_max_power_avg;	/* (dBm) regul. eeprom, normal Tx, any rate */
+	u8 ht40_flags;		/* flags copied from EEPROM */
+	u8 ht40_extension_channel; /* HT_IE_EXT_CHANNEL_* */
+
+	/* Radio/DSP gain settings for each "normal" data Tx rate.
+	 * These include, in addition to RF and DSP gain, a few fields for
+	 *   remembering/modifying gain settings (indexes). */
+	struct iwl3945_channel_power_info power_info[IWL4965_MAX_RATE];
+
+	/* Radio/DSP gain settings for each scan rate, for directed scans. */
+	struct iwl3945_scan_power_info scan_pwr_info[IWL_NUM_SCAN_RATES];
+};
+
+#define IWL_TX_FIFO_BK		0	/* shared */
+#define IWL_TX_FIFO_BE		1
+#define IWL_TX_FIFO_VI		2	/* shared */
+#define IWL_TX_FIFO_VO		3
+#define IWL_TX_FIFO_UNUSED	-1
+
+/* Minimum number of queues. MAX_NUM is defined in hw specific files.
+ * Set the minimum to accommodate the 4 standard TX queues, 1 command
+ * queue, 2 (unused) HCCA queues, and 4 HT queues (one for each AC) */
+#define IWL_MIN_NUM_QUEUES	10
+
+#define IWL_DEFAULT_CMD_QUEUE_NUM	4
+
+#define IEEE80211_DATA_LEN              2304
+#define IEEE80211_4ADDR_LEN             30
+#define IEEE80211_HLEN                  (IEEE80211_4ADDR_LEN)
+#define IEEE80211_FRAME_LEN             (IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+struct iwl_frame {
+	union {
+		struct ieee80211_hdr frame;
+		struct iwl_tx_beacon_cmd beacon;
+		u8 raw[IEEE80211_FRAME_LEN];
+		u8 cmd[360];
+	} u;
+	struct list_head list;
+};
+
+#define SEQ_TO_SN(seq) (((seq) & IEEE80211_SCTL_SEQ) >> 4)
+#define SN_TO_SEQ(ssn) (((ssn) << 4) & IEEE80211_SCTL_SEQ)
+#define MAX_SN ((IEEE80211_SCTL_SEQ) >> 4)
+
+enum {
+	CMD_SYNC = 0,
+	CMD_SIZE_NORMAL = 0,
+	CMD_NO_SKB = 0,
+	CMD_SIZE_HUGE = (1 << 0),
+	CMD_ASYNC = (1 << 1),
+	CMD_WANT_SKB = (1 << 2),
+};
+
+#define DEF_CMD_PAYLOAD_SIZE 320
+
+/**
+ * struct iwl_device_cmd
+ *
+ * For allocation of the command and tx queues, this establishes the overall
+ * size of the largest command we send to uCode, except for a scan command
+ * (which is relatively huge; space is allocated separately).
+ */
+struct iwl_device_cmd {
+	struct iwl_cmd_header hdr;	/* uCode API */
+	union {
+		u32 flags;
+		u8 val8;
+		u16 val16;
+		u32 val32;
+		struct iwl_tx_cmd tx;
+		u8 payload[DEF_CMD_PAYLOAD_SIZE];
+	} __packed cmd;
+} __packed;
+
+#define TFD_MAX_PAYLOAD_SIZE (sizeof(struct iwl_device_cmd))
+
+
+struct iwl_host_cmd {
+	const void *data;
+	unsigned long reply_page;
+	void (*callback)(struct iwl_priv *priv,
+			 struct iwl_device_cmd *cmd,
+			 struct iwl_rx_packet *pkt);
+	u32 flags;
+	u16 len;
+	u8 id;
+};
+
+#define SUP_RATE_11A_MAX_NUM_CHANNELS  8
+#define SUP_RATE_11B_MAX_NUM_CHANNELS  4
+#define SUP_RATE_11G_MAX_NUM_CHANNELS  12
+
+/**
+ * struct iwl_rx_queue - Rx queue
+ * @bd: driver's pointer to buffer of receive buffer descriptors (rbd)
+ * @bd_dma: bus address of buffer of receive buffer descriptors (rbd)
+ * @read: Shared index to newest available Rx buffer
+ * @write: Shared index to oldest written Rx packet
+ * @free_count: Number of pre-allocated buffers in rx_free
+ * @rx_free: list of free SKBs for use
+ * @rx_used: List of Rx buffers with no SKB
+ * @need_update: flag to indicate we need to update read/write index
+ * @rb_stts: driver's pointer to receive buffer status
+ * @rb_stts_dma: bus address of receive buffer status
+ *
+ * NOTE:  rx_free and rx_used are used as a FIFO for iwl_rx_mem_buffers
+ */
+struct iwl_rx_queue {
+	__le32 *bd;
+	dma_addr_t bd_dma;
+	struct iwl_rx_mem_buffer pool[RX_QUEUE_SIZE + RX_FREE_BUFFERS];
+	struct iwl_rx_mem_buffer *queue[RX_QUEUE_SIZE];
+	u32 read;
+	u32 write;
+	u32 free_count;
+	u32 write_actual;
+	struct list_head rx_free;
+	struct list_head rx_used;
+	int need_update;
+	struct iwl_rb_status *rb_stts;
+	dma_addr_t rb_stts_dma;
+	spinlock_t lock;
+};
+
+#define IWL_SUPPORTED_RATES_IE_LEN         8
+
+#define MAX_TID_COUNT        9
+
+#define IWL_INVALID_RATE     0xFF
+#define IWL_INVALID_VALUE    -1
+
+/**
+ * struct iwl_ht_agg -- aggregation status while waiting for block-ack
+ * @txq_id: Tx queue used for Tx attempt
+ * @frame_count: # frames attempted by Tx command
+ * @wait_for_ba: Expect block-ack before next Tx reply
+ * @start_idx: Index of 1st Transmit Frame Descriptor (TFD) in Tx window
+ * @bitmap0: Low order bitmap, one bit for each frame pending ACK in Tx window
+ * @bitmap1: High order, one bit for each frame pending ACK in Tx window
+ * @rate_n_flags: Rate at which Tx was attempted
+ *
+ * If REPLY_TX indicates that aggregation was attempted, driver must wait
+ * for block ack (REPLY_COMPRESSED_BA).  This struct stores tx reply info
+ * until block ack arrives.
+ */
+struct iwl_ht_agg {
+	u16 txq_id;
+	u16 frame_count;
+	u16 wait_for_ba;
+	u16 start_idx;
+	u64 bitmap;
+	u32 rate_n_flags;
+#define IWL_AGG_OFF 0
+#define IWL_AGG_ON 1
+#define IWL_EMPTYING_HW_QUEUE_ADDBA 2
+#define IWL_EMPTYING_HW_QUEUE_DELBA 3
+	u8 state;
+};
+
+
+struct iwl_tid_data {
+	u16 seq_number; /* 4965 only */
+	u16 tfds_in_queue;
+	struct iwl_ht_agg agg;
+};
+
+struct iwl_hw_key {
+	u32 cipher;
+	int keylen;
+	u8 keyidx;
+	u8 key[32];
+};
+
+union iwl_ht_rate_supp {
+	u16 rates;
+	struct {
+		u8 siso_rate;
+		u8 mimo_rate;
+	};
+};
+
+#define CFG_HT_RX_AMPDU_FACTOR_8K   (0x0)
+#define CFG_HT_RX_AMPDU_FACTOR_16K  (0x1)
+#define CFG_HT_RX_AMPDU_FACTOR_32K  (0x2)
+#define CFG_HT_RX_AMPDU_FACTOR_64K  (0x3)
+#define CFG_HT_RX_AMPDU_FACTOR_DEF  CFG_HT_RX_AMPDU_FACTOR_64K
+#define CFG_HT_RX_AMPDU_FACTOR_MAX  CFG_HT_RX_AMPDU_FACTOR_64K
+#define CFG_HT_RX_AMPDU_FACTOR_MIN  CFG_HT_RX_AMPDU_FACTOR_8K
+
+/*
+ * Maximal MPDU density for TX aggregation
+ * 4 - 2us density
+ * 5 - 4us density
+ * 6 - 8us density
+ * 7 - 16us density
+ */
+#define CFG_HT_MPDU_DENSITY_2USEC   (0x4)
+#define CFG_HT_MPDU_DENSITY_4USEC   (0x5)
+#define CFG_HT_MPDU_DENSITY_8USEC   (0x6)
+#define CFG_HT_MPDU_DENSITY_16USEC  (0x7)
+#define CFG_HT_MPDU_DENSITY_DEF CFG_HT_MPDU_DENSITY_4USEC
+#define CFG_HT_MPDU_DENSITY_MAX CFG_HT_MPDU_DENSITY_16USEC
+#define CFG_HT_MPDU_DENSITY_MIN     (0x1)
+
+struct iwl_ht_config {
+	bool single_chain_sufficient;
+	enum ieee80211_smps_mode smps; /* current smps mode */
+};
+
+/* QoS structures */
+struct iwl_qos_info {
+	int qos_active;
+	struct iwl_qosparam_cmd def_qos_parm;
+};
+
+/*
+ * Structure should be accessed with sta_lock held. When station addition
+ * is in progress (IWL_STA_UCODE_INPROGRESS) it is possible to access only
+ * the commands (iwl_legacy_addsta_cmd and iwl_link_quality_cmd) without
+ * sta_lock held.
+ */
+struct iwl_station_entry {
+	struct iwl_legacy_addsta_cmd sta;
+	struct iwl_tid_data tid[MAX_TID_COUNT];
+	u8 used, ctxid;
+	struct iwl_hw_key keyinfo;
+	struct iwl_link_quality_cmd *lq;
+};
+
+struct iwl_station_priv_common {
+	struct iwl_rxon_context *ctx;
+	u8 sta_id;
+};
+
+/*
+ * iwl_station_priv: Driver's private station information
+ *
+ * When mac80211 creates a station it reserves some space (hw->sta_data_size)
+ * in the structure for use by driver. This structure is places in that
+ * space.
+ *
+ * The common struct MUST be first because it is shared between
+ * 3945 and 4965!
+ */
+struct iwl_station_priv {
+	struct iwl_station_priv_common common;
+	struct iwl_lq_sta lq_sta;
+	atomic_t pending_frames;
+	bool client;
+	bool asleep;
+};
+
+/**
+ * struct iwl_vif_priv - driver's private per-interface information
+ *
+ * When mac80211 allocates a virtual interface, it can allocate
+ * space for us to put data into.
+ */
+struct iwl_vif_priv {
+	struct iwl_rxon_context *ctx;
+	u8 ibss_bssid_sta_id;
+};
+
+/* one for each uCode image (inst/data, boot/init/runtime) */
+struct fw_desc {
+	void *v_addr;		/* access by driver */
+	dma_addr_t p_addr;	/* access by card's busmaster DMA */
+	u32 len;		/* bytes */
+};
+
+/* uCode file layout */
+struct iwl_ucode_header {
+	__le32 ver;	/* major/minor/API/serial */
+	struct {
+		__le32 inst_size;	/* bytes of runtime code */
+		__le32 data_size;	/* bytes of runtime data */
+		__le32 init_size;	/* bytes of init code */
+		__le32 init_data_size;	/* bytes of init data */
+		__le32 boot_size;	/* bytes of bootstrap code */
+		u8 data[0];		/* in same order as sizes */
+	} v1;
+};
+
+struct iwl4965_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num;
+	u16 frag_num;
+	unsigned long packet_time;
+	struct list_head list;
+};
+
+struct iwl_sensitivity_ranges {
+	u16 min_nrg_cck;
+	u16 max_nrg_cck;
+
+	u16 nrg_th_cck;
+	u16 nrg_th_ofdm;
+
+	u16 auto_corr_min_ofdm;
+	u16 auto_corr_min_ofdm_mrc;
+	u16 auto_corr_min_ofdm_x1;
+	u16 auto_corr_min_ofdm_mrc_x1;
+
+	u16 auto_corr_max_ofdm;
+	u16 auto_corr_max_ofdm_mrc;
+	u16 auto_corr_max_ofdm_x1;
+	u16 auto_corr_max_ofdm_mrc_x1;
+
+	u16 auto_corr_max_cck;
+	u16 auto_corr_max_cck_mrc;
+	u16 auto_corr_min_cck;
+	u16 auto_corr_min_cck_mrc;
+
+	u16 barker_corr_th_min;
+	u16 barker_corr_th_min_mrc;
+	u16 nrg_th_cca;
+};
+
+
+#define KELVIN_TO_CELSIUS(x) ((x)-273)
+#define CELSIUS_TO_KELVIN(x) ((x)+273)
+
+
+/**
+ * struct iwl_hw_params
+ * @max_txq_num: Max # Tx queues supported
+ * @dma_chnl_num: Number of Tx DMA/FIFO channels
+ * @scd_bc_tbls_size: size of scheduler byte count tables
+ * @tfd_size: TFD size
+ * @tx/rx_chains_num: Number of TX/RX chains
+ * @valid_tx/rx_ant: usable antennas
+ * @max_rxq_size: Max # Rx frames in Rx queue (must be power-of-2)
+ * @max_rxq_log: Log-base-2 of max_rxq_size
+ * @rx_page_order: Rx buffer page order
+ * @rx_wrt_ptr_reg: FH{39}_RSCSR_CHNL0_WPTR
+ * @max_stations:
+ * @ht40_channel: is 40MHz width possible in band 2.4
+ * BIT(IEEE80211_BAND_5GHZ) BIT(IEEE80211_BAND_5GHZ)
+ * @sw_crypto: 0 for hw, 1 for sw
+ * @max_xxx_size: for ucode uses
+ * @ct_kill_threshold: temperature threshold
+ * @beacon_time_tsf_bits: number of valid tsf bits for beacon time
+ * @struct iwl_sensitivity_ranges: range of sensitivity values
+ */
+struct iwl_hw_params {
+	u8 max_txq_num;
+	u8 dma_chnl_num;
+	u16 scd_bc_tbls_size;
+	u32 tfd_size;
+	u8  tx_chains_num;
+	u8  rx_chains_num;
+	u8  valid_tx_ant;
+	u8  valid_rx_ant;
+	u16 max_rxq_size;
+	u16 max_rxq_log;
+	u32 rx_page_order;
+	u32 rx_wrt_ptr_reg;
+	u8  max_stations;
+	u8  ht40_channel;
+	u8  max_beacon_itrvl;	/* in 1024 ms */
+	u32 max_inst_size;
+	u32 max_data_size;
+	u32 max_bsm_size;
+	u32 ct_kill_threshold; /* value in hw-dependent units */
+	u16 beacon_time_tsf_bits;
+	const struct iwl_sensitivity_ranges *sens;
+};
+
+
+/******************************************************************************
+ *
+ * Functions implemented in core module which are forward declared here
+ * for use by iwl-[4-5].c
+ *
+ * NOTE:  The implementation of these functions are not hardware specific
+ * which is why they are in the core module files.
+ *
+ * Naming convention --
+ * iwl_         <-- Is part of iwlwifi
+ * iwlXXXX_     <-- Hardware specific (implemented in iwl-XXXX.c for XXXX)
+ * iwl4965_bg_      <-- Called from work queue context
+ * iwl4965_mac_     <-- mac80211 callback
+ *
+ ****************************************************************************/
+extern void iwl4965_update_chain_flags(struct iwl_priv *priv);
+extern const u8 iwlegacy_bcast_addr[ETH_ALEN];
+extern int iwl_legacy_queue_space(const struct iwl_queue *q);
+static inline int iwl_legacy_queue_used(const struct iwl_queue *q, int i)
+{
+	return q->write_ptr >= q->read_ptr ?
+		(i >= q->read_ptr && i < q->write_ptr) :
+		!(i < q->read_ptr && i >= q->write_ptr);
+}
+
+
+static inline u8 iwl_legacy_get_cmd_index(struct iwl_queue *q, u32 index,
+								int is_huge)
+{
+	/*
+	 * This is for init calibration result and scan command which
+	 * required buffer > TFD_MAX_PAYLOAD_SIZE,
+	 * the big buffer at end of command array
+	 */
+	if (is_huge)
+		return q->n_window;	/* must be power of 2 */
+
+	/* Otherwise, use normal size buffers */
+	return index & (q->n_window - 1);
+}
+
+
+struct iwl_dma_ptr {
+	dma_addr_t dma;
+	void *addr;
+	size_t size;
+};
+
+#define IWL_OPERATION_MODE_AUTO     0
+#define IWL_OPERATION_MODE_HT_ONLY  1
+#define IWL_OPERATION_MODE_MIXED    2
+#define IWL_OPERATION_MODE_20MHZ    3
+
+#define IWL_TX_CRC_SIZE 4
+#define IWL_TX_DELIMITER_SIZE 4
+
+#define TX_POWER_IWL_ILLEGAL_VOLTAGE -10000
+
+/* Sensitivity and chain noise calibration */
+#define INITIALIZATION_VALUE		0xFFFF
+#define IWL4965_CAL_NUM_BEACONS		20
+#define IWL_CAL_NUM_BEACONS		16
+#define MAXIMUM_ALLOWED_PATHLOSS	15
+
+#define CHAIN_NOISE_MAX_DELTA_GAIN_CODE 3
+
+#define MAX_FA_OFDM  50
+#define MIN_FA_OFDM  5
+#define MAX_FA_CCK   50
+#define MIN_FA_CCK   5
+
+#define AUTO_CORR_STEP_OFDM       1
+
+#define AUTO_CORR_STEP_CCK     3
+#define AUTO_CORR_MAX_TH_CCK   160
+
+#define NRG_DIFF               2
+#define NRG_STEP_CCK           2
+#define NRG_MARGIN             8
+#define MAX_NUMBER_CCK_NO_FA 100
+
+#define AUTO_CORR_CCK_MIN_VAL_DEF    (125)
+
+#define CHAIN_A             0
+#define CHAIN_B             1
+#define CHAIN_C             2
+#define CHAIN_NOISE_DELTA_GAIN_INIT_VAL 4
+#define ALL_BAND_FILTER			0xFF00
+#define IN_BAND_FILTER			0xFF
+#define MIN_AVERAGE_NOISE_MAX_VALUE	0xFFFFFFFF
+
+#define NRG_NUM_PREV_STAT_L     20
+#define NUM_RX_CHAINS           3
+
+enum iwl4965_false_alarm_state {
+	IWL_FA_TOO_MANY = 0,
+	IWL_FA_TOO_FEW = 1,
+	IWL_FA_GOOD_RANGE = 2,
+};
+
+enum iwl4965_chain_noise_state {
+	IWL_CHAIN_NOISE_ALIVE = 0,  /* must be 0 */
+	IWL_CHAIN_NOISE_ACCUMULATE,
+	IWL_CHAIN_NOISE_CALIBRATED,
+	IWL_CHAIN_NOISE_DONE,
+};
+
+enum iwl4965_calib_enabled_state {
+	IWL_CALIB_DISABLED = 0,  /* must be 0 */
+	IWL_CALIB_ENABLED = 1,
+};
+
+/*
+ * enum iwl_calib
+ * defines the order in which results of initial calibrations
+ * should be sent to the runtime uCode
+ */
+enum iwl_calib {
+	IWL_CALIB_MAX,
+};
+
+/* Opaque calibration results */
+struct iwl_calib_result {
+	void *buf;
+	size_t buf_len;
+};
+
+enum ucode_type {
+	UCODE_NONE = 0,
+	UCODE_INIT,
+	UCODE_RT
+};
+
+/* Sensitivity calib data */
+struct iwl_sensitivity_data {
+	u32 auto_corr_ofdm;
+	u32 auto_corr_ofdm_mrc;
+	u32 auto_corr_ofdm_x1;
+	u32 auto_corr_ofdm_mrc_x1;
+	u32 auto_corr_cck;
+	u32 auto_corr_cck_mrc;
+
+	u32 last_bad_plcp_cnt_ofdm;
+	u32 last_fa_cnt_ofdm;
+	u32 last_bad_plcp_cnt_cck;
+	u32 last_fa_cnt_cck;
+
+	u32 nrg_curr_state;
+	u32 nrg_prev_state;
+	u32 nrg_value[10];
+	u8  nrg_silence_rssi[NRG_NUM_PREV_STAT_L];
+	u32 nrg_silence_ref;
+	u32 nrg_energy_idx;
+	u32 nrg_silence_idx;
+	u32 nrg_th_cck;
+	s32 nrg_auto_corr_silence_diff;
+	u32 num_in_cck_no_fa;
+	u32 nrg_th_ofdm;
+
+	u16 barker_corr_th_min;
+	u16 barker_corr_th_min_mrc;
+	u16 nrg_th_cca;
+};
+
+/* Chain noise (differential Rx gain) calib data */
+struct iwl_chain_noise_data {
+	u32 active_chains;
+	u32 chain_noise_a;
+	u32 chain_noise_b;
+	u32 chain_noise_c;
+	u32 chain_signal_a;
+	u32 chain_signal_b;
+	u32 chain_signal_c;
+	u16 beacon_count;
+	u8 disconn_array[NUM_RX_CHAINS];
+	u8 delta_gain_code[NUM_RX_CHAINS];
+	u8 radio_write;
+	u8 state;
+};
+
+#define	EEPROM_SEM_TIMEOUT 10		/* milliseconds */
+#define EEPROM_SEM_RETRY_LIMIT 1000	/* number of attempts (not time) */
+
+#define IWL_TRAFFIC_ENTRIES	(256)
+#define IWL_TRAFFIC_ENTRY_SIZE  (64)
+
+enum {
+	MEASUREMENT_READY = (1 << 0),
+	MEASUREMENT_ACTIVE = (1 << 1),
+};
+
+/* interrupt statistics */
+struct isr_statistics {
+	u32 hw;
+	u32 sw;
+	u32 err_code;
+	u32 sch;
+	u32 alive;
+	u32 rfkill;
+	u32 ctkill;
+	u32 wakeup;
+	u32 rx;
+	u32 rx_handlers[REPLY_MAX];
+	u32 tx;
+	u32 unhandled;
+};
+
+/* management statistics */
+enum iwl_mgmt_stats {
+	MANAGEMENT_ASSOC_REQ = 0,
+	MANAGEMENT_ASSOC_RESP,
+	MANAGEMENT_REASSOC_REQ,
+	MANAGEMENT_REASSOC_RESP,
+	MANAGEMENT_PROBE_REQ,
+	MANAGEMENT_PROBE_RESP,
+	MANAGEMENT_BEACON,
+	MANAGEMENT_ATIM,
+	MANAGEMENT_DISASSOC,
+	MANAGEMENT_AUTH,
+	MANAGEMENT_DEAUTH,
+	MANAGEMENT_ACTION,
+	MANAGEMENT_MAX,
+};
+/* control statistics */
+enum iwl_ctrl_stats {
+	CONTROL_BACK_REQ =  0,
+	CONTROL_BACK,
+	CONTROL_PSPOLL,
+	CONTROL_RTS,
+	CONTROL_CTS,
+	CONTROL_ACK,
+	CONTROL_CFEND,
+	CONTROL_CFENDACK,
+	CONTROL_MAX,
+};
+
+struct traffic_stats {
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+	u32 mgmt[MANAGEMENT_MAX];
+	u32 ctrl[CONTROL_MAX];
+	u32 data_cnt;
+	u64 data_bytes;
+#endif
+};
+
+/*
+ * iwl_switch_rxon: "channel switch" structure
+ *
+ * @ switch_in_progress: channel switch in progress
+ * @ channel: new channel
+ */
+struct iwl_switch_rxon {
+	bool switch_in_progress;
+	__le16 channel;
+};
+
+/*
+ * schedule the timer to wake up every UCODE_TRACE_PERIOD milliseconds
+ * to perform continuous uCode event logging operation if enabled
+ */
+#define UCODE_TRACE_PERIOD (100)
+
+/*
+ * iwl_event_log: current uCode event log position
+ *
+ * @ucode_trace: enable/disable ucode continuous trace timer
+ * @num_wraps: how many times the event buffer wraps
+ * @next_entry:  the entry just before the next one that uCode would fill
+ * @non_wraps_count: counter for no wrap detected when dump ucode events
+ * @wraps_once_count: counter for wrap once detected when dump ucode events
+ * @wraps_more_count: counter for wrap more than once detected
+ *		      when dump ucode events
+ */
+struct iwl_event_log {
+	bool ucode_trace;
+	u32 num_wraps;
+	u32 next_entry;
+	int non_wraps_count;
+	int wraps_once_count;
+	int wraps_more_count;
+};
+
+/*
+ * host interrupt timeout value
+ * used with setting interrupt coalescing timer
+ * the CSR_INT_COALESCING is an 8 bit register in 32-usec unit
+ *
+ * default interrupt coalescing timer is 64 x 32 = 2048 usecs
+ * default interrupt coalescing calibration timer is 16 x 32 = 512 usecs
+ */
+#define IWL_HOST_INT_TIMEOUT_MAX	(0xFF)
+#define IWL_HOST_INT_TIMEOUT_DEF	(0x40)
+#define IWL_HOST_INT_TIMEOUT_MIN	(0x0)
+#define IWL_HOST_INT_CALIB_TIMEOUT_MAX	(0xFF)
+#define IWL_HOST_INT_CALIB_TIMEOUT_DEF	(0x10)
+#define IWL_HOST_INT_CALIB_TIMEOUT_MIN	(0x0)
+
+/*
+ * This is the threshold value of plcp error rate per 100mSecs.  It is
+ * used to set and check for the validity of plcp_delta.
+ */
+#define IWL_MAX_PLCP_ERR_THRESHOLD_MIN	(1)
+#define IWL_MAX_PLCP_ERR_THRESHOLD_DEF	(50)
+#define IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF	(100)
+#define IWL_MAX_PLCP_ERR_EXT_LONG_THRESHOLD_DEF	(200)
+#define IWL_MAX_PLCP_ERR_THRESHOLD_MAX	(255)
+#define IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE	(0)
+
+#define IWL_DELAY_NEXT_FORCE_RF_RESET  (HZ*3)
+#define IWL_DELAY_NEXT_FORCE_FW_RELOAD (HZ*5)
+
+/* TX queue watchdog timeouts in mSecs */
+#define IWL_DEF_WD_TIMEOUT	(2000)
+#define IWL_LONG_WD_TIMEOUT	(10000)
+#define IWL_MAX_WD_TIMEOUT	(120000)
+
+enum iwl_reset {
+	IWL_RF_RESET = 0,
+	IWL_FW_RESET,
+	IWL_MAX_FORCE_RESET,
+};
+
+struct iwl_force_reset {
+	int reset_request_count;
+	int reset_success_count;
+	int reset_reject_count;
+	unsigned long reset_duration;
+	unsigned long last_force_reset_jiffies;
+};
+
+/* extend beacon time format bit shifting  */
+/*
+ * for _3945 devices
+ * bits 31:24 - extended
+ * bits 23:0  - interval
+ */
+#define IWL3945_EXT_BEACON_TIME_POS	24
+/*
+ * for _4965 devices
+ * bits 31:22 - extended
+ * bits 21:0  - interval
+ */
+#define IWL4965_EXT_BEACON_TIME_POS	22
+
+enum iwl_rxon_context_id {
+	IWL_RXON_CTX_BSS,
+
+	NUM_IWL_RXON_CTX
+};
+
+struct iwl_rxon_context {
+	struct ieee80211_vif *vif;
+
+	const u8 *ac_to_fifo;
+	const u8 *ac_to_queue;
+	u8 mcast_queue;
+
+	/*
+	 * We could use the vif to indicate active, but we
+	 * also need it to be active during disabling when
+	 * we already removed the vif for type setting.
+	 */
+	bool always_active, is_active;
+
+	bool ht_need_multiple_chains;
+
+	enum iwl_rxon_context_id ctxid;
+
+	u32 interface_modes, exclusive_interface_modes;
+	u8 unused_devtype, ap_devtype, ibss_devtype, station_devtype;
+
+	/*
+	 * We declare this const so it can only be
+	 * changed via explicit cast within the
+	 * routines that actually update the physical
+	 * hardware.
+	 */
+	const struct iwl_legacy_rxon_cmd active;
+	struct iwl_legacy_rxon_cmd staging;
+
+	struct iwl_rxon_time_cmd timing;
+
+	struct iwl_qos_info qos_data;
+
+	u8 bcast_sta_id, ap_sta_id;
+
+	u8 rxon_cmd, rxon_assoc_cmd, rxon_timing_cmd;
+	u8 qos_cmd;
+	u8 wep_key_cmd;
+
+	struct iwl_wep_key wep_keys[WEP_KEYS_MAX];
+	u8 key_mapping_keys;
+
+	__le32 station_flags;
+
+	struct {
+		bool non_gf_sta_present;
+		u8 protection;
+		bool enabled, is_40mhz;
+		u8 extension_chan_offset;
+	} ht;
+};
+
+struct iwl_priv {
+
+	/* ieee device used by generic ieee processing code */
+	struct ieee80211_hw *hw;
+	struct ieee80211_channel *ieee_channels;
+	struct ieee80211_rate *ieee_rates;
+	struct iwl_cfg *cfg;
+
+	/* temporary frame storage list */
+	struct list_head free_frames;
+	int frames_count;
+
+	enum ieee80211_band band;
+	int alloc_rxb_page;
+
+	void (*rx_handlers[REPLY_MAX])(struct iwl_priv *priv,
+				       struct iwl_rx_mem_buffer *rxb);
+
+	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
+
+	/* spectrum measurement report caching */
+	struct iwl_spectrum_notification measure_report;
+	u8 measurement_status;
+
+	/* ucode beacon time */
+	u32 ucode_beacon_time;
+	int missed_beacon_threshold;
+
+	/* track IBSS manager (last beacon) status */
+	u32 ibss_manager;
+
+	/* storing the jiffies when the plcp error rate is received */
+	unsigned long plcp_jiffies;
+
+	/* force reset */
+	struct iwl_force_reset force_reset[IWL_MAX_FORCE_RESET];
+
+	/* we allocate array of iwl_channel_info for NIC's valid channels.
+	 *    Access via channel # using indirect index array */
+	struct iwl_channel_info *channel_info;	/* channel info array */
+	u8 channel_count;	/* # of channels */
+
+	/* thermal calibration */
+	s32 temperature;	/* degrees Kelvin */
+	s32 last_temperature;
+
+	/* init calibration results */
+	struct iwl_calib_result calib_results[IWL_CALIB_MAX];
+
+	/* Scan related variables */
+	unsigned long scan_start;
+	unsigned long scan_start_tsf;
+	void *scan_cmd;
+	enum ieee80211_band scan_band;
+	struct cfg80211_scan_request *scan_request;
+	struct ieee80211_vif *scan_vif;
+	bool is_internal_short_scan;
+	u8 scan_tx_ant[IEEE80211_NUM_BANDS];
+	u8 mgmt_tx_ant;
+
+	/* spinlock */
+	spinlock_t lock;	/* protect general shared data */
+	spinlock_t hcmd_lock;	/* protect hcmd */
+	spinlock_t reg_lock;	/* protect hw register access */
+	struct mutex mutex;
+	struct mutex sync_cmd_mutex; /* enable serialization of sync commands */
+
+	/* basic pci-network driver stuff */
+	struct pci_dev *pci_dev;
+
+	/* pci hardware address support */
+	void __iomem *hw_base;
+	u32  hw_rev;
+	u32  hw_wa_rev;
+	u8   rev_id;
+
+	/* microcode/device supports multiple contexts */
+	u8 valid_contexts;
+
+	/* command queue number */
+	u8 cmd_queue;
+
+	/* max number of station keys */
+	u8 sta_key_max_num;
+
+	/* EEPROM MAC addresses */
+	struct mac_address addresses[1];
+
+	/* uCode images, save to reload in case of failure */
+	int fw_index;			/* firmware we're trying to load */
+	u32 ucode_ver;			/* version of ucode, copy of
+					   iwl_ucode.ver */
+	struct fw_desc ucode_code;	/* runtime inst */
+	struct fw_desc ucode_data;	/* runtime data original */
+	struct fw_desc ucode_data_backup;	/* runtime data save/restore */
+	struct fw_desc ucode_init;	/* initialization inst */
+	struct fw_desc ucode_init_data;	/* initialization data */
+	struct fw_desc ucode_boot;	/* bootstrap inst */
+	enum ucode_type ucode_type;
+	u8 ucode_write_complete;	/* the image write is complete */
+	char firmware_name[25];
+
+	struct iwl_rxon_context contexts[NUM_IWL_RXON_CTX];
+
+	struct iwl_switch_rxon switch_rxon;
+
+	/* 1st responses from initialize and runtime uCode images.
+	 * _4965's initialize alive response contains some calibration data. */
+	struct iwl_init_alive_resp card_alive_init;
+	struct iwl_alive_resp card_alive;
+
+	u16 active_rate;
+
+	u8 start_calib;
+	struct iwl_sensitivity_data sensitivity_data;
+	struct iwl_chain_noise_data chain_noise_data;
+	__le16 sensitivity_tbl[HD_TABLE_SIZE];
+
+	struct iwl_ht_config current_ht_config;
+
+	/* Rate scaling data */
+	u8 retry_rate;
+
+	wait_queue_head_t wait_command_queue;
+
+	int activity_timer_active;
+
+	/* Rx and Tx DMA processing queues */
+	struct iwl_rx_queue rxq;
+	struct iwl_tx_queue *txq;
+	unsigned long txq_ctx_active_msk;
+	struct iwl_dma_ptr  kw;	/* keep warm address */
+	struct iwl_dma_ptr  scd_bc_tbls;
+
+	u32 scd_base_addr;	/* scheduler sram base address */
+
+	unsigned long status;
+
+	/* counts mgmt, ctl, and data packets */
+	struct traffic_stats tx_stats;
+	struct traffic_stats rx_stats;
+
+	/* counts interrupts */
+	struct isr_statistics isr_stats;
+
+	struct iwl_power_mgr power_data;
+
+	/* context information */
+	u8 bssid[ETH_ALEN]; /* used only on 3945 but filled by core */
+
+	/* station table variables */
+
+	/* Note: if lock and sta_lock are needed, lock must be acquired first */
+	spinlock_t sta_lock;
+	int num_stations;
+	struct iwl_station_entry stations[IWL_STATION_COUNT];
+	unsigned long ucode_key_table;
+
+	/* queue refcounts */
+#define IWL_MAX_HW_QUEUES	32
+	unsigned long queue_stopped[BITS_TO_LONGS(IWL_MAX_HW_QUEUES)];
+	/* for each AC */
+	atomic_t queue_stop_count[4];
+
+	/* Indication if ieee80211_ops->open has been called */
+	u8 is_open;
+
+	u8 mac80211_registered;
+
+	/* eeprom -- this is in the card's little endian byte order */
+	u8 *eeprom;
+	struct iwl_eeprom_calib_info *calib_info;
+
+	enum nl80211_iftype iw_mode;
+
+	/* Last Rx'd beacon timestamp */
+	u64 timestamp;
+
+	union {
+#if defined(CONFIG_IWL3945) || defined(CONFIG_IWL3945_MODULE)
+		struct {
+			void *shared_virt;
+			dma_addr_t shared_phys;
+
+			struct delayed_work thermal_periodic;
+			struct delayed_work rfkill_poll;
+
+			struct iwl3945_notif_statistics statistics;
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+			struct iwl3945_notif_statistics accum_statistics;
+			struct iwl3945_notif_statistics delta_statistics;
+			struct iwl3945_notif_statistics max_delta;
+#endif
+
+			u32 sta_supp_rates;
+			int last_rx_rssi;	/* From Rx packet statistics */
+
+			/* Rx'd packet timing information */
+			u32 last_beacon_time;
+			u64 last_tsf;
+
+			/*
+			 * each calibration channel group in the
+			 * EEPROM has a derived clip setting for
+			 * each rate.
+			 */
+			const struct iwl3945_clip_group clip_groups[5];
+
+		} _3945;
+#endif
+#if defined(CONFIG_IWL4965) || defined(CONFIG_IWL4965_MODULE)
+		struct {
+			/*
+			 * reporting the number of tids has AGG on. 0 means
+			 * no AGGREGATION
+			 */
+			u8 agg_tids_count;
+
+			struct iwl_rx_phy_res last_phy_res;
+			bool last_phy_res_valid;
+
+			struct completion firmware_loading_complete;
+
+			/*
+			 * chain noise reset and gain commands are the
+			 * two extra calibration commands follows the standard
+			 * phy calibration commands
+			 */
+			u8 phy_calib_chain_noise_reset_cmd;
+			u8 phy_calib_chain_noise_gain_cmd;
+
+			struct iwl_notif_statistics statistics;
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+			struct iwl_notif_statistics accum_statistics;
+			struct iwl_notif_statistics delta_statistics;
+			struct iwl_notif_statistics max_delta;
+#endif
+
+		} _4965;
+#endif
+	};
+
+	struct iwl_hw_params hw_params;
+
+	u32 inta_mask;
+
+	struct workqueue_struct *workqueue;
+
+	struct work_struct restart;
+	struct work_struct scan_completed;
+	struct work_struct rx_replenish;
+	struct work_struct abort_scan;
+
+	struct iwl_rxon_context *beacon_ctx;
+	struct sk_buff *beacon_skb;
+
+	struct work_struct start_internal_scan;
+	struct work_struct tx_flush;
+
+	struct tasklet_struct irq_tasklet;
+
+	struct delayed_work init_alive_start;
+	struct delayed_work alive_start;
+	struct delayed_work scan_check;
+
+	/* TX Power */
+	s8 tx_power_user_lmt;
+	s8 tx_power_device_lmt;
+	s8 tx_power_next;
+
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	/* debugging info */
+	u32 debug_level; /* per device debugging will override global
+			    iwlegacy_debug_level if set */
+#endif /* CONFIG_IWLWIFI_LEGACY_DEBUG */
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUGFS
+	/* debugfs */
+	u16 tx_traffic_idx;
+	u16 rx_traffic_idx;
+	u8 *tx_traffic;
+	u8 *rx_traffic;
+	struct dentry *debugfs_dir;
+	u32 dbgfs_sram_offset, dbgfs_sram_len;
+	bool disable_ht40;
+#endif /* CONFIG_IWLWIFI_LEGACY_DEBUGFS */
+
+	struct work_struct txpower_work;
+	u32 disable_sens_cal;
+	u32 disable_chain_noise_cal;
+	u32 disable_tx_power_cal;
+	struct work_struct run_time_calib_work;
+	struct timer_list statistics_periodic;
+	struct timer_list ucode_trace;
+	struct timer_list watchdog;
+	bool hw_ready;
+
+	struct iwl_event_log event_log;
+
+	struct led_classdev led;
+	unsigned long blink_on, blink_off;
+	bool led_registered;
+}; /*iwl_priv */
+
+static inline void iwl_txq_ctx_activate(struct iwl_priv *priv, int txq_id)
+{
+	set_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+static inline void iwl_txq_ctx_deactivate(struct iwl_priv *priv, int txq_id)
+{
+	clear_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+/*
+ * iwl_legacy_get_debug_level: Return active debug level for device
+ *
+ * Using sysfs it is possible to set per device debug level. This debug
+ * level will be used if set, otherwise the global debug level which can be
+ * set via module parameter is used.
+ */
+static inline u32 iwl_legacy_get_debug_level(struct iwl_priv *priv)
+{
+	if (priv->debug_level)
+		return priv->debug_level;
+	else
+		return iwlegacy_debug_level;
+}
+#else
+static inline u32 iwl_legacy_get_debug_level(struct iwl_priv *priv)
+{
+	return iwlegacy_debug_level;
+}
+#endif
+
+
+static inline struct ieee80211_hdr *
+iwl_legacy_tx_queue_get_hdr(struct iwl_priv *priv,
+						 int txq_id, int idx)
+{
+	if (priv->txq[txq_id].txb[idx].skb)
+		return (struct ieee80211_hdr *)priv->txq[txq_id].
+				txb[idx].skb->data;
+	return NULL;
+}
+
+static inline struct iwl_rxon_context *
+iwl_legacy_rxon_ctx_from_vif(struct ieee80211_vif *vif)
+{
+	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
+
+	return vif_priv->ctx;
+}
+
+#define for_each_context(priv, ctx)				\
+	for (ctx = &priv->contexts[IWL_RXON_CTX_BSS];		\
+	     ctx < &priv->contexts[NUM_IWL_RXON_CTX]; ctx++)	\
+		if (priv->valid_contexts & BIT(ctx->ctxid))
+
+static inline int iwl_legacy_is_associated(struct iwl_priv *priv,
+				    enum iwl_rxon_context_id ctxid)
+{
+	return (priv->contexts[ctxid].active.filter_flags &
+			RXON_FILTER_ASSOC_MSK) ? 1 : 0;
+}
+
+static inline int iwl_legacy_is_any_associated(struct iwl_priv *priv)
+{
+	return iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS);
+}
+
+static inline int iwl_legacy_is_associated_ctx(struct iwl_rxon_context *ctx)
+{
+	return (ctx->active.filter_flags & RXON_FILTER_ASSOC_MSK) ? 1 : 0;
+}
+
+static inline int iwl_legacy_is_channel_valid(const struct iwl_channel_info *ch_info)
+{
+	if (ch_info == NULL)
+		return 0;
+	return (ch_info->flags & EEPROM_CHANNEL_VALID) ? 1 : 0;
+}
+
+static inline int iwl_legacy_is_channel_radar(const struct iwl_channel_info *ch_info)
+{
+	return (ch_info->flags & EEPROM_CHANNEL_RADAR) ? 1 : 0;
+}
+
+static inline u8 iwl_legacy_is_channel_a_band(const struct iwl_channel_info *ch_info)
+{
+	return ch_info->band == IEEE80211_BAND_5GHZ;
+}
+
+static inline int
+iwl_legacy_is_channel_passive(const struct iwl_channel_info *ch)
+{
+	return (!(ch->flags & EEPROM_CHANNEL_ACTIVE)) ? 1 : 0;
+}
+
+static inline void
+__iwl_legacy_free_pages(struct iwl_priv *priv, struct page *page)
+{
+	__free_pages(page, priv->hw_params.rx_page_order);
+	priv->alloc_rxb_page--;
+}
+
+static inline void iwl_legacy_free_pages(struct iwl_priv *priv, unsigned long page)
+{
+	free_pages(page, priv->hw_params.rx_page_order);
+	priv->alloc_rxb_page--;
+}
+#endif				/* __iwl_legacy_dev_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-devtrace.c b/drivers/net/wireless/iwlegacy/iwl-devtrace.c
new file mode 100644
index 000000000000..080b852b33bd
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-devtrace.c
@@ -0,0 +1,45 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/module.h>
+
+/* sparse doesn't like tracepoint macros */
+#ifndef __CHECKER__
+#include "iwl-dev.h"
+
+#define CREATE_TRACE_POINTS
+#include "iwl-devtrace.h"
+
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_iowrite8);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_ioread32);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_iowrite32);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_rx);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_tx);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_ucode_event);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_ucode_error);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_ucode_cont_event);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_legacy_dev_ucode_wrap_event);
+#endif
diff --git a/drivers/net/wireless/iwlegacy/iwl-devtrace.h b/drivers/net/wireless/iwlegacy/iwl-devtrace.h
new file mode 100644
index 000000000000..9612aa0f6ec4
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-devtrace.h
@@ -0,0 +1,270 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#if !defined(__IWLWIFI_LEGACY_DEVICE_TRACE) || defined(TRACE_HEADER_MULTI_READ)
+#define __IWLWIFI_LEGACY_DEVICE_TRACE
+
+#include <linux/tracepoint.h>
+
+#if !defined(CONFIG_IWLWIFI_LEGACY_DEVICE_TRACING) || defined(__CHECKER__)
+#undef TRACE_EVENT
+#define TRACE_EVENT(name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+#endif
+
+
+#define PRIV_ENTRY	__field(struct iwl_priv *, priv)
+#define PRIV_ASSIGN	(__entry->priv = priv)
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_legacy_io
+
+TRACE_EVENT(iwlwifi_legacy_dev_ioread32,
+	TP_PROTO(struct iwl_priv *priv, u32 offs, u32 val),
+	TP_ARGS(priv, offs, val),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(u32, offs)
+		__field(u32, val)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%p] read io[%#x] = %#x", __entry->priv,
+					__entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_iowrite8,
+	TP_PROTO(struct iwl_priv *priv, u32 offs, u8 val),
+	TP_ARGS(priv, offs, val),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(u32, offs)
+		__field(u8, val)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%p] write io[%#x] = %#x)", __entry->priv,
+					__entry->offs, __entry->val)
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_iowrite32,
+	TP_PROTO(struct iwl_priv *priv, u32 offs, u32 val),
+	TP_ARGS(priv, offs, val),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(u32, offs)
+		__field(u32, val)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->offs = offs;
+		__entry->val = val;
+	),
+	TP_printk("[%p] write io[%#x] = %#x)", __entry->priv,
+					__entry->offs, __entry->val)
+);
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_legacy_ucode
+
+TRACE_EVENT(iwlwifi_legacy_dev_ucode_cont_event,
+	TP_PROTO(struct iwl_priv *priv, u32 time, u32 data, u32 ev),
+	TP_ARGS(priv, time, data, ev),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+
+		__field(u32, time)
+		__field(u32, data)
+		__field(u32, ev)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->time = time;
+		__entry->data = data;
+		__entry->ev = ev;
+	),
+	TP_printk("[%p] EVT_LOGT:%010u:0x%08x:%04u",
+		  __entry->priv, __entry->time, __entry->data, __entry->ev)
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_ucode_wrap_event,
+	TP_PROTO(struct iwl_priv *priv, u32 wraps, u32 n_entry, u32 p_entry),
+	TP_ARGS(priv, wraps, n_entry, p_entry),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+
+		__field(u32, wraps)
+		__field(u32, n_entry)
+		__field(u32, p_entry)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->wraps = wraps;
+		__entry->n_entry = n_entry;
+		__entry->p_entry = p_entry;
+	),
+	TP_printk("[%p] wraps=#%02d n=0x%X p=0x%X",
+		  __entry->priv, __entry->wraps, __entry->n_entry,
+		  __entry->p_entry)
+);
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi
+
+TRACE_EVENT(iwlwifi_legacy_dev_hcmd,
+	TP_PROTO(struct iwl_priv *priv, void *hcmd, size_t len, u32 flags),
+	TP_ARGS(priv, hcmd, len, flags),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__dynamic_array(u8, hcmd, len)
+		__field(u32, flags)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		memcpy(__get_dynamic_array(hcmd), hcmd, len);
+		__entry->flags = flags;
+	),
+	TP_printk("[%p] hcmd %#.2x (%ssync)",
+		  __entry->priv, ((u8 *)__get_dynamic_array(hcmd))[0],
+		  __entry->flags & CMD_ASYNC ? "a" : "")
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_rx,
+	TP_PROTO(struct iwl_priv *priv, void *rxbuf, size_t len),
+	TP_ARGS(priv, rxbuf, len),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__dynamic_array(u8, rxbuf, len)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		memcpy(__get_dynamic_array(rxbuf), rxbuf, len);
+	),
+	TP_printk("[%p] RX cmd %#.2x",
+		  __entry->priv, ((u8 *)__get_dynamic_array(rxbuf))[4])
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_tx,
+	TP_PROTO(struct iwl_priv *priv, void *tfd, size_t tfdlen,
+		 void *buf0, size_t buf0_len,
+		 void *buf1, size_t buf1_len),
+	TP_ARGS(priv, tfd, tfdlen, buf0, buf0_len, buf1, buf1_len),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+
+		__field(size_t, framelen)
+		__dynamic_array(u8, tfd, tfdlen)
+
+		/*
+		 * Do not insert between or below these items,
+		 * we want to keep the frame together (except
+		 * for the possible padding).
+		 */
+		__dynamic_array(u8, buf0, buf0_len)
+		__dynamic_array(u8, buf1, buf1_len)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->framelen = buf0_len + buf1_len;
+		memcpy(__get_dynamic_array(tfd), tfd, tfdlen);
+		memcpy(__get_dynamic_array(buf0), buf0, buf0_len);
+		memcpy(__get_dynamic_array(buf1), buf1, buf1_len);
+	),
+	TP_printk("[%p] TX %.2x (%zu bytes)",
+		  __entry->priv,
+		  ((u8 *)__get_dynamic_array(buf0))[0],
+		  __entry->framelen)
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_ucode_error,
+	TP_PROTO(struct iwl_priv *priv, u32 desc, u32 time,
+		 u32 data1, u32 data2, u32 line, u32 blink1,
+		 u32 blink2, u32 ilink1, u32 ilink2),
+	TP_ARGS(priv, desc, time, data1, data2, line,
+		blink1, blink2, ilink1, ilink2),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+		__field(u32, desc)
+		__field(u32, time)
+		__field(u32, data1)
+		__field(u32, data2)
+		__field(u32, line)
+		__field(u32, blink1)
+		__field(u32, blink2)
+		__field(u32, ilink1)
+		__field(u32, ilink2)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->desc = desc;
+		__entry->time = time;
+		__entry->data1 = data1;
+		__entry->data2 = data2;
+		__entry->line = line;
+		__entry->blink1 = blink1;
+		__entry->blink2 = blink2;
+		__entry->ilink1 = ilink1;
+		__entry->ilink2 = ilink2;
+	),
+	TP_printk("[%p] #%02d %010u data 0x%08X 0x%08X line %u, "
+		  "blink 0x%05X 0x%05X ilink 0x%05X 0x%05X",
+		  __entry->priv, __entry->desc, __entry->time, __entry->data1,
+		  __entry->data2, __entry->line, __entry->blink1,
+		  __entry->blink2, __entry->ilink1, __entry->ilink2)
+);
+
+TRACE_EVENT(iwlwifi_legacy_dev_ucode_event,
+	TP_PROTO(struct iwl_priv *priv, u32 time, u32 data, u32 ev),
+	TP_ARGS(priv, time, data, ev),
+	TP_STRUCT__entry(
+		PRIV_ENTRY
+
+		__field(u32, time)
+		__field(u32, data)
+		__field(u32, ev)
+	),
+	TP_fast_assign(
+		PRIV_ASSIGN;
+		__entry->time = time;
+		__entry->data = data;
+		__entry->ev = ev;
+	),
+	TP_printk("[%p] EVT_LOGT:%010u:0x%08x:%04u",
+		  __entry->priv, __entry->time, __entry->data, __entry->ev)
+);
+#endif /* __IWLWIFI_DEVICE_TRACE */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE iwl-devtrace
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwlegacy/iwl-eeprom.c b/drivers/net/wireless/iwlegacy/iwl-eeprom.c
new file mode 100644
index 000000000000..04c5648027df
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-eeprom.c
@@ -0,0 +1,561 @@
+/******************************************************************************
+ *
+ * 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 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+
+#include <net/mac80211.h>
+
+#include "iwl-commands.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-debug.h"
+#include "iwl-eeprom.h"
+#include "iwl-io.h"
+
+/************************** EEPROM BANDS ****************************
+ *
+ * The iwlegacy_eeprom_band definitions below provide the mapping from the
+ * EEPROM contents to the specific channel number supported for each
+ * band.
+ *
+ * For example, iwl_priv->eeprom.band_3_channels[4] from the band_3
+ * definition below maps to physical channel 42 in the 5.2GHz spectrum.
+ * The specific geography and calibration information for that channel
+ * is contained in the eeprom map itself.
+ *
+ * During init, we copy the eeprom information and channel map
+ * information into priv->channel_info_24/52 and priv->channel_map_24/52
+ *
+ * channel_map_24/52 provides the index in the channel_info array for a
+ * given channel.  We have to have two separate maps as there is channel
+ * overlap with the 2.4GHz and 5.2GHz spectrum as seen in band_1 and
+ * band_2
+ *
+ * A value of 0xff stored in the channel_map indicates that the channel
+ * is not supported by the hardware at all.
+ *
+ * A value of 0xfe in the channel_map indicates that the channel is not
+ * valid for Tx with the current hardware.  This means that
+ * while the system can tune and receive on a given channel, it may not
+ * be able to associate or transmit any frames on that
+ * channel.  There is no corresponding channel information for that
+ * entry.
+ *
+ *********************************************************************/
+
+/* 2.4 GHz */
+const u8 iwlegacy_eeprom_band_1[14] = {
+	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
+};
+
+/* 5.2 GHz bands */
+static const u8 iwlegacy_eeprom_band_2[] = {	/* 4915-5080MHz */
+	183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
+};
+
+static const u8 iwlegacy_eeprom_band_3[] = {	/* 5170-5320MHz */
+	34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
+};
+
+static const u8 iwlegacy_eeprom_band_4[] = {	/* 5500-5700MHz */
+	100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+};
+
+static const u8 iwlegacy_eeprom_band_5[] = {	/* 5725-5825MHz */
+	145, 149, 153, 157, 161, 165
+};
+
+static const u8 iwlegacy_eeprom_band_6[] = {       /* 2.4 ht40 channel */
+	1, 2, 3, 4, 5, 6, 7
+};
+
+static const u8 iwlegacy_eeprom_band_7[] = {       /* 5.2 ht40 channel */
+	36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157
+};
+
+/******************************************************************************
+ *
+ * EEPROM related functions
+ *
+******************************************************************************/
+
+static int iwl_legacy_eeprom_verify_signature(struct iwl_priv *priv)
+{
+	u32 gp = iwl_read32(priv, CSR_EEPROM_GP) & CSR_EEPROM_GP_VALID_MSK;
+	int ret = 0;
+
+	IWL_DEBUG_EEPROM(priv, "EEPROM signature=0x%08x\n", gp);
+	switch (gp) {
+	case CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K:
+	case CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K:
+		break;
+	default:
+		IWL_ERR(priv, "bad EEPROM signature,"
+			"EEPROM_GP=0x%08x\n", gp);
+		ret = -ENOENT;
+		break;
+	}
+	return ret;
+}
+
+const u8
+*iwl_legacy_eeprom_query_addr(const struct iwl_priv *priv, size_t offset)
+{
+	BUG_ON(offset >= priv->cfg->base_params->eeprom_size);
+	return &priv->eeprom[offset];
+}
+EXPORT_SYMBOL(iwl_legacy_eeprom_query_addr);
+
+u16 iwl_legacy_eeprom_query16(const struct iwl_priv *priv, size_t offset)
+{
+	if (!priv->eeprom)
+		return 0;
+	return (u16)priv->eeprom[offset] | ((u16)priv->eeprom[offset + 1] << 8);
+}
+EXPORT_SYMBOL(iwl_legacy_eeprom_query16);
+
+/**
+ * iwl_legacy_eeprom_init - read EEPROM contents
+ *
+ * Load the EEPROM contents from adapter into priv->eeprom
+ *
+ * NOTE:  This routine uses the non-debug IO access functions.
+ */
+int iwl_legacy_eeprom_init(struct iwl_priv *priv)
+{
+	__le16 *e;
+	u32 gp = iwl_read32(priv, CSR_EEPROM_GP);
+	int sz;
+	int ret;
+	u16 addr;
+
+	/* allocate eeprom */
+	sz = priv->cfg->base_params->eeprom_size;
+	IWL_DEBUG_EEPROM(priv, "NVM size = %d\n", sz);
+	priv->eeprom = kzalloc(sz, GFP_KERNEL);
+	if (!priv->eeprom) {
+		ret = -ENOMEM;
+		goto alloc_err;
+	}
+	e = (__le16 *)priv->eeprom;
+
+	priv->cfg->ops->lib->apm_ops.init(priv);
+
+	ret = iwl_legacy_eeprom_verify_signature(priv);
+	if (ret < 0) {
+		IWL_ERR(priv, "EEPROM not found, EEPROM_GP=0x%08x\n", gp);
+		ret = -ENOENT;
+		goto err;
+	}
+
+	/* Make sure driver (instead of uCode) is allowed to read EEPROM */
+	ret = priv->cfg->ops->lib->eeprom_ops.acquire_semaphore(priv);
+	if (ret < 0) {
+		IWL_ERR(priv, "Failed to acquire EEPROM semaphore.\n");
+		ret = -ENOENT;
+		goto err;
+	}
+
+	/* eeprom is an array of 16bit values */
+	for (addr = 0; addr < sz; addr += sizeof(u16)) {
+		u32 r;
+
+		_iwl_legacy_write32(priv, CSR_EEPROM_REG,
+			     CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
+
+		ret = iwl_poll_bit(priv, CSR_EEPROM_REG,
+					  CSR_EEPROM_REG_READ_VALID_MSK,
+					  CSR_EEPROM_REG_READ_VALID_MSK,
+					  IWL_EEPROM_ACCESS_TIMEOUT);
+		if (ret < 0) {
+			IWL_ERR(priv, "Time out reading EEPROM[%d]\n",
+							addr);
+			goto done;
+		}
+		r = _iwl_legacy_read_direct32(priv, CSR_EEPROM_REG);
+		e[addr / 2] = cpu_to_le16(r >> 16);
+	}
+
+	IWL_DEBUG_EEPROM(priv, "NVM Type: %s, version: 0x%x\n",
+		       "EEPROM",
+		       iwl_legacy_eeprom_query16(priv, EEPROM_VERSION));
+
+	ret = 0;
+done:
+	priv->cfg->ops->lib->eeprom_ops.release_semaphore(priv);
+
+err:
+	if (ret)
+		iwl_legacy_eeprom_free(priv);
+	/* Reset chip to save power until we load uCode during "up". */
+	iwl_legacy_apm_stop(priv);
+alloc_err:
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_eeprom_init);
+
+void iwl_legacy_eeprom_free(struct iwl_priv *priv)
+{
+	kfree(priv->eeprom);
+	priv->eeprom = NULL;
+}
+EXPORT_SYMBOL(iwl_legacy_eeprom_free);
+
+static void iwl_legacy_init_band_reference(const struct iwl_priv *priv,
+			int eep_band, int *eeprom_ch_count,
+			const struct iwl_eeprom_channel **eeprom_ch_info,
+			const u8 **eeprom_ch_index)
+{
+	u32 offset = priv->cfg->ops->lib->
+			eeprom_ops.regulatory_bands[eep_band - 1];
+	switch (eep_band) {
+	case 1:		/* 2.4GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_1);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_1;
+		break;
+	case 2:		/* 4.9GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_2);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_2;
+		break;
+	case 3:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_3);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_3;
+		break;
+	case 4:		/* 5.5GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_4);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_4;
+		break;
+	case 5:		/* 5.7GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_5);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_5;
+		break;
+	case 6:		/* 2.4GHz ht40 channels */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_6);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_6;
+		break;
+	case 7:		/* 5 GHz ht40 channels */
+		*eeprom_ch_count = ARRAY_SIZE(iwlegacy_eeprom_band_7);
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_legacy_eeprom_query_addr(priv, offset);
+		*eeprom_ch_index = iwlegacy_eeprom_band_7;
+		break;
+	default:
+		BUG();
+		return;
+	}
+}
+
+#define CHECK_AND_PRINT(x) ((eeprom_ch->flags & EEPROM_CHANNEL_##x) \
+			    ? # x " " : "")
+/**
+ * iwl_legacy_mod_ht40_chan_info - Copy ht40 channel info into driver's priv.
+ *
+ * Does not set up a command, or touch hardware.
+ */
+static int iwl_legacy_mod_ht40_chan_info(struct iwl_priv *priv,
+			      enum ieee80211_band band, u16 channel,
+			      const struct iwl_eeprom_channel *eeprom_ch,
+			      u8 clear_ht40_extension_channel)
+{
+	struct iwl_channel_info *ch_info;
+
+	ch_info = (struct iwl_channel_info *)
+			iwl_legacy_get_channel_info(priv, band, channel);
+
+	if (!iwl_legacy_is_channel_valid(ch_info))
+		return -1;
+
+	IWL_DEBUG_EEPROM(priv, "HT40 Ch. %d [%sGHz] %s%s%s%s%s(0x%02x %ddBm):"
+			" Ad-Hoc %ssupported\n",
+			ch_info->channel,
+			iwl_legacy_is_channel_a_band(ch_info) ?
+			"5.2" : "2.4",
+			CHECK_AND_PRINT(IBSS),
+			CHECK_AND_PRINT(ACTIVE),
+			CHECK_AND_PRINT(RADAR),
+			CHECK_AND_PRINT(WIDE),
+			CHECK_AND_PRINT(DFS),
+			eeprom_ch->flags,
+			eeprom_ch->max_power_avg,
+			((eeprom_ch->flags & EEPROM_CHANNEL_IBSS)
+			 && !(eeprom_ch->flags & EEPROM_CHANNEL_RADAR)) ?
+			"" : "not ");
+
+	ch_info->ht40_eeprom = *eeprom_ch;
+	ch_info->ht40_max_power_avg = eeprom_ch->max_power_avg;
+	ch_info->ht40_flags = eeprom_ch->flags;
+	if (eeprom_ch->flags & EEPROM_CHANNEL_VALID)
+		ch_info->ht40_extension_channel &=
+					~clear_ht40_extension_channel;
+
+	return 0;
+}
+
+#define CHECK_AND_PRINT_I(x) ((eeprom_ch_info[ch].flags & EEPROM_CHANNEL_##x) \
+			    ? # x " " : "")
+
+/**
+ * iwl_legacy_init_channel_map - Set up driver's info for all possible channels
+ */
+int iwl_legacy_init_channel_map(struct iwl_priv *priv)
+{
+	int eeprom_ch_count = 0;
+	const u8 *eeprom_ch_index = NULL;
+	const struct iwl_eeprom_channel *eeprom_ch_info = NULL;
+	int band, ch;
+	struct iwl_channel_info *ch_info;
+
+	if (priv->channel_count) {
+		IWL_DEBUG_EEPROM(priv, "Channel map already initialized.\n");
+		return 0;
+	}
+
+	IWL_DEBUG_EEPROM(priv, "Initializing regulatory info from EEPROM\n");
+
+	priv->channel_count =
+	    ARRAY_SIZE(iwlegacy_eeprom_band_1) +
+	    ARRAY_SIZE(iwlegacy_eeprom_band_2) +
+	    ARRAY_SIZE(iwlegacy_eeprom_band_3) +
+	    ARRAY_SIZE(iwlegacy_eeprom_band_4) +
+	    ARRAY_SIZE(iwlegacy_eeprom_band_5);
+
+	IWL_DEBUG_EEPROM(priv, "Parsing data for %d channels.\n",
+			priv->channel_count);
+
+	priv->channel_info = kzalloc(sizeof(struct iwl_channel_info) *
+				     priv->channel_count, GFP_KERNEL);
+	if (!priv->channel_info) {
+		IWL_ERR(priv, "Could not allocate channel_info\n");
+		priv->channel_count = 0;
+		return -ENOMEM;
+	}
+
+	ch_info = priv->channel_info;
+
+	/* Loop through the 5 EEPROM bands adding them in order to the
+	 * channel map we maintain (that contains additional information than
+	 * what just in the EEPROM) */
+	for (band = 1; band <= 5; band++) {
+
+		iwl_legacy_init_band_reference(priv, band, &eeprom_ch_count,
+					&eeprom_ch_info, &eeprom_ch_index);
+
+		/* Loop through each band adding each of the channels */
+		for (ch = 0; ch < eeprom_ch_count; ch++) {
+			ch_info->channel = eeprom_ch_index[ch];
+			ch_info->band = (band == 1) ? IEEE80211_BAND_2GHZ :
+			    IEEE80211_BAND_5GHZ;
+
+			/* permanently store EEPROM's channel regulatory flags
+			 *   and max power in channel info database. */
+			ch_info->eeprom = eeprom_ch_info[ch];
+
+			/* Copy the run-time flags so they are there even on
+			 * invalid channels */
+			ch_info->flags = eeprom_ch_info[ch].flags;
+			/* First write that ht40 is not enabled, and then enable
+			 * one by one */
+			ch_info->ht40_extension_channel =
+					IEEE80211_CHAN_NO_HT40;
+
+			if (!(iwl_legacy_is_channel_valid(ch_info))) {
+				IWL_DEBUG_EEPROM(priv,
+					       "Ch. %d Flags %x [%sGHz] - "
+					       "No traffic\n",
+					       ch_info->channel,
+					       ch_info->flags,
+					       iwl_legacy_is_channel_a_band(ch_info) ?
+					       "5.2" : "2.4");
+				ch_info++;
+				continue;
+			}
+
+			/* Initialize regulatory-based run-time data */
+			ch_info->max_power_avg = ch_info->curr_txpow =
+			    eeprom_ch_info[ch].max_power_avg;
+			ch_info->scan_power = eeprom_ch_info[ch].max_power_avg;
+			ch_info->min_power = 0;
+
+			IWL_DEBUG_EEPROM(priv, "Ch. %d [%sGHz] "
+				       "%s%s%s%s%s%s(0x%02x %ddBm):"
+				       " Ad-Hoc %ssupported\n",
+				       ch_info->channel,
+				       iwl_legacy_is_channel_a_band(ch_info) ?
+				       "5.2" : "2.4",
+				       CHECK_AND_PRINT_I(VALID),
+				       CHECK_AND_PRINT_I(IBSS),
+				       CHECK_AND_PRINT_I(ACTIVE),
+				       CHECK_AND_PRINT_I(RADAR),
+				       CHECK_AND_PRINT_I(WIDE),
+				       CHECK_AND_PRINT_I(DFS),
+				       eeprom_ch_info[ch].flags,
+				       eeprom_ch_info[ch].max_power_avg,
+				       ((eeprom_ch_info[ch].
+					 flags & EEPROM_CHANNEL_IBSS)
+					&& !(eeprom_ch_info[ch].
+					     flags & EEPROM_CHANNEL_RADAR))
+				       ? "" : "not ");
+
+			/* Set the tx_power_user_lmt to the highest power
+			 * supported by any channel */
+			if (eeprom_ch_info[ch].max_power_avg >
+						priv->tx_power_user_lmt)
+				priv->tx_power_user_lmt =
+				    eeprom_ch_info[ch].max_power_avg;
+
+			ch_info++;
+		}
+	}
+
+	/* Check if we do have HT40 channels */
+	if (priv->cfg->ops->lib->eeprom_ops.regulatory_bands[5] ==
+	    EEPROM_REGULATORY_BAND_NO_HT40 &&
+	    priv->cfg->ops->lib->eeprom_ops.regulatory_bands[6] ==
+	    EEPROM_REGULATORY_BAND_NO_HT40)
+		return 0;
+
+	/* Two additional EEPROM bands for 2.4 and 5 GHz HT40 channels */
+	for (band = 6; band <= 7; band++) {
+		enum ieee80211_band ieeeband;
+
+		iwl_legacy_init_band_reference(priv, band, &eeprom_ch_count,
+					&eeprom_ch_info, &eeprom_ch_index);
+
+		/* EEPROM band 6 is 2.4, band 7 is 5 GHz */
+		ieeeband =
+			(band == 6) ? IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+
+		/* Loop through each band adding each of the channels */
+		for (ch = 0; ch < eeprom_ch_count; ch++) {
+			/* Set up driver's info for lower half */
+			iwl_legacy_mod_ht40_chan_info(priv, ieeeband,
+						eeprom_ch_index[ch],
+						&eeprom_ch_info[ch],
+						IEEE80211_CHAN_NO_HT40PLUS);
+
+			/* Set up driver's info for upper half */
+			iwl_legacy_mod_ht40_chan_info(priv, ieeeband,
+						eeprom_ch_index[ch] + 4,
+						&eeprom_ch_info[ch],
+						IEEE80211_CHAN_NO_HT40MINUS);
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_init_channel_map);
+
+/*
+ * iwl_legacy_free_channel_map - undo allocations in iwl_legacy_init_channel_map
+ */
+void iwl_legacy_free_channel_map(struct iwl_priv *priv)
+{
+	kfree(priv->channel_info);
+	priv->channel_count = 0;
+}
+EXPORT_SYMBOL(iwl_legacy_free_channel_map);
+
+/**
+ * iwl_legacy_get_channel_info - Find driver's private channel info
+ *
+ * Based on band and channel number.
+ */
+const struct
+iwl_channel_info *iwl_legacy_get_channel_info(const struct iwl_priv *priv,
+					enum ieee80211_band band, u16 channel)
+{
+	int i;
+
+	switch (band) {
+	case IEEE80211_BAND_5GHZ:
+		for (i = 14; i < priv->channel_count; i++) {
+			if (priv->channel_info[i].channel == channel)
+				return &priv->channel_info[i];
+		}
+		break;
+	case IEEE80211_BAND_2GHZ:
+		if (channel >= 1 && channel <= 14)
+			return &priv->channel_info[channel - 1];
+		break;
+	default:
+		BUG();
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL(iwl_legacy_get_channel_info);
diff --git a/drivers/net/wireless/iwlegacy/iwl-eeprom.h b/drivers/net/wireless/iwlegacy/iwl-eeprom.h
new file mode 100644
index 000000000000..c59c81002022
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-eeprom.h
@@ -0,0 +1,344 @@
+/******************************************************************************
+ *
+ * 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 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_eeprom_h__
+#define __iwl_legacy_eeprom_h__
+
+#include <net/mac80211.h>
+
+struct iwl_priv;
+
+/*
+ * EEPROM access time values:
+ *
+ * Driver initiates EEPROM read by writing byte address << 1 to CSR_EEPROM_REG.
+ * Driver then polls CSR_EEPROM_REG for CSR_EEPROM_REG_READ_VALID_MSK (0x1).
+ * When polling, wait 10 uSec between polling loops, up to a maximum 5000 uSec.
+ * Driver reads 16-bit value from bits 31-16 of CSR_EEPROM_REG.
+ */
+#define IWL_EEPROM_ACCESS_TIMEOUT	5000 /* uSec */
+
+#define IWL_EEPROM_SEM_TIMEOUT		10   /* microseconds */
+#define IWL_EEPROM_SEM_RETRY_LIMIT	1000 /* number of attempts (not time) */
+
+
+/*
+ * Regulatory channel usage flags in EEPROM struct iwl4965_eeprom_channel.flags.
+ *
+ * IBSS and/or AP operation is allowed *only* on those channels with
+ * (VALID && IBSS && ACTIVE && !RADAR).  This restriction is in place because
+ * RADAR detection is not supported by the 4965 driver, but is a
+ * requirement for establishing a new network for legal operation on channels
+ * requiring RADAR detection or restricting ACTIVE scanning.
+ *
+ * NOTE:  "WIDE" flag does not indicate anything about "HT40" 40 MHz channels.
+ *        It only indicates that 20 MHz channel use is supported; HT40 channel
+ *        usage is indicated by a separate set of regulatory flags for each
+ *        HT40 channel pair.
+ *
+ * NOTE:  Using a channel inappropriately will result in a uCode error!
+ */
+#define IWL_NUM_TX_CALIB_GROUPS 5
+enum {
+	EEPROM_CHANNEL_VALID = (1 << 0),	/* usable for this SKU/geo */
+	EEPROM_CHANNEL_IBSS = (1 << 1),		/* usable as an IBSS channel */
+	/* Bit 2 Reserved */
+	EEPROM_CHANNEL_ACTIVE = (1 << 3),	/* active scanning allowed */
+	EEPROM_CHANNEL_RADAR = (1 << 4),	/* radar detection required */
+	EEPROM_CHANNEL_WIDE = (1 << 5),		/* 20 MHz channel okay */
+	/* Bit 6 Reserved (was Narrow Channel) */
+	EEPROM_CHANNEL_DFS = (1 << 7),	/* dynamic freq selection candidate */
+};
+
+/* SKU Capabilities */
+/* 3945 only */
+#define EEPROM_SKU_CAP_SW_RF_KILL_ENABLE                (1 << 0)
+#define EEPROM_SKU_CAP_HW_RF_KILL_ENABLE                (1 << 1)
+
+/* *regulatory* channel data format in eeprom, one for each channel.
+ * There are separate entries for HT40 (40 MHz) vs. normal (20 MHz) channels. */
+struct iwl_eeprom_channel {
+	u8 flags;		/* EEPROM_CHANNEL_* flags copied from EEPROM */
+	s8 max_power_avg;	/* max power (dBm) on this chnl, limit 31 */
+} __packed;
+
+/* 3945 Specific */
+#define EEPROM_3945_EEPROM_VERSION	(0x2f)
+
+/* 4965 has two radio transmitters (and 3 radio receivers) */
+#define EEPROM_TX_POWER_TX_CHAINS      (2)
+
+/* 4965 has room for up to 8 sets of txpower calibration data */
+#define EEPROM_TX_POWER_BANDS          (8)
+
+/* 4965 factory calibration measures txpower gain settings for
+ * each of 3 target output levels */
+#define EEPROM_TX_POWER_MEASUREMENTS   (3)
+
+/* 4965 Specific */
+/* 4965 driver does not work with txpower calibration version < 5 */
+#define EEPROM_4965_TX_POWER_VERSION    (5)
+#define EEPROM_4965_EEPROM_VERSION	(0x2f)
+#define EEPROM_4965_CALIB_VERSION_OFFSET       (2*0xB6) /* 2 bytes */
+#define EEPROM_4965_CALIB_TXPOWER_OFFSET       (2*0xE8) /* 48  bytes */
+#define EEPROM_4965_BOARD_REVISION             (2*0x4F) /* 2 bytes */
+#define EEPROM_4965_BOARD_PBA                  (2*0x56+1) /* 9 bytes */
+
+/* 2.4 GHz */
+extern const u8 iwlegacy_eeprom_band_1[14];
+
+/*
+ * factory calibration data for one txpower level, on one channel,
+ * measured on one of the 2 tx chains (radio transmitter and associated
+ * antenna).  EEPROM contains:
+ *
+ * 1)  Temperature (degrees Celsius) of device when measurement was made.
+ *
+ * 2)  Gain table index used to achieve the target measurement power.
+ *     This refers to the "well-known" gain tables (see iwl-4965-hw.h).
+ *
+ * 3)  Actual measured output power, in half-dBm ("34" = 17 dBm).
+ *
+ * 4)  RF power amplifier detector level measurement (not used).
+ */
+struct iwl_eeprom_calib_measure {
+	u8 temperature;		/* Device temperature (Celsius) */
+	u8 gain_idx;		/* Index into gain table */
+	u8 actual_pow;		/* Measured RF output power, half-dBm */
+	s8 pa_det;		/* Power amp detector level (not used) */
+} __packed;
+
+
+/*
+ * measurement set for one channel.  EEPROM contains:
+ *
+ * 1)  Channel number measured
+ *
+ * 2)  Measurements for each of 3 power levels for each of 2 radio transmitters
+ *     (a.k.a. "tx chains") (6 measurements altogether)
+ */
+struct iwl_eeprom_calib_ch_info {
+	u8 ch_num;
+	struct iwl_eeprom_calib_measure
+		measurements[EEPROM_TX_POWER_TX_CHAINS]
+			[EEPROM_TX_POWER_MEASUREMENTS];
+} __packed;
+
+/*
+ * txpower subband info.
+ *
+ * For each frequency subband, EEPROM contains the following:
+ *
+ * 1)  First and last channels within range of the subband.  "0" values
+ *     indicate that this sample set is not being used.
+ *
+ * 2)  Sample measurement sets for 2 channels close to the range endpoints.
+ */
+struct iwl_eeprom_calib_subband_info {
+	u8 ch_from;	/* channel number of lowest channel in subband */
+	u8 ch_to;	/* channel number of highest channel in subband */
+	struct iwl_eeprom_calib_ch_info ch1;
+	struct iwl_eeprom_calib_ch_info ch2;
+} __packed;
+
+
+/*
+ * txpower calibration info.  EEPROM contains:
+ *
+ * 1)  Factory-measured saturation power levels (maximum levels at which
+ *     tx power amplifier can output a signal without too much distortion).
+ *     There is one level for 2.4 GHz band and one for 5 GHz band.  These
+ *     values apply to all channels within each of the bands.
+ *
+ * 2)  Factory-measured power supply voltage level.  This is assumed to be
+ *     constant (i.e. same value applies to all channels/bands) while the
+ *     factory measurements are being made.
+ *
+ * 3)  Up to 8 sets of factory-measured txpower calibration values.
+ *     These are for different frequency ranges, since txpower gain
+ *     characteristics of the analog radio circuitry vary with frequency.
+ *
+ *     Not all sets need to be filled with data;
+ *     struct iwl_eeprom_calib_subband_info contains range of channels
+ *     (0 if unused) for each set of data.
+ */
+struct iwl_eeprom_calib_info {
+	u8 saturation_power24;	/* half-dBm (e.g. "34" = 17 dBm) */
+	u8 saturation_power52;	/* half-dBm */
+	__le16 voltage;		/* signed */
+	struct iwl_eeprom_calib_subband_info
+		band_info[EEPROM_TX_POWER_BANDS];
+} __packed;
+
+
+/* General */
+#define EEPROM_DEVICE_ID                    (2*0x08)	/* 2 bytes */
+#define EEPROM_MAC_ADDRESS                  (2*0x15)	/* 6  bytes */
+#define EEPROM_BOARD_REVISION               (2*0x35)	/* 2  bytes */
+#define EEPROM_BOARD_PBA_NUMBER             (2*0x3B+1)	/* 9  bytes */
+#define EEPROM_VERSION                      (2*0x44)	/* 2  bytes */
+#define EEPROM_SKU_CAP                      (2*0x45)	/* 2  bytes */
+#define EEPROM_OEM_MODE                     (2*0x46)	/* 2  bytes */
+#define EEPROM_WOWLAN_MODE                  (2*0x47)	/* 2  bytes */
+#define EEPROM_RADIO_CONFIG                 (2*0x48)	/* 2  bytes */
+#define EEPROM_NUM_MAC_ADDRESS              (2*0x4C)	/* 2  bytes */
+
+/* The following masks are to be applied on EEPROM_RADIO_CONFIG */
+#define EEPROM_RF_CFG_TYPE_MSK(x)   (x & 0x3)         /* bits 0-1   */
+#define EEPROM_RF_CFG_STEP_MSK(x)   ((x >> 2)  & 0x3) /* bits 2-3   */
+#define EEPROM_RF_CFG_DASH_MSK(x)   ((x >> 4)  & 0x3) /* bits 4-5   */
+#define EEPROM_RF_CFG_PNUM_MSK(x)   ((x >> 6)  & 0x3) /* bits 6-7   */
+#define EEPROM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
+#define EEPROM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
+
+#define EEPROM_3945_RF_CFG_TYPE_MAX  0x0
+#define EEPROM_4965_RF_CFG_TYPE_MAX  0x1
+
+/*
+ * Per-channel regulatory data.
+ *
+ * Each channel that *might* be supported by iwl has a fixed location
+ * in EEPROM containing EEPROM_CHANNEL_* usage flags (LSB) and max regulatory
+ * txpower (MSB).
+ *
+ * Entries immediately below are for 20 MHz channel width.  HT40 (40 MHz)
+ * channels (only for 4965, not supported by 3945) appear later in the EEPROM.
+ *
+ * 2.4 GHz channels 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
+ */
+#define EEPROM_REGULATORY_SKU_ID            (2*0x60)    /* 4  bytes */
+#define EEPROM_REGULATORY_BAND_1            (2*0x62)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_1_CHANNELS   (2*0x63)	/* 28 bytes */
+
+/*
+ * 4.9 GHz channels 183, 184, 185, 187, 188, 189, 192, 196,
+ * 5.0 GHz channels 7, 8, 11, 12, 16
+ * (4915-5080MHz) (none of these is ever supported)
+ */
+#define EEPROM_REGULATORY_BAND_2            (2*0x71)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_2_CHANNELS   (2*0x72)	/* 26 bytes */
+
+/*
+ * 5.2 GHz channels 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
+ * (5170-5320MHz)
+ */
+#define EEPROM_REGULATORY_BAND_3            (2*0x7F)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_3_CHANNELS   (2*0x80)	/* 24 bytes */
+
+/*
+ * 5.5 GHz channels 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+ * (5500-5700MHz)
+ */
+#define EEPROM_REGULATORY_BAND_4            (2*0x8C)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_4_CHANNELS   (2*0x8D)	/* 22 bytes */
+
+/*
+ * 5.7 GHz channels 145, 149, 153, 157, 161, 165
+ * (5725-5825MHz)
+ */
+#define EEPROM_REGULATORY_BAND_5            (2*0x98)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_5_CHANNELS   (2*0x99)	/* 12 bytes */
+
+/*
+ * 2.4 GHz HT40 channels 1 (5), 2 (6), 3 (7), 4 (8), 5 (9), 6 (10), 7 (11)
+ *
+ * The channel listed is the center of the lower 20 MHz half of the channel.
+ * The overall center frequency is actually 2 channels (10 MHz) above that,
+ * and the upper half of each HT40 channel is centered 4 channels (20 MHz) away
+ * from the lower half; e.g. the upper half of HT40 channel 1 is channel 5,
+ * and the overall HT40 channel width centers on channel 3.
+ *
+ * NOTE:  The RXON command uses 20 MHz channel numbers to specify the
+ *        control channel to which to tune.  RXON also specifies whether the
+ *        control channel is the upper or lower half of a HT40 channel.
+ *
+ * NOTE:  4965 does not support HT40 channels on 2.4 GHz.
+ */
+#define EEPROM_4965_REGULATORY_BAND_24_HT40_CHANNELS (2*0xA0)	/* 14 bytes */
+
+/*
+ * 5.2 GHz HT40 channels 36 (40), 44 (48), 52 (56), 60 (64),
+ * 100 (104), 108 (112), 116 (120), 124 (128), 132 (136), 149 (153), 157 (161)
+ */
+#define EEPROM_4965_REGULATORY_BAND_52_HT40_CHANNELS (2*0xA8)	/* 22 bytes */
+
+#define EEPROM_REGULATORY_BAND_NO_HT40			(0)
+
+struct iwl_eeprom_ops {
+	const u32 regulatory_bands[7];
+	int (*acquire_semaphore) (struct iwl_priv *priv);
+	void (*release_semaphore) (struct iwl_priv *priv);
+};
+
+
+int iwl_legacy_eeprom_init(struct iwl_priv *priv);
+void iwl_legacy_eeprom_free(struct iwl_priv *priv);
+const u8 *iwl_legacy_eeprom_query_addr(const struct iwl_priv *priv,
+					size_t offset);
+u16 iwl_legacy_eeprom_query16(const struct iwl_priv *priv, size_t offset);
+int iwl_legacy_init_channel_map(struct iwl_priv *priv);
+void iwl_legacy_free_channel_map(struct iwl_priv *priv);
+const struct iwl_channel_info *iwl_legacy_get_channel_info(
+		const struct iwl_priv *priv,
+		enum ieee80211_band band, u16 channel);
+
+#endif  /* __iwl_legacy_eeprom_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-fh.h b/drivers/net/wireless/iwlegacy/iwl-fh.h
new file mode 100644
index 000000000000..4e20c7e5c883
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-fh.h
@@ -0,0 +1,513 @@
+/******************************************************************************
+ *
+ * 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) 2005 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef __iwl_legacy_fh_h__
+#define __iwl_legacy_fh_h__
+
+/****************************/
+/* Flow Handler Definitions */
+/****************************/
+
+/**
+ * This I/O area is directly read/writable by driver (e.g. Linux uses writel())
+ * Addresses are offsets from device's PCI hardware base address.
+ */
+#define FH_MEM_LOWER_BOUND                   (0x1000)
+#define FH_MEM_UPPER_BOUND                   (0x2000)
+
+/**
+ * Keep-Warm (KW) buffer base address.
+ *
+ * Driver must allocate a 4KByte buffer that is used by 4965 for keeping the
+ * host DRAM powered on (via dummy accesses to DRAM) to maintain low-latency
+ * DRAM access when 4965 is Txing or Rxing.  The dummy accesses prevent host
+ * from going into a power-savings mode that would cause higher DRAM latency,
+ * and possible data over/under-runs, before all Tx/Rx is complete.
+ *
+ * Driver loads FH_KW_MEM_ADDR_REG with the physical address (bits 35:4)
+ * of the buffer, which must be 4K aligned.  Once this is set up, the 4965
+ * automatically invokes keep-warm accesses when normal accesses might not
+ * be sufficient to maintain fast DRAM response.
+ *
+ * Bit fields:
+ *  31-0:  Keep-warm buffer physical base address [35:4], must be 4K aligned
+ */
+#define FH_KW_MEM_ADDR_REG		     (FH_MEM_LOWER_BOUND + 0x97C)
+
+
+/**
+ * TFD Circular Buffers Base (CBBC) addresses
+ *
+ * 4965 has 16 base pointer registers, one for each of 16 host-DRAM-resident
+ * circular buffers (CBs/queues) containing Transmit Frame Descriptors (TFDs)
+ * (see struct iwl_tfd_frame).  These 16 pointer registers are offset by 0x04
+ * bytes from one another.  Each TFD circular buffer in DRAM must be 256-byte
+ * aligned (address bits 0-7 must be 0).
+ *
+ * Bit fields in each pointer register:
+ *  27-0: TFD CB physical base address [35:8], must be 256-byte aligned
+ */
+#define FH_MEM_CBBC_LOWER_BOUND          (FH_MEM_LOWER_BOUND + 0x9D0)
+#define FH_MEM_CBBC_UPPER_BOUND          (FH_MEM_LOWER_BOUND + 0xA10)
+
+/* Find TFD CB base pointer for given queue (range 0-15). */
+#define FH_MEM_CBBC_QUEUE(x)  (FH_MEM_CBBC_LOWER_BOUND + (x) * 0x4)
+
+
+/**
+ * Rx SRAM Control and Status Registers (RSCSR)
+ *
+ * These registers provide handshake between driver and 4965 for the Rx queue
+ * (this queue handles *all* command responses, notifications, Rx data, etc.
+ * sent from 4965 uCode to host driver).  Unlike Tx, there is only one Rx
+ * queue, and only one Rx DMA/FIFO channel.  Also unlike Tx, which can
+ * concatenate up to 20 DRAM buffers to form a Tx frame, each Receive Buffer
+ * Descriptor (RBD) points to only one Rx Buffer (RB); there is a 1:1
+ * mapping between RBDs and RBs.
+ *
+ * Driver must allocate host DRAM memory for the following, and set the
+ * physical address of each into 4965 registers:
+ *
+ * 1)  Receive Buffer Descriptor (RBD) circular buffer (CB), typically with 256
+ *     entries (although any power of 2, up to 4096, is selectable by driver).
+ *     Each entry (1 dword) points to a receive buffer (RB) of consistent size
+ *     (typically 4K, although 8K or 16K are also selectable by driver).
+ *     Driver sets up RB size and number of RBDs in the CB via Rx config
+ *     register FH_MEM_RCSR_CHNL0_CONFIG_REG.
+ *
+ *     Bit fields within one RBD:
+ *     27-0:  Receive Buffer physical address bits [35:8], 256-byte aligned
+ *
+ *     Driver sets physical address [35:8] of base of RBD circular buffer
+ *     into FH_RSCSR_CHNL0_RBDCB_BASE_REG [27:0].
+ *
+ * 2)  Rx status buffer, 8 bytes, in which 4965 indicates which Rx Buffers
+ *     (RBs) have been filled, via a "write pointer", actually the index of
+ *     the RB's corresponding RBD within the circular buffer.  Driver sets
+ *     physical address [35:4] into FH_RSCSR_CHNL0_STTS_WPTR_REG [31:0].
+ *
+ *     Bit fields in lower dword of Rx status buffer (upper dword not used
+ *     by driver; see struct iwl4965_shared, val0):
+ *     31-12:  Not used by driver
+ *     11- 0:  Index of last filled Rx buffer descriptor
+ *             (4965 writes, driver reads this value)
+ *
+ * As the driver prepares Receive Buffers (RBs) for 4965 to fill, driver must
+ * enter pointers to these RBs into contiguous RBD circular buffer entries,
+ * and update the 4965's "write" index register,
+ * FH_RSCSR_CHNL0_RBDCB_WPTR_REG.
+ *
+ * This "write" index corresponds to the *next* RBD that the driver will make
+ * available, i.e. one RBD past the tail of the ready-to-fill RBDs within
+ * the circular buffer.  This value should initially be 0 (before preparing any
+ * RBs), should be 8 after preparing the first 8 RBs (for example), and must
+ * wrap back to 0 at the end of the circular buffer (but don't wrap before
+ * "read" index has advanced past 1!  See below).
+ * NOTE:  4965 EXPECTS THE WRITE INDEX TO BE INCREMENTED IN MULTIPLES OF 8.
+ *
+ * As the 4965 fills RBs (referenced from contiguous RBDs within the circular
+ * buffer), it updates the Rx status buffer in host DRAM, 2) described above,
+ * to tell the driver the index of the latest filled RBD.  The driver must
+ * read this "read" index from DRAM after receiving an Rx interrupt from 4965.
+ *
+ * The driver must also internally keep track of a third index, which is the
+ * next RBD to process.  When receiving an Rx interrupt, driver should process
+ * all filled but unprocessed RBs up to, but not including, the RB
+ * corresponding to the "read" index.  For example, if "read" index becomes "1",
+ * driver may process the RB pointed to by RBD 0.  Depending on volume of
+ * traffic, there may be many RBs to process.
+ *
+ * If read index == write index, 4965 thinks there is no room to put new data.
+ * Due to this, the maximum number of filled RBs is 255, instead of 256.  To
+ * be safe, make sure that there is a gap of at least 2 RBDs between "write"
+ * and "read" indexes; that is, make sure that there are no more than 254
+ * buffers waiting to be filled.
+ */
+#define FH_MEM_RSCSR_LOWER_BOUND	(FH_MEM_LOWER_BOUND + 0xBC0)
+#define FH_MEM_RSCSR_UPPER_BOUND	(FH_MEM_LOWER_BOUND + 0xC00)
+#define FH_MEM_RSCSR_CHNL0		(FH_MEM_RSCSR_LOWER_BOUND)
+
+/**
+ * Physical base address of 8-byte Rx Status buffer.
+ * Bit fields:
+ *  31-0: Rx status buffer physical base address [35:4], must 16-byte aligned.
+ */
+#define FH_RSCSR_CHNL0_STTS_WPTR_REG	(FH_MEM_RSCSR_CHNL0)
+
+/**
+ * Physical base address of Rx Buffer Descriptor Circular Buffer.
+ * Bit fields:
+ *  27-0:  RBD CD physical base address [35:8], must be 256-byte aligned.
+ */
+#define FH_RSCSR_CHNL0_RBDCB_BASE_REG	(FH_MEM_RSCSR_CHNL0 + 0x004)
+
+/**
+ * Rx write pointer (index, really!).
+ * Bit fields:
+ *  11-0:  Index of driver's most recent prepared-to-be-filled RBD, + 1.
+ *         NOTE:  For 256-entry circular buffer, use only bits [7:0].
+ */
+#define FH_RSCSR_CHNL0_RBDCB_WPTR_REG	(FH_MEM_RSCSR_CHNL0 + 0x008)
+#define FH_RSCSR_CHNL0_WPTR        (FH_RSCSR_CHNL0_RBDCB_WPTR_REG)
+
+
+/**
+ * Rx Config/Status Registers (RCSR)
+ * Rx Config Reg for channel 0 (only channel used)
+ *
+ * Driver must initialize FH_MEM_RCSR_CHNL0_CONFIG_REG as follows for
+ * normal operation (see bit fields).
+ *
+ * Clearing FH_MEM_RCSR_CHNL0_CONFIG_REG to 0 turns off Rx DMA.
+ * Driver should poll FH_MEM_RSSR_RX_STATUS_REG	for
+ * FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE (bit 24) before continuing.
+ *
+ * Bit fields:
+ * 31-30: Rx DMA channel enable: '00' off/pause, '01' pause at end of frame,
+ *        '10' operate normally
+ * 29-24: reserved
+ * 23-20: # RBDs in circular buffer = 2^value; use "8" for 256 RBDs (normal),
+ *        min "5" for 32 RBDs, max "12" for 4096 RBDs.
+ * 19-18: reserved
+ * 17-16: size of each receive buffer; '00' 4K (normal), '01' 8K,
+ *        '10' 12K, '11' 16K.
+ * 15-14: reserved
+ * 13-12: IRQ destination; '00' none, '01' host driver (normal operation)
+ * 11- 4: timeout for closing Rx buffer and interrupting host (units 32 usec)
+ *        typical value 0x10 (about 1/2 msec)
+ *  3- 0: reserved
+ */
+#define FH_MEM_RCSR_LOWER_BOUND      (FH_MEM_LOWER_BOUND + 0xC00)
+#define FH_MEM_RCSR_UPPER_BOUND      (FH_MEM_LOWER_BOUND + 0xCC0)
+#define FH_MEM_RCSR_CHNL0            (FH_MEM_RCSR_LOWER_BOUND)
+
+#define FH_MEM_RCSR_CHNL0_CONFIG_REG	(FH_MEM_RCSR_CHNL0)
+
+#define FH_RCSR_CHNL0_RX_CONFIG_RB_TIMEOUT_MSK (0x00000FF0) /* bits 4-11 */
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_MSK   (0x00001000) /* bits 12 */
+#define FH_RCSR_CHNL0_RX_CONFIG_SINGLE_FRAME_MSK (0x00008000) /* bit 15 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RB_SIZE_MSK   (0x00030000) /* bits 16-17 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RBDBC_SIZE_MSK (0x00F00000) /* bits 20-23 */
+#define FH_RCSR_CHNL0_RX_CONFIG_DMA_CHNL_EN_MSK (0xC0000000) /* bits 30-31*/
+
+#define FH_RCSR_RX_CONFIG_RBDCB_SIZE_POS	(20)
+#define FH_RCSR_RX_CONFIG_REG_IRQ_RBTH_POS	(4)
+#define RX_RB_TIMEOUT	(0x10)
+
+#define FH_RCSR_RX_CONFIG_CHNL_EN_PAUSE_VAL         (0x00000000)
+#define FH_RCSR_RX_CONFIG_CHNL_EN_PAUSE_EOF_VAL     (0x40000000)
+#define FH_RCSR_RX_CONFIG_CHNL_EN_ENABLE_VAL        (0x80000000)
+
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K    (0x00000000)
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K    (0x00010000)
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_12K   (0x00020000)
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_16K   (0x00030000)
+
+#define FH_RCSR_CHNL0_RX_IGNORE_RXF_EMPTY              (0x00000004)
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_NO_INT_VAL    (0x00000000)
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL  (0x00001000)
+
+#define FH_RSCSR_FRAME_SIZE_MSK	(0x00003FFF)	/* bits 0-13 */
+
+/**
+ * Rx Shared Status Registers (RSSR)
+ *
+ * After stopping Rx DMA channel (writing 0 to
+ * FH_MEM_RCSR_CHNL0_CONFIG_REG), driver must poll
+ * FH_MEM_RSSR_RX_STATUS_REG until Rx channel is idle.
+ *
+ * Bit fields:
+ *  24:  1 = Channel 0 is idle
+ *
+ * FH_MEM_RSSR_SHARED_CTRL_REG and FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV
+ * contain default values that should not be altered by the driver.
+ */
+#define FH_MEM_RSSR_LOWER_BOUND           (FH_MEM_LOWER_BOUND + 0xC40)
+#define FH_MEM_RSSR_UPPER_BOUND           (FH_MEM_LOWER_BOUND + 0xD00)
+
+#define FH_MEM_RSSR_SHARED_CTRL_REG       (FH_MEM_RSSR_LOWER_BOUND)
+#define FH_MEM_RSSR_RX_STATUS_REG	(FH_MEM_RSSR_LOWER_BOUND + 0x004)
+#define FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV\
+					(FH_MEM_RSSR_LOWER_BOUND + 0x008)
+
+#define FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE	(0x01000000)
+
+#define FH_MEM_TFDIB_REG1_ADDR_BITSHIFT	28
+
+/* TFDB  Area - TFDs buffer table */
+#define FH_MEM_TFDIB_DRAM_ADDR_LSB_MSK      (0xFFFFFFFF)
+#define FH_TFDIB_LOWER_BOUND       (FH_MEM_LOWER_BOUND + 0x900)
+#define FH_TFDIB_UPPER_BOUND       (FH_MEM_LOWER_BOUND + 0x958)
+#define FH_TFDIB_CTRL0_REG(_chnl)  (FH_TFDIB_LOWER_BOUND + 0x8 * (_chnl))
+#define FH_TFDIB_CTRL1_REG(_chnl)  (FH_TFDIB_LOWER_BOUND + 0x8 * (_chnl) + 0x4)
+
+/**
+ * Transmit DMA Channel Control/Status Registers (TCSR)
+ *
+ * 4965 has one configuration register for each of 8 Tx DMA/FIFO channels
+ * supported in hardware (don't confuse these with the 16 Tx queues in DRAM,
+ * which feed the DMA/FIFO channels); config regs are separated by 0x20 bytes.
+ *
+ * To use a Tx DMA channel, driver must initialize its
+ * FH_TCSR_CHNL_TX_CONFIG_REG(chnl) with:
+ *
+ * FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
+ * FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL
+ *
+ * All other bits should be 0.
+ *
+ * Bit fields:
+ * 31-30: Tx DMA channel enable: '00' off/pause, '01' pause at end of frame,
+ *        '10' operate normally
+ * 29- 4: Reserved, set to "0"
+ *     3: Enable internal DMA requests (1, normal operation), disable (0)
+ *  2- 0: Reserved, set to "0"
+ */
+#define FH_TCSR_LOWER_BOUND  (FH_MEM_LOWER_BOUND + 0xD00)
+#define FH_TCSR_UPPER_BOUND  (FH_MEM_LOWER_BOUND + 0xE60)
+
+/* Find Control/Status reg for given Tx DMA/FIFO channel */
+#define FH49_TCSR_CHNL_NUM                            (7)
+#define FH50_TCSR_CHNL_NUM                            (8)
+
+/* TCSR: tx_config register values */
+#define FH_TCSR_CHNL_TX_CONFIG_REG(_chnl)	\
+		(FH_TCSR_LOWER_BOUND + 0x20 * (_chnl))
+#define FH_TCSR_CHNL_TX_CREDIT_REG(_chnl)	\
+		(FH_TCSR_LOWER_BOUND + 0x20 * (_chnl) + 0x4)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG(_chnl)	\
+		(FH_TCSR_LOWER_BOUND + 0x20 * (_chnl) + 0x8)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_TXF		(0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_DRV		(0x00000001)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE	(0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE	(0x00000008)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_NOINT	(0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD	(0x00100000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_IFTFD	(0x00200000)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_NOINT	(0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_ENDTFD	(0x00400000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_IFTFD	(0x00800000)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE	(0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE_EOF	(0x40000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE	(0x80000000)
+
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_EMPTY	(0x00000000)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_WAIT	(0x00002000)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID	(0x00000003)
+
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM		(20)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX		(12)
+
+/**
+ * Tx Shared Status Registers (TSSR)
+ *
+ * After stopping Tx DMA channel (writing 0 to
+ * FH_TCSR_CHNL_TX_CONFIG_REG(chnl)), driver must poll
+ * FH_TSSR_TX_STATUS_REG until selected Tx channel is idle
+ * (channel's buffers empty | no pending requests).
+ *
+ * Bit fields:
+ * 31-24:  1 = Channel buffers empty (channel 7:0)
+ * 23-16:  1 = No pending requests (channel 7:0)
+ */
+#define FH_TSSR_LOWER_BOUND		(FH_MEM_LOWER_BOUND + 0xEA0)
+#define FH_TSSR_UPPER_BOUND		(FH_MEM_LOWER_BOUND + 0xEC0)
+
+#define FH_TSSR_TX_STATUS_REG		(FH_TSSR_LOWER_BOUND + 0x010)
+
+/**
+ * Bit fields for TSSR(Tx Shared Status & Control) error status register:
+ * 31:  Indicates an address error when accessed to internal memory
+ *	uCode/driver must write "1" in order to clear this flag
+ * 30:  Indicates that Host did not send the expected number of dwords to FH
+ *	uCode/driver must write "1" in order to clear this flag
+ * 16-9:Each status bit is for one channel. Indicates that an (Error) ActDMA
+ *	command was received from the scheduler while the TRB was already full
+ *	with previous command
+ *	uCode/driver must write "1" in order to clear this flag
+ * 7-0: Each status bit indicates a channel's TxCredit error. When an error
+ *	bit is set, it indicates that the FH has received a full indication
+ *	from the RTC TxFIFO and the current value of the TxCredit counter was
+ *	not equal to zero. This mean that the credit mechanism was not
+ *	synchronized to the TxFIFO status
+ *	uCode/driver must write "1" in order to clear this flag
+ */
+#define FH_TSSR_TX_ERROR_REG		(FH_TSSR_LOWER_BOUND + 0x018)
+
+#define FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(_chnl) ((1 << (_chnl)) << 16)
+
+/* Tx service channels */
+#define FH_SRVC_CHNL		(9)
+#define FH_SRVC_LOWER_BOUND	(FH_MEM_LOWER_BOUND + 0x9C8)
+#define FH_SRVC_UPPER_BOUND	(FH_MEM_LOWER_BOUND + 0x9D0)
+#define FH_SRVC_CHNL_SRAM_ADDR_REG(_chnl) \
+		(FH_SRVC_LOWER_BOUND + ((_chnl) - 9) * 0x4)
+
+#define FH_TX_CHICKEN_BITS_REG	(FH_MEM_LOWER_BOUND + 0xE98)
+/* Instruct FH to increment the retry count of a packet when
+ * it is brought from the memory to TX-FIFO
+ */
+#define FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN	(0x00000002)
+
+#define RX_QUEUE_SIZE                         256
+#define RX_QUEUE_MASK                         255
+#define RX_QUEUE_SIZE_LOG                     8
+
+/*
+ * RX related structures and functions
+ */
+#define RX_FREE_BUFFERS 64
+#define RX_LOW_WATERMARK 8
+
+/* Size of one Rx buffer in host DRAM */
+#define IWL_RX_BUF_SIZE_3K (3 * 1000) /* 3945 only */
+#define IWL_RX_BUF_SIZE_4K (4 * 1024)
+#define IWL_RX_BUF_SIZE_8K (8 * 1024)
+
+/**
+ * struct iwl_rb_status - reseve buffer status
+ * 	host memory mapped FH registers
+ * @closed_rb_num [0:11] - Indicates the index of the RB which was closed
+ * @closed_fr_num [0:11] - Indicates the index of the RX Frame which was closed
+ * @finished_rb_num [0:11] - Indicates the index of the current RB
+ * 	in which the last frame was written to
+ * @finished_fr_num [0:11] - Indicates the index of the RX Frame
+ * 	which was transfered
+ */
+struct iwl_rb_status {
+	__le16 closed_rb_num;
+	__le16 closed_fr_num;
+	__le16 finished_rb_num;
+	__le16 finished_fr_nam;
+	__le32 __unused; /* 3945 only */
+} __packed;
+
+
+#define TFD_QUEUE_SIZE_MAX      (256)
+#define TFD_QUEUE_SIZE_BC_DUP	(64)
+#define TFD_QUEUE_BC_SIZE	(TFD_QUEUE_SIZE_MAX + TFD_QUEUE_SIZE_BC_DUP)
+#define IWL_TX_DMA_MASK        DMA_BIT_MASK(36)
+#define IWL_NUM_OF_TBS		20
+
+static inline u8 iwl_legacy_get_dma_hi_addr(dma_addr_t addr)
+{
+	return (sizeof(addr) > sizeof(u32) ? (addr >> 16) >> 16 : 0) & 0xF;
+}
+/**
+ * struct iwl_tfd_tb transmit buffer descriptor within transmit frame descriptor
+ *
+ * This structure contains dma address and length of transmission address
+ *
+ * @lo: low [31:0] portion of the dma address of TX buffer
+ * 	every even is unaligned on 16 bit boundary
+ * @hi_n_len 0-3 [35:32] portion of dma
+ *	     4-15 length of the tx buffer
+ */
+struct iwl_tfd_tb {
+	__le32 lo;
+	__le16 hi_n_len;
+} __packed;
+
+/**
+ * struct iwl_tfd
+ *
+ * Transmit Frame Descriptor (TFD)
+ *
+ * @ __reserved1[3] reserved
+ * @ num_tbs 0-4 number of active tbs
+ *	     5   reserved
+ * 	     6-7 padding (not used)
+ * @ tbs[20]	transmit frame buffer descriptors
+ * @ __pad 	padding
+ *
+ * Each Tx queue uses a circular buffer of 256 TFDs stored in host DRAM.
+ * Both driver and device share these circular buffers, each of which must be
+ * contiguous 256 TFDs x 128 bytes-per-TFD = 32 KBytes
+ *
+ * Driver must indicate the physical address of the base of each
+ * circular buffer via the FH_MEM_CBBC_QUEUE registers.
+ *
+ * Each TFD contains pointer/size information for up to 20 data buffers
+ * in host DRAM.  These buffers collectively contain the (one) frame described
+ * by the TFD.  Each buffer must be a single contiguous block of memory within
+ * itself, but buffers may be scattered in host DRAM.  Each buffer has max size
+ * of (4K - 4).  The concatenates all of a TFD's buffers into a single
+ * Tx frame, up to 8 KBytes in size.
+ *
+ * A maximum of 255 (not 256!) TFDs may be on a queue waiting for Tx.
+ */
+struct iwl_tfd {
+	u8 __reserved1[3];
+	u8 num_tbs;
+	struct iwl_tfd_tb tbs[IWL_NUM_OF_TBS];
+	__le32 __pad;
+} __packed;
+
+/* Keep Warm Size */
+#define IWL_KW_SIZE 0x1000	/* 4k */
+
+#endif /* !__iwl_legacy_fh_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-hcmd.c b/drivers/net/wireless/iwlegacy/iwl-hcmd.c
new file mode 100644
index 000000000000..9d721cbda5bb
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-hcmd.c
@@ -0,0 +1,271 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <net/mac80211.h>
+
+#include "iwl-dev.h"
+#include "iwl-debug.h"
+#include "iwl-eeprom.h"
+#include "iwl-core.h"
+
+
+const char *iwl_legacy_get_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+		IWL_CMD(REPLY_ALIVE);
+		IWL_CMD(REPLY_ERROR);
+		IWL_CMD(REPLY_RXON);
+		IWL_CMD(REPLY_RXON_ASSOC);
+		IWL_CMD(REPLY_QOS_PARAM);
+		IWL_CMD(REPLY_RXON_TIMING);
+		IWL_CMD(REPLY_ADD_STA);
+		IWL_CMD(REPLY_REMOVE_STA);
+		IWL_CMD(REPLY_WEPKEY);
+		IWL_CMD(REPLY_3945_RX);
+		IWL_CMD(REPLY_TX);
+		IWL_CMD(REPLY_RATE_SCALE);
+		IWL_CMD(REPLY_LEDS_CMD);
+		IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
+		IWL_CMD(REPLY_CHANNEL_SWITCH);
+		IWL_CMD(CHANNEL_SWITCH_NOTIFICATION);
+		IWL_CMD(REPLY_SPECTRUM_MEASUREMENT_CMD);
+		IWL_CMD(SPECTRUM_MEASURE_NOTIFICATION);
+		IWL_CMD(POWER_TABLE_CMD);
+		IWL_CMD(PM_SLEEP_NOTIFICATION);
+		IWL_CMD(PM_DEBUG_STATISTIC_NOTIFIC);
+		IWL_CMD(REPLY_SCAN_CMD);
+		IWL_CMD(REPLY_SCAN_ABORT_CMD);
+		IWL_CMD(SCAN_START_NOTIFICATION);
+		IWL_CMD(SCAN_RESULTS_NOTIFICATION);
+		IWL_CMD(SCAN_COMPLETE_NOTIFICATION);
+		IWL_CMD(BEACON_NOTIFICATION);
+		IWL_CMD(REPLY_TX_BEACON);
+		IWL_CMD(REPLY_TX_PWR_TABLE_CMD);
+		IWL_CMD(REPLY_BT_CONFIG);
+		IWL_CMD(REPLY_STATISTICS_CMD);
+		IWL_CMD(STATISTICS_NOTIFICATION);
+		IWL_CMD(CARD_STATE_NOTIFICATION);
+		IWL_CMD(MISSED_BEACONS_NOTIFICATION);
+		IWL_CMD(REPLY_CT_KILL_CONFIG_CMD);
+		IWL_CMD(SENSITIVITY_CMD);
+		IWL_CMD(REPLY_PHY_CALIBRATION_CMD);
+		IWL_CMD(REPLY_RX_PHY_CMD);
+		IWL_CMD(REPLY_RX_MPDU_CMD);
+		IWL_CMD(REPLY_RX);
+		IWL_CMD(REPLY_COMPRESSED_BA);
+	default:
+		return "UNKNOWN";
+
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_get_cmd_string);
+
+#define HOST_COMPLETE_TIMEOUT (HZ / 2)
+
+static void iwl_legacy_generic_cmd_callback(struct iwl_priv *priv,
+				     struct iwl_device_cmd *cmd,
+				     struct iwl_rx_packet *pkt)
+{
+	if (pkt->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERR(priv, "Bad return from %s (0x%08X)\n",
+		iwl_legacy_get_cmd_string(cmd->hdr.cmd), pkt->hdr.flags);
+		return;
+	}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	switch (cmd->hdr.cmd) {
+	case REPLY_TX_LINK_QUALITY_CMD:
+	case SENSITIVITY_CMD:
+		IWL_DEBUG_HC_DUMP(priv, "back from %s (0x%08X)\n",
+		iwl_legacy_get_cmd_string(cmd->hdr.cmd), pkt->hdr.flags);
+		break;
+	default:
+		IWL_DEBUG_HC(priv, "back from %s (0x%08X)\n",
+		iwl_legacy_get_cmd_string(cmd->hdr.cmd), pkt->hdr.flags);
+	}
+#endif
+}
+
+static int
+iwl_legacy_send_cmd_async(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	int ret;
+
+	BUG_ON(!(cmd->flags & CMD_ASYNC));
+
+	/* An asynchronous command can not expect an SKB to be set. */
+	BUG_ON(cmd->flags & CMD_WANT_SKB);
+
+	/* Assign a generic callback if one is not provided */
+	if (!cmd->callback)
+		cmd->callback = iwl_legacy_generic_cmd_callback;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return -EBUSY;
+
+	ret = iwl_legacy_enqueue_hcmd(priv, cmd);
+	if (ret < 0) {
+		IWL_ERR(priv, "Error sending %s: enqueue_hcmd failed: %d\n",
+			  iwl_legacy_get_cmd_string(cmd->id), ret);
+		return ret;
+	}
+	return 0;
+}
+
+int iwl_legacy_send_cmd_sync(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	int cmd_idx;
+	int ret;
+
+	BUG_ON(cmd->flags & CMD_ASYNC);
+
+	 /* A synchronous command can not have a callback set. */
+	BUG_ON(cmd->callback);
+
+	IWL_DEBUG_INFO(priv, "Attempting to send sync command %s\n",
+			iwl_legacy_get_cmd_string(cmd->id));
+	mutex_lock(&priv->sync_cmd_mutex);
+
+	set_bit(STATUS_HCMD_ACTIVE, &priv->status);
+	IWL_DEBUG_INFO(priv, "Setting HCMD_ACTIVE for command %s\n",
+			iwl_legacy_get_cmd_string(cmd->id));
+
+	cmd_idx = iwl_legacy_enqueue_hcmd(priv, cmd);
+	if (cmd_idx < 0) {
+		ret = cmd_idx;
+		IWL_ERR(priv, "Error sending %s: enqueue_hcmd failed: %d\n",
+			  iwl_legacy_get_cmd_string(cmd->id), ret);
+		goto out;
+	}
+
+	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+			!test_bit(STATUS_HCMD_ACTIVE, &priv->status),
+			HOST_COMPLETE_TIMEOUT);
+	if (!ret) {
+		if (test_bit(STATUS_HCMD_ACTIVE, &priv->status)) {
+			IWL_ERR(priv,
+				"Error sending %s: time out after %dms.\n",
+				iwl_legacy_get_cmd_string(cmd->id),
+				jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
+
+			clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+			IWL_DEBUG_INFO(priv,
+				"Clearing HCMD_ACTIVE for command %s\n",
+				       iwl_legacy_get_cmd_string(cmd->id));
+			ret = -ETIMEDOUT;
+			goto cancel;
+		}
+	}
+
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
+		IWL_ERR(priv, "Command %s aborted: RF KILL Switch\n",
+			       iwl_legacy_get_cmd_string(cmd->id));
+		ret = -ECANCELED;
+		goto fail;
+	}
+	if (test_bit(STATUS_FW_ERROR, &priv->status)) {
+		IWL_ERR(priv, "Command %s failed: FW Error\n",
+			       iwl_legacy_get_cmd_string(cmd->id));
+		ret = -EIO;
+		goto fail;
+	}
+	if ((cmd->flags & CMD_WANT_SKB) && !cmd->reply_page) {
+		IWL_ERR(priv, "Error: Response NULL in '%s'\n",
+			  iwl_legacy_get_cmd_string(cmd->id));
+		ret = -EIO;
+		goto cancel;
+	}
+
+	ret = 0;
+	goto out;
+
+cancel:
+	if (cmd->flags & CMD_WANT_SKB) {
+		/*
+		 * Cancel the CMD_WANT_SKB flag for the cmd in the
+		 * TX cmd queue. Otherwise in case the cmd comes
+		 * in later, it will possibly set an invalid
+		 * address (cmd->meta.source).
+		 */
+		priv->txq[priv->cmd_queue].meta[cmd_idx].flags &=
+							~CMD_WANT_SKB;
+	}
+fail:
+	if (cmd->reply_page) {
+		iwl_legacy_free_pages(priv, cmd->reply_page);
+		cmd->reply_page = 0;
+	}
+out:
+	mutex_unlock(&priv->sync_cmd_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_send_cmd_sync);
+
+int iwl_legacy_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	if (cmd->flags & CMD_ASYNC)
+		return iwl_legacy_send_cmd_async(priv, cmd);
+
+	return iwl_legacy_send_cmd_sync(priv, cmd);
+}
+EXPORT_SYMBOL(iwl_legacy_send_cmd);
+
+int
+iwl_legacy_send_cmd_pdu(struct iwl_priv *priv, u8 id, u16 len, const void *data)
+{
+	struct iwl_host_cmd cmd = {
+		.id = id,
+		.len = len,
+		.data = data,
+	};
+
+	return iwl_legacy_send_cmd_sync(priv, &cmd);
+}
+EXPORT_SYMBOL(iwl_legacy_send_cmd_pdu);
+
+int iwl_legacy_send_cmd_pdu_async(struct iwl_priv *priv,
+			   u8 id, u16 len, const void *data,
+			   void (*callback)(struct iwl_priv *priv,
+					    struct iwl_device_cmd *cmd,
+					    struct iwl_rx_packet *pkt))
+{
+	struct iwl_host_cmd cmd = {
+		.id = id,
+		.len = len,
+		.data = data,
+	};
+
+	cmd.flags |= CMD_ASYNC;
+	cmd.callback = callback;
+
+	return iwl_legacy_send_cmd_async(priv, &cmd);
+}
+EXPORT_SYMBOL(iwl_legacy_send_cmd_pdu_async);
diff --git a/drivers/net/wireless/iwlegacy/iwl-helpers.h b/drivers/net/wireless/iwlegacy/iwl-helpers.h
new file mode 100644
index 000000000000..02132e755831
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-helpers.h
@@ -0,0 +1,181 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_helpers_h__
+#define __iwl_legacy_helpers_h__
+
+#include <linux/ctype.h>
+#include <net/mac80211.h>
+
+#include "iwl-io.h"
+
+#define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
+
+
+static inline struct ieee80211_conf *iwl_legacy_ieee80211_get_hw_conf(
+	struct ieee80211_hw *hw)
+{
+	return &hw->conf;
+}
+
+/**
+ * iwl_legacy_queue_inc_wrap - increment queue index, wrap back to beginning
+ * @index -- current index
+ * @n_bd -- total number of entries in queue (must be power of 2)
+ */
+static inline int iwl_legacy_queue_inc_wrap(int index, int n_bd)
+{
+	return ++index & (n_bd - 1);
+}
+
+/**
+ * iwl_legacy_queue_dec_wrap - decrement queue index, wrap back to end
+ * @index -- current index
+ * @n_bd -- total number of entries in queue (must be power of 2)
+ */
+static inline int iwl_legacy_queue_dec_wrap(int index, int n_bd)
+{
+	return --index & (n_bd - 1);
+}
+
+/* TODO: Move fw_desc functions to iwl-pci.ko */
+static inline void iwl_legacy_free_fw_desc(struct pci_dev *pci_dev,
+				    struct fw_desc *desc)
+{
+	if (desc->v_addr)
+		dma_free_coherent(&pci_dev->dev, desc->len,
+				  desc->v_addr, desc->p_addr);
+	desc->v_addr = NULL;
+	desc->len = 0;
+}
+
+static inline int iwl_legacy_alloc_fw_desc(struct pci_dev *pci_dev,
+				    struct fw_desc *desc)
+{
+	if (!desc->len) {
+		desc->v_addr = NULL;
+		return -EINVAL;
+	}
+
+	desc->v_addr = dma_alloc_coherent(&pci_dev->dev, desc->len,
+					  &desc->p_addr, GFP_KERNEL);
+	return (desc->v_addr != NULL) ? 0 : -ENOMEM;
+}
+
+/*
+ * we have 8 bits used like this:
+ *
+ * 7 6 5 4 3 2 1 0
+ * | | | | | | | |
+ * | | | | | | +-+-------- AC queue (0-3)
+ * | | | | | |
+ * | +-+-+-+-+------------ HW queue ID
+ * |
+ * +---------------------- unused
+ */
+static inline void
+iwl_legacy_set_swq_id(struct iwl_tx_queue *txq, u8 ac, u8 hwq)
+{
+	BUG_ON(ac > 3);   /* only have 2 bits */
+	BUG_ON(hwq > 31); /* only use 5 bits */
+
+	txq->swq_id = (hwq << 2) | ac;
+}
+
+static inline void iwl_legacy_wake_queue(struct iwl_priv *priv,
+				  struct iwl_tx_queue *txq)
+{
+	u8 queue = txq->swq_id;
+	u8 ac = queue & 3;
+	u8 hwq = (queue >> 2) & 0x1f;
+
+	if (test_and_clear_bit(hwq, priv->queue_stopped))
+		if (atomic_dec_return(&priv->queue_stop_count[ac]) <= 0)
+			ieee80211_wake_queue(priv->hw, ac);
+}
+
+static inline void iwl_legacy_stop_queue(struct iwl_priv *priv,
+				  struct iwl_tx_queue *txq)
+{
+	u8 queue = txq->swq_id;
+	u8 ac = queue & 3;
+	u8 hwq = (queue >> 2) & 0x1f;
+
+	if (!test_and_set_bit(hwq, priv->queue_stopped))
+		if (atomic_inc_return(&priv->queue_stop_count[ac]) > 0)
+			ieee80211_stop_queue(priv->hw, ac);
+}
+
+#define ieee80211_stop_queue DO_NOT_USE_ieee80211_stop_queue
+#define ieee80211_wake_queue DO_NOT_USE_ieee80211_wake_queue
+
+static inline void iwl_legacy_disable_interrupts(struct iwl_priv *priv)
+{
+	clear_bit(STATUS_INT_ENABLED, &priv->status);
+
+	/* disable interrupts from uCode/NIC to host */
+	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
+
+	/* acknowledge/clear/reset any interrupts still pending
+	 * from uCode or flow handler (Rx/Tx DMA) */
+	iwl_write32(priv, CSR_INT, 0xffffffff);
+	iwl_write32(priv, CSR_FH_INT_STATUS, 0xffffffff);
+	IWL_DEBUG_ISR(priv, "Disabled interrupts\n");
+}
+
+static inline void iwl_legacy_enable_interrupts(struct iwl_priv *priv)
+{
+	IWL_DEBUG_ISR(priv, "Enabling interrupts\n");
+	set_bit(STATUS_INT_ENABLED, &priv->status);
+	iwl_write32(priv, CSR_INT_MASK, priv->inta_mask);
+}
+
+/**
+ * iwl_legacy_beacon_time_mask_low - mask of lower 32 bit of beacon time
+ * @priv -- pointer to iwl_priv data structure
+ * @tsf_bits -- number of bits need to shift for masking)
+ */
+static inline u32 iwl_legacy_beacon_time_mask_low(struct iwl_priv *priv,
+					   u16 tsf_bits)
+{
+	return (1 << tsf_bits) - 1;
+}
+
+/**
+ * iwl_legacy_beacon_time_mask_high - mask of higher 32 bit of beacon time
+ * @priv -- pointer to iwl_priv data structure
+ * @tsf_bits -- number of bits need to shift for masking)
+ */
+static inline u32 iwl_legacy_beacon_time_mask_high(struct iwl_priv *priv,
+					    u16 tsf_bits)
+{
+	return ((1 << (32 - tsf_bits)) - 1) << tsf_bits;
+}
+
+#endif				/* __iwl_legacy_helpers_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-io.h b/drivers/net/wireless/iwlegacy/iwl-io.h
new file mode 100644
index 000000000000..5cc5d342914f
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-io.h
@@ -0,0 +1,545 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_io_h__
+#define __iwl_legacy_io_h__
+
+#include <linux/io.h>
+
+#include "iwl-dev.h"
+#include "iwl-debug.h"
+#include "iwl-devtrace.h"
+
+/*
+ * IO, register, and NIC memory access functions
+ *
+ * NOTE on naming convention and macro usage for these
+ *
+ * A single _ prefix before a an access function means that no state
+ * check or debug information is printed when that function is called.
+ *
+ * A double __ prefix before an access function means that state is checked
+ * and the current line number and caller function name are printed in addition
+ * to any other debug output.
+ *
+ * The non-prefixed name is the #define that maps the caller into a
+ * #define that provides the caller's name and __LINE__ to the double
+ * prefix version.
+ *
+ * If you wish to call the function without any debug or state checking,
+ * you should use the single _ prefix version (as is used by dependent IO
+ * routines, for example _iwl_legacy_read_direct32 calls the non-check version of
+ * _iwl_legacy_read32.)
+ *
+ * These declarations are *extremely* useful in quickly isolating code deltas
+ * which result in misconfiguration of the hardware I/O.  In combination with
+ * git-bisect and the IO debug level you can quickly determine the specific
+ * commit which breaks the IO sequence to the hardware.
+ *
+ */
+
+static inline void _iwl_legacy_write8(struct iwl_priv *priv, u32 ofs, u8 val)
+{
+	trace_iwlwifi_legacy_dev_iowrite8(priv, ofs, val);
+	iowrite8(val, priv->hw_base + ofs);
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline void
+__iwl_legacy_write8(const char *f, u32 l, struct iwl_priv *priv,
+				 u32 ofs, u8 val)
+{
+	IWL_DEBUG_IO(priv, "write8(0x%08X, 0x%02X) - %s %d\n", ofs, val, f, l);
+	_iwl_legacy_write8(priv, ofs, val);
+}
+#define iwl_write8(priv, ofs, val) \
+	__iwl_legacy_write8(__FILE__, __LINE__, priv, ofs, val)
+#else
+#define iwl_write8(priv, ofs, val) _iwl_legacy_write8(priv, ofs, val)
+#endif
+
+
+static inline void _iwl_legacy_write32(struct iwl_priv *priv, u32 ofs, u32 val)
+{
+	trace_iwlwifi_legacy_dev_iowrite32(priv, ofs, val);
+	iowrite32(val, priv->hw_base + ofs);
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline void
+__iwl_legacy_write32(const char *f, u32 l, struct iwl_priv *priv,
+				 u32 ofs, u32 val)
+{
+	IWL_DEBUG_IO(priv, "write32(0x%08X, 0x%08X) - %s %d\n", ofs, val, f, l);
+	_iwl_legacy_write32(priv, ofs, val);
+}
+#define iwl_write32(priv, ofs, val) \
+	__iwl_legacy_write32(__FILE__, __LINE__, priv, ofs, val)
+#else
+#define iwl_write32(priv, ofs, val) _iwl_legacy_write32(priv, ofs, val)
+#endif
+
+static inline u32 _iwl_legacy_read32(struct iwl_priv *priv, u32 ofs)
+{
+	u32 val = ioread32(priv->hw_base + ofs);
+	trace_iwlwifi_legacy_dev_ioread32(priv, ofs, val);
+	return val;
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline u32
+__iwl_legacy_read32(char *f, u32 l, struct iwl_priv *priv, u32 ofs)
+{
+	IWL_DEBUG_IO(priv, "read_direct32(0x%08X) - %s %d\n", ofs, f, l);
+	return _iwl_legacy_read32(priv, ofs);
+}
+#define iwl_read32(priv, ofs) __iwl_legacy_read32(__FILE__, __LINE__, priv, ofs)
+#else
+#define iwl_read32(p, o) _iwl_legacy_read32(p, o)
+#endif
+
+#define IWL_POLL_INTERVAL 10	/* microseconds */
+static inline int
+_iwl_legacy_poll_bit(struct iwl_priv *priv, u32 addr,
+				u32 bits, u32 mask, int timeout)
+{
+	int t = 0;
+
+	do {
+		if ((_iwl_legacy_read32(priv, addr) & mask) == (bits & mask))
+			return t;
+		udelay(IWL_POLL_INTERVAL);
+		t += IWL_POLL_INTERVAL;
+	} while (t < timeout);
+
+	return -ETIMEDOUT;
+}
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline int __iwl_legacy_poll_bit(const char *f, u32 l,
+				 struct iwl_priv *priv, u32 addr,
+				 u32 bits, u32 mask, int timeout)
+{
+	int ret = _iwl_legacy_poll_bit(priv, addr, bits, mask, timeout);
+	IWL_DEBUG_IO(priv, "poll_bit(0x%08X, 0x%08X, 0x%08X) - %s- %s %d\n",
+		     addr, bits, mask,
+		     unlikely(ret  == -ETIMEDOUT) ? "timeout" : "", f, l);
+	return ret;
+}
+#define iwl_poll_bit(priv, addr, bits, mask, timeout) \
+	__iwl_legacy_poll_bit(__FILE__, __LINE__, priv, addr, \
+	bits, mask, timeout)
+#else
+#define iwl_poll_bit(p, a, b, m, t) _iwl_legacy_poll_bit(p, a, b, m, t)
+#endif
+
+static inline void _iwl_legacy_set_bit(struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	_iwl_legacy_write32(priv, reg, _iwl_legacy_read32(priv, reg) | mask);
+}
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline void __iwl_legacy_set_bit(const char *f, u32 l,
+				 struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	u32 val = _iwl_legacy_read32(priv, reg) | mask;
+	IWL_DEBUG_IO(priv, "set_bit(0x%08X, 0x%08X) = 0x%08X\n", reg,
+							mask, val);
+	_iwl_legacy_write32(priv, reg, val);
+}
+static inline void iwl_legacy_set_bit(struct iwl_priv *p, u32 r, u32 m)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&p->reg_lock, reg_flags);
+	__iwl_legacy_set_bit(__FILE__, __LINE__, p, r, m);
+	spin_unlock_irqrestore(&p->reg_lock, reg_flags);
+}
+#else
+static inline void iwl_legacy_set_bit(struct iwl_priv *p, u32 r, u32 m)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&p->reg_lock, reg_flags);
+	_iwl_legacy_set_bit(p, r, m);
+	spin_unlock_irqrestore(&p->reg_lock, reg_flags);
+}
+#endif
+
+static inline void
+_iwl_legacy_clear_bit(struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	_iwl_legacy_write32(priv, reg, _iwl_legacy_read32(priv, reg) & ~mask);
+}
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline void
+__iwl_legacy_clear_bit(const char *f, u32 l,
+				   struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	u32 val = _iwl_legacy_read32(priv, reg) & ~mask;
+	IWL_DEBUG_IO(priv, "clear_bit(0x%08X, 0x%08X) = 0x%08X\n", reg, mask, val);
+	_iwl_legacy_write32(priv, reg, val);
+}
+static inline void iwl_legacy_clear_bit(struct iwl_priv *p, u32 r, u32 m)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&p->reg_lock, reg_flags);
+	__iwl_legacy_clear_bit(__FILE__, __LINE__, p, r, m);
+	spin_unlock_irqrestore(&p->reg_lock, reg_flags);
+}
+#else
+static inline void iwl_legacy_clear_bit(struct iwl_priv *p, u32 r, u32 m)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&p->reg_lock, reg_flags);
+	_iwl_legacy_clear_bit(p, r, m);
+	spin_unlock_irqrestore(&p->reg_lock, reg_flags);
+}
+#endif
+
+static inline int _iwl_legacy_grab_nic_access(struct iwl_priv *priv)
+{
+	int ret;
+	u32 val;
+
+	/* this bit wakes up the NIC */
+	_iwl_legacy_set_bit(priv, CSR_GP_CNTRL,
+				CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+
+	/*
+	 * These bits say the device is running, and should keep running for
+	 * at least a short while (at least as long as MAC_ACCESS_REQ stays 1),
+	 * but they do not indicate that embedded SRAM is restored yet;
+	 * 3945 and 4965 have volatile SRAM, and must save/restore contents
+	 * to/from host DRAM when sleeping/waking for power-saving.
+	 * Each direction takes approximately 1/4 millisecond; with this
+	 * overhead, it's a good idea to grab and hold MAC_ACCESS_REQUEST if a
+	 * series of register accesses are expected (e.g. reading Event Log),
+	 * to keep device from sleeping.
+	 *
+	 * CSR_UCODE_DRV_GP1 register bit MAC_SLEEP == 0 indicates that
+	 * SRAM is okay/restored.  We don't check that here because this call
+	 * is just for hardware register access; but GP1 MAC_SLEEP check is a
+	 * good idea before accessing 3945/4965 SRAM (e.g. reading Event Log).
+	 *
+	 */
+	ret = _iwl_legacy_poll_bit(priv, CSR_GP_CNTRL,
+			   CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN,
+			   (CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY |
+			    CSR_GP_CNTRL_REG_FLAG_GOING_TO_SLEEP), 15000);
+	if (ret < 0) {
+		val = _iwl_legacy_read32(priv, CSR_GP_CNTRL);
+		IWL_ERR(priv,
+			"MAC is in deep sleep!.  CSR_GP_CNTRL = 0x%08X\n", val);
+		_iwl_legacy_write32(priv, CSR_RESET,
+				CSR_RESET_REG_FLAG_FORCE_NMI);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline int __iwl_legacy_grab_nic_access(const char *f, u32 l,
+					       struct iwl_priv *priv)
+{
+	IWL_DEBUG_IO(priv, "grabbing nic access - %s %d\n", f, l);
+	return _iwl_legacy_grab_nic_access(priv);
+}
+#define iwl_grab_nic_access(priv) \
+	__iwl_legacy_grab_nic_access(__FILE__, __LINE__, priv)
+#else
+#define iwl_grab_nic_access(priv) \
+	_iwl_legacy_grab_nic_access(priv)
+#endif
+
+static inline void _iwl_legacy_release_nic_access(struct iwl_priv *priv)
+{
+	_iwl_legacy_clear_bit(priv, CSR_GP_CNTRL,
+			CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+}
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline void __iwl_legacy_release_nic_access(const char *f, u32 l,
+					    struct iwl_priv *priv)
+{
+
+	IWL_DEBUG_IO(priv, "releasing nic access - %s %d\n", f, l);
+	_iwl_legacy_release_nic_access(priv);
+}
+#define iwl_release_nic_access(priv) \
+	__iwl_legacy_release_nic_access(__FILE__, __LINE__, priv)
+#else
+#define iwl_release_nic_access(priv) \
+	_iwl_legacy_release_nic_access(priv)
+#endif
+
+static inline u32 _iwl_legacy_read_direct32(struct iwl_priv *priv, u32 reg)
+{
+	return _iwl_legacy_read32(priv, reg);
+}
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline u32 __iwl_legacy_read_direct32(const char *f, u32 l,
+					struct iwl_priv *priv, u32 reg)
+{
+	u32 value = _iwl_legacy_read_direct32(priv, reg);
+	IWL_DEBUG_IO(priv,
+			"read_direct32(0x%4X) = 0x%08x - %s %d\n", reg, value,
+		     f, l);
+	return value;
+}
+static inline u32 iwl_legacy_read_direct32(struct iwl_priv *priv, u32 reg)
+{
+	u32 value;
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+	value = __iwl_legacy_read_direct32(__FILE__, __LINE__, priv, reg);
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+	return value;
+}
+
+#else
+static inline u32 iwl_legacy_read_direct32(struct iwl_priv *priv, u32 reg)
+{
+	u32 value;
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+	value = _iwl_legacy_read_direct32(priv, reg);
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+	return value;
+
+}
+#endif
+
+static inline void _iwl_legacy_write_direct32(struct iwl_priv *priv,
+					 u32 reg, u32 value)
+{
+	_iwl_legacy_write32(priv, reg, value);
+}
+static inline void
+iwl_legacy_write_direct32(struct iwl_priv *priv, u32 reg, u32 value)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	if (!iwl_grab_nic_access(priv)) {
+		_iwl_legacy_write_direct32(priv, reg, value);
+		iwl_release_nic_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+static inline void iwl_legacy_write_reg_buf(struct iwl_priv *priv,
+					       u32 reg, u32 len, u32 *values)
+{
+	u32 count = sizeof(u32);
+
+	if ((priv != NULL) && (values != NULL)) {
+		for (; 0 < len; len -= count, reg += count, values++)
+			iwl_legacy_write_direct32(priv, reg, *values);
+	}
+}
+
+static inline int _iwl_legacy_poll_direct_bit(struct iwl_priv *priv, u32 addr,
+				       u32 mask, int timeout)
+{
+	int t = 0;
+
+	do {
+		if ((iwl_legacy_read_direct32(priv, addr) & mask) == mask)
+			return t;
+		udelay(IWL_POLL_INTERVAL);
+		t += IWL_POLL_INTERVAL;
+	} while (t < timeout);
+
+	return -ETIMEDOUT;
+}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static inline int __iwl_legacy_poll_direct_bit(const char *f, u32 l,
+					    struct iwl_priv *priv,
+					    u32 addr, u32 mask, int timeout)
+{
+	int ret  = _iwl_legacy_poll_direct_bit(priv, addr, mask, timeout);
+
+	if (unlikely(ret == -ETIMEDOUT))
+		IWL_DEBUG_IO(priv, "poll_direct_bit(0x%08X, 0x%08X) - "
+			     "timedout - %s %d\n", addr, mask, f, l);
+	else
+		IWL_DEBUG_IO(priv, "poll_direct_bit(0x%08X, 0x%08X) = 0x%08X "
+			     "- %s %d\n", addr, mask, ret, f, l);
+	return ret;
+}
+#define iwl_poll_direct_bit(priv, addr, mask, timeout) \
+__iwl_legacy_poll_direct_bit(__FILE__, __LINE__, priv, addr, mask, timeout)
+#else
+#define iwl_poll_direct_bit _iwl_legacy_poll_direct_bit
+#endif
+
+static inline u32 _iwl_legacy_read_prph(struct iwl_priv *priv, u32 reg)
+{
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_PRPH_RADDR, reg | (3 << 24));
+	rmb();
+	return _iwl_legacy_read_direct32(priv, HBUS_TARG_PRPH_RDAT);
+}
+static inline u32 iwl_legacy_read_prph(struct iwl_priv *priv, u32 reg)
+{
+	unsigned long reg_flags;
+	u32 val;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+	val = _iwl_legacy_read_prph(priv, reg);
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+	return val;
+}
+
+static inline void _iwl_legacy_write_prph(struct iwl_priv *priv,
+					     u32 addr, u32 val)
+{
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_PRPH_WADDR,
+			      ((addr & 0x0000FFFF) | (3 << 24)));
+	wmb();
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_PRPH_WDAT, val);
+}
+
+static inline void
+iwl_legacy_write_prph(struct iwl_priv *priv, u32 addr, u32 val)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	if (!iwl_grab_nic_access(priv)) {
+		_iwl_legacy_write_prph(priv, addr, val);
+		iwl_release_nic_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+#define _iwl_legacy_set_bits_prph(priv, reg, mask) \
+_iwl_legacy_write_prph(priv, reg, (_iwl_legacy_read_prph(priv, reg) | mask))
+
+static inline void
+iwl_legacy_set_bits_prph(struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+	_iwl_legacy_set_bits_prph(priv, reg, mask);
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+#define _iwl_legacy_set_bits_mask_prph(priv, reg, bits, mask) \
+_iwl_legacy_write_prph(priv, reg,				\
+		 ((_iwl_legacy_read_prph(priv, reg) & mask) | bits))
+
+static inline void iwl_legacy_set_bits_mask_prph(struct iwl_priv *priv, u32 reg,
+				u32 bits, u32 mask)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+	_iwl_legacy_set_bits_mask_prph(priv, reg, bits, mask);
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+static inline void iwl_legacy_clear_bits_prph(struct iwl_priv
+						 *priv, u32 reg, u32 mask)
+{
+	unsigned long reg_flags;
+	u32 val;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+	val = _iwl_legacy_read_prph(priv, reg);
+	_iwl_legacy_write_prph(priv, reg, (val & ~mask));
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+static inline u32 iwl_legacy_read_targ_mem(struct iwl_priv *priv, u32 addr)
+{
+	unsigned long reg_flags;
+	u32 value;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR, addr);
+	rmb();
+	value = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+	return value;
+}
+
+static inline void
+iwl_legacy_write_targ_mem(struct iwl_priv *priv, u32 addr, u32 val)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	if (!iwl_grab_nic_access(priv)) {
+		_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_WADDR, addr);
+		wmb();
+		_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_WDAT, val);
+		iwl_release_nic_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+static inline void
+iwl_legacy_write_targ_mem_buf(struct iwl_priv *priv, u32 addr,
+					  u32 len, u32 *values)
+{
+	unsigned long reg_flags;
+
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	if (!iwl_grab_nic_access(priv)) {
+		_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_WADDR, addr);
+		wmb();
+		for (; 0 < len; len -= sizeof(u32), values++)
+			_iwl_legacy_write_direct32(priv,
+					HBUS_TARG_MEM_WDAT, *values);
+
+		iwl_release_nic_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+#endif
diff --git a/drivers/net/wireless/iwlegacy/iwl-led.c b/drivers/net/wireless/iwlegacy/iwl-led.c
new file mode 100644
index 000000000000..15eb8b707157
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-led.c
@@ -0,0 +1,188 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+#include <asm/unaligned.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+
+/* default: IWL_LED_BLINK(0) using blinking index table */
+static int led_mode;
+module_param(led_mode, int, S_IRUGO);
+MODULE_PARM_DESC(led_mode, "0=system default, "
+		"1=On(RF On)/Off(RF Off), 2=blinking");
+
+static const struct ieee80211_tpt_blink iwl_blink[] = {
+	{ .throughput = 0 * 1024 - 1, .blink_time = 334 },
+	{ .throughput = 1 * 1024 - 1, .blink_time = 260 },
+	{ .throughput = 5 * 1024 - 1, .blink_time = 220 },
+	{ .throughput = 10 * 1024 - 1, .blink_time = 190 },
+	{ .throughput = 20 * 1024 - 1, .blink_time = 170 },
+	{ .throughput = 50 * 1024 - 1, .blink_time = 150 },
+	{ .throughput = 70 * 1024 - 1, .blink_time = 130 },
+	{ .throughput = 100 * 1024 - 1, .blink_time = 110 },
+	{ .throughput = 200 * 1024 - 1, .blink_time = 80 },
+	{ .throughput = 300 * 1024 - 1, .blink_time = 50 },
+};
+
+/*
+ * Adjust led blink rate to compensate on a MAC Clock difference on every HW
+ * Led blink rate analysis showed an average deviation of 0% on 3945,
+ * 5% on 4965 HW.
+ * Need to compensate on the led on/off time per HW according to the deviation
+ * to achieve the desired led frequency
+ * The calculation is: (100-averageDeviation)/100 * blinkTime
+ * For code efficiency the calculation will be:
+ *     compensation = (100 - averageDeviation) * 64 / 100
+ *     NewBlinkTime = (compensation * BlinkTime) / 64
+ */
+static inline u8 iwl_legacy_blink_compensation(struct iwl_priv *priv,
+				    u8 time, u16 compensation)
+{
+	if (!compensation) {
+		IWL_ERR(priv, "undefined blink compensation: "
+			"use pre-defined blinking time\n");
+		return time;
+	}
+
+	return (u8)((time * compensation) >> 6);
+}
+
+/* Set led pattern command */
+static int iwl_legacy_led_cmd(struct iwl_priv *priv,
+		       unsigned long on,
+		       unsigned long off)
+{
+	struct iwl_led_cmd led_cmd = {
+		.id = IWL_LED_LINK,
+		.interval = IWL_DEF_LED_INTRVL
+	};
+	int ret;
+
+	if (!test_bit(STATUS_READY, &priv->status))
+		return -EBUSY;
+
+	if (priv->blink_on == on && priv->blink_off == off)
+		return 0;
+
+	IWL_DEBUG_LED(priv, "Led blink time compensation=%u\n",
+			priv->cfg->base_params->led_compensation);
+	led_cmd.on = iwl_legacy_blink_compensation(priv, on,
+				priv->cfg->base_params->led_compensation);
+	led_cmd.off = iwl_legacy_blink_compensation(priv, off,
+				priv->cfg->base_params->led_compensation);
+
+	ret = priv->cfg->ops->led->cmd(priv, &led_cmd);
+	if (!ret) {
+		priv->blink_on = on;
+		priv->blink_off = off;
+	}
+	return ret;
+}
+
+static void iwl_legacy_led_brightness_set(struct led_classdev *led_cdev,
+				   enum led_brightness brightness)
+{
+	struct iwl_priv *priv = container_of(led_cdev, struct iwl_priv, led);
+	unsigned long on = 0;
+
+	if (brightness > 0)
+		on = IWL_LED_SOLID;
+
+	iwl_legacy_led_cmd(priv, on, 0);
+}
+
+static int iwl_legacy_led_blink_set(struct led_classdev *led_cdev,
+			     unsigned long *delay_on,
+			     unsigned long *delay_off)
+{
+	struct iwl_priv *priv = container_of(led_cdev, struct iwl_priv, led);
+
+	return iwl_legacy_led_cmd(priv, *delay_on, *delay_off);
+}
+
+void iwl_legacy_leds_init(struct iwl_priv *priv)
+{
+	int mode = led_mode;
+	int ret;
+
+	if (mode == IWL_LED_DEFAULT)
+		mode = priv->cfg->led_mode;
+
+	priv->led.name = kasprintf(GFP_KERNEL, "%s-led",
+				   wiphy_name(priv->hw->wiphy));
+	priv->led.brightness_set = iwl_legacy_led_brightness_set;
+	priv->led.blink_set = iwl_legacy_led_blink_set;
+	priv->led.max_brightness = 1;
+
+	switch (mode) {
+	case IWL_LED_DEFAULT:
+		WARN_ON(1);
+		break;
+	case IWL_LED_BLINK:
+		priv->led.default_trigger =
+			ieee80211_create_tpt_led_trigger(priv->hw,
+					IEEE80211_TPT_LEDTRIG_FL_CONNECTED,
+					iwl_blink, ARRAY_SIZE(iwl_blink));
+		break;
+	case IWL_LED_RF_STATE:
+		priv->led.default_trigger =
+			ieee80211_get_radio_led_name(priv->hw);
+		break;
+	}
+
+	ret = led_classdev_register(&priv->pci_dev->dev, &priv->led);
+	if (ret) {
+		kfree(priv->led.name);
+		return;
+	}
+
+	priv->led_registered = true;
+}
+EXPORT_SYMBOL(iwl_legacy_leds_init);
+
+void iwl_legacy_leds_exit(struct iwl_priv *priv)
+{
+	if (!priv->led_registered)
+		return;
+
+	led_classdev_unregister(&priv->led);
+	kfree(priv->led.name);
+}
+EXPORT_SYMBOL(iwl_legacy_leds_exit);
diff --git a/drivers/net/wireless/iwlegacy/iwl-led.h b/drivers/net/wireless/iwlegacy/iwl-led.h
new file mode 100644
index 000000000000..f0791f70f79d
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-led.h
@@ -0,0 +1,56 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_leds_h__
+#define __iwl_legacy_leds_h__
+
+
+struct iwl_priv;
+
+#define IWL_LED_SOLID 11
+#define IWL_DEF_LED_INTRVL cpu_to_le32(1000)
+
+#define IWL_LED_ACTIVITY       (0<<1)
+#define IWL_LED_LINK           (1<<1)
+
+/*
+ * LED mode
+ *    IWL_LED_DEFAULT:  use device default
+ *    IWL_LED_RF_STATE: turn LED on/off based on RF state
+ *			LED ON  = RF ON
+ *			LED OFF = RF OFF
+ *    IWL_LED_BLINK:    adjust led blink rate based on blink table
+ */
+enum iwl_led_mode {
+	IWL_LED_DEFAULT,
+	IWL_LED_RF_STATE,
+	IWL_LED_BLINK,
+};
+
+void iwl_legacy_leds_init(struct iwl_priv *priv);
+void iwl_legacy_leds_exit(struct iwl_priv *priv);
+
+#endif /* __iwl_legacy_leds_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-legacy-rs.h b/drivers/net/wireless/iwlegacy/iwl-legacy-rs.h
new file mode 100644
index 000000000000..38647e481eb0
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-legacy-rs.h
@@ -0,0 +1,456 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_rs_h__
+#define __iwl_legacy_rs_h__
+
+struct iwl_rate_info {
+	u8 plcp;	/* uCode API:  IWL_RATE_6M_PLCP, etc. */
+	u8 plcp_siso;	/* uCode API:  IWL_RATE_SISO_6M_PLCP, etc. */
+	u8 plcp_mimo2;	/* uCode API:  IWL_RATE_MIMO2_6M_PLCP, etc. */
+	u8 ieee;	/* MAC header:  IWL_RATE_6M_IEEE, etc. */
+	u8 prev_ieee;    /* previous rate in IEEE speeds */
+	u8 next_ieee;    /* next rate in IEEE speeds */
+	u8 prev_rs;      /* previous rate used in rs algo */
+	u8 next_rs;      /* next rate used in rs algo */
+	u8 prev_rs_tgg;  /* previous rate used in TGG rs algo */
+	u8 next_rs_tgg;  /* next rate used in TGG rs algo */
+};
+
+struct iwl3945_rate_info {
+	u8 plcp;		/* uCode API:  IWL_RATE_6M_PLCP, etc. */
+	u8 ieee;		/* MAC header:  IWL_RATE_6M_IEEE, etc. */
+	u8 prev_ieee;		/* previous rate in IEEE speeds */
+	u8 next_ieee;		/* next rate in IEEE speeds */
+	u8 prev_rs;		/* previous rate used in rs algo */
+	u8 next_rs;		/* next rate used in rs algo */
+	u8 prev_rs_tgg;		/* previous rate used in TGG rs algo */
+	u8 next_rs_tgg;		/* next rate used in TGG rs algo */
+	u8 table_rs_index;	/* index in rate scale table cmd */
+	u8 prev_table_rs;	/* prev in rate table cmd */
+};
+
+
+/*
+ * These serve as indexes into
+ * struct iwl_rate_info iwlegacy_rates[IWL_RATE_COUNT];
+ */
+enum {
+	IWL_RATE_1M_INDEX = 0,
+	IWL_RATE_2M_INDEX,
+	IWL_RATE_5M_INDEX,
+	IWL_RATE_11M_INDEX,
+	IWL_RATE_6M_INDEX,
+	IWL_RATE_9M_INDEX,
+	IWL_RATE_12M_INDEX,
+	IWL_RATE_18M_INDEX,
+	IWL_RATE_24M_INDEX,
+	IWL_RATE_36M_INDEX,
+	IWL_RATE_48M_INDEX,
+	IWL_RATE_54M_INDEX,
+	IWL_RATE_60M_INDEX,
+	IWL_RATE_COUNT,
+	IWL_RATE_COUNT_LEGACY = IWL_RATE_COUNT - 1,	/* Excluding 60M */
+	IWL_RATE_COUNT_3945 = IWL_RATE_COUNT - 1,
+	IWL_RATE_INVM_INDEX = IWL_RATE_COUNT,
+	IWL_RATE_INVALID = IWL_RATE_COUNT,
+};
+
+enum {
+	IWL_RATE_6M_INDEX_TABLE = 0,
+	IWL_RATE_9M_INDEX_TABLE,
+	IWL_RATE_12M_INDEX_TABLE,
+	IWL_RATE_18M_INDEX_TABLE,
+	IWL_RATE_24M_INDEX_TABLE,
+	IWL_RATE_36M_INDEX_TABLE,
+	IWL_RATE_48M_INDEX_TABLE,
+	IWL_RATE_54M_INDEX_TABLE,
+	IWL_RATE_1M_INDEX_TABLE,
+	IWL_RATE_2M_INDEX_TABLE,
+	IWL_RATE_5M_INDEX_TABLE,
+	IWL_RATE_11M_INDEX_TABLE,
+	IWL_RATE_INVM_INDEX_TABLE = IWL_RATE_INVM_INDEX - 1,
+};
+
+enum {
+	IWL_FIRST_OFDM_RATE = IWL_RATE_6M_INDEX,
+	IWL39_LAST_OFDM_RATE = IWL_RATE_54M_INDEX,
+	IWL_LAST_OFDM_RATE = IWL_RATE_60M_INDEX,
+	IWL_FIRST_CCK_RATE = IWL_RATE_1M_INDEX,
+	IWL_LAST_CCK_RATE = IWL_RATE_11M_INDEX,
+};
+
+/* #define vs. enum to keep from defaulting to 'large integer' */
+#define	IWL_RATE_6M_MASK   (1 << IWL_RATE_6M_INDEX)
+#define	IWL_RATE_9M_MASK   (1 << IWL_RATE_9M_INDEX)
+#define	IWL_RATE_12M_MASK  (1 << IWL_RATE_12M_INDEX)
+#define	IWL_RATE_18M_MASK  (1 << IWL_RATE_18M_INDEX)
+#define	IWL_RATE_24M_MASK  (1 << IWL_RATE_24M_INDEX)
+#define	IWL_RATE_36M_MASK  (1 << IWL_RATE_36M_INDEX)
+#define	IWL_RATE_48M_MASK  (1 << IWL_RATE_48M_INDEX)
+#define	IWL_RATE_54M_MASK  (1 << IWL_RATE_54M_INDEX)
+#define IWL_RATE_60M_MASK  (1 << IWL_RATE_60M_INDEX)
+#define	IWL_RATE_1M_MASK   (1 << IWL_RATE_1M_INDEX)
+#define	IWL_RATE_2M_MASK   (1 << IWL_RATE_2M_INDEX)
+#define	IWL_RATE_5M_MASK   (1 << IWL_RATE_5M_INDEX)
+#define	IWL_RATE_11M_MASK  (1 << IWL_RATE_11M_INDEX)
+
+/* uCode API values for legacy bit rates, both OFDM and CCK */
+enum {
+	IWL_RATE_6M_PLCP  = 13,
+	IWL_RATE_9M_PLCP  = 15,
+	IWL_RATE_12M_PLCP = 5,
+	IWL_RATE_18M_PLCP = 7,
+	IWL_RATE_24M_PLCP = 9,
+	IWL_RATE_36M_PLCP = 11,
+	IWL_RATE_48M_PLCP = 1,
+	IWL_RATE_54M_PLCP = 3,
+	IWL_RATE_60M_PLCP = 3,/*FIXME:RS:should be removed*/
+	IWL_RATE_1M_PLCP  = 10,
+	IWL_RATE_2M_PLCP  = 20,
+	IWL_RATE_5M_PLCP  = 55,
+	IWL_RATE_11M_PLCP = 110,
+	/*FIXME:RS:add IWL_RATE_LEGACY_INVM_PLCP = 0,*/
+};
+
+/* uCode API values for OFDM high-throughput (HT) bit rates */
+enum {
+	IWL_RATE_SISO_6M_PLCP = 0,
+	IWL_RATE_SISO_12M_PLCP = 1,
+	IWL_RATE_SISO_18M_PLCP = 2,
+	IWL_RATE_SISO_24M_PLCP = 3,
+	IWL_RATE_SISO_36M_PLCP = 4,
+	IWL_RATE_SISO_48M_PLCP = 5,
+	IWL_RATE_SISO_54M_PLCP = 6,
+	IWL_RATE_SISO_60M_PLCP = 7,
+	IWL_RATE_MIMO2_6M_PLCP  = 0x8,
+	IWL_RATE_MIMO2_12M_PLCP = 0x9,
+	IWL_RATE_MIMO2_18M_PLCP = 0xa,
+	IWL_RATE_MIMO2_24M_PLCP = 0xb,
+	IWL_RATE_MIMO2_36M_PLCP = 0xc,
+	IWL_RATE_MIMO2_48M_PLCP = 0xd,
+	IWL_RATE_MIMO2_54M_PLCP = 0xe,
+	IWL_RATE_MIMO2_60M_PLCP = 0xf,
+	IWL_RATE_SISO_INVM_PLCP,
+	IWL_RATE_MIMO2_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
+};
+
+/* MAC header values for bit rates */
+enum {
+	IWL_RATE_6M_IEEE  = 12,
+	IWL_RATE_9M_IEEE  = 18,
+	IWL_RATE_12M_IEEE = 24,
+	IWL_RATE_18M_IEEE = 36,
+	IWL_RATE_24M_IEEE = 48,
+	IWL_RATE_36M_IEEE = 72,
+	IWL_RATE_48M_IEEE = 96,
+	IWL_RATE_54M_IEEE = 108,
+	IWL_RATE_60M_IEEE = 120,
+	IWL_RATE_1M_IEEE  = 2,
+	IWL_RATE_2M_IEEE  = 4,
+	IWL_RATE_5M_IEEE  = 11,
+	IWL_RATE_11M_IEEE = 22,
+};
+
+#define IWL_CCK_BASIC_RATES_MASK    \
+	(IWL_RATE_1M_MASK          | \
+	IWL_RATE_2M_MASK)
+
+#define IWL_CCK_RATES_MASK          \
+	(IWL_CCK_BASIC_RATES_MASK  | \
+	IWL_RATE_5M_MASK          | \
+	IWL_RATE_11M_MASK)
+
+#define IWL_OFDM_BASIC_RATES_MASK   \
+	(IWL_RATE_6M_MASK         | \
+	IWL_RATE_12M_MASK         | \
+	IWL_RATE_24M_MASK)
+
+#define IWL_OFDM_RATES_MASK         \
+	(IWL_OFDM_BASIC_RATES_MASK | \
+	IWL_RATE_9M_MASK          | \
+	IWL_RATE_18M_MASK         | \
+	IWL_RATE_36M_MASK         | \
+	IWL_RATE_48M_MASK         | \
+	IWL_RATE_54M_MASK)
+
+#define IWL_BASIC_RATES_MASK         \
+	(IWL_OFDM_BASIC_RATES_MASK | \
+	 IWL_CCK_BASIC_RATES_MASK)
+
+#define IWL_RATES_MASK ((1 << IWL_RATE_COUNT) - 1)
+#define IWL_RATES_MASK_3945 ((1 << IWL_RATE_COUNT_3945) - 1)
+
+#define IWL_INVALID_VALUE    -1
+
+#define IWL_MIN_RSSI_VAL                 -100
+#define IWL_MAX_RSSI_VAL                    0
+
+/* These values specify how many Tx frame attempts before
+ * searching for a new modulation mode */
+#define IWL_LEGACY_FAILURE_LIMIT	160
+#define IWL_LEGACY_SUCCESS_LIMIT	480
+#define IWL_LEGACY_TABLE_COUNT		160
+
+#define IWL_NONE_LEGACY_FAILURE_LIMIT	400
+#define IWL_NONE_LEGACY_SUCCESS_LIMIT	4500
+#define IWL_NONE_LEGACY_TABLE_COUNT	1500
+
+/* Success ratio (ACKed / attempted tx frames) values (perfect is 128 * 100) */
+#define IWL_RS_GOOD_RATIO		12800	/* 100% */
+#define IWL_RATE_SCALE_SWITCH		10880	/*  85% */
+#define IWL_RATE_HIGH_TH		10880	/*  85% */
+#define IWL_RATE_INCREASE_TH		6400	/*  50% */
+#define IWL_RATE_DECREASE_TH		1920	/*  15% */
+
+/* possible actions when in legacy mode */
+#define IWL_LEGACY_SWITCH_ANTENNA1      0
+#define IWL_LEGACY_SWITCH_ANTENNA2      1
+#define IWL_LEGACY_SWITCH_SISO          2
+#define IWL_LEGACY_SWITCH_MIMO2_AB      3
+#define IWL_LEGACY_SWITCH_MIMO2_AC      4
+#define IWL_LEGACY_SWITCH_MIMO2_BC      5
+
+/* possible actions when in siso mode */
+#define IWL_SISO_SWITCH_ANTENNA1        0
+#define IWL_SISO_SWITCH_ANTENNA2        1
+#define IWL_SISO_SWITCH_MIMO2_AB        2
+#define IWL_SISO_SWITCH_MIMO2_AC        3
+#define IWL_SISO_SWITCH_MIMO2_BC        4
+#define IWL_SISO_SWITCH_GI              5
+
+/* possible actions when in mimo mode */
+#define IWL_MIMO2_SWITCH_ANTENNA1       0
+#define IWL_MIMO2_SWITCH_ANTENNA2       1
+#define IWL_MIMO2_SWITCH_SISO_A         2
+#define IWL_MIMO2_SWITCH_SISO_B         3
+#define IWL_MIMO2_SWITCH_SISO_C         4
+#define IWL_MIMO2_SWITCH_GI             5
+
+#define IWL_MAX_SEARCH IWL_MIMO2_SWITCH_GI
+
+#define IWL_ACTION_LIMIT		3	/* # possible actions */
+
+#define LQ_SIZE		2	/* 2 mode tables:  "Active" and "Search" */
+
+/* load per tid defines for A-MPDU activation */
+#define IWL_AGG_TPT_THREHOLD	0
+#define IWL_AGG_LOAD_THRESHOLD	10
+#define IWL_AGG_ALL_TID		0xff
+#define TID_QUEUE_CELL_SPACING	50	/*mS */
+#define TID_QUEUE_MAX_SIZE	20
+#define TID_ROUND_VALUE		5	/* mS */
+#define TID_MAX_LOAD_COUNT	8
+
+#define TID_MAX_TIME_DIFF ((TID_QUEUE_MAX_SIZE - 1) * TID_QUEUE_CELL_SPACING)
+#define TIME_WRAP_AROUND(x, y) (((y) > (x)) ? (y) - (x) : (0-(x)) + (y))
+
+extern const struct iwl_rate_info iwlegacy_rates[IWL_RATE_COUNT];
+
+enum iwl_table_type {
+	LQ_NONE,
+	LQ_G,		/* legacy types */
+	LQ_A,
+	LQ_SISO,	/* high-throughput types */
+	LQ_MIMO2,
+	LQ_MAX,
+};
+
+#define is_legacy(tbl) (((tbl) == LQ_G) || ((tbl) == LQ_A))
+#define is_siso(tbl) ((tbl) == LQ_SISO)
+#define is_mimo2(tbl) ((tbl) == LQ_MIMO2)
+#define is_mimo(tbl) (is_mimo2(tbl))
+#define is_Ht(tbl) (is_siso(tbl) || is_mimo(tbl))
+#define is_a_band(tbl) ((tbl) == LQ_A)
+#define is_g_and(tbl) ((tbl) == LQ_G)
+
+#define	ANT_NONE	0x0
+#define	ANT_A		BIT(0)
+#define	ANT_B		BIT(1)
+#define	ANT_AB		(ANT_A | ANT_B)
+#define ANT_C		BIT(2)
+#define	ANT_AC		(ANT_A | ANT_C)
+#define ANT_BC		(ANT_B | ANT_C)
+#define ANT_ABC		(ANT_AB | ANT_C)
+
+#define IWL_MAX_MCS_DISPLAY_SIZE	12
+
+struct iwl_rate_mcs_info {
+	char	mbps[IWL_MAX_MCS_DISPLAY_SIZE];
+	char	mcs[IWL_MAX_MCS_DISPLAY_SIZE];
+};
+
+/**
+ * struct iwl_rate_scale_data -- tx success history for one rate
+ */
+struct iwl_rate_scale_data {
+	u64 data;		/* bitmap of successful frames */
+	s32 success_counter;	/* number of frames successful */
+	s32 success_ratio;	/* per-cent * 128  */
+	s32 counter;		/* number of frames attempted */
+	s32 average_tpt;	/* success ratio * expected throughput */
+	unsigned long stamp;
+};
+
+/**
+ * struct iwl_scale_tbl_info -- tx params and success history for all rates
+ *
+ * There are two of these in struct iwl_lq_sta,
+ * one for "active", and one for "search".
+ */
+struct iwl_scale_tbl_info {
+	enum iwl_table_type lq_type;
+	u8 ant_type;
+	u8 is_SGI;	/* 1 = short guard interval */
+	u8 is_ht40;	/* 1 = 40 MHz channel width */
+	u8 is_dup;	/* 1 = duplicated data streams */
+	u8 action;	/* change modulation; IWL_[LEGACY/SISO/MIMO]_SWITCH_* */
+	u8 max_search;	/* maximun number of tables we can search */
+	s32 *expected_tpt;	/* throughput metrics; expected_tpt_G, etc. */
+	u32 current_rate;  /* rate_n_flags, uCode API format */
+	struct iwl_rate_scale_data win[IWL_RATE_COUNT]; /* rate histories */
+};
+
+struct iwl_traffic_load {
+	unsigned long time_stamp;	/* age of the oldest statistics */
+	u32 packet_count[TID_QUEUE_MAX_SIZE];   /* packet count in this time
+						 * slice */
+	u32 total;			/* total num of packets during the
+					 * last TID_MAX_TIME_DIFF */
+	u8 queue_count;			/* number of queues that has
+					 * been used since the last cleanup */
+	u8 head;			/* start of the circular buffer */
+};
+
+/**
+ * struct iwl_lq_sta -- driver's rate scaling private structure
+ *
+ * Pointer to this gets passed back and forth between driver and mac80211.
+ */
+struct iwl_lq_sta {
+	u8 active_tbl;		/* index of active table, range 0-1 */
+	u8 enable_counter;	/* indicates HT mode */
+	u8 stay_in_tbl;		/* 1: disallow, 0: allow search for new mode */
+	u8 search_better_tbl;	/* 1: currently trying alternate mode */
+	s32 last_tpt;
+
+	/* The following determine when to search for a new mode */
+	u32 table_count_limit;
+	u32 max_failure_limit;	/* # failed frames before new search */
+	u32 max_success_limit;	/* # successful frames before new search */
+	u32 table_count;
+	u32 total_failed;	/* total failed frames, any/all rates */
+	u32 total_success;	/* total successful frames, any/all rates */
+	u64 flush_timer;	/* time staying in mode before new search */
+
+	u8 action_counter;	/* # mode-switch actions tried */
+	u8 is_green;
+	u8 is_dup;
+	enum ieee80211_band band;
+
+	/* The following are bitmaps of rates; IWL_RATE_6M_MASK, etc. */
+	u32 supp_rates;
+	u16 active_legacy_rate;
+	u16 active_siso_rate;
+	u16 active_mimo2_rate;
+	s8 max_rate_idx;     /* Max rate set by user */
+	u8 missed_rate_counter;
+
+	struct iwl_link_quality_cmd lq;
+	struct iwl_scale_tbl_info lq_info[LQ_SIZE]; /* "active", "search" */
+	struct iwl_traffic_load load[TID_MAX_LOAD_COUNT];
+	u8 tx_agg_tid_en;
+#ifdef CONFIG_MAC80211_DEBUGFS
+	struct dentry *rs_sta_dbgfs_scale_table_file;
+	struct dentry *rs_sta_dbgfs_stats_table_file;
+	struct dentry *rs_sta_dbgfs_rate_scale_data_file;
+	struct dentry *rs_sta_dbgfs_tx_agg_tid_en_file;
+	u32 dbg_fixed_rate;
+#endif
+	struct iwl_priv *drv;
+
+	/* used to be in sta_info */
+	int last_txrate_idx;
+	/* last tx rate_n_flags */
+	u32 last_rate_n_flags;
+	/* packets destined for this STA are aggregated */
+	u8 is_agg;
+};
+
+static inline u8 iwl4965_num_of_ant(u8 mask)
+{
+	return  !!((mask) & ANT_A) +
+		!!((mask) & ANT_B) +
+		!!((mask) & ANT_C);
+}
+
+static inline u8 iwl4965_first_antenna(u8 mask)
+{
+	if (mask & ANT_A)
+		return ANT_A;
+	if (mask & ANT_B)
+		return ANT_B;
+	return ANT_C;
+}
+
+
+/**
+ * iwl3945_rate_scale_init - Initialize the rate scale table based on assoc info
+ *
+ * The specific throughput table used is based on the type of network
+ * the associated with, including A, B, G, and G w/ TGG protection
+ */
+extern void iwl3945_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id);
+
+/* Initialize station's rate scaling information after adding station */
+extern void iwl4965_rs_rate_init(struct iwl_priv *priv,
+			     struct ieee80211_sta *sta, u8 sta_id);
+extern void iwl3945_rs_rate_init(struct iwl_priv *priv,
+				 struct ieee80211_sta *sta, u8 sta_id);
+
+/**
+ * iwl_rate_control_register - Register the rate control algorithm callbacks
+ *
+ * Since the rate control algorithm is hardware specific, there is no need
+ * or reason to place it as a stand alone module.  The driver can call
+ * iwl_rate_control_register in order to register the rate control callbacks
+ * with the mac80211 subsystem.  This should be performed prior to calling
+ * ieee80211_register_hw
+ *
+ */
+extern int iwl4965_rate_control_register(void);
+extern int iwl3945_rate_control_register(void);
+
+/**
+ * iwl_rate_control_unregister - Unregister the rate control callbacks
+ *
+ * This should be called after calling ieee80211_unregister_hw, but before
+ * the driver is unloaded.
+ */
+extern void iwl4965_rate_control_unregister(void);
+extern void iwl3945_rate_control_unregister(void);
+
+#endif /* __iwl_legacy_rs__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-power.c b/drivers/net/wireless/iwlegacy/iwl-power.c
new file mode 100644
index 000000000000..903ef0d6d6cb
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-power.c
@@ -0,0 +1,165 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+
+#include <net/mac80211.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-commands.h"
+#include "iwl-debug.h"
+#include "iwl-power.h"
+
+/*
+ * Setting power level allows the card to go to sleep when not busy.
+ *
+ * We calculate a sleep command based on the required latency, which
+ * we get from mac80211. In order to handle thermal throttling, we can
+ * also use pre-defined power levels.
+ */
+
+/*
+ * This defines the old power levels. They are still used by default
+ * (level 1) and for thermal throttle (levels 3 through 5)
+ */
+
+struct iwl_power_vec_entry {
+	struct iwl_powertable_cmd cmd;
+	u8 no_dtim;	/* number of skip dtim */
+};
+
+static void iwl_legacy_power_sleep_cam_cmd(struct iwl_priv *priv,
+				    struct iwl_powertable_cmd *cmd)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	if (priv->power_data.pci_pm)
+		cmd->flags |= IWL_POWER_PCI_PM_MSK;
+
+	IWL_DEBUG_POWER(priv, "Sleep command for CAM\n");
+}
+
+static int
+iwl_legacy_set_power(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd)
+{
+	IWL_DEBUG_POWER(priv, "Sending power/sleep command\n");
+	IWL_DEBUG_POWER(priv, "Flags value = 0x%08X\n", cmd->flags);
+	IWL_DEBUG_POWER(priv, "Tx timeout = %u\n",
+					le32_to_cpu(cmd->tx_data_timeout));
+	IWL_DEBUG_POWER(priv, "Rx timeout = %u\n",
+					le32_to_cpu(cmd->rx_data_timeout));
+	IWL_DEBUG_POWER(priv,
+			"Sleep interval vector = { %d , %d , %d , %d , %d }\n",
+			le32_to_cpu(cmd->sleep_interval[0]),
+			le32_to_cpu(cmd->sleep_interval[1]),
+			le32_to_cpu(cmd->sleep_interval[2]),
+			le32_to_cpu(cmd->sleep_interval[3]),
+			le32_to_cpu(cmd->sleep_interval[4]));
+
+	return iwl_legacy_send_cmd_pdu(priv, POWER_TABLE_CMD,
+				sizeof(struct iwl_powertable_cmd), cmd);
+}
+
+int
+iwl_legacy_power_set_mode(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd,
+		       bool force)
+{
+	int ret;
+	bool update_chains;
+
+	lockdep_assert_held(&priv->mutex);
+
+	/* Don't update the RX chain when chain noise calibration is running */
+	update_chains = priv->chain_noise_data.state == IWL_CHAIN_NOISE_DONE ||
+			priv->chain_noise_data.state == IWL_CHAIN_NOISE_ALIVE;
+
+	if (!memcmp(&priv->power_data.sleep_cmd, cmd, sizeof(*cmd)) && !force)
+		return 0;
+
+	if (!iwl_legacy_is_ready_rf(priv))
+		return -EIO;
+
+	/* scan complete use sleep_power_next, need to be updated */
+	memcpy(&priv->power_data.sleep_cmd_next, cmd, sizeof(*cmd));
+	if (test_bit(STATUS_SCANNING, &priv->status) && !force) {
+		IWL_DEBUG_INFO(priv, "Defer power set mode while scanning\n");
+		return 0;
+	}
+
+	if (cmd->flags & IWL_POWER_DRIVER_ALLOW_SLEEP_MSK)
+		set_bit(STATUS_POWER_PMI, &priv->status);
+
+	ret = iwl_legacy_set_power(priv, cmd);
+	if (!ret) {
+		if (!(cmd->flags & IWL_POWER_DRIVER_ALLOW_SLEEP_MSK))
+			clear_bit(STATUS_POWER_PMI, &priv->status);
+
+		if (priv->cfg->ops->lib->update_chain_flags && update_chains)
+			priv->cfg->ops->lib->update_chain_flags(priv);
+		else if (priv->cfg->ops->lib->update_chain_flags)
+			IWL_DEBUG_POWER(priv,
+					"Cannot update the power, chain noise "
+					"calibration running: %d\n",
+					priv->chain_noise_data.state);
+
+		memcpy(&priv->power_data.sleep_cmd, cmd, sizeof(*cmd));
+	} else
+		IWL_ERR(priv, "set power fail, ret = %d", ret);
+
+	return ret;
+}
+
+int iwl_legacy_power_update_mode(struct iwl_priv *priv, bool force)
+{
+	struct iwl_powertable_cmd cmd;
+
+	iwl_legacy_power_sleep_cam_cmd(priv, &cmd);
+	return iwl_legacy_power_set_mode(priv, &cmd, force);
+}
+EXPORT_SYMBOL(iwl_legacy_power_update_mode);
+
+/* initialize to default */
+void iwl_legacy_power_initialize(struct iwl_priv *priv)
+{
+	u16 lctl = iwl_legacy_pcie_link_ctl(priv);
+
+	priv->power_data.pci_pm = !(lctl & PCI_CFG_LINK_CTRL_VAL_L0S_EN);
+
+	priv->power_data.debug_sleep_level_override = -1;
+
+	memset(&priv->power_data.sleep_cmd, 0,
+		sizeof(priv->power_data.sleep_cmd));
+}
+EXPORT_SYMBOL(iwl_legacy_power_initialize);
diff --git a/drivers/net/wireless/iwlegacy/iwl-power.h b/drivers/net/wireless/iwlegacy/iwl-power.h
new file mode 100644
index 000000000000..d30b36acdc4a
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-power.h
@@ -0,0 +1,55 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+#ifndef __iwl_legacy_power_setting_h__
+#define __iwl_legacy_power_setting_h__
+
+#include "iwl-commands.h"
+
+enum iwl_power_level {
+	IWL_POWER_INDEX_1,
+	IWL_POWER_INDEX_2,
+	IWL_POWER_INDEX_3,
+	IWL_POWER_INDEX_4,
+	IWL_POWER_INDEX_5,
+	IWL_POWER_NUM
+};
+
+struct iwl_power_mgr {
+	struct iwl_powertable_cmd sleep_cmd;
+	struct iwl_powertable_cmd sleep_cmd_next;
+	int debug_sleep_level_override;
+	bool pci_pm;
+};
+
+int
+iwl_legacy_power_set_mode(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd,
+		       bool force);
+int iwl_legacy_power_update_mode(struct iwl_priv *priv, bool force);
+void iwl_legacy_power_initialize(struct iwl_priv *priv);
+
+#endif  /* __iwl_legacy_power_setting_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-prph.h b/drivers/net/wireless/iwlegacy/iwl-prph.h
new file mode 100644
index 000000000000..30a493003ab0
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-prph.h
@@ -0,0 +1,523 @@
+/******************************************************************************
+ *
+ * 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) 2005 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2011 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef	__iwl_legacy_prph_h__
+#define __iwl_legacy_prph_h__
+
+/*
+ * Registers in this file are internal, not PCI bus memory mapped.
+ * Driver accesses these via HBUS_TARG_PRPH_* registers.
+ */
+#define PRPH_BASE	(0x00000)
+#define PRPH_END	(0xFFFFF)
+
+/* APMG (power management) constants */
+#define APMG_BASE			(PRPH_BASE + 0x3000)
+#define APMG_CLK_CTRL_REG		(APMG_BASE + 0x0000)
+#define APMG_CLK_EN_REG			(APMG_BASE + 0x0004)
+#define APMG_CLK_DIS_REG		(APMG_BASE + 0x0008)
+#define APMG_PS_CTRL_REG		(APMG_BASE + 0x000c)
+#define APMG_PCIDEV_STT_REG		(APMG_BASE + 0x0010)
+#define APMG_RFKILL_REG			(APMG_BASE + 0x0014)
+#define APMG_RTC_INT_STT_REG		(APMG_BASE + 0x001c)
+#define APMG_RTC_INT_MSK_REG		(APMG_BASE + 0x0020)
+#define APMG_DIGITAL_SVR_REG		(APMG_BASE + 0x0058)
+#define APMG_ANALOG_SVR_REG		(APMG_BASE + 0x006C)
+
+#define APMS_CLK_VAL_MRB_FUNC_MODE	(0x00000001)
+#define APMG_CLK_VAL_DMA_CLK_RQT	(0x00000200)
+#define APMG_CLK_VAL_BSM_CLK_RQT	(0x00000800)
+
+#define APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS	(0x00400000)
+#define APMG_PS_CTRL_VAL_RESET_REQ		(0x04000000)
+#define APMG_PS_CTRL_MSK_PWR_SRC		(0x03000000)
+#define APMG_PS_CTRL_VAL_PWR_SRC_VMAIN		(0x00000000)
+#define APMG_PS_CTRL_VAL_PWR_SRC_MAX		(0x01000000) /* 3945 only */
+#define APMG_PS_CTRL_VAL_PWR_SRC_VAUX		(0x02000000)
+#define APMG_SVR_VOLTAGE_CONFIG_BIT_MSK	(0x000001E0) /* bit 8:5 */
+#define APMG_SVR_DIGITAL_VOLTAGE_1_32		(0x00000060)
+
+#define APMG_PCIDEV_STT_VAL_L1_ACT_DIS		(0x00000800)
+
+/**
+ * BSM (Bootstrap State Machine)
+ *
+ * The Bootstrap State Machine (BSM) stores a short bootstrap uCode program
+ * in special SRAM that does not power down when the embedded control
+ * processor is sleeping (e.g. for periodic power-saving shutdowns of radio).
+ *
+ * When powering back up after sleeps (or during initial uCode load), the BSM
+ * internally loads the short bootstrap program from the special SRAM into the
+ * embedded processor's instruction SRAM, and starts the processor so it runs
+ * the bootstrap program.
+ *
+ * This bootstrap program loads (via PCI busmaster DMA) instructions and data
+ * images for a uCode program from host DRAM locations.  The host driver
+ * indicates DRAM locations and sizes for instruction and data images via the
+ * four BSM_DRAM_* registers.  Once the bootstrap program loads the new program,
+ * the new program starts automatically.
+ *
+ * The uCode used for open-source drivers includes two programs:
+ *
+ * 1)  Initialization -- performs hardware calibration and sets up some
+ *     internal data, then notifies host via "initialize alive" notification
+ *     (struct iwl_init_alive_resp) that it has completed all of its work.
+ *     After signal from host, it then loads and starts the runtime program.
+ *     The initialization program must be used when initially setting up the
+ *     NIC after loading the driver.
+ *
+ * 2)  Runtime/Protocol -- performs all normal runtime operations.  This
+ *     notifies host via "alive" notification (struct iwl_alive_resp) that it
+ *     is ready to be used.
+ *
+ * When initializing the NIC, the host driver does the following procedure:
+ *
+ * 1)  Load bootstrap program (instructions only, no data image for bootstrap)
+ *     into bootstrap memory.  Use dword writes starting at BSM_SRAM_LOWER_BOUND
+ *
+ * 2)  Point (via BSM_DRAM_*) to the "initialize" uCode data and instruction
+ *     images in host DRAM.
+ *
+ * 3)  Set up BSM to copy from BSM SRAM into uCode instruction SRAM when asked:
+ *     BSM_WR_MEM_SRC_REG = 0
+ *     BSM_WR_MEM_DST_REG = RTC_INST_LOWER_BOUND
+ *     BSM_WR_MEM_DWCOUNT_REG = # dwords in bootstrap instruction image
+ *
+ * 4)  Load bootstrap into instruction SRAM:
+ *     BSM_WR_CTRL_REG = BSM_WR_CTRL_REG_BIT_START
+ *
+ * 5)  Wait for load completion:
+ *     Poll BSM_WR_CTRL_REG for BSM_WR_CTRL_REG_BIT_START = 0
+ *
+ * 6)  Enable future boot loads whenever NIC's power management triggers it:
+ *     BSM_WR_CTRL_REG = BSM_WR_CTRL_REG_BIT_START_EN
+ *
+ * 7)  Start the NIC by removing all reset bits:
+ *     CSR_RESET = 0
+ *
+ *     The bootstrap uCode (already in instruction SRAM) loads initialization
+ *     uCode.  Initialization uCode performs data initialization, sends
+ *     "initialize alive" notification to host, and waits for a signal from
+ *     host to load runtime code.
+ *
+ * 4)  Point (via BSM_DRAM_*) to the "runtime" uCode data and instruction
+ *     images in host DRAM.  The last register loaded must be the instruction
+ *     byte count register ("1" in MSbit tells initialization uCode to load
+ *     the runtime uCode):
+ *     BSM_DRAM_INST_BYTECOUNT_REG = byte count | BSM_DRAM_INST_LOAD
+ *
+ * 5)  Wait for "alive" notification, then issue normal runtime commands.
+ *
+ * Data caching during power-downs:
+ *
+ * Just before the embedded controller powers down (e.g for automatic
+ * power-saving modes, or for RFKILL), uCode stores (via PCI busmaster DMA)
+ * a current snapshot of the embedded processor's data SRAM into host DRAM.
+ * This caches the data while the embedded processor's memory is powered down.
+ * Location and size are controlled by BSM_DRAM_DATA_* registers.
+ *
+ * NOTE:  Instruction SRAM does not need to be saved, since that doesn't
+ *        change during operation; the original image (from uCode distribution
+ *        file) can be used for reload.
+ *
+ * When powering back up, the BSM loads the bootstrap program.  Bootstrap looks
+ * at the BSM_DRAM_* registers, which now point to the runtime instruction
+ * image and the cached (modified) runtime data (*not* the initialization
+ * uCode).  Bootstrap reloads these runtime images into SRAM, and restarts the
+ * uCode from where it left off before the power-down.
+ *
+ * NOTE:  Initialization uCode does *not* run as part of the save/restore
+ *        procedure.
+ *
+ * This save/restore method is mostly for autonomous power management during
+ * normal operation (result of POWER_TABLE_CMD).  Platform suspend/resume and
+ * RFKILL should use complete restarts (with total re-initialization) of uCode,
+ * allowing total shutdown (including BSM memory).
+ *
+ * Note that, during normal operation, the host DRAM that held the initial
+ * startup data for the runtime code is now being used as a backup data cache
+ * for modified data!  If you need to completely re-initialize the NIC, make
+ * sure that you use the runtime data image from the uCode distribution file,
+ * not the modified/saved runtime data.  You may want to store a separate
+ * "clean" runtime data image in DRAM to avoid disk reads of distribution file.
+ */
+
+/* BSM bit fields */
+#define BSM_WR_CTRL_REG_BIT_START     (0x80000000) /* start boot load now */
+#define BSM_WR_CTRL_REG_BIT_START_EN  (0x40000000) /* enable boot after pwrup*/
+#define BSM_DRAM_INST_LOAD            (0x80000000) /* start program load now */
+
+/* BSM addresses */
+#define BSM_BASE                     (PRPH_BASE + 0x3400)
+#define BSM_END                      (PRPH_BASE + 0x3800)
+
+#define BSM_WR_CTRL_REG              (BSM_BASE + 0x000) /* ctl and status */
+#define BSM_WR_MEM_SRC_REG           (BSM_BASE + 0x004) /* source in BSM mem */
+#define BSM_WR_MEM_DST_REG           (BSM_BASE + 0x008) /* dest in SRAM mem */
+#define BSM_WR_DWCOUNT_REG           (BSM_BASE + 0x00C) /* bytes */
+#define BSM_WR_STATUS_REG            (BSM_BASE + 0x010) /* bit 0:  1 == done */
+
+/*
+ * Pointers and size regs for bootstrap load and data SRAM save/restore.
+ * NOTE:  3945 pointers use bits 31:0 of DRAM address.
+ *        4965 pointers use bits 35:4 of DRAM address.
+ */
+#define BSM_DRAM_INST_PTR_REG        (BSM_BASE + 0x090)
+#define BSM_DRAM_INST_BYTECOUNT_REG  (BSM_BASE + 0x094)
+#define BSM_DRAM_DATA_PTR_REG        (BSM_BASE + 0x098)
+#define BSM_DRAM_DATA_BYTECOUNT_REG  (BSM_BASE + 0x09C)
+
+/*
+ * BSM special memory, stays powered on during power-save sleeps.
+ * Read/write, address range from LOWER_BOUND to (LOWER_BOUND + SIZE -1)
+ */
+#define BSM_SRAM_LOWER_BOUND         (PRPH_BASE + 0x3800)
+#define BSM_SRAM_SIZE			(1024) /* bytes */
+
+
+/* 3945 Tx scheduler registers */
+#define ALM_SCD_BASE                        (PRPH_BASE + 0x2E00)
+#define ALM_SCD_MODE_REG                    (ALM_SCD_BASE + 0x000)
+#define ALM_SCD_ARASTAT_REG                 (ALM_SCD_BASE + 0x004)
+#define ALM_SCD_TXFACT_REG                  (ALM_SCD_BASE + 0x010)
+#define ALM_SCD_TXF4MF_REG                  (ALM_SCD_BASE + 0x014)
+#define ALM_SCD_TXF5MF_REG                  (ALM_SCD_BASE + 0x020)
+#define ALM_SCD_SBYP_MODE_1_REG             (ALM_SCD_BASE + 0x02C)
+#define ALM_SCD_SBYP_MODE_2_REG             (ALM_SCD_BASE + 0x030)
+
+/**
+ * Tx Scheduler
+ *
+ * The Tx Scheduler selects the next frame to be transmitted, choosing TFDs
+ * (Transmit Frame Descriptors) from up to 16 circular Tx queues resident in
+ * host DRAM.  It steers each frame's Tx command (which contains the frame
+ * data) into one of up to 7 prioritized Tx DMA FIFO channels within the
+ * device.  A queue maps to only one (selectable by driver) Tx DMA channel,
+ * but one DMA channel may take input from several queues.
+ *
+ * Tx DMA FIFOs have dedicated purposes.  For 4965, they are used as follows
+ * (cf. default_queue_to_tx_fifo in iwl-4965.c):
+ *
+ * 0 -- EDCA BK (background) frames, lowest priority
+ * 1 -- EDCA BE (best effort) frames, normal priority
+ * 2 -- EDCA VI (video) frames, higher priority
+ * 3 -- EDCA VO (voice) and management frames, highest priority
+ * 4 -- Commands (e.g. RXON, etc.)
+ * 5 -- unused (HCCA)
+ * 6 -- unused (HCCA)
+ * 7 -- not used by driver (device-internal only)
+ *
+ *
+ * Driver should normally map queues 0-6 to Tx DMA/FIFO channels 0-6.
+ * In addition, driver can map the remaining queues to Tx DMA/FIFO
+ * channels 0-3 to support 11n aggregation via EDCA DMA channels.
+ *
+ * The driver sets up each queue to work in one of two modes:
+ *
+ * 1)  Scheduler-Ack, in which the scheduler automatically supports a
+ *     block-ack (BA) window of up to 64 TFDs.  In this mode, each queue
+ *     contains TFDs for a unique combination of Recipient Address (RA)
+ *     and Traffic Identifier (TID), that is, traffic of a given
+ *     Quality-Of-Service (QOS) priority, destined for a single station.
+ *
+ *     In scheduler-ack mode, the scheduler keeps track of the Tx status of
+ *     each frame within the BA window, including whether it's been transmitted,
+ *     and whether it's been acknowledged by the receiving station.  The device
+ *     automatically processes block-acks received from the receiving STA,
+ *     and reschedules un-acked frames to be retransmitted (successful
+ *     Tx completion may end up being out-of-order).
+ *
+ *     The driver must maintain the queue's Byte Count table in host DRAM
+ *     (struct iwl4965_sched_queue_byte_cnt_tbl) for this mode.
+ *     This mode does not support fragmentation.
+ *
+ * 2)  FIFO (a.k.a. non-Scheduler-ACK), in which each TFD is processed in order.
+ *     The device may automatically retry Tx, but will retry only one frame
+ *     at a time, until receiving ACK from receiving station, or reaching
+ *     retry limit and giving up.
+ *
+ *     The command queue (#4/#9) must use this mode!
+ *     This mode does not require use of the Byte Count table in host DRAM.
+ *
+ * Driver controls scheduler operation via 3 means:
+ * 1)  Scheduler registers
+ * 2)  Shared scheduler data base in internal 4956 SRAM
+ * 3)  Shared data in host DRAM
+ *
+ * Initialization:
+ *
+ * When loading, driver should allocate memory for:
+ * 1)  16 TFD circular buffers, each with space for (typically) 256 TFDs.
+ * 2)  16 Byte Count circular buffers in 16 KBytes contiguous memory
+ *     (1024 bytes for each queue).
+ *
+ * After receiving "Alive" response from uCode, driver must initialize
+ * the scheduler (especially for queue #4/#9, the command queue, otherwise
+ * the driver can't issue commands!):
+ */
+
+/**
+ * Max Tx window size is the max number of contiguous TFDs that the scheduler
+ * can keep track of at one time when creating block-ack chains of frames.
+ * Note that "64" matches the number of ack bits in a block-ack packet.
+ * Driver should use SCD_WIN_SIZE and SCD_FRAME_LIMIT values to initialize
+ * IWL49_SCD_CONTEXT_QUEUE_OFFSET(x) values.
+ */
+#define SCD_WIN_SIZE				64
+#define SCD_FRAME_LIMIT				64
+
+/* SCD registers are internal, must be accessed via HBUS_TARG_PRPH regs */
+#define IWL49_SCD_START_OFFSET		0xa02c00
+
+/*
+ * 4965 tells driver SRAM address for internal scheduler structs via this reg.
+ * Value is valid only after "Alive" response from uCode.
+ */
+#define IWL49_SCD_SRAM_BASE_ADDR           (IWL49_SCD_START_OFFSET + 0x0)
+
+/*
+ * Driver may need to update queue-empty bits after changing queue's
+ * write and read pointers (indexes) during (re-)initialization (i.e. when
+ * scheduler is not tracking what's happening).
+ * Bit fields:
+ * 31-16:  Write mask -- 1: update empty bit, 0: don't change empty bit
+ * 15-00:  Empty state, one for each queue -- 1: empty, 0: non-empty
+ * NOTE:  This register is not used by Linux driver.
+ */
+#define IWL49_SCD_EMPTY_BITS               (IWL49_SCD_START_OFFSET + 0x4)
+
+/*
+ * Physical base address of array of byte count (BC) circular buffers (CBs).
+ * Each Tx queue has a BC CB in host DRAM to support Scheduler-ACK mode.
+ * This register points to BC CB for queue 0, must be on 1024-byte boundary.
+ * Others are spaced by 1024 bytes.
+ * Each BC CB is 2 bytes * (256 + 64) = 740 bytes, followed by 384 bytes pad.
+ * (Index into a queue's BC CB) = (index into queue's TFD CB) = (SSN & 0xff).
+ * Bit fields:
+ * 25-00:  Byte Count CB physical address [35:10], must be 1024-byte aligned.
+ */
+#define IWL49_SCD_DRAM_BASE_ADDR           (IWL49_SCD_START_OFFSET + 0x10)
+
+/*
+ * Enables any/all Tx DMA/FIFO channels.
+ * Scheduler generates requests for only the active channels.
+ * Set this to 0xff to enable all 8 channels (normal usage).
+ * Bit fields:
+ *  7- 0:  Enable (1), disable (0), one bit for each channel 0-7
+ */
+#define IWL49_SCD_TXFACT                   (IWL49_SCD_START_OFFSET + 0x1c)
+/*
+ * Queue (x) Write Pointers (indexes, really!), one for each Tx queue.
+ * Initialized and updated by driver as new TFDs are added to queue.
+ * NOTE:  If using Block Ack, index must correspond to frame's
+ *        Start Sequence Number; index = (SSN & 0xff)
+ * NOTE:  Alternative to HBUS_TARG_WRPTR, which is what Linux driver uses?
+ */
+#define IWL49_SCD_QUEUE_WRPTR(x)  (IWL49_SCD_START_OFFSET + 0x24 + (x) * 4)
+
+/*
+ * Queue (x) Read Pointers (indexes, really!), one for each Tx queue.
+ * For FIFO mode, index indicates next frame to transmit.
+ * For Scheduler-ACK mode, index indicates first frame in Tx window.
+ * Initialized by driver, updated by scheduler.
+ */
+#define IWL49_SCD_QUEUE_RDPTR(x)  (IWL49_SCD_START_OFFSET + 0x64 + (x) * 4)
+
+/*
+ * Select which queues work in chain mode (1) vs. not (0).
+ * Use chain mode to build chains of aggregated frames.
+ * Bit fields:
+ * 31-16:  Reserved
+ * 15-00:  Mode, one bit for each queue -- 1: Chain mode, 0: one-at-a-time
+ * NOTE:  If driver sets up queue for chain mode, it should be also set up
+ *        Scheduler-ACK mode as well, via SCD_QUEUE_STATUS_BITS(x).
+ */
+#define IWL49_SCD_QUEUECHAIN_SEL  (IWL49_SCD_START_OFFSET + 0xd0)
+
+/*
+ * Select which queues interrupt driver when scheduler increments
+ * a queue's read pointer (index).
+ * Bit fields:
+ * 31-16:  Reserved
+ * 15-00:  Interrupt enable, one bit for each queue -- 1: enabled, 0: disabled
+ * NOTE:  This functionality is apparently a no-op; driver relies on interrupts
+ *        from Rx queue to read Tx command responses and update Tx queues.
+ */
+#define IWL49_SCD_INTERRUPT_MASK  (IWL49_SCD_START_OFFSET + 0xe4)
+
+/*
+ * Queue search status registers.  One for each queue.
+ * Sets up queue mode and assigns queue to Tx DMA channel.
+ * Bit fields:
+ * 19-10: Write mask/enable bits for bits 0-9
+ *     9: Driver should init to "0"
+ *     8: Scheduler-ACK mode (1), non-Scheduler-ACK (i.e. FIFO) mode (0).
+ *        Driver should init to "1" for aggregation mode, or "0" otherwise.
+ *   7-6: Driver should init to "0"
+ *     5: Window Size Left; indicates whether scheduler can request
+ *        another TFD, based on window size, etc.  Driver should init
+ *        this bit to "1" for aggregation mode, or "0" for non-agg.
+ *   4-1: Tx FIFO to use (range 0-7).
+ *     0: Queue is active (1), not active (0).
+ * Other bits should be written as "0"
+ *
+ * NOTE:  If enabling Scheduler-ACK mode, chain mode should also be enabled
+ *        via SCD_QUEUECHAIN_SEL.
+ */
+#define IWL49_SCD_QUEUE_STATUS_BITS(x)\
+	(IWL49_SCD_START_OFFSET + 0x104 + (x) * 4)
+
+/* Bit field positions */
+#define IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE	(0)
+#define IWL49_SCD_QUEUE_STTS_REG_POS_TXF	(1)
+#define IWL49_SCD_QUEUE_STTS_REG_POS_WSL	(5)
+#define IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACK	(8)
+
+/* Write masks */
+#define IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN	(10)
+#define IWL49_SCD_QUEUE_STTS_REG_MSK		(0x0007FC00)
+
+/**
+ * 4965 internal SRAM structures for scheduler, shared with driver ...
+ *
+ * Driver should clear and initialize the following areas after receiving
+ * "Alive" response from 4965 uCode, i.e. after initial
+ * uCode load, or after a uCode load done for error recovery:
+ *
+ * SCD_CONTEXT_DATA_OFFSET (size 128 bytes)
+ * SCD_TX_STTS_BITMAP_OFFSET (size 256 bytes)
+ * SCD_TRANSLATE_TBL_OFFSET (size 32 bytes)
+ *
+ * Driver accesses SRAM via HBUS_TARG_MEM_* registers.
+ * Driver reads base address of this scheduler area from SCD_SRAM_BASE_ADDR.
+ * All OFFSET values must be added to this base address.
+ */
+
+/*
+ * Queue context.  One 8-byte entry for each of 16 queues.
+ *
+ * Driver should clear this entire area (size 0x80) to 0 after receiving
+ * "Alive" notification from uCode.  Additionally, driver should init
+ * each queue's entry as follows:
+ *
+ * LS Dword bit fields:
+ *  0-06:  Max Tx window size for Scheduler-ACK.  Driver should init to 64.
+ *
+ * MS Dword bit fields:
+ * 16-22:  Frame limit.  Driver should init to 10 (0xa).
+ *
+ * Driver should init all other bits to 0.
+ *
+ * Init must be done after driver receives "Alive" response from 4965 uCode,
+ * and when setting up queue for aggregation.
+ */
+#define IWL49_SCD_CONTEXT_DATA_OFFSET			0x380
+#define IWL49_SCD_CONTEXT_QUEUE_OFFSET(x) \
+			(IWL49_SCD_CONTEXT_DATA_OFFSET + ((x) * 8))
+
+#define IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS		(0)
+#define IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK		(0x0000007F)
+#define IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS	(16)
+#define IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK	(0x007F0000)
+
+/*
+ * Tx Status Bitmap
+ *
+ * Driver should clear this entire area (size 0x100) to 0 after receiving
+ * "Alive" notification from uCode.  Area is used only by device itself;
+ * no other support (besides clearing) is required from driver.
+ */
+#define IWL49_SCD_TX_STTS_BITMAP_OFFSET		0x400
+
+/*
+ * RAxTID to queue translation mapping.
+ *
+ * When queue is in Scheduler-ACK mode, frames placed in a that queue must be
+ * for only one combination of receiver address (RA) and traffic ID (TID), i.e.
+ * one QOS priority level destined for one station (for this wireless link,
+ * not final destination).  The SCD_TRANSLATE_TABLE area provides 16 16-bit
+ * mappings, one for each of the 16 queues.  If queue is not in Scheduler-ACK
+ * mode, the device ignores the mapping value.
+ *
+ * Bit fields, for each 16-bit map:
+ * 15-9:  Reserved, set to 0
+ *  8-4:  Index into device's station table for recipient station
+ *  3-0:  Traffic ID (tid), range 0-15
+ *
+ * Driver should clear this entire area (size 32 bytes) to 0 after receiving
+ * "Alive" notification from uCode.  To update a 16-bit map value, driver
+ * must read a dword-aligned value from device SRAM, replace the 16-bit map
+ * value of interest, and write the dword value back into device SRAM.
+ */
+#define IWL49_SCD_TRANSLATE_TBL_OFFSET		0x500
+
+/* Find translation table dword to read/write for given queue */
+#define IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
+	((IWL49_SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffffffc)
+
+#define IWL_SCD_TXFIFO_POS_TID			(0)
+#define IWL_SCD_TXFIFO_POS_RA			(4)
+#define IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK	(0x01FF)
+
+/*********************** END TX SCHEDULER *************************************/
+
+#endif				/* __iwl_legacy_prph_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-rx.c b/drivers/net/wireless/iwlegacy/iwl-rx.c
new file mode 100644
index 000000000000..654cf233a384
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-rx.c
@@ -0,0 +1,302 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/etherdevice.h>
+#include <linux/slab.h>
+#include <net/mac80211.h>
+#include <asm/unaligned.h>
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+/************************** RX-FUNCTIONS ****************************/
+/*
+ * Rx theory of operation
+ *
+ * Driver allocates a circular buffer of Receive Buffer Descriptors (RBDs),
+ * each of which point to Receive Buffers to be filled by the NIC.  These get
+ * used not only for Rx frames, but for any command response or notification
+ * from the NIC.  The driver and NIC manage the Rx buffers by means
+ * of indexes into the circular buffer.
+ *
+ * Rx Queue Indexes
+ * The host/firmware share two index registers for managing the Rx buffers.
+ *
+ * The READ index maps to the first position that the firmware may be writing
+ * to -- the driver can read up to (but not including) this position and get
+ * good data.
+ * The READ index is managed by the firmware once the card is enabled.
+ *
+ * The WRITE index maps to the last position the driver has read from -- the
+ * position preceding WRITE is the last slot the firmware can place a packet.
+ *
+ * The queue is empty (no good data) if WRITE = READ - 1, and is full if
+ * WRITE = READ.
+ *
+ * During initialization, the host sets up the READ queue position to the first
+ * INDEX position, and WRITE to the last (READ - 1 wrapped)
+ *
+ * When the firmware places a packet in a buffer, it will advance the READ index
+ * and fire the RX interrupt.  The driver can then query the READ index and
+ * process as many packets as possible, moving the WRITE index forward as it
+ * resets the Rx queue buffers with new memory.
+ *
+ * The management in the driver is as follows:
+ * + A list of pre-allocated SKBs is stored in iwl->rxq->rx_free.  When
+ *   iwl->rxq->free_count drops to or below RX_LOW_WATERMARK, work is scheduled
+ *   to replenish the iwl->rxq->rx_free.
+ * + In iwl_rx_replenish (scheduled) if 'processed' != 'read' then the
+ *   iwl->rxq is replenished and the READ INDEX is updated (updating the
+ *   'processed' and 'read' driver indexes as well)
+ * + A received packet is processed and handed to the kernel network stack,
+ *   detached from the iwl->rxq.  The driver 'processed' index is updated.
+ * + The Host/Firmware iwl->rxq is replenished at tasklet time from the rx_free
+ *   list. If there are no allocated buffers in iwl->rxq->rx_free, the READ
+ *   INDEX is not incremented and iwl->status(RX_STALLED) is set.  If there
+ *   were enough free buffers and RX_STALLED is set it is cleared.
+ *
+ *
+ * Driver sequence:
+ *
+ * iwl_legacy_rx_queue_alloc()   Allocates rx_free
+ * iwl_rx_replenish()     Replenishes rx_free list from rx_used, and calls
+ *                            iwl_rx_queue_restock
+ * iwl_rx_queue_restock() Moves available buffers from rx_free into Rx
+ *                            queue, updates firmware pointers, and updates
+ *                            the WRITE index.  If insufficient rx_free buffers
+ *                            are available, schedules iwl_rx_replenish
+ *
+ * -- enable interrupts --
+ * ISR - iwl_rx()         Detach iwl_rx_mem_buffers from pool up to the
+ *                            READ INDEX, detaching the SKB from the pool.
+ *                            Moves the packet buffer from queue to rx_used.
+ *                            Calls iwl_rx_queue_restock to refill any empty
+ *                            slots.
+ * ...
+ *
+ */
+
+/**
+ * iwl_legacy_rx_queue_space - Return number of free slots available in queue.
+ */
+int iwl_legacy_rx_queue_space(const struct iwl_rx_queue *q)
+{
+	int s = q->read - q->write;
+	if (s <= 0)
+		s += RX_QUEUE_SIZE;
+	/* keep some buffer to not confuse full and empty queue */
+	s -= 2;
+	if (s < 0)
+		s = 0;
+	return s;
+}
+EXPORT_SYMBOL(iwl_legacy_rx_queue_space);
+
+/**
+ * iwl_legacy_rx_queue_update_write_ptr - Update the write pointer for the RX queue
+ */
+void
+iwl_legacy_rx_queue_update_write_ptr(struct iwl_priv *priv,
+					struct iwl_rx_queue *q)
+{
+	unsigned long flags;
+	u32 rx_wrt_ptr_reg = priv->hw_params.rx_wrt_ptr_reg;
+	u32 reg;
+
+	spin_lock_irqsave(&q->lock, flags);
+
+	if (q->need_update == 0)
+		goto exit_unlock;
+
+	/* If power-saving is in use, make sure device is awake */
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			IWL_DEBUG_INFO(priv,
+				"Rx queue requesting wakeup,"
+				" GP1 = 0x%x\n", reg);
+			iwl_legacy_set_bit(priv, CSR_GP_CNTRL,
+				CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			goto exit_unlock;
+		}
+
+		q->write_actual = (q->write & ~0x7);
+		iwl_legacy_write_direct32(priv, rx_wrt_ptr_reg,
+				q->write_actual);
+
+	/* Else device is assumed to be awake */
+	} else {
+		/* Device expects a multiple of 8 */
+		q->write_actual = (q->write & ~0x7);
+		iwl_legacy_write_direct32(priv, rx_wrt_ptr_reg,
+			q->write_actual);
+	}
+
+	q->need_update = 0;
+
+ exit_unlock:
+	spin_unlock_irqrestore(&q->lock, flags);
+}
+EXPORT_SYMBOL(iwl_legacy_rx_queue_update_write_ptr);
+
+int iwl_legacy_rx_queue_alloc(struct iwl_priv *priv)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct device *dev = &priv->pci_dev->dev;
+	int i;
+
+	spin_lock_init(&rxq->lock);
+	INIT_LIST_HEAD(&rxq->rx_free);
+	INIT_LIST_HEAD(&rxq->rx_used);
+
+	/* Alloc the circular buffer of Read Buffer Descriptors (RBDs) */
+	rxq->bd = dma_alloc_coherent(dev, 4 * RX_QUEUE_SIZE, &rxq->bd_dma,
+				     GFP_KERNEL);
+	if (!rxq->bd)
+		goto err_bd;
+
+	rxq->rb_stts = dma_alloc_coherent(dev, sizeof(struct iwl_rb_status),
+					  &rxq->rb_stts_dma, GFP_KERNEL);
+	if (!rxq->rb_stts)
+		goto err_rb;
+
+	/* Fill the rx_used queue with _all_ of the Rx buffers */
+	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++)
+		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
+
+	/* Set us so that we have processed and used all buffers, but have
+	 * not restocked the Rx queue with fresh buffers */
+	rxq->read = rxq->write = 0;
+	rxq->write_actual = 0;
+	rxq->free_count = 0;
+	rxq->need_update = 0;
+	return 0;
+
+err_rb:
+	dma_free_coherent(&priv->pci_dev->dev, 4 * RX_QUEUE_SIZE, rxq->bd,
+			  rxq->bd_dma);
+err_bd:
+	return -ENOMEM;
+}
+EXPORT_SYMBOL(iwl_legacy_rx_queue_alloc);
+
+
+void iwl_legacy_rx_spectrum_measure_notif(struct iwl_priv *priv,
+					  struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_spectrum_notification *report = &(pkt->u.spectrum_notif);
+
+	if (!report->state) {
+		IWL_DEBUG_11H(priv,
+			"Spectrum Measure Notification: Start\n");
+		return;
+	}
+
+	memcpy(&priv->measure_report, report, sizeof(*report));
+	priv->measurement_status |= MEASUREMENT_READY;
+}
+EXPORT_SYMBOL(iwl_legacy_rx_spectrum_measure_notif);
+
+void iwl_legacy_recover_from_statistics(struct iwl_priv *priv,
+				struct iwl_rx_packet *pkt)
+{
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+	if (iwl_legacy_is_any_associated(priv)) {
+		if (priv->cfg->ops->lib->check_plcp_health) {
+			if (!priv->cfg->ops->lib->check_plcp_health(
+			    priv, pkt)) {
+				/*
+				 * high plcp error detected
+				 * reset Radio
+				 */
+				iwl_legacy_force_reset(priv,
+							IWL_RF_RESET, false);
+			}
+		}
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_recover_from_statistics);
+
+/*
+ * returns non-zero if packet should be dropped
+ */
+int iwl_legacy_set_decrypted_flag(struct iwl_priv *priv,
+			   struct ieee80211_hdr *hdr,
+			   u32 decrypt_res,
+			   struct ieee80211_rx_status *stats)
+{
+	u16 fc = le16_to_cpu(hdr->frame_control);
+
+	/*
+	 * All contexts have the same setting here due to it being
+	 * a module parameter, so OK to check any context.
+	 */
+	if (priv->contexts[IWL_RXON_CTX_BSS].active.filter_flags &
+						RXON_FILTER_DIS_DECRYPT_MSK)
+		return 0;
+
+	if (!(fc & IEEE80211_FCTL_PROTECTED))
+		return 0;
+
+	IWL_DEBUG_RX(priv, "decrypt_res:0x%x\n", decrypt_res);
+	switch (decrypt_res & RX_RES_STATUS_SEC_TYPE_MSK) {
+	case RX_RES_STATUS_SEC_TYPE_TKIP:
+		/* The uCode has got a bad phase 1 Key, pushes the packet.
+		 * Decryption will be done in SW. */
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_BAD_KEY_TTAK)
+			break;
+
+	case RX_RES_STATUS_SEC_TYPE_WEP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_BAD_ICV_MIC) {
+			/* bad ICV, the packet is destroyed since the
+			 * decryption is inplace, drop it */
+			IWL_DEBUG_RX(priv, "Packet destroyed\n");
+			return -1;
+		}
+	case RX_RES_STATUS_SEC_TYPE_CCMP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_DECRYPT_OK) {
+			IWL_DEBUG_RX(priv, "hw decrypt successfully!!!\n");
+			stats->flag |= RX_FLAG_DECRYPTED;
+		}
+		break;
+
+	default:
+		break;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_set_decrypted_flag);
diff --git a/drivers/net/wireless/iwlegacy/iwl-scan.c b/drivers/net/wireless/iwlegacy/iwl-scan.c
new file mode 100644
index 000000000000..60f597f796ca
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-scan.c
@@ -0,0 +1,625 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2011 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+
+/* For active scan, listen ACTIVE_DWELL_TIME (msec) on each channel after
+ * sending probe req.  This should be set long enough to hear probe responses
+ * from more than one AP.  */
+#define IWL_ACTIVE_DWELL_TIME_24    (30)       /* all times in msec */
+#define IWL_ACTIVE_DWELL_TIME_52    (20)
+
+#define IWL_ACTIVE_DWELL_FACTOR_24GHZ (3)
+#define IWL_ACTIVE_DWELL_FACTOR_52GHZ (2)
+
+/* For passive scan, listen PASSIVE_DWELL_TIME (msec) on each channel.
+ * Must be set longer than active dwell time.
+ * For the most reliable scan, set > AP beacon interval (typically 100msec). */
+#define IWL_PASSIVE_DWELL_TIME_24   (20)       /* all times in msec */
+#define IWL_PASSIVE_DWELL_TIME_52   (10)
+#define IWL_PASSIVE_DWELL_BASE      (100)
+#define IWL_CHANNEL_TUNE_TIME       5
+
+static int iwl_legacy_send_scan_abort(struct iwl_priv *priv)
+{
+	int ret;
+	struct iwl_rx_packet *pkt;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SCAN_ABORT_CMD,
+		.flags = CMD_WANT_SKB,
+	};
+
+	/* Exit instantly with error when device is not ready
+	 * to receive scan abort command or it does not perform
+	 * hardware scan currently */
+	if (!test_bit(STATUS_READY, &priv->status) ||
+	    !test_bit(STATUS_GEO_CONFIGURED, &priv->status) ||
+	    !test_bit(STATUS_SCAN_HW, &priv->status) ||
+	    test_bit(STATUS_FW_ERROR, &priv->status) ||
+	    test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return -EIO;
+
+	ret = iwl_legacy_send_cmd_sync(priv, &cmd);
+	if (ret)
+		return ret;
+
+	pkt = (struct iwl_rx_packet *)cmd.reply_page;
+	if (pkt->u.status != CAN_ABORT_STATUS) {
+		/* The scan abort will return 1 for success or
+		 * 2 for "failure".  A failure condition can be
+		 * due to simply not being in an active scan which
+		 * can occur if we send the scan abort before we
+		 * the microcode has notified us that a scan is
+		 * completed. */
+		IWL_DEBUG_SCAN(priv, "SCAN_ABORT ret %d.\n", pkt->u.status);
+		ret = -EIO;
+	}
+
+	iwl_legacy_free_pages(priv, cmd.reply_page);
+	return ret;
+}
+
+static void iwl_legacy_complete_scan(struct iwl_priv *priv, bool aborted)
+{
+	/* check if scan was requested from mac80211 */
+	if (priv->scan_request) {
+		IWL_DEBUG_SCAN(priv, "Complete scan in mac80211\n");
+		ieee80211_scan_completed(priv->hw, aborted);
+	}
+
+	priv->is_internal_short_scan = false;
+	priv->scan_vif = NULL;
+	priv->scan_request = NULL;
+}
+
+void iwl_legacy_force_scan_end(struct iwl_priv *priv)
+{
+	lockdep_assert_held(&priv->mutex);
+
+	if (!test_bit(STATUS_SCANNING, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "Forcing scan end while not scanning\n");
+		return;
+	}
+
+	IWL_DEBUG_SCAN(priv, "Forcing scan end\n");
+	clear_bit(STATUS_SCANNING, &priv->status);
+	clear_bit(STATUS_SCAN_HW, &priv->status);
+	clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+	iwl_legacy_complete_scan(priv, true);
+}
+
+static void iwl_legacy_do_scan_abort(struct iwl_priv *priv)
+{
+	int ret;
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (!test_bit(STATUS_SCANNING, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "Not performing scan to abort\n");
+		return;
+	}
+
+	if (test_and_set_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "Scan abort in progress\n");
+		return;
+	}
+
+	ret = iwl_legacy_send_scan_abort(priv);
+	if (ret) {
+		IWL_DEBUG_SCAN(priv, "Send scan abort failed %d\n", ret);
+		iwl_legacy_force_scan_end(priv);
+	} else
+		IWL_DEBUG_SCAN(priv, "Sucessfully send scan abort\n");
+}
+
+/**
+ * iwl_scan_cancel - Cancel any currently executing HW scan
+ */
+int iwl_legacy_scan_cancel(struct iwl_priv *priv)
+{
+	IWL_DEBUG_SCAN(priv, "Queuing abort scan\n");
+	queue_work(priv->workqueue, &priv->abort_scan);
+	return 0;
+}
+EXPORT_SYMBOL(iwl_legacy_scan_cancel);
+
+/**
+ * iwl_legacy_scan_cancel_timeout - Cancel any currently executing HW scan
+ * @ms: amount of time to wait (in milliseconds) for scan to abort
+ *
+ */
+int iwl_legacy_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(ms);
+
+	lockdep_assert_held(&priv->mutex);
+
+	IWL_DEBUG_SCAN(priv, "Scan cancel timeout\n");
+
+	iwl_legacy_do_scan_abort(priv);
+
+	while (time_before_eq(jiffies, timeout)) {
+		if (!test_bit(STATUS_SCAN_HW, &priv->status))
+			break;
+		msleep(20);
+	}
+
+	return test_bit(STATUS_SCAN_HW, &priv->status);
+}
+EXPORT_SYMBOL(iwl_legacy_scan_cancel_timeout);
+
+/* Service response to REPLY_SCAN_CMD (0x80) */
+static void iwl_legacy_rx_reply_scan(struct iwl_priv *priv,
+			      struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_scanreq_notification *notif =
+	    (struct iwl_scanreq_notification *)pkt->u.raw;
+
+	IWL_DEBUG_SCAN(priv, "Scan request status = 0x%x\n", notif->status);
+#endif
+}
+
+/* Service SCAN_START_NOTIFICATION (0x82) */
+static void iwl_legacy_rx_scan_start_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_scanstart_notification *notif =
+	    (struct iwl_scanstart_notification *)pkt->u.raw;
+	priv->scan_start_tsf = le32_to_cpu(notif->tsf_low);
+	IWL_DEBUG_SCAN(priv, "Scan start: "
+		       "%d [802.11%s] "
+		       "(TSF: 0x%08X:%08X) - %d (beacon timer %u)\n",
+		       notif->channel,
+		       notif->band ? "bg" : "a",
+		       le32_to_cpu(notif->tsf_high),
+		       le32_to_cpu(notif->tsf_low),
+		       notif->status, notif->beacon_timer);
+}
+
+/* Service SCAN_RESULTS_NOTIFICATION (0x83) */
+static void iwl_legacy_rx_scan_results_notif(struct iwl_priv *priv,
+				      struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_scanresults_notification *notif =
+	    (struct iwl_scanresults_notification *)pkt->u.raw;
+
+	IWL_DEBUG_SCAN(priv, "Scan ch.res: "
+		       "%d [802.11%s] "
+		       "(TSF: 0x%08X:%08X) - %d "
+		       "elapsed=%lu usec\n",
+		       notif->channel,
+		       notif->band ? "bg" : "a",
+		       le32_to_cpu(notif->tsf_high),
+		       le32_to_cpu(notif->tsf_low),
+		       le32_to_cpu(notif->statistics[0]),
+		       le32_to_cpu(notif->tsf_low) - priv->scan_start_tsf);
+#endif
+}
+
+/* Service SCAN_COMPLETE_NOTIFICATION (0x84) */
+static void iwl_legacy_rx_scan_complete_notif(struct iwl_priv *priv,
+				       struct iwl_rx_mem_buffer *rxb)
+{
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_scancomplete_notification *scan_notif = (void *)pkt->u.raw;
+#endif
+
+	IWL_DEBUG_SCAN(priv,
+			"Scan complete: %d channels (TSF 0x%08X:%08X) - %d\n",
+		       scan_notif->scanned_channels,
+		       scan_notif->tsf_low,
+		       scan_notif->tsf_high, scan_notif->status);
+
+	/* The HW is no longer scanning */
+	clear_bit(STATUS_SCAN_HW, &priv->status);
+
+	IWL_DEBUG_SCAN(priv, "Scan on %sGHz took %dms\n",
+		       (priv->scan_band == IEEE80211_BAND_2GHZ) ? "2.4" : "5.2",
+		       jiffies_to_msecs(jiffies - priv->scan_start));
+
+	queue_work(priv->workqueue, &priv->scan_completed);
+}
+
+void iwl_legacy_setup_rx_scan_handlers(struct iwl_priv *priv)
+{
+	/* scan handlers */
+	priv->rx_handlers[REPLY_SCAN_CMD] = iwl_legacy_rx_reply_scan;
+	priv->rx_handlers[SCAN_START_NOTIFICATION] =
+					iwl_legacy_rx_scan_start_notif;
+	priv->rx_handlers[SCAN_RESULTS_NOTIFICATION] =
+					iwl_legacy_rx_scan_results_notif;
+	priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
+					iwl_legacy_rx_scan_complete_notif;
+}
+EXPORT_SYMBOL(iwl_legacy_setup_rx_scan_handlers);
+
+inline u16 iwl_legacy_get_active_dwell_time(struct iwl_priv *priv,
+				     enum ieee80211_band band,
+				     u8 n_probes)
+{
+	if (band == IEEE80211_BAND_5GHZ)
+		return IWL_ACTIVE_DWELL_TIME_52 +
+			IWL_ACTIVE_DWELL_FACTOR_52GHZ * (n_probes + 1);
+	else
+		return IWL_ACTIVE_DWELL_TIME_24 +
+			IWL_ACTIVE_DWELL_FACTOR_24GHZ * (n_probes + 1);
+}
+EXPORT_SYMBOL(iwl_legacy_get_active_dwell_time);
+
+u16 iwl_legacy_get_passive_dwell_time(struct iwl_priv *priv,
+			       enum ieee80211_band band,
+			       struct ieee80211_vif *vif)
+{
+	struct iwl_rxon_context *ctx;
+	u16 passive = (band == IEEE80211_BAND_2GHZ) ?
+	    IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_24 :
+	    IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_52;
+
+	if (iwl_legacy_is_any_associated(priv)) {
+		/*
+		 * If we're associated, we clamp the maximum passive
+		 * dwell time to be 98% of the smallest beacon interval
+		 * (minus 2 * channel tune time)
+		 */
+		for_each_context(priv, ctx) {
+			u16 value;
+
+			if (!iwl_legacy_is_associated_ctx(ctx))
+				continue;
+			value = ctx->vif ? ctx->vif->bss_conf.beacon_int : 0;
+			if ((value > IWL_PASSIVE_DWELL_BASE) || !value)
+				value = IWL_PASSIVE_DWELL_BASE;
+			value = (value * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
+			passive = min(value, passive);
+		}
+	}
+
+	return passive;
+}
+EXPORT_SYMBOL(iwl_legacy_get_passive_dwell_time);
+
+void iwl_legacy_init_scan_params(struct iwl_priv *priv)
+{
+	u8 ant_idx = fls(priv->hw_params.valid_tx_ant) - 1;
+	if (!priv->scan_tx_ant[IEEE80211_BAND_5GHZ])
+		priv->scan_tx_ant[IEEE80211_BAND_5GHZ] = ant_idx;
+	if (!priv->scan_tx_ant[IEEE80211_BAND_2GHZ])
+		priv->scan_tx_ant[IEEE80211_BAND_2GHZ] = ant_idx;
+}
+EXPORT_SYMBOL(iwl_legacy_init_scan_params);
+
+static int __must_check iwl_legacy_scan_initiate(struct iwl_priv *priv,
+					  struct ieee80211_vif *vif,
+					  bool internal,
+					  enum ieee80211_band band)
+{
+	int ret;
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (WARN_ON(!priv->cfg->ops->utils->request_scan))
+		return -EOPNOTSUPP;
+
+	cancel_delayed_work(&priv->scan_check);
+
+	if (!iwl_legacy_is_ready_rf(priv)) {
+		IWL_WARN(priv, "Request scan called when driver not ready.\n");
+		return -EIO;
+	}
+
+	if (test_bit(STATUS_SCAN_HW, &priv->status)) {
+		IWL_DEBUG_SCAN(priv,
+			"Multiple concurrent scan requests in parallel.\n");
+		return -EBUSY;
+	}
+
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "Scan request while abort pending.\n");
+		return -EBUSY;
+	}
+
+	IWL_DEBUG_SCAN(priv, "Starting %sscan...\n",
+			internal ? "internal short " : "");
+
+	set_bit(STATUS_SCANNING, &priv->status);
+	priv->is_internal_short_scan = internal;
+	priv->scan_start = jiffies;
+	priv->scan_band = band;
+
+	ret = priv->cfg->ops->utils->request_scan(priv, vif);
+	if (ret) {
+		clear_bit(STATUS_SCANNING, &priv->status);
+		priv->is_internal_short_scan = false;
+		return ret;
+	}
+
+	queue_delayed_work(priv->workqueue, &priv->scan_check,
+			   IWL_SCAN_CHECK_WATCHDOG);
+
+	return 0;
+}
+
+int iwl_legacy_mac_hw_scan(struct ieee80211_hw *hw,
+		    struct ieee80211_vif *vif,
+		    struct cfg80211_scan_request *req)
+{
+	struct iwl_priv *priv = hw->priv;
+	int ret;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	if (req->n_channels == 0)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+
+	if (test_bit(STATUS_SCANNING, &priv->status) &&
+	    !priv->is_internal_short_scan) {
+		IWL_DEBUG_SCAN(priv, "Scan already in progress.\n");
+		ret = -EAGAIN;
+		goto out_unlock;
+	}
+
+	/* mac80211 will only ask for one band at a time */
+	priv->scan_request = req;
+	priv->scan_vif = vif;
+
+	/*
+	 * If an internal scan is in progress, just set
+	 * up the scan_request as per above.
+	 */
+	if (priv->is_internal_short_scan) {
+		IWL_DEBUG_SCAN(priv, "SCAN request during internal scan\n");
+		ret = 0;
+	} else
+		ret = iwl_legacy_scan_initiate(priv, vif, false,
+					req->channels[0]->band);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+
+out_unlock:
+	mutex_unlock(&priv->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_mac_hw_scan);
+
+/*
+ * internal short scan, this function should only been called while associated.
+ * It will reset and tune the radio to prevent possible RF related problem
+ */
+void iwl_legacy_internal_short_hw_scan(struct iwl_priv *priv)
+{
+	queue_work(priv->workqueue, &priv->start_internal_scan);
+}
+
+static void iwl_legacy_bg_start_internal_scan(struct work_struct *work)
+{
+	struct iwl_priv *priv =
+		container_of(work, struct iwl_priv, start_internal_scan);
+
+	IWL_DEBUG_SCAN(priv, "Start internal scan\n");
+
+	mutex_lock(&priv->mutex);
+
+	if (priv->is_internal_short_scan == true) {
+		IWL_DEBUG_SCAN(priv, "Internal scan already in progress\n");
+		goto unlock;
+	}
+
+	if (test_bit(STATUS_SCANNING, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "Scan already in progress.\n");
+		goto unlock;
+	}
+
+	if (iwl_legacy_scan_initiate(priv, NULL, true, priv->band))
+		IWL_DEBUG_SCAN(priv, "failed to start internal short scan\n");
+ unlock:
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_legacy_bg_scan_check(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, scan_check.work);
+
+	IWL_DEBUG_SCAN(priv, "Scan check work\n");
+
+	/* Since we are here firmware does not finish scan and
+	 * most likely is in bad shape, so we don't bother to
+	 * send abort command, just force scan complete to mac80211 */
+	mutex_lock(&priv->mutex);
+	iwl_legacy_force_scan_end(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+/**
+ * iwl_legacy_fill_probe_req - fill in all required fields and IE for probe request
+ */
+
+u16
+iwl_legacy_fill_probe_req(struct iwl_priv *priv, struct ieee80211_mgmt *frame,
+		       const u8 *ta, const u8 *ies, int ie_len, int left)
+{
+	int len = 0;
+	u8 *pos = NULL;
+
+	/* Make sure there is enough space for the probe request,
+	 * two mandatory IEs and the data */
+	left -= 24;
+	if (left < 0)
+		return 0;
+
+	frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	memcpy(frame->da, iwlegacy_bcast_addr, ETH_ALEN);
+	memcpy(frame->sa, ta, ETH_ALEN);
+	memcpy(frame->bssid, iwlegacy_bcast_addr, ETH_ALEN);
+	frame->seq_ctrl = 0;
+
+	len += 24;
+
+	/* ...next IE... */
+	pos = &frame->u.probe_req.variable[0];
+
+	/* fill in our indirect SSID IE */
+	left -= 2;
+	if (left < 0)
+		return 0;
+	*pos++ = WLAN_EID_SSID;
+	*pos++ = 0;
+
+	len += 2;
+
+	if (WARN_ON(left < ie_len))
+		return len;
+
+	if (ies && ie_len) {
+		memcpy(pos, ies, ie_len);
+		len += ie_len;
+	}
+
+	return (u16)len;
+}
+EXPORT_SYMBOL(iwl_legacy_fill_probe_req);
+
+static void iwl_legacy_bg_abort_scan(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv, abort_scan);
+
+	IWL_DEBUG_SCAN(priv, "Abort scan work\n");
+
+	/* We keep scan_check work queued in case when firmware will not
+	 * report back scan completed notification */
+	mutex_lock(&priv->mutex);
+	iwl_legacy_scan_cancel_timeout(priv, 200);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_legacy_bg_scan_completed(struct work_struct *work)
+{
+	struct iwl_priv *priv =
+	    container_of(work, struct iwl_priv, scan_completed);
+	bool aborted;
+
+	IWL_DEBUG_SCAN(priv, "Completed %sscan.\n",
+		       priv->is_internal_short_scan ? "internal short " : "");
+
+	cancel_delayed_work(&priv->scan_check);
+
+	mutex_lock(&priv->mutex);
+
+	aborted = test_and_clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+	if (aborted)
+		IWL_DEBUG_SCAN(priv, "Aborted scan completed.\n");
+
+	if (!test_and_clear_bit(STATUS_SCANNING, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "Scan already completed.\n");
+		goto out_settings;
+	}
+
+	if (priv->is_internal_short_scan && !aborted) {
+		int err;
+
+		/* Check if mac80211 requested scan during our internal scan */
+		if (priv->scan_request == NULL)
+			goto out_complete;
+
+		/* If so request a new scan */
+		err = iwl_legacy_scan_initiate(priv, priv->scan_vif, false,
+					priv->scan_request->channels[0]->band);
+		if (err) {
+			IWL_DEBUG_SCAN(priv,
+				"failed to initiate pending scan: %d\n", err);
+			aborted = true;
+			goto out_complete;
+		}
+
+		goto out;
+	}
+
+out_complete:
+	iwl_legacy_complete_scan(priv, aborted);
+
+out_settings:
+	/* Can we still talk to firmware ? */
+	if (!iwl_legacy_is_ready_rf(priv))
+		goto out;
+
+	/*
+	 * We do not commit power settings while scan is pending,
+	 * do it now if the settings changed.
+	 */
+	iwl_legacy_power_set_mode(priv, &priv->power_data.sleep_cmd_next,
+								false);
+	iwl_legacy_set_tx_power(priv, priv->tx_power_next, false);
+
+	priv->cfg->ops->utils->post_scan(priv);
+
+out:
+	mutex_unlock(&priv->mutex);
+}
+
+void iwl_legacy_setup_scan_deferred_work(struct iwl_priv *priv)
+{
+	INIT_WORK(&priv->scan_completed, iwl_legacy_bg_scan_completed);
+	INIT_WORK(&priv->abort_scan, iwl_legacy_bg_abort_scan);
+	INIT_WORK(&priv->start_internal_scan,
+				iwl_legacy_bg_start_internal_scan);
+	INIT_DELAYED_WORK(&priv->scan_check, iwl_legacy_bg_scan_check);
+}
+EXPORT_SYMBOL(iwl_legacy_setup_scan_deferred_work);
+
+void iwl_legacy_cancel_scan_deferred_work(struct iwl_priv *priv)
+{
+	cancel_work_sync(&priv->start_internal_scan);
+	cancel_work_sync(&priv->abort_scan);
+	cancel_work_sync(&priv->scan_completed);
+
+	if (cancel_delayed_work_sync(&priv->scan_check)) {
+		mutex_lock(&priv->mutex);
+		iwl_legacy_force_scan_end(priv);
+		mutex_unlock(&priv->mutex);
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_cancel_scan_deferred_work);
diff --git a/drivers/net/wireless/iwlegacy/iwl-spectrum.h b/drivers/net/wireless/iwlegacy/iwl-spectrum.h
new file mode 100644
index 000000000000..9f70a4723103
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-spectrum.h
@@ -0,0 +1,92 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_legacy_spectrum_h__
+#define __iwl_legacy_spectrum_h__
+enum {				/* ieee80211_basic_report.map */
+	IEEE80211_BASIC_MAP_BSS = (1 << 0),
+	IEEE80211_BASIC_MAP_OFDM = (1 << 1),
+	IEEE80211_BASIC_MAP_UNIDENTIFIED = (1 << 2),
+	IEEE80211_BASIC_MAP_RADAR = (1 << 3),
+	IEEE80211_BASIC_MAP_UNMEASURED = (1 << 4),
+	/* Bits 5-7 are reserved */
+
+};
+struct ieee80211_basic_report {
+	u8 channel;
+	__le64 start_time;
+	__le16 duration;
+	u8 map;
+} __packed;
+
+enum {				/* ieee80211_measurement_request.mode */
+	/* Bit 0 is reserved */
+	IEEE80211_MEASUREMENT_ENABLE = (1 << 1),
+	IEEE80211_MEASUREMENT_REQUEST = (1 << 2),
+	IEEE80211_MEASUREMENT_REPORT = (1 << 3),
+	/* Bits 4-7 are reserved */
+};
+
+enum {
+	IEEE80211_REPORT_BASIC = 0,	/* required */
+	IEEE80211_REPORT_CCA = 1,	/* optional */
+	IEEE80211_REPORT_RPI = 2,	/* optional */
+	/* 3-255 reserved */
+};
+
+struct ieee80211_measurement_params {
+	u8 channel;
+	__le64 start_time;
+	__le16 duration;
+} __packed;
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __packed;
+
+struct ieee80211_measurement_request {
+	struct ieee80211_info_element ie;
+	u8 token;
+	u8 mode;
+	u8 type;
+	struct ieee80211_measurement_params params[0];
+} __packed;
+
+struct ieee80211_measurement_report {
+	struct ieee80211_info_element ie;
+	u8 token;
+	u8 mode;
+	u8 type;
+	union {
+		struct ieee80211_basic_report basic[0];
+	} u;
+} __packed;
+
+#endif
diff --git a/drivers/net/wireless/iwlegacy/iwl-sta.c b/drivers/net/wireless/iwlegacy/iwl-sta.c
new file mode 100644
index 000000000000..47c9da3834ea
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-sta.c
@@ -0,0 +1,816 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+#include <linux/sched.h>
+#include <linux/lockdep.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+
+/* priv->sta_lock must be held */
+static void iwl_legacy_sta_ucode_activate(struct iwl_priv *priv, u8 sta_id)
+{
+
+	if (!(priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE))
+		IWL_ERR(priv,
+			"ACTIVATE a non DRIVER active station id %u addr %pM\n",
+			sta_id, priv->stations[sta_id].sta.sta.addr);
+
+	if (priv->stations[sta_id].used & IWL_STA_UCODE_ACTIVE) {
+		IWL_DEBUG_ASSOC(priv,
+			"STA id %u addr %pM already present"
+			" in uCode (according to driver)\n",
+			sta_id, priv->stations[sta_id].sta.sta.addr);
+	} else {
+		priv->stations[sta_id].used |= IWL_STA_UCODE_ACTIVE;
+		IWL_DEBUG_ASSOC(priv, "Added STA id %u addr %pM to uCode\n",
+				sta_id, priv->stations[sta_id].sta.sta.addr);
+	}
+}
+
+static int iwl_legacy_process_add_sta_resp(struct iwl_priv *priv,
+				    struct iwl_legacy_addsta_cmd *addsta,
+				    struct iwl_rx_packet *pkt,
+				    bool sync)
+{
+	u8 sta_id = addsta->sta.sta_id;
+	unsigned long flags;
+	int ret = -EIO;
+
+	if (pkt->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERR(priv, "Bad return from REPLY_ADD_STA (0x%08X)\n",
+			pkt->hdr.flags);
+		return ret;
+	}
+
+	IWL_DEBUG_INFO(priv, "Processing response for adding station %u\n",
+		       sta_id);
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	switch (pkt->u.add_sta.status) {
+	case ADD_STA_SUCCESS_MSK:
+		IWL_DEBUG_INFO(priv, "REPLY_ADD_STA PASSED\n");
+		iwl_legacy_sta_ucode_activate(priv, sta_id);
+		ret = 0;
+		break;
+	case ADD_STA_NO_ROOM_IN_TABLE:
+		IWL_ERR(priv, "Adding station %d failed, no room in table.\n",
+			sta_id);
+		break;
+	case ADD_STA_NO_BLOCK_ACK_RESOURCE:
+		IWL_ERR(priv,
+			"Adding station %d failed, no block ack resource.\n",
+			sta_id);
+		break;
+	case ADD_STA_MODIFY_NON_EXIST_STA:
+		IWL_ERR(priv, "Attempting to modify non-existing station %d\n",
+			sta_id);
+		break;
+	default:
+		IWL_DEBUG_ASSOC(priv, "Received REPLY_ADD_STA:(0x%08X)\n",
+				pkt->u.add_sta.status);
+		break;
+	}
+
+	IWL_DEBUG_INFO(priv, "%s station id %u addr %pM\n",
+		       priv->stations[sta_id].sta.mode ==
+		       STA_CONTROL_MODIFY_MSK ?  "Modified" : "Added",
+		       sta_id, priv->stations[sta_id].sta.sta.addr);
+
+	/*
+	 * XXX: The MAC address in the command buffer is often changed from
+	 * the original sent to the device. That is, the MAC address
+	 * written to the command buffer often is not the same MAC adress
+	 * read from the command buffer when the command returns. This
+	 * issue has not yet been resolved and this debugging is left to
+	 * observe the problem.
+	 */
+	IWL_DEBUG_INFO(priv, "%s station according to cmd buffer %pM\n",
+		       priv->stations[sta_id].sta.mode ==
+		       STA_CONTROL_MODIFY_MSK ? "Modified" : "Added",
+		       addsta->sta.addr);
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return ret;
+}
+
+static void iwl_legacy_add_sta_callback(struct iwl_priv *priv,
+				 struct iwl_device_cmd *cmd,
+				 struct iwl_rx_packet *pkt)
+{
+	struct iwl_legacy_addsta_cmd *addsta =
+		(struct iwl_legacy_addsta_cmd *)cmd->cmd.payload;
+
+	iwl_legacy_process_add_sta_resp(priv, addsta, pkt, false);
+
+}
+
+int iwl_legacy_send_add_sta(struct iwl_priv *priv,
+		     struct iwl_legacy_addsta_cmd *sta, u8 flags)
+{
+	struct iwl_rx_packet *pkt = NULL;
+	int ret = 0;
+	u8 data[sizeof(*sta)];
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_ADD_STA,
+		.flags = flags,
+		.data = data,
+	};
+	u8 sta_id __maybe_unused = sta->sta.sta_id;
+
+	IWL_DEBUG_INFO(priv, "Adding sta %u (%pM) %ssynchronously\n",
+		       sta_id, sta->sta.addr, flags & CMD_ASYNC ?  "a" : "");
+
+	if (flags & CMD_ASYNC)
+		cmd.callback = iwl_legacy_add_sta_callback;
+	else {
+		cmd.flags |= CMD_WANT_SKB;
+		might_sleep();
+	}
+
+	cmd.len = priv->cfg->ops->utils->build_addsta_hcmd(sta, data);
+	ret = iwl_legacy_send_cmd(priv, &cmd);
+
+	if (ret || (flags & CMD_ASYNC))
+		return ret;
+
+	if (ret == 0) {
+		pkt = (struct iwl_rx_packet *)cmd.reply_page;
+		ret = iwl_legacy_process_add_sta_resp(priv, sta, pkt, true);
+	}
+	iwl_legacy_free_pages(priv, cmd.reply_page);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_send_add_sta);
+
+static void iwl_legacy_set_ht_add_station(struct iwl_priv *priv, u8 index,
+				   struct ieee80211_sta *sta,
+				   struct iwl_rxon_context *ctx)
+{
+	struct ieee80211_sta_ht_cap *sta_ht_inf = &sta->ht_cap;
+	__le32 sta_flags;
+	u8 mimo_ps_mode;
+
+	if (!sta || !sta_ht_inf->ht_supported)
+		goto done;
+
+	mimo_ps_mode = (sta_ht_inf->cap & IEEE80211_HT_CAP_SM_PS) >> 2;
+	IWL_DEBUG_ASSOC(priv, "spatial multiplexing power save mode: %s\n",
+			(mimo_ps_mode == WLAN_HT_CAP_SM_PS_STATIC) ?
+			"static" :
+			(mimo_ps_mode == WLAN_HT_CAP_SM_PS_DYNAMIC) ?
+			"dynamic" : "disabled");
+
+	sta_flags = priv->stations[index].sta.station_flags;
+
+	sta_flags &= ~(STA_FLG_RTS_MIMO_PROT_MSK | STA_FLG_MIMO_DIS_MSK);
+
+	switch (mimo_ps_mode) {
+	case WLAN_HT_CAP_SM_PS_STATIC:
+		sta_flags |= STA_FLG_MIMO_DIS_MSK;
+		break;
+	case WLAN_HT_CAP_SM_PS_DYNAMIC:
+		sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
+		break;
+	case WLAN_HT_CAP_SM_PS_DISABLED:
+		break;
+	default:
+		IWL_WARN(priv, "Invalid MIMO PS mode %d\n", mimo_ps_mode);
+		break;
+	}
+
+	sta_flags |= cpu_to_le32(
+	      (u32)sta_ht_inf->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
+
+	sta_flags |= cpu_to_le32(
+	      (u32)sta_ht_inf->ampdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
+
+	if (iwl_legacy_is_ht40_tx_allowed(priv, ctx, &sta->ht_cap))
+		sta_flags |= STA_FLG_HT40_EN_MSK;
+	else
+		sta_flags &= ~STA_FLG_HT40_EN_MSK;
+
+	priv->stations[index].sta.station_flags = sta_flags;
+ done:
+	return;
+}
+
+/**
+ * iwl_legacy_prep_station - Prepare station information for addition
+ *
+ * should be called with sta_lock held
+ */
+u8 iwl_legacy_prep_station(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
+		    const u8 *addr, bool is_ap, struct ieee80211_sta *sta)
+{
+	struct iwl_station_entry *station;
+	int i;
+	u8 sta_id = IWL_INVALID_STATION;
+	u16 rate;
+
+	if (is_ap)
+		sta_id = ctx->ap_sta_id;
+	else if (is_broadcast_ether_addr(addr))
+		sta_id = ctx->bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++) {
+			if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				sta_id = i;
+				break;
+			}
+
+			if (!priv->stations[i].used &&
+			    sta_id == IWL_INVALID_STATION)
+				sta_id = i;
+		}
+
+	/*
+	 * These two conditions have the same outcome, but keep them
+	 * separate
+	 */
+	if (unlikely(sta_id == IWL_INVALID_STATION))
+		return sta_id;
+
+	/*
+	 * uCode is not able to deal with multiple requests to add a
+	 * station. Keep track if one is in progress so that we do not send
+	 * another.
+	 */
+	if (priv->stations[sta_id].used & IWL_STA_UCODE_INPROGRESS) {
+		IWL_DEBUG_INFO(priv,
+				"STA %d already in process of being added.\n",
+				sta_id);
+		return sta_id;
+	}
+
+	if ((priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE) &&
+	    (priv->stations[sta_id].used & IWL_STA_UCODE_ACTIVE) &&
+	    !compare_ether_addr(priv->stations[sta_id].sta.sta.addr, addr)) {
+		IWL_DEBUG_ASSOC(priv,
+				"STA %d (%pM) already added, not adding again.\n",
+				sta_id, addr);
+		return sta_id;
+	}
+
+	station = &priv->stations[sta_id];
+	station->used = IWL_STA_DRIVER_ACTIVE;
+	IWL_DEBUG_ASSOC(priv, "Add STA to driver ID %d: %pM\n",
+			sta_id, addr);
+	priv->num_stations++;
+
+	/* Set up the REPLY_ADD_STA command to send to device */
+	memset(&station->sta, 0, sizeof(struct iwl_legacy_addsta_cmd));
+	memcpy(station->sta.sta.addr, addr, ETH_ALEN);
+	station->sta.mode = 0;
+	station->sta.sta.sta_id = sta_id;
+	station->sta.station_flags = ctx->station_flags;
+	station->ctxid = ctx->ctxid;
+
+	if (sta) {
+		struct iwl_station_priv_common *sta_priv;
+
+		sta_priv = (void *)sta->drv_priv;
+		sta_priv->ctx = ctx;
+	}
+
+	/*
+	 * OK to call unconditionally, since local stations (IBSS BSSID
+	 * STA and broadcast STA) pass in a NULL sta, and mac80211
+	 * doesn't allow HT IBSS.
+	 */
+	iwl_legacy_set_ht_add_station(priv, sta_id, sta, ctx);
+
+	/* 3945 only */
+	rate = (priv->band == IEEE80211_BAND_5GHZ) ?
+		IWL_RATE_6M_PLCP : IWL_RATE_1M_PLCP;
+	/* Turn on both antennas for the station... */
+	station->sta.rate_n_flags = cpu_to_le16(rate | RATE_MCS_ANT_AB_MSK);
+
+	return sta_id;
+
+}
+EXPORT_SYMBOL_GPL(iwl_legacy_prep_station);
+
+#define STA_WAIT_TIMEOUT (HZ/2)
+
+/**
+ * iwl_legacy_add_station_common -
+ */
+int
+iwl_legacy_add_station_common(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			   const u8 *addr, bool is_ap,
+			   struct ieee80211_sta *sta, u8 *sta_id_r)
+{
+	unsigned long flags_spin;
+	int ret = 0;
+	u8 sta_id;
+	struct iwl_legacy_addsta_cmd sta_cmd;
+
+	*sta_id_r = 0;
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	sta_id = iwl_legacy_prep_station(priv, ctx, addr, is_ap, sta);
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_ERR(priv, "Unable to prepare station %pM for addition\n",
+			addr);
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return -EINVAL;
+	}
+
+	/*
+	 * uCode is not able to deal with multiple requests to add a
+	 * station. Keep track if one is in progress so that we do not send
+	 * another.
+	 */
+	if (priv->stations[sta_id].used & IWL_STA_UCODE_INPROGRESS) {
+		IWL_DEBUG_INFO(priv,
+			"STA %d already in process of being added.\n",
+		       sta_id);
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return -EEXIST;
+	}
+
+	if ((priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE) &&
+	    (priv->stations[sta_id].used & IWL_STA_UCODE_ACTIVE)) {
+		IWL_DEBUG_ASSOC(priv,
+			"STA %d (%pM) already added, not adding again.\n",
+			sta_id, addr);
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return -EEXIST;
+	}
+
+	priv->stations[sta_id].used |= IWL_STA_UCODE_INPROGRESS;
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta,
+				sizeof(struct iwl_legacy_addsta_cmd));
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+
+	/* Add station to device's station table */
+	ret = iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+	if (ret) {
+		spin_lock_irqsave(&priv->sta_lock, flags_spin);
+		IWL_ERR(priv, "Adding station %pM failed.\n",
+			priv->stations[sta_id].sta.sta.addr);
+		priv->stations[sta_id].used &= ~IWL_STA_DRIVER_ACTIVE;
+		priv->stations[sta_id].used &= ~IWL_STA_UCODE_INPROGRESS;
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+	}
+	*sta_id_r = sta_id;
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_add_station_common);
+
+/**
+ * iwl_legacy_sta_ucode_deactivate - deactivate ucode status for a station
+ *
+ * priv->sta_lock must be held
+ */
+static void iwl_legacy_sta_ucode_deactivate(struct iwl_priv *priv, u8 sta_id)
+{
+	/* Ucode must be active and driver must be non active */
+	if ((priv->stations[sta_id].used &
+	     (IWL_STA_UCODE_ACTIVE | IWL_STA_DRIVER_ACTIVE)) !=
+						IWL_STA_UCODE_ACTIVE)
+		IWL_ERR(priv, "removed non active STA %u\n", sta_id);
+
+	priv->stations[sta_id].used &= ~IWL_STA_UCODE_ACTIVE;
+
+	memset(&priv->stations[sta_id], 0, sizeof(struct iwl_station_entry));
+	IWL_DEBUG_ASSOC(priv, "Removed STA %u\n", sta_id);
+}
+
+static int iwl_legacy_send_remove_station(struct iwl_priv *priv,
+				   const u8 *addr, int sta_id,
+				   bool temporary)
+{
+	struct iwl_rx_packet *pkt;
+	int ret;
+
+	unsigned long flags_spin;
+	struct iwl_rem_sta_cmd rm_sta_cmd;
+
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_REMOVE_STA,
+		.len = sizeof(struct iwl_rem_sta_cmd),
+		.flags = CMD_SYNC,
+		.data = &rm_sta_cmd,
+	};
+
+	memset(&rm_sta_cmd, 0, sizeof(rm_sta_cmd));
+	rm_sta_cmd.num_sta = 1;
+	memcpy(&rm_sta_cmd.addr, addr, ETH_ALEN);
+
+	cmd.flags |= CMD_WANT_SKB;
+
+	ret = iwl_legacy_send_cmd(priv, &cmd);
+
+	if (ret)
+		return ret;
+
+	pkt = (struct iwl_rx_packet *)cmd.reply_page;
+	if (pkt->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERR(priv, "Bad return from REPLY_REMOVE_STA (0x%08X)\n",
+			  pkt->hdr.flags);
+		ret = -EIO;
+	}
+
+	if (!ret) {
+		switch (pkt->u.rem_sta.status) {
+		case REM_STA_SUCCESS_MSK:
+			if (!temporary) {
+				spin_lock_irqsave(&priv->sta_lock, flags_spin);
+				iwl_legacy_sta_ucode_deactivate(priv, sta_id);
+				spin_unlock_irqrestore(&priv->sta_lock,
+								flags_spin);
+			}
+			IWL_DEBUG_ASSOC(priv, "REPLY_REMOVE_STA PASSED\n");
+			break;
+		default:
+			ret = -EIO;
+			IWL_ERR(priv, "REPLY_REMOVE_STA failed\n");
+			break;
+		}
+	}
+	iwl_legacy_free_pages(priv, cmd.reply_page);
+
+	return ret;
+}
+
+/**
+ * iwl_legacy_remove_station - Remove driver's knowledge of station.
+ */
+int iwl_legacy_remove_station(struct iwl_priv *priv, const u8 sta_id,
+		       const u8 *addr)
+{
+	unsigned long flags;
+
+	if (!iwl_legacy_is_ready(priv)) {
+		IWL_DEBUG_INFO(priv,
+			"Unable to remove station %pM, device not ready.\n",
+			addr);
+		/*
+		 * It is typical for stations to be removed when we are
+		 * going down. Return success since device will be down
+		 * soon anyway
+		 */
+		return 0;
+	}
+
+	IWL_DEBUG_ASSOC(priv, "Removing STA from driver:%d  %pM\n",
+			sta_id, addr);
+
+	if (WARN_ON(sta_id == IWL_INVALID_STATION))
+		return -EINVAL;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	if (!(priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE)) {
+		IWL_DEBUG_INFO(priv, "Removing %pM but non DRIVER active\n",
+				addr);
+		goto out_err;
+	}
+
+	if (!(priv->stations[sta_id].used & IWL_STA_UCODE_ACTIVE)) {
+		IWL_DEBUG_INFO(priv, "Removing %pM but non UCODE active\n",
+				addr);
+		goto out_err;
+	}
+
+	if (priv->stations[sta_id].used & IWL_STA_LOCAL) {
+		kfree(priv->stations[sta_id].lq);
+		priv->stations[sta_id].lq = NULL;
+	}
+
+	priv->stations[sta_id].used &= ~IWL_STA_DRIVER_ACTIVE;
+
+	priv->num_stations--;
+
+	BUG_ON(priv->num_stations < 0);
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	return iwl_legacy_send_remove_station(priv, addr, sta_id, false);
+out_err:
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(iwl_legacy_remove_station);
+
+/**
+ * iwl_legacy_clear_ucode_stations - clear ucode station table bits
+ *
+ * This function clears all the bits in the driver indicating
+ * which stations are active in the ucode. Call when something
+ * other than explicit station management would cause this in
+ * the ucode, e.g. unassociated RXON.
+ */
+void iwl_legacy_clear_ucode_stations(struct iwl_priv *priv,
+			      struct iwl_rxon_context *ctx)
+{
+	int i;
+	unsigned long flags_spin;
+	bool cleared = false;
+
+	IWL_DEBUG_INFO(priv, "Clearing ucode stations in driver\n");
+
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	for (i = 0; i < priv->hw_params.max_stations; i++) {
+		if (ctx && ctx->ctxid != priv->stations[i].ctxid)
+			continue;
+
+		if (priv->stations[i].used & IWL_STA_UCODE_ACTIVE) {
+			IWL_DEBUG_INFO(priv,
+				"Clearing ucode active for station %d\n", i);
+			priv->stations[i].used &= ~IWL_STA_UCODE_ACTIVE;
+			cleared = true;
+		}
+	}
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+
+	if (!cleared)
+		IWL_DEBUG_INFO(priv,
+			"No active stations found to be cleared\n");
+}
+EXPORT_SYMBOL(iwl_legacy_clear_ucode_stations);
+
+/**
+ * iwl_legacy_restore_stations() - Restore driver known stations to device
+ *
+ * All stations considered active by driver, but not present in ucode, is
+ * restored.
+ *
+ * Function sleeps.
+ */
+void
+iwl_legacy_restore_stations(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+{
+	struct iwl_legacy_addsta_cmd sta_cmd;
+	struct iwl_link_quality_cmd lq;
+	unsigned long flags_spin;
+	int i;
+	bool found = false;
+	int ret;
+	bool send_lq;
+
+	if (!iwl_legacy_is_ready(priv)) {
+		IWL_DEBUG_INFO(priv,
+			"Not ready yet, not restoring any stations.\n");
+		return;
+	}
+
+	IWL_DEBUG_ASSOC(priv, "Restoring all known stations ... start.\n");
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	for (i = 0; i < priv->hw_params.max_stations; i++) {
+		if (ctx->ctxid != priv->stations[i].ctxid)
+			continue;
+		if ((priv->stations[i].used & IWL_STA_DRIVER_ACTIVE) &&
+			    !(priv->stations[i].used & IWL_STA_UCODE_ACTIVE)) {
+			IWL_DEBUG_ASSOC(priv, "Restoring sta %pM\n",
+					priv->stations[i].sta.sta.addr);
+			priv->stations[i].sta.mode = 0;
+			priv->stations[i].used |= IWL_STA_UCODE_INPROGRESS;
+			found = true;
+		}
+	}
+
+	for (i = 0; i < priv->hw_params.max_stations; i++) {
+		if ((priv->stations[i].used & IWL_STA_UCODE_INPROGRESS)) {
+			memcpy(&sta_cmd, &priv->stations[i].sta,
+			       sizeof(struct iwl_legacy_addsta_cmd));
+			send_lq = false;
+			if (priv->stations[i].lq) {
+				memcpy(&lq, priv->stations[i].lq,
+				       sizeof(struct iwl_link_quality_cmd));
+				send_lq = true;
+			}
+			spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+			ret = iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+			if (ret) {
+				spin_lock_irqsave(&priv->sta_lock, flags_spin);
+				IWL_ERR(priv, "Adding station %pM failed.\n",
+					priv->stations[i].sta.sta.addr);
+				priv->stations[i].used &=
+						~IWL_STA_DRIVER_ACTIVE;
+				priv->stations[i].used &=
+						~IWL_STA_UCODE_INPROGRESS;
+				spin_unlock_irqrestore(&priv->sta_lock,
+								flags_spin);
+			}
+			/*
+			 * Rate scaling has already been initialized, send
+			 * current LQ command
+			 */
+			if (send_lq)
+				iwl_legacy_send_lq_cmd(priv, ctx, &lq,
+								CMD_SYNC, true);
+			spin_lock_irqsave(&priv->sta_lock, flags_spin);
+			priv->stations[i].used &= ~IWL_STA_UCODE_INPROGRESS;
+		}
+	}
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+	if (!found)
+		IWL_DEBUG_INFO(priv, "Restoring all known stations"
+				" .... no stations to be restored.\n");
+	else
+		IWL_DEBUG_INFO(priv, "Restoring all known stations"
+				" .... complete.\n");
+}
+EXPORT_SYMBOL(iwl_legacy_restore_stations);
+
+int iwl_legacy_get_free_ucode_key_index(struct iwl_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->sta_key_max_num; i++)
+		if (!test_and_set_bit(i, &priv->ucode_key_table))
+			return i;
+
+	return WEP_INVALID_OFFSET;
+}
+EXPORT_SYMBOL(iwl_legacy_get_free_ucode_key_index);
+
+void iwl_legacy_dealloc_bcast_stations(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	for (i = 0; i < priv->hw_params.max_stations; i++) {
+		if (!(priv->stations[i].used & IWL_STA_BCAST))
+			continue;
+
+		priv->stations[i].used &= ~IWL_STA_UCODE_ACTIVE;
+		priv->num_stations--;
+		BUG_ON(priv->num_stations < 0);
+		kfree(priv->stations[i].lq);
+		priv->stations[i].lq = NULL;
+	}
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+EXPORT_SYMBOL_GPL(iwl_legacy_dealloc_bcast_stations);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+static void iwl_legacy_dump_lq_cmd(struct iwl_priv *priv,
+			   struct iwl_link_quality_cmd *lq)
+{
+	int i;
+	IWL_DEBUG_RATE(priv, "lq station id 0x%x\n", lq->sta_id);
+	IWL_DEBUG_RATE(priv, "lq ant 0x%X 0x%X\n",
+		       lq->general_params.single_stream_ant_msk,
+		       lq->general_params.dual_stream_ant_msk);
+
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
+		IWL_DEBUG_RATE(priv, "lq index %d 0x%X\n",
+			       i, lq->rs_table[i].rate_n_flags);
+}
+#else
+static inline void iwl_legacy_dump_lq_cmd(struct iwl_priv *priv,
+				   struct iwl_link_quality_cmd *lq)
+{
+}
+#endif
+
+/**
+ * iwl_legacy_is_lq_table_valid() - Test one aspect of LQ cmd for validity
+ *
+ * It sometimes happens when a HT rate has been in use and we
+ * loose connectivity with AP then mac80211 will first tell us that the
+ * current channel is not HT anymore before removing the station. In such a
+ * scenario the RXON flags will be updated to indicate we are not
+ * communicating HT anymore, but the LQ command may still contain HT rates.
+ * Test for this to prevent driver from sending LQ command between the time
+ * RXON flags are updated and when LQ command is updated.
+ */
+static bool iwl_legacy_is_lq_table_valid(struct iwl_priv *priv,
+			      struct iwl_rxon_context *ctx,
+			      struct iwl_link_quality_cmd *lq)
+{
+	int i;
+
+	if (ctx->ht.enabled)
+		return true;
+
+	IWL_DEBUG_INFO(priv, "Channel %u is not an HT channel\n",
+		       ctx->active.channel);
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++) {
+		if (le32_to_cpu(lq->rs_table[i].rate_n_flags) &
+						RATE_MCS_HT_MSK) {
+			IWL_DEBUG_INFO(priv,
+				       "index %d of LQ expects HT channel\n",
+				       i);
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * iwl_legacy_send_lq_cmd() - Send link quality command
+ * @init: This command is sent as part of station initialization right
+ *        after station has been added.
+ *
+ * The link quality command is sent as the last step of station creation.
+ * This is the special case in which init is set and we call a callback in
+ * this case to clear the state indicating that station creation is in
+ * progress.
+ */
+int iwl_legacy_send_lq_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
+		    struct iwl_link_quality_cmd *lq, u8 flags, bool init)
+{
+	int ret = 0;
+	unsigned long flags_spin;
+
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_TX_LINK_QUALITY_CMD,
+		.len = sizeof(struct iwl_link_quality_cmd),
+		.flags = flags,
+		.data = lq,
+	};
+
+	if (WARN_ON(lq->sta_id == IWL_INVALID_STATION))
+		return -EINVAL;
+
+
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	if (!(priv->stations[lq->sta_id].used & IWL_STA_DRIVER_ACTIVE)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return -EINVAL;
+	}
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+
+	iwl_legacy_dump_lq_cmd(priv, lq);
+	BUG_ON(init && (cmd.flags & CMD_ASYNC));
+
+	if (iwl_legacy_is_lq_table_valid(priv, ctx, lq))
+		ret = iwl_legacy_send_cmd(priv, &cmd);
+	else
+		ret = -EINVAL;
+
+	if (cmd.flags & CMD_ASYNC)
+		return ret;
+
+	if (init) {
+		IWL_DEBUG_INFO(priv, "init LQ command complete,"
+				" clearing sta addition status for sta %d\n",
+			       lq->sta_id);
+		spin_lock_irqsave(&priv->sta_lock, flags_spin);
+		priv->stations[lq->sta_id].used &= ~IWL_STA_UCODE_INPROGRESS;
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+	}
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_send_lq_cmd);
+
+int iwl_legacy_mac_sta_remove(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_station_priv_common *sta_common = (void *)sta->drv_priv;
+	int ret;
+
+	IWL_DEBUG_INFO(priv, "received request to remove station %pM\n",
+			sta->addr);
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_INFO(priv, "proceeding to remove station %pM\n",
+			sta->addr);
+	ret = iwl_legacy_remove_station(priv, sta_common->sta_id, sta->addr);
+	if (ret)
+		IWL_ERR(priv, "Error removing station %pM\n",
+			sta->addr);
+	mutex_unlock(&priv->mutex);
+	return ret;
+}
+EXPORT_SYMBOL(iwl_legacy_mac_sta_remove);
diff --git a/drivers/net/wireless/iwlegacy/iwl-sta.h b/drivers/net/wireless/iwlegacy/iwl-sta.h
new file mode 100644
index 000000000000..67bd75fe01a1
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-sta.h
@@ -0,0 +1,148 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+#ifndef __iwl_legacy_sta_h__
+#define __iwl_legacy_sta_h__
+
+#include "iwl-dev.h"
+
+#define HW_KEY_DYNAMIC 0
+#define HW_KEY_DEFAULT 1
+
+#define IWL_STA_DRIVER_ACTIVE BIT(0) /* driver entry is active */
+#define IWL_STA_UCODE_ACTIVE  BIT(1) /* ucode entry is active */
+#define IWL_STA_UCODE_INPROGRESS  BIT(2) /* ucode entry is in process of
+					    being activated */
+#define IWL_STA_LOCAL BIT(3) /* station state not directed by mac80211;
+				(this is for the IBSS BSSID stations) */
+#define IWL_STA_BCAST BIT(4) /* this station is the special bcast station */
+
+
+void iwl_legacy_restore_stations(struct iwl_priv *priv,
+				struct iwl_rxon_context *ctx);
+void iwl_legacy_clear_ucode_stations(struct iwl_priv *priv,
+			      struct iwl_rxon_context *ctx);
+void iwl_legacy_dealloc_bcast_stations(struct iwl_priv *priv);
+int iwl_legacy_get_free_ucode_key_index(struct iwl_priv *priv);
+int iwl_legacy_send_add_sta(struct iwl_priv *priv,
+			struct iwl_legacy_addsta_cmd *sta, u8 flags);
+int iwl_legacy_add_station_common(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			const u8 *addr, bool is_ap,
+			struct ieee80211_sta *sta, u8 *sta_id_r);
+int iwl_legacy_remove_station(struct iwl_priv *priv,
+			const u8 sta_id,
+			const u8 *addr);
+int iwl_legacy_mac_sta_remove(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			struct ieee80211_sta *sta);
+
+u8 iwl_legacy_prep_station(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			const u8 *addr, bool is_ap,
+			struct ieee80211_sta *sta);
+
+int iwl_legacy_send_lq_cmd(struct iwl_priv *priv,
+			struct iwl_rxon_context *ctx,
+			struct iwl_link_quality_cmd *lq,
+			u8 flags, bool init);
+
+/**
+ * iwl_legacy_clear_driver_stations - clear knowledge of all stations from driver
+ * @priv: iwl priv struct
+ *
+ * This is called during iwl_down() to make sure that in the case
+ * we're coming there from a hardware restart mac80211 will be
+ * able to reconfigure stations -- if we're getting there in the
+ * normal down flow then the stations will already be cleared.
+ */
+static inline void iwl_legacy_clear_driver_stations(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	struct iwl_rxon_context *ctx;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	memset(priv->stations, 0, sizeof(priv->stations));
+	priv->num_stations = 0;
+
+	priv->ucode_key_table = 0;
+
+	for_each_context(priv, ctx) {
+		/*
+		 * Remove all key information that is not stored as part
+		 * of station information since mac80211 may not have had
+		 * a chance to remove all the keys. When device is
+		 * reconfigured by mac80211 after an error all keys will
+		 * be reconfigured.
+		 */
+		memset(ctx->wep_keys, 0, sizeof(ctx->wep_keys));
+		ctx->key_mapping_keys = 0;
+	}
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+
+static inline int iwl_legacy_sta_id(struct ieee80211_sta *sta)
+{
+	if (WARN_ON(!sta))
+		return IWL_INVALID_STATION;
+
+	return ((struct iwl_station_priv_common *)sta->drv_priv)->sta_id;
+}
+
+/**
+ * iwl_legacy_sta_id_or_broadcast - return sta_id or broadcast sta
+ * @priv: iwl priv
+ * @context: the current context
+ * @sta: mac80211 station
+ *
+ * In certain circumstances mac80211 passes a station pointer
+ * that may be %NULL, for example during TX or key setup. In
+ * that case, we need to use the broadcast station, so this
+ * inline wraps that pattern.
+ */
+static inline int iwl_legacy_sta_id_or_broadcast(struct iwl_priv *priv,
+					  struct iwl_rxon_context *context,
+					  struct ieee80211_sta *sta)
+{
+	int sta_id;
+
+	if (!sta)
+		return context->bcast_sta_id;
+
+	sta_id = iwl_legacy_sta_id(sta);
+
+	/*
+	 * mac80211 should not be passing a partially
+	 * initialised station!
+	 */
+	WARN_ON(sta_id == IWL_INVALID_STATION);
+
+	return sta_id;
+}
+#endif /* __iwl_legacy_sta_h__ */
diff --git a/drivers/net/wireless/iwlegacy/iwl-tx.c b/drivers/net/wireless/iwlegacy/iwl-tx.c
new file mode 100644
index 000000000000..a227773cb384
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl-tx.c
@@ -0,0 +1,660 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/etherdevice.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <net/mac80211.h>
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+
+/**
+ * iwl_legacy_txq_update_write_ptr - Send new write index to hardware
+ */
+void
+iwl_legacy_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	u32 reg = 0;
+	int txq_id = txq->q.id;
+
+	if (txq->need_update == 0)
+		return;
+
+	/* if we're trying to save power */
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		/* wake up nic if it's powered down ...
+		 * uCode will wake up, and interrupt us again, so next
+		 * time we'll skip this part. */
+		reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			IWL_DEBUG_INFO(priv,
+					"Tx queue %d requesting wakeup,"
+					" GP1 = 0x%x\n", txq_id, reg);
+			iwl_legacy_set_bit(priv, CSR_GP_CNTRL,
+					CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			return;
+		}
+
+		iwl_legacy_write_direct32(priv, HBUS_TARG_WRPTR,
+				txq->q.write_ptr | (txq_id << 8));
+
+		/*
+		 * else not in power-save mode,
+		 * uCode will never sleep when we're
+		 * trying to tx (during RFKILL, we're not trying to tx).
+		 */
+	} else
+		iwl_write32(priv, HBUS_TARG_WRPTR,
+			    txq->q.write_ptr | (txq_id << 8));
+	txq->need_update = 0;
+}
+EXPORT_SYMBOL(iwl_legacy_txq_update_write_ptr);
+
+/**
+ * iwl_legacy_tx_queue_unmap -  Unmap any remaining DMA mappings and free skb's
+ */
+void iwl_legacy_tx_queue_unmap(struct iwl_priv *priv, int txq_id)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct iwl_queue *q = &txq->q;
+
+	if (q->n_bd == 0)
+		return;
+
+	while (q->write_ptr != q->read_ptr) {
+		priv->cfg->ops->lib->txq_free_tfd(priv, txq);
+		q->read_ptr = iwl_legacy_queue_inc_wrap(q->read_ptr, q->n_bd);
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_tx_queue_unmap);
+
+/**
+ * iwl_legacy_tx_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.
+ * 0-fill, but do not free "txq" descriptor structure.
+ */
+void iwl_legacy_tx_queue_free(struct iwl_priv *priv, int txq_id)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct device *dev = &priv->pci_dev->dev;
+	int i;
+
+	iwl_legacy_tx_queue_unmap(priv, txq_id);
+
+	/* De-alloc array of command/tx buffers */
+	for (i = 0; i < TFD_TX_CMD_SLOTS; i++)
+		kfree(txq->cmd[i]);
+
+	/* De-alloc circular buffer of TFDs */
+	if (txq->q.n_bd)
+		dma_free_coherent(dev, priv->hw_params.tfd_size *
+				  txq->q.n_bd, txq->tfds, txq->q.dma_addr);
+
+	/* De-alloc array of per-TFD driver data */
+	kfree(txq->txb);
+	txq->txb = NULL;
+
+	/* deallocate arrays */
+	kfree(txq->cmd);
+	kfree(txq->meta);
+	txq->cmd = NULL;
+	txq->meta = NULL;
+
+	/* 0-fill queue descriptor structure */
+	memset(txq, 0, sizeof(*txq));
+}
+EXPORT_SYMBOL(iwl_legacy_tx_queue_free);
+
+/**
+ * iwl_cmd_queue_unmap - Unmap any remaining DMA mappings from command queue
+ */
+void iwl_legacy_cmd_queue_unmap(struct iwl_priv *priv)
+{
+	struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
+	struct iwl_queue *q = &txq->q;
+	bool huge = false;
+	int i;
+
+	if (q->n_bd == 0)
+		return;
+
+	while (q->read_ptr != q->write_ptr) {
+		/* we have no way to tell if it is a huge cmd ATM */
+		i = iwl_legacy_get_cmd_index(q, q->read_ptr, 0);
+
+		if (txq->meta[i].flags & CMD_SIZE_HUGE)
+			huge = true;
+		else
+			pci_unmap_single(priv->pci_dev,
+					 dma_unmap_addr(&txq->meta[i], mapping),
+					 dma_unmap_len(&txq->meta[i], len),
+					 PCI_DMA_BIDIRECTIONAL);
+
+		q->read_ptr = iwl_legacy_queue_inc_wrap(q->read_ptr, q->n_bd);
+	}
+
+	if (huge) {
+		i = q->n_window;
+		pci_unmap_single(priv->pci_dev,
+				 dma_unmap_addr(&txq->meta[i], mapping),
+				 dma_unmap_len(&txq->meta[i], len),
+				 PCI_DMA_BIDIRECTIONAL);
+	}
+}
+EXPORT_SYMBOL(iwl_legacy_cmd_queue_unmap);
+
+/**
+ * iwl_legacy_cmd_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.
+ * 0-fill, but do not free "txq" descriptor structure.
+ */
+void iwl_legacy_cmd_queue_free(struct iwl_priv *priv)
+{
+	struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
+	struct device *dev = &priv->pci_dev->dev;
+	int i;
+
+	iwl_legacy_cmd_queue_unmap(priv);
+
+	/* De-alloc array of command/tx buffers */
+	for (i = 0; i <= TFD_CMD_SLOTS; i++)
+		kfree(txq->cmd[i]);
+
+	/* De-alloc circular buffer of TFDs */
+	if (txq->q.n_bd)
+		dma_free_coherent(dev, priv->hw_params.tfd_size * txq->q.n_bd,
+				  txq->tfds, txq->q.dma_addr);
+
+	/* deallocate arrays */
+	kfree(txq->cmd);
+	kfree(txq->meta);
+	txq->cmd = NULL;
+	txq->meta = NULL;
+
+	/* 0-fill queue descriptor structure */
+	memset(txq, 0, sizeof(*txq));
+}
+EXPORT_SYMBOL(iwl_legacy_cmd_queue_free);
+
+/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
+ * DMA services
+ *
+ * Theory of operation
+ *
+ * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
+ * of buffer descriptors, each of which points to one or more data buffers for
+ * the device to read from or fill.  Driver and device exchange status of each
+ * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
+ * entries in each circular buffer, to protect against confusing empty and full
+ * queue states.
+ *
+ * The device reads or writes the data in the queues via the device's several
+ * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
+ *
+ * For Tx queue, there are low mark and high mark limits. If, after queuing
+ * the packet for Tx, free space become < low mark, Tx queue stopped. When
+ * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
+ * Tx queue resumed.
+ *
+ * See more detailed info in iwl-4965-hw.h.
+ ***************************************************/
+
+int iwl_legacy_queue_space(const struct iwl_queue *q)
+{
+	int s = q->read_ptr - q->write_ptr;
+
+	if (q->read_ptr > q->write_ptr)
+		s -= q->n_bd;
+
+	if (s <= 0)
+		s += q->n_window;
+	/* keep some reserve to not confuse empty and full situations */
+	s -= 2;
+	if (s < 0)
+		s = 0;
+	return s;
+}
+EXPORT_SYMBOL(iwl_legacy_queue_space);
+
+
+/**
+ * iwl_legacy_queue_init - Initialize queue's high/low-water and read/write indexes
+ */
+static int iwl_legacy_queue_init(struct iwl_priv *priv, struct iwl_queue *q,
+			  int count, int slots_num, u32 id)
+{
+	q->n_bd = count;
+	q->n_window = slots_num;
+	q->id = id;
+
+	/* count must be power-of-two size, otherwise iwl_legacy_queue_inc_wrap
+	 * and iwl_legacy_queue_dec_wrap are broken. */
+	BUG_ON(!is_power_of_2(count));
+
+	/* slots_num must be power-of-two size, otherwise
+	 * iwl_legacy_get_cmd_index is broken. */
+	BUG_ON(!is_power_of_2(slots_num));
+
+	q->low_mark = q->n_window / 4;
+	if (q->low_mark < 4)
+		q->low_mark = 4;
+
+	q->high_mark = q->n_window / 8;
+	if (q->high_mark < 2)
+		q->high_mark = 2;
+
+	q->write_ptr = q->read_ptr = 0;
+
+	return 0;
+}
+
+/**
+ * iwl_legacy_tx_queue_alloc - Alloc driver data and TFD CB for one Tx/cmd queue
+ */
+static int iwl_legacy_tx_queue_alloc(struct iwl_priv *priv,
+			      struct iwl_tx_queue *txq, u32 id)
+{
+	struct device *dev = &priv->pci_dev->dev;
+	size_t tfd_sz = priv->hw_params.tfd_size * TFD_QUEUE_SIZE_MAX;
+
+	/* Driver private data, only for Tx (not command) queues,
+	 * not shared with device. */
+	if (id != priv->cmd_queue) {
+		txq->txb = kzalloc(sizeof(txq->txb[0]) *
+				   TFD_QUEUE_SIZE_MAX, GFP_KERNEL);
+		if (!txq->txb) {
+			IWL_ERR(priv, "kmalloc for auxiliary BD "
+				  "structures failed\n");
+			goto error;
+		}
+	} else {
+		txq->txb = NULL;
+	}
+
+	/* Circular buffer of transmit frame descriptors (TFDs),
+	 * shared with device */
+	txq->tfds = dma_alloc_coherent(dev, tfd_sz, &txq->q.dma_addr,
+				       GFP_KERNEL);
+	if (!txq->tfds) {
+		IWL_ERR(priv, "pci_alloc_consistent(%zd) failed\n", tfd_sz);
+		goto error;
+	}
+	txq->q.id = id;
+
+	return 0;
+
+ error:
+	kfree(txq->txb);
+	txq->txb = NULL;
+
+	return -ENOMEM;
+}
+
+/**
+ * iwl_legacy_tx_queue_init - Allocate and initialize one tx/cmd queue
+ */
+int iwl_legacy_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq,
+		      int slots_num, u32 txq_id)
+{
+	int i, len;
+	int ret;
+	int actual_slots = slots_num;
+
+	/*
+	 * Alloc buffer array for commands (Tx or other types of commands).
+	 * For the command queue (#4/#9), allocate command space + one big
+	 * command for scan, since scan command is very huge; the system will
+	 * not have two scans at the same time, so only one is needed.
+	 * For normal Tx queues (all other queues), no super-size command
+	 * space is needed.
+	 */
+	if (txq_id == priv->cmd_queue)
+		actual_slots++;
+
+	txq->meta = kzalloc(sizeof(struct iwl_cmd_meta) * actual_slots,
+			    GFP_KERNEL);
+	txq->cmd = kzalloc(sizeof(struct iwl_device_cmd *) * actual_slots,
+			   GFP_KERNEL);
+
+	if (!txq->meta || !txq->cmd)
+		goto out_free_arrays;
+
+	len = sizeof(struct iwl_device_cmd);
+	for (i = 0; i < actual_slots; i++) {
+		/* only happens for cmd queue */
+		if (i == slots_num)
+			len = IWL_MAX_CMD_SIZE;
+
+		txq->cmd[i] = kmalloc(len, GFP_KERNEL);
+		if (!txq->cmd[i])
+			goto err;
+	}
+
+	/* Alloc driver data array and TFD circular buffer */
+	ret = iwl_legacy_tx_queue_alloc(priv, txq, txq_id);
+	if (ret)
+		goto err;
+
+	txq->need_update = 0;
+
+	/*
+	 * For the default queues 0-3, set up the swq_id
+	 * already -- all others need to get one later
+	 * (if they need one at all).
+	 */
+	if (txq_id < 4)
+		iwl_legacy_set_swq_id(txq, txq_id, txq_id);
+
+	/* TFD_QUEUE_SIZE_MAX must be power-of-two size, otherwise
+	 * iwl_legacy_queue_inc_wrap and iwl_legacy_queue_dec_wrap are broken. */
+	BUILD_BUG_ON(TFD_QUEUE_SIZE_MAX & (TFD_QUEUE_SIZE_MAX - 1));
+
+	/* Initialize queue's high/low-water marks, and head/tail indexes */
+	iwl_legacy_queue_init(priv, &txq->q,
+				TFD_QUEUE_SIZE_MAX, slots_num, txq_id);
+
+	/* Tell device where to find queue */
+	priv->cfg->ops->lib->txq_init(priv, txq);
+
+	return 0;
+err:
+	for (i = 0; i < actual_slots; i++)
+		kfree(txq->cmd[i]);
+out_free_arrays:
+	kfree(txq->meta);
+	kfree(txq->cmd);
+
+	return -ENOMEM;
+}
+EXPORT_SYMBOL(iwl_legacy_tx_queue_init);
+
+void iwl_legacy_tx_queue_reset(struct iwl_priv *priv, struct iwl_tx_queue *txq,
+			int slots_num, u32 txq_id)
+{
+	int actual_slots = slots_num;
+
+	if (txq_id == priv->cmd_queue)
+		actual_slots++;
+
+	memset(txq->meta, 0, sizeof(struct iwl_cmd_meta) * actual_slots);
+
+	txq->need_update = 0;
+
+	/* Initialize queue's high/low-water marks, and head/tail indexes */
+	iwl_legacy_queue_init(priv, &txq->q,
+				TFD_QUEUE_SIZE_MAX, slots_num, txq_id);
+
+	/* Tell device where to find queue */
+	priv->cfg->ops->lib->txq_init(priv, txq);
+}
+EXPORT_SYMBOL(iwl_legacy_tx_queue_reset);
+
+/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
+
+/**
+ * iwl_legacy_enqueue_hcmd - enqueue a uCode command
+ * @priv: device private data point
+ * @cmd: a point to the ucode command structure
+ *
+ * The function returns < 0 values to indicate the operation is
+ * failed. On success, it turns the index (> 0) of command in the
+ * command queue.
+ */
+int iwl_legacy_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
+	struct iwl_queue *q = &txq->q;
+	struct iwl_device_cmd *out_cmd;
+	struct iwl_cmd_meta *out_meta;
+	dma_addr_t phys_addr;
+	unsigned long flags;
+	int len;
+	u32 idx;
+	u16 fix_size;
+
+	cmd->len = priv->cfg->ops->utils->get_hcmd_size(cmd->id, cmd->len);
+	fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
+
+	/* If any of the command structures end up being larger than
+	 * the TFD_MAX_PAYLOAD_SIZE, and it sent as a 'small' command then
+	 * we will need to increase the size of the TFD entries
+	 * Also, check to see if command buffer should not exceed the size
+	 * of device_cmd and max_cmd_size. */
+	BUG_ON((fix_size > TFD_MAX_PAYLOAD_SIZE) &&
+	       !(cmd->flags & CMD_SIZE_HUGE));
+	BUG_ON(fix_size > IWL_MAX_CMD_SIZE);
+
+	if (iwl_legacy_is_rfkill(priv) || iwl_legacy_is_ctkill(priv)) {
+		IWL_WARN(priv, "Not sending command - %s KILL\n",
+			 iwl_legacy_is_rfkill(priv) ? "RF" : "CT");
+		return -EIO;
+	}
+
+	if (iwl_legacy_queue_space(q) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
+		IWL_ERR(priv, "No space in command queue\n");
+		IWL_ERR(priv, "Restarting adapter due to queue full\n");
+		queue_work(priv->workqueue, &priv->restart);
+		return -ENOSPC;
+	}
+
+	spin_lock_irqsave(&priv->hcmd_lock, flags);
+
+	/* If this is a huge cmd, mark the huge flag also on the meta.flags
+	 * of the _original_ cmd. This is used for DMA mapping clean up.
+	 */
+	if (cmd->flags & CMD_SIZE_HUGE) {
+		idx = iwl_legacy_get_cmd_index(q, q->write_ptr, 0);
+		txq->meta[idx].flags = CMD_SIZE_HUGE;
+	}
+
+	idx = iwl_legacy_get_cmd_index(q, q->write_ptr, cmd->flags & CMD_SIZE_HUGE);
+	out_cmd = txq->cmd[idx];
+	out_meta = &txq->meta[idx];
+
+	memset(out_meta, 0, sizeof(*out_meta));	/* re-initialize to NULL */
+	out_meta->flags = cmd->flags;
+	if (cmd->flags & CMD_WANT_SKB)
+		out_meta->source = cmd;
+	if (cmd->flags & CMD_ASYNC)
+		out_meta->callback = cmd->callback;
+
+	out_cmd->hdr.cmd = cmd->id;
+	memcpy(&out_cmd->cmd.payload, cmd->data, cmd->len);
+
+	/* At this point, the out_cmd now has all of the incoming cmd
+	 * information */
+
+	out_cmd->hdr.flags = 0;
+	out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(priv->cmd_queue) |
+			INDEX_TO_SEQ(q->write_ptr));
+	if (cmd->flags & CMD_SIZE_HUGE)
+		out_cmd->hdr.sequence |= SEQ_HUGE_FRAME;
+	len = sizeof(struct iwl_device_cmd);
+	if (idx == TFD_CMD_SLOTS)
+		len = IWL_MAX_CMD_SIZE;
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	switch (out_cmd->hdr.cmd) {
+	case REPLY_TX_LINK_QUALITY_CMD:
+	case SENSITIVITY_CMD:
+		IWL_DEBUG_HC_DUMP(priv,
+				"Sending command %s (#%x), seq: 0x%04X, "
+				"%d bytes at %d[%d]:%d\n",
+				iwl_legacy_get_cmd_string(out_cmd->hdr.cmd),
+				out_cmd->hdr.cmd,
+				le16_to_cpu(out_cmd->hdr.sequence), fix_size,
+				q->write_ptr, idx, priv->cmd_queue);
+		break;
+	default:
+		IWL_DEBUG_HC(priv, "Sending command %s (#%x), seq: 0x%04X, "
+				"%d bytes at %d[%d]:%d\n",
+				iwl_legacy_get_cmd_string(out_cmd->hdr.cmd),
+				out_cmd->hdr.cmd,
+				le16_to_cpu(out_cmd->hdr.sequence), fix_size,
+				q->write_ptr, idx, priv->cmd_queue);
+	}
+#endif
+	txq->need_update = 1;
+
+	if (priv->cfg->ops->lib->txq_update_byte_cnt_tbl)
+		/* Set up entry in queue's byte count circular buffer */
+		priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, 0);
+
+	phys_addr = pci_map_single(priv->pci_dev, &out_cmd->hdr,
+				   fix_size, PCI_DMA_BIDIRECTIONAL);
+	dma_unmap_addr_set(out_meta, mapping, phys_addr);
+	dma_unmap_len_set(out_meta, len, fix_size);
+
+	trace_iwlwifi_legacy_dev_hcmd(priv, &out_cmd->hdr,
+						fix_size, cmd->flags);
+
+	priv->cfg->ops->lib->txq_attach_buf_to_tfd(priv, txq,
+						   phys_addr, fix_size, 1,
+						   U32_PAD(cmd->len));
+
+	/* Increment and update queue's write index */
+	q->write_ptr = iwl_legacy_queue_inc_wrap(q->write_ptr, q->n_bd);
+	iwl_legacy_txq_update_write_ptr(priv, txq);
+
+	spin_unlock_irqrestore(&priv->hcmd_lock, flags);
+	return idx;
+}
+
+/**
+ * iwl_legacy_hcmd_queue_reclaim - Reclaim TX command queue entries already Tx'd
+ *
+ * When FW advances 'R' index, all entries between old and new 'R' index
+ * need to be reclaimed. As result, some free space forms.  If there is
+ * enough free space (> low mark), wake the stack that feeds us.
+ */
+static void iwl_legacy_hcmd_queue_reclaim(struct iwl_priv *priv, int txq_id,
+				   int idx, int cmd_idx)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct iwl_queue *q = &txq->q;
+	int nfreed = 0;
+
+	if ((idx >= q->n_bd) || (iwl_legacy_queue_used(q, idx) == 0)) {
+		IWL_ERR(priv, "Read index for DMA queue txq id (%d), index %d, "
+			  "is out of range [0-%d] %d %d.\n", txq_id,
+			  idx, q->n_bd, q->write_ptr, q->read_ptr);
+		return;
+	}
+
+	for (idx = iwl_legacy_queue_inc_wrap(idx, q->n_bd); q->read_ptr != idx;
+	     q->read_ptr = iwl_legacy_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+
+		if (nfreed++ > 0) {
+			IWL_ERR(priv, "HCMD skipped: index (%d) %d %d\n", idx,
+					q->write_ptr, q->read_ptr);
+			queue_work(priv->workqueue, &priv->restart);
+		}
+
+	}
+}
+
+/**
+ * iwl_legacy_tx_cmd_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_legacy_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+	int txq_id = SEQ_TO_QUEUE(sequence);
+	int index = SEQ_TO_INDEX(sequence);
+	int cmd_index;
+	bool huge = !!(pkt->hdr.sequence & SEQ_HUGE_FRAME);
+	struct iwl_device_cmd *cmd;
+	struct iwl_cmd_meta *meta;
+	struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
+
+	/* If a Tx command is being handled and it isn't in the actual
+	 * command queue then there a command routing bug has been introduced
+	 * in the queue management code. */
+	if (WARN(txq_id != priv->cmd_queue,
+		 "wrong command queue %d (should be %d), sequence 0x%X readp=%d writep=%d\n",
+		  txq_id, priv->cmd_queue, sequence,
+		  priv->txq[priv->cmd_queue].q.read_ptr,
+		  priv->txq[priv->cmd_queue].q.write_ptr)) {
+		iwl_print_hex_error(priv, pkt, 32);
+		return;
+	}
+
+	/* If this is a huge cmd, clear the huge flag on the meta.flags
+	 * of the _original_ cmd. So that iwl_legacy_cmd_queue_free won't unmap
+	 * the DMA buffer for the scan (huge) command.
+	 */
+	if (huge) {
+		cmd_index = iwl_legacy_get_cmd_index(&txq->q, index, 0);
+		txq->meta[cmd_index].flags = 0;
+	}
+	cmd_index = iwl_legacy_get_cmd_index(&txq->q, index, huge);
+	cmd = txq->cmd[cmd_index];
+	meta = &txq->meta[cmd_index];
+
+	pci_unmap_single(priv->pci_dev,
+			 dma_unmap_addr(meta, mapping),
+			 dma_unmap_len(meta, len),
+			 PCI_DMA_BIDIRECTIONAL);
+
+	/* Input error checking is done when commands are added to queue. */
+	if (meta->flags & CMD_WANT_SKB) {
+		meta->source->reply_page = (unsigned long)rxb_addr(rxb);
+		rxb->page = NULL;
+	} else if (meta->callback)
+		meta->callback(priv, cmd, pkt);
+
+	iwl_legacy_hcmd_queue_reclaim(priv, txq_id, index, cmd_index);
+
+	if (!(meta->flags & CMD_ASYNC)) {
+		clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+		IWL_DEBUG_INFO(priv, "Clearing HCMD_ACTIVE for command %s\n",
+			       iwl_legacy_get_cmd_string(cmd->hdr.cmd));
+		wake_up_interruptible(&priv->wait_command_queue);
+	}
+	meta->flags = 0;
+}
+EXPORT_SYMBOL(iwl_legacy_tx_cmd_complete);
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlegacy/iwl3945-base.c
index 371abbf60eac..ab87e1b73529 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlegacy/iwl3945-base.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2010 Intel Corporation. All rights reserved.
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -61,7 +61,6 @@
 #include "iwl-helpers.h"
 #include "iwl-dev.h"
 #include "iwl-spectrum.h"
-#include "iwl-legacy.h"
 
 /*
  * module name, copyright, version, etc.
@@ -70,7 +69,7 @@
 #define DRV_DESCRIPTION	\
 "Intel(R) PRO/Wireless 3945ABG/BG Network Connection driver for Linux"
 
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 #define VD "d"
 #else
 #define VD
@@ -82,7 +81,7 @@
  * this was configurable.
  */
 #define DRV_VERSION  IWLWIFI_VERSION VD "s"
-#define DRV_COPYRIGHT	"Copyright(c) 2003-2010 Intel Corporation"
+#define DRV_COPYRIGHT	"Copyright(c) 2003-2011 Intel Corporation"
 #define DRV_AUTHOR     "<ilw@linux.intel.com>"
 
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
@@ -164,7 +163,7 @@ static int iwl3945_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
 	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
 			== STA_KEY_FLG_NO_ENC)
 		priv->stations[sta_id].sta.key.key_offset =
-				 iwl_get_free_ucode_key_index(priv);
+				 iwl_legacy_get_free_ucode_key_index(priv);
 	/* else, we are overriding an existing key => no need to allocated room
 	* in uCode. */
 
@@ -177,7 +176,8 @@ static int iwl3945_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
 
 	IWL_DEBUG_INFO(priv, "hwcrypto: modify ucode station key info\n");
 
-	ret = iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+	ret = iwl_legacy_send_add_sta(priv,
+				&priv->stations[sta_id].sta, CMD_ASYNC);
 
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
@@ -201,7 +201,7 @@ static int iwl3945_set_wep_dynamic_key_info(struct iwl_priv *priv,
 static int iwl3945_clear_sta_key_info(struct iwl_priv *priv, u8 sta_id)
 {
 	unsigned long flags;
-	struct iwl_addsta_cmd sta_cmd;
+	struct iwl_legacy_addsta_cmd sta_cmd;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
 	memset(&priv->stations[sta_id].keyinfo, 0, sizeof(struct iwl_hw_key));
@@ -210,11 +210,11 @@ static int iwl3945_clear_sta_key_info(struct iwl_priv *priv, u8 sta_id)
 	priv->stations[sta_id].sta.key.key_flags = STA_KEY_FLG_NO_ENC;
 	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
 	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
-	memcpy(&sta_cmd, &priv->stations[sta_id].sta, sizeof(struct iwl_addsta_cmd));
+	memcpy(&sta_cmd, &priv->stations[sta_id].sta, sizeof(struct iwl_legacy_addsta_cmd));
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
 	IWL_DEBUG_INFO(priv, "hwcrypto: clear ucode station key info\n");
-	return iwl_send_add_sta(priv, &sta_cmd, CMD_SYNC);
+	return iwl_legacy_send_add_sta(priv, &sta_cmd, CMD_SYNC);
 }
 
 static int iwl3945_set_dynamic_key(struct iwl_priv *priv,
@@ -318,7 +318,7 @@ unsigned int iwl3945_fill_beacon_frame(struct iwl_priv *priv,
 				int left)
 {
 
-	if (!iwl_is_associated(priv, IWL_RXON_CTX_BSS) || !priv->beacon_skb)
+	if (!iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS) || !priv->beacon_skb)
 		return 0;
 
 	if (priv->beacon_skb->len > left)
@@ -344,12 +344,12 @@ static int iwl3945_send_beacon_cmd(struct iwl_priv *priv)
 		return -ENOMEM;
 	}
 
-	rate = iwl_rate_get_lowest_plcp(priv,
+	rate = iwl_legacy_get_lowest_plcp(priv,
 				&priv->contexts[IWL_RXON_CTX_BSS]);
 
 	frame_size = iwl3945_hw_get_beacon_cmd(priv, frame, rate);
 
-	rc = iwl_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
+	rc = iwl_legacy_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
 			      &frame->u.cmd[0]);
 
 	iwl3945_free_frame(priv, frame);
@@ -443,7 +443,7 @@ static void iwl3945_build_tx_cmd_basic(struct iwl_priv *priv,
 		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
 	}
 
-	priv->cfg->ops->utils->tx_cmd_protection(priv, info, fc, &tx_flags);
+	iwl_legacy_tx_cmd_protection(priv, info, fc, &tx_flags);
 
 	tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
 	if (ieee80211_is_mgmt(fc)) {
@@ -485,7 +485,7 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->lock, flags);
-	if (iwl_is_rfkill(priv)) {
+	if (iwl_legacy_is_rfkill(priv)) {
 		IWL_DEBUG_DROP(priv, "Dropping - RF KILL\n");
 		goto drop_unlock;
 	}
@@ -500,7 +500,7 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 
 	fc = hdr->frame_control;
 
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 	if (ieee80211_is_auth(fc))
 		IWL_DEBUG_TX(priv, "Sending AUTH frame\n");
 	else if (ieee80211_is_assoc_req(fc))
@@ -514,7 +514,7 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	hdr_len = ieee80211_hdrlen(fc);
 
 	/* Find index into station table for destination station */
-	sta_id = iwl_sta_id_or_broadcast(
+	sta_id = iwl_legacy_sta_id_or_broadcast(
 			priv, &priv->contexts[IWL_RXON_CTX_BSS],
 			info->control.sta);
 	if (sta_id == IWL_INVALID_STATION) {
@@ -536,12 +536,12 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	txq = &priv->txq[txq_id];
 	q = &txq->q;
 
-	if ((iwl_queue_space(q) < q->high_mark))
+	if ((iwl_legacy_queue_space(q) < q->high_mark))
 		goto drop;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	idx = get_cmd_index(q, q->write_ptr, 0);
+	idx = iwl_legacy_get_cmd_index(q, q->write_ptr, 0);
 
 	/* Set up driver data for this TFD */
 	memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl_tx_info));
@@ -582,8 +582,8 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	len = (u16)skb->len;
 	tx_cmd->len = cpu_to_le16(len);
 
-	iwl_dbg_log_tx_data_frame(priv, len, hdr);
-	iwl_update_stats(priv, true, fc, len);
+	iwl_legacy_dbg_log_tx_data_frame(priv, len, hdr);
+	iwl_legacy_update_stats(priv, true, fc, len);
 	tx_cmd->tx_flags &= ~TX_CMD_FLG_ANT_A_MSK;
 	tx_cmd->tx_flags &= ~TX_CMD_FLG_ANT_B_MSK;
 
@@ -642,20 +642,20 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 
 
 	/* Tell device the write index *just past* this latest filled TFD */
-	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
-	iwl_txq_update_write_ptr(priv, txq);
+	q->write_ptr = iwl_legacy_queue_inc_wrap(q->write_ptr, q->n_bd);
+	iwl_legacy_txq_update_write_ptr(priv, txq);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	if ((iwl_queue_space(q) < q->high_mark)
+	if ((iwl_legacy_queue_space(q) < q->high_mark)
 	    && priv->mac80211_registered) {
 		if (wait_write_ptr) {
 			spin_lock_irqsave(&priv->lock, flags);
 			txq->need_update = 1;
-			iwl_txq_update_write_ptr(priv, txq);
+			iwl_legacy_txq_update_write_ptr(priv, txq);
 			spin_unlock_irqrestore(&priv->lock, flags);
 		}
 
-		iwl_stop_queue(priv, txq);
+		iwl_legacy_stop_queue(priv, txq);
 	}
 
 	return 0;
@@ -683,8 +683,8 @@ static int iwl3945_get_measurement(struct iwl_priv *priv,
 	int duration = le16_to_cpu(params->duration);
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
 
-	if (iwl_is_associated(priv, IWL_RXON_CTX_BSS))
-		add_time = iwl_usecs_to_beacons(priv,
+	if (iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS))
+		add_time = iwl_legacy_usecs_to_beacons(priv,
 			le64_to_cpu(params->start_time) - priv->_3945.last_tsf,
 			le16_to_cpu(ctx->timing.beacon_interval));
 
@@ -697,9 +697,9 @@ static int iwl3945_get_measurement(struct iwl_priv *priv,
 	cmd.len = sizeof(spectrum);
 	spectrum.len = cpu_to_le16(cmd.len - sizeof(spectrum.len));
 
-	if (iwl_is_associated(priv, IWL_RXON_CTX_BSS))
+	if (iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS))
 		spectrum.start_time =
-			iwl_add_beacon_time(priv,
+			iwl_legacy_add_beacon_time(priv,
 				priv->_3945.last_beacon_time, add_time,
 				le16_to_cpu(ctx->timing.beacon_interval));
 	else
@@ -712,7 +712,7 @@ static int iwl3945_get_measurement(struct iwl_priv *priv,
 		spectrum.flags |= RXON_FLG_BAND_24G_MSK |
 		    RXON_FLG_AUTO_DETECT_MSK | RXON_FLG_TGG_PROTECT_MSK;
 
-	rc = iwl_send_cmd_sync(priv, &cmd);
+	rc = iwl_legacy_send_cmd_sync(priv, &cmd);
 	if (rc)
 		return rc;
 
@@ -739,7 +739,7 @@ static int iwl3945_get_measurement(struct iwl_priv *priv,
 		break;
 	}
 
-	iwl_free_pages(priv, cmd.reply_page);
+	iwl_legacy_free_pages(priv, cmd.reply_page);
 
 	return rc;
 }
@@ -783,45 +783,19 @@ static void iwl3945_rx_reply_alive(struct iwl_priv *priv,
 static void iwl3945_rx_reply_add_sta(struct iwl_priv *priv,
 				 struct iwl_rx_mem_buffer *rxb)
 {
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 #endif
 
 	IWL_DEBUG_RX(priv, "Received REPLY_ADD_STA: 0x%02X\n", pkt->u.status);
 }
 
-static void iwl3945_bg_beacon_update(struct work_struct *work)
-{
-	struct iwl_priv *priv =
-		container_of(work, struct iwl_priv, beacon_update);
-	struct sk_buff *beacon;
-
-	/* Pull updated AP beacon from mac80211. will fail if not in AP mode */
-	beacon = ieee80211_beacon_get(priv->hw,
-			priv->contexts[IWL_RXON_CTX_BSS].vif);
-
-	if (!beacon) {
-		IWL_ERR(priv, "update beacon failed\n");
-		return;
-	}
-
-	mutex_lock(&priv->mutex);
-	/* new beacon skb is allocated every time; dispose previous.*/
-	if (priv->beacon_skb)
-		dev_kfree_skb(priv->beacon_skb);
-
-	priv->beacon_skb = beacon;
-	mutex_unlock(&priv->mutex);
-
-	iwl3945_send_beacon_cmd(priv);
-}
-
 static void iwl3945_rx_beacon_notif(struct iwl_priv *priv,
 				struct iwl_rx_mem_buffer *rxb)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl3945_beacon_notif *beacon = &(pkt->u.beacon_status);
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 	u8 rate = beacon->beacon_notify_hdr.rate;
 
 	IWL_DEBUG_RX(priv, "beacon status %x retries %d iss %d "
@@ -835,9 +809,6 @@ static void iwl3945_rx_beacon_notif(struct iwl_priv *priv,
 
 	priv->ibss_manager = le32_to_cpu(beacon->ibss_mgr_status);
 
-	if ((priv->iw_mode == NL80211_IFTYPE_AP) &&
-	    (!test_bit(STATUS_EXIT_PENDING, &priv->status)))
-		queue_work(priv->workqueue, &priv->beacon_update);
 }
 
 /* Handle notification from uCode that card's power state is changing
@@ -862,7 +833,7 @@ static void iwl3945_rx_card_state_notif(struct iwl_priv *priv,
 		clear_bit(STATUS_RF_KILL_HW, &priv->status);
 
 
-	iwl_scan_cancel(priv);
+	iwl_legacy_scan_cancel(priv);
 
 	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
 	     test_bit(STATUS_RF_KILL_HW, &priv->status)))
@@ -885,13 +856,13 @@ static void iwl3945_setup_rx_handlers(struct iwl_priv *priv)
 {
 	priv->rx_handlers[REPLY_ALIVE] = iwl3945_rx_reply_alive;
 	priv->rx_handlers[REPLY_ADD_STA] = iwl3945_rx_reply_add_sta;
-	priv->rx_handlers[REPLY_ERROR] = iwl_rx_reply_error;
-	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl_rx_csa;
+	priv->rx_handlers[REPLY_ERROR] = iwl_legacy_rx_reply_error;
+	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl_legacy_rx_csa;
 	priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
-			iwl_rx_spectrum_measure_notif;
-	priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_rx_pm_sleep_notif;
+			iwl_legacy_rx_spectrum_measure_notif;
+	priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_legacy_rx_pm_sleep_notif;
 	priv->rx_handlers[PM_DEBUG_STATISTIC_NOTIFIC] =
-	    iwl_rx_pm_debug_statistics_notif;
+	    iwl_legacy_rx_pm_debug_statistics_notif;
 	priv->rx_handlers[BEACON_NOTIFICATION] = iwl3945_rx_beacon_notif;
 
 	/*
@@ -902,7 +873,7 @@ static void iwl3945_setup_rx_handlers(struct iwl_priv *priv)
 	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl3945_reply_statistics;
 	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl3945_hw_rx_statistics;
 
-	iwl_setup_rx_scan_handlers(priv);
+	iwl_legacy_setup_rx_scan_handlers(priv);
 	priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl3945_rx_card_state_notif;
 
 	/* Set up hardware specific Rx handlers */
@@ -1003,7 +974,7 @@ static void iwl3945_rx_queue_restock(struct iwl_priv *priv)
 
 	spin_lock_irqsave(&rxq->lock, flags);
 	write = rxq->write & ~0x7;
-	while ((iwl_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
+	while ((iwl_legacy_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
 		/* Get next free Rx buffer, remove from free list */
 		element = rxq->rx_free.next;
 		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
@@ -1029,7 +1000,7 @@ static void iwl3945_rx_queue_restock(struct iwl_priv *priv)
 		spin_lock_irqsave(&rxq->lock, flags);
 		rxq->need_update = 1;
 		spin_unlock_irqrestore(&rxq->lock, flags);
-		iwl_rx_queue_update_write_ptr(priv, rxq);
+		iwl_legacy_rx_queue_update_write_ptr(priv, rxq);
 	}
 }
 
@@ -1123,7 +1094,7 @@ void iwl3945_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
 			pci_unmap_page(priv->pci_dev, rxq->pool[i].page_dma,
 				PAGE_SIZE << priv->hw_params.rx_page_order,
 				PCI_DMA_FROMDEVICE);
-			__iwl_free_pages(priv, rxq->pool[i].page);
+			__iwl_legacy_free_pages(priv, rxq->pool[i].page);
 			rxq->pool[i].page = NULL;
 		}
 		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
@@ -1170,7 +1141,7 @@ static void iwl3945_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rx
 			pci_unmap_page(priv->pci_dev, rxq->pool[i].page_dma,
 				PAGE_SIZE << priv->hw_params.rx_page_order,
 				PCI_DMA_FROMDEVICE);
-			__iwl_free_pages(priv, rxq->pool[i].page);
+			__iwl_legacy_free_pages(priv, rxq->pool[i].page);
 			rxq->pool[i].page = NULL;
 		}
 	}
@@ -1275,7 +1246,7 @@ static void iwl3945_rx_handle(struct iwl_priv *priv)
 
 		len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
 		len += sizeof(u32); /* account for status word */
-		trace_iwlwifi_dev_rx(priv, pkt, len);
+		trace_iwlwifi_legacy_dev_rx(priv, pkt, len);
 
 		/* Reclaim a command buffer only if this packet is a response
 		 *   to a (driver-originated) command.
@@ -1292,14 +1263,14 @@ static void iwl3945_rx_handle(struct iwl_priv *priv)
 		 *   rx_handlers table.  See iwl3945_setup_rx_handlers() */
 		if (priv->rx_handlers[pkt->hdr.cmd]) {
 			IWL_DEBUG_RX(priv, "r = %d, i = %d, %s, 0x%02x\n", r, i,
-				get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
+			iwl_legacy_get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
 			priv->isr_stats.rx_handlers[pkt->hdr.cmd]++;
 			priv->rx_handlers[pkt->hdr.cmd] (priv, rxb);
 		} else {
 			/* No handling needed */
 			IWL_DEBUG_RX(priv,
 				"r %d i %d No handler needed for %s, 0x%02x\n",
-				r, i, get_cmd_string(pkt->hdr.cmd),
+				r, i, iwl_legacy_get_cmd_string(pkt->hdr.cmd),
 				pkt->hdr.cmd);
 		}
 
@@ -1312,10 +1283,10 @@ static void iwl3945_rx_handle(struct iwl_priv *priv)
 
 		if (reclaim) {
 			/* Invoke any callbacks, transfer the buffer to caller,
-			 * and fire off the (possibly) blocking iwl_send_cmd()
+			 * and fire off the (possibly) blocking iwl_legacy_send_cmd()
 			 * as we reclaim the driver command queue */
 			if (rxb->page)
-				iwl_tx_cmd_complete(priv, rxb);
+				iwl_legacy_tx_cmd_complete(priv, rxb);
 			else
 				IWL_WARN(priv, "Claim null rxb?\n");
 		}
@@ -1357,14 +1328,14 @@ static void iwl3945_rx_handle(struct iwl_priv *priv)
 }
 
 /* call this function to flush any scheduled tasklet */
-static inline void iwl_synchronize_irq(struct iwl_priv *priv)
+static inline void iwl3945_synchronize_irq(struct iwl_priv *priv)
 {
 	/* wait to make sure we flush pending tasklet*/
 	synchronize_irq(priv->pci_dev->irq);
 	tasklet_kill(&priv->irq_tasklet);
 }
 
-static const char *desc_lookup(int i)
+static const char *iwl3945_desc_lookup(int i)
 {
 	switch (i) {
 	case 1:
@@ -1401,7 +1372,7 @@ void iwl3945_dump_nic_error_log(struct iwl_priv *priv)
 	}
 
 
-	count = iwl_read_targ_mem(priv, base);
+	count = iwl_legacy_read_targ_mem(priv, base);
 
 	if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
 		IWL_ERR(priv, "Start IWL Error Log Dump:\n");
@@ -1414,25 +1385,25 @@ void iwl3945_dump_nic_error_log(struct iwl_priv *priv)
 	for (i = ERROR_START_OFFSET;
 	     i < (count * ERROR_ELEM_SIZE) + ERROR_START_OFFSET;
 	     i += ERROR_ELEM_SIZE) {
-		desc = iwl_read_targ_mem(priv, base + i);
+		desc = iwl_legacy_read_targ_mem(priv, base + i);
 		time =
-		    iwl_read_targ_mem(priv, base + i + 1 * sizeof(u32));
+		    iwl_legacy_read_targ_mem(priv, base + i + 1 * sizeof(u32));
 		blink1 =
-		    iwl_read_targ_mem(priv, base + i + 2 * sizeof(u32));
+		    iwl_legacy_read_targ_mem(priv, base + i + 2 * sizeof(u32));
 		blink2 =
-		    iwl_read_targ_mem(priv, base + i + 3 * sizeof(u32));
+		    iwl_legacy_read_targ_mem(priv, base + i + 3 * sizeof(u32));
 		ilink1 =
-		    iwl_read_targ_mem(priv, base + i + 4 * sizeof(u32));
+		    iwl_legacy_read_targ_mem(priv, base + i + 4 * sizeof(u32));
 		ilink2 =
-		    iwl_read_targ_mem(priv, base + i + 5 * sizeof(u32));
+		    iwl_legacy_read_targ_mem(priv, base + i + 5 * sizeof(u32));
 		data1 =
-		    iwl_read_targ_mem(priv, base + i + 6 * sizeof(u32));
+		    iwl_legacy_read_targ_mem(priv, base + i + 6 * sizeof(u32));
 
 		IWL_ERR(priv,
 			"%-13s (0x%X) %010u 0x%05X 0x%05X 0x%05X 0x%05X %u\n\n",
-			desc_lookup(desc), desc, time, blink1, blink2,
+			iwl3945_desc_lookup(desc), desc, time, blink1, blink2,
 			ilink1, ilink2, data1);
-		trace_iwlwifi_dev_ucode_error(priv, desc, time, data1, 0,
+		trace_iwlwifi_legacy_dev_ucode_error(priv, desc, time, data1, 0,
 					0, blink1, blink2, ilink1, ilink2);
 	}
 }
@@ -1471,14 +1442,14 @@ static int iwl3945_print_event_log(struct iwl_priv *priv, u32 start_idx,
 	iwl_grab_nic_access(priv);
 
 	/* Set starting address; reads will auto-increment */
-	_iwl_write_direct32(priv, HBUS_TARG_MEM_RADDR, ptr);
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR, ptr);
 	rmb();
 
 	/* "time" is actually "data" for mode 0 (no timestamp).
 	 * place event id # at far right for easier visual parsing. */
 	for (i = 0; i < num_events; i++) {
-		ev = _iwl_read_direct32(priv, HBUS_TARG_MEM_RDAT);
-		time = _iwl_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		ev = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		time = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
 		if (mode == 0) {
 			/* data, ev */
 			if (bufsz) {
@@ -1487,11 +1458,12 @@ static int iwl3945_print_event_log(struct iwl_priv *priv, u32 start_idx,
 						time, ev);
 			} else {
 				IWL_ERR(priv, "0x%08x\t%04u\n", time, ev);
-				trace_iwlwifi_dev_ucode_event(priv, 0,
+				trace_iwlwifi_legacy_dev_ucode_event(priv, 0,
 							      time, ev);
 			}
 		} else {
-			data = _iwl_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+			data = _iwl_legacy_read_direct32(priv,
+							HBUS_TARG_MEM_RDAT);
 			if (bufsz) {
 				pos += scnprintf(*buf + pos, bufsz - pos,
 						"%010u:0x%08x:%04u\n",
@@ -1499,7 +1471,7 @@ static int iwl3945_print_event_log(struct iwl_priv *priv, u32 start_idx,
 			} else {
 				IWL_ERR(priv, "%010u\t0x%08x\t%04u\n",
 					time, data, ev);
-				trace_iwlwifi_dev_ucode_event(priv, time,
+				trace_iwlwifi_legacy_dev_ucode_event(priv, time,
 							      data, ev);
 			}
 		}
@@ -1570,10 +1542,10 @@ int iwl3945_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
 	}
 
 	/* event log header */
-	capacity = iwl_read_targ_mem(priv, base);
-	mode = iwl_read_targ_mem(priv, base + (1 * sizeof(u32)));
-	num_wraps = iwl_read_targ_mem(priv, base + (2 * sizeof(u32)));
-	next_entry = iwl_read_targ_mem(priv, base + (3 * sizeof(u32)));
+	capacity = iwl_legacy_read_targ_mem(priv, base);
+	mode = iwl_legacy_read_targ_mem(priv, base + (1 * sizeof(u32)));
+	num_wraps = iwl_legacy_read_targ_mem(priv, base + (2 * sizeof(u32)));
+	next_entry = iwl_legacy_read_targ_mem(priv, base + (3 * sizeof(u32)));
 
 	if (capacity > priv->cfg->base_params->max_event_log_size) {
 		IWL_ERR(priv, "Log capacity %d is bogus, limit to %d entries\n",
@@ -1595,8 +1567,8 @@ int iwl3945_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
 		return pos;
 	}
 
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (!(iwl_get_debug_level(priv) & IWL_DL_FW_ERRORS) && !full_log)
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (!(iwl_legacy_get_debug_level(priv) & IWL_DL_FW_ERRORS) && !full_log)
 		size = (size > DEFAULT_IWL3945_DUMP_EVENT_LOG_ENTRIES)
 			? DEFAULT_IWL3945_DUMP_EVENT_LOG_ENTRIES : size;
 #else
@@ -1607,7 +1579,7 @@ int iwl3945_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
 	IWL_ERR(priv, "Start IWL Event Log Dump: display last %d count\n",
 		  size);
 
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 	if (display) {
 		if (full_log)
 			bufsz = capacity * 48;
@@ -1617,7 +1589,7 @@ int iwl3945_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
 		if (!*buf)
 			return -ENOMEM;
 	}
-	if ((iwl_get_debug_level(priv) & IWL_DL_FW_ERRORS) || full_log) {
+	if ((iwl_legacy_get_debug_level(priv) & IWL_DL_FW_ERRORS) || full_log) {
 		/* if uCode has wrapped back to top of log,
 		 * start at the oldest entry,
 		 * i.e the next one that uCode would fill.
@@ -1647,7 +1619,7 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 	u32 inta, handled = 0;
 	u32 inta_fh;
 	unsigned long flags;
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 	u32 inta_mask;
 #endif
 
@@ -1665,8 +1637,8 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
 	iwl_write32(priv, CSR_FH_INT_STATUS, inta_fh);
 
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_get_debug_level(priv) & IWL_DL_ISR) {
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & IWL_DL_ISR) {
 		/* just for debug */
 		inta_mask = iwl_read32(priv, CSR_INT_MASK);
 		IWL_DEBUG_ISR(priv, "inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
@@ -1690,18 +1662,18 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 		IWL_ERR(priv, "Hardware error detected.  Restarting.\n");
 
 		/* Tell the device to stop sending interrupts */
-		iwl_disable_interrupts(priv);
+		iwl_legacy_disable_interrupts(priv);
 
 		priv->isr_stats.hw++;
-		iwl_irq_handle_error(priv);
+		iwl_legacy_irq_handle_error(priv);
 
 		handled |= CSR_INT_BIT_HW_ERR;
 
 		return;
 	}
 
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_get_debug_level(priv) & (IWL_DL_ISR)) {
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & (IWL_DL_ISR)) {
 		/* NIC fires this, but we don't use it, redundant with WAKEUP */
 		if (inta & CSR_INT_BIT_SCD) {
 			IWL_DEBUG_ISR(priv, "Scheduler finished to transmit "
@@ -1724,20 +1696,20 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 		IWL_ERR(priv, "Microcode SW error detected. "
 			"Restarting 0x%X.\n", inta);
 		priv->isr_stats.sw++;
-		iwl_irq_handle_error(priv);
+		iwl_legacy_irq_handle_error(priv);
 		handled |= CSR_INT_BIT_SW_ERR;
 	}
 
 	/* uCode wakes up after power-down sleep */
 	if (inta & CSR_INT_BIT_WAKEUP) {
 		IWL_DEBUG_ISR(priv, "Wakeup interrupt\n");
-		iwl_rx_queue_update_write_ptr(priv, &priv->rxq);
-		iwl_txq_update_write_ptr(priv, &priv->txq[0]);
-		iwl_txq_update_write_ptr(priv, &priv->txq[1]);
-		iwl_txq_update_write_ptr(priv, &priv->txq[2]);
-		iwl_txq_update_write_ptr(priv, &priv->txq[3]);
-		iwl_txq_update_write_ptr(priv, &priv->txq[4]);
-		iwl_txq_update_write_ptr(priv, &priv->txq[5]);
+		iwl_legacy_rx_queue_update_write_ptr(priv, &priv->rxq);
+		iwl_legacy_txq_update_write_ptr(priv, &priv->txq[0]);
+		iwl_legacy_txq_update_write_ptr(priv, &priv->txq[1]);
+		iwl_legacy_txq_update_write_ptr(priv, &priv->txq[2]);
+		iwl_legacy_txq_update_write_ptr(priv, &priv->txq[3]);
+		iwl_legacy_txq_update_write_ptr(priv, &priv->txq[4]);
+		iwl_legacy_txq_update_write_ptr(priv, &priv->txq[5]);
 
 		priv->isr_stats.wakeup++;
 		handled |= CSR_INT_BIT_WAKEUP;
@@ -1757,7 +1729,7 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 		priv->isr_stats.tx++;
 
 		iwl_write32(priv, CSR_FH_INT_STATUS, (1 << 6));
-		iwl_write_direct32(priv, FH39_TCSR_CREDIT
+		iwl_legacy_write_direct32(priv, FH39_TCSR_CREDIT
 					(FH39_SRVC_CHNL), 0x0);
 		handled |= CSR_INT_BIT_FH_TX;
 	}
@@ -1776,10 +1748,10 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 	/* Re-enable all interrupts */
 	/* only Re-enable if disabled by irq */
 	if (test_bit(STATUS_INT_ENABLED, &priv->status))
-		iwl_enable_interrupts(priv);
+		iwl_legacy_enable_interrupts(priv);
 
-#ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_get_debug_level(priv) & (IWL_DL_ISR)) {
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & (IWL_DL_ISR)) {
 		inta = iwl_read32(priv, CSR_INT);
 		inta_mask = iwl_read32(priv, CSR_INT_MASK);
 		inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
@@ -1806,14 +1778,14 @@ static int iwl3945_get_single_channel_for_scan(struct iwl_priv *priv,
 		return added;
 	}
 
-	active_dwell = iwl_get_active_dwell_time(priv, band, 0);
-	passive_dwell = iwl_get_passive_dwell_time(priv, band, vif);
+	active_dwell = iwl_legacy_get_active_dwell_time(priv, band, 0);
+	passive_dwell = iwl_legacy_get_passive_dwell_time(priv, band, vif);
 
 	if (passive_dwell <= active_dwell)
 		passive_dwell = active_dwell + 1;
 
 
-	channel = iwl_get_single_channel_number(priv, band);
+	channel = iwl_legacy_get_single_channel_number(priv, band);
 
 	if (channel) {
 		scan_ch->channel = channel;
@@ -1849,8 +1821,8 @@ static int iwl3945_get_channels_for_scan(struct iwl_priv *priv,
 	if (!sband)
 		return 0;
 
-	active_dwell = iwl_get_active_dwell_time(priv, band, n_probes);
-	passive_dwell = iwl_get_passive_dwell_time(priv, band, vif);
+	active_dwell = iwl_legacy_get_active_dwell_time(priv, band, n_probes);
+	passive_dwell = iwl_legacy_get_passive_dwell_time(priv, band, vif);
 
 	if (passive_dwell <= active_dwell)
 		passive_dwell = active_dwell + 1;
@@ -1863,10 +1835,12 @@ static int iwl3945_get_channels_for_scan(struct iwl_priv *priv,
 
 		scan_ch->channel = chan->hw_value;
 
-		ch_info = iwl_get_channel_info(priv, band, scan_ch->channel);
-		if (!is_channel_valid(ch_info)) {
-			IWL_DEBUG_SCAN(priv, "Channel %d is INVALID for this band.\n",
-				       scan_ch->channel);
+		ch_info = iwl_legacy_get_channel_info(priv, band,
+							scan_ch->channel);
+		if (!iwl_legacy_is_channel_valid(ch_info)) {
+			IWL_DEBUG_SCAN(priv,
+				"Channel %d is INVALID for this band.\n",
+			       scan_ch->channel);
 			continue;
 		}
 
@@ -1875,7 +1849,7 @@ static int iwl3945_get_channels_for_scan(struct iwl_priv *priv,
 		/* If passive , set up for auto-switch
 		 *  and use long active_dwell time.
 		 */
-		if (!is_active || is_channel_passive(ch_info) ||
+		if (!is_active || iwl_legacy_is_channel_passive(ch_info) ||
 		    (chan->flags & IEEE80211_CHAN_PASSIVE_SCAN)) {
 			scan_ch->type = 0;	/* passive */
 			if (IWL_UCODE_API(priv->ucode_ver) == 1)
@@ -1955,12 +1929,12 @@ static void iwl3945_init_hw_rates(struct iwl_priv *priv,
 
 static void iwl3945_dealloc_ucode_pci(struct iwl_priv *priv)
 {
-	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_code);
-	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_data);
-	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_data_backup);
-	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_init);
-	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_init_data);
-	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_boot);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_code);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_data);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_data_backup);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_init);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_init_data);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_boot);
 }
 
 /**
@@ -1976,7 +1950,7 @@ static int iwl3945_verify_inst_full(struct iwl_priv *priv, __le32 *image, u32 le
 
 	IWL_DEBUG_INFO(priv, "ucode inst image size is %u\n", len);
 
-	iwl_write_direct32(priv, HBUS_TARG_MEM_RADDR,
+	iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR,
 			       IWL39_RTC_INST_LOWER_BOUND);
 
 	errcnt = 0;
@@ -1984,7 +1958,7 @@ static int iwl3945_verify_inst_full(struct iwl_priv *priv, __le32 *image, u32 le
 		/* read data comes through single port, auto-incr addr */
 		/* NOTE: Use the debugless read so we don't flood kernel log
 		 * if IWL_DL_IO is set */
-		val = _iwl_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		val = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
 		if (val != le32_to_cpu(*image)) {
 			IWL_ERR(priv, "uCode INST section is invalid at "
 				  "offset 0x%x, is 0x%x, s/b 0x%x\n",
@@ -2023,9 +1997,9 @@ static int iwl3945_verify_inst_sparse(struct iwl_priv *priv, __le32 *image, u32
 		/* read data comes through single port, auto-incr addr */
 		/* NOTE: Use the debugless read so we don't flood kernel log
 		 * if IWL_DL_IO is set */
-		iwl_write_direct32(priv, HBUS_TARG_MEM_RADDR,
+		iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR,
 			i + IWL39_RTC_INST_LOWER_BOUND);
-		val = _iwl_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		val = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
 		if (val != le32_to_cpu(*image)) {
 #if 0 /* Enable this if you want to see details */
 			IWL_ERR(priv, "uCode INST section is invalid at "
@@ -2101,7 +2075,7 @@ static void iwl3945_nic_start(struct iwl_priv *priv)
 #define IWL3945_UCODE_GET(item)						\
 static u32 iwl3945_ucode_get_##item(const struct iwl_ucode_header *ucode)\
 {									\
-	return le32_to_cpu(ucode->u.v1.item);				\
+	return le32_to_cpu(ucode->v1.item);				\
 }
 
 static u32 iwl3945_ucode_get_header_size(u32 api_ver)
@@ -2111,7 +2085,7 @@ static u32 iwl3945_ucode_get_header_size(u32 api_ver)
 
 static u8 *iwl3945_ucode_get_data(const struct iwl_ucode_header *ucode)
 {
-	return (u8 *) ucode->u.v1.data;
+	return (u8 *) ucode->v1.data;
 }
 
 IWL3945_UCODE_GET(inst_size);
@@ -2286,13 +2260,13 @@ static int iwl3945_read_ucode(struct iwl_priv *priv)
 	 * 1) unmodified from disk
 	 * 2) backup cache for save/restore during power-downs */
 	priv->ucode_code.len = inst_size;
-	iwl_alloc_fw_desc(priv->pci_dev, &priv->ucode_code);
+	iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_code);
 
 	priv->ucode_data.len = data_size;
-	iwl_alloc_fw_desc(priv->pci_dev, &priv->ucode_data);
+	iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_data);
 
 	priv->ucode_data_backup.len = data_size;
-	iwl_alloc_fw_desc(priv->pci_dev, &priv->ucode_data_backup);
+	iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_data_backup);
 
 	if (!priv->ucode_code.v_addr || !priv->ucode_data.v_addr ||
 	    !priv->ucode_data_backup.v_addr)
@@ -2301,10 +2275,10 @@ static int iwl3945_read_ucode(struct iwl_priv *priv)
 	/* Initialization instructions and data */
 	if (init_size && init_data_size) {
 		priv->ucode_init.len = init_size;
-		iwl_alloc_fw_desc(priv->pci_dev, &priv->ucode_init);
+		iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_init);
 
 		priv->ucode_init_data.len = init_data_size;
-		iwl_alloc_fw_desc(priv->pci_dev, &priv->ucode_init_data);
+		iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_init_data);
 
 		if (!priv->ucode_init.v_addr || !priv->ucode_init_data.v_addr)
 			goto err_pci_alloc;
@@ -2313,7 +2287,7 @@ static int iwl3945_read_ucode(struct iwl_priv *priv)
 	/* Bootstrap (instructions only, no data) */
 	if (boot_size) {
 		priv->ucode_boot.len = boot_size;
-		iwl_alloc_fw_desc(priv->pci_dev, &priv->ucode_boot);
+		iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_boot);
 
 		if (!priv->ucode_boot.v_addr)
 			goto err_pci_alloc;
@@ -2400,14 +2374,14 @@ static int iwl3945_set_ucode_ptrs(struct iwl_priv *priv)
 	pdata = priv->ucode_data_backup.p_addr;
 
 	/* Tell bootstrap uCode where to find image to load */
-	iwl_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
-	iwl_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
-	iwl_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_legacy_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
 				 priv->ucode_data.len);
 
 	/* Inst byte count must be last to set up, bit 31 signals uCode
 	 *   that all new ptr/size info is in place */
-	iwl_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG,
+	iwl_legacy_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG,
 				 priv->ucode_code.len | BSM_DRAM_INST_LOAD);
 
 	IWL_DEBUG_INFO(priv, "Runtime uCode pointers are set.\n");
@@ -2488,7 +2462,7 @@ static void iwl3945_alive_start(struct iwl_priv *priv)
 		goto restart;
 	}
 
-	rfkill = iwl_read_prph(priv, APMG_RFKILL_REG);
+	rfkill = iwl_legacy_read_prph(priv, APMG_RFKILL_REG);
 	IWL_DEBUG_INFO(priv, "RFKILL status: 0x%x\n", rfkill);
 
 	if (rfkill & 0x1) {
@@ -2510,18 +2484,18 @@ static void iwl3945_alive_start(struct iwl_priv *priv)
 	set_bit(STATUS_ALIVE, &priv->status);
 
 	/* Enable watchdog to monitor the driver tx queues */
-	iwl_setup_watchdog(priv);
+	iwl_legacy_setup_watchdog(priv);
 
-	if (iwl_is_rfkill(priv))
+	if (iwl_legacy_is_rfkill(priv))
 		return;
 
 	ieee80211_wake_queues(priv->hw);
 
-	priv->active_rate = IWL_RATES_MASK;
+	priv->active_rate = IWL_RATES_MASK_3945;
 
-	iwl_power_update_mode(priv, true);
+	iwl_legacy_power_update_mode(priv, true);
 
-	if (iwl_is_associated(priv, IWL_RXON_CTX_BSS)) {
+	if (iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS)) {
 		struct iwl3945_rxon_cmd *active_rxon =
 				(struct iwl3945_rxon_cmd *)(&ctx->active);
 
@@ -2529,21 +2503,20 @@ static void iwl3945_alive_start(struct iwl_priv *priv)
 		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 	} else {
 		/* Initialize our rx_config data */
-		iwl_connection_init_rx_config(priv, ctx);
+		iwl_legacy_connection_init_rx_config(priv, ctx);
 	}
 
 	/* Configure Bluetooth device coexistence support */
-	priv->cfg->ops->hcmd->send_bt_config(priv);
+	iwl_legacy_send_bt_config(priv);
+
+	set_bit(STATUS_READY, &priv->status);
 
 	/* Configure the adapter for unassociated operation */
 	iwl3945_commit_rxon(priv, ctx);
 
 	iwl3945_reg_txpower_periodic(priv);
 
-	iwl_leds_init(priv);
-
 	IWL_DEBUG_INFO(priv, "ALIVE processing complete.\n");
-	set_bit(STATUS_READY, &priv->status);
 	wake_up_interruptible(&priv->wait_command_queue);
 
 	return;
@@ -2561,7 +2534,7 @@ static void __iwl3945_down(struct iwl_priv *priv)
 
 	IWL_DEBUG_INFO(priv, DRV_NAME " is going down\n");
 
-	iwl_scan_cancel_timeout(priv, 200);
+	iwl_legacy_scan_cancel_timeout(priv, 200);
 
 	exit_pending = test_and_set_bit(STATUS_EXIT_PENDING, &priv->status);
 
@@ -2570,9 +2543,9 @@ static void __iwl3945_down(struct iwl_priv *priv)
 	del_timer_sync(&priv->watchdog);
 
 	/* Station information will now be cleared in device */
-	iwl_clear_ucode_stations(priv, NULL);
-	iwl_dealloc_bcast_stations(priv);
-	iwl_clear_driver_stations(priv);
+	iwl_legacy_clear_ucode_stations(priv, NULL);
+	iwl_legacy_dealloc_bcast_stations(priv);
+	iwl_legacy_clear_driver_stations(priv);
 
 	/* Unblock any waiting calls */
 	wake_up_interruptible_all(&priv->wait_command_queue);
@@ -2587,16 +2560,16 @@ static void __iwl3945_down(struct iwl_priv *priv)
 
 	/* tell the device to stop sending interrupts */
 	spin_lock_irqsave(&priv->lock, flags);
-	iwl_disable_interrupts(priv);
+	iwl_legacy_disable_interrupts(priv);
 	spin_unlock_irqrestore(&priv->lock, flags);
-	iwl_synchronize_irq(priv);
+	iwl3945_synchronize_irq(priv);
 
 	if (priv->mac80211_registered)
 		ieee80211_stop_queues(priv->hw);
 
 	/* If we have not previously called iwl3945_init() then
 	 * clear all bits but the RF Kill bits and return */
-	if (!iwl_is_init(priv)) {
+	if (!iwl_legacy_is_init(priv)) {
 		priv->status = test_bit(STATUS_RF_KILL_HW, &priv->status) <<
 					STATUS_RF_KILL_HW |
 			       test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
@@ -2621,11 +2594,11 @@ static void __iwl3945_down(struct iwl_priv *priv)
 	iwl3945_hw_rxq_stop(priv);
 
 	/* Power-down device's busmaster DMA clocks */
-	iwl_write_prph(priv, APMG_CLK_DIS_REG, APMG_CLK_VAL_DMA_CLK_RQT);
+	iwl_legacy_write_prph(priv, APMG_CLK_DIS_REG, APMG_CLK_VAL_DMA_CLK_RQT);
 	udelay(5);
 
 	/* Stop the device, and put it in low power state */
-	iwl_apm_stop(priv);
+	iwl_legacy_apm_stop(priv);
 
  exit:
 	memset(&priv->card_alive, 0, sizeof(struct iwl_alive_resp));
@@ -2656,7 +2629,8 @@ static int iwl3945_alloc_bcast_station(struct iwl_priv *priv)
 	u8 sta_id;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
-	sta_id = iwl_prep_station(priv, ctx, iwl_bcast_addr, false, NULL);
+	sta_id = iwl_legacy_prep_station(priv, ctx,
+					iwlegacy_bcast_addr, false, NULL);
 	if (sta_id == IWL_INVALID_STATION) {
 		IWL_ERR(priv, "Unable to prepare broadcast station\n");
 		spin_unlock_irqrestore(&priv->sta_lock, flags);
@@ -2714,7 +2688,7 @@ static int __iwl3945_up(struct iwl_priv *priv)
 
 	/* clear (again), then enable host interrupts */
 	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
-	iwl_enable_interrupts(priv);
+	iwl_legacy_enable_interrupts(priv);
 
 	/* really make sure rfkill handshake bits are cleared */
 	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
@@ -2856,21 +2830,18 @@ int iwl3945_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
 	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
 
-	if (iwl_is_associated(priv, IWL_RXON_CTX_BSS)) {
+	if (iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS)) {
 		u16 interval = 0;
 		u32 extra;
 		u32 suspend_time = 100;
 		u32 scan_suspend_time = 100;
-		unsigned long flags;
 
 		IWL_DEBUG_INFO(priv, "Scanning while associated...\n");
 
-		spin_lock_irqsave(&priv->lock, flags);
 		if (priv->is_internal_short_scan)
 			interval = 0;
 		else
 			interval = vif->bss_conf.beacon_int;
-		spin_unlock_irqrestore(&priv->lock, flags);
 
 		scan->suspend_time = 0;
 		scan->max_out_time = cpu_to_le32(200 * 1024);
@@ -2947,7 +2918,7 @@ int iwl3945_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 	if (!priv->is_internal_short_scan) {
 		scan->tx_cmd.len = cpu_to_le16(
-			iwl_fill_probe_req(priv,
+			iwl_legacy_fill_probe_req(priv,
 				(struct ieee80211_mgmt *)scan->data,
 				vif->addr,
 				priv->scan_request->ie,
@@ -2956,9 +2927,9 @@ int iwl3945_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	} else {
 		/* use bcast addr, will not be transmitted but must be valid */
 		scan->tx_cmd.len = cpu_to_le16(
-			iwl_fill_probe_req(priv,
+			iwl_legacy_fill_probe_req(priv,
 				(struct ieee80211_mgmt *)scan->data,
-				iwl_bcast_addr, NULL, 0,
+				iwlegacy_bcast_addr, NULL, 0,
 				IWL_MAX_SCAN_SIZE - sizeof(*scan)));
 	}
 	/* select Rx antennas */
@@ -2986,7 +2957,7 @@ int iwl3945_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	scan->len = cpu_to_le16(cmd.len);
 
 	set_bit(STATUS_SCAN_HW, &priv->status);
-	ret = iwl_send_cmd_sync(priv, &cmd);
+	ret = iwl_legacy_send_cmd_sync(priv, &cmd);
 	if (ret)
 		clear_bit(STATUS_SCAN_HW, &priv->status);
 	return ret;
@@ -3054,25 +3025,20 @@ void iwl3945_post_associate(struct iwl_priv *priv)
 	if (!ctx->vif || !priv->is_open)
 		return;
 
-	if (ctx->vif->type == NL80211_IFTYPE_AP) {
-		IWL_ERR(priv, "%s Should not be called in AP mode\n", __func__);
-		return;
-	}
-
 	IWL_DEBUG_ASSOC(priv, "Associated as %d to: %pM\n",
 			ctx->vif->bss_conf.aid, ctx->active.bssid_addr);
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
 
-	iwl_scan_cancel_timeout(priv, 200);
+	iwl_legacy_scan_cancel_timeout(priv, 200);
 
-	conf = ieee80211_get_hw_conf(priv->hw);
+	conf = iwl_legacy_ieee80211_get_hw_conf(priv->hw);
 
 	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 	iwl3945_commit_rxon(priv, ctx);
 
-	rc = iwl_send_rxon_timing(priv, ctx);
+	rc = iwl_legacy_send_rxon_timing(priv, ctx);
 	if (rc)
 		IWL_WARN(priv, "REPLY_RXON_TIMING failed - "
 			    "Attempting to continue.\n");
@@ -3170,8 +3136,6 @@ static int iwl3945_mac_start(struct ieee80211_hw *hw)
 	 * no need to poll the killswitch state anymore */
 	cancel_delayed_work(&priv->_3945.rfkill_poll);
 
-	iwl_led_start(priv);
-
 	priv->is_open = 1;
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 	return 0;
@@ -3206,7 +3170,7 @@ static void iwl3945_mac_stop(struct ieee80211_hw *hw)
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl_priv *priv = hw->priv;
 
@@ -3219,7 +3183,6 @@ static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		dev_kfree_skb_any(skb);
 
 	IWL_DEBUG_MAC80211(priv, "leave\n");
-	return NETDEV_TX_OK;
 }
 
 void iwl3945_config_ap(struct iwl_priv *priv)
@@ -3232,14 +3195,14 @@ void iwl3945_config_ap(struct iwl_priv *priv)
 		return;
 
 	/* The following should be done only at AP bring up */
-	if (!(iwl_is_associated(priv, IWL_RXON_CTX_BSS))) {
+	if (!(iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS))) {
 
 		/* RXON - unassoc (to set timing command) */
 		ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 		iwl3945_commit_rxon(priv, ctx);
 
 		/* RXON Timing */
-		rc = iwl_send_rxon_timing(priv, ctx);
+		rc = iwl_legacy_send_rxon_timing(priv, ctx);
 		if (rc)
 			IWL_WARN(priv, "REPLY_RXON_TIMING failed - "
 					"Attempting to continue.\n");
@@ -3266,10 +3229,6 @@ void iwl3945_config_ap(struct iwl_priv *priv)
 		iwl3945_commit_rxon(priv, ctx);
 	}
 	iwl3945_send_beacon_cmd(priv);
-
-	/* FIXME - we need to add code here to detect a totally new
-	 * configuration, reset the AP, unassoc, rxon timing, assoc,
-	 * clear sta table, add BCAST sta... */
 }
 
 static int iwl3945_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
@@ -3289,17 +3248,25 @@ static int iwl3945_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		return -EOPNOTSUPP;
 	}
 
-	static_key = !iwl_is_associated(priv, IWL_RXON_CTX_BSS);
+	/*
+	 * To support IBSS RSN, don't program group keys in IBSS, the
+	 * hardware will then not attempt to decrypt the frames.
+	 */
+	if (vif->type == NL80211_IFTYPE_ADHOC &&
+	    !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+		return -EOPNOTSUPP;
+
+	static_key = !iwl_legacy_is_associated(priv, IWL_RXON_CTX_BSS);
 
 	if (!static_key) {
-		sta_id = iwl_sta_id_or_broadcast(
+		sta_id = iwl_legacy_sta_id_or_broadcast(
 				priv, &priv->contexts[IWL_RXON_CTX_BSS], sta);
 		if (sta_id == IWL_INVALID_STATION)
 			return -EINVAL;
 	}
 
 	mutex_lock(&priv->mutex);
-	iwl_scan_cancel_timeout(priv, 100);
+	iwl_legacy_scan_cancel_timeout(priv, 100);
 
 	switch (cmd) {
 	case SET_KEY:
@@ -3344,7 +3311,8 @@ static int iwl3945_mac_sta_add(struct ieee80211_hw *hw,
 	sta_priv->common.sta_id = IWL_INVALID_STATION;
 
 
-	ret = iwl_add_station_common(priv, &priv->contexts[IWL_RXON_CTX_BSS],
+	ret = iwl_legacy_add_station_common(priv,
+				&priv->contexts[IWL_RXON_CTX_BSS],
 				     sta->addr, is_ap, sta, &sta_id);
 	if (ret) {
 		IWL_ERR(priv, "Unable to add station %pM (%d)\n",
@@ -3405,7 +3373,7 @@ static void iwl3945_configure_filter(struct ieee80211_hw *hw,
 
 	/*
 	 * Receiving all multicast frames is always enabled by the
-	 * default flags setup in iwl_connection_init_rx_config()
+	 * default flags setup in iwl_legacy_connection_init_rx_config()
 	 * since we currently do not support programming multicast
 	 * filters into the device.
 	 */
@@ -3420,7 +3388,7 @@ static void iwl3945_configure_filter(struct ieee80211_hw *hw,
  *
  *****************************************************************************/
 
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 
 /*
  * The following adds a new attribute to the sysfs representation
@@ -3433,13 +3401,13 @@ static void iwl3945_configure_filter(struct ieee80211_hw *hw,
  * level that is used instead of the global debug level if it (the per
  * device debug level) is set.
  */
-static ssize_t show_debug_level(struct device *d,
+static ssize_t iwl3945_show_debug_level(struct device *d,
 				struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
-	return sprintf(buf, "0x%08X\n", iwl_get_debug_level(priv));
+	return sprintf(buf, "0x%08X\n", iwl_legacy_get_debug_level(priv));
 }
-static ssize_t store_debug_level(struct device *d,
+static ssize_t iwl3945_store_debug_level(struct device *d,
 				struct device_attribute *attr,
 				 const char *buf, size_t count)
 {
@@ -3452,7 +3420,7 @@ static ssize_t store_debug_level(struct device *d,
 		IWL_INFO(priv, "%s is not in hex or decimal form.\n", buf);
 	else {
 		priv->debug_level = val;
-		if (iwl_alloc_traffic_mem(priv))
+		if (iwl_legacy_alloc_traffic_mem(priv))
 			IWL_ERR(priv,
 				"Not enough memory to generate traffic log\n");
 	}
@@ -3460,31 +3428,31 @@ static ssize_t store_debug_level(struct device *d,
 }
 
 static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
-			show_debug_level, store_debug_level);
+			iwl3945_show_debug_level, iwl3945_store_debug_level);
 
-#endif /* CONFIG_IWLWIFI_DEBUG */
+#endif /* CONFIG_IWLWIFI_LEGACY_DEBUG */
 
-static ssize_t show_temperature(struct device *d,
+static ssize_t iwl3945_show_temperature(struct device *d,
 				struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EAGAIN;
 
 	return sprintf(buf, "%d\n", iwl3945_hw_get_temperature(priv));
 }
 
-static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
+static DEVICE_ATTR(temperature, S_IRUGO, iwl3945_show_temperature, NULL);
 
-static ssize_t show_tx_power(struct device *d,
+static ssize_t iwl3945_show_tx_power(struct device *d,
 			     struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
 	return sprintf(buf, "%d\n", priv->tx_power_user_lmt);
 }
 
-static ssize_t store_tx_power(struct device *d,
+static ssize_t iwl3945_store_tx_power(struct device *d,
 			      struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
@@ -3501,9 +3469,9 @@ static ssize_t store_tx_power(struct device *d,
 	return count;
 }
 
-static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO, show_tx_power, store_tx_power);
+static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO, iwl3945_show_tx_power, iwl3945_store_tx_power);
 
-static ssize_t show_flags(struct device *d,
+static ssize_t iwl3945_show_flags(struct device *d,
 			  struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
@@ -3512,7 +3480,7 @@ static ssize_t show_flags(struct device *d,
 	return sprintf(buf, "0x%04X\n", ctx->active.flags);
 }
 
-static ssize_t store_flags(struct device *d,
+static ssize_t iwl3945_store_flags(struct device *d,
 			   struct device_attribute *attr,
 			   const char *buf, size_t count)
 {
@@ -3523,7 +3491,7 @@ static ssize_t store_flags(struct device *d,
 	mutex_lock(&priv->mutex);
 	if (le32_to_cpu(ctx->staging.flags) != flags) {
 		/* Cancel any currently running scans... */
-		if (iwl_scan_cancel_timeout(priv, 100))
+		if (iwl_legacy_scan_cancel_timeout(priv, 100))
 			IWL_WARN(priv, "Could not cancel scan.\n");
 		else {
 			IWL_DEBUG_INFO(priv, "Committing rxon.flags = 0x%04X\n",
@@ -3537,9 +3505,9 @@ static ssize_t store_flags(struct device *d,
 	return count;
 }
 
-static DEVICE_ATTR(flags, S_IWUSR | S_IRUGO, show_flags, store_flags);
+static DEVICE_ATTR(flags, S_IWUSR | S_IRUGO, iwl3945_show_flags, iwl3945_store_flags);
 
-static ssize_t show_filter_flags(struct device *d,
+static ssize_t iwl3945_show_filter_flags(struct device *d,
 				 struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
@@ -3549,7 +3517,7 @@ static ssize_t show_filter_flags(struct device *d,
 		le32_to_cpu(ctx->active.filter_flags));
 }
 
-static ssize_t store_filter_flags(struct device *d,
+static ssize_t iwl3945_store_filter_flags(struct device *d,
 				  struct device_attribute *attr,
 				  const char *buf, size_t count)
 {
@@ -3560,7 +3528,7 @@ static ssize_t store_filter_flags(struct device *d,
 	mutex_lock(&priv->mutex);
 	if (le32_to_cpu(ctx->staging.filter_flags) != filter_flags) {
 		/* Cancel any currently running scans... */
-		if (iwl_scan_cancel_timeout(priv, 100))
+		if (iwl_legacy_scan_cancel_timeout(priv, 100))
 			IWL_WARN(priv, "Could not cancel scan.\n");
 		else {
 			IWL_DEBUG_INFO(priv, "Committing rxon.filter_flags = "
@@ -3575,10 +3543,10 @@ static ssize_t store_filter_flags(struct device *d,
 	return count;
 }
 
-static DEVICE_ATTR(filter_flags, S_IWUSR | S_IRUGO, show_filter_flags,
-		   store_filter_flags);
+static DEVICE_ATTR(filter_flags, S_IWUSR | S_IRUGO, iwl3945_show_filter_flags,
+		   iwl3945_store_filter_flags);
 
-static ssize_t show_measurement(struct device *d,
+static ssize_t iwl3945_show_measurement(struct device *d,
 				struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
@@ -3610,7 +3578,7 @@ static ssize_t show_measurement(struct device *d,
 	return len;
 }
 
-static ssize_t store_measurement(struct device *d,
+static ssize_t iwl3945_store_measurement(struct device *d,
 				 struct device_attribute *attr,
 				 const char *buf, size_t count)
 {
@@ -3647,9 +3615,9 @@ static ssize_t store_measurement(struct device *d,
 }
 
 static DEVICE_ATTR(measurement, S_IRUSR | S_IWUSR,
-		   show_measurement, store_measurement);
+		   iwl3945_show_measurement, iwl3945_store_measurement);
 
-static ssize_t store_retry_rate(struct device *d,
+static ssize_t iwl3945_store_retry_rate(struct device *d,
 				struct device_attribute *attr,
 				const char *buf, size_t count)
 {
@@ -3662,38 +3630,38 @@ static ssize_t store_retry_rate(struct device *d,
 	return count;
 }
 
-static ssize_t show_retry_rate(struct device *d,
+static ssize_t iwl3945_show_retry_rate(struct device *d,
 			       struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
 	return sprintf(buf, "%d", priv->retry_rate);
 }
 
-static DEVICE_ATTR(retry_rate, S_IWUSR | S_IRUSR, show_retry_rate,
-		   store_retry_rate);
+static DEVICE_ATTR(retry_rate, S_IWUSR | S_IRUSR, iwl3945_show_retry_rate,
+		   iwl3945_store_retry_rate);
 
 
-static ssize_t show_channels(struct device *d,
+static ssize_t iwl3945_show_channels(struct device *d,
 			     struct device_attribute *attr, char *buf)
 {
 	/* all this shit doesn't belong into sysfs anyway */
 	return 0;
 }
 
-static DEVICE_ATTR(channels, S_IRUSR, show_channels, NULL);
+static DEVICE_ATTR(channels, S_IRUSR, iwl3945_show_channels, NULL);
 
-static ssize_t show_antenna(struct device *d,
+static ssize_t iwl3945_show_antenna(struct device *d,
 			    struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
 
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EAGAIN;
 
 	return sprintf(buf, "%d\n", iwl3945_mod_params.antenna);
 }
 
-static ssize_t store_antenna(struct device *d,
+static ssize_t iwl3945_store_antenna(struct device *d,
 			     struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
@@ -3718,20 +3686,20 @@ static ssize_t store_antenna(struct device *d,
 	return count;
 }
 
-static DEVICE_ATTR(antenna, S_IWUSR | S_IRUGO, show_antenna, store_antenna);
+static DEVICE_ATTR(antenna, S_IWUSR | S_IRUGO, iwl3945_show_antenna, iwl3945_store_antenna);
 
-static ssize_t show_status(struct device *d,
+static ssize_t iwl3945_show_status(struct device *d,
 			   struct device_attribute *attr, char *buf)
 {
 	struct iwl_priv *priv = dev_get_drvdata(d);
-	if (!iwl_is_alive(priv))
+	if (!iwl_legacy_is_alive(priv))
 		return -EAGAIN;
 	return sprintf(buf, "0x%08x\n", (int)priv->status);
 }
 
-static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
+static DEVICE_ATTR(status, S_IRUGO, iwl3945_show_status, NULL);
 
-static ssize_t dump_error_log(struct device *d,
+static ssize_t iwl3945_dump_error_log(struct device *d,
 			      struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
@@ -3744,7 +3712,7 @@ static ssize_t dump_error_log(struct device *d,
 	return strnlen(buf, count);
 }
 
-static DEVICE_ATTR(dump_errors, S_IWUSR, NULL, dump_error_log);
+static DEVICE_ATTR(dump_errors, S_IWUSR, NULL, iwl3945_dump_error_log);
 
 /*****************************************************************************
  *
@@ -3760,18 +3728,17 @@ static void iwl3945_setup_deferred_work(struct iwl_priv *priv)
 
 	INIT_WORK(&priv->restart, iwl3945_bg_restart);
 	INIT_WORK(&priv->rx_replenish, iwl3945_bg_rx_replenish);
-	INIT_WORK(&priv->beacon_update, iwl3945_bg_beacon_update);
 	INIT_DELAYED_WORK(&priv->init_alive_start, iwl3945_bg_init_alive_start);
 	INIT_DELAYED_WORK(&priv->alive_start, iwl3945_bg_alive_start);
 	INIT_DELAYED_WORK(&priv->_3945.rfkill_poll, iwl3945_rfkill_poll);
 
-	iwl_setup_scan_deferred_work(priv);
+	iwl_legacy_setup_scan_deferred_work(priv);
 
 	iwl3945_hw_setup_deferred_work(priv);
 
 	init_timer(&priv->watchdog);
 	priv->watchdog.data = (unsigned long)priv;
-	priv->watchdog.function = iwl_bg_watchdog;
+	priv->watchdog.function = iwl_legacy_bg_watchdog;
 
 	tasklet_init(&priv->irq_tasklet, (void (*)(unsigned long))
 		     iwl3945_irq_tasklet, (unsigned long)priv);
@@ -3783,9 +3750,8 @@ static void iwl3945_cancel_deferred_work(struct iwl_priv *priv)
 
 	cancel_delayed_work_sync(&priv->init_alive_start);
 	cancel_delayed_work(&priv->alive_start);
-	cancel_work_sync(&priv->beacon_update);
 
-	iwl_cancel_scan_deferred_work(priv);
+	iwl_legacy_cancel_scan_deferred_work(priv);
 }
 
 static struct attribute *iwl3945_sysfs_entries[] = {
@@ -3799,7 +3765,7 @@ static struct attribute *iwl3945_sysfs_entries[] = {
 	&dev_attr_status.attr,
 	&dev_attr_temperature.attr,
 	&dev_attr_tx_power.attr,
-#ifdef CONFIG_IWLWIFI_DEBUG
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
 	&dev_attr_debug_level.attr,
 #endif
 	NULL
@@ -3814,19 +3780,19 @@ struct ieee80211_ops iwl3945_hw_ops = {
 	.tx = iwl3945_mac_tx,
 	.start = iwl3945_mac_start,
 	.stop = iwl3945_mac_stop,
-	.add_interface = iwl_mac_add_interface,
-	.remove_interface = iwl_mac_remove_interface,
-	.change_interface = iwl_mac_change_interface,
+	.add_interface = iwl_legacy_mac_add_interface,
+	.remove_interface = iwl_legacy_mac_remove_interface,
+	.change_interface = iwl_legacy_mac_change_interface,
 	.config = iwl_legacy_mac_config,
 	.configure_filter = iwl3945_configure_filter,
 	.set_key = iwl3945_mac_set_key,
-	.conf_tx = iwl_mac_conf_tx,
+	.conf_tx = iwl_legacy_mac_conf_tx,
 	.reset_tsf = iwl_legacy_mac_reset_tsf,
 	.bss_info_changed = iwl_legacy_mac_bss_info_changed,
-	.hw_scan = iwl_mac_hw_scan,
+	.hw_scan = iwl_legacy_mac_hw_scan,
 	.sta_add = iwl3945_mac_sta_add,
-	.sta_remove = iwl_mac_sta_remove,
-	.tx_last_beacon = iwl_mac_tx_last_beacon,
+	.sta_remove = iwl_legacy_mac_sta_remove,
+	.tx_last_beacon = iwl_legacy_mac_tx_last_beacon,
 };
 
 static int iwl3945_init_drv(struct iwl_priv *priv)
@@ -3868,7 +3834,7 @@ static int iwl3945_init_drv(struct iwl_priv *priv)
 		ret = -EINVAL;
 		goto err;
 	}
-	ret = iwl_init_channel_map(priv);
+	ret = iwl_legacy_init_channel_map(priv);
 	if (ret) {
 		IWL_ERR(priv, "initializing regulatory failed: %d\n", ret);
 		goto err;
@@ -3880,7 +3846,7 @@ static int iwl3945_init_drv(struct iwl_priv *priv)
 		goto err_free_channel_map;
 	}
 
-	ret = iwlcore_init_geos(priv);
+	ret = iwl_legacy_init_geos(priv);
 	if (ret) {
 		IWL_ERR(priv, "initializing geos failed: %d\n", ret);
 		goto err_free_channel_map;
@@ -3890,7 +3856,7 @@ static int iwl3945_init_drv(struct iwl_priv *priv)
 	return 0;
 
 err_free_channel_map:
-	iwl_free_channel_map(priv);
+	iwl_legacy_free_channel_map(priv);
 err:
 	return ret;
 }
@@ -3910,15 +3876,12 @@ static int iwl3945_setup_mac(struct iwl_priv *priv)
 	hw->flags = IEEE80211_HW_SIGNAL_DBM |
 		    IEEE80211_HW_SPECTRUM_MGMT;
 
-	if (!priv->cfg->base_params->broken_powersave)
-		hw->flags |= IEEE80211_HW_SUPPORTS_PS |
-			     IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
-
 	hw->wiphy->interface_modes =
 		priv->contexts[IWL_RXON_CTX_BSS].interface_modes;
 
 	hw->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
-			    WIPHY_FLAG_DISABLE_BEACON_HINTS;
+			    WIPHY_FLAG_DISABLE_BEACON_HINTS |
+			    WIPHY_FLAG_IBSS_RSN;
 
 	hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX_3945;
 	/* we create the 802.11 header and a zero-length SSID element */
@@ -3935,6 +3898,8 @@ static int iwl3945_setup_mac(struct iwl_priv *priv)
 		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
 			&priv->bands[IEEE80211_BAND_5GHZ];
 
+	iwl_legacy_leds_init(priv);
+
 	ret = ieee80211_register_hw(priv->hw);
 	if (ret) {
 		IWL_ERR(priv, "Failed to register hw (error %d)\n", ret);
@@ -3960,7 +3925,7 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 
 	/* mac80211 allocates memory for this device instance, including
 	 *   space for this driver's private structure */
-	hw = iwl_alloc_all(cfg);
+	hw = iwl_legacy_alloc_all(cfg);
 	if (hw == NULL) {
 		pr_err("Can not allocate network device\n");
 		err = -ENOMEM;
@@ -4000,13 +3965,12 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 		iwl3945_hw_ops.hw_scan = NULL;
 	}
 
-
 	IWL_DEBUG_INFO(priv, "*** LOAD DRIVER ***\n");
 	priv->cfg = cfg;
 	priv->pci_dev = pdev;
 	priv->inta_mask = CSR_INI_SET_MASK;
 
-	if (iwl_alloc_traffic_mem(priv))
+	if (iwl_legacy_alloc_traffic_mem(priv))
 		IWL_ERR(priv, "Not enough memory to generate traffic log\n");
 
 	/***************************
@@ -4070,7 +4034,7 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	 * ********************/
 
 	/* Read the EEPROM */
-	err = iwl_eeprom_init(priv);
+	err = iwl_legacy_eeprom_init(priv);
 	if (err) {
 		IWL_ERR(priv, "Unable to init EEPROM\n");
 		goto out_iounmap;
@@ -4107,12 +4071,12 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	 * ********************/
 
 	spin_lock_irqsave(&priv->lock, flags);
-	iwl_disable_interrupts(priv);
+	iwl_legacy_disable_interrupts(priv);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	pci_enable_msi(priv->pci_dev);
 
-	err = request_irq(priv->pci_dev->irq, priv->cfg->ops->lib->isr_ops.isr,
+	err = request_irq(priv->pci_dev->irq, iwl_legacy_isr,
 			  IRQF_SHARED, DRV_NAME, priv);
 	if (err) {
 		IWL_ERR(priv, "Error allocating IRQ %d\n", priv->pci_dev->irq);
@@ -4125,24 +4089,24 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 		goto out_release_irq;
 	}
 
-	iwl_set_rxon_channel(priv,
+	iwl_legacy_set_rxon_channel(priv,
 			     &priv->bands[IEEE80211_BAND_2GHZ].channels[5],
 			     &priv->contexts[IWL_RXON_CTX_BSS]);
 	iwl3945_setup_deferred_work(priv);
 	iwl3945_setup_rx_handlers(priv);
-	iwl_power_initialize(priv);
+	iwl_legacy_power_initialize(priv);
 
 	/*********************************
 	 * 8. Setup and Register mac80211
 	 * *******************************/
 
-	iwl_enable_interrupts(priv);
+	iwl_legacy_enable_interrupts(priv);
 
 	err = iwl3945_setup_mac(priv);
 	if (err)
 		goto  out_remove_sysfs;
 
-	err = iwl_dbgfs_register(priv, DRV_NAME);
+	err = iwl_legacy_dbgfs_register(priv, DRV_NAME);
 	if (err)
 		IWL_ERR(priv, "failed to create debugfs files. Ignoring error: %d\n", err);
 
@@ -4160,12 +4124,12 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	free_irq(priv->pci_dev->irq, priv);
  out_disable_msi:
 	pci_disable_msi(priv->pci_dev);
-	iwlcore_free_geos(priv);
-	iwl_free_channel_map(priv);
+	iwl_legacy_free_geos(priv);
+	iwl_legacy_free_channel_map(priv);
  out_unset_hw_params:
 	iwl3945_unset_hw_params(priv);
  out_eeprom_free:
-	iwl_eeprom_free(priv);
+	iwl_legacy_eeprom_free(priv);
  out_iounmap:
 	pci_iounmap(pdev, priv->hw_base);
  out_pci_release_regions:
@@ -4174,7 +4138,7 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
  out_ieee80211_free_hw:
-	iwl_free_traffic_mem(priv);
+	iwl_legacy_free_traffic_mem(priv);
 	ieee80211_free_hw(priv->hw);
  out:
 	return err;
@@ -4190,10 +4154,12 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 
 	IWL_DEBUG_INFO(priv, "*** UNLOAD DRIVER ***\n");
 
-	iwl_dbgfs_unregister(priv);
+	iwl_legacy_dbgfs_unregister(priv);
 
 	set_bit(STATUS_EXIT_PENDING, &priv->status);
 
+	iwl_legacy_leds_exit(priv);
+
 	if (priv->mac80211_registered) {
 		ieee80211_unregister_hw(priv->hw);
 		priv->mac80211_registered = 0;
@@ -4208,16 +4174,16 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 	 * paths to avoid running iwl_down() at all before leaving driver.
 	 * This (inexpensive) call *makes sure* device is reset.
 	 */
-	iwl_apm_stop(priv);
+	iwl_legacy_apm_stop(priv);
 
 	/* make sure we flush any pending irq or
 	 * tasklet for the driver
 	 */
 	spin_lock_irqsave(&priv->lock, flags);
-	iwl_disable_interrupts(priv);
+	iwl_legacy_disable_interrupts(priv);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	iwl_synchronize_irq(priv);
+	iwl3945_synchronize_irq(priv);
 
 	sysfs_remove_group(&pdev->dev.kobj, &iwl3945_attribute_group);
 
@@ -4239,7 +4205,7 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 	 * until now... */
 	destroy_workqueue(priv->workqueue);
 	priv->workqueue = NULL;
-	iwl_free_traffic_mem(priv);
+	iwl_legacy_free_traffic_mem(priv);
 
 	free_irq(pdev->irq, priv);
 	pci_disable_msi(pdev);
@@ -4249,8 +4215,8 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 	pci_set_drvdata(pdev, NULL);
 
-	iwl_free_channel_map(priv);
-	iwlcore_free_geos(priv);
+	iwl_legacy_free_channel_map(priv);
+	iwl_legacy_free_geos(priv);
 	kfree(priv->scan_cmd);
 	if (priv->beacon_skb)
 		dev_kfree_skb(priv->beacon_skb);
@@ -4270,7 +4236,7 @@ static struct pci_driver iwl3945_driver = {
 	.id_table = iwl3945_hw_card_ids,
 	.probe = iwl3945_pci_probe,
 	.remove = __devexit_p(iwl3945_pci_remove),
-	.driver.pm = IWL_PM_OPS,
+	.driver.pm = IWL_LEGACY_PM_OPS,
 };
 
 static int __init iwl3945_init(void)
@@ -4311,17 +4277,17 @@ module_param_named(antenna, iwl3945_mod_params.antenna, int, S_IRUGO);
 MODULE_PARM_DESC(antenna, "select antenna (1=Main, 2=Aux, default 0 [both])");
 module_param_named(swcrypto, iwl3945_mod_params.sw_crypto, int, S_IRUGO);
 MODULE_PARM_DESC(swcrypto,
-		 "using software crypto (default 1 [software])\n");
-#ifdef CONFIG_IWLWIFI_DEBUG
-module_param_named(debug, iwl_debug_level, uint, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "debug output mask");
-#endif
+		"using software crypto (default 1 [software])");
 module_param_named(disable_hw_scan, iwl3945_mod_params.disable_hw_scan,
-		   int, S_IRUGO);
+		int, S_IRUGO);
 MODULE_PARM_DESC(disable_hw_scan,
-		 "disable hardware scanning (default 0) (deprecated)");
-module_param_named(fw_restart3945, iwl3945_mod_params.restart_fw, int, S_IRUGO);
-MODULE_PARM_DESC(fw_restart3945, "restart firmware in case of error");
+		"disable hardware scanning (default 0) (deprecated)");
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+module_param_named(debug, iwlegacy_debug_level, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "debug output mask");
+#endif
+module_param_named(fw_restart, iwl3945_mod_params.restart_fw, int, S_IRUGO);
+MODULE_PARM_DESC(fw_restart, "restart firmware in case of error");
 
 module_exit(iwl3945_exit);
 module_init(iwl3945_init);
diff --git a/drivers/net/wireless/iwlegacy/iwl4965-base.c b/drivers/net/wireless/iwlegacy/iwl4965-base.c
new file mode 100644
index 000000000000..91b3d8b9d7a5
--- /dev/null
+++ b/drivers/net/wireless/iwlegacy/iwl4965-base.c
@@ -0,0 +1,3632 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2011 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/pci-aspm.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+
+#include <net/mac80211.h>
+
+#include <asm/div64.h>
+
+#define DRV_NAME        "iwl4965"
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+#include "iwl-sta.h"
+#include "iwl-4965-calib.h"
+#include "iwl-4965.h"
+#include "iwl-4965-led.h"
+
+
+/******************************************************************************
+ *
+ * module boiler plate
+ *
+ ******************************************************************************/
+
+/*
+ * module name, copyright, version, etc.
+ */
+#define DRV_DESCRIPTION	"Intel(R) Wireless WiFi 4965 driver for Linux"
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+#define VD "d"
+#else
+#define VD
+#endif
+
+#define DRV_VERSION     IWLWIFI_VERSION VD
+
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("iwl4965");
+
+void iwl4965_update_chain_flags(struct iwl_priv *priv)
+{
+	struct iwl_rxon_context *ctx;
+
+	if (priv->cfg->ops->hcmd->set_rxon_chain) {
+		for_each_context(priv, ctx) {
+			priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
+			if (ctx->active.rx_chain != ctx->staging.rx_chain)
+				iwl_legacy_commit_rxon(priv, ctx);
+		}
+	}
+}
+
+static void iwl4965_clear_free_frames(struct iwl_priv *priv)
+{
+	struct list_head *element;
+
+	IWL_DEBUG_INFO(priv, "%d frames on pre-allocated heap on clear.\n",
+		       priv->frames_count);
+
+	while (!list_empty(&priv->free_frames)) {
+		element = priv->free_frames.next;
+		list_del(element);
+		kfree(list_entry(element, struct iwl_frame, list));
+		priv->frames_count--;
+	}
+
+	if (priv->frames_count) {
+		IWL_WARN(priv, "%d frames still in use.  Did we lose one?\n",
+			    priv->frames_count);
+		priv->frames_count = 0;
+	}
+}
+
+static struct iwl_frame *iwl4965_get_free_frame(struct iwl_priv *priv)
+{
+	struct iwl_frame *frame;
+	struct list_head *element;
+	if (list_empty(&priv->free_frames)) {
+		frame = kzalloc(sizeof(*frame), GFP_KERNEL);
+		if (!frame) {
+			IWL_ERR(priv, "Could not allocate frame!\n");
+			return NULL;
+		}
+
+		priv->frames_count++;
+		return frame;
+	}
+
+	element = priv->free_frames.next;
+	list_del(element);
+	return list_entry(element, struct iwl_frame, list);
+}
+
+static void iwl4965_free_frame(struct iwl_priv *priv, struct iwl_frame *frame)
+{
+	memset(frame, 0, sizeof(*frame));
+	list_add(&frame->list, &priv->free_frames);
+}
+
+static u32 iwl4965_fill_beacon_frame(struct iwl_priv *priv,
+				 struct ieee80211_hdr *hdr,
+				 int left)
+{
+	lockdep_assert_held(&priv->mutex);
+
+	if (!priv->beacon_skb)
+		return 0;
+
+	if (priv->beacon_skb->len > left)
+		return 0;
+
+	memcpy(hdr, priv->beacon_skb->data, priv->beacon_skb->len);
+
+	return priv->beacon_skb->len;
+}
+
+/* Parse the beacon frame to find the TIM element and set tim_idx & tim_size */
+static void iwl4965_set_beacon_tim(struct iwl_priv *priv,
+			       struct iwl_tx_beacon_cmd *tx_beacon_cmd,
+			       u8 *beacon, u32 frame_size)
+{
+	u16 tim_idx;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)beacon;
+
+	/*
+	 * The index is relative to frame start but we start looking at the
+	 * variable-length part of the beacon.
+	 */
+	tim_idx = mgmt->u.beacon.variable - beacon;
+
+	/* Parse variable-length elements of beacon to find WLAN_EID_TIM */
+	while ((tim_idx < (frame_size - 2)) &&
+			(beacon[tim_idx] != WLAN_EID_TIM))
+		tim_idx += beacon[tim_idx+1] + 2;
+
+	/* If TIM field was found, set variables */
+	if ((tim_idx < (frame_size - 1)) && (beacon[tim_idx] == WLAN_EID_TIM)) {
+		tx_beacon_cmd->tim_idx = cpu_to_le16(tim_idx);
+		tx_beacon_cmd->tim_size = beacon[tim_idx+1];
+	} else
+		IWL_WARN(priv, "Unable to find TIM Element in beacon\n");
+}
+
+static unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
+				       struct iwl_frame *frame)
+{
+	struct iwl_tx_beacon_cmd *tx_beacon_cmd;
+	u32 frame_size;
+	u32 rate_flags;
+	u32 rate;
+	/*
+	 * We have to set up the TX command, the TX Beacon command, and the
+	 * beacon contents.
+	 */
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (!priv->beacon_ctx) {
+		IWL_ERR(priv, "trying to build beacon w/o beacon context!\n");
+		return 0;
+	}
+
+	/* Initialize memory */
+	tx_beacon_cmd = &frame->u.beacon;
+	memset(tx_beacon_cmd, 0, sizeof(*tx_beacon_cmd));
+
+	/* Set up TX beacon contents */
+	frame_size = iwl4965_fill_beacon_frame(priv, tx_beacon_cmd->frame,
+				sizeof(frame->u) - sizeof(*tx_beacon_cmd));
+	if (WARN_ON_ONCE(frame_size > MAX_MPDU_SIZE))
+		return 0;
+	if (!frame_size)
+		return 0;
+
+	/* Set up TX command fields */
+	tx_beacon_cmd->tx.len = cpu_to_le16((u16)frame_size);
+	tx_beacon_cmd->tx.sta_id = priv->beacon_ctx->bcast_sta_id;
+	tx_beacon_cmd->tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+	tx_beacon_cmd->tx.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK |
+		TX_CMD_FLG_TSF_MSK | TX_CMD_FLG_STA_RATE_MSK;
+
+	/* Set up TX beacon command fields */
+	iwl4965_set_beacon_tim(priv, tx_beacon_cmd, (u8 *)tx_beacon_cmd->frame,
+			   frame_size);
+
+	/* Set up packet rate and flags */
+	rate = iwl_legacy_get_lowest_plcp(priv, priv->beacon_ctx);
+	priv->mgmt_tx_ant = iwl4965_toggle_tx_ant(priv, priv->mgmt_tx_ant,
+					      priv->hw_params.valid_tx_ant);
+	rate_flags = iwl4965_ant_idx_to_flags(priv->mgmt_tx_ant);
+	if ((rate >= IWL_FIRST_CCK_RATE) && (rate <= IWL_LAST_CCK_RATE))
+		rate_flags |= RATE_MCS_CCK_MSK;
+	tx_beacon_cmd->tx.rate_n_flags = iwl4965_hw_set_rate_n_flags(rate,
+			rate_flags);
+
+	return sizeof(*tx_beacon_cmd) + frame_size;
+}
+
+int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
+{
+	struct iwl_frame *frame;
+	unsigned int frame_size;
+	int rc;
+
+	frame = iwl4965_get_free_frame(priv);
+	if (!frame) {
+		IWL_ERR(priv, "Could not obtain free frame buffer for beacon "
+			  "command.\n");
+		return -ENOMEM;
+	}
+
+	frame_size = iwl4965_hw_get_beacon_cmd(priv, frame);
+	if (!frame_size) {
+		IWL_ERR(priv, "Error configuring the beacon command\n");
+		iwl4965_free_frame(priv, frame);
+		return -EINVAL;
+	}
+
+	rc = iwl_legacy_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
+			      &frame->u.cmd[0]);
+
+	iwl4965_free_frame(priv, frame);
+
+	return rc;
+}
+
+static inline dma_addr_t iwl4965_tfd_tb_get_addr(struct iwl_tfd *tfd, u8 idx)
+{
+	struct iwl_tfd_tb *tb = &tfd->tbs[idx];
+
+	dma_addr_t addr = get_unaligned_le32(&tb->lo);
+	if (sizeof(dma_addr_t) > sizeof(u32))
+		addr |=
+		((dma_addr_t)(le16_to_cpu(tb->hi_n_len) & 0xF) << 16) << 16;
+
+	return addr;
+}
+
+static inline u16 iwl4965_tfd_tb_get_len(struct iwl_tfd *tfd, u8 idx)
+{
+	struct iwl_tfd_tb *tb = &tfd->tbs[idx];
+
+	return le16_to_cpu(tb->hi_n_len) >> 4;
+}
+
+static inline void iwl4965_tfd_set_tb(struct iwl_tfd *tfd, u8 idx,
+				  dma_addr_t addr, u16 len)
+{
+	struct iwl_tfd_tb *tb = &tfd->tbs[idx];
+	u16 hi_n_len = len << 4;
+
+	put_unaligned_le32(addr, &tb->lo);
+	if (sizeof(dma_addr_t) > sizeof(u32))
+		hi_n_len |= ((addr >> 16) >> 16) & 0xF;
+
+	tb->hi_n_len = cpu_to_le16(hi_n_len);
+
+	tfd->num_tbs = idx + 1;
+}
+
+static inline u8 iwl4965_tfd_get_num_tbs(struct iwl_tfd *tfd)
+{
+	return tfd->num_tbs & 0x1f;
+}
+
+/**
+ * iwl4965_hw_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
+ * @priv - driver private data
+ * @txq - tx queue
+ *
+ * Does NOT advance any TFD circular buffer read/write indexes
+ * Does NOT free the TFD itself (which is within circular buffer)
+ */
+void iwl4965_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	struct iwl_tfd *tfd_tmp = (struct iwl_tfd *)txq->tfds;
+	struct iwl_tfd *tfd;
+	struct pci_dev *dev = priv->pci_dev;
+	int index = txq->q.read_ptr;
+	int i;
+	int num_tbs;
+
+	tfd = &tfd_tmp[index];
+
+	/* Sanity check on number of chunks */
+	num_tbs = iwl4965_tfd_get_num_tbs(tfd);
+
+	if (num_tbs >= IWL_NUM_OF_TBS) {
+		IWL_ERR(priv, "Too many chunks: %i\n", num_tbs);
+		/* @todo issue fatal error, it is quite serious situation */
+		return;
+	}
+
+	/* Unmap tx_cmd */
+	if (num_tbs)
+		pci_unmap_single(dev,
+				dma_unmap_addr(&txq->meta[index], mapping),
+				dma_unmap_len(&txq->meta[index], len),
+				PCI_DMA_BIDIRECTIONAL);
+
+	/* Unmap chunks, if any. */
+	for (i = 1; i < num_tbs; i++)
+		pci_unmap_single(dev, iwl4965_tfd_tb_get_addr(tfd, i),
+				iwl4965_tfd_tb_get_len(tfd, i),
+				PCI_DMA_TODEVICE);
+
+	/* free SKB */
+	if (txq->txb) {
+		struct sk_buff *skb;
+
+		skb = txq->txb[txq->q.read_ptr].skb;
+
+		/* can be called from irqs-disabled context */
+		if (skb) {
+			dev_kfree_skb_any(skb);
+			txq->txb[txq->q.read_ptr].skb = NULL;
+		}
+	}
+}
+
+int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv,
+				 struct iwl_tx_queue *txq,
+				 dma_addr_t addr, u16 len,
+				 u8 reset, u8 pad)
+{
+	struct iwl_queue *q;
+	struct iwl_tfd *tfd, *tfd_tmp;
+	u32 num_tbs;
+
+	q = &txq->q;
+	tfd_tmp = (struct iwl_tfd *)txq->tfds;
+	tfd = &tfd_tmp[q->write_ptr];
+
+	if (reset)
+		memset(tfd, 0, sizeof(*tfd));
+
+	num_tbs = iwl4965_tfd_get_num_tbs(tfd);
+
+	/* Each TFD can point to a maximum 20 Tx buffers */
+	if (num_tbs >= IWL_NUM_OF_TBS) {
+		IWL_ERR(priv, "Error can not send more than %d chunks\n",
+			  IWL_NUM_OF_TBS);
+		return -EINVAL;
+	}
+
+	BUG_ON(addr & ~DMA_BIT_MASK(36));
+	if (unlikely(addr & ~IWL_TX_DMA_MASK))
+		IWL_ERR(priv, "Unaligned address = %llx\n",
+			  (unsigned long long)addr);
+
+	iwl4965_tfd_set_tb(tfd, num_tbs, addr, len);
+
+	return 0;
+}
+
+/*
+ * Tell nic where to find circular buffer of Tx Frame Descriptors for
+ * given Tx queue, and enable the DMA channel used for that queue.
+ *
+ * 4965 supports up to 16 Tx queues in DRAM, mapped to up to 8 Tx DMA
+ * channels supported in hardware.
+ */
+int iwl4965_hw_tx_queue_init(struct iwl_priv *priv,
+			 struct iwl_tx_queue *txq)
+{
+	int txq_id = txq->q.id;
+
+	/* Circular buffer (TFD queue in DRAM) physical base address */
+	iwl_legacy_write_direct32(priv, FH_MEM_CBBC_QUEUE(txq_id),
+			     txq->q.dma_addr >> 8);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ * Generic RX handler implementations
+ *
+ ******************************************************************************/
+static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_alive_resp *palive;
+	struct delayed_work *pwork;
+
+	palive = &pkt->u.alive_frame;
+
+	IWL_DEBUG_INFO(priv, "Alive ucode status 0x%08X revision "
+		       "0x%01X 0x%01X\n",
+		       palive->is_valid, palive->ver_type,
+		       palive->ver_subtype);
+
+	if (palive->ver_subtype == INITIALIZE_SUBTYPE) {
+		IWL_DEBUG_INFO(priv, "Initialization Alive received.\n");
+		memcpy(&priv->card_alive_init,
+		       &pkt->u.alive_frame,
+		       sizeof(struct iwl_init_alive_resp));
+		pwork = &priv->init_alive_start;
+	} else {
+		IWL_DEBUG_INFO(priv, "Runtime Alive received.\n");
+		memcpy(&priv->card_alive, &pkt->u.alive_frame,
+		       sizeof(struct iwl_alive_resp));
+		pwork = &priv->alive_start;
+	}
+
+	/* We delay the ALIVE response by 5ms to
+	 * give the HW RF Kill time to activate... */
+	if (palive->is_valid == UCODE_VALID_OK)
+		queue_delayed_work(priv->workqueue, pwork,
+				   msecs_to_jiffies(5));
+	else
+		IWL_WARN(priv, "uCode did not respond OK.\n");
+}
+
+/**
+ * iwl4965_bg_statistics_periodic - Timer callback to queue statistics
+ *
+ * This callback is provided in order to send a statistics request.
+ *
+ * This timer function is continually reset to execute within
+ * REG_RECALIB_PERIOD seconds since the last STATISTICS_NOTIFICATION
+ * was received.  We need to ensure we receive the statistics in order
+ * to update the temperature used for calibrating the TXPOWER.
+ */
+static void iwl4965_bg_statistics_periodic(unsigned long data)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)data;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	/* dont send host command if rf-kill is on */
+	if (!iwl_legacy_is_ready_rf(priv))
+		return;
+
+	iwl_legacy_send_statistics_request(priv, CMD_ASYNC, false);
+}
+
+
+static void iwl4965_print_cont_event_trace(struct iwl_priv *priv, u32 base,
+					u32 start_idx, u32 num_events,
+					u32 mode)
+{
+	u32 i;
+	u32 ptr;        /* SRAM byte address of log data */
+	u32 ev, time, data; /* event log data */
+	unsigned long reg_flags;
+
+	if (mode == 0)
+		ptr = base + (4 * sizeof(u32)) + (start_idx * 2 * sizeof(u32));
+	else
+		ptr = base + (4 * sizeof(u32)) + (start_idx * 3 * sizeof(u32));
+
+	/* Make sure device is powered up for SRAM reads */
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	if (iwl_grab_nic_access(priv)) {
+		spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+		return;
+	}
+
+	/* Set starting address; reads will auto-increment */
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR, ptr);
+	rmb();
+
+	/*
+	 * "time" is actually "data" for mode 0 (no timestamp).
+	 * place event id # at far right for easier visual parsing.
+	 */
+	for (i = 0; i < num_events; i++) {
+		ev = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		time = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		if (mode == 0) {
+			trace_iwlwifi_legacy_dev_ucode_cont_event(priv,
+							0, time, ev);
+		} else {
+			data = _iwl_legacy_read_direct32(priv,
+						HBUS_TARG_MEM_RDAT);
+			trace_iwlwifi_legacy_dev_ucode_cont_event(priv,
+						time, data, ev);
+		}
+	}
+	/* Allow device to power down */
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+}
+
+static void iwl4965_continuous_event_trace(struct iwl_priv *priv)
+{
+	u32 capacity;   /* event log capacity in # entries */
+	u32 base;       /* SRAM byte address of event log header */
+	u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
+	u32 num_wraps;  /* # times uCode wrapped to top of log */
+	u32 next_entry; /* index of next entry to be written by uCode */
+
+	if (priv->ucode_type == UCODE_INIT)
+		base = le32_to_cpu(priv->card_alive_init.error_event_table_ptr);
+	else
+		base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+	if (priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+		capacity = iwl_legacy_read_targ_mem(priv, base);
+		num_wraps = iwl_legacy_read_targ_mem(priv,
+						base + (2 * sizeof(u32)));
+		mode = iwl_legacy_read_targ_mem(priv, base + (1 * sizeof(u32)));
+		next_entry = iwl_legacy_read_targ_mem(priv,
+						base + (3 * sizeof(u32)));
+	} else
+		return;
+
+	if (num_wraps == priv->event_log.num_wraps) {
+		iwl4965_print_cont_event_trace(priv,
+				       base, priv->event_log.next_entry,
+				       next_entry - priv->event_log.next_entry,
+				       mode);
+		priv->event_log.non_wraps_count++;
+	} else {
+		if ((num_wraps - priv->event_log.num_wraps) > 1)
+			priv->event_log.wraps_more_count++;
+		else
+			priv->event_log.wraps_once_count++;
+		trace_iwlwifi_legacy_dev_ucode_wrap_event(priv,
+				num_wraps - priv->event_log.num_wraps,
+				next_entry, priv->event_log.next_entry);
+		if (next_entry < priv->event_log.next_entry) {
+			iwl4965_print_cont_event_trace(priv, base,
+			       priv->event_log.next_entry,
+			       capacity - priv->event_log.next_entry,
+			       mode);
+
+			iwl4965_print_cont_event_trace(priv, base, 0,
+				next_entry, mode);
+		} else {
+			iwl4965_print_cont_event_trace(priv, base,
+			       next_entry, capacity - next_entry,
+			       mode);
+
+			iwl4965_print_cont_event_trace(priv, base, 0,
+				next_entry, mode);
+		}
+	}
+	priv->event_log.num_wraps = num_wraps;
+	priv->event_log.next_entry = next_entry;
+}
+
+/**
+ * iwl4965_bg_ucode_trace - Timer callback to log ucode event
+ *
+ * The timer is continually set to execute every
+ * UCODE_TRACE_PERIOD milliseconds after the last timer expired
+ * this function is to perform continuous uCode event logging operation
+ * if enabled
+ */
+static void iwl4965_bg_ucode_trace(unsigned long data)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)data;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (priv->event_log.ucode_trace) {
+		iwl4965_continuous_event_trace(priv);
+		/* Reschedule the timer to occur in UCODE_TRACE_PERIOD */
+		mod_timer(&priv->ucode_trace,
+			 jiffies + msecs_to_jiffies(UCODE_TRACE_PERIOD));
+	}
+}
+
+static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl4965_beacon_notif *beacon =
+		(struct iwl4965_beacon_notif *)pkt->u.raw;
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	u8 rate = iwl4965_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+
+	IWL_DEBUG_RX(priv, "beacon status %x retries %d iss %d "
+		"tsf %d %d rate %d\n",
+		le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
+		beacon->beacon_notify_hdr.failure_frame,
+		le32_to_cpu(beacon->ibss_mgr_status),
+		le32_to_cpu(beacon->high_tsf),
+		le32_to_cpu(beacon->low_tsf), rate);
+#endif
+
+	priv->ibss_manager = le32_to_cpu(beacon->ibss_mgr_status);
+}
+
+static void iwl4965_perform_ct_kill_task(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	IWL_DEBUG_POWER(priv, "Stop all queues\n");
+
+	if (priv->mac80211_registered)
+		ieee80211_stop_queues(priv->hw);
+
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+			CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
+	iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+	spin_lock_irqsave(&priv->reg_lock, flags);
+	if (!iwl_grab_nic_access(priv))
+		iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, flags);
+}
+
+/* Handle notification from uCode that card's power state is changing
+ * due to software, hardware, or critical temperature RFKILL */
+static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	u32 flags = le32_to_cpu(pkt->u.card_state_notif.flags);
+	unsigned long status = priv->status;
+
+	IWL_DEBUG_RF_KILL(priv, "Card state received: HW:%s SW:%s CT:%s\n",
+			  (flags & HW_CARD_DISABLED) ? "Kill" : "On",
+			  (flags & SW_CARD_DISABLED) ? "Kill" : "On",
+			  (flags & CT_CARD_DISABLED) ?
+			  "Reached" : "Not reached");
+
+	if (flags & (SW_CARD_DISABLED | HW_CARD_DISABLED |
+		     CT_CARD_DISABLED)) {
+
+		iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+			    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+		iwl_legacy_write_direct32(priv, HBUS_TARG_MBX_C,
+					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
+
+		if (!(flags & RXON_CARD_DISABLED)) {
+			iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+				    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+			iwl_legacy_write_direct32(priv, HBUS_TARG_MBX_C,
+					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
+		}
+	}
+
+	if (flags & CT_CARD_DISABLED)
+		iwl4965_perform_ct_kill_task(priv);
+
+	if (flags & HW_CARD_DISABLED)
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+
+	if (!(flags & RXON_CARD_DISABLED))
+		iwl_legacy_scan_cancel(priv);
+
+	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
+	     test_bit(STATUS_RF_KILL_HW, &priv->status)))
+		wiphy_rfkill_set_hw_state(priv->hw->wiphy,
+			test_bit(STATUS_RF_KILL_HW, &priv->status));
+	else
+		wake_up_interruptible(&priv->wait_command_queue);
+}
+
+/**
+ * iwl4965_setup_rx_handlers - Initialize Rx handler callbacks
+ *
+ * Setup the RX handlers for each of the reply types sent from the uCode
+ * to the host.
+ *
+ * This function chains into the hardware specific files for them to setup
+ * any hardware specific handlers as well.
+ */
+static void iwl4965_setup_rx_handlers(struct iwl_priv *priv)
+{
+	priv->rx_handlers[REPLY_ALIVE] = iwl4965_rx_reply_alive;
+	priv->rx_handlers[REPLY_ERROR] = iwl_legacy_rx_reply_error;
+	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl_legacy_rx_csa;
+	priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
+			iwl_legacy_rx_spectrum_measure_notif;
+	priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_legacy_rx_pm_sleep_notif;
+	priv->rx_handlers[PM_DEBUG_STATISTIC_NOTIFIC] =
+	    iwl_legacy_rx_pm_debug_statistics_notif;
+	priv->rx_handlers[BEACON_NOTIFICATION] = iwl4965_rx_beacon_notif;
+
+	/*
+	 * The same handler is used for both the REPLY to a discrete
+	 * statistics request from the host as well as for the periodic
+	 * statistics notifications (after received beacons) from the uCode.
+	 */
+	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl4965_reply_statistics;
+	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl4965_rx_statistics;
+
+	iwl_legacy_setup_rx_scan_handlers(priv);
+
+	/* status change handler */
+	priv->rx_handlers[CARD_STATE_NOTIFICATION] =
+					iwl4965_rx_card_state_notif;
+
+	priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
+	    iwl4965_rx_missed_beacon_notif;
+	/* Rx handlers */
+	priv->rx_handlers[REPLY_RX_PHY_CMD] = iwl4965_rx_reply_rx_phy;
+	priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwl4965_rx_reply_rx;
+	/* block ack */
+	priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
+	/* Set up hardware specific Rx handlers */
+	priv->cfg->ops->lib->rx_handler_setup(priv);
+}
+
+/**
+ * iwl4965_rx_handle - Main entry function for receiving responses from uCode
+ *
+ * Uses the priv->rx_handlers callback function array to invoke
+ * the appropriate handlers, including command responses,
+ * frame-received notifications, and other notifications.
+ */
+void iwl4965_rx_handle(struct iwl_priv *priv)
+{
+	struct iwl_rx_mem_buffer *rxb;
+	struct iwl_rx_packet *pkt;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	u32 r, i;
+	int reclaim;
+	unsigned long flags;
+	u8 fill_rx = 0;
+	u32 count = 8;
+	int total_empty;
+
+	/* uCode's read index (stored in shared DRAM) indicates the last Rx
+	 * buffer that the driver may process (last buffer filled by ucode). */
+	r = le16_to_cpu(rxq->rb_stts->closed_rb_num) &  0x0FFF;
+	i = rxq->read;
+
+	/* Rx interrupt, but nothing sent from uCode */
+	if (i == r)
+		IWL_DEBUG_RX(priv, "r = %d, i = %d\n", r, i);
+
+	/* calculate total frames need to be restock after handling RX */
+	total_empty = r - rxq->write_actual;
+	if (total_empty < 0)
+		total_empty += RX_QUEUE_SIZE;
+
+	if (total_empty > (RX_QUEUE_SIZE / 2))
+		fill_rx = 1;
+
+	while (i != r) {
+		int len;
+
+		rxb = rxq->queue[i];
+
+		/* If an RXB doesn't have a Rx queue slot associated with it,
+		 * then a bug has been introduced in the queue refilling
+		 * routines -- catch it here */
+		BUG_ON(rxb == NULL);
+
+		rxq->queue[i] = NULL;
+
+		pci_unmap_page(priv->pci_dev, rxb->page_dma,
+			       PAGE_SIZE << priv->hw_params.rx_page_order,
+			       PCI_DMA_FROMDEVICE);
+		pkt = rxb_addr(rxb);
+
+		len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+		len += sizeof(u32); /* account for status word */
+		trace_iwlwifi_legacy_dev_rx(priv, pkt, len);
+
+		/* Reclaim a command buffer only if this packet is a response
+		 *   to a (driver-originated) command.
+		 * If the packet (e.g. Rx frame) originated from uCode,
+		 *   there is no command buffer to reclaim.
+		 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
+		 *   but apparently a few don't get set; catch them here. */
+		reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME) &&
+			(pkt->hdr.cmd != REPLY_RX_PHY_CMD) &&
+			(pkt->hdr.cmd != REPLY_RX) &&
+			(pkt->hdr.cmd != REPLY_RX_MPDU_CMD) &&
+			(pkt->hdr.cmd != REPLY_COMPRESSED_BA) &&
+			(pkt->hdr.cmd != STATISTICS_NOTIFICATION) &&
+			(pkt->hdr.cmd != REPLY_TX);
+
+		/* Based on type of command response or notification,
+		 *   handle those that need handling via function in
+		 *   rx_handlers table.  See iwl4965_setup_rx_handlers() */
+		if (priv->rx_handlers[pkt->hdr.cmd]) {
+			IWL_DEBUG_RX(priv, "r = %d, i = %d, %s, 0x%02x\n", r,
+				i, iwl_legacy_get_cmd_string(pkt->hdr.cmd),
+				pkt->hdr.cmd);
+			priv->isr_stats.rx_handlers[pkt->hdr.cmd]++;
+			priv->rx_handlers[pkt->hdr.cmd] (priv, rxb);
+		} else {
+			/* No handling needed */
+			IWL_DEBUG_RX(priv,
+				"r %d i %d No handler needed for %s, 0x%02x\n",
+				r, i, iwl_legacy_get_cmd_string(pkt->hdr.cmd),
+				pkt->hdr.cmd);
+		}
+
+		/*
+		 * XXX: After here, we should always check rxb->page
+		 * against NULL before touching it or its virtual
+		 * memory (pkt). Because some rx_handler might have
+		 * already taken or freed the pages.
+		 */
+
+		if (reclaim) {
+			/* Invoke any callbacks, transfer the buffer to caller,
+			 * and fire off the (possibly) blocking iwl_legacy_send_cmd()
+			 * as we reclaim the driver command queue */
+			if (rxb->page)
+				iwl_legacy_tx_cmd_complete(priv, rxb);
+			else
+				IWL_WARN(priv, "Claim null rxb?\n");
+		}
+
+		/* Reuse the page if possible. For notification packets and
+		 * SKBs that fail to Rx correctly, add them back into the
+		 * rx_free list for reuse later. */
+		spin_lock_irqsave(&rxq->lock, flags);
+		if (rxb->page != NULL) {
+			rxb->page_dma = pci_map_page(priv->pci_dev, rxb->page,
+				0, PAGE_SIZE << priv->hw_params.rx_page_order,
+				PCI_DMA_FROMDEVICE);
+			list_add_tail(&rxb->list, &rxq->rx_free);
+			rxq->free_count++;
+		} else
+			list_add_tail(&rxb->list, &rxq->rx_used);
+
+		spin_unlock_irqrestore(&rxq->lock, flags);
+
+		i = (i + 1) & RX_QUEUE_MASK;
+		/* If there are a lot of unused frames,
+		 * restock the Rx queue so ucode wont assert. */
+		if (fill_rx) {
+			count++;
+			if (count >= 8) {
+				rxq->read = i;
+				iwl4965_rx_replenish_now(priv);
+				count = 0;
+			}
+		}
+	}
+
+	/* Backtrack one entry */
+	rxq->read = i;
+	if (fill_rx)
+		iwl4965_rx_replenish_now(priv);
+	else
+		iwl4965_rx_queue_restock(priv);
+}
+
+/* call this function to flush any scheduled tasklet */
+static inline void iwl4965_synchronize_irq(struct iwl_priv *priv)
+{
+	/* wait to make sure we flush pending tasklet*/
+	synchronize_irq(priv->pci_dev->irq);
+	tasklet_kill(&priv->irq_tasklet);
+}
+
+static void iwl4965_irq_tasklet(struct iwl_priv *priv)
+{
+	u32 inta, handled = 0;
+	u32 inta_fh;
+	unsigned long flags;
+	u32 i;
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	u32 inta_mask;
+#endif
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Ack/clear/reset pending uCode interrupts.
+	 * Note:  Some bits in CSR_INT are "OR" of bits in CSR_FH_INT_STATUS,
+	 *  and will clear only when CSR_FH_INT_STATUS gets cleared. */
+	inta = iwl_read32(priv, CSR_INT);
+	iwl_write32(priv, CSR_INT, inta);
+
+	/* Ack/clear/reset pending flow-handler (DMA) interrupts.
+	 * Any new interrupts that happen after this, either while we're
+	 * in this tasklet, or later, will show up in next ISR/tasklet. */
+	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+	iwl_write32(priv, CSR_FH_INT_STATUS, inta_fh);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & IWL_DL_ISR) {
+		/* just for debug */
+		inta_mask = iwl_read32(priv, CSR_INT_MASK);
+		IWL_DEBUG_ISR(priv, "inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
+			      inta, inta_mask, inta_fh);
+	}
+#endif
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Since CSR_INT and CSR_FH_INT_STATUS reads and clears are not
+	 * atomic, make sure that inta covers all the interrupts that
+	 * we've discovered, even if FH interrupt came in just after
+	 * reading CSR_INT. */
+	if (inta_fh & CSR49_FH_INT_RX_MASK)
+		inta |= CSR_INT_BIT_FH_RX;
+	if (inta_fh & CSR49_FH_INT_TX_MASK)
+		inta |= CSR_INT_BIT_FH_TX;
+
+	/* Now service all interrupt bits discovered above. */
+	if (inta & CSR_INT_BIT_HW_ERR) {
+		IWL_ERR(priv, "Hardware error detected.  Restarting.\n");
+
+		/* Tell the device to stop sending interrupts */
+		iwl_legacy_disable_interrupts(priv);
+
+		priv->isr_stats.hw++;
+		iwl_legacy_irq_handle_error(priv);
+
+		handled |= CSR_INT_BIT_HW_ERR;
+
+		return;
+	}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & (IWL_DL_ISR)) {
+		/* NIC fires this, but we don't use it, redundant with WAKEUP */
+		if (inta & CSR_INT_BIT_SCD) {
+			IWL_DEBUG_ISR(priv, "Scheduler finished to transmit "
+				      "the frame/frames.\n");
+			priv->isr_stats.sch++;
+		}
+
+		/* Alive notification via Rx interrupt will do the real work */
+		if (inta & CSR_INT_BIT_ALIVE) {
+			IWL_DEBUG_ISR(priv, "Alive interrupt\n");
+			priv->isr_stats.alive++;
+		}
+	}
+#endif
+	/* Safely ignore these bits for debug checks below */
+	inta &= ~(CSR_INT_BIT_SCD | CSR_INT_BIT_ALIVE);
+
+	/* HW RF KILL switch toggled */
+	if (inta & CSR_INT_BIT_RF_KILL) {
+		int hw_rf_kill = 0;
+		if (!(iwl_read32(priv, CSR_GP_CNTRL) &
+				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW))
+			hw_rf_kill = 1;
+
+		IWL_WARN(priv, "RF_KILL bit toggled to %s.\n",
+				hw_rf_kill ? "disable radio" : "enable radio");
+
+		priv->isr_stats.rfkill++;
+
+		/* driver only loads ucode once setting the interface up.
+		 * the driver allows loading the ucode even if the radio
+		 * is killed. Hence update the killswitch state here. The
+		 * rfkill handler will care about restarting if needed.
+		 */
+		if (!test_bit(STATUS_ALIVE, &priv->status)) {
+			if (hw_rf_kill)
+				set_bit(STATUS_RF_KILL_HW, &priv->status);
+			else
+				clear_bit(STATUS_RF_KILL_HW, &priv->status);
+			wiphy_rfkill_set_hw_state(priv->hw->wiphy, hw_rf_kill);
+		}
+
+		handled |= CSR_INT_BIT_RF_KILL;
+	}
+
+	/* Chip got too hot and stopped itself */
+	if (inta & CSR_INT_BIT_CT_KILL) {
+		IWL_ERR(priv, "Microcode CT kill error detected.\n");
+		priv->isr_stats.ctkill++;
+		handled |= CSR_INT_BIT_CT_KILL;
+	}
+
+	/* Error detected by uCode */
+	if (inta & CSR_INT_BIT_SW_ERR) {
+		IWL_ERR(priv, "Microcode SW error detected. "
+			" Restarting 0x%X.\n", inta);
+		priv->isr_stats.sw++;
+		iwl_legacy_irq_handle_error(priv);
+		handled |= CSR_INT_BIT_SW_ERR;
+	}
+
+	/*
+	 * uCode wakes up after power-down sleep.
+	 * Tell device about any new tx or host commands enqueued,
+	 * and about any Rx buffers made available while asleep.
+	 */
+	if (inta & CSR_INT_BIT_WAKEUP) {
+		IWL_DEBUG_ISR(priv, "Wakeup interrupt\n");
+		iwl_legacy_rx_queue_update_write_ptr(priv, &priv->rxq);
+		for (i = 0; i < priv->hw_params.max_txq_num; i++)
+			iwl_legacy_txq_update_write_ptr(priv, &priv->txq[i]);
+		priv->isr_stats.wakeup++;
+		handled |= CSR_INT_BIT_WAKEUP;
+	}
+
+	/* All uCode command responses, including Tx command responses,
+	 * Rx "responses" (frame-received notification), and other
+	 * notifications from uCode come through here*/
+	if (inta & (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX)) {
+		iwl4965_rx_handle(priv);
+		priv->isr_stats.rx++;
+		handled |= (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX);
+	}
+
+	/* This "Tx" DMA channel is used only for loading uCode */
+	if (inta & CSR_INT_BIT_FH_TX) {
+		IWL_DEBUG_ISR(priv, "uCode load interrupt\n");
+		priv->isr_stats.tx++;
+		handled |= CSR_INT_BIT_FH_TX;
+		/* Wake up uCode load routine, now that load is complete */
+		priv->ucode_write_complete = 1;
+		wake_up_interruptible(&priv->wait_command_queue);
+	}
+
+	if (inta & ~handled) {
+		IWL_ERR(priv, "Unhandled INTA bits 0x%08x\n", inta & ~handled);
+		priv->isr_stats.unhandled++;
+	}
+
+	if (inta & ~(priv->inta_mask)) {
+		IWL_WARN(priv, "Disabled INTA bits 0x%08x were pending\n",
+			 inta & ~priv->inta_mask);
+		IWL_WARN(priv, "   with FH_INT = 0x%08x\n", inta_fh);
+	}
+
+	/* Re-enable all interrupts */
+	/* only Re-enable if diabled by irq */
+	if (test_bit(STATUS_INT_ENABLED, &priv->status))
+		iwl_legacy_enable_interrupts(priv);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (iwl_legacy_get_debug_level(priv) & (IWL_DL_ISR)) {
+		inta = iwl_read32(priv, CSR_INT);
+		inta_mask = iwl_read32(priv, CSR_INT_MASK);
+		inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+		IWL_DEBUG_ISR(priv,
+			"End inta 0x%08x, enabled 0x%08x, fh 0x%08x, "
+			"flags 0x%08lx\n", inta, inta_mask, inta_fh, flags);
+	}
+#endif
+}
+
+/*****************************************************************************
+ *
+ * sysfs attributes
+ *
+ *****************************************************************************/
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+
+/*
+ * The following adds a new attribute to the sysfs representation
+ * of this device driver (i.e. a new file in /sys/class/net/wlan0/device/)
+ * used for controlling the debug level.
+ *
+ * See the level definitions in iwl for details.
+ *
+ * The debug_level being managed using sysfs below is a per device debug
+ * level that is used instead of the global debug level if it (the per
+ * device debug level) is set.
+ */
+static ssize_t iwl4965_show_debug_level(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	return sprintf(buf, "0x%08X\n", iwl_legacy_get_debug_level(priv));
+}
+static ssize_t iwl4965_store_debug_level(struct device *d,
+				struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	unsigned long val;
+	int ret;
+
+	ret = strict_strtoul(buf, 0, &val);
+	if (ret)
+		IWL_ERR(priv, "%s is not in hex or decimal form.\n", buf);
+	else {
+		priv->debug_level = val;
+		if (iwl_legacy_alloc_traffic_mem(priv))
+			IWL_ERR(priv,
+				"Not enough memory to generate traffic log\n");
+	}
+	return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
+			iwl4965_show_debug_level, iwl4965_store_debug_level);
+
+
+#endif /* CONFIG_IWLWIFI_LEGACY_DEBUG */
+
+
+static ssize_t iwl4965_show_temperature(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	if (!iwl_legacy_is_alive(priv))
+		return -EAGAIN;
+
+	return sprintf(buf, "%d\n", priv->temperature);
+}
+
+static DEVICE_ATTR(temperature, S_IRUGO, iwl4965_show_temperature, NULL);
+
+static ssize_t iwl4965_show_tx_power(struct device *d,
+			     struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	if (!iwl_legacy_is_ready_rf(priv))
+		return sprintf(buf, "off\n");
+	else
+		return sprintf(buf, "%d\n", priv->tx_power_user_lmt);
+}
+
+static ssize_t iwl4965_store_tx_power(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	unsigned long val;
+	int ret;
+
+	ret = strict_strtoul(buf, 10, &val);
+	if (ret)
+		IWL_INFO(priv, "%s is not in decimal form.\n", buf);
+	else {
+		ret = iwl_legacy_set_tx_power(priv, val, false);
+		if (ret)
+			IWL_ERR(priv, "failed setting tx power (0x%d).\n",
+				ret);
+		else
+			ret = count;
+	}
+	return ret;
+}
+
+static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO,
+			iwl4965_show_tx_power, iwl4965_store_tx_power);
+
+static struct attribute *iwl_sysfs_entries[] = {
+	&dev_attr_temperature.attr,
+	&dev_attr_tx_power.attr,
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	&dev_attr_debug_level.attr,
+#endif
+	NULL
+};
+
+static struct attribute_group iwl_attribute_group = {
+	.name = NULL,		/* put in device directory */
+	.attrs = iwl_sysfs_entries,
+};
+
+/******************************************************************************
+ *
+ * uCode download functions
+ *
+ ******************************************************************************/
+
+static void iwl4965_dealloc_ucode_pci(struct iwl_priv *priv)
+{
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_code);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_data);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_data_backup);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_init);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_init_data);
+	iwl_legacy_free_fw_desc(priv->pci_dev, &priv->ucode_boot);
+}
+
+static void iwl4965_nic_start(struct iwl_priv *priv)
+{
+	/* Remove all resets to allow NIC to operate */
+	iwl_write32(priv, CSR_RESET, 0);
+}
+
+static void iwl4965_ucode_callback(const struct firmware *ucode_raw,
+					void *context);
+static int iwl4965_mac_setup_register(struct iwl_priv *priv,
+						u32 max_probe_length);
+
+static int __must_check iwl4965_request_firmware(struct iwl_priv *priv, bool first)
+{
+	const char *name_pre = priv->cfg->fw_name_pre;
+	char tag[8];
+
+	if (first) {
+		priv->fw_index = priv->cfg->ucode_api_max;
+		sprintf(tag, "%d", priv->fw_index);
+	} else {
+		priv->fw_index--;
+		sprintf(tag, "%d", priv->fw_index);
+	}
+
+	if (priv->fw_index < priv->cfg->ucode_api_min) {
+		IWL_ERR(priv, "no suitable firmware found!\n");
+		return -ENOENT;
+	}
+
+	sprintf(priv->firmware_name, "%s%s%s", name_pre, tag, ".ucode");
+
+	IWL_DEBUG_INFO(priv, "attempting to load firmware '%s'\n",
+		       priv->firmware_name);
+
+	return request_firmware_nowait(THIS_MODULE, 1, priv->firmware_name,
+				       &priv->pci_dev->dev, GFP_KERNEL, priv,
+				       iwl4965_ucode_callback);
+}
+
+struct iwl4965_firmware_pieces {
+	const void *inst, *data, *init, *init_data, *boot;
+	size_t inst_size, data_size, init_size, init_data_size, boot_size;
+};
+
+static int iwl4965_load_firmware(struct iwl_priv *priv,
+				       const struct firmware *ucode_raw,
+				       struct iwl4965_firmware_pieces *pieces)
+{
+	struct iwl_ucode_header *ucode = (void *)ucode_raw->data;
+	u32 api_ver, hdr_size;
+	const u8 *src;
+
+	priv->ucode_ver = le32_to_cpu(ucode->ver);
+	api_ver = IWL_UCODE_API(priv->ucode_ver);
+
+	switch (api_ver) {
+	default:
+	case 0:
+	case 1:
+	case 2:
+		hdr_size = 24;
+		if (ucode_raw->size < hdr_size) {
+			IWL_ERR(priv, "File size too small!\n");
+			return -EINVAL;
+		}
+		pieces->inst_size = le32_to_cpu(ucode->v1.inst_size);
+		pieces->data_size = le32_to_cpu(ucode->v1.data_size);
+		pieces->init_size = le32_to_cpu(ucode->v1.init_size);
+		pieces->init_data_size =
+				le32_to_cpu(ucode->v1.init_data_size);
+		pieces->boot_size = le32_to_cpu(ucode->v1.boot_size);
+		src = ucode->v1.data;
+		break;
+	}
+
+	/* Verify size of file vs. image size info in file's header */
+	if (ucode_raw->size != hdr_size + pieces->inst_size +
+				pieces->data_size + pieces->init_size +
+				pieces->init_data_size + pieces->boot_size) {
+
+		IWL_ERR(priv,
+			"uCode file size %d does not match expected size\n",
+			(int)ucode_raw->size);
+		return -EINVAL;
+	}
+
+	pieces->inst = src;
+	src += pieces->inst_size;
+	pieces->data = src;
+	src += pieces->data_size;
+	pieces->init = src;
+	src += pieces->init_size;
+	pieces->init_data = src;
+	src += pieces->init_data_size;
+	pieces->boot = src;
+	src += pieces->boot_size;
+
+	return 0;
+}
+
+/**
+ * iwl4965_ucode_callback - callback when firmware was loaded
+ *
+ * If loaded successfully, copies the firmware into buffers
+ * for the card to fetch (via DMA).
+ */
+static void
+iwl4965_ucode_callback(const struct firmware *ucode_raw, void *context)
+{
+	struct iwl_priv *priv = context;
+	struct iwl_ucode_header *ucode;
+	int err;
+	struct iwl4965_firmware_pieces pieces;
+	const unsigned int api_max = priv->cfg->ucode_api_max;
+	const unsigned int api_min = priv->cfg->ucode_api_min;
+	u32 api_ver;
+
+	u32 max_probe_length = 200;
+	u32 standard_phy_calibration_size =
+			IWL_DEFAULT_STANDARD_PHY_CALIBRATE_TBL_SIZE;
+
+	memset(&pieces, 0, sizeof(pieces));
+
+	if (!ucode_raw) {
+		if (priv->fw_index <= priv->cfg->ucode_api_max)
+			IWL_ERR(priv,
+				"request for firmware file '%s' failed.\n",
+				priv->firmware_name);
+		goto try_again;
+	}
+
+	IWL_DEBUG_INFO(priv, "Loaded firmware file '%s' (%zd bytes).\n",
+		       priv->firmware_name, ucode_raw->size);
+
+	/* Make sure that we got at least the API version number */
+	if (ucode_raw->size < 4) {
+		IWL_ERR(priv, "File size way too small!\n");
+		goto try_again;
+	}
+
+	/* Data from ucode file:  header followed by uCode images */
+	ucode = (struct iwl_ucode_header *)ucode_raw->data;
+
+	err = iwl4965_load_firmware(priv, ucode_raw, &pieces);
+
+	if (err)
+		goto try_again;
+
+	api_ver = IWL_UCODE_API(priv->ucode_ver);
+
+	/*
+	 * api_ver should match the api version forming part of the
+	 * firmware filename ... but we don't check for that and only rely
+	 * on the API version read from firmware header from here on forward
+	 */
+	if (api_ver < api_min || api_ver > api_max) {
+		IWL_ERR(priv,
+			"Driver unable to support your firmware API. "
+			"Driver supports v%u, firmware is v%u.\n",
+			api_max, api_ver);
+		goto try_again;
+	}
+
+	if (api_ver != api_max)
+		IWL_ERR(priv,
+			"Firmware has old API version. Expected v%u, "
+			"got v%u. New firmware can be obtained "
+			"from http://www.intellinuxwireless.org.\n",
+			api_max, api_ver);
+
+	IWL_INFO(priv, "loaded firmware version %u.%u.%u.%u\n",
+		 IWL_UCODE_MAJOR(priv->ucode_ver),
+		 IWL_UCODE_MINOR(priv->ucode_ver),
+		 IWL_UCODE_API(priv->ucode_ver),
+		 IWL_UCODE_SERIAL(priv->ucode_ver));
+
+	snprintf(priv->hw->wiphy->fw_version,
+		 sizeof(priv->hw->wiphy->fw_version),
+		 "%u.%u.%u.%u",
+		 IWL_UCODE_MAJOR(priv->ucode_ver),
+		 IWL_UCODE_MINOR(priv->ucode_ver),
+		 IWL_UCODE_API(priv->ucode_ver),
+		 IWL_UCODE_SERIAL(priv->ucode_ver));
+
+	/*
+	 * For any of the failures below (before allocating pci memory)
+	 * we will try to load a version with a smaller API -- maybe the
+	 * user just got a corrupted version of the latest API.
+	 */
+
+	IWL_DEBUG_INFO(priv, "f/w package hdr ucode version raw = 0x%x\n",
+		       priv->ucode_ver);
+	IWL_DEBUG_INFO(priv, "f/w package hdr runtime inst size = %Zd\n",
+		       pieces.inst_size);
+	IWL_DEBUG_INFO(priv, "f/w package hdr runtime data size = %Zd\n",
+		       pieces.data_size);
+	IWL_DEBUG_INFO(priv, "f/w package hdr init inst size = %Zd\n",
+		       pieces.init_size);
+	IWL_DEBUG_INFO(priv, "f/w package hdr init data size = %Zd\n",
+		       pieces.init_data_size);
+	IWL_DEBUG_INFO(priv, "f/w package hdr boot inst size = %Zd\n",
+		       pieces.boot_size);
+
+	/* Verify that uCode images will fit in card's SRAM */
+	if (pieces.inst_size > priv->hw_params.max_inst_size) {
+		IWL_ERR(priv, "uCode instr len %Zd too large to fit in\n",
+			pieces.inst_size);
+		goto try_again;
+	}
+
+	if (pieces.data_size > priv->hw_params.max_data_size) {
+		IWL_ERR(priv, "uCode data len %Zd too large to fit in\n",
+			pieces.data_size);
+		goto try_again;
+	}
+
+	if (pieces.init_size > priv->hw_params.max_inst_size) {
+		IWL_ERR(priv, "uCode init instr len %Zd too large to fit in\n",
+			pieces.init_size);
+		goto try_again;
+	}
+
+	if (pieces.init_data_size > priv->hw_params.max_data_size) {
+		IWL_ERR(priv, "uCode init data len %Zd too large to fit in\n",
+			pieces.init_data_size);
+		goto try_again;
+	}
+
+	if (pieces.boot_size > priv->hw_params.max_bsm_size) {
+		IWL_ERR(priv, "uCode boot instr len %Zd too large to fit in\n",
+			pieces.boot_size);
+		goto try_again;
+	}
+
+	/* Allocate ucode buffers for card's bus-master loading ... */
+
+	/* Runtime instructions and 2 copies of data:
+	 * 1) unmodified from disk
+	 * 2) backup cache for save/restore during power-downs */
+	priv->ucode_code.len = pieces.inst_size;
+	iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_code);
+
+	priv->ucode_data.len = pieces.data_size;
+	iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_data);
+
+	priv->ucode_data_backup.len = pieces.data_size;
+	iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_data_backup);
+
+	if (!priv->ucode_code.v_addr || !priv->ucode_data.v_addr ||
+	    !priv->ucode_data_backup.v_addr)
+		goto err_pci_alloc;
+
+	/* Initialization instructions and data */
+	if (pieces.init_size && pieces.init_data_size) {
+		priv->ucode_init.len = pieces.init_size;
+		iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_init);
+
+		priv->ucode_init_data.len = pieces.init_data_size;
+		iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_init_data);
+
+		if (!priv->ucode_init.v_addr || !priv->ucode_init_data.v_addr)
+			goto err_pci_alloc;
+	}
+
+	/* Bootstrap (instructions only, no data) */
+	if (pieces.boot_size) {
+		priv->ucode_boot.len = pieces.boot_size;
+		iwl_legacy_alloc_fw_desc(priv->pci_dev, &priv->ucode_boot);
+
+		if (!priv->ucode_boot.v_addr)
+			goto err_pci_alloc;
+	}
+
+	/* Now that we can no longer fail, copy information */
+
+	priv->sta_key_max_num = STA_KEY_MAX_NUM;
+
+	/* Copy images into buffers for card's bus-master reads ... */
+
+	/* Runtime instructions (first block of data in file) */
+	IWL_DEBUG_INFO(priv, "Copying (but not loading) uCode instr len %Zd\n",
+			pieces.inst_size);
+	memcpy(priv->ucode_code.v_addr, pieces.inst, pieces.inst_size);
+
+	IWL_DEBUG_INFO(priv, "uCode instr buf vaddr = 0x%p, paddr = 0x%08x\n",
+		priv->ucode_code.v_addr, (u32)priv->ucode_code.p_addr);
+
+	/*
+	 * Runtime data
+	 * NOTE:  Copy into backup buffer will be done in iwl_up()
+	 */
+	IWL_DEBUG_INFO(priv, "Copying (but not loading) uCode data len %Zd\n",
+			pieces.data_size);
+	memcpy(priv->ucode_data.v_addr, pieces.data, pieces.data_size);
+	memcpy(priv->ucode_data_backup.v_addr, pieces.data, pieces.data_size);
+
+	/* Initialization instructions */
+	if (pieces.init_size) {
+		IWL_DEBUG_INFO(priv,
+				"Copying (but not loading) init instr len %Zd\n",
+				pieces.init_size);
+		memcpy(priv->ucode_init.v_addr, pieces.init, pieces.init_size);
+	}
+
+	/* Initialization data */
+	if (pieces.init_data_size) {
+		IWL_DEBUG_INFO(priv,
+				"Copying (but not loading) init data len %Zd\n",
+			       pieces.init_data_size);
+		memcpy(priv->ucode_init_data.v_addr, pieces.init_data,
+		       pieces.init_data_size);
+	}
+
+	/* Bootstrap instructions */
+	IWL_DEBUG_INFO(priv, "Copying (but not loading) boot instr len %Zd\n",
+			pieces.boot_size);
+	memcpy(priv->ucode_boot.v_addr, pieces.boot, pieces.boot_size);
+
+	/*
+	 * figure out the offset of chain noise reset and gain commands
+	 * base on the size of standard phy calibration commands table size
+	 */
+	priv->_4965.phy_calib_chain_noise_reset_cmd =
+		standard_phy_calibration_size;
+	priv->_4965.phy_calib_chain_noise_gain_cmd =
+		standard_phy_calibration_size + 1;
+
+	/**************************************************
+	 * This is still part of probe() in a sense...
+	 *
+	 * 9. Setup and register with mac80211 and debugfs
+	 **************************************************/
+	err = iwl4965_mac_setup_register(priv, max_probe_length);
+	if (err)
+		goto out_unbind;
+
+	err = iwl_legacy_dbgfs_register(priv, DRV_NAME);
+	if (err)
+		IWL_ERR(priv,
+		"failed to create debugfs files. Ignoring error: %d\n", err);
+
+	err = sysfs_create_group(&priv->pci_dev->dev.kobj,
+					&iwl_attribute_group);
+	if (err) {
+		IWL_ERR(priv, "failed to create sysfs device attributes\n");
+		goto out_unbind;
+	}
+
+	/* We have our copies now, allow OS release its copies */
+	release_firmware(ucode_raw);
+	complete(&priv->_4965.firmware_loading_complete);
+	return;
+
+ try_again:
+	/* try next, if any */
+	if (iwl4965_request_firmware(priv, false))
+		goto out_unbind;
+	release_firmware(ucode_raw);
+	return;
+
+ err_pci_alloc:
+	IWL_ERR(priv, "failed to allocate pci memory\n");
+	iwl4965_dealloc_ucode_pci(priv);
+ out_unbind:
+	complete(&priv->_4965.firmware_loading_complete);
+	device_release_driver(&priv->pci_dev->dev);
+	release_firmware(ucode_raw);
+}
+
+static const char * const desc_lookup_text[] = {
+	"OK",
+	"FAIL",
+	"BAD_PARAM",
+	"BAD_CHECKSUM",
+	"NMI_INTERRUPT_WDG",
+	"SYSASSERT",
+	"FATAL_ERROR",
+	"BAD_COMMAND",
+	"HW_ERROR_TUNE_LOCK",
+	"HW_ERROR_TEMPERATURE",
+	"ILLEGAL_CHAN_FREQ",
+	"VCC_NOT_STABLE",
+	"FH_ERROR",
+	"NMI_INTERRUPT_HOST",
+	"NMI_INTERRUPT_ACTION_PT",
+	"NMI_INTERRUPT_UNKNOWN",
+	"UCODE_VERSION_MISMATCH",
+	"HW_ERROR_ABS_LOCK",
+	"HW_ERROR_CAL_LOCK_FAIL",
+	"NMI_INTERRUPT_INST_ACTION_PT",
+	"NMI_INTERRUPT_DATA_ACTION_PT",
+	"NMI_TRM_HW_ER",
+	"NMI_INTERRUPT_TRM",
+	"NMI_INTERRUPT_BREAK_POINT"
+	"DEBUG_0",
+	"DEBUG_1",
+	"DEBUG_2",
+	"DEBUG_3",
+};
+
+static struct { char *name; u8 num; } advanced_lookup[] = {
+	{ "NMI_INTERRUPT_WDG", 0x34 },
+	{ "SYSASSERT", 0x35 },
+	{ "UCODE_VERSION_MISMATCH", 0x37 },
+	{ "BAD_COMMAND", 0x38 },
+	{ "NMI_INTERRUPT_DATA_ACTION_PT", 0x3C },
+	{ "FATAL_ERROR", 0x3D },
+	{ "NMI_TRM_HW_ERR", 0x46 },
+	{ "NMI_INTERRUPT_TRM", 0x4C },
+	{ "NMI_INTERRUPT_BREAK_POINT", 0x54 },
+	{ "NMI_INTERRUPT_WDG_RXF_FULL", 0x5C },
+	{ "NMI_INTERRUPT_WDG_NO_RBD_RXF_FULL", 0x64 },
+	{ "NMI_INTERRUPT_HOST", 0x66 },
+	{ "NMI_INTERRUPT_ACTION_PT", 0x7C },
+	{ "NMI_INTERRUPT_UNKNOWN", 0x84 },
+	{ "NMI_INTERRUPT_INST_ACTION_PT", 0x86 },
+	{ "ADVANCED_SYSASSERT", 0 },
+};
+
+static const char *iwl4965_desc_lookup(u32 num)
+{
+	int i;
+	int max = ARRAY_SIZE(desc_lookup_text);
+
+	if (num < max)
+		return desc_lookup_text[num];
+
+	max = ARRAY_SIZE(advanced_lookup) - 1;
+	for (i = 0; i < max; i++) {
+		if (advanced_lookup[i].num == num)
+			break;
+	}
+	return advanced_lookup[i].name;
+}
+
+#define ERROR_START_OFFSET  (1 * sizeof(u32))
+#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
+
+void iwl4965_dump_nic_error_log(struct iwl_priv *priv)
+{
+	u32 data2, line;
+	u32 desc, time, count, base, data1;
+	u32 blink1, blink2, ilink1, ilink2;
+	u32 pc, hcmd;
+
+	if (priv->ucode_type == UCODE_INIT) {
+		base = le32_to_cpu(priv->card_alive_init.error_event_table_ptr);
+	} else {
+		base = le32_to_cpu(priv->card_alive.error_event_table_ptr);
+	}
+
+	if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+		IWL_ERR(priv,
+			"Not valid error log pointer 0x%08X for %s uCode\n",
+			base, (priv->ucode_type == UCODE_INIT) ? "Init" : "RT");
+		return;
+	}
+
+	count = iwl_legacy_read_targ_mem(priv, base);
+
+	if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
+		IWL_ERR(priv, "Start IWL Error Log Dump:\n");
+		IWL_ERR(priv, "Status: 0x%08lX, count: %d\n",
+			priv->status, count);
+	}
+
+	desc = iwl_legacy_read_targ_mem(priv, base + 1 * sizeof(u32));
+	priv->isr_stats.err_code = desc;
+	pc = iwl_legacy_read_targ_mem(priv, base + 2 * sizeof(u32));
+	blink1 = iwl_legacy_read_targ_mem(priv, base + 3 * sizeof(u32));
+	blink2 = iwl_legacy_read_targ_mem(priv, base + 4 * sizeof(u32));
+	ilink1 = iwl_legacy_read_targ_mem(priv, base + 5 * sizeof(u32));
+	ilink2 = iwl_legacy_read_targ_mem(priv, base + 6 * sizeof(u32));
+	data1 = iwl_legacy_read_targ_mem(priv, base + 7 * sizeof(u32));
+	data2 = iwl_legacy_read_targ_mem(priv, base + 8 * sizeof(u32));
+	line = iwl_legacy_read_targ_mem(priv, base + 9 * sizeof(u32));
+	time = iwl_legacy_read_targ_mem(priv, base + 11 * sizeof(u32));
+	hcmd = iwl_legacy_read_targ_mem(priv, base + 22 * sizeof(u32));
+
+	trace_iwlwifi_legacy_dev_ucode_error(priv, desc,
+					time, data1, data2, line,
+				      blink1, blink2, ilink1, ilink2);
+
+	IWL_ERR(priv, "Desc                                  Time       "
+		"data1      data2      line\n");
+	IWL_ERR(priv, "%-28s (0x%04X) %010u 0x%08X 0x%08X %u\n",
+		iwl4965_desc_lookup(desc), desc, time, data1, data2, line);
+	IWL_ERR(priv, "pc      blink1  blink2  ilink1  ilink2  hcmd\n");
+	IWL_ERR(priv, "0x%05X 0x%05X 0x%05X 0x%05X 0x%05X 0x%05X\n",
+		pc, blink1, blink2, ilink1, ilink2, hcmd);
+}
+
+#define EVENT_START_OFFSET  (4 * sizeof(u32))
+
+/**
+ * iwl4965_print_event_log - Dump error event log to syslog
+ *
+ */
+static int iwl4965_print_event_log(struct iwl_priv *priv, u32 start_idx,
+			       u32 num_events, u32 mode,
+			       int pos, char **buf, size_t bufsz)
+{
+	u32 i;
+	u32 base;       /* SRAM byte address of event log header */
+	u32 event_size; /* 2 u32s, or 3 u32s if timestamp recorded */
+	u32 ptr;        /* SRAM byte address of log data */
+	u32 ev, time, data; /* event log data */
+	unsigned long reg_flags;
+
+	if (num_events == 0)
+		return pos;
+
+	if (priv->ucode_type == UCODE_INIT) {
+		base = le32_to_cpu(priv->card_alive_init.log_event_table_ptr);
+	} else {
+		base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+	}
+
+	if (mode == 0)
+		event_size = 2 * sizeof(u32);
+	else
+		event_size = 3 * sizeof(u32);
+
+	ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
+
+	/* Make sure device is powered up for SRAM reads */
+	spin_lock_irqsave(&priv->reg_lock, reg_flags);
+	iwl_grab_nic_access(priv);
+
+	/* Set starting address; reads will auto-increment */
+	_iwl_legacy_write_direct32(priv, HBUS_TARG_MEM_RADDR, ptr);
+	rmb();
+
+	/* "time" is actually "data" for mode 0 (no timestamp).
+	* place event id # at far right for easier visual parsing. */
+	for (i = 0; i < num_events; i++) {
+		ev = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		time = _iwl_legacy_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		if (mode == 0) {
+			/* data, ev */
+			if (bufsz) {
+				pos += scnprintf(*buf + pos, bufsz - pos,
+						"EVT_LOG:0x%08x:%04u\n",
+						time, ev);
+			} else {
+				trace_iwlwifi_legacy_dev_ucode_event(priv, 0,
+					time, ev);
+				IWL_ERR(priv, "EVT_LOG:0x%08x:%04u\n",
+					time, ev);
+			}
+		} else {
+			data = _iwl_legacy_read_direct32(priv,
+						HBUS_TARG_MEM_RDAT);
+			if (bufsz) {
+				pos += scnprintf(*buf + pos, bufsz - pos,
+						"EVT_LOGT:%010u:0x%08x:%04u\n",
+						 time, data, ev);
+			} else {
+				IWL_ERR(priv, "EVT_LOGT:%010u:0x%08x:%04u\n",
+					time, data, ev);
+				trace_iwlwifi_legacy_dev_ucode_event(priv, time,
+					data, ev);
+			}
+		}
+	}
+
+	/* Allow device to power down */
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->reg_lock, reg_flags);
+	return pos;
+}
+
+/**
+ * iwl4965_print_last_event_logs - Dump the newest # of event log to syslog
+ */
+static int iwl4965_print_last_event_logs(struct iwl_priv *priv, u32 capacity,
+				    u32 num_wraps, u32 next_entry,
+				    u32 size, u32 mode,
+				    int pos, char **buf, size_t bufsz)
+{
+	/*
+	 * display the newest DEFAULT_LOG_ENTRIES entries
+	 * i.e the entries just before the next ont that uCode would fill.
+	 */
+	if (num_wraps) {
+		if (next_entry < size) {
+			pos = iwl4965_print_event_log(priv,
+						capacity - (size - next_entry),
+						size - next_entry, mode,
+						pos, buf, bufsz);
+			pos = iwl4965_print_event_log(priv, 0,
+						  next_entry, mode,
+						  pos, buf, bufsz);
+		} else
+			pos = iwl4965_print_event_log(priv, next_entry - size,
+						  size, mode, pos, buf, bufsz);
+	} else {
+		if (next_entry < size) {
+			pos = iwl4965_print_event_log(priv, 0, next_entry,
+						  mode, pos, buf, bufsz);
+		} else {
+			pos = iwl4965_print_event_log(priv, next_entry - size,
+						  size, mode, pos, buf, bufsz);
+		}
+	}
+	return pos;
+}
+
+#define DEFAULT_DUMP_EVENT_LOG_ENTRIES (20)
+
+int iwl4965_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
+			    char **buf, bool display)
+{
+	u32 base;       /* SRAM byte address of event log header */
+	u32 capacity;   /* event log capacity in # entries */
+	u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
+	u32 num_wraps;  /* # times uCode wrapped to top of log */
+	u32 next_entry; /* index of next entry to be written by uCode */
+	u32 size;       /* # entries that we'll print */
+	int pos = 0;
+	size_t bufsz = 0;
+
+	if (priv->ucode_type == UCODE_INIT) {
+		base = le32_to_cpu(priv->card_alive_init.log_event_table_ptr);
+	} else {
+		base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+	}
+
+	if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+		IWL_ERR(priv,
+			"Invalid event log pointer 0x%08X for %s uCode\n",
+			base, (priv->ucode_type == UCODE_INIT) ? "Init" : "RT");
+		return -EINVAL;
+	}
+
+	/* event log header */
+	capacity = iwl_legacy_read_targ_mem(priv, base);
+	mode = iwl_legacy_read_targ_mem(priv, base + (1 * sizeof(u32)));
+	num_wraps = iwl_legacy_read_targ_mem(priv, base + (2 * sizeof(u32)));
+	next_entry = iwl_legacy_read_targ_mem(priv, base + (3 * sizeof(u32)));
+
+	size = num_wraps ? capacity : next_entry;
+
+	/* bail out if nothing in log */
+	if (size == 0) {
+		IWL_ERR(priv, "Start IWL Event Log Dump: nothing in log\n");
+		return pos;
+	}
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (!(iwl_legacy_get_debug_level(priv) & IWL_DL_FW_ERRORS) && !full_log)
+		size = (size > DEFAULT_DUMP_EVENT_LOG_ENTRIES)
+			? DEFAULT_DUMP_EVENT_LOG_ENTRIES : size;
+#else
+	size = (size > DEFAULT_DUMP_EVENT_LOG_ENTRIES)
+		? DEFAULT_DUMP_EVENT_LOG_ENTRIES : size;
+#endif
+	IWL_ERR(priv, "Start IWL Event Log Dump: display last %u entries\n",
+		size);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+	if (display) {
+		if (full_log)
+			bufsz = capacity * 48;
+		else
+			bufsz = size * 48;
+		*buf = kmalloc(bufsz, GFP_KERNEL);
+		if (!*buf)
+			return -ENOMEM;
+	}
+	if ((iwl_legacy_get_debug_level(priv) & IWL_DL_FW_ERRORS) || full_log) {
+		/*
+		 * if uCode has wrapped back to top of log,
+		 * start at the oldest entry,
+		 * i.e the next one that uCode would fill.
+		 */
+		if (num_wraps)
+			pos = iwl4965_print_event_log(priv, next_entry,
+						capacity - next_entry, mode,
+						pos, buf, bufsz);
+		/* (then/else) start at top of log */
+		pos = iwl4965_print_event_log(priv, 0,
+					  next_entry, mode, pos, buf, bufsz);
+	} else
+		pos = iwl4965_print_last_event_logs(priv, capacity, num_wraps,
+						next_entry, size, mode,
+						pos, buf, bufsz);
+#else
+	pos = iwl4965_print_last_event_logs(priv, capacity, num_wraps,
+					next_entry, size, mode,
+					pos, buf, bufsz);
+#endif
+	return pos;
+}
+
+static void iwl4965_rf_kill_ct_config(struct iwl_priv *priv)
+{
+	struct iwl_ct_kill_config cmd;
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	cmd.critical_temperature_R =
+		cpu_to_le32(priv->hw_params.ct_kill_threshold);
+
+	ret = iwl_legacy_send_cmd_pdu(priv, REPLY_CT_KILL_CONFIG_CMD,
+			       sizeof(cmd), &cmd);
+	if (ret)
+		IWL_ERR(priv, "REPLY_CT_KILL_CONFIG_CMD failed\n");
+	else
+		IWL_DEBUG_INFO(priv, "REPLY_CT_KILL_CONFIG_CMD "
+				"succeeded, "
+				"critical temperature is %d\n",
+				priv->hw_params.ct_kill_threshold);
+}
+
+static const s8 default_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+	IWL49_CMD_FIFO_NUM,
+	IWL_TX_FIFO_UNUSED,
+	IWL_TX_FIFO_UNUSED,
+};
+
+static int iwl4965_alive_notify(struct iwl_priv *priv)
+{
+	u32 a;
+	unsigned long flags;
+	int i, chan;
+	u32 reg_val;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Clear 4965's internal Tx Scheduler data base */
+	priv->scd_base_addr = iwl_legacy_read_prph(priv,
+					IWL49_SCD_SRAM_BASE_ADDR);
+	a = priv->scd_base_addr + IWL49_SCD_CONTEXT_DATA_OFFSET;
+	for (; a < priv->scd_base_addr + IWL49_SCD_TX_STTS_BITMAP_OFFSET; a += 4)
+		iwl_legacy_write_targ_mem(priv, a, 0);
+	for (; a < priv->scd_base_addr + IWL49_SCD_TRANSLATE_TBL_OFFSET; a += 4)
+		iwl_legacy_write_targ_mem(priv, a, 0);
+	for (; a < priv->scd_base_addr +
+	       IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(priv->hw_params.max_txq_num); a += 4)
+		iwl_legacy_write_targ_mem(priv, a, 0);
+
+	/* Tel 4965 where to find Tx byte count tables */
+	iwl_legacy_write_prph(priv, IWL49_SCD_DRAM_BASE_ADDR,
+			priv->scd_bc_tbls.dma >> 10);
+
+	/* Enable DMA channel */
+	for (chan = 0; chan < FH49_TCSR_CHNL_NUM ; chan++)
+		iwl_legacy_write_direct32(priv,
+				FH_TCSR_CHNL_TX_CONFIG_REG(chan),
+				FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
+				FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE);
+
+	/* Update FH chicken bits */
+	reg_val = iwl_legacy_read_direct32(priv, FH_TX_CHICKEN_BITS_REG);
+	iwl_legacy_write_direct32(priv, FH_TX_CHICKEN_BITS_REG,
+			   reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
+
+	/* Disable chain mode for all queues */
+	iwl_legacy_write_prph(priv, IWL49_SCD_QUEUECHAIN_SEL, 0);
+
+	/* Initialize each Tx queue (including the command queue) */
+	for (i = 0; i < priv->hw_params.max_txq_num; i++) {
+
+		/* TFD circular buffer read/write indexes */
+		iwl_legacy_write_prph(priv, IWL49_SCD_QUEUE_RDPTR(i), 0);
+		iwl_legacy_write_direct32(priv, HBUS_TARG_WRPTR, 0 | (i << 8));
+
+		/* Max Tx Window size for Scheduler-ACK mode */
+		iwl_legacy_write_targ_mem(priv, priv->scd_base_addr +
+				IWL49_SCD_CONTEXT_QUEUE_OFFSET(i),
+				(SCD_WIN_SIZE <<
+				IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
+				IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
+
+		/* Frame limit */
+		iwl_legacy_write_targ_mem(priv, priv->scd_base_addr +
+				IWL49_SCD_CONTEXT_QUEUE_OFFSET(i) +
+				sizeof(u32),
+				(SCD_FRAME_LIMIT <<
+				IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+				IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
+
+	}
+	iwl_legacy_write_prph(priv, IWL49_SCD_INTERRUPT_MASK,
+				 (1 << priv->hw_params.max_txq_num) - 1);
+
+	/* Activate all Tx DMA/FIFO channels */
+	iwl4965_txq_set_sched(priv, IWL_MASK(0, 6));
+
+	iwl4965_set_wr_ptrs(priv, IWL_DEFAULT_CMD_QUEUE_NUM, 0);
+
+	/* make sure all queue are not stopped */
+	memset(&priv->queue_stopped[0], 0, sizeof(priv->queue_stopped));
+	for (i = 0; i < 4; i++)
+		atomic_set(&priv->queue_stop_count[i], 0);
+
+	/* reset to 0 to enable all the queue first */
+	priv->txq_ctx_active_msk = 0;
+	/* Map each Tx/cmd queue to its corresponding fifo */
+	BUILD_BUG_ON(ARRAY_SIZE(default_queue_to_tx_fifo) != 7);
+
+	for (i = 0; i < ARRAY_SIZE(default_queue_to_tx_fifo); i++) {
+		int ac = default_queue_to_tx_fifo[i];
+
+		iwl_txq_ctx_activate(priv, i);
+
+		if (ac == IWL_TX_FIFO_UNUSED)
+			continue;
+
+		iwl4965_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/**
+ * iwl4965_alive_start - called after REPLY_ALIVE notification received
+ *                   from protocol/runtime uCode (initialization uCode's
+ *                   Alive gets handled by iwl_init_alive_start()).
+ */
+static void iwl4965_alive_start(struct iwl_priv *priv)
+{
+	int ret = 0;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+
+	IWL_DEBUG_INFO(priv, "Runtime Alive received.\n");
+
+	if (priv->card_alive.is_valid != UCODE_VALID_OK) {
+		/* We had an error bringing up the hardware, so take it
+		 * all the way back down so we can try again */
+		IWL_DEBUG_INFO(priv, "Alive failed.\n");
+		goto restart;
+	}
+
+	/* Initialize uCode has loaded Runtime uCode ... verify inst image.
+	 * This is a paranoid check, because we would not have gotten the
+	 * "runtime" alive if code weren't properly loaded.  */
+	if (iwl4965_verify_ucode(priv)) {
+		/* Runtime instruction load was bad;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO(priv, "Bad runtime uCode load.\n");
+		goto restart;
+	}
+
+	ret = iwl4965_alive_notify(priv);
+	if (ret) {
+		IWL_WARN(priv,
+			"Could not complete ALIVE transition [ntf]: %d\n", ret);
+		goto restart;
+	}
+
+
+	/* After the ALIVE response, we can send host commands to the uCode */
+	set_bit(STATUS_ALIVE, &priv->status);
+
+	/* Enable watchdog to monitor the driver tx queues */
+	iwl_legacy_setup_watchdog(priv);
+
+	if (iwl_legacy_is_rfkill(priv))
+		return;
+
+	ieee80211_wake_queues(priv->hw);
+
+	priv->active_rate = IWL_RATES_MASK;
+
+	if (iwl_legacy_is_associated_ctx(ctx)) {
+		struct iwl_legacy_rxon_cmd *active_rxon =
+				(struct iwl_legacy_rxon_cmd *)&ctx->active;
+		/* apply any changes in staging */
+		ctx->staging.filter_flags |= RXON_FILTER_ASSOC_MSK;
+		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	} else {
+		struct iwl_rxon_context *tmp;
+		/* Initialize our rx_config data */
+		for_each_context(priv, tmp)
+			iwl_legacy_connection_init_rx_config(priv, tmp);
+
+		if (priv->cfg->ops->hcmd->set_rxon_chain)
+			priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
+	}
+
+	/* Configure bluetooth coexistence if enabled */
+	iwl_legacy_send_bt_config(priv);
+
+	iwl4965_reset_run_time_calib(priv);
+
+	set_bit(STATUS_READY, &priv->status);
+
+	/* Configure the adapter for unassociated operation */
+	iwl_legacy_commit_rxon(priv, ctx);
+
+	/* At this point, the NIC is initialized and operational */
+	iwl4965_rf_kill_ct_config(priv);
+
+	IWL_DEBUG_INFO(priv, "ALIVE processing complete.\n");
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	iwl_legacy_power_update_mode(priv, true);
+	IWL_DEBUG_INFO(priv, "Updated power mode\n");
+
+	return;
+
+ restart:
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+static void iwl4965_cancel_deferred_work(struct iwl_priv *priv);
+
+static void __iwl4965_down(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	int exit_pending = test_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	IWL_DEBUG_INFO(priv, DRV_NAME " is going down\n");
+
+	iwl_legacy_scan_cancel_timeout(priv, 200);
+
+	exit_pending = test_and_set_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	/* Stop TX queues watchdog. We need to have STATUS_EXIT_PENDING bit set
+	 * to prevent rearm timer */
+	del_timer_sync(&priv->watchdog);
+
+	iwl_legacy_clear_ucode_stations(priv, NULL);
+	iwl_legacy_dealloc_bcast_stations(priv);
+	iwl_legacy_clear_driver_stations(priv);
+
+	/* Unblock any waiting calls */
+	wake_up_interruptible_all(&priv->wait_command_queue);
+
+	/* Wipe out the EXIT_PENDING status bit if we are not actually
+	 * exiting the module */
+	if (!exit_pending)
+		clear_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	/* stop and reset the on-board processor */
+	iwl_write32(priv, CSR_RESET, CSR_RESET_REG_FLAG_NEVO_RESET);
+
+	/* tell the device to stop sending interrupts */
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_legacy_disable_interrupts(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+	iwl4965_synchronize_irq(priv);
+
+	if (priv->mac80211_registered)
+		ieee80211_stop_queues(priv->hw);
+
+	/* If we have not previously called iwl_init() then
+	 * clear all bits but the RF Kill bit and return */
+	if (!iwl_legacy_is_init(priv)) {
+		priv->status = test_bit(STATUS_RF_KILL_HW, &priv->status) <<
+					STATUS_RF_KILL_HW |
+			       test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
+					STATUS_GEO_CONFIGURED |
+			       test_bit(STATUS_EXIT_PENDING, &priv->status) <<
+					STATUS_EXIT_PENDING;
+		goto exit;
+	}
+
+	/* ...otherwise clear out all the status bits but the RF Kill
+	 * bit and continue taking the NIC down. */
+	priv->status &= test_bit(STATUS_RF_KILL_HW, &priv->status) <<
+				STATUS_RF_KILL_HW |
+			test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
+				STATUS_GEO_CONFIGURED |
+			test_bit(STATUS_FW_ERROR, &priv->status) <<
+				STATUS_FW_ERROR |
+		       test_bit(STATUS_EXIT_PENDING, &priv->status) <<
+				STATUS_EXIT_PENDING;
+
+	iwl4965_txq_ctx_stop(priv);
+	iwl4965_rxq_stop(priv);
+
+	/* Power-down device's busmaster DMA clocks */
+	iwl_legacy_write_prph(priv, APMG_CLK_DIS_REG, APMG_CLK_VAL_DMA_CLK_RQT);
+	udelay(5);
+
+	/* Make sure (redundant) we've released our request to stay awake */
+	iwl_legacy_clear_bit(priv, CSR_GP_CNTRL,
+				CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+
+	/* Stop the device, and put it in low power state */
+	iwl_legacy_apm_stop(priv);
+
+ exit:
+	memset(&priv->card_alive, 0, sizeof(struct iwl_alive_resp));
+
+	dev_kfree_skb(priv->beacon_skb);
+	priv->beacon_skb = NULL;
+
+	/* clear out any free frames */
+	iwl4965_clear_free_frames(priv);
+}
+
+static void iwl4965_down(struct iwl_priv *priv)
+{
+	mutex_lock(&priv->mutex);
+	__iwl4965_down(priv);
+	mutex_unlock(&priv->mutex);
+
+	iwl4965_cancel_deferred_work(priv);
+}
+
+#define HW_READY_TIMEOUT (50)
+
+static int iwl4965_set_hw_ready(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		CSR_HW_IF_CONFIG_REG_BIT_NIC_READY);
+
+	/* See if we got it */
+	ret = iwl_poll_bit(priv, CSR_HW_IF_CONFIG_REG,
+				CSR_HW_IF_CONFIG_REG_BIT_NIC_READY,
+				CSR_HW_IF_CONFIG_REG_BIT_NIC_READY,
+				HW_READY_TIMEOUT);
+	if (ret != -ETIMEDOUT)
+		priv->hw_ready = true;
+	else
+		priv->hw_ready = false;
+
+	IWL_DEBUG_INFO(priv, "hardware %s\n",
+		      (priv->hw_ready == 1) ? "ready" : "not ready");
+	return ret;
+}
+
+static int iwl4965_prepare_card_hw(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	IWL_DEBUG_INFO(priv, "iwl4965_prepare_card_hw enter\n");
+
+	ret = iwl4965_set_hw_ready(priv);
+	if (priv->hw_ready)
+		return ret;
+
+	/* If HW is not ready, prepare the conditions to check again */
+	iwl_legacy_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			CSR_HW_IF_CONFIG_REG_PREPARE);
+
+	ret = iwl_poll_bit(priv, CSR_HW_IF_CONFIG_REG,
+			~CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE,
+			CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE, 150000);
+
+	/* HW should be ready by now, check again. */
+	if (ret != -ETIMEDOUT)
+		iwl4965_set_hw_ready(priv);
+
+	return ret;
+}
+
+#define MAX_HW_RESTARTS 5
+
+static int __iwl4965_up(struct iwl_priv *priv)
+{
+	struct iwl_rxon_context *ctx;
+	int i;
+	int ret;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_WARN(priv, "Exit pending; will not bring the NIC up\n");
+		return -EIO;
+	}
+
+	if (!priv->ucode_data_backup.v_addr || !priv->ucode_data.v_addr) {
+		IWL_ERR(priv, "ucode not available for device bringup\n");
+		return -EIO;
+	}
+
+	for_each_context(priv, ctx) {
+		ret = iwl4965_alloc_bcast_station(priv, ctx);
+		if (ret) {
+			iwl_legacy_dealloc_bcast_stations(priv);
+			return ret;
+		}
+	}
+
+	iwl4965_prepare_card_hw(priv);
+
+	if (!priv->hw_ready) {
+		IWL_WARN(priv, "Exit HW not ready\n");
+		return -EIO;
+	}
+
+	/* If platform's RF_KILL switch is NOT set to KILL */
+	if (iwl_read32(priv,
+		CSR_GP_CNTRL) & CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW)
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+
+	if (iwl_legacy_is_rfkill(priv)) {
+		wiphy_rfkill_set_hw_state(priv->hw->wiphy, true);
+
+		iwl_legacy_enable_interrupts(priv);
+		IWL_WARN(priv, "Radio disabled by HW RF Kill switch\n");
+		return 0;
+	}
+
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+
+	/* must be initialised before iwl_hw_nic_init */
+	priv->cmd_queue = IWL_DEFAULT_CMD_QUEUE_NUM;
+
+	ret = iwl4965_hw_nic_init(priv);
+	if (ret) {
+		IWL_ERR(priv, "Unable to init nic\n");
+		return ret;
+	}
+
+	/* make sure rfkill handshake bits are cleared */
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+	/* clear (again), then enable host interrupts */
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+	iwl_legacy_enable_interrupts(priv);
+
+	/* really make sure rfkill handshake bits are cleared */
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* Copy original ucode data image from disk into backup cache.
+	 * This will be used to initialize the on-board processor's
+	 * data SRAM for a clean start when the runtime program first loads. */
+	memcpy(priv->ucode_data_backup.v_addr, priv->ucode_data.v_addr,
+	       priv->ucode_data.len);
+
+	for (i = 0; i < MAX_HW_RESTARTS; i++) {
+
+		/* load bootstrap state machine,
+		 * load bootstrap program into processor's memory,
+		 * prepare to load the "initialize" uCode */
+		ret = priv->cfg->ops->lib->load_ucode(priv);
+
+		if (ret) {
+			IWL_ERR(priv, "Unable to set up bootstrap uCode: %d\n",
+				ret);
+			continue;
+		}
+
+		/* start card; "initialize" will load runtime ucode */
+		iwl4965_nic_start(priv);
+
+		IWL_DEBUG_INFO(priv, DRV_NAME " is coming up\n");
+
+		return 0;
+	}
+
+	set_bit(STATUS_EXIT_PENDING, &priv->status);
+	__iwl4965_down(priv);
+	clear_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	/* tried to restart and config the device for as long as our
+	 * patience could withstand */
+	IWL_ERR(priv, "Unable to initialize device after %d attempts.\n", i);
+	return -EIO;
+}
+
+
+/*****************************************************************************
+ *
+ * Workqueue callbacks
+ *
+ *****************************************************************************/
+
+static void iwl4965_bg_init_alive_start(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, init_alive_start.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	priv->cfg->ops->lib->init_alive_start(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl4965_bg_alive_start(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, alive_start.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl4965_alive_start(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl4965_bg_run_time_calib_work(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+			run_time_calib_work);
+
+	mutex_lock(&priv->mutex);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
+	    test_bit(STATUS_SCANNING, &priv->status)) {
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	if (priv->start_calib) {
+		iwl4965_chain_noise_calibration(priv,
+				(void *)&priv->_4965.statistics);
+		iwl4965_sensitivity_calibration(priv,
+				(void *)&priv->_4965.statistics);
+	}
+
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl4965_bg_restart(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv, restart);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (test_and_clear_bit(STATUS_FW_ERROR, &priv->status)) {
+		struct iwl_rxon_context *ctx;
+
+		mutex_lock(&priv->mutex);
+		for_each_context(priv, ctx)
+			ctx->vif = NULL;
+		priv->is_open = 0;
+
+		__iwl4965_down(priv);
+
+		mutex_unlock(&priv->mutex);
+		iwl4965_cancel_deferred_work(priv);
+		ieee80211_restart_hw(priv->hw);
+	} else {
+		iwl4965_down(priv);
+
+		if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+			return;
+
+		mutex_lock(&priv->mutex);
+		__iwl4965_up(priv);
+		mutex_unlock(&priv->mutex);
+	}
+}
+
+static void iwl4965_bg_rx_replenish(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, rx_replenish);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl4965_rx_replenish(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+/*****************************************************************************
+ *
+ * mac80211 entry point functions
+ *
+ *****************************************************************************/
+
+#define UCODE_READY_TIMEOUT	(4 * HZ)
+
+/*
+ * Not a mac80211 entry point function, but it fits in with all the
+ * other mac80211 functions grouped here.
+ */
+static int iwl4965_mac_setup_register(struct iwl_priv *priv,
+				  u32 max_probe_length)
+{
+	int ret;
+	struct ieee80211_hw *hw = priv->hw;
+	struct iwl_rxon_context *ctx;
+
+	hw->rate_control_algorithm = "iwl-4965-rs";
+
+	/* Tell mac80211 our characteristics */
+	hw->flags = IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_AMPDU_AGGREGATION |
+		    IEEE80211_HW_NEED_DTIM_PERIOD |
+		    IEEE80211_HW_SPECTRUM_MGMT |
+		    IEEE80211_HW_REPORTS_TX_ACK_STATUS;
+
+	if (priv->cfg->sku & IWL_SKU_N)
+		hw->flags |= IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
+			     IEEE80211_HW_SUPPORTS_STATIC_SMPS;
+
+	hw->sta_data_size = sizeof(struct iwl_station_priv);
+	hw->vif_data_size = sizeof(struct iwl_vif_priv);
+
+	for_each_context(priv, ctx) {
+		hw->wiphy->interface_modes |= ctx->interface_modes;
+		hw->wiphy->interface_modes |= ctx->exclusive_interface_modes;
+	}
+
+	hw->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
+			    WIPHY_FLAG_DISABLE_BEACON_HINTS;
+
+	/*
+	 * For now, disable PS by default because it affects
+	 * RX performance significantly.
+	 */
+	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+
+	hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX;
+	/* we create the 802.11 header and a zero-length SSID element */
+	hw->wiphy->max_scan_ie_len = max_probe_length - 24 - 2;
+
+	/* Default value; 4 EDCA QOS priorities */
+	hw->queues = 4;
+
+	hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
+
+	if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
+		priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
+			&priv->bands[IEEE80211_BAND_2GHZ];
+	if (priv->bands[IEEE80211_BAND_5GHZ].n_channels)
+		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
+			&priv->bands[IEEE80211_BAND_5GHZ];
+
+	iwl_legacy_leds_init(priv);
+
+	ret = ieee80211_register_hw(priv->hw);
+	if (ret) {
+		IWL_ERR(priv, "Failed to register hw (error %d)\n", ret);
+		return ret;
+	}
+	priv->mac80211_registered = 1;
+
+	return 0;
+}
+
+
+int iwl4965_mac_start(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+	int ret;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	/* we should be verifying the device is ready to be opened */
+	mutex_lock(&priv->mutex);
+	ret = __iwl4965_up(priv);
+	mutex_unlock(&priv->mutex);
+
+	if (ret)
+		return ret;
+
+	if (iwl_legacy_is_rfkill(priv))
+		goto out;
+
+	IWL_DEBUG_INFO(priv, "Start UP work done.\n");
+
+	/* Wait for START_ALIVE from Run Time ucode. Otherwise callbacks from
+	 * mac80211 will not be run successfully. */
+	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+			test_bit(STATUS_READY, &priv->status),
+			UCODE_READY_TIMEOUT);
+	if (!ret) {
+		if (!test_bit(STATUS_READY, &priv->status)) {
+			IWL_ERR(priv, "START_ALIVE timeout after %dms.\n",
+				jiffies_to_msecs(UCODE_READY_TIMEOUT));
+			return -ETIMEDOUT;
+		}
+	}
+
+	iwl4965_led_enable(priv);
+
+out:
+	priv->is_open = 1;
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+	return 0;
+}
+
+void iwl4965_mac_stop(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	if (!priv->is_open)
+		return;
+
+	priv->is_open = 0;
+
+	iwl4965_down(priv);
+
+	flush_workqueue(priv->workqueue);
+
+	/* enable interrupts again in order to receive rfkill changes */
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+	iwl_legacy_enable_interrupts(priv);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+}
+
+void iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MACDUMP(priv, "enter\n");
+
+	IWL_DEBUG_TX(priv, "dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
+		     ieee80211_get_tx_rate(hw, IEEE80211_SKB_CB(skb))->bitrate);
+
+	if (iwl4965_tx_skb(priv, skb))
+		dev_kfree_skb_any(skb);
+
+	IWL_DEBUG_MACDUMP(priv, "leave\n");
+}
+
+void iwl4965_mac_update_tkip_key(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_key_conf *keyconf,
+				struct ieee80211_sta *sta,
+				u32 iv32, u16 *phase1key)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	iwl4965_update_tkip_key(priv, vif_priv->ctx, keyconf, sta,
+			    iv32, phase1key);
+
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+}
+
+int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
+	struct iwl_rxon_context *ctx = vif_priv->ctx;
+	int ret;
+	u8 sta_id;
+	bool is_default_wep_key = false;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	if (priv->cfg->mod_params->sw_crypto) {
+		IWL_DEBUG_MAC80211(priv, "leave - hwcrypto disabled\n");
+		return -EOPNOTSUPP;
+	}
+
+	sta_id = iwl_legacy_sta_id_or_broadcast(priv, vif_priv->ctx, sta);
+	if (sta_id == IWL_INVALID_STATION)
+		return -EINVAL;
+
+	mutex_lock(&priv->mutex);
+	iwl_legacy_scan_cancel_timeout(priv, 100);
+
+	/*
+	 * If we are getting WEP group key and we didn't receive any key mapping
+	 * so far, we are in legacy wep mode (group key only), otherwise we are
+	 * in 1X mode.
+	 * In legacy wep mode, we use another host command to the uCode.
+	 */
+	if ((key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
+	     key->cipher == WLAN_CIPHER_SUITE_WEP104) &&
+	    !sta) {
+		if (cmd == SET_KEY)
+			is_default_wep_key = !ctx->key_mapping_keys;
+		else
+			is_default_wep_key =
+					(key->hw_key_idx == HW_KEY_DEFAULT);
+	}
+
+	switch (cmd) {
+	case SET_KEY:
+		if (is_default_wep_key)
+			ret = iwl4965_set_default_wep_key(priv,
+							vif_priv->ctx, key);
+		else
+			ret = iwl4965_set_dynamic_key(priv, vif_priv->ctx,
+						  key, sta_id);
+
+		IWL_DEBUG_MAC80211(priv, "enable hwcrypto key\n");
+		break;
+	case DISABLE_KEY:
+		if (is_default_wep_key)
+			ret = iwl4965_remove_default_wep_key(priv, ctx, key);
+		else
+			ret = iwl4965_remove_dynamic_key(priv, ctx,
+							key, sta_id);
+
+		IWL_DEBUG_MAC80211(priv, "disable hwcrypto key\n");
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&priv->mutex);
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+
+	return ret;
+}
+
+int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
+			    struct ieee80211_vif *vif,
+			    enum ieee80211_ampdu_mlme_action action,
+			    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			    u8 buf_size)
+{
+	struct iwl_priv *priv = hw->priv;
+	int ret = -EINVAL;
+
+	IWL_DEBUG_HT(priv, "A-MPDU action on addr %pM tid %d\n",
+		     sta->addr, tid);
+
+	if (!(priv->cfg->sku & IWL_SKU_N))
+		return -EACCES;
+
+	mutex_lock(&priv->mutex);
+
+	switch (action) {
+	case IEEE80211_AMPDU_RX_START:
+		IWL_DEBUG_HT(priv, "start Rx\n");
+		ret = iwl4965_sta_rx_agg_start(priv, sta, tid, *ssn);
+		break;
+	case IEEE80211_AMPDU_RX_STOP:
+		IWL_DEBUG_HT(priv, "stop Rx\n");
+		ret = iwl4965_sta_rx_agg_stop(priv, sta, tid);
+		if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+			ret = 0;
+		break;
+	case IEEE80211_AMPDU_TX_START:
+		IWL_DEBUG_HT(priv, "start Tx\n");
+		ret = iwl4965_tx_agg_start(priv, vif, sta, tid, ssn);
+		if (ret == 0) {
+			priv->_4965.agg_tids_count++;
+			IWL_DEBUG_HT(priv, "priv->_4965.agg_tids_count = %u\n",
+				     priv->_4965.agg_tids_count);
+		}
+		break;
+	case IEEE80211_AMPDU_TX_STOP:
+		IWL_DEBUG_HT(priv, "stop Tx\n");
+		ret = iwl4965_tx_agg_stop(priv, vif, sta, tid);
+		if ((ret == 0) && (priv->_4965.agg_tids_count > 0)) {
+			priv->_4965.agg_tids_count--;
+			IWL_DEBUG_HT(priv, "priv->_4965.agg_tids_count = %u\n",
+				     priv->_4965.agg_tids_count);
+		}
+		if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+			ret = 0;
+		break;
+	case IEEE80211_AMPDU_TX_OPERATIONAL:
+		ret = 0;
+		break;
+	}
+	mutex_unlock(&priv->mutex);
+
+	return ret;
+}
+
+int iwl4965_mac_sta_add(struct ieee80211_hw *hw,
+		       struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
+	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
+	bool is_ap = vif->type == NL80211_IFTYPE_STATION;
+	int ret;
+	u8 sta_id;
+
+	IWL_DEBUG_INFO(priv, "received request to add station %pM\n",
+			sta->addr);
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_INFO(priv, "proceeding to add station %pM\n",
+			sta->addr);
+	sta_priv->common.sta_id = IWL_INVALID_STATION;
+
+	atomic_set(&sta_priv->pending_frames, 0);
+
+	ret = iwl_legacy_add_station_common(priv, vif_priv->ctx, sta->addr,
+				     is_ap, sta, &sta_id);
+	if (ret) {
+		IWL_ERR(priv, "Unable to add station %pM (%d)\n",
+			sta->addr, ret);
+		/* Should we return success if return code is EEXIST ? */
+		mutex_unlock(&priv->mutex);
+		return ret;
+	}
+
+	sta_priv->common.sta_id = sta_id;
+
+	/* Initialize rate scaling */
+	IWL_DEBUG_INFO(priv, "Initializing rate scaling for station %pM\n",
+		       sta->addr);
+	iwl4965_rs_rate_init(priv, sta, sta_id);
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+void iwl4965_mac_channel_switch(struct ieee80211_hw *hw,
+			       struct ieee80211_channel_switch *ch_switch)
+{
+	struct iwl_priv *priv = hw->priv;
+	const struct iwl_channel_info *ch_info;
+	struct ieee80211_conf *conf = &hw->conf;
+	struct ieee80211_channel *channel = ch_switch->channel;
+	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
+
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	u16 ch;
+	unsigned long flags = 0;
+
+	IWL_DEBUG_MAC80211(priv, "enter\n");
+
+	if (iwl_legacy_is_rfkill(priv))
+		goto out_exit;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
+	    test_bit(STATUS_SCANNING, &priv->status))
+		goto out_exit;
+
+	if (!iwl_legacy_is_associated_ctx(ctx))
+		goto out_exit;
+
+	/* channel switch in progress */
+	if (priv->switch_rxon.switch_in_progress == true)
+		goto out_exit;
+
+	mutex_lock(&priv->mutex);
+	if (priv->cfg->ops->lib->set_channel_switch) {
+
+		ch = channel->hw_value;
+		if (le16_to_cpu(ctx->active.channel) != ch) {
+			ch_info = iwl_legacy_get_channel_info(priv,
+						       channel->band,
+						       ch);
+			if (!iwl_legacy_is_channel_valid(ch_info)) {
+				IWL_DEBUG_MAC80211(priv, "invalid channel\n");
+				goto out;
+			}
+			spin_lock_irqsave(&priv->lock, flags);
+
+			priv->current_ht_config.smps = conf->smps_mode;
+
+			/* Configure HT40 channels */
+			ctx->ht.enabled = conf_is_ht(conf);
+			if (ctx->ht.enabled) {
+				if (conf_is_ht40_minus(conf)) {
+					ctx->ht.extension_chan_offset =
+					IEEE80211_HT_PARAM_CHA_SEC_BELOW;
+					ctx->ht.is_40mhz = true;
+				} else if (conf_is_ht40_plus(conf)) {
+					ctx->ht.extension_chan_offset =
+					IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
+					ctx->ht.is_40mhz = true;
+				} else {
+					ctx->ht.extension_chan_offset =
+						IEEE80211_HT_PARAM_CHA_SEC_NONE;
+					ctx->ht.is_40mhz = false;
+				}
+			} else
+				ctx->ht.is_40mhz = false;
+
+			if ((le16_to_cpu(ctx->staging.channel) != ch))
+				ctx->staging.flags = 0;
+
+			iwl_legacy_set_rxon_channel(priv, channel, ctx);
+			iwl_legacy_set_rxon_ht(priv, ht_conf);
+			iwl_legacy_set_flags_for_band(priv, ctx, channel->band,
+					       ctx->vif);
+			spin_unlock_irqrestore(&priv->lock, flags);
+
+			iwl_legacy_set_rate(priv);
+			/*
+			 * at this point, staging_rxon has the
+			 * configuration for channel switch
+			 */
+			if (priv->cfg->ops->lib->set_channel_switch(priv,
+								    ch_switch))
+				priv->switch_rxon.switch_in_progress = false;
+		}
+	}
+out:
+	mutex_unlock(&priv->mutex);
+out_exit:
+	if (!priv->switch_rxon.switch_in_progress)
+		ieee80211_chswitch_done(ctx->vif, false);
+	IWL_DEBUG_MAC80211(priv, "leave\n");
+}
+
+void iwl4965_configure_filter(struct ieee80211_hw *hw,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast)
+{
+	struct iwl_priv *priv = hw->priv;
+	__le32 filter_or = 0, filter_nand = 0;
+	struct iwl_rxon_context *ctx;
+
+#define CHK(test, flag)	do { \
+	if (*total_flags & (test))		\
+		filter_or |= (flag);		\
+	else					\
+		filter_nand |= (flag);		\
+	} while (0)
+
+	IWL_DEBUG_MAC80211(priv, "Enter: changed: 0x%x, total: 0x%x\n",
+			changed_flags, *total_flags);
+
+	CHK(FIF_OTHER_BSS | FIF_PROMISC_IN_BSS, RXON_FILTER_PROMISC_MSK);
+	/* Setting _just_ RXON_FILTER_CTL2HOST_MSK causes FH errors */
+	CHK(FIF_CONTROL, RXON_FILTER_CTL2HOST_MSK | RXON_FILTER_PROMISC_MSK);
+	CHK(FIF_BCN_PRBRESP_PROMISC, RXON_FILTER_BCON_AWARE_MSK);
+
+#undef CHK
+
+	mutex_lock(&priv->mutex);
+
+	for_each_context(priv, ctx) {
+		ctx->staging.filter_flags &= ~filter_nand;
+		ctx->staging.filter_flags |= filter_or;
+
+		/*
+		 * Not committing directly because hardware can perform a scan,
+		 * but we'll eventually commit the filter flags change anyway.
+		 */
+	}
+
+	mutex_unlock(&priv->mutex);
+
+	/*
+	 * Receiving all multicast frames is always enabled by the
+	 * default flags setup in iwl_legacy_connection_init_rx_config()
+	 * since we currently do not support programming multicast
+	 * filters into the device.
+	 */
+	*total_flags &= FIF_OTHER_BSS | FIF_ALLMULTI | FIF_PROMISC_IN_BSS |
+			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
+}
+
+/*****************************************************************************
+ *
+ * driver setup and teardown
+ *
+ *****************************************************************************/
+
+static void iwl4965_bg_txpower_work(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+			txpower_work);
+
+	/* If a scan happened to start before we got here
+	 * then just return; the statistics notification will
+	 * kick off another scheduled work to compensate for
+	 * any temperature delta we missed here. */
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
+	    test_bit(STATUS_SCANNING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	/* Regardless of if we are associated, we must reconfigure the
+	 * TX power since frames can be sent on non-radar channels while
+	 * not associated */
+	priv->cfg->ops->lib->send_tx_power(priv);
+
+	/* Update last_temperature to keep is_calib_needed from running
+	 * when it isn't needed... */
+	priv->last_temperature = priv->temperature;
+
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
+{
+	priv->workqueue = create_singlethread_workqueue(DRV_NAME);
+
+	init_waitqueue_head(&priv->wait_command_queue);
+
+	INIT_WORK(&priv->restart, iwl4965_bg_restart);
+	INIT_WORK(&priv->rx_replenish, iwl4965_bg_rx_replenish);
+	INIT_WORK(&priv->run_time_calib_work, iwl4965_bg_run_time_calib_work);
+	INIT_DELAYED_WORK(&priv->init_alive_start, iwl4965_bg_init_alive_start);
+	INIT_DELAYED_WORK(&priv->alive_start, iwl4965_bg_alive_start);
+
+	iwl_legacy_setup_scan_deferred_work(priv);
+
+	INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
+
+	init_timer(&priv->statistics_periodic);
+	priv->statistics_periodic.data = (unsigned long)priv;
+	priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
+
+	init_timer(&priv->ucode_trace);
+	priv->ucode_trace.data = (unsigned long)priv;
+	priv->ucode_trace.function = iwl4965_bg_ucode_trace;
+
+	init_timer(&priv->watchdog);
+	priv->watchdog.data = (unsigned long)priv;
+	priv->watchdog.function = iwl_legacy_bg_watchdog;
+
+	tasklet_init(&priv->irq_tasklet, (void (*)(unsigned long))
+		iwl4965_irq_tasklet, (unsigned long)priv);
+}
+
+static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
+{
+	cancel_work_sync(&priv->txpower_work);
+	cancel_delayed_work_sync(&priv->init_alive_start);
+	cancel_delayed_work(&priv->alive_start);
+	cancel_work_sync(&priv->run_time_calib_work);
+
+	iwl_legacy_cancel_scan_deferred_work(priv);
+
+	del_timer_sync(&priv->statistics_periodic);
+	del_timer_sync(&priv->ucode_trace);
+}
+
+static void iwl4965_init_hw_rates(struct iwl_priv *priv,
+			      struct ieee80211_rate *rates)
+{
+	int i;
+
+	for (i = 0; i < IWL_RATE_COUNT_LEGACY; i++) {
+		rates[i].bitrate = iwlegacy_rates[i].ieee * 5;
+		rates[i].hw_value = i; /* Rate scaling will work on indexes */
+		rates[i].hw_value_short = i;
+		rates[i].flags = 0;
+		if ((i >= IWL_FIRST_CCK_RATE) && (i <= IWL_LAST_CCK_RATE)) {
+			/*
+			 * If CCK != 1M then set short preamble rate flag.
+			 */
+			rates[i].flags |=
+				(iwlegacy_rates[i].plcp == IWL_RATE_1M_PLCP) ?
+					0 : IEEE80211_RATE_SHORT_PREAMBLE;
+		}
+	}
+}
+/*
+ * Acquire priv->lock before calling this function !
+ */
+void iwl4965_set_wr_ptrs(struct iwl_priv *priv, int txq_id, u32 index)
+{
+	iwl_legacy_write_direct32(priv, HBUS_TARG_WRPTR,
+			     (index & 0xff) | (txq_id << 8));
+	iwl_legacy_write_prph(priv, IWL49_SCD_QUEUE_RDPTR(txq_id), index);
+}
+
+void iwl4965_tx_queue_set_status(struct iwl_priv *priv,
+					struct iwl_tx_queue *txq,
+					int tx_fifo_id, int scd_retry)
+{
+	int txq_id = txq->q.id;
+
+	/* Find out whether to activate Tx queue */
+	int active = test_bit(txq_id, &priv->txq_ctx_active_msk) ? 1 : 0;
+
+	/* Set up and activate */
+	iwl_legacy_write_prph(priv, IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
+			 (active << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+			 (tx_fifo_id << IWL49_SCD_QUEUE_STTS_REG_POS_TXF) |
+			 (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_WSL) |
+			 (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
+			 IWL49_SCD_QUEUE_STTS_REG_MSK);
+
+	txq->sched_retry = scd_retry;
+
+	IWL_DEBUG_INFO(priv, "%s %s Queue %d on AC %d\n",
+		       active ? "Activate" : "Deactivate",
+		       scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
+}
+
+
+static int iwl4965_init_drv(struct iwl_priv *priv)
+{
+	int ret;
+
+	spin_lock_init(&priv->sta_lock);
+	spin_lock_init(&priv->hcmd_lock);
+
+	INIT_LIST_HEAD(&priv->free_frames);
+
+	mutex_init(&priv->mutex);
+	mutex_init(&priv->sync_cmd_mutex);
+
+	priv->ieee_channels = NULL;
+	priv->ieee_rates = NULL;
+	priv->band = IEEE80211_BAND_2GHZ;
+
+	priv->iw_mode = NL80211_IFTYPE_STATION;
+	priv->current_ht_config.smps = IEEE80211_SMPS_STATIC;
+	priv->missed_beacon_threshold = IWL_MISSED_BEACON_THRESHOLD_DEF;
+	priv->_4965.agg_tids_count = 0;
+
+	/* initialize force reset */
+	priv->force_reset[IWL_RF_RESET].reset_duration =
+		IWL_DELAY_NEXT_FORCE_RF_RESET;
+	priv->force_reset[IWL_FW_RESET].reset_duration =
+		IWL_DELAY_NEXT_FORCE_FW_RELOAD;
+
+	/* Choose which receivers/antennas to use */
+	if (priv->cfg->ops->hcmd->set_rxon_chain)
+		priv->cfg->ops->hcmd->set_rxon_chain(priv,
+					&priv->contexts[IWL_RXON_CTX_BSS]);
+
+	iwl_legacy_init_scan_params(priv);
+
+	/* Set the tx_power_user_lmt to the lowest power level
+	 * this value will get overwritten by channel max power avg
+	 * from eeprom */
+	priv->tx_power_user_lmt = IWL4965_TX_POWER_TARGET_POWER_MIN;
+	priv->tx_power_next = IWL4965_TX_POWER_TARGET_POWER_MIN;
+
+	ret = iwl_legacy_init_channel_map(priv);
+	if (ret) {
+		IWL_ERR(priv, "initializing regulatory failed: %d\n", ret);
+		goto err;
+	}
+
+	ret = iwl_legacy_init_geos(priv);
+	if (ret) {
+		IWL_ERR(priv, "initializing geos failed: %d\n", ret);
+		goto err_free_channel_map;
+	}
+	iwl4965_init_hw_rates(priv, priv->ieee_rates);
+
+	return 0;
+
+err_free_channel_map:
+	iwl_legacy_free_channel_map(priv);
+err:
+	return ret;
+}
+
+static void iwl4965_uninit_drv(struct iwl_priv *priv)
+{
+	iwl4965_calib_free_results(priv);
+	iwl_legacy_free_geos(priv);
+	iwl_legacy_free_channel_map(priv);
+	kfree(priv->scan_cmd);
+}
+
+static void iwl4965_hw_detect(struct iwl_priv *priv)
+{
+	priv->hw_rev = _iwl_legacy_read32(priv, CSR_HW_REV);
+	priv->hw_wa_rev = _iwl_legacy_read32(priv, CSR_HW_REV_WA_REG);
+	pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &priv->rev_id);
+	IWL_DEBUG_INFO(priv, "HW Revision ID = 0x%X\n", priv->rev_id);
+}
+
+static int iwl4965_set_hw_params(struct iwl_priv *priv)
+{
+	priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
+	priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
+	if (priv->cfg->mod_params->amsdu_size_8K)
+		priv->hw_params.rx_page_order = get_order(IWL_RX_BUF_SIZE_8K);
+	else
+		priv->hw_params.rx_page_order = get_order(IWL_RX_BUF_SIZE_4K);
+
+	priv->hw_params.max_beacon_itrvl = IWL_MAX_UCODE_BEACON_INTERVAL;
+
+	if (priv->cfg->mod_params->disable_11n)
+		priv->cfg->sku &= ~IWL_SKU_N;
+
+	/* Device-specific setup */
+	return priv->cfg->ops->lib->set_hw_params(priv);
+}
+
+static const u8 iwl4965_bss_ac_to_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+};
+
+static const u8 iwl4965_bss_ac_to_queue[] = {
+	0, 1, 2, 3,
+};
+
+static int
+iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int err = 0, i;
+	struct iwl_priv *priv;
+	struct ieee80211_hw *hw;
+	struct iwl_cfg *cfg = (struct iwl_cfg *)(ent->driver_data);
+	unsigned long flags;
+	u16 pci_cmd;
+
+	/************************
+	 * 1. Allocating HW data
+	 ************************/
+
+	hw = iwl_legacy_alloc_all(cfg);
+	if (!hw) {
+		err = -ENOMEM;
+		goto out;
+	}
+	priv = hw->priv;
+	/* At this point both hw and priv are allocated. */
+
+	/*
+	 * The default context is always valid,
+	 * more may be discovered when firmware
+	 * is loaded.
+	 */
+	priv->valid_contexts = BIT(IWL_RXON_CTX_BSS);
+
+	for (i = 0; i < NUM_IWL_RXON_CTX; i++)
+		priv->contexts[i].ctxid = i;
+
+	priv->contexts[IWL_RXON_CTX_BSS].always_active = true;
+	priv->contexts[IWL_RXON_CTX_BSS].is_active = true;
+	priv->contexts[IWL_RXON_CTX_BSS].rxon_cmd = REPLY_RXON;
+	priv->contexts[IWL_RXON_CTX_BSS].rxon_timing_cmd = REPLY_RXON_TIMING;
+	priv->contexts[IWL_RXON_CTX_BSS].rxon_assoc_cmd = REPLY_RXON_ASSOC;
+	priv->contexts[IWL_RXON_CTX_BSS].qos_cmd = REPLY_QOS_PARAM;
+	priv->contexts[IWL_RXON_CTX_BSS].ap_sta_id = IWL_AP_ID;
+	priv->contexts[IWL_RXON_CTX_BSS].wep_key_cmd = REPLY_WEPKEY;
+	priv->contexts[IWL_RXON_CTX_BSS].ac_to_fifo = iwl4965_bss_ac_to_fifo;
+	priv->contexts[IWL_RXON_CTX_BSS].ac_to_queue = iwl4965_bss_ac_to_queue;
+	priv->contexts[IWL_RXON_CTX_BSS].exclusive_interface_modes =
+		BIT(NL80211_IFTYPE_ADHOC);
+	priv->contexts[IWL_RXON_CTX_BSS].interface_modes =
+		BIT(NL80211_IFTYPE_STATION);
+	priv->contexts[IWL_RXON_CTX_BSS].ap_devtype = RXON_DEV_TYPE_AP;
+	priv->contexts[IWL_RXON_CTX_BSS].ibss_devtype = RXON_DEV_TYPE_IBSS;
+	priv->contexts[IWL_RXON_CTX_BSS].station_devtype = RXON_DEV_TYPE_ESS;
+	priv->contexts[IWL_RXON_CTX_BSS].unused_devtype = RXON_DEV_TYPE_ESS;
+
+	BUILD_BUG_ON(NUM_IWL_RXON_CTX != 1);
+
+	SET_IEEE80211_DEV(hw, &pdev->dev);
+
+	IWL_DEBUG_INFO(priv, "*** LOAD DRIVER ***\n");
+	priv->cfg = cfg;
+	priv->pci_dev = pdev;
+	priv->inta_mask = CSR_INI_SET_MASK;
+
+	if (iwl_legacy_alloc_traffic_mem(priv))
+		IWL_ERR(priv, "Not enough memory to generate traffic log\n");
+
+	/**************************
+	 * 2. Initializing PCI bus
+	 **************************/
+	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1 |
+				PCIE_LINK_STATE_CLKPM);
+
+	if (pci_enable_device(pdev)) {
+		err = -ENODEV;
+		goto out_ieee80211_free_hw;
+	}
+
+	pci_set_master(pdev);
+
+	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(36));
+	if (!err)
+		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(36));
+	if (err) {
+		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (!err)
+			err = pci_set_consistent_dma_mask(pdev,
+							DMA_BIT_MASK(32));
+		/* both attempts failed: */
+		if (err) {
+			IWL_WARN(priv, "No suitable DMA available.\n");
+			goto out_pci_disable_device;
+		}
+	}
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err)
+		goto out_pci_disable_device;
+
+	pci_set_drvdata(pdev, priv);
+
+
+	/***********************
+	 * 3. Read REV register
+	 ***********************/
+	priv->hw_base = pci_iomap(pdev, 0, 0);
+	if (!priv->hw_base) {
+		err = -ENODEV;
+		goto out_pci_release_regions;
+	}
+
+	IWL_DEBUG_INFO(priv, "pci_resource_len = 0x%08llx\n",
+		(unsigned long long) pci_resource_len(pdev, 0));
+	IWL_DEBUG_INFO(priv, "pci_resource_base = %p\n", priv->hw_base);
+
+	/* these spin locks will be used in apm_ops.init and EEPROM access
+	 * we should init now
+	 */
+	spin_lock_init(&priv->reg_lock);
+	spin_lock_init(&priv->lock);
+
+	/*
+	 * stop and reset the on-board processor just in case it is in a
+	 * strange state ... like being left stranded by a primary kernel
+	 * and this is now the kdump kernel trying to start up
+	 */
+	iwl_write32(priv, CSR_RESET, CSR_RESET_REG_FLAG_NEVO_RESET);
+
+	iwl4965_hw_detect(priv);
+	IWL_INFO(priv, "Detected %s, REV=0x%X\n",
+		priv->cfg->name, priv->hw_rev);
+
+	/* We disable the RETRY_TIMEOUT register (0x41) to keep
+	 * PCI Tx retries from interfering with C3 CPU state */
+	pci_write_config_byte(pdev, PCI_CFG_RETRY_TIMEOUT, 0x00);
+
+	iwl4965_prepare_card_hw(priv);
+	if (!priv->hw_ready) {
+		IWL_WARN(priv, "Failed, HW not ready\n");
+		goto out_iounmap;
+	}
+
+	/*****************
+	 * 4. Read EEPROM
+	 *****************/
+	/* Read the EEPROM */
+	err = iwl_legacy_eeprom_init(priv);
+	if (err) {
+		IWL_ERR(priv, "Unable to init EEPROM\n");
+		goto out_iounmap;
+	}
+	err = iwl4965_eeprom_check_version(priv);
+	if (err)
+		goto out_free_eeprom;
+
+	if (err)
+		goto out_free_eeprom;
+
+	/* extract MAC Address */
+	iwl4965_eeprom_get_mac(priv, priv->addresses[0].addr);
+	IWL_DEBUG_INFO(priv, "MAC address: %pM\n", priv->addresses[0].addr);
+	priv->hw->wiphy->addresses = priv->addresses;
+	priv->hw->wiphy->n_addresses = 1;
+
+	/************************
+	 * 5. Setup HW constants
+	 ************************/
+	if (iwl4965_set_hw_params(priv)) {
+		IWL_ERR(priv, "failed to set hw parameters\n");
+		goto out_free_eeprom;
+	}
+
+	/*******************
+	 * 6. Setup priv
+	 *******************/
+
+	err = iwl4965_init_drv(priv);
+	if (err)
+		goto out_free_eeprom;
+	/* At this point both hw and priv are initialized. */
+
+	/********************
+	 * 7. Setup services
+	 ********************/
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_legacy_disable_interrupts(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	pci_enable_msi(priv->pci_dev);
+
+	err = request_irq(priv->pci_dev->irq, iwl_legacy_isr,
+			  IRQF_SHARED, DRV_NAME, priv);
+	if (err) {
+		IWL_ERR(priv, "Error allocating IRQ %d\n", priv->pci_dev->irq);
+		goto out_disable_msi;
+	}
+
+	iwl4965_setup_deferred_work(priv);
+	iwl4965_setup_rx_handlers(priv);
+
+	/*********************************************
+	 * 8. Enable interrupts and read RFKILL state
+	 *********************************************/
+
+	/* enable interrupts if needed: hw bug w/a */
+	pci_read_config_word(priv->pci_dev, PCI_COMMAND, &pci_cmd);
+	if (pci_cmd & PCI_COMMAND_INTX_DISABLE) {
+		pci_cmd &= ~PCI_COMMAND_INTX_DISABLE;
+		pci_write_config_word(priv->pci_dev, PCI_COMMAND, pci_cmd);
+	}
+
+	iwl_legacy_enable_interrupts(priv);
+
+	/* If platform's RF_KILL switch is NOT set to KILL */
+	if (iwl_read32(priv, CSR_GP_CNTRL) &
+		CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW)
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+
+	wiphy_rfkill_set_hw_state(priv->hw->wiphy,
+		test_bit(STATUS_RF_KILL_HW, &priv->status));
+
+	iwl_legacy_power_initialize(priv);
+
+	init_completion(&priv->_4965.firmware_loading_complete);
+
+	err = iwl4965_request_firmware(priv, true);
+	if (err)
+		goto out_destroy_workqueue;
+
+	return 0;
+
+ out_destroy_workqueue:
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+	free_irq(priv->pci_dev->irq, priv);
+ out_disable_msi:
+	pci_disable_msi(priv->pci_dev);
+	iwl4965_uninit_drv(priv);
+ out_free_eeprom:
+	iwl_legacy_eeprom_free(priv);
+ out_iounmap:
+	pci_iounmap(pdev, priv->hw_base);
+ out_pci_release_regions:
+	pci_set_drvdata(pdev, NULL);
+	pci_release_regions(pdev);
+ out_pci_disable_device:
+	pci_disable_device(pdev);
+ out_ieee80211_free_hw:
+	iwl_legacy_free_traffic_mem(priv);
+	ieee80211_free_hw(priv->hw);
+ out:
+	return err;
+}
+
+static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+	unsigned long flags;
+
+	if (!priv)
+		return;
+
+	wait_for_completion(&priv->_4965.firmware_loading_complete);
+
+	IWL_DEBUG_INFO(priv, "*** UNLOAD DRIVER ***\n");
+
+	iwl_legacy_dbgfs_unregister(priv);
+	sysfs_remove_group(&pdev->dev.kobj, &iwl_attribute_group);
+
+	/* ieee80211_unregister_hw call wil cause iwl_mac_stop to
+	 * to be called and iwl4965_down since we are removing the device
+	 * we need to set STATUS_EXIT_PENDING bit.
+	 */
+	set_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	iwl_legacy_leds_exit(priv);
+
+	if (priv->mac80211_registered) {
+		ieee80211_unregister_hw(priv->hw);
+		priv->mac80211_registered = 0;
+	} else {
+		iwl4965_down(priv);
+	}
+
+	/*
+	 * Make sure device is reset to low power before unloading driver.
+	 * This may be redundant with iwl4965_down(), but there are paths to
+	 * run iwl4965_down() without calling apm_ops.stop(), and there are
+	 * paths to avoid running iwl4965_down() at all before leaving driver.
+	 * This (inexpensive) call *makes sure* device is reset.
+	 */
+	iwl_legacy_apm_stop(priv);
+
+	/* make sure we flush any pending irq or
+	 * tasklet for the driver
+	 */
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_legacy_disable_interrupts(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl4965_synchronize_irq(priv);
+
+	iwl4965_dealloc_ucode_pci(priv);
+
+	if (priv->rxq.bd)
+		iwl4965_rx_queue_free(priv, &priv->rxq);
+	iwl4965_hw_txq_ctx_free(priv);
+
+	iwl_legacy_eeprom_free(priv);
+
+
+	/*netif_stop_queue(dev); */
+	flush_workqueue(priv->workqueue);
+
+	/* ieee80211_unregister_hw calls iwl_mac_stop, which flushes
+	 * priv->workqueue... so we can't take down the workqueue
+	 * until now... */
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+	iwl_legacy_free_traffic_mem(priv);
+
+	free_irq(priv->pci_dev->irq, priv);
+	pci_disable_msi(priv->pci_dev);
+	pci_iounmap(pdev, priv->hw_base);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+
+	iwl4965_uninit_drv(priv);
+
+	dev_kfree_skb(priv->beacon_skb);
+
+	ieee80211_free_hw(priv->hw);
+}
+
+/*
+ * Activate/Deactivate Tx DMA/FIFO channels according tx fifos mask
+ * must be called under priv->lock and mac access
+ */
+void iwl4965_txq_set_sched(struct iwl_priv *priv, u32 mask)
+{
+	iwl_legacy_write_prph(priv, IWL49_SCD_TXFACT, mask);
+}
+
+/*****************************************************************************
+ *
+ * driver and module entry point
+ *
+ *****************************************************************************/
+
+/* Hardware specific file defines the PCI IDs table for that hardware module */
+static DEFINE_PCI_DEVICE_TABLE(iwl4965_hw_card_ids) = {
+#if defined(CONFIG_IWL4965_MODULE) || defined(CONFIG_IWL4965)
+	{IWL_PCI_DEVICE(0x4229, PCI_ANY_ID, iwl4965_cfg)},
+	{IWL_PCI_DEVICE(0x4230, PCI_ANY_ID, iwl4965_cfg)},
+#endif /* CONFIG_IWL4965 */
+
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, iwl4965_hw_card_ids);
+
+static struct pci_driver iwl4965_driver = {
+	.name = DRV_NAME,
+	.id_table = iwl4965_hw_card_ids,
+	.probe = iwl4965_pci_probe,
+	.remove = __devexit_p(iwl4965_pci_remove),
+	.driver.pm = IWL_LEGACY_PM_OPS,
+};
+
+static int __init iwl4965_init(void)
+{
+
+	int ret;
+	pr_info(DRV_DESCRIPTION ", " DRV_VERSION "\n");
+	pr_info(DRV_COPYRIGHT "\n");
+
+	ret = iwl4965_rate_control_register();
+	if (ret) {
+		pr_err("Unable to register rate control algorithm: %d\n", ret);
+		return ret;
+	}
+
+	ret = pci_register_driver(&iwl4965_driver);
+	if (ret) {
+		pr_err("Unable to initialize PCI module\n");
+		goto error_register;
+	}
+
+	return ret;
+
+error_register:
+	iwl4965_rate_control_unregister();
+	return ret;
+}
+
+static void __exit iwl4965_exit(void)
+{
+	pci_unregister_driver(&iwl4965_driver);
+	iwl4965_rate_control_unregister();
+}
+
+module_exit(iwl4965_exit);
+module_init(iwl4965_init);
+
+#ifdef CONFIG_IWLWIFI_LEGACY_DEBUG
+module_param_named(debug, iwlegacy_debug_level, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "debug output mask");
+#endif
+
+module_param_named(swcrypto, iwl4965_mod_params.sw_crypto, int, S_IRUGO);
+MODULE_PARM_DESC(swcrypto, "using crypto in software (default 0 [hardware])");
+module_param_named(queues_num, iwl4965_mod_params.num_of_queues, int, S_IRUGO);
+MODULE_PARM_DESC(queues_num, "number of hw queues.");
+module_param_named(11n_disable, iwl4965_mod_params.disable_11n, int, S_IRUGO);
+MODULE_PARM_DESC(11n_disable, "disable 11n functionality");
+module_param_named(amsdu_size_8K, iwl4965_mod_params.amsdu_size_8K,
+		   int, S_IRUGO);
+MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size");
+module_param_named(fw_restart, iwl4965_mod_params.restart_fw, int, S_IRUGO);
+MODULE_PARM_DESC(fw_restart, "restart firmware in case of error");
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index ed424574160e..17d555f2215a 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -1,14 +1,52 @@
-config IWLWIFI
-	tristate "Intel Wireless Wifi"
+config IWLAGN
+	tristate "Intel Wireless WiFi Next Gen AGN - Wireless-N/Advanced-N/Ultimate-N (iwlagn) "
 	depends on PCI && MAC80211
 	select FW_LOADER
+	select NEW_LEDS
+	select LEDS_CLASS
+	select LEDS_TRIGGERS
+	select MAC80211_LEDS
+	---help---
+	  Select to build the driver supporting the:
+
+	  Intel Wireless WiFi Link Next-Gen AGN
+
+	  This option enables support for use with the following hardware:
+		Intel Wireless WiFi Link 6250AGN Adapter
+		Intel 6000 Series Wi-Fi Adapters (6200AGN and 6300AGN)
+		Intel WiFi Link 1000BGN
+		Intel Wireless WiFi 5150AGN
+		Intel Wireless WiFi 5100AGN, 5300AGN, and 5350AGN
+		Intel 6005 Series Wi-Fi Adapters
+		Intel 6030 Series Wi-Fi Adapters
+		Intel Wireless WiFi Link 6150BGN 2 Adapter
+		Intel 100 Series Wi-Fi Adapters (100BGN and 130BGN)
+		Intel 2000 Series Wi-Fi Adapters
+
+
+	  This driver uses the kernel's mac80211 subsystem.
+
+	  In order to use this driver, you will need a microcode (uCode)
+	  image for it. You can obtain the microcode from:
+
+	          <http://intellinuxwireless.org/>.
+
+	  The microcode is typically installed in /lib/firmware. You can
+	  look in the hotplug script /etc/hotplug/firmware.agent to
+	  determine which directory FIRMWARE_DIR is set to when the script
+	  runs.
+
+	  If you want to compile the driver as a module ( = code which can be
+	  inserted in and removed from the running kernel whenever you want),
+	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
+	  module will be called iwlagn.
 
 menu "Debugging Options"
-	depends on IWLWIFI
+	depends on IWLAGN
 
 config IWLWIFI_DEBUG
-	bool "Enable full debugging output in iwlagn and iwl3945 drivers"
-	depends on IWLWIFI
+	bool "Enable full debugging output in the iwlagn driver"
+	depends on IWLAGN
 	---help---
 	  This option will enable debug tracing output for the iwlwifi drivers
 
@@ -33,7 +71,7 @@ config IWLWIFI_DEBUG
 
 config IWLWIFI_DEBUGFS
         bool "iwlagn debugfs support"
-        depends on IWLWIFI && MAC80211_DEBUGFS
+        depends on IWLAGN && MAC80211_DEBUGFS
         ---help---
 	  Enable creation of debugfs files for the iwlwifi drivers. This
 	  is a low-impact option that allows getting insight into the
@@ -41,13 +79,13 @@ config IWLWIFI_DEBUGFS
 
 config IWLWIFI_DEBUG_EXPERIMENTAL_UCODE
         bool "Experimental uCode support"
-        depends on IWLWIFI && IWLWIFI_DEBUG
+        depends on IWLAGN && IWLWIFI_DEBUG
         ---help---
 	  Enable use of experimental ucode for testing and debugging.
 
 config IWLWIFI_DEVICE_TRACING
 	bool "iwlwifi device access tracing"
-	depends on IWLWIFI
+	depends on IWLAGN
 	depends on EVENT_TRACING
 	help
 	  Say Y here to trace all commands, including TX frames and IO
@@ -64,73 +102,19 @@ config IWLWIFI_DEVICE_TRACING
 	  occur.
 endmenu
 
-config IWLAGN
-	tristate "Intel Wireless WiFi Next Gen AGN (iwlagn)"
-	depends on IWLWIFI
-	---help---
-	  Select to build the driver supporting the:
-
-	  Intel Wireless WiFi Link Next-Gen AGN
-
-	  This driver uses the kernel's mac80211 subsystem.
-
-	  In order to use this driver, you will need a microcode (uCode)
-	  image for it. You can obtain the microcode from:
-
-	          <http://intellinuxwireless.org/>.
-
-	  The microcode is typically installed in /lib/firmware. You can
-	  look in the hotplug script /etc/hotplug/firmware.agent to
-	  determine which directory FIRMWARE_DIR is set to when the script
-	  runs.
-
-	  If you want to compile the driver as a module ( = code which can be
-	  inserted in and removed from the running kernel whenever you want),
-	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
-	  module will be called iwlagn.
-
-
-config IWL4965
-	bool "Intel Wireless WiFi 4965AGN"
-	depends on IWLAGN
-	---help---
-	  This option enables support for Intel Wireless WiFi Link 4965AGN
-
-config IWL5000
-	bool "Intel Wireless-N/Advanced-N/Ultimate-N WiFi Link"
+config IWL_P2P
+	bool "iwlwifi experimental P2P support"
 	depends on IWLAGN
-	---help---
-	  This option enables support for use with the following hardware:
-		Intel Wireless WiFi Link 6250AGN Adapter
-		Intel 6000 Series Wi-Fi Adapters (6200AGN and 6300AGN)
-		Intel WiFi Link 1000BGN
-		Intel Wireless WiFi 5150AGN
-		Intel Wireless WiFi 5100AGN, 5300AGN, and 5350AGN
-		Intel 6000 Gen 2 Series Wi-Fi Adapters (6000G2A and 6000G2B)
-		Intel WIreless WiFi Link 6050BGN Gen 2 Adapter
-		Intel 100 Series Wi-Fi Adapters (100BGN and 130BGN)
-
-config IWL3945
-	tristate "Intel PRO/Wireless 3945ABG/BG Network Connection (iwl3945)"
-	depends on IWLWIFI
-	---help---
-	  Select to build the driver supporting the:
-
-	  Intel PRO/Wireless 3945ABG/BG Network Connection
-
-	  This driver uses the kernel's mac80211 subsystem.
-
-	  In order to use this driver, you will need a microcode (uCode)
-	  image for it. You can obtain the microcode from:
+	help
+	  This option enables experimental P2P support for some devices
+	  based on microcode support. Since P2P support is still under
+	  development, this option may even enable it for some devices
+	  now that turn out to not support it in the future due to
+	  microcode restrictions.
 
-	          <http://intellinuxwireless.org/>.
+	  To determine if your microcode supports the experimental P2P
+	  offered by this option, check if the driver advertises AP
+	  support when it is loaded.
 
-	  The microcode is typically installed in /lib/firmware. You can
-	  look in the hotplug script /etc/hotplug/firmware.agent to
-	  determine which directory FIRMWARE_DIR is set to when the script
-	  runs.
+	  Say Y only if you want to experiment with P2P.
 
-	  If you want to compile the driver as a module ( = code which can be
-	  inserted in and removed from the running kernel whenever you want),
-	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
-	  module will be called iwl3945.
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 93380f97835f..9d6ee836426c 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -1,35 +1,23 @@
-obj-$(CONFIG_IWLWIFI)	+= iwlcore.o
-iwlcore-objs 		:= iwl-core.o iwl-eeprom.o iwl-hcmd.o iwl-power.o
-iwlcore-objs 		+= iwl-rx.o iwl-tx.o iwl-sta.o
-iwlcore-objs 		+= iwl-scan.o iwl-led.o
-iwlcore-$(CONFIG_IWL3945) += iwl-legacy.o
-iwlcore-$(CONFIG_IWL4965) += iwl-legacy.o
-iwlcore-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
-iwlcore-$(CONFIG_IWLWIFI_DEVICE_TRACING) += iwl-devtrace.o
-
-# If 3945 is selected only, iwl-legacy.o will be added
-# to iwlcore-m above, but it needs to be built in.
-iwlcore-objs += $(iwlcore-m)
-
-CFLAGS_iwl-devtrace.o := -I$(src)
-
 # AGN
 obj-$(CONFIG_IWLAGN)	+= iwlagn.o
 iwlagn-objs		:= iwl-agn.o iwl-agn-rs.o iwl-agn-led.o
 iwlagn-objs		+= iwl-agn-ucode.o iwl-agn-tx.o
-iwlagn-objs		+= iwl-agn-lib.o iwl-agn-rx.o iwl-agn-calib.o
+iwlagn-objs		+= iwl-agn-lib.o iwl-agn-calib.o
 iwlagn-objs		+= iwl-agn-tt.o iwl-agn-sta.o iwl-agn-eeprom.o
-iwlagn-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-agn-debugfs.o
 
-iwlagn-$(CONFIG_IWL4965) += iwl-4965.o
-iwlagn-$(CONFIG_IWL5000) += iwl-agn-rxon.o iwl-agn-hcmd.o iwl-agn-ict.o
-iwlagn-$(CONFIG_IWL5000) += iwl-5000.o
-iwlagn-$(CONFIG_IWL5000) += iwl-6000.o
-iwlagn-$(CONFIG_IWL5000) += iwl-1000.o
+iwlagn-objs 		+= iwl-core.o iwl-eeprom.o iwl-hcmd.o iwl-power.o
+iwlagn-objs 		+= iwl-rx.o iwl-tx.o iwl-sta.o
+iwlagn-objs 		+= iwl-scan.o iwl-led.o
+iwlagn-objs             += iwl-agn-rxon.o iwl-agn-hcmd.o iwl-agn-ict.o
+iwlagn-objs             += iwl-5000.o
+iwlagn-objs             += iwl-6000.o
+iwlagn-objs             += iwl-1000.o
+iwlagn-objs             += iwl-2000.o
+
+iwlagn-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-agn-debugfs.o
+iwlagn-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
+iwlagn-$(CONFIG_IWLWIFI_DEVICE_TRACING) += iwl-devtrace.o
 
-# 3945
-obj-$(CONFIG_IWL3945)	+= iwl3945.o
-iwl3945-objs		:= iwl3945-base.o iwl-3945.o iwl-3945-rs.o iwl-3945-led.o
-iwl3945-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-3945-debugfs.o
+CFLAGS_iwl-devtrace.o := -I$(src)
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/iwlwifi/iwl-1000.c
index ba78bc8a259f..27c5007e577c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-1000.c
@@ -49,7 +49,7 @@
 #include "iwl-agn-debugfs.h"
 
 /* Highest firmware API version supported */
-#define IWL1000_UCODE_API_MAX 3
+#define IWL1000_UCODE_API_MAX 5
 #define IWL100_UCODE_API_MAX 5
 
 /* Lowest firmware API version supported */
@@ -232,8 +232,6 @@ static struct iwl_lib_ops iwl1000_lib = {
 		.bt_stats_read = iwl_ucode_bt_stats_read,
 		.reply_tx_error = iwl_reply_tx_error_read,
 	},
-	.check_plcp_health = iwl_good_plcp_health,
-	.check_ack_health = iwl_good_ack_health,
 	.txfifo_flush = iwlagn_txfifo_flush,
 	.dev_txfifo_flush = iwlagn_dev_txfifo_flush,
 	.tt_ops = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/iwlwifi/iwl-2000.c
new file mode 100644
index 000000000000..d7b6126408c9
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-2000.c
@@ -0,0 +1,560 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2008 - 2010 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+#include <asm/unaligned.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-sta.h"
+#include "iwl-agn.h"
+#include "iwl-helpers.h"
+#include "iwl-agn-hw.h"
+#include "iwl-6000-hw.h"
+#include "iwl-agn-led.h"
+#include "iwl-agn-debugfs.h"
+
+/* Highest firmware API version supported */
+#define IWL2030_UCODE_API_MAX 5
+#define IWL2000_UCODE_API_MAX 5
+#define IWL200_UCODE_API_MAX 5
+
+/* Lowest firmware API version supported */
+#define IWL2030_UCODE_API_MIN 5
+#define IWL2000_UCODE_API_MIN 5
+#define IWL200_UCODE_API_MIN 5
+
+#define IWL2030_FW_PRE "iwlwifi-2030-"
+#define _IWL2030_MODULE_FIRMWARE(api) IWL2030_FW_PRE #api ".ucode"
+#define IWL2030_MODULE_FIRMWARE(api) _IWL2030_MODULE_FIRMWARE(api)
+
+#define IWL2000_FW_PRE "iwlwifi-2000-"
+#define _IWL2000_MODULE_FIRMWARE(api) IWL2000_FW_PRE #api ".ucode"
+#define IWL2000_MODULE_FIRMWARE(api) _IWL2000_MODULE_FIRMWARE(api)
+
+#define IWL200_FW_PRE "iwlwifi-200-"
+#define _IWL200_MODULE_FIRMWARE(api) IWL200_FW_PRE #api ".ucode"
+#define IWL200_MODULE_FIRMWARE(api) _IWL200_MODULE_FIRMWARE(api)
+
+static void iwl2000_set_ct_threshold(struct iwl_priv *priv)
+{
+	/* want Celsius */
+	priv->hw_params.ct_kill_threshold = CT_KILL_THRESHOLD;
+	priv->hw_params.ct_kill_exit_threshold = CT_KILL_EXIT_THRESHOLD;
+}
+
+/* NIC configuration for 2000 series */
+static void iwl2000_nic_config(struct iwl_priv *priv)
+{
+	u16 radio_cfg;
+
+	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
+
+	/* write radio config values to register */
+	if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <= EEPROM_RF_CONFIG_TYPE_MAX)
+	iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
+			EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
+			EEPROM_RF_CFG_DASH_MSK(radio_cfg));
+
+	/* set CSR_HW_CONFIG_REG for uCode use */
+	iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+
+	if (priv->cfg->iq_invert)
+		iwl_set_bit(priv, CSR_GP_DRIVER_REG,
+			    CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER);
+
+}
+
+static struct iwl_sensitivity_ranges iwl2000_sensitivity = {
+	.min_nrg_cck = 97,
+	.max_nrg_cck = 0, /* not used, set to 0 */
+	.auto_corr_min_ofdm = 80,
+	.auto_corr_min_ofdm_mrc = 128,
+	.auto_corr_min_ofdm_x1 = 105,
+	.auto_corr_min_ofdm_mrc_x1 = 192,
+
+	.auto_corr_max_ofdm = 145,
+	.auto_corr_max_ofdm_mrc = 232,
+	.auto_corr_max_ofdm_x1 = 110,
+	.auto_corr_max_ofdm_mrc_x1 = 232,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 175,
+	.auto_corr_min_cck_mrc = 160,
+	.auto_corr_max_cck_mrc = 310,
+	.nrg_th_cck = 97,
+	.nrg_th_ofdm = 100,
+
+	.barker_corr_th_min = 190,
+	.barker_corr_th_min_mrc = 390,
+	.nrg_th_cca = 62,
+};
+
+static int iwl2000_hw_set_hw_params(struct iwl_priv *priv)
+{
+	if (priv->cfg->mod_params->num_of_queues >= IWL_MIN_NUM_QUEUES &&
+	    priv->cfg->mod_params->num_of_queues <= IWLAGN_NUM_QUEUES)
+		priv->cfg->base_params->num_of_queues =
+			priv->cfg->mod_params->num_of_queues;
+
+	priv->hw_params.max_txq_num = priv->cfg->base_params->num_of_queues;
+	priv->hw_params.dma_chnl_num = FH50_TCSR_CHNL_NUM;
+	priv->hw_params.scd_bc_tbls_size =
+		priv->cfg->base_params->num_of_queues *
+		sizeof(struct iwlagn_scd_bc_tbl);
+	priv->hw_params.tfd_size = sizeof(struct iwl_tfd);
+	priv->hw_params.max_stations = IWLAGN_STATION_COUNT;
+	priv->contexts[IWL_RXON_CTX_BSS].bcast_sta_id = IWLAGN_BROADCAST_ID;
+
+	priv->hw_params.max_data_size = IWL60_RTC_DATA_SIZE;
+	priv->hw_params.max_inst_size = IWL60_RTC_INST_SIZE;
+
+	priv->hw_params.max_bsm_size = 0;
+	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
+					BIT(IEEE80211_BAND_5GHZ);
+	priv->hw_params.rx_wrt_ptr_reg = FH_RSCSR_CHNL0_WPTR;
+
+	priv->hw_params.tx_chains_num = num_of_ant(priv->cfg->valid_tx_ant);
+	if (priv->cfg->rx_with_siso_diversity)
+		priv->hw_params.rx_chains_num = 1;
+	else
+		priv->hw_params.rx_chains_num =
+			num_of_ant(priv->cfg->valid_rx_ant);
+	priv->hw_params.valid_tx_ant = priv->cfg->valid_tx_ant;
+	priv->hw_params.valid_rx_ant = priv->cfg->valid_rx_ant;
+
+	iwl2000_set_ct_threshold(priv);
+
+	/* Set initial sensitivity parameters */
+	/* Set initial calibration set */
+	priv->hw_params.sens = &iwl2000_sensitivity;
+	priv->hw_params.calib_init_cfg =
+		BIT(IWL_CALIB_XTAL)             |
+		BIT(IWL_CALIB_LO)               |
+		BIT(IWL_CALIB_TX_IQ)            |
+		BIT(IWL_CALIB_BASE_BAND);
+	if (priv->cfg->need_dc_calib)
+		priv->hw_params.calib_rt_cfg |= BIT(IWL_CALIB_CFG_DC_IDX);
+	if (priv->cfg->need_temp_offset_calib)
+		priv->hw_params.calib_init_cfg |= BIT(IWL_CALIB_TEMP_OFFSET);
+
+	priv->hw_params.beacon_time_tsf_bits = IWLAGN_EXT_BEACON_TIME_POS;
+
+	return 0;
+}
+
+static int iwl2030_hw_channel_switch(struct iwl_priv *priv,
+                                    struct ieee80211_channel_switch *ch_switch)
+{
+	/*
+	 * MULTI-FIXME
+	 * See iwl_mac_channel_switch.
+	 */
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	struct iwl6000_channel_switch_cmd cmd;
+	const struct iwl_channel_info *ch_info;
+	u32 switch_time_in_usec, ucode_switch_time;
+	u16 ch;
+	u32 tsf_low;
+	u8 switch_count;
+	u16 beacon_interval = le16_to_cpu(ctx->timing.beacon_interval);
+	struct ieee80211_vif *vif = ctx->vif;
+	struct iwl_host_cmd hcmd = {
+		.id = REPLY_CHANNEL_SWITCH,
+		.len = sizeof(cmd),
+		.flags = CMD_SYNC,
+		.data = &cmd,
+	};
+
+	cmd.band = priv->band == IEEE80211_BAND_2GHZ;
+	ch = ch_switch->channel->hw_value;
+	IWL_DEBUG_11H(priv, "channel switch from %u to %u\n",
+		ctx->active.channel, ch);
+	cmd.channel = cpu_to_le16(ch);
+	cmd.rxon_flags = ctx->staging.flags;
+	cmd.rxon_filter_flags = ctx->staging.filter_flags;
+	switch_count = ch_switch->count;
+	tsf_low = ch_switch->timestamp & 0x0ffffffff;
+	/*
+	 * calculate the ucode channel switch time
+	 * adding TSF as one of the factor for when to switch
+	 */
+	if ((priv->ucode_beacon_time > tsf_low) && beacon_interval) {
+		if (switch_count > ((priv->ucode_beacon_time - tsf_low) /
+		    beacon_interval)) {
+			switch_count -= (priv->ucode_beacon_time -
+				tsf_low) / beacon_interval;
+		} else
+			switch_count = 0;
+	}
+	if (switch_count <= 1)
+		cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+	else {
+		switch_time_in_usec =
+			vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
+		ucode_switch_time = iwl_usecs_to_beacons(priv,
+						switch_time_in_usec,
+						beacon_interval);
+		cmd.switch_time = iwl_add_beacon_time(priv,
+						priv->ucode_beacon_time,
+						ucode_switch_time,
+						beacon_interval);
+	}
+	IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
+		      cmd.switch_time);
+	ch_info = iwl_get_channel_info(priv, priv->band, ch);
+	if (ch_info)
+		cmd.expect_beacon = is_channel_radar(ch_info);
+	else {
+		IWL_ERR(priv, "invalid channel switch from %u to %u\n",
+			ctx->active.channel, ch);
+		return -EFAULT;
+	}
+	priv->switch_rxon.channel = cmd.channel;
+	priv->switch_rxon.switch_in_progress = true;
+
+	return iwl_send_cmd_sync(priv, &hcmd);
+}
+
+static struct iwl_lib_ops iwl2000_lib = {
+	.set_hw_params = iwl2000_hw_set_hw_params,
+	.txq_update_byte_cnt_tbl = iwlagn_txq_update_byte_cnt_tbl,
+	.txq_inval_byte_cnt_tbl = iwlagn_txq_inval_byte_cnt_tbl,
+	.txq_set_sched = iwlagn_txq_set_sched,
+	.txq_agg_enable = iwlagn_txq_agg_enable,
+	.txq_agg_disable = iwlagn_txq_agg_disable,
+	.txq_attach_buf_to_tfd = iwl_hw_txq_attach_buf_to_tfd,
+	.txq_free_tfd = iwl_hw_txq_free_tfd,
+	.txq_init = iwl_hw_tx_queue_init,
+	.rx_handler_setup = iwlagn_rx_handler_setup,
+	.setup_deferred_work = iwlagn_bt_setup_deferred_work,
+	.cancel_deferred_work = iwlagn_bt_cancel_deferred_work,
+	.is_valid_rtc_data_addr = iwlagn_hw_valid_rtc_data_addr,
+	.load_ucode = iwlagn_load_ucode,
+	.dump_nic_event_log = iwl_dump_nic_event_log,
+	.dump_nic_error_log = iwl_dump_nic_error_log,
+	.dump_csr = iwl_dump_csr,
+	.dump_fh = iwl_dump_fh,
+	.init_alive_start = iwlagn_init_alive_start,
+	.alive_notify = iwlagn_alive_notify,
+	.send_tx_power = iwlagn_send_tx_power,
+	.update_chain_flags = iwl_update_chain_flags,
+	.set_channel_switch = iwl2030_hw_channel_switch,
+	.apm_ops = {
+		.init = iwl_apm_init,
+		.config = iwl2000_nic_config,
+	},
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REG_BAND_1_CHANNELS,
+			EEPROM_REG_BAND_2_CHANNELS,
+			EEPROM_REG_BAND_3_CHANNELS,
+			EEPROM_REG_BAND_4_CHANNELS,
+			EEPROM_REG_BAND_5_CHANNELS,
+			EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+			EEPROM_REG_BAND_52_HT40_CHANNELS
+		},
+		.acquire_semaphore = iwlcore_eeprom_acquire_semaphore,
+		.release_semaphore = iwlcore_eeprom_release_semaphore,
+		.calib_version  = iwlagn_eeprom_calib_version,
+		.query_addr = iwlagn_eeprom_query_addr,
+		.update_enhanced_txpower = iwlcore_eeprom_enhanced_txpower,
+	},
+	.isr_ops = {
+		.isr = iwl_isr_ict,
+		.free = iwl_free_isr_ict,
+		.alloc = iwl_alloc_isr_ict,
+		.reset = iwl_reset_ict,
+		.disable = iwl_disable_ict,
+	},
+	.temp_ops = {
+		.temperature = iwlagn_temperature,
+	},
+	.debugfs_ops = {
+		.rx_stats_read = iwl_ucode_rx_stats_read,
+		.tx_stats_read = iwl_ucode_tx_stats_read,
+		.general_stats_read = iwl_ucode_general_stats_read,
+		.bt_stats_read = iwl_ucode_bt_stats_read,
+		.reply_tx_error = iwl_reply_tx_error_read,
+	},
+	.txfifo_flush = iwlagn_txfifo_flush,
+	.dev_txfifo_flush = iwlagn_dev_txfifo_flush,
+	.tt_ops = {
+		.lower_power_detection = iwl_tt_is_low_power_state,
+		.tt_power_mode = iwl_tt_current_power_mode,
+		.ct_kill_check = iwl_check_for_ct_kill,
+	}
+};
+
+static const struct iwl_ops iwl2000_ops = {
+	.lib = &iwl2000_lib,
+	.hcmd = &iwlagn_hcmd,
+	.utils = &iwlagn_hcmd_utils,
+	.led = &iwlagn_led_ops,
+	.ieee80211_ops = &iwlagn_hw_ops,
+};
+
+static const struct iwl_ops iwl2030_ops = {
+	.lib = &iwl2000_lib,
+	.hcmd = &iwlagn_bt_hcmd,
+	.utils = &iwlagn_hcmd_utils,
+	.led = &iwlagn_led_ops,
+	.ieee80211_ops = &iwlagn_hw_ops,
+};
+
+static const struct iwl_ops iwl200_ops = {
+	.lib = &iwl2000_lib,
+	.hcmd = &iwlagn_hcmd,
+	.utils = &iwlagn_hcmd_utils,
+	.led = &iwlagn_led_ops,
+	.ieee80211_ops = &iwlagn_hw_ops,
+};
+
+static const struct iwl_ops iwl230_ops = {
+	.lib = &iwl2000_lib,
+	.hcmd = &iwlagn_bt_hcmd,
+	.utils = &iwlagn_hcmd_utils,
+	.led = &iwlagn_led_ops,
+	.ieee80211_ops = &iwlagn_hw_ops,
+};
+
+static struct iwl_base_params iwl2000_base_params = {
+	.eeprom_size = OTP_LOW_IMAGE_SIZE,
+	.num_of_queues = IWLAGN_NUM_QUEUES,
+	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
+	.pll_cfg_val = 0,
+	.set_l0s = true,
+	.use_bsm = false,
+	.max_ll_items = OTP_MAX_LL_ITEMS_2x00,
+	.shadow_ram_support = true,
+	.led_compensation = 51,
+	.chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
+	.adv_thermal_throttle = true,
+	.support_ct_kill_exit = true,
+	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
+	.chain_noise_scale = 1000,
+	.wd_timeout = IWL_DEF_WD_TIMEOUT,
+	.max_event_log_size = 512,
+	.ucode_tracing = true,
+	.sensitivity_calib_by_driver = true,
+	.chain_noise_calib_by_driver = true,
+	.shadow_reg_enable = true,
+};
+
+
+static struct iwl_base_params iwl2030_base_params = {
+	.eeprom_size = OTP_LOW_IMAGE_SIZE,
+	.num_of_queues = IWLAGN_NUM_QUEUES,
+	.num_of_ampdu_queues = IWLAGN_NUM_AMPDU_QUEUES,
+	.pll_cfg_val = 0,
+	.set_l0s = true,
+	.use_bsm = false,
+	.max_ll_items = OTP_MAX_LL_ITEMS_2x00,
+	.shadow_ram_support = true,
+	.led_compensation = 57,
+	.chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
+	.adv_thermal_throttle = true,
+	.support_ct_kill_exit = true,
+	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
+	.chain_noise_scale = 1000,
+	.wd_timeout = IWL_LONG_WD_TIMEOUT,
+	.max_event_log_size = 512,
+	.ucode_tracing = true,
+	.sensitivity_calib_by_driver = true,
+	.chain_noise_calib_by_driver = true,
+	.shadow_reg_enable = true,
+};
+
+static struct iwl_ht_params iwl2000_ht_params = {
+	.ht_greenfield_support = true,
+	.use_rts_for_aggregation = true, /* use rts/cts protection */
+};
+
+static struct iwl_bt_params iwl2030_bt_params = {
+	.bt_statistics = true,
+	/* Due to bluetooth, we transmit 2.4 GHz probes only on antenna A */
+	.advanced_bt_coexist = true,
+	.agg_time_limit = BT_AGG_THRESHOLD_DEF,
+	.bt_init_traffic_load = IWL_BT_COEX_TRAFFIC_LOAD_NONE,
+	.bt_prio_boost = IWLAGN_BT_PRIO_BOOST_DEFAULT,
+	.bt_sco_disable = true,
+	.bt_session_2 = true,
+};
+
+#define IWL_DEVICE_2000						\
+	.fw_name_pre = IWL2000_FW_PRE,				\
+	.ucode_api_max = IWL2000_UCODE_API_MAX,			\
+	.ucode_api_min = IWL2000_UCODE_API_MIN,			\
+	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
+	.ops = &iwl2000_ops,					\
+	.mod_params = &iwlagn_mod_params,			\
+	.base_params = &iwl2000_base_params,			\
+	.need_dc_calib = true,					\
+	.need_temp_offset_calib = true,				\
+	.led_mode = IWL_LED_RF_STATE,				\
+	.iq_invert = true					\
+
+struct iwl_cfg iwl2000_2bgn_cfg = {
+	.name = "2000 Series 2x2 BGN",
+	IWL_DEVICE_2000,
+	.ht_params = &iwl2000_ht_params,
+};
+
+struct iwl_cfg iwl2000_2bg_cfg = {
+	.name = "2000 Series 2x2 BG",
+	IWL_DEVICE_2000,
+};
+
+#define IWL_DEVICE_2030						\
+	.fw_name_pre = IWL2030_FW_PRE,				\
+	.ucode_api_max = IWL2030_UCODE_API_MAX,			\
+	.ucode_api_min = IWL2030_UCODE_API_MIN,			\
+	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
+	.ops = &iwl2030_ops,					\
+	.mod_params = &iwlagn_mod_params,			\
+	.base_params = &iwl2030_base_params,			\
+	.bt_params = &iwl2030_bt_params,			\
+	.need_dc_calib = true,					\
+	.need_temp_offset_calib = true,				\
+	.led_mode = IWL_LED_RF_STATE,				\
+	.adv_pm = true,						\
+	.iq_invert = true					\
+
+struct iwl_cfg iwl2030_2bgn_cfg = {
+	.name = "2000 Series 2x2 BGN/BT",
+	IWL_DEVICE_2030,
+	.ht_params = &iwl2000_ht_params,
+};
+
+struct iwl_cfg iwl2030_2bg_cfg = {
+	.name = "2000 Series 2x2 BG/BT",
+	IWL_DEVICE_2030,
+};
+
+#define IWL_DEVICE_6035						\
+	.fw_name_pre = IWL2030_FW_PRE,				\
+	.ucode_api_max = IWL2030_UCODE_API_MAX,			\
+	.ucode_api_min = IWL2030_UCODE_API_MIN,			\
+	.eeprom_ver = EEPROM_6035_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_6035_TX_POWER_VERSION,	\
+	.ops = &iwl2030_ops,					\
+	.mod_params = &iwlagn_mod_params,			\
+	.base_params = &iwl2030_base_params,			\
+	.bt_params = &iwl2030_bt_params,			\
+	.need_dc_calib = true,					\
+	.need_temp_offset_calib = true,				\
+	.led_mode = IWL_LED_RF_STATE,				\
+	.adv_pm = true						\
+
+struct iwl_cfg iwl6035_2agn_cfg = {
+	.name = "2000 Series 2x2 AGN/BT",
+	IWL_DEVICE_6035,
+	.ht_params = &iwl2000_ht_params,
+};
+
+struct iwl_cfg iwl6035_2abg_cfg = {
+	.name = "2000 Series 2x2 ABG/BT",
+	IWL_DEVICE_6035,
+};
+
+struct iwl_cfg iwl6035_2bg_cfg = {
+	.name = "2000 Series 2x2 BG/BT",
+	IWL_DEVICE_6035,
+};
+
+#define IWL_DEVICE_200						\
+	.fw_name_pre = IWL200_FW_PRE,				\
+	.ucode_api_max = IWL200_UCODE_API_MAX,			\
+	.ucode_api_min = IWL200_UCODE_API_MIN,			\
+	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
+	.ops = &iwl200_ops,					\
+	.mod_params = &iwlagn_mod_params,			\
+	.base_params = &iwl2000_base_params,			\
+	.need_dc_calib = true,					\
+	.need_temp_offset_calib = true,				\
+	.led_mode = IWL_LED_RF_STATE,				\
+	.adv_pm = true,						\
+	.rx_with_siso_diversity = true				\
+
+struct iwl_cfg iwl200_bg_cfg = {
+	.name = "200 Series 1x1 BG",
+	IWL_DEVICE_200,
+};
+
+struct iwl_cfg iwl200_bgn_cfg = {
+	.name = "200 Series 1x1 BGN",
+	IWL_DEVICE_200,
+	.ht_params = &iwl2000_ht_params,
+};
+
+#define IWL_DEVICE_230						\
+	.fw_name_pre = IWL200_FW_PRE,				\
+	.ucode_api_max = IWL200_UCODE_API_MAX,			\
+	.ucode_api_min = IWL200_UCODE_API_MIN,			\
+	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
+	.ops = &iwl230_ops,					\
+	.mod_params = &iwlagn_mod_params,			\
+	.base_params = &iwl2030_base_params,			\
+	.bt_params = &iwl2030_bt_params,			\
+	.need_dc_calib = true,					\
+	.need_temp_offset_calib = true,				\
+	.led_mode = IWL_LED_RF_STATE,				\
+	.adv_pm = true,						\
+	.rx_with_siso_diversity = true				\
+
+struct iwl_cfg iwl230_bg_cfg = {
+	.name = "200 Series 1x1 BG/BT",
+	IWL_DEVICE_230,
+};
+
+struct iwl_cfg iwl230_bgn_cfg = {
+	.name = "200 Series 1x1 BGN/BT",
+	IWL_DEVICE_230,
+	.ht_params = &iwl2000_ht_params,
+};
+
+MODULE_FIRMWARE(IWL2000_MODULE_FIRMWARE(IWL2000_UCODE_API_MAX));
+MODULE_FIRMWARE(IWL2030_MODULE_FIRMWARE(IWL2030_UCODE_API_MAX));
+MODULE_FIRMWARE(IWL200_MODULE_FIRMWARE(IWL200_UCODE_API_MAX));
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index 79ab0a6b1386..3ea31b659d1a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -51,7 +51,7 @@
 #include "iwl-agn-debugfs.h"
 
 /* Highest firmware API version supported */
-#define IWL5000_UCODE_API_MAX 2
+#define IWL5000_UCODE_API_MAX 5
 #define IWL5150_UCODE_API_MAX 2
 
 /* Lowest firmware API version supported */
@@ -402,8 +402,6 @@ static struct iwl_lib_ops iwl5000_lib = {
 		.bt_stats_read = iwl_ucode_bt_stats_read,
 		.reply_tx_error = iwl_reply_tx_error_read,
 	},
-	.check_plcp_health = iwl_good_plcp_health,
-	.check_ack_health = iwl_good_ack_health,
 	.txfifo_flush = iwlagn_txfifo_flush,
 	.dev_txfifo_flush = iwlagn_dev_txfifo_flush,
 	.tt_ops = {
@@ -471,8 +469,6 @@ static struct iwl_lib_ops iwl5150_lib = {
 		.bt_stats_read = iwl_ucode_bt_stats_read,
 		.reply_tx_error = iwl_reply_tx_error_read,
 	},
-	.check_plcp_health = iwl_good_plcp_health,
-	.check_ack_health = iwl_good_ack_health,
 	.txfifo_flush = iwlagn_txfifo_flush,
 	.dev_txfifo_flush = iwlagn_dev_txfifo_flush,
 	.tt_ops = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/iwlwifi/iwl-6000.c
index ef36aff1bb43..a745b01c0ec1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-6000.c
@@ -67,13 +67,13 @@
 #define _IWL6050_MODULE_FIRMWARE(api) IWL6050_FW_PRE #api ".ucode"
 #define IWL6050_MODULE_FIRMWARE(api) _IWL6050_MODULE_FIRMWARE(api)
 
-#define IWL6000G2A_FW_PRE "iwlwifi-6000g2a-"
-#define _IWL6000G2A_MODULE_FIRMWARE(api) IWL6000G2A_FW_PRE #api ".ucode"
-#define IWL6000G2A_MODULE_FIRMWARE(api) _IWL6000G2A_MODULE_FIRMWARE(api)
+#define IWL6005_FW_PRE "iwlwifi-6000g2a-"
+#define _IWL6005_MODULE_FIRMWARE(api) IWL6005_FW_PRE #api ".ucode"
+#define IWL6005_MODULE_FIRMWARE(api) _IWL6005_MODULE_FIRMWARE(api)
 
-#define IWL6000G2B_FW_PRE "iwlwifi-6000g2b-"
-#define _IWL6000G2B_MODULE_FIRMWARE(api) IWL6000G2B_FW_PRE #api ".ucode"
-#define IWL6000G2B_MODULE_FIRMWARE(api) _IWL6000G2B_MODULE_FIRMWARE(api)
+#define IWL6030_FW_PRE "iwlwifi-6000g2b-"
+#define _IWL6030_MODULE_FIRMWARE(api) IWL6030_FW_PRE #api ".ucode"
+#define IWL6030_MODULE_FIRMWARE(api) _IWL6030_MODULE_FIRMWARE(api)
 
 static void iwl6000_set_ct_threshold(struct iwl_priv *priv)
 {
@@ -90,7 +90,7 @@ static void iwl6050_additional_nic_config(struct iwl_priv *priv)
 				CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
 }
 
-static void iwl6050g2_additional_nic_config(struct iwl_priv *priv)
+static void iwl6150_additional_nic_config(struct iwl_priv *priv)
 {
 	/* Indicate calibration version to uCode. */
 	if (priv->cfg->ops->lib->eeprom_ops.calib_version(priv) >= 6)
@@ -343,8 +343,6 @@ static struct iwl_lib_ops iwl6000_lib = {
 		.bt_stats_read = iwl_ucode_bt_stats_read,
 		.reply_tx_error = iwl_reply_tx_error_read,
 	},
-	.check_plcp_health = iwl_good_plcp_health,
-	.check_ack_health = iwl_good_ack_health,
 	.txfifo_flush = iwlagn_txfifo_flush,
 	.dev_txfifo_flush = iwlagn_dev_txfifo_flush,
 	.tt_ops = {
@@ -354,7 +352,7 @@ static struct iwl_lib_ops iwl6000_lib = {
 	}
 };
 
-static struct iwl_lib_ops iwl6000g2b_lib = {
+static struct iwl_lib_ops iwl6030_lib = {
 	.set_hw_params = iwl6000_hw_set_hw_params,
 	.txq_update_byte_cnt_tbl = iwlagn_txq_update_byte_cnt_tbl,
 	.txq_inval_byte_cnt_tbl = iwlagn_txq_inval_byte_cnt_tbl,
@@ -415,8 +413,6 @@ static struct iwl_lib_ops iwl6000g2b_lib = {
 		.bt_stats_read = iwl_ucode_bt_stats_read,
 		.reply_tx_error = iwl_reply_tx_error_read,
 	},
-	.check_plcp_health = iwl_good_plcp_health,
-	.check_ack_health = iwl_good_ack_health,
 	.txfifo_flush = iwlagn_txfifo_flush,
 	.dev_txfifo_flush = iwlagn_dev_txfifo_flush,
 	.tt_ops = {
@@ -430,8 +426,8 @@ static struct iwl_nic_ops iwl6050_nic_ops = {
 	.additional_nic_config = &iwl6050_additional_nic_config,
 };
 
-static struct iwl_nic_ops iwl6050g2_nic_ops = {
-	.additional_nic_config = &iwl6050g2_additional_nic_config,
+static struct iwl_nic_ops iwl6150_nic_ops = {
+	.additional_nic_config = &iwl6150_additional_nic_config,
 };
 
 static const struct iwl_ops iwl6000_ops = {
@@ -451,17 +447,17 @@ static const struct iwl_ops iwl6050_ops = {
 	.ieee80211_ops = &iwlagn_hw_ops,
 };
 
-static const struct iwl_ops iwl6050g2_ops = {
+static const struct iwl_ops iwl6150_ops = {
 	.lib = &iwl6000_lib,
 	.hcmd = &iwlagn_hcmd,
 	.utils = &iwlagn_hcmd_utils,
 	.led = &iwlagn_led_ops,
-	.nic = &iwl6050g2_nic_ops,
+	.nic = &iwl6150_nic_ops,
 	.ieee80211_ops = &iwlagn_hw_ops,
 };
 
-static const struct iwl_ops iwl6000g2b_ops = {
-	.lib = &iwl6000g2b_lib,
+static const struct iwl_ops iwl6030_ops = {
+	.lib = &iwl6030_lib,
 	.hcmd = &iwlagn_bt_hcmd,
 	.utils = &iwlagn_hcmd_utils,
 	.led = &iwlagn_led_ops,
@@ -479,7 +475,6 @@ static struct iwl_base_params iwl6000_base_params = {
 	.shadow_ram_support = true,
 	.led_compensation = 51,
 	.chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-	.supports_idle = true,
 	.adv_thermal_throttle = true,
 	.support_ct_kill_exit = true,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -503,7 +498,6 @@ static struct iwl_base_params iwl6050_base_params = {
 	.shadow_ram_support = true,
 	.led_compensation = 51,
 	.chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-	.supports_idle = true,
 	.adv_thermal_throttle = true,
 	.support_ct_kill_exit = true,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -526,7 +520,6 @@ static struct iwl_base_params iwl6000_g2_base_params = {
 	.shadow_ram_support = true,
 	.led_compensation = 57,
 	.chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-	.supports_idle = true,
 	.adv_thermal_throttle = true,
 	.support_ct_kill_exit = true,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -555,11 +548,11 @@ static struct iwl_bt_params iwl6000_bt_params = {
 };
 
 #define IWL_DEVICE_6005						\
-	.fw_name_pre = IWL6000G2A_FW_PRE,			\
+	.fw_name_pre = IWL6005_FW_PRE,			\
 	.ucode_api_max = IWL6000G2_UCODE_API_MAX,		\
 	.ucode_api_min = IWL6000G2_UCODE_API_MIN,		\
-	.eeprom_ver = EEPROM_6000G2_EEPROM_VERSION,		\
-	.eeprom_calib_ver = EEPROM_6000G2_TX_POWER_VERSION,	\
+	.eeprom_ver = EEPROM_6005_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_6005_TX_POWER_VERSION,	\
 	.ops = &iwl6000_ops,					\
 	.mod_params = &iwlagn_mod_params,			\
 	.base_params = &iwl6000_g2_base_params,			\
@@ -584,12 +577,12 @@ struct iwl_cfg iwl6005_2bg_cfg = {
 };
 
 #define IWL_DEVICE_6030						\
-	.fw_name_pre = IWL6000G2B_FW_PRE,			\
+	.fw_name_pre = IWL6030_FW_PRE,			\
 	.ucode_api_max = IWL6000G2_UCODE_API_MAX,		\
 	.ucode_api_min = IWL6000G2_UCODE_API_MIN,		\
-	.eeprom_ver = EEPROM_6000G2_EEPROM_VERSION,		\
-	.eeprom_calib_ver = EEPROM_6000G2_TX_POWER_VERSION,	\
-	.ops = &iwl6000g2b_ops,					\
+	.eeprom_ver = EEPROM_6030_EEPROM_VERSION,		\
+	.eeprom_calib_ver = EEPROM_6030_TX_POWER_VERSION,	\
+	.ops = &iwl6030_ops,					\
 	.mod_params = &iwlagn_mod_params,			\
 	.base_params = &iwl6000_g2_base_params,			\
 	.bt_params = &iwl6000_bt_params,			\
@@ -708,9 +701,9 @@ struct iwl_cfg iwl6150_bgn_cfg = {
 	.fw_name_pre = IWL6050_FW_PRE,
 	.ucode_api_max = IWL6050_UCODE_API_MAX,
 	.ucode_api_min = IWL6050_UCODE_API_MIN,
-	.eeprom_ver = EEPROM_6050G2_EEPROM_VERSION,
-	.eeprom_calib_ver = EEPROM_6050G2_TX_POWER_VERSION,
-	.ops = &iwl6050g2_ops,
+	.eeprom_ver = EEPROM_6150_EEPROM_VERSION,
+	.eeprom_calib_ver = EEPROM_6150_TX_POWER_VERSION,
+	.ops = &iwl6150_ops,
 	.mod_params = &iwlagn_mod_params,
 	.base_params = &iwl6050_base_params,
 	.ht_params = &iwl6000_ht_params,
@@ -736,5 +729,5 @@ struct iwl_cfg iwl6000_3agn_cfg = {
 
 MODULE_FIRMWARE(IWL6000_MODULE_FIRMWARE(IWL6000_UCODE_API_MAX));
 MODULE_FIRMWARE(IWL6050_MODULE_FIRMWARE(IWL6050_UCODE_API_MAX));
-MODULE_FIRMWARE(IWL6000G2A_MODULE_FIRMWARE(IWL6000G2_UCODE_API_MAX));
-MODULE_FIRMWARE(IWL6000G2B_MODULE_FIRMWARE(IWL6000G2_UCODE_API_MAX));
+MODULE_FIRMWARE(IWL6005_MODULE_FIRMWARE(IWL6000G2_UCODE_API_MAX));
+MODULE_FIRMWARE(IWL6030_MODULE_FIRMWARE(IWL6000G2_UCODE_API_MAX));
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c b/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
index d16bb5ede014..9006293e740c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
@@ -631,8 +631,7 @@ void iwl_sensitivity_calibration(struct iwl_priv *priv, void *resp)
 	}
 
 	spin_lock_irqsave(&priv->lock, flags);
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
+	if (iwl_bt_statistics(priv)) {
 		rx_info = &(((struct iwl_bt_notif_statistics *)resp)->
 			      rx.general.common);
 		ofdm = &(((struct iwl_bt_notif_statistics *)resp)->rx.ofdm);
@@ -897,8 +896,7 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv, void *stat_resp)
 	}
 
 	spin_lock_irqsave(&priv->lock, flags);
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
+	if (iwl_bt_statistics(priv)) {
 		rx_info = &(((struct iwl_bt_notif_statistics *)stat_resp)->
 			      rx.general.common);
 	} else {
@@ -913,8 +911,7 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv, void *stat_resp)
 
 	rxon_band24 = !!(ctx->staging.flags & RXON_FLG_BAND_24G_MSK);
 	rxon_chnum = le16_to_cpu(ctx->staging.channel);
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
+	if (iwl_bt_statistics(priv)) {
 		stat_band24 = !!(((struct iwl_bt_notif_statistics *)
 				 stat_resp)->flag &
 				 STATISTICS_REPLY_FLG_BAND_24G_MSK);
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-debugfs.c b/drivers/net/wireless/iwlwifi/iwl-agn-debugfs.c
index a6dbd8983dac..b500aaae53ec 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-debugfs.c
@@ -39,8 +39,7 @@ static int iwl_statistics_flag(struct iwl_priv *priv, char *buf, int bufsz)
 	int p = 0;
 	u32 flag;
 
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics)
+	if (iwl_bt_statistics(priv))
 		flag = le32_to_cpu(priv->_agn.statistics_bt.flag);
 	else
 		flag = le32_to_cpu(priv->_agn.statistics.flag);
@@ -89,8 +88,7 @@ ssize_t iwl_ucode_rx_stats_read(struct file *file, char __user *user_buf,
 	 * the last statistics notification from uCode
 	 * might not reflect the current uCode activity
 	 */
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
+	if (iwl_bt_statistics(priv)) {
 		ofdm = &priv->_agn.statistics_bt.rx.ofdm;
 		cck = &priv->_agn.statistics_bt.rx.cck;
 		general = &priv->_agn.statistics_bt.rx.general.common;
@@ -536,8 +534,7 @@ ssize_t iwl_ucode_tx_stats_read(struct file *file,
 	  * the last statistics notification from uCode
 	  * might not reflect the current uCode activity
 	  */
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
+	if (iwl_bt_statistics(priv)) {
 		tx = &priv->_agn.statistics_bt.tx;
 		accum_tx = &priv->_agn.accum_statistics_bt.tx;
 		delta_tx = &priv->_agn.delta_statistics_bt.tx;
@@ -737,8 +734,7 @@ ssize_t iwl_ucode_general_stats_read(struct file *file, char __user *user_buf,
 	  * the last statistics notification from uCode
 	  * might not reflect the current uCode activity
 	  */
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->bt_statistics) {
+	if (iwl_bt_statistics(priv)) {
 		general = &priv->_agn.statistics_bt.general.common;
 		dbg = &priv->_agn.statistics_bt.general.common.dbg;
 		div = &priv->_agn.statistics_bt.general.common.div;
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-hcmd.c b/drivers/net/wireless/iwlwifi/iwl-agn-hcmd.c
index 366340f3fb0f..41543ad4cb84 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-hcmd.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-hcmd.c
@@ -305,7 +305,11 @@ static int iwlagn_set_pan_params(struct iwl_priv *priv)
 	cmd.slots[0].type = 0; /* BSS */
 	cmd.slots[1].type = 1; /* PAN */
 
-	if (ctx_bss->vif && ctx_pan->vif) {
+	if (priv->_agn.hw_roc_channel) {
+		/* both contexts must be used for this to happen */
+		slot1 = priv->_agn.hw_roc_duration;
+		slot0 = IWL_MIN_SLOT_TIME;
+	} else if (ctx_bss->vif && ctx_pan->vif) {
 		int bcnint = ctx_pan->vif->bss_conf.beacon_int;
 		int dtim = ctx_pan->vif->bss_conf.dtim_period ?: 1;
 
@@ -330,12 +334,12 @@ static int iwlagn_set_pan_params(struct iwl_priv *priv)
 		if (test_bit(STATUS_SCAN_HW, &priv->status) ||
 		    (!ctx_bss->vif->bss_conf.idle &&
 		     !ctx_bss->vif->bss_conf.assoc)) {
-			slot0 = dtim * bcnint * 3 - 20;
-			slot1 = 20;
+			slot0 = dtim * bcnint * 3 - IWL_MIN_SLOT_TIME;
+			slot1 = IWL_MIN_SLOT_TIME;
 		} else if (!ctx_pan->vif->bss_conf.idle &&
 			   !ctx_pan->vif->bss_conf.assoc) {
-			slot1 = bcnint * 3 - 20;
-			slot0 = 20;
+			slot1 = bcnint * 3 - IWL_MIN_SLOT_TIME;
+			slot0 = IWL_MIN_SLOT_TIME;
 		}
 	} else if (ctx_pan->vif) {
 		slot0 = 0;
@@ -344,8 +348,8 @@ static int iwlagn_set_pan_params(struct iwl_priv *priv)
 		slot1 = max_t(int, DEFAULT_BEACON_INTERVAL, slot1);
 
 		if (test_bit(STATUS_SCAN_HW, &priv->status)) {
-			slot0 = slot1 * 3 - 20;
-			slot1 = 20;
+			slot0 = slot1 * 3 - IWL_MIN_SLOT_TIME;
+			slot1 = IWL_MIN_SLOT_TIME;
 		}
 	}
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-led.c b/drivers/net/wireless/iwlwifi/iwl-agn-led.c
index 1a24946bc203..c1190d965614 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-led.c
@@ -63,23 +63,11 @@ static int iwl_send_led_cmd(struct iwl_priv *priv, struct iwl_led_cmd *led_cmd)
 }
 
 /* Set led register off */
-static int iwl_led_on_reg(struct iwl_priv *priv)
+void iwlagn_led_enable(struct iwl_priv *priv)
 {
-	IWL_DEBUG_LED(priv, "led on\n");
 	iwl_write32(priv, CSR_LED_REG, CSR_LED_REG_TRUN_ON);
-	return 0;
-}
-
-/* Set led register off */
-static int iwl_led_off_reg(struct iwl_priv *priv)
-{
-	IWL_DEBUG_LED(priv, "LED Reg off\n");
-	iwl_write32(priv, CSR_LED_REG, CSR_LED_REG_TRUN_OFF);
-	return 0;
 }
 
 const struct iwl_led_ops iwlagn_led_ops = {
 	.cmd = iwl_send_led_cmd,
-	.on = iwl_led_on_reg,
-	.off = iwl_led_off_reg,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-led.h b/drivers/net/wireless/iwlwifi/iwl-agn-led.h
index a594e4fdc6b8..96f323dc5dd6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-led.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-led.h
@@ -28,5 +28,6 @@
 #define __iwl_agn_led_h__
 
 extern const struct iwl_led_ops iwlagn_led_ops;
+void iwlagn_led_enable(struct iwl_priv *priv);
 
 #endif /* __iwl_agn_led_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
index 3dee87e8f55d..25fccf9a3001 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
@@ -473,6 +473,11 @@ void iwlagn_rx_handler_setup(struct iwl_priv *priv)
 	priv->rx_handlers[CALIBRATION_COMPLETE_NOTIFICATION] =
 					iwlagn_rx_calib_complete;
 	priv->rx_handlers[REPLY_TX] = iwlagn_rx_reply_tx;
+
+	/* set up notification wait support */
+	spin_lock_init(&priv->_agn.notif_wait_lock);
+	INIT_LIST_HEAD(&priv->_agn.notif_waits);
+	init_waitqueue_head(&priv->_agn.notif_waitq);
 }
 
 void iwlagn_setup_deferred_work(struct iwl_priv *priv)
@@ -528,9 +533,10 @@ int iwlagn_send_tx_power(struct iwl_priv *priv)
 
 void iwlagn_temperature(struct iwl_priv *priv)
 {
-	/* store temperature from statistics (in Celsius) */
-	priv->temperature =
-		le32_to_cpu(priv->_agn.statistics.general.common.temperature);
+	/* store temperature from correct statistics (in Celsius) */
+	priv->temperature = le32_to_cpu((iwl_bt_statistics(priv)) ?
+		priv->_agn.statistics_bt.general.common.temperature :
+		priv->_agn.statistics.general.common.temperature);
 	iwl_tt_handler(priv);
 }
 
@@ -604,6 +610,7 @@ const u8 *iwlagn_eeprom_query_addr(const struct iwl_priv *priv,
 struct iwl_mod_params iwlagn_mod_params = {
 	.amsdu_size_8K = 1,
 	.restart_fw = 1,
+	.plcp_check = true,
 	/* the rest are 0 by default */
 };
 
@@ -988,240 +995,6 @@ int iwlagn_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band)
 	return -1;
 }
 
-/* Calc max signal level (dBm) among 3 possible receivers */
-static inline int iwlagn_calc_rssi(struct iwl_priv *priv,
-				struct iwl_rx_phy_res *rx_resp)
-{
-	return priv->cfg->ops->utils->calc_rssi(priv, rx_resp);
-}
-
-static u32 iwlagn_translate_rx_status(struct iwl_priv *priv, u32 decrypt_in)
-{
-	u32 decrypt_out = 0;
-
-	if ((decrypt_in & RX_RES_STATUS_STATION_FOUND) ==
-					RX_RES_STATUS_STATION_FOUND)
-		decrypt_out |= (RX_RES_STATUS_STATION_FOUND |
-				RX_RES_STATUS_NO_STATION_INFO_MISMATCH);
-
-	decrypt_out |= (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK);
-
-	/* packet was not encrypted */
-	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
-					RX_RES_STATUS_SEC_TYPE_NONE)
-		return decrypt_out;
-
-	/* packet was encrypted with unknown alg */
-	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
-					RX_RES_STATUS_SEC_TYPE_ERR)
-		return decrypt_out;
-
-	/* decryption was not done in HW */
-	if ((decrypt_in & RX_MPDU_RES_STATUS_DEC_DONE_MSK) !=
-					RX_MPDU_RES_STATUS_DEC_DONE_MSK)
-		return decrypt_out;
-
-	switch (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) {
-
-	case RX_RES_STATUS_SEC_TYPE_CCMP:
-		/* alg is CCM: check MIC only */
-		if (!(decrypt_in & RX_MPDU_RES_STATUS_MIC_OK))
-			/* Bad MIC */
-			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
-		else
-			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
-
-		break;
-
-	case RX_RES_STATUS_SEC_TYPE_TKIP:
-		if (!(decrypt_in & RX_MPDU_RES_STATUS_TTAK_OK)) {
-			/* Bad TTAK */
-			decrypt_out |= RX_RES_STATUS_BAD_KEY_TTAK;
-			break;
-		}
-		/* fall through if TTAK OK */
-	default:
-		if (!(decrypt_in & RX_MPDU_RES_STATUS_ICV_OK))
-			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
-		else
-			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
-		break;
-	}
-
-	IWL_DEBUG_RX(priv, "decrypt_in:0x%x  decrypt_out = 0x%x\n",
-					decrypt_in, decrypt_out);
-
-	return decrypt_out;
-}
-
-static void iwlagn_pass_packet_to_mac80211(struct iwl_priv *priv,
-					struct ieee80211_hdr *hdr,
-					u16 len,
-					u32 ampdu_status,
-					struct iwl_rx_mem_buffer *rxb,
-					struct ieee80211_rx_status *stats)
-{
-	struct sk_buff *skb;
-	__le16 fc = hdr->frame_control;
-
-	/* We only process data packets if the interface is open */
-	if (unlikely(!priv->is_open)) {
-		IWL_DEBUG_DROP_LIMIT(priv,
-		    "Dropping packet while interface is not open.\n");
-		return;
-	}
-
-	/* In case of HW accelerated crypto and bad decryption, drop */
-	if (!priv->cfg->mod_params->sw_crypto &&
-	    iwl_set_decrypted_flag(priv, hdr, ampdu_status, stats))
-		return;
-
-	skb = dev_alloc_skb(128);
-	if (!skb) {
-		IWL_ERR(priv, "dev_alloc_skb failed\n");
-		return;
-	}
-
-	skb_add_rx_frag(skb, 0, rxb->page, (void *)hdr - rxb_addr(rxb), len);
-
-	iwl_update_stats(priv, false, fc, len);
-	memcpy(IEEE80211_SKB_RXCB(skb), stats, sizeof(*stats));
-
-	ieee80211_rx(priv->hw, skb);
-	priv->alloc_rxb_page--;
-	rxb->page = NULL;
-}
-
-/* Called for REPLY_RX (legacy ABG frames), or
- * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
-void iwlagn_rx_reply_rx(struct iwl_priv *priv,
-				struct iwl_rx_mem_buffer *rxb)
-{
-	struct ieee80211_hdr *header;
-	struct ieee80211_rx_status rx_status;
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_rx_phy_res *phy_res;
-	__le32 rx_pkt_status;
-	struct iwl_rx_mpdu_res_start *amsdu;
-	u32 len;
-	u32 ampdu_status;
-	u32 rate_n_flags;
-
-	/**
-	 * REPLY_RX and REPLY_RX_MPDU_CMD are handled differently.
-	 *	REPLY_RX: physical layer info is in this buffer
-	 *	REPLY_RX_MPDU_CMD: physical layer info was sent in separate
-	 *		command and cached in priv->last_phy_res
-	 *
-	 * Here we set up local variables depending on which command is
-	 * received.
-	 */
-	if (pkt->hdr.cmd == REPLY_RX) {
-		phy_res = (struct iwl_rx_phy_res *)pkt->u.raw;
-		header = (struct ieee80211_hdr *)(pkt->u.raw + sizeof(*phy_res)
-				+ phy_res->cfg_phy_cnt);
-
-		len = le16_to_cpu(phy_res->byte_count);
-		rx_pkt_status = *(__le32 *)(pkt->u.raw + sizeof(*phy_res) +
-				phy_res->cfg_phy_cnt + len);
-		ampdu_status = le32_to_cpu(rx_pkt_status);
-	} else {
-		if (!priv->_agn.last_phy_res_valid) {
-			IWL_ERR(priv, "MPDU frame without cached PHY data\n");
-			return;
-		}
-		phy_res = &priv->_agn.last_phy_res;
-		amsdu = (struct iwl_rx_mpdu_res_start *)pkt->u.raw;
-		header = (struct ieee80211_hdr *)(pkt->u.raw + sizeof(*amsdu));
-		len = le16_to_cpu(amsdu->byte_count);
-		rx_pkt_status = *(__le32 *)(pkt->u.raw + sizeof(*amsdu) + len);
-		ampdu_status = iwlagn_translate_rx_status(priv,
-				le32_to_cpu(rx_pkt_status));
-	}
-
-	if ((unlikely(phy_res->cfg_phy_cnt > 20))) {
-		IWL_DEBUG_DROP(priv, "dsp size out of range [0,20]: %d/n",
-				phy_res->cfg_phy_cnt);
-		return;
-	}
-
-	if (!(rx_pkt_status & RX_RES_STATUS_NO_CRC32_ERROR) ||
-	    !(rx_pkt_status & RX_RES_STATUS_NO_RXE_OVERFLOW)) {
-		IWL_DEBUG_RX(priv, "Bad CRC or FIFO: 0x%08X.\n",
-				le32_to_cpu(rx_pkt_status));
-		return;
-	}
-
-	/* This will be used in several places later */
-	rate_n_flags = le32_to_cpu(phy_res->rate_n_flags);
-
-	/* rx_status carries information about the packet to mac80211 */
-	rx_status.mactime = le64_to_cpu(phy_res->timestamp);
-	rx_status.freq =
-		ieee80211_channel_to_frequency(le16_to_cpu(phy_res->channel));
-	rx_status.band = (phy_res->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
-				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
-	rx_status.rate_idx =
-		iwlagn_hwrate_to_mac80211_idx(rate_n_flags, rx_status.band);
-	rx_status.flag = 0;
-
-	/* TSF isn't reliable. In order to allow smooth user experience,
-	 * this W/A doesn't propagate it to the mac80211 */
-	/*rx_status.flag |= RX_FLAG_TSFT;*/
-
-	priv->ucode_beacon_time = le32_to_cpu(phy_res->beacon_time_stamp);
-
-	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
-	rx_status.signal = iwlagn_calc_rssi(priv, phy_res);
-
-	iwl_dbg_log_rx_data_frame(priv, len, header);
-	IWL_DEBUG_STATS_LIMIT(priv, "Rssi %d, TSF %llu\n",
-		rx_status.signal, (unsigned long long)rx_status.mactime);
-
-	/*
-	 * "antenna number"
-	 *
-	 * It seems that the antenna field in the phy flags value
-	 * is actually a bit field. This is undefined by radiotap,
-	 * it wants an actual antenna number but I always get "7"
-	 * for most legacy frames I receive indicating that the
-	 * same frame was received on all three RX chains.
-	 *
-	 * I think this field should be removed in favor of a
-	 * new 802.11n radiotap field "RX chains" that is defined
-	 * as a bitmask.
-	 */
-	rx_status.antenna =
-		(le16_to_cpu(phy_res->phy_flags) & RX_RES_PHY_FLAGS_ANTENNA_MSK)
-		>> RX_RES_PHY_FLAGS_ANTENNA_POS;
-
-	/* set the preamble flag if appropriate */
-	if (phy_res->phy_flags & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
-		rx_status.flag |= RX_FLAG_SHORTPRE;
-
-	/* Set up the HT phy flags */
-	if (rate_n_flags & RATE_MCS_HT_MSK)
-		rx_status.flag |= RX_FLAG_HT;
-	if (rate_n_flags & RATE_MCS_HT40_MSK)
-		rx_status.flag |= RX_FLAG_40MHZ;
-	if (rate_n_flags & RATE_MCS_SGI_MSK)
-		rx_status.flag |= RX_FLAG_SHORT_GI;
-
-	iwlagn_pass_packet_to_mac80211(priv, header, len, ampdu_status,
-				    rxb, &rx_status);
-}
-
-/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
- * This will be used later in iwl_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
-void iwlagn_rx_reply_rx_phy(struct iwl_priv *priv,
-			    struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	priv->_agn.last_phy_res_valid = true;
-	memcpy(&priv->_agn.last_phy_res, pkt->u.raw,
-	       sizeof(struct iwl_rx_phy_res));
-}
-
 static int iwl_get_single_channel_for_scan(struct iwl_priv *priv,
 					   struct ieee80211_vif *vif,
 					   enum ieee80211_band band,
@@ -1389,15 +1162,12 @@ int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 		u32 extra;
 		u32 suspend_time = 100;
 		u32 scan_suspend_time = 100;
-		unsigned long flags;
 
 		IWL_DEBUG_INFO(priv, "Scanning while associated...\n");
-		spin_lock_irqsave(&priv->lock, flags);
 		if (priv->is_internal_short_scan)
 			interval = 0;
 		else
 			interval = vif->bss_conf.beacon_int;
-		spin_unlock_irqrestore(&priv->lock, flags);
 
 		scan->suspend_time = 0;
 		scan->max_out_time = cpu_to_le32(200 * 1024);
@@ -1801,26 +1571,39 @@ static const __le32 iwlagn_concurrent_lookup[12] = {
 
 void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 {
-	struct iwlagn_bt_cmd bt_cmd = {
+	struct iwl_basic_bt_cmd basic = {
 		.max_kill = IWLAGN_BT_MAX_KILL_DEFAULT,
 		.bt3_timer_t7_value = IWLAGN_BT3_T7_DEFAULT,
 		.bt3_prio_sample_time = IWLAGN_BT3_PRIO_SAMPLE_DEFAULT,
 		.bt3_timer_t2_value = IWLAGN_BT3_T2_DEFAULT,
 	};
+	struct iwl6000_bt_cmd bt_cmd_6000;
+	struct iwl2000_bt_cmd bt_cmd_2000;
+	int ret;
 
 	BUILD_BUG_ON(sizeof(iwlagn_def_3w_lookup) !=
-			sizeof(bt_cmd.bt3_lookup_table));
-
-	if (priv->cfg->bt_params)
-		bt_cmd.prio_boost = priv->cfg->bt_params->bt_prio_boost;
-	else
-		bt_cmd.prio_boost = 0;
-	bt_cmd.kill_ack_mask = priv->kill_ack_mask;
-	bt_cmd.kill_cts_mask = priv->kill_cts_mask;
+			sizeof(basic.bt3_lookup_table));
+
+	if (priv->cfg->bt_params) {
+		if (priv->cfg->bt_params->bt_session_2) {
+			bt_cmd_2000.prio_boost = cpu_to_le32(
+				priv->cfg->bt_params->bt_prio_boost);
+			bt_cmd_2000.tx_prio_boost = 0;
+			bt_cmd_2000.rx_prio_boost = 0;
+		} else {
+			bt_cmd_6000.prio_boost =
+				priv->cfg->bt_params->bt_prio_boost;
+			bt_cmd_6000.tx_prio_boost = 0;
+			bt_cmd_6000.rx_prio_boost = 0;
+		}
+	} else {
+		IWL_ERR(priv, "failed to construct BT Coex Config\n");
+		return;
+	}
 
-	bt_cmd.valid = priv->bt_valid;
-	bt_cmd.tx_prio_boost = 0;
-	bt_cmd.rx_prio_boost = 0;
+	basic.kill_ack_mask = priv->kill_ack_mask;
+	basic.kill_cts_mask = priv->kill_cts_mask;
+	basic.valid = priv->bt_valid;
 
 	/*
 	 * Configure BT coex mode to "no coexistence" when the
@@ -1829,49 +1612,45 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 	 * IBSS mode (no proper uCode support for coex then).
 	 */
 	if (!bt_coex_active || priv->iw_mode == NL80211_IFTYPE_ADHOC) {
-		bt_cmd.flags = 0;
+		basic.flags = IWLAGN_BT_FLAG_COEX_MODE_DISABLED;
 	} else {
-		bt_cmd.flags = IWLAGN_BT_FLAG_COEX_MODE_3W <<
+		basic.flags = IWLAGN_BT_FLAG_COEX_MODE_3W <<
 					IWLAGN_BT_FLAG_COEX_MODE_SHIFT;
 		if (priv->cfg->bt_params &&
 		    priv->cfg->bt_params->bt_sco_disable)
-			bt_cmd.flags |= IWLAGN_BT_FLAG_SYNC_2_BT_DISABLE;
+			basic.flags |= IWLAGN_BT_FLAG_SYNC_2_BT_DISABLE;
 
 		if (priv->bt_ch_announce)
-			bt_cmd.flags |= IWLAGN_BT_FLAG_CHANNEL_INHIBITION;
-		IWL_DEBUG_INFO(priv, "BT coex flag: 0X%x\n", bt_cmd.flags);
+			basic.flags |= IWLAGN_BT_FLAG_CHANNEL_INHIBITION;
+		IWL_DEBUG_INFO(priv, "BT coex flag: 0X%x\n", basic.flags);
 	}
-	priv->bt_enable_flag = bt_cmd.flags;
+	priv->bt_enable_flag = basic.flags;
 	if (priv->bt_full_concurrent)
-		memcpy(bt_cmd.bt3_lookup_table, iwlagn_concurrent_lookup,
+		memcpy(basic.bt3_lookup_table, iwlagn_concurrent_lookup,
 			sizeof(iwlagn_concurrent_lookup));
 	else
-		memcpy(bt_cmd.bt3_lookup_table, iwlagn_def_3w_lookup,
+		memcpy(basic.bt3_lookup_table, iwlagn_def_3w_lookup,
 			sizeof(iwlagn_def_3w_lookup));
 
 	IWL_DEBUG_INFO(priv, "BT coex %s in %s mode\n",
-		       bt_cmd.flags ? "active" : "disabled",
+		       basic.flags ? "active" : "disabled",
 		       priv->bt_full_concurrent ?
 		       "full concurrency" : "3-wire");
 
-	if (iwl_send_cmd_pdu(priv, REPLY_BT_CONFIG, sizeof(bt_cmd), &bt_cmd))
+	if (priv->cfg->bt_params->bt_session_2) {
+		memcpy(&bt_cmd_2000.basic, &basic,
+			sizeof(basic));
+		ret = iwl_send_cmd_pdu(priv, REPLY_BT_CONFIG,
+			sizeof(bt_cmd_2000), &bt_cmd_2000);
+	} else {
+		memcpy(&bt_cmd_6000.basic, &basic,
+			sizeof(basic));
+		ret = iwl_send_cmd_pdu(priv, REPLY_BT_CONFIG,
+			sizeof(bt_cmd_6000), &bt_cmd_6000);
+	}
+	if (ret)
 		IWL_ERR(priv, "failed to send BT Coex Config\n");
 
-	/*
-	 * When we are doing a restart, need to also reconfigure BT
-	 * SCO to the device. If not doing a restart, bt_sco_active
-	 * will always be false, so there's no need to have an extra
-	 * variable to check for it.
-	 */
-	if (priv->bt_sco_active) {
-		struct iwlagn_bt_sco_cmd sco_cmd = { .flags = 0 };
-
-		if (priv->bt_sco_active)
-			sco_cmd.flags |= IWLAGN_BT_SCO_ACTIVE;
-		if (iwl_send_cmd_pdu(priv, REPLY_BT_COEX_SCO,
-				     sizeof(sco_cmd), &sco_cmd))
-			IWL_ERR(priv, "failed to send BT SCO command\n");
-	}
 }
 
 static void iwlagn_bt_traffic_change_work(struct work_struct *work)
@@ -1881,6 +1660,11 @@ static void iwlagn_bt_traffic_change_work(struct work_struct *work)
 	struct iwl_rxon_context *ctx;
 	int smps_request = -1;
 
+	if (priv->bt_enable_flag == IWLAGN_BT_FLAG_COEX_MODE_DISABLED) {
+		/* bt coex disabled */
+		return;
+	}
+
 	/*
 	 * Note: bt_traffic_load can be overridden by scan complete and
 	 * coex profile notifications. Ignore that since only bad consequence
@@ -1991,12 +1775,14 @@ static void iwlagn_print_uartmsg(struct iwl_priv *priv,
 		(BT_UART_MSG_FRAME6DISCOVERABLE_MSK & uart_msg->frame6) >>
 			BT_UART_MSG_FRAME6DISCOVERABLE_POS);
 
-	IWL_DEBUG_NOTIF(priv, "Sniff Activity = 0x%X, Inquiry/Page SR Mode = "
-			"0x%X, Connectable = 0x%X",
+	IWL_DEBUG_NOTIF(priv, "Sniff Activity = 0x%X, Page = "
+			"0x%X, Inquiry = 0x%X, Connectable = 0x%X",
 		(BT_UART_MSG_FRAME7SNIFFACTIVITY_MSK & uart_msg->frame7) >>
 			BT_UART_MSG_FRAME7SNIFFACTIVITY_POS,
-		(BT_UART_MSG_FRAME7INQUIRYPAGESRMODE_MSK & uart_msg->frame7) >>
-			BT_UART_MSG_FRAME7INQUIRYPAGESRMODE_POS,
+		(BT_UART_MSG_FRAME7PAGE_MSK & uart_msg->frame7) >>
+			BT_UART_MSG_FRAME7PAGE_POS,
+		(BT_UART_MSG_FRAME7INQUIRY_MSK & uart_msg->frame7) >>
+			BT_UART_MSG_FRAME7INQUIRY_POS,
 		(BT_UART_MSG_FRAME7CONNECTABLE_MSK & uart_msg->frame7) >>
 			BT_UART_MSG_FRAME7CONNECTABLE_POS);
 }
@@ -2032,9 +1818,13 @@ void iwlagn_bt_coex_profile_notif(struct iwl_priv *priv,
 	unsigned long flags;
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_bt_coex_profile_notif *coex = &pkt->u.bt_coex_profile_notif;
-	struct iwlagn_bt_sco_cmd sco_cmd = { .flags = 0 };
 	struct iwl_bt_uart_msg *uart_msg = &coex->last_bt_uart_msg;
 
+	if (priv->bt_enable_flag == IWLAGN_BT_FLAG_COEX_MODE_DISABLED) {
+		/* bt coex disabled */
+		return;
+	}
+
 	IWL_DEBUG_NOTIF(priv, "BT Coex notification:\n");
 	IWL_DEBUG_NOTIF(priv, "    status: %d\n", coex->bt_status);
 	IWL_DEBUG_NOTIF(priv, "    traffic load: %d\n", coex->bt_traffic_load);
@@ -2063,15 +1853,6 @@ void iwlagn_bt_coex_profile_notif(struct iwl_priv *priv,
 			queue_work(priv->workqueue,
 				   &priv->bt_traffic_change_work);
 		}
-		if (priv->bt_sco_active !=
-		    (uart_msg->frame3 & BT_UART_MSG_FRAME3SCOESCO_MSK)) {
-			priv->bt_sco_active = uart_msg->frame3 &
-				BT_UART_MSG_FRAME3SCOESCO_MSK;
-			if (priv->bt_sco_active)
-				sco_cmd.flags |= IWLAGN_BT_SCO_ACTIVE;
-			iwl_send_cmd_pdu_async(priv, REPLY_BT_COEX_SCO,
-				       sizeof(sco_cmd), &sco_cmd, NULL);
-		}
 	}
 
 	iwlagn_set_kill_msk(priv, uart_msg);
@@ -2389,3 +2170,44 @@ int iwl_dump_fh(struct iwl_priv *priv, char **buf, bool display)
 	}
 	return 0;
 }
+
+/* notification wait support */
+void iwlagn_init_notification_wait(struct iwl_priv *priv,
+				   struct iwl_notification_wait *wait_entry,
+				   void (*fn)(struct iwl_priv *priv,
+					      struct iwl_rx_packet *pkt),
+				   u8 cmd)
+{
+	wait_entry->fn = fn;
+	wait_entry->cmd = cmd;
+	wait_entry->triggered = false;
+
+	spin_lock_bh(&priv->_agn.notif_wait_lock);
+	list_add(&wait_entry->list, &priv->_agn.notif_waits);
+	spin_unlock_bh(&priv->_agn.notif_wait_lock);
+}
+
+signed long iwlagn_wait_notification(struct iwl_priv *priv,
+				     struct iwl_notification_wait *wait_entry,
+				     unsigned long timeout)
+{
+	int ret;
+
+	ret = wait_event_timeout(priv->_agn.notif_waitq,
+				 &wait_entry->triggered,
+				 timeout);
+
+	spin_lock_bh(&priv->_agn.notif_wait_lock);
+	list_del(&wait_entry->list);
+	spin_unlock_bh(&priv->_agn.notif_wait_lock);
+
+	return ret;
+}
+
+void iwlagn_remove_notification(struct iwl_priv *priv,
+				struct iwl_notification_wait *wait_entry)
+{
+	spin_lock_bh(&priv->_agn.notif_wait_lock);
+	list_del(&wait_entry->list);
+	spin_unlock_bh(&priv->_agn.notif_wait_lock);
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
index 75fcd30a7c13..d03b4734c892 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
@@ -179,31 +179,31 @@ static s32 expected_tpt_legacy[IWL_RATE_COUNT] = {
 };
 
 static s32 expected_tpt_siso20MHz[4][IWL_RATE_COUNT] = {
-	{0, 0, 0, 0, 42, 0,  76, 102, 124, 158, 183, 193, 202}, /* Norm */
-	{0, 0, 0, 0, 46, 0,  82, 110, 132, 167, 192, 202, 210}, /* SGI */
-	{0, 0, 0, 0, 48, 0,  93, 135, 176, 251, 319, 351, 381}, /* AGG */
-	{0, 0, 0, 0, 53, 0, 102, 149, 193, 275, 348, 381, 413}, /* AGG+SGI */
+	{0, 0, 0, 0, 42, 0,  76, 102, 124, 159, 183, 193, 202}, /* Norm */
+	{0, 0, 0, 0, 46, 0,  82, 110, 132, 168, 192, 202, 210}, /* SGI */
+	{0, 0, 0, 0, 47, 0,  91, 133, 171, 242, 305, 334, 362}, /* AGG */
+	{0, 0, 0, 0, 52, 0, 101, 145, 187, 264, 330, 361, 390}, /* AGG+SGI */
 };
 
 static s32 expected_tpt_siso40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250, 257}, /* Norm */
 	{0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257, 264}, /* SGI */
-	{0, 0, 0, 0,  96, 0, 182, 259, 328, 451, 553, 598, 640}, /* AGG */
-	{0, 0, 0, 0, 106, 0, 199, 282, 357, 487, 593, 640, 683}, /* AGG+SGI */
+	{0, 0, 0, 0,  94, 0, 177, 249, 313, 423, 512, 550, 586}, /* AGG */
+	{0, 0, 0, 0, 104, 0, 193, 270, 338, 454, 545, 584, 620}, /* AGG+SGI */
 };
 
 static s32 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
-	{0, 0, 0, 0,  74, 0, 123, 155, 179, 213, 235, 243, 250}, /* Norm */
-	{0, 0, 0, 0,  81, 0, 131, 164, 187, 221, 242, 250, 256}, /* SGI */
-	{0, 0, 0, 0,  92, 0, 175, 250, 317, 436, 534, 578, 619}, /* AGG */
-	{0, 0, 0, 0, 102, 0, 192, 273, 344, 470, 573, 619, 660}, /* AGG+SGI*/
+	{0, 0, 0, 0,  74, 0, 123, 155, 179, 214, 236, 244, 251}, /* Norm */
+	{0, 0, 0, 0,  81, 0, 131, 164, 188, 223, 243, 251, 257}, /* SGI */
+	{0, 0, 0, 0,  89, 0, 167, 235, 296, 402, 488, 526, 560}, /* AGG */
+	{0, 0, 0, 0,  97, 0, 182, 255, 320, 431, 520, 558, 593}, /* AGG+SGI*/
 };
 
 static s32 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 123, 0, 182, 214, 235, 264, 279, 285, 289}, /* Norm */
 	{0, 0, 0, 0, 131, 0, 191, 222, 242, 270, 284, 289, 293}, /* SGI */
-	{0, 0, 0, 0, 180, 0, 327, 446, 545, 708, 828, 878, 922}, /* AGG */
-	{0, 0, 0, 0, 197, 0, 355, 481, 584, 752, 872, 922, 966}, /* AGG+SGI */
+	{0, 0, 0, 0, 171, 0, 305, 410, 496, 634, 731, 771, 805}, /* AGG */
+	{0, 0, 0, 0, 186, 0, 329, 439, 527, 667, 764, 803, 838}, /* AGG+SGI */
 };
 
 static s32 expected_tpt_mimo3_20MHz[4][IWL_RATE_COUNT] = {
@@ -2890,6 +2890,8 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 	u8 ant_toggle_cnt = 0;
 	u8 use_ht_possible = 1;
 	u8 valid_tx_ant = 0;
+	struct iwl_station_priv *sta_priv =
+		container_of(lq_sta, struct iwl_station_priv, lq_sta);
 	struct iwl_link_quality_cmd *lq_cmd = &lq_sta->lq;
 
 	/* Override starting rate (index 0) if needed for debug purposes */
@@ -3008,7 +3010,8 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 		repeat_rate--;
 	}
 
-	lq_cmd->agg_params.agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+	lq_cmd->agg_params.agg_frame_cnt_limit =
+		sta_priv->max_agg_bufsize ?: LINK_QUAL_AGG_FRAME_LIMIT_DEF;
 	lq_cmd->agg_params.agg_dis_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
 
 	lq_cmd->agg_params.agg_time_limit =
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h b/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
index 75e50d33ecb3..184828c72b31 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
@@ -213,6 +213,7 @@ enum {
 	 IWL_CCK_BASIC_RATES_MASK)
 
 #define IWL_RATES_MASK ((1 << IWL_RATE_COUNT) - 1)
+#define IWL_RATES_MASK_3945 ((1 << IWL_RATE_COUNT_3945) - 1)
 
 #define IWL_INVALID_VALUE    -1
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
index 6d140bd53291..dfdbea6e8f99 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
@@ -52,10 +52,14 @@ static int iwlagn_disable_pan(struct iwl_priv *priv,
 			      struct iwl_rxon_context *ctx,
 			      struct iwl_rxon_cmd *send)
 {
+	struct iwl_notification_wait disable_wait;
 	__le32 old_filter = send->filter_flags;
 	u8 old_dev_type = send->dev_type;
 	int ret;
 
+	iwlagn_init_notification_wait(priv, &disable_wait, NULL,
+				      REPLY_WIPAN_DEACTIVATION_COMPLETE);
+
 	send->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 	send->dev_type = RXON_DEV_TYPE_P2P;
 	ret = iwl_send_cmd_pdu(priv, ctx->rxon_cmd, sizeof(*send), send);
@@ -63,11 +67,18 @@ static int iwlagn_disable_pan(struct iwl_priv *priv,
 	send->filter_flags = old_filter;
 	send->dev_type = old_dev_type;
 
-	if (ret)
+	if (ret) {
 		IWL_ERR(priv, "Error disabling PAN (%d)\n", ret);
-
-	/* FIXME: WAIT FOR PAN DISABLE */
-	msleep(300);
+		iwlagn_remove_notification(priv, &disable_wait);
+	} else {
+		signed long wait_res;
+
+		wait_res = iwlagn_wait_notification(priv, &disable_wait, HZ);
+		if (wait_res == 0) {
+			IWL_ERR(priv, "Timed out waiting for PAN disable\n");
+			ret = -EIO;
+		}
+	}
 
 	return ret;
 }
@@ -145,6 +156,23 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	/* always get timestamp with Rx frame */
 	ctx->staging.flags |= RXON_FLG_TSF2HOST_MSK;
 
+	if (ctx->ctxid == IWL_RXON_CTX_PAN && priv->_agn.hw_roc_channel) {
+		struct ieee80211_channel *chan = priv->_agn.hw_roc_channel;
+
+		iwl_set_rxon_channel(priv, chan, ctx);
+		iwl_set_flags_for_band(priv, ctx, chan->band, NULL);
+		ctx->staging.filter_flags |=
+			RXON_FILTER_ASSOC_MSK |
+			RXON_FILTER_PROMISC_MSK |
+			RXON_FILTER_CTL2HOST_MSK;
+		ctx->staging.dev_type = RXON_DEV_TYPE_P2P;
+		new_assoc = true;
+
+		if (memcmp(&ctx->staging, &ctx->active,
+			   sizeof(ctx->staging)) == 0)
+			return 0;
+	}
+
 	if ((ctx->vif && ctx->vif->bss_conf.use_short_slot) ||
 	    !(ctx->staging.flags & RXON_FLG_BAND_24G_MSK))
 		ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
@@ -288,10 +316,9 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	 * If we issue a new RXON command which required a tune then we must
 	 * send a new TXPOWER command or we won't be able to Tx any frames.
 	 *
-	 * FIXME: which RXON requires a tune? Can we optimise this out in
-	 *        some cases?
+	 * It's expected we set power here if channel is changing.
 	 */
-	ret = iwl_set_tx_power(priv, priv->tx_power_user_lmt, true);
+	ret = iwl_set_tx_power(priv, priv->tx_power_next, true);
 	if (ret) {
 		IWL_ERR(priv, "Error sending TX power (%d)\n", ret);
 		return ret;
@@ -444,6 +471,7 @@ static void iwlagn_check_needed_chains(struct iwl_priv *priv,
 	struct iwl_rxon_context *tmp;
 	struct ieee80211_sta *sta;
 	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
+	struct ieee80211_sta_ht_cap *ht_cap;
 	bool need_multiple;
 
 	lockdep_assert_held(&priv->mutex);
@@ -452,23 +480,7 @@ static void iwlagn_check_needed_chains(struct iwl_priv *priv,
 	case NL80211_IFTYPE_STATION:
 		rcu_read_lock();
 		sta = ieee80211_find_sta(vif, bss_conf->bssid);
-		if (sta) {
-			struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
-			int maxstreams;
-
-			maxstreams = (ht_cap->mcs.tx_params &
-				      IEEE80211_HT_MCS_TX_MAX_STREAMS_MASK)
-					>> IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT;
-			maxstreams += 1;
-
-			need_multiple = true;
-
-			if ((ht_cap->mcs.rx_mask[1] == 0) &&
-			    (ht_cap->mcs.rx_mask[2] == 0))
-				need_multiple = false;
-			if (maxstreams <= 1)
-				need_multiple = false;
-		} else {
+		if (!sta) {
 			/*
 			 * If at all, this can only happen through a race
 			 * when the AP disconnects us while we're still
@@ -476,7 +488,46 @@ static void iwlagn_check_needed_chains(struct iwl_priv *priv,
 			 * will soon tell us about that.
 			 */
 			need_multiple = false;
+			rcu_read_unlock();
+			break;
+		}
+
+		ht_cap = &sta->ht_cap;
+
+		need_multiple = true;
+
+		/*
+		 * If the peer advertises no support for receiving 2 and 3
+		 * stream MCS rates, it can't be transmitting them either.
+		 */
+		if (ht_cap->mcs.rx_mask[1] == 0 &&
+		    ht_cap->mcs.rx_mask[2] == 0) {
+			need_multiple = false;
+		} else if (!(ht_cap->mcs.tx_params &
+						IEEE80211_HT_MCS_TX_DEFINED)) {
+			/* If it can't TX MCS at all ... */
+			need_multiple = false;
+		} else if (ht_cap->mcs.tx_params &
+						IEEE80211_HT_MCS_TX_RX_DIFF) {
+			int maxstreams;
+
+			/*
+			 * But if it can receive them, it might still not
+			 * be able to transmit them, which is what we need
+			 * to check here -- so check the number of streams
+			 * it advertises for TX (if different from RX).
+			 */
+
+			maxstreams = (ht_cap->mcs.tx_params &
+				 IEEE80211_HT_MCS_TX_MAX_STREAMS_MASK);
+			maxstreams >>=
+				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT;
+			maxstreams += 1;
+
+			if (maxstreams <= 1)
+				need_multiple = false;
 		}
+
 		rcu_read_unlock();
 		break;
 	case NL80211_IFTYPE_ADHOC:
@@ -546,12 +597,10 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 
 	if (changes & BSS_CHANGED_ASSOC) {
 		if (bss_conf->assoc) {
-			iwl_led_associate(priv);
 			priv->timestamp = bss_conf->timestamp;
 			ctx->staging.filter_flags |= RXON_FILTER_ASSOC_MSK;
 		} else {
 			ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
-			iwl_led_disassociate(priv);
 		}
 	}
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index 24a11b8f73bc..a709d05c5868 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -539,7 +539,14 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	unsigned long flags;
 	bool is_agg = false;
 
-	if (info->control.vif)
+	/*
+	 * If the frame needs to go out off-channel, then
+	 * we'll have put the PAN context to that channel,
+	 * so make the frame go out there.
+	 */
+	if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
+		ctx = &priv->contexts[IWL_RXON_CTX_PAN];
+	else if (info->control.vif)
 		ctx = iwl_rxon_ctx_from_vif(info->control.vif);
 
 	spin_lock_irqsave(&priv->lock, flags);
@@ -940,7 +947,7 @@ void iwlagn_txq_ctx_reset(struct iwl_priv *priv)
  */
 void iwlagn_txq_ctx_stop(struct iwl_priv *priv)
 {
-	int ch;
+	int ch, txq_id;
 	unsigned long flags;
 
 	/* Turn off all Tx DMA fifos */
@@ -959,6 +966,16 @@ void iwlagn_txq_ctx_stop(struct iwl_priv *priv)
 			    iwl_read_direct32(priv, FH_TSSR_TX_STATUS_REG));
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (!priv->txq)
+		return;
+
+	/* Unmap DMA from host system and free skb's */
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+		if (txq_id == priv->cmd_queue)
+			iwl_cmd_queue_unmap(priv);
+		else
+			iwl_tx_queue_unmap(priv, txq_id);
 }
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c b/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
index 24dabcd2a36c..d807e5e2b718 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
@@ -308,14 +308,6 @@ void iwlagn_init_alive_start(struct iwl_priv *priv)
 {
 	int ret = 0;
 
-	/* Check alive response for "valid" sign from uCode */
-	if (priv->card_alive_init.is_valid != UCODE_VALID_OK) {
-		/* We had an error bringing up the hardware, so take it
-		 * all the way back down so we can try again */
-		IWL_DEBUG_INFO(priv, "Initialize Alive failed.\n");
-		goto restart;
-	}
-
 	/* initialize uCode was loaded... verify inst image.
 	 * This is a paranoid check, because we would not have gotten the
 	 * "initialize" alive if code weren't properly loaded.  */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index c1cfd9952e52..e0cd3e27a0d9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -59,6 +59,7 @@
 #include "iwl-sta.h"
 #include "iwl-agn-calib.h"
 #include "iwl-agn.h"
+#include "iwl-agn-led.h"
 
 
 /******************************************************************************
@@ -85,7 +86,6 @@ MODULE_DESCRIPTION(DRV_DESCRIPTION);
 MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("iwl4965");
 
 static int iwlagn_ant_coupling;
 static bool iwlagn_bt_ch_announce = 1;
@@ -424,47 +424,6 @@ int iwl_hw_tx_queue_init(struct iwl_priv *priv,
 	return 0;
 }
 
-/******************************************************************************
- *
- * Generic RX handler implementations
- *
- ******************************************************************************/
-static void iwl_rx_reply_alive(struct iwl_priv *priv,
-				struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_alive_resp *palive;
-	struct delayed_work *pwork;
-
-	palive = &pkt->u.alive_frame;
-
-	IWL_DEBUG_INFO(priv, "Alive ucode status 0x%08X revision "
-		       "0x%01X 0x%01X\n",
-		       palive->is_valid, palive->ver_type,
-		       palive->ver_subtype);
-
-	if (palive->ver_subtype == INITIALIZE_SUBTYPE) {
-		IWL_DEBUG_INFO(priv, "Initialization Alive received.\n");
-		memcpy(&priv->card_alive_init,
-		       &pkt->u.alive_frame,
-		       sizeof(struct iwl_init_alive_resp));
-		pwork = &priv->init_alive_start;
-	} else {
-		IWL_DEBUG_INFO(priv, "Runtime Alive received.\n");
-		memcpy(&priv->card_alive, &pkt->u.alive_frame,
-		       sizeof(struct iwl_alive_resp));
-		pwork = &priv->alive_start;
-	}
-
-	/* We delay the ALIVE response by 5ms to
-	 * give the HW RF Kill time to activate... */
-	if (palive->is_valid == UCODE_VALID_OK)
-		queue_delayed_work(priv->workqueue, pwork,
-				   msecs_to_jiffies(5));
-	else
-		IWL_WARN(priv, "uCode did not respond OK.\n");
-}
-
 static void iwl_bg_beacon_update(struct work_struct *work)
 {
 	struct iwl_priv *priv =
@@ -699,83 +658,6 @@ static void iwl_bg_ucode_trace(unsigned long data)
 	}
 }
 
-static void iwl_rx_beacon_notif(struct iwl_priv *priv,
-				struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl4965_beacon_notif *beacon =
-		(struct iwl4965_beacon_notif *)pkt->u.raw;
-#ifdef CONFIG_IWLWIFI_DEBUG
-	u8 rate = iwl_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
-
-	IWL_DEBUG_RX(priv, "beacon status %x retries %d iss %d "
-		"tsf %d %d rate %d\n",
-		le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
-		beacon->beacon_notify_hdr.failure_frame,
-		le32_to_cpu(beacon->ibss_mgr_status),
-		le32_to_cpu(beacon->high_tsf),
-		le32_to_cpu(beacon->low_tsf), rate);
-#endif
-
-	priv->ibss_manager = le32_to_cpu(beacon->ibss_mgr_status);
-
-	if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
-		queue_work(priv->workqueue, &priv->beacon_update);
-}
-
-/* Handle notification from uCode that card's power state is changing
- * due to software, hardware, or critical temperature RFKILL */
-static void iwl_rx_card_state_notif(struct iwl_priv *priv,
-				    struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	u32 flags = le32_to_cpu(pkt->u.card_state_notif.flags);
-	unsigned long status = priv->status;
-
-	IWL_DEBUG_RF_KILL(priv, "Card state received: HW:%s SW:%s CT:%s\n",
-			  (flags & HW_CARD_DISABLED) ? "Kill" : "On",
-			  (flags & SW_CARD_DISABLED) ? "Kill" : "On",
-			  (flags & CT_CARD_DISABLED) ?
-			  "Reached" : "Not reached");
-
-	if (flags & (SW_CARD_DISABLED | HW_CARD_DISABLED |
-		     CT_CARD_DISABLED)) {
-
-		iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
-			    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
-
-		iwl_write_direct32(priv, HBUS_TARG_MBX_C,
-					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
-
-		if (!(flags & RXON_CARD_DISABLED)) {
-			iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
-				    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
-			iwl_write_direct32(priv, HBUS_TARG_MBX_C,
-					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
-		}
-		if (flags & CT_CARD_DISABLED)
-			iwl_tt_enter_ct_kill(priv);
-	}
-	if (!(flags & CT_CARD_DISABLED))
-		iwl_tt_exit_ct_kill(priv);
-
-	if (flags & HW_CARD_DISABLED)
-		set_bit(STATUS_RF_KILL_HW, &priv->status);
-	else
-		clear_bit(STATUS_RF_KILL_HW, &priv->status);
-
-
-	if (!(flags & RXON_CARD_DISABLED))
-		iwl_scan_cancel(priv);
-
-	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
-	     test_bit(STATUS_RF_KILL_HW, &priv->status)))
-		wiphy_rfkill_set_hw_state(priv->hw->wiphy,
-			test_bit(STATUS_RF_KILL_HW, &priv->status));
-	else
-		wake_up_interruptible(&priv->wait_command_queue);
-}
-
 static void iwl_bg_tx_flush(struct work_struct *work)
 {
 	struct iwl_priv *priv =
@@ -795,58 +677,13 @@ static void iwl_bg_tx_flush(struct work_struct *work)
 }
 
 /**
- * iwl_setup_rx_handlers - Initialize Rx handler callbacks
- *
- * Setup the RX handlers for each of the reply types sent from the uCode
- * to the host.
- *
- * This function chains into the hardware specific files for them to setup
- * any hardware specific handlers as well.
- */
-static void iwl_setup_rx_handlers(struct iwl_priv *priv)
-{
-	priv->rx_handlers[REPLY_ALIVE] = iwl_rx_reply_alive;
-	priv->rx_handlers[REPLY_ERROR] = iwl_rx_reply_error;
-	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl_rx_csa;
-	priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
-			iwl_rx_spectrum_measure_notif;
-	priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_rx_pm_sleep_notif;
-	priv->rx_handlers[PM_DEBUG_STATISTIC_NOTIFIC] =
-	    iwl_rx_pm_debug_statistics_notif;
-	priv->rx_handlers[BEACON_NOTIFICATION] = iwl_rx_beacon_notif;
-
-	/*
-	 * The same handler is used for both the REPLY to a discrete
-	 * statistics request from the host as well as for the periodic
-	 * statistics notifications (after received beacons) from the uCode.
-	 */
-	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl_reply_statistics;
-	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl_rx_statistics;
-
-	iwl_setup_rx_scan_handlers(priv);
-
-	/* status change handler */
-	priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl_rx_card_state_notif;
-
-	priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
-	    iwl_rx_missed_beacon_notif;
-	/* Rx handlers */
-	priv->rx_handlers[REPLY_RX_PHY_CMD] = iwlagn_rx_reply_rx_phy;
-	priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwlagn_rx_reply_rx;
-	/* block ack */
-	priv->rx_handlers[REPLY_COMPRESSED_BA] = iwlagn_rx_reply_compressed_ba;
-	/* Set up hardware specific Rx handlers */
-	priv->cfg->ops->lib->rx_handler_setup(priv);
-}
-
-/**
  * iwl_rx_handle - Main entry function for receiving responses from uCode
  *
  * Uses the priv->rx_handlers callback function array to invoke
  * the appropriate handlers, including command responses,
  * frame-received notifications, and other notifications.
  */
-void iwl_rx_handle(struct iwl_priv *priv)
+static void iwl_rx_handle(struct iwl_priv *priv)
 {
 	struct iwl_rx_mem_buffer *rxb;
 	struct iwl_rx_packet *pkt;
@@ -910,6 +747,27 @@ void iwl_rx_handle(struct iwl_priv *priv)
 			(pkt->hdr.cmd != STATISTICS_NOTIFICATION) &&
 			(pkt->hdr.cmd != REPLY_TX);
 
+		/*
+		 * Do the notification wait before RX handlers so
+		 * even if the RX handler consumes the RXB we have
+		 * access to it in the notification wait entry.
+		 */
+		if (!list_empty(&priv->_agn.notif_waits)) {
+			struct iwl_notification_wait *w;
+
+			spin_lock(&priv->_agn.notif_wait_lock);
+			list_for_each_entry(w, &priv->_agn.notif_waits, list) {
+				if (w->cmd == pkt->hdr.cmd) {
+					w->triggered = true;
+					if (w->fn)
+						w->fn(priv, pkt);
+				}
+			}
+			spin_unlock(&priv->_agn.notif_wait_lock);
+
+			wake_up_all(&priv->_agn.notif_waitq);
+		}
+
 		/* Based on type of command response or notification,
 		 *   handle those that need handling via function in
 		 *   rx_handlers table.  See iwl_setup_rx_handlers() */
@@ -1379,66 +1237,6 @@ static void iwl_irq_tasklet(struct iwl_priv *priv)
 		iwl_enable_rfkill_int(priv);
 }
 
-/* the threshold ratio of actual_ack_cnt to expected_ack_cnt in percent */
-#define ACK_CNT_RATIO (50)
-#define BA_TIMEOUT_CNT (5)
-#define BA_TIMEOUT_MAX (16)
-
-/**
- * iwl_good_ack_health - checks for ACK count ratios, BA timeout retries.
- *
- * When the ACK count ratio is 0 and aggregated BA timeout retries exceeding
- * the BA_TIMEOUT_MAX, reload firmware and bring system back to normal
- * operation state.
- */
-bool iwl_good_ack_health(struct iwl_priv *priv,
-				struct iwl_rx_packet *pkt)
-{
-	bool rc = true;
-	int actual_ack_cnt_delta, expected_ack_cnt_delta;
-	int ba_timeout_delta;
-
-	actual_ack_cnt_delta =
-		le32_to_cpu(pkt->u.stats.tx.actual_ack_cnt) -
-		le32_to_cpu(priv->_agn.statistics.tx.actual_ack_cnt);
-	expected_ack_cnt_delta =
-		le32_to_cpu(pkt->u.stats.tx.expected_ack_cnt) -
-		le32_to_cpu(priv->_agn.statistics.tx.expected_ack_cnt);
-	ba_timeout_delta =
-		le32_to_cpu(pkt->u.stats.tx.agg.ba_timeout) -
-		le32_to_cpu(priv->_agn.statistics.tx.agg.ba_timeout);
-	if ((priv->_agn.agg_tids_count > 0) &&
-	    (expected_ack_cnt_delta > 0) &&
-	    (((actual_ack_cnt_delta * 100) / expected_ack_cnt_delta)
-		< ACK_CNT_RATIO) &&
-	    (ba_timeout_delta > BA_TIMEOUT_CNT)) {
-		IWL_DEBUG_RADIO(priv, "actual_ack_cnt delta = %d,"
-				" expected_ack_cnt = %d\n",
-				actual_ack_cnt_delta, expected_ack_cnt_delta);
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-		/*
-		 * This is ifdef'ed on DEBUGFS because otherwise the
-		 * statistics aren't available. If DEBUGFS is set but
-		 * DEBUG is not, these will just compile out.
-		 */
-		IWL_DEBUG_RADIO(priv, "rx_detected_cnt delta = %d\n",
-				priv->_agn.delta_statistics.tx.rx_detected_cnt);
-		IWL_DEBUG_RADIO(priv,
-				"ack_or_ba_timeout_collision delta = %d\n",
-				priv->_agn.delta_statistics.tx.
-				ack_or_ba_timeout_collision);
-#endif
-		IWL_DEBUG_RADIO(priv, "agg ba_timeout delta = %d\n",
-				ba_timeout_delta);
-		if (!actual_ack_cnt_delta &&
-		    (ba_timeout_delta >= BA_TIMEOUT_MAX))
-			rc = false;
-	}
-	return rc;
-}
-
-
 /*****************************************************************************
  *
  * sysfs attributes
@@ -2632,13 +2430,6 @@ static void iwl_alive_start(struct iwl_priv *priv)
 
 	IWL_DEBUG_INFO(priv, "Runtime Alive received.\n");
 
-	if (priv->card_alive.is_valid != UCODE_VALID_OK) {
-		/* We had an error bringing up the hardware, so take it
-		 * all the way back down so we can try again */
-		IWL_DEBUG_INFO(priv, "Alive failed.\n");
-		goto restart;
-	}
-
 	/* Initialize uCode has loaded Runtime uCode ... verify inst image.
 	 * This is a paranoid check, because we would not have gotten the
 	 * "runtime" alive if code weren't properly loaded.  */
@@ -2710,9 +2501,11 @@ static void iwl_alive_start(struct iwl_priv *priv)
 			priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
 	}
 
-	if (priv->cfg->bt_params &&
-	    !priv->cfg->bt_params->advanced_bt_coexist) {
-		/* Configure Bluetooth device coexistence support */
+	if (!priv->cfg->bt_params || (priv->cfg->bt_params &&
+	    !priv->cfg->bt_params->advanced_bt_coexist)) {
+		/*
+		 * default is 2-wire BT coexexistence support
+		 */
 		priv->cfg->ops->hcmd->send_bt_config(priv);
 	}
 
@@ -2726,8 +2519,6 @@ static void iwl_alive_start(struct iwl_priv *priv)
 	/* At this point, the NIC is initialized and operational */
 	iwl_rf_kill_ct_config(priv);
 
-	iwl_leds_init(priv);
-
 	IWL_DEBUG_INFO(priv, "ALIVE processing complete.\n");
 	wake_up_interruptible(&priv->wait_command_queue);
 
@@ -2769,7 +2560,6 @@ static void __iwl_down(struct iwl_priv *priv)
 			 priv->cfg->bt_params->bt_init_traffic_load;
 	else
 		priv->bt_traffic_load = 0;
-	priv->bt_sco_active = false;
 	priv->bt_full_concurrent = false;
 	priv->bt_ci_compliance = 0;
 
@@ -3063,8 +2853,7 @@ static void iwl_bg_run_time_calib_work(struct work_struct *work)
 	}
 
 	if (priv->start_calib) {
-		if (priv->cfg->bt_params &&
-		    priv->cfg->bt_params->bt_statistics) {
+		if (iwl_bt_statistics(priv)) {
 			iwl_chain_noise_calibration(priv,
 					(void *)&priv->_agn.statistics_bt);
 			iwl_sensitivity_calibration(priv,
@@ -3089,7 +2878,7 @@ static void iwl_bg_restart(struct work_struct *data)
 
 	if (test_and_clear_bit(STATUS_FW_ERROR, &priv->status)) {
 		struct iwl_rxon_context *ctx;
-		bool bt_sco, bt_full_concurrent;
+		bool bt_full_concurrent;
 		u8 bt_ci_compliance;
 		u8 bt_load;
 		u8 bt_status;
@@ -3108,7 +2897,6 @@ static void iwl_bg_restart(struct work_struct *data)
 		 * re-configure the hw when we reconfigure the BT
 		 * command.
 		 */
-		bt_sco = priv->bt_sco_active;
 		bt_full_concurrent = priv->bt_full_concurrent;
 		bt_ci_compliance = priv->bt_ci_compliance;
 		bt_load = priv->bt_traffic_load;
@@ -3116,7 +2904,6 @@ static void iwl_bg_restart(struct work_struct *data)
 
 		__iwl_down(priv);
 
-		priv->bt_sco_active = bt_sco;
 		priv->bt_full_concurrent = bt_full_concurrent;
 		priv->bt_ci_compliance = bt_ci_compliance;
 		priv->bt_traffic_load = bt_load;
@@ -3178,6 +2965,8 @@ static int iwl_mac_setup_register(struct iwl_priv *priv,
 		    IEEE80211_HW_SPECTRUM_MGMT |
 		    IEEE80211_HW_REPORTS_TX_ACK_STATUS;
 
+	hw->max_tx_aggregation_subframes = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+
 	if (!priv->cfg->base_params->broken_powersave)
 		hw->flags |= IEEE80211_HW_SUPPORTS_PS |
 			     IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
@@ -3194,8 +2983,11 @@ static int iwl_mac_setup_register(struct iwl_priv *priv,
 		hw->wiphy->interface_modes |= ctx->exclusive_interface_modes;
 	}
 
+	hw->wiphy->max_remain_on_channel_duration = 1000;
+
 	hw->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
-			    WIPHY_FLAG_DISABLE_BEACON_HINTS;
+			    WIPHY_FLAG_DISABLE_BEACON_HINTS |
+			    WIPHY_FLAG_IBSS_RSN;
 
 	/*
 	 * For now, disable PS by default because it affects
@@ -3219,6 +3011,8 @@ static int iwl_mac_setup_register(struct iwl_priv *priv,
 		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
 			&priv->bands[IEEE80211_BAND_5GHZ];
 
+	iwl_leds_init(priv);
+
 	ret = ieee80211_register_hw(priv->hw);
 	if (ret) {
 		IWL_ERR(priv, "Failed to register hw (error %d)\n", ret);
@@ -3263,7 +3057,7 @@ int iwlagn_mac_start(struct ieee80211_hw *hw)
 		}
 	}
 
-	iwl_led_start(priv);
+	iwlagn_led_enable(priv);
 
 out:
 	priv->is_open = 1;
@@ -3294,7 +3088,7 @@ void iwlagn_mac_stop(struct ieee80211_hw *hw)
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-int iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl_priv *priv = hw->priv;
 
@@ -3307,7 +3101,6 @@ int iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		dev_kfree_skb_any(skb);
 
 	IWL_DEBUG_MACDUMP(priv, "leave\n");
-	return NETDEV_TX_OK;
 }
 
 void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
@@ -3345,6 +3138,14 @@ int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		return -EOPNOTSUPP;
 	}
 
+	/*
+	 * To support IBSS RSN, don't program group keys in IBSS, the
+	 * hardware will then not attempt to decrypt the frames.
+	 */
+	if (vif->type == NL80211_IFTYPE_ADHOC &&
+	    !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+		return -EOPNOTSUPP;
+
 	sta_id = iwl_sta_id_or_broadcast(priv, vif_priv->ctx, sta);
 	if (sta_id == IWL_INVALID_STATION)
 		return -EINVAL;
@@ -3399,10 +3200,12 @@ int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 			    struct ieee80211_vif *vif,
 			    enum ieee80211_ampdu_mlme_action action,
-			    struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+			    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			    u8 buf_size)
 {
 	struct iwl_priv *priv = hw->priv;
 	int ret = -EINVAL;
+	struct iwl_station_priv *sta_priv = (void *) sta->drv_priv;
 
 	IWL_DEBUG_HT(priv, "A-MPDU action on addr %pM tid %d\n",
 		     sta->addr, tid);
@@ -3457,11 +3260,28 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 		}
 		break;
 	case IEEE80211_AMPDU_TX_OPERATIONAL:
+		/*
+		 * If the limit is 0, then it wasn't initialised yet,
+		 * use the default. We can do that since we take the
+		 * minimum below, and we don't want to go above our
+		 * default due to hardware restrictions.
+		 */
+		if (sta_priv->max_agg_bufsize == 0)
+			sta_priv->max_agg_bufsize =
+				LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+
+		/*
+		 * Even though in theory the peer could have different
+		 * aggregation reorder buffer sizes for different sessions,
+		 * our ucode doesn't allow for that and has a global limit
+		 * for each station. Therefore, use the minimum of all the
+		 * aggregation sessions and our default value.
+		 */
+		sta_priv->max_agg_bufsize =
+			min(sta_priv->max_agg_bufsize, buf_size);
+
 		if (priv->cfg->ht_params &&
 		    priv->cfg->ht_params->use_rts_for_aggregation) {
-			struct iwl_station_priv *sta_priv =
-				(void *) sta->drv_priv;
-
 			/*
 			 * switch to RTS/CTS if it is the prefer protection
 			 * method for HT traffic
@@ -3469,9 +3289,13 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 
 			sta_priv->lq_sta.lq.general_params.flags |=
 				LINK_QUAL_FLAGS_SET_STA_TLC_RTS_MSK;
-			iwl_send_lq_cmd(priv, iwl_rxon_ctx_from_vif(vif),
-					&sta_priv->lq_sta.lq, CMD_ASYNC, false);
 		}
+
+		sta_priv->lq_sta.lq.agg_params.agg_frame_cnt_limit =
+			sta_priv->max_agg_bufsize;
+
+		iwl_send_lq_cmd(priv, iwl_rxon_ctx_from_vif(vif),
+				&sta_priv->lq_sta.lq, CMD_ASYNC, false);
 		ret = 0;
 		break;
 	}
@@ -3709,6 +3533,95 @@ done:
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
+static void iwlagn_disable_roc(struct iwl_priv *priv)
+{
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_PAN];
+	struct ieee80211_channel *chan = ACCESS_ONCE(priv->hw->conf.channel);
+
+	lockdep_assert_held(&priv->mutex);
+
+	if (!ctx->is_active)
+		return;
+
+	ctx->staging.dev_type = RXON_DEV_TYPE_2STA;
+	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	iwl_set_rxon_channel(priv, chan, ctx);
+	iwl_set_flags_for_band(priv, ctx, chan->band, NULL);
+
+	priv->_agn.hw_roc_channel = NULL;
+
+	iwlcore_commit_rxon(priv, ctx);
+
+	ctx->is_active = false;
+}
+
+static void iwlagn_bg_roc_done(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+					     _agn.hw_roc_work.work);
+
+	mutex_lock(&priv->mutex);
+	ieee80211_remain_on_channel_expired(priv->hw);
+	iwlagn_disable_roc(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static int iwl_mac_remain_on_channel(struct ieee80211_hw *hw,
+				     struct ieee80211_channel *channel,
+				     enum nl80211_channel_type channel_type,
+				     int duration)
+{
+	struct iwl_priv *priv = hw->priv;
+	int err = 0;
+
+	if (!(priv->valid_contexts & BIT(IWL_RXON_CTX_PAN)))
+		return -EOPNOTSUPP;
+
+	if (!(priv->contexts[IWL_RXON_CTX_PAN].interface_modes &
+					BIT(NL80211_IFTYPE_P2P_CLIENT)))
+		return -EOPNOTSUPP;
+
+	mutex_lock(&priv->mutex);
+
+	if (priv->contexts[IWL_RXON_CTX_PAN].is_active ||
+	    test_bit(STATUS_SCAN_HW, &priv->status)) {
+		err = -EBUSY;
+		goto out;
+	}
+
+	priv->contexts[IWL_RXON_CTX_PAN].is_active = true;
+	priv->_agn.hw_roc_channel = channel;
+	priv->_agn.hw_roc_chantype = channel_type;
+	priv->_agn.hw_roc_duration = DIV_ROUND_UP(duration * 1000, 1024);
+	iwlcore_commit_rxon(priv, &priv->contexts[IWL_RXON_CTX_PAN]);
+	queue_delayed_work(priv->workqueue, &priv->_agn.hw_roc_work,
+			   msecs_to_jiffies(duration + 20));
+
+	msleep(IWL_MIN_SLOT_TIME); /* TU is almost ms */
+	ieee80211_ready_on_channel(priv->hw);
+
+ out:
+	mutex_unlock(&priv->mutex);
+
+	return err;
+}
+
+static int iwl_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	if (!(priv->valid_contexts & BIT(IWL_RXON_CTX_PAN)))
+		return -EOPNOTSUPP;
+
+	cancel_delayed_work_sync(&priv->_agn.hw_roc_work);
+
+	mutex_lock(&priv->mutex);
+	iwlagn_disable_roc(priv);
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
 /*****************************************************************************
  *
  * driver setup and teardown
@@ -3730,6 +3643,7 @@ static void iwl_setup_deferred_work(struct iwl_priv *priv)
 	INIT_WORK(&priv->bt_runtime_config, iwl_bg_bt_runtime_config);
 	INIT_DELAYED_WORK(&priv->init_alive_start, iwl_bg_init_alive_start);
 	INIT_DELAYED_WORK(&priv->alive_start, iwl_bg_alive_start);
+	INIT_DELAYED_WORK(&priv->_agn.hw_roc_work, iwlagn_bg_roc_done);
 
 	iwl_setup_scan_deferred_work(priv);
 
@@ -3823,6 +3737,8 @@ static int iwl_init_drv(struct iwl_priv *priv)
 	priv->force_reset[IWL_FW_RESET].reset_duration =
 		IWL_DELAY_NEXT_FORCE_FW_RELOAD;
 
+	priv->rx_statistics_jiffies = jiffies;
+
 	/* Choose which receivers/antennas to use */
 	if (priv->cfg->ops->hcmd->set_rxon_chain)
 		priv->cfg->ops->hcmd->set_rxon_chain(priv,
@@ -3876,7 +3792,6 @@ static void iwl_uninit_drv(struct iwl_priv *priv)
 	kfree(priv->scan_cmd);
 }
 
-#ifdef CONFIG_IWL5000
 struct ieee80211_ops iwlagn_hw_ops = {
 	.tx = iwlagn_mac_tx,
 	.start = iwlagn_mac_start,
@@ -3898,14 +3813,15 @@ struct ieee80211_ops iwlagn_hw_ops = {
 	.channel_switch = iwlagn_mac_channel_switch,
 	.flush = iwlagn_mac_flush,
 	.tx_last_beacon = iwl_mac_tx_last_beacon,
+	.remain_on_channel = iwl_mac_remain_on_channel,
+	.cancel_remain_on_channel = iwl_mac_cancel_remain_on_channel,
 };
-#endif
 
 static void iwl_hw_detect(struct iwl_priv *priv)
 {
 	priv->hw_rev = _iwl_read32(priv, CSR_HW_REV);
 	priv->hw_wa_rev = _iwl_read32(priv, CSR_HW_REV_WA_REG);
-	pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &priv->rev_id);
+	priv->rev_id = priv->pci_dev->revision;
 	IWL_DEBUG_INFO(priv, "HW Revision ID = 0x%X\n", priv->rev_id);
 }
 
@@ -3967,12 +3883,7 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (cfg->mod_params->disable_hw_scan) {
 		dev_printk(KERN_DEBUG, &(pdev->dev),
 			"sw scan support is deprecated\n");
-#ifdef CONFIG_IWL5000
 		iwlagn_hw_ops.hw_scan = NULL;
-#endif
-#ifdef CONFIG_IWL4965
-		iwl4965_hw_ops.hw_scan = NULL;
-#endif
 	}
 
 	hw = iwl_alloc_all(cfg);
@@ -4025,6 +3936,10 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	priv->contexts[IWL_RXON_CTX_PAN].mcast_queue = IWL_IPAN_MCAST_QUEUE;
 	priv->contexts[IWL_RXON_CTX_PAN].interface_modes =
 		BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_AP);
+#ifdef CONFIG_IWL_P2P
+	priv->contexts[IWL_RXON_CTX_PAN].interface_modes |=
+		BIT(NL80211_IFTYPE_P2P_CLIENT) | BIT(NL80211_IFTYPE_P2P_GO);
+#endif
 	priv->contexts[IWL_RXON_CTX_PAN].ap_devtype = RXON_DEV_TYPE_CP;
 	priv->contexts[IWL_RXON_CTX_PAN].station_devtype = RXON_DEV_TYPE_2STA;
 	priv->contexts[IWL_RXON_CTX_PAN].unused_devtype = RXON_DEV_TYPE_P2P;
@@ -4272,6 +4187,9 @@ static void __devexit iwl_pci_remove(struct pci_dev *pdev)
 	 * we need to set STATUS_EXIT_PENDING bit.
 	 */
 	set_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	iwl_leds_exit(priv);
+
 	if (priv->mac80211_registered) {
 		ieee80211_unregister_hw(priv->hw);
 		priv->mac80211_registered = 0;
@@ -4344,12 +4262,6 @@ static void __devexit iwl_pci_remove(struct pci_dev *pdev)
 
 /* Hardware specific file defines the PCI IDs table for that hardware module */
 static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
-#ifdef CONFIG_IWL4965
-	{IWL_PCI_DEVICE(0x4229, PCI_ANY_ID, iwl4965_agn_cfg)},
-	{IWL_PCI_DEVICE(0x4230, PCI_ANY_ID, iwl4965_agn_cfg)},
-#endif /* CONFIG_IWL4965 */
-#ifdef CONFIG_IWL5000
-/* 5100 Series WiFi */
 	{IWL_PCI_DEVICE(0x4232, 0x1201, iwl5100_agn_cfg)}, /* Mini Card */
 	{IWL_PCI_DEVICE(0x4232, 0x1301, iwl5100_agn_cfg)}, /* Half Mini Card */
 	{IWL_PCI_DEVICE(0x4232, 0x1204, iwl5100_agn_cfg)}, /* Mini Card */
@@ -4492,7 +4404,48 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 	{IWL_PCI_DEVICE(0x0896, 0x5025, iwl130_bgn_cfg)},
 	{IWL_PCI_DEVICE(0x0896, 0x5027, iwl130_bg_cfg)},
 
-#endif /* CONFIG_IWL5000 */
+/* 2x00 Series */
+	{IWL_PCI_DEVICE(0x0890, 0x4022, iwl2000_2bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0891, 0x4222, iwl2000_2bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0890, 0x4422, iwl2000_2bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0890, 0x4026, iwl2000_2bg_cfg)},
+	{IWL_PCI_DEVICE(0x0891, 0x4226, iwl2000_2bg_cfg)},
+	{IWL_PCI_DEVICE(0x0890, 0x4426, iwl2000_2bg_cfg)},
+
+/* 2x30 Series */
+	{IWL_PCI_DEVICE(0x0887, 0x4062, iwl2030_2bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0888, 0x4262, iwl2030_2bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0887, 0x4462, iwl2030_2bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0887, 0x4066, iwl2030_2bg_cfg)},
+	{IWL_PCI_DEVICE(0x0888, 0x4266, iwl2030_2bg_cfg)},
+	{IWL_PCI_DEVICE(0x0887, 0x4466, iwl2030_2bg_cfg)},
+
+/* 6x35 Series */
+	{IWL_PCI_DEVICE(0x088E, 0x4060, iwl6035_2agn_cfg)},
+	{IWL_PCI_DEVICE(0x088F, 0x4260, iwl6035_2agn_cfg)},
+	{IWL_PCI_DEVICE(0x088E, 0x4460, iwl6035_2agn_cfg)},
+	{IWL_PCI_DEVICE(0x088E, 0x4064, iwl6035_2abg_cfg)},
+	{IWL_PCI_DEVICE(0x088F, 0x4264, iwl6035_2abg_cfg)},
+	{IWL_PCI_DEVICE(0x088E, 0x4464, iwl6035_2abg_cfg)},
+	{IWL_PCI_DEVICE(0x088E, 0x4066, iwl6035_2bg_cfg)},
+	{IWL_PCI_DEVICE(0x088F, 0x4266, iwl6035_2bg_cfg)},
+	{IWL_PCI_DEVICE(0x088E, 0x4466, iwl6035_2bg_cfg)},
+
+/* 200 Series */
+	{IWL_PCI_DEVICE(0x0894, 0x0022, iwl200_bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0895, 0x0222, iwl200_bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0894, 0x0422, iwl200_bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0894, 0x0026, iwl200_bg_cfg)},
+	{IWL_PCI_DEVICE(0x0895, 0x0226, iwl200_bg_cfg)},
+	{IWL_PCI_DEVICE(0x0894, 0x0426, iwl200_bg_cfg)},
+
+/* 230 Series */
+	{IWL_PCI_DEVICE(0x0892, 0x0062, iwl230_bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0893, 0x0262, iwl230_bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0892, 0x0462, iwl230_bgn_cfg)},
+	{IWL_PCI_DEVICE(0x0892, 0x0066, iwl230_bg_cfg)},
+	{IWL_PCI_DEVICE(0x0893, 0x0266, iwl230_bg_cfg)},
+	{IWL_PCI_DEVICE(0x0892, 0x0466, iwl230_bg_cfg)},
 
 	{0}
 };
@@ -4592,3 +4545,9 @@ MODULE_PARM_DESC(antenna_coupling,
 module_param_named(bt_ch_inhibition, iwlagn_bt_ch_announce, bool, S_IRUGO);
 MODULE_PARM_DESC(bt_ch_inhibition,
 		 "Disable BT channel inhibition (default: enable)");
+
+module_param_named(plcp_check, iwlagn_mod_params.plcp_check, bool, S_IRUGO);
+MODULE_PARM_DESC(plcp_check, "Check plcp health (default: 1 [enabled])");
+
+module_param_named(ack_check, iwlagn_mod_params.ack_check, bool, S_IRUGO);
+MODULE_PARM_DESC(ack_check, "Check ack health (default: 0 [disabled])");
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.h b/drivers/net/wireless/iwlwifi/iwl-agn.h
index da303585f801..20f8e4188994 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.h
@@ -96,6 +96,17 @@ extern struct iwl_cfg iwl100_bgn_cfg;
 extern struct iwl_cfg iwl100_bg_cfg;
 extern struct iwl_cfg iwl130_bgn_cfg;
 extern struct iwl_cfg iwl130_bg_cfg;
+extern struct iwl_cfg iwl2000_2bgn_cfg;
+extern struct iwl_cfg iwl2000_2bg_cfg;
+extern struct iwl_cfg iwl2030_2bgn_cfg;
+extern struct iwl_cfg iwl2030_2bg_cfg;
+extern struct iwl_cfg iwl6035_2agn_cfg;
+extern struct iwl_cfg iwl6035_2abg_cfg;
+extern struct iwl_cfg iwl6035_2bg_cfg;
+extern struct iwl_cfg iwl200_bg_cfg;
+extern struct iwl_cfg iwl200_bgn_cfg;
+extern struct iwl_cfg iwl230_bg_cfg;
+extern struct iwl_cfg iwl230_bgn_cfg;
 
 extern struct iwl_mod_params iwlagn_mod_params;
 extern struct iwl_hcmd_ops iwlagn_hcmd;
@@ -110,8 +121,6 @@ void iwl_disable_ict(struct iwl_priv *priv);
 int iwl_alloc_isr_ict(struct iwl_priv *priv);
 void iwl_free_isr_ict(struct iwl_priv *priv);
 irqreturn_t iwl_isr_ict(int irq, void *data);
-bool iwl_good_ack_health(struct iwl_priv *priv,
-			 struct iwl_rx_packet *pkt);
 
 /* tx queue */
 void iwlagn_set_wr_ptrs(struct iwl_priv *priv,
@@ -181,11 +190,7 @@ void iwlagn_rx_replenish_now(struct iwl_priv *priv);
 void iwlagn_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
 int iwlagn_rxq_stop(struct iwl_priv *priv);
 int iwlagn_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band);
-void iwlagn_rx_reply_rx(struct iwl_priv *priv,
-		     struct iwl_rx_mem_buffer *rxb);
-void iwlagn_rx_reply_rx_phy(struct iwl_priv *priv,
-			 struct iwl_rx_mem_buffer *rxb);
-void iwl_rx_handle(struct iwl_priv *priv);
+void iwl_setup_rx_handlers(struct iwl_priv *priv);
 
 /* tx */
 void iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq);
@@ -235,16 +240,6 @@ static inline bool iwl_is_tx_success(u32 status)
 
 u8 iwl_toggle_tx_ant(struct iwl_priv *priv, u8 ant_idx, u8 valid);
 
-/* rx */
-void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
-				struct iwl_rx_mem_buffer *rxb);
-bool iwl_good_plcp_health(struct iwl_priv *priv,
-			  struct iwl_rx_packet *pkt);
-void iwl_rx_statistics(struct iwl_priv *priv,
-		       struct iwl_rx_mem_buffer *rxb);
-void iwl_reply_statistics(struct iwl_priv *priv,
-			  struct iwl_rx_mem_buffer *rxb);
-
 /* scan */
 int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif);
 void iwlagn_post_scan(struct iwl_priv *priv);
@@ -330,8 +325,23 @@ void iwl_eeprom_get_mac(const struct iwl_priv *priv, u8 *mac);
 int iwlcore_eeprom_acquire_semaphore(struct iwl_priv *priv);
 void iwlcore_eeprom_release_semaphore(struct iwl_priv *priv);
 
+/* notification wait support */
+void __acquires(wait_entry)
+iwlagn_init_notification_wait(struct iwl_priv *priv,
+			      struct iwl_notification_wait *wait_entry,
+			      void (*fn)(struct iwl_priv *priv,
+					 struct iwl_rx_packet *pkt),
+			      u8 cmd);
+signed long __releases(wait_entry)
+iwlagn_wait_notification(struct iwl_priv *priv,
+			 struct iwl_notification_wait *wait_entry,
+			 unsigned long timeout);
+void __releases(wait_entry)
+iwlagn_remove_notification(struct iwl_priv *priv,
+			   struct iwl_notification_wait *wait_entry);
+
 /* mac80211 handlers (for 4965) */
-int iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
+void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 int iwlagn_mac_start(struct ieee80211_hw *hw);
 void iwlagn_mac_stop(struct ieee80211_hw *hw);
 void iwlagn_configure_filter(struct ieee80211_hw *hw,
@@ -349,7 +359,8 @@ void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
 int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 			    struct ieee80211_vif *vif,
 			    enum ieee80211_ampdu_mlme_action action,
-			    struct ieee80211_sta *sta, u16 tid, u16 *ssn);
+			    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			    u8 buf_size);
 int iwlagn_mac_sta_add(struct ieee80211_hw *hw,
 		       struct ieee80211_vif *vif,
 		       struct ieee80211_sta *sta);
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
index f893d4a6aa87..03cfb74da2bc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -178,7 +178,6 @@ enum {
 	REPLY_BT_COEX_PRIO_TABLE = 0xcc,
 	REPLY_BT_COEX_PROT_ENV = 0xcd,
 	REPLY_BT_COEX_PROFILE_NOTIF = 0xce,
-	REPLY_BT_COEX_SCO = 0xcf,
 
 	/* PAN commands */
 	REPLY_WIPAN_PARAMS = 0xb2,
@@ -189,6 +188,7 @@ enum {
 	REPLY_WIPAN_WEPKEY = 0xb8,	/* use REPLY_WEPKEY structure */
 	REPLY_WIPAN_P2P_CHANNEL_SWITCH = 0xb9,
 	REPLY_WIPAN_NOA_NOTIFICATION = 0xbc,
+	REPLY_WIPAN_DEACTIVATION_COMPLETE = 0xbd,
 
 	REPLY_MAX = 0xff
 };
@@ -2477,7 +2477,7 @@ struct iwl_bt_cmd {
 					IWLAGN_BT_VALID_BT4_TIMES | \
 					IWLAGN_BT_VALID_3W_LUT)
 
-struct iwlagn_bt_cmd {
+struct iwl_basic_bt_cmd {
 	u8 flags;
 	u8 ledtime; /* unused */
 	u8 max_kill;
@@ -2490,6 +2490,10 @@ struct iwlagn_bt_cmd {
 	__le32 bt3_lookup_table[12];
 	__le16 bt4_decision_time; /* unused */
 	__le16 valid;
+};
+
+struct iwl6000_bt_cmd {
+	struct iwl_basic_bt_cmd basic;
 	u8 prio_boost;
 	/*
 	 * set IWLAGN_BT_VALID_BOOST to "1" in "valid" bitmask
@@ -2499,6 +2503,18 @@ struct iwlagn_bt_cmd {
 	__le16 rx_prio_boost;	/* SW boost of WiFi rx priority */
 };
 
+struct iwl2000_bt_cmd {
+	struct iwl_basic_bt_cmd basic;
+	__le32 prio_boost;
+	/*
+	 * set IWLAGN_BT_VALID_BOOST to "1" in "valid" bitmask
+	 * if configure the following patterns
+	 */
+	u8 reserved;
+	u8 tx_prio_boost;	/* SW boost of WiFi tx priority */
+	__le16 rx_prio_boost;	/* SW boost of WiFi rx priority */
+};
+
 #define IWLAGN_BT_SCO_ACTIVE	cpu_to_le32(BIT(0))
 
 struct iwlagn_bt_sco_cmd {
@@ -3082,6 +3098,13 @@ struct iwl4965_beacon_notif {
 	__le32 ibss_mgr_status;
 } __packed;
 
+struct iwlagn_beacon_notif {
+	struct iwlagn_tx_resp beacon_notify_hdr;
+	__le32 low_tsf;
+	__le32 high_tsf;
+	__le32 ibss_mgr_status;
+} __packed;
+
 /*
  * REPLY_TX_BEACON = 0x91 (command, has simple generic response)
  */
@@ -4143,6 +4166,10 @@ enum iwl_bt_coex_profile_traffic_load {
  */
 };
 
+#define BT_SESSION_ACTIVITY_1_UART_MSG		0x1
+#define BT_SESSION_ACTIVITY_2_UART_MSG		0x2
+
+/* BT UART message - Share Part (BT -> WiFi) */
 #define BT_UART_MSG_FRAME1MSGTYPE_POS		(0)
 #define BT_UART_MSG_FRAME1MSGTYPE_MSK		\
 		(0x7 << BT_UART_MSG_FRAME1MSGTYPE_POS)
@@ -4227,9 +4254,12 @@ enum iwl_bt_coex_profile_traffic_load {
 #define BT_UART_MSG_FRAME7SNIFFACTIVITY_POS	(0)
 #define BT_UART_MSG_FRAME7SNIFFACTIVITY_MSK	\
 		(0x7 << BT_UART_MSG_FRAME7SNIFFACTIVITY_POS)
-#define BT_UART_MSG_FRAME7INQUIRYPAGESRMODE_POS	(3)
-#define BT_UART_MSG_FRAME7INQUIRYPAGESRMODE_MSK	\
-		(0x3 << BT_UART_MSG_FRAME7INQUIRYPAGESRMODE_POS)
+#define BT_UART_MSG_FRAME7PAGE_POS		(3)
+#define BT_UART_MSG_FRAME7PAGE_MSK		\
+		(0x1 << BT_UART_MSG_FRAME7PAGE_POS)
+#define BT_UART_MSG_FRAME7INQUIRY_POS		(4)
+#define BT_UART_MSG_FRAME7INQUIRY_MSK		\
+		(0x1 << BT_UART_MSG_FRAME7INQUIRY_POS)
 #define BT_UART_MSG_FRAME7CONNECTABLE_POS	(5)
 #define BT_UART_MSG_FRAME7CONNECTABLE_MSK	\
 		(0x1 << BT_UART_MSG_FRAME7CONNECTABLE_POS)
@@ -4237,6 +4267,83 @@ enum iwl_bt_coex_profile_traffic_load {
 #define BT_UART_MSG_FRAME7RESERVED_MSK		\
 		(0x3 << BT_UART_MSG_FRAME7RESERVED_POS)
 
+/* BT Session Activity 2 UART message (BT -> WiFi) */
+#define BT_UART_MSG_2_FRAME1RESERVED1_POS	(5)
+#define BT_UART_MSG_2_FRAME1RESERVED1_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME1RESERVED1_POS)
+#define BT_UART_MSG_2_FRAME1RESERVED2_POS	(6)
+#define BT_UART_MSG_2_FRAME1RESERVED2_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME1RESERVED2_POS)
+
+#define BT_UART_MSG_2_FRAME2AGGTRAFFICLOAD_POS	(0)
+#define BT_UART_MSG_2_FRAME2AGGTRAFFICLOAD_MSK	\
+		(0x3F<<BT_UART_MSG_2_FRAME2AGGTRAFFICLOAD_POS)
+#define BT_UART_MSG_2_FRAME2RESERVED_POS	(6)
+#define BT_UART_MSG_2_FRAME2RESERVED_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME2RESERVED_POS)
+
+#define BT_UART_MSG_2_FRAME3BRLASTTXPOWER_POS	(0)
+#define BT_UART_MSG_2_FRAME3BRLASTTXPOWER_MSK	\
+		(0xF<<BT_UART_MSG_2_FRAME3BRLASTTXPOWER_POS)
+#define BT_UART_MSG_2_FRAME3INQPAGESRMODE_POS	(4)
+#define BT_UART_MSG_2_FRAME3INQPAGESRMODE_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME3INQPAGESRMODE_POS)
+#define BT_UART_MSG_2_FRAME3LEMASTER_POS	(5)
+#define BT_UART_MSG_2_FRAME3LEMASTER_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME3LEMASTER_POS)
+#define BT_UART_MSG_2_FRAME3RESERVED_POS	(6)
+#define BT_UART_MSG_2_FRAME3RESERVED_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME3RESERVED_POS)
+
+#define BT_UART_MSG_2_FRAME4LELASTTXPOWER_POS	(0)
+#define BT_UART_MSG_2_FRAME4LELASTTXPOWER_MSK	\
+		(0xF<<BT_UART_MSG_2_FRAME4LELASTTXPOWER_POS)
+#define BT_UART_MSG_2_FRAME4NUMLECONN_POS	(4)
+#define BT_UART_MSG_2_FRAME4NUMLECONN_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME4NUMLECONN_POS)
+#define BT_UART_MSG_2_FRAME4RESERVED_POS	(6)
+#define BT_UART_MSG_2_FRAME4RESERVED_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME4RESERVED_POS)
+
+#define BT_UART_MSG_2_FRAME5BTMINRSSI_POS	(0)
+#define BT_UART_MSG_2_FRAME5BTMINRSSI_MSK	\
+		(0xF<<BT_UART_MSG_2_FRAME5BTMINRSSI_POS)
+#define BT_UART_MSG_2_FRAME5LESCANINITMODE_POS	(4)
+#define BT_UART_MSG_2_FRAME5LESCANINITMODE_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME5LESCANINITMODE_POS)
+#define BT_UART_MSG_2_FRAME5LEADVERMODE_POS	(5)
+#define BT_UART_MSG_2_FRAME5LEADVERMODE_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME5LEADVERMODE_POS)
+#define BT_UART_MSG_2_FRAME5RESERVED_POS	(6)
+#define BT_UART_MSG_2_FRAME5RESERVED_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME5RESERVED_POS)
+
+#define BT_UART_MSG_2_FRAME6LECONNINTERVAL_POS	(0)
+#define BT_UART_MSG_2_FRAME6LECONNINTERVAL_MSK	\
+		(0x1F<<BT_UART_MSG_2_FRAME6LECONNINTERVAL_POS)
+#define BT_UART_MSG_2_FRAME6RFU_POS		(5)
+#define BT_UART_MSG_2_FRAME6RFU_MSK		\
+		(0x1<<BT_UART_MSG_2_FRAME6RFU_POS)
+#define BT_UART_MSG_2_FRAME6RESERVED_POS	(6)
+#define BT_UART_MSG_2_FRAME6RESERVED_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME6RESERVED_POS)
+
+#define BT_UART_MSG_2_FRAME7LECONNSLAVELAT_POS	(0)
+#define BT_UART_MSG_2_FRAME7LECONNSLAVELAT_MSK	\
+		(0x7<<BT_UART_MSG_2_FRAME7LECONNSLAVELAT_POS)
+#define BT_UART_MSG_2_FRAME7LEPROFILE1_POS	(3)
+#define BT_UART_MSG_2_FRAME7LEPROFILE1_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME7LEPROFILE1_POS)
+#define BT_UART_MSG_2_FRAME7LEPROFILE2_POS	(4)
+#define BT_UART_MSG_2_FRAME7LEPROFILE2_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME7LEPROFILE2_POS)
+#define BT_UART_MSG_2_FRAME7LEPROFILEOTHER_POS	(5)
+#define BT_UART_MSG_2_FRAME7LEPROFILEOTHER_MSK	\
+		(0x1<<BT_UART_MSG_2_FRAME7LEPROFILEOTHER_POS)
+#define BT_UART_MSG_2_FRAME7RESERVED_POS	(6)
+#define BT_UART_MSG_2_FRAME7RESERVED_MSK	\
+		(0x3<<BT_UART_MSG_2_FRAME7RESERVED_POS)
+
 
 struct iwl_bt_uart_msg {
 	u8 header;
@@ -4369,6 +4476,11 @@ int iwl_agn_check_rxon_cmd(struct iwl_priv *priv);
  * REPLY_WIPAN_PARAMS = 0xb2 (Commands and Notification)
  */
 
+/*
+ * Minimum slot time in TU
+ */
+#define IWL_MIN_SLOT_TIME	20
+
 /**
  * struct iwl_wipan_slot
  * @width: Time in TU
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index efbde1f1a8bf..6c30fa652e27 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -43,11 +43,6 @@
 #include "iwl-helpers.h"
 
 
-MODULE_DESCRIPTION("iwl core");
-MODULE_VERSION(IWLWIFI_VERSION);
-MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
-MODULE_LICENSE("GPL");
-
 /*
  * set bt_coex_active to true, uCode will do kill/defer
  * every time the priority line is asserted (BT is sending signals on the
@@ -65,15 +60,12 @@ MODULE_LICENSE("GPL");
  * default: bt_coex_active = true (BT_COEX_ENABLE)
  */
 bool bt_coex_active = true;
-EXPORT_SYMBOL_GPL(bt_coex_active);
 module_param(bt_coex_active, bool, S_IRUGO);
 MODULE_PARM_DESC(bt_coex_active, "enable wifi/bluetooth co-exist");
 
 u32 iwl_debug_level;
-EXPORT_SYMBOL(iwl_debug_level);
 
 const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-EXPORT_SYMBOL(iwl_bcast_addr);
 
 
 /* This function both allocates and initializes hw and priv. */
@@ -98,7 +90,6 @@ struct ieee80211_hw *iwl_alloc_all(struct iwl_cfg *cfg)
 out:
 	return hw;
 }
-EXPORT_SYMBOL(iwl_alloc_all);
 
 #define MAX_BIT_RATE_40_MHZ 150 /* Mbps */
 #define MAX_BIT_RATE_20_MHZ 72 /* Mbps */
@@ -219,15 +210,12 @@ int iwlcore_init_geos(struct iwl_priv *priv)
 		if (!is_channel_valid(ch))
 			continue;
 
-		if (is_channel_a_band(ch))
-			sband =  &priv->bands[IEEE80211_BAND_5GHZ];
-		else
-			sband =  &priv->bands[IEEE80211_BAND_2GHZ];
+		sband =  &priv->bands[ch->band];
 
 		geo_ch = &sband->channels[sband->n_channels++];
 
 		geo_ch->center_freq =
-				ieee80211_channel_to_frequency(ch->channel);
+			ieee80211_channel_to_frequency(ch->channel, ch->band);
 		geo_ch->max_power = ch->max_power_avg;
 		geo_ch->max_antenna_gain = 0xff;
 		geo_ch->hw_value = ch->channel;
@@ -275,7 +263,6 @@ int iwlcore_init_geos(struct iwl_priv *priv)
 
 	return 0;
 }
-EXPORT_SYMBOL(iwlcore_init_geos);
 
 /*
  * iwlcore_free_geos - undo allocations in iwlcore_init_geos
@@ -286,7 +273,6 @@ void iwlcore_free_geos(struct iwl_priv *priv)
 	kfree(priv->ieee_rates);
 	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
 }
-EXPORT_SYMBOL(iwlcore_free_geos);
 
 static bool iwl_is_channel_extension(struct iwl_priv *priv,
 				     enum ieee80211_band band,
@@ -331,7 +317,6 @@ bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
 			le16_to_cpu(ctx->staging.channel),
 			ctx->ht.extension_chan_offset);
 }
-EXPORT_SYMBOL(iwl_is_ht40_tx_allowed);
 
 static u16 iwl_adjust_beacon_interval(u16 beacon_val, u16 max_beacon_val)
 {
@@ -432,7 +417,6 @@ int iwl_send_rxon_timing(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	return iwl_send_cmd_pdu(priv, ctx->rxon_timing_cmd,
 				sizeof(ctx->timing), &ctx->timing);
 }
-EXPORT_SYMBOL(iwl_send_rxon_timing);
 
 void iwl_set_rxon_hwcrypto(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 			   int hw_decrypt)
@@ -445,7 +429,6 @@ void iwl_set_rxon_hwcrypto(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 		rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;
 
 }
-EXPORT_SYMBOL(iwl_set_rxon_hwcrypto);
 
 /* validate RXON structure is valid */
 int iwl_check_rxon_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
@@ -518,7 +501,6 @@ int iwl_check_rxon_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	}
 	return 0;
 }
-EXPORT_SYMBOL(iwl_check_rxon_cmd);
 
 /**
  * iwl_full_rxon_required - check if full RXON (vs RXON_ASSOC) cmd is needed
@@ -582,7 +564,6 @@ int iwl_full_rxon_required(struct iwl_priv *priv,
 
 	return 0;
 }
-EXPORT_SYMBOL(iwl_full_rxon_required);
 
 u8 iwl_rate_get_lowest_plcp(struct iwl_priv *priv,
 			    struct iwl_rxon_context *ctx)
@@ -596,7 +577,6 @@ u8 iwl_rate_get_lowest_plcp(struct iwl_priv *priv,
 	else
 		return IWL_RATE_6M_PLCP;
 }
-EXPORT_SYMBOL(iwl_rate_get_lowest_plcp);
 
 static void _iwl_set_rxon_ht(struct iwl_priv *priv,
 			     struct iwl_ht_config *ht_conf,
@@ -673,7 +653,6 @@ void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf)
 	for_each_context(priv, ctx)
 		_iwl_set_rxon_ht(priv, ht_conf, ctx);
 }
-EXPORT_SYMBOL(iwl_set_rxon_ht);
 
 /* Return valid, unused, channel for a passive scan to reset the RF */
 u8 iwl_get_single_channel_number(struct iwl_priv *priv,
@@ -714,7 +693,6 @@ u8 iwl_get_single_channel_number(struct iwl_priv *priv,
 
 	return channel;
 }
-EXPORT_SYMBOL(iwl_get_single_channel_number);
 
 /**
  * iwl_set_rxon_channel - Set the band and channel values in staging RXON
@@ -745,7 +723,6 @@ int iwl_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch,
 
 	return 0;
 }
-EXPORT_SYMBOL(iwl_set_rxon_channel);
 
 void iwl_set_flags_for_band(struct iwl_priv *priv,
 			    struct iwl_rxon_context *ctx,
@@ -769,7 +746,6 @@ void iwl_set_flags_for_band(struct iwl_priv *priv,
 		ctx->staging.flags &= ~RXON_FLG_CCK_MSK;
 	}
 }
-EXPORT_SYMBOL(iwl_set_flags_for_band);
 
 /*
  * initialize rxon structure with default values from eeprom
@@ -841,7 +817,6 @@ void iwl_connection_init_rx_config(struct iwl_priv *priv,
 	ctx->staging.ofdm_ht_dual_stream_basic_rates = 0xff;
 	ctx->staging.ofdm_ht_triple_stream_basic_rates = 0xff;
 }
-EXPORT_SYMBOL(iwl_connection_init_rx_config);
 
 void iwl_set_rate(struct iwl_priv *priv)
 {
@@ -874,7 +849,6 @@ void iwl_set_rate(struct iwl_priv *priv)
 		   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
 	}
 }
-EXPORT_SYMBOL(iwl_set_rate);
 
 void iwl_chswitch_done(struct iwl_priv *priv, bool is_success)
 {
@@ -894,35 +868,6 @@ void iwl_chswitch_done(struct iwl_priv *priv, bool is_success)
 		mutex_unlock(&priv->mutex);
 	}
 }
-EXPORT_SYMBOL(iwl_chswitch_done);
-
-void iwl_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_csa_notification *csa = &(pkt->u.csa_notif);
-	/*
-	 * MULTI-FIXME
-	 * See iwl_mac_channel_switch.
-	 */
-	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-	struct iwl_rxon_cmd *rxon = (void *)&ctx->active;
-
-	if (priv->switch_rxon.switch_in_progress) {
-		if (!le32_to_cpu(csa->status) &&
-		    (csa->channel == priv->switch_rxon.channel)) {
-			rxon->channel = csa->channel;
-			ctx->staging.channel = csa->channel;
-			IWL_DEBUG_11H(priv, "CSA notif: channel %d\n",
-			      le16_to_cpu(csa->channel));
-			iwl_chswitch_done(priv, true);
-		} else {
-			IWL_ERR(priv, "CSA notif (fail) : channel %d\n",
-			      le16_to_cpu(csa->channel));
-			iwl_chswitch_done(priv, false);
-		}
-	}
-}
-EXPORT_SYMBOL(iwl_rx_csa);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
 void iwl_print_rx_config_cmd(struct iwl_priv *priv,
@@ -944,13 +889,15 @@ void iwl_print_rx_config_cmd(struct iwl_priv *priv,
 	IWL_DEBUG_RADIO(priv, "u8[6] bssid_addr: %pM\n", rxon->bssid_addr);
 	IWL_DEBUG_RADIO(priv, "u16 assoc_id: 0x%x\n", le16_to_cpu(rxon->assoc_id));
 }
-EXPORT_SYMBOL(iwl_print_rx_config_cmd);
 #endif
 /**
  * iwl_irq_handle_error - called for HW or SW error interrupt from card
  */
 void iwl_irq_handle_error(struct iwl_priv *priv)
 {
+	unsigned int reload_msec;
+	unsigned long reload_jiffies;
+
 	/* Set the FW error flag -- cleared on iwl_down */
 	set_bit(STATUS_FW_ERROR, &priv->status);
 
@@ -994,6 +941,25 @@ void iwl_irq_handle_error(struct iwl_priv *priv)
 	 * commands by clearing the INIT status bit */
 	clear_bit(STATUS_READY, &priv->status);
 
+	/*
+	 * If firmware keep reloading, then it indicate something
+	 * serious wrong and firmware having problem to recover
+	 * from it. Instead of keep trying which will fill the syslog
+	 * and hang the system, let's just stop it
+	 */
+	reload_jiffies = jiffies;
+	reload_msec = jiffies_to_msecs((long) reload_jiffies -
+				(long) priv->reload_jiffies);
+	priv->reload_jiffies = reload_jiffies;
+	if (reload_msec <= IWL_MIN_RELOAD_DURATION) {
+		priv->reload_count++;
+		if (priv->reload_count >= IWL_MAX_CONTINUE_RELOAD_CNT) {
+			IWL_ERR(priv, "BUG_ON, Stop restarting\n");
+			return;
+		}
+	} else
+		priv->reload_count = 0;
+
 	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
 		IWL_DEBUG(priv, IWL_DL_FW_ERRORS,
 			  "Restarting adapter due to uCode error.\n");
@@ -1002,7 +968,6 @@ void iwl_irq_handle_error(struct iwl_priv *priv)
 			queue_work(priv->workqueue, &priv->restart);
 	}
 }
-EXPORT_SYMBOL(iwl_irq_handle_error);
 
 static int iwl_apm_stop_master(struct iwl_priv *priv)
 {
@@ -1039,7 +1004,6 @@ void iwl_apm_stop(struct iwl_priv *priv)
 	 */
 	iwl_clear_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 }
-EXPORT_SYMBOL(iwl_apm_stop);
 
 
 /*
@@ -1154,13 +1118,14 @@ int iwl_apm_init(struct iwl_priv *priv)
 out:
 	return ret;
 }
-EXPORT_SYMBOL(iwl_apm_init);
 
 
 int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
 {
 	int ret;
 	s8 prev_tx_power;
+	bool defer;
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
 
 	lockdep_assert_held(&priv->mutex);
 
@@ -1188,10 +1153,15 @@ int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
 	if (!iwl_is_ready_rf(priv))
 		return -EIO;
 
-	/* scan complete use tx_power_next, need to be updated */
+	/* scan complete and commit_rxon use tx_power_next value,
+	 * it always need to be updated for newest request */
 	priv->tx_power_next = tx_power;
-	if (test_bit(STATUS_SCANNING, &priv->status) && !force) {
-		IWL_DEBUG_INFO(priv, "Deferring tx power set while scanning\n");
+
+	/* do not set tx power when scanning or channel changing */
+	defer = test_bit(STATUS_SCANNING, &priv->status) ||
+		memcmp(&ctx->active, &ctx->staging, sizeof(ctx->staging));
+	if (defer && !force) {
+		IWL_DEBUG_INFO(priv, "Deferring tx power set\n");
 		return 0;
 	}
 
@@ -1207,7 +1177,6 @@ int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
 	}
 	return ret;
 }
-EXPORT_SYMBOL(iwl_set_tx_power);
 
 void iwl_send_bt_config(struct iwl_priv *priv)
 {
@@ -1231,7 +1200,6 @@ void iwl_send_bt_config(struct iwl_priv *priv)
 			     sizeof(struct iwl_bt_cmd), &bt_cmd))
 		IWL_ERR(priv, "failed to send BT Coex Config\n");
 }
-EXPORT_SYMBOL(iwl_send_bt_config);
 
 int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags, bool clear)
 {
@@ -1249,46 +1217,6 @@ int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags, bool clear)
 					sizeof(struct iwl_statistics_cmd),
 					&statistics_cmd);
 }
-EXPORT_SYMBOL(iwl_send_statistics_request);
-
-void iwl_rx_pm_sleep_notif(struct iwl_priv *priv,
-			   struct iwl_rx_mem_buffer *rxb)
-{
-#ifdef CONFIG_IWLWIFI_DEBUG
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_sleep_notification *sleep = &(pkt->u.sleep_notif);
-	IWL_DEBUG_RX(priv, "sleep mode: %d, src: %d\n",
-		     sleep->pm_sleep_mode, sleep->pm_wakeup_src);
-#endif
-}
-EXPORT_SYMBOL(iwl_rx_pm_sleep_notif);
-
-void iwl_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
-				      struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	u32 len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-	IWL_DEBUG_RADIO(priv, "Dumping %d bytes of unhandled "
-			"notification for %s:\n", len,
-			get_cmd_string(pkt->hdr.cmd));
-	iwl_print_hex_dump(priv, IWL_DL_RADIO, pkt->u.raw, len);
-}
-EXPORT_SYMBOL(iwl_rx_pm_debug_statistics_notif);
-
-void iwl_rx_reply_error(struct iwl_priv *priv,
-			struct iwl_rx_mem_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-
-	IWL_ERR(priv, "Error Reply type 0x%08X cmd %s (0x%02X) "
-		"seq 0x%04X ser 0x%08X\n",
-		le32_to_cpu(pkt->u.err_resp.error_type),
-		get_cmd_string(pkt->u.err_resp.cmd_id),
-		pkt->u.err_resp.cmd_id,
-		le16_to_cpu(pkt->u.err_resp.bad_cmd_seq_num),
-		le32_to_cpu(pkt->u.err_resp.error_info));
-}
-EXPORT_SYMBOL(iwl_rx_reply_error);
 
 void iwl_clear_isr_stats(struct iwl_priv *priv)
 {
@@ -1340,7 +1268,6 @@ int iwl_mac_conf_tx(struct ieee80211_hw *hw, u16 queue,
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 	return 0;
 }
-EXPORT_SYMBOL(iwl_mac_conf_tx);
 
 int iwl_mac_tx_last_beacon(struct ieee80211_hw *hw)
 {
@@ -1348,7 +1275,6 @@ int iwl_mac_tx_last_beacon(struct ieee80211_hw *hw)
 
 	return priv->ibss_manager == IWL_IBSS_MANAGER;
 }
-EXPORT_SYMBOL_GPL(iwl_mac_tx_last_beacon);
 
 static int iwl_set_mode(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
@@ -1403,9 +1329,10 @@ int iwl_mac_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
 	struct iwl_rxon_context *tmp, *ctx = NULL;
 	int err;
+	enum nl80211_iftype viftype = ieee80211_vif_type_p2p(vif);
 
 	IWL_DEBUG_MAC80211(priv, "enter: type %d, addr %pM\n",
-			   vif->type, vif->addr);
+			   viftype, vif->addr);
 
 	mutex_lock(&priv->mutex);
 
@@ -1429,7 +1356,7 @@ int iwl_mac_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 			continue;
 		}
 
-		if (!(possible_modes & BIT(vif->type)))
+		if (!(possible_modes & BIT(viftype)))
 			continue;
 
 		/* have maybe usable context w/o interface */
@@ -1457,7 +1384,6 @@ int iwl_mac_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 	return err;
 }
-EXPORT_SYMBOL(iwl_mac_add_interface);
 
 static void iwl_teardown_interface(struct iwl_priv *priv,
 				   struct ieee80211_vif *vif,
@@ -1510,7 +1436,6 @@ void iwl_mac_remove_interface(struct ieee80211_hw *hw,
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 
 }
-EXPORT_SYMBOL(iwl_mac_remove_interface);
 
 int iwl_alloc_txq_mem(struct iwl_priv *priv)
 {
@@ -1525,14 +1450,12 @@ int iwl_alloc_txq_mem(struct iwl_priv *priv)
 	}
 	return 0;
 }
-EXPORT_SYMBOL(iwl_alloc_txq_mem);
 
 void iwl_free_txq_mem(struct iwl_priv *priv)
 {
 	kfree(priv->txq);
 	priv->txq = NULL;
 }
-EXPORT_SYMBOL(iwl_free_txq_mem);
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 
@@ -1571,7 +1494,6 @@ int iwl_alloc_traffic_mem(struct iwl_priv *priv)
 	iwl_reset_traffic_log(priv);
 	return 0;
 }
-EXPORT_SYMBOL(iwl_alloc_traffic_mem);
 
 void iwl_free_traffic_mem(struct iwl_priv *priv)
 {
@@ -1581,7 +1503,6 @@ void iwl_free_traffic_mem(struct iwl_priv *priv)
 	kfree(priv->rx_traffic);
 	priv->rx_traffic = NULL;
 }
-EXPORT_SYMBOL(iwl_free_traffic_mem);
 
 void iwl_dbg_log_tx_data_frame(struct iwl_priv *priv,
 		      u16 length, struct ieee80211_hdr *header)
@@ -1606,7 +1527,6 @@ void iwl_dbg_log_tx_data_frame(struct iwl_priv *priv,
 			(priv->tx_traffic_idx + 1) % IWL_TRAFFIC_ENTRIES;
 	}
 }
-EXPORT_SYMBOL(iwl_dbg_log_tx_data_frame);
 
 void iwl_dbg_log_rx_data_frame(struct iwl_priv *priv,
 		      u16 length, struct ieee80211_hdr *header)
@@ -1631,7 +1551,6 @@ void iwl_dbg_log_rx_data_frame(struct iwl_priv *priv,
 			(priv->rx_traffic_idx + 1) % IWL_TRAFFIC_ENTRIES;
 	}
 }
-EXPORT_SYMBOL(iwl_dbg_log_rx_data_frame);
 
 const char *get_mgmt_string(int cmd)
 {
@@ -1675,7 +1594,6 @@ void iwl_clear_traffic_stats(struct iwl_priv *priv)
 {
 	memset(&priv->tx_stats, 0, sizeof(struct traffic_stats));
 	memset(&priv->rx_stats, 0, sizeof(struct traffic_stats));
-	priv->led_tpt = 0;
 }
 
 /*
@@ -1768,9 +1686,7 @@ void iwl_update_stats(struct iwl_priv *priv, bool is_tx, __le16 fc, u16 len)
 		stats->data_cnt++;
 		stats->data_bytes += len;
 	}
-	iwl_leds_background(priv);
 }
-EXPORT_SYMBOL(iwl_update_stats);
 #endif
 
 static void iwl_force_rf_reset(struct iwl_priv *priv)
@@ -1909,7 +1825,6 @@ int iwl_mac_change_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	mutex_unlock(&priv->mutex);
 	return err;
 }
-EXPORT_SYMBOL(iwl_mac_change_interface);
 
 /*
  * On every watchdog tick we check (latest) time stamp. If it does not
@@ -1981,7 +1896,6 @@ void iwl_bg_watchdog(unsigned long data)
 	mod_timer(&priv->watchdog, jiffies +
 		  msecs_to_jiffies(IWL_WD_TICK(timeout)));
 }
-EXPORT_SYMBOL(iwl_bg_watchdog);
 
 void iwl_setup_watchdog(struct iwl_priv *priv)
 {
@@ -1993,7 +1907,6 @@ void iwl_setup_watchdog(struct iwl_priv *priv)
 	else
 		del_timer(&priv->watchdog);
 }
-EXPORT_SYMBOL(iwl_setup_watchdog);
 
 /*
  * extended beacon time format
@@ -2019,7 +1932,6 @@ u32 iwl_usecs_to_beacons(struct iwl_priv *priv, u32 usec, u32 beacon_interval)
 
 	return (quot << priv->hw_params.beacon_time_tsf_bits) + rem;
 }
-EXPORT_SYMBOL(iwl_usecs_to_beacons);
 
 /* base is usually what we get from ucode with each received frame,
  * the same as HW timer counter counting down
@@ -2047,7 +1959,6 @@ __le32 iwl_add_beacon_time(struct iwl_priv *priv, u32 base,
 
 	return cpu_to_le32(res);
 }
-EXPORT_SYMBOL(iwl_add_beacon_time);
 
 #ifdef CONFIG_PM
 
@@ -2067,7 +1978,6 @@ int iwl_pci_suspend(struct device *device)
 
 	return 0;
 }
-EXPORT_SYMBOL(iwl_pci_suspend);
 
 int iwl_pci_resume(struct device *device)
 {
@@ -2096,7 +2006,6 @@ int iwl_pci_resume(struct device *device)
 
 	return 0;
 }
-EXPORT_SYMBOL(iwl_pci_resume);
 
 const struct dev_pm_ops iwl_pm_ops = {
 	.suspend = iwl_pci_suspend,
@@ -2106,6 +2015,5 @@ const struct dev_pm_ops iwl_pm_ops = {
 	.poweroff = iwl_pci_suspend,
 	.restore = iwl_pci_resume,
 };
-EXPORT_SYMBOL(iwl_pm_ops);
 
 #endif /* CONFIG_PM */
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index a3474376fdbc..af47750f8985 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -210,12 +210,7 @@ struct iwl_lib_ops {
 
 	/* temperature */
 	struct iwl_temp_ops temp_ops;
-	/* check for plcp health */
-	bool (*check_plcp_health)(struct iwl_priv *priv,
-					struct iwl_rx_packet *pkt);
-	/* check for ack health */
-	bool (*check_ack_health)(struct iwl_priv *priv,
-					struct iwl_rx_packet *pkt);
+
 	int (*txfifo_flush)(struct iwl_priv *priv, u16 flush_control);
 	void (*dev_txfifo_flush)(struct iwl_priv *priv, u16 flush_control);
 
@@ -227,8 +222,6 @@ struct iwl_lib_ops {
 
 struct iwl_led_ops {
 	int (*cmd)(struct iwl_priv *priv, struct iwl_led_cmd *led_cmd);
-	int (*on)(struct iwl_priv *priv);
-	int (*off)(struct iwl_priv *priv);
 };
 
 /* NIC specific ops */
@@ -263,6 +256,8 @@ struct iwl_mod_params {
 	int amsdu_size_8K;	/* def: 1 = enable 8K amsdu size */
 	int antenna;  		/* def: 0 = both antennas (use diversity) */
 	int restart_fw;		/* def: 1 = restart firmware */
+	bool plcp_check;	/* def: true = enable plcp health check */
+	bool ack_check;		/* def: false = disable ack health check */
 };
 
 /*
@@ -307,7 +302,6 @@ struct iwl_base_params {
 	u16 led_compensation;
 	const bool broken_powersave;
 	int chain_noise_num_beacons;
-	const bool supports_idle;
 	bool adv_thermal_throttle;
 	bool support_ct_kill_exit;
 	const bool support_wimax_coexist;
@@ -342,6 +336,7 @@ struct iwl_bt_params {
 	u8 ampdu_factor;
 	u8 ampdu_density;
 	bool bt_sco_disable;
+	bool bt_session_2;
 };
 /*
  * @use_rts_for_aggregation: use rts/cts protection for HT traffic
@@ -366,6 +361,7 @@ struct iwl_ht_params {
  * @adv_pm: advance power management
  * @rx_with_siso_diversity: 1x1 device with rx antenna diversity
  * @internal_wimax_coex: internal wifi/wimax combo device
+ * @iq_invert: I/Q inversion
  *
  * We enable the driver to be backward compatible wrt API version. The
  * driver specifies which APIs it supports (with @ucode_api_max being the
@@ -415,6 +411,7 @@ struct iwl_cfg {
 	const bool adv_pm;
 	const bool rx_with_siso_diversity;
 	const bool internal_wimax_coex;
+	const bool iq_invert;
 };
 
 /***************************
@@ -444,10 +441,6 @@ bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
 void iwl_connection_init_rx_config(struct iwl_priv *priv,
 				   struct iwl_rxon_context *ctx);
 void iwl_set_rate(struct iwl_priv *priv);
-int iwl_set_decrypted_flag(struct iwl_priv *priv,
-			   struct ieee80211_hdr *hdr,
-			   u32 decrypt_res,
-			   struct ieee80211_rx_status *stats);
 void iwl_irq_handle_error(struct iwl_priv *priv);
 int iwl_mac_add_interface(struct ieee80211_hw *hw,
 			  struct ieee80211_vif *vif);
@@ -494,46 +487,21 @@ static inline void iwl_dbg_log_rx_data_frame(struct iwl_priv *priv,
 static inline void iwl_update_stats(struct iwl_priv *priv, bool is_tx,
 				    __le16 fc, u16 len)
 {
-	struct traffic_stats	*stats;
-
-	if (is_tx)
-		stats = &priv->tx_stats;
-	else
-		stats = &priv->rx_stats;
-
-	if (ieee80211_is_data(fc)) {
-		/* data */
-		stats->data_bytes += len;
-	}
-	iwl_leds_background(priv);
 }
 #endif
-/*****************************************************
- * RX handlers.
- * **************************************************/
-void iwl_rx_pm_sleep_notif(struct iwl_priv *priv,
-			   struct iwl_rx_mem_buffer *rxb);
-void iwl_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
-				      struct iwl_rx_mem_buffer *rxb);
-void iwl_rx_reply_error(struct iwl_priv *priv,
-			struct iwl_rx_mem_buffer *rxb);
 
 /*****************************************************
 * RX
 ******************************************************/
 void iwl_cmd_queue_free(struct iwl_priv *priv);
+void iwl_cmd_queue_unmap(struct iwl_priv *priv);
 int iwl_rx_queue_alloc(struct iwl_priv *priv);
 void iwl_rx_queue_update_write_ptr(struct iwl_priv *priv,
 				  struct iwl_rx_queue *q);
 int iwl_rx_queue_space(const struct iwl_rx_queue *q);
 void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb);
-/* Handlers */
-void iwl_rx_spectrum_measure_notif(struct iwl_priv *priv,
-					  struct iwl_rx_mem_buffer *rxb);
-void iwl_recover_from_statistics(struct iwl_priv *priv,
-				struct iwl_rx_packet *pkt);
+
 void iwl_chswitch_done(struct iwl_priv *priv, bool is_success);
-void iwl_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb);
 
 /* TX helpers */
 
@@ -546,6 +514,7 @@ int iwl_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq,
 void iwl_tx_queue_reset(struct iwl_priv *priv, struct iwl_tx_queue *txq,
 			int slots_num, u32 txq_id);
 void iwl_tx_queue_free(struct iwl_priv *priv, int txq_id);
+void iwl_tx_queue_unmap(struct iwl_priv *priv, int txq_id);
 void iwl_setup_watchdog(struct iwl_priv *priv);
 /*****************************************************
  * TX power
@@ -755,6 +724,17 @@ static inline const struct ieee80211_supported_band *iwl_get_hw_mode(
 	return priv->hw->wiphy->bands[band];
 }
 
+static inline bool iwl_advanced_bt_coexist(struct iwl_priv *priv)
+{
+	return priv->cfg->bt_params &&
+	       priv->cfg->bt_params->advanced_bt_coexist;
+}
+
+static inline bool iwl_bt_statistics(struct iwl_priv *priv)
+{
+	return priv->cfg->bt_params && priv->cfg->bt_params->bt_statistics;
+}
+
 extern bool bt_coex_active;
 extern bool bt_siso_mode;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index b80bf7dff55b..f52bc040bcbf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -290,7 +290,7 @@
 
 
 /* HW REV */
-#define CSR_HW_REV_TYPE_MSK            (0x00000F0)
+#define CSR_HW_REV_TYPE_MSK            (0x00001F0)
 #define CSR_HW_REV_TYPE_3945           (0x00000D0)
 #define CSR_HW_REV_TYPE_4965           (0x0000000)
 #define CSR_HW_REV_TYPE_5300           (0x0000020)
@@ -300,9 +300,15 @@
 #define CSR_HW_REV_TYPE_1000           (0x0000060)
 #define CSR_HW_REV_TYPE_6x00           (0x0000070)
 #define CSR_HW_REV_TYPE_6x50           (0x0000080)
-#define CSR_HW_REV_TYPE_6x50g2         (0x0000084)
-#define CSR_HW_REV_TYPE_6x00g2         (0x00000B0)
-#define CSR_HW_REV_TYPE_NONE           (0x00000F0)
+#define CSR_HW_REV_TYPE_6150           (0x0000084)
+#define CSR_HW_REV_TYPE_6x05	       (0x00000B0)
+#define CSR_HW_REV_TYPE_6x30	       CSR_HW_REV_TYPE_6x05
+#define CSR_HW_REV_TYPE_6x35	       CSR_HW_REV_TYPE_6x05
+#define CSR_HW_REV_TYPE_2x30	       (0x00000C0)
+#define CSR_HW_REV_TYPE_2x00	       (0x0000100)
+#define CSR_HW_REV_TYPE_200	       (0x0000110)
+#define CSR_HW_REV_TYPE_230	       (0x0000120)
+#define CSR_HW_REV_TYPE_NONE           (0x00001F0)
 
 /* EEPROM REG */
 #define CSR_EEPROM_REG_READ_VALID_MSK	(0x00000001)
@@ -376,6 +382,8 @@
 #define CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6	    (0x00000004)
 #define CSR_GP_DRIVER_REG_BIT_6050_1x2		    (0x00000008)
 
+#define CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER	    (0x00000080)
+
 /* GIO Chicken Bits (PCI Express bus link power management) */
 #define CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX  (0x00800000)
 #define CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER  (0x20000000)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
index 6fe80b5e7a15..8842411f1cf3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
@@ -207,18 +207,19 @@ static ssize_t iwl_dbgfs_rx_statistics_read(struct file *file,
 	return ret;
 }
 
-#define BYTE1_MASK 0x000000ff;
-#define BYTE2_MASK 0x0000ffff;
-#define BYTE3_MASK 0x00ffffff;
 static ssize_t iwl_dbgfs_sram_read(struct file *file,
 					char __user *user_buf,
 					size_t count, loff_t *ppos)
 {
-	u32 val;
+	u32 val = 0;
 	char *buf;
 	ssize_t ret;
-	int i;
+	int i = 0;
+	bool device_format = false;
+	int offset = 0;
+	int len = 0;
 	int pos = 0;
+	int sram;
 	struct iwl_priv *priv = file->private_data;
 	size_t bufsz;
 
@@ -230,35 +231,62 @@ static ssize_t iwl_dbgfs_sram_read(struct file *file,
 		else
 			priv->dbgfs_sram_len = priv->ucode_data.len;
 	}
-	bufsz =  30 + priv->dbgfs_sram_len * sizeof(char) * 10;
+	len = priv->dbgfs_sram_len;
+
+	if (len == -4) {
+		device_format = true;
+		len = 4;
+	}
+
+	bufsz =  50 + len * 4;
 	buf = kmalloc(bufsz, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
+
 	pos += scnprintf(buf + pos, bufsz - pos, "sram_len: 0x%x\n",
-			priv->dbgfs_sram_len);
+			 len);
 	pos += scnprintf(buf + pos, bufsz - pos, "sram_offset: 0x%x\n",
 			priv->dbgfs_sram_offset);
-	for (i = priv->dbgfs_sram_len; i > 0; i -= 4) {
-		val = iwl_read_targ_mem(priv, priv->dbgfs_sram_offset + \
-					priv->dbgfs_sram_len - i);
-		if (i < 4) {
-			switch (i) {
-			case 1:
-				val &= BYTE1_MASK;
-				break;
-			case 2:
-				val &= BYTE2_MASK;
-				break;
-			case 3:
-				val &= BYTE3_MASK;
-				break;
-			}
+
+	/* adjust sram address since reads are only on even u32 boundaries */
+	offset = priv->dbgfs_sram_offset & 0x3;
+	sram = priv->dbgfs_sram_offset & ~0x3;
+
+	/* read the first u32 from sram */
+	val = iwl_read_targ_mem(priv, sram);
+
+	for (; len; len--) {
+		/* put the address at the start of every line */
+		if (i == 0)
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"%08X: ", sram + offset);
+
+		if (device_format)
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"%02x", (val >> (8 * (3 - offset))) & 0xff);
+		else
+			pos += scnprintf(buf + pos, bufsz - pos,
+				"%02x ", (val >> (8 * offset)) & 0xff);
+
+		/* if all bytes processed, read the next u32 from sram */
+		if (++offset == 4) {
+			sram += 4;
+			offset = 0;
+			val = iwl_read_targ_mem(priv, sram);
 		}
-		if (!(i % 16))
+
+		/* put in extra spaces and split lines for human readability */
+		if (++i == 16) {
+			i = 0;
 			pos += scnprintf(buf + pos, bufsz - pos, "\n");
-		pos += scnprintf(buf + pos, bufsz - pos, "0x%08x ", val);
+		} else if (!(i & 7)) {
+			pos += scnprintf(buf + pos, bufsz - pos, "   ");
+		} else if (!(i & 3)) {
+			pos += scnprintf(buf + pos, bufsz - pos, " ");
+		}
 	}
-	pos += scnprintf(buf + pos, bufsz - pos, "\n");
+	if (i)
+		pos += scnprintf(buf + pos, bufsz - pos, "\n");
 
 	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 	kfree(buf);
@@ -282,6 +310,9 @@ static ssize_t iwl_dbgfs_sram_write(struct file *file,
 	if (sscanf(buf, "%x,%x", &offset, &len) == 2) {
 		priv->dbgfs_sram_offset = offset;
 		priv->dbgfs_sram_len = len;
+	} else if (sscanf(buf, "%x", &offset) == 1) {
+		priv->dbgfs_sram_offset = offset;
+		priv->dbgfs_sram_len = -4;
 	} else {
 		priv->dbgfs_sram_offset = 0;
 		priv->dbgfs_sram_len = 0;
@@ -668,29 +699,6 @@ static ssize_t iwl_dbgfs_qos_read(struct file *file, char __user *user_buf,
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
-static ssize_t iwl_dbgfs_led_read(struct file *file, char __user *user_buf,
-				  size_t count, loff_t *ppos)
-{
-	struct iwl_priv *priv = file->private_data;
-	int pos = 0;
-	char buf[256];
-	const size_t bufsz = sizeof(buf);
-
-	pos += scnprintf(buf + pos, bufsz - pos,
-			 "allow blinking: %s\n",
-			 (priv->allow_blinking) ? "True" : "False");
-	if (priv->allow_blinking) {
-		pos += scnprintf(buf + pos, bufsz - pos,
-				 "Led blinking rate: %u\n",
-				 priv->last_blink_rate);
-		pos += scnprintf(buf + pos, bufsz - pos,
-				 "Last blink time: %lu\n",
-				 priv->last_blink_time);
-	}
-
-	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-}
-
 static ssize_t iwl_dbgfs_thermal_throttling_read(struct file *file,
 				char __user *user_buf,
 				size_t count, loff_t *ppos)
@@ -856,7 +864,6 @@ DEBUGFS_READ_FILE_OPS(channels);
 DEBUGFS_READ_FILE_OPS(status);
 DEBUGFS_READ_WRITE_FILE_OPS(interrupt);
 DEBUGFS_READ_FILE_OPS(qos);
-DEBUGFS_READ_FILE_OPS(led);
 DEBUGFS_READ_FILE_OPS(thermal_throttling);
 DEBUGFS_READ_WRITE_FILE_OPS(disable_ht40);
 DEBUGFS_READ_WRITE_FILE_OPS(sleep_level_override);
@@ -1580,10 +1587,9 @@ static ssize_t iwl_dbgfs_bt_traffic_read(struct file *file,
 			 "last traffic notif: %d\n",
 		priv->bt_status ? "On" : "Off", priv->last_bt_traffic_load);
 	pos += scnprintf(buf + pos, bufsz - pos, "ch_announcement: %d, "
-			 "sco_active: %d, kill_ack_mask: %x, "
-			 "kill_cts_mask: %x\n",
-		priv->bt_ch_announce, priv->bt_sco_active,
-		priv->kill_ack_mask, priv->kill_cts_mask);
+			 "kill_ack_mask: %x, kill_cts_mask: %x\n",
+		priv->bt_ch_announce, priv->kill_ack_mask,
+		priv->kill_cts_mask);
 
 	pos += scnprintf(buf + pos, bufsz - pos, "bluetooth traffic load: ");
 	switch (priv->bt_traffic_load) {
@@ -1725,7 +1731,6 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 	DEBUGFS_ADD_FILE(status, dir_data, S_IRUSR);
 	DEBUGFS_ADD_FILE(interrupt, dir_data, S_IWUSR | S_IRUSR);
 	DEBUGFS_ADD_FILE(qos, dir_data, S_IRUSR);
-	DEBUGFS_ADD_FILE(led, dir_data, S_IRUSR);
 	if (!priv->cfg->base_params->broken_powersave) {
 		DEBUGFS_ADD_FILE(sleep_level_override, dir_data,
 				 S_IWUSR | S_IRUSR);
@@ -1759,13 +1764,13 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 		DEBUGFS_ADD_FILE(chain_noise, dir_debug, S_IRUSR);
 	if (priv->cfg->base_params->ucode_tracing)
 		DEBUGFS_ADD_FILE(ucode_tracing, dir_debug, S_IWUSR | S_IRUSR);
-	if (priv->cfg->bt_params && priv->cfg->bt_params->bt_statistics)
+	if (iwl_bt_statistics(priv))
 		DEBUGFS_ADD_FILE(ucode_bt_stats, dir_debug, S_IRUSR);
 	DEBUGFS_ADD_FILE(reply_tx_error, dir_debug, S_IRUSR);
 	DEBUGFS_ADD_FILE(rxon_flags, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(rxon_filter_flags, dir_debug, S_IWUSR);
 	DEBUGFS_ADD_FILE(wd_timeout, dir_debug, S_IWUSR);
-	if (priv->cfg->bt_params && priv->cfg->bt_params->advanced_bt_coexist)
+	if (iwl_advanced_bt_coexist(priv))
 		DEBUGFS_ADD_FILE(bt_traffic, dir_debug, S_IRUSR);
 	if (priv->cfg->base_params->sensitivity_calib_by_driver)
 		DEBUGFS_ADD_BOOL(disable_sensitivity, dir_rf,
@@ -1783,7 +1788,6 @@ err:
 	iwl_dbgfs_unregister(priv);
 	return -ENOMEM;
 }
-EXPORT_SYMBOL(iwl_dbgfs_register);
 
 /**
  * Remove the debugfs files and directories
@@ -1797,7 +1801,6 @@ void iwl_dbgfs_unregister(struct iwl_priv *priv)
 	debugfs_remove_recursive(priv->debugfs_dir);
 	priv->debugfs_dir = NULL;
 }
-EXPORT_SYMBOL(iwl_dbgfs_unregister);
 
 
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 8dda67850af4..6a41deba6863 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -34,6 +34,8 @@
 
 #include <linux/pci.h> /* for struct pci_device_id */
 #include <linux/kernel.h>
+#include <linux/wait.h>
+#include <linux/leds.h>
 #include <net/ieee80211_radiotap.h>
 
 #include "iwl-eeprom.h"
@@ -41,14 +43,14 @@
 #include "iwl-prph.h"
 #include "iwl-fh.h"
 #include "iwl-debug.h"
-#include "iwl-4965-hw.h"
-#include "iwl-3945-hw.h"
 #include "iwl-agn-hw.h"
 #include "iwl-led.h"
 #include "iwl-power.h"
 #include "iwl-agn-rs.h"
 #include "iwl-agn-tt.h"
 
+#define U32_PAD(n)		((4-(n))&0x3)
+
 struct iwl_tx_queue;
 
 /* CT-KILL constants */
@@ -136,7 +138,7 @@ struct iwl_queue {
 				* space more than this */
 	int high_mark;         /* high watermark, stop queue if free
 				* space less than this */
-} __packed;
+};
 
 /* One for each TFD */
 struct iwl_tx_info {
@@ -507,6 +509,7 @@ struct iwl_station_priv {
 	atomic_t pending_frames;
 	bool client;
 	bool asleep;
+	u8 max_agg_bufsize;
 };
 
 /**
@@ -995,7 +998,6 @@ struct reply_agg_tx_error_statistics {
 	u32 unknown;
 };
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
 /* management statistics */
 enum iwl_mgmt_stats {
 	MANAGEMENT_ASSOC_REQ = 0,
@@ -1026,16 +1028,13 @@ enum iwl_ctrl_stats {
 };
 
 struct traffic_stats {
+#ifdef CONFIG_IWLWIFI_DEBUGFS
 	u32 mgmt[MANAGEMENT_MAX];
 	u32 ctrl[CONTROL_MAX];
 	u32 data_cnt;
 	u64 data_bytes;
-};
-#else
-struct traffic_stats {
-	u64 data_bytes;
-};
 #endif
+};
 
 /*
  * iwl_switch_rxon: "channel switch" structure
@@ -1111,6 +1110,11 @@ struct iwl_event_log {
 /* BT Antenna Coupling Threshold (dB) */
 #define IWL_BT_ANTENNA_COUPLING_THRESHOLD	(35)
 
+/* Firmware reload counter and Timestamp */
+#define IWL_MIN_RELOAD_DURATION		1000 /* 1000 ms */
+#define IWL_MAX_CONTINUE_RELOAD_CNT	4
+
+
 enum iwl_reset {
 	IWL_RF_RESET = 0,
 	IWL_FW_RESET,
@@ -1139,6 +1143,33 @@ struct iwl_force_reset {
  */
 #define IWLAGN_EXT_BEACON_TIME_POS	22
 
+/**
+ * struct iwl_notification_wait - notification wait entry
+ * @list: list head for global list
+ * @fn: function called with the notification
+ * @cmd: command ID
+ *
+ * This structure is not used directly, to wait for a
+ * notification declare it on the stack, and call
+ * iwlagn_init_notification_wait() with appropriate
+ * parameters. Then do whatever will cause the ucode
+ * to notify the driver, and to wait for that then
+ * call iwlagn_wait_notification().
+ *
+ * Each notification is one-shot. If at some point we
+ * need to support multi-shot notifications (which
+ * can't be allocated on the stack) we need to modify
+ * the code for them.
+ */
+struct iwl_notification_wait {
+	struct list_head list;
+
+	void (*fn)(struct iwl_priv *priv, struct iwl_rx_packet *pkt);
+
+	u8 cmd;
+	bool triggered;
+};
+
 enum iwl_rxon_context_id {
 	IWL_RXON_CTX_BSS,
 	IWL_RXON_CTX_PAN,
@@ -1230,12 +1261,16 @@ struct iwl_priv {
 	/* track IBSS manager (last beacon) status */
 	u32 ibss_manager;
 
-	/* storing the jiffies when the plcp error rate is received */
-	unsigned long plcp_jiffies;
+	/* jiffies when last recovery from statistics was performed */
+	unsigned long rx_statistics_jiffies;
 
 	/* force reset */
 	struct iwl_force_reset force_reset[IWL_MAX_FORCE_RESET];
 
+	/* firmware reload counter and timestamp */
+	unsigned long reload_jiffies;
+	int reload_count;
+
 	/* we allocate array of iwl_channel_info for NIC's valid channels.
 	 *    Access via channel # using indirect index array */
 	struct iwl_channel_info *channel_info;	/* channel info array */
@@ -1310,11 +1345,6 @@ struct iwl_priv {
 	struct iwl_init_alive_resp card_alive_init;
 	struct iwl_alive_resp card_alive;
 
-	unsigned long last_blink_time;
-	u8 last_blink_rate;
-	u8 allow_blinking;
-	u64 led_tpt;
-
 	u16 active_rate;
 
 	u8 start_calib;
@@ -1463,6 +1493,17 @@ struct iwl_priv {
 			struct iwl_bt_notif_statistics delta_statistics_bt;
 			struct iwl_bt_notif_statistics max_delta_bt;
 #endif
+
+			/* notification wait support */
+			struct list_head notif_waits;
+			spinlock_t notif_wait_lock;
+			wait_queue_head_t notif_waitq;
+
+			/* remain-on-channel offload support */
+			struct ieee80211_channel *hw_roc_channel;
+			struct delayed_work hw_roc_work;
+			enum nl80211_channel_type hw_roc_chantype;
+			int hw_roc_duration;
 		} _agn;
 #endif
 	};
@@ -1472,7 +1513,6 @@ struct iwl_priv {
 	u8 bt_status;
 	u8 bt_traffic_load, last_bt_traffic_load;
 	bool bt_ch_announce;
-	bool bt_sco_active;
 	bool bt_full_concurrent;
 	bool bt_ant_couple_ok;
 	__le32 kill_ack_mask;
@@ -1547,6 +1587,10 @@ struct iwl_priv {
 	bool hw_ready;
 
 	struct iwl_event_log event_log;
+
+	struct led_classdev led;
+	unsigned long blink_on, blink_off;
+	bool led_registered;
 }; /*iwl_priv */
 
 static inline void iwl_txq_ctx_activate(struct iwl_priv *priv, int txq_id)
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
index 358cfd7e5af1..833194a2c639 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
@@ -222,7 +222,6 @@ const u8 *iwlcore_eeprom_query_addr(const struct iwl_priv *priv, size_t offset)
 	BUG_ON(offset >= priv->cfg->base_params->eeprom_size);
 	return &priv->eeprom[offset];
 }
-EXPORT_SYMBOL(iwlcore_eeprom_query_addr);
 
 static int iwl_init_otp_access(struct iwl_priv *priv)
 {
@@ -382,7 +381,6 @@ const u8 *iwl_eeprom_query_addr(const struct iwl_priv *priv, size_t offset)
 {
 	return priv->cfg->ops->lib->eeprom_ops.query_addr(priv, offset);
 }
-EXPORT_SYMBOL(iwl_eeprom_query_addr);
 
 u16 iwl_eeprom_query16(const struct iwl_priv *priv, size_t offset)
 {
@@ -390,7 +388,6 @@ u16 iwl_eeprom_query16(const struct iwl_priv *priv, size_t offset)
 		return 0;
 	return (u16)priv->eeprom[offset] | ((u16)priv->eeprom[offset + 1] << 8);
 }
-EXPORT_SYMBOL(iwl_eeprom_query16);
 
 /**
  * iwl_eeprom_init - read EEPROM contents
@@ -509,14 +506,12 @@ err:
 alloc_err:
 	return ret;
 }
-EXPORT_SYMBOL(iwl_eeprom_init);
 
 void iwl_eeprom_free(struct iwl_priv *priv)
 {
 	kfree(priv->eeprom);
 	priv->eeprom = NULL;
 }
-EXPORT_SYMBOL(iwl_eeprom_free);
 
 static void iwl_init_band_reference(const struct iwl_priv *priv,
 			int eep_band, int *eeprom_ch_count,
@@ -779,7 +774,6 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 
 	return 0;
 }
-EXPORT_SYMBOL(iwl_init_channel_map);
 
 /*
  * iwl_free_channel_map - undo allocations in iwl_init_channel_map
@@ -789,7 +783,6 @@ void iwl_free_channel_map(struct iwl_priv *priv)
 	kfree(priv->channel_info);
 	priv->channel_count = 0;
 }
-EXPORT_SYMBOL(iwl_free_channel_map);
 
 /**
  * iwl_get_channel_info - Find driver's private channel info
@@ -818,4 +811,3 @@ const struct iwl_channel_info *iwl_get_channel_info(const struct iwl_priv *priv,
 
 	return NULL;
 }
-EXPORT_SYMBOL(iwl_get_channel_info);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
index 9e6f31355eee..98aa8af01192 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
@@ -247,13 +247,26 @@ struct iwl_eeprom_enhanced_txpwr {
 #define EEPROM_6050_TX_POWER_VERSION    (4)
 #define EEPROM_6050_EEPROM_VERSION	(0x532)
 
-/* 6x50g2 Specific */
-#define EEPROM_6050G2_TX_POWER_VERSION    (6)
-#define EEPROM_6050G2_EEPROM_VERSION	(0x553)
+/* 6150 Specific */
+#define EEPROM_6150_TX_POWER_VERSION    (6)
+#define EEPROM_6150_EEPROM_VERSION	(0x553)
+
+/* 6x05 Specific */
+#define EEPROM_6005_TX_POWER_VERSION    (6)
+#define EEPROM_6005_EEPROM_VERSION	(0x709)
+
+/* 6x30 Specific */
+#define EEPROM_6030_TX_POWER_VERSION    (6)
+#define EEPROM_6030_EEPROM_VERSION	(0x709)
+
+/* 2x00 Specific */
+#define EEPROM_2000_TX_POWER_VERSION    (6)
+#define EEPROM_2000_EEPROM_VERSION	(0x805)
+
+/* 6x35 Specific */
+#define EEPROM_6035_TX_POWER_VERSION    (6)
+#define EEPROM_6035_EEPROM_VERSION	(0x753)
 
-/* 6x00g2 Specific */
-#define EEPROM_6000G2_TX_POWER_VERSION    (6)
-#define EEPROM_6000G2_EEPROM_VERSION	(0x709)
 
 /* OTP */
 /* lower blocks contain EEPROM image and calibration data */
@@ -264,6 +277,7 @@ struct iwl_eeprom_enhanced_txpwr {
 #define OTP_MAX_LL_ITEMS_1000		(3)	/* OTP blocks for 1000 */
 #define OTP_MAX_LL_ITEMS_6x00		(4)	/* OTP blocks for 6x00 */
 #define OTP_MAX_LL_ITEMS_6x50		(7)	/* OTP blocks for 6x50 */
+#define OTP_MAX_LL_ITEMS_2x00		(4)	/* OTP blocks for 2x00 */
 
 /* 2.4 GHz */
 extern const u8 iwl_eeprom_band_1[14];
diff --git a/drivers/net/wireless/iwlwifi/iwl-hcmd.c b/drivers/net/wireless/iwlwifi/iwl-hcmd.c
index c373b53babea..02499f684683 100644
--- a/drivers/net/wireless/iwlwifi/iwl-hcmd.c
+++ b/drivers/net/wireless/iwlwifi/iwl-hcmd.c
@@ -108,12 +108,12 @@ const char *get_cmd_string(u8 cmd)
 		IWL_CMD(REPLY_WIPAN_WEPKEY);
 		IWL_CMD(REPLY_WIPAN_P2P_CHANNEL_SWITCH);
 		IWL_CMD(REPLY_WIPAN_NOA_NOTIFICATION);
+		IWL_CMD(REPLY_WIPAN_DEACTIVATION_COMPLETE);
 	default:
 		return "UNKNOWN";
 
 	}
 }
-EXPORT_SYMBOL(get_cmd_string);
 
 #define HOST_COMPLETE_TIMEOUT (HZ / 2)
 
@@ -252,7 +252,6 @@ out:
 	mutex_unlock(&priv->sync_cmd_mutex);
 	return ret;
 }
-EXPORT_SYMBOL(iwl_send_cmd_sync);
 
 int iwl_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 {
@@ -261,7 +260,6 @@ int iwl_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 
 	return iwl_send_cmd_sync(priv, cmd);
 }
-EXPORT_SYMBOL(iwl_send_cmd);
 
 int iwl_send_cmd_pdu(struct iwl_priv *priv, u8 id, u16 len, const void *data)
 {
@@ -273,7 +271,6 @@ int iwl_send_cmd_pdu(struct iwl_priv *priv, u8 id, u16 len, const void *data)
 
 	return iwl_send_cmd_sync(priv, &cmd);
 }
-EXPORT_SYMBOL(iwl_send_cmd_pdu);
 
 int iwl_send_cmd_pdu_async(struct iwl_priv *priv,
 			   u8 id, u16 len, const void *data,
@@ -292,4 +289,3 @@ int iwl_send_cmd_pdu_async(struct iwl_priv *priv,
 
 	return iwl_send_cmd_async(priv, &cmd);
 }
-EXPORT_SYMBOL(iwl_send_cmd_pdu_async);
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.c b/drivers/net/wireless/iwlwifi/iwl-led.c
index 46ccdf406e8e..d7f2a0bb32c9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-led.c
@@ -48,31 +48,19 @@ module_param(led_mode, int, S_IRUGO);
 MODULE_PARM_DESC(led_mode, "0=system default, "
 		"1=On(RF On)/Off(RF Off), 2=blinking");
 
-static const struct {
-	u16 tpt;	/* Mb/s */
-	u8 on_time;
-	u8 off_time;
-} blink_tbl[] =
-{
-	{300, 25, 25},
-	{200, 40, 40},
-	{100, 55, 55},
-	{70, 65, 65},
-	{50, 75, 75},
-	{20, 85, 85},
-	{10, 95, 95},
-	{5, 110, 110},
-	{1, 130, 130},
-	{0, 167, 167},
-	/* SOLID_ON */
-	{-1, IWL_LED_SOLID, 0}
+static const struct ieee80211_tpt_blink iwl_blink[] = {
+	{ .throughput = 0 * 1024 - 1, .blink_time = 334 },
+	{ .throughput = 1 * 1024 - 1, .blink_time = 260 },
+	{ .throughput = 5 * 1024 - 1, .blink_time = 220 },
+	{ .throughput = 10 * 1024 - 1, .blink_time = 190 },
+	{ .throughput = 20 * 1024 - 1, .blink_time = 170 },
+	{ .throughput = 50 * 1024 - 1, .blink_time = 150 },
+	{ .throughput = 70 * 1024 - 1, .blink_time = 130 },
+	{ .throughput = 100 * 1024 - 1, .blink_time = 110 },
+	{ .throughput = 200 * 1024 - 1, .blink_time = 80 },
+	{ .throughput = 300 * 1024 - 1, .blink_time = 50 },
 };
 
-#define IWL_1MB_RATE (128 * 1024)
-#define IWL_LED_THRESHOLD (16)
-#define IWL_MAX_BLINK_TBL (ARRAY_SIZE(blink_tbl) - 1) /* exclude SOLID_ON */
-#define IWL_SOLID_BLINK_IDX (ARRAY_SIZE(blink_tbl) - 1)
-
 /*
  * Adjust led blink rate to compensate on a MAC Clock difference on every HW
  * Led blink rate analysis showed an average deviation of 0% on 3945,
@@ -97,133 +85,102 @@ static inline u8 iwl_blink_compensation(struct iwl_priv *priv,
 }
 
 /* Set led pattern command */
-static int iwl_led_pattern(struct iwl_priv *priv, unsigned int idx)
+static int iwl_led_cmd(struct iwl_priv *priv,
+		       unsigned long on,
+		       unsigned long off)
 {
 	struct iwl_led_cmd led_cmd = {
 		.id = IWL_LED_LINK,
 		.interval = IWL_DEF_LED_INTRVL
 	};
+	int ret;
+
+	if (!test_bit(STATUS_READY, &priv->status))
+		return -EBUSY;
 
-	BUG_ON(idx > IWL_MAX_BLINK_TBL);
+	if (priv->blink_on == on && priv->blink_off == off)
+		return 0;
 
-	IWL_DEBUG_LED(priv, "Led blink time compensation= %u\n",
+	IWL_DEBUG_LED(priv, "Led blink time compensation=%u\n",
 			priv->cfg->base_params->led_compensation);
-	led_cmd.on =
-		iwl_blink_compensation(priv, blink_tbl[idx].on_time,
+	led_cmd.on = iwl_blink_compensation(priv, on,
 				priv->cfg->base_params->led_compensation);
-	led_cmd.off =
-		iwl_blink_compensation(priv, blink_tbl[idx].off_time,
+	led_cmd.off = iwl_blink_compensation(priv, off,
 				priv->cfg->base_params->led_compensation);
 
-	return priv->cfg->ops->led->cmd(priv, &led_cmd);
+	ret = priv->cfg->ops->led->cmd(priv, &led_cmd);
+	if (!ret) {
+		priv->blink_on = on;
+		priv->blink_off = off;
+	}
+	return ret;
 }
 
-int iwl_led_start(struct iwl_priv *priv)
+static void iwl_led_brightness_set(struct led_classdev *led_cdev,
+				   enum led_brightness brightness)
 {
-	return priv->cfg->ops->led->on(priv);
-}
-EXPORT_SYMBOL(iwl_led_start);
+	struct iwl_priv *priv = container_of(led_cdev, struct iwl_priv, led);
+	unsigned long on = 0;
 
-int iwl_led_associate(struct iwl_priv *priv)
-{
-	IWL_DEBUG_LED(priv, "Associated\n");
-	if (priv->cfg->led_mode == IWL_LED_BLINK)
-		priv->allow_blinking = 1;
-	priv->last_blink_time = jiffies;
+	if (brightness > 0)
+		on = IWL_LED_SOLID;
 
-	return 0;
+	iwl_led_cmd(priv, on, 0);
 }
-EXPORT_SYMBOL(iwl_led_associate);
 
-int iwl_led_disassociate(struct iwl_priv *priv)
+static int iwl_led_blink_set(struct led_classdev *led_cdev,
+			     unsigned long *delay_on,
+			     unsigned long *delay_off)
 {
-	priv->allow_blinking = 0;
+	struct iwl_priv *priv = container_of(led_cdev, struct iwl_priv, led);
 
-	return 0;
+	return iwl_led_cmd(priv, *delay_on, *delay_off);
 }
-EXPORT_SYMBOL(iwl_led_disassociate);
 
-/*
- * calculate blink rate according to last second Tx/Rx activities
- */
-static int iwl_get_blink_rate(struct iwl_priv *priv)
-{
-	int i;
-	/* count both tx and rx traffic to be able to
-	 * handle traffic in either direction
-	 */
-	u64 current_tpt = priv->tx_stats.data_bytes +
-			  priv->rx_stats.data_bytes;
-	s64 tpt = current_tpt - priv->led_tpt;
-
-	if (tpt < 0) /* wraparound */
-		tpt = -tpt;
-
-	IWL_DEBUG_LED(priv, "tpt %lld current_tpt %llu\n",
-		(long long)tpt,
-		(unsigned long long)current_tpt);
-	priv->led_tpt = current_tpt;
-
-	if (!priv->allow_blinking)
-		i = IWL_MAX_BLINK_TBL;
-	else
-		for (i = 0; i < IWL_MAX_BLINK_TBL; i++)
-			if (tpt > (blink_tbl[i].tpt * IWL_1MB_RATE))
-				break;
-
-	IWL_DEBUG_LED(priv, "LED BLINK IDX=%d\n", i);
-	return i;
-}
-
-/*
- * this function called from handler. Since setting Led command can
- * happen very frequent we postpone led command to be called from
- * REPLY handler so we know ucode is up
- */
-void iwl_leds_background(struct iwl_priv *priv)
+void iwl_leds_init(struct iwl_priv *priv)
 {
-	u8 blink_idx;
-
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
-		priv->last_blink_time = 0;
-		return;
-	}
-	if (iwl_is_rfkill(priv)) {
-		priv->last_blink_time = 0;
-		return;
+	int mode = led_mode;
+	int ret;
+
+	if (mode == IWL_LED_DEFAULT)
+		mode = priv->cfg->led_mode;
+
+	priv->led.name = kasprintf(GFP_KERNEL, "%s-led",
+				   wiphy_name(priv->hw->wiphy));
+	priv->led.brightness_set = iwl_led_brightness_set;
+	priv->led.blink_set = iwl_led_blink_set;
+	priv->led.max_brightness = 1;
+
+	switch (mode) {
+	case IWL_LED_DEFAULT:
+		WARN_ON(1);
+		break;
+	case IWL_LED_BLINK:
+		priv->led.default_trigger =
+			ieee80211_create_tpt_led_trigger(priv->hw,
+					IEEE80211_TPT_LEDTRIG_FL_CONNECTED,
+					iwl_blink, ARRAY_SIZE(iwl_blink));
+		break;
+	case IWL_LED_RF_STATE:
+		priv->led.default_trigger =
+			ieee80211_get_radio_led_name(priv->hw);
+		break;
 	}
 
-	if (!priv->allow_blinking) {
-		priv->last_blink_time = 0;
-		if (priv->last_blink_rate != IWL_SOLID_BLINK_IDX) {
-			priv->last_blink_rate = IWL_SOLID_BLINK_IDX;
-			iwl_led_pattern(priv, IWL_SOLID_BLINK_IDX);
-		}
+	ret = led_classdev_register(&priv->pci_dev->dev, &priv->led);
+	if (ret) {
+		kfree(priv->led.name);
 		return;
 	}
-	if (!priv->last_blink_time ||
-	    !time_after(jiffies, priv->last_blink_time +
-			msecs_to_jiffies(1000)))
-		return;
-
-	blink_idx = iwl_get_blink_rate(priv);
-
-	/* call only if blink rate change */
-	if (blink_idx != priv->last_blink_rate)
-		iwl_led_pattern(priv, blink_idx);
 
-	priv->last_blink_time = jiffies;
-	priv->last_blink_rate = blink_idx;
+	priv->led_registered = true;
 }
-EXPORT_SYMBOL(iwl_leds_background);
 
-void iwl_leds_init(struct iwl_priv *priv)
+void iwl_leds_exit(struct iwl_priv *priv)
 {
-	priv->last_blink_rate = 0;
-	priv->last_blink_time = 0;
-	priv->allow_blinking = 0;
-	if (led_mode != IWL_LED_DEFAULT &&
-	    led_mode != priv->cfg->led_mode)
-		priv->cfg->led_mode = led_mode;
+	if (!priv->led_registered)
+		return;
+
+	led_classdev_unregister(&priv->led);
+	kfree(priv->led.name);
 }
-EXPORT_SYMBOL(iwl_leds_init);
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.h b/drivers/net/wireless/iwlwifi/iwl-led.h
index 9079b33486ef..101eef12b3bb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.h
+++ b/drivers/net/wireless/iwlwifi/iwl-led.h
@@ -31,23 +31,14 @@
 struct iwl_priv;
 
 #define IWL_LED_SOLID 11
-#define IWL_LED_NAME_LEN 31
 #define IWL_DEF_LED_INTRVL cpu_to_le32(1000)
 
 #define IWL_LED_ACTIVITY       (0<<1)
 #define IWL_LED_LINK           (1<<1)
 
-enum led_type {
-	IWL_LED_TRG_TX,
-	IWL_LED_TRG_RX,
-	IWL_LED_TRG_ASSOC,
-	IWL_LED_TRG_RADIO,
-	IWL_LED_TRG_MAX,
-};
-
 /*
  * LED mode
- *    IWL_LED_DEFAULT:  use system default
+ *    IWL_LED_DEFAULT:  use device default
  *    IWL_LED_RF_STATE: turn LED on/off based on RF state
  *			LED ON  = RF ON
  *			LED OFF = RF OFF
@@ -60,9 +51,6 @@ enum iwl_led_mode {
 };
 
 void iwl_leds_init(struct iwl_priv *priv);
-void iwl_leds_background(struct iwl_priv *priv);
-int iwl_led_start(struct iwl_priv *priv);
-int iwl_led_associate(struct iwl_priv *priv);
-int iwl_led_disassociate(struct iwl_priv *priv);
+void iwl_leds_exit(struct iwl_priv *priv);
 
 #endif /* __iwl_leds_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-legacy.c b/drivers/net/wireless/iwlwifi/iwl-legacy.c
deleted file mode 100644
index bb1a742a98a0..000000000000
--- a/drivers/net/wireless/iwlwifi/iwl-legacy.c
+++ /dev/null
@@ -1,662 +0,0 @@
-/******************************************************************************
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2008 - 2010 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
- * USA
- *
- * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.GPL.
- *
- * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
- * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
- *****************************************************************************/
-
-#include <linux/kernel.h>
-#include <net/mac80211.h>
-
-#include "iwl-dev.h"
-#include "iwl-core.h"
-#include "iwl-helpers.h"
-#include "iwl-legacy.h"
-
-static void iwl_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
-{
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return;
-
-	if (!ctx->is_active)
-		return;
-
-	ctx->qos_data.def_qos_parm.qos_flags = 0;
-
-	if (ctx->qos_data.qos_active)
-		ctx->qos_data.def_qos_parm.qos_flags |=
-			QOS_PARAM_FLG_UPDATE_EDCA_MSK;
-
-	if (ctx->ht.enabled)
-		ctx->qos_data.def_qos_parm.qos_flags |= QOS_PARAM_FLG_TGN_MSK;
-
-	IWL_DEBUG_QOS(priv, "send QoS cmd with Qos active=%d FLAGS=0x%X\n",
-		      ctx->qos_data.qos_active,
-		      ctx->qos_data.def_qos_parm.qos_flags);
-
-	iwl_send_cmd_pdu_async(priv, ctx->qos_cmd,
-			       sizeof(struct iwl_qosparam_cmd),
-			       &ctx->qos_data.def_qos_parm, NULL);
-}
-
-/**
- * iwl_legacy_mac_config - mac80211 config callback
- */
-int iwl_legacy_mac_config(struct ieee80211_hw *hw, u32 changed)
-{
-	struct iwl_priv *priv = hw->priv;
-	const struct iwl_channel_info *ch_info;
-	struct ieee80211_conf *conf = &hw->conf;
-	struct ieee80211_channel *channel = conf->channel;
-	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
-	struct iwl_rxon_context *ctx;
-	unsigned long flags = 0;
-	int ret = 0;
-	u16 ch;
-	int scan_active = 0;
-	bool ht_changed[NUM_IWL_RXON_CTX] = {};
-
-	if (WARN_ON(!priv->cfg->ops->legacy))
-		return -EOPNOTSUPP;
-
-	mutex_lock(&priv->mutex);
-
-	IWL_DEBUG_MAC80211(priv, "enter to channel %d changed 0x%X\n",
-					channel->hw_value, changed);
-
-	if (unlikely(!priv->cfg->mod_params->disable_hw_scan &&
-			test_bit(STATUS_SCANNING, &priv->status))) {
-		scan_active = 1;
-		IWL_DEBUG_MAC80211(priv, "leave - scanning\n");
-	}
-
-	if (changed & (IEEE80211_CONF_CHANGE_SMPS |
-		       IEEE80211_CONF_CHANGE_CHANNEL)) {
-		/* mac80211 uses static for non-HT which is what we want */
-		priv->current_ht_config.smps = conf->smps_mode;
-
-		/*
-		 * Recalculate chain counts.
-		 *
-		 * If monitor mode is enabled then mac80211 will
-		 * set up the SM PS mode to OFF if an HT channel is
-		 * configured.
-		 */
-		if (priv->cfg->ops->hcmd->set_rxon_chain)
-			for_each_context(priv, ctx)
-				priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
-	}
-
-	/* during scanning mac80211 will delay channel setting until
-	 * scan finish with changed = 0
-	 */
-	if (!changed || (changed & IEEE80211_CONF_CHANGE_CHANNEL)) {
-		if (scan_active)
-			goto set_ch_out;
-
-		ch = channel->hw_value;
-		ch_info = iwl_get_channel_info(priv, channel->band, ch);
-		if (!is_channel_valid(ch_info)) {
-			IWL_DEBUG_MAC80211(priv, "leave - invalid channel\n");
-			ret = -EINVAL;
-			goto set_ch_out;
-		}
-
-		spin_lock_irqsave(&priv->lock, flags);
-
-		for_each_context(priv, ctx) {
-			/* Configure HT40 channels */
-			if (ctx->ht.enabled != conf_is_ht(conf)) {
-				ctx->ht.enabled = conf_is_ht(conf);
-				ht_changed[ctx->ctxid] = true;
-			}
-			if (ctx->ht.enabled) {
-				if (conf_is_ht40_minus(conf)) {
-					ctx->ht.extension_chan_offset =
-						IEEE80211_HT_PARAM_CHA_SEC_BELOW;
-					ctx->ht.is_40mhz = true;
-				} else if (conf_is_ht40_plus(conf)) {
-					ctx->ht.extension_chan_offset =
-						IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
-					ctx->ht.is_40mhz = true;
-				} else {
-					ctx->ht.extension_chan_offset =
-						IEEE80211_HT_PARAM_CHA_SEC_NONE;
-					ctx->ht.is_40mhz = false;
-				}
-			} else
-				ctx->ht.is_40mhz = false;
-
-			/*
-			 * Default to no protection. Protection mode will
-			 * later be set from BSS config in iwl_ht_conf
-			 */
-			ctx->ht.protection = IEEE80211_HT_OP_MODE_PROTECTION_NONE;
-
-			/* if we are switching from ht to 2.4 clear flags
-			 * from any ht related info since 2.4 does not
-			 * support ht */
-			if ((le16_to_cpu(ctx->staging.channel) != ch))
-				ctx->staging.flags = 0;
-
-			iwl_set_rxon_channel(priv, channel, ctx);
-			iwl_set_rxon_ht(priv, ht_conf);
-
-			iwl_set_flags_for_band(priv, ctx, channel->band,
-					       ctx->vif);
-		}
-
-		spin_unlock_irqrestore(&priv->lock, flags);
-
-		if (priv->cfg->ops->legacy->update_bcast_stations)
-			ret = priv->cfg->ops->legacy->update_bcast_stations(priv);
-
- set_ch_out:
-		/* The list of supported rates and rate mask can be different
-		 * for each band; since the band may have changed, reset
-		 * the rate mask to what mac80211 lists */
-		iwl_set_rate(priv);
-	}
-
-	if (changed & (IEEE80211_CONF_CHANGE_PS |
-			IEEE80211_CONF_CHANGE_IDLE)) {
-		ret = iwl_power_update_mode(priv, false);
-		if (ret)
-			IWL_DEBUG_MAC80211(priv, "Error setting sleep level\n");
-	}
-
-	if (changed & IEEE80211_CONF_CHANGE_POWER) {
-		IWL_DEBUG_MAC80211(priv, "TX Power old=%d new=%d\n",
-			priv->tx_power_user_lmt, conf->power_level);
-
-		iwl_set_tx_power(priv, conf->power_level, false);
-	}
-
-	if (!iwl_is_ready(priv)) {
-		IWL_DEBUG_MAC80211(priv, "leave - not ready\n");
-		goto out;
-	}
-
-	if (scan_active)
-		goto out;
-
-	for_each_context(priv, ctx) {
-		if (memcmp(&ctx->active, &ctx->staging, sizeof(ctx->staging)))
-			iwlcore_commit_rxon(priv, ctx);
-		else
-			IWL_DEBUG_INFO(priv,
-				"Not re-sending same RXON configuration.\n");
-		if (ht_changed[ctx->ctxid])
-			iwl_update_qos(priv, ctx);
-	}
-
-out:
-	IWL_DEBUG_MAC80211(priv, "leave\n");
-	mutex_unlock(&priv->mutex);
-	return ret;
-}
-EXPORT_SYMBOL(iwl_legacy_mac_config);
-
-void iwl_legacy_mac_reset_tsf(struct ieee80211_hw *hw)
-{
-	struct iwl_priv *priv = hw->priv;
-	unsigned long flags;
-	/* IBSS can only be the IWL_RXON_CTX_BSS context */
-	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-
-	if (WARN_ON(!priv->cfg->ops->legacy))
-		return;
-
-	mutex_lock(&priv->mutex);
-	IWL_DEBUG_MAC80211(priv, "enter\n");
-
-	spin_lock_irqsave(&priv->lock, flags);
-	memset(&priv->current_ht_config, 0, sizeof(struct iwl_ht_config));
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	/* new association get rid of ibss beacon skb */
-	if (priv->beacon_skb)
-		dev_kfree_skb(priv->beacon_skb);
-
-	priv->beacon_skb = NULL;
-
-	priv->timestamp = 0;
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	iwl_scan_cancel_timeout(priv, 100);
-	if (!iwl_is_ready_rf(priv)) {
-		IWL_DEBUG_MAC80211(priv, "leave - not ready\n");
-		mutex_unlock(&priv->mutex);
-		return;
-	}
-
-	/* we are restarting association process
-	 * clear RXON_FILTER_ASSOC_MSK bit
-	 */
-	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
-	iwlcore_commit_rxon(priv, ctx);
-
-	iwl_set_rate(priv);
-
-	mutex_unlock(&priv->mutex);
-
-	IWL_DEBUG_MAC80211(priv, "leave\n");
-}
-EXPORT_SYMBOL(iwl_legacy_mac_reset_tsf);
-
-static void iwl_ht_conf(struct iwl_priv *priv,
-			struct ieee80211_vif *vif)
-{
-	struct iwl_ht_config *ht_conf = &priv->current_ht_config;
-	struct ieee80211_sta *sta;
-	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
-	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
-
-	IWL_DEBUG_ASSOC(priv, "enter:\n");
-
-	if (!ctx->ht.enabled)
-		return;
-
-	ctx->ht.protection =
-		bss_conf->ht_operation_mode & IEEE80211_HT_OP_MODE_PROTECTION;
-	ctx->ht.non_gf_sta_present =
-		!!(bss_conf->ht_operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
-
-	ht_conf->single_chain_sufficient = false;
-
-	switch (vif->type) {
-	case NL80211_IFTYPE_STATION:
-		rcu_read_lock();
-		sta = ieee80211_find_sta(vif, bss_conf->bssid);
-		if (sta) {
-			struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
-			int maxstreams;
-
-			maxstreams = (ht_cap->mcs.tx_params &
-				      IEEE80211_HT_MCS_TX_MAX_STREAMS_MASK)
-					>> IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT;
-			maxstreams += 1;
-
-			if ((ht_cap->mcs.rx_mask[1] == 0) &&
-			    (ht_cap->mcs.rx_mask[2] == 0))
-				ht_conf->single_chain_sufficient = true;
-			if (maxstreams <= 1)
-				ht_conf->single_chain_sufficient = true;
-		} else {
-			/*
-			 * If at all, this can only happen through a race
-			 * when the AP disconnects us while we're still
-			 * setting up the connection, in that case mac80211
-			 * will soon tell us about that.
-			 */
-			ht_conf->single_chain_sufficient = true;
-		}
-		rcu_read_unlock();
-		break;
-	case NL80211_IFTYPE_ADHOC:
-		ht_conf->single_chain_sufficient = true;
-		break;
-	default:
-		break;
-	}
-
-	IWL_DEBUG_ASSOC(priv, "leave\n");
-}
-
-static inline void iwl_set_no_assoc(struct iwl_priv *priv,
-				    struct ieee80211_vif *vif)
-{
-	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
-
-	iwl_led_disassociate(priv);
-	/*
-	 * inform the ucode that there is no longer an
-	 * association and that no more packets should be
-	 * sent
-	 */
-	ctx->staging.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
-	ctx->staging.assoc_id = 0;
-	iwlcore_commit_rxon(priv, ctx);
-}
-
-static void iwlcore_beacon_update(struct ieee80211_hw *hw,
-				  struct ieee80211_vif *vif)
-{
-	struct iwl_priv *priv = hw->priv;
-	unsigned long flags;
-	__le64 timestamp;
-	struct sk_buff *skb = ieee80211_beacon_get(hw, vif);
-
-	if (!skb)
-		return;
-
-	IWL_DEBUG_MAC80211(priv, "enter\n");
-
-	lockdep_assert_held(&priv->mutex);
-
-	if (!priv->beacon_ctx) {
-		IWL_ERR(priv, "update beacon but no beacon context!\n");
-		dev_kfree_skb(skb);
-		return;
-	}
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	if (priv->beacon_skb)
-		dev_kfree_skb(priv->beacon_skb);
-
-	priv->beacon_skb = skb;
-
-	timestamp = ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp;
-	priv->timestamp = le64_to_cpu(timestamp);
-
-	IWL_DEBUG_MAC80211(priv, "leave\n");
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	if (!iwl_is_ready_rf(priv)) {
-		IWL_DEBUG_MAC80211(priv, "leave - RF not ready\n");
-		return;
-	}
-
-	priv->cfg->ops->legacy->post_associate(priv);
-}
-
-void iwl_legacy_mac_bss_info_changed(struct ieee80211_hw *hw,
-				     struct ieee80211_vif *vif,
-				     struct ieee80211_bss_conf *bss_conf,
-				     u32 changes)
-{
-	struct iwl_priv *priv = hw->priv;
-	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
-	int ret;
-
-	if (WARN_ON(!priv->cfg->ops->legacy))
-		return;
-
-	IWL_DEBUG_MAC80211(priv, "changes = 0x%X\n", changes);
-
-	if (!iwl_is_alive(priv))
-		return;
-
-	mutex_lock(&priv->mutex);
-
-	if (changes & BSS_CHANGED_QOS) {
-		unsigned long flags;
-
-		spin_lock_irqsave(&priv->lock, flags);
-		ctx->qos_data.qos_active = bss_conf->qos;
-		iwl_update_qos(priv, ctx);
-		spin_unlock_irqrestore(&priv->lock, flags);
-	}
-
-	if (changes & BSS_CHANGED_BEACON_ENABLED) {
-		/*
-		 * the add_interface code must make sure we only ever
-		 * have a single interface that could be beaconing at
-		 * any time.
-		 */
-		if (vif->bss_conf.enable_beacon)
-			priv->beacon_ctx = ctx;
-		else
-			priv->beacon_ctx = NULL;
-	}
-
-	if (changes & BSS_CHANGED_BEACON && vif->type == NL80211_IFTYPE_AP) {
-		dev_kfree_skb(priv->beacon_skb);
-		priv->beacon_skb = ieee80211_beacon_get(hw, vif);
-	}
-
-	if (changes & BSS_CHANGED_BEACON_INT && vif->type == NL80211_IFTYPE_AP)
-		iwl_send_rxon_timing(priv, ctx);
-
-	if (changes & BSS_CHANGED_BSSID) {
-		IWL_DEBUG_MAC80211(priv, "BSSID %pM\n", bss_conf->bssid);
-
-		/*
-		 * If there is currently a HW scan going on in the
-		 * background then we need to cancel it else the RXON
-		 * below/in post_associate will fail.
-		 */
-		if (iwl_scan_cancel_timeout(priv, 100)) {
-			IWL_WARN(priv, "Aborted scan still in progress after 100ms\n");
-			IWL_DEBUG_MAC80211(priv, "leaving - scan abort failed.\n");
-			mutex_unlock(&priv->mutex);
-			return;
-		}
-
-		/* mac80211 only sets assoc when in STATION mode */
-		if (vif->type == NL80211_IFTYPE_ADHOC || bss_conf->assoc) {
-			memcpy(ctx->staging.bssid_addr,
-			       bss_conf->bssid, ETH_ALEN);
-
-			/* currently needed in a few places */
-			memcpy(priv->bssid, bss_conf->bssid, ETH_ALEN);
-		} else {
-			ctx->staging.filter_flags &=
-				~RXON_FILTER_ASSOC_MSK;
-		}
-
-	}
-
-	/*
-	 * This needs to be after setting the BSSID in case
-	 * mac80211 decides to do both changes at once because
-	 * it will invoke post_associate.
-	 */
-	if (vif->type == NL80211_IFTYPE_ADHOC && changes & BSS_CHANGED_BEACON)
-		iwlcore_beacon_update(hw, vif);
-
-	if (changes & BSS_CHANGED_ERP_PREAMBLE) {
-		IWL_DEBUG_MAC80211(priv, "ERP_PREAMBLE %d\n",
-				   bss_conf->use_short_preamble);
-		if (bss_conf->use_short_preamble)
-			ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
-		else
-			ctx->staging.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
-	}
-
-	if (changes & BSS_CHANGED_ERP_CTS_PROT) {
-		IWL_DEBUG_MAC80211(priv, "ERP_CTS %d\n", bss_conf->use_cts_prot);
-		if (bss_conf->use_cts_prot && (priv->band != IEEE80211_BAND_5GHZ))
-			ctx->staging.flags |= RXON_FLG_TGG_PROTECT_MSK;
-		else
-			ctx->staging.flags &= ~RXON_FLG_TGG_PROTECT_MSK;
-		if (bss_conf->use_cts_prot)
-			ctx->staging.flags |= RXON_FLG_SELF_CTS_EN;
-		else
-			ctx->staging.flags &= ~RXON_FLG_SELF_CTS_EN;
-	}
-
-	if (changes & BSS_CHANGED_BASIC_RATES) {
-		/* XXX use this information
-		 *
-		 * To do that, remove code from iwl_set_rate() and put something
-		 * like this here:
-		 *
-		if (A-band)
-			ctx->staging.ofdm_basic_rates =
-				bss_conf->basic_rates;
-		else
-			ctx->staging.ofdm_basic_rates =
-				bss_conf->basic_rates >> 4;
-			ctx->staging.cck_basic_rates =
-				bss_conf->basic_rates & 0xF;
-		 */
-	}
-
-	if (changes & BSS_CHANGED_HT) {
-		iwl_ht_conf(priv, vif);
-
-		if (priv->cfg->ops->hcmd->set_rxon_chain)
-			priv->cfg->ops->hcmd->set_rxon_chain(priv, ctx);
-	}
-
-	if (changes & BSS_CHANGED_ASSOC) {
-		IWL_DEBUG_MAC80211(priv, "ASSOC %d\n", bss_conf->assoc);
-		if (bss_conf->assoc) {
-			priv->timestamp = bss_conf->timestamp;
-
-			iwl_led_associate(priv);
-
-			if (!iwl_is_rfkill(priv))
-				priv->cfg->ops->legacy->post_associate(priv);
-		} else
-			iwl_set_no_assoc(priv, vif);
-	}
-
-	if (changes && iwl_is_associated_ctx(ctx) && bss_conf->aid) {
-		IWL_DEBUG_MAC80211(priv, "Changes (%#x) while associated\n",
-				   changes);
-		ret = iwl_send_rxon_assoc(priv, ctx);
-		if (!ret) {
-			/* Sync active_rxon with latest change. */
-			memcpy((void *)&ctx->active,
-				&ctx->staging,
-				sizeof(struct iwl_rxon_cmd));
-		}
-	}
-
-	if (changes & BSS_CHANGED_BEACON_ENABLED) {
-		if (vif->bss_conf.enable_beacon) {
-			memcpy(ctx->staging.bssid_addr,
-			       bss_conf->bssid, ETH_ALEN);
-			memcpy(priv->bssid, bss_conf->bssid, ETH_ALEN);
-			iwl_led_associate(priv);
-			priv->cfg->ops->legacy->config_ap(priv);
-		} else
-			iwl_set_no_assoc(priv, vif);
-	}
-
-	if (changes & BSS_CHANGED_IBSS) {
-		ret = priv->cfg->ops->legacy->manage_ibss_station(priv, vif,
-							bss_conf->ibss_joined);
-		if (ret)
-			IWL_ERR(priv, "failed to %s IBSS station %pM\n",
-				bss_conf->ibss_joined ? "add" : "remove",
-				bss_conf->bssid);
-	}
-
-	mutex_unlock(&priv->mutex);
-
-	IWL_DEBUG_MAC80211(priv, "leave\n");
-}
-EXPORT_SYMBOL(iwl_legacy_mac_bss_info_changed);
-
-irqreturn_t iwl_isr_legacy(int irq, void *data)
-{
-	struct iwl_priv *priv = data;
-	u32 inta, inta_mask;
-	u32 inta_fh;
-	unsigned long flags;
-	if (!priv)
-		return IRQ_NONE;
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	/* Disable (but don't clear!) interrupts here to avoid
-	 *    back-to-back ISRs and sporadic interrupts from our NIC.
-	 * If we have something to service, the tasklet will re-enable ints.
-	 * If we *don't* have something, we'll re-enable before leaving here. */
-	inta_mask = iwl_read32(priv, CSR_INT_MASK);  /* just for debug */
-	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
-
-	/* Discover which interrupts are active/pending */
-	inta = iwl_read32(priv, CSR_INT);
-	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
-
-	/* Ignore interrupt if there's nothing in NIC to service.
-	 * This may be due to IRQ shared with another device,
-	 * or due to sporadic interrupts thrown from our NIC. */
-	if (!inta && !inta_fh) {
-		IWL_DEBUG_ISR(priv,
-			"Ignore interrupt, inta == 0, inta_fh == 0\n");
-		goto none;
-	}
-
-	if ((inta == 0xFFFFFFFF) || ((inta & 0xFFFFFFF0) == 0xa5a5a5a0)) {
-		/* Hardware disappeared. It might have already raised
-		 * an interrupt */
-		IWL_WARN(priv, "HARDWARE GONE?? INTA == 0x%08x\n", inta);
-		goto unplugged;
-	}
-
-	IWL_DEBUG_ISR(priv, "ISR inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
-		      inta, inta_mask, inta_fh);
-
-	inta &= ~CSR_INT_BIT_SCD;
-
-	/* iwl_irq_tasklet() will service interrupts and re-enable them */
-	if (likely(inta || inta_fh))
-		tasklet_schedule(&priv->irq_tasklet);
-
-unplugged:
-	spin_unlock_irqrestore(&priv->lock, flags);
-	return IRQ_HANDLED;
-
-none:
-	/* re-enable interrupts here since we don't have anything to service. */
-	/* only Re-enable if disabled by irq */
-	if (test_bit(STATUS_INT_ENABLED, &priv->status))
-		iwl_enable_interrupts(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-	return IRQ_NONE;
-}
-EXPORT_SYMBOL(iwl_isr_legacy);
-
-/*
- *  iwl_legacy_tx_cmd_protection: Set rts/cts. 3945 and 4965 only share this
- *  function.
- */
-void iwl_legacy_tx_cmd_protection(struct iwl_priv *priv,
-			       struct ieee80211_tx_info *info,
-			       __le16 fc, __le32 *tx_flags)
-{
-	if (info->control.rates[0].flags & IEEE80211_TX_RC_USE_RTS_CTS) {
-		*tx_flags |= TX_CMD_FLG_RTS_MSK;
-		*tx_flags &= ~TX_CMD_FLG_CTS_MSK;
-		*tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
-
-		if (!ieee80211_is_mgmt(fc))
-			return;
-
-		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
-		case cpu_to_le16(IEEE80211_STYPE_AUTH):
-		case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
-		case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
-		case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
-			*tx_flags &= ~TX_CMD_FLG_RTS_MSK;
-			*tx_flags |= TX_CMD_FLG_CTS_MSK;
-			break;
-		}
-	} else if (info->control.rates[0].flags &
-		   IEEE80211_TX_RC_USE_CTS_PROTECT) {
-		*tx_flags &= ~TX_CMD_FLG_RTS_MSK;
-		*tx_flags |= TX_CMD_FLG_CTS_MSK;
-		*tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
-	}
-}
-EXPORT_SYMBOL(iwl_legacy_tx_cmd_protection);
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/iwl-power.c
index 1eec18d909d8..576795e2c75b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/iwl-power.c
@@ -226,8 +226,7 @@ static void iwl_static_sleep_cmd(struct iwl_priv *priv,
 	else
 		cmd->flags &= ~IWL_POWER_SHADOW_REG_ENA;
 
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->advanced_bt_coexist) {
+	if (iwl_advanced_bt_coexist(priv)) {
 		if (!priv->cfg->bt_params->bt_sco_disable)
 			cmd->flags |= IWL_POWER_BT_SCO_ENA;
 		else
@@ -313,8 +312,7 @@ static void iwl_power_fill_sleep_cmd(struct iwl_priv *priv,
 	else
 		cmd->flags &= ~IWL_POWER_SHADOW_REG_ENA;
 
-	if (priv->cfg->bt_params &&
-	    priv->cfg->bt_params->advanced_bt_coexist) {
+	if (iwl_advanced_bt_coexist(priv)) {
 		if (!priv->cfg->bt_params->bt_sco_disable)
 			cmd->flags |= IWL_POWER_BT_SCO_ENA;
 		else
@@ -358,8 +356,7 @@ static void iwl_power_build_cmd(struct iwl_priv *priv,
 
 	if (priv->cfg->base_params->broken_powersave)
 		iwl_power_sleep_cam_cmd(priv, cmd);
-	else if (priv->cfg->base_params->supports_idle &&
-		 priv->hw->conf.flags & IEEE80211_CONF_IDLE)
+	else if (priv->hw->conf.flags & IEEE80211_CONF_IDLE)
 		iwl_static_sleep_cmd(priv, cmd, IWL_POWER_INDEX_5, 20);
 	else if (priv->cfg->ops->lib->tt_ops.lower_power_detection &&
 		 priv->cfg->ops->lib->tt_ops.tt_power_mode &&
@@ -428,7 +425,6 @@ int iwl_power_set_mode(struct iwl_priv *priv, struct iwl_powertable_cmd *cmd,
 
 	return ret;
 }
-EXPORT_SYMBOL(iwl_power_set_mode);
 
 int iwl_power_update_mode(struct iwl_priv *priv, bool force)
 {
@@ -437,7 +433,6 @@ int iwl_power_update_mode(struct iwl_priv *priv, bool force)
 	iwl_power_build_cmd(priv, &cmd);
 	return iwl_power_set_mode(priv, &cmd, force);
 }
-EXPORT_SYMBOL(iwl_power_update_mode);
 
 /* initialize to default */
 void iwl_power_initialize(struct iwl_priv *priv)
@@ -451,4 +446,3 @@ void iwl_power_initialize(struct iwl_priv *priv)
 	memset(&priv->power_data.sleep_cmd, 0,
 		sizeof(priv->power_data.sleep_cmd));
 }
-EXPORT_SYMBOL(iwl_power_initialize);
diff --git a/drivers/net/wireless/iwlwifi/iwl-rx.c b/drivers/net/wireless/iwlwifi/iwl-rx.c
index 87a6fd84d4d2..6f9a2fa04763 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rx.c
@@ -29,6 +29,7 @@
 
 #include <linux/etherdevice.h>
 #include <linux/slab.h>
+#include <linux/sched.h>
 #include <net/mac80211.h>
 #include <asm/unaligned.h>
 #include "iwl-eeprom.h"
@@ -37,7 +38,15 @@
 #include "iwl-sta.h"
 #include "iwl-io.h"
 #include "iwl-helpers.h"
-/************************** RX-FUNCTIONS ****************************/
+#include "iwl-agn-calib.h"
+#include "iwl-agn.h"
+
+/******************************************************************************
+ *
+ * RX path functions
+ *
+ ******************************************************************************/
+
 /*
  * Rx theory of operation
  *
@@ -118,7 +127,6 @@ int iwl_rx_queue_space(const struct iwl_rx_queue *q)
 		s = 0;
 	return s;
 }
-EXPORT_SYMBOL(iwl_rx_queue_space);
 
 /**
  * iwl_rx_queue_update_write_ptr - Update the write pointer for the RX queue
@@ -170,7 +178,6 @@ void iwl_rx_queue_update_write_ptr(struct iwl_priv *priv, struct iwl_rx_queue *q
  exit_unlock:
 	spin_unlock_irqrestore(&q->lock, flags);
 }
-EXPORT_SYMBOL(iwl_rx_queue_update_write_ptr);
 
 int iwl_rx_queue_alloc(struct iwl_priv *priv)
 {
@@ -211,10 +218,105 @@ err_rb:
 err_bd:
 	return -ENOMEM;
 }
-EXPORT_SYMBOL(iwl_rx_queue_alloc);
+
+/******************************************************************************
+ *
+ * Generic RX handler implementations
+ *
+ ******************************************************************************/
+
+static void iwl_rx_reply_alive(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_alive_resp *palive;
+	struct delayed_work *pwork;
+
+	palive = &pkt->u.alive_frame;
+
+	IWL_DEBUG_INFO(priv, "Alive ucode status 0x%08X revision "
+		       "0x%01X 0x%01X\n",
+		       palive->is_valid, palive->ver_type,
+		       palive->ver_subtype);
+
+	if (palive->ver_subtype == INITIALIZE_SUBTYPE) {
+		IWL_DEBUG_INFO(priv, "Initialization Alive received.\n");
+		memcpy(&priv->card_alive_init,
+		       &pkt->u.alive_frame,
+		       sizeof(struct iwl_init_alive_resp));
+		pwork = &priv->init_alive_start;
+	} else {
+		IWL_DEBUG_INFO(priv, "Runtime Alive received.\n");
+		memcpy(&priv->card_alive, &pkt->u.alive_frame,
+		       sizeof(struct iwl_alive_resp));
+		pwork = &priv->alive_start;
+	}
+
+	/* We delay the ALIVE response by 5ms to
+	 * give the HW RF Kill time to activate... */
+	if (palive->is_valid == UCODE_VALID_OK)
+		queue_delayed_work(priv->workqueue, pwork,
+				   msecs_to_jiffies(5));
+	else {
+		IWL_WARN(priv, "%s uCode did not respond OK.\n",
+			(palive->ver_subtype == INITIALIZE_SUBTYPE) ?
+			"init" : "runtime");
+		/*
+		 * If fail to load init uCode,
+		 * let's try to load the init uCode again.
+		 * We should not get into this situation, but if it
+		 * does happen, we should not move on and loading "runtime"
+		 * without proper calibrate the device.
+		 */
+		if (palive->ver_subtype == INITIALIZE_SUBTYPE)
+			priv->ucode_type = UCODE_NONE;
+		queue_work(priv->workqueue, &priv->restart);
+	}
+}
+
+static void iwl_rx_reply_error(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+
+	IWL_ERR(priv, "Error Reply type 0x%08X cmd %s (0x%02X) "
+		"seq 0x%04X ser 0x%08X\n",
+		le32_to_cpu(pkt->u.err_resp.error_type),
+		get_cmd_string(pkt->u.err_resp.cmd_id),
+		pkt->u.err_resp.cmd_id,
+		le16_to_cpu(pkt->u.err_resp.bad_cmd_seq_num),
+		le32_to_cpu(pkt->u.err_resp.error_info));
+}
+
+static void iwl_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_csa_notification *csa = &(pkt->u.csa_notif);
+	/*
+	 * MULTI-FIXME
+	 * See iwl_mac_channel_switch.
+	 */
+	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
+	struct iwl_rxon_cmd *rxon = (void *)&ctx->active;
+
+	if (priv->switch_rxon.switch_in_progress) {
+		if (!le32_to_cpu(csa->status) &&
+		    (csa->channel == priv->switch_rxon.channel)) {
+			rxon->channel = csa->channel;
+			ctx->staging.channel = csa->channel;
+			IWL_DEBUG_11H(priv, "CSA notif: channel %d\n",
+			      le16_to_cpu(csa->channel));
+			iwl_chswitch_done(priv, true);
+		} else {
+			IWL_ERR(priv, "CSA notif (fail) : channel %d\n",
+			      le16_to_cpu(csa->channel));
+			iwl_chswitch_done(priv, false);
+		}
+	}
+}
 
 
-void iwl_rx_spectrum_measure_notif(struct iwl_priv *priv,
+static void iwl_rx_spectrum_measure_notif(struct iwl_priv *priv,
 					  struct iwl_rx_mem_buffer *rxb)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
@@ -229,48 +331,494 @@ void iwl_rx_spectrum_measure_notif(struct iwl_priv *priv,
 	memcpy(&priv->measure_report, report, sizeof(*report));
 	priv->measurement_status |= MEASUREMENT_READY;
 }
-EXPORT_SYMBOL(iwl_rx_spectrum_measure_notif);
 
-void iwl_recover_from_statistics(struct iwl_priv *priv,
-				struct iwl_rx_packet *pkt)
+static void iwl_rx_pm_sleep_notif(struct iwl_priv *priv,
+				  struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_sleep_notification *sleep = &(pkt->u.sleep_notif);
+	IWL_DEBUG_RX(priv, "sleep mode: %d, src: %d\n",
+		     sleep->pm_sleep_mode, sleep->pm_wakeup_src);
+#endif
+}
+
+static void iwl_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
+					     struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	u32 len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+	IWL_DEBUG_RADIO(priv, "Dumping %d bytes of unhandled "
+			"notification for %s:\n", len,
+			get_cmd_string(pkt->hdr.cmd));
+	iwl_print_hex_dump(priv, IWL_DL_RADIO, pkt->u.raw, len);
+}
+
+static void iwl_rx_beacon_notif(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwlagn_beacon_notif *beacon = (void *)pkt->u.raw;
+#ifdef CONFIG_IWLWIFI_DEBUG
+	u16 status = le16_to_cpu(beacon->beacon_notify_hdr.status.status);
+	u8 rate = iwl_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+
+	IWL_DEBUG_RX(priv, "beacon status %#x, retries:%d ibssmgr:%d "
+		"tsf:0x%.8x%.8x rate:%d\n",
+		status & TX_STATUS_MSK,
+		beacon->beacon_notify_hdr.failure_frame,
+		le32_to_cpu(beacon->ibss_mgr_status),
+		le32_to_cpu(beacon->high_tsf),
+		le32_to_cpu(beacon->low_tsf), rate);
+#endif
+
+	priv->ibss_manager = le32_to_cpu(beacon->ibss_mgr_status);
+
+	if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+		queue_work(priv->workqueue, &priv->beacon_update);
+}
+
+/* the threshold ratio of actual_ack_cnt to expected_ack_cnt in percent */
+#define ACK_CNT_RATIO (50)
+#define BA_TIMEOUT_CNT (5)
+#define BA_TIMEOUT_MAX (16)
+
+/**
+ * iwl_good_ack_health - checks for ACK count ratios, BA timeout retries.
+ *
+ * When the ACK count ratio is low and aggregated BA timeout retries exceeding
+ * the BA_TIMEOUT_MAX, reload firmware and bring system back to normal
+ * operation state.
+ */
+static bool iwl_good_ack_health(struct iwl_priv *priv, struct iwl_rx_packet *pkt)
+{
+	int actual_delta, expected_delta, ba_timeout_delta;
+	struct statistics_tx *cur, *old;
+
+	if (priv->_agn.agg_tids_count)
+		return true;
+
+	if (iwl_bt_statistics(priv)) {
+		cur = &pkt->u.stats_bt.tx;
+		old = &priv->_agn.statistics_bt.tx;
+	} else {
+		cur = &pkt->u.stats.tx;
+		old = &priv->_agn.statistics.tx;
+	}
+
+	actual_delta = le32_to_cpu(cur->actual_ack_cnt) -
+		       le32_to_cpu(old->actual_ack_cnt);
+	expected_delta = le32_to_cpu(cur->expected_ack_cnt) -
+			 le32_to_cpu(old->expected_ack_cnt);
+
+	/* Values should not be negative, but we do not trust the firmware */
+	if (actual_delta <= 0 || expected_delta <= 0)
+		return true;
+
+	ba_timeout_delta = le32_to_cpu(cur->agg.ba_timeout) -
+			   le32_to_cpu(old->agg.ba_timeout);
+
+	if ((actual_delta * 100 / expected_delta) < ACK_CNT_RATIO &&
+	    ba_timeout_delta > BA_TIMEOUT_CNT) {
+		IWL_DEBUG_RADIO(priv, "deltas: actual %d expected %d ba_timeout %d\n",
+				actual_delta, expected_delta, ba_timeout_delta);
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+		/*
+		 * This is ifdef'ed on DEBUGFS because otherwise the
+		 * statistics aren't available. If DEBUGFS is set but
+		 * DEBUG is not, these will just compile out.
+		 */
+		IWL_DEBUG_RADIO(priv, "rx_detected_cnt delta %d\n",
+				priv->_agn.delta_statistics.tx.rx_detected_cnt);
+		IWL_DEBUG_RADIO(priv,
+				"ack_or_ba_timeout_collision delta %d\n",
+				priv->_agn.delta_statistics.tx.ack_or_ba_timeout_collision);
+#endif
+
+		if (ba_timeout_delta >= BA_TIMEOUT_MAX)
+			return false;
+	}
+
+	return true;
+}
+
+/**
+ * iwl_good_plcp_health - checks for plcp error.
+ *
+ * When the plcp error is exceeding the thresholds, reset the radio
+ * to improve the throughput.
+ */
+static bool iwl_good_plcp_health(struct iwl_priv *priv,
+				 struct iwl_rx_packet *pkt, unsigned int msecs)
 {
+	int delta;
+	int threshold = priv->cfg->base_params->plcp_delta_threshold;
+
+	if (threshold == IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE) {
+		IWL_DEBUG_RADIO(priv, "plcp_err check disabled\n");
+		return true;
+	}
+
+	if (iwl_bt_statistics(priv)) {
+		struct statistics_rx_bt *cur, *old;
+
+		cur = &pkt->u.stats_bt.rx;
+		old = &priv->_agn.statistics_bt.rx;
+
+		delta = le32_to_cpu(cur->ofdm.plcp_err) -
+			le32_to_cpu(old->ofdm.plcp_err) +
+			le32_to_cpu(cur->ofdm_ht.plcp_err) -
+			le32_to_cpu(old->ofdm_ht.plcp_err);
+	} else {
+		struct statistics_rx *cur, *old;
+
+		cur = &pkt->u.stats.rx;
+		old = &priv->_agn.statistics.rx;
+
+		delta = le32_to_cpu(cur->ofdm.plcp_err) -
+			le32_to_cpu(old->ofdm.plcp_err) +
+			le32_to_cpu(cur->ofdm_ht.plcp_err) -
+			le32_to_cpu(old->ofdm_ht.plcp_err);
+	}
+
+	/* Can be negative if firmware reseted statistics */
+	if (delta <= 0)
+		return true;
+
+	if ((delta * 100 / msecs) > threshold) {
+		IWL_DEBUG_RADIO(priv,
+				"plcp health threshold %u delta %d msecs %u\n",
+				threshold, delta, msecs);
+		return false;
+	}
+
+	return true;
+}
+
+static void iwl_recover_from_statistics(struct iwl_priv *priv,
+					struct iwl_rx_packet *pkt)
+{
+	const struct iwl_mod_params *mod_params = priv->cfg->mod_params;
+	unsigned int msecs;
+	unsigned long stamp;
+
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
-	if (iwl_is_any_associated(priv)) {
-		if (priv->cfg->ops->lib->check_ack_health) {
-			if (!priv->cfg->ops->lib->check_ack_health(
-			    priv, pkt)) {
-				/*
-				 * low ack count detected
-				 * restart Firmware
-				 */
-				IWL_ERR(priv, "low ack count detected, "
-					"restart firmware\n");
-				if (!iwl_force_reset(priv, IWL_FW_RESET, false))
-					return;
-			}
+
+	stamp = jiffies;
+	msecs = jiffies_to_msecs(stamp - priv->rx_statistics_jiffies);
+
+	/* Only gather statistics and update time stamp when not associated */
+	if (!iwl_is_any_associated(priv))
+		goto out;
+
+	/* Do not check/recover when do not have enough statistics data */
+	if (msecs < 99)
+		return;
+
+	if (mod_params->ack_check && !iwl_good_ack_health(priv, pkt)) {
+		IWL_ERR(priv, "low ack count detected, restart firmware\n");
+		if (!iwl_force_reset(priv, IWL_FW_RESET, false))
+			return;
+	}
+
+	if (mod_params->plcp_check && !iwl_good_plcp_health(priv, pkt, msecs))
+		iwl_force_reset(priv, IWL_RF_RESET, false);
+
+out:
+	if (iwl_bt_statistics(priv))
+		memcpy(&priv->_agn.statistics_bt, &pkt->u.stats_bt,
+			sizeof(priv->_agn.statistics_bt));
+	else
+		memcpy(&priv->_agn.statistics, &pkt->u.stats,
+			sizeof(priv->_agn.statistics));
+
+	priv->rx_statistics_jiffies = stamp;
+}
+
+/* Calculate noise level, based on measurements during network silence just
+ *   before arriving beacon.  This measurement can be done only if we know
+ *   exactly when to expect beacons, therefore only when we're associated. */
+static void iwl_rx_calc_noise(struct iwl_priv *priv)
+{
+	struct statistics_rx_non_phy *rx_info;
+	int num_active_rx = 0;
+	int total_silence = 0;
+	int bcn_silence_a, bcn_silence_b, bcn_silence_c;
+	int last_rx_noise;
+
+	if (iwl_bt_statistics(priv))
+		rx_info = &(priv->_agn.statistics_bt.rx.general.common);
+	else
+		rx_info = &(priv->_agn.statistics.rx.general);
+	bcn_silence_a =
+		le32_to_cpu(rx_info->beacon_silence_rssi_a) & IN_BAND_FILTER;
+	bcn_silence_b =
+		le32_to_cpu(rx_info->beacon_silence_rssi_b) & IN_BAND_FILTER;
+	bcn_silence_c =
+		le32_to_cpu(rx_info->beacon_silence_rssi_c) & IN_BAND_FILTER;
+
+	if (bcn_silence_a) {
+		total_silence += bcn_silence_a;
+		num_active_rx++;
+	}
+	if (bcn_silence_b) {
+		total_silence += bcn_silence_b;
+		num_active_rx++;
+	}
+	if (bcn_silence_c) {
+		total_silence += bcn_silence_c;
+		num_active_rx++;
+	}
+
+	/* Average among active antennas */
+	if (num_active_rx)
+		last_rx_noise = (total_silence / num_active_rx) - 107;
+	else
+		last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+
+	IWL_DEBUG_CALIB(priv, "inband silence a %u, b %u, c %u, dBm %d\n",
+			bcn_silence_a, bcn_silence_b, bcn_silence_c,
+			last_rx_noise);
+}
+
+/*
+ *  based on the assumption of all statistics counter are in DWORD
+ *  FIXME: This function is for debugging, do not deal with
+ *  the case of counters roll-over.
+ */
+static void iwl_accumulative_statistics(struct iwl_priv *priv,
+					__le32 *stats)
+{
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	int i, size;
+	__le32 *prev_stats;
+	u32 *accum_stats;
+	u32 *delta, *max_delta;
+	struct statistics_general_common *general, *accum_general;
+	struct statistics_tx *tx, *accum_tx;
+
+	if (iwl_bt_statistics(priv)) {
+		prev_stats = (__le32 *)&priv->_agn.statistics_bt;
+		accum_stats = (u32 *)&priv->_agn.accum_statistics_bt;
+		size = sizeof(struct iwl_bt_notif_statistics);
+		general = &priv->_agn.statistics_bt.general.common;
+		accum_general = &priv->_agn.accum_statistics_bt.general.common;
+		tx = &priv->_agn.statistics_bt.tx;
+		accum_tx = &priv->_agn.accum_statistics_bt.tx;
+		delta = (u32 *)&priv->_agn.delta_statistics_bt;
+		max_delta = (u32 *)&priv->_agn.max_delta_bt;
+	} else {
+		prev_stats = (__le32 *)&priv->_agn.statistics;
+		accum_stats = (u32 *)&priv->_agn.accum_statistics;
+		size = sizeof(struct iwl_notif_statistics);
+		general = &priv->_agn.statistics.general.common;
+		accum_general = &priv->_agn.accum_statistics.general.common;
+		tx = &priv->_agn.statistics.tx;
+		accum_tx = &priv->_agn.accum_statistics.tx;
+		delta = (u32 *)&priv->_agn.delta_statistics;
+		max_delta = (u32 *)&priv->_agn.max_delta;
+	}
+	for (i = sizeof(__le32); i < size;
+	     i += sizeof(__le32), stats++, prev_stats++, delta++,
+	     max_delta++, accum_stats++) {
+		if (le32_to_cpu(*stats) > le32_to_cpu(*prev_stats)) {
+			*delta = (le32_to_cpu(*stats) -
+				le32_to_cpu(*prev_stats));
+			*accum_stats += *delta;
+			if (*delta > *max_delta)
+				*max_delta = *delta;
 		}
-		if (priv->cfg->ops->lib->check_plcp_health) {
-			if (!priv->cfg->ops->lib->check_plcp_health(
-			    priv, pkt)) {
-				/*
-				 * high plcp error detected
-				 * reset Radio
-				 */
-				iwl_force_reset(priv, IWL_RF_RESET, false);
-			}
+	}
+
+	/* reset accumulative statistics for "no-counter" type statistics */
+	accum_general->temperature = general->temperature;
+	accum_general->temperature_m = general->temperature_m;
+	accum_general->ttl_timestamp = general->ttl_timestamp;
+	accum_tx->tx_power.ant_a = tx->tx_power.ant_a;
+	accum_tx->tx_power.ant_b = tx->tx_power.ant_b;
+	accum_tx->tx_power.ant_c = tx->tx_power.ant_c;
+#endif
+}
+
+static void iwl_rx_statistics(struct iwl_priv *priv,
+			      struct iwl_rx_mem_buffer *rxb)
+{
+	const int reg_recalib_period = 60;
+	int change;
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+
+	if (iwl_bt_statistics(priv)) {
+		IWL_DEBUG_RX(priv,
+			     "Statistics notification received (%d vs %d).\n",
+			     (int)sizeof(struct iwl_bt_notif_statistics),
+			     le32_to_cpu(pkt->len_n_flags) &
+			     FH_RSCSR_FRAME_SIZE_MSK);
+
+		change = ((priv->_agn.statistics_bt.general.common.temperature !=
+			   pkt->u.stats_bt.general.common.temperature) ||
+			   ((priv->_agn.statistics_bt.flag &
+			   STATISTICS_REPLY_FLG_HT40_MODE_MSK) !=
+			   (pkt->u.stats_bt.flag &
+			   STATISTICS_REPLY_FLG_HT40_MODE_MSK)));
+
+		iwl_accumulative_statistics(priv, (__le32 *)&pkt->u.stats_bt);
+	} else {
+		IWL_DEBUG_RX(priv,
+			     "Statistics notification received (%d vs %d).\n",
+			     (int)sizeof(struct iwl_notif_statistics),
+			     le32_to_cpu(pkt->len_n_flags) &
+			     FH_RSCSR_FRAME_SIZE_MSK);
+
+		change = ((priv->_agn.statistics.general.common.temperature !=
+			   pkt->u.stats.general.common.temperature) ||
+			   ((priv->_agn.statistics.flag &
+			   STATISTICS_REPLY_FLG_HT40_MODE_MSK) !=
+			   (pkt->u.stats.flag &
+			   STATISTICS_REPLY_FLG_HT40_MODE_MSK)));
+
+		iwl_accumulative_statistics(priv, (__le32 *)&pkt->u.stats);
+	}
+
+	iwl_recover_from_statistics(priv, pkt);
+
+	set_bit(STATUS_STATISTICS, &priv->status);
+
+	/* Reschedule the statistics timer to occur in
+	 * reg_recalib_period seconds to ensure we get a
+	 * thermal update even if the uCode doesn't give
+	 * us one */
+	mod_timer(&priv->statistics_periodic, jiffies +
+		  msecs_to_jiffies(reg_recalib_period * 1000));
+
+	if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
+	    (pkt->hdr.cmd == STATISTICS_NOTIFICATION)) {
+		iwl_rx_calc_noise(priv);
+		queue_work(priv->workqueue, &priv->run_time_calib_work);
+	}
+	if (priv->cfg->ops->lib->temp_ops.temperature && change)
+		priv->cfg->ops->lib->temp_ops.temperature(priv);
+}
+
+static void iwl_rx_reply_statistics(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+
+	if (le32_to_cpu(pkt->u.stats.flag) & UCODE_STATISTICS_CLEAR_MSK) {
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+		memset(&priv->_agn.accum_statistics, 0,
+			sizeof(struct iwl_notif_statistics));
+		memset(&priv->_agn.delta_statistics, 0,
+			sizeof(struct iwl_notif_statistics));
+		memset(&priv->_agn.max_delta, 0,
+			sizeof(struct iwl_notif_statistics));
+		memset(&priv->_agn.accum_statistics_bt, 0,
+			sizeof(struct iwl_bt_notif_statistics));
+		memset(&priv->_agn.delta_statistics_bt, 0,
+			sizeof(struct iwl_bt_notif_statistics));
+		memset(&priv->_agn.max_delta_bt, 0,
+			sizeof(struct iwl_bt_notif_statistics));
+#endif
+		IWL_DEBUG_RX(priv, "Statistics have been cleared\n");
+	}
+	iwl_rx_statistics(priv, rxb);
+}
+
+/* Handle notification from uCode that card's power state is changing
+ * due to software, hardware, or critical temperature RFKILL */
+static void iwl_rx_card_state_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	u32 flags = le32_to_cpu(pkt->u.card_state_notif.flags);
+	unsigned long status = priv->status;
+
+	IWL_DEBUG_RF_KILL(priv, "Card state received: HW:%s SW:%s CT:%s\n",
+			  (flags & HW_CARD_DISABLED) ? "Kill" : "On",
+			  (flags & SW_CARD_DISABLED) ? "Kill" : "On",
+			  (flags & CT_CARD_DISABLED) ?
+			  "Reached" : "Not reached");
+
+	if (flags & (SW_CARD_DISABLED | HW_CARD_DISABLED |
+		     CT_CARD_DISABLED)) {
+
+		iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+			    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+		iwl_write_direct32(priv, HBUS_TARG_MBX_C,
+					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
+
+		if (!(flags & RXON_CARD_DISABLED)) {
+			iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+				    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+			iwl_write_direct32(priv, HBUS_TARG_MBX_C,
+					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
 		}
+		if (flags & CT_CARD_DISABLED)
+			iwl_tt_enter_ct_kill(priv);
+	}
+	if (!(flags & CT_CARD_DISABLED))
+		iwl_tt_exit_ct_kill(priv);
+
+	if (flags & HW_CARD_DISABLED)
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+
+
+	if (!(flags & RXON_CARD_DISABLED))
+		iwl_scan_cancel(priv);
+
+	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
+	     test_bit(STATUS_RF_KILL_HW, &priv->status)))
+		wiphy_rfkill_set_hw_state(priv->hw->wiphy,
+			test_bit(STATUS_RF_KILL_HW, &priv->status));
+	else
+		wake_up_interruptible(&priv->wait_command_queue);
+}
+
+static void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
+				       struct iwl_rx_mem_buffer *rxb)
+
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_missed_beacon_notif *missed_beacon;
+
+	missed_beacon = &pkt->u.missed_beacon;
+	if (le32_to_cpu(missed_beacon->consecutive_missed_beacons) >
+	    priv->missed_beacon_threshold) {
+		IWL_DEBUG_CALIB(priv,
+		    "missed bcn cnsq %d totl %d rcd %d expctd %d\n",
+		    le32_to_cpu(missed_beacon->consecutive_missed_beacons),
+		    le32_to_cpu(missed_beacon->total_missed_becons),
+		    le32_to_cpu(missed_beacon->num_recvd_beacons),
+		    le32_to_cpu(missed_beacon->num_expected_beacons));
+		if (!test_bit(STATUS_SCANNING, &priv->status))
+			iwl_init_sensitivity(priv);
 	}
 }
-EXPORT_SYMBOL(iwl_recover_from_statistics);
+
+/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
+ * This will be used later in iwl_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
+static void iwl_rx_reply_rx_phy(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+
+	priv->_agn.last_phy_res_valid = true;
+	memcpy(&priv->_agn.last_phy_res, pkt->u.raw,
+	       sizeof(struct iwl_rx_phy_res));
+}
 
 /*
  * returns non-zero if packet should be dropped
  */
-int iwl_set_decrypted_flag(struct iwl_priv *priv,
-			   struct ieee80211_hdr *hdr,
-			   u32 decrypt_res,
-			   struct ieee80211_rx_status *stats)
+static int iwl_set_decrypted_flag(struct iwl_priv *priv,
+				  struct ieee80211_hdr *hdr,
+				  u32 decrypt_res,
+				  struct ieee80211_rx_status *stats)
 {
 	u16 fc = le16_to_cpu(hdr->frame_control);
 
@@ -315,4 +863,264 @@ int iwl_set_decrypted_flag(struct iwl_priv *priv,
 	}
 	return 0;
 }
-EXPORT_SYMBOL(iwl_set_decrypted_flag);
+
+static void iwl_pass_packet_to_mac80211(struct iwl_priv *priv,
+					struct ieee80211_hdr *hdr,
+					u16 len,
+					u32 ampdu_status,
+					struct iwl_rx_mem_buffer *rxb,
+					struct ieee80211_rx_status *stats)
+{
+	struct sk_buff *skb;
+	__le16 fc = hdr->frame_control;
+
+	/* We only process data packets if the interface is open */
+	if (unlikely(!priv->is_open)) {
+		IWL_DEBUG_DROP_LIMIT(priv,
+		    "Dropping packet while interface is not open.\n");
+		return;
+	}
+
+	/* In case of HW accelerated crypto and bad decryption, drop */
+	if (!priv->cfg->mod_params->sw_crypto &&
+	    iwl_set_decrypted_flag(priv, hdr, ampdu_status, stats))
+		return;
+
+	skb = dev_alloc_skb(128);
+	if (!skb) {
+		IWL_ERR(priv, "dev_alloc_skb failed\n");
+		return;
+	}
+
+	skb_add_rx_frag(skb, 0, rxb->page, (void *)hdr - rxb_addr(rxb), len);
+
+	iwl_update_stats(priv, false, fc, len);
+	memcpy(IEEE80211_SKB_RXCB(skb), stats, sizeof(*stats));
+
+	ieee80211_rx(priv->hw, skb);
+	priv->alloc_rxb_page--;
+	rxb->page = NULL;
+}
+
+static u32 iwl_translate_rx_status(struct iwl_priv *priv, u32 decrypt_in)
+{
+	u32 decrypt_out = 0;
+
+	if ((decrypt_in & RX_RES_STATUS_STATION_FOUND) ==
+					RX_RES_STATUS_STATION_FOUND)
+		decrypt_out |= (RX_RES_STATUS_STATION_FOUND |
+				RX_RES_STATUS_NO_STATION_INFO_MISMATCH);
+
+	decrypt_out |= (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK);
+
+	/* packet was not encrypted */
+	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
+					RX_RES_STATUS_SEC_TYPE_NONE)
+		return decrypt_out;
+
+	/* packet was encrypted with unknown alg */
+	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
+					RX_RES_STATUS_SEC_TYPE_ERR)
+		return decrypt_out;
+
+	/* decryption was not done in HW */
+	if ((decrypt_in & RX_MPDU_RES_STATUS_DEC_DONE_MSK) !=
+					RX_MPDU_RES_STATUS_DEC_DONE_MSK)
+		return decrypt_out;
+
+	switch (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) {
+
+	case RX_RES_STATUS_SEC_TYPE_CCMP:
+		/* alg is CCM: check MIC only */
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_MIC_OK))
+			/* Bad MIC */
+			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
+		else
+			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
+
+		break;
+
+	case RX_RES_STATUS_SEC_TYPE_TKIP:
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_TTAK_OK)) {
+			/* Bad TTAK */
+			decrypt_out |= RX_RES_STATUS_BAD_KEY_TTAK;
+			break;
+		}
+		/* fall through if TTAK OK */
+	default:
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_ICV_OK))
+			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
+		else
+			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
+		break;
+	}
+
+	IWL_DEBUG_RX(priv, "decrypt_in:0x%x  decrypt_out = 0x%x\n",
+					decrypt_in, decrypt_out);
+
+	return decrypt_out;
+}
+
+/* Called for REPLY_RX (legacy ABG frames), or
+ * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
+static void iwl_rx_reply_rx(struct iwl_priv *priv,
+			    struct iwl_rx_mem_buffer *rxb)
+{
+	struct ieee80211_hdr *header;
+	struct ieee80211_rx_status rx_status;
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_rx_phy_res *phy_res;
+	__le32 rx_pkt_status;
+	struct iwl_rx_mpdu_res_start *amsdu;
+	u32 len;
+	u32 ampdu_status;
+	u32 rate_n_flags;
+
+	/**
+	 * REPLY_RX and REPLY_RX_MPDU_CMD are handled differently.
+	 *	REPLY_RX: physical layer info is in this buffer
+	 *	REPLY_RX_MPDU_CMD: physical layer info was sent in separate
+	 *		command and cached in priv->last_phy_res
+	 *
+	 * Here we set up local variables depending on which command is
+	 * received.
+	 */
+	if (pkt->hdr.cmd == REPLY_RX) {
+		phy_res = (struct iwl_rx_phy_res *)pkt->u.raw;
+		header = (struct ieee80211_hdr *)(pkt->u.raw + sizeof(*phy_res)
+				+ phy_res->cfg_phy_cnt);
+
+		len = le16_to_cpu(phy_res->byte_count);
+		rx_pkt_status = *(__le32 *)(pkt->u.raw + sizeof(*phy_res) +
+				phy_res->cfg_phy_cnt + len);
+		ampdu_status = le32_to_cpu(rx_pkt_status);
+	} else {
+		if (!priv->_agn.last_phy_res_valid) {
+			IWL_ERR(priv, "MPDU frame without cached PHY data\n");
+			return;
+		}
+		phy_res = &priv->_agn.last_phy_res;
+		amsdu = (struct iwl_rx_mpdu_res_start *)pkt->u.raw;
+		header = (struct ieee80211_hdr *)(pkt->u.raw + sizeof(*amsdu));
+		len = le16_to_cpu(amsdu->byte_count);
+		rx_pkt_status = *(__le32 *)(pkt->u.raw + sizeof(*amsdu) + len);
+		ampdu_status = iwl_translate_rx_status(priv,
+						le32_to_cpu(rx_pkt_status));
+	}
+
+	if ((unlikely(phy_res->cfg_phy_cnt > 20))) {
+		IWL_DEBUG_DROP(priv, "dsp size out of range [0,20]: %d/n",
+				phy_res->cfg_phy_cnt);
+		return;
+	}
+
+	if (!(rx_pkt_status & RX_RES_STATUS_NO_CRC32_ERROR) ||
+	    !(rx_pkt_status & RX_RES_STATUS_NO_RXE_OVERFLOW)) {
+		IWL_DEBUG_RX(priv, "Bad CRC or FIFO: 0x%08X.\n",
+				le32_to_cpu(rx_pkt_status));
+		return;
+	}
+
+	/* This will be used in several places later */
+	rate_n_flags = le32_to_cpu(phy_res->rate_n_flags);
+
+	/* rx_status carries information about the packet to mac80211 */
+	rx_status.mactime = le64_to_cpu(phy_res->timestamp);
+	rx_status.band = (phy_res->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
+				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+	rx_status.freq =
+		ieee80211_channel_to_frequency(le16_to_cpu(phy_res->channel),
+					       rx_status.band);
+	rx_status.rate_idx =
+		iwlagn_hwrate_to_mac80211_idx(rate_n_flags, rx_status.band);
+	rx_status.flag = 0;
+
+	/* TSF isn't reliable. In order to allow smooth user experience,
+	 * this W/A doesn't propagate it to the mac80211 */
+	/*rx_status.flag |= RX_FLAG_MACTIME_MPDU;*/
+
+	priv->ucode_beacon_time = le32_to_cpu(phy_res->beacon_time_stamp);
+
+	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
+	rx_status.signal = priv->cfg->ops->utils->calc_rssi(priv, phy_res);
+
+	iwl_dbg_log_rx_data_frame(priv, len, header);
+	IWL_DEBUG_STATS_LIMIT(priv, "Rssi %d, TSF %llu\n",
+		rx_status.signal, (unsigned long long)rx_status.mactime);
+
+	/*
+	 * "antenna number"
+	 *
+	 * It seems that the antenna field in the phy flags value
+	 * is actually a bit field. This is undefined by radiotap,
+	 * it wants an actual antenna number but I always get "7"
+	 * for most legacy frames I receive indicating that the
+	 * same frame was received on all three RX chains.
+	 *
+	 * I think this field should be removed in favor of a
+	 * new 802.11n radiotap field "RX chains" that is defined
+	 * as a bitmask.
+	 */
+	rx_status.antenna =
+		(le16_to_cpu(phy_res->phy_flags) & RX_RES_PHY_FLAGS_ANTENNA_MSK)
+		>> RX_RES_PHY_FLAGS_ANTENNA_POS;
+
+	/* set the preamble flag if appropriate */
+	if (phy_res->phy_flags & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
+		rx_status.flag |= RX_FLAG_SHORTPRE;
+
+	/* Set up the HT phy flags */
+	if (rate_n_flags & RATE_MCS_HT_MSK)
+		rx_status.flag |= RX_FLAG_HT;
+	if (rate_n_flags & RATE_MCS_HT40_MSK)
+		rx_status.flag |= RX_FLAG_40MHZ;
+	if (rate_n_flags & RATE_MCS_SGI_MSK)
+		rx_status.flag |= RX_FLAG_SHORT_GI;
+
+	iwl_pass_packet_to_mac80211(priv, header, len, ampdu_status,
+				    rxb, &rx_status);
+}
+
+/**
+ * iwl_setup_rx_handlers - Initialize Rx handler callbacks
+ *
+ * Setup the RX handlers for each of the reply types sent from the uCode
+ * to the host.
+ */
+void iwl_setup_rx_handlers(struct iwl_priv *priv)
+{
+	void (**handlers)(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb);
+
+	handlers = priv->rx_handlers;
+
+	handlers[REPLY_ALIVE]			= iwl_rx_reply_alive;
+	handlers[REPLY_ERROR]			= iwl_rx_reply_error;
+	handlers[CHANNEL_SWITCH_NOTIFICATION]	= iwl_rx_csa;
+	handlers[SPECTRUM_MEASURE_NOTIFICATION]	= iwl_rx_spectrum_measure_notif;
+	handlers[PM_SLEEP_NOTIFICATION]		= iwl_rx_pm_sleep_notif;
+	handlers[PM_DEBUG_STATISTIC_NOTIFIC]	= iwl_rx_pm_debug_statistics_notif;
+	handlers[BEACON_NOTIFICATION]		= iwl_rx_beacon_notif;
+
+	/*
+	 * The same handler is used for both the REPLY to a discrete
+	 * statistics request from the host as well as for the periodic
+	 * statistics notifications (after received beacons) from the uCode.
+	 */
+	handlers[REPLY_STATISTICS_CMD]		= iwl_rx_reply_statistics;
+	handlers[STATISTICS_NOTIFICATION]	= iwl_rx_statistics;
+
+	iwl_setup_rx_scan_handlers(priv);
+
+	handlers[CARD_STATE_NOTIFICATION]	= iwl_rx_card_state_notif;
+	handlers[MISSED_BEACONS_NOTIFICATION]	= iwl_rx_missed_beacon_notif;
+
+	/* Rx handlers */
+	handlers[REPLY_RX_PHY_CMD]		= iwl_rx_reply_rx_phy;
+	handlers[REPLY_RX_MPDU_CMD]		= iwl_rx_reply_rx;
+
+	/* block ack */
+	handlers[REPLY_COMPRESSED_BA]		= iwlagn_rx_reply_compressed_ba;
+
+	/* Set up hardware specific Rx handlers */
+	priv->cfg->ops->lib->rx_handler_setup(priv);
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/iwl-scan.c
index 12d9363d0afe..faa6d34cb658 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scan.c
+++ b/drivers/net/wireless/iwlwifi/iwl-scan.c
@@ -155,7 +155,6 @@ int iwl_scan_cancel(struct iwl_priv *priv)
 	queue_work(priv->workqueue, &priv->abort_scan);
 	return 0;
 }
-EXPORT_SYMBOL(iwl_scan_cancel);
 
 /**
  * iwl_scan_cancel_timeout - Cancel any currently executing HW scan
@@ -180,7 +179,6 @@ int iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
 
 	return test_bit(STATUS_SCAN_HW, &priv->status);
 }
-EXPORT_SYMBOL(iwl_scan_cancel_timeout);
 
 /* Service response to REPLY_SCAN_CMD (0x80) */
 static void iwl_rx_reply_scan(struct iwl_priv *priv,
@@ -257,8 +255,7 @@ static void iwl_rx_scan_complete_notif(struct iwl_priv *priv,
 	queue_work(priv->workqueue, &priv->scan_completed);
 
 	if (priv->iw_mode != NL80211_IFTYPE_ADHOC &&
-	    priv->cfg->bt_params &&
-	    priv->cfg->bt_params->advanced_bt_coexist &&
+	    iwl_advanced_bt_coexist(priv) &&
 	    priv->bt_status != scan_notif->bt_status) {
 		if (scan_notif->bt_status) {
 			/* BT on */
@@ -289,7 +286,6 @@ void iwl_setup_rx_scan_handlers(struct iwl_priv *priv)
 	priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
 					iwl_rx_scan_complete_notif;
 }
-EXPORT_SYMBOL(iwl_setup_rx_scan_handlers);
 
 inline u16 iwl_get_active_dwell_time(struct iwl_priv *priv,
 				     enum ieee80211_band band,
@@ -302,7 +298,6 @@ inline u16 iwl_get_active_dwell_time(struct iwl_priv *priv,
 		return IWL_ACTIVE_DWELL_TIME_24 +
 			IWL_ACTIVE_DWELL_FACTOR_24GHZ * (n_probes + 1);
 }
-EXPORT_SYMBOL(iwl_get_active_dwell_time);
 
 u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
 			       enum ieee80211_band band,
@@ -334,7 +329,6 @@ u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
 
 	return passive;
 }
-EXPORT_SYMBOL(iwl_get_passive_dwell_time);
 
 void iwl_init_scan_params(struct iwl_priv *priv)
 {
@@ -344,7 +338,6 @@ void iwl_init_scan_params(struct iwl_priv *priv)
 	if (!priv->scan_tx_ant[IEEE80211_BAND_2GHZ])
 		priv->scan_tx_ant[IEEE80211_BAND_2GHZ] = ant_idx;
 }
-EXPORT_SYMBOL(iwl_init_scan_params);
 
 static int __must_check iwl_scan_initiate(struct iwl_priv *priv,
 					  struct ieee80211_vif *vif,
@@ -440,7 +433,6 @@ out_unlock:
 
 	return ret;
 }
-EXPORT_SYMBOL(iwl_mac_hw_scan);
 
 /*
  * internal short scan, this function should only been called while associated.
@@ -537,7 +529,6 @@ u16 iwl_fill_probe_req(struct iwl_priv *priv, struct ieee80211_mgmt *frame,
 
 	return (u16)len;
 }
-EXPORT_SYMBOL(iwl_fill_probe_req);
 
 static void iwl_bg_abort_scan(struct work_struct *work)
 {
@@ -622,7 +613,6 @@ void iwl_setup_scan_deferred_work(struct iwl_priv *priv)
 	INIT_WORK(&priv->start_internal_scan, iwl_bg_start_internal_scan);
 	INIT_DELAYED_WORK(&priv->scan_check, iwl_bg_scan_check);
 }
-EXPORT_SYMBOL(iwl_setup_scan_deferred_work);
 
 void iwl_cancel_scan_deferred_work(struct iwl_priv *priv)
 {
@@ -636,4 +626,3 @@ void iwl_cancel_scan_deferred_work(struct iwl_priv *priv)
 		mutex_unlock(&priv->mutex);
 	}
 }
-EXPORT_SYMBOL(iwl_cancel_scan_deferred_work);
diff --git a/drivers/net/wireless/iwlwifi/iwl-sta.c b/drivers/net/wireless/iwlwifi/iwl-sta.c
index 49493d176515..bc90a12408a3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.c
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.c
@@ -169,7 +169,6 @@ int iwl_send_add_sta(struct iwl_priv *priv,
 
 	return ret;
 }
-EXPORT_SYMBOL(iwl_send_add_sta);
 
 static void iwl_set_ht_add_station(struct iwl_priv *priv, u8 index,
 				   struct ieee80211_sta *sta,
@@ -316,7 +315,6 @@ u8 iwl_prep_station(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 	return sta_id;
 
 }
-EXPORT_SYMBOL_GPL(iwl_prep_station);
 
 #define STA_WAIT_TIMEOUT (HZ/2)
 
@@ -379,7 +377,6 @@ int iwl_add_station_common(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 	*sta_id_r = sta_id;
 	return ret;
 }
-EXPORT_SYMBOL(iwl_add_station_common);
 
 /**
  * iwl_sta_ucode_deactivate - deactivate ucode status for a station
@@ -513,7 +510,6 @@ out_err:
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 	return -EINVAL;
 }
-EXPORT_SYMBOL_GPL(iwl_remove_station);
 
 /**
  * iwl_clear_ucode_stations - clear ucode station table bits
@@ -548,7 +544,6 @@ void iwl_clear_ucode_stations(struct iwl_priv *priv,
 	if (!cleared)
 		IWL_DEBUG_INFO(priv, "No active stations found to be cleared\n");
 }
-EXPORT_SYMBOL(iwl_clear_ucode_stations);
 
 /**
  * iwl_restore_stations() - Restore driver known stations to device
@@ -625,7 +620,6 @@ void iwl_restore_stations(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	else
 		IWL_DEBUG_INFO(priv, "Restoring all known stations .... complete.\n");
 }
-EXPORT_SYMBOL(iwl_restore_stations);
 
 void iwl_reprogram_ap_sta(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
@@ -668,7 +662,6 @@ void iwl_reprogram_ap_sta(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 			priv->stations[sta_id].sta.sta.addr, ret);
 	iwl_send_lq_cmd(priv, ctx, &lq, CMD_SYNC, true);
 }
-EXPORT_SYMBOL(iwl_reprogram_ap_sta);
 
 int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
 {
@@ -680,7 +673,6 @@ int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
 
 	return WEP_INVALID_OFFSET;
 }
-EXPORT_SYMBOL(iwl_get_free_ucode_key_index);
 
 void iwl_dealloc_bcast_stations(struct iwl_priv *priv)
 {
@@ -700,7 +692,6 @@ void iwl_dealloc_bcast_stations(struct iwl_priv *priv)
 	}
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 }
-EXPORT_SYMBOL_GPL(iwl_dealloc_bcast_stations);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
 static void iwl_dump_lq_cmd(struct iwl_priv *priv,
@@ -810,7 +801,6 @@ int iwl_send_lq_cmd(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 	}
 	return ret;
 }
-EXPORT_SYMBOL(iwl_send_lq_cmd);
 
 int iwl_mac_sta_remove(struct ieee80211_hw *hw,
 		       struct ieee80211_vif *vif,
@@ -832,4 +822,3 @@ int iwl_mac_sta_remove(struct ieee80211_hw *hw,
 	mutex_unlock(&priv->mutex);
 	return ret;
 }
-EXPORT_SYMBOL(iwl_mac_sta_remove);
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
index 073b6ce6141c..277c9175dcf6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -84,7 +84,23 @@ void iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq)
 	}
 	txq->need_update = 0;
 }
-EXPORT_SYMBOL(iwl_txq_update_write_ptr);
+
+/**
+ * iwl_tx_queue_unmap -  Unmap any remaining DMA mappings and free skb's
+ */
+void iwl_tx_queue_unmap(struct iwl_priv *priv, int txq_id)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct iwl_queue *q = &txq->q;
+
+	if (q->n_bd == 0)
+		return;
+
+	 while (q->write_ptr != q->read_ptr) {
+		priv->cfg->ops->lib->txq_free_tfd(priv, txq);
+		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd);
+	}
+}
 
 /**
  * iwl_tx_queue_free - Deallocate DMA queue.
@@ -97,17 +113,10 @@ EXPORT_SYMBOL(iwl_txq_update_write_ptr);
 void iwl_tx_queue_free(struct iwl_priv *priv, int txq_id)
 {
 	struct iwl_tx_queue *txq = &priv->txq[txq_id];
-	struct iwl_queue *q = &txq->q;
 	struct device *dev = &priv->pci_dev->dev;
 	int i;
 
-	if (q->n_bd == 0)
-		return;
-
-	/* first, empty all BD's */
-	for (; q->write_ptr != q->read_ptr;
-	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd))
-		priv->cfg->ops->lib->txq_free_tfd(priv, txq);
+	iwl_tx_queue_unmap(priv, txq_id);
 
 	/* De-alloc array of command/tx buffers */
 	for (i = 0; i < TFD_TX_CMD_SLOTS; i++)
@@ -131,42 +140,35 @@ void iwl_tx_queue_free(struct iwl_priv *priv, int txq_id)
 	/* 0-fill queue descriptor structure */
 	memset(txq, 0, sizeof(*txq));
 }
-EXPORT_SYMBOL(iwl_tx_queue_free);
 
 /**
- * iwl_cmd_queue_free - Deallocate DMA queue.
- * @txq: Transmit queue to deallocate.
- *
- * Empty queue by removing and destroying all BD's.
- * Free all buffers.
- * 0-fill, but do not free "txq" descriptor structure.
+ * iwl_cmd_queue_unmap - Unmap any remaining DMA mappings from command queue
  */
-void iwl_cmd_queue_free(struct iwl_priv *priv)
+void iwl_cmd_queue_unmap(struct iwl_priv *priv)
 {
 	struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
 	struct iwl_queue *q = &txq->q;
-	struct device *dev = &priv->pci_dev->dev;
 	int i;
 	bool huge = false;
 
 	if (q->n_bd == 0)
 		return;
 
-	for (; q->read_ptr != q->write_ptr;
-	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+	while (q->read_ptr != q->write_ptr) {
 		/* we have no way to tell if it is a huge cmd ATM */
 		i = get_cmd_index(q, q->read_ptr, 0);
 
-		if (txq->meta[i].flags & CMD_SIZE_HUGE) {
+		if (txq->meta[i].flags & CMD_SIZE_HUGE)
 			huge = true;
-			continue;
-		}
+		else
+			pci_unmap_single(priv->pci_dev,
+					 dma_unmap_addr(&txq->meta[i], mapping),
+					 dma_unmap_len(&txq->meta[i], len),
+					 PCI_DMA_BIDIRECTIONAL);
 
-		pci_unmap_single(priv->pci_dev,
-				 dma_unmap_addr(&txq->meta[i], mapping),
-				 dma_unmap_len(&txq->meta[i], len),
-				 PCI_DMA_BIDIRECTIONAL);
+	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd);
 	}
+
 	if (huge) {
 		i = q->n_window;
 		pci_unmap_single(priv->pci_dev,
@@ -174,6 +176,23 @@ void iwl_cmd_queue_free(struct iwl_priv *priv)
 				 dma_unmap_len(&txq->meta[i], len),
 				 PCI_DMA_BIDIRECTIONAL);
 	}
+}
+
+/**
+ * iwl_cmd_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.
+ * 0-fill, but do not free "txq" descriptor structure.
+ */
+void iwl_cmd_queue_free(struct iwl_priv *priv)
+{
+	struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
+	struct device *dev = &priv->pci_dev->dev;
+	int i;
+
+	iwl_cmd_queue_unmap(priv);
 
 	/* De-alloc array of command/tx buffers */
 	for (i = 0; i <= TFD_CMD_SLOTS; i++)
@@ -193,7 +212,6 @@ void iwl_cmd_queue_free(struct iwl_priv *priv)
 	/* 0-fill queue descriptor structure */
 	memset(txq, 0, sizeof(*txq));
 }
-EXPORT_SYMBOL(iwl_cmd_queue_free);
 
 /*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
  * DMA services
@@ -233,7 +251,6 @@ int iwl_queue_space(const struct iwl_queue *q)
 		s = 0;
 	return s;
 }
-EXPORT_SYMBOL(iwl_queue_space);
 
 
 /**
@@ -384,7 +401,6 @@ out_free_arrays:
 
 	return -ENOMEM;
 }
-EXPORT_SYMBOL(iwl_tx_queue_init);
 
 void iwl_tx_queue_reset(struct iwl_priv *priv, struct iwl_tx_queue *txq,
 			int slots_num, u32 txq_id)
@@ -404,7 +420,6 @@ void iwl_tx_queue_reset(struct iwl_priv *priv, struct iwl_tx_queue *txq,
 	/* Tell device where to find queue */
 	priv->cfg->ops->lib->txq_init(priv, txq);
 }
-EXPORT_SYMBOL(iwl_tx_queue_reset);
 
 /*************** HOST COMMAND QUEUE FUNCTIONS   *****/
 
@@ -641,4 +656,3 @@ void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
 	}
 	meta->flags = 0;
 }
-EXPORT_SYMBOL(iwl_tx_cmd_complete);
diff --git a/drivers/net/wireless/iwmc3200wifi/cfg80211.c b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
index 5a4982271e96..ed57e4402800 100644
--- a/drivers/net/wireless/iwmc3200wifi/cfg80211.c
+++ b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
@@ -287,7 +287,8 @@ int iwm_cfg80211_inform_bss(struct iwm_priv *iwm)
 			return -EINVAL;
 		}
 
-		freq = ieee80211_channel_to_frequency(umac_bss->channel);
+		freq = ieee80211_channel_to_frequency(umac_bss->channel,
+						      band->band);
 		channel = ieee80211_get_channel(wiphy, freq);
 		signal = umac_bss->rssi * 100;
 
diff --git a/drivers/net/wireless/iwmc3200wifi/rx.c b/drivers/net/wireless/iwmc3200wifi/rx.c
index a944893ae3ca..9a57cf6a488f 100644
--- a/drivers/net/wireless/iwmc3200wifi/rx.c
+++ b/drivers/net/wireless/iwmc3200wifi/rx.c
@@ -543,7 +543,10 @@ static int iwm_mlme_assoc_complete(struct iwm_priv *iwm, u8 *buf,
 	switch (le32_to_cpu(complete->status)) {
 	case UMAC_ASSOC_COMPLETE_SUCCESS:
 		chan = ieee80211_get_channel(wiphy,
-			ieee80211_channel_to_frequency(complete->channel));
+			ieee80211_channel_to_frequency(complete->channel,
+				complete->band == UMAC_BAND_2GHZ ?
+					IEEE80211_BAND_2GHZ :
+					IEEE80211_BAND_5GHZ));
 		if (!chan || chan->flags & IEEE80211_CHAN_DISABLED) {
 			/* Associated to a unallowed channel, disassociate. */
 			__iwm_invalidate_mlme_profile(iwm);
@@ -841,7 +844,7 @@ static int iwm_mlme_update_bss_table(struct iwm_priv *iwm, u8 *buf,
 		goto err;
 	}
 
-	freq = ieee80211_channel_to_frequency(umac_bss->channel);
+	freq = ieee80211_channel_to_frequency(umac_bss->channel, band->band);
 	channel = ieee80211_get_channel(wiphy, freq);
 	signal = umac_bss->rssi * 100;
 
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index 698a1f7694ed..30ef0351bfc4 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -607,7 +607,8 @@ static int lbs_ret_scan(struct lbs_private *priv, unsigned long dummy,
 		/* No channel, no luck */
 		if (chan_no != -1) {
 			struct wiphy *wiphy = priv->wdev->wiphy;
-			int freq = ieee80211_channel_to_frequency(chan_no);
+			int freq = ieee80211_channel_to_frequency(chan_no,
+							IEEE80211_BAND_2GHZ);
 			struct ieee80211_channel *channel =
 				ieee80211_get_channel(wiphy, freq);
 
@@ -1597,7 +1598,8 @@ static int lbs_get_survey(struct wiphy *wiphy, struct net_device *dev,
 	lbs_deb_enter(LBS_DEB_CFG80211);
 
 	survey->channel = ieee80211_get_channel(wiphy,
-		ieee80211_channel_to_frequency(priv->channel));
+		ieee80211_channel_to_frequency(priv->channel,
+					       IEEE80211_BAND_2GHZ));
 
 	ret = lbs_get_rssi(priv, &signal, &noise);
 	if (ret == 0) {
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 78c4da150a74..7e8a658b7670 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -145,9 +145,13 @@ int lbs_update_hw_spec(struct lbs_private *priv)
 	if (priv->current_addr[0] == 0xff)
 		memmove(priv->current_addr, cmd.permanentaddr, ETH_ALEN);
 
-	memcpy(priv->dev->dev_addr, priv->current_addr, ETH_ALEN);
-	if (priv->mesh_dev)
-		memcpy(priv->mesh_dev->dev_addr, priv->current_addr, ETH_ALEN);
+	if (!priv->copied_hwaddr) {
+		memcpy(priv->dev->dev_addr, priv->current_addr, ETH_ALEN);
+		if (priv->mesh_dev)
+			memcpy(priv->mesh_dev->dev_addr,
+				priv->current_addr, ETH_ALEN);
+		priv->copied_hwaddr = 1;
+	}
 
 out:
 	lbs_deb_leave(LBS_DEB_CMD);
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 18dd9a02c459..bc461eb39660 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -90,6 +90,7 @@ struct lbs_private {
 	void *card;
 	u8 fw_ready;
 	u8 surpriseremoved;
+	u8 setup_fw_on_resume;
 	int (*hw_host_to_card) (struct lbs_private *priv, u8 type, u8 *payload, u16 nb);
 	void (*reset_card) (struct lbs_private *priv);
 	int (*enter_deep_sleep) (struct lbs_private *priv);
@@ -101,6 +102,7 @@ struct lbs_private {
 	u32 fwcapinfo;
 	u16 regioncode;
 	u8 current_addr[ETH_ALEN];
+	u8 copied_hwaddr;
 
 	/* Command download */
 	u8 dnld_sent;
diff --git a/drivers/net/wireless/libertas/if_spi.c b/drivers/net/wireless/libertas/if_spi.c
index 00600239a053..f6c2cd665f49 100644
--- a/drivers/net/wireless/libertas/if_spi.c
+++ b/drivers/net/wireless/libertas/if_spi.c
@@ -20,10 +20,8 @@
 #include <linux/moduleparam.h>
 #include <linux/firmware.h>
 #include <linux/jiffies.h>
-#include <linux/kthread.h>
 #include <linux/list.h>
 #include <linux/netdevice.h>
-#include <linux/semaphore.h>
 #include <linux/slab.h>
 #include <linux/spi/libertas_spi.h>
 #include <linux/spi/spi.h>
@@ -34,6 +32,12 @@
 #include "dev.h"
 #include "if_spi.h"
 
+struct if_spi_packet {
+	struct list_head		list;
+	u16				blen;
+	u8				buffer[0] __attribute__((aligned(4)));
+};
+
 struct if_spi_card {
 	struct spi_device		*spi;
 	struct lbs_private		*priv;
@@ -51,18 +55,36 @@ struct if_spi_card {
 	unsigned long			spu_reg_delay;
 
 	/* Handles all SPI communication (except for FW load) */
-	struct task_struct		*spi_thread;
-	int				run_thread;
-
-	/* Used to wake up the spi_thread */
-	struct semaphore		spi_ready;
-	struct semaphore		spi_thread_terminated;
+	struct workqueue_struct		*workqueue;
+	struct work_struct		packet_work;
 
 	u8				cmd_buffer[IF_SPI_CMD_BUF_SIZE];
+
+	/* A buffer of incoming packets from libertas core.
+	 * Since we can't sleep in hw_host_to_card, we have to buffer
+	 * them. */
+	struct list_head		cmd_packet_list;
+	struct list_head		data_packet_list;
+
+	/* Protects cmd_packet_list and data_packet_list */
+	spinlock_t			buffer_lock;
 };
 
 static void free_if_spi_card(struct if_spi_card *card)
 {
+	struct list_head *cursor, *next;
+	struct if_spi_packet *packet;
+
+	list_for_each_safe(cursor, next, &card->cmd_packet_list) {
+		packet = container_of(cursor, struct if_spi_packet, list);
+		list_del(&packet->list);
+		kfree(packet);
+	}
+	list_for_each_safe(cursor, next, &card->data_packet_list) {
+		packet = container_of(cursor, struct if_spi_packet, list);
+		list_del(&packet->list);
+		kfree(packet);
+	}
 	spi_set_drvdata(card->spi, NULL);
 	kfree(card);
 }
@@ -622,7 +644,7 @@ out:
 /*
  * SPI Transfer Thread
  *
- * The SPI thread handles all SPI transfers, so there is no need for a lock.
+ * The SPI worker handles all SPI transfers, so there is no need for a lock.
  */
 
 /* Move a command from the card to the host */
@@ -742,6 +764,40 @@ out:
 	return err;
 }
 
+/* Move data or a command from the host to the card. */
+static void if_spi_h2c(struct if_spi_card *card,
+			struct if_spi_packet *packet, int type)
+{
+	int err = 0;
+	u16 int_type, port_reg;
+
+	switch (type) {
+	case MVMS_DAT:
+		int_type = IF_SPI_CIC_TX_DOWNLOAD_OVER;
+		port_reg = IF_SPI_DATA_RDWRPORT_REG;
+		break;
+	case MVMS_CMD:
+		int_type = IF_SPI_CIC_CMD_DOWNLOAD_OVER;
+		port_reg = IF_SPI_CMD_RDWRPORT_REG;
+		break;
+	default:
+		lbs_pr_err("can't transfer buffer of type %d\n", type);
+		err = -EINVAL;
+		goto out;
+	}
+
+	/* Write the data to the card */
+	err = spu_write(card, port_reg, packet->buffer, packet->blen);
+	if (err)
+		goto out;
+
+out:
+	kfree(packet);
+
+	if (err)
+		lbs_pr_err("%s: error %d\n", __func__, err);
+}
+
 /* Inform the host about a card event */
 static void if_spi_e2h(struct if_spi_card *card)
 {
@@ -766,71 +822,88 @@ out:
 		lbs_pr_err("%s: error %d\n", __func__, err);
 }
 
-static int lbs_spi_thread(void *data)
+static void if_spi_host_to_card_worker(struct work_struct *work)
 {
 	int err;
-	struct if_spi_card *card = data;
+	struct if_spi_card *card;
 	u16 hiStatus;
+	unsigned long flags;
+	struct if_spi_packet *packet;
 
-	while (1) {
-		/* Wait to be woken up by one of two things.  First, our ISR
-		 * could tell us that something happened on the WLAN.
-		 * Secondly, libertas could call hw_host_to_card with more
-		 * data, which we might be able to send.
-		 */
-		do {
-			err = down_interruptible(&card->spi_ready);
-			if (!card->run_thread) {
-				up(&card->spi_thread_terminated);
-				do_exit(0);
-			}
-		} while (err == -EINTR);
+	card = container_of(work, struct if_spi_card, packet_work);
 
-		/* Read the host interrupt status register to see what we
-		 * can do. */
-		err = spu_read_u16(card, IF_SPI_HOST_INT_STATUS_REG,
-					&hiStatus);
-		if (err) {
-			lbs_pr_err("I/O error\n");
+	lbs_deb_enter(LBS_DEB_SPI);
+
+	/* Read the host interrupt status register to see what we
+	 * can do. */
+	err = spu_read_u16(card, IF_SPI_HOST_INT_STATUS_REG,
+				&hiStatus);
+	if (err) {
+		lbs_pr_err("I/O error\n");
+		goto err;
+	}
+
+	if (hiStatus & IF_SPI_HIST_CMD_UPLOAD_RDY) {
+		err = if_spi_c2h_cmd(card);
+		if (err)
 			goto err;
-		}
+	}
+	if (hiStatus & IF_SPI_HIST_RX_UPLOAD_RDY) {
+		err = if_spi_c2h_data(card);
+		if (err)
+			goto err;
+	}
 
-		if (hiStatus & IF_SPI_HIST_CMD_UPLOAD_RDY) {
-			err = if_spi_c2h_cmd(card);
-			if (err)
-				goto err;
-		}
-		if (hiStatus & IF_SPI_HIST_RX_UPLOAD_RDY) {
-			err = if_spi_c2h_data(card);
-			if (err)
-				goto err;
+	/* workaround: in PS mode, the card does not set the Command
+	 * Download Ready bit, but it sets TX Download Ready. */
+	if (hiStatus & IF_SPI_HIST_CMD_DOWNLOAD_RDY ||
+	   (card->priv->psstate != PS_STATE_FULL_POWER &&
+	    (hiStatus & IF_SPI_HIST_TX_DOWNLOAD_RDY))) {
+		/* This means two things. First of all,
+		 * if there was a previous command sent, the card has
+		 * successfully received it.
+		 * Secondly, it is now ready to download another
+		 * command.
+		 */
+		lbs_host_to_card_done(card->priv);
+
+		/* Do we have any command packets from the host to
+		 * send? */
+		packet = NULL;
+		spin_lock_irqsave(&card->buffer_lock, flags);
+		if (!list_empty(&card->cmd_packet_list)) {
+			packet = (struct if_spi_packet *)(card->
+					cmd_packet_list.next);
+			list_del(&packet->list);
 		}
+		spin_unlock_irqrestore(&card->buffer_lock, flags);
 
-		/* workaround: in PS mode, the card does not set the Command
-		 * Download Ready bit, but it sets TX Download Ready. */
-		if (hiStatus & IF_SPI_HIST_CMD_DOWNLOAD_RDY ||
-		   (card->priv->psstate != PS_STATE_FULL_POWER &&
-		    (hiStatus & IF_SPI_HIST_TX_DOWNLOAD_RDY))) {
-			lbs_host_to_card_done(card->priv);
+		if (packet)
+			if_spi_h2c(card, packet, MVMS_CMD);
+	}
+	if (hiStatus & IF_SPI_HIST_TX_DOWNLOAD_RDY) {
+		/* Do we have any data packets from the host to
+		 * send? */
+		packet = NULL;
+		spin_lock_irqsave(&card->buffer_lock, flags);
+		if (!list_empty(&card->data_packet_list)) {
+			packet = (struct if_spi_packet *)(card->
+					data_packet_list.next);
+			list_del(&packet->list);
 		}
+		spin_unlock_irqrestore(&card->buffer_lock, flags);
 
-		if (hiStatus & IF_SPI_HIST_CARD_EVENT)
-			if_spi_e2h(card);
+		if (packet)
+			if_spi_h2c(card, packet, MVMS_DAT);
+	}
+	if (hiStatus & IF_SPI_HIST_CARD_EVENT)
+		if_spi_e2h(card);
 
 err:
-		if (err)
-			lbs_pr_err("%s: got error %d\n", __func__, err);
-	}
-}
+	if (err)
+		lbs_pr_err("%s: got error %d\n", __func__, err);
 
-/* Block until lbs_spi_thread thread has terminated */
-static void if_spi_terminate_spi_thread(struct if_spi_card *card)
-{
-	/* It would be nice to use kthread_stop here, but that function
-	 * can't wake threads waiting for a semaphore. */
-	card->run_thread = 0;
-	up(&card->spi_ready);
-	down(&card->spi_thread_terminated);
+	lbs_deb_leave(LBS_DEB_SPI);
 }
 
 /*
@@ -842,18 +915,40 @@ static int if_spi_host_to_card(struct lbs_private *priv,
 				u8 type, u8 *buf, u16 nb)
 {
 	int err = 0;
+	unsigned long flags;
 	struct if_spi_card *card = priv->card;
+	struct if_spi_packet *packet;
+	u16 blen;
 
 	lbs_deb_enter_args(LBS_DEB_SPI, "type %d, bytes %d", type, nb);
 
-	nb = ALIGN(nb, 4);
+	if (nb == 0) {
+		lbs_pr_err("%s: invalid size requested: %d\n", __func__, nb);
+		err = -EINVAL;
+		goto out;
+	}
+	blen = ALIGN(nb, 4);
+	packet = kzalloc(sizeof(struct if_spi_packet) + blen, GFP_ATOMIC);
+	if (!packet) {
+		err = -ENOMEM;
+		goto out;
+	}
+	packet->blen = blen;
+	memcpy(packet->buffer, buf, nb);
+	memset(packet->buffer + nb, 0, blen - nb);
 
 	switch (type) {
 	case MVMS_CMD:
-		err = spu_write(card, IF_SPI_CMD_RDWRPORT_REG, buf, nb);
+		priv->dnld_sent = DNLD_CMD_SENT;
+		spin_lock_irqsave(&card->buffer_lock, flags);
+		list_add_tail(&packet->list, &card->cmd_packet_list);
+		spin_unlock_irqrestore(&card->buffer_lock, flags);
 		break;
 	case MVMS_DAT:
-		err = spu_write(card, IF_SPI_DATA_RDWRPORT_REG, buf, nb);
+		priv->dnld_sent = DNLD_DATA_SENT;
+		spin_lock_irqsave(&card->buffer_lock, flags);
+		list_add_tail(&packet->list, &card->data_packet_list);
+		spin_unlock_irqrestore(&card->buffer_lock, flags);
 		break;
 	default:
 		lbs_pr_err("can't transfer buffer of type %d", type);
@@ -861,6 +956,9 @@ static int if_spi_host_to_card(struct lbs_private *priv,
 		break;
 	}
 
+	/* Queue spi xfer work */
+	queue_work(card->workqueue, &card->packet_work);
+out:
 	lbs_deb_leave_args(LBS_DEB_SPI, "err=%d", err);
 	return err;
 }
@@ -869,13 +967,14 @@ static int if_spi_host_to_card(struct lbs_private *priv,
  * Host Interrupts
  *
  * Service incoming interrupts from the WLAN device. We can't sleep here, so
- * don't try to talk on the SPI bus, just wake up the SPI thread.
+ * don't try to talk on the SPI bus, just queue the SPI xfer work.
  */
 static irqreturn_t if_spi_host_interrupt(int irq, void *dev_id)
 {
 	struct if_spi_card *card = dev_id;
 
-	up(&card->spi_ready);
+	queue_work(card->workqueue, &card->packet_work);
+
 	return IRQ_HANDLED;
 }
 
@@ -883,56 +982,26 @@ static irqreturn_t if_spi_host_interrupt(int irq, void *dev_id)
  * SPI callbacks
  */
 
-static int __devinit if_spi_probe(struct spi_device *spi)
+static int if_spi_init_card(struct if_spi_card *card)
 {
-	struct if_spi_card *card;
-	struct lbs_private *priv = NULL;
-	struct libertas_spi_platform_data *pdata = spi->dev.platform_data;
-	int err = 0, i;
+	struct spi_device *spi = card->spi;
+	int err, i;
 	u32 scratch;
-	struct sched_param param = { .sched_priority = 1 };
 	const struct firmware *helper = NULL;
 	const struct firmware *mainfw = NULL;
 
 	lbs_deb_enter(LBS_DEB_SPI);
 
-	if (!pdata) {
-		err = -EINVAL;
-		goto out;
-	}
-
-	if (pdata->setup) {
-		err = pdata->setup(spi);
-		if (err)
-			goto out;
-	}
-
-	/* Allocate card structure to represent this specific device */
-	card = kzalloc(sizeof(struct if_spi_card), GFP_KERNEL);
-	if (!card) {
-		err = -ENOMEM;
-		goto out;
-	}
-	spi_set_drvdata(spi, card);
-	card->pdata = pdata;
-	card->spi = spi;
-	card->prev_xfer_time = jiffies;
-
-	sema_init(&card->spi_ready, 0);
-	sema_init(&card->spi_thread_terminated, 0);
-
-	/* Initialize the SPI Interface Unit */
-	err = spu_init(card, pdata->use_dummy_writes);
+	err = spu_init(card, card->pdata->use_dummy_writes);
 	if (err)
-		goto free_card;
+		goto out;
 	err = spu_get_chip_revision(card, &card->card_id, &card->card_rev);
 	if (err)
-		goto free_card;
+		goto out;
 
-	/* Firmware load */
 	err = spu_read_u32(card, IF_SPI_SCRATCH_4_REG, &scratch);
 	if (err)
-		goto free_card;
+		goto out;
 	if (scratch == SUCCESSFUL_FW_DOWNLOAD_MAGIC)
 		lbs_deb_spi("Firmware is already loaded for "
 			    "Marvell WLAN 802.11 adapter\n");
@@ -946,7 +1015,7 @@ static int __devinit if_spi_probe(struct spi_device *spi)
 			lbs_pr_err("Unsupported chip_id: 0x%02x\n",
 					card->card_id);
 			err = -ENODEV;
-			goto free_card;
+			goto out;
 		}
 
 		err = lbs_get_firmware(&card->spi->dev, NULL, NULL,
@@ -954,7 +1023,7 @@ static int __devinit if_spi_probe(struct spi_device *spi)
 					&mainfw);
 		if (err) {
 			lbs_pr_err("failed to find firmware (%d)\n", err);
-			goto free_card;
+			goto out;
 		}
 
 		lbs_deb_spi("Initializing FW for Marvell WLAN 802.11 adapter "
@@ -966,15 +1035,68 @@ static int __devinit if_spi_probe(struct spi_device *spi)
 				spi->max_speed_hz);
 		err = if_spi_prog_helper_firmware(card, helper);
 		if (err)
-			goto free_card;
+			goto out;
 		err = if_spi_prog_main_firmware(card, mainfw);
 		if (err)
-			goto free_card;
+			goto out;
 		lbs_deb_spi("loaded FW for Marvell WLAN 802.11 adapter\n");
 	}
 
 	err = spu_set_interrupt_mode(card, 0, 1);
 	if (err)
+		goto out;
+
+out:
+	if (helper)
+		release_firmware(helper);
+	if (mainfw)
+		release_firmware(mainfw);
+
+	lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
+
+	return err;
+}
+
+static int __devinit if_spi_probe(struct spi_device *spi)
+{
+	struct if_spi_card *card;
+	struct lbs_private *priv = NULL;
+	struct libertas_spi_platform_data *pdata = spi->dev.platform_data;
+	int err = 0;
+
+	lbs_deb_enter(LBS_DEB_SPI);
+
+	if (!pdata) {
+		err = -EINVAL;
+		goto out;
+	}
+
+	if (pdata->setup) {
+		err = pdata->setup(spi);
+		if (err)
+			goto out;
+	}
+
+	/* Allocate card structure to represent this specific device */
+	card = kzalloc(sizeof(struct if_spi_card), GFP_KERNEL);
+	if (!card) {
+		err = -ENOMEM;
+		goto teardown;
+	}
+	spi_set_drvdata(spi, card);
+	card->pdata = pdata;
+	card->spi = spi;
+	card->prev_xfer_time = jiffies;
+
+	INIT_LIST_HEAD(&card->cmd_packet_list);
+	INIT_LIST_HEAD(&card->data_packet_list);
+	spin_lock_init(&card->buffer_lock);
+
+	/* Initialize the SPI Interface Unit */
+
+	/* Firmware load */
+	err = if_spi_init_card(card);
+	if (err)
 		goto free_card;
 
 	/* Register our card with libertas.
@@ -993,27 +1115,16 @@ static int __devinit if_spi_probe(struct spi_device *spi)
 	priv->fw_ready = 1;
 
 	/* Initialize interrupt handling stuff. */
-	card->run_thread = 1;
-	card->spi_thread = kthread_run(lbs_spi_thread, card, "lbs_spi_thread");
-	if (IS_ERR(card->spi_thread)) {
-		card->run_thread = 0;
-		err = PTR_ERR(card->spi_thread);
-		lbs_pr_err("error creating SPI thread: err=%d\n", err);
-		goto remove_card;
-	}
-	if (sched_setscheduler(card->spi_thread, SCHED_FIFO, &param))
-		lbs_pr_err("Error setting scheduler, using default.\n");
+	card->workqueue = create_workqueue("libertas_spi");
+	INIT_WORK(&card->packet_work, if_spi_host_to_card_worker);
 
 	err = request_irq(spi->irq, if_spi_host_interrupt,
 			IRQF_TRIGGER_FALLING, "libertas_spi", card);
 	if (err) {
 		lbs_pr_err("can't get host irq line-- request_irq failed\n");
-		goto terminate_thread;
+		goto terminate_workqueue;
 	}
 
-	/* poke the IRQ handler so that we don't miss the first interrupt */
-	up(&card->spi_ready);
-
 	/* Start the card.
 	 * This will call register_netdev, and we'll start
 	 * getting interrupts... */
@@ -1028,18 +1139,16 @@ static int __devinit if_spi_probe(struct spi_device *spi)
 
 release_irq:
 	free_irq(spi->irq, card);
-terminate_thread:
-	if_spi_terminate_spi_thread(card);
-remove_card:
+terminate_workqueue:
+	flush_workqueue(card->workqueue);
+	destroy_workqueue(card->workqueue);
 	lbs_remove_card(priv); /* will call free_netdev */
 free_card:
 	free_if_spi_card(card);
+teardown:
+	if (pdata->teardown)
+		pdata->teardown(spi);
 out:
-	if (helper)
-		release_firmware(helper);
-	if (mainfw)
-		release_firmware(mainfw);
-
 	lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
 	return err;
 }
@@ -1056,7 +1165,8 @@ static int __devexit libertas_spi_remove(struct spi_device *spi)
 	lbs_remove_card(priv); /* will call free_netdev */
 
 	free_irq(spi->irq, card);
-	if_spi_terminate_spi_thread(card);
+	flush_workqueue(card->workqueue);
+	destroy_workqueue(card->workqueue);
 	if (card->pdata->teardown)
 		card->pdata->teardown(spi);
 	free_if_spi_card(card);
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 6836a6dd9853..ca8149cd5bd9 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -539,6 +539,43 @@ static int lbs_thread(void *data)
 	return 0;
 }
 
+/**
+ * @brief This function gets the HW spec from the firmware and sets
+ *        some basic parameters.
+ *
+ *  @param priv    A pointer to struct lbs_private structure
+ *  @return        0 or -1
+ */
+static int lbs_setup_firmware(struct lbs_private *priv)
+{
+	int ret = -1;
+	s16 curlevel = 0, minlevel = 0, maxlevel = 0;
+
+	lbs_deb_enter(LBS_DEB_FW);
+
+	/* Read MAC address from firmware */
+	memset(priv->current_addr, 0xff, ETH_ALEN);
+	ret = lbs_update_hw_spec(priv);
+	if (ret)
+		goto done;
+
+	/* Read power levels if available */
+	ret = lbs_get_tx_power(priv, &curlevel, &minlevel, &maxlevel);
+	if (ret == 0) {
+		priv->txpower_cur = curlevel;
+		priv->txpower_min = minlevel;
+		priv->txpower_max = maxlevel;
+	}
+
+	/* Send cmd to FW to enable 11D function */
+	ret = lbs_set_snmp_mib(priv, SNMP_MIB_OID_11D_ENABLE, 1);
+
+	lbs_set_mac_control(priv);
+done:
+	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
+	return ret;
+}
+
 int lbs_suspend(struct lbs_private *priv)
 {
 	int ret;
@@ -584,47 +621,13 @@ int lbs_resume(struct lbs_private *priv)
 			lbs_pr_err("deep sleep activation failed: %d\n", ret);
 	}
 
-	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
-	return ret;
-}
-EXPORT_SYMBOL_GPL(lbs_resume);
-
-/**
- * @brief This function gets the HW spec from the firmware and sets
- *        some basic parameters.
- *
- *  @param priv    A pointer to struct lbs_private structure
- *  @return 	   0 or -1
- */
-static int lbs_setup_firmware(struct lbs_private *priv)
-{
-	int ret = -1;
-	s16 curlevel = 0, minlevel = 0, maxlevel = 0;
-
-	lbs_deb_enter(LBS_DEB_FW);
-
-	/* Read MAC address from firmware */
-	memset(priv->current_addr, 0xff, ETH_ALEN);
-	ret = lbs_update_hw_spec(priv);
-	if (ret)
-		goto done;
-
-	/* Read power levels if available */
-	ret = lbs_get_tx_power(priv, &curlevel, &minlevel, &maxlevel);
-	if (ret == 0) {
-		priv->txpower_cur = curlevel;
-		priv->txpower_min = minlevel;
-		priv->txpower_max = maxlevel;
-	}
+	if (priv->setup_fw_on_resume)
+		ret = lbs_setup_firmware(priv);
 
-	/* Send cmd to FW to enable 11D function */
-	ret = lbs_set_snmp_mib(priv, SNMP_MIB_OID_11D_ENABLE, 1);
-
-	lbs_set_mac_control(priv);
-done:
 	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(lbs_resume);
 
 /**
  *  This function handles the timeout of command sending.
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/libertas_tf/main.c
index 9278b3c8ee30..d4005081f1df 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/libertas_tf/main.c
@@ -225,7 +225,7 @@ static void lbtf_free_adapter(struct lbtf_private *priv)
 	lbtf_deb_leave(LBTF_DEB_MAIN);
 }
 
-static int lbtf_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void lbtf_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct lbtf_private *priv = hw->priv;
 
@@ -236,7 +236,6 @@ static int lbtf_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	 * there are no buffered multicast frames to send
 	 */
 	ieee80211_stop_queues(priv->hw);
-	return NETDEV_TX_OK;
 }
 
 static void lbtf_tx_work(struct work_struct *work)
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 454f045ddff3..56f439d58013 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -541,7 +541,7 @@ static bool mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
 }
 
 
-static int mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	bool ack;
 	struct ieee80211_tx_info *txi;
@@ -551,7 +551,7 @@ static int mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	if (skb->len < 10) {
 		/* Should not happen; just a sanity check for addr1 use */
 		dev_kfree_skb(skb);
-		return NETDEV_TX_OK;
+		return;
 	}
 
 	ack = mac80211_hwsim_tx_frame(hw, skb);
@@ -571,7 +571,6 @@ static int mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK) && ack)
 		txi->flags |= IEEE80211_TX_STAT_ACK;
 	ieee80211_tx_status_irqsafe(hw, skb);
-	return NETDEV_TX_OK;
 }
 
 
@@ -943,7 +942,8 @@ static int mac80211_hwsim_testmode_cmd(struct ieee80211_hw *hw,
 static int mac80211_hwsim_ampdu_action(struct ieee80211_hw *hw,
 				       struct ieee80211_vif *vif,
 				       enum ieee80211_ampdu_mlme_action action,
-				       struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+				       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+				       u8 buf_size)
 {
 	switch (action) {
 	case IEEE80211_AMPDU_TX_START:
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index 9ecf8407cb1b..df5959f36d0b 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -232,6 +232,9 @@ struct mwl8k_priv {
 	struct completion firmware_loading_complete;
 };
 
+#define MAX_WEP_KEY_LEN         13
+#define NUM_WEP_KEYS            4
+
 /* Per interface specific private data */
 struct mwl8k_vif {
 	struct list_head list;
@@ -242,8 +245,21 @@ struct mwl8k_vif {
 
 	/* Non AMPDU sequence number assigned by driver.  */
 	u16 seqno;
+
+	/* Saved WEP keys */
+	struct {
+		u8 enabled;
+		u8 key[sizeof(struct ieee80211_key_conf) + MAX_WEP_KEY_LEN];
+	} wep_key_conf[NUM_WEP_KEYS];
+
+	/* BSSID */
+	u8 bssid[ETH_ALEN];
+
+	/* A flag to indicate is HW crypto is enabled for this bssid */
+	bool is_hw_crypto_enabled;
 };
 #define MWL8K_VIF(_vif) ((struct mwl8k_vif *)&((_vif)->drv_priv))
+#define IEEE80211_KEY_CONF(_u8) ((struct ieee80211_key_conf *)(_u8))
 
 struct mwl8k_sta {
 	/* Index into station database. Returned by UPDATE_STADB.  */
@@ -337,6 +353,7 @@ static const struct ieee80211_rate mwl8k_rates_50[] = {
 #define MWL8K_CMD_SET_RATEADAPT_MODE	0x0203
 #define MWL8K_CMD_BSS_START		0x1100		/* per-vif */
 #define MWL8K_CMD_SET_NEW_STN		0x1111		/* per-vif */
+#define MWL8K_CMD_UPDATE_ENCRYPTION	0x1122		/* per-vif */
 #define MWL8K_CMD_UPDATE_STADB		0x1123
 
 static const char *mwl8k_cmd_name(__le16 cmd, char *buf, int bufsize)
@@ -375,6 +392,7 @@ static const char *mwl8k_cmd_name(__le16 cmd, char *buf, int bufsize)
 		MWL8K_CMDNAME(SET_RATEADAPT_MODE);
 		MWL8K_CMDNAME(BSS_START);
 		MWL8K_CMDNAME(SET_NEW_STN);
+		MWL8K_CMDNAME(UPDATE_ENCRYPTION);
 		MWL8K_CMDNAME(UPDATE_STADB);
 	default:
 		snprintf(buf, bufsize, "0x%x", cmd);
@@ -715,10 +733,12 @@ static inline void mwl8k_remove_dma_header(struct sk_buff *skb, __le16 qos)
 		skb_pull(skb, sizeof(*tr) - hdrlen);
 }
 
-static inline void mwl8k_add_dma_header(struct sk_buff *skb)
+static void
+mwl8k_add_dma_header(struct sk_buff *skb, int tail_pad)
 {
 	struct ieee80211_hdr *wh;
 	int hdrlen;
+	int reqd_hdrlen;
 	struct mwl8k_dma_data *tr;
 
 	/*
@@ -730,11 +750,13 @@ static inline void mwl8k_add_dma_header(struct sk_buff *skb)
 	wh = (struct ieee80211_hdr *)skb->data;
 
 	hdrlen = ieee80211_hdrlen(wh->frame_control);
-	if (hdrlen != sizeof(*tr))
-		skb_push(skb, sizeof(*tr) - hdrlen);
+	reqd_hdrlen = sizeof(*tr);
+
+	if (hdrlen != reqd_hdrlen)
+		skb_push(skb, reqd_hdrlen - hdrlen);
 
 	if (ieee80211_is_data_qos(wh->frame_control))
-		hdrlen -= 2;
+		hdrlen -= IEEE80211_QOS_CTL_LEN;
 
 	tr = (struct mwl8k_dma_data *)skb->data;
 	if (wh != &tr->wh)
@@ -747,9 +769,52 @@ static inline void mwl8k_add_dma_header(struct sk_buff *skb)
 	 * payload".  That is, everything except for the 802.11 header.
 	 * This includes all crypto material including the MIC.
 	 */
-	tr->fwlen = cpu_to_le16(skb->len - sizeof(*tr));
+	tr->fwlen = cpu_to_le16(skb->len - sizeof(*tr) + tail_pad);
 }
 
+static void mwl8k_encapsulate_tx_frame(struct sk_buff *skb)
+{
+	struct ieee80211_hdr *wh;
+	struct ieee80211_tx_info *tx_info;
+	struct ieee80211_key_conf *key_conf;
+	int data_pad;
+
+	wh = (struct ieee80211_hdr *)skb->data;
+
+	tx_info = IEEE80211_SKB_CB(skb);
+
+	key_conf = NULL;
+	if (ieee80211_is_data(wh->frame_control))
+		key_conf = tx_info->control.hw_key;
+
+	/*
+	 * Make sure the packet header is in the DMA header format (4-address
+	 * without QoS), the necessary crypto padding between the header and the
+	 * payload has already been provided by mac80211, but it doesn't add tail
+	 * padding when HW crypto is enabled.
+	 *
+	 * We have the following trailer padding requirements:
+	 * - WEP: 4 trailer bytes (ICV)
+	 * - TKIP: 12 trailer bytes (8 MIC + 4 ICV)
+	 * - CCMP: 8 trailer bytes (MIC)
+	 */
+	data_pad = 0;
+	if (key_conf != NULL) {
+		switch (key_conf->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			data_pad = 4;
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			data_pad = 12;
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			data_pad = 8;
+			break;
+		}
+	}
+	mwl8k_add_dma_header(skb, data_pad);
+}
 
 /*
  * Packet reception for 88w8366 AP firmware.
@@ -778,6 +843,13 @@ struct mwl8k_rxd_8366_ap {
 
 #define MWL8K_8366_AP_RX_CTRL_OWNED_BY_HOST	0x80
 
+/* 8366 AP rx_status bits */
+#define MWL8K_8366_AP_RXSTAT_DECRYPT_ERR_MASK		0x80
+#define MWL8K_8366_AP_RXSTAT_GENERAL_DECRYPT_ERR	0xFF
+#define MWL8K_8366_AP_RXSTAT_TKIP_DECRYPT_MIC_ERR	0x02
+#define MWL8K_8366_AP_RXSTAT_WEP_DECRYPT_ICV_ERR	0x04
+#define MWL8K_8366_AP_RXSTAT_TKIP_DECRYPT_ICV_ERR	0x08
+
 static void mwl8k_rxd_8366_ap_init(void *_rxd, dma_addr_t next_dma_addr)
 {
 	struct mwl8k_rxd_8366_ap *rxd = _rxd;
@@ -834,10 +906,16 @@ mwl8k_rxd_8366_ap_process(void *_rxd, struct ieee80211_rx_status *status,
 	} else {
 		status->band = IEEE80211_BAND_2GHZ;
 	}
-	status->freq = ieee80211_channel_to_frequency(rxd->channel);
+	status->freq = ieee80211_channel_to_frequency(rxd->channel,
+						      status->band);
 
 	*qos = rxd->qos_control;
 
+	if ((rxd->rx_status != MWL8K_8366_AP_RXSTAT_GENERAL_DECRYPT_ERR) &&
+	    (rxd->rx_status & MWL8K_8366_AP_RXSTAT_DECRYPT_ERR_MASK) &&
+	    (rxd->rx_status & MWL8K_8366_AP_RXSTAT_TKIP_DECRYPT_MIC_ERR))
+		status->flag |= RX_FLAG_MMIC_ERROR;
+
 	return le16_to_cpu(rxd->pkt_len);
 }
 
@@ -876,6 +954,11 @@ struct mwl8k_rxd_sta {
 #define MWL8K_STA_RATE_INFO_MCS_FORMAT		0x0001
 
 #define MWL8K_STA_RX_CTRL_OWNED_BY_HOST		0x02
+#define MWL8K_STA_RX_CTRL_DECRYPT_ERROR		0x04
+/* ICV=0 or MIC=1 */
+#define MWL8K_STA_RX_CTRL_DEC_ERR_TYPE		0x08
+/* Key is uploaded only in failure case */
+#define MWL8K_STA_RX_CTRL_KEY_INDEX			0x30
 
 static void mwl8k_rxd_sta_init(void *_rxd, dma_addr_t next_dma_addr)
 {
@@ -931,9 +1014,13 @@ mwl8k_rxd_sta_process(void *_rxd, struct ieee80211_rx_status *status,
 	} else {
 		status->band = IEEE80211_BAND_2GHZ;
 	}
-	status->freq = ieee80211_channel_to_frequency(rxd->channel);
+	status->freq = ieee80211_channel_to_frequency(rxd->channel,
+						      status->band);
 
 	*qos = rxd->qos_control;
+	if ((rxd->rx_ctrl & MWL8K_STA_RX_CTRL_DECRYPT_ERROR) &&
+	    (rxd->rx_ctrl & MWL8K_STA_RX_CTRL_DEC_ERR_TYPE))
+		status->flag |= RX_FLAG_MMIC_ERROR;
 
 	return le16_to_cpu(rxd->pkt_len);
 }
@@ -1092,9 +1179,25 @@ static inline void mwl8k_save_beacon(struct ieee80211_hw *hw,
 		ieee80211_queue_work(hw, &priv->finalize_join_worker);
 }
 
+static inline struct mwl8k_vif *mwl8k_find_vif_bss(struct list_head *vif_list,
+						   u8 *bssid)
+{
+	struct mwl8k_vif *mwl8k_vif;
+
+	list_for_each_entry(mwl8k_vif,
+			    vif_list, list) {
+		if (memcmp(bssid, mwl8k_vif->bssid,
+			   ETH_ALEN) == 0)
+			return mwl8k_vif;
+	}
+
+	return NULL;
+}
+
 static int rxq_process(struct ieee80211_hw *hw, int index, int limit)
 {
 	struct mwl8k_priv *priv = hw->priv;
+	struct mwl8k_vif *mwl8k_vif = NULL;
 	struct mwl8k_rx_queue *rxq = priv->rxq + index;
 	int processed;
 
@@ -1104,6 +1207,7 @@ static int rxq_process(struct ieee80211_hw *hw, int index, int limit)
 		void *rxd;
 		int pkt_len;
 		struct ieee80211_rx_status status;
+		struct ieee80211_hdr *wh;
 		__le16 qos;
 
 		skb = rxq->buf[rxq->head].skb;
@@ -1130,8 +1234,7 @@ static int rxq_process(struct ieee80211_hw *hw, int index, int limit)
 
 		rxq->rxd_count--;
 
-		skb_put(skb, pkt_len);
-		mwl8k_remove_dma_header(skb, qos);
+		wh = &((struct mwl8k_dma_data *)skb->data)->wh;
 
 		/*
 		 * Check for a pending join operation.  Save a
@@ -1141,6 +1244,46 @@ static int rxq_process(struct ieee80211_hw *hw, int index, int limit)
 		if (mwl8k_capture_bssid(priv, (void *)skb->data))
 			mwl8k_save_beacon(hw, skb);
 
+		if (ieee80211_has_protected(wh->frame_control)) {
+
+			/* Check if hw crypto has been enabled for
+			 * this bss. If yes, set the status flags
+			 * accordingly
+			 */
+			mwl8k_vif = mwl8k_find_vif_bss(&priv->vif_list,
+								wh->addr1);
+
+			if (mwl8k_vif != NULL &&
+			    mwl8k_vif->is_hw_crypto_enabled == true) {
+				/*
+				 * When MMIC ERROR is encountered
+				 * by the firmware, payload is
+				 * dropped and only 32 bytes of
+				 * mwl8k Firmware header is sent
+				 * to the host.
+				 *
+				 * We need to add four bytes of
+				 * key information.  In it
+				 * MAC80211 expects keyidx set to
+				 * 0 for triggering Counter
+				 * Measure of MMIC failure.
+				 */
+				if (status.flag & RX_FLAG_MMIC_ERROR) {
+					struct mwl8k_dma_data *tr;
+					tr = (struct mwl8k_dma_data *)skb->data;
+					memset((void *)&(tr->data), 0, 4);
+					pkt_len += 4;
+				}
+
+				if (!ieee80211_is_auth(wh->frame_control))
+					status.flag |= RX_FLAG_IV_STRIPPED |
+						       RX_FLAG_DECRYPTED |
+						       RX_FLAG_MMIC_STRIPPED;
+			}
+		}
+
+		skb_put(skb, pkt_len);
+		mwl8k_remove_dma_header(skb, qos);
 		memcpy(IEEE80211_SKB_RXCB(skb), &status, sizeof(status));
 		ieee80211_rx_irqsafe(hw, skb);
 
@@ -1392,6 +1535,13 @@ mwl8k_txq_reclaim(struct ieee80211_hw *hw, int index, int limit, int force)
 
 		info = IEEE80211_SKB_CB(skb);
 		ieee80211_tx_info_clear_status(info);
+
+		/* Rate control is happening in the firmware.
+		 * Ensure no tx rate is being reported.
+		 */
+                info->status.rates[0].idx = -1;
+                info->status.rates[0].count = 1;
+
 		if (MWL8K_TXD_SUCCESS(status))
 			info->flags |= IEEE80211_TX_STAT_ACK;
 
@@ -1423,7 +1573,7 @@ static void mwl8k_txq_deinit(struct ieee80211_hw *hw, int index)
 	txq->txd = NULL;
 }
 
-static int
+static void
 mwl8k_txq_xmit(struct ieee80211_hw *hw, int index, struct sk_buff *skb)
 {
 	struct mwl8k_priv *priv = hw->priv;
@@ -1443,7 +1593,11 @@ mwl8k_txq_xmit(struct ieee80211_hw *hw, int index, struct sk_buff *skb)
 	else
 		qos = 0;
 
-	mwl8k_add_dma_header(skb);
+	if (priv->ap_fw)
+		mwl8k_encapsulate_tx_frame(skb);
+	else
+		mwl8k_add_dma_header(skb, 0);
+
 	wh = &((struct mwl8k_dma_data *)skb->data)->wh;
 
 	tx_info = IEEE80211_SKB_CB(skb);
@@ -1481,7 +1635,7 @@ mwl8k_txq_xmit(struct ieee80211_hw *hw, int index, struct sk_buff *skb)
 		wiphy_debug(hw->wiphy,
 			    "failed to dma map skb, dropping TX frame.\n");
 		dev_kfree_skb(skb);
-		return NETDEV_TX_OK;
+		return;
 	}
 
 	spin_lock_bh(&priv->tx_lock);
@@ -1518,8 +1672,6 @@ mwl8k_txq_xmit(struct ieee80211_hw *hw, int index, struct sk_buff *skb)
 	mwl8k_tx_start(priv);
 
 	spin_unlock_bh(&priv->tx_lock);
-
-	return NETDEV_TX_OK;
 }
 
 
@@ -1974,8 +2126,18 @@ static int mwl8k_cmd_set_hw_spec(struct ieee80211_hw *hw)
 	cmd->ps_cookie = cpu_to_le32(priv->cookie_dma);
 	cmd->rx_queue_ptr = cpu_to_le32(priv->rxq[0].rxd_dma);
 	cmd->num_tx_queues = cpu_to_le32(MWL8K_TX_QUEUES);
-	for (i = 0; i < MWL8K_TX_QUEUES; i++)
-		cmd->tx_queue_ptrs[i] = cpu_to_le32(priv->txq[i].txd_dma);
+
+	/*
+	 * Mac80211 stack has Q0 as highest priority and Q3 as lowest in
+	 * that order. Firmware has Q3 as highest priority and Q0 as lowest
+	 * in that order. Map Q3 of mac80211 to Q0 of firmware so that the
+	 * priority is interpreted the right way in firmware.
+	 */
+	for (i = 0; i < MWL8K_TX_QUEUES; i++) {
+		int j = MWL8K_TX_QUEUES - 1 - i;
+		cmd->tx_queue_ptrs[i] = cpu_to_le32(priv->txq[j].txd_dma);
+	}
+
 	cmd->flags = cpu_to_le32(MWL8K_SET_HW_SPEC_FLAG_HOST_DECR_MGMT |
 				 MWL8K_SET_HW_SPEC_FLAG_HOSTFORM_PROBERESP |
 				 MWL8K_SET_HW_SPEC_FLAG_HOSTFORM_BEACON);
@@ -3099,6 +3261,274 @@ static int mwl8k_cmd_set_new_stn_del(struct ieee80211_hw *hw,
 }
 
 /*
+ * CMD_UPDATE_ENCRYPTION.
+ */
+
+#define MAX_ENCR_KEY_LENGTH	16
+#define MIC_KEY_LENGTH		8
+
+struct mwl8k_cmd_update_encryption {
+	struct mwl8k_cmd_pkt header;
+
+	__le32 action;
+	__le32 reserved;
+	__u8 mac_addr[6];
+	__u8 encr_type;
+
+} __attribute__((packed));
+
+struct mwl8k_cmd_set_key {
+	struct mwl8k_cmd_pkt header;
+
+	__le32 action;
+	__le32 reserved;
+	__le16 length;
+	__le16 key_type_id;
+	__le32 key_info;
+	__le32 key_id;
+	__le16 key_len;
+	__u8 key_material[MAX_ENCR_KEY_LENGTH];
+	__u8 tkip_tx_mic_key[MIC_KEY_LENGTH];
+	__u8 tkip_rx_mic_key[MIC_KEY_LENGTH];
+	__le16 tkip_rsc_low;
+	__le32 tkip_rsc_high;
+	__le16 tkip_tsc_low;
+	__le32 tkip_tsc_high;
+	__u8 mac_addr[6];
+} __attribute__((packed));
+
+enum {
+	MWL8K_ENCR_ENABLE,
+	MWL8K_ENCR_SET_KEY,
+	MWL8K_ENCR_REMOVE_KEY,
+	MWL8K_ENCR_SET_GROUP_KEY,
+};
+
+#define MWL8K_UPDATE_ENCRYPTION_TYPE_WEP	0
+#define MWL8K_UPDATE_ENCRYPTION_TYPE_DISABLE	1
+#define MWL8K_UPDATE_ENCRYPTION_TYPE_TKIP	4
+#define MWL8K_UPDATE_ENCRYPTION_TYPE_MIXED	7
+#define MWL8K_UPDATE_ENCRYPTION_TYPE_AES	8
+
+enum {
+	MWL8K_ALG_WEP,
+	MWL8K_ALG_TKIP,
+	MWL8K_ALG_CCMP,
+};
+
+#define MWL8K_KEY_FLAG_TXGROUPKEY	0x00000004
+#define MWL8K_KEY_FLAG_PAIRWISE		0x00000008
+#define MWL8K_KEY_FLAG_TSC_VALID	0x00000040
+#define MWL8K_KEY_FLAG_WEP_TXKEY	0x01000000
+#define MWL8K_KEY_FLAG_MICKEY_VALID	0x02000000
+
+static int mwl8k_cmd_update_encryption_enable(struct ieee80211_hw *hw,
+					      struct ieee80211_vif *vif,
+					      u8 *addr,
+					      u8 encr_type)
+{
+	struct mwl8k_cmd_update_encryption *cmd;
+	int rc;
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	cmd->header.code = cpu_to_le16(MWL8K_CMD_UPDATE_ENCRYPTION);
+	cmd->header.length = cpu_to_le16(sizeof(*cmd));
+	cmd->action = cpu_to_le32(MWL8K_ENCR_ENABLE);
+	memcpy(cmd->mac_addr, addr, ETH_ALEN);
+	cmd->encr_type = encr_type;
+
+	rc = mwl8k_post_pervif_cmd(hw, vif, &cmd->header);
+	kfree(cmd);
+
+	return rc;
+}
+
+static int mwl8k_encryption_set_cmd_info(struct mwl8k_cmd_set_key *cmd,
+						u8 *addr,
+						struct ieee80211_key_conf *key)
+{
+	cmd->header.code = cpu_to_le16(MWL8K_CMD_UPDATE_ENCRYPTION);
+	cmd->header.length = cpu_to_le16(sizeof(*cmd));
+	cmd->length = cpu_to_le16(sizeof(*cmd) -
+				offsetof(struct mwl8k_cmd_set_key, length));
+	cmd->key_id = cpu_to_le32(key->keyidx);
+	cmd->key_len = cpu_to_le16(key->keylen);
+	memcpy(cmd->mac_addr, addr, ETH_ALEN);
+
+	switch (key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		cmd->key_type_id = cpu_to_le16(MWL8K_ALG_WEP);
+		if (key->keyidx == 0)
+			cmd->key_info =	cpu_to_le32(MWL8K_KEY_FLAG_WEP_TXKEY);
+
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		cmd->key_type_id = cpu_to_le16(MWL8K_ALG_TKIP);
+		cmd->key_info =	(key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+			? cpu_to_le32(MWL8K_KEY_FLAG_PAIRWISE)
+			: cpu_to_le32(MWL8K_KEY_FLAG_TXGROUPKEY);
+		cmd->key_info |= cpu_to_le32(MWL8K_KEY_FLAG_MICKEY_VALID
+						| MWL8K_KEY_FLAG_TSC_VALID);
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		cmd->key_type_id = cpu_to_le16(MWL8K_ALG_CCMP);
+		cmd->key_info =	(key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+			? cpu_to_le32(MWL8K_KEY_FLAG_PAIRWISE)
+			: cpu_to_le32(MWL8K_KEY_FLAG_TXGROUPKEY);
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static int mwl8k_cmd_encryption_set_key(struct ieee80211_hw *hw,
+						struct ieee80211_vif *vif,
+						u8 *addr,
+						struct ieee80211_key_conf *key)
+{
+	struct mwl8k_cmd_set_key *cmd;
+	int rc;
+	int keymlen;
+	u32 action;
+	u8 idx;
+	struct mwl8k_vif *mwl8k_vif = MWL8K_VIF(vif);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	rc = mwl8k_encryption_set_cmd_info(cmd, addr, key);
+	if (rc < 0)
+		goto done;
+
+	idx = key->keyidx;
+
+	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+		action = MWL8K_ENCR_SET_KEY;
+	else
+		action = MWL8K_ENCR_SET_GROUP_KEY;
+
+	switch (key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		if (!mwl8k_vif->wep_key_conf[idx].enabled) {
+			memcpy(mwl8k_vif->wep_key_conf[idx].key, key,
+						sizeof(*key) + key->keylen);
+			mwl8k_vif->wep_key_conf[idx].enabled = 1;
+		}
+
+		keymlen = 0;
+		action = MWL8K_ENCR_SET_KEY;
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		keymlen = MAX_ENCR_KEY_LENGTH + 2 * MIC_KEY_LENGTH;
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		keymlen = key->keylen;
+		break;
+	default:
+		rc = -ENOTSUPP;
+		goto done;
+	}
+
+	memcpy(cmd->key_material, key->key, keymlen);
+	cmd->action = cpu_to_le32(action);
+
+	rc = mwl8k_post_pervif_cmd(hw, vif, &cmd->header);
+done:
+	kfree(cmd);
+
+	return rc;
+}
+
+static int mwl8k_cmd_encryption_remove_key(struct ieee80211_hw *hw,
+						struct ieee80211_vif *vif,
+						u8 *addr,
+						struct ieee80211_key_conf *key)
+{
+	struct mwl8k_cmd_set_key *cmd;
+	int rc;
+	struct mwl8k_vif *mwl8k_vif = MWL8K_VIF(vif);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	rc = mwl8k_encryption_set_cmd_info(cmd, addr, key);
+	if (rc < 0)
+		goto done;
+
+	if (key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
+			WLAN_CIPHER_SUITE_WEP104)
+		mwl8k_vif->wep_key_conf[key->keyidx].enabled = 0;
+
+	cmd->action = cpu_to_le32(MWL8K_ENCR_REMOVE_KEY);
+
+	rc = mwl8k_post_pervif_cmd(hw, vif, &cmd->header);
+done:
+	kfree(cmd);
+
+	return rc;
+}
+
+static int mwl8k_set_key(struct ieee80211_hw *hw,
+			 enum set_key_cmd cmd_param,
+			 struct ieee80211_vif *vif,
+			 struct ieee80211_sta *sta,
+			 struct ieee80211_key_conf *key)
+{
+	int rc = 0;
+	u8 encr_type;
+	u8 *addr;
+	struct mwl8k_vif *mwl8k_vif = MWL8K_VIF(vif);
+
+	if (vif->type == NL80211_IFTYPE_STATION)
+		return -EOPNOTSUPP;
+
+	if (sta == NULL)
+		addr = hw->wiphy->perm_addr;
+	else
+		addr = sta->addr;
+
+	if (cmd_param == SET_KEY) {
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		rc = mwl8k_cmd_encryption_set_key(hw, vif, addr, key);
+		if (rc)
+			goto out;
+
+		if ((key->cipher == WLAN_CIPHER_SUITE_WEP40)
+				|| (key->cipher == WLAN_CIPHER_SUITE_WEP104))
+			encr_type = MWL8K_UPDATE_ENCRYPTION_TYPE_WEP;
+		else
+			encr_type = MWL8K_UPDATE_ENCRYPTION_TYPE_MIXED;
+
+		rc = mwl8k_cmd_update_encryption_enable(hw, vif, addr,
+								encr_type);
+		if (rc)
+			goto out;
+
+		mwl8k_vif->is_hw_crypto_enabled = true;
+
+	} else {
+		rc = mwl8k_cmd_encryption_remove_key(hw, vif, addr, key);
+
+		if (rc)
+			goto out;
+
+		mwl8k_vif->is_hw_crypto_enabled = false;
+
+	}
+out:
+	return rc;
+}
+
+/*
  * CMD_UPDATE_STADB.
  */
 struct ewc_ht_info {
@@ -3310,22 +3740,19 @@ static void mwl8k_rx_poll(unsigned long data)
 /*
  * Core driver operations.
  */
-static int mwl8k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void mwl8k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct mwl8k_priv *priv = hw->priv;
 	int index = skb_get_queue_mapping(skb);
-	int rc;
 
 	if (!priv->radio_on) {
 		wiphy_debug(hw->wiphy,
 			    "dropped TX frame since radio disabled\n");
 		dev_kfree_skb(skb);
-		return NETDEV_TX_OK;
+		return;
 	}
 
-	rc = mwl8k_txq_xmit(hw, index, skb);
-
-	return rc;
+	mwl8k_txq_xmit(hw, index, skb);
 }
 
 static int mwl8k_start(struct ieee80211_hw *hw)
@@ -3469,6 +3896,8 @@ static int mwl8k_add_interface(struct ieee80211_hw *hw,
 	mwl8k_vif->vif = vif;
 	mwl8k_vif->macid = macid;
 	mwl8k_vif->seqno = 0;
+	memcpy(mwl8k_vif->bssid, vif->addr, ETH_ALEN);
+	mwl8k_vif->is_hw_crypto_enabled = false;
 
 	/* Set the mac address.  */
 	mwl8k_cmd_set_mac_addr(hw, vif, vif->addr);
@@ -3528,9 +3957,13 @@ static int mwl8k_config(struct ieee80211_hw *hw, u32 changed)
 		if (rc)
 			goto out;
 
-		rc = mwl8k_cmd_rf_antenna(hw, MWL8K_RF_ANTENNA_RX, 0x7);
-		if (!rc)
-			rc = mwl8k_cmd_rf_antenna(hw, MWL8K_RF_ANTENNA_TX, 0x7);
+		rc = mwl8k_cmd_rf_antenna(hw, MWL8K_RF_ANTENNA_RX, 0x3);
+		if (rc)
+			wiphy_warn(hw->wiphy, "failed to set # of RX antennas");
+		rc = mwl8k_cmd_rf_antenna(hw, MWL8K_RF_ANTENNA_TX, 0x7);
+		if (rc)
+			wiphy_warn(hw->wiphy, "failed to set # of TX antennas");
+
 	} else {
 		rc = mwl8k_cmd_rf_tx_power(hw, conf->power_level);
 		if (rc)
@@ -3866,18 +4299,27 @@ static int mwl8k_sta_add(struct ieee80211_hw *hw,
 {
 	struct mwl8k_priv *priv = hw->priv;
 	int ret;
+	int i;
+	struct mwl8k_vif *mwl8k_vif = MWL8K_VIF(vif);
+	struct ieee80211_key_conf *key;
 
 	if (!priv->ap_fw) {
 		ret = mwl8k_cmd_update_stadb_add(hw, vif, sta);
 		if (ret >= 0) {
 			MWL8K_STA(sta)->peer_id = ret;
-			return 0;
+			ret = 0;
 		}
 
-		return ret;
+	} else {
+		ret = mwl8k_cmd_set_new_stn_add(hw, vif, sta);
 	}
 
-	return mwl8k_cmd_set_new_stn_add(hw, vif, sta);
+	for (i = 0; i < NUM_WEP_KEYS; i++) {
+		key = IEEE80211_KEY_CONF(mwl8k_vif->wep_key_conf[i].key);
+		if (mwl8k_vif->wep_key_conf[i].enabled)
+			mwl8k_set_key(hw, SET_KEY, vif, sta, key);
+	}
+	return ret;
 }
 
 static int mwl8k_conf_tx(struct ieee80211_hw *hw, u16 queue,
@@ -3894,12 +4336,14 @@ static int mwl8k_conf_tx(struct ieee80211_hw *hw, u16 queue,
 		if (!priv->wmm_enabled)
 			rc = mwl8k_cmd_set_wmm_mode(hw, 1);
 
-		if (!rc)
-			rc = mwl8k_cmd_set_edca_params(hw, queue,
+		if (!rc) {
+			int q = MWL8K_TX_QUEUES - 1 - queue;
+			rc = mwl8k_cmd_set_edca_params(hw, q,
 						       params->cw_min,
 						       params->cw_max,
 						       params->aifs,
 						       params->txop);
+		}
 
 		mwl8k_fw_unlock(hw);
 	}
@@ -3932,7 +4376,8 @@ static int mwl8k_get_survey(struct ieee80211_hw *hw, int idx,
 static int
 mwl8k_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		   enum ieee80211_ampdu_mlme_action action,
-		   struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+		   struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+		   u8 buf_size)
 {
 	switch (action) {
 	case IEEE80211_AMPDU_RX_START:
@@ -3955,6 +4400,7 @@ static const struct ieee80211_ops mwl8k_ops = {
 	.bss_info_changed	= mwl8k_bss_info_changed,
 	.prepare_multicast	= mwl8k_prepare_multicast,
 	.configure_filter	= mwl8k_configure_filter,
+	.set_key                = mwl8k_set_key,
 	.set_rts_threshold	= mwl8k_set_rts_threshold,
 	.sta_add		= mwl8k_sta_add,
 	.sta_remove		= mwl8k_sta_remove,
@@ -4332,7 +4778,7 @@ static int mwl8k_firmware_load_success(struct mwl8k_priv *priv)
 	hw->queues = MWL8K_TX_QUEUES;
 
 	/* Set rssi values to dBm */
-	hw->flags |= IEEE80211_HW_SIGNAL_DBM;
+	hw->flags |= IEEE80211_HW_SIGNAL_DBM | IEEE80211_HW_HAS_RATE_CONTROL;
 	hw->vif_data_size = sizeof(struct mwl8k_vif);
 	hw->sta_data_size = sizeof(struct mwl8k_sta);
 
diff --git a/drivers/net/wireless/orinoco/scan.c b/drivers/net/wireless/orinoco/scan.c
index 86cb54c842e7..e99ca1c1e0d8 100644
--- a/drivers/net/wireless/orinoco/scan.c
+++ b/drivers/net/wireless/orinoco/scan.c
@@ -111,6 +111,11 @@ static void orinoco_add_hostscan_result(struct orinoco_private *priv,
 
 	freq = ieee80211_dsss_chan_to_freq(le16_to_cpu(bss->a.channel));
 	channel = ieee80211_get_channel(wiphy, freq);
+	if (!channel) {
+		printk(KERN_DEBUG "Invalid channel designation %04X(%04X)",
+			bss->a.channel, freq);
+		return;	/* Then ignore it for now */
+	}
 	timestamp = 0;
 	capability = le16_to_cpu(bss->a.capabilities);
 	beacon_interval = le16_to_cpu(bss->a.beacon_interv);
diff --git a/drivers/net/wireless/p54/Kconfig b/drivers/net/wireless/p54/Kconfig
index 25f965ffc889..0ec55b50798e 100644
--- a/drivers/net/wireless/p54/Kconfig
+++ b/drivers/net/wireless/p54/Kconfig
@@ -43,9 +43,8 @@ config P54_SPI
 	tristate "Prism54 SPI (stlc45xx) support"
 	depends on P54_COMMON && SPI_MASTER && GENERIC_HARDIRQS
 	---help---
-	  This driver is for stlc4550 or stlc4560 based wireless chips.
-	  This driver is experimental, untested and will probably only work on
-	  Nokia's N800/N810 Portable Internet Tablet.
+	  This driver is for stlc4550 or stlc4560 based wireless chips
+	  such as Nokia's N800/N810 Portable Internet Tablet.
 
 	  If you choose to build a module, it'll be called p54spi.
 
diff --git a/drivers/net/wireless/p54/eeprom.c b/drivers/net/wireless/p54/eeprom.c
index 35b09aa0529b..13d750da9301 100644
--- a/drivers/net/wireless/p54/eeprom.c
+++ b/drivers/net/wireless/p54/eeprom.c
@@ -55,6 +55,17 @@ static struct ieee80211_rate p54_arates[] = {
 	{ .bitrate = 540, .hw_value = 11, },
 };
 
+static struct p54_rssi_db_entry p54_rssi_default = {
+	/*
+	 * The defaults are taken from usb-logs of the
+	 * vendor driver. So, they should be safe to
+	 * use in case we can't get a match from the
+	 * rssi <-> dBm conversion database.
+	 */
+	.mul = 130,
+	.add = -398,
+};
+
 #define CHAN_HAS_CAL		BIT(0)
 #define CHAN_HAS_LIMIT		BIT(1)
 #define CHAN_HAS_CURVE		BIT(2)
@@ -87,13 +98,27 @@ static int p54_get_band_from_freq(u16 freq)
 	return -1;
 }
 
+static int same_band(u16 freq, u16 freq2)
+{
+	return p54_get_band_from_freq(freq) == p54_get_band_from_freq(freq2);
+}
+
 static int p54_compare_channels(const void *_a,
 				const void *_b)
 {
 	const struct p54_channel_entry *a = _a;
 	const struct p54_channel_entry *b = _b;
 
-	return a->index - b->index;
+	return a->freq - b->freq;
+}
+
+static int p54_compare_rssichan(const void *_a,
+				const void *_b)
+{
+	const struct p54_rssi_db_entry *a = _a;
+	const struct p54_rssi_db_entry *b = _b;
+
+	return a->freq - b->freq;
 }
 
 static int p54_fill_band_bitrates(struct ieee80211_hw *dev,
@@ -145,25 +170,26 @@ static int p54_generate_band(struct ieee80211_hw *dev,
 
 	for (i = 0, j = 0; (j < list->band_channel_num[band]) &&
 			   (i < list->entries); i++) {
+		struct p54_channel_entry *chan = &list->channels[i];
 
-		if (list->channels[i].band != band)
+		if (chan->band != band)
 			continue;
 
-		if (list->channels[i].data != CHAN_HAS_ALL) {
-			wiphy_err(dev->wiphy,
-				  "%s%s%s is/are missing for channel:%d [%d MHz].\n",
-				  (list->channels[i].data & CHAN_HAS_CAL ? "" :
+		if (chan->data != CHAN_HAS_ALL) {
+			wiphy_err(dev->wiphy, "%s%s%s is/are missing for "
+				  "channel:%d [%d MHz].\n",
+				  (chan->data & CHAN_HAS_CAL ? "" :
 				   " [iqauto calibration data]"),
-				  (list->channels[i].data & CHAN_HAS_LIMIT ? "" :
+				  (chan->data & CHAN_HAS_LIMIT ? "" :
 				   " [output power limits]"),
-				  (list->channels[i].data & CHAN_HAS_CURVE ? "" :
+				  (chan->data & CHAN_HAS_CURVE ? "" :
 				   " [curve data]"),
-				  list->channels[i].index, list->channels[i].freq);
+				  chan->index, chan->freq);
 			continue;
 		}
 
-		tmp->channels[j].band = list->channels[i].band;
-		tmp->channels[j].center_freq = list->channels[i].freq;
+		tmp->channels[j].band = chan->band;
+		tmp->channels[j].center_freq = chan->freq;
 		j++;
 	}
 
@@ -291,7 +317,7 @@ static int p54_generate_channel_lists(struct ieee80211_hw *dev)
 		}
 	}
 
-	/* sort the list by the channel index */
+	/* sort the channel list by frequency */
 	sort(list->channels, list->entries, sizeof(struct p54_channel_entry),
 	     p54_compare_channels, NULL);
 
@@ -410,33 +436,121 @@ static int p54_convert_rev1(struct ieee80211_hw *dev,
 static const char *p54_rf_chips[] = { "INVALID-0", "Duette3", "Duette2",
 	"Frisbee", "Xbow", "Longbow", "INVALID-6", "INVALID-7" };
 
-static void p54_parse_rssical(struct ieee80211_hw *dev, void *data, int len,
-			     u16 type)
+static int p54_parse_rssical(struct ieee80211_hw *dev,
+			     u8 *data, int len, u16 type)
 {
 	struct p54_common *priv = dev->priv;
-	int offset = (type == PDR_RSSI_LINEAR_APPROXIMATION_EXTENDED) ? 2 : 0;
-	int entry_size = sizeof(struct pda_rssi_cal_entry) + offset;
-	int num_entries = (type == PDR_RSSI_LINEAR_APPROXIMATION) ? 1 : 2;
-	int i;
+	struct p54_rssi_db_entry *entry;
+	size_t db_len, entries;
+	int offset = 0, i;
+
+	if (type != PDR_RSSI_LINEAR_APPROXIMATION_EXTENDED) {
+		entries = (type == PDR_RSSI_LINEAR_APPROXIMATION) ? 1 : 2;
+		if (len != sizeof(struct pda_rssi_cal_entry) * entries) {
+			wiphy_err(dev->wiphy, "rssical size mismatch.\n");
+			goto err_data;
+		}
+	} else {
+		/*
+		 * Some devices (Dell 1450 USB, Xbow 5GHz card, etc...)
+		 * have an empty two byte header.
+		 */
+		if (*((__le16 *)&data[offset]) == cpu_to_le16(0))
+			offset += 2;
 
-	if (len != (entry_size * num_entries)) {
-		wiphy_err(dev->wiphy,
-			  "unknown rssi calibration data packing type:(%x) len:%d.\n",
-			  type, len);
+		entries = (len - offset) /
+			sizeof(struct pda_rssi_cal_ext_entry);
 
-		print_hex_dump_bytes("rssical:", DUMP_PREFIX_NONE,
-				     data, len);
+		if ((len - offset) % sizeof(struct pda_rssi_cal_ext_entry) ||
+		    entries <= 0) {
+			wiphy_err(dev->wiphy, "invalid rssi database.\n");
+			goto err_data;
+		}
+	}
 
-		wiphy_err(dev->wiphy, "please report this issue.\n");
-		return;
+	db_len = sizeof(*entry) * entries;
+	priv->rssi_db = kzalloc(db_len + sizeof(*priv->rssi_db), GFP_KERNEL);
+	if (!priv->rssi_db)
+		return -ENOMEM;
+
+	priv->rssi_db->offset = 0;
+	priv->rssi_db->entries = entries;
+	priv->rssi_db->entry_size = sizeof(*entry);
+	priv->rssi_db->len = db_len;
+
+	entry = (void *)((unsigned long)priv->rssi_db->data + priv->rssi_db->offset);
+	if (type == PDR_RSSI_LINEAR_APPROXIMATION_EXTENDED) {
+		struct pda_rssi_cal_ext_entry *cal = (void *) &data[offset];
+
+		for (i = 0; i < entries; i++) {
+			entry[i].freq = le16_to_cpu(cal[i].freq);
+			entry[i].mul = (s16) le16_to_cpu(cal[i].mul);
+			entry[i].add = (s16) le16_to_cpu(cal[i].add);
+		}
+	} else {
+		struct pda_rssi_cal_entry *cal = (void *) &data[offset];
+
+		for (i = 0; i < entries; i++) {
+			u16 freq;
+			switch (i) {
+			case IEEE80211_BAND_2GHZ:
+				freq = 2437;
+				break;
+			case IEEE80211_BAND_5GHZ:
+				freq = 5240;
+				break;
+			}
+
+			entry[i].freq = freq;
+			entry[i].mul = (s16) le16_to_cpu(cal[i].mul);
+			entry[i].add = (s16) le16_to_cpu(cal[i].add);
+		}
 	}
 
-	for (i = 0; i < num_entries; i++) {
-		struct pda_rssi_cal_entry *cal = data +
-						 (offset + i * entry_size);
-		priv->rssical_db[i].mul = (s16) le16_to_cpu(cal->mul);
-		priv->rssical_db[i].add = (s16) le16_to_cpu(cal->add);
+	/* sort the list by channel frequency */
+	sort(entry, entries, sizeof(*entry), p54_compare_rssichan, NULL);
+	return 0;
+
+err_data:
+	wiphy_err(dev->wiphy,
+		  "rssi calibration data packing type:(%x) len:%d.\n",
+		  type, len);
+
+	print_hex_dump_bytes("rssical:", DUMP_PREFIX_NONE, data, len);
+
+	wiphy_err(dev->wiphy, "please report this issue.\n");
+	return -EINVAL;
+}
+
+struct p54_rssi_db_entry *p54_rssi_find(struct p54_common *priv, const u16 freq)
+{
+	struct p54_rssi_db_entry *entry;
+	int i, found = -1;
+
+	if (!priv->rssi_db)
+		return &p54_rssi_default;
+
+	entry = (void *)(priv->rssi_db->data + priv->rssi_db->offset);
+	for (i = 0; i < priv->rssi_db->entries; i++) {
+		if (!same_band(freq, entry[i].freq))
+			continue;
+
+		if (found == -1) {
+			found = i;
+			continue;
+		}
+
+		/* nearest match */
+		if (abs(freq - entry[i].freq) <
+		    abs(freq - entry[found].freq)) {
+			found = i;
+			continue;
+		} else {
+			break;
+		}
 	}
+
+	return found < 0 ? &p54_rssi_default : &entry[found];
 }
 
 static void p54_parse_default_country(struct ieee80211_hw *dev,
@@ -627,21 +741,30 @@ int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
 		case PDR_RSSI_LINEAR_APPROXIMATION:
 		case PDR_RSSI_LINEAR_APPROXIMATION_DUAL_BAND:
 		case PDR_RSSI_LINEAR_APPROXIMATION_EXTENDED:
-			p54_parse_rssical(dev, entry->data, data_len,
-					  le16_to_cpu(entry->code));
+			err = p54_parse_rssical(dev, entry->data, data_len,
+						le16_to_cpu(entry->code));
+			if (err)
+				goto err;
 			break;
-		case PDR_RSSI_LINEAR_APPROXIMATION_CUSTOM: {
-			__le16 *src = (void *) entry->data;
-			s16 *dst = (void *) &priv->rssical_db;
+		case PDR_RSSI_LINEAR_APPROXIMATION_CUSTOMV2: {
+			struct pda_custom_wrapper *pda = (void *) entry->data;
+			__le16 *src;
+			u16 *dst;
 			int i;
 
-			if (data_len != sizeof(priv->rssical_db)) {
-				err = -EINVAL;
-				goto err;
-			}
-			for (i = 0; i < sizeof(priv->rssical_db) /
-					sizeof(*src); i++)
+			if (priv->rssi_db || data_len < sizeof(*pda))
+				break;
+
+			priv->rssi_db = p54_convert_db(pda, data_len);
+			if (!priv->rssi_db)
+				break;
+
+			src = (void *) priv->rssi_db->data;
+			dst = (void *) priv->rssi_db->data;
+
+			for (i = 0; i < priv->rssi_db->entries; i++)
 				*(dst++) = (s16) le16_to_cpu(*(src++));
+
 			}
 			break;
 		case PDR_PRISM_PA_CAL_OUTPUT_POWER_LIMITS_CUSTOM: {
@@ -717,6 +840,8 @@ good_eeprom:
 		SET_IEEE80211_PERM_ADDR(dev, perm_addr);
 	}
 
+	priv->cur_rssi = &p54_rssi_default;
+
 	wiphy_info(dev->wiphy, "hwaddr %pM, MAC:isl38%02x RF:%s\n",
 		   dev->wiphy->perm_addr, priv->version,
 		   p54_rf_chips[priv->rxhw]);
@@ -727,9 +852,11 @@ err:
 	kfree(priv->iq_autocal);
 	kfree(priv->output_limit);
 	kfree(priv->curve_data);
+	kfree(priv->rssi_db);
 	priv->iq_autocal = NULL;
 	priv->output_limit = NULL;
 	priv->curve_data = NULL;
+	priv->rssi_db = NULL;
 
 	wiphy_err(dev->wiphy, "eeprom parse failed!\n");
 	return err;
diff --git a/drivers/net/wireless/p54/eeprom.h b/drivers/net/wireless/p54/eeprom.h
index 9051aef11249..afde72b84606 100644
--- a/drivers/net/wireless/p54/eeprom.h
+++ b/drivers/net/wireless/p54/eeprom.h
@@ -81,6 +81,12 @@ struct pda_pa_curve_data {
 	u8 data[0];
 } __packed;
 
+struct pda_rssi_cal_ext_entry {
+	__le16 freq;
+	__le16 mul;
+	__le16 add;
+} __packed;
+
 struct pda_rssi_cal_entry {
 	__le16 mul;
 	__le16 add;
@@ -179,6 +185,7 @@ struct pda_custom_wrapper {
 
 /* used by our modificated eeprom image */
 #define PDR_RSSI_LINEAR_APPROXIMATION_CUSTOM		0xDEAD
+#define PDR_RSSI_LINEAR_APPROXIMATION_CUSTOMV2		0xCAFF
 #define PDR_PRISM_PA_CAL_OUTPUT_POWER_LIMITS_CUSTOM	0xBEEF
 #define PDR_PRISM_PA_CAL_CURVE_DATA_CUSTOM		0xB05D
 
diff --git a/drivers/net/wireless/p54/fwio.c b/drivers/net/wireless/p54/fwio.c
index 92b9b1f05fd5..2fab7d20ffc2 100644
--- a/drivers/net/wireless/p54/fwio.c
+++ b/drivers/net/wireless/p54/fwio.c
@@ -397,9 +397,9 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 	union p54_scan_body_union *body;
 	struct p54_scan_tail_rate *rate;
 	struct pda_rssi_cal_entry *rssi;
+	struct p54_rssi_db_entry *rssi_data;
 	unsigned int i;
 	void *entry;
-	int band = priv->hw->conf.channel->band;
 	__le16 freq = cpu_to_le16(priv->hw->conf.channel->center_freq);
 
 	skb = p54_alloc_skb(priv, P54_HDR_FLAG_CONTROL_OPSET, sizeof(*head) +
@@ -503,13 +503,14 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 	}
 
 	rssi = (struct pda_rssi_cal_entry *) skb_put(skb, sizeof(*rssi));
-	rssi->mul = cpu_to_le16(priv->rssical_db[band].mul);
-	rssi->add = cpu_to_le16(priv->rssical_db[band].add);
+	rssi_data = p54_rssi_find(priv, le16_to_cpu(freq));
+	rssi->mul = cpu_to_le16(rssi_data->mul);
+	rssi->add = cpu_to_le16(rssi_data->add);
 	if (priv->rxhw == PDR_SYNTH_FRONTEND_LONGBOW) {
 		/* Longbow frontend needs ever more */
 		rssi = (void *) skb_put(skb, sizeof(*rssi));
-		rssi->mul = cpu_to_le16(priv->rssical_db[band].longbow_unkn);
-		rssi->add = cpu_to_le16(priv->rssical_db[band].longbow_unk2);
+		rssi->mul = cpu_to_le16(rssi_data->longbow_unkn);
+		rssi->add = cpu_to_le16(rssi_data->longbow_unk2);
 	}
 
 	if (priv->fw_var >= 0x509) {
@@ -523,6 +524,7 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 	hdr->len = cpu_to_le16(skb->len - sizeof(*hdr));
 
 	p54_tx(priv, skb);
+	priv->cur_rssi = rssi_data;
 	return 0;
 
 err:
@@ -557,6 +559,7 @@ int p54_set_edcf(struct p54_common *priv)
 {
 	struct sk_buff *skb;
 	struct p54_edcf *edcf;
+	u8 rtd;
 
 	skb = p54_alloc_skb(priv, P54_HDR_FLAG_CONTROL_OPSET, sizeof(*edcf),
 			    P54_CONTROL_TYPE_DCFINIT, GFP_ATOMIC);
@@ -573,9 +576,15 @@ int p54_set_edcf(struct p54_common *priv)
 		edcf->sifs = 0x0a;
 		edcf->eofpad = 0x06;
 	}
+	/*
+	 * calculate the extra round trip delay according to the
+	 * formula from 802.11-2007 17.3.8.6.
+	 */
+	rtd = 3 * priv->coverage_class;
+	edcf->slottime += rtd;
+	edcf->round_trip_delay = cpu_to_le16(rtd);
 	/* (see prism54/isl_oid.h for further details) */
 	edcf->frameburst = cpu_to_le16(0);
-	edcf->round_trip_delay = cpu_to_le16(0);
 	edcf->flags = 0;
 	memset(edcf->mapping, 0, sizeof(edcf->mapping));
 	memcpy(edcf->queue, priv->qos_params, sizeof(edcf->queue));
diff --git a/drivers/net/wireless/p54/lmac.h b/drivers/net/wireless/p54/lmac.h
index 04b63ec80fa4..eb581abc1079 100644
--- a/drivers/net/wireless/p54/lmac.h
+++ b/drivers/net/wireless/p54/lmac.h
@@ -526,7 +526,7 @@ int p54_init_leds(struct p54_common *priv);
 void p54_unregister_leds(struct p54_common *priv);
 
 /* xmit functions */
-int p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb);
+void p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb);
 int p54_tx_cancel(struct p54_common *priv, __le32 req_id);
 void p54_tx(struct p54_common *priv, struct sk_buff *skb);
 
@@ -551,6 +551,7 @@ int p54_upload_key(struct p54_common *priv, u8 algo, int slot,
 /* eeprom */
 int p54_download_eeprom(struct p54_common *priv, void *buf,
 			u16 offset, u16 len);
+struct p54_rssi_db_entry *p54_rssi_find(struct p54_common *p, const u16 freq);
 
 /* utility */
 u8 *p54_find_ie(struct sk_buff *skb, u8 ie);
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index 622d27b6d8f2..356e6bb443a6 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -157,7 +157,7 @@ static int p54_beacon_update(struct p54_common *priv,
 	 * to cancel the old beacon template by hand, instead the firmware
 	 * will release the previous one through the feedback mechanism.
 	 */
-	WARN_ON(p54_tx_80211(priv->hw, beacon));
+	p54_tx_80211(priv->hw, beacon);
 	priv->tsf_high32 = 0;
 	priv->tsf_low32 = 0;
 
@@ -524,6 +524,59 @@ static int p54_get_survey(struct ieee80211_hw *dev, int idx,
 	return 0;
 }
 
+static unsigned int p54_flush_count(struct p54_common *priv)
+{
+	unsigned int total = 0, i;
+
+	BUILD_BUG_ON(P54_QUEUE_NUM > ARRAY_SIZE(priv->tx_stats));
+
+	/*
+	 * Because the firmware has the sole control over any frames
+	 * in the P54_QUEUE_BEACON or P54_QUEUE_SCAN queues, they
+	 * don't really count as pending or active.
+	 */
+	for (i = P54_QUEUE_MGMT; i < P54_QUEUE_NUM; i++)
+		total += priv->tx_stats[i].len;
+	return total;
+}
+
+static void p54_flush(struct ieee80211_hw *dev, bool drop)
+{
+	struct p54_common *priv = dev->priv;
+	unsigned int total, i;
+
+	/*
+	 * Currently, it wouldn't really matter if we wait for one second
+	 * or 15 minutes. But once someone gets around and completes the
+	 * TODOs [ancel stuck frames / reset device] in p54_work, it will
+	 * suddenly make sense to wait that long.
+	 */
+	i = P54_STATISTICS_UPDATE * 2 / 20;
+
+	/*
+	 * In this case no locking is required because as we speak the
+	 * queues have already been stopped and no new frames can sneak
+	 * up from behind.
+	 */
+	while ((total = p54_flush_count(priv) && i--)) {
+		/* waste time */
+		msleep(20);
+	}
+
+	WARN(total, "tx flush timeout, unresponsive firmware");
+}
+
+static void p54_set_coverage_class(struct ieee80211_hw *dev, u8 coverage_class)
+{
+	struct p54_common *priv = dev->priv;
+
+	mutex_lock(&priv->conf_mutex);
+	/* support all coverage class values as in 802.11-2007 Table 7-27 */
+	priv->coverage_class = clamp_t(u8, coverage_class, 0, 31);
+	p54_set_edcf(priv);
+	mutex_unlock(&priv->conf_mutex);
+}
+
 static const struct ieee80211_ops p54_ops = {
 	.tx			= p54_tx_80211,
 	.start			= p54_start,
@@ -536,11 +589,13 @@ static const struct ieee80211_ops p54_ops = {
 	.sta_remove		= p54_sta_add_remove,
 	.set_key		= p54_set_key,
 	.config			= p54_config,
+	.flush			= p54_flush,
 	.bss_info_changed	= p54_bss_info_changed,
 	.configure_filter	= p54_configure_filter,
 	.conf_tx		= p54_conf_tx,
 	.get_stats		= p54_get_stats,
 	.get_survey		= p54_get_survey,
+	.set_coverage_class	= p54_set_coverage_class,
 };
 
 struct ieee80211_hw *p54_init_common(size_t priv_data_len)
@@ -611,7 +666,7 @@ EXPORT_SYMBOL_GPL(p54_init_common);
 
 int p54_register_common(struct ieee80211_hw *dev, struct device *pdev)
 {
-	struct p54_common *priv = dev->priv;
+	struct p54_common __maybe_unused *priv = dev->priv;
 	int err;
 
 	err = ieee80211_register_hw(dev);
@@ -642,10 +697,12 @@ void p54_free_common(struct ieee80211_hw *dev)
 	kfree(priv->iq_autocal);
 	kfree(priv->output_limit);
 	kfree(priv->curve_data);
+	kfree(priv->rssi_db);
 	kfree(priv->used_rxkeys);
 	priv->iq_autocal = NULL;
 	priv->output_limit = NULL;
 	priv->curve_data = NULL;
+	priv->rssi_db = NULL;
 	priv->used_rxkeys = NULL;
 	ieee80211_free_hw(dev);
 }
diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/p54/p54.h
index 43a3b2ead81a..50730fc23fe5 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/p54/p54.h
@@ -116,7 +116,8 @@ struct p54_edcf_queue_param {
 	__le16 txop;
 } __packed;
 
-struct p54_rssi_linear_approximation {
+struct p54_rssi_db_entry {
+	u16 freq;
 	s16 mul;
 	s16 add;
 	s16 longbow_unkn;
@@ -197,13 +198,14 @@ struct p54_common {
 	u8 rx_diversity_mask;
 	u8 tx_diversity_mask;
 	unsigned int output_power;
+	struct p54_rssi_db_entry *cur_rssi;
 	int noise;
 	/* calibration, output power limit and rssi<->dBm conversation data */
 	struct pda_iq_autocal_entry *iq_autocal;
 	unsigned int iq_autocal_len;
 	struct p54_cal_database *curve_data;
 	struct p54_cal_database *output_limit;
-	struct p54_rssi_linear_approximation rssical_db[IEEE80211_NUM_BANDS];
+	struct p54_cal_database *rssi_db;
 	struct ieee80211_supported_band *band_table[IEEE80211_NUM_BANDS];
 
 	/* BBP/MAC state */
@@ -215,6 +217,7 @@ struct p54_common {
 	u32 tsf_low32, tsf_high32;
 	u32 basic_rate_mask;
 	u16 aid;
+	u8 coverage_class;
 	bool powersave_override;
 	__le32 beacon_req_id;
 	struct completion beacon_comp;
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/p54/p54pci.c
index 1eacba4daa5b..0494d7b102d4 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/p54/p54pci.c
@@ -199,6 +199,7 @@ static void p54p_check_rx_ring(struct ieee80211_hw *dev, u32 *index,
 	while (i != idx) {
 		u16 len;
 		struct sk_buff *skb;
+		dma_addr_t dma_addr;
 		desc = &ring[i];
 		len = le16_to_cpu(desc->len);
 		skb = rx_buf[i];
@@ -216,17 +217,20 @@ static void p54p_check_rx_ring(struct ieee80211_hw *dev, u32 *index,
 
 			len = priv->common.rx_mtu;
 		}
+		dma_addr = le32_to_cpu(desc->host_addr);
+		pci_dma_sync_single_for_cpu(priv->pdev, dma_addr,
+			priv->common.rx_mtu + 32, PCI_DMA_FROMDEVICE);
 		skb_put(skb, len);
 
 		if (p54_rx(dev, skb)) {
-			pci_unmap_single(priv->pdev,
-					 le32_to_cpu(desc->host_addr),
-					 priv->common.rx_mtu + 32,
-					 PCI_DMA_FROMDEVICE);
+			pci_unmap_single(priv->pdev, dma_addr,
+				priv->common.rx_mtu + 32, PCI_DMA_FROMDEVICE);
 			rx_buf[i] = NULL;
-			desc->host_addr = 0;
+			desc->host_addr = cpu_to_le32(0);
 		} else {
 			skb_trim(skb, 0);
+			pci_dma_sync_single_for_device(priv->pdev, dma_addr,
+				priv->common.rx_mtu + 32, PCI_DMA_FROMDEVICE);
 			desc->len = cpu_to_le16(priv->common.rx_mtu + 32);
 		}
 
diff --git a/drivers/net/wireless/p54/p54spi_eeprom.h b/drivers/net/wireless/p54/p54spi_eeprom.h
index d592cbd34d78..0b7bfb0adcf2 100644
--- a/drivers/net/wireless/p54/p54spi_eeprom.h
+++ b/drivers/net/wireless/p54/p54spi_eeprom.h
@@ -65,9 +65,10 @@ static unsigned char p54spi_eeprom[] = {
 0x03, 0x00, 0x00, 0x11,		/* PDR_ANTENNA_GAIN */
 	0x08, 0x08, 0x08, 0x08,
 
-0x09, 0x00, 0xad, 0xde,		/* PDR_RSSI_LINEAR_APPROXIMATION_CUSTOM */
-	0x0a, 0x01, 0x72, 0xfe, 0x1a, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x0a, 0x00, 0xff, 0xca,		/* PDR_RSSI_LINEAR_APPROXIMATION_CUSTOMV2 */
+	0x01, 0x00, 0x0a, 0x00,
+	0x00, 0x00, 0x0a, 0x00,
+		0x85, 0x09, 0x0a, 0x01, 0x72, 0xfe, 0x1a, 0x00, 0x00, 0x00,
 
 /* struct pda_custom_wrapper */
 0x10, 0x06, 0x5d, 0xb0,		/* PDR_PRISM_PA_CAL_CURVE_DATA_CUSTOM */
@@ -671,7 +672,7 @@ static unsigned char p54spi_eeprom[] = {
 	0xa8, 0x09, 0x25, 0x00, 0xf5, 0xff, 0xf9, 0xff, 0x00, 0x01,
 
 0x02, 0x00, 0x00, 0x00,		/* PDR_END */
-	0x67, 0x99,
+	0xb6, 0x04,
 };
 
 #endif /* P54SPI_EEPROM_H */
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/p54/p54usb.c
index 21713a7638c4..9b344a921e74 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/p54/p54usb.c
@@ -98,6 +98,7 @@ static struct usb_device_id p54u_table[] __devinitdata = {
 	{USB_DEVICE(0x1413, 0x5400)},   /* Telsey 802.11g USB2.0 Adapter */
 	{USB_DEVICE(0x1435, 0x0427)},	/* Inventel UR054G */
 	{USB_DEVICE(0x1668, 0x1050)},	/* Actiontec 802UIG-1 */
+	{USB_DEVICE(0x1740, 0x1000)},	/* Senao NUB-350 */
 	{USB_DEVICE(0x2001, 0x3704)},	/* DLink DWL-G122 rev A2 */
 	{USB_DEVICE(0x2001, 0x3705)},	/* D-Link DWL-G120 rev C1 */
 	{USB_DEVICE(0x413c, 0x5513)},	/* Dell WLA3310 USB Wireless Adapter */
diff --git a/drivers/net/wireless/p54/txrx.c b/drivers/net/wireless/p54/txrx.c
index f618b9623e5a..7834c26c2954 100644
--- a/drivers/net/wireless/p54/txrx.c
+++ b/drivers/net/wireless/p54/txrx.c
@@ -273,11 +273,9 @@ void p54_tx(struct p54_common *priv, struct sk_buff *skb)
 
 static int p54_rssi_to_dbm(struct p54_common *priv, int rssi)
 {
-	int band = priv->hw->conf.channel->band;
-
 	if (priv->rxhw != 5) {
-		return ((rssi * priv->rssical_db[band].mul) / 64 +
-			 priv->rssical_db[band].add) / 4;
+		return ((rssi * priv->cur_rssi->mul) / 64 +
+			 priv->cur_rssi->add) / 4;
 	} else {
 		/*
 		 * TODO: find the correct formula
@@ -369,7 +367,7 @@ static int p54_rx_data(struct p54_common *priv, struct sk_buff *skb)
 	rx_status->mactime = ((u64)priv->tsf_high32) << 32 | tsf32;
 	priv->tsf_low32 = tsf32;
 
-	rx_status->flag |= RX_FLAG_TSFT;
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
 
 	if (hdr->flags & cpu_to_le16(P54_HDR_FLAG_DATA_ALIGN))
 		header_len += hdr->align[0];
@@ -698,7 +696,7 @@ static u8 p54_convert_algo(u32 cipher)
 	}
 }
 
-int p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb)
+void p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct p54_common *priv = dev->priv;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -719,12 +717,8 @@ int p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb)
 			    &hdr_flags, &aid, &burst_allowed);
 
 	if (p54_tx_qos_accounting_alloc(priv, skb, queue)) {
-		if (!IS_QOS_QUEUE(queue)) {
-			dev_kfree_skb_any(skb);
-			return NETDEV_TX_OK;
-		} else {
-			return NETDEV_TX_BUSY;
-		}
+		dev_kfree_skb_any(skb);
+		return;
 	}
 
 	padding = (unsigned long)(skb->data - (sizeof(*hdr) + sizeof(*txhdr))) & 3;
@@ -867,5 +861,4 @@ int p54_tx_80211(struct ieee80211_hw *dev, struct sk_buff *skb)
 	p54info->extra_len = extra_len;
 
 	p54_tx(priv, skb);
-	return NETDEV_TX_OK;
 }
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 848cc2cce247..518542b4bf9e 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -2597,6 +2597,9 @@ static int rndis_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
 	__le32 mode;
 	int ret;
 
+	if (priv->device_type != RNDIS_BCM4320B)
+		return -ENOTSUPP;
+
 	netdev_dbg(usbdev->net, "%s(): %s, %d\n", __func__,
 				enabled ? "enabled" : "disabled",
 				timeout);
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 6f383cd684b0..f630552427b7 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -97,6 +97,18 @@ config RT2800PCI_RT35XX
 	  Support for these devices is non-functional at the moment and is
 	  intended for testers and developers.
 
+config RT2800PCI_RT53XX
+       bool "rt2800-pci - Include support for rt53xx devices (EXPERIMENTAL)"
+       depends on EXPERIMENTAL
+       default n
+       ---help---
+         This adds support for rt53xx wireless chipset family to the
+         rt2800pci driver.
+         Supported chips: RT5390
+
+         Support for these devices is non-functional at the moment and is
+         intended for testers and developers.
+
 endif
 
 config RT2500USB
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 54ca49ad3472..329f3283697b 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -46,7 +46,7 @@
  * These indirect registers work with busy bits,
  * and we will try maximal REGISTER_BUSY_COUNT times to access
  * the register while taking a REGISTER_BUSY_DELAY us delay
- * between each attampt. When the busy bit is still set at that time,
+ * between each attempt. When the busy bit is still set at that time,
  * the access attempt is considered to have failed,
  * and we will print an error.
  */
@@ -305,9 +305,7 @@ static void rt2400pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable synchronisation.
 		 */
 		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
-		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TSF_SYNC, conf->sync);
-		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
 	}
 
@@ -647,6 +645,11 @@ static void rt2400pci_start_queue(struct data_queue *queue)
 		rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
+		/*
+		 * Allow the tbtt tasklet to be scheduled.
+		 */
+		tasklet_enable(&rt2x00dev->tbtt_tasklet);
+
 		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
@@ -708,6 +711,11 @@ static void rt2400pci_stop_queue(struct data_queue *queue)
 		rt2x00_set_field32(&reg, CSR14_TBCN, 0);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
 		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+
+		/*
+		 * Wait for possibly running tbtt tasklets.
+		 */
+		tasklet_disable(&rt2x00dev->tbtt_tasklet);
 		break;
 	default:
 		break;
@@ -771,7 +779,7 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00pci_register_read(rt2x00dev, TXCSR2, &reg);
 	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE, rt2x00dev->tx[0].desc_size);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD, rt2x00dev->tx[1].limit);
-	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->bcn[1].limit);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->atim->limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
 	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
 
@@ -787,13 +795,13 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
 
-	entry_priv = rt2x00dev->bcn[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->atim->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
 
-	entry_priv = rt2x00dev->bcn[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->bcn->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
 			   entry_priv->desc_dma);
@@ -963,9 +971,9 @@ static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 				 enum dev_state state)
 {
-	int mask = (state == STATE_RADIO_IRQ_OFF) ||
-		   (state == STATE_RADIO_IRQ_OFF_ISR);
+	int mask = (state == STATE_RADIO_IRQ_OFF);
 	u32 reg;
+	unsigned long flags;
 
 	/*
 	 * When interrupts are being enabled, the interrupt registers
@@ -974,12 +982,20 @@ static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	if (state == STATE_RADIO_IRQ_ON) {
 		rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
 		rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+
+		/*
+		 * Enable tasklets.
+		 */
+		tasklet_enable(&rt2x00dev->txstatus_tasklet);
+		tasklet_enable(&rt2x00dev->rxdone_tasklet);
 	}
 
 	/*
 	 * Only toggle the interrupts bits we are going to use.
 	 * Non-checked interrupt bits are disabled by default.
 	 */
+	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
+
 	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
 	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
@@ -987,6 +1003,17 @@ static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, mask);
 	rt2x00_set_field32(&reg, CSR8_RXDONE, mask);
 	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+
+	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
+
+	if (state == STATE_RADIO_IRQ_OFF) {
+		/*
+		 * Ensure that all tasklets are finished before
+		 * disabling the interrupts.
+		 */
+		tasklet_disable(&rt2x00dev->txstatus_tasklet);
+		tasklet_disable(&rt2x00dev->rxdone_tasklet);
+	}
 }
 
 static int rt2400pci_enable_radio(struct rt2x00_dev *rt2x00dev)
@@ -1059,9 +1086,7 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt2400pci_disable_radio(rt2x00dev);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		rt2400pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
@@ -1106,19 +1131,21 @@ static void rt2400pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_desc_write(txd, 2, word);
 
 	rt2x00_desc_read(txd, 3, &word);
-	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, txdesc->signal);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, txdesc->u.plcp.signal);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL_REGNUM, 5);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL_BUSY, 1);
-	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, txdesc->service);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, txdesc->u.plcp.service);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE_REGNUM, 6);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE_BUSY, 1);
 	rt2x00_desc_write(txd, 3, word);
 
 	rt2x00_desc_read(txd, 4, &word);
-	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_LOW, txdesc->length_low);
+	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_LOW,
+			   txdesc->u.plcp.length_low);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW_REGNUM, 8);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW_BUSY, 1);
-	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_HIGH, txdesc->length_high);
+	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_HIGH,
+			   txdesc->u.plcp.length_high);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_HIGH_REGNUM, 7);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_HIGH_BUSY, 1);
 	rt2x00_desc_write(txd, 4, word);
@@ -1139,7 +1166,7 @@ static void rt2400pci_write_tx_desc(struct queue_entry *entry,
 			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_RTS,
 			   test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags));
-	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->u.plcp.ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
 			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_desc_write(txd, 0, word);
@@ -1183,8 +1210,6 @@ static void rt2400pci_write_beacon(struct queue_entry *entry,
 	/*
 	 * Enable beaconing again.
 	 */
-	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
-	rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
 	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
 }
@@ -1253,7 +1278,7 @@ static void rt2400pci_fill_rxdone(struct queue_entry *entry,
 static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
 			     const enum data_queue_qid queue_idx)
 {
-	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+	struct data_queue *queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
 	struct queue_entry_priv_pci *entry_priv;
 	struct queue_entry *entry;
 	struct txdone_entry_desc txdesc;
@@ -1289,57 +1314,68 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
 	}
 }
 
-static irqreturn_t rt2400pci_interrupt_thread(int irq, void *dev_instance)
+static void rt2400pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
+				       struct rt2x00_field32 irq_field)
 {
-	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg = rt2x00dev->irqvalue[0];
+	u32 reg;
 
 	/*
-	 * Handle interrupts, walk through all bits
-	 * and run the tasks, the bits are checked in order of
-	 * priority.
+	 * Enable a single interrupt. The interrupt mask register
+	 * access needs locking.
 	 */
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	/*
-	 * 1 - Beacon timer expired interrupt.
-	 */
-	if (rt2x00_get_field32(reg, CSR7_TBCN_EXPIRE))
-		rt2x00lib_beacondone(rt2x00dev);
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00_set_field32(&reg, irq_field, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
 
-	/*
-	 * 2 - Rx ring done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, CSR7_RXDONE))
-		rt2x00pci_rxdone(rt2x00dev);
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
 
-	/*
-	 * 3 - Atim ring transmit done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING))
-		rt2400pci_txdone(rt2x00dev, QID_ATIM);
+static void rt2400pci_txstatus_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	u32 reg;
 
 	/*
-	 * 4 - Priority ring transmit done interrupt.
+	 * Handle all tx queues.
 	 */
-	if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
-		rt2400pci_txdone(rt2x00dev, QID_AC_VO);
+	rt2400pci_txdone(rt2x00dev, QID_ATIM);
+	rt2400pci_txdone(rt2x00dev, QID_AC_VO);
+	rt2400pci_txdone(rt2x00dev, QID_AC_VI);
 
 	/*
-	 * 5 - Tx ring transmit done interrupt.
+	 * Enable all TXDONE interrupts again.
 	 */
-	if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
-		rt2400pci_txdone(rt2x00dev, QID_AC_VI);
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	/* Enable interrupts again. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_ON_ISR);
-	return IRQ_HANDLED;
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, 0);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, 0);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
+
+static void rt2400pci_tbtt_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00lib_beacondone(rt2x00dev);
+	rt2400pci_enable_interrupt(rt2x00dev, CSR8_TBCN_EXPIRE);
+}
+
+static void rt2400pci_rxdone_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00pci_rxdone(rt2x00dev);
+	rt2400pci_enable_interrupt(rt2x00dev, CSR8_RXDONE);
 }
 
 static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 {
 	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg;
+	u32 reg, mask;
 
 	/*
 	 * Get the interrupt sources & saved to local variable.
@@ -1354,14 +1390,44 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 	if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		return IRQ_HANDLED;
 
-	/* Store irqvalues for use in the interrupt thread. */
-	rt2x00dev->irqvalue[0] = reg;
+	mask = reg;
 
-	/* Disable interrupts, will be enabled again in the interrupt thread. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_OFF_ISR);
+	/*
+	 * Schedule tasklets for interrupt handling.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TBCN_EXPIRE))
+		tasklet_hi_schedule(&rt2x00dev->tbtt_tasklet);
 
-	return IRQ_WAKE_THREAD;
+	if (rt2x00_get_field32(reg, CSR7_RXDONE))
+		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
+
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING) ||
+	    rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING) ||
+	    rt2x00_get_field32(reg, CSR7_TXDONE_TXRING)) {
+		tasklet_schedule(&rt2x00dev->txstatus_tasklet);
+		/*
+		 * Mask out all txdone interrupts.
+		 */
+		rt2x00_set_field32(&mask, CSR8_TXDONE_TXRING, 1);
+		rt2x00_set_field32(&mask, CSR8_TXDONE_ATIMRING, 1);
+		rt2x00_set_field32(&mask, CSR8_TXDONE_PRIORING, 1);
+	}
+
+	/*
+	 * Disable all interrupts for which a tasklet was scheduled right now,
+	 * the tasklet will reenable the appropriate interrupts.
+	 */
+	spin_lock(&rt2x00dev->irqmask_lock);
+
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	reg |= mask;
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+
+	spin_unlock(&rt2x00dev->irqmask_lock);
+
+
+
+	return IRQ_HANDLED;
 }
 
 /*
@@ -1574,6 +1640,7 @@ static int rt2400pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 */
 	__set_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
 	__set_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_SW_SEQNO, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -1655,7 +1722,9 @@ static const struct ieee80211_ops rt2400pci_mac80211_ops = {
 
 static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
 	.irq_handler		= rt2400pci_interrupt,
-	.irq_handler_thread	= rt2400pci_interrupt_thread,
+	.txstatus_tasklet	= rt2400pci_txstatus_tasklet,
+	.tbtt_tasklet		= rt2400pci_tbtt_tasklet,
+	.rxdone_tasklet		= rt2400pci_rxdone_tasklet,
 	.probe_hw		= rt2400pci_probe_hw,
 	.initialize		= rt2x00pci_initialize,
 	.uninitialize		= rt2x00pci_uninitialize,
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index a9ff26a27724..58277878889e 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -293,7 +293,7 @@ static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
 				  struct rt2x00intf_conf *conf,
 				  const unsigned int flags)
 {
-	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, QID_BEACON);
+	struct data_queue *queue = rt2x00dev->bcn;
 	unsigned int bcn_preload;
 	u32 reg;
 
@@ -311,9 +311,7 @@ static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable synchronisation.
 		 */
 		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
-		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TSF_SYNC, conf->sync);
-		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
 	}
 
@@ -737,6 +735,11 @@ static void rt2500pci_start_queue(struct data_queue *queue)
 		rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
+		/*
+		 * Allow the tbtt tasklet to be scheduled.
+		 */
+		tasklet_enable(&rt2x00dev->tbtt_tasklet);
+
 		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
@@ -798,6 +801,11 @@ static void rt2500pci_stop_queue(struct data_queue *queue)
 		rt2x00_set_field32(&reg, CSR14_TBCN, 0);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
 		rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+
+		/*
+		 * Wait for possibly running tbtt tasklets.
+		 */
+		tasklet_disable(&rt2x00dev->tbtt_tasklet);
 		break;
 	default:
 		break;
@@ -857,7 +865,7 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00pci_register_read(rt2x00dev, TXCSR2, &reg);
 	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE, rt2x00dev->tx[0].desc_size);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD, rt2x00dev->tx[1].limit);
-	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->bcn[1].limit);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->atim->limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
 	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
 
@@ -873,13 +881,13 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
 
-	entry_priv = rt2x00dev->bcn[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->atim->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
 
-	entry_priv = rt2x00dev->bcn[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->bcn->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
 			   entry_priv->desc_dma);
@@ -1118,9 +1126,9 @@ static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 				 enum dev_state state)
 {
-	int mask = (state == STATE_RADIO_IRQ_OFF) ||
-		   (state == STATE_RADIO_IRQ_OFF_ISR);
+	int mask = (state == STATE_RADIO_IRQ_OFF);
 	u32 reg;
+	unsigned long flags;
 
 	/*
 	 * When interrupts are being enabled, the interrupt registers
@@ -1129,12 +1137,20 @@ static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	if (state == STATE_RADIO_IRQ_ON) {
 		rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
 		rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+
+		/*
+		 * Enable tasklets.
+		 */
+		tasklet_enable(&rt2x00dev->txstatus_tasklet);
+		tasklet_enable(&rt2x00dev->rxdone_tasklet);
 	}
 
 	/*
 	 * Only toggle the interrupts bits we are going to use.
 	 * Non-checked interrupt bits are disabled by default.
 	 */
+	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
+
 	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
 	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
@@ -1142,6 +1158,16 @@ static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, mask);
 	rt2x00_set_field32(&reg, CSR8_RXDONE, mask);
 	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+
+	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
+
+	if (state == STATE_RADIO_IRQ_OFF) {
+		/*
+		 * Ensure that all tasklets are finished.
+		 */
+		tasklet_disable(&rt2x00dev->txstatus_tasklet);
+		tasklet_disable(&rt2x00dev->rxdone_tasklet);
+	}
 }
 
 static int rt2500pci_enable_radio(struct rt2x00_dev *rt2x00dev)
@@ -1214,9 +1240,7 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt2500pci_disable_radio(rt2x00dev);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		rt2500pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
@@ -1263,10 +1287,12 @@ static void rt2500pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_desc_write(txd, 2, word);
 
 	rt2x00_desc_read(txd, 3, &word);
-	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, txdesc->signal);
-	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, txdesc->service);
-	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW, txdesc->length_low);
-	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_HIGH, txdesc->length_high);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, txdesc->u.plcp.signal);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, txdesc->u.plcp.service);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW,
+			   txdesc->u.plcp.length_low);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_HIGH,
+			   txdesc->u.plcp.length_high);
 	rt2x00_desc_write(txd, 3, word);
 
 	rt2x00_desc_read(txd, 10, &word);
@@ -1291,7 +1317,7 @@ static void rt2500pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_set_field32(&word, TXD_W0_OFDM,
 			   (txdesc->rate_mode == RATE_MODE_OFDM));
 	rt2x00_set_field32(&word, TXD_W0_CIPHER_OWNER, 1);
-	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->u.plcp.ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
 			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, txdesc->length);
@@ -1337,8 +1363,6 @@ static void rt2500pci_write_beacon(struct queue_entry *entry,
 	/*
 	 * Enable beaconing again.
 	 */
-	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
-	rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
 	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
 }
@@ -1386,7 +1410,7 @@ static void rt2500pci_fill_rxdone(struct queue_entry *entry,
 static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
 			     const enum data_queue_qid queue_idx)
 {
-	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+	struct data_queue *queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
 	struct queue_entry_priv_pci *entry_priv;
 	struct queue_entry *entry;
 	struct txdone_entry_desc txdesc;
@@ -1422,58 +1446,68 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
 	}
 }
 
-static irqreturn_t rt2500pci_interrupt_thread(int irq, void *dev_instance)
+static void rt2500pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
+				       struct rt2x00_field32 irq_field)
 {
-	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg = rt2x00dev->irqvalue[0];
+	u32 reg;
 
 	/*
-	 * Handle interrupts, walk through all bits
-	 * and run the tasks, the bits are checked in order of
-	 * priority.
+	 * Enable a single interrupt. The interrupt mask register
+	 * access needs locking.
 	 */
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	/*
-	 * 1 - Beacon timer expired interrupt.
-	 */
-	if (rt2x00_get_field32(reg, CSR7_TBCN_EXPIRE))
-		rt2x00lib_beacondone(rt2x00dev);
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00_set_field32(&reg, irq_field, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
 
-	/*
-	 * 2 - Rx ring done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, CSR7_RXDONE))
-		rt2x00pci_rxdone(rt2x00dev);
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
 
-	/*
-	 * 3 - Atim ring transmit done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING))
-		rt2500pci_txdone(rt2x00dev, QID_ATIM);
+static void rt2500pci_txstatus_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	u32 reg;
 
 	/*
-	 * 4 - Priority ring transmit done interrupt.
+	 * Handle all tx queues.
 	 */
-	if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
-		rt2500pci_txdone(rt2x00dev, QID_AC_VO);
+	rt2500pci_txdone(rt2x00dev, QID_ATIM);
+	rt2500pci_txdone(rt2x00dev, QID_AC_VO);
+	rt2500pci_txdone(rt2x00dev, QID_AC_VI);
 
 	/*
-	 * 5 - Tx ring transmit done interrupt.
+	 * Enable all TXDONE interrupts again.
 	 */
-	if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
-		rt2500pci_txdone(rt2x00dev, QID_AC_VI);
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
+
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, 0);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, 0);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
 
-	/* Enable interrupts again. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_ON_ISR);
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
 
-	return IRQ_HANDLED;
+static void rt2500pci_tbtt_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00lib_beacondone(rt2x00dev);
+	rt2500pci_enable_interrupt(rt2x00dev, CSR8_TBCN_EXPIRE);
+}
+
+static void rt2500pci_rxdone_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00pci_rxdone(rt2x00dev);
+	rt2500pci_enable_interrupt(rt2x00dev, CSR8_RXDONE);
 }
 
 static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 {
 	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg;
+	u32 reg, mask;
 
 	/*
 	 * Get the interrupt sources & saved to local variable.
@@ -1488,14 +1522,42 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 	if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		return IRQ_HANDLED;
 
-	/* Store irqvalues for use in the interrupt thread. */
-	rt2x00dev->irqvalue[0] = reg;
+	mask = reg;
 
-	/* Disable interrupts, will be enabled again in the interrupt thread. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_OFF_ISR);
+	/*
+	 * Schedule tasklets for interrupt handling.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TBCN_EXPIRE))
+		tasklet_hi_schedule(&rt2x00dev->tbtt_tasklet);
 
-	return IRQ_WAKE_THREAD;
+	if (rt2x00_get_field32(reg, CSR7_RXDONE))
+		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
+
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING) ||
+	    rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING) ||
+	    rt2x00_get_field32(reg, CSR7_TXDONE_TXRING)) {
+		tasklet_schedule(&rt2x00dev->txstatus_tasklet);
+		/*
+		 * Mask out all txdone interrupts.
+		 */
+		rt2x00_set_field32(&mask, CSR8_TXDONE_TXRING, 1);
+		rt2x00_set_field32(&mask, CSR8_TXDONE_ATIMRING, 1);
+		rt2x00_set_field32(&mask, CSR8_TXDONE_PRIORING, 1);
+	}
+
+	/*
+	 * Disable all interrupts for which a tasklet was scheduled right now,
+	 * the tasklet will reenable the appropriate interrupts.
+	 */
+	spin_lock(&rt2x00dev->irqmask_lock);
+
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	reg |= mask;
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+
+	spin_unlock(&rt2x00dev->irqmask_lock);
+
+	return IRQ_HANDLED;
 }
 
 /*
@@ -1896,6 +1958,7 @@ static int rt2500pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 */
 	__set_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
 	__set_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_SW_SEQNO, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -1952,7 +2015,9 @@ static const struct ieee80211_ops rt2500pci_mac80211_ops = {
 
 static const struct rt2x00lib_ops rt2500pci_rt2x00_ops = {
 	.irq_handler		= rt2500pci_interrupt,
-	.irq_handler_thread	= rt2500pci_interrupt_thread,
+	.txstatus_tasklet	= rt2500pci_txstatus_tasklet,
+	.tbtt_tasklet		= rt2500pci_tbtt_tasklet,
+	.rxdone_tasklet		= rt2500pci_rxdone_tasklet,
 	.probe_hw		= rt2500pci_probe_hw,
 	.initialize		= rt2x00pci_initialize,
 	.uninitialize		= rt2x00pci_uninitialize,
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 6b3b1de46792..979fe6596a2d 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -478,9 +478,7 @@ static void rt2500usb_config_intf(struct rt2x00_dev *rt2x00dev,
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);
 
 		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
-		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 1);
 		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_SYNC, conf->sync);
-		rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 1);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
 	}
 
@@ -1056,9 +1054,7 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt2500usb_disable_radio(rt2x00dev);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		/* No support, but no error either */
 		break;
 	case STATE_DEEP_SLEEP:
@@ -1104,7 +1100,7 @@ static void rt2500usb_write_tx_desc(struct queue_entry *entry,
 			   (txdesc->rate_mode == RATE_MODE_OFDM));
 	rt2x00_set_field32(&word, TXD_W0_NEW_SEQ,
 			   test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags));
-	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->u.plcp.ifs);
 	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, txdesc->length);
 	rt2x00_set_field32(&word, TXD_W0_CIPHER, !!txdesc->cipher);
 	rt2x00_set_field32(&word, TXD_W0_KEY_ID, txdesc->key_idx);
@@ -1118,10 +1114,12 @@ static void rt2500usb_write_tx_desc(struct queue_entry *entry,
 	rt2x00_desc_write(txd, 1, word);
 
 	rt2x00_desc_read(txd, 2, &word);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->signal);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->service);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW, txdesc->length_low);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH, txdesc->length_high);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->u.plcp.signal);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->u.plcp.service);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW,
+			   txdesc->u.plcp.length_low);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH,
+			   txdesc->u.plcp.length_high);
 	rt2x00_desc_write(txd, 2, word);
 
 	if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc->flags)) {
@@ -1799,6 +1797,7 @@ static int rt2500usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 		__set_bit(DRIVER_REQUIRE_COPY_IV, &rt2x00dev->flags);
 	}
 	__set_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_SW_SEQNO, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index 4c55e8525cad..70b9abbdeb9e 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -51,6 +51,7 @@
  * RF3320 2.4G 1T1R(RT3350/RT3370/RT3390)
  * RF3322 2.4G 2T2R(RT3352/RT3371/RT3372/RT3391/RT3392)
  * RF3853 2.4G/5G 3T3R(RT3883/RT3563/RT3573/RT3593/RT3662)
+ * RF5390 2.4G 1T1R
  */
 #define RF2820				0x0001
 #define RF2850				0x0002
@@ -65,6 +66,7 @@
 #define RF3320				0x000b
 #define RF3322				0x000c
 #define RF3853				0x000d
+#define RF5390				0x5390
 
 /*
  * Chipset revisions.
@@ -77,6 +79,7 @@
 #define REV_RT3071E			0x0211
 #define REV_RT3090E			0x0211
 #define REV_RT3390E			0x0211
+#define REV_RT5390F			0x0502
 
 /*
  * Signal information.
@@ -121,6 +124,13 @@
 #define E2PROM_CSR_RELOAD		FIELD32(0x00000080)
 
 /*
+ * AUX_CTRL: Aux/PCI-E related configuration
+ */
+#define AUX_CTRL			0x10c
+#define AUX_CTRL_WAKE_PCIE_EN		FIELD32(0x00000002)
+#define AUX_CTRL_FORCE_PCIE_CLK		FIELD32(0x00000400)
+
+/*
  * OPT_14: Unknown register used by rt3xxx devices.
  */
 #define OPT_14_CSR			0x0114
@@ -270,6 +280,7 @@
 
 /*
  * GPIO_CTRL_CFG:
+ * GPIOD: GPIO direction, 0: Output, 1: Input
  */
 #define GPIO_CTRL_CFG			0x0228
 #define GPIO_CTRL_CFG_BIT0		FIELD32(0x00000001)
@@ -280,7 +291,14 @@
 #define GPIO_CTRL_CFG_BIT5		FIELD32(0x00000020)
 #define GPIO_CTRL_CFG_BIT6		FIELD32(0x00000040)
 #define GPIO_CTRL_CFG_BIT7		FIELD32(0x00000080)
-#define GPIO_CTRL_CFG_BIT8		FIELD32(0x00000100)
+#define GPIO_CTRL_CFG_GPIOD_BIT0	FIELD32(0x00000100)
+#define GPIO_CTRL_CFG_GPIOD_BIT1	FIELD32(0x00000200)
+#define GPIO_CTRL_CFG_GPIOD_BIT2	FIELD32(0x00000400)
+#define GPIO_CTRL_CFG_GPIOD_BIT3	FIELD32(0x00000800)
+#define GPIO_CTRL_CFG_GPIOD_BIT4	FIELD32(0x00001000)
+#define GPIO_CTRL_CFG_GPIOD_BIT5	FIELD32(0x00002000)
+#define GPIO_CTRL_CFG_GPIOD_BIT6	FIELD32(0x00004000)
+#define GPIO_CTRL_CFG_GPIOD_BIT7	FIELD32(0x00008000)
 
 /*
  * MCU_CMD_CFG
@@ -372,8 +390,12 @@
 
 /*
  * US_CYC_CNT
+ * BT_MODE_EN: Bluetooth mode enable
+ * CLOCK CYCLE: Clock cycle count in 1us.
+ * PCI:0x21, PCIE:0x7d, USB:0x1e
  */
 #define US_CYC_CNT			0x02a4
+#define US_CYC_CNT_BT_MODE_EN		FIELD32(0x00000100)
 #define US_CYC_CNT_CLOCK_CYCLE		FIELD32(0x000000ff)
 
 /*
@@ -442,7 +464,7 @@
  */
 #define	RF_CSR_CFG			0x0500
 #define RF_CSR_CFG_DATA			FIELD32(0x000000ff)
-#define RF_CSR_CFG_REGNUM		FIELD32(0x00001f00)
+#define RF_CSR_CFG_REGNUM		FIELD32(0x00003f00)
 #define RF_CSR_CFG_WRITE		FIELD32(0x00010000)
 #define RF_CSR_CFG_BUSY			FIELD32(0x00020000)
 
@@ -1132,8 +1154,8 @@
  * PROTECT_RATE: Protection control frame rate for CCK TX(RTS/CTS/CFEnd)
  * PROTECT_CTRL: Protection control frame type for CCK TX
  *               0:none, 1:RTS/CTS, 2:CTS-to-self
- * PROTECT_NAV: TXOP protection type for CCK TX
- *              0:none, 1:ShortNAVprotect, 2:LongNAVProtect
+ * PROTECT_NAV_SHORT: TXOP protection type for CCK TX with short NAV
+ * PROTECT_NAV_LONG: TXOP protection type for CCK TX with long NAV
  * TX_OP_ALLOW_CCK: CCK TXOP allowance, 0:disallow
  * TX_OP_ALLOW_OFDM: CCK TXOP allowance, 0:disallow
  * TX_OP_ALLOW_MM20: CCK TXOP allowance, 0:disallow
@@ -1145,7 +1167,8 @@
 #define CCK_PROT_CFG			0x1364
 #define CCK_PROT_CFG_PROTECT_RATE	FIELD32(0x0000ffff)
 #define CCK_PROT_CFG_PROTECT_CTRL	FIELD32(0x00030000)
-#define CCK_PROT_CFG_PROTECT_NAV	FIELD32(0x000c0000)
+#define CCK_PROT_CFG_PROTECT_NAV_SHORT	FIELD32(0x00040000)
+#define CCK_PROT_CFG_PROTECT_NAV_LONG	FIELD32(0x00080000)
 #define CCK_PROT_CFG_TX_OP_ALLOW_CCK	FIELD32(0x00100000)
 #define CCK_PROT_CFG_TX_OP_ALLOW_OFDM	FIELD32(0x00200000)
 #define CCK_PROT_CFG_TX_OP_ALLOW_MM20	FIELD32(0x00400000)
@@ -1160,7 +1183,8 @@
 #define OFDM_PROT_CFG			0x1368
 #define OFDM_PROT_CFG_PROTECT_RATE	FIELD32(0x0000ffff)
 #define OFDM_PROT_CFG_PROTECT_CTRL	FIELD32(0x00030000)
-#define OFDM_PROT_CFG_PROTECT_NAV	FIELD32(0x000c0000)
+#define OFDM_PROT_CFG_PROTECT_NAV_SHORT	FIELD32(0x00040000)
+#define OFDM_PROT_CFG_PROTECT_NAV_LONG	FIELD32(0x00080000)
 #define OFDM_PROT_CFG_TX_OP_ALLOW_CCK	FIELD32(0x00100000)
 #define OFDM_PROT_CFG_TX_OP_ALLOW_OFDM	FIELD32(0x00200000)
 #define OFDM_PROT_CFG_TX_OP_ALLOW_MM20	FIELD32(0x00400000)
@@ -1175,7 +1199,8 @@
 #define MM20_PROT_CFG			0x136c
 #define MM20_PROT_CFG_PROTECT_RATE	FIELD32(0x0000ffff)
 #define MM20_PROT_CFG_PROTECT_CTRL	FIELD32(0x00030000)
-#define MM20_PROT_CFG_PROTECT_NAV	FIELD32(0x000c0000)
+#define MM20_PROT_CFG_PROTECT_NAV_SHORT	FIELD32(0x00040000)
+#define MM20_PROT_CFG_PROTECT_NAV_LONG	FIELD32(0x00080000)
 #define MM20_PROT_CFG_TX_OP_ALLOW_CCK	FIELD32(0x00100000)
 #define MM20_PROT_CFG_TX_OP_ALLOW_OFDM	FIELD32(0x00200000)
 #define MM20_PROT_CFG_TX_OP_ALLOW_MM20	FIELD32(0x00400000)
@@ -1190,7 +1215,8 @@
 #define MM40_PROT_CFG			0x1370
 #define MM40_PROT_CFG_PROTECT_RATE	FIELD32(0x0000ffff)
 #define MM40_PROT_CFG_PROTECT_CTRL	FIELD32(0x00030000)
-#define MM40_PROT_CFG_PROTECT_NAV	FIELD32(0x000c0000)
+#define MM40_PROT_CFG_PROTECT_NAV_SHORT	FIELD32(0x00040000)
+#define MM40_PROT_CFG_PROTECT_NAV_LONG	FIELD32(0x00080000)
 #define MM40_PROT_CFG_TX_OP_ALLOW_CCK	FIELD32(0x00100000)
 #define MM40_PROT_CFG_TX_OP_ALLOW_OFDM	FIELD32(0x00200000)
 #define MM40_PROT_CFG_TX_OP_ALLOW_MM20	FIELD32(0x00400000)
@@ -1205,7 +1231,8 @@
 #define GF20_PROT_CFG			0x1374
 #define GF20_PROT_CFG_PROTECT_RATE	FIELD32(0x0000ffff)
 #define GF20_PROT_CFG_PROTECT_CTRL	FIELD32(0x00030000)
-#define GF20_PROT_CFG_PROTECT_NAV	FIELD32(0x000c0000)
+#define GF20_PROT_CFG_PROTECT_NAV_SHORT	FIELD32(0x00040000)
+#define GF20_PROT_CFG_PROTECT_NAV_LONG	FIELD32(0x00080000)
 #define GF20_PROT_CFG_TX_OP_ALLOW_CCK	FIELD32(0x00100000)
 #define GF20_PROT_CFG_TX_OP_ALLOW_OFDM	FIELD32(0x00200000)
 #define GF20_PROT_CFG_TX_OP_ALLOW_MM20	FIELD32(0x00400000)
@@ -1220,7 +1247,8 @@
 #define GF40_PROT_CFG			0x1378
 #define GF40_PROT_CFG_PROTECT_RATE	FIELD32(0x0000ffff)
 #define GF40_PROT_CFG_PROTECT_CTRL	FIELD32(0x00030000)
-#define GF40_PROT_CFG_PROTECT_NAV	FIELD32(0x000c0000)
+#define GF40_PROT_CFG_PROTECT_NAV_SHORT	FIELD32(0x00040000)
+#define GF40_PROT_CFG_PROTECT_NAV_LONG	FIELD32(0x00080000)
 #define GF40_PROT_CFG_TX_OP_ALLOW_CCK	FIELD32(0x00100000)
 #define GF40_PROT_CFG_TX_OP_ALLOW_OFDM	FIELD32(0x00200000)
 #define GF40_PROT_CFG_TX_OP_ALLOW_MM20	FIELD32(0x00400000)
@@ -1697,11 +1725,14 @@ struct mac_iveiv_entry {
  */
 
 /*
- * BBP 1: TX Antenna & Power
- * POWER: 0 - normal, 1 - drop tx power by 6dBm, 2 - drop tx power by 12dBm,
- *	3 - increase tx power by 6dBm
+ * BBP 1: TX Antenna & Power Control
+ * POWER_CTRL:
+ * 0 - normal,
+ * 1 - drop tx power by 6dBm,
+ * 2 - drop tx power by 12dBm,
+ * 3 - increase tx power by 6dBm
  */
-#define BBP1_TX_POWER			FIELD8(0x07)
+#define BBP1_TX_POWER_CTRL		FIELD8(0x07)
 #define BBP1_TX_ANTENNA			FIELD8(0x18)
 
 /*
@@ -1715,6 +1746,13 @@ struct mac_iveiv_entry {
  */
 #define BBP4_TX_BF			FIELD8(0x01)
 #define BBP4_BANDWIDTH			FIELD8(0x18)
+#define BBP4_MAC_IF_CTRL		FIELD8(0x40)
+
+/*
+ * BBP 109
+ */
+#define BBP109_TX0_POWER		FIELD8(0x0f)
+#define BBP109_TX1_POWER		FIELD8(0xf0)
 
 /*
  * BBP 138: Unknown
@@ -1725,6 +1763,11 @@ struct mac_iveiv_entry {
 #define BBP138_TX_DAC2			FIELD8(0x40)
 
 /*
+ * BBP 152: Rx Ant
+ */
+#define BBP152_RX_DEFAULT_ANT		FIELD8(0x80)
+
+/*
  * RFCSR registers
  * The wordsize of the RFCSR is 8 bits.
  */
@@ -1733,12 +1776,18 @@ struct mac_iveiv_entry {
  * RFCSR 1:
  */
 #define RFCSR1_RF_BLOCK_EN		FIELD8(0x01)
+#define RFCSR1_PLL_PD			FIELD8(0x02)
 #define RFCSR1_RX0_PD			FIELD8(0x04)
 #define RFCSR1_TX0_PD			FIELD8(0x08)
 #define RFCSR1_RX1_PD			FIELD8(0x10)
 #define RFCSR1_TX1_PD			FIELD8(0x20)
 
 /*
+ * RFCSR 2:
+ */
+#define RFCSR2_RESCAL_EN		FIELD8(0x80)
+
+/*
  * RFCSR 6:
  */
 #define RFCSR6_R1			FIELD8(0x03)
@@ -1750,6 +1799,11 @@ struct mac_iveiv_entry {
 #define RFCSR7_RF_TUNING		FIELD8(0x01)
 
 /*
+ * RFCSR 11:
+ */
+#define RFCSR11_R			FIELD8(0x03)
+
+/*
  * RFCSR 12:
  */
 #define RFCSR12_TX_POWER		FIELD8(0x1f)
@@ -1770,6 +1824,7 @@ struct mac_iveiv_entry {
 #define RFCSR17_TXMIXER_GAIN		FIELD8(0x07)
 #define RFCSR17_TX_LO1_EN		FIELD8(0x08)
 #define RFCSR17_R			FIELD8(0x20)
+#define RFCSR17_CODE                   FIELD8(0x7f)
 
 /*
  * RFCSR 20:
@@ -1802,9 +1857,33 @@ struct mac_iveiv_entry {
 /*
  * RFCSR 30:
  */
+#define RFCSR30_TX_H20M			FIELD8(0x02)
+#define RFCSR30_RX_H20M			FIELD8(0x04)
+#define RFCSR30_RX_VCM			FIELD8(0x18)
 #define RFCSR30_RF_CALIBRATION		FIELD8(0x80)
 
 /*
+ * RFCSR 31:
+ */
+#define RFCSR31_RX_AGC_FC		FIELD8(0x1f)
+#define RFCSR31_RX_H20M			FIELD8(0x20)
+
+/*
+ * RFCSR 38:
+ */
+#define RFCSR38_RX_LO1_EN		FIELD8(0x20)
+
+/*
+ * RFCSR 39:
+ */
+#define RFCSR39_RX_LO2_EN		FIELD8(0x80)
+
+/*
+ * RFCSR 49:
+ */
+#define RFCSR49_TX			FIELD8(0x3f)
+
+/*
  * RF registers
  */
 
@@ -1837,6 +1916,11 @@ struct mac_iveiv_entry {
  */
 
 /*
+ * Chip ID
+ */
+#define EEPROM_CHIP_ID			0x0000
+
+/*
  * EEPROM Version
  */
 #define EEPROM_VERSION			0x0001
@@ -1989,23 +2073,26 @@ struct mac_iveiv_entry {
 #define EEPROM_RSSI_A2_LNA_A2		FIELD16(0xff00)
 
 /*
- * EEPROM Maximum TX power values
+ * EEPROM EIRP Maximum TX power values(unit: dbm)
  */
-#define EEPROM_MAX_TX_POWER		0x0027
-#define EEPROM_MAX_TX_POWER_24GHZ	FIELD16(0x00ff)
-#define EEPROM_MAX_TX_POWER_5GHZ	FIELD16(0xff00)
+#define EEPROM_EIRP_MAX_TX_POWER	0x0027
+#define EEPROM_EIRP_MAX_TX_POWER_2GHZ	FIELD16(0x00ff)
+#define EEPROM_EIRP_MAX_TX_POWER_5GHZ	FIELD16(0xff00)
 
 /*
  * EEPROM TXpower delta: 20MHZ AND 40 MHZ use different power.
  * This is delta in 40MHZ.
- * VALUE: Tx Power dalta value (MAX=4)
+ * VALUE: Tx Power dalta value, MAX=4(unit: dbm)
  * TYPE: 1: Plus the delta value, 0: minus the delta value
- * TXPOWER: Enable:
+ * ENABLE: enable tx power compensation for 40BW
  */
 #define EEPROM_TXPOWER_DELTA		0x0028
-#define EEPROM_TXPOWER_DELTA_VALUE	FIELD16(0x003f)
-#define EEPROM_TXPOWER_DELTA_TYPE	FIELD16(0x0040)
-#define EEPROM_TXPOWER_DELTA_TXPOWER	FIELD16(0x0080)
+#define EEPROM_TXPOWER_DELTA_VALUE_2G	FIELD16(0x003f)
+#define EEPROM_TXPOWER_DELTA_TYPE_2G	FIELD16(0x0040)
+#define EEPROM_TXPOWER_DELTA_ENABLE_2G	FIELD16(0x0080)
+#define EEPROM_TXPOWER_DELTA_VALUE_5G	FIELD16(0x3f00)
+#define EEPROM_TXPOWER_DELTA_TYPE_5G	FIELD16(0x4000)
+#define EEPROM_TXPOWER_DELTA_ENABLE_5G	FIELD16(0x8000)
 
 /*
  * EEPROM TXPOWER 802.11BG
@@ -2058,6 +2145,7 @@ struct mac_iveiv_entry {
 #define MCU_LED_LED_POLARITY		0x54
 #define MCU_RADAR			0x60
 #define MCU_BOOT_SIGNAL			0x72
+#define MCU_ANT_SELECT			0X73
 #define MCU_BBP_SIGNAL			0x80
 #define MCU_POWER_SAVE			0x83
 
@@ -2202,4 +2290,9 @@ struct mac_iveiv_entry {
 #define TXPOWER_A_TO_DEV(__txpower) \
 	clamp_t(char, __txpower, MIN_A_TXPOWER, MAX_A_TXPOWER)
 
+/*
+ *  Board's maximun TX power limitation
+ */
+#define EIRP_MAX_TX_POWER_LIMIT	0x50
+
 #endif /* RT2800_H */
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 54917a281398..2ee6cebb9b25 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -400,8 +400,15 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 	if (rt2800_wait_csr_ready(rt2x00dev))
 		return -EBUSY;
 
-	if (rt2x00_is_pci(rt2x00dev))
+	if (rt2x00_is_pci(rt2x00dev)) {
+		if (rt2x00_rt(rt2x00dev, RT5390)) {
+			rt2800_register_read(rt2x00dev, AUX_CTRL, &reg);
+			rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
+			rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
+			rt2800_register_write(rt2x00dev, AUX_CTRL, reg);
+		}
 		rt2800_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000002);
+	}
 
 	/*
 	 * Disable DMA, will be reenabled later when enabling
@@ -465,14 +472,15 @@ void rt2800_write_tx_data(struct queue_entry *entry,
 			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
 	rt2x00_set_field32(&word, TXWI_W0_AMPDU,
 			   test_bit(ENTRY_TXD_HT_AMPDU, &txdesc->flags));
-	rt2x00_set_field32(&word, TXWI_W0_MPDU_DENSITY, txdesc->mpdu_density);
-	rt2x00_set_field32(&word, TXWI_W0_TX_OP, txdesc->txop);
-	rt2x00_set_field32(&word, TXWI_W0_MCS, txdesc->mcs);
+	rt2x00_set_field32(&word, TXWI_W0_MPDU_DENSITY,
+			   txdesc->u.ht.mpdu_density);
+	rt2x00_set_field32(&word, TXWI_W0_TX_OP, txdesc->u.ht.txop);
+	rt2x00_set_field32(&word, TXWI_W0_MCS, txdesc->u.ht.mcs);
 	rt2x00_set_field32(&word, TXWI_W0_BW,
 			   test_bit(ENTRY_TXD_HT_BW_40, &txdesc->flags));
 	rt2x00_set_field32(&word, TXWI_W0_SHORT_GI,
 			   test_bit(ENTRY_TXD_HT_SHORT_GI, &txdesc->flags));
-	rt2x00_set_field32(&word, TXWI_W0_STBC, txdesc->stbc);
+	rt2x00_set_field32(&word, TXWI_W0_STBC, txdesc->u.ht.stbc);
 	rt2x00_set_field32(&word, TXWI_W0_PHYMODE, txdesc->rate_mode);
 	rt2x00_desc_write(txwi, 0, word);
 
@@ -481,7 +489,7 @@ void rt2800_write_tx_data(struct queue_entry *entry,
 			   test_bit(ENTRY_TXD_ACK, &txdesc->flags));
 	rt2x00_set_field32(&word, TXWI_W1_NSEQ,
 			   test_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags));
-	rt2x00_set_field32(&word, TXWI_W1_BW_WIN_SIZE, txdesc->ba_size);
+	rt2x00_set_field32(&word, TXWI_W1_BW_WIN_SIZE, txdesc->u.ht.ba_size);
 	rt2x00_set_field32(&word, TXWI_W1_WIRELESS_CLI_ID,
 			   test_bit(ENTRY_TXD_ENCRYPT, &txdesc->flags) ?
 			   txdesc->key_idx : 0xff);
@@ -674,7 +682,7 @@ void rt2800_txdone_entry(struct queue_entry *entry, u32 status)
 	 * confuse the rate control algortihm by providing clearly wrong
 	 * data.
 	 */
-	if (aggr == 1 && ampdu == 0 && real_mcs != mcs) {
+	if (unlikely(aggr == 1 && ampdu == 0 && real_mcs != mcs)) {
 		skbdesc->tx_rate_idx = real_mcs;
 		mcs = real_mcs;
 	}
@@ -744,7 +752,7 @@ void rt2800_txdone(struct rt2x00_dev *rt2x00dev)
 		if (pid >= QID_RX)
 			continue;
 
-		queue = rt2x00queue_get_queue(rt2x00dev, pid);
+		queue = rt2x00queue_get_tx_queue(rt2x00dev, pid);
 		if (unlikely(!queue))
 			continue;
 
@@ -773,13 +781,14 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	unsigned int beacon_base;
 	unsigned int padding_len;
-	u32 reg;
+	u32 orig_reg, reg;
 
 	/*
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
 	rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+	orig_reg = reg;
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
@@ -810,7 +819,14 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	 * Write entire beacon with TXWI and padding to register.
 	 */
 	padding_len = roundup(entry->skb->len, 4) - entry->skb->len;
-	skb_pad(entry->skb, padding_len);
+	if (padding_len && skb_pad(entry->skb, padding_len)) {
+		ERROR(rt2x00dev, "Failure padding beacon, aborting\n");
+		/* skb freed by skb_pad() on failure */
+		entry->skb = NULL;
+		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, orig_reg);
+		return;
+	}
+
 	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
 	rt2800_register_multiwrite(rt2x00dev, beacon_base, entry->skb->data,
 				   entry->skb->len + padding_len);
@@ -818,8 +834,6 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	/*
 	 * Enable beaconing again.
 	 */
-	rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
-	rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
 	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
@@ -831,8 +845,8 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 }
 EXPORT_SYMBOL_GPL(rt2800_write_beacon);
 
-static inline void rt2800_clear_beacon(struct rt2x00_dev *rt2x00dev,
-				       unsigned int beacon_base)
+static inline void rt2800_clear_beacon_register(struct rt2x00_dev *rt2x00dev,
+						unsigned int beacon_base)
 {
 	int i;
 
@@ -845,6 +859,33 @@ static inline void rt2800_clear_beacon(struct rt2x00_dev *rt2x00dev,
 		rt2800_register_write(rt2x00dev, beacon_base + i, 0);
 }
 
+void rt2800_clear_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	u32 reg;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
+	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+
+	/*
+	 * Clear beacon.
+	 */
+	rt2800_clear_beacon_register(rt2x00dev,
+				     HW_BEACON_OFFSET(entry->entry_idx));
+
+	/*
+	 * Enabled beaconing again.
+	 */
+	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
+	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+}
+EXPORT_SYMBOL_GPL(rt2800_clear_beacon);
+
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 const struct rt2x00debug rt2800_rt2x00debug = {
 	.owner	= THIS_MODULE,
@@ -1005,7 +1046,7 @@ static void rt2800_config_wcid_attr(struct rt2x00_dev *rt2x00dev,
 
 	memset(&wcid_entry, 0, sizeof(wcid_entry));
 	if (crypto->cmd == SET_KEY)
-		memcpy(&wcid_entry, crypto->address, ETH_ALEN);
+		memcpy(wcid_entry.mac, crypto->address, ETH_ALEN);
 	rt2800_register_multiwrite(rt2x00dev, offset,
 				      &wcid_entry, sizeof(wcid_entry));
 }
@@ -1060,27 +1101,44 @@ int rt2800_config_shared_key(struct rt2x00_dev *rt2x00dev,
 }
 EXPORT_SYMBOL_GPL(rt2800_config_shared_key);
 
+static inline int rt2800_find_pairwise_keyslot(struct rt2x00_dev *rt2x00dev)
+{
+	int idx;
+	u32 offset, reg;
+
+	/*
+	 * Search for the first free pairwise key entry and return the
+	 * corresponding index.
+	 *
+	 * Make sure the WCID starts _after_ the last possible shared key
+	 * entry (>32).
+	 *
+	 * Since parts of the pairwise key table might be shared with
+	 * the beacon frame buffers 6 & 7 we should only write into the
+	 * first 222 entries.
+	 */
+	for (idx = 33; idx <= 222; idx++) {
+		offset = MAC_WCID_ATTR_ENTRY(idx);
+		rt2800_register_read(rt2x00dev, offset, &reg);
+		if (!reg)
+			return idx;
+	}
+	return -1;
+}
+
 int rt2800_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 			       struct rt2x00lib_crypto *crypto,
 			       struct ieee80211_key_conf *key)
 {
 	struct hw_key_entry key_entry;
 	u32 offset;
+	int idx;
 
 	if (crypto->cmd == SET_KEY) {
-		/*
-		 * 1 pairwise key is possible per AID, this means that the AID
-		 * equals our hw_key_idx. Make sure the WCID starts _after_ the
-		 * last possible shared key entry.
-		 *
-		 * Since parts of the pairwise key table might be shared with
-		 * the beacon frame buffers 6 & 7 we should only write into the
-		 * first 222 entries.
-		 */
-		if (crypto->aid > (222 - 32))
+		idx = rt2800_find_pairwise_keyslot(rt2x00dev);
+		if (idx < 0)
 			return -ENOSPC;
-
-		key->hw_key_idx = 32 + crypto->aid;
+		key->hw_key_idx = idx;
 
 		memcpy(key_entry.key, crypto->key,
 		       sizeof(key_entry.key));
@@ -1155,29 +1213,11 @@ void rt2800_config_intf(struct rt2x00_dev *rt2x00dev, struct rt2x00_intf *intf,
 
 	if (flags & CONFIG_UPDATE_TYPE) {
 		/*
-		 * Clear current synchronisation setup.
-		 */
-		rt2800_clear_beacon(rt2x00dev,
-				    HW_BEACON_OFFSET(intf->beacon->entry_idx));
-		/*
 		 * Enable synchronisation.
 		 */
 		rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
-		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_SYNC, conf->sync);
-		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE,
-				   (conf->sync == TSF_SYNC_ADHOC ||
-				    conf->sync == TSF_SYNC_AP_NONE));
 		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
-
-		/*
-		 * Enable pre tbtt interrupt for beaconing modes
-		 */
-		rt2800_register_read(rt2x00dev, INT_TIMER_EN, &reg);
-		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER,
-				   (conf->sync == TSF_SYNC_AP_NONE));
-		rt2800_register_write(rt2x00dev, INT_TIMER_EN, reg);
-
 	}
 
 	if (flags & CONFIG_UPDATE_MAC) {
@@ -1361,10 +1401,32 @@ void rt2800_config_erp(struct rt2x00_dev *rt2x00dev, struct rt2x00lib_erp *erp,
 }
 EXPORT_SYMBOL_GPL(rt2800_config_erp);
 
+static void rt2800_set_ant_diversity(struct rt2x00_dev *rt2x00dev,
+				     enum antenna ant)
+{
+	u32 reg;
+	u8 eesk_pin = (ant == ANTENNA_A) ? 1 : 0;
+	u8 gpio_bit3 = (ant == ANTENNA_A) ? 0 : 1;
+
+	if (rt2x00_is_pci(rt2x00dev)) {
+		rt2800_register_read(rt2x00dev, E2PROM_CSR, &reg);
+		rt2x00_set_field32(&reg, E2PROM_CSR_DATA_CLOCK, eesk_pin);
+		rt2800_register_write(rt2x00dev, E2PROM_CSR, reg);
+	} else if (rt2x00_is_usb(rt2x00dev))
+		rt2800_mcu_request(rt2x00dev, MCU_ANT_SELECT, 0xff,
+				   eesk_pin, 0);
+
+	rt2800_register_read(rt2x00dev, GPIO_CTRL_CFG, &reg);
+	rt2x00_set_field32(&reg, GPIO_CTRL_CFG_GPIOD_BIT3, 0);
+	rt2x00_set_field32(&reg, GPIO_CTRL_CFG_BIT3, gpio_bit3);
+	rt2800_register_write(rt2x00dev, GPIO_CTRL_CFG, reg);
+}
+
 void rt2800_config_ant(struct rt2x00_dev *rt2x00dev, struct antenna_setup *ant)
 {
 	u8 r1;
 	u8 r3;
+	u16 eeprom;
 
 	rt2800_bbp_read(rt2x00dev, 1, &r1);
 	rt2800_bbp_read(rt2x00dev, 3, &r3);
@@ -1372,7 +1434,7 @@ void rt2800_config_ant(struct rt2x00_dev *rt2x00dev, struct antenna_setup *ant)
 	/*
 	 * Configure the TX antenna.
 	 */
-	switch ((int)ant->tx) {
+	switch (ant->tx_chain_num) {
 	case 1:
 		rt2x00_set_field8(&r1, BBP1_TX_ANTENNA, 0);
 		break;
@@ -1387,8 +1449,18 @@ void rt2800_config_ant(struct rt2x00_dev *rt2x00dev, struct antenna_setup *ant)
 	/*
 	 * Configure the RX antenna.
 	 */
-	switch ((int)ant->rx) {
+	switch (ant->rx_chain_num) {
 	case 1:
+		if (rt2x00_rt(rt2x00dev, RT3070) ||
+		    rt2x00_rt(rt2x00dev, RT3090) ||
+		    rt2x00_rt(rt2x00dev, RT3390)) {
+			rt2x00_eeprom_read(rt2x00dev,
+					   EEPROM_NIC_CONF1, &eeprom);
+			if (rt2x00_get_field16(eeprom,
+						EEPROM_NIC_CONF1_ANT_DIVERSITY))
+				rt2800_set_ant_diversity(rt2x00dev,
+						rt2x00dev->default_ant.rx);
+		}
 		rt2x00_set_field8(&r3, BBP3_RX_ANTENNA, 0);
 		break;
 	case 2:
@@ -1434,13 +1506,13 @@ static void rt2800_config_channel_rf2xxx(struct rt2x00_dev *rt2x00dev,
 {
 	rt2x00_set_field32(&rf->rf4, RF4_FREQ_OFFSET, rt2x00dev->freq_offset);
 
-	if (rt2x00dev->default_ant.tx == 1)
+	if (rt2x00dev->default_ant.tx_chain_num == 1)
 		rt2x00_set_field32(&rf->rf2, RF2_ANTENNA_TX1, 1);
 
-	if (rt2x00dev->default_ant.rx == 1) {
+	if (rt2x00dev->default_ant.rx_chain_num == 1) {
 		rt2x00_set_field32(&rf->rf2, RF2_ANTENNA_RX1, 1);
 		rt2x00_set_field32(&rf->rf2, RF2_ANTENNA_RX2, 1);
-	} else if (rt2x00dev->default_ant.rx == 2)
+	} else if (rt2x00dev->default_ant.rx_chain_num == 2)
 		rt2x00_set_field32(&rf->rf2, RF2_ANTENNA_RX2, 1);
 
 	if (rf->channel > 14) {
@@ -1526,6 +1598,105 @@ static void rt2800_config_channel_rf3xxx(struct rt2x00_dev *rt2x00dev,
 	rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 }
 
+
+#define RT5390_POWER_BOUND     0x27
+#define RT5390_FREQ_OFFSET_BOUND       0x5f
+
+static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
+					 struct ieee80211_conf *conf,
+					 struct rf_channel *rf,
+					 struct channel_info *info)
+{
+	u8 rfcsr;
+	u16 eeprom;
+
+	rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1);
+	rt2800_rfcsr_write(rt2x00dev, 9, rf->rf3);
+	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR11_R, rf->rf2);
+	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+	if (info->default_power1 > RT5390_POWER_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, RT5390_POWER_BOUND);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, info->default_power1);
+	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
+	rt2x00_set_field8(&rfcsr, RFCSR1_PLL_PD, 1);
+	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 1);
+	rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 1);
+	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+	if (rt2x00dev->freq_offset > RT5390_FREQ_OFFSET_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE,
+				  RT5390_FREQ_OFFSET_BOUND);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
+	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	if (rf->channel <= 14) {
+		int idx = rf->channel-1;
+
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_BT_COEXIST)) {
+			if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F)) {
+				/* r55/r59 value array of channel 1~14 */
+				static const char r55_bt_rev[] = {0x83, 0x83,
+					0x83, 0x73, 0x73, 0x63, 0x53, 0x53,
+					0x53, 0x43, 0x43, 0x43, 0x43, 0x43};
+				static const char r59_bt_rev[] = {0x0e, 0x0e,
+					0x0e, 0x0e, 0x0e, 0x0b, 0x0a, 0x09,
+					0x07, 0x07, 0x07, 0x07, 0x07, 0x07};
+
+				rt2800_rfcsr_write(rt2x00dev, 55,
+						   r55_bt_rev[idx]);
+				rt2800_rfcsr_write(rt2x00dev, 59,
+						   r59_bt_rev[idx]);
+			} else {
+				static const char r59_bt[] = {0x8b, 0x8b, 0x8b,
+					0x8b, 0x8b, 0x8b, 0x8b, 0x8a, 0x89,
+					0x88, 0x88, 0x86, 0x85, 0x84};
+
+				rt2800_rfcsr_write(rt2x00dev, 59, r59_bt[idx]);
+			}
+		} else {
+			if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F)) {
+				static const char r55_nonbt_rev[] = {0x23, 0x23,
+					0x23, 0x23, 0x13, 0x13, 0x03, 0x03,
+					0x03, 0x03, 0x03, 0x03, 0x03, 0x03};
+				static const char r59_nonbt_rev[] = {0x07, 0x07,
+					0x07, 0x07, 0x07, 0x07, 0x07, 0x07,
+					0x07, 0x07, 0x06, 0x05, 0x04, 0x04};
+
+				rt2800_rfcsr_write(rt2x00dev, 55,
+						   r55_nonbt_rev[idx]);
+				rt2800_rfcsr_write(rt2x00dev, 59,
+						   r59_nonbt_rev[idx]);
+			} else if (rt2x00_rt(rt2x00dev, RT5390)) {
+				static const char r59_non_bt[] = {0x8f, 0x8f,
+					0x8f, 0x8f, 0x8f, 0x8f, 0x8f, 0x8d,
+					0x8a, 0x88, 0x88, 0x87, 0x87, 0x86};
+
+				rt2800_rfcsr_write(rt2x00dev, 59,
+						   r59_non_bt[idx]);
+			}
+		}
+	}
+
+	rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR30_TX_H20M, 0);
+	rt2x00_set_field8(&rfcsr, RFCSR30_RX_H20M, 0);
+	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
+	rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
+}
+
 static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 				  struct ieee80211_conf *conf,
 				  struct rf_channel *rf,
@@ -1550,6 +1721,8 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	    rt2x00_rf(rt2x00dev, RF3052) ||
 	    rt2x00_rf(rt2x00dev, RF3320))
 		rt2800_config_channel_rf3xxx(rt2x00dev, conf, rf, info);
+	else if (rt2x00_rf(rt2x00dev, RF5390))
+		rt2800_config_channel_rf53xx(rt2x00dev, conf, rf, info);
 	else
 		rt2800_config_channel_rf2xxx(rt2x00dev, conf, rf, info);
 
@@ -1562,12 +1735,15 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	rt2800_bbp_write(rt2x00dev, 86, 0);
 
 	if (rf->channel <= 14) {
-		if (test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags)) {
-			rt2800_bbp_write(rt2x00dev, 82, 0x62);
-			rt2800_bbp_write(rt2x00dev, 75, 0x46);
-		} else {
-			rt2800_bbp_write(rt2x00dev, 82, 0x84);
-			rt2800_bbp_write(rt2x00dev, 75, 0x50);
+		if (!rt2x00_rt(rt2x00dev, RT5390)) {
+			if (test_bit(CONFIG_EXTERNAL_LNA_BG,
+				     &rt2x00dev->flags)) {
+				rt2800_bbp_write(rt2x00dev, 82, 0x62);
+				rt2800_bbp_write(rt2x00dev, 75, 0x46);
+			} else {
+				rt2800_bbp_write(rt2x00dev, 82, 0x84);
+				rt2800_bbp_write(rt2x00dev, 75, 0x50);
+			}
 		}
 	} else {
 		rt2800_bbp_write(rt2x00dev, 82, 0xf2);
@@ -1587,13 +1763,13 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	tx_pin = 0;
 
 	/* Turn on unused PA or LNA when not using 1T or 1R */
-	if (rt2x00dev->default_ant.tx != 1) {
+	if (rt2x00dev->default_ant.tx_chain_num == 2) {
 		rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_A1_EN, 1);
 		rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_G1_EN, 1);
 	}
 
 	/* Turn on unused PA or LNA when not using 1T or 1R */
-	if (rt2x00dev->default_ant.rx != 1) {
+	if (rt2x00dev->default_ant.rx_chain_num == 2) {
 		rt2x00_set_field32(&tx_pin, TX_PIN_CFG_LNA_PE_A1_EN, 1);
 		rt2x00_set_field32(&tx_pin, TX_PIN_CFG_LNA_PE_G1_EN, 1);
 	}
@@ -1637,30 +1813,116 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	rt2800_register_read(rt2x00dev, CH_BUSY_STA_SEC, &reg);
 }
 
+static int rt2800_get_txpower_bw_comp(struct rt2x00_dev *rt2x00dev,
+				      enum ieee80211_band band)
+{
+	u16 eeprom;
+	u8 comp_en;
+	u8 comp_type;
+	int comp_value;
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_DELTA, &eeprom);
+
+	if (eeprom == 0xffff)
+		return 0;
+
+	if (band == IEEE80211_BAND_2GHZ) {
+		comp_en = rt2x00_get_field16(eeprom,
+				 EEPROM_TXPOWER_DELTA_ENABLE_2G);
+		if (comp_en) {
+			comp_type = rt2x00_get_field16(eeprom,
+					   EEPROM_TXPOWER_DELTA_TYPE_2G);
+			comp_value = rt2x00_get_field16(eeprom,
+					    EEPROM_TXPOWER_DELTA_VALUE_2G);
+			if (!comp_type)
+				comp_value = -comp_value;
+		}
+	} else {
+		comp_en = rt2x00_get_field16(eeprom,
+				 EEPROM_TXPOWER_DELTA_ENABLE_5G);
+		if (comp_en) {
+			comp_type = rt2x00_get_field16(eeprom,
+					   EEPROM_TXPOWER_DELTA_TYPE_5G);
+			comp_value = rt2x00_get_field16(eeprom,
+					    EEPROM_TXPOWER_DELTA_VALUE_5G);
+			if (!comp_type)
+				comp_value = -comp_value;
+		}
+	}
+
+	return comp_value;
+}
+
+static u8 rt2800_compesate_txpower(struct rt2x00_dev *rt2x00dev,
+				     int is_rate_b,
+				     enum ieee80211_band band,
+				     int power_level,
+				     u8 txpower)
+{
+	u32 reg;
+	u16 eeprom;
+	u8 criterion;
+	u8 eirp_txpower;
+	u8 eirp_txpower_criterion;
+	u8 reg_limit;
+	int bw_comp = 0;
+
+	if (!((band == IEEE80211_BAND_5GHZ) && is_rate_b))
+		return txpower;
+
+	if (test_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags))
+		bw_comp = rt2800_get_txpower_bw_comp(rt2x00dev, band);
+
+	if (test_bit(CONFIG_SUPPORT_POWER_LIMIT, &rt2x00dev->flags)) {
+		/*
+		 * Check if eirp txpower exceed txpower_limit.
+		 * We use OFDM 6M as criterion and its eirp txpower
+		 * is stored at EEPROM_EIRP_MAX_TX_POWER.
+		 * .11b data rate need add additional 4dbm
+		 * when calculating eirp txpower.
+		 */
+		rt2800_register_read(rt2x00dev, TX_PWR_CFG_0, &reg);
+		criterion = rt2x00_get_field32(reg, TX_PWR_CFG_0_6MBS);
+
+		rt2x00_eeprom_read(rt2x00dev,
+				   EEPROM_EIRP_MAX_TX_POWER, &eeprom);
+
+		if (band == IEEE80211_BAND_2GHZ)
+			eirp_txpower_criterion = rt2x00_get_field16(eeprom,
+						 EEPROM_EIRP_MAX_TX_POWER_2GHZ);
+		else
+			eirp_txpower_criterion = rt2x00_get_field16(eeprom,
+						 EEPROM_EIRP_MAX_TX_POWER_5GHZ);
+
+		eirp_txpower = eirp_txpower_criterion + (txpower - criterion) +
+				       (is_rate_b ? 4 : 0) + bw_comp;
+
+		reg_limit = (eirp_txpower > power_level) ?
+					(eirp_txpower - power_level) : 0;
+	} else
+		reg_limit = 0;
+
+	return txpower + bw_comp - reg_limit;
+}
+
 static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
-				  const int max_txpower)
+				  struct ieee80211_conf *conf)
 {
 	u8 txpower;
-	u8 max_value = (u8)max_txpower;
 	u16 eeprom;
-	int i;
+	int i, is_rate_b;
 	u32 reg;
 	u8 r1;
 	u32 offset;
+	enum ieee80211_band band = conf->channel->band;
+	int power_level = conf->power_level;
 
 	/*
-	 * set to normal tx power mode: +/- 0dBm
+	 * set to normal bbp tx power control mode: +/- 0dBm
 	 */
 	rt2800_bbp_read(rt2x00dev, 1, &r1);
-	rt2x00_set_field8(&r1, BBP1_TX_POWER, 0);
+	rt2x00_set_field8(&r1, BBP1_TX_POWER_CTRL, 0);
 	rt2800_bbp_write(rt2x00dev, 1, r1);
-
-	/*
-	 * The eeprom contains the tx power values for each rate. These
-	 * values map to 100% tx power. Each 16bit word contains four tx
-	 * power values and the order is the same as used in the TX_PWR_CFG
-	 * registers.
-	 */
 	offset = TX_PWR_CFG_0;
 
 	for (i = 0; i < EEPROM_TXPOWER_BYRATE_SIZE; i += 2) {
@@ -1674,73 +1936,99 @@ static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_BYRATE + i,
 				   &eeprom);
 
-		/* TX_PWR_CFG_0: 1MBS, TX_PWR_CFG_1: 24MBS,
+		is_rate_b = i ? 0 : 1;
+		/*
+		 * TX_PWR_CFG_0: 1MBS, TX_PWR_CFG_1: 24MBS,
 		 * TX_PWR_CFG_2: MCS4, TX_PWR_CFG_3: MCS12,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE0);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE0,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE0, txpower);
 
-		/* TX_PWR_CFG_0: 2MBS, TX_PWR_CFG_1: 36MBS,
+		/*
+		 * TX_PWR_CFG_0: 2MBS, TX_PWR_CFG_1: 36MBS,
 		 * TX_PWR_CFG_2: MCS5, TX_PWR_CFG_3: MCS13,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE1);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE1,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE1, txpower);
 
-		/* TX_PWR_CFG_0: 55MBS, TX_PWR_CFG_1: 48MBS,
+		/*
+		 * TX_PWR_CFG_0: 5.5MBS, TX_PWR_CFG_1: 48MBS,
 		 * TX_PWR_CFG_2: MCS6,  TX_PWR_CFG_3: MCS14,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE2);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE2,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE2, txpower);
 
-		/* TX_PWR_CFG_0: 11MBS, TX_PWR_CFG_1: 54MBS,
+		/*
+		 * TX_PWR_CFG_0: 11MBS, TX_PWR_CFG_1: 54MBS,
 		 * TX_PWR_CFG_2: MCS7,  TX_PWR_CFG_3: MCS15,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE3);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE3,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE3, txpower);
 
 		/* read the next four txpower values */
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_BYRATE + i + 1,
 				   &eeprom);
 
-		/* TX_PWR_CFG_0: 6MBS, TX_PWR_CFG_1: MCS0,
+		is_rate_b = 0;
+		/*
+		 * TX_PWR_CFG_0: 6MBS, TX_PWR_CFG_1: MCS0,
 		 * TX_PWR_CFG_2: MCS8, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE0);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE4,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE4, txpower);
 
-		/* TX_PWR_CFG_0: 9MBS, TX_PWR_CFG_1: MCS1,
+		/*
+		 * TX_PWR_CFG_0: 9MBS, TX_PWR_CFG_1: MCS1,
 		 * TX_PWR_CFG_2: MCS9, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE1);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE5,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE5, txpower);
 
-		/* TX_PWR_CFG_0: 12MBS, TX_PWR_CFG_1: MCS2,
+		/*
+		 * TX_PWR_CFG_0: 12MBS, TX_PWR_CFG_1: MCS2,
 		 * TX_PWR_CFG_2: MCS10, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE2);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE6,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE6, txpower);
 
-		/* TX_PWR_CFG_0: 18MBS, TX_PWR_CFG_1: MCS3,
+		/*
+		 * TX_PWR_CFG_0: 18MBS, TX_PWR_CFG_1: MCS3,
 		 * TX_PWR_CFG_2: MCS11, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE3);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE7,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE7, txpower);
 
 		rt2800_register_write(rt2x00dev, offset, reg);
 
@@ -1799,11 +2087,13 @@ void rt2800_config(struct rt2x00_dev *rt2x00dev,
 	/* Always recalculate LNA gain before changing configuration */
 	rt2800_config_lna_gain(rt2x00dev, libconf);
 
-	if (flags & IEEE80211_CONF_CHANGE_CHANNEL)
+	if (flags & IEEE80211_CONF_CHANGE_CHANNEL) {
 		rt2800_config_channel(rt2x00dev, libconf->conf,
 				      &libconf->rf, &libconf->channel);
+		rt2800_config_txpower(rt2x00dev, libconf->conf);
+	}
 	if (flags & IEEE80211_CONF_CHANGE_POWER)
-		rt2800_config_txpower(rt2x00dev, libconf->conf->power_level);
+		rt2800_config_txpower(rt2x00dev, libconf->conf);
 	if (flags & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
 		rt2800_config_retry_limit(rt2x00dev, libconf);
 	if (flags & IEEE80211_CONF_CHANGE_PS)
@@ -1832,7 +2122,8 @@ static u8 rt2800_get_default_vgc(struct rt2x00_dev *rt2x00dev)
 		if (rt2x00_rt(rt2x00dev, RT3070) ||
 		    rt2x00_rt(rt2x00dev, RT3071) ||
 		    rt2x00_rt(rt2x00dev, RT3090) ||
-		    rt2x00_rt(rt2x00dev, RT3390))
+		    rt2x00_rt(rt2x00dev, RT3390) ||
+		    rt2x00_rt(rt2x00dev, RT5390))
 			return 0x1c + (2 * rt2x00dev->lna_gain);
 		else
 			return 0x2e + rt2x00dev->lna_gain;
@@ -1964,6 +2255,10 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000400);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00000000);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG2, 0x0000001f);
+	} else if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000404);
+		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00080606);
+		rt2800_register_write(rt2x00dev, TX_SW_CFG2, 0x00000000);
 	} else {
 		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000000);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00080606);
@@ -2032,7 +2327,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_read(rt2x00dev, CCK_PROT_CFG, &reg);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_RATE, 3);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_CTRL, 0);
-	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_NAV, 1);
+	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_NAV_SHORT, 1);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_TX_OP_ALLOW_CCK, 1);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_TX_OP_ALLOW_OFDM, 1);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_TX_OP_ALLOW_MM20, 1);
@@ -2045,7 +2340,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_read(rt2x00dev, OFDM_PROT_CFG, &reg);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_RATE, 3);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_CTRL, 0);
-	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_NAV, 1);
+	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_NAV_SHORT, 1);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_TX_OP_ALLOW_CCK, 1);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_TX_OP_ALLOW_OFDM, 1);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_TX_OP_ALLOW_MM20, 1);
@@ -2058,7 +2353,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_read(rt2x00dev, MM20_PROT_CFG, &reg);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_RATE, 0x4004);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_CTRL, 0);
-	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_NAV, 1);
+	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_NAV_SHORT, 1);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_TX_OP_ALLOW_CCK, 1);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_TX_OP_ALLOW_OFDM, 1);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_TX_OP_ALLOW_MM20, 1);
@@ -2071,7 +2366,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_read(rt2x00dev, MM40_PROT_CFG, &reg);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_RATE, 0x4084);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_CTRL, 0);
-	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_NAV, 1);
+	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_NAV_SHORT, 1);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_TX_OP_ALLOW_CCK, 1);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_TX_OP_ALLOW_OFDM, 1);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_TX_OP_ALLOW_MM20, 1);
@@ -2084,7 +2379,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_read(rt2x00dev, GF20_PROT_CFG, &reg);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_RATE, 0x4004);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_CTRL, 0);
-	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_NAV, 1);
+	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_NAV_SHORT, 1);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_TX_OP_ALLOW_CCK, 1);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_TX_OP_ALLOW_OFDM, 1);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_TX_OP_ALLOW_MM20, 1);
@@ -2097,7 +2392,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_read(rt2x00dev, GF40_PROT_CFG, &reg);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_RATE, 0x4084);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_CTRL, 0);
-	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_NAV, 1);
+	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_NAV_SHORT, 1);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_TX_OP_ALLOW_CCK, 1);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_TX_OP_ALLOW_OFDM, 1);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_TX_OP_ALLOW_MM20, 1);
@@ -2180,26 +2475,30 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		rt2800_register_multiwrite(rt2x00dev, MAC_WCID_ENTRY(i),
 					      wcid, sizeof(wcid));
 
-		rt2800_register_write(rt2x00dev, MAC_WCID_ATTR_ENTRY(i), 1);
+		rt2800_register_write(rt2x00dev, MAC_WCID_ATTR_ENTRY(i), 0);
 		rt2800_register_write(rt2x00dev, MAC_IVEIV_ENTRY(i), 0);
 	}
 
 	/*
 	 * Clear all beacons
 	 */
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE0);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE1);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE2);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE3);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE4);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE5);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE6);
-	rt2800_clear_beacon(rt2x00dev, HW_BEACON_BASE7);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE0);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE1);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE2);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE3);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE4);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE5);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE6);
+	rt2800_clear_beacon_register(rt2x00dev, HW_BEACON_BASE7);
 
 	if (rt2x00_is_usb(rt2x00dev)) {
 		rt2800_register_read(rt2x00dev, US_CYC_CNT, &reg);
 		rt2x00_set_field32(&reg, US_CYC_CNT_CLOCK_CYCLE, 30);
 		rt2800_register_write(rt2x00dev, US_CYC_CNT, reg);
+	} else if (rt2x00_is_pcie(rt2x00dev)) {
+		rt2800_register_read(rt2x00dev, US_CYC_CNT, &reg);
+		rt2x00_set_field32(&reg, US_CYC_CNT_CLOCK_CYCLE, 125);
+		rt2800_register_write(rt2x00dev, US_CYC_CNT, reg);
 	}
 
 	rt2800_register_read(rt2x00dev, HT_FBK_CFG0, &reg);
@@ -2335,15 +2634,31 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 		     rt2800_wait_bbp_ready(rt2x00dev)))
 		return -EACCES;
 
-	if (rt2800_is_305x_soc(rt2x00dev))
+	if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_bbp_read(rt2x00dev, 4, &value);
+		rt2x00_set_field8(&value, BBP4_MAC_IF_CTRL, 1);
+		rt2800_bbp_write(rt2x00dev, 4, value);
+	}
+
+	if (rt2800_is_305x_soc(rt2x00dev) ||
+	    rt2x00_rt(rt2x00dev, RT5390))
 		rt2800_bbp_write(rt2x00dev, 31, 0x08);
 
 	rt2800_bbp_write(rt2x00dev, 65, 0x2c);
 	rt2800_bbp_write(rt2x00dev, 66, 0x38);
 
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 68, 0x0b);
+
 	if (rt2x00_rt_rev(rt2x00dev, RT2860, REV_RT2860C)) {
 		rt2800_bbp_write(rt2x00dev, 69, 0x16);
 		rt2800_bbp_write(rt2x00dev, 73, 0x12);
+	} else if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_bbp_write(rt2x00dev, 69, 0x12);
+		rt2800_bbp_write(rt2x00dev, 73, 0x13);
+		rt2800_bbp_write(rt2x00dev, 75, 0x46);
+		rt2800_bbp_write(rt2x00dev, 76, 0x28);
+		rt2800_bbp_write(rt2x00dev, 77, 0x59);
 	} else {
 		rt2800_bbp_write(rt2x00dev, 69, 0x12);
 		rt2800_bbp_write(rt2x00dev, 73, 0x10);
@@ -2354,7 +2669,8 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	if (rt2x00_rt(rt2x00dev, RT3070) ||
 	    rt2x00_rt(rt2x00dev, RT3071) ||
 	    rt2x00_rt(rt2x00dev, RT3090) ||
-	    rt2x00_rt(rt2x00dev, RT3390)) {
+	    rt2x00_rt(rt2x00dev, RT3390) ||
+	    rt2x00_rt(rt2x00dev, RT5390)) {
 		rt2800_bbp_write(rt2x00dev, 79, 0x13);
 		rt2800_bbp_write(rt2x00dev, 80, 0x05);
 		rt2800_bbp_write(rt2x00dev, 81, 0x33);
@@ -2366,35 +2682,62 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	}
 
 	rt2800_bbp_write(rt2x00dev, 82, 0x62);
-	rt2800_bbp_write(rt2x00dev, 83, 0x6a);
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 83, 0x7a);
+	else
+		rt2800_bbp_write(rt2x00dev, 83, 0x6a);
 
 	if (rt2x00_rt_rev(rt2x00dev, RT2860, REV_RT2860D))
 		rt2800_bbp_write(rt2x00dev, 84, 0x19);
+	else if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 84, 0x9a);
 	else
 		rt2800_bbp_write(rt2x00dev, 84, 0x99);
 
-	rt2800_bbp_write(rt2x00dev, 86, 0x00);
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 86, 0x38);
+	else
+		rt2800_bbp_write(rt2x00dev, 86, 0x00);
+
 	rt2800_bbp_write(rt2x00dev, 91, 0x04);
-	rt2800_bbp_write(rt2x00dev, 92, 0x00);
+
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 92, 0x02);
+	else
+		rt2800_bbp_write(rt2x00dev, 92, 0x00);
 
 	if (rt2x00_rt_rev_gte(rt2x00dev, RT3070, REV_RT3070F) ||
 	    rt2x00_rt_rev_gte(rt2x00dev, RT3071, REV_RT3071E) ||
 	    rt2x00_rt_rev_gte(rt2x00dev, RT3090, REV_RT3090E) ||
 	    rt2x00_rt_rev_gte(rt2x00dev, RT3390, REV_RT3390E) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
 	    rt2800_is_305x_soc(rt2x00dev))
 		rt2800_bbp_write(rt2x00dev, 103, 0xc0);
 	else
 		rt2800_bbp_write(rt2x00dev, 103, 0x00);
 
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 104, 0x92);
+
 	if (rt2800_is_305x_soc(rt2x00dev))
 		rt2800_bbp_write(rt2x00dev, 105, 0x01);
+	else if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 105, 0x3c);
 	else
 		rt2800_bbp_write(rt2x00dev, 105, 0x05);
-	rt2800_bbp_write(rt2x00dev, 106, 0x35);
+
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 106, 0x03);
+	else
+		rt2800_bbp_write(rt2x00dev, 106, 0x35);
+
+	if (rt2x00_rt(rt2x00dev, RT5390))
+		rt2800_bbp_write(rt2x00dev, 128, 0x12);
 
 	if (rt2x00_rt(rt2x00dev, RT3071) ||
 	    rt2x00_rt(rt2x00dev, RT3090) ||
-	    rt2x00_rt(rt2x00dev, RT3390)) {
+	    rt2x00_rt(rt2x00dev, RT3390) ||
+	    rt2x00_rt(rt2x00dev, RT5390)) {
 		rt2800_bbp_read(rt2x00dev, 138, &value);
 
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
@@ -2406,6 +2749,42 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 		rt2800_bbp_write(rt2x00dev, 138, value);
 	}
 
+	if (rt2x00_rt(rt2x00dev, RT5390)) {
+		int ant, div_mode;
+
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+		div_mode = rt2x00_get_field16(eeprom,
+					      EEPROM_NIC_CONF1_ANT_DIVERSITY);
+		ant = (div_mode == 3) ? 1 : 0;
+
+		/* check if this is a Bluetooth combo card */
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_BT_COEXIST)) {
+			u32 reg;
+
+			rt2800_register_read(rt2x00dev, GPIO_CTRL_CFG, &reg);
+			rt2x00_set_field32(&reg, GPIO_CTRL_CFG_GPIOD_BIT3, 0);
+			rt2x00_set_field32(&reg, GPIO_CTRL_CFG_GPIOD_BIT6, 0);
+			rt2x00_set_field32(&reg, GPIO_CTRL_CFG_BIT3, 0);
+			rt2x00_set_field32(&reg, GPIO_CTRL_CFG_BIT6, 0);
+			if (ant == 0)
+				rt2x00_set_field32(&reg, GPIO_CTRL_CFG_BIT3, 1);
+			else if (ant == 1)
+				rt2x00_set_field32(&reg, GPIO_CTRL_CFG_BIT6, 1);
+			rt2800_register_write(rt2x00dev, GPIO_CTRL_CFG, reg);
+		}
+
+		rt2800_bbp_read(rt2x00dev, 152, &value);
+		if (ant == 0)
+			rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 1);
+		else
+			rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 0);
+		rt2800_bbp_write(rt2x00dev, 152, value);
+
+		/* Init frequency calibration */
+		rt2800_bbp_write(rt2x00dev, 142, 1);
+		rt2800_bbp_write(rt2x00dev, 143, 57);
+	}
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
@@ -2436,6 +2815,10 @@ static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 2 * bw40);
 	rt2800_bbp_write(rt2x00dev, 4, bbp);
 
+	rt2800_rfcsr_read(rt2x00dev, 31, &rfcsr);
+	rt2x00_set_field8(&rfcsr, RFCSR31_RX_H20M, bw40);
+	rt2800_rfcsr_write(rt2x00dev, 31, rfcsr);
+
 	rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
 	rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 1);
 	rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
@@ -2491,18 +2874,28 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rt(rt2x00dev, RT3071) &&
 	    !rt2x00_rt(rt2x00dev, RT3090) &&
 	    !rt2x00_rt(rt2x00dev, RT3390) &&
+	    !rt2x00_rt(rt2x00dev, RT5390) &&
 	    !rt2800_is_305x_soc(rt2x00dev))
 		return 0;
 
 	/*
 	 * Init RF calibration.
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
-	rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
-	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
-	msleep(1);
-	rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 0);
-	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
+	if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_rfcsr_read(rt2x00dev, 2, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 1);
+		rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
+		msleep(1);
+		rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
+	} else {
+		rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
+		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
+		msleep(1);
+		rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 0);
+		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
+	}
 
 	if (rt2x00_rt(rt2x00dev, RT3070) ||
 	    rt2x00_rt(rt2x00dev, RT3071) ||
@@ -2510,7 +2903,7 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 4, 0x40);
 		rt2800_rfcsr_write(rt2x00dev, 5, 0x03);
 		rt2800_rfcsr_write(rt2x00dev, 6, 0x02);
-		rt2800_rfcsr_write(rt2x00dev, 7, 0x70);
+		rt2800_rfcsr_write(rt2x00dev, 7, 0x60);
 		rt2800_rfcsr_write(rt2x00dev, 9, 0x0f);
 		rt2800_rfcsr_write(rt2x00dev, 10, 0x41);
 		rt2800_rfcsr_write(rt2x00dev, 11, 0x21);
@@ -2593,6 +2986,87 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 30, 0x00);
 		rt2800_rfcsr_write(rt2x00dev, 31, 0x00);
 		return 0;
+	} else if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_rfcsr_write(rt2x00dev, 1, 0x0f);
+		rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 3, 0x88);
+		rt2800_rfcsr_write(rt2x00dev, 5, 0x10);
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F))
+			rt2800_rfcsr_write(rt2x00dev, 6, 0xe0);
+		else
+			rt2800_rfcsr_write(rt2x00dev, 6, 0xa0);
+		rt2800_rfcsr_write(rt2x00dev, 7, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 10, 0x53);
+		rt2800_rfcsr_write(rt2x00dev, 11, 0x4a);
+		rt2800_rfcsr_write(rt2x00dev, 12, 0xc6);
+		rt2800_rfcsr_write(rt2x00dev, 13, 0x9f);
+		rt2800_rfcsr_write(rt2x00dev, 14, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 15, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 16, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 18, 0x03);
+		rt2800_rfcsr_write(rt2x00dev, 19, 0x00);
+
+		rt2800_rfcsr_write(rt2x00dev, 20, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 21, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 22, 0x20);
+		rt2800_rfcsr_write(rt2x00dev, 23, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 24, 0x00);
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F))
+			rt2800_rfcsr_write(rt2x00dev, 25, 0x80);
+		else
+			rt2800_rfcsr_write(rt2x00dev, 25, 0xc0);
+		rt2800_rfcsr_write(rt2x00dev, 26, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x09);
+		rt2800_rfcsr_write(rt2x00dev, 28, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 29, 0x10);
+
+		rt2800_rfcsr_write(rt2x00dev, 30, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 31, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 32, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 33, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 34, 0x07);
+		rt2800_rfcsr_write(rt2x00dev, 35, 0x12);
+		rt2800_rfcsr_write(rt2x00dev, 36, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 37, 0x08);
+		rt2800_rfcsr_write(rt2x00dev, 38, 0x85);
+		rt2800_rfcsr_write(rt2x00dev, 39, 0x1b);
+
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F))
+			rt2800_rfcsr_write(rt2x00dev, 40, 0x0b);
+		else
+			rt2800_rfcsr_write(rt2x00dev, 40, 0x4b);
+		rt2800_rfcsr_write(rt2x00dev, 41, 0xbb);
+		rt2800_rfcsr_write(rt2x00dev, 42, 0xd2);
+		rt2800_rfcsr_write(rt2x00dev, 43, 0x9a);
+		rt2800_rfcsr_write(rt2x00dev, 44, 0x0e);
+		rt2800_rfcsr_write(rt2x00dev, 45, 0xa2);
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F))
+			rt2800_rfcsr_write(rt2x00dev, 46, 0x73);
+		else
+			rt2800_rfcsr_write(rt2x00dev, 46, 0x7b);
+		rt2800_rfcsr_write(rt2x00dev, 47, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 48, 0x10);
+		rt2800_rfcsr_write(rt2x00dev, 49, 0x94);
+
+		rt2800_rfcsr_write(rt2x00dev, 52, 0x38);
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F))
+			rt2800_rfcsr_write(rt2x00dev, 53, 0x00);
+		else
+			rt2800_rfcsr_write(rt2x00dev, 53, 0x84);
+		rt2800_rfcsr_write(rt2x00dev, 54, 0x78);
+		rt2800_rfcsr_write(rt2x00dev, 55, 0x44);
+		rt2800_rfcsr_write(rt2x00dev, 56, 0x22);
+		rt2800_rfcsr_write(rt2x00dev, 57, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 58, 0x7f);
+		rt2800_rfcsr_write(rt2x00dev, 59, 0x63);
+
+		rt2800_rfcsr_write(rt2x00dev, 60, 0x45);
+		if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390F))
+			rt2800_rfcsr_write(rt2x00dev, 61, 0xd1);
+		else
+			rt2800_rfcsr_write(rt2x00dev, 61, 0xdd);
+		rt2800_rfcsr_write(rt2x00dev, 62, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 63, 0x00);
 	}
 
 	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F)) {
@@ -2602,12 +3076,12 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
 	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
 		   rt2x00_rt(rt2x00dev, RT3090)) {
+		rt2800_rfcsr_write(rt2x00dev, 31, 0x14);
+
 		rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
 		rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
 		rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
 
-		rt2800_rfcsr_write(rt2x00dev, 31, 0x14);
-
 		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
 		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
 		if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
@@ -2619,6 +3093,10 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 				rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
 		}
 		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
+
+		rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+		rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
+		rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
 	} else if (rt2x00_rt(rt2x00dev, RT3390)) {
 		rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
 		rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
@@ -2642,21 +3120,23 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 			rt2800_init_rx_filter(rt2x00dev, true, 0x27, 0x15);
 	}
 
-	/*
-	 * Set back to initial state
-	 */
-	rt2800_bbp_write(rt2x00dev, 24, 0);
+	if (!rt2x00_rt(rt2x00dev, RT5390)) {
+		/*
+		 * Set back to initial state
+		 */
+		rt2800_bbp_write(rt2x00dev, 24, 0);
 
-	rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
-	rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 0);
-	rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
+		rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 0);
+		rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
 
-	/*
-	 * set BBP back to BW20
-	 */
-	rt2800_bbp_read(rt2x00dev, 4, &bbp);
-	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 0);
-	rt2800_bbp_write(rt2x00dev, 4, bbp);
+		/*
+		 * Set BBP back to BW20
+		 */
+		rt2800_bbp_read(rt2x00dev, 4, &bbp);
+		rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 0);
+		rt2800_bbp_write(rt2x00dev, 4, bbp);
+	}
 
 	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F) ||
 	    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
@@ -2668,24 +3148,29 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, OPT_14_CSR_BIT0, 1);
 	rt2800_register_write(rt2x00dev, OPT_14_CSR, reg);
 
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
-	rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
-	if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
-	    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
-	    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
-		if (test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags))
-			rt2x00_set_field8(&rfcsr, RFCSR17_R, 1);
-	}
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_TXMIXER_GAIN_BG, &eeprom);
-	if (rt2x00_get_field16(eeprom, EEPROM_TXMIXER_GAIN_BG_VAL) >= 1)
-		rt2x00_set_field8(&rfcsr, RFCSR17_TXMIXER_GAIN,
-				  rt2x00_get_field16(eeprom,
-						   EEPROM_TXMIXER_GAIN_BG_VAL));
-	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+	if (!rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
+		if (rt2x00_rt(rt2x00dev, RT3070) ||
+		    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
+		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
+		    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
+			if (!test_bit(CONFIG_EXTERNAL_LNA_BG,
+				      &rt2x00dev->flags))
+				rt2x00_set_field8(&rfcsr, RFCSR17_R, 1);
+		}
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_TXMIXER_GAIN_BG, &eeprom);
+		if (rt2x00_get_field16(eeprom, EEPROM_TXMIXER_GAIN_BG_VAL) >= 1)
+			rt2x00_set_field8(&rfcsr, RFCSR17_TXMIXER_GAIN,
+					rt2x00_get_field16(eeprom,
+						EEPROM_TXMIXER_GAIN_BG_VAL));
+		rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+	}
 
 	if (rt2x00_rt(rt2x00dev, RT3090)) {
 		rt2800_bbp_read(rt2x00dev, 138, &bbp);
 
+		/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
 		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
 			rt2x00_set_field8(&bbp, BBP138_RX_ADC1, 0);
@@ -2719,10 +3204,9 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
 	}
 
-	if (rt2x00_rt(rt2x00dev, RT3070) || rt2x00_rt(rt2x00dev, RT3071)) {
+	if (rt2x00_rt(rt2x00dev, RT3070)) {
 		rt2800_rfcsr_read(rt2x00dev, 27, &rfcsr);
-		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E))
+		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F))
 			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 3);
 		else
 			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 0);
@@ -2732,6 +3216,20 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
 	}
 
+	if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_rfcsr_read(rt2x00dev, 38, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR38_RX_LO1_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 38, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 39, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR39_RX_LO2_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 39, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR30_RX_VCM, 2);
+		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
+	}
+
 	return 0;
 }
 
@@ -2810,10 +3308,7 @@ void rt2800_disable_radio(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_DMA_BUSY, 0);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_RX_DMA_BUSY, 0);
-	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_WRITEBACK_DONE, 1);
 	rt2800_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
 
 	/* Wait for DMA, ignore error */
@@ -2823,9 +3318,6 @@ void rt2800_disable_radio(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_TX, 0);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
-
-	rt2800_register_write(rt2x00dev, PWR_PIN_CFG, 0);
-	rt2800_register_write(rt2x00dev, TX_PIN_CFG, 0);
 }
 EXPORT_SYMBOL_GPL(rt2800_disable_radio);
 
@@ -2986,13 +3478,6 @@ int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 				   default_lna_gain);
 	rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_A2, word);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_MAX_TX_POWER, &word);
-	if (rt2x00_get_field16(word, EEPROM_MAX_TX_POWER_24GHZ) == 0xff)
-		rt2x00_set_field16(&word, EEPROM_MAX_TX_POWER_24GHZ, MAX_G_TXPOWER);
-	if (rt2x00_get_field16(word, EEPROM_MAX_TX_POWER_5GHZ) == 0xff)
-		rt2x00_set_field16(&word, EEPROM_MAX_TX_POWER_5GHZ, MAX_A_TXPOWER);
-	rt2x00_eeprom_write(rt2x00dev, EEPROM_MAX_TX_POWER, word);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rt2800_validate_eeprom);
@@ -3009,10 +3494,15 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
 
 	/*
-	 * Identify RF chipset.
+	 * Identify RF chipset by EEPROM value
+	 * RT28xx/RT30xx: defined in "EEPROM_NIC_CONF0_RF_TYPE" field
+	 * RT53xx: defined in "EEPROM_CHIP_ID" field
 	 */
-	value = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RF_TYPE);
 	rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
+	if (rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5390)
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_CHIP_ID, &value);
+	else
+		value = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RF_TYPE);
 
 	rt2x00_set_chip(rt2x00dev, rt2x00_get_field32(reg, MAC_CSR0_CHIPSET),
 			value, rt2x00_get_field32(reg, MAC_CSR0_REVISION));
@@ -3024,7 +3514,8 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rt(rt2x00dev, RT3071) &&
 	    !rt2x00_rt(rt2x00dev, RT3090) &&
 	    !rt2x00_rt(rt2x00dev, RT3390) &&
-	    !rt2x00_rt(rt2x00dev, RT3572)) {
+	    !rt2x00_rt(rt2x00dev, RT3572) &&
+	    !rt2x00_rt(rt2x00dev, RT5390)) {
 		ERROR(rt2x00dev, "Invalid RT chipset detected.\n");
 		return -ENODEV;
 	}
@@ -3038,7 +3529,8 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    !rt2x00_rf(rt2x00dev, RF3021) &&
 	    !rt2x00_rf(rt2x00dev, RF3022) &&
 	    !rt2x00_rf(rt2x00dev, RF3052) &&
-	    !rt2x00_rf(rt2x00dev, RF3320)) {
+	    !rt2x00_rf(rt2x00dev, RF3320) &&
+	    !rt2x00_rf(rt2x00dev, RF5390)) {
 		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
 		return -ENODEV;
 	}
@@ -3046,11 +3538,35 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Identify default antenna configuration.
 	 */
-	rt2x00dev->default_ant.tx =
+	rt2x00dev->default_ant.tx_chain_num =
 	    rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH);
-	rt2x00dev->default_ant.rx =
+	rt2x00dev->default_ant.rx_chain_num =
 	    rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH);
 
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+
+	if (rt2x00_rt(rt2x00dev, RT3070) ||
+	    rt2x00_rt(rt2x00dev, RT3090) ||
+	    rt2x00_rt(rt2x00dev, RT3390)) {
+		value = rt2x00_get_field16(eeprom,
+				EEPROM_NIC_CONF1_ANT_DIVERSITY);
+		switch (value) {
+		case 0:
+		case 1:
+		case 2:
+			rt2x00dev->default_ant.tx = ANTENNA_A;
+			rt2x00dev->default_ant.rx = ANTENNA_A;
+			break;
+		case 3:
+			rt2x00dev->default_ant.tx = ANTENNA_A;
+			rt2x00dev->default_ant.rx = ANTENNA_B;
+			break;
+		}
+	} else {
+		rt2x00dev->default_ant.tx = ANTENNA_A;
+		rt2x00dev->default_ant.rx = ANTENNA_A;
+	}
+
 	/*
 	 * Read frequency offset and RF programming sequence.
 	 */
@@ -3084,6 +3600,15 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &rt2x00dev->led_mcu_reg);
 #endif /* CONFIG_RT2X00_LIB_LEDS */
 
+	/*
+	 * Check if support EIRP tx power limit feature.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER, &eeprom);
+
+	if (rt2x00_get_field16(eeprom, EEPROM_EIRP_MAX_TX_POWER_2GHZ) <
+					EIRP_MAX_TX_POWER_LIMIT)
+		__set_bit(CONFIG_SUPPORT_POWER_LIMIT, &rt2x00dev->flags);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rt2800_init_eeprom);
@@ -3236,7 +3761,6 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	char *default_power1;
 	char *default_power2;
 	unsigned int i;
-	unsigned short max_power;
 	u16 eeprom;
 
 	/*
@@ -3303,7 +3827,8 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 		   rt2x00_rf(rt2x00dev, RF2020) ||
 		   rt2x00_rf(rt2x00dev, RF3021) ||
 		   rt2x00_rf(rt2x00dev, RF3022) ||
-		   rt2x00_rf(rt2x00dev, RF3320)) {
+		   rt2x00_rf(rt2x00dev, RF3320) ||
+		   rt2x00_rf(rt2x00dev, RF5390)) {
 		spec->num_channels = 14;
 		spec->channels = rf_vals_3x;
 	} else if (rt2x00_rf(rt2x00dev, RF3052)) {
@@ -3361,26 +3886,21 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 
 	spec->channels_info = info;
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_MAX_TX_POWER, &eeprom);
-	max_power = rt2x00_get_field16(eeprom, EEPROM_MAX_TX_POWER_24GHZ);
 	default_power1 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_BG1);
 	default_power2 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_BG2);
 
 	for (i = 0; i < 14; i++) {
-		info[i].max_power = max_power;
-		info[i].default_power1 = TXPOWER_G_FROM_DEV(default_power1[i]);
-		info[i].default_power2 = TXPOWER_G_FROM_DEV(default_power2[i]);
+		info[i].default_power1 = default_power1[i];
+		info[i].default_power2 = default_power2[i];
 	}
 
 	if (spec->num_channels > 14) {
-		max_power = rt2x00_get_field16(eeprom, EEPROM_MAX_TX_POWER_5GHZ);
 		default_power1 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_A1);
 		default_power2 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_A2);
 
 		for (i = 14; i < spec->num_channels; i++) {
-			info[i].max_power = max_power;
-			info[i].default_power1 = TXPOWER_A_FROM_DEV(default_power1[i]);
-			info[i].default_power2 = TXPOWER_A_FROM_DEV(default_power2[i]);
+			info[i].default_power1 = default_power1[i];
+			info[i].default_power2 = default_power2[i];
 		}
 	}
 
@@ -3472,7 +3992,7 @@ int rt2800_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
 	if (queue_idx >= 4)
 		return 0;
 
-	queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+	queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
 
 	/* Update WMM TXOP register */
 	offset = WMM_TXOP0_CFG + (sizeof(u32) * (!!(queue_idx & 2)));
@@ -3530,7 +4050,8 @@ EXPORT_SYMBOL_GPL(rt2800_get_tsf);
 
 int rt2800_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			enum ieee80211_ampdu_mlme_action action,
-			struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			u8 buf_size)
 {
 	int ret = 0;
 
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.h b/drivers/net/wireless/rt2x00/rt2800lib.h
index e3c995a9dec4..0c92d86a36f4 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.h
+++ b/drivers/net/wireless/rt2x00/rt2800lib.h
@@ -156,6 +156,7 @@ void rt2800_txdone(struct rt2x00_dev *rt2x00dev);
 void rt2800_txdone_entry(struct queue_entry *entry, u32 status);
 
 void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc);
+void rt2800_clear_beacon(struct queue_entry *entry);
 
 extern const struct rt2x00debug rt2800_rt2x00debug;
 
@@ -198,7 +199,8 @@ int rt2800_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
 u64 rt2800_get_tsf(struct ieee80211_hw *hw);
 int rt2800_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			enum ieee80211_ampdu_mlme_action action,
-			struct ieee80211_sta *sta, u16 tid, u16 *ssn);
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			u8 buf_size);
 int rt2800_get_survey(struct ieee80211_hw *hw, int idx,
 		      struct survey_info *survey);
 
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index aa97971a38af..808073aa9dcc 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -200,11 +200,22 @@ static void rt2800pci_start_queue(struct data_queue *queue)
 		rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
+		/*
+		 * Allow beacon tasklets to be scheduled for periodic
+		 * beacon updates.
+		 */
+		tasklet_enable(&rt2x00dev->tbtt_tasklet);
+		tasklet_enable(&rt2x00dev->pretbtt_tasklet);
+
 		rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
 		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+
+		rt2800_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 1);
+		rt2800_register_write(rt2x00dev, INT_TIMER_EN, reg);
 		break;
 	default:
 		break;
@@ -250,6 +261,16 @@ static void rt2800pci_stop_queue(struct data_queue *queue)
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+
+		rt2800_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 0);
+		rt2800_register_write(rt2x00dev, INT_TIMER_EN, reg);
+
+		/*
+		 * Wait for tbtt tasklets to finish.
+		 */
+		tasklet_disable(&rt2x00dev->tbtt_tasklet);
+		tasklet_disable(&rt2x00dev->pretbtt_tasklet);
 		break;
 	default:
 		break;
@@ -397,9 +418,9 @@ static int rt2800pci_init_queues(struct rt2x00_dev *rt2x00dev)
 static void rt2800pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 				 enum dev_state state)
 {
-	int mask = (state == STATE_RADIO_IRQ_ON) ||
-		   (state == STATE_RADIO_IRQ_ON_ISR);
+	int mask = (state == STATE_RADIO_IRQ_ON);
 	u32 reg;
+	unsigned long flags;
 
 	/*
 	 * When interrupts are being enabled, the interrupt registers
@@ -408,8 +429,17 @@ static void rt2800pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	if (state == STATE_RADIO_IRQ_ON) {
 		rt2800_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
 		rt2800_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+
+		/*
+		 * Enable tasklets. The beacon related tasklets are
+		 * enabled when the beacon queue is started.
+		 */
+		tasklet_enable(&rt2x00dev->txstatus_tasklet);
+		tasklet_enable(&rt2x00dev->rxdone_tasklet);
+		tasklet_enable(&rt2x00dev->autowake_tasklet);
 	}
 
+	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 	rt2800_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_RXDELAYINT, 0);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_TXDELAYINT, 0);
@@ -430,6 +460,17 @@ static void rt2800pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, INT_MASK_CSR_RX_COHERENT, 0);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_TX_COHERENT, 0);
 	rt2800_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
+
+	if (state == STATE_RADIO_IRQ_OFF) {
+		/*
+		 * Ensure that all tasklets are finished before
+		 * disabling the interrupts.
+		 */
+		tasklet_disable(&rt2x00dev->txstatus_tasklet);
+		tasklet_disable(&rt2x00dev->rxdone_tasklet);
+		tasklet_disable(&rt2x00dev->autowake_tasklet);
+	}
 }
 
 static int rt2800pci_init_registers(struct rt2x00_dev *rt2x00dev)
@@ -452,6 +493,13 @@ static int rt2800pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e1f);
 	rt2800_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e00);
 
+	if (rt2x00_rt(rt2x00dev, RT5390)) {
+		rt2800_register_read(rt2x00dev, AUX_CTRL, &reg);
+		rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
+		rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
+		rt2800_register_write(rt2x00dev, AUX_CTRL, reg);
+	}
+
 	rt2800_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000003);
 
 	rt2800_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
@@ -475,39 +523,23 @@ static int rt2800pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 
 static void rt2800pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
-	u32 reg;
-
-	rt2800_disable_radio(rt2x00dev);
-
-	rt2800_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00001280);
-
-	rt2800_register_read(rt2x00dev, WPDMA_RST_IDX, &reg);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX0, 1);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX1, 1);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX2, 1);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX3, 1);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX4, 1);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX5, 1);
-	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DRX_IDX0, 1);
-	rt2800_register_write(rt2x00dev, WPDMA_RST_IDX, reg);
-
-	rt2800_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e1f);
-	rt2800_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e00);
+	if (rt2x00_is_soc(rt2x00dev)) {
+		rt2800_disable_radio(rt2x00dev);
+		rt2800_register_write(rt2x00dev, PWR_PIN_CFG, 0);
+		rt2800_register_write(rt2x00dev, TX_PIN_CFG, 0);
+	}
 }
 
 static int rt2800pci_set_state(struct rt2x00_dev *rt2x00dev,
 			       enum dev_state state)
 {
-	/*
-	 * Always put the device to sleep (even when we intend to wakeup!)
-	 * if the device is booting and wasn't asleep it will return
-	 * failure when attempting to wakeup.
-	 */
-	rt2800_mcu_request(rt2x00dev, MCU_SLEEP, 0xff, 0xff, 2);
-
 	if (state == STATE_AWAKE) {
-		rt2800_mcu_request(rt2x00dev, MCU_WAKEUP, TOKEN_WAKUP, 0, 0);
+		rt2800_mcu_request(rt2x00dev, MCU_WAKEUP, TOKEN_WAKUP, 0, 0x02);
 		rt2800pci_mcu_status(rt2x00dev, TOKEN_WAKUP);
+	} else if (state == STATE_SLEEP) {
+		rt2800_register_write(rt2x00dev, H2M_MAILBOX_STATUS, 0xffffffff);
+		rt2800_register_write(rt2x00dev, H2M_MAILBOX_CID, 0xffffffff);
+		rt2800_mcu_request(rt2x00dev, MCU_SLEEP, 0x01, 0xff, 0x01);
 	}
 
 	return 0;
@@ -538,9 +570,7 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt2800pci_set_state(rt2x00dev, STATE_SLEEP);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		rt2800pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
@@ -652,6 +682,12 @@ static void rt2800pci_fill_rxdone(struct queue_entry *entry,
 		 */
 		rxdesc->flags |= RX_FLAG_IV_STRIPPED;
 
+		/*
+		 * The hardware has already checked the Michael Mic and has
+		 * stripped it from the frame. Signal this to mac80211.
+		 */
+		rxdesc->flags |= RX_FLAG_MMIC_STRIPPED;
+
 		if (rxdesc->cipher_status == RX_CRYPTO_SUCCESS)
 			rxdesc->flags |= RX_FLAG_DECRYPTED;
 		else if (rxdesc->cipher_status == RX_CRYPTO_FAIL_MIC)
@@ -690,7 +726,7 @@ static void rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 
 	while (kfifo_get(&rt2x00dev->txstatus_fifo, &status)) {
 		qid = rt2x00_get_field32(status, TX_STA_FIFO_PID_QUEUE);
-		if (qid >= QID_RX) {
+		if (unlikely(qid >= QID_RX)) {
 			/*
 			 * Unknown queue, this shouldn't happen. Just drop
 			 * this tx status.
@@ -700,7 +736,7 @@ static void rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 			break;
 		}
 
-		queue = rt2x00queue_get_queue(rt2x00dev, qid);
+		queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
 		if (unlikely(queue == NULL)) {
 			/*
 			 * The queue is NULL, this shouldn't happen. Stop
@@ -711,7 +747,7 @@ static void rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 			break;
 		}
 
-		if (rt2x00queue_empty(queue)) {
+		if (unlikely(rt2x00queue_empty(queue))) {
 			/*
 			 * The queue is empty. Stop processing here
 			 * and drop the tx status.
@@ -726,45 +762,59 @@ static void rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 	}
 }
 
-static void rt2800pci_txstatus_tasklet(unsigned long data)
+static void rt2800pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
+				       struct rt2x00_field32 irq_field)
 {
-	rt2800pci_txdone((struct rt2x00_dev *)data);
-}
-
-static irqreturn_t rt2800pci_interrupt_thread(int irq, void *dev_instance)
-{
-	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg = rt2x00dev->irqvalue[0];
+	u32 reg;
 
 	/*
-	 * 1 - Pre TBTT interrupt.
+	 * Enable a single interrupt. The interrupt mask register
+	 * access needs locking.
 	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_PRE_TBTT))
-		rt2x00lib_pretbtt(rt2x00dev);
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
+	rt2800_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00_set_field32(&reg, irq_field, 1);
+	rt2800_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
 
-	/*
-	 * 2 - Beacondone interrupt.
-	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TBTT))
-		rt2x00lib_beacondone(rt2x00dev);
+static void rt2800pci_txstatus_tasklet(unsigned long data)
+{
+	rt2800pci_txdone((struct rt2x00_dev *)data);
 
 	/*
-	 * 3 - Rx ring done interrupt.
+	 * No need to enable the tx status interrupt here as we always
+	 * leave it enabled to minimize the possibility of a tx status
+	 * register overflow. See comment in interrupt handler.
 	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RX_DONE))
-		rt2x00pci_rxdone(rt2x00dev);
+}
 
-	/*
-	 * 4 - Auto wakeup interrupt.
-	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_AUTO_WAKEUP))
-		rt2800pci_wakeup(rt2x00dev);
+static void rt2800pci_pretbtt_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00lib_pretbtt(rt2x00dev);
+	rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_PRE_TBTT);
+}
+
+static void rt2800pci_tbtt_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00lib_beacondone(rt2x00dev);
+	rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_TBTT);
+}
 
-	/* Enable interrupts again. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_ON_ISR);
+static void rt2800pci_rxdone_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00pci_rxdone(rt2x00dev);
+	rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_RX_DONE);
+}
 
-	return IRQ_HANDLED;
+static void rt2800pci_autowake_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2800pci_wakeup(rt2x00dev);
+	rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_AUTO_WAKEUP);
 }
 
 static void rt2800pci_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
@@ -785,7 +835,7 @@ static void rt2800pci_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
 	 *
 	 * Furthermore we don't disable the TX_FIFO_STATUS
 	 * interrupt here but leave it enabled so that the TX_STA_FIFO
-	 * can also be read while the interrupt thread gets executed.
+	 * can also be read while the tx status tasklet gets executed.
 	 *
 	 * Since we have only one producer and one consumer we don't
 	 * need to lock the kfifo.
@@ -810,8 +860,7 @@ static void rt2800pci_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
 static irqreturn_t rt2800pci_interrupt(int irq, void *dev_instance)
 {
 	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg;
-	irqreturn_t ret = IRQ_HANDLED;
+	u32 reg, mask;
 
 	/* Read status and ACK all interrupts */
 	rt2800_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
@@ -823,38 +872,44 @@ static irqreturn_t rt2800pci_interrupt(int irq, void *dev_instance)
 	if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		return IRQ_HANDLED;
 
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TX_FIFO_STATUS))
-		rt2800pci_txstatus_interrupt(rt2x00dev);
+	/*
+	 * Since INT_MASK_CSR and INT_SOURCE_CSR use the same bits
+	 * for interrupts and interrupt masks we can just use the value of
+	 * INT_SOURCE_CSR to create the interrupt mask.
+	 */
+	mask = ~reg;
 
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_PRE_TBTT) ||
-	    rt2x00_get_field32(reg, INT_SOURCE_CSR_TBTT) ||
-	    rt2x00_get_field32(reg, INT_SOURCE_CSR_RX_DONE) ||
-	    rt2x00_get_field32(reg, INT_SOURCE_CSR_AUTO_WAKEUP)) {
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TX_FIFO_STATUS)) {
+		rt2800pci_txstatus_interrupt(rt2x00dev);
 		/*
-		 * All other interrupts are handled in the interrupt thread.
-		 * Store irqvalue for use in the interrupt thread.
+		 * Never disable the TX_FIFO_STATUS interrupt.
 		 */
-		rt2x00dev->irqvalue[0] = reg;
+		rt2x00_set_field32(&mask, INT_MASK_CSR_TX_FIFO_STATUS, 1);
+	}
 
-		/*
-		 * Disable interrupts, will be enabled again in the
-		 * interrupt thread.
-		*/
-		rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-						      STATE_RADIO_IRQ_OFF_ISR);
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_PRE_TBTT))
+		tasklet_hi_schedule(&rt2x00dev->pretbtt_tasklet);
 
-		/*
-		 * Leave the TX_FIFO_STATUS interrupt enabled to not lose any
-		 * tx status reports.
-		 */
-		rt2800_register_read(rt2x00dev, INT_MASK_CSR, &reg);
-		rt2x00_set_field32(&reg, INT_MASK_CSR_TX_FIFO_STATUS, 1);
-		rt2800_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TBTT))
+		tasklet_hi_schedule(&rt2x00dev->tbtt_tasklet);
 
-		ret = IRQ_WAKE_THREAD;
-	}
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RX_DONE))
+		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
 
-	return ret;
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_AUTO_WAKEUP))
+		tasklet_schedule(&rt2x00dev->autowake_tasklet);
+
+	/*
+	 * Disable all interrupts for which a tasklet was scheduled right now,
+	 * the tasklet will reenable the appropriate interrupts.
+	 */
+	spin_lock(&rt2x00dev->irqmask_lock);
+	rt2800_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg &= mask;
+	rt2800_register_write(rt2x00dev, INT_MASK_CSR, reg);
+	spin_unlock(&rt2x00dev->irqmask_lock);
+
+	return IRQ_HANDLED;
 }
 
 /*
@@ -922,6 +977,7 @@ static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	if (!modparam_nohwcrypt)
 		__set_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags);
 	__set_bit(DRIVER_SUPPORT_LINK_TUNING, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_HT_TX_DESC, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -969,8 +1025,11 @@ static const struct rt2800_ops rt2800pci_rt2800_ops = {
 
 static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
 	.irq_handler		= rt2800pci_interrupt,
-	.irq_handler_thread	= rt2800pci_interrupt_thread,
-	.txstatus_tasklet       = rt2800pci_txstatus_tasklet,
+	.txstatus_tasklet	= rt2800pci_txstatus_tasklet,
+	.pretbtt_tasklet	= rt2800pci_pretbtt_tasklet,
+	.tbtt_tasklet		= rt2800pci_tbtt_tasklet,
+	.rxdone_tasklet		= rt2800pci_rxdone_tasklet,
+	.autowake_tasklet	= rt2800pci_autowake_tasklet,
 	.probe_hw		= rt2800pci_probe_hw,
 	.get_firmware_name	= rt2800pci_get_firmware_name,
 	.check_firmware		= rt2800_check_firmware,
@@ -990,6 +1049,7 @@ static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
 	.write_tx_desc		= rt2800pci_write_tx_desc,
 	.write_tx_data		= rt2800_write_tx_data,
 	.write_beacon		= rt2800_write_beacon,
+	.clear_beacon		= rt2800_clear_beacon,
 	.fill_rxdone		= rt2800pci_fill_rxdone,
 	.config_shared_key	= rt2800_config_shared_key,
 	.config_pairwise_key	= rt2800_config_pairwise_key,
@@ -1065,12 +1125,17 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
 	{ PCI_DEVICE(0x1814, 0x3390), PCI_DEVICE_DATA(&rt2800pci_ops) },
 #endif
 #ifdef CONFIG_RT2800PCI_RT35XX
+	{ PCI_DEVICE(0x1432, 0x7711), PCI_DEVICE_DATA(&rt2800pci_ops) },
+	{ PCI_DEVICE(0x1432, 0x7722), PCI_DEVICE_DATA(&rt2800pci_ops) },
 	{ PCI_DEVICE(0x1814, 0x3060), PCI_DEVICE_DATA(&rt2800pci_ops) },
 	{ PCI_DEVICE(0x1814, 0x3062), PCI_DEVICE_DATA(&rt2800pci_ops) },
 	{ PCI_DEVICE(0x1814, 0x3562), PCI_DEVICE_DATA(&rt2800pci_ops) },
 	{ PCI_DEVICE(0x1814, 0x3592), PCI_DEVICE_DATA(&rt2800pci_ops) },
 	{ PCI_DEVICE(0x1814, 0x3593), PCI_DEVICE_DATA(&rt2800pci_ops) },
 #endif
+#ifdef CONFIG_RT2800PCI_RT53XX
+	{ PCI_DEVICE(0x1814, 0x5390), PCI_DEVICE_DATA(&rt2800pci_ops) },
+#endif
 	{ 0, }
 };
 #endif /* CONFIG_PCI */
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index b97a4a54ff4c..f1a92144996f 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -253,9 +253,7 @@ static int rt2800usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt2800usb_set_state(rt2x00dev, STATE_SLEEP);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		/* No support, but no error either */
 		break;
 	case STATE_DEEP_SLEEP:
@@ -486,6 +484,12 @@ static void rt2800usb_fill_rxdone(struct queue_entry *entry,
 		 */
 		rxdesc->flags |= RX_FLAG_IV_STRIPPED;
 
+		/*
+		 * The hardware has already checked the Michael Mic and has
+		 * stripped it from the frame. Signal this to mac80211.
+		 */
+		rxdesc->flags |= RX_FLAG_MMIC_STRIPPED;
+
 		if (rxdesc->cipher_status == RX_CRYPTO_SUCCESS)
 			rxdesc->flags |= RX_FLAG_DECRYPTED;
 		else if (rxdesc->cipher_status == RX_CRYPTO_FAIL_MIC)
@@ -561,6 +565,7 @@ static int rt2800usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 		__set_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags);
 	__set_bit(DRIVER_SUPPORT_LINK_TUNING, &rt2x00dev->flags);
 	__set_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_HT_TX_DESC, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -633,6 +638,7 @@ static const struct rt2x00lib_ops rt2800usb_rt2x00_ops = {
 	.write_tx_desc		= rt2800usb_write_tx_desc,
 	.write_tx_data		= rt2800usb_write_tx_data,
 	.write_beacon		= rt2800_write_beacon,
+	.clear_beacon		= rt2800_clear_beacon,
 	.get_tx_data_len	= rt2800usb_get_tx_data_len,
 	.fill_rxdone		= rt2800usb_fill_rxdone,
 	.config_shared_key	= rt2800_config_shared_key,
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 84aaf393da43..a3940d7300a4 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -189,6 +189,7 @@ struct rt2x00_chip {
 #define RT3572		0x3572
 #define RT3593		0x3593	/* PCIe */
 #define RT3883		0x3883	/* WSOC */
+#define RT5390         0x5390  /* 2.4GHz */
 
 	u16 rf;
 	u16 rev;
@@ -225,6 +226,8 @@ struct channel_info {
 struct antenna_setup {
 	enum antenna rx;
 	enum antenna tx;
+	u8 rx_chain_num;
+	u8 tx_chain_num;
 };
 
 /*
@@ -368,6 +371,7 @@ struct rt2x00_intf {
 	 * dedicated beacon entry.
 	 */
 	struct queue_entry *beacon;
+	bool enable_beacon;
 
 	/*
 	 * Actions that needed rescheduling.
@@ -463,7 +467,6 @@ struct rt2x00lib_crypto {
 	const u8 *address;
 
 	u32 bssidx;
-	u32 aid;
 
 	u8 key[16];
 	u8 tx_mic[8];
@@ -511,14 +514,13 @@ struct rt2x00lib_ops {
 	irq_handler_t irq_handler;
 
 	/*
-	 * Threaded Interrupt handlers.
-	 */
-	irq_handler_t irq_handler_thread;
-
-	/*
 	 * TX status tasklet handler.
 	 */
 	void (*txstatus_tasklet) (unsigned long data);
+	void (*pretbtt_tasklet) (unsigned long data);
+	void (*tbtt_tasklet) (unsigned long data);
+	void (*rxdone_tasklet) (unsigned long data);
+	void (*autowake_tasklet) (unsigned long data);
 
 	/*
 	 * Device init handlers.
@@ -573,6 +575,7 @@ struct rt2x00lib_ops {
 			       struct txentry_desc *txdesc);
 	void (*write_beacon) (struct queue_entry *entry,
 			      struct txentry_desc *txdesc);
+	void (*clear_beacon) (struct queue_entry *entry);
 	int (*get_tx_data_len) (struct queue_entry *entry);
 
 	/*
@@ -658,12 +661,15 @@ enum rt2x00_flags {
 	DRIVER_REQUIRE_L2PAD,
 	DRIVER_REQUIRE_TXSTATUS_FIFO,
 	DRIVER_REQUIRE_TASKLET_CONTEXT,
+	DRIVER_REQUIRE_SW_SEQNO,
+	DRIVER_REQUIRE_HT_TX_DESC,
 
 	/*
 	 * Driver features
 	 */
 	CONFIG_SUPPORT_HW_BUTTON,
 	CONFIG_SUPPORT_HW_CRYPTO,
+	CONFIG_SUPPORT_POWER_LIMIT,
 	DRIVER_SUPPORT_CONTROL_FILTERS,
 	DRIVER_SUPPORT_CONTROL_FILTER_PSPOLL,
 	DRIVER_SUPPORT_PRE_TBTT_INTERRUPT,
@@ -788,10 +794,12 @@ struct rt2x00_dev {
 	 *  - Open ap interface count.
 	 *  - Open sta interface count.
 	 *  - Association count.
+	 *  - Beaconing enabled count.
 	 */
 	unsigned int intf_ap_count;
 	unsigned int intf_sta_count;
 	unsigned int intf_associated;
+	unsigned int intf_beaconing;
 
 	/*
 	 * Link quality
@@ -857,6 +865,13 @@ struct rt2x00_dev {
 	 */
 	struct ieee80211_low_level_stats low_level_stats;
 
+	/**
+	 * Work queue for all work which should not be placed
+	 * on the mac80211 workqueue (because of dependencies
+	 * between various work structures).
+	 */
+	struct workqueue_struct *workqueue;
+
 	/*
 	 * Scheduled work.
 	 * NOTE: intf_work will use ieee80211_iterate_active_interfaces()
@@ -872,14 +887,13 @@ struct rt2x00_dev {
 	struct work_struct txdone_work;
 
 	/*
-	 * Data queue arrays for RX, TX and Beacon.
-	 * The Beacon array also contains the Atim queue
-	 * if that is supported by the device.
+	 * Data queue arrays for RX, TX, Beacon and ATIM.
 	 */
 	unsigned int data_queues;
 	struct data_queue *rx;
 	struct data_queue *tx;
 	struct data_queue *bcn;
+	struct data_queue *atim;
 
 	/*
 	 * Firmware image.
@@ -887,12 +901,6 @@ struct rt2x00_dev {
 	const struct firmware *fw;
 
 	/*
-	 * Interrupt values, stored between interrupt service routine
-	 * and interrupt thread routine.
-	 */
-	u32 irqvalue[2];
-
-	/*
 	 * FIFO for storing tx status reports between isr and tasklet.
 	 */
 	DECLARE_KFIFO_PTR(txstatus_fifo, u32);
@@ -901,6 +909,15 @@ struct rt2x00_dev {
 	 * Tasklet for processing tx status reports (rt2800pci).
 	 */
 	struct tasklet_struct txstatus_tasklet;
+	struct tasklet_struct pretbtt_tasklet;
+	struct tasklet_struct tbtt_tasklet;
+	struct tasklet_struct rxdone_tasklet;
+	struct tasklet_struct autowake_tasklet;
+
+	/*
+	 * Protect the interrupt mask register.
+	 */
+	spinlock_t irqmask_lock;
 };
 
 /*
@@ -1046,12 +1063,24 @@ void rt2x00queue_map_txskb(struct queue_entry *entry);
 void rt2x00queue_unmap_skb(struct queue_entry *entry);
 
 /**
- * rt2x00queue_get_queue - Convert queue index to queue pointer
+ * rt2x00queue_get_tx_queue - Convert tx queue index to queue pointer
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
  * @queue: rt2x00 queue index (see &enum data_queue_qid).
+ *
+ * Returns NULL for non tx queues.
  */
-struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
-					 const enum data_queue_qid queue);
+static inline struct data_queue *
+rt2x00queue_get_tx_queue(struct rt2x00_dev *rt2x00dev,
+			 const enum data_queue_qid queue)
+{
+	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
+		return &rt2x00dev->tx[queue];
+
+	if (queue == QID_ATIM)
+		return rt2x00dev->atim;
+
+	return NULL;
+}
 
 /**
  * rt2x00queue_get_entry - Get queue entry where the given index points to.
@@ -1168,7 +1197,7 @@ void rt2x00lib_rxdone(struct queue_entry *entry);
 /*
  * mac80211 handlers.
  */
-int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 int rt2x00mac_start(struct ieee80211_hw *hw);
 void rt2x00mac_stop(struct ieee80211_hw *hw);
 int rt2x00mac_add_interface(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 9597a03242cc..9de9dbe94399 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -121,7 +121,7 @@ static void rt2x00lib_intf_scheduled_iter(void *data, u8 *mac,
 		return;
 
 	if (test_and_clear_bit(DELAYED_UPDATE_BEACON, &intf->delayed_flags))
-		rt2x00queue_update_beacon(rt2x00dev, vif, true);
+		rt2x00queue_update_beacon(rt2x00dev, vif);
 }
 
 static void rt2x00lib_intf_scheduled(struct work_struct *work)
@@ -174,7 +174,13 @@ static void rt2x00lib_beaconupdate_iter(void *data, u8 *mac,
 	    vif->type != NL80211_IFTYPE_WDS)
 		return;
 
-	rt2x00queue_update_beacon(rt2x00dev, vif, true);
+	/*
+	 * Update the beacon without locking. This is safe on PCI devices
+	 * as they only update the beacon periodically here. This should
+	 * never be called for USB devices.
+	 */
+	WARN_ON(rt2x00_is_usb(rt2x00dev));
+	rt2x00queue_update_beacon_locked(rt2x00dev, vif);
 }
 
 void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev)
@@ -183,9 +189,9 @@ void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev)
 		return;
 
 	/* send buffered bc/mc frames out for every bssid */
-	ieee80211_iterate_active_interfaces(rt2x00dev->hw,
-					    rt2x00lib_bc_buffer_iter,
-					    rt2x00dev);
+	ieee80211_iterate_active_interfaces_atomic(rt2x00dev->hw,
+						   rt2x00lib_bc_buffer_iter,
+						   rt2x00dev);
 	/*
 	 * Devices with pre tbtt interrupt don't need to update the beacon
 	 * here as they will fetch the next beacon directly prior to
@@ -195,9 +201,9 @@ void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev)
 		return;
 
 	/* fetch next beacon */
-	ieee80211_iterate_active_interfaces(rt2x00dev->hw,
-					    rt2x00lib_beaconupdate_iter,
-					    rt2x00dev);
+	ieee80211_iterate_active_interfaces_atomic(rt2x00dev->hw,
+						   rt2x00lib_beaconupdate_iter,
+						   rt2x00dev);
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_beacondone);
 
@@ -207,9 +213,9 @@ void rt2x00lib_pretbtt(struct rt2x00_dev *rt2x00dev)
 		return;
 
 	/* fetch next beacon */
-	ieee80211_iterate_active_interfaces(rt2x00dev->hw,
-					    rt2x00lib_beaconupdate_iter,
-					    rt2x00dev);
+	ieee80211_iterate_active_interfaces_atomic(rt2x00dev->hw,
+						   rt2x00lib_beaconupdate_iter,
+						   rt2x00dev);
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_pretbtt);
 
@@ -649,7 +655,10 @@ static void rt2x00lib_channel(struct ieee80211_channel *entry,
 			      const int channel, const int tx_power,
 			      const int value)
 {
-	entry->center_freq = ieee80211_channel_to_frequency(channel);
+	/* XXX: this assumption about the band is wrong for 802.11j */
+	entry->band = channel <= 14 ? IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+	entry->center_freq = ieee80211_channel_to_frequency(channel,
+							    entry->band);
 	entry->hw_value = value;
 	entry->max_power = tx_power;
 	entry->max_antenna_gain = 0xff;
@@ -812,15 +821,29 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
 				     GFP_KERNEL);
 		if (status)
 			return status;
+	}
 
-		/* tasklet for processing the tx status reports. */
-		if (rt2x00dev->ops->lib->txstatus_tasklet)
-			tasklet_init(&rt2x00dev->txstatus_tasklet,
-				     rt2x00dev->ops->lib->txstatus_tasklet,
-				     (unsigned long)rt2x00dev);
-
+	/*
+	 * Initialize tasklets if used by the driver. Tasklets are
+	 * disabled until the interrupts are turned on. The driver
+	 * has to handle that.
+	 */
+#define RT2X00_TASKLET_INIT(taskletname) \
+	if (rt2x00dev->ops->lib->taskletname) { \
+		tasklet_init(&rt2x00dev->taskletname, \
+			     rt2x00dev->ops->lib->taskletname, \
+			     (unsigned long)rt2x00dev); \
+		tasklet_disable(&rt2x00dev->taskletname); \
 	}
 
+	RT2X00_TASKLET_INIT(txstatus_tasklet);
+	RT2X00_TASKLET_INIT(pretbtt_tasklet);
+	RT2X00_TASKLET_INIT(tbtt_tasklet);
+	RT2X00_TASKLET_INIT(rxdone_tasklet);
+	RT2X00_TASKLET_INIT(autowake_tasklet);
+
+#undef RT2X00_TASKLET_INIT
+
 	/*
 	 * Register HW.
 	 */
@@ -949,6 +972,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 {
 	int retval = -ENOMEM;
 
+	spin_lock_init(&rt2x00dev->irqmask_lock);
 	mutex_init(&rt2x00dev->csr_mutex);
 
 	set_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags);
@@ -973,8 +997,15 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 		    BIT(NL80211_IFTYPE_WDS);
 
 	/*
-	 * Initialize configuration work.
+	 * Initialize work.
 	 */
+	rt2x00dev->workqueue =
+	    alloc_ordered_workqueue(wiphy_name(rt2x00dev->hw->wiphy), 0);
+	if (!rt2x00dev->workqueue) {
+		retval = -ENOMEM;
+		goto exit;
+	}
+
 	INIT_WORK(&rt2x00dev->intf_work, rt2x00lib_intf_scheduled);
 
 	/*
@@ -1033,6 +1064,7 @@ void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 	cancel_work_sync(&rt2x00dev->intf_work);
 	cancel_work_sync(&rt2x00dev->rxdone_work);
 	cancel_work_sync(&rt2x00dev->txdone_work);
+	destroy_workqueue(rt2x00dev->workqueue);
 
 	/*
 	 * Free the tx status fifo.
@@ -1043,6 +1075,10 @@ void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 	 * Kill the tx status tasklet.
 	 */
 	tasklet_kill(&rt2x00dev->txstatus_tasklet);
+	tasklet_kill(&rt2x00dev->pretbtt_tasklet);
+	tasklet_kill(&rt2x00dev->tbtt_tasklet);
+	tasklet_kill(&rt2x00dev->rxdone_tasklet);
+	tasklet_kill(&rt2x00dev->autowake_tasklet);
 
 	/*
 	 * Uninitialize device.
diff --git a/drivers/net/wireless/rt2x00/rt2x00ht.c b/drivers/net/wireless/rt2x00/rt2x00ht.c
index b7ad46ecaa1d..ae1219dffaae 100644
--- a/drivers/net/wireless/rt2x00/rt2x00ht.c
+++ b/drivers/net/wireless/rt2x00/rt2x00ht.c
@@ -38,12 +38,12 @@ void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
 
 	if (tx_info->control.sta)
-		txdesc->mpdu_density =
+		txdesc->u.ht.mpdu_density =
 		    tx_info->control.sta->ht_cap.ampdu_density;
 
-	txdesc->ba_size = 7;	/* FIXME: What value is needed? */
+	txdesc->u.ht.ba_size = 7;	/* FIXME: What value is needed? */
 
-	txdesc->stbc =
+	txdesc->u.ht.stbc =
 	    (tx_info->flags & IEEE80211_TX_CTL_STBC) >> IEEE80211_TX_CTL_STBC_SHIFT;
 
 	/*
@@ -51,25 +51,24 @@ void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 	 * mcs rate to be used
 	 */
 	if (txrate->flags & IEEE80211_TX_RC_MCS) {
-		txdesc->mcs = txrate->idx;
+		txdesc->u.ht.mcs = txrate->idx;
 
 		/*
 		 * MIMO PS should be set to 1 for STA's using dynamic SM PS
 		 * when using more then one tx stream (>MCS7).
 		 */
-		if (tx_info->control.sta && txdesc->mcs > 7 &&
+		if (tx_info->control.sta && txdesc->u.ht.mcs > 7 &&
 		    ((tx_info->control.sta->ht_cap.cap &
 		      IEEE80211_HT_CAP_SM_PS) >>
 		     IEEE80211_HT_CAP_SM_PS_SHIFT) ==
 		    WLAN_HT_CAP_SM_PS_DYNAMIC)
 			__set_bit(ENTRY_TXD_HT_MIMO_PS, &txdesc->flags);
 	} else {
-		txdesc->mcs = rt2x00_get_rate_mcs(hwrate->mcs);
+		txdesc->u.ht.mcs = rt2x00_get_rate_mcs(hwrate->mcs);
 		if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
-			txdesc->mcs |= 0x08;
+			txdesc->u.ht.mcs |= 0x08;
 	}
 
-
 	/*
 	 * This frame is eligible for an AMPDU, however, don't aggregate
 	 * frames that are intended to probe a specific tx rate.
@@ -79,14 +78,6 @@ void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 		__set_bit(ENTRY_TXD_HT_AMPDU, &txdesc->flags);
 
 	/*
-	 * Determine HT Mix/Greenfield rate mode
-	 */
-	if (txrate->flags & IEEE80211_TX_RC_MCS)
-		txdesc->rate_mode = RATE_MODE_HT_MIX;
-	if (txrate->flags & IEEE80211_TX_RC_GREEN_FIELD)
-		txdesc->rate_mode = RATE_MODE_HT_GREENFIELD;
-
-	/*
 	 * Set 40Mhz mode if necessary (for legacy rates this will
 	 * duplicate the frame to both channels).
 	 */
@@ -106,11 +97,11 @@ void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 	 * for frames not transmitted with TXOP_HTTXOP
 	 */
 	if (ieee80211_is_mgmt(hdr->frame_control))
-		txdesc->txop = TXOP_BACKOFF;
+		txdesc->u.ht.txop = TXOP_BACKOFF;
 	else if (!(tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT))
-		txdesc->txop = TXOP_SIFS;
+		txdesc->u.ht.txop = TXOP_SIFS;
 	else
-		txdesc->txop = TXOP_HTTXOP;
+		txdesc->u.ht.txop = TXOP_HTTXOP;
 }
 
 u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index a105c500627b..2d94cbaf5f4a 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -157,14 +157,30 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 			       bool local);
 
 /**
- * rt2x00queue_update_beacon - Send new beacon from mac80211 to hardware
+ * rt2x00queue_update_beacon - Send new beacon from mac80211
+ *	to hardware. Handles locking by itself (mutex).
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
  * @vif: Interface for which the beacon should be updated.
- * @enable_beacon: Enable beaconing
  */
 int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
-			      struct ieee80211_vif *vif,
-			      const bool enable_beacon);
+			      struct ieee80211_vif *vif);
+
+/**
+ * rt2x00queue_update_beacon_locked - Send new beacon from mac80211
+ *	to hardware. Caller needs to ensure locking.
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @vif: Interface for which the beacon should be updated.
+ */
+int rt2x00queue_update_beacon_locked(struct rt2x00_dev *rt2x00dev,
+				     struct ieee80211_vif *vif);
+
+/**
+ * rt2x00queue_clear_beacon - Clear beacon in hardware
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @vif: Interface for which the beacon should be updated.
+ */
+int rt2x00queue_clear_beacon(struct rt2x00_dev *rt2x00dev,
+			     struct ieee80211_vif *vif);
 
 /**
  * rt2x00queue_index_inc - Index incrementation function
diff --git a/drivers/net/wireless/rt2x00/rt2x00link.c b/drivers/net/wireless/rt2x00/rt2x00link.c
index bfda60eaf4ef..c975b0a12e95 100644
--- a/drivers/net/wireless/rt2x00/rt2x00link.c
+++ b/drivers/net/wireless/rt2x00/rt2x00link.c
@@ -417,7 +417,8 @@ void rt2x00link_start_watchdog(struct rt2x00_dev *rt2x00dev)
 	    !test_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags))
 		return;
 
-	schedule_delayed_work(&link->watchdog_work, WATCHDOG_INTERVAL);
+	ieee80211_queue_delayed_work(rt2x00dev->hw,
+				     &link->watchdog_work, WATCHDOG_INTERVAL);
 }
 
 void rt2x00link_stop_watchdog(struct rt2x00_dev *rt2x00dev)
@@ -441,7 +442,9 @@ static void rt2x00link_watchdog(struct work_struct *work)
 	rt2x00dev->ops->lib->watchdog(rt2x00dev);
 
 	if (test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
-		schedule_delayed_work(&link->watchdog_work, WATCHDOG_INTERVAL);
+		ieee80211_queue_delayed_work(rt2x00dev->hw,
+					     &link->watchdog_work,
+					     WATCHDOG_INTERVAL);
 }
 
 void rt2x00link_register(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index f3da051df39e..661c6baad2b9 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -99,7 +99,7 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
 	return retval;
 }
 
-int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+void rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
@@ -116,13 +116,13 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		goto exit_fail;
 
 	/*
-	 * Determine which queue to put packet on.
+	 * Use the ATIM queue if appropriate and present.
 	 */
 	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM &&
 	    test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
-		queue = rt2x00queue_get_queue(rt2x00dev, QID_ATIM);
-	else
-		queue = rt2x00queue_get_queue(rt2x00dev, qid);
+		qid = QID_ATIM;
+
+	queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
 	if (unlikely(!queue)) {
 		ERROR(rt2x00dev,
 		      "Attempt to send packet over invalid queue %d.\n"
@@ -139,9 +139,9 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	 * either RTS or CTS-to-self frame and handles everything
 	 * inside the hardware.
 	 */
-	if ((tx_info->control.rates[0].flags & (IEEE80211_TX_RC_USE_RTS_CTS |
-						IEEE80211_TX_RC_USE_CTS_PROTECT)) &&
-	    !rt2x00dev->ops->hw->set_rts_threshold) {
+	if (!rt2x00dev->ops->hw->set_rts_threshold &&
+	    (tx_info->control.rates[0].flags & (IEEE80211_TX_RC_USE_RTS_CTS |
+						IEEE80211_TX_RC_USE_CTS_PROTECT))) {
 		if (rt2x00queue_available(queue) <= 1)
 			goto exit_fail;
 
@@ -149,18 +149,17 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 			goto exit_fail;
 	}
 
-	if (rt2x00queue_write_tx_frame(queue, skb, false))
+	if (unlikely(rt2x00queue_write_tx_frame(queue, skb, false)))
 		goto exit_fail;
 
 	if (rt2x00queue_threshold(queue))
 		rt2x00queue_pause_queue(queue);
 
-	return NETDEV_TX_OK;
+	return;
 
  exit_fail:
 	ieee80211_stop_queue(rt2x00dev->hw, qid);
 	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_tx);
 
@@ -191,7 +190,7 @@ int rt2x00mac_add_interface(struct ieee80211_hw *hw,
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct rt2x00_intf *intf = vif_to_intf(vif);
-	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, QID_BEACON);
+	struct data_queue *queue = rt2x00dev->bcn;
 	struct queue_entry *entry = NULL;
 	unsigned int i;
 
@@ -519,11 +518,9 @@ int rt2x00mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	crypto.cmd = cmd;
 
-	if (sta) {
-		/* some drivers need the AID */
-		crypto.aid = sta->aid;
+	if (sta)
 		crypto.address = sta->addr;
-	} else
+	else
 		crypto.address = bcast_addr;
 
 	if (crypto.cipher == CIPHER_TKIP)
@@ -617,11 +614,47 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 				      bss_conf->bssid);
 
 	/*
-	 * Update the beacon.
+	 * Update the beacon. This is only required on USB devices. PCI
+	 * devices fetch beacons periodically.
 	 */
-	if (changes & (BSS_CHANGED_BEACON | BSS_CHANGED_BEACON_ENABLED))
-		rt2x00queue_update_beacon(rt2x00dev, vif,
-					  bss_conf->enable_beacon);
+	if (changes & BSS_CHANGED_BEACON && rt2x00_is_usb(rt2x00dev))
+		rt2x00queue_update_beacon(rt2x00dev, vif);
+
+	/*
+	 * Start/stop beaconing.
+	 */
+	if (changes & BSS_CHANGED_BEACON_ENABLED) {
+		if (!bss_conf->enable_beacon && intf->enable_beacon) {
+			rt2x00queue_clear_beacon(rt2x00dev, vif);
+			rt2x00dev->intf_beaconing--;
+			intf->enable_beacon = false;
+
+			if (rt2x00dev->intf_beaconing == 0) {
+				/*
+				 * Last beaconing interface disabled
+				 * -> stop beacon queue.
+				 */
+				mutex_lock(&intf->beacon_skb_mutex);
+				rt2x00queue_stop_queue(rt2x00dev->bcn);
+				mutex_unlock(&intf->beacon_skb_mutex);
+			}
+
+
+		} else if (bss_conf->enable_beacon && !intf->enable_beacon) {
+			rt2x00dev->intf_beaconing++;
+			intf->enable_beacon = true;
+
+			if (rt2x00dev->intf_beaconing == 1) {
+				/*
+				 * First beaconing interface enabled
+				 * -> start beacon queue.
+				 */
+				mutex_lock(&intf->beacon_skb_mutex);
+				rt2x00queue_start_queue(rt2x00dev->bcn);
+				mutex_unlock(&intf->beacon_skb_mutex);
+			}
+		}
+	}
 
 	/*
 	 * When the association status has changed we must reset the link
@@ -657,7 +690,7 @@ int rt2x00mac_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct data_queue *queue;
 
-	queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+	queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
 	if (unlikely(!queue))
 		return -EINVAL;
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index ace0b668c04e..4dd82b0b0520 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -160,10 +160,9 @@ int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Register interrupt handler.
 	 */
-	status = request_threaded_irq(rt2x00dev->irq,
-				      rt2x00dev->ops->lib->irq_handler,
-				      rt2x00dev->ops->lib->irq_handler_thread,
-				      IRQF_SHARED, rt2x00dev->name, rt2x00dev);
+	status = request_irq(rt2x00dev->irq,
+			     rt2x00dev->ops->lib->irq_handler,
+			     IRQF_SHARED, rt2x00dev->name, rt2x00dev);
 	if (status) {
 		ERROR(rt2x00dev, "IRQ %d allocation failed (error %d).\n",
 		      rt2x00dev->irq, status);
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index ca82b3a91697..4b3c70eeef1f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -221,14 +221,17 @@ static void rt2x00queue_create_tx_descriptor_seq(struct queue_entry *entry,
 	struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
 	unsigned long irqflags;
 
-	if (!(tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) ||
-	    unlikely(!tx_info->control.vif))
+	if (!(tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ))
+		return;
+
+	__set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
+
+	if (!test_bit(DRIVER_REQUIRE_SW_SEQNO, &entry->queue->rt2x00dev->flags))
 		return;
 
 	/*
-	 * Hardware should insert sequence counter.
-	 * FIXME: We insert a software sequence counter first for
-	 * hardware that doesn't support hardware sequence counting.
+	 * The hardware is not able to insert a sequence number. Assign a
+	 * software generated one here.
 	 *
 	 * This is wrong because beacons are not getting sequence
 	 * numbers assigned properly.
@@ -246,7 +249,6 @@ static void rt2x00queue_create_tx_descriptor_seq(struct queue_entry *entry,
 
 	spin_unlock_irqrestore(&intf->seqlock, irqflags);
 
-	__set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
 }
 
 static void rt2x00queue_create_tx_descriptor_plcp(struct queue_entry *entry,
@@ -260,6 +262,16 @@ static void rt2x00queue_create_tx_descriptor_plcp(struct queue_entry *entry,
 	unsigned int duration;
 	unsigned int residual;
 
+	/*
+	 * Determine with what IFS priority this frame should be send.
+	 * Set ifs to IFS_SIFS when the this is not the first fragment,
+	 * or this fragment came after RTS/CTS.
+	 */
+	if (test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags))
+		txdesc->u.plcp.ifs = IFS_BACKOFF;
+	else
+		txdesc->u.plcp.ifs = IFS_SIFS;
+
 	/* Data length + CRC + Crypto overhead (IV/EIV/ICV/MIC) */
 	data_length = entry->skb->len + 4;
 	data_length += rt2x00crypto_tx_overhead(rt2x00dev, entry->skb);
@@ -268,12 +280,12 @@ static void rt2x00queue_create_tx_descriptor_plcp(struct queue_entry *entry,
 	 * PLCP setup
 	 * Length calculation depends on OFDM/CCK rate.
 	 */
-	txdesc->signal = hwrate->plcp;
-	txdesc->service = 0x04;
+	txdesc->u.plcp.signal = hwrate->plcp;
+	txdesc->u.plcp.service = 0x04;
 
 	if (hwrate->flags & DEV_RATE_OFDM) {
-		txdesc->length_high = (data_length >> 6) & 0x3f;
-		txdesc->length_low = data_length & 0x3f;
+		txdesc->u.plcp.length_high = (data_length >> 6) & 0x3f;
+		txdesc->u.plcp.length_low = data_length & 0x3f;
 	} else {
 		/*
 		 * Convert length to microseconds.
@@ -288,18 +300,18 @@ static void rt2x00queue_create_tx_descriptor_plcp(struct queue_entry *entry,
 			 * Check if we need to set the Length Extension
 			 */
 			if (hwrate->bitrate == 110 && residual <= 30)
-				txdesc->service |= 0x80;
+				txdesc->u.plcp.service |= 0x80;
 		}
 
-		txdesc->length_high = (duration >> 8) & 0xff;
-		txdesc->length_low = duration & 0xff;
+		txdesc->u.plcp.length_high = (duration >> 8) & 0xff;
+		txdesc->u.plcp.length_low = duration & 0xff;
 
 		/*
 		 * When preamble is enabled we should set the
 		 * preamble bit for the signal.
 		 */
 		if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
-			txdesc->signal |= 0x08;
+			txdesc->u.plcp.signal |= 0x08;
 	}
 }
 
@@ -309,9 +321,9 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
-	struct ieee80211_rate *rate =
-	    ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
-	const struct rt2x00_rate *hwrate;
+	struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
+	struct ieee80211_rate *rate;
+	const struct rt2x00_rate *hwrate = NULL;
 
 	memset(txdesc, 0, sizeof(*txdesc));
 
@@ -365,42 +377,42 @@ static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
 
 	/*
 	 * Beacons and probe responses require the tsf timestamp
-	 * to be inserted into the frame, except for a frame that has been injected
-	 * through a monitor interface. This latter is needed for testing a
-	 * monitor interface.
+	 * to be inserted into the frame.
 	 */
-	if ((ieee80211_is_beacon(hdr->frame_control) ||
-	    ieee80211_is_probe_resp(hdr->frame_control)) &&
-	    (!(tx_info->flags & IEEE80211_TX_CTL_INJECTED)))
+	if (ieee80211_is_beacon(hdr->frame_control) ||
+	    ieee80211_is_probe_resp(hdr->frame_control))
 		__set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags);
 
-	/*
-	 * Determine with what IFS priority this frame should be send.
-	 * Set ifs to IFS_SIFS when the this is not the first fragment,
-	 * or this fragment came after RTS/CTS.
-	 */
 	if ((tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) &&
-	    !test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags)) {
+	    !test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags))
 		__set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
-		txdesc->ifs = IFS_BACKOFF;
-	} else
-		txdesc->ifs = IFS_SIFS;
 
 	/*
 	 * Determine rate modulation.
 	 */
-	hwrate = rt2x00_get_rate(rate->hw_value);
-	txdesc->rate_mode = RATE_MODE_CCK;
-	if (hwrate->flags & DEV_RATE_OFDM)
-		txdesc->rate_mode = RATE_MODE_OFDM;
+	if (txrate->flags & IEEE80211_TX_RC_GREEN_FIELD)
+		txdesc->rate_mode = RATE_MODE_HT_GREENFIELD;
+	else if (txrate->flags & IEEE80211_TX_RC_MCS)
+		txdesc->rate_mode = RATE_MODE_HT_MIX;
+	else {
+		rate = ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
+		hwrate = rt2x00_get_rate(rate->hw_value);
+		if (hwrate->flags & DEV_RATE_OFDM)
+			txdesc->rate_mode = RATE_MODE_OFDM;
+		else
+			txdesc->rate_mode = RATE_MODE_CCK;
+	}
 
 	/*
 	 * Apply TX descriptor handling by components
 	 */
 	rt2x00crypto_create_tx_descriptor(entry, txdesc);
-	rt2x00ht_create_tx_descriptor(entry, txdesc, hwrate);
 	rt2x00queue_create_tx_descriptor_seq(entry, txdesc);
-	rt2x00queue_create_tx_descriptor_plcp(entry, txdesc, hwrate);
+
+	if (test_bit(DRIVER_REQUIRE_HT_TX_DESC, &rt2x00dev->flags))
+		rt2x00ht_create_tx_descriptor(entry, txdesc, hwrate);
+	else
+		rt2x00queue_create_tx_descriptor_plcp(entry, txdesc, hwrate);
 }
 
 static int rt2x00queue_write_tx_data(struct queue_entry *entry,
@@ -566,13 +578,10 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 	return 0;
 }
 
-int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
-			      struct ieee80211_vif *vif,
-			      const bool enable_beacon)
+int rt2x00queue_clear_beacon(struct rt2x00_dev *rt2x00dev,
+			     struct ieee80211_vif *vif)
 {
 	struct rt2x00_intf *intf = vif_to_intf(vif);
-	struct skb_frame_desc *skbdesc;
-	struct txentry_desc txdesc;
 
 	if (unlikely(!intf->beacon))
 		return -ENOBUFS;
@@ -584,17 +593,36 @@ int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
 	 */
 	rt2x00queue_free_skb(intf->beacon);
 
-	if (!enable_beacon) {
-		rt2x00queue_stop_queue(intf->beacon->queue);
-		mutex_unlock(&intf->beacon_skb_mutex);
-		return 0;
-	}
+	/*
+	 * Clear beacon (single bssid devices don't need to clear the beacon
+	 * since the beacon queue will get stopped anyway).
+	 */
+	if (rt2x00dev->ops->lib->clear_beacon)
+		rt2x00dev->ops->lib->clear_beacon(intf->beacon);
+
+	mutex_unlock(&intf->beacon_skb_mutex);
+
+	return 0;
+}
+
+int rt2x00queue_update_beacon_locked(struct rt2x00_dev *rt2x00dev,
+				     struct ieee80211_vif *vif)
+{
+	struct rt2x00_intf *intf = vif_to_intf(vif);
+	struct skb_frame_desc *skbdesc;
+	struct txentry_desc txdesc;
+
+	if (unlikely(!intf->beacon))
+		return -ENOBUFS;
+
+	/*
+	 * Clean up the beacon skb.
+	 */
+	rt2x00queue_free_skb(intf->beacon);
 
 	intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
-	if (!intf->beacon->skb) {
-		mutex_unlock(&intf->beacon_skb_mutex);
+	if (!intf->beacon->skb)
 		return -ENOMEM;
-	}
 
 	/*
 	 * Copy all TX descriptor information into txdesc,
@@ -611,13 +639,25 @@ int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
 	skbdesc->entry = intf->beacon;
 
 	/*
-	 * Send beacon to hardware and enable beacon genaration..
+	 * Send beacon to hardware.
 	 */
 	rt2x00dev->ops->lib->write_beacon(intf->beacon, &txdesc);
 
+	return 0;
+
+}
+
+int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
+			      struct ieee80211_vif *vif)
+{
+	struct rt2x00_intf *intf = vif_to_intf(vif);
+	int ret;
+
+	mutex_lock(&intf->beacon_skb_mutex);
+	ret = rt2x00queue_update_beacon_locked(rt2x00dev, vif);
 	mutex_unlock(&intf->beacon_skb_mutex);
 
-	return 0;
+	return ret;
 }
 
 void rt2x00queue_for_each_entry(struct data_queue *queue,
@@ -665,29 +705,6 @@ void rt2x00queue_for_each_entry(struct data_queue *queue,
 }
 EXPORT_SYMBOL_GPL(rt2x00queue_for_each_entry);
 
-struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
-					 const enum data_queue_qid queue)
-{
-	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
-
-	if (queue == QID_RX)
-		return rt2x00dev->rx;
-
-	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
-		return &rt2x00dev->tx[queue];
-
-	if (!rt2x00dev->bcn)
-		return NULL;
-
-	if (queue == QID_BEACON)
-		return &rt2x00dev->bcn[0];
-	else if (queue == QID_ATIM && atim)
-		return &rt2x00dev->bcn[1];
-
-	return NULL;
-}
-EXPORT_SYMBOL_GPL(rt2x00queue_get_queue);
-
 struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
 					  enum queue_index index)
 {
@@ -885,7 +902,7 @@ void rt2x00queue_flush_queue(struct data_queue *queue, bool drop)
 	 * The queue flush has failed...
 	 */
 	if (unlikely(!rt2x00queue_empty(queue)))
-		WARNING(queue->rt2x00dev, "Queue %d failed to flush", queue->qid);
+		WARNING(queue->rt2x00dev, "Queue %d failed to flush\n", queue->qid);
 
 	/*
 	 * Restore the queue to the previous status
@@ -1063,7 +1080,7 @@ int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
 		goto exit;
 
 	if (test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags)) {
-		status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
+		status = rt2x00queue_alloc_entries(rt2x00dev->atim,
 						   rt2x00dev->ops->atim);
 		if (status)
 			goto exit;
@@ -1137,6 +1154,7 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	rt2x00dev->rx = queue;
 	rt2x00dev->tx = &queue[1];
 	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
+	rt2x00dev->atim = req_atim ? &queue[2 + rt2x00dev->ops->tx_queues] : NULL;
 
 	/*
 	 * Initialize queue parameters.
@@ -1153,9 +1171,9 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	tx_queue_for_each(rt2x00dev, queue)
 		rt2x00queue_init(rt2x00dev, queue, qid++);
 
-	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_BEACON);
+	rt2x00queue_init(rt2x00dev, rt2x00dev->bcn, QID_BEACON);
 	if (req_atim)
-		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_ATIM);
+		rt2x00queue_init(rt2x00dev, rt2x00dev->atim, QID_ATIM);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index fab8e2687f29..0c8b0c699679 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -305,20 +305,27 @@ struct txentry_desc {
 	u16 length;
 	u16 header_length;
 
-	u16 length_high;
-	u16 length_low;
-	u16 signal;
-	u16 service;
-
-	u16 mcs;
-	u16 stbc;
-	u16 ba_size;
-	u16 rate_mode;
-	u16 mpdu_density;
+	union {
+		struct {
+			u16 length_high;
+			u16 length_low;
+			u16 signal;
+			u16 service;
+			enum ifs ifs;
+		} plcp;
+
+		struct {
+			u16 mcs;
+			u8 stbc;
+			u8 ba_size;
+			u8 mpdu_density;
+			enum txop txop;
+		} ht;
+	} u;
+
+	enum rate_modulation rate_mode;
 
 	short retry_limit;
-	short ifs;
-	short txop;
 
 	enum cipher cipher;
 	u16 key_idx;
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/rt2x00/rt2x00reg.h
index e8259ae48ced..6f867eec49cc 100644
--- a/drivers/net/wireless/rt2x00/rt2x00reg.h
+++ b/drivers/net/wireless/rt2x00/rt2x00reg.h
@@ -85,8 +85,6 @@ enum dev_state {
 	STATE_RADIO_OFF,
 	STATE_RADIO_IRQ_ON,
 	STATE_RADIO_IRQ_OFF,
-	STATE_RADIO_IRQ_ON_ISR,
-	STATE_RADIO_IRQ_OFF_ISR,
 };
 
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 1a9937d5aff6..fbe735f5b352 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -227,7 +227,7 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 	 * Schedule the delayed work for reading the TX status
 	 * from the device.
 	 */
-	ieee80211_queue_work(rt2x00dev->hw, &rt2x00dev->txdone_work);
+	queue_work(rt2x00dev->workqueue, &rt2x00dev->txdone_work);
 }
 
 static void rt2x00usb_kick_tx_entry(struct queue_entry *entry)
@@ -320,7 +320,7 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
 	 * Schedule the delayed work for reading the RX status
 	 * from the device.
 	 */
-	ieee80211_queue_work(rt2x00dev->hw, &rt2x00dev->rxdone_work);
+	queue_work(rt2x00dev->workqueue, &rt2x00dev->rxdone_work);
 }
 
 static void rt2x00usb_kick_rx_entry(struct queue_entry *entry)
@@ -429,7 +429,7 @@ void rt2x00usb_flush_queue(struct data_queue *queue)
 		 * Schedule the completion handler manually, when this
 		 * worker function runs, it should cleanup the queue.
 		 */
-		ieee80211_queue_work(queue->rt2x00dev->hw, completion);
+		queue_work(queue->rt2x00dev->workqueue, completion);
 
 		/*
 		 * Wait for a little while to give the driver
@@ -453,7 +453,7 @@ static void rt2x00usb_watchdog_tx_status(struct data_queue *queue)
 	WARNING(queue->rt2x00dev, "TX queue %d status timed out,"
 		" invoke forced tx handler\n", queue->qid);
 
-	ieee80211_queue_work(queue->rt2x00dev->hw, &queue->rt2x00dev->txdone_work);
+	queue_work(queue->rt2x00dev->workqueue, &queue->rt2x00dev->txdone_work);
 }
 
 void rt2x00usb_watchdog(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 8de44dd401e0..77e8113b91e1 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -551,26 +551,14 @@ static void rt61pci_config_intf(struct rt2x00_dev *rt2x00dev,
 				struct rt2x00intf_conf *conf,
 				const unsigned int flags)
 {
-	unsigned int beacon_base;
 	u32 reg;
 
 	if (flags & CONFIG_UPDATE_TYPE) {
 		/*
-		 * Clear current synchronisation setup.
-		 * For the Beacon base registers, we only need to clear
-		 * the first byte since that byte contains the VALID and OWNER
-		 * bits which (when set to 0) will invalidate the entire beacon.
-		 */
-		beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
-		rt2x00pci_register_write(rt2x00dev, beacon_base, 0);
-
-		/*
 		 * Enable synchronisation.
 		 */
 		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
-		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, conf->sync);
-		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
 
@@ -1154,6 +1142,11 @@ static void rt61pci_start_queue(struct data_queue *queue)
 		rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
+		/*
+		 * Allow the tbtt tasklet to be scheduled.
+		 */
+		tasklet_enable(&rt2x00dev->tbtt_tasklet);
+
 		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
@@ -1233,6 +1226,11 @@ static void rt61pci_stop_queue(struct data_queue *queue)
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+
+		/*
+		 * Wait for possibly running tbtt tasklets.
+		 */
+		tasklet_disable(&rt2x00dev->tbtt_tasklet);
 		break;
 	default:
 		break;
@@ -1719,9 +1717,9 @@ static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 			       enum dev_state state)
 {
-	int mask = (state == STATE_RADIO_IRQ_OFF) ||
-		   (state == STATE_RADIO_IRQ_OFF_ISR);
+	int mask = (state == STATE_RADIO_IRQ_OFF);
 	u32 reg;
+	unsigned long flags;
 
 	/*
 	 * When interrupts are being enabled, the interrupt registers
@@ -1733,12 +1731,21 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 
 		rt2x00pci_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg);
 		rt2x00pci_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg);
+
+		/*
+		 * Enable tasklets.
+		 */
+		tasklet_enable(&rt2x00dev->txstatus_tasklet);
+		tasklet_enable(&rt2x00dev->rxdone_tasklet);
+		tasklet_enable(&rt2x00dev->autowake_tasklet);
 	}
 
 	/*
 	 * Only toggle the interrupts bits we are going to use.
 	 * Non-checked interrupt bits are disabled by default.
 	 */
+	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
+
 	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_TXDONE, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_RXDONE, mask);
@@ -1758,6 +1765,17 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_7, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_TWAKEUP, mask);
 	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
+
+	spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
+
+	if (state == STATE_RADIO_IRQ_OFF) {
+		/*
+		 * Ensure that all tasklets are finished.
+		 */
+		tasklet_disable(&rt2x00dev->txstatus_tasklet);
+		tasklet_disable(&rt2x00dev->rxdone_tasklet);
+		tasklet_disable(&rt2x00dev->autowake_tasklet);
+	}
 }
 
 static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
@@ -1833,9 +1851,7 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt61pci_disable_radio(rt2x00dev);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		rt61pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
@@ -1882,10 +1898,12 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_desc_write(txd, 1, word);
 
 	rt2x00_desc_read(txd, 2, &word);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->signal);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->service);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW, txdesc->length_low);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH, txdesc->length_high);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->u.plcp.signal);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->u.plcp.service);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW,
+			   txdesc->u.plcp.length_low);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH,
+			   txdesc->u.plcp.length_high);
 	rt2x00_desc_write(txd, 2, word);
 
 	if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc->flags)) {
@@ -1930,7 +1948,7 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_OFDM,
 			   (txdesc->rate_mode == RATE_MODE_OFDM));
-	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->u.plcp.ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
 			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_TKIP_MIC,
@@ -1962,13 +1980,14 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	unsigned int beacon_base;
 	unsigned int padding_len;
-	u32 reg;
+	u32 orig_reg, reg;
 
 	/*
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
 	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	orig_reg = reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
 
@@ -1986,7 +2005,14 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	 * Write entire beacon with descriptor and padding to register.
 	 */
 	padding_len = roundup(entry->skb->len, 4) - entry->skb->len;
-	skb_pad(entry->skb, padding_len);
+	if (padding_len && skb_pad(entry->skb, padding_len)) {
+		ERROR(rt2x00dev, "Failure padding beacon, aborting\n");
+		/* skb freed by skb_pad() on failure */
+		entry->skb = NULL;
+		rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, orig_reg);
+		return;
+	}
+
 	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
 	rt2x00pci_register_multiwrite(rt2x00dev, beacon_base,
 				      entry_priv->desc, TXINFO_SIZE);
@@ -2002,8 +2028,6 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	 */
 	rt2x00pci_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
 
-	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
-	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
 	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
 
@@ -2014,6 +2038,32 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	entry->skb = NULL;
 }
 
+static void rt61pci_clear_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	u32 reg;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+
+	/*
+	 * Clear beacon.
+	 */
+	rt2x00pci_register_write(rt2x00dev,
+				 HW_BEACON_OFFSET(entry->entry_idx), 0);
+
+	/*
+	 * Enable beaconing again.
+	 */
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+}
+
 /*
  * RX control handlers
  */
@@ -2078,9 +2128,8 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
 		rxdesc->flags |= RX_FLAG_IV_STRIPPED;
 
 		/*
-		 * FIXME: Legacy driver indicates that the frame does
-		 * contain the Michael Mic. Unfortunately, in rt2x00
-		 * the MIC seems to be missing completely...
+		 * The hardware has already checked the Michael Mic and has
+		 * stripped it from the frame. Signal this to mac80211.
 		 */
 		rxdesc->flags |= RX_FLAG_MMIC_STRIPPED;
 
@@ -2143,7 +2192,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 		 * queue identication number.
 		 */
 		type = rt2x00_get_field32(reg, STA_CSR4_PID_TYPE);
-		queue = rt2x00queue_get_queue(rt2x00dev, type);
+		queue = rt2x00queue_get_tx_queue(rt2x00dev, type);
 		if (unlikely(!queue))
 			continue;
 
@@ -2211,61 +2260,77 @@ static void rt61pci_wakeup(struct rt2x00_dev *rt2x00dev)
 	rt61pci_config(rt2x00dev, &libconf, IEEE80211_CONF_CHANGE_PS);
 }
 
-static irqreturn_t rt61pci_interrupt_thread(int irq, void *dev_instance)
+static void rt61pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
+				     struct rt2x00_field32 irq_field)
 {
-	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg = rt2x00dev->irqvalue[0];
-	u32 reg_mcu = rt2x00dev->irqvalue[1];
+	u32 reg;
 
 	/*
-	 * Handle interrupts, walk through all bits
-	 * and run the tasks, the bits are checked in order of
-	 * priority.
+	 * Enable a single interrupt. The interrupt mask register
+	 * access needs locking.
 	 */
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	/*
-	 * 1 - Rx ring done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RXDONE))
-		rt2x00pci_rxdone(rt2x00dev);
+	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00_set_field32(&reg, irq_field, 0);
+	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
-	/*
-	 * 2 - Tx ring done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TXDONE))
-		rt61pci_txdone(rt2x00dev);
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
 
-	/*
-	 * 3 - Handle MCU command done.
-	 */
-	if (reg_mcu)
-		rt2x00pci_register_write(rt2x00dev,
-					 M2H_CMD_DONE_CSR, 0xffffffff);
+static void rt61pci_enable_mcu_interrupt(struct rt2x00_dev *rt2x00dev,
+					 struct rt2x00_field32 irq_field)
+{
+	u32 reg;
 
 	/*
-	 * 4 - MCU Autowakeup interrupt.
+	 * Enable a single MCU interrupt. The interrupt mask register
+	 * access needs locking.
 	 */
-	if (rt2x00_get_field32(reg_mcu, MCU_INT_SOURCE_CSR_TWAKEUP))
-		rt61pci_wakeup(rt2x00dev);
+	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	/*
-	 * 5 - Beacon done interrupt.
-	 */
-	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_BEACON_DONE))
-		rt2x00lib_beacondone(rt2x00dev);
+	rt2x00pci_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	rt2x00_set_field32(&reg, irq_field, 0);
+	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
 
-	/* Enable interrupts again. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_ON_ISR);
-	return IRQ_HANDLED;
+	spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
+
+static void rt61pci_txstatus_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt61pci_txdone(rt2x00dev);
+	rt61pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_TXDONE);
+}
+
+static void rt61pci_tbtt_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00lib_beacondone(rt2x00dev);
+	rt61pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_BEACON_DONE);
+}
+
+static void rt61pci_rxdone_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt2x00pci_rxdone(rt2x00dev);
+	rt61pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_RXDONE);
 }
 
+static void rt61pci_autowake_tasklet(unsigned long data)
+{
+	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	rt61pci_wakeup(rt2x00dev);
+	rt2x00pci_register_write(rt2x00dev,
+				 M2H_CMD_DONE_CSR, 0xffffffff);
+	rt61pci_enable_mcu_interrupt(rt2x00dev, MCU_INT_MASK_CSR_TWAKEUP);
+}
 
 static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 {
 	struct rt2x00_dev *rt2x00dev = dev_instance;
-	u32 reg_mcu;
-	u32 reg;
+	u32 reg_mcu, mask_mcu;
+	u32 reg, mask;
 
 	/*
 	 * Get the interrupt sources & saved to local variable.
@@ -2283,14 +2348,46 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 	if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		return IRQ_HANDLED;
 
-	/* Store irqvalues for use in the interrupt thread. */
-	rt2x00dev->irqvalue[0] = reg;
-	rt2x00dev->irqvalue[1] = reg_mcu;
+	/*
+	 * Schedule tasklets for interrupt handling.
+	 */
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RXDONE))
+		tasklet_schedule(&rt2x00dev->rxdone_tasklet);
+
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TXDONE))
+		tasklet_schedule(&rt2x00dev->txstatus_tasklet);
+
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_BEACON_DONE))
+		tasklet_hi_schedule(&rt2x00dev->tbtt_tasklet);
+
+	if (rt2x00_get_field32(reg_mcu, MCU_INT_SOURCE_CSR_TWAKEUP))
+		tasklet_schedule(&rt2x00dev->autowake_tasklet);
+
+	/*
+	 * Since INT_MASK_CSR and INT_SOURCE_CSR use the same bits
+	 * for interrupts and interrupt masks we can just use the value of
+	 * INT_SOURCE_CSR to create the interrupt mask.
+	 */
+	mask = reg;
+	mask_mcu = reg_mcu;
 
-	/* Disable interrupts, will be enabled again in the interrupt thread. */
-	rt2x00dev->ops->lib->set_device_state(rt2x00dev,
-					      STATE_RADIO_IRQ_OFF_ISR);
-	return IRQ_WAKE_THREAD;
+	/*
+	 * Disable all interrupts for which a tasklet was scheduled right now,
+	 * the tasklet will reenable the appropriate interrupts.
+	 */
+	spin_lock(&rt2x00dev->irqmask_lock);
+
+	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg |= mask;
+	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	reg |= mask_mcu;
+	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
+
+	spin_unlock(&rt2x00dev->irqmask_lock);
+
+	return IRQ_HANDLED;
 }
 
 /*
@@ -2819,7 +2916,7 @@ static int rt61pci_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
 	if (queue_idx >= 4)
 		return 0;
 
-	queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+	queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
 
 	/* Update WMM TXOP register */
 	offset = AC_TXOP_CSR0 + (sizeof(u32) * (!!(queue_idx & 2)));
@@ -2884,7 +2981,10 @@ static const struct ieee80211_ops rt61pci_mac80211_ops = {
 
 static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
 	.irq_handler		= rt61pci_interrupt,
-	.irq_handler_thread	= rt61pci_interrupt_thread,
+	.txstatus_tasklet	= rt61pci_txstatus_tasklet,
+	.tbtt_tasklet		= rt61pci_tbtt_tasklet,
+	.rxdone_tasklet		= rt61pci_rxdone_tasklet,
+	.autowake_tasklet	= rt61pci_autowake_tasklet,
 	.probe_hw		= rt61pci_probe_hw,
 	.get_firmware_name	= rt61pci_get_firmware_name,
 	.check_firmware		= rt61pci_check_firmware,
@@ -2903,6 +3003,7 @@ static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
 	.stop_queue		= rt61pci_stop_queue,
 	.write_tx_desc		= rt61pci_write_tx_desc,
 	.write_beacon		= rt61pci_write_beacon,
+	.clear_beacon		= rt61pci_clear_beacon,
 	.fill_rxdone		= rt61pci_fill_rxdone,
 	.config_shared_key	= rt61pci_config_shared_key,
 	.config_pairwise_key	= rt61pci_config_pairwise_key,
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 029be3c6c030..02f1148c577e 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -502,26 +502,14 @@ static void rt73usb_config_intf(struct rt2x00_dev *rt2x00dev,
 				struct rt2x00intf_conf *conf,
 				const unsigned int flags)
 {
-	unsigned int beacon_base;
 	u32 reg;
 
 	if (flags & CONFIG_UPDATE_TYPE) {
 		/*
-		 * Clear current synchronisation setup.
-		 * For the Beacon base registers we only need to clear
-		 * the first byte since that byte contains the VALID and OWNER
-		 * bits which (when set to 0) will invalidate the entire beacon.
-		 */
-		beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
-		rt2x00usb_register_write(rt2x00dev, beacon_base, 0);
-
-		/*
 		 * Enable synchronisation.
 		 */
 		rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
-		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, conf->sync);
-		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 		rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
 
@@ -1440,9 +1428,7 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		rt73usb_disable_radio(rt2x00dev);
 		break;
 	case STATE_RADIO_IRQ_ON:
-	case STATE_RADIO_IRQ_ON_ISR:
 	case STATE_RADIO_IRQ_OFF:
-	case STATE_RADIO_IRQ_OFF_ISR:
 		/* No support, but no error either */
 		break;
 	case STATE_DEEP_SLEEP:
@@ -1488,7 +1474,7 @@ static void rt73usb_write_tx_desc(struct queue_entry *entry,
 			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_OFDM,
 			   (txdesc->rate_mode == RATE_MODE_OFDM));
-	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->u.plcp.ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
 			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_TKIP_MIC,
@@ -1513,10 +1499,12 @@ static void rt73usb_write_tx_desc(struct queue_entry *entry,
 	rt2x00_desc_write(txd, 1, word);
 
 	rt2x00_desc_read(txd, 2, &word);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->signal);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->service);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW, txdesc->length_low);
-	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH, txdesc->length_high);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->u.plcp.signal);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->u.plcp.service);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW,
+			   txdesc->u.plcp.length_low);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH,
+			   txdesc->u.plcp.length_high);
 	rt2x00_desc_write(txd, 2, word);
 
 	if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc->flags)) {
@@ -1547,13 +1535,14 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	unsigned int beacon_base;
 	unsigned int padding_len;
-	u32 reg;
+	u32 orig_reg, reg;
 
 	/*
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
 	rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	orig_reg = reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 
@@ -1577,7 +1566,14 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	 * Write entire beacon with descriptor and padding to register.
 	 */
 	padding_len = roundup(entry->skb->len, 4) - entry->skb->len;
-	skb_pad(entry->skb, padding_len);
+	if (padding_len && skb_pad(entry->skb, padding_len)) {
+		ERROR(rt2x00dev, "Failure padding beacon, aborting\n");
+		/* skb freed by skb_pad() on failure */
+		entry->skb = NULL;
+		rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, orig_reg);
+		return;
+	}
+
 	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
 	rt2x00usb_register_multiwrite(rt2x00dev, beacon_base, entry->skb->data,
 				      entry->skb->len + padding_len);
@@ -1590,8 +1586,6 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	 */
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
 
-	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
-	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 
@@ -1602,6 +1596,33 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	entry->skb = NULL;
 }
 
+static void rt73usb_clear_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	unsigned int beacon_base;
+	u32 reg;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+
+	/*
+	 * Clear beacon.
+	 */
+	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
+	rt2x00usb_register_write(rt2x00dev, beacon_base, 0);
+
+	/*
+	 * Enable beaconing again.
+	 */
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
+	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+}
+
 static int rt73usb_get_tx_data_len(struct queue_entry *entry)
 {
 	int length;
@@ -1698,9 +1719,8 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 		rxdesc->flags |= RX_FLAG_IV_STRIPPED;
 
 		/*
-		 * FIXME: Legacy driver indicates that the frame does
-		 * contain the Michael Mic. Unfortunately, in rt2x00
-		 * the MIC seems to be missing completely...
+		 * The hardware has already checked the Michael Mic and has
+		 * stripped it from the frame. Signal this to mac80211.
 		 */
 		rxdesc->flags |= RX_FLAG_MMIC_STRIPPED;
 
@@ -2229,7 +2249,7 @@ static int rt73usb_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
 	if (queue_idx >= 4)
 		return 0;
 
-	queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+	queue = rt2x00queue_get_tx_queue(rt2x00dev, queue_idx);
 
 	/* Update WMM TXOP register */
 	offset = AC_TXOP_CSR0 + (sizeof(u32) * (!!(queue_idx & 2)));
@@ -2313,6 +2333,7 @@ static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
 	.flush_queue		= rt2x00usb_flush_queue,
 	.write_tx_desc		= rt73usb_write_tx_desc,
 	.write_beacon		= rt73usb_write_beacon,
+	.clear_beacon		= rt73usb_clear_beacon,
 	.get_tx_data_len	= rt73usb_get_tx_data_len,
 	.fill_rxdone		= rt73usb_fill_rxdone,
 	.config_shared_key	= rt73usb_config_shared_key,
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 5851cbc1e957..80db5cabc9b9 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -146,7 +146,7 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 			rx_status.freq = dev->conf.channel->center_freq;
 			rx_status.band = dev->conf.channel->band;
 			rx_status.mactime = le64_to_cpu(entry->tsft);
-			rx_status.flag |= RX_FLAG_TSFT;
+			rx_status.flag |= RX_FLAG_MACTIME_MPDU;
 			if (flags & RTL818X_RX_DESC_FLAG_CRC32_ERR)
 				rx_status.flag |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -240,7 +240,7 @@ static irqreturn_t rtl8180_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+static void rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
@@ -321,8 +321,6 @@ static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	rtl818x_iowrite8(priv, &priv->map->TX_DMA_POLLING, (1 << (prio + 4)));
-
-	return 0;
 }
 
 void rtl8180_set_anaparam(struct rtl8180_priv *priv, u32 anaparam)
@@ -687,7 +685,6 @@ static void rtl8180_beacon_work(struct work_struct *work)
 	struct ieee80211_hw *dev = vif_priv->dev;
 	struct ieee80211_mgmt *mgmt;
 	struct sk_buff *skb;
-	int err = 0;
 
 	/* don't overflow the tx ring */
 	if (ieee80211_queue_stopped(dev, 0))
@@ -708,8 +705,7 @@ static void rtl8180_beacon_work(struct work_struct *work)
 	/* TODO: use actual beacon queue */
 	skb_set_queue_mapping(skb, 0);
 
-	err = rtl8180_tx(dev, skb);
-	WARN_ON(err);
+	rtl8180_tx(dev, skb);
 
 resched:
 	/*
diff --git a/drivers/net/wireless/rtl818x/rtl8187/dev.c b/drivers/net/wireless/rtl818x/rtl8187/dev.c
index 6b82cac37ee3..1e0be14d10d4 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/dev.c
@@ -227,7 +227,7 @@ static void rtl8187_tx_cb(struct urb *urb)
 	}
 }
 
-static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+static void rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct rtl8187_priv *priv = dev->priv;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -241,7 +241,7 @@ static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!urb) {
 		kfree_skb(skb);
-		return NETDEV_TX_OK;
+		return;
 	}
 
 	flags = skb->len;
@@ -309,8 +309,6 @@ static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		kfree_skb(skb);
 	}
 	usb_free_urb(urb);
-
-	return NETDEV_TX_OK;
 }
 
 static void rtl8187_rx_cb(struct urb *urb)
@@ -373,7 +371,7 @@ static void rtl8187_rx_cb(struct urb *urb)
 	rx_status.rate_idx = rate;
 	rx_status.freq = dev->conf.channel->center_freq;
 	rx_status.band = dev->conf.channel->band;
-	rx_status.flag |= RX_FLAG_TSFT;
+	rx_status.flag |= RX_FLAG_MACTIME_MPDU;
 	if (flags & RTL818X_RX_DESC_FLAG_CRC32_ERR)
 		rx_status.flag |= RX_FLAG_FAILED_FCS_CRC;
 	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
@@ -871,23 +869,35 @@ static void rtl8187_work(struct work_struct *work)
 	/* The RTL8187 returns the retry count through register 0xFFFA. In
 	 * addition, it appears to be a cumulative retry count, not the
 	 * value for the current TX packet. When multiple TX entries are
-	 * queued, the retry count will be valid for the last one in the queue.
-	 * The "error" should not matter for purposes of rate setting. */
+	 * waiting in the queue, the retry count will be the total for all.
+	 * The "error" may matter for purposes of rate setting, but there is
+	 * no other choice with this hardware.
+	 */
 	struct rtl8187_priv *priv = container_of(work, struct rtl8187_priv,
 				    work.work);
 	struct ieee80211_tx_info *info;
 	struct ieee80211_hw *dev = priv->dev;
 	static u16 retry;
 	u16 tmp;
+	u16 avg_retry;
+	int length;
 
 	mutex_lock(&priv->conf_mutex);
 	tmp = rtl818x_ioread16(priv, (__le16 *)0xFFFA);
+	length = skb_queue_len(&priv->b_tx_status.queue);
+	if (unlikely(!length))
+		length = 1;
+	if (unlikely(tmp < retry))
+		tmp = retry;
+	avg_retry = (tmp - retry) / length;
 	while (skb_queue_len(&priv->b_tx_status.queue) > 0) {
 		struct sk_buff *old_skb;
 
 		old_skb = skb_dequeue(&priv->b_tx_status.queue);
 		info = IEEE80211_SKB_CB(old_skb);
-		info->status.rates[0].count = tmp - retry + 1;
+		info->status.rates[0].count = avg_retry + 1;
+		if (info->status.rates[0].count > RETRY_COUNT)
+			info->flags &= ~IEEE80211_TX_STAT_ACK;
 		ieee80211_tx_status_irqsafe(dev, old_skb);
 	}
 	retry = tmp;
@@ -933,8 +943,8 @@ static int rtl8187_start(struct ieee80211_hw *dev)
 		rtl818x_iowrite32(priv, &priv->map->TX_CONF,
 				  RTL818X_TX_CONF_HW_SEQNUM |
 				  RTL818X_TX_CONF_DISREQQSIZE |
-				  (7 << 8  /* short retry limit */) |
-				  (7 << 0  /* long retry limit */) |
+				  (RETRY_COUNT << 8  /* short retry limit */) |
+				  (RETRY_COUNT << 0  /* long retry limit */) |
 				  (7 << 21 /* MAX TX DMA */));
 		rtl8187_init_urbs(dev);
 		rtl8187b_init_status_urb(dev);
@@ -1378,6 +1388,9 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
 		     IEEE80211_HW_SIGNAL_DBM |
 		     IEEE80211_HW_RX_INCLUDES_FCS;
+	/* Initialize rate-control variables */
+	dev->max_rates = 1;
+	dev->max_rate_tries = RETRY_COUNT;
 
 	eeprom.data = dev;
 	eeprom.register_read = rtl8187_eeprom_register_read;
diff --git a/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h b/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h
index 0d7b1423f77b..f1cc90751dbf 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h
+++ b/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h
@@ -35,6 +35,8 @@
 #define RFKILL_MASK_8187_89_97	0x2
 #define RFKILL_MASK_8198	0x4
 
+#define RETRY_COUNT		7
+
 struct rtl8187_rx_info {
 	struct urb *urb;
 	struct ieee80211_hw *dev;
diff --git a/drivers/net/wireless/rtlwifi/Kconfig b/drivers/net/wireless/rtlwifi/Kconfig
index 7f6573f7f470..ce49e0ce7cad 100644
--- a/drivers/net/wireless/rtlwifi/Kconfig
+++ b/drivers/net/wireless/rtlwifi/Kconfig
@@ -1,15 +1,33 @@
 config RTL8192CE
-	tristate "Realtek RTL8192CE/RTL8188SE Wireless Network Adapter"
-	depends on MAC80211 && EXPERIMENTAL
+	tristate "Realtek RTL8192CE/RTL8188CE Wireless Network Adapter"
+	depends on MAC80211 && PCI && EXPERIMENTAL
 	select FW_LOADER
 	select RTLWIFI
+	select RTL8192C_COMMON
 	---help---
 	This is the driver for Realtek RTL8192CE/RTL8188CE 802.11n PCIe
 	wireless network adapters.
 
 	If you choose to build it as a module, it will be called rtl8192ce
 
+config RTL8192CU
+	tristate "Realtek RTL8192CU/RTL8188CU USB Wireless Network Adapter"
+	depends on MAC80211 && USB && EXPERIMENTAL
+	select FW_LOADER
+	select RTLWIFI
+	select RTL8192C_COMMON
+	---help---
+	This is the driver for Realtek RTL8192CU/RTL8188CU 802.11n USB
+	wireless network adapters.
+
+	If you choose to build it as a module, it will be called rtl8192cu
+
 config RTLWIFI
 	tristate
-	depends on RTL8192CE
+	depends on RTL8192CE || RTL8192CU
+	default m
+
+config RTL8192C_COMMON
+	tristate
+	depends on RTL8192CE || RTL8192CU
 	default m
diff --git a/drivers/net/wireless/rtlwifi/Makefile b/drivers/net/wireless/rtlwifi/Makefile
index 2a7a4384f8ee..ec9393f24799 100644
--- a/drivers/net/wireless/rtlwifi/Makefile
+++ b/drivers/net/wireless/rtlwifi/Makefile
@@ -5,9 +5,22 @@ rtlwifi-objs	:=		\
 		core.o		\
 		debug.o		\
 		efuse.o		\
-		pci.o		\
 		ps.o		\
 		rc.o		\
 		regd.o
 
+rtl8192c_common-objs +=		\
+
+ifneq ($(CONFIG_PCI),)
+rtlwifi-objs	+= pci.o
+endif
+
+ifneq ($(CONFIG_USB),)
+rtlwifi-objs	+= usb.o
+endif
+
+obj-$(CONFIG_RTL8192C_COMMON)	+= rtl8192c/
 obj-$(CONFIG_RTL8192CE)		+= rtl8192ce/
+obj-$(CONFIG_RTL8192CU)		+= rtl8192cu/
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index cf0b73e51fc2..bb0c781f4a1b 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -144,7 +144,7 @@ static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
 		ht_cap->mcs.rx_mask[1] = 0xFF;
 		ht_cap->mcs.rx_mask[4] = 0x01;
 
-		ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS15;
+		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
 	} else if (get_rf_type(rtlphy) == RF_1T1R) {
 
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, ("1T1R\n"));
@@ -153,7 +153,7 @@ static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
 		ht_cap->mcs.rx_mask[1] = 0x00;
 		ht_cap->mcs.rx_mask[4] = 0x01;
 
-		ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS7;
+		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
 	}
 }
 
@@ -283,13 +283,7 @@ int rtl_init_core(struct ieee80211_hw *hw)
 	rtlmac->hw = hw;
 
 	/* <2> rate control register */
-	if (rtl_rate_control_register()) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 ("rtl: Unable to register rtl_rc,"
-			  "use default RC !!\n"));
-	} else {
-		hw->rate_control_algorithm = "rtl_rc";
-	}
+	hw->rate_control_algorithm = "rtl_rc";
 
 	/*
 	 * <3> init CRDA must come after init
@@ -325,8 +319,6 @@ int rtl_init_core(struct ieee80211_hw *hw)
 
 void rtl_deinit_core(struct ieee80211_hw *hw)
 {
-	 /*RC*/
-	rtl_rate_control_unregister();
 }
 
 void rtl_init_rx_config(struct ieee80211_hw *hw)
@@ -399,21 +391,21 @@ static void _rtl_query_protection_mode(struct ieee80211_hw *hw,
 	u8 rate_flag = info->control.rates[0].flags;
 
 	/* Common Settings */
-	tcb_desc->b_rts_stbc = false;
-	tcb_desc->b_cts_enable = false;
+	tcb_desc->rts_stbc = false;
+	tcb_desc->cts_enable = false;
 	tcb_desc->rts_sc = 0;
-	tcb_desc->b_rts_bw = false;
-	tcb_desc->b_rts_use_shortpreamble = false;
-	tcb_desc->b_rts_use_shortgi = false;
+	tcb_desc->rts_bw = false;
+	tcb_desc->rts_use_shortpreamble = false;
+	tcb_desc->rts_use_shortgi = false;
 
 	if (rate_flag & IEEE80211_TX_RC_USE_CTS_PROTECT) {
 		/* Use CTS-to-SELF in protection mode. */
-		tcb_desc->b_rts_enable = true;
-		tcb_desc->b_cts_enable = true;
+		tcb_desc->rts_enable = true;
+		tcb_desc->cts_enable = true;
 		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
 	} else if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
 		/* Use RTS-CTS in protection mode. */
-		tcb_desc->b_rts_enable = true;
+		tcb_desc->rts_enable = true;
 		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
 	}
 
@@ -429,7 +421,7 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 		if (mac->opmode == NL80211_IFTYPE_STATION)
 			tcb_desc->ratr_index = 0;
 		else if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-			if (tcb_desc->b_multicast || tcb_desc->b_broadcast) {
+			if (tcb_desc->multicast || tcb_desc->broadcast) {
 				tcb_desc->hw_rate =
 				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
 				tcb_desc->use_driver_rate = 1;
@@ -439,7 +431,7 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (rtlpriv->dm.b_useramask) {
+	if (rtlpriv->dm.useramask) {
 		/* TODO we will differentiate adhoc and station futrue  */
 		tcb_desc->mac_id = 0;
 
@@ -461,19 +453,19 @@ static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
-	tcb_desc->b_packet_bw = false;
+	tcb_desc->packet_bw = false;
 
 	if (!mac->bw_40 || !mac->ht_enable)
 		return;
 
-	if (tcb_desc->b_multicast || tcb_desc->b_broadcast)
+	if (tcb_desc->multicast || tcb_desc->broadcast)
 		return;
 
 	/*use legency rate, shall use 20MHz */
 	if (tcb_desc->hw_rate <= rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M])
 		return;
 
-	tcb_desc->b_packet_bw = true;
+	tcb_desc->packet_bw = true;
 }
 
 static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw)
@@ -498,7 +490,7 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
 	struct ieee80211_rate *txrate;
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 
 	memset(tcb_desc, 0, sizeof(struct rtl_tcb_desc));
 
@@ -545,9 +537,9 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 		}
 
 		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
-			tcb_desc->b_multicast = 1;
+			tcb_desc->multicast = 1;
 		else if (is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
-			tcb_desc->b_broadcast = 1;
+			tcb_desc->broadcast = 1;
 
 		_rtl_txrate_selectmode(hw, tcb_desc);
 		_rtl_query_bandwidth_mode(hw, tcb_desc);
@@ -570,7 +562,7 @@ bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 
 	if (ieee80211_is_auth(fc)) {
 		RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, ("MAC80211_LINKING\n"));
@@ -587,7 +579,7 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 	u8 *act = (u8 *) (((u8 *) skb->data + MAC80211_3ADDR_LEN));
 	u8 category;
 
@@ -632,7 +624,7 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 	u16 ether_type;
 	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
 	const struct iphdr *ip;
@@ -646,7 +638,6 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 	ip = (struct iphdr *)((u8 *) skb->data + mac_hdr_len +
 			      SNAP_SIZE + PROTOC_TYPE_SIZE);
 	ether_type = *(u16 *) ((u8 *) skb->data + mac_hdr_len + SNAP_SIZE);
-	ether_type = ntohs(ether_type);
 
 	if (ETH_P_IP == ether_type) {
 		if (IPPROTO_UDP == ip->protocol) {
@@ -690,7 +681,8 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 		}
 
 		return true;
-	} else if (0x86DD == ether_type) {
+	} else if (ETH_P_IPV6 == ether_type) {
+		/* IPv6 */
 		return true;
 	}
 
@@ -777,10 +769,10 @@ void rtl_watchdog_wq_callback(void *data)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
-	bool b_busytraffic = false;
-	bool b_higher_busytraffic = false;
-	bool b_higher_busyrxtraffic = false;
-	bool b_higher_busytxtraffic = false;
+	bool busytraffic = false;
+	bool higher_busytraffic = false;
+	bool higher_busyrxtraffic = false;
+	bool higher_busytxtraffic = false;
 
 	u8 idx = 0;
 	u32 rx_cnt_inp4eriod = 0;
@@ -788,7 +780,7 @@ void rtl_watchdog_wq_callback(void *data)
 	u32 aver_rx_cnt_inperiod = 0;
 	u32 aver_tx_cnt_inperiod = 0;
 
-	bool benter_ps = false;
+	bool enter_ps = false;
 
 	if (is_hal_stop(rtlhal))
 		return;
@@ -832,29 +824,29 @@ void rtl_watchdog_wq_callback(void *data)
 
 		/* (2) check traffic busy */
 		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100)
-			b_busytraffic = true;
+			busytraffic = true;
 
 		/* Higher Tx/Rx data. */
 		if (aver_rx_cnt_inperiod > 4000 ||
 		    aver_tx_cnt_inperiod > 4000) {
-			b_higher_busytraffic = true;
+			higher_busytraffic = true;
 
 			/* Extremely high Rx data. */
 			if (aver_rx_cnt_inperiod > 5000)
-				b_higher_busyrxtraffic = true;
+				higher_busyrxtraffic = true;
 			else
-				b_higher_busytxtraffic = false;
+				higher_busytxtraffic = false;
 		}
 
 		if (((rtlpriv->link_info.num_rx_inperiod +
 		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
 		    (rtlpriv->link_info.num_rx_inperiod > 2))
-			benter_ps = false;
+			enter_ps = false;
 		else
-			benter_ps = true;
+			enter_ps = true;
 
 		/* LeisurePS only work in infra mode. */
-		if (benter_ps)
+		if (enter_ps)
 			rtl_lps_enter(hw);
 		else
 			rtl_lps_leave(hw);
@@ -863,9 +855,9 @@ void rtl_watchdog_wq_callback(void *data)
 	rtlpriv->link_info.num_rx_inperiod = 0;
 	rtlpriv->link_info.num_tx_inperiod = 0;
 
-	rtlpriv->link_info.b_busytraffic = b_busytraffic;
-	rtlpriv->link_info.b_higher_busytraffic = b_higher_busytraffic;
-	rtlpriv->link_info.b_higher_busyrxtraffic = b_higher_busyrxtraffic;
+	rtlpriv->link_info.busytraffic = busytraffic;
+	rtlpriv->link_info.higher_busytraffic = higher_busytraffic;
+	rtlpriv->link_info.higher_busyrxtraffic = higher_busyrxtraffic;
 
 }
 
@@ -945,11 +937,16 @@ MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
 
 static int __init rtl_core_module_init(void)
 {
+	if (rtl_rate_control_register())
+		printk(KERN_ERR "rtlwifi: Unable to register rtl_rc,"
+		       "use default RC !!\n");
 	return 0;
 }
 
 static void __exit rtl_core_module_exit(void)
 {
+	 /*RC*/
+	rtl_rate_control_unregister();
 }
 
 module_init(rtl_core_module_init);
diff --git a/drivers/net/wireless/rtlwifi/base.h b/drivers/net/wireless/rtlwifi/base.h
index 3de5a14745f1..043045342bc7 100644
--- a/drivers/net/wireless/rtlwifi/base.h
+++ b/drivers/net/wireless/rtlwifi/base.h
@@ -30,6 +30,7 @@
 #define __RTL_BASE_H__
 
 #define RTL_DUMMY_OFFSET	0
+#define RTL_RX_DESC_SIZE	24
 #define RTL_DUMMY_UNIT		8
 #define RTL_TX_DUMMY_SIZE	(RTL_DUMMY_OFFSET * RTL_DUMMY_UNIT)
 #define RTL_TX_DESC_SIZE	32
@@ -52,46 +53,22 @@
 #define FRAME_OFFSET_SEQUENCE		22
 #define FRAME_OFFSET_ADDRESS4		24
 
-#define SET_80211_HDR_FRAME_CONTROL(_hdr, _val)		\
-	WRITEEF2BYTE(_hdr, _val)
-#define SET_80211_HDR_TYPE_AND_SUBTYPE(_hdr, _val)	\
-	WRITEEF1BYTE(_hdr, _val)
-#define SET_80211_HDR_PWR_MGNT(_hdr, _val)		\
-	SET_BITS_TO_LE_2BYTE(_hdr, 12, 1, _val)
-#define SET_80211_HDR_TO_DS(_hdr, _val)			\
-	SET_BITS_TO_LE_2BYTE(_hdr, 8, 1, _val)
 
 #define SET_80211_PS_POLL_AID(_hdr, _val)		\
-	WRITEEF2BYTE(((u8 *)(_hdr)) + 2, _val)
+	(*(u16 *)((u8 *)(_hdr) + 2) = le16_to_cpu(_val))
 #define SET_80211_PS_POLL_BSSID(_hdr, _val)		\
-	CP_MACADDR(((u8 *)(_hdr)) + 4, (u8 *)(_val))
+	memcpy(((u8 *)(_hdr)) + 4, (u8 *)(_val), ETH_ALEN)
 #define SET_80211_PS_POLL_TA(_hdr, _val)		\
-	CP_MACADDR(((u8 *)(_hdr)) + 10, (u8 *)(_val))
+	memcpy(((u8 *)(_hdr)) + 10, (u8 *)(_val), ETH_ALEN)
 
 #define SET_80211_HDR_DURATION(_hdr, _val)	\
-	WRITEEF2BYTE((u8 *)(_hdr)+FRAME_OFFSET_DURATION, _val)
+	(*(u16 *)((u8 *)(_hdr) + FRAME_OFFSET_DURATION) = le16_to_cpu(_val))
 #define SET_80211_HDR_ADDRESS1(_hdr, _val)	\
-	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS1, (u8*)(_val))
+	memcpy((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS1, (u8*)(_val), ETH_ALEN)
 #define SET_80211_HDR_ADDRESS2(_hdr, _val)	\
-	CP_MACADDR((u8 *)(_hdr) + FRAME_OFFSET_ADDRESS2, (u8 *)(_val))
+	memcpy((u8 *)(_hdr) + FRAME_OFFSET_ADDRESS2, (u8 *)(_val), ETH_ALEN)
 #define SET_80211_HDR_ADDRESS3(_hdr, _val)	\
-	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS3, (u8 *)(_val))
-#define SET_80211_HDR_FRAGMENT_SEQUENCE(_hdr, _val)  \
-	WRITEEF2BYTE((u8 *)(_hdr)+FRAME_OFFSET_SEQUENCE, _val)
-
-#define SET_BEACON_PROBE_RSP_TIME_STAMP_LOW(__phdr, __val)	\
-	WRITEEF4BYTE(((u8 *)(__phdr)) + 24, __val)
-#define SET_BEACON_PROBE_RSP_TIME_STAMP_HIGH(__phdr, __val) \
-	WRITEEF4BYTE(((u8 *)(__phdr)) + 28, __val)
-#define SET_BEACON_PROBE_RSP_BEACON_INTERVAL(__phdr, __val) \
-	WRITEEF2BYTE(((u8 *)(__phdr)) + 32, __val)
-#define GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr)	\
-	READEF2BYTE(((u8 *)(__phdr)) + 34)
-#define SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
-	WRITEEF2BYTE(((u8 *)(__phdr)) + 34, __val)
-#define MASK_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
-	SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, \
-	(GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) & (~(__val))))
+	memcpy((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS3, (u8 *)(_val), ETH_ALEN)
 
 int rtl_init_core(struct ieee80211_hw *hw);
 void rtl_deinit_core(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index d6a924a05654..e4f4aee8f298 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -82,7 +82,7 @@ static void rtl_op_stop(struct ieee80211_hw *hw)
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
 
-static int rtl_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void rtl_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -97,11 +97,10 @@ static int rtl_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	rtlpriv->intf_ops->adapter_tx(hw, skb);
 
-	return NETDEV_TX_OK;
+	return;
 
 err_free:
 	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
 }
 
 static int rtl_op_add_interface(struct ieee80211_hw *hw,
@@ -434,9 +433,9 @@ static int rtl_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
 
 	aci = _rtl_get_hal_qnum(queue);
 	mac->ac[aci].aifs = param->aifs;
-	mac->ac[aci].cw_min = param->cw_min;
-	mac->ac[aci].cw_max = param->cw_max;
-	mac->ac[aci].tx_op = param->txop;
+	mac->ac[aci].cw_min = cpu_to_le16(param->cw_min);
+	mac->ac[aci].cw_max = cpu_to_le16(param->cw_max);
+	mac->ac[aci].tx_op = cpu_to_le16(param->txop);
 	memcpy(&mac->edca_param[aci], param, sizeof(*param));
 	rtlpriv->cfg->ops->set_qos(hw, aci);
 	return 0;
@@ -552,6 +551,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 ("BSS_CHANGED_HT\n"));
 
+		rcu_read_lock();
 		sta = ieee80211_find_sta(mac->vif, mac->bssid);
 
 		if (sta) {
@@ -564,6 +564,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				mac->current_ampdu_factor =
 				    sta->ht_cap.ampdu_factor;
 		}
+		rcu_read_unlock();
 
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
 					      (u8 *) (&mac->max_mss_density));
@@ -615,6 +616,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		else
 			mac->mode = WIRELESS_MODE_G;
 
+		rcu_read_lock();
 		sta = ieee80211_find_sta(mac->vif, mac->bssid);
 
 		if (sta) {
@@ -649,6 +651,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				 */
 			}
 		}
+		rcu_read_unlock();
 
 		/*mac80211 just give us CCK rates any time
 		 *So we add G rate in basic rates when
@@ -666,7 +669,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
 					(u8 *) (&basic_rates));
 
-			if (rtlpriv->dm.b_useramask)
+			if (rtlpriv->dm.useramask)
 				rtlpriv->cfg->ops->update_rate_mask(hw, 0);
 			else
 				rtlpriv->cfg->ops->update_rate_table(hw);
@@ -681,7 +684,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 	 */
 	if (changed & BSS_CHANGED_ASSOC) {
 		if (bss_conf->assoc) {
-			if (ppsc->b_fwctrl_lps) {
+			if (ppsc->fwctrl_lps) {
 				u8 mstatus = RT_MEDIA_CONNECT;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_H2C_FW_JOINBSSRPT,
@@ -689,7 +692,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				ppsc->report_linked = true;
 			}
 		} else {
-			if (ppsc->b_fwctrl_lps) {
+			if (ppsc->fwctrl_lps) {
 				u8 mstatus = RT_MEDIA_DISCONNECT;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_H2C_FW_JOINBSSRPT,
@@ -748,7 +751,8 @@ static void rtl_op_sta_notify(struct ieee80211_hw *hw,
 static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif,
 			       enum ieee80211_ampdu_mlme_action action,
-			       struct ieee80211_sta *sta, u16 tid, u16 * ssn)
+			       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			       u8 buf_size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -817,7 +821,7 @@ static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
 		/* fix fwlps issue */
 		rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
 
-		if (rtlpriv->dm.b_useramask)
+		if (rtlpriv->dm.useramask)
 			rtlpriv->cfg->ops->update_rate_mask(hw, 0);
 		else
 			rtlpriv->cfg->ops->update_rate_table(hw);
diff --git a/drivers/net/wireless/rtlwifi/debug.h b/drivers/net/wireless/rtlwifi/debug.h
index 08bdec2ceda4..e4aa8687408c 100644
--- a/drivers/net/wireless/rtlwifi/debug.h
+++ b/drivers/net/wireless/rtlwifi/debug.h
@@ -105,6 +105,7 @@
 #define COMP_MAC80211		BIT(26)
 #define COMP_REGD			BIT(27)
 #define COMP_CHAN			BIT(28)
+#define COMP_USB			BIT(29)
 
 /*--------------------------------------------------------------
 		Define the rt_print components
diff --git a/drivers/net/wireless/rtlwifi/efuse.c b/drivers/net/wireless/rtlwifi/efuse.c
index 62876cd5c41a..4f92cba6810a 100644
--- a/drivers/net/wireless/rtlwifi/efuse.c
+++ b/drivers/net/wireless/rtlwifi/efuse.c
@@ -1169,21 +1169,3 @@ static u8 efuse_calculate_word_cnts(u8 word_en)
 	return word_cnts;
 }
 
-void efuse_reset_loader(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 tmp_u2b;
-
-	tmp_u2b = rtl_read_word(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN]);
-	rtl_write_word(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN],
-		       (tmp_u2b & ~(BIT(12))));
-	udelay(10000);
-	rtl_write_word(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN],
-		       (tmp_u2b | BIT(12)));
-	udelay(10000);
-}
-
-bool efuse_program_map(struct ieee80211_hw *hw, char *p_filename, u8 tabletype)
-{
-	return true;
-}
diff --git a/drivers/net/wireless/rtlwifi/efuse.h b/drivers/net/wireless/rtlwifi/efuse.h
index 2d39a4df181b..47774dd4c2a6 100644
--- a/drivers/net/wireless/rtlwifi/efuse.h
+++ b/drivers/net/wireless/rtlwifi/efuse.h
@@ -117,8 +117,5 @@ extern bool efuse_shadow_update_chk(struct ieee80211_hw *hw);
 extern void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw);
 extern void efuse_force_write_vendor_Id(struct ieee80211_hw *hw);
 extern void efuse_re_pg_section(struct ieee80211_hw *hw, u8 section_idx);
-extern bool efuse_program_map(struct ieee80211_hw *hw,
-			      char *p_filename, u8 tabletype);
-extern void efuse_reset_loader(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index 1758d4463247..9cd7703c2a30 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -50,7 +50,7 @@ static void _rtl_pci_update_default_setting(struct ieee80211_hw *hw)
 	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
 
 	ppsc->reg_rfps_level = 0;
-	ppsc->b_support_aspm = 0;
+	ppsc->support_aspm = 0;
 
 	/*Update PCI ASPM setting */
 	ppsc->const_amdpci_aspm = rtlpci->const_amdpci_aspm;
@@ -115,29 +115,29 @@ static void _rtl_pci_update_default_setting(struct ieee80211_hw *hw)
 	switch (rtlpci->const_support_pciaspm) {
 	case 0:{
 			/*Not support ASPM. */
-			bool b_support_aspm = false;
-			ppsc->b_support_aspm = b_support_aspm;
+			bool support_aspm = false;
+			ppsc->support_aspm = support_aspm;
 			break;
 		}
 	case 1:{
 			/*Support ASPM. */
-			bool b_support_aspm = true;
-			bool b_support_backdoor = true;
-			ppsc->b_support_aspm = b_support_aspm;
+			bool support_aspm = true;
+			bool support_backdoor = true;
+			ppsc->support_aspm = support_aspm;
 
 			/*if(priv->oem_id == RT_CID_TOSHIBA &&
 			   !priv->ndis_adapter.amd_l1_patch)
-			   b_support_backdoor = false; */
+			   support_backdoor = false; */
 
-			ppsc->b_support_backdoor = b_support_backdoor;
+			ppsc->support_backdoor = support_backdoor;
 
 			break;
 		}
 	case 2:
 		/*ASPM value set by chipset. */
 		if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL) {
-			bool b_support_aspm = true;
-			ppsc->b_support_aspm = b_support_aspm;
+			bool support_aspm = true;
+			ppsc->support_aspm = support_aspm;
 		}
 		break;
 	default:
@@ -476,9 +476,9 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 
 		skb = __skb_dequeue(&ring->queue);
 		pci_unmap_single(rtlpci->pdev,
-				 le32_to_cpu(rtlpriv->cfg->ops->
+				 rtlpriv->cfg->ops->
 					     get_desc((u8 *) entry, true,
-						      HW_DESC_TXBUFF_ADDR)),
+						      HW_DESC_TXBUFF_ADDR),
 				 skb->len, PCI_DMA_TODEVICE);
 
 		RT_TRACE(rtlpriv, (COMP_INTR | COMP_SEND), DBG_TRACE,
@@ -557,7 +557,7 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 			return;
 		} else {
 			struct ieee80211_hdr *hdr;
-			u16 fc;
+			__le16 fc;
 			struct sk_buff *new_skb = NULL;
 
 			rtlpriv->cfg->ops->query_rx_desc(hw, &stats,
@@ -583,9 +583,9 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 			 */
 
 			hdr = (struct ieee80211_hdr *)(skb->data);
-			fc = le16_to_cpu(hdr->frame_control);
+			fc = hdr->frame_control;
 
-			if (!stats.b_crc) {
+			if (!stats.crc) {
 				memcpy(IEEE80211_SKB_RXCB(skb), &rx_status,
 				       sizeof(rx_status));
 
@@ -666,7 +666,7 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 
 		}
 done:
-		bufferaddress = cpu_to_le32(*((dma_addr_t *) skb->cb));
+		bufferaddress = (u32)(*((dma_addr_t *) skb->cb));
 		tmp_one = 1;
 		rtlpriv->cfg->ops->set_desc((u8 *) pdesc, false,
 					    HW_DESC_RXBUFF_ADDR,
@@ -690,75 +690,6 @@ done:
 
 }
 
-void _rtl_pci_tx_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int prio;
-
-	for (prio = 0; prio < RTL_PCI_MAX_TX_QUEUE_COUNT; prio++) {
-		struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
-
-		while (skb_queue_len(&ring->queue)) {
-			struct rtl_tx_desc *entry = &ring->desc[ring->idx];
-			struct sk_buff *skb;
-			struct ieee80211_tx_info *info;
-			u8 own;
-
-			/*
-			 *beacon packet will only use the first
-			 *descriptor defautly, and the own may not
-			 *be cleared by the hardware, and
-			 *beacon will free in prepare beacon
-			 */
-			if (prio == BEACON_QUEUE || prio == TXCMD_QUEUE ||
-			    prio == HCCA_QUEUE)
-				break;
-
-			own = (u8)rtlpriv->cfg->ops->get_desc((u8 *)entry,
-							       true,
-							       HW_DESC_OWN);
-
-			if (own)
-				break;
-
-			skb = __skb_dequeue(&ring->queue);
-			pci_unmap_single(rtlpci->pdev,
-					 le32_to_cpu(rtlpriv->cfg->ops->
-						     get_desc((u8 *) entry,
-						     true,
-						     HW_DESC_TXBUFF_ADDR)),
-					 skb->len, PCI_DMA_TODEVICE);
-
-			ring->idx = (ring->idx + 1) % ring->entries;
-
-			info = IEEE80211_SKB_CB(skb);
-			ieee80211_tx_info_clear_status(info);
-
-			info->flags |= IEEE80211_TX_STAT_ACK;
-			/*info->status.rates[0].count = 1; */
-
-			ieee80211_tx_status_irqsafe(hw, skb);
-
-			if ((ring->entries - skb_queue_len(&ring->queue))
-			    == 2 && prio != BEACON_QUEUE) {
-				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-					 ("more desc left, wake "
-					  "skb_queue@%d,ring->idx = %d,"
-					  "skb_queue_len = 0x%d\n",
-					  prio, ring->idx,
-					  skb_queue_len(&ring->queue)));
-
-				ieee80211_wake_queue(hw,
-						     skb_get_queue_mapping
-						     (skb));
-			}
-
-			skb = NULL;
-		}
-	}
-}
-
 static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 {
 	struct ieee80211_hw *hw = dev_id;
@@ -959,17 +890,17 @@ static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
 	rtlhal->hw = hw;
 	rtlpci->pdev = pdev;
 
-	ppsc->b_inactiveps = false;
-	ppsc->b_leisure_ps = true;
-	ppsc->b_fwctrl_lps = true;
-	ppsc->b_reg_fwctrl_lps = 3;
+	ppsc->inactiveps = false;
+	ppsc->leisure_ps = true;
+	ppsc->fwctrl_lps = true;
+	ppsc->reg_fwctrl_lps = 3;
 	ppsc->reg_max_lps_awakeintvl = 5;
 
-	if (ppsc->b_reg_fwctrl_lps == 1)
+	if (ppsc->reg_fwctrl_lps == 1)
 		ppsc->fwctrl_psmode = FW_PS_MIN_MODE;
-	else if (ppsc->b_reg_fwctrl_lps == 2)
+	else if (ppsc->reg_fwctrl_lps == 2)
 		ppsc->fwctrl_psmode = FW_PS_MAX_MODE;
-	else if (ppsc->b_reg_fwctrl_lps == 3)
+	else if (ppsc->reg_fwctrl_lps == 3)
 		ppsc->fwctrl_psmode = FW_PS_DTIM_MODE;
 
 	/*Tx/Rx related var */
@@ -1024,9 +955,8 @@ static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
 		 ("queue:%d, ring_addr:%p\n", prio, ring));
 
 	for (i = 0; i < entries; i++) {
-		nextdescaddress = cpu_to_le32((u32) dma +
-					      ((i + 1) % entries) *
-					      sizeof(*ring));
+		nextdescaddress = (u32) dma + ((i + 1) % entries) *
+					      sizeof(*ring);
 
 		rtlpriv->cfg->ops->set_desc((u8 *)&(ring[i]),
 					    true, HW_DESC_TX_NEXTDESC_ADDR,
@@ -1090,7 +1020,7 @@ static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw)
 					   rtlpci->rxbuffersize,
 					   PCI_DMA_FROMDEVICE);
 
-			bufferaddress = cpu_to_le32(*((dma_addr_t *)skb->cb));
+			bufferaddress = (u32)(*((dma_addr_t *)skb->cb));
 			rtlpriv->cfg->ops->set_desc((u8 *)entry, false,
 						    HW_DESC_RXBUFF_ADDR,
 						    (u8 *)&bufferaddress);
@@ -1121,9 +1051,9 @@ static void _rtl_pci_free_tx_ring(struct ieee80211_hw *hw,
 		struct sk_buff *skb = __skb_dequeue(&ring->queue);
 
 		pci_unmap_single(rtlpci->pdev,
-				 le32_to_cpu(rtlpriv->cfg->
+				 rtlpriv->cfg->
 					     ops->get_desc((u8 *) entry, true,
-						   HW_DESC_TXBUFF_ADDR)),
+						   HW_DESC_TXBUFF_ADDR),
 				 skb->len, PCI_DMA_TODEVICE);
 		kfree_skb(skb);
 		ring->idx = (ring->idx + 1) % ring->entries;
@@ -1255,11 +1185,11 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 				    __skb_dequeue(&ring->queue);
 
 				pci_unmap_single(rtlpci->pdev,
-						 le32_to_cpu(rtlpriv->cfg->ops->
+						 rtlpriv->cfg->ops->
 							 get_desc((u8 *)
 							 entry,
 							 true,
-							 HW_DESC_TXBUFF_ADDR)),
+							 HW_DESC_TXBUFF_ADDR),
 						 skb->len, PCI_DMA_TODEVICE);
 				kfree_skb(skb);
 				ring->idx = (ring->idx + 1) % ring->entries;
@@ -1273,7 +1203,7 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 	return 0;
 }
 
-unsigned int _rtl_mac_to_hwqueue(u16 fc,
+static unsigned int _rtl_mac_to_hwqueue(__le16 fc,
 		unsigned int mac80211_queue_index)
 {
 	unsigned int hw_queue_index;
@@ -1312,7 +1242,7 @@ out:
 	return hw_queue_index;
 }
 
-int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -1323,7 +1253,7 @@ int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	unsigned int queue_index, hw_queue;
 	unsigned long flags;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 	u8 *pda_addr = hdr->addr1;
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	/*ssn */
@@ -1429,7 +1359,7 @@ int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	return 0;
 }
 
-void rtl_pci_deinit(struct ieee80211_hw *hw)
+static void rtl_pci_deinit(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -1444,7 +1374,7 @@ void rtl_pci_deinit(struct ieee80211_hw *hw)
 
 }
 
-int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
+static int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int err;
@@ -1461,7 +1391,7 @@ int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
 	return 1;
 }
 
-int rtl_pci_start(struct ieee80211_hw *hw)
+static int rtl_pci_start(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -1496,7 +1426,7 @@ int rtl_pci_start(struct ieee80211_hw *hw)
 	return 0;
 }
 
-void rtl_pci_stop(struct ieee80211_hw *hw)
+static void rtl_pci_stop(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -1547,13 +1477,11 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 	struct pci_dev *bridge_pdev = pdev->bus->self;
 	u16 venderid;
 	u16 deviceid;
-	u8 revisionid;
 	u16 irqline;
 	u8 tmp;
 
 	venderid = pdev->vendor;
 	deviceid = pdev->device;
-	pci_read_config_byte(pdev, 0x8, &revisionid);
 	pci_read_config_word(pdev, 0x3C, &irqline);
 
 	if (deviceid == RTL_PCI_8192_DID ||
@@ -1564,7 +1492,7 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 	    deviceid == RTL_PCI_8173_DID ||
 	    deviceid == RTL_PCI_8172_DID ||
 	    deviceid == RTL_PCI_8171_DID) {
-		switch (revisionid) {
+		switch (pdev->revision) {
 		case RTL_PCI_REVISION_ID_8192PCIE:
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 				 ("8192 PCI-E is found - "
@@ -1838,7 +1766,7 @@ fail3:
 	ieee80211_free_hw(hw);
 
 	if (rtlpriv->io.pci_mem_start != 0)
-		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
 
 fail2:
 	pci_release_regions(pdev);
@@ -1888,7 +1816,7 @@ void rtl_pci_disconnect(struct pci_dev *pdev)
 	}
 
 	if (rtlpriv->io.pci_mem_start != 0) {
-		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
 		pci_release_regions(pdev);
 	}
 
diff --git a/drivers/net/wireless/rtlwifi/pci.h b/drivers/net/wireless/rtlwifi/pci.h
index d36a66939958..0caa81429726 100644
--- a/drivers/net/wireless/rtlwifi/pci.h
+++ b/drivers/net/wireless/rtlwifi/pci.h
@@ -244,34 +244,34 @@ int rtl_pci_resume(struct pci_dev *pdev);
 
 static inline u8 pci_read8_sync(struct rtl_priv *rtlpriv, u32 addr)
 {
-	return 0xff & readb((u8 *) rtlpriv->io.pci_mem_start + addr);
+	return readb((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
 }
 
 static inline u16 pci_read16_sync(struct rtl_priv *rtlpriv, u32 addr)
 {
-	return readw((u8 *) rtlpriv->io.pci_mem_start + addr);
+	return readw((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
 }
 
 static inline u32 pci_read32_sync(struct rtl_priv *rtlpriv, u32 addr)
 {
-	return readl((u8 *) rtlpriv->io.pci_mem_start + addr);
+	return readl((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
 }
 
 static inline void pci_write8_async(struct rtl_priv *rtlpriv, u32 addr, u8 val)
 {
-	writeb(val, (u8 *) rtlpriv->io.pci_mem_start + addr);
+	writeb(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
 }
 
 static inline void pci_write16_async(struct rtl_priv *rtlpriv,
 				     u32 addr, u16 val)
 {
-	writew(val, (u8 *) rtlpriv->io.pci_mem_start + addr);
+	writew(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
 }
 
 static inline void pci_write32_async(struct rtl_priv *rtlpriv,
 				     u32 addr, u32 val)
 {
-	writel(val, (u8 *) rtlpriv->io.pci_mem_start + addr);
+	writel(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
 }
 
 static inline void rtl_pci_raw_write_port_ulong(u32 port, u32 val)
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index d2326c13449e..6b7e217b6b89 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -86,7 +86,7 @@ bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	enum rf_pwrstate rtstate;
-	bool b_actionallowed = false;
+	bool actionallowed = false;
 	u16 rfwait_cnt = 0;
 	unsigned long flag;
 
@@ -139,13 +139,13 @@ no_protect:
 		ppsc->rfoff_reason &= (~changesource);
 
 		if ((changesource == RF_CHANGE_BY_HW) &&
-		    (ppsc->b_hwradiooff == true)) {
-			ppsc->b_hwradiooff = false;
+		    (ppsc->hwradiooff == true)) {
+			ppsc->hwradiooff = false;
 		}
 
 		if (!ppsc->rfoff_reason) {
 			ppsc->rfoff_reason = 0;
-			b_actionallowed = true;
+			actionallowed = true;
 		}
 
 		break;
@@ -153,17 +153,17 @@ no_protect:
 	case ERFOFF:
 
 		if ((changesource == RF_CHANGE_BY_HW)
-		    && (ppsc->b_hwradiooff == false)) {
-			ppsc->b_hwradiooff = true;
+		    && (ppsc->hwradiooff == false)) {
+			ppsc->hwradiooff = true;
 		}
 
 		ppsc->rfoff_reason |= changesource;
-		b_actionallowed = true;
+		actionallowed = true;
 		break;
 
 	case ERFSLEEP:
 		ppsc->rfoff_reason |= changesource;
-		b_actionallowed = true;
+		actionallowed = true;
 		break;
 
 	default:
@@ -172,7 +172,7 @@ no_protect:
 		break;
 	}
 
-	if (b_actionallowed)
+	if (actionallowed)
 		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
 
 	if (!protect_or_not) {
@@ -181,7 +181,7 @@ no_protect:
 		spin_unlock_irqrestore(&rtlpriv->locks.rf_ps_lock, flag);
 	}
 
-	return b_actionallowed;
+	return actionallowed;
 }
 EXPORT_SYMBOL(rtl_ps_set_rf_state);
 
@@ -191,7 +191,7 @@ static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 
-	ppsc->b_swrf_processing = true;
+	ppsc->swrf_processing = true;
 
 	if (ppsc->inactive_pwrstate == ERFON && rtlhal->interface == INTF_PCI) {
 		if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
@@ -213,7 +213,7 @@ static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
 		}
 	}
 
-	ppsc->b_swrf_processing = false;
+	ppsc->swrf_processing = false;
 }
 
 void rtl_ips_nic_off_wq_callback(void *data)
@@ -239,13 +239,13 @@ void rtl_ips_nic_off_wq_callback(void *data)
 	if (rtlpriv->sec.being_setkey)
 		return;
 
-	if (ppsc->b_inactiveps) {
+	if (ppsc->inactiveps) {
 		rtstate = ppsc->rfpwr_state;
 
 		/*
 		 *Do not enter IPS in the following conditions:
 		 *(1) RF is already OFF or Sleep
-		 *(2) b_swrf_processing (indicates the IPS is still under going)
+		 *(2) swrf_processing (indicates the IPS is still under going)
 		 *(3) Connectted (only disconnected can trigger IPS)
 		 *(4) IBSS (send Beacon)
 		 *(5) AP mode (send Beacon)
@@ -253,14 +253,14 @@ void rtl_ips_nic_off_wq_callback(void *data)
 		 */
 
 		if (rtstate == ERFON &&
-		    !ppsc->b_swrf_processing &&
+		    !ppsc->swrf_processing &&
 		    (mac->link_state == MAC80211_NOLINK) &&
 		    !mac->act_scanning) {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 				 ("IPSEnter(): Turn off RF.\n"));
 
 			ppsc->inactive_pwrstate = ERFOFF;
-			ppsc->b_in_powersavemode = true;
+			ppsc->in_powersavemode = true;
 
 			/*rtl_pci_reset_trx_ring(hw); */
 			_rtl_ps_inactive_ps(hw);
@@ -290,15 +290,15 @@ void rtl_ips_nic_on(struct ieee80211_hw *hw)
 
 	spin_lock_irqsave(&rtlpriv->locks.ips_lock, flags);
 
-	if (ppsc->b_inactiveps) {
+	if (ppsc->inactiveps) {
 		rtstate = ppsc->rfpwr_state;
 
 		if (rtstate != ERFON &&
-		    !ppsc->b_swrf_processing &&
+		    !ppsc->swrf_processing &&
 		    ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
 
 			ppsc->inactive_pwrstate = ERFON;
-			ppsc->b_in_powersavemode = false;
+			ppsc->in_powersavemode = false;
 
 			_rtl_ps_inactive_ps(hw);
 		}
@@ -370,9 +370,9 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 	 *   mode and set RPWM to turn RF on.
 	 */
 
-	if ((ppsc->b_fwctrl_lps) && (ppsc->b_leisure_ps) &&
+	if ((ppsc->fwctrl_lps) && (ppsc->leisure_ps) &&
 	     ppsc->report_linked) {
-		bool b_fw_current_inps;
+		bool fw_current_inps;
 		if (ppsc->dot11_psmode == EACTIVE) {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 ("FW LPS leave ps_mode:%x\n",
@@ -385,11 +385,11 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 			rtlpriv->cfg->ops->set_hw_reg(hw,
 					HW_VAR_H2C_FW_PWRMODE,
 					(u8 *) (&fw_pwrmode));
-			b_fw_current_inps = false;
+			fw_current_inps = false;
 
 			rtlpriv->cfg->ops->set_hw_reg(hw,
 					HW_VAR_FW_PSMODE_STATUS,
-					(u8 *) (&b_fw_current_inps));
+					(u8 *) (&fw_current_inps));
 
 		} else {
 			if (rtl_get_fwlps_doze(hw)) {
@@ -398,10 +398,10 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 						 ppsc->fwctrl_psmode));
 
 				rpwm_val = 0x02;	/* RF off */
-				b_fw_current_inps = true;
+				fw_current_inps = true;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						HW_VAR_FW_PSMODE_STATUS,
-						(u8 *) (&b_fw_current_inps));
+						(u8 *) (&fw_current_inps));
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						HW_VAR_H2C_FW_PWRMODE,
 						(u8 *) (&ppsc->fwctrl_psmode));
@@ -425,13 +425,13 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	unsigned long flag;
 
-	if (!(ppsc->b_fwctrl_lps && ppsc->b_leisure_ps))
+	if (!(ppsc->fwctrl_lps && ppsc->leisure_ps))
 		return;
 
 	if (rtlpriv->sec.being_setkey)
 		return;
 
-	if (rtlpriv->link_info.b_busytraffic)
+	if (rtlpriv->link_info.busytraffic)
 		return;
 
 	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
@@ -446,7 +446,7 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 
 	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 
-	if (ppsc->b_leisure_ps) {
+	if (ppsc->leisure_ps) {
 		/* Idle for a while if we connect to AP a while ago. */
 		if (mac->cnt_after_linked >= 2) {
 			if (ppsc->dot11_psmode == EACTIVE) {
@@ -470,7 +470,7 @@ void rtl_lps_leave(struct ieee80211_hw *hw)
 
 	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 
-	if (ppsc->b_fwctrl_lps && ppsc->b_leisure_ps) {
+	if (ppsc->fwctrl_lps && ppsc->leisure_ps) {
 		if (ppsc->dot11_psmode != EACTIVE) {
 
 			/*FIX ME */
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/Makefile b/drivers/net/wireless/rtlwifi/rtl8192c/Makefile
new file mode 100644
index 000000000000..aee42d7ae8a2
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/Makefile
@@ -0,0 +1,9 @@
+rtl8192c-common-objs :=		\
+		main.o		\
+		dm_common.o	\
+		fw_common.o	\
+		phy_common.o
+
+obj-$(CONFIG_RTL8192C_COMMON) += rtl8192c-common.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
new file mode 100644
index 000000000000..bb023274414c
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -0,0 +1,1398 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "dm_common.h"
+
+struct dig_t dm_digtable;
+static struct ps_t dm_pstable;
+
+static const u32 ofdmswing_table[OFDM_TABLE_SIZE] = {
+	0x7f8001fe,
+	0x788001e2,
+	0x71c001c7,
+	0x6b8001ae,
+	0x65400195,
+	0x5fc0017f,
+	0x5a400169,
+	0x55400155,
+	0x50800142,
+	0x4c000130,
+	0x47c0011f,
+	0x43c0010f,
+	0x40000100,
+	0x3c8000f2,
+	0x390000e4,
+	0x35c000d7,
+	0x32c000cb,
+	0x300000c0,
+	0x2d4000b5,
+	0x2ac000ab,
+	0x288000a2,
+	0x26000098,
+	0x24000090,
+	0x22000088,
+	0x20000080,
+	0x1e400079,
+	0x1c800072,
+	0x1b00006c,
+	0x19800066,
+	0x18000060,
+	0x16c0005b,
+	0x15800056,
+	0x14400051,
+	0x1300004c,
+	0x12000048,
+	0x11000044,
+	0x10000040,
+};
+
+static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01},
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01},
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01},
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01},
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01},
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01},
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01},
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01},
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01},
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01},
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}
+};
+
+static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00},
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00},
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00},
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00},
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}
+};
+
+static void rtl92c_dm_diginit(struct ieee80211_hw *hw)
+{
+	dm_digtable.dig_enable_flag = true;
+	dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+	dm_digtable.cur_igvalue = 0x20;
+	dm_digtable.pre_igvalue = 0x0;
+	dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
+	dm_digtable.presta_connectstate = DIG_STA_DISCONNECT;
+	dm_digtable.curmultista_connectstate = DIG_MULTISTA_DISCONNECT;
+	dm_digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_digtable.rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_digtable.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_digtable.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_digtable.rx_gain_range_max = DM_DIG_MAX;
+	dm_digtable.rx_gain_range_min = DM_DIG_MIN;
+	dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_digtable.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
+	dm_digtable.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
+	dm_digtable.pre_cck_pd_state = CCK_PD_STAGE_MAX;
+	dm_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
+}
+
+static u8 rtl92c_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	long rssi_val_min = 0;
+
+	if ((dm_digtable.curmultista_connectstate == DIG_MULTISTA_CONNECT) &&
+	    (dm_digtable.cursta_connectctate == DIG_STA_CONNECT)) {
+		if (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb != 0)
+			rssi_val_min =
+			    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb >
+			     rtlpriv->dm.undecorated_smoothed_pwdb) ?
+			    rtlpriv->dm.undecorated_smoothed_pwdb :
+			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+		else
+			rssi_val_min = rtlpriv->dm.undecorated_smoothed_pwdb;
+	} else if (dm_digtable.cursta_connectctate == DIG_STA_CONNECT ||
+		   dm_digtable.cursta_connectctate == DIG_STA_BEFORE_CONNECT) {
+		rssi_val_min = rtlpriv->dm.undecorated_smoothed_pwdb;
+	} else if (dm_digtable.curmultista_connectstate ==
+		   DIG_MULTISTA_CONNECT) {
+		rssi_val_min = rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+	}
+
+	return (u8) rssi_val_min;
+}
+
+static void rtl92c_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+{
+	u32 ret_value;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER1, MASKDWORD);
+	falsealm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER2, MASKDWORD);
+	falsealm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
+	falsealm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER3, MASKDWORD);
+	falsealm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
+	falsealm_cnt->cnt_ofdm_fail = falsealm_cnt->cnt_parity_fail +
+	    falsealm_cnt->cnt_rate_illegal +
+	    falsealm_cnt->cnt_crc8_fail + falsealm_cnt->cnt_mcs_fail;
+
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(14), 1);
+	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERLOWER, MASKBYTE0);
+	falsealm_cnt->cnt_cck_fail = ret_value;
+
+	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERUPPER, MASKBYTE3);
+	falsealm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
+	falsealm_cnt->cnt_all = (falsealm_cnt->cnt_parity_fail +
+				 falsealm_cnt->cnt_rate_illegal +
+				 falsealm_cnt->cnt_crc8_fail +
+				 falsealm_cnt->cnt_mcs_fail +
+				 falsealm_cnt->cnt_cck_fail);
+
+	rtl_set_bbreg(hw, ROFDM1_LSTF, 0x08000000, 1);
+	rtl_set_bbreg(hw, ROFDM1_LSTF, 0x08000000, 0);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, 0x0000c000, 0);
+	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, 0x0000c000, 2);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("cnt_parity_fail = %d, cnt_rate_illegal = %d, "
+		  "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		  falsealm_cnt->cnt_parity_fail,
+		  falsealm_cnt->cnt_rate_illegal,
+		  falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail));
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
+		  falsealm_cnt->cnt_ofdm_fail,
+		  falsealm_cnt->cnt_cck_fail, falsealm_cnt->cnt_all));
+}
+
+static void rtl92c_dm_ctrl_initgain_by_fa(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value_igi = dm_digtable.cur_igvalue;
+
+	if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
+		value_igi--;
+	else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)
+		value_igi += 0;
+	else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH2)
+		value_igi++;
+	else if (rtlpriv->falsealm_cnt.cnt_all >= DM_DIG_FA_TH2)
+		value_igi += 2;
+	if (value_igi > DM_DIG_FA_UPPER)
+		value_igi = DM_DIG_FA_UPPER;
+	else if (value_igi < DM_DIG_FA_LOWER)
+		value_igi = DM_DIG_FA_LOWER;
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000)
+		value_igi = 0x32;
+
+	dm_digtable.cur_igvalue = value_igi;
+	rtl92c_dm_write_dig(hw);
+}
+
+static void rtl92c_dm_ctrl_initgain_by_rssi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->falsealm_cnt.cnt_all > dm_digtable.fa_highthresh) {
+		if ((dm_digtable.backoff_val - 2) <
+		    dm_digtable.backoff_val_range_min)
+			dm_digtable.backoff_val =
+			    dm_digtable.backoff_val_range_min;
+		else
+			dm_digtable.backoff_val -= 2;
+	} else if (rtlpriv->falsealm_cnt.cnt_all < dm_digtable.fa_lowthresh) {
+		if ((dm_digtable.backoff_val + 2) >
+		    dm_digtable.backoff_val_range_max)
+			dm_digtable.backoff_val =
+			    dm_digtable.backoff_val_range_max;
+		else
+			dm_digtable.backoff_val += 2;
+	}
+
+	if ((dm_digtable.rssi_val_min + 10 - dm_digtable.backoff_val) >
+	    dm_digtable.rx_gain_range_max)
+		dm_digtable.cur_igvalue = dm_digtable.rx_gain_range_max;
+	else if ((dm_digtable.rssi_val_min + 10 -
+		  dm_digtable.backoff_val) < dm_digtable.rx_gain_range_min)
+		dm_digtable.cur_igvalue = dm_digtable.rx_gain_range_min;
+	else
+		dm_digtable.cur_igvalue = dm_digtable.rssi_val_min + 10 -
+		    dm_digtable.backoff_val;
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("rssi_val_min = %x backoff_val %x\n",
+		  dm_digtable.rssi_val_min, dm_digtable.backoff_val));
+
+	rtl92c_dm_write_dig(hw);
+}
+
+static void rtl92c_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
+{
+	static u8 binitialized; /* initialized to false */
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	long rssi_strength = rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+	bool multi_sta = false;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		multi_sta = true;
+
+	if ((multi_sta == false) || (dm_digtable.cursta_connectctate !=
+				     DIG_STA_DISCONNECT)) {
+		binitialized = false;
+		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+		return;
+	} else if (binitialized == false) {
+		binitialized = true;
+		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
+		dm_digtable.cur_igvalue = 0x20;
+		rtl92c_dm_write_dig(hw);
+	}
+
+	if (dm_digtable.curmultista_connectstate == DIG_MULTISTA_CONNECT) {
+		if ((rssi_strength < dm_digtable.rssi_lowthresh) &&
+		    (dm_digtable.dig_ext_port_stage != DIG_EXT_PORT_STAGE_1)) {
+
+			if (dm_digtable.dig_ext_port_stage ==
+			    DIG_EXT_PORT_STAGE_2) {
+				dm_digtable.cur_igvalue = 0x20;
+				rtl92c_dm_write_dig(hw);
+			}
+
+			dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_1;
+		} else if (rssi_strength > dm_digtable.rssi_highthresh) {
+			dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_2;
+			rtl92c_dm_ctrl_initgain_by_fa(hw);
+		}
+	} else if (dm_digtable.dig_ext_port_stage != DIG_EXT_PORT_STAGE_0) {
+		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
+		dm_digtable.cur_igvalue = 0x20;
+		rtl92c_dm_write_dig(hw);
+	}
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("curmultista_connectstate = "
+		  "%x dig_ext_port_stage %x\n",
+		  dm_digtable.curmultista_connectstate,
+		  dm_digtable.dig_ext_port_stage));
+}
+
+static void rtl92c_dm_initial_gain_sta(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("presta_connectstate = %x,"
+		  " cursta_connectctate = %x\n",
+		  dm_digtable.presta_connectstate,
+		  dm_digtable.cursta_connectctate));
+
+	if (dm_digtable.presta_connectstate == dm_digtable.cursta_connectctate
+	    || dm_digtable.cursta_connectctate == DIG_STA_BEFORE_CONNECT
+	    || dm_digtable.cursta_connectctate == DIG_STA_CONNECT) {
+
+		if (dm_digtable.cursta_connectctate != DIG_STA_DISCONNECT) {
+			dm_digtable.rssi_val_min =
+			    rtl92c_dm_initial_gain_min_pwdb(hw);
+			rtl92c_dm_ctrl_initgain_by_rssi(hw);
+		}
+	} else {
+		dm_digtable.rssi_val_min = 0;
+		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
+		dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
+		dm_digtable.cur_igvalue = 0x20;
+		dm_digtable.pre_igvalue = 0;
+		rtl92c_dm_write_dig(hw);
+	}
+}
+
+static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (dm_digtable.cursta_connectctate == DIG_STA_CONNECT) {
+		dm_digtable.rssi_val_min = rtl92c_dm_initial_gain_min_pwdb(hw);
+
+		if (dm_digtable.pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
+			if (dm_digtable.rssi_val_min <= 25)
+				dm_digtable.cur_cck_pd_state =
+				    CCK_PD_STAGE_LowRssi;
+			else
+				dm_digtable.cur_cck_pd_state =
+				    CCK_PD_STAGE_HighRssi;
+		} else {
+			if (dm_digtable.rssi_val_min <= 20)
+				dm_digtable.cur_cck_pd_state =
+				    CCK_PD_STAGE_LowRssi;
+			else
+				dm_digtable.cur_cck_pd_state =
+				    CCK_PD_STAGE_HighRssi;
+		}
+	} else {
+		dm_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
+	}
+
+	if (dm_digtable.pre_cck_pd_state != dm_digtable.cur_cck_pd_state) {
+		if (dm_digtable.cur_cck_pd_state == CCK_PD_STAGE_LowRssi) {
+			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 800)
+				dm_digtable.cur_cck_fa_state =
+				    CCK_FA_STAGE_High;
+			else
+				dm_digtable.cur_cck_fa_state = CCK_FA_STAGE_Low;
+
+			if (dm_digtable.pre_cck_fa_state !=
+			    dm_digtable.cur_cck_fa_state) {
+				if (dm_digtable.cur_cck_fa_state ==
+				    CCK_FA_STAGE_Low)
+					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
+						      0x83);
+				else
+					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
+						      0xcd);
+
+				dm_digtable.pre_cck_fa_state =
+				    dm_digtable.cur_cck_fa_state;
+			}
+
+			rtl_set_bbreg(hw, RCCK0_SYSTEM, MASKBYTE1, 0x40);
+
+			if (IS_92C_SERIAL(rtlhal->version))
+				rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT,
+					      MASKBYTE2, 0xd7);
+		} else {
+			rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, 0xcd);
+			rtl_set_bbreg(hw, RCCK0_SYSTEM, MASKBYTE1, 0x47);
+
+			if (IS_92C_SERIAL(rtlhal->version))
+				rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT,
+					      MASKBYTE2, 0xd3);
+		}
+		dm_digtable.pre_cck_pd_state = dm_digtable.cur_cck_pd_state;
+	}
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("CCKPDStage=%x\n", dm_digtable.cur_cck_pd_state));
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 ("is92C=%x\n", IS_92C_SERIAL(rtlhal->version)));
+}
+
+static void rtl92c_dm_ctrl_initgain_by_twoport(struct ieee80211_hw *hw)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	if (mac->act_scanning == true)
+		return;
+
+	if ((mac->link_state > MAC80211_NOLINK) &&
+	    (mac->link_state < MAC80211_LINKED))
+		dm_digtable.cursta_connectctate = DIG_STA_BEFORE_CONNECT;
+	else if (mac->link_state >= MAC80211_LINKED)
+		dm_digtable.cursta_connectctate = DIG_STA_CONNECT;
+	else
+		dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
+
+	rtl92c_dm_initial_gain_sta(hw);
+	rtl92c_dm_initial_gain_multi_sta(hw);
+	rtl92c_dm_cck_packet_detection_thresh(hw);
+
+	dm_digtable.presta_connectstate = dm_digtable.cursta_connectctate;
+
+}
+
+static void rtl92c_dm_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->dm.dm_initialgain_enable == false)
+		return;
+	if (dm_digtable.dig_enable_flag == false)
+		return;
+
+	rtl92c_dm_ctrl_initgain_by_twoport(hw);
+
+}
+
+static void rtl92c_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dynamic_txpower_enable = false;
+
+	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
+	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+}
+
+void rtl92c_dm_write_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 ("cur_igvalue = 0x%x, "
+		  "pre_igvalue = 0x%x, backoff_val = %d\n",
+		  dm_digtable.cur_igvalue, dm_digtable.pre_igvalue,
+		  dm_digtable.backoff_val));
+
+	if (dm_digtable.pre_igvalue != dm_digtable.cur_igvalue) {
+		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f,
+			      dm_digtable.cur_igvalue);
+		rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f,
+			      dm_digtable.cur_igvalue);
+
+		dm_digtable.pre_igvalue = dm_digtable.cur_igvalue;
+	}
+}
+EXPORT_SYMBOL(rtl92c_dm_write_dig);
+
+static void rtl92c_dm_pwdb_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	long tmpentry_max_pwdb = 0, tmpentry_min_pwdb = 0xff;
+
+	u8 h2c_parameter[3] = { 0 };
+
+	return;
+
+	if (tmpentry_max_pwdb != 0) {
+		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb =
+		    tmpentry_max_pwdb;
+	} else {
+		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb = 0;
+	}
+
+	if (tmpentry_min_pwdb != 0xff) {
+		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb =
+		    tmpentry_min_pwdb;
+	} else {
+		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb = 0;
+	}
+
+	h2c_parameter[2] = (u8) (rtlpriv->dm.undecorated_smoothed_pwdb & 0xFF);
+	h2c_parameter[0] = 0;
+
+	rtl92c_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);
+}
+
+void rtl92c_dm_init_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtlpriv->dm.current_turbo_edca = false;
+	rtlpriv->dm.is_any_nonbepkts = false;
+	rtlpriv->dm.is_cur_rdlstate = false;
+}
+EXPORT_SYMBOL(rtl92c_dm_init_edca_turbo);
+
+static void rtl92c_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	static u64 last_txok_cnt;
+	static u64 last_rxok_cnt;
+	u64 cur_txok_cnt;
+	u64 cur_rxok_cnt;
+	u32 edca_be_ul = 0x5ea42b;
+	u32 edca_be_dl = 0x5ea42b;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		goto dm_checkedcaturbo_exit;
+
+	if (mac->link_state != MAC80211_LINKED) {
+		rtlpriv->dm.current_turbo_edca = false;
+		return;
+	}
+
+	if (!mac->ht_enable) {	/*FIX MERGE */
+		if (!(edca_be_ul & 0xffff0000))
+			edca_be_ul |= 0x005e0000;
+
+		if (!(edca_be_dl & 0xffff0000))
+			edca_be_dl |= 0x005e0000;
+	}
+
+	if ((!rtlpriv->dm.is_any_nonbepkts) &&
+	    (!rtlpriv->dm.disable_framebursting)) {
+		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
+		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
+		if (cur_rxok_cnt > 4 * cur_txok_cnt) {
+			if (!rtlpriv->dm.is_cur_rdlstate ||
+			    !rtlpriv->dm.current_turbo_edca) {
+				rtl_write_dword(rtlpriv,
+						REG_EDCA_BE_PARAM,
+						edca_be_dl);
+				rtlpriv->dm.is_cur_rdlstate = true;
+			}
+		} else {
+			if (rtlpriv->dm.is_cur_rdlstate ||
+			    !rtlpriv->dm.current_turbo_edca) {
+				rtl_write_dword(rtlpriv,
+						REG_EDCA_BE_PARAM,
+						edca_be_ul);
+				rtlpriv->dm.is_cur_rdlstate = false;
+			}
+		}
+		rtlpriv->dm.current_turbo_edca = true;
+	} else {
+		if (rtlpriv->dm.current_turbo_edca) {
+			u8 tmp = AC0_BE;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_AC_PARAM,
+						      (u8 *) (&tmp));
+			rtlpriv->dm.current_turbo_edca = false;
+		}
+	}
+
+dm_checkedcaturbo_exit:
+	rtlpriv->dm.is_any_nonbepkts = false;
+	last_txok_cnt = rtlpriv->stats.txbytesunicast;
+	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+static void rtl92c_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
+							     *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 thermalvalue, delta, delta_lck, delta_iqk;
+	long ele_a, ele_d, temp_cck, val_x, value32;
+	long val_y, ele_c;
+	u8 ofdm_index[2], cck_index, ofdm_index_old[2], cck_index_old;
+	int i;
+	bool is2t = IS_92C_SERIAL(rtlhal->version);
+	u8 txpwr_level[2] = {0, 0};
+	u8 ofdm_min_index = 6, rf;
+
+	rtlpriv->dm.txpower_trackingInit = true;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 ("rtl92c_dm_txpower_tracking_callback_thermalmeter\n"));
+
+	thermalvalue = (u8) rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER, 0x1f);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 ("Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
+		  "eeprom_thermalmeter 0x%x\n",
+		  thermalvalue, rtlpriv->dm.thermalvalue,
+		  rtlefuse->eeprom_thermalmeter));
+
+	rtl92c_phy_ap_calibrate(hw, (thermalvalue -
+				     rtlefuse->eeprom_thermalmeter));
+	if (is2t)
+		rf = 2;
+	else
+		rf = 1;
+
+	if (thermalvalue) {
+		ele_d = rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+				      MASKDWORD) & MASKOFDM_D;
+
+		for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
+			if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
+				ofdm_index_old[0] = (u8) i;
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					("Initial pathA ele_d reg0x%x = 0x%lx, "
+					 "ofdm_index=0x%x\n",
+					 ROFDM0_XATXIQIMBALANCE,
+					 ele_d, ofdm_index_old[0]));
+				break;
+			}
+		}
+
+		if (is2t) {
+			ele_d = rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
+					      MASKDWORD) & MASKOFDM_D;
+
+			for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
+				if (ele_d == (ofdmswing_table[i] &
+				    MASKOFDM_D)) {
+					ofdm_index_old[1] = (u8) i;
+
+					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+					   DBG_LOUD,
+					   ("Initial pathB ele_d reg0x%x = "
+					   "0x%lx, ofdm_index=0x%x\n",
+					   ROFDM0_XBTXIQIMBALANCE, ele_d,
+					   ofdm_index_old[1]));
+					break;
+				}
+			}
+		}
+
+		temp_cck =
+		    rtl_get_bbreg(hw, RCCK0_TXFILTER2, MASKDWORD) & MASKCCK;
+
+		for (i = 0; i < CCK_TABLE_LENGTH; i++) {
+			if (rtlpriv->dm.cck_inch14) {
+				if (memcmp((void *)&temp_cck,
+					   (void *)&cckswing_table_ch14[i][2],
+					   4) == 0) {
+					cck_index_old = (u8) i;
+
+					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+						 DBG_LOUD,
+						 ("Initial reg0x%x = 0x%lx, "
+						  "cck_index=0x%x, ch 14 %d\n",
+						  RCCK0_TXFILTER2, temp_cck,
+						  cck_index_old,
+						  rtlpriv->dm.cck_inch14));
+					break;
+				}
+			} else {
+				if (memcmp((void *)&temp_cck,
+					   (void *)
+					   &cckswing_table_ch1ch13[i][2],
+					   4) == 0) {
+					cck_index_old = (u8) i;
+
+					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+						 DBG_LOUD,
+						 ("Initial reg0x%x = 0x%lx, "
+						  "cck_index=0x%x, ch14 %d\n",
+						  RCCK0_TXFILTER2, temp_cck,
+						  cck_index_old,
+						  rtlpriv->dm.cck_inch14));
+					break;
+				}
+			}
+		}
+
+		if (!rtlpriv->dm.thermalvalue) {
+			rtlpriv->dm.thermalvalue =
+			    rtlefuse->eeprom_thermalmeter;
+			rtlpriv->dm.thermalvalue_lck = thermalvalue;
+			rtlpriv->dm.thermalvalue_iqk = thermalvalue;
+			for (i = 0; i < rf; i++)
+				rtlpriv->dm.ofdm_index[i] = ofdm_index_old[i];
+			rtlpriv->dm.cck_index = cck_index_old;
+		}
+
+		delta = (thermalvalue > rtlpriv->dm.thermalvalue) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue) :
+		    (rtlpriv->dm.thermalvalue - thermalvalue);
+
+		delta_lck = (thermalvalue > rtlpriv->dm.thermalvalue_lck) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue_lck) :
+		    (rtlpriv->dm.thermalvalue_lck - thermalvalue);
+
+		delta_iqk = (thermalvalue > rtlpriv->dm.thermalvalue_iqk) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue_iqk) :
+		    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
+
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			("Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
+			 "eeprom_thermalmeter 0x%x delta 0x%x "
+			 "delta_lck 0x%x delta_iqk 0x%x\n",
+			 thermalvalue, rtlpriv->dm.thermalvalue,
+			 rtlefuse->eeprom_thermalmeter, delta, delta_lck,
+			 delta_iqk));
+
+		if (delta_lck > 1) {
+			rtlpriv->dm.thermalvalue_lck = thermalvalue;
+			rtl92c_phy_lc_calibrate(hw);
+		}
+
+		if (delta > 0 && rtlpriv->dm.txpower_track_control) {
+			if (thermalvalue > rtlpriv->dm.thermalvalue) {
+				for (i = 0; i < rf; i++)
+					rtlpriv->dm.ofdm_index[i] -= delta;
+				rtlpriv->dm.cck_index -= delta;
+			} else {
+				for (i = 0; i < rf; i++)
+					rtlpriv->dm.ofdm_index[i] += delta;
+				rtlpriv->dm.cck_index += delta;
+			}
+
+			if (is2t) {
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 ("temp OFDM_A_index=0x%x, "
+					  "OFDM_B_index=0x%x,"
+					  "cck_index=0x%x\n",
+					  rtlpriv->dm.ofdm_index[0],
+					  rtlpriv->dm.ofdm_index[1],
+					  rtlpriv->dm.cck_index));
+			} else {
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 ("temp OFDM_A_index=0x%x,"
+					  "cck_index=0x%x\n",
+					  rtlpriv->dm.ofdm_index[0],
+					  rtlpriv->dm.cck_index));
+			}
+
+			if (thermalvalue > rtlefuse->eeprom_thermalmeter) {
+				for (i = 0; i < rf; i++)
+					ofdm_index[i] =
+					    rtlpriv->dm.ofdm_index[i]
+					    + 1;
+				cck_index = rtlpriv->dm.cck_index + 1;
+			} else {
+				for (i = 0; i < rf; i++)
+					ofdm_index[i] =
+					    rtlpriv->dm.ofdm_index[i];
+				cck_index = rtlpriv->dm.cck_index;
+			}
+
+			for (i = 0; i < rf; i++) {
+				if (txpwr_level[i] >= 0 &&
+				    txpwr_level[i] <= 26) {
+					if (thermalvalue >
+					    rtlefuse->eeprom_thermalmeter) {
+						if (delta < 5)
+							ofdm_index[i] -= 1;
+
+						else
+							ofdm_index[i] -= 2;
+					} else if (delta > 5 && thermalvalue <
+						   rtlefuse->
+						   eeprom_thermalmeter) {
+						ofdm_index[i] += 1;
+					}
+				} else if (txpwr_level[i] >= 27 &&
+					   txpwr_level[i] <= 32
+					   && thermalvalue >
+					   rtlefuse->eeprom_thermalmeter) {
+					if (delta < 5)
+						ofdm_index[i] -= 1;
+
+					else
+						ofdm_index[i] -= 2;
+				} else if (txpwr_level[i] >= 32 &&
+					   txpwr_level[i] <= 38 &&
+					   thermalvalue >
+					   rtlefuse->eeprom_thermalmeter
+					   && delta > 5) {
+					ofdm_index[i] -= 1;
+				}
+			}
+
+			if (txpwr_level[i] >= 0 && txpwr_level[i] <= 26) {
+				if (thermalvalue >
+				    rtlefuse->eeprom_thermalmeter) {
+					if (delta < 5)
+						cck_index -= 1;
+
+					else
+						cck_index -= 2;
+				} else if (delta > 5 && thermalvalue <
+					   rtlefuse->eeprom_thermalmeter) {
+					cck_index += 1;
+				}
+			} else if (txpwr_level[i] >= 27 &&
+				   txpwr_level[i] <= 32 &&
+				   thermalvalue >
+				   rtlefuse->eeprom_thermalmeter) {
+				if (delta < 5)
+					cck_index -= 1;
+
+				else
+					cck_index -= 2;
+			} else if (txpwr_level[i] >= 32 &&
+				   txpwr_level[i] <= 38 &&
+				   thermalvalue > rtlefuse->eeprom_thermalmeter
+				   && delta > 5) {
+				cck_index -= 1;
+			}
+
+			for (i = 0; i < rf; i++) {
+				if (ofdm_index[i] > OFDM_TABLE_SIZE - 1)
+					ofdm_index[i] = OFDM_TABLE_SIZE - 1;
+
+				else if (ofdm_index[i] < ofdm_min_index)
+					ofdm_index[i] = ofdm_min_index;
+			}
+
+			if (cck_index > CCK_TABLE_SIZE - 1)
+				cck_index = CCK_TABLE_SIZE - 1;
+			else if (cck_index < 0)
+				cck_index = 0;
+
+			if (is2t) {
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 ("new OFDM_A_index=0x%x, "
+					  "OFDM_B_index=0x%x,"
+					  "cck_index=0x%x\n",
+					  ofdm_index[0], ofdm_index[1],
+					  cck_index));
+			} else {
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 ("new OFDM_A_index=0x%x,"
+					  "cck_index=0x%x\n",
+					  ofdm_index[0], cck_index));
+			}
+		}
+
+		if (rtlpriv->dm.txpower_track_control && delta != 0) {
+			ele_d =
+			    (ofdmswing_table[ofdm_index[0]] & 0xFFC00000) >> 22;
+			val_x = rtlphy->reg_e94;
+			val_y = rtlphy->reg_e9c;
+
+			if (val_x != 0) {
+				if ((val_x & 0x00000200) != 0)
+					val_x = val_x | 0xFFFFFC00;
+				ele_a = ((val_x * ele_d) >> 8) & 0x000003FF;
+
+				if ((val_y & 0x00000200) != 0)
+					val_y = val_y | 0xFFFFFC00;
+				ele_c = ((val_y * ele_d) >> 8) & 0x000003FF;
+
+				value32 = (ele_d << 22) |
+				    ((ele_c & 0x3F) << 16) | ele_a;
+
+				rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+					      MASKDWORD, value32);
+
+				value32 = (ele_c & 0x000003C0) >> 6;
+				rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS,
+					      value32);
+
+				value32 = ((val_x * ele_d) >> 7) & 0x01;
+				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+					      BIT(31), value32);
+
+				value32 = ((val_y * ele_d) >> 7) & 0x01;
+				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+					      BIT(29), value32);
+			} else {
+				rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+					      MASKDWORD,
+					      ofdmswing_table[ofdm_index[0]]);
+
+				rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS,
+					      0x00);
+				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+					      BIT(31) | BIT(29), 0x00);
+			}
+
+			if (!rtlpriv->dm.cck_inch14) {
+				rtl_write_byte(rtlpriv, 0xa22,
+					       cckswing_table_ch1ch13[cck_index]
+					       [0]);
+				rtl_write_byte(rtlpriv, 0xa23,
+					       cckswing_table_ch1ch13[cck_index]
+					       [1]);
+				rtl_write_byte(rtlpriv, 0xa24,
+					       cckswing_table_ch1ch13[cck_index]
+					       [2]);
+				rtl_write_byte(rtlpriv, 0xa25,
+					       cckswing_table_ch1ch13[cck_index]
+					       [3]);
+				rtl_write_byte(rtlpriv, 0xa26,
+					       cckswing_table_ch1ch13[cck_index]
+					       [4]);
+				rtl_write_byte(rtlpriv, 0xa27,
+					       cckswing_table_ch1ch13[cck_index]
+					       [5]);
+				rtl_write_byte(rtlpriv, 0xa28,
+					       cckswing_table_ch1ch13[cck_index]
+					       [6]);
+				rtl_write_byte(rtlpriv, 0xa29,
+					       cckswing_table_ch1ch13[cck_index]
+					       [7]);
+			} else {
+				rtl_write_byte(rtlpriv, 0xa22,
+					       cckswing_table_ch14[cck_index]
+					       [0]);
+				rtl_write_byte(rtlpriv, 0xa23,
+					       cckswing_table_ch14[cck_index]
+					       [1]);
+				rtl_write_byte(rtlpriv, 0xa24,
+					       cckswing_table_ch14[cck_index]
+					       [2]);
+				rtl_write_byte(rtlpriv, 0xa25,
+					       cckswing_table_ch14[cck_index]
+					       [3]);
+				rtl_write_byte(rtlpriv, 0xa26,
+					       cckswing_table_ch14[cck_index]
+					       [4]);
+				rtl_write_byte(rtlpriv, 0xa27,
+					       cckswing_table_ch14[cck_index]
+					       [5]);
+				rtl_write_byte(rtlpriv, 0xa28,
+					       cckswing_table_ch14[cck_index]
+					       [6]);
+				rtl_write_byte(rtlpriv, 0xa29,
+					       cckswing_table_ch14[cck_index]
+					       [7]);
+			}
+
+			if (is2t) {
+				ele_d = (ofdmswing_table[ofdm_index[1]] &
+					 0xFFC00000) >> 22;
+
+				val_x = rtlphy->reg_eb4;
+				val_y = rtlphy->reg_ebc;
+
+				if (val_x != 0) {
+					if ((val_x & 0x00000200) != 0)
+						val_x = val_x | 0xFFFFFC00;
+					ele_a = ((val_x * ele_d) >> 8) &
+					    0x000003FF;
+
+					if ((val_y & 0x00000200) != 0)
+						val_y = val_y | 0xFFFFFC00;
+					ele_c = ((val_y * ele_d) >> 8) &
+					    0x00003FF;
+
+					value32 = (ele_d << 22) |
+					    ((ele_c & 0x3F) << 16) | ele_a;
+					rtl_set_bbreg(hw,
+						      ROFDM0_XBTXIQIMBALANCE,
+						      MASKDWORD, value32);
+
+					value32 = (ele_c & 0x000003C0) >> 6;
+					rtl_set_bbreg(hw, ROFDM0_XDTXAFE,
+						      MASKH4BITS, value32);
+
+					value32 = ((val_x * ele_d) >> 7) & 0x01;
+					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+						      BIT(27), value32);
+
+					value32 = ((val_y * ele_d) >> 7) & 0x01;
+					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+						      BIT(25), value32);
+				} else {
+					rtl_set_bbreg(hw,
+						      ROFDM0_XBTXIQIMBALANCE,
+						      MASKDWORD,
+						      ofdmswing_table[ofdm_index
+								      [1]]);
+					rtl_set_bbreg(hw, ROFDM0_XDTXAFE,
+						      MASKH4BITS, 0x00);
+					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+						      BIT(27) | BIT(25), 0x00);
+				}
+
+			}
+		}
+
+		if (delta_iqk > 3) {
+			rtlpriv->dm.thermalvalue_iqk = thermalvalue;
+			rtl92c_phy_iq_calibrate(hw, false);
+		}
+
+		if (rtlpriv->dm.txpower_track_control)
+			rtlpriv->dm.thermalvalue = thermalvalue;
+	}
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, ("<===\n"));
+
+}
+
+static void rtl92c_dm_initialize_txpower_tracking_thermalmeter(
+						struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.txpower_tracking = true;
+	rtlpriv->dm.txpower_trackingInit = false;
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 ("pMgntInfo->txpower_tracking = %d\n",
+		  rtlpriv->dm.txpower_tracking));
+}
+
+static void rtl92c_dm_initialize_txpower_tracking(struct ieee80211_hw *hw)
+{
+	rtl92c_dm_initialize_txpower_tracking_thermalmeter(hw);
+}
+
+static void rtl92c_dm_txpower_tracking_directcall(struct ieee80211_hw *hw)
+{
+	rtl92c_dm_txpower_tracking_callback_thermalmeter(hw);
+}
+
+static void rtl92c_dm_check_txpower_tracking_thermal_meter(
+						struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger;
+
+	if (!rtlpriv->dm.txpower_tracking)
+		return;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, RFREG_OFFSET_MASK,
+			      0x60);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 ("Trigger 92S Thermal Meter!!\n"));
+		tm_trigger = 1;
+		return;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 ("Schedule TxPowerTracking direct call!!\n"));
+		rtl92c_dm_txpower_tracking_directcall(hw);
+		tm_trigger = 0;
+	}
+}
+
+void rtl92c_dm_check_txpower_tracking(struct ieee80211_hw *hw)
+{
+	rtl92c_dm_check_txpower_tracking_thermal_meter(hw);
+}
+EXPORT_SYMBOL(rtl92c_dm_check_txpower_tracking);
+
+void rtl92c_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+
+	p_ra->ratr_state = DM_RATR_STA_INIT;
+	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
+
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+		rtlpriv->dm.useramask = true;
+	else
+		rtlpriv->dm.useramask = false;
+
+}
+EXPORT_SYMBOL(rtl92c_dm_init_rate_adaptive_mask);
+
+static void rtl92c_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	u32 low_rssithresh_for_ra, high_rssithresh_for_ra;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 ("<---- driver is going to unload\n"));
+		return;
+	}
+
+	if (!rtlpriv->dm.useramask) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			("<---- driver does not control rate adaptive mask\n"));
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED) {
+
+		switch (p_ra->pre_ratr_state) {
+		case DM_RATR_STA_HIGH:
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 20;
+			break;
+		case DM_RATR_STA_MIDDLE:
+			high_rssithresh_for_ra = 55;
+			low_rssithresh_for_ra = 20;
+			break;
+		case DM_RATR_STA_LOW:
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 25;
+			break;
+		default:
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 20;
+			break;
+		}
+
+		if (rtlpriv->dm.undecorated_smoothed_pwdb >
+		    (long)high_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_HIGH;
+		else if (rtlpriv->dm.undecorated_smoothed_pwdb >
+			 (long)low_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
+		else
+			p_ra->ratr_state = DM_RATR_STA_LOW;
+
+		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 ("RSSI = %ld\n",
+				  rtlpriv->dm.undecorated_smoothed_pwdb));
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 ("RSSI_LEVEL = %d\n", p_ra->ratr_state));
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 ("PreState = %d, CurState = %d\n",
+				  p_ra->pre_ratr_state, p_ra->ratr_state));
+
+			rtlpriv->cfg->ops->update_rate_mask(hw,
+					p_ra->ratr_state);
+
+			p_ra->pre_ratr_state = p_ra->ratr_state;
+		}
+	}
+}
+
+static void rtl92c_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
+{
+	dm_pstable.pre_ccastate = CCA_MAX;
+	dm_pstable.cur_ccasate = CCA_MAX;
+	dm_pstable.pre_rfstate = RF_MAX;
+	dm_pstable.cur_rfstate = RF_MAX;
+	dm_pstable.rssi_val_min = 0;
+}
+
+static void rtl92c_dm_1r_cca(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (dm_pstable.rssi_val_min != 0) {
+		if (dm_pstable.pre_ccastate == CCA_2R) {
+			if (dm_pstable.rssi_val_min >= 35)
+				dm_pstable.cur_ccasate = CCA_1R;
+			else
+				dm_pstable.cur_ccasate = CCA_2R;
+		} else {
+			if (dm_pstable.rssi_val_min <= 30)
+				dm_pstable.cur_ccasate = CCA_2R;
+			else
+				dm_pstable.cur_ccasate = CCA_1R;
+		}
+	} else {
+		dm_pstable.cur_ccasate = CCA_MAX;
+	}
+
+	if (dm_pstable.pre_ccastate != dm_pstable.cur_ccasate) {
+		if (dm_pstable.cur_ccasate == CCA_1R) {
+			if (get_rf_type(rtlphy) == RF_2T2R) {
+				rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE,
+					      MASKBYTE0, 0x13);
+				rtl_set_bbreg(hw, 0xe70, MASKBYTE3, 0x20);
+			} else {
+				rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE,
+					      MASKBYTE0, 0x23);
+				rtl_set_bbreg(hw, 0xe70, 0x7fc00000, 0x10c);
+			}
+		} else {
+			rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0,
+				      0x33);
+			rtl_set_bbreg(hw, 0xe70, MASKBYTE3, 0x63);
+		}
+		dm_pstable.pre_ccastate = dm_pstable.cur_ccasate;
+	}
+
+	RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD, ("CCAStage = %s\n",
+					       (dm_pstable.cur_ccasate ==
+						0) ? "1RCCA" : "2RCCA"));
+}
+
+void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal)
+{
+	static u8 initialize;
+	static u32 reg_874, reg_c70, reg_85c, reg_a74;
+
+	if (initialize == 0) {
+		reg_874 = (rtl_get_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
+					 MASKDWORD) & 0x1CC000) >> 14;
+
+		reg_c70 = (rtl_get_bbreg(hw, ROFDM0_AGCPARAMETER1,
+					 MASKDWORD) & BIT(3)) >> 3;
+
+		reg_85c = (rtl_get_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL,
+					 MASKDWORD) & 0xFF000000) >> 24;
+
+		reg_a74 = (rtl_get_bbreg(hw, 0xa74, MASKDWORD) & 0xF000) >> 12;
+
+		initialize = 1;
+	}
+
+	if (!bforce_in_normal) {
+		if (dm_pstable.rssi_val_min != 0) {
+			if (dm_pstable.pre_rfstate == RF_NORMAL) {
+				if (dm_pstable.rssi_val_min >= 30)
+					dm_pstable.cur_rfstate = RF_SAVE;
+				else
+					dm_pstable.cur_rfstate = RF_NORMAL;
+			} else {
+				if (dm_pstable.rssi_val_min <= 25)
+					dm_pstable.cur_rfstate = RF_NORMAL;
+				else
+					dm_pstable.cur_rfstate = RF_SAVE;
+			}
+		} else {
+			dm_pstable.cur_rfstate = RF_MAX;
+		}
+	} else {
+		dm_pstable.cur_rfstate = RF_NORMAL;
+	}
+
+	if (dm_pstable.pre_rfstate != dm_pstable.cur_rfstate) {
+		if (dm_pstable.cur_rfstate == RF_SAVE) {
+			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
+				      0x1C0000, 0x2);
+			rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, BIT(3), 0);
+			rtl_set_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL,
+				      0xFF000000, 0x63);
+			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
+				      0xC000, 0x2);
+			rtl_set_bbreg(hw, 0xa74, 0xF000, 0x3);
+			rtl_set_bbreg(hw, 0x818, BIT(28), 0x0);
+			rtl_set_bbreg(hw, 0x818, BIT(28), 0x1);
+		} else {
+			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
+				      0x1CC000, reg_874);
+			rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, BIT(3),
+				      reg_c70);
+			rtl_set_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL, 0xFF000000,
+				      reg_85c);
+			rtl_set_bbreg(hw, 0xa74, 0xF000, reg_a74);
+			rtl_set_bbreg(hw, 0x818, BIT(28), 0x0);
+		}
+
+		dm_pstable.pre_rfstate = dm_pstable.cur_rfstate;
+	}
+}
+EXPORT_SYMBOL(rtl92c_dm_rf_saving);
+
+static void rtl92c_dm_dynamic_bb_powersaving(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (((mac->link_state == MAC80211_NOLINK)) &&
+	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
+		dm_pstable.rssi_val_min = 0;
+		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
+			 ("Not connected to any\n"));
+	}
+
+	if (mac->link_state == MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+			dm_pstable.rssi_val_min =
+			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
+				 ("AP Client PWDB = 0x%lx\n",
+				  dm_pstable.rssi_val_min));
+		} else {
+			dm_pstable.rssi_val_min =
+			    rtlpriv->dm.undecorated_smoothed_pwdb;
+			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
+				 ("STA Default Port PWDB = 0x%lx\n",
+				  dm_pstable.rssi_val_min));
+		}
+	} else {
+		dm_pstable.rssi_val_min =
+		    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+
+		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
+			 ("AP Ext Port PWDB = 0x%lx\n",
+			  dm_pstable.rssi_val_min));
+	}
+
+	if (IS_92C_SERIAL(rtlhal->version))
+		rtl92c_dm_1r_cca(hw);
+}
+
+void rtl92c_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	rtl92c_dm_diginit(hw);
+	rtl92c_dm_init_dynamic_txpower(hw);
+	rtl92c_dm_init_edca_turbo(hw);
+	rtl92c_dm_init_rate_adaptive_mask(hw);
+	rtl92c_dm_initialize_txpower_tracking(hw);
+	rtl92c_dm_init_dynamic_bb_powersaving(hw);
+}
+EXPORT_SYMBOL(rtl92c_dm_init);
+
+void rtl92c_dm_watchdog(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool fw_current_inpsmode = false;
+	bool fw_ps_awake = true;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *) (&fw_current_inpsmode));
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+				      (u8 *) (&fw_ps_awake));
+
+	if ((ppsc->rfpwr_state == ERFON) && ((!fw_current_inpsmode) &&
+					     fw_ps_awake)
+	    && (!ppsc->rfchange_inprogress)) {
+		rtl92c_dm_pwdb_monitor(hw);
+		rtl92c_dm_dig(hw);
+		rtl92c_dm_false_alarm_counter_statistics(hw);
+		rtl92c_dm_dynamic_bb_powersaving(hw);
+		rtlpriv->cfg->ops->dm_dynamic_txpower(hw);
+		rtl92c_dm_check_txpower_tracking(hw);
+		rtl92c_dm_refresh_rate_adaptive_mask(hw);
+		rtl92c_dm_check_edca_turbo(hw);
+
+	}
+}
+EXPORT_SYMBOL(rtl92c_dm_watchdog);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
new file mode 100644
index 000000000000..b9cbb0a3c03f
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.h
@@ -0,0 +1,204 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef	__RTL92COMMON_DM_H__
+#define __RTL92COMMON_DM_H__
+
+#include "../wifi.h"
+#include "../rtl8192ce/def.h"
+#include "../rtl8192ce/reg.h"
+#include "fw_common.h"
+
+#define HAL_DM_DIG_DISABLE			BIT(0)
+#define HAL_DM_HIPWR_DISABLE			BIT(1)
+
+#define OFDM_TABLE_LENGTH			37
+#define CCK_TABLE_LENGTH			33
+
+#define OFDM_TABLE_SIZE				37
+#define CCK_TABLE_SIZE				33
+
+#define BW_AUTO_SWITCH_HIGH_LOW			25
+#define BW_AUTO_SWITCH_LOW_HIGH			30
+
+#define DM_DIG_THRESH_HIGH			40
+#define DM_DIG_THRESH_LOW			35
+
+#define DM_FALSEALARM_THRESH_LOW		400
+#define DM_FALSEALARM_THRESH_HIGH		1000
+
+#define DM_DIG_MAX				0x3e
+#define DM_DIG_MIN				0x1e
+
+#define DM_DIG_FA_UPPER				0x32
+#define DM_DIG_FA_LOWER				0x20
+#define DM_DIG_FA_TH0				0x20
+#define DM_DIG_FA_TH1				0x100
+#define DM_DIG_FA_TH2				0x200
+
+#define DM_DIG_BACKOFF_MAX			12
+#define DM_DIG_BACKOFF_MIN			-4
+#define DM_DIG_BACKOFF_DEFAULT			10
+
+#define RXPATHSELECTION_SS_TH_lOW		30
+#define RXPATHSELECTION_DIFF_TH			18
+
+#define DM_RATR_STA_INIT			0
+#define DM_RATR_STA_HIGH			1
+#define DM_RATR_STA_MIDDLE			2
+#define DM_RATR_STA_LOW				3
+
+#define CTS2SELF_THVAL				30
+#define REGC38_TH				20
+
+#define WAIOTTHVal				25
+
+#define TXHIGHPWRLEVEL_NORMAL			0
+#define TXHIGHPWRLEVEL_LEVEL1			1
+#define TXHIGHPWRLEVEL_LEVEL2			2
+#define TXHIGHPWRLEVEL_BT1			3
+#define TXHIGHPWRLEVEL_BT2			4
+
+#define DM_TYPE_BYFW				0
+#define DM_TYPE_BYDRIVER			1
+
+#define TX_POWER_NEAR_FIELD_THRESH_LVL2		74
+#define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
+
+struct ps_t {
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+	u8 pre_rfstate;
+	u8 cur_rfstate;
+	long rssi_val_min;
+};
+
+struct dig_t {
+	u8 dig_enable_flag;
+	u8 dig_ext_port_stage;
+	u32 rssi_lowthresh;
+	u32 rssi_highthresh;
+	u32 fa_lowthresh;
+	u32 fa_highthresh;
+	u8 cursta_connectctate;
+	u8 presta_connectstate;
+	u8 curmultista_connectstate;
+	u8 pre_igvalue;
+	u8 cur_igvalue;
+	char backoff_val;
+	char backoff_val_range_max;
+	char backoff_val_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 rssi_val_min;
+	u8 pre_cck_pd_state;
+	u8 cur_cck_pd_state;
+	u8 pre_cck_fa_state;
+	u8 cur_cck_fa_state;
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+};
+
+struct swat_t {
+	u8 failure_cnt;
+	u8 try_flag;
+	u8 stop_trying;
+	long pre_rssi;
+	long trying_threshold;
+	u8 cur_antenna;
+	u8 pre_antenna;
+};
+
+enum tag_dynamic_init_gain_operation_type_definition {
+	DIG_TYPE_THRESH_HIGH = 0,
+	DIG_TYPE_THRESH_LOW = 1,
+	DIG_TYPE_BACKOFF = 2,
+	DIG_TYPE_RX_GAIN_MIN = 3,
+	DIG_TYPE_RX_GAIN_MAX = 4,
+	DIG_TYPE_ENABLE = 5,
+	DIG_TYPE_DISABLE = 6,
+	DIG_OP_TYPE_MAX
+};
+
+enum tag_cck_packet_detection_threshold_type_definition {
+	CCK_PD_STAGE_LowRssi = 0,
+	CCK_PD_STAGE_HighRssi = 1,
+	CCK_FA_STAGE_Low = 2,
+	CCK_FA_STAGE_High = 3,
+	CCK_PD_STAGE_MAX = 4,
+};
+
+enum dm_1r_cca_e {
+	CCA_1R = 0,
+	CCA_2R = 1,
+	CCA_MAX = 2,
+};
+
+enum dm_rf_e {
+	RF_SAVE = 0,
+	RF_NORMAL = 1,
+	RF_MAX = 2,
+};
+
+enum dm_sw_ant_switch_e {
+	ANS_ANTENNA_B = 1,
+	ANS_ANTENNA_A = 2,
+	ANS_ANTENNA_MAX = 3,
+};
+
+enum dm_dig_ext_port_alg_e {
+	DIG_EXT_PORT_STAGE_0 = 0,
+	DIG_EXT_PORT_STAGE_1 = 1,
+	DIG_EXT_PORT_STAGE_2 = 2,
+	DIG_EXT_PORT_STAGE_3 = 3,
+	DIG_EXT_PORT_STAGE_MAX = 4,
+};
+
+enum dm_dig_connect_e {
+	DIG_STA_DISCONNECT = 0,
+	DIG_STA_CONNECT = 1,
+	DIG_STA_BEFORE_CONNECT = 2,
+	DIG_MULTISTA_DISCONNECT = 3,
+	DIG_MULTISTA_CONNECT = 4,
+	DIG_CONNECT_MAX
+};
+
+extern struct dig_t dm_digtable;
+void rtl92c_dm_init(struct ieee80211_hw *hw);
+void rtl92c_dm_watchdog(struct ieee80211_hw *hw);
+void rtl92c_dm_write_dig(struct ieee80211_hw *hw);
+void rtl92c_dm_init_edca_turbo(struct ieee80211_hw *hw);
+void rtl92c_dm_check_txpower_tracking(struct ieee80211_hw *hw);
+void rtl92c_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal);
+void rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
+void rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/fw.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 11dd22b987e7..5ef91374b230 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -31,10 +31,9 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
-#include "reg.h"
-#include "def.h"
-#include "fw.h"
-#include "table.h"
+#include "../rtl8192ce/reg.h"
+#include "../rtl8192ce/def.h"
+#include "fw_common.h"
 
 static void _rtl92c_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 {
@@ -133,17 +132,15 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool is_version_b;
 	u8 *bufferPtr = (u8 *) buffer;
 
 	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, ("FW size is %d bytes,\n", size));
 
-	is_version_b = IS_CHIP_VER_B(version);
-	if (is_version_b) {
+	if (IS_CHIP_VER_B(version)) {
 		u32 pageNums, remainSize;
 		u32 page, offset;
 
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192CE)
+		if (IS_HARDWARE_TYPE_8192CE(rtlhal))
 			_rtl92c_fill_dummy(bufferPtr, &size);
 
 		pageNums = size / FW_8192C_PAGE_SIZE;
@@ -231,14 +228,14 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 	u32 fwsize;
 	int err;
 	enum version_8192c version = rtlhal->version;
+	const struct firmware *firmware;
 
-	const struct firmware *firmware = NULL;
-
+	printk(KERN_INFO "rtl8192cu: Loading firmware file %s\n",
+	       rtlpriv->cfg->fw_name);
 	err = request_firmware(&firmware, rtlpriv->cfg->fw_name,
 			       rtlpriv->io.dev);
 	if (err) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 ("Failed to request firmware!\n"));
+		printk(KERN_ERR "rtl8192cu: Firmware loading failed\n");
 		return 1;
 	}
 
@@ -281,6 +278,7 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 
 	return 0;
 }
+EXPORT_SYMBOL(rtl92c_download_fw);
 
 static bool _rtl92c_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 {
@@ -318,12 +316,12 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 
 	while (true) {
 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
-		if (rtlhal->b_h2c_setinprogress) {
+		if (rtlhal->h2c_setinprogress) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 				 ("H2C set in progress! Wait to set.."
 				  "element_id(%d).\n", element_id));
 
-			while (rtlhal->b_h2c_setinprogress) {
+			while (rtlhal->h2c_setinprogress) {
 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
 						       flag);
 				h2c_waitcounter++;
@@ -339,7 +337,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			}
 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 		} else {
-			rtlhal->b_h2c_setinprogress = true;
+			rtlhal->h2c_setinprogress = true;
 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 			break;
 		}
@@ -495,7 +493,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 	}
 
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
-	rtlhal->b_h2c_setinprogress = false;
+	rtlhal->h2c_setinprogress = false;
 	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, ("go out\n"));
@@ -507,7 +505,7 @@ void rtl92c_fill_h2c_cmd(struct ieee80211_hw *hw,
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 tmp_cmdbuf[2];
 
-	if (rtlhal->bfw_ready == false) {
+	if (rtlhal->fw_ready == false) {
 		RT_ASSERT(false, ("return H2C cmd because of Fw "
 				  "download fail!!!\n"));
 		return;
@@ -519,6 +517,7 @@ void rtl92c_fill_h2c_cmd(struct ieee80211_hw *hw,
 
 	return;
 }
+EXPORT_SYMBOL(rtl92c_fill_h2c_cmd);
 
 void rtl92c_firmware_selfreset(struct ieee80211_hw *hw)
 {
@@ -539,6 +538,7 @@ void rtl92c_firmware_selfreset(struct ieee80211_hw *hw)
 		u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
 	}
 }
+EXPORT_SYMBOL(rtl92c_firmware_selfreset);
 
 void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 {
@@ -559,39 +559,7 @@ void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	rtl92c_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
 
 }
-
-static bool _rtl92c_cmd_send_packet(struct ieee80211_hw *hw,
-				    struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	u8 own;
-	unsigned long flags;
-	struct sk_buff *pskb = NULL;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-
-	pdesc = &ring->desc[0];
-	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc, true, HW_DESC_OWN);
-
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
-
-	__skb_queue_tail(&ring->queue, skb);
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
-
-	return true;
-}
+EXPORT_SYMBOL(rtl92c_set_fw_pwrmode_cmd);
 
 #define BEACON_PG		0 /*->1*/
 #define PSPOLL_PG		2
@@ -776,7 +744,7 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 	memcpy((u8 *) skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
-	rtstatus = _rtl92c_cmd_send_packet(hw, skb);
+	rtstatus = rtlpriv->cfg->ops->cmd_send_packet(hw, skb);
 
 	if (rtstatus)
 		b_dlok = true;
@@ -793,6 +761,7 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
 }
+EXPORT_SYMBOL(rtl92c_set_fw_rsvdpagepkt);
 
 void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 {
@@ -802,3 +771,4 @@ void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 
 	rtl92c_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
 }
+EXPORT_SYMBOL(rtl92c_set_fw_joinbss_report_cmd);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/fw.h b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
index 3db33bd14666..3db33bd14666 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/main.c b/drivers/net/wireless/rtlwifi/rtl8192c/main.c
new file mode 100644
index 000000000000..2f624fc27499
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/main.c
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+
+
+MODULE_AUTHOR("lizhaoming	<chaoming_li@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Georgia		<georgia@realtek.com>");
+MODULE_AUTHOR("Ziv Huang	<ziv_huang@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 8192C/8188C 802.11n PCI wireless");
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
new file mode 100644
index 000000000000..a70228278398
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
@@ -0,0 +1,2042 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../rtl8192ce/reg.h"
+#include "../rtl8192ce/def.h"
+#include "dm_common.h"
+#include "phy_common.h"
+
+/* Define macro to shorten lines */
+#define MCS_TXPWR	mcs_txpwrlevel_origoffset
+
+u32 rtl92c_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 returnvalue, originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), "
+					       "bitmask(%#x)\n", regaddr,
+					       bitmask));
+	originalvalue = rtl_read_dword(rtlpriv, regaddr);
+	bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
+	returnvalue = (originalvalue & bitmask) >> bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("BBR MASK=0x%x "
+					       "Addr[0x%x]=0x%x\n", bitmask,
+					       regaddr, originalvalue));
+
+	return returnvalue;
+
+}
+EXPORT_SYMBOL(rtl92c_phy_query_bb_reg);
+
+void rtl92c_phy_set_bb_reg(struct ieee80211_hw *hw,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
+					       " data(%#x)\n", regaddr, bitmask,
+					       data));
+
+	if (bitmask != MASKDWORD) {
+		originalvalue = rtl_read_dword(rtlpriv, regaddr);
+		bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) | (data << bitshift));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
+					       " data(%#x)\n", regaddr, bitmask,
+					       data));
+}
+EXPORT_SYMBOL(rtl92c_phy_set_bb_reg);
+
+u32 _rtl92c_phy_fw_rf_serial_read(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset)
+{
+	RT_ASSERT(false, ("deprecated!\n"));
+	return 0;
+}
+EXPORT_SYMBOL(_rtl92c_phy_fw_rf_serial_read);
+
+void _rtl92c_phy_fw_rf_serial_write(struct ieee80211_hw *hw,
+					   enum radio_path rfpath, u32 offset,
+					   u32 data)
+{
+	RT_ASSERT(false, ("deprecated!\n"));
+}
+EXPORT_SYMBOL(_rtl92c_phy_fw_rf_serial_write);
+
+u32 _rtl92c_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+	u32 retvalue;
+
+	offset &= 0x3f;
+	newoffset = offset;
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("return all one\n"));
+		return 0xFFFFFFFF;
+	}
+	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
+	if (rfpath == RF90_PATH_A)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
+	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
+	    (newoffset << 23) | BLSSIREADEDGE;
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong & (~BLSSIREADEDGE));
+	mdelay(1);
+	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
+	mdelay(1);
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong | BLSSIREADEDGE);
+	mdelay(1);
+	if (rfpath == RF90_PATH_A)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						 BIT(8));
+	else if (rfpath == RF90_PATH_B)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						 BIT(8));
+	if (rfpi_enable)
+		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readbackpi,
+					 BLSSIREADBACKDATA);
+	else
+		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readback,
+					 BLSSIREADBACKDATA);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("RFR-%d Addr[0x%x]=0x%x\n",
+					       rfpath, pphyreg->rflssi_readback,
+					       retvalue));
+	return retvalue;
+}
+EXPORT_SYMBOL(_rtl92c_phy_rf_serial_read);
+
+void _rtl92c_phy_rf_serial_write(struct ieee80211_hw *hw,
+					enum radio_path rfpath, u32 offset,
+					u32 data)
+{
+	u32 data_and_addr;
+	u32 newoffset;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("stop\n"));
+		return;
+	}
+	offset &= 0x3f;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("RFW-%d Addr[0x%x]=0x%x\n",
+					       rfpath, pphyreg->rf3wire_offset,
+					       data_and_addr));
+}
+EXPORT_SYMBOL(_rtl92c_phy_rf_serial_write);
+
+u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+EXPORT_SYMBOL(_rtl92c_phy_calculate_bit_shift);
+
+static void _rtl92c_phy_bb_config_1t(struct ieee80211_hw *hw)
+{
+	rtl_set_bbreg(hw, RFPGA0_TXINFO, 0x3, 0x2);
+	rtl_set_bbreg(hw, RFPGA1_TXINFO, 0x300033, 0x200022);
+	rtl_set_bbreg(hw, RCCK0_AFESETTING, MASKBYTE3, 0x45);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0, 0x23);
+	rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, 0x30, 0x1);
+	rtl_set_bbreg(hw, 0xe74, 0x0c000000, 0x2);
+	rtl_set_bbreg(hw, 0xe78, 0x0c000000, 0x2);
+	rtl_set_bbreg(hw, 0xe7c, 0x0c000000, 0x2);
+	rtl_set_bbreg(hw, 0xe80, 0x0c000000, 0x2);
+	rtl_set_bbreg(hw, 0xe88, 0x0c000000, 0x2);
+}
+bool rtl92c_phy_rf_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	return rtlpriv->cfg->ops->phy_rf6052_config(hw);
+}
+EXPORT_SYMBOL(rtl92c_phy_rf_config);
+
+bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("==>\n"));
+	rtstatus = rtlpriv->cfg->ops->config_bb_with_headerfile(hw,
+						 BASEBAND_CONFIG_PHY_REG);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
+		return false;
+	}
+	if (rtlphy->rf_type == RF_1T2R) {
+		_rtl92c_phy_bb_config_1t(hw);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("Config to 1T!!\n"));
+	}
+	if (rtlefuse->autoload_failflag == false) {
+		rtlphy->pwrgroup_cnt = 0;
+		rtstatus = rtlpriv->cfg->ops->config_bb_with_pgheaderfile(hw,
+						   BASEBAND_CONFIG_PHY_REG);
+	}
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
+		return false;
+	}
+	rtstatus = rtlpriv->cfg->ops->config_bb_with_headerfile(hw,
+						 BASEBAND_CONFIG_AGC_TAB);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("AGC Table Fail\n"));
+		return false;
+	}
+	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
+						RFPGA0_XA_HSSIPARAMETER2,
+						0x200));
+	return true;
+}
+EXPORT_SYMBOL(_rtl92c_phy_bb8192c_config_parafile);
+
+void _rtl92c_store_pwrIndex_diffrate_offset(struct ieee80211_hw *hw,
+						   u32 regaddr, u32 bitmask,
+						   u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (regaddr == RTXAGC_A_RATE18_06) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][0] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][0]));
+	}
+	if (regaddr == RTXAGC_A_RATE54_24) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][1] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][1] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][1]));
+	}
+	if (regaddr == RTXAGC_A_CCK1_MCS32) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][6] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][6] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][6]));
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][7] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][7]));
+	}
+	if (regaddr == RTXAGC_A_MCS03_MCS00) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][2] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][2] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][2]));
+	}
+	if (regaddr == RTXAGC_A_MCS07_MCS04) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][3] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][3] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][3]));
+	}
+	if (regaddr == RTXAGC_A_MCS11_MCS08) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][4] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][4] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][4]));
+	}
+	if (regaddr == RTXAGC_A_MCS15_MCS12) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][5] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][5] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][5]));
+	}
+	if (regaddr == RTXAGC_B_RATE18_06) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][8] = data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][8] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][8]));
+	}
+	if (regaddr == RTXAGC_B_RATE54_24) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][9] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][9] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][9]));
+	}
+
+	if (regaddr == RTXAGC_B_CCK1_55_MCS32) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][14] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][14] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][14]));
+	}
+
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][15] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][15]));
+	}
+
+	if (regaddr == RTXAGC_B_MCS03_MCS00) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][10] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][10] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][10]));
+	}
+
+	if (regaddr == RTXAGC_B_MCS07_MCS04) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][11] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][11] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][11]));
+	}
+
+	if (regaddr == RTXAGC_B_MCS11_MCS08) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][12] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][12] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][12]));
+	}
+
+	if (regaddr == RTXAGC_B_MCS15_MCS12) {
+		rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][13] = data;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("MCSTxPowerLevelOriginalOffset[%d][13] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->MCS_TXPWR[rtlphy->pwrgroup_cnt][13]));
+
+		rtlphy->pwrgroup_cnt++;
+	}
+}
+EXPORT_SYMBOL(_rtl92c_store_pwrIndex_diffrate_offset);
+
+void rtl92c_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->default_initialgain[0] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[1] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[2] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[3] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 ("Default initial gain (c50=0x%x, "
+		  "c58=0x%x, c60=0x%x, c68=0x%x\n",
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]));
+
+	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
+					       ROFDM0_RXDETECTOR3, MASKBYTE0);
+	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
+					      ROFDM0_RXDETECTOR2, MASKDWORD);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 ("Default framesync (0x%x) = 0x%x\n",
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync));
+}
+
+void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
+	    RFPGA0_XA_LSSIPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
+	    RFPGA0_XB_LSSIPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = rFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = rFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfswitch_control =
+	    RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_B].rfswitch_control =
+	    RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_C].rfswitch_control =
+	    RFPGA0_XCD_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_D].rfswitch_control =
+	    RFPGA0_XCD_SWITCHCONTROL;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control1 = ROFDM0_XAAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control1 = ROFDM0_XBAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control2 = ROFDM0_XAAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control2 = ROFDM0_XBAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfrxiq_imbalance =
+	    ROFDM0_XARXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfrxiq_imbalance =
+	    ROFDM0_XBRXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbalance =
+	    ROFDM0_XCRXIQIMBANLANCE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbalance =
+	    ROFDM0_XDRXIQIMBALANCE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfrx_afe = ROFDM0_XARXAFE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfrx_afe = ROFDM0_XBRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftxiq_imbalance =
+	    ROFDM0_XATXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftxiq_imbalance =
+	    ROFDM0_XBTXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbalance =
+	    ROFDM0_XCTXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbalance =
+	    ROFDM0_XDTXIQIMBALANCE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftx_afe = ROFDM0_XATXAFE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftx_afe = ROFDM0_XBTXAFE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftx_afe = ROFDM0_XCTXAFE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftx_afe = ROFDM0_XDTXAFE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readback =
+	    RFPGA0_XA_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readback =
+	    RFPGA0_XB_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_C].rflssi_readback =
+	    RFPGA0_XC_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_D].rflssi_readback =
+	    RFPGA0_XD_LSSIREADBACK;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readbackpi =
+	    TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi =
+	    TRANSCEIVEB_HSPI_READBACK;
+
+}
+EXPORT_SYMBOL(_rtl92c_phy_init_bb_rf_register_definition);
+
+void rtl92c_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = _rtl92c_phy_txpwr_idx_to_dbm(hw,
+						 WIRELESS_MODE_B, txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx +
+	    rtlefuse->legacy_ht_txpowerdiff;
+	if (_rtl92c_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+						 txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl92c_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_N_24G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+						 txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static void _rtl92c_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
+				      u8 *cckpowerlevel, u8 *ofdmpowerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 index = (channel - 1);
+
+	cckpowerlevel[RF90_PATH_A] =
+	    rtlefuse->txpwrlevel_cck[RF90_PATH_A][index];
+	cckpowerlevel[RF90_PATH_B] =
+	    rtlefuse->txpwrlevel_cck[RF90_PATH_B][index];
+	if (get_rf_type(rtlphy) == RF_1T2R || get_rf_type(rtlphy) == RF_1T1R) {
+		ofdmpowerlevel[RF90_PATH_A] =
+		    rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index];
+		ofdmpowerlevel[RF90_PATH_B] =
+		    rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_B][index];
+	} else if (get_rf_type(rtlphy) == RF_2T2R) {
+		ofdmpowerlevel[RF90_PATH_A] =
+		    rtlefuse->txpwrlevel_ht40_2s[RF90_PATH_A][index];
+		ofdmpowerlevel[RF90_PATH_B] =
+		    rtlefuse->txpwrlevel_ht40_2s[RF90_PATH_B][index];
+	}
+}
+
+static void _rtl92c_ccxpower_index_check(struct ieee80211_hw *hw,
+					 u8 channel, u8 *cckpowerlevel,
+					 u8 *ofdmpowerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->cur_cck_txpwridx = cckpowerlevel[0];
+	rtlphy->cur_ofdm24g_txpwridx = ofdmpowerlevel[0];
+}
+
+void rtl92c_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	u8 cckpowerlevel[2], ofdmpowerlevel[2];
+
+	if (rtlefuse->txpwr_fromeprom == false)
+		return;
+	_rtl92c_get_txpower_index(hw, channel,
+				  &cckpowerlevel[0], &ofdmpowerlevel[0]);
+	_rtl92c_ccxpower_index_check(hw,
+				     channel, &cckpowerlevel[0],
+				     &ofdmpowerlevel[0]);
+	rtlpriv->cfg->ops->phy_rf6052_set_cck_txpower(hw, &cckpowerlevel[0]);
+	rtlpriv->cfg->ops->phy_rf6052_set_ofdm_txpower(hw, &ofdmpowerlevel[0],
+						       channel);
+}
+EXPORT_SYMBOL(rtl92c_phy_set_txpower_level);
+
+bool rtl92c_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 idx;
+	u8 rf_path;
+
+	u8 ccktxpwridx = _rtl92c_phy_dbm_to_txpwr_Idx(hw,
+						      WIRELESS_MODE_B,
+						      power_indbm);
+	u8 ofdmtxpwridx = _rtl92c_phy_dbm_to_txpwr_Idx(hw,
+						       WIRELESS_MODE_N_24G,
+						       power_indbm);
+	if (ofdmtxpwridx - rtlefuse->legacy_ht_txpowerdiff > 0)
+		ofdmtxpwridx -= rtlefuse->legacy_ht_txpowerdiff;
+	else
+		ofdmtxpwridx = 0;
+	RT_TRACE(rtlpriv, COMP_TXAGC, DBG_TRACE,
+		 ("%lx dBm, ccktxpwridx = %d, ofdmtxpwridx = %d\n",
+		  power_indbm, ccktxpwridx, ofdmtxpwridx));
+	for (idx = 0; idx < 14; idx++) {
+		for (rf_path = 0; rf_path < 2; rf_path++) {
+			rtlefuse->txpwrlevel_cck[rf_path][idx] = ccktxpwridx;
+			rtlefuse->txpwrlevel_ht40_1s[rf_path][idx] =
+			    ofdmtxpwridx;
+			rtlefuse->txpwrlevel_ht40_2s[rf_path][idx] =
+			    ofdmtxpwridx;
+		}
+	}
+	rtl92c_phy_set_txpower_level(hw, rtlphy->current_channel);
+	return true;
+}
+EXPORT_SYMBOL(rtl92c_phy_update_txpower_dbm);
+
+void rtl92c_phy_set_beacon_hw_reg(struct ieee80211_hw *hw, u16 beaconinterval)
+{
+}
+EXPORT_SYMBOL(rtl92c_phy_set_beacon_hw_reg);
+
+u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
+				enum wireless_mode wirelessmode,
+				long power_indbm)
+{
+	u8 txpwridx;
+	long offset;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+
+	if ((power_indbm - offset) > 0)
+		txpwridx = (u8) ((power_indbm - offset) * 2);
+	else
+		txpwridx = 0;
+
+	if (txpwridx > MAX_TXPWR_IDX_NMODE_92S)
+		txpwridx = MAX_TXPWR_IDX_NMODE_92S;
+
+	return txpwridx;
+}
+EXPORT_SYMBOL(_rtl92c_phy_dbm_to_txpwr_Idx);
+
+long _rtl92c_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+				  enum wireless_mode wirelessmode,
+				  u8 txpwridx)
+{
+	long offset;
+	long pwrout_dbm;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+EXPORT_SYMBOL(_rtl92c_phy_txpwr_idx_to_dbm);
+
+void rtl92c_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum io_type iotype;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP:
+			iotype = IO_CMD_PAUSE_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 ("Unknown Scan Backup operation.\n"));
+			break;
+		}
+	}
+}
+EXPORT_SYMBOL(rtl92c_phy_scan_operation_backup);
+
+void rtl92c_phy_set_bw_mode(struct ieee80211_hw *hw,
+			    enum nl80211_channel_type ch_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_bw = rtlphy->current_chan_bw;
+
+	if (rtlphy->set_bwmode_inprogress)
+		return;
+	rtlphy->set_bwmode_inprogress = true;
+	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw)))
+		rtlpriv->cfg->ops->phy_set_bw_mode_callback(hw);
+	else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 ("FALSE driver sleep or unload\n"));
+		rtlphy->set_bwmode_inprogress = false;
+		rtlphy->current_chan_bw = tmp_bw;
+	}
+}
+EXPORT_SYMBOL(rtl92c_phy_set_bw_mode);
+
+void rtl92c_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 delay;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 ("switch to channel%d\n", rtlphy->current_channel));
+	if (is_hal_stop(rtlhal))
+		return;
+	do {
+		if (!rtlphy->sw_chnl_inprogress)
+			break;
+		if (!_rtl92c_phy_sw_chnl_step_by_step
+		    (hw, rtlphy->current_channel, &rtlphy->sw_chnl_stage,
+		     &rtlphy->sw_chnl_step, &delay)) {
+			if (delay > 0)
+				mdelay(delay);
+			else
+				continue;
+		} else
+			rtlphy->sw_chnl_inprogress = false;
+		break;
+	} while (true);
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, ("<==\n"));
+}
+EXPORT_SYMBOL(rtl92c_phy_sw_chnl_callback);
+
+u8 rtl92c_phy_sw_chnl(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlphy->sw_chnl_inprogress)
+		return 0;
+	if (rtlphy->set_bwmode_inprogress)
+		return 0;
+	RT_ASSERT((rtlphy->current_channel <= 14),
+		  ("WIRELESS_MODE_G but channel>14"));
+	rtlphy->sw_chnl_inprogress = true;
+	rtlphy->sw_chnl_stage = 0;
+	rtlphy->sw_chnl_step = 0;
+	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
+		rtl92c_phy_sw_chnl_callback(hw);
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 ("sw_chnl_inprogress false schdule workitem\n"));
+		rtlphy->sw_chnl_inprogress = false;
+	} else {
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 ("sw_chnl_inprogress false driver sleep or"
+			  " unload\n"));
+		rtlphy->sw_chnl_inprogress = false;
+	}
+	return 1;
+}
+EXPORT_SYMBOL(rtl92c_phy_sw_chnl);
+
+static bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					     u8 channel, u8 *stage, u8 *step,
+					     u32 *delay)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
+	u32 precommoncmdcnt;
+	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
+	u32 postcommoncmdcnt;
+	struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
+	u32 rfdependcmdcnt;
+	struct swchnlcmd *currentcmd = NULL;
+	u8 rfpath;
+	u8 num_total_rfpath = rtlphy->num_total_rfpath;
+
+	precommoncmdcnt = 0;
+	_rtl92c_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT,
+					 CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
+	_rtl92c_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
+	postcommoncmdcnt = 0;
+
+	_rtl92c_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
+					 MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
+
+	rfdependcmdcnt = 0;
+
+	RT_ASSERT((channel >= 1 && channel <= 14),
+		  ("illegal channel for Zebra: %d\n", channel));
+
+	_rtl92c_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
+					 RF_CHNLBW, channel, 10);
+
+	_rtl92c_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0,
+					 0);
+
+	do {
+		switch (*stage) {
+		case 0:
+			currentcmd = &precommoncmd[*step];
+			break;
+		case 1:
+			currentcmd = &rfdependcmd[*step];
+			break;
+		case 2:
+			currentcmd = &postcommoncmd[*step];
+			break;
+		}
+
+		if (currentcmd->cmdid == CMDID_END) {
+			if ((*stage) == 2) {
+				return true;
+			} else {
+				(*stage)++;
+				(*step) = 0;
+				continue;
+			}
+		}
+
+		switch (currentcmd->cmdid) {
+		case CMDID_SET_TXPOWEROWER_LEVEL:
+			rtl92c_phy_set_txpower_level(hw, channel);
+			break;
+		case CMDID_WRITEPORT_ULONG:
+			rtl_write_dword(rtlpriv, currentcmd->para1,
+					currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_USHORT:
+			rtl_write_word(rtlpriv, currentcmd->para1,
+				       (u16) currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_UCHAR:
+			rtl_write_byte(rtlpriv, currentcmd->para1,
+				       (u8) currentcmd->para2);
+			break;
+		case CMDID_RF_WRITEREG:
+			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
+				rtlphy->rfreg_chnlval[rfpath] =
+				    ((rtlphy->rfreg_chnlval[rfpath] &
+				      0xfffffc00) | currentcmd->para2);
+
+				rtl_set_rfreg(hw, (enum radio_path)rfpath,
+					      currentcmd->para1,
+					      RFREG_OFFSET_MASK,
+					      rtlphy->rfreg_chnlval[rfpath]);
+			}
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+
+		break;
+	} while (true);
+
+	(*delay) = currentcmd->msdelay;
+	(*step)++;
+	return false;
+}
+
+static bool _rtl92c_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					     u32 cmdtableidx, u32 cmdtablesz,
+					     enum swchnlcmd_id cmdid,
+					     u32 para1, u32 para2, u32 msdelay)
+{
+	struct swchnlcmd *pcmd;
+
+	if (cmdtable == NULL) {
+		RT_ASSERT(false, ("cmdtable cannot be NULL.\n"));
+		return false;
+	}
+
+	if (cmdtableidx >= cmdtablesz)
+		return false;
+
+	pcmd = cmdtable + cmdtableidx;
+	pcmd->cmdid = cmdid;
+	pcmd->para1 = para1;
+	pcmd->para2 = para2;
+	pcmd->msdelay = msdelay;
+	return true;
+}
+
+bool rtl8192_phy_check_is_legal_rfpath(struct ieee80211_hw *hw, u32 rfpath)
+{
+	return true;
+}
+EXPORT_SYMBOL(rtl8192_phy_check_is_legal_rfpath);
+
+static u8 _rtl92c_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
+	u8 result = 0x00;
+
+	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1f);
+	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x10008c1f);
+	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x82140102);
+	rtl_set_bbreg(hw, 0xe3c, MASKDWORD,
+		      config_pathb ? 0x28160202 : 0x28160502);
+
+	if (config_pathb) {
+		rtl_set_bbreg(hw, 0xe50, MASKDWORD, 0x10008c22);
+		rtl_set_bbreg(hw, 0xe54, MASKDWORD, 0x10008c22);
+		rtl_set_bbreg(hw, 0xe58, MASKDWORD, 0x82140102);
+		rtl_set_bbreg(hw, 0xe5c, MASKDWORD, 0x28160202);
+	}
+
+	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x001028d1);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	if (!(reg_eac & BIT(27)) &&
+	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	return result;
+}
+
+static u8 _rtl92c_phy_path_b_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_eac, reg_eb4, reg_ebc, reg_ec4, reg_ecc;
+	u8 result = 0x00;
+
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000002);
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000000);
+	mdelay(IQK_DELAY_TIME);
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_eb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
+	reg_ebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
+	reg_ec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
+	reg_ecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	if (!(reg_eac & BIT(30)) &&
+	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	return result;
+}
+
+static void _rtl92c_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
+					       bool iqk_ok, long result[][8],
+					       u8 final_candidate, bool btxonly)
+{
+	u32 oldval_0, x, tx0_a, reg;
+	long y, tx0_c;
+
+	if (final_candidate == 0xFF)
+		return;
+	else if (iqk_ok) {
+		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final_candidate][0];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx0_a = (x * oldval_0) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x3FF, tx0_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(31),
+			      ((x * oldval_0 >> 7) & 0x1));
+		y = result[final_candidate][1];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+		tx0_c = (y * oldval_0) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XCTXAFE, 0xF0000000,
+			      ((tx0_c & 0x3C0) >> 6));
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x003F0000,
+			      (tx0_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(29),
+			      ((y * oldval_0 >> 7) & 0x1));
+		if (btxonly)
+			return;
+		reg = result[final_candidate][2];
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0x3FF, reg);
+		reg = result[final_candidate][3] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0xFC00, reg);
+		reg = (result[final_candidate][3] >> 6) & 0xF;
+		rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg);
+	}
+}
+
+static void _rtl92c_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
+					       bool iqk_ok, long result[][8],
+					       u8 final_candidate, bool btxonly)
+{
+	u32 oldval_1, x, tx1_a, reg;
+	long y, tx1_c;
+
+	if (final_candidate == 0xFF)
+		return;
+	else if (iqk_ok) {
+		oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final_candidate][4];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx1_a = (x * oldval_1) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x3FF, tx1_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(27),
+			      ((x * oldval_1 >> 7) & 0x1));
+		y = result[final_candidate][5];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+		tx1_c = (y * oldval_1) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XDTXAFE, 0xF0000000,
+			      ((tx1_c & 0x3C0) >> 6));
+		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x003F0000,
+			      (tx1_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(25),
+			      ((y * oldval_1 >> 7) & 0x1));
+		if (btxonly)
+			return;
+		reg = result[final_candidate][6];
+		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0x3FF, reg);
+		reg = result[final_candidate][7] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0xFC00, reg);
+		reg = (result[final_candidate][7] >> 6) & 0xF;
+		rtl_set_bbreg(hw, ROFDM0_AGCRSSITABLE, 0x0000F000, reg);
+	}
+}
+
+static void _rtl92c_phy_save_adda_registers(struct ieee80211_hw *hw,
+					    u32 *addareg, u32 *addabackup,
+					    u32 registernum)
+{
+	u32 i;
+
+	for (i = 0; i < registernum; i++)
+		addabackup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
+}
+
+static void _rtl92c_phy_save_mac_registers(struct ieee80211_hw *hw,
+					   u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		macbackup[i] = rtl_read_byte(rtlpriv, macreg[i]);
+	macbackup[i] = rtl_read_dword(rtlpriv, macreg[i]);
+}
+
+static void _rtl92c_phy_reload_adda_registers(struct ieee80211_hw *hw,
+					      u32 *addareg, u32 *addabackup,
+					      u32 regiesternum)
+{
+	u32 i;
+
+	for (i = 0; i < regiesternum; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, addabackup[i]);
+}
+
+static void _rtl92c_phy_reload_mac_registers(struct ieee80211_hw *hw,
+					     u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
+	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
+}
+
+static void _rtl92c_phy_path_adda_on(struct ieee80211_hw *hw,
+				     u32 *addareg, bool is_patha_on, bool is2t)
+{
+	u32 pathOn;
+	u32 i;
+
+	pathOn = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
+	if (false == is2t) {
+		pathOn = 0x0bdb25a0;
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
+	} else {
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathOn);
+	}
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, pathOn);
+}
+
+static void _rtl92c_phy_mac_setting_calibration(struct ieee80211_hw *hw,
+						u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_write_byte(rtlpriv, macreg[0], 0x3F);
+
+	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i],
+			       (u8) (macbackup[i] & (~BIT(3))));
+	rtl_write_byte(rtlpriv, macreg[i], (u8) (macbackup[i] & (~BIT(5))));
+}
+
+static void _rtl92c_phy_path_a_standby(struct ieee80211_hw *hw)
+{
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
+	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+}
+
+static void _rtl92c_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode)
+{
+	u32 mode;
+
+	mode = pi_mode ? 0x01000100 : 0x01000000;
+	rtl_set_bbreg(hw, 0x820, MASKDWORD, mode);
+	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
+}
+
+static bool _rtl92c_phy_simularity_compare(struct ieee80211_hw *hw,
+					   long result[][8], u8 c1, u8 c2)
+{
+	u32 i, j, diff, simularity_bitmap, bound;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 final_candidate[2] = { 0xFF, 0xFF };
+	bool bresult = true, is2t = IS_92C_SERIAL(rtlhal->version);
+
+	if (is2t)
+		bound = 8;
+	else
+		bound = 4;
+
+	simularity_bitmap = 0;
+
+	for (i = 0; i < bound; i++) {
+		diff = (result[c1][i] > result[c2][i]) ?
+		    (result[c1][i] - result[c2][i]) :
+		    (result[c2][i] - result[c1][i]);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !simularity_bitmap) {
+				if (result[c1][i] + result[c1][i + 1] == 0)
+					final_candidate[(i / 4)] = c2;
+				else if (result[c2][i] + result[c2][i + 1] == 0)
+					final_candidate[(i / 4)] = c1;
+				else
+					simularity_bitmap = simularity_bitmap |
+					    (1 << i);
+			} else
+				simularity_bitmap =
+				    simularity_bitmap | (1 << i);
+		}
+	}
+
+	if (simularity_bitmap == 0) {
+		for (i = 0; i < (bound / 4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
+					result[3][j] =
+					    result[final_candidate[i]][j];
+				bresult = false;
+			}
+		}
+		return bresult;
+	} else if (!(simularity_bitmap & 0x0F)) {
+		for (i = 0; i < 4; i++)
+			result[3][i] = result[c1][i];
+		return false;
+	} else if (!(simularity_bitmap & 0xF0) && is2t) {
+		for (i = 4; i < 8; i++)
+			result[3][i] = result[c1][i];
+		return false;
+	} else {
+		return false;
+	}
+
+}
+
+static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
+				     long result[][8], u8 t, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 i;
+	u8 patha_ok, pathb_ok;
+	u32 adda_reg[IQK_ADDA_REG_NUM] = {
+		0x85c, 0xe6c, 0xe70, 0xe74,
+		0xe78, 0xe7c, 0xe80, 0xe84,
+		0xe88, 0xe8c, 0xed0, 0xed4,
+		0xed8, 0xedc, 0xee0, 0xeec
+	};
+
+	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
+		0x522, 0x550, 0x551, 0x040
+	};
+
+	const u32 retrycount = 2;
+
+	u32 bbvalue;
+
+	if (t == 0) {
+		bbvalue = rtl_get_bbreg(hw, 0x800, MASKDWORD);
+
+		_rtl92c_phy_save_adda_registers(hw, adda_reg,
+						rtlphy->adda_backup, 16);
+		_rtl92c_phy_save_mac_registers(hw, iqk_mac_reg,
+					       rtlphy->iqk_mac_backup);
+	}
+	_rtl92c_phy_path_adda_on(hw, adda_reg, true, is2t);
+	if (t == 0) {
+		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
+						   RFPGA0_XA_HSSIPARAMETER1,
+						   BIT(8));
+	}
+	if (!rtlphy->rfpi_enable)
+		_rtl92c_phy_pi_mode_switch(hw, true);
+	if (t == 0) {
+		rtlphy->reg_c04 = rtl_get_bbreg(hw, 0xc04, MASKDWORD);
+		rtlphy->reg_c08 = rtl_get_bbreg(hw, 0xc08, MASKDWORD);
+		rtlphy->reg_874 = rtl_get_bbreg(hw, 0x874, MASKDWORD);
+	}
+	rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
+	if (is2t) {
+		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
+		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00010000);
+	}
+	_rtl92c_phy_mac_setting_calibration(hw, iqk_mac_reg,
+					    rtlphy->iqk_mac_backup);
+	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x00080000);
+	if (is2t)
+		rtl_set_bbreg(hw, 0xb6c, MASKDWORD, 0x00080000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x01004800);
+	for (i = 0; i < retrycount; i++) {
+		patha_ok = _rtl92c_phy_path_a_iqk(hw, is2t);
+		if (patha_ok == 0x03) {
+			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
+					0x3FF0000) >> 16;
+			break;
+		} else if (i == (retrycount - 1) && patha_ok == 0x01)
+			result[t][0] = (rtl_get_bbreg(hw, 0xe94,
+						      MASKDWORD) & 0x3FF0000) >>
+						      16;
+		result[t][1] =
+		    (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) & 0x3FF0000) >> 16;
+
+	}
+
+	if (is2t) {
+		_rtl92c_phy_path_a_standby(hw);
+		_rtl92c_phy_path_adda_on(hw, adda_reg, false, is2t);
+		for (i = 0; i < retrycount; i++) {
+			pathb_ok = _rtl92c_phy_path_b_iqk(hw);
+			if (pathb_ok == 0x03) {
+				result[t][4] = (rtl_get_bbreg(hw,
+						      0xeb4,
+						      MASKDWORD) &
+						0x3FF0000) >> 16;
+				result[t][5] =
+				    (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
+				     0x3FF0000) >> 16;
+				result[t][6] =
+				    (rtl_get_bbreg(hw, 0xec4, MASKDWORD) &
+				     0x3FF0000) >> 16;
+				result[t][7] =
+				    (rtl_get_bbreg(hw, 0xecc, MASKDWORD) &
+				     0x3FF0000) >> 16;
+				break;
+			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
+				result[t][4] = (rtl_get_bbreg(hw,
+						      0xeb4,
+						      MASKDWORD) &
+						0x3FF0000) >> 16;
+			}
+			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
+					0x3FF0000) >> 16;
+		}
+	}
+	rtl_set_bbreg(hw, 0xc04, MASKDWORD, rtlphy->reg_c04);
+	rtl_set_bbreg(hw, 0x874, MASKDWORD, rtlphy->reg_874);
+	rtl_set_bbreg(hw, 0xc08, MASKDWORD, rtlphy->reg_c08);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
+	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00032ed3);
+	if (is2t)
+		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00032ed3);
+	if (t != 0) {
+		if (!rtlphy->rfpi_enable)
+			_rtl92c_phy_pi_mode_switch(hw, false);
+		_rtl92c_phy_reload_adda_registers(hw, adda_reg,
+						  rtlphy->adda_backup, 16);
+		_rtl92c_phy_reload_mac_registers(hw, iqk_mac_reg,
+						 rtlphy->iqk_mac_backup);
+	}
+}
+
+static void _rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw,
+				     char delta, bool is2t)
+{
+	/* This routine is deliberately dummied out for later fixes */
+#if 0
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	u32 reg_d[PATH_NUM];
+	u32 tmpreg, index, offset, path, i, pathbound = PATH_NUM, apkbound;
+
+	u32 bb_backup[APK_BB_REG_NUM];
+	u32 bb_reg[APK_BB_REG_NUM] = {
+		0x904, 0xc04, 0x800, 0xc08, 0x874
+	};
+	u32 bb_ap_mode[APK_BB_REG_NUM] = {
+		0x00000020, 0x00a05430, 0x02040000,
+		0x000800e4, 0x00204000
+	};
+	u32 bb_normal_ap_mode[APK_BB_REG_NUM] = {
+		0x00000020, 0x00a05430, 0x02040000,
+		0x000800e4, 0x22204000
+	};
+
+	u32 afe_backup[APK_AFE_REG_NUM];
+	u32 afe_reg[APK_AFE_REG_NUM] = {
+		0x85c, 0xe6c, 0xe70, 0xe74, 0xe78,
+		0xe7c, 0xe80, 0xe84, 0xe88, 0xe8c,
+		0xed0, 0xed4, 0xed8, 0xedc, 0xee0,
+		0xeec
+	};
+
+	u32 mac_backup[IQK_MAC_REG_NUM];
+	u32 mac_reg[IQK_MAC_REG_NUM] = {
+		0x522, 0x550, 0x551, 0x040
+	};
+
+	u32 apk_rf_init_value[PATH_NUM][APK_BB_REG_NUM] = {
+		{0x0852c, 0x1852c, 0x5852c, 0x1852c, 0x5852c},
+		{0x2852e, 0x0852e, 0x3852e, 0x0852e, 0x0852e}
+	};
+
+	u32 apk_normal_rf_init_value[PATH_NUM][APK_BB_REG_NUM] = {
+		{0x0852c, 0x0a52c, 0x3a52c, 0x5a52c, 0x5a52c},
+		{0x0852c, 0x0a52c, 0x5a52c, 0x5a52c, 0x5a52c}
+	};
+
+	u32 apk_rf_value_0[PATH_NUM][APK_BB_REG_NUM] = {
+		{0x52019, 0x52014, 0x52013, 0x5200f, 0x5208d},
+		{0x5201a, 0x52019, 0x52016, 0x52033, 0x52050}
+	};
+
+	u32 apk_normal_rf_value_0[PATH_NUM][APK_BB_REG_NUM] = {
+		{0x52019, 0x52017, 0x52010, 0x5200d, 0x5206a},
+		{0x52019, 0x52017, 0x52010, 0x5200d, 0x5206a}
+	};
+
+	u32 afe_on_off[PATH_NUM] = {
+		0x04db25a4, 0x0b1b25a4
+	};
+
+	u32 apk_offset[PATH_NUM] = { 0xb68, 0xb6c };
+
+	u32 apk_normal_offset[PATH_NUM] = { 0xb28, 0xb98 };
+
+	u32 apk_value[PATH_NUM] = { 0x92fc0000, 0x12fc0000 };
+
+	u32 apk_normal_value[PATH_NUM] = { 0x92680000, 0x12680000 };
+
+	const char apk_delta_mapping[APK_BB_REG_NUM][13] = {
+		{-4, -3, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
+		{-4, -3, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
+		{-6, -4, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
+		{-1, -1, -1, -1, -1, -1, 0, 1, 2, 3, 4, 5, 6},
+		{-11, -9, -7, -5, -3, -1, 0, 0, 0, 0, 0, 0, 0}
+	};
+
+	const u32 apk_normal_setting_value_1[13] = {
+		0x01017018, 0xf7ed8f84, 0x1b1a1816, 0x2522201e, 0x322e2b28,
+		0x433f3a36, 0x5b544e49, 0x7b726a62, 0xa69a8f84, 0xdfcfc0b3,
+		0x12680000, 0x00880000, 0x00880000
+	};
+
+	const u32 apk_normal_setting_value_2[16] = {
+		0x01c7021d, 0x01670183, 0x01000123, 0x00bf00e2, 0x008d00a3,
+		0x0068007b, 0x004d0059, 0x003a0042, 0x002b0031, 0x001f0025,
+		0x0017001b, 0x00110014, 0x000c000f, 0x0009000b, 0x00070008,
+		0x00050006
+	};
+
+	const u32 apk_result[PATH_NUM][APK_BB_REG_NUM];
+
+	long bb_offset, delta_v, delta_offset;
+
+	if (!is2t)
+		pathbound = 1;
+
+	for (index = 0; index < PATH_NUM; index++) {
+		apk_offset[index] = apk_normal_offset[index];
+		apk_value[index] = apk_normal_value[index];
+		afe_on_off[index] = 0x6fdb25a4;
+	}
+
+	for (index = 0; index < APK_BB_REG_NUM; index++) {
+		for (path = 0; path < pathbound; path++) {
+			apk_rf_init_value[path][index] =
+			    apk_normal_rf_init_value[path][index];
+			apk_rf_value_0[path][index] =
+			    apk_normal_rf_value_0[path][index];
+		}
+		bb_ap_mode[index] = bb_normal_ap_mode[index];
+
+		apkbound = 6;
+	}
+
+	for (index = 0; index < APK_BB_REG_NUM; index++) {
+		if (index == 0)
+			continue;
+		bb_backup[index] = rtl_get_bbreg(hw, bb_reg[index], MASKDWORD);
+	}
+
+	_rtl92c_phy_save_mac_registers(hw, mac_reg, mac_backup);
+
+	_rtl92c_phy_save_adda_registers(hw, afe_reg, afe_backup, 16);
+
+	for (path = 0; path < pathbound; path++) {
+		if (path == RF90_PATH_A) {
+			offset = 0xb00;
+			for (index = 0; index < 11; index++) {
+				rtl_set_bbreg(hw, offset, MASKDWORD,
+					      apk_normal_setting_value_1
+					      [index]);
+
+				offset += 0x04;
+			}
+
+			rtl_set_bbreg(hw, 0xb98, MASKDWORD, 0x12680000);
+
+			offset = 0xb68;
+			for (; index < 13; index++) {
+				rtl_set_bbreg(hw, offset, MASKDWORD,
+					      apk_normal_setting_value_1
+					      [index]);
+
+				offset += 0x04;
+			}
+
+			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x40000000);
+
+			offset = 0xb00;
+			for (index = 0; index < 16; index++) {
+				rtl_set_bbreg(hw, offset, MASKDWORD,
+					      apk_normal_setting_value_2
+					      [index]);
+
+				offset += 0x04;
+			}
+			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
+		} else if (path == RF90_PATH_B) {
+			offset = 0xb70;
+			for (index = 0; index < 10; index++) {
+				rtl_set_bbreg(hw, offset, MASKDWORD,
+					      apk_normal_setting_value_1
+					      [index]);
+
+				offset += 0x04;
+			}
+			rtl_set_bbreg(hw, 0xb28, MASKDWORD, 0x12680000);
+			rtl_set_bbreg(hw, 0xb98, MASKDWORD, 0x12680000);
+
+			offset = 0xb68;
+			index = 11;
+			for (; index < 13; index++) {
+				rtl_set_bbreg(hw, offset, MASKDWORD,
+					      apk_normal_setting_value_1
+					      [index]);
+
+				offset += 0x04;
+			}
+
+			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x40000000);
+
+			offset = 0xb60;
+			for (index = 0; index < 16; index++) {
+				rtl_set_bbreg(hw, offset, MASKDWORD,
+					      apk_normal_setting_value_2
+					      [index]);
+
+				offset += 0x04;
+			}
+			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
+		}
+
+		reg_d[path] = rtl_get_rfreg(hw, (enum radio_path)path,
+					    0xd, MASKDWORD);
+
+		for (index = 0; index < APK_AFE_REG_NUM; index++)
+			rtl_set_bbreg(hw, afe_reg[index], MASKDWORD,
+				      afe_on_off[path]);
+
+		if (path == RF90_PATH_A) {
+			for (index = 0; index < APK_BB_REG_NUM; index++) {
+				if (index == 0)
+					continue;
+				rtl_set_bbreg(hw, bb_reg[index], MASKDWORD,
+					      bb_ap_mode[index]);
+			}
+		}
+
+		_rtl92c_phy_mac_setting_calibration(hw, mac_reg, mac_backup);
+
+		if (path == 0) {
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x0, MASKDWORD, 0x10000);
+		} else {
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASKDWORD,
+				      0x10000);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x10, MASKDWORD,
+				      0x1000f);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x11, MASKDWORD,
+				      0x20103);
+		}
+
+		delta_offset = ((delta + 14) / 2);
+		if (delta_offset < 0)
+			delta_offset = 0;
+		else if (delta_offset > 12)
+			delta_offset = 12;
+
+		for (index = 0; index < APK_BB_REG_NUM; index++) {
+			if (index != 1)
+				continue;
+
+			tmpreg = apk_rf_init_value[path][index];
+
+			if (!rtlefuse->apk_thermalmeterignore) {
+				bb_offset = (tmpreg & 0xF0000) >> 16;
+
+				if (!(tmpreg & BIT(15)))
+					bb_offset = -bb_offset;
+
+				delta_v =
+				    apk_delta_mapping[index][delta_offset];
+
+				bb_offset += delta_v;
+
+				if (bb_offset < 0) {
+					tmpreg = tmpreg & (~BIT(15));
+					bb_offset = -bb_offset;
+				} else {
+					tmpreg = tmpreg | BIT(15);
+				}
+
+				tmpreg =
+				    (tmpreg & 0xFFF0FFFF) | (bb_offset << 16);
+			}
+
+			rtl_set_rfreg(hw, (enum radio_path)path, 0xc,
+				      MASKDWORD, 0x8992e);
+			rtl_set_rfreg(hw, (enum radio_path)path, 0x0,
+				      MASKDWORD, apk_rf_value_0[path][index]);
+			rtl_set_rfreg(hw, (enum radio_path)path, 0xd,
+				      MASKDWORD, tmpreg);
+
+			i = 0;
+			do {
+				rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80000000);
+				rtl_set_bbreg(hw, apk_offset[path],
+					      MASKDWORD, apk_value[0]);
+				RTPRINT(rtlpriv, FINIT, INIT_IQK,
+					("PHY_APCalibrate() offset 0x%x "
+					 "value 0x%x\n",
+					 apk_offset[path],
+					 rtl_get_bbreg(hw, apk_offset[path],
+						       MASKDWORD)));
+
+				mdelay(3);
+
+				rtl_set_bbreg(hw, apk_offset[path],
+					      MASKDWORD, apk_value[1]);
+				RTPRINT(rtlpriv, FINIT, INIT_IQK,
+					("PHY_APCalibrate() offset 0x%x "
+					 "value 0x%x\n",
+					 apk_offset[path],
+					 rtl_get_bbreg(hw, apk_offset[path],
+						       MASKDWORD)));
+
+				mdelay(20);
+
+				rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
+
+				if (path == RF90_PATH_A)
+					tmpreg = rtl_get_bbreg(hw, 0xbd8,
+							       0x03E00000);
+				else
+					tmpreg = rtl_get_bbreg(hw, 0xbd8,
+							       0xF8000000);
+
+				RTPRINT(rtlpriv, FINIT, INIT_IQK,
+					("PHY_APCalibrate() offset "
+					 "0xbd8[25:21] %x\n", tmpreg));
+
+				i++;
+
+			} while (tmpreg > apkbound && i < 4);
+
+			apk_result[path][index] = tmpreg;
+		}
+	}
+
+	_rtl92c_phy_reload_mac_registers(hw, mac_reg, mac_backup);
+
+	for (index = 0; index < APK_BB_REG_NUM; index++) {
+		if (index == 0)
+			continue;
+		rtl_set_bbreg(hw, bb_reg[index], MASKDWORD, bb_backup[index]);
+	}
+
+	_rtl92c_phy_reload_adda_registers(hw, afe_reg, afe_backup, 16);
+
+	for (path = 0; path < pathbound; path++) {
+		rtl_set_rfreg(hw, (enum radio_path)path, 0xd,
+			      MASKDWORD, reg_d[path]);
+
+		if (path == RF90_PATH_B) {
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x10, MASKDWORD,
+				      0x1000f);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x11, MASKDWORD,
+				      0x20101);
+		}
+
+		if (apk_result[path][1] > 6)
+			apk_result[path][1] = 6;
+	}
+
+	for (path = 0; path < pathbound; path++) {
+		rtl_set_rfreg(hw, (enum radio_path)path, 0x3, MASKDWORD,
+			      ((apk_result[path][1] << 15) |
+			       (apk_result[path][1] << 10) |
+			       (apk_result[path][1] << 5) |
+			       apk_result[path][1]));
+
+		if (path == RF90_PATH_A)
+			rtl_set_rfreg(hw, (enum radio_path)path, 0x4, MASKDWORD,
+				      ((apk_result[path][1] << 15) |
+				       (apk_result[path][1] << 10) |
+				       (0x00 << 5) | 0x05));
+		else
+			rtl_set_rfreg(hw, (enum radio_path)path, 0x4, MASKDWORD,
+				      ((apk_result[path][1] << 15) |
+				       (apk_result[path][1] << 10) |
+				       (0x02 << 5) | 0x05));
+
+		rtl_set_rfreg(hw, (enum radio_path)path, 0xe, MASKDWORD,
+			      ((0x08 << 15) | (0x08 << 10) | (0x08 << 5) |
+			       0x08));
+
+	}
+
+	rtlphy->apk_done = true;
+#endif
+}
+
+static void _rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw,
+					  bool bmain, bool is2t)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (is_hal_stop(rtlhal)) {
+		rtl_set_bbreg(hw, REG_LEDCFG0, BIT(23), 0x01);
+		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+	}
+	if (is2t) {
+		if (bmain)
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x1);
+		else
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x2);
+	} else {
+		if (bmain)
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x2);
+		else
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x1);
+
+	}
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	long result[4][8];
+	u8 i, final_candidate;
+	bool patha_ok, pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
+	    reg_ecc, reg_tmp = 0;
+	bool is12simular, is13simular, is23simular;
+	bool start_conttx = false, singletone = false;
+	u32 iqk_bb_reg[10] = {
+		ROFDM0_XARXIQIMBALANCE,
+		ROFDM0_XBRXIQIMBALANCE,
+		ROFDM0_ECCATHRESHOLD,
+		ROFDM0_AGCRSSITABLE,
+		ROFDM0_XATXIQIMBALANCE,
+		ROFDM0_XBTXIQIMBALANCE,
+		ROFDM0_XCTXIQIMBALANCE,
+		ROFDM0_XCTXAFE,
+		ROFDM0_XDTXAFE,
+		ROFDM0_RXIQEXTANTA
+	};
+
+	if (recovery) {
+		_rtl92c_phy_reload_adda_registers(hw,
+						  iqk_bb_reg,
+						  rtlphy->iqk_bb_backup, 10);
+		return;
+	}
+	if (start_conttx || singletone)
+		return;
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+		result[3][i] = 0;
+	}
+	final_candidate = 0xff;
+	patha_ok = false;
+	pathb_ok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+	for (i = 0; i < 3; i++) {
+		if (IS_92C_SERIAL(rtlhal->version))
+			_rtl92c_phy_iq_calibrate(hw, result, i, true);
+		else
+			_rtl92c_phy_iq_calibrate(hw, result, i, false);
+		if (i == 1) {
+			is12simular = _rtl92c_phy_simularity_compare(hw,
+								     result, 0,
+								     1);
+			if (is12simular) {
+				final_candidate = 0;
+				break;
+			}
+		}
+		if (i == 2) {
+			is13simular = _rtl92c_phy_simularity_compare(hw,
+								     result, 0,
+								     2);
+			if (is13simular) {
+				final_candidate = 0;
+				break;
+			}
+			is23simular = _rtl92c_phy_simularity_compare(hw,
+								     result, 1,
+								     2);
+			if (is23simular)
+				final_candidate = 1;
+			else {
+				for (i = 0; i < 8; i++)
+					reg_tmp += result[3][i];
+
+				if (reg_tmp != 0)
+					final_candidate = 3;
+				else
+					final_candidate = 0xFF;
+			}
+		}
+	}
+	for (i = 0; i < 4; i++) {
+		reg_e94 = result[i][0];
+		reg_e9c = result[i][1];
+		reg_ea4 = result[i][2];
+		reg_eac = result[i][3];
+		reg_eb4 = result[i][4];
+		reg_ebc = result[i][5];
+		reg_ec4 = result[i][6];
+		reg_ecc = result[i][7];
+	}
+	if (final_candidate != 0xff) {
+		rtlphy->reg_e94 = reg_e94 = result[final_candidate][0];
+		rtlphy->reg_e9c = reg_e9c = result[final_candidate][1];
+		reg_ea4 = result[final_candidate][2];
+		reg_eac = result[final_candidate][3];
+		rtlphy->reg_eb4 = reg_eb4 = result[final_candidate][4];
+		rtlphy->reg_ebc = reg_ebc = result[final_candidate][5];
+		reg_ec4 = result[final_candidate][6];
+		reg_ecc = result[final_candidate][7];
+		patha_ok = pathb_ok = true;
+	} else {
+		rtlphy->reg_e94 = rtlphy->reg_eb4 = 0x100;
+		rtlphy->reg_e9c = rtlphy->reg_ebc = 0x0;
+	}
+	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
+		_rtl92c_phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
+						   final_candidate,
+						   (reg_ea4 == 0));
+	if (IS_92C_SERIAL(rtlhal->version)) {
+		if (reg_eb4 != 0) /*&&(reg_ec4 != 0) */
+			_rtl92c_phy_path_b_fill_iqk_matrix(hw, pathb_ok,
+							   result,
+							   final_candidate,
+							   (reg_ec4 == 0));
+	}
+	_rtl92c_phy_save_adda_registers(hw, iqk_bb_reg,
+					rtlphy->iqk_bb_backup, 10);
+}
+EXPORT_SYMBOL(rtl92c_phy_iq_calibrate);
+
+void rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool start_conttx = false, singletone = false;
+
+	if (start_conttx || singletone)
+		return;
+	if (IS_92C_SERIAL(rtlhal->version))
+		rtlpriv->cfg->ops->phy_lc_calibrate(hw, true);
+	else
+		rtlpriv->cfg->ops->phy_lc_calibrate(hw, false);
+}
+EXPORT_SYMBOL(rtl92c_phy_lc_calibrate);
+
+void rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlphy->apk_done)
+		return;
+	if (IS_92C_SERIAL(rtlhal->version))
+		_rtl92c_phy_ap_calibrate(hw, delta, true);
+	else
+		_rtl92c_phy_ap_calibrate(hw, delta, false);
+}
+EXPORT_SYMBOL(rtl92c_phy_ap_calibrate);
+
+void rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (IS_92C_SERIAL(rtlhal->version))
+		_rtl92c_phy_set_rfpath_switch(hw, bmain, true);
+	else
+		_rtl92c_phy_set_rfpath_switch(hw, bmain, false);
+}
+EXPORT_SYMBOL(rtl92c_phy_set_rfpath_switch);
+
+bool rtl92c_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool postprocessing = false;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 ("-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		  iotype, rtlphy->set_io_inprogress));
+	do {
+		switch (iotype) {
+		case IO_CMD_RESUME_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 ("[IO CMD] Resume DM after scan.\n"));
+			postprocessing = true;
+			break;
+		case IO_CMD_PAUSE_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 ("[IO CMD] Pause DM before scan.\n"));
+			postprocessing = true;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+	} while (false);
+	if (postprocessing && !rtlphy->set_io_inprogress) {
+		rtlphy->set_io_inprogress = true;
+		rtlphy->current_io_type = iotype;
+	} else {
+		return false;
+	}
+	rtl92c_phy_set_io(hw);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, ("<--IO Type(%#x)\n", iotype));
+	return true;
+}
+EXPORT_SYMBOL(rtl92c_phy_set_io_cmd);
+
+void rtl92c_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 ("--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress));
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		dm_digtable.cur_igvalue = rtlphy->initgain_backup.xaagccore1;
+		rtl92c_dm_write_dig(hw);
+		rtl92c_phy_set_txpower_level(hw, rtlphy->current_channel);
+		break;
+	case IO_CMD_PAUSE_DM_BY_SCAN:
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable.cur_igvalue;
+		dm_digtable.cur_igvalue = 0x17;
+		rtl92c_dm_write_dig(hw);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 ("<---(%#x)\n", rtlphy->current_io_type));
+}
+EXPORT_SYMBOL(rtl92c_phy_set_io);
+
+void rtl92ce_phy_set_rf_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+}
+EXPORT_SYMBOL(rtl92ce_phy_set_rf_on);
+
+void _rtl92c_phy_set_rf_sleep(struct ieee80211_hw *hw)
+{
+	u32 u4b_tmp;
+	u8 delay = 5;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
+	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
+	u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
+	while (u4b_tmp != 0 && delay > 0) {
+		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x0);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
+		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
+		u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
+		delay--;
+	}
+	if (delay == 0) {
+		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+			 ("Switch RF timeout !!!.\n"));
+		return;
+	}
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
+}
+EXPORT_SYMBOL(_rtl92c_phy_set_rf_sleep);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h
new file mode 100644
index 000000000000..53ffb0981586
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h
@@ -0,0 +1,246 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C_PHY_H__
+#define __RTL92C_PHY_H__
+
+#define MAX_PRECMD_CNT			16
+#define MAX_RFDEPENDCMD_CNT		16
+#define MAX_POSTCMD_CNT			16
+
+#define MAX_DOZE_WAITING_TIMES_9x	64
+
+#define RT_CANNOT_IO(hw)		false
+#define HIGHPOWER_RADIOA_ARRAYLEN	22
+
+#define MAX_TOLERANCE			5
+#define	IQK_DELAY_TIME			1
+
+#define	APK_BB_REG_NUM			5
+#define	APK_AFE_REG_NUM			16
+#define	APK_CURVE_REG_NUM		4
+#define	PATH_NUM			2
+
+#define LOOP_LIMIT			5
+#define MAX_STALL_TIME			50
+#define AntennaDiversityValue		0x80
+#define MAX_TXPWR_IDX_NMODE_92S		63
+#define Reset_Cnt_Limit			3
+
+#define IQK_ADDA_REG_NUM		16
+#define IQK_MAC_REG_NUM			4
+
+#define RF90_PATH_MAX			2
+
+#define CT_OFFSET_MAC_ADDR		0X16
+
+#define CT_OFFSET_CCK_TX_PWR_IDX	0x5A
+#define CT_OFFSET_HT401S_TX_PWR_IDX	0x60
+#define CT_OFFSET_HT402S_TX_PWR_IDX_DIF	0x66
+#define CT_OFFSET_HT20_TX_PWR_IDX_DIFF	0x69
+#define CT_OFFSET_OFDM_TX_PWR_IDX_DIFF	0x6C
+
+#define CT_OFFSET_HT40_MAX_PWR_OFFSET	0x6F
+#define CT_OFFSET_HT20_MAX_PWR_OFFSET	0x72
+
+#define CT_OFFSET_CHANNEL_PLAH		0x75
+#define CT_OFFSET_THERMAL_METER		0x78
+#define CT_OFFSET_RF_OPTION		0x79
+#define CT_OFFSET_VERSION		0x7E
+#define CT_OFFSET_CUSTOMER_ID		0x7F
+
+#define RTL92C_MAX_PATH_NUM		2
+#define LLT_LAST_ENTRY_OF_TX_PKT_BUFFER	255
+enum swchnlcmd_id {
+	CMDID_END,
+	CMDID_SET_TXPOWEROWER_LEVEL,
+	CMDID_BBREGWRITE10,
+	CMDID_WRITEPORT_ULONG,
+	CMDID_WRITEPORT_USHORT,
+	CMDID_WRITEPORT_UCHAR,
+	CMDID_RF_WRITEREG,
+};
+
+struct swchnlcmd {
+	enum swchnlcmd_id cmdid;
+	u32 para1;
+	u32 para2;
+	u32 msdelay;
+};
+
+enum hw90_block_e {
+	HW90_BLOCK_MAC = 0,
+	HW90_BLOCK_PHY0 = 1,
+	HW90_BLOCK_PHY1 = 2,
+	HW90_BLOCK_RF = 3,
+	HW90_BLOCK_MAXIMUM = 4,
+};
+
+enum baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,
+	BASEBAND_CONFIG_AGC_TAB = 1,
+};
+
+enum ra_offset_area {
+	RA_OFFSET_LEGACY_OFDM1,
+	RA_OFFSET_LEGACY_OFDM2,
+	RA_OFFSET_HT_OFDM1,
+	RA_OFFSET_HT_OFDM2,
+	RA_OFFSET_HT_OFDM3,
+	RA_OFFSET_HT_OFDM4,
+	RA_OFFSET_HT_CCK,
+};
+
+enum antenna_path {
+	ANTENNA_NONE,
+	ANTENNA_D,
+	ANTENNA_C,
+	ANTENNA_CD,
+	ANTENNA_B,
+	ANTENNA_BD,
+	ANTENNA_BC,
+	ANTENNA_BCD,
+	ANTENNA_A,
+	ANTENNA_AD,
+	ANTENNA_AC,
+	ANTENNA_ACD,
+	ANTENNA_AB,
+	ANTENNA_ABD,
+	ANTENNA_ABC,
+	ANTENNA_ABCD
+};
+
+struct r_antenna_select_ofdm {
+	u32 r_tx_antenna:4;
+	u32 r_ant_l:4;
+	u32 r_ant_non_ht:4;
+	u32 r_ant_ht1:4;
+	u32 r_ant_ht2:4;
+	u32 r_ant_ht_s1:4;
+	u32 r_ant_non_ht_s1:4;
+	u32 ofdm_txsc:2;
+	u32 reserved:2;
+};
+
+struct r_antenna_select_cck {
+	u8 r_cckrx_enable_2:2;
+	u8 r_cckrx_enable:2;
+	u8 r_ccktx_enable:4;
+};
+
+struct efuse_contents {
+	u8 mac_addr[ETH_ALEN];
+	u8 cck_tx_power_idx[6];
+	u8 ht40_1s_tx_power_idx[6];
+	u8 ht40_2s_tx_power_idx_diff[3];
+	u8 ht20_tx_power_idx_diff[3];
+	u8 ofdm_tx_power_idx_diff[3];
+	u8 ht40_max_power_offset[3];
+	u8 ht20_max_power_offset[3];
+	u8 channel_plan;
+	u8 thermal_meter;
+	u8 rf_option[5];
+	u8 version;
+	u8 oem_id;
+	u8 regulatory;
+};
+
+struct tx_power_struct {
+	u8 cck[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_1s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_2s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht20_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_txpowerdiff;
+	u8 groupht20[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 groupht40[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_cnt;
+	u32 mcs_original_offset[4][16];
+};
+
+extern u32 rtl92c_phy_query_bb_reg(struct ieee80211_hw *hw,
+				   u32 regaddr, u32 bitmask);
+extern void rtl92c_phy_set_bb_reg(struct ieee80211_hw *hw,
+				  u32 regaddr, u32 bitmask, u32 data);
+extern u32 rtl92c_phy_query_rf_reg(struct ieee80211_hw *hw,
+				   enum radio_path rfpath, u32 regaddr,
+				   u32 bitmask);
+extern void rtl92c_phy_set_rf_reg(struct ieee80211_hw *hw,
+				  enum radio_path rfpath, u32 regaddr,
+				  u32 bitmask, u32 data);
+extern bool rtl92c_phy_mac_config(struct ieee80211_hw *hw);
+extern bool rtl92c_phy_bb_config(struct ieee80211_hw *hw);
+extern bool rtl92c_phy_rf_config(struct ieee80211_hw *hw);
+extern bool rtl92c_phy_config_rf_with_feaderfile(struct ieee80211_hw *hw,
+						 enum radio_path rfpath);
+extern void rtl92c_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+extern void rtl92c_phy_get_txpower_level(struct ieee80211_hw *hw,
+					 long *powerlevel);
+extern void rtl92c_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+extern bool rtl92c_phy_update_txpower_dbm(struct ieee80211_hw *hw,
+					  long power_indbm);
+extern void rtl92c_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					     u8 operation);
+extern void rtl92c_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+extern void rtl92c_phy_set_bw_mode(struct ieee80211_hw *hw,
+				   enum nl80211_channel_type ch_type);
+extern void rtl92c_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+extern u8 rtl92c_phy_sw_chnl(struct ieee80211_hw *hw);
+extern void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+extern void rtl92c_phy_set_beacon_hw_reg(struct ieee80211_hw *hw,
+					 u16 beaconinterval);
+void rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
+void rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
+bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath);
+extern bool rtl8192_phy_check_is_legal_rfpath(struct ieee80211_hw *hw,
+					      u32 rfpath);
+extern bool rtl92c_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					  enum rf_pwrstate rfpwr_state);
+void rtl92ce_phy_set_rf_on(struct ieee80211_hw *hw);
+void rtl92c_phy_set_io(struct ieee80211_hw *hw);
+void rtl92c_bb_block_on(struct ieee80211_hw *hw);
+u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask);
+long _rtl92c_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+				  enum wireless_mode wirelessmode,
+				  u8 txpwridx);
+u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
+				enum wireless_mode wirelessmode,
+				long power_indbm);
+void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+static bool _rtl92c_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					     u32 cmdtableidx, u32 cmdtablesz,
+					     enum swchnlcmd_id cmdid, u32 para1,
+					     u32 para2, u32 msdelay);
+static bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					     u8 channel, u8 *stage, u8 *step,
+					     u32 *delay);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/Makefile b/drivers/net/wireless/rtlwifi/rtl8192ce/Makefile
index 0f0be7c763b8..c0cb0cfe7d37 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/Makefile
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/Makefile
@@ -1,6 +1,5 @@
 rtl8192ce-objs :=		\
 		dm.o		\
-		fw.o		\
 		hw.o		\
 		led.o		\
 		phy.o		\
@@ -10,3 +9,5 @@ rtl8192ce-objs :=		\
 		trx.o
 
 obj-$(CONFIG_RTL8192CE) += rtl8192ce.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
index 83cd64895292..2f577c8828fc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
@@ -121,11 +121,37 @@
 #define CHIP_92C			0x01
 #define CHIP_88C			0x00
 
+/* Add vendor information into chip version definition.
+ * Add UMC B-Cut and RTL8723 chip info definition.
+ *
+ * BIT 7	Reserved
+ * BIT 6	UMC BCut
+ * BIT 5	Manufacturer(TSMC/UMC)
+ * BIT 4	TEST/NORMAL
+ * BIT 3	8723 Version
+ * BIT 2	8723?
+ * BIT 1	1T2R?
+ * BIT 0	88C/92C
+*/
+
 enum version_8192c {
 	VERSION_A_CHIP_92C = 0x01,
 	VERSION_A_CHIP_88C = 0x00,
 	VERSION_B_CHIP_92C = 0x11,
 	VERSION_B_CHIP_88C = 0x10,
+	VERSION_TEST_CHIP_88C = 0x00,
+	VERSION_TEST_CHIP_92C = 0x01,
+	VERSION_NORMAL_TSMC_CHIP_88C = 0x10,
+	VERSION_NORMAL_TSMC_CHIP_92C = 0x11,
+	VERSION_NORMAL_TSMC_CHIP_92C_1T2R = 0x13,
+	VERSION_NORMAL_UMC_CHIP_88C_A_CUT = 0x30,
+	VERSION_NORMAL_UMC_CHIP_92C_A_CUT = 0x31,
+	VERSION_NORMAL_UMC_CHIP_92C_1T2R_A_CUT = 0x33,
+	VERSION_NORMA_UMC_CHIP_8723_1T1R_A_CUT = 0x34,
+	VERSION_NORMA_UMC_CHIP_8723_1T1R_B_CUT = 0x3c,
+	VERSION_NORMAL_UMC_CHIP_88C_B_CUT = 0x70,
+	VERSION_NORMAL_UMC_CHIP_92C_B_CUT = 0x71,
+	VERSION_NORMAL_UMC_CHIP_92C_1T2R_B_CUT = 0x73,
 	VERSION_UNKNOWN = 0x88,
 };
 
@@ -254,4 +280,122 @@ struct h2c_cmd_8192c {
 	u8 *p_cmdbuffer;
 };
 
+static inline u8 _rtl92c_get_chnl_group(u8 chnl)
+{
+	u8 group = 0;
+
+	if (chnl < 3)
+		group = 0;
+	else if (chnl < 9)
+		group = 1;
+	else
+		group = 2;
+
+	return group;
+}
+
+/* NOTE: reference to rtl8192c_rates struct */
+static inline int _rtl92c_rate_mapping(struct ieee80211_hw *hw, bool isHT,
+				       u8 desc_rate, bool first_ampdu)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int rate_idx = 0;
+
+	if (first_ampdu) {
+		if (false == isHT) {
+			switch (desc_rate) {
+			case DESC92C_RATE1M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_DMESG,
+					 ("Rate %d is not support, set to "
+					"1M rate.\n", desc_rate));
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			rate_idx = 11;
+		}
+		return rate_idx;
+	}
+	switch (desc_rate) {
+	case DESC92C_RATE1M:
+		rate_idx = 0;
+		break;
+	case DESC92C_RATE2M:
+		rate_idx = 1;
+		break;
+	case DESC92C_RATE5_5M:
+		rate_idx = 2;
+		break;
+	case DESC92C_RATE11M:
+		rate_idx = 3;
+		break;
+	case DESC92C_RATE6M:
+		rate_idx = 4;
+		break;
+	case DESC92C_RATE9M:
+		rate_idx = 5;
+		break;
+	case DESC92C_RATE12M:
+		rate_idx = 6;
+		break;
+	case DESC92C_RATE18M:
+		rate_idx = 7;
+		break;
+	case DESC92C_RATE24M:
+		rate_idx = 8;
+		break;
+	case DESC92C_RATE36M:
+		rate_idx = 9;
+		break;
+	case DESC92C_RATE48M:
+		rate_idx = 10;
+		break;
+	case DESC92C_RATE54M:
+		rate_idx = 11;
+		break;
+	/* TODO: How to mapping MCS rate? */
+	/*  NOTE: referenc to __ieee80211_rx */
+	default:
+		rate_idx = 11;
+		break;
+	}
+	return rate_idx;
+}
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c
index 62e7c64e087b..7d76504df4d1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.c
@@ -33,487 +33,15 @@
 #include "def.h"
 #include "phy.h"
 #include "dm.h"
-#include "fw.h"
 
-struct dig_t dm_digtable;
-static struct ps_t dm_pstable;
-
-static const u32 ofdmswing_table[OFDM_TABLE_SIZE] = {
-	0x7f8001fe,
-	0x788001e2,
-	0x71c001c7,
-	0x6b8001ae,
-	0x65400195,
-	0x5fc0017f,
-	0x5a400169,
-	0x55400155,
-	0x50800142,
-	0x4c000130,
-	0x47c0011f,
-	0x43c0010f,
-	0x40000100,
-	0x3c8000f2,
-	0x390000e4,
-	0x35c000d7,
-	0x32c000cb,
-	0x300000c0,
-	0x2d4000b5,
-	0x2ac000ab,
-	0x288000a2,
-	0x26000098,
-	0x24000090,
-	0x22000088,
-	0x20000080,
-	0x1e400079,
-	0x1c800072,
-	0x1b00006c,
-	0x19800066,
-	0x18000060,
-	0x16c0005b,
-	0x15800056,
-	0x14400051,
-	0x1300004c,
-	0x12000048,
-	0x11000044,
-	0x10000040,
-};
-
-static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
-	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},
-	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},
-	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},
-	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},
-	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},
-	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},
-	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},
-	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},
-	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},
-	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},
-	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},
-	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},
-	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},
-	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},
-	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},
-	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},
-	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},
-	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},
-	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},
-	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
-	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
-	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},
-	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01},
-	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01},
-	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01},
-	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01},
-	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01},
-	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01},
-	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01},
-	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01},
-	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01},
-	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01},
-	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}
-};
-
-static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
-	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},
-	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},
-	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},
-	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00},
-	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},
-	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00},
-	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},
-	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},
-	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},
-	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},
-	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},
-	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},
-	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},
-	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},
-	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},
-	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},
-	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},
-	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},
-	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},
-	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
-	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
-	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},
-	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00},
-	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},
-	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},
-	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00},
-	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},
-	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},
-	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},
-	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},
-	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},
-	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},
-	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}
-};
-
-static void rtl92c_dm_diginit(struct ieee80211_hw *hw)
-{
-	dm_digtable.dig_enable_flag = true;
-	dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-	dm_digtable.cur_igvalue = 0x20;
-	dm_digtable.pre_igvalue = 0x0;
-	dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
-	dm_digtable.presta_connectstate = DIG_STA_DISCONNECT;
-	dm_digtable.curmultista_connectstate = DIG_MULTISTA_DISCONNECT;
-	dm_digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
-	dm_digtable.rssi_highthresh = DM_DIG_THRESH_HIGH;
-	dm_digtable.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
-	dm_digtable.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_digtable.rx_gain_range_max = DM_DIG_MAX;
-	dm_digtable.rx_gain_range_min = DM_DIG_MIN;
-	dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable.pre_cck_pd_state = CCK_PD_STAGE_MAX;
-	dm_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
-}
-
-static u8 rtl92c_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long rssi_val_min = 0;
-
-	if ((dm_digtable.curmultista_connectstate == DIG_MULTISTA_CONNECT) &&
-	    (dm_digtable.cursta_connectctate == DIG_STA_CONNECT)) {
-		if (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb != 0)
-			rssi_val_min =
-			    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb >
-			     rtlpriv->dm.undecorated_smoothed_pwdb) ?
-			    rtlpriv->dm.undecorated_smoothed_pwdb :
-			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-		else
-			rssi_val_min = rtlpriv->dm.undecorated_smoothed_pwdb;
-	} else if (dm_digtable.cursta_connectctate == DIG_STA_CONNECT ||
-		   dm_digtable.cursta_connectctate == DIG_STA_BEFORE_CONNECT) {
-		rssi_val_min = rtlpriv->dm.undecorated_smoothed_pwdb;
-	} else if (dm_digtable.curmultista_connectstate ==
-		   DIG_MULTISTA_CONNECT) {
-		rssi_val_min = rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-	}
-
-	return (u8) rssi_val_min;
-}
-
-static void rtl92c_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
-{
-	u32 ret_value;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
-
-	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER1, MASKDWORD);
-	falsealm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
-
-	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER2, MASKDWORD);
-	falsealm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
-	falsealm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
-
-	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER3, MASKDWORD);
-	falsealm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
-	falsealm_cnt->cnt_ofdm_fail = falsealm_cnt->cnt_parity_fail +
-	    falsealm_cnt->cnt_rate_illegal +
-	    falsealm_cnt->cnt_crc8_fail + falsealm_cnt->cnt_mcs_fail;
-
-	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(14), 1);
-	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERLOWER, MASKBYTE0);
-	falsealm_cnt->cnt_cck_fail = ret_value;
-
-	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERUPPER, MASKBYTE3);
-	falsealm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
-	falsealm_cnt->cnt_all = (falsealm_cnt->cnt_parity_fail +
-				 falsealm_cnt->cnt_rate_illegal +
-				 falsealm_cnt->cnt_crc8_fail +
-				 falsealm_cnt->cnt_mcs_fail +
-				 falsealm_cnt->cnt_cck_fail);
-
-	rtl_set_bbreg(hw, ROFDM1_LSTF, 0x08000000, 1);
-	rtl_set_bbreg(hw, ROFDM1_LSTF, 0x08000000, 0);
-	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, 0x0000c000, 0);
-	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, 0x0000c000, 2);
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("cnt_parity_fail = %d, cnt_rate_illegal = %d, "
-		  "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
-		  falsealm_cnt->cnt_parity_fail,
-		  falsealm_cnt->cnt_rate_illegal,
-		  falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail));
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
-		  falsealm_cnt->cnt_ofdm_fail,
-		  falsealm_cnt->cnt_cck_fail, falsealm_cnt->cnt_all));
-}
-
-static void rtl92c_dm_ctrl_initgain_by_fa(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 value_igi = dm_digtable.cur_igvalue;
-
-	if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
-		value_igi--;
-	else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)
-		value_igi += 0;
-	else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH2)
-		value_igi++;
-	else if (rtlpriv->falsealm_cnt.cnt_all >= DM_DIG_FA_TH2)
-		value_igi += 2;
-	if (value_igi > DM_DIG_FA_UPPER)
-		value_igi = DM_DIG_FA_UPPER;
-	else if (value_igi < DM_DIG_FA_LOWER)
-		value_igi = DM_DIG_FA_LOWER;
-	if (rtlpriv->falsealm_cnt.cnt_all > 10000)
-		value_igi = 0x32;
-
-	dm_digtable.cur_igvalue = value_igi;
-	rtl92c_dm_write_dig(hw);
-}
-
-static void rtl92c_dm_ctrl_initgain_by_rssi(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->falsealm_cnt.cnt_all > dm_digtable.fa_highthresh) {
-		if ((dm_digtable.backoff_val - 2) <
-		    dm_digtable.backoff_val_range_min)
-			dm_digtable.backoff_val =
-			    dm_digtable.backoff_val_range_min;
-		else
-			dm_digtable.backoff_val -= 2;
-	} else if (rtlpriv->falsealm_cnt.cnt_all < dm_digtable.fa_lowthresh) {
-		if ((dm_digtable.backoff_val + 2) >
-		    dm_digtable.backoff_val_range_max)
-			dm_digtable.backoff_val =
-			    dm_digtable.backoff_val_range_max;
-		else
-			dm_digtable.backoff_val += 2;
-	}
-
-	if ((dm_digtable.rssi_val_min + 10 - dm_digtable.backoff_val) >
-	    dm_digtable.rx_gain_range_max)
-		dm_digtable.cur_igvalue = dm_digtable.rx_gain_range_max;
-	else if ((dm_digtable.rssi_val_min + 10 -
-		  dm_digtable.backoff_val) < dm_digtable.rx_gain_range_min)
-		dm_digtable.cur_igvalue = dm_digtable.rx_gain_range_min;
-	else
-		dm_digtable.cur_igvalue = dm_digtable.rssi_val_min + 10 -
-		    dm_digtable.backoff_val;
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("rssi_val_min = %x backoff_val %x\n",
-		  dm_digtable.rssi_val_min, dm_digtable.backoff_val));
-
-	rtl92c_dm_write_dig(hw);
-}
-
-static void rtl92c_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
-{
-	static u8 binitialized; /* initialized to false */
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	long rssi_strength = rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-	bool b_multi_sta = false;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		b_multi_sta = true;
-
-	if ((b_multi_sta == false) || (dm_digtable.cursta_connectctate !=
-				       DIG_STA_DISCONNECT)) {
-		binitialized = false;
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-		return;
-	} else if (binitialized == false) {
-		binitialized = true;
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
-		dm_digtable.cur_igvalue = 0x20;
-		rtl92c_dm_write_dig(hw);
-	}
-
-	if (dm_digtable.curmultista_connectstate == DIG_MULTISTA_CONNECT) {
-		if ((rssi_strength < dm_digtable.rssi_lowthresh) &&
-		    (dm_digtable.dig_ext_port_stage != DIG_EXT_PORT_STAGE_1)) {
-
-			if (dm_digtable.dig_ext_port_stage ==
-			    DIG_EXT_PORT_STAGE_2) {
-				dm_digtable.cur_igvalue = 0x20;
-				rtl92c_dm_write_dig(hw);
-			}
-
-			dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_1;
-		} else if (rssi_strength > dm_digtable.rssi_highthresh) {
-			dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_2;
-			rtl92c_dm_ctrl_initgain_by_fa(hw);
-		}
-	} else if (dm_digtable.dig_ext_port_stage != DIG_EXT_PORT_STAGE_0) {
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
-		dm_digtable.cur_igvalue = 0x20;
-		rtl92c_dm_write_dig(hw);
-	}
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("curmultista_connectstate = "
-		  "%x dig_ext_port_stage %x\n",
-		  dm_digtable.curmultista_connectstate,
-		  dm_digtable.dig_ext_port_stage));
-}
-
-static void rtl92c_dm_initial_gain_sta(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("presta_connectstate = %x,"
-		  " cursta_connectctate = %x\n",
-		  dm_digtable.presta_connectstate,
-		  dm_digtable.cursta_connectctate));
-
-	if (dm_digtable.presta_connectstate == dm_digtable.cursta_connectctate
-	    || dm_digtable.cursta_connectctate == DIG_STA_BEFORE_CONNECT
-	    || dm_digtable.cursta_connectctate == DIG_STA_CONNECT) {
-
-		if (dm_digtable.cursta_connectctate != DIG_STA_DISCONNECT) {
-			dm_digtable.rssi_val_min =
-			    rtl92c_dm_initial_gain_min_pwdb(hw);
-			rtl92c_dm_ctrl_initgain_by_rssi(hw);
-		}
-	} else {
-		dm_digtable.rssi_val_min = 0;
-		dm_digtable.dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
-		dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-		dm_digtable.cur_igvalue = 0x20;
-		dm_digtable.pre_igvalue = 0;
-		rtl92c_dm_write_dig(hw);
-	}
-}
-
-static void rtl92c_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (dm_digtable.cursta_connectctate == DIG_STA_CONNECT) {
-		dm_digtable.rssi_val_min = rtl92c_dm_initial_gain_min_pwdb(hw);
-
-		if (dm_digtable.pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
-			if (dm_digtable.rssi_val_min <= 25)
-				dm_digtable.cur_cck_pd_state =
-				    CCK_PD_STAGE_LowRssi;
-			else
-				dm_digtable.cur_cck_pd_state =
-				    CCK_PD_STAGE_HighRssi;
-		} else {
-			if (dm_digtable.rssi_val_min <= 20)
-				dm_digtable.cur_cck_pd_state =
-				    CCK_PD_STAGE_LowRssi;
-			else
-				dm_digtable.cur_cck_pd_state =
-				    CCK_PD_STAGE_HighRssi;
-		}
-	} else {
-		dm_digtable.cur_cck_pd_state = CCK_PD_STAGE_MAX;
-	}
-
-	if (dm_digtable.pre_cck_pd_state != dm_digtable.cur_cck_pd_state) {
-		if (dm_digtable.cur_cck_pd_state == CCK_PD_STAGE_LowRssi) {
-			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 800)
-				dm_digtable.cur_cck_fa_state =
-				    CCK_FA_STAGE_High;
-			else
-				dm_digtable.cur_cck_fa_state = CCK_FA_STAGE_Low;
-
-			if (dm_digtable.pre_cck_fa_state !=
-			    dm_digtable.cur_cck_fa_state) {
-				if (dm_digtable.cur_cck_fa_state ==
-				    CCK_FA_STAGE_Low)
-					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
-						      0x83);
-				else
-					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
-						      0xcd);
-
-				dm_digtable.pre_cck_fa_state =
-				    dm_digtable.cur_cck_fa_state;
-			}
-
-			rtl_set_bbreg(hw, RCCK0_SYSTEM, MASKBYTE1, 0x40);
-
-			if (IS_92C_SERIAL(rtlhal->version))
-				rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT,
-					      MASKBYTE2, 0xd7);
-		} else {
-			rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, 0xcd);
-			rtl_set_bbreg(hw, RCCK0_SYSTEM, MASKBYTE1, 0x47);
-
-			if (IS_92C_SERIAL(rtlhal->version))
-				rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT,
-					      MASKBYTE2, 0xd3);
-		}
-		dm_digtable.pre_cck_pd_state = dm_digtable.cur_cck_pd_state;
-	}
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("CCKPDStage=%x\n", dm_digtable.cur_cck_pd_state));
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 ("is92C=%x\n", IS_92C_SERIAL(rtlhal->version)));
-}
-
-static void rtl92c_dm_ctrl_initgain_by_twoport(struct ieee80211_hw *hw)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	if (mac->act_scanning == true)
-		return;
-
-	if ((mac->link_state > MAC80211_NOLINK) &&
-	    (mac->link_state < MAC80211_LINKED))
-		dm_digtable.cursta_connectctate = DIG_STA_BEFORE_CONNECT;
-	else if (mac->link_state >= MAC80211_LINKED)
-		dm_digtable.cursta_connectctate = DIG_STA_CONNECT;
-	else
-		dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
-
-	rtl92c_dm_initial_gain_sta(hw);
-	rtl92c_dm_initial_gain_multi_sta(hw);
-	rtl92c_dm_cck_packet_detection_thresh(hw);
-
-	dm_digtable.presta_connectstate = dm_digtable.cursta_connectctate;
-
-}
-
-static void rtl92c_dm_dig(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->dm.b_dm_initialgain_enable == false)
-		return;
-	if (dm_digtable.dig_enable_flag == false)
-		return;
-
-	rtl92c_dm_ctrl_initgain_by_twoport(hw);
-
-}
-
-static void rtl92c_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.bdynamic_txpower_enable = false;
-
-	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
-	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
-}
-
-static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
+void rtl92ce_dm_dynamic_txpower(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	long undecorated_smoothed_pwdb;
 
-	if (!rtlpriv->dm.bdynamic_txpower_enable)
+	if (!rtlpriv->dm.dynamic_txpower_enable)
 		return;
 
 	if (rtlpriv->dm.dm_flag & HAL_DM_HIPWR_DISABLE) {
@@ -583,891 +111,3 @@ static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
 
 	rtlpriv->dm.last_dtp_lvl = rtlpriv->dm.dynamic_txhighpower_lvl;
 }
-
-void rtl92c_dm_write_dig(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-		 ("cur_igvalue = 0x%x, "
-		  "pre_igvalue = 0x%x, backoff_val = %d\n",
-		  dm_digtable.cur_igvalue, dm_digtable.pre_igvalue,
-		  dm_digtable.backoff_val));
-
-	if (dm_digtable.pre_igvalue != dm_digtable.cur_igvalue) {
-		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f,
-			      dm_digtable.cur_igvalue);
-		rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f,
-			      dm_digtable.cur_igvalue);
-
-		dm_digtable.pre_igvalue = dm_digtable.cur_igvalue;
-	}
-}
-
-static void rtl92c_dm_pwdb_monitor(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long tmpentry_max_pwdb = 0, tmpentry_min_pwdb = 0xff;
-
-	u8 h2c_parameter[3] = { 0 };
-
-	return;
-
-	if (tmpentry_max_pwdb != 0) {
-		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb =
-		    tmpentry_max_pwdb;
-	} else {
-		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb = 0;
-	}
-
-	if (tmpentry_min_pwdb != 0xff) {
-		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb =
-		    tmpentry_min_pwdb;
-	} else {
-		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb = 0;
-	}
-
-	h2c_parameter[2] = (u8) (rtlpriv->dm.undecorated_smoothed_pwdb & 0xFF);
-	h2c_parameter[0] = 0;
-
-	rtl92c_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);
-}
-
-void rtl92c_dm_init_edca_turbo(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	rtlpriv->dm.bcurrent_turbo_edca = false;
-	rtlpriv->dm.bis_any_nonbepkts = false;
-	rtlpriv->dm.bis_cur_rdlstate = false;
-}
-
-static void rtl92c_dm_check_edca_turbo(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	static u64 last_txok_cnt;
-	static u64 last_rxok_cnt;
-	u64 cur_txok_cnt;
-	u64 cur_rxok_cnt;
-	u32 edca_be_ul = 0x5ea42b;
-	u32 edca_be_dl = 0x5ea42b;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		goto dm_checkedcaturbo_exit;
-
-	if (mac->link_state != MAC80211_LINKED) {
-		rtlpriv->dm.bcurrent_turbo_edca = false;
-		return;
-	}
-
-	if (!mac->ht_enable) {	/*FIX MERGE */
-		if (!(edca_be_ul & 0xffff0000))
-			edca_be_ul |= 0x005e0000;
-
-		if (!(edca_be_dl & 0xffff0000))
-			edca_be_dl |= 0x005e0000;
-	}
-
-	if ((!rtlpriv->dm.bis_any_nonbepkts) &&
-	    (!rtlpriv->dm.b_disable_framebursting)) {
-		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
-		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
-		if (cur_rxok_cnt > 4 * cur_txok_cnt) {
-			if (!rtlpriv->dm.bis_cur_rdlstate ||
-			    !rtlpriv->dm.bcurrent_turbo_edca) {
-				rtl_write_dword(rtlpriv,
-						REG_EDCA_BE_PARAM,
-						edca_be_dl);
-				rtlpriv->dm.bis_cur_rdlstate = true;
-			}
-		} else {
-			if (rtlpriv->dm.bis_cur_rdlstate ||
-			    !rtlpriv->dm.bcurrent_turbo_edca) {
-				rtl_write_dword(rtlpriv,
-						REG_EDCA_BE_PARAM,
-						edca_be_ul);
-				rtlpriv->dm.bis_cur_rdlstate = false;
-			}
-		}
-		rtlpriv->dm.bcurrent_turbo_edca = true;
-	} else {
-		if (rtlpriv->dm.bcurrent_turbo_edca) {
-			u8 tmp = AC0_BE;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_AC_PARAM,
-						      (u8 *) (&tmp));
-			rtlpriv->dm.bcurrent_turbo_edca = false;
-		}
-	}
-
-dm_checkedcaturbo_exit:
-	rtlpriv->dm.bis_any_nonbepkts = false;
-	last_txok_cnt = rtlpriv->stats.txbytesunicast;
-	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
-}
-
-static void rtl92c_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
-							     *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 thermalvalue, delta, delta_lck, delta_iqk;
-	long ele_a, ele_d, temp_cck, val_x, value32;
-	long val_y, ele_c;
-	u8 ofdm_index[2], cck_index, ofdm_index_old[2], cck_index_old;
-	int i;
-	bool is2t = IS_92C_SERIAL(rtlhal->version);
-	u8 txpwr_level[2] = {0, 0};
-	u8 ofdm_min_index = 6, rf;
-
-	rtlpriv->dm.btxpower_trackingInit = true;
-	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 ("rtl92c_dm_txpower_tracking_callback_thermalmeter\n"));
-
-	thermalvalue = (u8) rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER, 0x1f);
-
-	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 ("Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
-		  "eeprom_thermalmeter 0x%x\n",
-		  thermalvalue, rtlpriv->dm.thermalvalue,
-		  rtlefuse->eeprom_thermalmeter));
-
-	rtl92c_phy_ap_calibrate(hw, (thermalvalue -
-				     rtlefuse->eeprom_thermalmeter));
-	if (is2t)
-		rf = 2;
-	else
-		rf = 1;
-
-	if (thermalvalue) {
-		ele_d = rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
-				      MASKDWORD) & MASKOFDM_D;
-
-		for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
-			if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
-				ofdm_index_old[0] = (u8) i;
-
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-					("Initial pathA ele_d reg0x%x = 0x%lx, "
-					 "ofdm_index=0x%x\n",
-					 ROFDM0_XATXIQIMBALANCE,
-					 ele_d, ofdm_index_old[0]));
-				break;
-			}
-		}
-
-		if (is2t) {
-			ele_d = rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
-					      MASKDWORD) & MASKOFDM_D;
-
-			for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
-				if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
-					ofdm_index_old[1] = (u8) i;
-
-					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
-					   DBG_LOUD,
-					   ("Initial pathB ele_d reg0x%x = "
-					   "0x%lx, ofdm_index=0x%x\n",
-					   ROFDM0_XBTXIQIMBALANCE, ele_d,
-					   ofdm_index_old[1]));
-					break;
-				}
-			}
-		}
-
-		temp_cck =
-		    rtl_get_bbreg(hw, RCCK0_TXFILTER2, MASKDWORD) & MASKCCK;
-
-		for (i = 0; i < CCK_TABLE_LENGTH; i++) {
-			if (rtlpriv->dm.b_cck_inch14) {
-				if (memcmp((void *)&temp_cck,
-					   (void *)&cckswing_table_ch14[i][2],
-					   4) == 0) {
-					cck_index_old = (u8) i;
-
-					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
-						 DBG_LOUD,
-						 ("Initial reg0x%x = 0x%lx, "
-						  "cck_index=0x%x, ch 14 %d\n",
-						  RCCK0_TXFILTER2, temp_cck,
-						  cck_index_old,
-						  rtlpriv->dm.b_cck_inch14));
-					break;
-				}
-			} else {
-				if (memcmp((void *)&temp_cck,
-					   (void *)
-					   &cckswing_table_ch1ch13[i][2],
-					   4) == 0) {
-					cck_index_old = (u8) i;
-
-					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
-						 DBG_LOUD,
-						 ("Initial reg0x%x = 0x%lx, "
-						  "cck_index=0x%x, ch14 %d\n",
-						  RCCK0_TXFILTER2, temp_cck,
-						  cck_index_old,
-						  rtlpriv->dm.b_cck_inch14));
-					break;
-				}
-			}
-		}
-
-		if (!rtlpriv->dm.thermalvalue) {
-			rtlpriv->dm.thermalvalue =
-			    rtlefuse->eeprom_thermalmeter;
-			rtlpriv->dm.thermalvalue_lck = thermalvalue;
-			rtlpriv->dm.thermalvalue_iqk = thermalvalue;
-			for (i = 0; i < rf; i++)
-				rtlpriv->dm.ofdm_index[i] = ofdm_index_old[i];
-			rtlpriv->dm.cck_index = cck_index_old;
-		}
-
-		delta = (thermalvalue > rtlpriv->dm.thermalvalue) ?
-		    (thermalvalue - rtlpriv->dm.thermalvalue) :
-		    (rtlpriv->dm.thermalvalue - thermalvalue);
-
-		delta_lck = (thermalvalue > rtlpriv->dm.thermalvalue_lck) ?
-		    (thermalvalue - rtlpriv->dm.thermalvalue_lck) :
-		    (rtlpriv->dm.thermalvalue_lck - thermalvalue);
-
-		delta_iqk = (thermalvalue > rtlpriv->dm.thermalvalue_iqk) ?
-		    (thermalvalue - rtlpriv->dm.thermalvalue_iqk) :
-		    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
-
-		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-			("Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
-			 "eeprom_thermalmeter 0x%x delta 0x%x "
-			 "delta_lck 0x%x delta_iqk 0x%x\n",
-			 thermalvalue, rtlpriv->dm.thermalvalue,
-			 rtlefuse->eeprom_thermalmeter, delta, delta_lck,
-			 delta_iqk));
-
-		if (delta_lck > 1) {
-			rtlpriv->dm.thermalvalue_lck = thermalvalue;
-			rtl92c_phy_lc_calibrate(hw);
-		}
-
-		if (delta > 0 && rtlpriv->dm.txpower_track_control) {
-			if (thermalvalue > rtlpriv->dm.thermalvalue) {
-				for (i = 0; i < rf; i++)
-					rtlpriv->dm.ofdm_index[i] -= delta;
-				rtlpriv->dm.cck_index -= delta;
-			} else {
-				for (i = 0; i < rf; i++)
-					rtlpriv->dm.ofdm_index[i] += delta;
-				rtlpriv->dm.cck_index += delta;
-			}
-
-			if (is2t) {
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-					 ("temp OFDM_A_index=0x%x, "
-					  "OFDM_B_index=0x%x,"
-					  "cck_index=0x%x\n",
-					  rtlpriv->dm.ofdm_index[0],
-					  rtlpriv->dm.ofdm_index[1],
-					  rtlpriv->dm.cck_index));
-			} else {
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-					 ("temp OFDM_A_index=0x%x,"
-					  "cck_index=0x%x\n",
-					  rtlpriv->dm.ofdm_index[0],
-					  rtlpriv->dm.cck_index));
-			}
-
-			if (thermalvalue > rtlefuse->eeprom_thermalmeter) {
-				for (i = 0; i < rf; i++)
-					ofdm_index[i] =
-					    rtlpriv->dm.ofdm_index[i]
-					    + 1;
-				cck_index = rtlpriv->dm.cck_index + 1;
-			} else {
-				for (i = 0; i < rf; i++)
-					ofdm_index[i] =
-					    rtlpriv->dm.ofdm_index[i];
-				cck_index = rtlpriv->dm.cck_index;
-			}
-
-			for (i = 0; i < rf; i++) {
-				if (txpwr_level[i] >= 0 &&
-				    txpwr_level[i] <= 26) {
-					if (thermalvalue >
-					    rtlefuse->eeprom_thermalmeter) {
-						if (delta < 5)
-							ofdm_index[i] -= 1;
-
-						else
-							ofdm_index[i] -= 2;
-					} else if (delta > 5 && thermalvalue <
-						   rtlefuse->
-						   eeprom_thermalmeter) {
-						ofdm_index[i] += 1;
-					}
-				} else if (txpwr_level[i] >= 27 &&
-					   txpwr_level[i] <= 32
-					   && thermalvalue >
-					   rtlefuse->eeprom_thermalmeter) {
-					if (delta < 5)
-						ofdm_index[i] -= 1;
-
-					else
-						ofdm_index[i] -= 2;
-				} else if (txpwr_level[i] >= 32 &&
-					   txpwr_level[i] <= 38 &&
-					   thermalvalue >
-					   rtlefuse->eeprom_thermalmeter
-					   && delta > 5) {
-					ofdm_index[i] -= 1;
-				}
-			}
-
-			if (txpwr_level[i] >= 0 && txpwr_level[i] <= 26) {
-				if (thermalvalue >
-				    rtlefuse->eeprom_thermalmeter) {
-					if (delta < 5)
-						cck_index -= 1;
-
-					else
-						cck_index -= 2;
-				} else if (delta > 5 && thermalvalue <
-					   rtlefuse->eeprom_thermalmeter) {
-					cck_index += 1;
-				}
-			} else if (txpwr_level[i] >= 27 &&
-				   txpwr_level[i] <= 32 &&
-				   thermalvalue >
-				   rtlefuse->eeprom_thermalmeter) {
-				if (delta < 5)
-					cck_index -= 1;
-
-				else
-					cck_index -= 2;
-			} else if (txpwr_level[i] >= 32 &&
-				   txpwr_level[i] <= 38 &&
-				   thermalvalue > rtlefuse->eeprom_thermalmeter
-				   && delta > 5) {
-				cck_index -= 1;
-			}
-
-			for (i = 0; i < rf; i++) {
-				if (ofdm_index[i] > OFDM_TABLE_SIZE - 1)
-					ofdm_index[i] = OFDM_TABLE_SIZE - 1;
-
-				else if (ofdm_index[i] < ofdm_min_index)
-					ofdm_index[i] = ofdm_min_index;
-			}
-
-			if (cck_index > CCK_TABLE_SIZE - 1)
-				cck_index = CCK_TABLE_SIZE - 1;
-			else if (cck_index < 0)
-				cck_index = 0;
-
-			if (is2t) {
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-					 ("new OFDM_A_index=0x%x, "
-					  "OFDM_B_index=0x%x,"
-					  "cck_index=0x%x\n",
-					  ofdm_index[0], ofdm_index[1],
-					  cck_index));
-			} else {
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-					 ("new OFDM_A_index=0x%x,"
-					  "cck_index=0x%x\n",
-					  ofdm_index[0], cck_index));
-			}
-		}
-
-		if (rtlpriv->dm.txpower_track_control && delta != 0) {
-			ele_d =
-			    (ofdmswing_table[ofdm_index[0]] & 0xFFC00000) >> 22;
-			val_x = rtlphy->reg_e94;
-			val_y = rtlphy->reg_e9c;
-
-			if (val_x != 0) {
-				if ((val_x & 0x00000200) != 0)
-					val_x = val_x | 0xFFFFFC00;
-				ele_a = ((val_x * ele_d) >> 8) & 0x000003FF;
-
-				if ((val_y & 0x00000200) != 0)
-					val_y = val_y | 0xFFFFFC00;
-				ele_c = ((val_y * ele_d) >> 8) & 0x000003FF;
-
-				value32 = (ele_d << 22) |
-				    ((ele_c & 0x3F) << 16) | ele_a;
-
-				rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
-					      MASKDWORD, value32);
-
-				value32 = (ele_c & 0x000003C0) >> 6;
-				rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS,
-					      value32);
-
-				value32 = ((val_x * ele_d) >> 7) & 0x01;
-				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
-					      BIT(31), value32);
-
-				value32 = ((val_y * ele_d) >> 7) & 0x01;
-				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
-					      BIT(29), value32);
-			} else {
-				rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
-					      MASKDWORD,
-					      ofdmswing_table[ofdm_index[0]]);
-
-				rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS,
-					      0x00);
-				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
-					      BIT(31) | BIT(29), 0x00);
-			}
-
-			if (!rtlpriv->dm.b_cck_inch14) {
-				rtl_write_byte(rtlpriv, 0xa22,
-					       cckswing_table_ch1ch13[cck_index]
-					       [0]);
-				rtl_write_byte(rtlpriv, 0xa23,
-					       cckswing_table_ch1ch13[cck_index]
-					       [1]);
-				rtl_write_byte(rtlpriv, 0xa24,
-					       cckswing_table_ch1ch13[cck_index]
-					       [2]);
-				rtl_write_byte(rtlpriv, 0xa25,
-					       cckswing_table_ch1ch13[cck_index]
-					       [3]);
-				rtl_write_byte(rtlpriv, 0xa26,
-					       cckswing_table_ch1ch13[cck_index]
-					       [4]);
-				rtl_write_byte(rtlpriv, 0xa27,
-					       cckswing_table_ch1ch13[cck_index]
-					       [5]);
-				rtl_write_byte(rtlpriv, 0xa28,
-					       cckswing_table_ch1ch13[cck_index]
-					       [6]);
-				rtl_write_byte(rtlpriv, 0xa29,
-					       cckswing_table_ch1ch13[cck_index]
-					       [7]);
-			} else {
-				rtl_write_byte(rtlpriv, 0xa22,
-					       cckswing_table_ch14[cck_index]
-					       [0]);
-				rtl_write_byte(rtlpriv, 0xa23,
-					       cckswing_table_ch14[cck_index]
-					       [1]);
-				rtl_write_byte(rtlpriv, 0xa24,
-					       cckswing_table_ch14[cck_index]
-					       [2]);
-				rtl_write_byte(rtlpriv, 0xa25,
-					       cckswing_table_ch14[cck_index]
-					       [3]);
-				rtl_write_byte(rtlpriv, 0xa26,
-					       cckswing_table_ch14[cck_index]
-					       [4]);
-				rtl_write_byte(rtlpriv, 0xa27,
-					       cckswing_table_ch14[cck_index]
-					       [5]);
-				rtl_write_byte(rtlpriv, 0xa28,
-					       cckswing_table_ch14[cck_index]
-					       [6]);
-				rtl_write_byte(rtlpriv, 0xa29,
-					       cckswing_table_ch14[cck_index]
-					       [7]);
-			}
-
-			if (is2t) {
-				ele_d = (ofdmswing_table[ofdm_index[1]] &
-					 0xFFC00000) >> 22;
-
-				val_x = rtlphy->reg_eb4;
-				val_y = rtlphy->reg_ebc;
-
-				if (val_x != 0) {
-					if ((val_x & 0x00000200) != 0)
-						val_x = val_x | 0xFFFFFC00;
-					ele_a = ((val_x * ele_d) >> 8) &
-					    0x000003FF;
-
-					if ((val_y & 0x00000200) != 0)
-						val_y = val_y | 0xFFFFFC00;
-					ele_c = ((val_y * ele_d) >> 8) &
-					    0x00003FF;
-
-					value32 = (ele_d << 22) |
-					    ((ele_c & 0x3F) << 16) | ele_a;
-					rtl_set_bbreg(hw,
-						      ROFDM0_XBTXIQIMBALANCE,
-						      MASKDWORD, value32);
-
-					value32 = (ele_c & 0x000003C0) >> 6;
-					rtl_set_bbreg(hw, ROFDM0_XDTXAFE,
-						      MASKH4BITS, value32);
-
-					value32 = ((val_x * ele_d) >> 7) & 0x01;
-					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
-						      BIT(27), value32);
-
-					value32 = ((val_y * ele_d) >> 7) & 0x01;
-					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
-						      BIT(25), value32);
-				} else {
-					rtl_set_bbreg(hw,
-						      ROFDM0_XBTXIQIMBALANCE,
-						      MASKDWORD,
-						      ofdmswing_table[ofdm_index
-								      [1]]);
-					rtl_set_bbreg(hw, ROFDM0_XDTXAFE,
-						      MASKH4BITS, 0x00);
-					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
-						      BIT(27) | BIT(25), 0x00);
-				}
-
-			}
-		}
-
-		if (delta_iqk > 3) {
-			rtlpriv->dm.thermalvalue_iqk = thermalvalue;
-			rtl92c_phy_iq_calibrate(hw, false);
-		}
-
-		if (rtlpriv->dm.txpower_track_control)
-			rtlpriv->dm.thermalvalue = thermalvalue;
-	}
-
-	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, ("<===\n"));
-
-}
-
-static void rtl92c_dm_initialize_txpower_tracking_thermalmeter(
-						struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.btxpower_tracking = true;
-	rtlpriv->dm.btxpower_trackingInit = false;
-
-	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 ("pMgntInfo->btxpower_tracking = %d\n",
-		  rtlpriv->dm.btxpower_tracking));
-}
-
-static void rtl92c_dm_initialize_txpower_tracking(struct ieee80211_hw *hw)
-{
-	rtl92c_dm_initialize_txpower_tracking_thermalmeter(hw);
-}
-
-static void rtl92c_dm_txpower_tracking_directcall(struct ieee80211_hw *hw)
-{
-	rtl92c_dm_txpower_tracking_callback_thermalmeter(hw);
-}
-
-static void rtl92c_dm_check_txpower_tracking_thermal_meter(
-						struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
-
-	if (!rtlpriv->dm.btxpower_tracking)
-		return;
-
-	if (!tm_trigger) {
-		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, RFREG_OFFSET_MASK,
-			      0x60);
-		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-			 ("Trigger 92S Thermal Meter!!\n"));
-		tm_trigger = 1;
-		return;
-	} else {
-		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-			 ("Schedule TxPowerTracking direct call!!\n"));
-		rtl92c_dm_txpower_tracking_directcall(hw);
-		tm_trigger = 0;
-	}
-}
-
-void rtl92c_dm_check_txpower_tracking(struct ieee80211_hw *hw)
-{
-	rtl92c_dm_check_txpower_tracking_thermal_meter(hw);
-}
-
-void rtl92c_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
-
-	p_ra->ratr_state = DM_RATR_STA_INIT;
-	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
-
-	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
-		rtlpriv->dm.b_useramask = true;
-	else
-		rtlpriv->dm.b_useramask = false;
-
-}
-
-static void rtl92c_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
-	u32 low_rssithresh_for_ra, high_rssithresh_for_ra;
-
-	if (is_hal_stop(rtlhal)) {
-		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-			 ("<---- driver is going to unload\n"));
-		return;
-	}
-
-	if (!rtlpriv->dm.b_useramask) {
-		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-			("<---- driver does not control rate adaptive mask\n"));
-		return;
-	}
-
-	if (mac->link_state == MAC80211_LINKED) {
-
-		switch (p_ra->pre_ratr_state) {
-		case DM_RATR_STA_HIGH:
-			high_rssithresh_for_ra = 50;
-			low_rssithresh_for_ra = 20;
-			break;
-		case DM_RATR_STA_MIDDLE:
-			high_rssithresh_for_ra = 55;
-			low_rssithresh_for_ra = 20;
-			break;
-		case DM_RATR_STA_LOW:
-			high_rssithresh_for_ra = 50;
-			low_rssithresh_for_ra = 25;
-			break;
-		default:
-			high_rssithresh_for_ra = 50;
-			low_rssithresh_for_ra = 20;
-			break;
-		}
-
-		if (rtlpriv->dm.undecorated_smoothed_pwdb >
-		    (long)high_rssithresh_for_ra)
-			p_ra->ratr_state = DM_RATR_STA_HIGH;
-		else if (rtlpriv->dm.undecorated_smoothed_pwdb >
-			 (long)low_rssithresh_for_ra)
-			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
-		else
-			p_ra->ratr_state = DM_RATR_STA_LOW;
-
-		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 ("RSSI = %ld\n",
-				  rtlpriv->dm.undecorated_smoothed_pwdb));
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 ("RSSI_LEVEL = %d\n", p_ra->ratr_state));
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 ("PreState = %d, CurState = %d\n",
-				  p_ra->pre_ratr_state, p_ra->ratr_state));
-
-			rtlpriv->cfg->ops->update_rate_mask(hw,
-					p_ra->ratr_state);
-
-			p_ra->pre_ratr_state = p_ra->ratr_state;
-		}
-	}
-}
-
-static void rtl92c_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
-{
-	dm_pstable.pre_ccastate = CCA_MAX;
-	dm_pstable.cur_ccasate = CCA_MAX;
-	dm_pstable.pre_rfstate = RF_MAX;
-	dm_pstable.cur_rfstate = RF_MAX;
-	dm_pstable.rssi_val_min = 0;
-}
-
-static void rtl92c_dm_1r_cca(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (dm_pstable.rssi_val_min != 0) {
-		if (dm_pstable.pre_ccastate == CCA_2R) {
-			if (dm_pstable.rssi_val_min >= 35)
-				dm_pstable.cur_ccasate = CCA_1R;
-			else
-				dm_pstable.cur_ccasate = CCA_2R;
-		} else {
-			if (dm_pstable.rssi_val_min <= 30)
-				dm_pstable.cur_ccasate = CCA_2R;
-			else
-				dm_pstable.cur_ccasate = CCA_1R;
-		}
-	} else {
-		dm_pstable.cur_ccasate = CCA_MAX;
-	}
-
-	if (dm_pstable.pre_ccastate != dm_pstable.cur_ccasate) {
-		if (dm_pstable.cur_ccasate == CCA_1R) {
-			if (get_rf_type(rtlphy) == RF_2T2R) {
-				rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE,
-					      MASKBYTE0, 0x13);
-				rtl_set_bbreg(hw, 0xe70, MASKBYTE3, 0x20);
-			} else {
-				rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE,
-					      MASKBYTE0, 0x23);
-				rtl_set_bbreg(hw, 0xe70, 0x7fc00000, 0x10c);
-			}
-		} else {
-			rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0,
-				      0x33);
-			rtl_set_bbreg(hw, 0xe70, MASKBYTE3, 0x63);
-		}
-		dm_pstable.pre_ccastate = dm_pstable.cur_ccasate;
-	}
-
-	RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD, ("CCAStage = %s\n",
-					       (dm_pstable.cur_ccasate ==
-						0) ? "1RCCA" : "2RCCA"));
-}
-
-void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal)
-{
-	static u8 initialize;
-	static u32 reg_874, reg_c70, reg_85c, reg_a74;
-
-	if (initialize == 0) {
-		reg_874 = (rtl_get_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
-					 MASKDWORD) & 0x1CC000) >> 14;
-
-		reg_c70 = (rtl_get_bbreg(hw, ROFDM0_AGCPARAMETER1,
-					 MASKDWORD) & BIT(3)) >> 3;
-
-		reg_85c = (rtl_get_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL,
-					 MASKDWORD) & 0xFF000000) >> 24;
-
-		reg_a74 = (rtl_get_bbreg(hw, 0xa74, MASKDWORD) & 0xF000) >> 12;
-
-		initialize = 1;
-	}
-
-	if (!bforce_in_normal) {
-		if (dm_pstable.rssi_val_min != 0) {
-			if (dm_pstable.pre_rfstate == RF_NORMAL) {
-				if (dm_pstable.rssi_val_min >= 30)
-					dm_pstable.cur_rfstate = RF_SAVE;
-				else
-					dm_pstable.cur_rfstate = RF_NORMAL;
-			} else {
-				if (dm_pstable.rssi_val_min <= 25)
-					dm_pstable.cur_rfstate = RF_NORMAL;
-				else
-					dm_pstable.cur_rfstate = RF_SAVE;
-			}
-		} else {
-			dm_pstable.cur_rfstate = RF_MAX;
-		}
-	} else {
-		dm_pstable.cur_rfstate = RF_NORMAL;
-	}
-
-	if (dm_pstable.pre_rfstate != dm_pstable.cur_rfstate) {
-		if (dm_pstable.cur_rfstate == RF_SAVE) {
-			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
-				      0x1C0000, 0x2);
-			rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, BIT(3), 0);
-			rtl_set_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL,
-				      0xFF000000, 0x63);
-			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
-				      0xC000, 0x2);
-			rtl_set_bbreg(hw, 0xa74, 0xF000, 0x3);
-			rtl_set_bbreg(hw, 0x818, BIT(28), 0x0);
-			rtl_set_bbreg(hw, 0x818, BIT(28), 0x1);
-		} else {
-			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
-				      0x1CC000, reg_874);
-			rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, BIT(3),
-				      reg_c70);
-			rtl_set_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL, 0xFF000000,
-				      reg_85c);
-			rtl_set_bbreg(hw, 0xa74, 0xF000, reg_a74);
-			rtl_set_bbreg(hw, 0x818, BIT(28), 0x0);
-		}
-
-		dm_pstable.pre_rfstate = dm_pstable.cur_rfstate;
-	}
-}
-
-static void rtl92c_dm_dynamic_bb_powersaving(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (((mac->link_state == MAC80211_NOLINK)) &&
-	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
-		dm_pstable.rssi_val_min = 0;
-		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
-			 ("Not connected to any\n"));
-	}
-
-	if (mac->link_state == MAC80211_LINKED) {
-		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-			dm_pstable.rssi_val_min =
-			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
-				 ("AP Client PWDB = 0x%lx\n",
-				  dm_pstable.rssi_val_min));
-		} else {
-			dm_pstable.rssi_val_min =
-			    rtlpriv->dm.undecorated_smoothed_pwdb;
-			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
-				 ("STA Default Port PWDB = 0x%lx\n",
-				  dm_pstable.rssi_val_min));
-		}
-	} else {
-		dm_pstable.rssi_val_min =
-		    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-
-		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
-			 ("AP Ext Port PWDB = 0x%lx\n",
-			  dm_pstable.rssi_val_min));
-	}
-
-	if (IS_92C_SERIAL(rtlhal->version))
-		rtl92c_dm_1r_cca(hw);
-}
-
-void rtl92c_dm_init(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl92c_dm_diginit(hw);
-	rtl92c_dm_init_dynamic_txpower(hw);
-	rtl92c_dm_init_edca_turbo(hw);
-	rtl92c_dm_init_rate_adaptive_mask(hw);
-	rtl92c_dm_initialize_txpower_tracking(hw);
-	rtl92c_dm_init_dynamic_bb_powersaving(hw);
-}
-
-void rtl92c_dm_watchdog(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool b_fw_current_inpsmode = false;
-	bool b_fw_ps_awake = true;
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				      (u8 *) (&b_fw_current_inpsmode));
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
-				      (u8 *) (&b_fw_ps_awake));
-
-	if ((ppsc->rfpwr_state == ERFON) && ((!b_fw_current_inpsmode) &&
-					     b_fw_ps_awake)
-	    && (!ppsc->rfchange_inprogress)) {
-		rtl92c_dm_pwdb_monitor(hw);
-		rtl92c_dm_dig(hw);
-		rtl92c_dm_false_alarm_counter_statistics(hw);
-		rtl92c_dm_dynamic_bb_powersaving(hw);
-		rtl92c_dm_dynamic_txpower(hw);
-		rtl92c_dm_check_txpower_tracking(hw);
-		rtl92c_dm_refresh_rate_adaptive_mask(hw);
-		rtl92c_dm_check_edca_turbo(hw);
-	}
-}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
index 463439e4074c..36302ebae4a3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
@@ -192,5 +192,6 @@ void rtl92c_dm_init_edca_turbo(struct ieee80211_hw *hw);
 void rtl92c_dm_check_txpower_tracking(struct ieee80211_hw *hw);
 void rtl92c_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
 void rtl92c_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal);
+void rtl92ce_dm_dynamic_txpower(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index 1c41a0c93506..05477f465a75 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -37,7 +37,6 @@
 #include "def.h"
 #include "phy.h"
 #include "dm.h"
-#include "fw.h"
 #include "led.h"
 #include "hw.h"
 
@@ -124,7 +123,7 @@ void rtl92ce_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_FW_PSMODE_STATUS:
-		*((bool *) (val)) = ppsc->b_fw_current_inpsmode;
+		*((bool *) (val)) = ppsc->fw_current_inpsmode;
 		break;
 	case HW_VAR_CORRECT_TSF:{
 		u64 tsf;
@@ -173,15 +172,15 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_BASIC_RATE:{
-			u16 b_rate_cfg = ((u16 *) val)[0];
+			u16 rate_cfg = ((u16 *) val)[0];
 			u8 rate_index = 0;
-			b_rate_cfg = b_rate_cfg & 0x15f;
-			b_rate_cfg |= 0x01;
-			rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+			rate_cfg &= 0x15f;
+			rate_cfg |= 0x01;
+			rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
 			rtl_write_byte(rtlpriv, REG_RRSR + 1,
-				       (b_rate_cfg >> 8)&0xff);
-			while (b_rate_cfg > 0x1) {
-				b_rate_cfg = (b_rate_cfg >> 1);
+				       (rate_cfg >> 8)&0xff);
+			while (rate_cfg > 0x1) {
+				rate_cfg = (rate_cfg >> 1);
 				rate_index++;
 			}
 			rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
@@ -318,15 +317,17 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		}
 	case HW_VAR_AC_PARAM:{
 			u8 e_aci = *((u8 *) val);
-			u32 u4b_ac_param = 0;
+			u32 u4b_ac_param;
+			u16 cw_min = le16_to_cpu(mac->ac[e_aci].cw_min);
+			u16 cw_max = le16_to_cpu(mac->ac[e_aci].cw_max);
+			u16 tx_op = le16_to_cpu(mac->ac[e_aci].tx_op);
 
-			u4b_ac_param |= (u32) mac->ac[e_aci].aifs;
-			u4b_ac_param |= ((u32) mac->ac[e_aci].cw_min
+			u4b_ac_param = (u32) mac->ac[e_aci].aifs;
+			u4b_ac_param |= ((u32)cw_min
 					 & 0xF) << AC_PARAM_ECW_MIN_OFFSET;
-			u4b_ac_param |= ((u32) mac->ac[e_aci].cw_max &
+			u4b_ac_param |= ((u32)cw_max &
 					 0xF) << AC_PARAM_ECW_MAX_OFFSET;
-			u4b_ac_param |= (u32) mac->ac[e_aci].tx_op
-			    << AC_PARAM_TXOP_LIMIT_OFFSET;
+			u4b_ac_param |= (u32)tx_op << AC_PARAM_TXOP_OFFSET;
 
 			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
 				 ("queue:%x, ac_param:%x\n", e_aci,
@@ -469,12 +470,12 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_FW_PSMODE_STATUS:
-		ppsc->b_fw_current_inpsmode = *((bool *) val);
+		ppsc->fw_current_inpsmode = *((bool *) val);
 		break;
 	case HW_VAR_H2C_FW_JOINBSSRPT:{
 			u8 mstatus = (*(u8 *) val);
 			u8 tmp_regcr, tmp_reg422;
-			bool b_recover = false;
+			bool recover = false;
 
 			if (mstatus == RT_MEDIA_CONNECT) {
 				rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID,
@@ -491,7 +492,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				    rtl_read_byte(rtlpriv,
 						  REG_FWHW_TXQ_CTRL + 2);
 				if (tmp_reg422 & BIT(6))
-					b_recover = true;
+					recover = true;
 				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
 					       tmp_reg422 & (~BIT(6)));
 
@@ -500,7 +501,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				_rtl92ce_set_bcn_ctrl_reg(hw, BIT(3), 0);
 				_rtl92ce_set_bcn_ctrl_reg(hw, 0, BIT(4));
 
-				if (b_recover) {
+				if (recover) {
 					rtl_write_byte(rtlpriv,
 						       REG_FWHW_TXQ_CTRL + 2,
 						       tmp_reg422);
@@ -868,7 +869,7 @@ static void _rtl92ce_enable_aspm_back_door(struct ieee80211_hw *hw)
 	rtl_write_word(rtlpriv, 0x350, 0x870c);
 	rtl_write_byte(rtlpriv, 0x352, 0x1);
 
-	if (ppsc->b_support_backdoor)
+	if (ppsc->support_backdoor)
 		rtl_write_byte(rtlpriv, 0x349, 0x1b);
 	else
 		rtl_write_byte(rtlpriv, 0x349, 0x03);
@@ -940,15 +941,15 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 			 ("Failed to download FW. Init HW "
 			  "without FW now..\n"));
 		err = 1;
-		rtlhal->bfw_ready = false;
+		rtlhal->fw_ready = false;
 		return err;
 	} else {
-		rtlhal->bfw_ready = true;
+		rtlhal->fw_ready = true;
 	}
 
 	rtlhal->last_hmeboxnum = 0;
-	rtl92c_phy_mac_config(hw);
-	rtl92c_phy_bb_config(hw);
+	rtl92ce_phy_mac_config(hw);
+	rtl92ce_phy_bb_config(hw);
 	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
 	rtl92c_phy_rf_config(hw);
 	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, (enum radio_path)0,
@@ -1170,21 +1171,20 @@ void rtl92ce_set_qos(struct ieee80211_hw *hw, int aci)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
 	u32 u4b_ac_param;
+	u16 cw_min = le16_to_cpu(mac->ac[aci].cw_min);
+	u16 cw_max = le16_to_cpu(mac->ac[aci].cw_max);
+	u16 tx_op = le16_to_cpu(mac->ac[aci].tx_op);
 
 	rtl92c_dm_init_edca_turbo(hw);
-
 	u4b_ac_param = (u32) mac->ac[aci].aifs;
-	u4b_ac_param |=
-	    ((u32) mac->ac[aci].cw_min & 0xF) << AC_PARAM_ECW_MIN_OFFSET;
-	u4b_ac_param |=
-	    ((u32) mac->ac[aci].cw_max & 0xF) << AC_PARAM_ECW_MAX_OFFSET;
-	u4b_ac_param |= (u32) mac->ac[aci].tx_op << AC_PARAM_TXOP_LIMIT_OFFSET;
+	u4b_ac_param |= (u32) ((cw_min & 0xF) << AC_PARAM_ECW_MIN_OFFSET);
+	u4b_ac_param |= (u32) ((cw_max & 0xF) << AC_PARAM_ECW_MAX_OFFSET);
+	u4b_ac_param |= (u32) (tx_op << AC_PARAM_TXOP_OFFSET);
 	RT_TRACE(rtlpriv, COMP_QOS, DBG_DMESG,
 		 ("queue:%x, ac_param:%x aifs:%x cwmin:%x cwmax:%x txop:%x\n",
-		  aci, u4b_ac_param, mac->ac[aci].aifs, mac->ac[aci].cw_min,
-		  mac->ac[aci].cw_max, mac->ac[aci].tx_op));
+		  aci, u4b_ac_param, mac->ac[aci].aifs, cw_min,
+		  cw_max, tx_op));
 	switch (aci) {
 	case AC1_BK:
 		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, u4b_ac_param);
@@ -1237,7 +1237,7 @@ static void _rtl92ce_poweroff_adapter(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE0);
-	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->bfw_ready)
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->fw_ready)
 		rtl92c_firmware_selfreset(hw);
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, 0x51);
 	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
@@ -1335,19 +1335,6 @@ void rtl92ce_update_interrupt_mask(struct ieee80211_hw *hw,
 	rtl92ce_enable_interrupt(hw);
 }
 
-static u8 _rtl92c_get_chnl_group(u8 chnl)
-{
-	u8 group;
-
-	if (chnl < 3)
-		group = 0;
-	else if (chnl < 9)
-		group = 1;
-	else
-		group = 2;
-	return group;
-}
-
 static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 						 bool autoload_fail,
 						 u8 *hwinfo)
@@ -1568,7 +1555,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	rtlefuse->eeprom_thermalmeter = (tempval & 0x1f);
 
 	if (rtlefuse->eeprom_thermalmeter == 0x1f || autoload_fail)
-		rtlefuse->b_apk_thermalmeterignore = true;
+		rtlefuse->apk_thermalmeterignore = true;
 
 	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
 	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
@@ -1625,7 +1612,7 @@ static void _rtl92ce_read_adapter_info(struct ieee80211_hw *hw)
 
 	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
 	rtlefuse->eeprom_version = *(u16 *)&hwinfo[EEPROM_VERSION];
-	rtlefuse->b_txpwr_fromeprom = true;
+	rtlefuse->txpwr_fromeprom = true;
 	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
@@ -1668,7 +1655,7 @@ static void _rtl92ce_hal_customized_behavior(struct ieee80211_hw *hw)
 
 	switch (rtlhal->oem_id) {
 	case RT_CID_819x_HP:
-		pcipriv->ledctl.bled_opendrain = true;
+		pcipriv->ledctl.led_opendrain = true;
 		break;
 	case RT_CID_819x_Lenovo:
 	case RT_CID_DEFAULT:
@@ -1693,10 +1680,10 @@ void rtl92ce_read_eeprom_info(struct ieee80211_hw *hw)
 
 	rtlhal->version = _rtl92ce_read_chip_version(hw);
 	if (get_rf_type(rtlphy) == RF_1T1R)
-		rtlpriv->dm.brfpath_rxenable[0] = true;
+		rtlpriv->dm.rfpath_rxenable[0] = true;
 	else
-		rtlpriv->dm.brfpath_rxenable[0] =
-		    rtlpriv->dm.brfpath_rxenable[1] = true;
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, ("VersionID = 0x%4x\n",
 						rtlhal->version));
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
@@ -1725,18 +1712,18 @@ void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
 	u32 ratr_value = (u32) mac->basic_rates;
-	u8 *p_mcsrate = mac->mcs;
+	u8 *mcsrate = mac->mcs;
 	u8 ratr_index = 0;
-	u8 b_nmode = mac->ht_enable;
+	u8 nmode = mac->ht_enable;
 	u8 mimo_ps = 1;
 	u16 shortgi_rate;
 	u32 tmp_ratr_value;
-	u8 b_curtxbw_40mhz = mac->bw_40;
-	u8 b_curshortgi_40mhz = mac->sgi_40;
-	u8 b_curshortgi_20mhz = mac->sgi_20;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 curshortgi_40mhz = mac->sgi_40;
+	u8 curshortgi_20mhz = mac->sgi_20;
 	enum wireless_mode wirelessmode = mac->mode;
 
-	ratr_value |= EF2BYTE((*(u16 *) (p_mcsrate))) << 12;
+	ratr_value |= ((*(u16 *) (mcsrate))) << 12;
 
 	switch (wirelessmode) {
 	case WIRELESS_MODE_B:
@@ -1750,7 +1737,7 @@ void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw)
 		break;
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
-		b_nmode = 1;
+		nmode = 1;
 		if (mimo_ps == 0) {
 			ratr_value &= 0x0007F005;
 		} else {
@@ -1776,9 +1763,8 @@ void rtl92ce_update_hal_rate_table(struct ieee80211_hw *hw)
 
 	ratr_value &= 0x0FFFFFFF;
 
-	if (b_nmode && ((b_curtxbw_40mhz &&
-			 b_curshortgi_40mhz) || (!b_curtxbw_40mhz &&
-						 b_curshortgi_20mhz))) {
+	if (nmode && ((curtxbw_40mhz && curshortgi_40mhz) || (!curtxbw_40mhz &&
+		       curshortgi_20mhz))) {
 
 		ratr_value |= 0x10000000;
 		tmp_ratr_value = (ratr_value >> 12);
@@ -1806,11 +1792,11 @@ void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
 	u32 ratr_bitmap = (u32) mac->basic_rates;
 	u8 *p_mcsrate = mac->mcs;
 	u8 ratr_index;
-	u8 b_curtxbw_40mhz = mac->bw_40;
-	u8 b_curshortgi_40mhz = mac->sgi_40;
-	u8 b_curshortgi_20mhz = mac->sgi_20;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 curshortgi_40mhz = mac->sgi_40;
+	u8 curshortgi_20mhz = mac->sgi_20;
 	enum wireless_mode wirelessmode = mac->mode;
-	bool b_shortgi = false;
+	bool shortgi = false;
 	u8 rate_mask[5];
 	u8 macid = 0;
 	u8 mimops = 1;
@@ -1852,7 +1838,7 @@ void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
 		} else {
 			if (rtlphy->rf_type == RF_1T2R ||
 			    rtlphy->rf_type == RF_1T1R) {
-				if (b_curtxbw_40mhz) {
+				if (curtxbw_40mhz) {
 					if (rssi_level == 1)
 						ratr_bitmap &= 0x000f0000;
 					else if (rssi_level == 2)
@@ -1868,7 +1854,7 @@ void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
 						ratr_bitmap &= 0x000ff005;
 				}
 			} else {
-				if (b_curtxbw_40mhz) {
+				if (curtxbw_40mhz) {
 					if (rssi_level == 1)
 						ratr_bitmap &= 0x0f0f0000;
 					else if (rssi_level == 2)
@@ -1886,13 +1872,13 @@ void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
 			}
 		}
 
-		if ((b_curtxbw_40mhz && b_curshortgi_40mhz) ||
-		    (!b_curtxbw_40mhz && b_curshortgi_20mhz)) {
+		if ((curtxbw_40mhz && curshortgi_40mhz) ||
+		    (!curtxbw_40mhz && curshortgi_20mhz)) {
 
 			if (macid == 0)
-				b_shortgi = true;
+				shortgi = true;
 			else if (macid == 1)
-				b_shortgi = false;
+				shortgi = false;
 		}
 		break;
 	default:
@@ -1906,9 +1892,9 @@ void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
 	}
 	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
 		 ("ratr_bitmap :%x\n", ratr_bitmap));
-	*(u32 *)&rate_mask = EF4BYTE((ratr_bitmap & 0x0fffffff) |
-				       (ratr_index << 28));
-	rate_mask[4] = macid | (b_shortgi ? 0x20 : 0x00) | 0x80;
+	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
+				       (ratr_index << 28);
+	rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
 	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG, ("Rate_index:%x, "
 						 "ratr_val:%x, %x:%x:%x:%x:%x\n",
 						 ratr_index, ratr_bitmap,
@@ -1940,13 +1926,13 @@ bool rtl92ce_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid)
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
 	u8 u1tmp;
-	bool b_actuallyset = false;
+	bool actuallyset = false;
 	unsigned long flag;
 
 	if ((rtlpci->up_first_time == 1) || (rtlpci->being_init_adapter))
 		return false;
 
-	if (ppsc->b_swrf_processing)
+	if (ppsc->swrf_processing)
 		return false;
 
 	spin_lock_irqsave(&rtlpriv->locks.rf_ps_lock, flag);
@@ -1972,24 +1958,24 @@ bool rtl92ce_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid)
 	u1tmp = rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL);
 	e_rfpowerstate_toset = (u1tmp & BIT(3)) ? ERFON : ERFOFF;
 
-	if ((ppsc->b_hwradiooff == true) && (e_rfpowerstate_toset == ERFON)) {
+	if ((ppsc->hwradiooff == true) && (e_rfpowerstate_toset == ERFON)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 ("GPIOChangeRF  - HW Radio ON, RF ON\n"));
 
 		e_rfpowerstate_toset = ERFON;
-		ppsc->b_hwradiooff = false;
-		b_actuallyset = true;
-	} else if ((ppsc->b_hwradiooff == false)
+		ppsc->hwradiooff = false;
+		actuallyset = true;
+	} else if ((ppsc->hwradiooff == false)
 		   && (e_rfpowerstate_toset == ERFOFF)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 ("GPIOChangeRF  - HW Radio OFF, RF OFF\n"));
 
 		e_rfpowerstate_toset = ERFOFF;
-		ppsc->b_hwradiooff = true;
-		b_actuallyset = true;
+		ppsc->hwradiooff = true;
+		actuallyset = true;
 	}
 
-	if (b_actuallyset) {
+	if (actuallyset) {
 		if (e_rfpowerstate_toset == ERFON) {
 			if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
 			    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_ASPM)) {
@@ -2028,7 +2014,7 @@ bool rtl92ce_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid)
 	}
 
 	*valid = 1;
-	return !ppsc->b_hwradiooff;
+	return !ppsc->hwradiooff;
 
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
index 305c819c8c78..a3dfdb635168 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
@@ -30,6 +30,8 @@
 #ifndef __RTL92CE_HW_H__
 #define __RTL92CE_HW_H__
 
+#define H2C_RA_MASK	6
+
 void rtl92ce_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl92ce_read_eeprom_info(struct ieee80211_hw *hw);
 void rtl92ce_interrupt_recognized(struct ieee80211_hw *hw,
@@ -53,5 +55,14 @@ void rtl92ce_enable_hw_security_config(struct ieee80211_hw *hw);
 void rtl92ce_set_key(struct ieee80211_hw *hw, u32 key_index,
 		     u8 *p_macaddr, bool is_group, u8 enc_algo,
 		     bool is_wepkey, bool clear_all);
+bool _rtl92ce_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+int rtl92c_download_fw(struct ieee80211_hw *hw);
+void rtl92c_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl92c_fill_h2c_cmd(struct ieee80211_hw *hw,
+			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer);
+bool rtl92ce_phy_mac_config(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/led.c b/drivers/net/wireless/rtlwifi/rtl8192ce/led.c
index 78a0569208ea..7b1da8d7508f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/led.c
@@ -57,7 +57,7 @@ void rtl92ce_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 			 ("switch case not process\n"));
 		break;
 	}
-	pled->b_ledon = true;
+	pled->ledon = true;
 }
 
 void rtl92ce_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
@@ -76,7 +76,7 @@ void rtl92ce_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 		break;
 	case LED_PIN_LED0:
 		ledcfg &= 0xf0;
-		if (pcipriv->ledctl.bled_opendrain == true)
+		if (pcipriv->ledctl.led_opendrain == true)
 			rtl_write_byte(rtlpriv, REG_LEDCFG2,
 				       (ledcfg | BIT(1) | BIT(5) | BIT(6)));
 		else
@@ -92,7 +92,7 @@ void rtl92ce_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 			 ("switch case not process\n"));
 		break;
 	}
-	pled->b_ledon = false;
+	pled->ledon = false;
 }
 
 void rtl92ce_init_sw_leds(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
index 45044117139a..d0541e8c6012 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
@@ -32,89 +32,13 @@
 #include "../ps.h"
 #include "reg.h"
 #include "def.h"
+#include "hw.h"
 #include "phy.h"
 #include "rf.h"
 #include "dm.h"
 #include "table.h"
 
-static u32 _rtl92c_phy_fw_rf_serial_read(struct ieee80211_hw *hw,
-					 enum radio_path rfpath, u32 offset);
-static void _rtl92c_phy_fw_rf_serial_write(struct ieee80211_hw *hw,
-					   enum radio_path rfpath, u32 offset,
-					   u32 data);
-static u32 _rtl92c_phy_rf_serial_read(struct ieee80211_hw *hw,
-				      enum radio_path rfpath, u32 offset);
-static void _rtl92c_phy_rf_serial_write(struct ieee80211_hw *hw,
-					enum radio_path rfpath, u32 offset,
-					u32 data);
-static u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask);
-static bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw);
-static bool _rtl92c_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
-static bool _rtl92c_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						  u8 configtype);
-static bool _rtl92c_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						    u8 configtype);
-static void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
-static bool _rtl92c_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
-					     u32 cmdtableidx, u32 cmdtablesz,
-					     enum swchnlcmd_id cmdid, u32 para1,
-					     u32 para2, u32 msdelay);
-static bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
-					     u8 channel, u8 *stage, u8 *step,
-					     u32 *delay);
-static u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
-				       enum wireless_mode wirelessmode,
-				       long power_indbm);
-static bool _rtl92c_phy_config_rf_external_pa(struct ieee80211_hw *hw,
-					      enum radio_path rfpath);
-static long _rtl92c_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
-					 enum wireless_mode wirelessmode,
-					 u8 txpwridx);
-u32 rtl92c_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 returnvalue, originalvalue, bitshift;
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), "
-					       "bitmask(%#x)\n", regaddr,
-					       bitmask));
-	originalvalue = rtl_read_dword(rtlpriv, regaddr);
-	bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
-	returnvalue = (originalvalue & bitmask) >> bitshift;
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("BBR MASK=0x%x "
-					       "Addr[0x%x]=0x%x\n", bitmask,
-					       regaddr, originalvalue));
-
-	return returnvalue;
-
-}
-
-void rtl92c_phy_set_bb_reg(struct ieee80211_hw *hw,
-			   u32 regaddr, u32 bitmask, u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 originalvalue, bitshift;
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
-					       " data(%#x)\n", regaddr, bitmask,
-					       data));
-
-	if (bitmask != MASKDWORD) {
-		originalvalue = rtl_read_dword(rtlpriv, regaddr);
-		bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
-		data = ((originalvalue & (~bitmask)) | (data << bitshift));
-	}
-
-	rtl_write_dword(rtlpriv, regaddr, data);
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
-					       " data(%#x)\n", regaddr, bitmask,
-					       data));
-
-}
-
-u32 rtl92c_phy_query_rf_reg(struct ieee80211_hw *hw,
+u32 rtl92ce_phy_query_rf_reg(struct ieee80211_hw *hw,
 			    enum radio_path rfpath, u32 regaddr, u32 bitmask)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -149,7 +73,7 @@ u32 rtl92c_phy_query_rf_reg(struct ieee80211_hw *hw,
 	return readback_value;
 }
 
-void rtl92c_phy_set_rf_reg(struct ieee80211_hw *hw,
+void rtl92ce_phy_set_rf_reg(struct ieee80211_hw *hw,
 			   enum radio_path rfpath,
 			   u32 regaddr, u32 bitmask, u32 data)
 {
@@ -197,137 +121,25 @@ void rtl92c_phy_set_rf_reg(struct ieee80211_hw *hw,
 					       bitmask, data, rfpath));
 }
 
-static u32 _rtl92c_phy_fw_rf_serial_read(struct ieee80211_hw *hw,
-					 enum radio_path rfpath, u32 offset)
-{
-	RT_ASSERT(false, ("deprecated!\n"));
-	return 0;
-}
-
-static void _rtl92c_phy_fw_rf_serial_write(struct ieee80211_hw *hw,
-					   enum radio_path rfpath, u32 offset,
-					   u32 data)
-{
-	RT_ASSERT(false, ("deprecated!\n"));
-}
-
-static u32 _rtl92c_phy_rf_serial_read(struct ieee80211_hw *hw,
-				      enum radio_path rfpath, u32 offset)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
-	u32 newoffset;
-	u32 tmplong, tmplong2;
-	u8 rfpi_enable = 0;
-	u32 retvalue;
-
-	offset &= 0x3f;
-	newoffset = offset;
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("return all one\n"));
-		return 0xFFFFFFFF;
-	}
-	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
-	if (rfpath == RF90_PATH_A)
-		tmplong2 = tmplong;
-	else
-		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
-	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
-	    (newoffset << 23) | BLSSIREADEDGE;
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong & (~BLSSIREADEDGE));
-	mdelay(1);
-	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
-	mdelay(1);
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong | BLSSIREADEDGE);
-	mdelay(1);
-	if (rfpath == RF90_PATH_A)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
-						 BIT(8));
-	else if (rfpath == RF90_PATH_B)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
-						 BIT(8));
-	if (rfpi_enable)
-		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readbackpi,
-					 BLSSIREADBACKDATA);
-	else
-		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readback,
-					 BLSSIREADBACKDATA);
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("RFR-%d Addr[0x%x]=0x%x\n",
-					       rfpath, pphyreg->rflssi_readback,
-					       retvalue));
-	return retvalue;
-}
-
-static void _rtl92c_phy_rf_serial_write(struct ieee80211_hw *hw,
-					enum radio_path rfpath, u32 offset,
-					u32 data)
-{
-	u32 data_and_addr;
-	u32 newoffset;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
-
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("stop\n"));
-		return;
-	}
-	offset &= 0x3f;
-	newoffset = offset;
-	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
-	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("RFW-%d Addr[0x%x]=0x%x\n",
-					       rfpath, pphyreg->rf3wire_offset,
-					       data_and_addr));
-}
-
-static u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++) {
-		if (((bitmask >> i) & 0x1) == 1)
-			break;
-	}
-	return i;
-}
-
-static void _rtl92c_phy_bb_config_1t(struct ieee80211_hw *hw)
-{
-	rtl_set_bbreg(hw, RFPGA0_TXINFO, 0x3, 0x2);
-	rtl_set_bbreg(hw, RFPGA1_TXINFO, 0x300033, 0x200022);
-	rtl_set_bbreg(hw, RCCK0_AFESETTING, MASKBYTE3, 0x45);
-	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0, 0x23);
-	rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, 0x30, 0x1);
-	rtl_set_bbreg(hw, 0xe74, 0x0c000000, 0x2);
-	rtl_set_bbreg(hw, 0xe78, 0x0c000000, 0x2);
-	rtl_set_bbreg(hw, 0xe7c, 0x0c000000, 0x2);
-	rtl_set_bbreg(hw, 0xe80, 0x0c000000, 0x2);
-	rtl_set_bbreg(hw, 0xe88, 0x0c000000, 0x2);
-}
-
-bool rtl92c_phy_mac_config(struct ieee80211_hw *hw)
+bool rtl92ce_phy_mac_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	bool is92c = IS_92C_SERIAL(rtlhal->version);
-	bool rtstatus = _rtl92c_phy_config_mac_with_headerfile(hw);
+	bool rtstatus = _rtl92ce_phy_config_mac_with_headerfile(hw);
 
 	if (is92c)
 		rtl_write_byte(rtlpriv, 0x14, 0x71);
 	return rtstatus;
 }
 
-bool rtl92c_phy_bb_config(struct ieee80211_hw *hw)
+bool rtl92ce_phy_bb_config(struct ieee80211_hw *hw)
 {
 	bool rtstatus = true;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 regval;
 	u32 regvaldw;
-	u8 b_reg_hwparafile = 1;
+	u8 reg_hwparafile = 1;
 
 	_rtl92c_phy_init_bb_rf_register_definition(hw);
 	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
@@ -342,56 +154,12 @@ bool rtl92c_phy_bb_config(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
 	regvaldw = rtl_read_dword(rtlpriv, REG_LEDCFG0);
 	rtl_write_dword(rtlpriv, REG_LEDCFG0, regvaldw | BIT(23));
-	if (b_reg_hwparafile == 1)
+	if (reg_hwparafile == 1)
 		rtstatus = _rtl92c_phy_bb8192c_config_parafile(hw);
 	return rtstatus;
 }
 
-bool rtl92c_phy_rf_config(struct ieee80211_hw *hw)
-{
-	return rtl92c_phy_rf6052_config(hw);
-}
-
-static bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	bool rtstatus;
-
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("==>\n"));
-	rtstatus = _rtl92c_phy_config_bb_with_headerfile(hw,
-						 BASEBAND_CONFIG_PHY_REG);
-	if (rtstatus != true) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
-		return false;
-	}
-	if (rtlphy->rf_type == RF_1T2R) {
-		_rtl92c_phy_bb_config_1t(hw);
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("Config to 1T!!\n"));
-	}
-	if (rtlefuse->autoload_failflag == false) {
-		rtlphy->pwrgroup_cnt = 0;
-		rtstatus = _rtl92c_phy_config_bb_with_pgheaderfile(hw,
-						   BASEBAND_CONFIG_PHY_REG);
-	}
-	if (rtstatus != true) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
-		return false;
-	}
-	rtstatus = _rtl92c_phy_config_bb_with_headerfile(hw,
-						 BASEBAND_CONFIG_AGC_TAB);
-	if (rtstatus != true) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("AGC Table Fail\n"));
-		return false;
-	}
-	rtlphy->bcck_high_power = (bool) (rtl_get_bbreg(hw,
-						RFPGA0_XA_HSSIPARAMETER2,
-						0x200));
-	return true;
-}
-
-static bool _rtl92c_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+bool _rtl92ce_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 i;
@@ -408,11 +176,7 @@ static bool _rtl92c_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
 	return true;
 }
 
-void rtl92c_phy_config_bb_external_pa(struct ieee80211_hw *hw)
-{
-}
-
-static bool _rtl92c_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+bool _rtl92ce_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 						  u8 configtype)
 {
 	int i;
@@ -456,7 +220,6 @@ static bool _rtl92c_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 				  phy_regarray_table[i],
 				  phy_regarray_table[i + 1]));
 		}
-		rtl92c_phy_config_bb_external_pa(hw);
 	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
 		for (i = 0; i < agctab_arraylen; i = i + 2) {
 			rtl_set_bbreg(hw, agctab_array_table[i], MASKDWORD,
@@ -472,175 +235,7 @@ static bool _rtl92c_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 	return true;
 }
 
-static void _rtl92c_store_pwrIndex_diffrate_offset(struct ieee80211_hw *hw,
-						   u32 regaddr, u32 bitmask,
-						   u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (regaddr == RTXAGC_A_RATE18_06) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][0] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][0]));
-	}
-	if (regaddr == RTXAGC_A_RATE54_24) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][1] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][1] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][1]));
-	}
-	if (regaddr == RTXAGC_A_CCK1_MCS32) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][6] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][6] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][6]));
-	}
-	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][7] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][7]));
-	}
-	if (regaddr == RTXAGC_A_MCS03_MCS00) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][2] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][2] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][2]));
-	}
-	if (regaddr == RTXAGC_A_MCS07_MCS04) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][3] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][3] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][3]));
-	}
-	if (regaddr == RTXAGC_A_MCS11_MCS08) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][4] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][4] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][4]));
-	}
-	if (regaddr == RTXAGC_A_MCS15_MCS12) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][5] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][5] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][5]));
-	}
-	if (regaddr == RTXAGC_B_RATE18_06) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][8] =
-		    data;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][8] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][8]));
-	}
-	if (regaddr == RTXAGC_B_RATE54_24) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][9] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][9] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][9]));
-	}
-
-	if (regaddr == RTXAGC_B_CCK1_55_MCS32) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][14] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][14] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][14]));
-	}
-
-	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][15] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][15]));
-	}
-
-	if (regaddr == RTXAGC_B_MCS03_MCS00) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][10] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][10] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][10]));
-	}
-
-	if (regaddr == RTXAGC_B_MCS07_MCS04) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][11] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][11] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][11]));
-	}
-
-	if (regaddr == RTXAGC_B_MCS11_MCS08) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][12] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][12] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][12]));
-	}
-
-	if (regaddr == RTXAGC_B_MCS15_MCS12) {
-		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][13] =
-		    data;
-
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 ("MCSTxPowerLevelOriginalOffset[%d][13] = 0x%x\n",
-			  rtlphy->pwrgroup_cnt,
-			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
-							    pwrgroup_cnt][13]));
-
-		rtlphy->pwrgroup_cnt++;
-	}
-}
-
-static bool _rtl92c_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+bool _rtl92ce_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 						    u8 configtype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -679,13 +274,7 @@ static bool _rtl92c_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 	return true;
 }
 
-static bool _rtl92c_phy_config_rf_external_pa(struct ieee80211_hw *hw,
-					      enum radio_path rfpath)
-{
-	return true;
-}
-
-bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+bool rtl92ce_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 					  enum radio_path rfpath)
 {
 
@@ -740,7 +329,6 @@ bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 				udelay(1);
 			}
 		}
-		_rtl92c_phy_config_rf_external_pa(hw, rfpath);
 		break;
 	case RF90_PATH_B:
 		for (i = 0; i < radiob_arraylen; i = i + 2) {
@@ -776,346 +364,7 @@ bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 	return true;
 }
 
-void rtl92c_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	rtlphy->default_initialgain[0] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
-	rtlphy->default_initialgain[1] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
-	rtlphy->default_initialgain[2] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
-	rtlphy->default_initialgain[3] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
-
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-		 ("Default initial gain (c50=0x%x, "
-		  "c58=0x%x, c60=0x%x, c68=0x%x\n",
-		  rtlphy->default_initialgain[0],
-		  rtlphy->default_initialgain[1],
-		  rtlphy->default_initialgain[2],
-		  rtlphy->default_initialgain[3]));
-
-	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
-					       ROFDM0_RXDETECTOR3, MASKBYTE0);
-	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
-					      ROFDM0_RXDETECTOR2, MASKDWORD);
-
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-		 ("Default framesync (0x%x) = 0x%x\n",
-		  ROFDM0_RXDETECTOR3, rtlphy->framesync));
-}
-
-static void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfi = RFPGA0_XAB_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfi = RFPGA0_XAB_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
-	    RFPGA0_XA_LSSIPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
-	    RFPGA0_XB_LSSIPARAMETER;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_B].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
-	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
-	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfswitch_control =
-	    RFPGA0_XAB_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_B].rfswitch_control =
-	    RFPGA0_XAB_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_C].rfswitch_control =
-	    RFPGA0_XCD_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_D].rfswitch_control =
-	    RFPGA0_XCD_SWITCHCONTROL;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control1 = ROFDM0_XAAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control1 = ROFDM0_XBAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control2 = ROFDM0_XAAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control2 = ROFDM0_XBAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfrxiq_imbalance =
-	    ROFDM0_XARXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfrxiq_imbalance =
-	    ROFDM0_XBRXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbalance =
-	    ROFDM0_XCRXIQIMBANLANCE;
-	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbalance =
-	    ROFDM0_XDRXIQIMBALANCE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfrx_afe = ROFDM0_XARXAFE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfrx_afe = ROFDM0_XBRXAFE;
-	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
-	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rftxiq_imbalance =
-	    ROFDM0_XATXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_B].rftxiq_imbalance =
-	    ROFDM0_XBTXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbalance =
-	    ROFDM0_XCTXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbalance =
-	    ROFDM0_XDTXIQIMBALANCE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rftx_afe = ROFDM0_XATXAFE;
-	rtlphy->phyreg_def[RF90_PATH_B].rftx_afe = ROFDM0_XBTXAFE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftx_afe = ROFDM0_XCTXAFE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftx_afe = ROFDM0_XDTXAFE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readback =
-	    RFPGA0_XA_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readback =
-	    RFPGA0_XB_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_C].rflssi_readback =
-	    RFPGA0_XC_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_D].rflssi_readback =
-	    RFPGA0_XD_LSSIREADBACK;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readbackpi =
-	    TRANSCEIVEA_HSPI_READBACK;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi =
-	    TRANSCEIVEB_HSPI_READBACK;
-
-}
-
-void rtl92c_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 txpwr_level;
-	long txpwr_dbm;
-
-	txpwr_level = rtlphy->cur_cck_txpwridx;
-	txpwr_dbm = _rtl92c_phy_txpwr_idx_to_dbm(hw,
-						 WIRELESS_MODE_B, txpwr_level);
-	txpwr_level = rtlphy->cur_ofdm24g_txpwridx +
-	    rtlefuse->legacy_ht_txpowerdiff;
-	if (_rtl92c_phy_txpwr_idx_to_dbm(hw,
-					 WIRELESS_MODE_G,
-					 txpwr_level) > txpwr_dbm)
-		txpwr_dbm =
-		    _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
-						 txpwr_level);
-	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (_rtl92c_phy_txpwr_idx_to_dbm(hw,
-					 WIRELESS_MODE_N_24G,
-					 txpwr_level) > txpwr_dbm)
-		txpwr_dbm =
-		    _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
-						 txpwr_level);
-	*powerlevel = txpwr_dbm;
-}
-
-static void _rtl92c_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
-				      u8 *cckpowerlevel, u8 *ofdmpowerlevel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 index = (channel - 1);
-
-	cckpowerlevel[RF90_PATH_A] =
-	    rtlefuse->txpwrlevel_cck[RF90_PATH_A][index];
-	cckpowerlevel[RF90_PATH_B] =
-	    rtlefuse->txpwrlevel_cck[RF90_PATH_B][index];
-	if (get_rf_type(rtlphy) == RF_1T2R || get_rf_type(rtlphy) == RF_1T1R) {
-		ofdmpowerlevel[RF90_PATH_A] =
-		    rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index];
-		ofdmpowerlevel[RF90_PATH_B] =
-		    rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_B][index];
-	} else if (get_rf_type(rtlphy) == RF_2T2R) {
-		ofdmpowerlevel[RF90_PATH_A] =
-		    rtlefuse->txpwrlevel_ht40_2s[RF90_PATH_A][index];
-		ofdmpowerlevel[RF90_PATH_B] =
-		    rtlefuse->txpwrlevel_ht40_2s[RF90_PATH_B][index];
-	}
-}
-
-static void _rtl92c_ccxpower_index_check(struct ieee80211_hw *hw,
-					 u8 channel, u8 *cckpowerlevel,
-					 u8 *ofdmpowerlevel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	rtlphy->cur_cck_txpwridx = cckpowerlevel[0];
-	rtlphy->cur_ofdm24g_txpwridx = ofdmpowerlevel[0];
-}
-
-void rtl92c_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 cckpowerlevel[2], ofdmpowerlevel[2];
-
-	if (rtlefuse->b_txpwr_fromeprom == false)
-		return;
-	_rtl92c_get_txpower_index(hw, channel,
-				  &cckpowerlevel[0], &ofdmpowerlevel[0]);
-	_rtl92c_ccxpower_index_check(hw,
-				     channel, &cckpowerlevel[0],
-				     &ofdmpowerlevel[0]);
-	rtl92c_phy_rf6052_set_cck_txpower(hw, &cckpowerlevel[0]);
-	rtl92c_phy_rf6052_set_ofdm_txpower(hw, &ofdmpowerlevel[0], channel);
-}
-
-bool rtl92c_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 idx;
-	u8 rf_path;
-
-	u8 ccktxpwridx = _rtl92c_phy_dbm_to_txpwr_Idx(hw,
-						      WIRELESS_MODE_B,
-						      power_indbm);
-	u8 ofdmtxpwridx = _rtl92c_phy_dbm_to_txpwr_Idx(hw,
-						       WIRELESS_MODE_N_24G,
-						       power_indbm);
-	if (ofdmtxpwridx - rtlefuse->legacy_ht_txpowerdiff > 0)
-		ofdmtxpwridx -= rtlefuse->legacy_ht_txpowerdiff;
-	else
-		ofdmtxpwridx = 0;
-	RT_TRACE(rtlpriv, COMP_TXAGC, DBG_TRACE,
-		 ("%lx dBm, ccktxpwridx = %d, ofdmtxpwridx = %d\n",
-		  power_indbm, ccktxpwridx, ofdmtxpwridx));
-	for (idx = 0; idx < 14; idx++) {
-		for (rf_path = 0; rf_path < 2; rf_path++) {
-			rtlefuse->txpwrlevel_cck[rf_path][idx] = ccktxpwridx;
-			rtlefuse->txpwrlevel_ht40_1s[rf_path][idx] =
-			    ofdmtxpwridx;
-			rtlefuse->txpwrlevel_ht40_2s[rf_path][idx] =
-			    ofdmtxpwridx;
-		}
-	}
-	rtl92c_phy_set_txpower_level(hw, rtlphy->current_channel);
-	return true;
-}
-
-void rtl92c_phy_set_beacon_hw_reg(struct ieee80211_hw *hw, u16 beaconinterval)
-{
-}
-
-static u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
-				       enum wireless_mode wirelessmode,
-				       long power_indbm)
-{
-	u8 txpwridx;
-	long offset;
-
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		offset = -7;
-		break;
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-		offset = -8;
-		break;
-	default:
-		offset = -8;
-		break;
-	}
-
-	if ((power_indbm - offset) > 0)
-		txpwridx = (u8) ((power_indbm - offset) * 2);
-	else
-		txpwridx = 0;
-
-	if (txpwridx > MAX_TXPWR_IDX_NMODE_92S)
-		txpwridx = MAX_TXPWR_IDX_NMODE_92S;
-
-	return txpwridx;
-}
-
-static long _rtl92c_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
-					 enum wireless_mode wirelessmode,
-					 u8 txpwridx)
-{
-	long offset;
-	long pwrout_dbm;
-
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		offset = -7;
-		break;
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-		offset = -8;
-		break;
-	default:
-		offset = -8;
-		break;
-	}
-	pwrout_dbm = txpwridx / 2 + offset;
-	return pwrout_dbm;
-}
-
-void rtl92c_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	enum io_type iotype;
-
-	if (!is_hal_stop(rtlhal)) {
-		switch (operation) {
-		case SCAN_OPT_BACKUP:
-			iotype = IO_CMD_PAUSE_DM_BY_SCAN;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_IO_CMD,
-						      (u8 *)&iotype);
-
-			break;
-		case SCAN_OPT_RESTORE:
-			iotype = IO_CMD_RESUME_DM_BY_SCAN;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_IO_CMD,
-						      (u8 *)&iotype);
-			break;
-		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 ("Unknown Scan Backup operation.\n"));
-			break;
-		}
-	}
-}
-
-void rtl92c_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+void rtl92ce_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -1183,645 +432,7 @@ void rtl92c_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, ("<==\n"));
 }
 
-void rtl92c_phy_set_bw_mode(struct ieee80211_hw *hw,
-			    enum nl80211_channel_type ch_type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 tmp_bw = rtlphy->current_chan_bw;
-
-	if (rtlphy->set_bwmode_inprogress)
-		return;
-	rtlphy->set_bwmode_inprogress = true;
-	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw)))
-		rtl92c_phy_set_bw_mode_callback(hw);
-	else {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 ("FALSE driver sleep or unload\n"));
-		rtlphy->set_bwmode_inprogress = false;
-		rtlphy->current_chan_bw = tmp_bw;
-	}
-}
-
-void rtl92c_phy_sw_chnl_callback(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 delay;
-
-	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
-		 ("switch to channel%d\n", rtlphy->current_channel));
-	if (is_hal_stop(rtlhal))
-		return;
-	do {
-		if (!rtlphy->sw_chnl_inprogress)
-			break;
-		if (!_rtl92c_phy_sw_chnl_step_by_step
-		    (hw, rtlphy->current_channel, &rtlphy->sw_chnl_stage,
-		     &rtlphy->sw_chnl_step, &delay)) {
-			if (delay > 0)
-				mdelay(delay);
-			else
-				continue;
-		} else
-			rtlphy->sw_chnl_inprogress = false;
-		break;
-	} while (true);
-	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, ("<==\n"));
-}
-
-u8 rtl92c_phy_sw_chnl(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (rtlphy->sw_chnl_inprogress)
-		return 0;
-	if (rtlphy->set_bwmode_inprogress)
-		return 0;
-	RT_ASSERT((rtlphy->current_channel <= 14),
-		  ("WIRELESS_MODE_G but channel>14"));
-	rtlphy->sw_chnl_inprogress = true;
-	rtlphy->sw_chnl_stage = 0;
-	rtlphy->sw_chnl_step = 0;
-	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
-		rtl92c_phy_sw_chnl_callback(hw);
-		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
-			 ("sw_chnl_inprogress false schdule workitem\n"));
-		rtlphy->sw_chnl_inprogress = false;
-	} else {
-		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
-			 ("sw_chnl_inprogress false driver sleep or"
-			  " unload\n"));
-		rtlphy->sw_chnl_inprogress = false;
-	}
-	return 1;
-}
-
-static bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
-					     u8 channel, u8 *stage, u8 *step,
-					     u32 *delay)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
-	u32 precommoncmdcnt;
-	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
-	u32 postcommoncmdcnt;
-	struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
-	u32 rfdependcmdcnt;
-	struct swchnlcmd *currentcmd = NULL;
-	u8 rfpath;
-	u8 num_total_rfpath = rtlphy->num_total_rfpath;
-
-	precommoncmdcnt = 0;
-	_rtl92c_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-					 MAX_PRECMD_CNT,
-					 CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
-	_rtl92c_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
-
-	postcommoncmdcnt = 0;
-
-	_rtl92c_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
-					 MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
-
-	rfdependcmdcnt = 0;
-
-	RT_ASSERT((channel >= 1 && channel <= 14),
-		  ("illegal channel for Zebra: %d\n", channel));
-
-	_rtl92c_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-					 MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
-					 RF_CHNLBW, channel, 10);
-
-	_rtl92c_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-					 MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0,
-					 0);
-
-	do {
-		switch (*stage) {
-		case 0:
-			currentcmd = &precommoncmd[*step];
-			break;
-		case 1:
-			currentcmd = &rfdependcmd[*step];
-			break;
-		case 2:
-			currentcmd = &postcommoncmd[*step];
-			break;
-		}
-
-		if (currentcmd->cmdid == CMDID_END) {
-			if ((*stage) == 2) {
-				return true;
-			} else {
-				(*stage)++;
-				(*step) = 0;
-				continue;
-			}
-		}
-
-		switch (currentcmd->cmdid) {
-		case CMDID_SET_TXPOWEROWER_LEVEL:
-			rtl92c_phy_set_txpower_level(hw, channel);
-			break;
-		case CMDID_WRITEPORT_ULONG:
-			rtl_write_dword(rtlpriv, currentcmd->para1,
-					currentcmd->para2);
-			break;
-		case CMDID_WRITEPORT_USHORT:
-			rtl_write_word(rtlpriv, currentcmd->para1,
-				       (u16) currentcmd->para2);
-			break;
-		case CMDID_WRITEPORT_UCHAR:
-			rtl_write_byte(rtlpriv, currentcmd->para1,
-				       (u8) currentcmd->para2);
-			break;
-		case CMDID_RF_WRITEREG:
-			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
-				rtlphy->rfreg_chnlval[rfpath] =
-				    ((rtlphy->rfreg_chnlval[rfpath] &
-				      0xfffffc00) | currentcmd->para2);
-
-				rtl_set_rfreg(hw, (enum radio_path)rfpath,
-					      currentcmd->para1,
-					      RFREG_OFFSET_MASK,
-					      rtlphy->rfreg_chnlval[rfpath]);
-			}
-			break;
-		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 ("switch case not process\n"));
-			break;
-		}
-
-		break;
-	} while (true);
-
-	(*delay) = currentcmd->msdelay;
-	(*step)++;
-	return false;
-}
-
-static bool _rtl92c_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
-					     u32 cmdtableidx, u32 cmdtablesz,
-					     enum swchnlcmd_id cmdid,
-					     u32 para1, u32 para2, u32 msdelay)
-{
-	struct swchnlcmd *pcmd;
-
-	if (cmdtable == NULL) {
-		RT_ASSERT(false, ("cmdtable cannot be NULL.\n"));
-		return false;
-	}
-
-	if (cmdtableidx >= cmdtablesz)
-		return false;
-
-	pcmd = cmdtable + cmdtableidx;
-	pcmd->cmdid = cmdid;
-	pcmd->para1 = para1;
-	pcmd->para2 = para2;
-	pcmd->msdelay = msdelay;
-	return true;
-}
-
-bool rtl8192_phy_check_is_legal_rfpath(struct ieee80211_hw *hw, u32 rfpath)
-{
-	return true;
-}
-
-static u8 _rtl92c_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
-{
-	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
-	u8 result = 0x00;
-
-	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1f);
-	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x10008c1f);
-	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x82140102);
-	rtl_set_bbreg(hw, 0xe3c, MASKDWORD,
-		      config_pathb ? 0x28160202 : 0x28160502);
-
-	if (config_pathb) {
-		rtl_set_bbreg(hw, 0xe50, MASKDWORD, 0x10008c22);
-		rtl_set_bbreg(hw, 0xe54, MASKDWORD, 0x10008c22);
-		rtl_set_bbreg(hw, 0xe58, MASKDWORD, 0x82140102);
-		rtl_set_bbreg(hw, 0xe5c, MASKDWORD, 0x28160202);
-	}
-
-	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x001028d1);
-	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
-	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
-
-	mdelay(IQK_DELAY_TIME);
-
-	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
-	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
-	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
-	reg_ea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
-
-	if (!(reg_eac & BIT(28)) &&
-	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
-	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
-		result |= 0x01;
-	else
-		return result;
-
-	if (!(reg_eac & BIT(27)) &&
-	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
-	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
-		result |= 0x02;
-	return result;
-}
-
-static u8 _rtl92c_phy_path_b_iqk(struct ieee80211_hw *hw)
-{
-	u32 reg_eac, reg_eb4, reg_ebc, reg_ec4, reg_ecc;
-	u8 result = 0x00;
-
-	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000002);
-	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000000);
-	mdelay(IQK_DELAY_TIME);
-	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
-	reg_eb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
-	reg_ebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
-	reg_ec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
-	reg_ecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
-	if (!(reg_eac & BIT(31)) &&
-	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
-	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
-		result |= 0x01;
-	else
-		return result;
-
-	if (!(reg_eac & BIT(30)) &&
-	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
-	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
-		result |= 0x02;
-	return result;
-}
-
-static void _rtl92c_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
-					       bool b_iqk_ok, long result[][8],
-					       u8 final_candidate, bool btxonly)
-{
-	u32 oldval_0, x, tx0_a, reg;
-	long y, tx0_c;
-
-	if (final_candidate == 0xFF)
-		return;
-	else if (b_iqk_ok) {
-		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
-					  MASKDWORD) >> 22) & 0x3FF;
-		x = result[final_candidate][0];
-		if ((x & 0x00000200) != 0)
-			x = x | 0xFFFFFC00;
-		tx0_a = (x * oldval_0) >> 8;
-		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x3FF, tx0_a);
-		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(31),
-			      ((x * oldval_0 >> 7) & 0x1));
-		y = result[final_candidate][1];
-		if ((y & 0x00000200) != 0)
-			y = y | 0xFFFFFC00;
-		tx0_c = (y * oldval_0) >> 8;
-		rtl_set_bbreg(hw, ROFDM0_XCTXAFE, 0xF0000000,
-			      ((tx0_c & 0x3C0) >> 6));
-		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x003F0000,
-			      (tx0_c & 0x3F));
-		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(29),
-			      ((y * oldval_0 >> 7) & 0x1));
-		if (btxonly)
-			return;
-		reg = result[final_candidate][2];
-		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0x3FF, reg);
-		reg = result[final_candidate][3] & 0x3F;
-		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0xFC00, reg);
-		reg = (result[final_candidate][3] >> 6) & 0xF;
-		rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg);
-	}
-}
-
-static void _rtl92c_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
-					       bool b_iqk_ok, long result[][8],
-					       u8 final_candidate, bool btxonly)
-{
-	u32 oldval_1, x, tx1_a, reg;
-	long y, tx1_c;
-
-	if (final_candidate == 0xFF)
-		return;
-	else if (b_iqk_ok) {
-		oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
-					  MASKDWORD) >> 22) & 0x3FF;
-		x = result[final_candidate][4];
-		if ((x & 0x00000200) != 0)
-			x = x | 0xFFFFFC00;
-		tx1_a = (x * oldval_1) >> 8;
-		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x3FF, tx1_a);
-		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(27),
-			      ((x * oldval_1 >> 7) & 0x1));
-		y = result[final_candidate][5];
-		if ((y & 0x00000200) != 0)
-			y = y | 0xFFFFFC00;
-		tx1_c = (y * oldval_1) >> 8;
-		rtl_set_bbreg(hw, ROFDM0_XDTXAFE, 0xF0000000,
-			      ((tx1_c & 0x3C0) >> 6));
-		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x003F0000,
-			      (tx1_c & 0x3F));
-		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(25),
-			      ((y * oldval_1 >> 7) & 0x1));
-		if (btxonly)
-			return;
-		reg = result[final_candidate][6];
-		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0x3FF, reg);
-		reg = result[final_candidate][7] & 0x3F;
-		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0xFC00, reg);
-		reg = (result[final_candidate][7] >> 6) & 0xF;
-		rtl_set_bbreg(hw, ROFDM0_AGCRSSITABLE, 0x0000F000, reg);
-	}
-}
-
-static void _rtl92c_phy_save_adda_registers(struct ieee80211_hw *hw,
-					    u32 *addareg, u32 *addabackup,
-					    u32 registernum)
-{
-	u32 i;
-
-	for (i = 0; i < registernum; i++)
-		addabackup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
-}
-
-static void _rtl92c_phy_save_mac_registers(struct ieee80211_hw *hw,
-					   u32 *macreg, u32 *macbackup)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
-		macbackup[i] = rtl_read_byte(rtlpriv, macreg[i]);
-	macbackup[i] = rtl_read_dword(rtlpriv, macreg[i]);
-}
-
-static void _rtl92c_phy_reload_adda_registers(struct ieee80211_hw *hw,
-					      u32 *addareg, u32 *addabackup,
-					      u32 regiesternum)
-{
-	u32 i;
-
-	for (i = 0; i < regiesternum; i++)
-		rtl_set_bbreg(hw, addareg[i], MASKDWORD, addabackup[i]);
-}
-
-static void _rtl92c_phy_reload_mac_registers(struct ieee80211_hw *hw,
-					     u32 *macreg, u32 *macbackup)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
-		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
-	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
-}
-
-static void _rtl92c_phy_path_adda_on(struct ieee80211_hw *hw,
-				     u32 *addareg, bool is_patha_on, bool is2t)
-{
-	u32 pathOn;
-	u32 i;
-
-	pathOn = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
-	if (false == is2t) {
-		pathOn = 0x0bdb25a0;
-		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
-	} else {
-		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathOn);
-	}
-
-	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
-		rtl_set_bbreg(hw, addareg[i], MASKDWORD, pathOn);
-}
-
-static void _rtl92c_phy_mac_setting_calibration(struct ieee80211_hw *hw,
-						u32 *macreg, u32 *macbackup)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_write_byte(rtlpriv, macreg[0], 0x3F);
-
-	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
-		rtl_write_byte(rtlpriv, macreg[i],
-			       (u8) (macbackup[i] & (~BIT(3))));
-	rtl_write_byte(rtlpriv, macreg[i], (u8) (macbackup[i] & (~BIT(5))));
-}
-
-static void _rtl92c_phy_path_a_standby(struct ieee80211_hw *hw)
-{
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
-	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
-}
-
-static void _rtl92c_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode)
-{
-	u32 mode;
-
-	mode = pi_mode ? 0x01000100 : 0x01000000;
-	rtl_set_bbreg(hw, 0x820, MASKDWORD, mode);
-	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
-}
-
-static bool _rtl92c_phy_simularity_compare(struct ieee80211_hw *hw,
-					   long result[][8], u8 c1, u8 c2)
-{
-	u32 i, j, diff, simularity_bitmap, bound;
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	u8 final_candidate[2] = { 0xFF, 0xFF };
-	bool bresult = true, is2t = IS_92C_SERIAL(rtlhal->version);
-
-	if (is2t)
-		bound = 8;
-	else
-		bound = 4;
-
-	simularity_bitmap = 0;
-
-	for (i = 0; i < bound; i++) {
-		diff = (result[c1][i] > result[c2][i]) ?
-		    (result[c1][i] - result[c2][i]) :
-		    (result[c2][i] - result[c1][i]);
-
-		if (diff > MAX_TOLERANCE) {
-			if ((i == 2 || i == 6) && !simularity_bitmap) {
-				if (result[c1][i] + result[c1][i + 1] == 0)
-					final_candidate[(i / 4)] = c2;
-				else if (result[c2][i] + result[c2][i + 1] == 0)
-					final_candidate[(i / 4)] = c1;
-				else
-					simularity_bitmap = simularity_bitmap |
-					    (1 << i);
-			} else
-				simularity_bitmap =
-				    simularity_bitmap | (1 << i);
-		}
-	}
-
-	if (simularity_bitmap == 0) {
-		for (i = 0; i < (bound / 4); i++) {
-			if (final_candidate[i] != 0xFF) {
-				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
-					result[3][j] =
-					    result[final_candidate[i]][j];
-				bresult = false;
-			}
-		}
-		return bresult;
-	} else if (!(simularity_bitmap & 0x0F)) {
-		for (i = 0; i < 4; i++)
-			result[3][i] = result[c1][i];
-		return false;
-	} else if (!(simularity_bitmap & 0xF0) && is2t) {
-		for (i = 4; i < 8; i++)
-			result[3][i] = result[c1][i];
-		return false;
-	} else {
-		return false;
-	}
-
-}
-
-static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
-				     long result[][8], u8 t, bool is2t)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 i;
-	u8 patha_ok, pathb_ok;
-	u32 adda_reg[IQK_ADDA_REG_NUM] = {
-		0x85c, 0xe6c, 0xe70, 0xe74,
-		0xe78, 0xe7c, 0xe80, 0xe84,
-		0xe88, 0xe8c, 0xed0, 0xed4,
-		0xed8, 0xedc, 0xee0, 0xeec
-	};
-
-	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
-		0x522, 0x550, 0x551, 0x040
-	};
-
-	const u32 retrycount = 2;
-
-	u32 bbvalue;
-
-	if (t == 0) {
-		bbvalue = rtl_get_bbreg(hw, 0x800, MASKDWORD);
-
-		_rtl92c_phy_save_adda_registers(hw, adda_reg,
-						rtlphy->adda_backup, 16);
-		_rtl92c_phy_save_mac_registers(hw, iqk_mac_reg,
-					       rtlphy->iqk_mac_backup);
-	}
-	_rtl92c_phy_path_adda_on(hw, adda_reg, true, is2t);
-	if (t == 0) {
-		rtlphy->b_rfpi_enable = (u8) rtl_get_bbreg(hw,
-						   RFPGA0_XA_HSSIPARAMETER1,
-						   BIT(8));
-	}
-	if (!rtlphy->b_rfpi_enable)
-		_rtl92c_phy_pi_mode_switch(hw, true);
-	if (t == 0) {
-		rtlphy->reg_c04 = rtl_get_bbreg(hw, 0xc04, MASKDWORD);
-		rtlphy->reg_c08 = rtl_get_bbreg(hw, 0xc08, MASKDWORD);
-		rtlphy->reg_874 = rtl_get_bbreg(hw, 0x874, MASKDWORD);
-	}
-	rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
-	rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
-	rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
-	if (is2t) {
-		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
-		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00010000);
-	}
-	_rtl92c_phy_mac_setting_calibration(hw, iqk_mac_reg,
-					    rtlphy->iqk_mac_backup);
-	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x00080000);
-	if (is2t)
-		rtl_set_bbreg(hw, 0xb6c, MASKDWORD, 0x00080000);
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
-	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x01004800);
-	for (i = 0; i < retrycount; i++) {
-		patha_ok = _rtl92c_phy_path_a_iqk(hw, is2t);
-		if (patha_ok == 0x03) {
-			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
-					0x3FF0000) >> 16;
-			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
-					0x3FF0000) >> 16;
-			result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
-					0x3FF0000) >> 16;
-			result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
-					0x3FF0000) >> 16;
-			break;
-		} else if (i == (retrycount - 1) && patha_ok == 0x01)
-			result[t][0] = (rtl_get_bbreg(hw, 0xe94,
-						      MASKDWORD) & 0x3FF0000) >>
-						      16;
-		result[t][1] =
-		    (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) & 0x3FF0000) >> 16;
-
-	}
-
-	if (is2t) {
-		_rtl92c_phy_path_a_standby(hw);
-		_rtl92c_phy_path_adda_on(hw, adda_reg, false, is2t);
-		for (i = 0; i < retrycount; i++) {
-			pathb_ok = _rtl92c_phy_path_b_iqk(hw);
-			if (pathb_ok == 0x03) {
-				result[t][4] = (rtl_get_bbreg(hw,
-						      0xeb4,
-						      MASKDWORD) &
-						0x3FF0000) >> 16;
-				result[t][5] =
-				    (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
-				     0x3FF0000) >> 16;
-				result[t][6] =
-				    (rtl_get_bbreg(hw, 0xec4, MASKDWORD) &
-				     0x3FF0000) >> 16;
-				result[t][7] =
-				    (rtl_get_bbreg(hw, 0xecc, MASKDWORD) &
-				     0x3FF0000) >> 16;
-				break;
-			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
-				result[t][4] = (rtl_get_bbreg(hw,
-						      0xeb4,
-						      MASKDWORD) &
-						0x3FF0000) >> 16;
-			}
-			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
-					0x3FF0000) >> 16;
-		}
-	}
-	rtl_set_bbreg(hw, 0xc04, MASKDWORD, rtlphy->reg_c04);
-	rtl_set_bbreg(hw, 0x874, MASKDWORD, rtlphy->reg_874);
-	rtl_set_bbreg(hw, 0xc08, MASKDWORD, rtlphy->reg_c08);
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
-	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00032ed3);
-	if (is2t)
-		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00032ed3);
-	if (t != 0) {
-		if (!rtlphy->b_rfpi_enable)
-			_rtl92c_phy_pi_mode_switch(hw, false);
-		_rtl92c_phy_reload_adda_registers(hw, adda_reg,
-						  rtlphy->adda_backup, 16);
-		_rtl92c_phy_reload_mac_registers(hw, iqk_mac_reg,
-						 rtlphy->iqk_mac_backup);
-	}
-}
-
-static void _rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+void _rtl92ce_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 {
 	u8 tmpreg;
 	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
@@ -1866,666 +477,6 @@ static void _rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 	}
 }
 
-static void _rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw,
-				     char delta, bool is2t)
-{
-	/* This routine is deliberately dummied out for later fixes */
-#if 0
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	u32 reg_d[PATH_NUM];
-	u32 tmpreg, index, offset, path, i, pathbound = PATH_NUM, apkbound;
-
-	u32 bb_backup[APK_BB_REG_NUM];
-	u32 bb_reg[APK_BB_REG_NUM] = {
-		0x904, 0xc04, 0x800, 0xc08, 0x874
-	};
-	u32 bb_ap_mode[APK_BB_REG_NUM] = {
-		0x00000020, 0x00a05430, 0x02040000,
-		0x000800e4, 0x00204000
-	};
-	u32 bb_normal_ap_mode[APK_BB_REG_NUM] = {
-		0x00000020, 0x00a05430, 0x02040000,
-		0x000800e4, 0x22204000
-	};
-
-	u32 afe_backup[APK_AFE_REG_NUM];
-	u32 afe_reg[APK_AFE_REG_NUM] = {
-		0x85c, 0xe6c, 0xe70, 0xe74, 0xe78,
-		0xe7c, 0xe80, 0xe84, 0xe88, 0xe8c,
-		0xed0, 0xed4, 0xed8, 0xedc, 0xee0,
-		0xeec
-	};
-
-	u32 mac_backup[IQK_MAC_REG_NUM];
-	u32 mac_reg[IQK_MAC_REG_NUM] = {
-		0x522, 0x550, 0x551, 0x040
-	};
-
-	u32 apk_rf_init_value[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x0852c, 0x1852c, 0x5852c, 0x1852c, 0x5852c},
-		{0x2852e, 0x0852e, 0x3852e, 0x0852e, 0x0852e}
-	};
-
-	u32 apk_normal_rf_init_value[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x0852c, 0x0a52c, 0x3a52c, 0x5a52c, 0x5a52c},
-		{0x0852c, 0x0a52c, 0x5a52c, 0x5a52c, 0x5a52c}
-	};
-
-	u32 apk_rf_value_0[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x52019, 0x52014, 0x52013, 0x5200f, 0x5208d},
-		{0x5201a, 0x52019, 0x52016, 0x52033, 0x52050}
-	};
-
-	u32 apk_normal_rf_value_0[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x52019, 0x52017, 0x52010, 0x5200d, 0x5206a},
-		{0x52019, 0x52017, 0x52010, 0x5200d, 0x5206a}
-	};
-
-	u32 afe_on_off[PATH_NUM] = {
-		0x04db25a4, 0x0b1b25a4
-	};
-
-	u32 apk_offset[PATH_NUM] = { 0xb68, 0xb6c };
-
-	u32 apk_normal_offset[PATH_NUM] = { 0xb28, 0xb98 };
-
-	u32 apk_value[PATH_NUM] = { 0x92fc0000, 0x12fc0000 };
-
-	u32 apk_normal_value[PATH_NUM] = { 0x92680000, 0x12680000 };
-
-	const char apk_delta_mapping[APK_BB_REG_NUM][13] = {
-		{-4, -3, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-4, -3, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-6, -4, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-1, -1, -1, -1, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-11, -9, -7, -5, -3, -1, 0, 0, 0, 0, 0, 0, 0}
-	};
-
-	const u32 apk_normal_setting_value_1[13] = {
-		0x01017018, 0xf7ed8f84, 0x1b1a1816, 0x2522201e, 0x322e2b28,
-		0x433f3a36, 0x5b544e49, 0x7b726a62, 0xa69a8f84, 0xdfcfc0b3,
-		0x12680000, 0x00880000, 0x00880000
-	};
-
-	const u32 apk_normal_setting_value_2[16] = {
-		0x01c7021d, 0x01670183, 0x01000123, 0x00bf00e2, 0x008d00a3,
-		0x0068007b, 0x004d0059, 0x003a0042, 0x002b0031, 0x001f0025,
-		0x0017001b, 0x00110014, 0x000c000f, 0x0009000b, 0x00070008,
-		0x00050006
-	};
-
-	const u32 apk_result[PATH_NUM][APK_BB_REG_NUM];
-
-	long bb_offset, delta_v, delta_offset;
-
-	if (!is2t)
-		pathbound = 1;
-
-	for (index = 0; index < PATH_NUM; index++) {
-		apk_offset[index] = apk_normal_offset[index];
-		apk_value[index] = apk_normal_value[index];
-		afe_on_off[index] = 0x6fdb25a4;
-	}
-
-	for (index = 0; index < APK_BB_REG_NUM; index++) {
-		for (path = 0; path < pathbound; path++) {
-			apk_rf_init_value[path][index] =
-			    apk_normal_rf_init_value[path][index];
-			apk_rf_value_0[path][index] =
-			    apk_normal_rf_value_0[path][index];
-		}
-		bb_ap_mode[index] = bb_normal_ap_mode[index];
-
-		apkbound = 6;
-	}
-
-	for (index = 0; index < APK_BB_REG_NUM; index++) {
-		if (index == 0)
-			continue;
-		bb_backup[index] = rtl_get_bbreg(hw, bb_reg[index], MASKDWORD);
-	}
-
-	_rtl92c_phy_save_mac_registers(hw, mac_reg, mac_backup);
-
-	_rtl92c_phy_save_adda_registers(hw, afe_reg, afe_backup, 16);
-
-	for (path = 0; path < pathbound; path++) {
-		if (path == RF90_PATH_A) {
-			offset = 0xb00;
-			for (index = 0; index < 11; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-
-			rtl_set_bbreg(hw, 0xb98, MASKDWORD, 0x12680000);
-
-			offset = 0xb68;
-			for (; index < 13; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x40000000);
-
-			offset = 0xb00;
-			for (index = 0; index < 16; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_2
-					      [index]);
-
-				offset += 0x04;
-			}
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
-		} else if (path == RF90_PATH_B) {
-			offset = 0xb70;
-			for (index = 0; index < 10; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-			rtl_set_bbreg(hw, 0xb28, MASKDWORD, 0x12680000);
-			rtl_set_bbreg(hw, 0xb98, MASKDWORD, 0x12680000);
-
-			offset = 0xb68;
-			index = 11;
-			for (; index < 13; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x40000000);
-
-			offset = 0xb60;
-			for (index = 0; index < 16; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_2
-					      [index]);
-
-				offset += 0x04;
-			}
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
-		}
-
-		reg_d[path] = rtl_get_rfreg(hw, (enum radio_path)path,
-					    0xd, MASKDWORD);
-
-		for (index = 0; index < APK_AFE_REG_NUM; index++)
-			rtl_set_bbreg(hw, afe_reg[index], MASKDWORD,
-				      afe_on_off[path]);
-
-		if (path == RF90_PATH_A) {
-			for (index = 0; index < APK_BB_REG_NUM; index++) {
-				if (index == 0)
-					continue;
-				rtl_set_bbreg(hw, bb_reg[index], MASKDWORD,
-					      bb_ap_mode[index]);
-			}
-		}
-
-		_rtl92c_phy_mac_setting_calibration(hw, mac_reg, mac_backup);
-
-		if (path == 0) {
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x0, MASKDWORD, 0x10000);
-		} else {
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASKDWORD,
-				      0x10000);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x10, MASKDWORD,
-				      0x1000f);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x11, MASKDWORD,
-				      0x20103);
-		}
-
-		delta_offset = ((delta + 14) / 2);
-		if (delta_offset < 0)
-			delta_offset = 0;
-		else if (delta_offset > 12)
-			delta_offset = 12;
-
-		for (index = 0; index < APK_BB_REG_NUM; index++) {
-			if (index != 1)
-				continue;
-
-			tmpreg = apk_rf_init_value[path][index];
-
-			if (!rtlefuse->b_apk_thermalmeterignore) {
-				bb_offset = (tmpreg & 0xF0000) >> 16;
-
-				if (!(tmpreg & BIT(15)))
-					bb_offset = -bb_offset;
-
-				delta_v =
-				    apk_delta_mapping[index][delta_offset];
-
-				bb_offset += delta_v;
-
-				if (bb_offset < 0) {
-					tmpreg = tmpreg & (~BIT(15));
-					bb_offset = -bb_offset;
-				} else {
-					tmpreg = tmpreg | BIT(15);
-				}
-
-				tmpreg =
-				    (tmpreg & 0xFFF0FFFF) | (bb_offset << 16);
-			}
-
-			rtl_set_rfreg(hw, (enum radio_path)path, 0xc,
-				      MASKDWORD, 0x8992e);
-			rtl_set_rfreg(hw, (enum radio_path)path, 0x0,
-				      MASKDWORD, apk_rf_value_0[path][index]);
-			rtl_set_rfreg(hw, (enum radio_path)path, 0xd,
-				      MASKDWORD, tmpreg);
-
-			i = 0;
-			do {
-				rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80000000);
-				rtl_set_bbreg(hw, apk_offset[path],
-					      MASKDWORD, apk_value[0]);
-				RTPRINT(rtlpriv, FINIT, INIT_IQK,
-					("PHY_APCalibrate() offset 0x%x "
-					 "value 0x%x\n",
-					 apk_offset[path],
-					 rtl_get_bbreg(hw, apk_offset[path],
-						       MASKDWORD)));
-
-				mdelay(3);
-
-				rtl_set_bbreg(hw, apk_offset[path],
-					      MASKDWORD, apk_value[1]);
-				RTPRINT(rtlpriv, FINIT, INIT_IQK,
-					("PHY_APCalibrate() offset 0x%x "
-					 "value 0x%x\n",
-					 apk_offset[path],
-					 rtl_get_bbreg(hw, apk_offset[path],
-						       MASKDWORD)));
-
-				mdelay(20);
-
-				rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
-
-				if (path == RF90_PATH_A)
-					tmpreg = rtl_get_bbreg(hw, 0xbd8,
-							       0x03E00000);
-				else
-					tmpreg = rtl_get_bbreg(hw, 0xbd8,
-							       0xF8000000);
-
-				RTPRINT(rtlpriv, FINIT, INIT_IQK,
-					("PHY_APCalibrate() offset "
-					 "0xbd8[25:21] %x\n", tmpreg));
-
-				i++;
-
-			} while (tmpreg > apkbound && i < 4);
-
-			apk_result[path][index] = tmpreg;
-		}
-	}
-
-	_rtl92c_phy_reload_mac_registers(hw, mac_reg, mac_backup);
-
-	for (index = 0; index < APK_BB_REG_NUM; index++) {
-		if (index == 0)
-			continue;
-		rtl_set_bbreg(hw, bb_reg[index], MASKDWORD, bb_backup[index]);
-	}
-
-	_rtl92c_phy_reload_adda_registers(hw, afe_reg, afe_backup, 16);
-
-	for (path = 0; path < pathbound; path++) {
-		rtl_set_rfreg(hw, (enum radio_path)path, 0xd,
-			      MASKDWORD, reg_d[path]);
-
-		if (path == RF90_PATH_B) {
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x10, MASKDWORD,
-				      0x1000f);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x11, MASKDWORD,
-				      0x20101);
-		}
-
-		if (apk_result[path][1] > 6)
-			apk_result[path][1] = 6;
-	}
-
-	for (path = 0; path < pathbound; path++) {
-		rtl_set_rfreg(hw, (enum radio_path)path, 0x3, MASKDWORD,
-			      ((apk_result[path][1] << 15) |
-			       (apk_result[path][1] << 10) |
-			       (apk_result[path][1] << 5) |
-			       apk_result[path][1]));
-
-		if (path == RF90_PATH_A)
-			rtl_set_rfreg(hw, (enum radio_path)path, 0x4, MASKDWORD,
-				      ((apk_result[path][1] << 15) |
-				       (apk_result[path][1] << 10) |
-				       (0x00 << 5) | 0x05));
-		else
-			rtl_set_rfreg(hw, (enum radio_path)path, 0x4, MASKDWORD,
-				      ((apk_result[path][1] << 15) |
-				       (apk_result[path][1] << 10) |
-				       (0x02 << 5) | 0x05));
-
-		rtl_set_rfreg(hw, (enum radio_path)path, 0xe, MASKDWORD,
-			      ((0x08 << 15) | (0x08 << 10) | (0x08 << 5) |
-			       0x08));
-
-	}
-
-	rtlphy->b_apk_done = true;
-#endif
-}
-
-static void _rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw,
-					  bool bmain, bool is2t)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (is_hal_stop(rtlhal)) {
-		rtl_set_bbreg(hw, REG_LEDCFG0, BIT(23), 0x01);
-		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
-	}
-	if (is2t) {
-		if (bmain)
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
-				      BIT(5) | BIT(6), 0x1);
-		else
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
-				      BIT(5) | BIT(6), 0x2);
-	} else {
-		if (bmain)
-			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x2);
-		else
-			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x1);
-
-	}
-}
-
-#undef IQK_ADDA_REG_NUM
-#undef IQK_DELAY_TIME
-
-void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	long result[4][8];
-	u8 i, final_candidate;
-	bool b_patha_ok, b_pathb_ok;
-	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
-	    reg_ecc, reg_tmp = 0;
-	bool is12simular, is13simular, is23simular;
-	bool b_start_conttx = false, b_singletone = false;
-	u32 iqk_bb_reg[10] = {
-		ROFDM0_XARXIQIMBALANCE,
-		ROFDM0_XBRXIQIMBALANCE,
-		ROFDM0_ECCATHRESHOLD,
-		ROFDM0_AGCRSSITABLE,
-		ROFDM0_XATXIQIMBALANCE,
-		ROFDM0_XBTXIQIMBALANCE,
-		ROFDM0_XCTXIQIMBALANCE,
-		ROFDM0_XCTXAFE,
-		ROFDM0_XDTXAFE,
-		ROFDM0_RXIQEXTANTA
-	};
-
-	if (b_recovery) {
-		_rtl92c_phy_reload_adda_registers(hw,
-						  iqk_bb_reg,
-						  rtlphy->iqk_bb_backup, 10);
-		return;
-	}
-	if (b_start_conttx || b_singletone)
-		return;
-	for (i = 0; i < 8; i++) {
-		result[0][i] = 0;
-		result[1][i] = 0;
-		result[2][i] = 0;
-		result[3][i] = 0;
-	}
-	final_candidate = 0xff;
-	b_patha_ok = false;
-	b_pathb_ok = false;
-	is12simular = false;
-	is23simular = false;
-	is13simular = false;
-	for (i = 0; i < 3; i++) {
-		if (IS_92C_SERIAL(rtlhal->version))
-			_rtl92c_phy_iq_calibrate(hw, result, i, true);
-		else
-			_rtl92c_phy_iq_calibrate(hw, result, i, false);
-		if (i == 1) {
-			is12simular = _rtl92c_phy_simularity_compare(hw,
-								     result, 0,
-								     1);
-			if (is12simular) {
-				final_candidate = 0;
-				break;
-			}
-		}
-		if (i == 2) {
-			is13simular = _rtl92c_phy_simularity_compare(hw,
-								     result, 0,
-								     2);
-			if (is13simular) {
-				final_candidate = 0;
-				break;
-			}
-			is23simular = _rtl92c_phy_simularity_compare(hw,
-								     result, 1,
-								     2);
-			if (is23simular)
-				final_candidate = 1;
-			else {
-				for (i = 0; i < 8; i++)
-					reg_tmp += result[3][i];
-
-				if (reg_tmp != 0)
-					final_candidate = 3;
-				else
-					final_candidate = 0xFF;
-			}
-		}
-	}
-	for (i = 0; i < 4; i++) {
-		reg_e94 = result[i][0];
-		reg_e9c = result[i][1];
-		reg_ea4 = result[i][2];
-		reg_eac = result[i][3];
-		reg_eb4 = result[i][4];
-		reg_ebc = result[i][5];
-		reg_ec4 = result[i][6];
-		reg_ecc = result[i][7];
-	}
-	if (final_candidate != 0xff) {
-		rtlphy->reg_e94 = reg_e94 = result[final_candidate][0];
-		rtlphy->reg_e9c = reg_e9c = result[final_candidate][1];
-		reg_ea4 = result[final_candidate][2];
-		reg_eac = result[final_candidate][3];
-		rtlphy->reg_eb4 = reg_eb4 = result[final_candidate][4];
-		rtlphy->reg_ebc = reg_ebc = result[final_candidate][5];
-		reg_ec4 = result[final_candidate][6];
-		reg_ecc = result[final_candidate][7];
-		b_patha_ok = b_pathb_ok = true;
-	} else {
-		rtlphy->reg_e94 = rtlphy->reg_eb4 = 0x100;
-		rtlphy->reg_e9c = rtlphy->reg_ebc = 0x0;
-	}
-	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
-		_rtl92c_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
-						   final_candidate,
-						   (reg_ea4 == 0));
-	if (IS_92C_SERIAL(rtlhal->version)) {
-		if (reg_eb4 != 0) /*&&(reg_ec4 != 0) */
-			_rtl92c_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok,
-							   result,
-							   final_candidate,
-							   (reg_ec4 == 0));
-	}
-	_rtl92c_phy_save_adda_registers(hw, iqk_bb_reg,
-					rtlphy->iqk_bb_backup, 10);
-}
-
-void rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_start_conttx = false, b_singletone = false;
-
-	if (b_start_conttx || b_singletone)
-		return;
-	if (IS_92C_SERIAL(rtlhal->version))
-		_rtl92c_phy_lc_calibrate(hw, true);
-	else
-		_rtl92c_phy_lc_calibrate(hw, false);
-}
-
-void rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (rtlphy->b_apk_done)
-		return;
-	if (IS_92C_SERIAL(rtlhal->version))
-		_rtl92c_phy_ap_calibrate(hw, delta, true);
-	else
-		_rtl92c_phy_ap_calibrate(hw, delta, false);
-}
-
-void rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (IS_92C_SERIAL(rtlhal->version))
-		_rtl92c_phy_set_rfpath_switch(hw, bmain, true);
-	else
-		_rtl92c_phy_set_rfpath_switch(hw, bmain, false);
-}
-
-bool rtl92c_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	bool b_postprocessing = false;
-
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 ("-->IO Cmd(%#x), set_io_inprogress(%d)\n",
-		  iotype, rtlphy->set_io_inprogress));
-	do {
-		switch (iotype) {
-		case IO_CMD_RESUME_DM_BY_SCAN:
-			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-				 ("[IO CMD] Resume DM after scan.\n"));
-			b_postprocessing = true;
-			break;
-		case IO_CMD_PAUSE_DM_BY_SCAN:
-			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-				 ("[IO CMD] Pause DM before scan.\n"));
-			b_postprocessing = true;
-			break;
-		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 ("switch case not process\n"));
-			break;
-		}
-	} while (false);
-	if (b_postprocessing && !rtlphy->set_io_inprogress) {
-		rtlphy->set_io_inprogress = true;
-		rtlphy->current_io_type = iotype;
-	} else {
-		return false;
-	}
-	rtl92c_phy_set_io(hw);
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, ("<--IO Type(%#x)\n", iotype));
-	return true;
-}
-
-void rtl92c_phy_set_io(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 ("--->Cmd(%#x), set_io_inprogress(%d)\n",
-		  rtlphy->current_io_type, rtlphy->set_io_inprogress));
-	switch (rtlphy->current_io_type) {
-	case IO_CMD_RESUME_DM_BY_SCAN:
-		dm_digtable.cur_igvalue = rtlphy->initgain_backup.xaagccore1;
-		rtl92c_dm_write_dig(hw);
-		rtl92c_phy_set_txpower_level(hw, rtlphy->current_channel);
-		break;
-	case IO_CMD_PAUSE_DM_BY_SCAN:
-		rtlphy->initgain_backup.xaagccore1 = dm_digtable.cur_igvalue;
-		dm_digtable.cur_igvalue = 0x17;
-		rtl92c_dm_write_dig(hw);
-		break;
-	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
-		break;
-	}
-	rtlphy->set_io_inprogress = false;
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 ("<---(%#x)\n", rtlphy->current_io_type));
-}
-
-void rtl92ce_phy_set_rf_on(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
-	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
-	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
-}
-
-static void _rtl92ce_phy_set_rf_sleep(struct ieee80211_hw *hw)
-{
-	u32 u4b_tmp;
-	u8 delay = 5;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
-	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
-	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
-	u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
-	while (u4b_tmp != 0 && delay > 0) {
-		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x0);
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
-		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
-		u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
-		delay--;
-	}
-	if (delay == 0) {
-		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
-		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
-		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
-			 ("Switch RF timeout !!!.\n"));
-		return;
-	}
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
-	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
-}
-
 static bool _rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 					    enum rf_pwrstate rfpwr_state)
 {
@@ -2648,7 +599,7 @@ static bool _rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 				  jiffies_to_msecs(jiffies -
 						   ppsc->last_awake_jiffies)));
 			ppsc->last_sleep_jiffies = jiffies;
-			_rtl92ce_phy_set_rf_sleep(hw);
+			_rtl92c_phy_set_rf_sleep(hw);
 			break;
 		}
 	default:
@@ -2663,7 +614,7 @@ static bool _rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 	return bresult;
 }
 
-bool rtl92c_phy_set_rf_power_state(struct ieee80211_hw *hw,
+bool rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 				   enum rf_pwrstate rfpwr_state)
 {
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h
index ca4daee6e9a8..a37267e3fc22 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h
@@ -57,8 +57,6 @@
 #define IQK_MAC_REG_NUM			4
 
 #define RF90_PATH_MAX			2
-#define CHANNEL_MAX_NUMBER		14
-#define CHANNEL_GROUP_MAX		3
 
 #define CT_OFFSET_MAC_ADDR		0X16
 
@@ -78,9 +76,7 @@
 #define CT_OFFSET_CUSTOMER_ID		0x7F
 
 #define RTL92C_MAX_PATH_NUM		2
-#define CHANNEL_MAX_NUMBER		14
-#define CHANNEL_GROUP_MAX		3
-
+#define LLT_LAST_ENTRY_OF_TX_PKT_BUFFER	255
 enum swchnlcmd_id {
 	CMDID_END,
 	CMDID_SET_TXPOWEROWER_LEVEL,
@@ -195,11 +191,11 @@ extern void rtl92c_phy_set_bb_reg(struct ieee80211_hw *hw,
 extern u32 rtl92c_phy_query_rf_reg(struct ieee80211_hw *hw,
 				   enum radio_path rfpath, u32 regaddr,
 				   u32 bitmask);
-extern void rtl92c_phy_set_rf_reg(struct ieee80211_hw *hw,
+extern void rtl92ce_phy_set_rf_reg(struct ieee80211_hw *hw,
 				  enum radio_path rfpath, u32 regaddr,
 				  u32 bitmask, u32 data);
 extern bool rtl92c_phy_mac_config(struct ieee80211_hw *hw);
-extern bool rtl92c_phy_bb_config(struct ieee80211_hw *hw);
+bool rtl92ce_phy_bb_config(struct ieee80211_hw *hw);
 extern bool rtl92c_phy_rf_config(struct ieee80211_hw *hw);
 extern bool rtl92c_phy_config_rf_with_feaderfile(struct ieee80211_hw *hw,
 						 enum radio_path rfpath);
@@ -227,11 +223,32 @@ bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 extern bool rtl8192_phy_check_is_legal_rfpath(struct ieee80211_hw *hw,
 					      u32 rfpath);
 bool rtl92c_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
-extern bool rtl92c_phy_set_rf_power_state(struct ieee80211_hw *hw,
+bool rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 					  enum rf_pwrstate rfpwr_state);
-void rtl92c_phy_config_bb_external_pa(struct ieee80211_hw *hw);
 void rtl92ce_phy_set_rf_on(struct ieee80211_hw *hw);
 bool rtl92c_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
 void rtl92c_phy_set_io(struct ieee80211_hw *hw);
+void rtl92c_bb_block_on(struct ieee80211_hw *hw);
+u32 _rtl92c_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset);
+u32 _rtl92c_phy_fw_rf_serial_read(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset);
+u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask);
+void _rtl92c_phy_rf_serial_write(struct ieee80211_hw *hw,
+					enum radio_path rfpath, u32 offset,
+					u32 data);
+void _rtl92c_store_pwrIndex_diffrate_offset(struct ieee80211_hw *hw,
+						   u32 regaddr, u32 bitmask,
+						   u32 data);
+void _rtl92c_phy_fw_rf_serial_write(struct ieee80211_hw *hw,
+					   enum radio_path rfpath, u32 offset,
+					   u32 data);
+void _rtl92c_store_pwrIndex_diffrate_offset(struct ieee80211_hw *hw,
+						   u32 regaddr, u32 bitmask,
+						   u32 data);
+bool _rtl92ce_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw);
+void _rtl92c_phy_set_rf_sleep(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h b/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
index 875d51465225..b0868a613841 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
@@ -63,7 +63,15 @@
 #define REG_LEDCFG3				0x004F
 #define REG_FSIMR				0x0050
 #define REG_FSISR				0x0054
-
+#define REG_HSIMR				0x0058
+#define REG_HSISR				0x005c
+
+/* RTL8723 WIFI/BT/GPS Multi-Function GPIO Pin Control. */
+#define REG_GPIO_PIN_CTRL_2			0x0060
+/* RTL8723 WIFI/BT/GPS Multi-Function GPIO Select. */
+#define REG_GPIO_IO_SEL_2			0x0062
+/* RTL8723 WIFI/BT/GPS Multi-Function control source. */
+#define REG_MULTI_FUNC_CTRL			0x0068
 #define REG_MCUFWDL				0x0080
 
 #define REG_HMEBOX_EXT_0			0x0088
@@ -79,6 +87,7 @@
 #define REG_PCIE_MIO_INTD			0x00E8
 #define REG_HPON_FSM				0x00EC
 #define REG_SYS_CFG				0x00F0
+#define REG_GPIO_OUTSTS				0x00F4	/* For RTL8723 only.*/
 
 #define REG_CR					0x0100
 #define REG_PBP					0x0104
@@ -209,6 +218,8 @@
 #define REG_RDG_PIFS				0x0513
 #define REG_SIFS_CTX				0x0514
 #define REG_SIFS_TRX				0x0516
+#define REG_SIFS_CCK				0x0514
+#define REG_SIFS_OFDM				0x0516
 #define REG_AGGR_BREAK_TIME			0x051A
 #define REG_SLOT				0x051B
 #define REG_TX_PTCL_CTRL			0x0520
@@ -261,6 +272,10 @@
 #define REG_MAC_SPEC_SIFS			0x063A
 #define REG_RESP_SIFS_CCK			0x063C
 #define REG_RESP_SIFS_OFDM			0x063E
+/* [15:8]SIFS_R2T_OFDM, [7:0]SIFS_R2T_CCK */
+#define REG_R2T_SIFS				0x063C
+/* [15:8]SIFS_T2T_OFDM, [7:0]SIFS_T2T_CCK */
+#define REG_T2T_SIFS				0x063E
 #define REG_ACKTO				0x0640
 #define REG_CTS2TO				0x0641
 #define REG_EIFS				0x0642
@@ -641,9 +656,10 @@
 #define	STOPBE					BIT(1)
 #define	STOPBK					BIT(0)
 
-#define	RCR_APPFCS				BIT(31)
+#define	RCR_APP_FCS				BIT(31)
 #define	RCR_APP_MIC				BIT(30)
 #define	RCR_APP_ICV				BIT(29)
+#define	RCR_APP_PHYSTS				BIT(28)
 #define	RCR_APP_PHYST_RXFF			BIT(28)
 #define	RCR_APP_BA_SSN				BIT(27)
 #define	RCR_ENMBID				BIT(24)
@@ -759,6 +775,7 @@
 
 #define	BOOT_FROM_EEPROM			BIT(4)
 #define	EEPROM_EN				BIT(5)
+#define	EEPROMSEL				BOOT_FROM_EEPROM
 
 #define AFE_BGEN				BIT(0)
 #define AFE_MBEN				BIT(1)
@@ -876,6 +893,8 @@
 #define BD_MAC2					BIT(9)
 #define BD_MAC1					BIT(10)
 #define IC_MACPHY_MODE				BIT(11)
+#define BT_FUNC					BIT(16)
+#define VENDOR_ID				BIT(19)
 #define PAD_HWPD_IDN				BIT(22)
 #define TRP_VAUX_EN				BIT(23)
 #define TRP_BT_EN				BIT(24)
@@ -883,6 +902,28 @@
 #define BD_HCI_SEL				BIT(26)
 #define TYPE_ID					BIT(27)
 
+/* REG_GPIO_OUTSTS (For RTL8723 only) */
+#define	EFS_HCI_SEL				(BIT(0)|BIT(1))
+#define	PAD_HCI_SEL				(BIT(2)|BIT(3))
+#define	HCI_SEL					(BIT(4)|BIT(5))
+#define	PKG_SEL_HCI				BIT(6)
+#define	FEN_GPS					BIT(7)
+#define	FEN_BT					BIT(8)
+#define	FEN_WL					BIT(9)
+#define	FEN_PCI					BIT(10)
+#define	FEN_USB					BIT(11)
+#define	BTRF_HWPDN_N				BIT(12)
+#define	WLRF_HWPDN_N				BIT(13)
+#define	PDN_BT_N				BIT(14)
+#define	PDN_GPS_N				BIT(15)
+#define	BT_CTL_HWPDN				BIT(16)
+#define	GPS_CTL_HWPDN				BIT(17)
+#define	PPHY_SUSB				BIT(20)
+#define	UPHY_SUSB				BIT(21)
+#define	PCI_SUSEN				BIT(22)
+#define	USB_SUSEN				BIT(23)
+#define	RF_RL_ID			(BIT(31) | BIT(30) | BIT(29) | BIT(28))
+
 #define CHIP_VER_RTL_MASK			0xF000
 #define CHIP_VER_RTL_SHIFT			12
 
@@ -1035,7 +1076,7 @@
 #define _RARF_RC7(x)				(((x) & 0x1F) << 16)
 #define _RARF_RC8(x)				(((x) & 0x1F) << 24)
 
-#define AC_PARAM_TXOP_LIMIT_OFFSET		16
+#define AC_PARAM_TXOP_OFFSET			16
 #define AC_PARAM_ECW_MAX_OFFSET			12
 #define AC_PARAM_ECW_MIN_OFFSET			8
 #define AC_PARAM_AIFS_OFFSET			0
@@ -1184,6 +1225,30 @@
 
 #define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
 
+/* REG_MULTI_FUNC_CTRL(For RTL8723 Only) */
+/* Enable GPIO[9] as WiFi HW PDn source */
+#define	WL_HWPDN_EN				BIT(0)
+/* WiFi HW PDn polarity control */
+#define	WL_HWPDN_SL				BIT(1)
+/* WiFi function enable */
+#define	WL_FUNC_EN				BIT(2)
+/* Enable GPIO[9] as WiFi RF HW PDn source */
+#define	WL_HWROF_EN				BIT(3)
+/* Enable GPIO[11] as BT HW PDn source */
+#define	BT_HWPDN_EN				BIT(16)
+/* BT HW PDn polarity control */
+#define	BT_HWPDN_SL				BIT(17)
+/* BT function enable */
+#define	BT_FUNC_EN				BIT(18)
+/* Enable GPIO[11] as BT/GPS RF HW PDn source */
+#define	BT_HWROF_EN				BIT(19)
+/* Enable GPIO[10] as GPS HW PDn source */
+#define	GPS_HWPDN_EN				BIT(20)
+/* GPS HW PDn polarity control */
+#define	GPS_HWPDN_SL				BIT(21)
+/* GPS function enable */
+#define	GPS_FUNC_EN				BIT(22)
+
 #define	RPMAC_RESET				0x100
 #define	RPMAC_TXSTART				0x104
 #define	RPMAC_TXLEGACYSIG			0x108
@@ -1496,7 +1561,7 @@
 #define	BTXHTSTBC				0x30
 #define	BTXHTADVANCECODING			0x40
 #define	BTXHTSHORTGI				0x80
-#define	BTXHTNUMBERHT_LT	F		0x300
+#define	BTXHTNUMBERHT_LTF			0x300
 #define	BTXHTCRC8				0x3fc00
 #define	BCOUNTERRESET				0x10000
 #define	BNUMOFOFDMTX				0xffff
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/rf.c b/drivers/net/wireless/rtlwifi/rtl8192ce/rf.c
index ffd8e04c4028..669b1168dbec 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/rf.c
@@ -61,7 +61,7 @@ void rtl92c_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 	}
 }
 
-void rtl92c_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+void rtl92ce_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 				       u8 *ppowerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -410,7 +410,7 @@ static void _rtl92c_write_ofdm_power_reg(struct ieee80211_hw *hw,
 	}
 }
 
-void rtl92c_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+void rtl92ce_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
 					u8 *ppowerlevel, u8 channel)
 {
 	u32 writeVal[2], powerBase0[2], powerBase1[2];
@@ -430,7 +430,7 @@ void rtl92c_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
 	}
 }
 
-bool rtl92c_phy_rf6052_config(struct ieee80211_hw *hw)
+bool rtl92ce_phy_rf6052_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -484,11 +484,11 @@ static bool _rtl92c_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 
 		switch (rfpath) {
 		case RF90_PATH_A:
-			rtstatus = rtl92c_phy_config_rf_with_headerfile(hw,
+			rtstatus = rtl92ce_phy_config_rf_with_headerfile(hw,
 					(enum radio_path) rfpath);
 			break;
 		case RF90_PATH_B:
-			rtstatus = rtl92c_phy_config_rf_with_headerfile(hw,
+			rtstatus = rtl92ce_phy_config_rf_with_headerfile(hw,
 					(enum radio_path) rfpath);
 			break;
 		case RF90_PATH_C:
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h b/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h
index d3014f99bb7b..3aa520c1c171 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/rf.h
@@ -40,5 +40,8 @@ extern void rtl92c_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 					      u8 *ppowerlevel);
 extern void rtl92c_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
 					       u8 *ppowerlevel, u8 channel);
-extern bool rtl92c_phy_rf6052_config(struct ieee80211_hw *hw);
+bool rtl92ce_phy_rf6052_config(struct ieee80211_hw *hw);
+bool rtl92ce_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
index b366e8862929..b1cc4d44f534 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
@@ -37,6 +37,7 @@
 #include "phy.h"
 #include "dm.h"
 #include "hw.h"
+#include "rf.h"
 #include "sw.h"
 #include "trx.h"
 #include "led.h"
@@ -46,13 +47,13 @@ int rtl92c_init_sw_vars(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	rtlpriv->dm.b_dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_initialgain_enable = 1;
 	rtlpriv->dm.dm_flag = 0;
-	rtlpriv->dm.b_disable_framebursting = 0;;
+	rtlpriv->dm.disable_framebursting = 0;
 	rtlpriv->dm.thermalvalue = 0;
 	rtlpci->transmit_config = CFENDFORM | BIT(12) | BIT(13);
 
-	rtlpci->receive_config = (RCR_APPFCS |
+	rtlpci->receive_config = (RCR_APP_FCS |
 				  RCR_AMF |
 				  RCR_ADF |
 				  RCR_APP_MIC |
@@ -122,7 +123,7 @@ static struct rtl_hal_ops rtl8192ce_hal_ops = {
 	.switch_channel = rtl92c_phy_sw_chnl,
 	.dm_watchdog = rtl92c_dm_watchdog,
 	.scan_operation_backup = rtl92c_phy_scan_operation_backup,
-	.set_rf_power_state = rtl92c_phy_set_rf_power_state,
+	.set_rf_power_state = rtl92ce_phy_set_rf_power_state,
 	.led_control = rtl92ce_led_control,
 	.set_desc = rtl92ce_set_desc,
 	.get_desc = rtl92ce_get_desc,
@@ -133,8 +134,17 @@ static struct rtl_hal_ops rtl8192ce_hal_ops = {
 	.deinit_sw_leds = rtl92ce_deinit_sw_leds,
 	.get_bbreg = rtl92c_phy_query_bb_reg,
 	.set_bbreg = rtl92c_phy_set_bb_reg,
-	.get_rfreg = rtl92c_phy_query_rf_reg,
-	.set_rfreg = rtl92c_phy_set_rf_reg,
+	.get_rfreg = rtl92ce_phy_query_rf_reg,
+	.set_rfreg = rtl92ce_phy_set_rf_reg,
+	.cmd_send_packet = _rtl92c_cmd_send_packet,
+	.phy_rf6052_config = rtl92ce_phy_rf6052_config,
+	.phy_rf6052_set_cck_txpower = rtl92ce_phy_rf6052_set_cck_txpower,
+	.phy_rf6052_set_ofdm_txpower = rtl92ce_phy_rf6052_set_ofdm_txpower,
+	.config_bb_with_headerfile = _rtl92ce_phy_config_bb_with_headerfile,
+	.config_bb_with_pgheaderfile = _rtl92ce_phy_config_bb_with_pgheaderfile,
+	.phy_lc_calibrate = _rtl92ce_phy_lc_calibrate,
+	.phy_set_bw_mode_callback = rtl92ce_phy_set_bw_mode_callback,
+	.dm_dynamic_txpower = rtl92ce_dm_dynamic_txpower,
 };
 
 static struct rtl_mod_params rtl92ce_mod_params = {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.h b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.h
index de1198c38d4e..36e657668c1e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.h
@@ -33,5 +33,19 @@
 int rtl92c_init_sw_vars(struct ieee80211_hw *hw);
 void rtl92c_deinit_sw_vars(struct ieee80211_hw *hw);
 void rtl92c_init_var_map(struct ieee80211_hw *hw);
+bool _rtl92c_cmd_send_packet(struct ieee80211_hw *hw,
+			     struct sk_buff *skb);
+void rtl92ce_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					u8 *ppowerlevel);
+void rtl92ce_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					 u8 *ppowerlevel, u8 channel);
+bool _rtl92ce_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						  u8 configtype);
+bool _rtl92ce_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						    u8 configtype);
+void _rtl92ce_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t);
+u32 rtl92ce_phy_query_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 regaddr, u32 bitmask);
+void rtl92ce_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index bf5852f2d634..aa2b5815600f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -36,7 +36,7 @@
 #include "trx.h"
 #include "led.h"
 
-static enum rtl_desc_qsel _rtl92ce_map_hwqueue_to_fwqueue(u16 fc,
+static enum rtl_desc_qsel _rtl92ce_map_hwqueue_to_fwqueue(__le16 fc,
 							  unsigned int
 							  skb_queue)
 {
@@ -245,24 +245,24 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 				       struct rtl_stats *pstats,
 				       struct rx_desc_92c *pdesc,
 				       struct rx_fwinfo_92c *p_drvinfo,
-				       bool bpacket_match_bssid,
-				       bool bpacket_toself,
-				       bool b_packet_beacon)
+				       bool packet_match_bssid,
+				       bool packet_toself,
+				       bool packet_beacon)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct phy_sts_cck_8192s_t *cck_buf;
 	s8 rx_pwr_all, rx_pwr[4];
-	u8 rf_rx_num, evm, pwdb_all;
+	u8 evm, pwdb_all, rf_rx_num = 0;
 	u8 i, max_spatial_stream;
-	u32 rssi, total_rssi;
+	u32 rssi, total_rssi = 0;
 	bool is_cck_rate;
 
 	is_cck_rate = RX_HAL_IS_CCK_RATE(pdesc);
-	pstats->b_packet_matchbssid = bpacket_match_bssid;
-	pstats->b_packet_toself = bpacket_toself;
-	pstats->b_is_cck = is_cck_rate;
-	pstats->b_packet_beacon = b_packet_beacon;
-	pstats->b_is_cck = is_cck_rate;
+	pstats->packet_matchbssid = packet_match_bssid;
+	pstats->packet_toself = packet_toself;
+	pstats->is_cck = is_cck_rate;
+	pstats->packet_beacon = packet_beacon;
+	pstats->is_cck = is_cck_rate;
 	pstats->rx_mimo_signalquality[0] = -1;
 	pstats->rx_mimo_signalquality[1] = -1;
 
@@ -315,7 +315,7 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 		pstats->rx_pwdb_all = pwdb_all;
 		pstats->recvsignalpower = rx_pwr_all;
 
-		if (bpacket_match_bssid) {
+		if (packet_match_bssid) {
 			u8 sq;
 			if (pstats->rx_pwdb_all > 40)
 				sq = 100;
@@ -334,10 +334,10 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 			pstats->rx_mimo_signalquality[1] = -1;
 		}
 	} else {
-		rtlpriv->dm.brfpath_rxenable[0] =
-		    rtlpriv->dm.brfpath_rxenable[1] = true;
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
 		for (i = RF90_PATH_A; i < RF90_PATH_MAX; i++) {
-			if (rtlpriv->dm.brfpath_rxenable[i])
+			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
 			rx_pwr[i] =
@@ -347,7 +347,7 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 			rtlpriv->stats.rx_snr_db[i] =
 			    (long)(p_drvinfo->rxsnr[i] / 2);
 
-			if (bpacket_match_bssid)
+			if (packet_match_bssid)
 				pstats->rx_mimo_signalstrength[i] = (u8) rssi;
 		}
 
@@ -366,7 +366,7 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 		for (i = 0; i < max_spatial_stream; i++) {
 			evm = _rtl92c_evm_db_to_percentage(p_drvinfo->rxevm[i]);
 
-			if (bpacket_match_bssid) {
+			if (packet_match_bssid) {
 				if (i == 0)
 					pstats->signalquality =
 					    (u8) (evm & 0xff);
@@ -393,7 +393,7 @@ static void _rtl92ce_process_ui_rssi(struct ieee80211_hw *hw,
 	u8 rfpath;
 	u32 last_rssi, tmpval;
 
-	if (pstats->b_packet_toself || pstats->b_packet_beacon) {
+	if (pstats->packet_toself || pstats->packet_beacon) {
 		rtlpriv->stats.rssi_calculate_cnt++;
 
 		if (rtlpriv->stats.ui_rssi.total_num++ >=
@@ -421,7 +421,7 @@ static void _rtl92ce_process_ui_rssi(struct ieee80211_hw *hw,
 		pstats->rssi = rtlpriv->stats.signal_strength;
 	}
 
-	if (!pstats->b_is_cck && pstats->b_packet_toself) {
+	if (!pstats->is_cck && pstats->packet_toself) {
 		for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
 		     rfpath++) {
 
@@ -463,7 +463,7 @@ static void _rtl92ce_update_rxsignalstatistics(struct ieee80211_hw *hw,
 					       struct rtl_stats *pstats)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int weighting;
+	int weighting = 0;
 
 	if (rtlpriv->stats.recv_signal_power == 0)
 		rtlpriv->stats.recv_signal_power = pstats->recvsignalpower;
@@ -493,7 +493,7 @@ static void _rtl92ce_process_pwdb(struct ieee80211_hw *hw,
 		    rtlpriv->dm.undecorated_smoothed_pwdb;
 	}
 
-	if (pstats->b_packet_toself || pstats->b_packet_beacon) {
+	if (pstats->packet_toself || pstats->packet_beacon) {
 		if (undecorated_smoothed_pwdb < 0)
 			undecorated_smoothed_pwdb = pstats->rx_pwdb_all;
 
@@ -525,7 +525,7 @@ static void _rtl92ce_process_ui_link_quality(struct ieee80211_hw *hw,
 	u32 last_evm, n_spatialstream, tmpval;
 
 	if (pstats->signalquality != 0) {
-		if (pstats->b_packet_toself || pstats->b_packet_beacon) {
+		if (pstats->packet_toself || pstats->packet_beacon) {
 
 			if (rtlpriv->stats.ui_link_quality.total_num++ >=
 			    PHY_LINKQUALITY_SLID_WIN_MAX) {
@@ -595,8 +595,8 @@ static void _rtl92ce_process_phyinfo(struct ieee80211_hw *hw,
 				     struct rtl_stats *pcurrent_stats)
 {
 
-	if (!pcurrent_stats->b_packet_matchbssid &&
-	    !pcurrent_stats->b_packet_beacon)
+	if (!pcurrent_stats->packet_matchbssid &&
+	    !pcurrent_stats->packet_beacon)
 		return;
 
 	_rtl92ce_process_ui_rssi(hw, pcurrent_stats);
@@ -617,34 +617,36 @@ static void _rtl92ce_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	u8 *tmp_buf;
 	u8 *praddr;
 	u8 *psaddr;
-	u16 fc, type;
-	bool b_packet_matchbssid, b_packet_toself, b_packet_beacon;
+	__le16 fc;
+	u16 type, c_fc;
+	bool packet_matchbssid, packet_toself, packet_beacon;
 
 	tmp_buf = skb->data + pstats->rx_drvinfo_size + pstats->rx_bufshift;
 
 	hdr = (struct ieee80211_hdr *)tmp_buf;
-	fc = le16_to_cpu(hdr->frame_control);
+	fc = hdr->frame_control;
+	c_fc = le16_to_cpu(fc);
 	type = WLAN_FC_GET_TYPE(fc);
 	praddr = hdr->addr1;
 	psaddr = hdr->addr2;
 
-	b_packet_matchbssid =
+	packet_matchbssid =
 	    ((IEEE80211_FTYPE_CTL != type) &&
 	     (!compare_ether_addr(mac->bssid,
-				  (fc & IEEE80211_FCTL_TODS) ?
-				  hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
+				  (c_fc & IEEE80211_FCTL_TODS) ?
+				  hdr->addr1 : (c_fc & IEEE80211_FCTL_FROMDS) ?
 				  hdr->addr2 : hdr->addr3)) &&
-	     (!pstats->b_hwerror) && (!pstats->b_crc) && (!pstats->b_icv));
+	     (!pstats->hwerror) && (!pstats->crc) && (!pstats->icv));
 
-	b_packet_toself = b_packet_matchbssid &&
+	packet_toself = packet_matchbssid &&
 	    (!compare_ether_addr(praddr, rtlefuse->dev_addr));
 
 	if (ieee80211_is_beacon(fc))
-		b_packet_beacon = true;
+		packet_beacon = true;
 
 	_rtl92ce_query_rxphystatus(hw, pstats, pdesc, p_drvinfo,
-				   b_packet_matchbssid, b_packet_toself,
-				   b_packet_beacon);
+				   packet_matchbssid, packet_toself,
+				   packet_beacon);
 
 	_rtl92ce_process_phyinfo(hw, tmp_buf, pstats);
 }
@@ -662,14 +664,14 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	stats->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
 	    RX_DRV_INFO_SIZE_UNIT;
 	stats->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
-	stats->b_icv = (u16) GET_RX_DESC_ICV(pdesc);
-	stats->b_crc = (u16) GET_RX_DESC_CRC32(pdesc);
-	stats->b_hwerror = (stats->b_crc | stats->b_icv);
+	stats->icv = (u16) GET_RX_DESC_ICV(pdesc);
+	stats->crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	stats->hwerror = (stats->crc | stats->icv);
 	stats->decrypted = !GET_RX_DESC_SWDEC(pdesc);
 	stats->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
-	stats->b_shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
-	stats->b_isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	stats->b_isampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
+	stats->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
+	stats->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	stats->isampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
 				   && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
@@ -689,7 +691,7 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	if (GET_RX_DESC_RXHT(pdesc))
 		rx_status->flag |= RX_FLAG_HT;
 
-	rx_status->flag |= RX_FLAG_TSFT;
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
 
 	if (stats->decrypted)
 		rx_status->flag |= RX_FLAG_DECRYPTED;
@@ -727,27 +729,24 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool b_defaultadapter = true;
-
-	struct ieee80211_sta *sta = ieee80211_find_sta(mac->vif, mac->bssid);
-
+	bool defaultadapter = true;
+	struct ieee80211_sta *sta;
 	u8 *pdesc = (u8 *) pdesc_tx;
 	struct rtl_tcb_desc tcb_desc;
 	u8 *qc = ieee80211_get_qos_ctl(hdr);
 	u8 tid = qc[0] & IEEE80211_QOS_CTL_TID_MASK;
 	u16 seq_number;
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 	u8 rate_flag = info->control.rates[0].flags;
 
 	enum rtl_desc_qsel fw_qsel =
-	    _rtl92ce_map_hwqueue_to_fwqueue(le16_to_cpu(hdr->frame_control),
-					    queue_index);
+	    _rtl92ce_map_hwqueue_to_fwqueue(fc, queue_index);
 
-	bool b_firstseg = ((hdr->seq_ctrl &
-			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+	bool firstseg = ((hdr->seq_ctrl &
+			  cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
 
-	bool b_lastseg = ((hdr->frame_control &
-			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+	bool lastseg = ((hdr->frame_control &
+			 cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
 
 	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
 					    skb->data, skb->len,
@@ -759,7 +758,7 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 
 	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_92c));
 
-	if (b_firstseg) {
+	if (firstseg) {
 		SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 
 		SET_TX_DESC_TX_RATE(pdesc, tcb_desc.hw_rate);
@@ -774,25 +773,25 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		SET_TX_DESC_SEQ(pdesc, seq_number);
 
-		SET_TX_DESC_RTS_ENABLE(pdesc, ((tcb_desc.b_rts_enable &&
+		SET_TX_DESC_RTS_ENABLE(pdesc, ((tcb_desc.rts_enable &&
 						!tcb_desc.
-						b_cts_enable) ? 1 : 0));
+						cts_enable) ? 1 : 0));
 		SET_TX_DESC_HW_RTS_ENABLE(pdesc,
-					  ((tcb_desc.b_rts_enable
-					    || tcb_desc.b_cts_enable) ? 1 : 0));
-		SET_TX_DESC_CTS2SELF(pdesc, ((tcb_desc.b_cts_enable) ? 1 : 0));
-		SET_TX_DESC_RTS_STBC(pdesc, ((tcb_desc.b_rts_stbc) ? 1 : 0));
+					  ((tcb_desc.rts_enable
+					    || tcb_desc.cts_enable) ? 1 : 0));
+		SET_TX_DESC_CTS2SELF(pdesc, ((tcb_desc.cts_enable) ? 1 : 0));
+		SET_TX_DESC_RTS_STBC(pdesc, ((tcb_desc.rts_stbc) ? 1 : 0));
 
 		SET_TX_DESC_RTS_RATE(pdesc, tcb_desc.rts_rate);
 		SET_TX_DESC_RTS_BW(pdesc, 0);
 		SET_TX_DESC_RTS_SC(pdesc, tcb_desc.rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc,
 				      ((tcb_desc.rts_rate <= DESC92C_RATE54M) ?
-				      (tcb_desc.b_rts_use_shortpreamble ? 1 : 0)
-				      : (tcb_desc.b_rts_use_shortgi ? 1 : 0)));
+				      (tcb_desc.rts_use_shortpreamble ? 1 : 0)
+				      : (tcb_desc.rts_use_shortgi ? 1 : 0)));
 
 		if (mac->bw_40) {
-			if (tcb_desc.b_packet_bw) {
+			if (tcb_desc.packet_bw) {
 				SET_TX_DESC_DATA_BW(pdesc, 1);
 				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
 			} else {
@@ -811,10 +810,13 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_LINIP(pdesc, 0);
 		SET_TX_DESC_PKT_SIZE(pdesc, (u16) skb->len);
 
+		rcu_read_lock();
+		sta = ieee80211_find_sta(mac->vif, mac->bssid);
 		if (sta) {
 			u8 ampdu_density = sta->ht_cap.ampdu_density;
 			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
 		}
+		rcu_read_unlock();
 
 		if (info->control.hw_key) {
 			struct ieee80211_key_conf *keyconf =
@@ -854,14 +856,14 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 	}
 
-	SET_TX_DESC_FIRST_SEG(pdesc, (b_firstseg ? 1 : 0));
-	SET_TX_DESC_LAST_SEG(pdesc, (b_lastseg ? 1 : 0));
+	SET_TX_DESC_FIRST_SEG(pdesc, (firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
 
 	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) skb->len);
 
 	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, cpu_to_le32(mapping));
 
-	if (rtlpriv->dm.b_useramask) {
+	if (rtlpriv->dm.useramask) {
 		SET_TX_DESC_RATE_ID(pdesc, tcb_desc.ratr_index);
 		SET_TX_DESC_MACID(pdesc, tcb_desc.mac_id);
 	} else {
@@ -869,16 +871,16 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_MACID(pdesc, tcb_desc.ratr_index);
 	}
 
-	if ((!ieee80211_is_data_qos(fc)) && ppsc->b_leisure_ps &&
-	    ppsc->b_fwctrl_lps) {
+	if ((!ieee80211_is_data_qos(fc)) && ppsc->leisure_ps &&
+	    ppsc->fwctrl_lps) {
 		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
 		SET_TX_DESC_PKT_ID(pdesc, 8);
 
-		if (!b_defaultadapter)
+		if (!defaultadapter)
 			SET_TX_DESC_QOS(pdesc, 1);
 	}
 
-	SET_TX_DESC_MORE_FRAG(pdesc, (b_lastseg ? 0 : 1));
+	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
 
 	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
 	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
@@ -889,8 +891,8 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 }
 
 void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw,
-			     u8 *pdesc, bool b_firstseg,
-			     bool b_lastseg, struct sk_buff *skb)
+			     u8 *pdesc, bool firstseg,
+			     bool lastseg, struct sk_buff *skb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -901,11 +903,11 @@ void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw,
 					    PCI_DMA_TODEVICE);
 
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 
 	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE);
 
-	if (b_firstseg)
+	if (firstseg)
 		SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 
 	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
@@ -1029,3 +1031,36 @@ void rtl92ce_tx_polling(struct ieee80211_hw *hw, unsigned int hw_queue)
 			       BIT(0) << (hw_queue));
 	}
 }
+
+bool _rtl92c_cmd_send_packet(struct ieee80211_hw *hw,
+			     struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	u8 own;
+	unsigned long flags;
+	struct sk_buff *pskb = NULL;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	pdesc = &ring->desc[0];
+	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc, true, HW_DESC_OWN);
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+
+	return true;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
index 53d0e0a5af5c..803adcc80c96 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
@@ -40,470 +40,494 @@
 #define USB_HWDESC_HEADER_LEN			32
 #define CRCLENGTH				4
 
+/* Define a macro that takes a le32 word, converts it to host ordering,
+ * right shifts by a specified count, creates a mask of the specified
+ * bit count, and extracts that number of bits.
+ */
+
+#define SHIFT_AND_MASK_LE(__pdesc, __shift, __mask)		\
+	((le32_to_cpu(*(((__le32 *)(__pdesc)))) >> (__shift)) &	\
+	BIT_LEN_MASK_32(__mask))
+
+/* Define a macro that clears a bit field in an le32 word and
+ * sets the specified value into that bit field. The resulting
+ * value remains in le32 ordering; however, it is properly converted
+ * to host ordering for the clear and set operations before conversion
+ * back to le32.
+ */
+
+#define SET_BITS_OFFSET_LE(__pdesc, __shift, __len, __val)	\
+	(*(__le32 *)(__pdesc) = 				\
+	(cpu_to_le32((le32_to_cpu(*((__le32 *)(__pdesc))) &	\
+	(~(BIT_OFFSET_LEN_MASK_32((__shift), __len)))) |		\
+	(((u32)(__val) & BIT_LEN_MASK_32(__len)) << (__shift)))));
+
+/* macros to read/write various fields in RX or TX descriptors */
+
 #define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 0, 16, __val)
 #define SET_TX_DESC_OFFSET(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 16, 8, __val)
 #define SET_TX_DESC_BMC(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 24, 1, __val)
 #define SET_TX_DESC_HTC(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 25, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 25, 1, __val)
 #define SET_TX_DESC_LAST_SEG(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 26, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 26, 1, __val)
 #define SET_TX_DESC_FIRST_SEG(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 27, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 27, 1, __val)
 #define SET_TX_DESC_LINIP(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 28, 1, __val)
 #define SET_TX_DESC_NO_ACM(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 29, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 29, 1, __val)
 #define SET_TX_DESC_GF(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 30, 1, __val)
 #define SET_TX_DESC_OWN(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 31, 1, __val)
 
 #define GET_TX_DESC_PKT_SIZE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 0, 16)
+	SHIFT_AND_MASK_LE(__pdesc, 0, 16)
 #define GET_TX_DESC_OFFSET(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 16, 8)
+	SHIFT_AND_MASK_LE(__pdesc, 16, 8)
 #define GET_TX_DESC_BMC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 24, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 24, 1)
 #define GET_TX_DESC_HTC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 25, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 25, 1)
 #define GET_TX_DESC_LAST_SEG(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 26, 1)
 #define GET_TX_DESC_FIRST_SEG(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 27, 1)
 #define GET_TX_DESC_LINIP(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 28, 1)
 #define GET_TX_DESC_NO_ACM(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 29, 1)
 #define GET_TX_DESC_GF(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 30, 1)
 #define GET_TX_DESC_OWN(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 31, 1)
 
 #define SET_TX_DESC_MACID(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 0, 5, __val)
 #define SET_TX_DESC_AGG_BREAK(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 5, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 5, 1, __val)
 #define SET_TX_DESC_BK(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 6, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 6, 1, __val)
 #define SET_TX_DESC_RDG_ENABLE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 7, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 7, 1, __val)
 #define SET_TX_DESC_QUEUE_SEL(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 8, 5, __val)
 #define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 13, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 13, 1, __val)
 #define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 14, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 14, 1, __val)
 #define SET_TX_DESC_PIFS(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 15, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 15, 1, __val)
 #define SET_TX_DESC_RATE_ID(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 16, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 16, 4, __val)
 #define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 20, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 20, 1, __val)
 #define SET_TX_DESC_EN_DESC_ID(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 21, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 21, 1, __val)
 #define SET_TX_DESC_SEC_TYPE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 22, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 22, 2, __val)
 #define SET_TX_DESC_PKT_OFFSET(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+4, 24, 8, __val)
+	SET_BITS_OFFSET_LE(__pdesc+4, 24, 8, __val)
 
 #define GET_TX_DESC_MACID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
+	SHIFT_AND_MASK_LE(__pdesc+4, 0, 5)
 #define GET_TX_DESC_AGG_ENABLE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 5, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 5, 1)
 #define GET_TX_DESC_AGG_BREAK(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 6, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 6, 1)
 #define GET_TX_DESC_RDG_ENABLE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 7, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 7, 1)
 #define GET_TX_DESC_QUEUE_SEL(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 8, 5)
+	SHIFT_AND_MASK_LE(__pdesc+4, 8, 5)
 #define GET_TX_DESC_RDG_NAV_EXT(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 13, 1)
 #define GET_TX_DESC_LSIG_TXOP_EN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 14, 1)
 #define GET_TX_DESC_PIFS(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 15, 1)
 #define GET_TX_DESC_RATE_ID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+	SHIFT_AND_MASK_LE(__pdesc+4, 16, 4)
 #define GET_TX_DESC_NAV_USE_HDR(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 20, 1)
 #define GET_TX_DESC_EN_DESC_ID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 21, 1)
 #define GET_TX_DESC_SEC_TYPE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 22, 2)
+	SHIFT_AND_MASK_LE(__pdesc+4, 22, 2)
 #define GET_TX_DESC_PKT_OFFSET(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 24, 8)
+	SHIFT_AND_MASK_LE(__pdesc+4, 24, 8)
 
 #define SET_TX_DESC_RTS_RC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 6, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 0, 6, __val)
 #define SET_TX_DESC_DATA_RC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 6, 6, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 6, 6, __val)
 #define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 14, 2, __val)
 #define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 17, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 17, 1, __val)
 #define SET_TX_DESC_RAW(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 18, 1, __val)
 #define SET_TX_DESC_CCX(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 19, 1, __val)
 #define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 20, 3, __val)
 #define SET_TX_DESC_ANTSEL_A(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 24, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 24, 1, __val)
 #define SET_TX_DESC_ANTSEL_B(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 25, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 25, 1, __val)
 #define SET_TX_DESC_TX_ANT_CCK(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 26, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 26, 2, __val)
 #define SET_TX_DESC_TX_ANTL(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 28, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 28, 2, __val)
 #define SET_TX_DESC_TX_ANT_HT(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 30, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+8, 30, 2, __val)
 
 #define GET_TX_DESC_RTS_RC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 0, 6)
+	SHIFT_AND_MASK_LE(__pdesc+8, 0, 6)
 #define GET_TX_DESC_DATA_RC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 6, 6)
+	SHIFT_AND_MASK_LE(__pdesc+8, 6, 6)
 #define GET_TX_DESC_BAR_RTY_TH(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 14, 2)
+	SHIFT_AND_MASK_LE(__pdesc+8, 14, 2)
 #define GET_TX_DESC_MORE_FRAG(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 17, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 17, 1)
 #define GET_TX_DESC_RAW(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 18, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 18, 1)
 #define GET_TX_DESC_CCX(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 19, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 19, 1)
 #define GET_TX_DESC_AMPDU_DENSITY(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+8, 20, 3)
+	SHIFT_AND_MASK_LE(__pdesc+8, 20, 3)
 #define GET_TX_DESC_ANTSEL_A(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 24, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 24, 1)
 #define GET_TX_DESC_ANTSEL_B(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 25, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 25, 1)
 #define GET_TX_DESC_TX_ANT_CCK(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 26, 2)
+	SHIFT_AND_MASK_LE(__pdesc+8, 26, 2)
 #define GET_TX_DESC_TX_ANTL(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 28, 2)
+	SHIFT_AND_MASK_LE(__pdesc+8, 28, 2)
 #define GET_TX_DESC_TX_ANT_HT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 30, 2)
+	SHIFT_AND_MASK_LE(__pdesc+8, 30, 2)
 
 #define SET_TX_DESC_NEXT_HEAP_PAGE(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 8, __val)
+	SET_BITS_OFFSET_LE(__pdesc+12, 0, 8, __val)
 #define SET_TX_DESC_TAIL_PAGE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+12, 8, 8, __val)
+	SET_BITS_OFFSET_LE(__pdesc+12, 8, 8, __val)
 #define SET_TX_DESC_SEQ(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+12, 16, 12, __val)
+	SET_BITS_OFFSET_LE(__pdesc+12, 16, 12, __val)
 #define SET_TX_DESC_PKT_ID(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+12, 28, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+12, 28, 4, __val)
 
 #define GET_TX_DESC_NEXT_HEAP_PAGE(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+12, 0, 8)
+	SHIFT_AND_MASK_LE(__pdesc+12, 0, 8)
 #define GET_TX_DESC_TAIL_PAGE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 8, 8)
+	SHIFT_AND_MASK_LE(__pdesc+12, 8, 8)
 #define GET_TX_DESC_SEQ(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 16, 12)
+	SHIFT_AND_MASK_LE(__pdesc+12, 16, 12)
 #define GET_TX_DESC_PKT_ID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 28, 4)
+	SHIFT_AND_MASK_LE(__pdesc+12, 28, 4)
 
 #define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 0, 5, __val)
 #define SET_TX_DESC_AP_DCFE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 5, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 5, 1, __val)
 #define SET_TX_DESC_QOS(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 6, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 6, 1, __val)
 #define SET_TX_DESC_HWSEQ_EN(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 7, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 7, 1, __val)
 #define SET_TX_DESC_USE_RATE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 8, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 8, 1, __val)
 #define SET_TX_DESC_DISABLE_RTS_FB(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 9, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 9, 1, __val)
 #define SET_TX_DESC_DISABLE_FB(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 10, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 10, 1, __val)
 #define SET_TX_DESC_CTS2SELF(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 11, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 11, 1, __val)
 #define SET_TX_DESC_RTS_ENABLE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 12, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 12, 1, __val)
 #define SET_TX_DESC_HW_RTS_ENABLE(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 13, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 13, 1, __val)
 #define SET_TX_DESC_PORT_ID(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 14, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 14, 1, __val)
 #define SET_TX_DESC_WAIT_DCTS(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 18, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 18, 1, __val)
 #define SET_TX_DESC_CTS2AP_EN(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 19, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 19, 1, __val)
 #define SET_TX_DESC_TX_SUB_CARRIER(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 20, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 20, 2, __val)
 #define SET_TX_DESC_TX_STBC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 22, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 22, 2, __val)
 #define SET_TX_DESC_DATA_SHORT(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 24, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 24, 1, __val)
 #define SET_TX_DESC_DATA_BW(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 25, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 25, 1, __val)
 #define SET_TX_DESC_RTS_SHORT(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 26, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 26, 1, __val)
 #define SET_TX_DESC_RTS_BW(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 27, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 27, 1, __val)
 #define SET_TX_DESC_RTS_SC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 28, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 28, 2, __val)
 #define SET_TX_DESC_RTS_STBC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+16, 30, 2, __val)
+	SET_BITS_OFFSET_LE(__pdesc+16, 30, 2, __val)
 
 #define GET_TX_DESC_RTS_RATE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 0, 5)
+	SHIFT_AND_MASK_LE(__pdesc+16, 0, 5)
 #define GET_TX_DESC_AP_DCFE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 5, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 5, 1)
 #define GET_TX_DESC_QOS(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 6, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 6, 1)
 #define GET_TX_DESC_HWSEQ_EN(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 7, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 7, 1)
 #define GET_TX_DESC_USE_RATE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 8, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 8, 1)
 #define GET_TX_DESC_DISABLE_RTS_FB(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+16, 9, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 9, 1)
 #define GET_TX_DESC_DISABLE_FB(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 10, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 10, 1)
 #define GET_TX_DESC_CTS2SELF(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 11, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 11, 1)
 #define GET_TX_DESC_RTS_ENABLE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 12, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 12, 1)
 #define GET_TX_DESC_HW_RTS_ENABLE(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+16, 13, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 13, 1)
 #define GET_TX_DESC_PORT_ID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 14, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 14, 1)
 #define GET_TX_DESC_WAIT_DCTS(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 18, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 18, 1)
 #define GET_TX_DESC_CTS2AP_EN(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 19, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 19, 1)
 #define GET_TX_DESC_TX_SUB_CARRIER(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+16, 20, 2)
+	SHIFT_AND_MASK_LE(__pdesc+16, 20, 2)
 #define GET_TX_DESC_TX_STBC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 22, 2)
+	SHIFT_AND_MASK_LE(__pdesc+16, 22, 2)
 #define GET_TX_DESC_DATA_SHORT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 24, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 24, 1)
 #define GET_TX_DESC_DATA_BW(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 25, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 25, 1)
 #define GET_TX_DESC_RTS_SHORT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 26, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 26, 1)
 #define GET_TX_DESC_RTS_BW(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 27, 1)
+	SHIFT_AND_MASK_LE(__pdesc+16, 27, 1)
 #define GET_TX_DESC_RTS_SC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 28, 2)
+	SHIFT_AND_MASK_LE(__pdesc+16, 28, 2)
 #define GET_TX_DESC_RTS_STBC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 30, 2)
+	SHIFT_AND_MASK_LE(__pdesc+16, 30, 2)
 
 #define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 6, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 0, 6, __val)
 #define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 6, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 6, 1, __val)
 #define SET_TX_DESC_CCX_TAG(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 7, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 7, 1, __val)
 #define SET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 8, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 8, 5, __val)
 #define SET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 13, 4, __val)
 #define SET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 17, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 17, 1, __val)
 #define SET_TX_DESC_DATA_RETRY_LIMIT(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 18, 6, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 18, 6, __val)
 #define SET_TX_DESC_USB_TXAGG_NUM(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 8, __val)
+	SET_BITS_OFFSET_LE(__pdesc+20, 24, 8, __val)
 
 #define GET_TX_DESC_TX_RATE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+20, 0, 6)
+	SHIFT_AND_MASK_LE(__pdesc+20, 0, 6)
 #define GET_TX_DESC_DATA_SHORTGI(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+20, 6, 1)
+	SHIFT_AND_MASK_LE(__pdesc+20, 6, 1)
 #define GET_TX_DESC_CCX_TAG(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+20, 7, 1)
+	SHIFT_AND_MASK_LE(__pdesc+20, 7, 1)
 #define GET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+20, 8, 5)
+	SHIFT_AND_MASK_LE(__pdesc+20, 8, 5)
 #define GET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+20, 13, 4)
+	SHIFT_AND_MASK_LE(__pdesc+20, 13, 4)
 #define GET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+20, 17, 1)
+	SHIFT_AND_MASK_LE(__pdesc+20, 17, 1)
 #define GET_TX_DESC_DATA_RETRY_LIMIT(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+20, 18, 6)
+	SHIFT_AND_MASK_LE(__pdesc+20, 18, 6)
 #define GET_TX_DESC_USB_TXAGG_NUM(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+20, 24, 8)
+	SHIFT_AND_MASK_LE(__pdesc+20, 24, 8)
 
 #define SET_TX_DESC_TXAGC_A(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 0, 5, __val)
 #define SET_TX_DESC_TXAGC_B(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 5, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 5, 5, __val)
 #define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 10, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 10, 1, __val)
 #define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 11, 5, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 11, 5, __val)
 #define SET_TX_DESC_MCSG1_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 16, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 16, 4, __val)
 #define SET_TX_DESC_MCSG2_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 20, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 20, 4, __val)
 #define SET_TX_DESC_MCSG3_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 24, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 24, 4, __val)
 #define SET_TX_DESC_MCS7_SGI_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 28, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 28, 4, __val)
 
 #define GET_TX_DESC_TXAGC_A(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+24, 0, 5)
+	SHIFT_AND_MASK_LE(__pdesc+24, 0, 5)
 #define GET_TX_DESC_TXAGC_B(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+24, 5, 5)
+	SHIFT_AND_MASK_LE(__pdesc+24, 5, 5)
 #define GET_TX_DESC_USE_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+24, 10, 1)
+	SHIFT_AND_MASK_LE(__pdesc+24, 10, 1)
 #define GET_TX_DESC_MAX_AGG_NUM(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+24, 11, 5)
+	SHIFT_AND_MASK_LE(__pdesc+24, 11, 5)
 #define GET_TX_DESC_MCSG1_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+24, 16, 4)
+	SHIFT_AND_MASK_LE(__pdesc+24, 16, 4)
 #define GET_TX_DESC_MCSG2_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+24, 20, 4)
+	SHIFT_AND_MASK_LE(__pdesc+24, 20, 4)
 #define GET_TX_DESC_MCSG3_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+24, 24, 4)
+	SHIFT_AND_MASK_LE(__pdesc+24, 24, 4)
 #define GET_TX_DESC_MCS7_SGI_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+24, 28, 4)
+	SHIFT_AND_MASK_LE(__pdesc+24, 28, 4)
 
 #define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+	SET_BITS_OFFSET_LE(__pdesc+28, 0, 16, __val)
 #define SET_TX_DESC_MCSG4_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 16, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+28, 16, 4, __val)
 #define SET_TX_DESC_MCSG5_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 20, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+28, 20, 4, __val)
 #define SET_TX_DESC_MCSG6_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 24, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+28, 24, 4, __val)
 #define SET_TX_DESC_MCS15_SGI_MAX_LEN(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 28, 4, __val)
+	SET_BITS_OFFSET_LE(__pdesc+28, 28, 4, __val)
 
 #define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
+	SHIFT_AND_MASK_LE(__pdesc+28, 0, 16)
 #define GET_TX_DESC_MCSG4_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+28, 16, 4)
+	SHIFT_AND_MASK_LE(__pdesc+28, 16, 4)
 #define GET_TX_DESC_MCSG5_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+28, 20, 4)
+	SHIFT_AND_MASK_LE(__pdesc+28, 20, 4)
 #define GET_TX_DESC_MCSG6_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+28, 24, 4)
+	SHIFT_AND_MASK_LE(__pdesc+28, 24, 4)
 #define GET_TX_DESC_MCS15_SGI_MAX_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+28, 28, 4)
+	SHIFT_AND_MASK_LE(__pdesc+28, 28, 4)
 
 #define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+32, 0, 32, __val)
+	SET_BITS_OFFSET_LE(__pdesc+32, 0, 32, __val)
 #define SET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc, __val) \
-	SET_BITS_TO_LE_4BYTE(__pdesc+36, 0, 32, __val)
+	SET_BITS_OFFSET_LE(__pdesc+36, 0, 32, __val)
 
 #define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+32, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+32, 0, 32)
 #define GET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc)	\
-	LE_BITS_TO_4BYTE(__pdesc+36, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+36, 0, 32)
 
 #define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
+	SET_BITS_OFFSET_LE(__pdesc+40, 0, 32, __val)
 #define SET_TX_DESC_NEXT_DESC_ADDRESS64(__pdesc, __val) \
-	SET_BITS_TO_LE_4BYTE(__pdesc+44, 0, 32, __val)
+	SET_BITS_OFFSET_LE(__pdesc+44, 0, 32, __val)
 
 #define GET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+40, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+40, 0, 32)
 #define GET_TX_DESC_NEXT_DESC_ADDRESS64(__pdesc)	\
-	LE_BITS_TO_4BYTE(__pdesc+44, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+44, 0, 32)
 
 #define GET_RX_DESC_PKT_LEN(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
+	SHIFT_AND_MASK_LE(__pdesc, 0, 14)
 #define GET_RX_DESC_CRC32(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 14, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 14, 1)
 #define GET_RX_DESC_ICV(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 15, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 15, 1)
 #define GET_RX_DESC_DRV_INFO_SIZE(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc, 16, 4)
+	SHIFT_AND_MASK_LE(__pdesc, 16, 4)
 #define GET_RX_DESC_SECURITY(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 20, 3)
+	SHIFT_AND_MASK_LE(__pdesc, 20, 3)
 #define GET_RX_DESC_QOS(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 23, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 23, 1)
 #define GET_RX_DESC_SHIFT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 24, 2)
+	SHIFT_AND_MASK_LE(__pdesc, 24, 2)
 #define GET_RX_DESC_PHYST(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 26, 1)
 #define GET_RX_DESC_SWDEC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 27, 1)
 #define GET_RX_DESC_LS(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 28, 1)
 #define GET_RX_DESC_FS(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 29, 1)
 #define GET_RX_DESC_EOR(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 30, 1)
 #define GET_RX_DESC_OWN(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+	SHIFT_AND_MASK_LE(__pdesc, 31, 1)
 
 #define SET_RX_DESC_PKT_LEN(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 0, 14, __val)
 #define SET_RX_DESC_EOR(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 30, 1, __val)
 #define SET_RX_DESC_OWN(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+	SET_BITS_OFFSET_LE(__pdesc, 31, 1, __val)
 
 #define GET_RX_DESC_MACID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
+	SHIFT_AND_MASK_LE(__pdesc+4, 0, 5)
 #define GET_RX_DESC_TID(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 5, 4)
+	SHIFT_AND_MASK_LE(__pdesc+4, 5, 4)
 #define GET_RX_DESC_HWRSVD(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 9, 5)
+	SHIFT_AND_MASK_LE(__pdesc+4, 9, 5)
 #define GET_RX_DESC_PAGGR(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 14, 1)
 #define GET_RX_DESC_FAGGR(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 15, 1)
 #define GET_RX_DESC_A1_FIT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+	SHIFT_AND_MASK_LE(__pdesc+4, 16, 4)
 #define GET_RX_DESC_A2_FIT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 20, 4)
+	SHIFT_AND_MASK_LE(__pdesc+4, 20, 4)
 #define GET_RX_DESC_PAM(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 24, 1)
 #define GET_RX_DESC_PWR(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 25, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 25, 1)
 #define GET_RX_DESC_MD(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc+4, 26, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 26, 1)
 #define GET_RX_DESC_MF(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc+4, 27, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 27, 1)
 #define GET_RX_DESC_TYPE(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+4, 28, 2)
+	SHIFT_AND_MASK_LE(__pdesc+4, 28, 2)
 #define GET_RX_DESC_MC(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc+4, 30, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 30, 1)
 #define GET_RX_DESC_BC(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc+4, 31, 1)
+	SHIFT_AND_MASK_LE(__pdesc+4, 31, 1)
 #define GET_RX_DESC_SEQ(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 0, 12)
+	SHIFT_AND_MASK_LE(__pdesc+8, 0, 12)
 #define GET_RX_DESC_FRAG(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 12, 4)
+	SHIFT_AND_MASK_LE(__pdesc+8, 12, 4)
 #define GET_RX_DESC_NEXT_PKT_LEN(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+8, 16, 14)
+	SHIFT_AND_MASK_LE(__pdesc+8, 16, 14)
 #define GET_RX_DESC_NEXT_IND(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 30, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 30, 1)
 #define GET_RX_DESC_RSVD(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+8, 31, 1)
+	SHIFT_AND_MASK_LE(__pdesc+8, 31, 1)
 
 #define GET_RX_DESC_RXMCS(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 0, 6)
+	SHIFT_AND_MASK_LE(__pdesc+12, 0, 6)
 #define GET_RX_DESC_RXHT(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 6, 1)
+	SHIFT_AND_MASK_LE(__pdesc+12, 6, 1)
 #define GET_RX_DESC_SPLCP(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 8, 1)
+	SHIFT_AND_MASK_LE(__pdesc+12, 8, 1)
 #define GET_RX_DESC_BW(__pdesc)				\
-	LE_BITS_TO_4BYTE(__pdesc+12, 9, 1)
+	SHIFT_AND_MASK_LE(__pdesc+12, 9, 1)
 #define GET_RX_DESC_HTC(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 10, 1)
+	SHIFT_AND_MASK_LE(__pdesc+12, 10, 1)
 #define GET_RX_DESC_HWPC_ERR(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 14, 1)
+	SHIFT_AND_MASK_LE(__pdesc+12, 14, 1)
 #define GET_RX_DESC_HWPC_IND(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 15, 1)
+	SHIFT_AND_MASK_LE(__pdesc+12, 15, 1)
 #define GET_RX_DESC_IV0(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+12, 16, 16)
+	SHIFT_AND_MASK_LE(__pdesc+12, 16, 16)
 
 #define GET_RX_DESC_IV1(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+16, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+16, 0, 32)
 #define GET_RX_DESC_TSFL(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+20, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+20, 0, 32)
 
 #define GET_RX_DESC_BUFF_ADDR(__pdesc)			\
-	LE_BITS_TO_4BYTE(__pdesc+24, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+24, 0, 32)
 #define GET_RX_DESC_BUFF_ADDR64(__pdesc)		\
-	LE_BITS_TO_4BYTE(__pdesc+28, 0, 32)
+	SHIFT_AND_MASK_LE(__pdesc+28, 0, 32)
 
 #define SET_RX_DESC_BUFF_ADDR(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
+	SET_BITS_OFFSET_LE(__pdesc+24, 0, 32, __val)
 #define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val) 	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
+	SET_BITS_OFFSET_LE(__pdesc+28, 0, 32, __val)
 
 #define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)	\
 do {							\
@@ -711,4 +735,6 @@ void rtl92ce_tx_polling(struct ieee80211_hw *hw, unsigned int hw_queue);
 void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
 			     bool b_firstseg, bool b_lastseg,
 			     struct sk_buff *skb);
+bool _rtl92c_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/Makefile b/drivers/net/wireless/rtlwifi/rtl8192cu/Makefile
new file mode 100644
index 000000000000..ad2de6b839ef
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/Makefile
@@ -0,0 +1,14 @@
+rtl8192cu-objs :=		\
+		dm.o		\
+		hw.o		\
+		led.o		\
+		mac.o		\
+		phy.o		\
+		rf.o		\
+		sw.o		\
+		table.o		\
+		trx.o
+
+obj-$(CONFIG_RTL8192CU) += rtl8192cu.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/def.h b/drivers/net/wireless/rtlwifi/rtl8192cu/def.h
new file mode 100644
index 000000000000..c54940ea72fe
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/def.h
@@ -0,0 +1,62 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../rtl8192ce/def.h"
+
+/*-------------------------------------------------------------------------
+ *	Chip specific
+ *-------------------------------------------------------------------------*/
+#define CHIP_8723			BIT(2) /* RTL8723 With BT feature */
+#define CHIP_8723_DRV_REV		BIT(3) /* RTL8723 Driver Revised */
+#define NORMAL_CHIP			BIT(4)
+#define CHIP_VENDOR_UMC			BIT(5)
+#define CHIP_VENDOR_UMC_B_CUT		BIT(6)
+
+#define IS_NORMAL_CHIP(version)		\
+	(((version) & NORMAL_CHIP) ? true : false)
+
+#define IS_8723_SERIES(version)		\
+	(((version) & CHIP_8723) ? true : false)
+
+#define IS_92C_1T2R(version)		\
+	(((version) & CHIP_92C) && ((version) & CHIP_92C_1T2R))
+
+#define IS_VENDOR_UMC(version)		\
+	(((version) & CHIP_VENDOR_UMC) ? true : false)
+
+#define IS_VENDOR_UMC_A_CUT(version)	\
+	(((version) & CHIP_VENDOR_UMC) ? (((version) & (BIT(6) | BIT(7))) ? \
+	false : true) : false)
+
+#define IS_VENDOR_8723_A_CUT(version)	\
+	(((version) & CHIP_VENDOR_UMC) ? (((version) & (BIT(6))) ? \
+	false : true) : false)
+
+#define CHIP_BONDING_92C_1T2R	0x1
+#define CHIP_BONDING_IDENTIFIER(_value)	(((_value) >> 22) & 0x3)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/dm.c b/drivers/net/wireless/rtlwifi/rtl8192cu/dm.c
new file mode 100644
index 000000000000..f311baee668d
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/dm.c
@@ -0,0 +1,113 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../base.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+
+void rtl92cu_dm_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	long undecorated_smoothed_pwdb;
+
+	if (!rtlpriv->dm.dynamic_txpower_enable)
+		return;
+
+	if (rtlpriv->dm.dm_flag & HAL_DM_HIPWR_DISABLE) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+		return;
+	}
+
+	if ((mac->link_state < MAC80211_LINKED) &&
+	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+			 ("Not connected to any\n"));
+
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+
+		rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
+		return;
+	}
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+			undecorated_smoothed_pwdb =
+			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 ("AP Client PWDB = 0x%lx\n",
+				  undecorated_smoothed_pwdb));
+		} else {
+			undecorated_smoothed_pwdb =
+			    rtlpriv->dm.undecorated_smoothed_pwdb;
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 ("STA Default Port PWDB = 0x%lx\n",
+				  undecorated_smoothed_pwdb));
+		}
+	} else {
+		undecorated_smoothed_pwdb =
+		    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 ("AP Ext Port PWDB = 0x%lx\n",
+			  undecorated_smoothed_pwdb));
+	}
+
+	if (undecorated_smoothed_pwdb >= TX_POWER_NEAR_FIELD_THRESH_LVL2) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 ("TXHIGHPWRLEVEL_LEVEL1 (TxPwr=0x0)\n"));
+	} else if ((undecorated_smoothed_pwdb <
+		    (TX_POWER_NEAR_FIELD_THRESH_LVL2 - 3)) &&
+		   (undecorated_smoothed_pwdb >=
+		    TX_POWER_NEAR_FIELD_THRESH_LVL1)) {
+
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 ("TXHIGHPWRLEVEL_LEVEL1 (TxPwr=0x10)\n"));
+	} else if (undecorated_smoothed_pwdb <
+		   (TX_POWER_NEAR_FIELD_THRESH_LVL1 - 5)) {
+		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 ("TXHIGHPWRLEVEL_NORMAL\n"));
+	}
+
+	if ((rtlpriv->dm.dynamic_txhighpower_lvl != rtlpriv->dm.last_dtp_lvl)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 ("PHY_SetTxPowerLevel8192S() Channel = %d\n",
+			  rtlphy->current_channel));
+		rtl92c_phy_set_txpower_level(hw, rtlphy->current_channel);
+	}
+
+	rtlpriv->dm.last_dtp_lvl = rtlpriv->dm.dynamic_txhighpower_lvl;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/dm.h b/drivers/net/wireless/rtlwifi/rtl8192cu/dm.h
new file mode 100644
index 000000000000..7f966c666b5a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/dm.h
@@ -0,0 +1,32 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../rtl8192ce/dm.h"
+
+void rtl92cu_dm_dynamic_txpower(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
new file mode 100644
index 000000000000..9444e76838cf
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -0,0 +1,2504 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../usb.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "mac.h"
+#include "dm.h"
+#include "hw.h"
+#include "trx.h"
+#include "led.h"
+#include "table.h"
+
+static void _rtl92cu_phy_param_tab_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+
+	rtlphy->hwparam_tables[MAC_REG].length = RTL8192CUMAC_2T_ARRAYLENGTH;
+	rtlphy->hwparam_tables[MAC_REG].pdata = RTL8192CUMAC_2T_ARRAY;
+	if (IS_HIGHT_PA(rtlefuse->board_type)) {
+		rtlphy->hwparam_tables[PHY_REG_PG].length =
+			RTL8192CUPHY_REG_Array_PG_HPLength;
+		rtlphy->hwparam_tables[PHY_REG_PG].pdata =
+			RTL8192CUPHY_REG_Array_PG_HP;
+	} else {
+		rtlphy->hwparam_tables[PHY_REG_PG].length =
+			RTL8192CUPHY_REG_ARRAY_PGLENGTH;
+		rtlphy->hwparam_tables[PHY_REG_PG].pdata =
+			RTL8192CUPHY_REG_ARRAY_PG;
+	}
+	/* 2T */
+	rtlphy->hwparam_tables[PHY_REG_2T].length =
+			RTL8192CUPHY_REG_2TARRAY_LENGTH;
+	rtlphy->hwparam_tables[PHY_REG_2T].pdata =
+			RTL8192CUPHY_REG_2TARRAY;
+	rtlphy->hwparam_tables[RADIOA_2T].length =
+			RTL8192CURADIOA_2TARRAYLENGTH;
+	rtlphy->hwparam_tables[RADIOA_2T].pdata =
+			RTL8192CURADIOA_2TARRAY;
+	rtlphy->hwparam_tables[RADIOB_2T].length =
+			RTL8192CURADIOB_2TARRAYLENGTH;
+	rtlphy->hwparam_tables[RADIOB_2T].pdata =
+			RTL8192CU_RADIOB_2TARRAY;
+	rtlphy->hwparam_tables[AGCTAB_2T].length =
+			RTL8192CUAGCTAB_2TARRAYLENGTH;
+	rtlphy->hwparam_tables[AGCTAB_2T].pdata =
+			RTL8192CUAGCTAB_2TARRAY;
+	/* 1T */
+	if (IS_HIGHT_PA(rtlefuse->board_type)) {
+		rtlphy->hwparam_tables[PHY_REG_1T].length =
+			RTL8192CUPHY_REG_1T_HPArrayLength;
+		rtlphy->hwparam_tables[PHY_REG_1T].pdata =
+			RTL8192CUPHY_REG_1T_HPArray;
+		rtlphy->hwparam_tables[RADIOA_1T].length =
+			RTL8192CURadioA_1T_HPArrayLength;
+		rtlphy->hwparam_tables[RADIOA_1T].pdata =
+			RTL8192CURadioA_1T_HPArray;
+		rtlphy->hwparam_tables[RADIOB_1T].length =
+			RTL8192CURADIOB_1TARRAYLENGTH;
+		rtlphy->hwparam_tables[RADIOB_1T].pdata =
+			RTL8192CU_RADIOB_1TARRAY;
+		rtlphy->hwparam_tables[AGCTAB_1T].length =
+			RTL8192CUAGCTAB_1T_HPArrayLength;
+		rtlphy->hwparam_tables[AGCTAB_1T].pdata =
+			Rtl8192CUAGCTAB_1T_HPArray;
+	} else {
+		rtlphy->hwparam_tables[PHY_REG_1T].length =
+			 RTL8192CUPHY_REG_1TARRAY_LENGTH;
+		rtlphy->hwparam_tables[PHY_REG_1T].pdata =
+			RTL8192CUPHY_REG_1TARRAY;
+		rtlphy->hwparam_tables[RADIOA_1T].length =
+			RTL8192CURADIOA_1TARRAYLENGTH;
+		rtlphy->hwparam_tables[RADIOA_1T].pdata =
+			RTL8192CU_RADIOA_1TARRAY;
+		rtlphy->hwparam_tables[RADIOB_1T].length =
+			RTL8192CURADIOB_1TARRAYLENGTH;
+		rtlphy->hwparam_tables[RADIOB_1T].pdata =
+			RTL8192CU_RADIOB_1TARRAY;
+		rtlphy->hwparam_tables[AGCTAB_1T].length =
+			RTL8192CUAGCTAB_1TARRAYLENGTH;
+		rtlphy->hwparam_tables[AGCTAB_1T].pdata =
+			RTL8192CUAGCTAB_1TARRAY;
+	}
+}
+
+static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail,
+						 u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 rf_path, index, tempval;
+	u16 i;
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < 3; i++) {
+			if (!autoload_fail) {
+				rtlefuse->
+				    eeprom_chnlarea_txpwr_cck[rf_path][i] =
+				    hwinfo[EEPROM_TXPOWERCCK + rf_path * 3 + i];
+				rtlefuse->
+				    eeprom_chnlarea_txpwr_ht40_1s[rf_path][i] =
+				    hwinfo[EEPROM_TXPOWERHT40_1S + rf_path * 3 +
+					   i];
+			} else {
+				rtlefuse->
+				    eeprom_chnlarea_txpwr_cck[rf_path][i] =
+				    EEPROM_DEFAULT_TXPOWERLEVEL;
+				rtlefuse->
+				    eeprom_chnlarea_txpwr_ht40_1s[rf_path][i] =
+				    EEPROM_DEFAULT_TXPOWERLEVEL;
+			}
+		}
+	}
+	for (i = 0; i < 3; i++) {
+		if (!autoload_fail)
+			tempval = hwinfo[EEPROM_TXPOWERHT40_2SDIFF + i];
+		else
+			tempval = EEPROM_DEFAULT_HT40_2SDIFF;
+		rtlefuse->eeprom_chnlarea_txpwr_ht40_2sdiif[RF90_PATH_A][i] =
+		    (tempval & 0xf);
+		rtlefuse->eeprom_chnlarea_txpwr_ht40_2sdiif[RF90_PATH_B][i] =
+		    ((tempval & 0xf0) >> 4);
+	}
+	for (rf_path = 0; rf_path < 2; rf_path++)
+		for (i = 0; i < 3; i++)
+			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+				("RF(%d) EEPROM CCK Area(%d) = 0x%x\n", rf_path,
+				 i, rtlefuse->
+				 eeprom_chnlarea_txpwr_cck[rf_path][i]));
+	for (rf_path = 0; rf_path < 2; rf_path++)
+		for (i = 0; i < 3; i++)
+			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+				("RF(%d) EEPROM HT40 1S Area(%d) = 0x%x\n",
+				 rf_path, i,
+				 rtlefuse->
+				 eeprom_chnlarea_txpwr_ht40_1s[rf_path][i]));
+	for (rf_path = 0; rf_path < 2; rf_path++)
+		for (i = 0; i < 3; i++)
+			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+				("RF(%d) EEPROM HT40 2S Diff Area(%d) = 0x%x\n",
+				 rf_path, i,
+				 rtlefuse->
+				 eeprom_chnlarea_txpwr_ht40_2sdiif[rf_path]
+				 [i]));
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < 14; i++) {
+			index = _rtl92c_get_chnl_group((u8) i);
+			rtlefuse->txpwrlevel_cck[rf_path][i] =
+			    rtlefuse->eeprom_chnlarea_txpwr_cck[rf_path][index];
+			rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+			    rtlefuse->
+			    eeprom_chnlarea_txpwr_ht40_1s[rf_path][index];
+			if ((rtlefuse->
+			     eeprom_chnlarea_txpwr_ht40_1s[rf_path][index] -
+			     rtlefuse->
+			     eeprom_chnlarea_txpwr_ht40_2sdiif[rf_path][index])
+			    > 0) {
+				rtlefuse->txpwrlevel_ht40_2s[rf_path][i] =
+				    rtlefuse->
+				    eeprom_chnlarea_txpwr_ht40_1s[rf_path]
+				    [index] - rtlefuse->
+				    eeprom_chnlarea_txpwr_ht40_2sdiif[rf_path]
+				    [index];
+			} else {
+				rtlefuse->txpwrlevel_ht40_2s[rf_path][i] = 0;
+			}
+		}
+		for (i = 0; i < 14; i++) {
+			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+				("RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = "
+				 "[0x%x / 0x%x / 0x%x]\n", rf_path, i,
+				 rtlefuse->txpwrlevel_cck[rf_path][i],
+				 rtlefuse->txpwrlevel_ht40_1s[rf_path][i],
+				 rtlefuse->txpwrlevel_ht40_2s[rf_path][i]));
+		}
+	}
+	for (i = 0; i < 3; i++) {
+		if (!autoload_fail) {
+			rtlefuse->eeprom_pwrlimit_ht40[i] =
+			    hwinfo[EEPROM_TXPWR_GROUP + i];
+			rtlefuse->eeprom_pwrlimit_ht20[i] =
+			    hwinfo[EEPROM_TXPWR_GROUP + 3 + i];
+		} else {
+			rtlefuse->eeprom_pwrlimit_ht40[i] = 0;
+			rtlefuse->eeprom_pwrlimit_ht20[i] = 0;
+		}
+	}
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < 14; i++) {
+			index = _rtl92c_get_chnl_group((u8) i);
+			if (rf_path == RF90_PATH_A) {
+				rtlefuse->pwrgroup_ht20[rf_path][i] =
+				    (rtlefuse->eeprom_pwrlimit_ht20[index]
+				     & 0xf);
+				rtlefuse->pwrgroup_ht40[rf_path][i] =
+				    (rtlefuse->eeprom_pwrlimit_ht40[index]
+				     & 0xf);
+			} else if (rf_path == RF90_PATH_B) {
+				rtlefuse->pwrgroup_ht20[rf_path][i] =
+				    ((rtlefuse->eeprom_pwrlimit_ht20[index]
+				      & 0xf0) >> 4);
+				rtlefuse->pwrgroup_ht40[rf_path][i] =
+				    ((rtlefuse->eeprom_pwrlimit_ht40[index]
+				      & 0xf0) >> 4);
+			}
+			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+				("RF-%d pwrgroup_ht20[%d] = 0x%x\n",
+				 rf_path, i,
+				 rtlefuse->pwrgroup_ht20[rf_path][i]));
+			RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+				("RF-%d pwrgroup_ht40[%d] = 0x%x\n",
+				 rf_path, i,
+				 rtlefuse->pwrgroup_ht40[rf_path][i]));
+		}
+	}
+	for (i = 0; i < 14; i++) {
+		index = _rtl92c_get_chnl_group((u8) i);
+		if (!autoload_fail)
+			tempval = hwinfo[EEPROM_TXPOWERHT20DIFF + index];
+		else
+			tempval = EEPROM_DEFAULT_HT20_DIFF;
+		rtlefuse->txpwr_ht20diff[RF90_PATH_A][i] = (tempval & 0xF);
+		rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] =
+		    ((tempval >> 4) & 0xF);
+		if (rtlefuse->txpwr_ht20diff[RF90_PATH_A][i] & BIT(3))
+			rtlefuse->txpwr_ht20diff[RF90_PATH_A][i] |= 0xF0;
+		if (rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] & BIT(3))
+			rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] |= 0xF0;
+		index = _rtl92c_get_chnl_group((u8) i);
+		if (!autoload_fail)
+			tempval = hwinfo[EEPROM_TXPOWER_OFDMDIFF + index];
+		else
+			tempval = EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF;
+		rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i] = (tempval & 0xF);
+		rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i] =
+		    ((tempval >> 4) & 0xF);
+	}
+	rtlefuse->legacy_ht_txpowerdiff =
+	    rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][7];
+	for (i = 0; i < 14; i++)
+		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			("RF-A Ht20 to HT40 Diff[%d] = 0x%x\n", i,
+			 rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]));
+	for (i = 0; i < 14; i++)
+		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			("RF-A Legacy to Ht40 Diff[%d] = 0x%x\n", i,
+			 rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]));
+	for (i = 0; i < 14; i++)
+		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			("RF-B Ht20 to HT40 Diff[%d] = 0x%x\n", i,
+			 rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]));
+	for (i = 0; i < 14; i++)
+		RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+			("RF-B Legacy to HT40 Diff[%d] = 0x%x\n", i,
+			 rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]));
+	if (!autoload_fail)
+		rtlefuse->eeprom_regulatory = (hwinfo[RF_OPTION1] & 0x7);
+	else
+		rtlefuse->eeprom_regulatory = 0;
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		("eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory));
+	if (!autoload_fail) {
+		rtlefuse->eeprom_tssi[RF90_PATH_A] = hwinfo[EEPROM_TSSI_A];
+		rtlefuse->eeprom_tssi[RF90_PATH_B] = hwinfo[EEPROM_TSSI_B];
+	} else {
+		rtlefuse->eeprom_tssi[RF90_PATH_A] = EEPROM_DEFAULT_TSSI;
+		rtlefuse->eeprom_tssi[RF90_PATH_B] = EEPROM_DEFAULT_TSSI;
+	}
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		("TSSI_A = 0x%x, TSSI_B = 0x%x\n",
+		 rtlefuse->eeprom_tssi[RF90_PATH_A],
+		 rtlefuse->eeprom_tssi[RF90_PATH_B]));
+	if (!autoload_fail)
+		tempval = hwinfo[EEPROM_THERMAL_METER];
+	else
+		tempval = EEPROM_DEFAULT_THERMALMETER;
+	rtlefuse->eeprom_thermalmeter = (tempval & 0x1f);
+	if (rtlefuse->eeprom_thermalmeter < 0x06 ||
+	    rtlefuse->eeprom_thermalmeter > 0x1c)
+		rtlefuse->eeprom_thermalmeter = 0x12;
+	if (rtlefuse->eeprom_thermalmeter == 0x1f || autoload_fail)
+		rtlefuse->apk_thermalmeterignore = true;
+	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		("thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter));
+}
+
+static void _rtl92cu_read_board_type(struct ieee80211_hw *hw, u8 *contents)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 boardType;
+
+	if (IS_NORMAL_CHIP(rtlhal->version)) {
+		boardType = ((contents[EEPROM_RF_OPT1]) &
+			    BOARD_TYPE_NORMAL_MASK) >> 5; /*bit[7:5]*/
+	} else {
+		boardType = contents[EEPROM_RF_OPT4];
+		boardType &= BOARD_TYPE_TEST_MASK;
+	}
+	rtlefuse->board_type = boardType;
+	if (IS_HIGHT_PA(rtlefuse->board_type))
+		rtlefuse->external_pa = 1;
+	printk(KERN_INFO "rtl8192cu: Board Type %x\n", rtlefuse->board_type);
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	/* Antenna Diversity setting. */
+	if (registry_par->antdiv_cfg == 2) /* 2: From Efuse */
+		rtl_efuse->antenna_cfg = (contents[EEPROM_RF_OPT1]&0x18)>>3;
+	else
+		rtl_efuse->antenna_cfg = registry_par->antdiv_cfg; /* 0:OFF, */
+
+	printk(KERN_INFO "rtl8192cu: Antenna Config %x\n",
+	       rtl_efuse->antenna_cfg);
+#endif
+}
+
+#ifdef CONFIG_BT_COEXIST
+static void _update_bt_param(_adapter *padapter)
+{
+	struct btcoexist_priv	 *pbtpriv = &(padapter->halpriv.bt_coexist);
+	struct registry_priv	*registry_par = &padapter->registrypriv;
+	if (2 != registry_par->bt_iso) {
+		/* 0:Low, 1:High, 2:From Efuse */
+		pbtpriv->BT_Ant_isolation = registry_par->bt_iso;
+	}
+	if (registry_par->bt_sco == 1) {
+		/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter, 4.Busy,
+		 * 5.OtherBusy */
+		pbtpriv->BT_Service = BT_OtherAction;
+	} else if (registry_par->bt_sco == 2) {
+		pbtpriv->BT_Service = BT_SCO;
+	} else if (registry_par->bt_sco == 4) {
+		pbtpriv->BT_Service = BT_Busy;
+	} else if (registry_par->bt_sco == 5) {
+		pbtpriv->BT_Service = BT_OtherBusy;
+	} else {
+		pbtpriv->BT_Service = BT_Idle;
+	}
+	pbtpriv->BT_Ampdu = registry_par->bt_ampdu;
+	pbtpriv->bCOBT = _TRUE;
+	pbtpriv->BtEdcaUL = 0;
+	pbtpriv->BtEdcaDL = 0;
+	pbtpriv->BtRssiState = 0xff;
+	pbtpriv->bInitSet = _FALSE;
+	pbtpriv->bBTBusyTraffic = _FALSE;
+	pbtpriv->bBTTrafficModeSet = _FALSE;
+	pbtpriv->bBTNonTrafficModeSet = _FALSE;
+	pbtpriv->CurrentState = 0;
+	pbtpriv->PreviousState = 0;
+	printk(KERN_INFO "rtl8192cu: BT Coexistance = %s\n",
+	       (pbtpriv->BT_Coexist == _TRUE) ? "enable" : "disable");
+	if (pbtpriv->BT_Coexist) {
+		if (pbtpriv->BT_Ant_Num == Ant_x2)
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "Ant_Num = Antx2\n");
+		else if (pbtpriv->BT_Ant_Num == Ant_x1)
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "Ant_Num = Antx1\n");
+		switch (pbtpriv->BT_CoexistType) {
+		case BT_2Wire:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = BT_2Wire\n");
+			break;
+		case BT_ISSC_3Wire:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = BT_ISSC_3Wire\n");
+			break;
+		case BT_Accel:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = BT_Accel\n");
+			break;
+		case BT_CSR_BC4:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = BT_CSR_BC4\n");
+			break;
+		case BT_CSR_BC8:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = BT_CSR_BC8\n");
+			break;
+		case BT_RTL8756:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = BT_RTL8756\n");
+			break;
+		default:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_"
+			       "CoexistType = Unknown\n");
+			break;
+		}
+		printk(KERN_INFO "rtl8192cu: BlueTooth BT_Ant_isolation = %d\n",
+		       pbtpriv->BT_Ant_isolation);
+		switch (pbtpriv->BT_Service) {
+		case BT_OtherAction:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_Service = "
+			       "BT_OtherAction\n");
+			break;
+		case BT_SCO:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_Service = "
+			       "BT_SCO\n");
+			break;
+		case BT_Busy:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_Service = "
+			       "BT_Busy\n");
+			break;
+		case BT_OtherBusy:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_Service = "
+			       "BT_OtherBusy\n");
+			break;
+		default:
+			printk(KERN_INFO "rtl8192cu: BlueTooth BT_Service = "
+			       "BT_Idle\n");
+			break;
+		}
+		printk(KERN_INFO "rtl8192cu: BT_RadioSharedType = 0x%x\n",
+		       pbtpriv->BT_RadioSharedType);
+	}
+}
+
+#define GET_BT_COEXIST(priv) (&priv->bt_coexist)
+
+static void _rtl92cu_read_bluetooth_coexistInfo(struct ieee80211_hw *hw,
+						u8 *contents,
+						bool bautoloadfailed);
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(Adapter);
+	bool isNormal = IS_NORMAL_CHIP(pHalData->VersionID);
+	struct btcoexist_priv	 *pbtpriv = &pHalData->bt_coexist;
+	u8	rf_opt4;
+
+	_rtw_memset(pbtpriv, 0, sizeof(struct btcoexist_priv));
+	if (AutoloadFail) {
+		pbtpriv->BT_Coexist = _FALSE;
+		pbtpriv->BT_CoexistType = BT_2Wire;
+		pbtpriv->BT_Ant_Num = Ant_x2;
+		pbtpriv->BT_Ant_isolation = 0;
+		pbtpriv->BT_RadioSharedType = BT_Radio_Shared;
+		return;
+	}
+	if (isNormal) {
+		if (pHalData->BoardType == BOARD_USB_COMBO)
+			pbtpriv->BT_Coexist = _TRUE;
+		else
+			pbtpriv->BT_Coexist = ((PROMContent[EEPROM_RF_OPT3] &
+					      0x20) >> 5); /* bit[5] */
+		rf_opt4 = PROMContent[EEPROM_RF_OPT4];
+		pbtpriv->BT_CoexistType = ((rf_opt4&0xe)>>1); /* bit [3:1] */
+		pbtpriv->BT_Ant_Num = (rf_opt4&0x1); /* bit [0] */
+		pbtpriv->BT_Ant_isolation = ((rf_opt4&0x10)>>4); /* bit [4] */
+		pbtpriv->BT_RadioSharedType = ((rf_opt4&0x20)>>5); /* bit [5] */
+	} else {
+		pbtpriv->BT_Coexist = (PROMContent[EEPROM_RF_OPT4] >> 4) ?
+				       _TRUE : _FALSE;
+	}
+	_update_bt_param(Adapter);
+}
+#endif
+
+static void _rtl92cu_read_adapter_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE] = {0};
+	u16 eeprom_id;
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		rtl_efuse_shadow_map_update(hw);
+		memcpy((void *)hwinfo,
+		       (void *)&rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("RTL819X Not boot from eeprom, check it !!"));
+	}
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD, ("MAP\n"),
+		      hwinfo, HWSET_MAX_SIZE);
+	eeprom_id = *((u16 *)&hwinfo[0]);
+	if (eeprom_id != RTL8190_EEPROM_ID) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+	}
+	if (rtlefuse->autoload_failflag == true)
+		return;
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
+	}
+	printk(KERN_INFO "rtl8192cu: MAC address: %pM\n", rtlefuse->dev_addr);
+	_rtl92cu_read_txpower_info_from_hwpg(hw,
+					   rtlefuse->autoload_failflag, hwinfo);
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 (" VID = 0x%02x PID = 0x%02x\n",
+		 rtlefuse->eeprom_vid, rtlefuse->eeprom_did));
+	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
+	rtlefuse->eeprom_version = *(u16 *)&hwinfo[EEPROM_VERSION];
+	rtlefuse->txpwr_fromeprom = true;
+	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case EEPROM_CID_DEFAULT:
+			if (rtlefuse->eeprom_did == 0x8176) {
+				if ((rtlefuse->eeprom_svid == 0x103C &&
+				     rtlefuse->eeprom_smid == 0x1629))
+					rtlhal->oem_id = RT_CID_819x_HP;
+				else
+					rtlhal->oem_id = RT_CID_DEFAULT;
+			} else {
+				rtlhal->oem_id = RT_CID_DEFAULT;
+			}
+			break;
+		case EEPROM_CID_TOSHIBA:
+			rtlhal->oem_id = RT_CID_TOSHIBA;
+			break;
+		case EEPROM_CID_QMI:
+			rtlhal->oem_id = RT_CID_819x_QMI;
+			break;
+		case EEPROM_CID_WHQL:
+		default:
+			rtlhal->oem_id = RT_CID_DEFAULT;
+			break;
+		}
+	}
+	_rtl92cu_read_board_type(hw, hwinfo);
+#ifdef CONFIG_BT_COEXIST
+	_rtl92cu_read_bluetooth_coexistInfo(hw, hwinfo,
+					    rtlefuse->autoload_failflag);
+#endif
+}
+
+static void _rtl92cu_hal_customized_behavior(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	switch (rtlhal->oem_id) {
+	case RT_CID_819x_HP:
+		usb_priv->ledctl.led_opendrain = true;
+		break;
+	case RT_CID_819x_Lenovo:
+	case RT_CID_DEFAULT:
+	case RT_CID_TOSHIBA:
+	case RT_CID_CCX:
+	case RT_CID_819x_Acer:
+	case RT_CID_WHQL:
+	default:
+		break;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 ("RT Customized ID: 0x%02X\n", rtlhal->oem_id));
+}
+
+void rtl92cu_read_eeprom_info(struct ieee80211_hw *hw)
+{
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_u1b;
+
+	if (!IS_NORMAL_CHIP(rtlhal->version))
+		return;
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
+	rtlefuse->epromtype = (tmp_u1b & EEPROMSEL) ?
+			       EEPROM_93C46 : EEPROM_BOOT_EFUSE;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, ("Boot from %s\n",
+		 (tmp_u1b & EEPROMSEL) ? "EERROM" : "EFUSE"));
+	rtlefuse->autoload_failflag = (tmp_u1b & EEPROM_EN) ? false : true;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, ("Autoload %s\n",
+		 (tmp_u1b & EEPROM_EN) ? "OK!!" : "ERR!!"));
+	_rtl92cu_read_adapter_info(hw);
+	_rtl92cu_hal_customized_behavior(hw);
+	return;
+}
+
+static int _rtl92cu_init_power_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int		status = 0;
+	u16		value16;
+	u8		value8;
+	/*  polling autoload done. */
+	u32	pollingCount = 0;
+
+	do {
+		if (rtl_read_byte(rtlpriv, REG_APS_FSMCO) & PFM_ALDN) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				 ("Autoload Done!\n"));
+			break;
+		}
+		if (pollingCount++ > 100) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+				 ("Failed to polling REG_APS_FSMCO[PFM_ALDN]"
+				 " done!\n"));
+			return -ENODEV;
+		}
+	} while (true);
+	/* 0. RSV_CTRL 0x1C[7:0] = 0 unlock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0);
+	/* Power on when re-enter from IPS/Radio off/card disable */
+	/* enable SPS into PWM mode */
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	udelay(100);
+	value8 = rtl_read_byte(rtlpriv, REG_LDOV12D_CTRL);
+	if (0 == (value8 & LDV12_EN)) {
+		value8 |= LDV12_EN;
+		rtl_write_byte(rtlpriv, REG_LDOV12D_CTRL, value8);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 (" power-on :REG_LDOV12D_CTRL Reg0x21:0x%02x.\n",
+			 value8));
+		udelay(100);
+		value8 = rtl_read_byte(rtlpriv, REG_SYS_ISO_CTRL);
+		value8 &= ~ISO_MD2PP;
+		rtl_write_byte(rtlpriv, REG_SYS_ISO_CTRL, value8);
+	}
+	/*  auto enable WLAN */
+	pollingCount = 0;
+	value16 = rtl_read_word(rtlpriv, REG_APS_FSMCO);
+	value16 |= APFM_ONMAC;
+	rtl_write_word(rtlpriv, REG_APS_FSMCO, value16);
+	do {
+		if (!(rtl_read_word(rtlpriv, REG_APS_FSMCO) & APFM_ONMAC)) {
+			printk(KERN_INFO "rtl8192cu: MAC auto ON okay!\n");
+			break;
+		}
+		if (pollingCount++ > 100) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+				 ("Failed to polling REG_APS_FSMCO[APFM_ONMAC]"
+				 " done!\n"));
+			return -ENODEV;
+		}
+	} while (true);
+	/* Enable Radio ,GPIO ,and LED function */
+	rtl_write_word(rtlpriv, REG_APS_FSMCO, 0x0812);
+	/* release RF digital isolation */
+	value16 = rtl_read_word(rtlpriv, REG_SYS_ISO_CTRL);
+	value16 &= ~ISO_DIOR;
+	rtl_write_word(rtlpriv, REG_SYS_ISO_CTRL, value16);
+	/* Reconsider when to do this operation after asking HWSD. */
+	pollingCount = 0;
+	rtl_write_byte(rtlpriv, REG_APSD_CTRL, (rtl_read_byte(rtlpriv,
+						REG_APSD_CTRL) & ~BIT(6)));
+	do {
+		pollingCount++;
+	} while ((pollingCount < 200) &&
+		 (rtl_read_byte(rtlpriv, REG_APSD_CTRL) & BIT(7)));
+	/* Enable MAC DMA/WMAC/SCHEDULE/SEC block */
+	value16 = rtl_read_word(rtlpriv,  REG_CR);
+	value16 |= (HCI_TXDMA_EN | HCI_RXDMA_EN | TXDMA_EN | RXDMA_EN |
+		    PROTOCOL_EN | SCHEDULE_EN | MACTXEN | MACRXEN | ENSEC);
+	rtl_write_word(rtlpriv, REG_CR, value16);
+	return status;
+}
+
+static void _rtl92cu_init_queue_reserved_page(struct ieee80211_hw *hw,
+					      bool wmm_enable,
+					      u8 out_ep_num,
+					      u8 queue_sel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool isChipN = IS_NORMAL_CHIP(rtlhal->version);
+	u32 outEPNum = (u32)out_ep_num;
+	u32 numHQ = 0;
+	u32 numLQ = 0;
+	u32 numNQ = 0;
+	u32 numPubQ;
+	u32 value32;
+	u8 value8;
+	u32 txQPageNum, txQPageUnit, txQRemainPage;
+
+	if (!wmm_enable) {
+		numPubQ = (isChipN) ? CHIP_B_PAGE_NUM_PUBQ :
+			  CHIP_A_PAGE_NUM_PUBQ;
+		txQPageNum = TX_TOTAL_PAGE_NUMBER - numPubQ;
+
+		txQPageUnit = txQPageNum/outEPNum;
+		txQRemainPage = txQPageNum % outEPNum;
+		if (queue_sel & TX_SELE_HQ)
+			numHQ = txQPageUnit;
+		if (queue_sel & TX_SELE_LQ)
+			numLQ = txQPageUnit;
+		/* HIGH priority queue always present in the configuration of
+		 * 2 out-ep. Remainder pages have assigned to High queue */
+		if ((outEPNum > 1) && (txQRemainPage))
+			numHQ += txQRemainPage;
+		/* NOTE: This step done before writting REG_RQPN. */
+		if (isChipN) {
+			if (queue_sel & TX_SELE_NQ)
+				numNQ = txQPageUnit;
+			value8 = (u8)_NPQ(numNQ);
+			rtl_write_byte(rtlpriv,  REG_RQPN_NPQ, value8);
+		}
+	} else {
+		/* for WMM ,number of out-ep must more than or equal to 2! */
+		numPubQ = isChipN ? WMM_CHIP_B_PAGE_NUM_PUBQ :
+			  WMM_CHIP_A_PAGE_NUM_PUBQ;
+		if (queue_sel & TX_SELE_HQ) {
+			numHQ = isChipN ? WMM_CHIP_B_PAGE_NUM_HPQ :
+				WMM_CHIP_A_PAGE_NUM_HPQ;
+		}
+		if (queue_sel & TX_SELE_LQ) {
+			numLQ = isChipN ? WMM_CHIP_B_PAGE_NUM_LPQ :
+				WMM_CHIP_A_PAGE_NUM_LPQ;
+		}
+		/* NOTE: This step done before writting REG_RQPN. */
+		if (isChipN) {
+			if (queue_sel & TX_SELE_NQ)
+				numNQ = WMM_CHIP_B_PAGE_NUM_NPQ;
+			value8 = (u8)_NPQ(numNQ);
+			rtl_write_byte(rtlpriv, REG_RQPN_NPQ, value8);
+		}
+	}
+	/* TX DMA */
+	value32 = _HPQ(numHQ) | _LPQ(numLQ) | _PUBQ(numPubQ) | LD_RQPN;
+	rtl_write_dword(rtlpriv, REG_RQPN, value32);
+}
+
+static void _rtl92c_init_trx_buffer(struct ieee80211_hw *hw, bool wmm_enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8	txpktbuf_bndy;
+	u8	value8;
+
+	if (!wmm_enable)
+		txpktbuf_bndy = TX_PAGE_BOUNDARY;
+	else /* for WMM */
+		txpktbuf_bndy = (IS_NORMAL_CHIP(rtlhal->version))
+						? WMM_CHIP_B_TX_PAGE_BOUNDARY
+						: WMM_CHIP_A_TX_PAGE_BOUNDARY;
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_WMAC_LBK_BF_HD, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TDECTRL+1, txpktbuf_bndy);
+	rtl_write_word(rtlpriv,  (REG_TRXFF_BNDY + 2), 0x27FF);
+	value8 = _PSRX(RX_PAGE_SIZE_REG_VALUE) | _PSTX(PBP_128);
+	rtl_write_byte(rtlpriv, REG_PBP, value8);
+}
+
+static void _rtl92c_init_chipN_reg_priority(struct ieee80211_hw *hw, u16 beQ,
+					    u16 bkQ, u16 viQ, u16 voQ,
+					    u16 mgtQ, u16 hiQ)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 value16 = (rtl_read_word(rtlpriv, REG_TRXDMA_CTRL) & 0x7);
+
+	value16 |= _TXDMA_BEQ_MAP(beQ) | _TXDMA_BKQ_MAP(bkQ) |
+		   _TXDMA_VIQ_MAP(viQ) | _TXDMA_VOQ_MAP(voQ) |
+		   _TXDMA_MGQ_MAP(mgtQ) | _TXDMA_HIQ_MAP(hiQ);
+	rtl_write_word(rtlpriv,  REG_TRXDMA_CTRL, value16);
+}
+
+static void _rtl92cu_init_chipN_one_out_ep_priority(struct ieee80211_hw *hw,
+						    bool wmm_enable,
+						    u8 queue_sel)
+{
+	u16 uninitialized_var(value);
+
+	switch (queue_sel) {
+	case TX_SELE_HQ:
+		value = QUEUE_HIGH;
+		break;
+	case TX_SELE_LQ:
+		value = QUEUE_LOW;
+		break;
+	case TX_SELE_NQ:
+		value = QUEUE_NORMAL;
+		break;
+	default:
+		WARN_ON(1); /* Shall not reach here! */
+		break;
+	}
+	_rtl92c_init_chipN_reg_priority(hw, value, value, value, value,
+					value, value);
+	printk(KERN_INFO "rtl8192cu: Tx queue select: 0x%02x\n", queue_sel);
+}
+
+static void _rtl92cu_init_chipN_two_out_ep_priority(struct ieee80211_hw *hw,
+								bool wmm_enable,
+								u8 queue_sel)
+{
+	u16 beQ, bkQ, viQ, voQ, mgtQ, hiQ;
+	u16 uninitialized_var(valueHi);
+	u16 uninitialized_var(valueLow);
+
+	switch (queue_sel) {
+	case (TX_SELE_HQ | TX_SELE_LQ):
+		valueHi = QUEUE_HIGH;
+		valueLow = QUEUE_LOW;
+		break;
+	case (TX_SELE_NQ | TX_SELE_LQ):
+		valueHi = QUEUE_NORMAL;
+		valueLow = QUEUE_LOW;
+		break;
+	case (TX_SELE_HQ | TX_SELE_NQ):
+		valueHi = QUEUE_HIGH;
+		valueLow = QUEUE_NORMAL;
+		break;
+	default:
+		WARN_ON(1);
+		break;
+	}
+	if (!wmm_enable) {
+		beQ = valueLow;
+		bkQ = valueLow;
+		viQ = valueHi;
+		voQ = valueHi;
+		mgtQ = valueHi;
+		hiQ = valueHi;
+	} else {/* for WMM ,CONFIG_OUT_EP_WIFI_MODE */
+		beQ = valueHi;
+		bkQ = valueLow;
+		viQ = valueLow;
+		voQ = valueHi;
+		mgtQ = valueHi;
+		hiQ = valueHi;
+	}
+	_rtl92c_init_chipN_reg_priority(hw, beQ, bkQ, viQ, voQ, mgtQ, hiQ);
+	printk(KERN_INFO "rtl8192cu: Tx queue select: 0x%02x\n", queue_sel);
+}
+
+static void _rtl92cu_init_chipN_three_out_ep_priority(struct ieee80211_hw *hw,
+						      bool wmm_enable,
+						      u8 queue_sel)
+{
+	u16 beQ, bkQ, viQ, voQ, mgtQ, hiQ;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (!wmm_enable) { /* typical setting */
+		beQ	= QUEUE_LOW;
+		bkQ	= QUEUE_LOW;
+		viQ	= QUEUE_NORMAL;
+		voQ	= QUEUE_HIGH;
+		mgtQ	= QUEUE_HIGH;
+		hiQ	= QUEUE_HIGH;
+	} else { /* for WMM */
+		beQ	= QUEUE_LOW;
+		bkQ	= QUEUE_NORMAL;
+		viQ	= QUEUE_NORMAL;
+		voQ	= QUEUE_HIGH;
+		mgtQ	= QUEUE_HIGH;
+		hiQ	= QUEUE_HIGH;
+	}
+	_rtl92c_init_chipN_reg_priority(hw, beQ, bkQ, viQ, voQ, mgtQ, hiQ);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+		 ("Tx queue select :0x%02x..\n", queue_sel));
+}
+
+static void _rtl92cu_init_chipN_queue_priority(struct ieee80211_hw *hw,
+					       bool wmm_enable,
+					       u8 out_ep_num,
+					       u8 queue_sel)
+{
+	switch (out_ep_num) {
+	case 1:
+		_rtl92cu_init_chipN_one_out_ep_priority(hw, wmm_enable,
+							queue_sel);
+		break;
+	case 2:
+		_rtl92cu_init_chipN_two_out_ep_priority(hw, wmm_enable,
+							queue_sel);
+		break;
+	case 3:
+		_rtl92cu_init_chipN_three_out_ep_priority(hw, wmm_enable,
+							  queue_sel);
+		break;
+	default:
+		WARN_ON(1); /* Shall not reach here! */
+		break;
+	}
+}
+
+static void _rtl92cu_init_chipT_queue_priority(struct ieee80211_hw *hw,
+					       bool wmm_enable,
+					       u8 out_ep_num,
+					       u8 queue_sel)
+{
+	u8	hq_sele;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (out_ep_num) {
+	case 2:	/* (TX_SELE_HQ|TX_SELE_LQ) */
+		if (!wmm_enable) /* typical setting */
+			hq_sele =  HQSEL_VOQ | HQSEL_VIQ | HQSEL_MGTQ |
+				   HQSEL_HIQ;
+		else	/* for WMM */
+			hq_sele = HQSEL_VOQ | HQSEL_BEQ | HQSEL_MGTQ |
+				  HQSEL_HIQ;
+		break;
+	case 1:
+		if (TX_SELE_LQ == queue_sel) {
+			/* map all endpoint to Low queue */
+			hq_sele = 0;
+		} else if (TX_SELE_HQ == queue_sel) {
+			/* map all endpoint to High queue */
+			hq_sele =  HQSEL_VOQ | HQSEL_VIQ | HQSEL_BEQ |
+				   HQSEL_BKQ | HQSEL_MGTQ | HQSEL_HIQ;
+		}
+		break;
+	default:
+		WARN_ON(1); /* Shall not reach here! */
+		break;
+	}
+	rtl_write_byte(rtlpriv, (REG_TRXDMA_CTRL+1), hq_sele);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+		 ("Tx queue select :0x%02x..\n", hq_sele));
+}
+
+static void _rtl92cu_init_queue_priority(struct ieee80211_hw *hw,
+						bool wmm_enable,
+						u8 out_ep_num,
+						u8 queue_sel)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		_rtl92cu_init_chipN_queue_priority(hw, wmm_enable, out_ep_num,
+						   queue_sel);
+	else
+		_rtl92cu_init_chipT_queue_priority(hw, wmm_enable, out_ep_num,
+						   queue_sel);
+}
+
+static void _rtl92cu_init_usb_aggregation(struct ieee80211_hw *hw)
+{
+}
+
+static void _rtl92cu_init_wmac_setting(struct ieee80211_hw *hw)
+{
+	u16			value16;
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	mac->rx_conf = (RCR_APM | RCR_AM | RCR_ADF | RCR_AB | RCR_APP_FCS |
+		      RCR_APP_ICV | RCR_AMF | RCR_HTC_LOC_CTRL |
+		      RCR_APP_MIC | RCR_APP_PHYSTS | RCR_ACRC32);
+	rtl_write_dword(rtlpriv, REG_RCR, mac->rx_conf);
+	/* Accept all multicast address */
+	rtl_write_dword(rtlpriv,  REG_MAR, 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv,  REG_MAR + 4, 0xFFFFFFFF);
+	/* Accept all management frames */
+	value16 = 0xFFFF;
+	rtl92c_set_mgt_filter(hw, value16);
+	/* Reject all control frame - default value is 0 */
+	rtl92c_set_ctrl_filter(hw, 0x0);
+	/* Accept all data frames */
+	value16 = 0xFFFF;
+	rtl92c_set_data_filter(hw, value16);
+}
+
+static int _rtl92cu_init_mac(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(usb_priv);
+	int err = 0;
+	u32	boundary = 0;
+	u8 wmm_enable = false; /* TODO */
+	u8 out_ep_nums = rtlusb->out_ep_nums;
+	u8 queue_sel = rtlusb->out_queue_sel;
+	err = _rtl92cu_init_power_on(hw);
+
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			("Failed to init power on!\n"));
+		return err;
+	}
+	if (!wmm_enable) {
+		boundary = TX_PAGE_BOUNDARY;
+	} else { /* for WMM */
+		boundary = (IS_NORMAL_CHIP(rtlhal->version))
+					? WMM_CHIP_B_TX_PAGE_BOUNDARY
+					: WMM_CHIP_A_TX_PAGE_BOUNDARY;
+	}
+	if (false == rtl92c_init_llt_table(hw, boundary)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			("Failed to init LLT Table!\n"));
+		return -EINVAL;
+	}
+	_rtl92cu_init_queue_reserved_page(hw, wmm_enable, out_ep_nums,
+					  queue_sel);
+	_rtl92c_init_trx_buffer(hw, wmm_enable);
+	_rtl92cu_init_queue_priority(hw, wmm_enable, out_ep_nums,
+				     queue_sel);
+	/* Get Rx PHY status in order to report RSSI and others. */
+	rtl92c_init_driver_info_size(hw, RTL92C_DRIVER_INFO_SIZE);
+	rtl92c_init_interrupt(hw);
+	rtl92c_init_network_type(hw);
+	_rtl92cu_init_wmac_setting(hw);
+	rtl92c_init_adaptive_ctrl(hw);
+	rtl92c_init_edca(hw);
+	rtl92c_init_rate_fallback(hw);
+	rtl92c_init_retry_function(hw);
+	_rtl92cu_init_usb_aggregation(hw);
+	rtlpriv->cfg->ops->set_bw_mode(hw, NL80211_CHAN_HT20);
+	rtl92c_set_min_space(hw, IS_92C_SERIAL(rtlhal->version));
+	rtl92c_init_beacon_parameters(hw, rtlhal->version);
+	rtl92c_init_ampdu_aggregation(hw);
+	rtl92c_init_beacon_max_error(hw, true);
+	return err;
+}
+
+void rtl92cu_enable_hw_security_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 sec_reg_value = 0x0;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 ("PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm));
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 ("not open sw encryption\n"));
+		return;
+	}
+	sec_reg_value = SCR_TxEncEnable | SCR_RxDecEnable;
+	if (rtlpriv->sec.use_defaultkey) {
+		sec_reg_value |= SCR_TxUseDK;
+		sec_reg_value |= SCR_RxUseDK;
+	}
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
+	rtl_write_byte(rtlpriv, REG_CR + 1, 0x02);
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+		 ("The SECR-value %x\n", sec_reg_value));
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
+}
+
+static void _rtl92cu_hw_configure(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	/* To Fix MAC loopback mode fail. */
+	rtl_write_byte(rtlpriv, REG_LDOHCI12_CTRL, 0x0f);
+	rtl_write_byte(rtlpriv, 0x15, 0xe9);
+	/* HW SEQ CTRL */
+	/* set 0x0 to 0xFF by tynli. Default enable HW SEQ NUM. */
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, 0xFF);
+	/* fixed USB interface interference issue */
+	rtl_write_byte(rtlpriv, 0xfe40, 0xe0);
+	rtl_write_byte(rtlpriv, 0xfe41, 0x8d);
+	rtl_write_byte(rtlpriv, 0xfe42, 0x80);
+	rtlusb->reg_bcn_ctrl_val = 0x18;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlusb->reg_bcn_ctrl_val);
+}
+
+static void _InitPABias(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 pa_setting;
+
+	/* FIXED PA current issue */
+	pa_setting = efuse_read_1byte(hw, 0x1FA);
+	if (!(pa_setting & BIT(0))) {
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0FFFFF, 0x0F406);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0FFFFF, 0x4F406);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0FFFFF, 0x8F406);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0FFFFF, 0xCF406);
+	}
+	if (!(pa_setting & BIT(1)) && IS_NORMAL_CHIP(rtlhal->version) &&
+	    IS_92C_SERIAL(rtlhal->version)) {
+		rtl_set_rfreg(hw, RF90_PATH_B, 0x15, 0x0FFFFF, 0x0F406);
+		rtl_set_rfreg(hw, RF90_PATH_B, 0x15, 0x0FFFFF, 0x4F406);
+		rtl_set_rfreg(hw, RF90_PATH_B, 0x15, 0x0FFFFF, 0x8F406);
+		rtl_set_rfreg(hw, RF90_PATH_B, 0x15, 0x0FFFFF, 0xCF406);
+	}
+	if (!(pa_setting & BIT(4))) {
+		pa_setting = rtl_read_byte(rtlpriv, 0x16);
+		pa_setting &= 0x0F;
+		rtl_write_byte(rtlpriv, 0x16, pa_setting | 0x90);
+	}
+}
+
+static void _InitAntenna_Selection(struct ieee80211_hw *hw)
+{
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (pHalData->AntDivCfg == 0)
+		return;
+
+	if (rtlphy->rf_type == RF_1T1R) {
+		rtl_write_dword(rtlpriv, REG_LEDCFG0,
+				rtl_read_dword(rtlpriv,
+				REG_LEDCFG0)|BIT(23));
+		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+		if (rtl_get_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300) ==
+		    Antenna_A)
+			pHalData->CurAntenna = Antenna_A;
+		else
+			pHalData->CurAntenna = Antenna_B;
+	}
+#endif
+}
+
+static void _dump_registers(struct ieee80211_hw *hw)
+{
+}
+
+static void _update_mac_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	mac->rx_conf = rtl_read_dword(rtlpriv, REG_RCR);
+	mac->rx_mgt_filter = rtl_read_word(rtlpriv, REG_RXFLTMAP0);
+	mac->rx_ctrl_filter = rtl_read_word(rtlpriv, REG_RXFLTMAP1);
+	mac->rx_data_filter = rtl_read_word(rtlpriv, REG_RXFLTMAP2);
+}
+
+int rtl92cu_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	int err = 0;
+	static bool iqk_initialized;
+
+	rtlhal->hw_type = HARDWARE_TYPE_RTL8192CU;
+	err = _rtl92cu_init_mac(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("init mac failed!\n"));
+		return err;
+	}
+	err = rtl92c_download_fw(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 ("Failed to download FW. Init HW without FW now..\n"));
+		err = 1;
+		rtlhal->fw_ready = false;
+		return err;
+	} else {
+		rtlhal->fw_ready = true;
+	}
+	rtlhal->last_hmeboxnum = 0; /* h2c */
+	_rtl92cu_phy_param_tab_init(hw);
+	rtl92cu_phy_mac_config(hw);
+	rtl92cu_phy_bb_config(hw);
+	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
+	rtl92c_phy_rf_config(hw);
+	if (IS_VENDOR_UMC_A_CUT(rtlhal->version) &&
+	    !IS_92C_SERIAL(rtlhal->version)) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD, 0x30255);
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G2, MASKDWORD, 0x50a00);
+	}
+	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, (enum radio_path)0,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtlphy->rfreg_chnlval[1] = rtl_get_rfreg(hw, (enum radio_path)1,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtl92cu_bb_block_on(hw);
+	rtl_cam_reset_all_entry(hw);
+	rtl92cu_enable_hw_security_config(hw);
+	ppsc->rfpwr_state = ERFON;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	if (ppsc->rfpwr_state == ERFON) {
+		rtl92c_phy_set_rfpath_switch(hw, 1);
+		if (iqk_initialized) {
+			rtl92c_phy_iq_calibrate(hw, false);
+		} else {
+			rtl92c_phy_iq_calibrate(hw, false);
+			iqk_initialized = true;
+		}
+		rtl92c_dm_check_txpower_tracking(hw);
+		rtl92c_phy_lc_calibrate(hw);
+	}
+	_rtl92cu_hw_configure(hw);
+	_InitPABias(hw);
+	_InitAntenna_Selection(hw);
+	_update_mac_setting(hw);
+	rtl92c_dm_init(hw);
+	_dump_registers(hw);
+	return err;
+}
+
+static void _DisableRFAFEAndResetBB(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+/**************************************
+a.	TXPAUSE 0x522[7:0] = 0xFF	Pause MAC TX queue
+b.	RF path 0 offset 0x00 = 0x00	disable RF
+c.	APSD_CTRL 0x600[7:0] = 0x40
+d.	SYS_FUNC_EN 0x02[7:0] = 0x16	reset BB state machine
+e.	SYS_FUNC_EN 0x02[7:0] = 0x14	reset BB state machine
+***************************************/
+	u8 eRFPath = 0, value8 = 0;
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+	rtl_set_rfreg(hw, (enum radio_path)eRFPath, 0x0, MASKBYTE0, 0x0);
+
+	value8 |= APSDOFF;
+	rtl_write_byte(rtlpriv, REG_APSD_CTRL, value8); /*0x40*/
+	value8 = 0;
+	value8 |= (FEN_USBD | FEN_USBA | FEN_BB_GLB_RSTn);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, value8);/*0x16*/
+	value8 &= (~FEN_BB_GLB_RSTn);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, value8); /*0x14*/
+}
+
+static void  _ResetDigitalProcedure1(struct ieee80211_hw *hw, bool bWithoutHWSM)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlhal->fw_version <=  0x20) {
+		/*****************************
+		f. MCUFWDL 0x80[7:0]=0		reset MCU ready status
+		g. SYS_FUNC_EN 0x02[10]= 0	reset MCU reg, (8051 reset)
+		h. SYS_FUNC_EN 0x02[15-12]= 5	reset MAC reg, DCORE
+		i. SYS_FUNC_EN 0x02[10]= 1	enable MCU reg, (8051 enable)
+		******************************/
+		u16 valu16 = 0;
+
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+		valu16 = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
+		rtl_write_word(rtlpriv, REG_SYS_FUNC_EN, (valu16 &
+			       (~FEN_CPUEN))); /* reset MCU ,8051 */
+		valu16 = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN)&0x0FFF;
+		rtl_write_word(rtlpriv, REG_SYS_FUNC_EN, (valu16 |
+			      (FEN_HWPDN|FEN_ELDR))); /* reset MAC */
+		valu16 = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
+		rtl_write_word(rtlpriv, REG_SYS_FUNC_EN, (valu16 |
+			       FEN_CPUEN)); /* enable MCU ,8051 */
+	} else {
+		u8 retry_cnts = 0;
+
+		/* IF fw in RAM code, do reset */
+		if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(1)) {
+			/* reset MCU ready status */
+			rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+			if (rtlhal->fw_ready) {
+				/* 8051 reset by self */
+				rtl_write_byte(rtlpriv, REG_HMETFR+3, 0x20);
+				while ((retry_cnts++ < 100) &&
+				       (FEN_CPUEN & rtl_read_word(rtlpriv,
+				       REG_SYS_FUNC_EN))) {
+					udelay(50);
+				}
+				if (retry_cnts >= 100) {
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+						("#####=> 8051 reset failed!.."
+						".......................\n"););
+					/* if 8051 reset fail, reset MAC. */
+					rtl_write_byte(rtlpriv,
+						       REG_SYS_FUNC_EN + 1,
+						       0x50);
+					udelay(100);
+				}
+			}
+		}
+		/* Reset MAC and Enable 8051 */
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, 0x54);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+	}
+	if (bWithoutHWSM) {
+		/*****************************
+		  Without HW auto state machine
+		g.SYS_CLKR 0x08[15:0] = 0x30A3		disable MAC clock
+		h.AFE_PLL_CTRL 0x28[7:0] = 0x80		disable AFE PLL
+		i.AFE_XTAL_CTRL 0x24[15:0] = 0x880F	gated AFE DIG_CLOCK
+		j.SYS_ISu_CTRL 0x00[7:0] = 0xF9		isolated digital to PON
+		******************************/
+		rtl_write_word(rtlpriv, REG_SYS_CLKR, 0x70A3);
+		rtl_write_byte(rtlpriv, REG_AFE_PLL_CTRL, 0x80);
+		rtl_write_word(rtlpriv, REG_AFE_XTAL_CTRL, 0x880F);
+		rtl_write_byte(rtlpriv, REG_SYS_ISO_CTRL, 0xF9);
+	}
+}
+
+static void _ResetDigitalProcedure2(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+/*****************************
+k. SYS_FUNC_EN 0x03[7:0] = 0x44		disable ELDR runction
+l. SYS_CLKR 0x08[15:0] = 0x3083		disable ELDR clock
+m. SYS_ISO_CTRL 0x01[7:0] = 0x83	isolated ELDR to PON
+******************************/
+	rtl_write_word(rtlpriv, REG_SYS_CLKR, 0x70A3);
+	rtl_write_byte(rtlpriv, REG_SYS_ISO_CTRL+1, 0x82);
+}
+
+static void _DisableGPIO(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+/***************************************
+j. GPIO_PIN_CTRL 0x44[31:0]=0x000
+k. Value = GPIO_PIN_CTRL[7:0]
+l.  GPIO_PIN_CTRL 0x44[31:0] = 0x00FF0000 | (value <<8); write ext PIN level
+m. GPIO_MUXCFG 0x42 [15:0] = 0x0780
+n. LEDCFG 0x4C[15:0] = 0x8080
+***************************************/
+	u8	value8;
+	u16	value16;
+	u32	value32;
+
+	/* 1. Disable GPIO[7:0] */
+	rtl_write_word(rtlpriv, REG_GPIO_PIN_CTRL+2, 0x0000);
+	value32 = rtl_read_dword(rtlpriv, REG_GPIO_PIN_CTRL) & 0xFFFF00FF;
+	value8 = (u8) (value32&0x000000FF);
+	value32 |= ((value8<<8) | 0x00FF0000);
+	rtl_write_dword(rtlpriv, REG_GPIO_PIN_CTRL, value32);
+	/* 2. Disable GPIO[10:8] */
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG+3, 0x00);
+	value16 = rtl_read_word(rtlpriv, REG_GPIO_MUXCFG+2) & 0xFF0F;
+	value8 = (u8) (value16&0x000F);
+	value16 |= ((value8<<4) | 0x0780);
+	rtl_write_word(rtlpriv, REG_GPIO_PIN_CTRL+2, value16);
+	/* 3. Disable LED0 & 1 */
+	rtl_write_word(rtlpriv, REG_LEDCFG0, 0x8080);
+}
+
+static void _DisableAnalog(struct ieee80211_hw *hw, bool bWithoutHWSM)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 value16 = 0;
+	u8 value8 = 0;
+
+	if (bWithoutHWSM) {
+		/*****************************
+		n. LDOA15_CTRL 0x20[7:0] = 0x04	 disable A15 power
+		o. LDOV12D_CTRL 0x21[7:0] = 0x54 disable digital core power
+		r. When driver call disable, the ASIC will turn off remaining
+		   clock automatically
+		******************************/
+		rtl_write_byte(rtlpriv, REG_LDOA15_CTRL, 0x04);
+		value8 = rtl_read_byte(rtlpriv, REG_LDOV12D_CTRL);
+		value8 &= (~LDV12_EN);
+		rtl_write_byte(rtlpriv, REG_LDOV12D_CTRL, value8);
+	}
+
+/*****************************
+h. SPS0_CTRL 0x11[7:0] = 0x23		enter PFM mode
+i. APS_FSMCO 0x04[15:0] = 0x4802	set USB suspend
+******************************/
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x23);
+	value16 |= (APDM_HOST | AFSM_HSUS | PFM_ALDN);
+	rtl_write_word(rtlpriv, REG_APS_FSMCO, (u16)value16);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0E);
+}
+
+static void _CardDisableHWSM(struct ieee80211_hw *hw)
+{
+	/* ==== RF Off Sequence ==== */
+	_DisableRFAFEAndResetBB(hw);
+	/* ==== Reset digital sequence   ====== */
+	_ResetDigitalProcedure1(hw, false);
+	/*  ==== Pull GPIO PIN to balance level and LED control ====== */
+	_DisableGPIO(hw);
+	/* ==== Disable analog sequence === */
+	_DisableAnalog(hw, false);
+}
+
+static void _CardDisableWithoutHWSM(struct ieee80211_hw *hw)
+{
+	/*==== RF Off Sequence ==== */
+	_DisableRFAFEAndResetBB(hw);
+	/*  ==== Reset digital sequence   ====== */
+	_ResetDigitalProcedure1(hw, true);
+	/*  ==== Pull GPIO PIN to balance level and LED control ====== */
+	_DisableGPIO(hw);
+	/*  ==== Reset digital sequence   ====== */
+	_ResetDigitalProcedure2(hw);
+	/*  ==== Disable analog sequence === */
+	_DisableAnalog(hw, true);
+}
+
+static void _rtl92cu_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
+				      u8 set_bits, u8 clear_bits)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	rtlusb->reg_bcn_ctrl_val |= set_bits;
+	rtlusb->reg_bcn_ctrl_val &= ~clear_bits;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlusb->reg_bcn_ctrl_val);
+}
+
+static void _rtl92cu_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 tmp1byte = 0;
+	if (IS_NORMAL_CHIP(rtlhal->version)) {
+		tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+		rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+			       tmp1byte & (~BIT(6)));
+		rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+		tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+		tmp1byte &= ~(BIT(0));
+		rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE,
+			       rtl_read_byte(rtlpriv, REG_TXPAUSE) | BIT(6));
+	}
+}
+
+static void _rtl92cu_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 tmp1byte = 0;
+
+	if (IS_NORMAL_CHIP(rtlhal->version)) {
+		tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+		rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+			       tmp1byte | BIT(6));
+		rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+		tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+		tmp1byte |= BIT(0);
+		rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE,
+			       rtl_read_byte(rtlpriv, REG_TXPAUSE) & (~BIT(6)));
+	}
+}
+
+static void _rtl92cu_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		_rtl92cu_set_bcn_ctrl_reg(hw, 0, BIT(1));
+	else
+		_rtl92cu_set_bcn_ctrl_reg(hw, 0, BIT(4));
+}
+
+static void _rtl92cu_disable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		_rtl92cu_set_bcn_ctrl_reg(hw, BIT(1), 0);
+	else
+		_rtl92cu_set_bcn_ctrl_reg(hw, BIT(4), 0);
+}
+
+static int _rtl92cu_set_media_status(struct ieee80211_hw *hw,
+				     enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+
+	bt_msr &= 0xfc;
+	rtl_write_byte(rtlpriv, REG_BCN_MAX_ERR, 0xFF);
+	if (type == NL80211_IFTYPE_UNSPECIFIED || type ==
+	    NL80211_IFTYPE_STATION) {
+		_rtl92cu_stop_tx_beacon(hw);
+		_rtl92cu_enable_bcn_sub_func(hw);
+	} else if (type == NL80211_IFTYPE_ADHOC || type == NL80211_IFTYPE_AP) {
+		_rtl92cu_resume_tx_beacon(hw);
+		_rtl92cu_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, ("Set HW_VAR_MEDIA_"
+			 "STATUS:No such media status(%x).\n", type));
+	}
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		bt_msr |= MSR_NOLINK;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Set Network type to NO LINK!\n"));
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Set Network type to Ad Hoc!\n"));
+		break;
+	case NL80211_IFTYPE_STATION:
+		bt_msr |= MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Set Network type to STA!\n"));
+		break;
+	case NL80211_IFTYPE_AP:
+		bt_msr |= MSR_AP;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Set Network type to AP!\n"));
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("Network type %d not support!\n", type));
+		goto error_out;
+	}
+	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if ((bt_msr & 0xfc) == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+	return 0;
+error_out:
+	return 1;
+}
+
+void rtl92cu_card_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	enum nl80211_iftype opmode;
+
+	mac->link_state = MAC80211_NOLINK;
+	opmode = NL80211_IFTYPE_UNSPECIFIED;
+	_rtl92cu_set_media_status(hw, opmode);
+	rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+	if (rtlusb->disableHWSM)
+		_CardDisableHWSM(hw);
+	else
+		_CardDisableWithoutHWSM(hw);
+}
+
+void rtl92cu_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+{
+	/* dummy routine needed for callback from rtl_op_configure_filter() */
+}
+
+/*========================================================================== */
+
+static void _rtl92cu_set_check_bssid(struct ieee80211_hw *hw,
+			      enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 reg_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 filterout_non_associated_bssid = false;
+
+	switch (type) {
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_STATION:
+		filterout_non_associated_bssid = true;
+		break;
+	case NL80211_IFTYPE_UNSPECIFIED:
+	case NL80211_IFTYPE_AP:
+	default:
+		break;
+	}
+	if (filterout_non_associated_bssid == true) {
+		if (IS_NORMAL_CHIP(rtlhal->version)) {
+			switch (rtlphy->current_io_type) {
+			case IO_CMD_RESUME_DM_BY_SCAN:
+				reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						 HW_VAR_RCR, (u8 *)(&reg_rcr));
+				/* enable update TSF */
+				_rtl92cu_set_bcn_ctrl_reg(hw, 0, BIT(4));
+				break;
+			case IO_CMD_PAUSE_DM_BY_SCAN:
+				reg_rcr &= ~(RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						 HW_VAR_RCR, (u8 *)(&reg_rcr));
+				/* disable update TSF */
+				_rtl92cu_set_bcn_ctrl_reg(hw, BIT(4), 0);
+				break;
+			}
+		} else {
+			reg_rcr |= (RCR_CBSSID);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+						      (u8 *)(&reg_rcr));
+			_rtl92cu_set_bcn_ctrl_reg(hw, 0, (BIT(4)|BIT(5)));
+		}
+	} else if (filterout_non_associated_bssid == false) {
+		if (IS_NORMAL_CHIP(rtlhal->version)) {
+			reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+						      (u8 *)(&reg_rcr));
+			_rtl92cu_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		} else {
+			reg_rcr &= (~RCR_CBSSID);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+						      (u8 *)(&reg_rcr));
+			_rtl92cu_set_bcn_ctrl_reg(hw, (BIT(4)|BIT(5)), 0);
+		}
+	}
+}
+
+int rtl92cu_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	if (_rtl92cu_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+	_rtl92cu_set_check_bssid(hw, type);
+	return 0;
+}
+
+static void _InitBeaconParameters(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	rtl_write_word(rtlpriv, REG_BCN_CTRL, 0x1010);
+
+	/* TODO: Remove these magic number */
+	rtl_write_word(rtlpriv, REG_TBTT_PROHIBIT, 0x6404);
+	rtl_write_byte(rtlpriv, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);
+	rtl_write_byte(rtlpriv, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
+	/* Change beacon AIFS to the largest number
+	 * beacause test chip does not contension before sending beacon. */
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660F);
+	else
+		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x66FF);
+}
+
+static void _beacon_function_enable(struct ieee80211_hw *hw, bool Enable,
+				    bool Linked)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	_rtl92cu_set_bcn_ctrl_reg(hw, (BIT(4) | BIT(3) | BIT(1)), 0x00);
+	rtl_write_byte(rtlpriv, REG_RD_CTRL+1, 0x6F);
+}
+
+void rtl92cu_set_beacon_related_registers(struct ieee80211_hw *hw)
+{
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval, atim_window;
+	u32 value32;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	_InitBeaconParameters(hw);
+	rtl_write_byte(rtlpriv, REG_SLOT, 0x09);
+	/*
+	 * Force beacon frame transmission even after receiving beacon frame
+	 * from other ad hoc STA
+	 *
+	 *
+	 * Reset TSF Timer to zero, added by Roger. 2008.06.24
+	 */
+	value32 = rtl_read_dword(rtlpriv, REG_TCR);
+	value32 &= ~TSFRST;
+	rtl_write_dword(rtlpriv, REG_TCR, value32);
+	value32 |= TSFRST;
+	rtl_write_dword(rtlpriv, REG_TCR, value32);
+	RT_TRACE(rtlpriv, COMP_INIT|COMP_BEACON, DBG_LOUD,
+		 ("SetBeaconRelatedRegisters8192CUsb(): Set TCR(%x)\n",
+		 value32));
+	/* TODO: Modify later (Find the right parameters)
+	 * NOTE: Fix test chip's bug (about contention windows's randomness) */
+	if ((mac->opmode == NL80211_IFTYPE_ADHOC) ||
+	    (mac->opmode == NL80211_IFTYPE_AP)) {
+		rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x50);
+		rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x50);
+	}
+	_beacon_function_enable(hw, true, true);
+}
+
+void rtl92cu_set_beacon_interval(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval = mac->beacon_interval;
+
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_DMESG,
+		 ("beacon_interval:%d\n", bcn_interval));
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+}
+
+void rtl92cu_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr)
+{
+}
+
+void rtl92cu_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	switch (variable) {
+	case HW_VAR_RCR:
+		*((u32 *)(val)) = mac->rx_conf;
+		break;
+	case HW_VAR_RF_STATE:
+		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
+		break;
+	case HW_VAR_FWLPS_RF_ON:{
+			enum rf_pwrstate rfState;
+			u32 val_rcr;
+
+			rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
+						      (u8 *)(&rfState));
+			if (rfState == ERFOFF) {
+				*((bool *) (val)) = true;
+			} else {
+				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+				val_rcr &= 0x00070000;
+				if (val_rcr)
+					*((bool *) (val)) = false;
+				else
+					*((bool *) (val)) = true;
+			}
+			break;
+		}
+	case HW_VAR_FW_PSMODE_STATUS:
+		*((bool *) (val)) = ppsc->fw_current_inpsmode;
+		break;
+	case HW_VAR_CORRECT_TSF:{
+			u64 tsf;
+			u32 *ptsf_low = (u32 *)&tsf;
+			u32 *ptsf_high = ((u32 *)&tsf) + 1;
+
+			*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
+			*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+			*((u64 *)(val)) = tsf;
+			break;
+		}
+	case HW_VAR_MGT_FILTER:
+		*((u16 *) (val)) = rtl_read_word(rtlpriv, REG_RXFLTMAP0);
+		break;
+	case HW_VAR_CTRL_FILTER:
+		*((u16 *) (val)) = rtl_read_word(rtlpriv, REG_RXFLTMAP1);
+		break;
+	case HW_VAR_DATA_FILTER:
+		*((u16 *) (val)) = rtl_read_word(rtlpriv, REG_RXFLTMAP2);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+}
+
+void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	enum wireless_mode wirelessmode = mac->mode;
+	u8 idx = 0;
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_MACID + idx),
+					       val[idx]);
+			}
+			break;
+		}
+	case HW_VAR_BASIC_RATE:{
+			u16 rate_cfg = ((u16 *) val)[0];
+			u8 rate_index = 0;
+
+			rate_cfg &= 0x15f;
+			/* TODO */
+			/* if (mac->current_network.vender == HT_IOT_PEER_CISCO
+			 *     && ((rate_cfg & 0x150) == 0)) {
+			 *	  rate_cfg |= 0x010;
+			 * } */
+			rate_cfg |= 0x01;
+			rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
+			rtl_write_byte(rtlpriv, REG_RRSR + 1,
+				       (rate_cfg >> 8) & 0xff);
+			while (rate_cfg > 0x1) {
+				rate_cfg >>= 1;
+				rate_index++;
+			}
+			rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
+				       rate_index);
+			break;
+		}
+	case HW_VAR_BSSID:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_BSSID + idx),
+					       val[idx]);
+			}
+			break;
+		}
+	case HW_VAR_SIFS:{
+			rtl_write_byte(rtlpriv, REG_SIFS_CCK + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_SIFS_OFDM + 1, val[1]);
+			rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_R2T_SIFS+1, val[0]);
+			rtl_write_byte(rtlpriv, REG_T2T_SIFS+1, val[0]);
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 ("HW_VAR_SIFS\n"));
+			break;
+		}
+	case HW_VAR_SLOT_TIME:{
+			u8 e_aci;
+			u8 QOS_MODE = 1;
+
+			rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 ("HW_VAR_SLOT_TIME %x\n", val[0]));
+			if (QOS_MODE) {
+				for (e_aci = 0; e_aci < AC_MAX; e_aci++)
+					rtlpriv->cfg->ops->set_hw_reg(hw,
+								HW_VAR_AC_PARAM,
+								(u8 *)(&e_aci));
+			} else {
+				u8 sifstime = 0;
+				u8	u1bAIFS;
+
+				if (IS_WIRELESS_MODE_A(wirelessmode) ||
+				    IS_WIRELESS_MODE_N_24G(wirelessmode) ||
+				    IS_WIRELESS_MODE_N_5G(wirelessmode))
+					sifstime = 16;
+				else
+					sifstime = 10;
+				u1bAIFS = sifstime + (2 *  val[0]);
+				rtl_write_byte(rtlpriv, REG_EDCA_VO_PARAM,
+					       u1bAIFS);
+				rtl_write_byte(rtlpriv, REG_EDCA_VI_PARAM,
+					       u1bAIFS);
+				rtl_write_byte(rtlpriv, REG_EDCA_BE_PARAM,
+					       u1bAIFS);
+				rtl_write_byte(rtlpriv, REG_EDCA_BK_PARAM,
+					       u1bAIFS);
+			}
+			break;
+		}
+	case HW_VAR_ACK_PREAMBLE:{
+			u8 reg_tmp;
+			u8 short_preamble = (bool) (*(u8 *) val);
+			reg_tmp = 0;
+			if (short_preamble)
+				reg_tmp |= 0x80;
+			rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_tmp);
+			break;
+		}
+	case HW_VAR_AMPDU_MIN_SPACE:{
+			u8 min_spacing_to_set;
+			u8 sec_min_space;
+
+			min_spacing_to_set = *((u8 *) val);
+			if (min_spacing_to_set <= 7) {
+				switch (rtlpriv->sec.pairwise_enc_algorithm) {
+				case NO_ENCRYPTION:
+				case AESCCMP_ENCRYPTION:
+					sec_min_space = 0;
+					break;
+				case WEP40_ENCRYPTION:
+				case WEP104_ENCRYPTION:
+				case TKIP_ENCRYPTION:
+					sec_min_space = 6;
+					break;
+				default:
+					sec_min_space = 7;
+					break;
+				}
+				if (min_spacing_to_set < sec_min_space)
+					min_spacing_to_set = sec_min_space;
+				mac->min_space_cfg = ((mac->min_space_cfg &
+						     0xf8) |
+						     min_spacing_to_set);
+				*val = min_spacing_to_set;
+				RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+					("Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
+					mac->min_space_cfg));
+				rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+					       mac->min_space_cfg);
+			}
+			break;
+		}
+	case HW_VAR_SHORTGI_DENSITY:{
+			u8 density_to_set;
+
+			density_to_set = *((u8 *) val);
+			density_to_set &= 0x1f;
+			mac->min_space_cfg &= 0x07;
+			mac->min_space_cfg |= (density_to_set << 3);
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 ("Set HW_VAR_SHORTGI_DENSITY: %#x\n",
+				  mac->min_space_cfg));
+			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+				       mac->min_space_cfg);
+			break;
+		}
+	case HW_VAR_AMPDU_FACTOR:{
+			u8 regtoset_normal[4] = {0x41, 0xa8, 0x72, 0xb9};
+			u8 factor_toset;
+			u8 *p_regtoset = NULL;
+			u8 index = 0;
+
+			p_regtoset = regtoset_normal;
+			factor_toset = *((u8 *) val);
+			if (factor_toset <= 3) {
+				factor_toset = (1 << (factor_toset + 2));
+				if (factor_toset > 0xf)
+					factor_toset = 0xf;
+				for (index = 0; index < 4; index++) {
+					if ((p_regtoset[index] & 0xf0) >
+					    (factor_toset << 4))
+						p_regtoset[index] =
+						     (p_regtoset[index] & 0x0f)
+						     | (factor_toset << 4);
+					if ((p_regtoset[index] & 0x0f) >
+					     factor_toset)
+						p_regtoset[index] =
+						     (p_regtoset[index] & 0xf0)
+						     | (factor_toset);
+					rtl_write_byte(rtlpriv,
+						       (REG_AGGLEN_LMT + index),
+						       p_regtoset[index]);
+				}
+				RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+					 ("Set HW_VAR_AMPDU_FACTOR: %#x\n",
+					  factor_toset));
+			}
+			break;
+		}
+	case HW_VAR_AC_PARAM:{
+			u8 e_aci = *((u8 *) val);
+			u32 u4b_ac_param;
+			u16 cw_min = le16_to_cpu(mac->ac[e_aci].cw_min);
+			u16 cw_max = le16_to_cpu(mac->ac[e_aci].cw_max);
+			u16 tx_op = le16_to_cpu(mac->ac[e_aci].tx_op);
+
+			u4b_ac_param = (u32) mac->ac[e_aci].aifs;
+			u4b_ac_param |= (u32) ((cw_min & 0xF) <<
+					 AC_PARAM_ECW_MIN_OFFSET);
+			u4b_ac_param |= (u32) ((cw_max & 0xF) <<
+					 AC_PARAM_ECW_MAX_OFFSET);
+			u4b_ac_param |= (u32) tx_op << AC_PARAM_TXOP_OFFSET;
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 ("queue:%x, ac_param:%x\n", e_aci,
+				  u4b_ac_param));
+			switch (e_aci) {
+			case AC1_BK:
+				rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM,
+						u4b_ac_param);
+				break;
+			case AC0_BE:
+				rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM,
+						u4b_ac_param);
+				break;
+			case AC2_VI:
+				rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM,
+						u4b_ac_param);
+				break;
+			case AC3_VO:
+				rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM,
+						u4b_ac_param);
+				break;
+			default:
+				RT_ASSERT(false, ("SetHwReg8185(): invalid"
+					  " aci: %d !\n", e_aci));
+				break;
+			}
+			if (rtlusb->acm_method != eAcmWay2_SW)
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+					 HW_VAR_ACM_CTRL, (u8 *)(&e_aci));
+			break;
+		}
+	case HW_VAR_ACM_CTRL:{
+			u8 e_aci = *((u8 *) val);
+			union aci_aifsn *p_aci_aifsn = (union aci_aifsn *)
+							(&(mac->ac[0].aifs));
+			u8 acm = p_aci_aifsn->f.acm;
+			u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+			acm_ctrl =
+			    acm_ctrl | ((rtlusb->acm_method == 2) ? 0x0 : 0x1);
+			if (acm) {
+				switch (e_aci) {
+				case AC0_BE:
+					acm_ctrl |= AcmHw_BeqEn;
+					break;
+				case AC2_VI:
+					acm_ctrl |= AcmHw_ViqEn;
+					break;
+				case AC3_VO:
+					acm_ctrl |= AcmHw_VoqEn;
+					break;
+				default:
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+						 ("HW_VAR_ACM_CTRL acm set "
+						  "failed: eACI is %d\n", acm));
+					break;
+				}
+			} else {
+				switch (e_aci) {
+				case AC0_BE:
+					acm_ctrl &= (~AcmHw_BeqEn);
+					break;
+				case AC2_VI:
+					acm_ctrl &= (~AcmHw_ViqEn);
+					break;
+				case AC3_VO:
+					acm_ctrl &= (~AcmHw_BeqEn);
+					break;
+				default:
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+						 ("switch case not process\n"));
+					break;
+				}
+			}
+			RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
+				 ("SetHwReg8190pci(): [HW_VAR_ACM_CTRL] "
+				  "Write 0x%X\n", acm_ctrl));
+			rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+			break;
+		}
+	case HW_VAR_RCR:{
+			rtl_write_dword(rtlpriv, REG_RCR, ((u32 *) (val))[0]);
+			mac->rx_conf = ((u32 *) (val))[0];
+			RT_TRACE(rtlpriv, COMP_RECV, DBG_DMESG,
+				 ("### Set RCR(0x%08x) ###\n", mac->rx_conf));
+			break;
+		}
+	case HW_VAR_RETRY_LIMIT:{
+			u8 retry_limit = ((u8 *) (val))[0];
+
+			rtl_write_word(rtlpriv, REG_RL,
+				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+				       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_DMESG, ("Set HW_VAR_R"
+				 "ETRY_LIMIT(0x%08x)\n", retry_limit));
+			break;
+		}
+	case HW_VAR_DUAL_TSF_RST:
+		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+		break;
+	case HW_VAR_EFUSE_BYTES:
+		rtlefuse->efuse_usedbytes = *((u16 *) val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		rtlefuse->efuse_usedpercentage = *((u8 *) val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl92c_phy_set_io_cmd(hw, (*(enum io_type *)val));
+		break;
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
+		break;
+	case HW_VAR_SET_RPWM:{
+			u8 rpwm_val = rtl_read_byte(rtlpriv, REG_USB_HRPWM);
+
+			if (rpwm_val & BIT(7))
+				rtl_write_byte(rtlpriv, REG_USB_HRPWM,
+					       (*(u8 *)val));
+			else
+				rtl_write_byte(rtlpriv, REG_USB_HRPWM,
+					       ((*(u8 *)val) | BIT(7)));
+			break;
+		}
+	case HW_VAR_H2C_FW_PWRMODE:{
+			u8 psmode = (*(u8 *) val);
+
+			if ((psmode != FW_PS_ACTIVE_MODE) &&
+			   (!IS_92C_SERIAL(rtlhal->version)))
+				rtl92c_dm_rf_saving(hw, true);
+			rtl92c_set_fw_pwrmode_cmd(hw, (*(u8 *) val));
+			break;
+		}
+	case HW_VAR_FW_PSMODE_STATUS:
+		ppsc->fw_current_inpsmode = *((bool *) val);
+		break;
+	case HW_VAR_H2C_FW_JOINBSSRPT:{
+			u8 mstatus = (*(u8 *) val);
+			u8 tmp_reg422;
+			bool recover = false;
+
+			if (mstatus == RT_MEDIA_CONNECT) {
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							 HW_VAR_AID, NULL);
+				rtl_write_byte(rtlpriv, REG_CR + 1, 0x03);
+				_rtl92cu_set_bcn_ctrl_reg(hw, 0, BIT(3));
+				_rtl92cu_set_bcn_ctrl_reg(hw, BIT(4), 0);
+				tmp_reg422 = rtl_read_byte(rtlpriv,
+							REG_FWHW_TXQ_CTRL + 2);
+				if (tmp_reg422 & BIT(6))
+					recover = true;
+				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+					       tmp_reg422 & (~BIT(6)));
+				rtl92c_set_fw_rsvdpagepkt(hw, 0);
+				_rtl92cu_set_bcn_ctrl_reg(hw, BIT(3), 0);
+				_rtl92cu_set_bcn_ctrl_reg(hw, 0, BIT(4));
+				if (recover)
+					rtl_write_byte(rtlpriv,
+						 REG_FWHW_TXQ_CTRL + 2,
+						tmp_reg422 | BIT(6));
+				rtl_write_byte(rtlpriv, REG_CR + 1, 0x02);
+			}
+			rtl92c_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
+			break;
+		}
+	case HW_VAR_AID:{
+			u16 u2btmp;
+
+			u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
+			u2btmp &= 0xC000;
+			rtl_write_word(rtlpriv, REG_BCN_PSR_RPT,
+				       (u2btmp | mac->assoc_id));
+			break;
+		}
+	case HW_VAR_CORRECT_TSF:{
+			u8 btype_ibss = ((u8 *) (val))[0];
+
+			if (btype_ibss == true)
+				_rtl92cu_stop_tx_beacon(hw);
+			_rtl92cu_set_bcn_ctrl_reg(hw, 0, BIT(3));
+			rtl_write_dword(rtlpriv, REG_TSFTR, (u32)(mac->tsf &
+					0xffffffff));
+			rtl_write_dword(rtlpriv, REG_TSFTR + 4,
+					(u32)((mac->tsf >> 32) & 0xffffffff));
+			_rtl92cu_set_bcn_ctrl_reg(hw, BIT(3), 0);
+			if (btype_ibss == true)
+				_rtl92cu_resume_tx_beacon(hw);
+			break;
+		}
+	case HW_VAR_MGT_FILTER:
+		rtl_write_word(rtlpriv, REG_RXFLTMAP0, *(u16 *)val);
+		break;
+	case HW_VAR_CTRL_FILTER:
+		rtl_write_word(rtlpriv, REG_RXFLTMAP1, *(u16 *)val);
+		break;
+	case HW_VAR_DATA_FILTER:
+		rtl_write_word(rtlpriv, REG_RXFLTMAP2, *(u16 *)val);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, ("switch case "
+							"not process\n"));
+		break;
+	}
+}
+
+void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u32 ratr_value = (u32) mac->basic_rates;
+	u8 *mcsrate = mac->mcs;
+	u8 ratr_index = 0;
+	u8 nmode = mac->ht_enable;
+	u8 mimo_ps = 1;
+	u16 shortgi_rate = 0;
+	u32 tmp_ratr_value = 0;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 curshortgi_40mhz = mac->sgi_40;
+	u8 curshortgi_20mhz = mac->sgi_20;
+	enum wireless_mode wirelessmode = mac->mode;
+
+	ratr_value |= ((*(u16 *) (mcsrate))) << 12;
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		if (ratr_value & 0x0000000c)
+			ratr_value &= 0x0000000d;
+		else
+			ratr_value &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_value &= 0x00000FF5;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		nmode = 1;
+		if (mimo_ps == 0) {
+			ratr_value &= 0x0007F005;
+		} else {
+			u32 ratr_mask;
+
+			if (get_rf_type(rtlphy) == RF_1T2R ||
+			    get_rf_type(rtlphy) == RF_1T1R)
+				ratr_mask = 0x000ff005;
+			else
+				ratr_mask = 0x0f0ff005;
+			if (curtxbw_40mhz)
+				ratr_mask |= 0x00000010;
+			ratr_value &= ratr_mask;
+		}
+		break;
+	default:
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_value &= 0x000ff0ff;
+		else
+			ratr_value &= 0x0f0ff0ff;
+		break;
+	}
+	ratr_value &= 0x0FFFFFFF;
+	if (nmode && ((curtxbw_40mhz && curshortgi_40mhz) ||
+	    (!curtxbw_40mhz && curshortgi_20mhz))) {
+		ratr_value |= 0x10000000;
+		tmp_ratr_value = (ratr_value >> 12);
+		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
+			if ((1 << shortgi_rate) & tmp_ratr_value)
+				break;
+		}
+		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
+			       (shortgi_rate << 4) | (shortgi_rate);
+	}
+	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG, ("%x\n", rtl_read_dword(rtlpriv,
+		 REG_ARFR0)));
+}
+
+void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u32 ratr_bitmap = (u32) mac->basic_rates;
+	u8 *p_mcsrate = mac->mcs;
+	u8 ratr_index = 0;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 curshortgi_40mhz = mac->sgi_40;
+	u8 curshortgi_20mhz = mac->sgi_20;
+	enum wireless_mode wirelessmode = mac->mode;
+	bool shortgi = false;
+	u8 rate_mask[5];
+	u8 macid = 0;
+	u8 mimops = 1;
+
+	ratr_bitmap |= (p_mcsrate[1] << 20) | (p_mcsrate[0] << 12);
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		ratr_index = RATR_INX_WIRELESS_B;
+		if (ratr_bitmap & 0x0000000c)
+			ratr_bitmap &= 0x0000000d;
+		else
+			ratr_bitmap &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_index = RATR_INX_WIRELESS_GB;
+		if (rssi_level == 1)
+			ratr_bitmap &= 0x00000f00;
+		else if (rssi_level == 2)
+			ratr_bitmap &= 0x00000ff0;
+		else
+			ratr_bitmap &= 0x00000ff5;
+		break;
+	case WIRELESS_MODE_A:
+		ratr_index = RATR_INX_WIRELESS_A;
+		ratr_bitmap &= 0x00000ff0;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+		if (mimops == 0) {
+			if (rssi_level == 1)
+				ratr_bitmap &= 0x00070000;
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0x0007f000;
+			else
+				ratr_bitmap &= 0x0007f005;
+		} else {
+			if (rtlphy->rf_type == RF_1T2R ||
+			    rtlphy->rf_type == RF_1T1R) {
+				if (curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff005;
+				}
+			} else {
+				if (curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0f0f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0f0ff000;
+					else
+						ratr_bitmap &= 0x0f0ff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0f0f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0f0ff000;
+					else
+						ratr_bitmap &= 0x0f0ff005;
+				}
+			}
+		}
+		if ((curtxbw_40mhz && curshortgi_40mhz) ||
+		    (!curtxbw_40mhz && curshortgi_20mhz)) {
+			if (macid == 0)
+				shortgi = true;
+			else if (macid == 1)
+				shortgi = false;
+		}
+		break;
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f0ff0ff;
+		break;
+	}
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG, ("ratr_bitmap :%x\n",
+		 ratr_bitmap));
+	*(u32 *)&rate_mask = ((ratr_bitmap & 0x0fffffff) |
+				      ratr_index << 28);
+	rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG, ("Rate_index:%x, "
+						"ratr_val:%x, %x:%x:%x:%x:%x\n",
+						ratr_index, ratr_bitmap,
+						rate_mask[0], rate_mask[1],
+						rate_mask[2], rate_mask[3],
+						rate_mask[4]));
+	rtl92c_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
+}
+
+void rtl92cu_update_channel_access_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 sifs_timer;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+				      (u8 *)&mac->slot_time);
+	if (!mac->ht_enable)
+		sifs_timer = 0x0a0a;
+	else
+		sifs_timer = 0x0e0e;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *)&sifs_timer);
+}
+
+bool rtl92cu_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
+	u8 u1tmp = 0;
+	bool actuallyset = false;
+	unsigned long flag = 0;
+	/* to do - usb autosuspend */
+	u8 usb_autosuspend = 0;
+
+	if (ppsc->swrf_processing)
+		return false;
+	spin_lock_irqsave(&rtlpriv->locks.rf_ps_lock, flag);
+	if (ppsc->rfchange_inprogress) {
+		spin_unlock_irqrestore(&rtlpriv->locks.rf_ps_lock, flag);
+		return false;
+	} else {
+		ppsc->rfchange_inprogress = true;
+		spin_unlock_irqrestore(&rtlpriv->locks.rf_ps_lock, flag);
+	}
+	cur_rfstate = ppsc->rfpwr_state;
+	if (usb_autosuspend) {
+		/* to do................... */
+	} else {
+		if (ppsc->pwrdown_mode) {
+			u1tmp = rtl_read_byte(rtlpriv, REG_HSISR);
+			e_rfpowerstate_toset = (u1tmp & BIT(7)) ?
+					       ERFOFF : ERFON;
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+				 ("pwrdown, 0x5c(BIT7)=%02x\n", u1tmp));
+		} else {
+			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG,
+				       rtl_read_byte(rtlpriv,
+				       REG_MAC_PINMUX_CFG) & ~(BIT(3)));
+			u1tmp = rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL);
+			e_rfpowerstate_toset  = (u1tmp & BIT(3)) ?
+						 ERFON : ERFOFF;
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+				("GPIO_IN=%02x\n", u1tmp));
+		}
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, ("N-SS RF =%x\n",
+			 e_rfpowerstate_toset));
+	}
+	if ((ppsc->hwradiooff) && (e_rfpowerstate_toset == ERFON)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, ("GPIOChangeRF  - HW "
+			 "Radio ON, RF ON\n"));
+		ppsc->hwradiooff = false;
+		actuallyset = true;
+	} else if ((!ppsc->hwradiooff) && (e_rfpowerstate_toset  ==
+		    ERFOFF)) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, ("GPIOChangeRF  - HW"
+			 " Radio OFF\n"));
+		ppsc->hwradiooff = true;
+		actuallyset = true;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD ,
+			 ("pHalData->bHwRadioOff and eRfPowerStateToSet do not"
+			 " match: pHalData->bHwRadioOff %x, eRfPowerStateToSet "
+			 "%x\n", ppsc->hwradiooff, e_rfpowerstate_toset));
+	}
+	if (actuallyset) {
+		ppsc->hwradiooff = 1;
+		if (e_rfpowerstate_toset == ERFON) {
+			if ((ppsc->reg_rfps_level  & RT_RF_OFF_LEVL_ASPM) &&
+			     RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_ASPM))
+				RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_ASPM);
+			else if ((ppsc->reg_rfps_level  & RT_RF_OFF_LEVL_PCI_D3)
+				 && RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_PCI_D3))
+				RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_PCI_D3);
+		}
+		spin_lock_irqsave(&rtlpriv->locks.rf_ps_lock, flag);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock_irqrestore(&rtlpriv->locks.rf_ps_lock, flag);
+		/* For power down module, we need to enable register block
+		 * contrl reg at 0x1c. Then enable power down control bit
+		 * of register 0x04 BIT4 and BIT15 as 1.
+		 */
+		if (ppsc->pwrdown_mode && e_rfpowerstate_toset == ERFOFF) {
+			/* Enable register area 0x0-0xc. */
+			rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0);
+			if (IS_HARDWARE_TYPE_8723U(rtlhal)) {
+				/*
+				 * We should configure HW PDn source for WiFi
+				 * ONLY, and then our HW will be set in
+				 * power-down mode if PDn source from all
+				 * functions are configured.
+				 */
+				u1tmp = rtl_read_byte(rtlpriv,
+						      REG_MULTI_FUNC_CTRL);
+				rtl_write_byte(rtlpriv, REG_MULTI_FUNC_CTRL,
+					       (u1tmp|WL_HWPDN_EN));
+			} else {
+				rtl_write_word(rtlpriv, REG_APS_FSMCO, 0x8812);
+			}
+		}
+		if (e_rfpowerstate_toset == ERFOFF) {
+			if (ppsc->reg_rfps_level  & RT_RF_OFF_LEVL_ASPM)
+				RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_ASPM);
+			else if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_PCI_D3)
+				RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_PCI_D3);
+		}
+	} else if (e_rfpowerstate_toset == ERFOFF || cur_rfstate == ERFOFF) {
+		/* Enter D3 or ASPM after GPIO had been done. */
+		if (ppsc->reg_rfps_level  & RT_RF_OFF_LEVL_ASPM)
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_ASPM);
+		else if (ppsc->reg_rfps_level  & RT_RF_OFF_LEVL_PCI_D3)
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_PCI_D3);
+		spin_lock_irqsave(&rtlpriv->locks.rf_ps_lock, flag);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock_irqrestore(&rtlpriv->locks.rf_ps_lock, flag);
+	} else {
+		spin_lock_irqsave(&rtlpriv->locks.rf_ps_lock, flag);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock_irqrestore(&rtlpriv->locks.rf_ps_lock, flag);
+	}
+	*valid = 1;
+	return !ppsc->hwradiooff;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h
new file mode 100644
index 000000000000..62af555bb61c
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.h
@@ -0,0 +1,116 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CU_HW_H__
+#define __RTL92CU_HW_H__
+
+#define H2C_RA_MASK	6
+
+#define LLT_POLLING_LLT_THRESHOLD		20
+#define LLT_POLLING_READY_TIMEOUT_COUNT		100
+#define LLT_LAST_ENTRY_OF_TX_PKT_BUFFER		255
+
+#define RX_PAGE_SIZE_REG_VALUE			PBP_128
+/* Note: We will divide number of page equally for each queue
+ * other than public queue! */
+#define TX_TOTAL_PAGE_NUMBER			0xF8
+#define TX_PAGE_BOUNDARY			(TX_TOTAL_PAGE_NUMBER + 1)
+
+
+#define CHIP_B_PAGE_NUM_PUBQ			0xE7
+
+/* For Test Chip Setting
+ * (HPQ + LPQ + PUBQ) shall be TX_TOTAL_PAGE_NUMBER */
+#define CHIP_A_PAGE_NUM_PUBQ			0x7E
+
+
+/* For Chip A Setting */
+#define WMM_CHIP_A_TX_TOTAL_PAGE_NUMBER		0xF5
+#define WMM_CHIP_A_TX_PAGE_BOUNDARY		\
+	(WMM_CHIP_A_TX_TOTAL_PAGE_NUMBER + 1) /* F6 */
+
+#define WMM_CHIP_A_PAGE_NUM_PUBQ		0xA3
+#define WMM_CHIP_A_PAGE_NUM_HPQ			0x29
+#define WMM_CHIP_A_PAGE_NUM_LPQ			0x29
+
+
+
+/* Note: For Chip B Setting ,modify later */
+#define WMM_CHIP_B_TX_TOTAL_PAGE_NUMBER		0xF5
+#define WMM_CHIP_B_TX_PAGE_BOUNDARY		\
+	(WMM_CHIP_B_TX_TOTAL_PAGE_NUMBER + 1) /* F6 */
+
+#define WMM_CHIP_B_PAGE_NUM_PUBQ		0xB0
+#define WMM_CHIP_B_PAGE_NUM_HPQ			0x29
+#define WMM_CHIP_B_PAGE_NUM_LPQ			0x1C
+#define WMM_CHIP_B_PAGE_NUM_NPQ			0x1C
+
+#define BOARD_TYPE_NORMAL_MASK			0xE0
+#define BOARD_TYPE_TEST_MASK			0x0F
+
+/* should be renamed and moved to another file */
+enum _BOARD_TYPE_8192CUSB {
+	BOARD_USB_DONGLE		= 0,	/* USB dongle */
+	BOARD_USB_High_PA		= 1,	/* USB dongle - high power PA */
+	BOARD_MINICARD			= 2,	/* Minicard */
+	BOARD_USB_SOLO			= 3,	/* USB solo-Slim module */
+	BOARD_USB_COMBO			= 4,	/* USB Combo-Slim module */
+};
+
+#define IS_HIGHT_PA(boardtype)		\
+	((boardtype == BOARD_USB_High_PA) ? true : false)
+
+#define RTL92C_DRIVER_INFO_SIZE				4
+void rtl92cu_read_eeprom_info(struct ieee80211_hw *hw);
+void rtl92cu_enable_hw_security_config(struct ieee80211_hw *hw);
+int rtl92cu_hw_init(struct ieee80211_hw *hw);
+void rtl92cu_card_disable(struct ieee80211_hw *hw);
+int rtl92cu_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+void rtl92cu_set_beacon_related_registers(struct ieee80211_hw *hw);
+void rtl92cu_set_beacon_interval(struct ieee80211_hw *hw);
+void rtl92cu_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr);
+void rtl92cu_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw);
+void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level);
+
+void rtl92cu_update_channel_access_setting(struct ieee80211_hw *hw);
+bool rtl92cu_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid);
+void rtl92cu_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
+u8 _rtl92c_get_chnl_group(u8 chnl);
+int rtl92c_download_fw(struct ieee80211_hw *hw);
+void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished);
+void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl92c_fill_h2c_cmd(struct ieee80211_hw *hw,
+			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer);
+bool rtl92cu_phy_mac_config(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/led.c b/drivers/net/wireless/rtlwifi/rtl8192cu/led.c
new file mode 100644
index 000000000000..332c74348a69
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/led.c
@@ -0,0 +1,142 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../usb.h"
+#include "reg.h"
+#include "led.h"
+
+static void _rtl92cu_init_led(struct ieee80211_hw *hw,
+			      struct rtl_led *pled, enum rtl_led_pin ledpin)
+{
+	pled->hw = hw;
+	pled->ledpin = ledpin;
+	pled->ledon = false;
+}
+
+static void _rtl92cu_deInit_led(struct rtl_led *pled)
+{
+}
+
+void rtl92cu_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	u8 ledcfg;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 ("LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin));
+	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		rtl_write_byte(rtlpriv,
+			       REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5) | BIT(6));
+		break;
+	case LED_PIN_LED1:
+		rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg & 0x0f) | BIT(5));
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	pled->ledon = true;
+}
+
+void rtl92cu_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb_priv *usbpriv = rtl_usbpriv(hw);
+	u8 ledcfg;
+
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 ("LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin));
+	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg &= 0xf0;
+		if (usbpriv->ledctl.led_opendrain == true)
+			rtl_write_byte(rtlpriv, REG_LEDCFG2,
+				       (ledcfg | BIT(1) | BIT(5) | BIT(6)));
+		else
+			rtl_write_byte(rtlpriv, REG_LEDCFG2,
+				       (ledcfg | BIT(3) | BIT(5) | BIT(6)));
+		break;
+	case LED_PIN_LED1:
+		ledcfg &= 0x0f;
+		rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg | BIT(3)));
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	pled->ledon = false;
+}
+
+void rtl92cu_init_sw_leds(struct ieee80211_hw *hw)
+{
+	struct rtl_usb_priv *usbpriv = rtl_usbpriv(hw);
+	_rtl92cu_init_led(hw, &(usbpriv->ledctl.sw_led0), LED_PIN_LED0);
+	_rtl92cu_init_led(hw, &(usbpriv->ledctl.sw_led1), LED_PIN_LED1);
+}
+
+void rtl92cu_deinit_sw_leds(struct ieee80211_hw *hw)
+{
+	struct rtl_usb_priv *usbpriv = rtl_usbpriv(hw);
+	_rtl92cu_deInit_led(&(usbpriv->ledctl.sw_led0));
+	_rtl92cu_deInit_led(&(usbpriv->ledctl.sw_led1));
+}
+
+static void _rtl92cu_sw_led_control(struct ieee80211_hw *hw,
+				    enum led_ctl_mode ledaction)
+{
+}
+
+void rtl92cu_led_control(struct ieee80211_hw *hw,
+			enum led_ctl_mode ledaction)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	if ((ppsc->rfoff_reason > RF_CHANGE_BY_PS) &&
+	    (ledaction == LED_CTL_TX ||
+	     ledaction == LED_CTL_RX ||
+	     ledaction == LED_CTL_SITE_SURVEY ||
+	     ledaction == LED_CTL_LINK ||
+	     ledaction == LED_CTL_NO_LINK ||
+	     ledaction == LED_CTL_START_TO_LINK ||
+	     ledaction == LED_CTL_POWER_ON)) {
+		return;
+	}
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD, ("ledaction %d,\n",
+				ledaction));
+	_rtl92cu_sw_led_control(hw, ledaction);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/led.h b/drivers/net/wireless/rtlwifi/rtl8192cu/led.h
new file mode 100644
index 000000000000..decaee4d1eb1
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/led.h
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CU_LED_H__
+#define __RTL92CU_LED_H__
+
+void rtl92cu_init_sw_leds(struct ieee80211_hw *hw);
+void rtl92cu_deinit_sw_leds(struct ieee80211_hw *hw);
+void rtl92cu_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl92cu_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl92cu_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
new file mode 100644
index 000000000000..f8514cba17b6
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
@@ -0,0 +1,1144 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+****************************************************************************/
+#include <linux/module.h>
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../usb.h"
+#include "../ps.h"
+#include "../cam.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "mac.h"
+#include "trx.h"
+
+/* macro to shorten lines */
+
+#define LINK_Q	ui_link_quality
+#define RX_EVM	rx_evm_percentage
+#define RX_SIGQ	rx_mimo_signalquality
+
+
+void rtl92c_read_chip_version(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	enum version_8192c chip_version = VERSION_UNKNOWN;
+	u32 value32;
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG);
+	if (value32 & TRP_VAUX_EN) {
+		chip_version = (value32 & TYPE_ID) ? VERSION_TEST_CHIP_92C :
+			       VERSION_TEST_CHIP_88C;
+	} else {
+		/* Normal mass production chip. */
+		chip_version = NORMAL_CHIP;
+		chip_version |= ((value32 & TYPE_ID) ? CHIP_92C : 0);
+		chip_version |= ((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : 0);
+		/* RTL8723 with BT function. */
+		chip_version |= ((value32 & BT_FUNC) ? CHIP_8723 : 0);
+		if (IS_VENDOR_UMC(chip_version))
+			chip_version |= ((value32 & CHIP_VER_RTL_MASK) ?
+					 CHIP_VENDOR_UMC_B_CUT : 0);
+		if (IS_92C_SERIAL(chip_version)) {
+			value32 = rtl_read_dword(rtlpriv, REG_HPON_FSM);
+			chip_version |= ((CHIP_BONDING_IDENTIFIER(value32) ==
+				 CHIP_BONDING_92C_1T2R) ? CHIP_92C_1T2R : 0);
+		} else if (IS_8723_SERIES(chip_version)) {
+			value32 = rtl_read_dword(rtlpriv, REG_GPIO_OUTSTS);
+			chip_version |= ((value32 & RF_RL_ID) ?
+					  CHIP_8723_DRV_REV : 0);
+		}
+	}
+	rtlhal->version  = (enum version_8192c)chip_version;
+	switch (rtlhal->version) {
+	case VERSION_NORMAL_TSMC_CHIP_92C_1T2R:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 ("Chip Version ID: VERSION_B_CHIP_92C.\n"));
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_92C:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_92C.\n"));
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_88C:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_88C.\n"));
+		break;
+	case VERSION_NORMAL_UMC_CHIP_92C_1T2R_A_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_UMC_CHIP_i"
+			"92C_1T2R_A_CUT.\n"));
+		break;
+	case VERSION_NORMAL_UMC_CHIP_92C_A_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_UMC_CHIP_"
+			"92C_A_CUT.\n"));
+		break;
+	case VERSION_NORMAL_UMC_CHIP_88C_A_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_UMC_CHIP"
+			"_88C_A_CUT.\n"));
+		break;
+	case VERSION_NORMAL_UMC_CHIP_92C_1T2R_B_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_UMC_CHIP"
+			"_92C_1T2R_B_CUT.\n"));
+		break;
+	case VERSION_NORMAL_UMC_CHIP_92C_B_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_UMC_CHIP"
+			"_92C_B_CUT.\n"));
+		break;
+	case VERSION_NORMAL_UMC_CHIP_88C_B_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMAL_UMC_CHIP"
+			"_88C_B_CUT.\n"));
+		break;
+	case VERSION_NORMA_UMC_CHIP_8723_1T1R_A_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMA_UMC_CHIP"
+			"_8723_1T1R_A_CUT.\n"));
+		break;
+	case VERSION_NORMA_UMC_CHIP_8723_1T1R_B_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_NORMA_UMC_CHIP"
+			"_8723_1T1R_B_CUT.\n"));
+		break;
+	case VERSION_TEST_CHIP_92C:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_TEST_CHIP_92C.\n"));
+		break;
+	case VERSION_TEST_CHIP_88C:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: VERSION_TEST_CHIP_88C.\n"));
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			("Chip Version ID: ???????????????.\n"));
+		break;
+	}
+	if (IS_92C_SERIAL(rtlhal->version))
+		rtlphy->rf_type =
+			 (IS_92C_1T2R(rtlhal->version)) ? RF_1T2R : RF_2T2R;
+	else
+		rtlphy->rf_type = RF_1T1R;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 ("Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
+		  "RF_2T2R" : "RF_1T1R"));
+	if (get_rf_type(rtlphy) == RF_1T1R)
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+	else
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, ("VersionID = 0x%4x\n",
+						rtlhal->version));
+}
+
+/**
+ * writeLLT - LLT table write access
+ * @io: io callback
+ * @address: LLT logical address.
+ * @data: LLT data content
+ *
+ * Realtek hardware access function.
+ *
+ */
+bool rtl92c_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool status = true;
+	long count = 0;
+	u32 value = _LLT_INIT_ADDR(address) |
+	    _LLT_INIT_DATA(data) | _LLT_OP(_LLT_WRITE_ACCESS);
+
+	rtl_write_dword(rtlpriv, REG_LLT_INIT, value);
+	do {
+		value = rtl_read_dword(rtlpriv, REG_LLT_INIT);
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
+			break;
+		if (count > POLLING_LLT_THRESHOLD) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 ("Failed to polling write LLT done at"
+				 " address %d! _LLT_OP_VALUE(%x)\n",
+				 address, _LLT_OP_VALUE(value)));
+			status = false;
+			break;
+		}
+	} while (++count);
+	return status;
+}
+/**
+ * rtl92c_init_LLT_table - Init LLT table
+ * @io: io callback
+ * @boundary:
+ *
+ * Realtek hardware access function.
+ *
+ */
+bool rtl92c_init_llt_table(struct ieee80211_hw *hw, u32 boundary)
+{
+	bool rst = true;
+	u32	i;
+
+	for (i = 0; i < (boundary - 1); i++) {
+		rst = rtl92c_llt_write(hw, i , i + 1);
+		if (true != rst) {
+			printk(KERN_ERR "===> %s #1 fail\n", __func__);
+			return rst;
+		}
+	}
+	/* end of list */
+	rst = rtl92c_llt_write(hw, (boundary - 1), 0xFF);
+	if (true != rst) {
+		printk(KERN_ERR "===> %s #2 fail\n", __func__);
+		return rst;
+	}
+	/* Make the other pages as ring buffer
+	 * This ring buffer is used as beacon buffer if we config this MAC
+	 *  as two MAC transfer.
+	 * Otherwise used as local loopback buffer.
+	 */
+	for (i = boundary; i < LLT_LAST_ENTRY_OF_TX_PKT_BUFFER; i++) {
+		rst = rtl92c_llt_write(hw, i, (i + 1));
+		if (true != rst) {
+			printk(KERN_ERR "===> %s #3 fail\n", __func__);
+			return rst;
+		}
+	}
+	/* Let last entry point to the start entry of ring buffer */
+	rst = rtl92c_llt_write(hw, LLT_LAST_ENTRY_OF_TX_PKT_BUFFER, boundary);
+	if (true != rst) {
+		printk(KERN_ERR "===> %s #4 fail\n", __func__);
+		return rst;
+	}
+	return rst;
+}
+void rtl92c_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *macaddr = p_macaddr;
+	u32 entry_id = 0;
+	bool is_pairwise = false;
+	static u8 cam_const_addr[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8 cam_const_broad[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if (clear_all) {
+		u8 idx = 0;
+		u8 cam_offset = 0;
+		u8 clear_number = 5;
+
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, ("clear_all\n"));
+		for (idx = 0; idx < clear_number; idx++) {
+			rtl_cam_mark_invalid(hw, cam_offset + idx);
+			rtl_cam_empty_entry(hw, cam_offset + idx);
+			if (idx < 5) {
+				memset(rtlpriv->sec.key_buf[idx], 0,
+				       MAX_KEY_LEN);
+				rtlpriv->sec.key_len[idx] = 0;
+			}
+		}
+	} else {
+		switch (enc_algo) {
+		case WEP40_ENCRYPTION:
+			enc_algo = CAM_WEP40;
+			break;
+		case WEP104_ENCRYPTION:
+			enc_algo = CAM_WEP104;
+			break;
+		case TKIP_ENCRYPTION:
+			enc_algo = CAM_TKIP;
+			break;
+		case AESCCMP_ENCRYPTION:
+			enc_algo = CAM_AES;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				("iillegal switch case\n"));
+			enc_algo = CAM_TKIP;
+			break;
+		}
+		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
+			macaddr = cam_const_addr[key_index];
+			entry_id = key_index;
+		} else {
+			if (is_group) {
+				macaddr = cam_const_broad;
+				entry_id = key_index;
+			} else {
+				key_index = PAIRWISE_KEYIDX;
+				entry_id = CAM_PAIRWISE_KEY_POSITION;
+				is_pairwise = true;
+			}
+		}
+		if (rtlpriv->sec.key_len[key_index] == 0) {
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 ("delete one entry\n"));
+			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
+		} else {
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 ("The insert KEY length is %d\n",
+				  rtlpriv->sec.key_len[PAIRWISE_KEYIDX]));
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 ("The insert KEY  is %x %x\n",
+				  rtlpriv->sec.key_buf[0][0],
+				  rtlpriv->sec.key_buf[0][1]));
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 ("add one entry\n"));
+			if (is_pairwise) {
+				RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_LOUD,
+					      "Pairwiase Key content :",
+					      rtlpriv->sec.pairwise_key,
+					      rtlpriv->sec.
+					      key_len[PAIRWISE_KEYIDX]);
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 ("set Pairwiase key\n"));
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.
+						key_buf[key_index]);
+			} else {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 ("set group key\n"));
+				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+					rtl_cam_add_one_entry(hw,
+						rtlefuse->dev_addr,
+						PAIRWISE_KEYIDX,
+						CAM_PAIRWISE_KEY_POSITION,
+						enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf
+						[entry_id]);
+				}
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
+			}
+		}
+	}
+}
+
+u32 rtl92c_get_txdma_status(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	return rtl_read_dword(rtlpriv, REG_TXDMA_STATUS);
+}
+
+void rtl92c_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	if (IS_HARDWARE_TYPE_8192CE(rtlhal)) {
+		rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] &
+				0xFFFFFFFF);
+		rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] &
+				0xFFFFFFFF);
+		rtlpci->irq_enabled = true;
+	} else {
+		rtl_write_dword(rtlpriv, REG_HIMR, rtlusb->irq_mask[0] &
+				0xFFFFFFFF);
+		rtl_write_dword(rtlpriv, REG_HIMRE, rtlusb->irq_mask[1] &
+				0xFFFFFFFF);
+		rtlusb->irq_enabled = true;
+	}
+}
+
+void rtl92c_init_interrupt(struct ieee80211_hw *hw)
+{
+	 rtl92c_enable_interrupt(hw);
+}
+
+void rtl92c_disable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, IMR8190_DISABLED);
+	rtl_write_dword(rtlpriv, REG_HIMRE, IMR8190_DISABLED);
+	if (IS_HARDWARE_TYPE_8192CE(rtlhal))
+		rtlpci->irq_enabled = false;
+	else if (IS_HARDWARE_TYPE_8192CU(rtlhal))
+		rtlusb->irq_enabled = false;
+}
+
+void rtl92c_set_qos(struct ieee80211_hw *hw, int aci)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u32 u4b_ac_param;
+
+	rtl92c_dm_init_edca_turbo(hw);
+	u4b_ac_param = (u32) mac->ac[aci].aifs;
+	u4b_ac_param |=
+	    ((u32) le16_to_cpu(mac->ac[aci].cw_min) & 0xF) <<
+	    AC_PARAM_ECW_MIN_OFFSET;
+	u4b_ac_param |=
+	    ((u32) le16_to_cpu(mac->ac[aci].cw_max) & 0xF) <<
+	    AC_PARAM_ECW_MAX_OFFSET;
+	u4b_ac_param |= (u32) le16_to_cpu(mac->ac[aci].tx_op) <<
+			 AC_PARAM_TXOP_OFFSET;
+	RT_TRACE(rtlpriv, COMP_QOS, DBG_LOUD,
+		 ("queue:%x, ac_param:%x\n", aci, u4b_ac_param));
+	switch (aci) {
+	case AC1_BK:
+		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, u4b_ac_param);
+		break;
+	case AC0_BE:
+		rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, u4b_ac_param);
+		break;
+	case AC2_VI:
+		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, u4b_ac_param);
+		break;
+	case AC3_VO:
+		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, u4b_ac_param);
+		break;
+	default:
+		RT_ASSERT(false, ("invalid aci: %d !\n", aci));
+		break;
+	}
+}
+
+/*-------------------------------------------------------------------------
+ * HW MAC Address
+ *-------------------------------------------------------------------------*/
+void rtl92c_set_mac_addr(struct ieee80211_hw *hw, const u8 *addr)
+{
+	u32 i;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	for (i = 0 ; i < ETH_ALEN ; i++)
+		rtl_write_byte(rtlpriv, (REG_MACID + i), *(addr+i));
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_DMESG, ("MAC Address: %02X-%02X-%02X-"
+		"%02X-%02X-%02X\n",
+		rtl_read_byte(rtlpriv, REG_MACID),
+		rtl_read_byte(rtlpriv, REG_MACID+1),
+		rtl_read_byte(rtlpriv, REG_MACID+2),
+		rtl_read_byte(rtlpriv, REG_MACID+3),
+		rtl_read_byte(rtlpriv, REG_MACID+4),
+		rtl_read_byte(rtlpriv, REG_MACID+5)));
+}
+
+void rtl92c_init_driver_info_size(struct ieee80211_hw *hw, u8 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, size);
+}
+
+int rtl92c_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	u8 value;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		value = NT_NO_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			("Set Network type to NO LINK!\n"));
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		value = NT_LINK_AD_HOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			("Set Network type to Ad Hoc!\n"));
+		break;
+	case NL80211_IFTYPE_STATION:
+		value = NT_LINK_AP;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			("Set Network type to STA!\n"));
+		break;
+	case NL80211_IFTYPE_AP:
+		value = NT_AS_AP;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			("Set Network type to AP!\n"));
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			("Network type %d not support!\n", type));
+		return -EOPNOTSUPP;
+	}
+	rtl_write_byte(rtlpriv, (REG_CR + 2), value);
+	return 0;
+}
+
+void rtl92c_init_network_type(struct ieee80211_hw *hw)
+{
+	rtl92c_set_network_type(hw, NL80211_IFTYPE_UNSPECIFIED);
+}
+
+void rtl92c_init_adaptive_ctrl(struct ieee80211_hw *hw)
+{
+	u16	value16;
+	u32	value32;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/* Response Rate Set */
+	value32 = rtl_read_dword(rtlpriv, REG_RRSR);
+	value32 &= ~RATE_BITMAP_ALL;
+	value32 |= RATE_RRSR_CCK_ONLY_1M;
+	rtl_write_dword(rtlpriv, REG_RRSR, value32);
+	/* SIFS (used in NAV) */
+	value16 = _SPEC_SIFS_CCK(0x10) | _SPEC_SIFS_OFDM(0x10);
+	rtl_write_word(rtlpriv,  REG_SPEC_SIFS, value16);
+	/* Retry Limit */
+	value16 = _LRL(0x30) | _SRL(0x30);
+	rtl_write_dword(rtlpriv,  REG_RL, value16);
+}
+
+void rtl92c_init_rate_fallback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/* Set Data Auto Rate Fallback Retry Count register. */
+	rtl_write_dword(rtlpriv,  REG_DARFRC, 0x00000000);
+	rtl_write_dword(rtlpriv,  REG_DARFRC+4, 0x10080404);
+	rtl_write_dword(rtlpriv,  REG_RARFRC, 0x04030201);
+	rtl_write_dword(rtlpriv,  REG_RARFRC+4, 0x08070605);
+}
+
+static void rtl92c_set_cck_sifs(struct ieee80211_hw *hw, u8 trx_sifs,
+				u8 ctx_sifs)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SIFS_CCK, trx_sifs);
+	rtl_write_byte(rtlpriv, (REG_SIFS_CCK + 1), ctx_sifs);
+}
+
+static void rtl92c_set_ofdm_sifs(struct ieee80211_hw *hw, u8 trx_sifs,
+				 u8 ctx_sifs)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SIFS_OFDM, trx_sifs);
+	rtl_write_byte(rtlpriv, (REG_SIFS_OFDM + 1), ctx_sifs);
+}
+
+void rtl92c_init_edca_param(struct ieee80211_hw *hw,
+			    u16 queue, u16 txop, u8 cw_min, u8 cw_max, u8 aifs)
+{
+	/* sequence: VO, VI, BE, BK ==> the same as 92C hardware design.
+	 * referenc : enum nl80211_txq_q or ieee80211_set_wmm_default function.
+	 */
+	u32 value;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	value = (u32)aifs;
+	value |= ((u32)cw_min & 0xF) << 8;
+	value |= ((u32)cw_max & 0xF) << 12;
+	value |= (u32)txop << 16;
+	/* 92C hardware register sequence is the same as queue number. */
+	rtl_write_dword(rtlpriv, (REG_EDCA_VO_PARAM + (queue * 4)), value);
+}
+
+void rtl92c_init_edca(struct ieee80211_hw *hw)
+{
+	u16 value16;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/* disable EDCCA count down, to reduce collison and retry */
+	value16 = rtl_read_word(rtlpriv, REG_RD_CTRL);
+	value16 |= DIS_EDCA_CNT_DWN;
+	rtl_write_word(rtlpriv, REG_RD_CTRL, value16);
+	/* Update SIFS timing.  ??????????
+	 * pHalData->SifsTime = 0x0e0e0a0a; */
+	rtl92c_set_cck_sifs(hw, 0xa, 0xa);
+	rtl92c_set_ofdm_sifs(hw, 0xe, 0xe);
+	/* Set CCK/OFDM SIFS to be 10us. */
+	rtl_write_word(rtlpriv, REG_SIFS_CCK, 0x0a0a);
+	rtl_write_word(rtlpriv, REG_SIFS_OFDM, 0x1010);
+	rtl_write_word(rtlpriv, REG_PROT_MODE_CTRL, 0x0204);
+	rtl_write_dword(rtlpriv, REG_BAR_MODE_CTRL, 0x014004);
+	/* TXOP */
+	rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, 0x005EA42B);
+	rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0x0000A44F);
+	rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x005EA324);
+	rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x002FA226);
+	/* PIFS */
+	rtl_write_byte(rtlpriv, REG_PIFS, 0x1C);
+	/* AGGR BREAK TIME Register */
+	rtl_write_byte(rtlpriv, REG_AGGR_BREAK_TIME, 0x16);
+	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
+	rtl_write_byte(rtlpriv, REG_BCNDMATIM, 0x02);
+	rtl_write_byte(rtlpriv, REG_ATIMWND, 0x02);
+}
+
+void rtl92c_init_ampdu_aggregation(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_dword(rtlpriv, REG_AGGLEN_LMT, 0x99997631);
+	rtl_write_byte(rtlpriv, REG_AGGR_BREAK_TIME, 0x16);
+	/* init AMPDU aggregation number, tuning for Tx's TP, */
+	rtl_write_word(rtlpriv, 0x4CA, 0x0708);
+}
+
+void rtl92c_init_beacon_max_error(struct ieee80211_hw *hw, bool infra_mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_BCN_MAX_ERR, 0xFF);
+}
+
+void rtl92c_init_rdg_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_RD_CTRL, 0xFF);
+	rtl_write_word(rtlpriv, REG_RD_NAV_NXT, 0x200);
+	rtl_write_byte(rtlpriv, REG_RD_RESP_PKT_TH, 0x05);
+}
+
+void rtl92c_init_retry_function(struct ieee80211_hw *hw)
+{
+	u8	value8;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	value8 = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL);
+	value8 |= EN_AMPDU_RTY_NEW;
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL, value8);
+	/* Set ACK timeout */
+	rtl_write_byte(rtlpriv, REG_ACKTO, 0x40);
+}
+
+void rtl92c_init_beacon_parameters(struct ieee80211_hw *hw,
+				   enum version_8192c version)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	rtl_write_word(rtlpriv, REG_TBTT_PROHIBIT, 0x6404);/* ms */
+	rtl_write_byte(rtlpriv, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);/*ms*/
+	rtl_write_byte(rtlpriv, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660F);
+	else
+		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x66FF);
+}
+
+void rtl92c_disable_fast_edca(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_word(rtlpriv, REG_FAST_EDCA_CTRL, 0);
+}
+
+void rtl92c_set_min_space(struct ieee80211_hw *hw, bool is2T)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value = is2T ? MAX_MSS_DENSITY_2T : MAX_MSS_DENSITY_1T;
+
+	rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE, value);
+}
+
+u16 rtl92c_get_mgt_filter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	return rtl_read_word(rtlpriv, REG_RXFLTMAP0);
+}
+
+void rtl92c_set_mgt_filter(struct ieee80211_hw *hw, u16 filter)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_word(rtlpriv, REG_RXFLTMAP0, filter);
+}
+
+u16 rtl92c_get_ctrl_filter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	return rtl_read_word(rtlpriv, REG_RXFLTMAP1);
+}
+
+void rtl92c_set_ctrl_filter(struct ieee80211_hw *hw, u16 filter)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_word(rtlpriv, REG_RXFLTMAP1, filter);
+}
+
+u16 rtl92c_get_data_filter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	return rtl_read_word(rtlpriv,  REG_RXFLTMAP2);
+}
+
+void rtl92c_set_data_filter(struct ieee80211_hw *hw, u16 filter)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, filter);
+}
+/*==============================================================*/
+
+static u8 _rtl92c_query_rxpwrpercentage(char antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return 0;
+	else if (antpower >= 0)
+		return 100;
+	else
+		return 100 + antpower;
+}
+
+static u8 _rtl92c_evm_db_to_percentage(char value)
+{
+	char ret_val;
+
+	ret_val = value;
+	if (ret_val >= 0)
+		ret_val = 0;
+	if (ret_val <= -33)
+		ret_val = -33;
+	ret_val = 0 - ret_val;
+	ret_val *= 3;
+	if (ret_val == 99)
+		ret_val = 100;
+	return ret_val;
+}
+
+static long _rtl92c_translate_todbm(struct ieee80211_hw *hw,
+				     u8 signal_strength_index)
+{
+	long signal_power;
+
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+	return signal_power;
+}
+
+static long _rtl92c_signal_scale_mapping(struct ieee80211_hw *hw,
+		long currsig)
+{
+	long retsig;
+
+	if (currsig >= 61 && currsig <= 100)
+		retsig = 90 + ((currsig - 60) / 4);
+	else if (currsig >= 41 && currsig <= 60)
+		retsig = 78 + ((currsig - 40) / 2);
+	else if (currsig >= 31 && currsig <= 40)
+		retsig = 66 + (currsig - 30);
+	else if (currsig >= 21 && currsig <= 30)
+		retsig = 54 + (currsig - 20);
+	else if (currsig >= 5 && currsig <= 20)
+		retsig = 42 + (((currsig - 5) * 2) / 3);
+	else if (currsig == 4)
+		retsig = 36;
+	else if (currsig == 3)
+		retsig = 27;
+	else if (currsig == 2)
+		retsig = 18;
+	else if (currsig == 1)
+		retsig = 9;
+	else
+		retsig = currsig;
+	return retsig;
+}
+
+static void _rtl92c_query_rxphystatus(struct ieee80211_hw *hw,
+				      struct rtl_stats *pstats,
+				      struct rx_desc_92c *pdesc,
+				      struct rx_fwinfo_92c *p_drvinfo,
+				      bool packet_match_bssid,
+				      bool packet_toself,
+				      bool packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct phy_sts_cck_8192s_t *cck_buf;
+	s8 rx_pwr_all = 0, rx_pwr[4];
+	u8 rf_rx_num = 0, evm, pwdb_all;
+	u8 i, max_spatial_stream;
+	u32 rssi, total_rssi = 0;
+	bool in_powersavemode = false;
+	bool is_cck_rate;
+
+	is_cck_rate = RX_HAL_IS_CCK_RATE(pdesc);
+	pstats->packet_matchbssid = packet_match_bssid;
+	pstats->packet_toself = packet_toself;
+	pstats->is_cck = is_cck_rate;
+	pstats->packet_beacon = packet_beacon;
+	pstats->is_cck = is_cck_rate;
+	pstats->RX_SIGQ[0] = -1;
+	pstats->RX_SIGQ[1] = -1;
+	if (is_cck_rate) {
+		u8 report, cck_highpwr;
+		cck_buf = (struct phy_sts_cck_8192s_t *)p_drvinfo;
+		if (!in_powersavemode)
+			cck_highpwr = rtlphy->cck_high_power;
+		else
+			cck_highpwr = false;
+		if (!cck_highpwr) {
+			u8 cck_agc_rpt = cck_buf->cck_agc_rpt;
+			report = cck_buf->cck_agc_rpt & 0xc0;
+			report = report >> 6;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -46 - (cck_agc_rpt & 0x3e);
+				break;
+			case 0x2:
+				rx_pwr_all = -26 - (cck_agc_rpt & 0x3e);
+				break;
+			case 0x1:
+				rx_pwr_all = -12 - (cck_agc_rpt & 0x3e);
+				break;
+			case 0x0:
+				rx_pwr_all = 16 - (cck_agc_rpt & 0x3e);
+				break;
+			}
+		} else {
+			u8 cck_agc_rpt = cck_buf->cck_agc_rpt;
+			report = p_drvinfo->cfosho[0] & 0x60;
+			report = report >> 5;
+			switch (report) {
+			case 0x3:
+				rx_pwr_all = -46 - ((cck_agc_rpt & 0x1f) << 1);
+				break;
+			case 0x2:
+				rx_pwr_all = -26 - ((cck_agc_rpt & 0x1f) << 1);
+				break;
+			case 0x1:
+				rx_pwr_all = -12 - ((cck_agc_rpt & 0x1f) << 1);
+				break;
+			case 0x0:
+				rx_pwr_all = 16 - ((cck_agc_rpt & 0x1f) << 1);
+				break;
+			}
+		}
+		pwdb_all = _rtl92c_query_rxpwrpercentage(rx_pwr_all);
+		pstats->rx_pwdb_all = pwdb_all;
+		pstats->recvsignalpower = rx_pwr_all;
+		if (packet_match_bssid) {
+			u8 sq;
+			if (pstats->rx_pwdb_all > 40)
+				sq = 100;
+			else {
+				sq = cck_buf->sq_rpt;
+				if (sq > 64)
+					sq = 0;
+				else if (sq < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq) * 100) / 44;
+			}
+			pstats->signalquality = sq;
+			pstats->RX_SIGQ[0] = sq;
+			pstats->RX_SIGQ[1] = -1;
+		}
+	} else {
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
+		for (i = RF90_PATH_A; i < RF90_PATH_MAX; i++) {
+			if (rtlpriv->dm.rfpath_rxenable[i])
+				rf_rx_num++;
+			rx_pwr[i] =
+			    ((p_drvinfo->gain_trsw[i] & 0x3f) * 2) - 110;
+			rssi = _rtl92c_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += rssi;
+			rtlpriv->stats.rx_snr_db[i] =
+			    (long)(p_drvinfo->rxsnr[i] / 2);
+
+			if (packet_match_bssid)
+				pstats->rx_mimo_signalstrength[i] = (u8) rssi;
+		}
+		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
+		pwdb_all = _rtl92c_query_rxpwrpercentage(rx_pwr_all);
+		pstats->rx_pwdb_all = pwdb_all;
+		pstats->rxpower = rx_pwr_all;
+		pstats->recvsignalpower = rx_pwr_all;
+		if (GET_RX_DESC_RX_MCS(pdesc) &&
+		    GET_RX_DESC_RX_MCS(pdesc) >= DESC92C_RATEMCS8 &&
+		    GET_RX_DESC_RX_MCS(pdesc) <= DESC92C_RATEMCS15)
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+		for (i = 0; i < max_spatial_stream; i++) {
+			evm = _rtl92c_evm_db_to_percentage(p_drvinfo->rxevm[i]);
+			if (packet_match_bssid) {
+				if (i == 0)
+					pstats->signalquality =
+					    (u8) (evm & 0xff);
+				pstats->RX_SIGQ[i] =
+				    (u8) (evm & 0xff);
+			}
+		}
+	}
+	if (is_cck_rate)
+		pstats->signalstrength =
+		    (u8) (_rtl92c_signal_scale_mapping(hw, pwdb_all));
+	else if (rf_rx_num != 0)
+		pstats->signalstrength =
+		    (u8) (_rtl92c_signal_scale_mapping
+			  (hw, total_rssi /= rf_rx_num));
+}
+
+static void _rtl92c_process_ui_rssi(struct ieee80211_hw *hw,
+		struct rtl_stats *pstats)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 rfpath;
+	u32 last_rssi, tmpval;
+
+	if (pstats->packet_toself || pstats->packet_beacon) {
+		rtlpriv->stats.rssi_calculate_cnt++;
+		if (rtlpriv->stats.ui_rssi.total_num++ >=
+		    PHY_RSSI_SLID_WIN_MAX) {
+			rtlpriv->stats.ui_rssi.total_num =
+			    PHY_RSSI_SLID_WIN_MAX;
+			last_rssi =
+			    rtlpriv->stats.ui_rssi.elements[rtlpriv->
+							   stats.ui_rssi.index];
+			rtlpriv->stats.ui_rssi.total_val -= last_rssi;
+		}
+		rtlpriv->stats.ui_rssi.total_val += pstats->signalstrength;
+		rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.
+					index++] = pstats->signalstrength;
+		if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
+			rtlpriv->stats.ui_rssi.index = 0;
+		tmpval = rtlpriv->stats.ui_rssi.total_val /
+		    rtlpriv->stats.ui_rssi.total_num;
+		rtlpriv->stats.signal_strength =
+		    _rtl92c_translate_todbm(hw, (u8) tmpval);
+		pstats->rssi = rtlpriv->stats.signal_strength;
+	}
+	if (!pstats->is_cck && pstats->packet_toself) {
+		for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
+		     rfpath++) {
+			if (!rtl8192_phy_check_is_legal_rfpath(hw, rfpath))
+				continue;
+			if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
+				rtlpriv->stats.rx_rssi_percentage[rfpath] =
+				    pstats->rx_mimo_signalstrength[rfpath];
+			}
+			if (pstats->rx_mimo_signalstrength[rfpath] >
+			    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
+				rtlpriv->stats.rx_rssi_percentage[rfpath] =
+				    ((rtlpriv->stats.
+				      rx_rssi_percentage[rfpath] *
+				      (RX_SMOOTH_FACTOR - 1)) +
+				     (pstats->rx_mimo_signalstrength[rfpath])) /
+				    (RX_SMOOTH_FACTOR);
+
+				rtlpriv->stats.rx_rssi_percentage[rfpath] =
+				    rtlpriv->stats.rx_rssi_percentage[rfpath] +
+				    1;
+			} else {
+				rtlpriv->stats.rx_rssi_percentage[rfpath] =
+				    ((rtlpriv->stats.
+				      rx_rssi_percentage[rfpath] *
+				      (RX_SMOOTH_FACTOR - 1)) +
+				     (pstats->rx_mimo_signalstrength[rfpath])) /
+				    (RX_SMOOTH_FACTOR);
+			}
+		}
+	}
+}
+
+static void _rtl92c_update_rxsignalstatistics(struct ieee80211_hw *hw,
+					       struct rtl_stats *pstats)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int weighting = 0;
+
+	if (rtlpriv->stats.recv_signal_power == 0)
+		rtlpriv->stats.recv_signal_power = pstats->recvsignalpower;
+	if (pstats->recvsignalpower > rtlpriv->stats.recv_signal_power)
+		weighting = 5;
+	else if (pstats->recvsignalpower < rtlpriv->stats.recv_signal_power)
+		weighting = (-5);
+	rtlpriv->stats.recv_signal_power =
+	    (rtlpriv->stats.recv_signal_power * 5 +
+	     pstats->recvsignalpower + weighting) / 6;
+}
+
+static void _rtl92c_process_pwdb(struct ieee80211_hw *hw,
+		struct rtl_stats *pstats)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	long undecorated_smoothed_pwdb = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+		return;
+	} else {
+		undecorated_smoothed_pwdb =
+		    rtlpriv->dm.undecorated_smoothed_pwdb;
+	}
+	if (pstats->packet_toself || pstats->packet_beacon) {
+		if (undecorated_smoothed_pwdb < 0)
+			undecorated_smoothed_pwdb = pstats->rx_pwdb_all;
+		if (pstats->rx_pwdb_all > (u32) undecorated_smoothed_pwdb) {
+			undecorated_smoothed_pwdb =
+			    (((undecorated_smoothed_pwdb) *
+			      (RX_SMOOTH_FACTOR - 1)) +
+			     (pstats->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
+			undecorated_smoothed_pwdb = undecorated_smoothed_pwdb
+			    + 1;
+		} else {
+			undecorated_smoothed_pwdb =
+			    (((undecorated_smoothed_pwdb) *
+			      (RX_SMOOTH_FACTOR - 1)) +
+			     (pstats->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
+		}
+		rtlpriv->dm.undecorated_smoothed_pwdb =
+		    undecorated_smoothed_pwdb;
+		_rtl92c_update_rxsignalstatistics(hw, pstats);
+	}
+}
+
+static void _rtl92c_process_LINK_Q(struct ieee80211_hw *hw,
+					     struct rtl_stats *pstats)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 last_evm = 0, n_stream, tmpval;
+
+	if (pstats->signalquality != 0) {
+		if (pstats->packet_toself || pstats->packet_beacon) {
+			if (rtlpriv->stats.LINK_Q.total_num++ >=
+			    PHY_LINKQUALITY_SLID_WIN_MAX) {
+				rtlpriv->stats.LINK_Q.total_num =
+				    PHY_LINKQUALITY_SLID_WIN_MAX;
+				last_evm =
+				    rtlpriv->stats.LINK_Q.elements
+				    [rtlpriv->stats.LINK_Q.index];
+				rtlpriv->stats.LINK_Q.total_val -=
+				    last_evm;
+			}
+			rtlpriv->stats.LINK_Q.total_val +=
+			    pstats->signalquality;
+			rtlpriv->stats.LINK_Q.elements
+			   [rtlpriv->stats.LINK_Q.index++] =
+			    pstats->signalquality;
+			if (rtlpriv->stats.LINK_Q.index >=
+			    PHY_LINKQUALITY_SLID_WIN_MAX)
+				rtlpriv->stats.LINK_Q.index = 0;
+			tmpval = rtlpriv->stats.LINK_Q.total_val /
+			    rtlpriv->stats.LINK_Q.total_num;
+			rtlpriv->stats.signal_quality = tmpval;
+			rtlpriv->stats.last_sigstrength_inpercent = tmpval;
+			for (n_stream = 0; n_stream < 2;
+			     n_stream++) {
+				if (pstats->RX_SIGQ[n_stream] != -1) {
+					if (!rtlpriv->stats.RX_EVM[n_stream]) {
+						rtlpriv->stats.RX_EVM[n_stream]
+						 = pstats->RX_SIGQ[n_stream];
+					}
+					rtlpriv->stats.RX_EVM[n_stream] =
+					    ((rtlpriv->stats.RX_EVM
+					    [n_stream] *
+					    (RX_SMOOTH_FACTOR - 1)) +
+					    (pstats->RX_SIGQ
+					    [n_stream] * 1)) /
+					    (RX_SMOOTH_FACTOR);
+				}
+			}
+		}
+	} else {
+		;
+	}
+}
+
+static void _rtl92c_process_phyinfo(struct ieee80211_hw *hw,
+				     u8 *buffer,
+				     struct rtl_stats *pcurrent_stats)
+{
+	if (!pcurrent_stats->packet_matchbssid &&
+	    !pcurrent_stats->packet_beacon)
+		return;
+	_rtl92c_process_ui_rssi(hw, pcurrent_stats);
+	_rtl92c_process_pwdb(hw, pcurrent_stats);
+	_rtl92c_process_LINK_Q(hw, pcurrent_stats);
+}
+
+void rtl92c_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+					       struct sk_buff *skb,
+					       struct rtl_stats *pstats,
+					       struct rx_desc_92c *pdesc,
+					       struct rx_fwinfo_92c *p_drvinfo)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_hdr *hdr;
+	u8 *tmp_buf;
+	u8 *praddr;
+	u8 *psaddr;
+	__le16 fc;
+	u16 type, cpu_fc;
+	bool packet_matchbssid, packet_toself, packet_beacon;
+
+	tmp_buf = skb->data + pstats->rx_drvinfo_size + pstats->rx_bufshift;
+	hdr = (struct ieee80211_hdr *)tmp_buf;
+	fc = hdr->frame_control;
+	cpu_fc = le16_to_cpu(fc);
+	type = WLAN_FC_GET_TYPE(fc);
+	praddr = hdr->addr1;
+	psaddr = hdr->addr2;
+	packet_matchbssid =
+	    ((IEEE80211_FTYPE_CTL != type) &&
+	     (!compare_ether_addr(mac->bssid,
+			  (cpu_fc & IEEE80211_FCTL_TODS) ?
+			  hdr->addr1 : (cpu_fc & IEEE80211_FCTL_FROMDS) ?
+			  hdr->addr2 : hdr->addr3)) &&
+	     (!pstats->hwerror) && (!pstats->crc) && (!pstats->icv));
+
+	packet_toself = packet_matchbssid &&
+	    (!compare_ether_addr(praddr, rtlefuse->dev_addr));
+	if (ieee80211_is_beacon(fc))
+		packet_beacon = true;
+	_rtl92c_query_rxphystatus(hw, pstats, pdesc, p_drvinfo,
+				   packet_matchbssid, packet_toself,
+				   packet_beacon);
+	_rtl92c_process_phyinfo(hw, tmp_buf, pstats);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h
new file mode 100644
index 000000000000..298fdb724aa5
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h
@@ -0,0 +1,180 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92C_MAC_H__
+#define __RTL92C_MAC_H__
+
+#define LLT_LAST_ENTRY_OF_TX_PKT_BUFFER		255
+#define DRIVER_EARLY_INT_TIME					0x05
+#define BCN_DMA_ATIME_INT_TIME				0x02
+
+void rtl92c_read_chip_version(struct ieee80211_hw *hw);
+bool rtl92c_llt_write(struct ieee80211_hw *hw, u32 address, u32 data);
+bool rtl92c_init_llt_table(struct ieee80211_hw *hw, u32 boundary);
+void rtl92c_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all);
+void rtl92c_enable_interrupt(struct ieee80211_hw *hw);
+void rtl92c_disable_interrupt(struct ieee80211_hw *hw);
+void rtl92c_set_qos(struct ieee80211_hw *hw, int aci);
+
+
+/*---------------------------------------------------------------
+ *	Hardware init functions
+ *---------------------------------------------------------------*/
+void rtl92c_set_mac_addr(struct ieee80211_hw *hw, const u8 *addr);
+void rtl92c_init_interrupt(struct ieee80211_hw *hw);
+void rtl92c_init_driver_info_size(struct ieee80211_hw *hw, u8 size);
+
+int rtl92c_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+void rtl92c_init_network_type(struct ieee80211_hw *hw);
+void rtl92c_init_adaptive_ctrl(struct ieee80211_hw *hw);
+void rtl92c_init_rate_fallback(struct ieee80211_hw *hw);
+
+void rtl92c_init_edca_param(struct ieee80211_hw *hw,
+							u16 queue,
+							u16 txop,
+							u8 ecwmax,
+							u8 ecwmin,
+							u8 aifs);
+
+void rtl92c_init_edca(struct ieee80211_hw *hw);
+void rtl92c_init_ampdu_aggregation(struct ieee80211_hw *hw);
+void rtl92c_init_beacon_max_error(struct ieee80211_hw *hw, bool infra_mode);
+void rtl92c_init_rdg_setting(struct ieee80211_hw *hw);
+void rtl92c_init_retry_function(struct ieee80211_hw *hw);
+
+void rtl92c_init_beacon_parameters(struct ieee80211_hw *hw,
+				   enum version_8192c version);
+
+void rtl92c_disable_fast_edca(struct ieee80211_hw *hw);
+void rtl92c_set_min_space(struct ieee80211_hw *hw, bool is2T);
+
+/* For filter */
+u16 rtl92c_get_mgt_filter(struct ieee80211_hw *hw);
+void rtl92c_set_mgt_filter(struct ieee80211_hw *hw, u16 filter);
+u16 rtl92c_get_ctrl_filter(struct ieee80211_hw *hw);
+void rtl92c_set_ctrl_filter(struct ieee80211_hw *hw, u16 filter);
+u16 rtl92c_get_data_filter(struct ieee80211_hw *hw);
+void rtl92c_set_data_filter(struct ieee80211_hw *hw, u16 filter);
+
+
+u32 rtl92c_get_txdma_status(struct ieee80211_hw *hw);
+
+#define RX_HAL_IS_CCK_RATE(_pdesc)\
+	(GET_RX_DESC_RX_MCS(_pdesc) == DESC92C_RATE1M ||\
+	 GET_RX_DESC_RX_MCS(_pdesc) == DESC92C_RATE2M ||\
+	 GET_RX_DESC_RX_MCS(_pdesc) == DESC92C_RATE5_5M ||\
+	 GET_RX_DESC_RX_MCS(_pdesc) == DESC92C_RATE11M)
+
+struct rx_fwinfo_92c {
+	u8 gain_trsw[4];
+	u8 pwdb_all;
+	u8 cfosho[4];
+	u8 cfotail[4];
+	char rxevm[2];
+	char rxsnr[4];
+	u8 pdsnr[2];
+	u8 csi_current[2];
+	u8 csi_target[2];
+	u8 sigevm;
+	u8 max_ex_pwr;
+	u8 ex_intf_flag:1;
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 reserve:4;
+} __packed;
+
+struct rx_desc_92c {
+	u32 length:14;
+	u32 crc32:1;
+	u32 icverror:1;
+	u32 drv_infosize:4;
+	u32 security:3;
+	u32 qos:1;
+	u32 shift:2;
+	u32 phystatus:1;
+	u32 swdec:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 eor:1;
+	u32 own:1;
+	u32 macid:5;	/* word 1 */
+	u32 tid:4;
+	u32 hwrsvd:5;
+	u32 paggr:1;
+	u32 faggr:1;
+	u32 a1_fit:4;
+	u32 a2_fit:4;
+	u32 pam:1;
+	u32 pwr:1;
+	u32 moredata:1;
+	u32 morefrag:1;
+	u32 type:2;
+	u32 mc:1;
+	u32 bc:1;
+	u32 seq:12;	/* word 2 */
+	u32 frag:4;
+	u32 nextpktlen:14;
+	u32 nextind:1;
+	u32 rsvd:1;
+	u32 rxmcs:6;	/* word 3 */
+	u32 rxht:1;
+	u32 amsdu:1;
+	u32 splcp:1;
+	u32 bandwidth:1;
+	u32 htc:1;
+	u32 tcpchk_rpt:1;
+	u32 ipcchk_rpt:1;
+	u32 tcpchk_valid:1;
+	u32 hwpcerr:1;
+	u32 hwpcind:1;
+	u32 iv0:16;
+	u32 iv1;	/* word 4 */
+	u32 tsfl;	/* word 5 */
+	u32 bufferaddress;	/* word 6 */
+	u32 bufferaddress64;	/* word 7 */
+} __packed;
+
+enum rtl_desc_qsel rtl92c_map_hwqueue_to_fwqueue(u16 fc,
+							  unsigned int
+							  skb_queue);
+void rtl92c_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+				      struct sk_buff *skb,
+				      struct rtl_stats *pstats,
+				      struct rx_desc_92c *pdesc,
+				      struct rx_fwinfo_92c *p_drvinfo);
+
+/*---------------------------------------------------------------
+ *	Card disable functions
+ *---------------------------------------------------------------*/
+
+
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
new file mode 100644
index 000000000000..4e020e654e6b
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
@@ -0,0 +1,607 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../ps.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "table.h"
+
+u32 rtl92cu_phy_query_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, readback_value, bitshift;
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), "
+					       "rfpath(%#x), bitmask(%#x)\n",
+					       regaddr, rfpath, bitmask));
+	if (rtlphy->rf_mode != RF_OP_BY_FW) {
+		original_value = _rtl92c_phy_rf_serial_read(hw,
+							    rfpath, regaddr);
+	} else {
+		original_value = _rtl92c_phy_fw_rf_serial_read(hw,
+							       rfpath, regaddr);
+	}
+	bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
+	readback_value = (original_value & bitmask) >> bitshift;
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), rfpath(%#x), "
+		  "bitmask(%#x), original_value(%#x)\n",
+		  regaddr, rfpath, bitmask, original_value));
+	return readback_value;
+}
+
+void rtl92cu_phy_set_rf_reg(struct ieee80211_hw *hw,
+			   enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 original_value, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  regaddr, bitmask, data, rfpath));
+	if (rtlphy->rf_mode != RF_OP_BY_FW) {
+		if (bitmask != RFREG_OFFSET_MASK) {
+			original_value = _rtl92c_phy_rf_serial_read(hw,
+								    rfpath,
+								    regaddr);
+			bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
+			data =
+			    ((original_value & (~bitmask)) |
+			     (data << bitshift));
+		}
+		_rtl92c_phy_rf_serial_write(hw, rfpath, regaddr, data);
+	} else {
+		if (bitmask != RFREG_OFFSET_MASK) {
+			original_value = _rtl92c_phy_fw_rf_serial_read(hw,
+								       rfpath,
+								       regaddr);
+			bitshift = _rtl92c_phy_calculate_bit_shift(bitmask);
+			data =
+			    ((original_value & (~bitmask)) |
+			     (data << bitshift));
+		}
+		_rtl92c_phy_fw_rf_serial_write(hw, rfpath, regaddr, data);
+	}
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, ("regaddr(%#x), "
+					       "bitmask(%#x), data(%#x), rfpath(%#x)\n",
+					       regaddr, bitmask, data, rfpath));
+}
+
+bool rtl92cu_phy_mac_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool is92c = IS_92C_SERIAL(rtlhal->version);
+
+	rtstatus = _rtl92cu_phy_config_mac_with_headerfile(hw);
+	if (is92c && IS_HARDWARE_TYPE_8192CE(rtlhal))
+		rtl_write_byte(rtlpriv, 0x14, 0x71);
+	return rtstatus;
+}
+
+bool rtl92cu_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u16 regval;
+	u8 b_reg_hwparafile = 1;
+
+	_rtl92c_phy_init_bb_rf_register_definition(hw);
+	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
+	rtl_write_word(rtlpriv, REG_SYS_FUNC_EN, regval | BIT(13) |
+		       BIT(0) | BIT(1));
+	rtl_write_byte(rtlpriv, REG_AFE_PLL_CTRL, 0x83);
+	rtl_write_byte(rtlpriv, REG_AFE_PLL_CTRL + 1, 0xdb);
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
+	if (IS_HARDWARE_TYPE_8192CE(rtlhal)) {
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, FEN_PPLL | FEN_PCIEA |
+			       FEN_DIO_PCIE |	FEN_BB_GLB_RSTn | FEN_BBRSTB);
+	} else if (IS_HARDWARE_TYPE_8192CU(rtlhal)) {
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, FEN_USBA | FEN_USBD |
+			       FEN_BB_GLB_RSTn | FEN_BBRSTB);
+		rtl_write_byte(rtlpriv, REG_LDOHCI12_CTRL, 0x0f);
+	}
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
+	if (b_reg_hwparafile == 1)
+		rtstatus = _rtl92c_phy_bb8192c_config_parafile(hw);
+	return rtstatus;
+}
+
+bool _rtl92cu_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("Read Rtl819XMACPHY_Array\n"));
+	arraylength =  rtlphy->hwparam_tables[MAC_REG].length ;
+	ptrarray = rtlphy->hwparam_tables[MAC_REG].pdata;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 ("Img:RTL8192CEMAC_2T_ARRAY\n"));
+	for (i = 0; i < arraylength; i = i + 2)
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
+	return true;
+}
+
+bool _rtl92cu_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						  u8 configtype)
+{
+	int i;
+	u32 *phy_regarray_table;
+	u32 *agctab_array_table;
+	u16 phy_reg_arraylen, agctab_arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (IS_92C_SERIAL(rtlhal->version)) {
+		agctab_arraylen = rtlphy->hwparam_tables[AGCTAB_2T].length;
+		agctab_array_table =  rtlphy->hwparam_tables[AGCTAB_2T].pdata;
+		phy_reg_arraylen = rtlphy->hwparam_tables[PHY_REG_2T].length;
+		phy_regarray_table = rtlphy->hwparam_tables[PHY_REG_2T].pdata;
+	} else {
+		agctab_arraylen = rtlphy->hwparam_tables[AGCTAB_1T].length;
+		agctab_array_table =  rtlphy->hwparam_tables[AGCTAB_1T].pdata;
+		phy_reg_arraylen = rtlphy->hwparam_tables[PHY_REG_1T].length;
+		phy_regarray_table = rtlphy->hwparam_tables[PHY_REG_1T].pdata;
+	}
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_reg_arraylen; i = i + 2) {
+			if (phy_regarray_table[i] == 0xfe)
+				mdelay(50);
+			else if (phy_regarray_table[i] == 0xfd)
+				mdelay(5);
+			else if (phy_regarray_table[i] == 0xfc)
+				mdelay(1);
+			else if (phy_regarray_table[i] == 0xfb)
+				udelay(50);
+			else if (phy_regarray_table[i] == 0xfa)
+				udelay(5);
+			else if (phy_regarray_table[i] == 0xf9)
+				udelay(1);
+			rtl_set_bbreg(hw, phy_regarray_table[i], MASKDWORD,
+				      phy_regarray_table[i + 1]);
+			udelay(1);
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 ("The phy_regarray_table[0] is %x"
+				  " Rtl819XPHY_REGArray[1] is %x\n",
+				  phy_regarray_table[i],
+				  phy_regarray_table[i + 1]));
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+		for (i = 0; i < agctab_arraylen; i = i + 2) {
+			rtl_set_bbreg(hw, agctab_array_table[i], MASKDWORD,
+				      agctab_array_table[i + 1]);
+			udelay(1);
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 ("The agctab_array_table[0] is "
+				  "%x Rtl819XPHY_REGArray[1] is %x\n",
+				  agctab_array_table[i],
+				  agctab_array_table[i + 1]));
+		}
+	}
+	return true;
+}
+
+bool _rtl92cu_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						    u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	int i;
+	u32 *phy_regarray_table_pg;
+	u16 phy_regarray_pg_len;
+
+	rtlphy->pwrgroup_cnt = 0;
+	phy_regarray_pg_len = rtlphy->hwparam_tables[PHY_REG_PG].length;
+	phy_regarray_table_pg = rtlphy->hwparam_tables[PHY_REG_PG].pdata;
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_regarray_pg_len; i = i + 3) {
+			if (phy_regarray_table_pg[i] == 0xfe)
+				mdelay(50);
+			else if (phy_regarray_table_pg[i] == 0xfd)
+				mdelay(5);
+			else if (phy_regarray_table_pg[i] == 0xfc)
+				mdelay(1);
+			else if (phy_regarray_table_pg[i] == 0xfb)
+				udelay(50);
+			else if (phy_regarray_table_pg[i] == 0xfa)
+				udelay(5);
+			else if (phy_regarray_table_pg[i] == 0xf9)
+				udelay(1);
+			_rtl92c_store_pwrIndex_diffrate_offset(hw,
+						  phy_regarray_table_pg[i],
+						  phy_regarray_table_pg[i + 1],
+						  phy_regarray_table_pg[i + 2]);
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 ("configtype != BaseBand_Config_PHY_REG\n"));
+	}
+	return true;
+}
+
+bool rtl92cu_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath)
+{
+	int i;
+	u32 *radioa_array_table;
+	u32 *radiob_array_table;
+	u16 radioa_arraylen, radiob_arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (IS_92C_SERIAL(rtlhal->version)) {
+		radioa_arraylen = rtlphy->hwparam_tables[RADIOA_2T].length;
+		radioa_array_table = rtlphy->hwparam_tables[RADIOA_2T].pdata;
+		radiob_arraylen = rtlphy->hwparam_tables[RADIOB_2T].length;
+		radiob_array_table = rtlphy->hwparam_tables[RADIOB_2T].pdata;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Radio_A:RTL8192CERADIOA_2TARRAY\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Radio_B:RTL8192CE_RADIOB_2TARRAY\n"));
+	} else {
+		radioa_arraylen = rtlphy->hwparam_tables[RADIOA_1T].length;
+		radioa_array_table = rtlphy->hwparam_tables[RADIOA_1T].pdata;
+		radiob_arraylen = rtlphy->hwparam_tables[RADIOB_1T].length;
+		radiob_array_table = rtlphy->hwparam_tables[RADIOB_1T].pdata;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Radio_A:RTL8192CE_RADIOA_1TARRAY\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 ("Radio_B:RTL8192CE_RADIOB_1TARRAY\n"));
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("Radio No %x\n", rfpath));
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < radioa_arraylen; i = i + 2) {
+			if (radioa_array_table[i] == 0xfe)
+				mdelay(50);
+			else if (radioa_array_table[i] == 0xfd)
+				mdelay(5);
+			else if (radioa_array_table[i] == 0xfc)
+				mdelay(1);
+			else if (radioa_array_table[i] == 0xfb)
+				udelay(50);
+			else if (radioa_array_table[i] == 0xfa)
+				udelay(5);
+			else if (radioa_array_table[i] == 0xf9)
+				udelay(1);
+			else {
+				rtl_set_rfreg(hw, rfpath, radioa_array_table[i],
+					      RFREG_OFFSET_MASK,
+					      radioa_array_table[i + 1]);
+				udelay(1);
+			}
+		}
+		break;
+	case RF90_PATH_B:
+		for (i = 0; i < radiob_arraylen; i = i + 2) {
+			if (radiob_array_table[i] == 0xfe) {
+				mdelay(50);
+			} else if (radiob_array_table[i] == 0xfd)
+				mdelay(5);
+			else if (radiob_array_table[i] == 0xfc)
+				mdelay(1);
+			else if (radiob_array_table[i] == 0xfb)
+				udelay(50);
+			else if (radiob_array_table[i] == 0xfa)
+				udelay(5);
+			else if (radiob_array_table[i] == 0xf9)
+				udelay(1);
+			else {
+				rtl_set_rfreg(hw, rfpath, radiob_array_table[i],
+					      RFREG_OFFSET_MASK,
+					      radiob_array_table[i + 1]);
+				udelay(1);
+			}
+		}
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	return true;
+}
+
+void rtl92cu_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 reg_bw_opmode;
+	u8 reg_prsr_rsc;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 ("Switch to %s bandwidth\n",
+		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" : "40MHz"))
+	if (is_hal_stop(rtlhal)) {
+		rtlphy->set_bwmode_inprogress = false;
+		return;
+	}
+	reg_bw_opmode = rtl_read_byte(rtlpriv, REG_BWOPMODE);
+	reg_prsr_rsc = rtl_read_byte(rtlpriv, REG_RRSR + 2);
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		reg_bw_opmode |= BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		reg_bw_opmode &= ~BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		reg_prsr_rsc =
+		    (reg_prsr_rsc & 0x90) | (mac->cur_40_prime_sc << 5);
+		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_prsr_rsc);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		break;
+	}
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x0);
+		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x0);
+		rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x1);
+		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x1);
+		rtl_set_bbreg(hw, RCCK0_SYSTEM, BCCK_SIDEBAND,
+			      (mac->cur_40_prime_sc >> 1));
+		rtl_set_bbreg(hw, ROFDM1_LSTF, 0xC00, mac->cur_40_prime_sc);
+		rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 0);
+		rtl_set_bbreg(hw, 0x818, (BIT(26) | BIT(27)),
+			      (mac->cur_40_prime_sc ==
+			       HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		break;
+	}
+	rtl92cu_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtlphy->set_bwmode_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, ("<==\n"));
+}
+
+void rtl92cu_bb_block_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	mutex_lock(&rtlpriv->io.bb_mutex);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
+	mutex_unlock(&rtlpriv->io.bb_mutex);
+}
+
+void _rtl92cu_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+{
+	u8 tmpreg;
+	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
+
+	if ((tmpreg & 0x70) != 0)
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
+	else
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+
+	if ((tmpreg & 0x70) != 0) {
+		rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
+		if (is2t)
+			rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
+						  MASK12BITS);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
+			      (rf_a_mode & 0x8FFFF) | 0x10000);
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
+				      (rf_b_mode & 0x8FFFF) | 0x10000);
+	}
+	lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, lc_cal | 0x08000);
+	mdelay(100);
+	if ((tmpreg & 0x70) != 0) {
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
+				      rf_b_mode);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+	}
+}
+
+bool _rtl92cu_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					    enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool bresult = true;
+	u8 i, queue_id;
+	struct rtl8192_tx_ring *ring = NULL;
+
+	ppsc->set_rfpowerstate_inprogress = true;
+	switch (rfpwr_state) {
+	case ERFON:
+		if ((ppsc->rfpwr_state == ERFOFF) &&
+		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
+			bool rtstatus;
+			u32 InitializeCount = 0;
+
+			do {
+				InitializeCount++;
+				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+					 ("IPS Set eRf nic enable\n"));
+				rtstatus = rtl_ps_enable_nic(hw);
+			} while ((rtstatus != true)
+				 && (InitializeCount < 10));
+			RT_CLEAR_PS_LEVEL(ppsc,
+					  RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 ("Set ERFON sleeped:%d ms\n",
+				  jiffies_to_msecs(jiffies -
+						   ppsc->
+						   last_sleep_jiffies)));
+			ppsc->last_awake_jiffies = jiffies;
+			rtl92ce_phy_set_rf_on(hw);
+		}
+		if (mac->link_state == MAC80211_LINKED) {
+			rtlpriv->cfg->ops->led_control(hw,
+						       LED_CTL_LINK);
+		} else {
+			rtlpriv->cfg->ops->led_control(hw,
+						       LED_CTL_NO_LINK);
+		}
+		break;
+	case ERFOFF:
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0 ||
+				queue_id == BEACON_QUEUE) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 ("eRf Off/Sleep: %d times "
+					  "TcbBusyQueue[%d] "
+					  "=%d before doze!\n", (i + 1),
+					  queue_id,
+					  skb_queue_len(&ring->queue)));
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 ("\nERFOFF: %d times "
+					  "TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue)));
+				break;
+			}
+		}
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 ("IPS Set eRf nic disable\n"));
+			rtl_ps_disable_nic(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+				rtlpriv->cfg->ops->led_control(hw,
+							 LED_CTL_NO_LINK);
+			} else {
+				rtlpriv->cfg->ops->led_control(hw,
+							 LED_CTL_POWER_OFF);
+			}
+		}
+		break;
+	case ERFSLEEP:
+		if (ppsc->rfpwr_state == ERFOFF)
+			break;
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 ("eRf Off/Sleep: %d times "
+					  "TcbBusyQueue[%d] =%d before "
+					  "doze!\n", (i + 1), queue_id,
+					  skb_queue_len(&ring->queue)));
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 ("\n ERFSLEEP: %d times "
+					  "TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue)));
+				break;
+			}
+		}
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 ("Set ERFSLEEP awaked:%d ms\n",
+			  jiffies_to_msecs(jiffies -
+					   ppsc->last_awake_jiffies)));
+		ppsc->last_sleep_jiffies = jiffies;
+		_rtl92c_phy_set_rf_sleep(hw);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		bresult = false;
+		break;
+	}
+	if (bresult)
+		ppsc->rfpwr_state = rfpwr_state;
+	ppsc->set_rfpowerstate_inprogress = false;
+	return bresult;
+}
+
+bool rtl92cu_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				   enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool bresult = false;
+
+	if (rfpwr_state == ppsc->rfpwr_state)
+		return bresult;
+	bresult = _rtl92cu_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.h b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.h
new file mode 100644
index 000000000000..06299559ab68
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.h
@@ -0,0 +1,36 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../rtl8192ce/phy.h"
+
+void rtl92cu_bb_block_on(struct ieee80211_hw *hw);
+bool rtl8192_phy_check_is_legal_rfpath(struct ieee80211_hw *hw, u32 rfpath);
+void rtl92c_phy_set_io(struct ieee80211_hw *hw);
+bool _rtl92cu_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+bool rtl92cu_phy_bb_config(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/reg.h b/drivers/net/wireless/rtlwifi/rtl8192cu/reg.h
new file mode 100644
index 000000000000..7f1be614c998
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/reg.h
@@ -0,0 +1,30 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../rtl8192ce/reg.h"
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c
new file mode 100644
index 000000000000..1c79c226f145
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c
@@ -0,0 +1,493 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+
+static bool _rtl92c_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
+
+void rtl92cu_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	switch (bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | 0x0400);
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", bandwidth));
+		break;
+	}
+}
+
+void rtl92cu_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+				       u8 *ppowerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 tx_agc[2] = { 0, 0 }, tmpval = 0;
+	bool turbo_scanoff = false;
+	u8 idx1, idx2;
+	u8 *ptr;
+
+	if (rtlhal->interface == INTF_PCI) {
+		if (rtlefuse->eeprom_regulatory != 0)
+			turbo_scanoff = true;
+	} else {
+		if ((rtlefuse->eeprom_regulatory != 0) ||
+		    (rtlefuse->external_pa))
+			turbo_scanoff = true;
+	}
+	if (mac->act_scanning == true) {
+		tx_agc[RF90_PATH_A] = 0x3f3f3f3f;
+		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
+		if (turbo_scanoff) {
+			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+				tx_agc[idx1] = ppowerlevel[idx1] |
+				    (ppowerlevel[idx1] << 8) |
+				    (ppowerlevel[idx1] << 16) |
+				    (ppowerlevel[idx1] << 24);
+				if (rtlhal->interface == INTF_USB) {
+					if (tx_agc[idx1] > 0x20 &&
+					    rtlefuse->external_pa)
+						tx_agc[idx1] = 0x20;
+				}
+			}
+		}
+	} else {
+		if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+		    TXHIGHPWRLEVEL_LEVEL1) {
+			tx_agc[RF90_PATH_A] = 0x10101010;
+			tx_agc[RF90_PATH_B] = 0x10101010;
+		} else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+			   TXHIGHPWRLEVEL_LEVEL1) {
+			tx_agc[RF90_PATH_A] = 0x00000000;
+			tx_agc[RF90_PATH_B] = 0x00000000;
+		} else{
+			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+				tx_agc[idx1] = ppowerlevel[idx1] |
+				    (ppowerlevel[idx1] << 8) |
+				    (ppowerlevel[idx1] << 16) |
+				    (ppowerlevel[idx1] << 24);
+			}
+			if (rtlefuse->eeprom_regulatory == 0) {
+				tmpval = (rtlphy->mcs_txpwrlevel_origoffset
+					[0][6]) +
+					(rtlphy->mcs_txpwrlevel_origoffset
+					[0][7] <<  8);
+				tx_agc[RF90_PATH_A] += tmpval;
+				tmpval = (rtlphy->mcs_txpwrlevel_origoffset
+					[0][14]) +
+					(rtlphy->mcs_txpwrlevel_origoffset
+					[0][15] << 24);
+				tx_agc[RF90_PATH_B] += tmpval;
+			}
+		}
+	}
+	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+		ptr = (u8 *) (&(tx_agc[idx1]));
+		for (idx2 = 0; idx2 < 4; idx2++) {
+			if (*ptr > RF6052_MAX_TX_PWR)
+				*ptr = RF6052_MAX_TX_PWR;
+			ptr++;
+		}
+	}
+	tmpval = tx_agc[RF90_PATH_A] & 0xff;
+	rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, MASKBYTE1, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		("CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_A_CCK1_MCS32));
+
+	tmpval = tx_agc[RF90_PATH_A] >> 8;
+	if (mac->mode == WIRELESS_MODE_B)
+		tmpval = tmpval & 0xff00ffff;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		("CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_A_CCK2_11));
+	tmpval = tx_agc[RF90_PATH_B] >> 24;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0, tmpval);
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		("CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_A_CCK2_11));
+	tmpval = tx_agc[RF90_PATH_B] & 0x00ffffff;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, 0xffffff00, tmpval);
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		("CCK PWR 1~5.5M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK1_55_MCS32));
+}
+
+static void rtl92c_phy_get_power_base(struct ieee80211_hw *hw,
+				      u8 *ppowerlevel, u8 channel,
+				      u32 *ofdmbase, u32 *mcsbase)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 powerBase0, powerBase1;
+	u8 legacy_pwrdiff = 0, ht20_pwrdiff = 0;
+	u8 i, powerlevel[2];
+
+	for (i = 0; i < 2; i++) {
+		powerlevel[i] = ppowerlevel[i];
+		legacy_pwrdiff = rtlefuse->txpwr_legacyhtdiff[i][channel - 1];
+		powerBase0 = powerlevel[i] + legacy_pwrdiff;
+		powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) |
+		    (powerBase0 << 8) | powerBase0;
+		*(ofdmbase + i) = powerBase0;
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			(" [OFDM power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i)));
+	}
+	for (i = 0; i < 2; i++) {
+		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20) {
+			ht20_pwrdiff = rtlefuse->txpwr_ht20diff[i][channel - 1];
+			powerlevel[i] += ht20_pwrdiff;
+		}
+		powerBase1 = powerlevel[i];
+		powerBase1 = (powerBase1 << 24) |
+		    (powerBase1 << 16) | (powerBase1 << 8) | powerBase1;
+		*(mcsbase + i) = powerBase1;
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			(" [MCS power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i)));
+	}
+}
+
+static void _rtl92c_get_txpower_writeval_by_regulatory(struct ieee80211_hw *hw,
+						       u8 channel, u8 index,
+						       u32 *powerBase0,
+						       u32 *powerBase1,
+						       u32 *p_outwriteval)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 i, chnlgroup = 0, pwr_diff_limit[4];
+	u32 writeVal, customer_limit, rf;
+
+	for (rf = 0; rf < 2; rf++) {
+		switch (rtlefuse->eeprom_regulatory) {
+		case 0:
+			chnlgroup = 0;
+			writeVal = rtlphy->mcs_txpwrlevel_origoffset
+			    [chnlgroup][index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("RTK better performance,writeVal(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		case 1:
+			if (rtlphy->pwrgroup_cnt == 1)
+				chnlgroup = 0;
+			if (rtlphy->pwrgroup_cnt >= 3) {
+				if (channel <= 3)
+					chnlgroup = 0;
+				else if (channel >= 4 && channel <= 9)
+					chnlgroup = 1;
+				else if (channel > 9)
+					chnlgroup = 2;
+				if (rtlphy->current_chan_bw ==
+				    HT_CHANNEL_WIDTH_20)
+					chnlgroup++;
+				else
+					chnlgroup += 4;
+			}
+			writeVal = rtlphy->mcs_txpwrlevel_origoffset
+					[chnlgroup][index +
+					(rf ? 8 : 0)] +
+					((index < 2) ? powerBase0[rf] :
+					powerBase1[rf]);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Realtek regulatory, 20MHz, "
+				"writeVal(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		case 2:
+			writeVal = ((index < 2) ? powerBase0[rf] :
+				   powerBase1[rf]);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Better regulatory,writeVal(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		case 3:
+			chnlgroup = 0;
+			if (rtlphy->current_chan_bw ==
+			    HT_CHANNEL_WIDTH_20_40) {
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					("customer's limit, 40MHzrf(%c) = "
+					"0x%x\n", ((rf == 0) ? 'A' : 'B'),
+					rtlefuse->pwrgroup_ht40[rf]
+					[channel - 1]));
+			} else {
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					("customer's limit, 20MHz rf(%c) = "
+					"0x%x\n", ((rf == 0) ? 'A' : 'B'),
+					rtlefuse->pwrgroup_ht20[rf]
+					[channel - 1]));
+			}
+			for (i = 0; i < 4; i++) {
+				pwr_diff_limit[i] =
+				    (u8) ((rtlphy->mcs_txpwrlevel_origoffset
+				    [chnlgroup][index + (rf ? 8 : 0)]
+				    & (0x7f << (i * 8))) >> (i * 8));
+				if (rtlphy->current_chan_bw ==
+				    HT_CHANNEL_WIDTH_20_40) {
+					if (pwr_diff_limit[i] >
+					    rtlefuse->pwrgroup_ht40[rf]
+						[channel - 1])
+						pwr_diff_limit[i] = rtlefuse->
+						    pwrgroup_ht40[rf]
+						    [channel - 1];
+				} else {
+					if (pwr_diff_limit[i] >
+					    rtlefuse->pwrgroup_ht20[rf]
+						[channel - 1])
+						pwr_diff_limit[i] =
+						    rtlefuse->pwrgroup_ht20[rf]
+						    [channel - 1];
+				}
+			}
+			customer_limit = (pwr_diff_limit[3] << 24) |
+			    (pwr_diff_limit[2] << 16) |
+			    (pwr_diff_limit[1] << 8) | (pwr_diff_limit[0]);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Customer's limit rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), customer_limit));
+			writeVal = customer_limit + ((index < 2) ?
+				   powerBase0[rf] : powerBase1[rf]);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Customer, writeVal rf(%c)= 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		default:
+			chnlgroup = 0;
+			writeVal = rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
+				   [index + (rf ? 8 : 0)] + ((index < 2) ?
+				   powerBase0[rf] : powerBase1[rf]);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR, ("RTK better "
+				"performance, writeValrf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		}
+		if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+		    TXHIGHPWRLEVEL_LEVEL1)
+			writeVal = 0x14141414;
+		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+			 TXHIGHPWRLEVEL_LEVEL2)
+			writeVal = 0x00000000;
+		if (rtlpriv->dm.dynamic_txhighpower_lvl == TXHIGHPWRLEVEL_BT1)
+			writeVal = writeVal - 0x06060606;
+		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+			 TXHIGHPWRLEVEL_BT2)
+			writeVal = writeVal;
+		*(p_outwriteval + rf) = writeVal;
+	}
+}
+
+static void _rtl92c_write_ofdm_power_reg(struct ieee80211_hw *hw,
+					 u8 index, u32 *pValue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u16 regoffset_a[6] = {
+		RTXAGC_A_RATE18_06, RTXAGC_A_RATE54_24,
+		RTXAGC_A_MCS03_MCS00, RTXAGC_A_MCS07_MCS04,
+		RTXAGC_A_MCS11_MCS08, RTXAGC_A_MCS15_MCS12
+	};
+	u16 regoffset_b[6] = {
+		RTXAGC_B_RATE18_06, RTXAGC_B_RATE54_24,
+		RTXAGC_B_MCS03_MCS00, RTXAGC_B_MCS07_MCS04,
+		RTXAGC_B_MCS11_MCS08, RTXAGC_B_MCS15_MCS12
+	};
+	u8 i, rf, pwr_val[4];
+	u32 writeVal;
+	u16 regoffset;
+
+	for (rf = 0; rf < 2; rf++) {
+		writeVal = pValue[rf];
+		for (i = 0; i < 4; i++) {
+			pwr_val[i] = (u8)((writeVal & (0x7f << (i * 8))) >>
+					  (i * 8));
+			if (pwr_val[i] > RF6052_MAX_TX_PWR)
+				pwr_val[i] = RF6052_MAX_TX_PWR;
+		}
+		writeVal = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
+		    (pwr_val[1] << 8) | pwr_val[0];
+		if (rf == 0)
+			regoffset = regoffset_a[index];
+		else
+			regoffset = regoffset_b[index];
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeVal);
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			("Set 0x%x = %08x\n", regoffset, writeVal));
+		if (((get_rf_type(rtlphy) == RF_2T2R) &&
+		     (regoffset == RTXAGC_A_MCS15_MCS12 ||
+		      regoffset == RTXAGC_B_MCS15_MCS12)) ||
+		    ((get_rf_type(rtlphy) != RF_2T2R) &&
+		     (regoffset == RTXAGC_A_MCS07_MCS04 ||
+		      regoffset == RTXAGC_B_MCS07_MCS04))) {
+			writeVal = pwr_val[3];
+			if (regoffset == RTXAGC_A_MCS15_MCS12 ||
+			    regoffset == RTXAGC_A_MCS07_MCS04)
+				regoffset = 0xc90;
+			if (regoffset == RTXAGC_B_MCS15_MCS12 ||
+			    regoffset == RTXAGC_B_MCS07_MCS04)
+				regoffset = 0xc98;
+			for (i = 0; i < 3; i++) {
+				writeVal = (writeVal > 6) ? (writeVal - 6) : 0;
+				rtl_write_byte(rtlpriv, (u32)(regoffset + i),
+					      (u8)writeVal);
+			}
+		}
+	}
+}
+
+void rtl92cu_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					u8 *ppowerlevel, u8 channel)
+{
+	u32 writeVal[2], powerBase0[2], powerBase1[2];
+	u8 index = 0;
+
+	rtl92c_phy_get_power_base(hw, ppowerlevel,
+				  channel, &powerBase0[0], &powerBase1[0]);
+	for (index = 0; index < 6; index++) {
+		_rtl92c_get_txpower_writeval_by_regulatory(hw,
+							   channel, index,
+							   &powerBase0[0],
+							   &powerBase1[0],
+							   &writeVal[0]);
+		_rtl92c_write_ofdm_power_reg(hw, index, &writeVal[0]);
+	}
+}
+
+bool rtl92cu_phy_rf6052_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool rtstatus = true;
+	u8 b_reg_hwparafile = 1;
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rtlphy->num_total_rfpath = 1;
+	else
+		rtlphy->num_total_rfpath = 2;
+	if (b_reg_hwparafile == 1)
+		rtstatus = _rtl92c_phy_rf6052_config_parafile(hw);
+	return rtstatus;
+}
+
+static bool _rtl92c_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 u4_regvalue = 0;
+	u8 rfpath;
+	bool rtstatus = true;
+	struct bb_reg_def *pphyreg;
+
+	for (rfpath = 0; rfpath < rtlphy->num_total_rfpath; rfpath++) {
+		pphyreg = &rtlphy->phyreg_def[rfpath];
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV << 16);
+			break;
+		}
+		rtl_set_bbreg(hw, pphyreg->rfintfe, BRFSI_RFENV << 16, 0x1);
+		udelay(1);
+		rtl_set_bbreg(hw, pphyreg->rfintfo, BRFSI_RFENV, 0x1);
+		udelay(1);
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2,
+			      B3WIREADDREAALENGTH, 0x0);
+		udelay(1);
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2, B3WIREDATALENGTH, 0x0);
+		udelay(1);
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtstatus = rtl92cu_phy_config_rf_with_headerfile(hw,
+					(enum radio_path) rfpath);
+			break;
+		case RF90_PATH_B:
+			rtstatus = rtl92cu_phy_config_rf_with_headerfile(hw,
+					(enum radio_path) rfpath);
+			break;
+		case RF90_PATH_C:
+			break;
+		case RF90_PATH_D:
+			break;
+		}
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			rtl_set_bbreg(hw, pphyreg->rfintfs,
+				      BRFSI_RFENV, u4_regvalue);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			rtl_set_bbreg(hw, pphyreg->rfintfs,
+				      BRFSI_RFENV << 16, u4_regvalue);
+			break;
+		}
+		if (rtstatus != true) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 ("Radio[%d] Fail!!", rfpath));
+			goto phy_rf_cfg_fail;
+		}
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, ("<---\n"));
+	return rtstatus;
+phy_rf_cfg_fail:
+	return rtstatus;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h
new file mode 100644
index 000000000000..86c2728cfa00
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CU_RF_H__
+#define __RTL92CU_RF_H__
+
+#define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
+#define RF6052_MAX_PATH			2
+
+extern void rtl92cu_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+					    u8 bandwidth);
+extern void rtl92c_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					      u8 *ppowerlevel);
+extern void rtl92c_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					       u8 *ppowerlevel, u8 channel);
+bool rtl92cu_phy_rf6052_config(struct ieee80211_hw *hw);
+bool rtl92cu_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
new file mode 100644
index 000000000000..71244a38d49e
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
@@ -0,0 +1,336 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../core.h"
+#include "../usb.h"
+#include "../efuse.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "mac.h"
+#include "dm.h"
+#include "rf.h"
+#include "sw.h"
+#include "trx.h"
+#include "led.h"
+#include "hw.h"
+#include <linux/vmalloc.h>
+
+MODULE_AUTHOR("Georgia		<georgia@realtek.com>");
+MODULE_AUTHOR("Ziv Huang	<ziv_huang@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 8192C/8188C 802.11n USB wireless");
+MODULE_FIRMWARE("rtlwifi/rtl8192cufw.bin");
+
+static int rtl92cu_init_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_flag = 0;
+	rtlpriv->dm.disable_framebursting = 0;
+	rtlpriv->dm.thermalvalue = 0;
+	rtlpriv->rtlhal.pfirmware = vmalloc(0x4000);
+	if (!rtlpriv->rtlhal.pfirmware) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("Can't alloc buffer for fw.\n"));
+		return 1;
+	}
+	return 0;
+}
+
+static void rtl92cu_deinit_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->rtlhal.pfirmware) {
+		vfree(rtlpriv->rtlhal.pfirmware);
+		rtlpriv->rtlhal.pfirmware = NULL;
+	}
+}
+
+static struct rtl_hal_ops rtl8192cu_hal_ops = {
+	.init_sw_vars = rtl92cu_init_sw_vars,
+	.deinit_sw_vars = rtl92cu_deinit_sw_vars,
+	.read_chip_version = rtl92c_read_chip_version,
+	.read_eeprom_info = rtl92cu_read_eeprom_info,
+	.enable_interrupt = rtl92c_enable_interrupt,
+	.disable_interrupt = rtl92c_disable_interrupt,
+	.hw_init = rtl92cu_hw_init,
+	.hw_disable = rtl92cu_card_disable,
+	.set_network_type = rtl92cu_set_network_type,
+	.set_chk_bssid = rtl92cu_set_check_bssid,
+	.set_qos = rtl92c_set_qos,
+	.set_bcn_reg = rtl92cu_set_beacon_related_registers,
+	.set_bcn_intv = rtl92cu_set_beacon_interval,
+	.update_interrupt_mask = rtl92cu_update_interrupt_mask,
+	.get_hw_reg = rtl92cu_get_hw_reg,
+	.set_hw_reg = rtl92cu_set_hw_reg,
+	.update_rate_table = rtl92cu_update_hal_rate_table,
+	.update_rate_mask = rtl92cu_update_hal_rate_mask,
+	.fill_tx_desc = rtl92cu_tx_fill_desc,
+	.fill_fake_txdesc = rtl92cu_fill_fake_txdesc,
+	.fill_tx_cmddesc = rtl92cu_tx_fill_cmddesc,
+	.cmd_send_packet = rtl92cu_cmd_send_packet,
+	.query_rx_desc = rtl92cu_rx_query_desc,
+	.set_channel_access = rtl92cu_update_channel_access_setting,
+	.radio_onoff_checking = rtl92cu_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl92c_phy_set_bw_mode,
+	.switch_channel = rtl92c_phy_sw_chnl,
+	.dm_watchdog = rtl92c_dm_watchdog,
+	.scan_operation_backup = rtl92c_phy_scan_operation_backup,
+	.set_rf_power_state = rtl92cu_phy_set_rf_power_state,
+	.led_control = rtl92cu_led_control,
+	.enable_hw_sec = rtl92cu_enable_hw_security_config,
+	.set_key = rtl92c_set_key,
+	.init_sw_leds = rtl92cu_init_sw_leds,
+	.deinit_sw_leds = rtl92cu_deinit_sw_leds,
+	.get_bbreg = rtl92c_phy_query_bb_reg,
+	.set_bbreg = rtl92c_phy_set_bb_reg,
+	.get_rfreg = rtl92cu_phy_query_rf_reg,
+	.set_rfreg = rtl92cu_phy_set_rf_reg,
+	.phy_rf6052_config = rtl92cu_phy_rf6052_config,
+	.phy_rf6052_set_cck_txpower = rtl92cu_phy_rf6052_set_cck_txpower,
+	.phy_rf6052_set_ofdm_txpower = rtl92cu_phy_rf6052_set_ofdm_txpower,
+	.config_bb_with_headerfile = _rtl92cu_phy_config_bb_with_headerfile,
+	.config_bb_with_pgheaderfile = _rtl92cu_phy_config_bb_with_pgheaderfile,
+	.phy_lc_calibrate = _rtl92cu_phy_lc_calibrate,
+	.phy_set_bw_mode_callback = rtl92cu_phy_set_bw_mode_callback,
+	.dm_dynamic_txpower = rtl92cu_dm_dynamic_txpower,
+};
+
+static struct rtl_mod_params rtl92cu_mod_params = {
+	.sw_crypto = 0,
+};
+
+static struct rtl_hal_usbint_cfg rtl92cu_interface_cfg = {
+	/* rx */
+	.in_ep_num = RTL92C_USB_BULK_IN_NUM,
+	.rx_urb_num = RTL92C_NUM_RX_URBS,
+	.rx_max_size = RTL92C_SIZE_MAX_RX_BUFFER,
+	.usb_rx_hdl = rtl8192cu_rx_hdl,
+	.usb_rx_segregate_hdl = NULL, /* rtl8192c_rx_segregate_hdl; */
+	/* tx */
+	.usb_tx_cleanup = rtl8192c_tx_cleanup,
+	.usb_tx_post_hdl = rtl8192c_tx_post_hdl,
+	.usb_tx_aggregate_hdl = rtl8192c_tx_aggregate_hdl,
+	/* endpoint mapping */
+	.usb_endpoint_mapping = rtl8192cu_endpoint_mapping,
+	.usb_mq_to_hwq = rtl8192cu_mq_to_hwq,
+};
+
+static struct rtl_hal_cfg rtl92cu_hal_cfg = {
+	.name = "rtl92c_usb",
+	.fw_name = "rtlwifi/rtl8192cufw.bin",
+	.ops = &rtl8192cu_hal_ops,
+	.mod_params = &rtl92cu_mod_params,
+	.usb_interface_cfg = &rtl92cu_interface_cfg,
+
+	.maps[SYS_ISO_CTRL] = REG_SYS_ISO_CTRL,
+	.maps[SYS_FUNC_EN] = REG_SYS_FUNC_EN,
+	.maps[SYS_CLK] = REG_SYS_CLKR,
+	.maps[MAC_RCR_AM] = AM,
+	.maps[MAC_RCR_AB] = AB,
+	.maps[MAC_RCR_ACRC32] = ACRC32,
+	.maps[MAC_RCR_ACF] = ACF,
+	.maps[MAC_RCR_AAP] = AAP,
+
+	.maps[EFUSE_TEST] = REG_EFUSE_TEST,
+	.maps[EFUSE_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_CLK] = 0,
+	.maps[EFUSE_CLK_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_PWC_EV12V] = PWC_EV12V,
+	.maps[EFUSE_FEN_ELDR] = FEN_ELDR,
+	.maps[EFUSE_LOADER_CLK_EN] = LOADER_CLK_EN,
+	.maps[EFUSE_ANA8M] = EFUSE_ANA8M,
+	.maps[EFUSE_HWSET_MAX_SIZE] = HWSET_MAX_SIZE,
+	.maps[EFUSE_MAX_SECTION_MAP] = EFUSE_MAX_SECTION,
+	.maps[EFUSE_REAL_CONTENT_SIZE] = EFUSE_REAL_CONTENT_LEN,
+
+	.maps[RWCAM] = REG_CAMCMD,
+	.maps[WCAMI] = REG_CAMWRITE,
+	.maps[RCAMO] = REG_CAMREAD,
+	.maps[CAMDBG] = REG_CAMDBG,
+	.maps[SECR] = REG_SECCFG,
+	.maps[SEC_CAM_NONE] = CAM_NONE,
+	.maps[SEC_CAM_WEP40] = CAM_WEP40,
+	.maps[SEC_CAM_TKIP] = CAM_TKIP,
+	.maps[SEC_CAM_AES] = CAM_AES,
+	.maps[SEC_CAM_WEP104] = CAM_WEP104,
+
+	.maps[RTL_IMR_BCNDMAINT6] = IMR_BCNDMAINT6,
+	.maps[RTL_IMR_BCNDMAINT5] = IMR_BCNDMAINT5,
+	.maps[RTL_IMR_BCNDMAINT4] = IMR_BCNDMAINT4,
+	.maps[RTL_IMR_BCNDMAINT3] = IMR_BCNDMAINT3,
+	.maps[RTL_IMR_BCNDMAINT2] = IMR_BCNDMAINT2,
+	.maps[RTL_IMR_BCNDMAINT1] = IMR_BCNDMAINT1,
+	.maps[RTL_IMR_BCNDOK8] = IMR_BCNDOK8,
+	.maps[RTL_IMR_BCNDOK7] = IMR_BCNDOK7,
+	.maps[RTL_IMR_BCNDOK6] = IMR_BCNDOK6,
+	.maps[RTL_IMR_BCNDOK5] = IMR_BCNDOK5,
+	.maps[RTL_IMR_BCNDOK4] = IMR_BCNDOK4,
+	.maps[RTL_IMR_BCNDOK3] = IMR_BCNDOK3,
+	.maps[RTL_IMR_BCNDOK2] = IMR_BCNDOK2,
+	.maps[RTL_IMR_BCNDOK1] = IMR_BCNDOK1,
+	.maps[RTL_IMR_TIMEOUT2] = IMR_TIMEOUT2,
+	.maps[RTL_IMR_TIMEOUT1] = IMR_TIMEOUT1,
+
+	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
+	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
+	.maps[RTL_IMR_BcnInt] = IMR_BCNINT,
+	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
+	.maps[RTL_IMR_RDU] = IMR_RDU,
+	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
+	.maps[RTL_IMR_BDOK] = IMR_BDOK,
+	.maps[RTL_IMR_MGNTDOK] = IMR_MGNTDOK,
+	.maps[RTL_IMR_TBDER] = IMR_TBDER,
+	.maps[RTL_IMR_HIGHDOK] = IMR_HIGHDOK,
+	.maps[RTL_IMR_TBDOK] = IMR_TBDOK,
+	.maps[RTL_IMR_BKDOK] = IMR_BKDOK,
+	.maps[RTL_IMR_BEDOK] = IMR_BEDOK,
+	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
+	.maps[RTL_IMR_VODOK] = IMR_VODOK,
+	.maps[RTL_IMR_ROK] = IMR_ROK,
+	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNINT | IMR_TBDOK | IMR_TBDER),
+
+	.maps[RTL_RC_CCK_RATE1M] = DESC92C_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC92C_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC92C_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC92C_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC92C_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC92C_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC92C_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC92C_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC92C_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC92C_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC92C_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC92C_RATE54M,
+	.maps[RTL_RC_HT_RATEMCS7] = DESC92C_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC92C_RATEMCS15,
+};
+
+#define USB_VENDER_ID_REALTEK		0x0bda
+
+/* 2010-10-19 DID_USB_V3.4 */
+static struct usb_device_id rtl8192c_usb_ids[] = {
+
+	/*=== Realtek demoboard ===*/
+	/* Default ID */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8191, rtl92cu_hal_cfg)},
+
+	/****** 8188CU ********/
+	/* 8188CE-VAU USB minCard */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8170, rtl92cu_hal_cfg)},
+	/* 8188cu 1*1 dongle */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8176, rtl92cu_hal_cfg)},
+	/* 8188cu 1*1 dongle, (b/g mode only) */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8177, rtl92cu_hal_cfg)},
+	/* 8188cu Slim Solo */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x817a, rtl92cu_hal_cfg)},
+	/* 8188cu Slim Combo */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x817b, rtl92cu_hal_cfg)},
+	/* 8188RU High-power USB Dongle */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x817d, rtl92cu_hal_cfg)},
+	/* 8188CE-VAU USB minCard (b/g mode only) */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x817e, rtl92cu_hal_cfg)},
+	/* 8188 Combo for BC4 */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8754, rtl92cu_hal_cfg)},
+
+	/****** 8192CU ********/
+	/* 8191cu 1*2 */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x8177, rtl92cu_hal_cfg)},
+	/* 8192cu 2*2 */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x817b, rtl92cu_hal_cfg)},
+	/* 8192CE-VAU USB minCard */
+	{RTL_USB_DEVICE(USB_VENDER_ID_REALTEK, 0x817c, rtl92cu_hal_cfg)},
+
+	/*=== Customer ID ===*/
+	/****** 8188CU ********/
+	{RTL_USB_DEVICE(0x050d, 0x1102, rtl92cu_hal_cfg)}, /*Belkin - Edimax*/
+	{RTL_USB_DEVICE(0x06f8, 0xe033, rtl92cu_hal_cfg)}, /*Hercules - Edimax*/
+	{RTL_USB_DEVICE(0x07b8, 0x8188, rtl92cu_hal_cfg)}, /*Abocom - Abocom*/
+	{RTL_USB_DEVICE(0x07b8, 0x8189, rtl92cu_hal_cfg)}, /*Funai - Abocom*/
+	{RTL_USB_DEVICE(0x0Df6, 0x0052, rtl92cu_hal_cfg)}, /*Sitecom - Edimax*/
+	{RTL_USB_DEVICE(0x0eb0, 0x9071, rtl92cu_hal_cfg)}, /*NO Brand - Etop*/
+	/* HP - Lite-On ,8188CUS Slim Combo */
+	{RTL_USB_DEVICE(0x103c, 0x1629, rtl92cu_hal_cfg)},
+	{RTL_USB_DEVICE(0x2001, 0x3308, rtl92cu_hal_cfg)}, /*D-Link - Alpha*/
+	{RTL_USB_DEVICE(0x2019, 0xab2a, rtl92cu_hal_cfg)}, /*Planex - Abocom*/
+	{RTL_USB_DEVICE(0x2019, 0xed17, rtl92cu_hal_cfg)}, /*PCI - Edimax*/
+	{RTL_USB_DEVICE(0x20f4, 0x648b, rtl92cu_hal_cfg)}, /*TRENDnet - Cameo*/
+	{RTL_USB_DEVICE(0x7392, 0x7811, rtl92cu_hal_cfg)}, /*Edimax - Edimax*/
+	{RTL_USB_DEVICE(0x3358, 0x13d3, rtl92cu_hal_cfg)}, /*Azwave 8188CE-VAU*/
+	/* Russian customer -Azwave (8188CE-VAU  b/g mode only) */
+	{RTL_USB_DEVICE(0x3359, 0x13d3, rtl92cu_hal_cfg)},
+
+	/****** 8192CU ********/
+	{RTL_USB_DEVICE(0x0586, 0x341f, rtl92cu_hal_cfg)}, /*Zyxel -Abocom*/
+	{RTL_USB_DEVICE(0x07aa, 0x0056, rtl92cu_hal_cfg)}, /*ATKK-Gemtek*/
+	{RTL_USB_DEVICE(0x07b8, 0x8178, rtl92cu_hal_cfg)}, /*Funai -Abocom*/
+	{RTL_USB_DEVICE(0x07b8, 0x8178, rtl92cu_hal_cfg)}, /*Abocom -Abocom*/
+	{RTL_USB_DEVICE(0x2001, 0x3307, rtl92cu_hal_cfg)}, /*D-Link-Cameo*/
+	{RTL_USB_DEVICE(0x2001, 0x3309, rtl92cu_hal_cfg)}, /*D-Link-Alpha*/
+	{RTL_USB_DEVICE(0x2001, 0x330a, rtl92cu_hal_cfg)}, /*D-Link-Alpha*/
+	{RTL_USB_DEVICE(0x2019, 0xab2b, rtl92cu_hal_cfg)}, /*Planex -Abocom*/
+	{RTL_USB_DEVICE(0x7392, 0x7822, rtl92cu_hal_cfg)}, /*Edimax -Edimax*/
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, rtl8192c_usb_ids);
+
+static struct usb_driver rtl8192cu_driver = {
+	.name = "rtl8192cu",
+	.probe = rtl_usb_probe,
+	.disconnect = rtl_usb_disconnect,
+	.id_table = rtl8192c_usb_ids,
+
+#ifdef CONFIG_PM
+	/* .suspend = rtl_usb_suspend, */
+	/* .resume = rtl_usb_resume, */
+	/* .reset_resume = rtl8192c_resume, */
+#endif /* CONFIG_PM */
+#ifdef CONFIG_AUTOSUSPEND
+	.supports_autosuspend = 1,
+#endif
+};
+
+static int __init rtl8192cu_init(void)
+{
+	return usb_register(&rtl8192cu_driver);
+}
+
+static void __exit rtl8192cu_exit(void)
+{
+	usb_deregister(&rtl8192cu_driver);
+}
+
+module_init(rtl8192cu_init);
+module_exit(rtl8192cu_exit);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.h b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.h
new file mode 100644
index 000000000000..43b1177924ab
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.h
@@ -0,0 +1,53 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CU_SW_H__
+#define __RTL92CU_SW_H__
+
+#define EFUSE_MAX_SECTION	16
+
+void rtl92cu_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					u8 *powerlevel);
+void rtl92cu_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					u8 *ppowerlevel, u8 channel);
+bool _rtl92cu_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+					    u8 configtype);
+bool _rtl92cu_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						    u8 configtype);
+void _rtl92cu_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t);
+void rtl92cu_phy_set_rf_reg(struct ieee80211_hw *hw,
+			   enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data);
+bool rtl92cu_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				   enum rf_pwrstate rfpwr_state);
+u32 rtl92cu_phy_query_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 regaddr, u32 bitmask);
+void rtl92cu_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/table.c b/drivers/net/wireless/rtlwifi/rtl8192cu/table.c
new file mode 100644
index 000000000000..d57ef5e88a9e
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/table.c
@@ -0,0 +1,1888 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "table.h"
+
+u32 RTL8192CUPHY_REG_2TARRAY[RTL8192CUPHY_REG_2TARRAY_LENGTH] = {
+	0x024, 0x0011800f,
+	0x028, 0x00ffdb83,
+	0x800, 0x80040002,
+	0x804, 0x00000003,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000000a,
+	0x810, 0x10005388,
+	0x814, 0x020c3d10,
+	0x818, 0x02200385,
+	0x81c, 0x00000000,
+	0x820, 0x01000100,
+	0x824, 0x00390004,
+	0x828, 0x01000100,
+	0x82c, 0x00390004,
+	0x830, 0x27272727,
+	0x834, 0x27272727,
+	0x838, 0x27272727,
+	0x83c, 0x27272727,
+	0x840, 0x00010000,
+	0x844, 0x00010000,
+	0x848, 0x27272727,
+	0x84c, 0x27272727,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x569a569a,
+	0x85c, 0x0c1b25a4,
+	0x860, 0x66e60230,
+	0x864, 0x061f0130,
+	0x868, 0x27272727,
+	0x86c, 0x2b2b2b27,
+	0x870, 0x07000700,
+	0x874, 0x22184000,
+	0x878, 0x08080808,
+	0x87c, 0x00000000,
+	0x880, 0xc0083070,
+	0x884, 0x000004d5,
+	0x888, 0x00000000,
+	0x88c, 0xcc0000c0,
+	0x890, 0x00000800,
+	0x894, 0xfffffffe,
+	0x898, 0x40302010,
+	0x89c, 0x00706050,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x81121313,
+	0xa00, 0x00d047c8,
+	0xa04, 0x80ff000c,
+	0xa08, 0x8c838300,
+	0xa0c, 0x2e68120f,
+	0xa10, 0x9500bb78,
+	0xa14, 0x11144028,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140f00,
+	0xa20, 0x1a1b0000,
+	0xa24, 0x090e1317,
+	0xa28, 0x00000204,
+	0xa2c, 0x00d30000,
+	0xa70, 0x101fbf00,
+	0xa74, 0x00000007,
+	0xc00, 0x48071d40,
+	0xc04, 0x03a05633,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08800000,
+	0xc1c, 0x40000100,
+	0xc20, 0x00000000,
+	0xc24, 0x00000000,
+	0xc28, 0x00000000,
+	0xc2c, 0x00000000,
+	0xc30, 0x69e9ac44,
+	0xc34, 0x469652cf,
+	0xc38, 0x49795994,
+	0xc3c, 0x0a97971c,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020107,
+	0xc4c, 0x007f037f,
+	0xc50, 0x6954341e,
+	0xc54, 0x43bc0094,
+	0xc58, 0x6954341e,
+	0xc5c, 0x433c0094,
+	0xc60, 0x00000000,
+	0xc64, 0x5116848b,
+	0xc68, 0x47c00bff,
+	0xc6c, 0x00000036,
+	0xc70, 0x2c7f000d,
+	0xc74, 0x0186115b,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b99612,
+	0xc80, 0x40000100,
+	0xc84, 0x20f60000,
+	0xc88, 0x40000100,
+	0xc8c, 0x20200000,
+	0xc90, 0x00121820,
+	0xc94, 0x00000000,
+	0xc98, 0x00121820,
+	0xc9c, 0x00007f7f,
+	0xca0, 0x00000000,
+	0xca4, 0x00000080,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x28000000,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xce4, 0x00000000,
+	0xce8, 0x37644302,
+	0xcec, 0x2f97d40c,
+	0xd00, 0x00080740,
+	0xd04, 0x00020403,
+	0xd08, 0x0000907f,
+	0xd0c, 0x20010201,
+	0xd10, 0xa0633333,
+	0xd14, 0x3333bc43,
+	0xd18, 0x7a8f5b6b,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x80608000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x00000000,
+	0xd58, 0x00000000,
+	0xd5c, 0x30032064,
+	0xd60, 0x4653de68,
+	0xd64, 0x04518a3c,
+	0xd68, 0x00002101,
+	0xd6c, 0x2a201c16,
+	0xd70, 0x1812362e,
+	0xd74, 0x322c2220,
+	0xd78, 0x000e3c24,
+	0xe00, 0x2a2a2a2a,
+	0xe04, 0x2a2a2a2a,
+	0xe08, 0x03902a2a,
+	0xe10, 0x2a2a2a2a,
+	0xe14, 0x2a2a2a2a,
+	0xe18, 0x2a2a2a2a,
+	0xe1c, 0x2a2a2a2a,
+	0xe28, 0x00000000,
+	0xe30, 0x1000dc1f,
+	0xe34, 0x10008c1f,
+	0xe38, 0x02140102,
+	0xe3c, 0x681604c2,
+	0xe40, 0x01007c00,
+	0xe44, 0x01004800,
+	0xe48, 0xfb000000,
+	0xe4c, 0x000028d1,
+	0xe50, 0x1000dc1f,
+	0xe54, 0x10008c1f,
+	0xe58, 0x02140102,
+	0xe5c, 0x28160d05,
+	0xe60, 0x00000010,
+	0xe68, 0x001b25a4,
+	0xe6c, 0x63db25a4,
+	0xe70, 0x63db25a4,
+	0xe74, 0x0c1b25a4,
+	0xe78, 0x0c1b25a4,
+	0xe7c, 0x0c1b25a4,
+	0xe80, 0x0c1b25a4,
+	0xe84, 0x63db25a4,
+	0xe88, 0x0c1b25a4,
+	0xe8c, 0x63db25a4,
+	0xed0, 0x63db25a4,
+	0xed4, 0x63db25a4,
+	0xed8, 0x63db25a4,
+	0xedc, 0x001b25a4,
+	0xee0, 0x001b25a4,
+	0xeec, 0x6fdb25a4,
+	0xf14, 0x00000003,
+	0xf4c, 0x00000000,
+	0xf00, 0x00000300,
+};
+
+u32 RTL8192CUPHY_REG_1TARRAY[RTL8192CUPHY_REG_1TARRAY_LENGTH] = {
+	0x024, 0x0011800f,
+	0x028, 0x00ffdb83,
+	0x800, 0x80040000,
+	0x804, 0x00000001,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000000a,
+	0x810, 0x10005388,
+	0x814, 0x020c3d10,
+	0x818, 0x02200385,
+	0x81c, 0x00000000,
+	0x820, 0x01000100,
+	0x824, 0x00390004,
+	0x828, 0x00000000,
+	0x82c, 0x00000000,
+	0x830, 0x00000000,
+	0x834, 0x00000000,
+	0x838, 0x00000000,
+	0x83c, 0x00000000,
+	0x840, 0x00010000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84c, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x569a569a,
+	0x85c, 0x001b25a4,
+	0x860, 0x66e60230,
+	0x864, 0x061f0130,
+	0x868, 0x00000000,
+	0x86c, 0x32323200,
+	0x870, 0x07000700,
+	0x874, 0x22004000,
+	0x878, 0x00000808,
+	0x87c, 0x00000000,
+	0x880, 0xc0083070,
+	0x884, 0x000004d5,
+	0x888, 0x00000000,
+	0x88c, 0xccc000c0,
+	0x890, 0x00000800,
+	0x894, 0xfffffffe,
+	0x898, 0x40302010,
+	0x89c, 0x00706050,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x81121111,
+	0xa00, 0x00d047c8,
+	0xa04, 0x80ff000c,
+	0xa08, 0x8c838300,
+	0xa0c, 0x2e68120f,
+	0xa10, 0x9500bb78,
+	0xa14, 0x11144028,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140f00,
+	0xa20, 0x1a1b0000,
+	0xa24, 0x090e1317,
+	0xa28, 0x00000204,
+	0xa2c, 0x00d30000,
+	0xa70, 0x101fbf00,
+	0xa74, 0x00000007,
+	0xc00, 0x48071d40,
+	0xc04, 0x03a05611,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08800000,
+	0xc1c, 0x40000100,
+	0xc20, 0x00000000,
+	0xc24, 0x00000000,
+	0xc28, 0x00000000,
+	0xc2c, 0x00000000,
+	0xc30, 0x69e9ac44,
+	0xc34, 0x469652cf,
+	0xc38, 0x49795994,
+	0xc3c, 0x0a97971c,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020107,
+	0xc4c, 0x007f037f,
+	0xc50, 0x6954341e,
+	0xc54, 0x43bc0094,
+	0xc58, 0x6954341e,
+	0xc5c, 0x433c0094,
+	0xc60, 0x00000000,
+	0xc64, 0x5116848b,
+	0xc68, 0x47c00bff,
+	0xc6c, 0x00000036,
+	0xc70, 0x2c7f000d,
+	0xc74, 0x018610db,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b91612,
+	0xc80, 0x40000100,
+	0xc84, 0x20f60000,
+	0xc88, 0x40000100,
+	0xc8c, 0x20200000,
+	0xc90, 0x00121820,
+	0xc94, 0x00000000,
+	0xc98, 0x00121820,
+	0xc9c, 0x00007f7f,
+	0xca0, 0x00000000,
+	0xca4, 0x00000080,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x28000000,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xce4, 0x00000000,
+	0xce8, 0x37644302,
+	0xcec, 0x2f97d40c,
+	0xd00, 0x00080740,
+	0xd04, 0x00020401,
+	0xd08, 0x0000907f,
+	0xd0c, 0x20010201,
+	0xd10, 0xa0633333,
+	0xd14, 0x3333bc43,
+	0xd18, 0x7a8f5b6b,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x80608000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x00000000,
+	0xd58, 0x00000000,
+	0xd5c, 0x30032064,
+	0xd60, 0x4653de68,
+	0xd64, 0x04518a3c,
+	0xd68, 0x00002101,
+	0xd6c, 0x2a201c16,
+	0xd70, 0x1812362e,
+	0xd74, 0x322c2220,
+	0xd78, 0x000e3c24,
+	0xe00, 0x2a2a2a2a,
+	0xe04, 0x2a2a2a2a,
+	0xe08, 0x03902a2a,
+	0xe10, 0x2a2a2a2a,
+	0xe14, 0x2a2a2a2a,
+	0xe18, 0x2a2a2a2a,
+	0xe1c, 0x2a2a2a2a,
+	0xe28, 0x00000000,
+	0xe30, 0x1000dc1f,
+	0xe34, 0x10008c1f,
+	0xe38, 0x02140102,
+	0xe3c, 0x681604c2,
+	0xe40, 0x01007c00,
+	0xe44, 0x01004800,
+	0xe48, 0xfb000000,
+	0xe4c, 0x000028d1,
+	0xe50, 0x1000dc1f,
+	0xe54, 0x10008c1f,
+	0xe58, 0x02140102,
+	0xe5c, 0x28160d05,
+	0xe60, 0x00000008,
+	0xe68, 0x001b25a4,
+	0xe6c, 0x631b25a0,
+	0xe70, 0x631b25a0,
+	0xe74, 0x081b25a0,
+	0xe78, 0x081b25a0,
+	0xe7c, 0x081b25a0,
+	0xe80, 0x081b25a0,
+	0xe84, 0x631b25a0,
+	0xe88, 0x081b25a0,
+	0xe8c, 0x631b25a0,
+	0xed0, 0x631b25a0,
+	0xed4, 0x631b25a0,
+	0xed8, 0x631b25a0,
+	0xedc, 0x001b25a0,
+	0xee0, 0x001b25a0,
+	0xeec, 0x6b1b25a0,
+	0xf14, 0x00000003,
+	0xf4c, 0x00000000,
+	0xf00, 0x00000300,
+};
+
+u32 RTL8192CUPHY_REG_ARRAY_PG[RTL8192CUPHY_REG_ARRAY_PGLENGTH] = {
+	0xe00, 0xffffffff, 0x07090c0c,
+	0xe04, 0xffffffff, 0x01020405,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x0b0c0c0e,
+	0xe14, 0xffffffff, 0x01030506,
+	0xe18, 0xffffffff, 0x0b0c0d0e,
+	0xe1c, 0xffffffff, 0x01030509,
+	0x830, 0xffffffff, 0x07090c0c,
+	0x834, 0xffffffff, 0x01020405,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x0b0c0d0e,
+	0x848, 0xffffffff, 0x01030509,
+	0x84c, 0xffffffff, 0x0b0c0d0e,
+	0x868, 0xffffffff, 0x01030509,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x04040404,
+	0xe04, 0xffffffff, 0x00020204,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x06060606,
+	0xe14, 0xffffffff, 0x00020406,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x04040404,
+	0x834, 0xffffffff, 0x00020204,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x06060606,
+	0x848, 0xffffffff, 0x00020406,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x04040404,
+	0xe04, 0xffffffff, 0x00020204,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x04040404,
+	0x834, 0xffffffff, 0x00020204,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+};
+
+u32 RTL8192CURADIOA_2TARRAY[RTL8192CURADIOA_2TARRAYLENGTH] = {
+	0x000, 0x00030159,
+	0x001, 0x00031284,
+	0x002, 0x00098000,
+	0x003, 0x00018c63,
+	0x004, 0x000210e7,
+	0x009, 0x0002044f,
+	0x00a, 0x0001adb1,
+	0x00b, 0x00054867,
+	0x00c, 0x0008992e,
+	0x00d, 0x0000e52c,
+	0x00e, 0x00039ce7,
+	0x00f, 0x00000451,
+	0x019, 0x00000000,
+	0x01a, 0x00010255,
+	0x01b, 0x00060a00,
+	0x01c, 0x000fc378,
+	0x01d, 0x000a1250,
+	0x01e, 0x0004445f,
+	0x01f, 0x00080001,
+	0x020, 0x0000b614,
+	0x021, 0x0006c000,
+	0x022, 0x00000000,
+	0x023, 0x00001558,
+	0x024, 0x00000060,
+	0x025, 0x00000483,
+	0x026, 0x0004f000,
+	0x027, 0x000ec7d9,
+	0x028, 0x000577c0,
+	0x029, 0x00004783,
+	0x02a, 0x00000001,
+	0x02b, 0x00021334,
+	0x02a, 0x00000000,
+	0x02b, 0x00000054,
+	0x02a, 0x00000001,
+	0x02b, 0x00000808,
+	0x02b, 0x00053333,
+	0x02c, 0x0000000c,
+	0x02a, 0x00000002,
+	0x02b, 0x00000808,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000003,
+	0x02b, 0x00000808,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000004,
+	0x02b, 0x00000808,
+	0x02b, 0x0006b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000005,
+	0x02b, 0x00000808,
+	0x02b, 0x00073333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000006,
+	0x02b, 0x00000709,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000007,
+	0x02b, 0x00000709,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000008,
+	0x02b, 0x0000060a,
+	0x02b, 0x0004b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000009,
+	0x02b, 0x0000060a,
+	0x02b, 0x00053333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000a,
+	0x02b, 0x0000060a,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000b,
+	0x02b, 0x0000060a,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000c,
+	0x02b, 0x0000060a,
+	0x02b, 0x0006b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000d,
+	0x02b, 0x0000060a,
+	0x02b, 0x00073333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000e,
+	0x02b, 0x0000050b,
+	0x02b, 0x00066666,
+	0x02c, 0x0000001a,
+	0x02a, 0x000e0000,
+	0x010, 0x0004000f,
+	0x011, 0x000e31fc,
+	0x010, 0x0006000f,
+	0x011, 0x000ff9f8,
+	0x010, 0x0002000f,
+	0x011, 0x000203f9,
+	0x010, 0x0003000f,
+	0x011, 0x000ff500,
+	0x010, 0x00000000,
+	0x011, 0x00000000,
+	0x010, 0x0008000f,
+	0x011, 0x0003f100,
+	0x010, 0x0009000f,
+	0x011, 0x00023100,
+	0x012, 0x00032000,
+	0x012, 0x00071000,
+	0x012, 0x000b0000,
+	0x012, 0x000fc000,
+	0x013, 0x000287af,
+	0x013, 0x000244b7,
+	0x013, 0x000204ab,
+	0x013, 0x0001c49f,
+	0x013, 0x00018493,
+	0x013, 0x00014297,
+	0x013, 0x00010295,
+	0x013, 0x0000c298,
+	0x013, 0x0000819c,
+	0x013, 0x000040a8,
+	0x013, 0x0000001c,
+	0x014, 0x0001944c,
+	0x014, 0x00059444,
+	0x014, 0x0009944c,
+	0x014, 0x000d9444,
+	0x015, 0x0000f424,
+	0x015, 0x0004f424,
+	0x015, 0x0008f424,
+	0x015, 0x000cf424,
+	0x016, 0x000e0330,
+	0x016, 0x000a0330,
+	0x016, 0x00060330,
+	0x016, 0x00020330,
+	0x000, 0x00010159,
+	0x018, 0x0000f401,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x01f, 0x00080003,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x01e, 0x00044457,
+	0x01f, 0x00080000,
+	0x000, 0x00030159,
+};
+
+u32 RTL8192CU_RADIOB_2TARRAY[RTL8192CURADIOB_2TARRAYLENGTH] = {
+	0x000, 0x00030159,
+	0x001, 0x00031284,
+	0x002, 0x00098000,
+	0x003, 0x00018c63,
+	0x004, 0x000210e7,
+	0x009, 0x0002044f,
+	0x00a, 0x0001adb1,
+	0x00b, 0x00054867,
+	0x00c, 0x0008992e,
+	0x00d, 0x0000e52c,
+	0x00e, 0x00039ce7,
+	0x00f, 0x00000451,
+	0x012, 0x00032000,
+	0x012, 0x00071000,
+	0x012, 0x000b0000,
+	0x012, 0x000fc000,
+	0x013, 0x000287af,
+	0x013, 0x000244b7,
+	0x013, 0x000204ab,
+	0x013, 0x0001c49f,
+	0x013, 0x00018493,
+	0x013, 0x00014297,
+	0x013, 0x00010295,
+	0x013, 0x0000c298,
+	0x013, 0x0000819c,
+	0x013, 0x000040a8,
+	0x013, 0x0000001c,
+	0x014, 0x0001944c,
+	0x014, 0x00059444,
+	0x014, 0x0009944c,
+	0x014, 0x000d9444,
+	0x015, 0x0000f424,
+	0x015, 0x0004f424,
+	0x015, 0x0008f424,
+	0x015, 0x000cf424,
+	0x016, 0x000e0330,
+	0x016, 0x000a0330,
+	0x016, 0x00060330,
+	0x016, 0x00020330,
+};
+
+u32 RTL8192CU_RADIOA_1TARRAY[RTL8192CURADIOA_1TARRAYLENGTH] = {
+	0x000, 0x00030159,
+	0x001, 0x00031284,
+	0x002, 0x00098000,
+	0x003, 0x00018c63,
+	0x004, 0x000210e7,
+	0x009, 0x0002044f,
+	0x00a, 0x0001adb1,
+	0x00b, 0x00054867,
+	0x00c, 0x0008992e,
+	0x00d, 0x0000e52c,
+	0x00e, 0x00039ce7,
+	0x00f, 0x00000451,
+	0x019, 0x00000000,
+	0x01a, 0x00010255,
+	0x01b, 0x00060a00,
+	0x01c, 0x000fc378,
+	0x01d, 0x000a1250,
+	0x01e, 0x0004445f,
+	0x01f, 0x00080001,
+	0x020, 0x0000b614,
+	0x021, 0x0006c000,
+	0x022, 0x00000000,
+	0x023, 0x00001558,
+	0x024, 0x00000060,
+	0x025, 0x00000483,
+	0x026, 0x0004f000,
+	0x027, 0x000ec7d9,
+	0x028, 0x000577c0,
+	0x029, 0x00004783,
+	0x02a, 0x00000001,
+	0x02b, 0x00021334,
+	0x02a, 0x00000000,
+	0x02b, 0x00000054,
+	0x02a, 0x00000001,
+	0x02b, 0x00000808,
+	0x02b, 0x00053333,
+	0x02c, 0x0000000c,
+	0x02a, 0x00000002,
+	0x02b, 0x00000808,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000003,
+	0x02b, 0x00000808,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000004,
+	0x02b, 0x00000808,
+	0x02b, 0x0006b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000005,
+	0x02b, 0x00000808,
+	0x02b, 0x00073333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000006,
+	0x02b, 0x00000709,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000007,
+	0x02b, 0x00000709,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000008,
+	0x02b, 0x0000060a,
+	0x02b, 0x0004b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000009,
+	0x02b, 0x0000060a,
+	0x02b, 0x00053333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000a,
+	0x02b, 0x0000060a,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000b,
+	0x02b, 0x0000060a,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000c,
+	0x02b, 0x0000060a,
+	0x02b, 0x0006b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000d,
+	0x02b, 0x0000060a,
+	0x02b, 0x00073333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000e,
+	0x02b, 0x0000050b,
+	0x02b, 0x00066666,
+	0x02c, 0x0000001a,
+	0x02a, 0x000e0000,
+	0x010, 0x0004000f,
+	0x011, 0x000e31fc,
+	0x010, 0x0006000f,
+	0x011, 0x000ff9f8,
+	0x010, 0x0002000f,
+	0x011, 0x000203f9,
+	0x010, 0x0003000f,
+	0x011, 0x000ff500,
+	0x010, 0x00000000,
+	0x011, 0x00000000,
+	0x010, 0x0008000f,
+	0x011, 0x0003f100,
+	0x010, 0x0009000f,
+	0x011, 0x00023100,
+	0x012, 0x00032000,
+	0x012, 0x00071000,
+	0x012, 0x000b0000,
+	0x012, 0x000fc000,
+	0x013, 0x000287b3,
+	0x013, 0x000244b7,
+	0x013, 0x000204ab,
+	0x013, 0x0001c49f,
+	0x013, 0x00018493,
+	0x013, 0x0001429b,
+	0x013, 0x00010299,
+	0x013, 0x0000c29c,
+	0x013, 0x000081a0,
+	0x013, 0x000040ac,
+	0x013, 0x00000020,
+	0x014, 0x0001944c,
+	0x014, 0x00059444,
+	0x014, 0x0009944c,
+	0x014, 0x000d9444,
+	0x015, 0x0000f405,
+	0x015, 0x0004f405,
+	0x015, 0x0008f405,
+	0x015, 0x000cf405,
+	0x016, 0x000e0330,
+	0x016, 0x000a0330,
+	0x016, 0x00060330,
+	0x016, 0x00020330,
+	0x000, 0x00010159,
+	0x018, 0x0000f401,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x01f, 0x00080003,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x01e, 0x00044457,
+	0x01f, 0x00080000,
+	0x000, 0x00030159,
+};
+
+u32 RTL8192CU_RADIOB_1TARRAY[RTL8192CURADIOB_1TARRAYLENGTH] = {
+	0x0,
+};
+
+u32 RTL8192CUMAC_2T_ARRAY[RTL8192CUMAC_2T_ARRAYLENGTH] = {
+	0x420, 0x00000080,
+	0x423, 0x00000000,
+	0x430, 0x00000000,
+	0x431, 0x00000000,
+	0x432, 0x00000000,
+	0x433, 0x00000001,
+	0x434, 0x00000004,
+	0x435, 0x00000005,
+	0x436, 0x00000006,
+	0x437, 0x00000007,
+	0x438, 0x00000000,
+	0x439, 0x00000000,
+	0x43a, 0x00000000,
+	0x43b, 0x00000001,
+	0x43c, 0x00000004,
+	0x43d, 0x00000005,
+	0x43e, 0x00000006,
+	0x43f, 0x00000007,
+	0x440, 0x0000005d,
+	0x441, 0x00000001,
+	0x442, 0x00000000,
+	0x444, 0x00000015,
+	0x445, 0x000000f0,
+	0x446, 0x0000000f,
+	0x447, 0x00000000,
+	0x458, 0x00000041,
+	0x459, 0x000000a8,
+	0x45a, 0x00000072,
+	0x45b, 0x000000b9,
+	0x460, 0x00000066,
+	0x461, 0x00000066,
+	0x462, 0x00000008,
+	0x463, 0x00000003,
+	0x4c8, 0x000000ff,
+	0x4c9, 0x00000008,
+	0x4cc, 0x000000ff,
+	0x4cd, 0x000000ff,
+	0x4ce, 0x00000001,
+	0x500, 0x00000026,
+	0x501, 0x000000a2,
+	0x502, 0x0000002f,
+	0x503, 0x00000000,
+	0x504, 0x00000028,
+	0x505, 0x000000a3,
+	0x506, 0x0000005e,
+	0x507, 0x00000000,
+	0x508, 0x0000002b,
+	0x509, 0x000000a4,
+	0x50a, 0x0000005e,
+	0x50b, 0x00000000,
+	0x50c, 0x0000004f,
+	0x50d, 0x000000a4,
+	0x50e, 0x00000000,
+	0x50f, 0x00000000,
+	0x512, 0x0000001c,
+	0x514, 0x0000000a,
+	0x515, 0x00000010,
+	0x516, 0x0000000a,
+	0x517, 0x00000010,
+	0x51a, 0x00000016,
+	0x524, 0x0000000f,
+	0x525, 0x0000004f,
+	0x546, 0x00000040,
+	0x547, 0x00000000,
+	0x550, 0x00000010,
+	0x551, 0x00000010,
+	0x559, 0x00000002,
+	0x55a, 0x00000002,
+	0x55d, 0x000000ff,
+	0x605, 0x00000030,
+	0x608, 0x0000000e,
+	0x609, 0x0000002a,
+	0x652, 0x00000020,
+	0x63c, 0x0000000a,
+	0x63d, 0x0000000e,
+	0x63e, 0x0000000a,
+	0x63f, 0x0000000e,
+	0x66e, 0x00000005,
+	0x700, 0x00000021,
+	0x701, 0x00000043,
+	0x702, 0x00000065,
+	0x703, 0x00000087,
+	0x708, 0x00000021,
+	0x709, 0x00000043,
+	0x70a, 0x00000065,
+	0x70b, 0x00000087,
+};
+
+u32 RTL8192CUAGCTAB_2TARRAY[RTL8192CUAGCTAB_2TARRAYLENGTH] = {
+	0xc78, 0x7b000001,
+	0xc78, 0x7b010001,
+	0xc78, 0x7b020001,
+	0xc78, 0x7b030001,
+	0xc78, 0x7b040001,
+	0xc78, 0x7b050001,
+	0xc78, 0x7a060001,
+	0xc78, 0x79070001,
+	0xc78, 0x78080001,
+	0xc78, 0x77090001,
+	0xc78, 0x760a0001,
+	0xc78, 0x750b0001,
+	0xc78, 0x740c0001,
+	0xc78, 0x730d0001,
+	0xc78, 0x720e0001,
+	0xc78, 0x710f0001,
+	0xc78, 0x70100001,
+	0xc78, 0x6f110001,
+	0xc78, 0x6e120001,
+	0xc78, 0x6d130001,
+	0xc78, 0x6c140001,
+	0xc78, 0x6b150001,
+	0xc78, 0x6a160001,
+	0xc78, 0x69170001,
+	0xc78, 0x68180001,
+	0xc78, 0x67190001,
+	0xc78, 0x661a0001,
+	0xc78, 0x651b0001,
+	0xc78, 0x641c0001,
+	0xc78, 0x631d0001,
+	0xc78, 0x621e0001,
+	0xc78, 0x611f0001,
+	0xc78, 0x60200001,
+	0xc78, 0x49210001,
+	0xc78, 0x48220001,
+	0xc78, 0x47230001,
+	0xc78, 0x46240001,
+	0xc78, 0x45250001,
+	0xc78, 0x44260001,
+	0xc78, 0x43270001,
+	0xc78, 0x42280001,
+	0xc78, 0x41290001,
+	0xc78, 0x402a0001,
+	0xc78, 0x262b0001,
+	0xc78, 0x252c0001,
+	0xc78, 0x242d0001,
+	0xc78, 0x232e0001,
+	0xc78, 0x222f0001,
+	0xc78, 0x21300001,
+	0xc78, 0x20310001,
+	0xc78, 0x06320001,
+	0xc78, 0x05330001,
+	0xc78, 0x04340001,
+	0xc78, 0x03350001,
+	0xc78, 0x02360001,
+	0xc78, 0x01370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7b400001,
+	0xc78, 0x7b410001,
+	0xc78, 0x7b420001,
+	0xc78, 0x7b430001,
+	0xc78, 0x7b440001,
+	0xc78, 0x7b450001,
+	0xc78, 0x7a460001,
+	0xc78, 0x79470001,
+	0xc78, 0x78480001,
+	0xc78, 0x77490001,
+	0xc78, 0x764a0001,
+	0xc78, 0x754b0001,
+	0xc78, 0x744c0001,
+	0xc78, 0x734d0001,
+	0xc78, 0x724e0001,
+	0xc78, 0x714f0001,
+	0xc78, 0x70500001,
+	0xc78, 0x6f510001,
+	0xc78, 0x6e520001,
+	0xc78, 0x6d530001,
+	0xc78, 0x6c540001,
+	0xc78, 0x6b550001,
+	0xc78, 0x6a560001,
+	0xc78, 0x69570001,
+	0xc78, 0x68580001,
+	0xc78, 0x67590001,
+	0xc78, 0x665a0001,
+	0xc78, 0x655b0001,
+	0xc78, 0x645c0001,
+	0xc78, 0x635d0001,
+	0xc78, 0x625e0001,
+	0xc78, 0x615f0001,
+	0xc78, 0x60600001,
+	0xc78, 0x49610001,
+	0xc78, 0x48620001,
+	0xc78, 0x47630001,
+	0xc78, 0x46640001,
+	0xc78, 0x45650001,
+	0xc78, 0x44660001,
+	0xc78, 0x43670001,
+	0xc78, 0x42680001,
+	0xc78, 0x41690001,
+	0xc78, 0x406a0001,
+	0xc78, 0x266b0001,
+	0xc78, 0x256c0001,
+	0xc78, 0x246d0001,
+	0xc78, 0x236e0001,
+	0xc78, 0x226f0001,
+	0xc78, 0x21700001,
+	0xc78, 0x20710001,
+	0xc78, 0x06720001,
+	0xc78, 0x05730001,
+	0xc78, 0x04740001,
+	0xc78, 0x03750001,
+	0xc78, 0x02760001,
+	0xc78, 0x01770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x3800001e,
+	0xc78, 0x3801001e,
+	0xc78, 0x3802001e,
+	0xc78, 0x3803001e,
+	0xc78, 0x3804001e,
+	0xc78, 0x3805001e,
+	0xc78, 0x3806001e,
+	0xc78, 0x3807001e,
+	0xc78, 0x3808001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3e0a001e,
+	0xc78, 0x400b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x480d001e,
+	0xc78, 0x4c0e001e,
+	0xc78, 0x500f001e,
+	0xc78, 0x5210001e,
+	0xc78, 0x5611001e,
+	0xc78, 0x5a12001e,
+	0xc78, 0x5e13001e,
+	0xc78, 0x6014001e,
+	0xc78, 0x6015001e,
+	0xc78, 0x6016001e,
+	0xc78, 0x6217001e,
+	0xc78, 0x6218001e,
+	0xc78, 0x6219001e,
+	0xc78, 0x621a001e,
+	0xc78, 0x621b001e,
+	0xc78, 0x621c001e,
+	0xc78, 0x621d001e,
+	0xc78, 0x621e001e,
+	0xc78, 0x621f001e,
+};
+
+u32 RTL8192CUAGCTAB_1TARRAY[RTL8192CUAGCTAB_1TARRAYLENGTH] = {
+	0xc78, 0x7b000001,
+	0xc78, 0x7b010001,
+	0xc78, 0x7b020001,
+	0xc78, 0x7b030001,
+	0xc78, 0x7b040001,
+	0xc78, 0x7b050001,
+	0xc78, 0x7a060001,
+	0xc78, 0x79070001,
+	0xc78, 0x78080001,
+	0xc78, 0x77090001,
+	0xc78, 0x760a0001,
+	0xc78, 0x750b0001,
+	0xc78, 0x740c0001,
+	0xc78, 0x730d0001,
+	0xc78, 0x720e0001,
+	0xc78, 0x710f0001,
+	0xc78, 0x70100001,
+	0xc78, 0x6f110001,
+	0xc78, 0x6e120001,
+	0xc78, 0x6d130001,
+	0xc78, 0x6c140001,
+	0xc78, 0x6b150001,
+	0xc78, 0x6a160001,
+	0xc78, 0x69170001,
+	0xc78, 0x68180001,
+	0xc78, 0x67190001,
+	0xc78, 0x661a0001,
+	0xc78, 0x651b0001,
+	0xc78, 0x641c0001,
+	0xc78, 0x631d0001,
+	0xc78, 0x621e0001,
+	0xc78, 0x611f0001,
+	0xc78, 0x60200001,
+	0xc78, 0x49210001,
+	0xc78, 0x48220001,
+	0xc78, 0x47230001,
+	0xc78, 0x46240001,
+	0xc78, 0x45250001,
+	0xc78, 0x44260001,
+	0xc78, 0x43270001,
+	0xc78, 0x42280001,
+	0xc78, 0x41290001,
+	0xc78, 0x402a0001,
+	0xc78, 0x262b0001,
+	0xc78, 0x252c0001,
+	0xc78, 0x242d0001,
+	0xc78, 0x232e0001,
+	0xc78, 0x222f0001,
+	0xc78, 0x21300001,
+	0xc78, 0x20310001,
+	0xc78, 0x06320001,
+	0xc78, 0x05330001,
+	0xc78, 0x04340001,
+	0xc78, 0x03350001,
+	0xc78, 0x02360001,
+	0xc78, 0x01370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7b400001,
+	0xc78, 0x7b410001,
+	0xc78, 0x7b420001,
+	0xc78, 0x7b430001,
+	0xc78, 0x7b440001,
+	0xc78, 0x7b450001,
+	0xc78, 0x7a460001,
+	0xc78, 0x79470001,
+	0xc78, 0x78480001,
+	0xc78, 0x77490001,
+	0xc78, 0x764a0001,
+	0xc78, 0x754b0001,
+	0xc78, 0x744c0001,
+	0xc78, 0x734d0001,
+	0xc78, 0x724e0001,
+	0xc78, 0x714f0001,
+	0xc78, 0x70500001,
+	0xc78, 0x6f510001,
+	0xc78, 0x6e520001,
+	0xc78, 0x6d530001,
+	0xc78, 0x6c540001,
+	0xc78, 0x6b550001,
+	0xc78, 0x6a560001,
+	0xc78, 0x69570001,
+	0xc78, 0x68580001,
+	0xc78, 0x67590001,
+	0xc78, 0x665a0001,
+	0xc78, 0x655b0001,
+	0xc78, 0x645c0001,
+	0xc78, 0x635d0001,
+	0xc78, 0x625e0001,
+	0xc78, 0x615f0001,
+	0xc78, 0x60600001,
+	0xc78, 0x49610001,
+	0xc78, 0x48620001,
+	0xc78, 0x47630001,
+	0xc78, 0x46640001,
+	0xc78, 0x45650001,
+	0xc78, 0x44660001,
+	0xc78, 0x43670001,
+	0xc78, 0x42680001,
+	0xc78, 0x41690001,
+	0xc78, 0x406a0001,
+	0xc78, 0x266b0001,
+	0xc78, 0x256c0001,
+	0xc78, 0x246d0001,
+	0xc78, 0x236e0001,
+	0xc78, 0x226f0001,
+	0xc78, 0x21700001,
+	0xc78, 0x20710001,
+	0xc78, 0x06720001,
+	0xc78, 0x05730001,
+	0xc78, 0x04740001,
+	0xc78, 0x03750001,
+	0xc78, 0x02760001,
+	0xc78, 0x01770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x3800001e,
+	0xc78, 0x3801001e,
+	0xc78, 0x3802001e,
+	0xc78, 0x3803001e,
+	0xc78, 0x3804001e,
+	0xc78, 0x3805001e,
+	0xc78, 0x3806001e,
+	0xc78, 0x3807001e,
+	0xc78, 0x3808001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3e0a001e,
+	0xc78, 0x400b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x480d001e,
+	0xc78, 0x4c0e001e,
+	0xc78, 0x500f001e,
+	0xc78, 0x5210001e,
+	0xc78, 0x5611001e,
+	0xc78, 0x5a12001e,
+	0xc78, 0x5e13001e,
+	0xc78, 0x6014001e,
+	0xc78, 0x6015001e,
+	0xc78, 0x6016001e,
+	0xc78, 0x6217001e,
+	0xc78, 0x6218001e,
+	0xc78, 0x6219001e,
+	0xc78, 0x621a001e,
+	0xc78, 0x621b001e,
+	0xc78, 0x621c001e,
+	0xc78, 0x621d001e,
+	0xc78, 0x621e001e,
+	0xc78, 0x621f001e,
+};
+
+u32 RTL8192CUPHY_REG_1T_HPArray[RTL8192CUPHY_REG_1T_HPArrayLength] = {
+	0x024, 0x0011800f,
+	0x028, 0x00ffdb83,
+	0x040, 0x000c0004,
+	0x800, 0x80040000,
+	0x804, 0x00000001,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000000a,
+	0x810, 0x10005388,
+	0x814, 0x020c3d10,
+	0x818, 0x02200385,
+	0x81c, 0x00000000,
+	0x820, 0x01000100,
+	0x824, 0x00390204,
+	0x828, 0x00000000,
+	0x82c, 0x00000000,
+	0x830, 0x00000000,
+	0x834, 0x00000000,
+	0x838, 0x00000000,
+	0x83c, 0x00000000,
+	0x840, 0x00010000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84c, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x569a569a,
+	0x85c, 0x001b25a4,
+	0x860, 0x66e60230,
+	0x864, 0x061f0130,
+	0x868, 0x00000000,
+	0x86c, 0x20202000,
+	0x870, 0x03000300,
+	0x874, 0x22004000,
+	0x878, 0x00000808,
+	0x87c, 0x00ffc3f1,
+	0x880, 0xc0083070,
+	0x884, 0x000004d5,
+	0x888, 0x00000000,
+	0x88c, 0xccc000c0,
+	0x890, 0x00000800,
+	0x894, 0xfffffffe,
+	0x898, 0x40302010,
+	0x89c, 0x00706050,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x81121111,
+	0xa00, 0x00d047c8,
+	0xa04, 0x80ff000c,
+	0xa08, 0x8c838300,
+	0xa0c, 0x2e68120f,
+	0xa10, 0x9500bb78,
+	0xa14, 0x11144028,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140f00,
+	0xa20, 0x15160000,
+	0xa24, 0x070b0f12,
+	0xa28, 0x00000104,
+	0xa2c, 0x00d30000,
+	0xa70, 0x101fbf00,
+	0xa74, 0x00000007,
+	0xc00, 0x48071d40,
+	0xc04, 0x03a05611,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08800000,
+	0xc1c, 0x40000100,
+	0xc20, 0x00000000,
+	0xc24, 0x00000000,
+	0xc28, 0x00000000,
+	0xc2c, 0x00000000,
+	0xc30, 0x69e9ac44,
+	0xc34, 0x469652cf,
+	0xc38, 0x49795994,
+	0xc3c, 0x0a97971c,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020107,
+	0xc4c, 0x007f037f,
+	0xc50, 0x6954342e,
+	0xc54, 0x43bc0094,
+	0xc58, 0x6954342f,
+	0xc5c, 0x433c0094,
+	0xc60, 0x00000000,
+	0xc64, 0x5116848b,
+	0xc68, 0x47c00bff,
+	0xc6c, 0x00000036,
+	0xc70, 0x2c46000d,
+	0xc74, 0x018610db,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b91612,
+	0xc80, 0x24000090,
+	0xc84, 0x20f60000,
+	0xc88, 0x24000090,
+	0xc8c, 0x20200000,
+	0xc90, 0x00121820,
+	0xc94, 0x00000000,
+	0xc98, 0x00121820,
+	0xc9c, 0x00007f7f,
+	0xca0, 0x00000000,
+	0xca4, 0x00000080,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x28000000,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xce4, 0x00000000,
+	0xce8, 0x37644302,
+	0xcec, 0x2f97d40c,
+	0xd00, 0x00080740,
+	0xd04, 0x00020401,
+	0xd08, 0x0000907f,
+	0xd0c, 0x20010201,
+	0xd10, 0xa0633333,
+	0xd14, 0x3333bc43,
+	0xd18, 0x7a8f5b6b,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x80608000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x00000000,
+	0xd58, 0x00000000,
+	0xd5c, 0x30032064,
+	0xd60, 0x4653de68,
+	0xd64, 0x04518a3c,
+	0xd68, 0x00002101,
+	0xd6c, 0x2a201c16,
+	0xd70, 0x1812362e,
+	0xd74, 0x322c2220,
+	0xd78, 0x000e3c24,
+	0xe00, 0x24242424,
+	0xe04, 0x24242424,
+	0xe08, 0x03902024,
+	0xe10, 0x24242424,
+	0xe14, 0x24242424,
+	0xe18, 0x24242424,
+	0xe1c, 0x24242424,
+	0xe28, 0x00000000,
+	0xe30, 0x1000dc1f,
+	0xe34, 0x10008c1f,
+	0xe38, 0x02140102,
+	0xe3c, 0x681604c2,
+	0xe40, 0x01007c00,
+	0xe44, 0x01004800,
+	0xe48, 0xfb000000,
+	0xe4c, 0x000028d1,
+	0xe50, 0x1000dc1f,
+	0xe54, 0x10008c1f,
+	0xe58, 0x02140102,
+	0xe5c, 0x28160d05,
+	0xe60, 0x00000008,
+	0xe68, 0x001b25a4,
+	0xe6c, 0x631b25a0,
+	0xe70, 0x631b25a0,
+	0xe74, 0x081b25a0,
+	0xe78, 0x081b25a0,
+	0xe7c, 0x081b25a0,
+	0xe80, 0x081b25a0,
+	0xe84, 0x631b25a0,
+	0xe88, 0x081b25a0,
+	0xe8c, 0x631b25a0,
+	0xed0, 0x631b25a0,
+	0xed4, 0x631b25a0,
+	0xed8, 0x631b25a0,
+	0xedc, 0x001b25a0,
+	0xee0, 0x001b25a0,
+	0xeec, 0x6b1b25a0,
+	0xee8, 0x31555448,
+	0xf14, 0x00000003,
+	0xf4c, 0x00000000,
+	0xf00, 0x00000300,
+};
+
+u32 RTL8192CUPHY_REG_Array_PG_HP[RTL8192CUPHY_REG_Array_PG_HPLength] = {
+	0xe00, 0xffffffff, 0x06080808,
+	0xe04, 0xffffffff, 0x00040406,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x04060608,
+	0xe14, 0xffffffff, 0x00020204,
+	0xe18, 0xffffffff, 0x04060608,
+	0xe1c, 0xffffffff, 0x00020204,
+	0x830, 0xffffffff, 0x06080808,
+	0x834, 0xffffffff, 0x00040406,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x04060608,
+	0x848, 0xffffffff, 0x00020204,
+	0x84c, 0xffffffff, 0x04060608,
+	0x868, 0xffffffff, 0x00020204,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+	0xe00, 0xffffffff, 0x00000000,
+	0xe04, 0xffffffff, 0x00000000,
+	0xe08, 0x0000ff00, 0x00000000,
+	0x86c, 0xffffff00, 0x00000000,
+	0xe10, 0xffffffff, 0x00000000,
+	0xe14, 0xffffffff, 0x00000000,
+	0xe18, 0xffffffff, 0x00000000,
+	0xe1c, 0xffffffff, 0x00000000,
+	0x830, 0xffffffff, 0x00000000,
+	0x834, 0xffffffff, 0x00000000,
+	0x838, 0xffffff00, 0x00000000,
+	0x86c, 0x000000ff, 0x00000000,
+	0x83c, 0xffffffff, 0x00000000,
+	0x848, 0xffffffff, 0x00000000,
+	0x84c, 0xffffffff, 0x00000000,
+	0x868, 0xffffffff, 0x00000000,
+};
+
+u32 RTL8192CURadioA_1T_HPArray[RTL8192CURadioA_1T_HPArrayLength] = {
+	0x000, 0x00030159,
+	0x001, 0x00031284,
+	0x002, 0x00098000,
+	0x003, 0x00018c63,
+	0x004, 0x000210e7,
+	0x009, 0x0002044f,
+	0x00a, 0x0001adb0,
+	0x00b, 0x00054867,
+	0x00c, 0x0008992e,
+	0x00d, 0x0000e529,
+	0x00e, 0x00039ce7,
+	0x00f, 0x00000451,
+	0x019, 0x00000000,
+	0x01a, 0x00000255,
+	0x01b, 0x00060a00,
+	0x01c, 0x000fc378,
+	0x01d, 0x000a1250,
+	0x01e, 0x0004445f,
+	0x01f, 0x00080001,
+	0x020, 0x0000b614,
+	0x021, 0x0006c000,
+	0x022, 0x0000083c,
+	0x023, 0x00001558,
+	0x024, 0x00000060,
+	0x025, 0x00000483,
+	0x026, 0x0004f000,
+	0x027, 0x000ec7d9,
+	0x028, 0x000977c0,
+	0x029, 0x00004783,
+	0x02a, 0x00000001,
+	0x02b, 0x00021334,
+	0x02a, 0x00000000,
+	0x02b, 0x00000054,
+	0x02a, 0x00000001,
+	0x02b, 0x00000808,
+	0x02b, 0x00053333,
+	0x02c, 0x0000000c,
+	0x02a, 0x00000002,
+	0x02b, 0x00000808,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000003,
+	0x02b, 0x00000808,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000004,
+	0x02b, 0x00000808,
+	0x02b, 0x0006b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000005,
+	0x02b, 0x00000808,
+	0x02b, 0x00073333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000006,
+	0x02b, 0x00000709,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000007,
+	0x02b, 0x00000709,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000008,
+	0x02b, 0x0000060a,
+	0x02b, 0x0004b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x00000009,
+	0x02b, 0x0000060a,
+	0x02b, 0x00053333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000a,
+	0x02b, 0x0000060a,
+	0x02b, 0x0005b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000b,
+	0x02b, 0x0000060a,
+	0x02b, 0x00063333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000c,
+	0x02b, 0x0000060a,
+	0x02b, 0x0006b333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000d,
+	0x02b, 0x0000060a,
+	0x02b, 0x00073333,
+	0x02c, 0x0000000d,
+	0x02a, 0x0000000e,
+	0x02b, 0x0000050b,
+	0x02b, 0x00066666,
+	0x02c, 0x0000001a,
+	0x02a, 0x000e0000,
+	0x010, 0x0004000f,
+	0x011, 0x000e31fc,
+	0x010, 0x0006000f,
+	0x011, 0x000ff9f8,
+	0x010, 0x0002000f,
+	0x011, 0x000203f9,
+	0x010, 0x0003000f,
+	0x011, 0x000ff500,
+	0x010, 0x00000000,
+	0x011, 0x00000000,
+	0x010, 0x0008000f,
+	0x011, 0x0003f100,
+	0x010, 0x0009000f,
+	0x011, 0x00023100,
+	0x012, 0x000d8000,
+	0x012, 0x00090000,
+	0x012, 0x00051000,
+	0x012, 0x00012000,
+	0x013, 0x00028fb4,
+	0x013, 0x00024fa8,
+	0x013, 0x000207a4,
+	0x013, 0x0001c798,
+	0x013, 0x000183a4,
+	0x013, 0x00014398,
+	0x013, 0x000101a4,
+	0x013, 0x0000c198,
+	0x013, 0x000080a4,
+	0x013, 0x00004098,
+	0x013, 0x00000000,
+	0x014, 0x0001944c,
+	0x014, 0x00059444,
+	0x014, 0x0009944c,
+	0x014, 0x000d9444,
+	0x015, 0x0000f405,
+	0x015, 0x0004f405,
+	0x015, 0x0008f405,
+	0x015, 0x000cf405,
+	0x016, 0x000e0330,
+	0x016, 0x000a0330,
+	0x016, 0x00060330,
+	0x016, 0x00020330,
+	0x000, 0x00010159,
+	0x018, 0x0000f401,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x01f, 0x00080003,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x01e, 0x00044457,
+	0x01f, 0x00080000,
+	0x000, 0x00030159,
+};
+
+u32 Rtl8192CUAGCTAB_1T_HPArray[RTL8192CUAGCTAB_1T_HPArrayLength] = {
+	0xc78, 0x7b000001,
+	0xc78, 0x7b010001,
+	0xc78, 0x7b020001,
+	0xc78, 0x7b030001,
+	0xc78, 0x7b040001,
+	0xc78, 0x7b050001,
+	0xc78, 0x7b060001,
+	0xc78, 0x7b070001,
+	0xc78, 0x7b080001,
+	0xc78, 0x7a090001,
+	0xc78, 0x790a0001,
+	0xc78, 0x780b0001,
+	0xc78, 0x770c0001,
+	0xc78, 0x760d0001,
+	0xc78, 0x750e0001,
+	0xc78, 0x740f0001,
+	0xc78, 0x73100001,
+	0xc78, 0x72110001,
+	0xc78, 0x71120001,
+	0xc78, 0x70130001,
+	0xc78, 0x6f140001,
+	0xc78, 0x6e150001,
+	0xc78, 0x6d160001,
+	0xc78, 0x6c170001,
+	0xc78, 0x6b180001,
+	0xc78, 0x6a190001,
+	0xc78, 0x691a0001,
+	0xc78, 0x681b0001,
+	0xc78, 0x671c0001,
+	0xc78, 0x661d0001,
+	0xc78, 0x651e0001,
+	0xc78, 0x641f0001,
+	0xc78, 0x63200001,
+	0xc78, 0x62210001,
+	0xc78, 0x61220001,
+	0xc78, 0x60230001,
+	0xc78, 0x46240001,
+	0xc78, 0x45250001,
+	0xc78, 0x44260001,
+	0xc78, 0x43270001,
+	0xc78, 0x42280001,
+	0xc78, 0x41290001,
+	0xc78, 0x402a0001,
+	0xc78, 0x262b0001,
+	0xc78, 0x252c0001,
+	0xc78, 0x242d0001,
+	0xc78, 0x232e0001,
+	0xc78, 0x222f0001,
+	0xc78, 0x21300001,
+	0xc78, 0x20310001,
+	0xc78, 0x06320001,
+	0xc78, 0x05330001,
+	0xc78, 0x04340001,
+	0xc78, 0x03350001,
+	0xc78, 0x02360001,
+	0xc78, 0x01370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7b400001,
+	0xc78, 0x7b410001,
+	0xc78, 0x7b420001,
+	0xc78, 0x7b430001,
+	0xc78, 0x7b440001,
+	0xc78, 0x7b450001,
+	0xc78, 0x7b460001,
+	0xc78, 0x7b470001,
+	0xc78, 0x7b480001,
+	0xc78, 0x7a490001,
+	0xc78, 0x794a0001,
+	0xc78, 0x784b0001,
+	0xc78, 0x774c0001,
+	0xc78, 0x764d0001,
+	0xc78, 0x754e0001,
+	0xc78, 0x744f0001,
+	0xc78, 0x73500001,
+	0xc78, 0x72510001,
+	0xc78, 0x71520001,
+	0xc78, 0x70530001,
+	0xc78, 0x6f540001,
+	0xc78, 0x6e550001,
+	0xc78, 0x6d560001,
+	0xc78, 0x6c570001,
+	0xc78, 0x6b580001,
+	0xc78, 0x6a590001,
+	0xc78, 0x695a0001,
+	0xc78, 0x685b0001,
+	0xc78, 0x675c0001,
+	0xc78, 0x665d0001,
+	0xc78, 0x655e0001,
+	0xc78, 0x645f0001,
+	0xc78, 0x63600001,
+	0xc78, 0x62610001,
+	0xc78, 0x61620001,
+	0xc78, 0x60630001,
+	0xc78, 0x46640001,
+	0xc78, 0x45650001,
+	0xc78, 0x44660001,
+	0xc78, 0x43670001,
+	0xc78, 0x42680001,
+	0xc78, 0x41690001,
+	0xc78, 0x406a0001,
+	0xc78, 0x266b0001,
+	0xc78, 0x256c0001,
+	0xc78, 0x246d0001,
+	0xc78, 0x236e0001,
+	0xc78, 0x226f0001,
+	0xc78, 0x21700001,
+	0xc78, 0x20710001,
+	0xc78, 0x06720001,
+	0xc78, 0x05730001,
+	0xc78, 0x04740001,
+	0xc78, 0x03750001,
+	0xc78, 0x02760001,
+	0xc78, 0x01770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x3800001e,
+	0xc78, 0x3801001e,
+	0xc78, 0x3802001e,
+	0xc78, 0x3803001e,
+	0xc78, 0x3804001e,
+	0xc78, 0x3805001e,
+	0xc78, 0x3806001e,
+	0xc78, 0x3807001e,
+	0xc78, 0x3808001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3e0a001e,
+	0xc78, 0x400b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x480d001e,
+	0xc78, 0x4c0e001e,
+	0xc78, 0x500f001e,
+	0xc78, 0x5210001e,
+	0xc78, 0x5611001e,
+	0xc78, 0x5a12001e,
+	0xc78, 0x5e13001e,
+	0xc78, 0x6014001e,
+	0xc78, 0x6015001e,
+	0xc78, 0x6016001e,
+	0xc78, 0x6217001e,
+	0xc78, 0x6218001e,
+	0xc78, 0x6219001e,
+	0xc78, 0x621a001e,
+	0xc78, 0x621b001e,
+	0xc78, 0x621c001e,
+	0xc78, 0x621d001e,
+	0xc78, 0x621e001e,
+	0xc78, 0x621f001e,
+};
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/table.h b/drivers/net/wireless/rtlwifi/rtl8192cu/table.h
new file mode 100644
index 000000000000..c3d5cd826cfa
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/table.h
@@ -0,0 +1,71 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CU_TABLE__H_
+#define __RTL92CU_TABLE__H_
+
+#include <linux/types.h>
+
+#define RTL8192CUPHY_REG_2TARRAY_LENGTH		374
+extern u32 RTL8192CUPHY_REG_2TARRAY[RTL8192CUPHY_REG_2TARRAY_LENGTH];
+#define RTL8192CUPHY_REG_1TARRAY_LENGTH		374
+extern u32 RTL8192CUPHY_REG_1TARRAY[RTL8192CUPHY_REG_1TARRAY_LENGTH];
+
+#define RTL8192CUPHY_REG_ARRAY_PGLENGTH		336
+extern u32 RTL8192CUPHY_REG_ARRAY_PG[RTL8192CUPHY_REG_ARRAY_PGLENGTH];
+
+#define RTL8192CURADIOA_2TARRAYLENGTH	282
+extern u32 RTL8192CURADIOA_2TARRAY[RTL8192CURADIOA_2TARRAYLENGTH];
+#define RTL8192CURADIOB_2TARRAYLENGTH	78
+extern u32 RTL8192CU_RADIOB_2TARRAY[RTL8192CURADIOB_2TARRAYLENGTH];
+#define RTL8192CURADIOA_1TARRAYLENGTH	282
+extern u32 RTL8192CU_RADIOA_1TARRAY[RTL8192CURADIOA_1TARRAYLENGTH];
+#define RTL8192CURADIOB_1TARRAYLENGTH	1
+extern u32 RTL8192CU_RADIOB_1TARRAY[RTL8192CURADIOB_1TARRAYLENGTH];
+
+#define RTL8192CUMAC_2T_ARRAYLENGTH		172
+extern u32 RTL8192CUMAC_2T_ARRAY[RTL8192CUMAC_2T_ARRAYLENGTH];
+
+#define RTL8192CUAGCTAB_2TARRAYLENGTH	320
+extern u32 RTL8192CUAGCTAB_2TARRAY[RTL8192CUAGCTAB_2TARRAYLENGTH];
+#define RTL8192CUAGCTAB_1TARRAYLENGTH	320
+extern u32 RTL8192CUAGCTAB_1TARRAY[RTL8192CUAGCTAB_1TARRAYLENGTH];
+
+#define RTL8192CUPHY_REG_1T_HPArrayLength 378
+extern u32 RTL8192CUPHY_REG_1T_HPArray[RTL8192CUPHY_REG_1T_HPArrayLength];
+
+#define RTL8192CUPHY_REG_Array_PG_HPLength 336
+extern u32 RTL8192CUPHY_REG_Array_PG_HP[RTL8192CUPHY_REG_Array_PG_HPLength];
+
+#define RTL8192CURadioA_1T_HPArrayLength 282
+extern u32 RTL8192CURadioA_1T_HPArray[RTL8192CURadioA_1T_HPArrayLength];
+#define RTL8192CUAGCTAB_1T_HPArrayLength 320
+extern u32 Rtl8192CUAGCTAB_1T_HPArray[RTL8192CUAGCTAB_1T_HPArrayLength];
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
new file mode 100644
index 000000000000..d0b0d43b9a6d
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -0,0 +1,687 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../usb.h"
+#include "../ps.h"
+#include "../base.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "mac.h"
+#include "trx.h"
+
+static int _ConfigVerTOutEP(struct ieee80211_hw *hw)
+{
+	u8 ep_cfg, txqsele;
+	u8 ep_nums = 0;
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(usb_priv);
+
+	rtlusb->out_queue_sel = 0;
+	ep_cfg = rtl_read_byte(rtlpriv, REG_TEST_SIE_OPTIONAL);
+	ep_cfg = (ep_cfg & USB_TEST_EP_MASK) >> USB_TEST_EP_SHIFT;
+	switch (ep_cfg)	{
+	case 0:		/* 2 bulk OUT, 1 bulk IN */
+	case 3:
+		rtlusb->out_queue_sel  = TX_SELE_HQ | TX_SELE_LQ;
+		ep_nums = 2;
+		break;
+	case 1:	/* 1 bulk IN/OUT => map all endpoint to Low queue */
+	case 2:	/* 1 bulk IN, 1 bulk OUT => map all endpoint to High queue */
+		txqsele = rtl_read_byte(rtlpriv, REG_TEST_USB_TXQS);
+		if (txqsele & 0x0F) /* /map all endpoint to High queue */
+			rtlusb->out_queue_sel =  TX_SELE_HQ;
+		else if (txqsele&0xF0) /* map all endpoint to Low queue */
+			rtlusb->out_queue_sel =  TX_SELE_LQ;
+		ep_nums = 1;
+		break;
+	default:
+		break;
+	}
+	return (rtlusb->out_ep_nums == ep_nums) ? 0 : -EINVAL;
+}
+
+static int _ConfigVerNOutEP(struct ieee80211_hw *hw)
+{
+	u8 ep_cfg;
+	u8 ep_nums = 0;
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(usb_priv);
+
+	rtlusb->out_queue_sel = 0;
+	/* Normal and High queue */
+	ep_cfg =  rtl_read_byte(rtlpriv, (REG_NORMAL_SIE_EP + 1));
+	if (ep_cfg & USB_NORMAL_SIE_EP_MASK) {
+		rtlusb->out_queue_sel |= TX_SELE_HQ;
+		ep_nums++;
+	}
+	if ((ep_cfg >> USB_NORMAL_SIE_EP_SHIFT) & USB_NORMAL_SIE_EP_MASK) {
+		rtlusb->out_queue_sel |= TX_SELE_NQ;
+		ep_nums++;
+	}
+	/* Low queue */
+	ep_cfg =  rtl_read_byte(rtlpriv, (REG_NORMAL_SIE_EP + 2));
+	if (ep_cfg & USB_NORMAL_SIE_EP_MASK) {
+		rtlusb->out_queue_sel |= TX_SELE_LQ;
+		ep_nums++;
+	}
+	return (rtlusb->out_ep_nums == ep_nums) ? 0 : -EINVAL;
+}
+
+static void _TwoOutEpMapping(struct ieee80211_hw *hw, bool bIsChipB,
+			     bool  bwificfg, struct rtl_ep_map *ep_map)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (bwificfg) { /* for WMM */
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("USB Chip-B & WMM Setting.....\n"));
+		ep_map->ep_mapping[RTL_TXQ_BE]	= 2;
+		ep_map->ep_mapping[RTL_TXQ_BK]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_VI]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_VO] = 2;
+		ep_map->ep_mapping[RTL_TXQ_MGT] = 2;
+		ep_map->ep_mapping[RTL_TXQ_BCN] = 2;
+		ep_map->ep_mapping[RTL_TXQ_HI]	= 2;
+	} else { /* typical setting */
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("USB typical Setting.....\n"));
+		ep_map->ep_mapping[RTL_TXQ_BE]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_BK]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_VI]	= 2;
+		ep_map->ep_mapping[RTL_TXQ_VO]	= 2;
+		ep_map->ep_mapping[RTL_TXQ_MGT] = 2;
+		ep_map->ep_mapping[RTL_TXQ_BCN] = 2;
+		ep_map->ep_mapping[RTL_TXQ_HI]	= 2;
+	}
+}
+
+static void _ThreeOutEpMapping(struct ieee80211_hw *hw, bool  bwificfg,
+			       struct rtl_ep_map *ep_map)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (bwificfg) { /* for WMM */
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("USB 3EP Setting for WMM.....\n"));
+		ep_map->ep_mapping[RTL_TXQ_BE]	= 5;
+		ep_map->ep_mapping[RTL_TXQ_BK]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_VI]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_VO]	= 2;
+		ep_map->ep_mapping[RTL_TXQ_MGT] = 2;
+		ep_map->ep_mapping[RTL_TXQ_BCN] = 2;
+		ep_map->ep_mapping[RTL_TXQ_HI]	= 2;
+	} else { /* typical setting */
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("USB 3EP Setting for typical.....\n"));
+		ep_map->ep_mapping[RTL_TXQ_BE]	= 5;
+		ep_map->ep_mapping[RTL_TXQ_BK]	= 5;
+		ep_map->ep_mapping[RTL_TXQ_VI]	= 3;
+		ep_map->ep_mapping[RTL_TXQ_VO]	= 2;
+		ep_map->ep_mapping[RTL_TXQ_MGT] = 2;
+		ep_map->ep_mapping[RTL_TXQ_BCN] = 2;
+		ep_map->ep_mapping[RTL_TXQ_HI]	= 2;
+	}
+}
+
+static void _OneOutEpMapping(struct ieee80211_hw *hw, struct rtl_ep_map *ep_map)
+{
+	ep_map->ep_mapping[RTL_TXQ_BE]	= 2;
+	ep_map->ep_mapping[RTL_TXQ_BK]	= 2;
+	ep_map->ep_mapping[RTL_TXQ_VI]	= 2;
+	ep_map->ep_mapping[RTL_TXQ_VO] = 2;
+	ep_map->ep_mapping[RTL_TXQ_MGT] = 2;
+	ep_map->ep_mapping[RTL_TXQ_BCN] = 2;
+	ep_map->ep_mapping[RTL_TXQ_HI]	= 2;
+}
+static int _out_ep_mapping(struct ieee80211_hw *hw)
+{
+	int err = 0;
+	bool bIsChipN, bwificfg = false;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(usb_priv);
+	struct rtl_ep_map *ep_map = &(rtlusb->ep_map);
+
+	bIsChipN = IS_NORMAL_CHIP(rtlhal->version);
+	switch (rtlusb->out_ep_nums) {
+	case 2:
+		_TwoOutEpMapping(hw, bIsChipN, bwificfg, ep_map);
+		break;
+	case 3:
+		/* Test chip doesn't support three out EPs. */
+		if (!bIsChipN) {
+			err  =  -EINVAL;
+			goto err_out;
+		}
+		_ThreeOutEpMapping(hw, bIsChipN, ep_map);
+		break;
+	case 1:
+		_OneOutEpMapping(hw, ep_map);
+		break;
+	default:
+		err  =  -EINVAL;
+		break;
+	}
+err_out:
+	return err;
+
+}
+/* endpoint mapping */
+int  rtl8192cu_endpoint_mapping(struct ieee80211_hw *hw)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	int error = 0;
+	if (likely(IS_NORMAL_CHIP(rtlhal->version)))
+		error = _ConfigVerNOutEP(hw);
+	else
+		error = _ConfigVerTOutEP(hw);
+	if (error)
+		goto err_out;
+	error = _out_ep_mapping(hw);
+	if (error)
+		goto err_out;
+err_out:
+	return error;
+}
+
+u16 rtl8192cu_mq_to_hwq(__le16 fc, u16 mac80211_queue_index)
+{
+	u16 hw_queue_index;
+
+	if (unlikely(ieee80211_is_beacon(fc))) {
+		hw_queue_index = RTL_TXQ_BCN;
+		goto out;
+	}
+	if (ieee80211_is_mgmt(fc)) {
+		hw_queue_index = RTL_TXQ_MGT;
+		goto out;
+	}
+	switch (mac80211_queue_index) {
+	case 0:
+		hw_queue_index = RTL_TXQ_VO;
+		break;
+	case 1:
+		hw_queue_index = RTL_TXQ_VI;
+		break;
+	case 2:
+		hw_queue_index = RTL_TXQ_BE;
+		break;
+	case 3:
+		hw_queue_index = RTL_TXQ_BK;
+		break;
+	default:
+		hw_queue_index = RTL_TXQ_BE;
+		RT_ASSERT(false, ("QSLT_BE queue, skb_queue:%d\n",
+			  mac80211_queue_index));
+		break;
+	}
+out:
+	return hw_queue_index;
+}
+
+static enum rtl_desc_qsel _rtl8192cu_mq_to_descq(struct ieee80211_hw *hw,
+					 __le16 fc, u16 mac80211_queue_index)
+{
+	enum rtl_desc_qsel qsel;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (unlikely(ieee80211_is_beacon(fc))) {
+		qsel = QSLT_BEACON;
+		goto out;
+	}
+	if (ieee80211_is_mgmt(fc)) {
+		qsel = QSLT_MGNT;
+		goto out;
+	}
+	switch (mac80211_queue_index) {
+	case 0:	/* VO */
+		qsel = QSLT_VO;
+		RT_TRACE(rtlpriv, COMP_USB, DBG_DMESG,
+			 ("VO queue, set qsel = 0x%x\n", QSLT_VO));
+		break;
+	case 1:	/* VI */
+		qsel = QSLT_VI;
+		RT_TRACE(rtlpriv, COMP_USB, DBG_DMESG,
+			 ("VI queue, set qsel = 0x%x\n", QSLT_VI));
+		break;
+	case 3:	/* BK */
+		qsel = QSLT_BK;
+		RT_TRACE(rtlpriv, COMP_USB, DBG_DMESG,
+			 ("BK queue, set qsel = 0x%x\n", QSLT_BK));
+		break;
+	case 2:	/* BE */
+	default:
+		qsel = QSLT_BE;
+		RT_TRACE(rtlpriv, COMP_USB, DBG_DMESG,
+			 ("BE queue, set qsel = 0x%x\n", QSLT_BE));
+		break;
+	}
+out:
+	return qsel;
+}
+
+/* =============================================================== */
+
+/*----------------------------------------------------------------------
+ *
+ *	Rx handler
+ *
+ *---------------------------------------------------------------------- */
+bool rtl92cu_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *stats,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *p_desc, struct sk_buff *skb)
+{
+	struct rx_fwinfo_92c *p_drvinfo;
+	struct rx_desc_92c *pdesc = (struct rx_desc_92c *)p_desc;
+	u32 phystatus = GET_RX_DESC_PHY_STATUS(pdesc);
+
+	stats->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
+	stats->rx_drvinfo_size = (u8)GET_RX_DESC_DRVINFO_SIZE(pdesc) *
+				 RX_DRV_INFO_SIZE_UNIT;
+	stats->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	stats->icv = (u16) GET_RX_DESC_ICV(pdesc);
+	stats->crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	stats->hwerror = (stats->crc | stats->icv);
+	stats->decrypted = !GET_RX_DESC_SWDEC(pdesc);
+	stats->rate = (u8) GET_RX_DESC_RX_MCS(pdesc);
+	stats->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
+	stats->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	stats->isampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
+				   && (GET_RX_DESC_FAGGR(pdesc) == 1));
+	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	rx_status->freq = hw->conf.channel->center_freq;
+	rx_status->band = hw->conf.channel->band;
+	if (GET_RX_DESC_CRC32(pdesc))
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+	if (!GET_RX_DESC_SWDEC(pdesc))
+		rx_status->flag |= RX_FLAG_DECRYPTED;
+	if (GET_RX_DESC_BW(pdesc))
+		rx_status->flag |= RX_FLAG_40MHZ;
+	if (GET_RX_DESC_RX_HT(pdesc))
+		rx_status->flag |= RX_FLAG_HT;
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
+	if (stats->decrypted)
+		rx_status->flag |= RX_FLAG_DECRYPTED;
+	rx_status->rate_idx = _rtl92c_rate_mapping(hw,
+						(bool)GET_RX_DESC_RX_HT(pdesc),
+						(u8)GET_RX_DESC_RX_MCS(pdesc),
+						(bool)GET_RX_DESC_PAGGR(pdesc));
+	rx_status->mactime = GET_RX_DESC_TSFL(pdesc);
+	if (phystatus == true) {
+		p_drvinfo = (struct rx_fwinfo_92c *)(pdesc + RTL_RX_DESC_SIZE);
+		rtl92c_translate_rx_signal_stuff(hw, skb, stats, pdesc,
+						 p_drvinfo);
+	}
+	/*rx_status->qual = stats->signal; */
+	rx_status->signal = stats->rssi + 10;
+	/*rx_status->noise = -stats->noise; */
+	return true;
+}
+
+#define RTL_RX_DRV_INFO_UNIT		8
+
+static void _rtl_rx_process(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct ieee80211_rx_status *rx_status =
+		 (struct ieee80211_rx_status *)IEEE80211_SKB_RXCB(skb);
+	u32 skb_len, pkt_len, drvinfo_len;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *rxdesc;
+	struct rtl_stats stats = {
+		.signal = 0,
+		.noise = -98,
+		.rate = 0,
+	};
+	struct rx_fwinfo_92c *p_drvinfo;
+	bool bv;
+	__le16 fc;
+	struct ieee80211_hdr *hdr;
+
+	memset(rx_status, 0, sizeof(rx_status));
+	rxdesc	= skb->data;
+	skb_len	= skb->len;
+	drvinfo_len = (GET_RX_DESC_DRVINFO_SIZE(rxdesc) * RTL_RX_DRV_INFO_UNIT);
+	pkt_len		= GET_RX_DESC_PKT_LEN(rxdesc);
+	/* TODO: Error recovery. drop this skb or something. */
+	WARN_ON(skb_len < (pkt_len + RTL_RX_DESC_SIZE + drvinfo_len));
+	stats.length = (u16) GET_RX_DESC_PKT_LEN(rxdesc);
+	stats.rx_drvinfo_size = (u8)GET_RX_DESC_DRVINFO_SIZE(rxdesc) *
+				RX_DRV_INFO_SIZE_UNIT;
+	stats.rx_bufshift = (u8) (GET_RX_DESC_SHIFT(rxdesc) & 0x03);
+	stats.icv = (u16) GET_RX_DESC_ICV(rxdesc);
+	stats.crc = (u16) GET_RX_DESC_CRC32(rxdesc);
+	stats.hwerror = (stats.crc | stats.icv);
+	stats.decrypted = !GET_RX_DESC_SWDEC(rxdesc);
+	stats.rate = (u8) GET_RX_DESC_RX_MCS(rxdesc);
+	stats.shortpreamble = (u16) GET_RX_DESC_SPLCP(rxdesc);
+	stats.isampdu = (bool) ((GET_RX_DESC_PAGGR(rxdesc) == 1)
+				   && (GET_RX_DESC_FAGGR(rxdesc) == 1));
+	stats.timestamp_low = GET_RX_DESC_TSFL(rxdesc);
+	stats.rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(rxdesc);
+	/* TODO: is center_freq changed when doing scan? */
+	/* TODO: Shall we add protection or just skip those two step? */
+	rx_status->freq = hw->conf.channel->center_freq;
+	rx_status->band = hw->conf.channel->band;
+	if (GET_RX_DESC_CRC32(rxdesc))
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+	if (!GET_RX_DESC_SWDEC(rxdesc))
+		rx_status->flag |= RX_FLAG_DECRYPTED;
+	if (GET_RX_DESC_BW(rxdesc))
+		rx_status->flag |= RX_FLAG_40MHZ;
+	if (GET_RX_DESC_RX_HT(rxdesc))
+		rx_status->flag |= RX_FLAG_HT;
+	/* Data rate */
+	rx_status->rate_idx = _rtl92c_rate_mapping(hw,
+						(bool)GET_RX_DESC_RX_HT(rxdesc),
+						(u8)GET_RX_DESC_RX_MCS(rxdesc),
+						(bool)GET_RX_DESC_PAGGR(rxdesc)
+						);
+	/*  There is a phy status after this rx descriptor. */
+	if (GET_RX_DESC_PHY_STATUS(rxdesc)) {
+		p_drvinfo = (struct rx_fwinfo_92c *)(rxdesc + RTL_RX_DESC_SIZE);
+		rtl92c_translate_rx_signal_stuff(hw, skb, &stats,
+				 (struct rx_desc_92c *)rxdesc, p_drvinfo);
+	}
+	skb_pull(skb, (drvinfo_len + RTL_RX_DESC_SIZE));
+	hdr = (struct ieee80211_hdr *)(skb->data);
+	fc = hdr->frame_control;
+	bv = ieee80211_is_probe_resp(fc);
+	if (bv)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("Got probe response frame.\n"));
+	if (ieee80211_is_beacon(fc))
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("Got beacon frame.\n"));
+	if (ieee80211_is_data(fc))
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, ("Got data frame.\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 ("Fram: fc = 0x%X addr1 = 0x%02X:0x%02X:0x%02X:0x%02X:0x%02X:"
+		 "0x%02X\n", fc, (u32)hdr->addr1[0], (u32)hdr->addr1[1],
+		 (u32)hdr->addr1[2], (u32)hdr->addr1[3], (u32)hdr->addr1[4],
+		 (u32)hdr->addr1[5]));
+	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
+	ieee80211_rx_irqsafe(hw, skb);
+}
+
+void  rtl8192cu_rx_hdl(struct ieee80211_hw *hw, struct sk_buff * skb)
+{
+	_rtl_rx_process(hw, skb);
+}
+
+void rtl8192c_rx_segregate_hdl(
+	struct ieee80211_hw *hw,
+	struct sk_buff *skb,
+	struct sk_buff_head *skb_list)
+{
+}
+
+/*----------------------------------------------------------------------
+ *
+ *	Tx handler
+ *
+ *---------------------------------------------------------------------- */
+void rtl8192c_tx_cleanup(struct ieee80211_hw *hw, struct sk_buff  *skb)
+{
+}
+
+int rtl8192c_tx_post_hdl(struct ieee80211_hw *hw, struct urb *urb,
+			 struct sk_buff *skb)
+{
+	return 0;
+}
+
+struct sk_buff *rtl8192c_tx_aggregate_hdl(struct ieee80211_hw *hw,
+					   struct sk_buff_head *list)
+{
+	return skb_dequeue(list);
+}
+
+/*======================================== trx ===============================*/
+
+static void _rtl_fill_usb_tx_desc(u8 *txdesc)
+{
+	SET_TX_DESC_OWN(txdesc, 1);
+	SET_TX_DESC_LAST_SEG(txdesc, 1);
+	SET_TX_DESC_FIRST_SEG(txdesc, 1);
+}
+/**
+ *	For HW recovery information
+ */
+static void _rtl_tx_desc_checksum(u8 *txdesc)
+{
+	u16 *ptr = (u16 *)txdesc;
+	u16	checksum = 0;
+	u32 index;
+
+	/* Clear first */
+	SET_TX_DESC_TX_DESC_CHECKSUM(txdesc, 0);
+	for (index = 0; index < 16; index++)
+		checksum = checksum ^ (*(ptr + index));
+	SET_TX_DESC_TX_DESC_CHECKSUM(txdesc, checksum);
+}
+
+void rtl92cu_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			  struct ieee80211_tx_info *info, struct sk_buff *skb,
+			  unsigned int queue_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool defaultadapter = true;
+	struct ieee80211_sta *sta;
+	struct rtl_tcb_desc tcb_desc;
+	u8 *qc = ieee80211_get_qos_ctl(hdr);
+	u8 tid = qc[0] & IEEE80211_QOS_CTL_TID_MASK;
+	u16 seq_number;
+	__le16 fc = hdr->frame_control;
+	u8 rate_flag = info->control.rates[0].flags;
+	u16 pktlen = skb->len;
+	enum rtl_desc_qsel fw_qsel = _rtl8192cu_mq_to_descq(hw, fc,
+						skb_get_queue_mapping(skb));
+	u8 *txdesc;
+
+	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
+	rtl_get_tcb_desc(hw, info, skb, &tcb_desc);
+	txdesc = (u8 *)skb_push(skb, RTL_TX_HEADER_SIZE);
+	memset(txdesc, 0, RTL_TX_HEADER_SIZE);
+	SET_TX_DESC_PKT_SIZE(txdesc, pktlen);
+	SET_TX_DESC_LINIP(txdesc, 0);
+	SET_TX_DESC_PKT_OFFSET(txdesc, RTL_DUMMY_OFFSET);
+	SET_TX_DESC_OFFSET(txdesc, RTL_TX_HEADER_SIZE);
+	SET_TX_DESC_TX_RATE(txdesc, tcb_desc.hw_rate);
+	if (tcb_desc.use_shortgi || tcb_desc.use_shortpreamble)
+		SET_TX_DESC_DATA_SHORTGI(txdesc, 1);
+	if (mac->tids[tid].agg.agg_state == RTL_AGG_ON &&
+		    info->flags & IEEE80211_TX_CTL_AMPDU) {
+		SET_TX_DESC_AGG_ENABLE(txdesc, 1);
+		SET_TX_DESC_MAX_AGG_NUM(txdesc, 0x14);
+	} else {
+		SET_TX_DESC_AGG_BREAK(txdesc, 1);
+	}
+	SET_TX_DESC_SEQ(txdesc, seq_number);
+	SET_TX_DESC_RTS_ENABLE(txdesc, ((tcb_desc.rts_enable &&
+			       !tcb_desc.cts_enable) ? 1 : 0));
+	SET_TX_DESC_HW_RTS_ENABLE(txdesc, ((tcb_desc.rts_enable ||
+				  tcb_desc.cts_enable) ? 1 : 0));
+	SET_TX_DESC_CTS2SELF(txdesc, ((tcb_desc.cts_enable) ? 1 : 0));
+	SET_TX_DESC_RTS_STBC(txdesc, ((tcb_desc.rts_stbc) ? 1 : 0));
+	SET_TX_DESC_RTS_RATE(txdesc, tcb_desc.rts_rate);
+	SET_TX_DESC_RTS_BW(txdesc, 0);
+	SET_TX_DESC_RTS_SC(txdesc, tcb_desc.rts_sc);
+	SET_TX_DESC_RTS_SHORT(txdesc,
+			      ((tcb_desc.rts_rate <= DESC92C_RATE54M) ?
+			       (tcb_desc.rts_use_shortpreamble ? 1 : 0)
+			       : (tcb_desc.rts_use_shortgi ? 1 : 0)));
+	if (mac->bw_40) {
+		if (tcb_desc.packet_bw) {
+			SET_TX_DESC_DATA_BW(txdesc, 1);
+			SET_TX_DESC_DATA_SC(txdesc, 3);
+		} else {
+			SET_TX_DESC_DATA_BW(txdesc, 0);
+				if (rate_flag & IEEE80211_TX_RC_DUP_DATA)
+					SET_TX_DESC_DATA_SC(txdesc,
+							  mac->cur_40_prime_sc);
+			}
+	} else {
+		SET_TX_DESC_DATA_BW(txdesc, 0);
+		SET_TX_DESC_DATA_SC(txdesc, 0);
+	}
+	rcu_read_lock();
+	sta = ieee80211_find_sta(mac->vif, mac->bssid);
+	if (sta) {
+		u8 ampdu_density = sta->ht_cap.ampdu_density;
+		SET_TX_DESC_AMPDU_DENSITY(txdesc, ampdu_density);
+	}
+	rcu_read_unlock();
+	if (info->control.hw_key) {
+		struct ieee80211_key_conf *keyconf = info->control.hw_key;
+		switch (keyconf->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+		case WLAN_CIPHER_SUITE_TKIP:
+			SET_TX_DESC_SEC_TYPE(txdesc, 0x1);
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			SET_TX_DESC_SEC_TYPE(txdesc, 0x3);
+			break;
+		default:
+			SET_TX_DESC_SEC_TYPE(txdesc, 0x0);
+			break;
+		}
+	}
+	SET_TX_DESC_PKT_ID(txdesc, 0);
+	SET_TX_DESC_QUEUE_SEL(txdesc, fw_qsel);
+	SET_TX_DESC_DATA_RATE_FB_LIMIT(txdesc, 0x1F);
+	SET_TX_DESC_RTS_RATE_FB_LIMIT(txdesc, 0xF);
+	SET_TX_DESC_DISABLE_FB(txdesc, 0);
+	SET_TX_DESC_USE_RATE(txdesc, tcb_desc.use_driver_rate ? 1 : 0);
+	if (ieee80211_is_data_qos(fc)) {
+		if (mac->rdg_en) {
+			RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+				 ("Enable RDG function.\n"));
+			SET_TX_DESC_RDG_ENABLE(txdesc, 1);
+			SET_TX_DESC_HTC(txdesc, 1);
+		}
+	}
+	if (rtlpriv->dm.useramask) {
+		SET_TX_DESC_RATE_ID(txdesc, tcb_desc.ratr_index);
+		SET_TX_DESC_MACID(txdesc, tcb_desc.mac_id);
+	} else {
+		SET_TX_DESC_RATE_ID(txdesc, 0xC + tcb_desc.ratr_index);
+		SET_TX_DESC_MACID(txdesc, tcb_desc.ratr_index);
+	}
+	if ((!ieee80211_is_data_qos(fc)) && ppsc->leisure_ps &&
+	      ppsc->fwctrl_lps) {
+		SET_TX_DESC_HWSEQ_EN(txdesc, 1);
+		SET_TX_DESC_PKT_ID(txdesc, 8);
+		if (!defaultadapter)
+			SET_TX_DESC_QOS(txdesc, 1);
+	}
+	if (ieee80211_has_morefrags(fc))
+		SET_TX_DESC_MORE_FRAG(txdesc, 1);
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+		SET_TX_DESC_BMC(txdesc, 1);
+	_rtl_fill_usb_tx_desc(txdesc);
+	_rtl_tx_desc_checksum(txdesc);
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, (" %s ==>\n", __func__));
+}
+
+void rtl92cu_fill_fake_txdesc(struct ieee80211_hw *hw, u8 * pDesc,
+			      u32 buffer_len, bool bIsPsPoll)
+{
+	/* Clear all status */
+	memset(pDesc, 0, RTL_TX_HEADER_SIZE);
+	SET_TX_DESC_FIRST_SEG(pDesc, 1); /* bFirstSeg; */
+	SET_TX_DESC_LAST_SEG(pDesc, 1); /* bLastSeg; */
+	SET_TX_DESC_OFFSET(pDesc, RTL_TX_HEADER_SIZE); /* Offset = 32 */
+	SET_TX_DESC_PKT_SIZE(pDesc, buffer_len); /* Buffer size + command hdr */
+	SET_TX_DESC_QUEUE_SEL(pDesc, QSLT_MGNT); /* Fixed queue of Mgnt queue */
+	/* Set NAVUSEHDR to prevent Ps-poll AId filed to be changed to error
+	 * vlaue by Hw. */
+	if (bIsPsPoll) {
+		SET_TX_DESC_NAV_USE_HDR(pDesc, 1);
+	} else {
+		SET_TX_DESC_HWSEQ_EN(pDesc, 1); /* Hw set sequence number */
+		SET_TX_DESC_PKT_ID(pDesc, 0x100); /* set bit3 to 1. */
+	}
+	SET_TX_DESC_USE_RATE(pDesc, 1); /* use data rate which is set by Sw */
+	SET_TX_DESC_OWN(pDesc, 1);
+	SET_TX_DESC_TX_RATE(pDesc, DESC92C_RATE1M);
+	_rtl_tx_desc_checksum(pDesc);
+}
+
+void rtl92cu_tx_fill_cmddesc(struct ieee80211_hw *hw,
+			     u8 *pdesc, bool firstseg,
+			     bool lastseg, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 fw_queue = QSLT_BEACON;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
+	__le16 fc = hdr->frame_control;
+
+	memset((void *)pdesc, 0, RTL_TX_HEADER_SIZE);
+	if (firstseg)
+		SET_TX_DESC_OFFSET(pdesc, RTL_TX_HEADER_SIZE);
+	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
+	SET_TX_DESC_SEQ(pdesc, 0);
+	SET_TX_DESC_LINIP(pdesc, 0);
+	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+	SET_TX_DESC_RATE_ID(pdesc, 7);
+	SET_TX_DESC_MACID(pdesc, 0);
+	SET_TX_DESC_OWN(pdesc, 1);
+	SET_TX_DESC_PKT_SIZE((u8 *) pdesc, (u16) (skb->len));
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+	SET_TX_DESC_OFFSET(pdesc, 0x20);
+	SET_TX_DESC_USE_RATE(pdesc, 1);
+	if (!ieee80211_is_data_qos(fc)) {
+		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+		SET_TX_DESC_PKT_ID(pdesc, 8);
+	}
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD, "H2C Tx Cmd Content\n",
+		      pdesc, RTL_TX_DESC_SIZE);
+}
+
+bool rtl92cu_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	return true;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h
new file mode 100644
index 000000000000..b396d46edbb7
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h
@@ -0,0 +1,430 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92CU_TRX_H__
+#define __RTL92CU_TRX_H__
+
+#define RTL92C_USB_BULK_IN_NUM			1
+#define RTL92C_NUM_RX_URBS			8
+#define RTL92C_NUM_TX_URBS			32
+
+#define RTL92C_SIZE_MAX_RX_BUFFER		15360   /* 8192 */
+#define RX_DRV_INFO_SIZE_UNIT			8
+
+enum usb_rx_agg_mode {
+	USB_RX_AGG_DISABLE,
+	USB_RX_AGG_DMA,
+	USB_RX_AGG_USB,
+	USB_RX_AGG_DMA_USB
+};
+
+#define TX_SELE_HQ				BIT(0)	/* High Queue */
+#define TX_SELE_LQ				BIT(1)	/* Low Queue */
+#define TX_SELE_NQ				BIT(2)	/* Normal Queue */
+
+#define RTL_USB_TX_AGG_NUM_DESC			5
+
+#define RTL_USB_RX_AGG_PAGE_NUM			4
+#define RTL_USB_RX_AGG_PAGE_TIMEOUT		3
+
+#define RTL_USB_RX_AGG_BLOCK_NUM		5
+#define RTL_USB_RX_AGG_BLOCK_TIMEOUT		3
+
+/*======================== rx status =========================================*/
+
+struct rx_drv_info_92c {
+	/*
+	 * Driver info contain PHY status and other variabel size info
+	 * PHY Status content as below
+	 */
+
+	/* DWORD 0 */
+	u8 gain_trsw[4];
+
+	/* DWORD 1 */
+	u8 pwdb_all;
+	u8 cfosho[4];
+
+	/* DWORD 2 */
+	u8 cfotail[4];
+
+	/* DWORD 3 */
+	s8 rxevm[2];
+	s8 rxsnr[4];
+
+	/* DWORD 4 */
+	u8 pdsnr[2];
+
+	/* DWORD 5 */
+	u8 csi_current[2];
+	u8 csi_target[2];
+
+	/* DWORD 6 */
+	u8 sigevm;
+	u8 max_ex_pwr;
+	u8 ex_intf_flag:1;
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 reserve:4;
+} __packed;
+
+/* Define a macro that takes a le32 word, converts it to host ordering,
+ * right shifts by a specified count, creates a mask of the specified
+ * bit count, and extracts that number of bits.
+ */
+
+#define SHIFT_AND_MASK_LE(__pdesc, __shift, __bits)		\
+	((le32_to_cpu(*(((__le32 *)(__pdesc)))) >> (__shift)) &	\
+	BIT_LEN_MASK_32(__bits))
+
+/* Define a macro that clears a bit field in an le32 word and
+ * sets the specified value into that bit field. The resulting
+ * value remains in le32 ordering; however, it is properly converted
+ * to host ordering for the clear and set operations before conversion
+ * back to le32.
+ */
+
+#define SET_BITS_OFFSET_LE(__pdesc, __shift, __len, __val)	\
+	(*(__le32 *)(__pdesc) = 				\
+	(cpu_to_le32((le32_to_cpu(*((__le32 *)(__pdesc))) &	\
+	(~(BIT_OFFSET_LEN_MASK_32((__shift), __len)))) |		\
+	(((u32)(__val) & BIT_LEN_MASK_32(__len)) << (__shift)))));
+
+/* macros to read various fields in RX descriptor */
+
+/* DWORD 0 */
+#define GET_RX_DESC_PKT_LEN(__rxdesc)		\
+	SHIFT_AND_MASK_LE((__rxdesc), 0, 14)
+#define GET_RX_DESC_CRC32(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 14, 1)
+#define GET_RX_DESC_ICV(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 15, 1)
+#define GET_RX_DESC_DRVINFO_SIZE(__rxdesc)	\
+	SHIFT_AND_MASK_LE(__rxdesc, 16, 4)
+#define GET_RX_DESC_SECURITY(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 20, 3)
+#define GET_RX_DESC_QOS(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 23, 1)
+#define GET_RX_DESC_SHIFT(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 24, 2)
+#define GET_RX_DESC_PHY_STATUS(__rxdesc)	\
+	SHIFT_AND_MASK_LE(__rxdesc, 26, 1)
+#define GET_RX_DESC_SWDEC(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 27, 1)
+#define GET_RX_DESC_LAST_SEG(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 28, 1)
+#define GET_RX_DESC_FIRST_SEG(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 29, 1)
+#define GET_RX_DESC_EOR(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 30, 1)
+#define GET_RX_DESC_OWN(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc, 31, 1)
+
+/* DWORD 1 */
+#define GET_RX_DESC_MACID(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 0, 5)
+#define GET_RX_DESC_TID(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 5, 4)
+#define GET_RX_DESC_PAGGR(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 14, 1)
+#define GET_RX_DESC_FAGGR(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 15, 1)
+#define GET_RX_DESC_A1_FIT(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 16, 4)
+#define GET_RX_DESC_A2_FIT(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 20, 4)
+#define GET_RX_DESC_PAM(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 24, 1)
+#define GET_RX_DESC_PWR(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 25, 1)
+#define GET_RX_DESC_MORE_DATA(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 26, 1)
+#define GET_RX_DESC_MORE_FRAG(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 27, 1)
+#define GET_RX_DESC_TYPE(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 28, 2)
+#define GET_RX_DESC_MC(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 30, 1)
+#define GET_RX_DESC_BC(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+4, 31, 1)
+
+/* DWORD 2 */
+#define GET_RX_DESC_SEQ(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+8, 0, 12)
+#define GET_RX_DESC_FRAG(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+8, 12, 4)
+#define GET_RX_DESC_USB_AGG_PKTNUM(__rxdesc)	\
+	SHIFT_AND_MASK_LE(__rxdesc+8, 16, 8)
+#define GET_RX_DESC_NEXT_IND(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+8, 30, 1)
+
+/* DWORD 3 */
+#define GET_RX_DESC_RX_MCS(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 0, 6)
+#define GET_RX_DESC_RX_HT(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 6, 1)
+#define GET_RX_DESC_AMSDU(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 7, 1)
+#define GET_RX_DESC_SPLCP(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 8, 1)
+#define GET_RX_DESC_BW(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 9, 1)
+#define GET_RX_DESC_HTC(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 10, 1)
+#define GET_RX_DESC_TCP_CHK_RPT(__rxdesc)	\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 11, 1)
+#define GET_RX_DESC_IP_CHK_RPT(__rxdesc)	\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 12, 1)
+#define GET_RX_DESC_TCP_CHK_VALID(__rxdesc)	\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 13, 1)
+#define GET_RX_DESC_HWPC_ERR(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 14, 1)
+#define GET_RX_DESC_HWPC_IND(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 15, 1)
+#define GET_RX_DESC_IV0(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+12, 16, 16)
+
+/* DWORD 4 */
+#define GET_RX_DESC_IV1(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+16, 0, 32)
+
+/* DWORD 5 */
+#define GET_RX_DESC_TSFL(__rxdesc)		\
+	SHIFT_AND_MASK_LE(__rxdesc+20, 0, 32)
+
+/*======================= tx desc ============================================*/
+
+/* macros to set various fields in TX descriptor */
+
+/* Dword 0 */
+#define SET_TX_DESC_PKT_SIZE(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 0, 16, __value)
+#define SET_TX_DESC_OFFSET(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 16, 8, __value)
+#define SET_TX_DESC_BMC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 24, 1, __value)
+#define SET_TX_DESC_HTC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 25, 1, __value)
+#define SET_TX_DESC_LAST_SEG(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 26, 1, __value)
+#define SET_TX_DESC_FIRST_SEG(__txdesc, __value)	\
+	 SET_BITS_OFFSET_LE(__txdesc, 27, 1, __value)
+#define SET_TX_DESC_LINIP(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 28, 1, __value)
+#define SET_TX_DESC_NO_ACM(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 29, 1, __value)
+#define SET_TX_DESC_GF(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 30, 1, __value)
+#define SET_TX_DESC_OWN(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc, 31, 1, __value)
+
+
+/* Dword 1 */
+#define SET_TX_DESC_MACID(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+4, 0, 5, __value)
+#define SET_TX_DESC_AGG_ENABLE(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 5, 1, __value)
+#define SET_TX_DESC_AGG_BREAK(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 6, 1, __value)
+#define SET_TX_DESC_RDG_ENABLE(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 7, 1, __value)
+#define SET_TX_DESC_QUEUE_SEL(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 8, 5, __value)
+#define SET_TX_DESC_RDG_NAV_EXT(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 13, 1, __value)
+#define SET_TX_DESC_LSIG_TXOP_EN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 14, 1, __value)
+#define SET_TX_DESC_PIFS(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+4, 15, 1, __value)
+#define SET_TX_DESC_RATE_ID(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+4, 16, 4, __value)
+#define SET_TX_DESC_RA_BRSR_ID(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 16, 4, __value)
+#define SET_TX_DESC_NAV_USE_HDR(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 20, 1, __value)
+#define SET_TX_DESC_EN_DESC_ID(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 21, 1, __value)
+#define SET_TX_DESC_SEC_TYPE(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+4, 22, 2, __value)
+#define SET_TX_DESC_PKT_OFFSET(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+4, 26, 5, __value)
+
+/* Dword 2 */
+#define SET_TX_DESC_RTS_RC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 0, 6, __value)
+#define SET_TX_DESC_DATA_RC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 6, 6, __value)
+#define SET_TX_DESC_BAR_RTY_TH(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+8, 14, 2, __value)
+#define SET_TX_DESC_MORE_FRAG(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+8, 17, 1, __value)
+#define SET_TX_DESC_RAW(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 18, 1, __value)
+#define SET_TX_DESC_CCX(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 19, 1, __value)
+#define SET_TX_DESC_AMPDU_DENSITY(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+8, 20, 3, __value)
+#define SET_TX_DESC_ANTSEL_A(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 24, 1, __value)
+#define SET_TX_DESC_ANTSEL_B(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 25, 1, __value)
+#define SET_TX_DESC_TX_ANT_CCK(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+8, 26, 2, __value)
+#define SET_TX_DESC_TX_ANTL(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+8, 28, 2, __value)
+#define SET_TX_DESC_TX_ANT_HT(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+8, 30, 2, __value)
+
+/* Dword 3 */
+#define SET_TX_DESC_NEXT_HEAP_PAGE(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+12, 0, 8, __value)
+#define SET_TX_DESC_TAIL_PAGE(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+12, 8, 8, __value)
+#define SET_TX_DESC_SEQ(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+12, 16, 12, __value)
+#define SET_TX_DESC_PKT_ID(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+12, 28, 4, __value)
+
+/* Dword 4 */
+#define SET_TX_DESC_RTS_RATE(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 0, 5, __value)
+#define SET_TX_DESC_AP_DCFE(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 5, 1, __value)
+#define SET_TX_DESC_QOS(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 6, 1, __value)
+#define SET_TX_DESC_HWSEQ_EN(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 7, 1, __value)
+#define SET_TX_DESC_USE_RATE(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 8, 1, __value)
+#define SET_TX_DESC_DISABLE_RTS_FB(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 9, 1, __value)
+#define SET_TX_DESC_DISABLE_FB(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 10, 1, __value)
+#define SET_TX_DESC_CTS2SELF(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 11, 1, __value)
+#define SET_TX_DESC_RTS_ENABLE(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 12, 1, __value)
+#define SET_TX_DESC_HW_RTS_ENABLE(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 13, 1, __value)
+#define SET_TX_DESC_WAIT_DCTS(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 18, 1, __value)
+#define SET_TX_DESC_CTS2AP_EN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 19, 1, __value)
+#define SET_TX_DESC_DATA_SC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 20, 2, __value)
+#define SET_TX_DESC_DATA_STBC(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 22, 2, __value)
+#define SET_TX_DESC_DATA_SHORT(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 24, 1, __value)
+#define SET_TX_DESC_DATA_BW(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 25, 1, __value)
+#define SET_TX_DESC_RTS_SHORT(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+16, 26, 1, __value)
+#define SET_TX_DESC_RTS_BW(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 27, 1, __value)
+#define SET_TX_DESC_RTS_SC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 28, 2, __value)
+#define SET_TX_DESC_RTS_STBC(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+16, 30, 2, __value)
+
+/* Dword 5 */
+#define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
+	SET_BITS_OFFSET_LE(__pdesc+20, 0, 6, __val)
+#define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val)	\
+	SET_BITS_OFFSET_LE(__pdesc+20, 6, 1, __val)
+#define SET_TX_DESC_CCX_TAG(__pdesc, __val)		\
+	SET_BITS_OFFSET_LE(__pdesc+20, 7, 1, __val)
+#define SET_TX_DESC_DATA_RATE_FB_LIMIT(__txdesc, __value) \
+	SET_BITS_OFFSET_LE(__txdesc+20, 8, 5, __value)
+#define SET_TX_DESC_RTS_RATE_FB_LIMIT(__txdesc, __value) \
+	SET_BITS_OFFSET_LE(__txdesc+20, 13, 4, __value)
+#define SET_TX_DESC_RETRY_LIMIT_ENABLE(__txdesc, __value) \
+	SET_BITS_OFFSET_LE(__txdesc+20, 17, 1, __value)
+#define SET_TX_DESC_DATA_RETRY_LIMIT(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+20, 18, 6, __value)
+#define SET_TX_DESC_USB_TXAGG_NUM(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+20, 24, 8, __value)
+
+/* Dword 6 */
+#define SET_TX_DESC_TXAGC_A(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+24, 0, 5, __value)
+#define SET_TX_DESC_TXAGC_B(__txdesc, __value)		\
+	SET_BITS_OFFSET_LE(__txdesc+24, 5, 5, __value)
+#define SET_TX_DESC_USB_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+24, 10, 1, __value)
+#define SET_TX_DESC_MAX_AGG_NUM(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+24, 11, 5, __value)
+#define SET_TX_DESC_MCSG1_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+24, 16, 4, __value)
+#define SET_TX_DESC_MCSG2_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+24, 20, 4, __value)
+#define SET_TX_DESC_MCSG3_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+24, 24, 4, __value)
+#define SET_TX_DESC_MCSG7_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+24, 28, 4, __value)
+
+/* Dword 7 */
+#define SET_TX_DESC_TX_DESC_CHECKSUM(__txdesc, __value) \
+	SET_BITS_OFFSET_LE(__txdesc+28, 0, 16, __value)
+#define SET_TX_DESC_MCSG4_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+28, 16, 4, __value)
+#define SET_TX_DESC_MCSG5_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+28, 20, 4, __value)
+#define SET_TX_DESC_MCSG6_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+28, 24, 4, __value)
+#define SET_TX_DESC_MCSG15_MAX_LEN(__txdesc, __value)	\
+	SET_BITS_OFFSET_LE(__txdesc+28, 28, 4, __value)
+
+
+int  rtl8192cu_endpoint_mapping(struct ieee80211_hw *hw);
+u16 rtl8192cu_mq_to_hwq(__le16 fc, u16 mac80211_queue_index);
+bool rtl92cu_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *stats,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *p_desc, struct sk_buff *skb);
+void  rtl8192cu_rx_hdl(struct ieee80211_hw *hw, struct sk_buff * skb);
+void rtl8192c_rx_segregate_hdl(struct ieee80211_hw *, struct sk_buff *,
+			       struct sk_buff_head *);
+void rtl8192c_tx_cleanup(struct ieee80211_hw *hw, struct sk_buff  *skb);
+int rtl8192c_tx_post_hdl(struct ieee80211_hw *hw, struct urb *urb,
+			 struct sk_buff *skb);
+struct sk_buff *rtl8192c_tx_aggregate_hdl(struct ieee80211_hw *,
+					   struct sk_buff_head *);
+void rtl92cu_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			  struct ieee80211_tx_info *info, struct sk_buff *skb,
+			  unsigned int queue_index);
+void rtl92cu_fill_fake_txdesc(struct ieee80211_hw *hw, u8 * pDesc,
+			      u32 buffer_len, bool bIsPsPoll);
+void rtl92cu_tx_fill_cmddesc(struct ieee80211_hw *hw,
+			     u8 *pdesc, bool b_firstseg,
+			     bool b_lastseg, struct sk_buff *skb);
+bool rtl92cu_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
new file mode 100644
index 000000000000..a4b2613d6a8c
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -0,0 +1,1035 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2011  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ *****************************************************************************/
+#include <linux/usb.h>
+#include "core.h"
+#include "wifi.h"
+#include "usb.h"
+#include "base.h"
+#include "ps.h"
+
+#define	REALTEK_USB_VENQT_READ			0xC0
+#define	REALTEK_USB_VENQT_WRITE			0x40
+#define REALTEK_USB_VENQT_CMD_REQ		0x05
+#define	REALTEK_USB_VENQT_CMD_IDX		0x00
+
+#define REALTEK_USB_VENQT_MAX_BUF_SIZE		254
+
+static void usbctrl_async_callback(struct urb *urb)
+{
+	if (urb)
+		kfree(urb->context);
+}
+
+static int _usbctrl_vendorreq_async_write(struct usb_device *udev, u8 request,
+					  u16 value, u16 index, void *pdata,
+					  u16 len)
+{
+	int rc;
+	unsigned int pipe;
+	u8 reqtype;
+	struct usb_ctrlrequest *dr;
+	struct urb *urb;
+	struct rtl819x_async_write_data {
+		u8 data[REALTEK_USB_VENQT_MAX_BUF_SIZE];
+		struct usb_ctrlrequest dr;
+	} *buf;
+
+	pipe = usb_sndctrlpipe(udev, 0); /* write_out */
+	reqtype =  REALTEK_USB_VENQT_WRITE;
+
+	buf = kmalloc(sizeof(*buf), GFP_ATOMIC);
+	if (!buf)
+		return -ENOMEM;
+
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!urb) {
+		kfree(buf);
+		return -ENOMEM;
+	}
+
+	dr = &buf->dr;
+
+	dr->bRequestType = reqtype;
+	dr->bRequest = request;
+	dr->wValue = cpu_to_le16(value);
+	dr->wIndex = cpu_to_le16(index);
+	dr->wLength = cpu_to_le16(len);
+	memcpy(buf, pdata, len);
+	usb_fill_control_urb(urb, udev, pipe,
+			     (unsigned char *)dr, buf, len,
+			     usbctrl_async_callback, buf);
+	rc = usb_submit_urb(urb, GFP_ATOMIC);
+	if (rc < 0)
+		kfree(buf);
+	usb_free_urb(urb);
+	return rc;
+}
+
+static int _usbctrl_vendorreq_sync_read(struct usb_device *udev, u8 request,
+					u16 value, u16 index, void *pdata,
+					u16 len)
+{
+	unsigned int pipe;
+	int status;
+	u8 reqtype;
+
+	pipe = usb_rcvctrlpipe(udev, 0); /* read_in */
+	reqtype =  REALTEK_USB_VENQT_READ;
+
+	status = usb_control_msg(udev, pipe, request, reqtype, value, index,
+				 pdata, len, 0); /* max. timeout */
+
+	if (status < 0)
+		printk(KERN_ERR "reg 0x%x, usbctrl_vendorreq TimeOut! "
+		       "status:0x%x value=0x%x\n", value, status,
+		       *(u32 *)pdata);
+	return status;
+}
+
+static u32 _usb_read_sync(struct usb_device *udev, u32 addr, u16 len)
+{
+	u8 request;
+	u16 wvalue;
+	u16 index;
+	u32 *data;
+	u32 ret;
+
+	data = kmalloc(sizeof(u32), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+	request = REALTEK_USB_VENQT_CMD_REQ;
+	index = REALTEK_USB_VENQT_CMD_IDX; /* n/a */
+
+	wvalue = (u16)addr;
+	_usbctrl_vendorreq_sync_read(udev, request, wvalue, index, data, len);
+	ret = *data;
+	kfree(data);
+	return ret;
+}
+
+static u8 _usb_read8_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	return (u8)_usb_read_sync(to_usb_device(dev), addr, 1);
+}
+
+static u16 _usb_read16_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	return (u16)_usb_read_sync(to_usb_device(dev), addr, 2);
+}
+
+static u32 _usb_read32_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	return _usb_read_sync(to_usb_device(dev), addr, 4);
+}
+
+static void _usb_write_async(struct usb_device *udev, u32 addr, u32 val,
+			     u16 len)
+{
+	u8 request;
+	u16 wvalue;
+	u16 index;
+	u32 data;
+
+	request = REALTEK_USB_VENQT_CMD_REQ;
+	index = REALTEK_USB_VENQT_CMD_IDX; /* n/a */
+	wvalue = (u16)(addr&0x0000ffff);
+	data = val;
+	_usbctrl_vendorreq_async_write(udev, request, wvalue, index, &data,
+				       len);
+}
+
+static void _usb_write8_async(struct rtl_priv *rtlpriv, u32 addr, u8 val)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	_usb_write_async(to_usb_device(dev), addr, val, 1);
+}
+
+static void _usb_write16_async(struct rtl_priv *rtlpriv, u32 addr, u16 val)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	_usb_write_async(to_usb_device(dev), addr, val, 2);
+}
+
+static void _usb_write32_async(struct rtl_priv *rtlpriv, u32 addr, u32 val)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	_usb_write_async(to_usb_device(dev), addr, val, 4);
+}
+
+static int _usb_nbytes_read_write(struct usb_device *udev, bool read, u32 addr,
+				  u16 len, u8 *pdata)
+{
+	int status;
+	u8 request;
+	u16 wvalue;
+	u16 index;
+
+	request = REALTEK_USB_VENQT_CMD_REQ;
+	index = REALTEK_USB_VENQT_CMD_IDX; /* n/a */
+	wvalue = (u16)addr;
+	if (read)
+		status = _usbctrl_vendorreq_sync_read(udev, request, wvalue,
+						      index, pdata, len);
+	else
+		status = _usbctrl_vendorreq_async_write(udev, request, wvalue,
+							index, pdata, len);
+	return status;
+}
+
+static int _usb_readN_sync(struct rtl_priv *rtlpriv, u32 addr, u16 len,
+			   u8 *pdata)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	return _usb_nbytes_read_write(to_usb_device(dev), true, addr, len,
+				       pdata);
+}
+
+static int _usb_writeN_async(struct rtl_priv *rtlpriv, u32 addr, u16 len,
+			     u8 *pdata)
+{
+	struct device *dev = rtlpriv->io.dev;
+
+	return _usb_nbytes_read_write(to_usb_device(dev), false, addr, len,
+				      pdata);
+}
+
+static void _rtl_usb_io_handler_init(struct device *dev,
+				     struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->io.dev = dev;
+	mutex_init(&rtlpriv->io.bb_mutex);
+	rtlpriv->io.write8_async	= _usb_write8_async;
+	rtlpriv->io.write16_async	= _usb_write16_async;
+	rtlpriv->io.write32_async	= _usb_write32_async;
+	rtlpriv->io.writeN_async	= _usb_writeN_async;
+	rtlpriv->io.read8_sync		= _usb_read8_sync;
+	rtlpriv->io.read16_sync		= _usb_read16_sync;
+	rtlpriv->io.read32_sync		= _usb_read32_sync;
+	rtlpriv->io.readN_sync		= _usb_readN_sync;
+}
+
+static void _rtl_usb_io_handler_release(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	mutex_destroy(&rtlpriv->io.bb_mutex);
+}
+
+/**
+ *
+ *	Default aggregation handler. Do nothing and just return the oldest skb.
+ */
+static struct sk_buff *_none_usb_tx_aggregate_hdl(struct ieee80211_hw *hw,
+						  struct sk_buff_head *list)
+{
+	return skb_dequeue(list);
+}
+
+#define IS_HIGH_SPEED_USB(udev) \
+		((USB_SPEED_HIGH == (udev)->speed) ? true : false)
+
+static int _rtl_usb_init_tx(struct ieee80211_hw *hw)
+{
+	u32 i;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	rtlusb->max_bulk_out_size = IS_HIGH_SPEED_USB(rtlusb->udev)
+						    ? USB_HIGH_SPEED_BULK_SIZE
+						    : USB_FULL_SPEED_BULK_SIZE;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, ("USB Max Bulk-out Size=%d\n",
+		 rtlusb->max_bulk_out_size));
+
+	for (i = 0; i < __RTL_TXQ_NUM; i++) {
+		u32 ep_num = rtlusb->ep_map.ep_mapping[i];
+		if (!ep_num) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				 ("Invalid endpoint map setting!\n"));
+			return -EINVAL;
+		}
+	}
+
+	rtlusb->usb_tx_post_hdl =
+		 rtlpriv->cfg->usb_interface_cfg->usb_tx_post_hdl;
+	rtlusb->usb_tx_cleanup	=
+		 rtlpriv->cfg->usb_interface_cfg->usb_tx_cleanup;
+	rtlusb->usb_tx_aggregate_hdl =
+		 (rtlpriv->cfg->usb_interface_cfg->usb_tx_aggregate_hdl)
+		 ? rtlpriv->cfg->usb_interface_cfg->usb_tx_aggregate_hdl
+		 : &_none_usb_tx_aggregate_hdl;
+
+	init_usb_anchor(&rtlusb->tx_submitted);
+	for (i = 0; i < RTL_USB_MAX_EP_NUM; i++) {
+		skb_queue_head_init(&rtlusb->tx_skb_queue[i]);
+		init_usb_anchor(&rtlusb->tx_pending[i]);
+	}
+	return 0;
+}
+
+static int _rtl_usb_init_rx(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(usb_priv);
+
+	rtlusb->rx_max_size = rtlpriv->cfg->usb_interface_cfg->rx_max_size;
+	rtlusb->rx_urb_num = rtlpriv->cfg->usb_interface_cfg->rx_urb_num;
+	rtlusb->in_ep = rtlpriv->cfg->usb_interface_cfg->in_ep_num;
+	rtlusb->usb_rx_hdl = rtlpriv->cfg->usb_interface_cfg->usb_rx_hdl;
+	rtlusb->usb_rx_segregate_hdl =
+		rtlpriv->cfg->usb_interface_cfg->usb_rx_segregate_hdl;
+
+	printk(KERN_INFO "rtl8192cu: rx_max_size %d, rx_urb_num %d, in_ep %d\n",
+		rtlusb->rx_max_size, rtlusb->rx_urb_num, rtlusb->in_ep);
+	init_usb_anchor(&rtlusb->rx_submitted);
+	return 0;
+}
+
+static int _rtl_usb_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb_priv *usb_priv = rtl_usbpriv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(usb_priv);
+	int err;
+	u8 epidx;
+	struct usb_interface	*usb_intf = rtlusb->intf;
+	u8 epnums = usb_intf->cur_altsetting->desc.bNumEndpoints;
+
+	rtlusb->out_ep_nums = rtlusb->in_ep_nums = 0;
+	for (epidx = 0; epidx < epnums; epidx++) {
+		struct usb_endpoint_descriptor *pep_desc;
+		pep_desc = &usb_intf->cur_altsetting->endpoint[epidx].desc;
+
+		if (usb_endpoint_dir_in(pep_desc))
+			rtlusb->in_ep_nums++;
+		else if (usb_endpoint_dir_out(pep_desc))
+			rtlusb->out_ep_nums++;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 ("USB EP(0x%02x), MaxPacketSize=%d ,Interval=%d.\n",
+			 pep_desc->bEndpointAddress, pep_desc->wMaxPacketSize,
+			 pep_desc->bInterval));
+	}
+	if (rtlusb->in_ep_nums <  rtlpriv->cfg->usb_interface_cfg->in_ep_num)
+		return -EINVAL ;
+
+	/* usb endpoint mapping */
+	err = rtlpriv->cfg->usb_interface_cfg->usb_endpoint_mapping(hw);
+	rtlusb->usb_mq_to_hwq =  rtlpriv->cfg->usb_interface_cfg->usb_mq_to_hwq;
+	_rtl_usb_init_tx(hw);
+	_rtl_usb_init_rx(hw);
+	return err;
+}
+
+static int _rtl_usb_init_sw(struct ieee80211_hw *hw)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	rtlhal->hw = hw;
+	ppsc->inactiveps = false;
+	ppsc->leisure_ps = false;
+	ppsc->fwctrl_lps = false;
+	ppsc->reg_fwctrl_lps = 3;
+	ppsc->reg_max_lps_awakeintvl = 5;
+	ppsc->fwctrl_psmode = FW_PS_DTIM_MODE;
+
+	 /* IBSS */
+	mac->beacon_interval = 100;
+
+	 /* AMPDU */
+	mac->min_space_cfg = 0;
+	mac->max_mss_density = 0;
+
+	/* set sane AMPDU defaults */
+	mac->current_ampdu_density = 7;
+	mac->current_ampdu_factor = 3;
+
+	/* QOS */
+	rtlusb->acm_method = eAcmWay2_SW;
+
+	/* IRQ */
+	/* HIMR - turn all on */
+	rtlusb->irq_mask[0] = 0xFFFFFFFF;
+	/* HIMR_EX - turn all on */
+	rtlusb->irq_mask[1] = 0xFFFFFFFF;
+	rtlusb->disableHWSM =  true;
+	return 0;
+}
+
+#define __RADIO_TAP_SIZE_RSV	32
+
+static void _rtl_rx_completed(struct urb *urb);
+
+static struct sk_buff *_rtl_prep_rx_urb(struct ieee80211_hw *hw,
+					struct rtl_usb *rtlusb,
+					struct urb *urb,
+					gfp_t gfp_mask)
+{
+	struct sk_buff *skb;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	skb = __dev_alloc_skb((rtlusb->rx_max_size + __RADIO_TAP_SIZE_RSV),
+			       gfp_mask);
+	if (!skb) {
+		RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+			 ("Failed to __dev_alloc_skb!!\n"))
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* reserve some space for mac80211's radiotap */
+	skb_reserve(skb, __RADIO_TAP_SIZE_RSV);
+	usb_fill_bulk_urb(urb, rtlusb->udev,
+			  usb_rcvbulkpipe(rtlusb->udev, rtlusb->in_ep),
+			  skb->data, min(skb_tailroom(skb),
+			  (int)rtlusb->rx_max_size),
+			  _rtl_rx_completed, skb);
+
+	_rtl_install_trx_info(rtlusb, skb, rtlusb->in_ep);
+	return skb;
+}
+
+#undef __RADIO_TAP_SIZE_RSV
+
+static void _rtl_usb_rx_process_agg(struct ieee80211_hw *hw,
+				    struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *rxdesc = skb->data;
+	struct ieee80211_hdr *hdr;
+	bool unicast = false;
+	__le16 fc;
+	struct ieee80211_rx_status rx_status = {0};
+	struct rtl_stats stats = {
+		.signal = 0,
+		.noise = -98,
+		.rate = 0,
+	};
+
+	skb_pull(skb, RTL_RX_DESC_SIZE);
+	rtlpriv->cfg->ops->query_rx_desc(hw, &stats, &rx_status, rxdesc, skb);
+	skb_pull(skb, (stats.rx_drvinfo_size + stats.rx_bufshift));
+	hdr = (struct ieee80211_hdr *)(skb->data);
+	fc = hdr->frame_control;
+	if (!stats.crc) {
+		memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
+
+		if (is_broadcast_ether_addr(hdr->addr1)) {
+			/*TODO*/;
+		} else if (is_multicast_ether_addr(hdr->addr1)) {
+			/*TODO*/
+		} else {
+			unicast = true;
+			rtlpriv->stats.rxbytesunicast +=  skb->len;
+		}
+
+		rtl_is_special_data(hw, skb, false);
+
+		if (ieee80211_is_data(fc)) {
+			rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
+
+			if (unicast)
+				rtlpriv->link_info.num_rx_inperiod++;
+		}
+	}
+}
+
+static void _rtl_usb_rx_process_noagg(struct ieee80211_hw *hw,
+				      struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *rxdesc = skb->data;
+	struct ieee80211_hdr *hdr;
+	bool unicast = false;
+	__le16 fc;
+	struct ieee80211_rx_status rx_status = {0};
+	struct rtl_stats stats = {
+		.signal = 0,
+		.noise = -98,
+		.rate = 0,
+	};
+
+	skb_pull(skb, RTL_RX_DESC_SIZE);
+	rtlpriv->cfg->ops->query_rx_desc(hw, &stats, &rx_status, rxdesc, skb);
+	skb_pull(skb, (stats.rx_drvinfo_size + stats.rx_bufshift));
+	hdr = (struct ieee80211_hdr *)(skb->data);
+	fc = hdr->frame_control;
+	if (!stats.crc) {
+		memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
+
+		if (is_broadcast_ether_addr(hdr->addr1)) {
+			/*TODO*/;
+		} else if (is_multicast_ether_addr(hdr->addr1)) {
+			/*TODO*/
+		} else {
+			unicast = true;
+			rtlpriv->stats.rxbytesunicast +=  skb->len;
+		}
+
+		rtl_is_special_data(hw, skb, false);
+
+		if (ieee80211_is_data(fc)) {
+			rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
+
+			if (unicast)
+				rtlpriv->link_info.num_rx_inperiod++;
+		}
+		if (likely(rtl_action_proc(hw, skb, false))) {
+			struct sk_buff *uskb = NULL;
+			u8 *pdata;
+
+			uskb = dev_alloc_skb(skb->len + 128);
+			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
+			       sizeof(rx_status));
+			pdata = (u8 *)skb_put(uskb, skb->len);
+			memcpy(pdata, skb->data, skb->len);
+			dev_kfree_skb_any(skb);
+			ieee80211_rx_irqsafe(hw, uskb);
+		} else {
+			dev_kfree_skb_any(skb);
+		}
+	}
+}
+
+static void _rtl_rx_pre_process(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct sk_buff *_skb;
+	struct sk_buff_head rx_queue;
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	skb_queue_head_init(&rx_queue);
+	if (rtlusb->usb_rx_segregate_hdl)
+		rtlusb->usb_rx_segregate_hdl(hw, skb, &rx_queue);
+	WARN_ON(skb_queue_empty(&rx_queue));
+	while (!skb_queue_empty(&rx_queue)) {
+		_skb = skb_dequeue(&rx_queue);
+		_rtl_usb_rx_process_agg(hw, skb);
+		ieee80211_rx_irqsafe(hw, skb);
+	}
+}
+
+static void _rtl_rx_completed(struct urb *_urb)
+{
+	struct sk_buff *skb = (struct sk_buff *)_urb->context;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct rtl_usb *rtlusb = (struct rtl_usb *)info->rate_driver_data[0];
+	struct ieee80211_hw *hw = usb_get_intfdata(rtlusb->intf);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = 0;
+
+	if (unlikely(IS_USB_STOP(rtlusb)))
+		goto free;
+
+	if (likely(0 == _urb->status)) {
+		/* If this code were moved to work queue, would CPU
+		 * utilization be improved?  NOTE: We shall allocate another skb
+		 * and reuse the original one.
+		 */
+		skb_put(skb, _urb->actual_length);
+
+		if (likely(!rtlusb->usb_rx_segregate_hdl)) {
+			struct sk_buff *_skb;
+			_rtl_usb_rx_process_noagg(hw, skb);
+			_skb = _rtl_prep_rx_urb(hw, rtlusb, _urb, GFP_ATOMIC);
+			if (IS_ERR(_skb)) {
+				err = PTR_ERR(_skb);
+				RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+					("Can't allocate skb for bulk IN!\n"));
+				return;
+			}
+			skb = _skb;
+		} else{
+			/* TO DO */
+			_rtl_rx_pre_process(hw, skb);
+			printk(KERN_ERR "rtlwifi: rx agg not supported\n");
+		}
+		goto resubmit;
+	}
+
+	switch (_urb->status) {
+	/* disconnect */
+	case -ENOENT:
+	case -ECONNRESET:
+	case -ENODEV:
+	case -ESHUTDOWN:
+		goto free;
+	default:
+		break;
+	}
+
+resubmit:
+	skb_reset_tail_pointer(skb);
+	skb_trim(skb, 0);
+
+	usb_anchor_urb(_urb, &rtlusb->rx_submitted);
+	err = usb_submit_urb(_urb, GFP_ATOMIC);
+	if (unlikely(err)) {
+		usb_unanchor_urb(_urb);
+		goto free;
+	}
+	return;
+
+free:
+	dev_kfree_skb_irq(skb);
+}
+
+static int _rtl_usb_receive(struct ieee80211_hw *hw)
+{
+	struct urb *urb;
+	struct sk_buff *skb;
+	int err;
+	int i;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	WARN_ON(0 == rtlusb->rx_urb_num);
+	/* 1600 == 1514 + max WLAN header + rtk info */
+	WARN_ON(rtlusb->rx_max_size < 1600);
+
+	for (i = 0; i < rtlusb->rx_urb_num; i++) {
+		err = -ENOMEM;
+		urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!urb) {
+			RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+				 ("Failed to alloc URB!!\n"))
+			goto err_out;
+		}
+
+		skb = _rtl_prep_rx_urb(hw, rtlusb, urb, GFP_KERNEL);
+		if (IS_ERR(skb)) {
+			RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+				 ("Failed to prep_rx_urb!!\n"))
+			err = PTR_ERR(skb);
+			goto err_out;
+		}
+
+		usb_anchor_urb(urb, &rtlusb->rx_submitted);
+		err = usb_submit_urb(urb, GFP_KERNEL);
+		if (err)
+			goto err_out;
+		usb_free_urb(urb);
+	}
+	return 0;
+
+err_out:
+	usb_kill_anchored_urbs(&rtlusb->rx_submitted);
+	return err;
+}
+
+static int rtl_usb_start(struct ieee80211_hw *hw)
+{
+	int err;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	err = rtlpriv->cfg->ops->hw_init(hw);
+	rtl_init_rx_config(hw);
+
+	/* Enable software */
+	SET_USB_START(rtlusb);
+	/* should after adapter start and interrupt enable. */
+	set_hal_start(rtlhal);
+
+	/* Start bulk IN */
+	_rtl_usb_receive(hw);
+
+	return err;
+}
+/**
+ *
+ *
+ */
+
+/*=======================  tx =========================================*/
+static void rtl_usb_cleanup(struct ieee80211_hw *hw)
+{
+	u32 i;
+	struct sk_buff *_skb;
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	struct ieee80211_tx_info *txinfo;
+
+	SET_USB_STOP(rtlusb);
+
+	/* clean up rx stuff. */
+	usb_kill_anchored_urbs(&rtlusb->rx_submitted);
+
+	/* clean up tx stuff */
+	for (i = 0; i < RTL_USB_MAX_EP_NUM; i++) {
+		while ((_skb = skb_dequeue(&rtlusb->tx_skb_queue[i]))) {
+			rtlusb->usb_tx_cleanup(hw, _skb);
+			txinfo = IEEE80211_SKB_CB(_skb);
+			ieee80211_tx_info_clear_status(txinfo);
+			txinfo->flags |= IEEE80211_TX_STAT_ACK;
+			ieee80211_tx_status_irqsafe(hw, _skb);
+		}
+		usb_kill_anchored_urbs(&rtlusb->tx_pending[i]);
+	}
+	usb_kill_anchored_urbs(&rtlusb->tx_submitted);
+}
+
+/**
+ *
+ * We may add some struct into struct rtl_usb later. Do deinit here.
+ *
+ */
+static void rtl_usb_deinit(struct ieee80211_hw *hw)
+{
+	rtl_usb_cleanup(hw);
+}
+
+static void rtl_usb_stop(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	/* should after adapter start and interrupt enable. */
+	set_hal_stop(rtlhal);
+	/* Enable software */
+	SET_USB_STOP(rtlusb);
+	rtl_usb_deinit(hw);
+	rtlpriv->cfg->ops->hw_disable(hw);
+}
+
+static void _rtl_submit_tx_urb(struct ieee80211_hw *hw, struct urb *_urb)
+{
+	int err;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	usb_anchor_urb(_urb, &rtlusb->tx_submitted);
+	err = usb_submit_urb(_urb, GFP_ATOMIC);
+	if (err < 0) {
+		struct sk_buff *skb;
+
+		RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+			 ("Failed to submit urb.\n"));
+		usb_unanchor_urb(_urb);
+		skb = (struct sk_buff *)_urb->context;
+		kfree_skb(skb);
+	}
+	usb_free_urb(_urb);
+}
+
+static int _usb_tx_post(struct ieee80211_hw *hw, struct urb *urb,
+			struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	struct ieee80211_tx_info *txinfo;
+
+	rtlusb->usb_tx_post_hdl(hw, urb, skb);
+	skb_pull(skb, RTL_TX_HEADER_SIZE);
+	txinfo = IEEE80211_SKB_CB(skb);
+	ieee80211_tx_info_clear_status(txinfo);
+	txinfo->flags |= IEEE80211_TX_STAT_ACK;
+
+	if (urb->status) {
+		RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+			 ("Urb has error status 0x%X\n", urb->status));
+		goto out;
+	}
+	/*  TODO:	statistics */
+out:
+	ieee80211_tx_status_irqsafe(hw, skb);
+	return urb->status;
+}
+
+static void _rtl_tx_complete(struct urb *urb)
+{
+	struct sk_buff *skb = (struct sk_buff *)urb->context;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct rtl_usb *rtlusb = (struct rtl_usb *)info->rate_driver_data[0];
+	struct ieee80211_hw *hw = usb_get_intfdata(rtlusb->intf);
+	int err;
+
+	if (unlikely(IS_USB_STOP(rtlusb)))
+		return;
+	err = _usb_tx_post(hw, urb, skb);
+	if (err) {
+		/* Ignore error and keep issuiing other urbs */
+		return;
+	}
+}
+
+static struct urb *_rtl_usb_tx_urb_setup(struct ieee80211_hw *hw,
+				struct sk_buff *skb, u32 ep_num)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	struct urb *_urb;
+
+	WARN_ON(NULL == skb);
+	_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!_urb) {
+		RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+			 ("Can't allocate URB for bulk out!\n"));
+		kfree_skb(skb);
+		return NULL;
+	}
+	_rtl_install_trx_info(rtlusb, skb, ep_num);
+	usb_fill_bulk_urb(_urb, rtlusb->udev, usb_sndbulkpipe(rtlusb->udev,
+			  ep_num), skb->data, skb->len, _rtl_tx_complete, skb);
+	_urb->transfer_flags |= URB_ZERO_PACKET;
+	return _urb;
+}
+
+static void _rtl_usb_transmit(struct ieee80211_hw *hw, struct sk_buff *skb,
+		       enum rtl_txq qnum)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	u32 ep_num;
+	struct urb *_urb = NULL;
+	struct sk_buff *_skb = NULL;
+	struct sk_buff_head *skb_list;
+	struct usb_anchor *urb_list;
+
+	WARN_ON(NULL == rtlusb->usb_tx_aggregate_hdl);
+	if (unlikely(IS_USB_STOP(rtlusb))) {
+		RT_TRACE(rtlpriv, COMP_USB, DBG_EMERG,
+			 ("USB device is stopping...\n"));
+		kfree_skb(skb);
+		return;
+	}
+	ep_num = rtlusb->ep_map.ep_mapping[qnum];
+	skb_list = &rtlusb->tx_skb_queue[ep_num];
+	_skb = skb;
+	_urb = _rtl_usb_tx_urb_setup(hw, _skb, ep_num);
+	if (unlikely(!_urb)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("Can't allocate urb. Drop skb!\n"));
+		return;
+	}
+	urb_list = &rtlusb->tx_pending[ep_num];
+	_rtl_submit_tx_urb(hw, _urb);
+}
+
+static void _rtl_usb_tx_preprocess(struct ieee80211_hw *hw, struct sk_buff *skb,
+			    u16 hw_queue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct rtl_tx_desc *pdesc = NULL;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
+	__le16 fc = hdr->frame_control;
+	u8 *pda_addr = hdr->addr1;
+	/* ssn */
+	u8 *qc = NULL;
+	u8 tid = 0;
+	u16 seq_number = 0;
+
+	if (ieee80211_is_mgmt(fc))
+		rtl_tx_mgmt_proc(hw, skb);
+	rtl_action_proc(hw, skb, true);
+	if (is_multicast_ether_addr(pda_addr))
+		rtlpriv->stats.txbytesmulticast += skb->len;
+	else if (is_broadcast_ether_addr(pda_addr))
+		rtlpriv->stats.txbytesbroadcast += skb->len;
+	else
+		rtlpriv->stats.txbytesunicast += skb->len;
+	if (ieee80211_is_data_qos(fc)) {
+		qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & IEEE80211_QOS_CTL_TID_MASK;
+		seq_number = (le16_to_cpu(hdr->seq_ctrl) &
+			     IEEE80211_SCTL_SEQ) >> 4;
+		seq_number += 1;
+		seq_number <<= 4;
+	}
+	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc, info, skb,
+					hw_queue);
+	if (!ieee80211_has_morefrags(hdr->frame_control)) {
+		if (qc)
+			mac->tids[tid].seq_number = seq_number;
+	}
+	if (ieee80211_is_data(fc))
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
+}
+
+static int rtl_usb_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
+	__le16 fc = hdr->frame_control;
+	u16 hw_queue;
+
+	if (unlikely(is_hal_stop(rtlhal)))
+		goto err_free;
+	hw_queue = rtlusb->usb_mq_to_hwq(fc, skb_get_queue_mapping(skb));
+	_rtl_usb_tx_preprocess(hw, skb, hw_queue);
+	_rtl_usb_transmit(hw, skb, hw_queue);
+	return NETDEV_TX_OK;
+
+err_free:
+	dev_kfree_skb_any(skb);
+	return NETDEV_TX_OK;
+}
+
+static bool rtl_usb_tx_chk_waitq_insert(struct ieee80211_hw *hw,
+					struct sk_buff *skb)
+{
+	return false;
+}
+
+static struct rtl_intf_ops rtl_usb_ops = {
+	.adapter_start = rtl_usb_start,
+	.adapter_stop = rtl_usb_stop,
+	.adapter_tx = rtl_usb_tx,
+	.waitq_insert = rtl_usb_tx_chk_waitq_insert,
+};
+
+int __devinit rtl_usb_probe(struct usb_interface *intf,
+			const struct usb_device_id *id)
+{
+	int err;
+	struct ieee80211_hw *hw = NULL;
+	struct rtl_priv *rtlpriv = NULL;
+	struct usb_device	*udev;
+	struct rtl_usb_priv *usb_priv;
+
+	hw = ieee80211_alloc_hw(sizeof(struct rtl_priv) +
+				sizeof(struct rtl_usb_priv), &rtl_ops);
+	if (!hw) {
+		RT_ASSERT(false, ("%s : ieee80211 alloc failed\n", __func__));
+		return -ENOMEM;
+	}
+	rtlpriv = hw->priv;
+	SET_IEEE80211_DEV(hw, &intf->dev);
+	udev = interface_to_usbdev(intf);
+	usb_get_dev(udev);
+	usb_priv = rtl_usbpriv(hw);
+	memset(usb_priv, 0, sizeof(*usb_priv));
+	usb_priv->dev.intf = intf;
+	usb_priv->dev.udev = udev;
+	usb_set_intfdata(intf, hw);
+	/* init cfg & intf_ops */
+	rtlpriv->rtlhal.interface = INTF_USB;
+	rtlpriv->cfg = (struct rtl_hal_cfg *)(id->driver_info);
+	rtlpriv->intf_ops = &rtl_usb_ops;
+	rtl_dbgp_flag_init(hw);
+	/* Init IO handler */
+	_rtl_usb_io_handler_init(&udev->dev, hw);
+	rtlpriv->cfg->ops->read_chip_version(hw);
+	/*like read eeprom and so on */
+	rtlpriv->cfg->ops->read_eeprom_info(hw);
+	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("Can't init_sw_vars.\n"));
+		goto error_out;
+	}
+	rtlpriv->cfg->ops->init_sw_leds(hw);
+	err = _rtl_usb_init(hw);
+	err = _rtl_usb_init_sw(hw);
+	/* Init mac80211 sw */
+	err = rtl_init_core(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 ("Can't allocate sw for mac80211.\n"));
+		goto error_out;
+	}
+
+	/*init rfkill */
+	/* rtl_init_rfkill(hw); */
+
+	err = ieee80211_register_hw(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+			 ("Can't register mac80211 hw.\n"));
+		goto error_out;
+	} else {
+		rtlpriv->mac80211.mac80211_registered = 1;
+	}
+	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
+	return 0;
+error_out:
+	rtl_deinit_core(hw);
+	_rtl_usb_io_handler_release(hw);
+	ieee80211_free_hw(hw);
+	usb_put_dev(udev);
+	return -ENODEV;
+}
+EXPORT_SYMBOL(rtl_usb_probe);
+
+void rtl_usb_disconnect(struct usb_interface *intf)
+{
+	struct ieee80211_hw *hw = usb_get_intfdata(intf);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+	struct rtl_usb *rtlusb = rtl_usbdev(rtl_usbpriv(hw));
+
+	if (unlikely(!rtlpriv))
+		return;
+	/*ieee80211_unregister_hw will call ops_stop */
+	if (rtlmac->mac80211_registered == 1) {
+		ieee80211_unregister_hw(hw);
+		rtlmac->mac80211_registered = 0;
+	} else {
+		rtl_deinit_deferred_work(hw);
+		rtlpriv->intf_ops->adapter_stop(hw);
+	}
+	/*deinit rfkill */
+	/* rtl_deinit_rfkill(hw); */
+	rtl_usb_deinit(hw);
+	rtl_deinit_core(hw);
+	rtlpriv->cfg->ops->deinit_sw_leds(hw);
+	rtlpriv->cfg->ops->deinit_sw_vars(hw);
+	_rtl_usb_io_handler_release(hw);
+	usb_put_dev(rtlusb->udev);
+	usb_set_intfdata(intf, NULL);
+	ieee80211_free_hw(hw);
+}
+EXPORT_SYMBOL(rtl_usb_disconnect);
+
+int rtl_usb_suspend(struct usb_interface *pusb_intf, pm_message_t message)
+{
+	return 0;
+}
+EXPORT_SYMBOL(rtl_usb_suspend);
+
+int rtl_usb_resume(struct usb_interface *pusb_intf)
+{
+	return 0;
+}
+EXPORT_SYMBOL(rtl_usb_resume);
diff --git a/drivers/net/wireless/rtlwifi/usb.h b/drivers/net/wireless/rtlwifi/usb.h
new file mode 100644
index 000000000000..abadfe918d30
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/usb.h
@@ -0,0 +1,164 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2011  Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_USB_H__
+#define __RTL_USB_H__
+
+#include <linux/usb.h>
+#include <linux/skbuff.h>
+
+#define RTL_USB_DEVICE(vend, prod, cfg) \
+	.match_flags = USB_DEVICE_ID_MATCH_DEVICE, \
+	.idVendor = (vend), \
+	.idProduct = (prod), \
+	.driver_info = (kernel_ulong_t)&(cfg)
+
+#define USB_HIGH_SPEED_BULK_SIZE	512
+#define USB_FULL_SPEED_BULK_SIZE	64
+
+
+#define RTL_USB_MAX_TXQ_NUM		4		/* max tx queue */
+#define RTL_USB_MAX_EP_NUM		6		/* max ep number */
+#define RTL_USB_MAX_TX_URBS_NUM		8
+
+enum rtl_txq {
+	/* These definitions shall be consistent with value
+	 * returned by skb_get_queue_mapping
+	 *------------------------------------*/
+	RTL_TXQ_BK,
+	RTL_TXQ_BE,
+	RTL_TXQ_VI,
+	RTL_TXQ_VO,
+	/*------------------------------------*/
+	RTL_TXQ_BCN,
+	RTL_TXQ_MGT,
+	RTL_TXQ_HI,
+
+	/* Must be last */
+	__RTL_TXQ_NUM,
+};
+
+struct rtl_ep_map {
+	u32 ep_mapping[__RTL_TXQ_NUM];
+};
+
+struct _trx_info {
+	struct rtl_usb *rtlusb;
+	u32 ep_num;
+};
+
+static inline void _rtl_install_trx_info(struct rtl_usb *rtlusb,
+					 struct sk_buff *skb,
+					 u32 ep_num)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	info->rate_driver_data[0] = rtlusb;
+	info->rate_driver_data[1] = (void *)(__kernel_size_t)ep_num;
+}
+
+
+/*  Add suspend/resume later */
+enum rtl_usb_state {
+	USB_STATE_STOP	= 0,
+	USB_STATE_START	= 1,
+};
+
+#define IS_USB_STOP(rtlusb_ptr) (USB_STATE_STOP == (rtlusb_ptr)->state)
+#define IS_USB_START(rtlusb_ptr) (USB_STATE_START == (rtlusb_ptr)->state)
+#define SET_USB_STOP(rtlusb_ptr) \
+	do {							\
+		(rtlusb_ptr)->state = USB_STATE_STOP;		\
+	} while (0)
+
+#define SET_USB_START(rtlusb_ptr)				\
+	do { \
+		(rtlusb_ptr)->state = USB_STATE_START;		\
+	} while (0)
+
+struct rtl_usb {
+	struct usb_device *udev;
+	struct usb_interface *intf;
+	enum rtl_usb_state state;
+
+	/* Bcn control register setting */
+	u32 reg_bcn_ctrl_val;
+	/* for 88/92cu card disable */
+	u8	disableHWSM;
+	/*QOS & EDCA */
+	enum acm_method acm_method;
+	/* irq  . HIMR,HIMR_EX */
+	u32 irq_mask[2];
+	bool irq_enabled;
+
+	u16 (*usb_mq_to_hwq)(__le16 fc, u16 mac80211_queue_index);
+
+	/* Tx */
+	u8 out_ep_nums ;
+	u8 out_queue_sel;
+	struct rtl_ep_map ep_map;
+
+	u32 max_bulk_out_size;
+	u32 tx_submitted_urbs;
+	struct sk_buff_head tx_skb_queue[RTL_USB_MAX_EP_NUM];
+
+	struct usb_anchor tx_pending[RTL_USB_MAX_EP_NUM];
+	struct usb_anchor tx_submitted;
+
+	struct sk_buff *(*usb_tx_aggregate_hdl)(struct ieee80211_hw *,
+						struct sk_buff_head *);
+	int (*usb_tx_post_hdl)(struct ieee80211_hw *,
+			       struct urb *, struct sk_buff *);
+	void (*usb_tx_cleanup)(struct ieee80211_hw *, struct sk_buff *);
+
+	/* Rx */
+	u8 in_ep_nums ;
+	u32 in_ep;		/* Bulk IN endpoint number */
+	u32 rx_max_size;	/* Bulk IN max buffer size */
+	u32 rx_urb_num;		/* How many Bulk INs are submitted to host. */
+	struct usb_anchor	rx_submitted;
+	void (*usb_rx_segregate_hdl)(struct ieee80211_hw *, struct sk_buff *,
+				     struct sk_buff_head *);
+	void (*usb_rx_hdl)(struct ieee80211_hw *, struct sk_buff *);
+};
+
+struct rtl_usb_priv {
+	struct rtl_usb dev;
+	struct rtl_led_ctl ledctl;
+};
+
+#define rtl_usbpriv(hw)	 (((struct rtl_usb_priv *)(rtl_priv(hw))->priv))
+#define rtl_usbdev(usbpriv)	(&((usbpriv)->dev))
+
+
+
+int __devinit rtl_usb_probe(struct usb_interface *intf,
+			    const struct usb_device_id *id);
+void rtl_usb_disconnect(struct usb_interface *intf);
+int rtl_usb_suspend(struct usb_interface *pusb_intf, pm_message_t message);
+int rtl_usb_resume(struct usb_interface *pusb_intf);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index d44d79613d2d..01226f8e70f9 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -34,6 +34,8 @@
 #include <linux/firmware.h>
 #include <linux/version.h>
 #include <linux/etherdevice.h>
+#include <linux/vmalloc.h>
+#include <linux/usb.h>
 #include <net/mac80211.h>
 #include "debug.h"
 
@@ -82,6 +84,19 @@
 #define MAC80211_3ADDR_LEN			24
 #define MAC80211_4ADDR_LEN			30
 
+#define CHANNEL_MAX_NUMBER	(14 + 24 + 21)	/* 14 is the max channel no */
+#define CHANNEL_GROUP_MAX	(3 + 9)	/*  ch1~3, 4~9, 10~14 = three groups */
+#define MAX_PG_GROUP			13
+#define	CHANNEL_GROUP_MAX_2G		3
+#define	CHANNEL_GROUP_IDX_5GL		3
+#define	CHANNEL_GROUP_IDX_5GM		6
+#define	CHANNEL_GROUP_IDX_5GH		9
+#define	CHANNEL_GROUP_MAX_5G		9
+#define CHANNEL_MAX_NUMBER_2G		14
+#define AVG_THERMAL_NUM			8
+
+/* for early mode */
+#define EM_HDR_LEN			8
 enum intf_type {
 	INTF_PCI = 0,
 	INTF_USB = 1,
@@ -113,11 +128,38 @@ enum hardware_type {
 	HARDWARE_TYPE_RTL8192CU,
 	HARDWARE_TYPE_RTL8192DE,
 	HARDWARE_TYPE_RTL8192DU,
+	HARDWARE_TYPE_RTL8723E,
+	HARDWARE_TYPE_RTL8723U,
 
-	/*keep it last*/
+	/* keep it last */
 	HARDWARE_TYPE_NUM
 };
 
+#define IS_HARDWARE_TYPE_8192SU(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8192SU)
+#define IS_HARDWARE_TYPE_8192SE(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
+#define IS_HARDWARE_TYPE_8192CE(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8192CE)
+#define IS_HARDWARE_TYPE_8192CU(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8192CU)
+#define IS_HARDWARE_TYPE_8192DE(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8192DE)
+#define IS_HARDWARE_TYPE_8192DU(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8192DU)
+#define IS_HARDWARE_TYPE_8723E(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8723E)
+#define IS_HARDWARE_TYPE_8723U(rtlhal)			\
+	(rtlhal->hw_type == HARDWARE_TYPE_RTL8723U)
+#define	IS_HARDWARE_TYPE_8192S(rtlhal)			\
+(IS_HARDWARE_TYPE_8192SE(rtlhal) || IS_HARDWARE_TYPE_8192SU(rtlhal))
+#define	IS_HARDWARE_TYPE_8192C(rtlhal)			\
+(IS_HARDWARE_TYPE_8192CE(rtlhal) || IS_HARDWARE_TYPE_8192CU(rtlhal))
+#define	IS_HARDWARE_TYPE_8192D(rtlhal)			\
+(IS_HARDWARE_TYPE_8192DE(rtlhal) || IS_HARDWARE_TYPE_8192DU(rtlhal))
+#define	IS_HARDWARE_TYPE_8723(rtlhal)			\
+(IS_HARDWARE_TYPE_8723E(rtlhal) || IS_HARDWARE_TYPE_8723U(rtlhal))
+
 enum scan_operation_backup_opt {
 	SCAN_OPT_BACKUP = 0,
 	SCAN_OPT_RESTORE,
@@ -315,6 +357,7 @@ enum rf_type {
 	RF_1T1R = 0,
 	RF_1T2R = 1,
 	RF_2T2R = 2,
+	RF_2T2R_GREEN = 3,
 };
 
 enum ht_channel_width {
@@ -359,6 +402,8 @@ enum rtl_var_map {
 	EFUSE_LOADER_CLK_EN,
 	EFUSE_ANA8M,
 	EFUSE_HWSET_MAX_SIZE,
+	EFUSE_MAX_SECTION_MAP,
+	EFUSE_REAL_CONTENT_SIZE,
 
 	/*CAM map */
 	RWCAM,
@@ -397,6 +442,7 @@ enum rtl_var_map {
 	RTL_IMR_ATIMEND,	/*For 92C,ATIM Window End Interrupt */
 	RTL_IMR_BDOK,		/*Beacon Queue DMA OK Interrup */
 	RTL_IMR_HIGHDOK,	/*High Queue DMA OK Interrupt */
+	RTL_IMR_COMDOK,		/*Command Queue DMA OK Interrupt*/
 	RTL_IMR_TBDOK,		/*Transmit Beacon OK interrup */
 	RTL_IMR_MGNTDOK,	/*Management Queue DMA OK Interrupt */
 	RTL_IMR_TBDER,		/*For 92C,Transmit Beacon Error Interrupt */
@@ -405,7 +451,8 @@ enum rtl_var_map {
 	RTL_IMR_VIDOK,		/*AC_VI DMA OK Interrupt */
 	RTL_IMR_VODOK,		/*AC_VO DMA Interrupt */
 	RTL_IMR_ROK,		/*Receive DMA OK Interrupt */
-	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BcnInt|RTL_IMR_TBDOK|RTL_IMR_TBDER)*/
+	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BcnInt | RTL_IMR_TBDOK |
+				 * RTL_IMR_TBDER) */
 
 	/*CCK Rates, TxHT = 0 */
 	RTL_RC_CCK_RATE1M,
@@ -481,6 +528,19 @@ enum acm_method {
 	eAcmWay2_SW = 2,
 };
 
+enum macphy_mode {
+	SINGLEMAC_SINGLEPHY = 0,
+	DUALMAC_DUALPHY,
+	DUALMAC_SINGLEPHY,
+};
+
+enum band_type {
+	BAND_ON_2_4G = 0,
+	BAND_ON_5G,
+	BAND_ON_BOTH,
+	BANDMAX
+};
+
 /*aci/aifsn Field.
 Ref: WMM spec 2.2.2: WME Parameter Element, p.12.*/
 union aci_aifsn {
@@ -505,6 +565,17 @@ enum wireless_mode {
 	WIRELESS_MODE_N_5G = 0x20
 };
 
+#define IS_WIRELESS_MODE_A(wirelessmode)	\
+	(wirelessmode == WIRELESS_MODE_A)
+#define IS_WIRELESS_MODE_B(wirelessmode)	\
+	(wirelessmode == WIRELESS_MODE_B)
+#define IS_WIRELESS_MODE_G(wirelessmode)	\
+	(wirelessmode == WIRELESS_MODE_G)
+#define IS_WIRELESS_MODE_N_24G(wirelessmode)	\
+	(wirelessmode == WIRELESS_MODE_N_24G)
+#define IS_WIRELESS_MODE_N_5G(wirelessmode)	\
+	(wirelessmode == WIRELESS_MODE_N_5G)
+
 enum ratr_table_mode {
 	RATR_INX_WIRELESS_NGB = 0,
 	RATR_INX_WIRELESS_NG = 1,
@@ -574,11 +645,11 @@ struct rtl_probe_rsp {
 struct rtl_led {
 	void *hw;
 	enum rtl_led_pin ledpin;
-	bool b_ledon;
+	bool ledon;
 };
 
 struct rtl_led_ctl {
-	bool bled_opendrain;
+	bool led_opendrain;
 	struct rtl_led sw_led0;
 	struct rtl_led sw_led1;
 };
@@ -603,6 +674,8 @@ struct false_alarm_statistics {
 	u32 cnt_rate_illegal;
 	u32 cnt_crc8_fail;
 	u32 cnt_mcs_fail;
+	u32 cnt_fast_fsync_fail;
+	u32 cnt_sb_search_fail;
 	u32 cnt_ofdm_fail;
 	u32 cnt_cck_fail;
 	u32 cnt_all;
@@ -690,6 +763,32 @@ struct rtl_rfkill {
 	bool rfkill_state;	/*0 is off, 1 is on */
 };
 
+#define IQK_MATRIX_REG_NUM	8
+#define IQK_MATRIX_SETTINGS_NUM	(1 + 24 + 21)
+struct iqk_matrix_regs {
+	bool b_iqk_done;
+	long value[1][IQK_MATRIX_REG_NUM];
+};
+
+struct phy_parameters {
+	u16 length;
+	u32 *pdata;
+};
+
+enum hw_param_tab_index {
+	PHY_REG_2T,
+	PHY_REG_1T,
+	PHY_REG_PG,
+	RADIOA_2T,
+	RADIOB_2T,
+	RADIOA_1T,
+	RADIOB_1T,
+	MAC_REG,
+	AGCTAB_2T,
+	AGCTAB_1T,
+	MAX_TAB
+};
+
 struct rtl_phy {
 	struct bb_reg_def phyreg_def[4];	/*Radio A/B/C/D */
 	struct init_gain initgain_backup;
@@ -705,8 +804,9 @@ struct rtl_phy {
 	u8 current_channel;
 	u8 h2c_box_num;
 	u8 set_io_inprogress;
+	u8 lck_inprogress;
 
-	/*record for power tracking*/
+	/* record for power tracking */
 	s32 reg_e94;
 	s32 reg_e9c;
 	s32 reg_ea4;
@@ -723,26 +823,32 @@ struct rtl_phy {
 	u32 iqk_mac_backup[IQK_MAC_REG_NUM];
 	u32 iqk_bb_backup[10];
 
-	bool b_rfpi_enable;
+	/* Dual mac */
+	bool need_iqk;
+	struct iqk_matrix_regs iqk_matrix_regsetting[IQK_MATRIX_SETTINGS_NUM];
+
+	bool rfpi_enable;
 
 	u8 pwrgroup_cnt;
-	u8 bcck_high_power;
-	/* 3 groups of pwr diff by rates*/
-	u32 mcs_txpwrlevel_origoffset[4][16];
+	u8 cck_high_power;
+	/* MAX_PG_GROUP groups of pwr diff by rates */
+	u32 mcs_txpwrlevel_origoffset[MAX_PG_GROUP][16];
 	u8 default_initialgain[4];
 
-	/*the current Tx power level*/
+	/* the current Tx power level */
 	u8 cur_cck_txpwridx;
 	u8 cur_ofdm24g_txpwridx;
 
 	u32 rfreg_chnlval[2];
-	bool b_apk_done;
+	bool apk_done;
+	u32 reg_rf3c[2];	/* pathA / pathB  */
 
-	/*fsync*/
 	u8 framesync;
 	u32 framesync_c34;
 
 	u8 num_total_rfpath;
+	struct phy_parameters hwparam_tables[MAX_TAB];
+	u16 rf_pathmap;
 };
 
 #define MAX_TID_COUNT				9
@@ -768,6 +874,7 @@ struct rtl_tid_data {
 struct rtl_priv;
 struct rtl_io {
 	struct device *dev;
+	struct mutex bb_mutex;
 
 	/*PCI MEM map */
 	unsigned long pci_mem_end;	/*shared mem end        */
@@ -779,11 +886,14 @@ struct rtl_io {
 	void (*write8_async) (struct rtl_priv *rtlpriv, u32 addr, u8 val);
 	void (*write16_async) (struct rtl_priv *rtlpriv, u32 addr, u16 val);
 	void (*write32_async) (struct rtl_priv *rtlpriv, u32 addr, u32 val);
-
-	 u8(*read8_sync) (struct rtl_priv *rtlpriv, u32 addr);
-	 u16(*read16_sync) (struct rtl_priv *rtlpriv, u32 addr);
-	 u32(*read32_sync) (struct rtl_priv *rtlpriv, u32 addr);
-
+	int (*writeN_async) (struct rtl_priv *rtlpriv, u32 addr, u16 len,
+			     u8 *pdata);
+
+	u8(*read8_sync) (struct rtl_priv *rtlpriv, u32 addr);
+	u16(*read16_sync) (struct rtl_priv *rtlpriv, u32 addr);
+	u32(*read32_sync) (struct rtl_priv *rtlpriv, u32 addr);
+	int (*readN_sync) (struct rtl_priv *rtlpriv, u32 addr, u16 len,
+			    u8 *pdata);
 };
 
 struct rtl_mac {
@@ -815,16 +925,24 @@ struct rtl_mac {
 	bool act_scanning;
 	u8 cnt_after_linked;
 
-	 /*RDG*/ bool rdg_en;
+	/* early mode */
+	/* skb wait queue */
+	struct sk_buff_head skb_waitq[MAX_TID_COUNT];
+	u8 earlymode_threshold;
+
+	/*RDG*/
+	bool rdg_en;
 
-	 /*AP*/ u8 bssid[6];
-	u8 mcs[16];	/*16 bytes mcs for HT rates.*/
-	u32 basic_rates; /*b/g rates*/
+	/*AP*/
+	u8 bssid[6];
+	u32 vendor;
+	u8 mcs[16];	/* 16 bytes mcs for HT rates. */
+	u32 basic_rates; /* b/g rates */
 	u8 ht_enable;
 	u8 sgi_40;
 	u8 sgi_20;
 	u8 bw_40;
-	u8 mode;		/*wireless mode*/
+	u8 mode;		/* wireless mode */
 	u8 slot_time;
 	u8 short_preamble;
 	u8 use_cts_protect;
@@ -835,9 +953,11 @@ struct rtl_mac {
 	u8 retry_long;
 	u16 assoc_id;
 
-	 /*IBSS*/ int beacon_interval;
+	/*IBSS*/
+	int beacon_interval;
 
-	 /*AMPDU*/ u8 min_space_cfg;	/*For Min spacing configurations */
+	/*AMPDU*/
+	u8 min_space_cfg;	/*For Min spacing configurations */
 	u8 max_mss_density;
 	u8 current_ampdu_factor;
 	u8 current_ampdu_density;
@@ -852,17 +972,54 @@ struct rtl_hal {
 
 	enum intf_type interface;
 	u16 hw_type;		/*92c or 92d or 92s and so on */
+	u8 ic_class;
 	u8 oem_id;
-	u8 version;		/*version of chip */
+	u32 version;		/*version of chip */
 	u8 state;		/*stop 0, start 1 */
 
 	/*firmware */
+	u32 fwsize;
 	u8 *pfirmware;
-	bool b_h2c_setinprogress;
+	u16 fw_version;
+	u16 fw_subversion;
+	bool h2c_setinprogress;
 	u8 last_hmeboxnum;
-	bool bfw_ready;
+	bool fw_ready;
 	/*Reserve page start offset except beacon in TxQ. */
 	u8 fw_rsvdpage_startoffset;
+	u8 h2c_txcmd_seq;
+
+	/* FW Cmd IO related */
+	u16 fwcmd_iomap;
+	u32 fwcmd_ioparam;
+	bool set_fwcmd_inprogress;
+	u8 current_fwcmd_io;
+
+	/**/
+	bool driver_going2unload;
+
+	/*AMPDU init min space*/
+	u8 minspace_cfg;	/*For Min spacing configurations */
+
+	/* Dual mac */
+	enum macphy_mode macphymode;
+	enum band_type current_bandtype;	/* 0:2.4G, 1:5G */
+	enum band_type current_bandtypebackup;
+	enum band_type bandset;
+	/* dual MAC 0--Mac0 1--Mac1 */
+	u32 interfaceindex;
+	/* just for DualMac S3S4 */
+	u8 macphyctl_reg;
+	bool earlymode_enable;
+	/* Dual mac*/
+	bool during_mac0init_radiob;
+	bool during_mac1init_radioa;
+	bool reloadtxpowerindex;
+	/* True if IMR or IQK  have done
+	for 2.4G in scan progress */
+	bool load_imrandiqk_setting_for2g;
+
+	bool disable_amsdu_8k;
 };
 
 struct rtl_security {
@@ -887,48 +1044,61 @@ struct rtl_security {
 };
 
 struct rtl_dm {
-	/*PHY status for DM */
+	/*PHY status for Dynamic Management */
 	long entry_min_undecoratedsmoothed_pwdb;
 	long undecorated_smoothed_pwdb;	/*out dm */
 	long entry_max_undecoratedsmoothed_pwdb;
-	bool b_dm_initialgain_enable;
-	bool bdynamic_txpower_enable;
-	bool bcurrent_turbo_edca;
-	bool bis_any_nonbepkts;	/*out dm */
-	bool bis_cur_rdlstate;
-	bool btxpower_trackingInit;
-	bool b_disable_framebursting;
-	bool b_cck_inch14;
-	bool btxpower_tracking;
-	bool b_useramask;
-	bool brfpath_rxenable[4];
-
+	bool dm_initialgain_enable;
+	bool dynamic_txpower_enable;
+	bool current_turbo_edca;
+	bool is_any_nonbepkts;	/*out dm */
+	bool is_cur_rdlstate;
+	bool txpower_trackingInit;
+	bool disable_framebursting;
+	bool cck_inch14;
+	bool txpower_tracking;
+	bool useramask;
+	bool rfpath_rxenable[4];
+	bool inform_fw_driverctrldm;
+	bool current_mrc_switch;
+	u8 txpowercount;
+
+	u8 thermalvalue_rxgain;
 	u8 thermalvalue_iqk;
 	u8 thermalvalue_lck;
 	u8 thermalvalue;
 	u8 last_dtp_lvl;
+	u8 thermalvalue_avg[AVG_THERMAL_NUM];
+	u8 thermalvalue_avg_index;
+	bool done_txpower;
 	u8 dynamic_txhighpower_lvl;	/*Tx high power level */
-	u8 dm_flag;	/*Indicate if each dynamic mechanism's status. */
+	u8 dm_flag;		/*Indicate each dynamic mechanism's status. */
 	u8 dm_type;
 	u8 txpower_track_control;
-
+	bool interrupt_migration;
+	bool disable_tx_int;
 	char ofdm_index[2];
 	char cck_index;
+	u8 power_index_backup[6];
 };
 
-#define	EFUSE_MAX_LOGICAL_SIZE			 128
+#define	EFUSE_MAX_LOGICAL_SIZE			256
 
 struct rtl_efuse {
-	bool bautoLoad_ok;
+	bool autoLoad_ok;
 	bool bootfromefuse;
 	u16 max_physical_size;
-	u8 contents[EFUSE_MAX_LOGICAL_SIZE];
 
 	u8 efuse_map[2][EFUSE_MAX_LOGICAL_SIZE];
 	u16 efuse_usedbytes;
 	u8 efuse_usedpercentage;
+#ifdef EFUSE_REPG_WORKAROUND
+	bool efuse_re_pg_sec1flag;
+	u8 efuse_re_pg_data[8];
+#endif
 
 	u8 autoload_failflag;
+	u8 autoload_status;
 
 	short epromtype;
 	u16 eeprom_vid;
@@ -938,69 +1108,90 @@ struct rtl_efuse {
 	u8 eeprom_oemid;
 	u16 eeprom_channelplan;
 	u8 eeprom_version;
+	u8 board_type;
+	u8 external_pa;
 
 	u8 dev_addr[6];
 
-	bool b_txpwr_fromeprom;
+	bool txpwr_fromeprom;
+	u8 eeprom_crystalcap;
 	u8 eeprom_tssi[2];
-	u8 eeprom_pwrlimit_ht20[3];
-	u8 eeprom_pwrlimit_ht40[3];
-	u8 eeprom_chnlarea_txpwr_cck[2][3];
-	u8 eeprom_chnlarea_txpwr_ht40_1s[2][3];
-	u8 eeprom_chnlarea_txpwr_ht40_2sdiif[2][3];
-	u8 txpwrlevel_cck[2][14];
-	u8 txpwrlevel_ht40_1s[2][14];	/*For HT 40MHZ pwr */
-	u8 txpwrlevel_ht40_2s[2][14];	/*For HT 40MHZ pwr */
+	u8 eeprom_tssi_5g[3][2]; /* for 5GL/5GM/5GH band. */
+	u8 eeprom_pwrlimit_ht20[CHANNEL_GROUP_MAX];
+	u8 eeprom_pwrlimit_ht40[CHANNEL_GROUP_MAX];
+	u8 eeprom_chnlarea_txpwr_cck[2][CHANNEL_GROUP_MAX_2G];
+	u8 eeprom_chnlarea_txpwr_ht40_1s[2][CHANNEL_GROUP_MAX];
+	u8 eeprom_chnlarea_txpwr_ht40_2sdiif[2][CHANNEL_GROUP_MAX];
+	u8 txpwrlevel_cck[2][CHANNEL_MAX_NUMBER_2G];
+	u8 txpwrlevel_ht40_1s[2][CHANNEL_MAX_NUMBER];	/*For HT 40MHZ pwr */
+	u8 txpwrlevel_ht40_2s[2][CHANNEL_MAX_NUMBER];	/*For HT 40MHZ pwr */
+
+	u8 internal_pa_5g[2];	/* pathA / pathB */
+	u8 eeprom_c9;
+	u8 eeprom_cc;
 
 	/*For power group */
-	u8 pwrgroup_ht20[2][14];
-	u8 pwrgroup_ht40[2][14];
-
-	char txpwr_ht20diff[2][14];	/*HT 20<->40 Pwr diff */
-	u8 txpwr_legacyhtdiff[2][14];	/*For HT<->legacy pwr diff */
+	u8 eeprom_pwrgroup[2][3];
+	u8 pwrgroup_ht20[2][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_ht40[2][CHANNEL_MAX_NUMBER];
+
+	char txpwr_ht20diff[2][CHANNEL_MAX_NUMBER]; /*HT 20<->40 Pwr diff */
+	/*For HT<->legacy pwr diff*/
+	u8 txpwr_legacyhtdiff[2][CHANNEL_MAX_NUMBER];
+	u8 txpwr_safetyflag;			/* Band edge enable flag */
+	u16 eeprom_txpowerdiff;
+	u8 legacy_httxpowerdiff;	/* Legacy to HT rate power diff */
+	u8 antenna_txpwdiff[3];
 
 	u8 eeprom_regulatory;
 	u8 eeprom_thermalmeter;
-	/*ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
-	u8 thermalmeter[2];
+	u8 thermalmeter[2]; /*ThermalMeter, index 0 for RFIC0, 1 for RFIC1 */
+	u16 tssi_13dbm;
+	u8 crystalcap;		/* CrystalCap. */
+	u8 delta_iqk;
+	u8 delta_lck;
 
 	u8 legacy_ht_txpowerdiff;	/*Legacy to HT rate power diff */
-	bool b_apk_thermalmeterignore;
+	bool apk_thermalmeterignore;
+
+	bool b1x1_recvcombine;
+	bool b1ss_support;
+
+	/*channel plan */
+	u8 channel_plan;
 };
 
 struct rtl_ps_ctl {
+	bool pwrdomain_protect;
 	bool set_rfpowerstate_inprogress;
-	bool b_in_powersavemode;
+	bool in_powersavemode;
 	bool rfchange_inprogress;
-	bool b_swrf_processing;
-	bool b_hwradiooff;
-
-	u32 last_sleep_jiffies;
-	u32 last_awake_jiffies;
-	u32 last_delaylps_stamp_jiffies;
+	bool swrf_processing;
+	bool hwradiooff;
 
 	/*
 	 * just for PCIE ASPM
 	 * If it supports ASPM, Offset[560h] = 0x40,
 	 * otherwise Offset[560h] = 0x00.
 	 * */
-	bool b_support_aspm;
-	bool b_support_backdoor;
+	bool support_aspm;
+	bool support_backdoor;
 
 	/*for LPS */
 	enum rt_psmode dot11_psmode;	/*Power save mode configured. */
-	bool b_leisure_ps;
-	bool b_fwctrl_lps;
+	bool swctrl_lps;
+	bool leisure_ps;
+	bool fwctrl_lps;
 	u8 fwctrl_psmode;
 	/*For Fw control LPS mode */
-	u8 b_reg_fwctrl_lps;
+	u8 reg_fwctrl_lps;
 	/*Record Fw PS mode status. */
-	bool b_fw_current_inpsmode;
+	bool fw_current_inpsmode;
 	u8 reg_max_lps_awakeintvl;
 	bool report_linked;
 
 	/*for IPS */
-	bool b_inactiveps;
+	bool inactiveps;
 
 	u32 rfoff_reason;
 
@@ -1011,8 +1202,26 @@ struct rtl_ps_ctl {
 	/*just for PCIE ASPM */
 	u8 const_amdpci_aspm;
 
+	bool pwrdown_mode;
+
 	enum rf_pwrstate inactive_pwrstate;
 	enum rf_pwrstate rfpwr_state;	/*cur power state */
+
+	/* for SW LPS*/
+	bool sw_ps_enabled;
+	bool state;
+	bool state_inap;
+	bool multi_buffered;
+	u16 nullfunc_seq;
+	unsigned int dtim_counter;
+	unsigned int sleep_ms;
+	unsigned long last_sleep_jiffies;
+	unsigned long last_awake_jiffies;
+	unsigned long last_delaylps_stamp_jiffies;
+	unsigned long last_dtim;
+	unsigned long last_beacon;
+	unsigned long last_action;
+	unsigned long last_slept;
 };
 
 struct rtl_stats {
@@ -1038,10 +1247,10 @@ struct rtl_stats {
 	s32 recvsignalpower;
 	s8 rxpower;		/*in dBm Translate from PWdB */
 	u8 signalstrength;	/*in 0-100 index. */
-	u16 b_hwerror:1;
-	u16 b_crc:1;
-	u16 b_icv:1;
-	u16 b_shortpreamble:1;
+	u16 hwerror:1;
+	u16 crc:1;
+	u16 icv:1;
+	u16 shortpreamble:1;
 	u16 antenna:1;
 	u16 decrypted:1;
 	u16 wakeup:1;
@@ -1050,15 +1259,16 @@ struct rtl_stats {
 
 	u8 rx_drvinfo_size;
 	u8 rx_bufshift;
-	bool b_isampdu;
+	bool isampdu;
+	bool isfirst_ampdu;
 	bool rx_is40Mhzpacket;
 	u32 rx_pwdb_all;
 	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
 	s8 rx_mimo_signalquality[2];
-	bool b_packet_matchbssid;
-	bool b_is_cck;
-	bool b_packet_toself;
-	bool b_packet_beacon;	/*for rssi */
+	bool packet_matchbssid;
+	bool is_cck;
+	bool packet_toself;
+	bool packet_beacon;	/*for rssi */
 	char cck_adc_pwdb[4];	/*for rx path selection */
 };
 
@@ -1069,23 +1279,23 @@ struct rt_link_detect {
 	u32 num_tx_inperiod;
 	u32 num_rx_inperiod;
 
-	bool b_busytraffic;
-	bool b_higher_busytraffic;
-	bool b_higher_busyrxtraffic;
+	bool busytraffic;
+	bool higher_busytraffic;
+	bool higher_busyrxtraffic;
 };
 
 struct rtl_tcb_desc {
-	u8 b_packet_bw:1;
-	u8 b_multicast:1;
-	u8 b_broadcast:1;
-
-	u8 b_rts_stbc:1;
-	u8 b_rts_enable:1;
-	u8 b_cts_enable:1;
-	u8 b_rts_use_shortpreamble:1;
-	u8 b_rts_use_shortgi:1;
+	u8 packet_bw:1;
+	u8 multicast:1;
+	u8 broadcast:1;
+
+	u8 rts_stbc:1;
+	u8 rts_enable:1;
+	u8 cts_enable:1;
+	u8 rts_use_shortpreamble:1;
+	u8 rts_use_shortgi:1;
 	u8 rts_sc:1;
-	u8 b_rts_bw:1;
+	u8 rts_bw:1;
 	u8 rts_rate;
 
 	u8 use_shortgi:1;
@@ -1096,20 +1306,34 @@ struct rtl_tcb_desc {
 	u8 ratr_index;
 	u8 mac_id;
 	u8 hw_rate;
+
+	u8 last_inipkt:1;
+	u8 cmd_or_init:1;
+	u8 queue_index;
+
+	/* early mode */
+	u8 empkt_num;
+	/* The max value by HW */
+	u32 empkt_len[5];
 };
 
 struct rtl_hal_ops {
 	int (*init_sw_vars) (struct ieee80211_hw *hw);
 	void (*deinit_sw_vars) (struct ieee80211_hw *hw);
+	void (*read_chip_version)(struct ieee80211_hw *hw);
 	void (*read_eeprom_info) (struct ieee80211_hw *hw);
 	void (*interrupt_recognized) (struct ieee80211_hw *hw,
 				      u32 *p_inta, u32 *p_intb);
 	int (*hw_init) (struct ieee80211_hw *hw);
 	void (*hw_disable) (struct ieee80211_hw *hw);
+	void (*hw_suspend) (struct ieee80211_hw *hw);
+	void (*hw_resume) (struct ieee80211_hw *hw);
 	void (*enable_interrupt) (struct ieee80211_hw *hw);
 	void (*disable_interrupt) (struct ieee80211_hw *hw);
 	int (*set_network_type) (struct ieee80211_hw *hw,
 				 enum nl80211_iftype type);
+	void (*set_chk_bssid)(struct ieee80211_hw *hw,
+				bool check_bssid);
 	void (*set_bw_mode) (struct ieee80211_hw *hw,
 			     enum nl80211_channel_type ch_type);
 	 u8(*switch_channel) (struct ieee80211_hw *hw);
@@ -1126,23 +1350,26 @@ struct rtl_hal_ops {
 			      struct ieee80211_hdr *hdr, u8 *pdesc_tx,
 			      struct ieee80211_tx_info *info,
 			      struct sk_buff *skb, unsigned int queue_index);
+	void (*fill_fake_txdesc) (struct ieee80211_hw *hw, u8 * pDesc,
+				  u32 buffer_len, bool bIsPsPoll);
 	void (*fill_tx_cmddesc) (struct ieee80211_hw *hw, u8 *pdesc,
-				 bool b_firstseg, bool b_lastseg,
+				 bool firstseg, bool lastseg,
 				 struct sk_buff *skb);
-	 bool(*query_rx_desc) (struct ieee80211_hw *hw,
+	bool (*cmd_send_packet)(struct ieee80211_hw *hw, struct sk_buff *skb);
+	bool (*query_rx_desc) (struct ieee80211_hw *hw,
 			       struct rtl_stats *stats,
 			       struct ieee80211_rx_status *rx_status,
 			       u8 *pdesc, struct sk_buff *skb);
 	void (*set_channel_access) (struct ieee80211_hw *hw);
-	 bool(*radio_onoff_checking) (struct ieee80211_hw *hw, u8 *valid);
+	bool (*radio_onoff_checking) (struct ieee80211_hw *hw, u8 *valid);
 	void (*dm_watchdog) (struct ieee80211_hw *hw);
 	void (*scan_operation_backup) (struct ieee80211_hw *hw, u8 operation);
-	 bool(*set_rf_power_state) (struct ieee80211_hw *hw,
+	bool (*set_rf_power_state) (struct ieee80211_hw *hw,
 				    enum rf_pwrstate rfpwr_state);
 	void (*led_control) (struct ieee80211_hw *hw,
 			     enum led_ctl_mode ledaction);
 	void (*set_desc) (u8 *pdesc, bool istx, u8 desc_name, u8 *val);
-	 u32(*get_desc) (u8 *pdesc, bool istx, u8 desc_name);
+	u32 (*get_desc) (u8 *pdesc, bool istx, u8 desc_name);
 	void (*tx_polling) (struct ieee80211_hw *hw, unsigned int hw_queue);
 	void (*enable_hw_sec) (struct ieee80211_hw *hw);
 	void (*set_key) (struct ieee80211_hw *hw, u32 key_index,
@@ -1150,22 +1377,36 @@ struct rtl_hal_ops {
 			 bool is_wepkey, bool clear_all);
 	void (*init_sw_leds) (struct ieee80211_hw *hw);
 	void (*deinit_sw_leds) (struct ieee80211_hw *hw);
-	 u32(*get_bbreg) (struct ieee80211_hw *hw, u32 regaddr, u32 bitmask);
+	u32 (*get_bbreg) (struct ieee80211_hw *hw, u32 regaddr, u32 bitmask);
 	void (*set_bbreg) (struct ieee80211_hw *hw, u32 regaddr, u32 bitmask,
 			   u32 data);
-	 u32(*get_rfreg) (struct ieee80211_hw *hw, enum radio_path rfpath,
+	u32 (*get_rfreg) (struct ieee80211_hw *hw, enum radio_path rfpath,
 			  u32 regaddr, u32 bitmask);
 	void (*set_rfreg) (struct ieee80211_hw *hw, enum radio_path rfpath,
 			   u32 regaddr, u32 bitmask, u32 data);
+	bool (*phy_rf6052_config) (struct ieee80211_hw *hw);
+	void (*phy_rf6052_set_cck_txpower) (struct ieee80211_hw *hw,
+					    u8 *powerlevel);
+	void (*phy_rf6052_set_ofdm_txpower) (struct ieee80211_hw *hw,
+					     u8 *ppowerlevel, u8 channel);
+	bool (*config_bb_with_headerfile) (struct ieee80211_hw *hw,
+					   u8 configtype);
+	bool (*config_bb_with_pgheaderfile) (struct ieee80211_hw *hw,
+					     u8 configtype);
+	void (*phy_lc_calibrate) (struct ieee80211_hw *hw, bool is2t);
+	void (*phy_set_bw_mode_callback) (struct ieee80211_hw *hw);
+	void (*dm_dynamic_txpower) (struct ieee80211_hw *hw);
 };
 
 struct rtl_intf_ops {
 	/*com */
+	void (*read_efuse_byte)(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
 	int (*adapter_start) (struct ieee80211_hw *hw);
 	void (*adapter_stop) (struct ieee80211_hw *hw);
 
 	int (*adapter_tx) (struct ieee80211_hw *hw, struct sk_buff *skb);
 	int (*reset_trx_ring) (struct ieee80211_hw *hw);
+	bool (*waitq_insert) (struct ieee80211_hw *hw, struct sk_buff *skb);
 
 	/*pci */
 	void (*disable_aspm) (struct ieee80211_hw *hw);
@@ -1179,11 +1420,36 @@ struct rtl_mod_params {
 	int sw_crypto;
 };
 
+struct rtl_hal_usbint_cfg {
+	/* data - rx */
+	u32 in_ep_num;
+	u32 rx_urb_num;
+	u32 rx_max_size;
+
+	/* op - rx */
+	void (*usb_rx_hdl)(struct ieee80211_hw *, struct sk_buff *);
+	void (*usb_rx_segregate_hdl)(struct ieee80211_hw *, struct sk_buff *,
+				     struct sk_buff_head *);
+
+	/* tx */
+	void (*usb_tx_cleanup)(struct ieee80211_hw *, struct sk_buff *);
+	int (*usb_tx_post_hdl)(struct ieee80211_hw *, struct urb *,
+			       struct sk_buff *);
+	struct sk_buff *(*usb_tx_aggregate_hdl)(struct ieee80211_hw *,
+						struct sk_buff_head *);
+
+	/* endpoint mapping */
+	int (*usb_endpoint_mapping)(struct ieee80211_hw *hw);
+	u16 (*usb_mq_to_hwq)(__le16 fc, u16 mac80211_queue_index);
+};
+
 struct rtl_hal_cfg {
+	u8 bar_id;
 	char *name;
 	char *fw_name;
 	struct rtl_hal_ops *ops;
 	struct rtl_mod_params *mod_params;
+	struct rtl_hal_usbint_cfg *usb_interface_cfg;
 
 	/*this map used for some registers or vars
 	   defined int HAL but used in MAIN */
@@ -1202,6 +1468,11 @@ struct rtl_locks {
 	spinlock_t rf_ps_lock;
 	spinlock_t rf_lock;
 	spinlock_t lps_lock;
+	spinlock_t waitq_lock;
+	spinlock_t tx_urb_lock;
+
+	/*Dual mac*/
+	spinlock_t cck_and_rw_pagea_lock;
 };
 
 struct rtl_works {
@@ -1218,12 +1489,20 @@ struct rtl_works {
 	struct workqueue_struct *rtl_wq;
 	struct delayed_work watchdog_wq;
 	struct delayed_work ips_nic_off_wq;
+
+	/* For SW LPS */
+	struct delayed_work ps_work;
+	struct delayed_work ps_rfon_wq;
 };
 
 struct rtl_debug {
 	u32 dbgp_type[DBGP_TYPE_MAX];
 	u32 global_debuglevel;
 	u64 global_debugcomponents;
+
+	/* add for proc debug */
+	struct proc_dir_entry *proc_dir;
+	char proc_name[20];
 };
 
 struct rtl_priv {
@@ -1274,6 +1553,91 @@ struct rtl_priv {
 #define rtl_efuse(rtlpriv)	(&((rtlpriv)->efuse))
 #define rtl_psc(rtlpriv)	(&((rtlpriv)->psc))
 
+
+/***************************************
+    Bluetooth Co-existance Related
+****************************************/
+
+enum bt_ant_num {
+	ANT_X2 = 0,
+	ANT_X1 = 1,
+};
+
+enum bt_co_type {
+	BT_2WIRE = 0,
+	BT_ISSC_3WIRE = 1,
+	BT_ACCEL = 2,
+	BT_CSR_BC4 = 3,
+	BT_CSR_BC8 = 4,
+	BT_RTL8756 = 5,
+};
+
+enum bt_cur_state {
+	BT_OFF = 0,
+	BT_ON = 1,
+};
+
+enum bt_service_type {
+	BT_SCO = 0,
+	BT_A2DP = 1,
+	BT_HID = 2,
+	BT_HID_IDLE = 3,
+	BT_SCAN = 4,
+	BT_IDLE = 5,
+	BT_OTHER_ACTION = 6,
+	BT_BUSY = 7,
+	BT_OTHERBUSY = 8,
+	BT_PAN = 9,
+};
+
+enum bt_radio_shared {
+	BT_RADIO_SHARED = 0,
+	BT_RADIO_INDIVIDUAL = 1,
+};
+
+struct bt_coexist_info {
+
+	/* EEPROM BT info. */
+	u8 eeprom_bt_coexist;
+	u8 eeprom_bt_type;
+	u8 eeprom_bt_ant_num;
+	u8 eeprom_bt_ant_isolation;
+	u8 eeprom_bt_radio_shared;
+
+	u8 bt_coexistence;
+	u8 bt_ant_num;
+	u8 bt_coexist_type;
+	u8 bt_state;
+	u8 bt_cur_state;	/* 0:on, 1:off */
+	u8 bt_ant_isolation;	/* 0:good, 1:bad */
+	u8 bt_pape_ctrl;	/* 0:SW, 1:SW/HW dynamic */
+	u8 bt_service;
+	u8 bt_radio_shared_type;
+	u8 bt_rfreg_origin_1e;
+	u8 bt_rfreg_origin_1f;
+	u8 bt_rssi_state;
+	u32 ratio_tx;
+	u32 ratio_pri;
+	u32 bt_edca_ul;
+	u32 bt_edca_dl;
+
+	bool b_init_set;
+	bool b_bt_busy_traffic;
+	bool b_bt_traffic_mode_set;
+	bool b_bt_non_traffic_mode_set;
+
+	bool b_fw_coexist_all_off;
+	bool b_sw_coexist_all_off;
+	u32 current_state;
+	u32 previous_state;
+	u8 bt_pre_rssi_state;
+
+	u8 b_reg_bt_iso;
+	u8 b_reg_bt_sco;
+
+};
+
+
 /****************************************
 	mem access macro define start
 	Call endian free function when
@@ -1281,7 +1645,7 @@ struct rtl_priv {
 	2. Before write integer to IO.
 	3. After read integer from IO.
 ****************************************/
-/* Convert little data endian to host */
+/* Convert little data endian to host ordering */
 #define EF1BYTE(_val)		\
 	((u8)(_val))
 #define EF2BYTE(_val)		\
@@ -1289,27 +1653,21 @@ struct rtl_priv {
 #define EF4BYTE(_val)		\
 	(le32_to_cpu(_val))
 
-/* Read data from memory */
-#define READEF1BYTE(_ptr)	\
-	EF1BYTE(*((u8 *)(_ptr)))
+/* Read le16 data from memory and convert to host ordering */
 #define READEF2BYTE(_ptr)	\
 	EF2BYTE(*((u16 *)(_ptr)))
-#define READEF4BYTE(_ptr)	\
-	EF4BYTE(*((u32 *)(_ptr)))
 
-/* Write data to memory */
-#define WRITEEF1BYTE(_ptr, _val)	\
-	(*((u8 *)(_ptr))) = EF1BYTE(_val)
+/* Write le16 data to memory in host ordering */
 #define WRITEEF2BYTE(_ptr, _val)	\
 	(*((u16 *)(_ptr))) = EF2BYTE(_val)
-#define WRITEEF4BYTE(_ptr, _val)	\
-	(*((u32 *)(_ptr))) = EF4BYTE(_val)
-
-/*Example:
-BIT_LEN_MASK_32(0) => 0x00000000
-BIT_LEN_MASK_32(1) => 0x00000001
-BIT_LEN_MASK_32(2) => 0x00000003
-BIT_LEN_MASK_32(32) => 0xFFFFFFFF*/
+
+/* Create a bit mask
+ * Examples:
+ * BIT_LEN_MASK_32(0) => 0x00000000
+ * BIT_LEN_MASK_32(1) => 0x00000001
+ * BIT_LEN_MASK_32(2) => 0x00000003
+ * BIT_LEN_MASK_32(32) => 0xFFFFFFFF
+ */
 #define BIT_LEN_MASK_32(__bitlen)	 \
 	(0xFFFFFFFF >> (32 - (__bitlen)))
 #define BIT_LEN_MASK_16(__bitlen)	 \
@@ -1317,9 +1675,11 @@ BIT_LEN_MASK_32(32) => 0xFFFFFFFF*/
 #define BIT_LEN_MASK_8(__bitlen) \
 	(0xFF >> (8 - (__bitlen)))
 
-/*Example:
-BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
-BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000*/
+/* Create an offset bit mask
+ * Examples:
+ * BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
+ * BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000
+ */
 #define BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) \
 	(BIT_LEN_MASK_32(__bitlen) << (__bitoffset))
 #define BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) \
@@ -1328,8 +1688,9 @@ BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000*/
 	(BIT_LEN_MASK_8(__bitlen) << (__bitoffset))
 
 /*Description:
-Return 4-byte value in host byte ordering from
-4-byte pointer in little-endian system.*/
+ * Return 4-byte value in host byte ordering from
+ * 4-byte pointer in little-endian system.
+ */
 #define LE_P4BYTE_TO_HOST_4BYTE(__pstart) \
 	(EF4BYTE(*((u32 *)(__pstart))))
 #define LE_P2BYTE_TO_HOST_2BYTE(__pstart) \
@@ -1337,28 +1698,10 @@ Return 4-byte value in host byte ordering from
 #define LE_P1BYTE_TO_HOST_1BYTE(__pstart) \
 	(EF1BYTE(*((u8 *)(__pstart))))
 
-/*Description:
-Translate subfield (continuous bits in little-endian) of 4-byte
-value to host byte ordering.*/
-#define LE_BITS_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P4BYTE_TO_HOST_4BYTE(__pstart) >> (__bitoffset))  & \
-		BIT_LEN_MASK_32(__bitlen) \
-	)
-#define LE_BITS_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P2BYTE_TO_HOST_2BYTE(__pstart) >> (__bitoffset)) & \
-		BIT_LEN_MASK_16(__bitlen) \
-	)
-#define LE_BITS_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P1BYTE_TO_HOST_1BYTE(__pstart) >> (__bitoffset)) & \
-		BIT_LEN_MASK_8(__bitlen) \
-	)
-
-/*Description:
-Mask subfield (continuous bits in little-endian) of 4-byte value
-and return the result in 4-byte value in host byte ordering.*/
+/* Description:
+ * Mask subfield (continuous bits in little-endian) of 4-byte value
+ * and return the result in 4-byte value in host byte ordering.
+ */
 #define LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
 	( \
 		LE_P4BYTE_TO_HOST_4BYTE(__pstart)  & \
@@ -1375,20 +1718,9 @@ and return the result in 4-byte value in host byte ordering.*/
 		(~BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen)) \
 	)
 
-/*Description:
-Set subfield of little-endian 4-byte value to specified value.	*/
-#define SET_BITS_TO_LE_4BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u32 *)(__pstart)) = EF4BYTE \
-	( \
-		LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u32)__val) & BIT_LEN_MASK_32(__bitlen)) << (__bitoffset)) \
-	);
-#define SET_BITS_TO_LE_2BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u16 *)(__pstart)) = EF2BYTE \
-	( \
-		LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u16)__val) & BIT_LEN_MASK_16(__bitlen)) << (__bitoffset)) \
-	);
+/* Description:
+ * Set subfield of little-endian 4-byte value to specified value.
+ */
 #define SET_BITS_TO_LE_1BYTE(__pstart, __bitoffset, __bitlen, __val) \
 	*((u8 *)(__pstart)) = EF1BYTE \
 	( \
@@ -1400,13 +1732,14 @@ Set subfield of little-endian 4-byte value to specified value.	*/
 	mem access macro define end
 ****************************************/
 
-#define packet_get_type(_packet) (EF1BYTE((_packet).octet[0]) & 0xFC)
+#define byte(x, n) ((x >> (8 * n)) & 0xff)
+
 #define RTL_WATCH_DOG_TIME	2000
 #define MSECS(t)		msecs_to_jiffies(t)
-#define WLAN_FC_GET_VERS(fc)	((fc) & IEEE80211_FCTL_VERS)
-#define WLAN_FC_GET_TYPE(fc)	((fc) & IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc)	((fc) & IEEE80211_FCTL_STYPE)
-#define WLAN_FC_MORE_DATA(fc)	((fc) & IEEE80211_FCTL_MOREDATA)
+#define WLAN_FC_GET_VERS(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_VERS)
+#define WLAN_FC_GET_TYPE(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_STYPE)
+#define WLAN_FC_MORE_DATA(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_MOREDATA)
 #define SEQ_TO_SN(seq)		(((seq) & IEEE80211_SCTL_SEQ) >> 4)
 #define SN_TO_SEQ(ssn)		(((ssn) << 4) & IEEE80211_SCTL_SEQ)
 #define MAX_SN			((IEEE80211_SCTL_SEQ) >> 4)
@@ -1420,6 +1753,8 @@ Set subfield of little-endian 4-byte value to specified value.	*/
 #define	RT_RF_OFF_LEVL_FW_32K		BIT(5)	/*FW in 32k */
 /*Always enable ASPM and Clock Req in initialization.*/
 #define	RT_RF_PS_LEVEL_ALWAYS_ASPM	BIT(6)
+/* no matter RFOFF or SLEEP we set PS_ASPM_LEVL*/
+#define	RT_PS_LEVEL_ASPM		BIT(7)
 /*When LPS is on, disable 2R if no packet is received or transmittd.*/
 #define	RT_RF_LPS_DISALBE_2R		BIT(30)
 #define	RT_RF_LPS_LEVEL_ASPM		BIT(31)	/*LPS with ASPM */
@@ -1433,15 +1768,6 @@ Set subfield of little-endian 4-byte value to specified value.	*/
 #define container_of_dwork_rtl(x, y, z) \
 	container_of(container_of(x, struct delayed_work, work), y, z)
 
-#define FILL_OCTET_STRING(_os, _octet, _len)	\
-		(_os).octet = (u8 *)(_octet);		\
-		(_os).length = (_len);
-
-#define CP_MACADDR(des, src)	\
-	((des)[0] = (src)[0], (des)[1] = (src)[1],\
-	(des)[2] = (src)[2], (des)[3] = (src)[3],\
-	(des)[4] = (src)[4], (des)[5] = (src)[5])
-
 static inline u8 rtl_read_byte(struct rtl_priv *rtlpriv, u32 addr)
 {
 	return rtlpriv->io.read8_sync(rtlpriv, addr);
diff --git a/drivers/net/wireless/wl1251/acx.c b/drivers/net/wireless/wl1251/acx.c
index 64a0214cfb29..ef8370edace7 100644
--- a/drivers/net/wireless/wl1251/acx.c
+++ b/drivers/net/wireless/wl1251/acx.c
@@ -776,6 +776,31 @@ out:
 	return ret;
 }
 
+int wl1251_acx_low_rssi(struct wl1251 *wl, s8 threshold, u8 weight,
+			u8 depth, enum wl1251_acx_low_rssi_type type)
+{
+	struct acx_low_rssi *rssi;
+	int ret;
+
+	wl1251_debug(DEBUG_ACX, "acx low rssi");
+
+	rssi = kzalloc(sizeof(*rssi), GFP_KERNEL);
+	if (!rssi)
+		return -ENOMEM;
+
+	rssi->threshold = threshold;
+	rssi->weight = weight;
+	rssi->depth = depth;
+	rssi->type = type;
+
+	ret = wl1251_cmd_configure(wl, ACX_LOW_RSSI, rssi, sizeof(*rssi));
+	if (ret < 0)
+		wl1251_warning("failed to set low rssi threshold: %d", ret);
+
+	kfree(rssi);
+	return ret;
+}
+
 int wl1251_acx_set_preamble(struct wl1251 *wl, enum acx_preamble_type preamble)
 {
 	struct acx_preamble *acx;
@@ -978,6 +1003,34 @@ out:
 	return ret;
 }
 
+int wl1251_acx_bet_enable(struct wl1251 *wl, enum wl1251_acx_bet_mode mode,
+			  u8 max_consecutive)
+{
+	struct wl1251_acx_bet_enable *acx;
+	int ret;
+
+	wl1251_debug(DEBUG_ACX, "acx bet enable");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->enable = mode;
+	acx->max_consecutive = max_consecutive;
+
+	ret = wl1251_cmd_configure(wl, ACX_BET_ENABLE, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1251_warning("wl1251 acx bet enable failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
 int wl1251_acx_ac_cfg(struct wl1251 *wl, u8 ac, u8 cw_min, u16 cw_max,
 		      u8 aifs, u16 txop)
 {
diff --git a/drivers/net/wireless/wl1251/acx.h b/drivers/net/wireless/wl1251/acx.h
index efcc3aaca14f..c2ba100f9b1a 100644
--- a/drivers/net/wireless/wl1251/acx.h
+++ b/drivers/net/wireless/wl1251/acx.h
@@ -399,6 +399,49 @@ struct acx_rts_threshold {
 	u8 pad[2];
 } __packed;
 
+enum wl1251_acx_low_rssi_type {
+	/*
+	 * The event is a "Level" indication which keeps triggering
+	 * as long as the average RSSI is below the threshold.
+	 */
+	WL1251_ACX_LOW_RSSI_TYPE_LEVEL = 0,
+
+	/*
+	 * The event is an "Edge" indication which triggers
+	 * only when the RSSI threshold is crossed from above.
+	 */
+	WL1251_ACX_LOW_RSSI_TYPE_EDGE = 1,
+};
+
+struct acx_low_rssi {
+	struct acx_header header;
+
+	/*
+	 * The threshold (in dBm) below (or above after low rssi
+	 * indication) which the firmware generates an interrupt to the
+	 * host. This parameter is signed.
+	 */
+	s8 threshold;
+
+	/*
+	 * The weight of the current RSSI sample, before adding the new
+	 * sample, that is used to calculate the average RSSI.
+	 */
+	u8 weight;
+
+	/*
+	 * The number of Beacons/Probe response frames that will be
+	 * received before issuing the Low or Regained RSSI event.
+	 */
+	u8 depth;
+
+	/*
+	 * Configures how the Low RSSI Event is triggered. Refer to
+	 * enum wl1251_acx_low_rssi_type for more.
+	 */
+	u8 type;
+} __packed;
+
 struct acx_beacon_filter_option {
 	struct acx_header header;
 
@@ -1164,6 +1207,31 @@ struct wl1251_acx_wr_tbtt_and_dtim {
 	u8  padding;
 } __packed;
 
+enum wl1251_acx_bet_mode {
+	WL1251_ACX_BET_DISABLE = 0,
+	WL1251_ACX_BET_ENABLE = 1,
+};
+
+struct wl1251_acx_bet_enable {
+	struct acx_header header;
+
+	/*
+	 * Specifies if beacon early termination procedure is enabled or
+	 * disabled, see enum wl1251_acx_bet_mode.
+	 */
+	u8 enable;
+
+	/*
+	 * Specifies the maximum number of consecutive beacons that may be
+	 * early terminated. After this number is reached at least one full
+	 * beacon must be correctly received in FW before beacon ET
+	 * resumes. Range 0 - 255.
+	 */
+	u8 max_consecutive;
+
+	u8 padding[2];
+} __packed;
+
 struct wl1251_acx_ac_cfg {
 	struct acx_header header;
 
@@ -1393,6 +1461,8 @@ int wl1251_acx_cca_threshold(struct wl1251 *wl);
 int wl1251_acx_bcn_dtim_options(struct wl1251 *wl);
 int wl1251_acx_aid(struct wl1251 *wl, u16 aid);
 int wl1251_acx_event_mbox_mask(struct wl1251 *wl, u32 event_mask);
+int wl1251_acx_low_rssi(struct wl1251 *wl, s8 threshold, u8 weight,
+			u8 depth, enum wl1251_acx_low_rssi_type type);
 int wl1251_acx_set_preamble(struct wl1251 *wl, enum acx_preamble_type preamble);
 int wl1251_acx_cts_protect(struct wl1251 *wl,
 			    enum acx_ctsprotect_type ctsprotect);
@@ -1401,6 +1471,8 @@ int wl1251_acx_tsf_info(struct wl1251 *wl, u64 *mactime);
 int wl1251_acx_rate_policies(struct wl1251 *wl);
 int wl1251_acx_mem_cfg(struct wl1251 *wl);
 int wl1251_acx_wr_tbtt_and_dtim(struct wl1251 *wl, u16 tbtt, u8 dtim);
+int wl1251_acx_bet_enable(struct wl1251 *wl, enum wl1251_acx_bet_mode mode,
+			  u8 max_consecutive);
 int wl1251_acx_ac_cfg(struct wl1251 *wl, u8 ac, u8 cw_min, u16 cw_max,
 		      u8 aifs, u16 txop);
 int wl1251_acx_tid_cfg(struct wl1251 *wl, u8 queue,
diff --git a/drivers/net/wireless/wl1251/event.c b/drivers/net/wireless/wl1251/event.c
index 712372e50a87..dfc4579acb06 100644
--- a/drivers/net/wireless/wl1251/event.c
+++ b/drivers/net/wireless/wl1251/event.c
@@ -90,6 +90,24 @@ static int wl1251_event_process(struct wl1251 *wl, struct event_mailbox *mbox)
 		}
 	}
 
+	if (wl->vif && wl->rssi_thold) {
+		if (vector & ROAMING_TRIGGER_LOW_RSSI_EVENT_ID) {
+			wl1251_debug(DEBUG_EVENT,
+				     "ROAMING_TRIGGER_LOW_RSSI_EVENT");
+			ieee80211_cqm_rssi_notify(wl->vif,
+				NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
+				GFP_KERNEL);
+		}
+
+		if (vector & ROAMING_TRIGGER_REGAINED_RSSI_EVENT_ID) {
+			wl1251_debug(DEBUG_EVENT,
+				     "ROAMING_TRIGGER_REGAINED_RSSI_EVENT");
+			ieee80211_cqm_rssi_notify(wl->vif,
+				NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
+				GFP_KERNEL);
+		}
+	}
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/wl1251/main.c b/drivers/net/wireless/wl1251/main.c
index 40372bac9482..12c9e635a6d6 100644
--- a/drivers/net/wireless/wl1251/main.c
+++ b/drivers/net/wireless/wl1251/main.c
@@ -375,7 +375,7 @@ out:
 	mutex_unlock(&wl->mutex);
 }
 
-static int wl1251_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void wl1251_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct wl1251 *wl = hw->priv;
 	unsigned long flags;
@@ -401,8 +401,6 @@ static int wl1251_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		wl->tx_queue_stopped = true;
 		spin_unlock_irqrestore(&wl->wl_lock, flags);
 	}
-
-	return NETDEV_TX_OK;
 }
 
 static int wl1251_op_start(struct ieee80211_hw *hw)
@@ -502,6 +500,7 @@ static void wl1251_op_stop(struct ieee80211_hw *hw)
 	wl->psm = 0;
 	wl->tx_queue_stopped = false;
 	wl->power_level = WL1251_DEFAULT_POWER_LEVEL;
+	wl->rssi_thold = 0;
 	wl->channel = WL1251_DEFAULT_CHANNEL;
 
 	wl1251_debugfs_reset(wl);
@@ -959,6 +958,16 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
 	if (ret < 0)
 		goto out;
 
+	if (changed & BSS_CHANGED_CQM) {
+		ret = wl1251_acx_low_rssi(wl, bss_conf->cqm_rssi_thold,
+					  WL1251_DEFAULT_LOW_RSSI_WEIGHT,
+					  WL1251_DEFAULT_LOW_RSSI_DEPTH,
+					  WL1251_ACX_LOW_RSSI_TYPE_EDGE);
+		if (ret < 0)
+			goto out;
+		wl->rssi_thold = bss_conf->cqm_rssi_thold;
+	}
+
 	if (changed & BSS_CHANGED_BSSID) {
 		memcpy(wl->bssid, bss_conf->bssid, ETH_ALEN);
 
@@ -1313,9 +1322,11 @@ int wl1251_init_ieee80211(struct wl1251 *wl)
 	wl->hw->flags = IEEE80211_HW_SIGNAL_DBM |
 		IEEE80211_HW_SUPPORTS_PS |
 		IEEE80211_HW_BEACON_FILTER |
-		IEEE80211_HW_SUPPORTS_UAPSD;
+		IEEE80211_HW_SUPPORTS_UAPSD |
+		IEEE80211_HW_SUPPORTS_CQM_RSSI;
 
-	wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+	wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+					 BIT(NL80211_IFTYPE_ADHOC);
 	wl->hw->wiphy->max_scan_ssids = 1;
 	wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &wl1251_band_2ghz;
 
@@ -1377,6 +1388,7 @@ struct ieee80211_hw *wl1251_alloc_hw(void)
 	wl->psm_requested = false;
 	wl->tx_queue_stopped = false;
 	wl->power_level = WL1251_DEFAULT_POWER_LEVEL;
+	wl->rssi_thold = 0;
 	wl->beacon_int = WL1251_DEFAULT_BEACON_INT;
 	wl->dtim_period = WL1251_DEFAULT_DTIM_PERIOD;
 	wl->vif = NULL;
diff --git a/drivers/net/wireless/wl1251/ps.c b/drivers/net/wireless/wl1251/ps.c
index 5ed47c8373d2..9ba23ede51bd 100644
--- a/drivers/net/wireless/wl1251/ps.c
+++ b/drivers/net/wireless/wl1251/ps.c
@@ -153,6 +153,11 @@ int wl1251_ps_set_mode(struct wl1251 *wl, enum wl1251_cmd_ps_mode mode)
 		if (ret < 0)
 			return ret;
 
+		ret = wl1251_acx_bet_enable(wl, WL1251_ACX_BET_ENABLE,
+					    WL1251_DEFAULT_BET_CONSECUTIVE);
+		if (ret < 0)
+			return ret;
+
 		ret = wl1251_cmd_ps_mode(wl, STATION_POWER_SAVE_MODE);
 		if (ret < 0)
 			return ret;
@@ -170,6 +175,12 @@ int wl1251_ps_set_mode(struct wl1251 *wl, enum wl1251_cmd_ps_mode mode)
 		if (ret < 0)
 			return ret;
 
+		/* disable BET */
+		ret = wl1251_acx_bet_enable(wl, WL1251_ACX_BET_DISABLE,
+					    WL1251_DEFAULT_BET_CONSECUTIVE);
+		if (ret < 0)
+			return ret;
+
 		/* disable beacon filtering */
 		ret = wl1251_acx_beacon_filter_opt(wl, false);
 		if (ret < 0)
diff --git a/drivers/net/wireless/wl1251/rx.c b/drivers/net/wireless/wl1251/rx.c
index efa53607d5c9..c1b3b3f03da2 100644
--- a/drivers/net/wireless/wl1251/rx.c
+++ b/drivers/net/wireless/wl1251/rx.c
@@ -78,9 +78,10 @@ static void wl1251_rx_status(struct wl1251 *wl,
 	 */
 	wl->noise = desc->rssi - desc->snr / 2;
 
-	status->freq = ieee80211_channel_to_frequency(desc->channel);
+	status->freq = ieee80211_channel_to_frequency(desc->channel,
+						      status->band);
 
-	status->flag |= RX_FLAG_TSFT;
+	status->flag |= RX_FLAG_MACTIME_MPDU;
 
 	if (desc->flags & RX_DESC_ENCRYPTION_MASK) {
 		status->flag |= RX_FLAG_IV_STRIPPED | RX_FLAG_MMIC_STRIPPED;
@@ -95,8 +96,52 @@ static void wl1251_rx_status(struct wl1251 *wl,
 	if (unlikely(!(desc->flags & RX_DESC_VALID_FCS)))
 		status->flag |= RX_FLAG_FAILED_FCS_CRC;
 
+	switch (desc->rate) {
+		/* skip 1 and 12 Mbps because they have same value 0x0a */
+	case RATE_2MBPS:
+		status->rate_idx = 1;
+		break;
+	case RATE_5_5MBPS:
+		status->rate_idx = 2;
+		break;
+	case RATE_11MBPS:
+		status->rate_idx = 3;
+		break;
+	case RATE_6MBPS:
+		status->rate_idx = 4;
+		break;
+	case RATE_9MBPS:
+		status->rate_idx = 5;
+		break;
+	case RATE_18MBPS:
+		status->rate_idx = 7;
+		break;
+	case RATE_24MBPS:
+		status->rate_idx = 8;
+		break;
+	case RATE_36MBPS:
+		status->rate_idx = 9;
+		break;
+	case RATE_48MBPS:
+		status->rate_idx = 10;
+		break;
+	case RATE_54MBPS:
+		status->rate_idx = 11;
+		break;
+	}
+
+	/* for 1 and 12 Mbps we have to check the modulation */
+	if (desc->rate == RATE_1MBPS) {
+		if (!(desc->mod_pre & OFDM_RATE_BIT))
+			/* CCK -> RATE_1MBPS */
+			status->rate_idx = 0;
+		else
+			/* OFDM -> RATE_12MBPS */
+			status->rate_idx = 6;
+	}
 
-	/* FIXME: set status->rate_idx */
+	if (desc->mod_pre & SHORT_PREAMBLE_BIT)
+		status->flag |= RX_FLAG_SHORTPRE;
 }
 
 static void wl1251_rx_body(struct wl1251 *wl,
diff --git a/drivers/net/wireless/wl1251/tx.c b/drivers/net/wireless/wl1251/tx.c
index 554b4f9a3d3e..28121c590a2b 100644
--- a/drivers/net/wireless/wl1251/tx.c
+++ b/drivers/net/wireless/wl1251/tx.c
@@ -213,16 +213,30 @@ static int wl1251_tx_send_packet(struct wl1251 *wl, struct sk_buff *skb,
 		wl1251_debug(DEBUG_TX, "skb offset %d", offset);
 
 		/* check whether the current skb can be used */
-		if (!skb_cloned(skb) && (skb_tailroom(skb) >= offset)) {
-			unsigned char *src = skb->data;
+		if (skb_cloned(skb) || (skb_tailroom(skb) < offset)) {
+			struct sk_buff *newskb = skb_copy_expand(skb, 0, 3,
+								 GFP_KERNEL);
+
+			if (unlikely(newskb == NULL)) {
+				wl1251_error("Can't allocate skb!");
+				return -EINVAL;
+			}
 
-			/* align the buffer on a 4-byte boundary */
+			tx_hdr = (struct tx_double_buffer_desc *) newskb->data;
+
+			dev_kfree_skb_any(skb);
+			wl->tx_frames[tx_hdr->id] = skb = newskb;
+
+			offset = (4 - (long)skb->data) & 0x03;
+			wl1251_debug(DEBUG_TX, "new skb offset %d", offset);
+		}
+
+		/* align the buffer on a 4-byte boundary */
+		if (offset) {
+			unsigned char *src = skb->data;
 			skb_reserve(skb, offset);
 			memmove(skb->data, src, skb->len);
 			tx_hdr = (struct tx_double_buffer_desc *) skb->data;
-		} else {
-			wl1251_info("No handler, fixme!");
-			return -EINVAL;
 		}
 	}
 
@@ -368,7 +382,7 @@ static void wl1251_tx_packet_cb(struct wl1251 *wl,
 {
 	struct ieee80211_tx_info *info;
 	struct sk_buff *skb;
-	int hdrlen, ret;
+	int hdrlen;
 	u8 *frame;
 
 	skb = wl->tx_frames[result->id];
@@ -407,40 +421,12 @@ static void wl1251_tx_packet_cb(struct wl1251 *wl,
 	ieee80211_tx_status(wl->hw, skb);
 
 	wl->tx_frames[result->id] = NULL;
-
-	if (wl->tx_queue_stopped) {
-		wl1251_debug(DEBUG_TX, "cb: queue was stopped");
-
-		skb = skb_dequeue(&wl->tx_queue);
-
-		/* The skb can be NULL because tx_work might have been
-		   scheduled before the queue was stopped making the
-		   queue empty */
-
-		if (skb) {
-			ret = wl1251_tx_frame(wl, skb);
-			if (ret == -EBUSY) {
-				/* firmware buffer is still full */
-				wl1251_debug(DEBUG_TX, "cb: fw buffer "
-					     "still full");
-				skb_queue_head(&wl->tx_queue, skb);
-				return;
-			} else if (ret < 0) {
-				dev_kfree_skb(skb);
-				return;
-			}
-		}
-
-		wl1251_debug(DEBUG_TX, "cb: waking queues");
-		ieee80211_wake_queues(wl->hw);
-		wl->tx_queue_stopped = false;
-	}
 }
 
 /* Called upon reception of a TX complete interrupt */
 void wl1251_tx_complete(struct wl1251 *wl)
 {
-	int i, result_index, num_complete = 0;
+	int i, result_index, num_complete = 0, queue_len;
 	struct tx_result result[FW_TX_CMPLT_BLOCK_SIZE], *result_ptr;
 	unsigned long flags;
 
@@ -471,18 +457,22 @@ void wl1251_tx_complete(struct wl1251 *wl)
 		}
 	}
 
-	if (wl->tx_queue_stopped
-	    &&
-	    skb_queue_len(&wl->tx_queue) <= WL1251_TX_QUEUE_LOW_WATERMARK){
+	queue_len = skb_queue_len(&wl->tx_queue);
 
-		/* firmware buffer has space, restart queues */
+	if ((num_complete > 0) && (queue_len > 0)) {
+		/* firmware buffer has space, reschedule tx_work */
+		wl1251_debug(DEBUG_TX, "tx_complete: reschedule tx_work");
+		ieee80211_queue_work(wl->hw, &wl->tx_work);
+	}
+
+	if (wl->tx_queue_stopped &&
+	    queue_len <= WL1251_TX_QUEUE_LOW_WATERMARK) {
+		/* tx_queue has space, restart queues */
 		wl1251_debug(DEBUG_TX, "tx_complete: waking queues");
 		spin_lock_irqsave(&wl->wl_lock, flags);
 		ieee80211_wake_queues(wl->hw);
 		wl->tx_queue_stopped = false;
 		spin_unlock_irqrestore(&wl->wl_lock, flags);
-		ieee80211_queue_work(wl->hw, &wl->tx_work);
-
 	}
 
 	/* Every completed frame needs to be acknowledged */
diff --git a/drivers/net/wireless/wl1251/wl1251.h b/drivers/net/wireless/wl1251/wl1251.h
index c0ce2c8b43b8..bb23cd522b22 100644
--- a/drivers/net/wireless/wl1251/wl1251.h
+++ b/drivers/net/wireless/wl1251/wl1251.h
@@ -370,6 +370,8 @@ struct wl1251 {
 	/* in dBm */
 	int power_level;
 
+	int rssi_thold;
+
 	struct wl1251_stats stats;
 	struct wl1251_debugfs debugfs;
 
@@ -410,6 +412,8 @@ void wl1251_disable_interrupts(struct wl1251 *wl);
 
 #define WL1251_DEFAULT_CHANNEL 0
 
+#define WL1251_DEFAULT_BET_CONSECUTIVE 10
+
 #define CHIP_ID_1251_PG10	           (0x7010101)
 #define CHIP_ID_1251_PG11	           (0x7020101)
 #define CHIP_ID_1251_PG12	           (0x7030101)
@@ -431,4 +435,7 @@ void wl1251_disable_interrupts(struct wl1251 *wl);
 #define WL1251_PART_WORK_REG_START	REGISTERS_BASE
 #define WL1251_PART_WORK_REG_SIZE	REGISTERS_WORK_SIZE
 
+#define WL1251_DEFAULT_LOW_RSSI_WEIGHT          10
+#define WL1251_DEFAULT_LOW_RSSI_DEPTH           10
+
 #endif
diff --git a/drivers/net/wireless/wl12xx/Kconfig b/drivers/net/wireless/wl12xx/Kconfig
index 0e65bce457d6..692ebff38fc8 100644
--- a/drivers/net/wireless/wl12xx/Kconfig
+++ b/drivers/net/wireless/wl12xx/Kconfig
@@ -54,7 +54,7 @@ config WL12XX_SDIO
 
 config WL12XX_SDIO_TEST
 	tristate "TI wl12xx SDIO testing support"
-	depends on WL12XX && MMC
+	depends on WL12XX && MMC && WL12XX_SDIO
 	default n
 	---help---
 	  This module adds support for the SDIO bus testing with the
diff --git a/drivers/net/wireless/wl12xx/acx.c b/drivers/net/wireless/wl12xx/acx.c
index cc4068d2b4a8..a3db755ceeda 100644
--- a/drivers/net/wireless/wl12xx/acx.c
+++ b/drivers/net/wireless/wl12xx/acx.c
@@ -751,10 +751,10 @@ int wl1271_acx_statistics(struct wl1271 *wl, struct acx_statistics *stats)
 	return 0;
 }
 
-int wl1271_acx_rate_policies(struct wl1271 *wl)
+int wl1271_acx_sta_rate_policies(struct wl1271 *wl)
 {
-	struct acx_rate_policy *acx;
-	struct conf_tx_rate_class *c = &wl->conf.tx.rc_conf;
+	struct acx_sta_rate_policy *acx;
+	struct conf_tx_rate_class *c = &wl->conf.tx.sta_rc_conf;
 	int idx = 0;
 	int ret = 0;
 
@@ -783,6 +783,10 @@ int wl1271_acx_rate_policies(struct wl1271 *wl)
 
 	acx->rate_class_cnt = cpu_to_le32(ACX_TX_RATE_POLICY_CNT);
 
+	wl1271_debug(DEBUG_ACX, "basic_rate: 0x%x, full_rate: 0x%x",
+		acx->rate_class[ACX_TX_BASIC_RATE].enabled_rates,
+		acx->rate_class[ACX_TX_AP_FULL_RATE].enabled_rates);
+
 	ret = wl1271_cmd_configure(wl, ACX_RATE_POLICY, acx, sizeof(*acx));
 	if (ret < 0) {
 		wl1271_warning("Setting of rate policies failed: %d", ret);
@@ -794,6 +798,38 @@ out:
 	return ret;
 }
 
+int wl1271_acx_ap_rate_policy(struct wl1271 *wl, struct conf_tx_rate_class *c,
+		      u8 idx)
+{
+	struct acx_ap_rate_policy *acx;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_ACX, "acx ap rate policy");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->rate_policy.enabled_rates = cpu_to_le32(c->enabled_rates);
+	acx->rate_policy.short_retry_limit = c->short_retry_limit;
+	acx->rate_policy.long_retry_limit = c->long_retry_limit;
+	acx->rate_policy.aflags = c->aflags;
+
+	acx->rate_policy_idx = cpu_to_le32(idx);
+
+	ret = wl1271_cmd_configure(wl, ACX_RATE_POLICY, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("Setting of ap rate policy failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
 int wl1271_acx_ac_cfg(struct wl1271 *wl, u8 ac, u8 cw_min, u16 cw_max,
 		      u8 aifsn, u16 txop)
 {
@@ -915,9 +951,9 @@ out:
 	return ret;
 }
 
-int wl1271_acx_mem_cfg(struct wl1271 *wl)
+int wl1271_acx_ap_mem_cfg(struct wl1271 *wl)
 {
-	struct wl1271_acx_config_memory *mem_conf;
+	struct wl1271_acx_ap_config_memory *mem_conf;
 	int ret;
 
 	wl1271_debug(DEBUG_ACX, "wl1271 mem cfg");
@@ -929,10 +965,10 @@ int wl1271_acx_mem_cfg(struct wl1271 *wl)
 	}
 
 	/* memory config */
-	mem_conf->num_stations = DEFAULT_NUM_STATIONS;
-	mem_conf->rx_mem_block_num = ACX_RX_MEM_BLOCKS;
-	mem_conf->tx_min_mem_block_num = ACX_TX_MIN_MEM_BLOCKS;
-	mem_conf->num_ssid_profiles = ACX_NUM_SSID_PROFILES;
+	mem_conf->num_stations = wl->conf.mem.num_stations;
+	mem_conf->rx_mem_block_num = wl->conf.mem.rx_block_num;
+	mem_conf->tx_min_mem_block_num = wl->conf.mem.tx_min_block_num;
+	mem_conf->num_ssid_profiles = wl->conf.mem.ssid_profiles;
 	mem_conf->total_tx_descriptors = cpu_to_le32(ACX_TX_DESCRIPTORS);
 
 	ret = wl1271_cmd_configure(wl, ACX_MEM_CFG, mem_conf,
@@ -947,13 +983,45 @@ out:
 	return ret;
 }
 
-int wl1271_acx_init_mem_config(struct wl1271 *wl)
+int wl1271_acx_sta_mem_cfg(struct wl1271 *wl)
 {
+	struct wl1271_acx_sta_config_memory *mem_conf;
 	int ret;
 
-	ret = wl1271_acx_mem_cfg(wl);
-	if (ret < 0)
-		return ret;
+	wl1271_debug(DEBUG_ACX, "wl1271 mem cfg");
+
+	mem_conf = kzalloc(sizeof(*mem_conf), GFP_KERNEL);
+	if (!mem_conf) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* memory config */
+	mem_conf->num_stations = wl->conf.mem.num_stations;
+	mem_conf->rx_mem_block_num = wl->conf.mem.rx_block_num;
+	mem_conf->tx_min_mem_block_num = wl->conf.mem.tx_min_block_num;
+	mem_conf->num_ssid_profiles = wl->conf.mem.ssid_profiles;
+	mem_conf->total_tx_descriptors = cpu_to_le32(ACX_TX_DESCRIPTORS);
+	mem_conf->dyn_mem_enable = wl->conf.mem.dynamic_memory;
+	mem_conf->tx_free_req = wl->conf.mem.min_req_tx_blocks;
+	mem_conf->rx_free_req = wl->conf.mem.min_req_rx_blocks;
+	mem_conf->tx_min = wl->conf.mem.tx_min;
+
+	ret = wl1271_cmd_configure(wl, ACX_MEM_CFG, mem_conf,
+				   sizeof(*mem_conf));
+	if (ret < 0) {
+		wl1271_warning("wl1271 mem config failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(mem_conf);
+	return ret;
+}
+
+int wl1271_acx_init_mem_config(struct wl1271 *wl)
+{
+	int ret;
 
 	wl->target_mem_map = kzalloc(sizeof(struct wl1271_acx_mem_map),
 				     GFP_KERNEL);
@@ -1233,6 +1301,7 @@ int wl1271_acx_set_ht_capabilities(struct wl1271 *wl,
 	struct wl1271_acx_ht_capabilities *acx;
 	u8 mac_address[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	int ret = 0;
+	u32 ht_capabilites = 0;
 
 	wl1271_debug(DEBUG_ACX, "acx ht capabilities setting");
 
@@ -1244,27 +1313,26 @@ int wl1271_acx_set_ht_capabilities(struct wl1271 *wl,
 
 	/* Allow HT Operation ? */
 	if (allow_ht_operation) {
-		acx->ht_capabilites =
+		ht_capabilites =
 			WL1271_ACX_FW_CAP_HT_OPERATION;
 		if (ht_cap->cap & IEEE80211_HT_CAP_GRN_FLD)
-			acx->ht_capabilites |=
+			ht_capabilites |=
 				WL1271_ACX_FW_CAP_GREENFIELD_FRAME_FORMAT;
 		if (ht_cap->cap & IEEE80211_HT_CAP_SGI_20)
-			acx->ht_capabilites |=
+			ht_capabilites |=
 				WL1271_ACX_FW_CAP_SHORT_GI_FOR_20MHZ_PACKETS;
 		if (ht_cap->cap & IEEE80211_HT_CAP_LSIG_TXOP_PROT)
-			acx->ht_capabilites |=
+			ht_capabilites |=
 				WL1271_ACX_FW_CAP_LSIG_TXOP_PROTECTION;
 
 		/* get data from A-MPDU parameters field */
 		acx->ampdu_max_length = ht_cap->ampdu_factor;
 		acx->ampdu_min_spacing = ht_cap->ampdu_density;
-
-		memcpy(acx->mac_address, mac_address, ETH_ALEN);
-	} else { /* HT operations are not allowed */
-		acx->ht_capabilites = 0;
 	}
 
+	memcpy(acx->mac_address, mac_address, ETH_ALEN);
+	acx->ht_capabilites = cpu_to_le32(ht_capabilites);
+
 	ret = wl1271_cmd_configure(wl, ACX_PEER_HT_CAP, acx, sizeof(*acx));
 	if (ret < 0) {
 		wl1271_warning("acx ht capabilities setting failed: %d", ret);
@@ -1293,7 +1361,8 @@ int wl1271_acx_set_ht_information(struct wl1271 *wl,
 	acx->ht_protection =
 		(u8)(ht_operation_mode & IEEE80211_HT_OP_MODE_PROTECTION);
 	acx->rifs_mode = 0;
-	acx->gf_protection = 0;
+	acx->gf_protection =
+		!!(ht_operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
 	acx->ht_tx_burst_limit = 0;
 	acx->dual_cts_protection = 0;
 
@@ -1309,6 +1378,91 @@ out:
 	return ret;
 }
 
+/* Configure BA session initiator/receiver parameters setting in the FW. */
+int wl1271_acx_set_ba_session(struct wl1271 *wl,
+			       enum ieee80211_back_parties direction,
+			       u8 tid_index, u8 policy)
+{
+	struct wl1271_acx_ba_session_policy *acx;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx ba session setting");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* ANY role */
+	acx->role_id = 0xff;
+	acx->tid = tid_index;
+	acx->enable = policy;
+	acx->ba_direction = direction;
+
+	switch (direction) {
+	case WLAN_BACK_INITIATOR:
+		acx->win_size = wl->conf.ht.tx_ba_win_size;
+		acx->inactivity_timeout = wl->conf.ht.inactivity_timeout;
+		break;
+	case WLAN_BACK_RECIPIENT:
+		acx->win_size = RX_BA_WIN_SIZE;
+		acx->inactivity_timeout = 0;
+		break;
+	default:
+		wl1271_error("Incorrect acx command id=%x\n", direction);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = wl1271_cmd_configure(wl,
+				   ACX_BA_SESSION_POLICY_CFG,
+				   acx,
+				   sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx ba session setting failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+/* setup BA session receiver setting in the FW. */
+int wl1271_acx_set_ba_receiver_session(struct wl1271 *wl, u8 tid_index, u16 ssn,
+					bool enable)
+{
+	struct wl1271_acx_ba_receiver_setup *acx;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx ba receiver session setting");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Single link for now */
+	acx->link_id = 1;
+	acx->tid = tid_index;
+	acx->enable = enable;
+	acx->win_size = 0;
+	acx->ssn = ssn;
+
+	ret = wl1271_cmd_configure(wl, ACX_BA_SESSION_RX_SETUP, acx,
+				   sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx ba receiver session failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
 int wl1271_acx_tsf_info(struct wl1271 *wl, u64 *mactime)
 {
 	struct wl1271_acx_fw_tsf_information *tsf_info;
@@ -1334,3 +1488,82 @@ out:
 	kfree(tsf_info);
 	return ret;
 }
+
+int wl1271_acx_max_tx_retry(struct wl1271 *wl)
+{
+	struct wl1271_acx_max_tx_retry *acx = NULL;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx max tx retry");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx)
+		return -ENOMEM;
+
+	acx->max_tx_retry = cpu_to_le16(wl->conf.tx.ap_max_tx_retries);
+
+	ret = wl1271_cmd_configure(wl, ACX_MAX_TX_FAILURE, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx max tx retry failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+int wl1271_acx_config_ps(struct wl1271 *wl)
+{
+	struct wl1271_acx_config_ps *config_ps;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx config ps");
+
+	config_ps = kzalloc(sizeof(*config_ps), GFP_KERNEL);
+	if (!config_ps) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	config_ps->exit_retries = wl->conf.conn.psm_exit_retries;
+	config_ps->enter_retries = wl->conf.conn.psm_entry_retries;
+	config_ps->null_data_rate = cpu_to_le32(wl->basic_rate);
+
+	ret = wl1271_cmd_configure(wl, ACX_CONFIG_PS, config_ps,
+				   sizeof(*config_ps));
+
+	if (ret < 0) {
+		wl1271_warning("acx config ps failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(config_ps);
+	return ret;
+}
+
+int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr)
+{
+	struct wl1271_acx_inconnection_sta *acx = NULL;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx set inconnaction sta %pM", addr);
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx)
+		return -ENOMEM;
+
+	memcpy(acx->addr, addr, ETH_ALEN);
+
+	ret = wl1271_cmd_configure(wl, ACX_UPDATE_INCONNECTION_STA_LIST,
+				   acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("acx set inconnaction sta failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
diff --git a/drivers/net/wireless/wl12xx/acx.h b/drivers/net/wireless/wl12xx/acx.h
index 7bd8e4db4a71..dd19b01d807b 100644
--- a/drivers/net/wireless/wl12xx/acx.h
+++ b/drivers/net/wireless/wl12xx/acx.h
@@ -133,7 +133,6 @@ enum {
 
 #define DEFAULT_UCAST_PRIORITY          0
 #define DEFAULT_RX_Q_PRIORITY           0
-#define DEFAULT_NUM_STATIONS            1
 #define DEFAULT_RXQ_PRIORITY            0 /* low 0 .. 15 high  */
 #define DEFAULT_RXQ_TYPE                0x07    /* All frames, Data/Ctrl/Mgmt */
 #define TRACE_BUFFER_MAX_SIZE           256
@@ -747,13 +746,23 @@ struct acx_rate_class {
 #define ACX_TX_BASIC_RATE      0
 #define ACX_TX_AP_FULL_RATE    1
 #define ACX_TX_RATE_POLICY_CNT 2
-struct acx_rate_policy {
+struct acx_sta_rate_policy {
 	struct acx_header header;
 
 	__le32 rate_class_cnt;
 	struct acx_rate_class rate_class[CONF_TX_MAX_RATE_CLASSES];
 } __packed;
 
+
+#define ACX_TX_AP_MODE_MGMT_RATE 4
+#define ACX_TX_AP_MODE_BCST_RATE 5
+struct acx_ap_rate_policy {
+	struct acx_header header;
+
+	__le32 rate_policy_idx;
+	struct acx_rate_class rate_policy;
+} __packed;
+
 struct acx_ac_cfg {
 	struct acx_header header;
 	u8 ac;
@@ -787,12 +796,9 @@ struct acx_tx_config_options {
 	__le16 tx_compl_threshold;   /* number of packets */
 } __packed;
 
-#define ACX_RX_MEM_BLOCKS     70
-#define ACX_TX_MIN_MEM_BLOCKS 40
 #define ACX_TX_DESCRIPTORS    32
-#define ACX_NUM_SSID_PROFILES 1
 
-struct wl1271_acx_config_memory {
+struct wl1271_acx_ap_config_memory {
 	struct acx_header header;
 
 	u8 rx_mem_block_num;
@@ -802,6 +808,20 @@ struct wl1271_acx_config_memory {
 	__le32 total_tx_descriptors;
 } __packed;
 
+struct wl1271_acx_sta_config_memory {
+	struct acx_header header;
+
+	u8 rx_mem_block_num;
+	u8 tx_min_mem_block_num;
+	u8 num_stations;
+	u8 num_ssid_profiles;
+	__le32 total_tx_descriptors;
+	u8 dyn_mem_enable;
+	u8 tx_free_req;
+	u8 rx_free_req;
+	u8 tx_min;
+} __packed;
+
 struct wl1271_acx_mem_map {
 	struct acx_header header;
 
@@ -1051,6 +1071,59 @@ struct wl1271_acx_ht_information {
 	u8 padding[3];
 } __packed;
 
+#define RX_BA_WIN_SIZE 8
+
+struct wl1271_acx_ba_session_policy {
+	struct acx_header header;
+	/*
+	 * Specifies role Id, Range 0-7, 0xFF means ANY role.
+	 * Future use. For now this field is irrelevant
+	 */
+	u8 role_id;
+	/*
+	 * Specifies Link Id, Range 0-31, 0xFF means ANY  Link Id.
+	 * Not applicable if Role Id is set to ANY.
+	 */
+	u8 link_id;
+
+	u8 tid;
+
+	u8 enable;
+
+	/* Windows size in number of packets */
+	u16 win_size;
+
+	/*
+	 * As initiator inactivity timeout in time units(TU) of 1024us.
+	 * As receiver reserved
+	 */
+	u16 inactivity_timeout;
+
+	/* Initiator = 1/Receiver = 0 */
+	u8 ba_direction;
+
+	u8 padding[3];
+} __packed;
+
+struct wl1271_acx_ba_receiver_setup {
+	struct acx_header header;
+
+	/* Specifies Link Id, Range 0-31, 0xFF means ANY  Link Id */
+	u8 link_id;
+
+	u8 tid;
+
+	u8 enable;
+
+	u8 padding[1];
+
+	/* Windows size in number of packets */
+	u16 win_size;
+
+	/* BA session starting sequence number.  RANGE 0-FFF */
+	u16 ssn;
+} __packed;
+
 struct wl1271_acx_fw_tsf_information {
 	struct acx_header header;
 
@@ -1062,6 +1135,33 @@ struct wl1271_acx_fw_tsf_information {
 	u8 padding[3];
 } __packed;
 
+struct wl1271_acx_max_tx_retry {
+	struct acx_header header;
+
+	/*
+	 * the number of frames transmission failures before
+	 * issuing the aging event.
+	 */
+	__le16 max_tx_retry;
+	u8 padding_1[2];
+} __packed;
+
+struct wl1271_acx_config_ps {
+	struct acx_header header;
+
+	u8 exit_retries;
+	u8 enter_retries;
+	u8 padding[2];
+	__le32 null_data_rate;
+} __packed;
+
+struct wl1271_acx_inconnection_sta {
+	struct acx_header header;
+
+	u8 addr[ETH_ALEN];
+	u8 padding1[2];
+} __packed;
+
 enum {
 	ACX_WAKE_UP_CONDITIONS      = 0x0002,
 	ACX_MEM_CFG                 = 0x0003,
@@ -1113,22 +1213,24 @@ enum {
 	ACX_RSSI_SNR_WEIGHTS        = 0x0052,
 	ACX_KEEP_ALIVE_MODE         = 0x0053,
 	ACX_SET_KEEP_ALIVE_CONFIG   = 0x0054,
-	ACX_BA_SESSION_RESPONDER_POLICY = 0x0055,
-	ACX_BA_SESSION_INITIATOR_POLICY = 0x0056,
+	ACX_BA_SESSION_POLICY_CFG   = 0x0055,
+	ACX_BA_SESSION_RX_SETUP     = 0x0056,
 	ACX_PEER_HT_CAP             = 0x0057,
 	ACX_HT_BSS_OPERATION        = 0x0058,
 	ACX_COEX_ACTIVITY           = 0x0059,
 	ACX_SET_DCO_ITRIM_PARAMS    = 0x0061,
+	ACX_GEN_FW_CMD              = 0x0070,
+	ACX_HOST_IF_CFG_BITMAP      = 0x0071,
+	ACX_MAX_TX_FAILURE          = 0x0072,
+	ACX_UPDATE_INCONNECTION_STA_LIST = 0x0073,
 	DOT11_RX_MSDU_LIFE_TIME     = 0x1004,
 	DOT11_CUR_TX_PWR            = 0x100D,
 	DOT11_RX_DOT11_MODE         = 0x1012,
 	DOT11_RTS_THRESHOLD         = 0x1013,
 	DOT11_GROUP_ADDRESS_TBL     = 0x1014,
 	ACX_PM_CONFIG               = 0x1016,
-
-	MAX_DOT11_IE = DOT11_GROUP_ADDRESS_TBL,
-
-	MAX_IE = 0xFFFF
+	ACX_CONFIG_PS               = 0x1017,
+	ACX_CONFIG_HANGOVER         = 0x1018,
 };
 
 
@@ -1160,7 +1262,9 @@ int wl1271_acx_set_preamble(struct wl1271 *wl, enum acx_preamble_type preamble);
 int wl1271_acx_cts_protect(struct wl1271 *wl,
 			   enum acx_ctsprotect_type ctsprotect);
 int wl1271_acx_statistics(struct wl1271 *wl, struct acx_statistics *stats);
-int wl1271_acx_rate_policies(struct wl1271 *wl);
+int wl1271_acx_sta_rate_policies(struct wl1271 *wl);
+int wl1271_acx_ap_rate_policy(struct wl1271 *wl, struct conf_tx_rate_class *c,
+		      u8 idx);
 int wl1271_acx_ac_cfg(struct wl1271 *wl, u8 ac, u8 cw_min, u16 cw_max,
 		      u8 aifsn, u16 txop);
 int wl1271_acx_tid_cfg(struct wl1271 *wl, u8 queue_id, u8 channel_type,
@@ -1168,7 +1272,8 @@ int wl1271_acx_tid_cfg(struct wl1271 *wl, u8 queue_id, u8 channel_type,
 		       u32 apsd_conf0, u32 apsd_conf1);
 int wl1271_acx_frag_threshold(struct wl1271 *wl, u16 frag_threshold);
 int wl1271_acx_tx_config_options(struct wl1271 *wl);
-int wl1271_acx_mem_cfg(struct wl1271 *wl);
+int wl1271_acx_ap_mem_cfg(struct wl1271 *wl);
+int wl1271_acx_sta_mem_cfg(struct wl1271 *wl);
 int wl1271_acx_init_mem_config(struct wl1271 *wl);
 int wl1271_acx_init_rx_interrupt(struct wl1271 *wl);
 int wl1271_acx_smart_reflex(struct wl1271 *wl);
@@ -1185,6 +1290,14 @@ int wl1271_acx_set_ht_capabilities(struct wl1271 *wl,
 				    bool allow_ht_operation);
 int wl1271_acx_set_ht_information(struct wl1271 *wl,
 				   u16 ht_operation_mode);
+int wl1271_acx_set_ba_session(struct wl1271 *wl,
+			      enum ieee80211_back_parties direction,
+			      u8 tid_index, u8 policy);
+int wl1271_acx_set_ba_receiver_session(struct wl1271 *wl, u8 tid_index, u16 ssn,
+				       bool enable);
 int wl1271_acx_tsf_info(struct wl1271 *wl, u64 *mactime);
+int wl1271_acx_max_tx_retry(struct wl1271 *wl);
+int wl1271_acx_config_ps(struct wl1271 *wl);
+int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr);
 
 #endif /* __WL1271_ACX_H__ */
diff --git a/drivers/net/wireless/wl12xx/boot.c b/drivers/net/wireless/wl12xx/boot.c
index 4df04f84d7f1..6934dffd5174 100644
--- a/drivers/net/wireless/wl12xx/boot.c
+++ b/drivers/net/wireless/wl12xx/boot.c
@@ -28,6 +28,7 @@
 #include "boot.h"
 #include "io.h"
 #include "event.h"
+#include "rx.h"
 
 static struct wl1271_partition_set part_table[PART_TABLE_LEN] = {
 	[PART_DOWN] = {
@@ -100,6 +101,22 @@ static void wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
 	wl1271_write32(wl, ACX_REG_ECPU_CONTROL, cpu_ctrl);
 }
 
+static void wl1271_parse_fw_ver(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = sscanf(wl->chip.fw_ver_str + 4, "%u.%u.%u.%u.%u",
+		     &wl->chip.fw_ver[0], &wl->chip.fw_ver[1],
+		     &wl->chip.fw_ver[2], &wl->chip.fw_ver[3],
+		     &wl->chip.fw_ver[4]);
+
+	if (ret != 5) {
+		wl1271_warning("fw version incorrect value");
+		memset(wl->chip.fw_ver, 0, sizeof(wl->chip.fw_ver));
+		return;
+	}
+}
+
 static void wl1271_boot_fw_version(struct wl1271 *wl)
 {
 	struct wl1271_static_data static_data;
@@ -107,11 +124,13 @@ static void wl1271_boot_fw_version(struct wl1271 *wl)
 	wl1271_read(wl, wl->cmd_box_addr, &static_data, sizeof(static_data),
 		    false);
 
-	strncpy(wl->chip.fw_ver, static_data.fw_version,
-		sizeof(wl->chip.fw_ver));
+	strncpy(wl->chip.fw_ver_str, static_data.fw_version,
+		sizeof(wl->chip.fw_ver_str));
 
 	/* make sure the string is NULL-terminated */
-	wl->chip.fw_ver[sizeof(wl->chip.fw_ver) - 1] = '\0';
+	wl->chip.fw_ver_str[sizeof(wl->chip.fw_ver_str) - 1] = '\0';
+
+	wl1271_parse_fw_ver(wl);
 }
 
 static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
@@ -231,7 +250,9 @@ static int wl1271_boot_upload_nvs(struct wl1271 *wl)
 	 */
 	if (wl->nvs_len == sizeof(struct wl1271_nvs_file) ||
 	    wl->nvs_len == WL1271_INI_LEGACY_NVS_FILE_SIZE) {
-		if (wl->nvs->general_params.dual_mode_select)
+		/* for now 11a is unsupported in AP mode */
+		if (wl->bss_type != BSS_TYPE_AP_BSS &&
+		    wl->nvs->general_params.dual_mode_select)
 			wl->enable_11a = true;
 	}
 
@@ -431,6 +452,9 @@ static int wl1271_boot_run_firmware(struct wl1271 *wl)
 		PSPOLL_DELIVERY_FAILURE_EVENT_ID |
 		SOFT_GEMINI_SENSE_EVENT_ID;
 
+	if (wl->bss_type == BSS_TYPE_AP_BSS)
+		wl->event_mask |= STA_REMOVE_COMPLETE_EVENT_ID;
+
 	ret = wl1271_event_unmask(wl);
 	if (ret < 0) {
 		wl1271_error("EVENT mask setting failed");
@@ -464,6 +488,9 @@ static void wl1271_boot_hw_version(struct wl1271 *wl)
 	fuse = (fuse & PG_VER_MASK) >> PG_VER_OFFSET;
 
 	wl->hw_pg_ver = (s8)fuse;
+
+	if (((wl->hw_pg_ver & PG_MAJOR_VER_MASK) >> PG_MAJOR_VER_OFFSET) < 3)
+		wl->quirks |= WL12XX_QUIRK_END_OF_TRANSACTION;
 }
 
 /* uploads NVS and firmware */
@@ -595,8 +622,7 @@ int wl1271_boot(struct wl1271 *wl)
 	wl1271_boot_enable_interrupts(wl);
 
 	/* set the wl1271 default filters */
-	wl->rx_config = WL1271_DEFAULT_RX_CONFIG;
-	wl->rx_filter = WL1271_DEFAULT_RX_FILTER;
+	wl1271_set_default_filters(wl);
 
 	wl1271_event_mbox_config(wl);
 
diff --git a/drivers/net/wireless/wl12xx/boot.h b/drivers/net/wireless/wl12xx/boot.h
index d67dcffa31eb..17229b86fc71 100644
--- a/drivers/net/wireless/wl12xx/boot.h
+++ b/drivers/net/wireless/wl12xx/boot.h
@@ -59,6 +59,11 @@ struct wl1271_static_data {
 #define PG_VER_MASK          0x3c
 #define PG_VER_OFFSET        2
 
+#define PG_MAJOR_VER_MASK    0x3
+#define PG_MAJOR_VER_OFFSET  0x0
+#define PG_MINOR_VER_MASK    0xc
+#define PG_MINOR_VER_OFFSET  0x2
+
 #define CMD_MBOX_ADDRESS     0x407B4
 
 #define POLARITY_LOW         BIT(1)
diff --git a/drivers/net/wireless/wl12xx/cmd.c b/drivers/net/wireless/wl12xx/cmd.c
index 0106628aa5a2..f0aa7ab97bf7 100644
--- a/drivers/net/wireless/wl12xx/cmd.c
+++ b/drivers/net/wireless/wl12xx/cmd.c
@@ -36,6 +36,7 @@
 #include "wl12xx_80211.h"
 #include "cmd.h"
 #include "event.h"
+#include "tx.h"
 
 #define WL1271_CMD_FAST_POLL_COUNT       50
 
@@ -62,6 +63,7 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 	cmd->status = 0;
 
 	WARN_ON(len % 4 != 0);
+	WARN_ON(test_bit(WL1271_FLAG_IN_ELP, &wl->flags));
 
 	wl1271_write(wl, wl->cmd_box_addr, buf, len, false);
 
@@ -221,7 +223,7 @@ int wl1271_cmd_ext_radio_parms(struct wl1271 *wl)
  * Poll the mailbox event field until any of the bits in the mask is set or a
  * timeout occurs (WL1271_EVENT_TIMEOUT in msecs)
  */
-static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
+static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl, u32 mask)
 {
 	u32 events_vector, event;
 	unsigned long timeout;
@@ -230,7 +232,8 @@ static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
 
 	do {
 		if (time_after(jiffies, timeout)) {
-			ieee80211_queue_work(wl->hw, &wl->recovery_work);
+			wl1271_debug(DEBUG_CMD, "timeout waiting for event %d",
+				     (int)mask);
 			return -ETIMEDOUT;
 		}
 
@@ -248,6 +251,19 @@ static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
 	return 0;
 }
 
+static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
+{
+	int ret;
+
+	ret = wl1271_cmd_wait_for_event_or_timeout(wl, mask);
+	if (ret != 0) {
+		ieee80211_queue_work(wl->hw, &wl->recovery_work);
+		return ret;
+	}
+
+	return 0;
+}
+
 int wl1271_cmd_join(struct wl1271 *wl, u8 bss_type)
 {
 	struct wl1271_cmd_join *join;
@@ -271,6 +287,7 @@ int wl1271_cmd_join(struct wl1271 *wl, u8 bss_type)
 	join->rx_filter_options = cpu_to_le32(wl->rx_filter);
 	join->bss_type = bss_type;
 	join->basic_rate_set = cpu_to_le32(wl->basic_rate_set);
+	join->supported_rate_set = cpu_to_le32(wl->rate_set);
 
 	if (wl->band == IEEE80211_BAND_5GHZ)
 		join->bss_type |= WL1271_JOIN_CMD_BSS_TYPE_5GHZ;
@@ -288,6 +305,9 @@ int wl1271_cmd_join(struct wl1271 *wl, u8 bss_type)
 	wl->tx_security_last_seq = 0;
 	wl->tx_security_seq = 0;
 
+	wl1271_debug(DEBUG_CMD, "cmd join: basic_rate_set=0x%x, rate_set=0x%x",
+		join->basic_rate_set, join->supported_rate_set);
+
 	ret = wl1271_cmd_send(wl, CMD_START_JOIN, join, sizeof(*join), 0);
 	if (ret < 0) {
 		wl1271_error("failed to initiate cmd join");
@@ -439,7 +459,7 @@ out:
 	return ret;
 }
 
-int wl1271_cmd_ps_mode(struct wl1271 *wl, u8 ps_mode, u32 rates, bool send)
+int wl1271_cmd_ps_mode(struct wl1271 *wl, u8 ps_mode)
 {
 	struct wl1271_cmd_ps_params *ps_params = NULL;
 	int ret = 0;
@@ -453,10 +473,6 @@ int wl1271_cmd_ps_mode(struct wl1271 *wl, u8 ps_mode, u32 rates, bool send)
 	}
 
 	ps_params->ps_mode = ps_mode;
-	ps_params->send_null_data = send;
-	ps_params->retries = wl->conf.conn.psm_entry_nullfunc_retries;
-	ps_params->hang_over_period = wl->conf.conn.psm_entry_hangover_period;
-	ps_params->null_data_rate = cpu_to_le32(rates);
 
 	ret = wl1271_cmd_send(wl, CMD_SET_PS_MODE, ps_params,
 			      sizeof(*ps_params), 0);
@@ -490,8 +506,8 @@ int wl1271_cmd_template_set(struct wl1271 *wl, u16 template_id,
 	cmd->len = cpu_to_le16(buf_len);
 	cmd->template_type = template_id;
 	cmd->enabled_rates = cpu_to_le32(rates);
-	cmd->short_retry_limit = wl->conf.tx.rc_conf.short_retry_limit;
-	cmd->long_retry_limit = wl->conf.tx.rc_conf.long_retry_limit;
+	cmd->short_retry_limit = wl->conf.tx.tmpl_short_retry_limit;
+	cmd->long_retry_limit = wl->conf.tx.tmpl_long_retry_limit;
 	cmd->index = index;
 
 	if (buf)
@@ -659,15 +675,15 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, __be32 ip_addr)
 
 	/* llc layer */
 	memcpy(tmpl.llc_hdr, rfc1042_header, sizeof(rfc1042_header));
-	tmpl.llc_type = htons(ETH_P_ARP);
+	tmpl.llc_type = cpu_to_be16(ETH_P_ARP);
 
 	/* arp header */
 	arp_hdr = &tmpl.arp_hdr;
-	arp_hdr->ar_hrd = htons(ARPHRD_ETHER);
-	arp_hdr->ar_pro = htons(ETH_P_IP);
+	arp_hdr->ar_hrd = cpu_to_be16(ARPHRD_ETHER);
+	arp_hdr->ar_pro = cpu_to_be16(ETH_P_IP);
 	arp_hdr->ar_hln = ETH_ALEN;
 	arp_hdr->ar_pln = 4;
-	arp_hdr->ar_op = htons(ARPOP_REPLY);
+	arp_hdr->ar_op = cpu_to_be16(ARPOP_REPLY);
 
 	/* arp payload */
 	memcpy(tmpl.sender_hw, wl->vif->addr, ETH_ALEN);
@@ -702,9 +718,9 @@ int wl1271_build_qos_null_data(struct wl1271 *wl)
 				       wl->basic_rate);
 }
 
-int wl1271_cmd_set_default_wep_key(struct wl1271 *wl, u8 id)
+int wl1271_cmd_set_sta_default_wep_key(struct wl1271 *wl, u8 id)
 {
-	struct wl1271_cmd_set_keys *cmd;
+	struct wl1271_cmd_set_sta_keys *cmd;
 	int ret = 0;
 
 	wl1271_debug(DEBUG_CMD, "cmd set_default_wep_key %d", id);
@@ -731,11 +747,42 @@ out:
 	return ret;
 }
 
-int wl1271_cmd_set_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
+int wl1271_cmd_set_ap_default_wep_key(struct wl1271 *wl, u8 id)
+{
+	struct wl1271_cmd_set_ap_keys *cmd;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_CMD, "cmd set_ap_default_wep_key %d", id);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	cmd->hlid = WL1271_AP_BROADCAST_HLID;
+	cmd->key_id = id;
+	cmd->lid_key_type = WEP_DEFAULT_LID_TYPE;
+	cmd->key_action = cpu_to_le16(KEY_SET_ID);
+	cmd->key_type = KEY_WEP;
+
+	ret = wl1271_cmd_send(wl, CMD_SET_KEYS, cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_warning("cmd set_ap_default_wep_key failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(cmd);
+
+	return ret;
+}
+
+int wl1271_cmd_set_sta_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
 		       u8 key_size, const u8 *key, const u8 *addr,
 		       u32 tx_seq_32, u16 tx_seq_16)
 {
-	struct wl1271_cmd_set_keys *cmd;
+	struct wl1271_cmd_set_sta_keys *cmd;
 	int ret = 0;
 
 	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
@@ -788,6 +835,67 @@ out:
 	return ret;
 }
 
+int wl1271_cmd_set_ap_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
+			u8 key_size, const u8 *key, u8 hlid, u32 tx_seq_32,
+			u16 tx_seq_16)
+{
+	struct wl1271_cmd_set_ap_keys *cmd;
+	int ret = 0;
+	u8 lid_type;
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	if (hlid == WL1271_AP_BROADCAST_HLID) {
+		if (key_type == KEY_WEP)
+			lid_type = WEP_DEFAULT_LID_TYPE;
+		else
+			lid_type = BROADCAST_LID_TYPE;
+	} else {
+		lid_type = UNICAST_LID_TYPE;
+	}
+
+	wl1271_debug(DEBUG_CRYPT, "ap key action: %d id: %d lid: %d type: %d"
+		     " hlid: %d", (int)action, (int)id, (int)lid_type,
+		     (int)key_type, (int)hlid);
+
+	cmd->lid_key_type = lid_type;
+	cmd->hlid = hlid;
+	cmd->key_action = cpu_to_le16(action);
+	cmd->key_size = key_size;
+	cmd->key_type = key_type;
+	cmd->key_id = id;
+	cmd->ac_seq_num16[0] = cpu_to_le16(tx_seq_16);
+	cmd->ac_seq_num32[0] = cpu_to_le32(tx_seq_32);
+
+	if (key_type == KEY_TKIP) {
+		/*
+		 * We get the key in the following form:
+		 * TKIP (16 bytes) - TX MIC (8 bytes) - RX MIC (8 bytes)
+		 * but the target is expecting:
+		 * TKIP - RX MIC - TX MIC
+		 */
+		memcpy(cmd->key, key, 16);
+		memcpy(cmd->key + 16, key + 24, 8);
+		memcpy(cmd->key + 24, key + 16, 8);
+	} else {
+		memcpy(cmd->key, key, key_size);
+	}
+
+	wl1271_dump(DEBUG_CRYPT, "TARGET AP KEY: ", cmd, sizeof(*cmd));
+
+	ret = wl1271_cmd_send(wl, CMD_SET_KEYS, cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_warning("could not set ap keys");
+		goto out;
+	}
+
+out:
+	kfree(cmd);
+	return ret;
+}
+
 int wl1271_cmd_disconnect(struct wl1271 *wl)
 {
 	struct wl1271_cmd_disconnect *cmd;
@@ -850,3 +958,180 @@ out_free:
 out:
 	return ret;
 }
+
+int wl1271_cmd_start_bss(struct wl1271 *wl)
+{
+	struct wl1271_cmd_bss_start *cmd;
+	struct ieee80211_bss_conf *bss_conf = &wl->vif->bss_conf;
+	int ret;
+
+	wl1271_debug(DEBUG_CMD, "cmd start bss");
+
+	/*
+	 * FIXME: We currently do not support hidden SSID. The real SSID
+	 * should be fetched from mac80211 first.
+	 */
+	if (wl->ssid_len == 0) {
+		wl1271_warning("Hidden SSID currently not supported for AP");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	memcpy(cmd->bssid, bss_conf->bssid, ETH_ALEN);
+
+	cmd->aging_period = cpu_to_le16(WL1271_AP_DEF_INACTIV_SEC);
+	cmd->bss_index = WL1271_AP_BSS_INDEX;
+	cmd->global_hlid = WL1271_AP_GLOBAL_HLID;
+	cmd->broadcast_hlid = WL1271_AP_BROADCAST_HLID;
+	cmd->basic_rate_set = cpu_to_le32(wl->basic_rate_set);
+	cmd->beacon_interval = cpu_to_le16(wl->beacon_int);
+	cmd->dtim_interval = bss_conf->dtim_period;
+	cmd->beacon_expiry = WL1271_AP_DEF_BEACON_EXP;
+	cmd->channel = wl->channel;
+	cmd->ssid_len = wl->ssid_len;
+	cmd->ssid_type = SSID_TYPE_PUBLIC;
+	memcpy(cmd->ssid, wl->ssid, wl->ssid_len);
+
+	switch (wl->band) {
+	case IEEE80211_BAND_2GHZ:
+		cmd->band = RADIO_BAND_2_4GHZ;
+		break;
+	case IEEE80211_BAND_5GHZ:
+		cmd->band = RADIO_BAND_5GHZ;
+		break;
+	default:
+		wl1271_warning("bss start - unknown band: %d", (int)wl->band);
+		cmd->band = RADIO_BAND_2_4GHZ;
+		break;
+	}
+
+	ret = wl1271_cmd_send(wl, CMD_BSS_START, cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to initiate cmd start bss");
+		goto out_free;
+	}
+
+out_free:
+	kfree(cmd);
+
+out:
+	return ret;
+}
+
+int wl1271_cmd_stop_bss(struct wl1271 *wl)
+{
+	struct wl1271_cmd_bss_start *cmd;
+	int ret;
+
+	wl1271_debug(DEBUG_CMD, "cmd stop bss");
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	cmd->bss_index = WL1271_AP_BSS_INDEX;
+
+	ret = wl1271_cmd_send(wl, CMD_BSS_STOP, cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to initiate cmd stop bss");
+		goto out_free;
+	}
+
+out_free:
+	kfree(cmd);
+
+out:
+	return ret;
+}
+
+int wl1271_cmd_add_sta(struct wl1271 *wl, struct ieee80211_sta *sta, u8 hlid)
+{
+	struct wl1271_cmd_add_sta *cmd;
+	int ret;
+
+	wl1271_debug(DEBUG_CMD, "cmd add sta %d", (int)hlid);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* currently we don't support UAPSD */
+	cmd->sp_len = 0;
+
+	memcpy(cmd->addr, sta->addr, ETH_ALEN);
+	cmd->bss_index = WL1271_AP_BSS_INDEX;
+	cmd->aid = sta->aid;
+	cmd->hlid = hlid;
+
+	/*
+	 * FIXME: Does STA support QOS? We need to propagate this info from
+	 * hostapd. Currently not that important since this is only used for
+	 * sending the correct flavor of null-data packet in response to a
+	 * trigger.
+	 */
+	cmd->wmm = 0;
+
+	cmd->supported_rates = cpu_to_le32(wl1271_tx_enabled_rates_get(wl,
+						sta->supp_rates[wl->band]));
+
+	wl1271_debug(DEBUG_CMD, "new sta rates: 0x%x", cmd->supported_rates);
+
+	ret = wl1271_cmd_send(wl, CMD_ADD_STA, cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to initiate cmd add sta");
+		goto out_free;
+	}
+
+out_free:
+	kfree(cmd);
+
+out:
+	return ret;
+}
+
+int wl1271_cmd_remove_sta(struct wl1271 *wl, u8 hlid)
+{
+	struct wl1271_cmd_remove_sta *cmd;
+	int ret;
+
+	wl1271_debug(DEBUG_CMD, "cmd remove sta %d", (int)hlid);
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	cmd->hlid = hlid;
+	/* We never send a deauth, mac80211 is in charge of this */
+	cmd->reason_opcode = 0;
+	cmd->send_deauth_flag = 0;
+
+	ret = wl1271_cmd_send(wl, CMD_REMOVE_STA, cmd, sizeof(*cmd), 0);
+	if (ret < 0) {
+		wl1271_error("failed to initiate cmd remove sta");
+		goto out_free;
+	}
+
+	/*
+	 * We are ok with a timeout here. The event is sometimes not sent
+	 * due to a firmware bug.
+	 */
+	wl1271_cmd_wait_for_event_or_timeout(wl, STA_REMOVE_COMPLETE_EVENT_ID);
+
+out_free:
+	kfree(cmd);
+
+out:
+	return ret;
+}
diff --git a/drivers/net/wireless/wl12xx/cmd.h b/drivers/net/wireless/wl12xx/cmd.h
index 2a1d9db7ceb8..54c12e71417e 100644
--- a/drivers/net/wireless/wl12xx/cmd.h
+++ b/drivers/net/wireless/wl12xx/cmd.h
@@ -39,7 +39,7 @@ int wl1271_cmd_test(struct wl1271 *wl, void *buf, size_t buf_len, u8 answer);
 int wl1271_cmd_interrogate(struct wl1271 *wl, u16 id, void *buf, size_t len);
 int wl1271_cmd_configure(struct wl1271 *wl, u16 id, void *buf, size_t len);
 int wl1271_cmd_data_path(struct wl1271 *wl, bool enable);
-int wl1271_cmd_ps_mode(struct wl1271 *wl, u8 ps_mode, u32 rates, bool send);
+int wl1271_cmd_ps_mode(struct wl1271 *wl, u8 ps_mode);
 int wl1271_cmd_read_memory(struct wl1271 *wl, u32 addr, void *answer,
 			   size_t len);
 int wl1271_cmd_template_set(struct wl1271 *wl, u16 template_id,
@@ -54,12 +54,20 @@ struct sk_buff *wl1271_cmd_build_ap_probe_req(struct wl1271 *wl,
 int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, __be32 ip_addr);
 int wl1271_build_qos_null_data(struct wl1271 *wl);
 int wl1271_cmd_build_klv_null_data(struct wl1271 *wl);
-int wl1271_cmd_set_default_wep_key(struct wl1271 *wl, u8 id);
-int wl1271_cmd_set_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
-		       u8 key_size, const u8 *key, const u8 *addr,
-		       u32 tx_seq_32, u16 tx_seq_16);
+int wl1271_cmd_set_sta_default_wep_key(struct wl1271 *wl, u8 id);
+int wl1271_cmd_set_ap_default_wep_key(struct wl1271 *wl, u8 id);
+int wl1271_cmd_set_sta_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
+			   u8 key_size, const u8 *key, const u8 *addr,
+			   u32 tx_seq_32, u16 tx_seq_16);
+int wl1271_cmd_set_ap_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
+			  u8 key_size, const u8 *key, u8 hlid, u32 tx_seq_32,
+			  u16 tx_seq_16);
 int wl1271_cmd_disconnect(struct wl1271 *wl);
 int wl1271_cmd_set_sta_state(struct wl1271 *wl);
+int wl1271_cmd_start_bss(struct wl1271 *wl);
+int wl1271_cmd_stop_bss(struct wl1271 *wl);
+int wl1271_cmd_add_sta(struct wl1271 *wl, struct ieee80211_sta *sta, u8 hlid);
+int wl1271_cmd_remove_sta(struct wl1271 *wl, u8 hlid);
 
 enum wl1271_commands {
 	CMD_INTERROGATE     = 1,    /*use this to read information elements*/
@@ -98,6 +106,12 @@ enum wl1271_commands {
 	CMD_STOP_PERIODIC_SCAN       = 51,
 	CMD_SET_STA_STATE            = 52,
 
+	/* AP mode commands */
+	CMD_BSS_START                = 60,
+	CMD_BSS_STOP                 = 61,
+	CMD_ADD_STA                  = 62,
+	CMD_REMOVE_STA               = 63,
+
 	NUM_COMMANDS,
 	MAX_COMMAND_ID = 0xFFFF,
 };
@@ -126,6 +140,14 @@ enum cmd_templ {
 				  * For CTS-to-self (FastCTS) mechanism
 				  * for BT/WLAN coexistence (SoftGemini). */
 	CMD_TEMPL_ARP_RSP,
+	CMD_TEMPL_LINK_MEASUREMENT_REPORT,
+
+	/* AP-mode specific */
+	CMD_TEMPL_AP_BEACON = 13,
+	CMD_TEMPL_AP_PROBE_RESPONSE,
+	CMD_TEMPL_AP_ARP_RSP,
+	CMD_TEMPL_DEAUTH_AP,
+
 	CMD_TEMPL_MAX = 0xff
 };
 
@@ -195,6 +217,7 @@ struct wl1271_cmd_join {
 	 * ACK or CTS frames).
 	 */
 	__le32 basic_rate_set;
+	__le32 supported_rate_set;
 	u8 dtim_interval;
 	/*
 	 * bits 0-2: This bitwise field specifies the type
@@ -257,20 +280,11 @@ struct wl1271_cmd_ps_params {
 	struct wl1271_cmd_header header;
 
 	u8 ps_mode; /* STATION_* */
-	u8 send_null_data; /* Do we have to send NULL data packet ? */
-	u8 retries; /* Number of retires for the initial NULL data packet */
-
-	 /*
-	  * TUs during which the target stays awake after switching
-	  * to power save mode.
-	  */
-	u8 hang_over_period;
-	__le32 null_data_rate;
+	u8 padding[3];
 } __packed;
 
 /* HW encryption keys */
 #define NUM_ACCESS_CATEGORIES_COPY 4
-#define MAX_KEY_SIZE 32
 
 enum wl1271_cmd_key_action {
 	KEY_ADD_OR_REPLACE = 1,
@@ -289,7 +303,7 @@ enum wl1271_cmd_key_type {
 
 /* FIXME: Add description for key-types */
 
-struct wl1271_cmd_set_keys {
+struct wl1271_cmd_set_sta_keys {
 	struct wl1271_cmd_header header;
 
 	/* Ignored for default WEP key */
@@ -318,6 +332,57 @@ struct wl1271_cmd_set_keys {
 	__le32 ac_seq_num32[NUM_ACCESS_CATEGORIES_COPY];
 } __packed;
 
+enum wl1271_cmd_lid_key_type {
+	UNICAST_LID_TYPE     = 0,
+	BROADCAST_LID_TYPE   = 1,
+	WEP_DEFAULT_LID_TYPE = 2
+};
+
+struct wl1271_cmd_set_ap_keys {
+	struct wl1271_cmd_header header;
+
+	/*
+	 * Indicates whether the HLID is a unicast key set
+	 * or broadcast key set. A special value 0xFF is
+	 * used to indicate that the HLID is on WEP-default
+	 * (multi-hlids). of type wl1271_cmd_lid_key_type.
+	 */
+	u8 hlid;
+
+	/*
+	 * In WEP-default network (hlid == 0xFF) used to
+	 * indicate which network STA/IBSS/AP role should be
+	 * changed
+	 */
+	u8 lid_key_type;
+
+	/*
+	 * Key ID - For TKIP and AES key types, this field
+	 * indicates the value that should be inserted into
+	 * the KeyID field of frames transmitted using this
+	 * key entry. For broadcast keys the index use as a
+	 * marker for TX/RX key.
+	 * For WEP default network (HLID=0xFF), this field
+	 * indicates the ID of the key to add or remove.
+	 */
+	u8 key_id;
+	u8 reserved_1;
+
+	/* key_action_e */
+	__le16 key_action;
+
+	/* key size in bytes */
+	u8 key_size;
+
+	/* key_type_e */
+	u8 key_type;
+
+	/* This field holds the security key data to add to the STA table */
+	u8 key[MAX_KEY_SIZE];
+	__le16 ac_seq_num16[NUM_ACCESS_CATEGORIES_COPY];
+	__le32 ac_seq_num32[NUM_ACCESS_CATEGORIES_COPY];
+} __packed;
+
 struct wl1271_cmd_test_header {
 	u8 id;
 	u8 padding[3];
@@ -412,4 +477,68 @@ struct wl1271_cmd_set_sta_state {
 	u8 padding[3];
 } __packed;
 
+enum wl1271_ssid_type {
+	SSID_TYPE_PUBLIC = 0,
+	SSID_TYPE_HIDDEN = 1
+};
+
+struct wl1271_cmd_bss_start {
+	struct wl1271_cmd_header header;
+
+	/* wl1271_ssid_type */
+	u8 ssid_type;
+	u8 ssid_len;
+	u8 ssid[IW_ESSID_MAX_SIZE];
+	u8 padding_1[2];
+
+	/* Basic rate set */
+	__le32 basic_rate_set;
+	/* Aging period in seconds*/
+	__le16 aging_period;
+
+	/*
+	 * This field specifies the time between target beacon
+	 * transmission times (TBTTs), in time units (TUs).
+	 * Valid values are 1 to 1024.
+	 */
+	__le16 beacon_interval;
+	u8 bssid[ETH_ALEN];
+	u8 bss_index;
+	/* Radio band */
+	u8 band;
+	u8 channel;
+	/* The host link id for the AP's global queue */
+	u8 global_hlid;
+	/* The host link id for the AP's broadcast queue */
+	u8 broadcast_hlid;
+	/* DTIM count */
+	u8 dtim_interval;
+	/* Beacon expiry time in ms */
+	u8 beacon_expiry;
+	u8 padding_2[3];
+} __packed;
+
+struct wl1271_cmd_add_sta {
+	struct wl1271_cmd_header header;
+
+	u8 addr[ETH_ALEN];
+	u8 hlid;
+	u8 aid;
+	u8 psd_type[NUM_ACCESS_CATEGORIES_COPY];
+	__le32 supported_rates;
+	u8 bss_index;
+	u8 sp_len;
+	u8 wmm;
+	u8 padding1;
+} __packed;
+
+struct wl1271_cmd_remove_sta {
+	struct wl1271_cmd_header header;
+
+	u8 hlid;
+	u8 reason_opcode;
+	u8 send_deauth_flag;
+	u8 padding1;
+} __packed;
+
 #endif /* __WL1271_CMD_H__ */
diff --git a/drivers/net/wireless/wl12xx/conf.h b/drivers/net/wireless/wl12xx/conf.h
index a16b3616e430..856a8a2fff4f 100644
--- a/drivers/net/wireless/wl12xx/conf.h
+++ b/drivers/net/wireless/wl12xx/conf.h
@@ -496,6 +496,26 @@ struct conf_rx_settings {
 					CONF_HW_BIT_RATE_2MBPS)
 #define CONF_TX_RATE_RETRY_LIMIT       10
 
+/*
+ * Rates supported for data packets when operating as AP. Note the absense
+ * of the 22Mbps rate. There is a FW limitation on 12 rates so we must drop
+ * one. The rate dropped is not mandatory under any operating mode.
+ */
+#define CONF_TX_AP_ENABLED_RATES       (CONF_HW_BIT_RATE_1MBPS | \
+	CONF_HW_BIT_RATE_2MBPS | CONF_HW_BIT_RATE_5_5MBPS |      \
+	CONF_HW_BIT_RATE_6MBPS | CONF_HW_BIT_RATE_9MBPS |        \
+	CONF_HW_BIT_RATE_11MBPS | CONF_HW_BIT_RATE_12MBPS |      \
+	CONF_HW_BIT_RATE_18MBPS | CONF_HW_BIT_RATE_24MBPS |      \
+	CONF_HW_BIT_RATE_36MBPS | CONF_HW_BIT_RATE_48MBPS |      \
+	CONF_HW_BIT_RATE_54MBPS)
+
+/*
+ * Default rates for management traffic when operating in AP mode. This
+ * should be configured according to the basic rate set of the AP
+ */
+#define CONF_TX_AP_DEFAULT_MGMT_RATES  (CONF_HW_BIT_RATE_1MBPS | \
+	CONF_HW_BIT_RATE_2MBPS | CONF_HW_BIT_RATE_5_5MBPS)
+
 struct conf_tx_rate_class {
 
 	/*
@@ -636,9 +656,9 @@ struct conf_tx_settings {
 
 	/*
 	 * Configuration for rate classes for TX (currently only one
-	 * rate class supported.)
+	 * rate class supported). Used in non-AP mode.
 	 */
-	struct conf_tx_rate_class rc_conf;
+	struct conf_tx_rate_class sta_rc_conf;
 
 	/*
 	 * Configuration for access categories for TX rate control.
@@ -647,6 +667,28 @@ struct conf_tx_settings {
 	struct conf_tx_ac_category ac_conf[CONF_TX_MAX_AC_COUNT];
 
 	/*
+	 * Configuration for rate classes in AP-mode. These rate classes
+	 * are for the AC TX queues
+	 */
+	struct conf_tx_rate_class ap_rc_conf[CONF_TX_MAX_AC_COUNT];
+
+	/*
+	 * Management TX rate class for AP-mode.
+	 */
+	struct conf_tx_rate_class ap_mgmt_conf;
+
+	/*
+	 * Broadcast TX rate class for AP-mode.
+	 */
+	struct conf_tx_rate_class ap_bcst_conf;
+
+	/*
+	 * AP-mode - allow this number of TX retries to a station before an
+	 * event is triggered from FW.
+	 */
+	u16 ap_max_tx_retries;
+
+	/*
 	 * Configuration for TID parameters.
 	 */
 	u8 tid_conf_count;
@@ -687,6 +729,12 @@ struct conf_tx_settings {
 	 * Range: CONF_HW_BIT_RATE_* bit mask
 	 */
 	u32 basic_rate_5;
+
+	/*
+	 * TX retry limits for templates
+	 */
+	u8 tmpl_short_retry_limit;
+	u8 tmpl_long_retry_limit;
 };
 
 enum {
@@ -912,6 +960,14 @@ struct conf_conn_settings {
 	u8 psm_entry_retries;
 
 	/*
+	 * Specifies the maximum number of times to try PSM exit if it fails
+	 * (if sending the appropriate null-func message fails.)
+	 *
+	 * Range 0 - 255
+	 */
+	u8 psm_exit_retries;
+
+	/*
 	 * Specifies the maximum number of times to try transmit the PSM entry
 	 * null-func frame for each PSM entry attempt
 	 *
@@ -1036,30 +1092,30 @@ struct conf_scan_settings {
 	/*
 	 * The minimum time to wait on each channel for active scans
 	 *
-	 * Range: 0 - 65536 tu
+	 * Range: u32 tu/1000
 	 */
-	u16 min_dwell_time_active;
+	u32 min_dwell_time_active;
 
 	/*
 	 * The maximum time to wait on each channel for active scans
 	 *
-	 * Range: 0 - 65536 tu
+	 * Range: u32 tu/1000
 	 */
-	u16 max_dwell_time_active;
+	u32 max_dwell_time_active;
 
 	/*
-	 * The maximum time to wait on each channel for passive scans
+	 * The minimum time to wait on each channel for passive scans
 	 *
-	 * Range: 0 - 65536 tu
+	 * Range: u32 tu/1000
 	 */
-	u16 min_dwell_time_passive;
+	u32 min_dwell_time_passive;
 
 	/*
 	 * The maximum time to wait on each channel for passive scans
 	 *
-	 * Range: 0 - 65536 tu
+	 * Range: u32 tu/1000
 	 */
-	u16 max_dwell_time_passive;
+	u32 max_dwell_time_passive;
 
 	/*
 	 * Number of probe requests to transmit on each active scan channel
@@ -1090,6 +1146,51 @@ struct conf_rf_settings {
 	u8 tx_per_channel_power_compensation_5[CONF_TX_PWR_COMPENSATION_LEN_5];
 };
 
+struct conf_ht_setting {
+	u16 tx_ba_win_size;
+	u16 inactivity_timeout;
+};
+
+struct conf_memory_settings {
+	/* Number of stations supported in IBSS mode */
+	u8 num_stations;
+
+	/* Number of ssid profiles used in IBSS mode */
+	u8 ssid_profiles;
+
+	/* Number of memory buffers allocated to rx pool */
+	u8 rx_block_num;
+
+	/* Minimum number of blocks allocated to tx pool */
+	u8 tx_min_block_num;
+
+	/* Disable/Enable dynamic memory */
+	u8 dynamic_memory;
+
+	/*
+	 * Minimum required free tx memory blocks in order to assure optimum
+	 * performence
+	 *
+	 * Range: 0-120
+	 */
+	u8 min_req_tx_blocks;
+
+	/*
+	 * Minimum required free rx memory blocks in order to assure optimum
+	 * performence
+	 *
+	 * Range: 0-120
+	 */
+	u8 min_req_rx_blocks;
+
+	/*
+	 * Minimum number of mem blocks (free+used) guaranteed for TX
+	 *
+	 * Range: 0-120
+	 */
+	u8 tx_min;
+};
+
 struct conf_drv_settings {
 	struct conf_sg_settings sg;
 	struct conf_rx_settings rx;
@@ -1100,6 +1201,8 @@ struct conf_drv_settings {
 	struct conf_roam_trigger_settings roam_trigger;
 	struct conf_scan_settings scan;
 	struct conf_rf_settings rf;
+	struct conf_ht_setting ht;
+	struct conf_memory_settings mem;
 };
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/debugfs.c b/drivers/net/wireless/wl12xx/debugfs.c
index ec6077760157..8e75b09723b9 100644
--- a/drivers/net/wireless/wl12xx/debugfs.c
+++ b/drivers/net/wireless/wl12xx/debugfs.c
@@ -99,7 +99,7 @@ static void wl1271_debugfs_update_stats(struct wl1271 *wl)
 
 	mutex_lock(&wl->mutex);
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -261,27 +261,25 @@ static ssize_t gpio_power_write(struct file *file,
 	unsigned long value;
 	int ret;
 
-	mutex_lock(&wl->mutex);
-
 	len = min(count, sizeof(buf) - 1);
 	if (copy_from_user(buf, user_buf, len)) {
-		ret = -EFAULT;
-		goto out;
+		return -EFAULT;
 	}
 	buf[len] = '\0';
 
 	ret = strict_strtoul(buf, 0, &value);
 	if (ret < 0) {
 		wl1271_warning("illegal value in gpio_power");
-		goto out;
+		return -EINVAL;
 	}
 
+	mutex_lock(&wl->mutex);
+
 	if (value)
 		wl1271_power_on(wl);
 	else
 		wl1271_power_off(wl);
 
-out:
 	mutex_unlock(&wl->mutex);
 	return count;
 }
@@ -293,12 +291,13 @@ static const struct file_operations gpio_power_ops = {
 	.llseek = default_llseek,
 };
 
-static int wl1271_debugfs_add_files(struct wl1271 *wl)
+static int wl1271_debugfs_add_files(struct wl1271 *wl,
+				     struct dentry *rootdir)
 {
 	int ret = 0;
 	struct dentry *entry, *stats;
 
-	stats = debugfs_create_dir("fw-statistics", wl->rootdir);
+	stats = debugfs_create_dir("fw-statistics", rootdir);
 	if (!stats || IS_ERR(stats)) {
 		entry = stats;
 		goto err;
@@ -395,16 +394,11 @@ static int wl1271_debugfs_add_files(struct wl1271 *wl)
 	DEBUGFS_FWSTATS_ADD(rxpipe, missed_beacon_host_int_trig_rx_data);
 	DEBUGFS_FWSTATS_ADD(rxpipe, tx_xfr_host_int_trig_rx_data);
 
-	DEBUGFS_ADD(tx_queue_len, wl->rootdir);
-	DEBUGFS_ADD(retry_count, wl->rootdir);
-	DEBUGFS_ADD(excessive_retries, wl->rootdir);
-
-	DEBUGFS_ADD(gpio_power, wl->rootdir);
+	DEBUGFS_ADD(tx_queue_len, rootdir);
+	DEBUGFS_ADD(retry_count, rootdir);
+	DEBUGFS_ADD(excessive_retries, rootdir);
 
-	entry = debugfs_create_x32("debug_level", 0600, wl->rootdir,
-				   &wl12xx_debug_level);
-	if (!entry || IS_ERR(entry))
-		goto err;
+	DEBUGFS_ADD(gpio_power, rootdir);
 
 	return 0;
 
@@ -419,7 +413,7 @@ err:
 
 void wl1271_debugfs_reset(struct wl1271 *wl)
 {
-	if (!wl->rootdir)
+	if (!wl->stats.fw_stats)
 		return;
 
 	memset(wl->stats.fw_stats, 0, sizeof(*wl->stats.fw_stats));
@@ -430,13 +424,13 @@ void wl1271_debugfs_reset(struct wl1271 *wl)
 int wl1271_debugfs_init(struct wl1271 *wl)
 {
 	int ret;
+	struct dentry *rootdir;
 
-	wl->rootdir = debugfs_create_dir(KBUILD_MODNAME,
-					 wl->hw->wiphy->debugfsdir);
+	rootdir = debugfs_create_dir(KBUILD_MODNAME,
+				     wl->hw->wiphy->debugfsdir);
 
-	if (IS_ERR(wl->rootdir)) {
-		ret = PTR_ERR(wl->rootdir);
-		wl->rootdir = NULL;
+	if (IS_ERR(rootdir)) {
+		ret = PTR_ERR(rootdir);
 		goto err;
 	}
 
@@ -450,7 +444,7 @@ int wl1271_debugfs_init(struct wl1271 *wl)
 
 	wl->stats.fw_stats_update = jiffies;
 
-	ret = wl1271_debugfs_add_files(wl);
+	ret = wl1271_debugfs_add_files(wl, rootdir);
 
 	if (ret < 0)
 		goto err_file;
@@ -462,8 +456,7 @@ err_file:
 	wl->stats.fw_stats = NULL;
 
 err_fw:
-	debugfs_remove_recursive(wl->rootdir);
-	wl->rootdir = NULL;
+	debugfs_remove_recursive(rootdir);
 
 err:
 	return ret;
@@ -473,8 +466,4 @@ void wl1271_debugfs_exit(struct wl1271 *wl)
 {
 	kfree(wl->stats.fw_stats);
 	wl->stats.fw_stats = NULL;
-
-	debugfs_remove_recursive(wl->rootdir);
-	wl->rootdir = NULL;
-
 }
diff --git a/drivers/net/wireless/wl12xx/event.c b/drivers/net/wireless/wl12xx/event.c
index f9146f5242fb..1b170c5cc595 100644
--- a/drivers/net/wireless/wl12xx/event.c
+++ b/drivers/net/wireless/wl12xx/event.c
@@ -135,20 +135,6 @@ static int wl1271_event_ps_report(struct wl1271 *wl,
 		/* go to extremely low power mode */
 		wl1271_ps_elp_sleep(wl);
 		break;
-	case EVENT_EXIT_POWER_SAVE_FAIL:
-		wl1271_debug(DEBUG_PSM, "PSM exit failed");
-
-		if (test_bit(WL1271_FLAG_PSM, &wl->flags)) {
-			wl->psm_entry_retry = 0;
-			break;
-		}
-
-		/* make sure the firmware goes to active mode - the frame to
-		   be sent next will indicate to the AP, that we are active. */
-		ret = wl1271_ps_set_mode(wl, STATION_ACTIVE_MODE,
-					 wl->basic_rate, false);
-		break;
-	case EVENT_EXIT_POWER_SAVE_SUCCESS:
 	default:
 		break;
 	}
@@ -186,6 +172,7 @@ static int wl1271_event_process(struct wl1271 *wl, struct event_mailbox *mbox)
 	int ret;
 	u32 vector;
 	bool beacon_loss = false;
+	bool is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
 
 	wl1271_event_mbox_dump(mbox);
 
@@ -218,21 +205,21 @@ static int wl1271_event_process(struct wl1271 *wl, struct event_mailbox *mbox)
 	 * BSS_LOSE_EVENT, beacon loss has to be reported to the stack.
 	 *
 	 */
-	if (vector & BSS_LOSE_EVENT_ID) {
+	if ((vector & BSS_LOSE_EVENT_ID) && !is_ap) {
 		wl1271_info("Beacon loss detected.");
 
 		/* indicate to the stack, that beacons have been lost */
 		beacon_loss = true;
 	}
 
-	if (vector & PS_REPORT_EVENT_ID) {
+	if ((vector & PS_REPORT_EVENT_ID) && !is_ap) {
 		wl1271_debug(DEBUG_EVENT, "PS_REPORT_EVENT");
 		ret = wl1271_event_ps_report(wl, mbox, &beacon_loss);
 		if (ret < 0)
 			return ret;
 	}
 
-	if (vector & PSPOLL_DELIVERY_FAILURE_EVENT_ID)
+	if ((vector & PSPOLL_DELIVERY_FAILURE_EVENT_ID) && !is_ap)
 		wl1271_event_pspoll_delivery_fail(wl);
 
 	if (vector & RSSI_SNR_TRIGGER_0_EVENT_ID) {
diff --git a/drivers/net/wireless/wl12xx/event.h b/drivers/net/wireless/wl12xx/event.h
index 6cce0143adb5..0e80886f3031 100644
--- a/drivers/net/wireless/wl12xx/event.h
+++ b/drivers/net/wireless/wl12xx/event.h
@@ -59,6 +59,7 @@ enum {
 	BSS_LOSE_EVENT_ID			 = BIT(18),
 	REGAINED_BSS_EVENT_ID			 = BIT(19),
 	ROAMING_TRIGGER_MAX_TX_RETRY_EVENT_ID	 = BIT(20),
+	STA_REMOVE_COMPLETE_EVENT_ID		 = BIT(21), /* AP */
 	SOFT_GEMINI_SENSE_EVENT_ID		 = BIT(22),
 	SOFT_GEMINI_PREDICTION_EVENT_ID		 = BIT(23),
 	SOFT_GEMINI_AVALANCHE_EVENT_ID		 = BIT(24),
@@ -74,8 +75,6 @@ enum {
 enum {
 	EVENT_ENTER_POWER_SAVE_FAIL = 0,
 	EVENT_ENTER_POWER_SAVE_SUCCESS,
-	EVENT_EXIT_POWER_SAVE_FAIL,
-	EVENT_EXIT_POWER_SAVE_SUCCESS,
 };
 
 struct event_debug_report {
@@ -115,7 +114,12 @@ struct event_mailbox {
 	u8 scheduled_scan_status;
 	u8 ps_status;
 
-	u8 reserved_5[29];
+	/* AP FW only */
+	u8 hlid_removed;
+	__le16 sta_aging_status;
+	__le16 sta_tx_retry_exceeded;
+
+	u8 reserved_5[24];
 } __packed;
 
 int wl1271_event_unmask(struct wl1271 *wl);
diff --git a/drivers/net/wireless/wl12xx/init.c b/drivers/net/wireless/wl12xx/init.c
index 785a5304bfc4..6072fe457135 100644
--- a/drivers/net/wireless/wl12xx/init.c
+++ b/drivers/net/wireless/wl12xx/init.c
@@ -30,27 +30,9 @@
 #include "acx.h"
 #include "cmd.h"
 #include "reg.h"
+#include "tx.h"
 
-static int wl1271_init_hwenc_config(struct wl1271 *wl)
-{
-	int ret;
-
-	ret = wl1271_acx_feature_cfg(wl);
-	if (ret < 0) {
-		wl1271_warning("couldn't set feature config");
-		return ret;
-	}
-
-	ret = wl1271_cmd_set_default_wep_key(wl, wl->default_key);
-	if (ret < 0) {
-		wl1271_warning("couldn't set default key");
-		return ret;
-	}
-
-	return 0;
-}
-
-int wl1271_init_templates_config(struct wl1271 *wl)
+int wl1271_sta_init_templates_config(struct wl1271 *wl)
 {
 	int ret, i;
 
@@ -118,6 +100,132 @@ int wl1271_init_templates_config(struct wl1271 *wl)
 	return 0;
 }
 
+static int wl1271_ap_init_deauth_template(struct wl1271 *wl)
+{
+	struct wl12xx_disconn_template *tmpl;
+	int ret;
+
+	tmpl = kzalloc(sizeof(*tmpl), GFP_KERNEL);
+	if (!tmpl) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	tmpl->header.frame_ctl = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					     IEEE80211_STYPE_DEAUTH);
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_DEAUTH_AP,
+				      tmpl, sizeof(*tmpl), 0,
+				      wl1271_tx_min_rate_get(wl));
+
+out:
+	kfree(tmpl);
+	return ret;
+}
+
+static int wl1271_ap_init_null_template(struct wl1271 *wl)
+{
+	struct ieee80211_hdr_3addr *nullfunc;
+	int ret;
+
+	nullfunc = kzalloc(sizeof(*nullfunc), GFP_KERNEL);
+	if (!nullfunc) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	nullfunc->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					      IEEE80211_STYPE_NULLFUNC |
+					      IEEE80211_FCTL_FROMDS);
+
+	/* nullfunc->addr1 is filled by FW */
+
+	memcpy(nullfunc->addr2, wl->mac_addr, ETH_ALEN);
+	memcpy(nullfunc->addr3, wl->mac_addr, ETH_ALEN);
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_NULL_DATA, nullfunc,
+				      sizeof(*nullfunc), 0,
+				      wl1271_tx_min_rate_get(wl));
+
+out:
+	kfree(nullfunc);
+	return ret;
+}
+
+static int wl1271_ap_init_qos_null_template(struct wl1271 *wl)
+{
+	struct ieee80211_qos_hdr *qosnull;
+	int ret;
+
+	qosnull = kzalloc(sizeof(*qosnull), GFP_KERNEL);
+	if (!qosnull) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	qosnull->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					     IEEE80211_STYPE_QOS_NULLFUNC |
+					     IEEE80211_FCTL_FROMDS);
+
+	/* qosnull->addr1 is filled by FW */
+
+	memcpy(qosnull->addr2, wl->mac_addr, ETH_ALEN);
+	memcpy(qosnull->addr3, wl->mac_addr, ETH_ALEN);
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_QOS_NULL_DATA, qosnull,
+				      sizeof(*qosnull), 0,
+				      wl1271_tx_min_rate_get(wl));
+
+out:
+	kfree(qosnull);
+	return ret;
+}
+
+static int wl1271_ap_init_templates_config(struct wl1271 *wl)
+{
+	int ret;
+
+	/*
+	 * Put very large empty placeholders for all templates. These
+	 * reserve memory for later.
+	 */
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_AP_PROBE_RESPONSE, NULL,
+				      sizeof
+				      (struct wl12xx_probe_resp_template),
+				      0, WL1271_RATE_AUTOMATIC);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_AP_BEACON, NULL,
+				      sizeof
+				      (struct wl12xx_beacon_template),
+				      0, WL1271_RATE_AUTOMATIC);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_DEAUTH_AP, NULL,
+				      sizeof
+				      (struct wl12xx_disconn_template),
+				      0, WL1271_RATE_AUTOMATIC);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_NULL_DATA, NULL,
+				      sizeof(struct wl12xx_null_data_template),
+				      0, WL1271_RATE_AUTOMATIC);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_cmd_template_set(wl, CMD_TEMPL_QOS_NULL_DATA, NULL,
+				      sizeof
+				      (struct wl12xx_qos_null_data_template),
+				      0, WL1271_RATE_AUTOMATIC);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
 static int wl1271_init_rx_config(struct wl1271 *wl, u32 config, u32 filter)
 {
 	int ret;
@@ -145,10 +253,6 @@ int wl1271_init_phy_config(struct wl1271 *wl)
 	if (ret < 0)
 		return ret;
 
-	ret = wl1271_acx_group_address_tbl(wl, true, NULL, 0);
-	if (ret < 0)
-		return ret;
-
 	ret = wl1271_acx_service_period_timeout(wl);
 	if (ret < 0)
 		return ret;
@@ -213,11 +317,199 @@ static int wl1271_init_beacon_broadcast(struct wl1271 *wl)
 	return 0;
 }
 
+static int wl1271_sta_hw_init(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wl1271_cmd_ext_radio_parms(wl);
+	if (ret < 0)
+		return ret;
+
+	/* PS config */
+	ret = wl1271_acx_config_ps(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_sta_init_templates_config(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_acx_group_address_tbl(wl, true, NULL, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Initialize connection monitoring thresholds */
+	ret = wl1271_acx_conn_monit_params(wl, false);
+	if (ret < 0)
+		return ret;
+
+	/* Beacon filtering */
+	ret = wl1271_init_beacon_filter(wl);
+	if (ret < 0)
+		return ret;
+
+	/* Bluetooth WLAN coexistence */
+	ret = wl1271_init_pta(wl);
+	if (ret < 0)
+		return ret;
+
+	/* Beacons and broadcast settings */
+	ret = wl1271_init_beacon_broadcast(wl);
+	if (ret < 0)
+		return ret;
+
+	/* Configure for ELP power saving */
+	ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
+	if (ret < 0)
+		return ret;
+
+	/* Configure rssi/snr averaging weights */
+	ret = wl1271_acx_rssi_snr_avg_weights(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_acx_sta_rate_policies(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_acx_sta_mem_cfg(wl);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wl1271_sta_hw_init_post_mem(struct wl1271 *wl)
+{
+	int ret, i;
+
+	ret = wl1271_cmd_set_sta_default_wep_key(wl, wl->default_key);
+	if (ret < 0) {
+		wl1271_warning("couldn't set default key");
+		return ret;
+	}
+
+	/* disable all keep-alive templates */
+	for (i = 0; i < CMD_TEMPL_KLV_IDX_MAX; i++) {
+		ret = wl1271_acx_keep_alive_config(wl, i,
+						   ACX_KEEP_ALIVE_TPL_INVALID);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* disable the keep-alive feature */
+	ret = wl1271_acx_keep_alive_mode(wl, false);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wl1271_ap_hw_init(struct wl1271 *wl)
+{
+	int ret, i;
+
+	ret = wl1271_ap_init_templates_config(wl);
+	if (ret < 0)
+		return ret;
+
+	/* Configure for power always on */
+	ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+	if (ret < 0)
+		return ret;
+
+	/* Configure initial TX rate classes */
+	for (i = 0; i < wl->conf.tx.ac_conf_count; i++) {
+		ret = wl1271_acx_ap_rate_policy(wl,
+				&wl->conf.tx.ap_rc_conf[i], i);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = wl1271_acx_ap_rate_policy(wl,
+					&wl->conf.tx.ap_mgmt_conf,
+					ACX_TX_AP_MODE_MGMT_RATE);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_acx_ap_rate_policy(wl,
+					&wl->conf.tx.ap_bcst_conf,
+					ACX_TX_AP_MODE_BCST_RATE);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_acx_max_tx_retry(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_acx_ap_mem_cfg(wl);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wl1271_ap_hw_init_post_mem(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wl1271_ap_init_deauth_template(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_ap_init_null_template(wl);
+	if (ret < 0)
+		return ret;
+
+	ret = wl1271_ap_init_qos_null_template(wl);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static void wl1271_check_ba_support(struct wl1271 *wl)
+{
+	/* validate FW cose ver x.x.x.50-60.x */
+	if ((wl->chip.fw_ver[3] >= WL12XX_BA_SUPPORT_FW_COST_VER2_START) &&
+	    (wl->chip.fw_ver[3] < WL12XX_BA_SUPPORT_FW_COST_VER2_END)) {
+		wl->ba_support = true;
+		return;
+	}
+
+	wl->ba_support = false;
+}
+
+static int wl1271_set_ba_policies(struct wl1271 *wl)
+{
+	u8 tid_index;
+	int ret = 0;
+
+	/* Reset the BA RX indicators */
+	wl->ba_rx_bitmap = 0;
+
+	/* validate that FW support BA */
+	wl1271_check_ba_support(wl);
+
+	if (wl->ba_support)
+		/* 802.11n initiator BA session setting */
+		for (tid_index = 0; tid_index < CONF_TX_MAX_TID_COUNT;
+		     ++tid_index) {
+			ret = wl1271_acx_set_ba_session(wl, WLAN_BACK_INITIATOR,
+							tid_index, true);
+			if (ret < 0)
+				break;
+		}
+
+	return ret;
+}
+
 int wl1271_hw_init(struct wl1271 *wl)
 {
 	struct conf_tx_ac_category *conf_ac;
 	struct conf_tx_tid *conf_tid;
 	int ret, i;
+	bool is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
 
 	ret = wl1271_cmd_general_parms(wl);
 	if (ret < 0)
@@ -227,12 +519,12 @@ int wl1271_hw_init(struct wl1271 *wl)
 	if (ret < 0)
 		return ret;
 
-	ret = wl1271_cmd_ext_radio_parms(wl);
-	if (ret < 0)
-		return ret;
+	/* Mode specific init */
+	if (is_ap)
+		ret = wl1271_ap_hw_init(wl);
+	else
+		ret = wl1271_sta_hw_init(wl);
 
-	/* Template settings */
-	ret = wl1271_init_templates_config(wl);
 	if (ret < 0)
 		return ret;
 
@@ -259,16 +551,6 @@ int wl1271_hw_init(struct wl1271 *wl)
 	if (ret < 0)
 		goto out_free_memmap;
 
-	/* Initialize connection monitoring thresholds */
-	ret = wl1271_acx_conn_monit_params(wl, false);
-	if (ret < 0)
-		goto out_free_memmap;
-
-	/* Beacon filtering */
-	ret = wl1271_init_beacon_filter(wl);
-	if (ret < 0)
-		goto out_free_memmap;
-
 	/* Configure TX patch complete interrupt behavior */
 	ret = wl1271_acx_tx_config_options(wl);
 	if (ret < 0)
@@ -279,21 +561,11 @@ int wl1271_hw_init(struct wl1271 *wl)
 	if (ret < 0)
 		goto out_free_memmap;
 
-	/* Bluetooth WLAN coexistence */
-	ret = wl1271_init_pta(wl);
-	if (ret < 0)
-		goto out_free_memmap;
-
 	/* Energy detection */
 	ret = wl1271_init_energy_detection(wl);
 	if (ret < 0)
 		goto out_free_memmap;
 
-	/* Beacons and boradcast settings */
-	ret = wl1271_init_beacon_broadcast(wl);
-	if (ret < 0)
-		goto out_free_memmap;
-
 	/* Default fragmentation threshold */
 	ret = wl1271_acx_frag_threshold(wl, wl->conf.tx.frag_threshold);
 	if (ret < 0)
@@ -321,23 +593,13 @@ int wl1271_hw_init(struct wl1271 *wl)
 			goto out_free_memmap;
 	}
 
-	/* Configure TX rate classes */
-	ret = wl1271_acx_rate_policies(wl);
-	if (ret < 0)
-		goto out_free_memmap;
-
 	/* Enable data path */
 	ret = wl1271_cmd_data_path(wl, 1);
 	if (ret < 0)
 		goto out_free_memmap;
 
-	/* Configure for ELP power saving */
-	ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
-	if (ret < 0)
-		goto out_free_memmap;
-
 	/* Configure HW encryption */
-	ret = wl1271_init_hwenc_config(wl);
+	ret = wl1271_acx_feature_cfg(wl);
 	if (ret < 0)
 		goto out_free_memmap;
 
@@ -346,21 +608,17 @@ int wl1271_hw_init(struct wl1271 *wl)
 	if (ret < 0)
 		goto out_free_memmap;
 
-	/* disable all keep-alive templates */
-	for (i = 0; i < CMD_TEMPL_KLV_IDX_MAX; i++) {
-		ret = wl1271_acx_keep_alive_config(wl, i,
-						   ACX_KEEP_ALIVE_TPL_INVALID);
-		if (ret < 0)
-			goto out_free_memmap;
-	}
+	/* Mode specific init - post mem init */
+	if (is_ap)
+		ret = wl1271_ap_hw_init_post_mem(wl);
+	else
+		ret = wl1271_sta_hw_init_post_mem(wl);
 
-	/* disable the keep-alive feature */
-	ret = wl1271_acx_keep_alive_mode(wl, false);
 	if (ret < 0)
 		goto out_free_memmap;
 
-	/* Configure rssi/snr averaging weights */
-	ret = wl1271_acx_rssi_snr_avg_weights(wl);
+	/* Configure initiator BA sessions policies */
+	ret = wl1271_set_ba_policies(wl);
 	if (ret < 0)
 		goto out_free_memmap;
 
diff --git a/drivers/net/wireless/wl12xx/init.h b/drivers/net/wireless/wl12xx/init.h
index 7762421f8602..3a8bd3f426d2 100644
--- a/drivers/net/wireless/wl12xx/init.h
+++ b/drivers/net/wireless/wl12xx/init.h
@@ -27,7 +27,7 @@
 #include "wl12xx.h"
 
 int wl1271_hw_init_power_auth(struct wl1271 *wl);
-int wl1271_init_templates_config(struct wl1271 *wl);
+int wl1271_sta_init_templates_config(struct wl1271 *wl);
 int wl1271_init_phy_config(struct wl1271 *wl);
 int wl1271_init_pta(struct wl1271 *wl);
 int wl1271_init_energy_detection(struct wl1271 *wl);
diff --git a/drivers/net/wireless/wl12xx/io.h b/drivers/net/wireless/wl12xx/io.h
index 844b32b170bb..c1aac8292089 100644
--- a/drivers/net/wireless/wl12xx/io.h
+++ b/drivers/net/wireless/wl12xx/io.h
@@ -168,5 +168,6 @@ void wl1271_unregister_hw(struct wl1271 *wl);
 int wl1271_init_ieee80211(struct wl1271 *wl);
 struct ieee80211_hw *wl1271_alloc_hw(void);
 int wl1271_free_hw(struct wl1271 *wl);
+irqreturn_t wl1271_irq(int irq, void *data);
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/main.c b/drivers/net/wireless/wl12xx/main.c
index 062247ef3ad2..8b3c8d196b03 100644
--- a/drivers/net/wireless/wl12xx/main.c
+++ b/drivers/net/wireless/wl12xx/main.c
@@ -116,11 +116,11 @@ static struct conf_drv_settings default_conf = {
 	},
 	.tx = {
 		.tx_energy_detection         = 0,
-		.rc_conf                     = {
+		.sta_rc_conf                 = {
 			.enabled_rates       = 0,
 			.short_retry_limit   = 10,
 			.long_retry_limit    = 10,
-			.aflags              = 0
+			.aflags              = 0,
 		},
 		.ac_conf_count               = 4,
 		.ac_conf                     = {
@@ -153,6 +153,45 @@ static struct conf_drv_settings default_conf = {
 				.tx_op_limit = 1504,
 			},
 		},
+		.ap_rc_conf                  = {
+			[0] = {
+				.enabled_rates = CONF_TX_AP_ENABLED_RATES,
+				.short_retry_limit = 10,
+				.long_retry_limit = 10,
+				.aflags      = 0,
+			},
+			[1] = {
+				.enabled_rates = CONF_TX_AP_ENABLED_RATES,
+				.short_retry_limit = 10,
+				.long_retry_limit = 10,
+				.aflags      = 0,
+			},
+			[2] = {
+				.enabled_rates = CONF_TX_AP_ENABLED_RATES,
+				.short_retry_limit = 10,
+				.long_retry_limit = 10,
+				.aflags      = 0,
+			},
+			[3] = {
+				.enabled_rates = CONF_TX_AP_ENABLED_RATES,
+				.short_retry_limit = 10,
+				.long_retry_limit = 10,
+				.aflags      = 0,
+			},
+		},
+		.ap_mgmt_conf = {
+			.enabled_rates       = CONF_TX_AP_DEFAULT_MGMT_RATES,
+			.short_retry_limit   = 10,
+			.long_retry_limit    = 10,
+			.aflags              = 0,
+		},
+		.ap_bcst_conf = {
+			.enabled_rates       = CONF_HW_BIT_RATE_1MBPS,
+			.short_retry_limit   = 10,
+			.long_retry_limit    = 10,
+			.aflags              = 0,
+		},
+		.ap_max_tx_retries = 100,
 		.tid_conf_count = 4,
 		.tid_conf = {
 			[CONF_TX_AC_BE] = {
@@ -193,6 +232,8 @@ static struct conf_drv_settings default_conf = {
 		.tx_compl_threshold          = 4,
 		.basic_rate                  = CONF_HW_BIT_RATE_1MBPS,
 		.basic_rate_5                = CONF_HW_BIT_RATE_6MBPS,
+		.tmpl_short_retry_limit      = 10,
+		.tmpl_long_retry_limit       = 10,
 	},
 	.conn = {
 		.wake_up_event               = CONF_WAKE_UP_EVENT_DTIM,
@@ -215,6 +256,7 @@ static struct conf_drv_settings default_conf = {
 		.bet_enable                  = CONF_BET_MODE_ENABLE,
 		.bet_max_consecutive         = 10,
 		.psm_entry_retries           = 5,
+		.psm_exit_retries            = 255,
 		.psm_entry_nullfunc_retries  = 3,
 		.psm_entry_hangover_period   = 1,
 		.keep_alive_interval         = 55000,
@@ -233,13 +275,13 @@ static struct conf_drv_settings default_conf = {
 		.avg_weight_rssi_beacon       = 20,
 		.avg_weight_rssi_data         = 10,
 		.avg_weight_snr_beacon        = 20,
-		.avg_weight_snr_data          = 10
+		.avg_weight_snr_data          = 10,
 	},
 	.scan = {
 		.min_dwell_time_active        = 7500,
 		.max_dwell_time_active        = 30000,
-		.min_dwell_time_passive       = 30000,
-		.max_dwell_time_passive       = 60000,
+		.min_dwell_time_passive       = 100000,
+		.max_dwell_time_passive       = 100000,
 		.num_probe_reqs               = 2,
 	},
 	.rf = {
@@ -252,9 +294,24 @@ static struct conf_drv_settings default_conf = {
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 		},
 	},
+	.ht = {
+		.tx_ba_win_size = 64,
+		.inactivity_timeout = 10000,
+	},
+	.mem = {
+		.num_stations                 = 1,
+		.ssid_profiles                = 1,
+		.rx_block_num                 = 70,
+		.tx_min_block_num             = 40,
+		.dynamic_memory               = 0,
+		.min_req_tx_blocks            = 100,
+		.min_req_rx_blocks            = 22,
+		.tx_min                       = 27,
+	}
 };
 
 static void __wl1271_op_remove_interface(struct wl1271 *wl);
+static void wl1271_free_ap_keys(struct wl1271 *wl);
 
 
 static void wl1271_device_release(struct device *dev)
@@ -317,7 +374,7 @@ static int wl1271_dev_notify(struct notifier_block *me, unsigned long what,
 	if (!test_bit(WL1271_FLAG_STA_ASSOCIATED, &wl->flags))
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -393,7 +450,7 @@ static int wl1271_plt_init(struct wl1271 *wl)
 	if (ret < 0)
 		return ret;
 
-	ret = wl1271_init_templates_config(wl);
+	ret = wl1271_sta_init_templates_config(wl);
 	if (ret < 0)
 		return ret;
 
@@ -425,6 +482,10 @@ static int wl1271_plt_init(struct wl1271 *wl)
 	if (ret < 0)
 		goto out_free_memmap;
 
+	ret = wl1271_acx_sta_mem_cfg(wl);
+	if (ret < 0)
+		goto out_free_memmap;
+
 	/* Default fragmentation threshold */
 	ret = wl1271_acx_frag_threshold(wl, wl->conf.tx.frag_threshold);
 	if (ret < 0)
@@ -476,14 +537,71 @@ static int wl1271_plt_init(struct wl1271 *wl)
 	return ret;
 }
 
+static void wl1271_irq_ps_regulate_link(struct wl1271 *wl, u8 hlid, u8 tx_blks)
+{
+	bool fw_ps;
+
+	/* only regulate station links */
+	if (hlid < WL1271_AP_STA_HLID_START)
+		return;
+
+	fw_ps = test_bit(hlid, (unsigned long *)&wl->ap_fw_ps_map);
+
+	/*
+	 * Wake up from high level PS if the STA is asleep with too little
+	 * blocks in FW or if the STA is awake.
+	 */
+	if (!fw_ps || tx_blks < WL1271_PS_STA_MAX_BLOCKS)
+		wl1271_ps_link_end(wl, hlid);
+
+	/* Start high-level PS if the STA is asleep with enough blocks in FW */
+	else if (fw_ps && tx_blks >= WL1271_PS_STA_MAX_BLOCKS)
+		wl1271_ps_link_start(wl, hlid, true);
+}
+
+static void wl1271_irq_update_links_status(struct wl1271 *wl,
+				       struct wl1271_fw_ap_status *status)
+{
+	u32 cur_fw_ps_map;
+	u8 hlid;
+
+	cur_fw_ps_map = le32_to_cpu(status->link_ps_bitmap);
+	if (wl->ap_fw_ps_map != cur_fw_ps_map) {
+		wl1271_debug(DEBUG_PSM,
+			     "link ps prev 0x%x cur 0x%x changed 0x%x",
+			     wl->ap_fw_ps_map, cur_fw_ps_map,
+			     wl->ap_fw_ps_map ^ cur_fw_ps_map);
+
+		wl->ap_fw_ps_map = cur_fw_ps_map;
+	}
+
+	for (hlid = WL1271_AP_STA_HLID_START; hlid < AP_MAX_LINKS; hlid++) {
+		u8 cnt = status->tx_lnk_free_blks[hlid] -
+			wl->links[hlid].prev_freed_blks;
+
+		wl->links[hlid].prev_freed_blks =
+			status->tx_lnk_free_blks[hlid];
+		wl->links[hlid].allocated_blks -= cnt;
+
+		wl1271_irq_ps_regulate_link(wl, hlid,
+					    wl->links[hlid].allocated_blks);
+	}
+}
+
 static void wl1271_fw_status(struct wl1271 *wl,
-			     struct wl1271_fw_status *status)
+			     struct wl1271_fw_full_status *full_status)
 {
+	struct wl1271_fw_common_status *status = &full_status->common;
 	struct timespec ts;
 	u32 total = 0;
 	int i;
 
-	wl1271_raw_read(wl, FW_STATUS_ADDR, status, sizeof(*status), false);
+	if (wl->bss_type == BSS_TYPE_AP_BSS)
+		wl1271_raw_read(wl, FW_STATUS_ADDR, status,
+				sizeof(struct wl1271_fw_ap_status), false);
+	else
+		wl1271_raw_read(wl, FW_STATUS_ADDR, status,
+				sizeof(struct wl1271_fw_sta_status), false);
 
 	wl1271_debug(DEBUG_IRQ, "intr: 0x%x (fw_rx_counter = %d, "
 		     "drv_rx_counter = %d, tx_results_counter = %d)",
@@ -507,22 +625,54 @@ static void wl1271_fw_status(struct wl1271 *wl,
 	if (total)
 		clear_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags);
 
+	/* for AP update num of allocated TX blocks per link and ps status */
+	if (wl->bss_type == BSS_TYPE_AP_BSS)
+		wl1271_irq_update_links_status(wl, &full_status->ap);
+
 	/* update the host-chipset time offset */
 	getnstimeofday(&ts);
 	wl->time_offset = (timespec_to_ns(&ts) >> 10) -
 		(s64)le32_to_cpu(status->fw_localtime);
 }
 
-#define WL1271_IRQ_MAX_LOOPS 10
+static void wl1271_flush_deferred_work(struct wl1271 *wl)
+{
+	struct sk_buff *skb;
+
+	/* Pass all received frames to the network stack */
+	while ((skb = skb_dequeue(&wl->deferred_rx_queue)))
+		ieee80211_rx_ni(wl->hw, skb);
+
+	/* Return sent skbs to the network stack */
+	while ((skb = skb_dequeue(&wl->deferred_tx_queue)))
+		ieee80211_tx_status(wl->hw, skb);
+}
+
+static void wl1271_netstack_work(struct work_struct *work)
+{
+	struct wl1271 *wl =
+		container_of(work, struct wl1271, netstack_work);
+
+	do {
+		wl1271_flush_deferred_work(wl);
+	} while (skb_queue_len(&wl->deferred_rx_queue));
+}
+
+#define WL1271_IRQ_MAX_LOOPS 256
 
-static void wl1271_irq_work(struct work_struct *work)
+irqreturn_t wl1271_irq(int irq, void *cookie)
 {
 	int ret;
 	u32 intr;
 	int loopcount = WL1271_IRQ_MAX_LOOPS;
+	struct wl1271 *wl = (struct wl1271 *)cookie;
+	bool done = false;
+	unsigned int defer_count;
 	unsigned long flags;
-	struct wl1271 *wl =
-		container_of(work, struct wl1271, irq_work);
+
+	/* TX might be handled here, avoid redundant work */
+	set_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
+	cancel_work_sync(&wl->tx_work);
 
 	mutex_lock(&wl->mutex);
 
@@ -531,26 +681,27 @@ static void wl1271_irq_work(struct work_struct *work)
 	if (unlikely(wl->state == WL1271_STATE_OFF))
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl, true);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
-	spin_lock_irqsave(&wl->wl_lock, flags);
-	while (test_bit(WL1271_FLAG_IRQ_PENDING, &wl->flags) && loopcount) {
-		clear_bit(WL1271_FLAG_IRQ_PENDING, &wl->flags);
-		spin_unlock_irqrestore(&wl->wl_lock, flags);
-		loopcount--;
+	while (!done && loopcount--) {
+		/*
+		 * In order to avoid a race with the hardirq, clear the flag
+		 * before acknowledging the chip. Since the mutex is held,
+		 * wl1271_ps_elp_wakeup cannot be called concurrently.
+		 */
+		clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
+		smp_mb__after_clear_bit();
 
 		wl1271_fw_status(wl, wl->fw_status);
-		intr = le32_to_cpu(wl->fw_status->intr);
+		intr = le32_to_cpu(wl->fw_status->common.intr);
+		intr &= WL1271_INTR_MASK;
 		if (!intr) {
-			wl1271_debug(DEBUG_IRQ, "Zero interrupt received.");
-			spin_lock_irqsave(&wl->wl_lock, flags);
+			done = true;
 			continue;
 		}
 
-		intr &= WL1271_INTR_MASK;
-
 		if (unlikely(intr & WL1271_ACX_INTR_WATCHDOG)) {
 			wl1271_error("watchdog interrupt received! "
 				     "starting recovery.");
@@ -560,25 +711,35 @@ static void wl1271_irq_work(struct work_struct *work)
 			goto out;
 		}
 
-		if (intr & WL1271_ACX_INTR_DATA) {
+		if (likely(intr & WL1271_ACX_INTR_DATA)) {
 			wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_DATA");
 
-			/* check for tx results */
-			if (wl->fw_status->tx_results_counter !=
-			    (wl->tx_results_count & 0xff))
-				wl1271_tx_complete(wl);
+			wl1271_rx(wl, &wl->fw_status->common);
 
 			/* Check if any tx blocks were freed */
+			spin_lock_irqsave(&wl->wl_lock, flags);
 			if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags) &&
 			    wl->tx_queue_count) {
+				spin_unlock_irqrestore(&wl->wl_lock, flags);
 				/*
 				 * In order to avoid starvation of the TX path,
 				 * call the work function directly.
 				 */
 				wl1271_tx_work_locked(wl);
+			} else {
+				spin_unlock_irqrestore(&wl->wl_lock, flags);
 			}
 
-			wl1271_rx(wl, wl->fw_status);
+			/* check for tx results */
+			if (wl->fw_status->common.tx_results_counter !=
+			    (wl->tx_results_count & 0xff))
+				wl1271_tx_complete(wl);
+
+			/* Make sure the deferred queues don't get too long */
+			defer_count = skb_queue_len(&wl->deferred_tx_queue) +
+				      skb_queue_len(&wl->deferred_rx_queue);
+			if (defer_count > WL1271_DEFERRED_QUEUE_LIMIT)
+				wl1271_flush_deferred_work(wl);
 		}
 
 		if (intr & WL1271_ACX_INTR_EVENT_A) {
@@ -597,28 +758,48 @@ static void wl1271_irq_work(struct work_struct *work)
 
 		if (intr & WL1271_ACX_INTR_HW_AVAILABLE)
 			wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_HW_AVAILABLE");
-
-		spin_lock_irqsave(&wl->wl_lock, flags);
 	}
 
-	if (test_bit(WL1271_FLAG_IRQ_PENDING, &wl->flags))
-		ieee80211_queue_work(wl->hw, &wl->irq_work);
-	else
-		clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
-	spin_unlock_irqrestore(&wl->wl_lock, flags);
-
 	wl1271_ps_elp_sleep(wl);
 
 out:
+	spin_lock_irqsave(&wl->wl_lock, flags);
+	/* In case TX was not handled here, queue TX work */
+	clear_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
+	if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags) &&
+	    wl->tx_queue_count)
+		ieee80211_queue_work(wl->hw, &wl->tx_work);
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+
 	mutex_unlock(&wl->mutex);
+
+	return IRQ_HANDLED;
 }
+EXPORT_SYMBOL_GPL(wl1271_irq);
 
 static int wl1271_fetch_firmware(struct wl1271 *wl)
 {
 	const struct firmware *fw;
+	const char *fw_name;
 	int ret;
 
-	ret = request_firmware(&fw, WL1271_FW_NAME, wl1271_wl_to_dev(wl));
+	switch (wl->bss_type) {
+	case BSS_TYPE_AP_BSS:
+		fw_name = WL1271_AP_FW_NAME;
+		break;
+	case BSS_TYPE_IBSS:
+	case BSS_TYPE_STA_BSS:
+		fw_name = WL1271_FW_NAME;
+		break;
+	default:
+		wl1271_error("no compatible firmware for bss_type %d",
+			     wl->bss_type);
+		return -EINVAL;
+	}
+
+	wl1271_debug(DEBUG_BOOT, "booting firmware %s", fw_name);
+
+	ret = request_firmware(&fw, fw_name, wl1271_wl_to_dev(wl));
 
 	if (ret < 0) {
 		wl1271_error("could not get firmware: %d", ret);
@@ -632,6 +813,7 @@ static int wl1271_fetch_firmware(struct wl1271 *wl)
 		goto out;
 	}
 
+	vfree(wl->fw);
 	wl->fw_len = fw->size;
 	wl->fw = vmalloc(wl->fw_len);
 
@@ -642,7 +824,7 @@ static int wl1271_fetch_firmware(struct wl1271 *wl)
 	}
 
 	memcpy(wl->fw, fw->data, wl->fw_len);
-
+	wl->fw_bss_type = wl->bss_type;
 	ret = 0;
 
 out:
@@ -778,7 +960,8 @@ static int wl1271_chip_wakeup(struct wl1271 *wl)
 		goto out;
 	}
 
-	if (wl->fw == NULL) {
+	/* Make sure the firmware type matches the BSS type */
+	if (wl->fw == NULL || wl->fw_bss_type != wl->bss_type) {
 		ret = wl1271_fetch_firmware(wl);
 		if (ret < 0)
 			goto out;
@@ -811,6 +994,8 @@ int wl1271_plt_start(struct wl1271 *wl)
 		goto out;
 	}
 
+	wl->bss_type = BSS_TYPE_STA_BSS;
+
 	while (retries) {
 		retries--;
 		ret = wl1271_chip_wakeup(wl);
@@ -827,11 +1012,10 @@ int wl1271_plt_start(struct wl1271 *wl)
 
 		wl->state = WL1271_STATE_PLT;
 		wl1271_notice("firmware booted in PLT mode (%s)",
-			      wl->chip.fw_ver);
+			      wl->chip.fw_ver_str);
 		goto out;
 
 irq_disable:
-		wl1271_disable_interrupts(wl);
 		mutex_unlock(&wl->mutex);
 		/* Unlocking the mutex in the middle of handling is
 		   inherently unsafe. In this case we deem it safe to do,
@@ -840,7 +1024,9 @@ irq_disable:
 		   work function will not do anything.) Also, any other
 		   possible concurrent operations will fail due to the
 		   current state, hence the wl1271 struct should be safe. */
-		cancel_work_sync(&wl->irq_work);
+		wl1271_disable_interrupts(wl);
+		wl1271_flush_deferred_work(wl);
+		cancel_work_sync(&wl->netstack_work);
 		mutex_lock(&wl->mutex);
 power_off:
 		wl1271_power_off(wl);
@@ -854,12 +1040,10 @@ out:
 	return ret;
 }
 
-int wl1271_plt_stop(struct wl1271 *wl)
+int __wl1271_plt_stop(struct wl1271 *wl)
 {
 	int ret = 0;
 
-	mutex_lock(&wl->mutex);
-
 	wl1271_notice("power down");
 
 	if (wl->state != WL1271_STATE_PLT) {
@@ -869,70 +1053,46 @@ int wl1271_plt_stop(struct wl1271 *wl)
 		goto out;
 	}
 
-	wl1271_disable_interrupts(wl);
 	wl1271_power_off(wl);
 
 	wl->state = WL1271_STATE_OFF;
 	wl->rx_counter = 0;
 
-out:
 	mutex_unlock(&wl->mutex);
-
-	cancel_work_sync(&wl->irq_work);
+	wl1271_disable_interrupts(wl);
+	wl1271_flush_deferred_work(wl);
+	cancel_work_sync(&wl->netstack_work);
 	cancel_work_sync(&wl->recovery_work);
+	mutex_lock(&wl->mutex);
+out:
+	return ret;
+}
+
+int wl1271_plt_stop(struct wl1271 *wl)
+{
+	int ret;
 
+	mutex_lock(&wl->mutex);
+	ret = __wl1271_plt_stop(wl);
+	mutex_unlock(&wl->mutex);
 	return ret;
 }
 
-static int wl1271_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void wl1271_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct wl1271 *wl = hw->priv;
-	struct ieee80211_conf *conf = &hw->conf;
-	struct ieee80211_tx_info *txinfo = IEEE80211_SKB_CB(skb);
-	struct ieee80211_sta *sta = txinfo->control.sta;
 	unsigned long flags;
 	int q;
+	u8 hlid = 0;
 
-	/*
-	 * peek into the rates configured in the STA entry.
-	 * The rates set after connection stage, The first block only BG sets:
-	 * the compare is for bit 0-16 of sta_rate_set. The second block add
-	 * HT rates in case of HT supported.
-	 */
-	spin_lock_irqsave(&wl->wl_lock, flags);
-	if (sta &&
-	    (sta->supp_rates[conf->channel->band] !=
-	    (wl->sta_rate_set & HW_BG_RATES_MASK))) {
-		wl->sta_rate_set = sta->supp_rates[conf->channel->band];
-		set_bit(WL1271_FLAG_STA_RATES_CHANGED, &wl->flags);
-	}
-
-#ifdef CONFIG_WL12XX_HT
-	if (sta &&
-	    sta->ht_cap.ht_supported &&
-	    ((wl->sta_rate_set >> HW_HT_RATES_OFFSET) !=
-	      sta->ht_cap.mcs.rx_mask[0])) {
-		/* Clean MCS bits before setting them */
-		wl->sta_rate_set &= HW_BG_RATES_MASK;
-		wl->sta_rate_set |=
-			(sta->ht_cap.mcs.rx_mask[0] << HW_HT_RATES_OFFSET);
-		set_bit(WL1271_FLAG_STA_RATES_CHANGED, &wl->flags);
-	}
-#endif
-	wl->tx_queue_count++;
-	spin_unlock_irqrestore(&wl->wl_lock, flags);
-
-	/* queue the packet */
 	q = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
-	skb_queue_tail(&wl->tx_queue[q], skb);
 
-	/*
-	 * The chip specific setup must run before the first TX packet -
-	 * before that, the tx_work will not be initialized!
-	 */
+	if (wl->bss_type == BSS_TYPE_AP_BSS)
+		hlid = wl1271_tx_get_hlid(skb);
 
-	if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags))
-		ieee80211_queue_work(wl->hw, &wl->tx_work);
+	spin_lock_irqsave(&wl->wl_lock, flags);
+
+	wl->tx_queue_count++;
 
 	/*
 	 * The workqueue is slow to process the tx_queue and we need stop
@@ -940,14 +1100,28 @@ static int wl1271_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	 */
 	if (wl->tx_queue_count >= WL1271_TX_QUEUE_HIGH_WATERMARK) {
 		wl1271_debug(DEBUG_TX, "op_tx: stopping queues");
-
-		spin_lock_irqsave(&wl->wl_lock, flags);
 		ieee80211_stop_queues(wl->hw);
 		set_bit(WL1271_FLAG_TX_QUEUE_STOPPED, &wl->flags);
-		spin_unlock_irqrestore(&wl->wl_lock, flags);
 	}
 
-	return NETDEV_TX_OK;
+	/* queue the packet */
+	if (wl->bss_type == BSS_TYPE_AP_BSS) {
+		wl1271_debug(DEBUG_TX, "queue skb hlid %d q %d", hlid, q);
+		skb_queue_tail(&wl->links[hlid].tx_queue[q], skb);
+	} else {
+		skb_queue_tail(&wl->tx_queue[q], skb);
+	}
+
+	/*
+	 * The chip specific setup must run before the first TX packet -
+	 * before that, the tx_work will not be initialized!
+	 */
+
+	if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags) &&
+	    !test_bit(WL1271_FLAG_TX_PENDING, &wl->flags))
+		ieee80211_queue_work(wl->hw, &wl->tx_work);
+
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
 }
 
 static struct notifier_block wl1271_dev_notifier = {
@@ -967,6 +1141,9 @@ static int wl1271_op_start(struct ieee80211_hw *hw)
 	 *
 	 * The MAC address is first known when the corresponding interface
 	 * is added. That is where we will initialize the hardware.
+	 *
+	 * In addition, we currently have different firmwares for AP and managed
+	 * operation. We will know which to boot according to interface type.
 	 */
 
 	return 0;
@@ -1006,6 +1183,9 @@ static int wl1271_op_add_interface(struct ieee80211_hw *hw,
 		wl->bss_type = BSS_TYPE_IBSS;
 		wl->set_bss_type = BSS_TYPE_STA_BSS;
 		break;
+	case NL80211_IFTYPE_AP:
+		wl->bss_type = BSS_TYPE_AP_BSS;
+		break;
 	default:
 		ret = -EOPNOTSUPP;
 		goto out;
@@ -1038,7 +1218,6 @@ static int wl1271_op_add_interface(struct ieee80211_hw *hw,
 		break;
 
 irq_disable:
-		wl1271_disable_interrupts(wl);
 		mutex_unlock(&wl->mutex);
 		/* Unlocking the mutex in the middle of handling is
 		   inherently unsafe. In this case we deem it safe to do,
@@ -1047,7 +1226,9 @@ irq_disable:
 		   work function will not do anything.) Also, any other
 		   possible concurrent operations will fail due to the
 		   current state, hence the wl1271 struct should be safe. */
-		cancel_work_sync(&wl->irq_work);
+		wl1271_disable_interrupts(wl);
+		wl1271_flush_deferred_work(wl);
+		cancel_work_sync(&wl->netstack_work);
 		mutex_lock(&wl->mutex);
 power_off:
 		wl1271_power_off(wl);
@@ -1061,11 +1242,11 @@ power_off:
 
 	wl->vif = vif;
 	wl->state = WL1271_STATE_ON;
-	wl1271_info("firmware booted (%s)", wl->chip.fw_ver);
+	wl1271_info("firmware booted (%s)", wl->chip.fw_ver_str);
 
 	/* update hw/fw version info in wiphy struct */
 	wiphy->hw_version = wl->chip.id;
-	strncpy(wiphy->fw_version, wl->chip.fw_ver,
+	strncpy(wiphy->fw_version, wl->chip.fw_ver_str,
 		sizeof(wiphy->fw_version));
 
 	/*
@@ -1113,12 +1294,12 @@ static void __wl1271_op_remove_interface(struct wl1271 *wl)
 
 	wl->state = WL1271_STATE_OFF;
 
-	wl1271_disable_interrupts(wl);
-
 	mutex_unlock(&wl->mutex);
 
+	wl1271_disable_interrupts(wl);
+	wl1271_flush_deferred_work(wl);
 	cancel_delayed_work_sync(&wl->scan_complete_work);
-	cancel_work_sync(&wl->irq_work);
+	cancel_work_sync(&wl->netstack_work);
 	cancel_work_sync(&wl->tx_work);
 	cancel_delayed_work_sync(&wl->pspoll_work);
 	cancel_delayed_work_sync(&wl->elp_work);
@@ -1147,10 +1328,13 @@ static void __wl1271_op_remove_interface(struct wl1271 *wl)
 	wl->time_offset = 0;
 	wl->session_counter = 0;
 	wl->rate_set = CONF_TX_RATE_MASK_BASIC;
-	wl->sta_rate_set = 0;
 	wl->flags = 0;
 	wl->vif = NULL;
 	wl->filters = 0;
+	wl1271_free_ap_keys(wl);
+	memset(wl->ap_hlid_map, 0, sizeof(wl->ap_hlid_map));
+	wl->ap_fw_ps_map = 0;
+	wl->ap_ps_map = 0;
 
 	for (i = 0; i < NUM_TX_QUEUES; i++)
 		wl->tx_blocks_freed[i] = 0;
@@ -1186,8 +1370,7 @@ static void wl1271_op_remove_interface(struct ieee80211_hw *hw,
 
 static void wl1271_configure_filters(struct wl1271 *wl, unsigned int filters)
 {
-	wl->rx_config = WL1271_DEFAULT_RX_CONFIG;
-	wl->rx_filter = WL1271_DEFAULT_RX_FILTER;
+	wl1271_set_default_filters(wl);
 
 	/* combine requested filters with current filter config */
 	filters = wl->filters | filters;
@@ -1322,25 +1505,7 @@ static void wl1271_set_band_rate(struct wl1271 *wl)
 		wl->basic_rate_set = wl->conf.tx.basic_rate_5;
 }
 
-static u32 wl1271_min_rate_get(struct wl1271 *wl)
-{
-	int i;
-	u32 rate = 0;
-
-	if (!wl->basic_rate_set) {
-		WARN_ON(1);
-		wl->basic_rate_set = wl->conf.tx.basic_rate;
-	}
-
-	for (i = 0; !rate; i++) {
-		if ((wl->basic_rate_set >> i) & 0x1)
-			rate = 1 << i;
-	}
-
-	return rate;
-}
-
-static int wl1271_handle_idle(struct wl1271 *wl, bool idle)
+static int wl1271_sta_handle_idle(struct wl1271 *wl, bool idle)
 {
 	int ret;
 
@@ -1350,9 +1515,8 @@ static int wl1271_handle_idle(struct wl1271 *wl, bool idle)
 			if (ret < 0)
 				goto out;
 		}
-		wl->rate_set = wl1271_min_rate_get(wl);
-		wl->sta_rate_set = 0;
-		ret = wl1271_acx_rate_policies(wl);
+		wl->rate_set = wl1271_tx_min_rate_get(wl);
+		ret = wl1271_acx_sta_rate_policies(wl);
 		if (ret < 0)
 			goto out;
 		ret = wl1271_acx_keep_alive_config(
@@ -1381,14 +1545,17 @@ static int wl1271_op_config(struct ieee80211_hw *hw, u32 changed)
 	struct wl1271 *wl = hw->priv;
 	struct ieee80211_conf *conf = &hw->conf;
 	int channel, ret = 0;
+	bool is_ap;
 
 	channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
 
-	wl1271_debug(DEBUG_MAC80211, "mac80211 config ch %d psm %s power %d %s",
+	wl1271_debug(DEBUG_MAC80211, "mac80211 config ch %d psm %s power %d %s"
+		     " changed 0x%x",
 		     channel,
 		     conf->flags & IEEE80211_CONF_PS ? "on" : "off",
 		     conf->power_level,
-		     conf->flags & IEEE80211_CONF_IDLE ? "idle" : "in use");
+		     conf->flags & IEEE80211_CONF_IDLE ? "idle" : "in use",
+			 changed);
 
 	/*
 	 * mac80211 will go to idle nearly immediately after transmitting some
@@ -1406,7 +1573,9 @@ static int wl1271_op_config(struct ieee80211_hw *hw, u32 changed)
 		goto out;
 	}
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
+
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -1417,31 +1586,34 @@ static int wl1271_op_config(struct ieee80211_hw *hw, u32 changed)
 		wl->band = conf->channel->band;
 		wl->channel = channel;
 
-		/*
-		 * FIXME: the mac80211 should really provide a fixed rate
-		 * to use here. for now, just use the smallest possible rate
-		 * for the band as a fixed rate for association frames and
-		 * other control messages.
-		 */
-		if (!test_bit(WL1271_FLAG_STA_ASSOCIATED, &wl->flags))
-			wl1271_set_band_rate(wl);
-
-		wl->basic_rate = wl1271_min_rate_get(wl);
-		ret = wl1271_acx_rate_policies(wl);
-		if (ret < 0)
-			wl1271_warning("rate policy for update channel "
-				       "failed %d", ret);
+		if (!is_ap) {
+			/*
+			 * FIXME: the mac80211 should really provide a fixed
+			 * rate to use here. for now, just use the smallest
+			 * possible rate for the band as a fixed rate for
+			 * association frames and other control messages.
+			 */
+			if (!test_bit(WL1271_FLAG_STA_ASSOCIATED, &wl->flags))
+				wl1271_set_band_rate(wl);
 
-		if (test_bit(WL1271_FLAG_JOINED, &wl->flags)) {
-			ret = wl1271_join(wl, false);
+			wl->basic_rate = wl1271_tx_min_rate_get(wl);
+			ret = wl1271_acx_sta_rate_policies(wl);
 			if (ret < 0)
-				wl1271_warning("cmd join to update channel "
+				wl1271_warning("rate policy for channel "
 					       "failed %d", ret);
+
+			if (test_bit(WL1271_FLAG_JOINED, &wl->flags)) {
+				ret = wl1271_join(wl, false);
+				if (ret < 0)
+					wl1271_warning("cmd join on channel "
+						       "failed %d", ret);
+			}
 		}
 	}
 
-	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-		ret = wl1271_handle_idle(wl, conf->flags & IEEE80211_CONF_IDLE);
+	if (changed & IEEE80211_CONF_CHANGE_IDLE && !is_ap) {
+		ret = wl1271_sta_handle_idle(wl,
+					conf->flags & IEEE80211_CONF_IDLE);
 		if (ret < 0)
 			wl1271_warning("idle mode change failed %d", ret);
 	}
@@ -1548,7 +1720,8 @@ static void wl1271_op_configure_filter(struct ieee80211_hw *hw,
 	struct wl1271 *wl = hw->priv;
 	int ret;
 
-	wl1271_debug(DEBUG_MAC80211, "mac80211 configure filter");
+	wl1271_debug(DEBUG_MAC80211, "mac80211 configure filter changed %x"
+		     " total %x", changed, *total);
 
 	mutex_lock(&wl->mutex);
 
@@ -1558,19 +1731,20 @@ static void wl1271_op_configure_filter(struct ieee80211_hw *hw,
 	if (unlikely(wl->state == WL1271_STATE_OFF))
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
-
-	if (*total & FIF_ALLMULTI)
-		ret = wl1271_acx_group_address_tbl(wl, false, NULL, 0);
-	else if (fp)
-		ret = wl1271_acx_group_address_tbl(wl, fp->enabled,
-						   fp->mc_list,
-						   fp->mc_list_length);
-	if (ret < 0)
-		goto out_sleep;
+	if (wl->bss_type != BSS_TYPE_AP_BSS) {
+		if (*total & FIF_ALLMULTI)
+			ret = wl1271_acx_group_address_tbl(wl, false, NULL, 0);
+		else if (fp)
+			ret = wl1271_acx_group_address_tbl(wl, fp->enabled,
+							   fp->mc_list,
+							   fp->mc_list_length);
+		if (ret < 0)
+			goto out_sleep;
+	}
 
 	/* determine, whether supported filter values have changed */
 	if (changed == 0)
@@ -1593,38 +1767,192 @@ out:
 	kfree(fp);
 }
 
+static int wl1271_record_ap_key(struct wl1271 *wl, u8 id, u8 key_type,
+			u8 key_size, const u8 *key, u8 hlid, u32 tx_seq_32,
+			u16 tx_seq_16)
+{
+	struct wl1271_ap_key *ap_key;
+	int i;
+
+	wl1271_debug(DEBUG_CRYPT, "record ap key id %d", (int)id);
+
+	if (key_size > MAX_KEY_SIZE)
+		return -EINVAL;
+
+	/*
+	 * Find next free entry in ap_keys. Also check we are not replacing
+	 * an existing key.
+	 */
+	for (i = 0; i < MAX_NUM_KEYS; i++) {
+		if (wl->recorded_ap_keys[i] == NULL)
+			break;
+
+		if (wl->recorded_ap_keys[i]->id == id) {
+			wl1271_warning("trying to record key replacement");
+			return -EINVAL;
+		}
+	}
+
+	if (i == MAX_NUM_KEYS)
+		return -EBUSY;
+
+	ap_key = kzalloc(sizeof(*ap_key), GFP_KERNEL);
+	if (!ap_key)
+		return -ENOMEM;
+
+	ap_key->id = id;
+	ap_key->key_type = key_type;
+	ap_key->key_size = key_size;
+	memcpy(ap_key->key, key, key_size);
+	ap_key->hlid = hlid;
+	ap_key->tx_seq_32 = tx_seq_32;
+	ap_key->tx_seq_16 = tx_seq_16;
+
+	wl->recorded_ap_keys[i] = ap_key;
+	return 0;
+}
+
+static void wl1271_free_ap_keys(struct wl1271 *wl)
+{
+	int i;
+
+	for (i = 0; i < MAX_NUM_KEYS; i++) {
+		kfree(wl->recorded_ap_keys[i]);
+		wl->recorded_ap_keys[i] = NULL;
+	}
+}
+
+static int wl1271_ap_init_hwenc(struct wl1271 *wl)
+{
+	int i, ret = 0;
+	struct wl1271_ap_key *key;
+	bool wep_key_added = false;
+
+	for (i = 0; i < MAX_NUM_KEYS; i++) {
+		if (wl->recorded_ap_keys[i] == NULL)
+			break;
+
+		key = wl->recorded_ap_keys[i];
+		ret = wl1271_cmd_set_ap_key(wl, KEY_ADD_OR_REPLACE,
+					    key->id, key->key_type,
+					    key->key_size, key->key,
+					    key->hlid, key->tx_seq_32,
+					    key->tx_seq_16);
+		if (ret < 0)
+			goto out;
+
+		if (key->key_type == KEY_WEP)
+			wep_key_added = true;
+	}
+
+	if (wep_key_added) {
+		ret = wl1271_cmd_set_ap_default_wep_key(wl, wl->default_key);
+		if (ret < 0)
+			goto out;
+	}
+
+out:
+	wl1271_free_ap_keys(wl);
+	return ret;
+}
+
+static int wl1271_set_key(struct wl1271 *wl, u16 action, u8 id, u8 key_type,
+		       u8 key_size, const u8 *key, u32 tx_seq_32,
+		       u16 tx_seq_16, struct ieee80211_sta *sta)
+{
+	int ret;
+	bool is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
+
+	if (is_ap) {
+		struct wl1271_station *wl_sta;
+		u8 hlid;
+
+		if (sta) {
+			wl_sta = (struct wl1271_station *)sta->drv_priv;
+			hlid = wl_sta->hlid;
+		} else {
+			hlid = WL1271_AP_BROADCAST_HLID;
+		}
+
+		if (!test_bit(WL1271_FLAG_AP_STARTED, &wl->flags)) {
+			/*
+			 * We do not support removing keys after AP shutdown.
+			 * Pretend we do to make mac80211 happy.
+			 */
+			if (action != KEY_ADD_OR_REPLACE)
+				return 0;
+
+			ret = wl1271_record_ap_key(wl, id,
+					     key_type, key_size,
+					     key, hlid, tx_seq_32,
+					     tx_seq_16);
+		} else {
+			ret = wl1271_cmd_set_ap_key(wl, action,
+					     id, key_type, key_size,
+					     key, hlid, tx_seq_32,
+					     tx_seq_16);
+		}
+
+		if (ret < 0)
+			return ret;
+	} else {
+		const u8 *addr;
+		static const u8 bcast_addr[ETH_ALEN] = {
+			0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+		};
+
+		addr = sta ? sta->addr : bcast_addr;
+
+		if (is_zero_ether_addr(addr)) {
+			/* We dont support TX only encryption */
+			return -EOPNOTSUPP;
+		}
+
+		/* The wl1271 does not allow to remove unicast keys - they
+		   will be cleared automatically on next CMD_JOIN. Ignore the
+		   request silently, as we dont want the mac80211 to emit
+		   an error message. */
+		if (action == KEY_REMOVE && !is_broadcast_ether_addr(addr))
+			return 0;
+
+		ret = wl1271_cmd_set_sta_key(wl, action,
+					     id, key_type, key_size,
+					     key, addr, tx_seq_32,
+					     tx_seq_16);
+		if (ret < 0)
+			return ret;
+
+		/* the default WEP key needs to be configured at least once */
+		if (key_type == KEY_WEP) {
+			ret = wl1271_cmd_set_sta_default_wep_key(wl,
+							wl->default_key);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
 static int wl1271_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			     struct ieee80211_vif *vif,
 			     struct ieee80211_sta *sta,
 			     struct ieee80211_key_conf *key_conf)
 {
 	struct wl1271 *wl = hw->priv;
-	const u8 *addr;
 	int ret;
 	u32 tx_seq_32 = 0;
 	u16 tx_seq_16 = 0;
 	u8 key_type;
 
-	static const u8 bcast_addr[ETH_ALEN] =
-		{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-
 	wl1271_debug(DEBUG_MAC80211, "mac80211 set key");
 
-	addr = sta ? sta->addr : bcast_addr;
-
-	wl1271_debug(DEBUG_CRYPT, "CMD: 0x%x", cmd);
-	wl1271_dump(DEBUG_CRYPT, "ADDR: ", addr, ETH_ALEN);
+	wl1271_debug(DEBUG_CRYPT, "CMD: 0x%x sta: %p", cmd, sta);
 	wl1271_debug(DEBUG_CRYPT, "Key: algo:0x%x, id:%d, len:%d flags 0x%x",
 		     key_conf->cipher, key_conf->keyidx,
 		     key_conf->keylen, key_conf->flags);
 	wl1271_dump(DEBUG_CRYPT, "KEY: ", key_conf->key, key_conf->keylen);
 
-	if (is_zero_ether_addr(addr)) {
-		/* We dont support TX only encryption */
-		ret = -EOPNOTSUPP;
-		goto out;
-	}
-
 	mutex_lock(&wl->mutex);
 
 	if (unlikely(wl->state == WL1271_STATE_OFF)) {
@@ -1632,7 +1960,7 @@ static int wl1271_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		goto out_unlock;
 	}
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out_unlock;
 
@@ -1671,36 +1999,21 @@ static int wl1271_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	switch (cmd) {
 	case SET_KEY:
-		ret = wl1271_cmd_set_key(wl, KEY_ADD_OR_REPLACE,
-					 key_conf->keyidx, key_type,
-					 key_conf->keylen, key_conf->key,
-					 addr, tx_seq_32, tx_seq_16);
+		ret = wl1271_set_key(wl, KEY_ADD_OR_REPLACE,
+				 key_conf->keyidx, key_type,
+				 key_conf->keylen, key_conf->key,
+				 tx_seq_32, tx_seq_16, sta);
 		if (ret < 0) {
 			wl1271_error("Could not add or replace key");
 			goto out_sleep;
 		}
-
-		/* the default WEP key needs to be configured at least once */
-		if (key_type == KEY_WEP) {
-			ret = wl1271_cmd_set_default_wep_key(wl,
-							     wl->default_key);
-			if (ret < 0)
-				goto out_sleep;
-		}
 		break;
 
 	case DISABLE_KEY:
-		/* The wl1271 does not allow to remove unicast keys - they
-		   will be cleared automatically on next CMD_JOIN. Ignore the
-		   request silently, as we dont want the mac80211 to emit
-		   an error message. */
-		if (!is_broadcast_ether_addr(addr))
-			break;
-
-		ret = wl1271_cmd_set_key(wl, KEY_REMOVE,
-					 key_conf->keyidx, key_type,
-					 key_conf->keylen, key_conf->key,
-					 addr, 0, 0);
+		ret = wl1271_set_key(wl, KEY_REMOVE,
+				     key_conf->keyidx, key_type,
+				     key_conf->keylen, key_conf->key,
+				     0, 0, sta);
 		if (ret < 0) {
 			wl1271_error("Could not remove key");
 			goto out_sleep;
@@ -1719,7 +2032,6 @@ out_sleep:
 out_unlock:
 	mutex_unlock(&wl->mutex);
 
-out:
 	return ret;
 }
 
@@ -1751,7 +2063,7 @@ static int wl1271_op_hw_scan(struct ieee80211_hw *hw,
 		goto out;
 	}
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -1777,7 +2089,7 @@ static int wl1271_op_set_frag_threshold(struct ieee80211_hw *hw, u32 value)
 		goto out;
 	}
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -1805,7 +2117,7 @@ static int wl1271_op_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 		goto out;
 	}
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -1821,7 +2133,7 @@ out:
 	return ret;
 }
 
-static void wl1271_ssid_set(struct wl1271 *wl, struct sk_buff *skb,
+static int wl1271_ssid_set(struct wl1271 *wl, struct sk_buff *skb,
 			    int offset)
 {
 	u8 *ptr = skb->data + offset;
@@ -1831,89 +2143,213 @@ static void wl1271_ssid_set(struct wl1271 *wl, struct sk_buff *skb,
 		if (ptr[0] == WLAN_EID_SSID) {
 			wl->ssid_len = ptr[1];
 			memcpy(wl->ssid, ptr+2, wl->ssid_len);
-			return;
+			return 0;
 		}
 		ptr += (ptr[1] + 2);
 	}
+
 	wl1271_error("No SSID in IEs!\n");
+	return -ENOENT;
 }
 
-static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
-				       struct ieee80211_vif *vif,
+static int wl1271_bss_erp_info_changed(struct wl1271 *wl,
 				       struct ieee80211_bss_conf *bss_conf,
 				       u32 changed)
 {
-	enum wl1271_cmd_ps_mode mode;
-	struct wl1271 *wl = hw->priv;
-	struct ieee80211_sta *sta = ieee80211_find_sta(vif, bss_conf->bssid);
-	bool do_join = false;
-	bool set_assoc = false;
-	int ret;
+	int ret = 0;
 
-	wl1271_debug(DEBUG_MAC80211, "mac80211 bss info changed");
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		if (bss_conf->use_short_slot)
+			ret = wl1271_acx_slot(wl, SLOT_TIME_SHORT);
+		else
+			ret = wl1271_acx_slot(wl, SLOT_TIME_LONG);
+		if (ret < 0) {
+			wl1271_warning("Set slot time failed %d", ret);
+			goto out;
+		}
+	}
 
-	mutex_lock(&wl->mutex);
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+		if (bss_conf->use_short_preamble)
+			wl1271_acx_set_preamble(wl, ACX_PREAMBLE_SHORT);
+		else
+			wl1271_acx_set_preamble(wl, ACX_PREAMBLE_LONG);
+	}
 
-	if (unlikely(wl->state == WL1271_STATE_OFF))
-		goto out;
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		if (bss_conf->use_cts_prot)
+			ret = wl1271_acx_cts_protect(wl, CTSPROTECT_ENABLE);
+		else
+			ret = wl1271_acx_cts_protect(wl, CTSPROTECT_DISABLE);
+		if (ret < 0) {
+			wl1271_warning("Set ctsprotect failed %d", ret);
+			goto out;
+		}
+	}
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
-	if (ret < 0)
-		goto out;
+out:
+	return ret;
+}
+
+static int wl1271_bss_beacon_info_changed(struct wl1271 *wl,
+					  struct ieee80211_vif *vif,
+					  struct ieee80211_bss_conf *bss_conf,
+					  u32 changed)
+{
+	bool is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
+	int ret = 0;
 
-	if ((changed & BSS_CHANGED_BEACON_INT) &&
-	    (wl->bss_type == BSS_TYPE_IBSS)) {
-		wl1271_debug(DEBUG_ADHOC, "ad-hoc beacon interval updated: %d",
+	if ((changed & BSS_CHANGED_BEACON_INT)) {
+		wl1271_debug(DEBUG_MASTER, "beacon interval updated: %d",
 			bss_conf->beacon_int);
 
 		wl->beacon_int = bss_conf->beacon_int;
-		do_join = true;
 	}
 
-	if ((changed & BSS_CHANGED_BEACON) &&
-	    (wl->bss_type == BSS_TYPE_IBSS)) {
-		struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
+	if ((changed & BSS_CHANGED_BEACON)) {
+		struct ieee80211_hdr *hdr;
+		int ieoffset = offsetof(struct ieee80211_mgmt,
+					u.beacon.variable);
+		struct sk_buff *beacon = ieee80211_beacon_get(wl->hw, vif);
+		u16 tmpl_id;
 
-		wl1271_debug(DEBUG_ADHOC, "ad-hoc beacon updated");
+		if (!beacon)
+			goto out;
 
-		if (beacon) {
-			struct ieee80211_hdr *hdr;
-			int ieoffset = offsetof(struct ieee80211_mgmt,
-						u.beacon.variable);
+		wl1271_debug(DEBUG_MASTER, "beacon updated");
 
-			wl1271_ssid_set(wl, beacon, ieoffset);
+		ret = wl1271_ssid_set(wl, beacon, ieoffset);
+		if (ret < 0) {
+			dev_kfree_skb(beacon);
+			goto out;
+		}
+		tmpl_id = is_ap ? CMD_TEMPL_AP_BEACON :
+				  CMD_TEMPL_BEACON;
+		ret = wl1271_cmd_template_set(wl, tmpl_id,
+					      beacon->data,
+					      beacon->len, 0,
+					      wl1271_tx_min_rate_get(wl));
+		if (ret < 0) {
+			dev_kfree_skb(beacon);
+			goto out;
+		}
 
-			ret = wl1271_cmd_template_set(wl, CMD_TEMPL_BEACON,
-						      beacon->data,
-						      beacon->len, 0,
-						      wl1271_min_rate_get(wl));
+		hdr = (struct ieee80211_hdr *) beacon->data;
+		hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						 IEEE80211_STYPE_PROBE_RESP);
+
+		tmpl_id = is_ap ? CMD_TEMPL_AP_PROBE_RESPONSE :
+				  CMD_TEMPL_PROBE_RESPONSE;
+		ret = wl1271_cmd_template_set(wl,
+					      tmpl_id,
+					      beacon->data,
+					      beacon->len, 0,
+					      wl1271_tx_min_rate_get(wl));
+		dev_kfree_skb(beacon);
+		if (ret < 0)
+			goto out;
+	}
 
-			if (ret < 0) {
-				dev_kfree_skb(beacon);
-				goto out_sleep;
-			}
+out:
+	return ret;
+}
 
-			hdr = (struct ieee80211_hdr *) beacon->data;
-			hdr->frame_control = cpu_to_le16(
-				IEEE80211_FTYPE_MGMT |
-				IEEE80211_STYPE_PROBE_RESP);
+/* AP mode changes */
+static void wl1271_bss_info_changed_ap(struct wl1271 *wl,
+				       struct ieee80211_vif *vif,
+				       struct ieee80211_bss_conf *bss_conf,
+				       u32 changed)
+{
+	int ret = 0;
 
-			ret = wl1271_cmd_template_set(wl,
-						      CMD_TEMPL_PROBE_RESPONSE,
-						      beacon->data,
-						      beacon->len, 0,
-						      wl1271_min_rate_get(wl));
-			dev_kfree_skb(beacon);
-			if (ret < 0)
-				goto out_sleep;
+	if ((changed & BSS_CHANGED_BASIC_RATES)) {
+		u32 rates = bss_conf->basic_rates;
+		struct conf_tx_rate_class mgmt_rc;
+
+		wl->basic_rate_set = wl1271_tx_enabled_rates_get(wl, rates);
+		wl->basic_rate = wl1271_tx_min_rate_get(wl);
+		wl1271_debug(DEBUG_AP, "basic rates: 0x%x",
+			     wl->basic_rate_set);
+
+		/* update the AP management rate policy with the new rates */
+		mgmt_rc.enabled_rates = wl->basic_rate_set;
+		mgmt_rc.long_retry_limit = 10;
+		mgmt_rc.short_retry_limit = 10;
+		mgmt_rc.aflags = 0;
+		ret = wl1271_acx_ap_rate_policy(wl, &mgmt_rc,
+						ACX_TX_AP_MODE_MGMT_RATE);
+		if (ret < 0) {
+			wl1271_error("AP mgmt policy change failed %d", ret);
+			goto out;
+		}
+	}
 
-			/* Need to update the SSID (for filtering etc) */
-			do_join = true;
+	ret = wl1271_bss_beacon_info_changed(wl, vif, bss_conf, changed);
+	if (ret < 0)
+		goto out;
+
+	if ((changed & BSS_CHANGED_BEACON_ENABLED)) {
+		if (bss_conf->enable_beacon) {
+			if (!test_bit(WL1271_FLAG_AP_STARTED, &wl->flags)) {
+				ret = wl1271_cmd_start_bss(wl);
+				if (ret < 0)
+					goto out;
+
+				set_bit(WL1271_FLAG_AP_STARTED, &wl->flags);
+				wl1271_debug(DEBUG_AP, "started AP");
+
+				ret = wl1271_ap_init_hwenc(wl);
+				if (ret < 0)
+					goto out;
+			}
+		} else {
+			if (test_bit(WL1271_FLAG_AP_STARTED, &wl->flags)) {
+				ret = wl1271_cmd_stop_bss(wl);
+				if (ret < 0)
+					goto out;
+
+				clear_bit(WL1271_FLAG_AP_STARTED, &wl->flags);
+				wl1271_debug(DEBUG_AP, "stopped AP");
+			}
 		}
 	}
 
-	if ((changed & BSS_CHANGED_BEACON_ENABLED) &&
-	    (wl->bss_type == BSS_TYPE_IBSS)) {
+	ret = wl1271_bss_erp_info_changed(wl, bss_conf, changed);
+	if (ret < 0)
+		goto out;
+out:
+	return;
+}
+
+/* STA/IBSS mode changes */
+static void wl1271_bss_info_changed_sta(struct wl1271 *wl,
+					struct ieee80211_vif *vif,
+					struct ieee80211_bss_conf *bss_conf,
+					u32 changed)
+{
+	bool do_join = false, set_assoc = false;
+	bool is_ibss = (wl->bss_type == BSS_TYPE_IBSS);
+	u32 sta_rate_set = 0;
+	int ret;
+	struct ieee80211_sta *sta;
+	bool sta_exists = false;
+	struct ieee80211_sta_ht_cap sta_ht_cap;
+
+	if (is_ibss) {
+		ret = wl1271_bss_beacon_info_changed(wl, vif, bss_conf,
+						     changed);
+		if (ret < 0)
+			goto out;
+	}
+
+	if ((changed & BSS_CHANGED_BEACON_INT)  && is_ibss)
+		do_join = true;
+
+	/* Need to update the SSID (for filtering etc) */
+	if ((changed & BSS_CHANGED_BEACON) && is_ibss)
+		do_join = true;
+
+	if ((changed & BSS_CHANGED_BEACON_ENABLED) && is_ibss) {
 		wl1271_debug(DEBUG_ADHOC, "ad-hoc beaconing: %s",
 			     bss_conf->enable_beacon ? "enabled" : "disabled");
 
@@ -1924,7 +2360,7 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 		do_join = true;
 	}
 
-	if (changed & BSS_CHANGED_CQM) {
+	if ((changed & BSS_CHANGED_CQM)) {
 		bool enable = false;
 		if (bss_conf->cqm_rssi_thold)
 			enable = true;
@@ -1942,24 +2378,70 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 	     * and enable the BSSID filter
 	     */
 	    memcmp(wl->bssid, bss_conf->bssid, ETH_ALEN)) {
-			memcpy(wl->bssid, bss_conf->bssid, ETH_ALEN);
+		memcpy(wl->bssid, bss_conf->bssid, ETH_ALEN);
 
+		if (!is_zero_ether_addr(wl->bssid)) {
 			ret = wl1271_cmd_build_null_data(wl);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			ret = wl1271_build_qos_null_data(wl);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			/* filter out all packets not from this BSSID */
 			wl1271_configure_filters(wl, 0);
 
 			/* Need to update the BSSID (for filtering etc) */
 			do_join = true;
+		}
 	}
 
-	if (changed & BSS_CHANGED_ASSOC) {
+	rcu_read_lock();
+	sta = ieee80211_find_sta(vif, bss_conf->bssid);
+	if (sta)  {
+		/* save the supp_rates of the ap */
+		sta_rate_set = sta->supp_rates[wl->hw->conf.channel->band];
+		if (sta->ht_cap.ht_supported)
+			sta_rate_set |=
+			    (sta->ht_cap.mcs.rx_mask[0] << HW_HT_RATES_OFFSET);
+		sta_ht_cap = sta->ht_cap;
+		sta_exists = true;
+	}
+	rcu_read_unlock();
+
+	if (sta_exists) {
+		/* handle new association with HT and HT information change */
+		if ((changed & BSS_CHANGED_HT) &&
+		    (bss_conf->channel_type != NL80211_CHAN_NO_HT)) {
+			ret = wl1271_acx_set_ht_capabilities(wl, &sta_ht_cap,
+							     true);
+			if (ret < 0) {
+				wl1271_warning("Set ht cap true failed %d",
+					       ret);
+				goto out;
+			}
+			ret = wl1271_acx_set_ht_information(wl,
+						bss_conf->ht_operation_mode);
+			if (ret < 0) {
+				wl1271_warning("Set ht information failed %d",
+					       ret);
+				goto out;
+			}
+		}
+		/* handle new association without HT and disassociation */
+		else if (changed & BSS_CHANGED_ASSOC) {
+			ret = wl1271_acx_set_ht_capabilities(wl, &sta_ht_cap,
+							     false);
+			if (ret < 0) {
+				wl1271_warning("Set ht cap false failed %d",
+					       ret);
+				goto out;
+			}
+		}
+	}
+
+	if ((changed & BSS_CHANGED_ASSOC)) {
 		if (bss_conf->assoc) {
 			u32 rates;
 			int ieoffset;
@@ -1975,10 +2457,13 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 			rates = bss_conf->basic_rates;
 			wl->basic_rate_set = wl1271_tx_enabled_rates_get(wl,
 									 rates);
-			wl->basic_rate = wl1271_min_rate_get(wl);
-			ret = wl1271_acx_rate_policies(wl);
+			wl->basic_rate = wl1271_tx_min_rate_get(wl);
+			if (sta_rate_set)
+				wl->rate_set = wl1271_tx_enabled_rates_get(wl,
+								sta_rate_set);
+			ret = wl1271_acx_sta_rate_policies(wl);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			/*
 			 * with wl1271, we don't need to update the
@@ -1988,7 +2473,7 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 			 */
 			ret = wl1271_cmd_build_ps_poll(wl, wl->aid);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			/*
 			 * Get a template for hardware connection maintenance
@@ -2002,17 +2487,19 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 			/* enable the connection monitoring feature */
 			ret = wl1271_acx_conn_monit_params(wl, true);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			/* If we want to go in PSM but we're not there yet */
 			if (test_bit(WL1271_FLAG_PSM_REQUESTED, &wl->flags) &&
 			    !test_bit(WL1271_FLAG_PSM, &wl->flags)) {
+				enum wl1271_cmd_ps_mode mode;
+
 				mode = STATION_POWER_SAVE_MODE;
 				ret = wl1271_ps_set_mode(wl, mode,
 							 wl->basic_rate,
 							 true);
 				if (ret < 0)
-					goto out_sleep;
+					goto out;
 			}
 		} else {
 			/* use defaults when not associated */
@@ -2029,10 +2516,10 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 
 			/* revert back to minimum rates for the current band */
 			wl1271_set_band_rate(wl);
-			wl->basic_rate = wl1271_min_rate_get(wl);
-			ret = wl1271_acx_rate_policies(wl);
+			wl->basic_rate = wl1271_tx_min_rate_get(wl);
+			ret = wl1271_acx_sta_rate_policies(wl);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			/* disable connection monitor features */
 			ret = wl1271_acx_conn_monit_params(wl, false);
@@ -2040,74 +2527,17 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 			/* Disable the keep-alive feature */
 			ret = wl1271_acx_keep_alive_mode(wl, false);
 			if (ret < 0)
-				goto out_sleep;
+				goto out;
 
 			/* restore the bssid filter and go to dummy bssid */
 			wl1271_unjoin(wl);
 			wl1271_dummy_join(wl);
 		}
-
-	}
-
-	if (changed & BSS_CHANGED_ERP_SLOT) {
-		if (bss_conf->use_short_slot)
-			ret = wl1271_acx_slot(wl, SLOT_TIME_SHORT);
-		else
-			ret = wl1271_acx_slot(wl, SLOT_TIME_LONG);
-		if (ret < 0) {
-			wl1271_warning("Set slot time failed %d", ret);
-			goto out_sleep;
-		}
 	}
 
-	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		if (bss_conf->use_short_preamble)
-			wl1271_acx_set_preamble(wl, ACX_PREAMBLE_SHORT);
-		else
-			wl1271_acx_set_preamble(wl, ACX_PREAMBLE_LONG);
-	}
-
-	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
-		if (bss_conf->use_cts_prot)
-			ret = wl1271_acx_cts_protect(wl, CTSPROTECT_ENABLE);
-		else
-			ret = wl1271_acx_cts_protect(wl, CTSPROTECT_DISABLE);
-		if (ret < 0) {
-			wl1271_warning("Set ctsprotect failed %d", ret);
-			goto out_sleep;
-		}
-	}
-
-	/*
-	 * Takes care of: New association with HT enable,
-	 *                HT information change in beacon.
-	 */
-	if (sta &&
-	    (changed & BSS_CHANGED_HT) &&
-	    (bss_conf->channel_type != NL80211_CHAN_NO_HT)) {
-		ret = wl1271_acx_set_ht_capabilities(wl, &sta->ht_cap, true);
-		if (ret < 0) {
-			wl1271_warning("Set ht cap true failed %d", ret);
-			goto out_sleep;
-		}
-			ret = wl1271_acx_set_ht_information(wl,
-				bss_conf->ht_operation_mode);
-		if (ret < 0) {
-			wl1271_warning("Set ht information failed %d", ret);
-			goto out_sleep;
-		}
-	}
-	/*
-	 * Takes care of: New association without HT,
-	 *                Disassociation.
-	 */
-	else if (sta && (changed & BSS_CHANGED_ASSOC)) {
-		ret = wl1271_acx_set_ht_capabilities(wl, &sta->ht_cap, false);
-		if (ret < 0) {
-			wl1271_warning("Set ht cap false failed %d", ret);
-			goto out_sleep;
-		}
-	}
+	ret = wl1271_bss_erp_info_changed(wl, bss_conf, changed);
+	if (ret < 0)
+		goto out;
 
 	if (changed & BSS_CHANGED_ARP_FILTER) {
 		__be32 addr = bss_conf->arp_addr_list[0];
@@ -2124,29 +2554,57 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 			ret = wl1271_cmd_build_arp_rsp(wl, addr);
 			if (ret < 0) {
 				wl1271_warning("build arp rsp failed: %d", ret);
-				goto out_sleep;
+				goto out;
 			}
 
 			ret = wl1271_acx_arp_ip_filter(wl,
-				(ACX_ARP_FILTER_ARP_FILTERING |
-				 ACX_ARP_FILTER_AUTO_ARP),
+				ACX_ARP_FILTER_ARP_FILTERING,
 				addr);
 		} else
 			ret = wl1271_acx_arp_ip_filter(wl, 0, addr);
 
 		if (ret < 0)
-			goto out_sleep;
+			goto out;
 	}
 
 	if (do_join) {
 		ret = wl1271_join(wl, set_assoc);
 		if (ret < 0) {
 			wl1271_warning("cmd join failed %d", ret);
-			goto out_sleep;
+			goto out;
 		}
 	}
 
-out_sleep:
+out:
+	return;
+}
+
+static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif,
+				       struct ieee80211_bss_conf *bss_conf,
+				       u32 changed)
+{
+	struct wl1271 *wl = hw->priv;
+	bool is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
+	int ret;
+
+	wl1271_debug(DEBUG_MAC80211, "mac80211 bss info changed 0x%x",
+		     (int)changed);
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state == WL1271_STATE_OFF))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	if (is_ap)
+		wl1271_bss_info_changed_ap(wl, vif, bss_conf, changed);
+	else
+		wl1271_bss_info_changed_sta(wl, vif, bss_conf, changed);
+
 	wl1271_ps_elp_sleep(wl);
 
 out:
@@ -2158,42 +2616,66 @@ static int wl1271_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
 {
 	struct wl1271 *wl = hw->priv;
 	u8 ps_scheme;
-	int ret;
+	int ret = 0;
 
 	mutex_lock(&wl->mutex);
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 conf tx %d", queue);
 
-	if (unlikely(wl->state == WL1271_STATE_OFF)) {
-		ret = -EAGAIN;
-		goto out;
-	}
-
-	ret = wl1271_ps_elp_wakeup(wl, false);
-	if (ret < 0)
-		goto out;
-
-	/* the txop is confed in units of 32us by the mac80211, we need us */
-	ret = wl1271_acx_ac_cfg(wl, wl1271_tx_get_queue(queue),
-				params->cw_min, params->cw_max,
-				params->aifs, params->txop << 5);
-	if (ret < 0)
-		goto out_sleep;
-
 	if (params->uapsd)
 		ps_scheme = CONF_PS_SCHEME_UPSD_TRIGGER;
 	else
 		ps_scheme = CONF_PS_SCHEME_LEGACY;
 
-	ret = wl1271_acx_tid_cfg(wl, wl1271_tx_get_queue(queue),
-				 CONF_CHANNEL_TYPE_EDCF,
-				 wl1271_tx_get_queue(queue),
-				 ps_scheme, CONF_ACK_POLICY_LEGACY, 0, 0);
-	if (ret < 0)
-		goto out_sleep;
+	if (wl->state == WL1271_STATE_OFF) {
+		/*
+		 * If the state is off, the parameters will be recorded and
+		 * configured on init. This happens in AP-mode.
+		 */
+		struct conf_tx_ac_category *conf_ac =
+			&wl->conf.tx.ac_conf[wl1271_tx_get_queue(queue)];
+		struct conf_tx_tid *conf_tid =
+			&wl->conf.tx.tid_conf[wl1271_tx_get_queue(queue)];
+
+		conf_ac->ac = wl1271_tx_get_queue(queue);
+		conf_ac->cw_min = (u8)params->cw_min;
+		conf_ac->cw_max = params->cw_max;
+		conf_ac->aifsn = params->aifs;
+		conf_ac->tx_op_limit = params->txop << 5;
+
+		conf_tid->queue_id = wl1271_tx_get_queue(queue);
+		conf_tid->channel_type = CONF_CHANNEL_TYPE_EDCF;
+		conf_tid->tsid = wl1271_tx_get_queue(queue);
+		conf_tid->ps_scheme = ps_scheme;
+		conf_tid->ack_policy = CONF_ACK_POLICY_LEGACY;
+		conf_tid->apsd_conf[0] = 0;
+		conf_tid->apsd_conf[1] = 0;
+	} else {
+		ret = wl1271_ps_elp_wakeup(wl);
+		if (ret < 0)
+			goto out;
+
+		/*
+		 * the txop is confed in units of 32us by the mac80211,
+		 * we need us
+		 */
+		ret = wl1271_acx_ac_cfg(wl, wl1271_tx_get_queue(queue),
+					params->cw_min, params->cw_max,
+					params->aifs, params->txop << 5);
+		if (ret < 0)
+			goto out_sleep;
+
+		ret = wl1271_acx_tid_cfg(wl, wl1271_tx_get_queue(queue),
+					 CONF_CHANNEL_TYPE_EDCF,
+					 wl1271_tx_get_queue(queue),
+					 ps_scheme, CONF_ACK_POLICY_LEGACY,
+					 0, 0);
+		if (ret < 0)
+			goto out_sleep;
 
 out_sleep:
-	wl1271_ps_elp_sleep(wl);
+		wl1271_ps_elp_sleep(wl);
+	}
 
 out:
 	mutex_unlock(&wl->mutex);
@@ -2215,7 +2697,7 @@ static u64 wl1271_op_get_tsf(struct ieee80211_hw *hw)
 	if (unlikely(wl->state == WL1271_STATE_OFF))
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -2247,6 +2729,184 @@ static int wl1271_op_get_survey(struct ieee80211_hw *hw, int idx,
 	return 0;
 }
 
+static int wl1271_allocate_sta(struct wl1271 *wl,
+			     struct ieee80211_sta *sta,
+			     u8 *hlid)
+{
+	struct wl1271_station *wl_sta;
+	int id;
+
+	id = find_first_zero_bit(wl->ap_hlid_map, AP_MAX_STATIONS);
+	if (id >= AP_MAX_STATIONS) {
+		wl1271_warning("could not allocate HLID - too much stations");
+		return -EBUSY;
+	}
+
+	wl_sta = (struct wl1271_station *)sta->drv_priv;
+	__set_bit(id, wl->ap_hlid_map);
+	wl_sta->hlid = WL1271_AP_STA_HLID_START + id;
+	*hlid = wl_sta->hlid;
+	memcpy(wl->links[wl_sta->hlid].addr, sta->addr, ETH_ALEN);
+	return 0;
+}
+
+static void wl1271_free_sta(struct wl1271 *wl, u8 hlid)
+{
+	int id = hlid - WL1271_AP_STA_HLID_START;
+
+	if (WARN_ON(!test_bit(id, wl->ap_hlid_map)))
+		return;
+
+	__clear_bit(id, wl->ap_hlid_map);
+	memset(wl->links[hlid].addr, 0, ETH_ALEN);
+	wl1271_tx_reset_link_queues(wl, hlid);
+	__clear_bit(hlid, &wl->ap_ps_map);
+	__clear_bit(hlid, (unsigned long *)&wl->ap_fw_ps_map);
+}
+
+static int wl1271_op_sta_add(struct ieee80211_hw *hw,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_sta *sta)
+{
+	struct wl1271 *wl = hw->priv;
+	int ret = 0;
+	u8 hlid;
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state == WL1271_STATE_OFF))
+		goto out;
+
+	if (wl->bss_type != BSS_TYPE_AP_BSS)
+		goto out;
+
+	wl1271_debug(DEBUG_MAC80211, "mac80211 add sta %d", (int)sta->aid);
+
+	ret = wl1271_allocate_sta(wl, sta, &hlid);
+	if (ret < 0)
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out_free_sta;
+
+	ret = wl1271_cmd_add_sta(wl, sta, hlid);
+	if (ret < 0)
+		goto out_sleep;
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+
+out_free_sta:
+	if (ret < 0)
+		wl1271_free_sta(wl, hlid);
+
+out:
+	mutex_unlock(&wl->mutex);
+	return ret;
+}
+
+static int wl1271_op_sta_remove(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_sta *sta)
+{
+	struct wl1271 *wl = hw->priv;
+	struct wl1271_station *wl_sta;
+	int ret = 0, id;
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state == WL1271_STATE_OFF))
+		goto out;
+
+	if (wl->bss_type != BSS_TYPE_AP_BSS)
+		goto out;
+
+	wl1271_debug(DEBUG_MAC80211, "mac80211 remove sta %d", (int)sta->aid);
+
+	wl_sta = (struct wl1271_station *)sta->drv_priv;
+	id = wl_sta->hlid - WL1271_AP_STA_HLID_START;
+	if (WARN_ON(!test_bit(id, wl->ap_hlid_map)))
+		goto out;
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl1271_cmd_remove_sta(wl, wl_sta->hlid);
+	if (ret < 0)
+		goto out_sleep;
+
+	wl1271_free_sta(wl, wl_sta->hlid);
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+
+out:
+	mutex_unlock(&wl->mutex);
+	return ret;
+}
+
+int wl1271_op_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			   enum ieee80211_ampdu_mlme_action action,
+			   struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			   u8 buf_size)
+{
+	struct wl1271 *wl = hw->priv;
+	int ret;
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(wl->state == WL1271_STATE_OFF)) {
+		ret = -EAGAIN;
+		goto out;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	switch (action) {
+	case IEEE80211_AMPDU_RX_START:
+		if (wl->ba_support) {
+			ret = wl1271_acx_set_ba_receiver_session(wl, tid, *ssn,
+								 true);
+			if (!ret)
+				wl->ba_rx_bitmap |= BIT(tid);
+		} else {
+			ret = -ENOTSUPP;
+		}
+		break;
+
+	case IEEE80211_AMPDU_RX_STOP:
+		ret = wl1271_acx_set_ba_receiver_session(wl, tid, 0, false);
+		if (!ret)
+			wl->ba_rx_bitmap &= ~BIT(tid);
+		break;
+
+	/*
+	 * The BA initiator session management in FW independently.
+	 * Falling break here on purpose for all TX APDU commands.
+	 */
+	case IEEE80211_AMPDU_TX_START:
+	case IEEE80211_AMPDU_TX_STOP:
+	case IEEE80211_AMPDU_TX_OPERATIONAL:
+		ret = -EINVAL;
+		break;
+
+	default:
+		wl1271_error("Incorrect ampdu action id=%x\n", action);
+		ret = -EINVAL;
+	}
+
+	wl1271_ps_elp_sleep(wl);
+
+out:
+	mutex_unlock(&wl->mutex);
+
+	return ret;
+}
+
 /* can't be const, mac80211 writes to this */
 static struct ieee80211_rate wl1271_rates[] = {
 	{ .bitrate = 10,
@@ -2305,6 +2965,7 @@ static struct ieee80211_channel wl1271_channels[] = {
 	{ .hw_value = 11, .center_freq = 2462, .max_power = 25 },
 	{ .hw_value = 12, .center_freq = 2467, .max_power = 25 },
 	{ .hw_value = 13, .center_freq = 2472, .max_power = 25 },
+	{ .hw_value = 14, .center_freq = 2484, .max_power = 25 },
 };
 
 /* mapping to indexes for wl1271_rates */
@@ -2493,6 +3154,9 @@ static const struct ieee80211_ops wl1271_ops = {
 	.conf_tx = wl1271_op_conf_tx,
 	.get_tsf = wl1271_op_get_tsf,
 	.get_survey = wl1271_op_get_survey,
+	.sta_add = wl1271_op_sta_add,
+	.sta_remove = wl1271_op_sta_remove,
+	.ampdu_action = wl1271_op_ampdu_action,
 	CFG80211_TESTMODE_CMD(wl1271_tm_cmd)
 };
 
@@ -2562,7 +3226,7 @@ static ssize_t wl1271_sysfs_store_bt_coex_state(struct device *dev,
 	if (wl->state == WL1271_STATE_OFF)
 		goto out;
 
-	ret = wl1271_ps_elp_wakeup(wl, false);
+	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
@@ -2607,6 +3271,18 @@ int wl1271_register_hw(struct wl1271 *wl)
 	if (wl->mac80211_registered)
 		return 0;
 
+	ret = wl1271_fetch_nvs(wl);
+	if (ret == 0) {
+		u8 *nvs_ptr = (u8 *)wl->nvs->nvs;
+
+		wl->mac_addr[0] = nvs_ptr[11];
+		wl->mac_addr[1] = nvs_ptr[10];
+		wl->mac_addr[2] = nvs_ptr[6];
+		wl->mac_addr[3] = nvs_ptr[5];
+		wl->mac_addr[4] = nvs_ptr[4];
+		wl->mac_addr[5] = nvs_ptr[3];
+	}
+
 	SET_IEEE80211_PERM_ADDR(wl->hw, wl->mac_addr);
 
 	ret = ieee80211_register_hw(wl->hw);
@@ -2629,6 +3305,9 @@ EXPORT_SYMBOL_GPL(wl1271_register_hw);
 
 void wl1271_unregister_hw(struct wl1271 *wl)
 {
+	if (wl->state == WL1271_STATE_PLT)
+		__wl1271_plt_stop(wl);
+
 	unregister_netdevice_notifier(&wl1271_dev_notifier);
 	ieee80211_unregister_hw(wl->hw);
 	wl->mac80211_registered = false;
@@ -2661,13 +3340,15 @@ int wl1271_init_ieee80211(struct wl1271 *wl)
 		IEEE80211_HW_SUPPORTS_UAPSD |
 		IEEE80211_HW_HAS_RATE_CONTROL |
 		IEEE80211_HW_CONNECTION_MONITOR |
-		IEEE80211_HW_SUPPORTS_CQM_RSSI;
+		IEEE80211_HW_SUPPORTS_CQM_RSSI |
+		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+		IEEE80211_HW_AP_LINK_PS;
 
 	wl->hw->wiphy->cipher_suites = cipher_suites;
 	wl->hw->wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
 
 	wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
-		BIT(NL80211_IFTYPE_ADHOC);
+		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_AP);
 	wl->hw->wiphy->max_scan_ssids = 1;
 	/*
 	 * Maximum length of elements in scanning probe request templates
@@ -2676,8 +3357,20 @@ int wl1271_init_ieee80211(struct wl1271 *wl)
 	 */
 	wl->hw->wiphy->max_scan_ie_len = WL1271_CMD_TEMPL_MAX_SIZE -
 			sizeof(struct ieee80211_header);
-	wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &wl1271_band_2ghz;
-	wl->hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &wl1271_band_5ghz;
+
+	/*
+	 * We keep local copies of the band structs because we need to
+	 * modify them on a per-device basis.
+	 */
+	memcpy(&wl->bands[IEEE80211_BAND_2GHZ], &wl1271_band_2ghz,
+	       sizeof(wl1271_band_2ghz));
+	memcpy(&wl->bands[IEEE80211_BAND_5GHZ], &wl1271_band_5ghz,
+	       sizeof(wl1271_band_5ghz));
+
+	wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
+		&wl->bands[IEEE80211_BAND_2GHZ];
+	wl->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
+		&wl->bands[IEEE80211_BAND_5GHZ];
 
 	wl->hw->queues = 4;
 	wl->hw->max_rates = 1;
@@ -2686,6 +3379,10 @@ int wl1271_init_ieee80211(struct wl1271 *wl)
 
 	SET_IEEE80211_DEV(wl->hw, wl1271_wl_to_dev(wl));
 
+	wl->hw->sta_data_size = sizeof(struct wl1271_station);
+
+	wl->hw->max_rx_aggregation_subframes = 8;
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(wl1271_init_ieee80211);
@@ -2697,7 +3394,7 @@ struct ieee80211_hw *wl1271_alloc_hw(void)
 	struct ieee80211_hw *hw;
 	struct platform_device *plat_dev = NULL;
 	struct wl1271 *wl;
-	int i, ret;
+	int i, j, ret;
 	unsigned int order;
 
 	hw = ieee80211_alloc_hw(sizeof(*wl), &wl1271_ops);
@@ -2725,9 +3422,16 @@ struct ieee80211_hw *wl1271_alloc_hw(void)
 	for (i = 0; i < NUM_TX_QUEUES; i++)
 		skb_queue_head_init(&wl->tx_queue[i]);
 
+	for (i = 0; i < NUM_TX_QUEUES; i++)
+		for (j = 0; j < AP_MAX_LINKS; j++)
+			skb_queue_head_init(&wl->links[j].tx_queue[i]);
+
+	skb_queue_head_init(&wl->deferred_rx_queue);
+	skb_queue_head_init(&wl->deferred_tx_queue);
+
 	INIT_DELAYED_WORK(&wl->elp_work, wl1271_elp_work);
 	INIT_DELAYED_WORK(&wl->pspoll_work, wl1271_pspoll_work);
-	INIT_WORK(&wl->irq_work, wl1271_irq_work);
+	INIT_WORK(&wl->netstack_work, wl1271_netstack_work);
 	INIT_WORK(&wl->tx_work, wl1271_tx_work);
 	INIT_WORK(&wl->recovery_work, wl1271_recovery_work);
 	INIT_DELAYED_WORK(&wl->scan_complete_work, wl1271_scan_complete_work);
@@ -2735,19 +3439,25 @@ struct ieee80211_hw *wl1271_alloc_hw(void)
 	wl->beacon_int = WL1271_DEFAULT_BEACON_INT;
 	wl->default_key = 0;
 	wl->rx_counter = 0;
-	wl->rx_config = WL1271_DEFAULT_RX_CONFIG;
-	wl->rx_filter = WL1271_DEFAULT_RX_FILTER;
+	wl->rx_config = WL1271_DEFAULT_STA_RX_CONFIG;
+	wl->rx_filter = WL1271_DEFAULT_STA_RX_FILTER;
 	wl->psm_entry_retry = 0;
 	wl->power_level = WL1271_DEFAULT_POWER_LEVEL;
 	wl->basic_rate_set = CONF_TX_RATE_MASK_BASIC;
 	wl->basic_rate = CONF_TX_RATE_MASK_BASIC;
 	wl->rate_set = CONF_TX_RATE_MASK_BASIC;
-	wl->sta_rate_set = 0;
 	wl->band = IEEE80211_BAND_2GHZ;
 	wl->vif = NULL;
 	wl->flags = 0;
 	wl->sg_enabled = true;
 	wl->hw_pg_ver = -1;
+	wl->bss_type = MAX_BSS_TYPE;
+	wl->set_bss_type = MAX_BSS_TYPE;
+	wl->fw_bss_type = MAX_BSS_TYPE;
+	wl->last_tx_hlid = 0;
+	wl->ap_ps_map = 0;
+	wl->ap_fw_ps_map = 0;
+	wl->quirks = 0;
 
 	memset(wl->tx_frames_map, 0, sizeof(wl->tx_frames_map));
 	for (i = 0; i < ACX_TX_DESCRIPTORS; i++)
@@ -2837,11 +3547,11 @@ int wl1271_free_hw(struct wl1271 *wl)
 }
 EXPORT_SYMBOL_GPL(wl1271_free_hw);
 
-u32 wl12xx_debug_level;
+u32 wl12xx_debug_level = DEBUG_NONE;
 EXPORT_SYMBOL_GPL(wl12xx_debug_level);
-module_param_named(debug_level, wl12xx_debug_level, uint, DEBUG_NONE);
+module_param_named(debug_level, wl12xx_debug_level, uint, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(debug_level, "wl12xx debugging level");
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Luciano Coelho <luciano.coelho@nokia.com>");
+MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
 MODULE_AUTHOR("Juuso Oikarinen <juuso.oikarinen@nokia.com>");
diff --git a/drivers/net/wireless/wl12xx/ps.c b/drivers/net/wireless/wl12xx/ps.c
index 60a3738eadb0..971f13e792da 100644
--- a/drivers/net/wireless/wl12xx/ps.c
+++ b/drivers/net/wireless/wl12xx/ps.c
@@ -24,6 +24,7 @@
 #include "reg.h"
 #include "ps.h"
 #include "io.h"
+#include "tx.h"
 
 #define WL1271_WAKEUP_TIMEOUT 500
 
@@ -68,7 +69,7 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
 	}
 }
 
-int wl1271_ps_elp_wakeup(struct wl1271 *wl, bool chip_awake)
+int wl1271_ps_elp_wakeup(struct wl1271 *wl)
 {
 	DECLARE_COMPLETION_ONSTACK(compl);
 	unsigned long flags;
@@ -86,7 +87,7 @@ int wl1271_ps_elp_wakeup(struct wl1271 *wl, bool chip_awake)
 	 * the completion variable in one entity.
 	 */
 	spin_lock_irqsave(&wl->wl_lock, flags);
-	if (work_pending(&wl->irq_work) || chip_awake)
+	if (test_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags))
 		pending = true;
 	else
 		wl->elp_compl = &compl;
@@ -139,8 +140,7 @@ int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
 			return ret;
 		}
 
-		ret = wl1271_cmd_ps_mode(wl, STATION_POWER_SAVE_MODE,
-					 rates, send);
+		ret = wl1271_cmd_ps_mode(wl, STATION_POWER_SAVE_MODE);
 		if (ret < 0)
 			return ret;
 
@@ -149,7 +149,7 @@ int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
 	case STATION_ACTIVE_MODE:
 	default:
 		wl1271_debug(DEBUG_PSM, "leaving psm");
-		ret = wl1271_ps_elp_wakeup(wl, false);
+		ret = wl1271_ps_elp_wakeup(wl);
 		if (ret < 0)
 			return ret;
 
@@ -163,8 +163,7 @@ int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
 		if (ret < 0)
 			return ret;
 
-		ret = wl1271_cmd_ps_mode(wl, STATION_ACTIVE_MODE,
-					 rates, send);
+		ret = wl1271_cmd_ps_mode(wl, STATION_ACTIVE_MODE);
 		if (ret < 0)
 			return ret;
 
@@ -175,4 +174,81 @@ int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
 	return ret;
 }
 
+static void wl1271_ps_filter_frames(struct wl1271 *wl, u8 hlid)
+{
+	int i, filtered = 0;
+	struct sk_buff *skb;
+	struct ieee80211_tx_info *info;
+	unsigned long flags;
+
+	/* filter all frames currently the low level queus for this hlid */
+	for (i = 0; i < NUM_TX_QUEUES; i++) {
+		while ((skb = skb_dequeue(&wl->links[hlid].tx_queue[i]))) {
+			info = IEEE80211_SKB_CB(skb);
+			info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
+			info->status.rates[0].idx = -1;
+			ieee80211_tx_status(wl->hw, skb);
+			filtered++;
+		}
+	}
+
+	spin_lock_irqsave(&wl->wl_lock, flags);
+	wl->tx_queue_count -= filtered;
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+
+	wl1271_handle_tx_low_watermark(wl);
+}
+
+void wl1271_ps_link_start(struct wl1271 *wl, u8 hlid, bool clean_queues)
+{
+	struct ieee80211_sta *sta;
+
+	if (test_bit(hlid, &wl->ap_ps_map))
+		return;
+
+	wl1271_debug(DEBUG_PSM, "start mac80211 PSM on hlid %d blks %d "
+		     "clean_queues %d", hlid, wl->links[hlid].allocated_blks,
+		     clean_queues);
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(wl->vif, wl->links[hlid].addr);
+	if (!sta) {
+		wl1271_error("could not find sta %pM for starting ps",
+			     wl->links[hlid].addr);
+		rcu_read_unlock();
+		return;
+	}
 
+	ieee80211_sta_ps_transition_ni(sta, true);
+	rcu_read_unlock();
+
+	/* do we want to filter all frames from this link's queues? */
+	if (clean_queues)
+		wl1271_ps_filter_frames(wl, hlid);
+
+	__set_bit(hlid, &wl->ap_ps_map);
+}
+
+void wl1271_ps_link_end(struct wl1271 *wl, u8 hlid)
+{
+	struct ieee80211_sta *sta;
+
+	if (!test_bit(hlid, &wl->ap_ps_map))
+		return;
+
+	wl1271_debug(DEBUG_PSM, "end mac80211 PSM on hlid %d", hlid);
+
+	__clear_bit(hlid, &wl->ap_ps_map);
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(wl->vif, wl->links[hlid].addr);
+	if (!sta) {
+		wl1271_error("could not find sta %pM for ending ps",
+			     wl->links[hlid].addr);
+		goto end;
+	}
+
+	ieee80211_sta_ps_transition_ni(sta, false);
+end:
+	rcu_read_unlock();
+}
diff --git a/drivers/net/wireless/wl12xx/ps.h b/drivers/net/wireless/wl12xx/ps.h
index 8415060f08e5..c41bd0a711bc 100644
--- a/drivers/net/wireless/wl12xx/ps.h
+++ b/drivers/net/wireless/wl12xx/ps.h
@@ -30,7 +30,9 @@
 int wl1271_ps_set_mode(struct wl1271 *wl, enum wl1271_cmd_ps_mode mode,
 		       u32 rates, bool send);
 void wl1271_ps_elp_sleep(struct wl1271 *wl);
-int wl1271_ps_elp_wakeup(struct wl1271 *wl, bool chip_awake);
+int wl1271_ps_elp_wakeup(struct wl1271 *wl);
 void wl1271_elp_work(struct work_struct *work);
+void wl1271_ps_link_start(struct wl1271 *wl, u8 hlid, bool clean_queues);
+void wl1271_ps_link_end(struct wl1271 *wl, u8 hlid);
 
 #endif /* __WL1271_PS_H__ */
diff --git a/drivers/net/wireless/wl12xx/rx.c b/drivers/net/wireless/wl12xx/rx.c
index 682304c30b81..919b59f00301 100644
--- a/drivers/net/wireless/wl12xx/rx.c
+++ b/drivers/net/wireless/wl12xx/rx.c
@@ -29,14 +29,14 @@
 #include "rx.h"
 #include "io.h"
 
-static u8 wl1271_rx_get_mem_block(struct wl1271_fw_status *status,
+static u8 wl1271_rx_get_mem_block(struct wl1271_fw_common_status *status,
 				  u32 drv_rx_counter)
 {
 	return le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]) &
 		RX_MEM_BLOCK_MASK;
 }
 
-static u32 wl1271_rx_get_buf_size(struct wl1271_fw_status *status,
+static u32 wl1271_rx_get_buf_size(struct wl1271_fw_common_status *status,
 				 u32 drv_rx_counter)
 {
 	return (le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]) &
@@ -76,7 +76,7 @@ static void wl1271_rx_status(struct wl1271 *wl,
 	 */
 	wl->noise = desc->rssi - (desc->snr >> 1);
 
-	status->freq = ieee80211_channel_to_frequency(desc->channel);
+	status->freq = ieee80211_channel_to_frequency(desc->channel, desc_band);
 
 	if (desc->flags & WL1271_RX_DESC_ENCRYPT_MASK) {
 		status->flag |= RX_FLAG_IV_STRIPPED | RX_FLAG_MMIC_STRIPPED;
@@ -92,7 +92,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length)
 {
 	struct wl1271_rx_descriptor *desc;
 	struct sk_buff *skb;
-	u16 *fc;
+	struct ieee80211_hdr *hdr;
 	u8 *buf;
 	u8 beacon = 0;
 
@@ -118,8 +118,8 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length)
 	/* now we pull the descriptor out of the buffer */
 	skb_pull(skb, sizeof(*desc));
 
-	fc = (u16 *)skb->data;
-	if ((*fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BEACON)
+	hdr = (struct ieee80211_hdr *)skb->data;
+	if (ieee80211_is_beacon(hdr->frame_control))
 		beacon = 1;
 
 	wl1271_rx_status(wl, desc, IEEE80211_SKB_RXCB(skb), beacon);
@@ -129,12 +129,13 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length)
 
 	skb_trim(skb, skb->len - desc->pad_len);
 
-	ieee80211_rx_ni(wl->hw, skb);
+	skb_queue_tail(&wl->deferred_rx_queue, skb);
+	ieee80211_queue_work(wl->hw, &wl->netstack_work);
 
 	return 0;
 }
 
-void wl1271_rx(struct wl1271 *wl, struct wl1271_fw_status *status)
+void wl1271_rx(struct wl1271 *wl, struct wl1271_fw_common_status *status)
 {
 	struct wl1271_acx_mem_map *wl_mem_map = wl->target_mem_map;
 	u32 buf_size;
@@ -198,6 +199,22 @@ void wl1271_rx(struct wl1271 *wl, struct wl1271_fw_status *status)
 			pkt_offset += pkt_length;
 		}
 	}
-	wl1271_write32(wl, RX_DRIVER_COUNTER_ADDRESS,
-			cpu_to_le32(wl->rx_counter));
+
+	/*
+	 * Write the driver's packet counter to the FW. This is only required
+	 * for older hardware revisions
+	 */
+	if (wl->quirks & WL12XX_QUIRK_END_OF_TRANSACTION)
+		wl1271_write32(wl, RX_DRIVER_COUNTER_ADDRESS, wl->rx_counter);
+}
+
+void wl1271_set_default_filters(struct wl1271 *wl)
+{
+	if (wl->bss_type == BSS_TYPE_AP_BSS) {
+		wl->rx_config = WL1271_DEFAULT_AP_RX_CONFIG;
+		wl->rx_filter = WL1271_DEFAULT_AP_RX_FILTER;
+	} else {
+		wl->rx_config = WL1271_DEFAULT_STA_RX_CONFIG;
+		wl->rx_filter = WL1271_DEFAULT_STA_RX_FILTER;
+	}
 }
diff --git a/drivers/net/wireless/wl12xx/rx.h b/drivers/net/wireless/wl12xx/rx.h
index 3abb26fe0364..75fabf836491 100644
--- a/drivers/net/wireless/wl12xx/rx.h
+++ b/drivers/net/wireless/wl12xx/rx.h
@@ -30,10 +30,6 @@
 #define WL1271_RX_MAX_RSSI -30
 #define WL1271_RX_MIN_RSSI -95
 
-#define WL1271_RX_ALIGN_TO 4
-#define WL1271_RX_ALIGN(len) (((len) + WL1271_RX_ALIGN_TO - 1) & \
-			     ~(WL1271_RX_ALIGN_TO - 1))
-
 #define SHORT_PREAMBLE_BIT   BIT(0)
 #define OFDM_RATE_BIT        BIT(6)
 #define PBCC_RATE_BIT        BIT(7)
@@ -86,8 +82,9 @@
 /*
  * RX Descriptor status
  *
- * Bits 0-2 - status
- * Bits 3-7 - reserved
+ * Bits 0-2 - error code
+ * Bits 3-5 - process_id tag (AP mode FW)
+ * Bits 6-7 - reserved
  */
 #define WL1271_RX_DESC_STATUS_MASK      0x07
 
@@ -110,12 +107,16 @@ struct wl1271_rx_descriptor {
 	u8  snr;
 	__le32 timestamp;
 	u8  packet_class;
-	u8  process_id;
+	union {
+		u8  process_id; /* STA FW */
+		u8  hlid; /* AP FW */
+	} __packed;
 	u8  pad_len;
 	u8  reserved;
 } __packed;
 
-void wl1271_rx(struct wl1271 *wl, struct wl1271_fw_status *status);
+void wl1271_rx(struct wl1271 *wl, struct wl1271_fw_common_status *status);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
+void wl1271_set_default_filters(struct wl1271 *wl);
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/scan.c b/drivers/net/wireless/wl12xx/scan.c
index 6f897b9d90ca..420653a2859c 100644
--- a/drivers/net/wireless/wl12xx/scan.c
+++ b/drivers/net/wireless/wl12xx/scan.c
@@ -27,6 +27,7 @@
 #include "cmd.h"
 #include "scan.h"
 #include "acx.h"
+#include "ps.h"
 
 void wl1271_scan_complete_work(struct work_struct *work)
 {
@@ -40,10 +41,11 @@ void wl1271_scan_complete_work(struct work_struct *work)
 
 	mutex_lock(&wl->mutex);
 
-	if (wl->scan.state == WL1271_SCAN_STATE_IDLE) {
-		mutex_unlock(&wl->mutex);
-		return;
-	}
+	if (wl->state == WL1271_STATE_OFF)
+		goto out;
+
+	if (wl->scan.state == WL1271_SCAN_STATE_IDLE)
+		goto out;
 
 	wl->scan.state = WL1271_SCAN_STATE_IDLE;
 	kfree(wl->scan.scanned_ch);
@@ -52,13 +54,19 @@ void wl1271_scan_complete_work(struct work_struct *work)
 	ieee80211_scan_completed(wl->hw, false);
 
 	/* restore hardware connection monitoring template */
-	if (test_bit(WL1271_FLAG_STA_ASSOCIATED, &wl->flags))
-		wl1271_cmd_build_ap_probe_req(wl, wl->probereq);
+	if (test_bit(WL1271_FLAG_STA_ASSOCIATED, &wl->flags)) {
+		if (wl1271_ps_elp_wakeup(wl) == 0) {
+			wl1271_cmd_build_ap_probe_req(wl, wl->probereq);
+			wl1271_ps_elp_sleep(wl);
+		}
+	}
 
 	if (wl->scan.failed) {
 		wl1271_info("Scan completed due to error.");
 		ieee80211_queue_work(wl->hw, &wl->recovery_work);
 	}
+
+out:
 	mutex_unlock(&wl->mutex);
 
 }
diff --git a/drivers/net/wireless/wl12xx/sdio.c b/drivers/net/wireless/wl12xx/sdio.c
index 93cbb8d5aba9..5b9dbeafec06 100644
--- a/drivers/net/wireless/wl12xx/sdio.c
+++ b/drivers/net/wireless/wl12xx/sdio.c
@@ -28,6 +28,7 @@
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/sdio_ids.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
 #include <linux/gpio.h>
 #include <linux/wl12xx.h>
 #include <linux/pm_runtime.h>
@@ -60,7 +61,7 @@ static struct device *wl1271_sdio_wl_to_dev(struct wl1271 *wl)
 	return &(wl_to_func(wl)->dev);
 }
 
-static irqreturn_t wl1271_irq(int irq, void *cookie)
+static irqreturn_t wl1271_hardirq(int irq, void *cookie)
 {
 	struct wl1271 *wl = cookie;
 	unsigned long flags;
@@ -69,17 +70,14 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 
 	/* complete the ELP completion */
 	spin_lock_irqsave(&wl->wl_lock, flags);
+	set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
 	if (wl->elp_compl) {
 		complete(wl->elp_compl);
 		wl->elp_compl = NULL;
 	}
-
-	if (!test_and_set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags))
-		ieee80211_queue_work(wl->hw, &wl->irq_work);
-	set_bit(WL1271_FLAG_IRQ_PENDING, &wl->flags);
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
 
-	return IRQ_HANDLED;
+	return IRQ_WAKE_THREAD;
 }
 
 static void wl1271_sdio_disable_interrupts(struct wl1271 *wl)
@@ -106,8 +104,6 @@ static void wl1271_sdio_raw_read(struct wl1271 *wl, int addr, void *buf,
 	int ret;
 	struct sdio_func *func = wl_to_func(wl);
 
-	sdio_claim_host(func);
-
 	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG_ADDR)) {
 		((u8 *)buf)[0] = sdio_f0_readb(func, addr, &ret);
 		wl1271_debug(DEBUG_SDIO, "sdio read 52 addr 0x%x, byte 0x%02x",
@@ -123,8 +119,6 @@ static void wl1271_sdio_raw_read(struct wl1271 *wl, int addr, void *buf,
 		wl1271_dump_ascii(DEBUG_SDIO, "data: ", buf, len);
 	}
 
-	sdio_release_host(func);
-
 	if (ret)
 		wl1271_error("sdio read failed (%d)", ret);
 }
@@ -135,8 +129,6 @@ static void wl1271_sdio_raw_write(struct wl1271 *wl, int addr, void *buf,
 	int ret;
 	struct sdio_func *func = wl_to_func(wl);
 
-	sdio_claim_host(func);
-
 	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG_ADDR)) {
 		sdio_f0_writeb(func, ((u8 *)buf)[0], addr, &ret);
 		wl1271_debug(DEBUG_SDIO, "sdio write 52 addr 0x%x, byte 0x%02x",
@@ -152,8 +144,6 @@ static void wl1271_sdio_raw_write(struct wl1271 *wl, int addr, void *buf,
 			ret = sdio_memcpy_toio(func, addr, buf, len);
 	}
 
-	sdio_release_host(func);
-
 	if (ret)
 		wl1271_error("sdio write failed (%d)", ret);
 }
@@ -163,14 +153,18 @@ static int wl1271_sdio_power_on(struct wl1271 *wl)
 	struct sdio_func *func = wl_to_func(wl);
 	int ret;
 
-	/* Power up the card */
+	/* Make sure the card will not be powered off by runtime PM */
 	ret = pm_runtime_get_sync(&func->dev);
 	if (ret < 0)
 		goto out;
 
+	/* Runtime PM might be disabled, so power up the card manually */
+	ret = mmc_power_restore_host(func->card->host);
+	if (ret < 0)
+		goto out;
+
 	sdio_claim_host(func);
 	sdio_enable_func(func);
-	sdio_release_host(func);
 
 out:
 	return ret;
@@ -179,12 +173,17 @@ out:
 static int wl1271_sdio_power_off(struct wl1271 *wl)
 {
 	struct sdio_func *func = wl_to_func(wl);
+	int ret;
 
-	sdio_claim_host(func);
 	sdio_disable_func(func);
 	sdio_release_host(func);
 
-	/* Power down the card */
+	/* Runtime PM might be disabled, so power off the card manually */
+	ret = mmc_power_save_host(func->card->host);
+	if (ret < 0)
+		return ret;
+
+	/* Let runtime PM know the card is powered off */
 	return pm_runtime_put_sync(&func->dev);
 }
 
@@ -241,14 +240,14 @@ static int __devinit wl1271_probe(struct sdio_func *func,
 	wl->irq = wlan_data->irq;
 	wl->ref_clock = wlan_data->board_ref_clock;
 
-	ret = request_irq(wl->irq, wl1271_irq, 0, DRIVER_NAME, wl);
+	ret = request_threaded_irq(wl->irq, wl1271_hardirq, wl1271_irq,
+				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				   DRIVER_NAME, wl);
 	if (ret < 0) {
 		wl1271_error("request_irq() failed: %d", ret);
 		goto out_free;
 	}
 
-	set_irq_type(wl->irq, IRQ_TYPE_EDGE_RISING);
-
 	disable_irq(wl->irq);
 
 	ret = wl1271_init_ieee80211(wl);
@@ -271,7 +270,6 @@ static int __devinit wl1271_probe(struct sdio_func *func,
  out_irq:
 	free_irq(wl->irq, wl);
 
-
  out_free:
 	wl1271_free_hw(wl);
 
@@ -345,3 +343,4 @@ MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Luciano Coelho <luciano.coelho@nokia.com>");
 MODULE_AUTHOR("Juuso Oikarinen <juuso.oikarinen@nokia.com>");
 MODULE_FIRMWARE(WL1271_FW_NAME);
+MODULE_FIRMWARE(WL1271_AP_FW_NAME);
diff --git a/drivers/net/wireless/wl12xx/spi.c b/drivers/net/wireless/wl12xx/spi.c
index 7145ea543783..18cf01719ae0 100644
--- a/drivers/net/wireless/wl12xx/spi.c
+++ b/drivers/net/wireless/wl12xx/spi.c
@@ -110,6 +110,7 @@ static void wl1271_spi_reset(struct wl1271 *wl)
 	spi_message_add_tail(&t, &m);
 
 	spi_sync(wl_to_spi(wl), &m);
+
 	wl1271_dump(DEBUG_SPI, "spi reset -> ", cmd, WSPI_INIT_CMD_LEN);
 	kfree(cmd);
 }
@@ -319,28 +320,23 @@ static void wl1271_spi_raw_write(struct wl1271 *wl, int addr, void *buf,
 	spi_sync(wl_to_spi(wl), &m);
 }
 
-static irqreturn_t wl1271_irq(int irq, void *cookie)
+static irqreturn_t wl1271_hardirq(int irq, void *cookie)
 {
-	struct wl1271 *wl;
+	struct wl1271 *wl = cookie;
 	unsigned long flags;
 
 	wl1271_debug(DEBUG_IRQ, "IRQ");
 
-	wl = cookie;
-
 	/* complete the ELP completion */
 	spin_lock_irqsave(&wl->wl_lock, flags);
+	set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
 	if (wl->elp_compl) {
 		complete(wl->elp_compl);
 		wl->elp_compl = NULL;
 	}
-
-	if (!test_and_set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags))
-		ieee80211_queue_work(wl->hw, &wl->irq_work);
-	set_bit(WL1271_FLAG_IRQ_PENDING, &wl->flags);
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
 
-	return IRQ_HANDLED;
+	return IRQ_WAKE_THREAD;
 }
 
 static int wl1271_spi_set_power(struct wl1271 *wl, bool enable)
@@ -412,14 +408,14 @@ static int __devinit wl1271_probe(struct spi_device *spi)
 		goto out_free;
 	}
 
-	ret = request_irq(wl->irq, wl1271_irq, 0, DRIVER_NAME, wl);
+	ret = request_threaded_irq(wl->irq, wl1271_hardirq, wl1271_irq,
+				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				   DRIVER_NAME, wl);
 	if (ret < 0) {
 		wl1271_error("request_irq() failed: %d", ret);
 		goto out_free;
 	}
 
-	set_irq_type(wl->irq, IRQ_TYPE_EDGE_RISING);
-
 	disable_irq(wl->irq);
 
 	ret = wl1271_init_ieee80211(wl);
@@ -494,4 +490,5 @@ MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Luciano Coelho <luciano.coelho@nokia.com>");
 MODULE_AUTHOR("Juuso Oikarinen <juuso.oikarinen@nokia.com>");
 MODULE_FIRMWARE(WL1271_FW_NAME);
+MODULE_FIRMWARE(WL1271_AP_FW_NAME);
 MODULE_ALIAS("spi:wl1271");
diff --git a/drivers/net/wireless/wl12xx/tx.c b/drivers/net/wireless/wl12xx/tx.c
index b44c75cd8c1e..5e9ef7d53e7e 100644
--- a/drivers/net/wireless/wl12xx/tx.c
+++ b/drivers/net/wireless/wl12xx/tx.c
@@ -23,6 +23,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/etherdevice.h>
 
 #include "wl12xx.h"
 #include "io.h"
@@ -30,6 +31,23 @@
 #include "ps.h"
 #include "tx.h"
 
+static int wl1271_set_default_wep_key(struct wl1271 *wl, u8 id)
+{
+	int ret;
+	bool is_ap = (wl->bss_type == BSS_TYPE_AP_BSS);
+
+	if (is_ap)
+		ret = wl1271_cmd_set_ap_default_wep_key(wl, id);
+	else
+		ret = wl1271_cmd_set_sta_default_wep_key(wl, id);
+
+	if (ret < 0)
+		return ret;
+
+	wl1271_debug(DEBUG_CRYPT, "default wep key idx: %d", (int)id);
+	return 0;
+}
+
 static int wl1271_alloc_tx_id(struct wl1271 *wl, struct sk_buff *skb)
 {
 	int id;
@@ -52,8 +70,65 @@ static void wl1271_free_tx_id(struct wl1271 *wl, int id)
 	}
 }
 
+static void wl1271_tx_ap_update_inconnection_sta(struct wl1271 *wl,
+						 struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr;
+
+	/*
+	 * add the station to the known list before transmitting the
+	 * authentication response. this way it won't get de-authed by FW
+	 * when transmitting too soon.
+	 */
+	hdr = (struct ieee80211_hdr *)(skb->data +
+				       sizeof(struct wl1271_tx_hw_descr));
+	if (ieee80211_is_auth(hdr->frame_control))
+		wl1271_acx_set_inconnection_sta(wl, hdr->addr1);
+}
+
+static void wl1271_tx_regulate_link(struct wl1271 *wl, u8 hlid)
+{
+	bool fw_ps;
+	u8 tx_blks;
+
+	/* only regulate station links */
+	if (hlid < WL1271_AP_STA_HLID_START)
+		return;
+
+	fw_ps = test_bit(hlid, (unsigned long *)&wl->ap_fw_ps_map);
+	tx_blks = wl->links[hlid].allocated_blks;
+
+	/*
+	 * if in FW PS and there is enough data in FW we can put the link
+	 * into high-level PS and clean out its TX queues.
+	 */
+	if (fw_ps && tx_blks >= WL1271_PS_STA_MAX_BLOCKS)
+		wl1271_ps_link_start(wl, hlid, true);
+}
+
+u8 wl1271_tx_get_hlid(struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *control = IEEE80211_SKB_CB(skb);
+
+	if (control->control.sta) {
+		struct wl1271_station *wl_sta;
+
+		wl_sta = (struct wl1271_station *)
+				control->control.sta->drv_priv;
+		return wl_sta->hlid;
+	} else {
+		struct ieee80211_hdr *hdr;
+
+		hdr = (struct ieee80211_hdr *)skb->data;
+		if (ieee80211_is_mgmt(hdr->frame_control))
+			return WL1271_AP_GLOBAL_HLID;
+		else
+			return WL1271_AP_BROADCAST_HLID;
+	}
+}
+
 static int wl1271_tx_allocate(struct wl1271 *wl, struct sk_buff *skb, u32 extra,
-				u32 buf_offset)
+				u32 buf_offset, u8 hlid)
 {
 	struct wl1271_tx_hw_descr *desc;
 	u32 total_len = skb->len + sizeof(struct wl1271_tx_hw_descr) + extra;
@@ -82,6 +157,9 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct sk_buff *skb, u32 extra,
 
 		wl->tx_blocks_available -= total_blocks;
 
+		if (wl->bss_type == BSS_TYPE_AP_BSS)
+			wl->links[hlid].allocated_blks += total_blocks;
+
 		ret = 0;
 
 		wl1271_debug(DEBUG_TX,
@@ -95,11 +173,12 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct sk_buff *skb, u32 extra,
 }
 
 static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct sk_buff *skb,
-			      u32 extra, struct ieee80211_tx_info *control)
+			      u32 extra, struct ieee80211_tx_info *control,
+			      u8 hlid)
 {
 	struct timespec ts;
 	struct wl1271_tx_hw_descr *desc;
-	int pad, ac;
+	int pad, ac, rate_idx;
 	s64 hosttime;
 	u16 tx_attr;
 
@@ -117,7 +196,11 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct sk_buff *skb,
 	getnstimeofday(&ts);
 	hosttime = (timespec_to_ns(&ts) >> 10);
 	desc->start_time = cpu_to_le32(hosttime - wl->time_offset);
-	desc->life_time = cpu_to_le16(TX_HW_MGMT_PKT_LIFETIME_TU);
+
+	if (wl->bss_type != BSS_TYPE_AP_BSS)
+		desc->life_time = cpu_to_le16(TX_HW_MGMT_PKT_LIFETIME_TU);
+	else
+		desc->life_time = cpu_to_le16(TX_HW_AP_MODE_PKT_LIFETIME_TU);
 
 	/* configure the tx attributes */
 	tx_attr = wl->session_counter << TX_HW_ATTR_OFST_SESSION_COUNTER;
@@ -125,25 +208,49 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct sk_buff *skb,
 	/* queue (we use same identifiers for tid's and ac's */
 	ac = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 	desc->tid = ac;
-	desc->aid = TX_HW_DEFAULT_AID;
+
+	if (wl->bss_type != BSS_TYPE_AP_BSS) {
+		desc->aid = hlid;
+
+		/* if the packets are destined for AP (have a STA entry)
+		   send them with AP rate policies, otherwise use default
+		   basic rates */
+		if (control->control.sta)
+			rate_idx = ACX_TX_AP_FULL_RATE;
+		else
+			rate_idx = ACX_TX_BASIC_RATE;
+	} else {
+		desc->hlid = hlid;
+		switch (hlid) {
+		case WL1271_AP_GLOBAL_HLID:
+			rate_idx = ACX_TX_AP_MODE_MGMT_RATE;
+			break;
+		case WL1271_AP_BROADCAST_HLID:
+			rate_idx = ACX_TX_AP_MODE_BCST_RATE;
+			break;
+		default:
+			rate_idx = ac;
+			break;
+		}
+	}
+
+	tx_attr |= rate_idx << TX_HW_ATTR_OFST_RATE_POLICY;
 	desc->reserved = 0;
 
 	/* align the length (and store in terms of words) */
-	pad = WL1271_TX_ALIGN(skb->len);
+	pad = ALIGN(skb->len, WL1271_TX_ALIGN_TO);
 	desc->length = cpu_to_le16(pad >> 2);
 
 	/* calculate number of padding bytes */
 	pad = pad - skb->len;
 	tx_attr |= pad << TX_HW_ATTR_OFST_LAST_WORD_PAD;
 
-	/* if the packets are destined for AP (have a STA entry) send them
-	   with AP rate policies, otherwise use default basic rates */
-	if (control->control.sta)
-		tx_attr |= ACX_TX_AP_FULL_RATE << TX_HW_ATTR_OFST_RATE_POLICY;
-
 	desc->tx_attr = cpu_to_le16(tx_attr);
 
-	wl1271_debug(DEBUG_TX, "tx_fill_hdr: pad: %d", pad);
+	wl1271_debug(DEBUG_TX, "tx_fill_hdr: pad: %d hlid: %d "
+		"tx_attr: 0x%x len: %d life: %d mem: %d", pad, desc->hlid,
+		le16_to_cpu(desc->tx_attr), le16_to_cpu(desc->length),
+		le16_to_cpu(desc->life_time), desc->total_mem_blocks);
 }
 
 /* caller must hold wl->mutex */
@@ -153,8 +260,8 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct sk_buff *skb,
 	struct ieee80211_tx_info *info;
 	u32 extra = 0;
 	int ret = 0;
-	u8 idx;
 	u32 total_len;
+	u8 hlid;
 
 	if (!skb)
 		return -EINVAL;
@@ -166,29 +273,43 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct sk_buff *skb,
 		extra = WL1271_TKIP_IV_SPACE;
 
 	if (info->control.hw_key) {
-		idx = info->control.hw_key->hw_key_idx;
+		bool is_wep;
+		u8 idx = info->control.hw_key->hw_key_idx;
+		u32 cipher = info->control.hw_key->cipher;
 
-		/* FIXME: do we have to do this if we're not using WEP? */
-		if (unlikely(wl->default_key != idx)) {
-			ret = wl1271_cmd_set_default_wep_key(wl, idx);
+		is_wep = (cipher == WLAN_CIPHER_SUITE_WEP40) ||
+			 (cipher == WLAN_CIPHER_SUITE_WEP104);
+
+		if (unlikely(is_wep && wl->default_key != idx)) {
+			ret = wl1271_set_default_wep_key(wl, idx);
 			if (ret < 0)
 				return ret;
 			wl->default_key = idx;
 		}
 	}
 
-	ret = wl1271_tx_allocate(wl, skb, extra, buf_offset);
+	if (wl->bss_type == BSS_TYPE_AP_BSS)
+		hlid = wl1271_tx_get_hlid(skb);
+	else
+		hlid = TX_HW_DEFAULT_AID;
+
+	ret = wl1271_tx_allocate(wl, skb, extra, buf_offset, hlid);
 	if (ret < 0)
 		return ret;
 
-	wl1271_tx_fill_hdr(wl, skb, extra, info);
+	if (wl->bss_type == BSS_TYPE_AP_BSS) {
+		wl1271_tx_ap_update_inconnection_sta(wl, skb);
+		wl1271_tx_regulate_link(wl, hlid);
+	}
+
+	wl1271_tx_fill_hdr(wl, skb, extra, info, hlid);
 
 	/*
 	 * The length of each packet is stored in terms of words. Thus, we must
 	 * pad the skb data to make sure its length is aligned.
 	 * The number of padding bytes is computed and set in wl1271_tx_fill_hdr
 	 */
-	total_len = WL1271_TX_ALIGN(skb->len);
+	total_len = ALIGN(skb->len, WL1271_TX_ALIGN_TO);
 	memcpy(wl->aggr_buf + buf_offset, skb->data, skb->len);
 	memset(wl->aggr_buf + buf_offset + skb->len, 0, total_len - skb->len);
 
@@ -222,7 +343,7 @@ u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set)
 	return enabled_rates;
 }
 
-static void handle_tx_low_watermark(struct wl1271 *wl)
+void wl1271_handle_tx_low_watermark(struct wl1271 *wl)
 {
 	unsigned long flags;
 
@@ -236,7 +357,7 @@ static void handle_tx_low_watermark(struct wl1271 *wl)
 	}
 }
 
-static struct sk_buff *wl1271_skb_dequeue(struct wl1271 *wl)
+static struct sk_buff *wl1271_sta_skb_dequeue(struct wl1271 *wl)
 {
 	struct sk_buff *skb = NULL;
 	unsigned long flags;
@@ -262,12 +383,69 @@ out:
 	return skb;
 }
 
+static struct sk_buff *wl1271_ap_skb_dequeue(struct wl1271 *wl)
+{
+	struct sk_buff *skb = NULL;
+	unsigned long flags;
+	int i, h, start_hlid;
+
+	/* start from the link after the last one */
+	start_hlid = (wl->last_tx_hlid + 1) % AP_MAX_LINKS;
+
+	/* dequeue according to AC, round robin on each link */
+	for (i = 0; i < AP_MAX_LINKS; i++) {
+		h = (start_hlid + i) % AP_MAX_LINKS;
+
+		skb = skb_dequeue(&wl->links[h].tx_queue[CONF_TX_AC_VO]);
+		if (skb)
+			goto out;
+		skb = skb_dequeue(&wl->links[h].tx_queue[CONF_TX_AC_VI]);
+		if (skb)
+			goto out;
+		skb = skb_dequeue(&wl->links[h].tx_queue[CONF_TX_AC_BE]);
+		if (skb)
+			goto out;
+		skb = skb_dequeue(&wl->links[h].tx_queue[CONF_TX_AC_BK]);
+		if (skb)
+			goto out;
+	}
+
+out:
+	if (skb) {
+		wl->last_tx_hlid = h;
+		spin_lock_irqsave(&wl->wl_lock, flags);
+		wl->tx_queue_count--;
+		spin_unlock_irqrestore(&wl->wl_lock, flags);
+	} else {
+		wl->last_tx_hlid = 0;
+	}
+
+	return skb;
+}
+
+static struct sk_buff *wl1271_skb_dequeue(struct wl1271 *wl)
+{
+	if (wl->bss_type == BSS_TYPE_AP_BSS)
+		return wl1271_ap_skb_dequeue(wl);
+
+	return wl1271_sta_skb_dequeue(wl);
+}
+
 static void wl1271_skb_queue_head(struct wl1271 *wl, struct sk_buff *skb)
 {
 	unsigned long flags;
 	int q = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 
-	skb_queue_head(&wl->tx_queue[q], skb);
+	if (wl->bss_type == BSS_TYPE_AP_BSS) {
+		u8 hlid = wl1271_tx_get_hlid(skb);
+		skb_queue_head(&wl->links[hlid].tx_queue[q], skb);
+
+		/* make sure we dequeue the same packet next time */
+		wl->last_tx_hlid = (hlid + AP_MAX_LINKS - 1) % AP_MAX_LINKS;
+	} else {
+		skb_queue_head(&wl->tx_queue[q], skb);
+	}
+
 	spin_lock_irqsave(&wl->wl_lock, flags);
 	wl->tx_queue_count++;
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
@@ -277,38 +455,16 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 {
 	struct sk_buff *skb;
 	bool woken_up = false;
-	u32 sta_rates = 0;
 	u32 buf_offset = 0;
 	bool sent_packets = false;
 	int ret;
 
-	/* check if the rates supported by the AP have changed */
-	if (unlikely(test_and_clear_bit(WL1271_FLAG_STA_RATES_CHANGED,
-					&wl->flags))) {
-		unsigned long flags;
-
-		spin_lock_irqsave(&wl->wl_lock, flags);
-		sta_rates = wl->sta_rate_set;
-		spin_unlock_irqrestore(&wl->wl_lock, flags);
-	}
-
 	if (unlikely(wl->state == WL1271_STATE_OFF))
 		goto out;
 
-	/* if rates have changed, re-configure the rate policy */
-	if (unlikely(sta_rates)) {
-		ret = wl1271_ps_elp_wakeup(wl, false);
-		if (ret < 0)
-			goto out;
-		woken_up = true;
-
-		wl->rate_set = wl1271_tx_enabled_rates_get(wl, sta_rates);
-		wl1271_acx_rate_policies(wl);
-	}
-
 	while ((skb = wl1271_skb_dequeue(wl))) {
 		if (!woken_up) {
-			ret = wl1271_ps_elp_wakeup(wl, false);
+			ret = wl1271_ps_elp_wakeup(wl);
 			if (ret < 0)
 				goto out_ack;
 			woken_up = true;
@@ -350,9 +506,15 @@ out_ack:
 		sent_packets = true;
 	}
 	if (sent_packets) {
-		/* interrupt the firmware with the new packets */
-		wl1271_write32(wl, WL1271_HOST_WR_ACCESS, wl->tx_packets_count);
-		handle_tx_low_watermark(wl);
+		/*
+		 * Interrupt the firmware with the new packets. This is only
+		 * required for older hardware revisions
+		 */
+		if (wl->quirks & WL12XX_QUIRK_END_OF_TRANSACTION)
+			wl1271_write32(wl, WL1271_HOST_WR_ACCESS,
+				       wl->tx_packets_count);
+
+		wl1271_handle_tx_low_watermark(wl);
 	}
 
 out:
@@ -427,7 +589,8 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 		     result->rate_class_index, result->status);
 
 	/* return the packet to the stack */
-	ieee80211_tx_status(wl->hw, skb);
+	skb_queue_tail(&wl->deferred_tx_queue, skb);
+	ieee80211_queue_work(wl->hw, &wl->netstack_work);
 	wl1271_free_tx_id(wl, result->id);
 }
 
@@ -469,34 +632,92 @@ void wl1271_tx_complete(struct wl1271 *wl)
 	}
 }
 
+void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid)
+{
+	struct sk_buff *skb;
+	int i, total = 0;
+	unsigned long flags;
+	struct ieee80211_tx_info *info;
+
+	for (i = 0; i < NUM_TX_QUEUES; i++) {
+		while ((skb = skb_dequeue(&wl->links[hlid].tx_queue[i]))) {
+			wl1271_debug(DEBUG_TX, "link freeing skb 0x%p", skb);
+			info = IEEE80211_SKB_CB(skb);
+			info->status.rates[0].idx = -1;
+			info->status.rates[0].count = 0;
+			ieee80211_tx_status(wl->hw, skb);
+			total++;
+		}
+	}
+
+	spin_lock_irqsave(&wl->wl_lock, flags);
+	wl->tx_queue_count -= total;
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+
+	wl1271_handle_tx_low_watermark(wl);
+}
+
 /* caller must hold wl->mutex */
 void wl1271_tx_reset(struct wl1271 *wl)
 {
 	int i;
 	struct sk_buff *skb;
+	struct ieee80211_tx_info *info;
 
 	/* TX failure */
-	for (i = 0; i < NUM_TX_QUEUES; i++) {
-		while ((skb = skb_dequeue(&wl->tx_queue[i]))) {
-			wl1271_debug(DEBUG_TX, "freeing skb 0x%p", skb);
-			ieee80211_tx_status(wl->hw, skb);
+	if (wl->bss_type == BSS_TYPE_AP_BSS) {
+		for (i = 0; i < AP_MAX_LINKS; i++) {
+			wl1271_tx_reset_link_queues(wl, i);
+			wl->links[i].allocated_blks = 0;
+			wl->links[i].prev_freed_blks = 0;
+		}
+
+		wl->last_tx_hlid = 0;
+	} else {
+		for (i = 0; i < NUM_TX_QUEUES; i++) {
+			while ((skb = skb_dequeue(&wl->tx_queue[i]))) {
+				wl1271_debug(DEBUG_TX, "freeing skb 0x%p",
+					     skb);
+				info = IEEE80211_SKB_CB(skb);
+				info->status.rates[0].idx = -1;
+				info->status.rates[0].count = 0;
+				ieee80211_tx_status(wl->hw, skb);
+			}
 		}
 	}
+
 	wl->tx_queue_count = 0;
 
 	/*
 	 * Make sure the driver is at a consistent state, in case this
 	 * function is called from a context other than interface removal.
 	 */
-	handle_tx_low_watermark(wl);
+	wl1271_handle_tx_low_watermark(wl);
 
-	for (i = 0; i < ACX_TX_DESCRIPTORS; i++)
-		if (wl->tx_frames[i] != NULL) {
-			skb = wl->tx_frames[i];
-			wl1271_free_tx_id(wl, i);
-			wl1271_debug(DEBUG_TX, "freeing skb 0x%p", skb);
-			ieee80211_tx_status(wl->hw, skb);
+	for (i = 0; i < ACX_TX_DESCRIPTORS; i++) {
+		if (wl->tx_frames[i] == NULL)
+			continue;
+
+		skb = wl->tx_frames[i];
+		wl1271_free_tx_id(wl, i);
+		wl1271_debug(DEBUG_TX, "freeing skb 0x%p", skb);
+
+		/* Remove private headers before passing the skb to mac80211 */
+		info = IEEE80211_SKB_CB(skb);
+		skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
+		if (info->control.hw_key &&
+		    info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP) {
+			int hdrlen = ieee80211_get_hdrlen_from_skb(skb);
+			memmove(skb->data + WL1271_TKIP_IV_SPACE, skb->data,
+				hdrlen);
+			skb_pull(skb, WL1271_TKIP_IV_SPACE);
 		}
+
+		info->status.rates[0].idx = -1;
+		info->status.rates[0].count = 0;
+
+		ieee80211_tx_status(wl->hw, skb);
+	}
 }
 
 #define WL1271_TX_FLUSH_TIMEOUT 500000
@@ -509,8 +730,8 @@ void wl1271_tx_flush(struct wl1271 *wl)
 
 	while (!time_after(jiffies, timeout)) {
 		mutex_lock(&wl->mutex);
-		wl1271_debug(DEBUG_TX, "flushing tx buffer: %d",
-			     wl->tx_frames_cnt);
+		wl1271_debug(DEBUG_TX, "flushing tx buffer: %d %d",
+			     wl->tx_frames_cnt, wl->tx_queue_count);
 		if ((wl->tx_frames_cnt == 0) && (wl->tx_queue_count == 0)) {
 			mutex_unlock(&wl->mutex);
 			return;
@@ -521,3 +742,21 @@ void wl1271_tx_flush(struct wl1271 *wl)
 
 	wl1271_warning("Unable to flush all TX buffers, timed out.");
 }
+
+u32 wl1271_tx_min_rate_get(struct wl1271 *wl)
+{
+	int i;
+	u32 rate = 0;
+
+	if (!wl->basic_rate_set) {
+		WARN_ON(1);
+		wl->basic_rate_set = wl->conf.tx.basic_rate;
+	}
+
+	for (i = 0; !rate; i++) {
+		if ((wl->basic_rate_set >> i) & 0x1)
+			rate = 1 << i;
+	}
+
+	return rate;
+}
diff --git a/drivers/net/wireless/wl12xx/tx.h b/drivers/net/wireless/wl12xx/tx.h
index 903e5dc69b7a..02f07fa66e82 100644
--- a/drivers/net/wireless/wl12xx/tx.h
+++ b/drivers/net/wireless/wl12xx/tx.h
@@ -29,6 +29,7 @@
 #define TX_HW_BLOCK_SIZE                 252
 
 #define TX_HW_MGMT_PKT_LIFETIME_TU       2000
+#define TX_HW_AP_MODE_PKT_LIFETIME_TU    8000
 /* The chipset reference driver states, that the "aid" value 1
  * is for infra-BSS, but is still always used */
 #define TX_HW_DEFAULT_AID                1
@@ -52,8 +53,6 @@
 #define TX_HW_RESULT_QUEUE_LEN_MASK      0xf
 
 #define WL1271_TX_ALIGN_TO 4
-#define WL1271_TX_ALIGN(len) (((len) + WL1271_TX_ALIGN_TO - 1) & \
-			     ~(WL1271_TX_ALIGN_TO - 1))
 #define WL1271_TKIP_IV_SPACE 4
 
 struct wl1271_tx_hw_descr {
@@ -77,8 +76,12 @@ struct wl1271_tx_hw_descr {
 	u8 id;
 	/* The packet TID value (as User-Priority) */
 	u8 tid;
-	/* Identifier of the remote STA in IBSS, 1 in infra-BSS */
-	u8 aid;
+	union {
+		/* STA - Identifier of the remote STA in IBSS, 1 in infra-BSS */
+		u8 aid;
+		/* AP - host link ID (HLID) */
+		u8 hlid;
+	} __packed;
 	u8 reserved;
 } __packed;
 
@@ -146,5 +149,9 @@ void wl1271_tx_reset(struct wl1271 *wl);
 void wl1271_tx_flush(struct wl1271 *wl);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
 u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set);
+u32 wl1271_tx_min_rate_get(struct wl1271 *wl);
+u8 wl1271_tx_get_hlid(struct sk_buff *skb);
+void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid);
+void wl1271_handle_tx_low_watermark(struct wl1271 *wl);
 
 #endif
diff --git a/drivers/net/wireless/wl12xx/wl12xx.h b/drivers/net/wireless/wl12xx/wl12xx.h
index 9050dd9b62d2..86be83e25ec5 100644
--- a/drivers/net/wireless/wl12xx/wl12xx.h
+++ b/drivers/net/wireless/wl12xx/wl12xx.h
@@ -38,6 +38,13 @@
 #define DRIVER_NAME "wl1271"
 #define DRIVER_PREFIX DRIVER_NAME ": "
 
+/*
+ * FW versions support BA 11n
+ * versions marks x.x.x.50-60.x
+ */
+#define WL12XX_BA_SUPPORT_FW_COST_VER2_START    50
+#define WL12XX_BA_SUPPORT_FW_COST_VER2_END      60
+
 enum {
 	DEBUG_NONE	= 0,
 	DEBUG_IRQ	= BIT(0),
@@ -57,6 +64,8 @@ enum {
 	DEBUG_SDIO	= BIT(14),
 	DEBUG_FILTERS   = BIT(15),
 	DEBUG_ADHOC     = BIT(16),
+	DEBUG_AP	= BIT(17),
+	DEBUG_MASTER	= (DEBUG_ADHOC | DEBUG_AP),
 	DEBUG_ALL	= ~0,
 };
 
@@ -103,17 +112,28 @@ extern u32 wl12xx_debug_level;
 				       true);				\
 	} while (0)
 
-#define WL1271_DEFAULT_RX_CONFIG (CFG_UNI_FILTER_EN |	\
+#define WL1271_DEFAULT_STA_RX_CONFIG (CFG_UNI_FILTER_EN |	\
 				  CFG_BSSID_FILTER_EN | \
 				  CFG_MC_FILTER_EN)
 
-#define WL1271_DEFAULT_RX_FILTER (CFG_RX_RCTS_ACK | CFG_RX_PRSP_EN |  \
+#define WL1271_DEFAULT_STA_RX_FILTER (CFG_RX_RCTS_ACK | CFG_RX_PRSP_EN |  \
 				  CFG_RX_MGMT_EN | CFG_RX_DATA_EN |   \
 				  CFG_RX_CTL_EN | CFG_RX_BCN_EN |     \
 				  CFG_RX_AUTH_EN | CFG_RX_ASSOC_EN)
 
-#define WL1271_FW_NAME "wl1271-fw.bin"
-#define WL1271_NVS_NAME "wl1271-nvs.bin"
+#define WL1271_DEFAULT_AP_RX_CONFIG  0
+
+#define WL1271_DEFAULT_AP_RX_FILTER  (CFG_RX_RCTS_ACK | CFG_RX_PREQ_EN | \
+				  CFG_RX_MGMT_EN | CFG_RX_DATA_EN | \
+				  CFG_RX_CTL_EN | CFG_RX_AUTH_EN | \
+				  CFG_RX_ASSOC_EN)
+
+
+
+#define WL1271_FW_NAME "ti-connectivity/wl1271-fw-2.bin"
+#define WL1271_AP_FW_NAME "ti-connectivity/wl1271-fw-ap.bin"
+
+#define WL1271_NVS_NAME "ti-connectivity/wl1271-nvs.bin"
 
 #define WL1271_TX_SECURITY_LO16(s) ((u16)((s) & 0xffff))
 #define WL1271_TX_SECURITY_HI32(s) ((u32)(((s) >> 16) & 0xffffffff))
@@ -129,6 +149,25 @@ extern u32 wl12xx_debug_level;
 #define WL1271_DEFAULT_BEACON_INT  100
 #define WL1271_DEFAULT_DTIM_PERIOD 1
 
+#define WL1271_AP_GLOBAL_HLID      0
+#define WL1271_AP_BROADCAST_HLID   1
+#define WL1271_AP_STA_HLID_START   2
+
+/*
+ * When in AP-mode, we allow (at least) this number of mem-blocks
+ * to be transmitted to FW for a STA in PS-mode. Only when packets are
+ * present in the FW buffers it will wake the sleeping STA. We want to put
+ * enough packets for the driver to transmit all of its buffered data before
+ * the STA goes to sleep again. But we don't want to take too much mem-blocks
+ * as it might hurt the throughput of active STAs.
+ * The number of blocks (18) is enough for 2 large packets.
+ */
+#define WL1271_PS_STA_MAX_BLOCKS  (2 * 9)
+
+#define WL1271_AP_BSS_INDEX        0
+#define WL1271_AP_DEF_INACTIV_SEC  300
+#define WL1271_AP_DEF_BEACON_EXP   20
+
 #define ACX_TX_DESCRIPTORS         32
 
 #define WL1271_AGGR_BUFFER_SIZE (4 * PAGE_SIZE)
@@ -161,10 +200,13 @@ struct wl1271_partition_set {
 
 struct wl1271;
 
+#define WL12XX_NUM_FW_VER 5
+
 /* FIXME: I'm not sure about this structure name */
 struct wl1271_chip {
 	u32 id;
-	char fw_ver[21];
+	char fw_ver_str[ETHTOOL_BUSINFO_LEN];
+	unsigned int fw_ver[WL12XX_NUM_FW_VER];
 };
 
 struct wl1271_stats {
@@ -178,8 +220,13 @@ struct wl1271_stats {
 #define NUM_TX_QUEUES              4
 #define NUM_RX_PKT_DESC            8
 
-/* FW status registers */
-struct wl1271_fw_status {
+#define AP_MAX_STATIONS            5
+
+/* Broadcast and Global links + links to stations */
+#define AP_MAX_LINKS               (AP_MAX_STATIONS + 2)
+
+/* FW status registers common for AP/STA */
+struct wl1271_fw_common_status {
 	__le32 intr;
 	u8  fw_rx_counter;
 	u8  drv_rx_counter;
@@ -188,9 +235,43 @@ struct wl1271_fw_status {
 	__le32 rx_pkt_descs[NUM_RX_PKT_DESC];
 	__le32 tx_released_blks[NUM_TX_QUEUES];
 	__le32 fw_localtime;
-	__le32 padding[2];
 } __packed;
 
+/* FW status registers for AP */
+struct wl1271_fw_ap_status {
+	struct wl1271_fw_common_status common;
+
+	/* Next fields valid only in AP FW */
+
+	/*
+	 * A bitmap (where each bit represents a single HLID)
+	 * to indicate if the station is in PS mode.
+	 */
+	__le32 link_ps_bitmap;
+
+	/* Number of freed MBs per HLID */
+	u8 tx_lnk_free_blks[AP_MAX_LINKS];
+	u8 padding_1[1];
+} __packed;
+
+/* FW status registers for STA */
+struct wl1271_fw_sta_status {
+	struct wl1271_fw_common_status common;
+
+	u8  tx_total;
+	u8  reserved1;
+	__le16 reserved2;
+} __packed;
+
+struct wl1271_fw_full_status {
+	union {
+		struct wl1271_fw_common_status common;
+		struct wl1271_fw_sta_status sta;
+		struct wl1271_fw_ap_status ap;
+	};
+} __packed;
+
+
 struct wl1271_rx_mem_pool_addr {
 	u32 addr;
 	u32 addr_extra;
@@ -218,6 +299,48 @@ struct wl1271_if_operations {
 	void (*disable_irq)(struct wl1271 *wl);
 };
 
+#define MAX_NUM_KEYS 14
+#define MAX_KEY_SIZE 32
+
+struct wl1271_ap_key {
+	u8 id;
+	u8 key_type;
+	u8 key_size;
+	u8 key[MAX_KEY_SIZE];
+	u8 hlid;
+	u32 tx_seq_32;
+	u16 tx_seq_16;
+};
+
+enum wl12xx_flags {
+	WL1271_FLAG_STA_ASSOCIATED,
+	WL1271_FLAG_JOINED,
+	WL1271_FLAG_GPIO_POWER,
+	WL1271_FLAG_TX_QUEUE_STOPPED,
+	WL1271_FLAG_TX_PENDING,
+	WL1271_FLAG_IN_ELP,
+	WL1271_FLAG_PSM,
+	WL1271_FLAG_PSM_REQUESTED,
+	WL1271_FLAG_IRQ_RUNNING,
+	WL1271_FLAG_IDLE,
+	WL1271_FLAG_IDLE_REQUESTED,
+	WL1271_FLAG_PSPOLL_FAILURE,
+	WL1271_FLAG_STA_STATE_SENT,
+	WL1271_FLAG_FW_TX_BUSY,
+	WL1271_FLAG_AP_STARTED
+};
+
+struct wl1271_link {
+	/* AP-mode - TX queue per AC in link */
+	struct sk_buff_head tx_queue[NUM_TX_QUEUES];
+
+	/* accounting for allocated / available TX blocks in FW */
+	u8 allocated_blks;
+	u8 prev_freed_blks;
+
+	u8 addr[ETH_ALEN];
+};
+
 struct wl1271 {
 	struct platform_device *plat_dev;
 	struct ieee80211_hw *hw;
@@ -236,21 +359,6 @@ struct wl1271 {
 	enum wl1271_state state;
 	struct mutex mutex;
 
-#define WL1271_FLAG_STA_RATES_CHANGED  (0)
-#define WL1271_FLAG_STA_ASSOCIATED     (1)
-#define WL1271_FLAG_JOINED             (2)
-#define WL1271_FLAG_GPIO_POWER         (3)
-#define WL1271_FLAG_TX_QUEUE_STOPPED   (4)
-#define WL1271_FLAG_IN_ELP             (5)
-#define WL1271_FLAG_PSM                (6)
-#define WL1271_FLAG_PSM_REQUESTED      (7)
-#define WL1271_FLAG_IRQ_PENDING        (8)
-#define WL1271_FLAG_IRQ_RUNNING        (9)
-#define WL1271_FLAG_IDLE              (10)
-#define WL1271_FLAG_IDLE_REQUESTED    (11)
-#define WL1271_FLAG_PSPOLL_FAILURE    (12)
-#define WL1271_FLAG_STA_STATE_SENT    (13)
-#define WL1271_FLAG_FW_TX_BUSY        (14)
 	unsigned long flags;
 
 	struct wl1271_partition_set part;
@@ -262,6 +370,7 @@ struct wl1271 {
 
 	u8 *fw;
 	size_t fw_len;
+	u8 fw_bss_type;
 	struct wl1271_nvs_file *nvs;
 	size_t nvs_len;
 
@@ -295,6 +404,12 @@ struct wl1271 {
 	struct sk_buff_head tx_queue[NUM_TX_QUEUES];
 	int tx_queue_count;
 
+	/* Frames received, not handled yet by mac80211 */
+	struct sk_buff_head deferred_rx_queue;
+
+	/* Frames sent, not returned yet to mac80211 */
+	struct sk_buff_head deferred_tx_queue;
+
 	struct work_struct tx_work;
 
 	/* Pending TX frames */
@@ -315,8 +430,8 @@ struct wl1271 {
 	/* Intermediate buffer, used for packet aggregation */
 	u8 *aggr_buf;
 
-	/* The target interrupt mask */
-	struct work_struct irq_work;
+	/* Network stack work  */
+	struct work_struct netstack_work;
 
 	/* Hardware recovery work */
 	struct work_struct recovery_work;
@@ -343,7 +458,6 @@ struct wl1271 {
 	 *	bits 16-23 - 802.11n   MCS index mask
 	 * support only 1 stream, thus only 8 bits for the MCS rates (0-7).
 	 */
-	u32 sta_rate_set;
 	u32 basic_rate_set;
 	u32 basic_rate;
 	u32 rate_set;
@@ -378,13 +492,12 @@ struct wl1271 {
 	int last_rssi_event;
 
 	struct wl1271_stats stats;
-	struct dentry *rootdir;
 
 	__le32 buffer_32;
 	u32 buffer_cmd;
 	u32 buffer_busyword[WL1271_BUSY_WORD_CNT];
 
-	struct wl1271_fw_status *fw_status;
+	struct wl1271_fw_full_status *fw_status;
 	struct wl1271_tx_hw_res_if *tx_res_if;
 
 	struct ieee80211_vif *vif;
@@ -400,6 +513,41 @@ struct wl1271 {
 
 	/* Most recently reported noise in dBm */
 	s8 noise;
+
+	/* map for HLIDs of associated stations - when operating in AP mode */
+	unsigned long ap_hlid_map[BITS_TO_LONGS(AP_MAX_STATIONS)];
+
+	/* recoreded keys for AP-mode - set here before AP startup */
+	struct wl1271_ap_key *recorded_ap_keys[MAX_NUM_KEYS];
+
+	/* bands supported by this instance of wl12xx */
+	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
+
+	/* RX BA constraint value */
+	bool ba_support;
+	u8 ba_rx_bitmap;
+
+	/*
+	 * AP-mode - links indexed by HLID. The global and broadcast links
+	 * are always active.
+	 */
+	struct wl1271_link links[AP_MAX_LINKS];
+
+	/* the hlid of the link where the last transmitted skb came from */
+	int last_tx_hlid;
+
+	/* AP-mode - a bitmap of links currently in PS mode according to FW */
+	u32 ap_fw_ps_map;
+
+	/* AP-mode - a bitmap of links currently in PS mode in mac80211 */
+	unsigned long ap_ps_map;
+
+	/* Quirks of specific hardware revisions */
+	unsigned int quirks;
+};
+
+struct wl1271_station {
+	u8 hlid;
 };
 
 int wl1271_plt_start(struct wl1271 *wl);
@@ -414,6 +562,8 @@ int wl1271_plt_stop(struct wl1271 *wl);
 #define WL1271_TX_QUEUE_LOW_WATERMARK  10
 #define WL1271_TX_QUEUE_HIGH_WATERMARK 25
 
+#define WL1271_DEFERRED_QUEUE_LIMIT    64
+
 /* WL1271 needs a 200ms sleep after power on, and a 20ms sleep before power
    on in case is has been shut down shortly before */
 #define WL1271_PRE_POWER_ON_SLEEP 20 /* in milliseconds */
@@ -423,4 +573,9 @@ int wl1271_plt_stop(struct wl1271 *wl);
 #define HW_BG_RATES_MASK	0xffff
 #define HW_HT_RATES_OFFSET	16
 
+/* Quirks */
+
+/* Each RX/TX transaction requires an end-of-transaction transfer */
+#define WL12XX_QUIRK_END_OF_TRANSACTION	BIT(0)
+
 #endif
diff --git a/drivers/net/wireless/wl12xx/wl12xx_80211.h b/drivers/net/wireless/wl12xx/wl12xx_80211.h
index be21032f4dc1..67dcf8f28cd3 100644
--- a/drivers/net/wireless/wl12xx/wl12xx_80211.h
+++ b/drivers/net/wireless/wl12xx/wl12xx_80211.h
@@ -138,13 +138,13 @@ struct wl12xx_arp_rsp_template {
 	struct ieee80211_hdr_3addr hdr;
 
 	u8 llc_hdr[sizeof(rfc1042_header)];
-	u16 llc_type;
+	__be16 llc_type;
 
 	struct arphdr arp_hdr;
 	u8 sender_hw[ETH_ALEN];
-	u32 sender_ip;
+	__be32 sender_ip;
 	u8 target_hw[ETH_ALEN];
-	u32 target_ip;
+	__be32 target_ip;
 } __packed;
 
 
@@ -160,4 +160,9 @@ struct wl12xx_probe_resp_template {
 	struct wl12xx_ie_country country;
 } __packed;
 
+struct wl12xx_disconn_template {
+	struct ieee80211_header header;
+	__le16 disconn_reason;
+} __packed;
+
 #endif
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.c b/drivers/net/wireless/zd1211rw/zd_chip.c
index 6a9b66051cf7..a73a305d3cba 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.c
+++ b/drivers/net/wireless/zd1211rw/zd_chip.c
@@ -108,25 +108,17 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
 {
 	int r;
 	int i;
-	zd_addr_t *a16;
-	u16 *v16;
+	zd_addr_t a16[USB_MAX_IOREAD32_COUNT * 2];
+	u16 v16[USB_MAX_IOREAD32_COUNT * 2];
 	unsigned int count16;
 
 	if (count > USB_MAX_IOREAD32_COUNT)
 		return -EINVAL;
 
-	/* Allocate a single memory block for values and addresses. */
-	count16 = 2*count;
-	/* zd_addr_t is __nocast, so the kmalloc needs an explicit cast */
-	a16 = (zd_addr_t *) kmalloc(count16 * (sizeof(zd_addr_t) + sizeof(u16)),
-		                   GFP_KERNEL);
-	if (!a16) {
-		dev_dbg_f(zd_chip_dev(chip),
-			  "error ENOMEM in allocation of a16\n");
-		r = -ENOMEM;
-		goto out;
-	}
-	v16 = (u16 *)(a16 + count16);
+	/* Use stack for values and addresses. */
+	count16 = 2 * count;
+	BUG_ON(count16 * sizeof(zd_addr_t) > sizeof(a16));
+	BUG_ON(count16 * sizeof(u16) > sizeof(v16));
 
 	for (i = 0; i < count; i++) {
 		int j = 2*i;
@@ -139,7 +131,7 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
 	if (r) {
 		dev_dbg_f(zd_chip_dev(chip),
 			  "error: zd_ioread16v_locked. Error number %d\n", r);
-		goto out;
+		return r;
 	}
 
 	for (i = 0; i < count; i++) {
@@ -147,18 +139,19 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
 		values[i] = (v16[j] << 16) | v16[j+1];
 	}
 
-out:
-	kfree((void *)a16);
-	return r;
+	return 0;
 }
 
-int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
-	           unsigned int count)
+static int _zd_iowrite32v_async_locked(struct zd_chip *chip,
+				       const struct zd_ioreq32 *ioreqs,
+				       unsigned int count)
 {
 	int i, j, r;
-	struct zd_ioreq16 *ioreqs16;
+	struct zd_ioreq16 ioreqs16[USB_MAX_IOWRITE32_COUNT * 2];
 	unsigned int count16;
 
+	/* Use stack for values and addresses. */
+
 	ZD_ASSERT(mutex_is_locked(&chip->mutex));
 
 	if (count == 0)
@@ -166,15 +159,8 @@ int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
 	if (count > USB_MAX_IOWRITE32_COUNT)
 		return -EINVAL;
 
-	/* Allocate a single memory block for values and addresses. */
-	count16 = 2*count;
-	ioreqs16 = kmalloc(count16 * sizeof(struct zd_ioreq16), GFP_KERNEL);
-	if (!ioreqs16) {
-		r = -ENOMEM;
-		dev_dbg_f(zd_chip_dev(chip),
-			  "error %d in ioreqs16 allocation\n", r);
-		goto out;
-	}
+	count16 = 2 * count;
+	BUG_ON(count16 * sizeof(struct zd_ioreq16) > sizeof(ioreqs16));
 
 	for (i = 0; i < count; i++) {
 		j = 2*i;
@@ -185,18 +171,30 @@ int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
 		ioreqs16[j+1].addr  = ioreqs[i].addr;
 	}
 
-	r = zd_usb_iowrite16v(&chip->usb, ioreqs16, count16);
+	r = zd_usb_iowrite16v_async(&chip->usb, ioreqs16, count16);
 #ifdef DEBUG
 	if (r) {
 		dev_dbg_f(zd_chip_dev(chip),
 			  "error %d in zd_usb_write16v\n", r);
 	}
 #endif /* DEBUG */
-out:
-	kfree(ioreqs16);
 	return r;
 }
 
+int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
+			  unsigned int count)
+{
+	int r;
+
+	zd_usb_iowrite16v_async_start(&chip->usb);
+	r = _zd_iowrite32v_async_locked(chip, ioreqs, count);
+	if (r) {
+		zd_usb_iowrite16v_async_end(&chip->usb, 0);
+		return r;
+	}
+	return zd_usb_iowrite16v_async_end(&chip->usb, 50 /* ms */);
+}
+
 int zd_iowrite16a_locked(struct zd_chip *chip,
                   const struct zd_ioreq16 *ioreqs, unsigned int count)
 {
@@ -204,6 +202,8 @@ int zd_iowrite16a_locked(struct zd_chip *chip,
 	unsigned int i, j, t, max;
 
 	ZD_ASSERT(mutex_is_locked(&chip->mutex));
+	zd_usb_iowrite16v_async_start(&chip->usb);
+
 	for (i = 0; i < count; i += j + t) {
 		t = 0;
 		max = count-i;
@@ -216,8 +216,9 @@ int zd_iowrite16a_locked(struct zd_chip *chip,
 			}
 		}
 
-		r = zd_usb_iowrite16v(&chip->usb, &ioreqs[i], j);
+		r = zd_usb_iowrite16v_async(&chip->usb, &ioreqs[i], j);
 		if (r) {
+			zd_usb_iowrite16v_async_end(&chip->usb, 0);
 			dev_dbg_f(zd_chip_dev(chip),
 				  "error zd_usb_iowrite16v. Error number %d\n",
 				  r);
@@ -225,7 +226,7 @@ int zd_iowrite16a_locked(struct zd_chip *chip,
 		}
 	}
 
-	return 0;
+	return zd_usb_iowrite16v_async_end(&chip->usb, 50 /* ms */);
 }
 
 /* Writes a variable number of 32 bit registers. The functions will split
@@ -238,6 +239,8 @@ int zd_iowrite32a_locked(struct zd_chip *chip,
 	int r;
 	unsigned int i, j, t, max;
 
+	zd_usb_iowrite16v_async_start(&chip->usb);
+
 	for (i = 0; i < count; i += j + t) {
 		t = 0;
 		max = count-i;
@@ -250,8 +253,9 @@ int zd_iowrite32a_locked(struct zd_chip *chip,
 			}
 		}
 
-		r = _zd_iowrite32v_locked(chip, &ioreqs[i], j);
+		r = _zd_iowrite32v_async_locked(chip, &ioreqs[i], j);
 		if (r) {
+			zd_usb_iowrite16v_async_end(&chip->usb, 0);
 			dev_dbg_f(zd_chip_dev(chip),
 				"error _zd_iowrite32v_locked."
 				" Error number %d\n", r);
@@ -259,7 +263,7 @@ int zd_iowrite32a_locked(struct zd_chip *chip,
 		}
 	}
 
-	return 0;
+	return zd_usb_iowrite16v_async_end(&chip->usb, 50 /* ms */);
 }
 
 int zd_ioread16(struct zd_chip *chip, zd_addr_t addr, u16 *value)
@@ -370,16 +374,12 @@ error:
 	return r;
 }
 
-/* MAC address: if custom mac addresses are to be used CR_MAC_ADDR_P1 and
- *              CR_MAC_ADDR_P2 must be overwritten
- */
-int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
+static int zd_write_mac_addr_common(struct zd_chip *chip, const u8 *mac_addr,
+				    const struct zd_ioreq32 *in_reqs,
+				    const char *type)
 {
 	int r;
-	struct zd_ioreq32 reqs[2] = {
-		[0] = { .addr = CR_MAC_ADDR_P1 },
-		[1] = { .addr = CR_MAC_ADDR_P2 },
-	};
+	struct zd_ioreq32 reqs[2] = {in_reqs[0], in_reqs[1]};
 
 	if (mac_addr) {
 		reqs[0].value = (mac_addr[3] << 24)
@@ -388,9 +388,9 @@ int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
 			      |  mac_addr[0];
 		reqs[1].value = (mac_addr[5] <<  8)
 			      |  mac_addr[4];
-		dev_dbg_f(zd_chip_dev(chip), "mac addr %pM\n", mac_addr);
+		dev_dbg_f(zd_chip_dev(chip), "%s addr %pM\n", type, mac_addr);
 	} else {
-		dev_dbg_f(zd_chip_dev(chip), "set NULL mac\n");
+		dev_dbg_f(zd_chip_dev(chip), "set NULL %s\n", type);
 	}
 
 	mutex_lock(&chip->mutex);
@@ -399,6 +399,29 @@ int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
 	return r;
 }
 
+/* MAC address: if custom mac addresses are to be used CR_MAC_ADDR_P1 and
+ *              CR_MAC_ADDR_P2 must be overwritten
+ */
+int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
+{
+	static const struct zd_ioreq32 reqs[2] = {
+		[0] = { .addr = CR_MAC_ADDR_P1 },
+		[1] = { .addr = CR_MAC_ADDR_P2 },
+	};
+
+	return zd_write_mac_addr_common(chip, mac_addr, reqs, "mac");
+}
+
+int zd_write_bssid(struct zd_chip *chip, const u8 *bssid)
+{
+	static const struct zd_ioreq32 reqs[2] = {
+		[0] = { .addr = CR_BSSID_P1 },
+		[1] = { .addr = CR_BSSID_P2 },
+	};
+
+	return zd_write_mac_addr_common(chip, bssid, reqs, "bssid");
+}
+
 int zd_read_regdomain(struct zd_chip *chip, u8 *regdomain)
 {
 	int r;
@@ -849,11 +872,12 @@ static int get_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 static int set_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 {
 	struct zd_ioreq32 reqs[3];
+	u16 b_interval = s->beacon_interval & 0xffff;
 
-	if (s->beacon_interval <= 5)
-		s->beacon_interval = 5;
-	if (s->pre_tbtt < 4 || s->pre_tbtt >= s->beacon_interval)
-		s->pre_tbtt = s->beacon_interval - 1;
+	if (b_interval <= 5)
+		b_interval = 5;
+	if (s->pre_tbtt < 4 || s->pre_tbtt >= b_interval)
+		s->pre_tbtt = b_interval - 1;
 	if (s->atim_wnd_period >= s->pre_tbtt)
 		s->atim_wnd_period = s->pre_tbtt - 1;
 
@@ -862,31 +886,57 @@ static int set_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 	reqs[1].addr = CR_PRE_TBTT;
 	reqs[1].value = s->pre_tbtt;
 	reqs[2].addr = CR_BCN_INTERVAL;
-	reqs[2].value = s->beacon_interval;
+	reqs[2].value = (s->beacon_interval & ~0xffff) | b_interval;
 
 	return zd_iowrite32a_locked(chip, reqs, ARRAY_SIZE(reqs));
 }
 
 
-static int set_beacon_interval(struct zd_chip *chip, u32 interval)
+static int set_beacon_interval(struct zd_chip *chip, u16 interval,
+			       u8 dtim_period, int type)
 {
 	int r;
 	struct aw_pt_bi s;
+	u32 b_interval, mode_flag;
 
 	ZD_ASSERT(mutex_is_locked(&chip->mutex));
+
+	if (interval > 0) {
+		switch (type) {
+		case NL80211_IFTYPE_ADHOC:
+		case NL80211_IFTYPE_MESH_POINT:
+			mode_flag = BCN_MODE_IBSS;
+			break;
+		case NL80211_IFTYPE_AP:
+			mode_flag = BCN_MODE_AP;
+			break;
+		default:
+			mode_flag = 0;
+			break;
+		}
+	} else {
+		dtim_period = 0;
+		mode_flag = 0;
+	}
+
+	b_interval = mode_flag | (dtim_period << 16) | interval;
+
+	r = zd_iowrite32_locked(chip, b_interval, CR_BCN_INTERVAL);
+	if (r)
+		return r;
 	r = get_aw_pt_bi(chip, &s);
 	if (r)
 		return r;
-	s.beacon_interval = interval;
 	return set_aw_pt_bi(chip, &s);
 }
 
-int zd_set_beacon_interval(struct zd_chip *chip, u32 interval)
+int zd_set_beacon_interval(struct zd_chip *chip, u16 interval, u8 dtim_period,
+			   int type)
 {
 	int r;
 
 	mutex_lock(&chip->mutex);
-	r = set_beacon_interval(chip, interval);
+	r = set_beacon_interval(chip, interval, dtim_period, type);
 	mutex_unlock(&chip->mutex);
 	return r;
 }
@@ -905,7 +955,7 @@ static int hw_init(struct zd_chip *chip)
 	if (r)
 		return r;
 
-	return set_beacon_interval(chip, 100);
+	return set_beacon_interval(chip, 100, 0, NL80211_IFTYPE_UNSPECIFIED);
 }
 
 static zd_addr_t fw_reg_addr(struct zd_chip *chip, u16 offset)
@@ -1407,6 +1457,9 @@ void zd_chip_disable_int(struct zd_chip *chip)
 	mutex_lock(&chip->mutex);
 	zd_usb_disable_int(&chip->usb);
 	mutex_unlock(&chip->mutex);
+
+	/* cancel pending interrupt work */
+	cancel_work_sync(&zd_chip_to_mac(chip)->process_intr);
 }
 
 int zd_chip_enable_rxtx(struct zd_chip *chip)
@@ -1416,6 +1469,7 @@ int zd_chip_enable_rxtx(struct zd_chip *chip)
 	mutex_lock(&chip->mutex);
 	zd_usb_enable_tx(&chip->usb);
 	r = zd_usb_enable_rx(&chip->usb);
+	zd_tx_watchdog_enable(&chip->usb);
 	mutex_unlock(&chip->mutex);
 	return r;
 }
@@ -1423,6 +1477,7 @@ int zd_chip_enable_rxtx(struct zd_chip *chip)
 void zd_chip_disable_rxtx(struct zd_chip *chip)
 {
 	mutex_lock(&chip->mutex);
+	zd_tx_watchdog_disable(&chip->usb);
 	zd_usb_disable_rx(&chip->usb);
 	zd_usb_disable_tx(&chip->usb);
 	mutex_unlock(&chip->mutex);
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.h b/drivers/net/wireless/zd1211rw/zd_chip.h
index f8bbf7d302ae..14e4402a6111 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.h
+++ b/drivers/net/wireless/zd1211rw/zd_chip.h
@@ -546,6 +546,7 @@ enum {
 #define RX_FILTER_CTRL (RX_FILTER_RTS | RX_FILTER_CTS | \
 	RX_FILTER_CFEND | RX_FILTER_CFACK)
 
+#define BCN_MODE_AP			0x1000000
 #define BCN_MODE_IBSS			0x2000000
 
 /* Monitor mode sets filter to 0xfffff */
@@ -881,6 +882,7 @@ static inline u8 _zd_chip_get_channel(struct zd_chip *chip)
 u8  zd_chip_get_channel(struct zd_chip *chip);
 int zd_read_regdomain(struct zd_chip *chip, u8 *regdomain);
 int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr);
+int zd_write_bssid(struct zd_chip *chip, const u8 *bssid);
 int zd_chip_switch_radio_on(struct zd_chip *chip);
 int zd_chip_switch_radio_off(struct zd_chip *chip);
 int zd_chip_enable_int(struct zd_chip *chip);
@@ -920,7 +922,8 @@ enum led_status {
 
 int zd_chip_control_leds(struct zd_chip *chip, enum led_status status);
 
-int zd_set_beacon_interval(struct zd_chip *chip, u32 interval);
+int zd_set_beacon_interval(struct zd_chip *chip, u16 interval, u8 dtim_period,
+			   int type);
 
 static inline int zd_get_beacon_interval(struct zd_chip *chip, u32 *interval)
 {
diff --git a/drivers/net/wireless/zd1211rw/zd_def.h b/drivers/net/wireless/zd1211rw/zd_def.h
index 6ac597ffd3b9..5463ca9ebc01 100644
--- a/drivers/net/wireless/zd1211rw/zd_def.h
+++ b/drivers/net/wireless/zd1211rw/zd_def.h
@@ -45,7 +45,7 @@ typedef u16 __nocast zd_addr_t;
 #ifdef DEBUG
 #  define ZD_ASSERT(x) \
 do { \
-	if (!(x)) { \
+	if (unlikely(!(x))) { \
 		pr_debug("%s:%d ASSERT %s VIOLATED!\n", \
 			__FILE__, __LINE__, __stringify(x)); \
 		dump_stack(); \
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index 6107304cb94c..5037c8b2b415 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -138,6 +138,12 @@ static const struct ieee80211_channel zd_channels[] = {
 static void housekeeping_init(struct zd_mac *mac);
 static void housekeeping_enable(struct zd_mac *mac);
 static void housekeeping_disable(struct zd_mac *mac);
+static void beacon_init(struct zd_mac *mac);
+static void beacon_enable(struct zd_mac *mac);
+static void beacon_disable(struct zd_mac *mac);
+static void set_rts_cts(struct zd_mac *mac, unsigned int short_preamble);
+static int zd_mac_config_beacon(struct ieee80211_hw *hw,
+				struct sk_buff *beacon);
 
 static int zd_reg2alpha2(u8 regdomain, char *alpha2)
 {
@@ -231,6 +237,26 @@ static int set_rx_filter(struct zd_mac *mac)
 	return zd_iowrite32(&mac->chip, CR_RX_FILTER, filter);
 }
 
+static int set_mac_and_bssid(struct zd_mac *mac)
+{
+	int r;
+
+	if (!mac->vif)
+		return -1;
+
+	r = zd_write_mac_addr(&mac->chip, mac->vif->addr);
+	if (r)
+		return r;
+
+	/* Vendor driver after setting MAC either sets BSSID for AP or
+	 * filter for other modes.
+	 */
+	if (mac->type != NL80211_IFTYPE_AP)
+		return set_rx_filter(mac);
+	else
+		return zd_write_bssid(&mac->chip, mac->vif->addr);
+}
+
 static int set_mc_hash(struct zd_mac *mac)
 {
 	struct zd_mc_hash hash;
@@ -238,7 +264,7 @@ static int set_mc_hash(struct zd_mac *mac)
 	return zd_chip_set_multicast_hash(&mac->chip, &hash);
 }
 
-static int zd_op_start(struct ieee80211_hw *hw)
+int zd_op_start(struct ieee80211_hw *hw)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
 	struct zd_chip *chip = &mac->chip;
@@ -275,6 +301,8 @@ static int zd_op_start(struct ieee80211_hw *hw)
 		goto disable_rxtx;
 
 	housekeeping_enable(mac);
+	beacon_enable(mac);
+	set_bit(ZD_DEVICE_RUNNING, &mac->flags);
 	return 0;
 disable_rxtx:
 	zd_chip_disable_rxtx(chip);
@@ -286,19 +314,22 @@ out:
 	return r;
 }
 
-static void zd_op_stop(struct ieee80211_hw *hw)
+void zd_op_stop(struct ieee80211_hw *hw)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
 	struct zd_chip *chip = &mac->chip;
 	struct sk_buff *skb;
 	struct sk_buff_head *ack_wait_queue = &mac->ack_wait_queue;
 
+	clear_bit(ZD_DEVICE_RUNNING, &mac->flags);
+
 	/* The order here deliberately is a little different from the open()
 	 * method, since we need to make sure there is no opportunity for RX
 	 * frames to be processed by mac80211 after we have stopped it.
 	 */
 
 	zd_chip_disable_rxtx(chip);
+	beacon_disable(mac);
 	housekeeping_disable(mac);
 	flush_workqueue(zd_workqueue);
 
@@ -311,6 +342,68 @@ static void zd_op_stop(struct ieee80211_hw *hw)
 		dev_kfree_skb_any(skb);
 }
 
+int zd_restore_settings(struct zd_mac *mac)
+{
+	struct sk_buff *beacon;
+	struct zd_mc_hash multicast_hash;
+	unsigned int short_preamble;
+	int r, beacon_interval, beacon_period;
+	u8 channel;
+
+	dev_dbg_f(zd_mac_dev(mac), "\n");
+
+	spin_lock_irq(&mac->lock);
+	multicast_hash = mac->multicast_hash;
+	short_preamble = mac->short_preamble;
+	beacon_interval = mac->beacon.interval;
+	beacon_period = mac->beacon.period;
+	channel = mac->channel;
+	spin_unlock_irq(&mac->lock);
+
+	r = set_mac_and_bssid(mac);
+	if (r < 0) {
+		dev_dbg_f(zd_mac_dev(mac), "set_mac_and_bssid failed, %d\n", r);
+		return r;
+	}
+
+	r = zd_chip_set_channel(&mac->chip, channel);
+	if (r < 0) {
+		dev_dbg_f(zd_mac_dev(mac), "zd_chip_set_channel failed, %d\n",
+			  r);
+		return r;
+	}
+
+	set_rts_cts(mac, short_preamble);
+
+	r = zd_chip_set_multicast_hash(&mac->chip, &multicast_hash);
+	if (r < 0) {
+		dev_dbg_f(zd_mac_dev(mac),
+			  "zd_chip_set_multicast_hash failed, %d\n", r);
+		return r;
+	}
+
+	if (mac->type == NL80211_IFTYPE_MESH_POINT ||
+	    mac->type == NL80211_IFTYPE_ADHOC ||
+	    mac->type == NL80211_IFTYPE_AP) {
+		if (mac->vif != NULL) {
+			beacon = ieee80211_beacon_get(mac->hw, mac->vif);
+			if (beacon) {
+				zd_mac_config_beacon(mac->hw, beacon);
+				kfree_skb(beacon);
+			}
+		}
+
+		zd_set_beacon_interval(&mac->chip, beacon_interval,
+					beacon_period, mac->type);
+
+		spin_lock_irq(&mac->lock);
+		mac->beacon.last_update = jiffies;
+		spin_unlock_irq(&mac->lock);
+	}
+
+	return 0;
+}
+
 /**
  * zd_mac_tx_status - reports tx status of a packet if required
  * @hw - a &struct ieee80211_hw pointer
@@ -574,64 +667,120 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
 static int zd_mac_config_beacon(struct ieee80211_hw *hw, struct sk_buff *beacon)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
-	int r;
+	int r, ret, num_cmds, req_pos = 0;
 	u32 tmp, j = 0;
 	/* 4 more bytes for tail CRC */
 	u32 full_len = beacon->len + 4;
+	unsigned long end_jiffies, message_jiffies;
+	struct zd_ioreq32 *ioreqs;
 
-	r = zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 0);
+	/* Alloc memory for full beacon write at once. */
+	num_cmds = 1 + zd_chip_is_zd1211b(&mac->chip) + full_len;
+	ioreqs = kmalloc(num_cmds * sizeof(struct zd_ioreq32), GFP_KERNEL);
+	if (!ioreqs)
+		return -ENOMEM;
+
+	mutex_lock(&mac->chip.mutex);
+
+	r = zd_iowrite32_locked(&mac->chip, 0, CR_BCN_FIFO_SEMAPHORE);
 	if (r < 0)
-		return r;
-	r = zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+		goto out;
+	r = zd_ioread32_locked(&mac->chip, &tmp, CR_BCN_FIFO_SEMAPHORE);
 	if (r < 0)
-		return r;
+		goto release_sema;
 
+	end_jiffies = jiffies + HZ / 2; /*~500ms*/
+	message_jiffies = jiffies + HZ / 10; /*~100ms*/
 	while (tmp & 0x2) {
-		r = zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+		r = zd_ioread32_locked(&mac->chip, &tmp, CR_BCN_FIFO_SEMAPHORE);
 		if (r < 0)
-			return r;
-		if ((++j % 100) == 0) {
-			printk(KERN_ERR "CR_BCN_FIFO_SEMAPHORE not ready\n");
-			if (j >= 500)  {
-				printk(KERN_ERR "Giving up beacon config.\n");
-				return -ETIMEDOUT;
+			goto release_sema;
+		if (time_is_before_eq_jiffies(message_jiffies)) {
+			message_jiffies = jiffies + HZ / 10;
+			dev_err(zd_mac_dev(mac),
+					"CR_BCN_FIFO_SEMAPHORE not ready\n");
+			if (time_is_before_eq_jiffies(end_jiffies))  {
+				dev_err(zd_mac_dev(mac),
+						"Giving up beacon config.\n");
+				r = -ETIMEDOUT;
+				goto reset_device;
 			}
 		}
-		msleep(1);
+		msleep(20);
 	}
 
-	r = zd_iowrite32(&mac->chip, CR_BCN_FIFO, full_len - 1);
-	if (r < 0)
-		return r;
+	ioreqs[req_pos].addr = CR_BCN_FIFO;
+	ioreqs[req_pos].value = full_len - 1;
+	req_pos++;
 	if (zd_chip_is_zd1211b(&mac->chip)) {
-		r = zd_iowrite32(&mac->chip, CR_BCN_LENGTH, full_len - 1);
-		if (r < 0)
-			return r;
+		ioreqs[req_pos].addr = CR_BCN_LENGTH;
+		ioreqs[req_pos].value = full_len - 1;
+		req_pos++;
 	}
 
 	for (j = 0 ; j < beacon->len; j++) {
-		r = zd_iowrite32(&mac->chip, CR_BCN_FIFO,
-				*((u8 *)(beacon->data + j)));
-		if (r < 0)
-			return r;
+		ioreqs[req_pos].addr = CR_BCN_FIFO;
+		ioreqs[req_pos].value = *((u8 *)(beacon->data + j));
+		req_pos++;
 	}
 
 	for (j = 0; j < 4; j++) {
-		r = zd_iowrite32(&mac->chip, CR_BCN_FIFO, 0x0);
-		if (r < 0)
-			return r;
+		ioreqs[req_pos].addr = CR_BCN_FIFO;
+		ioreqs[req_pos].value = 0x0;
+		req_pos++;
 	}
 
-	r = zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 1);
-	if (r < 0)
-		return r;
+	BUG_ON(req_pos != num_cmds);
+
+	r = zd_iowrite32a_locked(&mac->chip, ioreqs, num_cmds);
+
+release_sema:
+	/*
+	 * Try very hard to release device beacon semaphore, as otherwise
+	 * device/driver can be left in unusable state.
+	 */
+	end_jiffies = jiffies + HZ / 2; /*~500ms*/
+	ret = zd_iowrite32_locked(&mac->chip, 1, CR_BCN_FIFO_SEMAPHORE);
+	while (ret < 0) {
+		if (time_is_before_eq_jiffies(end_jiffies)) {
+			ret = -ETIMEDOUT;
+			break;
+		}
+
+		msleep(20);
+		ret = zd_iowrite32_locked(&mac->chip, 1, CR_BCN_FIFO_SEMAPHORE);
+	}
+
+	if (ret < 0)
+		dev_err(zd_mac_dev(mac), "Could not release "
+					 "CR_BCN_FIFO_SEMAPHORE!\n");
+	if (r < 0 || ret < 0) {
+		if (r >= 0)
+			r = ret;
+		goto out;
+	}
 
 	/* 802.11b/g 2.4G CCK 1Mb
 	 * 802.11a, not yet implemented, uses different values (see GPL vendor
 	 * driver)
 	 */
-	return zd_iowrite32(&mac->chip, CR_BCN_PLCP_CFG, 0x00000400 |
-			(full_len << 19));
+	r = zd_iowrite32_locked(&mac->chip, 0x00000400 | (full_len << 19),
+				CR_BCN_PLCP_CFG);
+out:
+	mutex_unlock(&mac->chip.mutex);
+	kfree(ioreqs);
+	return r;
+
+reset_device:
+	mutex_unlock(&mac->chip.mutex);
+	kfree(ioreqs);
+
+	/* semaphore stuck, reset device to avoid fw freeze later */
+	dev_warn(zd_mac_dev(mac), "CR_BCN_FIFO_SEMAPHORE stuck, "
+				  "reseting device...");
+	usb_queue_reset_device(mac->chip.usb.intf);
+
+	return r;
 }
 
 static int fill_ctrlset(struct zd_mac *mac,
@@ -701,7 +850,7 @@ static int fill_ctrlset(struct zd_mac *mac,
  * control block of the skbuff will be initialized. If necessary the incoming
  * mac80211 queues will be stopped.
  */
-static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -716,11 +865,10 @@ static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	r = zd_usb_tx(&mac->chip.usb, skb);
 	if (r)
 		goto fail;
-	return 0;
+	return;
 
 fail:
 	dev_kfree_skb(skb);
-	return 0;
 }
 
 /**
@@ -779,6 +927,13 @@ static int filter_ack(struct ieee80211_hw *hw, struct ieee80211_hdr *rx_hdr,
 
 		mac->ack_pending = 1;
 		mac->ack_signal = stats->signal;
+
+		/* Prevent pending tx-packet on AP-mode */
+		if (mac->type == NL80211_IFTYPE_AP) {
+			skb = __skb_dequeue(q);
+			zd_mac_tx_status(hw, skb, mac->ack_signal, NULL);
+			mac->ack_pending = 0;
+		}
 	}
 
 	spin_unlock_irqrestore(&q->lock, flags);
@@ -882,13 +1037,16 @@ static int zd_op_add_interface(struct ieee80211_hw *hw,
 	case NL80211_IFTYPE_MESH_POINT:
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_AP:
 		mac->type = vif->type;
 		break;
 	default:
 		return -EOPNOTSUPP;
 	}
 
-	return zd_write_mac_addr(&mac->chip, vif->addr);
+	mac->vif = vif;
+
+	return set_mac_and_bssid(mac);
 }
 
 static void zd_op_remove_interface(struct ieee80211_hw *hw,
@@ -896,7 +1054,8 @@ static void zd_op_remove_interface(struct ieee80211_hw *hw,
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
 	mac->type = NL80211_IFTYPE_UNSPECIFIED;
-	zd_set_beacon_interval(&mac->chip, 0);
+	mac->vif = NULL;
+	zd_set_beacon_interval(&mac->chip, 0, 0, NL80211_IFTYPE_UNSPECIFIED);
 	zd_write_mac_addr(&mac->chip, NULL);
 }
 
@@ -905,49 +1064,67 @@ static int zd_op_config(struct ieee80211_hw *hw, u32 changed)
 	struct zd_mac *mac = zd_hw_mac(hw);
 	struct ieee80211_conf *conf = &hw->conf;
 
+	spin_lock_irq(&mac->lock);
+	mac->channel = conf->channel->hw_value;
+	spin_unlock_irq(&mac->lock);
+
 	return zd_chip_set_channel(&mac->chip, conf->channel->hw_value);
 }
 
-static void zd_process_intr(struct work_struct *work)
+static void zd_beacon_done(struct zd_mac *mac)
 {
-	u16 int_status;
-	struct zd_mac *mac = container_of(work, struct zd_mac, process_intr);
+	struct sk_buff *skb, *beacon;
 
-	int_status = le16_to_cpu(*(__le16 *)(mac->intr_buffer+4));
-	if (int_status & INT_CFG_NEXT_BCN)
-		dev_dbg_f_limit(zd_mac_dev(mac), "INT_CFG_NEXT_BCN\n");
-	else
-		dev_dbg_f(zd_mac_dev(mac), "Unsupported interrupt\n");
-
-	zd_chip_enable_hwint(&mac->chip);
-}
+	if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+		return;
+	if (!mac->vif || mac->vif->type != NL80211_IFTYPE_AP)
+		return;
 
+	/*
+	 * Send out buffered broad- and multicast frames.
+	 */
+	while (!ieee80211_queue_stopped(mac->hw, 0)) {
+		skb = ieee80211_get_buffered_bc(mac->hw, mac->vif);
+		if (!skb)
+			break;
+		zd_op_tx(mac->hw, skb);
+	}
 
-static void set_multicast_hash_handler(struct work_struct *work)
-{
-	struct zd_mac *mac =
-		container_of(work, struct zd_mac, set_multicast_hash_work);
-	struct zd_mc_hash hash;
+	/*
+	 * Fetch next beacon so that tim_count is updated.
+	 */
+	beacon = ieee80211_beacon_get(mac->hw, mac->vif);
+	if (beacon) {
+		zd_mac_config_beacon(mac->hw, beacon);
+		kfree_skb(beacon);
+	}
 
 	spin_lock_irq(&mac->lock);
-	hash = mac->multicast_hash;
+	mac->beacon.last_update = jiffies;
 	spin_unlock_irq(&mac->lock);
-
-	zd_chip_set_multicast_hash(&mac->chip, &hash);
 }
 
-static void set_rx_filter_handler(struct work_struct *work)
+static void zd_process_intr(struct work_struct *work)
 {
-	struct zd_mac *mac =
-		container_of(work, struct zd_mac, set_rx_filter_work);
-	int r;
+	u16 int_status;
+	unsigned long flags;
+	struct zd_mac *mac = container_of(work, struct zd_mac, process_intr);
 
-	dev_dbg_f(zd_mac_dev(mac), "\n");
-	r = set_rx_filter(mac);
-	if (r)
-		dev_err(zd_mac_dev(mac), "set_rx_filter_handler error %d\n", r);
+	spin_lock_irqsave(&mac->lock, flags);
+	int_status = le16_to_cpu(*(__le16 *)(mac->intr_buffer + 4));
+	spin_unlock_irqrestore(&mac->lock, flags);
+
+	if (int_status & INT_CFG_NEXT_BCN) {
+		/*dev_dbg_f_limit(zd_mac_dev(mac), "INT_CFG_NEXT_BCN\n");*/
+		zd_beacon_done(mac);
+	} else {
+		dev_dbg_f(zd_mac_dev(mac), "Unsupported interrupt\n");
+	}
+
+	zd_chip_enable_hwint(&mac->chip);
 }
 
+
 static u64 zd_op_prepare_multicast(struct ieee80211_hw *hw,
 				   struct netdev_hw_addr_list *mc_list)
 {
@@ -979,6 +1156,7 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
 	};
 	struct zd_mac *mac = zd_hw_mac(hw);
 	unsigned long flags;
+	int r;
 
 	/* Only deal with supported flags */
 	changed_flags &= SUPPORTED_FIF_FLAGS;
@@ -1000,11 +1178,13 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
 	mac->multicast_hash = hash;
 	spin_unlock_irqrestore(&mac->lock, flags);
 
-	/* XXX: these can be called here now, can sleep now! */
-	queue_work(zd_workqueue, &mac->set_multicast_hash_work);
+	zd_chip_set_multicast_hash(&mac->chip, &hash);
 
-	if (changed_flags & FIF_CONTROL)
-		queue_work(zd_workqueue, &mac->set_rx_filter_work);
+	if (changed_flags & FIF_CONTROL) {
+		r = set_rx_filter(mac);
+		if (r)
+			dev_err(zd_mac_dev(mac), "set_rx_filter error %d\n", r);
+	}
 
 	/* no handling required for FIF_OTHER_BSS as we don't currently
 	 * do BSSID filtering */
@@ -1016,20 +1196,9 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
 	 * time. */
 }
 
-static void set_rts_cts_work(struct work_struct *work)
+static void set_rts_cts(struct zd_mac *mac, unsigned int short_preamble)
 {
-	struct zd_mac *mac =
-		container_of(work, struct zd_mac, set_rts_cts_work);
-	unsigned long flags;
-	unsigned int short_preamble;
-
 	mutex_lock(&mac->chip.mutex);
-
-	spin_lock_irqsave(&mac->lock, flags);
-	mac->updating_rts_rate = 0;
-	short_preamble = mac->short_preamble;
-	spin_unlock_irqrestore(&mac->lock, flags);
-
 	zd_chip_set_rts_cts_rate_locked(&mac->chip, short_preamble);
 	mutex_unlock(&mac->chip.mutex);
 }
@@ -1040,33 +1209,42 @@ static void zd_op_bss_info_changed(struct ieee80211_hw *hw,
 				   u32 changes)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
-	unsigned long flags;
 	int associated;
 
 	dev_dbg_f(zd_mac_dev(mac), "changes: %x\n", changes);
 
 	if (mac->type == NL80211_IFTYPE_MESH_POINT ||
-	    mac->type == NL80211_IFTYPE_ADHOC) {
+	    mac->type == NL80211_IFTYPE_ADHOC ||
+	    mac->type == NL80211_IFTYPE_AP) {
 		associated = true;
 		if (changes & BSS_CHANGED_BEACON) {
 			struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
 
 			if (beacon) {
+				zd_chip_disable_hwint(&mac->chip);
 				zd_mac_config_beacon(hw, beacon);
+				zd_chip_enable_hwint(&mac->chip);
 				kfree_skb(beacon);
 			}
 		}
 
 		if (changes & BSS_CHANGED_BEACON_ENABLED) {
-			u32 interval;
+			u16 interval = 0;
+			u8 period = 0;
 
-			if (bss_conf->enable_beacon)
-				interval = BCN_MODE_IBSS |
-						bss_conf->beacon_int;
-			else
-				interval = 0;
+			if (bss_conf->enable_beacon) {
+				period = bss_conf->dtim_period;
+				interval = bss_conf->beacon_int;
+			}
 
-			zd_set_beacon_interval(&mac->chip, interval);
+			spin_lock_irq(&mac->lock);
+			mac->beacon.period = period;
+			mac->beacon.interval = interval;
+			mac->beacon.last_update = jiffies;
+			spin_unlock_irq(&mac->lock);
+
+			zd_set_beacon_interval(&mac->chip, interval, period,
+					       mac->type);
 		}
 	} else
 		associated = is_valid_ether_addr(bss_conf->bssid);
@@ -1078,15 +1256,11 @@ static void zd_op_bss_info_changed(struct ieee80211_hw *hw,
 	/* TODO: do hardware bssid filtering */
 
 	if (changes & BSS_CHANGED_ERP_PREAMBLE) {
-		spin_lock_irqsave(&mac->lock, flags);
+		spin_lock_irq(&mac->lock);
 		mac->short_preamble = bss_conf->use_short_preamble;
-		if (!mac->updating_rts_rate) {
-			mac->updating_rts_rate = 1;
-			/* FIXME: should disable TX here, until work has
-			 * completed and RTS_CTS reg is updated */
-			queue_work(zd_workqueue, &mac->set_rts_cts_work);
-		}
-		spin_unlock_irqrestore(&mac->lock, flags);
+		spin_unlock_irq(&mac->lock);
+
+		set_rts_cts(mac, bss_conf->use_short_preamble);
 	}
 }
 
@@ -1138,12 +1312,14 @@ struct ieee80211_hw *zd_mac_alloc_hw(struct usb_interface *intf)
 	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &mac->band;
 
 	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		    IEEE80211_HW_SIGNAL_UNSPEC;
+		    IEEE80211_HW_SIGNAL_UNSPEC |
+		    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
 
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_MESH_POINT) |
 		BIT(NL80211_IFTYPE_STATION) |
-		BIT(NL80211_IFTYPE_ADHOC);
+		BIT(NL80211_IFTYPE_ADHOC) |
+		BIT(NL80211_IFTYPE_AP);
 
 	hw->max_signal = 100;
 	hw->queues = 1;
@@ -1160,15 +1336,82 @@ struct ieee80211_hw *zd_mac_alloc_hw(struct usb_interface *intf)
 
 	zd_chip_init(&mac->chip, hw, intf);
 	housekeeping_init(mac);
-	INIT_WORK(&mac->set_multicast_hash_work, set_multicast_hash_handler);
-	INIT_WORK(&mac->set_rts_cts_work, set_rts_cts_work);
-	INIT_WORK(&mac->set_rx_filter_work, set_rx_filter_handler);
+	beacon_init(mac);
 	INIT_WORK(&mac->process_intr, zd_process_intr);
 
 	SET_IEEE80211_DEV(hw, &intf->dev);
 	return hw;
 }
 
+#define BEACON_WATCHDOG_DELAY round_jiffies_relative(HZ)
+
+static void beacon_watchdog_handler(struct work_struct *work)
+{
+	struct zd_mac *mac =
+		container_of(work, struct zd_mac, beacon.watchdog_work.work);
+	struct sk_buff *beacon;
+	unsigned long timeout;
+	int interval, period;
+
+	if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+		goto rearm;
+	if (mac->type != NL80211_IFTYPE_AP || !mac->vif)
+		goto rearm;
+
+	spin_lock_irq(&mac->lock);
+	interval = mac->beacon.interval;
+	period = mac->beacon.period;
+	timeout = mac->beacon.last_update + msecs_to_jiffies(interval) + HZ;
+	spin_unlock_irq(&mac->lock);
+
+	if (interval > 0 && time_is_before_jiffies(timeout)) {
+		dev_dbg_f(zd_mac_dev(mac), "beacon interrupt stalled, "
+					   "restarting. "
+					   "(interval: %d, dtim: %d)\n",
+					   interval, period);
+
+		zd_chip_disable_hwint(&mac->chip);
+
+		beacon = ieee80211_beacon_get(mac->hw, mac->vif);
+		if (beacon) {
+			zd_mac_config_beacon(mac->hw, beacon);
+			kfree_skb(beacon);
+		}
+
+		zd_set_beacon_interval(&mac->chip, interval, period, mac->type);
+
+		zd_chip_enable_hwint(&mac->chip);
+
+		spin_lock_irq(&mac->lock);
+		mac->beacon.last_update = jiffies;
+		spin_unlock_irq(&mac->lock);
+	}
+
+rearm:
+	queue_delayed_work(zd_workqueue, &mac->beacon.watchdog_work,
+			   BEACON_WATCHDOG_DELAY);
+}
+
+static void beacon_init(struct zd_mac *mac)
+{
+	INIT_DELAYED_WORK(&mac->beacon.watchdog_work, beacon_watchdog_handler);
+}
+
+static void beacon_enable(struct zd_mac *mac)
+{
+	dev_dbg_f(zd_mac_dev(mac), "\n");
+
+	mac->beacon.last_update = jiffies;
+	queue_delayed_work(zd_workqueue, &mac->beacon.watchdog_work,
+			   BEACON_WATCHDOG_DELAY);
+}
+
+static void beacon_disable(struct zd_mac *mac)
+{
+	dev_dbg_f(zd_mac_dev(mac), "\n");
+	cancel_delayed_work_sync(&mac->beacon.watchdog_work);
+}
+
 #define LINK_LED_WORK_DELAY HZ
 
 static void link_led_handler(struct work_struct *work)
@@ -1179,6 +1422,9 @@ static void link_led_handler(struct work_struct *work)
 	int is_associated;
 	int r;
 
+	if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+		goto requeue;
+
 	spin_lock_irq(&mac->lock);
 	is_associated = mac->associated;
 	spin_unlock_irq(&mac->lock);
@@ -1188,6 +1434,7 @@ static void link_led_handler(struct work_struct *work)
 	if (r)
 		dev_dbg_f(zd_mac_dev(mac), "zd_chip_control_leds error %d\n", r);
 
+requeue:
 	queue_delayed_work(zd_workqueue, &mac->housekeeping.link_led_work,
 		           LINK_LED_WORK_DELAY);
 }
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zd1211rw/zd_mac.h
index a6d86b996c79..f8c93c3fe755 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zd1211rw/zd_mac.h
@@ -163,6 +163,17 @@ struct housekeeping {
 	struct delayed_work link_led_work;
 };
 
+struct beacon {
+	struct delayed_work watchdog_work;
+	unsigned long last_update;
+	u16 interval;
+	u8 period;
+};
+
+enum zd_device_flags {
+	ZD_DEVICE_RUNNING,
+};
+
 #define ZD_MAC_STATS_BUFFER_SIZE 16
 
 #define ZD_MAC_MAX_ACK_WAITERS 50
@@ -172,17 +183,19 @@ struct zd_mac {
 	spinlock_t lock;
 	spinlock_t intr_lock;
 	struct ieee80211_hw *hw;
+	struct ieee80211_vif *vif;
 	struct housekeeping housekeeping;
-	struct work_struct set_multicast_hash_work;
+	struct beacon beacon;
 	struct work_struct set_rts_cts_work;
-	struct work_struct set_rx_filter_work;
 	struct work_struct process_intr;
 	struct zd_mc_hash multicast_hash;
 	u8 intr_buffer[USB_MAX_EP_INT_BUFFER];
 	u8 regdomain;
 	u8 default_regdomain;
+	u8 channel;
 	int type;
 	int associated;
+	unsigned long flags;
 	struct sk_buff_head ack_wait_queue;
 	struct ieee80211_channel channels[14];
 	struct ieee80211_rate rates[12];
@@ -191,9 +204,6 @@ struct zd_mac {
 	/* Short preamble (used for RTS/CTS) */
 	unsigned int short_preamble:1;
 
-	/* flags to indicate update in progress */
-	unsigned int updating_rts_rate:1;
-
 	/* whether to pass frames with CRC errors to stack */
 	unsigned int pass_failed_fcs:1;
 
@@ -304,6 +314,10 @@ int zd_mac_rx(struct ieee80211_hw *hw, const u8 *buffer, unsigned int length);
 void zd_mac_tx_failed(struct urb *urb);
 void zd_mac_tx_to_dev(struct sk_buff *skb, int error);
 
+int zd_op_start(struct ieee80211_hw *hw);
+void zd_op_stop(struct ieee80211_hw *hw);
+int zd_restore_settings(struct zd_mac *mac);
+
 #ifdef DEBUG
 void zd_dump_rx_status(const struct rx_status *status);
 #else
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index 06041cb1c422..81e80489a052 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -377,8 +377,10 @@ static inline void handle_regs_int(struct urb *urb)
 	int_num = le16_to_cpu(*(__le16 *)(urb->transfer_buffer+2));
 	if (int_num == CR_INTERRUPT) {
 		struct zd_mac *mac = zd_hw_mac(zd_usb_to_hw(urb->context));
+		spin_lock(&mac->lock);
 		memcpy(&mac->intr_buffer, urb->transfer_buffer,
 				USB_MAX_EP_INT_BUFFER);
+		spin_unlock(&mac->lock);
 		schedule_work(&mac->process_intr);
 	} else if (intr->read_regs_enabled) {
 		intr->read_regs.length = len = urb->actual_length;
@@ -409,8 +411,10 @@ static void int_urb_complete(struct urb *urb)
 	case -ENOENT:
 	case -ECONNRESET:
 	case -EPIPE:
-		goto kfree;
+		dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
+		return;
 	default:
+		dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
 		goto resubmit;
 	}
 
@@ -441,12 +445,11 @@ static void int_urb_complete(struct urb *urb)
 resubmit:
 	r = usb_submit_urb(urb, GFP_ATOMIC);
 	if (r) {
-		dev_dbg_f(urb_dev(urb), "resubmit urb %p\n", urb);
-		goto kfree;
+		dev_dbg_f(urb_dev(urb), "error: resubmit urb %p err code %d\n",
+			  urb, r);
+		/* TODO: add worker to reset intr->urb */
 	}
 	return;
-kfree:
-	kfree(urb->transfer_buffer);
 }
 
 static inline int int_urb_interval(struct usb_device *udev)
@@ -477,9 +480,8 @@ static inline int usb_int_enabled(struct zd_usb *usb)
 int zd_usb_enable_int(struct zd_usb *usb)
 {
 	int r;
-	struct usb_device *udev;
+	struct usb_device *udev = zd_usb_to_usbdev(usb);
 	struct zd_usb_interrupt *intr = &usb->intr;
-	void *transfer_buffer = NULL;
 	struct urb *urb;
 
 	dev_dbg_f(zd_usb_dev(usb), "\n");
@@ -500,20 +502,21 @@ int zd_usb_enable_int(struct zd_usb *usb)
 	intr->urb = urb;
 	spin_unlock_irq(&intr->lock);
 
-	/* TODO: make it a DMA buffer */
 	r = -ENOMEM;
-	transfer_buffer = kmalloc(USB_MAX_EP_INT_BUFFER, GFP_KERNEL);
-	if (!transfer_buffer) {
+	intr->buffer = usb_alloc_coherent(udev, USB_MAX_EP_INT_BUFFER,
+					  GFP_KERNEL, &intr->buffer_dma);
+	if (!intr->buffer) {
 		dev_dbg_f(zd_usb_dev(usb),
 			"couldn't allocate transfer_buffer\n");
 		goto error_set_urb_null;
 	}
 
-	udev = zd_usb_to_usbdev(usb);
 	usb_fill_int_urb(urb, udev, usb_rcvintpipe(udev, EP_INT_IN),
-			 transfer_buffer, USB_MAX_EP_INT_BUFFER,
+			 intr->buffer, USB_MAX_EP_INT_BUFFER,
 			 int_urb_complete, usb,
 			 intr->interval);
+	urb->transfer_dma = intr->buffer_dma;
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
 	dev_dbg_f(zd_usb_dev(usb), "submit urb %p\n", intr->urb);
 	r = usb_submit_urb(urb, GFP_KERNEL);
@@ -525,7 +528,8 @@ int zd_usb_enable_int(struct zd_usb *usb)
 
 	return 0;
 error:
-	kfree(transfer_buffer);
+	usb_free_coherent(udev, USB_MAX_EP_INT_BUFFER,
+			  intr->buffer, intr->buffer_dma);
 error_set_urb_null:
 	spin_lock_irq(&intr->lock);
 	intr->urb = NULL;
@@ -539,8 +543,11 @@ out:
 void zd_usb_disable_int(struct zd_usb *usb)
 {
 	unsigned long flags;
+	struct usb_device *udev = zd_usb_to_usbdev(usb);
 	struct zd_usb_interrupt *intr = &usb->intr;
 	struct urb *urb;
+	void *buffer;
+	dma_addr_t buffer_dma;
 
 	spin_lock_irqsave(&intr->lock, flags);
 	urb = intr->urb;
@@ -549,11 +556,18 @@ void zd_usb_disable_int(struct zd_usb *usb)
 		return;
 	}
 	intr->urb = NULL;
+	buffer = intr->buffer;
+	buffer_dma = intr->buffer_dma;
+	intr->buffer = NULL;
 	spin_unlock_irqrestore(&intr->lock, flags);
 
 	usb_kill_urb(urb);
 	dev_dbg_f(zd_usb_dev(usb), "urb %p killed\n", urb);
 	usb_free_urb(urb);
+
+	if (buffer)
+		usb_free_coherent(udev, USB_MAX_EP_INT_BUFFER,
+				  buffer, buffer_dma);
 }
 
 static void handle_rx_packet(struct zd_usb *usb, const u8 *buffer,
@@ -601,6 +615,7 @@ static void handle_rx_packet(struct zd_usb *usb, const u8 *buffer,
 
 static void rx_urb_complete(struct urb *urb)
 {
+	int r;
 	struct zd_usb *usb;
 	struct zd_usb_rx *rx;
 	const u8 *buffer;
@@ -615,6 +630,7 @@ static void rx_urb_complete(struct urb *urb)
 	case -ENOENT:
 	case -ECONNRESET:
 	case -EPIPE:
+		dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
 		return;
 	default:
 		dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
@@ -626,6 +642,8 @@ static void rx_urb_complete(struct urb *urb)
 	usb = urb->context;
 	rx = &usb->rx;
 
+	zd_usb_reset_rx_idle_timer(usb);
+
 	if (length%rx->usb_packet_size > rx->usb_packet_size-4) {
 		/* If there is an old first fragment, we don't care. */
 		dev_dbg_f(urb_dev(urb), "*** first fragment ***\n");
@@ -654,7 +672,9 @@ static void rx_urb_complete(struct urb *urb)
 	}
 
 resubmit:
-	usb_submit_urb(urb, GFP_ATOMIC);
+	r = usb_submit_urb(urb, GFP_ATOMIC);
+	if (r)
+		dev_dbg_f(urb_dev(urb), "urb %p resubmit error %d\n", urb, r);
 }
 
 static struct urb *alloc_rx_urb(struct zd_usb *usb)
@@ -690,7 +710,7 @@ static void free_rx_urb(struct urb *urb)
 	usb_free_urb(urb);
 }
 
-int zd_usb_enable_rx(struct zd_usb *usb)
+static int __zd_usb_enable_rx(struct zd_usb *usb)
 {
 	int i, r;
 	struct zd_usb_rx *rx = &usb->rx;
@@ -742,7 +762,21 @@ error:
 	return r;
 }
 
-void zd_usb_disable_rx(struct zd_usb *usb)
+int zd_usb_enable_rx(struct zd_usb *usb)
+{
+	int r;
+	struct zd_usb_rx *rx = &usb->rx;
+
+	mutex_lock(&rx->setup_mutex);
+	r = __zd_usb_enable_rx(usb);
+	mutex_unlock(&rx->setup_mutex);
+
+	zd_usb_reset_rx_idle_timer(usb);
+
+	return r;
+}
+
+static void __zd_usb_disable_rx(struct zd_usb *usb)
 {
 	int i;
 	unsigned long flags;
@@ -769,6 +803,40 @@ void zd_usb_disable_rx(struct zd_usb *usb)
 	spin_unlock_irqrestore(&rx->lock, flags);
 }
 
+void zd_usb_disable_rx(struct zd_usb *usb)
+{
+	struct zd_usb_rx *rx = &usb->rx;
+
+	mutex_lock(&rx->setup_mutex);
+	__zd_usb_disable_rx(usb);
+	mutex_unlock(&rx->setup_mutex);
+
+	cancel_delayed_work_sync(&rx->idle_work);
+}
+
+static void zd_usb_reset_rx(struct zd_usb *usb)
+{
+	bool do_reset;
+	struct zd_usb_rx *rx = &usb->rx;
+	unsigned long flags;
+
+	mutex_lock(&rx->setup_mutex);
+
+	spin_lock_irqsave(&rx->lock, flags);
+	do_reset = rx->urbs != NULL;
+	spin_unlock_irqrestore(&rx->lock, flags);
+
+	if (do_reset) {
+		__zd_usb_disable_rx(usb);
+		__zd_usb_enable_rx(usb);
+	}
+
+	mutex_unlock(&rx->setup_mutex);
+
+	if (do_reset)
+		zd_usb_reset_rx_idle_timer(usb);
+}
+
 /**
  * zd_usb_disable_tx - disable transmission
  * @usb: the zd1211rw-private USB structure
@@ -779,19 +847,21 @@ void zd_usb_disable_tx(struct zd_usb *usb)
 {
 	struct zd_usb_tx *tx = &usb->tx;
 	unsigned long flags;
-	struct list_head *pos, *n;
+
+	atomic_set(&tx->enabled, 0);
+
+	/* kill all submitted tx-urbs */
+	usb_kill_anchored_urbs(&tx->submitted);
 
 	spin_lock_irqsave(&tx->lock, flags);
-	list_for_each_safe(pos, n, &tx->free_urb_list) {
-		list_del(pos);
-		usb_free_urb(list_entry(pos, struct urb, urb_list));
-	}
-	tx->enabled = 0;
+	WARN_ON(!skb_queue_empty(&tx->submitted_skbs));
+	WARN_ON(tx->submitted_urbs != 0);
 	tx->submitted_urbs = 0;
+	spin_unlock_irqrestore(&tx->lock, flags);
+
 	/* The stopped state is ignored, relying on ieee80211_wake_queues()
 	 * in a potentionally following zd_usb_enable_tx().
 	 */
-	spin_unlock_irqrestore(&tx->lock, flags);
 }
 
 /**
@@ -807,63 +877,13 @@ void zd_usb_enable_tx(struct zd_usb *usb)
 	struct zd_usb_tx *tx = &usb->tx;
 
 	spin_lock_irqsave(&tx->lock, flags);
-	tx->enabled = 1;
+	atomic_set(&tx->enabled, 1);
 	tx->submitted_urbs = 0;
 	ieee80211_wake_queues(zd_usb_to_hw(usb));
 	tx->stopped = 0;
 	spin_unlock_irqrestore(&tx->lock, flags);
 }
 
-/**
- * alloc_tx_urb - provides an tx URB
- * @usb: a &struct zd_usb pointer
- *
- * Allocates a new URB. If possible takes the urb from the free list in
- * usb->tx.
- */
-static struct urb *alloc_tx_urb(struct zd_usb *usb)
-{
-	struct zd_usb_tx *tx = &usb->tx;
-	unsigned long flags;
-	struct list_head *entry;
-	struct urb *urb;
-
-	spin_lock_irqsave(&tx->lock, flags);
-	if (list_empty(&tx->free_urb_list)) {
-		urb = usb_alloc_urb(0, GFP_ATOMIC);
-		goto out;
-	}
-	entry = tx->free_urb_list.next;
-	list_del(entry);
-	urb = list_entry(entry, struct urb, urb_list);
-out:
-	spin_unlock_irqrestore(&tx->lock, flags);
-	return urb;
-}
-
-/**
- * free_tx_urb - frees a used tx URB
- * @usb: a &struct zd_usb pointer
- * @urb: URB to be freed
- *
- * Frees the transmission URB, which means to put it on the free URB
- * list.
- */
-static void free_tx_urb(struct zd_usb *usb, struct urb *urb)
-{
-	struct zd_usb_tx *tx = &usb->tx;
-	unsigned long flags;
-
-	spin_lock_irqsave(&tx->lock, flags);
-	if (!tx->enabled) {
-		usb_free_urb(urb);
-		goto out;
-	}
-	list_add(&urb->urb_list, &tx->free_urb_list);
-out:
-	spin_unlock_irqrestore(&tx->lock, flags);
-}
-
 static void tx_dec_submitted_urbs(struct zd_usb *usb)
 {
 	struct zd_usb_tx *tx = &usb->tx;
@@ -905,6 +925,16 @@ static void tx_urb_complete(struct urb *urb)
 	struct sk_buff *skb;
 	struct ieee80211_tx_info *info;
 	struct zd_usb *usb;
+	struct zd_usb_tx *tx;
+
+	skb = (struct sk_buff *)urb->context;
+	info = IEEE80211_SKB_CB(skb);
+	/*
+	 * grab 'usb' pointer before handing off the skb (since
+	 * it might be freed by zd_mac_tx_to_dev or mac80211)
+	 */
+	usb = &zd_hw_mac(info->rate_driver_data[0])->chip.usb;
+	tx = &usb->tx;
 
 	switch (urb->status) {
 	case 0:
@@ -922,20 +952,16 @@ static void tx_urb_complete(struct urb *urb)
 		goto resubmit;
 	}
 free_urb:
-	skb = (struct sk_buff *)urb->context;
-	/*
-	 * grab 'usb' pointer before handing off the skb (since
-	 * it might be freed by zd_mac_tx_to_dev or mac80211)
-	 */
-	info = IEEE80211_SKB_CB(skb);
-	usb = &zd_hw_mac(info->rate_driver_data[0])->chip.usb;
+	skb_unlink(skb, &usb->tx.submitted_skbs);
 	zd_mac_tx_to_dev(skb, urb->status);
-	free_tx_urb(usb, urb);
+	usb_free_urb(urb);
 	tx_dec_submitted_urbs(usb);
 	return;
 resubmit:
+	usb_anchor_urb(urb, &tx->submitted);
 	r = usb_submit_urb(urb, GFP_ATOMIC);
 	if (r) {
+		usb_unanchor_urb(urb);
 		dev_dbg_f(urb_dev(urb), "error resubmit urb %p %d\n", urb, r);
 		goto free_urb;
 	}
@@ -956,10 +982,17 @@ resubmit:
 int zd_usb_tx(struct zd_usb *usb, struct sk_buff *skb)
 {
 	int r;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct usb_device *udev = zd_usb_to_usbdev(usb);
 	struct urb *urb;
+	struct zd_usb_tx *tx = &usb->tx;
+
+	if (!atomic_read(&tx->enabled)) {
+		r = -ENOENT;
+		goto out;
+	}
 
-	urb = alloc_tx_urb(usb);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!urb) {
 		r = -ENOMEM;
 		goto out;
@@ -968,17 +1001,118 @@ int zd_usb_tx(struct zd_usb *usb, struct sk_buff *skb)
 	usb_fill_bulk_urb(urb, udev, usb_sndbulkpipe(udev, EP_DATA_OUT),
 		          skb->data, skb->len, tx_urb_complete, skb);
 
+	info->rate_driver_data[1] = (void *)jiffies;
+	skb_queue_tail(&tx->submitted_skbs, skb);
+	usb_anchor_urb(urb, &tx->submitted);
+
 	r = usb_submit_urb(urb, GFP_ATOMIC);
-	if (r)
+	if (r) {
+		dev_dbg_f(zd_usb_dev(usb), "error submit urb %p %d\n", urb, r);
+		usb_unanchor_urb(urb);
+		skb_unlink(skb, &tx->submitted_skbs);
 		goto error;
+	}
 	tx_inc_submitted_urbs(usb);
 	return 0;
 error:
-	free_tx_urb(usb, urb);
+	usb_free_urb(urb);
 out:
 	return r;
 }
 
+static bool zd_tx_timeout(struct zd_usb *usb)
+{
+	struct zd_usb_tx *tx = &usb->tx;
+	struct sk_buff_head *q = &tx->submitted_skbs;
+	struct sk_buff *skb, *skbnext;
+	struct ieee80211_tx_info *info;
+	unsigned long flags, trans_start;
+	bool have_timedout = false;
+
+	spin_lock_irqsave(&q->lock, flags);
+	skb_queue_walk_safe(q, skb, skbnext) {
+		info = IEEE80211_SKB_CB(skb);
+		trans_start = (unsigned long)info->rate_driver_data[1];
+
+		if (time_is_before_jiffies(trans_start + ZD_TX_TIMEOUT)) {
+			have_timedout = true;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&q->lock, flags);
+
+	return have_timedout;
+}
+
+static void zd_tx_watchdog_handler(struct work_struct *work)
+{
+	struct zd_usb *usb =
+		container_of(work, struct zd_usb, tx.watchdog_work.work);
+	struct zd_usb_tx *tx = &usb->tx;
+
+	if (!atomic_read(&tx->enabled) || !tx->watchdog_enabled)
+		goto out;
+	if (!zd_tx_timeout(usb))
+		goto out;
+
+	/* TX halted, try reset */
+	dev_warn(zd_usb_dev(usb), "TX-stall detected, reseting device...");
+
+	usb_queue_reset_device(usb->intf);
+
+	/* reset will stop this worker, don't rearm */
+	return;
+out:
+	queue_delayed_work(zd_workqueue, &tx->watchdog_work,
+			   ZD_TX_WATCHDOG_INTERVAL);
+}
+
+void zd_tx_watchdog_enable(struct zd_usb *usb)
+{
+	struct zd_usb_tx *tx = &usb->tx;
+
+	if (!tx->watchdog_enabled) {
+		dev_dbg_f(zd_usb_dev(usb), "\n");
+		queue_delayed_work(zd_workqueue, &tx->watchdog_work,
+				   ZD_TX_WATCHDOG_INTERVAL);
+		tx->watchdog_enabled = 1;
+	}
+}
+
+void zd_tx_watchdog_disable(struct zd_usb *usb)
+{
+	struct zd_usb_tx *tx = &usb->tx;
+
+	if (tx->watchdog_enabled) {
+		dev_dbg_f(zd_usb_dev(usb), "\n");
+		tx->watchdog_enabled = 0;
+		cancel_delayed_work_sync(&tx->watchdog_work);
+	}
+}
+
+static void zd_rx_idle_timer_handler(struct work_struct *work)
+{
+	struct zd_usb *usb =
+		container_of(work, struct zd_usb, rx.idle_work.work);
+	struct zd_mac *mac = zd_usb_to_mac(usb);
+
+	if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+		return;
+
+	dev_dbg_f(zd_usb_dev(usb), "\n");
+
+	/* 30 seconds since last rx, reset rx */
+	zd_usb_reset_rx(usb);
+}
+
+void zd_usb_reset_rx_idle_timer(struct zd_usb *usb)
+{
+	struct zd_usb_rx *rx = &usb->rx;
+
+	cancel_delayed_work(&rx->idle_work);
+	queue_delayed_work(zd_workqueue, &rx->idle_work, ZD_RX_IDLE_INTERVAL);
+}
+
 static inline void init_usb_interrupt(struct zd_usb *usb)
 {
 	struct zd_usb_interrupt *intr = &usb->intr;
@@ -993,22 +1127,27 @@ static inline void init_usb_rx(struct zd_usb *usb)
 {
 	struct zd_usb_rx *rx = &usb->rx;
 	spin_lock_init(&rx->lock);
+	mutex_init(&rx->setup_mutex);
 	if (interface_to_usbdev(usb->intf)->speed == USB_SPEED_HIGH) {
 		rx->usb_packet_size = 512;
 	} else {
 		rx->usb_packet_size = 64;
 	}
 	ZD_ASSERT(rx->fragment_length == 0);
+	INIT_DELAYED_WORK(&rx->idle_work, zd_rx_idle_timer_handler);
 }
 
 static inline void init_usb_tx(struct zd_usb *usb)
 {
 	struct zd_usb_tx *tx = &usb->tx;
 	spin_lock_init(&tx->lock);
-	tx->enabled = 0;
+	atomic_set(&tx->enabled, 0);
 	tx->stopped = 0;
-	INIT_LIST_HEAD(&tx->free_urb_list);
+	skb_queue_head_init(&tx->submitted_skbs);
+	init_usb_anchor(&tx->submitted);
 	tx->submitted_urbs = 0;
+	tx->watchdog_enabled = 0;
+	INIT_DELAYED_WORK(&tx->watchdog_work, zd_tx_watchdog_handler);
 }
 
 void zd_usb_init(struct zd_usb *usb, struct ieee80211_hw *hw,
@@ -1017,6 +1156,7 @@ void zd_usb_init(struct zd_usb *usb, struct ieee80211_hw *hw,
 	memset(usb, 0, sizeof(*usb));
 	usb->intf = usb_get_intf(intf);
 	usb_set_intfdata(usb->intf, hw);
+	init_usb_anchor(&usb->submitted_cmds);
 	init_usb_interrupt(usb);
 	init_usb_tx(usb);
 	init_usb_rx(usb);
@@ -1240,6 +1380,7 @@ static void disconnect(struct usb_interface *intf)
 	ieee80211_unregister_hw(hw);
 
 	/* Just in case something has gone wrong! */
+	zd_usb_disable_tx(usb);
 	zd_usb_disable_rx(usb);
 	zd_usb_disable_int(usb);
 
@@ -1255,11 +1396,92 @@ static void disconnect(struct usb_interface *intf)
 	dev_dbg(&intf->dev, "disconnected\n");
 }
 
+static void zd_usb_resume(struct zd_usb *usb)
+{
+	struct zd_mac *mac = zd_usb_to_mac(usb);
+	int r;
+
+	dev_dbg_f(zd_usb_dev(usb), "\n");
+
+	r = zd_op_start(zd_usb_to_hw(usb));
+	if (r < 0) {
+		dev_warn(zd_usb_dev(usb), "Device resume failed "
+			 "with error code %d. Retrying...\n", r);
+		if (usb->was_running)
+			set_bit(ZD_DEVICE_RUNNING, &mac->flags);
+		usb_queue_reset_device(usb->intf);
+		return;
+	}
+
+	if (mac->type != NL80211_IFTYPE_UNSPECIFIED) {
+		r = zd_restore_settings(mac);
+		if (r < 0) {
+			dev_dbg(zd_usb_dev(usb),
+				"failed to restore settings, %d\n", r);
+			return;
+		}
+	}
+}
+
+static void zd_usb_stop(struct zd_usb *usb)
+{
+	dev_dbg_f(zd_usb_dev(usb), "\n");
+
+	zd_op_stop(zd_usb_to_hw(usb));
+
+	zd_usb_disable_tx(usb);
+	zd_usb_disable_rx(usb);
+	zd_usb_disable_int(usb);
+
+	usb->initialized = 0;
+}
+
+static int pre_reset(struct usb_interface *intf)
+{
+	struct ieee80211_hw *hw = usb_get_intfdata(intf);
+	struct zd_mac *mac;
+	struct zd_usb *usb;
+
+	if (!hw || intf->condition != USB_INTERFACE_BOUND)
+		return 0;
+
+	mac = zd_hw_mac(hw);
+	usb = &mac->chip.usb;
+
+	usb->was_running = test_bit(ZD_DEVICE_RUNNING, &mac->flags);
+
+	zd_usb_stop(usb);
+
+	mutex_lock(&mac->chip.mutex);
+	return 0;
+}
+
+static int post_reset(struct usb_interface *intf)
+{
+	struct ieee80211_hw *hw = usb_get_intfdata(intf);
+	struct zd_mac *mac;
+	struct zd_usb *usb;
+
+	if (!hw || intf->condition != USB_INTERFACE_BOUND)
+		return 0;
+
+	mac = zd_hw_mac(hw);
+	usb = &mac->chip.usb;
+
+	mutex_unlock(&mac->chip.mutex);
+
+	if (usb->was_running)
+		zd_usb_resume(usb);
+	return 0;
+}
+
 static struct usb_driver driver = {
 	.name		= KBUILD_MODNAME,
 	.id_table	= usb_ids,
 	.probe		= probe,
 	.disconnect	= disconnect,
+	.pre_reset	= pre_reset,
+	.post_reset	= post_reset,
 };
 
 struct workqueue_struct *zd_workqueue;
@@ -1393,30 +1615,35 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
 		return -EWOULDBLOCK;
 	}
 	if (!usb_int_enabled(usb)) {
-		 dev_dbg_f(zd_usb_dev(usb),
+		dev_dbg_f(zd_usb_dev(usb),
 			  "error: usb interrupt not enabled\n");
 		return -EWOULDBLOCK;
 	}
 
+	ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+	BUILD_BUG_ON(sizeof(struct usb_req_read_regs) + USB_MAX_IOREAD16_COUNT *
+		     sizeof(__le16) > sizeof(usb->req_buf));
+	BUG_ON(sizeof(struct usb_req_read_regs) + count * sizeof(__le16) >
+	       sizeof(usb->req_buf));
+
 	req_len = sizeof(struct usb_req_read_regs) + count * sizeof(__le16);
-	req = kmalloc(req_len, GFP_KERNEL);
-	if (!req)
-		return -ENOMEM;
+	req = (void *)usb->req_buf;
+
 	req->id = cpu_to_le16(USB_REQ_READ_REGS);
 	for (i = 0; i < count; i++)
 		req->addr[i] = cpu_to_le16((u16)addresses[i]);
 
 	udev = zd_usb_to_usbdev(usb);
 	prepare_read_regs_int(usb);
-	r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, EP_REGS_OUT),
-		         req, req_len, &actual_req_len, 1000 /* ms */);
+	r = usb_interrupt_msg(udev, usb_sndintpipe(udev, EP_REGS_OUT),
+			      req, req_len, &actual_req_len, 50 /* ms */);
 	if (r) {
 		dev_dbg_f(zd_usb_dev(usb),
-			"error in usb_bulk_msg(). Error number %d\n", r);
+			"error in usb_interrupt_msg(). Error number %d\n", r);
 		goto error;
 	}
 	if (req_len != actual_req_len) {
-		dev_dbg_f(zd_usb_dev(usb), "error in usb_bulk_msg()\n"
+		dev_dbg_f(zd_usb_dev(usb), "error in usb_interrupt_msg()\n"
 			" req_len %d != actual_req_len %d\n",
 			req_len, actual_req_len);
 		r = -EIO;
@@ -1424,7 +1651,7 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
 	}
 
 	timeout = wait_for_completion_timeout(&usb->intr.read_regs.completion,
-	                                      msecs_to_jiffies(1000));
+					      msecs_to_jiffies(50));
 	if (!timeout) {
 		disable_read_regs_int(usb);
 		dev_dbg_f(zd_usb_dev(usb), "read timed out\n");
@@ -1434,17 +1661,106 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
 
 	r = get_results(usb, values, req, count);
 error:
-	kfree(req);
 	return r;
 }
 
-int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
-	              unsigned int count)
+static void iowrite16v_urb_complete(struct urb *urb)
+{
+	struct zd_usb *usb = urb->context;
+
+	if (urb->status && !usb->cmd_error)
+		usb->cmd_error = urb->status;
+}
+
+static int zd_submit_waiting_urb(struct zd_usb *usb, bool last)
+{
+	int r = 0;
+	struct urb *urb = usb->urb_async_waiting;
+
+	if (!urb)
+		return 0;
+
+	usb->urb_async_waiting = NULL;
+
+	if (!last)
+		urb->transfer_flags |= URB_NO_INTERRUPT;
+
+	usb_anchor_urb(urb, &usb->submitted_cmds);
+	r = usb_submit_urb(urb, GFP_KERNEL);
+	if (r) {
+		usb_unanchor_urb(urb);
+		dev_dbg_f(zd_usb_dev(usb),
+			"error in usb_submit_urb(). Error number %d\n", r);
+		goto error;
+	}
+
+	/* fall-through with r == 0 */
+error:
+	usb_free_urb(urb);
+	return r;
+}
+
+void zd_usb_iowrite16v_async_start(struct zd_usb *usb)
+{
+	ZD_ASSERT(usb_anchor_empty(&usb->submitted_cmds));
+	ZD_ASSERT(usb->urb_async_waiting == NULL);
+	ZD_ASSERT(!usb->in_async);
+
+	ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+
+	usb->in_async = 1;
+	usb->cmd_error = 0;
+	usb->urb_async_waiting = NULL;
+}
+
+int zd_usb_iowrite16v_async_end(struct zd_usb *usb, unsigned int timeout)
+{
+	int r;
+
+	ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+	ZD_ASSERT(usb->in_async);
+
+	/* Submit last iowrite16v URB */
+	r = zd_submit_waiting_urb(usb, true);
+	if (r) {
+		dev_dbg_f(zd_usb_dev(usb),
+			"error in zd_submit_waiting_usb(). "
+			"Error number %d\n", r);
+
+		usb_kill_anchored_urbs(&usb->submitted_cmds);
+		goto error;
+	}
+
+	if (timeout)
+		timeout = usb_wait_anchor_empty_timeout(&usb->submitted_cmds,
+							timeout);
+	if (!timeout) {
+		usb_kill_anchored_urbs(&usb->submitted_cmds);
+		if (usb->cmd_error == -ENOENT) {
+			dev_dbg_f(zd_usb_dev(usb), "timed out");
+			r = -ETIMEDOUT;
+			goto error;
+		}
+	}
+
+	r = usb->cmd_error;
+error:
+	usb->in_async = 0;
+	return r;
+}
+
+int zd_usb_iowrite16v_async(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
+			    unsigned int count)
 {
 	int r;
 	struct usb_device *udev;
 	struct usb_req_write_regs *req = NULL;
-	int i, req_len, actual_req_len;
+	int i, req_len;
+	struct urb *urb;
+	struct usb_host_endpoint *ep;
+
+	ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+	ZD_ASSERT(usb->in_async);
 
 	if (count == 0)
 		return 0;
@@ -1460,11 +1776,23 @@ int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
 		return -EWOULDBLOCK;
 	}
 
+	udev = zd_usb_to_usbdev(usb);
+
+	ep = usb_pipe_endpoint(udev, usb_sndintpipe(udev, EP_REGS_OUT));
+	if (!ep)
+		return -ENOENT;
+
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb)
+		return -ENOMEM;
+
 	req_len = sizeof(struct usb_req_write_regs) +
 		  count * sizeof(struct reg_data);
 	req = kmalloc(req_len, GFP_KERNEL);
-	if (!req)
-		return -ENOMEM;
+	if (!req) {
+		r = -ENOMEM;
+		goto error;
+	}
 
 	req->id = cpu_to_le16(USB_REQ_WRITE_REGS);
 	for (i = 0; i < count; i++) {
@@ -1473,29 +1801,44 @@ int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
 		rw->value = cpu_to_le16(ioreqs[i].value);
 	}
 
-	udev = zd_usb_to_usbdev(usb);
-	r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, EP_REGS_OUT),
-		         req, req_len, &actual_req_len, 1000 /* ms */);
+	usb_fill_int_urb(urb, udev, usb_sndintpipe(udev, EP_REGS_OUT),
+			 req, req_len, iowrite16v_urb_complete, usb,
+			 ep->desc.bInterval);
+	urb->transfer_flags |= URB_FREE_BUFFER | URB_SHORT_NOT_OK;
+
+	/* Submit previous URB */
+	r = zd_submit_waiting_urb(usb, false);
 	if (r) {
 		dev_dbg_f(zd_usb_dev(usb),
-			"error in usb_bulk_msg(). Error number %d\n", r);
-		goto error;
-	}
-	if (req_len != actual_req_len) {
-		dev_dbg_f(zd_usb_dev(usb),
-			"error in usb_bulk_msg()"
-			" req_len %d != actual_req_len %d\n",
-			req_len, actual_req_len);
-		r = -EIO;
+			"error in zd_submit_waiting_usb(). "
+			"Error number %d\n", r);
 		goto error;
 	}
 
-	/* FALL-THROUGH with r == 0 */
+	/* Delay submit so that URB_NO_INTERRUPT flag can be set for all URBs
+	 * of currect batch except for very last.
+	 */
+	usb->urb_async_waiting = urb;
+	return 0;
 error:
-	kfree(req);
+	usb_free_urb(urb);
 	return r;
 }
 
+int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
+			unsigned int count)
+{
+	int r;
+
+	zd_usb_iowrite16v_async_start(usb);
+	r = zd_usb_iowrite16v_async(usb, ioreqs, count);
+	if (r) {
+		zd_usb_iowrite16v_async_end(usb, 0);
+		return r;
+	}
+	return zd_usb_iowrite16v_async_end(usb, 50 /* ms */);
+}
+
 int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
 {
 	int r;
@@ -1537,14 +1880,19 @@ int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
 	if (r) {
 		dev_dbg_f(zd_usb_dev(usb),
 			"error %d: Couldn't read CR203\n", r);
-		goto out;
+		return r;
 	}
 	bit_value_template &= ~(RF_IF_LE|RF_CLK|RF_DATA);
 
+	ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+	BUILD_BUG_ON(sizeof(struct usb_req_rfwrite) +
+		     USB_MAX_RFWRITE_BIT_COUNT * sizeof(__le16) >
+		     sizeof(usb->req_buf));
+	BUG_ON(sizeof(struct usb_req_rfwrite) + bits * sizeof(__le16) >
+	       sizeof(usb->req_buf));
+
 	req_len = sizeof(struct usb_req_rfwrite) + bits * sizeof(__le16);
-	req = kmalloc(req_len, GFP_KERNEL);
-	if (!req)
-		return -ENOMEM;
+	req = (void *)usb->req_buf;
 
 	req->id = cpu_to_le16(USB_REQ_WRITE_RF);
 	/* 1: 3683a, but not used in ZYDAS driver */
@@ -1559,15 +1907,15 @@ int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
 	}
 
 	udev = zd_usb_to_usbdev(usb);
-	r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, EP_REGS_OUT),
-		         req, req_len, &actual_req_len, 1000 /* ms */);
+	r = usb_interrupt_msg(udev, usb_sndintpipe(udev, EP_REGS_OUT),
+			      req, req_len, &actual_req_len, 50 /* ms */);
 	if (r) {
 		dev_dbg_f(zd_usb_dev(usb),
-			"error in usb_bulk_msg(). Error number %d\n", r);
+			"error in usb_interrupt_msg(). Error number %d\n", r);
 		goto out;
 	}
 	if (req_len != actual_req_len) {
-		dev_dbg_f(zd_usb_dev(usb), "error in usb_bulk_msg()"
+		dev_dbg_f(zd_usb_dev(usb), "error in usb_interrupt_msg()"
 			" req_len %d != actual_req_len %d\n",
 			req_len, actual_req_len);
 		r = -EIO;
@@ -1576,6 +1924,5 @@ int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
 
 	/* FALL-THROUGH with r == 0 */
 out:
-	kfree(req);
 	return r;
 }
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.h b/drivers/net/wireless/zd1211rw/zd_usb.h
index 1b1655cb7cb4..b3df2c8116cc 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.h
+++ b/drivers/net/wireless/zd1211rw/zd_usb.h
@@ -32,6 +32,10 @@
 #define ZD_USB_TX_HIGH  5
 #define ZD_USB_TX_LOW   2
 
+#define ZD_TX_TIMEOUT		(HZ * 5)
+#define ZD_TX_WATCHDOG_INTERVAL	round_jiffies_relative(HZ)
+#define ZD_RX_IDLE_INTERVAL	round_jiffies_relative(30 * HZ)
+
 enum devicetype {
 	DEVICE_ZD1211  = 0,
 	DEVICE_ZD1211B = 1,
@@ -162,6 +166,8 @@ struct zd_usb_interrupt {
 	struct read_regs_int read_regs;
 	spinlock_t lock;
 	struct urb *urb;
+	void *buffer;
+	dma_addr_t buffer_dma;
 	int interval;
 	u8 read_regs_enabled:1;
 };
@@ -175,7 +181,9 @@ static inline struct usb_int_regs *get_read_regs(struct zd_usb_interrupt *intr)
 
 struct zd_usb_rx {
 	spinlock_t lock;
-	u8 fragment[2*USB_MAX_RX_SIZE];
+	struct mutex setup_mutex;
+	struct delayed_work idle_work;
+	u8 fragment[2 * USB_MAX_RX_SIZE];
 	unsigned int fragment_length;
 	unsigned int usb_packet_size;
 	struct urb **urbs;
@@ -184,19 +192,21 @@ struct zd_usb_rx {
 
 /**
  * struct zd_usb_tx - structure used for transmitting frames
+ * @enabled: atomic enabled flag, indicates whether tx is enabled
  * @lock: lock for transmission
- * @free_urb_list: list of free URBs, contains all the URBs, which can be used
+ * @submitted: anchor for URBs sent to device
  * @submitted_urbs: atomic integer that counts the URBs having sent to the
  *	device, which haven't been completed
- * @enabled: enabled flag, indicates whether tx is enabled
  * @stopped: indicates whether higher level tx queues are stopped
  */
 struct zd_usb_tx {
+	atomic_t enabled;
 	spinlock_t lock;
-	struct list_head free_urb_list;
+	struct delayed_work watchdog_work;
+	struct sk_buff_head submitted_skbs;
+	struct usb_anchor submitted;
 	int submitted_urbs;
-	int enabled;
-	int stopped;
+	u8 stopped:1, watchdog_enabled:1;
 };
 
 /* Contains the usb parts. The structure doesn't require a lock because intf
@@ -207,7 +217,11 @@ struct zd_usb {
 	struct zd_usb_rx rx;
 	struct zd_usb_tx tx;
 	struct usb_interface *intf;
-	u8 is_zd1211b:1, initialized:1;
+	struct usb_anchor submitted_cmds;
+	struct urb *urb_async_waiting;
+	int cmd_error;
+	u8 req_buf[64]; /* zd_usb_iowrite16v needs 62 bytes */
+	u8 is_zd1211b:1, initialized:1, was_running:1, in_async:1;
 };
 
 #define zd_usb_dev(usb) (&usb->intf->dev)
@@ -234,12 +248,17 @@ void zd_usb_clear(struct zd_usb *usb);
 
 int zd_usb_scnprint_id(struct zd_usb *usb, char *buffer, size_t size);
 
+void zd_tx_watchdog_enable(struct zd_usb *usb);
+void zd_tx_watchdog_disable(struct zd_usb *usb);
+
 int zd_usb_enable_int(struct zd_usb *usb);
 void zd_usb_disable_int(struct zd_usb *usb);
 
 int zd_usb_enable_rx(struct zd_usb *usb);
 void zd_usb_disable_rx(struct zd_usb *usb);
 
+void zd_usb_reset_rx_idle_timer(struct zd_usb *usb);
+
 void zd_usb_enable_tx(struct zd_usb *usb);
 void zd_usb_disable_tx(struct zd_usb *usb);
 
@@ -254,6 +273,10 @@ static inline int zd_usb_ioread16(struct zd_usb *usb, u16 *value,
 	return zd_usb_ioread16v(usb, value, (const zd_addr_t *)&addr, 1);
 }
 
+void zd_usb_iowrite16v_async_start(struct zd_usb *usb);
+int zd_usb_iowrite16v_async_end(struct zd_usb *usb, unsigned int timeout);
+int zd_usb_iowrite16v_async(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
+			    unsigned int count);
 int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
 	              unsigned int count);
 
diff --git a/drivers/net/xilinx_emaclite.c b/drivers/net/xilinx_emaclite.c
index cad66ce1640b..2642af4ee491 100644
--- a/drivers/net/xilinx_emaclite.c
+++ b/drivers/net/xilinx_emaclite.c
@@ -1101,8 +1101,7 @@ static struct net_device_ops xemaclite_netdev_ops;
  * Return:	0, if the driver is bound to the Emaclite device, or
  *		a negative error if there is failure.
  */
-static int __devinit xemaclite_of_probe(struct platform_device *ofdev,
-					const struct of_device_id *match)
+static int __devinit xemaclite_of_probe(struct platform_device *ofdev)
 {
 	struct resource r_irq; /* Interrupt resources */
 	struct resource r_mem; /* IO mem resources */
@@ -1288,7 +1287,7 @@ static struct of_device_id xemaclite_of_match[] __devinitdata = {
 };
 MODULE_DEVICE_TABLE(of, xemaclite_of_match);
 
-static struct of_platform_driver xemaclite_of_driver = {
+static struct platform_driver xemaclite_of_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -1306,7 +1305,7 @@ static struct of_platform_driver xemaclite_of_driver = {
 static int __init xemaclite_init(void)
 {
 	/* No kernel boot options used, we just need to register the driver */
-	return of_register_platform_driver(&xemaclite_of_driver);
+	return platform_driver_register(&xemaclite_of_driver);
 }
 
 /**
@@ -1314,7 +1313,7 @@ static int __init xemaclite_init(void)
  */
 static void __exit xemaclite_cleanup(void)
 {
-	of_unregister_platform_driver(&xemaclite_of_driver);
+	platform_driver_unregister(&xemaclite_of_driver);
 }
 
 module_init(xemaclite_init);
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index ffedfd492754..ea1580085347 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -3,7 +3,7 @@
 #
 
 menuconfig NFC_DEVICES
-	bool "NFC devices"
+	bool "Near Field Communication (NFC) devices"
 	default n
 	---help---
 	  You'll have to say Y if your computer contains an NFC device that
diff --git a/drivers/nfc/pn544.c b/drivers/nfc/pn544.c
index bae647264dd6..724f65d8f9e4 100644
--- a/drivers/nfc/pn544.c
+++ b/drivers/nfc/pn544.c
@@ -60,7 +60,7 @@ enum pn544_irq {
 struct pn544_info {
 	struct miscdevice miscdev;
 	struct i2c_client *i2c_dev;
-	struct regulator_bulk_data regs[2];
+	struct regulator_bulk_data regs[3];
 
 	enum pn544_state state;
 	wait_queue_head_t read_wait;
@@ -74,6 +74,7 @@ struct pn544_info {
 
 static const char reg_vdd_io[]	= "Vdd_IO";
 static const char reg_vbat[]	= "VBat";
+static const char reg_vsim[]	= "VSim";
 
 /* sysfs interface */
 static ssize_t pn544_test(struct device *dev,
@@ -740,6 +741,7 @@ static int __devinit pn544_probe(struct i2c_client *client,
 
 	info->regs[0].supply = reg_vdd_io;
 	info->regs[1].supply = reg_vbat;
+	info->regs[2].supply = reg_vsim;
 	r = regulator_bulk_get(&client->dev, ARRAY_SIZE(info->regs),
 				 info->regs);
 	if (r < 0)
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 3c6e100a3ad0..d06a6374ed6c 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -69,4 +69,10 @@ config OF_MDIO
 	help
 	  OpenFirmware MDIO bus (Ethernet PHY) accessors
 
+config OF_PCI
+	def_tristate PCI
+	depends on PCI && (PPC || MICROBLAZE || X86)
+	help
+	  OpenFirmware PCI bus accessors
+
 endmenu # OF
diff --git a/drivers/of/Makefile b/drivers/of/Makefile
index 3ab21a0a4907..f7861ed2f287 100644
--- a/drivers/of/Makefile
+++ b/drivers/of/Makefile
@@ -9,3 +9,4 @@ obj-$(CONFIG_OF_I2C)	+= of_i2c.o
 obj-$(CONFIG_OF_NET)	+= of_net.o
 obj-$(CONFIG_OF_SPI)	+= of_spi.o
 obj-$(CONFIG_OF_MDIO)	+= of_mdio.o
+obj-$(CONFIG_OF_PCI)	+= of_pci.o
diff --git a/drivers/of/device.c b/drivers/of/device.c
index 45d86530799f..62b4b32ac887 100644
--- a/drivers/of/device.c
+++ b/drivers/of/device.c
@@ -47,40 +47,6 @@ void of_dev_put(struct platform_device *dev)
 }
 EXPORT_SYMBOL(of_dev_put);
 
-static ssize_t devspec_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct platform_device *ofdev;
-
-	ofdev = to_platform_device(dev);
-	return sprintf(buf, "%s\n", ofdev->dev.of_node->full_name);
-}
-
-static ssize_t name_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct platform_device *ofdev;
-
-	ofdev = to_platform_device(dev);
-	return sprintf(buf, "%s\n", ofdev->dev.of_node->name);
-}
-
-static ssize_t modalias_show(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	ssize_t len = of_device_get_modalias(dev, buf, PAGE_SIZE - 2);
-	buf[len] = '\n';
-	buf[len+1] = 0;
-	return len+1;
-}
-
-struct device_attribute of_platform_device_attrs[] = {
-	__ATTR_RO(devspec),
-	__ATTR_RO(name),
-	__ATTR_RO(modalias),
-	__ATTR_NULL
-};
-
 int of_device_add(struct platform_device *ofdev)
 {
 	BUG_ON(ofdev->dev.of_node == NULL);
diff --git a/drivers/of/of_pci.c b/drivers/of/of_pci.c
new file mode 100644
index 000000000000..ac1ec54e4fd5
--- /dev/null
+++ b/drivers/of/of_pci.c
@@ -0,0 +1,92 @@
+#include <linux/kernel.h>
+#include <linux/of_pci.h>
+#include <linux/of_irq.h>
+#include <asm/prom.h>
+
+/**
+ * of_irq_map_pci - Resolve the interrupt for a PCI device
+ * @pdev:       the device whose interrupt is to be resolved
+ * @out_irq:    structure of_irq filled by this function
+ *
+ * This function resolves the PCI interrupt for a given PCI device. If a
+ * device-node exists for a given pci_dev, it will use normal OF tree
+ * walking. If not, it will implement standard swizzling and walk up the
+ * PCI tree until an device-node is found, at which point it will finish
+ * resolving using the OF tree walking.
+ */
+int of_irq_map_pci(struct pci_dev *pdev, struct of_irq *out_irq)
+{
+	struct device_node *dn, *ppnode;
+	struct pci_dev *ppdev;
+	u32 lspec;
+	__be32 lspec_be;
+	__be32 laddr[3];
+	u8 pin;
+	int rc;
+
+	/* Check if we have a device node, if yes, fallback to standard
+	 * device tree parsing
+	 */
+	dn = pci_device_to_OF_node(pdev);
+	if (dn) {
+		rc = of_irq_map_one(dn, 0, out_irq);
+		if (!rc)
+			return rc;
+	}
+
+	/* Ok, we don't, time to have fun. Let's start by building up an
+	 * interrupt spec.  we assume #interrupt-cells is 1, which is standard
+	 * for PCI. If you do different, then don't use that routine.
+	 */
+	rc = pci_read_config_byte(pdev, PCI_INTERRUPT_PIN, &pin);
+	if (rc != 0)
+		return rc;
+	/* No pin, exit */
+	if (pin == 0)
+		return -ENODEV;
+
+	/* Now we walk up the PCI tree */
+	lspec = pin;
+	for (;;) {
+		/* Get the pci_dev of our parent */
+		ppdev = pdev->bus->self;
+
+		/* Ouch, it's a host bridge... */
+		if (ppdev == NULL) {
+			ppnode = pci_bus_to_OF_node(pdev->bus);
+
+			/* No node for host bridge ? give up */
+			if (ppnode == NULL)
+				return -EINVAL;
+		} else {
+			/* We found a P2P bridge, check if it has a node */
+			ppnode = pci_device_to_OF_node(ppdev);
+		}
+
+		/* Ok, we have found a parent with a device-node, hand over to
+		 * the OF parsing code.
+		 * We build a unit address from the linux device to be used for
+		 * resolution. Note that we use the linux bus number which may
+		 * not match your firmware bus numbering.
+		 * Fortunately, in most cases, interrupt-map-mask doesn't
+		 * include the bus number as part of the matching.
+		 * You should still be careful about that though if you intend
+		 * to rely on this function (you ship  a firmware that doesn't
+		 * create device nodes for all PCI devices).
+		 */
+		if (ppnode)
+			break;
+
+		/* We can only get here if we hit a P2P bridge with no node,
+		 * let's do standard swizzling and try again
+		 */
+		lspec = pci_swizzle_interrupt_pin(pdev, lspec);
+		pdev = ppdev;
+	}
+
+	lspec_be = cpu_to_be32(lspec);
+	laddr[0] = cpu_to_be32((pdev->bus->number << 16) | (pdev->devfn << 8));
+	laddr[1]  = laddr[2] = cpu_to_be32(0);
+	return of_irq_map_raw(ppnode, &lspec_be, 1, laddr, out_irq);
+}
+EXPORT_SYMBOL_GPL(of_irq_map_pci);
diff --git a/drivers/of/pdt.c b/drivers/of/pdt.c
index 28295d0a50f6..4d87b5dc9284 100644
--- a/drivers/of/pdt.c
+++ b/drivers/of/pdt.c
@@ -36,19 +36,55 @@ unsigned int of_pdt_unique_id __initdata;
 	(p)->unique_id = of_pdt_unique_id++; \
 } while (0)
 
-static inline const char *of_pdt_node_name(struct device_node *dp)
+static char * __init of_pdt_build_full_name(struct device_node *dp)
 {
-	return dp->path_component_name;
+	int len, ourlen, plen;
+	char *n;
+
+	dp->path_component_name = build_path_component(dp);
+
+	plen = strlen(dp->parent->full_name);
+	ourlen = strlen(dp->path_component_name);
+	len = ourlen + plen + 2;
+
+	n = prom_early_alloc(len);
+	strcpy(n, dp->parent->full_name);
+	if (!of_node_is_root(dp->parent)) {
+		strcpy(n + plen, "/");
+		plen++;
+	}
+	strcpy(n + plen, dp->path_component_name);
+
+	return n;
 }
 
-#else
+#else /* CONFIG_SPARC */
 
 static inline void of_pdt_incr_unique_id(void *p) { }
 static inline void irq_trans_init(struct device_node *dp) { }
 
-static inline const char *of_pdt_node_name(struct device_node *dp)
+static char * __init of_pdt_build_full_name(struct device_node *dp)
 {
-	return dp->name;
+	static int failsafe_id = 0; /* for generating unique names on failure */
+	char *buf;
+	int len;
+
+	if (of_pdt_prom_ops->pkg2path(dp->phandle, NULL, 0, &len))
+		goto failsafe;
+
+	buf = prom_early_alloc(len + 1);
+	if (of_pdt_prom_ops->pkg2path(dp->phandle, buf, len, &len))
+		goto failsafe;
+	return buf;
+
+ failsafe:
+	buf = prom_early_alloc(strlen(dp->parent->full_name) +
+			       strlen(dp->name) + 16);
+	sprintf(buf, "%s/%s@unknown%i",
+		of_node_is_root(dp->parent) ? "" : dp->parent->full_name,
+		dp->name, failsafe_id++);
+	pr_err("%s: pkg2path failed; assigning %s\n", __func__, buf);
+	return buf;
 }
 
 #endif /* !CONFIG_SPARC */
@@ -132,47 +168,6 @@ static char * __init of_pdt_get_one_property(phandle node, const char *name)
 	return buf;
 }
 
-static char * __init of_pdt_try_pkg2path(phandle node)
-{
-	char *res, *buf = NULL;
-	int len;
-
-	if (!of_pdt_prom_ops->pkg2path)
-		return NULL;
-
-	if (of_pdt_prom_ops->pkg2path(node, buf, 0, &len))
-		return NULL;
-	buf = prom_early_alloc(len + 1);
-	if (of_pdt_prom_ops->pkg2path(node, buf, len, &len)) {
-		pr_err("%s: package-to-path failed\n", __func__);
-		return NULL;
-	}
-
-	res = strrchr(buf, '/');
-	if (!res) {
-		pr_err("%s: couldn't find / in %s\n", __func__, buf);
-		return NULL;
-	}
-	return res+1;
-}
-
-/*
- * When fetching the node's name, first try using package-to-path; if
- * that fails (either because the arch hasn't supplied a PROM callback,
- * or some other random failure), fall back to just looking at the node's
- * 'name' property.
- */
-static char * __init of_pdt_build_name(phandle node)
-{
-	char *buf;
-
-	buf = of_pdt_try_pkg2path(node);
-	if (!buf)
-		buf = of_pdt_get_one_property(node, "name");
-
-	return buf;
-}
-
 static struct device_node * __init of_pdt_create_node(phandle node,
 						    struct device_node *parent)
 {
@@ -187,7 +182,7 @@ static struct device_node * __init of_pdt_create_node(phandle node,
 
 	kref_init(&dp->kref);
 
-	dp->name = of_pdt_build_name(node);
+	dp->name = of_pdt_get_one_property(node, "name");
 	dp->type = of_pdt_get_one_property(node, "device_type");
 	dp->phandle = node;
 
@@ -198,26 +193,6 @@ static struct device_node * __init of_pdt_create_node(phandle node,
 	return dp;
 }
 
-static char * __init of_pdt_build_full_name(struct device_node *dp)
-{
-	int len, ourlen, plen;
-	char *n;
-
-	plen = strlen(dp->parent->full_name);
-	ourlen = strlen(of_pdt_node_name(dp));
-	len = ourlen + plen + 2;
-
-	n = prom_early_alloc(len);
-	strcpy(n, dp->parent->full_name);
-	if (!of_node_is_root(dp->parent)) {
-		strcpy(n + plen, "/");
-		plen++;
-	}
-	strcpy(n + plen, of_pdt_node_name(dp));
-
-	return n;
-}
-
 static struct device_node * __init of_pdt_build_tree(struct device_node *parent,
 						   phandle node,
 						   struct device_node ***nextp)
@@ -240,9 +215,6 @@ static struct device_node * __init of_pdt_build_tree(struct device_node *parent,
 		*(*nextp) = dp;
 		*nextp = &dp->allnext;
 
-#if defined(CONFIG_SPARC)
-		dp->path_component_name = build_path_component(dp);
-#endif
 		dp->full_name = of_pdt_build_full_name(dp);
 
 		dp->child = of_pdt_build_tree(dp,
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index c01cd1ac7617..1ce4c45c4ab2 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -42,471 +42,10 @@ struct platform_device *of_find_device_by_node(struct device_node *np)
 }
 EXPORT_SYMBOL(of_find_device_by_node);
 
-static int platform_driver_probe_shim(struct platform_device *pdev)
-{
-	struct platform_driver *pdrv;
-	struct of_platform_driver *ofpdrv;
-	const struct of_device_id *match;
-
-	pdrv = container_of(pdev->dev.driver, struct platform_driver, driver);
-	ofpdrv = container_of(pdrv, struct of_platform_driver, platform_driver);
-
-	/* There is an unlikely chance that an of_platform driver might match
-	 * on a non-OF platform device.  If so, then of_match_device() will
-	 * come up empty.  Return -EINVAL in this case so other drivers get
-	 * the chance to bind. */
-	match = of_match_device(pdev->dev.driver->of_match_table, &pdev->dev);
-	return match ? ofpdrv->probe(pdev, match) : -EINVAL;
-}
-
-static void platform_driver_shutdown_shim(struct platform_device *pdev)
-{
-	struct platform_driver *pdrv;
-	struct of_platform_driver *ofpdrv;
-
-	pdrv = container_of(pdev->dev.driver, struct platform_driver, driver);
-	ofpdrv = container_of(pdrv, struct of_platform_driver, platform_driver);
-	ofpdrv->shutdown(pdev);
-}
-
-/**
- * of_register_platform_driver
- */
-int of_register_platform_driver(struct of_platform_driver *drv)
-{
-	char *of_name;
-
-	/* setup of_platform_driver to platform_driver adaptors */
-	drv->platform_driver.driver = drv->driver;
-
-	/* Prefix the driver name with 'of:' to avoid namespace collisions
-	 * and bogus matches.  There are some drivers in the tree that
-	 * register both an of_platform_driver and a platform_driver with
-	 * the same name.  This is a temporary measure until they are all
-	 * cleaned up --gcl July 29, 2010 */
-	of_name = kmalloc(strlen(drv->driver.name) + 5, GFP_KERNEL);
-	if (!of_name)
-		return -ENOMEM;
-	sprintf(of_name, "of:%s", drv->driver.name);
-	drv->platform_driver.driver.name = of_name;
-
-	if (drv->probe)
-		drv->platform_driver.probe = platform_driver_probe_shim;
-	drv->platform_driver.remove = drv->remove;
-	if (drv->shutdown)
-		drv->platform_driver.shutdown = platform_driver_shutdown_shim;
-	drv->platform_driver.suspend = drv->suspend;
-	drv->platform_driver.resume = drv->resume;
-
-	return platform_driver_register(&drv->platform_driver);
-}
-EXPORT_SYMBOL(of_register_platform_driver);
-
-void of_unregister_platform_driver(struct of_platform_driver *drv)
-{
-	platform_driver_unregister(&drv->platform_driver);
-	kfree(drv->platform_driver.driver.name);
-	drv->platform_driver.driver.name = NULL;
-}
-EXPORT_SYMBOL(of_unregister_platform_driver);
-
 #if defined(CONFIG_PPC_DCR)
 #include <asm/dcr.h>
 #endif
 
-extern struct device_attribute of_platform_device_attrs[];
-
-static int of_platform_bus_match(struct device *dev, struct device_driver *drv)
-{
-	const struct of_device_id *matches = drv->of_match_table;
-
-	if (!matches)
-		return 0;
-
-	return of_match_device(matches, dev) != NULL;
-}
-
-static int of_platform_device_probe(struct device *dev)
-{
-	int error = -ENODEV;
-	struct of_platform_driver *drv;
-	struct platform_device *of_dev;
-	const struct of_device_id *match;
-
-	drv = to_of_platform_driver(dev->driver);
-	of_dev = to_platform_device(dev);
-
-	if (!drv->probe)
-		return error;
-
-	of_dev_get(of_dev);
-
-	match = of_match_device(drv->driver.of_match_table, dev);
-	if (match)
-		error = drv->probe(of_dev, match);
-	if (error)
-		of_dev_put(of_dev);
-
-	return error;
-}
-
-static int of_platform_device_remove(struct device *dev)
-{
-	struct platform_device *of_dev = to_platform_device(dev);
-	struct of_platform_driver *drv = to_of_platform_driver(dev->driver);
-
-	if (dev->driver && drv->remove)
-		drv->remove(of_dev);
-	return 0;
-}
-
-static void of_platform_device_shutdown(struct device *dev)
-{
-	struct platform_device *of_dev = to_platform_device(dev);
-	struct of_platform_driver *drv = to_of_platform_driver(dev->driver);
-
-	if (dev->driver && drv->shutdown)
-		drv->shutdown(of_dev);
-}
-
-#ifdef CONFIG_PM_SLEEP
-
-static int of_platform_legacy_suspend(struct device *dev, pm_message_t mesg)
-{
-	struct platform_device *of_dev = to_platform_device(dev);
-	struct of_platform_driver *drv = to_of_platform_driver(dev->driver);
-	int ret = 0;
-
-	if (dev->driver && drv->suspend)
-		ret = drv->suspend(of_dev, mesg);
-	return ret;
-}
-
-static int of_platform_legacy_resume(struct device *dev)
-{
-	struct platform_device *of_dev = to_platform_device(dev);
-	struct of_platform_driver *drv = to_of_platform_driver(dev->driver);
-	int ret = 0;
-
-	if (dev->driver && drv->resume)
-		ret = drv->resume(of_dev);
-	return ret;
-}
-
-static int of_platform_pm_prepare(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (drv && drv->pm && drv->pm->prepare)
-		ret = drv->pm->prepare(dev);
-
-	return ret;
-}
-
-static void of_platform_pm_complete(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-
-	if (drv && drv->pm && drv->pm->complete)
-		drv->pm->complete(dev);
-}
-
-#ifdef CONFIG_SUSPEND
-
-static int of_platform_pm_suspend(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->suspend)
-			ret = drv->pm->suspend(dev);
-	} else {
-		ret = of_platform_legacy_suspend(dev, PMSG_SUSPEND);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_suspend_noirq(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->suspend_noirq)
-			ret = drv->pm->suspend_noirq(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_resume(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->resume)
-			ret = drv->pm->resume(dev);
-	} else {
-		ret = of_platform_legacy_resume(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_resume_noirq(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->resume_noirq)
-			ret = drv->pm->resume_noirq(dev);
-	}
-
-	return ret;
-}
-
-#else /* !CONFIG_SUSPEND */
-
-#define of_platform_pm_suspend		NULL
-#define of_platform_pm_resume		NULL
-#define of_platform_pm_suspend_noirq	NULL
-#define of_platform_pm_resume_noirq	NULL
-
-#endif /* !CONFIG_SUSPEND */
-
-#ifdef CONFIG_HIBERNATION
-
-static int of_platform_pm_freeze(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->freeze)
-			ret = drv->pm->freeze(dev);
-	} else {
-		ret = of_platform_legacy_suspend(dev, PMSG_FREEZE);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_freeze_noirq(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->freeze_noirq)
-			ret = drv->pm->freeze_noirq(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_thaw(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->thaw)
-			ret = drv->pm->thaw(dev);
-	} else {
-		ret = of_platform_legacy_resume(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_thaw_noirq(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->thaw_noirq)
-			ret = drv->pm->thaw_noirq(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_poweroff(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->poweroff)
-			ret = drv->pm->poweroff(dev);
-	} else {
-		ret = of_platform_legacy_suspend(dev, PMSG_HIBERNATE);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_poweroff_noirq(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->poweroff_noirq)
-			ret = drv->pm->poweroff_noirq(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_restore(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->restore)
-			ret = drv->pm->restore(dev);
-	} else {
-		ret = of_platform_legacy_resume(dev);
-	}
-
-	return ret;
-}
-
-static int of_platform_pm_restore_noirq(struct device *dev)
-{
-	struct device_driver *drv = dev->driver;
-	int ret = 0;
-
-	if (!drv)
-		return 0;
-
-	if (drv->pm) {
-		if (drv->pm->restore_noirq)
-			ret = drv->pm->restore_noirq(dev);
-	}
-
-	return ret;
-}
-
-#else /* !CONFIG_HIBERNATION */
-
-#define of_platform_pm_freeze		NULL
-#define of_platform_pm_thaw		NULL
-#define of_platform_pm_poweroff		NULL
-#define of_platform_pm_restore		NULL
-#define of_platform_pm_freeze_noirq	NULL
-#define of_platform_pm_thaw_noirq		NULL
-#define of_platform_pm_poweroff_noirq	NULL
-#define of_platform_pm_restore_noirq	NULL
-
-#endif /* !CONFIG_HIBERNATION */
-
-static struct dev_pm_ops of_platform_dev_pm_ops = {
-	.prepare = of_platform_pm_prepare,
-	.complete = of_platform_pm_complete,
-	.suspend = of_platform_pm_suspend,
-	.resume = of_platform_pm_resume,
-	.freeze = of_platform_pm_freeze,
-	.thaw = of_platform_pm_thaw,
-	.poweroff = of_platform_pm_poweroff,
-	.restore = of_platform_pm_restore,
-	.suspend_noirq = of_platform_pm_suspend_noirq,
-	.resume_noirq = of_platform_pm_resume_noirq,
-	.freeze_noirq = of_platform_pm_freeze_noirq,
-	.thaw_noirq = of_platform_pm_thaw_noirq,
-	.poweroff_noirq = of_platform_pm_poweroff_noirq,
-	.restore_noirq = of_platform_pm_restore_noirq,
-};
-
-#define OF_PLATFORM_PM_OPS_PTR	(&of_platform_dev_pm_ops)
-
-#else /* !CONFIG_PM_SLEEP */
-
-#define OF_PLATFORM_PM_OPS_PTR	NULL
-
-#endif /* !CONFIG_PM_SLEEP */
-
-int of_bus_type_init(struct bus_type *bus, const char *name)
-{
-	bus->name = name;
-	bus->match = of_platform_bus_match;
-	bus->probe = of_platform_device_probe;
-	bus->remove = of_platform_device_remove;
-	bus->shutdown = of_platform_device_shutdown;
-	bus->dev_attrs = of_platform_device_attrs;
-	bus->pm = OF_PLATFORM_PM_OPS_PTR;
-	return bus_register(bus);
-}
-
-int of_register_driver(struct of_platform_driver *drv, struct bus_type *bus)
-{
-	/*
-	 * Temporary: of_platform_bus used to be distinct from the platform
-	 * bus.  It isn't anymore, and so drivers on the platform bus need
-	 * to be registered in a special way.
-	 *
-	 * After all of_platform_bus_type drivers are converted to
-	 * platform_drivers, this exception can be removed.
-	 */
-	if (bus == &platform_bus_type)
-		return of_register_platform_driver(drv);
-
-	/* register with core */
-	drv->driver.bus = bus;
-	return driver_register(&drv->driver);
-}
-EXPORT_SYMBOL(of_register_driver);
-
-void of_unregister_driver(struct of_platform_driver *drv)
-{
-	if (drv->driver.bus == &platform_bus_type)
-		of_unregister_platform_driver(drv);
-	else
-		driver_unregister(&drv->driver);
-}
-EXPORT_SYMBOL(of_unregister_driver);
-
 #if !defined(CONFIG_SPARC)
 /*
  * The following routines scan a subtree and registers a device for
diff --git a/drivers/oprofile/cpu_buffer.c b/drivers/oprofile/cpu_buffer.c
index 59f55441e075..b8ef8ddcc292 100644
--- a/drivers/oprofile/cpu_buffer.c
+++ b/drivers/oprofile/cpu_buffer.c
@@ -258,8 +258,10 @@ op_add_sample(struct oprofile_cpu_buffer *cpu_buf,
  */
 static int
 log_sample(struct oprofile_cpu_buffer *cpu_buf, unsigned long pc,
-	   unsigned long backtrace, int is_kernel, unsigned long event)
+	   unsigned long backtrace, int is_kernel, unsigned long event,
+	   struct task_struct *task)
 {
+	struct task_struct *tsk = task ? task : current;
 	cpu_buf->sample_received++;
 
 	if (pc == ESCAPE_CODE) {
@@ -267,7 +269,7 @@ log_sample(struct oprofile_cpu_buffer *cpu_buf, unsigned long pc,
 		return 0;
 	}
 
-	if (op_add_code(cpu_buf, backtrace, is_kernel, current))
+	if (op_add_code(cpu_buf, backtrace, is_kernel, tsk))
 		goto fail;
 
 	if (op_add_sample(cpu_buf, pc, event))
@@ -292,7 +294,8 @@ static inline void oprofile_end_trace(struct oprofile_cpu_buffer *cpu_buf)
 
 static inline void
 __oprofile_add_ext_sample(unsigned long pc, struct pt_regs * const regs,
-			  unsigned long event, int is_kernel)
+			  unsigned long event, int is_kernel,
+			  struct task_struct *task)
 {
 	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
 	unsigned long backtrace = oprofile_backtrace_depth;
@@ -301,7 +304,7 @@ __oprofile_add_ext_sample(unsigned long pc, struct pt_regs * const regs,
 	 * if log_sample() fail we can't backtrace since we lost the
 	 * source of this event
 	 */
-	if (!log_sample(cpu_buf, pc, backtrace, is_kernel, event))
+	if (!log_sample(cpu_buf, pc, backtrace, is_kernel, event, task))
 		/* failed */
 		return;
 
@@ -313,10 +316,17 @@ __oprofile_add_ext_sample(unsigned long pc, struct pt_regs * const regs,
 	oprofile_end_trace(cpu_buf);
 }
 
+void oprofile_add_ext_hw_sample(unsigned long pc, struct pt_regs * const regs,
+				unsigned long event, int is_kernel,
+				struct task_struct *task)
+{
+	__oprofile_add_ext_sample(pc, regs, event, is_kernel, task);
+}
+
 void oprofile_add_ext_sample(unsigned long pc, struct pt_regs * const regs,
 			     unsigned long event, int is_kernel)
 {
-	__oprofile_add_ext_sample(pc, regs, event, is_kernel);
+	__oprofile_add_ext_sample(pc, regs, event, is_kernel, NULL);
 }
 
 void oprofile_add_sample(struct pt_regs * const regs, unsigned long event)
@@ -332,7 +342,7 @@ void oprofile_add_sample(struct pt_regs * const regs, unsigned long event)
 		pc = ESCAPE_CODE; /* as this causes an early return. */
 	}
 
-	__oprofile_add_ext_sample(pc, regs, event, is_kernel);
+	__oprofile_add_ext_sample(pc, regs, event, is_kernel, NULL);
 }
 
 /*
@@ -403,7 +413,7 @@ int oprofile_write_commit(struct op_entry *entry)
 void oprofile_add_pc(unsigned long pc, int is_kernel, unsigned long event)
 {
 	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
-	log_sample(cpu_buf, pc, 0, is_kernel, event);
+	log_sample(cpu_buf, pc, 0, is_kernel, event, NULL);
 }
 
 void oprofile_add_trace(unsigned long pc)
diff --git a/drivers/oprofile/timer_int.c b/drivers/oprofile/timer_int.c
index 010725117dbb..3ef44624f510 100644
--- a/drivers/oprofile/timer_int.c
+++ b/drivers/oprofile/timer_int.c
@@ -97,7 +97,7 @@ static struct notifier_block __refdata oprofile_cpu_notifier = {
 	.notifier_call = oprofile_cpu_notify,
 };
 
-int __init oprofile_timer_init(struct oprofile_operations *ops)
+int oprofile_timer_init(struct oprofile_operations *ops)
 {
 	int rc;
 
@@ -113,7 +113,7 @@ int __init oprofile_timer_init(struct oprofile_operations *ops)
 	return 0;
 }
 
-void __exit oprofile_timer_exit(void)
+void oprofile_timer_exit(void)
 {
 	unregister_hotcpu_notifier(&oprofile_cpu_notifier);
 }
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index 9383063d2b16..bcd5d54b7d4d 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -296,25 +296,25 @@ static struct pci_port_ops dino_port_ops = {
 	.outl	= dino_out32
 };
 
-static void dino_mask_irq(unsigned int irq)
+static void dino_mask_irq(struct irq_data *d)
 {
-	struct dino_device *dino_dev = get_irq_chip_data(irq);
-	int local_irq = gsc_find_local_irq(irq, dino_dev->global_irq, DINO_LOCAL_IRQS);
+	struct dino_device *dino_dev = irq_data_get_irq_chip_data(d);
+	int local_irq = gsc_find_local_irq(d->irq, dino_dev->global_irq, DINO_LOCAL_IRQS);
 
-	DBG(KERN_WARNING "%s(0x%p, %d)\n", __func__, dino_dev, irq);
+	DBG(KERN_WARNING "%s(0x%p, %d)\n", __func__, dino_dev, d->irq);
 
 	/* Clear the matching bit in the IMR register */
 	dino_dev->imr &= ~(DINO_MASK_IRQ(local_irq));
 	__raw_writel(dino_dev->imr, dino_dev->hba.base_addr+DINO_IMR);
 }
 
-static void dino_unmask_irq(unsigned int irq)
+static void dino_unmask_irq(struct irq_data *d)
 {
-	struct dino_device *dino_dev = get_irq_chip_data(irq);
-	int local_irq = gsc_find_local_irq(irq, dino_dev->global_irq, DINO_LOCAL_IRQS);
+	struct dino_device *dino_dev = irq_data_get_irq_chip_data(d);
+	int local_irq = gsc_find_local_irq(d->irq, dino_dev->global_irq, DINO_LOCAL_IRQS);
 	u32 tmp;
 
-	DBG(KERN_WARNING "%s(0x%p, %d)\n", __func__, dino_dev, irq);
+	DBG(KERN_WARNING "%s(0x%p, %d)\n", __func__, dino_dev, d->irq);
 
 	/*
 	** clear pending IRQ bits
@@ -346,9 +346,9 @@ static void dino_unmask_irq(unsigned int irq)
 }
 
 static struct irq_chip dino_interrupt_type = {
-	.name	= "GSC-PCI",
-	.unmask	= dino_unmask_irq,
-	.mask	= dino_mask_irq,
+	.name		= "GSC-PCI",
+	.irq_unmask	= dino_unmask_irq,
+	.irq_mask	= dino_mask_irq,
 };
 
 
diff --git a/drivers/parisc/eisa.c b/drivers/parisc/eisa.c
index e860038b0b84..deeec32a5803 100644
--- a/drivers/parisc/eisa.c
+++ b/drivers/parisc/eisa.c
@@ -144,8 +144,9 @@ static unsigned int eisa_irq_level __read_mostly; /* default to edge triggered *
 
 
 /* called by free irq */
-static void eisa_mask_irq(unsigned int irq)
+static void eisa_mask_irq(struct irq_data *d)
 {
+	unsigned int irq = d->irq;
 	unsigned long flags;
 
 	EISA_DBG("disable irq %d\n", irq);
@@ -164,8 +165,9 @@ static void eisa_mask_irq(unsigned int irq)
 }
 
 /* called by request irq */
-static void eisa_unmask_irq(unsigned int irq)
+static void eisa_unmask_irq(struct irq_data *d)
 {
+	unsigned int irq = d->irq;
 	unsigned long flags;
 	EISA_DBG("enable irq %d\n", irq);
 		
@@ -183,9 +185,9 @@ static void eisa_unmask_irq(unsigned int irq)
 }
 
 static struct irq_chip eisa_interrupt_type = {
-	.name	=	"EISA",
-	.unmask	=	eisa_unmask_irq,
-	.mask	=	eisa_mask_irq,
+	.name		=	"EISA",
+	.irq_unmask	=	eisa_unmask_irq,
+	.irq_mask	=	eisa_mask_irq,
 };
 
 static irqreturn_t eisa_irq(int wax_irq, void *intr_dev)
diff --git a/drivers/parisc/gsc.c b/drivers/parisc/gsc.c
index 772b1939ac21..ef31080cf591 100644
--- a/drivers/parisc/gsc.c
+++ b/drivers/parisc/gsc.c
@@ -105,13 +105,13 @@ int gsc_find_local_irq(unsigned int irq, int *global_irqs, int limit)
 	return NO_IRQ;
 }
 
-static void gsc_asic_mask_irq(unsigned int irq)
+static void gsc_asic_mask_irq(struct irq_data *d)
 {
-	struct gsc_asic *irq_dev = get_irq_chip_data(irq);
-	int local_irq = gsc_find_local_irq(irq, irq_dev->global_irq, 32);
+	struct gsc_asic *irq_dev = irq_data_get_irq_chip_data(d);
+	int local_irq = gsc_find_local_irq(d->irq, irq_dev->global_irq, 32);
 	u32 imr;
 
-	DEBPRINTK(KERN_DEBUG "%s(%d) %s: IMR 0x%x\n", __func__, irq,
+	DEBPRINTK(KERN_DEBUG "%s(%d) %s: IMR 0x%x\n", __func__, d->irq,
 			irq_dev->name, imr);
 
 	/* Disable the IRQ line by clearing the bit in the IMR */
@@ -120,13 +120,13 @@ static void gsc_asic_mask_irq(unsigned int irq)
 	gsc_writel(imr, irq_dev->hpa + OFFSET_IMR);
 }
 
-static void gsc_asic_unmask_irq(unsigned int irq)
+static void gsc_asic_unmask_irq(struct irq_data *d)
 {
-	struct gsc_asic *irq_dev = get_irq_chip_data(irq);
-	int local_irq = gsc_find_local_irq(irq, irq_dev->global_irq, 32);
+	struct gsc_asic *irq_dev = irq_data_get_irq_chip_data(d);
+	int local_irq = gsc_find_local_irq(d->irq, irq_dev->global_irq, 32);
 	u32 imr;
 
-	DEBPRINTK(KERN_DEBUG "%s(%d) %s: IMR 0x%x\n", __func__, irq,
+	DEBPRINTK(KERN_DEBUG "%s(%d) %s: IMR 0x%x\n", __func__, d->irq,
 			irq_dev->name, imr);
 
 	/* Enable the IRQ line by setting the bit in the IMR */
@@ -140,9 +140,9 @@ static void gsc_asic_unmask_irq(unsigned int irq)
 }
 
 static struct irq_chip gsc_asic_interrupt_type = {
-	.name	=	"GSC-ASIC",
-	.unmask	=	gsc_asic_unmask_irq,
-	.mask	=	gsc_asic_mask_irq,
+	.name		=	"GSC-ASIC",
+	.irq_unmask	=	gsc_asic_unmask_irq,
+	.irq_mask	=	gsc_asic_mask_irq,
 };
 
 int gsc_assign_irq(struct irq_chip *type, void *data)
diff --git a/drivers/parisc/iosapic.c b/drivers/parisc/iosapic.c
index 0327894bf235..95930d016235 100644
--- a/drivers/parisc/iosapic.c
+++ b/drivers/parisc/iosapic.c
@@ -615,10 +615,10 @@ iosapic_set_irt_data( struct vector_info *vi, u32 *dp0, u32 *dp1)
 }
 
 
-static void iosapic_mask_irq(unsigned int irq)
+static void iosapic_mask_irq(struct irq_data *d)
 {
 	unsigned long flags;
-	struct vector_info *vi = get_irq_chip_data(irq);
+	struct vector_info *vi = irq_data_get_irq_chip_data(d);
 	u32 d0, d1;
 
 	spin_lock_irqsave(&iosapic_lock, flags);
@@ -628,9 +628,9 @@ static void iosapic_mask_irq(unsigned int irq)
 	spin_unlock_irqrestore(&iosapic_lock, flags);
 }
 
-static void iosapic_unmask_irq(unsigned int irq)
+static void iosapic_unmask_irq(struct irq_data *d)
 {
-	struct vector_info *vi = get_irq_chip_data(irq);
+	struct vector_info *vi = irq_data_get_irq_chip_data(d);
 	u32 d0, d1;
 
 	/* data is initialized by fixup_irq */
@@ -666,34 +666,34 @@ printk("\n");
 	 * enables their IRQ. It can lead to "interesting" race conditions
 	 * in the driver initialization sequence.
 	 */
-	DBG(KERN_DEBUG "enable_irq(%d): eoi(%p, 0x%x)\n", irq,
+	DBG(KERN_DEBUG "enable_irq(%d): eoi(%p, 0x%x)\n", d->irq,
 			vi->eoi_addr, vi->eoi_data);
 	iosapic_eoi(vi->eoi_addr, vi->eoi_data);
 }
 
-static void iosapic_eoi_irq(unsigned int irq)
+static void iosapic_eoi_irq(struct irq_data *d)
 {
-	struct vector_info *vi = get_irq_chip_data(irq);
+	struct vector_info *vi = irq_data_get_irq_chip_data(d);
 
 	iosapic_eoi(vi->eoi_addr, vi->eoi_data);
-	cpu_eoi_irq(irq);
+	cpu_eoi_irq(d);
 }
 
 #ifdef CONFIG_SMP
-static int iosapic_set_affinity_irq(unsigned int irq,
-				     const struct cpumask *dest)
+static int iosapic_set_affinity_irq(struct irq_data *d,
+				    const struct cpumask *dest, bool force)
 {
-	struct vector_info *vi = get_irq_chip_data(irq);
+	struct vector_info *vi = irq_data_get_irq_chip_data(d);
 	u32 d0, d1, dummy_d0;
 	unsigned long flags;
 	int dest_cpu;
 
-	dest_cpu = cpu_check_affinity(irq, dest);
+	dest_cpu = cpu_check_affinity(d, dest);
 	if (dest_cpu < 0)
 		return -1;
 
-	cpumask_copy(irq_desc[irq].affinity, cpumask_of(dest_cpu));
-	vi->txn_addr = txn_affinity_addr(irq, dest_cpu);
+	cpumask_copy(d->affinity, cpumask_of(dest_cpu));
+	vi->txn_addr = txn_affinity_addr(d->irq, dest_cpu);
 
 	spin_lock_irqsave(&iosapic_lock, flags);
 	/* d1 contains the destination CPU, so only want to set that
@@ -708,13 +708,13 @@ static int iosapic_set_affinity_irq(unsigned int irq,
 #endif
 
 static struct irq_chip iosapic_interrupt_type = {
-	.name	=	"IO-SAPIC-level",
-	.unmask	=	iosapic_unmask_irq,
-	.mask	=	iosapic_mask_irq,
-	.ack	=	cpu_ack_irq,
-	.eoi	=	iosapic_eoi_irq,
+	.name		=	"IO-SAPIC-level",
+	.irq_unmask	=	iosapic_unmask_irq,
+	.irq_mask	=	iosapic_mask_irq,
+	.irq_ack	=	cpu_ack_irq,
+	.irq_eoi	=	iosapic_eoi_irq,
 #ifdef CONFIG_SMP
-	.set_affinity =	iosapic_set_affinity_irq,
+	.irq_set_affinity =	iosapic_set_affinity_irq,
 #endif
 };
 
diff --git a/drivers/parisc/superio.c b/drivers/parisc/superio.c
index 28241532c0fd..a4d8ff66a639 100644
--- a/drivers/parisc/superio.c
+++ b/drivers/parisc/superio.c
@@ -286,8 +286,9 @@ superio_init(struct pci_dev *pcidev)
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_NS, PCI_DEVICE_ID_NS_87560_LIO, superio_init);
 
-static void superio_mask_irq(unsigned int irq)
+static void superio_mask_irq(struct irq_data *d)
 {
+	unsigned int irq = d->irq;
 	u8 r8;
 
 	if ((irq < 1) || (irq == 2) || (irq > 7)) {
@@ -303,8 +304,9 @@ static void superio_mask_irq(unsigned int irq)
 	outb (r8,IC_PIC1+1);
 }
 
-static void superio_unmask_irq(unsigned int irq)
+static void superio_unmask_irq(struct irq_data *d)
 {
+	unsigned int irq = d->irq;
 	u8 r8;
 
 	if ((irq < 1) || (irq == 2) || (irq > 7)) {
@@ -320,9 +322,9 @@ static void superio_unmask_irq(unsigned int irq)
 }
 
 static struct irq_chip superio_interrupt_type = {
-	.name	=	SUPERIO,
-	.unmask	=	superio_unmask_irq,
-	.mask	=	superio_mask_irq,
+	.name		=	SUPERIO,
+	.irq_unmask	=	superio_unmask_irq,
+	.irq_mask	=	superio_mask_irq,
 };
 
 #ifdef DEBUG_SUPERIO_INIT
diff --git a/drivers/parport/parport_sunbpp.c b/drivers/parport/parport_sunbpp.c
index 55ba118f1cf1..910c5a26e347 100644
--- a/drivers/parport/parport_sunbpp.c
+++ b/drivers/parport/parport_sunbpp.c
@@ -286,7 +286,7 @@ static struct parport_operations parport_sunbpp_ops =
 	.owner		= THIS_MODULE,
 };
 
-static int __devinit bpp_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit bpp_probe(struct platform_device *op)
 {
 	struct parport_operations *ops;
 	struct bpp_regs __iomem *regs;
@@ -381,7 +381,7 @@ static const struct of_device_id bpp_match[] = {
 
 MODULE_DEVICE_TABLE(of, bpp_match);
 
-static struct of_platform_driver bpp_sbus_driver = {
+static struct platform_driver bpp_sbus_driver = {
 	.driver = {
 		.name = "bpp",
 		.owner = THIS_MODULE,
@@ -393,12 +393,12 @@ static struct of_platform_driver bpp_sbus_driver = {
 
 static int __init parport_sunbpp_init(void)
 {
-	return of_register_platform_driver(&bpp_sbus_driver);
+	return platform_driver_register(&bpp_sbus_driver);
 }
 
 static void __exit parport_sunbpp_exit(void)
 {
-	of_unregister_platform_driver(&bpp_sbus_driver);
+	platform_driver_unregister(&bpp_sbus_driver);
 }
 
 MODULE_AUTHOR("Derrick J Brashear");
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index a9523fdc6911..c8ff646c0b05 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -87,3 +87,5 @@ config PCI_IOAPIC
 	depends on ACPI
 	depends on HOTPLUG
 	default y
+
+select NLS if (DMI || ACPI)
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index 98e6fdf34d30..98d61c8e984b 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -42,6 +42,7 @@ obj-$(CONFIG_PCI_IOV) += iov.o
 obj-$(CONFIG_X86) += setup-bus.o
 obj-$(CONFIG_ALPHA) += setup-bus.o setup-irq.o
 obj-$(CONFIG_ARM) += setup-bus.o setup-irq.o
+obj-$(CONFIG_UNICORE32) += setup-bus.o setup-irq.o
 obj-$(CONFIG_PARISC) += setup-bus.o
 obj-$(CONFIG_SUPERH) += setup-bus.o setup-irq.o
 obj-$(CONFIG_PPC) += setup-bus.o
@@ -53,8 +54,9 @@ obj-$(CONFIG_TILE) += setup-bus.o setup-irq.o
 
 #
 # ACPI Related PCI FW Functions
+# ACPI _DSM provided firmware instance and string name
 #
-obj-$(CONFIG_ACPI)    += pci-acpi.o
+obj-$(CONFIG_ACPI)    += pci-acpi.o pci-label.o
 
 # SMBIOS provided firmware instance and labels
 obj-$(CONFIG_DMI)    += pci-label.o
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index cb23aa2ebf96..e610cfe4f07b 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -212,6 +212,7 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 
 	pdev = pci_get_slot(pbus, PCI_DEVFN(device, function));
 	if (pdev) {
+		pdev->current_state = PCI_D0;
 		slot->flags |= (SLOT_ENABLED | SLOT_POWEREDON);
 		pci_dev_put(pdev);
 	}
diff --git a/drivers/pci/intel-iommu.c b/drivers/pci/intel-iommu.c
index 4789f8e8bf7a..35463ddf10a1 100644
--- a/drivers/pci/intel-iommu.c
+++ b/drivers/pci/intel-iommu.c
@@ -3627,9 +3627,9 @@ static int intel_iommu_attach_device(struct iommu_domain *domain,
 
 		pte = dmar_domain->pgd;
 		if (dma_pte_present(pte)) {
-			free_pgtable_page(dmar_domain->pgd);
 			dmar_domain->pgd = (struct dma_pte *)
 				phys_to_virt(dma_pte_addr(pte));
+			free_pgtable_page(pte);
 		}
 		dmar_domain->agaw--;
 	}
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 6fe0772e0e7d..7c3b18e78cee 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -293,19 +293,11 @@ static int acpi_dev_run_wake(struct device *phys_dev, bool enable)
 	}
 
 	if (enable) {
-		if (!dev->wakeup.run_wake_count++) {
-			acpi_enable_wakeup_device_power(dev, ACPI_STATE_S0);
-			acpi_enable_gpe(dev->wakeup.gpe_device,
-					dev->wakeup.gpe_number);
-		}
-	} else if (dev->wakeup.run_wake_count > 0) {
-		if (!--dev->wakeup.run_wake_count) {
-			acpi_disable_gpe(dev->wakeup.gpe_device,
-					 dev->wakeup.gpe_number);
-			acpi_disable_wakeup_device_power(dev);
-		}
+		acpi_enable_wakeup_device_power(dev, ACPI_STATE_S0);
+		acpi_enable_gpe(dev->wakeup.gpe_device, dev->wakeup.gpe_number);
 	} else {
-		error = -EALREADY;
+		acpi_disable_gpe(dev->wakeup.gpe_device, dev->wakeup.gpe_number);
+		acpi_disable_wakeup_device_power(dev);
 	}
 
 	return error;
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index 88246dd46452..d86ea8b01137 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -431,7 +431,7 @@ static void pci_device_shutdown(struct device *dev)
 	pci_msix_shutdown(pci_dev);
 }
 
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 
 /* Auxiliary functions used for system resume and run-time resume. */
 
@@ -1059,7 +1059,7 @@ static int pci_pm_runtime_idle(struct device *dev)
 
 #endif /* !CONFIG_PM_RUNTIME */
 
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 
 const struct dev_pm_ops pci_dev_pm_ops = {
 	.prepare = pci_pm_prepare,
diff --git a/drivers/pci/pci-label.c b/drivers/pci/pci-label.c
index 90c0a729cd3a..824e247edc58 100644
--- a/drivers/pci/pci-label.c
+++ b/drivers/pci/pci-label.c
@@ -5,6 +5,13 @@
  * by Narendra K <Narendra_K@dell.com>,
  * Jordan Hargrave <Jordan_Hargrave@dell.com>
  *
+ * PCI Firmware Specification Revision 3.1 section 4.6.7 (DSM for Naming a
+ * PCI or PCI Express Device Under Operating Systems) defines an instance
+ * number and string name. This code retrieves them and exports them to sysfs.
+ * If the system firmware does not provide the ACPI _DSM (Device Specific
+ * Method), then the SMBIOS type 41 instance number and string is exported to
+ * sysfs.
+ *
  * SMBIOS defines type 41 for onboard pci devices. This code retrieves
  * the instance number and string from the type 41 record and exports
  * it to sysfs.
@@ -19,8 +26,30 @@
 #include <linux/pci_ids.h>
 #include <linux/module.h>
 #include <linux/device.h>
+#include <linux/nls.h>
+#include <linux/acpi.h>
+#include <linux/pci-acpi.h>
+#include <acpi/acpi_drivers.h>
+#include <acpi/acpi_bus.h>
 #include "pci.h"
 
+#define	DEVICE_LABEL_DSM	0x07
+
+#ifndef CONFIG_DMI
+
+static inline int
+pci_create_smbiosname_file(struct pci_dev *pdev)
+{
+	return -1;
+}
+
+static inline void
+pci_remove_smbiosname_file(struct pci_dev *pdev)
+{
+}
+
+#else
+
 enum smbios_attr_enum {
 	SMBIOS_ATTR_NONE = 0,
 	SMBIOS_ATTR_LABEL_SHOW,
@@ -120,9 +149,7 @@ static struct attribute_group smbios_attr_group = {
 static int
 pci_create_smbiosname_file(struct pci_dev *pdev)
 {
-	if (!sysfs_create_group(&pdev->dev.kobj, &smbios_attr_group))
-		return 0;
-	return -ENODEV;
+	return sysfs_create_group(&pdev->dev.kobj, &smbios_attr_group);
 }
 
 static void
@@ -131,13 +158,221 @@ pci_remove_smbiosname_file(struct pci_dev *pdev)
 	sysfs_remove_group(&pdev->dev.kobj, &smbios_attr_group);
 }
 
+#endif
+
+#ifndef CONFIG_ACPI
+
+static inline int
+pci_create_acpi_index_label_files(struct pci_dev *pdev)
+{
+	return -1;
+}
+
+static inline int
+pci_remove_acpi_index_label_files(struct pci_dev *pdev)
+{
+	return -1;
+}
+
+#else
+
+static const char device_label_dsm_uuid[] = {
+	0xD0, 0x37, 0xC9, 0xE5, 0x53, 0x35, 0x7A, 0x4D,
+	0x91, 0x17, 0xEA, 0x4D, 0x19, 0xC3, 0x43, 0x4D
+};
+
+enum acpi_attr_enum {
+	ACPI_ATTR_NONE = 0,
+	ACPI_ATTR_LABEL_SHOW,
+	ACPI_ATTR_INDEX_SHOW,
+};
+
+static void dsm_label_utf16s_to_utf8s(union acpi_object *obj, char *buf)
+{
+	int len;
+	len = utf16s_to_utf8s((const wchar_t *)obj->
+			      package.elements[1].string.pointer,
+			      obj->package.elements[1].string.length,
+			      UTF16_LITTLE_ENDIAN,
+			      buf, PAGE_SIZE);
+	buf[len] = '\n';
+}
+
+static int
+dsm_get_label(acpi_handle handle, int func,
+	      struct acpi_buffer *output,
+	      char *buf, enum acpi_attr_enum attribute)
+{
+	struct acpi_object_list input;
+	union acpi_object params[4];
+	union acpi_object *obj;
+	int len = 0;
+
+	int err;
+
+	input.count = 4;
+	input.pointer = params;
+	params[0].type = ACPI_TYPE_BUFFER;
+	params[0].buffer.length = sizeof(device_label_dsm_uuid);
+	params[0].buffer.pointer = (char *)device_label_dsm_uuid;
+	params[1].type = ACPI_TYPE_INTEGER;
+	params[1].integer.value = 0x02;
+	params[2].type = ACPI_TYPE_INTEGER;
+	params[2].integer.value = func;
+	params[3].type = ACPI_TYPE_PACKAGE;
+	params[3].package.count = 0;
+	params[3].package.elements = NULL;
+
+	err = acpi_evaluate_object(handle, "_DSM", &input, output);
+	if (err)
+		return -1;
+
+	obj = (union acpi_object *)output->pointer;
+
+	switch (obj->type) {
+	case ACPI_TYPE_PACKAGE:
+		if (obj->package.count != 2)
+			break;
+		len = obj->package.elements[0].integer.value;
+		if (buf) {
+			if (attribute == ACPI_ATTR_INDEX_SHOW)
+				scnprintf(buf, PAGE_SIZE, "%llu\n",
+				obj->package.elements[0].integer.value);
+			else if (attribute == ACPI_ATTR_LABEL_SHOW)
+				dsm_label_utf16s_to_utf8s(obj, buf);
+			kfree(output->pointer);
+			return strlen(buf);
+		}
+		kfree(output->pointer);
+		return len;
+	break;
+	default:
+		kfree(output->pointer);
+	}
+	return -1;
+}
+
+static bool
+device_has_dsm(struct device *dev)
+{
+	acpi_handle handle;
+	struct acpi_buffer output = {ACPI_ALLOCATE_BUFFER, NULL};
+
+	handle = DEVICE_ACPI_HANDLE(dev);
+
+	if (!handle)
+		return FALSE;
+
+	if (dsm_get_label(handle, DEVICE_LABEL_DSM, &output, NULL,
+			  ACPI_ATTR_NONE) > 0)
+		return TRUE;
+
+	return FALSE;
+}
+
+static mode_t
+acpi_index_string_exist(struct kobject *kobj, struct attribute *attr, int n)
+{
+	struct device *dev;
+
+	dev = container_of(kobj, struct device, kobj);
+
+	if (device_has_dsm(dev))
+		return S_IRUGO;
+
+	return 0;
+}
+
+static ssize_t
+acpilabel_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct acpi_buffer output = {ACPI_ALLOCATE_BUFFER, NULL};
+	acpi_handle handle;
+	int length;
+
+	handle = DEVICE_ACPI_HANDLE(dev);
+
+	if (!handle)
+		return -1;
+
+	length = dsm_get_label(handle, DEVICE_LABEL_DSM,
+			       &output, buf, ACPI_ATTR_LABEL_SHOW);
+
+	if (length < 1)
+		return -1;
+
+	return length;
+}
+
+static ssize_t
+acpiindex_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct acpi_buffer output = {ACPI_ALLOCATE_BUFFER, NULL};
+	acpi_handle handle;
+	int length;
+
+	handle = DEVICE_ACPI_HANDLE(dev);
+
+	if (!handle)
+		return -1;
+
+	length = dsm_get_label(handle, DEVICE_LABEL_DSM,
+			       &output, buf, ACPI_ATTR_INDEX_SHOW);
+
+	if (length < 0)
+		return -1;
+
+	return length;
+
+}
+
+static struct device_attribute acpi_attr_label = {
+	.attr = {.name = "label", .mode = 0444},
+	.show = acpilabel_show,
+};
+
+static struct device_attribute acpi_attr_index = {
+	.attr = {.name = "acpi_index", .mode = 0444},
+	.show = acpiindex_show,
+};
+
+static struct attribute *acpi_attributes[] = {
+	&acpi_attr_label.attr,
+	&acpi_attr_index.attr,
+	NULL,
+};
+
+static struct attribute_group acpi_attr_group = {
+	.attrs = acpi_attributes,
+	.is_visible = acpi_index_string_exist,
+};
+
+static int
+pci_create_acpi_index_label_files(struct pci_dev *pdev)
+{
+	return sysfs_create_group(&pdev->dev.kobj, &acpi_attr_group);
+}
+
+static int
+pci_remove_acpi_index_label_files(struct pci_dev *pdev)
+{
+	sysfs_remove_group(&pdev->dev.kobj, &acpi_attr_group);
+	return 0;
+}
+#endif
+
 void pci_create_firmware_label_files(struct pci_dev *pdev)
 {
-	if (!pci_create_smbiosname_file(pdev))
-		;
+	if (device_has_dsm(&pdev->dev))
+		pci_create_acpi_index_label_files(pdev);
+	else
+		pci_create_smbiosname_file(pdev);
 }
 
 void pci_remove_firmware_label_files(struct pci_dev *pdev)
 {
-	pci_remove_smbiosname_file(pdev);
+	if (device_has_dsm(&pdev->dev))
+		pci_remove_acpi_index_label_files(pdev);
+	else
+		pci_remove_smbiosname_file(pdev);
 }
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index ea25e5bfcf23..c85438a367d5 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -1088,7 +1088,7 @@ static int pci_create_capabilities_sysfs(struct pci_dev *dev)
 		attr->write = write_vpd_attr;
 		retval = sysfs_create_bin_file(&dev->dev.kobj, attr);
 		if (retval) {
-			kfree(dev->vpd->attr);
+			kfree(attr);
 			return retval;
 		}
 		dev->vpd->attr = attr;
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index f69d6e0fda75..a6ec200fe5ee 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -11,7 +11,7 @@
 extern int pci_uevent(struct device *dev, struct kobj_uevent_env *env);
 extern int pci_create_sysfs_dev_files(struct pci_dev *pdev);
 extern void pci_remove_sysfs_dev_files(struct pci_dev *pdev);
-#ifndef CONFIG_DMI
+#if !defined(CONFIG_DMI) && !defined(CONFIG_ACPI)
 static inline void pci_create_firmware_label_files(struct pci_dev *pdev)
 { return; }
 static inline void pci_remove_firmware_label_files(struct pci_dev *pdev)
diff --git a/drivers/pci/pcie/aer/aer_inject.c b/drivers/pci/pcie/aer/aer_inject.c
index b3cf6223f63a..f62079ff06dd 100644
--- a/drivers/pci/pcie/aer/aer_inject.c
+++ b/drivers/pci/pcie/aer/aer_inject.c
@@ -27,6 +27,10 @@
 #include <linux/stddef.h>
 #include "aerdrv.h"
 
+/* Override the existing corrected and uncorrected error masks */
+static int aer_mask_override;
+module_param(aer_mask_override, bool, 0);
+
 struct aer_error_inj {
 	u8 bus;
 	u8 dev;
@@ -322,7 +326,7 @@ static int aer_inject(struct aer_error_inj *einj)
 	unsigned long flags;
 	unsigned int devfn = PCI_DEVFN(einj->dev, einj->fn);
 	int pos_cap_err, rp_pos_cap_err;
-	u32 sever, cor_mask, uncor_mask;
+	u32 sever, cor_mask, uncor_mask, cor_mask_orig, uncor_mask_orig;
 	int ret = 0;
 
 	dev = pci_get_domain_bus_and_slot((int)einj->domain, einj->bus, devfn);
@@ -361,6 +365,18 @@ static int aer_inject(struct aer_error_inj *einj)
 		goto out_put;
 	}
 
+	if (aer_mask_override) {
+		cor_mask_orig = cor_mask;
+		cor_mask &= !(einj->cor_status);
+		pci_write_config_dword(dev, pos_cap_err + PCI_ERR_COR_MASK,
+				       cor_mask);
+
+		uncor_mask_orig = uncor_mask;
+		uncor_mask &= !(einj->uncor_status);
+		pci_write_config_dword(dev, pos_cap_err + PCI_ERR_UNCOR_MASK,
+				       uncor_mask);
+	}
+
 	spin_lock_irqsave(&inject_lock, flags);
 
 	err = __find_aer_error_by_dev(dev);
@@ -378,14 +394,16 @@ static int aer_inject(struct aer_error_inj *einj)
 	err->header_log2 = einj->header_log2;
 	err->header_log3 = einj->header_log3;
 
-	if (einj->cor_status && !(einj->cor_status & ~cor_mask)) {
+	if (!aer_mask_override && einj->cor_status &&
+	    !(einj->cor_status & ~cor_mask)) {
 		ret = -EINVAL;
 		printk(KERN_WARNING "The correctable error(s) is masked "
 				"by device\n");
 		spin_unlock_irqrestore(&inject_lock, flags);
 		goto out_put;
 	}
-	if (einj->uncor_status && !(einj->uncor_status & ~uncor_mask)) {
+	if (!aer_mask_override && einj->uncor_status &&
+	    !(einj->uncor_status & ~uncor_mask)) {
 		ret = -EINVAL;
 		printk(KERN_WARNING "The uncorrectable error(s) is masked "
 				"by device\n");
@@ -425,6 +443,13 @@ static int aer_inject(struct aer_error_inj *einj)
 	}
 	spin_unlock_irqrestore(&inject_lock, flags);
 
+	if (aer_mask_override) {
+		pci_write_config_dword(dev, pos_cap_err + PCI_ERR_COR_MASK,
+				       cor_mask_orig);
+		pci_write_config_dword(dev, pos_cap_err + PCI_ERR_UNCOR_MASK,
+				       uncor_mask_orig);
+	}
+
 	ret = pci_bus_set_aer_ops(dev->bus);
 	if (ret)
 		goto out_put;
diff --git a/drivers/pci/pcie/aer/aerdrv.h b/drivers/pci/pcie/aer/aerdrv.h
index 80c11d131499..3eb77080366a 100644
--- a/drivers/pci/pcie/aer/aerdrv.h
+++ b/drivers/pci/pcie/aer/aerdrv.h
@@ -35,13 +35,6 @@
 					PCI_ERR_UNC_UNX_COMP|		\
 					PCI_ERR_UNC_MALF_TLP)
 
-struct header_log_regs {
-	unsigned int dw0;
-	unsigned int dw1;
-	unsigned int dw2;
-	unsigned int dw3;
-};
-
 #define AER_MAX_MULTI_ERR_DEVICES	5	/* Not likely to have more */
 struct aer_err_info {
 	struct pci_dev *dev[AER_MAX_MULTI_ERR_DEVICES];
@@ -59,7 +52,7 @@ struct aer_err_info {
 
 	unsigned int status;		/* COR/UNCOR Error Status */
 	unsigned int mask;		/* COR/UNCOR Error Mask */
-	struct header_log_regs tlp;	/* TLP Header */
+	struct aer_header_log_regs tlp;	/* TLP Header */
 };
 
 struct aer_err_source {
diff --git a/drivers/pci/pcie/aer/aerdrv_errprint.c b/drivers/pci/pcie/aer/aerdrv_errprint.c
index 9d3e4c8d0184..b07a42e0b350 100644
--- a/drivers/pci/pcie/aer/aerdrv_errprint.c
+++ b/drivers/pci/pcie/aer/aerdrv_errprint.c
@@ -19,6 +19,7 @@
 #include <linux/errno.h>
 #include <linux/pm.h>
 #include <linux/suspend.h>
+#include <linux/cper.h>
 
 #include "aerdrv.h"
 
@@ -57,86 +58,44 @@
 	(e & AER_DATA_LINK_LAYER_ERROR_MASK(t)) ? AER_DATA_LINK_LAYER_ERROR : \
 	AER_TRANSACTION_LAYER_ERROR)
 
-#define AER_PR(info, pdev, fmt, args...)				\
-	printk("%s%s %s: " fmt, (info->severity == AER_CORRECTABLE) ?	\
-		KERN_WARNING : KERN_ERR, dev_driver_string(&pdev->dev),	\
-		dev_name(&pdev->dev), ## args)
-
 /*
  * AER error strings
  */
-static char *aer_error_severity_string[] = {
+static const char *aer_error_severity_string[] = {
 	"Uncorrected (Non-Fatal)",
 	"Uncorrected (Fatal)",
 	"Corrected"
 };
 
-static char *aer_error_layer[] = {
+static const char *aer_error_layer[] = {
 	"Physical Layer",
 	"Data Link Layer",
 	"Transaction Layer"
 };
-static char *aer_correctable_error_string[] = {
-	"Receiver Error        ",	/* Bit Position 0	*/
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	"Bad TLP               ",	/* Bit Position 6	*/
-	"Bad DLLP              ",	/* Bit Position 7	*/
-	"RELAY_NUM Rollover    ",	/* Bit Position 8	*/
-	NULL,
-	NULL,
-	NULL,
-	"Replay Timer Timeout  ",	/* Bit Position 12	*/
-	"Advisory Non-Fatal    ",	/* Bit Position 13	*/
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
+
+static const char *aer_correctable_error_string[] = {
+	"Receiver Error",		/* Bit Position 0	*/
 	NULL,
 	NULL,
 	NULL,
 	NULL,
 	NULL,
+	"Bad TLP",			/* Bit Position 6	*/
+	"Bad DLLP",			/* Bit Position 7	*/
+	"RELAY_NUM Rollover",		/* Bit Position 8	*/
 	NULL,
 	NULL,
 	NULL,
+	"Replay Timer Timeout",		/* Bit Position 12	*/
+	"Advisory Non-Fatal",		/* Bit Position 13	*/
 };
 
-static char *aer_uncorrectable_error_string[] = {
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	"Data Link Protocol    ",	/* Bit Position 4	*/
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-	"Poisoned TLP          ",	/* Bit Position 12	*/
-	"Flow Control Protocol ",	/* Bit Position 13	*/
-	"Completion Timeout    ",	/* Bit Position 14	*/
-	"Completer Abort       ",	/* Bit Position 15	*/
-	"Unexpected Completion ",	/* Bit Position 16	*/
-	"Receiver Overflow     ",	/* Bit Position 17	*/
-	"Malformed TLP         ",	/* Bit Position 18	*/
-	"ECRC                  ",	/* Bit Position 19	*/
-	"Unsupported Request   ",	/* Bit Position 20	*/
+static const char *aer_uncorrectable_error_string[] = {
 	NULL,
 	NULL,
 	NULL,
 	NULL,
+	"Data Link Protocol",		/* Bit Position 4	*/
 	NULL,
 	NULL,
 	NULL,
@@ -144,19 +103,29 @@ static char *aer_uncorrectable_error_string[] = {
 	NULL,
 	NULL,
 	NULL,
+	"Poisoned TLP",			/* Bit Position 12	*/
+	"Flow Control Protocol",	/* Bit Position 13	*/
+	"Completion Timeout",		/* Bit Position 14	*/
+	"Completer Abort",		/* Bit Position 15	*/
+	"Unexpected Completion",	/* Bit Position 16	*/
+	"Receiver Overflow",		/* Bit Position 17	*/
+	"Malformed TLP",		/* Bit Position 18	*/
+	"ECRC",				/* Bit Position 19	*/
+	"Unsupported Request",		/* Bit Position 20	*/
 };
 
-static char *aer_agent_string[] = {
+static const char *aer_agent_string[] = {
 	"Receiver ID",
 	"Requester ID",
 	"Completer ID",
 	"Transmitter ID"
 };
 
-static void __aer_print_error(struct aer_err_info *info, struct pci_dev *dev)
+static void __aer_print_error(const char *prefix,
+			      struct aer_err_info *info)
 {
 	int i, status;
-	char *errmsg = NULL;
+	const char *errmsg = NULL;
 
 	status = (info->status & ~info->mask);
 
@@ -165,15 +134,17 @@ static void __aer_print_error(struct aer_err_info *info, struct pci_dev *dev)
 			continue;
 
 		if (info->severity == AER_CORRECTABLE)
-			errmsg = aer_correctable_error_string[i];
+			errmsg = i < ARRAY_SIZE(aer_correctable_error_string) ?
+				aer_correctable_error_string[i] : NULL;
 		else
-			errmsg = aer_uncorrectable_error_string[i];
+			errmsg = i < ARRAY_SIZE(aer_uncorrectable_error_string) ?
+				aer_uncorrectable_error_string[i] : NULL;
 
 		if (errmsg)
-			AER_PR(info, dev, "   [%2d] %s%s\n", i, errmsg,
+			printk("%s""   [%2d] %-22s%s\n", prefix, i, errmsg,
 				info->first_error == i ? " (First)" : "");
 		else
-			AER_PR(info, dev, "   [%2d] Unknown Error Bit%s\n", i,
+			printk("%s""   [%2d] Unknown Error Bit%s\n", prefix, i,
 				info->first_error == i ? " (First)" : "");
 	}
 }
@@ -181,11 +152,15 @@ static void __aer_print_error(struct aer_err_info *info, struct pci_dev *dev)
 void aer_print_error(struct pci_dev *dev, struct aer_err_info *info)
 {
 	int id = ((dev->bus->number << 8) | dev->devfn);
+	char prefix[44];
+
+	snprintf(prefix, sizeof(prefix), "%s%s %s: ",
+		 (info->severity == AER_CORRECTABLE) ? KERN_WARNING : KERN_ERR,
+		 dev_driver_string(&dev->dev), dev_name(&dev->dev));
 
 	if (info->status == 0) {
-		AER_PR(info, dev,
-			"PCIe Bus Error: severity=%s, type=Unaccessible, "
-			"id=%04x(Unregistered Agent ID)\n",
+		printk("%s""PCIe Bus Error: severity=%s, type=Unaccessible, "
+			"id=%04x(Unregistered Agent ID)\n", prefix,
 			aer_error_severity_string[info->severity], id);
 	} else {
 		int layer, agent;
@@ -193,23 +168,22 @@ void aer_print_error(struct pci_dev *dev, struct aer_err_info *info)
 		layer = AER_GET_LAYER_ERROR(info->severity, info->status);
 		agent = AER_GET_AGENT(info->severity, info->status);
 
-		AER_PR(info, dev,
-			"PCIe Bus Error: severity=%s, type=%s, id=%04x(%s)\n",
-			aer_error_severity_string[info->severity],
+		printk("%s""PCIe Bus Error: severity=%s, type=%s, id=%04x(%s)\n",
+			prefix, aer_error_severity_string[info->severity],
 			aer_error_layer[layer], id, aer_agent_string[agent]);
 
-		AER_PR(info, dev,
-			"  device [%04x:%04x] error status/mask=%08x/%08x\n",
-			dev->vendor, dev->device, info->status, info->mask);
+		printk("%s""  device [%04x:%04x] error status/mask=%08x/%08x\n",
+			prefix, dev->vendor, dev->device,
+			info->status, info->mask);
 
-		__aer_print_error(info, dev);
+		__aer_print_error(prefix, info);
 
 		if (info->tlp_header_valid) {
 			unsigned char *tlp = (unsigned char *) &info->tlp;
-			AER_PR(info, dev, "  TLP Header:"
+			printk("%s""  TLP Header:"
 				" %02x%02x%02x%02x %02x%02x%02x%02x"
 				" %02x%02x%02x%02x %02x%02x%02x%02x\n",
-				*(tlp + 3), *(tlp + 2), *(tlp + 1), *tlp,
+				prefix, *(tlp + 3), *(tlp + 2), *(tlp + 1), *tlp,
 				*(tlp + 7), *(tlp + 6), *(tlp + 5), *(tlp + 4),
 				*(tlp + 11), *(tlp + 10), *(tlp + 9),
 				*(tlp + 8), *(tlp + 15), *(tlp + 14),
@@ -218,8 +192,8 @@ void aer_print_error(struct pci_dev *dev, struct aer_err_info *info)
 	}
 
 	if (info->id && info->error_dev_num > 1 && info->id == id)
-		AER_PR(info, dev,
-			"  Error of this Agent(%04x) is reported first\n", id);
+		printk("%s""  Error of this Agent(%04x) is reported first\n",
+			prefix, id);
 }
 
 void aer_print_port_info(struct pci_dev *dev, struct aer_err_info *info)
@@ -228,3 +202,61 @@ void aer_print_port_info(struct pci_dev *dev, struct aer_err_info *info)
 		info->multi_error_valid ? "Multiple " : "",
 		aer_error_severity_string[info->severity], info->id);
 }
+
+#ifdef CONFIG_ACPI_APEI_PCIEAER
+static int cper_severity_to_aer(int cper_severity)
+{
+	switch (cper_severity) {
+	case CPER_SEV_RECOVERABLE:
+		return AER_NONFATAL;
+	case CPER_SEV_FATAL:
+		return AER_FATAL;
+	default:
+		return AER_CORRECTABLE;
+	}
+}
+
+void cper_print_aer(const char *prefix, int cper_severity,
+		    struct aer_capability_regs *aer)
+{
+	int aer_severity, layer, agent, status_strs_size, tlp_header_valid = 0;
+	u32 status, mask;
+	const char **status_strs;
+
+	aer_severity = cper_severity_to_aer(cper_severity);
+	if (aer_severity == AER_CORRECTABLE) {
+		status = aer->cor_status;
+		mask = aer->cor_mask;
+		status_strs = aer_correctable_error_string;
+		status_strs_size = ARRAY_SIZE(aer_correctable_error_string);
+	} else {
+		status = aer->uncor_status;
+		mask = aer->uncor_mask;
+		status_strs = aer_uncorrectable_error_string;
+		status_strs_size = ARRAY_SIZE(aer_uncorrectable_error_string);
+		tlp_header_valid = status & AER_LOG_TLP_MASKS;
+	}
+	layer = AER_GET_LAYER_ERROR(aer_severity, status);
+	agent = AER_GET_AGENT(aer_severity, status);
+	printk("%s""aer_status: 0x%08x, aer_mask: 0x%08x\n",
+	       prefix, status, mask);
+	cper_print_bits(prefix, status, status_strs, status_strs_size);
+	printk("%s""aer_layer=%s, aer_agent=%s\n", prefix,
+	       aer_error_layer[layer], aer_agent_string[agent]);
+	if (aer_severity != AER_CORRECTABLE)
+		printk("%s""aer_uncor_severity: 0x%08x\n",
+		       prefix, aer->uncor_severity);
+	if (tlp_header_valid) {
+		const unsigned char *tlp;
+		tlp = (const unsigned char *)&aer->header_log;
+		printk("%s""aer_tlp_header:"
+			" %02x%02x%02x%02x %02x%02x%02x%02x"
+			" %02x%02x%02x%02x %02x%02x%02x%02x\n",
+			prefix, *(tlp + 3), *(tlp + 2), *(tlp + 1), *tlp,
+			*(tlp + 7), *(tlp + 6), *(tlp + 5), *(tlp + 4),
+			*(tlp + 11), *(tlp + 10), *(tlp + 9),
+			*(tlp + 8), *(tlp + 15), *(tlp + 14),
+			*(tlp + 13), *(tlp + 12));
+	}
+}
+#endif
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index c84900da3c59..44cbbbaa499d 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -764,6 +764,8 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 		if (pci_find_bus(pci_domain_nr(bus), max+1))
 			goto out;
 		child = pci_add_new_bus(bus, dev, ++max);
+		if (!child)
+			goto out;
 		buses = (buses & 0xff000000)
 		      | ((unsigned int)(child->primary)     <<  0)
 		      | ((unsigned int)(child->secondary)   <<  8)
@@ -777,7 +779,7 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 			buses &= ~0xff000000;
 			buses |= CARDBUS_LATENCY_TIMER << 24;
 		}
-			
+
 		/*
 		 * We need to blast all three values with a single write.
 		 */
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 53a786fd0d40..bd80f6378463 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -533,6 +533,17 @@ static void __devinit quirk_piix4_acpi(struct pci_dev *dev)
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82371AB_3,	quirk_piix4_acpi);
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82443MX_3,	quirk_piix4_acpi);
 
+#define ICH_PMBASE	0x40
+#define ICH_ACPI_CNTL	0x44
+#define  ICH4_ACPI_EN	0x10
+#define  ICH6_ACPI_EN	0x80
+#define ICH4_GPIOBASE	0x58
+#define ICH4_GPIO_CNTL	0x5c
+#define  ICH4_GPIO_EN	0x10
+#define ICH6_GPIOBASE	0x48
+#define ICH6_GPIO_CNTL	0x4c
+#define  ICH6_GPIO_EN	0x10
+
 /*
  * ICH4, ICH4-M, ICH5, ICH5-M ACPI: Three IO regions pointed to by longwords at
  *	0x40 (128 bytes of ACPI, GPIO & TCO registers)
@@ -541,12 +552,33 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82443MX_3,	qui
 static void __devinit quirk_ich4_lpc_acpi(struct pci_dev *dev)
 {
 	u32 region;
+	u8 enable;
 
-	pci_read_config_dword(dev, 0x40, &region);
-	quirk_io_region(dev, region, 128, PCI_BRIDGE_RESOURCES, "ICH4 ACPI/GPIO/TCO");
+	/*
+	 * The check for PCIBIOS_MIN_IO is to ensure we won't create a conflict
+	 * with low legacy (and fixed) ports. We don't know the decoding
+	 * priority and can't tell whether the legacy device or the one created
+	 * here is really at that address.  This happens on boards with broken
+	 * BIOSes.
+	*/
+
+	pci_read_config_byte(dev, ICH_ACPI_CNTL, &enable);
+	if (enable & ICH4_ACPI_EN) {
+		pci_read_config_dword(dev, ICH_PMBASE, &region);
+		region &= PCI_BASE_ADDRESS_IO_MASK;
+		if (region >= PCIBIOS_MIN_IO)
+			quirk_io_region(dev, region, 128, PCI_BRIDGE_RESOURCES,
+					"ICH4 ACPI/GPIO/TCO");
+	}
 
-	pci_read_config_dword(dev, 0x58, &region);
-	quirk_io_region(dev, region, 64, PCI_BRIDGE_RESOURCES+1, "ICH4 GPIO");
+	pci_read_config_byte(dev, ICH4_GPIO_CNTL, &enable);
+	if (enable & ICH4_GPIO_EN) {
+		pci_read_config_dword(dev, ICH4_GPIOBASE, &region);
+		region &= PCI_BASE_ADDRESS_IO_MASK;
+		if (region >= PCIBIOS_MIN_IO)
+			quirk_io_region(dev, region, 64,
+					PCI_BRIDGE_RESOURCES + 1, "ICH4 GPIO");
+	}
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_82801AA_0,		quirk_ich4_lpc_acpi);
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_82801AB_0,		quirk_ich4_lpc_acpi);
@@ -562,12 +594,25 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,    PCI_DEVICE_ID_INTEL_ESB_1,		qui
 static void __devinit ich6_lpc_acpi_gpio(struct pci_dev *dev)
 {
 	u32 region;
+	u8 enable;
 
-	pci_read_config_dword(dev, 0x40, &region);
-	quirk_io_region(dev, region, 128, PCI_BRIDGE_RESOURCES, "ICH6 ACPI/GPIO/TCO");
+	pci_read_config_byte(dev, ICH_ACPI_CNTL, &enable);
+	if (enable & ICH6_ACPI_EN) {
+		pci_read_config_dword(dev, ICH_PMBASE, &region);
+		region &= PCI_BASE_ADDRESS_IO_MASK;
+		if (region >= PCIBIOS_MIN_IO)
+			quirk_io_region(dev, region, 128, PCI_BRIDGE_RESOURCES,
+					"ICH6 ACPI/GPIO/TCO");
+	}
 
-	pci_read_config_dword(dev, 0x48, &region);
-	quirk_io_region(dev, region, 64, PCI_BRIDGE_RESOURCES+1, "ICH6 GPIO");
+	pci_read_config_byte(dev, ICH6_GPIO_CNTL, &enable);
+	if (enable & ICH4_GPIO_EN) {
+		pci_read_config_dword(dev, ICH6_GPIOBASE, &region);
+		region &= PCI_BASE_ADDRESS_IO_MASK;
+		if (region >= PCIBIOS_MIN_IO)
+			quirk_io_region(dev, region, 64,
+					PCI_BRIDGE_RESOURCES + 1, "ICH6 GPIO");
+	}
 }
 
 static void __devinit ich6_lpc_generic_decode(struct pci_dev *dev, unsigned reg, const char *name, int dynsize)
@@ -2618,58 +2663,6 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_ATI, 0x4375,
 
 #endif /* CONFIG_PCI_MSI */
 
-#ifdef CONFIG_PCI_IOV
-
-/*
- * For Intel 82576 SR-IOV NIC, if BIOS doesn't allocate resources for the
- * SR-IOV BARs, zero the Flash BAR and program the SR-IOV BARs to use the
- * old Flash Memory Space.
- */
-static void __devinit quirk_i82576_sriov(struct pci_dev *dev)
-{
-	int pos, flags;
-	u32 bar, start, size;
-
-	if (PAGE_SIZE > 0x10000)
-		return;
-
-	flags = pci_resource_flags(dev, 0);
-	if ((flags & PCI_BASE_ADDRESS_SPACE) !=
-			PCI_BASE_ADDRESS_SPACE_MEMORY ||
-	    (flags & PCI_BASE_ADDRESS_MEM_TYPE_MASK) !=
-			PCI_BASE_ADDRESS_MEM_TYPE_32)
-		return;
-
-	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_SRIOV);
-	if (!pos)
-		return;
-
-	pci_read_config_dword(dev, pos + PCI_SRIOV_BAR, &bar);
-	if (bar & PCI_BASE_ADDRESS_MEM_MASK)
-		return;
-
-	start = pci_resource_start(dev, 1);
-	size = pci_resource_len(dev, 1);
-	if (!start || size != 0x400000 || start & (size - 1))
-		return;
-
-	pci_resource_flags(dev, 1) = 0;
-	pci_write_config_dword(dev, PCI_BASE_ADDRESS_1, 0);
-	pci_write_config_dword(dev, pos + PCI_SRIOV_BAR, start);
-	pci_write_config_dword(dev, pos + PCI_SRIOV_BAR + 12, start + size / 2);
-
-	dev_info(&dev->dev, "use Flash Memory Space for SR-IOV BARs\n");
-}
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x10c9, quirk_i82576_sriov);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x10e6, quirk_i82576_sriov);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x10e7, quirk_i82576_sriov);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x10e8, quirk_i82576_sriov);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x150a, quirk_i82576_sriov);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x150d, quirk_i82576_sriov);
-DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x1518, quirk_i82576_sriov);
-
-#endif	/* CONFIG_PCI_IOV */
-
 /* Allow manual resource allocation for PCI hotplug bridges
  * via pci=hpmemsize=nnM and pci=hpiosize=nnM parameters. For
  * some PCI-PCI hotplug bridges, like PLX 6254 (former HINT HB6),
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 66cb8f4cc5f4..89d0a6a88df7 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -33,11 +33,32 @@ struct resource_list_x {
 	struct pci_dev *dev;
 	resource_size_t start;
 	resource_size_t end;
+	resource_size_t add_size;
 	unsigned long flags;
 };
 
-static void add_to_failed_list(struct resource_list_x *head,
-				 struct pci_dev *dev, struct resource *res)
+#define free_list(type, head) do {                      \
+	struct type *list, *tmp;			\
+	for (list = (head)->next; list;) {		\
+		tmp = list;				\
+		list = list->next;			\
+		kfree(tmp);				\
+	}						\
+	(head)->next = NULL;				\
+} while (0)
+
+/**
+ * add_to_list() - add a new resource tracker to the list
+ * @head:	Head of the list
+ * @dev:	device corresponding to which the resource
+ *		belongs
+ * @res:	The resource to be tracked
+ * @add_size:	additional size to be optionally added
+ *              to the resource
+ */
+static void add_to_list(struct resource_list_x *head,
+		 struct pci_dev *dev, struct resource *res,
+		 resource_size_t add_size)
 {
 	struct resource_list_x *list = head;
 	struct resource_list_x *ln = list->next;
@@ -45,7 +66,7 @@ static void add_to_failed_list(struct resource_list_x *head,
 
 	tmp = kmalloc(sizeof(*tmp), GFP_KERNEL);
 	if (!tmp) {
-		pr_warning("add_to_failed_list: kmalloc() failed!\n");
+		pr_warning("add_to_list: kmalloc() failed!\n");
 		return;
 	}
 
@@ -55,20 +76,14 @@ static void add_to_failed_list(struct resource_list_x *head,
 	tmp->start = res->start;
 	tmp->end = res->end;
 	tmp->flags = res->flags;
+	tmp->add_size = add_size;
 	list->next = tmp;
 }
 
-static void free_failed_list(struct resource_list_x *head)
+static void add_to_failed_list(struct resource_list_x *head,
+				struct pci_dev *dev, struct resource *res)
 {
-	struct resource_list_x *list, *tmp;
-
-	for (list = head->next; list;) {
-		tmp = list;
-		list = list->next;
-		kfree(tmp);
-	}
-
-	head->next = NULL;
+	add_to_list(head, dev, res, 0);
 }
 
 static void __dev_sort_resources(struct pci_dev *dev,
@@ -91,18 +106,88 @@ static void __dev_sort_resources(struct pci_dev *dev,
 	pdev_sort_resources(dev, head);
 }
 
-static void __assign_resources_sorted(struct resource_list *head,
-				 struct resource_list_x *fail_head)
+static inline void reset_resource(struct resource *res)
+{
+	res->start = 0;
+	res->end = 0;
+	res->flags = 0;
+}
+
+/**
+ * adjust_resources_sorted() - satisfy any additional resource requests
+ *
+ * @add_head : head of the list tracking requests requiring additional
+ *             resources
+ * @head     : head of the list tracking requests with allocated
+ *             resources
+ *
+ * Walk through each element of the add_head and try to procure
+ * additional resources for the element, provided the element
+ * is in the head list.
+ */
+static void adjust_resources_sorted(struct resource_list_x *add_head,
+		struct resource_list *head)
 {
 	struct resource *res;
-	struct resource_list *list, *tmp;
+	struct resource_list_x *list, *tmp, *prev;
+	struct resource_list *hlist;
+	resource_size_t add_size;
 	int idx;
 
-	for (list = head->next; list;) {
+	prev = add_head;
+	for (list = add_head->next; list;) {
 		res = list->res;
+		/* skip resource that has been reset */
+		if (!res->flags)
+			goto out;
+
+		/* skip this resource if not found in head list */
+		for (hlist = head->next; hlist && hlist->res != res;
+				hlist = hlist->next);
+		if (!hlist) { /* just skip */
+			prev = list;
+			list = list->next;
+			continue;
+		}
+
 		idx = res - &list->dev->resource[0];
+		add_size=list->add_size;
+		if (!resource_size(res) && add_size) {
+			 res->end = res->start + add_size - 1;
+			 if(pci_assign_resource(list->dev, idx))
+				reset_resource(res);
+		} else if (add_size) {
+			adjust_resource(res, res->start,
+				resource_size(res) + add_size);
+		}
+out:
+		tmp = list;
+		prev->next = list = list->next;
+		kfree(tmp);
+	}
+}
+
+/**
+ * assign_requested_resources_sorted() - satisfy resource requests
+ *
+ * @head : head of the list tracking requests for resources
+ * @failed_list : head of the list tracking requests that could
+ *		not be allocated
+ *
+ * Satisfy resource requests of each element in the list. Add
+ * requests that could not satisfied to the failed_list.
+ */
+static void assign_requested_resources_sorted(struct resource_list *head,
+				 struct resource_list_x *fail_head)
+{
+	struct resource *res;
+	struct resource_list *list;
+	int idx;
 
-		if (pci_assign_resource(list->dev, idx)) {
+	for (list = head->next; list; list = list->next) {
+		res = list->res;
+		idx = res - &list->dev->resource[0];
+		if (resource_size(res) && pci_assign_resource(list->dev, idx)) {
 			if (fail_head && !pci_is_root_bus(list->dev->bus)) {
 				/*
 				 * if the failed res is for ROM BAR, and it will
@@ -112,16 +197,25 @@ static void __assign_resources_sorted(struct resource_list *head,
 				      (!(res->flags & IORESOURCE_ROM_ENABLE))))
 					add_to_failed_list(fail_head, list->dev, res);
 			}
-			res->start = 0;
-			res->end = 0;
-			res->flags = 0;
+			reset_resource(res);
 		}
-		tmp = list;
-		list = list->next;
-		kfree(tmp);
 	}
 }
 
+static void __assign_resources_sorted(struct resource_list *head,
+				 struct resource_list_x *add_head,
+				 struct resource_list_x *fail_head)
+{
+	/* Satisfy the must-have resource requests */
+	assign_requested_resources_sorted(head, fail_head);
+
+	/* Try to satisfy any additional nice-to-have resource
+		requests */
+	if (add_head)
+		adjust_resources_sorted(add_head, head);
+	free_list(resource_list, head);
+}
+
 static void pdev_assign_resources_sorted(struct pci_dev *dev,
 				 struct resource_list_x *fail_head)
 {
@@ -129,11 +223,12 @@ static void pdev_assign_resources_sorted(struct pci_dev *dev,
 
 	head.next = NULL;
 	__dev_sort_resources(dev, &head);
-	__assign_resources_sorted(&head, fail_head);
+	__assign_resources_sorted(&head, NULL, fail_head);
 
 }
 
 static void pbus_assign_resources_sorted(const struct pci_bus *bus,
+					 struct resource_list_x *add_head,
 					 struct resource_list_x *fail_head)
 {
 	struct pci_dev *dev;
@@ -143,7 +238,7 @@ static void pbus_assign_resources_sorted(const struct pci_bus *bus,
 	list_for_each_entry(dev, &bus->devices, bus_list)
 		__dev_sort_resources(dev, &head);
 
-	__assign_resources_sorted(&head, fail_head);
+	__assign_resources_sorted(&head, add_head, fail_head);
 }
 
 void pci_setup_cardbus(struct pci_bus *bus)
@@ -404,15 +499,62 @@ static struct resource *find_free_bus_resource(struct pci_bus *bus, unsigned lon
 	return NULL;
 }
 
-/* Sizing the IO windows of the PCI-PCI bridge is trivial,
-   since these windows have 4K granularity and the IO ranges
-   of non-bridge PCI devices are limited to 256 bytes.
-   We must be careful with the ISA aliasing though. */
-static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size)
+static resource_size_t calculate_iosize(resource_size_t size,
+		resource_size_t min_size,
+		resource_size_t size1,
+		resource_size_t old_size,
+		resource_size_t align)
+{
+	if (size < min_size)
+		size = min_size;
+	if (old_size == 1 )
+		old_size = 0;
+	/* To be fixed in 2.5: we should have sort of HAVE_ISA
+	   flag in the struct pci_bus. */
+#if defined(CONFIG_ISA) || defined(CONFIG_EISA)
+	size = (size & 0xff) + ((size & ~0xffUL) << 2);
+#endif
+	size = ALIGN(size + size1, align);
+	if (size < old_size)
+		size = old_size;
+	return size;
+}
+
+static resource_size_t calculate_memsize(resource_size_t size,
+		resource_size_t min_size,
+		resource_size_t size1,
+		resource_size_t old_size,
+		resource_size_t align)
+{
+	if (size < min_size)
+		size = min_size;
+	if (old_size == 1 )
+		old_size = 0;
+	if (size < old_size)
+		size = old_size;
+	size = ALIGN(size + size1, align);
+	return size;
+}
+
+/**
+ * pbus_size_io() - size the io window of a given bus
+ *
+ * @bus : the bus
+ * @min_size : the minimum io window that must to be allocated
+ * @add_size : additional optional io window
+ * @add_head : track the additional io window on this list
+ *
+ * Sizing the IO windows of the PCI-PCI bridge is trivial,
+ * since these windows have 4K granularity and the IO ranges
+ * of non-bridge PCI devices are limited to 256 bytes.
+ * We must be careful with the ISA aliasing though.
+ */
+static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
+		resource_size_t add_size, struct resource_list_x *add_head)
 {
 	struct pci_dev *dev;
 	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO);
-	unsigned long size = 0, size1 = 0, old_size;
+	unsigned long size = 0, size0 = 0, size1 = 0;
 
 	if (!b_res)
  		return;
@@ -435,20 +577,12 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size)
 				size1 += r_size;
 		}
 	}
-	if (size < min_size)
-		size = min_size;
-	old_size = resource_size(b_res);
-	if (old_size == 1)
-		old_size = 0;
-/* To be fixed in 2.5: we should have sort of HAVE_ISA
-   flag in the struct pci_bus. */
-#if defined(CONFIG_ISA) || defined(CONFIG_EISA)
-	size = (size & 0xff) + ((size & ~0xffUL) << 2);
-#endif
-	size = ALIGN(size + size1, 4096);
-	if (size < old_size)
-		size = old_size;
-	if (!size) {
+	size0 = calculate_iosize(size, min_size, size1,
+			resource_size(b_res), 4096);
+	size1 = !add_size? size0:
+		calculate_iosize(size, min_size+add_size, size1,
+			resource_size(b_res), 4096);
+	if (!size0 && !size1) {
 		if (b_res->start || b_res->end)
 			dev_info(&bus->self->dev, "disabling bridge window "
 				 "%pR to [bus %02x-%02x] (unused)\n", b_res,
@@ -458,17 +592,30 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size)
 	}
 	/* Alignment of the IO window is always 4K */
 	b_res->start = 4096;
-	b_res->end = b_res->start + size - 1;
+	b_res->end = b_res->start + size0 - 1;
 	b_res->flags |= IORESOURCE_STARTALIGN;
+	if (size1 > size0 && add_head)
+		add_to_list(add_head, bus->self, b_res, size1-size0);
 }
 
-/* Calculate the size of the bus and minimal alignment which
-   guarantees that all child resources fit in this size. */
+/**
+ * pbus_size_mem() - size the memory window of a given bus
+ *
+ * @bus : the bus
+ * @min_size : the minimum memory window that must to be allocated
+ * @add_size : additional optional memory window
+ * @add_head : track the additional memory window on this list
+ *
+ * Calculate the size of the bus and minimal alignment which
+ * guarantees that all child resources fit in this size.
+ */
 static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
-			 unsigned long type, resource_size_t min_size)
+			 unsigned long type, resource_size_t min_size,
+			resource_size_t add_size,
+			struct resource_list_x *add_head)
 {
 	struct pci_dev *dev;
-	resource_size_t min_align, align, size, old_size;
+	resource_size_t min_align, align, size, size0, size1;
 	resource_size_t aligns[12];	/* Alignments from 1Mb to 2Gb */
 	int order, max_order;
 	struct resource *b_res = find_free_bus_resource(bus, type);
@@ -516,14 +663,6 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 			mem64_mask &= r->flags & IORESOURCE_MEM_64;
 		}
 	}
-	if (size < min_size)
-		size = min_size;
-	old_size = resource_size(b_res);
-	if (old_size == 1)
-		old_size = 0;
-	if (size < old_size)
-		size = old_size;
-
 	align = 0;
 	min_align = 0;
 	for (order = 0; order <= max_order; order++) {
@@ -537,8 +676,11 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 			min_align = align1 >> 1;
 		align += aligns[order];
 	}
-	size = ALIGN(size, min_align);
-	if (!size) {
+	size0 = calculate_memsize(size, min_size, 0, resource_size(b_res), align);
+	size1 = !add_size ? size :
+		calculate_memsize(size, min_size+add_size, 0,
+				resource_size(b_res), align);
+	if (!size0 && !size1) {
 		if (b_res->start || b_res->end)
 			dev_info(&bus->self->dev, "disabling bridge window "
 				 "%pR to [bus %02x-%02x] (unused)\n", b_res,
@@ -547,9 +689,10 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 		return 1;
 	}
 	b_res->start = min_align;
-	b_res->end = size + min_align - 1;
-	b_res->flags |= IORESOURCE_STARTALIGN;
-	b_res->flags |= mem64_mask;
+	b_res->end = size0 + min_align - 1;
+	b_res->flags |= IORESOURCE_STARTALIGN | mem64_mask;
+	if (size1 > size0 && add_head)
+		add_to_list(add_head, bus->self, b_res, size1-size0);
 	return 1;
 }
 
@@ -602,11 +745,12 @@ static void pci_bus_size_cardbus(struct pci_bus *bus)
 	}
 }
 
-void __ref pci_bus_size_bridges(struct pci_bus *bus)
+void __ref __pci_bus_size_bridges(struct pci_bus *bus,
+			struct resource_list_x *add_head)
 {
 	struct pci_dev *dev;
 	unsigned long mask, prefmask;
-	resource_size_t min_mem_size = 0, min_io_size = 0;
+	resource_size_t additional_mem_size = 0, additional_io_size = 0;
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		struct pci_bus *b = dev->subordinate;
@@ -620,7 +764,7 @@ void __ref pci_bus_size_bridges(struct pci_bus *bus)
 
 		case PCI_CLASS_BRIDGE_PCI:
 		default:
-			pci_bus_size_bridges(b);
+			__pci_bus_size_bridges(b, add_head);
 			break;
 		}
 	}
@@ -637,11 +781,14 @@ void __ref pci_bus_size_bridges(struct pci_bus *bus)
 	case PCI_CLASS_BRIDGE_PCI:
 		pci_bridge_check_ranges(bus);
 		if (bus->self->is_hotplug_bridge) {
-			min_io_size  = pci_hotplug_io_size;
-			min_mem_size = pci_hotplug_mem_size;
+			additional_io_size  = pci_hotplug_io_size;
+			additional_mem_size = pci_hotplug_mem_size;
 		}
+		/*
+		 * Follow thru
+		 */
 	default:
-		pbus_size_io(bus, min_io_size);
+		pbus_size_io(bus, 0, additional_io_size, add_head);
 		/* If the bridge supports prefetchable range, size it
 		   separately. If it doesn't, or its prefetchable window
 		   has already been allocated by arch code, try
@@ -649,30 +796,36 @@ void __ref pci_bus_size_bridges(struct pci_bus *bus)
 		   resources. */
 		mask = IORESOURCE_MEM;
 		prefmask = IORESOURCE_MEM | IORESOURCE_PREFETCH;
-		if (pbus_size_mem(bus, prefmask, prefmask, min_mem_size))
+		if (pbus_size_mem(bus, prefmask, prefmask, 0, additional_mem_size, add_head))
 			mask = prefmask; /* Success, size non-prefetch only. */
 		else
-			min_mem_size += min_mem_size;
-		pbus_size_mem(bus, mask, IORESOURCE_MEM, min_mem_size);
+			additional_mem_size += additional_mem_size;
+		pbus_size_mem(bus, mask, IORESOURCE_MEM, 0, additional_mem_size, add_head);
 		break;
 	}
 }
+
+void __ref pci_bus_size_bridges(struct pci_bus *bus)
+{
+	__pci_bus_size_bridges(bus, NULL);
+}
 EXPORT_SYMBOL(pci_bus_size_bridges);
 
 static void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
+					 struct resource_list_x *add_head,
 					 struct resource_list_x *fail_head)
 {
 	struct pci_bus *b;
 	struct pci_dev *dev;
 
-	pbus_assign_resources_sorted(bus, fail_head);
+	pbus_assign_resources_sorted(bus, add_head, fail_head);
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		b = dev->subordinate;
 		if (!b)
 			continue;
 
-		__pci_bus_assign_resources(b, fail_head);
+		__pci_bus_assign_resources(b, add_head, fail_head);
 
 		switch (dev->class >> 8) {
 		case PCI_CLASS_BRIDGE_PCI:
@@ -694,7 +847,7 @@ static void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
 
 void __ref pci_bus_assign_resources(const struct pci_bus *bus)
 {
-	__pci_bus_assign_resources(bus, NULL);
+	__pci_bus_assign_resources(bus, NULL, NULL);
 }
 EXPORT_SYMBOL(pci_bus_assign_resources);
 
@@ -709,7 +862,7 @@ static void __ref __pci_bridge_assign_resources(const struct pci_dev *bridge,
 	if (!b)
 		return;
 
-	__pci_bus_assign_resources(b, fail_head);
+	__pci_bus_assign_resources(b, NULL, fail_head);
 
 	switch (bridge->class >> 8) {
 	case PCI_CLASS_BRIDGE_PCI:
@@ -842,17 +995,21 @@ void __init
 pci_assign_unassigned_resources(void)
 {
 	struct pci_bus *bus;
-
+	struct resource_list_x add_list; /* list of resources that
+					want additional resources */
+	add_list.next = NULL;
 	/* Depth first, calculate sizes and alignments of all
 	   subordinate buses. */
 	list_for_each_entry(bus, &pci_root_buses, node) {
-		pci_bus_size_bridges(bus);
+		__pci_bus_size_bridges(bus, &add_list);
 	}
+
 	/* Depth last, allocate resources and update the hardware. */
 	list_for_each_entry(bus, &pci_root_buses, node) {
-		pci_bus_assign_resources(bus);
+		__pci_bus_assign_resources(bus, &add_list, NULL);
 		pci_enable_bridges(bus);
 	}
+	BUG_ON(add_list.next);
 
 	/* dump the resource on buses */
 	list_for_each_entry(bus, &pci_root_buses, node) {
@@ -882,7 +1039,7 @@ again:
 
 	if (tried_times >= 2) {
 		/* still fail, don't need to try more */
-		free_failed_list(&head);
+		free_list(resource_list_x, &head);
 		goto enable_all;
 	}
 
@@ -913,7 +1070,7 @@ again:
 
 		list = list->next;
 	}
-	free_failed_list(&head);
+	free_list(resource_list_x, &head);
 
 	goto again;
 
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index 3a5a6fcc0ead..492b7d807fe8 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -243,7 +243,7 @@ struct pci_ops pcifront_bus_ops = {
 
 #ifdef CONFIG_PCI_MSI
 static int pci_frontend_enable_msix(struct pci_dev *dev,
-				    int **vector, int nvec)
+				    int vector[], int nvec)
 {
 	int err;
 	int i;
@@ -277,18 +277,24 @@ static int pci_frontend_enable_msix(struct pci_dev *dev,
 	if (likely(!err)) {
 		if (likely(!op.value)) {
 			/* we get the result */
-			for (i = 0; i < nvec; i++)
-				*(*vector+i) = op.msix_entries[i].vector;
-			return 0;
+			for (i = 0; i < nvec; i++) {
+				if (op.msix_entries[i].vector <= 0) {
+					dev_warn(&dev->dev, "MSI-X entry %d is invalid: %d!\n",
+						i, op.msix_entries[i].vector);
+					err = -EINVAL;
+					vector[i] = -1;
+					continue;
+				}
+				vector[i] = op.msix_entries[i].vector;
+			}
 		} else {
 			printk(KERN_DEBUG "enable msix get value %x\n",
 				op.value);
-			return op.value;
 		}
 	} else {
 		dev_err(&dev->dev, "enable msix get err %x\n", err);
-		return err;
 	}
+	return err;
 }
 
 static void pci_frontend_disable_msix(struct pci_dev *dev)
@@ -310,7 +316,7 @@ static void pci_frontend_disable_msix(struct pci_dev *dev)
 		dev_err(&dev->dev, "pci_disable_msix get err %x\n", err);
 }
 
-static int pci_frontend_enable_msi(struct pci_dev *dev, int **vector)
+static int pci_frontend_enable_msi(struct pci_dev *dev, int vector[])
 {
 	int err;
 	struct xen_pci_op op = {
@@ -324,7 +330,13 @@ static int pci_frontend_enable_msi(struct pci_dev *dev, int **vector)
 
 	err = do_pci_op(pdev, &op);
 	if (likely(!err)) {
-		*(*vector) = op.value;
+		vector[0] = op.value;
+		if (op.value <= 0) {
+			dev_warn(&dev->dev, "MSI entry is invalid: %d!\n",
+				op.value);
+			err = -EINVAL;
+			vector[0] = -1;
+		}
 	} else {
 		dev_err(&dev->dev, "pci frontend enable msi failed for dev "
 				    "%x:%x\n", op.bus, op.devfn);
@@ -733,8 +745,7 @@ static void free_pdev(struct pcifront_device *pdev)
 
 	pcifront_free_roots(pdev);
 
-	/*For PCIE_AER error handling job*/
-	flush_scheduled_work();
+	cancel_work_sync(&pdev->op_work);
 
 	if (pdev->irq >= 0)
 		unbind_from_irqhandler(pdev->irq, pdev);
diff --git a/drivers/pcmcia/electra_cf.c b/drivers/pcmcia/electra_cf.c
index 546d3024b6f0..6defd4a8168e 100644
--- a/drivers/pcmcia/electra_cf.c
+++ b/drivers/pcmcia/electra_cf.c
@@ -181,8 +181,7 @@ static struct pccard_operations electra_cf_ops = {
 	.set_mem_map		= electra_cf_set_mem_map,
 };
 
-static int __devinit electra_cf_probe(struct platform_device *ofdev,
-				      const struct of_device_id *match)
+static int __devinit electra_cf_probe(struct platform_device *ofdev)
 {
 	struct device *device = &ofdev->dev;
 	struct device_node *np = ofdev->dev.of_node;
@@ -356,7 +355,7 @@ static const struct of_device_id electra_cf_match[] = {
 };
 MODULE_DEVICE_TABLE(of, electra_cf_match);
 
-static struct of_platform_driver electra_cf_driver = {
+static struct platform_driver electra_cf_driver = {
 	.driver = {
 		.name = (char *)driver_name,
 		.owner = THIS_MODULE,
@@ -368,13 +367,13 @@ static struct of_platform_driver electra_cf_driver = {
 
 static int __init electra_cf_init(void)
 {
-	return of_register_platform_driver(&electra_cf_driver);
+	return platform_driver_register(&electra_cf_driver);
 }
 module_init(electra_cf_init);
 
 static void __exit electra_cf_exit(void)
 {
-	of_unregister_platform_driver(&electra_cf_driver);
+	platform_driver_unregister(&electra_cf_driver);
 }
 module_exit(electra_cf_exit);
 
diff --git a/drivers/pcmcia/m8xx_pcmcia.c b/drivers/pcmcia/m8xx_pcmcia.c
index 0db482771fb5..271a590a5f3c 100644
--- a/drivers/pcmcia/m8xx_pcmcia.c
+++ b/drivers/pcmcia/m8xx_pcmcia.c
@@ -1148,8 +1148,7 @@ static struct pccard_operations m8xx_services = {
 	.set_mem_map = m8xx_set_mem_map,
 };
 
-static int __init m8xx_probe(struct platform_device *ofdev,
-			     const struct of_device_id *match)
+static int __init m8xx_probe(struct platform_device *ofdev)
 {
 	struct pcmcia_win *w;
 	unsigned int i, m, hwirq;
@@ -1295,7 +1294,7 @@ static const struct of_device_id m8xx_pcmcia_match[] = {
 
 MODULE_DEVICE_TABLE(of, m8xx_pcmcia_match);
 
-static struct of_platform_driver m8xx_pcmcia_driver = {
+static struct platform_driver m8xx_pcmcia_driver = {
 	.driver = {
 		.name = driver_name,
 		.owner = THIS_MODULE,
@@ -1307,12 +1306,12 @@ static struct of_platform_driver m8xx_pcmcia_driver = {
 
 static int __init m8xx_init(void)
 {
-	return of_register_platform_driver(&m8xx_pcmcia_driver);
+	return platform_driver_register(&m8xx_pcmcia_driver);
 }
 
 static void __exit m8xx_exit(void)
 {
-	of_unregister_platform_driver(&m8xx_pcmcia_driver);
+	platform_driver_unregister(&m8xx_pcmcia_driver);
 }
 
 module_init(m8xx_init);
diff --git a/drivers/pcmcia/pcmcia_resource.c b/drivers/pcmcia/pcmcia_resource.c
index 0bdda5b3ed55..42fbf1a75576 100644
--- a/drivers/pcmcia/pcmcia_resource.c
+++ b/drivers/pcmcia/pcmcia_resource.c
@@ -518,6 +518,8 @@ int pcmcia_enable_device(struct pcmcia_device *p_dev)
 		flags |= CONF_ENABLE_IOCARD;
 	if (flags & CONF_ENABLE_IOCARD)
 		s->socket.flags |= SS_IOCARD;
+	if (flags & CONF_ENABLE_ZVCARD)
+		s->socket.flags |= SS_ZVCARD | SS_IOCARD;
 	if (flags & CONF_ENABLE_SPKR) {
 		s->socket.flags |= SS_SPKR_ENA;
 		status = CCSR_AUDIO_ENA;
diff --git a/drivers/pcmcia/pxa2xx_base.c b/drivers/pcmcia/pxa2xx_base.c
index 3755e7c8c715..2c540542b5af 100644
--- a/drivers/pcmcia/pxa2xx_base.c
+++ b/drivers/pcmcia/pxa2xx_base.c
@@ -215,7 +215,7 @@ pxa2xx_pcmcia_frequency_change(struct soc_pcmcia_socket *skt,
 }
 #endif
 
-static void pxa2xx_configure_sockets(struct device *dev)
+void pxa2xx_configure_sockets(struct device *dev)
 {
 	struct pcmcia_low_level *ops = dev->platform_data;
 	/*
diff --git a/drivers/pcmcia/pxa2xx_base.h b/drivers/pcmcia/pxa2xx_base.h
index bb62ea87b8f9..b609b45469ed 100644
--- a/drivers/pcmcia/pxa2xx_base.h
+++ b/drivers/pcmcia/pxa2xx_base.h
@@ -1,3 +1,4 @@
 int pxa2xx_drv_pcmcia_add_one(struct soc_pcmcia_socket *skt);
 void pxa2xx_drv_pcmcia_ops(struct pcmcia_low_level *ops);
+void pxa2xx_configure_sockets(struct device *dev);
 
diff --git a/drivers/pcmcia/pxa2xx_colibri.c b/drivers/pcmcia/pxa2xx_colibri.c
index c3f72192af66..a52039564e74 100644
--- a/drivers/pcmcia/pxa2xx_colibri.c
+++ b/drivers/pcmcia/pxa2xx_colibri.c
@@ -181,6 +181,9 @@ static int __init colibri_pcmcia_init(void)
 {
 	int ret;
 
+	if (!machine_is_colibri() && !machine_is_colibri320())
+		return -ENODEV;
+
 	colibri_pcmcia_device = platform_device_alloc("pxa2xx-pcmcia", -1);
 	if (!colibri_pcmcia_device)
 		return -ENOMEM;
diff --git a/drivers/pcmcia/pxa2xx_lubbock.c b/drivers/pcmcia/pxa2xx_lubbock.c
index b9f8c8fb42bd..25afe637c657 100644
--- a/drivers/pcmcia/pxa2xx_lubbock.c
+++ b/drivers/pcmcia/pxa2xx_lubbock.c
@@ -226,6 +226,7 @@ int pcmcia_lubbock_init(struct sa1111_dev *sadev)
 		lubbock_set_misc_wr((1 << 15) | (1 << 14), 0);
 
 		pxa2xx_drv_pcmcia_ops(&lubbock_pcmcia_ops);
+		pxa2xx_configure_sockets(&sadev->dev);
 		ret = sa1111_pcmcia_add(sadev, &lubbock_pcmcia_ops,
 				pxa2xx_drv_pcmcia_add_one);
 	}
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index a59af5b24f0a..b039d6a93f17 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -101,6 +101,19 @@ config DELL_WMI
 	  To compile this driver as a module, choose M here: the module will
 	  be called dell-wmi.
 
+config DELL_WMI_AIO
+	tristate "WMI Hotkeys for Dell All-In-One series"
+	depends on ACPI_WMI
+	depends on INPUT
+	select INPUT_SPARSEKMAP
+	---help---
+	  Say Y here if you want to support WMI-based hotkeys on Dell
+	  All-In-One machines.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called dell-wmi.
+
+
 config FUJITSU_LAPTOP
 	tristate "Fujitsu Laptop Extras"
 	depends on ACPI
@@ -138,6 +151,24 @@ config TC1100_WMI
 	  This is a driver for the WMI extensions (wireless and bluetooth power
 	  control) of the HP Compaq TC1100 tablet.
 
+config HP_ACCEL
+	tristate "HP laptop accelerometer"
+	depends on INPUT && ACPI
+	select SENSORS_LIS3LV02D
+	select NEW_LEDS
+	select LEDS_CLASS
+	help
+	  This driver provides support for the "Mobile Data Protection System 3D"
+	  or "3D DriveGuard" feature of HP laptops. On such systems the driver
+	  should load automatically (via ACPI alias).
+
+	  Support for a led indicating disk protection will be provided as
+	  hp::hddprotect. For more information on the feature, refer to
+	  Documentation/hwmon/lis3lv02d.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called hp_accel.
+
 config HP_WMI
 	tristate "HP WMI extras"
 	depends on ACPI_WMI
@@ -598,6 +629,21 @@ config GPIO_INTEL_PMIC
 	  Say Y here to support GPIO via the SCU IPC interface
 	  on Intel MID platforms.
 
+config INTEL_MID_POWER_BUTTON
+	tristate "power button driver for Intel MID platforms"
+	depends on INTEL_SCU_IPC && INPUT
+	help
+	  This driver handles the power button on the Intel MID platforms.
+
+	  If unsure, say N.
+
+config INTEL_MFLD_THERMAL
+       tristate "Thermal driver for Intel Medfield platform"
+       depends on INTEL_SCU_IPC && THERMAL
+       help
+         Say Y here to enable thermal driver support for the  Intel Medfield
+         platform.
+
 config RAR_REGISTER
 	bool "Restricted Access Region Register Driver"
 	depends on PCI && X86_MRST
@@ -654,4 +700,13 @@ config XO1_RFKILL
 	  Support for enabling/disabling the WLAN interface on the OLPC XO-1
 	  laptop.
 
+config XO15_EBOOK
+	tristate "OLPC XO-1.5 ebook switch"
+	depends on ACPI && INPUT
+	---help---
+	  Support for the ebook switch on the OLPC XO-1.5 laptop.
+
+	  This switch is triggered as the screen is rotated and folded down to
+	  convert the device into ebook form.
+
 endif # X86_PLATFORM_DEVICES
diff --git a/drivers/platform/x86/Makefile b/drivers/platform/x86/Makefile
index 4ec4ff8f9182..a9877711b3f1 100644
--- a/drivers/platform/x86/Makefile
+++ b/drivers/platform/x86/Makefile
@@ -10,8 +10,10 @@ obj-$(CONFIG_ACPI_CMPC)		+= classmate-laptop.o
 obj-$(CONFIG_COMPAL_LAPTOP)	+= compal-laptop.o
 obj-$(CONFIG_DELL_LAPTOP)	+= dell-laptop.o
 obj-$(CONFIG_DELL_WMI)		+= dell-wmi.o
+obj-$(CONFIG_DELL_WMI_AIO)	+= dell-wmi-aio.o
 obj-$(CONFIG_ACER_WMI)		+= acer-wmi.o
 obj-$(CONFIG_ACERHDF)		+= acerhdf.o
+obj-$(CONFIG_HP_ACCEL)		+= hp_accel.o
 obj-$(CONFIG_HP_WMI)		+= hp-wmi.o
 obj-$(CONFIG_TC1100_WMI)	+= tc1100-wmi.o
 obj-$(CONFIG_SONY_LAPTOP)	+= sony-laptop.o
@@ -28,9 +30,12 @@ obj-$(CONFIG_TOPSTAR_LAPTOP)	+= topstar-laptop.o
 obj-$(CONFIG_ACPI_TOSHIBA)	+= toshiba_acpi.o
 obj-$(CONFIG_TOSHIBA_BT_RFKILL)	+= toshiba_bluetooth.o
 obj-$(CONFIG_INTEL_SCU_IPC)	+= intel_scu_ipc.o
-obj-$(CONFIG_INTEL_SCU_IPC_UTIL)+= intel_scu_ipcutil.o
+obj-$(CONFIG_INTEL_SCU_IPC_UTIL) += intel_scu_ipcutil.o
+obj-$(CONFIG_INTEL_MFLD_THERMAL) += intel_mid_thermal.o
 obj-$(CONFIG_RAR_REGISTER)	+= intel_rar_register.o
 obj-$(CONFIG_INTEL_IPS)		+= intel_ips.o
 obj-$(CONFIG_GPIO_INTEL_PMIC)	+= intel_pmic_gpio.o
 obj-$(CONFIG_XO1_RFKILL)	+= xo1-rfkill.o
+obj-$(CONFIG_XO15_EBOOK)	+= xo15-ebook.o
 obj-$(CONFIG_IBM_RTL)		+= ibm_rtl.o
+obj-$(CONFIG_INTEL_MFLD_THERMAL)	+= intel_mid_thermal.o
diff --git a/drivers/platform/x86/acer-wmi.c b/drivers/platform/x86/acer-wmi.c
index 38b34a73866a..ad3d099bf5c1 100644
--- a/drivers/platform/x86/acer-wmi.c
+++ b/drivers/platform/x86/acer-wmi.c
@@ -39,7 +39,6 @@
 #include <linux/slab.h>
 #include <linux/input.h>
 #include <linux/input/sparse-keymap.h>
-#include <linux/dmi.h>
 
 #include <acpi/acpi_drivers.h>
 
diff --git a/drivers/platform/x86/asus-laptop.c b/drivers/platform/x86/asus-laptop.c
index f3aa6a7fdab6..92e2a0b16328 100644
--- a/drivers/platform/x86/asus-laptop.c
+++ b/drivers/platform/x86/asus-laptop.c
@@ -50,6 +50,7 @@
 #include <linux/input/sparse-keymap.h>
 #include <linux/rfkill.h>
 #include <linux/slab.h>
+#include <linux/dmi.h>
 #include <acpi/acpi_drivers.h>
 #include <acpi/acpi_bus.h>
 
@@ -157,46 +158,9 @@ MODULE_PARM_DESC(wwan_status, "Set the wireless status on boot "
 #define METHOD_BRIGHTNESS_SET	"SPLV"
 #define METHOD_BRIGHTNESS_GET	"GPLV"
 
-/* Backlight */
-static acpi_handle lcd_switch_handle;
-static char *lcd_switch_paths[] = {
-  "\\_SB.PCI0.SBRG.EC0._Q10",	/* All new models */
-  "\\_SB.PCI0.ISA.EC0._Q10",	/* A1x */
-  "\\_SB.PCI0.PX40.ECD0._Q10",	/* L3C */
-  "\\_SB.PCI0.PX40.EC0.Q10",	/* M1A */
-  "\\_SB.PCI0.LPCB.EC0._Q10",	/* P30 */
-  "\\_SB.PCI0.LPCB.EC0._Q0E", /* P30/P35 */
-  "\\_SB.PCI0.PX40.Q10",	/* S1x */
-  "\\Q10"};		/* A2x, L2D, L3D, M2E */
-
 /* Display */
 #define METHOD_SWITCH_DISPLAY	"SDSP"
 
-static acpi_handle display_get_handle;
-static char *display_get_paths[] = {
-  /* A6B, A6K A6R A7D F3JM L4R M6R A3G M6A M6V VX-1 V6J V6V W3Z */
-  "\\_SB.PCI0.P0P1.VGA.GETD",
-  /* A3E A4K, A4D A4L A6J A7J A8J Z71V M9V S5A M5A z33A W1Jc W2V G1 */
-  "\\_SB.PCI0.P0P2.VGA.GETD",
-  /* A6V A6Q */
-  "\\_SB.PCI0.P0P3.VGA.GETD",
-  /* A6T, A6M */
-  "\\_SB.PCI0.P0PA.VGA.GETD",
-  /* L3C */
-  "\\_SB.PCI0.PCI1.VGAC.NMAP",
-  /* Z96F */
-  "\\_SB.PCI0.VGA.GETD",
-  /* A2D */
-  "\\ACTD",
-  /* A4G Z71A W1N W5A W5F M2N M3N M5N M6N S1N S5N */
-  "\\ADVG",
-  /* P30 */
-  "\\DNXT",
-  /* A2H D1 L2D L3D L3H L2E L5D L5C M1A M2E L4L W3V */
-  "\\INFB",
-  /* A3F A6F A3N A3L M6N W3N W6A */
-  "\\SSTE"};
-
 #define METHOD_ALS_CONTROL	"ALSC" /* Z71A Z71V */
 #define METHOD_ALS_LEVEL	"ALSL" /* Z71A Z71V */
 
@@ -246,7 +210,6 @@ struct asus_laptop {
 
 	int wireless_status;
 	bool have_rsts;
-	int lcd_state;
 
 	struct rfkill *gps_rfkill;
 
@@ -559,48 +522,6 @@ error:
 /*
  * Backlight device
  */
-static int asus_lcd_status(struct asus_laptop *asus)
-{
-	return asus->lcd_state;
-}
-
-static int asus_lcd_set(struct asus_laptop *asus, int value)
-{
-	int lcd = 0;
-	acpi_status status = 0;
-
-	lcd = !!value;
-
-	if (lcd == asus_lcd_status(asus))
-		return 0;
-
-	if (!lcd_switch_handle)
-		return -ENODEV;
-
-	status = acpi_evaluate_object(lcd_switch_handle,
-				      NULL, NULL, NULL);
-
-	if (ACPI_FAILURE(status)) {
-		pr_warning("Error switching LCD\n");
-		return -ENODEV;
-	}
-
-	asus->lcd_state = lcd;
-	return 0;
-}
-
-static void lcd_blank(struct asus_laptop *asus, int blank)
-{
-	struct backlight_device *bd = asus->backlight_device;
-
-	asus->lcd_state = (blank == FB_BLANK_UNBLANK);
-
-	if (bd) {
-		bd->props.power = blank;
-		backlight_update_status(bd);
-	}
-}
-
 static int asus_read_brightness(struct backlight_device *bd)
 {
 	struct asus_laptop *asus = bl_get_data(bd);
@@ -628,16 +549,9 @@ static int asus_set_brightness(struct backlight_device *bd, int value)
 
 static int update_bl_status(struct backlight_device *bd)
 {
-	struct asus_laptop *asus = bl_get_data(bd);
-	int rv;
 	int value = bd->props.brightness;
 
-	rv = asus_set_brightness(bd, value);
-	if (rv)
-		return rv;
-
-	value = (bd->props.power == FB_BLANK_UNBLANK) ? 1 : 0;
-	return asus_lcd_set(asus, value);
+	return asus_set_brightness(bd, value);
 }
 
 static const struct backlight_ops asusbl_ops = {
@@ -661,8 +575,7 @@ static int asus_backlight_init(struct asus_laptop *asus)
 	struct backlight_properties props;
 
 	if (acpi_check_handle(asus->handle, METHOD_BRIGHTNESS_GET, NULL) ||
-	    acpi_check_handle(asus->handle, METHOD_BRIGHTNESS_SET, NULL) ||
-	    !lcd_switch_handle)
+	    acpi_check_handle(asus->handle, METHOD_BRIGHTNESS_SET, NULL))
 		return 0;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
@@ -970,41 +883,6 @@ static void asus_set_display(struct asus_laptop *asus, int value)
 	return;
 }
 
-static int read_display(struct asus_laptop *asus)
-{
-	unsigned long long value = 0;
-	acpi_status rv = AE_OK;
-
-	/*
-	 * In most of the case, we know how to set the display, but sometime
-	 * we can't read it
-	 */
-	if (display_get_handle) {
-		rv = acpi_evaluate_integer(display_get_handle, NULL,
-					   NULL, &value);
-		if (ACPI_FAILURE(rv))
-			pr_warning("Error reading display status\n");
-	}
-
-	value &= 0x0F; /* needed for some models, shouldn't hurt others */
-
-	return value;
-}
-
-/*
- * Now, *this* one could be more user-friendly, but so far, no-one has
- * complained. The significance of bits is the same as in store_disp()
- */
-static ssize_t show_disp(struct device *dev,
-			 struct device_attribute *attr, char *buf)
-{
-	struct asus_laptop *asus = dev_get_drvdata(dev);
-
-	if (!display_get_handle)
-		return -ENODEV;
-	return sprintf(buf, "%d\n", read_display(asus));
-}
-
 /*
  * Experimental support for display switching. As of now: 1 should activate
  * the LCD output, 2 should do for CRT, 4 for TV-Out and 8 for DVI.
@@ -1246,15 +1124,6 @@ static void asus_acpi_notify(struct acpi_device *device, u32 event)
 	struct asus_laptop *asus = acpi_driver_data(device);
 	u16 count;
 
-	/*
-	 * We need to tell the backlight device when the backlight power is
-	 * switched
-	 */
-	if (event == ATKD_LCD_ON)
-		lcd_blank(asus, FB_BLANK_UNBLANK);
-	else if (event == ATKD_LCD_OFF)
-		lcd_blank(asus, FB_BLANK_POWERDOWN);
-
 	/* TODO Find a better way to handle events count. */
 	count = asus->event_count[event % 128]++;
 	acpi_bus_generate_proc_event(asus->device, event, count);
@@ -1281,7 +1150,7 @@ static DEVICE_ATTR(bluetooth, S_IRUGO | S_IWUSR,
 		   show_bluetooth, store_bluetooth);
 static DEVICE_ATTR(wimax, S_IRUGO | S_IWUSR, show_wimax, store_wimax);
 static DEVICE_ATTR(wwan, S_IRUGO | S_IWUSR, show_wwan, store_wwan);
-static DEVICE_ATTR(display, S_IRUGO | S_IWUSR, show_disp, store_disp);
+static DEVICE_ATTR(display, S_IWUSR, NULL, store_disp);
 static DEVICE_ATTR(ledd, S_IRUGO | S_IWUSR, show_ledd, store_ledd);
 static DEVICE_ATTR(ls_level, S_IRUGO | S_IWUSR, show_lslvl, store_lslvl);
 static DEVICE_ATTR(ls_switch, S_IRUGO | S_IWUSR, show_lssw, store_lssw);
@@ -1392,26 +1261,6 @@ static struct platform_driver platform_driver = {
 	}
 };
 
-static int asus_handle_init(char *name, acpi_handle * handle,
-			    char **paths, int num_paths)
-{
-	int i;
-	acpi_status status;
-
-	for (i = 0; i < num_paths; i++) {
-		status = acpi_get_handle(NULL, paths[i], handle);
-		if (ACPI_SUCCESS(status))
-			return 0;
-	}
-
-	*handle = NULL;
-	return -ENODEV;
-}
-
-#define ASUS_HANDLE_INIT(object)					\
-	asus_handle_init(#object, &object##_handle, object##_paths,	\
-			 ARRAY_SIZE(object##_paths))
-
 /*
  * This function is used to initialize the context with right values. In this
  * method, we can make all the detection we want, and modify the asus_laptop
@@ -1497,10 +1346,6 @@ static int asus_laptop_get_info(struct asus_laptop *asus)
 	if (!acpi_check_handle(asus->handle, METHOD_WL_STATUS, NULL))
 		asus->have_rsts = true;
 
-	/* Scheduled for removal */
-	ASUS_HANDLE_INIT(lcd_switch);
-	ASUS_HANDLE_INIT(display_get);
-
 	kfree(model);
 
 	return AE_OK;
@@ -1552,10 +1397,23 @@ static int __devinit asus_acpi_init(struct asus_laptop *asus)
 		asus_als_level(asus, asus->light_level);
 	}
 
-	asus->lcd_state = 1; /* LCD should be on when the module load */
 	return result;
 }
 
+static void __devinit asus_dmi_check(void)
+{
+	const char *model;
+
+	model = dmi_get_system_info(DMI_PRODUCT_NAME);
+	if (!model)
+		return;
+
+	/* On L1400B WLED control the sound card, don't mess with it ... */
+	if (strncmp(model, "L1400B", 6) == 0) {
+		wlan_status = -1;
+	}
+}
+
 static bool asus_device_present;
 
 static int __devinit asus_acpi_add(struct acpi_device *device)
@@ -1574,6 +1432,8 @@ static int __devinit asus_acpi_add(struct acpi_device *device)
 	device->driver_data = asus;
 	asus->device = device;
 
+	asus_dmi_check();
+
 	result = asus_acpi_init(asus);
 	if (result)
 		goto fail_platform;
diff --git a/drivers/platform/x86/dell-wmi-aio.c b/drivers/platform/x86/dell-wmi-aio.c
new file mode 100644
index 000000000000..0ed84573ae1f
--- /dev/null
+++ b/drivers/platform/x86/dell-wmi-aio.c
@@ -0,0 +1,171 @@
+/*
+ *  WMI hotkeys support for Dell All-In-One series
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/input.h>
+#include <linux/input/sparse-keymap.h>
+#include <acpi/acpi_drivers.h>
+#include <linux/acpi.h>
+#include <linux/string.h>
+
+MODULE_DESCRIPTION("WMI hotkeys driver for Dell All-In-One series");
+MODULE_LICENSE("GPL");
+
+#define EVENT_GUID1 "284A0E6B-380E-472A-921F-E52786257FB4"
+#define EVENT_GUID2 "02314822-307C-4F66-BF0E-48AEAEB26CC8"
+
+static const char *dell_wmi_aio_guids[] = {
+	EVENT_GUID1,
+	EVENT_GUID2,
+	NULL
+};
+
+MODULE_ALIAS("wmi:"EVENT_GUID1);
+MODULE_ALIAS("wmi:"EVENT_GUID2);
+
+static const struct key_entry dell_wmi_aio_keymap[] = {
+	{ KE_KEY, 0xc0, { KEY_VOLUMEUP } },
+	{ KE_KEY, 0xc1, { KEY_VOLUMEDOWN } },
+	{ KE_END, 0 }
+};
+
+static struct input_dev *dell_wmi_aio_input_dev;
+
+static void dell_wmi_aio_notify(u32 value, void *context)
+{
+	struct acpi_buffer response = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *obj;
+	acpi_status status;
+
+	status = wmi_get_event_data(value, &response);
+	if (status != AE_OK) {
+		pr_info("bad event status 0x%x\n", status);
+		return;
+	}
+
+	obj = (union acpi_object *)response.pointer;
+	if (obj) {
+		unsigned int scancode;
+
+		switch (obj->type) {
+		case ACPI_TYPE_INTEGER:
+			/* Most All-In-One correctly return integer scancode */
+			scancode = obj->integer.value;
+			sparse_keymap_report_event(dell_wmi_aio_input_dev,
+				scancode, 1, true);
+			break;
+		case ACPI_TYPE_BUFFER:
+			/* Broken machines return the scancode in a buffer */
+			if (obj->buffer.pointer && obj->buffer.length > 0) {
+				scancode = obj->buffer.pointer[0];
+				sparse_keymap_report_event(
+					dell_wmi_aio_input_dev,
+					scancode, 1, true);
+			}
+			break;
+		}
+	}
+	kfree(obj);
+}
+
+static int __init dell_wmi_aio_input_setup(void)
+{
+	int err;
+
+	dell_wmi_aio_input_dev = input_allocate_device();
+
+	if (!dell_wmi_aio_input_dev)
+		return -ENOMEM;
+
+	dell_wmi_aio_input_dev->name = "Dell AIO WMI hotkeys";
+	dell_wmi_aio_input_dev->phys = "wmi/input0";
+	dell_wmi_aio_input_dev->id.bustype = BUS_HOST;
+
+	err = sparse_keymap_setup(dell_wmi_aio_input_dev,
+			dell_wmi_aio_keymap, NULL);
+	if (err) {
+		pr_err("Unable to setup input device keymap\n");
+		goto err_free_dev;
+	}
+	err = input_register_device(dell_wmi_aio_input_dev);
+	if (err) {
+		pr_info("Unable to register input device\n");
+		goto err_free_keymap;
+	}
+	return 0;
+
+err_free_keymap:
+	sparse_keymap_free(dell_wmi_aio_input_dev);
+err_free_dev:
+	input_free_device(dell_wmi_aio_input_dev);
+	return err;
+}
+
+static const char *dell_wmi_aio_find(void)
+{
+	int i;
+
+	for (i = 0; dell_wmi_aio_guids[i] != NULL; i++)
+		if (wmi_has_guid(dell_wmi_aio_guids[i]))
+			return dell_wmi_aio_guids[i];
+
+	return NULL;
+}
+
+static int __init dell_wmi_aio_init(void)
+{
+	int err;
+	const char *guid;
+
+	guid = dell_wmi_aio_find();
+	if (!guid) {
+		pr_warning("No known WMI GUID found\n");
+		return -ENXIO;
+	}
+
+	err = dell_wmi_aio_input_setup();
+	if (err)
+		return err;
+
+	err = wmi_install_notify_handler(guid, dell_wmi_aio_notify, NULL);
+	if (err) {
+		pr_err("Unable to register notify handler - %d\n", err);
+		sparse_keymap_free(dell_wmi_aio_input_dev);
+		input_unregister_device(dell_wmi_aio_input_dev);
+		return err;
+	}
+
+	return 0;
+}
+
+static void __exit dell_wmi_aio_exit(void)
+{
+	const char *guid;
+
+	guid = dell_wmi_aio_find();
+	wmi_remove_notify_handler(guid);
+	sparse_keymap_free(dell_wmi_aio_input_dev);
+	input_unregister_device(dell_wmi_aio_input_dev);
+}
+
+module_init(dell_wmi_aio_init);
+module_exit(dell_wmi_aio_exit);
diff --git a/drivers/platform/x86/eeepc-wmi.c b/drivers/platform/x86/eeepc-wmi.c
index 4d38f98aa976..d3997757e790 100644
--- a/drivers/platform/x86/eeepc-wmi.c
+++ b/drivers/platform/x86/eeepc-wmi.c
@@ -37,9 +37,12 @@
 #include <linux/backlight.h>
 #include <linux/leds.h>
 #include <linux/rfkill.h>
+#include <linux/pci.h>
+#include <linux/pci_hotplug.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/platform_device.h>
+#include <linux/dmi.h>
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
 
@@ -57,35 +60,72 @@ MODULE_LICENSE("GPL");
 MODULE_ALIAS("wmi:"EEEPC_WMI_EVENT_GUID);
 MODULE_ALIAS("wmi:"EEEPC_WMI_MGMT_GUID);
 
-#define NOTIFY_BRNUP_MIN	0x11
-#define NOTIFY_BRNUP_MAX	0x1f
-#define NOTIFY_BRNDOWN_MIN	0x20
-#define NOTIFY_BRNDOWN_MAX	0x2e
+#define NOTIFY_BRNUP_MIN		0x11
+#define NOTIFY_BRNUP_MAX		0x1f
+#define NOTIFY_BRNDOWN_MIN		0x20
+#define NOTIFY_BRNDOWN_MAX		0x2e
 
-#define EEEPC_WMI_METHODID_DEVS	0x53564544
-#define EEEPC_WMI_METHODID_DSTS	0x53544344
-#define EEEPC_WMI_METHODID_CFVS	0x53564643
+/* WMI Methods */
+#define EEEPC_WMI_METHODID_DSTS		0x53544344
+#define EEEPC_WMI_METHODID_DEVS		0x53564544
+#define EEEPC_WMI_METHODID_CFVS		0x53564643
 
-#define EEEPC_WMI_DEVID_BACKLIGHT	0x00050012
-#define EEEPC_WMI_DEVID_TPDLED		0x00100011
+/* Wireless */
 #define EEEPC_WMI_DEVID_WLAN		0x00010011
 #define EEEPC_WMI_DEVID_BLUETOOTH	0x00010013
+#define EEEPC_WMI_DEVID_WIMAX		0x00010017
 #define EEEPC_WMI_DEVID_WWAN3G		0x00010019
 
+/* Backlight and Brightness */
+#define EEEPC_WMI_DEVID_BACKLIGHT	0x00050011
+#define EEEPC_WMI_DEVID_BRIGHTNESS	0x00050012
+
+/* Misc */
+#define EEEPC_WMI_DEVID_CAMERA		0x00060013
+
+/* Storage */
+#define EEEPC_WMI_DEVID_CARDREADER	0x00080013
+
+/* Input */
+#define EEEPC_WMI_DEVID_TOUCHPAD	0x00100011
+#define EEEPC_WMI_DEVID_TOUCHPAD_LED	0x00100012
+
+/* DSTS masks */
+#define EEEPC_WMI_DSTS_STATUS_BIT	0x00000001
+#define EEEPC_WMI_DSTS_PRESENCE_BIT	0x00010000
+#define EEEPC_WMI_DSTS_BRIGHTNESS_MASK	0x000000FF
+#define EEEPC_WMI_DSTS_MAX_BRIGTH_MASK	0x0000FF00
+
+static bool hotplug_wireless;
+
+module_param(hotplug_wireless, bool, 0444);
+MODULE_PARM_DESC(hotplug_wireless,
+		 "Enable hotplug for wireless device. "
+		 "If your laptop needs that, please report to "
+		 "acpi4asus-user@lists.sourceforge.net.");
+
 static const struct key_entry eeepc_wmi_keymap[] = {
 	/* Sleep already handled via generic ACPI code */
-	{ KE_KEY, 0x5d, { KEY_WLAN } },
-	{ KE_KEY, 0x32, { KEY_MUTE } },
-	{ KE_KEY, 0x31, { KEY_VOLUMEDOWN } },
-	{ KE_KEY, 0x30, { KEY_VOLUMEUP } },
 	{ KE_IGNORE, NOTIFY_BRNDOWN_MIN, { KEY_BRIGHTNESSDOWN } },
 	{ KE_IGNORE, NOTIFY_BRNUP_MIN, { KEY_BRIGHTNESSUP } },
-	{ KE_KEY, 0xcc, { KEY_SWITCHVIDEOMODE } },
+	{ KE_KEY, 0x30, { KEY_VOLUMEUP } },
+	{ KE_KEY, 0x31, { KEY_VOLUMEDOWN } },
+	{ KE_KEY, 0x32, { KEY_MUTE } },
+	{ KE_KEY, 0x5c, { KEY_F15 } }, /* Power Gear key */
+	{ KE_KEY, 0x5d, { KEY_WLAN } },
 	{ KE_KEY, 0x6b, { KEY_F13 } }, /* Disable Touchpad */
-	{ KE_KEY, 0xe1, { KEY_F14 } },
-	{ KE_KEY, 0xe9, { KEY_DISPLAY_OFF } },
-	{ KE_KEY, 0xe0, { KEY_PROG1 } },
-	{ KE_KEY, 0x5c, { KEY_F15 } },
+	{ KE_KEY, 0x82, { KEY_CAMERA } },
+	{ KE_KEY, 0x83, { KEY_CAMERA_ZOOMIN } },
+	{ KE_KEY, 0x88, { KEY_WLAN } },
+	{ KE_KEY, 0xcc, { KEY_SWITCHVIDEOMODE } },
+	{ KE_KEY, 0xe0, { KEY_PROG1 } }, /* Task Manager */
+	{ KE_KEY, 0xe1, { KEY_F14 } }, /* Change Resolution */
+	{ KE_KEY, 0xe9, { KEY_BRIGHTNESS_ZERO } },
+	{ KE_KEY, 0xeb, { KEY_CAMERA_ZOOMOUT } },
+	{ KE_KEY, 0xec, { KEY_CAMERA_UP } },
+	{ KE_KEY, 0xed, { KEY_CAMERA_DOWN } },
+	{ KE_KEY, 0xee, { KEY_CAMERA_LEFT } },
+	{ KE_KEY, 0xef, { KEY_CAMERA_RIGHT } },
 	{ KE_END, 0},
 };
 
@@ -108,6 +148,8 @@ struct eeepc_wmi_debug {
 };
 
 struct eeepc_wmi {
+	bool hotplug_wireless;
+
 	struct input_dev *inputdev;
 	struct backlight_device *backlight_device;
 	struct platform_device *platform_device;
@@ -119,14 +161,18 @@ struct eeepc_wmi {
 
 	struct rfkill *wlan_rfkill;
 	struct rfkill *bluetooth_rfkill;
+	struct rfkill *wimax_rfkill;
 	struct rfkill *wwan3g_rfkill;
 
+	struct hotplug_slot *hotplug_slot;
+	struct mutex hotplug_lock;
+	struct mutex wmi_lock;
+	struct workqueue_struct *hotplug_workqueue;
+	struct work_struct hotplug_work;
+
 	struct eeepc_wmi_debug debug;
 };
 
-/* Only used in eeepc_wmi_init() and eeepc_wmi_exit() */
-static struct platform_device *platform_device;
-
 static int eeepc_wmi_input_init(struct eeepc_wmi *eeepc)
 {
 	int err;
@@ -176,7 +222,8 @@ static acpi_status eeepc_wmi_get_devstate(u32 dev_id, u32 *retval)
 	u32 tmp;
 
 	status = wmi_evaluate_method(EEEPC_WMI_MGMT_GUID,
-			1, EEEPC_WMI_METHODID_DSTS, &input, &output);
+				     1, EEEPC_WMI_METHODID_DSTS,
+				     &input, &output);
 
 	if (ACPI_FAILURE(status))
 		return status;
@@ -236,6 +283,28 @@ static acpi_status eeepc_wmi_set_devstate(u32 dev_id, u32 ctrl_param,
 	return status;
 }
 
+/* Helper for special devices with magic return codes */
+static int eeepc_wmi_get_devstate_bits(u32 dev_id, u32 mask)
+{
+	u32 retval = 0;
+	acpi_status status;
+
+	status = eeepc_wmi_get_devstate(dev_id, &retval);
+
+	if (ACPI_FAILURE(status))
+		return -EINVAL;
+
+	if (!(retval & EEEPC_WMI_DSTS_PRESENCE_BIT))
+		return -ENODEV;
+
+	return retval & mask;
+}
+
+static int eeepc_wmi_get_devstate_simple(u32 dev_id)
+{
+	return eeepc_wmi_get_devstate_bits(dev_id, EEEPC_WMI_DSTS_STATUS_BIT);
+}
+
 /*
  * LEDs
  */
@@ -253,7 +322,7 @@ static void tpd_led_update(struct work_struct *work)
 	eeepc = container_of(work, struct eeepc_wmi, tpd_led_work);
 
 	ctrl_param = eeepc->tpd_led_wk;
-	eeepc_wmi_set_devstate(EEEPC_WMI_DEVID_TPDLED, ctrl_param, NULL);
+	eeepc_wmi_set_devstate(EEEPC_WMI_DEVID_TOUCHPAD_LED, ctrl_param, NULL);
 }
 
 static void tpd_led_set(struct led_classdev *led_cdev,
@@ -267,25 +336,9 @@ static void tpd_led_set(struct led_classdev *led_cdev,
 	queue_work(eeepc->led_workqueue, &eeepc->tpd_led_work);
 }
 
-static int read_tpd_state(struct eeepc_wmi *eeepc)
+static int read_tpd_led_state(struct eeepc_wmi *eeepc)
 {
-	u32 retval;
-	acpi_status status;
-
-	status = eeepc_wmi_get_devstate(EEEPC_WMI_DEVID_TPDLED, &retval);
-
-	if (ACPI_FAILURE(status))
-		return -1;
-	else if (!retval || retval == 0x00060000)
-		/*
-		 * if touchpad led is present, DSTS will set some bits,
-		 * usually 0x00020000.
-		 * 0x00060000 means that the device is not supported
-		 */
-		return -ENODEV;
-	else
-		/* Status is stored in the first bit */
-		return retval & 0x1;
+	return eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_TOUCHPAD_LED);
 }
 
 static enum led_brightness tpd_led_get(struct led_classdev *led_cdev)
@@ -294,14 +347,14 @@ static enum led_brightness tpd_led_get(struct led_classdev *led_cdev)
 
 	eeepc = container_of(led_cdev, struct eeepc_wmi, tpd_led);
 
-	return read_tpd_state(eeepc);
+	return read_tpd_led_state(eeepc);
 }
 
 static int eeepc_wmi_led_init(struct eeepc_wmi *eeepc)
 {
 	int rv;
 
-	if (read_tpd_state(eeepc) < 0)
+	if (read_tpd_led_state(eeepc) < 0)
 		return 0;
 
 	eeepc->led_workqueue = create_singlethread_workqueue("led_workqueue");
@@ -333,30 +386,280 @@ static void eeepc_wmi_led_exit(struct eeepc_wmi *eeepc)
 }
 
 /*
+ * PCI hotplug (for wlan rfkill)
+ */
+static bool eeepc_wlan_rfkill_blocked(struct eeepc_wmi *eeepc)
+{
+	int result = eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_WLAN);
+
+	if (result < 0)
+		return false;
+	return !result;
+}
+
+static void eeepc_rfkill_hotplug(struct eeepc_wmi *eeepc)
+{
+	struct pci_dev *dev;
+	struct pci_bus *bus;
+	bool blocked;
+	bool absent;
+	u32 l;
+
+	mutex_lock(&eeepc->wmi_lock);
+	blocked = eeepc_wlan_rfkill_blocked(eeepc);
+	mutex_unlock(&eeepc->wmi_lock);
+
+	mutex_lock(&eeepc->hotplug_lock);
+
+	if (eeepc->wlan_rfkill)
+		rfkill_set_sw_state(eeepc->wlan_rfkill, blocked);
+
+	if (eeepc->hotplug_slot) {
+		bus = pci_find_bus(0, 1);
+		if (!bus) {
+			pr_warning("Unable to find PCI bus 1?\n");
+			goto out_unlock;
+		}
+
+		if (pci_bus_read_config_dword(bus, 0, PCI_VENDOR_ID, &l)) {
+			pr_err("Unable to read PCI config space?\n");
+			goto out_unlock;
+		}
+		absent = (l == 0xffffffff);
+
+		if (blocked != absent) {
+			pr_warning("BIOS says wireless lan is %s, "
+					"but the pci device is %s\n",
+				blocked ? "blocked" : "unblocked",
+				absent ? "absent" : "present");
+			pr_warning("skipped wireless hotplug as probably "
+					"inappropriate for this model\n");
+			goto out_unlock;
+		}
+
+		if (!blocked) {
+			dev = pci_get_slot(bus, 0);
+			if (dev) {
+				/* Device already present */
+				pci_dev_put(dev);
+				goto out_unlock;
+			}
+			dev = pci_scan_single_device(bus, 0);
+			if (dev) {
+				pci_bus_assign_resources(bus);
+				if (pci_bus_add_device(dev))
+					pr_err("Unable to hotplug wifi\n");
+			}
+		} else {
+			dev = pci_get_slot(bus, 0);
+			if (dev) {
+				pci_remove_bus_device(dev);
+				pci_dev_put(dev);
+			}
+		}
+	}
+
+out_unlock:
+	mutex_unlock(&eeepc->hotplug_lock);
+}
+
+static void eeepc_rfkill_notify(acpi_handle handle, u32 event, void *data)
+{
+	struct eeepc_wmi *eeepc = data;
+
+	if (event != ACPI_NOTIFY_BUS_CHECK)
+		return;
+
+	/*
+	 * We can't call directly eeepc_rfkill_hotplug because most
+	 * of the time WMBC is still being executed and not reetrant.
+	 * There is currently no way to tell ACPICA that  we want this
+	 * method to be serialized, we schedule a eeepc_rfkill_hotplug
+	 * call later, in a safer context.
+	 */
+	queue_work(eeepc->hotplug_workqueue, &eeepc->hotplug_work);
+}
+
+static int eeepc_register_rfkill_notifier(struct eeepc_wmi *eeepc,
+					  char *node)
+{
+	acpi_status status;
+	acpi_handle handle;
+
+	status = acpi_get_handle(NULL, node, &handle);
+
+	if (ACPI_SUCCESS(status)) {
+		status = acpi_install_notify_handler(handle,
+						     ACPI_SYSTEM_NOTIFY,
+						     eeepc_rfkill_notify,
+						     eeepc);
+		if (ACPI_FAILURE(status))
+			pr_warning("Failed to register notify on %s\n", node);
+	} else
+		return -ENODEV;
+
+	return 0;
+}
+
+static void eeepc_unregister_rfkill_notifier(struct eeepc_wmi *eeepc,
+					     char *node)
+{
+	acpi_status status = AE_OK;
+	acpi_handle handle;
+
+	status = acpi_get_handle(NULL, node, &handle);
+
+	if (ACPI_SUCCESS(status)) {
+		status = acpi_remove_notify_handler(handle,
+						     ACPI_SYSTEM_NOTIFY,
+						     eeepc_rfkill_notify);
+		if (ACPI_FAILURE(status))
+			pr_err("Error removing rfkill notify handler %s\n",
+				node);
+	}
+}
+
+static int eeepc_get_adapter_status(struct hotplug_slot *hotplug_slot,
+				    u8 *value)
+{
+	int result = eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_WLAN);
+
+	if (result < 0)
+		return result;
+
+	*value = !!result;
+	return 0;
+}
+
+static void eeepc_cleanup_pci_hotplug(struct hotplug_slot *hotplug_slot)
+{
+	kfree(hotplug_slot->info);
+	kfree(hotplug_slot);
+}
+
+static struct hotplug_slot_ops eeepc_hotplug_slot_ops = {
+	.owner = THIS_MODULE,
+	.get_adapter_status = eeepc_get_adapter_status,
+	.get_power_status = eeepc_get_adapter_status,
+};
+
+static void eeepc_hotplug_work(struct work_struct *work)
+{
+	struct eeepc_wmi *eeepc;
+
+	eeepc = container_of(work, struct eeepc_wmi, hotplug_work);
+	eeepc_rfkill_hotplug(eeepc);
+}
+
+static int eeepc_setup_pci_hotplug(struct eeepc_wmi *eeepc)
+{
+	int ret = -ENOMEM;
+	struct pci_bus *bus = pci_find_bus(0, 1);
+
+	if (!bus) {
+		pr_err("Unable to find wifi PCI bus\n");
+		return -ENODEV;
+	}
+
+	eeepc->hotplug_workqueue =
+		create_singlethread_workqueue("hotplug_workqueue");
+	if (!eeepc->hotplug_workqueue)
+		goto error_workqueue;
+
+	INIT_WORK(&eeepc->hotplug_work, eeepc_hotplug_work);
+
+	eeepc->hotplug_slot = kzalloc(sizeof(struct hotplug_slot), GFP_KERNEL);
+	if (!eeepc->hotplug_slot)
+		goto error_slot;
+
+	eeepc->hotplug_slot->info = kzalloc(sizeof(struct hotplug_slot_info),
+					    GFP_KERNEL);
+	if (!eeepc->hotplug_slot->info)
+		goto error_info;
+
+	eeepc->hotplug_slot->private = eeepc;
+	eeepc->hotplug_slot->release = &eeepc_cleanup_pci_hotplug;
+	eeepc->hotplug_slot->ops = &eeepc_hotplug_slot_ops;
+	eeepc_get_adapter_status(eeepc->hotplug_slot,
+				 &eeepc->hotplug_slot->info->adapter_status);
+
+	ret = pci_hp_register(eeepc->hotplug_slot, bus, 0, "eeepc-wifi");
+	if (ret) {
+		pr_err("Unable to register hotplug slot - %d\n", ret);
+		goto error_register;
+	}
+
+	return 0;
+
+error_register:
+	kfree(eeepc->hotplug_slot->info);
+error_info:
+	kfree(eeepc->hotplug_slot);
+	eeepc->hotplug_slot = NULL;
+error_slot:
+	destroy_workqueue(eeepc->hotplug_workqueue);
+error_workqueue:
+	return ret;
+}
+
+/*
  * Rfkill devices
  */
 static int eeepc_rfkill_set(void *data, bool blocked)
 {
 	int dev_id = (unsigned long)data;
 	u32 ctrl_param = !blocked;
+	acpi_status status;
+
+	status = eeepc_wmi_set_devstate(dev_id, ctrl_param, NULL);
+
+	if (ACPI_FAILURE(status))
+		return -EIO;
 
-	return eeepc_wmi_set_devstate(dev_id, ctrl_param, NULL);
+	return 0;
 }
 
 static void eeepc_rfkill_query(struct rfkill *rfkill, void *data)
 {
 	int dev_id = (unsigned long)data;
-	u32 retval;
-	acpi_status status;
+	int result;
 
-	status = eeepc_wmi_get_devstate(dev_id, &retval);
+	result = eeepc_wmi_get_devstate_simple(dev_id);
 
-	if (ACPI_FAILURE(status))
+	if (result < 0)
 		return ;
 
-	rfkill_set_sw_state(rfkill, !(retval & 0x1));
+	rfkill_set_sw_state(rfkill, !result);
+}
+
+static int eeepc_rfkill_wlan_set(void *data, bool blocked)
+{
+	struct eeepc_wmi *eeepc = data;
+	int ret;
+
+	/*
+	 * This handler is enabled only if hotplug is enabled.
+	 * In this case, the eeepc_wmi_set_devstate() will
+	 * trigger a wmi notification and we need to wait
+	 * this call to finish before being able to call
+	 * any wmi method
+	 */
+	mutex_lock(&eeepc->wmi_lock);
+	ret = eeepc_rfkill_set((void *)(long)EEEPC_WMI_DEVID_WLAN, blocked);
+	mutex_unlock(&eeepc->wmi_lock);
+	return ret;
 }
 
+static void eeepc_rfkill_wlan_query(struct rfkill *rfkill, void *data)
+{
+	eeepc_rfkill_query(rfkill, (void *)(long)EEEPC_WMI_DEVID_WLAN);
+}
+
+static const struct rfkill_ops eeepc_rfkill_wlan_ops = {
+	.set_block = eeepc_rfkill_wlan_set,
+	.query = eeepc_rfkill_wlan_query,
+};
+
 static const struct rfkill_ops eeepc_rfkill_ops = {
 	.set_block = eeepc_rfkill_set,
 	.query = eeepc_rfkill_query,
@@ -367,31 +670,22 @@ static int eeepc_new_rfkill(struct eeepc_wmi *eeepc,
 			    const char *name,
 			    enum rfkill_type type, int dev_id)
 {
-	int result;
-	u32 retval;
-	acpi_status status;
-
-	status = eeepc_wmi_get_devstate(dev_id, &retval);
+	int result = eeepc_wmi_get_devstate_simple(dev_id);
 
-	if (ACPI_FAILURE(status))
-		return -1;
-
-	/* If the device is present, DSTS will always set some bits
-	 * 0x00070000 - 1110000000000000000 - device supported
-	 * 0x00060000 - 1100000000000000000 - not supported
-	 * 0x00020000 - 0100000000000000000 - device supported
-	 * 0x00010000 - 0010000000000000000 - not supported / special mode ?
-	 */
-	if (!retval || retval == 0x00060000)
-		return -ENODEV;
+	if (result < 0)
+		return result;
 
-	*rfkill = rfkill_alloc(name, &eeepc->platform_device->dev, type,
-			       &eeepc_rfkill_ops, (void *)(long)dev_id);
+	if (dev_id == EEEPC_WMI_DEVID_WLAN && eeepc->hotplug_wireless)
+		*rfkill = rfkill_alloc(name, &eeepc->platform_device->dev, type,
+				       &eeepc_rfkill_wlan_ops, eeepc);
+	else
+		*rfkill = rfkill_alloc(name, &eeepc->platform_device->dev, type,
+				       &eeepc_rfkill_ops, (void *)(long)dev_id);
 
 	if (!*rfkill)
 		return -EINVAL;
 
-	rfkill_init_sw_state(*rfkill, !(retval & 0x1));
+	rfkill_init_sw_state(*rfkill, !result);
 	result = rfkill_register(*rfkill);
 	if (result) {
 		rfkill_destroy(*rfkill);
@@ -403,16 +697,34 @@ static int eeepc_new_rfkill(struct eeepc_wmi *eeepc,
 
 static void eeepc_wmi_rfkill_exit(struct eeepc_wmi *eeepc)
 {
+	eeepc_unregister_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P5");
+	eeepc_unregister_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P6");
+	eeepc_unregister_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P7");
 	if (eeepc->wlan_rfkill) {
 		rfkill_unregister(eeepc->wlan_rfkill);
 		rfkill_destroy(eeepc->wlan_rfkill);
 		eeepc->wlan_rfkill = NULL;
 	}
+	/*
+	 * Refresh pci hotplug in case the rfkill state was changed after
+	 * eeepc_unregister_rfkill_notifier()
+	 */
+	eeepc_rfkill_hotplug(eeepc);
+	if (eeepc->hotplug_slot)
+		pci_hp_deregister(eeepc->hotplug_slot);
+	if (eeepc->hotplug_workqueue)
+		destroy_workqueue(eeepc->hotplug_workqueue);
+
 	if (eeepc->bluetooth_rfkill) {
 		rfkill_unregister(eeepc->bluetooth_rfkill);
 		rfkill_destroy(eeepc->bluetooth_rfkill);
 		eeepc->bluetooth_rfkill = NULL;
 	}
+	if (eeepc->wimax_rfkill) {
+		rfkill_unregister(eeepc->wimax_rfkill);
+		rfkill_destroy(eeepc->wimax_rfkill);
+		eeepc->wimax_rfkill = NULL;
+	}
 	if (eeepc->wwan3g_rfkill) {
 		rfkill_unregister(eeepc->wwan3g_rfkill);
 		rfkill_destroy(eeepc->wwan3g_rfkill);
@@ -424,6 +736,9 @@ static int eeepc_wmi_rfkill_init(struct eeepc_wmi *eeepc)
 {
 	int result = 0;
 
+	mutex_init(&eeepc->hotplug_lock);
+	mutex_init(&eeepc->wmi_lock);
+
 	result = eeepc_new_rfkill(eeepc, &eeepc->wlan_rfkill,
 				  "eeepc-wlan", RFKILL_TYPE_WLAN,
 				  EEEPC_WMI_DEVID_WLAN);
@@ -438,6 +753,13 @@ static int eeepc_wmi_rfkill_init(struct eeepc_wmi *eeepc)
 	if (result && result != -ENODEV)
 		goto exit;
 
+	result = eeepc_new_rfkill(eeepc, &eeepc->wimax_rfkill,
+				  "eeepc-wimax", RFKILL_TYPE_WIMAX,
+				  EEEPC_WMI_DEVID_WIMAX);
+
+	if (result && result != -ENODEV)
+		goto exit;
+
 	result = eeepc_new_rfkill(eeepc, &eeepc->wwan3g_rfkill,
 				  "eeepc-wwan3g", RFKILL_TYPE_WWAN,
 				  EEEPC_WMI_DEVID_WWAN3G);
@@ -445,6 +767,26 @@ static int eeepc_wmi_rfkill_init(struct eeepc_wmi *eeepc)
 	if (result && result != -ENODEV)
 		goto exit;
 
+	if (!eeepc->hotplug_wireless)
+		goto exit;
+
+	result = eeepc_setup_pci_hotplug(eeepc);
+	/*
+	 * If we get -EBUSY then something else is handling the PCI hotplug -
+	 * don't fail in this case
+	 */
+	if (result == -EBUSY)
+		result = 0;
+
+	eeepc_register_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P5");
+	eeepc_register_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P6");
+	eeepc_register_rfkill_notifier(eeepc, "\\_SB.PCI0.P0P7");
+	/*
+	 * Refresh pci hotplug in case the rfkill state was changed during
+	 * setup.
+	 */
+	eeepc_rfkill_hotplug(eeepc);
+
 exit:
 	if (result && result != -ENODEV)
 		eeepc_wmi_rfkill_exit(eeepc);
@@ -458,34 +800,53 @@ exit:
 /*
  * Backlight
  */
+static int read_backlight_power(void)
+{
+	int ret = eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_BACKLIGHT);
+
+	if (ret < 0)
+		return ret;
+
+	return ret ? FB_BLANK_UNBLANK : FB_BLANK_POWERDOWN;
+}
+
 static int read_brightness(struct backlight_device *bd)
 {
 	u32 retval;
 	acpi_status status;
 
-	status = eeepc_wmi_get_devstate(EEEPC_WMI_DEVID_BACKLIGHT, &retval);
+	status = eeepc_wmi_get_devstate(EEEPC_WMI_DEVID_BRIGHTNESS, &retval);
 
 	if (ACPI_FAILURE(status))
-		return -1;
+		return -EIO;
 	else
-		return retval & 0xFF;
+		return retval & EEEPC_WMI_DSTS_BRIGHTNESS_MASK;
 }
 
 static int update_bl_status(struct backlight_device *bd)
 {
-
 	u32 ctrl_param;
 	acpi_status status;
+	int power;
 
 	ctrl_param = bd->props.brightness;
 
-	status = eeepc_wmi_set_devstate(EEEPC_WMI_DEVID_BACKLIGHT,
+	status = eeepc_wmi_set_devstate(EEEPC_WMI_DEVID_BRIGHTNESS,
 					ctrl_param, NULL);
 
 	if (ACPI_FAILURE(status))
-		return -1;
-	else
-		return 0;
+		return -EIO;
+
+	power = read_backlight_power();
+	if (power != -ENODEV && bd->props.power != power) {
+		ctrl_param = !!(bd->props.power == FB_BLANK_UNBLANK);
+		status = eeepc_wmi_set_devstate(EEEPC_WMI_DEVID_BACKLIGHT,
+						ctrl_param, NULL);
+
+		if (ACPI_FAILURE(status))
+			return -EIO;
+	}
+	return 0;
 }
 
 static const struct backlight_ops eeepc_wmi_bl_ops = {
@@ -515,9 +876,29 @@ static int eeepc_wmi_backlight_init(struct eeepc_wmi *eeepc)
 {
 	struct backlight_device *bd;
 	struct backlight_properties props;
+	int max;
+	int power;
+
+	max = eeepc_wmi_get_devstate_bits(EEEPC_WMI_DEVID_BRIGHTNESS,
+					  EEEPC_WMI_DSTS_MAX_BRIGTH_MASK);
+	power = read_backlight_power();
+
+	if (max < 0 && power < 0) {
+		/* Try to keep the original error */
+		if (max == -ENODEV && power == -ENODEV)
+			return -ENODEV;
+		if (max != -ENODEV)
+			return max;
+		else
+			return power;
+	}
+	if (max == -ENODEV)
+		max = 0;
+	if (power == -ENODEV)
+		power = FB_BLANK_UNBLANK;
 
 	memset(&props, 0, sizeof(struct backlight_properties));
-	props.max_brightness = 15;
+	props.max_brightness = max;
 	bd = backlight_device_register(EEEPC_WMI_FILE,
 				       &eeepc->platform_device->dev, eeepc,
 				       &eeepc_wmi_bl_ops, &props);
@@ -529,7 +910,7 @@ static int eeepc_wmi_backlight_init(struct eeepc_wmi *eeepc)
 	eeepc->backlight_device = bd;
 
 	bd->props.brightness = read_brightness(bd);
-	bd->props.power = FB_BLANK_UNBLANK;
+	bd->props.power = power;
 	backlight_update_status(bd);
 
 	return 0;
@@ -583,6 +964,71 @@ static void eeepc_wmi_notify(u32 value, void *context)
 	kfree(obj);
 }
 
+/*
+ * Sys helpers
+ */
+static int parse_arg(const char *buf, unsigned long count, int *val)
+{
+	if (!count)
+		return 0;
+	if (sscanf(buf, "%i", val) != 1)
+		return -EINVAL;
+	return count;
+}
+
+static ssize_t store_sys_wmi(int devid, const char *buf, size_t count)
+{
+	acpi_status status;
+	u32 retval;
+	int rv, value;
+
+	value = eeepc_wmi_get_devstate_simple(devid);
+	if (value == -ENODEV) /* Check device presence */
+		return value;
+
+	rv = parse_arg(buf, count, &value);
+	status = eeepc_wmi_set_devstate(devid, value, &retval);
+
+	if (ACPI_FAILURE(status))
+		return -EIO;
+	return rv;
+}
+
+static ssize_t show_sys_wmi(int devid, char *buf)
+{
+	int value = eeepc_wmi_get_devstate_simple(devid);
+
+	if (value < 0)
+		return value;
+
+	return sprintf(buf, "%d\n", value);
+}
+
+#define EEEPC_WMI_CREATE_DEVICE_ATTR(_name, _mode, _cm)			\
+	static ssize_t show_##_name(struct device *dev,			\
+				    struct device_attribute *attr,	\
+				    char *buf)				\
+	{								\
+		return show_sys_wmi(_cm, buf);				\
+	}								\
+	static ssize_t store_##_name(struct device *dev,		\
+				     struct device_attribute *attr,	\
+				     const char *buf, size_t count)	\
+	{								\
+		return store_sys_wmi(_cm, buf, count);			\
+	}								\
+	static struct device_attribute dev_attr_##_name = {		\
+		.attr = {						\
+			.name = __stringify(_name),			\
+			.mode = _mode },				\
+		.show   = show_##_name,					\
+		.store  = store_##_name,				\
+	}
+
+EEEPC_WMI_CREATE_DEVICE_ATTR(touchpad, 0644, EEEPC_WMI_DEVID_TOUCHPAD);
+EEEPC_WMI_CREATE_DEVICE_ATTR(camera, 0644, EEEPC_WMI_DEVID_CAMERA);
+EEEPC_WMI_CREATE_DEVICE_ATTR(cardr, 0644, EEEPC_WMI_DEVID_CARDREADER);
+
 static ssize_t store_cpufv(struct device *dev, struct device_attribute *attr,
 			   const char *buf, size_t count)
 {
@@ -608,11 +1054,35 @@ static DEVICE_ATTR(cpufv, S_IRUGO | S_IWUSR, NULL, store_cpufv);
 
 static struct attribute *platform_attributes[] = {
 	&dev_attr_cpufv.attr,
+	&dev_attr_camera.attr,
+	&dev_attr_cardr.attr,
+	&dev_attr_touchpad.attr,
 	NULL
 };
 
+static mode_t eeepc_sysfs_is_visible(struct kobject *kobj,
+				     struct attribute *attr,
+				     int idx)
+{
+	bool supported = true;
+	int devid = -1;
+
+	if (attr == &dev_attr_camera.attr)
+		devid = EEEPC_WMI_DEVID_CAMERA;
+	else if (attr == &dev_attr_cardr.attr)
+		devid = EEEPC_WMI_DEVID_CARDREADER;
+	else if (attr == &dev_attr_touchpad.attr)
+		devid = EEEPC_WMI_DEVID_TOUCHPAD;
+
+	if (devid != -1)
+		supported = eeepc_wmi_get_devstate_simple(devid) != -ENODEV;
+
+	return supported ? attr->mode : 0;
+}
+
 static struct attribute_group platform_attribute_group = {
-	.attrs = platform_attributes
+	.is_visible	= eeepc_sysfs_is_visible,
+	.attrs		= platform_attributes
 };
 
 static void eeepc_wmi_sysfs_exit(struct platform_device *device)
@@ -630,33 +1100,12 @@ static int eeepc_wmi_sysfs_init(struct platform_device *device)
  */
 static int __init eeepc_wmi_platform_init(struct eeepc_wmi *eeepc)
 {
-	int err;
-
-	eeepc->platform_device = platform_device_alloc(EEEPC_WMI_FILE, -1);
-	if (!eeepc->platform_device)
-		return -ENOMEM;
-	platform_set_drvdata(eeepc->platform_device, eeepc);
-
-	err = platform_device_add(eeepc->platform_device);
-	if (err)
-		goto fail_platform_device;
-
-	err = eeepc_wmi_sysfs_init(eeepc->platform_device);
-	if (err)
-		goto fail_sysfs;
-	return 0;
-
-fail_sysfs:
-	platform_device_del(eeepc->platform_device);
-fail_platform_device:
-	platform_device_put(eeepc->platform_device);
-	return err;
+	return eeepc_wmi_sysfs_init(eeepc->platform_device);
 }
 
 static void eeepc_wmi_platform_exit(struct eeepc_wmi *eeepc)
 {
 	eeepc_wmi_sysfs_exit(eeepc->platform_device);
-	platform_device_unregister(eeepc->platform_device);
 }
 
 /*
@@ -770,7 +1219,29 @@ error_debugfs:
 /*
  * WMI Driver
  */
-static struct platform_device * __init eeepc_wmi_add(void)
+static void eeepc_dmi_check(struct eeepc_wmi *eeepc)
+{
+	const char *model;
+
+	model = dmi_get_system_info(DMI_PRODUCT_NAME);
+	if (!model)
+		return;
+
+	/*
+	 * Whitelist for wlan hotplug
+	 *
+	 * Eeepc 1000H needs the current hotplug code to handle
+	 * Fn+F2 correctly. We may add other Eeepc here later, but
+	 * it seems that most of the laptops supported by eeepc-wmi
+	 * don't need to be on this list
+	 */
+	if (strcmp(model, "1000H") == 0) {
+		eeepc->hotplug_wireless = true;
+		pr_info("wlan hotplug enabled\n");
+	}
+}
+
+static int __init eeepc_wmi_add(struct platform_device *pdev)
 {
 	struct eeepc_wmi *eeepc;
 	acpi_status status;
@@ -778,12 +1249,14 @@ static struct platform_device * __init eeepc_wmi_add(void)
 
 	eeepc = kzalloc(sizeof(struct eeepc_wmi), GFP_KERNEL);
 	if (!eeepc)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
+
+	eeepc->platform_device = pdev;
+	platform_set_drvdata(eeepc->platform_device, eeepc);
+
+	eeepc->hotplug_wireless = hotplug_wireless;
+	eeepc_dmi_check(eeepc);
 
-	/*
-	 * Register the platform device first.  It is used as a parent for the
-	 * sub-devices below.
-	 */
 	err = eeepc_wmi_platform_init(eeepc);
 	if (err)
 		goto fail_platform;
@@ -802,7 +1275,7 @@ static struct platform_device * __init eeepc_wmi_add(void)
 
 	if (!acpi_video_backlight_support()) {
 		err = eeepc_wmi_backlight_init(eeepc);
-		if (err)
+		if (err && err != -ENODEV)
 			goto fail_backlight;
 	} else
 		pr_info("Backlight controlled by ACPI video driver\n");
@@ -820,7 +1293,7 @@ static struct platform_device * __init eeepc_wmi_add(void)
 	if (err)
 		goto fail_debugfs;
 
-	return eeepc->platform_device;
+	return 0;
 
 fail_debugfs:
 	wmi_remove_notify_handler(EEEPC_WMI_EVENT_GUID);
@@ -836,10 +1309,10 @@ fail_input:
 	eeepc_wmi_platform_exit(eeepc);
 fail_platform:
 	kfree(eeepc);
-	return ERR_PTR(err);
+	return err;
 }
 
-static int eeepc_wmi_remove(struct platform_device *device)
+static int __exit eeepc_wmi_remove(struct platform_device *device)
 {
 	struct eeepc_wmi *eeepc;
 
@@ -856,10 +1329,64 @@ static int eeepc_wmi_remove(struct platform_device *device)
 	return 0;
 }
 
+/*
+ * Platform driver - hibernate/resume callbacks
+ */
+static int eeepc_hotk_thaw(struct device *device)
+{
+	struct eeepc_wmi *eeepc = dev_get_drvdata(device);
+
+	if (eeepc->wlan_rfkill) {
+		bool wlan;
+
+		/*
+		 * Work around bios bug - acpi _PTS turns off the wireless led
+		 * during suspend.  Normally it restores it on resume, but
+		 * we should kick it ourselves in case hibernation is aborted.
+		 */
+		wlan = eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_WLAN);
+		eeepc_wmi_set_devstate(EEEPC_WMI_DEVID_WLAN, wlan, NULL);
+	}
+
+	return 0;
+}
+
+static int eeepc_hotk_restore(struct device *device)
+{
+	struct eeepc_wmi *eeepc = dev_get_drvdata(device);
+	int bl;
+
+	/* Refresh both wlan rfkill state and pci hotplug */
+	if (eeepc->wlan_rfkill)
+		eeepc_rfkill_hotplug(eeepc);
+
+	if (eeepc->bluetooth_rfkill) {
+		bl = !eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_BLUETOOTH);
+		rfkill_set_sw_state(eeepc->bluetooth_rfkill, bl);
+	}
+	if (eeepc->wimax_rfkill) {
+		bl = !eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_WIMAX);
+		rfkill_set_sw_state(eeepc->wimax_rfkill, bl);
+	}
+	if (eeepc->wwan3g_rfkill) {
+		bl = !eeepc_wmi_get_devstate_simple(EEEPC_WMI_DEVID_WWAN3G);
+		rfkill_set_sw_state(eeepc->wwan3g_rfkill, bl);
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops eeepc_pm_ops = {
+	.thaw = eeepc_hotk_thaw,
+	.restore = eeepc_hotk_restore,
+};
+
 static struct platform_driver platform_driver = {
+	.remove = __exit_p(eeepc_wmi_remove),
 	.driver = {
 		.name = EEEPC_WMI_FILE,
 		.owner = THIS_MODULE,
+		.pm = &eeepc_pm_ops,
 	},
 };
 
@@ -884,10 +1411,8 @@ static int __init eeepc_wmi_check_atkd(void)
 	return -1;
 }
 
-static int __init eeepc_wmi_init(void)
+static int __init eeepc_wmi_probe(struct platform_device *pdev)
 {
-	int err;
-
 	if (!wmi_has_guid(EEEPC_WMI_EVENT_GUID) ||
 	    !wmi_has_guid(EEEPC_WMI_MGMT_GUID)) {
 		pr_warning("No known WMI GUID found\n");
@@ -904,29 +1429,24 @@ static int __init eeepc_wmi_init(void)
 		return -ENODEV;
 	}
 
-	platform_device = eeepc_wmi_add();
-	if (IS_ERR(platform_device)) {
-		err = PTR_ERR(platform_device);
-		goto fail_eeepc_wmi;
-	}
+	return eeepc_wmi_add(pdev);
+}
 
-	err = platform_driver_register(&platform_driver);
-	if (err) {
-		pr_warning("Unable to register platform driver\n");
-		goto fail_platform_driver;
-	}
+static struct platform_device *platform_device;
 
+static int __init eeepc_wmi_init(void)
+{
+	platform_device = platform_create_bundle(&platform_driver,
+						 eeepc_wmi_probe,
+						 NULL, 0, NULL, 0);
+	if (IS_ERR(platform_device))
+		return PTR_ERR(platform_device);
 	return 0;
-
-fail_platform_driver:
-	eeepc_wmi_remove(platform_device);
-fail_eeepc_wmi:
-	return err;
 }
 
 static void __exit eeepc_wmi_exit(void)
 {
-	eeepc_wmi_remove(platform_device);
+	platform_device_unregister(platform_device);
 	platform_driver_unregister(&platform_driver);
 }
 
diff --git a/drivers/platform/x86/hp-wmi.c b/drivers/platform/x86/hp-wmi.c
index 9e05af9c41cb..1bc4a7539ba9 100644
--- a/drivers/platform/x86/hp-wmi.c
+++ b/drivers/platform/x86/hp-wmi.c
@@ -2,6 +2,7 @@
  * HP WMI hotkeys
  *
  * Copyright (C) 2008 Red Hat <mjg@redhat.com>
+ * Copyright (C) 2010, 2011 Anssi Hannula <anssi.hannula@iki.fi>
  *
  * Portions based on wistron_btns.c:
  * Copyright (C) 2005 Miloslav Trmac <mitr@volny.cz>
@@ -51,6 +52,7 @@ MODULE_ALIAS("wmi:5FB7F034-2C63-45e9-BE91-3D44E2C707E4");
 #define HPWMI_HARDWARE_QUERY 0x4
 #define HPWMI_WIRELESS_QUERY 0x5
 #define HPWMI_HOTKEY_QUERY 0xc
+#define HPWMI_WIRELESS2_QUERY 0x1b
 
 #define PREFIX "HP WMI: "
 #define UNIMP "Unimplemented "
@@ -86,7 +88,46 @@ struct bios_args {
 struct bios_return {
 	u32 sigpass;
 	u32 return_code;
-	u32 value;
+};
+
+enum hp_return_value {
+	HPWMI_RET_WRONG_SIGNATURE	= 0x02,
+	HPWMI_RET_UNKNOWN_COMMAND	= 0x03,
+	HPWMI_RET_UNKNOWN_CMDTYPE	= 0x04,
+	HPWMI_RET_INVALID_PARAMETERS	= 0x05,
+};
+
+enum hp_wireless2_bits {
+	HPWMI_POWER_STATE	= 0x01,
+	HPWMI_POWER_SOFT	= 0x02,
+	HPWMI_POWER_BIOS	= 0x04,
+	HPWMI_POWER_HARD	= 0x08,
+};
+
+#define IS_HWBLOCKED(x) ((x & (HPWMI_POWER_BIOS | HPWMI_POWER_HARD)) \
+			 != (HPWMI_POWER_BIOS | HPWMI_POWER_HARD))
+#define IS_SWBLOCKED(x) !(x & HPWMI_POWER_SOFT)
+
+struct bios_rfkill2_device_state {
+	u8 radio_type;
+	u8 bus_type;
+	u16 vendor_id;
+	u16 product_id;
+	u16 subsys_vendor_id;
+	u16 subsys_product_id;
+	u8 rfkill_id;
+	u8 power;
+	u8 unknown[4];
+};
+
+/* 7 devices fit into the 128 byte buffer */
+#define HPWMI_MAX_RFKILL2_DEVICES	7
+
+struct bios_rfkill2_state {
+	u8 unknown[7];
+	u8 count;
+	u8 pad[8];
+	struct bios_rfkill2_device_state device[HPWMI_MAX_RFKILL2_DEVICES];
 };
 
 static const struct key_entry hp_wmi_keymap[] = {
@@ -108,6 +149,15 @@ static struct rfkill *wifi_rfkill;
 static struct rfkill *bluetooth_rfkill;
 static struct rfkill *wwan_rfkill;
 
+struct rfkill2_device {
+	u8 id;
+	int num;
+	struct rfkill *rfkill;
+};
+
+static int rfkill2_count;
+static struct rfkill2_device rfkill2[HPWMI_MAX_RFKILL2_DEVICES];
+
 static const struct dev_pm_ops hp_wmi_pm_ops = {
 	.resume  = hp_wmi_resume_handler,
 	.restore  = hp_wmi_resume_handler,
@@ -129,7 +179,8 @@ static struct platform_driver hp_wmi_driver = {
  * query:	The commandtype -> What should be queried
  * write:	The command -> 0 read, 1 write, 3 ODM specific
  * buffer:	Buffer used as input and/or output
- * buffersize:	Size of buffer
+ * insize:	Size of input buffer
+ * outsize:	Size of output buffer
  *
  * returns zero on success
  *         an HP WMI query specific error code (which is positive)
@@ -140,25 +191,29 @@ static struct platform_driver hp_wmi_driver = {
  *       size. E.g. Battery info query (0x7) is defined to have 1 byte input
  *       and 128 byte output. The caller would do:
  *       buffer = kzalloc(128, GFP_KERNEL);
- *       ret = hp_wmi_perform_query(0x7, 0, buffer, 128)
+ *       ret = hp_wmi_perform_query(0x7, 0, buffer, 1, 128)
  */
-static int hp_wmi_perform_query(int query, int write, u32 *buffer,
-				int buffersize)
+static int hp_wmi_perform_query(int query, int write, void *buffer,
+				int insize, int outsize)
 {
-	struct bios_return bios_return;
-	acpi_status status;
+	struct bios_return *bios_return;
+	int actual_outsize;
 	union acpi_object *obj;
 	struct bios_args args = {
 		.signature = 0x55434553,
 		.command = write ? 0x2 : 0x1,
 		.commandtype = query,
-		.datasize = buffersize,
-		.data = *buffer,
+		.datasize = insize,
+		.data = 0,
 	};
 	struct acpi_buffer input = { sizeof(struct bios_args), &args };
 	struct acpi_buffer output = { ACPI_ALLOCATE_BUFFER, NULL };
 
-	status = wmi_evaluate_method(HPWMI_BIOS_GUID, 0, 0x3, &input, &output);
+	if (WARN_ON(insize > sizeof(args.data)))
+		return -EINVAL;
+	memcpy(&args.data, buffer, insize);
+
+	wmi_evaluate_method(HPWMI_BIOS_GUID, 0, 0x3, &input, &output);
 
 	obj = output.pointer;
 
@@ -169,10 +224,26 @@ static int hp_wmi_perform_query(int query, int write, u32 *buffer,
 		return -EINVAL;
 	}
 
-	bios_return = *((struct bios_return *)obj->buffer.pointer);
+	bios_return = (struct bios_return *)obj->buffer.pointer;
 
-	memcpy(buffer, &bios_return.value, sizeof(bios_return.value));
+	if (bios_return->return_code) {
+		if (bios_return->return_code != HPWMI_RET_UNKNOWN_CMDTYPE)
+			printk(KERN_WARNING PREFIX "query 0x%x returned "
+						   "error 0x%x\n",
+			       query, bios_return->return_code);
+		kfree(obj);
+		return bios_return->return_code;
+	}
+
+	if (!outsize) {
+		/* ignore output data */
+		kfree(obj);
+		return 0;
+	}
 
+	actual_outsize = min(outsize, (int)(obj->buffer.length - sizeof(*bios_return)));
+	memcpy(buffer, obj->buffer.pointer + sizeof(*bios_return), actual_outsize);
+	memset(buffer + actual_outsize, 0, outsize - actual_outsize);
 	kfree(obj);
 	return 0;
 }
@@ -181,7 +252,7 @@ static int hp_wmi_display_state(void)
 {
 	int state = 0;
 	int ret = hp_wmi_perform_query(HPWMI_DISPLAY_QUERY, 0, &state,
-				       sizeof(state));
+				       sizeof(state), sizeof(state));
 	if (ret)
 		return -EINVAL;
 	return state;
@@ -191,7 +262,7 @@ static int hp_wmi_hddtemp_state(void)
 {
 	int state = 0;
 	int ret = hp_wmi_perform_query(HPWMI_HDDTEMP_QUERY, 0, &state,
-				       sizeof(state));
+				       sizeof(state), sizeof(state));
 	if (ret)
 		return -EINVAL;
 	return state;
@@ -201,7 +272,7 @@ static int hp_wmi_als_state(void)
 {
 	int state = 0;
 	int ret = hp_wmi_perform_query(HPWMI_ALS_QUERY, 0, &state,
-				       sizeof(state));
+				       sizeof(state), sizeof(state));
 	if (ret)
 		return -EINVAL;
 	return state;
@@ -211,7 +282,7 @@ static int hp_wmi_dock_state(void)
 {
 	int state = 0;
 	int ret = hp_wmi_perform_query(HPWMI_HARDWARE_QUERY, 0, &state,
-				       sizeof(state));
+				       sizeof(state), sizeof(state));
 
 	if (ret)
 		return -EINVAL;
@@ -223,7 +294,7 @@ static int hp_wmi_tablet_state(void)
 {
 	int state = 0;
 	int ret = hp_wmi_perform_query(HPWMI_HARDWARE_QUERY, 0, &state,
-				       sizeof(state));
+				       sizeof(state), sizeof(state));
 	if (ret)
 		return ret;
 
@@ -237,7 +308,7 @@ static int hp_wmi_set_block(void *data, bool blocked)
 	int ret;
 
 	ret = hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 1,
-				   &query, sizeof(query));
+				   &query, sizeof(query), 0);
 	if (ret)
 		return -EINVAL;
 	return 0;
@@ -252,7 +323,8 @@ static bool hp_wmi_get_sw_state(enum hp_wmi_radio r)
 	int wireless = 0;
 	int mask;
 	hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 0,
-			     &wireless, sizeof(wireless));
+			     &wireless, sizeof(wireless),
+			     sizeof(wireless));
 	/* TBD: Pass error */
 
 	mask = 0x200 << (r * 8);
@@ -268,7 +340,8 @@ static bool hp_wmi_get_hw_state(enum hp_wmi_radio r)
 	int wireless = 0;
 	int mask;
 	hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 0,
-			     &wireless, sizeof(wireless));
+			     &wireless, sizeof(wireless),
+			     sizeof(wireless));
 	/* TBD: Pass error */
 
 	mask = 0x800 << (r * 8);
@@ -279,6 +352,51 @@ static bool hp_wmi_get_hw_state(enum hp_wmi_radio r)
 		return true;
 }
 
+static int hp_wmi_rfkill2_set_block(void *data, bool blocked)
+{
+	int rfkill_id = (int)(long)data;
+	char buffer[4] = { 0x01, 0x00, rfkill_id, !blocked };
+
+	if (hp_wmi_perform_query(HPWMI_WIRELESS2_QUERY, 1,
+				   buffer, sizeof(buffer), 0))
+		return -EINVAL;
+	return 0;
+}
+
+static const struct rfkill_ops hp_wmi_rfkill2_ops = {
+	.set_block = hp_wmi_rfkill2_set_block,
+};
+
+static int hp_wmi_rfkill2_refresh(void)
+{
+	int err, i;
+	struct bios_rfkill2_state state;
+
+	err = hp_wmi_perform_query(HPWMI_WIRELESS2_QUERY, 0, &state,
+				   0, sizeof(state));
+	if (err)
+		return err;
+
+	for (i = 0; i < rfkill2_count; i++) {
+		int num = rfkill2[i].num;
+		struct bios_rfkill2_device_state *devstate;
+		devstate = &state.device[num];
+
+		if (num >= state.count ||
+		    devstate->rfkill_id != rfkill2[i].id) {
+			printk(KERN_WARNING PREFIX "power configuration of "
+			       "the wireless devices unexpectedly changed\n");
+			continue;
+		}
+
+		rfkill_set_states(rfkill2[i].rfkill,
+				  IS_SWBLOCKED(devstate->power),
+				  IS_HWBLOCKED(devstate->power));
+	}
+
+	return 0;
+}
+
 static ssize_t show_display(struct device *dev, struct device_attribute *attr,
 			    char *buf)
 {
@@ -329,7 +447,7 @@ static ssize_t set_als(struct device *dev, struct device_attribute *attr,
 {
 	u32 tmp = simple_strtoul(buf, NULL, 10);
 	int ret = hp_wmi_perform_query(HPWMI_ALS_QUERY, 1, &tmp,
-				       sizeof(tmp));
+				       sizeof(tmp), sizeof(tmp));
 	if (ret)
 		return -EINVAL;
 
@@ -402,6 +520,7 @@ static void hp_wmi_notify(u32 value, void *context)
 	case HPWMI_BEZEL_BUTTON:
 		ret = hp_wmi_perform_query(HPWMI_HOTKEY_QUERY, 0,
 					   &key_code,
+					   sizeof(key_code),
 					   sizeof(key_code));
 		if (ret)
 			break;
@@ -412,6 +531,11 @@ static void hp_wmi_notify(u32 value, void *context)
 			       key_code);
 		break;
 	case HPWMI_WIRELESS:
+		if (rfkill2_count) {
+			hp_wmi_rfkill2_refresh();
+			break;
+		}
+
 		if (wifi_rfkill)
 			rfkill_set_states(wifi_rfkill,
 					  hp_wmi_get_sw_state(HPWMI_WIFI),
@@ -502,32 +626,16 @@ static void cleanup_sysfs(struct platform_device *device)
 	device_remove_file(&device->dev, &dev_attr_tablet);
 }
 
-static int __devinit hp_wmi_bios_setup(struct platform_device *device)
+static int __devinit hp_wmi_rfkill_setup(struct platform_device *device)
 {
 	int err;
 	int wireless = 0;
 
 	err = hp_wmi_perform_query(HPWMI_WIRELESS_QUERY, 0, &wireless,
-				   sizeof(wireless));
+				   sizeof(wireless), sizeof(wireless));
 	if (err)
 		return err;
 
-	err = device_create_file(&device->dev, &dev_attr_display);
-	if (err)
-		goto add_sysfs_error;
-	err = device_create_file(&device->dev, &dev_attr_hddtemp);
-	if (err)
-		goto add_sysfs_error;
-	err = device_create_file(&device->dev, &dev_attr_als);
-	if (err)
-		goto add_sysfs_error;
-	err = device_create_file(&device->dev, &dev_attr_dock);
-	if (err)
-		goto add_sysfs_error;
-	err = device_create_file(&device->dev, &dev_attr_tablet);
-	if (err)
-		goto add_sysfs_error;
-
 	if (wireless & 0x1) {
 		wifi_rfkill = rfkill_alloc("hp-wifi", &device->dev,
 					   RFKILL_TYPE_WLAN,
@@ -573,14 +681,131 @@ static int __devinit hp_wmi_bios_setup(struct platform_device *device)
 	return 0;
 register_wwan_err:
 	rfkill_destroy(wwan_rfkill);
+	wwan_rfkill = NULL;
 	if (bluetooth_rfkill)
 		rfkill_unregister(bluetooth_rfkill);
 register_bluetooth_error:
 	rfkill_destroy(bluetooth_rfkill);
+	bluetooth_rfkill = NULL;
 	if (wifi_rfkill)
 		rfkill_unregister(wifi_rfkill);
 register_wifi_error:
 	rfkill_destroy(wifi_rfkill);
+	wifi_rfkill = NULL;
+	return err;
+}
+
+static int __devinit hp_wmi_rfkill2_setup(struct platform_device *device)
+{
+	int err, i;
+	struct bios_rfkill2_state state;
+	err = hp_wmi_perform_query(HPWMI_WIRELESS2_QUERY, 0, &state,
+				   0, sizeof(state));
+	if (err)
+		return err;
+
+	if (state.count > HPWMI_MAX_RFKILL2_DEVICES) {
+		printk(KERN_WARNING PREFIX "unable to parse 0x1b query output\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < state.count; i++) {
+		struct rfkill *rfkill;
+		enum rfkill_type type;
+		char *name;
+		switch (state.device[i].radio_type) {
+		case HPWMI_WIFI:
+			type = RFKILL_TYPE_WLAN;
+			name = "hp-wifi";
+			break;
+		case HPWMI_BLUETOOTH:
+			type = RFKILL_TYPE_BLUETOOTH;
+			name = "hp-bluetooth";
+			break;
+		case HPWMI_WWAN:
+			type = RFKILL_TYPE_WWAN;
+			name = "hp-wwan";
+			break;
+		default:
+			printk(KERN_WARNING PREFIX "unknown device type 0x%x\n",
+				 state.device[i].radio_type);
+			continue;
+		}
+
+		if (!state.device[i].vendor_id) {
+			printk(KERN_WARNING PREFIX "zero device %d while %d "
+			       "reported\n", i, state.count);
+			continue;
+		}
+
+		rfkill = rfkill_alloc(name, &device->dev, type,
+				      &hp_wmi_rfkill2_ops, (void *)(long)i);
+		if (!rfkill) {
+			err = -ENOMEM;
+			goto fail;
+		}
+
+		rfkill2[rfkill2_count].id = state.device[i].rfkill_id;
+		rfkill2[rfkill2_count].num = i;
+		rfkill2[rfkill2_count].rfkill = rfkill;
+
+		rfkill_init_sw_state(rfkill,
+				     IS_SWBLOCKED(state.device[i].power));
+		rfkill_set_hw_state(rfkill,
+				    IS_HWBLOCKED(state.device[i].power));
+
+		if (!(state.device[i].power & HPWMI_POWER_BIOS))
+			printk(KERN_INFO PREFIX "device %s blocked by BIOS\n",
+			       name);
+
+		err = rfkill_register(rfkill);
+		if (err) {
+			rfkill_destroy(rfkill);
+			goto fail;
+		}
+
+		rfkill2_count++;
+	}
+
+	return 0;
+fail:
+	for (; rfkill2_count > 0; rfkill2_count--) {
+		rfkill_unregister(rfkill2[rfkill2_count - 1].rfkill);
+		rfkill_destroy(rfkill2[rfkill2_count - 1].rfkill);
+	}
+	return err;
+}
+
+static int __devinit hp_wmi_bios_setup(struct platform_device *device)
+{
+	int err;
+
+	/* clear detected rfkill devices */
+	wifi_rfkill = NULL;
+	bluetooth_rfkill = NULL;
+	wwan_rfkill = NULL;
+	rfkill2_count = 0;
+
+	if (hp_wmi_rfkill_setup(device))
+		hp_wmi_rfkill2_setup(device);
+
+	err = device_create_file(&device->dev, &dev_attr_display);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_hddtemp);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_als);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_dock);
+	if (err)
+		goto add_sysfs_error;
+	err = device_create_file(&device->dev, &dev_attr_tablet);
+	if (err)
+		goto add_sysfs_error;
+	return 0;
+
 add_sysfs_error:
 	cleanup_sysfs(device);
 	return err;
@@ -588,8 +813,14 @@ add_sysfs_error:
 
 static int __exit hp_wmi_bios_remove(struct platform_device *device)
 {
+	int i;
 	cleanup_sysfs(device);
 
+	for (i = 0; i < rfkill2_count; i++) {
+		rfkill_unregister(rfkill2[i].rfkill);
+		rfkill_destroy(rfkill2[i].rfkill);
+	}
+
 	if (wifi_rfkill) {
 		rfkill_unregister(wifi_rfkill);
 		rfkill_destroy(wifi_rfkill);
@@ -622,6 +853,9 @@ static int hp_wmi_resume_handler(struct device *device)
 		input_sync(hp_wmi_input_dev);
 	}
 
+	if (rfkill2_count)
+		hp_wmi_rfkill2_refresh();
+
 	if (wifi_rfkill)
 		rfkill_set_states(wifi_rfkill,
 				  hp_wmi_get_sw_state(HPWMI_WIFI),
diff --git a/drivers/hwmon/hp_accel.c b/drivers/platform/x86/hp_accel.c
index 3d21fa2b97cd..16b233fac8f7 100644
--- a/drivers/hwmon/hp_accel.c
+++ b/drivers/platform/x86/hp_accel.c
@@ -35,11 +35,11 @@
 #include <linux/freezer.h>
 #include <linux/uaccess.h>
 #include <linux/leds.h>
+#include <linux/atomic.h>
 #include <acpi/acpi_drivers.h>
-#include <asm/atomic.h>
-#include "lis3lv02d.h"
+#include "../../misc/lis3lv02d/lis3lv02d.h"
 
-#define DRIVER_NAME     "lis3lv02d"
+#define DRIVER_NAME     "hp_accel"
 #define ACPI_MDPS_CLASS "accelerometer"
 
 /* Delayed LEDs infrastructure ------------------------------------ */
diff --git a/drivers/platform/x86/intel_mid_powerbtn.c b/drivers/platform/x86/intel_mid_powerbtn.c
new file mode 100644
index 000000000000..213e79ba68d5
--- /dev/null
+++ b/drivers/platform/x86/intel_mid_powerbtn.c
@@ -0,0 +1,148 @@
+/*
+ * Power button driver for Medfield.
+ *
+ * Copyright (C) 2010 Intel 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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <asm/intel_scu_ipc.h>
+
+#define DRIVER_NAME "msic_power_btn"
+
+#define MSIC_IRQ_STAT	0x02
+  #define MSIC_IRQ_PB	(1 << 0)
+#define MSIC_PB_CONFIG	0x3e
+#define MSIC_PB_STATUS	0x3f
+  #define MSIC_PB_LEVEL (1 << 3) /* 1 - release, 0 - press */
+
+struct mfld_pb_priv {
+	struct input_dev *input;
+	unsigned int irq;
+};
+
+static irqreturn_t mfld_pb_isr(int irq, void *dev_id)
+{
+	struct mfld_pb_priv *priv = dev_id;
+	int ret;
+	u8 pbstat;
+
+	ret = intel_scu_ipc_ioread8(MSIC_PB_STATUS, &pbstat);
+	if (ret < 0)
+		return IRQ_HANDLED;
+
+	input_event(priv->input, EV_KEY, KEY_POWER, !(pbstat & MSIC_PB_LEVEL));
+	input_sync(priv->input);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit mfld_pb_probe(struct platform_device *pdev)
+{
+	struct mfld_pb_priv *priv;
+	struct input_dev *input;
+	int irq;
+	int error;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return -EINVAL;
+
+	priv = kzalloc(sizeof(struct mfld_pb_priv), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!priv || !input) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	priv->input = input;
+	priv->irq = irq;
+
+	input->name = pdev->name;
+	input->phys = "power-button/input0";
+	input->id.bustype = BUS_HOST;
+	input->dev.parent = &pdev->dev;
+
+	input_set_capability(input, EV_KEY, KEY_POWER);
+
+	error = request_threaded_irq(priv->irq, NULL, mfld_pb_isr,
+				     0, DRIVER_NAME, priv);
+	if (error) {
+		dev_err(&pdev->dev,
+			"unable to request irq %d for mfld power button\n",
+			irq);
+		goto err_free_mem;
+	}
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(&pdev->dev,
+			"unable to register input dev, error %d\n", error);
+		goto err_free_irq;
+	}
+
+	platform_set_drvdata(pdev, priv);
+	return 0;
+
+err_free_irq:
+	free_irq(priv->irq, priv);
+err_free_mem:
+	input_free_device(input);
+	kfree(priv);
+	return error;
+}
+
+static int __devexit mfld_pb_remove(struct platform_device *pdev)
+{
+	struct mfld_pb_priv *priv = platform_get_drvdata(pdev);
+
+	free_irq(priv->irq, priv);
+	input_unregister_device(priv->input);
+	kfree(priv);
+
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+static struct platform_driver mfld_pb_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe	= mfld_pb_probe,
+	.remove	= __devexit_p(mfld_pb_remove),
+};
+
+static int __init mfld_pb_init(void)
+{
+	return platform_driver_register(&mfld_pb_driver);
+}
+module_init(mfld_pb_init);
+
+static void __exit mfld_pb_exit(void)
+{
+	platform_driver_unregister(&mfld_pb_driver);
+}
+module_exit(mfld_pb_exit);
+
+MODULE_AUTHOR("Hong Liu <hong.liu@intel.com>");
+MODULE_DESCRIPTION("Intel Medfield Power Button Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/platform/x86/intel_mid_thermal.c b/drivers/platform/x86/intel_mid_thermal.c
new file mode 100644
index 000000000000..1215c3096868
--- /dev/null
+++ b/drivers/platform/x86/intel_mid_thermal.c
@@ -0,0 +1,575 @@
+/*
+ * intel_mid_thermal.c - Intel MID platform thermal driver
+ *
+ * Copyright (C) 2011 Intel Corporation
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.        See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ * Author: Durgadoss R <durgadoss.r@intel.com>
+ */
+
+#define pr_fmt(fmt) "intel_mid_thermal: " fmt
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/param.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/pm.h>
+#include <linux/thermal.h>
+
+#include <asm/intel_scu_ipc.h>
+
+/* Number of thermal sensors */
+#define MSIC_THERMAL_SENSORS   4
+
+/* ADC1 - thermal registers */
+#define MSIC_THERM_ADC1CNTL1   0x1C0
+#define MSIC_ADC_ENBL          0x10
+#define MSIC_ADC_START         0x08
+
+#define MSIC_THERM_ADC1CNTL3   0x1C2
+#define MSIC_ADCTHERM_ENBL     0x04
+#define MSIC_ADCRRDATA_ENBL    0x05
+#define MSIC_CHANL_MASK_VAL    0x0F
+
+#define MSIC_STOPBIT_MASK      16
+#define MSIC_ADCTHERM_MASK     4
+#define ADC_CHANLS_MAX         15 /* Number of ADC channels */
+#define ADC_LOOP_MAX           (ADC_CHANLS_MAX - MSIC_THERMAL_SENSORS)
+
+/* ADC channel code values */
+#define SKIN_SENSOR0_CODE      0x08
+#define SKIN_SENSOR1_CODE      0x09
+#define SYS_SENSOR_CODE                0x0A
+#define MSIC_DIE_SENSOR_CODE   0x03
+
+#define SKIN_THERM_SENSOR0     0
+#define SKIN_THERM_SENSOR1     1
+#define SYS_THERM_SENSOR2      2
+#define MSIC_DIE_THERM_SENSOR3 3
+
+/* ADC code range */
+#define ADC_MAX                        977
+#define ADC_MIN                        162
+#define ADC_VAL0C              887
+#define ADC_VAL20C             720
+#define ADC_VAL40C             508
+#define ADC_VAL60C             315
+
+/* ADC base addresses */
+#define ADC_CHNL_START_ADDR    0x1C5   /* increments by 1 */
+#define ADC_DATA_START_ADDR     0x1D4   /* increments by 2 */
+
+/* MSIC die attributes */
+#define MSIC_DIE_ADC_MIN       488
+#define MSIC_DIE_ADC_MAX       1004
+
+/* This holds the address of the first free ADC channel,
+ * among the 15 channels
+ */
+static int channel_index;
+
+struct platform_info {
+       struct platform_device *pdev;
+       struct thermal_zone_device *tzd[MSIC_THERMAL_SENSORS];
+};
+
+struct thermal_device_info {
+       unsigned int chnl_addr;
+       int direct;
+       /* This holds the current temperature in millidegree celsius */
+       long curr_temp;
+};
+
+/**
+ * to_msic_die_temp - converts adc_val to msic_die temperature
+ * @adc_val: ADC value to be converted
+ *
+ * Can sleep
+ */
+static int to_msic_die_temp(uint16_t adc_val)
+{
+       return (368 * (adc_val) / 1000) - 220;
+}
+
+/**
+ * is_valid_adc - checks whether the adc code is within the defined range
+ * @min: minimum value for the sensor
+ * @max: maximum value for the sensor
+ *
+ * Can sleep
+ */
+static int is_valid_adc(uint16_t adc_val, uint16_t min, uint16_t max)
+{
+       return (adc_val >= min) && (adc_val <= max);
+}
+
+/**
+ * adc_to_temp - converts the ADC code to temperature in C
+ * @direct: true if ths channel is direct index
+ * @adc_val: the adc_val that needs to be converted
+ * @tp: temperature return value
+ *
+ * Linear approximation is used to covert the skin adc value into temperature.
+ * This technique is used to avoid very long look-up table to get
+ * the appropriate temp value from ADC value.
+ * The adc code vs sensor temp curve is split into five parts
+ * to achieve very close approximate temp value with less than
+ * 0.5C error
+ */
+static int adc_to_temp(int direct, uint16_t adc_val, unsigned long *tp)
+{
+       int temp;
+
+       /* Direct conversion for die temperature */
+       if (direct) {
+               if (is_valid_adc(adc_val, MSIC_DIE_ADC_MIN, MSIC_DIE_ADC_MAX)) {
+                       *tp = to_msic_die_temp(adc_val) * 1000;
+                       return 0;
+               }
+               return -ERANGE;
+       }
+
+       if (!is_valid_adc(adc_val, ADC_MIN, ADC_MAX))
+               return -ERANGE;
+
+       /* Linear approximation for skin temperature */
+       if (adc_val > ADC_VAL0C)
+               temp = 177 - (adc_val/5);
+       else if ((adc_val <= ADC_VAL0C) && (adc_val > ADC_VAL20C))
+               temp = 111 - (adc_val/8);
+       else if ((adc_val <= ADC_VAL20C) && (adc_val > ADC_VAL40C))
+               temp = 92 - (adc_val/10);
+       else if ((adc_val <= ADC_VAL40C) && (adc_val > ADC_VAL60C))
+               temp = 91 - (adc_val/10);
+       else
+               temp = 112 - (adc_val/6);
+
+       /* Convert temperature in celsius to milli degree celsius */
+       *tp = temp * 1000;
+       return 0;
+}
+
+/**
+ * mid_read_temp - read sensors for temperature
+ * @temp: holds the current temperature for the sensor after reading
+ *
+ * reads the adc_code from the channel and converts it to real
+ * temperature. The converted value is stored in temp.
+ *
+ * Can sleep
+ */
+static int mid_read_temp(struct thermal_zone_device *tzd, unsigned long *temp)
+{
+       struct thermal_device_info *td_info = tzd->devdata;
+       uint16_t adc_val, addr;
+       uint8_t data = 0;
+       int ret;
+       unsigned long curr_temp;
+
+
+       addr = td_info->chnl_addr;
+
+       /* Enable the msic for conversion before reading */
+       ret = intel_scu_ipc_iowrite8(MSIC_THERM_ADC1CNTL3, MSIC_ADCRRDATA_ENBL);
+       if (ret)
+               return ret;
+
+       /* Re-toggle the RRDATARD bit (temporary workaround) */
+       ret = intel_scu_ipc_iowrite8(MSIC_THERM_ADC1CNTL3, MSIC_ADCTHERM_ENBL);
+       if (ret)
+               return ret;
+
+       /* Read the higher bits of data */
+       ret = intel_scu_ipc_ioread8(addr, &data);
+       if (ret)
+               return ret;
+
+       /* Shift bits to accomodate the lower two data bits */
+       adc_val = (data << 2);
+       addr++;
+
+       ret = intel_scu_ipc_ioread8(addr, &data);/* Read lower bits */
+       if (ret)
+               return ret;
+
+       /* Adding lower two bits to the higher bits */
+       data &= 03;
+       adc_val += data;
+
+       /* Convert ADC value to temperature */
+       ret = adc_to_temp(td_info->direct, adc_val, &curr_temp);
+       if (ret == 0)
+               *temp = td_info->curr_temp = curr_temp;
+       return ret;
+}
+
+/**
+ * configure_adc - enables/disables the ADC for conversion
+ * @val: zero: disables the ADC non-zero:enables the ADC
+ *
+ * Enable/Disable the ADC depending on the argument
+ *
+ * Can sleep
+ */
+static int configure_adc(int val)
+{
+       int ret;
+       uint8_t data;
+
+       ret = intel_scu_ipc_ioread8(MSIC_THERM_ADC1CNTL1, &data);
+       if (ret)
+               return ret;
+
+       if (val) {
+               /* Enable and start the ADC */
+               data |= (MSIC_ADC_ENBL | MSIC_ADC_START);
+       } else {
+               /* Just stop the ADC */
+               data &= (~MSIC_ADC_START);
+       }
+
+       return intel_scu_ipc_iowrite8(MSIC_THERM_ADC1CNTL1, data);
+}
+
+/**
+ * set_up_therm_channel - enable thermal channel for conversion
+ * @base_addr: index of free msic ADC channel
+ *
+ * Enable all the three channels for conversion
+ *
+ * Can sleep
+ */
+static int set_up_therm_channel(u16 base_addr)
+{
+       int ret;
+
+       /* Enable all the sensor channels */
+       ret = intel_scu_ipc_iowrite8(base_addr, SKIN_SENSOR0_CODE);
+       if (ret)
+               return ret;
+
+       ret = intel_scu_ipc_iowrite8(base_addr + 1, SKIN_SENSOR1_CODE);
+       if (ret)
+               return ret;
+
+       ret = intel_scu_ipc_iowrite8(base_addr + 2, SYS_SENSOR_CODE);
+       if (ret)
+               return ret;
+
+       /* Since this is the last channel, set the stop bit
+          to 1 by ORing the DIE_SENSOR_CODE with 0x10 */
+       ret = intel_scu_ipc_iowrite8(base_addr + 3,
+                                       (MSIC_DIE_SENSOR_CODE | 0x10));
+       if (ret)
+               return ret;
+
+       /* Enable ADC and start it */
+       return configure_adc(1);
+}
+
+/**
+ * reset_stopbit - sets the stop bit to 0 on the given channel
+ * @addr: address of the channel
+ *
+ * Can sleep
+ */
+static int reset_stopbit(uint16_t addr)
+{
+       int ret;
+       uint8_t data;
+       ret = intel_scu_ipc_ioread8(addr, &data);
+       if (ret)
+               return ret;
+       /* Set the stop bit to zero */
+       return intel_scu_ipc_iowrite8(addr, (data & 0xEF));
+}
+
+/**
+ * find_free_channel - finds an empty channel for conversion
+ *
+ * If the ADC is not enabled then start using 0th channel
+ * itself. Otherwise find an empty channel by looking for a
+ * channel in which the stopbit is set to 1. returns the index
+ * of the first free channel if succeeds or an error code.
+ *
+ * Context: can sleep
+ *
+ * FIXME: Ultimately the channel allocator will move into the intel_scu_ipc
+ * code.
+ */
+static int find_free_channel(void)
+{
+       int ret;
+       int i;
+       uint8_t data;
+
+       /* check whether ADC is enabled */
+       ret = intel_scu_ipc_ioread8(MSIC_THERM_ADC1CNTL1, &data);
+       if (ret)
+               return ret;
+
+       if ((data & MSIC_ADC_ENBL) == 0)
+               return 0;
+
+       /* ADC is already enabled; Looking for an empty channel */
+       for (i = 0; i < ADC_CHANLS_MAX; i++) {
+               ret = intel_scu_ipc_ioread8(ADC_CHNL_START_ADDR + i, &data);
+               if (ret)
+                       return ret;
+
+               if (data & MSIC_STOPBIT_MASK) {
+                       ret = i;
+                       break;
+               }
+       }
+       return (ret > ADC_LOOP_MAX) ? (-EINVAL) : ret;
+}
+
+/**
+ * mid_initialize_adc - initializing the ADC
+ * @dev: our device structure
+ *
+ * Initialize the ADC for reading thermistor values. Can sleep.
+ */
+static int mid_initialize_adc(struct device *dev)
+{
+       u8  data;
+       u16 base_addr;
+       int ret;
+
+       /*
+        * Ensure that adctherm is disabled before we
+        * initialize the ADC
+        */
+       ret = intel_scu_ipc_ioread8(MSIC_THERM_ADC1CNTL3, &data);
+       if (ret)
+               return ret;
+
+       if (data & MSIC_ADCTHERM_MASK)
+               dev_warn(dev, "ADCTHERM already set");
+
+       /* Index of the first channel in which the stop bit is set */
+       channel_index = find_free_channel();
+       if (channel_index < 0) {
+               dev_err(dev, "No free ADC channels");
+               return channel_index;
+       }
+
+       base_addr = ADC_CHNL_START_ADDR + channel_index;
+
+       if (!(channel_index == 0 || channel_index == ADC_LOOP_MAX)) {
+               /* Reset stop bit for channels other than 0 and 12 */
+               ret = reset_stopbit(base_addr);
+               if (ret)
+                       return ret;
+
+               /* Index of the first free channel */
+               base_addr++;
+               channel_index++;
+       }
+
+       ret = set_up_therm_channel(base_addr);
+       if (ret) {
+               dev_err(dev, "unable to enable ADC");
+               return ret;
+       }
+       dev_dbg(dev, "ADC initialization successful");
+       return ret;
+}
+
+/**
+ * initialize_sensor - sets default temp and timer ranges
+ * @index: index of the sensor
+ *
+ * Context: can sleep
+ */
+static struct thermal_device_info *initialize_sensor(int index)
+{
+       struct thermal_device_info *td_info =
+               kzalloc(sizeof(struct thermal_device_info), GFP_KERNEL);
+
+       if (!td_info)
+               return NULL;
+
+       /* Set the base addr of the channel for this sensor */
+       td_info->chnl_addr = ADC_DATA_START_ADDR + 2 * (channel_index + index);
+       /* Sensor 3 is direct conversion */
+       if (index == 3)
+               td_info->direct = 1;
+       return td_info;
+}
+
+/**
+ * mid_thermal_resume - resume routine
+ * @pdev: platform device structure
+ *
+ * mid thermal resume: re-initializes the adc. Can sleep.
+ */
+static int mid_thermal_resume(struct platform_device *pdev)
+{
+       return mid_initialize_adc(&pdev->dev);
+}
+
+/**
+ * mid_thermal_suspend - suspend routine
+ * @pdev: platform device structure
+ *
+ * mid thermal suspend implements the suspend functionality
+ * by stopping the ADC. Can sleep.
+ */
+static int mid_thermal_suspend(struct platform_device *pdev, pm_message_t mesg)
+{
+       /*
+        * This just stops the ADC and does not disable it.
+        * temporary workaround until we have a generic ADC driver.
+        * If 0 is passed, it disables the ADC.
+        */
+       return configure_adc(0);
+}
+
+/**
+ * read_curr_temp - reads the current temperature and stores in temp
+ * @temp: holds the current temperature value after reading
+ *
+ * Can sleep
+ */
+static int read_curr_temp(struct thermal_zone_device *tzd, unsigned long *temp)
+{
+       WARN_ON(tzd == NULL);
+       return mid_read_temp(tzd, temp);
+}
+
+/* Can't be const */
+static struct thermal_zone_device_ops tzd_ops = {
+       .get_temp = read_curr_temp,
+};
+
+
+/**
+ * mid_thermal_probe - mfld thermal initialize
+ * @pdev: platform device structure
+ *
+ * mid thermal probe initializes the hardware and registers
+ * all the sensors with the generic thermal framework. Can sleep.
+ */
+static int mid_thermal_probe(struct platform_device *pdev)
+{
+       static char *name[MSIC_THERMAL_SENSORS] = {
+               "skin0", "skin1", "sys", "msicdie"
+       };
+
+       int ret;
+       int i;
+       struct platform_info *pinfo;
+
+       pinfo = kzalloc(sizeof(struct platform_info), GFP_KERNEL);
+       if (!pinfo)
+               return -ENOMEM;
+
+       /* Initializing the hardware */
+       ret = mid_initialize_adc(&pdev->dev);
+       if (ret) {
+               dev_err(&pdev->dev, "ADC init failed");
+               kfree(pinfo);
+               return ret;
+       }
+
+       /* Register each sensor with the generic thermal framework*/
+       for (i = 0; i < MSIC_THERMAL_SENSORS; i++) {
+               pinfo->tzd[i] = thermal_zone_device_register(name[i],
+                                       0, initialize_sensor(i),
+                                       &tzd_ops, 0, 0, 0, 0);
+               if (IS_ERR(pinfo->tzd[i]))
+                       goto reg_fail;
+       }
+
+       pinfo->pdev = pdev;
+       platform_set_drvdata(pdev, pinfo);
+       return 0;
+
+reg_fail:
+       ret = PTR_ERR(pinfo->tzd[i]);
+       while (--i >= 0)
+               thermal_zone_device_unregister(pinfo->tzd[i]);
+       configure_adc(0);
+       kfree(pinfo);
+       return ret;
+}
+
+/**
+ * mid_thermal_remove - mfld thermal finalize
+ * @dev: platform device structure
+ *
+ * MLFD thermal remove unregisters all the sensors from the generic
+ * thermal framework. Can sleep.
+ */
+static int mid_thermal_remove(struct platform_device *pdev)
+{
+       int i;
+       struct platform_info *pinfo = platform_get_drvdata(pdev);
+
+       for (i = 0; i < MSIC_THERMAL_SENSORS; i++)
+               thermal_zone_device_unregister(pinfo->tzd[i]);
+
+       platform_set_drvdata(pdev, NULL);
+
+       /* Stop the ADC */
+       return configure_adc(0);
+}
+
+/*********************************************************************
+ *             Driver initialisation and finalization
+ *********************************************************************/
+
+#define DRIVER_NAME "msic_sensor"
+
+static const struct platform_device_id therm_id_table[] = {
+       { DRIVER_NAME, 1 },
+};
+
+static struct platform_driver mid_thermal_driver = {
+       .driver = {
+               .name = DRIVER_NAME,
+               .owner = THIS_MODULE,
+       },
+       .probe = mid_thermal_probe,
+       .suspend = mid_thermal_suspend,
+       .resume = mid_thermal_resume,
+       .remove = __devexit_p(mid_thermal_remove),
+       .id_table = therm_id_table,
+};
+
+static int __init mid_thermal_module_init(void)
+{
+       return platform_driver_register(&mid_thermal_driver);
+}
+
+static void __exit mid_thermal_module_exit(void)
+{
+       platform_driver_unregister(&mid_thermal_driver);
+}
+
+module_init(mid_thermal_module_init);
+module_exit(mid_thermal_module_exit);
+
+MODULE_AUTHOR("Durgadoss R <durgadoss.r@intel.com>");
+MODULE_DESCRIPTION("Intel Medfield Platform Thermal Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/sony-laptop.c b/drivers/platform/x86/sony-laptop.c
index 5e83370b0812..a0ba0e795b67 100644
--- a/drivers/platform/x86/sony-laptop.c
+++ b/drivers/platform/x86/sony-laptop.c
@@ -71,8 +71,9 @@
 #endif
 
 #define DRV_PFX			"sony-laptop: "
-#define dprintk(msg...)		do {			\
-	if (debug) printk(KERN_WARNING DRV_PFX  msg);	\
+#define dprintk(msg...)		do {	\
+	if (debug)			\
+		pr_warn(DRV_PFX msg);	\
 } while (0)
 
 #define SONY_LAPTOP_DRIVER_VERSION	"0.6"
@@ -124,6 +125,19 @@ MODULE_PARM_DESC(minor,
 		 "default is -1 (automatic)");
 #endif
 
+static int kbd_backlight;	/* = 1 */
+module_param(kbd_backlight, int, 0444);
+MODULE_PARM_DESC(kbd_backlight,
+		 "set this to 0 to disable keyboard backlight, "
+		 "1 to enable it (default: 0)");
+
+static int kbd_backlight_timeout;	/* = 0 */
+module_param(kbd_backlight_timeout, int, 0444);
+MODULE_PARM_DESC(kbd_backlight_timeout,
+		 "set this to 0 to set the default 10 seconds timeout, "
+		 "1 for 30 seconds, 2 for 60 seconds and 3 to disable timeout "
+		 "(default: 0)");
+
 enum sony_nc_rfkill {
 	SONY_WIFI,
 	SONY_BLUETOOTH,
@@ -402,7 +416,7 @@ static int sony_laptop_setup_input(struct acpi_device *acpi_device)
 	error = kfifo_alloc(&sony_laptop_input.fifo,
 			    SONY_LAPTOP_BUF_SIZE, GFP_KERNEL);
 	if (error) {
-		printk(KERN_ERR DRV_PFX "kfifo_alloc failed\n");
+		pr_err(DRV_PFX "kfifo_alloc failed\n");
 		goto err_dec_users;
 	}
 
@@ -686,7 +700,7 @@ static int acpi_callgetfunc(acpi_handle handle, char *name, int *result)
 		return 0;
 	}
 
-	printk(KERN_WARNING DRV_PFX "acpi_callreadfunc failed\n");
+	pr_warn(DRV_PFX "acpi_callreadfunc failed\n");
 
 	return -1;
 }
@@ -712,7 +726,7 @@ static int acpi_callsetfunc(acpi_handle handle, char *name, int value,
 	if (status == AE_OK) {
 		if (result != NULL) {
 			if (out_obj.type != ACPI_TYPE_INTEGER) {
-				printk(KERN_WARNING DRV_PFX "acpi_evaluate_object bad "
+				pr_warn(DRV_PFX "acpi_evaluate_object bad "
 				       "return type\n");
 				return -1;
 			}
@@ -721,34 +735,101 @@ static int acpi_callsetfunc(acpi_handle handle, char *name, int value,
 		return 0;
 	}
 
-	printk(KERN_WARNING DRV_PFX "acpi_evaluate_object failed\n");
+	pr_warn(DRV_PFX "acpi_evaluate_object failed\n");
 
 	return -1;
 }
 
-static int sony_find_snc_handle(int handle)
+struct sony_nc_handles {
+	u16 cap[0x10];
+	struct device_attribute devattr;
+};
+
+struct sony_nc_handles *handles;
+
+static ssize_t sony_nc_handles_show(struct device *dev,
+		struct device_attribute *attr, char *buffer)
+{
+	ssize_t len = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(handles->cap); i++) {
+		len += snprintf(buffer + len, PAGE_SIZE - len, "0x%.4x ",
+				handles->cap[i]);
+	}
+	len += snprintf(buffer + len, PAGE_SIZE - len, "\n");
+
+	return len;
+}
+
+static int sony_nc_handles_setup(struct platform_device *pd)
 {
 	int i;
 	int result;
 
-	for (i = 0x20; i < 0x30; i++) {
-		acpi_callsetfunc(sony_nc_acpi_handle, "SN00", i, &result);
-		if (result == handle)
-			return i-0x20;
+	handles = kzalloc(sizeof(*handles), GFP_KERNEL);
+
+	sysfs_attr_init(&handles->devattr.attr);
+	handles->devattr.attr.name = "handles";
+	handles->devattr.attr.mode = S_IRUGO;
+	handles->devattr.show = sony_nc_handles_show;
+
+	for (i = 0; i < ARRAY_SIZE(handles->cap); i++) {
+		if (!acpi_callsetfunc(sony_nc_acpi_handle,
+					"SN00", i + 0x20, &result)) {
+			dprintk("caching handle 0x%.4x (offset: 0x%.2x)\n",
+					result, i);
+			handles->cap[i] = result;
+		}
+	}
+
+	/* allow reading capabilities via sysfs */
+	if (device_create_file(&pd->dev, &handles->devattr)) {
+		kfree(handles);
+		handles = NULL;
+		return -1;
 	}
 
+	return 0;
+}
+
+static int sony_nc_handles_cleanup(struct platform_device *pd)
+{
+	if (handles) {
+		device_remove_file(&pd->dev, &handles->devattr);
+		kfree(handles);
+		handles = NULL;
+	}
+	return 0;
+}
+
+static int sony_find_snc_handle(int handle)
+{
+	int i;
+	for (i = 0; i < 0x10; i++) {
+		if (handles->cap[i] == handle) {
+			dprintk("found handle 0x%.4x (offset: 0x%.2x)\n",
+					handle, i);
+			return i;
+		}
+	}
+	dprintk("handle 0x%.4x not found\n", handle);
 	return -1;
 }
 
 static int sony_call_snc_handle(int handle, int argument, int *result)
 {
+	int ret = 0;
 	int offset = sony_find_snc_handle(handle);
 
 	if (offset < 0)
 		return -1;
 
-	return acpi_callsetfunc(sony_nc_acpi_handle, "SN07", offset | argument,
-				result);
+	ret = acpi_callsetfunc(sony_nc_acpi_handle, "SN07", offset | argument,
+			result);
+	dprintk("called SN07 with 0x%.4x (result: 0x%.4x)\n", offset | argument,
+			*result);
+	return ret;
 }
 
 /*
@@ -857,11 +938,39 @@ static int sony_backlight_get_brightness(struct backlight_device *bd)
 	return value - 1;
 }
 
-static struct backlight_device *sony_backlight_device;
+static int sony_nc_get_brightness_ng(struct backlight_device *bd)
+{
+	int result;
+	int *handle = (int *)bl_get_data(bd);
+
+	sony_call_snc_handle(*handle, 0x0200, &result);
+
+	return result & 0xff;
+}
+
+static int sony_nc_update_status_ng(struct backlight_device *bd)
+{
+	int value, result;
+	int *handle = (int *)bl_get_data(bd);
+
+	value = bd->props.brightness;
+	sony_call_snc_handle(*handle, 0x0100 | (value << 16), &result);
+
+	return sony_nc_get_brightness_ng(bd);
+}
+
 static const struct backlight_ops sony_backlight_ops = {
+	.options = BL_CORE_SUSPENDRESUME,
 	.update_status = sony_backlight_update_status,
 	.get_brightness = sony_backlight_get_brightness,
 };
+static const struct backlight_ops sony_backlight_ng_ops = {
+	.options = BL_CORE_SUSPENDRESUME,
+	.update_status = sony_nc_update_status_ng,
+	.get_brightness = sony_nc_get_brightness_ng,
+};
+static int backlight_ng_handle;
+static struct backlight_device *sony_backlight_device;
 
 /*
  * New SNC-only Vaios event mapping to driver known keys
@@ -972,7 +1081,7 @@ static void sony_nc_notify(struct acpi_device *device, u32 event)
 				}
 
 				if (!key_event->data)
-					printk(KERN_INFO DRV_PFX
+					pr_info(DRV_PFX
 							"Unknown event: 0x%x 0x%x\n",
 							key_handle,
 							ev);
@@ -996,7 +1105,7 @@ static acpi_status sony_walk_callback(acpi_handle handle, u32 level,
 	struct acpi_device_info *info;
 
 	if (ACPI_SUCCESS(acpi_get_object_info(handle, &info))) {
-		printk(KERN_WARNING DRV_PFX "method: name: %4.4s, args %X\n",
+		pr_warn(DRV_PFX "method: name: %4.4s, args %X\n",
 			(char *)&info->name, info->param_count);
 
 		kfree(info);
@@ -1037,7 +1146,7 @@ static int sony_nc_resume(struct acpi_device *device)
 		ret = acpi_callsetfunc(sony_nc_acpi_handle, *item->acpiset,
 				       item->value, NULL);
 		if (ret < 0) {
-			printk("%s: %d\n", __func__, ret);
+			pr_err(DRV_PFX "%s: %d\n", __func__, ret);
 			break;
 		}
 	}
@@ -1054,11 +1163,6 @@ static int sony_nc_resume(struct acpi_device *device)
 		sony_nc_function_setup(device);
 	}
 
-	/* set the last requested brightness level */
-	if (sony_backlight_device &&
-			sony_backlight_update_status(sony_backlight_device) < 0)
-		printk(KERN_WARNING DRV_PFX "unable to restore brightness level\n");
-
 	/* re-read rfkill state */
 	sony_nc_rfkill_update();
 
@@ -1205,13 +1309,9 @@ static void sony_nc_rfkill_setup(struct acpi_device *device)
 	}
 
 	device_enum = (union acpi_object *) buffer.pointer;
-	if (!device_enum) {
-		pr_err("Invalid SN06 return object\n");
-		goto out_no_enum;
-	}
-	if (device_enum->type != ACPI_TYPE_BUFFER) {
-		pr_err("Invalid SN06 return object type 0x%.2x\n",
-		       device_enum->type);
+	if (!device_enum || device_enum->type != ACPI_TYPE_BUFFER) {
+		pr_err(DRV_PFX "Invalid SN06 return object 0x%.2x\n",
+				device_enum->type);
 		goto out_no_enum;
 	}
 
@@ -1245,6 +1345,206 @@ out_no_enum:
 	return;
 }
 
+/* Keyboard backlight feature */
+#define KBDBL_HANDLER	0x137
+#define KBDBL_PRESENT	0xB00
+#define	SET_MODE	0xC00
+#define SET_TIMEOUT	0xE00
+
+struct kbd_backlight {
+	int mode;
+	int timeout;
+	struct device_attribute mode_attr;
+	struct device_attribute timeout_attr;
+};
+
+struct kbd_backlight *kbdbl_handle;
+
+static ssize_t __sony_nc_kbd_backlight_mode_set(u8 value)
+{
+	int result;
+
+	if (value > 1)
+		return -EINVAL;
+
+	if (sony_call_snc_handle(KBDBL_HANDLER,
+				(value << 0x10) | SET_MODE, &result))
+		return -EIO;
+
+	kbdbl_handle->mode = value;
+
+	return 0;
+}
+
+static ssize_t sony_nc_kbd_backlight_mode_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buffer, size_t count)
+{
+	int ret = 0;
+	unsigned long value;
+
+	if (count > 31)
+		return -EINVAL;
+
+	if (strict_strtoul(buffer, 10, &value))
+		return -EINVAL;
+
+	ret = __sony_nc_kbd_backlight_mode_set(value);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static ssize_t sony_nc_kbd_backlight_mode_show(struct device *dev,
+		struct device_attribute *attr, char *buffer)
+{
+	ssize_t count = 0;
+	count = snprintf(buffer, PAGE_SIZE, "%d\n", kbdbl_handle->mode);
+	return count;
+}
+
+static int __sony_nc_kbd_backlight_timeout_set(u8 value)
+{
+	int result;
+
+	if (value > 3)
+		return -EINVAL;
+
+	if (sony_call_snc_handle(KBDBL_HANDLER,
+				(value << 0x10) | SET_TIMEOUT, &result))
+		return -EIO;
+
+	kbdbl_handle->timeout = value;
+
+	return 0;
+}
+
+static ssize_t sony_nc_kbd_backlight_timeout_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buffer, size_t count)
+{
+	int ret = 0;
+	unsigned long value;
+
+	if (count > 31)
+		return -EINVAL;
+
+	if (strict_strtoul(buffer, 10, &value))
+		return -EINVAL;
+
+	ret = __sony_nc_kbd_backlight_timeout_set(value);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static ssize_t sony_nc_kbd_backlight_timeout_show(struct device *dev,
+		struct device_attribute *attr, char *buffer)
+{
+	ssize_t count = 0;
+	count = snprintf(buffer, PAGE_SIZE, "%d\n", kbdbl_handle->timeout);
+	return count;
+}
+
+static int sony_nc_kbd_backlight_setup(struct platform_device *pd)
+{
+	int result;
+
+	if (sony_call_snc_handle(0x137, KBDBL_PRESENT, &result))
+		return 0;
+	if (!(result & 0x02))
+		return 0;
+
+	kbdbl_handle = kzalloc(sizeof(*kbdbl_handle), GFP_KERNEL);
+
+	sysfs_attr_init(&kbdbl_handle->mode_attr.attr);
+	kbdbl_handle->mode_attr.attr.name = "kbd_backlight";
+	kbdbl_handle->mode_attr.attr.mode = S_IRUGO | S_IWUSR;
+	kbdbl_handle->mode_attr.show = sony_nc_kbd_backlight_mode_show;
+	kbdbl_handle->mode_attr.store = sony_nc_kbd_backlight_mode_store;
+
+	sysfs_attr_init(&kbdbl_handle->timeout_attr.attr);
+	kbdbl_handle->timeout_attr.attr.name = "kbd_backlight_timeout";
+	kbdbl_handle->timeout_attr.attr.mode = S_IRUGO | S_IWUSR;
+	kbdbl_handle->timeout_attr.show = sony_nc_kbd_backlight_timeout_show;
+	kbdbl_handle->timeout_attr.store = sony_nc_kbd_backlight_timeout_store;
+
+	if (device_create_file(&pd->dev, &kbdbl_handle->mode_attr))
+		goto outkzalloc;
+
+	if (device_create_file(&pd->dev, &kbdbl_handle->timeout_attr))
+		goto outmode;
+
+	__sony_nc_kbd_backlight_mode_set(kbd_backlight);
+	__sony_nc_kbd_backlight_timeout_set(kbd_backlight_timeout);
+
+	return 0;
+
+outmode:
+	device_remove_file(&pd->dev, &kbdbl_handle->mode_attr);
+outkzalloc:
+	kfree(kbdbl_handle);
+	kbdbl_handle = NULL;
+	return -1;
+}
+
+static int sony_nc_kbd_backlight_cleanup(struct platform_device *pd)
+{
+	if (kbdbl_handle) {
+		device_remove_file(&pd->dev, &kbdbl_handle->mode_attr);
+		device_remove_file(&pd->dev, &kbdbl_handle->timeout_attr);
+		kfree(kbdbl_handle);
+	}
+	return 0;
+}
+
+static void sony_nc_backlight_setup(void)
+{
+	acpi_handle unused;
+	int max_brightness = 0;
+	const struct backlight_ops *ops = NULL;
+	struct backlight_properties props;
+
+	if (sony_find_snc_handle(0x12f) != -1) {
+		backlight_ng_handle = 0x12f;
+		ops = &sony_backlight_ng_ops;
+		max_brightness = 0xff;
+
+	} else if (sony_find_snc_handle(0x137) != -1) {
+		backlight_ng_handle = 0x137;
+		ops = &sony_backlight_ng_ops;
+		max_brightness = 0xff;
+
+	} else if (ACPI_SUCCESS(acpi_get_handle(sony_nc_acpi_handle, "GBRT",
+						&unused))) {
+		ops = &sony_backlight_ops;
+		max_brightness = SONY_MAX_BRIGHTNESS - 1;
+
+	} else
+		return;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.max_brightness = max_brightness;
+	sony_backlight_device = backlight_device_register("sony", NULL,
+							  &backlight_ng_handle,
+							  ops, &props);
+
+	if (IS_ERR(sony_backlight_device)) {
+		pr_warning(DRV_PFX "unable to register backlight device\n");
+		sony_backlight_device = NULL;
+	} else
+		sony_backlight_device->props.brightness =
+		    ops->get_brightness(sony_backlight_device);
+}
+
+static void sony_nc_backlight_cleanup(void)
+{
+	if (sony_backlight_device)
+		backlight_device_unregister(sony_backlight_device);
+}
+
 static int sony_nc_add(struct acpi_device *device)
 {
 	acpi_status status;
@@ -1252,8 +1552,8 @@ static int sony_nc_add(struct acpi_device *device)
 	acpi_handle handle;
 	struct sony_nc_value *item;
 
-	printk(KERN_INFO DRV_PFX "%s v%s.\n",
-		SONY_NC_DRIVER_NAME, SONY_LAPTOP_DRIVER_VERSION);
+	pr_info(DRV_PFX "%s v%s.\n", SONY_NC_DRIVER_NAME,
+			SONY_LAPTOP_DRIVER_VERSION);
 
 	sony_nc_acpi_device = device;
 	strcpy(acpi_device_class(device), "sony/hotkey");
@@ -1269,13 +1569,18 @@ static int sony_nc_add(struct acpi_device *device)
 		goto outwalk;
 	}
 
+	result = sony_pf_add();
+	if (result)
+		goto outpresent;
+
 	if (debug) {
-		status = acpi_walk_namespace(ACPI_TYPE_METHOD, sony_nc_acpi_handle,
-					     1, sony_walk_callback, NULL, NULL, NULL);
+		status = acpi_walk_namespace(ACPI_TYPE_METHOD,
+				sony_nc_acpi_handle, 1, sony_walk_callback,
+				NULL, NULL, NULL);
 		if (ACPI_FAILURE(status)) {
-			printk(KERN_WARNING DRV_PFX "unable to walk acpi resources\n");
+			pr_warn(DRV_PFX "unable to walk acpi resources\n");
 			result = -ENODEV;
-			goto outwalk;
+			goto outpresent;
 		}
 	}
 
@@ -1288,6 +1593,10 @@ static int sony_nc_add(struct acpi_device *device)
 	if (ACPI_SUCCESS(acpi_get_handle(sony_nc_acpi_handle, "SN00",
 					 &handle))) {
 		dprintk("Doing SNC setup\n");
+		if (sony_nc_handles_setup(sony_pf_device))
+			goto outpresent;
+		if (sony_nc_kbd_backlight_setup(sony_pf_device))
+			goto outsnc;
 		sony_nc_function_setup(device);
 		sony_nc_rfkill_setup(device);
 	}
@@ -1295,38 +1604,15 @@ static int sony_nc_add(struct acpi_device *device)
 	/* setup input devices and helper fifo */
 	result = sony_laptop_setup_input(device);
 	if (result) {
-		printk(KERN_ERR DRV_PFX
-				"Unable to create input devices.\n");
-		goto outwalk;
+		pr_err(DRV_PFX "Unable to create input devices.\n");
+		goto outkbdbacklight;
 	}
 
 	if (acpi_video_backlight_support()) {
-		printk(KERN_INFO DRV_PFX "brightness ignored, must be "
+		pr_info(DRV_PFX "brightness ignored, must be "
 		       "controlled by ACPI video driver\n");
-	} else if (ACPI_SUCCESS(acpi_get_handle(sony_nc_acpi_handle, "GBRT",
-						&handle))) {
-							struct backlight_properties props;
-		memset(&props, 0, sizeof(struct backlight_properties));
-		props.max_brightness = SONY_MAX_BRIGHTNESS - 1;
-		sony_backlight_device = backlight_device_register("sony", NULL,
-								  NULL,
-								  &sony_backlight_ops,
-								  &props);
-
-		if (IS_ERR(sony_backlight_device)) {
-			printk(KERN_WARNING DRV_PFX "unable to register backlight device\n");
-			sony_backlight_device = NULL;
-		} else {
-			sony_backlight_device->props.brightness =
-			    sony_backlight_get_brightness
-			    (sony_backlight_device);
-		}
-
-	}
-
-	result = sony_pf_add();
-	if (result)
-		goto outbacklight;
+	} else
+		sony_nc_backlight_setup();
 
 	/* create sony_pf sysfs attributes related to the SNC device */
 	for (item = sony_nc_values; item->name; ++item) {
@@ -1373,14 +1659,19 @@ static int sony_nc_add(struct acpi_device *device)
 	for (item = sony_nc_values; item->name; ++item) {
 		device_remove_file(&sony_pf_device->dev, &item->devattr);
 	}
-	sony_pf_remove();
-
-      outbacklight:
-	if (sony_backlight_device)
-		backlight_device_unregister(sony_backlight_device);
+	sony_nc_backlight_cleanup();
 
 	sony_laptop_remove_input();
 
+      outkbdbacklight:
+	sony_nc_kbd_backlight_cleanup(sony_pf_device);
+
+      outsnc:
+	sony_nc_handles_cleanup(sony_pf_device);
+
+      outpresent:
+	sony_pf_remove();
+
       outwalk:
 	sony_nc_rfkill_cleanup();
 	return result;
@@ -1390,8 +1681,7 @@ static int sony_nc_remove(struct acpi_device *device, int type)
 {
 	struct sony_nc_value *item;
 
-	if (sony_backlight_device)
-		backlight_device_unregister(sony_backlight_device);
+	sony_nc_backlight_cleanup();
 
 	sony_nc_acpi_device = NULL;
 
@@ -1399,6 +1689,8 @@ static int sony_nc_remove(struct acpi_device *device, int type)
 		device_remove_file(&sony_pf_device->dev, &item->devattr);
 	}
 
+	sony_nc_kbd_backlight_cleanup(sony_pf_device);
+	sony_nc_handles_cleanup(sony_pf_device);
 	sony_pf_remove();
 	sony_laptop_remove_input();
 	sony_nc_rfkill_cleanup();
@@ -1437,7 +1729,6 @@ static struct acpi_driver sony_nc_driver = {
 #define SONYPI_DEVICE_TYPE1	0x00000001
 #define SONYPI_DEVICE_TYPE2	0x00000002
 #define SONYPI_DEVICE_TYPE3	0x00000004
-#define SONYPI_DEVICE_TYPE4	0x00000008
 
 #define SONYPI_TYPE1_OFFSET	0x04
 #define SONYPI_TYPE2_OFFSET	0x12
@@ -1583,8 +1874,8 @@ static struct sonypi_event sonypi_blueev[] = {
 
 /* The set of possible wireless events */
 static struct sonypi_event sonypi_wlessev[] = {
-	{ 0x59, SONYPI_EVENT_WIRELESS_ON },
-	{ 0x5a, SONYPI_EVENT_WIRELESS_OFF },
+	{ 0x59, SONYPI_EVENT_IGNORE },
+	{ 0x5a, SONYPI_EVENT_IGNORE },
 	{ 0, 0 }
 };
 
@@ -1841,7 +2132,7 @@ out:
 	if (pcidev)
 		pci_dev_put(pcidev);
 
-	printk(KERN_INFO DRV_PFX "detected Type%d model\n",
+	pr_info(DRV_PFX "detected Type%d model\n",
 			dev->model == SONYPI_DEVICE_TYPE1 ? 1 :
 			dev->model == SONYPI_DEVICE_TYPE2 ? 2 : 3);
 }
@@ -1889,7 +2180,7 @@ static int __sony_pic_camera_ready(void)
 static int __sony_pic_camera_off(void)
 {
 	if (!camera) {
-		printk(KERN_WARNING DRV_PFX "camera control not enabled\n");
+		pr_warn(DRV_PFX "camera control not enabled\n");
 		return -ENODEV;
 	}
 
@@ -1909,7 +2200,7 @@ static int __sony_pic_camera_on(void)
 	int i, j, x;
 
 	if (!camera) {
-		printk(KERN_WARNING DRV_PFX "camera control not enabled\n");
+		pr_warn(DRV_PFX "camera control not enabled\n");
 		return -ENODEV;
 	}
 
@@ -1932,7 +2223,7 @@ static int __sony_pic_camera_on(void)
 	}
 
 	if (j == 0) {
-		printk(KERN_WARNING DRV_PFX "failed to power on camera\n");
+		pr_warn(DRV_PFX "failed to power on camera\n");
 		return -ENODEV;
 	}
 
@@ -1988,7 +2279,7 @@ int sony_pic_camera_command(int command, u8 value)
 				ITERATIONS_SHORT);
 		break;
 	default:
-		printk(KERN_ERR DRV_PFX "sony_pic_camera_command invalid: %d\n",
+		pr_err(DRV_PFX "sony_pic_camera_command invalid: %d\n",
 		       command);
 		break;
 	}
@@ -2395,7 +2686,7 @@ static int sonypi_compat_init(void)
 	error =
 	 kfifo_alloc(&sonypi_compat.fifo, SONY_LAPTOP_BUF_SIZE, GFP_KERNEL);
 	if (error) {
-		printk(KERN_ERR DRV_PFX "kfifo_alloc failed\n");
+		pr_err(DRV_PFX "kfifo_alloc failed\n");
 		return error;
 	}
 
@@ -2405,11 +2696,11 @@ static int sonypi_compat_init(void)
 		sonypi_misc_device.minor = minor;
 	error = misc_register(&sonypi_misc_device);
 	if (error) {
-		printk(KERN_ERR DRV_PFX "misc_register failed\n");
+		pr_err(DRV_PFX "misc_register failed\n");
 		goto err_free_kfifo;
 	}
 	if (minor == -1)
-		printk(KERN_INFO DRV_PFX "device allocated minor is %d\n",
+		pr_info(DRV_PFX "device allocated minor is %d\n",
 		       sonypi_misc_device.minor);
 
 	return 0;
@@ -2469,8 +2760,7 @@ sony_pic_read_possible_resource(struct acpi_resource *resource, void *context)
 			}
 			for (i = 0; i < p->interrupt_count; i++) {
 				if (!p->interrupts[i]) {
-					printk(KERN_WARNING DRV_PFX
-							"Invalid IRQ %d\n",
+					pr_warn(DRV_PFX "Invalid IRQ %d\n",
 							p->interrupts[i]);
 					continue;
 				}
@@ -2509,7 +2799,7 @@ sony_pic_read_possible_resource(struct acpi_resource *resource, void *context)
 						ioport->io2.address_length);
 			}
 			else {
-				printk(KERN_ERR DRV_PFX "Unknown SPIC Type, more than 2 IO Ports\n");
+				pr_err(DRV_PFX "Unknown SPIC Type, more than 2 IO Ports\n");
 				return AE_ERROR;
 			}
 			return AE_OK;
@@ -2537,7 +2827,7 @@ static int sony_pic_possible_resources(struct acpi_device *device)
 	dprintk("Evaluating _STA\n");
 	result = acpi_bus_get_status(device);
 	if (result) {
-		printk(KERN_WARNING DRV_PFX "Unable to read status\n");
+		pr_warn(DRV_PFX "Unable to read status\n");
 		goto end;
 	}
 
@@ -2553,8 +2843,7 @@ static int sony_pic_possible_resources(struct acpi_device *device)
 	status = acpi_walk_resources(device->handle, METHOD_NAME__PRS,
 			sony_pic_read_possible_resource, &spic_dev);
 	if (ACPI_FAILURE(status)) {
-		printk(KERN_WARNING DRV_PFX
-				"Failure evaluating %s\n",
+		pr_warn(DRV_PFX "Failure evaluating %s\n",
 				METHOD_NAME__PRS);
 		result = -ENODEV;
 	}
@@ -2668,7 +2957,7 @@ static int sony_pic_enable(struct acpi_device *device,
 
 	/* check for total failure */
 	if (ACPI_FAILURE(status)) {
-		printk(KERN_ERR DRV_PFX "Error evaluating _SRS\n");
+		pr_err(DRV_PFX "Error evaluating _SRS\n");
 		result = -ENODEV;
 		goto end;
 	}
@@ -2724,6 +3013,9 @@ static irqreturn_t sony_pic_irq(int irq, void *dev_id)
 			if (ev == dev->event_types[i].events[j].data) {
 				device_event =
 					dev->event_types[i].events[j].event;
+				/* some events may require ignoring */
+				if (!device_event)
+					return IRQ_HANDLED;
 				goto found;
 			}
 		}
@@ -2743,7 +3035,6 @@ found:
 	sony_laptop_report_input_event(device_event);
 	acpi_bus_generate_proc_event(dev->acpi_dev, 1, device_event);
 	sonypi_compat_report_event(device_event);
-
 	return IRQ_HANDLED;
 }
 
@@ -2758,7 +3049,7 @@ static int sony_pic_remove(struct acpi_device *device, int type)
 	struct sony_pic_irq *irq, *tmp_irq;
 
 	if (sony_pic_disable(device)) {
-		printk(KERN_ERR DRV_PFX "Couldn't disable device.\n");
+		pr_err(DRV_PFX "Couldn't disable device.\n");
 		return -ENXIO;
 	}
 
@@ -2798,8 +3089,8 @@ static int sony_pic_add(struct acpi_device *device)
 	struct sony_pic_ioport *io, *tmp_io;
 	struct sony_pic_irq *irq, *tmp_irq;
 
-	printk(KERN_INFO DRV_PFX "%s v%s.\n",
-		SONY_PIC_DRIVER_NAME, SONY_LAPTOP_DRIVER_VERSION);
+	pr_info(DRV_PFX "%s v%s.\n", SONY_PIC_DRIVER_NAME,
+			SONY_LAPTOP_DRIVER_VERSION);
 
 	spic_dev.acpi_dev = device;
 	strcpy(acpi_device_class(device), "sony/hotkey");
@@ -2809,16 +3100,14 @@ static int sony_pic_add(struct acpi_device *device)
 	/* read _PRS resources */
 	result = sony_pic_possible_resources(device);
 	if (result) {
-		printk(KERN_ERR DRV_PFX
-				"Unable to read possible resources.\n");
+		pr_err(DRV_PFX "Unable to read possible resources.\n");
 		goto err_free_resources;
 	}
 
 	/* setup input devices and helper fifo */
 	result = sony_laptop_setup_input(device);
 	if (result) {
-		printk(KERN_ERR DRV_PFX
-				"Unable to create input devices.\n");
+		pr_err(DRV_PFX "Unable to create input devices.\n");
 		goto err_free_resources;
 	}
 
@@ -2859,7 +3148,7 @@ static int sony_pic_add(struct acpi_device *device)
 		}
 	}
 	if (!spic_dev.cur_ioport) {
-		printk(KERN_ERR DRV_PFX "Failed to request_region.\n");
+		pr_err(DRV_PFX "Failed to request_region.\n");
 		result = -ENODEV;
 		goto err_remove_compat;
 	}
@@ -2879,7 +3168,7 @@ static int sony_pic_add(struct acpi_device *device)
 		}
 	}
 	if (!spic_dev.cur_irq) {
-		printk(KERN_ERR DRV_PFX "Failed to request_irq.\n");
+		pr_err(DRV_PFX "Failed to request_irq.\n");
 		result = -ENODEV;
 		goto err_release_region;
 	}
@@ -2887,7 +3176,7 @@ static int sony_pic_add(struct acpi_device *device)
 	/* set resource status _SRS */
 	result = sony_pic_enable(device, spic_dev.cur_ioport, spic_dev.cur_irq);
 	if (result) {
-		printk(KERN_ERR DRV_PFX "Couldn't enable device.\n");
+		pr_err(DRV_PFX "Couldn't enable device.\n");
 		goto err_free_irq;
 	}
 
@@ -2996,8 +3285,7 @@ static int __init sony_laptop_init(void)
 	if (!no_spic && dmi_check_system(sonypi_dmi_table)) {
 		result = acpi_bus_register_driver(&sony_pic_driver);
 		if (result) {
-			printk(KERN_ERR DRV_PFX
-					"Unable to register SPIC driver.");
+			pr_err(DRV_PFX "Unable to register SPIC driver.");
 			goto out;
 		}
 		spic_drv_registered = 1;
@@ -3005,7 +3293,7 @@ static int __init sony_laptop_init(void)
 
 	result = acpi_bus_register_driver(&sony_nc_driver);
 	if (result) {
-		printk(KERN_ERR DRV_PFX "Unable to register SNC driver.");
+		pr_err(DRV_PFX "Unable to register SNC driver.");
 		goto out_unregister_pic;
 	}
 
diff --git a/drivers/platform/x86/xo15-ebook.c b/drivers/platform/x86/xo15-ebook.c
new file mode 100644
index 000000000000..c1372ed9d2e9
--- /dev/null
+++ b/drivers/platform/x86/xo15-ebook.c
@@ -0,0 +1,180 @@
+/*
+ *  OLPC XO-1.5 ebook switch driver
+ *  (based on generic ACPI button driver)
+ *
+ *  Copyright (C) 2009 Paul Fox <pgf@laptop.org>
+ *  Copyright (C) 2010 One Laptop per Child
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or (at
+ *  your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/input.h>
+#include <acpi/acpi_bus.h>
+#include <acpi/acpi_drivers.h>
+
+#define MODULE_NAME "xo15-ebook"
+#define PREFIX MODULE_NAME ": "
+
+#define XO15_EBOOK_CLASS		MODULE_NAME
+#define XO15_EBOOK_TYPE_UNKNOWN	0x00
+#define XO15_EBOOK_NOTIFY_STATUS	0x80
+
+#define XO15_EBOOK_SUBCLASS		"ebook"
+#define XO15_EBOOK_HID			"XO15EBK"
+#define XO15_EBOOK_DEVICE_NAME		"EBook Switch"
+
+ACPI_MODULE_NAME(MODULE_NAME);
+
+MODULE_DESCRIPTION("OLPC XO-1.5 ebook switch driver");
+MODULE_LICENSE("GPL");
+
+static const struct acpi_device_id ebook_device_ids[] = {
+	{ XO15_EBOOK_HID, 0 },
+	{ "", 0 },
+};
+MODULE_DEVICE_TABLE(acpi, ebook_device_ids);
+
+struct ebook_switch {
+	struct input_dev *input;
+	char phys[32];			/* for input device */
+};
+
+static int ebook_send_state(struct acpi_device *device)
+{
+	struct ebook_switch *button = acpi_driver_data(device);
+	unsigned long long state;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "EBK", NULL, &state);
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	/* input layer checks if event is redundant */
+	input_report_switch(button->input, SW_TABLET_MODE, !state);
+	input_sync(button->input);
+	return 0;
+}
+
+static void ebook_switch_notify(struct acpi_device *device, u32 event)
+{
+	switch (event) {
+	case ACPI_FIXED_HARDWARE_EVENT:
+	case XO15_EBOOK_NOTIFY_STATUS:
+		ebook_send_state(device);
+		break;
+	default:
+		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
+				  "Unsupported event [0x%x]\n", event));
+		break;
+	}
+}
+
+static int ebook_switch_resume(struct acpi_device *device)
+{
+	return ebook_send_state(device);
+}
+
+static int ebook_switch_add(struct acpi_device *device)
+{
+	struct ebook_switch *button;
+	struct input_dev *input;
+	const char *hid = acpi_device_hid(device);
+	char *name, *class;
+	int error;
+
+	button = kzalloc(sizeof(struct ebook_switch), GFP_KERNEL);
+	if (!button)
+		return -ENOMEM;
+
+	device->driver_data = button;
+
+	button->input = input = input_allocate_device();
+	if (!input) {
+		error = -ENOMEM;
+		goto err_free_button;
+	}
+
+	name = acpi_device_name(device);
+	class = acpi_device_class(device);
+
+	if (strcmp(hid, XO15_EBOOK_HID)) {
+		printk(KERN_ERR PREFIX "Unsupported hid [%s]\n", hid);
+		error = -ENODEV;
+		goto err_free_input;
+	}
+
+	strcpy(name, XO15_EBOOK_DEVICE_NAME);
+	sprintf(class, "%s/%s", XO15_EBOOK_CLASS, XO15_EBOOK_SUBCLASS);
+
+	snprintf(button->phys, sizeof(button->phys), "%s/button/input0", hid);
+
+	input->name = name;
+	input->phys = button->phys;
+	input->id.bustype = BUS_HOST;
+	input->dev.parent = &device->dev;
+
+	input->evbit[0] = BIT_MASK(EV_SW);
+	set_bit(SW_TABLET_MODE, input->swbit);
+
+	error = input_register_device(input);
+	if (error)
+		goto err_free_input;
+
+	ebook_send_state(device);
+
+	if (device->wakeup.flags.valid) {
+		/* Button's GPE is run-wake GPE */
+		acpi_enable_gpe(device->wakeup.gpe_device,
+				device->wakeup.gpe_number);
+		device_set_wakeup_enable(&device->dev, true);
+	}
+
+	return 0;
+
+ err_free_input:
+	input_free_device(input);
+ err_free_button:
+	kfree(button);
+	return error;
+}
+
+static int ebook_switch_remove(struct acpi_device *device, int type)
+{
+	struct ebook_switch *button = acpi_driver_data(device);
+
+	input_unregister_device(button->input);
+	kfree(button);
+	return 0;
+}
+
+static struct acpi_driver xo15_ebook_driver = {
+	.name = MODULE_NAME,
+	.class = XO15_EBOOK_CLASS,
+	.ids = ebook_device_ids,
+	.ops = {
+		.add = ebook_switch_add,
+		.resume = ebook_switch_resume,
+		.remove = ebook_switch_remove,
+		.notify = ebook_switch_notify,
+	},
+};
+
+static int __init xo15_ebook_init(void)
+{
+	return acpi_bus_register_driver(&xo15_ebook_driver);
+}
+
+static void __exit xo15_ebook_exit(void)
+{
+	acpi_bus_unregister_driver(&xo15_ebook_driver);
+}
+
+module_init(xo15_ebook_init);
+module_exit(xo15_ebook_exit);
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 61bf5d724139..52a462fc6b84 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -117,10 +117,24 @@ config BATTERY_BQ20Z75
 
 config BATTERY_BQ27x00
 	tristate "BQ27x00 battery driver"
+	help
+	  Say Y here to enable support for batteries with BQ27x00 (I2C/HDQ) chips.
+
+config BATTERY_BQ27X00_I2C
+	bool "BQ27200/BQ27500 support"
+	depends on BATTERY_BQ27x00
 	depends on I2C
+	default y
 	help
 	  Say Y here to enable support for batteries with BQ27x00 (I2C) chips.
 
+config BATTERY_BQ27X00_PLATFORM
+	bool "BQ27000 support"
+	depends on BATTERY_BQ27x00
+	default y
+	help
+	  Say Y here to enable support for batteries with BQ27000 (HDQ) chips.
+
 config BATTERY_DA9030
 	tristate "DA9030 battery driver"
 	depends on PMIC_DA903X
diff --git a/drivers/power/bq20z75.c b/drivers/power/bq20z75.c
index 492da27e1a47..8018e8cc7a74 100644
--- a/drivers/power/bq20z75.c
+++ b/drivers/power/bq20z75.c
@@ -25,6 +25,10 @@
 #include <linux/power_supply.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+
+#include <linux/power/bq20z75.h>
 
 enum {
 	REG_MANUFACTURER_DATA,
@@ -38,11 +42,22 @@ enum {
 	REG_CYCLE_COUNT,
 	REG_SERIAL_NUMBER,
 	REG_REMAINING_CAPACITY,
+	REG_REMAINING_CAPACITY_CHARGE,
 	REG_FULL_CHARGE_CAPACITY,
+	REG_FULL_CHARGE_CAPACITY_CHARGE,
 	REG_DESIGN_CAPACITY,
+	REG_DESIGN_CAPACITY_CHARGE,
 	REG_DESIGN_VOLTAGE,
 };
 
+/* Battery Mode defines */
+#define BATTERY_MODE_OFFSET		0x03
+#define BATTERY_MODE_MASK		0x8000
+enum bq20z75_battery_mode {
+	BATTERY_MODE_AMPS,
+	BATTERY_MODE_WATTS
+};
+
 /* manufacturer access defines */
 #define MANUFACTURER_ACCESS_STATUS	0x0006
 #define MANUFACTURER_ACCESS_SLEEP	0x0011
@@ -78,8 +93,12 @@ static const struct bq20z75_device_data {
 		BQ20Z75_DATA(POWER_SUPPLY_PROP_CAPACITY, 0x0E, 0, 100),
 	[REG_REMAINING_CAPACITY] =
 		BQ20Z75_DATA(POWER_SUPPLY_PROP_ENERGY_NOW, 0x0F, 0, 65535),
+	[REG_REMAINING_CAPACITY_CHARGE] =
+		BQ20Z75_DATA(POWER_SUPPLY_PROP_CHARGE_NOW, 0x0F, 0, 65535),
 	[REG_FULL_CHARGE_CAPACITY] =
 		BQ20Z75_DATA(POWER_SUPPLY_PROP_ENERGY_FULL, 0x10, 0, 65535),
+	[REG_FULL_CHARGE_CAPACITY_CHARGE] =
+		BQ20Z75_DATA(POWER_SUPPLY_PROP_CHARGE_FULL, 0x10, 0, 65535),
 	[REG_TIME_TO_EMPTY] =
 		BQ20Z75_DATA(POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG, 0x12, 0,
 			65535),
@@ -93,6 +112,9 @@ static const struct bq20z75_device_data {
 	[REG_DESIGN_CAPACITY] =
 		BQ20Z75_DATA(POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN, 0x18, 0,
 			65535),
+	[REG_DESIGN_CAPACITY_CHARGE] =
+		BQ20Z75_DATA(POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN, 0x18, 0,
+			65535),
 	[REG_DESIGN_VOLTAGE] =
 		BQ20Z75_DATA(POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN, 0x19, 0,
 			65535),
@@ -117,39 +139,72 @@ static enum power_supply_property bq20z75_properties[] = {
 	POWER_SUPPLY_PROP_ENERGY_NOW,
 	POWER_SUPPLY_PROP_ENERGY_FULL,
 	POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
 };
 
 struct bq20z75_info {
-	struct i2c_client	*client;
-	struct power_supply	power_supply;
+	struct i2c_client		*client;
+	struct power_supply		power_supply;
+	struct bq20z75_platform_data	*pdata;
+	bool				is_present;
+	bool				gpio_detect;
+	bool				enable_detection;
+	int				irq;
 };
 
 static int bq20z75_read_word_data(struct i2c_client *client, u8 address)
 {
-	s32 ret;
+	struct bq20z75_info *bq20z75_device = i2c_get_clientdata(client);
+	s32 ret = 0;
+	int retries = 1;
+
+	if (bq20z75_device->pdata)
+		retries = max(bq20z75_device->pdata->i2c_retry_count + 1, 1);
+
+	while (retries > 0) {
+		ret = i2c_smbus_read_word_data(client, address);
+		if (ret >= 0)
+			break;
+		retries--;
+	}
 
-	ret = i2c_smbus_read_word_data(client, address);
 	if (ret < 0) {
-		dev_err(&client->dev,
+		dev_warn(&client->dev,
 			"%s: i2c read at address 0x%x failed\n",
 			__func__, address);
 		return ret;
 	}
+
 	return le16_to_cpu(ret);
 }
 
 static int bq20z75_write_word_data(struct i2c_client *client, u8 address,
 	u16 value)
 {
-	s32 ret;
+	struct bq20z75_info *bq20z75_device = i2c_get_clientdata(client);
+	s32 ret = 0;
+	int retries = 1;
+
+	if (bq20z75_device->pdata)
+		retries = max(bq20z75_device->pdata->i2c_retry_count + 1, 1);
+
+	while (retries > 0) {
+		ret = i2c_smbus_write_word_data(client, address,
+			le16_to_cpu(value));
+		if (ret >= 0)
+			break;
+		retries--;
+	}
 
-	ret = i2c_smbus_write_word_data(client, address, le16_to_cpu(value));
 	if (ret < 0) {
-		dev_err(&client->dev,
+		dev_warn(&client->dev,
 			"%s: i2c write to address 0x%x failed\n",
 			__func__, address);
 		return ret;
 	}
+
 	return 0;
 }
 
@@ -158,6 +213,18 @@ static int bq20z75_get_battery_presence_and_health(
 	union power_supply_propval *val)
 {
 	s32 ret;
+	struct bq20z75_info *bq20z75_device = i2c_get_clientdata(client);
+
+	if (psp == POWER_SUPPLY_PROP_PRESENT &&
+		bq20z75_device->gpio_detect) {
+		ret = gpio_get_value(
+			bq20z75_device->pdata->battery_detect);
+		if (ret == bq20z75_device->pdata->battery_detect_present)
+			val->intval = 1;
+		else
+			val->intval = 0;
+		return ret;
+	}
 
 	/* Write to ManufacturerAccess with
 	 * ManufacturerAccess command and then
@@ -171,8 +238,11 @@ static int bq20z75_get_battery_presence_and_health(
 
 	ret = bq20z75_read_word_data(client,
 		bq20z75_data[REG_MANUFACTURER_DATA].addr);
-	if (ret < 0)
+	if (ret < 0) {
+		if (psp == POWER_SUPPLY_PROP_PRESENT)
+			val->intval = 0; /* battery removed */
 		return ret;
+	}
 
 	if (ret < bq20z75_data[REG_MANUFACTURER_DATA].min_value ||
 	    ret > bq20z75_data[REG_MANUFACTURER_DATA].max_value) {
@@ -260,6 +330,9 @@ static void  bq20z75_unit_adjustment(struct i2c_client *client,
 	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
 	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
 		val->intval *= BASE_UNIT_CONVERSION;
 		break;
 
@@ -281,11 +354,44 @@ static void  bq20z75_unit_adjustment(struct i2c_client *client,
 	}
 }
 
+static enum bq20z75_battery_mode
+bq20z75_set_battery_mode(struct i2c_client *client,
+	enum bq20z75_battery_mode mode)
+{
+	int ret, original_val;
+
+	original_val = bq20z75_read_word_data(client, BATTERY_MODE_OFFSET);
+	if (original_val < 0)
+		return original_val;
+
+	if ((original_val & BATTERY_MODE_MASK) == mode)
+		return mode;
+
+	if (mode == BATTERY_MODE_AMPS)
+		ret = original_val & ~BATTERY_MODE_MASK;
+	else
+		ret = original_val | BATTERY_MODE_MASK;
+
+	ret = bq20z75_write_word_data(client, BATTERY_MODE_OFFSET, ret);
+	if (ret < 0)
+		return ret;
+
+	return original_val & BATTERY_MODE_MASK;
+}
+
 static int bq20z75_get_battery_capacity(struct i2c_client *client,
 	int reg_offset, enum power_supply_property psp,
 	union power_supply_propval *val)
 {
 	s32 ret;
+	enum bq20z75_battery_mode mode = BATTERY_MODE_WATTS;
+
+	if (power_supply_is_amp_property(psp))
+		mode = BATTERY_MODE_AMPS;
+
+	mode = bq20z75_set_battery_mode(client, mode);
+	if (mode < 0)
+		return mode;
 
 	ret = bq20z75_read_word_data(client, bq20z75_data[reg_offset].addr);
 	if (ret < 0)
@@ -298,6 +404,10 @@ static int bq20z75_get_battery_capacity(struct i2c_client *client,
 	} else
 		val->intval = ret;
 
+	ret = bq20z75_set_battery_mode(client, mode);
+	if (ret < 0)
+		return ret;
+
 	return 0;
 }
 
@@ -318,12 +428,25 @@ static int bq20z75_get_battery_serial_number(struct i2c_client *client,
 	return 0;
 }
 
+static int bq20z75_get_property_index(struct i2c_client *client,
+	enum power_supply_property psp)
+{
+	int count;
+	for (count = 0; count < ARRAY_SIZE(bq20z75_data); count++)
+		if (psp == bq20z75_data[count].psp)
+			return count;
+
+	dev_warn(&client->dev,
+		"%s: Invalid Property - %d\n", __func__, psp);
+
+	return -EINVAL;
+}
+
 static int bq20z75_get_property(struct power_supply *psy,
 	enum power_supply_property psp,
 	union power_supply_propval *val)
 {
-	int count;
-	int ret;
+	int ret = 0;
 	struct bq20z75_info *bq20z75_device = container_of(psy,
 				struct bq20z75_info, power_supply);
 	struct i2c_client *client = bq20z75_device->client;
@@ -332,8 +455,6 @@ static int bq20z75_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_PRESENT:
 	case POWER_SUPPLY_PROP_HEALTH:
 		ret = bq20z75_get_battery_presence_and_health(client, psp, val);
-		if (ret)
-			return ret;
 		break;
 
 	case POWER_SUPPLY_PROP_TECHNOLOGY:
@@ -343,22 +464,19 @@ static int bq20z75_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_ENERGY_NOW:
 	case POWER_SUPPLY_PROP_ENERGY_FULL:
 	case POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN:
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
 	case POWER_SUPPLY_PROP_CAPACITY:
-		for (count = 0; count < ARRAY_SIZE(bq20z75_data); count++) {
-			if (psp == bq20z75_data[count].psp)
-				break;
-		}
-
-		ret = bq20z75_get_battery_capacity(client, count, psp, val);
-		if (ret)
-			return ret;
+		ret = bq20z75_get_property_index(client, psp);
+		if (ret < 0)
+			break;
 
+		ret = bq20z75_get_battery_capacity(client, ret, psp, val);
 		break;
 
 	case POWER_SUPPLY_PROP_SERIAL_NUMBER:
 		ret = bq20z75_get_battery_serial_number(client, val);
-		if (ret)
-			return ret;
 		break;
 
 	case POWER_SUPPLY_PROP_STATUS:
@@ -369,15 +487,11 @@ static int bq20z75_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG:
 	case POWER_SUPPLY_PROP_TIME_TO_FULL_AVG:
 	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
-		for (count = 0; count < ARRAY_SIZE(bq20z75_data); count++) {
-			if (psp == bq20z75_data[count].psp)
-				break;
-		}
-
-		ret = bq20z75_get_battery_property(client, count, psp, val);
-		if (ret)
-			return ret;
+		ret = bq20z75_get_property_index(client, psp);
+		if (ret < 0)
+			break;
 
+		ret = bq20z75_get_battery_property(client, ret, psp, val);
 		break;
 
 	default:
@@ -386,26 +500,51 @@ static int bq20z75_get_property(struct power_supply *psy,
 		return -EINVAL;
 	}
 
-	/* Convert units to match requirements for power supply class */
-	bq20z75_unit_adjustment(client, psp, val);
+	if (!bq20z75_device->enable_detection)
+		goto done;
+
+	if (!bq20z75_device->gpio_detect &&
+		bq20z75_device->is_present != (ret >= 0)) {
+		bq20z75_device->is_present = (ret >= 0);
+		power_supply_changed(&bq20z75_device->power_supply);
+	}
+
+done:
+	if (!ret) {
+		/* Convert units to match requirements for power supply class */
+		bq20z75_unit_adjustment(client, psp, val);
+	}
 
 	dev_dbg(&client->dev,
 		"%s: property = %d, value = %d\n", __func__, psp, val->intval);
 
-	return 0;
+	return ret;
+}
+
+static irqreturn_t bq20z75_irq(int irq, void *devid)
+{
+	struct power_supply *battery = devid;
+
+	power_supply_changed(battery);
+
+	return IRQ_HANDLED;
 }
 
-static int bq20z75_probe(struct i2c_client *client,
+static int __devinit bq20z75_probe(struct i2c_client *client,
 	const struct i2c_device_id *id)
 {
 	struct bq20z75_info *bq20z75_device;
+	struct bq20z75_platform_data *pdata = client->dev.platform_data;
 	int rc;
+	int irq;
 
 	bq20z75_device = kzalloc(sizeof(struct bq20z75_info), GFP_KERNEL);
 	if (!bq20z75_device)
 		return -ENOMEM;
 
 	bq20z75_device->client = client;
+	bq20z75_device->enable_detection = false;
+	bq20z75_device->gpio_detect = false;
 	bq20z75_device->power_supply.name = "battery";
 	bq20z75_device->power_supply.type = POWER_SUPPLY_TYPE_BATTERY;
 	bq20z75_device->power_supply.properties = bq20z75_properties;
@@ -413,26 +552,86 @@ static int bq20z75_probe(struct i2c_client *client,
 		ARRAY_SIZE(bq20z75_properties);
 	bq20z75_device->power_supply.get_property = bq20z75_get_property;
 
+	if (pdata) {
+		bq20z75_device->gpio_detect =
+			gpio_is_valid(pdata->battery_detect);
+		bq20z75_device->pdata = pdata;
+	}
+
 	i2c_set_clientdata(client, bq20z75_device);
 
+	if (!bq20z75_device->gpio_detect)
+		goto skip_gpio;
+
+	rc = gpio_request(pdata->battery_detect, dev_name(&client->dev));
+	if (rc) {
+		dev_warn(&client->dev, "Failed to request gpio: %d\n", rc);
+		bq20z75_device->gpio_detect = false;
+		goto skip_gpio;
+	}
+
+	rc = gpio_direction_input(pdata->battery_detect);
+	if (rc) {
+		dev_warn(&client->dev, "Failed to get gpio as input: %d\n", rc);
+		gpio_free(pdata->battery_detect);
+		bq20z75_device->gpio_detect = false;
+		goto skip_gpio;
+	}
+
+	irq = gpio_to_irq(pdata->battery_detect);
+	if (irq <= 0) {
+		dev_warn(&client->dev, "Failed to get gpio as irq: %d\n", irq);
+		gpio_free(pdata->battery_detect);
+		bq20z75_device->gpio_detect = false;
+		goto skip_gpio;
+	}
+
+	rc = request_irq(irq, bq20z75_irq,
+		IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+		dev_name(&client->dev), &bq20z75_device->power_supply);
+	if (rc) {
+		dev_warn(&client->dev, "Failed to request irq: %d\n", rc);
+		gpio_free(pdata->battery_detect);
+		bq20z75_device->gpio_detect = false;
+		goto skip_gpio;
+	}
+
+	bq20z75_device->irq = irq;
+
+skip_gpio:
+
 	rc = power_supply_register(&client->dev, &bq20z75_device->power_supply);
 	if (rc) {
 		dev_err(&client->dev,
 			"%s: Failed to register power supply\n", __func__);
-		kfree(bq20z75_device);
-		return rc;
+		goto exit_psupply;
 	}
 
 	dev_info(&client->dev,
 		"%s: battery gas gauge device registered\n", client->name);
 
 	return 0;
+
+exit_psupply:
+	if (bq20z75_device->irq)
+		free_irq(bq20z75_device->irq, &bq20z75_device->power_supply);
+	if (bq20z75_device->gpio_detect)
+		gpio_free(pdata->battery_detect);
+
+	kfree(bq20z75_device);
+
+	return rc;
 }
 
-static int bq20z75_remove(struct i2c_client *client)
+static int __devexit bq20z75_remove(struct i2c_client *client)
 {
 	struct bq20z75_info *bq20z75_device = i2c_get_clientdata(client);
 
+	if (bq20z75_device->irq)
+		free_irq(bq20z75_device->irq, &bq20z75_device->power_supply);
+	if (bq20z75_device->gpio_detect)
+		gpio_free(bq20z75_device->pdata->battery_detect);
+
 	power_supply_unregister(&bq20z75_device->power_supply);
 	kfree(bq20z75_device);
 	bq20z75_device = NULL;
@@ -465,10 +664,11 @@ static const struct i2c_device_id bq20z75_id[] = {
 	{ "bq20z75", 0 },
 	{}
 };
+MODULE_DEVICE_TABLE(i2c, bq20z75_id);
 
 static struct i2c_driver bq20z75_battery_driver = {
 	.probe		= bq20z75_probe,
-	.remove		= bq20z75_remove,
+	.remove		= __devexit_p(bq20z75_remove),
 	.suspend	= bq20z75_suspend,
 	.resume		= bq20z75_resume,
 	.id_table	= bq20z75_id,
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index eff0273d4030..59e68dbd028b 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2008 Rodolfo Giometti <giometti@linux.it>
  * Copyright (C) 2008 Eurotech S.p.A. <info@eurotech.it>
+ * Copyright (C) 2010-2011 Lars-Peter Clausen <lars@metafoo.de>
  *
  * Based on a previous work by Copyright (C) 2008 Texas Instruments, Inc.
  *
@@ -15,6 +16,13 @@
  * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  *
  */
+
+/*
+ * Datasheets:
+ * http://focus.ti.com/docs/prod/folders/print/bq27000.html
+ * http://focus.ti.com/docs/prod/folders/print/bq27500.html
+ */
+
 #include <linux/module.h>
 #include <linux/param.h>
 #include <linux/jiffies.h>
@@ -27,7 +35,9 @@
 #include <linux/slab.h>
 #include <asm/unaligned.h>
 
-#define DRIVER_VERSION			"1.1.0"
+#include <linux/power/bq27x00_battery.h>
+
+#define DRIVER_VERSION			"1.2.0"
 
 #define BQ27x00_REG_TEMP		0x06
 #define BQ27x00_REG_VOLT		0x08
@@ -36,36 +46,59 @@
 #define BQ27x00_REG_TTE			0x16
 #define BQ27x00_REG_TTF			0x18
 #define BQ27x00_REG_TTECP		0x26
+#define BQ27x00_REG_NAC			0x0C /* Nominal available capaciy */
+#define BQ27x00_REG_LMD			0x12 /* Last measured discharge */
+#define BQ27x00_REG_CYCT		0x2A /* Cycle count total */
+#define BQ27x00_REG_AE			0x22 /* Available enery */
 
 #define BQ27000_REG_RSOC		0x0B /* Relative State-of-Charge */
+#define BQ27000_REG_ILMD		0x76 /* Initial last measured discharge */
 #define BQ27000_FLAG_CHGS		BIT(7)
+#define BQ27000_FLAG_FC			BIT(5)
 
-#define BQ27500_REG_SOC			0x2c
+#define BQ27500_REG_SOC			0x2C
+#define BQ27500_REG_DCAP		0x3C /* Design capacity */
 #define BQ27500_FLAG_DSC		BIT(0)
 #define BQ27500_FLAG_FC			BIT(9)
 
-/* 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);
+#define BQ27000_RS			20 /* Resistor sense */
 
 struct bq27x00_device_info;
 struct bq27x00_access_methods {
-	int (*read)(u8 reg, int *rt_value, int b_single,
-		struct bq27x00_device_info *di);
+	int (*read)(struct bq27x00_device_info *di, u8 reg, bool single);
 };
 
 enum bq27x00_chip { BQ27000, BQ27500 };
 
+struct bq27x00_reg_cache {
+	int temperature;
+	int time_to_empty;
+	int time_to_empty_avg;
+	int time_to_full;
+	int charge_full;
+	int charge_counter;
+	int capacity;
+	int flags;
+
+	int current_now;
+};
+
 struct bq27x00_device_info {
 	struct device 		*dev;
 	int			id;
-	struct bq27x00_access_methods	*bus;
-	struct power_supply	bat;
 	enum bq27x00_chip	chip;
 
-	struct i2c_client	*client;
+	struct bq27x00_reg_cache cache;
+	int charge_design_full;
+
+	unsigned long last_update;
+	struct delayed_work work;
+
+	struct power_supply	bat;
+
+	struct bq27x00_access_methods bus;
+
+	struct mutex lock;
 };
 
 static enum power_supply_property bq27x00_battery_props[] = {
@@ -78,164 +111,328 @@ static enum power_supply_property bq27x00_battery_props[] = {
 	POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW,
 	POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG,
 	POWER_SUPPLY_PROP_TIME_TO_FULL_NOW,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_COUNTER,
+	POWER_SUPPLY_PROP_ENERGY_NOW,
 };
 
+static unsigned int poll_interval = 360;
+module_param(poll_interval, uint, 0644);
+MODULE_PARM_DESC(poll_interval, "battery poll interval in seconds - " \
+				"0 disables polling");
+
 /*
  * Common code for BQ27x00 devices
  */
 
-static int bq27x00_read(u8 reg, int *rt_value, int b_single,
-			struct bq27x00_device_info *di)
+static inline int bq27x00_read(struct bq27x00_device_info *di, u8 reg,
+		bool single)
 {
-	return di->bus->read(reg, rt_value, b_single, di);
+	return di->bus.read(di, reg, single);
 }
 
 /*
- * Return the battery temperature in tenths of degree Celsius
+ * Return the battery Relative State-of-Charge
  * Or < 0 if something fails.
  */
-static int bq27x00_battery_temperature(struct bq27x00_device_info *di)
+static int bq27x00_battery_read_rsoc(struct bq27x00_device_info *di)
 {
-	int ret;
-	int temp = 0;
+	int rsoc;
 
-	ret = bq27x00_read(BQ27x00_REG_TEMP, &temp, 0, di);
-	if (ret) {
-		dev_err(di->dev, "error reading temperature\n");
-		return ret;
+	if (di->chip == BQ27500)
+		rsoc = bq27x00_read(di, BQ27500_REG_SOC, false);
+	else
+		rsoc = bq27x00_read(di, BQ27000_REG_RSOC, true);
+
+	if (rsoc < 0)
+		dev_err(di->dev, "error reading relative State-of-Charge\n");
+
+	return rsoc;
+}
+
+/*
+ * Return a battery charge value in µAh
+ * Or < 0 if something fails.
+ */
+static int bq27x00_battery_read_charge(struct bq27x00_device_info *di, u8 reg)
+{
+	int charge;
+
+	charge = bq27x00_read(di, reg, false);
+	if (charge < 0) {
+		dev_err(di->dev, "error reading nominal available capacity\n");
+		return charge;
 	}
 
 	if (di->chip == BQ27500)
-		return temp - 2731;
+		charge *= 1000;
 	else
-		return ((temp >> 2) - 273) * 10;
+		charge = charge * 3570 / BQ27000_RS;
+
+	return charge;
 }
 
 /*
- * Return the battery Voltage in milivolts
+ * Return the battery Nominal available capaciy in µAh
  * Or < 0 if something fails.
  */
-static int bq27x00_battery_voltage(struct bq27x00_device_info *di)
+static inline int bq27x00_battery_read_nac(struct bq27x00_device_info *di)
 {
-	int ret;
-	int volt = 0;
+	return bq27x00_battery_read_charge(di, BQ27x00_REG_NAC);
+}
 
-	ret = bq27x00_read(BQ27x00_REG_VOLT, &volt, 0, di);
-	if (ret) {
-		dev_err(di->dev, "error reading voltage\n");
-		return ret;
+/*
+ * Return the battery Last measured discharge in µAh
+ * Or < 0 if something fails.
+ */
+static inline int bq27x00_battery_read_lmd(struct bq27x00_device_info *di)
+{
+	return bq27x00_battery_read_charge(di, BQ27x00_REG_LMD);
+}
+
+/*
+ * Return the battery Initial last measured discharge in µAh
+ * Or < 0 if something fails.
+ */
+static int bq27x00_battery_read_ilmd(struct bq27x00_device_info *di)
+{
+	int ilmd;
+
+	if (di->chip == BQ27500)
+		ilmd = bq27x00_read(di, BQ27500_REG_DCAP, false);
+	else
+		ilmd = bq27x00_read(di, BQ27000_REG_ILMD, true);
+
+	if (ilmd < 0) {
+		dev_err(di->dev, "error reading initial last measured discharge\n");
+		return ilmd;
 	}
 
-	return volt * 1000;
+	if (di->chip == BQ27500)
+		ilmd *= 1000;
+	else
+		ilmd = ilmd * 256 * 3570 / BQ27000_RS;
+
+	return ilmd;
 }
 
 /*
- * Return the battery average current
- * Note that current can be negative signed as well
- * Or 0 if something fails.
+ * Return the battery Cycle count total
+ * Or < 0 if something fails.
  */
-static int bq27x00_battery_current(struct bq27x00_device_info *di)
+static int bq27x00_battery_read_cyct(struct bq27x00_device_info *di)
 {
-	int ret;
-	int curr = 0;
-	int flags = 0;
+	int cyct;
 
-	ret = bq27x00_read(BQ27x00_REG_AI, &curr, 0, di);
-	if (ret) {
-		dev_err(di->dev, "error reading current\n");
-		return 0;
+	cyct = bq27x00_read(di, BQ27x00_REG_CYCT, false);
+	if (cyct < 0)
+		dev_err(di->dev, "error reading cycle count total\n");
+
+	return cyct;
+}
+
+/*
+ * Read a time register.
+ * Return < 0 if something fails.
+ */
+static int bq27x00_battery_read_time(struct bq27x00_device_info *di, u8 reg)
+{
+	int tval;
+
+	tval = bq27x00_read(di, reg, false);
+	if (tval < 0) {
+		dev_err(di->dev, "error reading register %02x: %d\n", reg, tval);
+		return tval;
 	}
 
-	if (di->chip == BQ27500) {
-		/* bq27500 returns signed value */
-		curr = (int)(s16)curr;
-	} else {
-		ret = bq27x00_read(BQ27x00_REG_FLAGS, &flags, 0, di);
-		if (ret < 0) {
-			dev_err(di->dev, "error reading flags\n");
-			return 0;
-		}
-		if (flags & BQ27000_FLAG_CHGS) {
-			dev_dbg(di->dev, "negative current!\n");
-			curr = -curr;
-		}
+	if (tval == 65535)
+		return -ENODATA;
+
+	return tval * 60;
+}
+
+static void bq27x00_update(struct bq27x00_device_info *di)
+{
+	struct bq27x00_reg_cache cache = {0, };
+	bool is_bq27500 = di->chip == BQ27500;
+
+	cache.flags = bq27x00_read(di, BQ27x00_REG_FLAGS, is_bq27500);
+	if (cache.flags >= 0) {
+		cache.capacity = bq27x00_battery_read_rsoc(di);
+		cache.temperature = bq27x00_read(di, BQ27x00_REG_TEMP, false);
+		cache.time_to_empty = bq27x00_battery_read_time(di, BQ27x00_REG_TTE);
+		cache.time_to_empty_avg = bq27x00_battery_read_time(di, BQ27x00_REG_TTECP);
+		cache.time_to_full = bq27x00_battery_read_time(di, BQ27x00_REG_TTF);
+		cache.charge_full = bq27x00_battery_read_lmd(di);
+		cache.charge_counter = bq27x00_battery_read_cyct(di);
+
+		if (!is_bq27500)
+			cache.current_now = bq27x00_read(di, BQ27x00_REG_AI, false);
+
+		/* We only have to read charge design full once */
+		if (di->charge_design_full <= 0)
+			di->charge_design_full = bq27x00_battery_read_ilmd(di);
+	}
+
+	/* Ignore current_now which is a snapshot of the current battery state
+	 * and is likely to be different even between two consecutive reads */
+	if (memcmp(&di->cache, &cache, sizeof(cache) - sizeof(int)) != 0) {
+		di->cache = cache;
+		power_supply_changed(&di->bat);
 	}
 
-	return curr * 1000;
+	di->last_update = jiffies;
+}
+
+static void bq27x00_battery_poll(struct work_struct *work)
+{
+	struct bq27x00_device_info *di =
+		container_of(work, struct bq27x00_device_info, work.work);
+
+	bq27x00_update(di);
+
+	if (poll_interval > 0) {
+		/* The timer does not have to be accurate. */
+		set_timer_slack(&di->work.timer, poll_interval * HZ / 4);
+		schedule_delayed_work(&di->work, poll_interval * HZ);
+	}
 }
 
+
 /*
- * Return the battery Relative State-of-Charge
+ * Return the battery temperature in tenths of degree Celsius
  * Or < 0 if something fails.
  */
-static int bq27x00_battery_rsoc(struct bq27x00_device_info *di)
+static int bq27x00_battery_temperature(struct bq27x00_device_info *di,
+	union power_supply_propval *val)
 {
-	int ret;
-	int rsoc = 0;
+	if (di->cache.temperature < 0)
+		return di->cache.temperature;
 
 	if (di->chip == BQ27500)
-		ret = bq27x00_read(BQ27500_REG_SOC, &rsoc, 0, di);
+		val->intval = di->cache.temperature - 2731;
 	else
-		ret = bq27x00_read(BQ27000_REG_RSOC, &rsoc, 1, di);
-	if (ret) {
-		dev_err(di->dev, "error reading relative State-of-Charge\n");
-		return ret;
+		val->intval = ((di->cache.temperature * 5) - 5463) / 2;
+
+	return 0;
+}
+
+/*
+ * Return the battery average current in µA
+ * Note that current can be negative signed as well
+ * Or 0 if something fails.
+ */
+static int bq27x00_battery_current(struct bq27x00_device_info *di,
+	union power_supply_propval *val)
+{
+	int curr;
+
+	if (di->chip == BQ27500)
+	    curr = bq27x00_read(di, BQ27x00_REG_AI, false);
+	else
+	    curr = di->cache.current_now;
+
+	if (curr < 0)
+		return curr;
+
+	if (di->chip == BQ27500) {
+		/* bq27500 returns signed value */
+		val->intval = (int)((s16)curr) * 1000;
+	} else {
+		if (di->cache.flags & BQ27000_FLAG_CHGS) {
+			dev_dbg(di->dev, "negative current!\n");
+			curr = -curr;
+		}
+
+		val->intval = curr * 3570 / BQ27000_RS;
 	}
 
-	return rsoc;
+	return 0;
 }
 
 static int bq27x00_battery_status(struct bq27x00_device_info *di,
-				  union power_supply_propval *val)
+	union power_supply_propval *val)
 {
-	int flags = 0;
 	int status;
-	int ret;
-
-	ret = bq27x00_read(BQ27x00_REG_FLAGS, &flags, 0, di);
-	if (ret < 0) {
-		dev_err(di->dev, "error reading flags\n");
-		return ret;
-	}
 
 	if (di->chip == BQ27500) {
-		if (flags & BQ27500_FLAG_FC)
+		if (di->cache.flags & BQ27500_FLAG_FC)
 			status = POWER_SUPPLY_STATUS_FULL;
-		else if (flags & BQ27500_FLAG_DSC)
+		else if (di->cache.flags & BQ27500_FLAG_DSC)
 			status = POWER_SUPPLY_STATUS_DISCHARGING;
 		else
 			status = POWER_SUPPLY_STATUS_CHARGING;
 	} else {
-		if (flags & BQ27000_FLAG_CHGS)
+		if (di->cache.flags & BQ27000_FLAG_FC)
+			status = POWER_SUPPLY_STATUS_FULL;
+		else if (di->cache.flags & BQ27000_FLAG_CHGS)
 			status = POWER_SUPPLY_STATUS_CHARGING;
+		else if (power_supply_am_i_supplied(&di->bat))
+			status = POWER_SUPPLY_STATUS_NOT_CHARGING;
 		else
 			status = POWER_SUPPLY_STATUS_DISCHARGING;
 	}
 
 	val->intval = status;
+
 	return 0;
 }
 
 /*
- * Read a time register.
- * Return < 0 if something fails.
+ * Return the battery Voltage in milivolts
+ * Or < 0 if something fails.
  */
-static int bq27x00_battery_time(struct bq27x00_device_info *di, int reg,
-				union power_supply_propval *val)
+static int bq27x00_battery_voltage(struct bq27x00_device_info *di,
+	union power_supply_propval *val)
 {
-	int tval = 0;
-	int ret;
+	int volt;
 
-	ret = bq27x00_read(reg, &tval, 0, di);
-	if (ret) {
-		dev_err(di->dev, "error reading register %02x\n", reg);
-		return ret;
+	volt = bq27x00_read(di, BQ27x00_REG_VOLT, false);
+	if (volt < 0)
+		return volt;
+
+	val->intval = volt * 1000;
+
+	return 0;
+}
+
+/*
+ * Return the battery Available energy in µWh
+ * Or < 0 if something fails.
+ */
+static int bq27x00_battery_energy(struct bq27x00_device_info *di,
+	union power_supply_propval *val)
+{
+	int ae;
+
+	ae = bq27x00_read(di, BQ27x00_REG_AE, false);
+	if (ae < 0) {
+		dev_err(di->dev, "error reading available energy\n");
+		return ae;
 	}
 
-	if (tval == 65535)
-		return -ENODATA;
+	if (di->chip == BQ27500)
+		ae *= 1000;
+	else
+		ae = ae * 29200 / BQ27000_RS;
+
+	val->intval = ae;
+
+	return 0;
+}
+
+
+static int bq27x00_simple_value(int value,
+	union power_supply_propval *val)
+{
+	if (value < 0)
+		return value;
+
+	val->intval = value;
 
-	val->intval = tval * 60;
 	return 0;
 }
 
@@ -249,33 +446,61 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 	int ret = 0;
 	struct bq27x00_device_info *di = to_bq27x00_device_info(psy);
 
+	mutex_lock(&di->lock);
+	if (time_is_before_jiffies(di->last_update + 5 * HZ)) {
+		cancel_delayed_work_sync(&di->work);
+		bq27x00_battery_poll(&di->work.work);
+	}
+	mutex_unlock(&di->lock);
+
+	if (psp != POWER_SUPPLY_PROP_PRESENT && di->cache.flags < 0)
+		return -ENODEV;
+
 	switch (psp) {
 	case POWER_SUPPLY_PROP_STATUS:
 		ret = bq27x00_battery_status(di, val);
 		break;
 	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		ret = bq27x00_battery_voltage(di, val);
+		break;
 	case POWER_SUPPLY_PROP_PRESENT:
-		val->intval = bq27x00_battery_voltage(di);
-		if (psp == POWER_SUPPLY_PROP_PRESENT)
-			val->intval = val->intval <= 0 ? 0 : 1;
+		val->intval = di->cache.flags < 0 ? 0 : 1;
 		break;
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
-		val->intval = bq27x00_battery_current(di);
+		ret = bq27x00_battery_current(di, val);
 		break;
 	case POWER_SUPPLY_PROP_CAPACITY:
-		val->intval = bq27x00_battery_rsoc(di);
+		ret = bq27x00_simple_value(di->cache.capacity, val);
 		break;
 	case POWER_SUPPLY_PROP_TEMP:
-		val->intval = bq27x00_battery_temperature(di);
+		ret = bq27x00_battery_temperature(di, val);
 		break;
 	case POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW:
-		ret = bq27x00_battery_time(di, BQ27x00_REG_TTE, val);
+		ret = bq27x00_simple_value(di->cache.time_to_empty, val);
 		break;
 	case POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG:
-		ret = bq27x00_battery_time(di, BQ27x00_REG_TTECP, val);
+		ret = bq27x00_simple_value(di->cache.time_to_empty_avg, val);
 		break;
 	case POWER_SUPPLY_PROP_TIME_TO_FULL_NOW:
-		ret = bq27x00_battery_time(di, BQ27x00_REG_TTF, val);
+		ret = bq27x00_simple_value(di->cache.time_to_full, val);
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		ret = bq27x00_simple_value(bq27x00_battery_read_nac(di), val);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+		ret = bq27x00_simple_value(di->cache.charge_full, val);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		ret = bq27x00_simple_value(di->charge_design_full, val);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_COUNTER:
+		ret = bq27x00_simple_value(di->cache.charge_counter, val);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_NOW:
+		ret = bq27x00_battery_energy(di, val);
 		break;
 	default:
 		return -EINVAL;
@@ -284,56 +509,91 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 	return ret;
 }
 
-static void bq27x00_powersupply_init(struct bq27x00_device_info *di)
+static void bq27x00_external_power_changed(struct power_supply *psy)
 {
+	struct bq27x00_device_info *di = to_bq27x00_device_info(psy);
+
+	cancel_delayed_work_sync(&di->work);
+	schedule_delayed_work(&di->work, 0);
+}
+
+static int bq27x00_powersupply_init(struct bq27x00_device_info *di)
+{
+	int ret;
+
 	di->bat.type = POWER_SUPPLY_TYPE_BATTERY;
 	di->bat.properties = bq27x00_battery_props;
 	di->bat.num_properties = ARRAY_SIZE(bq27x00_battery_props);
 	di->bat.get_property = bq27x00_battery_get_property;
-	di->bat.external_power_changed = NULL;
+	di->bat.external_power_changed = bq27x00_external_power_changed;
+
+	INIT_DELAYED_WORK(&di->work, bq27x00_battery_poll);
+	mutex_init(&di->lock);
+
+	ret = power_supply_register(di->dev, &di->bat);
+	if (ret) {
+		dev_err(di->dev, "failed to register battery: %d\n", ret);
+		return ret;
+	}
+
+	dev_info(di->dev, "support ver. %s enabled\n", DRIVER_VERSION);
+
+	bq27x00_update(di);
+
+	return 0;
 }
 
-/*
- * i2c specific code
+static void bq27x00_powersupply_unregister(struct bq27x00_device_info *di)
+{
+	cancel_delayed_work_sync(&di->work);
+
+	power_supply_unregister(&di->bat);
+
+	mutex_destroy(&di->lock);
+}
+
+
+/* i2c specific code */
+#ifdef CONFIG_BATTERY_BQ27X00_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 int bq27x00_read_i2c(u8 reg, int *rt_value, int b_single,
-			struct bq27x00_device_info *di)
+static int bq27x00_read_i2c(struct bq27x00_device_info *di, u8 reg, bool single)
 {
-	struct i2c_client *client = di->client;
-	struct i2c_msg msg[1];
+	struct i2c_client *client = to_i2c_client(di->dev);
+	struct i2c_msg msg[2];
 	unsigned char data[2];
-	int err;
+	int ret;
 
 	if (!client->adapter)
 		return -ENODEV;
 
-	msg->addr = client->addr;
-	msg->flags = 0;
-	msg->len = 1;
-	msg->buf = data;
-
-	data[0] = reg;
-	err = i2c_transfer(client->adapter, msg, 1);
+	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;
 
-	if (err >= 0) {
-		if (!b_single)
-			msg->len = 2;
-		else
-			msg->len = 1;
+	ret = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (ret < 0)
+		return ret;
 
-		msg->flags = I2C_M_RD;
-		err = i2c_transfer(client->adapter, msg, 1);
-		if (err >= 0) {
-			if (!b_single)
-				*rt_value = get_unaligned_le16(data);
-			else
-				*rt_value = data[0];
+	if (!single)
+		ret = get_unaligned_le16(data);
+	else
+		ret = data[0];
 
-			return 0;
-		}
-	}
-	return err;
+	return ret;
 }
 
 static int bq27x00_battery_probe(struct i2c_client *client,
@@ -341,7 +601,6 @@ static int bq27x00_battery_probe(struct i2c_client *client,
 {
 	char *name;
 	struct bq27x00_device_info *di;
-	struct bq27x00_access_methods *bus;
 	int num;
 	int retval = 0;
 
@@ -368,38 +627,20 @@ static int bq27x00_battery_probe(struct i2c_client *client,
 		retval = -ENOMEM;
 		goto batt_failed_2;
 	}
+
 	di->id = num;
+	di->dev = &client->dev;
 	di->chip = id->driver_data;
+	di->bat.name = name;
+	di->bus.read = &bq27x00_read_i2c;
 
-	bus = kzalloc(sizeof(*bus), GFP_KERNEL);
-	if (!bus) {
-		dev_err(&client->dev, "failed to allocate access method "
-					"data\n");
-		retval = -ENOMEM;
+	if (bq27x00_powersupply_init(di))
 		goto batt_failed_3;
-	}
 
 	i2c_set_clientdata(client, di);
-	di->dev = &client->dev;
-	di->bat.name = name;
-	bus->read = &bq27x00_read_i2c;
-	di->bus = bus;
-	di->client = client;
-
-	bq27x00_powersupply_init(di);
-
-	retval = power_supply_register(&client->dev, &di->bat);
-	if (retval) {
-		dev_err(&client->dev, "failed to register battery\n");
-		goto batt_failed_4;
-	}
-
-	dev_info(&client->dev, "support ver. %s enabled\n", DRIVER_VERSION);
 
 	return 0;
 
-batt_failed_4:
-	kfree(bus);
 batt_failed_3:
 	kfree(di);
 batt_failed_2:
@@ -416,9 +657,8 @@ static int bq27x00_battery_remove(struct i2c_client *client)
 {
 	struct bq27x00_device_info *di = i2c_get_clientdata(client);
 
-	power_supply_unregister(&di->bat);
+	bq27x00_powersupply_unregister(di);
 
-	kfree(di->bus);
 	kfree(di->bat.name);
 
 	mutex_lock(&battery_mutex);
@@ -430,15 +670,12 @@ static int bq27x00_battery_remove(struct i2c_client *client)
 	return 0;
 }
 
-/*
- * Module stuff
- */
-
 static const struct i2c_device_id bq27x00_id[] = {
 	{ "bq27200", BQ27000 },	/* bq27200 is same as bq27000, but with i2c */
 	{ "bq27500", BQ27500 },
 	{},
 };
+MODULE_DEVICE_TABLE(i2c, bq27x00_id);
 
 static struct i2c_driver bq27x00_battery_driver = {
 	.driver = {
@@ -449,13 +686,164 @@ static struct i2c_driver bq27x00_battery_driver = {
 	.id_table = bq27x00_id,
 };
 
+static inline int bq27x00_battery_i2c_init(void)
+{
+	int ret = i2c_add_driver(&bq27x00_battery_driver);
+	if (ret)
+		printk(KERN_ERR "Unable to register BQ27x00 i2c driver\n");
+
+	return ret;
+}
+
+static inline void bq27x00_battery_i2c_exit(void)
+{
+	i2c_del_driver(&bq27x00_battery_driver);
+}
+
+#else
+
+static inline int bq27x00_battery_i2c_init(void) { return 0; }
+static inline void bq27x00_battery_i2c_exit(void) {};
+
+#endif
+
+/* platform specific code */
+#ifdef CONFIG_BATTERY_BQ27X00_PLATFORM
+
+static int bq27000_read_platform(struct bq27x00_device_info *di, u8 reg,
+			bool single)
+{
+	struct device *dev = di->dev;
+	struct bq27000_platform_data *pdata = dev->platform_data;
+	unsigned int timeout = 3;
+	int upper, lower;
+	int temp;
+
+	if (!single) {
+		/* Make sure the value has not changed in between reading the
+		 * lower and the upper part */
+		upper = pdata->read(dev, reg + 1);
+		do {
+			temp = upper;
+			if (upper < 0)
+				return upper;
+
+			lower = pdata->read(dev, reg);
+			if (lower < 0)
+				return lower;
+
+			upper = pdata->read(dev, reg + 1);
+		} while (temp != upper && --timeout);
+
+		if (timeout == 0)
+			return -EIO;
+
+		return (upper << 8) | lower;
+	}
+
+	return pdata->read(dev, reg);
+}
+
+static int __devinit bq27000_battery_probe(struct platform_device *pdev)
+{
+	struct bq27x00_device_info *di;
+	struct bq27000_platform_data *pdata = pdev->dev.platform_data;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform_data supplied\n");
+		return -EINVAL;
+	}
+
+	if (!pdata->read) {
+		dev_err(&pdev->dev, "no hdq read callback supplied\n");
+		return -EINVAL;
+	}
+
+	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	if (!di) {
+		dev_err(&pdev->dev, "failed to allocate device info data\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	di->dev = &pdev->dev;
+	di->chip = BQ27000;
+
+	di->bat.name = pdata->name ?: dev_name(&pdev->dev);
+	di->bus.read = &bq27000_read_platform;
+
+	ret = bq27x00_powersupply_init(di);
+	if (ret)
+		goto err_free;
+
+	return 0;
+
+err_free:
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return ret;
+}
+
+static int __devexit bq27000_battery_remove(struct platform_device *pdev)
+{
+	struct bq27x00_device_info *di = platform_get_drvdata(pdev);
+
+	bq27x00_powersupply_unregister(di);
+
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static struct platform_driver bq27000_battery_driver = {
+	.probe	= bq27000_battery_probe,
+	.remove = __devexit_p(bq27000_battery_remove),
+	.driver = {
+		.name = "bq27000-battery",
+		.owner = THIS_MODULE,
+	},
+};
+
+static inline int bq27x00_battery_platform_init(void)
+{
+	int ret = platform_driver_register(&bq27000_battery_driver);
+	if (ret)
+		printk(KERN_ERR "Unable to register BQ27000 platform driver\n");
+
+	return ret;
+}
+
+static inline void bq27x00_battery_platform_exit(void)
+{
+	platform_driver_unregister(&bq27000_battery_driver);
+}
+
+#else
+
+static inline int bq27x00_battery_platform_init(void) { return 0; }
+static inline void bq27x00_battery_platform_exit(void) {};
+
+#endif
+
+/*
+ * Module stuff
+ */
+
 static int __init bq27x00_battery_init(void)
 {
 	int ret;
 
-	ret = i2c_add_driver(&bq27x00_battery_driver);
+	ret = bq27x00_battery_i2c_init();
 	if (ret)
-		printk(KERN_ERR "Unable to register BQ27x00 driver\n");
+		return ret;
+
+	ret = bq27x00_battery_platform_init();
+	if (ret)
+		bq27x00_battery_i2c_exit();
 
 	return ret;
 }
@@ -463,7 +851,8 @@ module_init(bq27x00_battery_init);
 
 static void __exit bq27x00_battery_exit(void)
 {
-	i2c_del_driver(&bq27x00_battery_driver);
+	bq27x00_battery_platform_exit();
+	bq27x00_battery_i2c_exit();
 }
 module_exit(bq27x00_battery_exit);
 
diff --git a/drivers/power/ds2782_battery.c b/drivers/power/ds2782_battery.c
index 6957e8af6449..4d2dc4fa2888 100644
--- a/drivers/power/ds2782_battery.c
+++ b/drivers/power/ds2782_battery.c
@@ -393,6 +393,7 @@ static const struct i2c_device_id ds278x_id[] = {
 	{"ds2786", DS2786},
 	{},
 };
+MODULE_DEVICE_TABLE(i2c, ds278x_id);
 
 static struct i2c_driver ds278x_battery_driver = {
 	.driver 	= {
diff --git a/drivers/power/jz4740-battery.c b/drivers/power/jz4740-battery.c
index 02414db6a94c..763f894ed188 100644
--- a/drivers/power/jz4740-battery.c
+++ b/drivers/power/jz4740-battery.c
@@ -39,7 +39,7 @@ struct jz_battery {
 	int irq;
 	int charge_irq;
 
-	struct mfd_cell *cell;
+	const struct mfd_cell *cell;
 
 	int status;
 	long voltage;
@@ -258,7 +258,7 @@ static int __devinit jz_battery_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	jz_battery->cell = pdev->dev.platform_data;
+	jz_battery->cell = mfd_get_cell(pdev);
 
 	jz_battery->irq = platform_get_irq(pdev, 0);
 	if (jz_battery->irq < 0) {
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 970f7335d3a7..329b46b2327d 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -171,6 +171,8 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	dev_set_drvdata(dev, psy);
 	psy->dev = dev;
 
+	INIT_WORK(&psy->changed_work, power_supply_changed_work);
+
 	rc = kobject_set_name(&dev->kobj, "%s", psy->name);
 	if (rc)
 		goto kobject_set_name_failed;
@@ -179,8 +181,6 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	if (rc)
 		goto device_add_failed;
 
-	INIT_WORK(&psy->changed_work, power_supply_changed_work);
-
 	rc = power_supply_create_triggers(psy);
 	if (rc)
 		goto create_triggers_failed;
diff --git a/drivers/power/power_supply_leds.c b/drivers/power/power_supply_leds.c
index 031a554837f7..da25eb94e5c6 100644
--- a/drivers/power/power_supply_leds.c
+++ b/drivers/power/power_supply_leds.c
@@ -21,6 +21,8 @@
 static void power_supply_update_bat_leds(struct power_supply *psy)
 {
 	union power_supply_propval status;
+	unsigned long delay_on = 0;
+	unsigned long delay_off = 0;
 
 	if (psy->get_property(psy, POWER_SUPPLY_PROP_STATUS, &status))
 		return;
@@ -32,16 +34,22 @@ static void power_supply_update_bat_leds(struct power_supply *psy)
 		led_trigger_event(psy->charging_full_trig, LED_FULL);
 		led_trigger_event(psy->charging_trig, LED_OFF);
 		led_trigger_event(psy->full_trig, LED_FULL);
+		led_trigger_event(psy->charging_blink_full_solid_trig,
+			LED_FULL);
 		break;
 	case POWER_SUPPLY_STATUS_CHARGING:
 		led_trigger_event(psy->charging_full_trig, LED_FULL);
 		led_trigger_event(psy->charging_trig, LED_FULL);
 		led_trigger_event(psy->full_trig, LED_OFF);
+		led_trigger_blink(psy->charging_blink_full_solid_trig,
+			&delay_on, &delay_off);
 		break;
 	default:
 		led_trigger_event(psy->charging_full_trig, LED_OFF);
 		led_trigger_event(psy->charging_trig, LED_OFF);
 		led_trigger_event(psy->full_trig, LED_OFF);
+		led_trigger_event(psy->charging_blink_full_solid_trig,
+			LED_OFF);
 		break;
 	}
 }
@@ -64,15 +72,24 @@ static int power_supply_create_bat_triggers(struct power_supply *psy)
 	if (!psy->full_trig_name)
 		goto full_failed;
 
+	psy->charging_blink_full_solid_trig_name = kasprintf(GFP_KERNEL,
+		"%s-charging-blink-full-solid", psy->name);
+	if (!psy->charging_blink_full_solid_trig_name)
+		goto charging_blink_full_solid_failed;
+
 	led_trigger_register_simple(psy->charging_full_trig_name,
 				    &psy->charging_full_trig);
 	led_trigger_register_simple(psy->charging_trig_name,
 				    &psy->charging_trig);
 	led_trigger_register_simple(psy->full_trig_name,
 				    &psy->full_trig);
+	led_trigger_register_simple(psy->charging_blink_full_solid_trig_name,
+				    &psy->charging_blink_full_solid_trig);
 
 	goto success;
 
+charging_blink_full_solid_failed:
+	kfree(psy->full_trig_name);
 full_failed:
 	kfree(psy->charging_trig_name);
 charging_failed:
@@ -88,6 +105,8 @@ static void power_supply_remove_bat_triggers(struct power_supply *psy)
 	led_trigger_unregister_simple(psy->charging_full_trig);
 	led_trigger_unregister_simple(psy->charging_trig);
 	led_trigger_unregister_simple(psy->full_trig);
+	led_trigger_unregister_simple(psy->charging_blink_full_solid_trig);
+	kfree(psy->charging_blink_full_solid_trig_name);
 	kfree(psy->full_trig_name);
 	kfree(psy->charging_trig_name);
 	kfree(psy->charging_full_trig_name);
diff --git a/drivers/power/power_supply_sysfs.c b/drivers/power/power_supply_sysfs.c
index cd1f90754a3a..605514afc29f 100644
--- a/drivers/power/power_supply_sysfs.c
+++ b/drivers/power/power_supply_sysfs.c
@@ -270,7 +270,7 @@ int power_supply_uevent(struct device *dev, struct kobj_uevent_env *env)
 		attr = &power_supply_attrs[psy->properties[j]];
 
 		ret = power_supply_show_property(dev, attr, prop_buf);
-		if (ret == -ENODEV) {
+		if (ret == -ENODEV || ret == -ENODATA) {
 			/* When a battery is absent, we expect -ENODEV. Don't abort;
 			   send the uevent with at least the the PRESENT=0 property */
 			ret = 0;
diff --git a/drivers/power/twl4030_charger.c b/drivers/power/twl4030_charger.c
index ff1f42398a2e..92c16e1677bd 100644
--- a/drivers/power/twl4030_charger.c
+++ b/drivers/power/twl4030_charger.c
@@ -71,8 +71,11 @@ struct twl4030_bci {
 	struct power_supply	usb;
 	struct otg_transceiver	*transceiver;
 	struct notifier_block	otg_nb;
+	struct work_struct	work;
 	int			irq_chg;
 	int			irq_bci;
+
+	unsigned long		event;
 };
 
 /*
@@ -258,14 +261,11 @@ static irqreturn_t twl4030_bci_interrupt(int irq, void *arg)
 	return IRQ_HANDLED;
 }
 
-static int twl4030_bci_usb_ncb(struct notifier_block *nb, unsigned long val,
-			       void *priv)
+static void twl4030_bci_usb_work(struct work_struct *data)
 {
-	struct twl4030_bci *bci = container_of(nb, struct twl4030_bci, otg_nb);
+	struct twl4030_bci *bci = container_of(data, struct twl4030_bci, work);
 
-	dev_dbg(bci->dev, "OTG notify %lu\n", val);
-
-	switch (val) {
+	switch (bci->event) {
 	case USB_EVENT_VBUS:
 	case USB_EVENT_CHARGER:
 		twl4030_charger_enable_usb(bci, true);
@@ -274,6 +274,17 @@ static int twl4030_bci_usb_ncb(struct notifier_block *nb, unsigned long val,
 		twl4030_charger_enable_usb(bci, false);
 		break;
 	}
+}
+
+static int twl4030_bci_usb_ncb(struct notifier_block *nb, unsigned long val,
+			       void *priv)
+{
+	struct twl4030_bci *bci = container_of(nb, struct twl4030_bci, otg_nb);
+
+	dev_dbg(bci->dev, "OTG notify %lu\n", val);
+
+	bci->event = val;
+	schedule_work(&bci->work);
 
 	return NOTIFY_OK;
 }
@@ -466,6 +477,8 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 		goto fail_bci_irq;
 	}
 
+	INIT_WORK(&bci->work, twl4030_bci_usb_work);
+
 	bci->transceiver = otg_get_transceiver();
 	if (bci->transceiver != NULL) {
 		bci->otg_nb.notifier_call = twl4030_bci_usb_ncb;
diff --git a/drivers/power/z2_battery.c b/drivers/power/z2_battery.c
index e5ed52d71937..81304c0bb9d3 100644
--- a/drivers/power/z2_battery.c
+++ b/drivers/power/z2_battery.c
@@ -293,6 +293,7 @@ static const struct i2c_device_id z2_batt_id[] = {
 	{ "aer915", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, z2_batt_id);
 
 static struct i2c_driver z2_batt_driver = {
 	.driver	= {
diff --git a/drivers/pps/generators/Kconfig b/drivers/pps/generators/Kconfig
index f3a73dd77660..e4c4f3dc0728 100644
--- a/drivers/pps/generators/Kconfig
+++ b/drivers/pps/generators/Kconfig
@@ -6,7 +6,7 @@ comment "PPS generators support"
 
 config PPS_GENERATOR_PARPORT
 	tristate "Parallel port PPS signal generator"
-	depends on PARPORT
+	depends on PARPORT && BROKEN
 	help
 	  If you say yes here you get support for a PPS signal generator which
 	  utilizes STROBE pin of a parallel port to send PPS signals. It uses
diff --git a/drivers/pps/kapi.c b/drivers/pps/kapi.c
index cba1b43f7519..a4e8eb9fece6 100644
--- a/drivers/pps/kapi.c
+++ b/drivers/pps/kapi.c
@@ -168,7 +168,7 @@ void pps_event(struct pps_device *pps, struct pps_event_time *ts, int event,
 {
 	unsigned long flags;
 	int captured = 0;
-	struct pps_ktime ts_real;
+	struct pps_ktime ts_real = { .sec = 0, .nsec = 0, .flags = 0 };
 
 	/* check event type */
 	BUG_ON((event & (PPS_CAPTUREASSERT | PPS_CAPTURECLEAR)) == 0);
diff --git a/drivers/rapidio/rio-sysfs.c b/drivers/rapidio/rio-sysfs.c
index 76b41853a877..1269fbd2deca 100644
--- a/drivers/rapidio/rio-sysfs.c
+++ b/drivers/rapidio/rio-sysfs.c
@@ -77,9 +77,9 @@ rio_read_config(struct file *filp, struct kobject *kobj,
 
 	/* Several chips lock up trying to read undefined config space */
 	if (capable(CAP_SYS_ADMIN))
-		size = 0x200000;
+		size = RIO_MAINT_SPACE_SZ;
 
-	if (off > size)
+	if (off >= size)
 		return 0;
 	if (off + count > size) {
 		size -= off;
@@ -147,10 +147,10 @@ rio_write_config(struct file *filp, struct kobject *kobj,
 	loff_t init_off = off;
 	u8 *data = (u8 *) buf;
 
-	if (off > 0x200000)
+	if (off >= RIO_MAINT_SPACE_SZ)
 		return 0;
-	if (off + count > 0x200000) {
-		size = 0x200000 - off;
+	if (off + count > RIO_MAINT_SPACE_SZ) {
+		size = RIO_MAINT_SPACE_SZ - off;
 		count = size;
 	}
 
@@ -200,7 +200,7 @@ static struct bin_attribute rio_config_attr = {
 		 .name = "config",
 		 .mode = S_IRUGO | S_IWUSR,
 		 },
-	.size = 0x200000,
+	.size = RIO_MAINT_SPACE_SZ,
 	.read = rio_read_config,
 	.write = rio_write_config,
 };
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index e1d943619ab8..66a82e11ba9b 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -117,7 +117,7 @@ config REGULATOR_MAX8998
 	  and S5PC1XX chips to control VCC_CORE and VCC_USIM voltages.
 
 config REGULATOR_TWL4030
-	bool "TI TWL4030/TWL5030/TWL6030/TPS695x0 PMIC"
+	bool "TI TWL4030/TWL5030/TWL6030/TPS659x0 PMIC"
 	depends on TWL4030_CORE
 	help
 	  This driver supports the voltage regulators provided by
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index ed6feaf9398d..2dec589a8908 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -17,6 +17,7 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/mfd/abx500.h>
+#include <linux/mfd/core.h>
 
 /* LDO registers and some handy masking definitions for AB3100 */
 #define AB3100_LDO_A		0x40
@@ -576,7 +577,7 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 
 static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 {
-	struct ab3100_platform_data *plfdata = pdev->dev.platform_data;
+	struct ab3100_platform_data *plfdata = mfd_get_data(pdev);
 	int err = 0;
 	u8 data;
 	int i;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 9fa20957847d..a2dc6223e8d2 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -2565,8 +2565,11 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 			init_data->consumer_supplies[i].dev,
 			init_data->consumer_supplies[i].dev_name,
 			init_data->consumer_supplies[i].supply);
-		if (ret < 0)
+		if (ret < 0) {
+			dev_err(dev, "Failed to set supply %s\n",
+				init_data->consumer_supplies[i].supply);
 			goto unset_supplies;
+		}
 	}
 
 	list_add(&rdev->list, &regulator_list);
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 3e5d0c3b4e53..23249cb0a8bd 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -15,6 +15,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/platform_device.h>
 #include <linux/kernel.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/err.h>
@@ -336,8 +337,7 @@ static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
 {
 	struct mc13xxx_regulator_priv *priv;
 	struct mc13xxx *mc13783 = dev_get_drvdata(pdev->dev.parent);
-	struct mc13783_regulator_platform_data *pdata =
-		dev_get_platdata(&pdev->dev);
+	struct mc13783_regulator_platform_data *pdata = mfd_get_data(pdev);
 	struct mc13783_regulator_init_data *init_data;
 	int i, ret;
 
@@ -381,8 +381,7 @@ err:
 static int __devexit mc13783_regulator_remove(struct platform_device *pdev)
 {
 	struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
-	struct mc13783_regulator_platform_data *pdata =
-		dev_get_platdata(&pdev->dev);
+	struct mc13783_regulator_platform_data *pdata = mfd_get_data(pdev);
 	int i;
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 1b8f7398a4a8..6f15168e5ed4 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -15,6 +15,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/platform_device.h>
 #include <linux/kernel.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/err.h>
@@ -520,8 +521,7 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 {
 	struct mc13xxx_regulator_priv *priv;
 	struct mc13xxx *mc13892 = dev_get_drvdata(pdev->dev.parent);
-	struct mc13xxx_regulator_platform_data *pdata =
-		dev_get_platdata(&pdev->dev);
+	struct mc13xxx_regulator_platform_data *pdata = mfd_get_data(pdev);
 	struct mc13xxx_regulator_init_data *init_data;
 	int i, ret;
 	u32 val;
@@ -595,8 +595,7 @@ err_free:
 static int __devexit mc13892_regulator_remove(struct platform_device *pdev)
 {
 	struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
-	struct mc13xxx_regulator_platform_data *pdata =
-		dev_get_platdata(&pdev->dev);
+	struct mc13xxx_regulator_platform_data *pdata = mfd_get_data(pdev);
 	int i;
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/regulator/mc13xxx-regulator-core.c b/drivers/regulator/mc13xxx-regulator-core.c
index f53d31b950d4..2bb5de1f2421 100644
--- a/drivers/regulator/mc13xxx-regulator-core.c
+++ b/drivers/regulator/mc13xxx-regulator-core.c
@@ -174,7 +174,7 @@ static int mc13xxx_regulator_get_voltage(struct regulator_dev *rdev)
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d val: %d\n", __func__, id, val);
 
-	BUG_ON(val < 0 || val > mc13xxx_regulators[id].desc.n_voltages);
+	BUG_ON(val > mc13xxx_regulators[id].desc.n_voltages);
 
 	return mc13xxx_regulators[id].voltages[val];
 }
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index bd332cf1cc3f..6a292852a358 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -475,6 +475,13 @@ static struct regulator_ops twlfixed_ops = {
 	.get_status	= twlreg_get_status,
 };
 
+static struct regulator_ops twl6030_fixed_resource = {
+	.enable		= twlreg_enable,
+	.disable	= twlreg_disable,
+	.is_enabled	= twlreg_is_enabled,
+	.get_status	= twlreg_get_status,
+};
+
 /*----------------------------------------------------------------------*/
 
 #define TWL4030_FIXED_LDO(label, offset, mVolts, num, turnon_delay, \
@@ -538,6 +545,20 @@ static struct regulator_ops twlfixed_ops = {
 		}, \
 	}
 
+#define TWL6030_FIXED_RESOURCE(label, offset, num, turnon_delay, remap_conf) { \
+	.base = offset, \
+	.id = num, \
+	.delay = turnon_delay, \
+	.remap = remap_conf, \
+	.desc = { \
+		.name = #label, \
+		.id = TWL6030_REG_##label, \
+		.ops = &twl6030_fixed_resource, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+		}, \
+	}
+
 /*
  * We list regulators here if systems need some level of
  * software control over them after boot.
@@ -577,7 +598,8 @@ static struct twlreg_info twl_regs[] = {
 	TWL6030_FIXED_LDO(VANA, 0x50, 2100, 15, 0, 0x21),
 	TWL6030_FIXED_LDO(VCXIO, 0x60, 1800, 16, 0, 0x21),
 	TWL6030_FIXED_LDO(VDAC, 0x64, 1800, 17, 0, 0x21),
-	TWL6030_FIXED_LDO(VUSB, 0x70, 3300, 18, 0, 0x21)
+	TWL6030_FIXED_LDO(VUSB, 0x70, 3300, 18, 0, 0x21),
+	TWL6030_FIXED_RESOURCE(CLK32KG, 0x8C, 48, 0, 0x21),
 };
 
 static int __devinit twlreg_probe(struct platform_device *pdev)
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index 8b0d2c4bde91..857d741539da 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -120,6 +120,7 @@ static unsigned int wm831x_dcdc_get_mode(struct regulator_dev *rdev)
 		return REGULATOR_MODE_IDLE;
 	default:
 		BUG();
+		return -EINVAL;
 	}
 }
 
@@ -564,9 +565,8 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq_byname(pdev, "UV");
-	ret = wm831x_request_irq(wm831x, irq, wm831x_dcdc_uv_irq,
-				 IRQF_TRIGGER_RISING, dcdc->name,
-				 dcdc);
+	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_uv_irq,
+				   IRQF_TRIGGER_RISING, dcdc->name, dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -574,9 +574,8 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq_byname(pdev, "HC");
-	ret = wm831x_request_irq(wm831x, irq, wm831x_dcdc_oc_irq,
-				 IRQF_TRIGGER_RISING, dcdc->name,
-				 dcdc);
+	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_oc_irq,
+				   IRQF_TRIGGER_RISING, dcdc->name, dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request HC IRQ %d: %d\n",
 			irq, ret);
@@ -588,7 +587,7 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 	return 0;
 
 err_uv:
-	wm831x_free_irq(wm831x, platform_get_irq_byname(pdev, "UV"), dcdc);
+	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
 err_regulator:
 	regulator_unregister(dcdc->regulator);
 err:
@@ -755,9 +754,8 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq_byname(pdev, "UV");
-	ret = wm831x_request_irq(wm831x, irq, wm831x_dcdc_uv_irq,
-				 IRQF_TRIGGER_RISING, dcdc->name,
-				 dcdc);
+	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_uv_irq,
+				   IRQF_TRIGGER_RISING,	dcdc->name, dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -884,9 +882,9 @@ static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq_byname(pdev, "UV");
-	ret = wm831x_request_irq(wm831x, irq, wm831x_dcdc_uv_irq,
-				 IRQF_TRIGGER_RISING, dcdc->name,
-				 dcdc);
+	ret = request_threaded_irq(irq, NULL, wm831x_dcdc_uv_irq,
+				   IRQF_TRIGGER_RISING, dcdc->name,
+				   dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -907,11 +905,10 @@ err:
 static __devexit int wm831x_boostp_remove(struct platform_device *pdev)
 {
 	struct wm831x_dcdc *dcdc = platform_get_drvdata(pdev);
-	struct wm831x *wm831x = dcdc->wm831x;
 
 	platform_set_drvdata(pdev, NULL);
 
-	wm831x_free_irq(wm831x, platform_get_irq_byname(pdev, "UV"), dcdc);
+	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
 	regulator_unregister(dcdc->regulator);
 	kfree(dcdc);
 
diff --git a/drivers/regulator/wm831x-isink.c b/drivers/regulator/wm831x-isink.c
index 6c446cd6ad54..01f27c7f4236 100644
--- a/drivers/regulator/wm831x-isink.c
+++ b/drivers/regulator/wm831x-isink.c
@@ -198,9 +198,8 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq(pdev, 0);
-	ret = wm831x_request_irq(wm831x, irq, wm831x_isink_irq,
-				 IRQF_TRIGGER_RISING, isink->name,
-				 isink);
+	ret = request_threaded_irq(irq, NULL, wm831x_isink_irq,
+				   IRQF_TRIGGER_RISING, isink->name, isink);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request ISINK IRQ %d: %d\n",
 			irq, ret);
@@ -221,11 +220,10 @@ err:
 static __devexit int wm831x_isink_remove(struct platform_device *pdev)
 {
 	struct wm831x_isink *isink = platform_get_drvdata(pdev);
-	struct wm831x *wm831x = isink->wm831x;
 
 	platform_set_drvdata(pdev, NULL);
 
-	wm831x_free_irq(wm831x, platform_get_irq(pdev, 0), isink);
+	free_irq(platform_get_irq(pdev, 0), isink);
 
 	regulator_unregister(isink->regulator);
 	kfree(isink);
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index c94fc5b7cd5b..2220cf8defb1 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -354,9 +354,9 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq_byname(pdev, "UV");
-	ret = wm831x_request_irq(wm831x, irq, wm831x_ldo_uv_irq,
-				 IRQF_TRIGGER_RISING, ldo->name,
-				 ldo);
+	ret = request_threaded_irq(irq, NULL, wm831x_ldo_uv_irq,
+				   IRQF_TRIGGER_RISING, ldo->name,
+				   ldo);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -377,11 +377,10 @@ err:
 static __devexit int wm831x_gp_ldo_remove(struct platform_device *pdev)
 {
 	struct wm831x_ldo *ldo = platform_get_drvdata(pdev);
-	struct wm831x *wm831x = ldo->wm831x;
 
 	platform_set_drvdata(pdev, NULL);
 
-	wm831x_free_irq(wm831x, platform_get_irq_byname(pdev, "UV"), ldo);
+	free_irq(platform_get_irq_byname(pdev, "UV"), ldo);
 	regulator_unregister(ldo->regulator);
 	kfree(ldo);
 
@@ -619,9 +618,8 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq_byname(pdev, "UV");
-	ret = wm831x_request_irq(wm831x, irq, wm831x_ldo_uv_irq,
-				 IRQF_TRIGGER_RISING, ldo->name,
-				 ldo);
+	ret = request_threaded_irq(irq, NULL, wm831x_ldo_uv_irq,
+				   IRQF_TRIGGER_RISING, ldo->name, ldo);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -642,9 +640,8 @@ err:
 static __devexit int wm831x_aldo_remove(struct platform_device *pdev)
 {
 	struct wm831x_ldo *ldo = platform_get_drvdata(pdev);
-	struct wm831x *wm831x = ldo->wm831x;
 
-	wm831x_free_irq(wm831x, platform_get_irq_byname(pdev, "UV"), ldo);
+	free_irq(platform_get_irq_byname(pdev, "UV"), ldo);
 	regulator_unregister(ldo->regulator);
 	kfree(ldo);
 
diff --git a/drivers/rtc/rtc-at91sam9.c b/drivers/rtc/rtc-at91sam9.c
index c36749e4c926..5469c52cba3d 100644
--- a/drivers/rtc/rtc-at91sam9.c
+++ b/drivers/rtc/rtc-at91sam9.c
@@ -309,7 +309,7 @@ static const struct rtc_class_ops at91_rtc_ops = {
 	.read_alarm	= at91_rtc_readalarm,
 	.set_alarm	= at91_rtc_setalarm,
 	.proc		= at91_rtc_proc,
-	.alarm_irq_enabled = at91_rtc_alarm_irq_enable,
+	.alarm_irq_enable = at91_rtc_alarm_irq_enable,
 };
 
 /*
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index c7ff8df347e7..159b95e4b420 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -37,6 +37,8 @@
 #include <linux/mod_devicetable.h>
 #include <linux/log2.h>
 #include <linux/pm.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
 
 /* this is for "generic access to PC-style RTC" using CMOS_READ/CMOS_WRITE */
 #include <asm-generic/rtc.h>
@@ -1123,6 +1125,47 @@ static struct pnp_driver cmos_pnp_driver = {
 
 #endif	/* CONFIG_PNP */
 
+#ifdef CONFIG_OF
+static const struct of_device_id of_cmos_match[] = {
+	{
+		.compatible = "motorola,mc146818",
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, of_cmos_match);
+
+static __init void cmos_of_init(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct rtc_time time;
+	int ret;
+	const __be32 *val;
+
+	if (!node)
+		return;
+
+	val = of_get_property(node, "ctrl-reg", NULL);
+	if (val)
+		CMOS_WRITE(be32_to_cpup(val), RTC_CONTROL);
+
+	val = of_get_property(node, "freq-reg", NULL);
+	if (val)
+		CMOS_WRITE(be32_to_cpup(val), RTC_FREQ_SELECT);
+
+	get_rtc_time(&time);
+	ret = rtc_valid_tm(&time);
+	if (ret) {
+		struct rtc_time def_time = {
+			.tm_year = 1,
+			.tm_mday = 1,
+		};
+		set_rtc_time(&def_time);
+	}
+}
+#else
+static inline void cmos_of_init(struct platform_device *pdev) {}
+#define of_cmos_match NULL
+#endif
 /*----------------------------------------------------------------*/
 
 /* Platform setup should have set up an RTC device, when PNP is
@@ -1131,6 +1174,7 @@ static struct pnp_driver cmos_pnp_driver = {
 
 static int __init cmos_platform_probe(struct platform_device *pdev)
 {
+	cmos_of_init(pdev);
 	cmos_wake_setup(&pdev->dev);
 	return cmos_do_probe(&pdev->dev,
 			platform_get_resource(pdev, IORESOURCE_IO, 0),
@@ -1162,6 +1206,7 @@ static struct platform_driver cmos_platform_driver = {
 #ifdef CONFIG_PM
 		.pm		= &cmos_pm_ops,
 #endif
+		.of_match_table = of_cmos_match,
 	}
 };
 
diff --git a/drivers/rtc/rtc-ds3232.c b/drivers/rtc/rtc-ds3232.c
index 23a9ee19764c..950735415a7c 100644
--- a/drivers/rtc/rtc-ds3232.c
+++ b/drivers/rtc/rtc-ds3232.c
@@ -1,7 +1,7 @@
 /*
  * RTC client/driver for the Maxim/Dallas DS3232 Real-Time Clock over I2C
  *
- * Copyright (C) 2009-2010 Freescale Semiconductor.
+ * Copyright (C) 2009-2011 Freescale Semiconductor.
  * Author: Jack Lan <jack.lan@freescale.com>
  *
  * This program is free software; you can redistribute  it and/or modify it
@@ -141,9 +141,11 @@ static int ds3232_read_time(struct device *dev, struct rtc_time *time)
 		time->tm_hour = bcd2bin(hour);
 	}
 
-	time->tm_wday = bcd2bin(week);
+	/* Day of the week in linux range is 0~6 while 1~7 in RTC chip */
+	time->tm_wday = bcd2bin(week) - 1;
 	time->tm_mday = bcd2bin(day);
-	time->tm_mon = bcd2bin(month & 0x7F);
+	/* linux tm_mon range:0~11, while month range is 1~12 in RTC chip */
+	time->tm_mon = bcd2bin(month & 0x7F) - 1;
 	if (century)
 		add_century = 100;
 
@@ -162,9 +164,11 @@ static int ds3232_set_time(struct device *dev, struct rtc_time *time)
 	buf[0] = bin2bcd(time->tm_sec);
 	buf[1] = bin2bcd(time->tm_min);
 	buf[2] = bin2bcd(time->tm_hour);
-	buf[3] = bin2bcd(time->tm_wday); /* Day of the week */
+	/* Day of the week in linux range is 0~6 while 1~7 in RTC chip */
+	buf[3] = bin2bcd(time->tm_wday + 1);
 	buf[4] = bin2bcd(time->tm_mday); /* Date */
-	buf[5] = bin2bcd(time->tm_mon);
+	/* linux tm_mon range:0~11, while month range is 1~12 in RTC chip */
+	buf[5] = bin2bcd(time->tm_mon + 1);
 	if (time->tm_year >= 100) {
 		buf[5] |= 0x80;
 		buf[6] = bin2bcd(time->tm_year - 100);
diff --git a/drivers/rtc/rtc-mpc5121.c b/drivers/rtc/rtc-mpc5121.c
index dfcdf0901d21..2b952c654b14 100644
--- a/drivers/rtc/rtc-mpc5121.c
+++ b/drivers/rtc/rtc-mpc5121.c
@@ -268,8 +268,7 @@ static const struct rtc_class_ops mpc5121_rtc_ops = {
 	.update_irq_enable = mpc5121_rtc_update_irq_enable,
 };
 
-static int __devinit mpc5121_rtc_probe(struct platform_device *op,
-					const struct of_device_id *match)
+static int __devinit mpc5121_rtc_probe(struct platform_device *op)
 {
 	struct mpc5121_rtc_data *rtc;
 	int err = 0;
@@ -364,7 +363,7 @@ static struct of_device_id mpc5121_rtc_match[] __devinitdata = {
 	{},
 };
 
-static struct of_platform_driver mpc5121_rtc_driver = {
+static struct platform_driver mpc5121_rtc_driver = {
 	.driver = {
 		.name = "mpc5121-rtc",
 		.owner = THIS_MODULE,
@@ -376,13 +375,13 @@ static struct of_platform_driver mpc5121_rtc_driver = {
 
 static int __init mpc5121_rtc_init(void)
 {
-	return of_register_platform_driver(&mpc5121_rtc_driver);
+	return platform_driver_register(&mpc5121_rtc_driver);
 }
 module_init(mpc5121_rtc_init);
 
 static void __exit mpc5121_rtc_exit(void)
 {
-	of_unregister_platform_driver(&mpc5121_rtc_driver);
+	platform_driver_unregister(&mpc5121_rtc_driver);
 }
 module_exit(mpc5121_rtc_exit);
 
diff --git a/drivers/rtc/rtc-mrst.c b/drivers/rtc/rtc-mrst.c
index 1db62db8469d..8d2cf6027120 100644
--- a/drivers/rtc/rtc-mrst.c
+++ b/drivers/rtc/rtc-mrst.c
@@ -62,6 +62,17 @@ static inline int is_intr(u8 rtc_intr)
 	return rtc_intr & RTC_IRQMASK;
 }
 
+static inline unsigned char vrtc_is_updating(void)
+{
+	unsigned char uip;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rtc_lock, flags);
+	uip = (vrtc_cmos_read(RTC_FREQ_SELECT) & RTC_UIP);
+	spin_unlock_irqrestore(&rtc_lock, flags);
+	return uip;
+}
+
 /*
  * rtc_time's year contains the increment over 1900, but vRTC's YEAR
  * register can't be programmed to value larger than 0x64, so vRTC
@@ -76,7 +87,7 @@ static int mrst_read_time(struct device *dev, struct rtc_time *time)
 {
 	unsigned long flags;
 
-	if (rtc_is_updating())
+	if (vrtc_is_updating())
 		mdelay(20);
 
 	spin_lock_irqsave(&rtc_lock, flags);
diff --git a/drivers/rtc/rtc-pl030.c b/drivers/rtc/rtc-pl030.c
index bbdb2f02798a..baff724cd40f 100644
--- a/drivers/rtc/rtc-pl030.c
+++ b/drivers/rtc/rtc-pl030.c
@@ -103,7 +103,7 @@ static const struct rtc_class_ops pl030_ops = {
 	.set_alarm	= pl030_set_alarm,
 };
 
-static int pl030_probe(struct amba_device *dev, struct amba_id *id)
+static int pl030_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct pl030_rtc *rtc;
 	int ret;
diff --git a/drivers/rtc/rtc-pl031.c b/drivers/rtc/rtc-pl031.c
index b7a6690e5b35..6568f4b37e19 100644
--- a/drivers/rtc/rtc-pl031.c
+++ b/drivers/rtc/rtc-pl031.c
@@ -358,7 +358,7 @@ static int pl031_remove(struct amba_device *adev)
 	return 0;
 }
 
-static int pl031_probe(struct amba_device *adev, struct amba_id *id)
+static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	int ret;
 	struct pl031_local *ldata;
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index cf953ecbfca9..b80fa2882408 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -77,18 +77,20 @@ static irqreturn_t s3c_rtc_tickirq(int irq, void *id)
 }
 
 /* Update control registers */
-static void s3c_rtc_setaie(int to)
+static int s3c_rtc_setaie(struct device *dev, unsigned int enabled)
 {
 	unsigned int tmp;
 
-	pr_debug("%s: aie=%d\n", __func__, to);
+	pr_debug("%s: aie=%d\n", __func__, enabled);
 
 	tmp = readb(s3c_rtc_base + S3C2410_RTCALM) & ~S3C2410_RTCALM_ALMEN;
 
-	if (to)
+	if (enabled)
 		tmp |= S3C2410_RTCALM_ALMEN;
 
 	writeb(tmp, s3c_rtc_base + S3C2410_RTCALM);
+
+	return 0;
 }
 
 static int s3c_rtc_setpie(struct device *dev, int enabled)
@@ -308,7 +310,7 @@ static int s3c_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 
 	writeb(alrm_en, base + S3C2410_RTCALM);
 
-	s3c_rtc_setaie(alrm->enabled);
+	s3c_rtc_setaie(dev, alrm->enabled);
 
 	return 0;
 }
@@ -440,7 +442,7 @@ static int __devexit s3c_rtc_remove(struct platform_device *dev)
 	rtc_device_unregister(rtc);
 
 	s3c_rtc_setpie(&dev->dev, 0);
-	s3c_rtc_setaie(0);
+	s3c_rtc_setaie(&dev->dev, 0);
 
 	clk_disable(rtc_clk);
 	clk_put(rtc_clk);
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index a9fe23d5bd0f..379d8592bc6e 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -2648,6 +2648,7 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_tpm_track(
 		dasd_sfree_request(cqr, startdev);
 		return ERR_PTR(-EAGAIN);
 	}
+	len_to_track_end = 0;
 	/*
 	 * A tidaw can address 4k of memory, but must not cross page boundaries
 	 * We can let the block layer handle this by setting
diff --git a/drivers/s390/block/xpram.c b/drivers/s390/block/xpram.c
index c881a14fa5dd..1f6a4d894e73 100644
--- a/drivers/s390/block/xpram.c
+++ b/drivers/s390/block/xpram.c
@@ -62,8 +62,8 @@ static int xpram_devs;
 /*
  * Parameter parsing functions.
  */
-static int __initdata devs = XPRAM_DEVS;
-static char __initdata *sizes[XPRAM_MAX_DEVS];
+static int devs = XPRAM_DEVS;
+static char *sizes[XPRAM_MAX_DEVS];
 
 module_param(devs, int, 0);
 module_param_array(sizes, charp, NULL, 0);
diff --git a/drivers/s390/char/keyboard.c b/drivers/s390/char/keyboard.c
index d6673345c5f4..806588192483 100644
--- a/drivers/s390/char/keyboard.c
+++ b/drivers/s390/char/keyboard.c
@@ -458,7 +458,8 @@ do_kdgkb_ioctl(struct kbd_data *kbd, struct kbsentry __user *u_kbs,
 int kbd_ioctl(struct kbd_data *kbd, unsigned int cmd, unsigned long arg)
 {
 	void __user *argp;
-	int ct, perm;
+	unsigned int ct;
+	int perm;
 
 	argp = (void __user *)arg;
 
diff --git a/drivers/s390/char/tape.h b/drivers/s390/char/tape.h
index 7a242f073632..267b54e8ff5a 100644
--- a/drivers/s390/char/tape.h
+++ b/drivers/s390/char/tape.h
@@ -280,6 +280,14 @@ tape_do_io_free(struct tape_device *device, struct tape_request *request)
 	return rc;
 }
 
+static inline void
+tape_do_io_async_free(struct tape_device *device, struct tape_request *request)
+{
+	request->callback = (void *) tape_free_request;
+	request->callback_data = NULL;
+	tape_do_io_async(device, request);
+}
+
 extern int tape_oper_handler(int irq, int status);
 extern void tape_noper_handler(int irq, int status);
 extern int tape_open(struct tape_device *);
diff --git a/drivers/s390/char/tape_34xx.c b/drivers/s390/char/tape_34xx.c
index c17f35b6136a..c26511171ffe 100644
--- a/drivers/s390/char/tape_34xx.c
+++ b/drivers/s390/char/tape_34xx.c
@@ -53,23 +53,11 @@ static void tape_34xx_delete_sbid_from(struct tape_device *, int);
  * Medium sense for 34xx tapes. There is no 'real' medium sense call.
  * So we just do a normal sense.
  */
-static int
-tape_34xx_medium_sense(struct tape_device *device)
+static void __tape_34xx_medium_sense(struct tape_request *request)
 {
-	struct tape_request *request;
-	unsigned char       *sense;
-	int                  rc;
-
-	request = tape_alloc_request(1, 32);
-	if (IS_ERR(request)) {
-		DBF_EXCEPTION(6, "MSEN fail\n");
-		return PTR_ERR(request);
-	}
-
-	request->op = TO_MSEN;
-	tape_ccw_end(request->cpaddr, SENSE, 32, request->cpdata);
+	struct tape_device *device = request->device;
+	unsigned char *sense;
 
-	rc = tape_do_io_interruptible(device, request);
 	if (request->rc == 0) {
 		sense = request->cpdata;
 
@@ -88,15 +76,47 @@ tape_34xx_medium_sense(struct tape_device *device)
 			device->tape_generic_status |= GMT_WR_PROT(~0);
 		else
 			device->tape_generic_status &= ~GMT_WR_PROT(~0);
-	} else {
+	} else
 		DBF_EVENT(4, "tape_34xx: medium sense failed with rc=%d\n",
 			request->rc);
-	}
 	tape_free_request(request);
+}
+
+static int tape_34xx_medium_sense(struct tape_device *device)
+{
+	struct tape_request *request;
+	int rc;
+
+	request = tape_alloc_request(1, 32);
+	if (IS_ERR(request)) {
+		DBF_EXCEPTION(6, "MSEN fail\n");
+		return PTR_ERR(request);
+	}
 
+	request->op = TO_MSEN;
+	tape_ccw_end(request->cpaddr, SENSE, 32, request->cpdata);
+	rc = tape_do_io_interruptible(device, request);
+	__tape_34xx_medium_sense(request);
 	return rc;
 }
 
+static void tape_34xx_medium_sense_async(struct tape_device *device)
+{
+	struct tape_request *request;
+
+	request = tape_alloc_request(1, 32);
+	if (IS_ERR(request)) {
+		DBF_EXCEPTION(6, "MSEN fail\n");
+		return;
+	}
+
+	request->op = TO_MSEN;
+	tape_ccw_end(request->cpaddr, SENSE, 32, request->cpdata);
+	request->callback = (void *) __tape_34xx_medium_sense;
+	request->callback_data = NULL;
+	tape_do_io_async(device, request);
+}
+
 struct tape_34xx_work {
 	struct tape_device	*device;
 	enum tape_op		 op;
@@ -109,6 +129,9 @@ struct tape_34xx_work {
  * is inserted but cannot call tape_do_io* from an interrupt context.
  * Maybe that's useful for other actions we want to start from the
  * interrupt handler.
+ * Note: the work handler is called by the system work queue. The tape
+ * commands started by the handler need to be asynchrounous, otherwise
+ * a deadlock can occur e.g. in case of a deferred cc=1 (see __tape_do_irq).
  */
 static void
 tape_34xx_work_handler(struct work_struct *work)
@@ -119,7 +142,7 @@ tape_34xx_work_handler(struct work_struct *work)
 
 	switch(p->op) {
 		case TO_MSEN:
-			tape_34xx_medium_sense(device);
+			tape_34xx_medium_sense_async(device);
 			break;
 		default:
 			DBF_EVENT(3, "T34XX: internal error: unknown work\n");
diff --git a/drivers/s390/char/tape_3590.c b/drivers/s390/char/tape_3590.c
index fbe361fcd2c0..de2e99e0a71b 100644
--- a/drivers/s390/char/tape_3590.c
+++ b/drivers/s390/char/tape_3590.c
@@ -329,17 +329,17 @@ out:
 /*
  * Enable encryption
  */
-static int tape_3592_enable_crypt(struct tape_device *device)
+static struct tape_request *__tape_3592_enable_crypt(struct tape_device *device)
 {
 	struct tape_request *request;
 	char *data;
 
 	DBF_EVENT(6, "tape_3592_enable_crypt\n");
 	if (!crypt_supported(device))
-		return -ENOSYS;
+		return ERR_PTR(-ENOSYS);
 	request = tape_alloc_request(2, 72);
 	if (IS_ERR(request))
-		return PTR_ERR(request);
+		return request;
 	data = request->cpdata;
 	memset(data,0,72);
 
@@ -354,23 +354,42 @@ static int tape_3592_enable_crypt(struct tape_device *device)
 	request->op = TO_CRYPT_ON;
 	tape_ccw_cc(request->cpaddr, MODE_SET_CB, 36, data);
 	tape_ccw_end(request->cpaddr + 1, MODE_SET_CB, 36, data + 36);
+	return request;
+}
+
+static int tape_3592_enable_crypt(struct tape_device *device)
+{
+	struct tape_request *request;
+
+	request = __tape_3592_enable_crypt(device);
+	if (IS_ERR(request))
+		return PTR_ERR(request);
 	return tape_do_io_free(device, request);
 }
 
+static void tape_3592_enable_crypt_async(struct tape_device *device)
+{
+	struct tape_request *request;
+
+	request = __tape_3592_enable_crypt(device);
+	if (!IS_ERR(request))
+		tape_do_io_async_free(device, request);
+}
+
 /*
  * Disable encryption
  */
-static int tape_3592_disable_crypt(struct tape_device *device)
+static struct tape_request *__tape_3592_disable_crypt(struct tape_device *device)
 {
 	struct tape_request *request;
 	char *data;
 
 	DBF_EVENT(6, "tape_3592_disable_crypt\n");
 	if (!crypt_supported(device))
-		return -ENOSYS;
+		return ERR_PTR(-ENOSYS);
 	request = tape_alloc_request(2, 72);
 	if (IS_ERR(request))
-		return PTR_ERR(request);
+		return request;
 	data = request->cpdata;
 	memset(data,0,72);
 
@@ -383,9 +402,28 @@ static int tape_3592_disable_crypt(struct tape_device *device)
 	tape_ccw_cc(request->cpaddr, MODE_SET_CB, 36, data);
 	tape_ccw_end(request->cpaddr + 1, MODE_SET_CB, 36, data + 36);
 
+	return request;
+}
+
+static int tape_3592_disable_crypt(struct tape_device *device)
+{
+	struct tape_request *request;
+
+	request = __tape_3592_disable_crypt(device);
+	if (IS_ERR(request))
+		return PTR_ERR(request);
 	return tape_do_io_free(device, request);
 }
 
+static void tape_3592_disable_crypt_async(struct tape_device *device)
+{
+	struct tape_request *request;
+
+	request = __tape_3592_disable_crypt(device);
+	if (!IS_ERR(request))
+		tape_do_io_async_free(device, request);
+}
+
 /*
  * IOCTL: Set encryption status
  */
@@ -457,8 +495,7 @@ tape_3590_ioctl(struct tape_device *device, unsigned int cmd, unsigned long arg)
 /*
  * SENSE Medium: Get Sense data about medium state
  */
-static int
-tape_3590_sense_medium(struct tape_device *device)
+static int tape_3590_sense_medium(struct tape_device *device)
 {
 	struct tape_request *request;
 
@@ -470,6 +507,18 @@ tape_3590_sense_medium(struct tape_device *device)
 	return tape_do_io_free(device, request);
 }
 
+static void tape_3590_sense_medium_async(struct tape_device *device)
+{
+	struct tape_request *request;
+
+	request = tape_alloc_request(1, 128);
+	if (IS_ERR(request))
+		return;
+	request->op = TO_MSEN;
+	tape_ccw_end(request->cpaddr, MEDIUM_SENSE, 128, request->cpdata);
+	tape_do_io_async_free(device, request);
+}
+
 /*
  * MTTELL: Tell block. Return the number of block relative to current file.
  */
@@ -546,15 +595,14 @@ tape_3590_read_opposite(struct tape_device *device,
  * 2. The attention msg is written to the "read subsystem data" buffer.
  * In this case we probably should print it to the console.
  */
-static int
-tape_3590_read_attmsg(struct tape_device *device)
+static void tape_3590_read_attmsg_async(struct tape_device *device)
 {
 	struct tape_request *request;
 	char *buf;
 
 	request = tape_alloc_request(3, 4096);
 	if (IS_ERR(request))
-		return PTR_ERR(request);
+		return;
 	request->op = TO_READ_ATTMSG;
 	buf = request->cpdata;
 	buf[0] = PREP_RD_SS_DATA;
@@ -562,12 +610,15 @@ tape_3590_read_attmsg(struct tape_device *device)
 	tape_ccw_cc(request->cpaddr, PERFORM_SS_FUNC, 12, buf);
 	tape_ccw_cc(request->cpaddr + 1, READ_SS_DATA, 4096 - 12, buf + 12);
 	tape_ccw_end(request->cpaddr + 2, NOP, 0, NULL);
-	return tape_do_io_free(device, request);
+	tape_do_io_async_free(device, request);
 }
 
 /*
  * These functions are used to schedule follow-up actions from within an
  * interrupt context (like unsolicited interrupts).
+ * Note: the work handler is called by the system work queue. The tape
+ * commands started by the handler need to be asynchrounous, otherwise
+ * a deadlock can occur e.g. in case of a deferred cc=1 (see __tape_do_irq).
  */
 struct work_handler_data {
 	struct tape_device *device;
@@ -583,16 +634,16 @@ tape_3590_work_handler(struct work_struct *work)
 
 	switch (p->op) {
 	case TO_MSEN:
-		tape_3590_sense_medium(p->device);
+		tape_3590_sense_medium_async(p->device);
 		break;
 	case TO_READ_ATTMSG:
-		tape_3590_read_attmsg(p->device);
+		tape_3590_read_attmsg_async(p->device);
 		break;
 	case TO_CRYPT_ON:
-		tape_3592_enable_crypt(p->device);
+		tape_3592_enable_crypt_async(p->device);
 		break;
 	case TO_CRYPT_OFF:
-		tape_3592_disable_crypt(p->device);
+		tape_3592_disable_crypt_async(p->device);
 		break;
 	default:
 		DBF_EVENT(3, "T3590: work handler undefined for "
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 430f875006f2..f3147542e52e 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -630,11 +630,7 @@ void __irq_entry do_IRQ(struct pt_regs *regs)
 	irb = (struct irb *)&S390_lowcore.irb;
 	do {
 		kstat_cpu(smp_processor_id()).irqs[IO_INTERRUPT]++;
-		/*
-		 * Non I/O-subchannel thin interrupts are processed differently
-		 */
-		if (tpi_info->adapter_IO == 1 &&
-		    tpi_info->int_type == IO_INTERRUPT_TYPE) {
+		if (tpi_info->adapter_IO) {
 			do_adapter_IO(tpi_info->isc);
 			continue;
 		}
diff --git a/drivers/s390/cio/cio.h b/drivers/s390/cio/cio.h
index bf7f80f5a330..7a9032d01fb8 100644
--- a/drivers/s390/cio/cio.h
+++ b/drivers/s390/cio/cio.h
@@ -105,8 +105,6 @@ struct subchannel {
 	struct schib_config config;
 } __attribute__ ((aligned(8)));
 
-#define IO_INTERRUPT_TYPE	   0 /* I/O interrupt type */
-
 #define to_subchannel(n) container_of(n, struct subchannel, dev)
 
 extern int cio_validate_subchannel (struct subchannel *, struct subchannel_id);
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index f47a714538db..c5d763ed406e 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -741,7 +741,6 @@ struct qeth_card {
 	/* QDIO buffer handling */
 	struct qeth_qdio_info qdio;
 	struct qeth_perf_stats perf_stats;
-	int use_hard_stop;
 	int read_or_write_problem;
 	struct qeth_osn_info osn_info;
 	struct qeth_discipline discipline;
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 019ae58ab913..f3d98ac16e9f 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -302,12 +302,15 @@ static void qeth_issue_ipa_msg(struct qeth_ipa_cmd *cmd, int rc,
 	int com = cmd->hdr.command;
 	ipa_name = qeth_get_ipa_cmd_name(com);
 	if (rc)
-		QETH_DBF_MESSAGE(2, "IPA: %s(x%X) for %s returned x%X \"%s\"\n",
-				ipa_name, com, QETH_CARD_IFNAME(card),
-					rc, qeth_get_ipa_msg(rc));
+		QETH_DBF_MESSAGE(2, "IPA: %s(x%X) for %s/%s returned "
+				"x%X \"%s\"\n",
+				ipa_name, com, dev_name(&card->gdev->dev),
+				QETH_CARD_IFNAME(card), rc,
+				qeth_get_ipa_msg(rc));
 	else
-		QETH_DBF_MESSAGE(5, "IPA: %s(x%X) for %s succeeded\n",
-				ipa_name, com, QETH_CARD_IFNAME(card));
+		QETH_DBF_MESSAGE(5, "IPA: %s(x%X) for %s/%s succeeded\n",
+				ipa_name, com, dev_name(&card->gdev->dev),
+				QETH_CARD_IFNAME(card));
 }
 
 static struct qeth_ipa_cmd *qeth_check_ipa_data(struct qeth_card *card,
@@ -1083,7 +1086,6 @@ static int qeth_setup_card(struct qeth_card *card)
 	card->data.state  = CH_STATE_DOWN;
 	card->state = CARD_STATE_DOWN;
 	card->lan_online = 0;
-	card->use_hard_stop = 0;
 	card->read_or_write_problem = 0;
 	card->dev = NULL;
 	spin_lock_init(&card->vlanlock);
@@ -1732,20 +1734,22 @@ int qeth_send_control_data(struct qeth_card *card, int len,
 		};
 	}
 
+	if (reply->rc == -EIO)
+		goto error;
 	rc = reply->rc;
 	qeth_put_reply(reply);
 	return rc;
 
 time_err:
+	reply->rc = -ETIME;
 	spin_lock_irqsave(&reply->card->lock, flags);
 	list_del_init(&reply->list);
 	spin_unlock_irqrestore(&reply->card->lock, flags);
-	reply->rc = -ETIME;
 	atomic_inc(&reply->received);
+error:
 	atomic_set(&card->write.irq_pending, 0);
 	qeth_release_buffer(iob->channel, iob);
 	card->write.buf_no = (card->write.buf_no + 1) % QETH_CMD_BUFFER_NO;
-	wake_up(&reply->wait_q);
 	rc = reply->rc;
 	qeth_put_reply(reply);
 	return rc;
@@ -2490,45 +2494,19 @@ int qeth_send_ipa_cmd(struct qeth_card *card, struct qeth_cmd_buffer *iob,
 }
 EXPORT_SYMBOL_GPL(qeth_send_ipa_cmd);
 
-static int qeth_send_startstoplan(struct qeth_card *card,
-		enum qeth_ipa_cmds ipacmd, enum qeth_prot_versions prot)
-{
-	int rc;
-	struct qeth_cmd_buffer *iob;
-
-	iob = qeth_get_ipacmd_buffer(card, ipacmd, prot);
-	rc = qeth_send_ipa_cmd(card, iob, NULL, NULL);
-
-	return rc;
-}
-
 int qeth_send_startlan(struct qeth_card *card)
 {
 	int rc;
+	struct qeth_cmd_buffer *iob;
 
 	QETH_DBF_TEXT(SETUP, 2, "strtlan");
 
-	rc = qeth_send_startstoplan(card, IPA_CMD_STARTLAN, 0);
+	iob = qeth_get_ipacmd_buffer(card, IPA_CMD_STARTLAN, 0);
+	rc = qeth_send_ipa_cmd(card, iob, NULL, NULL);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(qeth_send_startlan);
 
-int qeth_send_stoplan(struct qeth_card *card)
-{
-	int rc = 0;
-
-	/*
-	 * TODO: according to the IPA format document page 14,
-	 * TCP/IP (we!) never issue a STOPLAN
-	 * is this right ?!?
-	 */
-	QETH_DBF_TEXT(SETUP, 2, "stoplan");
-
-	rc = qeth_send_startstoplan(card, IPA_CMD_STOPLAN, 0);
-	return rc;
-}
-EXPORT_SYMBOL_GPL(qeth_send_stoplan);
-
 int qeth_default_setadapterparms_cb(struct qeth_card *card,
 		struct qeth_reply *reply, unsigned long data)
 {
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index ada0fe782373..6fbaacb21943 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -202,17 +202,19 @@ static void qeth_l2_add_mc(struct qeth_card *card, __u8 *mac, int vmac)
 		kfree(mc);
 }
 
-static void qeth_l2_del_all_mc(struct qeth_card *card)
+static void qeth_l2_del_all_mc(struct qeth_card *card, int del)
 {
 	struct qeth_mc_mac *mc, *tmp;
 
 	spin_lock_bh(&card->mclock);
 	list_for_each_entry_safe(mc, tmp, &card->mc_list, list) {
-		if (mc->is_vmac)
-			qeth_l2_send_setdelmac(card, mc->mc_addr,
+		if (del) {
+			if (mc->is_vmac)
+				qeth_l2_send_setdelmac(card, mc->mc_addr,
 					IPA_CMD_DELVMAC, NULL);
-		else
-			qeth_l2_send_delgroupmac(card, mc->mc_addr);
+			else
+				qeth_l2_send_delgroupmac(card, mc->mc_addr);
+		}
 		list_del(&mc->list);
 		kfree(mc);
 	}
@@ -288,18 +290,13 @@ static int qeth_l2_send_setdelvlan(struct qeth_card *card, __u16 i,
 				 qeth_l2_send_setdelvlan_cb, NULL);
 }
 
-static void qeth_l2_process_vlans(struct qeth_card *card, int clear)
+static void qeth_l2_process_vlans(struct qeth_card *card)
 {
 	struct qeth_vlan_vid *id;
 	QETH_CARD_TEXT(card, 3, "L2prcvln");
 	spin_lock_bh(&card->vlanlock);
 	list_for_each_entry(id, &card->vid_list, list) {
-		if (clear)
-			qeth_l2_send_setdelvlan(card, id->vid,
-				IPA_CMD_DELVLAN);
-		else
-			qeth_l2_send_setdelvlan(card, id->vid,
-				IPA_CMD_SETVLAN);
+		qeth_l2_send_setdelvlan(card, id->vid, IPA_CMD_SETVLAN);
 	}
 	spin_unlock_bh(&card->vlanlock);
 }
@@ -379,19 +376,11 @@ static int qeth_l2_stop_card(struct qeth_card *card, int recovery_mode)
 			dev_close(card->dev);
 			rtnl_unlock();
 		}
-		if (!card->use_hard_stop ||
-			recovery_mode) {
-			__u8 *mac = &card->dev->dev_addr[0];
-			rc = qeth_l2_send_delmac(card, mac);
-			QETH_DBF_TEXT_(SETUP, 2, "Lerr%d", rc);
-		}
+		card->info.mac_bits &= ~QETH_LAYER2_MAC_REGISTERED;
 		card->state = CARD_STATE_SOFTSETUP;
 	}
 	if (card->state == CARD_STATE_SOFTSETUP) {
-		qeth_l2_process_vlans(card, 1);
-		if (!card->use_hard_stop ||
-			recovery_mode)
-			qeth_l2_del_all_mc(card);
+		qeth_l2_del_all_mc(card, 0);
 		qeth_clear_ipacmd_list(card);
 		card->state = CARD_STATE_HARDSETUP;
 	}
@@ -405,7 +394,6 @@ static int qeth_l2_stop_card(struct qeth_card *card, int recovery_mode)
 		qeth_clear_cmd_buffers(&card->read);
 		qeth_clear_cmd_buffers(&card->write);
 	}
-	card->use_hard_stop = 0;
 	return rc;
 }
 
@@ -705,7 +693,7 @@ static void qeth_l2_set_multicast_list(struct net_device *dev)
 	if (qeth_threads_running(card, QETH_RECOVER_THREAD) &&
 	    (card->state != CARD_STATE_UP))
 		return;
-	qeth_l2_del_all_mc(card);
+	qeth_l2_del_all_mc(card, 1);
 	spin_lock_bh(&card->mclock);
 	netdev_for_each_mc_addr(ha, dev)
 		qeth_l2_add_mc(card, ha->addr, 0);
@@ -907,10 +895,8 @@ static void qeth_l2_remove_device(struct ccwgroup_device *cgdev)
 	qeth_set_allowed_threads(card, 0, 1);
 	wait_event(card->wait_q, qeth_threads_running(card, 0xffffffff) == 0);
 
-	if (cgdev->state == CCWGROUP_ONLINE) {
-		card->use_hard_stop = 1;
+	if (cgdev->state == CCWGROUP_ONLINE)
 		qeth_l2_set_offline(cgdev);
-	}
 
 	if (card->dev) {
 		unregister_netdev(card->dev);
@@ -1040,7 +1026,7 @@ contin:
 
 	if (card->info.type != QETH_CARD_TYPE_OSN &&
 	    card->info.type != QETH_CARD_TYPE_OSM)
-		qeth_l2_process_vlans(card, 0);
+		qeth_l2_process_vlans(card);
 
 	netif_tx_disable(card->dev);
 
@@ -1076,7 +1062,6 @@ contin:
 	return 0;
 
 out_remove:
-	card->use_hard_stop = 1;
 	qeth_l2_stop_card(card, 0);
 	ccw_device_set_offline(CARD_DDEV(card));
 	ccw_device_set_offline(CARD_WDEV(card));
@@ -1144,7 +1129,6 @@ static int qeth_l2_recover(void *ptr)
 	QETH_CARD_TEXT(card, 2, "recover2");
 	dev_warn(&card->gdev->dev,
 		"A recovery process has been started for the device\n");
-	card->use_hard_stop = 1;
 	__qeth_l2_set_offline(card->gdev, 1);
 	rc = __qeth_l2_set_online(card->gdev, 1);
 	if (!rc)
@@ -1191,7 +1175,6 @@ static int qeth_l2_pm_suspend(struct ccwgroup_device *gdev)
 	if (gdev->state == CCWGROUP_OFFLINE)
 		return 0;
 	if (card->state == CARD_STATE_UP) {
-		card->use_hard_stop = 1;
 		__qeth_l2_set_offline(card->gdev, 1);
 	} else
 		__qeth_l2_set_offline(card->gdev, 0);
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index d09b0c44fc3d..6a9cc58321a0 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -510,8 +510,7 @@ static void qeth_l3_set_ip_addr_list(struct qeth_card *card)
 	kfree(tbd_list);
 }
 
-static void qeth_l3_clear_ip_list(struct qeth_card *card, int clean,
-					int recover)
+static void qeth_l3_clear_ip_list(struct qeth_card *card, int recover)
 {
 	struct qeth_ipaddr *addr, *tmp;
 	unsigned long flags;
@@ -530,11 +529,6 @@ static void qeth_l3_clear_ip_list(struct qeth_card *card, int clean,
 		addr = list_entry(card->ip_list.next,
 				  struct qeth_ipaddr, entry);
 		list_del_init(&addr->entry);
-		if (clean) {
-			spin_unlock_irqrestore(&card->ip_lock, flags);
-			qeth_l3_deregister_addr_entry(card, addr);
-			spin_lock_irqsave(&card->ip_lock, flags);
-		}
 		if (!recover || addr->is_multicast) {
 			kfree(addr);
 			continue;
@@ -1611,29 +1605,6 @@ static int qeth_l3_start_ipassists(struct qeth_card *card)
 	return 0;
 }
 
-static int qeth_l3_put_unique_id(struct qeth_card *card)
-{
-
-	int rc = 0;
-	struct qeth_cmd_buffer *iob;
-	struct qeth_ipa_cmd *cmd;
-
-	QETH_CARD_TEXT(card, 2, "puniqeid");
-
-	if ((card->info.unique_id & UNIQUE_ID_NOT_BY_CARD) ==
-		UNIQUE_ID_NOT_BY_CARD)
-		return -1;
-	iob = qeth_get_ipacmd_buffer(card, IPA_CMD_DESTROY_ADDR,
-				     QETH_PROT_IPV6);
-	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
-	*((__u16 *) &cmd->data.create_destroy_addr.unique_id[6]) =
-				card->info.unique_id;
-	memcpy(&cmd->data.create_destroy_addr.unique_id[0],
-	       card->dev->dev_addr, OSA_ADDR_LEN);
-	rc = qeth_send_ipa_cmd(card, iob, NULL, NULL);
-	return rc;
-}
-
 static int qeth_l3_iqd_read_initial_mac_cb(struct qeth_card *card,
 		struct qeth_reply *reply, unsigned long data)
 {
@@ -2324,25 +2295,14 @@ static int qeth_l3_stop_card(struct qeth_card *card, int recovery_mode)
 			dev_close(card->dev);
 			rtnl_unlock();
 		}
-		if (!card->use_hard_stop) {
-			rc = qeth_send_stoplan(card);
-			if (rc)
-				QETH_DBF_TEXT_(SETUP, 2, "1err%d", rc);
-		}
 		card->state = CARD_STATE_SOFTSETUP;
 	}
 	if (card->state == CARD_STATE_SOFTSETUP) {
-		qeth_l3_clear_ip_list(card, !card->use_hard_stop, 1);
+		qeth_l3_clear_ip_list(card, 1);
 		qeth_clear_ipacmd_list(card);
 		card->state = CARD_STATE_HARDSETUP;
 	}
 	if (card->state == CARD_STATE_HARDSETUP) {
-		if (!card->use_hard_stop &&
-		    (card->info.type != QETH_CARD_TYPE_IQD)) {
-			rc = qeth_l3_put_unique_id(card);
-			if (rc)
-				QETH_DBF_TEXT_(SETUP, 2, "2err%d", rc);
-		}
 		qeth_qdio_clear_card(card, 0);
 		qeth_clear_qdio_buffers(card);
 		qeth_clear_working_pool_list(card);
@@ -2352,7 +2312,6 @@ static int qeth_l3_stop_card(struct qeth_card *card, int recovery_mode)
 		qeth_clear_cmd_buffers(&card->read);
 		qeth_clear_cmd_buffers(&card->write);
 	}
-	card->use_hard_stop = 0;
 	return rc;
 }
 
@@ -3483,17 +3442,15 @@ static void qeth_l3_remove_device(struct ccwgroup_device *cgdev)
 	qeth_set_allowed_threads(card, 0, 1);
 	wait_event(card->wait_q, qeth_threads_running(card, 0xffffffff) == 0);
 
-	if (cgdev->state == CCWGROUP_ONLINE) {
-		card->use_hard_stop = 1;
+	if (cgdev->state == CCWGROUP_ONLINE)
 		qeth_l3_set_offline(cgdev);
-	}
 
 	if (card->dev) {
 		unregister_netdev(card->dev);
 		card->dev = NULL;
 	}
 
-	qeth_l3_clear_ip_list(card, 0, 0);
+	qeth_l3_clear_ip_list(card, 0);
 	qeth_l3_clear_ipato_list(card);
 	return;
 }
@@ -3594,7 +3551,6 @@ contin:
 	mutex_unlock(&card->discipline_mutex);
 	return 0;
 out_remove:
-	card->use_hard_stop = 1;
 	qeth_l3_stop_card(card, 0);
 	ccw_device_set_offline(CARD_DDEV(card));
 	ccw_device_set_offline(CARD_WDEV(card));
@@ -3663,7 +3619,6 @@ static int qeth_l3_recover(void *ptr)
 	QETH_CARD_TEXT(card, 2, "recover2");
 	dev_warn(&card->gdev->dev,
 		"A recovery process has been started for the device\n");
-	card->use_hard_stop = 1;
 	__qeth_l3_set_offline(card->gdev, 1);
 	rc = __qeth_l3_set_online(card->gdev, 1);
 	if (!rc)
@@ -3684,7 +3639,6 @@ static int qeth_l3_recover(void *ptr)
 static void qeth_l3_shutdown(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
-	qeth_l3_clear_ip_list(card, 0, 0);
 	qeth_qdio_clear_card(card, 0);
 	qeth_clear_qdio_buffers(card);
 }
@@ -3700,7 +3654,6 @@ static int qeth_l3_pm_suspend(struct ccwgroup_device *gdev)
 	if (gdev->state == CCWGROUP_OFFLINE)
 		return 0;
 	if (card->state == CARD_STATE_UP) {
-		card->use_hard_stop = 1;
 		__qeth_l3_set_offline(card->gdev, 1);
 	} else
 		__qeth_l3_set_offline(card->gdev, 0);
diff --git a/drivers/s390/scsi/zfcp_aux.c b/drivers/s390/scsi/zfcp_aux.c
index 51c666fb67a4..645b0fcbb370 100644
--- a/drivers/s390/scsi/zfcp_aux.c
+++ b/drivers/s390/scsi/zfcp_aux.c
@@ -122,36 +122,21 @@ static int __init zfcp_module_init(void)
 {
 	int retval = -ENOMEM;
 
-	zfcp_data.gpn_ft_cache = zfcp_cache_hw_align("zfcp_gpn",
-					sizeof(struct zfcp_fc_gpn_ft_req));
-	if (!zfcp_data.gpn_ft_cache)
-		goto out;
-
-	zfcp_data.qtcb_cache = zfcp_cache_hw_align("zfcp_qtcb",
-					sizeof(struct fsf_qtcb));
-	if (!zfcp_data.qtcb_cache)
+	zfcp_fsf_qtcb_cache = zfcp_cache_hw_align("zfcp_fsf_qtcb",
+						  sizeof(struct fsf_qtcb));
+	if (!zfcp_fsf_qtcb_cache)
 		goto out_qtcb_cache;
 
-	zfcp_data.sr_buffer_cache = zfcp_cache_hw_align("zfcp_sr",
-					sizeof(struct fsf_status_read_buffer));
-	if (!zfcp_data.sr_buffer_cache)
-		goto out_sr_cache;
+	zfcp_fc_req_cache = zfcp_cache_hw_align("zfcp_fc_req",
+						sizeof(struct zfcp_fc_req));
+	if (!zfcp_fc_req_cache)
+		goto out_fc_cache;
 
-	zfcp_data.gid_pn_cache = zfcp_cache_hw_align("zfcp_gid",
-					sizeof(struct zfcp_fc_gid_pn));
-	if (!zfcp_data.gid_pn_cache)
-		goto out_gid_cache;
-
-	zfcp_data.adisc_cache = zfcp_cache_hw_align("zfcp_adisc",
-					sizeof(struct zfcp_fc_els_adisc));
-	if (!zfcp_data.adisc_cache)
-		goto out_adisc_cache;
-
-	zfcp_data.scsi_transport_template =
+	zfcp_scsi_transport_template =
 		fc_attach_transport(&zfcp_transport_functions);
-	if (!zfcp_data.scsi_transport_template)
+	if (!zfcp_scsi_transport_template)
 		goto out_transport;
-	scsi_transport_reserve_device(zfcp_data.scsi_transport_template,
+	scsi_transport_reserve_device(zfcp_scsi_transport_template,
 				      sizeof(struct zfcp_scsi_dev));
 
 
@@ -175,18 +160,12 @@ static int __init zfcp_module_init(void)
 out_ccw_register:
 	misc_deregister(&zfcp_cfdc_misc);
 out_misc:
-	fc_release_transport(zfcp_data.scsi_transport_template);
+	fc_release_transport(zfcp_scsi_transport_template);
 out_transport:
-	kmem_cache_destroy(zfcp_data.adisc_cache);
-out_adisc_cache:
-	kmem_cache_destroy(zfcp_data.gid_pn_cache);
-out_gid_cache:
-	kmem_cache_destroy(zfcp_data.sr_buffer_cache);
-out_sr_cache:
-	kmem_cache_destroy(zfcp_data.qtcb_cache);
+	kmem_cache_destroy(zfcp_fc_req_cache);
+out_fc_cache:
+	kmem_cache_destroy(zfcp_fsf_qtcb_cache);
 out_qtcb_cache:
-	kmem_cache_destroy(zfcp_data.gpn_ft_cache);
-out:
 	return retval;
 }
 
@@ -196,12 +175,9 @@ static void __exit zfcp_module_exit(void)
 {
 	ccw_driver_unregister(&zfcp_ccw_driver);
 	misc_deregister(&zfcp_cfdc_misc);
-	fc_release_transport(zfcp_data.scsi_transport_template);
-	kmem_cache_destroy(zfcp_data.adisc_cache);
-	kmem_cache_destroy(zfcp_data.gid_pn_cache);
-	kmem_cache_destroy(zfcp_data.sr_buffer_cache);
-	kmem_cache_destroy(zfcp_data.qtcb_cache);
-	kmem_cache_destroy(zfcp_data.gpn_ft_cache);
+	fc_release_transport(zfcp_scsi_transport_template);
+	kmem_cache_destroy(zfcp_fc_req_cache);
+	kmem_cache_destroy(zfcp_fsf_qtcb_cache);
 }
 
 module_exit(zfcp_module_exit);
@@ -260,18 +236,18 @@ static int zfcp_allocate_low_mem_buffers(struct zfcp_adapter *adapter)
 		return -ENOMEM;
 
 	adapter->pool.qtcb_pool =
-		mempool_create_slab_pool(4, zfcp_data.qtcb_cache);
+		mempool_create_slab_pool(4, zfcp_fsf_qtcb_cache);
 	if (!adapter->pool.qtcb_pool)
 		return -ENOMEM;
 
-	adapter->pool.status_read_data =
-		mempool_create_slab_pool(FSF_STATUS_READS_RECOM,
-					 zfcp_data.sr_buffer_cache);
-	if (!adapter->pool.status_read_data)
+	BUILD_BUG_ON(sizeof(struct fsf_status_read_buffer) > PAGE_SIZE);
+	adapter->pool.sr_data =
+		mempool_create_page_pool(FSF_STATUS_READS_RECOM, 0);
+	if (!adapter->pool.sr_data)
 		return -ENOMEM;
 
 	adapter->pool.gid_pn =
-		mempool_create_slab_pool(1, zfcp_data.gid_pn_cache);
+		mempool_create_slab_pool(1, zfcp_fc_req_cache);
 	if (!adapter->pool.gid_pn)
 		return -ENOMEM;
 
@@ -290,8 +266,8 @@ static void zfcp_free_low_mem_buffers(struct zfcp_adapter *adapter)
 		mempool_destroy(adapter->pool.qtcb_pool);
 	if (adapter->pool.status_read_req)
 		mempool_destroy(adapter->pool.status_read_req);
-	if (adapter->pool.status_read_data)
-		mempool_destroy(adapter->pool.status_read_data);
+	if (adapter->pool.sr_data)
+		mempool_destroy(adapter->pool.sr_data);
 	if (adapter->pool.gid_pn)
 		mempool_destroy(adapter->pool.gid_pn);
 }
@@ -386,6 +362,7 @@ struct zfcp_adapter *zfcp_adapter_enqueue(struct ccw_device *ccw_device)
 
 	INIT_WORK(&adapter->stat_work, _zfcp_status_read_scheduler);
 	INIT_WORK(&adapter->scan_work, zfcp_fc_scan_ports);
+	INIT_WORK(&adapter->ns_up_work, zfcp_fc_sym_name_update);
 
 	if (zfcp_qdio_setup(adapter))
 		goto failed;
@@ -437,7 +414,7 @@ struct zfcp_adapter *zfcp_adapter_enqueue(struct ccw_device *ccw_device)
 	adapter->dma_parms.max_segment_size = ZFCP_QDIO_SBALE_LEN;
 	adapter->ccw_device->dev.dma_parms = &adapter->dma_parms;
 
-	if (!zfcp_adapter_scsi_register(adapter))
+	if (!zfcp_scsi_adapter_register(adapter))
 		return adapter;
 
 failed:
@@ -451,10 +428,11 @@ void zfcp_adapter_unregister(struct zfcp_adapter *adapter)
 
 	cancel_work_sync(&adapter->scan_work);
 	cancel_work_sync(&adapter->stat_work);
+	cancel_work_sync(&adapter->ns_up_work);
 	zfcp_destroy_adapter_work_queue(adapter);
 
 	zfcp_fc_wka_ports_force_offline(adapter->gs);
-	zfcp_adapter_scsi_unregister(adapter);
+	zfcp_scsi_adapter_unregister(adapter);
 	sysfs_remove_group(&cdev->dev.kobj, &zfcp_sysfs_adapter_attrs);
 
 	zfcp_erp_thread_kill(adapter);
diff --git a/drivers/s390/scsi/zfcp_def.h b/drivers/s390/scsi/zfcp_def.h
index 9ae1d0a6f627..527ba48eea57 100644
--- a/drivers/s390/scsi/zfcp_def.h
+++ b/drivers/s390/scsi/zfcp_def.h
@@ -89,7 +89,6 @@ struct zfcp_reqlist;
 #define ZFCP_STATUS_LUN_READONLY		0x00000008
 
 /* FSF request status (this does not have a common part) */
-#define ZFCP_STATUS_FSFREQ_TASK_MANAGEMENT	0x00000002
 #define ZFCP_STATUS_FSFREQ_ERROR		0x00000008
 #define ZFCP_STATUS_FSFREQ_CLEANUP		0x00000010
 #define ZFCP_STATUS_FSFREQ_ABORTSUCCEEDED	0x00000040
@@ -108,7 +107,7 @@ struct zfcp_adapter_mempool {
 	mempool_t *scsi_req;
 	mempool_t *scsi_abort;
 	mempool_t *status_read_req;
-	mempool_t *status_read_data;
+	mempool_t *sr_data;
 	mempool_t *gid_pn;
 	mempool_t *qtcb_pool;
 };
@@ -190,6 +189,7 @@ struct zfcp_adapter {
 	struct fsf_qtcb_bottom_port *stats_reset_data;
 	unsigned long		stats_reset;
 	struct work_struct	scan_work;
+	struct work_struct	ns_up_work;
 	struct service_level	service_level;
 	struct workqueue_struct	*work_queue;
 	struct device_dma_parameters dma_parms;
@@ -314,15 +314,4 @@ struct zfcp_fsf_req {
 	void			(*handler)(struct zfcp_fsf_req *);
 };
 
-/* driver data */
-struct zfcp_data {
-	struct scsi_host_template scsi_host_template;
-	struct scsi_transport_template *scsi_transport_template;
-	struct kmem_cache	*gpn_ft_cache;
-	struct kmem_cache	*qtcb_cache;
-	struct kmem_cache	*sr_buffer_cache;
-	struct kmem_cache	*gid_pn_cache;
-	struct kmem_cache	*adisc_cache;
-};
-
 #endif /* ZFCP_DEF_H */
diff --git a/drivers/s390/scsi/zfcp_erp.c b/drivers/s390/scsi/zfcp_erp.c
index e003e306f870..e1b4f800e226 100644
--- a/drivers/s390/scsi/zfcp_erp.c
+++ b/drivers/s390/scsi/zfcp_erp.c
@@ -732,7 +732,7 @@ static int zfcp_erp_adapter_strategy_open_fsf(struct zfcp_erp_action *act)
 	if (zfcp_erp_adapter_strategy_open_fsf_xport(act) == ZFCP_ERP_FAILED)
 		return ZFCP_ERP_FAILED;
 
-	if (mempool_resize(act->adapter->pool.status_read_data,
+	if (mempool_resize(act->adapter->pool.sr_data,
 			   act->adapter->stat_read_buf_num, GFP_KERNEL))
 		return ZFCP_ERP_FAILED;
 
@@ -1231,8 +1231,10 @@ static void zfcp_erp_action_cleanup(struct zfcp_erp_action *act, int result)
 		if (result == ZFCP_ERP_SUCCEEDED) {
 			register_service_level(&adapter->service_level);
 			queue_work(adapter->work_queue, &adapter->scan_work);
+			queue_work(adapter->work_queue, &adapter->ns_up_work);
 		} else
 			unregister_service_level(&adapter->service_level);
+
 		kref_put(&adapter->ref, zfcp_adapter_release);
 		break;
 	}
diff --git a/drivers/s390/scsi/zfcp_ext.h b/drivers/s390/scsi/zfcp_ext.h
index 6e325284fbe7..03627cfd81cd 100644
--- a/drivers/s390/scsi/zfcp_ext.h
+++ b/drivers/s390/scsi/zfcp_ext.h
@@ -80,6 +80,7 @@ extern void zfcp_erp_notify(struct zfcp_erp_action *, unsigned long);
 extern void zfcp_erp_timeout_handler(unsigned long);
 
 /* zfcp_fc.c */
+extern struct kmem_cache *zfcp_fc_req_cache;
 extern void zfcp_fc_enqueue_event(struct zfcp_adapter *,
 				enum fc_host_event_code event_code, u32);
 extern void zfcp_fc_post_event(struct work_struct *);
@@ -95,8 +96,10 @@ extern int zfcp_fc_gs_setup(struct zfcp_adapter *);
 extern void zfcp_fc_gs_destroy(struct zfcp_adapter *);
 extern int zfcp_fc_exec_bsg_job(struct fc_bsg_job *);
 extern int zfcp_fc_timeout_bsg_job(struct fc_bsg_job *);
+extern void zfcp_fc_sym_name_update(struct work_struct *);
 
 /* zfcp_fsf.c */
+extern struct kmem_cache *zfcp_fsf_qtcb_cache;
 extern int zfcp_fsf_open_port(struct zfcp_erp_action *);
 extern int zfcp_fsf_open_wka_port(struct zfcp_fc_wka_port *);
 extern int zfcp_fsf_close_wka_port(struct zfcp_fc_wka_port *);
@@ -139,9 +142,9 @@ extern struct zfcp_fsf_req *zfcp_fsf_get_req(struct zfcp_qdio *,
 					     struct qdio_buffer *);
 
 /* zfcp_scsi.c */
-extern struct zfcp_data zfcp_data;
-extern int zfcp_adapter_scsi_register(struct zfcp_adapter *);
-extern void zfcp_adapter_scsi_unregister(struct zfcp_adapter *);
+extern struct scsi_transport_template *zfcp_scsi_transport_template;
+extern int zfcp_scsi_adapter_register(struct zfcp_adapter *);
+extern void zfcp_scsi_adapter_unregister(struct zfcp_adapter *);
 extern struct fc_function_template zfcp_transport_functions;
 extern void zfcp_scsi_rport_work(struct work_struct *);
 extern void zfcp_scsi_schedule_rport_register(struct zfcp_port *);
diff --git a/drivers/s390/scsi/zfcp_fc.c b/drivers/s390/scsi/zfcp_fc.c
index 30cf91a787a3..297e6b71ce9c 100644
--- a/drivers/s390/scsi/zfcp_fc.c
+++ b/drivers/s390/scsi/zfcp_fc.c
@@ -11,11 +11,14 @@
 
 #include <linux/types.h>
 #include <linux/slab.h>
+#include <linux/utsname.h>
 #include <scsi/fc/fc_els.h>
 #include <scsi/libfc.h>
 #include "zfcp_ext.h"
 #include "zfcp_fc.h"
 
+struct kmem_cache *zfcp_fc_req_cache;
+
 static u32 zfcp_fc_rscn_range_mask[] = {
 	[ELS_ADDR_FMT_PORT]		= 0xFFFFFF,
 	[ELS_ADDR_FMT_AREA]		= 0xFFFF00,
@@ -260,24 +263,18 @@ void zfcp_fc_incoming_els(struct zfcp_fsf_req *fsf_req)
 		zfcp_fc_incoming_rscn(fsf_req);
 }
 
-static void zfcp_fc_ns_gid_pn_eval(void *data)
+static void zfcp_fc_ns_gid_pn_eval(struct zfcp_fc_req *fc_req)
 {
-	struct zfcp_fc_gid_pn *gid_pn = data;
-	struct zfcp_fsf_ct_els *ct = &gid_pn->ct;
-	struct zfcp_fc_gid_pn_req *gid_pn_req = sg_virt(ct->req);
-	struct zfcp_fc_gid_pn_resp *gid_pn_resp = sg_virt(ct->resp);
-	struct zfcp_port *port = gid_pn->port;
+	struct zfcp_fsf_ct_els *ct_els = &fc_req->ct_els;
+	struct zfcp_fc_gid_pn_rsp *gid_pn_rsp = &fc_req->u.gid_pn.rsp;
 
-	if (ct->status)
+	if (ct_els->status)
 		return;
-	if (gid_pn_resp->ct_hdr.ct_cmd != FC_FS_ACC)
+	if (gid_pn_rsp->ct_hdr.ct_cmd != FC_FS_ACC)
 		return;
 
-	/* paranoia */
-	if (gid_pn_req->gid_pn.fn_wwpn != port->wwpn)
-		return;
 	/* looks like a valid d_id */
-	port->d_id = ntoh24(gid_pn_resp->gid_pn.fp_fid);
+	ct_els->port->d_id = ntoh24(gid_pn_rsp->gid_pn.fp_fid);
 }
 
 static void zfcp_fc_complete(void *data)
@@ -285,69 +282,73 @@ static void zfcp_fc_complete(void *data)
 	complete(data);
 }
 
+static void zfcp_fc_ct_ns_init(struct fc_ct_hdr *ct_hdr, u16 cmd, u16 mr_size)
+{
+	ct_hdr->ct_rev = FC_CT_REV;
+	ct_hdr->ct_fs_type = FC_FST_DIR;
+	ct_hdr->ct_fs_subtype = FC_NS_SUBTYPE;
+	ct_hdr->ct_cmd = cmd;
+	ct_hdr->ct_mr_size = mr_size / 4;
+}
+
 static int zfcp_fc_ns_gid_pn_request(struct zfcp_port *port,
-				     struct zfcp_fc_gid_pn *gid_pn)
+				     struct zfcp_fc_req *fc_req)
 {
 	struct zfcp_adapter *adapter = port->adapter;
 	DECLARE_COMPLETION_ONSTACK(completion);
+	struct zfcp_fc_gid_pn_req *gid_pn_req = &fc_req->u.gid_pn.req;
+	struct zfcp_fc_gid_pn_rsp *gid_pn_rsp = &fc_req->u.gid_pn.rsp;
 	int ret;
 
 	/* setup parameters for send generic command */
-	gid_pn->port = port;
-	gid_pn->ct.handler = zfcp_fc_complete;
-	gid_pn->ct.handler_data = &completion;
-	gid_pn->ct.req = &gid_pn->sg_req;
-	gid_pn->ct.resp = &gid_pn->sg_resp;
-	sg_init_one(&gid_pn->sg_req, &gid_pn->gid_pn_req,
-		    sizeof(struct zfcp_fc_gid_pn_req));
-	sg_init_one(&gid_pn->sg_resp, &gid_pn->gid_pn_resp,
-		    sizeof(struct zfcp_fc_gid_pn_resp));
-
-	/* setup nameserver request */
-	gid_pn->gid_pn_req.ct_hdr.ct_rev = FC_CT_REV;
-	gid_pn->gid_pn_req.ct_hdr.ct_fs_type = FC_FST_DIR;
-	gid_pn->gid_pn_req.ct_hdr.ct_fs_subtype = FC_NS_SUBTYPE;
-	gid_pn->gid_pn_req.ct_hdr.ct_options = 0;
-	gid_pn->gid_pn_req.ct_hdr.ct_cmd = FC_NS_GID_PN;
-	gid_pn->gid_pn_req.ct_hdr.ct_mr_size = ZFCP_FC_CT_SIZE_PAGE / 4;
-	gid_pn->gid_pn_req.gid_pn.fn_wwpn = port->wwpn;
-
-	ret = zfcp_fsf_send_ct(&adapter->gs->ds, &gid_pn->ct,
+	fc_req->ct_els.port = port;
+	fc_req->ct_els.handler = zfcp_fc_complete;
+	fc_req->ct_els.handler_data = &completion;
+	fc_req->ct_els.req = &fc_req->sg_req;
+	fc_req->ct_els.resp = &fc_req->sg_rsp;
+	sg_init_one(&fc_req->sg_req, gid_pn_req, sizeof(*gid_pn_req));
+	sg_init_one(&fc_req->sg_rsp, gid_pn_rsp, sizeof(*gid_pn_rsp));
+
+	zfcp_fc_ct_ns_init(&gid_pn_req->ct_hdr,
+			   FC_NS_GID_PN, ZFCP_FC_CT_SIZE_PAGE);
+	gid_pn_req->gid_pn.fn_wwpn = port->wwpn;
+
+	ret = zfcp_fsf_send_ct(&adapter->gs->ds, &fc_req->ct_els,
 			       adapter->pool.gid_pn_req,
 			       ZFCP_FC_CTELS_TMO);
 	if (!ret) {
 		wait_for_completion(&completion);
-		zfcp_fc_ns_gid_pn_eval(gid_pn);
+		zfcp_fc_ns_gid_pn_eval(fc_req);
 	}
 	return ret;
 }
 
 /**
- * zfcp_fc_ns_gid_pn_request - initiate GID_PN nameserver request
+ * zfcp_fc_ns_gid_pn - initiate GID_PN nameserver request
  * @port: port where GID_PN request is needed
  * return: -ENOMEM on error, 0 otherwise
  */
 static int zfcp_fc_ns_gid_pn(struct zfcp_port *port)
 {
 	int ret;
-	struct zfcp_fc_gid_pn *gid_pn;
+	struct zfcp_fc_req *fc_req;
 	struct zfcp_adapter *adapter = port->adapter;
 
-	gid_pn = mempool_alloc(adapter->pool.gid_pn, GFP_ATOMIC);
-	if (!gid_pn)
+	fc_req = mempool_alloc(adapter->pool.gid_pn, GFP_ATOMIC);
+	if (!fc_req)
 		return -ENOMEM;
 
-	memset(gid_pn, 0, sizeof(*gid_pn));
+	memset(fc_req, 0, sizeof(*fc_req));
 
 	ret = zfcp_fc_wka_port_get(&adapter->gs->ds);
 	if (ret)
 		goto out;
 
-	ret = zfcp_fc_ns_gid_pn_request(port, gid_pn);
+	ret = zfcp_fc_ns_gid_pn_request(port, fc_req);
 
 	zfcp_fc_wka_port_put(&adapter->gs->ds);
 out:
-	mempool_free(gid_pn, adapter->pool.gid_pn);
+	mempool_free(fc_req, adapter->pool.gid_pn);
 	return ret;
 }
 
@@ -419,11 +420,11 @@ void zfcp_fc_plogi_evaluate(struct zfcp_port *port, struct fc_els_flogi *plogi)
 
 static void zfcp_fc_adisc_handler(void *data)
 {
-	struct zfcp_fc_els_adisc *adisc = data;
-	struct zfcp_port *port = adisc->els.port;
-	struct fc_els_adisc *adisc_resp = &adisc->adisc_resp;
+	struct zfcp_fc_req *fc_req = data;
+	struct zfcp_port *port = fc_req->ct_els.port;
+	struct fc_els_adisc *adisc_resp = &fc_req->u.adisc.rsp;
 
-	if (adisc->els.status) {
+	if (fc_req->ct_els.status) {
 		/* request rejected or timed out */
 		zfcp_erp_port_forced_reopen(port, ZFCP_STATUS_COMMON_ERP_FAILED,
 					    "fcadh_1");
@@ -445,42 +446,42 @@ static void zfcp_fc_adisc_handler(void *data)
  out:
 	atomic_clear_mask(ZFCP_STATUS_PORT_LINK_TEST, &port->status);
 	put_device(&port->dev);
-	kmem_cache_free(zfcp_data.adisc_cache, adisc);
+	kmem_cache_free(zfcp_fc_req_cache, fc_req);
 }
 
 static int zfcp_fc_adisc(struct zfcp_port *port)
 {
-	struct zfcp_fc_els_adisc *adisc;
+	struct zfcp_fc_req *fc_req;
 	struct zfcp_adapter *adapter = port->adapter;
+	struct Scsi_Host *shost = adapter->scsi_host;
 	int ret;
 
-	adisc = kmem_cache_zalloc(zfcp_data.adisc_cache, GFP_ATOMIC);
-	if (!adisc)
+	fc_req = kmem_cache_zalloc(zfcp_fc_req_cache, GFP_ATOMIC);
+	if (!fc_req)
 		return -ENOMEM;
 
-	adisc->els.port = port;
-	adisc->els.req = &adisc->req;
-	adisc->els.resp = &adisc->resp;
-	sg_init_one(adisc->els.req, &adisc->adisc_req,
+	fc_req->ct_els.port = port;
+	fc_req->ct_els.req = &fc_req->sg_req;
+	fc_req->ct_els.resp = &fc_req->sg_rsp;
+	sg_init_one(&fc_req->sg_req, &fc_req->u.adisc.req,
 		    sizeof(struct fc_els_adisc));
-	sg_init_one(adisc->els.resp, &adisc->adisc_resp,
+	sg_init_one(&fc_req->sg_rsp, &fc_req->u.adisc.rsp,
 		    sizeof(struct fc_els_adisc));
 
-	adisc->els.handler = zfcp_fc_adisc_handler;
-	adisc->els.handler_data = adisc;
+	fc_req->ct_els.handler = zfcp_fc_adisc_handler;
+	fc_req->ct_els.handler_data = fc_req;
 
 	/* acc. to FC-FS, hard_nport_id in ADISC should not be set for ports
 	   without FC-AL-2 capability, so we don't set it */
-	adisc->adisc_req.adisc_wwpn = fc_host_port_name(adapter->scsi_host);
-	adisc->adisc_req.adisc_wwnn = fc_host_node_name(adapter->scsi_host);
-	adisc->adisc_req.adisc_cmd = ELS_ADISC;
-	hton24(adisc->adisc_req.adisc_port_id,
-	       fc_host_port_id(adapter->scsi_host));
+	fc_req->u.adisc.req.adisc_wwpn = fc_host_port_name(shost);
+	fc_req->u.adisc.req.adisc_wwnn = fc_host_node_name(shost);
+	fc_req->u.adisc.req.adisc_cmd = ELS_ADISC;
+	hton24(fc_req->u.adisc.req.adisc_port_id, fc_host_port_id(shost));
 
-	ret = zfcp_fsf_send_els(adapter, port->d_id, &adisc->els,
+	ret = zfcp_fsf_send_els(adapter, port->d_id, &fc_req->ct_els,
 				ZFCP_FC_CTELS_TMO);
 	if (ret)
-		kmem_cache_free(zfcp_data.adisc_cache, adisc);
+		kmem_cache_free(zfcp_fc_req_cache, fc_req);
 
 	return ret;
 }
@@ -528,68 +529,42 @@ void zfcp_fc_test_link(struct zfcp_port *port)
 		put_device(&port->dev);
 }
 
-static void zfcp_free_sg_env(struct zfcp_fc_gpn_ft *gpn_ft, int buf_num)
+static struct zfcp_fc_req *zfcp_alloc_sg_env(int buf_num)
 {
-	struct scatterlist *sg = &gpn_ft->sg_req;
-
-	kmem_cache_free(zfcp_data.gpn_ft_cache, sg_virt(sg));
-	zfcp_sg_free_table(gpn_ft->sg_resp, buf_num);
-
-	kfree(gpn_ft);
-}
+	struct zfcp_fc_req *fc_req;
 
-static struct zfcp_fc_gpn_ft *zfcp_alloc_sg_env(int buf_num)
-{
-	struct zfcp_fc_gpn_ft *gpn_ft;
-	struct zfcp_fc_gpn_ft_req *req;
-
-	gpn_ft = kzalloc(sizeof(*gpn_ft), GFP_KERNEL);
-	if (!gpn_ft)
+	fc_req = kmem_cache_zalloc(zfcp_fc_req_cache, GFP_KERNEL);
+	if (!fc_req)
 		return NULL;
 
-	req = kmem_cache_zalloc(zfcp_data.gpn_ft_cache, GFP_KERNEL);
-	if (!req) {
-		kfree(gpn_ft);
-		gpn_ft = NULL;
-		goto out;
+	if (zfcp_sg_setup_table(&fc_req->sg_rsp, buf_num)) {
+		kmem_cache_free(zfcp_fc_req_cache, fc_req);
+		return NULL;
 	}
-	sg_init_one(&gpn_ft->sg_req, req, sizeof(*req));
 
-	if (zfcp_sg_setup_table(gpn_ft->sg_resp, buf_num)) {
-		zfcp_free_sg_env(gpn_ft, buf_num);
-		gpn_ft = NULL;
-	}
-out:
-	return gpn_ft;
-}
+	sg_init_one(&fc_req->sg_req, &fc_req->u.gpn_ft.req,
+		    sizeof(struct zfcp_fc_gpn_ft_req));
 
+	return fc_req;
+}
 
-static int zfcp_fc_send_gpn_ft(struct zfcp_fc_gpn_ft *gpn_ft,
+static int zfcp_fc_send_gpn_ft(struct zfcp_fc_req *fc_req,
 			       struct zfcp_adapter *adapter, int max_bytes)
 {
-	struct zfcp_fsf_ct_els *ct = &gpn_ft->ct;
-	struct zfcp_fc_gpn_ft_req *req = sg_virt(&gpn_ft->sg_req);
+	struct zfcp_fsf_ct_els *ct_els = &fc_req->ct_els;
+	struct zfcp_fc_gpn_ft_req *req = &fc_req->u.gpn_ft.req;
 	DECLARE_COMPLETION_ONSTACK(completion);
 	int ret;
 
-	/* prepare CT IU for GPN_FT */
-	req->ct_hdr.ct_rev = FC_CT_REV;
-	req->ct_hdr.ct_fs_type = FC_FST_DIR;
-	req->ct_hdr.ct_fs_subtype = FC_NS_SUBTYPE;
-	req->ct_hdr.ct_options = 0;
-	req->ct_hdr.ct_cmd = FC_NS_GPN_FT;
-	req->ct_hdr.ct_mr_size = max_bytes / 4;
-	req->gpn_ft.fn_domain_id_scope = 0;
-	req->gpn_ft.fn_area_id_scope = 0;
+	zfcp_fc_ct_ns_init(&req->ct_hdr, FC_NS_GPN_FT, max_bytes);
 	req->gpn_ft.fn_fc4_type = FC_TYPE_FCP;
 
-	/* prepare zfcp_send_ct */
-	ct->handler = zfcp_fc_complete;
-	ct->handler_data = &completion;
-	ct->req = &gpn_ft->sg_req;
-	ct->resp = gpn_ft->sg_resp;
+	ct_els->handler = zfcp_fc_complete;
+	ct_els->handler_data = &completion;
+	ct_els->req = &fc_req->sg_req;
+	ct_els->resp = &fc_req->sg_rsp;
 
-	ret = zfcp_fsf_send_ct(&adapter->gs->ds, ct, NULL,
+	ret = zfcp_fsf_send_ct(&adapter->gs->ds, ct_els, NULL,
 			       ZFCP_FC_CTELS_TMO);
 	if (!ret)
 		wait_for_completion(&completion);
@@ -610,11 +585,11 @@ static void zfcp_fc_validate_port(struct zfcp_port *port, struct list_head *lh)
 	list_move_tail(&port->list, lh);
 }
 
-static int zfcp_fc_eval_gpn_ft(struct zfcp_fc_gpn_ft *gpn_ft,
+static int zfcp_fc_eval_gpn_ft(struct zfcp_fc_req *fc_req,
 			       struct zfcp_adapter *adapter, int max_entries)
 {
-	struct zfcp_fsf_ct_els *ct = &gpn_ft->ct;
-	struct scatterlist *sg = gpn_ft->sg_resp;
+	struct zfcp_fsf_ct_els *ct_els = &fc_req->ct_els;
+	struct scatterlist *sg = &fc_req->sg_rsp;
 	struct fc_ct_hdr *hdr = sg_virt(sg);
 	struct fc_gpn_ft_resp *acc = sg_virt(sg);
 	struct zfcp_port *port, *tmp;
@@ -623,7 +598,7 @@ static int zfcp_fc_eval_gpn_ft(struct zfcp_fc_gpn_ft *gpn_ft,
 	u32 d_id;
 	int ret = 0, x, last = 0;
 
-	if (ct->status)
+	if (ct_els->status)
 		return -EIO;
 
 	if (hdr->ct_cmd != FC_FS_ACC) {
@@ -687,7 +662,7 @@ void zfcp_fc_scan_ports(struct work_struct *work)
 	struct zfcp_adapter *adapter = container_of(work, struct zfcp_adapter,
 						    scan_work);
 	int ret, i;
-	struct zfcp_fc_gpn_ft *gpn_ft;
+	struct zfcp_fc_req *fc_req;
 	int chain, max_entries, buf_num, max_bytes;
 
 	chain = adapter->adapter_features & FSF_FEATURE_ELS_CT_CHAINED_SBALS;
@@ -702,25 +677,145 @@ void zfcp_fc_scan_ports(struct work_struct *work)
 	if (zfcp_fc_wka_port_get(&adapter->gs->ds))
 		return;
 
-	gpn_ft = zfcp_alloc_sg_env(buf_num);
-	if (!gpn_ft)
+	fc_req = zfcp_alloc_sg_env(buf_num);
+	if (!fc_req)
 		goto out;
 
 	for (i = 0; i < 3; i++) {
-		ret = zfcp_fc_send_gpn_ft(gpn_ft, adapter, max_bytes);
+		ret = zfcp_fc_send_gpn_ft(fc_req, adapter, max_bytes);
 		if (!ret) {
-			ret = zfcp_fc_eval_gpn_ft(gpn_ft, adapter, max_entries);
+			ret = zfcp_fc_eval_gpn_ft(fc_req, adapter, max_entries);
 			if (ret == -EAGAIN)
 				ssleep(1);
 			else
 				break;
 		}
 	}
-	zfcp_free_sg_env(gpn_ft, buf_num);
+	zfcp_sg_free_table(&fc_req->sg_rsp, buf_num);
+	kmem_cache_free(zfcp_fc_req_cache, fc_req);
 out:
 	zfcp_fc_wka_port_put(&adapter->gs->ds);
 }
 
+static int zfcp_fc_gspn(struct zfcp_adapter *adapter,
+			struct zfcp_fc_req *fc_req)
+{
+	DECLARE_COMPLETION_ONSTACK(completion);
+	char devno[] = "DEVNO:";
+	struct zfcp_fsf_ct_els *ct_els = &fc_req->ct_els;
+	struct zfcp_fc_gspn_req *gspn_req = &fc_req->u.gspn.req;
+	struct zfcp_fc_gspn_rsp *gspn_rsp = &fc_req->u.gspn.rsp;
+	int ret;
+
+	zfcp_fc_ct_ns_init(&gspn_req->ct_hdr, FC_NS_GSPN_ID,
+			   FC_SYMBOLIC_NAME_SIZE);
+	hton24(gspn_req->gspn.fp_fid, fc_host_port_id(adapter->scsi_host));
+
+	sg_init_one(&fc_req->sg_req, gspn_req, sizeof(*gspn_req));
+	sg_init_one(&fc_req->sg_rsp, gspn_rsp, sizeof(*gspn_rsp));
+
+	ct_els->handler = zfcp_fc_complete;
+	ct_els->handler_data = &completion;
+	ct_els->req = &fc_req->sg_req;
+	ct_els->resp = &fc_req->sg_rsp;
+
+	ret = zfcp_fsf_send_ct(&adapter->gs->ds, ct_els, NULL,
+			       ZFCP_FC_CTELS_TMO);
+	if (ret)
+		return ret;
+
+	wait_for_completion(&completion);
+	if (ct_els->status)
+		return ct_els->status;
+
+	if (fc_host_port_type(adapter->scsi_host) == FC_PORTTYPE_NPIV &&
+	    !(strstr(gspn_rsp->gspn.fp_name, devno)))
+		snprintf(fc_host_symbolic_name(adapter->scsi_host),
+			 FC_SYMBOLIC_NAME_SIZE, "%s%s %s NAME: %s",
+			 gspn_rsp->gspn.fp_name, devno,
+			 dev_name(&adapter->ccw_device->dev),
+			 init_utsname()->nodename);
+	else
+		strlcpy(fc_host_symbolic_name(adapter->scsi_host),
+			gspn_rsp->gspn.fp_name, FC_SYMBOLIC_NAME_SIZE);
+
+	return 0;
+}
+
+static void zfcp_fc_rspn(struct zfcp_adapter *adapter,
+			 struct zfcp_fc_req *fc_req)
+{
+	DECLARE_COMPLETION_ONSTACK(completion);
+	struct Scsi_Host *shost = adapter->scsi_host;
+	struct zfcp_fsf_ct_els *ct_els = &fc_req->ct_els;
+	struct zfcp_fc_rspn_req *rspn_req = &fc_req->u.rspn.req;
+	struct fc_ct_hdr *rspn_rsp = &fc_req->u.rspn.rsp;
+	int ret, len;
+
+	zfcp_fc_ct_ns_init(&rspn_req->ct_hdr, FC_NS_RSPN_ID,
+			   FC_SYMBOLIC_NAME_SIZE);
+	hton24(rspn_req->rspn.fr_fid.fp_fid, fc_host_port_id(shost));
+	len = strlcpy(rspn_req->rspn.fr_name, fc_host_symbolic_name(shost),
+		      FC_SYMBOLIC_NAME_SIZE);
+	rspn_req->rspn.fr_name_len = len;
+
+	sg_init_one(&fc_req->sg_req, rspn_req, sizeof(*rspn_req));
+	sg_init_one(&fc_req->sg_rsp, rspn_rsp, sizeof(*rspn_rsp));
+
+	ct_els->handler = zfcp_fc_complete;
+	ct_els->handler_data = &completion;
+	ct_els->req = &fc_req->sg_req;
+	ct_els->resp = &fc_req->sg_rsp;
+
+	ret = zfcp_fsf_send_ct(&adapter->gs->ds, ct_els, NULL,
+			       ZFCP_FC_CTELS_TMO);
+	if (!ret)
+		wait_for_completion(&completion);
+}
+
+/**
+ * zfcp_fc_sym_name_update - Retrieve and update the symbolic port name
+ * @work: ns_up_work of the adapter where to update the symbolic port name
+ *
+ * Retrieve the current symbolic port name that may have been set by
+ * the hardware using the GSPN request and update the fc_host
+ * symbolic_name sysfs attribute. When running in NPIV mode (and hence
+ * the port name is unique for this system), update the symbolic port
+ * name to add Linux specific information and update the FC nameserver
+ * using the RSPN request.
+ */
+void zfcp_fc_sym_name_update(struct work_struct *work)
+{
+	struct zfcp_adapter *adapter = container_of(work, struct zfcp_adapter,
+						    ns_up_work);
+	int ret;
+	struct zfcp_fc_req *fc_req;
+
+	if (fc_host_port_type(adapter->scsi_host) != FC_PORTTYPE_NPORT &&
+	    fc_host_port_type(adapter->scsi_host) != FC_PORTTYPE_NPIV)
+		return;
+
+	fc_req = kmem_cache_zalloc(zfcp_fc_req_cache, GFP_KERNEL);
+	if (!fc_req)
+		return;
+
+	ret = zfcp_fc_wka_port_get(&adapter->gs->ds);
+	if (ret)
+		goto out_free;
+
+	ret = zfcp_fc_gspn(adapter, fc_req);
+	if (ret || fc_host_port_type(adapter->scsi_host) != FC_PORTTYPE_NPIV)
+		goto out_ds_put;
+
+	memset(fc_req, 0, sizeof(*fc_req));
+	zfcp_fc_rspn(adapter, fc_req);
+
+out_ds_put:
+	zfcp_fc_wka_port_put(&adapter->gs->ds);
+out_free:
+	kmem_cache_free(zfcp_fc_req_cache, fc_req);
+}
+
 static void zfcp_fc_ct_els_job_handler(void *data)
 {
 	struct fc_bsg_job *job = data;
diff --git a/drivers/s390/scsi/zfcp_fc.h b/drivers/s390/scsi/zfcp_fc.h
index b464ae01086c..4561f3bf7300 100644
--- a/drivers/s390/scsi/zfcp_fc.h
+++ b/drivers/s390/scsi/zfcp_fc.h
@@ -64,33 +64,16 @@ struct zfcp_fc_gid_pn_req {
 } __packed;
 
 /**
- * struct zfcp_fc_gid_pn_resp - container for ct header plus gid_pn response
+ * struct zfcp_fc_gid_pn_rsp - container for ct header plus gid_pn response
  * @ct_hdr: FC GS common transport header
  * @gid_pn: GID_PN response
  */
-struct zfcp_fc_gid_pn_resp {
+struct zfcp_fc_gid_pn_rsp {
 	struct fc_ct_hdr	ct_hdr;
 	struct fc_gid_pn_resp	gid_pn;
 } __packed;
 
 /**
- * struct zfcp_fc_gid_pn - everything required in zfcp for gid_pn request
- * @ct: data passed to zfcp_fsf for issuing fsf request
- * @sg_req: scatterlist entry for request data
- * @sg_resp: scatterlist entry for response data
- * @gid_pn_req: GID_PN request data
- * @gid_pn_resp: GID_PN response data
- */
-struct zfcp_fc_gid_pn {
-	struct zfcp_fsf_ct_els ct;
-	struct scatterlist sg_req;
-	struct scatterlist sg_resp;
-	struct zfcp_fc_gid_pn_req gid_pn_req;
-	struct zfcp_fc_gid_pn_resp gid_pn_resp;
-	struct zfcp_port *port;
-};
-
-/**
  * struct zfcp_fc_gpn_ft - container for ct header plus gpn_ft request
  * @ct_hdr: FC GS common transport header
  * @gpn_ft: GPN_FT request
@@ -101,41 +84,72 @@ struct zfcp_fc_gpn_ft_req {
 } __packed;
 
 /**
- * struct zfcp_fc_gpn_ft_resp - container for ct header plus gpn_ft response
+ * struct zfcp_fc_gspn_req - container for ct header plus GSPN_ID request
  * @ct_hdr: FC GS common transport header
- * @gpn_ft: Array of gpn_ft response data to fill one memory page
+ * @gspn: GSPN_ID request
  */
-struct zfcp_fc_gpn_ft_resp {
+struct zfcp_fc_gspn_req {
 	struct fc_ct_hdr	ct_hdr;
-	struct fc_gpn_ft_resp	gpn_ft[ZFCP_FC_GPN_FT_ENT_PAGE];
+	struct fc_gid_pn_resp	gspn;
 } __packed;
 
 /**
- * struct zfcp_fc_gpn_ft - zfcp data for gpn_ft request
- * @ct: data passed to zfcp_fsf for issuing fsf request
- * @sg_req: scatter list entry for gpn_ft request
- * @sg_resp: scatter list entries for gpn_ft responses (per memory page)
+ * struct zfcp_fc_gspn_rsp - container for ct header plus GSPN_ID response
+ * @ct_hdr: FC GS common transport header
+ * @gspn: GSPN_ID response
+ * @name: The name string of the GSPN_ID response
  */
-struct zfcp_fc_gpn_ft {
-	struct zfcp_fsf_ct_els ct;
-	struct scatterlist sg_req;
-	struct scatterlist sg_resp[ZFCP_FC_GPN_FT_NUM_BUFS];
-};
+struct zfcp_fc_gspn_rsp {
+	struct fc_ct_hdr	ct_hdr;
+	struct fc_gspn_resp	gspn;
+	char			name[FC_SYMBOLIC_NAME_SIZE];
+} __packed;
 
 /**
- * struct zfcp_fc_els_adisc - everything required in zfcp for issuing ELS ADISC
- * @els: data required for issuing els fsf command
- * @req: scatterlist entry for ELS ADISC request
- * @resp: scatterlist entry for ELS ADISC response
- * @adisc_req: ELS ADISC request data
- * @adisc_resp: ELS ADISC response data
+ * struct zfcp_fc_rspn_req - container for ct header plus RSPN_ID request
+ * @ct_hdr: FC GS common transport header
+ * @rspn: RSPN_ID request
+ * @name: The name string of the RSPN_ID request
  */
-struct zfcp_fc_els_adisc {
-	struct zfcp_fsf_ct_els els;
-	struct scatterlist req;
-	struct scatterlist resp;
-	struct fc_els_adisc adisc_req;
-	struct fc_els_adisc adisc_resp;
+struct zfcp_fc_rspn_req {
+	struct fc_ct_hdr	ct_hdr;
+	struct fc_ns_rspn	rspn;
+	char			name[FC_SYMBOLIC_NAME_SIZE];
+} __packed;
+
+/**
+ * struct zfcp_fc_req - Container for FC ELS and CT requests sent from zfcp
+ * @ct_els: data required for issuing fsf command
+ * @sg_req: scatterlist entry for request data
+ * @sg_rsp: scatterlist entry for response data
+ * @u: request specific data
+ */
+struct zfcp_fc_req {
+	struct zfcp_fsf_ct_els				ct_els;
+	struct scatterlist				sg_req;
+	struct scatterlist				sg_rsp;
+	union {
+		struct {
+			struct fc_els_adisc		req;
+			struct fc_els_adisc		rsp;
+		} adisc;
+		struct {
+			struct zfcp_fc_gid_pn_req	req;
+			struct zfcp_fc_gid_pn_rsp	rsp;
+		} gid_pn;
+		struct {
+			struct scatterlist sg_rsp2[ZFCP_FC_GPN_FT_NUM_BUFS - 1];
+			struct zfcp_fc_gpn_ft_req	req;
+		} gpn_ft;
+		struct {
+			struct zfcp_fc_gspn_req		req;
+			struct zfcp_fc_gspn_rsp		rsp;
+		} gspn;
+		struct {
+			struct zfcp_fc_rspn_req		req;
+			struct fc_ct_hdr		rsp;
+		} rspn;
+	} u;
 };
 
 /**
@@ -192,14 +206,21 @@ struct zfcp_fc_wka_ports {
  * zfcp_fc_scsi_to_fcp - setup FCP command with data from scsi_cmnd
  * @fcp: fcp_cmnd to setup
  * @scsi: scsi_cmnd where to get LUN, task attributes/flags and CDB
+ * @tm: task management flags to setup task management command
  */
 static inline
-void zfcp_fc_scsi_to_fcp(struct fcp_cmnd *fcp, struct scsi_cmnd *scsi)
+void zfcp_fc_scsi_to_fcp(struct fcp_cmnd *fcp, struct scsi_cmnd *scsi,
+			 u8 tm_flags)
 {
 	char tag[2];
 
 	int_to_scsilun(scsi->device->lun, (struct scsi_lun *) &fcp->fc_lun);
 
+	if (unlikely(tm_flags)) {
+		fcp->fc_tm_flags = tm_flags;
+		return;
+	}
+
 	if (scsi_populate_tag_msg(scsi, tag)) {
 		switch (tag[0]) {
 		case MSG_ORDERED_TAG:
@@ -226,19 +247,6 @@ void zfcp_fc_scsi_to_fcp(struct fcp_cmnd *fcp, struct scsi_cmnd *scsi)
 }
 
 /**
- * zfcp_fc_fcp_tm - setup FCP command as task management command
- * @fcp: fcp_cmnd to setup
- * @dev: scsi_device where to send the task management command
- * @tm: task management flags to setup tm command
- */
-static inline
-void zfcp_fc_fcp_tm(struct fcp_cmnd *fcp, struct scsi_device *dev, u8 tm_flags)
-{
-	int_to_scsilun(dev->lun, (struct scsi_lun *) &fcp->fc_lun);
-	fcp->fc_tm_flags |= tm_flags;
-}
-
-/**
  * zfcp_fc_evap_fcp_rsp - evaluate FCP RSP IU and update scsi_cmnd accordingly
  * @fcp_rsp: FCP RSP IU to evaluate
  * @scsi: SCSI command where to update status and sense buffer
diff --git a/drivers/s390/scsi/zfcp_fsf.c b/drivers/s390/scsi/zfcp_fsf.c
index 60ff9d172c79..a0e05ef65924 100644
--- a/drivers/s390/scsi/zfcp_fsf.c
+++ b/drivers/s390/scsi/zfcp_fsf.c
@@ -18,6 +18,8 @@
 #include "zfcp_qdio.h"
 #include "zfcp_reqlist.h"
 
+struct kmem_cache *zfcp_fsf_qtcb_cache;
+
 static void zfcp_fsf_request_timeout_handler(unsigned long data)
 {
 	struct zfcp_adapter *adapter = (struct zfcp_adapter *) data;
@@ -83,7 +85,7 @@ void zfcp_fsf_req_free(struct zfcp_fsf_req *req)
 	}
 
 	if (likely(req->qtcb))
-		kmem_cache_free(zfcp_data.qtcb_cache, req->qtcb);
+		kmem_cache_free(zfcp_fsf_qtcb_cache, req->qtcb);
 	kfree(req);
 }
 
@@ -212,7 +214,7 @@ static void zfcp_fsf_status_read_handler(struct zfcp_fsf_req *req)
 
 	if (req->status & ZFCP_STATUS_FSFREQ_DISMISSED) {
 		zfcp_dbf_hba_fsf_uss("fssrh_1", req);
-		mempool_free(sr_buf, adapter->pool.status_read_data);
+		mempool_free(virt_to_page(sr_buf), adapter->pool.sr_data);
 		zfcp_fsf_req_free(req);
 		return;
 	}
@@ -265,7 +267,7 @@ static void zfcp_fsf_status_read_handler(struct zfcp_fsf_req *req)
 		break;
 	}
 
-	mempool_free(sr_buf, adapter->pool.status_read_data);
+	mempool_free(virt_to_page(sr_buf), adapter->pool.sr_data);
 	zfcp_fsf_req_free(req);
 
 	atomic_inc(&adapter->stat_miss);
@@ -628,7 +630,7 @@ static struct fsf_qtcb *zfcp_qtcb_alloc(mempool_t *pool)
 	if (likely(pool))
 		qtcb = mempool_alloc(pool, GFP_ATOMIC);
 	else
-		qtcb = kmem_cache_alloc(zfcp_data.qtcb_cache, GFP_ATOMIC);
+		qtcb = kmem_cache_alloc(zfcp_fsf_qtcb_cache, GFP_ATOMIC);
 
 	if (unlikely(!qtcb))
 		return NULL;
@@ -723,6 +725,7 @@ int zfcp_fsf_status_read(struct zfcp_qdio *qdio)
 	struct zfcp_adapter *adapter = qdio->adapter;
 	struct zfcp_fsf_req *req;
 	struct fsf_status_read_buffer *sr_buf;
+	struct page *page;
 	int retval = -EIO;
 
 	spin_lock_irq(&qdio->req_q_lock);
@@ -736,11 +739,12 @@ int zfcp_fsf_status_read(struct zfcp_qdio *qdio)
 		goto out;
 	}
 
-	sr_buf = mempool_alloc(adapter->pool.status_read_data, GFP_ATOMIC);
-	if (!sr_buf) {
+	page = mempool_alloc(adapter->pool.sr_data, GFP_ATOMIC);
+	if (!page) {
 		retval = -ENOMEM;
 		goto failed_buf;
 	}
+	sr_buf = page_address(page);
 	memset(sr_buf, 0, sizeof(*sr_buf));
 	req->data = sr_buf;
 
@@ -755,7 +759,7 @@ int zfcp_fsf_status_read(struct zfcp_qdio *qdio)
 
 failed_req_send:
 	req->data = NULL;
-	mempool_free(sr_buf, adapter->pool.status_read_data);
+	mempool_free(virt_to_page(sr_buf), adapter->pool.sr_data);
 failed_buf:
 	zfcp_dbf_hba_fsf_uss("fssr__1", req);
 	zfcp_fsf_req_free(req);
@@ -1552,7 +1556,7 @@ int zfcp_fsf_open_wka_port(struct zfcp_fc_wka_port *wka_port)
 				  SBAL_FLAGS0_TYPE_READ,
 				  qdio->adapter->pool.erp_req);
 
-	if (unlikely(IS_ERR(req))) {
+	if (IS_ERR(req)) {
 		retval = PTR_ERR(req);
 		goto out;
 	}
@@ -1605,7 +1609,7 @@ int zfcp_fsf_close_wka_port(struct zfcp_fc_wka_port *wka_port)
 				  SBAL_FLAGS0_TYPE_READ,
 				  qdio->adapter->pool.erp_req);
 
-	if (unlikely(IS_ERR(req))) {
+	if (IS_ERR(req)) {
 		retval = PTR_ERR(req);
 		goto out;
 	}
@@ -2206,7 +2210,7 @@ int zfcp_fsf_fcp_cmnd(struct scsi_cmnd *scsi_cmnd)
 	zfcp_fsf_set_data_dir(scsi_cmnd, &io->data_direction);
 
 	fcp_cmnd = (struct fcp_cmnd *) &req->qtcb->bottom.io.fcp_cmnd;
-	zfcp_fc_scsi_to_fcp(fcp_cmnd, scsi_cmnd);
+	zfcp_fc_scsi_to_fcp(fcp_cmnd, scsi_cmnd, 0);
 
 	if (scsi_prot_sg_count(scsi_cmnd)) {
 		zfcp_qdio_set_data_div(qdio, &req->qdio_req,
@@ -2284,7 +2288,6 @@ struct zfcp_fsf_req *zfcp_fsf_fcp_task_mgmt(struct scsi_cmnd *scmnd,
 		goto out;
 	}
 
-	req->status |= ZFCP_STATUS_FSFREQ_TASK_MANAGEMENT;
 	req->data = scmnd;
 	req->handler = zfcp_fsf_fcp_task_mgmt_handler;
 	req->qtcb->header.lun_handle = zfcp_sdev->lun_handle;
@@ -2296,7 +2299,7 @@ struct zfcp_fsf_req *zfcp_fsf_fcp_task_mgmt(struct scsi_cmnd *scmnd,
 	zfcp_qdio_set_sbale_last(qdio, &req->qdio_req);
 
 	fcp_cmnd = (struct fcp_cmnd *) &req->qtcb->bottom.io.fcp_cmnd;
-	zfcp_fc_fcp_tm(fcp_cmnd, scmnd->device, tm_flags);
+	zfcp_fc_scsi_to_fcp(fcp_cmnd, scmnd, tm_flags);
 
 	zfcp_fsf_start_timer(req, ZFCP_SCSI_ER_TIMEOUT);
 	if (!zfcp_fsf_req_send(req))
diff --git a/drivers/s390/scsi/zfcp_scsi.c b/drivers/s390/scsi/zfcp_scsi.c
index ddb5800823a9..2a4991d6d4d5 100644
--- a/drivers/s390/scsi/zfcp_scsi.c
+++ b/drivers/s390/scsi/zfcp_scsi.c
@@ -292,7 +292,37 @@ static int zfcp_scsi_eh_host_reset_handler(struct scsi_cmnd *scpnt)
 	return SUCCESS;
 }
 
-int zfcp_adapter_scsi_register(struct zfcp_adapter *adapter)
+struct scsi_transport_template *zfcp_scsi_transport_template;
+
+static struct scsi_host_template zfcp_scsi_host_template = {
+	.module			 = THIS_MODULE,
+	.name			 = "zfcp",
+	.queuecommand		 = zfcp_scsi_queuecommand,
+	.eh_abort_handler	 = zfcp_scsi_eh_abort_handler,
+	.eh_device_reset_handler = zfcp_scsi_eh_device_reset_handler,
+	.eh_target_reset_handler = zfcp_scsi_eh_target_reset_handler,
+	.eh_host_reset_handler	 = zfcp_scsi_eh_host_reset_handler,
+	.slave_alloc		 = zfcp_scsi_slave_alloc,
+	.slave_configure	 = zfcp_scsi_slave_configure,
+	.slave_destroy		 = zfcp_scsi_slave_destroy,
+	.change_queue_depth	 = zfcp_scsi_change_queue_depth,
+	.proc_name		 = "zfcp",
+	.can_queue		 = 4096,
+	.this_id		 = -1,
+	.sg_tablesize		 = ZFCP_QDIO_MAX_SBALES_PER_REQ,
+	.max_sectors		 = (ZFCP_QDIO_MAX_SBALES_PER_REQ * 8),
+	.dma_boundary		 = ZFCP_QDIO_SBALE_LEN - 1,
+	.cmd_per_lun		 = 1,
+	.use_clustering		 = 1,
+	.shost_attrs		 = zfcp_sysfs_shost_attrs,
+	.sdev_attrs		 = zfcp_sysfs_sdev_attrs,
+};
+
+/**
+ * zfcp_scsi_adapter_register - Register SCSI and FC host with SCSI midlayer
+ * @adapter: The zfcp adapter to register with the SCSI midlayer
+ */
+int zfcp_scsi_adapter_register(struct zfcp_adapter *adapter)
 {
 	struct ccw_dev_id dev_id;
 
@@ -301,7 +331,7 @@ int zfcp_adapter_scsi_register(struct zfcp_adapter *adapter)
 
 	ccw_device_get_id(adapter->ccw_device, &dev_id);
 	/* register adapter as SCSI host with mid layer of SCSI stack */
-	adapter->scsi_host = scsi_host_alloc(&zfcp_data.scsi_host_template,
+	adapter->scsi_host = scsi_host_alloc(&zfcp_scsi_host_template,
 					     sizeof (struct zfcp_adapter *));
 	if (!adapter->scsi_host) {
 		dev_err(&adapter->ccw_device->dev,
@@ -316,7 +346,7 @@ int zfcp_adapter_scsi_register(struct zfcp_adapter *adapter)
 	adapter->scsi_host->max_channel = 0;
 	adapter->scsi_host->unique_id = dev_id.devno;
 	adapter->scsi_host->max_cmd_len = 16; /* in struct fcp_cmnd */
-	adapter->scsi_host->transportt = zfcp_data.scsi_transport_template;
+	adapter->scsi_host->transportt = zfcp_scsi_transport_template;
 
 	adapter->scsi_host->hostdata[0] = (unsigned long) adapter;
 
@@ -328,7 +358,11 @@ int zfcp_adapter_scsi_register(struct zfcp_adapter *adapter)
 	return 0;
 }
 
-void zfcp_adapter_scsi_unregister(struct zfcp_adapter *adapter)
+/**
+ * zfcp_scsi_adapter_unregister - Unregister SCSI and FC host from SCSI midlayer
+ * @adapter: The zfcp adapter to unregister.
+ */
+void zfcp_scsi_adapter_unregister(struct zfcp_adapter *adapter)
 {
 	struct Scsi_Host *shost;
 	struct zfcp_port *port;
@@ -346,8 +380,6 @@ void zfcp_adapter_scsi_unregister(struct zfcp_adapter *adapter)
 	scsi_remove_host(shost);
 	scsi_host_put(shost);
 	adapter->scsi_host = NULL;
-
-	return;
 }
 
 static struct fc_host_statistics*
@@ -688,33 +720,8 @@ struct fc_function_template zfcp_transport_functions = {
 	/* no functions registered for following dynamic attributes but
 	   directly set by LLDD */
 	.show_host_port_type = 1,
+	.show_host_symbolic_name = 1,
 	.show_host_speed = 1,
 	.show_host_port_id = 1,
 	.dd_bsg_size = sizeof(struct zfcp_fsf_ct_els),
 };
-
-struct zfcp_data zfcp_data = {
-	.scsi_host_template = {
-		.name			 = "zfcp",
-		.module			 = THIS_MODULE,
-		.proc_name		 = "zfcp",
-		.change_queue_depth	 = zfcp_scsi_change_queue_depth,
-		.slave_alloc		 = zfcp_scsi_slave_alloc,
-		.slave_configure	 = zfcp_scsi_slave_configure,
-		.slave_destroy		 = zfcp_scsi_slave_destroy,
-		.queuecommand		 = zfcp_scsi_queuecommand,
-		.eh_abort_handler	 = zfcp_scsi_eh_abort_handler,
-		.eh_device_reset_handler = zfcp_scsi_eh_device_reset_handler,
-		.eh_target_reset_handler = zfcp_scsi_eh_target_reset_handler,
-		.eh_host_reset_handler	 = zfcp_scsi_eh_host_reset_handler,
-		.can_queue		 = 4096,
-		.this_id		 = -1,
-		.sg_tablesize		 = ZFCP_QDIO_MAX_SBALES_PER_REQ,
-		.cmd_per_lun		 = 1,
-		.use_clustering		 = 1,
-		.sdev_attrs		 = zfcp_sysfs_sdev_attrs,
-		.max_sectors		 = (ZFCP_QDIO_MAX_SBALES_PER_REQ * 8),
-		.dma_boundary		 = ZFCP_QDIO_SBALE_LEN - 1,
-		.shost_attrs		 = zfcp_sysfs_shost_attrs,
-	},
-};
diff --git a/drivers/sbus/char/bbc_i2c.c b/drivers/sbus/char/bbc_i2c.c
index 614a5e114a19..5f94d22c491e 100644
--- a/drivers/sbus/char/bbc_i2c.c
+++ b/drivers/sbus/char/bbc_i2c.c
@@ -361,8 +361,7 @@ fail:
 extern int bbc_envctrl_init(struct bbc_i2c_bus *bp);
 extern void bbc_envctrl_cleanup(struct bbc_i2c_bus *bp);
 
-static int __devinit bbc_i2c_probe(struct platform_device *op,
-				   const struct of_device_id *match)
+static int __devinit bbc_i2c_probe(struct platform_device *op)
 {
 	struct bbc_i2c_bus *bp;
 	int err, index = 0;
@@ -413,7 +412,7 @@ static const struct of_device_id bbc_i2c_match[] = {
 };
 MODULE_DEVICE_TABLE(of, bbc_i2c_match);
 
-static struct of_platform_driver bbc_i2c_driver = {
+static struct platform_driver bbc_i2c_driver = {
 	.driver = {
 		.name = "bbc_i2c",
 		.owner = THIS_MODULE,
@@ -425,12 +424,12 @@ static struct of_platform_driver bbc_i2c_driver = {
 
 static int __init bbc_i2c_init(void)
 {
-	return of_register_platform_driver(&bbc_i2c_driver);
+	return platform_driver_register(&bbc_i2c_driver);
 }
 
 static void __exit bbc_i2c_exit(void)
 {
-	of_unregister_platform_driver(&bbc_i2c_driver);
+	platform_driver_unregister(&bbc_i2c_driver);
 }
 
 module_init(bbc_i2c_init);
diff --git a/drivers/sbus/char/display7seg.c b/drivers/sbus/char/display7seg.c
index 55f71ea9c418..740da4465447 100644
--- a/drivers/sbus/char/display7seg.c
+++ b/drivers/sbus/char/display7seg.c
@@ -171,8 +171,7 @@ static struct miscdevice d7s_miscdev = {
 	.fops		= &d7s_fops
 };
 
-static int __devinit d7s_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit d7s_probe(struct platform_device *op)
 {
 	struct device_node *opts;
 	int err = -EINVAL;
@@ -266,7 +265,7 @@ static const struct of_device_id d7s_match[] = {
 };
 MODULE_DEVICE_TABLE(of, d7s_match);
 
-static struct of_platform_driver d7s_driver = {
+static struct platform_driver d7s_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -278,12 +277,12 @@ static struct of_platform_driver d7s_driver = {
 
 static int __init d7s_init(void)
 {
-	return of_register_platform_driver(&d7s_driver);
+	return platform_driver_register(&d7s_driver);
 }
 
 static void __exit d7s_exit(void)
 {
-	of_unregister_platform_driver(&d7s_driver);
+	platform_driver_unregister(&d7s_driver);
 }
 
 module_init(d7s_init);
diff --git a/drivers/sbus/char/envctrl.c b/drivers/sbus/char/envctrl.c
index 8ce414e39489..be7b4e56154f 100644
--- a/drivers/sbus/char/envctrl.c
+++ b/drivers/sbus/char/envctrl.c
@@ -1028,8 +1028,7 @@ static int kenvctrld(void *__unused)
 	return 0;
 }
 
-static int __devinit envctrl_probe(struct platform_device *op,
-				   const struct of_device_id *match)
+static int __devinit envctrl_probe(struct platform_device *op)
 {
 	struct device_node *dp;
 	int index, err;
@@ -1129,7 +1128,7 @@ static const struct of_device_id envctrl_match[] = {
 };
 MODULE_DEVICE_TABLE(of, envctrl_match);
 
-static struct of_platform_driver envctrl_driver = {
+static struct platform_driver envctrl_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -1141,12 +1140,12 @@ static struct of_platform_driver envctrl_driver = {
 
 static int __init envctrl_init(void)
 {
-	return of_register_platform_driver(&envctrl_driver);
+	return platform_driver_register(&envctrl_driver);
 }
 
 static void __exit envctrl_exit(void)
 {
-	of_unregister_platform_driver(&envctrl_driver);
+	platform_driver_unregister(&envctrl_driver);
 }
 
 module_init(envctrl_init);
diff --git a/drivers/sbus/char/flash.c b/drivers/sbus/char/flash.c
index 2b4b4b613c48..73dd4e7afaaa 100644
--- a/drivers/sbus/char/flash.c
+++ b/drivers/sbus/char/flash.c
@@ -160,8 +160,7 @@ static const struct file_operations flash_fops = {
 
 static struct miscdevice flash_dev = { FLASH_MINOR, "flash", &flash_fops };
 
-static int __devinit flash_probe(struct platform_device *op,
-				 const struct of_device_id *match)
+static int __devinit flash_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct device_node *parent;
@@ -207,7 +206,7 @@ static const struct of_device_id flash_match[] = {
 };
 MODULE_DEVICE_TABLE(of, flash_match);
 
-static struct of_platform_driver flash_driver = {
+static struct platform_driver flash_driver = {
 	.driver = {
 		.name = "flash",
 		.owner = THIS_MODULE,
@@ -219,12 +218,12 @@ static struct of_platform_driver flash_driver = {
 
 static int __init flash_init(void)
 {
-	return of_register_platform_driver(&flash_driver);
+	return platform_driver_register(&flash_driver);
 }
 
 static void __exit flash_cleanup(void)
 {
-	of_unregister_platform_driver(&flash_driver);
+	platform_driver_unregister(&flash_driver);
 }
 
 module_init(flash_init);
diff --git a/drivers/sbus/char/uctrl.c b/drivers/sbus/char/uctrl.c
index 1b345be5cc02..ebce9639a26a 100644
--- a/drivers/sbus/char/uctrl.c
+++ b/drivers/sbus/char/uctrl.c
@@ -348,8 +348,7 @@ static void uctrl_get_external_status(struct uctrl_driver *driver)
 	
 }
 
-static int __devinit uctrl_probe(struct platform_device *op,
-				 const struct of_device_id *match)
+static int __devinit uctrl_probe(struct platform_device *op)
 {
 	struct uctrl_driver *p;
 	int err = -ENOMEM;
@@ -425,7 +424,7 @@ static const struct of_device_id uctrl_match[] = {
 };
 MODULE_DEVICE_TABLE(of, uctrl_match);
 
-static struct of_platform_driver uctrl_driver = {
+static struct platform_driver uctrl_driver = {
 	.driver = {
 		.name = "uctrl",
 		.owner = THIS_MODULE,
@@ -438,12 +437,12 @@ static struct of_platform_driver uctrl_driver = {
 
 static int __init uctrl_init(void)
 {
-	return of_register_platform_driver(&uctrl_driver);
+	return platform_driver_register(&uctrl_driver);
 }
 
 static void __exit uctrl_exit(void)
 {
-	of_unregister_platform_driver(&uctrl_driver);
+	platform_driver_unregister(&uctrl_driver);
 }
 
 module_init(uctrl_init);
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index 8616496ffc02..4a1f029c4fe9 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -381,6 +381,7 @@ config ISCSI_BOOT_SYSFS
 
 source "drivers/scsi/cxgbi/Kconfig"
 source "drivers/scsi/bnx2i/Kconfig"
+source "drivers/scsi/bnx2fc/Kconfig"
 source "drivers/scsi/be2iscsi/Kconfig"
 
 config SGIWD93_SCSI
diff --git a/drivers/scsi/Makefile b/drivers/scsi/Makefile
index 2e9a87e8e7d8..7ad0b8a79ae8 100644
--- a/drivers/scsi/Makefile
+++ b/drivers/scsi/Makefile
@@ -40,6 +40,7 @@ obj-$(CONFIG_LIBFC)		+= libfc/
 obj-$(CONFIG_LIBFCOE)		+= fcoe/
 obj-$(CONFIG_FCOE)		+= fcoe/
 obj-$(CONFIG_FCOE_FNIC)		+= fnic/
+obj-$(CONFIG_SCSI_BNX2X_FCOE)	+= libfc/ fcoe/ bnx2fc/
 obj-$(CONFIG_ISCSI_TCP) 	+= libiscsi.o	libiscsi_tcp.o iscsi_tcp.o
 obj-$(CONFIG_INFINIBAND_ISER) 	+= libiscsi.o
 obj-$(CONFIG_ISCSI_BOOT_SYSFS)	+= iscsi_boot_sysfs.o
@@ -165,7 +166,7 @@ scsi_mod-$(CONFIG_SCSI_NETLINK)	+= scsi_netlink.o
 scsi_mod-$(CONFIG_SYSCTL)	+= scsi_sysctl.o
 scsi_mod-$(CONFIG_SCSI_PROC_FS)	+= scsi_proc.o
 scsi_mod-y			+= scsi_trace.o
-scsi_mod-$(CONFIG_PM_OPS)	+= scsi_pm.o
+scsi_mod-$(CONFIG_PM)		+= scsi_pm.o
 
 scsi_tgt-y			+= scsi_tgt_lib.o scsi_tgt_if.o
 
diff --git a/drivers/scsi/NCR5380.c b/drivers/scsi/NCR5380.c
index 9a5629f94f95..e7cd2fcbe036 100644
--- a/drivers/scsi/NCR5380.c
+++ b/drivers/scsi/NCR5380.c
@@ -936,8 +936,7 @@ static void NCR5380_exit(struct Scsi_Host *instance)
 {
 	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
 
-	cancel_delayed_work(&hostdata->coroutine);
-	flush_scheduled_work();
+	cancel_delayed_work_sync(&hostdata->coroutine);
 }
 
 /**
diff --git a/drivers/scsi/aic7xxx/aic79xx.h b/drivers/scsi/aic7xxx/aic79xx.h
index be5558ab84ea..95ee50385188 100644
--- a/drivers/scsi/aic7xxx/aic79xx.h
+++ b/drivers/scsi/aic7xxx/aic79xx.h
@@ -672,7 +672,7 @@ struct scb_data {
 /************************ Target Mode Definitions *****************************/
 
 /*
- * Connection desciptor for select-in requests in target mode.
+ * Connection descriptor for select-in requests in target mode.
  */
 struct target_cmd {
 	uint8_t scsiid;		/* Our ID and the initiator's ID */
diff --git a/drivers/scsi/aic7xxx/aic7xxx.h b/drivers/scsi/aic7xxx/aic7xxx.h
index e4e651cca3e4..17444bc18bca 100644
--- a/drivers/scsi/aic7xxx/aic7xxx.h
+++ b/drivers/scsi/aic7xxx/aic7xxx.h
@@ -618,7 +618,7 @@ struct scb_data {
 /************************ Target Mode Definitions *****************************/
 
 /*
- * Connection desciptor for select-in requests in target mode.
+ * Connection descriptor for select-in requests in target mode.
  */
 struct target_cmd {
 	uint8_t scsiid;		/* Our ID and the initiator's ID */
diff --git a/drivers/scsi/aic7xxx/aic7xxx_core.c b/drivers/scsi/aic7xxx/aic7xxx_core.c
index 3f5a542a7793..e021b4812d58 100644
--- a/drivers/scsi/aic7xxx/aic7xxx_core.c
+++ b/drivers/scsi/aic7xxx/aic7xxx_core.c
@@ -4780,7 +4780,7 @@ ahc_init_scbdata(struct ahc_softc *ahc)
 	SLIST_INIT(&scb_data->sg_maps);
 
 	/* Allocate SCB resources */
-	scb_data->scbarray = (struct scb *)kmalloc(sizeof(struct scb) * AHC_SCB_MAX_ALLOC, GFP_ATOMIC);
+	scb_data->scbarray = kmalloc(sizeof(struct scb) * AHC_SCB_MAX_ALLOC, GFP_ATOMIC);
 	if (scb_data->scbarray == NULL)
 		return (ENOMEM);
 	memset(scb_data->scbarray, 0, sizeof(struct scb) * AHC_SCB_MAX_ALLOC);
diff --git a/drivers/scsi/arcmsr/arcmsr_hba.c b/drivers/scsi/arcmsr/arcmsr_hba.c
index 984bd527c6c9..da7b9887ec48 100644
--- a/drivers/scsi/arcmsr/arcmsr_hba.c
+++ b/drivers/scsi/arcmsr/arcmsr_hba.c
@@ -1020,7 +1020,7 @@ static void arcmsr_remove(struct pci_dev *pdev)
 	int poll_count = 0;
 	arcmsr_free_sysfs_attr(acb);
 	scsi_remove_host(host);
-	flush_scheduled_work();
+	flush_work_sync(&acb->arcmsr_do_message_isr_bh);
 	del_timer_sync(&acb->eternal_timer);
 	arcmsr_disable_outbound_ints(acb);
 	arcmsr_stop_adapter_bgrb(acb);
@@ -1066,7 +1066,7 @@ static void arcmsr_shutdown(struct pci_dev *pdev)
 		(struct AdapterControlBlock *)host->hostdata;
 	del_timer_sync(&acb->eternal_timer);
 	arcmsr_disable_outbound_ints(acb);
-	flush_scheduled_work();
+	flush_work_sync(&acb->arcmsr_do_message_isr_bh);
 	arcmsr_stop_adapter_bgrb(acb);
 	arcmsr_flush_adapter_cache(acb);
 }
diff --git a/drivers/scsi/be2iscsi/be_iscsi.c b/drivers/scsi/be2iscsi/be_iscsi.c
index eaaa8813067d..868cc5590145 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.c
+++ b/drivers/scsi/be2iscsi/be_iscsi.c
@@ -210,28 +210,20 @@ int beiscsi_conn_bind(struct iscsi_cls_session *cls_session,
 }
 
 /**
- * beiscsi_conn_get_param - get the iscsi parameter
- * @cls_conn: pointer to iscsi cls conn
+ * beiscsi_ep_get_param - get the iscsi parameter
+ * @ep: pointer to iscsi ep
  * @param: parameter type identifier
  * @buf: buffer pointer
  *
  * returns iscsi parameter
  */
-int beiscsi_conn_get_param(struct iscsi_cls_conn *cls_conn,
+int beiscsi_ep_get_param(struct iscsi_endpoint *ep,
 			   enum iscsi_param param, char *buf)
 {
-	struct beiscsi_endpoint *beiscsi_ep;
-	struct iscsi_conn *conn = cls_conn->dd_data;
-	struct beiscsi_conn *beiscsi_conn = conn->dd_data;
+	struct beiscsi_endpoint *beiscsi_ep = ep->dd_data;
 	int len = 0;
 
 	SE_DEBUG(DBG_LVL_8, "In beiscsi_conn_get_param, param= %d\n", param);
-	beiscsi_ep = beiscsi_conn->ep;
-	if (!beiscsi_ep) {
-		SE_DEBUG(DBG_LVL_1,
-			 "In beiscsi_conn_get_param , no beiscsi_ep\n");
-		return -ENODEV;
-	}
 
 	switch (param) {
 	case ISCSI_PARAM_CONN_PORT:
@@ -244,7 +236,7 @@ int beiscsi_conn_get_param(struct iscsi_cls_conn *cls_conn,
 			len = sprintf(buf, "%pI6\n", &beiscsi_ep->dst6_addr);
 		break;
 	default:
-		return iscsi_conn_get_param(cls_conn, param, buf);
+		return -ENOSYS;
 	}
 	return len;
 }
diff --git a/drivers/scsi/be2iscsi/be_iscsi.h b/drivers/scsi/be2iscsi/be_iscsi.h
index 8950a702b9f4..9c532797c29e 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.h
+++ b/drivers/scsi/be2iscsi/be_iscsi.h
@@ -48,8 +48,8 @@ int beiscsi_conn_bind(struct iscsi_cls_session *cls_session,
 		      struct iscsi_cls_conn *cls_conn,
 		      uint64_t transport_fd, int is_leading);
 
-int beiscsi_conn_get_param(struct iscsi_cls_conn *cls_conn,
-			   enum iscsi_param param, char *buf);
+int beiscsi_ep_get_param(struct iscsi_endpoint *ep, enum iscsi_param param,
+			 char *buf);
 
 int beiscsi_get_host_param(struct Scsi_Host *shost,
 			   enum iscsi_host_param param, char *buf);
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index 79cefbe31367..24e20ba9633c 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -4277,7 +4277,7 @@ static int __devinit beiscsi_dev_probe(struct pci_dev *pcidev,
 
 	snprintf(phba->wq_name, sizeof(phba->wq_name), "beiscsi_q_irq%u",
 		 phba->shost->host_no);
-	phba->wq = create_workqueue(phba->wq_name);
+	phba->wq = alloc_workqueue(phba->wq_name, WQ_MEM_RECLAIM, 1);
 	if (!phba->wq) {
 		shost_printk(KERN_ERR, phba->shost, "beiscsi_dev_probe-"
 				"Failed to allocate work queue\n");
@@ -4384,7 +4384,7 @@ struct iscsi_transport beiscsi_iscsi_transport = {
 	.bind_conn = beiscsi_conn_bind,
 	.destroy_conn = iscsi_conn_teardown,
 	.set_param = beiscsi_set_param,
-	.get_conn_param = beiscsi_conn_get_param,
+	.get_conn_param = iscsi_conn_get_param,
 	.get_session_param = iscsi_session_get_param,
 	.get_host_param = beiscsi_get_host_param,
 	.start_conn = beiscsi_conn_start,
@@ -4395,6 +4395,7 @@ struct iscsi_transport beiscsi_iscsi_transport = {
 	.alloc_pdu = beiscsi_alloc_pdu,
 	.parse_pdu_itt = beiscsi_parse_pdu,
 	.get_stats = beiscsi_conn_get_stats,
+	.get_ep_param = beiscsi_ep_get_param,
 	.ep_connect = beiscsi_ep_connect,
 	.ep_poll = beiscsi_ep_poll,
 	.ep_disconnect = beiscsi_ep_disconnect,
diff --git a/drivers/scsi/bnx2fc/57xx_hsi_bnx2fc.h b/drivers/scsi/bnx2fc/57xx_hsi_bnx2fc.h
new file mode 100644
index 000000000000..69d031d98469
--- /dev/null
+++ b/drivers/scsi/bnx2fc/57xx_hsi_bnx2fc.h
@@ -0,0 +1,1080 @@
+#ifndef __57XX_FCOE_HSI_LINUX_LE__
+#define __57XX_FCOE_HSI_LINUX_LE__
+
+/*
+ * common data for all protocols
+ */
+struct b577xx_doorbell_hdr {
+	u8 header;
+#define B577XX_DOORBELL_HDR_RX (0x1<<0)
+#define B577XX_DOORBELL_HDR_RX_SHIFT 0
+#define B577XX_DOORBELL_HDR_DB_TYPE (0x1<<1)
+#define B577XX_DOORBELL_HDR_DB_TYPE_SHIFT 1
+#define B577XX_DOORBELL_HDR_DPM_SIZE (0x3<<2)
+#define B577XX_DOORBELL_HDR_DPM_SIZE_SHIFT 2
+#define B577XX_DOORBELL_HDR_CONN_TYPE (0xF<<4)
+#define B577XX_DOORBELL_HDR_CONN_TYPE_SHIFT 4
+};
+
+/*
+ * doorbell message sent to the chip
+ */
+struct b577xx_doorbell_set_prod {
+#if defined(__BIG_ENDIAN)
+	u16 prod;
+	u8 zero_fill1;
+	struct b577xx_doorbell_hdr header;
+#elif defined(__LITTLE_ENDIAN)
+	struct b577xx_doorbell_hdr header;
+	u8 zero_fill1;
+	u16 prod;
+#endif
+};
+
+
+struct regpair {
+	__le32 lo;
+	__le32 hi;
+};
+
+
+/*
+ * Fixed size structure in order to plant it in Union structure
+ */
+struct fcoe_abts_rsp_union {
+	u32 r_ctl;
+	u32 abts_rsp_payload[7];
+};
+
+
+/*
+ * 4 regs size
+ */
+struct fcoe_bd_ctx {
+	u32 buf_addr_hi;
+	u32 buf_addr_lo;
+#if defined(__BIG_ENDIAN)
+	u16 rsrv0;
+	u16 buf_len;
+#elif defined(__LITTLE_ENDIAN)
+	u16 buf_len;
+	u16 rsrv0;
+#endif
+#if defined(__BIG_ENDIAN)
+	u16 rsrv1;
+	u16 flags;
+#elif defined(__LITTLE_ENDIAN)
+	u16 flags;
+	u16 rsrv1;
+#endif
+};
+
+
+struct fcoe_cleanup_flow_info {
+#if defined(__BIG_ENDIAN)
+	u16 reserved1;
+	u16 task_id;
+#elif defined(__LITTLE_ENDIAN)
+	u16 task_id;
+	u16 reserved1;
+#endif
+	u32 reserved2[7];
+};
+
+
+struct fcoe_fcp_cmd_payload {
+	u32 opaque[8];
+};
+
+struct fcoe_fc_hdr {
+#if defined(__BIG_ENDIAN)
+	u8 cs_ctl;
+	u8 s_id[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 s_id[3];
+	u8 cs_ctl;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 r_ctl;
+	u8 d_id[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 d_id[3];
+	u8 r_ctl;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 seq_id;
+	u8 df_ctl;
+	u16 seq_cnt;
+#elif defined(__LITTLE_ENDIAN)
+	u16 seq_cnt;
+	u8 df_ctl;
+	u8 seq_id;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 type;
+	u8 f_ctl[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 f_ctl[3];
+	u8 type;
+#endif
+	u32 parameters;
+#if defined(__BIG_ENDIAN)
+	u16 ox_id;
+	u16 rx_id;
+#elif defined(__LITTLE_ENDIAN)
+	u16 rx_id;
+	u16 ox_id;
+#endif
+};
+
+struct fcoe_fc_frame {
+	struct fcoe_fc_hdr fc_hdr;
+	u32 reserved0[2];
+};
+
+union fcoe_cmd_flow_info {
+	struct fcoe_fcp_cmd_payload fcp_cmd_payload;
+	struct fcoe_fc_frame mp_fc_frame;
+};
+
+
+
+struct fcoe_fcp_rsp_flags {
+	u8 flags;
+#define FCOE_FCP_RSP_FLAGS_FCP_RSP_LEN_VALID (0x1<<0)
+#define FCOE_FCP_RSP_FLAGS_FCP_RSP_LEN_VALID_SHIFT 0
+#define FCOE_FCP_RSP_FLAGS_FCP_SNS_LEN_VALID (0x1<<1)
+#define FCOE_FCP_RSP_FLAGS_FCP_SNS_LEN_VALID_SHIFT 1
+#define FCOE_FCP_RSP_FLAGS_FCP_RESID_OVER (0x1<<2)
+#define FCOE_FCP_RSP_FLAGS_FCP_RESID_OVER_SHIFT 2
+#define FCOE_FCP_RSP_FLAGS_FCP_RESID_UNDER (0x1<<3)
+#define FCOE_FCP_RSP_FLAGS_FCP_RESID_UNDER_SHIFT 3
+#define FCOE_FCP_RSP_FLAGS_FCP_CONF_REQ (0x1<<4)
+#define FCOE_FCP_RSP_FLAGS_FCP_CONF_REQ_SHIFT 4
+#define FCOE_FCP_RSP_FLAGS_FCP_BIDI_FLAGS (0x7<<5)
+#define FCOE_FCP_RSP_FLAGS_FCP_BIDI_FLAGS_SHIFT 5
+};
+
+
+struct fcoe_fcp_rsp_payload {
+	struct regpair reserved0;
+	u32 fcp_resid;
+#if defined(__BIG_ENDIAN)
+	u16 retry_delay_timer;
+	struct fcoe_fcp_rsp_flags fcp_flags;
+	u8 scsi_status_code;
+#elif defined(__LITTLE_ENDIAN)
+	u8 scsi_status_code;
+	struct fcoe_fcp_rsp_flags fcp_flags;
+	u16 retry_delay_timer;
+#endif
+	u32 fcp_rsp_len;
+	u32 fcp_sns_len;
+};
+
+
+/*
+ * Fixed size structure in order to plant it in Union structure
+ */
+struct fcoe_fcp_rsp_union {
+	struct fcoe_fcp_rsp_payload payload;
+	struct regpair reserved0;
+};
+
+
+struct fcoe_fcp_xfr_rdy_payload {
+	u32 burst_len;
+	u32 data_ro;
+};
+
+struct fcoe_read_flow_info {
+	struct fcoe_fc_hdr fc_data_in_hdr;
+	u32 reserved[2];
+};
+
+struct fcoe_write_flow_info {
+	struct fcoe_fc_hdr fc_data_out_hdr;
+	struct fcoe_fcp_xfr_rdy_payload fcp_xfr_payload;
+};
+
+union fcoe_rsp_flow_info {
+	struct fcoe_fcp_rsp_union fcp_rsp;
+	struct fcoe_abts_rsp_union abts_rsp;
+};
+
+/*
+ * 32 bytes used for general purposes
+ */
+union fcoe_general_task_ctx {
+	union fcoe_cmd_flow_info cmd_info;
+	struct fcoe_read_flow_info read_info;
+	struct fcoe_write_flow_info write_info;
+	union fcoe_rsp_flow_info rsp_info;
+	struct fcoe_cleanup_flow_info cleanup_info;
+	u32 comp_info[8];
+};
+
+
+/*
+ * FCoE KCQ CQE parameters
+ */
+union fcoe_kcqe_params {
+	u32 reserved0[4];
+};
+
+/*
+ * FCoE KCQ CQE
+ */
+struct fcoe_kcqe {
+	u32 fcoe_conn_id;
+	u32 completion_status;
+	u32 fcoe_conn_context_id;
+	union fcoe_kcqe_params params;
+#if defined(__BIG_ENDIAN)
+	u8 flags;
+#define FCOE_KCQE_RESERVED0 (0x7<<0)
+#define FCOE_KCQE_RESERVED0_SHIFT 0
+#define FCOE_KCQE_RAMROD_COMPLETION (0x1<<3)
+#define FCOE_KCQE_RAMROD_COMPLETION_SHIFT 3
+#define FCOE_KCQE_LAYER_CODE (0x7<<4)
+#define FCOE_KCQE_LAYER_CODE_SHIFT 4
+#define FCOE_KCQE_LINKED_WITH_NEXT (0x1<<7)
+#define FCOE_KCQE_LINKED_WITH_NEXT_SHIFT 7
+	u8 op_code;
+	u16 qe_self_seq;
+#elif defined(__LITTLE_ENDIAN)
+	u16 qe_self_seq;
+	u8 op_code;
+	u8 flags;
+#define FCOE_KCQE_RESERVED0 (0x7<<0)
+#define FCOE_KCQE_RESERVED0_SHIFT 0
+#define FCOE_KCQE_RAMROD_COMPLETION (0x1<<3)
+#define FCOE_KCQE_RAMROD_COMPLETION_SHIFT 3
+#define FCOE_KCQE_LAYER_CODE (0x7<<4)
+#define FCOE_KCQE_LAYER_CODE_SHIFT 4
+#define FCOE_KCQE_LINKED_WITH_NEXT (0x1<<7)
+#define FCOE_KCQE_LINKED_WITH_NEXT_SHIFT 7
+#endif
+};
+
+/*
+ * FCoE KWQE header
+ */
+struct fcoe_kwqe_header {
+#if defined(__BIG_ENDIAN)
+	u8 flags;
+#define FCOE_KWQE_HEADER_RESERVED0 (0xF<<0)
+#define FCOE_KWQE_HEADER_RESERVED0_SHIFT 0
+#define FCOE_KWQE_HEADER_LAYER_CODE (0x7<<4)
+#define FCOE_KWQE_HEADER_LAYER_CODE_SHIFT 4
+#define FCOE_KWQE_HEADER_RESERVED1 (0x1<<7)
+#define FCOE_KWQE_HEADER_RESERVED1_SHIFT 7
+	u8 op_code;
+#elif defined(__LITTLE_ENDIAN)
+	u8 op_code;
+	u8 flags;
+#define FCOE_KWQE_HEADER_RESERVED0 (0xF<<0)
+#define FCOE_KWQE_HEADER_RESERVED0_SHIFT 0
+#define FCOE_KWQE_HEADER_LAYER_CODE (0x7<<4)
+#define FCOE_KWQE_HEADER_LAYER_CODE_SHIFT 4
+#define FCOE_KWQE_HEADER_RESERVED1 (0x1<<7)
+#define FCOE_KWQE_HEADER_RESERVED1_SHIFT 7
+#endif
+};
+
+/*
+ * FCoE firmware init request 1
+ */
+struct fcoe_kwqe_init1 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 num_tasks;
+#elif defined(__LITTLE_ENDIAN)
+	u16 num_tasks;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 task_list_pbl_addr_lo;
+	u32 task_list_pbl_addr_hi;
+	u32 dummy_buffer_addr_lo;
+	u32 dummy_buffer_addr_hi;
+#if defined(__BIG_ENDIAN)
+	u16 rq_num_wqes;
+	u16 sq_num_wqes;
+#elif defined(__LITTLE_ENDIAN)
+	u16 sq_num_wqes;
+	u16 rq_num_wqes;
+#endif
+#if defined(__BIG_ENDIAN)
+	u16 cq_num_wqes;
+	u16 rq_buffer_log_size;
+#elif defined(__LITTLE_ENDIAN)
+	u16 rq_buffer_log_size;
+	u16 cq_num_wqes;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 flags;
+#define FCOE_KWQE_INIT1_LOG_PAGE_SIZE (0xF<<0)
+#define FCOE_KWQE_INIT1_LOG_PAGE_SIZE_SHIFT 0
+#define FCOE_KWQE_INIT1_LOG_CACHED_PBES_PER_FUNC (0x7<<4)
+#define FCOE_KWQE_INIT1_LOG_CACHED_PBES_PER_FUNC_SHIFT 4
+#define FCOE_KWQE_INIT1_RESERVED1 (0x1<<7)
+#define FCOE_KWQE_INIT1_RESERVED1_SHIFT 7
+	u8 num_sessions_log;
+	u16 mtu;
+#elif defined(__LITTLE_ENDIAN)
+	u16 mtu;
+	u8 num_sessions_log;
+	u8 flags;
+#define FCOE_KWQE_INIT1_LOG_PAGE_SIZE (0xF<<0)
+#define FCOE_KWQE_INIT1_LOG_PAGE_SIZE_SHIFT 0
+#define FCOE_KWQE_INIT1_LOG_CACHED_PBES_PER_FUNC (0x7<<4)
+#define FCOE_KWQE_INIT1_LOG_CACHED_PBES_PER_FUNC_SHIFT 4
+#define FCOE_KWQE_INIT1_RESERVED1 (0x1<<7)
+#define FCOE_KWQE_INIT1_RESERVED1_SHIFT 7
+#endif
+};
+
+/*
+ * FCoE firmware init request 2
+ */
+struct fcoe_kwqe_init2 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 reserved0;
+#elif defined(__LITTLE_ENDIAN)
+	u16 reserved0;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 hash_tbl_pbl_addr_lo;
+	u32 hash_tbl_pbl_addr_hi;
+	u32 t2_hash_tbl_addr_lo;
+	u32 t2_hash_tbl_addr_hi;
+	u32 t2_ptr_hash_tbl_addr_lo;
+	u32 t2_ptr_hash_tbl_addr_hi;
+	u32 free_list_count;
+};
+
+/*
+ * FCoE firmware init request 3
+ */
+struct fcoe_kwqe_init3 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 reserved0;
+#elif defined(__LITTLE_ENDIAN)
+	u16 reserved0;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 error_bit_map_lo;
+	u32 error_bit_map_hi;
+#if defined(__BIG_ENDIAN)
+	u8 reserved21[3];
+	u8 cached_session_enable;
+#elif defined(__LITTLE_ENDIAN)
+	u8 cached_session_enable;
+	u8 reserved21[3];
+#endif
+	u32 reserved2[4];
+};
+
+/*
+ * FCoE connection offload request 1
+ */
+struct fcoe_kwqe_conn_offload1 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 fcoe_conn_id;
+#elif defined(__LITTLE_ENDIAN)
+	u16 fcoe_conn_id;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 sq_addr_lo;
+	u32 sq_addr_hi;
+	u32 rq_pbl_addr_lo;
+	u32 rq_pbl_addr_hi;
+	u32 rq_first_pbe_addr_lo;
+	u32 rq_first_pbe_addr_hi;
+#if defined(__BIG_ENDIAN)
+	u16 reserved0;
+	u16 rq_prod;
+#elif defined(__LITTLE_ENDIAN)
+	u16 rq_prod;
+	u16 reserved0;
+#endif
+};
+
+/*
+ * FCoE connection offload request 2
+ */
+struct fcoe_kwqe_conn_offload2 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 tx_max_fc_pay_len;
+#elif defined(__LITTLE_ENDIAN)
+	u16 tx_max_fc_pay_len;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 cq_addr_lo;
+	u32 cq_addr_hi;
+	u32 xferq_addr_lo;
+	u32 xferq_addr_hi;
+	u32 conn_db_addr_lo;
+	u32 conn_db_addr_hi;
+	u32 reserved1;
+};
+
+/*
+ * FCoE connection offload request 3
+ */
+struct fcoe_kwqe_conn_offload3 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 vlan_tag;
+#define FCOE_KWQE_CONN_OFFLOAD3_VLAN_ID (0xFFF<<0)
+#define FCOE_KWQE_CONN_OFFLOAD3_VLAN_ID_SHIFT 0
+#define FCOE_KWQE_CONN_OFFLOAD3_CFI (0x1<<12)
+#define FCOE_KWQE_CONN_OFFLOAD3_CFI_SHIFT 12
+#define FCOE_KWQE_CONN_OFFLOAD3_PRIORITY (0x7<<13)
+#define FCOE_KWQE_CONN_OFFLOAD3_PRIORITY_SHIFT 13
+#elif defined(__LITTLE_ENDIAN)
+	u16 vlan_tag;
+#define FCOE_KWQE_CONN_OFFLOAD3_VLAN_ID (0xFFF<<0)
+#define FCOE_KWQE_CONN_OFFLOAD3_VLAN_ID_SHIFT 0
+#define FCOE_KWQE_CONN_OFFLOAD3_CFI (0x1<<12)
+#define FCOE_KWQE_CONN_OFFLOAD3_CFI_SHIFT 12
+#define FCOE_KWQE_CONN_OFFLOAD3_PRIORITY (0x7<<13)
+#define FCOE_KWQE_CONN_OFFLOAD3_PRIORITY_SHIFT 13
+	struct fcoe_kwqe_header hdr;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 tx_max_conc_seqs_c3;
+	u8 s_id[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 s_id[3];
+	u8 tx_max_conc_seqs_c3;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 flags;
+#define FCOE_KWQE_CONN_OFFLOAD3_B_MUL_N_PORT_IDS (0x1<<0)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_MUL_N_PORT_IDS_SHIFT 0
+#define FCOE_KWQE_CONN_OFFLOAD3_B_E_D_TOV_RES (0x1<<1)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_E_D_TOV_RES_SHIFT 1
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONT_INCR_SEQ_CNT (0x1<<2)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONT_INCR_SEQ_CNT_SHIFT 2
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONF_REQ (0x1<<3)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONF_REQ_SHIFT 3
+#define FCOE_KWQE_CONN_OFFLOAD3_B_REC_VALID (0x1<<4)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_REC_VALID_SHIFT 4
+#define FCOE_KWQE_CONN_OFFLOAD3_B_C2_VALID (0x1<<5)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_C2_VALID_SHIFT 5
+#define FCOE_KWQE_CONN_OFFLOAD3_B_ACK_0 (0x1<<6)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_ACK_0_SHIFT 6
+#define FCOE_KWQE_CONN_OFFLOAD3_B_VLAN_FLAG (0x1<<7)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_VLAN_FLAG_SHIFT 7
+	u8 d_id[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 d_id[3];
+	u8 flags;
+#define FCOE_KWQE_CONN_OFFLOAD3_B_MUL_N_PORT_IDS (0x1<<0)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_MUL_N_PORT_IDS_SHIFT 0
+#define FCOE_KWQE_CONN_OFFLOAD3_B_E_D_TOV_RES (0x1<<1)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_E_D_TOV_RES_SHIFT 1
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONT_INCR_SEQ_CNT (0x1<<2)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONT_INCR_SEQ_CNT_SHIFT 2
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONF_REQ (0x1<<3)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_CONF_REQ_SHIFT 3
+#define FCOE_KWQE_CONN_OFFLOAD3_B_REC_VALID (0x1<<4)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_REC_VALID_SHIFT 4
+#define FCOE_KWQE_CONN_OFFLOAD3_B_C2_VALID (0x1<<5)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_C2_VALID_SHIFT 5
+#define FCOE_KWQE_CONN_OFFLOAD3_B_ACK_0 (0x1<<6)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_ACK_0_SHIFT 6
+#define FCOE_KWQE_CONN_OFFLOAD3_B_VLAN_FLAG (0x1<<7)
+#define FCOE_KWQE_CONN_OFFLOAD3_B_VLAN_FLAG_SHIFT 7
+#endif
+	u32 reserved;
+	u32 confq_first_pbe_addr_lo;
+	u32 confq_first_pbe_addr_hi;
+#if defined(__BIG_ENDIAN)
+	u16 rx_max_fc_pay_len;
+	u16 tx_total_conc_seqs;
+#elif defined(__LITTLE_ENDIAN)
+	u16 tx_total_conc_seqs;
+	u16 rx_max_fc_pay_len;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 rx_open_seqs_exch_c3;
+	u8 rx_max_conc_seqs_c3;
+	u16 rx_total_conc_seqs;
+#elif defined(__LITTLE_ENDIAN)
+	u16 rx_total_conc_seqs;
+	u8 rx_max_conc_seqs_c3;
+	u8 rx_open_seqs_exch_c3;
+#endif
+};
+
+/*
+ * FCoE connection offload request 4
+ */
+struct fcoe_kwqe_conn_offload4 {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u8 reserved2;
+	u8 e_d_tov_timer_val;
+#elif defined(__LITTLE_ENDIAN)
+	u8 e_d_tov_timer_val;
+	u8 reserved2;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u8 src_mac_addr_lo32[4];
+#if defined(__BIG_ENDIAN)
+	u8 dst_mac_addr_hi16[2];
+	u8 src_mac_addr_hi16[2];
+#elif defined(__LITTLE_ENDIAN)
+	u8 src_mac_addr_hi16[2];
+	u8 dst_mac_addr_hi16[2];
+#endif
+	u8 dst_mac_addr_lo32[4];
+	u32 lcq_addr_lo;
+	u32 lcq_addr_hi;
+	u32 confq_pbl_base_addr_lo;
+	u32 confq_pbl_base_addr_hi;
+};
+
+/*
+ * FCoE connection enable request
+ */
+struct fcoe_kwqe_conn_enable_disable {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 reserved0;
+#elif defined(__LITTLE_ENDIAN)
+	u16 reserved0;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u8 src_mac_addr_lo32[4];
+#if defined(__BIG_ENDIAN)
+	u16 vlan_tag;
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_VLAN_ID (0xFFF<<0)
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_VLAN_ID_SHIFT 0
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_CFI (0x1<<12)
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_CFI_SHIFT 12
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_PRIORITY (0x7<<13)
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_PRIORITY_SHIFT 13
+	u8 src_mac_addr_hi16[2];
+#elif defined(__LITTLE_ENDIAN)
+	u8 src_mac_addr_hi16[2];
+	u16 vlan_tag;
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_VLAN_ID (0xFFF<<0)
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_VLAN_ID_SHIFT 0
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_CFI (0x1<<12)
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_CFI_SHIFT 12
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_PRIORITY (0x7<<13)
+#define FCOE_KWQE_CONN_ENABLE_DISABLE_PRIORITY_SHIFT 13
+#endif
+	u8 dst_mac_addr_lo32[4];
+#if defined(__BIG_ENDIAN)
+	u16 reserved1;
+	u8 dst_mac_addr_hi16[2];
+#elif defined(__LITTLE_ENDIAN)
+	u8 dst_mac_addr_hi16[2];
+	u16 reserved1;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 vlan_flag;
+	u8 s_id[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 s_id[3];
+	u8 vlan_flag;
+#endif
+#if defined(__BIG_ENDIAN)
+	u8 reserved3;
+	u8 d_id[3];
+#elif defined(__LITTLE_ENDIAN)
+	u8 d_id[3];
+	u8 reserved3;
+#endif
+	u32 context_id;
+	u32 conn_id;
+	u32 reserved4;
+};
+
+/*
+ * FCoE connection destroy request
+ */
+struct fcoe_kwqe_conn_destroy {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 reserved0;
+#elif defined(__LITTLE_ENDIAN)
+	u16 reserved0;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 context_id;
+	u32 conn_id;
+	u32 reserved1[5];
+};
+
+/*
+ * FCoe destroy request
+ */
+struct fcoe_kwqe_destroy {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 reserved0;
+#elif defined(__LITTLE_ENDIAN)
+	u16 reserved0;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 reserved1[7];
+};
+
+/*
+ * FCoe statistics request
+ */
+struct fcoe_kwqe_stat {
+#if defined(__BIG_ENDIAN)
+	struct fcoe_kwqe_header hdr;
+	u16 reserved0;
+#elif defined(__LITTLE_ENDIAN)
+	u16 reserved0;
+	struct fcoe_kwqe_header hdr;
+#endif
+	u32 stat_params_addr_lo;
+	u32 stat_params_addr_hi;
+	u32 reserved1[5];
+};
+
+/*
+ * FCoE KWQ WQE
+ */
+union fcoe_kwqe {
+	struct fcoe_kwqe_init1 init1;
+	struct fcoe_kwqe_init2 init2;
+	struct fcoe_kwqe_init3 init3;
+	struct fcoe_kwqe_conn_offload1 conn_offload1;
+	struct fcoe_kwqe_conn_offload2 conn_offload2;
+	struct fcoe_kwqe_conn_offload3 conn_offload3;
+	struct fcoe_kwqe_conn_offload4 conn_offload4;
+	struct fcoe_kwqe_conn_enable_disable conn_enable_disable;
+	struct fcoe_kwqe_conn_destroy conn_destroy;
+	struct fcoe_kwqe_destroy destroy;
+	struct fcoe_kwqe_stat statistics;
+};
+
+struct fcoe_mul_sges_ctx {
+	struct regpair cur_sge_addr;
+#if defined(__BIG_ENDIAN)
+	u8 sgl_size;
+	u8 cur_sge_idx;
+	u16 cur_sge_off;
+#elif defined(__LITTLE_ENDIAN)
+	u16 cur_sge_off;
+	u8 cur_sge_idx;
+	u8 sgl_size;
+#endif
+};
+
+struct fcoe_s_stat_ctx {
+	u8 flags;
+#define FCOE_S_STAT_CTX_ACTIVE (0x1<<0)
+#define FCOE_S_STAT_CTX_ACTIVE_SHIFT 0
+#define FCOE_S_STAT_CTX_ACK_ABORT_SEQ_COND (0x1<<1)
+#define FCOE_S_STAT_CTX_ACK_ABORT_SEQ_COND_SHIFT 1
+#define FCOE_S_STAT_CTX_ABTS_PERFORMED (0x1<<2)
+#define FCOE_S_STAT_CTX_ABTS_PERFORMED_SHIFT 2
+#define FCOE_S_STAT_CTX_SEQ_TIMEOUT (0x1<<3)
+#define FCOE_S_STAT_CTX_SEQ_TIMEOUT_SHIFT 3
+#define FCOE_S_STAT_CTX_P_RJT (0x1<<4)
+#define FCOE_S_STAT_CTX_P_RJT_SHIFT 4
+#define FCOE_S_STAT_CTX_ACK_EOFT (0x1<<5)
+#define FCOE_S_STAT_CTX_ACK_EOFT_SHIFT 5
+#define FCOE_S_STAT_CTX_RSRV1 (0x3<<6)
+#define FCOE_S_STAT_CTX_RSRV1_SHIFT 6
+};
+
+struct fcoe_seq_ctx {
+#if defined(__BIG_ENDIAN)
+	u16 low_seq_cnt;
+	struct fcoe_s_stat_ctx s_stat;
+	u8 seq_id;
+#elif defined(__LITTLE_ENDIAN)
+	u8 seq_id;
+	struct fcoe_s_stat_ctx s_stat;
+	u16 low_seq_cnt;
+#endif
+#if defined(__BIG_ENDIAN)
+	u16 err_seq_cnt;
+	u16 high_seq_cnt;
+#elif defined(__LITTLE_ENDIAN)
+	u16 high_seq_cnt;
+	u16 err_seq_cnt;
+#endif
+	u32 low_exp_ro;
+	u32 high_exp_ro;
+};
+
+
+struct fcoe_single_sge_ctx {
+	struct regpair cur_buf_addr;
+#if defined(__BIG_ENDIAN)
+	u16 reserved0;
+	u16 cur_buf_rem;
+#elif defined(__LITTLE_ENDIAN)
+	u16 cur_buf_rem;
+	u16 reserved0;
+#endif
+};
+
+union fcoe_sgl_ctx {
+	struct fcoe_single_sge_ctx single_sge;
+	struct fcoe_mul_sges_ctx mul_sges;
+};
+
+
+
+/*
+ * FCoE SQ element
+ */
+struct fcoe_sqe {
+	u16 wqe;
+#define FCOE_SQE_TASK_ID (0x7FFF<<0)
+#define FCOE_SQE_TASK_ID_SHIFT 0
+#define FCOE_SQE_TOGGLE_BIT (0x1<<15)
+#define FCOE_SQE_TOGGLE_BIT_SHIFT 15
+};
+
+
+
+struct fcoe_task_ctx_entry_tx_only {
+	union fcoe_sgl_ctx sgl_ctx;
+};
+
+struct fcoe_task_ctx_entry_txwr_rxrd {
+#if defined(__BIG_ENDIAN)
+	u16 verify_tx_seq;
+	u8 init_flags;
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE (0x7<<0)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_DEV_TYPE (0x1<<3)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_DEV_TYPE_SHIFT 3
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE (0x1<<4)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE_SHIFT 4
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_SINGLE_SGE (0x1<<5)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_SINGLE_SGE_SHIFT 5
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV5 (0x3<<6)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV5_SHIFT 6
+	u8 tx_flags;
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE (0xF<<0)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV4 (0xF<<4)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV4_SHIFT 4
+#elif defined(__LITTLE_ENDIAN)
+	u8 tx_flags;
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE (0xF<<0)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV4 (0xF<<4)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV4_SHIFT 4
+	u8 init_flags;
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE (0x7<<0)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_DEV_TYPE (0x1<<3)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_DEV_TYPE_SHIFT 3
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE (0x1<<4)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE_SHIFT 4
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_SINGLE_SGE (0x1<<5)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_SINGLE_SGE_SHIFT 5
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV5 (0x3<<6)
+#define FCOE_TASK_CTX_ENTRY_TXWR_RXRD_RSRV5_SHIFT 6
+	u16 verify_tx_seq;
+#endif
+};
+
+/*
+ * Common section. Both TX and RX processing might write and read from it in
+ * different flows
+ */
+struct fcoe_task_ctx_entry_tx_rx_cmn {
+	u32 data_2_trns;
+	union fcoe_general_task_ctx general;
+#if defined(__BIG_ENDIAN)
+	u16 tx_low_seq_cnt;
+	struct fcoe_s_stat_ctx tx_s_stat;
+	u8 tx_seq_id;
+#elif defined(__LITTLE_ENDIAN)
+	u8 tx_seq_id;
+	struct fcoe_s_stat_ctx tx_s_stat;
+	u16 tx_low_seq_cnt;
+#endif
+	u32 common_flags;
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_CID (0xFFFFFF<<0)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_CID_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_VALID (0x1<<24)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_VALID_SHIFT 24
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_SEQ_INIT (0x1<<25)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_SEQ_INIT_SHIFT 25
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_PEND_XFER (0x1<<26)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_PEND_XFER_SHIFT 26
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_PEND_CONF (0x1<<27)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_PEND_CONF_SHIFT 27
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_EXP_FIRST_FRAME (0x1<<28)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_EXP_FIRST_FRAME_SHIFT 28
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_RSRV (0x7<<29)
+#define FCOE_TASK_CTX_ENTRY_TX_RX_CMN_RSRV_SHIFT 29
+};
+
+struct fcoe_task_ctx_entry_rxwr_txrd {
+#if defined(__BIG_ENDIAN)
+	u16 rx_id;
+	u16 rx_flags;
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RX_STATE (0xF<<0)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RX_STATE_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_NUM_RQ_WQE (0x7<<4)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_NUM_RQ_WQE_SHIFT 4
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_CONF_REQ (0x1<<7)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_CONF_REQ_SHIFT 7
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_MISS_FRAME (0x1<<8)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_MISS_FRAME_SHIFT 8
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RESERVED0 (0x7F<<9)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RESERVED0_SHIFT 9
+#elif defined(__LITTLE_ENDIAN)
+	u16 rx_flags;
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RX_STATE (0xF<<0)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RX_STATE_SHIFT 0
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_NUM_RQ_WQE (0x7<<4)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_NUM_RQ_WQE_SHIFT 4
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_CONF_REQ (0x1<<7)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_CONF_REQ_SHIFT 7
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_MISS_FRAME (0x1<<8)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_MISS_FRAME_SHIFT 8
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RESERVED0 (0x7F<<9)
+#define FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RESERVED0_SHIFT 9
+	u16 rx_id;
+#endif
+};
+
+struct fcoe_task_ctx_entry_rx_only {
+	struct fcoe_seq_ctx seq_ctx;
+	struct fcoe_seq_ctx ooo_seq_ctx;
+	u32 rsrv3;
+	union fcoe_sgl_ctx sgl_ctx;
+};
+
+struct fcoe_task_ctx_entry {
+	struct fcoe_task_ctx_entry_tx_only tx_wr_only;
+	struct fcoe_task_ctx_entry_txwr_rxrd tx_wr_rx_rd;
+	struct fcoe_task_ctx_entry_tx_rx_cmn cmn;
+	struct fcoe_task_ctx_entry_rxwr_txrd rx_wr_tx_rd;
+	struct fcoe_task_ctx_entry_rx_only rx_wr_only;
+	u32 reserved[4];
+};
+
+
+/*
+ * FCoE XFRQ element
+ */
+struct fcoe_xfrqe {
+	u16 wqe;
+#define FCOE_XFRQE_TASK_ID (0x7FFF<<0)
+#define FCOE_XFRQE_TASK_ID_SHIFT 0
+#define FCOE_XFRQE_TOGGLE_BIT (0x1<<15)
+#define FCOE_XFRQE_TOGGLE_BIT_SHIFT 15
+};
+
+
+/*
+ * FCoE CONFQ element
+ */
+struct fcoe_confqe {
+#if defined(__BIG_ENDIAN)
+	u16 rx_id;
+	u16 ox_id;
+#elif defined(__LITTLE_ENDIAN)
+	u16 ox_id;
+	u16 rx_id;
+#endif
+	u32 param;
+};
+
+
+/*
+ * FCoE conection data base
+ */
+struct fcoe_conn_db {
+#if defined(__BIG_ENDIAN)
+	u16 rsrv0;
+	u16 rq_prod;
+#elif defined(__LITTLE_ENDIAN)
+	u16 rq_prod;
+	u16 rsrv0;
+#endif
+	u32 rsrv1;
+	struct regpair cq_arm;
+};
+
+
+/*
+ * FCoE CQ element
+ */
+struct fcoe_cqe {
+	u16 wqe;
+#define FCOE_CQE_CQE_INFO (0x3FFF<<0)
+#define FCOE_CQE_CQE_INFO_SHIFT 0
+#define FCOE_CQE_CQE_TYPE (0x1<<14)
+#define FCOE_CQE_CQE_TYPE_SHIFT 14
+#define FCOE_CQE_TOGGLE_BIT (0x1<<15)
+#define FCOE_CQE_TOGGLE_BIT_SHIFT 15
+};
+
+
+/*
+ * FCoE error/warning resporting entry
+ */
+struct fcoe_err_report_entry {
+	u32 err_warn_bitmap_lo;
+	u32 err_warn_bitmap_hi;
+	u32 tx_buf_off;
+	u32 rx_buf_off;
+	struct fcoe_fc_hdr fc_hdr;
+};
+
+
+/*
+ * FCoE hash table entry (32 bytes)
+ */
+struct fcoe_hash_table_entry {
+#if defined(__BIG_ENDIAN)
+	u8 d_id_0;
+	u8 s_id_2;
+	u8 s_id_1;
+	u8 s_id_0;
+#elif defined(__LITTLE_ENDIAN)
+	u8 s_id_0;
+	u8 s_id_1;
+	u8 s_id_2;
+	u8 d_id_0;
+#endif
+#if defined(__BIG_ENDIAN)
+	u16 dst_mac_addr_hi;
+	u8 d_id_2;
+	u8 d_id_1;
+#elif defined(__LITTLE_ENDIAN)
+	u8 d_id_1;
+	u8 d_id_2;
+	u16 dst_mac_addr_hi;
+#endif
+	u32 dst_mac_addr_lo;
+#if defined(__BIG_ENDIAN)
+	u16 vlan_id;
+	u16 src_mac_addr_hi;
+#elif defined(__LITTLE_ENDIAN)
+	u16 src_mac_addr_hi;
+	u16 vlan_id;
+#endif
+	u32 src_mac_addr_lo;
+#if defined(__BIG_ENDIAN)
+	u16 reserved1;
+	u8 reserved0;
+	u8 vlan_flag;
+#elif defined(__LITTLE_ENDIAN)
+	u8 vlan_flag;
+	u8 reserved0;
+	u16 reserved1;
+#endif
+	u32 reserved2;
+	u32 field_id;
+#define FCOE_HASH_TABLE_ENTRY_CID (0xFFFFFF<<0)
+#define FCOE_HASH_TABLE_ENTRY_CID_SHIFT 0
+#define FCOE_HASH_TABLE_ENTRY_RESERVED3 (0x7F<<24)
+#define FCOE_HASH_TABLE_ENTRY_RESERVED3_SHIFT 24
+#define FCOE_HASH_TABLE_ENTRY_VALID (0x1<<31)
+#define FCOE_HASH_TABLE_ENTRY_VALID_SHIFT 31
+};
+
+/*
+ * FCoE pending work request CQE
+ */
+struct fcoe_pend_wq_cqe {
+	u16 wqe;
+#define FCOE_PEND_WQ_CQE_TASK_ID (0x3FFF<<0)
+#define FCOE_PEND_WQ_CQE_TASK_ID_SHIFT 0
+#define FCOE_PEND_WQ_CQE_CQE_TYPE (0x1<<14)
+#define FCOE_PEND_WQ_CQE_CQE_TYPE_SHIFT 14
+#define FCOE_PEND_WQ_CQE_TOGGLE_BIT (0x1<<15)
+#define FCOE_PEND_WQ_CQE_TOGGLE_BIT_SHIFT 15
+};
+
+
+/*
+ * FCoE RX statistics parameters section#0
+ */
+struct fcoe_rx_stat_params_section0 {
+	u32 fcoe_ver_cnt;
+	u32 fcoe_rx_pkt_cnt;
+	u32 fcoe_rx_byte_cnt;
+	u32 fcoe_rx_drop_pkt_cnt;
+};
+
+
+/*
+ * FCoE RX statistics parameters section#1
+ */
+struct fcoe_rx_stat_params_section1 {
+	u32 fc_crc_cnt;
+	u32 eofa_del_cnt;
+	u32 miss_frame_cnt;
+	u32 seq_timeout_cnt;
+	u32 drop_seq_cnt;
+	u32 fcoe_rx_drop_pkt_cnt;
+	u32 fcp_rx_pkt_cnt;
+	u32 reserved0;
+};
+
+
+/*
+ * FCoE TX statistics parameters
+ */
+struct fcoe_tx_stat_params {
+	u32 fcoe_tx_pkt_cnt;
+	u32 fcoe_tx_byte_cnt;
+	u32 fcp_tx_pkt_cnt;
+	u32 reserved0;
+};
+
+/*
+ * FCoE statistics parameters
+ */
+struct fcoe_statistics_params {
+	struct fcoe_tx_stat_params tx_stat;
+	struct fcoe_rx_stat_params_section0 rx_stat0;
+	struct fcoe_rx_stat_params_section1 rx_stat1;
+};
+
+
+/*
+ * FCoE t2 hash table entry (64 bytes)
+ */
+struct fcoe_t2_hash_table_entry {
+	struct fcoe_hash_table_entry data;
+	struct regpair next;
+	struct regpair reserved0[3];
+};
+
+/*
+ * FCoE unsolicited CQE
+ */
+struct fcoe_unsolicited_cqe {
+	u16 wqe;
+#define FCOE_UNSOLICITED_CQE_SUBTYPE (0x3<<0)
+#define FCOE_UNSOLICITED_CQE_SUBTYPE_SHIFT 0
+#define FCOE_UNSOLICITED_CQE_PKT_LEN (0xFFF<<2)
+#define FCOE_UNSOLICITED_CQE_PKT_LEN_SHIFT 2
+#define FCOE_UNSOLICITED_CQE_CQE_TYPE (0x1<<14)
+#define FCOE_UNSOLICITED_CQE_CQE_TYPE_SHIFT 14
+#define FCOE_UNSOLICITED_CQE_TOGGLE_BIT (0x1<<15)
+#define FCOE_UNSOLICITED_CQE_TOGGLE_BIT_SHIFT 15
+};
+
+
+
+#endif /* __57XX_FCOE_HSI_LINUX_LE__ */
diff --git a/drivers/scsi/bnx2fc/Kconfig b/drivers/scsi/bnx2fc/Kconfig
new file mode 100644
index 000000000000..6a38080e35ed
--- /dev/null
+++ b/drivers/scsi/bnx2fc/Kconfig
@@ -0,0 +1,11 @@
+config SCSI_BNX2X_FCOE
+	tristate "Broadcom NetXtreme II FCoE support"
+	depends on PCI
+	select NETDEVICES
+	select NETDEV_1000
+	select LIBFC
+	select LIBFCOE
+	select CNIC
+	---help---
+	This driver supports FCoE offload for the Broadcom NetXtreme II
+	devices.
diff --git a/drivers/scsi/bnx2fc/Makefile b/drivers/scsi/bnx2fc/Makefile
new file mode 100644
index 000000000000..a92695a25176
--- /dev/null
+++ b/drivers/scsi/bnx2fc/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_SCSI_BNX2X_FCOE) += bnx2fc.o
+
+bnx2fc-y := bnx2fc_els.o bnx2fc_fcoe.o bnx2fc_hwi.o bnx2fc_io.o bnx2fc_tgt.o
diff --git a/drivers/scsi/bnx2fc/bnx2fc.h b/drivers/scsi/bnx2fc/bnx2fc.h
new file mode 100644
index 000000000000..df2fc09ba479
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc.h
@@ -0,0 +1,511 @@
+#ifndef _BNX2FC_H_
+#define _BNX2FC_H_
+/* bnx2fc.h: Broadcom NetXtreme II Linux FCoE offload driver.
+ *
+ * Copyright (c) 2008 - 2010 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.
+ *
+ * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/kthread.h>
+#include <linux/crc32.h>
+#include <linux/cpu.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/timer.h>
+#include <linux/errno.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+#include <linux/dma-mapping.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/bitops.h>
+#include <linux/log2.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/io.h>
+
+#include <scsi/scsi.h>
+#include <scsi/scsi_host.h>
+#include <scsi/scsi_device.h>
+#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_eh.h>
+#include <scsi/scsi_tcq.h>
+#include <scsi/libfc.h>
+#include <scsi/libfcoe.h>
+#include <scsi/fc_encode.h>
+#include <scsi/scsi_transport.h>
+#include <scsi/scsi_transport_fc.h>
+#include <scsi/fc/fc_fip.h>
+#include <scsi/fc/fc_fc2.h>
+#include <scsi/fc_frame.h>
+#include <scsi/fc/fc_fcoe.h>
+#include <scsi/fc/fc_fcp.h>
+
+#include "57xx_hsi_bnx2fc.h"
+#include "bnx2fc_debug.h"
+#include "../../net/cnic_if.h"
+#include "bnx2fc_constants.h"
+
+#define BNX2FC_NAME		"bnx2fc"
+#define BNX2FC_VERSION		"1.0.0"
+
+#define PFX			"bnx2fc: "
+
+#define BNX2X_DOORBELL_PCI_BAR		2
+
+#define BNX2FC_MAX_BD_LEN		0xffff
+#define BNX2FC_BD_SPLIT_SZ		0x8000
+#define BNX2FC_MAX_BDS_PER_CMD		256
+
+#define BNX2FC_SQ_WQES_MAX	256
+
+#define BNX2FC_SCSI_MAX_SQES	((3 * BNX2FC_SQ_WQES_MAX) / 8)
+#define BNX2FC_TM_MAX_SQES	((BNX2FC_SQ_WQES_MAX) / 2)
+#define BNX2FC_ELS_MAX_SQES	(BNX2FC_TM_MAX_SQES - 1)
+
+#define BNX2FC_RQ_WQES_MAX	16
+#define BNX2FC_CQ_WQES_MAX	(BNX2FC_SQ_WQES_MAX + BNX2FC_RQ_WQES_MAX)
+
+#define BNX2FC_NUM_MAX_SESS	128
+#define BNX2FC_NUM_MAX_SESS_LOG	(ilog2(BNX2FC_NUM_MAX_SESS))
+
+#define BNX2FC_MAX_OUTSTANDING_CMNDS	4096
+#define BNX2FC_MIN_PAYLOAD		256
+#define BNX2FC_MAX_PAYLOAD		2048
+
+#define BNX2FC_RQ_BUF_SZ		256
+#define BNX2FC_RQ_BUF_LOG_SZ		(ilog2(BNX2FC_RQ_BUF_SZ))
+
+#define BNX2FC_SQ_WQE_SIZE		(sizeof(struct fcoe_sqe))
+#define BNX2FC_CQ_WQE_SIZE		(sizeof(struct fcoe_cqe))
+#define BNX2FC_RQ_WQE_SIZE		(BNX2FC_RQ_BUF_SZ)
+#define BNX2FC_XFERQ_WQE_SIZE		(sizeof(struct fcoe_xfrqe))
+#define BNX2FC_CONFQ_WQE_SIZE		(sizeof(struct fcoe_confqe))
+#define BNX2FC_5771X_DB_PAGE_SIZE	128
+
+#define BNX2FC_MAX_TASKS		BNX2FC_MAX_OUTSTANDING_CMNDS
+#define BNX2FC_TASK_SIZE		128
+#define	BNX2FC_TASKS_PER_PAGE		(PAGE_SIZE/BNX2FC_TASK_SIZE)
+#define BNX2FC_TASK_CTX_ARR_SZ		(BNX2FC_MAX_TASKS/BNX2FC_TASKS_PER_PAGE)
+
+#define BNX2FC_MAX_ROWS_IN_HASH_TBL	8
+#define BNX2FC_HASH_TBL_CHUNK_SIZE	(16 * 1024)
+
+#define BNX2FC_MAX_SEQS			255
+
+#define BNX2FC_READ			(1 << 1)
+#define BNX2FC_WRITE			(1 << 0)
+
+#define BNX2FC_MIN_XID			0
+#define BNX2FC_MAX_XID			(BNX2FC_MAX_OUTSTANDING_CMNDS - 1)
+#define FCOE_MIN_XID			(BNX2FC_MAX_OUTSTANDING_CMNDS)
+#define FCOE_MAX_XID		\
+			(BNX2FC_MAX_OUTSTANDING_CMNDS + (nr_cpu_ids * 256))
+#define BNX2FC_MAX_LUN			0xFFFF
+#define BNX2FC_MAX_FCP_TGT		256
+#define BNX2FC_MAX_CMD_LEN		16
+
+#define BNX2FC_TM_TIMEOUT		60	/* secs */
+#define BNX2FC_IO_TIMEOUT		20000UL	/* msecs */
+
+#define BNX2FC_WAIT_CNT			120
+#define BNX2FC_FW_TIMEOUT		(3 * HZ)
+
+#define PORT_MAX			2
+
+#define CMD_SCSI_STATUS(Cmnd)		((Cmnd)->SCp.Status)
+
+/* FC FCP Status */
+#define	FC_GOOD				0
+
+#define BNX2FC_RNID_HBA			0x7
+
+/* bnx2fc driver uses only one instance of fcoe_percpu_s */
+extern struct fcoe_percpu_s bnx2fc_global;
+
+extern struct workqueue_struct *bnx2fc_wq;
+
+struct bnx2fc_percpu_s {
+	struct task_struct *iothread;
+	struct list_head work_list;
+	spinlock_t fp_work_lock;
+};
+
+
+struct bnx2fc_hba {
+	struct list_head link;
+	struct cnic_dev *cnic;
+	struct pci_dev *pcidev;
+	struct net_device *netdev;
+	struct net_device *phys_dev;
+	unsigned long reg_with_cnic;
+		#define BNX2FC_CNIC_REGISTERED           1
+	struct packet_type fcoe_packet_type;
+	struct packet_type fip_packet_type;
+	struct bnx2fc_cmd_mgr *cmd_mgr;
+	struct workqueue_struct *timer_work_queue;
+	struct kref kref;
+	spinlock_t hba_lock;
+	struct mutex hba_mutex;
+	unsigned long adapter_state;
+		#define ADAPTER_STATE_UP		0
+		#define ADAPTER_STATE_GOING_DOWN	1
+		#define ADAPTER_STATE_LINK_DOWN		2
+		#define ADAPTER_STATE_READY		3
+	u32 flags;
+	unsigned long init_done;
+		#define BNX2FC_FW_INIT_DONE		0
+		#define BNX2FC_CTLR_INIT_DONE		1
+		#define BNX2FC_CREATE_DONE		2
+	struct fcoe_ctlr ctlr;
+	u8 vlan_enabled;
+	int vlan_id;
+	u32 next_conn_id;
+	struct fcoe_task_ctx_entry **task_ctx;
+	dma_addr_t *task_ctx_dma;
+	struct regpair *task_ctx_bd_tbl;
+	dma_addr_t task_ctx_bd_dma;
+
+	int hash_tbl_segment_count;
+	void **hash_tbl_segments;
+	void *hash_tbl_pbl;
+	dma_addr_t hash_tbl_pbl_dma;
+	struct fcoe_t2_hash_table_entry *t2_hash_tbl;
+	dma_addr_t t2_hash_tbl_dma;
+	char *t2_hash_tbl_ptr;
+	dma_addr_t t2_hash_tbl_ptr_dma;
+
+	char *dummy_buffer;
+	dma_addr_t dummy_buf_dma;
+
+	struct fcoe_statistics_params *stats_buffer;
+	dma_addr_t stats_buf_dma;
+
+	/*
+	 * PCI related info.
+	 */
+	u16 pci_did;
+	u16 pci_vid;
+	u16 pci_sdid;
+	u16 pci_svid;
+	u16 pci_func;
+	u16 pci_devno;
+
+	struct task_struct *l2_thread;
+
+	/* linkdown handling */
+	wait_queue_head_t shutdown_wait;
+	int wait_for_link_down;
+
+	/*destroy handling */
+	struct timer_list destroy_timer;
+	wait_queue_head_t destroy_wait;
+
+	/* Active list of offloaded sessions */
+	struct bnx2fc_rport *tgt_ofld_list[BNX2FC_NUM_MAX_SESS];
+	int num_ofld_sess;
+
+	/* statistics */
+	struct completion stat_req_done;
+};
+
+#define bnx2fc_from_ctlr(fip) container_of(fip, struct bnx2fc_hba, ctlr)
+
+struct bnx2fc_cmd_mgr {
+	struct bnx2fc_hba *hba;
+	u16 next_idx;
+	struct list_head *free_list;
+	spinlock_t *free_list_lock;
+	struct io_bdt **io_bdt_pool;
+	struct bnx2fc_cmd **cmds;
+};
+
+struct bnx2fc_rport {
+	struct fcoe_port *port;
+	struct fc_rport *rport;
+	struct fc_rport_priv *rdata;
+	void __iomem *ctx_base;
+#define DPM_TRIGER_TYPE		0x40
+	u32 fcoe_conn_id;
+	u32 context_id;
+	u32 sid;
+
+	unsigned long flags;
+#define BNX2FC_FLAG_SESSION_READY	0x1
+#define BNX2FC_FLAG_OFFLOADED		0x2
+#define BNX2FC_FLAG_DISABLED		0x3
+#define BNX2FC_FLAG_DESTROYED		0x4
+#define BNX2FC_FLAG_OFLD_REQ_CMPL	0x5
+#define BNX2FC_FLAG_DESTROY_CMPL	0x6
+#define BNX2FC_FLAG_CTX_ALLOC_FAILURE	0x7
+#define BNX2FC_FLAG_UPLD_REQ_COMPL	0x8
+#define BNX2FC_FLAG_EXPL_LOGO		0x9
+
+	u32 max_sqes;
+	u32 max_rqes;
+	u32 max_cqes;
+
+	struct fcoe_sqe *sq;
+	dma_addr_t sq_dma;
+	u16 sq_prod_idx;
+	u8 sq_curr_toggle_bit;
+	u32 sq_mem_size;
+
+	struct fcoe_cqe *cq;
+	dma_addr_t cq_dma;
+	u32 cq_cons_idx;
+	u8 cq_curr_toggle_bit;
+	u32 cq_mem_size;
+
+	void *rq;
+	dma_addr_t rq_dma;
+	u32 rq_prod_idx;
+	u32 rq_cons_idx;
+	u32 rq_mem_size;
+
+	void *rq_pbl;
+	dma_addr_t rq_pbl_dma;
+	u32 rq_pbl_size;
+
+	struct fcoe_xfrqe *xferq;
+	dma_addr_t xferq_dma;
+	u32 xferq_mem_size;
+
+	struct fcoe_confqe *confq;
+	dma_addr_t confq_dma;
+	u32 confq_mem_size;
+
+	void *confq_pbl;
+	dma_addr_t confq_pbl_dma;
+	u32 confq_pbl_size;
+
+	struct fcoe_conn_db *conn_db;
+	dma_addr_t conn_db_dma;
+	u32 conn_db_mem_size;
+
+	struct fcoe_sqe *lcq;
+	dma_addr_t lcq_dma;
+	u32 lcq_mem_size;
+
+	void *ofld_req[4];
+	dma_addr_t ofld_req_dma[4];
+	void *enbl_req;
+	dma_addr_t enbl_req_dma;
+
+	spinlock_t tgt_lock;
+	spinlock_t cq_lock;
+	atomic_t num_active_ios;
+	u32 flush_in_prog;
+	unsigned long work_time_slice;
+	unsigned long timestamp;
+	struct list_head free_task_list;
+	struct bnx2fc_cmd *pending_queue[BNX2FC_SQ_WQES_MAX+1];
+	atomic_t pi;
+	atomic_t ci;
+	struct list_head active_cmd_queue;
+	struct list_head els_queue;
+	struct list_head io_retire_queue;
+	struct list_head active_tm_queue;
+
+	struct timer_list ofld_timer;
+	wait_queue_head_t ofld_wait;
+
+	struct timer_list upld_timer;
+	wait_queue_head_t upld_wait;
+};
+
+struct bnx2fc_mp_req {
+	u8 tm_flags;
+
+	u32 req_len;
+	void *req_buf;
+	dma_addr_t req_buf_dma;
+	struct fcoe_bd_ctx *mp_req_bd;
+	dma_addr_t mp_req_bd_dma;
+	struct fc_frame_header req_fc_hdr;
+
+	u32 resp_len;
+	void *resp_buf;
+	dma_addr_t resp_buf_dma;
+	struct fcoe_bd_ctx *mp_resp_bd;
+	dma_addr_t mp_resp_bd_dma;
+	struct fc_frame_header resp_fc_hdr;
+};
+
+struct bnx2fc_els_cb_arg {
+	struct bnx2fc_cmd *aborted_io_req;
+	struct bnx2fc_cmd *io_req;
+	u16 l2_oxid;
+};
+
+/* bnx2fc command structure */
+struct bnx2fc_cmd {
+	struct list_head link;
+	u8 on_active_queue;
+	u8 on_tmf_queue;
+	u8 cmd_type;
+#define BNX2FC_SCSI_CMD		1
+#define BNX2FC_TASK_MGMT_CMD		2
+#define BNX2FC_ABTS			3
+#define BNX2FC_ELS			4
+#define BNX2FC_CLEANUP			5
+	u8 io_req_flags;
+	struct kref refcount;
+	struct fcoe_port *port;
+	struct bnx2fc_rport *tgt;
+	struct scsi_cmnd *sc_cmd;
+	struct bnx2fc_cmd_mgr *cmd_mgr;
+	struct bnx2fc_mp_req mp_req;
+	void (*cb_func)(struct bnx2fc_els_cb_arg *cb_arg);
+	struct bnx2fc_els_cb_arg *cb_arg;
+	struct delayed_work timeout_work; /* timer for ULP timeouts */
+	struct completion tm_done;
+	int wait_for_comp;
+	u16 xid;
+	struct fcoe_task_ctx_entry *task;
+	struct io_bdt *bd_tbl;
+	struct fcp_rsp *rsp;
+	size_t data_xfer_len;
+	unsigned long req_flags;
+#define BNX2FC_FLAG_ISSUE_RRQ		0x1
+#define BNX2FC_FLAG_ISSUE_ABTS		0x2
+#define BNX2FC_FLAG_ABTS_DONE		0x3
+#define BNX2FC_FLAG_TM_COMPL		0x4
+#define BNX2FC_FLAG_TM_TIMEOUT		0x5
+#define BNX2FC_FLAG_IO_CLEANUP		0x6
+#define BNX2FC_FLAG_RETIRE_OXID		0x7
+#define	BNX2FC_FLAG_EH_ABORT		0x8
+#define BNX2FC_FLAG_IO_COMPL		0x9
+#define BNX2FC_FLAG_ELS_DONE		0xa
+#define BNX2FC_FLAG_ELS_TIMEOUT		0xb
+	u32 fcp_resid;
+	u32 fcp_rsp_len;
+	u32 fcp_sns_len;
+	u8 cdb_status; /* SCSI IO status */
+	u8 fcp_status; /* FCP IO status */
+	u8 fcp_rsp_code;
+	u8 scsi_comp_flags;
+};
+
+struct io_bdt {
+	struct bnx2fc_cmd *io_req;
+	struct fcoe_bd_ctx *bd_tbl;
+	dma_addr_t bd_tbl_dma;
+	u16 bd_valid;
+};
+
+struct bnx2fc_work {
+	struct list_head list;
+	struct bnx2fc_rport *tgt;
+	u16 wqe;
+};
+struct bnx2fc_unsol_els {
+	struct fc_lport *lport;
+	struct fc_frame *fp;
+	struct work_struct unsol_els_work;
+};
+
+
+
+struct bnx2fc_cmd *bnx2fc_elstm_alloc(struct bnx2fc_rport *tgt, int type);
+void bnx2fc_cmd_release(struct kref *ref);
+int bnx2fc_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *sc_cmd);
+int bnx2fc_send_fw_fcoe_init_msg(struct bnx2fc_hba *hba);
+int bnx2fc_send_fw_fcoe_destroy_msg(struct bnx2fc_hba *hba);
+int bnx2fc_send_session_ofld_req(struct fcoe_port *port,
+					struct bnx2fc_rport *tgt);
+int bnx2fc_send_session_disable_req(struct fcoe_port *port,
+				    struct bnx2fc_rport *tgt);
+int bnx2fc_send_session_destroy_req(struct bnx2fc_hba *hba,
+					struct bnx2fc_rport *tgt);
+int bnx2fc_map_doorbell(struct bnx2fc_rport *tgt);
+void bnx2fc_indicate_kcqe(void *context, struct kcqe *kcq[],
+					u32 num_cqe);
+int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba);
+void bnx2fc_free_task_ctx(struct bnx2fc_hba *hba);
+int bnx2fc_setup_fw_resc(struct bnx2fc_hba *hba);
+void bnx2fc_free_fw_resc(struct bnx2fc_hba *hba);
+struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba,
+						u16 min_xid, u16 max_xid);
+void bnx2fc_cmd_mgr_free(struct bnx2fc_cmd_mgr *cmgr);
+void bnx2fc_get_link_state(struct bnx2fc_hba *hba);
+char *bnx2fc_get_next_rqe(struct bnx2fc_rport *tgt, u8 num_items);
+void bnx2fc_return_rqe(struct bnx2fc_rport *tgt, u8 num_items);
+int bnx2fc_get_paged_crc_eof(struct sk_buff *skb, int tlen);
+int bnx2fc_send_rrq(struct bnx2fc_cmd *aborted_io_req);
+int bnx2fc_send_adisc(struct bnx2fc_rport *tgt, struct fc_frame *fp);
+int bnx2fc_send_logo(struct bnx2fc_rport *tgt, struct fc_frame *fp);
+int bnx2fc_send_rls(struct bnx2fc_rport *tgt, struct fc_frame *fp);
+int bnx2fc_initiate_cleanup(struct bnx2fc_cmd *io_req);
+int bnx2fc_initiate_abts(struct bnx2fc_cmd *io_req);
+void bnx2fc_cmd_timer_set(struct bnx2fc_cmd *io_req,
+			  unsigned int timer_msec);
+int bnx2fc_init_mp_req(struct bnx2fc_cmd *io_req);
+void bnx2fc_init_cleanup_task(struct bnx2fc_cmd *io_req,
+			      struct fcoe_task_ctx_entry *task,
+			      u16 orig_xid);
+void bnx2fc_init_mp_task(struct bnx2fc_cmd *io_req,
+			 struct fcoe_task_ctx_entry *task);
+void bnx2fc_init_task(struct bnx2fc_cmd *io_req,
+			     struct fcoe_task_ctx_entry *task);
+void bnx2fc_add_2_sq(struct bnx2fc_rport *tgt, u16 xid);
+void bnx2fc_ring_doorbell(struct bnx2fc_rport *tgt);
+int bnx2fc_eh_abort(struct scsi_cmnd *sc_cmd);
+int bnx2fc_eh_host_reset(struct scsi_cmnd *sc_cmd);
+int bnx2fc_eh_target_reset(struct scsi_cmnd *sc_cmd);
+int bnx2fc_eh_device_reset(struct scsi_cmnd *sc_cmd);
+void bnx2fc_rport_event_handler(struct fc_lport *lport,
+				struct fc_rport_priv *rport,
+				enum fc_rport_event event);
+void bnx2fc_process_scsi_cmd_compl(struct bnx2fc_cmd *io_req,
+				   struct fcoe_task_ctx_entry *task,
+				   u8 num_rq);
+void bnx2fc_process_cleanup_compl(struct bnx2fc_cmd *io_req,
+			       struct fcoe_task_ctx_entry *task,
+			       u8 num_rq);
+void bnx2fc_process_abts_compl(struct bnx2fc_cmd *io_req,
+			       struct fcoe_task_ctx_entry *task,
+			       u8 num_rq);
+void bnx2fc_process_tm_compl(struct bnx2fc_cmd *io_req,
+			     struct fcoe_task_ctx_entry *task,
+			     u8 num_rq);
+void bnx2fc_process_els_compl(struct bnx2fc_cmd *els_req,
+			      struct fcoe_task_ctx_entry *task,
+			      u8 num_rq);
+void bnx2fc_build_fcp_cmnd(struct bnx2fc_cmd *io_req,
+			   struct fcp_cmnd *fcp_cmnd);
+
+
+
+void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt);
+struct fc_seq *bnx2fc_elsct_send(struct fc_lport *lport, u32 did,
+				      struct fc_frame *fp, unsigned int op,
+				      void (*resp)(struct fc_seq *,
+						   struct fc_frame *,
+						   void *),
+				      void *arg, u32 timeout);
+int bnx2fc_process_new_cqes(struct bnx2fc_rport *tgt);
+void bnx2fc_process_cq_compl(struct bnx2fc_rport *tgt, u16 wqe);
+struct bnx2fc_rport *bnx2fc_tgt_lookup(struct fcoe_port *port,
+					     u32 port_id);
+void bnx2fc_process_l2_frame_compl(struct bnx2fc_rport *tgt,
+				   unsigned char *buf,
+				   u32 frame_len, u16 l2_oxid);
+int bnx2fc_send_stat_req(struct bnx2fc_hba *hba);
+
+#endif
diff --git a/drivers/scsi/bnx2fc/bnx2fc_constants.h b/drivers/scsi/bnx2fc/bnx2fc_constants.h
new file mode 100644
index 000000000000..fe7769173c43
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_constants.h
@@ -0,0 +1,206 @@
+#ifndef __BNX2FC_CONSTANTS_H_
+#define __BNX2FC_CONSTANTS_H_
+
+/**
+ * This file defines HSI constants for the FCoE flows
+ */
+
+/* KWQ/KCQ FCoE layer code */
+#define FCOE_KWQE_LAYER_CODE   (7)
+
+/* KWQ (kernel work queue) request op codes */
+#define FCOE_KWQE_OPCODE_INIT1			(0)
+#define FCOE_KWQE_OPCODE_INIT2			(1)
+#define FCOE_KWQE_OPCODE_INIT3			(2)
+#define FCOE_KWQE_OPCODE_OFFLOAD_CONN1	(3)
+#define FCOE_KWQE_OPCODE_OFFLOAD_CONN2	(4)
+#define FCOE_KWQE_OPCODE_OFFLOAD_CONN3	(5)
+#define FCOE_KWQE_OPCODE_OFFLOAD_CONN4	(6)
+#define FCOE_KWQE_OPCODE_ENABLE_CONN	(7)
+#define FCOE_KWQE_OPCODE_DISABLE_CONN	(8)
+#define FCOE_KWQE_OPCODE_DESTROY_CONN	(9)
+#define FCOE_KWQE_OPCODE_DESTROY		(10)
+#define FCOE_KWQE_OPCODE_STAT			(11)
+
+/* KCQ (kernel completion queue) response op codes */
+#define FCOE_KCQE_OPCODE_INIT_FUNC				(0x10)
+#define FCOE_KCQE_OPCODE_DESTROY_FUNC			(0x11)
+#define FCOE_KCQE_OPCODE_STAT_FUNC				(0x12)
+#define FCOE_KCQE_OPCODE_OFFLOAD_CONN			(0x15)
+#define FCOE_KCQE_OPCODE_ENABLE_CONN			(0x16)
+#define FCOE_KCQE_OPCODE_DISABLE_CONN			(0x17)
+#define FCOE_KCQE_OPCODE_DESTROY_CONN			(0x18)
+#define FCOE_KCQE_OPCODE_CQ_EVENT_NOTIFICATION  (0x20)
+#define FCOE_KCQE_OPCODE_FCOE_ERROR				(0x21)
+
+/* KCQ (kernel completion queue) completion status */
+#define FCOE_KCQE_COMPLETION_STATUS_SUCCESS				(0x0)
+#define FCOE_KCQE_COMPLETION_STATUS_ERROR				(0x1)
+#define FCOE_KCQE_COMPLETION_STATUS_INVALID_OPCODE		(0x2)
+#define FCOE_KCQE_COMPLETION_STATUS_CTX_ALLOC_FAILURE	(0x3)
+#define FCOE_KCQE_COMPLETION_STATUS_CTX_FREE_FAILURE	(0x4)
+#define FCOE_KCQE_COMPLETION_STATUS_NIC_ERROR			(0x5)
+
+/* Unsolicited CQE type */
+#define FCOE_UNSOLICITED_FRAME_CQE_TYPE			0
+#define FCOE_ERROR_DETECTION_CQE_TYPE			1
+#define FCOE_WARNING_DETECTION_CQE_TYPE			2
+
+/* Task context constants */
+/* After driver has initialize the task in case timer services required */
+#define	FCOE_TASK_TX_STATE_INIT					0
+/* In case timer services are required then shall be updated by Xstorm after
+ * start processing the task. In case no timer facilities are required then the
+ * driver would initialize the state to this value */
+#define	FCOE_TASK_TX_STATE_NORMAL				1
+/* Task is under abort procedure. Updated in order to stop processing of
+ * pending WQEs on this task */
+#define	FCOE_TASK_TX_STATE_ABORT				2
+/* For E_D_T_TOV timer expiration in Xstorm (Class 2 only) */
+#define	FCOE_TASK_TX_STATE_ERROR				3
+/* For REC_TOV timer expiration indication received from Xstorm */
+#define	FCOE_TASK_TX_STATE_WARNING				4
+/* For completed unsolicited task */
+#define	FCOE_TASK_TX_STATE_UNSOLICITED_COMPLETED		5
+/* For exchange cleanup request task */
+#define	FCOE_TASK_TX_STATE_EXCHANGE_CLEANUP			6
+/* For sequence cleanup request task */
+#define	FCOE_TASK_TX_STATE_SEQUENCE_CLEANUP			7
+/* Mark task as aborted and indicate that ABTS was not transmitted */
+#define	FCOE_TASK_TX_STATE_BEFORE_ABTS_TX			8
+/* Mark task as aborted and indicate that ABTS was transmitted */
+#define	FCOE_TASK_TX_STATE_AFTER_ABTS_TX			9
+/* For completion the ABTS task. */
+#define	FCOE_TASK_TX_STATE_ABTS_TX_COMPLETED			10
+/* Mark task as aborted and indicate that Exchange cleanup was not transmitted
+ */
+#define	FCOE_TASK_TX_STATE_BEFORE_EXCHANGE_CLEANUP_TX		11
+/* Mark task as aborted and indicate that Exchange cleanup was transmitted */
+#define	FCOE_TASK_TX_STATE_AFTER_EXCHANGE_CLEANUP_TX		12
+
+#define	FCOE_TASK_RX_STATE_NORMAL				0
+#define	FCOE_TASK_RX_STATE_COMPLETED				1
+/* Obsolete: Intermediate completion (middle path with local completion) */
+#define	FCOE_TASK_RX_STATE_INTER_COMP				2
+/* For REC_TOV timer expiration indication received from Xstorm */
+#define	FCOE_TASK_RX_STATE_WARNING				3
+/* For E_D_T_TOV timer expiration in Ustorm */
+#define	FCOE_TASK_RX_STATE_ERROR				4
+/* ABTS ACC arrived wait for local completion to finally complete the task. */
+#define	FCOE_TASK_RX_STATE_ABTS_ACC_ARRIVED			5
+/* local completion arrived wait for ABTS ACC to finally complete the task. */
+#define	FCOE_TASK_RX_STATE_ABTS_LOCAL_COMP_ARRIVED		6
+/* Special completion indication in case of task was aborted. */
+#define FCOE_TASK_RX_STATE_ABTS_COMPLETED			7
+/* Special completion indication in case of task was cleaned. */
+#define FCOE_TASK_RX_STATE_EXCHANGE_CLEANUP_COMPLETED		8
+/* Special completion indication (in task requested the exchange cleanup) in
+ * case cleaned task is in non-valid. */
+#define FCOE_TASK_RX_STATE_ABORT_CLEANUP_COMPLETED		9
+/* Special completion indication (in task requested the sequence cleanup) in
+ * case cleaned task was already returned to normal. */
+#define FCOE_TASK_RX_STATE_IGNORED_SEQUENCE_CLEANUP		10
+/* Exchange cleanup arrived wait until xfer will be handled to finally
+ * complete the task. */
+#define	FCOE_TASK_RX_STATE_EXCHANGE_CLEANUP_ARRIVED		11
+/* Xfer handled, wait for exchange cleanup to finally complete the task. */
+#define	FCOE_TASK_RX_STATE_EXCHANGE_CLEANUP_HANDLED_XFER	12
+
+#define	FCOE_TASK_TYPE_WRITE			0
+#define	FCOE_TASK_TYPE_READ				1
+#define	FCOE_TASK_TYPE_MIDPATH			2
+#define	FCOE_TASK_TYPE_UNSOLICITED		3
+#define	FCOE_TASK_TYPE_ABTS				4
+#define	FCOE_TASK_TYPE_EXCHANGE_CLEANUP	5
+#define	FCOE_TASK_TYPE_SEQUENCE_CLEANUP	6
+
+#define FCOE_TASK_DEV_TYPE_DISK			0
+#define FCOE_TASK_DEV_TYPE_TAPE			1
+
+#define FCOE_TASK_CLASS_TYPE_3			0
+#define FCOE_TASK_CLASS_TYPE_2			1
+
+/* Everest FCoE connection type */
+#define B577XX_FCOE_CONNECTION_TYPE		4
+
+/* Error codes for Error Reporting in fast path flows */
+/* XFER error codes */
+#define FCOE_ERROR_CODE_XFER_OOO_RO					0
+#define FCOE_ERROR_CODE_XFER_RO_NOT_ALIGNED				1
+#define FCOE_ERROR_CODE_XFER_NULL_BURST_LEN				2
+#define FCOE_ERROR_CODE_XFER_RO_GREATER_THAN_DATA2TRNS			3
+#define FCOE_ERROR_CODE_XFER_INVALID_PAYLOAD_SIZE			4
+#define FCOE_ERROR_CODE_XFER_TASK_TYPE_NOT_WRITE			5
+#define FCOE_ERROR_CODE_XFER_PEND_XFER_SET				6
+#define FCOE_ERROR_CODE_XFER_OPENED_SEQ					7
+#define FCOE_ERROR_CODE_XFER_FCTL					8
+
+/* FCP RSP error codes */
+#define FCOE_ERROR_CODE_FCP_RSP_BIDI_FLAGS_SET				9
+#define FCOE_ERROR_CODE_FCP_RSP_UNDERFLOW				10
+#define FCOE_ERROR_CODE_FCP_RSP_OVERFLOW				11
+#define FCOE_ERROR_CODE_FCP_RSP_INVALID_LENGTH_FIELD			12
+#define FCOE_ERROR_CODE_FCP_RSP_INVALID_SNS_FIELD			13
+#define FCOE_ERROR_CODE_FCP_RSP_INVALID_PAYLOAD_SIZE			14
+#define FCOE_ERROR_CODE_FCP_RSP_PEND_XFER_SET				15
+#define FCOE_ERROR_CODE_FCP_RSP_OPENED_SEQ				16
+#define FCOE_ERROR_CODE_FCP_RSP_FCTL					17
+#define FCOE_ERROR_CODE_FCP_RSP_LAST_SEQ_RESET				18
+#define FCOE_ERROR_CODE_FCP_RSP_CONF_REQ_NOT_SUPPORTED_YET		19
+
+/* FCP DATA error codes */
+#define FCOE_ERROR_CODE_DATA_OOO_RO					20
+#define FCOE_ERROR_CODE_DATA_EXCEEDS_DEFINED_MAX_FRAME_SIZE		21
+#define FCOE_ERROR_CODE_DATA_EXCEEDS_DATA2TRNS				22
+#define FCOE_ERROR_CODE_DATA_SOFI3_SEQ_ACTIVE_SET			23
+#define FCOE_ERROR_CODE_DATA_SOFN_SEQ_ACTIVE_RESET			24
+#define FCOE_ERROR_CODE_DATA_EOFN_END_SEQ_SET				25
+#define FCOE_ERROR_CODE_DATA_EOFT_END_SEQ_RESET			26
+#define FCOE_ERROR_CODE_DATA_TASK_TYPE_NOT_READ			27
+#define FCOE_ERROR_CODE_DATA_FCTL					28
+
+/* Middle path error codes */
+#define FCOE_ERROR_CODE_MIDPATH_TYPE_NOT_ELS				29
+#define FCOE_ERROR_CODE_MIDPATH_SOFI3_SEQ_ACTIVE_SET			30
+#define FCOE_ERROR_CODE_MIDPATH_SOFN_SEQ_ACTIVE_RESET			31
+#define FCOE_ERROR_CODE_MIDPATH_EOFN_END_SEQ_SET			32
+#define FCOE_ERROR_CODE_MIDPATH_EOFT_END_SEQ_RESET			33
+#define FCOE_ERROR_CODE_MIDPATH_ELS_REPLY_FCTL				34
+#define FCOE_ERROR_CODE_MIDPATH_INVALID_REPLY				35
+#define FCOE_ERROR_CODE_MIDPATH_ELS_REPLY_RCTL				36
+
+/* ABTS error codes */
+#define FCOE_ERROR_CODE_ABTS_REPLY_F_CTL				37
+#define FCOE_ERROR_CODE_ABTS_REPLY_DDF_RCTL_FIELD			38
+#define FCOE_ERROR_CODE_ABTS_REPLY_INVALID_BLS_RCTL			39
+#define FCOE_ERROR_CODE_ABTS_REPLY_INVALID_RCTL			40
+#define FCOE_ERROR_CODE_ABTS_REPLY_RCTL_GENERAL_MISMATCH		41
+
+/* Common error codes */
+#define FCOE_ERROR_CODE_COMMON_MIDDLE_FRAME_WITH_PAD			42
+#define FCOE_ERROR_CODE_COMMON_SEQ_INIT_IN_TCE				43
+#define FCOE_ERROR_CODE_COMMON_FC_HDR_RX_ID_MISMATCH			44
+#define FCOE_ERROR_CODE_COMMON_INCORRECT_SEQ_CNT			45
+#define FCOE_ERROR_CODE_COMMON_DATA_FC_HDR_FCP_TYPE_MISMATCH		46
+#define FCOE_ERROR_CODE_COMMON_DATA_NO_MORE_SGES			47
+#define FCOE_ERROR_CODE_COMMON_OPTIONAL_FC_HDR				48
+#define FCOE_ERROR_CODE_COMMON_READ_TCE_OX_ID_TOO_BIG			49
+#define FCOE_ERROR_CODE_COMMON_DATA_WAS_NOT_TRANSMITTED		50
+
+/* Unsolicited Rx error codes */
+#define FCOE_ERROR_CODE_UNSOLICITED_TYPE_NOT_ELS			51
+#define FCOE_ERROR_CODE_UNSOLICITED_TYPE_NOT_BLS			52
+#define FCOE_ERROR_CODE_UNSOLICITED_FCTL_ELS				53
+#define FCOE_ERROR_CODE_UNSOLICITED_FCTL_BLS				54
+#define FCOE_ERROR_CODE_UNSOLICITED_R_CTL				55
+
+#define FCOE_ERROR_CODE_RW_TASK_DDF_RCTL_INFO_FIELD			56
+#define FCOE_ERROR_CODE_RW_TASK_INVALID_RCTL				57
+#define FCOE_ERROR_CODE_RW_TASK_RCTL_GENERAL_MISMATCH			58
+
+/* Timer error codes */
+#define FCOE_ERROR_CODE_E_D_TOV_TIMER_EXPIRATION			60
+#define FCOE_ERROR_CODE_REC_TOV_TIMER_EXPIRATION			61
+
+
+#endif /* BNX2FC_CONSTANTS_H_ */
diff --git a/drivers/scsi/bnx2fc/bnx2fc_debug.h b/drivers/scsi/bnx2fc/bnx2fc_debug.h
new file mode 100644
index 000000000000..7f6aff68cc53
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_debug.h
@@ -0,0 +1,70 @@
+#ifndef __BNX2FC_DEBUG__
+#define __BNX2FC_DEBUG__
+
+/* Log level bit mask */
+#define LOG_IO		0x01	/* scsi cmd error, cleanup */
+#define LOG_TGT		0x02	/* Session setup, cleanup, etc' */
+#define LOG_HBA		0x04	/* lport events, link, mtu, etc' */
+#define LOG_ELS		0x08	/* ELS logs */
+#define LOG_MISC	0x10	/* fcoe L2 frame related logs*/
+#define LOG_ALL		0xff	/* LOG all messages */
+
+extern unsigned int bnx2fc_debug_level;
+
+#define BNX2FC_CHK_LOGGING(LEVEL, CMD)					\
+	do {								\
+		if (unlikely(bnx2fc_debug_level & LEVEL))		\
+			do {						\
+				CMD;					\
+			} while (0);					\
+	} while (0)
+
+#define BNX2FC_ELS_DBG(fmt, arg...)					\
+	BNX2FC_CHK_LOGGING(LOG_ELS,					\
+			   printk(KERN_ALERT PFX fmt, ##arg))
+
+#define BNX2FC_MISC_DBG(fmt, arg...)					\
+	BNX2FC_CHK_LOGGING(LOG_MISC,					\
+			   printk(KERN_ALERT PFX fmt, ##arg))
+
+#define BNX2FC_IO_DBG(io_req, fmt, arg...)				\
+	do {								\
+		if (!io_req || !io_req->port || !io_req->port->lport ||	\
+		    !io_req->port->lport->host)				\
+			BNX2FC_CHK_LOGGING(LOG_IO,			\
+			   printk(KERN_ALERT PFX "NULL " fmt, ##arg));	\
+		else							\
+			BNX2FC_CHK_LOGGING(LOG_IO,			\
+			   shost_printk(KERN_ALERT,			\
+				   (io_req)->port->lport->host,		\
+				   PFX "xid:0x%x " fmt,			\
+				   (io_req)->xid, ##arg));		\
+	} while (0)
+
+#define BNX2FC_TGT_DBG(tgt, fmt, arg...)				\
+	do {								\
+		if (!tgt || !tgt->port || !tgt->port->lport ||		\
+		    !tgt->port->lport->host || !tgt->rport)		\
+			BNX2FC_CHK_LOGGING(LOG_TGT,			\
+			   printk(KERN_ALERT PFX "NULL " fmt, ##arg));	\
+		else							\
+			BNX2FC_CHK_LOGGING(LOG_TGT,			\
+			   shost_printk(KERN_ALERT,			\
+				   (tgt)->port->lport->host,		\
+				   PFX "port:%x " fmt,			\
+				   (tgt)->rport->port_id, ##arg));	\
+	} while (0)
+
+
+#define BNX2FC_HBA_DBG(lport, fmt, arg...)				\
+	do {								\
+		if (!lport || !lport->host)				\
+			BNX2FC_CHK_LOGGING(LOG_HBA,			\
+			   printk(KERN_ALERT PFX "NULL " fmt, ##arg));	\
+		else							\
+			BNX2FC_CHK_LOGGING(LOG_HBA,			\
+			   shost_printk(KERN_ALERT, lport->host,	\
+				   PFX fmt, ##arg));			\
+	} while (0)
+
+#endif
diff --git a/drivers/scsi/bnx2fc/bnx2fc_els.c b/drivers/scsi/bnx2fc/bnx2fc_els.c
new file mode 100644
index 000000000000..7a11a255157f
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_els.c
@@ -0,0 +1,515 @@
+/*
+ * bnx2fc_els.c: Broadcom NetXtreme II Linux FCoE offload driver.
+ * This file contains helper routines that handle ELS requests
+ * and responses.
+ *
+ * Copyright (c) 2008 - 2010 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.
+ *
+ * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
+ */
+
+#include "bnx2fc.h"
+
+static void bnx2fc_logo_resp(struct fc_seq *seq, struct fc_frame *fp,
+			     void *arg);
+static void bnx2fc_flogi_resp(struct fc_seq *seq, struct fc_frame *fp,
+			      void *arg);
+static int bnx2fc_initiate_els(struct bnx2fc_rport *tgt, unsigned int op,
+			void *data, u32 data_len,
+			void (*cb_func)(struct bnx2fc_els_cb_arg *cb_arg),
+			struct bnx2fc_els_cb_arg *cb_arg, u32 timer_msec);
+
+static void bnx2fc_rrq_compl(struct bnx2fc_els_cb_arg *cb_arg)
+{
+	struct bnx2fc_cmd *orig_io_req;
+	struct bnx2fc_cmd *rrq_req;
+	int rc = 0;
+
+	BUG_ON(!cb_arg);
+	rrq_req = cb_arg->io_req;
+	orig_io_req = cb_arg->aborted_io_req;
+	BUG_ON(!orig_io_req);
+	BNX2FC_ELS_DBG("rrq_compl: orig xid = 0x%x, rrq_xid = 0x%x\n",
+		   orig_io_req->xid, rrq_req->xid);
+
+	kref_put(&orig_io_req->refcount, bnx2fc_cmd_release);
+
+	if (test_and_clear_bit(BNX2FC_FLAG_ELS_TIMEOUT, &rrq_req->req_flags)) {
+		/*
+		 * els req is timed out. cleanup the IO with FW and
+		 * drop the completion. Remove from active_cmd_queue.
+		 */
+		BNX2FC_ELS_DBG("rrq xid - 0x%x timed out, clean it up\n",
+			   rrq_req->xid);
+
+		if (rrq_req->on_active_queue) {
+			list_del_init(&rrq_req->link);
+			rrq_req->on_active_queue = 0;
+			rc = bnx2fc_initiate_cleanup(rrq_req);
+			BUG_ON(rc);
+		}
+	}
+	kfree(cb_arg);
+}
+int bnx2fc_send_rrq(struct bnx2fc_cmd *aborted_io_req)
+{
+
+	struct fc_els_rrq rrq;
+	struct bnx2fc_rport *tgt = aborted_io_req->tgt;
+	struct fc_lport *lport = tgt->rdata->local_port;
+	struct bnx2fc_els_cb_arg *cb_arg = NULL;
+	u32 sid = tgt->sid;
+	u32 r_a_tov = lport->r_a_tov;
+	unsigned long start = jiffies;
+	int rc;
+
+	BNX2FC_ELS_DBG("Sending RRQ orig_xid = 0x%x\n",
+		   aborted_io_req->xid);
+	memset(&rrq, 0, sizeof(rrq));
+
+	cb_arg = kzalloc(sizeof(struct bnx2fc_els_cb_arg), GFP_NOIO);
+	if (!cb_arg) {
+		printk(KERN_ERR PFX "Unable to allocate cb_arg for RRQ\n");
+		rc = -ENOMEM;
+		goto rrq_err;
+	}
+
+	cb_arg->aborted_io_req = aborted_io_req;
+
+	rrq.rrq_cmd = ELS_RRQ;
+	hton24(rrq.rrq_s_id, sid);
+	rrq.rrq_ox_id = htons(aborted_io_req->xid);
+	rrq.rrq_rx_id = htons(aborted_io_req->task->rx_wr_tx_rd.rx_id);
+
+retry_rrq:
+	rc = bnx2fc_initiate_els(tgt, ELS_RRQ, &rrq, sizeof(rrq),
+				 bnx2fc_rrq_compl, cb_arg,
+				 r_a_tov);
+	if (rc == -ENOMEM) {
+		if (time_after(jiffies, start + (10 * HZ))) {
+			BNX2FC_ELS_DBG("rrq Failed\n");
+			rc = FAILED;
+			goto rrq_err;
+		}
+		msleep(20);
+		goto retry_rrq;
+	}
+rrq_err:
+	if (rc) {
+		BNX2FC_ELS_DBG("RRQ failed - release orig io req 0x%x\n",
+			aborted_io_req->xid);
+		kfree(cb_arg);
+		spin_lock_bh(&tgt->tgt_lock);
+		kref_put(&aborted_io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+	}
+	return rc;
+}
+
+static void bnx2fc_l2_els_compl(struct bnx2fc_els_cb_arg *cb_arg)
+{
+	struct bnx2fc_cmd *els_req;
+	struct bnx2fc_rport *tgt;
+	struct bnx2fc_mp_req *mp_req;
+	struct fc_frame_header *fc_hdr;
+	unsigned char *buf;
+	void *resp_buf;
+	u32 resp_len, hdr_len;
+	u16 l2_oxid;
+	int frame_len;
+	int rc = 0;
+
+	l2_oxid = cb_arg->l2_oxid;
+	BNX2FC_ELS_DBG("ELS COMPL - l2_oxid = 0x%x\n", l2_oxid);
+
+	els_req = cb_arg->io_req;
+	if (test_and_clear_bit(BNX2FC_FLAG_ELS_TIMEOUT, &els_req->req_flags)) {
+		/*
+		 * els req is timed out. cleanup the IO with FW and
+		 * drop the completion. libfc will handle the els timeout
+		 */
+		if (els_req->on_active_queue) {
+			list_del_init(&els_req->link);
+			els_req->on_active_queue = 0;
+			rc = bnx2fc_initiate_cleanup(els_req);
+			BUG_ON(rc);
+		}
+		goto free_arg;
+	}
+
+	tgt = els_req->tgt;
+	mp_req = &(els_req->mp_req);
+	fc_hdr = &(mp_req->resp_fc_hdr);
+	resp_len = mp_req->resp_len;
+	resp_buf = mp_req->resp_buf;
+
+	buf = kzalloc(PAGE_SIZE, GFP_ATOMIC);
+	if (!buf) {
+		printk(KERN_ERR PFX "Unable to alloc mp buf\n");
+		goto free_arg;
+	}
+	hdr_len = sizeof(*fc_hdr);
+	if (hdr_len + resp_len > PAGE_SIZE) {
+		printk(KERN_ERR PFX "l2_els_compl: resp len is "
+				    "beyond page size\n");
+		goto free_buf;
+	}
+	memcpy(buf, fc_hdr, hdr_len);
+	memcpy(buf + hdr_len, resp_buf, resp_len);
+	frame_len = hdr_len + resp_len;
+
+	bnx2fc_process_l2_frame_compl(tgt, buf, frame_len, l2_oxid);
+
+free_buf:
+	kfree(buf);
+free_arg:
+	kfree(cb_arg);
+}
+
+int bnx2fc_send_adisc(struct bnx2fc_rport *tgt, struct fc_frame *fp)
+{
+	struct fc_els_adisc *adisc;
+	struct fc_frame_header *fh;
+	struct bnx2fc_els_cb_arg *cb_arg;
+	struct fc_lport *lport = tgt->rdata->local_port;
+	u32 r_a_tov = lport->r_a_tov;
+	int rc;
+
+	fh = fc_frame_header_get(fp);
+	cb_arg = kzalloc(sizeof(struct bnx2fc_els_cb_arg), GFP_ATOMIC);
+	if (!cb_arg) {
+		printk(KERN_ERR PFX "Unable to allocate cb_arg for ADISC\n");
+		return -ENOMEM;
+	}
+
+	cb_arg->l2_oxid = ntohs(fh->fh_ox_id);
+
+	BNX2FC_ELS_DBG("send ADISC: l2_oxid = 0x%x\n", cb_arg->l2_oxid);
+	adisc = fc_frame_payload_get(fp, sizeof(*adisc));
+	/* adisc is initialized by libfc */
+	rc = bnx2fc_initiate_els(tgt, ELS_ADISC, adisc, sizeof(*adisc),
+				 bnx2fc_l2_els_compl, cb_arg, 2 * r_a_tov);
+	if (rc)
+		kfree(cb_arg);
+	return rc;
+}
+
+int bnx2fc_send_logo(struct bnx2fc_rport *tgt, struct fc_frame *fp)
+{
+	struct fc_els_logo *logo;
+	struct fc_frame_header *fh;
+	struct bnx2fc_els_cb_arg *cb_arg;
+	struct fc_lport *lport = tgt->rdata->local_port;
+	u32 r_a_tov = lport->r_a_tov;
+	int rc;
+
+	fh = fc_frame_header_get(fp);
+	cb_arg = kzalloc(sizeof(struct bnx2fc_els_cb_arg), GFP_ATOMIC);
+	if (!cb_arg) {
+		printk(KERN_ERR PFX "Unable to allocate cb_arg for LOGO\n");
+		return -ENOMEM;
+	}
+
+	cb_arg->l2_oxid = ntohs(fh->fh_ox_id);
+
+	BNX2FC_ELS_DBG("Send LOGO: l2_oxid = 0x%x\n", cb_arg->l2_oxid);
+	logo = fc_frame_payload_get(fp, sizeof(*logo));
+	/* logo is initialized by libfc */
+	rc = bnx2fc_initiate_els(tgt, ELS_LOGO, logo, sizeof(*logo),
+				 bnx2fc_l2_els_compl, cb_arg, 2 * r_a_tov);
+	if (rc)
+		kfree(cb_arg);
+	return rc;
+}
+
+int bnx2fc_send_rls(struct bnx2fc_rport *tgt, struct fc_frame *fp)
+{
+	struct fc_els_rls *rls;
+	struct fc_frame_header *fh;
+	struct bnx2fc_els_cb_arg *cb_arg;
+	struct fc_lport *lport = tgt->rdata->local_port;
+	u32 r_a_tov = lport->r_a_tov;
+	int rc;
+
+	fh = fc_frame_header_get(fp);
+	cb_arg = kzalloc(sizeof(struct bnx2fc_els_cb_arg), GFP_ATOMIC);
+	if (!cb_arg) {
+		printk(KERN_ERR PFX "Unable to allocate cb_arg for LOGO\n");
+		return -ENOMEM;
+	}
+
+	cb_arg->l2_oxid = ntohs(fh->fh_ox_id);
+
+	rls = fc_frame_payload_get(fp, sizeof(*rls));
+	/* rls is initialized by libfc */
+	rc = bnx2fc_initiate_els(tgt, ELS_RLS, rls, sizeof(*rls),
+				  bnx2fc_l2_els_compl, cb_arg, 2 * r_a_tov);
+	if (rc)
+		kfree(cb_arg);
+	return rc;
+}
+
+static int bnx2fc_initiate_els(struct bnx2fc_rport *tgt, unsigned int op,
+			void *data, u32 data_len,
+			void (*cb_func)(struct bnx2fc_els_cb_arg *cb_arg),
+			struct bnx2fc_els_cb_arg *cb_arg, u32 timer_msec)
+{
+	struct fcoe_port *port = tgt->port;
+	struct bnx2fc_hba *hba = port->priv;
+	struct fc_rport *rport = tgt->rport;
+	struct fc_lport *lport = port->lport;
+	struct bnx2fc_cmd *els_req;
+	struct bnx2fc_mp_req *mp_req;
+	struct fc_frame_header *fc_hdr;
+	struct fcoe_task_ctx_entry *task;
+	struct fcoe_task_ctx_entry *task_page;
+	int rc = 0;
+	int task_idx, index;
+	u32 did, sid;
+	u16 xid;
+
+	rc = fc_remote_port_chkready(rport);
+	if (rc) {
+		printk(KERN_ALERT PFX "els 0x%x: rport not ready\n", op);
+		rc = -EINVAL;
+		goto els_err;
+	}
+	if (lport->state != LPORT_ST_READY || !(lport->link_up)) {
+		printk(KERN_ALERT PFX "els 0x%x: link is not ready\n", op);
+		rc = -EINVAL;
+		goto els_err;
+	}
+	if (!(test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags)) ||
+	     (test_bit(BNX2FC_FLAG_EXPL_LOGO, &tgt->flags))) {
+		printk(KERN_ERR PFX "els 0x%x: tgt not ready\n", op);
+		rc = -EINVAL;
+		goto els_err;
+	}
+	els_req = bnx2fc_elstm_alloc(tgt, BNX2FC_ELS);
+	if (!els_req) {
+		rc = -ENOMEM;
+		goto els_err;
+	}
+
+	els_req->sc_cmd = NULL;
+	els_req->port = port;
+	els_req->tgt = tgt;
+	els_req->cb_func = cb_func;
+	cb_arg->io_req = els_req;
+	els_req->cb_arg = cb_arg;
+
+	mp_req = (struct bnx2fc_mp_req *)&(els_req->mp_req);
+	rc = bnx2fc_init_mp_req(els_req);
+	if (rc == FAILED) {
+		printk(KERN_ALERT PFX "ELS MP request init failed\n");
+		spin_lock_bh(&tgt->tgt_lock);
+		kref_put(&els_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		rc = -ENOMEM;
+		goto els_err;
+	} else {
+		/* rc SUCCESS */
+		rc = 0;
+	}
+
+	/* Set the data_xfer_len to the size of ELS payload */
+	mp_req->req_len = data_len;
+	els_req->data_xfer_len = mp_req->req_len;
+
+	/* Fill ELS Payload */
+	if ((op >= ELS_LS_RJT) && (op <= ELS_AUTH_ELS)) {
+		memcpy(mp_req->req_buf, data, data_len);
+	} else {
+		printk(KERN_ALERT PFX "Invalid ELS op 0x%x\n", op);
+		els_req->cb_func = NULL;
+		els_req->cb_arg = NULL;
+		spin_lock_bh(&tgt->tgt_lock);
+		kref_put(&els_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		rc = -EINVAL;
+	}
+
+	if (rc)
+		goto els_err;
+
+	/* Fill FC header */
+	fc_hdr = &(mp_req->req_fc_hdr);
+
+	did = tgt->rport->port_id;
+	sid = tgt->sid;
+
+	__fc_fill_fc_hdr(fc_hdr, FC_RCTL_ELS_REQ, did, sid,
+			   FC_TYPE_ELS, FC_FC_FIRST_SEQ | FC_FC_END_SEQ |
+			   FC_FC_SEQ_INIT, 0);
+
+	/* Obtain exchange id */
+	xid = els_req->xid;
+	task_idx = xid/BNX2FC_TASKS_PER_PAGE;
+	index = xid % BNX2FC_TASKS_PER_PAGE;
+
+	/* Initialize task context for this IO request */
+	task_page = (struct fcoe_task_ctx_entry *) hba->task_ctx[task_idx];
+	task = &(task_page[index]);
+	bnx2fc_init_mp_task(els_req, task);
+
+	spin_lock_bh(&tgt->tgt_lock);
+
+	if (!test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags)) {
+		printk(KERN_ERR PFX "initiate_els.. session not ready\n");
+		els_req->cb_func = NULL;
+		els_req->cb_arg = NULL;
+		kref_put(&els_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return -EINVAL;
+	}
+
+	if (timer_msec)
+		bnx2fc_cmd_timer_set(els_req, timer_msec);
+	bnx2fc_add_2_sq(tgt, xid);
+
+	els_req->on_active_queue = 1;
+	list_add_tail(&els_req->link, &tgt->els_queue);
+
+	/* Ring doorbell */
+	bnx2fc_ring_doorbell(tgt);
+	spin_unlock_bh(&tgt->tgt_lock);
+
+els_err:
+	return rc;
+}
+
+void bnx2fc_process_els_compl(struct bnx2fc_cmd *els_req,
+			      struct fcoe_task_ctx_entry *task, u8 num_rq)
+{
+	struct bnx2fc_mp_req *mp_req;
+	struct fc_frame_header *fc_hdr;
+	u64 *hdr;
+	u64 *temp_hdr;
+
+	BNX2FC_ELS_DBG("Entered process_els_compl xid = 0x%x"
+			"cmd_type = %d\n", els_req->xid, els_req->cmd_type);
+
+	if (test_and_set_bit(BNX2FC_FLAG_ELS_DONE,
+			     &els_req->req_flags)) {
+		BNX2FC_ELS_DBG("Timer context finished processing this "
+			   "els - 0x%x\n", els_req->xid);
+		/* This IO doesnt receive cleanup completion */
+		kref_put(&els_req->refcount, bnx2fc_cmd_release);
+		return;
+	}
+
+	/* Cancel the timeout_work, as we received the response */
+	if (cancel_delayed_work(&els_req->timeout_work))
+		kref_put(&els_req->refcount,
+			 bnx2fc_cmd_release); /* drop timer hold */
+
+	if (els_req->on_active_queue) {
+		list_del_init(&els_req->link);
+		els_req->on_active_queue = 0;
+	}
+
+	mp_req = &(els_req->mp_req);
+	fc_hdr = &(mp_req->resp_fc_hdr);
+
+	hdr = (u64 *)fc_hdr;
+	temp_hdr = (u64 *)
+		&task->cmn.general.cmd_info.mp_fc_frame.fc_hdr;
+	hdr[0] = cpu_to_be64(temp_hdr[0]);
+	hdr[1] = cpu_to_be64(temp_hdr[1]);
+	hdr[2] = cpu_to_be64(temp_hdr[2]);
+
+	mp_req->resp_len = task->rx_wr_only.sgl_ctx.mul_sges.cur_sge_off;
+
+	/* Parse ELS response */
+	if ((els_req->cb_func) && (els_req->cb_arg)) {
+		els_req->cb_func(els_req->cb_arg);
+		els_req->cb_arg = NULL;
+	}
+
+	kref_put(&els_req->refcount, bnx2fc_cmd_release);
+}
+
+static void bnx2fc_flogi_resp(struct fc_seq *seq, struct fc_frame *fp,
+			      void *arg)
+{
+	struct fcoe_ctlr *fip = arg;
+	struct fc_exch *exch = fc_seq_exch(seq);
+	struct fc_lport *lport = exch->lp;
+	u8 *mac;
+	struct fc_frame_header *fh;
+	u8 op;
+
+	if (IS_ERR(fp))
+		goto done;
+
+	mac = fr_cb(fp)->granted_mac;
+	if (is_zero_ether_addr(mac)) {
+		fh = fc_frame_header_get(fp);
+		if (fh->fh_type != FC_TYPE_ELS) {
+			printk(KERN_ERR PFX "bnx2fc_flogi_resp:"
+				"fh_type != FC_TYPE_ELS\n");
+			fc_frame_free(fp);
+			return;
+		}
+		op = fc_frame_payload_op(fp);
+		if (lport->vport) {
+			if (op == ELS_LS_RJT) {
+				printk(KERN_ERR PFX "bnx2fc_flogi_resp is LS_RJT\n");
+				fc_vport_terminate(lport->vport);
+				fc_frame_free(fp);
+				return;
+			}
+		}
+		if (fcoe_ctlr_recv_flogi(fip, lport, fp)) {
+			fc_frame_free(fp);
+			return;
+		}
+	}
+	fip->update_mac(lport, mac);
+done:
+	fc_lport_flogi_resp(seq, fp, lport);
+}
+
+static void bnx2fc_logo_resp(struct fc_seq *seq, struct fc_frame *fp,
+			     void *arg)
+{
+	struct fcoe_ctlr *fip = arg;
+	struct fc_exch *exch = fc_seq_exch(seq);
+	struct fc_lport *lport = exch->lp;
+	static u8 zero_mac[ETH_ALEN] = { 0 };
+
+	if (!IS_ERR(fp))
+		fip->update_mac(lport, zero_mac);
+	fc_lport_logo_resp(seq, fp, lport);
+}
+
+struct fc_seq *bnx2fc_elsct_send(struct fc_lport *lport, u32 did,
+				      struct fc_frame *fp, unsigned int op,
+				      void (*resp)(struct fc_seq *,
+						   struct fc_frame *,
+						   void *),
+				      void *arg, u32 timeout)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct fcoe_ctlr *fip = &hba->ctlr;
+	struct fc_frame_header *fh = fc_frame_header_get(fp);
+
+	switch (op) {
+	case ELS_FLOGI:
+	case ELS_FDISC:
+		return fc_elsct_send(lport, did, fp, op, bnx2fc_flogi_resp,
+				     fip, timeout);
+	case ELS_LOGO:
+		/* only hook onto fabric logouts, not port logouts */
+		if (ntoh24(fh->fh_d_id) != FC_FID_FLOGI)
+			break;
+		return fc_elsct_send(lport, did, fp, op, bnx2fc_logo_resp,
+				     fip, timeout);
+	}
+	return fc_elsct_send(lport, did, fp, op, resp, arg, timeout);
+}
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
new file mode 100644
index 000000000000..e476e8753079
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -0,0 +1,2535 @@
+/* bnx2fc_fcoe.c: Broadcom NetXtreme II Linux FCoE offload driver.
+ * This file contains the code that interacts with libfc, libfcoe,
+ * cnic modules to create FCoE instances, send/receive non-offloaded
+ * FIP/FCoE packets, listen to link events etc.
+ *
+ * Copyright (c) 2008 - 2010 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.
+ *
+ * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
+ */
+
+#include "bnx2fc.h"
+
+static struct list_head adapter_list;
+static u32 adapter_count;
+static DEFINE_MUTEX(bnx2fc_dev_lock);
+DEFINE_PER_CPU(struct bnx2fc_percpu_s, bnx2fc_percpu);
+
+#define DRV_MODULE_NAME		"bnx2fc"
+#define DRV_MODULE_VERSION	BNX2FC_VERSION
+#define DRV_MODULE_RELDATE	"Jan 25, 2011"
+
+
+static char version[] __devinitdata =
+		"Broadcom NetXtreme II FCoE Driver " DRV_MODULE_NAME \
+		" v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
+
+
+MODULE_AUTHOR("Bhanu Prakash Gollapudi <bprakash@broadcom.com>");
+MODULE_DESCRIPTION("Broadcom NetXtreme II BCM57710 FCoE Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRV_MODULE_VERSION);
+
+#define BNX2FC_MAX_QUEUE_DEPTH	256
+#define BNX2FC_MIN_QUEUE_DEPTH	32
+#define FCOE_WORD_TO_BYTE  4
+
+static struct scsi_transport_template	*bnx2fc_transport_template;
+static struct scsi_transport_template	*bnx2fc_vport_xport_template;
+
+struct workqueue_struct *bnx2fc_wq;
+
+/* bnx2fc structure needs only one instance of the fcoe_percpu_s structure.
+ * Here the io threads are per cpu but the l2 thread is just one
+ */
+struct fcoe_percpu_s bnx2fc_global;
+DEFINE_SPINLOCK(bnx2fc_global_lock);
+
+static struct cnic_ulp_ops bnx2fc_cnic_cb;
+static struct libfc_function_template bnx2fc_libfc_fcn_templ;
+static struct scsi_host_template bnx2fc_shost_template;
+static struct fc_function_template bnx2fc_transport_function;
+static struct fc_function_template bnx2fc_vport_xport_function;
+static int bnx2fc_create(struct net_device *netdev, enum fip_state fip_mode);
+static int bnx2fc_destroy(struct net_device *net_device);
+static int bnx2fc_enable(struct net_device *netdev);
+static int bnx2fc_disable(struct net_device *netdev);
+
+static void bnx2fc_recv_frame(struct sk_buff *skb);
+
+static void bnx2fc_start_disc(struct bnx2fc_hba *hba);
+static int bnx2fc_shost_config(struct fc_lport *lport, struct device *dev);
+static int bnx2fc_net_config(struct fc_lport *lp);
+static int bnx2fc_lport_config(struct fc_lport *lport);
+static int bnx2fc_em_config(struct fc_lport *lport);
+static int bnx2fc_bind_adapter_devices(struct bnx2fc_hba *hba);
+static void bnx2fc_unbind_adapter_devices(struct bnx2fc_hba *hba);
+static int bnx2fc_bind_pcidev(struct bnx2fc_hba *hba);
+static void bnx2fc_unbind_pcidev(struct bnx2fc_hba *hba);
+static struct fc_lport *bnx2fc_if_create(struct bnx2fc_hba *hba,
+				  struct device *parent, int npiv);
+static void bnx2fc_destroy_work(struct work_struct *work);
+
+static struct bnx2fc_hba *bnx2fc_hba_lookup(struct net_device *phys_dev);
+static struct bnx2fc_hba *bnx2fc_find_hba_for_cnic(struct cnic_dev *cnic);
+
+static int bnx2fc_fw_init(struct bnx2fc_hba *hba);
+static void bnx2fc_fw_destroy(struct bnx2fc_hba *hba);
+
+static void bnx2fc_port_shutdown(struct fc_lport *lport);
+static void bnx2fc_stop(struct bnx2fc_hba *hba);
+static int __init bnx2fc_mod_init(void);
+static void __exit bnx2fc_mod_exit(void);
+
+unsigned int bnx2fc_debug_level;
+module_param_named(debug_logging, bnx2fc_debug_level, int, S_IRUGO|S_IWUSR);
+
+static int bnx2fc_cpu_callback(struct notifier_block *nfb,
+			     unsigned long action, void *hcpu);
+/* notification function for CPU hotplug events */
+static struct notifier_block bnx2fc_cpu_notifier = {
+	.notifier_call = bnx2fc_cpu_callback,
+};
+
+static void bnx2fc_clean_rx_queue(struct fc_lport *lp)
+{
+	struct fcoe_percpu_s *bg;
+	struct fcoe_rcv_info *fr;
+	struct sk_buff_head *list;
+	struct sk_buff *skb, *next;
+	struct sk_buff *head;
+
+	bg = &bnx2fc_global;
+	spin_lock_bh(&bg->fcoe_rx_list.lock);
+	list = &bg->fcoe_rx_list;
+	head = list->next;
+	for (skb = head; skb != (struct sk_buff *)list;
+	     skb = next) {
+		next = skb->next;
+		fr = fcoe_dev_from_skb(skb);
+		if (fr->fr_dev == lp) {
+			__skb_unlink(skb, list);
+			kfree_skb(skb);
+		}
+	}
+	spin_unlock_bh(&bg->fcoe_rx_list.lock);
+}
+
+int bnx2fc_get_paged_crc_eof(struct sk_buff *skb, int tlen)
+{
+	int rc;
+	spin_lock(&bnx2fc_global_lock);
+	rc = fcoe_get_paged_crc_eof(skb, tlen, &bnx2fc_global);
+	spin_unlock(&bnx2fc_global_lock);
+
+	return rc;
+}
+
+static void bnx2fc_abort_io(struct fc_lport *lport)
+{
+	/*
+	 * This function is no-op for bnx2fc, but we do
+	 * not want to leave it as NULL either, as libfc
+	 * can call the default function which is
+	 * fc_fcp_abort_io.
+	 */
+}
+
+static void bnx2fc_cleanup(struct fc_lport *lport)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct bnx2fc_rport *tgt;
+	int i;
+
+	BNX2FC_MISC_DBG("Entered %s\n", __func__);
+	mutex_lock(&hba->hba_mutex);
+	spin_lock_bh(&hba->hba_lock);
+	for (i = 0; i < BNX2FC_NUM_MAX_SESS; i++) {
+		tgt = hba->tgt_ofld_list[i];
+		if (tgt) {
+			/* Cleanup IOs belonging to requested vport */
+			if (tgt->port == port) {
+				spin_unlock_bh(&hba->hba_lock);
+				BNX2FC_TGT_DBG(tgt, "flush/cleanup\n");
+				bnx2fc_flush_active_ios(tgt);
+				spin_lock_bh(&hba->hba_lock);
+			}
+		}
+	}
+	spin_unlock_bh(&hba->hba_lock);
+	mutex_unlock(&hba->hba_mutex);
+}
+
+static int bnx2fc_xmit_l2_frame(struct bnx2fc_rport *tgt,
+			     struct fc_frame *fp)
+{
+	struct fc_rport_priv *rdata = tgt->rdata;
+	struct fc_frame_header *fh;
+	int rc = 0;
+
+	fh = fc_frame_header_get(fp);
+	BNX2FC_TGT_DBG(tgt, "Xmit L2 frame rport = 0x%x, oxid = 0x%x, "
+			"r_ctl = 0x%x\n", rdata->ids.port_id,
+			ntohs(fh->fh_ox_id), fh->fh_r_ctl);
+	if ((fh->fh_type == FC_TYPE_ELS) &&
+	    (fh->fh_r_ctl == FC_RCTL_ELS_REQ)) {
+
+		switch (fc_frame_payload_op(fp)) {
+		case ELS_ADISC:
+			rc = bnx2fc_send_adisc(tgt, fp);
+			break;
+		case ELS_LOGO:
+			rc = bnx2fc_send_logo(tgt, fp);
+			break;
+		case ELS_RLS:
+			rc = bnx2fc_send_rls(tgt, fp);
+			break;
+		default:
+			break;
+		}
+	} else if ((fh->fh_type ==  FC_TYPE_BLS) &&
+	    (fh->fh_r_ctl == FC_RCTL_BA_ABTS))
+		BNX2FC_TGT_DBG(tgt, "ABTS frame\n");
+	else {
+		BNX2FC_TGT_DBG(tgt, "Send L2 frame type 0x%x "
+				"rctl 0x%x thru non-offload path\n",
+				fh->fh_type, fh->fh_r_ctl);
+		return -ENODEV;
+	}
+	if (rc)
+		return -ENOMEM;
+	else
+		return 0;
+}
+
+/**
+ * bnx2fc_xmit - bnx2fc's FCoE frame transmit function
+ *
+ * @lport:	the associated local port
+ * @fp:	the fc_frame to be transmitted
+ */
+static int bnx2fc_xmit(struct fc_lport *lport, struct fc_frame *fp)
+{
+	struct ethhdr		*eh;
+	struct fcoe_crc_eof	*cp;
+	struct sk_buff		*skb;
+	struct fc_frame_header	*fh;
+	struct bnx2fc_hba	*hba;
+	struct fcoe_port	*port;
+	struct fcoe_hdr		*hp;
+	struct bnx2fc_rport	*tgt;
+	struct fcoe_dev_stats	*stats;
+	u8			sof, eof;
+	u32			crc;
+	unsigned int		hlen, tlen, elen;
+	int			wlen, rc = 0;
+
+	port = (struct fcoe_port *)lport_priv(lport);
+	hba = port->priv;
+
+	fh = fc_frame_header_get(fp);
+
+	skb = fp_skb(fp);
+	if (!lport->link_up) {
+		BNX2FC_HBA_DBG(lport, "bnx2fc_xmit link down\n");
+		kfree_skb(skb);
+		return 0;
+	}
+
+	if (unlikely(fh->fh_r_ctl == FC_RCTL_ELS_REQ)) {
+		if (!hba->ctlr.sel_fcf) {
+			BNX2FC_HBA_DBG(lport, "FCF not selected yet!\n");
+			kfree_skb(skb);
+			return -EINVAL;
+		}
+		if (fcoe_ctlr_els_send(&hba->ctlr, lport, skb))
+			return 0;
+	}
+
+	sof = fr_sof(fp);
+	eof = fr_eof(fp);
+
+	/*
+	 * Snoop the frame header to check if the frame is for
+	 * an offloaded session
+	 */
+	/*
+	 * tgt_ofld_list access is synchronized using
+	 * both hba mutex and hba lock. Atleast hba mutex or
+	 * hba lock needs to be held for read access.
+	 */
+
+	spin_lock_bh(&hba->hba_lock);
+	tgt = bnx2fc_tgt_lookup(port, ntoh24(fh->fh_d_id));
+	if (tgt && (test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags))) {
+		/* This frame is for offloaded session */
+		BNX2FC_HBA_DBG(lport, "xmit: Frame is for offloaded session "
+				"port_id = 0x%x\n", ntoh24(fh->fh_d_id));
+		spin_unlock_bh(&hba->hba_lock);
+		rc = bnx2fc_xmit_l2_frame(tgt, fp);
+		if (rc != -ENODEV) {
+			kfree_skb(skb);
+			return rc;
+		}
+	} else {
+		spin_unlock_bh(&hba->hba_lock);
+	}
+
+	elen = sizeof(struct ethhdr);
+	hlen = sizeof(struct fcoe_hdr);
+	tlen = sizeof(struct fcoe_crc_eof);
+	wlen = (skb->len - tlen + sizeof(crc)) / FCOE_WORD_TO_BYTE;
+
+	skb->ip_summed = CHECKSUM_NONE;
+	crc = fcoe_fc_crc(fp);
+
+	/* copy port crc and eof to the skb buff */
+	if (skb_is_nonlinear(skb)) {
+		skb_frag_t *frag;
+		if (bnx2fc_get_paged_crc_eof(skb, tlen)) {
+			kfree_skb(skb);
+			return -ENOMEM;
+		}
+		frag = &skb_shinfo(skb)->frags[skb_shinfo(skb)->nr_frags - 1];
+		cp = kmap_atomic(frag->page, KM_SKB_DATA_SOFTIRQ)
+				+ frag->page_offset;
+	} else {
+		cp = (struct fcoe_crc_eof *)skb_put(skb, tlen);
+	}
+
+	memset(cp, 0, sizeof(*cp));
+	cp->fcoe_eof = eof;
+	cp->fcoe_crc32 = cpu_to_le32(~crc);
+	if (skb_is_nonlinear(skb)) {
+		kunmap_atomic(cp, KM_SKB_DATA_SOFTIRQ);
+		cp = NULL;
+	}
+
+	/* adjust skb network/transport offsets to match mac/fcoe/port */
+	skb_push(skb, elen + hlen);
+	skb_reset_mac_header(skb);
+	skb_reset_network_header(skb);
+	skb->mac_len = elen;
+	skb->protocol = htons(ETH_P_FCOE);
+	skb->dev = hba->netdev;
+
+	/* fill up mac and fcoe headers */
+	eh = eth_hdr(skb);
+	eh->h_proto = htons(ETH_P_FCOE);
+	if (hba->ctlr.map_dest)
+		fc_fcoe_set_mac(eh->h_dest, fh->fh_d_id);
+	else
+		/* insert GW address */
+		memcpy(eh->h_dest, hba->ctlr.dest_addr, ETH_ALEN);
+
+	if (unlikely(hba->ctlr.flogi_oxid != FC_XID_UNKNOWN))
+		memcpy(eh->h_source, hba->ctlr.ctl_src_addr, ETH_ALEN);
+	else
+		memcpy(eh->h_source, port->data_src_addr, ETH_ALEN);
+
+	hp = (struct fcoe_hdr *)(eh + 1);
+	memset(hp, 0, sizeof(*hp));
+	if (FC_FCOE_VER)
+		FC_FCOE_ENCAPS_VER(hp, FC_FCOE_VER);
+	hp->fcoe_sof = sof;
+
+	/* fcoe lso, mss is in max_payload which is non-zero for FCP data */
+	if (lport->seq_offload && fr_max_payload(fp)) {
+		skb_shinfo(skb)->gso_type = SKB_GSO_FCOE;
+		skb_shinfo(skb)->gso_size = fr_max_payload(fp);
+	} else {
+		skb_shinfo(skb)->gso_type = 0;
+		skb_shinfo(skb)->gso_size = 0;
+	}
+
+	/*update tx stats */
+	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats->TxFrames++;
+	stats->TxWords += wlen;
+	put_cpu();
+
+	/* send down to lld */
+	fr_dev(fp) = lport;
+	if (port->fcoe_pending_queue.qlen)
+		fcoe_check_wait_queue(lport, skb);
+	else if (fcoe_start_io(skb))
+		fcoe_check_wait_queue(lport, skb);
+
+	return 0;
+}
+
+/**
+ * bnx2fc_rcv - This is bnx2fc's receive function called by NET_RX_SOFTIRQ
+ *
+ * @skb:	the receive socket buffer
+ * @dev:	associated net device
+ * @ptype:	context
+ * @olddev:	last device
+ *
+ * This function receives the packet and builds FC frame and passes it up
+ */
+static int bnx2fc_rcv(struct sk_buff *skb, struct net_device *dev,
+		struct packet_type *ptype, struct net_device *olddev)
+{
+	struct fc_lport *lport;
+	struct bnx2fc_hba *hba;
+	struct fc_frame_header *fh;
+	struct fcoe_rcv_info *fr;
+	struct fcoe_percpu_s *bg;
+	unsigned short oxid;
+
+	hba = container_of(ptype, struct bnx2fc_hba, fcoe_packet_type);
+	lport = hba->ctlr.lp;
+
+	if (unlikely(lport == NULL)) {
+		printk(KERN_ALERT PFX "bnx2fc_rcv: lport is NULL\n");
+		goto err;
+	}
+
+	if (unlikely(eth_hdr(skb)->h_proto != htons(ETH_P_FCOE))) {
+		printk(KERN_ALERT PFX "bnx2fc_rcv: Wrong FC type frame\n");
+		goto err;
+	}
+
+	/*
+	 * Check for minimum frame length, and make sure required FCoE
+	 * and FC headers are pulled into the linear data area.
+	 */
+	if (unlikely((skb->len < FCOE_MIN_FRAME) ||
+	    !pskb_may_pull(skb, FCOE_HEADER_LEN)))
+		goto err;
+
+	skb_set_transport_header(skb, sizeof(struct fcoe_hdr));
+	fh = (struct fc_frame_header *) skb_transport_header(skb);
+
+	oxid = ntohs(fh->fh_ox_id);
+
+	fr = fcoe_dev_from_skb(skb);
+	fr->fr_dev = lport;
+	fr->ptype = ptype;
+
+	bg = &bnx2fc_global;
+	spin_lock_bh(&bg->fcoe_rx_list.lock);
+
+	__skb_queue_tail(&bg->fcoe_rx_list, skb);
+	if (bg->fcoe_rx_list.qlen == 1)
+		wake_up_process(bg->thread);
+
+	spin_unlock_bh(&bg->fcoe_rx_list.lock);
+
+	return 0;
+err:
+	kfree_skb(skb);
+	return -1;
+}
+
+static int bnx2fc_l2_rcv_thread(void *arg)
+{
+	struct fcoe_percpu_s *bg = arg;
+	struct sk_buff *skb;
+
+	set_user_nice(current, -20);
+	set_current_state(TASK_INTERRUPTIBLE);
+	while (!kthread_should_stop()) {
+		schedule();
+		set_current_state(TASK_RUNNING);
+		spin_lock_bh(&bg->fcoe_rx_list.lock);
+		while ((skb = __skb_dequeue(&bg->fcoe_rx_list)) != NULL) {
+			spin_unlock_bh(&bg->fcoe_rx_list.lock);
+			bnx2fc_recv_frame(skb);
+			spin_lock_bh(&bg->fcoe_rx_list.lock);
+		}
+		spin_unlock_bh(&bg->fcoe_rx_list.lock);
+		set_current_state(TASK_INTERRUPTIBLE);
+	}
+	set_current_state(TASK_RUNNING);
+	return 0;
+}
+
+
+static void bnx2fc_recv_frame(struct sk_buff *skb)
+{
+	u32 fr_len;
+	struct fc_lport *lport;
+	struct fcoe_rcv_info *fr;
+	struct fcoe_dev_stats *stats;
+	struct fc_frame_header *fh;
+	struct fcoe_crc_eof crc_eof;
+	struct fc_frame *fp;
+	struct fc_lport *vn_port;
+	struct fcoe_port *port;
+	u8 *mac = NULL;
+	u8 *dest_mac = NULL;
+	struct fcoe_hdr *hp;
+
+	fr = fcoe_dev_from_skb(skb);
+	lport = fr->fr_dev;
+	if (unlikely(lport == NULL)) {
+		printk(KERN_ALERT PFX "Invalid lport struct\n");
+		kfree_skb(skb);
+		return;
+	}
+
+	if (skb_is_nonlinear(skb))
+		skb_linearize(skb);
+	mac = eth_hdr(skb)->h_source;
+	dest_mac = eth_hdr(skb)->h_dest;
+
+	/* Pull the header */
+	hp = (struct fcoe_hdr *) skb_network_header(skb);
+	fh = (struct fc_frame_header *) skb_transport_header(skb);
+	skb_pull(skb, sizeof(struct fcoe_hdr));
+	fr_len = skb->len - sizeof(struct fcoe_crc_eof);
+
+	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats->RxFrames++;
+	stats->RxWords += fr_len / FCOE_WORD_TO_BYTE;
+
+	fp = (struct fc_frame *)skb;
+	fc_frame_init(fp);
+	fr_dev(fp) = lport;
+	fr_sof(fp) = hp->fcoe_sof;
+	if (skb_copy_bits(skb, fr_len, &crc_eof, sizeof(crc_eof))) {
+		put_cpu();
+		kfree_skb(skb);
+		return;
+	}
+	fr_eof(fp) = crc_eof.fcoe_eof;
+	fr_crc(fp) = crc_eof.fcoe_crc32;
+	if (pskb_trim(skb, fr_len)) {
+		put_cpu();
+		kfree_skb(skb);
+		return;
+	}
+
+	fh = fc_frame_header_get(fp);
+
+	vn_port = fc_vport_id_lookup(lport, ntoh24(fh->fh_d_id));
+	if (vn_port) {
+		port = lport_priv(vn_port);
+		if (compare_ether_addr(port->data_src_addr, dest_mac)
+		    != 0) {
+			BNX2FC_HBA_DBG(lport, "fpma mismatch\n");
+			put_cpu();
+			kfree_skb(skb);
+			return;
+		}
+	}
+	if (fh->fh_r_ctl == FC_RCTL_DD_SOL_DATA &&
+	    fh->fh_type == FC_TYPE_FCP) {
+		/* Drop FCP data. We dont this in L2 path */
+		put_cpu();
+		kfree_skb(skb);
+		return;
+	}
+	if (fh->fh_r_ctl == FC_RCTL_ELS_REQ &&
+	    fh->fh_type == FC_TYPE_ELS) {
+		switch (fc_frame_payload_op(fp)) {
+		case ELS_LOGO:
+			if (ntoh24(fh->fh_s_id) == FC_FID_FLOGI) {
+				/* drop non-FIP LOGO */
+				put_cpu();
+				kfree_skb(skb);
+				return;
+			}
+			break;
+		}
+	}
+	if (le32_to_cpu(fr_crc(fp)) !=
+			~crc32(~0, skb->data, fr_len)) {
+		if (stats->InvalidCRCCount < 5)
+			printk(KERN_WARNING PFX "dropping frame with "
+			       "CRC error\n");
+		stats->InvalidCRCCount++;
+		put_cpu();
+		kfree_skb(skb);
+		return;
+	}
+	put_cpu();
+	fc_exch_recv(lport, fp);
+}
+
+/**
+ * bnx2fc_percpu_io_thread - thread per cpu for ios
+ *
+ * @arg:	ptr to bnx2fc_percpu_info structure
+ */
+int bnx2fc_percpu_io_thread(void *arg)
+{
+	struct bnx2fc_percpu_s *p = arg;
+	struct bnx2fc_work *work, *tmp;
+	LIST_HEAD(work_list);
+
+	set_user_nice(current, -20);
+	set_current_state(TASK_INTERRUPTIBLE);
+	while (!kthread_should_stop()) {
+		schedule();
+		set_current_state(TASK_RUNNING);
+		spin_lock_bh(&p->fp_work_lock);
+		while (!list_empty(&p->work_list)) {
+			list_splice_init(&p->work_list, &work_list);
+			spin_unlock_bh(&p->fp_work_lock);
+
+			list_for_each_entry_safe(work, tmp, &work_list, list) {
+				list_del_init(&work->list);
+				bnx2fc_process_cq_compl(work->tgt, work->wqe);
+				kfree(work);
+			}
+
+			spin_lock_bh(&p->fp_work_lock);
+		}
+		spin_unlock_bh(&p->fp_work_lock);
+		set_current_state(TASK_INTERRUPTIBLE);
+	}
+	set_current_state(TASK_RUNNING);
+
+	return 0;
+}
+
+static struct fc_host_statistics *bnx2fc_get_host_stats(struct Scsi_Host *shost)
+{
+	struct fc_host_statistics *bnx2fc_stats;
+	struct fc_lport *lport = shost_priv(shost);
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct fcoe_statistics_params *fw_stats;
+	int rc = 0;
+
+	fw_stats = (struct fcoe_statistics_params *)hba->stats_buffer;
+	if (!fw_stats)
+		return NULL;
+
+	bnx2fc_stats = fc_get_host_stats(shost);
+
+	init_completion(&hba->stat_req_done);
+	if (bnx2fc_send_stat_req(hba))
+		return bnx2fc_stats;
+	rc = wait_for_completion_timeout(&hba->stat_req_done, (2 * HZ));
+	if (!rc) {
+		BNX2FC_HBA_DBG(lport, "FW stat req timed out\n");
+		return bnx2fc_stats;
+	}
+	bnx2fc_stats->invalid_crc_count += fw_stats->rx_stat1.fc_crc_cnt;
+	bnx2fc_stats->tx_frames += fw_stats->tx_stat.fcoe_tx_pkt_cnt;
+	bnx2fc_stats->tx_words += (fw_stats->tx_stat.fcoe_tx_byte_cnt) / 4;
+	bnx2fc_stats->rx_frames += fw_stats->rx_stat0.fcoe_rx_pkt_cnt;
+	bnx2fc_stats->rx_words += (fw_stats->rx_stat0.fcoe_rx_byte_cnt) / 4;
+
+	bnx2fc_stats->dumped_frames = 0;
+	bnx2fc_stats->lip_count = 0;
+	bnx2fc_stats->nos_count = 0;
+	bnx2fc_stats->loss_of_sync_count = 0;
+	bnx2fc_stats->loss_of_signal_count = 0;
+	bnx2fc_stats->prim_seq_protocol_err_count = 0;
+
+	return bnx2fc_stats;
+}
+
+static int bnx2fc_shost_config(struct fc_lport *lport, struct device *dev)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct Scsi_Host *shost = lport->host;
+	int rc = 0;
+
+	shost->max_cmd_len = BNX2FC_MAX_CMD_LEN;
+	shost->max_lun = BNX2FC_MAX_LUN;
+	shost->max_id = BNX2FC_MAX_FCP_TGT;
+	shost->max_channel = 0;
+	if (lport->vport)
+		shost->transportt = bnx2fc_vport_xport_template;
+	else
+		shost->transportt = bnx2fc_transport_template;
+
+	/* Add the new host to SCSI-ml */
+	rc = scsi_add_host(lport->host, dev);
+	if (rc) {
+		printk(KERN_ERR PFX "Error on scsi_add_host\n");
+		return rc;
+	}
+	if (!lport->vport)
+		fc_host_max_npiv_vports(lport->host) = USHRT_MAX;
+	sprintf(fc_host_symbolic_name(lport->host), "%s v%s over %s",
+		BNX2FC_NAME, BNX2FC_VERSION,
+		hba->netdev->name);
+
+	return 0;
+}
+
+static int  bnx2fc_mfs_update(struct fc_lport *lport)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct net_device *netdev = hba->netdev;
+	u32 mfs;
+	u32 max_mfs;
+
+	mfs = netdev->mtu - (sizeof(struct fcoe_hdr) +
+			     sizeof(struct fcoe_crc_eof));
+	max_mfs = BNX2FC_MAX_PAYLOAD + sizeof(struct fc_frame_header);
+	BNX2FC_HBA_DBG(lport, "mfs = %d, max_mfs = %d\n", mfs, max_mfs);
+	if (mfs > max_mfs)
+		mfs = max_mfs;
+
+	/* Adjust mfs to be a multiple of 256 bytes */
+	mfs = (((mfs - sizeof(struct fc_frame_header)) / BNX2FC_MIN_PAYLOAD) *
+			BNX2FC_MIN_PAYLOAD);
+	mfs = mfs + sizeof(struct fc_frame_header);
+
+	BNX2FC_HBA_DBG(lport, "Set MFS = %d\n", mfs);
+	if (fc_set_mfs(lport, mfs))
+		return -EINVAL;
+	return 0;
+}
+static void bnx2fc_link_speed_update(struct fc_lport *lport)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct net_device *netdev = hba->netdev;
+	struct ethtool_cmd ecmd = { ETHTOOL_GSET };
+
+	if (!dev_ethtool_get_settings(netdev, &ecmd)) {
+		lport->link_supported_speeds &=
+			~(FC_PORTSPEED_1GBIT | FC_PORTSPEED_10GBIT);
+		if (ecmd.supported & (SUPPORTED_1000baseT_Half |
+				      SUPPORTED_1000baseT_Full))
+			lport->link_supported_speeds |= FC_PORTSPEED_1GBIT;
+		if (ecmd.supported & SUPPORTED_10000baseT_Full)
+			lport->link_supported_speeds |= FC_PORTSPEED_10GBIT;
+
+		if (ecmd.speed == SPEED_1000)
+			lport->link_speed = FC_PORTSPEED_1GBIT;
+		if (ecmd.speed == SPEED_10000)
+			lport->link_speed = FC_PORTSPEED_10GBIT;
+	}
+	return;
+}
+static int bnx2fc_link_ok(struct fc_lport *lport)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct net_device *dev = hba->phys_dev;
+	int rc = 0;
+
+	if ((dev->flags & IFF_UP) && netif_carrier_ok(dev))
+		clear_bit(ADAPTER_STATE_LINK_DOWN, &hba->adapter_state);
+	else {
+		set_bit(ADAPTER_STATE_LINK_DOWN, &hba->adapter_state);
+		rc = -1;
+	}
+	return rc;
+}
+
+/**
+ * bnx2fc_get_link_state - get network link state
+ *
+ * @hba:	adapter instance pointer
+ *
+ * updates adapter structure flag based on netdev state
+ */
+void bnx2fc_get_link_state(struct bnx2fc_hba *hba)
+{
+	if (test_bit(__LINK_STATE_NOCARRIER, &hba->netdev->state))
+		set_bit(ADAPTER_STATE_LINK_DOWN, &hba->adapter_state);
+	else
+		clear_bit(ADAPTER_STATE_LINK_DOWN, &hba->adapter_state);
+}
+
+static int bnx2fc_net_config(struct fc_lport *lport)
+{
+	struct bnx2fc_hba *hba;
+	struct fcoe_port *port;
+	u64 wwnn, wwpn;
+
+	port = lport_priv(lport);
+	hba = port->priv;
+
+	/* require support for get_pauseparam ethtool op. */
+	if (!hba->phys_dev->ethtool_ops ||
+	    !hba->phys_dev->ethtool_ops->get_pauseparam)
+		return -EOPNOTSUPP;
+
+	if (bnx2fc_mfs_update(lport))
+		return -EINVAL;
+
+	skb_queue_head_init(&port->fcoe_pending_queue);
+	port->fcoe_pending_queue_active = 0;
+	setup_timer(&port->timer, fcoe_queue_timer, (unsigned long) lport);
+
+	bnx2fc_link_speed_update(lport);
+
+	if (!lport->vport) {
+		wwnn = fcoe_wwn_from_mac(hba->ctlr.ctl_src_addr, 1, 0);
+		BNX2FC_HBA_DBG(lport, "WWNN = 0x%llx\n", wwnn);
+		fc_set_wwnn(lport, wwnn);
+
+		wwpn = fcoe_wwn_from_mac(hba->ctlr.ctl_src_addr, 2, 0);
+		BNX2FC_HBA_DBG(lport, "WWPN = 0x%llx\n", wwpn);
+		fc_set_wwpn(lport, wwpn);
+	}
+
+	return 0;
+}
+
+static void bnx2fc_destroy_timer(unsigned long data)
+{
+	struct bnx2fc_hba *hba = (struct bnx2fc_hba *)data;
+
+	BNX2FC_HBA_DBG(hba->ctlr.lp, "ERROR:bnx2fc_destroy_timer - "
+		   "Destroy compl not received!!\n");
+	hba->flags |= BNX2FC_FLAG_DESTROY_CMPL;
+	wake_up_interruptible(&hba->destroy_wait);
+}
+
+/**
+ * bnx2fc_indicate_netevent - Generic netdev event handler
+ *
+ * @context:	adapter structure pointer
+ * @event:	event type
+ *
+ * Handles NETDEV_UP, NETDEV_DOWN, NETDEV_GOING_DOWN,NETDEV_CHANGE and
+ * NETDEV_CHANGE_MTU events
+ */
+static void bnx2fc_indicate_netevent(void *context, unsigned long event)
+{
+	struct bnx2fc_hba *hba = (struct bnx2fc_hba *)context;
+	struct fc_lport *lport = hba->ctlr.lp;
+	struct fc_lport *vport;
+	u32 link_possible = 1;
+
+	if (!test_bit(BNX2FC_CREATE_DONE, &hba->init_done)) {
+		BNX2FC_MISC_DBG("driver not ready. event=%s %ld\n",
+			   hba->netdev->name, event);
+		return;
+	}
+
+	/*
+	 * ASSUMPTION:
+	 * indicate_netevent cannot be called from cnic unless bnx2fc
+	 * does register_device
+	 */
+	BUG_ON(!lport);
+
+	BNX2FC_HBA_DBG(lport, "enter netevent handler - event=%s %ld\n",
+				hba->netdev->name, event);
+
+	switch (event) {
+	case NETDEV_UP:
+		BNX2FC_HBA_DBG(lport, "Port up, adapter_state = %ld\n",
+			hba->adapter_state);
+		if (!test_bit(ADAPTER_STATE_UP, &hba->adapter_state))
+			printk(KERN_ERR "indicate_netevent: "\
+					"adapter is not UP!!\n");
+		/* fall thru to update mfs if MTU has changed */
+	case NETDEV_CHANGEMTU:
+		BNX2FC_HBA_DBG(lport, "NETDEV_CHANGEMTU event\n");
+		bnx2fc_mfs_update(lport);
+		mutex_lock(&lport->lp_mutex);
+		list_for_each_entry(vport, &lport->vports, list)
+			bnx2fc_mfs_update(vport);
+		mutex_unlock(&lport->lp_mutex);
+		break;
+
+	case NETDEV_DOWN:
+		BNX2FC_HBA_DBG(lport, "Port down\n");
+		clear_bit(ADAPTER_STATE_GOING_DOWN, &hba->adapter_state);
+		clear_bit(ADAPTER_STATE_UP, &hba->adapter_state);
+		link_possible = 0;
+		break;
+
+	case NETDEV_GOING_DOWN:
+		BNX2FC_HBA_DBG(lport, "Port going down\n");
+		set_bit(ADAPTER_STATE_GOING_DOWN, &hba->adapter_state);
+		link_possible = 0;
+		break;
+
+	case NETDEV_CHANGE:
+		BNX2FC_HBA_DBG(lport, "NETDEV_CHANGE\n");
+		break;
+
+	default:
+		printk(KERN_ERR PFX "Unkonwn netevent %ld", event);
+		return;
+	}
+
+	bnx2fc_link_speed_update(lport);
+
+	if (link_possible && !bnx2fc_link_ok(lport)) {
+		printk(KERN_ERR "indicate_netevent: call ctlr_link_up\n");
+		fcoe_ctlr_link_up(&hba->ctlr);
+	} else {
+		printk(KERN_ERR "indicate_netevent: call ctlr_link_down\n");
+		if (fcoe_ctlr_link_down(&hba->ctlr)) {
+			clear_bit(ADAPTER_STATE_READY, &hba->adapter_state);
+			mutex_lock(&lport->lp_mutex);
+			list_for_each_entry(vport, &lport->vports, list)
+				fc_host_port_type(vport->host) =
+							FC_PORTTYPE_UNKNOWN;
+			mutex_unlock(&lport->lp_mutex);
+			fc_host_port_type(lport->host) = FC_PORTTYPE_UNKNOWN;
+			per_cpu_ptr(lport->dev_stats,
+				    get_cpu())->LinkFailureCount++;
+			put_cpu();
+			fcoe_clean_pending_queue(lport);
+
+			init_waitqueue_head(&hba->shutdown_wait);
+			BNX2FC_HBA_DBG(lport, "indicate_netevent "
+					     "num_ofld_sess = %d\n",
+				   hba->num_ofld_sess);
+			hba->wait_for_link_down = 1;
+			BNX2FC_HBA_DBG(lport, "waiting for uploads to "
+					     "compl proc = %s\n",
+				   current->comm);
+			wait_event_interruptible(hba->shutdown_wait,
+						 (hba->num_ofld_sess == 0));
+			BNX2FC_HBA_DBG(lport, "wakeup - num_ofld_sess = %d\n",
+				hba->num_ofld_sess);
+			hba->wait_for_link_down = 0;
+
+			if (signal_pending(current))
+				flush_signals(current);
+		}
+	}
+}
+
+static int bnx2fc_libfc_config(struct fc_lport *lport)
+{
+
+	/* Set the function pointers set by bnx2fc driver */
+	memcpy(&lport->tt, &bnx2fc_libfc_fcn_templ,
+		sizeof(struct libfc_function_template));
+	fc_elsct_init(lport);
+	fc_exch_init(lport);
+	fc_rport_init(lport);
+	fc_disc_init(lport);
+	return 0;
+}
+
+static int bnx2fc_em_config(struct fc_lport *lport)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+
+	if (!fc_exch_mgr_alloc(lport, FC_CLASS_3, FCOE_MIN_XID,
+				FCOE_MAX_XID, NULL)) {
+		printk(KERN_ERR PFX "em_config:fc_exch_mgr_alloc failed\n");
+		return -ENOMEM;
+	}
+
+	hba->cmd_mgr = bnx2fc_cmd_mgr_alloc(hba, BNX2FC_MIN_XID,
+					    BNX2FC_MAX_XID);
+
+	if (!hba->cmd_mgr) {
+		printk(KERN_ERR PFX "em_config:bnx2fc_cmd_mgr_alloc failed\n");
+		fc_exch_mgr_free(lport);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static int bnx2fc_lport_config(struct fc_lport *lport)
+{
+	lport->link_up = 0;
+	lport->qfull = 0;
+	lport->max_retry_count = 3;
+	lport->max_rport_retry_count = 3;
+	lport->e_d_tov = 2 * 1000;
+	lport->r_a_tov = 10 * 1000;
+
+	/* REVISIT: enable when supporting tape devices
+	lport->service_params = (FCP_SPPF_INIT_FCN | FCP_SPPF_RD_XRDY_DIS |
+				FCP_SPPF_RETRY | FCP_SPPF_CONF_COMPL);
+	*/
+	lport->service_params = (FCP_SPPF_INIT_FCN | FCP_SPPF_RD_XRDY_DIS);
+	lport->does_npiv = 1;
+
+	memset(&lport->rnid_gen, 0, sizeof(struct fc_els_rnid_gen));
+	lport->rnid_gen.rnid_atype = BNX2FC_RNID_HBA;
+
+	/* alloc stats structure */
+	if (fc_lport_init_stats(lport))
+		return -ENOMEM;
+
+	/* Finish fc_lport configuration */
+	fc_lport_config(lport);
+
+	return 0;
+}
+
+/**
+ * bnx2fc_fip_recv - handle a received FIP frame.
+ *
+ * @skb: the received skb
+ * @dev: associated &net_device
+ * @ptype: the &packet_type structure which was used to register this handler.
+ * @orig_dev: original receive &net_device, in case @ dev is a bond.
+ *
+ * Returns: 0 for success
+ */
+static int bnx2fc_fip_recv(struct sk_buff *skb, struct net_device *dev,
+			   struct packet_type *ptype,
+			   struct net_device *orig_dev)
+{
+	struct bnx2fc_hba *hba;
+	hba = container_of(ptype, struct bnx2fc_hba, fip_packet_type);
+	fcoe_ctlr_recv(&hba->ctlr, skb);
+	return 0;
+}
+
+/**
+ * bnx2fc_update_src_mac - Update Ethernet MAC filters.
+ *
+ * @fip: FCoE controller.
+ * @old: Unicast MAC address to delete if the MAC is non-zero.
+ * @new: Unicast MAC address to add.
+ *
+ * Remove any previously-set unicast MAC filter.
+ * Add secondary FCoE MAC address filter for our OUI.
+ */
+static void bnx2fc_update_src_mac(struct fc_lport *lport, u8 *addr)
+{
+	struct fcoe_port *port = lport_priv(lport);
+
+	memcpy(port->data_src_addr, addr, ETH_ALEN);
+}
+
+/**
+ * bnx2fc_get_src_mac - return the ethernet source address for an lport
+ *
+ * @lport: libfc port
+ */
+static u8 *bnx2fc_get_src_mac(struct fc_lport *lport)
+{
+	struct fcoe_port *port;
+
+	port = (struct fcoe_port *)lport_priv(lport);
+	return port->data_src_addr;
+}
+
+/**
+ * bnx2fc_fip_send - send an Ethernet-encapsulated FIP frame.
+ *
+ * @fip: FCoE controller.
+ * @skb: FIP Packet.
+ */
+static void bnx2fc_fip_send(struct fcoe_ctlr *fip, struct sk_buff *skb)
+{
+	skb->dev = bnx2fc_from_ctlr(fip)->netdev;
+	dev_queue_xmit(skb);
+}
+
+static int bnx2fc_vport_create(struct fc_vport *vport, bool disabled)
+{
+	struct Scsi_Host *shost = vport_to_shost(vport);
+	struct fc_lport *n_port = shost_priv(shost);
+	struct fcoe_port *port = lport_priv(n_port);
+	struct bnx2fc_hba *hba = port->priv;
+	struct net_device *netdev = hba->netdev;
+	struct fc_lport *vn_port;
+
+	if (!test_bit(BNX2FC_FW_INIT_DONE, &hba->init_done)) {
+		printk(KERN_ERR PFX "vn ports cannot be created on"
+			"this hba\n");
+		return -EIO;
+	}
+	mutex_lock(&bnx2fc_dev_lock);
+	vn_port = bnx2fc_if_create(hba, &vport->dev, 1);
+	mutex_unlock(&bnx2fc_dev_lock);
+
+	if (IS_ERR(vn_port)) {
+		printk(KERN_ERR PFX "bnx2fc_vport_create (%s) failed\n",
+			netdev->name);
+		return -EIO;
+	}
+
+	if (disabled) {
+		fc_vport_set_state(vport, FC_VPORT_DISABLED);
+	} else {
+		vn_port->boot_time = jiffies;
+		fc_lport_init(vn_port);
+		fc_fabric_login(vn_port);
+		fc_vport_setlink(vn_port);
+	}
+	return 0;
+}
+
+static int bnx2fc_vport_destroy(struct fc_vport *vport)
+{
+	struct Scsi_Host *shost = vport_to_shost(vport);
+	struct fc_lport *n_port = shost_priv(shost);
+	struct fc_lport *vn_port = vport->dd_data;
+	struct fcoe_port *port = lport_priv(vn_port);
+
+	mutex_lock(&n_port->lp_mutex);
+	list_del(&vn_port->list);
+	mutex_unlock(&n_port->lp_mutex);
+	queue_work(bnx2fc_wq, &port->destroy_work);
+	return 0;
+}
+
+static int bnx2fc_vport_disable(struct fc_vport *vport, bool disable)
+{
+	struct fc_lport *lport = vport->dd_data;
+
+	if (disable) {
+		fc_vport_set_state(vport, FC_VPORT_DISABLED);
+		fc_fabric_logoff(lport);
+	} else {
+		lport->boot_time = jiffies;
+		fc_fabric_login(lport);
+		fc_vport_setlink(lport);
+	}
+	return 0;
+}
+
+
+static int bnx2fc_netdev_setup(struct bnx2fc_hba *hba)
+{
+	struct net_device *netdev = hba->netdev;
+	struct net_device *physdev = hba->phys_dev;
+	struct netdev_hw_addr *ha;
+	int sel_san_mac = 0;
+
+	/* Do not support for bonding device */
+	if ((netdev->priv_flags & IFF_MASTER_ALB) ||
+			(netdev->priv_flags & IFF_SLAVE_INACTIVE) ||
+			(netdev->priv_flags & IFF_MASTER_8023AD)) {
+		return -EOPNOTSUPP;
+	}
+
+	/* setup Source MAC Address */
+	rcu_read_lock();
+	for_each_dev_addr(physdev, ha) {
+		BNX2FC_MISC_DBG("net_config: ha->type = %d, fip_mac = ",
+				ha->type);
+		printk(KERN_INFO "%2x:%2x:%2x:%2x:%2x:%2x\n", ha->addr[0],
+				ha->addr[1], ha->addr[2], ha->addr[3],
+				ha->addr[4], ha->addr[5]);
+
+		if ((ha->type == NETDEV_HW_ADDR_T_SAN) &&
+		    (is_valid_ether_addr(ha->addr))) {
+			memcpy(hba->ctlr.ctl_src_addr, ha->addr, ETH_ALEN);
+			sel_san_mac = 1;
+			BNX2FC_MISC_DBG("Found SAN MAC\n");
+		}
+	}
+	rcu_read_unlock();
+
+	if (!sel_san_mac)
+		return -ENODEV;
+
+	hba->fip_packet_type.func = bnx2fc_fip_recv;
+	hba->fip_packet_type.type = htons(ETH_P_FIP);
+	hba->fip_packet_type.dev = netdev;
+	dev_add_pack(&hba->fip_packet_type);
+
+	hba->fcoe_packet_type.func = bnx2fc_rcv;
+	hba->fcoe_packet_type.type = __constant_htons(ETH_P_FCOE);
+	hba->fcoe_packet_type.dev = netdev;
+	dev_add_pack(&hba->fcoe_packet_type);
+
+	return 0;
+}
+
+static int bnx2fc_attach_transport(void)
+{
+	bnx2fc_transport_template =
+		fc_attach_transport(&bnx2fc_transport_function);
+
+	if (bnx2fc_transport_template == NULL) {
+		printk(KERN_ERR PFX "Failed to attach FC transport\n");
+		return -ENODEV;
+	}
+
+	bnx2fc_vport_xport_template =
+		fc_attach_transport(&bnx2fc_vport_xport_function);
+	if (bnx2fc_vport_xport_template == NULL) {
+		printk(KERN_ERR PFX
+		       "Failed to attach FC transport for vport\n");
+		fc_release_transport(bnx2fc_transport_template);
+		bnx2fc_transport_template = NULL;
+		return -ENODEV;
+	}
+	return 0;
+}
+static void bnx2fc_release_transport(void)
+{
+	fc_release_transport(bnx2fc_transport_template);
+	fc_release_transport(bnx2fc_vport_xport_template);
+	bnx2fc_transport_template = NULL;
+	bnx2fc_vport_xport_template = NULL;
+}
+
+static void bnx2fc_interface_release(struct kref *kref)
+{
+	struct bnx2fc_hba *hba;
+	struct net_device *netdev;
+	struct net_device *phys_dev;
+
+	hba = container_of(kref, struct bnx2fc_hba, kref);
+	BNX2FC_HBA_DBG(hba->ctlr.lp, "Interface is being released\n");
+
+	netdev = hba->netdev;
+	phys_dev = hba->phys_dev;
+
+	/* tear-down FIP controller */
+	if (test_and_clear_bit(BNX2FC_CTLR_INIT_DONE, &hba->init_done))
+		fcoe_ctlr_destroy(&hba->ctlr);
+
+	/* Free the command manager */
+	if (hba->cmd_mgr) {
+		bnx2fc_cmd_mgr_free(hba->cmd_mgr);
+		hba->cmd_mgr = NULL;
+	}
+	dev_put(netdev);
+	module_put(THIS_MODULE);
+}
+
+static inline void bnx2fc_interface_get(struct bnx2fc_hba *hba)
+{
+	kref_get(&hba->kref);
+}
+
+static inline void bnx2fc_interface_put(struct bnx2fc_hba *hba)
+{
+	kref_put(&hba->kref, bnx2fc_interface_release);
+}
+static void bnx2fc_interface_destroy(struct bnx2fc_hba *hba)
+{
+	bnx2fc_unbind_pcidev(hba);
+	kfree(hba);
+}
+
+/**
+ * bnx2fc_interface_create - create a new fcoe instance
+ *
+ * @cnic:	pointer to cnic device
+ *
+ * Creates a new FCoE instance on the given device which include allocating
+ *	hba structure, scsi_host and lport structures.
+ */
+static struct bnx2fc_hba *bnx2fc_interface_create(struct cnic_dev *cnic)
+{
+	struct bnx2fc_hba *hba;
+	int rc;
+
+	hba = kzalloc(sizeof(*hba), GFP_KERNEL);
+	if (!hba) {
+		printk(KERN_ERR PFX "Unable to allocate hba structure\n");
+		return NULL;
+	}
+	spin_lock_init(&hba->hba_lock);
+	mutex_init(&hba->hba_mutex);
+
+	hba->cnic = cnic;
+	rc = bnx2fc_bind_pcidev(hba);
+	if (rc)
+		goto bind_err;
+	hba->phys_dev = cnic->netdev;
+	/* will get overwritten after we do vlan discovery */
+	hba->netdev = hba->phys_dev;
+
+	init_waitqueue_head(&hba->shutdown_wait);
+	init_waitqueue_head(&hba->destroy_wait);
+
+	return hba;
+bind_err:
+	printk(KERN_ERR PFX "create_interface: bind error\n");
+	kfree(hba);
+	return NULL;
+}
+
+static int bnx2fc_interface_setup(struct bnx2fc_hba *hba,
+				  enum fip_state fip_mode)
+{
+	int rc = 0;
+	struct net_device *netdev = hba->netdev;
+	struct fcoe_ctlr *fip = &hba->ctlr;
+
+	dev_hold(netdev);
+	kref_init(&hba->kref);
+
+	hba->flags = 0;
+
+	/* Initialize FIP */
+	memset(fip, 0, sizeof(*fip));
+	fcoe_ctlr_init(fip, fip_mode);
+	hba->ctlr.send = bnx2fc_fip_send;
+	hba->ctlr.update_mac = bnx2fc_update_src_mac;
+	hba->ctlr.get_src_addr = bnx2fc_get_src_mac;
+	set_bit(BNX2FC_CTLR_INIT_DONE, &hba->init_done);
+
+	rc = bnx2fc_netdev_setup(hba);
+	if (rc)
+		goto setup_err;
+
+	hba->next_conn_id = 0;
+
+	memset(hba->tgt_ofld_list, 0, sizeof(hba->tgt_ofld_list));
+	hba->num_ofld_sess = 0;
+
+	return 0;
+
+setup_err:
+	fcoe_ctlr_destroy(&hba->ctlr);
+	dev_put(netdev);
+	bnx2fc_interface_put(hba);
+	return rc;
+}
+
+/**
+ * bnx2fc_if_create - Create FCoE instance on a given interface
+ *
+ * @hba:	FCoE interface to create a local port on
+ * @parent:	Device pointer to be the parent in sysfs for the SCSI host
+ * @npiv:	Indicates if the port is vport or not
+ *
+ * Creates a fc_lport instance and a Scsi_Host instance and configure them.
+ *
+ * Returns:	Allocated fc_lport or an error pointer
+ */
+static struct fc_lport *bnx2fc_if_create(struct bnx2fc_hba *hba,
+				  struct device *parent, int npiv)
+{
+	struct fc_lport		*lport = NULL;
+	struct fcoe_port	*port;
+	struct Scsi_Host	*shost;
+	struct fc_vport		*vport = dev_to_vport(parent);
+	int			rc = 0;
+
+	/* Allocate Scsi_Host structure */
+	if (!npiv) {
+		lport = libfc_host_alloc(&bnx2fc_shost_template,
+					  sizeof(struct fcoe_port));
+	} else {
+		lport = libfc_vport_create(vport,
+					   sizeof(struct fcoe_port));
+	}
+
+	if (!lport) {
+		printk(KERN_ERR PFX "could not allocate scsi host structure\n");
+		return NULL;
+	}
+	shost = lport->host;
+	port = lport_priv(lport);
+	port->lport = lport;
+	port->priv = hba;
+	INIT_WORK(&port->destroy_work, bnx2fc_destroy_work);
+
+	/* Configure fcoe_port */
+	rc = bnx2fc_lport_config(lport);
+	if (rc)
+		goto lp_config_err;
+
+	if (npiv) {
+		vport = dev_to_vport(parent);
+		printk(KERN_ERR PFX "Setting vport names, 0x%llX 0x%llX\n",
+			vport->node_name, vport->port_name);
+		fc_set_wwnn(lport, vport->node_name);
+		fc_set_wwpn(lport, vport->port_name);
+	}
+	/* Configure netdev and networking properties of the lport */
+	rc = bnx2fc_net_config(lport);
+	if (rc) {
+		printk(KERN_ERR PFX "Error on bnx2fc_net_config\n");
+		goto lp_config_err;
+	}
+
+	rc = bnx2fc_shost_config(lport, parent);
+	if (rc) {
+		printk(KERN_ERR PFX "Couldnt configure shost for %s\n",
+			hba->netdev->name);
+		goto lp_config_err;
+	}
+
+	/* Initialize the libfc library */
+	rc = bnx2fc_libfc_config(lport);
+	if (rc) {
+		printk(KERN_ERR PFX "Couldnt configure libfc\n");
+		goto shost_err;
+	}
+	fc_host_port_type(lport->host) = FC_PORTTYPE_UNKNOWN;
+
+	/* Allocate exchange manager */
+	if (!npiv) {
+		rc = bnx2fc_em_config(lport);
+		if (rc) {
+			printk(KERN_ERR PFX "Error on bnx2fc_em_config\n");
+			goto shost_err;
+		}
+	}
+
+	bnx2fc_interface_get(hba);
+	return lport;
+
+shost_err:
+	scsi_remove_host(shost);
+lp_config_err:
+	scsi_host_put(lport->host);
+	return NULL;
+}
+
+static void bnx2fc_netdev_cleanup(struct bnx2fc_hba *hba)
+{
+	/* Dont listen for Ethernet packets anymore */
+	__dev_remove_pack(&hba->fcoe_packet_type);
+	__dev_remove_pack(&hba->fip_packet_type);
+	synchronize_net();
+}
+
+static void bnx2fc_if_destroy(struct fc_lport *lport)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+
+	BNX2FC_HBA_DBG(hba->ctlr.lp, "ENTERED bnx2fc_if_destroy\n");
+	/* Stop the transmit retry timer */
+	del_timer_sync(&port->timer);
+
+	/* Free existing transmit skbs */
+	fcoe_clean_pending_queue(lport);
+
+	bnx2fc_interface_put(hba);
+
+	/* Free queued packets for the receive thread */
+	bnx2fc_clean_rx_queue(lport);
+
+	/* Detach from scsi-ml */
+	fc_remove_host(lport->host);
+	scsi_remove_host(lport->host);
+
+	/*
+	 * Note that only the physical lport will have the exchange manager.
+	 * for vports, this function is NOP
+	 */
+	fc_exch_mgr_free(lport);
+
+	/* Free memory used by statistical counters */
+	fc_lport_free_stats(lport);
+
+	/* Release Scsi_Host */
+	scsi_host_put(lport->host);
+}
+
+/**
+ * bnx2fc_destroy - Destroy a bnx2fc FCoE interface
+ *
+ * @buffer: The name of the Ethernet interface to be destroyed
+ * @kp:     The associated kernel parameter
+ *
+ * Called from sysfs.
+ *
+ * Returns: 0 for success
+ */
+static int bnx2fc_destroy(struct net_device *netdev)
+{
+	struct bnx2fc_hba *hba = NULL;
+	struct net_device *phys_dev;
+	int rc = 0;
+
+	if (!rtnl_trylock())
+		return restart_syscall();
+
+	mutex_lock(&bnx2fc_dev_lock);
+#ifdef CONFIG_SCSI_BNX2X_FCOE_MODULE
+	if (THIS_MODULE->state != MODULE_STATE_LIVE) {
+		rc = -ENODEV;
+		goto netdev_err;
+	}
+#endif
+	/* obtain physical netdev */
+	if (netdev->priv_flags & IFF_802_1Q_VLAN)
+		phys_dev = vlan_dev_real_dev(netdev);
+	else {
+		printk(KERN_ERR PFX "Not a vlan device\n");
+		rc = -ENODEV;
+		goto netdev_err;
+	}
+
+	hba = bnx2fc_hba_lookup(phys_dev);
+	if (!hba || !hba->ctlr.lp) {
+		rc = -ENODEV;
+		printk(KERN_ERR PFX "bnx2fc_destroy: hba or lport not found\n");
+		goto netdev_err;
+	}
+
+	if (!test_bit(BNX2FC_CREATE_DONE, &hba->init_done)) {
+		printk(KERN_ERR PFX "bnx2fc_destroy: Create not called\n");
+		goto netdev_err;
+	}
+
+	bnx2fc_netdev_cleanup(hba);
+
+	bnx2fc_stop(hba);
+
+	bnx2fc_if_destroy(hba->ctlr.lp);
+
+	destroy_workqueue(hba->timer_work_queue);
+
+	if (test_bit(BNX2FC_FW_INIT_DONE, &hba->init_done))
+		bnx2fc_fw_destroy(hba);
+
+	clear_bit(BNX2FC_CREATE_DONE, &hba->init_done);
+netdev_err:
+	mutex_unlock(&bnx2fc_dev_lock);
+	rtnl_unlock();
+	return rc;
+}
+
+static void bnx2fc_destroy_work(struct work_struct *work)
+{
+	struct fcoe_port *port;
+	struct fc_lport *lport;
+
+	port = container_of(work, struct fcoe_port, destroy_work);
+	lport = port->lport;
+
+	BNX2FC_HBA_DBG(lport, "Entered bnx2fc_destroy_work\n");
+
+	bnx2fc_port_shutdown(lport);
+	rtnl_lock();
+	mutex_lock(&bnx2fc_dev_lock);
+	bnx2fc_if_destroy(lport);
+	mutex_unlock(&bnx2fc_dev_lock);
+	rtnl_unlock();
+}
+
+static void bnx2fc_unbind_adapter_devices(struct bnx2fc_hba *hba)
+{
+	bnx2fc_free_fw_resc(hba);
+	bnx2fc_free_task_ctx(hba);
+}
+
+/**
+ * bnx2fc_bind_adapter_devices - binds bnx2fc adapter with the associated
+ *			pci structure
+ *
+ * @hba:		Adapter instance
+ */
+static int bnx2fc_bind_adapter_devices(struct bnx2fc_hba *hba)
+{
+	if (bnx2fc_setup_task_ctx(hba))
+		goto mem_err;
+
+	if (bnx2fc_setup_fw_resc(hba))
+		goto mem_err;
+
+	return 0;
+mem_err:
+	bnx2fc_unbind_adapter_devices(hba);
+	return -ENOMEM;
+}
+
+static int bnx2fc_bind_pcidev(struct bnx2fc_hba *hba)
+{
+	struct cnic_dev *cnic;
+
+	if (!hba->cnic) {
+		printk(KERN_ERR PFX "cnic is NULL\n");
+		return -ENODEV;
+	}
+	cnic = hba->cnic;
+	hba->pcidev = cnic->pcidev;
+	if (hba->pcidev)
+		pci_dev_get(hba->pcidev);
+
+	return 0;
+}
+
+static void bnx2fc_unbind_pcidev(struct bnx2fc_hba *hba)
+{
+	if (hba->pcidev)
+		pci_dev_put(hba->pcidev);
+	hba->pcidev = NULL;
+}
+
+
+
+/**
+ * bnx2fc_ulp_start - cnic callback to initialize & start adapter instance
+ *
+ * @handle:	transport handle pointing to adapter struture
+ *
+ * This function maps adapter structure to pcidev structure and initiates
+ *	firmware handshake to enable/initialize on-chip FCoE components.
+ *	This bnx2fc - cnic interface api callback is used after following
+ *	conditions are met -
+ *	a) underlying network interface is up (marked by event NETDEV_UP
+ *		from netdev
+ *	b) bnx2fc adatper structure is registered.
+ */
+static void bnx2fc_ulp_start(void *handle)
+{
+	struct bnx2fc_hba *hba = handle;
+	struct fc_lport *lport = hba->ctlr.lp;
+
+	BNX2FC_MISC_DBG("Entered %s\n", __func__);
+	mutex_lock(&bnx2fc_dev_lock);
+
+	if (test_bit(BNX2FC_FW_INIT_DONE, &hba->init_done))
+		goto start_disc;
+
+	if (test_bit(BNX2FC_CREATE_DONE, &hba->init_done))
+		bnx2fc_fw_init(hba);
+
+start_disc:
+	mutex_unlock(&bnx2fc_dev_lock);
+
+	BNX2FC_MISC_DBG("bnx2fc started.\n");
+
+	/* Kick off Fabric discovery*/
+	if (test_bit(BNX2FC_CREATE_DONE, &hba->init_done)) {
+		printk(KERN_ERR PFX "ulp_init: start discovery\n");
+		lport->tt.frame_send = bnx2fc_xmit;
+		bnx2fc_start_disc(hba);
+	}
+}
+
+static void bnx2fc_port_shutdown(struct fc_lport *lport)
+{
+	BNX2FC_MISC_DBG("Entered %s\n", __func__);
+	fc_fabric_logoff(lport);
+	fc_lport_destroy(lport);
+}
+
+static void bnx2fc_stop(struct bnx2fc_hba *hba)
+{
+	struct fc_lport *lport;
+	struct fc_lport *vport;
+
+	BNX2FC_MISC_DBG("ENTERED %s - init_done = %ld\n", __func__,
+		   hba->init_done);
+	if (test_bit(BNX2FC_FW_INIT_DONE, &hba->init_done) &&
+	    test_bit(BNX2FC_CREATE_DONE, &hba->init_done)) {
+		lport = hba->ctlr.lp;
+		bnx2fc_port_shutdown(lport);
+		BNX2FC_HBA_DBG(lport, "bnx2fc_stop: waiting for %d "
+				"offloaded sessions\n",
+				hba->num_ofld_sess);
+		wait_event_interruptible(hba->shutdown_wait,
+					 (hba->num_ofld_sess == 0));
+		mutex_lock(&lport->lp_mutex);
+		list_for_each_entry(vport, &lport->vports, list)
+			fc_host_port_type(vport->host) = FC_PORTTYPE_UNKNOWN;
+		mutex_unlock(&lport->lp_mutex);
+		fc_host_port_type(lport->host) = FC_PORTTYPE_UNKNOWN;
+		fcoe_ctlr_link_down(&hba->ctlr);
+		fcoe_clean_pending_queue(lport);
+
+		mutex_lock(&hba->hba_mutex);
+		clear_bit(ADAPTER_STATE_UP, &hba->adapter_state);
+		clear_bit(ADAPTER_STATE_GOING_DOWN, &hba->adapter_state);
+
+		clear_bit(ADAPTER_STATE_READY, &hba->adapter_state);
+		mutex_unlock(&hba->hba_mutex);
+	}
+}
+
+static int bnx2fc_fw_init(struct bnx2fc_hba *hba)
+{
+#define BNX2FC_INIT_POLL_TIME		(1000 / HZ)
+	int rc = -1;
+	int i = HZ;
+
+	rc = bnx2fc_bind_adapter_devices(hba);
+	if (rc) {
+		printk(KERN_ALERT PFX
+			"bnx2fc_bind_adapter_devices failed - rc = %d\n", rc);
+		goto err_out;
+	}
+
+	rc = bnx2fc_send_fw_fcoe_init_msg(hba);
+	if (rc) {
+		printk(KERN_ALERT PFX
+			"bnx2fc_send_fw_fcoe_init_msg failed - rc = %d\n", rc);
+		goto err_unbind;
+	}
+
+	/*
+	 * Wait until the adapter init message is complete, and adapter
+	 * state is UP.
+	 */
+	while (!test_bit(ADAPTER_STATE_UP, &hba->adapter_state) && i--)
+		msleep(BNX2FC_INIT_POLL_TIME);
+
+	if (!test_bit(ADAPTER_STATE_UP, &hba->adapter_state)) {
+		printk(KERN_ERR PFX "bnx2fc_start: %s failed to initialize.  "
+				"Ignoring...\n",
+				hba->cnic->netdev->name);
+		rc = -1;
+		goto err_unbind;
+	}
+
+
+	/* Mark HBA to indicate that the FW INIT is done */
+	set_bit(BNX2FC_FW_INIT_DONE, &hba->init_done);
+	return 0;
+
+err_unbind:
+	bnx2fc_unbind_adapter_devices(hba);
+err_out:
+	return rc;
+}
+
+static void bnx2fc_fw_destroy(struct bnx2fc_hba *hba)
+{
+	if (test_and_clear_bit(BNX2FC_FW_INIT_DONE, &hba->init_done)) {
+		if (bnx2fc_send_fw_fcoe_destroy_msg(hba) == 0) {
+			init_timer(&hba->destroy_timer);
+			hba->destroy_timer.expires = BNX2FC_FW_TIMEOUT +
+								jiffies;
+			hba->destroy_timer.function = bnx2fc_destroy_timer;
+			hba->destroy_timer.data = (unsigned long)hba;
+			add_timer(&hba->destroy_timer);
+			wait_event_interruptible(hba->destroy_wait,
+						 (hba->flags &
+						  BNX2FC_FLAG_DESTROY_CMPL));
+			/* This should never happen */
+			if (signal_pending(current))
+				flush_signals(current);
+
+			del_timer_sync(&hba->destroy_timer);
+		}
+		bnx2fc_unbind_adapter_devices(hba);
+	}
+}
+
+/**
+ * bnx2fc_ulp_stop - cnic callback to shutdown adapter instance
+ *
+ * @handle:	transport handle pointing to adapter structure
+ *
+ * Driver checks if adapter is already in shutdown mode, if not start
+ *	the shutdown process.
+ */
+static void bnx2fc_ulp_stop(void *handle)
+{
+	struct bnx2fc_hba *hba = (struct bnx2fc_hba *)handle;
+
+	printk(KERN_ERR "ULP_STOP\n");
+
+	mutex_lock(&bnx2fc_dev_lock);
+	bnx2fc_stop(hba);
+	bnx2fc_fw_destroy(hba);
+	mutex_unlock(&bnx2fc_dev_lock);
+}
+
+static void bnx2fc_start_disc(struct bnx2fc_hba *hba)
+{
+	struct fc_lport *lport;
+	int wait_cnt = 0;
+
+	BNX2FC_MISC_DBG("Entered %s\n", __func__);
+	/* Kick off FIP/FLOGI */
+	if (!test_bit(BNX2FC_FW_INIT_DONE, &hba->init_done)) {
+		printk(KERN_ERR PFX "Init not done yet\n");
+		return;
+	}
+
+	lport = hba->ctlr.lp;
+	BNX2FC_HBA_DBG(lport, "calling fc_fabric_login\n");
+
+	if (!bnx2fc_link_ok(lport)) {
+		BNX2FC_HBA_DBG(lport, "ctlr_link_up\n");
+		fcoe_ctlr_link_up(&hba->ctlr);
+		fc_host_port_type(lport->host) = FC_PORTTYPE_NPORT;
+		set_bit(ADAPTER_STATE_READY, &hba->adapter_state);
+	}
+
+	/* wait for the FCF to be selected before issuing FLOGI */
+	while (!hba->ctlr.sel_fcf) {
+		msleep(250);
+		/* give up after 3 secs */
+		if (++wait_cnt > 12)
+			break;
+	}
+	fc_lport_init(lport);
+	fc_fabric_login(lport);
+}
+
+
+/**
+ * bnx2fc_ulp_init - Initialize an adapter instance
+ *
+ * @dev :	cnic device handle
+ * Called from cnic_register_driver() context to initialize all
+ *	enumerated cnic devices. This routine allocates adapter structure
+ *	and other device specific resources.
+ */
+static void bnx2fc_ulp_init(struct cnic_dev *dev)
+{
+	struct bnx2fc_hba *hba;
+	int rc = 0;
+
+	BNX2FC_MISC_DBG("Entered %s\n", __func__);
+	/* bnx2fc works only when bnx2x is loaded */
+	if (!test_bit(CNIC_F_BNX2X_CLASS, &dev->flags)) {
+		printk(KERN_ERR PFX "bnx2fc FCoE not supported on %s,"
+				    " flags: %lx\n",
+			dev->netdev->name, dev->flags);
+		return;
+	}
+
+	/* Configure FCoE interface */
+	hba = bnx2fc_interface_create(dev);
+	if (!hba) {
+		printk(KERN_ERR PFX "hba initialization failed\n");
+		return;
+	}
+
+	/* Add HBA to the adapter list */
+	mutex_lock(&bnx2fc_dev_lock);
+	list_add_tail(&hba->link, &adapter_list);
+	adapter_count++;
+	mutex_unlock(&bnx2fc_dev_lock);
+
+	clear_bit(BNX2FC_CNIC_REGISTERED, &hba->reg_with_cnic);
+	rc = dev->register_device(dev, CNIC_ULP_FCOE,
+						(void *) hba);
+	if (rc)
+		printk(KERN_ALERT PFX "register_device failed, rc = %d\n", rc);
+	else
+		set_bit(BNX2FC_CNIC_REGISTERED, &hba->reg_with_cnic);
+}
+
+
+static int bnx2fc_disable(struct net_device *netdev)
+{
+	struct bnx2fc_hba *hba;
+	struct net_device *phys_dev;
+	struct ethtool_drvinfo drvinfo;
+	int rc = 0;
+
+	if (!rtnl_trylock()) {
+		printk(KERN_ERR PFX "retrying for rtnl_lock\n");
+		return -EIO;
+	}
+
+	mutex_lock(&bnx2fc_dev_lock);
+
+	if (THIS_MODULE->state != MODULE_STATE_LIVE) {
+		rc = -ENODEV;
+		goto nodev;
+	}
+
+	/* obtain physical netdev */
+	if (netdev->priv_flags & IFF_802_1Q_VLAN)
+		phys_dev = vlan_dev_real_dev(netdev);
+	else {
+		printk(KERN_ERR PFX "Not a vlan device\n");
+		rc = -ENODEV;
+		goto nodev;
+	}
+
+	/* verify if the physical device is a netxtreme2 device */
+	if (phys_dev->ethtool_ops && phys_dev->ethtool_ops->get_drvinfo) {
+		memset(&drvinfo, 0, sizeof(drvinfo));
+		phys_dev->ethtool_ops->get_drvinfo(phys_dev, &drvinfo);
+		if (strcmp(drvinfo.driver, "bnx2x")) {
+			printk(KERN_ERR PFX "Not a netxtreme2 device\n");
+			rc = -ENODEV;
+			goto nodev;
+		}
+	} else {
+		printk(KERN_ERR PFX "unable to obtain drv_info\n");
+		rc = -ENODEV;
+		goto nodev;
+	}
+
+	printk(KERN_ERR PFX "phys_dev is netxtreme2 device\n");
+
+	/* obtain hba and initialize rest of the structure */
+	hba = bnx2fc_hba_lookup(phys_dev);
+	if (!hba || !hba->ctlr.lp) {
+		rc = -ENODEV;
+		printk(KERN_ERR PFX "bnx2fc_disable: hba or lport not found\n");
+	} else {
+		fcoe_ctlr_link_down(&hba->ctlr);
+		fcoe_clean_pending_queue(hba->ctlr.lp);
+	}
+
+nodev:
+	mutex_unlock(&bnx2fc_dev_lock);
+	rtnl_unlock();
+	return rc;
+}
+
+
+static int bnx2fc_enable(struct net_device *netdev)
+{
+	struct bnx2fc_hba *hba;
+	struct net_device *phys_dev;
+	struct ethtool_drvinfo drvinfo;
+	int rc = 0;
+
+	if (!rtnl_trylock()) {
+		printk(KERN_ERR PFX "retrying for rtnl_lock\n");
+		return -EIO;
+	}
+
+	BNX2FC_MISC_DBG("Entered %s\n", __func__);
+	mutex_lock(&bnx2fc_dev_lock);
+
+	if (THIS_MODULE->state != MODULE_STATE_LIVE) {
+		rc = -ENODEV;
+		goto nodev;
+	}
+
+	/* obtain physical netdev */
+	if (netdev->priv_flags & IFF_802_1Q_VLAN)
+		phys_dev = vlan_dev_real_dev(netdev);
+	else {
+		printk(KERN_ERR PFX "Not a vlan device\n");
+		rc = -ENODEV;
+		goto nodev;
+	}
+	/* verify if the physical device is a netxtreme2 device */
+	if (phys_dev->ethtool_ops && phys_dev->ethtool_ops->get_drvinfo) {
+		memset(&drvinfo, 0, sizeof(drvinfo));
+		phys_dev->ethtool_ops->get_drvinfo(phys_dev, &drvinfo);
+		if (strcmp(drvinfo.driver, "bnx2x")) {
+			printk(KERN_ERR PFX "Not a netxtreme2 device\n");
+			rc = -ENODEV;
+			goto nodev;
+		}
+	} else {
+		printk(KERN_ERR PFX "unable to obtain drv_info\n");
+		rc = -ENODEV;
+		goto nodev;
+	}
+
+	/* obtain hba and initialize rest of the structure */
+	hba = bnx2fc_hba_lookup(phys_dev);
+	if (!hba || !hba->ctlr.lp) {
+		rc = -ENODEV;
+		printk(KERN_ERR PFX "bnx2fc_enable: hba or lport not found\n");
+	} else if (!bnx2fc_link_ok(hba->ctlr.lp))
+		fcoe_ctlr_link_up(&hba->ctlr);
+
+nodev:
+	mutex_unlock(&bnx2fc_dev_lock);
+	rtnl_unlock();
+	return rc;
+}
+
+/**
+ * bnx2fc_create - Create bnx2fc FCoE interface
+ *
+ * @buffer: The name of Ethernet interface to create on
+ * @kp:     The associated kernel param
+ *
+ * Called from sysfs.
+ *
+ * Returns: 0 for success
+ */
+static int bnx2fc_create(struct net_device *netdev, enum fip_state fip_mode)
+{
+	struct bnx2fc_hba *hba;
+	struct net_device *phys_dev;
+	struct fc_lport *lport;
+	struct ethtool_drvinfo drvinfo;
+	int rc = 0;
+	int vlan_id;
+
+	BNX2FC_MISC_DBG("Entered bnx2fc_create\n");
+	if (fip_mode != FIP_MODE_FABRIC) {
+		printk(KERN_ERR "fip mode not FABRIC\n");
+		return -EIO;
+	}
+
+	if (!rtnl_trylock()) {
+		printk(KERN_ERR "trying for rtnl_lock\n");
+		return -EIO;
+	}
+	mutex_lock(&bnx2fc_dev_lock);
+
+#ifdef CONFIG_SCSI_BNX2X_FCOE_MODULE
+	if (THIS_MODULE->state != MODULE_STATE_LIVE) {
+		rc = -ENODEV;
+		goto mod_err;
+	}
+#endif
+
+	if (!try_module_get(THIS_MODULE)) {
+		rc = -EINVAL;
+		goto mod_err;
+	}
+
+	/* obtain physical netdev */
+	if (netdev->priv_flags & IFF_802_1Q_VLAN) {
+		phys_dev = vlan_dev_real_dev(netdev);
+		vlan_id = vlan_dev_vlan_id(netdev);
+	} else {
+		printk(KERN_ERR PFX "Not a vlan device\n");
+		rc = -EINVAL;
+		goto netdev_err;
+	}
+	/* verify if the physical device is a netxtreme2 device */
+	if (phys_dev->ethtool_ops && phys_dev->ethtool_ops->get_drvinfo) {
+		memset(&drvinfo, 0, sizeof(drvinfo));
+		phys_dev->ethtool_ops->get_drvinfo(phys_dev, &drvinfo);
+		if (strcmp(drvinfo.driver, "bnx2x")) {
+			printk(KERN_ERR PFX "Not a netxtreme2 device\n");
+			rc = -EINVAL;
+			goto netdev_err;
+		}
+	} else {
+		printk(KERN_ERR PFX "unable to obtain drv_info\n");
+		rc = -EINVAL;
+		goto netdev_err;
+	}
+
+	/* obtain hba and initialize rest of the structure */
+	hba = bnx2fc_hba_lookup(phys_dev);
+	if (!hba) {
+		rc = -ENODEV;
+		printk(KERN_ERR PFX "bnx2fc_create: hba not found\n");
+		goto netdev_err;
+	}
+
+	if (!test_bit(BNX2FC_FW_INIT_DONE, &hba->init_done)) {
+		rc = bnx2fc_fw_init(hba);
+		if (rc)
+			goto netdev_err;
+	}
+
+	if (test_bit(BNX2FC_CREATE_DONE, &hba->init_done)) {
+		rc = -EEXIST;
+		goto netdev_err;
+	}
+
+	/* update netdev with vlan netdev */
+	hba->netdev = netdev;
+	hba->vlan_id = vlan_id;
+	hba->vlan_enabled = 1;
+
+	rc = bnx2fc_interface_setup(hba, fip_mode);
+	if (rc) {
+		printk(KERN_ERR PFX "bnx2fc_interface_setup failed\n");
+		goto ifput_err;
+	}
+
+	hba->timer_work_queue =
+			create_singlethread_workqueue("bnx2fc_timer_wq");
+	if (!hba->timer_work_queue) {
+		printk(KERN_ERR PFX "ulp_init could not create timer_wq\n");
+		rc = -EINVAL;
+		goto ifput_err;
+	}
+
+	lport = bnx2fc_if_create(hba, &hba->pcidev->dev, 0);
+	if (!lport) {
+		printk(KERN_ERR PFX "Failed to create interface (%s)\n",
+			netdev->name);
+		bnx2fc_netdev_cleanup(hba);
+		rc = -EINVAL;
+		goto if_create_err;
+	}
+
+	lport->boot_time = jiffies;
+
+	/* Make this master N_port */
+	hba->ctlr.lp = lport;
+
+	set_bit(BNX2FC_CREATE_DONE, &hba->init_done);
+	printk(KERN_ERR PFX "create: START DISC\n");
+	bnx2fc_start_disc(hba);
+	/*
+	 * Release from kref_init in bnx2fc_interface_setup, on success
+	 * lport should be holding a reference taken in bnx2fc_if_create
+	 */
+	bnx2fc_interface_put(hba);
+	/* put netdev that was held while calling dev_get_by_name */
+	mutex_unlock(&bnx2fc_dev_lock);
+	rtnl_unlock();
+	return 0;
+
+if_create_err:
+	destroy_workqueue(hba->timer_work_queue);
+ifput_err:
+	bnx2fc_interface_put(hba);
+netdev_err:
+	module_put(THIS_MODULE);
+mod_err:
+	mutex_unlock(&bnx2fc_dev_lock);
+	rtnl_unlock();
+	return rc;
+}
+
+/**
+ * bnx2fc_find_hba_for_cnic - maps cnic instance to bnx2fc adapter instance
+ *
+ * @cnic:	Pointer to cnic device instance
+ *
+ **/
+static struct bnx2fc_hba *bnx2fc_find_hba_for_cnic(struct cnic_dev *cnic)
+{
+	struct list_head *list;
+	struct list_head *temp;
+	struct bnx2fc_hba *hba;
+
+	/* Called with bnx2fc_dev_lock held */
+	list_for_each_safe(list, temp, &adapter_list) {
+		hba = (struct bnx2fc_hba *)list;
+		if (hba->cnic == cnic)
+			return hba;
+	}
+	return NULL;
+}
+
+static struct bnx2fc_hba *bnx2fc_hba_lookup(struct net_device *phys_dev)
+{
+	struct list_head *list;
+	struct list_head *temp;
+	struct bnx2fc_hba *hba;
+
+	/* Called with bnx2fc_dev_lock held */
+	list_for_each_safe(list, temp, &adapter_list) {
+		hba = (struct bnx2fc_hba *)list;
+		if (hba->phys_dev == phys_dev)
+			return hba;
+	}
+	printk(KERN_ERR PFX "hba_lookup: hba NULL\n");
+	return NULL;
+}
+
+/**
+ * bnx2fc_ulp_exit - shuts down adapter instance and frees all resources
+ *
+ * @dev		cnic device handle
+ */
+static void bnx2fc_ulp_exit(struct cnic_dev *dev)
+{
+	struct bnx2fc_hba *hba;
+
+	BNX2FC_MISC_DBG("Entered bnx2fc_ulp_exit\n");
+
+	if (!test_bit(CNIC_F_BNX2X_CLASS, &dev->flags)) {
+		printk(KERN_ERR PFX "bnx2fc port check: %s, flags: %lx\n",
+			dev->netdev->name, dev->flags);
+		return;
+	}
+
+	mutex_lock(&bnx2fc_dev_lock);
+	hba = bnx2fc_find_hba_for_cnic(dev);
+	if (!hba) {
+		printk(KERN_ERR PFX "bnx2fc_ulp_exit: hba not found, dev 0%p\n",
+		       dev);
+		mutex_unlock(&bnx2fc_dev_lock);
+		return;
+	}
+
+	list_del_init(&hba->link);
+	adapter_count--;
+
+	if (test_bit(BNX2FC_CREATE_DONE, &hba->init_done)) {
+		/* destroy not called yet, move to quiesced list */
+		bnx2fc_netdev_cleanup(hba);
+		bnx2fc_if_destroy(hba->ctlr.lp);
+	}
+	mutex_unlock(&bnx2fc_dev_lock);
+
+	bnx2fc_ulp_stop(hba);
+	/* unregister cnic device */
+	if (test_and_clear_bit(BNX2FC_CNIC_REGISTERED, &hba->reg_with_cnic))
+		hba->cnic->unregister_device(hba->cnic, CNIC_ULP_FCOE);
+	bnx2fc_interface_destroy(hba);
+}
+
+/**
+ * bnx2fc_fcoe_reset - Resets the fcoe
+ *
+ * @shost: shost the reset is from
+ *
+ * Returns: always 0
+ */
+static int bnx2fc_fcoe_reset(struct Scsi_Host *shost)
+{
+	struct fc_lport *lport = shost_priv(shost);
+	fc_lport_reset(lport);
+	return 0;
+}
+
+
+static bool bnx2fc_match(struct net_device *netdev)
+{
+	mutex_lock(&bnx2fc_dev_lock);
+	if (netdev->priv_flags & IFF_802_1Q_VLAN) {
+		struct net_device *phys_dev = vlan_dev_real_dev(netdev);
+
+		if (bnx2fc_hba_lookup(phys_dev)) {
+			mutex_unlock(&bnx2fc_dev_lock);
+			return true;
+		}
+	}
+	mutex_unlock(&bnx2fc_dev_lock);
+	return false;
+}
+
+
+static struct fcoe_transport bnx2fc_transport = {
+	.name = {"bnx2fc"},
+	.attached = false,
+	.list = LIST_HEAD_INIT(bnx2fc_transport.list),
+	.match = bnx2fc_match,
+	.create = bnx2fc_create,
+	.destroy = bnx2fc_destroy,
+	.enable = bnx2fc_enable,
+	.disable = bnx2fc_disable,
+};
+
+/**
+ * bnx2fc_percpu_thread_create - Create a receive thread for an
+ *				 online CPU
+ *
+ * @cpu: cpu index for the online cpu
+ */
+static void bnx2fc_percpu_thread_create(unsigned int cpu)
+{
+	struct bnx2fc_percpu_s *p;
+	struct task_struct *thread;
+
+	p = &per_cpu(bnx2fc_percpu, cpu);
+
+	thread = kthread_create(bnx2fc_percpu_io_thread,
+				(void *)p,
+				"bnx2fc_thread/%d", cpu);
+	/* bind thread to the cpu */
+	if (likely(!IS_ERR(p->iothread))) {
+		kthread_bind(thread, cpu);
+		p->iothread = thread;
+		wake_up_process(thread);
+	}
+}
+
+static void bnx2fc_percpu_thread_destroy(unsigned int cpu)
+{
+	struct bnx2fc_percpu_s *p;
+	struct task_struct *thread;
+	struct bnx2fc_work *work, *tmp;
+	LIST_HEAD(work_list);
+
+	BNX2FC_MISC_DBG("destroying io thread for CPU %d\n", cpu);
+
+	/* Prevent any new work from being queued for this CPU */
+	p = &per_cpu(bnx2fc_percpu, cpu);
+	spin_lock_bh(&p->fp_work_lock);
+	thread = p->iothread;
+	p->iothread = NULL;
+
+
+	/* Free all work in the list */
+	list_for_each_entry_safe(work, tmp, &work_list, list) {
+		list_del_init(&work->list);
+		bnx2fc_process_cq_compl(work->tgt, work->wqe);
+		kfree(work);
+	}
+
+	spin_unlock_bh(&p->fp_work_lock);
+
+	if (thread)
+		kthread_stop(thread);
+}
+
+/**
+ * bnx2fc_cpu_callback - Handler for CPU hotplug events
+ *
+ * @nfb:    The callback data block
+ * @action: The event triggering the callback
+ * @hcpu:   The index of the CPU that the event is for
+ *
+ * This creates or destroys per-CPU data for fcoe
+ *
+ * Returns NOTIFY_OK always.
+ */
+static int bnx2fc_cpu_callback(struct notifier_block *nfb,
+			     unsigned long action, void *hcpu)
+{
+	unsigned cpu = (unsigned long)hcpu;
+
+	switch (action) {
+	case CPU_ONLINE:
+	case CPU_ONLINE_FROZEN:
+		printk(PFX "CPU %x online: Create Rx thread\n", cpu);
+		bnx2fc_percpu_thread_create(cpu);
+		break;
+	case CPU_DEAD:
+	case CPU_DEAD_FROZEN:
+		printk(PFX "CPU %x offline: Remove Rx thread\n", cpu);
+		bnx2fc_percpu_thread_destroy(cpu);
+		break;
+	default:
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+/**
+ * bnx2fc_mod_init - module init entry point
+ *
+ * Initialize driver wide global data structures, and register
+ * with cnic module
+ **/
+static int __init bnx2fc_mod_init(void)
+{
+	struct fcoe_percpu_s *bg;
+	struct task_struct *l2_thread;
+	int rc = 0;
+	unsigned int cpu = 0;
+	struct bnx2fc_percpu_s *p;
+
+	printk(KERN_INFO PFX "%s", version);
+
+	/* register as a fcoe transport */
+	rc = fcoe_transport_attach(&bnx2fc_transport);
+	if (rc) {
+		printk(KERN_ERR "failed to register an fcoe transport, check "
+			"if libfcoe is loaded\n");
+		goto out;
+	}
+
+	INIT_LIST_HEAD(&adapter_list);
+	mutex_init(&bnx2fc_dev_lock);
+	adapter_count = 0;
+
+	/* Attach FC transport template */
+	rc = bnx2fc_attach_transport();
+	if (rc)
+		goto detach_ft;
+
+	bnx2fc_wq = alloc_workqueue("bnx2fc", 0, 0);
+	if (!bnx2fc_wq) {
+		rc = -ENOMEM;
+		goto release_bt;
+	}
+
+	bg = &bnx2fc_global;
+	skb_queue_head_init(&bg->fcoe_rx_list);
+	l2_thread = kthread_create(bnx2fc_l2_rcv_thread,
+				   (void *)bg,
+				   "bnx2fc_l2_thread");
+	if (IS_ERR(l2_thread)) {
+		rc = PTR_ERR(l2_thread);
+		goto free_wq;
+	}
+	wake_up_process(l2_thread);
+	spin_lock_bh(&bg->fcoe_rx_list.lock);
+	bg->thread = l2_thread;
+	spin_unlock_bh(&bg->fcoe_rx_list.lock);
+
+	for_each_possible_cpu(cpu) {
+		p = &per_cpu(bnx2fc_percpu, cpu);
+		INIT_LIST_HEAD(&p->work_list);
+		spin_lock_init(&p->fp_work_lock);
+	}
+
+	for_each_online_cpu(cpu) {
+		bnx2fc_percpu_thread_create(cpu);
+	}
+
+	/* Initialize per CPU interrupt thread */
+	register_hotcpu_notifier(&bnx2fc_cpu_notifier);
+
+	cnic_register_driver(CNIC_ULP_FCOE, &bnx2fc_cnic_cb);
+
+	return 0;
+
+free_wq:
+	destroy_workqueue(bnx2fc_wq);
+release_bt:
+	bnx2fc_release_transport();
+detach_ft:
+	fcoe_transport_detach(&bnx2fc_transport);
+out:
+	return rc;
+}
+
+static void __exit bnx2fc_mod_exit(void)
+{
+	LIST_HEAD(to_be_deleted);
+	struct bnx2fc_hba *hba, *next;
+	struct fcoe_percpu_s *bg;
+	struct task_struct *l2_thread;
+	struct sk_buff *skb;
+	unsigned int cpu = 0;
+
+	/*
+	 * NOTE: Since cnic calls register_driver routine rtnl_lock,
+	 * it will have higher precedence than bnx2fc_dev_lock.
+	 * unregister_device() cannot be called with bnx2fc_dev_lock
+	 * held.
+	 */
+	mutex_lock(&bnx2fc_dev_lock);
+	list_splice(&adapter_list, &to_be_deleted);
+	INIT_LIST_HEAD(&adapter_list);
+	adapter_count = 0;
+	mutex_unlock(&bnx2fc_dev_lock);
+
+	/* Unregister with cnic */
+	list_for_each_entry_safe(hba, next, &to_be_deleted, link) {
+		list_del_init(&hba->link);
+		printk(KERN_ERR PFX "MOD_EXIT:destroy hba = 0x%p, kref = %d\n",
+			hba, atomic_read(&hba->kref.refcount));
+		bnx2fc_ulp_stop(hba);
+		/* unregister cnic device */
+		if (test_and_clear_bit(BNX2FC_CNIC_REGISTERED,
+				       &hba->reg_with_cnic))
+			hba->cnic->unregister_device(hba->cnic, CNIC_ULP_FCOE);
+		bnx2fc_interface_destroy(hba);
+	}
+	cnic_unregister_driver(CNIC_ULP_FCOE);
+
+	/* Destroy global thread */
+	bg = &bnx2fc_global;
+	spin_lock_bh(&bg->fcoe_rx_list.lock);
+	l2_thread = bg->thread;
+	bg->thread = NULL;
+	while ((skb = __skb_dequeue(&bg->fcoe_rx_list)) != NULL)
+		kfree_skb(skb);
+
+	spin_unlock_bh(&bg->fcoe_rx_list.lock);
+
+	if (l2_thread)
+		kthread_stop(l2_thread);
+
+	unregister_hotcpu_notifier(&bnx2fc_cpu_notifier);
+
+	/* Destroy per cpu threads */
+	for_each_online_cpu(cpu) {
+		bnx2fc_percpu_thread_destroy(cpu);
+	}
+
+	destroy_workqueue(bnx2fc_wq);
+	/*
+	 * detach from scsi transport
+	 * must happen after all destroys are done
+	 */
+	bnx2fc_release_transport();
+
+	/* detach from fcoe transport */
+	fcoe_transport_detach(&bnx2fc_transport);
+}
+
+module_init(bnx2fc_mod_init);
+module_exit(bnx2fc_mod_exit);
+
+static struct fc_function_template bnx2fc_transport_function = {
+	.show_host_node_name = 1,
+	.show_host_port_name = 1,
+	.show_host_supported_classes = 1,
+	.show_host_supported_fc4s = 1,
+	.show_host_active_fc4s = 1,
+	.show_host_maxframe_size = 1,
+
+	.show_host_port_id = 1,
+	.show_host_supported_speeds = 1,
+	.get_host_speed = fc_get_host_speed,
+	.show_host_speed = 1,
+	.show_host_port_type = 1,
+	.get_host_port_state = fc_get_host_port_state,
+	.show_host_port_state = 1,
+	.show_host_symbolic_name = 1,
+
+	.dd_fcrport_size = (sizeof(struct fc_rport_libfc_priv) +
+				sizeof(struct bnx2fc_rport)),
+	.show_rport_maxframe_size = 1,
+	.show_rport_supported_classes = 1,
+
+	.show_host_fabric_name = 1,
+	.show_starget_node_name = 1,
+	.show_starget_port_name = 1,
+	.show_starget_port_id = 1,
+	.set_rport_dev_loss_tmo = fc_set_rport_loss_tmo,
+	.show_rport_dev_loss_tmo = 1,
+	.get_fc_host_stats = bnx2fc_get_host_stats,
+
+	.issue_fc_host_lip = bnx2fc_fcoe_reset,
+
+	.terminate_rport_io = fc_rport_terminate_io,
+
+	.vport_create = bnx2fc_vport_create,
+	.vport_delete = bnx2fc_vport_destroy,
+	.vport_disable = bnx2fc_vport_disable,
+};
+
+static struct fc_function_template bnx2fc_vport_xport_function = {
+	.show_host_node_name = 1,
+	.show_host_port_name = 1,
+	.show_host_supported_classes = 1,
+	.show_host_supported_fc4s = 1,
+	.show_host_active_fc4s = 1,
+	.show_host_maxframe_size = 1,
+
+	.show_host_port_id = 1,
+	.show_host_supported_speeds = 1,
+	.get_host_speed = fc_get_host_speed,
+	.show_host_speed = 1,
+	.show_host_port_type = 1,
+	.get_host_port_state = fc_get_host_port_state,
+	.show_host_port_state = 1,
+	.show_host_symbolic_name = 1,
+
+	.dd_fcrport_size = (sizeof(struct fc_rport_libfc_priv) +
+				sizeof(struct bnx2fc_rport)),
+	.show_rport_maxframe_size = 1,
+	.show_rport_supported_classes = 1,
+
+	.show_host_fabric_name = 1,
+	.show_starget_node_name = 1,
+	.show_starget_port_name = 1,
+	.show_starget_port_id = 1,
+	.set_rport_dev_loss_tmo = fc_set_rport_loss_tmo,
+	.show_rport_dev_loss_tmo = 1,
+	.get_fc_host_stats = fc_get_host_stats,
+	.issue_fc_host_lip = bnx2fc_fcoe_reset,
+	.terminate_rport_io = fc_rport_terminate_io,
+};
+
+/**
+ * scsi_host_template structure used while registering with SCSI-ml
+ */
+static struct scsi_host_template bnx2fc_shost_template = {
+	.module			= THIS_MODULE,
+	.name			= "Broadcom Offload FCoE Initiator",
+	.queuecommand		= bnx2fc_queuecommand,
+	.eh_abort_handler	= bnx2fc_eh_abort,	  /* abts */
+	.eh_device_reset_handler = bnx2fc_eh_device_reset, /* lun reset */
+	.eh_target_reset_handler = bnx2fc_eh_target_reset, /* tgt reset */
+	.eh_host_reset_handler	= fc_eh_host_reset,
+	.slave_alloc		= fc_slave_alloc,
+	.change_queue_depth	= fc_change_queue_depth,
+	.change_queue_type	= fc_change_queue_type,
+	.this_id		= -1,
+	.cmd_per_lun		= 3,
+	.can_queue		= (BNX2FC_MAX_OUTSTANDING_CMNDS/2),
+	.use_clustering		= ENABLE_CLUSTERING,
+	.sg_tablesize		= BNX2FC_MAX_BDS_PER_CMD,
+	.max_sectors		= 512,
+};
+
+static struct libfc_function_template bnx2fc_libfc_fcn_templ = {
+	.frame_send		= bnx2fc_xmit,
+	.elsct_send		= bnx2fc_elsct_send,
+	.fcp_abort_io		= bnx2fc_abort_io,
+	.fcp_cleanup		= bnx2fc_cleanup,
+	.rport_event_callback	= bnx2fc_rport_event_handler,
+};
+
+/**
+ * bnx2fc_cnic_cb - global template of bnx2fc - cnic driver interface
+ *			structure carrying callback function pointers
+ */
+static struct cnic_ulp_ops bnx2fc_cnic_cb = {
+	.owner			= THIS_MODULE,
+	.cnic_init		= bnx2fc_ulp_init,
+	.cnic_exit		= bnx2fc_ulp_exit,
+	.cnic_start		= bnx2fc_ulp_start,
+	.cnic_stop		= bnx2fc_ulp_stop,
+	.indicate_kcqes		= bnx2fc_indicate_kcqe,
+	.indicate_netevent	= bnx2fc_indicate_netevent,
+};
diff --git a/drivers/scsi/bnx2fc/bnx2fc_hwi.c b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
new file mode 100644
index 000000000000..4f4096836742
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
@@ -0,0 +1,1868 @@
+/* bnx2fc_hwi.c: Broadcom NetXtreme II Linux FCoE offload driver.
+ * This file contains the code that low level functions that interact
+ * with 57712 FCoE firmware.
+ *
+ * Copyright (c) 2008 - 2010 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.
+ *
+ * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
+ */
+
+#include "bnx2fc.h"
+
+DECLARE_PER_CPU(struct bnx2fc_percpu_s, bnx2fc_percpu);
+
+static void bnx2fc_fastpath_notification(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *new_cqe_kcqe);
+static void bnx2fc_process_ofld_cmpl(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *ofld_kcqe);
+static void bnx2fc_process_enable_conn_cmpl(struct bnx2fc_hba *hba,
+						struct fcoe_kcqe *ofld_kcqe);
+static void bnx2fc_init_failure(struct bnx2fc_hba *hba, u32 err_code);
+static void bnx2fc_process_conn_destroy_cmpl(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *conn_destroy);
+
+int bnx2fc_send_stat_req(struct bnx2fc_hba *hba)
+{
+	struct fcoe_kwqe_stat stat_req;
+	struct kwqe *kwqe_arr[2];
+	int num_kwqes = 1;
+	int rc = 0;
+
+	memset(&stat_req, 0x00, sizeof(struct fcoe_kwqe_stat));
+	stat_req.hdr.op_code = FCOE_KWQE_OPCODE_STAT;
+	stat_req.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	stat_req.stat_params_addr_lo = (u32) hba->stats_buf_dma;
+	stat_req.stat_params_addr_hi = (u32) ((u64)hba->stats_buf_dma >> 32);
+
+	kwqe_arr[0] = (struct kwqe *) &stat_req;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+
+	return rc;
+}
+
+/**
+ * bnx2fc_send_fw_fcoe_init_msg - initiates initial handshake with FCoE f/w
+ *
+ * @hba:	adapter structure pointer
+ *
+ * Send down FCoE firmware init KWQEs which initiates the initial handshake
+ *	with the f/w.
+ *
+ */
+int bnx2fc_send_fw_fcoe_init_msg(struct bnx2fc_hba *hba)
+{
+	struct fcoe_kwqe_init1 fcoe_init1;
+	struct fcoe_kwqe_init2 fcoe_init2;
+	struct fcoe_kwqe_init3 fcoe_init3;
+	struct kwqe *kwqe_arr[3];
+	int num_kwqes = 3;
+	int rc = 0;
+
+	if (!hba->cnic) {
+		printk(KERN_ALERT PFX "hba->cnic NULL during fcoe fw init\n");
+		return -ENODEV;
+	}
+
+	/* fill init1 KWQE */
+	memset(&fcoe_init1, 0x00, sizeof(struct fcoe_kwqe_init1));
+	fcoe_init1.hdr.op_code = FCOE_KWQE_OPCODE_INIT1;
+	fcoe_init1.hdr.flags = (FCOE_KWQE_LAYER_CODE <<
+					FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	fcoe_init1.num_tasks = BNX2FC_MAX_TASKS;
+	fcoe_init1.sq_num_wqes = BNX2FC_SQ_WQES_MAX;
+	fcoe_init1.rq_num_wqes = BNX2FC_RQ_WQES_MAX;
+	fcoe_init1.rq_buffer_log_size = BNX2FC_RQ_BUF_LOG_SZ;
+	fcoe_init1.cq_num_wqes = BNX2FC_CQ_WQES_MAX;
+	fcoe_init1.dummy_buffer_addr_lo = (u32) hba->dummy_buf_dma;
+	fcoe_init1.dummy_buffer_addr_hi = (u32) ((u64)hba->dummy_buf_dma >> 32);
+	fcoe_init1.task_list_pbl_addr_lo = (u32) hba->task_ctx_bd_dma;
+	fcoe_init1.task_list_pbl_addr_hi =
+				(u32) ((u64) hba->task_ctx_bd_dma >> 32);
+	fcoe_init1.mtu = hba->netdev->mtu;
+
+	fcoe_init1.flags = (PAGE_SHIFT <<
+				FCOE_KWQE_INIT1_LOG_PAGE_SIZE_SHIFT);
+
+	fcoe_init1.num_sessions_log = BNX2FC_NUM_MAX_SESS_LOG;
+
+	/* fill init2 KWQE */
+	memset(&fcoe_init2, 0x00, sizeof(struct fcoe_kwqe_init2));
+	fcoe_init2.hdr.op_code = FCOE_KWQE_OPCODE_INIT2;
+	fcoe_init2.hdr.flags = (FCOE_KWQE_LAYER_CODE <<
+					FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	fcoe_init2.hash_tbl_pbl_addr_lo = (u32) hba->hash_tbl_pbl_dma;
+	fcoe_init2.hash_tbl_pbl_addr_hi = (u32)
+					   ((u64) hba->hash_tbl_pbl_dma >> 32);
+
+	fcoe_init2.t2_hash_tbl_addr_lo = (u32) hba->t2_hash_tbl_dma;
+	fcoe_init2.t2_hash_tbl_addr_hi = (u32)
+					  ((u64) hba->t2_hash_tbl_dma >> 32);
+
+	fcoe_init2.t2_ptr_hash_tbl_addr_lo = (u32) hba->t2_hash_tbl_ptr_dma;
+	fcoe_init2.t2_ptr_hash_tbl_addr_hi = (u32)
+					((u64) hba->t2_hash_tbl_ptr_dma >> 32);
+
+	fcoe_init2.free_list_count = BNX2FC_NUM_MAX_SESS;
+
+	/* fill init3 KWQE */
+	memset(&fcoe_init3, 0x00, sizeof(struct fcoe_kwqe_init3));
+	fcoe_init3.hdr.op_code = FCOE_KWQE_OPCODE_INIT3;
+	fcoe_init3.hdr.flags = (FCOE_KWQE_LAYER_CODE <<
+					FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+	fcoe_init3.error_bit_map_lo = 0xffffffff;
+	fcoe_init3.error_bit_map_hi = 0xffffffff;
+
+
+	kwqe_arr[0] = (struct kwqe *) &fcoe_init1;
+	kwqe_arr[1] = (struct kwqe *) &fcoe_init2;
+	kwqe_arr[2] = (struct kwqe *) &fcoe_init3;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+
+	return rc;
+}
+int bnx2fc_send_fw_fcoe_destroy_msg(struct bnx2fc_hba *hba)
+{
+	struct fcoe_kwqe_destroy fcoe_destroy;
+	struct kwqe *kwqe_arr[2];
+	int num_kwqes = 1;
+	int rc = -1;
+
+	/* fill destroy KWQE */
+	memset(&fcoe_destroy, 0x00, sizeof(struct fcoe_kwqe_destroy));
+	fcoe_destroy.hdr.op_code = FCOE_KWQE_OPCODE_DESTROY;
+	fcoe_destroy.hdr.flags = (FCOE_KWQE_LAYER_CODE <<
+					FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+	kwqe_arr[0] = (struct kwqe *) &fcoe_destroy;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+	return rc;
+}
+
+/**
+ * bnx2fc_send_session_ofld_req - initiates FCoE Session offload process
+ *
+ * @port:		port structure pointer
+ * @tgt:		bnx2fc_rport structure pointer
+ */
+int bnx2fc_send_session_ofld_req(struct fcoe_port *port,
+					struct bnx2fc_rport *tgt)
+{
+	struct fc_lport *lport = port->lport;
+	struct bnx2fc_hba *hba = port->priv;
+	struct kwqe *kwqe_arr[4];
+	struct fcoe_kwqe_conn_offload1 ofld_req1;
+	struct fcoe_kwqe_conn_offload2 ofld_req2;
+	struct fcoe_kwqe_conn_offload3 ofld_req3;
+	struct fcoe_kwqe_conn_offload4 ofld_req4;
+	struct fc_rport_priv *rdata = tgt->rdata;
+	struct fc_rport *rport = tgt->rport;
+	int num_kwqes = 4;
+	u32 port_id;
+	int rc = 0;
+	u16 conn_id;
+
+	/* Initialize offload request 1 structure */
+	memset(&ofld_req1, 0x00, sizeof(struct fcoe_kwqe_conn_offload1));
+
+	ofld_req1.hdr.op_code = FCOE_KWQE_OPCODE_OFFLOAD_CONN1;
+	ofld_req1.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+
+	conn_id = (u16)tgt->fcoe_conn_id;
+	ofld_req1.fcoe_conn_id = conn_id;
+
+
+	ofld_req1.sq_addr_lo = (u32) tgt->sq_dma;
+	ofld_req1.sq_addr_hi = (u32)((u64) tgt->sq_dma >> 32);
+
+	ofld_req1.rq_pbl_addr_lo = (u32) tgt->rq_pbl_dma;
+	ofld_req1.rq_pbl_addr_hi = (u32)((u64) tgt->rq_pbl_dma >> 32);
+
+	ofld_req1.rq_first_pbe_addr_lo = (u32) tgt->rq_dma;
+	ofld_req1.rq_first_pbe_addr_hi =
+				(u32)((u64) tgt->rq_dma >> 32);
+
+	ofld_req1.rq_prod = 0x8000;
+
+	/* Initialize offload request 2 structure */
+	memset(&ofld_req2, 0x00, sizeof(struct fcoe_kwqe_conn_offload2));
+
+	ofld_req2.hdr.op_code = FCOE_KWQE_OPCODE_OFFLOAD_CONN2;
+	ofld_req2.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	ofld_req2.tx_max_fc_pay_len = rdata->maxframe_size;
+
+	ofld_req2.cq_addr_lo = (u32) tgt->cq_dma;
+	ofld_req2.cq_addr_hi = (u32)((u64)tgt->cq_dma >> 32);
+
+	ofld_req2.xferq_addr_lo = (u32) tgt->xferq_dma;
+	ofld_req2.xferq_addr_hi = (u32)((u64)tgt->xferq_dma >> 32);
+
+	ofld_req2.conn_db_addr_lo = (u32)tgt->conn_db_dma;
+	ofld_req2.conn_db_addr_hi = (u32)((u64)tgt->conn_db_dma >> 32);
+
+	/* Initialize offload request 3 structure */
+	memset(&ofld_req3, 0x00, sizeof(struct fcoe_kwqe_conn_offload3));
+
+	ofld_req3.hdr.op_code = FCOE_KWQE_OPCODE_OFFLOAD_CONN3;
+	ofld_req3.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	ofld_req3.vlan_tag = hba->vlan_id <<
+				FCOE_KWQE_CONN_OFFLOAD3_VLAN_ID_SHIFT;
+	ofld_req3.vlan_tag |= 3 << FCOE_KWQE_CONN_OFFLOAD3_PRIORITY_SHIFT;
+
+	port_id = fc_host_port_id(lport->host);
+	if (port_id == 0) {
+		BNX2FC_HBA_DBG(lport, "ofld_req: port_id = 0, link down?\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Store s_id of the initiator for further reference. This will
+	 * be used during disable/destroy during linkdown processing as
+	 * when the lport is reset, the port_id also is reset to 0
+	 */
+	tgt->sid = port_id;
+	ofld_req3.s_id[0] = (port_id & 0x000000FF);
+	ofld_req3.s_id[1] = (port_id & 0x0000FF00) >> 8;
+	ofld_req3.s_id[2] = (port_id & 0x00FF0000) >> 16;
+
+	port_id = rport->port_id;
+	ofld_req3.d_id[0] = (port_id & 0x000000FF);
+	ofld_req3.d_id[1] = (port_id & 0x0000FF00) >> 8;
+	ofld_req3.d_id[2] = (port_id & 0x00FF0000) >> 16;
+
+	ofld_req3.tx_total_conc_seqs = rdata->max_seq;
+
+	ofld_req3.tx_max_conc_seqs_c3 = rdata->max_seq;
+	ofld_req3.rx_max_fc_pay_len  = lport->mfs;
+
+	ofld_req3.rx_total_conc_seqs = BNX2FC_MAX_SEQS;
+	ofld_req3.rx_max_conc_seqs_c3 = BNX2FC_MAX_SEQS;
+	ofld_req3.rx_open_seqs_exch_c3 = 1;
+
+	ofld_req3.confq_first_pbe_addr_lo = tgt->confq_dma;
+	ofld_req3.confq_first_pbe_addr_hi = (u32)((u64) tgt->confq_dma >> 32);
+
+	/* set mul_n_port_ids supported flag to 0, until it is supported */
+	ofld_req3.flags = 0;
+	/*
+	ofld_req3.flags |= (((lport->send_sp_features & FC_SP_FT_MNA) ? 1:0) <<
+			    FCOE_KWQE_CONN_OFFLOAD3_B_MUL_N_PORT_IDS_SHIFT);
+	*/
+	/* Info from PLOGI response */
+	ofld_req3.flags |= (((rdata->sp_features & FC_SP_FT_EDTR) ? 1 : 0) <<
+			     FCOE_KWQE_CONN_OFFLOAD3_B_E_D_TOV_RES_SHIFT);
+
+	ofld_req3.flags |= (((rdata->sp_features & FC_SP_FT_SEQC) ? 1 : 0) <<
+			     FCOE_KWQE_CONN_OFFLOAD3_B_CONT_INCR_SEQ_CNT_SHIFT);
+
+	/* vlan flag */
+	ofld_req3.flags |= (hba->vlan_enabled <<
+			    FCOE_KWQE_CONN_OFFLOAD3_B_VLAN_FLAG_SHIFT);
+
+	/* C2_VALID and ACK flags are not set as they are not suppported */
+
+
+	/* Initialize offload request 4 structure */
+	memset(&ofld_req4, 0x00, sizeof(struct fcoe_kwqe_conn_offload4));
+	ofld_req4.hdr.op_code = FCOE_KWQE_OPCODE_OFFLOAD_CONN4;
+	ofld_req4.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	ofld_req4.e_d_tov_timer_val = lport->e_d_tov / 20;
+
+
+	ofld_req4.src_mac_addr_lo32[0] =  port->data_src_addr[5];
+							/* local mac */
+	ofld_req4.src_mac_addr_lo32[1] =  port->data_src_addr[4];
+	ofld_req4.src_mac_addr_lo32[2] =  port->data_src_addr[3];
+	ofld_req4.src_mac_addr_lo32[3] =  port->data_src_addr[2];
+	ofld_req4.src_mac_addr_hi16[0] =  port->data_src_addr[1];
+	ofld_req4.src_mac_addr_hi16[1] =  port->data_src_addr[0];
+	ofld_req4.dst_mac_addr_lo32[0] =  hba->ctlr.dest_addr[5];/* fcf mac */
+	ofld_req4.dst_mac_addr_lo32[1] =  hba->ctlr.dest_addr[4];
+	ofld_req4.dst_mac_addr_lo32[2] =  hba->ctlr.dest_addr[3];
+	ofld_req4.dst_mac_addr_lo32[3] =  hba->ctlr.dest_addr[2];
+	ofld_req4.dst_mac_addr_hi16[0] =  hba->ctlr.dest_addr[1];
+	ofld_req4.dst_mac_addr_hi16[1] =  hba->ctlr.dest_addr[0];
+
+	ofld_req4.lcq_addr_lo = (u32) tgt->lcq_dma;
+	ofld_req4.lcq_addr_hi = (u32)((u64) tgt->lcq_dma >> 32);
+
+	ofld_req4.confq_pbl_base_addr_lo = (u32) tgt->confq_pbl_dma;
+	ofld_req4.confq_pbl_base_addr_hi =
+					(u32)((u64) tgt->confq_pbl_dma >> 32);
+
+	kwqe_arr[0] = (struct kwqe *) &ofld_req1;
+	kwqe_arr[1] = (struct kwqe *) &ofld_req2;
+	kwqe_arr[2] = (struct kwqe *) &ofld_req3;
+	kwqe_arr[3] = (struct kwqe *) &ofld_req4;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+
+	return rc;
+}
+
+/**
+ * bnx2fc_send_session_enable_req - initiates FCoE Session enablement
+ *
+ * @port:		port structure pointer
+ * @tgt:		bnx2fc_rport structure pointer
+ */
+static int bnx2fc_send_session_enable_req(struct fcoe_port *port,
+					struct bnx2fc_rport *tgt)
+{
+	struct kwqe *kwqe_arr[2];
+	struct bnx2fc_hba *hba = port->priv;
+	struct fcoe_kwqe_conn_enable_disable enbl_req;
+	struct fc_lport *lport = port->lport;
+	struct fc_rport *rport = tgt->rport;
+	int num_kwqes = 1;
+	int rc = 0;
+	u32 port_id;
+
+	memset(&enbl_req, 0x00,
+	       sizeof(struct fcoe_kwqe_conn_enable_disable));
+	enbl_req.hdr.op_code = FCOE_KWQE_OPCODE_ENABLE_CONN;
+	enbl_req.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	enbl_req.src_mac_addr_lo32[0] =  port->data_src_addr[5];
+							/* local mac */
+	enbl_req.src_mac_addr_lo32[1] =  port->data_src_addr[4];
+	enbl_req.src_mac_addr_lo32[2] =  port->data_src_addr[3];
+	enbl_req.src_mac_addr_lo32[3] =  port->data_src_addr[2];
+	enbl_req.src_mac_addr_hi16[0] =  port->data_src_addr[1];
+	enbl_req.src_mac_addr_hi16[1] =  port->data_src_addr[0];
+
+	enbl_req.dst_mac_addr_lo32[0] =  hba->ctlr.dest_addr[5];/* fcf mac */
+	enbl_req.dst_mac_addr_lo32[1] =  hba->ctlr.dest_addr[4];
+	enbl_req.dst_mac_addr_lo32[2] =  hba->ctlr.dest_addr[3];
+	enbl_req.dst_mac_addr_lo32[3] =  hba->ctlr.dest_addr[2];
+	enbl_req.dst_mac_addr_hi16[0] =  hba->ctlr.dest_addr[1];
+	enbl_req.dst_mac_addr_hi16[1] =  hba->ctlr.dest_addr[0];
+
+	port_id = fc_host_port_id(lport->host);
+	if (port_id != tgt->sid) {
+		printk(KERN_ERR PFX "WARN: enable_req port_id = 0x%x,"
+				"sid = 0x%x\n", port_id, tgt->sid);
+		port_id = tgt->sid;
+	}
+	enbl_req.s_id[0] = (port_id & 0x000000FF);
+	enbl_req.s_id[1] = (port_id & 0x0000FF00) >> 8;
+	enbl_req.s_id[2] = (port_id & 0x00FF0000) >> 16;
+
+	port_id = rport->port_id;
+	enbl_req.d_id[0] = (port_id & 0x000000FF);
+	enbl_req.d_id[1] = (port_id & 0x0000FF00) >> 8;
+	enbl_req.d_id[2] = (port_id & 0x00FF0000) >> 16;
+	enbl_req.vlan_tag = hba->vlan_id <<
+				FCOE_KWQE_CONN_ENABLE_DISABLE_VLAN_ID_SHIFT;
+	enbl_req.vlan_tag |= 3 << FCOE_KWQE_CONN_ENABLE_DISABLE_PRIORITY_SHIFT;
+	enbl_req.vlan_flag = hba->vlan_enabled;
+	enbl_req.context_id = tgt->context_id;
+	enbl_req.conn_id = tgt->fcoe_conn_id;
+
+	kwqe_arr[0] = (struct kwqe *) &enbl_req;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+	return rc;
+}
+
+/**
+ * bnx2fc_send_session_disable_req - initiates FCoE Session disable
+ *
+ * @port:		port structure pointer
+ * @tgt:		bnx2fc_rport structure pointer
+ */
+int bnx2fc_send_session_disable_req(struct fcoe_port *port,
+				    struct bnx2fc_rport *tgt)
+{
+	struct bnx2fc_hba *hba = port->priv;
+	struct fcoe_kwqe_conn_enable_disable disable_req;
+	struct kwqe *kwqe_arr[2];
+	struct fc_rport *rport = tgt->rport;
+	int num_kwqes = 1;
+	int rc = 0;
+	u32 port_id;
+
+	memset(&disable_req, 0x00,
+	       sizeof(struct fcoe_kwqe_conn_enable_disable));
+	disable_req.hdr.op_code = FCOE_KWQE_OPCODE_DISABLE_CONN;
+	disable_req.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	disable_req.src_mac_addr_lo32[0] =  port->data_src_addr[5];
+	disable_req.src_mac_addr_lo32[2] =  port->data_src_addr[3];
+	disable_req.src_mac_addr_lo32[3] =  port->data_src_addr[2];
+	disable_req.src_mac_addr_hi16[0] =  port->data_src_addr[1];
+	disable_req.src_mac_addr_hi16[1] =  port->data_src_addr[0];
+
+	disable_req.dst_mac_addr_lo32[0] =  hba->ctlr.dest_addr[5];/* fcf mac */
+	disable_req.dst_mac_addr_lo32[1] =  hba->ctlr.dest_addr[4];
+	disable_req.dst_mac_addr_lo32[2] =  hba->ctlr.dest_addr[3];
+	disable_req.dst_mac_addr_lo32[3] =  hba->ctlr.dest_addr[2];
+	disable_req.dst_mac_addr_hi16[0] =  hba->ctlr.dest_addr[1];
+	disable_req.dst_mac_addr_hi16[1] =  hba->ctlr.dest_addr[0];
+
+	port_id = tgt->sid;
+	disable_req.s_id[0] = (port_id & 0x000000FF);
+	disable_req.s_id[1] = (port_id & 0x0000FF00) >> 8;
+	disable_req.s_id[2] = (port_id & 0x00FF0000) >> 16;
+
+
+	port_id = rport->port_id;
+	disable_req.d_id[0] = (port_id & 0x000000FF);
+	disable_req.d_id[1] = (port_id & 0x0000FF00) >> 8;
+	disable_req.d_id[2] = (port_id & 0x00FF0000) >> 16;
+	disable_req.context_id = tgt->context_id;
+	disable_req.conn_id = tgt->fcoe_conn_id;
+	disable_req.vlan_tag = hba->vlan_id <<
+				FCOE_KWQE_CONN_ENABLE_DISABLE_VLAN_ID_SHIFT;
+	disable_req.vlan_tag |=
+			3 << FCOE_KWQE_CONN_ENABLE_DISABLE_PRIORITY_SHIFT;
+	disable_req.vlan_flag = hba->vlan_enabled;
+
+	kwqe_arr[0] = (struct kwqe *) &disable_req;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+
+	return rc;
+}
+
+/**
+ * bnx2fc_send_session_destroy_req - initiates FCoE Session destroy
+ *
+ * @port:		port structure pointer
+ * @tgt:		bnx2fc_rport structure pointer
+ */
+int bnx2fc_send_session_destroy_req(struct bnx2fc_hba *hba,
+					struct bnx2fc_rport *tgt)
+{
+	struct fcoe_kwqe_conn_destroy destroy_req;
+	struct kwqe *kwqe_arr[2];
+	int num_kwqes = 1;
+	int rc = 0;
+
+	memset(&destroy_req, 0x00, sizeof(struct fcoe_kwqe_conn_destroy));
+	destroy_req.hdr.op_code = FCOE_KWQE_OPCODE_DESTROY_CONN;
+	destroy_req.hdr.flags =
+		(FCOE_KWQE_LAYER_CODE << FCOE_KWQE_HEADER_LAYER_CODE_SHIFT);
+
+	destroy_req.context_id = tgt->context_id;
+	destroy_req.conn_id = tgt->fcoe_conn_id;
+
+	kwqe_arr[0] = (struct kwqe *) &destroy_req;
+
+	if (hba->cnic && hba->cnic->submit_kwqes)
+		rc = hba->cnic->submit_kwqes(hba->cnic, kwqe_arr, num_kwqes);
+
+	return rc;
+}
+
+static void bnx2fc_unsol_els_work(struct work_struct *work)
+{
+	struct bnx2fc_unsol_els *unsol_els;
+	struct fc_lport *lport;
+	struct fc_frame *fp;
+
+	unsol_els = container_of(work, struct bnx2fc_unsol_els, unsol_els_work);
+	lport = unsol_els->lport;
+	fp = unsol_els->fp;
+	fc_exch_recv(lport, fp);
+	kfree(unsol_els);
+}
+
+void bnx2fc_process_l2_frame_compl(struct bnx2fc_rport *tgt,
+				   unsigned char *buf,
+				   u32 frame_len, u16 l2_oxid)
+{
+	struct fcoe_port *port = tgt->port;
+	struct fc_lport *lport = port->lport;
+	struct bnx2fc_unsol_els *unsol_els;
+	struct fc_frame_header *fh;
+	struct fc_frame *fp;
+	struct sk_buff *skb;
+	u32 payload_len;
+	u32 crc;
+	u8 op;
+
+
+	unsol_els = kzalloc(sizeof(*unsol_els), GFP_ATOMIC);
+	if (!unsol_els) {
+		BNX2FC_TGT_DBG(tgt, "Unable to allocate unsol_work\n");
+		return;
+	}
+
+	BNX2FC_TGT_DBG(tgt, "l2_frame_compl l2_oxid = 0x%x, frame_len = %d\n",
+		l2_oxid, frame_len);
+
+	payload_len = frame_len - sizeof(struct fc_frame_header);
+
+	fp = fc_frame_alloc(lport, payload_len);
+	if (!fp) {
+		printk(KERN_ERR PFX "fc_frame_alloc failure\n");
+		return;
+	}
+
+	fh = (struct fc_frame_header *) fc_frame_header_get(fp);
+	/* Copy FC Frame header and payload into the frame */
+	memcpy(fh, buf, frame_len);
+
+	if (l2_oxid != FC_XID_UNKNOWN)
+		fh->fh_ox_id = htons(l2_oxid);
+
+	skb = fp_skb(fp);
+
+	if ((fh->fh_r_ctl == FC_RCTL_ELS_REQ) ||
+	    (fh->fh_r_ctl == FC_RCTL_ELS_REP)) {
+
+		if (fh->fh_type == FC_TYPE_ELS) {
+			op = fc_frame_payload_op(fp);
+			if ((op == ELS_TEST) ||	(op == ELS_ESTC) ||
+			    (op == ELS_FAN) || (op == ELS_CSU)) {
+				/*
+				 * No need to reply for these
+				 * ELS requests
+				 */
+				printk(KERN_ERR PFX "dropping ELS 0x%x\n", op);
+				kfree_skb(skb);
+				return;
+			}
+		}
+		crc = fcoe_fc_crc(fp);
+		fc_frame_init(fp);
+		fr_dev(fp) = lport;
+		fr_sof(fp) = FC_SOF_I3;
+		fr_eof(fp) = FC_EOF_T;
+		fr_crc(fp) = cpu_to_le32(~crc);
+		unsol_els->lport = lport;
+		unsol_els->fp = fp;
+		INIT_WORK(&unsol_els->unsol_els_work, bnx2fc_unsol_els_work);
+		queue_work(bnx2fc_wq, &unsol_els->unsol_els_work);
+	} else {
+		BNX2FC_HBA_DBG(lport, "fh_r_ctl = 0x%x\n", fh->fh_r_ctl);
+		kfree_skb(skb);
+	}
+}
+
+static void bnx2fc_process_unsol_compl(struct bnx2fc_rport *tgt, u16 wqe)
+{
+	u8 num_rq;
+	struct fcoe_err_report_entry *err_entry;
+	unsigned char *rq_data;
+	unsigned char *buf = NULL, *buf1;
+	int i;
+	u16 xid;
+	u32 frame_len, len;
+	struct bnx2fc_cmd *io_req = NULL;
+	struct fcoe_task_ctx_entry *task, *task_page;
+	struct bnx2fc_hba *hba = tgt->port->priv;
+	int task_idx, index;
+	int rc = 0;
+
+
+	BNX2FC_TGT_DBG(tgt, "Entered UNSOL COMPLETION wqe = 0x%x\n", wqe);
+	switch (wqe & FCOE_UNSOLICITED_CQE_SUBTYPE) {
+	case FCOE_UNSOLICITED_FRAME_CQE_TYPE:
+		frame_len = (wqe & FCOE_UNSOLICITED_CQE_PKT_LEN) >>
+			     FCOE_UNSOLICITED_CQE_PKT_LEN_SHIFT;
+
+		num_rq = (frame_len + BNX2FC_RQ_BUF_SZ - 1) / BNX2FC_RQ_BUF_SZ;
+
+		rq_data = (unsigned char *)bnx2fc_get_next_rqe(tgt, num_rq);
+		if (rq_data) {
+			buf = rq_data;
+		} else {
+			buf1 = buf = kmalloc((num_rq * BNX2FC_RQ_BUF_SZ),
+					      GFP_ATOMIC);
+
+			if (!buf1) {
+				BNX2FC_TGT_DBG(tgt, "Memory alloc failure\n");
+				break;
+			}
+
+			for (i = 0; i < num_rq; i++) {
+				rq_data = (unsigned char *)
+					   bnx2fc_get_next_rqe(tgt, 1);
+				len = BNX2FC_RQ_BUF_SZ;
+				memcpy(buf1, rq_data, len);
+				buf1 += len;
+			}
+		}
+		bnx2fc_process_l2_frame_compl(tgt, buf, frame_len,
+					      FC_XID_UNKNOWN);
+
+		if (buf != rq_data)
+			kfree(buf);
+		bnx2fc_return_rqe(tgt, num_rq);
+		break;
+
+	case FCOE_ERROR_DETECTION_CQE_TYPE:
+		/*
+		 *In case of error reporting CQE a single RQ entry
+		 * is consumes.
+		 */
+		spin_lock_bh(&tgt->tgt_lock);
+		num_rq = 1;
+		err_entry = (struct fcoe_err_report_entry *)
+			     bnx2fc_get_next_rqe(tgt, 1);
+		xid = err_entry->fc_hdr.ox_id;
+		BNX2FC_TGT_DBG(tgt, "Unsol Error Frame OX_ID = 0x%x\n", xid);
+		BNX2FC_TGT_DBG(tgt, "err_warn_bitmap = %08x:%08x\n",
+			err_entry->err_warn_bitmap_hi,
+			err_entry->err_warn_bitmap_lo);
+		BNX2FC_TGT_DBG(tgt, "buf_offsets - tx = 0x%x, rx = 0x%x\n",
+			err_entry->tx_buf_off, err_entry->rx_buf_off);
+
+		bnx2fc_return_rqe(tgt, 1);
+
+		if (xid > BNX2FC_MAX_XID) {
+			BNX2FC_TGT_DBG(tgt, "xid(0x%x) out of FW range\n",
+				   xid);
+			spin_unlock_bh(&tgt->tgt_lock);
+			break;
+		}
+
+		task_idx = xid / BNX2FC_TASKS_PER_PAGE;
+		index = xid % BNX2FC_TASKS_PER_PAGE;
+		task_page = (struct fcoe_task_ctx_entry *)
+						hba->task_ctx[task_idx];
+		task = &(task_page[index]);
+
+		io_req = (struct bnx2fc_cmd *)hba->cmd_mgr->cmds[xid];
+		if (!io_req) {
+			spin_unlock_bh(&tgt->tgt_lock);
+			break;
+		}
+
+		if (io_req->cmd_type != BNX2FC_SCSI_CMD) {
+			printk(KERN_ERR PFX "err_warn: Not a SCSI cmd\n");
+			spin_unlock_bh(&tgt->tgt_lock);
+			break;
+		}
+
+		if (test_and_clear_bit(BNX2FC_FLAG_IO_CLEANUP,
+				       &io_req->req_flags)) {
+			BNX2FC_IO_DBG(io_req, "unsol_err: cleanup in "
+					    "progress.. ignore unsol err\n");
+			spin_unlock_bh(&tgt->tgt_lock);
+			break;
+		}
+
+		/*
+		 * If ABTS is already in progress, and FW error is
+		 * received after that, do not cancel the timeout_work
+		 * and let the error recovery continue by explicitly
+		 * logging out the target, when the ABTS eventually
+		 * times out.
+		 */
+		if (!test_and_set_bit(BNX2FC_FLAG_ISSUE_ABTS,
+				      &io_req->req_flags)) {
+			/*
+			 * Cancel the timeout_work, as we received IO
+			 * completion with FW error.
+			 */
+			if (cancel_delayed_work(&io_req->timeout_work))
+				kref_put(&io_req->refcount,
+					 bnx2fc_cmd_release); /* timer hold */
+
+			rc = bnx2fc_initiate_abts(io_req);
+			if (rc != SUCCESS) {
+				BNX2FC_IO_DBG(io_req, "err_warn: initiate_abts "
+					"failed. issue cleanup\n");
+				rc = bnx2fc_initiate_cleanup(io_req);
+				BUG_ON(rc);
+			}
+		} else
+			printk(KERN_ERR PFX "err_warn: io_req (0x%x) already "
+					    "in ABTS processing\n", xid);
+		spin_unlock_bh(&tgt->tgt_lock);
+		break;
+
+	case FCOE_WARNING_DETECTION_CQE_TYPE:
+		/*
+		 *In case of warning reporting CQE a single RQ entry
+		 * is consumes.
+		 */
+		num_rq = 1;
+		err_entry = (struct fcoe_err_report_entry *)
+			     bnx2fc_get_next_rqe(tgt, 1);
+		xid = cpu_to_be16(err_entry->fc_hdr.ox_id);
+		BNX2FC_TGT_DBG(tgt, "Unsol Warning Frame OX_ID = 0x%x\n", xid);
+		BNX2FC_TGT_DBG(tgt, "err_warn_bitmap = %08x:%08x",
+			err_entry->err_warn_bitmap_hi,
+			err_entry->err_warn_bitmap_lo);
+		BNX2FC_TGT_DBG(tgt, "buf_offsets - tx = 0x%x, rx = 0x%x",
+			err_entry->tx_buf_off, err_entry->rx_buf_off);
+
+		bnx2fc_return_rqe(tgt, 1);
+		break;
+
+	default:
+		printk(KERN_ERR PFX "Unsol Compl: Invalid CQE Subtype\n");
+		break;
+	}
+}
+
+void bnx2fc_process_cq_compl(struct bnx2fc_rport *tgt, u16 wqe)
+{
+	struct fcoe_task_ctx_entry *task;
+	struct fcoe_task_ctx_entry *task_page;
+	struct fcoe_port *port = tgt->port;
+	struct bnx2fc_hba *hba = port->priv;
+	struct bnx2fc_cmd *io_req;
+	int task_idx, index;
+	u16 xid;
+	u8  cmd_type;
+	u8 rx_state = 0;
+	u8 num_rq;
+
+	spin_lock_bh(&tgt->tgt_lock);
+	xid = wqe & FCOE_PEND_WQ_CQE_TASK_ID;
+	if (xid >= BNX2FC_MAX_TASKS) {
+		printk(KERN_ALERT PFX "ERROR:xid out of range\n");
+		spin_unlock_bh(&tgt->tgt_lock);
+		return;
+	}
+	task_idx = xid / BNX2FC_TASKS_PER_PAGE;
+	index = xid % BNX2FC_TASKS_PER_PAGE;
+	task_page = (struct fcoe_task_ctx_entry *)hba->task_ctx[task_idx];
+	task = &(task_page[index]);
+
+	num_rq = ((task->rx_wr_tx_rd.rx_flags &
+		   FCOE_TASK_CTX_ENTRY_RXWR_TXRD_NUM_RQ_WQE) >>
+		   FCOE_TASK_CTX_ENTRY_RXWR_TXRD_NUM_RQ_WQE_SHIFT);
+
+	io_req = (struct bnx2fc_cmd *)hba->cmd_mgr->cmds[xid];
+
+	if (io_req == NULL) {
+		printk(KERN_ERR PFX "ERROR? cq_compl - io_req is NULL\n");
+		spin_unlock_bh(&tgt->tgt_lock);
+		return;
+	}
+
+	/* Timestamp IO completion time */
+	cmd_type = io_req->cmd_type;
+
+	/* optimized completion path */
+	if (cmd_type == BNX2FC_SCSI_CMD) {
+		rx_state = ((task->rx_wr_tx_rd.rx_flags &
+			    FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RX_STATE) >>
+			    FCOE_TASK_CTX_ENTRY_RXWR_TXRD_RX_STATE_SHIFT);
+
+		if (rx_state == FCOE_TASK_RX_STATE_COMPLETED) {
+			bnx2fc_process_scsi_cmd_compl(io_req, task, num_rq);
+			spin_unlock_bh(&tgt->tgt_lock);
+			return;
+		}
+	}
+
+	/* Process other IO completion types */
+	switch (cmd_type) {
+	case BNX2FC_SCSI_CMD:
+		if (rx_state == FCOE_TASK_RX_STATE_ABTS_COMPLETED)
+			bnx2fc_process_abts_compl(io_req, task, num_rq);
+		else if (rx_state ==
+			 FCOE_TASK_RX_STATE_EXCHANGE_CLEANUP_COMPLETED)
+			bnx2fc_process_cleanup_compl(io_req, task, num_rq);
+		else
+			printk(KERN_ERR PFX "Invalid rx state - %d\n",
+				rx_state);
+		break;
+
+	case BNX2FC_TASK_MGMT_CMD:
+		BNX2FC_IO_DBG(io_req, "Processing TM complete\n");
+		bnx2fc_process_tm_compl(io_req, task, num_rq);
+		break;
+
+	case BNX2FC_ABTS:
+		/*
+		 * ABTS request received by firmware. ABTS response
+		 * will be delivered to the task belonging to the IO
+		 * that was aborted
+		 */
+		BNX2FC_IO_DBG(io_req, "cq_compl- ABTS sent out by fw\n");
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		break;
+
+	case BNX2FC_ELS:
+		BNX2FC_IO_DBG(io_req, "cq_compl - call process_els_compl\n");
+		bnx2fc_process_els_compl(io_req, task, num_rq);
+		break;
+
+	case BNX2FC_CLEANUP:
+		BNX2FC_IO_DBG(io_req, "cq_compl- cleanup resp rcvd\n");
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		break;
+
+	default:
+		printk(KERN_ERR PFX "Invalid cmd_type %d\n", cmd_type);
+		break;
+	}
+	spin_unlock_bh(&tgt->tgt_lock);
+}
+
+struct bnx2fc_work *bnx2fc_alloc_work(struct bnx2fc_rport *tgt, u16 wqe)
+{
+	struct bnx2fc_work *work;
+	work = kzalloc(sizeof(struct bnx2fc_work), GFP_ATOMIC);
+	if (!work)
+		return NULL;
+
+	INIT_LIST_HEAD(&work->list);
+	work->tgt = tgt;
+	work->wqe = wqe;
+	return work;
+}
+
+int bnx2fc_process_new_cqes(struct bnx2fc_rport *tgt)
+{
+	struct fcoe_cqe *cq;
+	u32 cq_cons;
+	struct fcoe_cqe *cqe;
+	u16 wqe;
+	bool more_cqes_found = false;
+
+	/*
+	 * cq_lock is a low contention lock used to protect
+	 * the CQ data structure from being freed up during
+	 * the upload operation
+	 */
+	spin_lock_bh(&tgt->cq_lock);
+
+	if (!tgt->cq) {
+		printk(KERN_ERR PFX "process_new_cqes: cq is NULL\n");
+		spin_unlock_bh(&tgt->cq_lock);
+		return 0;
+	}
+	cq = tgt->cq;
+	cq_cons = tgt->cq_cons_idx;
+	cqe = &cq[cq_cons];
+
+	do {
+		more_cqes_found ^= true;
+
+		while (((wqe = cqe->wqe) & FCOE_CQE_TOGGLE_BIT) ==
+		       (tgt->cq_curr_toggle_bit <<
+		       FCOE_CQE_TOGGLE_BIT_SHIFT)) {
+
+			/* new entry on the cq */
+			if (wqe & FCOE_CQE_CQE_TYPE) {
+				/* Unsolicited event notification */
+				bnx2fc_process_unsol_compl(tgt, wqe);
+			} else {
+				struct bnx2fc_work *work = NULL;
+				struct bnx2fc_percpu_s *fps = NULL;
+				unsigned int cpu = wqe % num_possible_cpus();
+
+				fps = &per_cpu(bnx2fc_percpu, cpu);
+				spin_lock_bh(&fps->fp_work_lock);
+				if (unlikely(!fps->iothread))
+					goto unlock;
+
+				work = bnx2fc_alloc_work(tgt, wqe);
+				if (work)
+					list_add_tail(&work->list,
+							&fps->work_list);
+unlock:
+				spin_unlock_bh(&fps->fp_work_lock);
+
+				/* Pending work request completion */
+				if (fps->iothread && work)
+					wake_up_process(fps->iothread);
+				else
+					bnx2fc_process_cq_compl(tgt, wqe);
+			}
+			cqe++;
+			tgt->cq_cons_idx++;
+
+			if (tgt->cq_cons_idx == BNX2FC_CQ_WQES_MAX) {
+				tgt->cq_cons_idx = 0;
+				cqe = cq;
+				tgt->cq_curr_toggle_bit =
+					1 - tgt->cq_curr_toggle_bit;
+			}
+		}
+		/* Re-arm CQ */
+		if (more_cqes_found) {
+			tgt->conn_db->cq_arm.lo = -1;
+			wmb();
+		}
+	} while (more_cqes_found);
+
+	/*
+	 * Commit tgt->cq_cons_idx change to the memory
+	 * spin_lock implies full memory barrier, no need to smp_wmb
+	 */
+
+	spin_unlock_bh(&tgt->cq_lock);
+	return 0;
+}
+
+/**
+ * bnx2fc_fastpath_notification - process global event queue (KCQ)
+ *
+ * @hba:		adapter structure pointer
+ * @new_cqe_kcqe:	pointer to newly DMA'd KCQ entry
+ *
+ * Fast path event notification handler
+ */
+static void bnx2fc_fastpath_notification(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *new_cqe_kcqe)
+{
+	u32 conn_id = new_cqe_kcqe->fcoe_conn_id;
+	struct bnx2fc_rport *tgt = hba->tgt_ofld_list[conn_id];
+
+	if (!tgt) {
+		printk(KERN_ALERT PFX "conn_id 0x%x not valid\n", conn_id);
+		return;
+	}
+
+	bnx2fc_process_new_cqes(tgt);
+}
+
+/**
+ * bnx2fc_process_ofld_cmpl - process FCoE session offload completion
+ *
+ * @hba:	adapter structure pointer
+ * @ofld_kcqe:	connection offload kcqe pointer
+ *
+ * handle session offload completion, enable the session if offload is
+ * successful.
+ */
+static void bnx2fc_process_ofld_cmpl(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *ofld_kcqe)
+{
+	struct bnx2fc_rport		*tgt;
+	struct fcoe_port		*port;
+	u32				conn_id;
+	u32				context_id;
+	int				rc;
+
+	conn_id = ofld_kcqe->fcoe_conn_id;
+	context_id = ofld_kcqe->fcoe_conn_context_id;
+	tgt = hba->tgt_ofld_list[conn_id];
+	if (!tgt) {
+		printk(KERN_ALERT PFX "ERROR:ofld_cmpl: No pending ofld req\n");
+		return;
+	}
+	BNX2FC_TGT_DBG(tgt, "Entered ofld compl - context_id = 0x%x\n",
+		ofld_kcqe->fcoe_conn_context_id);
+	port = tgt->port;
+	if (hba != tgt->port->priv) {
+		printk(KERN_ALERT PFX "ERROR:ofld_cmpl: HBA mis-match\n");
+		goto ofld_cmpl_err;
+	}
+	/*
+	 * cnic has allocated a context_id for this session; use this
+	 * while enabling the session.
+	 */
+	tgt->context_id = context_id;
+	if (ofld_kcqe->completion_status) {
+		if (ofld_kcqe->completion_status ==
+				FCOE_KCQE_COMPLETION_STATUS_CTX_ALLOC_FAILURE) {
+			printk(KERN_ERR PFX "unable to allocate FCoE context "
+				"resources\n");
+			set_bit(BNX2FC_FLAG_CTX_ALLOC_FAILURE, &tgt->flags);
+		}
+		goto ofld_cmpl_err;
+	} else {
+
+		/* now enable the session */
+		rc = bnx2fc_send_session_enable_req(port, tgt);
+		if (rc) {
+			printk(KERN_ALERT PFX "enable session failed\n");
+			goto ofld_cmpl_err;
+		}
+	}
+	return;
+ofld_cmpl_err:
+	set_bit(BNX2FC_FLAG_OFLD_REQ_CMPL, &tgt->flags);
+	wake_up_interruptible(&tgt->ofld_wait);
+}
+
+/**
+ * bnx2fc_process_enable_conn_cmpl - process FCoE session enable completion
+ *
+ * @hba:	adapter structure pointer
+ * @ofld_kcqe:	connection offload kcqe pointer
+ *
+ * handle session enable completion, mark the rport as ready
+ */
+
+static void bnx2fc_process_enable_conn_cmpl(struct bnx2fc_hba *hba,
+						struct fcoe_kcqe *ofld_kcqe)
+{
+	struct bnx2fc_rport		*tgt;
+	u32				conn_id;
+	u32				context_id;
+
+	context_id = ofld_kcqe->fcoe_conn_context_id;
+	conn_id = ofld_kcqe->fcoe_conn_id;
+	tgt = hba->tgt_ofld_list[conn_id];
+	if (!tgt) {
+		printk(KERN_ALERT PFX "ERROR:enbl_cmpl: No pending ofld req\n");
+		return;
+	}
+
+	BNX2FC_TGT_DBG(tgt, "Enable compl - context_id = 0x%x\n",
+		ofld_kcqe->fcoe_conn_context_id);
+
+	/*
+	 * context_id should be the same for this target during offload
+	 * and enable
+	 */
+	if (tgt->context_id != context_id) {
+		printk(KERN_ALERT PFX "context id mis-match\n");
+		return;
+	}
+	if (hba != tgt->port->priv) {
+		printk(KERN_ALERT PFX "bnx2fc-enbl_cmpl: HBA mis-match\n");
+		goto enbl_cmpl_err;
+	}
+	if (ofld_kcqe->completion_status) {
+		goto enbl_cmpl_err;
+	} else {
+		/* enable successful - rport ready for issuing IOs */
+		set_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags);
+		set_bit(BNX2FC_FLAG_OFLD_REQ_CMPL, &tgt->flags);
+		wake_up_interruptible(&tgt->ofld_wait);
+	}
+	return;
+
+enbl_cmpl_err:
+	set_bit(BNX2FC_FLAG_OFLD_REQ_CMPL, &tgt->flags);
+	wake_up_interruptible(&tgt->ofld_wait);
+}
+
+static void bnx2fc_process_conn_disable_cmpl(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *disable_kcqe)
+{
+
+	struct bnx2fc_rport		*tgt;
+	u32				conn_id;
+
+	conn_id = disable_kcqe->fcoe_conn_id;
+	tgt = hba->tgt_ofld_list[conn_id];
+	if (!tgt) {
+		printk(KERN_ALERT PFX "ERROR: disable_cmpl: No disable req\n");
+		return;
+	}
+
+	BNX2FC_TGT_DBG(tgt, PFX "disable_cmpl: conn_id %d\n", conn_id);
+
+	if (disable_kcqe->completion_status) {
+		printk(KERN_ALERT PFX "ERROR: Disable failed with cmpl status %d\n",
+			disable_kcqe->completion_status);
+		return;
+	} else {
+		/* disable successful */
+		BNX2FC_TGT_DBG(tgt, "disable successful\n");
+		clear_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags);
+		set_bit(BNX2FC_FLAG_DISABLED, &tgt->flags);
+		set_bit(BNX2FC_FLAG_UPLD_REQ_COMPL, &tgt->flags);
+		wake_up_interruptible(&tgt->upld_wait);
+	}
+}
+
+static void bnx2fc_process_conn_destroy_cmpl(struct bnx2fc_hba *hba,
+					struct fcoe_kcqe *destroy_kcqe)
+{
+	struct bnx2fc_rport		*tgt;
+	u32				conn_id;
+
+	conn_id = destroy_kcqe->fcoe_conn_id;
+	tgt = hba->tgt_ofld_list[conn_id];
+	if (!tgt) {
+		printk(KERN_ALERT PFX "destroy_cmpl: No destroy req\n");
+		return;
+	}
+
+	BNX2FC_TGT_DBG(tgt, "destroy_cmpl: conn_id %d\n", conn_id);
+
+	if (destroy_kcqe->completion_status) {
+		printk(KERN_ALERT PFX "Destroy conn failed, cmpl status %d\n",
+			destroy_kcqe->completion_status);
+		return;
+	} else {
+		/* destroy successful */
+		BNX2FC_TGT_DBG(tgt, "upload successful\n");
+		clear_bit(BNX2FC_FLAG_DISABLED, &tgt->flags);
+		set_bit(BNX2FC_FLAG_DESTROYED, &tgt->flags);
+		set_bit(BNX2FC_FLAG_UPLD_REQ_COMPL, &tgt->flags);
+		wake_up_interruptible(&tgt->upld_wait);
+	}
+}
+
+static void bnx2fc_init_failure(struct bnx2fc_hba *hba, u32 err_code)
+{
+	switch (err_code) {
+	case FCOE_KCQE_COMPLETION_STATUS_INVALID_OPCODE:
+		printk(KERN_ERR PFX "init_failure due to invalid opcode\n");
+		break;
+
+	case FCOE_KCQE_COMPLETION_STATUS_CTX_ALLOC_FAILURE:
+		printk(KERN_ERR PFX "init failed due to ctx alloc failure\n");
+		break;
+
+	case FCOE_KCQE_COMPLETION_STATUS_NIC_ERROR:
+		printk(KERN_ERR PFX "init_failure due to NIC error\n");
+		break;
+
+	default:
+		printk(KERN_ERR PFX "Unknown Error code %d\n", err_code);
+	}
+}
+
+/**
+ * bnx2fc_indicae_kcqe - process KCQE
+ *
+ * @hba:	adapter structure pointer
+ * @kcqe:	kcqe pointer
+ * @num_cqe:	Number of completion queue elements
+ *
+ * Generic KCQ event handler
+ */
+void bnx2fc_indicate_kcqe(void *context, struct kcqe *kcq[],
+					u32 num_cqe)
+{
+	struct bnx2fc_hba *hba = (struct bnx2fc_hba *)context;
+	int i = 0;
+	struct fcoe_kcqe *kcqe = NULL;
+
+	while (i < num_cqe) {
+		kcqe = (struct fcoe_kcqe *) kcq[i++];
+
+		switch (kcqe->op_code) {
+		case FCOE_KCQE_OPCODE_CQ_EVENT_NOTIFICATION:
+			bnx2fc_fastpath_notification(hba, kcqe);
+			break;
+
+		case FCOE_KCQE_OPCODE_OFFLOAD_CONN:
+			bnx2fc_process_ofld_cmpl(hba, kcqe);
+			break;
+
+		case FCOE_KCQE_OPCODE_ENABLE_CONN:
+			bnx2fc_process_enable_conn_cmpl(hba, kcqe);
+			break;
+
+		case FCOE_KCQE_OPCODE_INIT_FUNC:
+			if (kcqe->completion_status !=
+					FCOE_KCQE_COMPLETION_STATUS_SUCCESS) {
+				bnx2fc_init_failure(hba,
+						kcqe->completion_status);
+			} else {
+				set_bit(ADAPTER_STATE_UP, &hba->adapter_state);
+				bnx2fc_get_link_state(hba);
+				printk(KERN_INFO PFX "[%.2x]: FCOE_INIT passed\n",
+					(u8)hba->pcidev->bus->number);
+			}
+			break;
+
+		case FCOE_KCQE_OPCODE_DESTROY_FUNC:
+			if (kcqe->completion_status !=
+					FCOE_KCQE_COMPLETION_STATUS_SUCCESS) {
+
+				printk(KERN_ERR PFX "DESTROY failed\n");
+			} else {
+				printk(KERN_ERR PFX "DESTROY success\n");
+			}
+			hba->flags |= BNX2FC_FLAG_DESTROY_CMPL;
+			wake_up_interruptible(&hba->destroy_wait);
+			break;
+
+		case FCOE_KCQE_OPCODE_DISABLE_CONN:
+			bnx2fc_process_conn_disable_cmpl(hba, kcqe);
+			break;
+
+		case FCOE_KCQE_OPCODE_DESTROY_CONN:
+			bnx2fc_process_conn_destroy_cmpl(hba, kcqe);
+			break;
+
+		case FCOE_KCQE_OPCODE_STAT_FUNC:
+			if (kcqe->completion_status !=
+			    FCOE_KCQE_COMPLETION_STATUS_SUCCESS)
+				printk(KERN_ERR PFX "STAT failed\n");
+			complete(&hba->stat_req_done);
+			break;
+
+		case FCOE_KCQE_OPCODE_FCOE_ERROR:
+			/* fall thru */
+		default:
+			printk(KERN_ALERT PFX "unknown opcode 0x%x\n",
+								kcqe->op_code);
+		}
+	}
+}
+
+void bnx2fc_add_2_sq(struct bnx2fc_rport *tgt, u16 xid)
+{
+	struct fcoe_sqe *sqe;
+
+	sqe = &tgt->sq[tgt->sq_prod_idx];
+
+	/* Fill SQ WQE */
+	sqe->wqe = xid << FCOE_SQE_TASK_ID_SHIFT;
+	sqe->wqe |= tgt->sq_curr_toggle_bit << FCOE_SQE_TOGGLE_BIT_SHIFT;
+
+	/* Advance SQ Prod Idx */
+	if (++tgt->sq_prod_idx == BNX2FC_SQ_WQES_MAX) {
+		tgt->sq_prod_idx = 0;
+		tgt->sq_curr_toggle_bit = 1 - tgt->sq_curr_toggle_bit;
+	}
+}
+
+void bnx2fc_ring_doorbell(struct bnx2fc_rport *tgt)
+{
+	struct b577xx_doorbell_set_prod ev_doorbell;
+	u32 msg;
+
+	wmb();
+
+	memset(&ev_doorbell, 0, sizeof(struct b577xx_doorbell_set_prod));
+	ev_doorbell.header.header = B577XX_DOORBELL_HDR_DB_TYPE;
+
+	ev_doorbell.prod = tgt->sq_prod_idx |
+				(tgt->sq_curr_toggle_bit << 15);
+	ev_doorbell.header.header |= B577XX_FCOE_CONNECTION_TYPE <<
+					B577XX_DOORBELL_HDR_CONN_TYPE_SHIFT;
+	msg = *((u32 *)&ev_doorbell);
+	writel(cpu_to_le32(msg), tgt->ctx_base);
+
+	mmiowb();
+
+}
+
+int bnx2fc_map_doorbell(struct bnx2fc_rport *tgt)
+{
+	u32 context_id = tgt->context_id;
+	struct fcoe_port *port = tgt->port;
+	u32 reg_off;
+	resource_size_t reg_base;
+	struct bnx2fc_hba *hba = port->priv;
+
+	reg_base = pci_resource_start(hba->pcidev,
+					BNX2X_DOORBELL_PCI_BAR);
+	reg_off = BNX2FC_5771X_DB_PAGE_SIZE *
+			(context_id & 0x1FFFF) + DPM_TRIGER_TYPE;
+	tgt->ctx_base = ioremap_nocache(reg_base + reg_off, 4);
+	if (!tgt->ctx_base)
+		return -ENOMEM;
+	return 0;
+}
+
+char *bnx2fc_get_next_rqe(struct bnx2fc_rport *tgt, u8 num_items)
+{
+	char *buf = (char *)tgt->rq + (tgt->rq_cons_idx * BNX2FC_RQ_BUF_SZ);
+
+	if (tgt->rq_cons_idx + num_items > BNX2FC_RQ_WQES_MAX)
+		return NULL;
+
+	tgt->rq_cons_idx += num_items;
+
+	if (tgt->rq_cons_idx >= BNX2FC_RQ_WQES_MAX)
+		tgt->rq_cons_idx -= BNX2FC_RQ_WQES_MAX;
+
+	return buf;
+}
+
+void bnx2fc_return_rqe(struct bnx2fc_rport *tgt, u8 num_items)
+{
+	/* return the rq buffer */
+	u32 next_prod_idx = tgt->rq_prod_idx + num_items;
+	if ((next_prod_idx & 0x7fff) == BNX2FC_RQ_WQES_MAX) {
+		/* Wrap around RQ */
+		next_prod_idx += 0x8000 - BNX2FC_RQ_WQES_MAX;
+	}
+	tgt->rq_prod_idx = next_prod_idx;
+	tgt->conn_db->rq_prod = tgt->rq_prod_idx;
+}
+
+void bnx2fc_init_cleanup_task(struct bnx2fc_cmd *io_req,
+			      struct fcoe_task_ctx_entry *task,
+			      u16 orig_xid)
+{
+	u8 task_type = FCOE_TASK_TYPE_EXCHANGE_CLEANUP;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	u32 context_id = tgt->context_id;
+
+	memset(task, 0, sizeof(struct fcoe_task_ctx_entry));
+
+	/* Tx Write Rx Read */
+	task->tx_wr_rx_rd.tx_flags = FCOE_TASK_TX_STATE_EXCHANGE_CLEANUP <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE_SHIFT;
+	task->tx_wr_rx_rd.init_flags = task_type <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE_SHIFT;
+	task->tx_wr_rx_rd.init_flags |= FCOE_TASK_CLASS_TYPE_3 <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE_SHIFT;
+	/* Common */
+	task->cmn.common_flags = context_id <<
+				FCOE_TASK_CTX_ENTRY_TX_RX_CMN_CID_SHIFT;
+	task->cmn.general.cleanup_info.task_id = orig_xid;
+
+
+}
+
+void bnx2fc_init_mp_task(struct bnx2fc_cmd *io_req,
+				struct fcoe_task_ctx_entry *task)
+{
+	struct bnx2fc_mp_req *mp_req = &(io_req->mp_req);
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	struct fc_frame_header *fc_hdr;
+	u8 task_type = 0;
+	u64 *hdr;
+	u64 temp_hdr[3];
+	u32 context_id;
+
+
+	/* Obtain task_type */
+	if ((io_req->cmd_type == BNX2FC_TASK_MGMT_CMD) ||
+	    (io_req->cmd_type == BNX2FC_ELS)) {
+		task_type = FCOE_TASK_TYPE_MIDPATH;
+	} else if (io_req->cmd_type == BNX2FC_ABTS) {
+		task_type = FCOE_TASK_TYPE_ABTS;
+	}
+
+	memset(task, 0, sizeof(struct fcoe_task_ctx_entry));
+
+	/* Setup the task from io_req for easy reference */
+	io_req->task = task;
+
+	BNX2FC_IO_DBG(io_req, "Init MP task for cmd_type = %d task_type = %d\n",
+		io_req->cmd_type, task_type);
+
+	/* Tx only */
+	if ((task_type == FCOE_TASK_TYPE_MIDPATH) ||
+	    (task_type == FCOE_TASK_TYPE_UNSOLICITED)) {
+		task->tx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.lo =
+				(u32)mp_req->mp_req_bd_dma;
+		task->tx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.hi =
+				(u32)((u64)mp_req->mp_req_bd_dma >> 32);
+		task->tx_wr_only.sgl_ctx.mul_sges.sgl_size = 1;
+		BNX2FC_IO_DBG(io_req, "init_mp_task - bd_dma = 0x%llx\n",
+			      (unsigned long long)mp_req->mp_req_bd_dma);
+	}
+
+	/* Tx Write Rx Read */
+	task->tx_wr_rx_rd.tx_flags = FCOE_TASK_TX_STATE_INIT <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE_SHIFT;
+	task->tx_wr_rx_rd.init_flags = task_type <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE_SHIFT;
+	task->tx_wr_rx_rd.init_flags |= FCOE_TASK_DEV_TYPE_DISK <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_DEV_TYPE_SHIFT;
+	task->tx_wr_rx_rd.init_flags |= FCOE_TASK_CLASS_TYPE_3 <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE_SHIFT;
+
+	/* Common */
+	task->cmn.data_2_trns = io_req->data_xfer_len;
+	context_id = tgt->context_id;
+	task->cmn.common_flags = context_id <<
+				FCOE_TASK_CTX_ENTRY_TX_RX_CMN_CID_SHIFT;
+	task->cmn.common_flags |= 1 <<
+				FCOE_TASK_CTX_ENTRY_TX_RX_CMN_VALID_SHIFT;
+	task->cmn.common_flags |= 1 <<
+			FCOE_TASK_CTX_ENTRY_TX_RX_CMN_EXP_FIRST_FRAME_SHIFT;
+
+	/* Rx Write Tx Read */
+	fc_hdr = &(mp_req->req_fc_hdr);
+	if (task_type == FCOE_TASK_TYPE_MIDPATH) {
+		fc_hdr->fh_ox_id = cpu_to_be16(io_req->xid);
+		fc_hdr->fh_rx_id = htons(0xffff);
+		task->rx_wr_tx_rd.rx_id = 0xffff;
+	} else if (task_type == FCOE_TASK_TYPE_UNSOLICITED) {
+		fc_hdr->fh_rx_id = cpu_to_be16(io_req->xid);
+	}
+
+	/* Fill FC Header into middle path buffer */
+	hdr = (u64 *) &task->cmn.general.cmd_info.mp_fc_frame.fc_hdr;
+	memcpy(temp_hdr, fc_hdr, sizeof(temp_hdr));
+	hdr[0] = cpu_to_be64(temp_hdr[0]);
+	hdr[1] = cpu_to_be64(temp_hdr[1]);
+	hdr[2] = cpu_to_be64(temp_hdr[2]);
+
+	/* Rx Only */
+	if (task_type == FCOE_TASK_TYPE_MIDPATH) {
+
+		task->rx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.lo =
+				(u32)mp_req->mp_resp_bd_dma;
+		task->rx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.hi =
+				(u32)((u64)mp_req->mp_resp_bd_dma >> 32);
+		task->rx_wr_only.sgl_ctx.mul_sges.sgl_size = 1;
+	}
+}
+
+void bnx2fc_init_task(struct bnx2fc_cmd *io_req,
+			     struct fcoe_task_ctx_entry *task)
+{
+	u8 task_type;
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+	struct io_bdt *bd_tbl = io_req->bd_tbl;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	u64 *fcp_cmnd;
+	u64 tmp_fcp_cmnd[4];
+	u32 context_id;
+	int cnt, i;
+	int bd_count;
+
+	memset(task, 0, sizeof(struct fcoe_task_ctx_entry));
+
+	/* Setup the task from io_req for easy reference */
+	io_req->task = task;
+
+	if (sc_cmd->sc_data_direction == DMA_TO_DEVICE)
+		task_type = FCOE_TASK_TYPE_WRITE;
+	else
+		task_type = FCOE_TASK_TYPE_READ;
+
+	/* Tx only */
+	if (task_type == FCOE_TASK_TYPE_WRITE) {
+		task->tx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.lo =
+				(u32)bd_tbl->bd_tbl_dma;
+		task->tx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.hi =
+				(u32)((u64)bd_tbl->bd_tbl_dma >> 32);
+		task->tx_wr_only.sgl_ctx.mul_sges.sgl_size =
+				bd_tbl->bd_valid;
+	}
+
+	/*Tx Write Rx Read */
+	/* Init state to NORMAL */
+	task->tx_wr_rx_rd.tx_flags = FCOE_TASK_TX_STATE_NORMAL <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TX_STATE_SHIFT;
+	task->tx_wr_rx_rd.init_flags = task_type <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_TASK_TYPE_SHIFT;
+	task->tx_wr_rx_rd.init_flags |= FCOE_TASK_DEV_TYPE_DISK <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_DEV_TYPE_SHIFT;
+	task->tx_wr_rx_rd.init_flags |= FCOE_TASK_CLASS_TYPE_3 <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_CLASS_TYPE_SHIFT;
+
+	/* Common */
+	task->cmn.data_2_trns = io_req->data_xfer_len;
+	context_id = tgt->context_id;
+	task->cmn.common_flags = context_id <<
+				FCOE_TASK_CTX_ENTRY_TX_RX_CMN_CID_SHIFT;
+	task->cmn.common_flags |= 1 <<
+				FCOE_TASK_CTX_ENTRY_TX_RX_CMN_VALID_SHIFT;
+	task->cmn.common_flags |= 1 <<
+			FCOE_TASK_CTX_ENTRY_TX_RX_CMN_EXP_FIRST_FRAME_SHIFT;
+
+	/* Set initiative ownership */
+	task->cmn.common_flags |= FCOE_TASK_CTX_ENTRY_TX_RX_CMN_SEQ_INIT;
+
+	/* Set initial seq counter */
+	task->cmn.tx_low_seq_cnt = 1;
+
+	/* Set state to "waiting for the first packet" */
+	task->cmn.common_flags |= FCOE_TASK_CTX_ENTRY_TX_RX_CMN_EXP_FIRST_FRAME;
+
+	/* Fill FCP_CMND IU */
+	fcp_cmnd = (u64 *)
+		    task->cmn.general.cmd_info.fcp_cmd_payload.opaque;
+	bnx2fc_build_fcp_cmnd(io_req, (struct fcp_cmnd *)&tmp_fcp_cmnd);
+
+	/* swap fcp_cmnd */
+	cnt = sizeof(struct fcp_cmnd) / sizeof(u64);
+
+	for (i = 0; i < cnt; i++) {
+		*fcp_cmnd = cpu_to_be64(tmp_fcp_cmnd[i]);
+		fcp_cmnd++;
+	}
+
+	/* Rx Write Tx Read */
+	task->rx_wr_tx_rd.rx_id = 0xffff;
+
+	/* Rx Only */
+	if (task_type == FCOE_TASK_TYPE_READ) {
+
+		bd_count = bd_tbl->bd_valid;
+		if (bd_count == 1) {
+
+			struct fcoe_bd_ctx *fcoe_bd_tbl = bd_tbl->bd_tbl;
+
+			task->rx_wr_only.sgl_ctx.single_sge.cur_buf_addr.lo =
+					fcoe_bd_tbl->buf_addr_lo;
+			task->rx_wr_only.sgl_ctx.single_sge.cur_buf_addr.hi =
+					fcoe_bd_tbl->buf_addr_hi;
+			task->rx_wr_only.sgl_ctx.single_sge.cur_buf_rem =
+					fcoe_bd_tbl->buf_len;
+			task->tx_wr_rx_rd.init_flags |= 1 <<
+				FCOE_TASK_CTX_ENTRY_TXWR_RXRD_SINGLE_SGE_SHIFT;
+		} else {
+
+			task->rx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.lo =
+					(u32)bd_tbl->bd_tbl_dma;
+			task->rx_wr_only.sgl_ctx.mul_sges.cur_sge_addr.hi =
+					(u32)((u64)bd_tbl->bd_tbl_dma >> 32);
+			task->rx_wr_only.sgl_ctx.mul_sges.sgl_size =
+					bd_tbl->bd_valid;
+		}
+	}
+}
+
+/**
+ * bnx2fc_setup_task_ctx - allocate and map task context
+ *
+ * @hba:	pointer to adapter structure
+ *
+ * allocate memory for task context, and associated BD table to be used
+ * by firmware
+ *
+ */
+int bnx2fc_setup_task_ctx(struct bnx2fc_hba *hba)
+{
+	int rc = 0;
+	struct regpair *task_ctx_bdt;
+	dma_addr_t addr;
+	int i;
+
+	/*
+	 * Allocate task context bd table. A page size of bd table
+	 * can map 256 buffers. Each buffer contains 32 task context
+	 * entries. Hence the limit with one page is 8192 task context
+	 * entries.
+	 */
+	hba->task_ctx_bd_tbl = dma_alloc_coherent(&hba->pcidev->dev,
+						  PAGE_SIZE,
+						  &hba->task_ctx_bd_dma,
+						  GFP_KERNEL);
+	if (!hba->task_ctx_bd_tbl) {
+		printk(KERN_ERR PFX "unable to allocate task context BDT\n");
+		rc = -1;
+		goto out;
+	}
+	memset(hba->task_ctx_bd_tbl, 0, PAGE_SIZE);
+
+	/*
+	 * Allocate task_ctx which is an array of pointers pointing to
+	 * a page containing 32 task contexts
+	 */
+	hba->task_ctx = kzalloc((BNX2FC_TASK_CTX_ARR_SZ * sizeof(void *)),
+				 GFP_KERNEL);
+	if (!hba->task_ctx) {
+		printk(KERN_ERR PFX "unable to allocate task context array\n");
+		rc = -1;
+		goto out1;
+	}
+
+	/*
+	 * Allocate task_ctx_dma which is an array of dma addresses
+	 */
+	hba->task_ctx_dma = kmalloc((BNX2FC_TASK_CTX_ARR_SZ *
+					sizeof(dma_addr_t)), GFP_KERNEL);
+	if (!hba->task_ctx_dma) {
+		printk(KERN_ERR PFX "unable to alloc context mapping array\n");
+		rc = -1;
+		goto out2;
+	}
+
+	task_ctx_bdt = (struct regpair *)hba->task_ctx_bd_tbl;
+	for (i = 0; i < BNX2FC_TASK_CTX_ARR_SZ; i++) {
+
+		hba->task_ctx[i] = dma_alloc_coherent(&hba->pcidev->dev,
+						      PAGE_SIZE,
+						      &hba->task_ctx_dma[i],
+						      GFP_KERNEL);
+		if (!hba->task_ctx[i]) {
+			printk(KERN_ERR PFX "unable to alloc task context\n");
+			rc = -1;
+			goto out3;
+		}
+		memset(hba->task_ctx[i], 0, PAGE_SIZE);
+		addr = (u64)hba->task_ctx_dma[i];
+		task_ctx_bdt->hi = cpu_to_le32((u64)addr >> 32);
+		task_ctx_bdt->lo = cpu_to_le32((u32)addr);
+		task_ctx_bdt++;
+	}
+	return 0;
+
+out3:
+	for (i = 0; i < BNX2FC_TASK_CTX_ARR_SZ; i++) {
+		if (hba->task_ctx[i]) {
+
+			dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				hba->task_ctx[i], hba->task_ctx_dma[i]);
+			hba->task_ctx[i] = NULL;
+		}
+	}
+
+	kfree(hba->task_ctx_dma);
+	hba->task_ctx_dma = NULL;
+out2:
+	kfree(hba->task_ctx);
+	hba->task_ctx = NULL;
+out1:
+	dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+			hba->task_ctx_bd_tbl, hba->task_ctx_bd_dma);
+	hba->task_ctx_bd_tbl = NULL;
+out:
+	return rc;
+}
+
+void bnx2fc_free_task_ctx(struct bnx2fc_hba *hba)
+{
+	int i;
+
+	if (hba->task_ctx_bd_tbl) {
+		dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				    hba->task_ctx_bd_tbl,
+				    hba->task_ctx_bd_dma);
+		hba->task_ctx_bd_tbl = NULL;
+	}
+
+	if (hba->task_ctx) {
+		for (i = 0; i < BNX2FC_TASK_CTX_ARR_SZ; i++) {
+			if (hba->task_ctx[i]) {
+				dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+						    hba->task_ctx[i],
+						    hba->task_ctx_dma[i]);
+				hba->task_ctx[i] = NULL;
+			}
+		}
+		kfree(hba->task_ctx);
+		hba->task_ctx = NULL;
+	}
+
+	kfree(hba->task_ctx_dma);
+	hba->task_ctx_dma = NULL;
+}
+
+static void bnx2fc_free_hash_table(struct bnx2fc_hba *hba)
+{
+	int i;
+	int segment_count;
+	int hash_table_size;
+	u32 *pbl;
+
+	segment_count = hba->hash_tbl_segment_count;
+	hash_table_size = BNX2FC_NUM_MAX_SESS * BNX2FC_MAX_ROWS_IN_HASH_TBL *
+		sizeof(struct fcoe_hash_table_entry);
+
+	pbl = hba->hash_tbl_pbl;
+	for (i = 0; i < segment_count; ++i) {
+		dma_addr_t dma_address;
+
+		dma_address = le32_to_cpu(*pbl);
+		++pbl;
+		dma_address += ((u64)le32_to_cpu(*pbl)) << 32;
+		++pbl;
+		dma_free_coherent(&hba->pcidev->dev,
+				  BNX2FC_HASH_TBL_CHUNK_SIZE,
+				  hba->hash_tbl_segments[i],
+				  dma_address);
+
+	}
+
+	if (hba->hash_tbl_pbl) {
+		dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				    hba->hash_tbl_pbl,
+				    hba->hash_tbl_pbl_dma);
+		hba->hash_tbl_pbl = NULL;
+	}
+}
+
+static int bnx2fc_allocate_hash_table(struct bnx2fc_hba *hba)
+{
+	int i;
+	int hash_table_size;
+	int segment_count;
+	int segment_array_size;
+	int dma_segment_array_size;
+	dma_addr_t *dma_segment_array;
+	u32 *pbl;
+
+	hash_table_size = BNX2FC_NUM_MAX_SESS * BNX2FC_MAX_ROWS_IN_HASH_TBL *
+		sizeof(struct fcoe_hash_table_entry);
+
+	segment_count = hash_table_size + BNX2FC_HASH_TBL_CHUNK_SIZE - 1;
+	segment_count /= BNX2FC_HASH_TBL_CHUNK_SIZE;
+	hba->hash_tbl_segment_count = segment_count;
+
+	segment_array_size = segment_count * sizeof(*hba->hash_tbl_segments);
+	hba->hash_tbl_segments = kzalloc(segment_array_size, GFP_KERNEL);
+	if (!hba->hash_tbl_segments) {
+		printk(KERN_ERR PFX "hash table pointers alloc failed\n");
+		return -ENOMEM;
+	}
+	dma_segment_array_size = segment_count * sizeof(*dma_segment_array);
+	dma_segment_array = kzalloc(dma_segment_array_size, GFP_KERNEL);
+	if (!dma_segment_array) {
+		printk(KERN_ERR PFX "hash table pointers (dma) alloc failed\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < segment_count; ++i) {
+		hba->hash_tbl_segments[i] =
+			dma_alloc_coherent(&hba->pcidev->dev,
+					   BNX2FC_HASH_TBL_CHUNK_SIZE,
+					   &dma_segment_array[i],
+					   GFP_KERNEL);
+		if (!hba->hash_tbl_segments[i]) {
+			printk(KERN_ERR PFX "hash segment alloc failed\n");
+			while (--i >= 0) {
+				dma_free_coherent(&hba->pcidev->dev,
+						    BNX2FC_HASH_TBL_CHUNK_SIZE,
+						    hba->hash_tbl_segments[i],
+						    dma_segment_array[i]);
+				hba->hash_tbl_segments[i] = NULL;
+			}
+			kfree(dma_segment_array);
+			return -ENOMEM;
+		}
+		memset(hba->hash_tbl_segments[i], 0,
+		       BNX2FC_HASH_TBL_CHUNK_SIZE);
+	}
+
+	hba->hash_tbl_pbl = dma_alloc_coherent(&hba->pcidev->dev,
+					       PAGE_SIZE,
+					       &hba->hash_tbl_pbl_dma,
+					       GFP_KERNEL);
+	if (!hba->hash_tbl_pbl) {
+		printk(KERN_ERR PFX "hash table pbl alloc failed\n");
+		kfree(dma_segment_array);
+		return -ENOMEM;
+	}
+	memset(hba->hash_tbl_pbl, 0, PAGE_SIZE);
+
+	pbl = hba->hash_tbl_pbl;
+	for (i = 0; i < segment_count; ++i) {
+		u64 paddr = dma_segment_array[i];
+		*pbl = cpu_to_le32((u32) paddr);
+		++pbl;
+		*pbl = cpu_to_le32((u32) (paddr >> 32));
+		++pbl;
+	}
+	pbl = hba->hash_tbl_pbl;
+	i = 0;
+	while (*pbl && *(pbl + 1)) {
+		u32 lo;
+		u32 hi;
+		lo = *pbl;
+		++pbl;
+		hi = *pbl;
+		++pbl;
+		++i;
+	}
+	kfree(dma_segment_array);
+	return 0;
+}
+
+/**
+ * bnx2fc_setup_fw_resc - Allocate and map hash table and dummy buffer
+ *
+ * @hba:	Pointer to adapter structure
+ *
+ */
+int bnx2fc_setup_fw_resc(struct bnx2fc_hba *hba)
+{
+	u64 addr;
+	u32 mem_size;
+	int i;
+
+	if (bnx2fc_allocate_hash_table(hba))
+		return -ENOMEM;
+
+	mem_size = BNX2FC_NUM_MAX_SESS * sizeof(struct regpair);
+	hba->t2_hash_tbl_ptr = dma_alloc_coherent(&hba->pcidev->dev, mem_size,
+						  &hba->t2_hash_tbl_ptr_dma,
+						  GFP_KERNEL);
+	if (!hba->t2_hash_tbl_ptr) {
+		printk(KERN_ERR PFX "unable to allocate t2 hash table ptr\n");
+		bnx2fc_free_fw_resc(hba);
+		return -ENOMEM;
+	}
+	memset(hba->t2_hash_tbl_ptr, 0x00, mem_size);
+
+	mem_size = BNX2FC_NUM_MAX_SESS *
+				sizeof(struct fcoe_t2_hash_table_entry);
+	hba->t2_hash_tbl = dma_alloc_coherent(&hba->pcidev->dev, mem_size,
+					      &hba->t2_hash_tbl_dma,
+					      GFP_KERNEL);
+	if (!hba->t2_hash_tbl) {
+		printk(KERN_ERR PFX "unable to allocate t2 hash table\n");
+		bnx2fc_free_fw_resc(hba);
+		return -ENOMEM;
+	}
+	memset(hba->t2_hash_tbl, 0x00, mem_size);
+	for (i = 0; i < BNX2FC_NUM_MAX_SESS; i++) {
+		addr = (unsigned long) hba->t2_hash_tbl_dma +
+			 ((i+1) * sizeof(struct fcoe_t2_hash_table_entry));
+		hba->t2_hash_tbl[i].next.lo = addr & 0xffffffff;
+		hba->t2_hash_tbl[i].next.hi = addr >> 32;
+	}
+
+	hba->dummy_buffer = dma_alloc_coherent(&hba->pcidev->dev,
+					       PAGE_SIZE, &hba->dummy_buf_dma,
+					       GFP_KERNEL);
+	if (!hba->dummy_buffer) {
+		printk(KERN_ERR PFX "unable to alloc MP Dummy Buffer\n");
+		bnx2fc_free_fw_resc(hba);
+		return -ENOMEM;
+	}
+
+	hba->stats_buffer = dma_alloc_coherent(&hba->pcidev->dev,
+					       PAGE_SIZE,
+					       &hba->stats_buf_dma,
+					       GFP_KERNEL);
+	if (!hba->stats_buffer) {
+		printk(KERN_ERR PFX "unable to alloc Stats Buffer\n");
+		bnx2fc_free_fw_resc(hba);
+		return -ENOMEM;
+	}
+	memset(hba->stats_buffer, 0x00, PAGE_SIZE);
+
+	return 0;
+}
+
+void bnx2fc_free_fw_resc(struct bnx2fc_hba *hba)
+{
+	u32 mem_size;
+
+	if (hba->stats_buffer) {
+		dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				  hba->stats_buffer, hba->stats_buf_dma);
+		hba->stats_buffer = NULL;
+	}
+
+	if (hba->dummy_buffer) {
+		dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				  hba->dummy_buffer, hba->dummy_buf_dma);
+		hba->dummy_buffer = NULL;
+	}
+
+	if (hba->t2_hash_tbl_ptr) {
+		mem_size = BNX2FC_NUM_MAX_SESS * sizeof(struct regpair);
+		dma_free_coherent(&hba->pcidev->dev, mem_size,
+				    hba->t2_hash_tbl_ptr,
+				    hba->t2_hash_tbl_ptr_dma);
+		hba->t2_hash_tbl_ptr = NULL;
+	}
+
+	if (hba->t2_hash_tbl) {
+		mem_size = BNX2FC_NUM_MAX_SESS *
+			    sizeof(struct fcoe_t2_hash_table_entry);
+		dma_free_coherent(&hba->pcidev->dev, mem_size,
+				    hba->t2_hash_tbl, hba->t2_hash_tbl_dma);
+		hba->t2_hash_tbl = NULL;
+	}
+	bnx2fc_free_hash_table(hba);
+}
diff --git a/drivers/scsi/bnx2fc/bnx2fc_io.c b/drivers/scsi/bnx2fc/bnx2fc_io.c
new file mode 100644
index 000000000000..0f1dd23730db
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_io.c
@@ -0,0 +1,1833 @@
+/* bnx2fc_io.c: Broadcom NetXtreme II Linux FCoE offload driver.
+ * IO manager and SCSI IO processing.
+ *
+ * Copyright (c) 2008 - 2010 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.
+ *
+ * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
+ */
+
+#include "bnx2fc.h"
+static int bnx2fc_split_bd(struct bnx2fc_cmd *io_req, u64 addr, int sg_len,
+			   int bd_index);
+static int bnx2fc_map_sg(struct bnx2fc_cmd *io_req);
+static void bnx2fc_build_bd_list_from_sg(struct bnx2fc_cmd *io_req);
+static int bnx2fc_post_io_req(struct bnx2fc_rport *tgt,
+			       struct bnx2fc_cmd *io_req);
+static void bnx2fc_unmap_sg_list(struct bnx2fc_cmd *io_req);
+static void bnx2fc_free_mp_resc(struct bnx2fc_cmd *io_req);
+static void bnx2fc_parse_fcp_rsp(struct bnx2fc_cmd *io_req,
+				 struct fcoe_fcp_rsp_payload *fcp_rsp,
+				 u8 num_rq);
+
+void bnx2fc_cmd_timer_set(struct bnx2fc_cmd *io_req,
+			  unsigned int timer_msec)
+{
+	struct bnx2fc_hba *hba = io_req->port->priv;
+
+	if (queue_delayed_work(hba->timer_work_queue, &io_req->timeout_work,
+				  msecs_to_jiffies(timer_msec)))
+		kref_get(&io_req->refcount);
+}
+
+static void bnx2fc_cmd_timeout(struct work_struct *work)
+{
+	struct bnx2fc_cmd *io_req = container_of(work, struct bnx2fc_cmd,
+						 timeout_work.work);
+	struct fc_lport *lport;
+	struct fc_rport_priv *rdata;
+	u8 cmd_type = io_req->cmd_type;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	int logo_issued;
+	int rc;
+
+	BNX2FC_IO_DBG(io_req, "cmd_timeout, cmd_type = %d,"
+		      "req_flags = %lx\n", cmd_type, io_req->req_flags);
+
+	spin_lock_bh(&tgt->tgt_lock);
+	if (test_and_clear_bit(BNX2FC_FLAG_ISSUE_RRQ, &io_req->req_flags)) {
+		clear_bit(BNX2FC_FLAG_RETIRE_OXID, &io_req->req_flags);
+		/*
+		 * ideally we should hold the io_req until RRQ complets,
+		 * and release io_req from timeout hold.
+		 */
+		spin_unlock_bh(&tgt->tgt_lock);
+		bnx2fc_send_rrq(io_req);
+		return;
+	}
+	if (test_and_clear_bit(BNX2FC_FLAG_RETIRE_OXID, &io_req->req_flags)) {
+		BNX2FC_IO_DBG(io_req, "IO ready for reuse now\n");
+		goto done;
+	}
+
+	switch (cmd_type) {
+	case BNX2FC_SCSI_CMD:
+		if (test_and_clear_bit(BNX2FC_FLAG_EH_ABORT,
+							&io_req->req_flags)) {
+			/* Handle eh_abort timeout */
+			BNX2FC_IO_DBG(io_req, "eh_abort timed out\n");
+			complete(&io_req->tm_done);
+		} else if (test_bit(BNX2FC_FLAG_ISSUE_ABTS,
+				    &io_req->req_flags)) {
+			/* Handle internally generated ABTS timeout */
+			BNX2FC_IO_DBG(io_req, "ABTS timed out refcnt = %d\n",
+					io_req->refcount.refcount.counter);
+			if (!(test_and_set_bit(BNX2FC_FLAG_ABTS_DONE,
+					       &io_req->req_flags))) {
+
+				lport = io_req->port->lport;
+				rdata = io_req->tgt->rdata;
+				logo_issued = test_and_set_bit(
+						BNX2FC_FLAG_EXPL_LOGO,
+						&tgt->flags);
+				kref_put(&io_req->refcount, bnx2fc_cmd_release);
+				spin_unlock_bh(&tgt->tgt_lock);
+
+				/* Explicitly logo the target */
+				if (!logo_issued) {
+					BNX2FC_IO_DBG(io_req, "Explicit "
+						   "logo - tgt flags = 0x%lx\n",
+						   tgt->flags);
+
+					mutex_lock(&lport->disc.disc_mutex);
+					lport->tt.rport_logoff(rdata);
+					mutex_unlock(&lport->disc.disc_mutex);
+				}
+				return;
+			}
+		} else {
+			/* Hanlde IO timeout */
+			BNX2FC_IO_DBG(io_req, "IO timed out. issue ABTS\n");
+			if (test_and_set_bit(BNX2FC_FLAG_IO_COMPL,
+					     &io_req->req_flags)) {
+				BNX2FC_IO_DBG(io_req, "IO completed before "
+							   " timer expiry\n");
+				goto done;
+			}
+
+			if (!test_and_set_bit(BNX2FC_FLAG_ISSUE_ABTS,
+					      &io_req->req_flags)) {
+				rc = bnx2fc_initiate_abts(io_req);
+				if (rc == SUCCESS)
+					goto done;
+				/*
+				 * Explicitly logo the target if
+				 * abts initiation fails
+				 */
+				lport = io_req->port->lport;
+				rdata = io_req->tgt->rdata;
+				logo_issued = test_and_set_bit(
+						BNX2FC_FLAG_EXPL_LOGO,
+						&tgt->flags);
+				kref_put(&io_req->refcount, bnx2fc_cmd_release);
+				spin_unlock_bh(&tgt->tgt_lock);
+
+				if (!logo_issued) {
+					BNX2FC_IO_DBG(io_req, "Explicit "
+						   "logo - tgt flags = 0x%lx\n",
+						   tgt->flags);
+
+
+					mutex_lock(&lport->disc.disc_mutex);
+					lport->tt.rport_logoff(rdata);
+					mutex_unlock(&lport->disc.disc_mutex);
+				}
+				return;
+			} else {
+				BNX2FC_IO_DBG(io_req, "IO already in "
+						      "ABTS processing\n");
+			}
+		}
+		break;
+	case BNX2FC_ELS:
+
+		if (test_bit(BNX2FC_FLAG_ISSUE_ABTS, &io_req->req_flags)) {
+			BNX2FC_IO_DBG(io_req, "ABTS for ELS timed out\n");
+
+			if (!test_and_set_bit(BNX2FC_FLAG_ABTS_DONE,
+					      &io_req->req_flags)) {
+				lport = io_req->port->lport;
+				rdata = io_req->tgt->rdata;
+				logo_issued = test_and_set_bit(
+						BNX2FC_FLAG_EXPL_LOGO,
+						&tgt->flags);
+				kref_put(&io_req->refcount, bnx2fc_cmd_release);
+				spin_unlock_bh(&tgt->tgt_lock);
+
+				/* Explicitly logo the target */
+				if (!logo_issued) {
+					BNX2FC_IO_DBG(io_req, "Explicitly logo"
+						   "(els)\n");
+					mutex_lock(&lport->disc.disc_mutex);
+					lport->tt.rport_logoff(rdata);
+					mutex_unlock(&lport->disc.disc_mutex);
+				}
+				return;
+			}
+		} else {
+			/*
+			 * Handle ELS timeout.
+			 * tgt_lock is used to sync compl path and timeout
+			 * path. If els compl path is processing this IO, we
+			 * have nothing to do here, just release the timer hold
+			 */
+			BNX2FC_IO_DBG(io_req, "ELS timed out\n");
+			if (test_and_set_bit(BNX2FC_FLAG_ELS_DONE,
+					       &io_req->req_flags))
+				goto done;
+
+			/* Indicate the cb_func that this ELS is timed out */
+			set_bit(BNX2FC_FLAG_ELS_TIMEOUT, &io_req->req_flags);
+
+			if ((io_req->cb_func) && (io_req->cb_arg)) {
+				io_req->cb_func(io_req->cb_arg);
+				io_req->cb_arg = NULL;
+			}
+		}
+		break;
+	default:
+		printk(KERN_ERR PFX "cmd_timeout: invalid cmd_type %d\n",
+			cmd_type);
+		break;
+	}
+
+done:
+	/* release the cmd that was held when timer was set */
+	kref_put(&io_req->refcount, bnx2fc_cmd_release);
+	spin_unlock_bh(&tgt->tgt_lock);
+}
+
+static void bnx2fc_scsi_done(struct bnx2fc_cmd *io_req, int err_code)
+{
+	/* Called with host lock held */
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+
+	/*
+	 * active_cmd_queue may have other command types as well,
+	 * and during flush operation,  we want to error back only
+	 * scsi commands.
+	 */
+	if (io_req->cmd_type != BNX2FC_SCSI_CMD)
+		return;
+
+	BNX2FC_IO_DBG(io_req, "scsi_done. err_code = 0x%x\n", err_code);
+	bnx2fc_unmap_sg_list(io_req);
+	io_req->sc_cmd = NULL;
+	if (!sc_cmd) {
+		printk(KERN_ERR PFX "scsi_done - sc_cmd NULL. "
+				    "IO(0x%x) already cleaned up\n",
+		       io_req->xid);
+		return;
+	}
+	sc_cmd->result = err_code << 16;
+
+	BNX2FC_IO_DBG(io_req, "sc=%p, result=0x%x, retries=%d, allowed=%d\n",
+		sc_cmd, host_byte(sc_cmd->result), sc_cmd->retries,
+		sc_cmd->allowed);
+	scsi_set_resid(sc_cmd, scsi_bufflen(sc_cmd));
+	sc_cmd->SCp.ptr = NULL;
+	sc_cmd->scsi_done(sc_cmd);
+}
+
+struct bnx2fc_cmd_mgr *bnx2fc_cmd_mgr_alloc(struct bnx2fc_hba *hba,
+						u16 min_xid, u16 max_xid)
+{
+	struct bnx2fc_cmd_mgr *cmgr;
+	struct io_bdt *bdt_info;
+	struct bnx2fc_cmd *io_req;
+	size_t len;
+	u32 mem_size;
+	u16 xid;
+	int i;
+	int num_ios;
+	size_t bd_tbl_sz;
+
+	if (max_xid <= min_xid || max_xid == FC_XID_UNKNOWN) {
+		printk(KERN_ERR PFX "cmd_mgr_alloc: Invalid min_xid 0x%x \
+					and max_xid 0x%x\n", min_xid, max_xid);
+		return NULL;
+	}
+	BNX2FC_MISC_DBG("min xid 0x%x, max xid 0x%x\n", min_xid, max_xid);
+
+	num_ios = max_xid - min_xid + 1;
+	len = (num_ios * (sizeof(struct bnx2fc_cmd *)));
+	len += sizeof(struct bnx2fc_cmd_mgr);
+
+	cmgr = kzalloc(len, GFP_KERNEL);
+	if (!cmgr) {
+		printk(KERN_ERR PFX "failed to alloc cmgr\n");
+		return NULL;
+	}
+
+	cmgr->free_list = kzalloc(sizeof(*cmgr->free_list) *
+				  num_possible_cpus(), GFP_KERNEL);
+	if (!cmgr->free_list) {
+		printk(KERN_ERR PFX "failed to alloc free_list\n");
+		goto mem_err;
+	}
+
+	cmgr->free_list_lock = kzalloc(sizeof(*cmgr->free_list_lock) *
+				       num_possible_cpus(), GFP_KERNEL);
+	if (!cmgr->free_list_lock) {
+		printk(KERN_ERR PFX "failed to alloc free_list_lock\n");
+		goto mem_err;
+	}
+
+	cmgr->hba = hba;
+	cmgr->cmds = (struct bnx2fc_cmd **)(cmgr + 1);
+
+	for (i = 0; i < num_possible_cpus(); i++)  {
+		INIT_LIST_HEAD(&cmgr->free_list[i]);
+		spin_lock_init(&cmgr->free_list_lock[i]);
+	}
+
+	/* Pre-allocated pool of bnx2fc_cmds */
+	xid = BNX2FC_MIN_XID;
+	for (i = 0; i < num_ios; i++) {
+		io_req = kzalloc(sizeof(*io_req), GFP_KERNEL);
+
+		if (!io_req) {
+			printk(KERN_ERR PFX "failed to alloc io_req\n");
+			goto mem_err;
+		}
+
+		INIT_LIST_HEAD(&io_req->link);
+		INIT_DELAYED_WORK(&io_req->timeout_work, bnx2fc_cmd_timeout);
+
+		io_req->xid = xid++;
+		if (io_req->xid >= BNX2FC_MAX_OUTSTANDING_CMNDS)
+			printk(KERN_ERR PFX "ERROR allocating xids - 0x%x\n",
+				io_req->xid);
+		list_add_tail(&io_req->link,
+			&cmgr->free_list[io_req->xid % num_possible_cpus()]);
+		io_req++;
+	}
+
+	/* Allocate pool of io_bdts - one for each bnx2fc_cmd */
+	mem_size = num_ios * sizeof(struct io_bdt *);
+	cmgr->io_bdt_pool = kmalloc(mem_size, GFP_KERNEL);
+	if (!cmgr->io_bdt_pool) {
+		printk(KERN_ERR PFX "failed to alloc io_bdt_pool\n");
+		goto mem_err;
+	}
+
+	mem_size = sizeof(struct io_bdt);
+	for (i = 0; i < num_ios; i++) {
+		cmgr->io_bdt_pool[i] = kmalloc(mem_size, GFP_KERNEL);
+		if (!cmgr->io_bdt_pool[i]) {
+			printk(KERN_ERR PFX "failed to alloc "
+				"io_bdt_pool[%d]\n", i);
+			goto mem_err;
+		}
+	}
+
+	/* Allocate an map fcoe_bdt_ctx structures */
+	bd_tbl_sz = BNX2FC_MAX_BDS_PER_CMD * sizeof(struct fcoe_bd_ctx);
+	for (i = 0; i < num_ios; i++) {
+		bdt_info = cmgr->io_bdt_pool[i];
+		bdt_info->bd_tbl = dma_alloc_coherent(&hba->pcidev->dev,
+						      bd_tbl_sz,
+						      &bdt_info->bd_tbl_dma,
+						      GFP_KERNEL);
+		if (!bdt_info->bd_tbl) {
+			printk(KERN_ERR PFX "failed to alloc "
+				"bdt_tbl[%d]\n", i);
+			goto mem_err;
+		}
+	}
+
+	return cmgr;
+
+mem_err:
+	bnx2fc_cmd_mgr_free(cmgr);
+	return NULL;
+}
+
+void bnx2fc_cmd_mgr_free(struct bnx2fc_cmd_mgr *cmgr)
+{
+	struct io_bdt *bdt_info;
+	struct bnx2fc_hba *hba = cmgr->hba;
+	size_t bd_tbl_sz;
+	u16 min_xid = BNX2FC_MIN_XID;
+	u16 max_xid = BNX2FC_MAX_XID;
+	int num_ios;
+	int i;
+
+	num_ios = max_xid - min_xid + 1;
+
+	/* Free fcoe_bdt_ctx structures */
+	if (!cmgr->io_bdt_pool)
+		goto free_cmd_pool;
+
+	bd_tbl_sz = BNX2FC_MAX_BDS_PER_CMD * sizeof(struct fcoe_bd_ctx);
+	for (i = 0; i < num_ios; i++) {
+		bdt_info = cmgr->io_bdt_pool[i];
+		if (bdt_info->bd_tbl) {
+			dma_free_coherent(&hba->pcidev->dev, bd_tbl_sz,
+					    bdt_info->bd_tbl,
+					    bdt_info->bd_tbl_dma);
+			bdt_info->bd_tbl = NULL;
+		}
+	}
+
+	/* Destroy io_bdt pool */
+	for (i = 0; i < num_ios; i++) {
+		kfree(cmgr->io_bdt_pool[i]);
+		cmgr->io_bdt_pool[i] = NULL;
+	}
+
+	kfree(cmgr->io_bdt_pool);
+	cmgr->io_bdt_pool = NULL;
+
+free_cmd_pool:
+	kfree(cmgr->free_list_lock);
+
+	/* Destroy cmd pool */
+	if (!cmgr->free_list)
+		goto free_cmgr;
+
+	for (i = 0; i < num_possible_cpus(); i++)  {
+		struct list_head *list;
+		struct list_head *tmp;
+
+		list_for_each_safe(list, tmp, &cmgr->free_list[i]) {
+			struct bnx2fc_cmd *io_req = (struct bnx2fc_cmd *)list;
+			list_del(&io_req->link);
+			kfree(io_req);
+		}
+	}
+	kfree(cmgr->free_list);
+free_cmgr:
+	/* Free command manager itself */
+	kfree(cmgr);
+}
+
+struct bnx2fc_cmd *bnx2fc_elstm_alloc(struct bnx2fc_rport *tgt, int type)
+{
+	struct fcoe_port *port = tgt->port;
+	struct bnx2fc_hba *hba = port->priv;
+	struct bnx2fc_cmd_mgr *cmd_mgr = hba->cmd_mgr;
+	struct bnx2fc_cmd *io_req;
+	struct list_head *listp;
+	struct io_bdt *bd_tbl;
+	u32 max_sqes;
+	u16 xid;
+
+	max_sqes = tgt->max_sqes;
+	switch (type) {
+	case BNX2FC_TASK_MGMT_CMD:
+		max_sqes = BNX2FC_TM_MAX_SQES;
+		break;
+	case BNX2FC_ELS:
+		max_sqes = BNX2FC_ELS_MAX_SQES;
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * NOTE: Free list insertions and deletions are protected with
+	 * cmgr lock
+	 */
+	spin_lock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+	if ((list_empty(&(cmd_mgr->free_list[smp_processor_id()]))) ||
+	    (tgt->num_active_ios.counter  >= max_sqes)) {
+		BNX2FC_TGT_DBG(tgt, "No free els_tm cmds available "
+			"ios(%d):sqes(%d)\n",
+			tgt->num_active_ios.counter, tgt->max_sqes);
+		if (list_empty(&(cmd_mgr->free_list[smp_processor_id()])))
+			printk(KERN_ERR PFX "elstm_alloc: list_empty\n");
+		spin_unlock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+		return NULL;
+	}
+
+	listp = (struct list_head *)
+			cmd_mgr->free_list[smp_processor_id()].next;
+	list_del_init(listp);
+	io_req = (struct bnx2fc_cmd *) listp;
+	xid = io_req->xid;
+	cmd_mgr->cmds[xid] = io_req;
+	atomic_inc(&tgt->num_active_ios);
+	spin_unlock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+
+	INIT_LIST_HEAD(&io_req->link);
+
+	io_req->port = port;
+	io_req->cmd_mgr = cmd_mgr;
+	io_req->req_flags = 0;
+	io_req->cmd_type = type;
+
+	/* Bind io_bdt for this io_req */
+	/* Have a static link between io_req and io_bdt_pool */
+	bd_tbl = io_req->bd_tbl = cmd_mgr->io_bdt_pool[xid];
+	bd_tbl->io_req = io_req;
+
+	/* Hold the io_req  against deletion */
+	kref_init(&io_req->refcount);
+	return io_req;
+}
+static struct bnx2fc_cmd *bnx2fc_cmd_alloc(struct bnx2fc_rport *tgt)
+{
+	struct fcoe_port *port = tgt->port;
+	struct bnx2fc_hba *hba = port->priv;
+	struct bnx2fc_cmd_mgr *cmd_mgr = hba->cmd_mgr;
+	struct bnx2fc_cmd *io_req;
+	struct list_head *listp;
+	struct io_bdt *bd_tbl;
+	u32 max_sqes;
+	u16 xid;
+
+	max_sqes = BNX2FC_SCSI_MAX_SQES;
+	/*
+	 * NOTE: Free list insertions and deletions are protected with
+	 * cmgr lock
+	 */
+	spin_lock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+	if ((list_empty(&cmd_mgr->free_list[smp_processor_id()])) ||
+	    (tgt->num_active_ios.counter  >= max_sqes)) {
+		spin_unlock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+		return NULL;
+	}
+
+	listp = (struct list_head *)
+		cmd_mgr->free_list[smp_processor_id()].next;
+	list_del_init(listp);
+	io_req = (struct bnx2fc_cmd *) listp;
+	xid = io_req->xid;
+	cmd_mgr->cmds[xid] = io_req;
+	atomic_inc(&tgt->num_active_ios);
+	spin_unlock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+
+	INIT_LIST_HEAD(&io_req->link);
+
+	io_req->port = port;
+	io_req->cmd_mgr = cmd_mgr;
+	io_req->req_flags = 0;
+
+	/* Bind io_bdt for this io_req */
+	/* Have a static link between io_req and io_bdt_pool */
+	bd_tbl = io_req->bd_tbl = cmd_mgr->io_bdt_pool[xid];
+	bd_tbl->io_req = io_req;
+
+	/* Hold the io_req  against deletion */
+	kref_init(&io_req->refcount);
+	return io_req;
+}
+
+void bnx2fc_cmd_release(struct kref *ref)
+{
+	struct bnx2fc_cmd *io_req = container_of(ref,
+						struct bnx2fc_cmd, refcount);
+	struct bnx2fc_cmd_mgr *cmd_mgr = io_req->cmd_mgr;
+
+	spin_lock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+	if (io_req->cmd_type != BNX2FC_SCSI_CMD)
+		bnx2fc_free_mp_resc(io_req);
+	cmd_mgr->cmds[io_req->xid] = NULL;
+	/* Delete IO from retire queue */
+	list_del_init(&io_req->link);
+	/* Add it to the free list */
+	list_add(&io_req->link,
+			&cmd_mgr->free_list[smp_processor_id()]);
+	atomic_dec(&io_req->tgt->num_active_ios);
+	spin_unlock_bh(&cmd_mgr->free_list_lock[smp_processor_id()]);
+}
+
+static void bnx2fc_free_mp_resc(struct bnx2fc_cmd *io_req)
+{
+	struct bnx2fc_mp_req *mp_req = &(io_req->mp_req);
+	struct bnx2fc_hba *hba = io_req->port->priv;
+	size_t sz = sizeof(struct fcoe_bd_ctx);
+
+	/* clear tm flags */
+	mp_req->tm_flags = 0;
+	if (mp_req->mp_req_bd) {
+		dma_free_coherent(&hba->pcidev->dev, sz,
+				     mp_req->mp_req_bd,
+				     mp_req->mp_req_bd_dma);
+		mp_req->mp_req_bd = NULL;
+	}
+	if (mp_req->mp_resp_bd) {
+		dma_free_coherent(&hba->pcidev->dev, sz,
+				     mp_req->mp_resp_bd,
+				     mp_req->mp_resp_bd_dma);
+		mp_req->mp_resp_bd = NULL;
+	}
+	if (mp_req->req_buf) {
+		dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				     mp_req->req_buf,
+				     mp_req->req_buf_dma);
+		mp_req->req_buf = NULL;
+	}
+	if (mp_req->resp_buf) {
+		dma_free_coherent(&hba->pcidev->dev, PAGE_SIZE,
+				     mp_req->resp_buf,
+				     mp_req->resp_buf_dma);
+		mp_req->resp_buf = NULL;
+	}
+}
+
+int bnx2fc_init_mp_req(struct bnx2fc_cmd *io_req)
+{
+	struct bnx2fc_mp_req *mp_req;
+	struct fcoe_bd_ctx *mp_req_bd;
+	struct fcoe_bd_ctx *mp_resp_bd;
+	struct bnx2fc_hba *hba = io_req->port->priv;
+	dma_addr_t addr;
+	size_t sz;
+
+	mp_req = (struct bnx2fc_mp_req *)&(io_req->mp_req);
+	memset(mp_req, 0, sizeof(struct bnx2fc_mp_req));
+
+	mp_req->req_len = sizeof(struct fcp_cmnd);
+	io_req->data_xfer_len = mp_req->req_len;
+	mp_req->req_buf = dma_alloc_coherent(&hba->pcidev->dev, PAGE_SIZE,
+					     &mp_req->req_buf_dma,
+					     GFP_ATOMIC);
+	if (!mp_req->req_buf) {
+		printk(KERN_ERR PFX "unable to alloc MP req buffer\n");
+		bnx2fc_free_mp_resc(io_req);
+		return FAILED;
+	}
+
+	mp_req->resp_buf = dma_alloc_coherent(&hba->pcidev->dev, PAGE_SIZE,
+					      &mp_req->resp_buf_dma,
+					      GFP_ATOMIC);
+	if (!mp_req->resp_buf) {
+		printk(KERN_ERR PFX "unable to alloc TM resp buffer\n");
+		bnx2fc_free_mp_resc(io_req);
+		return FAILED;
+	}
+	memset(mp_req->req_buf, 0, PAGE_SIZE);
+	memset(mp_req->resp_buf, 0, PAGE_SIZE);
+
+	/* Allocate and map mp_req_bd and mp_resp_bd */
+	sz = sizeof(struct fcoe_bd_ctx);
+	mp_req->mp_req_bd = dma_alloc_coherent(&hba->pcidev->dev, sz,
+						 &mp_req->mp_req_bd_dma,
+						 GFP_ATOMIC);
+	if (!mp_req->mp_req_bd) {
+		printk(KERN_ERR PFX "unable to alloc MP req bd\n");
+		bnx2fc_free_mp_resc(io_req);
+		return FAILED;
+	}
+	mp_req->mp_resp_bd = dma_alloc_coherent(&hba->pcidev->dev, sz,
+						 &mp_req->mp_resp_bd_dma,
+						 GFP_ATOMIC);
+	if (!mp_req->mp_req_bd) {
+		printk(KERN_ERR PFX "unable to alloc MP resp bd\n");
+		bnx2fc_free_mp_resc(io_req);
+		return FAILED;
+	}
+	/* Fill bd table */
+	addr = mp_req->req_buf_dma;
+	mp_req_bd = mp_req->mp_req_bd;
+	mp_req_bd->buf_addr_lo = (u32)addr & 0xffffffff;
+	mp_req_bd->buf_addr_hi = (u32)((u64)addr >> 32);
+	mp_req_bd->buf_len = PAGE_SIZE;
+	mp_req_bd->flags = 0;
+
+	/*
+	 * MP buffer is either a task mgmt command or an ELS.
+	 * So the assumption is that it consumes a single bd
+	 * entry in the bd table
+	 */
+	mp_resp_bd = mp_req->mp_resp_bd;
+	addr = mp_req->resp_buf_dma;
+	mp_resp_bd->buf_addr_lo = (u32)addr & 0xffffffff;
+	mp_resp_bd->buf_addr_hi = (u32)((u64)addr >> 32);
+	mp_resp_bd->buf_len = PAGE_SIZE;
+	mp_resp_bd->flags = 0;
+
+	return SUCCESS;
+}
+
+static int bnx2fc_initiate_tmf(struct scsi_cmnd *sc_cmd, u8 tm_flags)
+{
+	struct fc_lport *lport;
+	struct fc_rport *rport = starget_to_rport(scsi_target(sc_cmd->device));
+	struct fc_rport_libfc_priv *rp = rport->dd_data;
+	struct fcoe_port *port;
+	struct bnx2fc_hba *hba;
+	struct bnx2fc_rport *tgt;
+	struct bnx2fc_cmd *io_req;
+	struct bnx2fc_mp_req *tm_req;
+	struct fcoe_task_ctx_entry *task;
+	struct fcoe_task_ctx_entry *task_page;
+	struct Scsi_Host *host = sc_cmd->device->host;
+	struct fc_frame_header *fc_hdr;
+	struct fcp_cmnd *fcp_cmnd;
+	int task_idx, index;
+	int rc = SUCCESS;
+	u16 xid;
+	u32 sid, did;
+	unsigned long start = jiffies;
+
+	lport = shost_priv(host);
+	port = lport_priv(lport);
+	hba = port->priv;
+
+	if (rport == NULL) {
+		printk(KERN_ALERT PFX "device_reset: rport is NULL\n");
+		rc = FAILED;
+		goto tmf_err;
+	}
+
+	rc = fc_block_scsi_eh(sc_cmd);
+	if (rc)
+		return rc;
+
+	if (lport->state != LPORT_ST_READY || !(lport->link_up)) {
+		printk(KERN_ERR PFX "device_reset: link is not ready\n");
+		rc = FAILED;
+		goto tmf_err;
+	}
+	/* rport and tgt are allocated together, so tgt should be non-NULL */
+	tgt = (struct bnx2fc_rport *)&rp[1];
+
+	if (!(test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags))) {
+		printk(KERN_ERR PFX "device_reset: tgt not offloaded\n");
+		rc = FAILED;
+		goto tmf_err;
+	}
+retry_tmf:
+	io_req = bnx2fc_elstm_alloc(tgt, BNX2FC_TASK_MGMT_CMD);
+	if (!io_req) {
+		if (time_after(jiffies, start + HZ)) {
+			printk(KERN_ERR PFX "tmf: Failed TMF");
+			rc = FAILED;
+			goto tmf_err;
+		}
+		msleep(20);
+		goto retry_tmf;
+	}
+	/* Initialize rest of io_req fields */
+	io_req->sc_cmd = sc_cmd;
+	io_req->port = port;
+	io_req->tgt = tgt;
+
+	tm_req = (struct bnx2fc_mp_req *)&(io_req->mp_req);
+
+	rc = bnx2fc_init_mp_req(io_req);
+	if (rc == FAILED) {
+		printk(KERN_ERR PFX "Task mgmt MP request init failed\n");
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		goto tmf_err;
+	}
+
+	/* Set TM flags */
+	io_req->io_req_flags = 0;
+	tm_req->tm_flags = tm_flags;
+
+	/* Fill FCP_CMND */
+	bnx2fc_build_fcp_cmnd(io_req, (struct fcp_cmnd *)tm_req->req_buf);
+	fcp_cmnd = (struct fcp_cmnd *)tm_req->req_buf;
+	memset(fcp_cmnd->fc_cdb, 0,  sc_cmd->cmd_len);
+	fcp_cmnd->fc_dl = 0;
+
+	/* Fill FC header */
+	fc_hdr = &(tm_req->req_fc_hdr);
+	sid = tgt->sid;
+	did = rport->port_id;
+	__fc_fill_fc_hdr(fc_hdr, FC_RCTL_DD_UNSOL_CMD, did, sid,
+			   FC_TYPE_FCP, FC_FC_FIRST_SEQ | FC_FC_END_SEQ |
+			   FC_FC_SEQ_INIT, 0);
+	/* Obtain exchange id */
+	xid = io_req->xid;
+
+	BNX2FC_TGT_DBG(tgt, "Initiate TMF - xid = 0x%x\n", xid);
+	task_idx = xid/BNX2FC_TASKS_PER_PAGE;
+	index = xid % BNX2FC_TASKS_PER_PAGE;
+
+	/* Initialize task context for this IO request */
+	task_page = (struct fcoe_task_ctx_entry *) hba->task_ctx[task_idx];
+	task = &(task_page[index]);
+	bnx2fc_init_mp_task(io_req, task);
+
+	sc_cmd->SCp.ptr = (char *)io_req;
+
+	/* Obtain free SQ entry */
+	spin_lock_bh(&tgt->tgt_lock);
+	bnx2fc_add_2_sq(tgt, xid);
+
+	/* Enqueue the io_req to active_tm_queue */
+	io_req->on_tmf_queue = 1;
+	list_add_tail(&io_req->link, &tgt->active_tm_queue);
+
+	init_completion(&io_req->tm_done);
+	io_req->wait_for_comp = 1;
+
+	/* Ring doorbell */
+	bnx2fc_ring_doorbell(tgt);
+	spin_unlock_bh(&tgt->tgt_lock);
+
+	rc = wait_for_completion_timeout(&io_req->tm_done,
+					 BNX2FC_TM_TIMEOUT * HZ);
+	spin_lock_bh(&tgt->tgt_lock);
+
+	io_req->wait_for_comp = 0;
+	if (!(test_bit(BNX2FC_FLAG_TM_COMPL, &io_req->req_flags)))
+		set_bit(BNX2FC_FLAG_TM_TIMEOUT, &io_req->req_flags);
+
+	spin_unlock_bh(&tgt->tgt_lock);
+
+	if (!rc) {
+		printk(KERN_ERR PFX "task mgmt command failed...\n");
+		rc = FAILED;
+	} else {
+		printk(KERN_ERR PFX "task mgmt command success...\n");
+		rc = SUCCESS;
+	}
+tmf_err:
+	return rc;
+}
+
+int bnx2fc_initiate_abts(struct bnx2fc_cmd *io_req)
+{
+	struct fc_lport *lport;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	struct fc_rport *rport = tgt->rport;
+	struct fc_rport_priv *rdata = tgt->rdata;
+	struct bnx2fc_hba *hba;
+	struct fcoe_port *port;
+	struct bnx2fc_cmd *abts_io_req;
+	struct fcoe_task_ctx_entry *task;
+	struct fcoe_task_ctx_entry *task_page;
+	struct fc_frame_header *fc_hdr;
+	struct bnx2fc_mp_req *abts_req;
+	int task_idx, index;
+	u32 sid, did;
+	u16 xid;
+	int rc = SUCCESS;
+	u32 r_a_tov = rdata->r_a_tov;
+
+	/* called with tgt_lock held */
+	BNX2FC_IO_DBG(io_req, "Entered bnx2fc_initiate_abts\n");
+
+	port = io_req->port;
+	hba = port->priv;
+	lport = port->lport;
+
+	if (!test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags)) {
+		printk(KERN_ERR PFX "initiate_abts: tgt not offloaded\n");
+		rc = FAILED;
+		goto abts_err;
+	}
+
+	if (rport == NULL) {
+		printk(KERN_ALERT PFX "initiate_abts: rport is NULL\n");
+		rc = FAILED;
+		goto abts_err;
+	}
+
+	if (lport->state != LPORT_ST_READY || !(lport->link_up)) {
+		printk(KERN_ERR PFX "initiate_abts: link is not ready\n");
+		rc = FAILED;
+		goto abts_err;
+	}
+
+	abts_io_req = bnx2fc_elstm_alloc(tgt, BNX2FC_ABTS);
+	if (!abts_io_req) {
+		printk(KERN_ERR PFX "abts: couldnt allocate cmd\n");
+		rc = FAILED;
+		goto abts_err;
+	}
+
+	/* Initialize rest of io_req fields */
+	abts_io_req->sc_cmd = NULL;
+	abts_io_req->port = port;
+	abts_io_req->tgt = tgt;
+	abts_io_req->data_xfer_len = 0; /* No data transfer for ABTS */
+
+	abts_req = (struct bnx2fc_mp_req *)&(abts_io_req->mp_req);
+	memset(abts_req, 0, sizeof(struct bnx2fc_mp_req));
+
+	/* Fill FC header */
+	fc_hdr = &(abts_req->req_fc_hdr);
+
+	/* Obtain oxid and rxid for the original exchange to be aborted */
+	fc_hdr->fh_ox_id = htons(io_req->xid);
+	fc_hdr->fh_rx_id = htons(io_req->task->rx_wr_tx_rd.rx_id);
+
+	sid = tgt->sid;
+	did = rport->port_id;
+
+	__fc_fill_fc_hdr(fc_hdr, FC_RCTL_BA_ABTS, did, sid,
+			   FC_TYPE_BLS, FC_FC_FIRST_SEQ | FC_FC_END_SEQ |
+			   FC_FC_SEQ_INIT, 0);
+
+	xid = abts_io_req->xid;
+	BNX2FC_IO_DBG(abts_io_req, "ABTS io_req\n");
+	task_idx = xid/BNX2FC_TASKS_PER_PAGE;
+	index = xid % BNX2FC_TASKS_PER_PAGE;
+
+	/* Initialize task context for this IO request */
+	task_page = (struct fcoe_task_ctx_entry *) hba->task_ctx[task_idx];
+	task = &(task_page[index]);
+	bnx2fc_init_mp_task(abts_io_req, task);
+
+	/*
+	 * ABTS task is a temporary task that will be cleaned up
+	 * irrespective of ABTS response. We need to start the timer
+	 * for the original exchange, as the CQE is posted for the original
+	 * IO request.
+	 *
+	 * Timer for ABTS is started only when it is originated by a
+	 * TM request. For the ABTS issued as part of ULP timeout,
+	 * scsi-ml maintains the timers.
+	 */
+
+	/* if (test_bit(BNX2FC_FLAG_ISSUE_ABTS, &io_req->req_flags))*/
+	bnx2fc_cmd_timer_set(io_req, 2 * r_a_tov);
+
+	/* Obtain free SQ entry */
+	bnx2fc_add_2_sq(tgt, xid);
+
+	/* Ring doorbell */
+	bnx2fc_ring_doorbell(tgt);
+
+abts_err:
+	return rc;
+}
+
+int bnx2fc_initiate_cleanup(struct bnx2fc_cmd *io_req)
+{
+	struct fc_lport *lport;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	struct bnx2fc_hba *hba;
+	struct fcoe_port *port;
+	struct bnx2fc_cmd *cleanup_io_req;
+	struct fcoe_task_ctx_entry *task;
+	struct fcoe_task_ctx_entry *task_page;
+	int task_idx, index;
+	u16 xid, orig_xid;
+	int rc = 0;
+
+	/* ASSUMPTION: called with tgt_lock held */
+	BNX2FC_IO_DBG(io_req, "Entered bnx2fc_initiate_cleanup\n");
+
+	port = io_req->port;
+	hba = port->priv;
+	lport = port->lport;
+
+	cleanup_io_req = bnx2fc_elstm_alloc(tgt, BNX2FC_CLEANUP);
+	if (!cleanup_io_req) {
+		printk(KERN_ERR PFX "cleanup: couldnt allocate cmd\n");
+		rc = -1;
+		goto cleanup_err;
+	}
+
+	/* Initialize rest of io_req fields */
+	cleanup_io_req->sc_cmd = NULL;
+	cleanup_io_req->port = port;
+	cleanup_io_req->tgt = tgt;
+	cleanup_io_req->data_xfer_len = 0; /* No data transfer for cleanup */
+
+	xid = cleanup_io_req->xid;
+
+	task_idx = xid/BNX2FC_TASKS_PER_PAGE;
+	index = xid % BNX2FC_TASKS_PER_PAGE;
+
+	/* Initialize task context for this IO request */
+	task_page = (struct fcoe_task_ctx_entry *) hba->task_ctx[task_idx];
+	task = &(task_page[index]);
+	orig_xid = io_req->xid;
+
+	BNX2FC_IO_DBG(io_req, "CLEANUP io_req xid = 0x%x\n", xid);
+
+	bnx2fc_init_cleanup_task(cleanup_io_req, task, orig_xid);
+
+	/* Obtain free SQ entry */
+	bnx2fc_add_2_sq(tgt, xid);
+
+	/* Ring doorbell */
+	bnx2fc_ring_doorbell(tgt);
+
+cleanup_err:
+	return rc;
+}
+
+/**
+ * bnx2fc_eh_target_reset: Reset a target
+ *
+ * @sc_cmd:	SCSI command
+ *
+ * Set from SCSI host template to send task mgmt command to the target
+ *	and wait for the response
+ */
+int bnx2fc_eh_target_reset(struct scsi_cmnd *sc_cmd)
+{
+	return bnx2fc_initiate_tmf(sc_cmd, FCP_TMF_TGT_RESET);
+}
+
+/**
+ * bnx2fc_eh_device_reset - Reset a single LUN
+ *
+ * @sc_cmd:	SCSI command
+ *
+ * Set from SCSI host template to send task mgmt command to the target
+ *	and wait for the response
+ */
+int bnx2fc_eh_device_reset(struct scsi_cmnd *sc_cmd)
+{
+	return bnx2fc_initiate_tmf(sc_cmd, FCP_TMF_LUN_RESET);
+}
+
+/**
+ * bnx2fc_eh_abort - eh_abort_handler api to abort an outstanding
+ *			SCSI command
+ *
+ * @sc_cmd:	SCSI_ML command pointer
+ *
+ * SCSI abort request handler
+ */
+int bnx2fc_eh_abort(struct scsi_cmnd *sc_cmd)
+{
+	struct fc_rport *rport = starget_to_rport(scsi_target(sc_cmd->device));
+	struct fc_rport_libfc_priv *rp = rport->dd_data;
+	struct bnx2fc_cmd *io_req;
+	struct fc_lport *lport;
+	struct bnx2fc_rport *tgt;
+	int rc = FAILED;
+
+
+	rc = fc_block_scsi_eh(sc_cmd);
+	if (rc)
+		return rc;
+
+	lport = shost_priv(sc_cmd->device->host);
+	if ((lport->state != LPORT_ST_READY) || !(lport->link_up)) {
+		printk(KERN_ALERT PFX "eh_abort: link not ready\n");
+		return rc;
+	}
+
+	tgt = (struct bnx2fc_rport *)&rp[1];
+
+	BNX2FC_TGT_DBG(tgt, "Entered bnx2fc_eh_abort\n");
+
+	spin_lock_bh(&tgt->tgt_lock);
+	io_req = (struct bnx2fc_cmd *)sc_cmd->SCp.ptr;
+	if (!io_req) {
+		/* Command might have just completed */
+		printk(KERN_ERR PFX "eh_abort: io_req is NULL\n");
+		spin_unlock_bh(&tgt->tgt_lock);
+		return SUCCESS;
+	}
+	BNX2FC_IO_DBG(io_req, "eh_abort - refcnt = %d\n",
+		      io_req->refcount.refcount.counter);
+
+	/* Hold IO request across abort processing */
+	kref_get(&io_req->refcount);
+
+	BUG_ON(tgt != io_req->tgt);
+
+	/* Remove the io_req from the active_q. */
+	/*
+	 * Task Mgmt functions (LUN RESET & TGT RESET) will not
+	 * issue an ABTS on this particular IO req, as the
+	 * io_req is no longer in the active_q.
+	 */
+	if (tgt->flush_in_prog) {
+		printk(KERN_ALERT PFX "eh_abort: io_req (xid = 0x%x) "
+			"flush in progress\n", io_req->xid);
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return SUCCESS;
+	}
+
+	if (io_req->on_active_queue == 0) {
+		printk(KERN_ALERT PFX "eh_abort: io_req (xid = 0x%x) "
+				"not on active_q\n", io_req->xid);
+		/*
+		 * This condition can happen only due to the FW bug,
+		 * where we do not receive cleanup response from
+		 * the FW. Handle this case gracefully by erroring
+		 * back the IO request to SCSI-ml
+		 */
+		bnx2fc_scsi_done(io_req, DID_ABORT);
+
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return SUCCESS;
+	}
+
+	/*
+	 * Only eh_abort processing will remove the IO from
+	 * active_cmd_q before processing the request. this is
+	 * done to avoid race conditions between IOs aborted
+	 * as part of task management completion and eh_abort
+	 * processing
+	 */
+	list_del_init(&io_req->link);
+	io_req->on_active_queue = 0;
+	/* Move IO req to retire queue */
+	list_add_tail(&io_req->link, &tgt->io_retire_queue);
+
+	init_completion(&io_req->tm_done);
+	io_req->wait_for_comp = 1;
+
+	if (!test_and_set_bit(BNX2FC_FLAG_ISSUE_ABTS, &io_req->req_flags)) {
+		/* Cancel the current timer running on this io_req */
+		if (cancel_delayed_work(&io_req->timeout_work))
+			kref_put(&io_req->refcount,
+				 bnx2fc_cmd_release); /* drop timer hold */
+		set_bit(BNX2FC_FLAG_EH_ABORT, &io_req->req_flags);
+		rc = bnx2fc_initiate_abts(io_req);
+	} else {
+		printk(KERN_ALERT PFX "eh_abort: io_req (xid = 0x%x) "
+				"already in abts processing\n", io_req->xid);
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return SUCCESS;
+	}
+	if (rc == FAILED) {
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return rc;
+	}
+	spin_unlock_bh(&tgt->tgt_lock);
+
+	wait_for_completion(&io_req->tm_done);
+
+	spin_lock_bh(&tgt->tgt_lock);
+	io_req->wait_for_comp = 0;
+	if (!(test_and_set_bit(BNX2FC_FLAG_ABTS_DONE,
+				    &io_req->req_flags))) {
+		/* Let the scsi-ml try to recover this command */
+		printk(KERN_ERR PFX "abort failed, xid = 0x%x\n",
+		       io_req->xid);
+		rc = FAILED;
+	} else {
+		/*
+		 * We come here even when there was a race condition
+		 * between timeout and abts completion, and abts
+		 * completion happens just in time.
+		 */
+		BNX2FC_IO_DBG(io_req, "abort succeeded\n");
+		rc = SUCCESS;
+		bnx2fc_scsi_done(io_req, DID_ABORT);
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+	}
+
+	/* release the reference taken in eh_abort */
+	kref_put(&io_req->refcount, bnx2fc_cmd_release);
+	spin_unlock_bh(&tgt->tgt_lock);
+	return rc;
+}
+
+void bnx2fc_process_cleanup_compl(struct bnx2fc_cmd *io_req,
+				  struct fcoe_task_ctx_entry *task,
+				  u8 num_rq)
+{
+	BNX2FC_IO_DBG(io_req, "Entered process_cleanup_compl "
+			      "refcnt = %d, cmd_type = %d\n",
+		   io_req->refcount.refcount.counter, io_req->cmd_type);
+	bnx2fc_scsi_done(io_req, DID_ERROR);
+	kref_put(&io_req->refcount, bnx2fc_cmd_release);
+}
+
+void bnx2fc_process_abts_compl(struct bnx2fc_cmd *io_req,
+			       struct fcoe_task_ctx_entry *task,
+			       u8 num_rq)
+{
+	u32 r_ctl;
+	u32 r_a_tov = FC_DEF_R_A_TOV;
+	u8 issue_rrq = 0;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+
+	BNX2FC_IO_DBG(io_req, "Entered process_abts_compl xid = 0x%x"
+			      "refcnt = %d, cmd_type = %d\n",
+		   io_req->xid,
+		   io_req->refcount.refcount.counter, io_req->cmd_type);
+
+	if (test_and_set_bit(BNX2FC_FLAG_ABTS_DONE,
+				       &io_req->req_flags)) {
+		BNX2FC_IO_DBG(io_req, "Timer context finished processing"
+				" this io\n");
+		return;
+	}
+
+	/* Do not issue RRQ as this IO is already cleanedup */
+	if (test_and_set_bit(BNX2FC_FLAG_IO_CLEANUP,
+				&io_req->req_flags))
+		goto io_compl;
+
+	/*
+	 * For ABTS issued due to SCSI eh_abort_handler, timeout
+	 * values are maintained by scsi-ml itself. Cancel timeout
+	 * in case ABTS issued as part of task management function
+	 * or due to FW error.
+	 */
+	if (test_bit(BNX2FC_FLAG_ISSUE_ABTS, &io_req->req_flags))
+		if (cancel_delayed_work(&io_req->timeout_work))
+			kref_put(&io_req->refcount,
+				 bnx2fc_cmd_release); /* drop timer hold */
+
+	r_ctl = task->cmn.general.rsp_info.abts_rsp.r_ctl;
+
+	switch (r_ctl) {
+	case FC_RCTL_BA_ACC:
+		/*
+		 * Dont release this cmd yet. It will be relesed
+		 * after we get RRQ response
+		 */
+		BNX2FC_IO_DBG(io_req, "ABTS response - ACC Send RRQ\n");
+		issue_rrq = 1;
+		break;
+
+	case FC_RCTL_BA_RJT:
+		BNX2FC_IO_DBG(io_req, "ABTS response - RJT\n");
+		break;
+	default:
+		printk(KERN_ERR PFX "Unknown ABTS response\n");
+		break;
+	}
+
+	if (issue_rrq) {
+		BNX2FC_IO_DBG(io_req, "Issue RRQ after R_A_TOV\n");
+		set_bit(BNX2FC_FLAG_ISSUE_RRQ, &io_req->req_flags);
+	}
+	set_bit(BNX2FC_FLAG_RETIRE_OXID, &io_req->req_flags);
+	bnx2fc_cmd_timer_set(io_req, r_a_tov);
+
+io_compl:
+	if (io_req->wait_for_comp) {
+		if (test_and_clear_bit(BNX2FC_FLAG_EH_ABORT,
+				       &io_req->req_flags))
+			complete(&io_req->tm_done);
+	} else {
+		/*
+		 * We end up here when ABTS is issued as
+		 * in asynchronous context, i.e., as part
+		 * of task management completion, or
+		 * when FW error is received or when the
+		 * ABTS is issued when the IO is timed
+		 * out.
+		 */
+
+		if (io_req->on_active_queue) {
+			list_del_init(&io_req->link);
+			io_req->on_active_queue = 0;
+			/* Move IO req to retire queue */
+			list_add_tail(&io_req->link, &tgt->io_retire_queue);
+		}
+		bnx2fc_scsi_done(io_req, DID_ERROR);
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+	}
+}
+
+static void bnx2fc_lun_reset_cmpl(struct bnx2fc_cmd *io_req)
+{
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	struct list_head *list;
+	struct list_head *tmp;
+	struct bnx2fc_cmd *cmd;
+	int tm_lun = sc_cmd->device->lun;
+	int rc = 0;
+	int lun;
+
+	/* called with tgt_lock held */
+	BNX2FC_IO_DBG(io_req, "Entered bnx2fc_lun_reset_cmpl\n");
+	/*
+	 * Walk thru the active_ios queue and ABORT the IO
+	 * that matches with the LUN that was reset
+	 */
+	list_for_each_safe(list, tmp, &tgt->active_cmd_queue) {
+		BNX2FC_TGT_DBG(tgt, "LUN RST cmpl: scan for pending IOs\n");
+		cmd = (struct bnx2fc_cmd *)list;
+		lun = cmd->sc_cmd->device->lun;
+		if (lun == tm_lun) {
+			/* Initiate ABTS on this cmd */
+			if (!test_and_set_bit(BNX2FC_FLAG_ISSUE_ABTS,
+					      &cmd->req_flags)) {
+				/* cancel the IO timeout */
+				if (cancel_delayed_work(&io_req->timeout_work))
+					kref_put(&io_req->refcount,
+						 bnx2fc_cmd_release);
+							/* timer hold */
+				rc = bnx2fc_initiate_abts(cmd);
+				/* abts shouldnt fail in this context */
+				WARN_ON(rc != SUCCESS);
+			} else
+				printk(KERN_ERR PFX "lun_rst: abts already in"
+					" progress for this IO 0x%x\n",
+					cmd->xid);
+		}
+	}
+}
+
+static void bnx2fc_tgt_reset_cmpl(struct bnx2fc_cmd *io_req)
+{
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	struct list_head *list;
+	struct list_head *tmp;
+	struct bnx2fc_cmd *cmd;
+	int rc = 0;
+
+	/* called with tgt_lock held */
+	BNX2FC_IO_DBG(io_req, "Entered bnx2fc_tgt_reset_cmpl\n");
+	/*
+	 * Walk thru the active_ios queue and ABORT the IO
+	 * that matches with the LUN that was reset
+	 */
+	list_for_each_safe(list, tmp, &tgt->active_cmd_queue) {
+		BNX2FC_TGT_DBG(tgt, "TGT RST cmpl: scan for pending IOs\n");
+		cmd = (struct bnx2fc_cmd *)list;
+		/* Initiate ABTS */
+		if (!test_and_set_bit(BNX2FC_FLAG_ISSUE_ABTS,
+							&cmd->req_flags)) {
+			/* cancel the IO timeout */
+			if (cancel_delayed_work(&io_req->timeout_work))
+				kref_put(&io_req->refcount,
+					 bnx2fc_cmd_release); /* timer hold */
+			rc = bnx2fc_initiate_abts(cmd);
+			/* abts shouldnt fail in this context */
+			WARN_ON(rc != SUCCESS);
+
+		} else
+			printk(KERN_ERR PFX "tgt_rst: abts already in progress"
+				" for this IO 0x%x\n", cmd->xid);
+	}
+}
+
+void bnx2fc_process_tm_compl(struct bnx2fc_cmd *io_req,
+			     struct fcoe_task_ctx_entry *task, u8 num_rq)
+{
+	struct bnx2fc_mp_req *tm_req;
+	struct fc_frame_header *fc_hdr;
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+	u64 *hdr;
+	u64 *temp_hdr;
+	void *rsp_buf;
+
+	/* Called with tgt_lock held */
+	BNX2FC_IO_DBG(io_req, "Entered process_tm_compl\n");
+
+	if (!(test_bit(BNX2FC_FLAG_TM_TIMEOUT, &io_req->req_flags)))
+		set_bit(BNX2FC_FLAG_TM_COMPL, &io_req->req_flags);
+	else {
+		/* TM has already timed out and we got
+		 * delayed completion. Ignore completion
+		 * processing.
+		 */
+		return;
+	}
+
+	tm_req = &(io_req->mp_req);
+	fc_hdr = &(tm_req->resp_fc_hdr);
+	hdr = (u64 *)fc_hdr;
+	temp_hdr = (u64 *)
+		&task->cmn.general.cmd_info.mp_fc_frame.fc_hdr;
+	hdr[0] = cpu_to_be64(temp_hdr[0]);
+	hdr[1] = cpu_to_be64(temp_hdr[1]);
+	hdr[2] = cpu_to_be64(temp_hdr[2]);
+
+	tm_req->resp_len = task->rx_wr_only.sgl_ctx.mul_sges.cur_sge_off;
+
+	rsp_buf = tm_req->resp_buf;
+
+	if (fc_hdr->fh_r_ctl == FC_RCTL_DD_CMD_STATUS) {
+		bnx2fc_parse_fcp_rsp(io_req,
+				     (struct fcoe_fcp_rsp_payload *)
+				     rsp_buf, num_rq);
+		if (io_req->fcp_rsp_code == 0) {
+			/* TM successful */
+			if (tm_req->tm_flags & FCP_TMF_LUN_RESET)
+				bnx2fc_lun_reset_cmpl(io_req);
+			else if (tm_req->tm_flags & FCP_TMF_TGT_RESET)
+				bnx2fc_tgt_reset_cmpl(io_req);
+		}
+	} else {
+		printk(KERN_ERR PFX "tmf's fc_hdr r_ctl = 0x%x\n",
+			fc_hdr->fh_r_ctl);
+	}
+	if (!sc_cmd->SCp.ptr) {
+		printk(KERN_ALERT PFX "tm_compl: SCp.ptr is NULL\n");
+		return;
+	}
+	switch (io_req->fcp_status) {
+	case FC_GOOD:
+		if (io_req->cdb_status == 0) {
+			/* Good IO completion */
+			sc_cmd->result = DID_OK << 16;
+		} else {
+			/* Transport status is good, SCSI status not good */
+			sc_cmd->result = (DID_OK << 16) | io_req->cdb_status;
+		}
+		if (io_req->fcp_resid)
+			scsi_set_resid(sc_cmd, io_req->fcp_resid);
+		break;
+
+	default:
+		BNX2FC_IO_DBG(io_req, "process_tm_compl: fcp_status = %d\n",
+			   io_req->fcp_status);
+		break;
+	}
+
+	sc_cmd = io_req->sc_cmd;
+	io_req->sc_cmd = NULL;
+
+	/* check if the io_req exists in tgt's tmf_q */
+	if (io_req->on_tmf_queue) {
+
+		list_del_init(&io_req->link);
+		io_req->on_tmf_queue = 0;
+	} else {
+
+		printk(KERN_ALERT PFX "Command not on active_cmd_queue!\n");
+		return;
+	}
+
+	sc_cmd->SCp.ptr = NULL;
+	sc_cmd->scsi_done(sc_cmd);
+
+	kref_put(&io_req->refcount, bnx2fc_cmd_release);
+	if (io_req->wait_for_comp) {
+		BNX2FC_IO_DBG(io_req, "tm_compl - wake up the waiter\n");
+		complete(&io_req->tm_done);
+	}
+}
+
+static int bnx2fc_split_bd(struct bnx2fc_cmd *io_req, u64 addr, int sg_len,
+			   int bd_index)
+{
+	struct fcoe_bd_ctx *bd = io_req->bd_tbl->bd_tbl;
+	int frag_size, sg_frags;
+
+	sg_frags = 0;
+	while (sg_len) {
+		if (sg_len >= BNX2FC_BD_SPLIT_SZ)
+			frag_size = BNX2FC_BD_SPLIT_SZ;
+		else
+			frag_size = sg_len;
+		bd[bd_index + sg_frags].buf_addr_lo = addr & 0xffffffff;
+		bd[bd_index + sg_frags].buf_addr_hi  = addr >> 32;
+		bd[bd_index + sg_frags].buf_len = (u16)frag_size;
+		bd[bd_index + sg_frags].flags = 0;
+
+		addr += (u64) frag_size;
+		sg_frags++;
+		sg_len -= frag_size;
+	}
+	return sg_frags;
+
+}
+
+static int bnx2fc_map_sg(struct bnx2fc_cmd *io_req)
+{
+	struct scsi_cmnd *sc = io_req->sc_cmd;
+	struct fcoe_bd_ctx *bd = io_req->bd_tbl->bd_tbl;
+	struct scatterlist *sg;
+	int byte_count = 0;
+	int sg_count = 0;
+	int bd_count = 0;
+	int sg_frags;
+	unsigned int sg_len;
+	u64 addr;
+	int i;
+
+	sg_count = scsi_dma_map(sc);
+	scsi_for_each_sg(sc, sg, sg_count, i) {
+		sg_len = sg_dma_len(sg);
+		addr = sg_dma_address(sg);
+		if (sg_len > BNX2FC_MAX_BD_LEN) {
+			sg_frags = bnx2fc_split_bd(io_req, addr, sg_len,
+						   bd_count);
+		} else {
+
+			sg_frags = 1;
+			bd[bd_count].buf_addr_lo = addr & 0xffffffff;
+			bd[bd_count].buf_addr_hi  = addr >> 32;
+			bd[bd_count].buf_len = (u16)sg_len;
+			bd[bd_count].flags = 0;
+		}
+		bd_count += sg_frags;
+		byte_count += sg_len;
+	}
+	if (byte_count != scsi_bufflen(sc))
+		printk(KERN_ERR PFX "byte_count = %d != scsi_bufflen = %d, "
+			"task_id = 0x%x\n", byte_count, scsi_bufflen(sc),
+			io_req->xid);
+	return bd_count;
+}
+
+static void bnx2fc_build_bd_list_from_sg(struct bnx2fc_cmd *io_req)
+{
+	struct scsi_cmnd *sc = io_req->sc_cmd;
+	struct fcoe_bd_ctx *bd = io_req->bd_tbl->bd_tbl;
+	int bd_count;
+
+	if (scsi_sg_count(sc))
+		bd_count = bnx2fc_map_sg(io_req);
+	else {
+		bd_count = 0;
+		bd[0].buf_addr_lo = bd[0].buf_addr_hi = 0;
+		bd[0].buf_len = bd[0].flags = 0;
+	}
+	io_req->bd_tbl->bd_valid = bd_count;
+}
+
+static void bnx2fc_unmap_sg_list(struct bnx2fc_cmd *io_req)
+{
+	struct scsi_cmnd *sc = io_req->sc_cmd;
+
+	if (io_req->bd_tbl->bd_valid && sc) {
+		scsi_dma_unmap(sc);
+		io_req->bd_tbl->bd_valid = 0;
+	}
+}
+
+void bnx2fc_build_fcp_cmnd(struct bnx2fc_cmd *io_req,
+				  struct fcp_cmnd *fcp_cmnd)
+{
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+	char tag[2];
+
+	memset(fcp_cmnd, 0, sizeof(struct fcp_cmnd));
+
+	int_to_scsilun(sc_cmd->device->lun,
+			(struct scsi_lun *) fcp_cmnd->fc_lun);
+
+
+	fcp_cmnd->fc_dl = htonl(io_req->data_xfer_len);
+	memcpy(fcp_cmnd->fc_cdb, sc_cmd->cmnd, sc_cmd->cmd_len);
+
+	fcp_cmnd->fc_cmdref = 0;
+	fcp_cmnd->fc_pri_ta = 0;
+	fcp_cmnd->fc_tm_flags = io_req->mp_req.tm_flags;
+	fcp_cmnd->fc_flags = io_req->io_req_flags;
+
+	if (scsi_populate_tag_msg(sc_cmd, tag)) {
+		switch (tag[0]) {
+		case HEAD_OF_QUEUE_TAG:
+			fcp_cmnd->fc_pri_ta = FCP_PTA_HEADQ;
+			break;
+		case ORDERED_QUEUE_TAG:
+			fcp_cmnd->fc_pri_ta = FCP_PTA_ORDERED;
+			break;
+		default:
+			fcp_cmnd->fc_pri_ta = FCP_PTA_SIMPLE;
+			break;
+		}
+	} else {
+		fcp_cmnd->fc_pri_ta = 0;
+	}
+}
+
+static void bnx2fc_parse_fcp_rsp(struct bnx2fc_cmd *io_req,
+				 struct fcoe_fcp_rsp_payload *fcp_rsp,
+				 u8 num_rq)
+{
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	u8 rsp_flags = fcp_rsp->fcp_flags.flags;
+	u32 rq_buff_len = 0;
+	int i;
+	unsigned char *rq_data;
+	unsigned char *dummy;
+	int fcp_sns_len = 0;
+	int fcp_rsp_len = 0;
+
+	io_req->fcp_status = FC_GOOD;
+	io_req->fcp_resid = fcp_rsp->fcp_resid;
+
+	io_req->scsi_comp_flags = rsp_flags;
+	CMD_SCSI_STATUS(sc_cmd) = io_req->cdb_status =
+				fcp_rsp->scsi_status_code;
+
+	/* Fetch fcp_rsp_info and fcp_sns_info if available */
+	if (num_rq) {
+
+		/*
+		 * We do not anticipate num_rq >1, as the linux defined
+		 * SCSI_SENSE_BUFFERSIZE is 96 bytes + 8 bytes of FCP_RSP_INFO
+		 * 256 bytes of single rq buffer is good enough to hold this.
+		 */
+
+		if (rsp_flags &
+		    FCOE_FCP_RSP_FLAGS_FCP_RSP_LEN_VALID) {
+			fcp_rsp_len = rq_buff_len
+					= fcp_rsp->fcp_rsp_len;
+		}
+
+		if (rsp_flags &
+		    FCOE_FCP_RSP_FLAGS_FCP_SNS_LEN_VALID) {
+			fcp_sns_len = fcp_rsp->fcp_sns_len;
+			rq_buff_len += fcp_rsp->fcp_sns_len;
+		}
+
+		io_req->fcp_rsp_len = fcp_rsp_len;
+		io_req->fcp_sns_len = fcp_sns_len;
+
+		if (rq_buff_len > num_rq * BNX2FC_RQ_BUF_SZ) {
+			/* Invalid sense sense length. */
+			printk(KERN_ALERT PFX "invalid sns length %d\n",
+				rq_buff_len);
+			/* reset rq_buff_len */
+			rq_buff_len =  num_rq * BNX2FC_RQ_BUF_SZ;
+		}
+
+		rq_data = bnx2fc_get_next_rqe(tgt, 1);
+
+		if (num_rq > 1) {
+			/* We do not need extra sense data */
+			for (i = 1; i < num_rq; i++)
+				dummy = bnx2fc_get_next_rqe(tgt, 1);
+		}
+
+		/* fetch fcp_rsp_code */
+		if ((fcp_rsp_len == 4) || (fcp_rsp_len == 8)) {
+			/* Only for task management function */
+			io_req->fcp_rsp_code = rq_data[3];
+			printk(KERN_ERR PFX "fcp_rsp_code = %d\n",
+				io_req->fcp_rsp_code);
+		}
+
+		/* fetch sense data */
+		rq_data += fcp_rsp_len;
+
+		if (fcp_sns_len > SCSI_SENSE_BUFFERSIZE) {
+			printk(KERN_ERR PFX "Truncating sense buffer\n");
+			fcp_sns_len = SCSI_SENSE_BUFFERSIZE;
+		}
+
+		memset(sc_cmd->sense_buffer, 0, sizeof(sc_cmd->sense_buffer));
+		if (fcp_sns_len)
+			memcpy(sc_cmd->sense_buffer, rq_data, fcp_sns_len);
+
+		/* return RQ entries */
+		for (i = 0; i < num_rq; i++)
+			bnx2fc_return_rqe(tgt, 1);
+	}
+}
+
+/**
+ * bnx2fc_queuecommand - Queuecommand function of the scsi template
+ *
+ * @host:	The Scsi_Host the command was issued to
+ * @sc_cmd:	struct scsi_cmnd to be executed
+ *
+ * This is the IO strategy routine, called by SCSI-ML
+ **/
+int bnx2fc_queuecommand(struct Scsi_Host *host,
+			struct scsi_cmnd *sc_cmd)
+{
+	struct fc_lport *lport = shost_priv(host);
+	struct fc_rport *rport = starget_to_rport(scsi_target(sc_cmd->device));
+	struct fc_rport_libfc_priv *rp = rport->dd_data;
+	struct bnx2fc_rport *tgt;
+	struct bnx2fc_cmd *io_req;
+	int rc = 0;
+	int rval;
+
+	rval = fc_remote_port_chkready(rport);
+	if (rval) {
+		sc_cmd->result = rval;
+		sc_cmd->scsi_done(sc_cmd);
+		return 0;
+	}
+
+	if ((lport->state != LPORT_ST_READY) || !(lport->link_up)) {
+		rc = SCSI_MLQUEUE_HOST_BUSY;
+		goto exit_qcmd;
+	}
+
+	/* rport and tgt are allocated together, so tgt should be non-NULL */
+	tgt = (struct bnx2fc_rport *)&rp[1];
+
+	if (!test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags)) {
+		/*
+		 * Session is not offloaded yet. Let SCSI-ml retry
+		 * the command.
+		 */
+		rc = SCSI_MLQUEUE_TARGET_BUSY;
+		goto exit_qcmd;
+	}
+
+	io_req = bnx2fc_cmd_alloc(tgt);
+	if (!io_req) {
+		rc = SCSI_MLQUEUE_HOST_BUSY;
+		goto exit_qcmd;
+	}
+	io_req->sc_cmd = sc_cmd;
+
+	if (bnx2fc_post_io_req(tgt, io_req)) {
+		printk(KERN_ERR PFX "Unable to post io_req\n");
+		rc = SCSI_MLQUEUE_HOST_BUSY;
+		goto exit_qcmd;
+	}
+exit_qcmd:
+	return rc;
+}
+
+void bnx2fc_process_scsi_cmd_compl(struct bnx2fc_cmd *io_req,
+				   struct fcoe_task_ctx_entry *task,
+				   u8 num_rq)
+{
+	struct fcoe_fcp_rsp_payload *fcp_rsp;
+	struct bnx2fc_rport *tgt = io_req->tgt;
+	struct scsi_cmnd *sc_cmd;
+	struct Scsi_Host *host;
+
+
+	/* scsi_cmd_cmpl is called with tgt lock held */
+
+	if (test_and_set_bit(BNX2FC_FLAG_IO_COMPL, &io_req->req_flags)) {
+		/* we will not receive ABTS response for this IO */
+		BNX2FC_IO_DBG(io_req, "Timer context finished processing "
+			   "this scsi cmd\n");
+	}
+
+	/* Cancel the timeout_work, as we received IO completion */
+	if (cancel_delayed_work(&io_req->timeout_work))
+		kref_put(&io_req->refcount,
+			 bnx2fc_cmd_release); /* drop timer hold */
+
+	sc_cmd = io_req->sc_cmd;
+	if (sc_cmd == NULL) {
+		printk(KERN_ERR PFX "scsi_cmd_compl - sc_cmd is NULL\n");
+		return;
+	}
+
+	/* Fetch fcp_rsp from task context and perform cmd completion */
+	fcp_rsp = (struct fcoe_fcp_rsp_payload *)
+		   &(task->cmn.general.rsp_info.fcp_rsp.payload);
+
+	/* parse fcp_rsp and obtain sense data from RQ if available */
+	bnx2fc_parse_fcp_rsp(io_req, fcp_rsp, num_rq);
+
+	host = sc_cmd->device->host;
+	if (!sc_cmd->SCp.ptr) {
+		printk(KERN_ERR PFX "SCp.ptr is NULL\n");
+		return;
+	}
+	io_req->sc_cmd = NULL;
+
+	if (io_req->on_active_queue) {
+		list_del_init(&io_req->link);
+		io_req->on_active_queue = 0;
+		/* Move IO req to retire queue */
+		list_add_tail(&io_req->link, &tgt->io_retire_queue);
+	} else {
+		/* This should not happen, but could have been pulled
+		 * by bnx2fc_flush_active_ios(), or during a race
+		 * between command abort and (late) completion.
+		 */
+		BNX2FC_IO_DBG(io_req, "xid not on active_cmd_queue\n");
+		if (io_req->wait_for_comp)
+			if (test_and_clear_bit(BNX2FC_FLAG_EH_ABORT,
+					       &io_req->req_flags))
+				complete(&io_req->tm_done);
+	}
+
+	bnx2fc_unmap_sg_list(io_req);
+
+	switch (io_req->fcp_status) {
+	case FC_GOOD:
+		if (io_req->cdb_status == 0) {
+			/* Good IO completion */
+			sc_cmd->result = DID_OK << 16;
+		} else {
+			/* Transport status is good, SCSI status not good */
+			BNX2FC_IO_DBG(io_req, "scsi_cmpl: cdb_status = %d"
+				 " fcp_resid = 0x%x\n",
+				io_req->cdb_status, io_req->fcp_resid);
+			sc_cmd->result = (DID_OK << 16) | io_req->cdb_status;
+		}
+		if (io_req->fcp_resid)
+			scsi_set_resid(sc_cmd, io_req->fcp_resid);
+		break;
+	default:
+		printk(KERN_ALERT PFX "scsi_cmd_compl: fcp_status = %d\n",
+			io_req->fcp_status);
+		break;
+	}
+	sc_cmd->SCp.ptr = NULL;
+	sc_cmd->scsi_done(sc_cmd);
+	kref_put(&io_req->refcount, bnx2fc_cmd_release);
+}
+
+static int bnx2fc_post_io_req(struct bnx2fc_rport *tgt,
+			       struct bnx2fc_cmd *io_req)
+{
+	struct fcoe_task_ctx_entry *task;
+	struct fcoe_task_ctx_entry *task_page;
+	struct scsi_cmnd *sc_cmd = io_req->sc_cmd;
+	struct fcoe_port *port = tgt->port;
+	struct bnx2fc_hba *hba = port->priv;
+	struct fc_lport *lport = port->lport;
+	struct fcoe_dev_stats *stats;
+	int task_idx, index;
+	u16 xid;
+
+	/* Initialize rest of io_req fields */
+	io_req->cmd_type = BNX2FC_SCSI_CMD;
+	io_req->port = port;
+	io_req->tgt = tgt;
+	io_req->data_xfer_len = scsi_bufflen(sc_cmd);
+	sc_cmd->SCp.ptr = (char *)io_req;
+
+	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	if (sc_cmd->sc_data_direction == DMA_FROM_DEVICE) {
+		io_req->io_req_flags = BNX2FC_READ;
+		stats->InputRequests++;
+		stats->InputBytes += io_req->data_xfer_len;
+	} else if (sc_cmd->sc_data_direction == DMA_TO_DEVICE) {
+		io_req->io_req_flags = BNX2FC_WRITE;
+		stats->OutputRequests++;
+		stats->OutputBytes += io_req->data_xfer_len;
+	} else {
+		io_req->io_req_flags = 0;
+		stats->ControlRequests++;
+	}
+	put_cpu();
+
+	xid = io_req->xid;
+
+	/* Build buffer descriptor list for firmware from sg list */
+	bnx2fc_build_bd_list_from_sg(io_req);
+
+	task_idx = xid / BNX2FC_TASKS_PER_PAGE;
+	index = xid % BNX2FC_TASKS_PER_PAGE;
+
+	/* Initialize task context for this IO request */
+	task_page = (struct fcoe_task_ctx_entry *) hba->task_ctx[task_idx];
+	task = &(task_page[index]);
+	bnx2fc_init_task(io_req, task);
+
+	spin_lock_bh(&tgt->tgt_lock);
+
+	if (tgt->flush_in_prog) {
+		printk(KERN_ERR PFX "Flush in progress..Host Busy\n");
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return -EAGAIN;
+	}
+
+	if (!test_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags)) {
+		printk(KERN_ERR PFX "Session not ready...post_io\n");
+		kref_put(&io_req->refcount, bnx2fc_cmd_release);
+		spin_unlock_bh(&tgt->tgt_lock);
+		return -EAGAIN;
+	}
+
+	/* Time IO req */
+	bnx2fc_cmd_timer_set(io_req, BNX2FC_IO_TIMEOUT);
+	/* Obtain free SQ entry */
+	bnx2fc_add_2_sq(tgt, xid);
+
+	/* Enqueue the io_req to active_cmd_queue */
+
+	io_req->on_active_queue = 1;
+	/* move io_req from pending_queue to active_queue */
+	list_add_tail(&io_req->link, &tgt->active_cmd_queue);
+
+	/* Ring doorbell */
+	bnx2fc_ring_doorbell(tgt);
+	spin_unlock_bh(&tgt->tgt_lock);
+	return 0;
+}
diff --git a/drivers/scsi/bnx2fc/bnx2fc_tgt.c b/drivers/scsi/bnx2fc/bnx2fc_tgt.c
new file mode 100644
index 000000000000..7ea93af60260
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_tgt.c
@@ -0,0 +1,844 @@
+/* bnx2fc_tgt.c: Broadcom NetXtreme II Linux FCoE offload driver.
+ * Handles operations such as session offload/upload etc, and manages
+ * session resources such as connection id and qp resources.
+ *
+ * Copyright (c) 2008 - 2010 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.
+ *
+ * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
+ */
+
+#include "bnx2fc.h"
+static void bnx2fc_upld_timer(unsigned long data);
+static void bnx2fc_ofld_timer(unsigned long data);
+static int bnx2fc_init_tgt(struct bnx2fc_rport *tgt,
+			   struct fcoe_port *port,
+			   struct fc_rport_priv *rdata);
+static u32 bnx2fc_alloc_conn_id(struct bnx2fc_hba *hba,
+				struct bnx2fc_rport *tgt);
+static int bnx2fc_alloc_session_resc(struct bnx2fc_hba *hba,
+			      struct bnx2fc_rport *tgt);
+static void bnx2fc_free_session_resc(struct bnx2fc_hba *hba,
+			      struct bnx2fc_rport *tgt);
+static void bnx2fc_free_conn_id(struct bnx2fc_hba *hba, u32 conn_id);
+
+static void bnx2fc_upld_timer(unsigned long data)
+{
+
+	struct bnx2fc_rport *tgt = (struct bnx2fc_rport *)data;
+
+	BNX2FC_TGT_DBG(tgt, "upld_timer - Upload compl not received!!\n");
+	/* fake upload completion */
+	clear_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags);
+	set_bit(BNX2FC_FLAG_UPLD_REQ_COMPL, &tgt->flags);
+	wake_up_interruptible(&tgt->upld_wait);
+}
+
+static void bnx2fc_ofld_timer(unsigned long data)
+{
+
+	struct bnx2fc_rport *tgt = (struct bnx2fc_rport *)data;
+
+	BNX2FC_TGT_DBG(tgt, "entered bnx2fc_ofld_timer\n");
+	/* NOTE: This function should never be called, as
+	 * offload should never timeout
+	 */
+	/*
+	 * If the timer has expired, this session is dead
+	 * Clear offloaded flag and logout of this device.
+	 * Since OFFLOADED flag is cleared, this case
+	 * will be considered as offload error and the
+	 * port will be logged off, and conn_id, session
+	 * resources are freed up in bnx2fc_offload_session
+	 */
+	clear_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags);
+	set_bit(BNX2FC_FLAG_OFLD_REQ_CMPL, &tgt->flags);
+	wake_up_interruptible(&tgt->ofld_wait);
+}
+
+static void bnx2fc_offload_session(struct fcoe_port *port,
+					struct bnx2fc_rport *tgt,
+					struct fc_rport_priv *rdata)
+{
+	struct fc_lport *lport = rdata->local_port;
+	struct fc_rport *rport = rdata->rport;
+	struct bnx2fc_hba *hba = port->priv;
+	int rval;
+	int i = 0;
+
+	/* Initialize bnx2fc_rport */
+	/* NOTE: tgt is already bzero'd */
+	rval = bnx2fc_init_tgt(tgt, port, rdata);
+	if (rval) {
+		printk(KERN_ERR PFX "Failed to allocate conn id for "
+			"port_id (%6x)\n", rport->port_id);
+		goto ofld_err;
+	}
+
+	/* Allocate session resources */
+	rval = bnx2fc_alloc_session_resc(hba, tgt);
+	if (rval) {
+		printk(KERN_ERR PFX "Failed to allocate resources\n");
+		goto ofld_err;
+	}
+
+	/*
+	 * Initialize FCoE session offload process.
+	 * Upon completion of offload process add
+	 * rport to list of rports
+	 */
+retry_ofld:
+	clear_bit(BNX2FC_FLAG_OFLD_REQ_CMPL, &tgt->flags);
+	rval = bnx2fc_send_session_ofld_req(port, tgt);
+	if (rval) {
+		printk(KERN_ERR PFX "ofld_req failed\n");
+		goto ofld_err;
+	}
+
+	/*
+	 * wait for the session is offloaded and enabled. 3 Secs
+	 * should be ample time for this process to complete.
+	 */
+	setup_timer(&tgt->ofld_timer, bnx2fc_ofld_timer, (unsigned long)tgt);
+	mod_timer(&tgt->ofld_timer, jiffies + BNX2FC_FW_TIMEOUT);
+
+	wait_event_interruptible(tgt->ofld_wait,
+				 (test_bit(
+				  BNX2FC_FLAG_OFLD_REQ_CMPL,
+				  &tgt->flags)));
+	if (signal_pending(current))
+		flush_signals(current);
+
+	del_timer_sync(&tgt->ofld_timer);
+
+	if (!(test_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags))) {
+		if (test_and_clear_bit(BNX2FC_FLAG_CTX_ALLOC_FAILURE,
+				       &tgt->flags)) {
+			BNX2FC_TGT_DBG(tgt, "ctx_alloc_failure, "
+				"retry ofld..%d\n", i++);
+			msleep_interruptible(1000);
+			if (i > 3) {
+				i = 0;
+				goto ofld_err;
+			}
+			goto retry_ofld;
+		}
+		goto ofld_err;
+	}
+	if (bnx2fc_map_doorbell(tgt)) {
+		printk(KERN_ERR PFX "map doorbell failed - no mem\n");
+		/* upload will take care of cleaning up sess resc */
+		lport->tt.rport_logoff(rdata);
+	}
+	return;
+
+ofld_err:
+	/* couldn't offload the session. log off from this rport */
+	BNX2FC_TGT_DBG(tgt, "bnx2fc_offload_session - offload error\n");
+	lport->tt.rport_logoff(rdata);
+	/* Free session resources */
+	bnx2fc_free_session_resc(hba, tgt);
+	if (tgt->fcoe_conn_id != -1)
+		bnx2fc_free_conn_id(hba, tgt->fcoe_conn_id);
+}
+
+void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt)
+{
+	struct bnx2fc_cmd *io_req;
+	struct list_head *list;
+	struct list_head *tmp;
+	int rc;
+	int i = 0;
+	BNX2FC_TGT_DBG(tgt, "Entered flush_active_ios - %d\n",
+		       tgt->num_active_ios.counter);
+
+	spin_lock_bh(&tgt->tgt_lock);
+	tgt->flush_in_prog = 1;
+
+	list_for_each_safe(list, tmp, &tgt->active_cmd_queue) {
+		i++;
+		io_req = (struct bnx2fc_cmd *)list;
+		list_del_init(&io_req->link);
+		io_req->on_active_queue = 0;
+		BNX2FC_IO_DBG(io_req, "cmd_queue cleanup\n");
+
+		if (cancel_delayed_work(&io_req->timeout_work)) {
+			if (test_and_clear_bit(BNX2FC_FLAG_EH_ABORT,
+						&io_req->req_flags)) {
+				/* Handle eh_abort timeout */
+				BNX2FC_IO_DBG(io_req, "eh_abort for IO "
+					      "cleaned up\n");
+				complete(&io_req->tm_done);
+			}
+			kref_put(&io_req->refcount,
+				 bnx2fc_cmd_release); /* drop timer hold */
+		}
+
+		set_bit(BNX2FC_FLAG_IO_COMPL, &io_req->req_flags);
+		set_bit(BNX2FC_FLAG_IO_CLEANUP, &io_req->req_flags);
+		rc = bnx2fc_initiate_cleanup(io_req);
+		BUG_ON(rc);
+	}
+
+	list_for_each_safe(list, tmp, &tgt->els_queue) {
+		i++;
+		io_req = (struct bnx2fc_cmd *)list;
+		list_del_init(&io_req->link);
+		io_req->on_active_queue = 0;
+
+		BNX2FC_IO_DBG(io_req, "els_queue cleanup\n");
+
+		if (cancel_delayed_work(&io_req->timeout_work))
+			kref_put(&io_req->refcount,
+				 bnx2fc_cmd_release); /* drop timer hold */
+
+		if ((io_req->cb_func) && (io_req->cb_arg)) {
+			io_req->cb_func(io_req->cb_arg);
+			io_req->cb_arg = NULL;
+		}
+
+		rc = bnx2fc_initiate_cleanup(io_req);
+		BUG_ON(rc);
+	}
+
+	list_for_each_safe(list, tmp, &tgt->io_retire_queue) {
+		i++;
+		io_req = (struct bnx2fc_cmd *)list;
+		list_del_init(&io_req->link);
+
+		BNX2FC_IO_DBG(io_req, "retire_queue flush\n");
+
+		if (cancel_delayed_work(&io_req->timeout_work))
+			kref_put(&io_req->refcount, bnx2fc_cmd_release);
+
+		clear_bit(BNX2FC_FLAG_ISSUE_RRQ, &io_req->req_flags);
+	}
+
+	BNX2FC_TGT_DBG(tgt, "IOs flushed = %d\n", i);
+	i = 0;
+	spin_unlock_bh(&tgt->tgt_lock);
+	/* wait for active_ios to go to 0 */
+	while ((tgt->num_active_ios.counter != 0) && (i++ < BNX2FC_WAIT_CNT))
+		msleep(25);
+	if (tgt->num_active_ios.counter != 0)
+		printk(KERN_ERR PFX "CLEANUP on port 0x%x:"
+				    " active_ios = %d\n",
+			tgt->rdata->ids.port_id, tgt->num_active_ios.counter);
+	spin_lock_bh(&tgt->tgt_lock);
+	tgt->flush_in_prog = 0;
+	spin_unlock_bh(&tgt->tgt_lock);
+}
+
+static void bnx2fc_upload_session(struct fcoe_port *port,
+					struct bnx2fc_rport *tgt)
+{
+	struct bnx2fc_hba *hba = port->priv;
+
+	BNX2FC_TGT_DBG(tgt, "upload_session: active_ios = %d\n",
+		tgt->num_active_ios.counter);
+
+	/*
+	 * Called with hba->hba_mutex held.
+	 * This is a blocking call
+	 */
+	clear_bit(BNX2FC_FLAG_UPLD_REQ_COMPL, &tgt->flags);
+	bnx2fc_send_session_disable_req(port, tgt);
+
+	/*
+	 * wait for upload to complete. 3 Secs
+	 * should be sufficient time for this process to complete.
+	 */
+	setup_timer(&tgt->upld_timer, bnx2fc_upld_timer, (unsigned long)tgt);
+	mod_timer(&tgt->upld_timer, jiffies + BNX2FC_FW_TIMEOUT);
+
+	BNX2FC_TGT_DBG(tgt, "waiting for disable compl\n");
+	wait_event_interruptible(tgt->upld_wait,
+				 (test_bit(
+				  BNX2FC_FLAG_UPLD_REQ_COMPL,
+				  &tgt->flags)));
+
+	if (signal_pending(current))
+		flush_signals(current);
+
+	del_timer_sync(&tgt->upld_timer);
+
+	/*
+	 * traverse thru the active_q and tmf_q and cleanup
+	 * IOs in these lists
+	 */
+	BNX2FC_TGT_DBG(tgt, "flush/upload - disable wait flags = 0x%lx\n",
+		       tgt->flags);
+	bnx2fc_flush_active_ios(tgt);
+
+	/* Issue destroy KWQE */
+	if (test_bit(BNX2FC_FLAG_DISABLED, &tgt->flags)) {
+		BNX2FC_TGT_DBG(tgt, "send destroy req\n");
+		clear_bit(BNX2FC_FLAG_UPLD_REQ_COMPL, &tgt->flags);
+		bnx2fc_send_session_destroy_req(hba, tgt);
+
+		/* wait for destroy to complete */
+		setup_timer(&tgt->upld_timer,
+			    bnx2fc_upld_timer, (unsigned long)tgt);
+		mod_timer(&tgt->upld_timer, jiffies + BNX2FC_FW_TIMEOUT);
+
+		wait_event_interruptible(tgt->upld_wait,
+					 (test_bit(
+					  BNX2FC_FLAG_UPLD_REQ_COMPL,
+					  &tgt->flags)));
+
+		if (!(test_bit(BNX2FC_FLAG_DESTROYED, &tgt->flags)))
+			printk(KERN_ERR PFX "ERROR!! destroy timed out\n");
+
+		BNX2FC_TGT_DBG(tgt, "destroy wait complete flags = 0x%lx\n",
+			tgt->flags);
+		if (signal_pending(current))
+			flush_signals(current);
+
+		del_timer_sync(&tgt->upld_timer);
+
+	} else
+		printk(KERN_ERR PFX "ERROR!! DISABLE req timed out, destroy"
+				" not sent to FW\n");
+
+	/* Free session resources */
+	spin_lock_bh(&tgt->cq_lock);
+	bnx2fc_free_session_resc(hba, tgt);
+	bnx2fc_free_conn_id(hba, tgt->fcoe_conn_id);
+	spin_unlock_bh(&tgt->cq_lock);
+}
+
+static int bnx2fc_init_tgt(struct bnx2fc_rport *tgt,
+			   struct fcoe_port *port,
+			   struct fc_rport_priv *rdata)
+{
+
+	struct fc_rport *rport = rdata->rport;
+	struct bnx2fc_hba *hba = port->priv;
+
+	tgt->rport = rport;
+	tgt->rdata = rdata;
+	tgt->port = port;
+
+	if (hba->num_ofld_sess >= BNX2FC_NUM_MAX_SESS) {
+		BNX2FC_TGT_DBG(tgt, "exceeded max sessions. logoff this tgt\n");
+		tgt->fcoe_conn_id = -1;
+		return -1;
+	}
+
+	tgt->fcoe_conn_id = bnx2fc_alloc_conn_id(hba, tgt);
+	if (tgt->fcoe_conn_id == -1)
+		return -1;
+
+	BNX2FC_TGT_DBG(tgt, "init_tgt - conn_id = 0x%x\n", tgt->fcoe_conn_id);
+
+	tgt->max_sqes = BNX2FC_SQ_WQES_MAX;
+	tgt->max_rqes = BNX2FC_RQ_WQES_MAX;
+	tgt->max_cqes = BNX2FC_CQ_WQES_MAX;
+
+	/* Initialize the toggle bit */
+	tgt->sq_curr_toggle_bit = 1;
+	tgt->cq_curr_toggle_bit = 1;
+	tgt->sq_prod_idx = 0;
+	tgt->cq_cons_idx = 0;
+	tgt->rq_prod_idx = 0x8000;
+	tgt->rq_cons_idx = 0;
+	atomic_set(&tgt->num_active_ios, 0);
+
+	tgt->work_time_slice = 2;
+
+	spin_lock_init(&tgt->tgt_lock);
+	spin_lock_init(&tgt->cq_lock);
+
+	/* Initialize active_cmd_queue list */
+	INIT_LIST_HEAD(&tgt->active_cmd_queue);
+
+	/* Initialize IO retire queue */
+	INIT_LIST_HEAD(&tgt->io_retire_queue);
+
+	INIT_LIST_HEAD(&tgt->els_queue);
+
+	/* Initialize active_tm_queue list */
+	INIT_LIST_HEAD(&tgt->active_tm_queue);
+
+	init_waitqueue_head(&tgt->ofld_wait);
+	init_waitqueue_head(&tgt->upld_wait);
+
+	return 0;
+}
+
+/**
+ * This event_callback is called after successful completion of libfc
+ * initiated target login. bnx2fc can proceed with initiating the session
+ * establishment.
+ */
+void bnx2fc_rport_event_handler(struct fc_lport *lport,
+				struct fc_rport_priv *rdata,
+				enum fc_rport_event event)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	struct bnx2fc_hba *hba = port->priv;
+	struct fc_rport *rport = rdata->rport;
+	struct fc_rport_libfc_priv *rp;
+	struct bnx2fc_rport *tgt;
+	u32 port_id;
+
+	BNX2FC_HBA_DBG(lport, "rport_event_hdlr: event = %d, port_id = 0x%x\n",
+		event, rdata->ids.port_id);
+	switch (event) {
+	case RPORT_EV_READY:
+		if (!rport) {
+			printk(KERN_ALERT PFX "rport is NULL: ERROR!\n");
+			break;
+		}
+
+		rp = rport->dd_data;
+		if (rport->port_id == FC_FID_DIR_SERV) {
+			/*
+			 * bnx2fc_rport structure doesnt exist for
+			 * directory server.
+			 * We should not come here, as lport will
+			 * take care of fabric login
+			 */
+			printk(KERN_ALERT PFX "%x - rport_event_handler ERROR\n",
+				rdata->ids.port_id);
+			break;
+		}
+
+		if (rdata->spp_type != FC_TYPE_FCP) {
+			BNX2FC_HBA_DBG(lport, "not FCP type target."
+				   " not offloading\n");
+			break;
+		}
+		if (!(rdata->ids.roles & FC_RPORT_ROLE_FCP_TARGET)) {
+			BNX2FC_HBA_DBG(lport, "not FCP_TARGET"
+				   " not offloading\n");
+			break;
+		}
+
+		/*
+		 * Offlaod process is protected with hba mutex.
+		 * Use the same mutex_lock for upload process too
+		 */
+		mutex_lock(&hba->hba_mutex);
+		tgt = (struct bnx2fc_rport *)&rp[1];
+
+		/* This can happen when ADISC finds the same target */
+		if (test_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags)) {
+			BNX2FC_TGT_DBG(tgt, "already offloaded\n");
+			mutex_unlock(&hba->hba_mutex);
+			return;
+		}
+
+		/*
+		 * Offload the session. This is a blocking call, and will
+		 * wait until the session is offloaded.
+		 */
+		bnx2fc_offload_session(port, tgt, rdata);
+
+		BNX2FC_TGT_DBG(tgt, "OFFLOAD num_ofld_sess = %d\n",
+			hba->num_ofld_sess);
+
+		if (test_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags)) {
+			/*
+			 * Session is offloaded and enabled. Map
+			 * doorbell register for this target
+			 */
+			BNX2FC_TGT_DBG(tgt, "sess offloaded\n");
+			/* This counter is protected with hba mutex */
+			hba->num_ofld_sess++;
+
+			set_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags);
+		} else {
+			/*
+			 * Offload or enable would have failed.
+			 * In offload/enable completion path, the
+			 * rport would have already been removed
+			 */
+			BNX2FC_TGT_DBG(tgt, "Port is being logged off as "
+				   "offloaded flag not set\n");
+		}
+		mutex_unlock(&hba->hba_mutex);
+		break;
+	case RPORT_EV_LOGO:
+	case RPORT_EV_FAILED:
+	case RPORT_EV_STOP:
+		port_id = rdata->ids.port_id;
+		if (port_id == FC_FID_DIR_SERV)
+			break;
+
+		if (!rport) {
+			printk(KERN_ALERT PFX "%x - rport not created Yet!!\n",
+				port_id);
+			break;
+		}
+		rp = rport->dd_data;
+		mutex_lock(&hba->hba_mutex);
+		/*
+		 * Perform session upload. Note that rdata->peers is already
+		 * removed from disc->rports list before we get this event.
+		 */
+		tgt = (struct bnx2fc_rport *)&rp[1];
+
+		if (!(test_bit(BNX2FC_FLAG_OFFLOADED, &tgt->flags))) {
+			mutex_unlock(&hba->hba_mutex);
+			break;
+		}
+		clear_bit(BNX2FC_FLAG_SESSION_READY, &tgt->flags);
+
+		bnx2fc_upload_session(port, tgt);
+		hba->num_ofld_sess--;
+		BNX2FC_TGT_DBG(tgt, "UPLOAD num_ofld_sess = %d\n",
+			hba->num_ofld_sess);
+		/*
+		 * Try to wake up the linkdown wait thread. If num_ofld_sess
+		 * is 0, the waiting therad wakes up
+		 */
+		if ((hba->wait_for_link_down) &&
+		    (hba->num_ofld_sess == 0)) {
+			wake_up_interruptible(&hba->shutdown_wait);
+		}
+		if (test_bit(BNX2FC_FLAG_EXPL_LOGO, &tgt->flags)) {
+			printk(KERN_ERR PFX "Relogin to the tgt\n");
+			mutex_lock(&lport->disc.disc_mutex);
+			lport->tt.rport_login(rdata);
+			mutex_unlock(&lport->disc.disc_mutex);
+		}
+		mutex_unlock(&hba->hba_mutex);
+
+		break;
+
+	case RPORT_EV_NONE:
+		break;
+	}
+}
+
+/**
+ * bnx2fc_tgt_lookup() - Lookup a bnx2fc_rport by port_id
+ *
+ * @port:  fcoe_port struct to lookup the target port on
+ * @port_id: The remote port ID to look up
+ */
+struct bnx2fc_rport *bnx2fc_tgt_lookup(struct fcoe_port *port,
+					     u32 port_id)
+{
+	struct bnx2fc_hba *hba = port->priv;
+	struct bnx2fc_rport *tgt;
+	struct fc_rport_priv *rdata;
+	int i;
+
+	for (i = 0; i < BNX2FC_NUM_MAX_SESS; i++) {
+		tgt = hba->tgt_ofld_list[i];
+		if ((tgt) && (tgt->port == port)) {
+			rdata = tgt->rdata;
+			if (rdata->ids.port_id == port_id) {
+				if (rdata->rp_state != RPORT_ST_DELETE) {
+					BNX2FC_TGT_DBG(tgt, "rport "
+						"obtained\n");
+					return tgt;
+				} else {
+					printk(KERN_ERR PFX "rport 0x%x "
+						"is in DELETED state\n",
+						rdata->ids.port_id);
+					return NULL;
+				}
+			}
+		}
+	}
+	return NULL;
+}
+
+
+/**
+ * bnx2fc_alloc_conn_id - allocates FCOE Connection id
+ *
+ * @hba:	pointer to adapter structure
+ * @tgt:	pointer to bnx2fc_rport structure
+ */
+static u32 bnx2fc_alloc_conn_id(struct bnx2fc_hba *hba,
+				struct bnx2fc_rport *tgt)
+{
+	u32 conn_id, next;
+
+	/* called with hba mutex held */
+
+	/*
+	 * tgt_ofld_list access is synchronized using
+	 * both hba mutex and hba lock. Atleast hba mutex or
+	 * hba lock needs to be held for read access.
+	 */
+
+	spin_lock_bh(&hba->hba_lock);
+	next = hba->next_conn_id;
+	conn_id = hba->next_conn_id++;
+	if (hba->next_conn_id == BNX2FC_NUM_MAX_SESS)
+		hba->next_conn_id = 0;
+
+	while (hba->tgt_ofld_list[conn_id] != NULL) {
+		conn_id++;
+		if (conn_id == BNX2FC_NUM_MAX_SESS)
+			conn_id = 0;
+
+		if (conn_id == next) {
+			/* No free conn_ids are available */
+			spin_unlock_bh(&hba->hba_lock);
+			return -1;
+		}
+	}
+	hba->tgt_ofld_list[conn_id] = tgt;
+	tgt->fcoe_conn_id = conn_id;
+	spin_unlock_bh(&hba->hba_lock);
+	return conn_id;
+}
+
+static void bnx2fc_free_conn_id(struct bnx2fc_hba *hba, u32 conn_id)
+{
+	/* called with hba mutex held */
+	spin_lock_bh(&hba->hba_lock);
+	hba->tgt_ofld_list[conn_id] = NULL;
+	hba->next_conn_id = conn_id;
+	spin_unlock_bh(&hba->hba_lock);
+}
+
+/**
+ *bnx2fc_alloc_session_resc - Allocate qp resources for the session
+ *
+ */
+static int bnx2fc_alloc_session_resc(struct bnx2fc_hba *hba,
+					struct bnx2fc_rport *tgt)
+{
+	dma_addr_t page;
+	int num_pages;
+	u32 *pbl;
+
+	/* Allocate and map SQ */
+	tgt->sq_mem_size = tgt->max_sqes * BNX2FC_SQ_WQE_SIZE;
+	tgt->sq_mem_size = (tgt->sq_mem_size + (PAGE_SIZE - 1)) & PAGE_MASK;
+
+	tgt->sq = dma_alloc_coherent(&hba->pcidev->dev, tgt->sq_mem_size,
+				     &tgt->sq_dma, GFP_KERNEL);
+	if (!tgt->sq) {
+		printk(KERN_ALERT PFX "unable to allocate SQ memory %d\n",
+			tgt->sq_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->sq, 0, tgt->sq_mem_size);
+
+	/* Allocate and map CQ */
+	tgt->cq_mem_size = tgt->max_cqes * BNX2FC_CQ_WQE_SIZE;
+	tgt->cq_mem_size = (tgt->cq_mem_size + (PAGE_SIZE - 1)) & PAGE_MASK;
+
+	tgt->cq = dma_alloc_coherent(&hba->pcidev->dev, tgt->cq_mem_size,
+				     &tgt->cq_dma, GFP_KERNEL);
+	if (!tgt->cq) {
+		printk(KERN_ALERT PFX "unable to allocate CQ memory %d\n",
+			tgt->cq_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->cq, 0, tgt->cq_mem_size);
+
+	/* Allocate and map RQ and RQ PBL */
+	tgt->rq_mem_size = tgt->max_rqes * BNX2FC_RQ_WQE_SIZE;
+	tgt->rq_mem_size = (tgt->rq_mem_size + (PAGE_SIZE - 1)) & PAGE_MASK;
+
+	tgt->rq = dma_alloc_coherent(&hba->pcidev->dev, tgt->rq_mem_size,
+					&tgt->rq_dma, GFP_KERNEL);
+	if (!tgt->rq) {
+		printk(KERN_ALERT PFX "unable to allocate RQ memory %d\n",
+			tgt->rq_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->rq, 0, tgt->rq_mem_size);
+
+	tgt->rq_pbl_size = (tgt->rq_mem_size / PAGE_SIZE) * sizeof(void *);
+	tgt->rq_pbl_size = (tgt->rq_pbl_size + (PAGE_SIZE - 1)) & PAGE_MASK;
+
+	tgt->rq_pbl = dma_alloc_coherent(&hba->pcidev->dev, tgt->rq_pbl_size,
+					 &tgt->rq_pbl_dma, GFP_KERNEL);
+	if (!tgt->rq_pbl) {
+		printk(KERN_ALERT PFX "unable to allocate RQ PBL %d\n",
+			tgt->rq_pbl_size);
+		goto mem_alloc_failure;
+	}
+
+	memset(tgt->rq_pbl, 0, tgt->rq_pbl_size);
+	num_pages = tgt->rq_mem_size / PAGE_SIZE;
+	page = tgt->rq_dma;
+	pbl = (u32 *)tgt->rq_pbl;
+
+	while (num_pages--) {
+		*pbl = (u32)page;
+		pbl++;
+		*pbl = (u32)((u64)page >> 32);
+		pbl++;
+		page += PAGE_SIZE;
+	}
+
+	/* Allocate and map XFERQ */
+	tgt->xferq_mem_size = tgt->max_sqes * BNX2FC_XFERQ_WQE_SIZE;
+	tgt->xferq_mem_size = (tgt->xferq_mem_size + (PAGE_SIZE - 1)) &
+			       PAGE_MASK;
+
+	tgt->xferq = dma_alloc_coherent(&hba->pcidev->dev, tgt->xferq_mem_size,
+					&tgt->xferq_dma, GFP_KERNEL);
+	if (!tgt->xferq) {
+		printk(KERN_ALERT PFX "unable to allocate XFERQ %d\n",
+			tgt->xferq_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->xferq, 0, tgt->xferq_mem_size);
+
+	/* Allocate and map CONFQ & CONFQ PBL */
+	tgt->confq_mem_size = tgt->max_sqes * BNX2FC_CONFQ_WQE_SIZE;
+	tgt->confq_mem_size = (tgt->confq_mem_size + (PAGE_SIZE - 1)) &
+			       PAGE_MASK;
+
+	tgt->confq = dma_alloc_coherent(&hba->pcidev->dev, tgt->confq_mem_size,
+					&tgt->confq_dma, GFP_KERNEL);
+	if (!tgt->confq) {
+		printk(KERN_ALERT PFX "unable to allocate CONFQ %d\n",
+			tgt->confq_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->confq, 0, tgt->confq_mem_size);
+
+	tgt->confq_pbl_size =
+		(tgt->confq_mem_size / PAGE_SIZE) * sizeof(void *);
+	tgt->confq_pbl_size =
+		(tgt->confq_pbl_size + (PAGE_SIZE - 1)) & PAGE_MASK;
+
+	tgt->confq_pbl = dma_alloc_coherent(&hba->pcidev->dev,
+					    tgt->confq_pbl_size,
+					    &tgt->confq_pbl_dma, GFP_KERNEL);
+	if (!tgt->confq_pbl) {
+		printk(KERN_ALERT PFX "unable to allocate CONFQ PBL %d\n",
+			tgt->confq_pbl_size);
+		goto mem_alloc_failure;
+	}
+
+	memset(tgt->confq_pbl, 0, tgt->confq_pbl_size);
+	num_pages = tgt->confq_mem_size / PAGE_SIZE;
+	page = tgt->confq_dma;
+	pbl = (u32 *)tgt->confq_pbl;
+
+	while (num_pages--) {
+		*pbl = (u32)page;
+		pbl++;
+		*pbl = (u32)((u64)page >> 32);
+		pbl++;
+		page += PAGE_SIZE;
+	}
+
+	/* Allocate and map ConnDB */
+	tgt->conn_db_mem_size = sizeof(struct fcoe_conn_db);
+
+	tgt->conn_db = dma_alloc_coherent(&hba->pcidev->dev,
+					  tgt->conn_db_mem_size,
+					  &tgt->conn_db_dma, GFP_KERNEL);
+	if (!tgt->conn_db) {
+		printk(KERN_ALERT PFX "unable to allocate conn_db %d\n",
+						tgt->conn_db_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->conn_db, 0, tgt->conn_db_mem_size);
+
+
+	/* Allocate and map LCQ */
+	tgt->lcq_mem_size = (tgt->max_sqes + 8) * BNX2FC_SQ_WQE_SIZE;
+	tgt->lcq_mem_size = (tgt->lcq_mem_size + (PAGE_SIZE - 1)) &
+			     PAGE_MASK;
+
+	tgt->lcq = dma_alloc_coherent(&hba->pcidev->dev, tgt->lcq_mem_size,
+				      &tgt->lcq_dma, GFP_KERNEL);
+
+	if (!tgt->lcq) {
+		printk(KERN_ALERT PFX "unable to allocate lcq %d\n",
+		       tgt->lcq_mem_size);
+		goto mem_alloc_failure;
+	}
+	memset(tgt->lcq, 0, tgt->lcq_mem_size);
+
+	/* Arm CQ */
+	tgt->conn_db->cq_arm.lo = -1;
+	tgt->conn_db->rq_prod = 0x8000;
+
+	return 0;
+
+mem_alloc_failure:
+	bnx2fc_free_session_resc(hba, tgt);
+	bnx2fc_free_conn_id(hba, tgt->fcoe_conn_id);
+	return -ENOMEM;
+}
+
+/**
+ * bnx2i_free_session_resc - free qp resources for the session
+ *
+ * @hba:	adapter structure pointer
+ * @tgt:	bnx2fc_rport structure pointer
+ *
+ * Free QP resources - SQ/RQ/CQ/XFERQ memory and PBL
+ */
+static void bnx2fc_free_session_resc(struct bnx2fc_hba *hba,
+						struct bnx2fc_rport *tgt)
+{
+	BNX2FC_TGT_DBG(tgt, "Freeing up session resources\n");
+
+	if (tgt->ctx_base) {
+		iounmap(tgt->ctx_base);
+		tgt->ctx_base = NULL;
+	}
+	/* Free LCQ */
+	if (tgt->lcq) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->lcq_mem_size,
+				    tgt->lcq, tgt->lcq_dma);
+		tgt->lcq = NULL;
+	}
+	/* Free connDB */
+	if (tgt->conn_db) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->conn_db_mem_size,
+				    tgt->conn_db, tgt->conn_db_dma);
+		tgt->conn_db = NULL;
+	}
+	/* Free confq  and confq pbl */
+	if (tgt->confq_pbl) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->confq_pbl_size,
+				    tgt->confq_pbl, tgt->confq_pbl_dma);
+		tgt->confq_pbl = NULL;
+	}
+	if (tgt->confq) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->confq_mem_size,
+				    tgt->confq, tgt->confq_dma);
+		tgt->confq = NULL;
+	}
+	/* Free XFERQ */
+	if (tgt->xferq) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->xferq_mem_size,
+				    tgt->xferq, tgt->xferq_dma);
+		tgt->xferq = NULL;
+	}
+	/* Free RQ PBL and RQ */
+	if (tgt->rq_pbl) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->rq_pbl_size,
+				    tgt->rq_pbl, tgt->rq_pbl_dma);
+		tgt->rq_pbl = NULL;
+	}
+	if (tgt->rq) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->rq_mem_size,
+				    tgt->rq, tgt->rq_dma);
+		tgt->rq = NULL;
+	}
+	/* Free CQ */
+	if (tgt->cq) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->cq_mem_size,
+				    tgt->cq, tgt->cq_dma);
+		tgt->cq = NULL;
+	}
+	/* Free SQ */
+	if (tgt->sq) {
+		dma_free_coherent(&hba->pcidev->dev, tgt->sq_mem_size,
+				    tgt->sq, tgt->sq_dma);
+		tgt->sq = NULL;
+	}
+}
diff --git a/drivers/scsi/bnx2i/bnx2i.h b/drivers/scsi/bnx2i/bnx2i.h
index e1ca5fe7e6bb..cfd59023227b 100644
--- a/drivers/scsi/bnx2i/bnx2i.h
+++ b/drivers/scsi/bnx2i/bnx2i.h
@@ -360,7 +360,7 @@ struct bnx2i_hba {
 		#define ADAPTER_STATE_LINK_DOWN		2
 		#define ADAPTER_STATE_INIT_FAILED	31
 	unsigned int mtu_supported;
-		#define BNX2I_MAX_MTU_SUPPORTED		1500
+		#define BNX2I_MAX_MTU_SUPPORTED		9000
 
 	struct Scsi_Host *shost;
 
@@ -751,6 +751,8 @@ extern int bnx2i_send_iscsi_login(struct bnx2i_conn *conn,
 				  struct iscsi_task *mtask);
 extern int bnx2i_send_iscsi_tmf(struct bnx2i_conn *conn,
 				  struct iscsi_task *mtask);
+extern int bnx2i_send_iscsi_text(struct bnx2i_conn *conn,
+				 struct iscsi_task *mtask);
 extern int bnx2i_send_iscsi_scsicmd(struct bnx2i_conn *conn,
 				    struct bnx2i_cmd *cmnd);
 extern int bnx2i_send_iscsi_nopout(struct bnx2i_conn *conn,
diff --git a/drivers/scsi/bnx2i/bnx2i_hwi.c b/drivers/scsi/bnx2i/bnx2i_hwi.c
index 96505e3ab986..1da34c019b8a 100644
--- a/drivers/scsi/bnx2i/bnx2i_hwi.c
+++ b/drivers/scsi/bnx2i/bnx2i_hwi.c
@@ -445,6 +445,56 @@ int bnx2i_send_iscsi_tmf(struct bnx2i_conn *bnx2i_conn,
 }
 
 /**
+ * bnx2i_send_iscsi_text - post iSCSI text WQE to hardware
+ * @conn:	iscsi connection
+ * @mtask:	driver command structure which is requesting
+ *		a WQE to sent to chip for further processing
+ *
+ * prepare and post an iSCSI Text request WQE to CNIC firmware
+ */
+int bnx2i_send_iscsi_text(struct bnx2i_conn *bnx2i_conn,
+			  struct iscsi_task *mtask)
+{
+	struct bnx2i_cmd *bnx2i_cmd;
+	struct bnx2i_text_request *text_wqe;
+	struct iscsi_text *text_hdr;
+	u32 dword;
+
+	bnx2i_cmd = (struct bnx2i_cmd *)mtask->dd_data;
+	text_hdr = (struct iscsi_text *)mtask->hdr;
+	text_wqe = (struct bnx2i_text_request *) bnx2i_conn->ep->qp.sq_prod_qe;
+
+	memset(text_wqe, 0, sizeof(struct bnx2i_text_request));
+
+	text_wqe->op_code = text_hdr->opcode;
+	text_wqe->op_attr = text_hdr->flags;
+	text_wqe->data_length = ntoh24(text_hdr->dlength);
+	text_wqe->itt = mtask->itt |
+		(ISCSI_TASK_TYPE_MPATH << ISCSI_TEXT_REQUEST_TYPE_SHIFT);
+	text_wqe->ttt = be32_to_cpu(text_hdr->ttt);
+
+	text_wqe->cmd_sn = be32_to_cpu(text_hdr->cmdsn);
+
+	text_wqe->resp_bd_list_addr_lo = (u32) bnx2i_conn->gen_pdu.resp_bd_dma;
+	text_wqe->resp_bd_list_addr_hi =
+			(u32) ((u64) bnx2i_conn->gen_pdu.resp_bd_dma >> 32);
+
+	dword = ((1 << ISCSI_TEXT_REQUEST_NUM_RESP_BDS_SHIFT) |
+		 (bnx2i_conn->gen_pdu.resp_buf_size <<
+		  ISCSI_TEXT_REQUEST_RESP_BUFFER_LENGTH_SHIFT));
+	text_wqe->resp_buffer = dword;
+	text_wqe->bd_list_addr_lo = (u32) bnx2i_conn->gen_pdu.req_bd_dma;
+	text_wqe->bd_list_addr_hi =
+			(u32) ((u64) bnx2i_conn->gen_pdu.req_bd_dma >> 32);
+	text_wqe->num_bds = 1;
+	text_wqe->cq_index = 0; /* CQ# used for completion, 5771x only */
+
+	bnx2i_ring_dbell_update_sq_params(bnx2i_conn, 1);
+	return 0;
+}
+
+
+/**
  * bnx2i_send_iscsi_scsicmd - post iSCSI scsicmd request WQE to hardware
  * @conn:	iscsi connection
  * @cmd:	driver command structure which is requesting
@@ -490,15 +540,18 @@ int bnx2i_send_iscsi_nopout(struct bnx2i_conn *bnx2i_conn,
 	bnx2i_cmd = (struct bnx2i_cmd *)task->dd_data;
 	nopout_hdr = (struct iscsi_nopout *)task->hdr;
 	nopout_wqe = (struct bnx2i_nop_out_request *)ep->qp.sq_prod_qe;
+
+	memset(nopout_wqe, 0x00, sizeof(struct bnx2i_nop_out_request));
+
 	nopout_wqe->op_code = nopout_hdr->opcode;
 	nopout_wqe->op_attr = ISCSI_FLAG_CMD_FINAL;
 	memcpy(nopout_wqe->lun, nopout_hdr->lun, 8);
 
 	if (test_bit(BNX2I_NX2_DEV_57710, &ep->hba->cnic_dev_type)) {
-		u32 tmp = nopout_hdr->lun[0];
+		u32 tmp = nopout_wqe->lun[0];
 		/* 57710 requires LUN field to be swapped */
-		nopout_hdr->lun[0] = nopout_hdr->lun[1];
-		nopout_hdr->lun[1] = tmp;
+		nopout_wqe->lun[0] = nopout_wqe->lun[1];
+		nopout_wqe->lun[1] = tmp;
 	}
 
 	nopout_wqe->itt = ((u16)task->itt |
@@ -1425,6 +1478,68 @@ done:
 	return 0;
 }
 
+
+/**
+ * bnx2i_process_text_resp - this function handles iscsi text response
+ * @session:	iscsi session pointer
+ * @bnx2i_conn:	iscsi connection pointer
+ * @cqe:	pointer to newly DMA'ed CQE entry for processing
+ *
+ * process iSCSI Text Response CQE&  complete it to open-iscsi user daemon
+ */
+static int bnx2i_process_text_resp(struct iscsi_session *session,
+				   struct bnx2i_conn *bnx2i_conn,
+				   struct cqe *cqe)
+{
+	struct iscsi_conn *conn = bnx2i_conn->cls_conn->dd_data;
+	struct iscsi_task *task;
+	struct bnx2i_text_response *text;
+	struct iscsi_text_rsp *resp_hdr;
+	int pld_len;
+	int pad_len;
+
+	text = (struct bnx2i_text_response *) cqe;
+	spin_lock(&session->lock);
+	task = iscsi_itt_to_task(conn, text->itt & ISCSI_LOGIN_RESPONSE_INDEX);
+	if (!task)
+		goto done;
+
+	resp_hdr = (struct iscsi_text_rsp *)&bnx2i_conn->gen_pdu.resp_hdr;
+	memset(resp_hdr, 0, sizeof(struct iscsi_hdr));
+	resp_hdr->opcode = text->op_code;
+	resp_hdr->flags = text->response_flags;
+	resp_hdr->hlength = 0;
+
+	hton24(resp_hdr->dlength, text->data_length);
+	resp_hdr->itt = task->hdr->itt;
+	resp_hdr->ttt = cpu_to_be32(text->ttt);
+	resp_hdr->statsn = task->hdr->exp_statsn;
+	resp_hdr->exp_cmdsn = cpu_to_be32(text->exp_cmd_sn);
+	resp_hdr->max_cmdsn = cpu_to_be32(text->max_cmd_sn);
+	pld_len = text->data_length;
+	bnx2i_conn->gen_pdu.resp_wr_ptr = bnx2i_conn->gen_pdu.resp_buf +
+					  pld_len;
+	pad_len = 0;
+	if (pld_len & 0x3)
+		pad_len = 4 - (pld_len % 4);
+
+	if (pad_len) {
+		int i = 0;
+		for (i = 0; i < pad_len; i++) {
+			bnx2i_conn->gen_pdu.resp_wr_ptr[0] = 0;
+			bnx2i_conn->gen_pdu.resp_wr_ptr++;
+		}
+	}
+	__iscsi_complete_pdu(conn, (struct iscsi_hdr *)resp_hdr,
+			     bnx2i_conn->gen_pdu.resp_buf,
+			     bnx2i_conn->gen_pdu.resp_wr_ptr -
+			     bnx2i_conn->gen_pdu.resp_buf);
+done:
+	spin_unlock(&session->lock);
+	return 0;
+}
+
+
 /**
  * bnx2i_process_tmf_resp - this function handles iscsi TMF response
  * @session:		iscsi session pointer
@@ -1766,6 +1881,10 @@ static void bnx2i_process_new_cqes(struct bnx2i_conn *bnx2i_conn)
 			bnx2i_process_tmf_resp(session, bnx2i_conn,
 					       qp->cq_cons_qe);
 			break;
+		case ISCSI_OP_TEXT_RSP:
+			bnx2i_process_text_resp(session, bnx2i_conn,
+						qp->cq_cons_qe);
+			break;
 		case ISCSI_OP_LOGOUT_RSP:
 			bnx2i_process_logout_resp(session, bnx2i_conn,
 						  qp->cq_cons_qe);
diff --git a/drivers/scsi/bnx2i/bnx2i_init.c b/drivers/scsi/bnx2i/bnx2i_init.c
index 72a7b2d4a439..1d24a2819736 100644
--- a/drivers/scsi/bnx2i/bnx2i_init.c
+++ b/drivers/scsi/bnx2i/bnx2i_init.c
@@ -18,8 +18,8 @@ static struct list_head adapter_list = LIST_HEAD_INIT(adapter_list);
 static u32 adapter_count;
 
 #define DRV_MODULE_NAME		"bnx2i"
-#define DRV_MODULE_VERSION	"2.6.2.2"
-#define DRV_MODULE_RELDATE	"Nov 23, 2010"
+#define DRV_MODULE_VERSION	"2.6.2.3"
+#define DRV_MODULE_RELDATE	"Dec 31, 2010"
 
 static char version[] __devinitdata =
 		"Broadcom NetXtreme II iSCSI Driver " DRV_MODULE_NAME \
@@ -29,7 +29,7 @@ static char version[] __devinitdata =
 MODULE_AUTHOR("Anil Veerabhadrappa <anilgv@broadcom.com> and "
 	      "Eddie Wai <eddie.wai@broadcom.com>");
 
-MODULE_DESCRIPTION("Broadcom NetXtreme II BCM5706/5708/5709/57710/57711"
+MODULE_DESCRIPTION("Broadcom NetXtreme II BCM5706/5708/5709/57710/57711/57712"
 		   " iSCSI Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_MODULE_VERSION);
@@ -88,9 +88,11 @@ void bnx2i_identify_device(struct bnx2i_hba *hba)
 	    (hba->pci_did == PCI_DEVICE_ID_NX2_5709S)) {
 		set_bit(BNX2I_NX2_DEV_5709, &hba->cnic_dev_type);
 		hba->mail_queue_access = BNX2I_MQ_BIN_MODE;
-	} else if (hba->pci_did == PCI_DEVICE_ID_NX2_57710 ||
-		   hba->pci_did == PCI_DEVICE_ID_NX2_57711 ||
-		   hba->pci_did == PCI_DEVICE_ID_NX2_57711E)
+	} else if (hba->pci_did == PCI_DEVICE_ID_NX2_57710  ||
+		   hba->pci_did == PCI_DEVICE_ID_NX2_57711  ||
+		   hba->pci_did == PCI_DEVICE_ID_NX2_57711E ||
+		   hba->pci_did == PCI_DEVICE_ID_NX2_57712  ||
+		   hba->pci_did == PCI_DEVICE_ID_NX2_57712E)
 		set_bit(BNX2I_NX2_DEV_57710, &hba->cnic_dev_type);
 	else
 		printk(KERN_ALERT "bnx2i: unknown device, 0x%x\n",
@@ -161,6 +163,21 @@ void bnx2i_start(void *handle)
 	struct bnx2i_hba *hba = handle;
 	int i = HZ;
 
+	if (!hba->cnic->max_iscsi_conn) {
+		printk(KERN_ALERT "bnx2i: dev %s does not support "
+			"iSCSI\n", hba->netdev->name);
+
+		if (test_bit(BNX2I_CNIC_REGISTERED, &hba->reg_with_cnic)) {
+			mutex_lock(&bnx2i_dev_lock);
+			list_del_init(&hba->link);
+			adapter_count--;
+			hba->cnic->unregister_device(hba->cnic, CNIC_ULP_ISCSI);
+			clear_bit(BNX2I_CNIC_REGISTERED, &hba->reg_with_cnic);
+			mutex_unlock(&bnx2i_dev_lock);
+			bnx2i_free_hba(hba);
+		}
+		return;
+	}
 	bnx2i_send_fw_iscsi_init_msg(hba);
 	while (!test_bit(ADAPTER_STATE_UP, &hba->adapter_state) && i--)
 		msleep(BNX2I_INIT_POLL_TIME);
diff --git a/drivers/scsi/bnx2i/bnx2i_iscsi.c b/drivers/scsi/bnx2i/bnx2i_iscsi.c
index f0dce26593eb..1809f9ccc4ce 100644
--- a/drivers/scsi/bnx2i/bnx2i_iscsi.c
+++ b/drivers/scsi/bnx2i/bnx2i_iscsi.c
@@ -1092,6 +1092,9 @@ static int bnx2i_iscsi_send_generic_request(struct iscsi_task *task)
 	case ISCSI_OP_SCSI_TMFUNC:
 		rc = bnx2i_send_iscsi_tmf(bnx2i_conn, task);
 		break;
+	case ISCSI_OP_TEXT:
+		rc = bnx2i_send_iscsi_text(bnx2i_conn, task);
+		break;
 	default:
 		iscsi_conn_printk(KERN_ALERT, bnx2i_conn->cls_conn->dd_data,
 				  "send_gen: unsupported op 0x%x\n",
@@ -1455,42 +1458,40 @@ static void bnx2i_conn_destroy(struct iscsi_cls_conn *cls_conn)
 
 
 /**
- * bnx2i_conn_get_param - return iscsi connection parameter to caller
- * @cls_conn:	pointer to iscsi cls conn
+ * bnx2i_ep_get_param - return iscsi ep parameter to caller
+ * @ep:		pointer to iscsi endpoint
  * @param:	parameter type identifier
  * @buf: 	buffer pointer
  *
- * returns iSCSI connection parameters
+ * returns iSCSI ep parameters
  */
-static int bnx2i_conn_get_param(struct iscsi_cls_conn *cls_conn,
-				enum iscsi_param param, char *buf)
+static int bnx2i_ep_get_param(struct iscsi_endpoint *ep,
+			      enum iscsi_param param, char *buf)
 {
-	struct iscsi_conn *conn = cls_conn->dd_data;
-	struct bnx2i_conn *bnx2i_conn = conn->dd_data;
-	int len = 0;
+	struct bnx2i_endpoint *bnx2i_ep = ep->dd_data;
+	struct bnx2i_hba *hba = bnx2i_ep->hba;
+	int len = -ENOTCONN;
 
-	if (!(bnx2i_conn && bnx2i_conn->ep && bnx2i_conn->ep->hba))
-		goto out;
+	if (!hba)
+		return -ENOTCONN;
 
 	switch (param) {
 	case ISCSI_PARAM_CONN_PORT:
-		mutex_lock(&bnx2i_conn->ep->hba->net_dev_lock);
-		if (bnx2i_conn->ep->cm_sk)
-			len = sprintf(buf, "%hu\n",
-				      bnx2i_conn->ep->cm_sk->dst_port);
-		mutex_unlock(&bnx2i_conn->ep->hba->net_dev_lock);
+		mutex_lock(&hba->net_dev_lock);
+		if (bnx2i_ep->cm_sk)
+			len = sprintf(buf, "%hu\n", bnx2i_ep->cm_sk->dst_port);
+		mutex_unlock(&hba->net_dev_lock);
 		break;
 	case ISCSI_PARAM_CONN_ADDRESS:
-		mutex_lock(&bnx2i_conn->ep->hba->net_dev_lock);
-		if (bnx2i_conn->ep->cm_sk)
-			len = sprintf(buf, "%pI4\n",
-				      &bnx2i_conn->ep->cm_sk->dst_ip);
-		mutex_unlock(&bnx2i_conn->ep->hba->net_dev_lock);
+		mutex_lock(&hba->net_dev_lock);
+		if (bnx2i_ep->cm_sk)
+			len = sprintf(buf, "%pI4\n", &bnx2i_ep->cm_sk->dst_ip);
+		mutex_unlock(&hba->net_dev_lock);
 		break;
 	default:
-		return iscsi_conn_get_param(cls_conn, param, buf);
+		return -ENOSYS;
 	}
-out:
+
 	return len;
 }
 
@@ -1935,13 +1936,13 @@ static int bnx2i_ep_tcp_conn_active(struct bnx2i_endpoint *bnx2i_ep)
 		cnic_dev_10g = 1;
 
 	switch (bnx2i_ep->state) {
-	case EP_STATE_CONNECT_FAILED:
 	case EP_STATE_CLEANUP_FAILED:
 	case EP_STATE_OFLD_FAILED:
 	case EP_STATE_DISCONN_TIMEDOUT:
 		ret = 0;
 		break;
 	case EP_STATE_CONNECT_START:
+	case EP_STATE_CONNECT_FAILED:
 	case EP_STATE_CONNECT_COMPL:
 	case EP_STATE_ULP_UPDATE_START:
 	case EP_STATE_ULP_UPDATE_COMPL:
@@ -2167,7 +2168,8 @@ struct iscsi_transport bnx2i_iscsi_transport = {
 	.name			= "bnx2i",
 	.caps			= CAP_RECOVERY_L0 | CAP_HDRDGST |
 				  CAP_MULTI_R2T | CAP_DATADGST |
-				  CAP_DATA_PATH_OFFLOAD,
+				  CAP_DATA_PATH_OFFLOAD |
+				  CAP_TEXT_NEGO,
 	.param_mask		= ISCSI_MAX_RECV_DLENGTH |
 				  ISCSI_MAX_XMIT_DLENGTH |
 				  ISCSI_HDRDGST_EN |
@@ -2200,7 +2202,7 @@ struct iscsi_transport bnx2i_iscsi_transport = {
 	.bind_conn		= bnx2i_conn_bind,
 	.destroy_conn		= bnx2i_conn_destroy,
 	.set_param		= iscsi_set_param,
-	.get_conn_param		= bnx2i_conn_get_param,
+	.get_conn_param		= iscsi_conn_get_param,
 	.get_session_param	= iscsi_session_get_param,
 	.get_host_param		= bnx2i_host_get_param,
 	.start_conn		= bnx2i_conn_start,
@@ -2209,6 +2211,7 @@ struct iscsi_transport bnx2i_iscsi_transport = {
 	.xmit_task		= bnx2i_task_xmit,
 	.get_stats		= bnx2i_conn_get_stats,
 	/* TCP connect - disconnect - option-2 interface calls */
+	.get_ep_param		= bnx2i_ep_get_param,
 	.ep_connect		= bnx2i_ep_connect,
 	.ep_poll		= bnx2i_ep_poll,
 	.ep_disconnect		= bnx2i_ep_disconnect,
diff --git a/drivers/scsi/cxgbi/cxgb3i/Kconfig b/drivers/scsi/cxgbi/cxgb3i/Kconfig
index 5cf4e9831f1b..11dff23f7838 100644
--- a/drivers/scsi/cxgbi/cxgb3i/Kconfig
+++ b/drivers/scsi/cxgbi/cxgb3i/Kconfig
@@ -1,6 +1,8 @@
 config SCSI_CXGB3_ISCSI
 	tristate "Chelsio T3 iSCSI support"
-	depends on CHELSIO_T3_DEPENDS
+	depends on PCI && INET
+	select NETDEVICES
+	select NETDEV_10000
 	select CHELSIO_T3
 	select SCSI_ISCSI_ATTRS
 	---help---
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
index a129a170b47b..fc2cdb62f53b 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
@@ -105,7 +105,7 @@ static struct iscsi_transport cxgb3i_iscsi_transport = {
 	/* owner and name should be set already */
 	.caps		= CAP_RECOVERY_L0 | CAP_MULTI_R2T | CAP_HDRDGST
 				| CAP_DATADGST | CAP_DIGEST_OFFLOAD |
-				CAP_PADDING_OFFLOAD,
+				CAP_PADDING_OFFLOAD | CAP_TEXT_NEGO,
 	.param_mask	= ISCSI_MAX_RECV_DLENGTH | ISCSI_MAX_XMIT_DLENGTH |
 				ISCSI_HDRDGST_EN | ISCSI_DATADGST_EN |
 				ISCSI_INITIAL_R2T_EN | ISCSI_MAX_R2T |
@@ -137,7 +137,7 @@ static struct iscsi_transport cxgb3i_iscsi_transport = {
 	.destroy_conn	= iscsi_tcp_conn_teardown,
 	.start_conn	= iscsi_conn_start,
 	.stop_conn	= iscsi_conn_stop,
-	.get_conn_param	= cxgbi_get_conn_param,
+	.get_conn_param	= iscsi_conn_get_param,
 	.set_param	= cxgbi_set_conn_param,
 	.get_stats	= cxgbi_get_conn_stats,
 	/* pdu xmit req from user space */
@@ -152,6 +152,7 @@ static struct iscsi_transport cxgb3i_iscsi_transport = {
 	.xmit_pdu	= cxgbi_conn_xmit_pdu,
 	.parse_pdu_itt	= cxgbi_parse_pdu_itt,
 	/* TCP connect/disconnect */
+	.get_ep_param	= cxgbi_get_ep_param,
 	.ep_connect	= cxgbi_ep_connect,
 	.ep_poll	= cxgbi_ep_poll,
 	.ep_disconnect	= cxgbi_ep_disconnect,
@@ -1108,10 +1109,11 @@ static int ddp_set_map(struct cxgbi_sock *csk, struct cxgbi_pagepod_hdr *hdr,
 		csk, idx, npods, gl);
 
 	for (i = 0; i < npods; i++, idx++, pm_addr += PPOD_SIZE) {
-		struct sk_buff *skb = ddp->gl_skb[idx];
+		struct sk_buff *skb = alloc_wr(sizeof(struct ulp_mem_io) +
+						PPOD_SIZE, 0, GFP_ATOMIC);
 
-		/* hold on to the skb until we clear the ddp mapping */
-		skb_get(skb);
+		if (!skb)
+			return -ENOMEM;
 
 		ulp_mem_io_set_hdr(skb, pm_addr);
 		cxgbi_ddp_ppod_set((struct cxgbi_pagepod *)(skb->head +
@@ -1136,56 +1138,20 @@ static void ddp_clear_map(struct cxgbi_hba *chba, unsigned int tag,
 		cdev, idx, npods, tag);
 
 	for (i = 0; i < npods; i++, idx++, pm_addr += PPOD_SIZE) {
-		struct sk_buff *skb = ddp->gl_skb[idx];
+		struct sk_buff *skb = alloc_wr(sizeof(struct ulp_mem_io) +
+						PPOD_SIZE, 0, GFP_ATOMIC);
 
 		if (!skb) {
-			pr_err("tag 0x%x, 0x%x, %d/%u, skb NULL.\n",
+			pr_err("tag 0x%x, 0x%x, %d/%u, skb OOM.\n",
 				tag, idx, i, npods);
 			continue;
 		}
-		ddp->gl_skb[idx] = NULL;
-		memset(skb->head + sizeof(struct ulp_mem_io), 0, PPOD_SIZE);
 		ulp_mem_io_set_hdr(skb, pm_addr);
 		skb->priority = CPL_PRIORITY_CONTROL;
 		cxgb3_ofld_send(cdev->lldev, skb);
 	}
 }
 
-static void ddp_free_gl_skb(struct cxgbi_ddp_info *ddp, int idx, int cnt)
-{
-	int i;
-
-	log_debug(1 << CXGBI_DBG_DDP,
-		"ddp 0x%p, idx %d, cnt %d.\n", ddp, idx, cnt);
-
-	for (i = 0; i < cnt; i++, idx++)
-		if (ddp->gl_skb[idx]) {
-			kfree_skb(ddp->gl_skb[idx]);
-			ddp->gl_skb[idx] = NULL;
-		}
-}
-
-static int ddp_alloc_gl_skb(struct cxgbi_ddp_info *ddp, int idx,
-				   int cnt, gfp_t gfp)
-{
-	int i;
-
-	log_debug(1 << CXGBI_DBG_DDP,
-		"ddp 0x%p, idx %d, cnt %d.\n", ddp, idx, cnt);
-
-	for (i = 0; i < cnt; i++) {
-		struct sk_buff *skb = alloc_wr(sizeof(struct ulp_mem_io) +
-						PPOD_SIZE, 0, gfp);
-		if (skb)
-			ddp->gl_skb[idx + i] = skb;
-		else {
-			ddp_free_gl_skb(ddp, idx, i);
-			return -ENOMEM;
-		}
-	}
-	return 0;
-}
-
 static int ddp_setup_conn_pgidx(struct cxgbi_sock *csk,
 				       unsigned int tid, int pg_idx, bool reply)
 {
@@ -1316,8 +1282,6 @@ static int cxgb3i_ddp_init(struct cxgbi_device *cdev)
 	}
 	tdev->ulp_iscsi = ddp;
 
-	cdev->csk_ddp_free_gl_skb = ddp_free_gl_skb;
-	cdev->csk_ddp_alloc_gl_skb = ddp_alloc_gl_skb;
 	cdev->csk_ddp_setup_digest = ddp_setup_conn_digest;
 	cdev->csk_ddp_setup_pgidx = ddp_setup_conn_pgidx;
 	cdev->csk_ddp_set = ddp_set_map;
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h
index 5f5e3394b594..20593fd69d8f 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h
@@ -24,10 +24,21 @@
 
 extern cxgb3_cpl_handler_func cxgb3i_cpl_handlers[NUM_CPL_CMDS];
 
-#define cxgb3i_get_private_ipv4addr(ndev) \
-	(((struct port_info *)(netdev_priv(ndev)))->iscsi_ipv4addr)
-#define cxgb3i_set_private_ipv4addr(ndev, addr) \
-	(((struct port_info *)(netdev_priv(ndev)))->iscsi_ipv4addr) = addr
+static inline unsigned int cxgb3i_get_private_ipv4addr(struct net_device *ndev)
+{
+	return ((struct port_info *)(netdev_priv(ndev)))->iscsi_ipv4addr;
+}
+
+static inline void cxgb3i_set_private_ipv4addr(struct net_device *ndev,
+						unsigned int addr)
+{
+	struct port_info *pi =  (struct port_info *)netdev_priv(ndev);
+
+	pi->iscsic.flags = addr ? 1 : 0;
+	pi->iscsi_ipv4addr = addr;
+	if (addr)
+		memcpy(pi->iscsic.mac_addr, ndev->dev_addr, ETH_ALEN);
+}
 
 struct cpl_iscsi_hdr_norss {
 	union opcode_tid ot;
diff --git a/drivers/scsi/cxgbi/cxgb4i/Kconfig b/drivers/scsi/cxgbi/cxgb4i/Kconfig
index bb94b39b17b3..d5302c27f377 100644
--- a/drivers/scsi/cxgbi/cxgb4i/Kconfig
+++ b/drivers/scsi/cxgbi/cxgb4i/Kconfig
@@ -1,6 +1,8 @@
 config SCSI_CXGB4_ISCSI
 	tristate "Chelsio T4 iSCSI support"
-	depends on CHELSIO_T4_DEPENDS
+	depends on PCI && INET
+	select NETDEVICES
+	select NETDEV_10000
 	select CHELSIO_T4
 	select SCSI_ISCSI_ATTRS
 	---help---
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
index 8c04fada710b..f3a4cd7cf782 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
@@ -106,7 +106,7 @@ static struct iscsi_transport cxgb4i_iscsi_transport = {
 	.name		= DRV_MODULE_NAME,
 	.caps		= CAP_RECOVERY_L0 | CAP_MULTI_R2T | CAP_HDRDGST |
 				CAP_DATADGST | CAP_DIGEST_OFFLOAD |
-				CAP_PADDING_OFFLOAD,
+				CAP_PADDING_OFFLOAD | CAP_TEXT_NEGO,
 	.param_mask	= ISCSI_MAX_RECV_DLENGTH | ISCSI_MAX_XMIT_DLENGTH |
 				ISCSI_HDRDGST_EN | ISCSI_DATADGST_EN |
 				ISCSI_INITIAL_R2T_EN | ISCSI_MAX_R2T |
@@ -138,7 +138,7 @@ static struct iscsi_transport cxgb4i_iscsi_transport = {
 	.destroy_conn	= iscsi_tcp_conn_teardown,
 	.start_conn		= iscsi_conn_start,
 	.stop_conn		= iscsi_conn_stop,
-	.get_conn_param	= cxgbi_get_conn_param,
+	.get_conn_param	= iscsi_conn_get_param,
 	.set_param	= cxgbi_set_conn_param,
 	.get_stats	= cxgbi_get_conn_stats,
 	/* pdu xmit req from user space */
@@ -153,6 +153,7 @@ static struct iscsi_transport cxgb4i_iscsi_transport = {
 	.xmit_pdu	= cxgbi_conn_xmit_pdu,
 	.parse_pdu_itt	= cxgbi_parse_pdu_itt,
 	/* TCP connect/disconnect */
+	.get_ep_param	= cxgbi_get_ep_param,
 	.ep_connect	= cxgbi_ep_connect,
 	.ep_poll	= cxgbi_ep_poll,
 	.ep_disconnect	= cxgbi_ep_disconnect,
@@ -1425,8 +1426,6 @@ static int cxgb4i_ddp_init(struct cxgbi_device *cdev)
 	cxgbi_ddp_page_size_factor(pgsz_factor);
 	cxgb4_iscsi_init(lldi->ports[0], tagmask, pgsz_factor);
 
-	cdev->csk_ddp_free_gl_skb = NULL;
-	cdev->csk_ddp_alloc_gl_skb = NULL;
 	cdev->csk_ddp_setup_digest = ddp_setup_conn_digest;
 	cdev->csk_ddp_setup_pgidx = ddp_setup_conn_pgidx;
 	cdev->csk_ddp_set = ddp_set_map;
diff --git a/drivers/scsi/cxgbi/libcxgbi.c b/drivers/scsi/cxgbi/libcxgbi.c
index d2ad3d676724..b193719b670a 100644
--- a/drivers/scsi/cxgbi/libcxgbi.c
+++ b/drivers/scsi/cxgbi/libcxgbi.c
@@ -470,7 +470,8 @@ static struct rtable *find_route_ipv4(__be32 saddr, __be32 daddr,
 			}
 	};
 
-	if (ip_route_output_flow(&init_net, &rt, &fl, NULL, 0))
+	rt = ip_route_output_flow(&init_net, &fl, NULL);
+	if (IS_ERR(rt))
 		return NULL;
 
 	return rt;
@@ -543,6 +544,7 @@ static struct cxgbi_sock *cxgbi_check_route(struct sockaddr *dst_addr)
 	csk->dst = dst;
 	csk->daddr.sin_addr.s_addr = daddr->sin_addr.s_addr;
 	csk->daddr.sin_port = daddr->sin_port;
+	csk->daddr.sin_family = daddr->sin_family;
 	csk->saddr.sin_addr.s_addr = rt->rt_src;
 
 	return csk;
@@ -1277,12 +1279,6 @@ static int ddp_tag_reserve(struct cxgbi_sock *csk, unsigned int tid,
 		return idx;
 	}
 
-	if (cdev->csk_ddp_alloc_gl_skb) {
-		err = cdev->csk_ddp_alloc_gl_skb(ddp, idx, npods, gfp);
-		if (err < 0)
-			goto unmark_entries;
-	}
-
 	tag = cxgbi_ddp_tag_base(tformat, sw_tag);
 	tag |= idx << PPOD_IDX_SHIFT;
 
@@ -1293,11 +1289,8 @@ static int ddp_tag_reserve(struct cxgbi_sock *csk, unsigned int tid,
 	hdr.page_offset = htonl(gl->offset);
 
 	err = cdev->csk_ddp_set(csk, &hdr, idx, npods, gl);
-	if (err < 0) {
-		if (cdev->csk_ddp_free_gl_skb)
-			cdev->csk_ddp_free_gl_skb(ddp, idx, npods);
+	if (err < 0)
 		goto unmark_entries;
-	}
 
 	ddp->idx_last = idx;
 	log_debug(1 << CXGBI_DBG_DDP,
@@ -1363,8 +1356,6 @@ static void ddp_destroy(struct kref *kref)
 					>> PPOD_PAGES_SHIFT;
 			pr_info("cdev 0x%p, ddp %d + %d.\n", cdev, i, npods);
 			kfree(gl);
-			if (cdev->csk_ddp_free_gl_skb)
-				cdev->csk_ddp_free_gl_skb(ddp, i, npods);
 			i += npods;
 		} else
 			i++;
@@ -1407,8 +1398,6 @@ int cxgbi_ddp_init(struct cxgbi_device *cdev,
 		return -ENOMEM;
 	}
 	ddp->gl_map = (struct cxgbi_gather_list **)(ddp + 1);
-	ddp->gl_skb = (struct sk_buff **)(((char *)ddp->gl_map) +
-				ppmax * sizeof(struct cxgbi_gather_list *));
 	cdev->ddp = ddp;
 
 	spin_lock_init(&ddp->map_lock);
@@ -1908,13 +1897,16 @@ EXPORT_SYMBOL_GPL(cxgbi_conn_alloc_pdu);
 
 static inline void tx_skb_setmode(struct sk_buff *skb, int hcrc, int dcrc)
 {
-	u8 submode = 0;
+	if (hcrc || dcrc) {
+		u8 submode = 0;
 
-	if (hcrc)
-		submode |= 1;
-	if (dcrc)
-		submode |= 2;
-	cxgbi_skcb_ulp_mode(skb) = (ULP2_MODE_ISCSI << 4) | submode;
+		if (hcrc)
+			submode |= 1;
+		if (dcrc)
+			submode |= 2;
+		cxgbi_skcb_ulp_mode(skb) = (ULP2_MODE_ISCSI << 4) | submode;
+	} else
+		cxgbi_skcb_ulp_mode(skb) = 0;
 }
 
 int cxgbi_conn_init_pdu(struct iscsi_task *task, unsigned int offset,
@@ -2210,32 +2202,34 @@ int cxgbi_set_conn_param(struct iscsi_cls_conn *cls_conn,
 }
 EXPORT_SYMBOL_GPL(cxgbi_set_conn_param);
 
-int cxgbi_get_conn_param(struct iscsi_cls_conn *cls_conn,
-			enum iscsi_param param, char *buf)
+int cxgbi_get_ep_param(struct iscsi_endpoint *ep, enum iscsi_param param,
+		       char *buf)
 {
-	struct iscsi_conn *iconn = cls_conn->dd_data;
+	struct cxgbi_endpoint *cep = ep->dd_data;
+	struct cxgbi_sock *csk;
 	int len;
 
 	log_debug(1 << CXGBI_DBG_ISCSI,
-		"cls_conn 0x%p, param %d.\n", cls_conn, param);
+		"cls_conn 0x%p, param %d.\n", ep, param);
 
 	switch (param) {
 	case ISCSI_PARAM_CONN_PORT:
-		spin_lock_bh(&iconn->session->lock);
-		len = sprintf(buf, "%hu\n", iconn->portal_port);
-		spin_unlock_bh(&iconn->session->lock);
-		break;
 	case ISCSI_PARAM_CONN_ADDRESS:
-		spin_lock_bh(&iconn->session->lock);
-		len = sprintf(buf, "%s\n", iconn->portal_address);
-		spin_unlock_bh(&iconn->session->lock);
-		break;
+		if (!cep)
+			return -ENOTCONN;
+
+		csk = cep->csk;
+		if (!csk)
+			return -ENOTCONN;
+
+		return iscsi_conn_get_addr_param((struct sockaddr_storage *)
+						 &csk->daddr, param, buf);
 	default:
-		return iscsi_conn_get_param(cls_conn, param, buf);
+		return -ENOSYS;
 	}
 	return len;
 }
-EXPORT_SYMBOL_GPL(cxgbi_get_conn_param);
+EXPORT_SYMBOL_GPL(cxgbi_get_ep_param);
 
 struct iscsi_cls_conn *
 cxgbi_create_conn(struct iscsi_cls_session *cls_session, u32 cid)
@@ -2302,11 +2296,6 @@ int cxgbi_bind_conn(struct iscsi_cls_session *cls_session,
 	cxgbi_conn_max_xmit_dlength(conn);
 	cxgbi_conn_max_recv_dlength(conn);
 
-	spin_lock_bh(&conn->session->lock);
-	sprintf(conn->portal_address, "%pI4", &csk->daddr.sin_addr.s_addr);
-	conn->portal_port = ntohs(csk->daddr.sin_port);
-	spin_unlock_bh(&conn->session->lock);
-
 	log_debug(1 << CXGBI_DBG_ISCSI,
 		"cls 0x%p,0x%p, ep 0x%p, cconn 0x%p, csk 0x%p.\n",
 		cls_session, cls_conn, ep, cconn, csk);
diff --git a/drivers/scsi/cxgbi/libcxgbi.h b/drivers/scsi/cxgbi/libcxgbi.h
index c57d59db000c..0a20fd5f7102 100644
--- a/drivers/scsi/cxgbi/libcxgbi.h
+++ b/drivers/scsi/cxgbi/libcxgbi.h
@@ -131,7 +131,6 @@ struct cxgbi_ddp_info {
 	unsigned int rsvd_tag_mask;
 	spinlock_t map_lock;
 	struct cxgbi_gather_list **gl_map;
-	struct sk_buff **gl_skb;
 };
 
 #define DDP_PGIDX_MAX		4
@@ -536,8 +535,6 @@ struct cxgbi_device {
 	struct cxgbi_ddp_info *ddp;
 
 	void (*dev_ddp_cleanup)(struct cxgbi_device *);
-	void (*csk_ddp_free_gl_skb)(struct cxgbi_ddp_info *, int, int);
-	int (*csk_ddp_alloc_gl_skb)(struct cxgbi_ddp_info *, int, int, gfp_t);
 	int (*csk_ddp_set)(struct cxgbi_sock *, struct cxgbi_pagepod_hdr *,
 				unsigned int, unsigned int,
 				struct cxgbi_gather_list *);
@@ -715,7 +712,7 @@ void cxgbi_cleanup_task(struct iscsi_task *task);
 void cxgbi_get_conn_stats(struct iscsi_cls_conn *, struct iscsi_stats *);
 int cxgbi_set_conn_param(struct iscsi_cls_conn *,
 			enum iscsi_param, char *, int);
-int cxgbi_get_conn_param(struct iscsi_cls_conn *, enum iscsi_param, char *);
+int cxgbi_get_ep_param(struct iscsi_endpoint *ep, enum iscsi_param, char *);
 struct iscsi_cls_conn *cxgbi_create_conn(struct iscsi_cls_session *, u32);
 int cxgbi_bind_conn(struct iscsi_cls_session *,
 			struct iscsi_cls_conn *, u64, int);
diff --git a/drivers/scsi/device_handler/scsi_dh.c b/drivers/scsi/device_handler/scsi_dh.c
index b837c5b3c8f9..564e6ecd17c2 100644
--- a/drivers/scsi/device_handler/scsi_dh.c
+++ b/drivers/scsi/device_handler/scsi_dh.c
@@ -25,16 +25,9 @@
 #include <scsi/scsi_dh.h>
 #include "../scsi_priv.h"
 
-struct scsi_dh_devinfo_list {
-	struct list_head node;
-	char vendor[9];
-	char model[17];
-	struct scsi_device_handler *handler;
-};
-
 static DEFINE_SPINLOCK(list_lock);
 static LIST_HEAD(scsi_dh_list);
-static LIST_HEAD(scsi_dh_dev_list);
+static int scsi_dh_list_idx = 1;
 
 static struct scsi_device_handler *get_device_handler(const char *name)
 {
@@ -51,40 +44,18 @@ static struct scsi_device_handler *get_device_handler(const char *name)
 	return found;
 }
 
-
-static struct scsi_device_handler *
-scsi_dh_cache_lookup(struct scsi_device *sdev)
+static struct scsi_device_handler *get_device_handler_by_idx(int idx)
 {
-	struct scsi_dh_devinfo_list *tmp;
-	struct scsi_device_handler *found_dh = NULL;
+	struct scsi_device_handler *tmp, *found = NULL;
 
 	spin_lock(&list_lock);
-	list_for_each_entry(tmp, &scsi_dh_dev_list, node) {
-		if (!strncmp(sdev->vendor, tmp->vendor, strlen(tmp->vendor)) &&
-		    !strncmp(sdev->model, tmp->model, strlen(tmp->model))) {
-			found_dh = tmp->handler;
+	list_for_each_entry(tmp, &scsi_dh_list, list) {
+		if (tmp->idx == idx) {
+			found = tmp;
 			break;
 		}
 	}
 	spin_unlock(&list_lock);
-
-	return found_dh;
-}
-
-static int scsi_dh_handler_lookup(struct scsi_device_handler *scsi_dh,
-				  struct scsi_device *sdev)
-{
-	int i, found = 0;
-
-	for(i = 0; scsi_dh->devlist[i].vendor; i++) {
-		if (!strncmp(sdev->vendor, scsi_dh->devlist[i].vendor,
-			     strlen(scsi_dh->devlist[i].vendor)) &&
-		    !strncmp(sdev->model, scsi_dh->devlist[i].model,
-			     strlen(scsi_dh->devlist[i].model))) {
-			found = 1;
-			break;
-		}
-	}
 	return found;
 }
 
@@ -102,41 +73,14 @@ device_handler_match(struct scsi_device_handler *scsi_dh,
 		     struct scsi_device *sdev)
 {
 	struct scsi_device_handler *found_dh = NULL;
-	struct scsi_dh_devinfo_list *tmp;
+	int idx;
 
-	found_dh = scsi_dh_cache_lookup(sdev);
-	if (found_dh)
-		return found_dh;
+	idx = scsi_get_device_flags_keyed(sdev, sdev->vendor, sdev->model,
+					  SCSI_DEVINFO_DH);
+	found_dh = get_device_handler_by_idx(idx);
 
-	if (scsi_dh) {
-		if (scsi_dh_handler_lookup(scsi_dh, sdev))
-			found_dh = scsi_dh;
-	} else {
-		struct scsi_device_handler *tmp_dh;
-
-		spin_lock(&list_lock);
-		list_for_each_entry(tmp_dh, &scsi_dh_list, list) {
-			if (scsi_dh_handler_lookup(tmp_dh, sdev))
-				found_dh = tmp_dh;
-		}
-		spin_unlock(&list_lock);
-	}
-
-	if (found_dh) { /* If device is found, add it to the cache */
-		tmp = kmalloc(sizeof(*tmp), GFP_KERNEL);
-		if (tmp) {
-			strncpy(tmp->vendor, sdev->vendor, 8);
-			strncpy(tmp->model, sdev->model, 16);
-			tmp->vendor[8] = '\0';
-			tmp->model[16] = '\0';
-			tmp->handler = found_dh;
-			spin_lock(&list_lock);
-			list_add(&tmp->node, &scsi_dh_dev_list);
-			spin_unlock(&list_lock);
-		} else {
-			found_dh = NULL;
-		}
-	}
+	if (scsi_dh && found_dh != scsi_dh)
+		found_dh = NULL;
 
 	return found_dh;
 }
@@ -373,12 +317,25 @@ static int scsi_dh_notifier_remove(struct device *dev, void *data)
  */
 int scsi_register_device_handler(struct scsi_device_handler *scsi_dh)
 {
+	int i;
+
 	if (get_device_handler(scsi_dh->name))
 		return -EBUSY;
 
 	spin_lock(&list_lock);
+	scsi_dh->idx = scsi_dh_list_idx++;
 	list_add(&scsi_dh->list, &scsi_dh_list);
 	spin_unlock(&list_lock);
+
+	for (i = 0; scsi_dh->devlist[i].vendor; i++) {
+		scsi_dev_info_list_add_keyed(0,
+					scsi_dh->devlist[i].vendor,
+					scsi_dh->devlist[i].model,
+					NULL,
+					scsi_dh->idx,
+					SCSI_DEVINFO_DH);
+	}
+
 	bus_for_each_dev(&scsi_bus_type, NULL, scsi_dh, scsi_dh_notifier_add);
 	printk(KERN_INFO "%s: device handler registered\n", scsi_dh->name);
 
@@ -395,7 +352,7 @@ EXPORT_SYMBOL_GPL(scsi_register_device_handler);
  */
 int scsi_unregister_device_handler(struct scsi_device_handler *scsi_dh)
 {
-	struct scsi_dh_devinfo_list *tmp, *pos;
+	int i;
 
 	if (!get_device_handler(scsi_dh->name))
 		return -ENODEV;
@@ -403,14 +360,14 @@ int scsi_unregister_device_handler(struct scsi_device_handler *scsi_dh)
 	bus_for_each_dev(&scsi_bus_type, NULL, scsi_dh,
 			 scsi_dh_notifier_remove);
 
+	for (i = 0; scsi_dh->devlist[i].vendor; i++) {
+		scsi_dev_info_list_del_keyed(scsi_dh->devlist[i].vendor,
+					     scsi_dh->devlist[i].model,
+					     SCSI_DEVINFO_DH);
+	}
+
 	spin_lock(&list_lock);
 	list_del(&scsi_dh->list);
-	list_for_each_entry_safe(pos, tmp, &scsi_dh_dev_list, node) {
-		if (pos->handler == scsi_dh) {
-			list_del(&pos->node);
-			kfree(pos);
-		}
-	}
 	spin_unlock(&list_lock);
 	printk(KERN_INFO "%s: device handler unregistered\n", scsi_dh->name);
 
@@ -576,6 +533,10 @@ static int __init scsi_dh_init(void)
 {
 	int r;
 
+	r = scsi_dev_info_add_list(SCSI_DEVINFO_DH, "SCSI Device Handler");
+	if (r)
+		return r;
+
 	r = bus_register_notifier(&scsi_bus_type, &scsi_dh_nb);
 
 	if (!r)
@@ -590,6 +551,7 @@ static void __exit scsi_dh_exit(void)
 	bus_for_each_dev(&scsi_bus_type, NULL, NULL,
 			 scsi_dh_sysfs_attr_remove);
 	bus_unregister_notifier(&scsi_bus_type, &scsi_dh_nb);
+	scsi_dev_info_remove_list(SCSI_DEVINFO_DH);
 }
 
 module_init(scsi_dh_init);
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index 6b729324b8d3..7cae0bc85390 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -253,13 +253,15 @@ static void stpg_endio(struct request *req, int error)
 {
 	struct alua_dh_data *h = req->end_io_data;
 	struct scsi_sense_hdr sense_hdr;
-	unsigned err = SCSI_DH_IO;
+	unsigned err = SCSI_DH_OK;
 
 	if (error || host_byte(req->errors) != DID_OK ||
-			msg_byte(req->errors) != COMMAND_COMPLETE)
+			msg_byte(req->errors) != COMMAND_COMPLETE) {
+		err = SCSI_DH_IO;
 		goto done;
+	}
 
-	if (err == SCSI_DH_IO && h->senselen > 0) {
+	if (h->senselen > 0) {
 		err = scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
 					   &sense_hdr);
 		if (!err) {
@@ -285,7 +287,8 @@ static void stpg_endio(struct request *req, int error)
 			    print_alua_state(h->state));
 	}
 done:
-	blk_put_request(req);
+	req->end_io_data = NULL;
+	__blk_put_request(req->q, req);
 	if (h->callback_fn) {
 		h->callback_fn(h->callback_data, err);
 		h->callback_fn = h->callback_data = NULL;
@@ -303,7 +306,6 @@ done:
 static unsigned submit_stpg(struct alua_dh_data *h)
 {
 	struct request *rq;
-	int err = SCSI_DH_RES_TEMP_UNAVAIL;
 	int stpg_len = 8;
 	struct scsi_device *sdev = h->sdev;
 
@@ -332,7 +334,7 @@ static unsigned submit_stpg(struct alua_dh_data *h)
 	rq->end_io_data = h;
 
 	blk_execute_rq_nowait(rq->q, NULL, rq, 1, stpg_endio);
-	return err;
+	return SCSI_DH_OK;
 }
 
 /*
@@ -730,7 +732,9 @@ static const struct scsi_dh_devlist alua_dev_list[] = {
 	{"Pillar", "Axiom" },
 	{"Intel", "Multi-Flex"},
 	{"NETAPP", "LUN"},
+	{"NETAPP", "LUN C-Mode"},
 	{"AIX", "NVDISK"},
+	{"Promise", "VTrak"},
 	{NULL, NULL}
 };
 
@@ -759,7 +763,7 @@ static int alua_bus_attach(struct scsi_device *sdev)
 	unsigned long flags;
 	int err = SCSI_DH_OK;
 
-	scsi_dh_data = kzalloc(sizeof(struct scsi_device_handler *)
+	scsi_dh_data = kzalloc(sizeof(*scsi_dh_data)
 			       + sizeof(*h) , GFP_KERNEL);
 	if (!scsi_dh_data) {
 		sdev_printk(KERN_ERR, sdev, "%s: Attach failed\n",
diff --git a/drivers/scsi/device_handler/scsi_dh_emc.c b/drivers/scsi/device_handler/scsi_dh_emc.c
index 6faf472f7537..48441f6908a4 100644
--- a/drivers/scsi/device_handler/scsi_dh_emc.c
+++ b/drivers/scsi/device_handler/scsi_dh_emc.c
@@ -650,7 +650,7 @@ static int clariion_bus_attach(struct scsi_device *sdev)
 	unsigned long flags;
 	int err;
 
-	scsi_dh_data = kzalloc(sizeof(struct scsi_device_handler *)
+	scsi_dh_data = kzalloc(sizeof(*scsi_dh_data)
 			       + sizeof(*h) , GFP_KERNEL);
 	if (!scsi_dh_data) {
 		sdev_printk(KERN_ERR, sdev, "%s: Attach failed\n",
diff --git a/drivers/scsi/device_handler/scsi_dh_hp_sw.c b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
index e3916641e627..b479f1eef968 100644
--- a/drivers/scsi/device_handler/scsi_dh_hp_sw.c
+++ b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
@@ -225,7 +225,8 @@ static void start_stop_endio(struct request *req, int error)
 		}
 	}
 done:
-	blk_put_request(req);
+	req->end_io_data = NULL;
+	__blk_put_request(req->q, req);
 	if (h->callback_fn) {
 		h->callback_fn(h->callback_data, err);
 		h->callback_fn = h->callback_data = NULL;
@@ -338,8 +339,8 @@ static int hp_sw_bus_attach(struct scsi_device *sdev)
 	unsigned long flags;
 	int ret;
 
-	scsi_dh_data = kzalloc(sizeof(struct scsi_device_handler *)
-			       + sizeof(struct hp_sw_dh_data) , GFP_KERNEL);
+	scsi_dh_data = kzalloc(sizeof(*scsi_dh_data)
+			       + sizeof(*h) , GFP_KERNEL);
 	if (!scsi_dh_data) {
 		sdev_printk(KERN_ERR, sdev, "%s: Attach Failed\n",
 			    HP_SW_NAME);
diff --git a/drivers/scsi/device_handler/scsi_dh_rdac.c b/drivers/scsi/device_handler/scsi_dh_rdac.c
index 5be3ae15cb71..f468b05cfaac 100644
--- a/drivers/scsi/device_handler/scsi_dh_rdac.c
+++ b/drivers/scsi/device_handler/scsi_dh_rdac.c
@@ -281,11 +281,13 @@ static struct request *get_rdac_req(struct scsi_device *sdev,
 }
 
 static struct request *rdac_failover_get(struct scsi_device *sdev,
-					 struct rdac_dh_data *h)
+			struct rdac_dh_data *h, struct list_head *list)
 {
 	struct request *rq;
 	struct rdac_mode_common *common;
 	unsigned data_size;
+	struct rdac_queue_data *qdata;
+	u8 *lun_table;
 
 	if (h->ctlr->use_ms10) {
 		struct rdac_pg_expanded *rdac_pg;
@@ -298,6 +300,7 @@ static struct request *rdac_failover_get(struct scsi_device *sdev,
 		rdac_pg->subpage_code = 0x1;
 		rdac_pg->page_len[0] = 0x01;
 		rdac_pg->page_len[1] = 0x28;
+		lun_table = rdac_pg->lun_table;
 	} else {
 		struct rdac_pg_legacy *rdac_pg;
 
@@ -307,11 +310,16 @@ static struct request *rdac_failover_get(struct scsi_device *sdev,
 		common = &rdac_pg->common;
 		rdac_pg->page_code = RDAC_PAGE_CODE_REDUNDANT_CONTROLLER;
 		rdac_pg->page_len = 0x68;
+		lun_table = rdac_pg->lun_table;
 	}
 	common->rdac_mode[1] = RDAC_MODE_TRANSFER_SPECIFIED_LUNS;
 	common->quiescence_timeout = RDAC_QUIESCENCE_TIME;
 	common->rdac_options = RDAC_FORCED_QUIESENCE;
 
+	list_for_each_entry(qdata, list, entry) {
+		lun_table[qdata->h->lun] = 0x81;
+	}
+
 	/* get request for block layer packet command */
 	rq = get_rdac_req(sdev, &h->ctlr->mode_select, data_size, WRITE);
 	if (!rq)
@@ -565,7 +573,6 @@ static void send_mode_select(struct work_struct *work)
 	int err, retry_cnt = RDAC_RETRY_COUNT;
 	struct rdac_queue_data *tmp, *qdata;
 	LIST_HEAD(list);
-	u8 *lun_table;
 
 	spin_lock(&ctlr->ms_lock);
 	list_splice_init(&ctlr->ms_head, &list);
@@ -573,21 +580,12 @@ static void send_mode_select(struct work_struct *work)
 	ctlr->ms_sdev = NULL;
 	spin_unlock(&ctlr->ms_lock);
 
-	if (ctlr->use_ms10)
-		lun_table = ctlr->mode_select.expanded.lun_table;
-	else
-		lun_table = ctlr->mode_select.legacy.lun_table;
-
 retry:
 	err = SCSI_DH_RES_TEMP_UNAVAIL;
-	rq = rdac_failover_get(sdev, h);
+	rq = rdac_failover_get(sdev, h, &list);
 	if (!rq)
 		goto done;
 
-	list_for_each_entry(qdata, &list, entry) {
-		lun_table[qdata->h->lun] = 0x81;
-	}
-
 	RDAC_LOG(RDAC_LOG_FAILOVER, sdev, "array %s, ctlr %d, "
 		"%s MODE_SELECT command",
 		(char *) h->ctlr->array_name, h->ctlr->index,
@@ -800,7 +798,7 @@ static int rdac_bus_attach(struct scsi_device *sdev)
 	int err;
 	char array_name[ARRAY_LABEL_LEN];
 
-	scsi_dh_data = kzalloc(sizeof(struct scsi_device_handler *)
+	scsi_dh_data = kzalloc(sizeof(*scsi_dh_data)
 			       + sizeof(*h) , GFP_KERNEL);
 	if (!scsi_dh_data) {
 		sdev_printk(KERN_ERR, sdev, "%s: Attach failed\n",
@@ -906,4 +904,5 @@ module_exit(rdac_exit);
 
 MODULE_DESCRIPTION("Multipath LSI/Engenio RDAC driver");
 MODULE_AUTHOR("Mike Christie, Chandra Seetharaman");
+MODULE_VERSION("01.00.0000.0000");
 MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/fcoe/Makefile b/drivers/scsi/fcoe/Makefile
index 950f27615c76..f6d37d0271f7 100644
--- a/drivers/scsi/fcoe/Makefile
+++ b/drivers/scsi/fcoe/Makefile
@@ -1,2 +1,4 @@
 obj-$(CONFIG_FCOE) += fcoe.o
 obj-$(CONFIG_LIBFCOE) += libfcoe.o
+
+libfcoe-objs := fcoe_ctlr.o fcoe_transport.o
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 9f9600b67001..94fb4802e580 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -31,6 +31,7 @@
 #include <linux/fs.h>
 #include <linux/sysfs.h>
 #include <linux/ctype.h>
+#include <linux/workqueue.h>
 #include <scsi/scsi_tcq.h>
 #include <scsi/scsicam.h>
 #include <scsi/scsi_transport.h>
@@ -58,6 +59,8 @@ MODULE_PARM_DESC(ddp_min, "Minimum I/O size in bytes for "	\
 
 DEFINE_MUTEX(fcoe_config_mutex);
 
+static struct workqueue_struct *fcoe_wq;
+
 /* fcoe_percpu_clean completion.  Waiter protected by fcoe_create_mutex */
 static DECLARE_COMPLETION(fcoe_flush_completion);
 
@@ -72,7 +75,6 @@ static int fcoe_xmit(struct fc_lport *, struct fc_frame *);
 static int fcoe_rcv(struct sk_buff *, struct net_device *,
 		    struct packet_type *, struct net_device *);
 static int fcoe_percpu_receive_thread(void *);
-static void fcoe_clean_pending_queue(struct fc_lport *);
 static void fcoe_percpu_clean(struct fc_lport *);
 static int fcoe_link_speed_update(struct fc_lport *);
 static int fcoe_link_ok(struct fc_lport *);
@@ -80,7 +82,6 @@ static int fcoe_link_ok(struct fc_lport *);
 static struct fc_lport *fcoe_hostlist_lookup(const struct net_device *);
 static int fcoe_hostlist_add(const struct fc_lport *);
 
-static void fcoe_check_wait_queue(struct fc_lport *, struct sk_buff *);
 static int fcoe_device_notification(struct notifier_block *, ulong, void *);
 static void fcoe_dev_setup(void);
 static void fcoe_dev_cleanup(void);
@@ -101,10 +102,11 @@ static int fcoe_ddp_done(struct fc_lport *, u16);
 
 static int fcoe_cpu_callback(struct notifier_block *, unsigned long, void *);
 
-static int fcoe_create(const char *, struct kernel_param *);
-static int fcoe_destroy(const char *, struct kernel_param *);
-static int fcoe_enable(const char *, struct kernel_param *);
-static int fcoe_disable(const char *, struct kernel_param *);
+static bool fcoe_match(struct net_device *netdev);
+static int fcoe_create(struct net_device *netdev, enum fip_state fip_mode);
+static int fcoe_destroy(struct net_device *netdev);
+static int fcoe_enable(struct net_device *netdev);
+static int fcoe_disable(struct net_device *netdev);
 
 static struct fc_seq *fcoe_elsct_send(struct fc_lport *,
 				      u32 did, struct fc_frame *,
@@ -117,24 +119,6 @@ static void fcoe_recv_frame(struct sk_buff *skb);
 
 static void fcoe_get_lesb(struct fc_lport *, struct fc_els_lesb *);
 
-module_param_call(create, fcoe_create, NULL, (void *)FIP_MODE_FABRIC, S_IWUSR);
-__MODULE_PARM_TYPE(create, "string");
-MODULE_PARM_DESC(create, " Creates fcoe instance on a ethernet interface");
-module_param_call(create_vn2vn, fcoe_create, NULL,
-		  (void *)FIP_MODE_VN2VN, S_IWUSR);
-__MODULE_PARM_TYPE(create_vn2vn, "string");
-MODULE_PARM_DESC(create_vn2vn, " Creates a VN_node to VN_node FCoE instance "
-		 "on an Ethernet interface");
-module_param_call(destroy, fcoe_destroy, NULL, NULL, S_IWUSR);
-__MODULE_PARM_TYPE(destroy, "string");
-MODULE_PARM_DESC(destroy, " Destroys fcoe instance on a ethernet interface");
-module_param_call(enable, fcoe_enable, NULL, NULL, S_IWUSR);
-__MODULE_PARM_TYPE(enable, "string");
-MODULE_PARM_DESC(enable, " Enables fcoe on a ethernet interface.");
-module_param_call(disable, fcoe_disable, NULL, NULL, S_IWUSR);
-__MODULE_PARM_TYPE(disable, "string");
-MODULE_PARM_DESC(disable, " Disables fcoe on a ethernet interface.");
-
 /* notification function for packets from net device */
 static struct notifier_block fcoe_notifier = {
 	.notifier_call = fcoe_device_notification,
@@ -145,8 +129,8 @@ static struct notifier_block fcoe_cpu_notifier = {
 	.notifier_call = fcoe_cpu_callback,
 };
 
-static struct scsi_transport_template *fcoe_transport_template;
-static struct scsi_transport_template *fcoe_vport_transport_template;
+static struct scsi_transport_template *fcoe_nport_scsi_transport;
+static struct scsi_transport_template *fcoe_vport_scsi_transport;
 
 static int fcoe_vport_destroy(struct fc_vport *);
 static int fcoe_vport_create(struct fc_vport *, bool disabled);
@@ -163,7 +147,7 @@ static struct libfc_function_template fcoe_libfc_fcn_templ = {
 	.lport_set_port_id = fcoe_set_port_id,
 };
 
-struct fc_function_template fcoe_transport_function = {
+struct fc_function_template fcoe_nport_fc_functions = {
 	.show_host_node_name = 1,
 	.show_host_port_name = 1,
 	.show_host_supported_classes = 1,
@@ -203,7 +187,7 @@ struct fc_function_template fcoe_transport_function = {
 	.bsg_request = fc_lport_bsg_request,
 };
 
-struct fc_function_template fcoe_vport_transport_function = {
+struct fc_function_template fcoe_vport_fc_functions = {
 	.show_host_node_name = 1,
 	.show_host_port_name = 1,
 	.show_host_supported_classes = 1,
@@ -356,10 +340,18 @@ static struct fcoe_interface *fcoe_interface_create(struct net_device *netdev,
 	struct fcoe_interface *fcoe;
 	int err;
 
+	if (!try_module_get(THIS_MODULE)) {
+		FCOE_NETDEV_DBG(netdev,
+				"Could not get a reference to the module\n");
+		fcoe = ERR_PTR(-EBUSY);
+		goto out;
+	}
+
 	fcoe = kzalloc(sizeof(*fcoe), GFP_KERNEL);
 	if (!fcoe) {
 		FCOE_NETDEV_DBG(netdev, "Could not allocate fcoe structure\n");
-		return NULL;
+		fcoe = ERR_PTR(-ENOMEM);
+		goto out_nomod;
 	}
 
 	dev_hold(netdev);
@@ -378,9 +370,15 @@ static struct fcoe_interface *fcoe_interface_create(struct net_device *netdev,
 		fcoe_ctlr_destroy(&fcoe->ctlr);
 		kfree(fcoe);
 		dev_put(netdev);
-		return NULL;
+		fcoe = ERR_PTR(err);
+		goto out_nomod;
 	}
 
+	goto out;
+
+out_nomod:
+	module_put(THIS_MODULE);
+out:
 	return fcoe;
 }
 
@@ -442,6 +440,7 @@ static void fcoe_interface_release(struct kref *kref)
 	fcoe_ctlr_destroy(&fcoe->ctlr);
 	kfree(fcoe);
 	dev_put(netdev);
+	module_put(THIS_MODULE);
 }
 
 /**
@@ -505,7 +504,7 @@ static void fcoe_fip_send(struct fcoe_ctlr *fip, struct sk_buff *skb)
 static void fcoe_update_src_mac(struct fc_lport *lport, u8 *addr)
 {
 	struct fcoe_port *port = lport_priv(lport);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 
 	rtnl_lock();
 	if (!is_zero_ether_addr(port->data_src_addr))
@@ -561,17 +560,6 @@ static int fcoe_lport_config(struct fc_lport *lport)
 }
 
 /**
- * fcoe_queue_timer() - The fcoe queue timer
- * @lport: The local port
- *
- * Calls fcoe_check_wait_queue on timeout
- */
-static void fcoe_queue_timer(ulong lport)
-{
-	fcoe_check_wait_queue((struct fc_lport *)lport, NULL);
-}
-
-/**
  * fcoe_get_wwn() - Get the world wide name from LLD if it supports it
  * @netdev: the associated net device
  * @wwn: the output WWN
@@ -650,7 +638,7 @@ static int fcoe_netdev_config(struct fc_lport *lport, struct net_device *netdev)
 
 	/* Setup lport private data to point to fcoe softc */
 	port = lport_priv(lport);
-	fcoe = port->fcoe;
+	fcoe = port->priv;
 
 	/*
 	 * Determine max frame size based on underlying device and optional
@@ -708,9 +696,9 @@ static int fcoe_shost_config(struct fc_lport *lport, struct device *dev)
 	lport->host->max_cmd_len = FCOE_MAX_CMD_LEN;
 
 	if (lport->vport)
-		lport->host->transportt = fcoe_vport_transport_template;
+		lport->host->transportt = fcoe_vport_scsi_transport;
 	else
-		lport->host->transportt = fcoe_transport_template;
+		lport->host->transportt = fcoe_nport_scsi_transport;
 
 	/* add the new host to the SCSI-ml */
 	rc = scsi_add_host(lport->host, dev);
@@ -760,7 +748,7 @@ bool fcoe_oem_match(struct fc_frame *fp)
 static inline int fcoe_em_config(struct fc_lport *lport)
 {
 	struct fcoe_port *port = lport_priv(lport);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 	struct fcoe_interface *oldfcoe = NULL;
 	struct net_device *old_real_dev, *cur_real_dev;
 	u16 min_xid = FCOE_MIN_XID;
@@ -844,7 +832,7 @@ skip_oem:
 static void fcoe_if_destroy(struct fc_lport *lport)
 {
 	struct fcoe_port *port = lport_priv(lport);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 	struct net_device *netdev = fcoe->netdev;
 
 	FCOE_NETDEV_DBG(netdev, "Destroying interface\n");
@@ -886,7 +874,6 @@ static void fcoe_if_destroy(struct fc_lport *lport)
 
 	/* Release the Scsi_Host */
 	scsi_host_put(lport->host);
-	module_put(THIS_MODULE);
 }
 
 /**
@@ -941,8 +928,9 @@ static struct fc_lport *fcoe_if_create(struct fcoe_interface *fcoe,
 				       struct device *parent, int npiv)
 {
 	struct net_device *netdev = fcoe->netdev;
-	struct fc_lport *lport = NULL;
+	struct fc_lport *lport, *n_port;
 	struct fcoe_port *port;
+	struct Scsi_Host *shost;
 	int rc;
 	/*
 	 * parent is only a vport if npiv is 1,
@@ -952,13 +940,11 @@ static struct fc_lport *fcoe_if_create(struct fcoe_interface *fcoe,
 
 	FCOE_NETDEV_DBG(netdev, "Create Interface\n");
 
-	if (!npiv) {
-		lport = libfc_host_alloc(&fcoe_shost_template,
-					 sizeof(struct fcoe_port));
-	} else	{
-		lport = libfc_vport_create(vport,
-					   sizeof(struct fcoe_port));
-	}
+	if (!npiv)
+		lport = libfc_host_alloc(&fcoe_shost_template, sizeof(*port));
+	else
+		lport = libfc_vport_create(vport, sizeof(*port));
+
 	if (!lport) {
 		FCOE_NETDEV_DBG(netdev, "Could not allocate host structure\n");
 		rc = -ENOMEM;
@@ -966,7 +952,9 @@ static struct fc_lport *fcoe_if_create(struct fcoe_interface *fcoe,
 	}
 	port = lport_priv(lport);
 	port->lport = lport;
-	port->fcoe = fcoe;
+	port->priv = fcoe;
+	port->max_queue_depth = FCOE_MAX_QUEUE_DEPTH;
+	port->min_queue_depth = FCOE_MIN_QUEUE_DEPTH;
 	INIT_WORK(&port->destroy_work, fcoe_destroy_work);
 
 	/* configure a fc_lport including the exchange manager */
@@ -1009,24 +997,27 @@ static struct fc_lport *fcoe_if_create(struct fcoe_interface *fcoe,
 		goto out_lp_destroy;
 	}
 
-	if (!npiv) {
-		/*
-		 * fcoe_em_alloc() and fcoe_hostlist_add() both
-		 * need to be atomic with respect to other changes to the
-		 * hostlist since fcoe_em_alloc() looks for an existing EM
-		 * instance on host list updated by fcoe_hostlist_add().
-		 *
-		 * This is currently handled through the fcoe_config_mutex
-		 * begin held.
-		 */
-
+	/*
+	 * fcoe_em_alloc() and fcoe_hostlist_add() both
+	 * need to be atomic with respect to other changes to the
+	 * hostlist since fcoe_em_alloc() looks for an existing EM
+	 * instance on host list updated by fcoe_hostlist_add().
+	 *
+	 * This is currently handled through the fcoe_config_mutex
+	 * begin held.
+	 */
+	if (!npiv)
 		/* lport exch manager allocation */
 		rc = fcoe_em_config(lport);
-		if (rc) {
-			FCOE_NETDEV_DBG(netdev, "Could not configure the EM "
-					"for the interface\n");
-			goto out_lp_destroy;
-		}
+	else {
+		shost = vport_to_shost(vport);
+		n_port = shost_priv(shost);
+		rc = fc_exch_mgr_list_clone(n_port, lport);
+	}
+
+	if (rc) {
+		FCOE_NETDEV_DBG(netdev, "Could not configure the EM\n");
+		goto out_lp_destroy;
 	}
 
 	fcoe_interface_get(fcoe);
@@ -1050,11 +1041,12 @@ out:
 static int __init fcoe_if_init(void)
 {
 	/* attach to scsi transport */
-	fcoe_transport_template = fc_attach_transport(&fcoe_transport_function);
-	fcoe_vport_transport_template =
-		fc_attach_transport(&fcoe_vport_transport_function);
+	fcoe_nport_scsi_transport =
+		fc_attach_transport(&fcoe_nport_fc_functions);
+	fcoe_vport_scsi_transport =
+		fc_attach_transport(&fcoe_vport_fc_functions);
 
-	if (!fcoe_transport_template) {
+	if (!fcoe_nport_scsi_transport) {
 		printk(KERN_ERR "fcoe: Failed to attach to the FC transport\n");
 		return -ENODEV;
 	}
@@ -1071,10 +1063,10 @@ static int __init fcoe_if_init(void)
  */
 int __exit fcoe_if_exit(void)
 {
-	fc_release_transport(fcoe_transport_template);
-	fc_release_transport(fcoe_vport_transport_template);
-	fcoe_transport_template = NULL;
-	fcoe_vport_transport_template = NULL;
+	fc_release_transport(fcoe_nport_scsi_transport);
+	fc_release_transport(fcoe_vport_scsi_transport);
+	fcoe_nport_scsi_transport = NULL;
+	fcoe_vport_scsi_transport = NULL;
 	return 0;
 }
 
@@ -1361,108 +1353,22 @@ err2:
 }
 
 /**
- * fcoe_start_io() - Start FCoE I/O
- * @skb: The packet to be transmitted
- *
- * This routine is called from the net device to start transmitting
- * FCoE packets.
- *
- * Returns: 0 for success
- */
-static inline int fcoe_start_io(struct sk_buff *skb)
-{
-	struct sk_buff *nskb;
-	int rc;
-
-	nskb = skb_clone(skb, GFP_ATOMIC);
-	rc = dev_queue_xmit(nskb);
-	if (rc != 0)
-		return rc;
-	kfree_skb(skb);
-	return 0;
-}
-
-/**
- * fcoe_get_paged_crc_eof() - Allocate a page to be used for the trailer CRC
+ * fcoe_alloc_paged_crc_eof() - Allocate a page to be used for the trailer CRC
  * @skb:  The packet to be transmitted
  * @tlen: The total length of the trailer
  *
- * This routine allocates a page for frame trailers. The page is re-used if
- * there is enough room left on it for the current trailer. If there isn't
- * enough buffer left a new page is allocated for the trailer. Reference to
- * the page from this function as well as the skbs using the page fragments
- * ensure that the page is freed at the appropriate time.
- *
  * Returns: 0 for success
  */
-static int fcoe_get_paged_crc_eof(struct sk_buff *skb, int tlen)
+static int fcoe_alloc_paged_crc_eof(struct sk_buff *skb, int tlen)
 {
 	struct fcoe_percpu_s *fps;
-	struct page *page;
+	int rc;
 
 	fps = &get_cpu_var(fcoe_percpu);
-	page = fps->crc_eof_page;
-	if (!page) {
-		page = alloc_page(GFP_ATOMIC);
-		if (!page) {
-			put_cpu_var(fcoe_percpu);
-			return -ENOMEM;
-		}
-		fps->crc_eof_page = page;
-		fps->crc_eof_offset = 0;
-	}
-
-	get_page(page);
-	skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags, page,
-			   fps->crc_eof_offset, tlen);
-	skb->len += tlen;
-	skb->data_len += tlen;
-	skb->truesize += tlen;
-	fps->crc_eof_offset += sizeof(struct fcoe_crc_eof);
-
-	if (fps->crc_eof_offset >= PAGE_SIZE) {
-		fps->crc_eof_page = NULL;
-		fps->crc_eof_offset = 0;
-		put_page(page);
-	}
+	rc = fcoe_get_paged_crc_eof(skb, tlen, fps);
 	put_cpu_var(fcoe_percpu);
-	return 0;
-}
 
-/**
- * fcoe_fc_crc() - Calculates the CRC for a given frame
- * @fp: The frame to be checksumed
- *
- * This uses crc32() routine to calculate the CRC for a frame
- *
- * Return: The 32 bit CRC value
- */
-u32 fcoe_fc_crc(struct fc_frame *fp)
-{
-	struct sk_buff *skb = fp_skb(fp);
-	struct skb_frag_struct *frag;
-	unsigned char *data;
-	unsigned long off, len, clen;
-	u32 crc;
-	unsigned i;
-
-	crc = crc32(~0, skb->data, skb_headlen(skb));
-
-	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
-		frag = &skb_shinfo(skb)->frags[i];
-		off = frag->page_offset;
-		len = frag->size;
-		while (len > 0) {
-			clen = min(len, PAGE_SIZE - (off & ~PAGE_MASK));
-			data = kmap_atomic(frag->page + (off >> PAGE_SHIFT),
-					   KM_SKB_DATA_SOFTIRQ);
-			crc = crc32(crc, data + (off & ~PAGE_MASK), clen);
-			kunmap_atomic(data, KM_SKB_DATA_SOFTIRQ);
-			off += clen;
-			len -= clen;
-		}
-	}
-	return crc;
+	return rc;
 }
 
 /**
@@ -1485,7 +1391,7 @@ int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	unsigned int tlen;		/* trailer length */
 	unsigned int elen;		/* eth header, may include vlan */
 	struct fcoe_port *port = lport_priv(lport);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 	u8 sof, eof;
 	struct fcoe_hdr *hp;
 
@@ -1526,7 +1432,7 @@ int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	/* copy port crc and eof to the skb buff */
 	if (skb_is_nonlinear(skb)) {
 		skb_frag_t *frag;
-		if (fcoe_get_paged_crc_eof(skb, tlen)) {
+		if (fcoe_alloc_paged_crc_eof(skb, tlen)) {
 			kfree_skb(skb);
 			return -ENOMEM;
 		}
@@ -1606,6 +1512,56 @@ static void fcoe_percpu_flush_done(struct sk_buff *skb)
 }
 
 /**
+ * fcoe_filter_frames() - filter out bad fcoe frames, i.e. bad CRC
+ * @lport: The local port the frame was received on
+ * @fp:	   The received frame
+ *
+ * Return: 0 on passing filtering checks
+ */
+static inline int fcoe_filter_frames(struct fc_lport *lport,
+				     struct fc_frame *fp)
+{
+	struct fcoe_interface *fcoe;
+	struct fc_frame_header *fh;
+	struct sk_buff *skb = (struct sk_buff *)fp;
+	struct fcoe_dev_stats *stats;
+
+	/*
+	 * We only check CRC if no offload is available and if it is
+	 * it's solicited data, in which case, the FCP layer would
+	 * check it during the copy.
+	 */
+	if (lport->crc_offload && skb->ip_summed == CHECKSUM_UNNECESSARY)
+		fr_flags(fp) &= ~FCPHF_CRC_UNCHECKED;
+	else
+		fr_flags(fp) |= FCPHF_CRC_UNCHECKED;
+
+	fh = (struct fc_frame_header *) skb_transport_header(skb);
+	fh = fc_frame_header_get(fp);
+	if (fh->fh_r_ctl == FC_RCTL_DD_SOL_DATA && fh->fh_type == FC_TYPE_FCP)
+		return 0;
+
+	fcoe = ((struct fcoe_port *)lport_priv(lport))->priv;
+	if (is_fip_mode(&fcoe->ctlr) && fc_frame_payload_op(fp) == ELS_LOGO &&
+	    ntoh24(fh->fh_s_id) == FC_FID_FLOGI) {
+		FCOE_DBG("fcoe: dropping FCoE lport LOGO in fip mode\n");
+		return -EINVAL;
+	}
+
+	if (!(fr_flags(fp) & FCPHF_CRC_UNCHECKED) ||
+	    le32_to_cpu(fr_crc(fp)) == ~crc32(~0, skb->data, skb->len)) {
+		fr_flags(fp) &= ~FCPHF_CRC_UNCHECKED;
+		return 0;
+	}
+
+	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats->InvalidCRCCount++;
+	if (stats->InvalidCRCCount < 5)
+		printk(KERN_WARNING "fcoe: dropping frame with CRC error\n");
+	return -EINVAL;
+}
+
+/**
  * fcoe_recv_frame() - process a single received frame
  * @skb: frame to process
  */
@@ -1615,7 +1571,6 @@ static void fcoe_recv_frame(struct sk_buff *skb)
 	struct fc_lport *lport;
 	struct fcoe_rcv_info *fr;
 	struct fcoe_dev_stats *stats;
-	struct fc_frame_header *fh;
 	struct fcoe_crc_eof crc_eof;
 	struct fc_frame *fp;
 	struct fcoe_port *port;
@@ -1646,7 +1601,6 @@ static void fcoe_recv_frame(struct sk_buff *skb)
 	 * was done in fcoe_rcv already.
 	 */
 	hp = (struct fcoe_hdr *) skb_network_header(skb);
-	fh = (struct fc_frame_header *) skb_transport_header(skb);
 
 	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
 	if (unlikely(FC_FCOE_DECAPS_VER(hp) != FC_FCOE_VER)) {
@@ -1679,35 +1633,11 @@ static void fcoe_recv_frame(struct sk_buff *skb)
 	if (pskb_trim(skb, fr_len))
 		goto drop;
 
-	/*
-	 * We only check CRC if no offload is available and if it is
-	 * it's solicited data, in which case, the FCP layer would
-	 * check it during the copy.
-	 */
-	if (lport->crc_offload &&
-	    skb->ip_summed == CHECKSUM_UNNECESSARY)
-		fr_flags(fp) &= ~FCPHF_CRC_UNCHECKED;
-	else
-		fr_flags(fp) |= FCPHF_CRC_UNCHECKED;
-
-	fh = fc_frame_header_get(fp);
-	if ((fh->fh_r_ctl != FC_RCTL_DD_SOL_DATA ||
-	    fh->fh_type != FC_TYPE_FCP) &&
-	    (fr_flags(fp) & FCPHF_CRC_UNCHECKED)) {
-		if (le32_to_cpu(fr_crc(fp)) !=
-		    ~crc32(~0, skb->data, fr_len)) {
-			if (stats->InvalidCRCCount < 5)
-				printk(KERN_WARNING "fcoe: dropping "
-				       "frame with CRC error\n");
-			stats->InvalidCRCCount++;
-			goto drop;
-		}
-		fr_flags(fp) &= ~FCPHF_CRC_UNCHECKED;
+	if (!fcoe_filter_frames(lport, fp)) {
+		put_cpu();
+		fc_exch_recv(lport, fp);
+		return;
 	}
-	put_cpu();
-	fc_exch_recv(lport, fp);
-	return;
-
 drop:
 	stats->ErrorFrames++;
 	put_cpu();
@@ -1746,64 +1676,6 @@ int fcoe_percpu_receive_thread(void *arg)
 }
 
 /**
- * fcoe_check_wait_queue() - Attempt to clear the transmit backlog
- * @lport: The local port whose backlog is to be cleared
- *
- * This empties the wait_queue, dequeues the head of the wait_queue queue
- * and calls fcoe_start_io() for each packet. If all skb have been
- * transmitted it returns the qlen. If an error occurs it restores
- * wait_queue (to try again later) and returns -1.
- *
- * The wait_queue is used when the skb transmit fails. The failed skb
- * will go in the wait_queue which will be emptied by the timer function or
- * by the next skb transmit.
- */
-static void fcoe_check_wait_queue(struct fc_lport *lport, struct sk_buff *skb)
-{
-	struct fcoe_port *port = lport_priv(lport);
-	int rc;
-
-	spin_lock_bh(&port->fcoe_pending_queue.lock);
-
-	if (skb)
-		__skb_queue_tail(&port->fcoe_pending_queue, skb);
-
-	if (port->fcoe_pending_queue_active)
-		goto out;
-	port->fcoe_pending_queue_active = 1;
-
-	while (port->fcoe_pending_queue.qlen) {
-		/* keep qlen > 0 until fcoe_start_io succeeds */
-		port->fcoe_pending_queue.qlen++;
-		skb = __skb_dequeue(&port->fcoe_pending_queue);
-
-		spin_unlock_bh(&port->fcoe_pending_queue.lock);
-		rc = fcoe_start_io(skb);
-		spin_lock_bh(&port->fcoe_pending_queue.lock);
-
-		if (rc) {
-			__skb_queue_head(&port->fcoe_pending_queue, skb);
-			/* undo temporary increment above */
-			port->fcoe_pending_queue.qlen--;
-			break;
-		}
-		/* undo temporary increment above */
-		port->fcoe_pending_queue.qlen--;
-	}
-
-	if (port->fcoe_pending_queue.qlen < FCOE_LOW_QUEUE_DEPTH)
-		lport->qfull = 0;
-	if (port->fcoe_pending_queue.qlen && !timer_pending(&port->timer))
-		mod_timer(&port->timer, jiffies + 2);
-	port->fcoe_pending_queue_active = 0;
-out:
-	if (port->fcoe_pending_queue.qlen > FCOE_MAX_QUEUE_DEPTH)
-		lport->qfull = 1;
-	spin_unlock_bh(&port->fcoe_pending_queue.lock);
-	return;
-}
-
-/**
  * fcoe_dev_setup() - Setup the link change notification interface
  */
 static void fcoe_dev_setup(void)
@@ -1874,7 +1746,7 @@ static int fcoe_device_notification(struct notifier_block *notifier,
 		list_del(&fcoe->list);
 		port = lport_priv(fcoe->ctlr.lp);
 		fcoe_interface_cleanup(fcoe);
-		schedule_work(&port->destroy_work);
+		queue_work(fcoe_wq, &port->destroy_work);
 		goto out;
 		break;
 	case NETDEV_FEAT_CHANGE:
@@ -1900,39 +1772,16 @@ out:
 }
 
 /**
- * fcoe_if_to_netdev() - Parse a name buffer to get a net device
- * @buffer: The name of the net device
- *
- * Returns: NULL or a ptr to net_device
- */
-static struct net_device *fcoe_if_to_netdev(const char *buffer)
-{
-	char *cp;
-	char ifname[IFNAMSIZ + 2];
-
-	if (buffer) {
-		strlcpy(ifname, buffer, IFNAMSIZ);
-		cp = ifname + strlen(ifname);
-		while (--cp >= ifname && *cp == '\n')
-			*cp = '\0';
-		return dev_get_by_name(&init_net, ifname);
-	}
-	return NULL;
-}
-
-/**
  * fcoe_disable() - Disables a FCoE interface
- * @buffer: The name of the Ethernet interface to be disabled
- * @kp:	    The associated kernel parameter
+ * @netdev  : The net_device object the Ethernet interface to create on
  *
- * Called from sysfs.
+ * Called from fcoe transport.
  *
  * Returns: 0 for success
  */
-static int fcoe_disable(const char *buffer, struct kernel_param *kp)
+static int fcoe_disable(struct net_device *netdev)
 {
 	struct fcoe_interface *fcoe;
-	struct net_device *netdev;
 	int rc = 0;
 
 	mutex_lock(&fcoe_config_mutex);
@@ -1948,16 +1797,9 @@ static int fcoe_disable(const char *buffer, struct kernel_param *kp)
 	}
 #endif
 
-	netdev = fcoe_if_to_netdev(buffer);
-	if (!netdev) {
-		rc = -ENODEV;
-		goto out_nodev;
-	}
-
 	if (!rtnl_trylock()) {
-		dev_put(netdev);
 		mutex_unlock(&fcoe_config_mutex);
-		return restart_syscall();
+		return -ERESTARTSYS;
 	}
 
 	fcoe = fcoe_hostlist_lookup_port(netdev);
@@ -1969,7 +1811,6 @@ static int fcoe_disable(const char *buffer, struct kernel_param *kp)
 	} else
 		rc = -ENODEV;
 
-	dev_put(netdev);
 out_nodev:
 	mutex_unlock(&fcoe_config_mutex);
 	return rc;
@@ -1977,17 +1818,15 @@ out_nodev:
 
 /**
  * fcoe_enable() - Enables a FCoE interface
- * @buffer: The name of the Ethernet interface to be enabled
- * @kp:     The associated kernel parameter
+ * @netdev  : The net_device object the Ethernet interface to create on
  *
- * Called from sysfs.
+ * Called from fcoe transport.
  *
  * Returns: 0 for success
  */
-static int fcoe_enable(const char *buffer, struct kernel_param *kp)
+static int fcoe_enable(struct net_device *netdev)
 {
 	struct fcoe_interface *fcoe;
-	struct net_device *netdev;
 	int rc = 0;
 
 	mutex_lock(&fcoe_config_mutex);
@@ -2002,17 +1841,9 @@ static int fcoe_enable(const char *buffer, struct kernel_param *kp)
 		goto out_nodev;
 	}
 #endif
-
-	netdev = fcoe_if_to_netdev(buffer);
-	if (!netdev) {
-		rc = -ENODEV;
-		goto out_nodev;
-	}
-
 	if (!rtnl_trylock()) {
-		dev_put(netdev);
 		mutex_unlock(&fcoe_config_mutex);
-		return restart_syscall();
+		return -ERESTARTSYS;
 	}
 
 	fcoe = fcoe_hostlist_lookup_port(netdev);
@@ -2023,7 +1854,6 @@ static int fcoe_enable(const char *buffer, struct kernel_param *kp)
 	else if (!fcoe_link_ok(fcoe->ctlr.lp))
 		fcoe_ctlr_link_up(&fcoe->ctlr);
 
-	dev_put(netdev);
 out_nodev:
 	mutex_unlock(&fcoe_config_mutex);
 	return rc;
@@ -2031,17 +1861,15 @@ out_nodev:
 
 /**
  * fcoe_destroy() - Destroy a FCoE interface
- * @buffer: The name of the Ethernet interface to be destroyed
- * @kp:	    The associated kernel parameter
+ * @netdev  : The net_device object the Ethernet interface to create on
  *
- * Called from sysfs.
+ * Called from fcoe transport
  *
  * Returns: 0 for success
  */
-static int fcoe_destroy(const char *buffer, struct kernel_param *kp)
+static int fcoe_destroy(struct net_device *netdev)
 {
 	struct fcoe_interface *fcoe;
-	struct net_device *netdev;
 	int rc = 0;
 
 	mutex_lock(&fcoe_config_mutex);
@@ -2056,32 +1884,21 @@ static int fcoe_destroy(const char *buffer, struct kernel_param *kp)
 		goto out_nodev;
 	}
 #endif
-
-	netdev = fcoe_if_to_netdev(buffer);
-	if (!netdev) {
-		rc = -ENODEV;
-		goto out_nodev;
-	}
-
 	if (!rtnl_trylock()) {
-		dev_put(netdev);
 		mutex_unlock(&fcoe_config_mutex);
-		return restart_syscall();
+		return -ERESTARTSYS;
 	}
 
 	fcoe = fcoe_hostlist_lookup_port(netdev);
 	if (!fcoe) {
 		rtnl_unlock();
 		rc = -ENODEV;
-		goto out_putdev;
+		goto out_nodev;
 	}
 	fcoe_interface_cleanup(fcoe);
 	list_del(&fcoe->list);
 	/* RTNL mutex is dropped by fcoe_if_destroy */
 	fcoe_if_destroy(fcoe->ctlr.lp);
-
-out_putdev:
-	dev_put(netdev);
 out_nodev:
 	mutex_unlock(&fcoe_config_mutex);
 	return rc;
@@ -2104,27 +1921,39 @@ static void fcoe_destroy_work(struct work_struct *work)
 }
 
 /**
+ * fcoe_match() - Check if the FCoE is supported on the given netdevice
+ * @netdev  : The net_device object the Ethernet interface to create on
+ *
+ * Called from fcoe transport.
+ *
+ * Returns: always returns true as this is the default FCoE transport,
+ * i.e., support all netdevs.
+ */
+static bool fcoe_match(struct net_device *netdev)
+{
+	return true;
+}
+
+/**
  * fcoe_create() - Create a fcoe interface
- * @buffer: The name of the Ethernet interface to create on
- * @kp:	    The associated kernel param
+ * @netdev  : The net_device object the Ethernet interface to create on
+ * @fip_mode: The FIP mode for this creation
  *
- * Called from sysfs.
+ * Called from fcoe transport
  *
  * Returns: 0 for success
  */
-static int fcoe_create(const char *buffer, struct kernel_param *kp)
+static int fcoe_create(struct net_device *netdev, enum fip_state fip_mode)
 {
-	enum fip_state fip_mode = (enum fip_state)(long)kp->arg;
 	int rc;
 	struct fcoe_interface *fcoe;
 	struct fc_lport *lport;
-	struct net_device *netdev;
 
 	mutex_lock(&fcoe_config_mutex);
 
 	if (!rtnl_trylock()) {
 		mutex_unlock(&fcoe_config_mutex);
-		return restart_syscall();
+		return -ERESTARTSYS;
 	}
 
 #ifdef CONFIG_FCOE_MODULE
@@ -2135,31 +1964,20 @@ static int fcoe_create(const char *buffer, struct kernel_param *kp)
 	 */
 	if (THIS_MODULE->state != MODULE_STATE_LIVE) {
 		rc = -ENODEV;
-		goto out_nomod;
-	}
-#endif
-
-	if (!try_module_get(THIS_MODULE)) {
-		rc = -EINVAL;
-		goto out_nomod;
-	}
-
-	netdev = fcoe_if_to_netdev(buffer);
-	if (!netdev) {
-		rc = -ENODEV;
 		goto out_nodev;
 	}
+#endif
 
 	/* look for existing lport */
 	if (fcoe_hostlist_lookup(netdev)) {
 		rc = -EEXIST;
-		goto out_putdev;
+		goto out_nodev;
 	}
 
 	fcoe = fcoe_interface_create(netdev, fip_mode);
-	if (!fcoe) {
-		rc = -ENOMEM;
-		goto out_putdev;
+	if (IS_ERR(fcoe)) {
+		rc = PTR_ERR(fcoe);
+		goto out_nodev;
 	}
 
 	lport = fcoe_if_create(fcoe, &netdev->dev, 0);
@@ -2188,18 +2006,13 @@ static int fcoe_create(const char *buffer, struct kernel_param *kp)
 	 * should be holding a reference taken in fcoe_if_create().
 	 */
 	fcoe_interface_put(fcoe);
-	dev_put(netdev);
 	rtnl_unlock();
 	mutex_unlock(&fcoe_config_mutex);
 
 	return 0;
 out_free:
 	fcoe_interface_put(fcoe);
-out_putdev:
-	dev_put(netdev);
 out_nodev:
-	module_put(THIS_MODULE);
-out_nomod:
 	rtnl_unlock();
 	mutex_unlock(&fcoe_config_mutex);
 	return rc;
@@ -2214,8 +2027,7 @@ out_nomod:
  */
 int fcoe_link_speed_update(struct fc_lport *lport)
 {
-	struct fcoe_port *port = lport_priv(lport);
-	struct net_device *netdev = port->fcoe->netdev;
+	struct net_device *netdev = fcoe_netdev(lport);
 	struct ethtool_cmd ecmd = { ETHTOOL_GSET };
 
 	if (!dev_ethtool_get_settings(netdev, &ecmd)) {
@@ -2246,8 +2058,7 @@ int fcoe_link_speed_update(struct fc_lport *lport)
  */
 int fcoe_link_ok(struct fc_lport *lport)
 {
-	struct fcoe_port *port = lport_priv(lport);
-	struct net_device *netdev = port->fcoe->netdev;
+	struct net_device *netdev = fcoe_netdev(lport);
 
 	if (netif_oper_up(netdev))
 		return 0;
@@ -2311,24 +2122,6 @@ void fcoe_percpu_clean(struct fc_lport *lport)
 }
 
 /**
- * fcoe_clean_pending_queue() - Dequeue a skb and free it
- * @lport: The local port to dequeue a skb on
- */
-void fcoe_clean_pending_queue(struct fc_lport *lport)
-{
-	struct fcoe_port  *port = lport_priv(lport);
-	struct sk_buff *skb;
-
-	spin_lock_bh(&port->fcoe_pending_queue.lock);
-	while ((skb = __skb_dequeue(&port->fcoe_pending_queue)) != NULL) {
-		spin_unlock_bh(&port->fcoe_pending_queue.lock);
-		kfree_skb(skb);
-		spin_lock_bh(&port->fcoe_pending_queue.lock);
-	}
-	spin_unlock_bh(&port->fcoe_pending_queue.lock);
-}
-
-/**
  * fcoe_reset() - Reset a local port
  * @shost: The SCSI host associated with the local port to be reset
  *
@@ -2337,7 +2130,13 @@ void fcoe_clean_pending_queue(struct fc_lport *lport)
 int fcoe_reset(struct Scsi_Host *shost)
 {
 	struct fc_lport *lport = shost_priv(shost);
-	fc_lport_reset(lport);
+	struct fcoe_port *port = lport_priv(lport);
+	struct fcoe_interface *fcoe = port->priv;
+
+	fcoe_ctlr_link_down(&fcoe->ctlr);
+	fcoe_clean_pending_queue(fcoe->ctlr.lp);
+	if (!fcoe_link_ok(fcoe->ctlr.lp))
+		fcoe_ctlr_link_up(&fcoe->ctlr);
 	return 0;
 }
 
@@ -2395,12 +2194,24 @@ static int fcoe_hostlist_add(const struct fc_lport *lport)
 	fcoe = fcoe_hostlist_lookup_port(fcoe_netdev(lport));
 	if (!fcoe) {
 		port = lport_priv(lport);
-		fcoe = port->fcoe;
+		fcoe = port->priv;
 		list_add_tail(&fcoe->list, &fcoe_hostlist);
 	}
 	return 0;
 }
 
+
+static struct fcoe_transport fcoe_sw_transport = {
+	.name = {FCOE_TRANSPORT_DEFAULT},
+	.attached = false,
+	.list = LIST_HEAD_INIT(fcoe_sw_transport.list),
+	.match = fcoe_match,
+	.create = fcoe_create,
+	.destroy = fcoe_destroy,
+	.enable = fcoe_enable,
+	.disable = fcoe_disable,
+};
+
 /**
  * fcoe_init() - Initialize fcoe.ko
  *
@@ -2412,6 +2223,18 @@ static int __init fcoe_init(void)
 	unsigned int cpu;
 	int rc = 0;
 
+	fcoe_wq = alloc_workqueue("fcoe", 0, 0);
+	if (!fcoe_wq)
+		return -ENOMEM;
+
+	/* register as a fcoe transport */
+	rc = fcoe_transport_attach(&fcoe_sw_transport);
+	if (rc) {
+		printk(KERN_ERR "failed to register an fcoe transport, check "
+			"if libfcoe is loaded\n");
+		return rc;
+	}
+
 	mutex_lock(&fcoe_config_mutex);
 
 	for_each_possible_cpu(cpu) {
@@ -2442,6 +2265,7 @@ out_free:
 		fcoe_percpu_thread_destroy(cpu);
 	}
 	mutex_unlock(&fcoe_config_mutex);
+	destroy_workqueue(fcoe_wq);
 	return rc;
 }
 module_init(fcoe_init);
@@ -2467,7 +2291,7 @@ static void __exit fcoe_exit(void)
 		list_del(&fcoe->list);
 		port = lport_priv(fcoe->ctlr.lp);
 		fcoe_interface_cleanup(fcoe);
-		schedule_work(&port->destroy_work);
+		queue_work(fcoe_wq, &port->destroy_work);
 	}
 	rtnl_unlock();
 
@@ -2478,16 +2302,21 @@ static void __exit fcoe_exit(void)
 
 	mutex_unlock(&fcoe_config_mutex);
 
-	/* flush any asyncronous interface destroys,
-	 * this should happen after the netdev notifier is unregistered */
-	flush_scheduled_work();
-	/* That will flush out all the N_Ports on the hostlist, but now we
-	 * may have NPIV VN_Ports scheduled for destruction */
-	flush_scheduled_work();
+	/*
+	 * destroy_work's may be chained but destroy_workqueue()
+	 * can take care of them. Just kill the fcoe_wq.
+	 */
+	destroy_workqueue(fcoe_wq);
 
-	/* detach from scsi transport
-	 * must happen after all destroys are done, therefor after the flush */
+	/*
+	 * Detaching from the scsi transport must happen after all
+	 * destroys are done on the fcoe_wq. destroy_workqueue will
+	 * enusre the fcoe_wq is flushed.
+	 */
 	fcoe_if_exit();
+
+	/* detach from fcoe transport */
+	fcoe_transport_detach(&fcoe_sw_transport);
 }
 module_exit(fcoe_exit);
 
@@ -2559,7 +2388,7 @@ static struct fc_seq *fcoe_elsct_send(struct fc_lport *lport, u32 did,
 				      void *arg, u32 timeout)
 {
 	struct fcoe_port *port = lport_priv(lport);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 	struct fcoe_ctlr *fip = &fcoe->ctlr;
 	struct fc_frame_header *fh = fc_frame_header_get(fp);
 
@@ -2592,7 +2421,7 @@ static int fcoe_vport_create(struct fc_vport *vport, bool disabled)
 	struct Scsi_Host *shost = vport_to_shost(vport);
 	struct fc_lport *n_port = shost_priv(shost);
 	struct fcoe_port *port = lport_priv(n_port);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 	struct net_device *netdev = fcoe->netdev;
 	struct fc_lport *vn_port;
 
@@ -2632,7 +2461,7 @@ static int fcoe_vport_destroy(struct fc_vport *vport)
 	mutex_lock(&n_port->lp_mutex);
 	list_del(&vn_port->list);
 	mutex_unlock(&n_port->lp_mutex);
-	schedule_work(&port->destroy_work);
+	queue_work(fcoe_wq, &port->destroy_work);
 	return 0;
 }
 
@@ -2736,7 +2565,7 @@ static void fcoe_set_port_id(struct fc_lport *lport,
 			     u32 port_id, struct fc_frame *fp)
 {
 	struct fcoe_port *port = lport_priv(lport);
-	struct fcoe_interface *fcoe = port->fcoe;
+	struct fcoe_interface *fcoe = port->priv;
 
 	if (fp && fc_frame_payload_op(fp) == ELS_FLOGI)
 		fcoe_ctlr_recv_flogi(&fcoe->ctlr, lport, fp);
diff --git a/drivers/scsi/fcoe/fcoe.h b/drivers/scsi/fcoe/fcoe.h
index c69b2c56c2d1..408a6fd78fb4 100644
--- a/drivers/scsi/fcoe/fcoe.h
+++ b/drivers/scsi/fcoe/fcoe.h
@@ -24,7 +24,7 @@
 #include <linux/kthread.h>
 
 #define FCOE_MAX_QUEUE_DEPTH	256
-#define FCOE_LOW_QUEUE_DEPTH	32
+#define FCOE_MIN_QUEUE_DEPTH	32
 
 #define FCOE_WORD_TO_BYTE	4
 
@@ -40,12 +40,6 @@
 #define FCOE_MIN_XID		0x0000	/* the min xid supported by fcoe_sw */
 #define FCOE_MAX_XID		0x0FFF	/* the max xid supported by fcoe_sw */
 
-/*
- * Max MTU for FCoE: 14 (FCoE header) + 24 (FC header) + 2112 (max FC payload)
- * + 4 (FC CRC) + 4 (FCoE trailer) =  2158 bytes
- */
-#define FCOE_MTU	2158
-
 unsigned int fcoe_debug_logging;
 module_param_named(debug_logging, fcoe_debug_logging, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(debug_logging, "a bit mask of logging levels");
@@ -71,21 +65,6 @@ do {                                                            	\
 				  netdev->name, ##args);)
 
 /**
- * struct fcoe_percpu_s - The per-CPU context for FCoE receive threads
- * @thread:	    The thread context
- * @fcoe_rx_list:   The queue of pending packets to process
- * @page:	    The memory page for calculating frame trailer CRCs
- * @crc_eof_offset: The offset into the CRC page pointing to available
- *		    memory for a new trailer
- */
-struct fcoe_percpu_s {
-	struct task_struct *thread;
-	struct sk_buff_head fcoe_rx_list;
-	struct page *crc_eof_page;
-	int crc_eof_offset;
-};
-
-/**
  * struct fcoe_interface - A FCoE interface
  * @list:	      Handle for a list of FCoE interfaces
  * @netdev:	      The associated net device
@@ -108,30 +87,6 @@ struct fcoe_interface {
 	struct kref	   kref;
 };
 
-/**
- * struct fcoe_port - The FCoE private structure
- * @fcoe:		       The associated fcoe interface
- * @lport:		       The associated local port
- * @fcoe_pending_queue:	       The pending Rx queue of skbs
- * @fcoe_pending_queue_active: Indicates if the pending queue is active
- * @timer:		       The queue timer
- * @destroy_work:	       Handle for work context
- *			       (to prevent RTNL deadlocks)
- * @data_srt_addr:	       Source address for data
- *
- * An instance of this structure is to be allocated along with the
- * Scsi_Host and libfc fc_lport structures.
- */
-struct fcoe_port {
-	struct fcoe_interface *fcoe;
-	struct fc_lport	      *lport;
-	struct sk_buff_head   fcoe_pending_queue;
-	u8		      fcoe_pending_queue_active;
-	struct timer_list     timer;
-	struct work_struct    destroy_work;
-	u8		      data_src_addr[ETH_ALEN];
-};
-
 #define fcoe_from_ctlr(fip) container_of(fip, struct fcoe_interface, ctlr)
 
 /**
@@ -140,7 +95,8 @@ struct fcoe_port {
  */
 static inline struct net_device *fcoe_netdev(const struct fc_lport *lport)
 {
-	return ((struct fcoe_port *)lport_priv(lport))->fcoe->netdev;
+	return ((struct fcoe_interface *)
+			((struct fcoe_port *)lport_priv(lport))->priv)->netdev;
 }
 
 #endif /* _FCOE_H_ */
diff --git a/drivers/scsi/fcoe/libfcoe.c b/drivers/scsi/fcoe/fcoe_ctlr.c
index 625c6be25396..c93f007e702f 100644
--- a/drivers/scsi/fcoe/libfcoe.c
+++ b/drivers/scsi/fcoe/fcoe_ctlr.c
@@ -44,9 +44,7 @@
 #include <scsi/libfc.h>
 #include <scsi/libfcoe.h>
 
-MODULE_AUTHOR("Open-FCoE.org");
-MODULE_DESCRIPTION("FIP discovery protocol support for FCoE HBAs");
-MODULE_LICENSE("GPL v2");
+#include "libfcoe.h"
 
 #define	FCOE_CTLR_MIN_FKA	500		/* min keep alive (mS) */
 #define	FCOE_CTLR_DEF_FKA	FIP_DEF_FKA	/* default keep alive (mS) */
@@ -66,31 +64,7 @@ static u8 fcoe_all_enode[ETH_ALEN] = FIP_ALL_ENODE_MACS;
 static u8 fcoe_all_vn2vn[ETH_ALEN] = FIP_ALL_VN2VN_MACS;
 static u8 fcoe_all_p2p[ETH_ALEN] = FIP_ALL_P2P_MACS;
 
-unsigned int libfcoe_debug_logging;
-module_param_named(debug_logging, libfcoe_debug_logging, int, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(debug_logging, "a bit mask of logging levels");
-
-#define LIBFCOE_LOGGING	    0x01 /* General logging, not categorized */
-#define LIBFCOE_FIP_LOGGING 0x02 /* FIP logging */
-
-#define LIBFCOE_CHECK_LOGGING(LEVEL, CMD)		\
-do {							\
-	if (unlikely(libfcoe_debug_logging & LEVEL))	\
-		do {					\
-			CMD;				\
-		} while (0);				\
-} while (0)
-
-#define LIBFCOE_DBG(fmt, args...)					\
-	LIBFCOE_CHECK_LOGGING(LIBFCOE_LOGGING,				\
-			      printk(KERN_INFO "libfcoe: " fmt, ##args);)
-
-#define LIBFCOE_FIP_DBG(fip, fmt, args...)				\
-	LIBFCOE_CHECK_LOGGING(LIBFCOE_FIP_LOGGING,			\
-			      printk(KERN_INFO "host%d: fip: " fmt, 	\
-				     (fip)->lp->host->host_no, ##args);)
-
-static const char *fcoe_ctlr_states[] = {
+static const char * const fcoe_ctlr_states[] = {
 	[FIP_ST_DISABLED] =	"DISABLED",
 	[FIP_ST_LINK_WAIT] =	"LINK_WAIT",
 	[FIP_ST_AUTO] =		"AUTO",
@@ -308,8 +282,8 @@ static void fcoe_ctlr_solicit(struct fcoe_ctlr *fip, struct fcoe_fcf *fcf)
 			struct fip_mac_desc mac;
 			struct fip_wwn_desc wwnn;
 			struct fip_size_desc size;
-		} __attribute__((packed)) desc;
-	}  __attribute__((packed)) *sol;
+		} __packed desc;
+	}  __packed * sol;
 	u32 fcoe_size;
 
 	skb = dev_alloc_skb(sizeof(*sol));
@@ -456,7 +430,7 @@ static void fcoe_ctlr_send_keep_alive(struct fcoe_ctlr *fip,
 		struct ethhdr eth;
 		struct fip_header fip;
 		struct fip_mac_desc mac;
-	} __attribute__((packed)) *kal;
+	} __packed * kal;
 	struct fip_vn_desc *vn;
 	u32 len;
 	struct fc_lport *lp;
@@ -527,7 +501,7 @@ static int fcoe_ctlr_encaps(struct fcoe_ctlr *fip, struct fc_lport *lport,
 		struct ethhdr eth;
 		struct fip_header fip;
 		struct fip_encaps encaps;
-	} __attribute__((packed)) *cap;
+	} __packed * cap;
 	struct fc_frame_header *fh;
 	struct fip_mac_desc *mac;
 	struct fcoe_fcf *fcf;
@@ -1819,7 +1793,7 @@ static void fcoe_ctlr_vn_send(struct fcoe_ctlr *fip,
 		struct fip_mac_desc mac;
 		struct fip_wwn_desc wwnn;
 		struct fip_vn_desc vn;
-	} __attribute__((packed)) *frame;
+	} __packed * frame;
 	struct fip_fc4_feat *ff;
 	struct fip_size_desc *size;
 	u32 fcp_feat;
diff --git a/drivers/scsi/fcoe/fcoe_transport.c b/drivers/scsi/fcoe/fcoe_transport.c
new file mode 100644
index 000000000000..258684101bfd
--- /dev/null
+++ b/drivers/scsi/fcoe/fcoe_transport.c
@@ -0,0 +1,770 @@
+/*
+ * Copyright(c) 2008 - 2011 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Maintained at www.Open-FCoE.org
+ */
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/netdevice.h>
+#include <linux/errno.h>
+#include <linux/crc32.h>
+#include <scsi/libfcoe.h>
+
+#include "libfcoe.h"
+
+MODULE_AUTHOR("Open-FCoE.org");
+MODULE_DESCRIPTION("FIP discovery protocol and FCoE transport for FCoE HBAs");
+MODULE_LICENSE("GPL v2");
+
+static int fcoe_transport_create(const char *, struct kernel_param *);
+static int fcoe_transport_destroy(const char *, struct kernel_param *);
+static int fcoe_transport_show(char *buffer, const struct kernel_param *kp);
+static struct fcoe_transport *fcoe_transport_lookup(struct net_device *device);
+static struct fcoe_transport *fcoe_netdev_map_lookup(struct net_device *device);
+static int fcoe_transport_enable(const char *, struct kernel_param *);
+static int fcoe_transport_disable(const char *, struct kernel_param *);
+static int libfcoe_device_notification(struct notifier_block *notifier,
+				    ulong event, void *ptr);
+
+static LIST_HEAD(fcoe_transports);
+static DEFINE_MUTEX(ft_mutex);
+static LIST_HEAD(fcoe_netdevs);
+static DEFINE_MUTEX(fn_mutex);
+
+unsigned int libfcoe_debug_logging;
+module_param_named(debug_logging, libfcoe_debug_logging, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(debug_logging, "a bit mask of logging levels");
+
+module_param_call(show, NULL, fcoe_transport_show, NULL, S_IRUSR);
+__MODULE_PARM_TYPE(show, "string");
+MODULE_PARM_DESC(show, " Show attached FCoE transports");
+
+module_param_call(create, fcoe_transport_create, NULL,
+		  (void *)FIP_MODE_FABRIC, S_IWUSR);
+__MODULE_PARM_TYPE(create, "string");
+MODULE_PARM_DESC(create, " Creates fcoe instance on a ethernet interface");
+
+module_param_call(create_vn2vn, fcoe_transport_create, NULL,
+		  (void *)FIP_MODE_VN2VN, S_IWUSR);
+__MODULE_PARM_TYPE(create_vn2vn, "string");
+MODULE_PARM_DESC(create_vn2vn, " Creates a VN_node to VN_node FCoE instance "
+		 "on an Ethernet interface");
+
+module_param_call(destroy, fcoe_transport_destroy, NULL, NULL, S_IWUSR);
+__MODULE_PARM_TYPE(destroy, "string");
+MODULE_PARM_DESC(destroy, " Destroys fcoe instance on a ethernet interface");
+
+module_param_call(enable, fcoe_transport_enable, NULL, NULL, S_IWUSR);
+__MODULE_PARM_TYPE(enable, "string");
+MODULE_PARM_DESC(enable, " Enables fcoe on a ethernet interface.");
+
+module_param_call(disable, fcoe_transport_disable, NULL, NULL, S_IWUSR);
+__MODULE_PARM_TYPE(disable, "string");
+MODULE_PARM_DESC(disable, " Disables fcoe on a ethernet interface.");
+
+/* notification function for packets from net device */
+static struct notifier_block libfcoe_notifier = {
+	.notifier_call = libfcoe_device_notification,
+};
+
+/**
+ * fcoe_fc_crc() - Calculates the CRC for a given frame
+ * @fp: The frame to be checksumed
+ *
+ * This uses crc32() routine to calculate the CRC for a frame
+ *
+ * Return: The 32 bit CRC value
+ */
+u32 fcoe_fc_crc(struct fc_frame *fp)
+{
+	struct sk_buff *skb = fp_skb(fp);
+	struct skb_frag_struct *frag;
+	unsigned char *data;
+	unsigned long off, len, clen;
+	u32 crc;
+	unsigned i;
+
+	crc = crc32(~0, skb->data, skb_headlen(skb));
+
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		frag = &skb_shinfo(skb)->frags[i];
+		off = frag->page_offset;
+		len = frag->size;
+		while (len > 0) {
+			clen = min(len, PAGE_SIZE - (off & ~PAGE_MASK));
+			data = kmap_atomic(frag->page + (off >> PAGE_SHIFT),
+					   KM_SKB_DATA_SOFTIRQ);
+			crc = crc32(crc, data + (off & ~PAGE_MASK), clen);
+			kunmap_atomic(data, KM_SKB_DATA_SOFTIRQ);
+			off += clen;
+			len -= clen;
+		}
+	}
+	return crc;
+}
+EXPORT_SYMBOL_GPL(fcoe_fc_crc);
+
+/**
+ * fcoe_start_io() - Start FCoE I/O
+ * @skb: The packet to be transmitted
+ *
+ * This routine is called from the net device to start transmitting
+ * FCoE packets.
+ *
+ * Returns: 0 for success
+ */
+int fcoe_start_io(struct sk_buff *skb)
+{
+	struct sk_buff *nskb;
+	int rc;
+
+	nskb = skb_clone(skb, GFP_ATOMIC);
+	if (!nskb)
+		return -ENOMEM;
+	rc = dev_queue_xmit(nskb);
+	if (rc != 0)
+		return rc;
+	kfree_skb(skb);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fcoe_start_io);
+
+
+/**
+ * fcoe_clean_pending_queue() - Dequeue a skb and free it
+ * @lport: The local port to dequeue a skb on
+ */
+void fcoe_clean_pending_queue(struct fc_lport *lport)
+{
+	struct fcoe_port  *port = lport_priv(lport);
+	struct sk_buff *skb;
+
+	spin_lock_bh(&port->fcoe_pending_queue.lock);
+	while ((skb = __skb_dequeue(&port->fcoe_pending_queue)) != NULL) {
+		spin_unlock_bh(&port->fcoe_pending_queue.lock);
+		kfree_skb(skb);
+		spin_lock_bh(&port->fcoe_pending_queue.lock);
+	}
+	spin_unlock_bh(&port->fcoe_pending_queue.lock);
+}
+EXPORT_SYMBOL_GPL(fcoe_clean_pending_queue);
+
+/**
+ * fcoe_check_wait_queue() - Attempt to clear the transmit backlog
+ * @lport: The local port whose backlog is to be cleared
+ *
+ * This empties the wait_queue, dequeues the head of the wait_queue queue
+ * and calls fcoe_start_io() for each packet. If all skb have been
+ * transmitted it returns the qlen. If an error occurs it restores
+ * wait_queue (to try again later) and returns -1.
+ *
+ * The wait_queue is used when the skb transmit fails. The failed skb
+ * will go in the wait_queue which will be emptied by the timer function or
+ * by the next skb transmit.
+ */
+void fcoe_check_wait_queue(struct fc_lport *lport, struct sk_buff *skb)
+{
+	struct fcoe_port *port = lport_priv(lport);
+	int rc;
+
+	spin_lock_bh(&port->fcoe_pending_queue.lock);
+
+	if (skb)
+		__skb_queue_tail(&port->fcoe_pending_queue, skb);
+
+	if (port->fcoe_pending_queue_active)
+		goto out;
+	port->fcoe_pending_queue_active = 1;
+
+	while (port->fcoe_pending_queue.qlen) {
+		/* keep qlen > 0 until fcoe_start_io succeeds */
+		port->fcoe_pending_queue.qlen++;
+		skb = __skb_dequeue(&port->fcoe_pending_queue);
+
+		spin_unlock_bh(&port->fcoe_pending_queue.lock);
+		rc = fcoe_start_io(skb);
+		spin_lock_bh(&port->fcoe_pending_queue.lock);
+
+		if (rc) {
+			__skb_queue_head(&port->fcoe_pending_queue, skb);
+			/* undo temporary increment above */
+			port->fcoe_pending_queue.qlen--;
+			break;
+		}
+		/* undo temporary increment above */
+		port->fcoe_pending_queue.qlen--;
+	}
+
+	if (port->fcoe_pending_queue.qlen < port->min_queue_depth)
+		lport->qfull = 0;
+	if (port->fcoe_pending_queue.qlen && !timer_pending(&port->timer))
+		mod_timer(&port->timer, jiffies + 2);
+	port->fcoe_pending_queue_active = 0;
+out:
+	if (port->fcoe_pending_queue.qlen > port->max_queue_depth)
+		lport->qfull = 1;
+	spin_unlock_bh(&port->fcoe_pending_queue.lock);
+}
+EXPORT_SYMBOL_GPL(fcoe_check_wait_queue);
+
+/**
+ * fcoe_queue_timer() - The fcoe queue timer
+ * @lport: The local port
+ *
+ * Calls fcoe_check_wait_queue on timeout
+ */
+void fcoe_queue_timer(ulong lport)
+{
+	fcoe_check_wait_queue((struct fc_lport *)lport, NULL);
+}
+EXPORT_SYMBOL_GPL(fcoe_queue_timer);
+
+/**
+ * fcoe_get_paged_crc_eof() - Allocate a page to be used for the trailer CRC
+ * @skb:  The packet to be transmitted
+ * @tlen: The total length of the trailer
+ * @fps:  The fcoe context
+ *
+ * This routine allocates a page for frame trailers. The page is re-used if
+ * there is enough room left on it for the current trailer. If there isn't
+ * enough buffer left a new page is allocated for the trailer. Reference to
+ * the page from this function as well as the skbs using the page fragments
+ * ensure that the page is freed at the appropriate time.
+ *
+ * Returns: 0 for success
+ */
+int fcoe_get_paged_crc_eof(struct sk_buff *skb, int tlen,
+			   struct fcoe_percpu_s *fps)
+{
+	struct page *page;
+
+	page = fps->crc_eof_page;
+	if (!page) {
+		page = alloc_page(GFP_ATOMIC);
+		if (!page)
+			return -ENOMEM;
+
+		fps->crc_eof_page = page;
+		fps->crc_eof_offset = 0;
+	}
+
+	get_page(page);
+	skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags, page,
+			   fps->crc_eof_offset, tlen);
+	skb->len += tlen;
+	skb->data_len += tlen;
+	skb->truesize += tlen;
+	fps->crc_eof_offset += sizeof(struct fcoe_crc_eof);
+
+	if (fps->crc_eof_offset >= PAGE_SIZE) {
+		fps->crc_eof_page = NULL;
+		fps->crc_eof_offset = 0;
+		put_page(page);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fcoe_get_paged_crc_eof);
+
+/**
+ * fcoe_transport_lookup - find an fcoe transport that matches a netdev
+ * @netdev: The netdev to look for from all attached transports
+ *
+ * Returns : ptr to the fcoe transport that supports this netdev or NULL
+ * if not found.
+ *
+ * The ft_mutex should be held when this is called
+ */
+static struct fcoe_transport *fcoe_transport_lookup(struct net_device *netdev)
+{
+	struct fcoe_transport *ft = NULL;
+
+	list_for_each_entry(ft, &fcoe_transports, list)
+		if (ft->match && ft->match(netdev))
+			return ft;
+	return NULL;
+}
+
+/**
+ * fcoe_transport_attach - Attaches an FCoE transport
+ * @ft: The fcoe transport to be attached
+ *
+ * Returns : 0 for success
+ */
+int fcoe_transport_attach(struct fcoe_transport *ft)
+{
+	int rc = 0;
+
+	mutex_lock(&ft_mutex);
+	if (ft->attached) {
+		LIBFCOE_TRANSPORT_DBG("transport %s already attached\n",
+				       ft->name);
+		rc = -EEXIST;
+		goto out_attach;
+	}
+
+	/* Add default transport to the tail */
+	if (strcmp(ft->name, FCOE_TRANSPORT_DEFAULT))
+		list_add(&ft->list, &fcoe_transports);
+	else
+		list_add_tail(&ft->list, &fcoe_transports);
+
+	ft->attached = true;
+	LIBFCOE_TRANSPORT_DBG("attaching transport %s\n", ft->name);
+
+out_attach:
+	mutex_unlock(&ft_mutex);
+	return rc;
+}
+EXPORT_SYMBOL(fcoe_transport_attach);
+
+/**
+ * fcoe_transport_attach - Detaches an FCoE transport
+ * @ft: The fcoe transport to be attached
+ *
+ * Returns : 0 for success
+ */
+int fcoe_transport_detach(struct fcoe_transport *ft)
+{
+	int rc = 0;
+
+	mutex_lock(&ft_mutex);
+	if (!ft->attached) {
+		LIBFCOE_TRANSPORT_DBG("transport %s already detached\n",
+			ft->name);
+		rc = -ENODEV;
+		goto out_attach;
+	}
+
+	list_del(&ft->list);
+	ft->attached = false;
+	LIBFCOE_TRANSPORT_DBG("detaching transport %s\n", ft->name);
+
+out_attach:
+	mutex_unlock(&ft_mutex);
+	return rc;
+
+}
+EXPORT_SYMBOL(fcoe_transport_detach);
+
+static int fcoe_transport_show(char *buffer, const struct kernel_param *kp)
+{
+	int i, j;
+	struct fcoe_transport *ft = NULL;
+
+	i = j = sprintf(buffer, "Attached FCoE transports:");
+	mutex_lock(&ft_mutex);
+	list_for_each_entry(ft, &fcoe_transports, list) {
+		i += snprintf(&buffer[i], IFNAMSIZ, "%s ", ft->name);
+		if (i >= PAGE_SIZE)
+			break;
+	}
+	mutex_unlock(&ft_mutex);
+	if (i == j)
+		i += snprintf(&buffer[i], IFNAMSIZ, "none");
+	return i;
+}
+
+static int __init fcoe_transport_init(void)
+{
+	register_netdevice_notifier(&libfcoe_notifier);
+	return 0;
+}
+
+static int __exit fcoe_transport_exit(void)
+{
+	struct fcoe_transport *ft;
+
+	unregister_netdevice_notifier(&libfcoe_notifier);
+	mutex_lock(&ft_mutex);
+	list_for_each_entry(ft, &fcoe_transports, list)
+		printk(KERN_ERR "FCoE transport %s is still attached!\n",
+		      ft->name);
+	mutex_unlock(&ft_mutex);
+	return 0;
+}
+
+
+static int fcoe_add_netdev_mapping(struct net_device *netdev,
+					struct fcoe_transport *ft)
+{
+	struct fcoe_netdev_mapping *nm;
+
+	nm = kmalloc(sizeof(*nm), GFP_KERNEL);
+	if (!nm) {
+		printk(KERN_ERR "Unable to allocate netdev_mapping");
+		return -ENOMEM;
+	}
+
+	nm->netdev = netdev;
+	nm->ft = ft;
+
+	mutex_lock(&fn_mutex);
+	list_add(&nm->list, &fcoe_netdevs);
+	mutex_unlock(&fn_mutex);
+	return 0;
+}
+
+
+static void fcoe_del_netdev_mapping(struct net_device *netdev)
+{
+	struct fcoe_netdev_mapping *nm = NULL, *tmp;
+
+	mutex_lock(&fn_mutex);
+	list_for_each_entry_safe(nm, tmp, &fcoe_netdevs, list) {
+		if (nm->netdev == netdev) {
+			list_del(&nm->list);
+			kfree(nm);
+			mutex_unlock(&fn_mutex);
+			return;
+		}
+	}
+	mutex_unlock(&fn_mutex);
+}
+
+
+/**
+ * fcoe_netdev_map_lookup - find the fcoe transport that matches the netdev on which
+ * it was created
+ *
+ * Returns : ptr to the fcoe transport that supports this netdev or NULL
+ * if not found.
+ *
+ * The ft_mutex should be held when this is called
+ */
+static struct fcoe_transport *fcoe_netdev_map_lookup(struct net_device *netdev)
+{
+	struct fcoe_transport *ft = NULL;
+	struct fcoe_netdev_mapping *nm;
+
+	mutex_lock(&fn_mutex);
+	list_for_each_entry(nm, &fcoe_netdevs, list) {
+		if (netdev == nm->netdev) {
+			ft = nm->ft;
+			mutex_unlock(&fn_mutex);
+			return ft;
+		}
+	}
+
+	mutex_unlock(&fn_mutex);
+	return NULL;
+}
+
+/**
+ * fcoe_if_to_netdev() - Parse a name buffer to get a net device
+ * @buffer: The name of the net device
+ *
+ * Returns: NULL or a ptr to net_device
+ */
+static struct net_device *fcoe_if_to_netdev(const char *buffer)
+{
+	char *cp;
+	char ifname[IFNAMSIZ + 2];
+
+	if (buffer) {
+		strlcpy(ifname, buffer, IFNAMSIZ);
+		cp = ifname + strlen(ifname);
+		while (--cp >= ifname && *cp == '\n')
+			*cp = '\0';
+		return dev_get_by_name(&init_net, ifname);
+	}
+	return NULL;
+}
+
+/**
+ * libfcoe_device_notification() - Handler for net device events
+ * @notifier: The context of the notification
+ * @event:    The type of event
+ * @ptr:      The net device that the event was on
+ *
+ * This function is called by the Ethernet driver in case of link change event.
+ *
+ * Returns: 0 for success
+ */
+static int libfcoe_device_notification(struct notifier_block *notifier,
+				    ulong event, void *ptr)
+{
+	struct net_device *netdev = ptr;
+
+	switch (event) {
+	case NETDEV_UNREGISTER:
+		printk(KERN_ERR "libfcoe_device_notification: NETDEV_UNREGISTER %s\n",
+				netdev->name);
+		fcoe_del_netdev_mapping(netdev);
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+
+/**
+ * fcoe_transport_create() - Create a fcoe interface
+ * @buffer: The name of the Ethernet interface to create on
+ * @kp:	    The associated kernel param
+ *
+ * Called from sysfs. This holds the ft_mutex while calling the
+ * registered fcoe transport's create function.
+ *
+ * Returns: 0 for success
+ */
+static int fcoe_transport_create(const char *buffer, struct kernel_param *kp)
+{
+	int rc = -ENODEV;
+	struct net_device *netdev = NULL;
+	struct fcoe_transport *ft = NULL;
+	enum fip_state fip_mode = (enum fip_state)(long)kp->arg;
+
+	if (!mutex_trylock(&ft_mutex))
+		return restart_syscall();
+
+#ifdef CONFIG_LIBFCOE_MODULE
+	/*
+	 * Make sure the module has been initialized, and is not about to be
+	 * removed.  Module parameter sysfs files are writable before the
+	 * module_init function is called and after module_exit.
+	 */
+	if (THIS_MODULE->state != MODULE_STATE_LIVE)
+		goto out_nodev;
+#endif
+
+	netdev = fcoe_if_to_netdev(buffer);
+	if (!netdev) {
+		LIBFCOE_TRANSPORT_DBG("Invalid device %s.\n", buffer);
+		goto out_nodev;
+	}
+
+	ft = fcoe_netdev_map_lookup(netdev);
+	if (ft) {
+		LIBFCOE_TRANSPORT_DBG("transport %s already has existing "
+				      "FCoE instance on %s.\n",
+				      ft->name, netdev->name);
+		rc = -EEXIST;
+		goto out_putdev;
+	}
+
+	ft = fcoe_transport_lookup(netdev);
+	if (!ft) {
+		LIBFCOE_TRANSPORT_DBG("no FCoE transport found for %s.\n",
+				      netdev->name);
+		goto out_putdev;
+	}
+
+	rc = fcoe_add_netdev_mapping(netdev, ft);
+	if (rc) {
+		LIBFCOE_TRANSPORT_DBG("failed to add new netdev mapping "
+				      "for FCoE transport %s for %s.\n",
+				      ft->name, netdev->name);
+		goto out_putdev;
+	}
+
+	/* pass to transport create */
+	rc = ft->create ? ft->create(netdev, fip_mode) : -ENODEV;
+	if (rc)
+		fcoe_del_netdev_mapping(netdev);
+
+	LIBFCOE_TRANSPORT_DBG("transport %s %s to create fcoe on %s.\n",
+			      ft->name, (rc) ? "failed" : "succeeded",
+			      netdev->name);
+
+out_putdev:
+	dev_put(netdev);
+out_nodev:
+	mutex_unlock(&ft_mutex);
+	if (rc == -ERESTARTSYS)
+		return restart_syscall();
+	else
+		return rc;
+}
+
+/**
+ * fcoe_transport_destroy() - Destroy a FCoE interface
+ * @buffer: The name of the Ethernet interface to be destroyed
+ * @kp:	    The associated kernel parameter
+ *
+ * Called from sysfs. This holds the ft_mutex while calling the
+ * registered fcoe transport's destroy function.
+ *
+ * Returns: 0 for success
+ */
+static int fcoe_transport_destroy(const char *buffer, struct kernel_param *kp)
+{
+	int rc = -ENODEV;
+	struct net_device *netdev = NULL;
+	struct fcoe_transport *ft = NULL;
+
+	if (!mutex_trylock(&ft_mutex))
+		return restart_syscall();
+
+#ifdef CONFIG_LIBFCOE_MODULE
+	/*
+	 * Make sure the module has been initialized, and is not about to be
+	 * removed.  Module parameter sysfs files are writable before the
+	 * module_init function is called and after module_exit.
+	 */
+	if (THIS_MODULE->state != MODULE_STATE_LIVE)
+		goto out_nodev;
+#endif
+
+	netdev = fcoe_if_to_netdev(buffer);
+	if (!netdev) {
+		LIBFCOE_TRANSPORT_DBG("invalid device %s.\n", buffer);
+		goto out_nodev;
+	}
+
+	ft = fcoe_netdev_map_lookup(netdev);
+	if (!ft) {
+		LIBFCOE_TRANSPORT_DBG("no FCoE transport found for %s.\n",
+				      netdev->name);
+		goto out_putdev;
+	}
+
+	/* pass to transport destroy */
+	rc = ft->destroy ? ft->destroy(netdev) : -ENODEV;
+	fcoe_del_netdev_mapping(netdev);
+	LIBFCOE_TRANSPORT_DBG("transport %s %s to destroy fcoe on %s.\n",
+			      ft->name, (rc) ? "failed" : "succeeded",
+			      netdev->name);
+
+out_putdev:
+	dev_put(netdev);
+out_nodev:
+	mutex_unlock(&ft_mutex);
+
+	if (rc == -ERESTARTSYS)
+		return restart_syscall();
+	else
+		return rc;
+}
+
+/**
+ * fcoe_transport_disable() - Disables a FCoE interface
+ * @buffer: The name of the Ethernet interface to be disabled
+ * @kp:	    The associated kernel parameter
+ *
+ * Called from sysfs.
+ *
+ * Returns: 0 for success
+ */
+static int fcoe_transport_disable(const char *buffer, struct kernel_param *kp)
+{
+	int rc = -ENODEV;
+	struct net_device *netdev = NULL;
+	struct fcoe_transport *ft = NULL;
+
+	if (!mutex_trylock(&ft_mutex))
+		return restart_syscall();
+
+#ifdef CONFIG_LIBFCOE_MODULE
+	/*
+	 * Make sure the module has been initialized, and is not about to be
+	 * removed.  Module parameter sysfs files are writable before the
+	 * module_init function is called and after module_exit.
+	 */
+	if (THIS_MODULE->state != MODULE_STATE_LIVE)
+		goto out_nodev;
+#endif
+
+	netdev = fcoe_if_to_netdev(buffer);
+	if (!netdev)
+		goto out_nodev;
+
+	ft = fcoe_netdev_map_lookup(netdev);
+	if (!ft)
+		goto out_putdev;
+
+	rc = ft->disable ? ft->disable(netdev) : -ENODEV;
+
+out_putdev:
+	dev_put(netdev);
+out_nodev:
+	mutex_unlock(&ft_mutex);
+
+	if (rc == -ERESTARTSYS)
+		return restart_syscall();
+	else
+		return rc;
+}
+
+/**
+ * fcoe_transport_enable() - Enables a FCoE interface
+ * @buffer: The name of the Ethernet interface to be enabled
+ * @kp:     The associated kernel parameter
+ *
+ * Called from sysfs.
+ *
+ * Returns: 0 for success
+ */
+static int fcoe_transport_enable(const char *buffer, struct kernel_param *kp)
+{
+	int rc = -ENODEV;
+	struct net_device *netdev = NULL;
+	struct fcoe_transport *ft = NULL;
+
+	if (!mutex_trylock(&ft_mutex))
+		return restart_syscall();
+
+#ifdef CONFIG_LIBFCOE_MODULE
+	/*
+	 * Make sure the module has been initialized, and is not about to be
+	 * removed.  Module parameter sysfs files are writable before the
+	 * module_init function is called and after module_exit.
+	 */
+	if (THIS_MODULE->state != MODULE_STATE_LIVE)
+		goto out_nodev;
+#endif
+
+	netdev = fcoe_if_to_netdev(buffer);
+	if (!netdev)
+		goto out_nodev;
+
+	ft = fcoe_netdev_map_lookup(netdev);
+	if (!ft)
+		goto out_putdev;
+
+	rc = ft->enable ? ft->enable(netdev) : -ENODEV;
+
+out_putdev:
+	dev_put(netdev);
+out_nodev:
+	mutex_unlock(&ft_mutex);
+	if (rc == -ERESTARTSYS)
+		return restart_syscall();
+	else
+		return rc;
+}
+
+/**
+ * libfcoe_init() - Initialization routine for libfcoe.ko
+ */
+static int __init libfcoe_init(void)
+{
+	fcoe_transport_init();
+
+	return 0;
+}
+module_init(libfcoe_init);
+
+/**
+ * libfcoe_exit() - Tear down libfcoe.ko
+ */
+static void __exit libfcoe_exit(void)
+{
+	fcoe_transport_exit();
+}
+module_exit(libfcoe_exit);
diff --git a/drivers/scsi/fcoe/libfcoe.h b/drivers/scsi/fcoe/libfcoe.h
new file mode 100644
index 000000000000..6af5fc3a17d8
--- /dev/null
+++ b/drivers/scsi/fcoe/libfcoe.h
@@ -0,0 +1,31 @@
+#ifndef _FCOE_LIBFCOE_H_
+#define _FCOE_LIBFCOE_H_
+
+extern unsigned int libfcoe_debug_logging;
+#define LIBFCOE_LOGGING	    0x01 /* General logging, not categorized */
+#define LIBFCOE_FIP_LOGGING 0x02 /* FIP logging */
+#define LIBFCOE_TRANSPORT_LOGGING	0x04 /* FCoE transport logging */
+
+#define LIBFCOE_CHECK_LOGGING(LEVEL, CMD)		\
+do {							\
+	if (unlikely(libfcoe_debug_logging & LEVEL))	\
+		do {					\
+			CMD;				\
+		} while (0);				\
+} while (0)
+
+#define LIBFCOE_DBG(fmt, args...)					\
+	LIBFCOE_CHECK_LOGGING(LIBFCOE_LOGGING,				\
+			      printk(KERN_INFO "libfcoe: " fmt, ##args);)
+
+#define LIBFCOE_FIP_DBG(fip, fmt, args...)				\
+	LIBFCOE_CHECK_LOGGING(LIBFCOE_FIP_LOGGING,			\
+			      printk(KERN_INFO "host%d: fip: " fmt,	\
+				     (fip)->lp->host->host_no, ##args);)
+
+#define LIBFCOE_TRANSPORT_DBG(fmt, args...)				\
+	LIBFCOE_CHECK_LOGGING(LIBFCOE_TRANSPORT_LOGGING,		\
+			      printk(KERN_INFO "%s: " fmt,		\
+				     __func__, ##args);)
+
+#endif /* _FCOE_LIBFCOE_H_ */
diff --git a/drivers/scsi/fnic/fnic.h b/drivers/scsi/fnic/fnic.h
index 92f185081e62..671cde9d4060 100644
--- a/drivers/scsi/fnic/fnic.h
+++ b/drivers/scsi/fnic/fnic.h
@@ -37,7 +37,7 @@
 
 #define DRV_NAME		"fnic"
 #define DRV_DESCRIPTION		"Cisco FCoE HBA Driver"
-#define DRV_VERSION		"1.4.0.145"
+#define DRV_VERSION		"1.5.0.1"
 #define PFX			DRV_NAME ": "
 #define DFX                     DRV_NAME "%d: "
 
diff --git a/drivers/scsi/fnic/vnic_dev.c b/drivers/scsi/fnic/vnic_dev.c
index db710148d156..b576be734e2e 100644
--- a/drivers/scsi/fnic/vnic_dev.c
+++ b/drivers/scsi/fnic/vnic_dev.c
@@ -654,7 +654,7 @@ void vnic_dev_unregister(struct vnic_dev *vdev)
 				vdev->linkstatus_pa);
 		if (vdev->stats)
 			pci_free_consistent(vdev->pdev,
-				sizeof(struct vnic_dev),
+				sizeof(struct vnic_stats),
 				vdev->stats, vdev->stats_pa);
 		if (vdev->fw_info)
 			pci_free_consistent(vdev->pdev,
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 12deffccb8da..c30591f84a06 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -74,6 +74,10 @@ static int hpsa_allow_any;
 module_param(hpsa_allow_any, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(hpsa_allow_any,
 		"Allow hpsa driver to access unknown HP Smart Array hardware");
+static int hpsa_simple_mode;
+module_param(hpsa_simple_mode, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(hpsa_simple_mode,
+	"Use 'simple mode' rather than 'performant mode'");
 
 /* define the PCI info for the cards we can control */
 static const struct pci_device_id hpsa_pci_device_id[] = {
@@ -155,6 +159,10 @@ static ssize_t unique_id_show(struct device *dev,
 	struct device_attribute *attr, char *buf);
 static ssize_t host_show_firmware_revision(struct device *dev,
 	     struct device_attribute *attr, char *buf);
+static ssize_t host_show_commands_outstanding(struct device *dev,
+	     struct device_attribute *attr, char *buf);
+static ssize_t host_show_transport_mode(struct device *dev,
+	struct device_attribute *attr, char *buf);
 static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno);
 static ssize_t host_store_rescan(struct device *dev,
 	 struct device_attribute *attr, const char *buf, size_t count);
@@ -173,6 +181,10 @@ static int __devinit hpsa_find_cfg_addrs(struct pci_dev *pdev,
 static int __devinit hpsa_pci_find_memory_BAR(struct pci_dev *pdev,
 	unsigned long *memory_bar);
 static int __devinit hpsa_lookup_board_id(struct pci_dev *pdev, u32 *board_id);
+static int __devinit hpsa_wait_for_board_state(struct pci_dev *pdev,
+	void __iomem *vaddr, int wait_for_ready);
+#define BOARD_NOT_READY 0
+#define BOARD_READY 1
 
 static DEVICE_ATTR(raid_level, S_IRUGO, raid_level_show, NULL);
 static DEVICE_ATTR(lunid, S_IRUGO, lunid_show, NULL);
@@ -180,6 +192,10 @@ static DEVICE_ATTR(unique_id, S_IRUGO, unique_id_show, NULL);
 static DEVICE_ATTR(rescan, S_IWUSR, NULL, host_store_rescan);
 static DEVICE_ATTR(firmware_revision, S_IRUGO,
 	host_show_firmware_revision, NULL);
+static DEVICE_ATTR(commands_outstanding, S_IRUGO,
+	host_show_commands_outstanding, NULL);
+static DEVICE_ATTR(transport_mode, S_IRUGO,
+	host_show_transport_mode, NULL);
 
 static struct device_attribute *hpsa_sdev_attrs[] = {
 	&dev_attr_raid_level,
@@ -191,6 +207,8 @@ static struct device_attribute *hpsa_sdev_attrs[] = {
 static struct device_attribute *hpsa_shost_attrs[] = {
 	&dev_attr_rescan,
 	&dev_attr_firmware_revision,
+	&dev_attr_commands_outstanding,
+	&dev_attr_transport_mode,
 	NULL,
 };
 
@@ -291,17 +309,38 @@ static ssize_t host_show_firmware_revision(struct device *dev,
 		fwrev[0], fwrev[1], fwrev[2], fwrev[3]);
 }
 
+static ssize_t host_show_commands_outstanding(struct device *dev,
+	     struct device_attribute *attr, char *buf)
+{
+	struct Scsi_Host *shost = class_to_shost(dev);
+	struct ctlr_info *h = shost_to_hba(shost);
+
+	return snprintf(buf, 20, "%d\n", h->commands_outstanding);
+}
+
+static ssize_t host_show_transport_mode(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct ctlr_info *h;
+	struct Scsi_Host *shost = class_to_shost(dev);
+
+	h = shost_to_hba(shost);
+	return snprintf(buf, 20, "%s\n",
+		h->transMethod & CFGTBL_Trans_Performant ?
+			"performant" : "simple");
+}
+
 /* Enqueuing and dequeuing functions for cmdlists. */
-static inline void addQ(struct hlist_head *list, struct CommandList *c)
+static inline void addQ(struct list_head *list, struct CommandList *c)
 {
-	hlist_add_head(&c->list, list);
+	list_add_tail(&c->list, list);
 }
 
 static inline u32 next_command(struct ctlr_info *h)
 {
 	u32 a;
 
-	if (unlikely(h->transMethod != CFGTBL_Trans_Performant))
+	if (unlikely(!(h->transMethod & CFGTBL_Trans_Performant)))
 		return h->access.command_completed(h);
 
 	if ((*(h->reply_pool_head) & 1) == (h->reply_pool_wraparound)) {
@@ -325,7 +364,7 @@ static inline u32 next_command(struct ctlr_info *h)
  */
 static void set_performant_mode(struct ctlr_info *h, struct CommandList *c)
 {
-	if (likely(h->transMethod == CFGTBL_Trans_Performant))
+	if (likely(h->transMethod & CFGTBL_Trans_Performant))
 		c->busaddr |= 1 | (h->blockFetchTable[c->Header.SGList] << 1);
 }
 
@@ -344,9 +383,9 @@ static void enqueue_cmd_and_start_io(struct ctlr_info *h,
 
 static inline void removeQ(struct CommandList *c)
 {
-	if (WARN_ON(hlist_unhashed(&c->list)))
+	if (WARN_ON(list_empty(&c->list)))
 		return;
-	hlist_del_init(&c->list);
+	list_del_init(&c->list);
 }
 
 static inline int is_hba_lunid(unsigned char scsi3addr[])
@@ -1130,6 +1169,10 @@ static void complete_scsi_command(struct CommandList *cp,
 		cmd->result = DID_TIME_OUT << 16;
 		dev_warn(&h->pdev->dev, "cp %p timedout\n", cp);
 		break;
+	case CMD_UNABORTABLE:
+		cmd->result = DID_ERROR << 16;
+		dev_warn(&h->pdev->dev, "Command unabortable\n");
+		break;
 	default:
 		cmd->result = DID_ERROR << 16;
 		dev_warn(&h->pdev->dev, "cp %p returned unknown status %x\n",
@@ -1160,7 +1203,7 @@ static int hpsa_scsi_detect(struct ctlr_info *h)
 	sh->sg_tablesize = h->maxsgentries;
 	h->scsi_host = sh;
 	sh->hostdata[0] = (unsigned long) h;
-	sh->irq = h->intr[PERF_MODE_INT];
+	sh->irq = h->intr[h->intr_mode];
 	sh->unique_id = sh->irq;
 	error = scsi_add_host(sh, &h->pdev->dev);
 	if (error)
@@ -1295,6 +1338,9 @@ static void hpsa_scsi_interpret_error(struct CommandList *cp)
 	case CMD_TIMEOUT:
 		dev_warn(d, "cp %p timed out\n", cp);
 		break;
+	case CMD_UNABORTABLE:
+		dev_warn(d, "Command unabortable\n");
+		break;
 	default:
 		dev_warn(d, "cp %p returned unknown status %x\n", cp,
 				ei->CommandStatus);
@@ -1595,6 +1641,8 @@ static int add_msa2xxx_enclosure_device(struct ctlr_info *h,
 	if (lun == 0) /* if lun is 0, then obviously we have a lun 0. */
 		return 0;
 
+	memset(scsi3addr, 0, 8);
+	scsi3addr[3] = target;
 	if (is_hba_lunid(scsi3addr))
 		return 0; /* Don't add the RAID controller here. */
 
@@ -1609,8 +1657,6 @@ static int add_msa2xxx_enclosure_device(struct ctlr_info *h,
 		return 0;
 	}
 
-	memset(scsi3addr, 0, 8);
-	scsi3addr[3] = target;
 	if (hpsa_update_device_info(h, scsi3addr, this_device))
 		return 0;
 	(*nmsa2xxx_enclosures)++;
@@ -2199,7 +2245,7 @@ static struct CommandList *cmd_alloc(struct ctlr_info *h)
 
 	c->cmdindex = i;
 
-	INIT_HLIST_NODE(&c->list);
+	INIT_LIST_HEAD(&c->list);
 	c->busaddr = (u32) cmd_dma_handle;
 	temp64.val = (u64) err_dma_handle;
 	c->ErrDesc.Addr.lower = temp64.val32.lower;
@@ -2237,7 +2283,7 @@ static struct CommandList *cmd_special_alloc(struct ctlr_info *h)
 	}
 	memset(c->err_info, 0, sizeof(*c->err_info));
 
-	INIT_HLIST_NODE(&c->list);
+	INIT_LIST_HEAD(&c->list);
 	c->busaddr = (u32) cmd_dma_handle;
 	temp64.val = (u64) err_dma_handle;
 	c->ErrDesc.Addr.lower = temp64.val32.lower;
@@ -2267,7 +2313,7 @@ static void cmd_special_free(struct ctlr_info *h, struct CommandList *c)
 	pci_free_consistent(h->pdev, sizeof(*c->err_info),
 			    c->err_info, (dma_addr_t) temp64.val);
 	pci_free_consistent(h->pdev, sizeof(*c),
-			    c, (dma_addr_t) c->busaddr);
+			    c, (dma_addr_t) (c->busaddr & DIRECT_LOOKUP_MASK));
 }
 
 #ifdef CONFIG_COMPAT
@@ -2281,6 +2327,7 @@ static int hpsa_ioctl32_passthru(struct scsi_device *dev, int cmd, void *arg)
 	int err;
 	u32 cp;
 
+	memset(&arg64, 0, sizeof(arg64));
 	err = 0;
 	err |= copy_from_user(&arg64.LUN_info, &arg32->LUN_info,
 			   sizeof(arg64.LUN_info));
@@ -2317,6 +2364,7 @@ static int hpsa_ioctl32_big_passthru(struct scsi_device *dev,
 	int err;
 	u32 cp;
 
+	memset(&arg64, 0, sizeof(arg64));
 	err = 0;
 	err |= copy_from_user(&arg64.LUN_info, &arg32->LUN_info,
 			   sizeof(arg64.LUN_info));
@@ -2433,15 +2481,17 @@ static int hpsa_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		buff = kmalloc(iocommand.buf_size, GFP_KERNEL);
 		if (buff == NULL)
 			return -EFAULT;
-	}
-	if (iocommand.Request.Type.Direction == XFER_WRITE) {
-		/* Copy the data into the buffer we created */
-		if (copy_from_user(buff, iocommand.buf, iocommand.buf_size)) {
-			kfree(buff);
-			return -EFAULT;
+		if (iocommand.Request.Type.Direction == XFER_WRITE) {
+			/* Copy the data into the buffer we created */
+			if (copy_from_user(buff, iocommand.buf,
+				iocommand.buf_size)) {
+				kfree(buff);
+				return -EFAULT;
+			}
+		} else {
+			memset(buff, 0, iocommand.buf_size);
 		}
-	} else
-		memset(buff, 0, iocommand.buf_size);
+	}
 	c = cmd_special_alloc(h);
 	if (c == NULL) {
 		kfree(buff);
@@ -2487,8 +2537,8 @@ static int hpsa_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		cmd_special_free(h, c);
 		return -EFAULT;
 	}
-
-	if (iocommand.Request.Type.Direction == XFER_READ) {
+	if (iocommand.Request.Type.Direction == XFER_READ &&
+		iocommand.buf_size > 0) {
 		/* Copy the data out of the buffer we created */
 		if (copy_to_user(iocommand.buf, buff, iocommand.buf_size)) {
 			kfree(buff);
@@ -2581,14 +2631,7 @@ static int hpsa_big_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 	}
 	c->cmd_type = CMD_IOCTL_PEND;
 	c->Header.ReplyQueue = 0;
-
-	if (ioc->buf_size > 0) {
-		c->Header.SGList = sg_used;
-		c->Header.SGTotal = sg_used;
-	} else {
-		c->Header.SGList = 0;
-		c->Header.SGTotal = 0;
-	}
+	c->Header.SGList = c->Header.SGTotal = sg_used;
 	memcpy(&c->Header.LUN, &ioc->LUN_info, sizeof(c->Header.LUN));
 	c->Header.Tag.lower = c->busaddr;
 	memcpy(&c->Request, &ioc->Request, sizeof(c->Request));
@@ -2605,7 +2648,8 @@ static int hpsa_big_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		}
 	}
 	hpsa_scsi_do_simple_cmd_core(h, c);
-	hpsa_pci_unmap(h->pdev, c, sg_used, PCI_DMA_BIDIRECTIONAL);
+	if (sg_used)
+		hpsa_pci_unmap(h->pdev, c, sg_used, PCI_DMA_BIDIRECTIONAL);
 	check_ioctl_unit_attention(h, c);
 	/* Copy the error information out */
 	memcpy(&ioc->error_info, c->err_info, sizeof(ioc->error_info));
@@ -2614,7 +2658,7 @@ static int hpsa_big_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		status = -EFAULT;
 		goto cleanup1;
 	}
-	if (ioc->Request.Type.Direction == XFER_READ) {
+	if (ioc->Request.Type.Direction == XFER_READ && ioc->buf_size > 0) {
 		/* Copy the data out of the buffer we created */
 		BYTE __user *ptr = ioc->buf;
 		for (i = 0; i < sg_used; i++) {
@@ -2810,8 +2854,8 @@ static void start_io(struct ctlr_info *h)
 {
 	struct CommandList *c;
 
-	while (!hlist_empty(&h->reqQ)) {
-		c = hlist_entry(h->reqQ.first, struct CommandList, list);
+	while (!list_empty(&h->reqQ)) {
+		c = list_entry(h->reqQ.next, struct CommandList, list);
 		/* can't do anything if fifo is full */
 		if ((h->access.fifo_full(h))) {
 			dev_warn(&h->pdev->dev, "fifo full\n");
@@ -2867,20 +2911,22 @@ static inline void finish_cmd(struct CommandList *c, u32 raw_tag)
 
 static inline u32 hpsa_tag_contains_index(u32 tag)
 {
-#define DIRECT_LOOKUP_BIT 0x10
 	return tag & DIRECT_LOOKUP_BIT;
 }
 
 static inline u32 hpsa_tag_to_index(u32 tag)
 {
-#define DIRECT_LOOKUP_SHIFT 5
 	return tag >> DIRECT_LOOKUP_SHIFT;
 }
 
-static inline u32 hpsa_tag_discard_error_bits(u32 tag)
+
+static inline u32 hpsa_tag_discard_error_bits(struct ctlr_info *h, u32 tag)
 {
-#define HPSA_ERROR_BITS 0x03
-	return tag & ~HPSA_ERROR_BITS;
+#define HPSA_PERF_ERROR_BITS ((1 << DIRECT_LOOKUP_SHIFT) - 1)
+#define HPSA_SIMPLE_ERROR_BITS 0x03
+	if (unlikely(!(h->transMethod & CFGTBL_Trans_Performant)))
+		return tag & ~HPSA_SIMPLE_ERROR_BITS;
+	return tag & ~HPSA_PERF_ERROR_BITS;
 }
 
 /* process completion of an indexed ("direct lookup") command */
@@ -2904,10 +2950,9 @@ static inline u32 process_nonindexed_cmd(struct ctlr_info *h,
 {
 	u32 tag;
 	struct CommandList *c = NULL;
-	struct hlist_node *tmp;
 
-	tag = hpsa_tag_discard_error_bits(raw_tag);
-	hlist_for_each_entry(c, tmp, &h->cmpQ, list) {
+	tag = hpsa_tag_discard_error_bits(h, raw_tag);
+	list_for_each_entry(c, &h->cmpQ, list) {
 		if ((c->busaddr & 0xFFFFFFE0) == (tag & 0xFFFFFFE0)) {
 			finish_cmd(c, raw_tag);
 			return next_command(h);
@@ -2957,7 +3002,10 @@ static irqreturn_t do_hpsa_intr_msi(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-/* Send a message CDB to the firmware. */
+/* Send a message CDB to the firmware. Careful, this only works
+ * in simple mode, not performant mode due to the tag lookup.
+ * We only ever use this immediately after a controller reset.
+ */
 static __devinit int hpsa_message(struct pci_dev *pdev, unsigned char opcode,
 						unsigned char type)
 {
@@ -3023,7 +3071,7 @@ static __devinit int hpsa_message(struct pci_dev *pdev, unsigned char opcode,
 
 	for (i = 0; i < HPSA_MSG_SEND_RETRY_LIMIT; i++) {
 		tag = readl(vaddr + SA5_REPLY_PORT_OFFSET);
-		if (hpsa_tag_discard_error_bits(tag) == paddr32)
+		if ((tag & ~HPSA_SIMPLE_ERROR_BITS) == paddr32)
 			break;
 		msleep(HPSA_MSG_SEND_RETRY_INTERVAL_MSECS);
 	}
@@ -3055,38 +3103,6 @@ static __devinit int hpsa_message(struct pci_dev *pdev, unsigned char opcode,
 #define hpsa_soft_reset_controller(p) hpsa_message(p, 1, 0)
 #define hpsa_noop(p) hpsa_message(p, 3, 0)
 
-static __devinit int hpsa_reset_msi(struct pci_dev *pdev)
-{
-/* the #defines are stolen from drivers/pci/msi.h. */
-#define msi_control_reg(base)		(base + PCI_MSI_FLAGS)
-#define PCI_MSIX_FLAGS_ENABLE		(1 << 15)
-
-	int pos;
-	u16 control = 0;
-
-	pos = pci_find_capability(pdev, PCI_CAP_ID_MSI);
-	if (pos) {
-		pci_read_config_word(pdev, msi_control_reg(pos), &control);
-		if (control & PCI_MSI_FLAGS_ENABLE) {
-			dev_info(&pdev->dev, "resetting MSI\n");
-			pci_write_config_word(pdev, msi_control_reg(pos),
-					control & ~PCI_MSI_FLAGS_ENABLE);
-		}
-	}
-
-	pos = pci_find_capability(pdev, PCI_CAP_ID_MSIX);
-	if (pos) {
-		pci_read_config_word(pdev, msi_control_reg(pos), &control);
-		if (control & PCI_MSIX_FLAGS_ENABLE) {
-			dev_info(&pdev->dev, "resetting MSI-X\n");
-			pci_write_config_word(pdev, msi_control_reg(pos),
-					control & ~PCI_MSIX_FLAGS_ENABLE);
-		}
-	}
-
-	return 0;
-}
-
 static int hpsa_controller_hard_reset(struct pci_dev *pdev,
 	void * __iomem vaddr, bool use_doorbell)
 {
@@ -3142,17 +3158,17 @@ static int hpsa_controller_hard_reset(struct pci_dev *pdev,
  */
 static __devinit int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 {
-	u16 saved_config_space[32];
 	u64 cfg_offset;
 	u32 cfg_base_addr;
 	u64 cfg_base_addr_index;
 	void __iomem *vaddr;
 	unsigned long paddr;
 	u32 misc_fw_support, active_transport;
-	int rc, i;
+	int rc;
 	struct CfgTable __iomem *cfgtable;
 	bool use_doorbell;
 	u32 board_id;
+	u16 command_register;
 
 	/* For controllers as old as the P600, this is very nearly
 	 * the same thing as
@@ -3162,14 +3178,6 @@ static __devinit int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 	 * pci_set_power_state(pci_dev, PCI_D0);
 	 * pci_restore_state(pci_dev);
 	 *
-	 * but we can't use these nice canned kernel routines on
-	 * kexec, because they also check the MSI/MSI-X state in PCI
-	 * configuration space and do the wrong thing when it is
-	 * set/cleared.  Also, the pci_save/restore_state functions
-	 * violate the ordering requirements for restoring the
-	 * configuration space from the CCISS document (see the
-	 * comment below).  So we roll our own ....
-	 *
 	 * For controllers newer than the P600, the pci power state
 	 * method of resetting doesn't work so we have another way
 	 * using the doorbell register.
@@ -3182,13 +3190,21 @@ static __devinit int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 	 * likely not be happy.  Just forbid resetting this conjoined mess.
 	 * The 640x isn't really supported by hpsa anyway.
 	 */
-	hpsa_lookup_board_id(pdev, &board_id);
+	rc = hpsa_lookup_board_id(pdev, &board_id);
+	if (rc < 0) {
+		dev_warn(&pdev->dev, "Not resetting device.\n");
+		return -ENODEV;
+	}
 	if (board_id == 0x409C0E11 || board_id == 0x409D0E11)
 		return -ENOTSUPP;
 
-	for (i = 0; i < 32; i++)
-		pci_read_config_word(pdev, 2*i, &saved_config_space[i]);
-
+	/* Save the PCI command register */
+	pci_read_config_word(pdev, 4, &command_register);
+	/* Turn the board off.  This is so that later pci_restore_state()
+	 * won't turn the board on before the rest of config space is ready.
+	 */
+	pci_disable_device(pdev);
+	pci_save_state(pdev);
 
 	/* find the first memory BAR, so we can find the cfg table */
 	rc = hpsa_pci_find_memory_BAR(pdev, &paddr);
@@ -3214,46 +3230,47 @@ static __devinit int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 	misc_fw_support = readl(&cfgtable->misc_fw_support);
 	use_doorbell = misc_fw_support & MISC_FW_DOORBELL_RESET;
 
-	/* The doorbell reset seems to cause lockups on some Smart
-	 * Arrays (e.g. P410, P410i, maybe others).  Until this is
-	 * fixed or at least isolated, avoid the doorbell reset.
-	 */
-	use_doorbell = 0;
-
 	rc = hpsa_controller_hard_reset(pdev, vaddr, use_doorbell);
 	if (rc)
 		goto unmap_cfgtable;
 
-	/* Restore the PCI configuration space.  The Open CISS
-	 * Specification says, "Restore the PCI Configuration
-	 * Registers, offsets 00h through 60h. It is important to
-	 * restore the command register, 16-bits at offset 04h,
-	 * last. Do not restore the configuration status register,
-	 * 16-bits at offset 06h."  Note that the offset is 2*i.
-	 */
-	for (i = 0; i < 32; i++) {
-		if (i == 2 || i == 3)
-			continue;
-		pci_write_config_word(pdev, 2*i, saved_config_space[i]);
+	pci_restore_state(pdev);
+	rc = pci_enable_device(pdev);
+	if (rc) {
+		dev_warn(&pdev->dev, "failed to enable device.\n");
+		goto unmap_cfgtable;
 	}
-	wmb();
-	pci_write_config_word(pdev, 4, saved_config_space[2]);
+	pci_write_config_word(pdev, 4, command_register);
 
 	/* Some devices (notably the HP Smart Array 5i Controller)
 	   need a little pause here */
 	msleep(HPSA_POST_RESET_PAUSE_MSECS);
 
+	/* Wait for board to become not ready, then ready. */
+	dev_info(&pdev->dev, "Waiting for board to become ready.\n");
+	rc = hpsa_wait_for_board_state(pdev, vaddr, BOARD_NOT_READY);
+	if (rc)
+		dev_warn(&pdev->dev,
+			"failed waiting for board to become not ready\n");
+	rc = hpsa_wait_for_board_state(pdev, vaddr, BOARD_READY);
+	if (rc) {
+		dev_warn(&pdev->dev,
+			"failed waiting for board to become ready\n");
+		goto unmap_cfgtable;
+	}
+	dev_info(&pdev->dev, "board ready.\n");
+
 	/* Controller should be in simple mode at this point.  If it's not,
 	 * It means we're on one of those controllers which doesn't support
 	 * the doorbell reset method and on which the PCI power management reset
 	 * method doesn't work (P800, for example.)
-	 * In those cases, pretend the reset worked and hope for the best.
+	 * In those cases, don't try to proceed, as it generally doesn't work.
 	 */
 	active_transport = readl(&cfgtable->TransportActive);
 	if (active_transport & PERFORMANT_MODE) {
 		dev_warn(&pdev->dev, "Unable to successfully reset controller,"
-			" proceeding anyway.\n");
-		rc = -ENOTSUPP;
+			" Ignoring controller.\n");
+		rc = -ENODEV;
 	}
 
 unmap_cfgtable:
@@ -3386,7 +3403,7 @@ static void __devinit hpsa_interrupt_mode(struct ctlr_info *h)
 default_int_mode:
 #endif				/* CONFIG_PCI_MSI */
 	/* if we get here we're going to use the default interrupt mode */
-	h->intr[PERF_MODE_INT] = h->pdev->irq;
+	h->intr[h->intr_mode] = h->pdev->irq;
 }
 
 static int __devinit hpsa_lookup_board_id(struct pci_dev *pdev, u32 *board_id)
@@ -3438,18 +3455,28 @@ static int __devinit hpsa_pci_find_memory_BAR(struct pci_dev *pdev,
 	return -ENODEV;
 }
 
-static int __devinit hpsa_wait_for_board_ready(struct ctlr_info *h)
+static int __devinit hpsa_wait_for_board_state(struct pci_dev *pdev,
+	void __iomem *vaddr, int wait_for_ready)
 {
-	int i;
+	int i, iterations;
 	u32 scratchpad;
+	if (wait_for_ready)
+		iterations = HPSA_BOARD_READY_ITERATIONS;
+	else
+		iterations = HPSA_BOARD_NOT_READY_ITERATIONS;
 
-	for (i = 0; i < HPSA_BOARD_READY_ITERATIONS; i++) {
-		scratchpad = readl(h->vaddr + SA5_SCRATCHPAD_OFFSET);
-		if (scratchpad == HPSA_FIRMWARE_READY)
-			return 0;
+	for (i = 0; i < iterations; i++) {
+		scratchpad = readl(vaddr + SA5_SCRATCHPAD_OFFSET);
+		if (wait_for_ready) {
+			if (scratchpad == HPSA_FIRMWARE_READY)
+				return 0;
+		} else {
+			if (scratchpad != HPSA_FIRMWARE_READY)
+				return 0;
+		}
 		msleep(HPSA_BOARD_READY_POLL_INTERVAL_MSECS);
 	}
-	dev_warn(&h->pdev->dev, "board not ready, timed out.\n");
+	dev_warn(&pdev->dev, "board not ready, timed out.\n");
 	return -ENODEV;
 }
 
@@ -3497,6 +3524,11 @@ static int __devinit hpsa_find_cfgtables(struct ctlr_info *h)
 static void __devinit hpsa_get_max_perf_mode_cmds(struct ctlr_info *h)
 {
 	h->max_commands = readl(&(h->cfgtable->MaxPerformantModeCommands));
+
+	/* Limit commands in memory limited kdump scenario. */
+	if (reset_devices && h->max_commands > 32)
+		h->max_commands = 32;
+
 	if (h->max_commands < 16) {
 		dev_warn(&h->pdev->dev, "Controller reports "
 			"max supported commands of %d, an obvious lie. "
@@ -3571,16 +3603,21 @@ static inline void hpsa_p600_dma_prefetch_quirk(struct ctlr_info *h)
 static void __devinit hpsa_wait_for_mode_change_ack(struct ctlr_info *h)
 {
 	int i;
+	u32 doorbell_value;
+	unsigned long flags;
 
 	/* under certain very rare conditions, this can take awhile.
 	 * (e.g.: hot replace a failed 144GB drive in a RAID 5 set right
 	 * as we enter this code.)
 	 */
 	for (i = 0; i < MAX_CONFIG_WAIT; i++) {
-		if (!(readl(h->vaddr + SA5_DOORBELL) & CFGTBL_ChangeReq))
+		spin_lock_irqsave(&h->lock, flags);
+		doorbell_value = readl(h->vaddr + SA5_DOORBELL);
+		spin_unlock_irqrestore(&h->lock, flags);
+		if (!(doorbell_value & CFGTBL_ChangeReq))
 			break;
 		/* delay and try again */
-		msleep(10);
+		usleep_range(10000, 20000);
 	}
 }
 
@@ -3603,6 +3640,7 @@ static int __devinit hpsa_enter_simple_mode(struct ctlr_info *h)
 			"unable to get board into simple mode\n");
 		return -ENODEV;
 	}
+	h->transMethod = CFGTBL_Trans_Simple;
 	return 0;
 }
 
@@ -3641,7 +3679,7 @@ static int __devinit hpsa_pci_init(struct ctlr_info *h)
 		err = -ENOMEM;
 		goto err_out_free_res;
 	}
-	err = hpsa_wait_for_board_ready(h);
+	err = hpsa_wait_for_board_state(h->pdev, h->vaddr, BOARD_READY);
 	if (err)
 		goto err_out_free_res;
 	err = hpsa_find_cfgtables(h);
@@ -3710,8 +3748,6 @@ static __devinit int hpsa_init_reset_devices(struct pci_dev *pdev)
 		return 0; /* just try to do the kdump anyhow. */
 	if (rc)
 		return -ENODEV;
-	if (hpsa_reset_msi(pdev))
-		return -ENODEV;
 
 	/* Now try to get the controller to respond to a no-op */
 	for (i = 0; i < HPSA_POST_RESET_NOOP_RETRIES; i++) {
@@ -3749,8 +3785,11 @@ static int __devinit hpsa_init_one(struct pci_dev *pdev,
 
 	h->pdev = pdev;
 	h->busy_initializing = 1;
-	INIT_HLIST_HEAD(&h->cmpQ);
-	INIT_HLIST_HEAD(&h->reqQ);
+	h->intr_mode = hpsa_simple_mode ? SIMPLE_MODE_INT : PERF_MODE_INT;
+	INIT_LIST_HEAD(&h->cmpQ);
+	INIT_LIST_HEAD(&h->reqQ);
+	spin_lock_init(&h->lock);
+	spin_lock_init(&h->scan_lock);
 	rc = hpsa_pci_init(h);
 	if (rc != 0)
 		goto clean1;
@@ -3777,20 +3816,20 @@ static int __devinit hpsa_init_one(struct pci_dev *pdev,
 	h->access.set_intr_mask(h, HPSA_INTR_OFF);
 
 	if (h->msix_vector || h->msi_vector)
-		rc = request_irq(h->intr[PERF_MODE_INT], do_hpsa_intr_msi,
+		rc = request_irq(h->intr[h->intr_mode], do_hpsa_intr_msi,
 				IRQF_DISABLED, h->devname, h);
 	else
-		rc = request_irq(h->intr[PERF_MODE_INT], do_hpsa_intr_intx,
+		rc = request_irq(h->intr[h->intr_mode], do_hpsa_intr_intx,
 				IRQF_DISABLED, h->devname, h);
 	if (rc) {
 		dev_err(&pdev->dev, "unable to get irq %d for %s\n",
-		       h->intr[PERF_MODE_INT], h->devname);
+		       h->intr[h->intr_mode], h->devname);
 		goto clean2;
 	}
 
 	dev_info(&pdev->dev, "%s: <0x%x> at IRQ %d%s using DAC\n",
 	       h->devname, pdev->device,
-	       h->intr[PERF_MODE_INT], dac ? "" : " not");
+	       h->intr[h->intr_mode], dac ? "" : " not");
 
 	h->cmd_pool_bits =
 	    kmalloc(((h->nr_cmds + BITS_PER_LONG -
@@ -3810,8 +3849,6 @@ static int __devinit hpsa_init_one(struct pci_dev *pdev,
 	}
 	if (hpsa_allocate_sg_chain_blocks(h))
 		goto clean4;
-	spin_lock_init(&h->lock);
-	spin_lock_init(&h->scan_lock);
 	init_waitqueue_head(&h->scan_wait_queue);
 	h->scan_finished = 1; /* no scan currently in progress */
 
@@ -3843,7 +3880,7 @@ clean4:
 			    h->nr_cmds * sizeof(struct ErrorInfo),
 			    h->errinfo_pool,
 			    h->errinfo_pool_dhandle);
-	free_irq(h->intr[PERF_MODE_INT], h);
+	free_irq(h->intr[h->intr_mode], h);
 clean2:
 clean1:
 	h->busy_initializing = 0;
@@ -3887,7 +3924,7 @@ static void hpsa_shutdown(struct pci_dev *pdev)
 	 */
 	hpsa_flush_cache(h);
 	h->access.set_intr_mask(h, HPSA_INTR_OFF);
-	free_irq(h->intr[PERF_MODE_INT], h);
+	free_irq(h->intr[h->intr_mode], h);
 #ifdef CONFIG_PCI_MSI
 	if (h->msix_vector)
 		pci_disable_msix(h->pdev);
@@ -3989,7 +4026,8 @@ static void  calc_bucket_map(int bucket[], int num_buckets,
 	}
 }
 
-static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h)
+static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h,
+	u32 use_short_tags)
 {
 	int i;
 	unsigned long register_value;
@@ -4037,7 +4075,7 @@ static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h)
 	writel(0, &h->transtable->RepQCtrAddrHigh32);
 	writel(h->reply_pool_dhandle, &h->transtable->RepQAddr0Low32);
 	writel(0, &h->transtable->RepQAddr0High32);
-	writel(CFGTBL_Trans_Performant,
+	writel(CFGTBL_Trans_Performant | use_short_tags,
 		&(h->cfgtable->HostWrite.TransportRequest));
 	writel(CFGTBL_ChangeReq, h->vaddr + SA5_DOORBELL);
 	hpsa_wait_for_mode_change_ack(h);
@@ -4047,12 +4085,18 @@ static __devinit void hpsa_enter_performant_mode(struct ctlr_info *h)
 					" performant mode\n");
 		return;
 	}
+	/* Change the access methods to the performant access methods */
+	h->access = SA5_performant_access;
+	h->transMethod = CFGTBL_Trans_Performant;
 }
 
 static __devinit void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 {
 	u32 trans_support;
 
+	if (hpsa_simple_mode)
+		return;
+
 	trans_support = readl(&(h->cfgtable->TransportSupport));
 	if (!(trans_support & PERFORMANT_MODE))
 		return;
@@ -4072,11 +4116,8 @@ static __devinit void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 		|| (h->blockFetchTable == NULL))
 		goto clean_up;
 
-	hpsa_enter_performant_mode(h);
-
-	/* Change the access methods to the performant access methods */
-	h->access = SA5_performant_access;
-	h->transMethod = CFGTBL_Trans_Performant;
+	hpsa_enter_performant_mode(h,
+		trans_support & CFGTBL_Trans_use_short_tags);
 
 	return;
 
diff --git a/drivers/scsi/hpsa.h b/drivers/scsi/hpsa.h
index 19586e189f0f..621a1530054a 100644
--- a/drivers/scsi/hpsa.h
+++ b/drivers/scsi/hpsa.h
@@ -72,11 +72,12 @@ struct ctlr_info {
 	unsigned int intr[4];
 	unsigned int msix_vector;
 	unsigned int msi_vector;
+	int intr_mode; /* either PERF_MODE_INT or SIMPLE_MODE_INT */
 	struct access_method access;
 
 	/* queue and queue Info */
-	struct hlist_head reqQ;
-	struct hlist_head cmpQ;
+	struct list_head reqQ;
+	struct list_head cmpQ;
 	unsigned int Qdepth;
 	unsigned int maxQsinceinit;
 	unsigned int maxSG;
@@ -154,12 +155,16 @@ struct ctlr_info {
  * HPSA_BOARD_READY_ITERATIONS are derived from those.
  */
 #define HPSA_BOARD_READY_WAIT_SECS (120)
+#define HPSA_BOARD_NOT_READY_WAIT_SECS (10)
 #define HPSA_BOARD_READY_POLL_INTERVAL_MSECS (100)
 #define HPSA_BOARD_READY_POLL_INTERVAL \
 	((HPSA_BOARD_READY_POLL_INTERVAL_MSECS * HZ) / 1000)
 #define HPSA_BOARD_READY_ITERATIONS \
 	((HPSA_BOARD_READY_WAIT_SECS * 1000) / \
 		HPSA_BOARD_READY_POLL_INTERVAL_MSECS)
+#define HPSA_BOARD_NOT_READY_ITERATIONS \
+	((HPSA_BOARD_NOT_READY_WAIT_SECS * 1000) / \
+		HPSA_BOARD_READY_POLL_INTERVAL_MSECS)
 #define HPSA_POST_RESET_PAUSE_MSECS (3000)
 #define HPSA_POST_RESET_NOOP_RETRIES (12)
 
diff --git a/drivers/scsi/hpsa_cmd.h b/drivers/scsi/hpsa_cmd.h
index f5c4c3cc0530..18464900e761 100644
--- a/drivers/scsi/hpsa_cmd.h
+++ b/drivers/scsi/hpsa_cmd.h
@@ -104,6 +104,7 @@
 
 #define CFGTBL_Trans_Simple     0x00000002l
 #define CFGTBL_Trans_Performant 0x00000004l
+#define CFGTBL_Trans_use_short_tags 0x20000000l
 
 #define CFGTBL_BusType_Ultra2   0x00000001l
 #define CFGTBL_BusType_Ultra3   0x00000002l
@@ -265,6 +266,7 @@ struct ErrorInfo {
 
 #define DIRECT_LOOKUP_SHIFT 5
 #define DIRECT_LOOKUP_BIT 0x10
+#define DIRECT_LOOKUP_MASK (~((1 << DIRECT_LOOKUP_SHIFT) - 1))
 
 #define HPSA_ERROR_BIT          0x02
 struct ctlr_info; /* defined in hpsa.h */
@@ -291,7 +293,7 @@ struct CommandList {
 	struct ctlr_info	   *h;
 	int			   cmd_type;
 	long			   cmdindex;
-	struct hlist_node list;
+	struct list_head list;
 	struct request *rq;
 	struct completion *waiting;
 	void   *scsi_cmd;
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 9c5c8be72231..0621238fac4a 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -1301,7 +1301,7 @@ static void ipr_handle_config_change(struct ipr_ioa_cfg *ioa_cfg,
 			ipr_clear_res_target(res);
 			list_move_tail(&res->queue, &ioa_cfg->free_res_q);
 		}
-	} else if (!res->sdev) {
+	} else if (!res->sdev || res->del_from_ml) {
 		res->add_to_ml = 1;
 		if (ioa_cfg->allow_ml_add_del)
 			schedule_work(&ioa_cfg->work_q);
@@ -3104,7 +3104,10 @@ restart:
 				did_work = 1;
 				sdev = res->sdev;
 				if (!scsi_device_get(sdev)) {
-					list_move_tail(&res->queue, &ioa_cfg->free_res_q);
+					if (!res->add_to_ml)
+						list_move_tail(&res->queue, &ioa_cfg->free_res_q);
+					else
+						res->del_from_ml = 0;
 					spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
 					scsi_remove_device(sdev);
 					scsi_device_put(sdev);
@@ -6219,11 +6222,10 @@ static struct ata_port_operations ipr_sata_ops = {
 };
 
 static struct ata_port_info sata_port_info = {
-	.flags	= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY | ATA_FLAG_SATA_RESET |
-	ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA,
-	.pio_mask	= 0x10, /* pio4 */
-	.mwdma_mask = 0x07,
-	.udma_mask	= 0x7f, /* udma0-6 */
+	.flags		= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA,
+	.pio_mask	= ATA_PIO4_ONLY,
+	.mwdma_mask	= ATA_MWDMA2,
+	.udma_mask	= ATA_UDMA6,
 	.port_ops	= &ipr_sata_ops
 };
 
@@ -8865,7 +8867,7 @@ static void __ipr_remove(struct pci_dev *pdev)
 
 	spin_unlock_irqrestore(ioa_cfg->host->host_lock, host_lock_flags);
 	wait_event(ioa_cfg->reset_wait_q, !ioa_cfg->in_reset_reload);
-	flush_scheduled_work();
+	flush_work_sync(&ioa_cfg->work_q);
 	spin_lock_irqsave(ioa_cfg->host->host_lock, host_lock_flags);
 
 	spin_lock(&ipr_driver_lock);
diff --git a/drivers/scsi/iscsi_tcp.c b/drivers/scsi/iscsi_tcp.c
index fec47de72535..a860452a8f71 100644
--- a/drivers/scsi/iscsi_tcp.c
+++ b/drivers/scsi/iscsi_tcp.c
@@ -608,54 +608,12 @@ static void iscsi_sw_tcp_conn_stop(struct iscsi_cls_conn *cls_conn, int flag)
 	iscsi_sw_tcp_release_conn(conn);
 }
 
-static int iscsi_sw_tcp_get_addr(struct iscsi_conn *conn, struct socket *sock,
-				 char *buf, int *port,
-				 int (*getname)(struct socket *,
-						struct sockaddr *,
-						int *addrlen))
-{
-	struct sockaddr_storage *addr;
-	struct sockaddr_in6 *sin6;
-	struct sockaddr_in *sin;
-	int rc = 0, len;
-
-	addr = kmalloc(sizeof(*addr), GFP_KERNEL);
-	if (!addr)
-		return -ENOMEM;
-
-	if (getname(sock, (struct sockaddr *) addr, &len)) {
-		rc = -ENODEV;
-		goto free_addr;
-	}
-
-	switch (addr->ss_family) {
-	case AF_INET:
-		sin = (struct sockaddr_in *)addr;
-		spin_lock_bh(&conn->session->lock);
-		sprintf(buf, "%pI4", &sin->sin_addr.s_addr);
-		*port = be16_to_cpu(sin->sin_port);
-		spin_unlock_bh(&conn->session->lock);
-		break;
-	case AF_INET6:
-		sin6 = (struct sockaddr_in6 *)addr;
-		spin_lock_bh(&conn->session->lock);
-		sprintf(buf, "%pI6", &sin6->sin6_addr);
-		*port = be16_to_cpu(sin6->sin6_port);
-		spin_unlock_bh(&conn->session->lock);
-		break;
-	}
-free_addr:
-	kfree(addr);
-	return rc;
-}
-
 static int
 iscsi_sw_tcp_conn_bind(struct iscsi_cls_session *cls_session,
 		       struct iscsi_cls_conn *cls_conn, uint64_t transport_eph,
 		       int is_leading)
 {
-	struct Scsi_Host *shost = iscsi_session_to_shost(cls_session);
-	struct iscsi_host *ihost = shost_priv(shost);
+	struct iscsi_session *session = cls_session->dd_data;
 	struct iscsi_conn *conn = cls_conn->dd_data;
 	struct iscsi_tcp_conn *tcp_conn = conn->dd_data;
 	struct iscsi_sw_tcp_conn *tcp_sw_conn = tcp_conn->dd_data;
@@ -670,27 +628,15 @@ iscsi_sw_tcp_conn_bind(struct iscsi_cls_session *cls_session,
 				  "sockfd_lookup failed %d\n", err);
 		return -EEXIST;
 	}
-	/*
-	 * copy these values now because if we drop the session
-	 * userspace may still want to query the values since we will
-	 * be using them for the reconnect
-	 */
-	err = iscsi_sw_tcp_get_addr(conn, sock, conn->portal_address,
-				    &conn->portal_port, kernel_getpeername);
-	if (err)
-		goto free_socket;
-
-	err = iscsi_sw_tcp_get_addr(conn, sock, ihost->local_address,
-				    &ihost->local_port, kernel_getsockname);
-	if (err)
-		goto free_socket;
 
 	err = iscsi_conn_bind(cls_session, cls_conn, is_leading);
 	if (err)
 		goto free_socket;
 
+	spin_lock_bh(&session->lock);
 	/* bind iSCSI connection and socket */
 	tcp_sw_conn->sock = sock;
+	spin_unlock_bh(&session->lock);
 
 	/* setup Socket parameters */
 	sk = sock->sk;
@@ -752,24 +698,74 @@ static int iscsi_sw_tcp_conn_get_param(struct iscsi_cls_conn *cls_conn,
 				       enum iscsi_param param, char *buf)
 {
 	struct iscsi_conn *conn = cls_conn->dd_data;
-	int len;
+	struct iscsi_tcp_conn *tcp_conn = conn->dd_data;
+	struct iscsi_sw_tcp_conn *tcp_sw_conn = tcp_conn->dd_data;
+	struct sockaddr_in6 addr;
+	int rc, len;
 
 	switch(param) {
 	case ISCSI_PARAM_CONN_PORT:
-		spin_lock_bh(&conn->session->lock);
-		len = sprintf(buf, "%hu\n", conn->portal_port);
-		spin_unlock_bh(&conn->session->lock);
-		break;
 	case ISCSI_PARAM_CONN_ADDRESS:
 		spin_lock_bh(&conn->session->lock);
-		len = sprintf(buf, "%s\n", conn->portal_address);
+		if (!tcp_sw_conn || !tcp_sw_conn->sock) {
+			spin_unlock_bh(&conn->session->lock);
+			return -ENOTCONN;
+		}
+		rc = kernel_getpeername(tcp_sw_conn->sock,
+					(struct sockaddr *)&addr, &len);
 		spin_unlock_bh(&conn->session->lock);
-		break;
+		if (rc)
+			return rc;
+
+		return iscsi_conn_get_addr_param((struct sockaddr_storage *)
+						 &addr, param, buf);
 	default:
 		return iscsi_conn_get_param(cls_conn, param, buf);
 	}
 
-	return len;
+	return 0;
+}
+
+static int iscsi_sw_tcp_host_get_param(struct Scsi_Host *shost,
+				       enum iscsi_host_param param, char *buf)
+{
+	struct iscsi_sw_tcp_host *tcp_sw_host = iscsi_host_priv(shost);
+	struct iscsi_session *session = tcp_sw_host->session;
+	struct iscsi_conn *conn;
+	struct iscsi_tcp_conn *tcp_conn;
+	struct iscsi_sw_tcp_conn *tcp_sw_conn;
+	struct sockaddr_in6 addr;
+	int rc, len;
+
+	switch (param) {
+	case ISCSI_HOST_PARAM_IPADDRESS:
+		spin_lock_bh(&session->lock);
+		conn = session->leadconn;
+		if (!conn) {
+			spin_unlock_bh(&session->lock);
+			return -ENOTCONN;
+		}
+		tcp_conn = conn->dd_data;
+
+		tcp_sw_conn = tcp_conn->dd_data;
+		if (!tcp_sw_conn->sock) {
+			spin_unlock_bh(&session->lock);
+			return -ENOTCONN;
+		}
+
+		rc = kernel_getsockname(tcp_sw_conn->sock,
+					(struct sockaddr *)&addr, &len);
+		spin_unlock_bh(&session->lock);
+		if (rc)
+			return rc;
+
+		return iscsi_conn_get_addr_param((struct sockaddr_storage *)
+						 &addr, param, buf);
+	default:
+		return iscsi_host_get_param(shost, param, buf);
+	}
+
+	return 0;
 }
 
 static void
@@ -797,6 +793,7 @@ iscsi_sw_tcp_session_create(struct iscsi_endpoint *ep, uint16_t cmds_max,
 {
 	struct iscsi_cls_session *cls_session;
 	struct iscsi_session *session;
+	struct iscsi_sw_tcp_host *tcp_sw_host;
 	struct Scsi_Host *shost;
 
 	if (ep) {
@@ -804,7 +801,8 @@ iscsi_sw_tcp_session_create(struct iscsi_endpoint *ep, uint16_t cmds_max,
 		return NULL;
 	}
 
-	shost = iscsi_host_alloc(&iscsi_sw_tcp_sht, 0, 1);
+	shost = iscsi_host_alloc(&iscsi_sw_tcp_sht,
+				 sizeof(struct iscsi_sw_tcp_host), 1);
 	if (!shost)
 		return NULL;
 	shost->transportt = iscsi_sw_tcp_scsi_transport;
@@ -825,6 +823,8 @@ iscsi_sw_tcp_session_create(struct iscsi_endpoint *ep, uint16_t cmds_max,
 	if (!cls_session)
 		goto remove_host;
 	session = cls_session->dd_data;
+	tcp_sw_host = iscsi_host_priv(shost);
+	tcp_sw_host->session = session;
 
 	shost->can_queue = session->scsi_cmds_max;
 	if (iscsi_tcp_r2tpool_alloc(session))
@@ -929,7 +929,7 @@ static struct iscsi_transport iscsi_sw_tcp_transport = {
 	.start_conn		= iscsi_conn_start,
 	.stop_conn		= iscsi_sw_tcp_conn_stop,
 	/* iscsi host params */
-	.get_host_param		= iscsi_host_get_param,
+	.get_host_param		= iscsi_sw_tcp_host_get_param,
 	.set_host_param		= iscsi_host_set_param,
 	/* IO */
 	.send_pdu		= iscsi_conn_send_pdu,
diff --git a/drivers/scsi/iscsi_tcp.h b/drivers/scsi/iscsi_tcp.h
index 94644bad0ed7..666fe09378fa 100644
--- a/drivers/scsi/iscsi_tcp.h
+++ b/drivers/scsi/iscsi_tcp.h
@@ -55,6 +55,10 @@ struct iscsi_sw_tcp_conn {
 	ssize_t (*sendpage)(struct socket *, struct page *, int, size_t, int);
 };
 
+struct iscsi_sw_tcp_host {
+	struct iscsi_session	*session;
+};
+
 struct iscsi_sw_tcp_hdrbuf {
 	struct iscsi_hdr	hdrbuf;
 	char			hdrextbuf[ISCSI_MAX_AHS_SIZE +
diff --git a/drivers/scsi/libfc/fc_exch.c b/drivers/scsi/libfc/fc_exch.c
index d21367d3305f..28231badd9e6 100644
--- a/drivers/scsi/libfc/fc_exch.c
+++ b/drivers/scsi/libfc/fc_exch.c
@@ -38,7 +38,7 @@ u16	fc_cpu_mask;		/* cpu mask for possible cpus */
 EXPORT_SYMBOL(fc_cpu_mask);
 static u16	fc_cpu_order;	/* 2's power to represent total possible cpus */
 static struct kmem_cache *fc_em_cachep;	       /* cache for exchanges */
-struct workqueue_struct *fc_exch_workqueue;
+static struct workqueue_struct *fc_exch_workqueue;
 
 /*
  * Structure and function definitions for managing Fibre Channel Exchanges
@@ -558,6 +558,22 @@ static struct fc_seq *fc_seq_start_next(struct fc_seq *sp)
 	return sp;
 }
 
+/*
+ * Set the response handler for the exchange associated with a sequence.
+ */
+static void fc_seq_set_resp(struct fc_seq *sp,
+			    void (*resp)(struct fc_seq *, struct fc_frame *,
+					 void *),
+			    void *arg)
+{
+	struct fc_exch *ep = fc_seq_exch(sp);
+
+	spin_lock_bh(&ep->ex_lock);
+	ep->resp = resp;
+	ep->arg = arg;
+	spin_unlock_bh(&ep->ex_lock);
+}
+
 /**
  * fc_seq_exch_abort() - Abort an exchange and sequence
  * @req_sp:	The sequence to be aborted
@@ -650,13 +666,10 @@ static void fc_exch_timeout(struct work_struct *work)
 		if (e_stat & ESB_ST_ABNORMAL)
 			rc = fc_exch_done_locked(ep);
 		spin_unlock_bh(&ep->ex_lock);
+		if (!rc)
+			fc_exch_delete(ep);
 		if (resp)
 			resp(sp, ERR_PTR(-FC_EX_TIMEOUT), arg);
-		if (!rc) {
-			/* delete the exchange if it's already being aborted */
-			fc_exch_delete(ep);
-			return;
-		}
 		fc_seq_exch_abort(sp, 2 * ep->r_a_tov);
 		goto done;
 	}
@@ -1266,6 +1279,8 @@ free:
  * @fp:    The request frame
  *
  * On success, the sequence pointer will be returned and also in fr_seq(@fp).
+ * A reference will be held on the exchange/sequence for the caller, which
+ * must call fc_seq_release().
  */
 static struct fc_seq *fc_seq_assign(struct fc_lport *lport, struct fc_frame *fp)
 {
@@ -1283,6 +1298,15 @@ static struct fc_seq *fc_seq_assign(struct fc_lport *lport, struct fc_frame *fp)
 }
 
 /**
+ * fc_seq_release() - Release the hold
+ * @sp:    The sequence.
+ */
+static void fc_seq_release(struct fc_seq *sp)
+{
+	fc_exch_release(fc_seq_exch(sp));
+}
+
+/**
  * fc_exch_recv_req() - Handler for an incoming request
  * @lport: The local port that received the request
  * @mp:	   The EM that the exchange is on
@@ -2151,6 +2175,7 @@ err:
 		fc_exch_mgr_del(ema);
 	return -ENOMEM;
 }
+EXPORT_SYMBOL(fc_exch_mgr_list_clone);
 
 /**
  * fc_exch_mgr_alloc() - Allocate an exchange manager
@@ -2254,16 +2279,45 @@ void fc_exch_mgr_free(struct fc_lport *lport)
 EXPORT_SYMBOL(fc_exch_mgr_free);
 
 /**
+ * fc_find_ema() - Lookup and return appropriate Exchange Manager Anchor depending
+ * upon 'xid'.
+ * @f_ctl: f_ctl
+ * @lport: The local port the frame was received on
+ * @fh: The received frame header
+ */
+static struct fc_exch_mgr_anchor *fc_find_ema(u32 f_ctl,
+					      struct fc_lport *lport,
+					      struct fc_frame_header *fh)
+{
+	struct fc_exch_mgr_anchor *ema;
+	u16 xid;
+
+	if (f_ctl & FC_FC_EX_CTX)
+		xid = ntohs(fh->fh_ox_id);
+	else {
+		xid = ntohs(fh->fh_rx_id);
+		if (xid == FC_XID_UNKNOWN)
+			return list_entry(lport->ema_list.prev,
+					  typeof(*ema), ema_list);
+	}
+
+	list_for_each_entry(ema, &lport->ema_list, ema_list) {
+		if ((xid >= ema->mp->min_xid) &&
+		    (xid <= ema->mp->max_xid))
+			return ema;
+	}
+	return NULL;
+}
+/**
  * fc_exch_recv() - Handler for received frames
  * @lport: The local port the frame was received on
- * @fp:	   The received frame
+ * @fp:	The received frame
  */
 void fc_exch_recv(struct fc_lport *lport, struct fc_frame *fp)
 {
 	struct fc_frame_header *fh = fc_frame_header_get(fp);
 	struct fc_exch_mgr_anchor *ema;
-	u32 f_ctl, found = 0;
-	u16 oxid;
+	u32 f_ctl;
 
 	/* lport lock ? */
 	if (!lport || lport->state == LPORT_ST_DISABLED) {
@@ -2274,24 +2328,17 @@ void fc_exch_recv(struct fc_lport *lport, struct fc_frame *fp)
 	}
 
 	f_ctl = ntoh24(fh->fh_f_ctl);
-	oxid = ntohs(fh->fh_ox_id);
-	if (f_ctl & FC_FC_EX_CTX) {
-		list_for_each_entry(ema, &lport->ema_list, ema_list) {
-			if ((oxid >= ema->mp->min_xid) &&
-			    (oxid <= ema->mp->max_xid)) {
-				found = 1;
-				break;
-			}
-		}
-
-		if (!found) {
-			FC_LPORT_DBG(lport, "Received response for out "
-				     "of range oxid:%hx\n", oxid);
-			fc_frame_free(fp);
-			return;
-		}
-	} else
-		ema = list_entry(lport->ema_list.prev, typeof(*ema), ema_list);
+	ema = fc_find_ema(f_ctl, lport, fh);
+	if (!ema) {
+		FC_LPORT_DBG(lport, "Unable to find Exchange Manager Anchor,"
+				    "fc_ctl <0x%x>, xid <0x%x>\n",
+				     f_ctl,
+				     (f_ctl & FC_FC_EX_CTX) ?
+				     ntohs(fh->fh_ox_id) :
+				     ntohs(fh->fh_rx_id));
+		fc_frame_free(fp);
+		return;
+	}
 
 	/*
 	 * If frame is marked invalid, just drop it.
@@ -2329,6 +2376,9 @@ int fc_exch_init(struct fc_lport *lport)
 	if (!lport->tt.seq_start_next)
 		lport->tt.seq_start_next = fc_seq_start_next;
 
+	if (!lport->tt.seq_set_resp)
+		lport->tt.seq_set_resp = fc_seq_set_resp;
+
 	if (!lport->tt.exch_seq_send)
 		lport->tt.exch_seq_send = fc_exch_seq_send;
 
@@ -2350,6 +2400,9 @@ int fc_exch_init(struct fc_lport *lport)
 	if (!lport->tt.seq_assign)
 		lport->tt.seq_assign = fc_seq_assign;
 
+	if (!lport->tt.seq_release)
+		lport->tt.seq_release = fc_seq_release;
+
 	return 0;
 }
 EXPORT_SYMBOL(fc_exch_init);
@@ -2357,7 +2410,7 @@ EXPORT_SYMBOL(fc_exch_init);
 /**
  * fc_setup_exch_mgr() - Setup an exchange manager
  */
-int fc_setup_exch_mgr()
+int fc_setup_exch_mgr(void)
 {
 	fc_em_cachep = kmem_cache_create("libfc_em", sizeof(struct fc_exch),
 					 0, SLAB_HWCACHE_ALIGN, NULL);
@@ -2395,7 +2448,7 @@ int fc_setup_exch_mgr()
 /**
  * fc_destroy_exch_mgr() - Destroy an exchange manager
  */
-void fc_destroy_exch_mgr()
+void fc_destroy_exch_mgr(void)
 {
 	destroy_workqueue(fc_exch_workqueue);
 	kmem_cache_destroy(fc_em_cachep);
diff --git a/drivers/scsi/libfc/fc_fcp.c b/drivers/scsi/libfc/fc_fcp.c
index 5962d1a5a674..b1b03af158bf 100644
--- a/drivers/scsi/libfc/fc_fcp.c
+++ b/drivers/scsi/libfc/fc_fcp.c
@@ -42,7 +42,7 @@
 
 #include "fc_libfc.h"
 
-struct kmem_cache *scsi_pkt_cachep;
+static struct kmem_cache *scsi_pkt_cachep;
 
 /* SRB state definitions */
 #define FC_SRB_FREE		0		/* cmd is free */
@@ -155,6 +155,7 @@ static struct fc_fcp_pkt *fc_fcp_pkt_alloc(struct fc_lport *lport, gfp_t gfp)
 	if (fsp) {
 		memset(fsp, 0, sizeof(*fsp));
 		fsp->lp = lport;
+		fsp->xfer_ddp = FC_XID_UNKNOWN;
 		atomic_set(&fsp->ref_cnt, 1);
 		init_timer(&fsp->timer);
 		INIT_LIST_HEAD(&fsp->list);
@@ -1201,6 +1202,7 @@ unlock:
 static int fc_fcp_pkt_abort(struct fc_fcp_pkt *fsp)
 {
 	int rc = FAILED;
+	unsigned long ticks_left;
 
 	if (fc_fcp_send_abort(fsp))
 		return FAILED;
@@ -1209,13 +1211,13 @@ static int fc_fcp_pkt_abort(struct fc_fcp_pkt *fsp)
 	fsp->wait_for_comp = 1;
 
 	spin_unlock_bh(&fsp->scsi_pkt_lock);
-	rc = wait_for_completion_timeout(&fsp->tm_done, FC_SCSI_TM_TOV);
+	ticks_left = wait_for_completion_timeout(&fsp->tm_done,
+							FC_SCSI_TM_TOV);
 	spin_lock_bh(&fsp->scsi_pkt_lock);
 	fsp->wait_for_comp = 0;
 
-	if (!rc) {
+	if (!ticks_left) {
 		FC_FCP_DBG(fsp, "target abort cmd  failed\n");
-		rc = FAILED;
 	} else if (fsp->state & FC_SRB_ABORTED) {
 		FC_FCP_DBG(fsp, "target abort cmd  passed\n");
 		rc = SUCCESS;
@@ -1321,7 +1323,7 @@ static void fc_tm_done(struct fc_seq *seq, struct fc_frame *fp, void *arg)
 		 *
 		 * scsi-eh will escalate for when either happens.
 		 */
-		goto out;
+		return;
 	}
 
 	if (fc_fcp_lock_pkt(fsp))
@@ -1787,15 +1789,14 @@ static inline int fc_fcp_lport_queue_ready(struct fc_lport *lport)
 
 /**
  * fc_queuecommand() - The queuecommand function of the SCSI template
+ * @shost: The Scsi_Host that the command was issued to
  * @cmd:   The scsi_cmnd to be executed
- * @done:  The callback function to be called when the scsi_cmnd is complete
  *
- * This is the i/o strategy routine, called by the SCSI layer. This routine
- * is called with the host_lock held.
+ * This is the i/o strategy routine, called by the SCSI layer.
  */
-static int fc_queuecommand_lck(struct scsi_cmnd *sc_cmd, void (*done)(struct scsi_cmnd *))
+int fc_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *sc_cmd)
 {
-	struct fc_lport *lport;
+	struct fc_lport *lport = shost_priv(shost);
 	struct fc_rport *rport = starget_to_rport(scsi_target(sc_cmd->device));
 	struct fc_fcp_pkt *fsp;
 	struct fc_rport_libfc_priv *rpriv;
@@ -1803,15 +1804,12 @@ static int fc_queuecommand_lck(struct scsi_cmnd *sc_cmd, void (*done)(struct scs
 	int rc = 0;
 	struct fcoe_dev_stats *stats;
 
-	lport = shost_priv(sc_cmd->device->host);
-
 	rval = fc_remote_port_chkready(rport);
 	if (rval) {
 		sc_cmd->result = rval;
-		done(sc_cmd);
+		sc_cmd->scsi_done(sc_cmd);
 		return 0;
 	}
-	spin_unlock_irq(lport->host->host_lock);
 
 	if (!*(struct fc_remote_port **)rport->dd_data) {
 		/*
@@ -1819,7 +1817,7 @@ static int fc_queuecommand_lck(struct scsi_cmnd *sc_cmd, void (*done)(struct scs
 		 * online
 		 */
 		sc_cmd->result = DID_IMM_RETRY << 16;
-		done(sc_cmd);
+		sc_cmd->scsi_done(sc_cmd);
 		goto out;
 	}
 
@@ -1842,10 +1840,7 @@ static int fc_queuecommand_lck(struct scsi_cmnd *sc_cmd, void (*done)(struct scs
 	 * build the libfc request pkt
 	 */
 	fsp->cmd = sc_cmd;	/* save the cmd */
-	fsp->lp = lport;	/* save the softc ptr */
 	fsp->rport = rport;	/* set the remote port ptr */
-	fsp->xfer_ddp = FC_XID_UNKNOWN;
-	sc_cmd->scsi_done = done;
 
 	/*
 	 * set up the transfer length
@@ -1886,11 +1881,8 @@ static int fc_queuecommand_lck(struct scsi_cmnd *sc_cmd, void (*done)(struct scs
 		rc = SCSI_MLQUEUE_HOST_BUSY;
 	}
 out:
-	spin_lock_irq(lport->host->host_lock);
 	return rc;
 }
-
-DEF_SCSI_QCMD(fc_queuecommand)
 EXPORT_SYMBOL(fc_queuecommand);
 
 /**
@@ -2112,7 +2104,6 @@ int fc_eh_device_reset(struct scsi_cmnd *sc_cmd)
 	 * the sc passed in is not setup for execution like when sent
 	 * through the queuecommand callout.
 	 */
-	fsp->lp = lport;	/* save the softc ptr */
 	fsp->rport = rport;	/* set the remote port ptr */
 
 	/*
@@ -2245,7 +2236,7 @@ void fc_fcp_destroy(struct fc_lport *lport)
 }
 EXPORT_SYMBOL(fc_fcp_destroy);
 
-int fc_setup_fcp()
+int fc_setup_fcp(void)
 {
 	int rc = 0;
 
@@ -2261,7 +2252,7 @@ int fc_setup_fcp()
 	return rc;
 }
 
-void fc_destroy_fcp()
+void fc_destroy_fcp(void)
 {
 	if (scsi_pkt_cachep)
 		kmem_cache_destroy(scsi_pkt_cachep);
diff --git a/drivers/scsi/libfc/fc_libfc.c b/drivers/scsi/libfc/fc_libfc.c
index 6a48c28e4420..b7735129f1f3 100644
--- a/drivers/scsi/libfc/fc_libfc.c
+++ b/drivers/scsi/libfc/fc_libfc.c
@@ -35,6 +35,27 @@ unsigned int fc_debug_logging;
 module_param_named(debug_logging, fc_debug_logging, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(debug_logging, "a bit mask of logging levels");
 
+DEFINE_MUTEX(fc_prov_mutex);
+static LIST_HEAD(fc_local_ports);
+struct blocking_notifier_head fc_lport_notifier_head =
+		BLOCKING_NOTIFIER_INIT(fc_lport_notifier_head);
+EXPORT_SYMBOL(fc_lport_notifier_head);
+
+/*
+ * Providers which primarily send requests and PRLIs.
+ */
+struct fc4_prov *fc_active_prov[FC_FC4_PROV_SIZE] = {
+	[0] = &fc_rport_t0_prov,
+	[FC_TYPE_FCP] = &fc_rport_fcp_init,
+};
+
+/*
+ * Providers which receive requests.
+ */
+struct fc4_prov *fc_passive_prov[FC_FC4_PROV_SIZE] = {
+	[FC_TYPE_ELS] = &fc_lport_els_prov,
+};
+
 /**
  * libfc_init() - Initialize libfc.ko
  */
@@ -210,3 +231,102 @@ void fc_fill_reply_hdr(struct fc_frame *fp, const struct fc_frame *in_fp,
 	fc_fill_hdr(fp, in_fp, r_ctl, FC_FCTL_RESP, 0, parm_offset);
 }
 EXPORT_SYMBOL(fc_fill_reply_hdr);
+
+/**
+ * fc_fc4_conf_lport_params() - Modify "service_params" of specified lport
+ * if there is service provider (target provider) registered with libfc
+ * for specified "fc_ft_type"
+ * @lport: Local port which service_params needs to be modified
+ * @type: FC-4 type, such as FC_TYPE_FCP
+ */
+void fc_fc4_conf_lport_params(struct fc_lport *lport, enum fc_fh_type type)
+{
+	struct fc4_prov *prov_entry;
+	BUG_ON(type >= FC_FC4_PROV_SIZE);
+	BUG_ON(!lport);
+	prov_entry = fc_passive_prov[type];
+	if (type == FC_TYPE_FCP) {
+		if (prov_entry && prov_entry->recv)
+			lport->service_params |= FCP_SPPF_TARG_FCN;
+	}
+}
+
+void fc_lport_iterate(void (*notify)(struct fc_lport *, void *), void *arg)
+{
+	struct fc_lport *lport;
+
+	mutex_lock(&fc_prov_mutex);
+	list_for_each_entry(lport, &fc_local_ports, lport_list)
+		notify(lport, arg);
+	mutex_unlock(&fc_prov_mutex);
+}
+EXPORT_SYMBOL(fc_lport_iterate);
+
+/**
+ * fc_fc4_register_provider() - register FC-4 upper-level provider.
+ * @type: FC-4 type, such as FC_TYPE_FCP
+ * @prov: structure describing provider including ops vector.
+ *
+ * Returns 0 on success, negative error otherwise.
+ */
+int fc_fc4_register_provider(enum fc_fh_type type, struct fc4_prov *prov)
+{
+	struct fc4_prov **prov_entry;
+	int ret = 0;
+
+	if (type >= FC_FC4_PROV_SIZE)
+		return -EINVAL;
+	mutex_lock(&fc_prov_mutex);
+	prov_entry = (prov->recv ? fc_passive_prov : fc_active_prov) + type;
+	if (*prov_entry)
+		ret = -EBUSY;
+	else
+		*prov_entry = prov;
+	mutex_unlock(&fc_prov_mutex);
+	return ret;
+}
+EXPORT_SYMBOL(fc_fc4_register_provider);
+
+/**
+ * fc_fc4_deregister_provider() - deregister FC-4 upper-level provider.
+ * @type: FC-4 type, such as FC_TYPE_FCP
+ * @prov: structure describing provider including ops vector.
+ */
+void fc_fc4_deregister_provider(enum fc_fh_type type, struct fc4_prov *prov)
+{
+	BUG_ON(type >= FC_FC4_PROV_SIZE);
+	mutex_lock(&fc_prov_mutex);
+	if (prov->recv)
+		rcu_assign_pointer(fc_passive_prov[type], NULL);
+	else
+		rcu_assign_pointer(fc_active_prov[type], NULL);
+	mutex_unlock(&fc_prov_mutex);
+	synchronize_rcu();
+}
+EXPORT_SYMBOL(fc_fc4_deregister_provider);
+
+/**
+ * fc_fc4_add_lport() - add new local port to list and run notifiers.
+ * @lport:  The new local port.
+ */
+void fc_fc4_add_lport(struct fc_lport *lport)
+{
+	mutex_lock(&fc_prov_mutex);
+	list_add_tail(&lport->lport_list, &fc_local_ports);
+	blocking_notifier_call_chain(&fc_lport_notifier_head,
+				     FC_LPORT_EV_ADD, lport);
+	mutex_unlock(&fc_prov_mutex);
+}
+
+/**
+ * fc_fc4_del_lport() - remove local port from list and run notifiers.
+ * @lport:  The new local port.
+ */
+void fc_fc4_del_lport(struct fc_lport *lport)
+{
+	mutex_lock(&fc_prov_mutex);
+	list_del(&lport->lport_list);
+	blocking_notifier_call_chain(&fc_lport_notifier_head,
+				     FC_LPORT_EV_DEL, lport);
+	mutex_unlock(&fc_prov_mutex);
+}
diff --git a/drivers/scsi/libfc/fc_libfc.h b/drivers/scsi/libfc/fc_libfc.h
index eea0c3541b71..fedc819d70c0 100644
--- a/drivers/scsi/libfc/fc_libfc.h
+++ b/drivers/scsi/libfc/fc_libfc.h
@@ -94,6 +94,17 @@ extern unsigned int fc_debug_logging;
 				(lport)->host->host_no,	##args))
 
 /*
+ * FC-4 Providers.
+ */
+extern struct fc4_prov *fc_active_prov[];	/* providers without recv */
+extern struct fc4_prov *fc_passive_prov[];	/* providers with recv */
+extern struct mutex fc_prov_mutex;		/* lock over table changes */
+
+extern struct fc4_prov fc_rport_t0_prov;	/* type 0 provider */
+extern struct fc4_prov fc_lport_els_prov;	/* ELS provider */
+extern struct fc4_prov fc_rport_fcp_init;	/* FCP initiator provider */
+
+/*
  * Set up direct-data placement for this I/O request
  */
 void fc_fcp_ddp_setup(struct fc_fcp_pkt *fsp, u16 xid);
@@ -112,6 +123,9 @@ void fc_destroy_fcp(void);
  * Internal libfc functions
  */
 const char *fc_els_resp_type(struct fc_frame *);
+extern void fc_fc4_add_lport(struct fc_lport *);
+extern void fc_fc4_del_lport(struct fc_lport *);
+extern void fc_fc4_conf_lport_params(struct fc_lport *, enum fc_fh_type);
 
 /*
  * Copies a buffer into an sg list
diff --git a/drivers/scsi/libfc/fc_lport.c b/drivers/scsi/libfc/fc_lport.c
index c5a10f94f845..8c08b210001d 100644
--- a/drivers/scsi/libfc/fc_lport.c
+++ b/drivers/scsi/libfc/fc_lport.c
@@ -633,6 +633,7 @@ int fc_lport_destroy(struct fc_lport *lport)
 	lport->tt.fcp_abort_io(lport);
 	lport->tt.disc_stop_final(lport);
 	lport->tt.exch_mgr_reset(lport, 0, 0);
+	fc_fc4_del_lport(lport);
 	return 0;
 }
 EXPORT_SYMBOL(fc_lport_destroy);
@@ -849,7 +850,7 @@ out:
 }
 
 /**
- * fc_lport_recv_req() - The generic lport request handler
+ * fc_lport_recv_els_req() - The generic lport ELS request handler
  * @lport: The local port that received the request
  * @fp:	   The request frame
  *
@@ -859,9 +860,9 @@ out:
  * Locking Note: This function should not be called with the lport
  *		 lock held becuase it will grab the lock.
  */
-static void fc_lport_recv_req(struct fc_lport *lport, struct fc_frame *fp)
+static void fc_lport_recv_els_req(struct fc_lport *lport,
+				  struct fc_frame *fp)
 {
-	struct fc_frame_header *fh = fc_frame_header_get(fp);
 	void (*recv)(struct fc_lport *, struct fc_frame *);
 
 	mutex_lock(&lport->lp_mutex);
@@ -873,8 +874,7 @@ static void fc_lport_recv_req(struct fc_lport *lport, struct fc_frame *fp)
 	 */
 	if (!lport->link_up)
 		fc_frame_free(fp);
-	else if (fh->fh_type == FC_TYPE_ELS &&
-		 fh->fh_r_ctl == FC_RCTL_ELS_REQ) {
+	else {
 		/*
 		 * Check opcode.
 		 */
@@ -903,14 +903,62 @@ static void fc_lport_recv_req(struct fc_lport *lport, struct fc_frame *fp)
 		}
 
 		recv(lport, fp);
-	} else {
-		FC_LPORT_DBG(lport, "dropping invalid frame (eof %x)\n",
-			     fr_eof(fp));
-		fc_frame_free(fp);
 	}
 	mutex_unlock(&lport->lp_mutex);
 }
 
+static int fc_lport_els_prli(struct fc_rport_priv *rdata, u32 spp_len,
+			     const struct fc_els_spp *spp_in,
+			     struct fc_els_spp *spp_out)
+{
+	return FC_SPP_RESP_INVL;
+}
+
+struct fc4_prov fc_lport_els_prov = {
+	.prli = fc_lport_els_prli,
+	.recv = fc_lport_recv_els_req,
+};
+
+/**
+ * fc_lport_recv_req() - The generic lport request handler
+ * @lport: The lport that received the request
+ * @fp: The frame the request is in
+ *
+ * Locking Note: This function should not be called with the lport
+ *		 lock held becuase it may grab the lock.
+ */
+static void fc_lport_recv_req(struct fc_lport *lport,
+			      struct fc_frame *fp)
+{
+	struct fc_frame_header *fh = fc_frame_header_get(fp);
+	struct fc_seq *sp = fr_seq(fp);
+	struct fc4_prov *prov;
+
+	/*
+	 * Use RCU read lock and module_lock to be sure module doesn't
+	 * deregister and get unloaded while we're calling it.
+	 * try_module_get() is inlined and accepts a NULL parameter.
+	 * Only ELSes and FCP target ops should come through here.
+	 * The locking is unfortunate, and a better scheme is being sought.
+	 */
+
+	rcu_read_lock();
+	if (fh->fh_type >= FC_FC4_PROV_SIZE)
+		goto drop;
+	prov = rcu_dereference(fc_passive_prov[fh->fh_type]);
+	if (!prov || !try_module_get(prov->module))
+		goto drop;
+	rcu_read_unlock();
+	prov->recv(lport, fp);
+	module_put(prov->module);
+	return;
+drop:
+	rcu_read_unlock();
+	FC_LPORT_DBG(lport, "dropping unexpected frame type %x\n", fh->fh_type);
+	fc_frame_free(fp);
+	lport->tt.exch_done(sp);
+}
+
 /**
  * fc_lport_reset() - Reset a local port
  * @lport: The local port which should be reset
@@ -1542,6 +1590,7 @@ void fc_lport_enter_flogi(struct fc_lport *lport)
  */
 int fc_lport_config(struct fc_lport *lport)
 {
+	INIT_LIST_HEAD(&lport->ema_list);
 	INIT_DELAYED_WORK(&lport->retry_work, fc_lport_timeout);
 	mutex_init(&lport->lp_mutex);
 
@@ -1549,6 +1598,7 @@ int fc_lport_config(struct fc_lport *lport)
 
 	fc_lport_add_fc4_type(lport, FC_TYPE_FCP);
 	fc_lport_add_fc4_type(lport, FC_TYPE_CT);
+	fc_fc4_conf_lport_params(lport, FC_TYPE_FCP);
 
 	return 0;
 }
@@ -1586,6 +1636,7 @@ int fc_lport_init(struct fc_lport *lport)
 		fc_host_supported_speeds(lport->host) |= FC_PORTSPEED_1GBIT;
 	if (lport->link_supported_speeds & FC_PORTSPEED_10GBIT)
 		fc_host_supported_speeds(lport->host) |= FC_PORTSPEED_10GBIT;
+	fc_fc4_add_lport(lport);
 
 	return 0;
 }
diff --git a/drivers/scsi/libfc/fc_npiv.c b/drivers/scsi/libfc/fc_npiv.c
index dd2b43bb1c70..f33b897e4784 100644
--- a/drivers/scsi/libfc/fc_npiv.c
+++ b/drivers/scsi/libfc/fc_npiv.c
@@ -37,9 +37,7 @@ struct fc_lport *libfc_vport_create(struct fc_vport *vport, int privsize)
 
 	vn_port = libfc_host_alloc(shost->hostt, privsize);
 	if (!vn_port)
-		goto err_out;
-	if (fc_exch_mgr_list_clone(n_port, vn_port))
-		goto err_put;
+		return vn_port;
 
 	vn_port->vport = vport;
 	vport->dd_data = vn_port;
@@ -49,11 +47,6 @@ struct fc_lport *libfc_vport_create(struct fc_vport *vport, int privsize)
 	mutex_unlock(&n_port->lp_mutex);
 
 	return vn_port;
-
-err_put:
-	scsi_host_put(vn_port->host);
-err_out:
-	return NULL;
 }
 EXPORT_SYMBOL(libfc_vport_create);
 
@@ -86,6 +79,7 @@ struct fc_lport *fc_vport_id_lookup(struct fc_lport *n_port, u32 port_id)
 
 	return lport;
 }
+EXPORT_SYMBOL(fc_vport_id_lookup);
 
 /*
  * When setting the link state of vports during an lport state change, it's
diff --git a/drivers/scsi/libfc/fc_rport.c b/drivers/scsi/libfc/fc_rport.c
index a7175adab32d..49e1ccca09d5 100644
--- a/drivers/scsi/libfc/fc_rport.c
+++ b/drivers/scsi/libfc/fc_rport.c
@@ -58,7 +58,7 @@
 
 #include "fc_libfc.h"
 
-struct workqueue_struct *rport_event_queue;
+static struct workqueue_struct *rport_event_queue;
 
 static void fc_rport_enter_flogi(struct fc_rport_priv *);
 static void fc_rport_enter_plogi(struct fc_rport_priv *);
@@ -145,8 +145,10 @@ static struct fc_rport_priv *fc_rport_create(struct fc_lport *lport,
 	rdata->maxframe_size = FC_MIN_MAX_PAYLOAD;
 	INIT_DELAYED_WORK(&rdata->retry_work, fc_rport_timeout);
 	INIT_WORK(&rdata->event_work, fc_rport_work);
-	if (port_id != FC_FID_DIR_SERV)
+	if (port_id != FC_FID_DIR_SERV) {
+		rdata->lld_event_callback = lport->tt.rport_event_callback;
 		list_add_rcu(&rdata->peers, &lport->disc.rports);
+	}
 	return rdata;
 }
 
@@ -257,6 +259,8 @@ static void fc_rport_work(struct work_struct *work)
 	struct fc_rport_operations *rport_ops;
 	struct fc_rport_identifiers ids;
 	struct fc_rport *rport;
+	struct fc4_prov *prov;
+	u8 type;
 
 	mutex_lock(&rdata->rp_mutex);
 	event = rdata->event;
@@ -300,12 +304,25 @@ static void fc_rport_work(struct work_struct *work)
 			FC_RPORT_DBG(rdata, "callback ev %d\n", event);
 			rport_ops->event_callback(lport, rdata, event);
 		}
+		if (rdata->lld_event_callback) {
+			FC_RPORT_DBG(rdata, "lld callback ev %d\n", event);
+			rdata->lld_event_callback(lport, rdata, event);
+		}
 		kref_put(&rdata->kref, lport->tt.rport_destroy);
 		break;
 
 	case RPORT_EV_FAILED:
 	case RPORT_EV_LOGO:
 	case RPORT_EV_STOP:
+		if (rdata->prli_count) {
+			mutex_lock(&fc_prov_mutex);
+			for (type = 1; type < FC_FC4_PROV_SIZE; type++) {
+				prov = fc_passive_prov[type];
+				if (prov && prov->prlo)
+					prov->prlo(rdata);
+			}
+			mutex_unlock(&fc_prov_mutex);
+		}
 		port_id = rdata->ids.port_id;
 		mutex_unlock(&rdata->rp_mutex);
 
@@ -313,6 +330,10 @@ static void fc_rport_work(struct work_struct *work)
 			FC_RPORT_DBG(rdata, "callback ev %d\n", event);
 			rport_ops->event_callback(lport, rdata, event);
 		}
+		if (rdata->lld_event_callback) {
+			FC_RPORT_DBG(rdata, "lld callback ev %d\n", event);
+			rdata->lld_event_callback(lport, rdata, event);
+		}
 		cancel_delayed_work_sync(&rdata->retry_work);
 
 		/*
@@ -336,6 +357,7 @@ static void fc_rport_work(struct work_struct *work)
 			if (port_id == FC_FID_DIR_SERV) {
 				rdata->event = RPORT_EV_NONE;
 				mutex_unlock(&rdata->rp_mutex);
+				kref_put(&rdata->kref, lport->tt.rport_destroy);
 			} else if ((rdata->flags & FC_RP_STARTED) &&
 				   rdata->major_retries <
 				   lport->max_rport_retry_count) {
@@ -575,7 +597,7 @@ static void fc_rport_error_retry(struct fc_rport_priv *rdata,
 
 	/* make sure this isn't an FC_EX_CLOSED error, never retry those */
 	if (PTR_ERR(fp) == -FC_EX_CLOSED)
-		return fc_rport_error(rdata, fp);
+		goto out;
 
 	if (rdata->retries < rdata->local_port->max_rport_retry_count) {
 		FC_RPORT_DBG(rdata, "Error %ld in state %s, retrying\n",
@@ -588,7 +610,8 @@ static void fc_rport_error_retry(struct fc_rport_priv *rdata,
 		return;
 	}
 
-	return fc_rport_error(rdata, fp);
+out:
+	fc_rport_error(rdata, fp);
 }
 
 /**
@@ -878,6 +901,9 @@ static void fc_rport_plogi_resp(struct fc_seq *sp, struct fc_frame *fp,
 		rdata->ids.port_name = get_unaligned_be64(&plp->fl_wwpn);
 		rdata->ids.node_name = get_unaligned_be64(&plp->fl_wwnn);
 
+		/* save plogi response sp_features for further reference */
+		rdata->sp_features = ntohs(plp->fl_csp.sp_features);
+
 		if (lport->point_to_multipoint)
 			fc_rport_login_complete(rdata, fp);
 		csp_seq = ntohs(plp->fl_csp.sp_tot_seq);
@@ -949,6 +975,8 @@ static void fc_rport_prli_resp(struct fc_seq *sp, struct fc_frame *fp,
 		struct fc_els_prli prli;
 		struct fc_els_spp spp;
 	} *pp;
+	struct fc_els_spp temp_spp;
+	struct fc4_prov *prov;
 	u32 roles = FC_RPORT_ROLE_UNKNOWN;
 	u32 fcp_parm = 0;
 	u8 op;
@@ -983,6 +1011,7 @@ static void fc_rport_prli_resp(struct fc_seq *sp, struct fc_frame *fp,
 		resp_code = (pp->spp.spp_flags & FC_SPP_RESP_MASK);
 		FC_RPORT_DBG(rdata, "PRLI spp_flags = 0x%x\n",
 			     pp->spp.spp_flags);
+		rdata->spp_type = pp->spp.spp_type;
 		if (resp_code != FC_SPP_RESP_ACK) {
 			if (resp_code == FC_SPP_RESP_CONF)
 				fc_rport_error(rdata, fp);
@@ -996,6 +1025,15 @@ static void fc_rport_prli_resp(struct fc_seq *sp, struct fc_frame *fp,
 		fcp_parm = ntohl(pp->spp.spp_params);
 		if (fcp_parm & FCP_SPPF_RETRY)
 			rdata->flags |= FC_RP_FLAGS_RETRY;
+		if (fcp_parm & FCP_SPPF_CONF_COMPL)
+			rdata->flags |= FC_RP_FLAGS_CONF_REQ;
+
+		prov = fc_passive_prov[FC_TYPE_FCP];
+		if (prov) {
+			memset(&temp_spp, 0, sizeof(temp_spp));
+			prov->prli(rdata, pp->prli.prli_spp_len,
+				   &pp->spp, &temp_spp);
+		}
 
 		rdata->supported_classes = FC_COS_CLASS3;
 		if (fcp_parm & FCP_SPPF_INIT_FCN)
@@ -1033,6 +1071,7 @@ static void fc_rport_enter_prli(struct fc_rport_priv *rdata)
 		struct fc_els_spp spp;
 	} *pp;
 	struct fc_frame *fp;
+	struct fc4_prov *prov;
 
 	/*
 	 * If the rport is one of the well known addresses
@@ -1054,9 +1093,20 @@ static void fc_rport_enter_prli(struct fc_rport_priv *rdata)
 		return;
 	}
 
-	if (!lport->tt.elsct_send(lport, rdata->ids.port_id, fp, ELS_PRLI,
-				  fc_rport_prli_resp, rdata,
-				  2 * lport->r_a_tov))
+	fc_prli_fill(lport, fp);
+
+	prov = fc_passive_prov[FC_TYPE_FCP];
+	if (prov) {
+		pp = fc_frame_payload_get(fp, sizeof(*pp));
+		prov->prli(rdata, sizeof(pp->spp), NULL, &pp->spp);
+	}
+
+	fc_fill_fc_hdr(fp, FC_RCTL_ELS_REQ, rdata->ids.port_id,
+		       fc_host_port_id(lport->host), FC_TYPE_ELS,
+		       FC_FC_FIRST_SEQ | FC_FC_END_SEQ | FC_FC_SEQ_INIT, 0);
+
+	if (!lport->tt.exch_seq_send(lport, fp, fc_rport_prli_resp,
+				    NULL, rdata, 2 * lport->r_a_tov))
 		fc_rport_error_retry(rdata, NULL);
 	else
 		kref_get(&rdata->kref);
@@ -1642,9 +1692,9 @@ static void fc_rport_recv_prli_req(struct fc_rport_priv *rdata,
 	unsigned int len;
 	unsigned int plen;
 	enum fc_els_spp_resp resp;
+	enum fc_els_spp_resp passive;
 	struct fc_seq_els_data rjt_data;
-	u32 fcp_parm;
-	u32 roles = FC_RPORT_ROLE_UNKNOWN;
+	struct fc4_prov *prov;
 
 	FC_RPORT_DBG(rdata, "Received PRLI request while in state %s\n",
 		     fc_rport_state(rdata));
@@ -1678,46 +1728,42 @@ static void fc_rport_recv_prli_req(struct fc_rport_priv *rdata,
 	pp->prli.prli_len = htons(len);
 	len -= sizeof(struct fc_els_prli);
 
-	/* reinitialize remote port roles */
-	rdata->ids.roles = FC_RPORT_ROLE_UNKNOWN;
-
 	/*
 	 * Go through all the service parameter pages and build
 	 * response.  If plen indicates longer SPP than standard,
 	 * use that.  The entire response has been pre-cleared above.
 	 */
 	spp = &pp->spp;
+	mutex_lock(&fc_prov_mutex);
 	while (len >= plen) {
+		rdata->spp_type = rspp->spp_type;
 		spp->spp_type = rspp->spp_type;
 		spp->spp_type_ext = rspp->spp_type_ext;
-		spp->spp_flags = rspp->spp_flags & FC_SPP_EST_IMG_PAIR;
-		resp = FC_SPP_RESP_ACK;
-
-		switch (rspp->spp_type) {
-		case 0:	/* common to all FC-4 types */
-			break;
-		case FC_TYPE_FCP:
-			fcp_parm = ntohl(rspp->spp_params);
-			if (fcp_parm & FCP_SPPF_RETRY)
-				rdata->flags |= FC_RP_FLAGS_RETRY;
-			rdata->supported_classes = FC_COS_CLASS3;
-			if (fcp_parm & FCP_SPPF_INIT_FCN)
-				roles |= FC_RPORT_ROLE_FCP_INITIATOR;
-			if (fcp_parm & FCP_SPPF_TARG_FCN)
-				roles |= FC_RPORT_ROLE_FCP_TARGET;
-			rdata->ids.roles = roles;
-
-			spp->spp_params = htonl(lport->service_params);
-			break;
-		default:
-			resp = FC_SPP_RESP_INVL;
-			break;
+		resp = 0;
+
+		if (rspp->spp_type < FC_FC4_PROV_SIZE) {
+			prov = fc_active_prov[rspp->spp_type];
+			if (prov)
+				resp = prov->prli(rdata, plen, rspp, spp);
+			prov = fc_passive_prov[rspp->spp_type];
+			if (prov) {
+				passive = prov->prli(rdata, plen, rspp, spp);
+				if (!resp || passive == FC_SPP_RESP_ACK)
+					resp = passive;
+			}
+		}
+		if (!resp) {
+			if (spp->spp_flags & FC_SPP_EST_IMG_PAIR)
+				resp |= FC_SPP_RESP_CONF;
+			else
+				resp |= FC_SPP_RESP_INVL;
 		}
 		spp->spp_flags |= resp;
 		len -= plen;
 		rspp = (struct fc_els_spp *)((char *)rspp + plen);
 		spp = (struct fc_els_spp *)((char *)spp + plen);
 	}
+	mutex_unlock(&fc_prov_mutex);
 
 	/*
 	 * Send LS_ACC.	 If this fails, the originator should retry.
@@ -1887,9 +1933,82 @@ int fc_rport_init(struct fc_lport *lport)
 EXPORT_SYMBOL(fc_rport_init);
 
 /**
+ * fc_rport_fcp_prli() - Handle incoming PRLI for the FCP initiator.
+ * @rdata: remote port private
+ * @spp_len: service parameter page length
+ * @rspp: received service parameter page
+ * @spp: response service parameter page
+ *
+ * Returns the value for the response code to be placed in spp_flags;
+ * Returns 0 if not an initiator.
+ */
+static int fc_rport_fcp_prli(struct fc_rport_priv *rdata, u32 spp_len,
+			     const struct fc_els_spp *rspp,
+			     struct fc_els_spp *spp)
+{
+	struct fc_lport *lport = rdata->local_port;
+	u32 fcp_parm;
+
+	fcp_parm = ntohl(rspp->spp_params);
+	rdata->ids.roles = FC_RPORT_ROLE_UNKNOWN;
+	if (fcp_parm & FCP_SPPF_INIT_FCN)
+		rdata->ids.roles |= FC_RPORT_ROLE_FCP_INITIATOR;
+	if (fcp_parm & FCP_SPPF_TARG_FCN)
+		rdata->ids.roles |= FC_RPORT_ROLE_FCP_TARGET;
+	if (fcp_parm & FCP_SPPF_RETRY)
+		rdata->flags |= FC_RP_FLAGS_RETRY;
+	rdata->supported_classes = FC_COS_CLASS3;
+
+	if (!(lport->service_params & FC_RPORT_ROLE_FCP_INITIATOR))
+		return 0;
+
+	spp->spp_flags |= rspp->spp_flags & FC_SPP_EST_IMG_PAIR;
+
+	/*
+	 * OR in our service parameters with other providers (target), if any.
+	 */
+	fcp_parm = ntohl(spp->spp_params);
+	spp->spp_params = htonl(fcp_parm | lport->service_params);
+	return FC_SPP_RESP_ACK;
+}
+
+/*
+ * FC-4 provider ops for FCP initiator.
+ */
+struct fc4_prov fc_rport_fcp_init = {
+	.prli = fc_rport_fcp_prli,
+};
+
+/**
+ * fc_rport_t0_prli() - Handle incoming PRLI parameters for type 0
+ * @rdata: remote port private
+ * @spp_len: service parameter page length
+ * @rspp: received service parameter page
+ * @spp: response service parameter page
+ */
+static int fc_rport_t0_prli(struct fc_rport_priv *rdata, u32 spp_len,
+			    const struct fc_els_spp *rspp,
+			    struct fc_els_spp *spp)
+{
+	if (rspp->spp_flags & FC_SPP_EST_IMG_PAIR)
+		return FC_SPP_RESP_INVL;
+	return FC_SPP_RESP_ACK;
+}
+
+/*
+ * FC-4 provider ops for type 0 service parameters.
+ *
+ * This handles the special case of type 0 which is always successful
+ * but doesn't do anything otherwise.
+ */
+struct fc4_prov fc_rport_t0_prov = {
+	.prli = fc_rport_t0_prli,
+};
+
+/**
  * fc_setup_rport() - Initialize the rport_event_queue
  */
-int fc_setup_rport()
+int fc_setup_rport(void)
 {
 	rport_event_queue = create_singlethread_workqueue("fc_rport_eq");
 	if (!rport_event_queue)
@@ -1900,7 +2019,7 @@ int fc_setup_rport()
 /**
  * fc_destroy_rport() - Destroy the rport_event_queue
  */
-void fc_destroy_rport()
+void fc_destroy_rport(void)
 {
 	destroy_workqueue(rport_event_queue);
 }
diff --git a/drivers/scsi/libiscsi.c b/drivers/scsi/libiscsi.c
index da8b61543ee4..0c550d5b9133 100644
--- a/drivers/scsi/libiscsi.c
+++ b/drivers/scsi/libiscsi.c
@@ -3352,6 +3352,47 @@ int iscsi_session_get_param(struct iscsi_cls_session *cls_session,
 }
 EXPORT_SYMBOL_GPL(iscsi_session_get_param);
 
+int iscsi_conn_get_addr_param(struct sockaddr_storage *addr,
+			      enum iscsi_param param, char *buf)
+{
+	struct sockaddr_in6 *sin6 = NULL;
+	struct sockaddr_in *sin = NULL;
+	int len;
+
+	switch (addr->ss_family) {
+	case AF_INET:
+		sin = (struct sockaddr_in *)addr;
+		break;
+	case AF_INET6:
+		sin6 = (struct sockaddr_in6 *)addr;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (param) {
+	case ISCSI_PARAM_CONN_ADDRESS:
+	case ISCSI_HOST_PARAM_IPADDRESS:
+		if (sin)
+			len = sprintf(buf, "%pI4\n", &sin->sin_addr.s_addr);
+		else
+			len = sprintf(buf, "%pI6\n", &sin6->sin6_addr);
+		break;
+	case ISCSI_PARAM_CONN_PORT:
+		if (sin)
+			len = sprintf(buf, "%hu\n", be16_to_cpu(sin->sin_port));
+		else
+			len = sprintf(buf, "%hu\n",
+				      be16_to_cpu(sin6->sin6_port));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return len;
+}
+EXPORT_SYMBOL_GPL(iscsi_conn_get_addr_param);
+
 int iscsi_conn_get_param(struct iscsi_cls_conn *cls_conn,
 			 enum iscsi_param param, char *buf)
 {
@@ -3416,9 +3457,6 @@ int iscsi_host_get_param(struct Scsi_Host *shost, enum iscsi_host_param param,
 	case ISCSI_HOST_PARAM_INITIATOR_NAME:
 		len = sprintf(buf, "%s\n", ihost->initiatorname);
 		break;
-	case ISCSI_HOST_PARAM_IPADDRESS:
-		len = sprintf(buf, "%s\n", ihost->local_address);
-		break;
 	default:
 		return -ENOSYS;
 	}
diff --git a/drivers/scsi/libsas/Kconfig b/drivers/scsi/libsas/Kconfig
index 18f33cd54411..9dafe64e7c7a 100644
--- a/drivers/scsi/libsas/Kconfig
+++ b/drivers/scsi/libsas/Kconfig
@@ -46,11 +46,3 @@ config SCSI_SAS_HOST_SMP
 		Allows sas hosts to receive SMP frames.  Selecting this
 		option builds an SMP interpreter into libsas.  Say
 		N here if you want to save the few kb this consumes.
-
-config SCSI_SAS_LIBSAS_DEBUG
-	bool "Compile the SAS Domain Transport Attributes in debug mode"
-	default y
-	depends on SCSI_SAS_LIBSAS
-	help
-		Compiles the SAS Layer in debug mode.  In debug mode, the
-		SAS Layer prints diagnostic and debug messages.
diff --git a/drivers/scsi/libsas/Makefile b/drivers/scsi/libsas/Makefile
index 1ad1323c60fa..566a10024598 100644
--- a/drivers/scsi/libsas/Makefile
+++ b/drivers/scsi/libsas/Makefile
@@ -21,10 +21,6 @@
 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 # USA
 
-ifeq ($(CONFIG_SCSI_SAS_LIBSAS_DEBUG),y)
-	EXTRA_CFLAGS += -DSAS_DEBUG
-endif
-
 obj-$(CONFIG_SCSI_SAS_LIBSAS) += libsas.o
 libsas-y +=  sas_init.o     \
 		sas_phy.o      \
diff --git a/drivers/scsi/libsas/sas_ata.c b/drivers/scsi/libsas/sas_ata.c
index e1a395b438ee..d2b2d1474c28 100644
--- a/drivers/scsi/libsas/sas_ata.c
+++ b/drivers/scsi/libsas/sas_ata.c
@@ -71,13 +71,13 @@ static enum ata_completion_errors sas_to_ata_err(struct task_status_struct *ts)
 		case SAS_SG_ERR:
 			return AC_ERR_INVALID;
 
-		case SAM_STAT_CHECK_CONDITION:
 		case SAS_OPEN_TO:
 		case SAS_OPEN_REJECT:
 			SAS_DPRINTK("%s: Saw error %d.  What to do?\n",
 				    __func__, ts->stat);
 			return AC_ERR_OTHER;
 
+		case SAM_STAT_CHECK_CONDITION:
 		case SAS_ABORTED_TASK:
 			return AC_ERR_DEV;
 
@@ -107,13 +107,15 @@ static void sas_ata_task_done(struct sas_task *task)
 	sas_ha = dev->port->ha;
 
 	spin_lock_irqsave(dev->sata_dev.ap->lock, flags);
-	if (stat->stat == SAS_PROTO_RESPONSE || stat->stat == SAM_STAT_GOOD) {
+	if (stat->stat == SAS_PROTO_RESPONSE || stat->stat == SAM_STAT_GOOD ||
+	    ((stat->stat == SAM_STAT_CHECK_CONDITION &&
+	      dev->sata_dev.command_set == ATAPI_COMMAND_SET))) {
 		ata_tf_from_fis(resp->ending_fis, &dev->sata_dev.tf);
 		qc->err_mask |= ac_err_mask(dev->sata_dev.tf.command);
 		dev->sata_dev.sstatus = resp->sstatus;
 		dev->sata_dev.serror = resp->serror;
 		dev->sata_dev.scontrol = resp->scontrol;
-	} else if (stat->stat != SAM_STAT_GOOD) {
+	} else {
 		ac = sas_to_ata_err(stat);
 		if (ac) {
 			SAS_DPRINTK("%s: SAS error %x\n", __func__,
@@ -238,37 +240,43 @@ static bool sas_ata_qc_fill_rtf(struct ata_queued_cmd *qc)
 	return true;
 }
 
-static void sas_ata_phy_reset(struct ata_port *ap)
+static int sas_ata_hard_reset(struct ata_link *link, unsigned int *class,
+			       unsigned long deadline)
 {
+	struct ata_port *ap = link->ap;
 	struct domain_device *dev = ap->private_data;
 	struct sas_internal *i =
 		to_sas_internal(dev->port->ha->core.shost->transportt);
 	int res = TMF_RESP_FUNC_FAILED;
+	int ret = 0;
 
 	if (i->dft->lldd_I_T_nexus_reset)
 		res = i->dft->lldd_I_T_nexus_reset(dev);
 
-	if (res != TMF_RESP_FUNC_COMPLETE)
+	if (res != TMF_RESP_FUNC_COMPLETE) {
 		SAS_DPRINTK("%s: Unable to reset I T nexus?\n", __func__);
+		ret = -EAGAIN;
+	}
 
 	switch (dev->sata_dev.command_set) {
 		case ATA_COMMAND_SET:
 			SAS_DPRINTK("%s: Found ATA device.\n", __func__);
-			ap->link.device[0].class = ATA_DEV_ATA;
+			*class = ATA_DEV_ATA;
 			break;
 		case ATAPI_COMMAND_SET:
 			SAS_DPRINTK("%s: Found ATAPI device.\n", __func__);
-			ap->link.device[0].class = ATA_DEV_ATAPI;
+			*class = ATA_DEV_ATAPI;
 			break;
 		default:
 			SAS_DPRINTK("%s: Unknown SATA command set: %d.\n",
 				    __func__,
 				    dev->sata_dev.command_set);
-			ap->link.device[0].class = ATA_DEV_UNKNOWN;
+			*class = ATA_DEV_UNKNOWN;
 			break;
 	}
 
 	ap->cbl = ATA_CBL_SATA;
+	return ret;
 }
 
 static void sas_ata_post_internal(struct ata_queued_cmd *qc)
@@ -299,57 +307,12 @@ static void sas_ata_post_internal(struct ata_queued_cmd *qc)
 	}
 }
 
-static int sas_ata_scr_write(struct ata_link *link, unsigned int sc_reg_in,
-			      u32 val)
-{
-	struct domain_device *dev = link->ap->private_data;
-
-	SAS_DPRINTK("STUB %s\n", __func__);
-	switch (sc_reg_in) {
-		case SCR_STATUS:
-			dev->sata_dev.sstatus = val;
-			break;
-		case SCR_CONTROL:
-			dev->sata_dev.scontrol = val;
-			break;
-		case SCR_ERROR:
-			dev->sata_dev.serror = val;
-			break;
-		case SCR_ACTIVE:
-			dev->sata_dev.ap->link.sactive = val;
-			break;
-		default:
-			return -EINVAL;
-	}
-	return 0;
-}
-
-static int sas_ata_scr_read(struct ata_link *link, unsigned int sc_reg_in,
-			    u32 *val)
-{
-	struct domain_device *dev = link->ap->private_data;
-
-	SAS_DPRINTK("STUB %s\n", __func__);
-	switch (sc_reg_in) {
-		case SCR_STATUS:
-			*val = dev->sata_dev.sstatus;
-			return 0;
-		case SCR_CONTROL:
-			*val = dev->sata_dev.scontrol;
-			return 0;
-		case SCR_ERROR:
-			*val = dev->sata_dev.serror;
-			return 0;
-		case SCR_ACTIVE:
-			*val = dev->sata_dev.ap->link.sactive;
-			return 0;
-		default:
-			return -EINVAL;
-	}
-}
-
 static struct ata_port_operations sas_sata_ops = {
-	.phy_reset		= sas_ata_phy_reset,
+	.prereset		= ata_std_prereset,
+	.softreset		= NULL,
+	.hardreset		= sas_ata_hard_reset,
+	.postreset		= ata_std_postreset,
+	.error_handler		= ata_std_error_handler,
 	.post_internal_cmd	= sas_ata_post_internal,
 	.qc_defer               = ata_std_qc_defer,
 	.qc_prep		= ata_noop_qc_prep,
@@ -357,15 +320,12 @@ static struct ata_port_operations sas_sata_ops = {
 	.qc_fill_rtf		= sas_ata_qc_fill_rtf,
 	.port_start		= ata_sas_port_start,
 	.port_stop		= ata_sas_port_stop,
-	.scr_read		= sas_ata_scr_read,
-	.scr_write		= sas_ata_scr_write
 };
 
 static struct ata_port_info sata_port_info = {
-	.flags = ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY | ATA_FLAG_SATA_RESET |
-		ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA | ATA_FLAG_NCQ,
-	.pio_mask = 0x1f, /* PIO0-4 */
-	.mwdma_mask = 0x07, /* MWDMA0-2 */
+	.flags = ATA_FLAG_SATA | ATA_FLAG_PIO_DMA | ATA_FLAG_NCQ,
+	.pio_mask = ATA_PIO4,
+	.mwdma_mask = ATA_MWDMA2,
 	.udma_mask = ATA_UDMA6,
 	.port_ops = &sas_sata_ops
 };
@@ -781,3 +741,68 @@ int sas_discover_sata(struct domain_device *dev)
 
 	return res;
 }
+
+void sas_ata_strategy_handler(struct Scsi_Host *shost)
+{
+	struct scsi_device *sdev;
+
+	shost_for_each_device(sdev, shost) {
+		struct domain_device *ddev = sdev_to_domain_dev(sdev);
+		struct ata_port *ap = ddev->sata_dev.ap;
+
+		if (!dev_is_sata(ddev))
+			continue;
+
+		ata_port_printk(ap, KERN_DEBUG, "sas eh calling libata port error handler");
+		ata_scsi_port_error_handler(shost, ap);
+	}
+}
+
+int sas_ata_timed_out(struct scsi_cmnd *cmd, struct sas_task *task,
+		      enum blk_eh_timer_return *rtn)
+{
+	struct domain_device *ddev = cmd_to_domain_dev(cmd);
+
+	if (!dev_is_sata(ddev) || task)
+		return 0;
+
+	/* we're a sata device with no task, so this must be a libata
+	 * eh timeout.  Ideally should hook into libata timeout
+	 * handling, but there's no point, it just wants to activate
+	 * the eh thread */
+	*rtn = BLK_EH_NOT_HANDLED;
+	return 1;
+}
+
+int sas_ata_eh(struct Scsi_Host *shost, struct list_head *work_q,
+	       struct list_head *done_q)
+{
+	int rtn = 0;
+	struct scsi_cmnd *cmd, *n;
+	struct ata_port *ap;
+
+	do {
+		LIST_HEAD(sata_q);
+
+		ap = NULL;
+
+		list_for_each_entry_safe(cmd, n, work_q, eh_entry) {
+			struct domain_device *ddev = cmd_to_domain_dev(cmd);
+
+			if (!dev_is_sata(ddev) || TO_SAS_TASK(cmd))
+				continue;
+			if (ap && ap != ddev->sata_dev.ap)
+				continue;
+			ap = ddev->sata_dev.ap;
+			rtn = 1;
+			list_move(&cmd->eh_entry, &sata_q);
+		}
+
+		if (!list_empty(&sata_q)) {
+			ata_port_printk(ap, KERN_DEBUG, "sas eh calling libata cmd error handler\n");
+			ata_scsi_cmd_error_handler(shost, ap, &sata_q);
+		}
+	} while (ap);
+
+	return rtn;
+}
diff --git a/drivers/scsi/libsas/sas_dump.c b/drivers/scsi/libsas/sas_dump.c
index c17c25030f1c..fc460933575c 100644
--- a/drivers/scsi/libsas/sas_dump.c
+++ b/drivers/scsi/libsas/sas_dump.c
@@ -24,8 +24,6 @@
 
 #include "sas_dump.h"
 
-#ifdef SAS_DEBUG
-
 static const char *sas_hae_str[] = {
 	[0] = "HAE_RESET",
 };
@@ -72,5 +70,3 @@ void sas_dump_port(struct asd_sas_port *port)
 	SAS_DPRINTK("port%d: oob_mode:0x%x\n", port->id, port->oob_mode);
 	SAS_DPRINTK("port%d: num_phys:%d\n", port->id, port->num_phys);
 }
-
-#endif /* SAS_DEBUG */
diff --git a/drivers/scsi/libsas/sas_dump.h b/drivers/scsi/libsas/sas_dump.h
index 47b45d4f5258..800e4c69093f 100644
--- a/drivers/scsi/libsas/sas_dump.h
+++ b/drivers/scsi/libsas/sas_dump.h
@@ -24,19 +24,7 @@
 
 #include "sas_internal.h"
 
-#ifdef SAS_DEBUG
-
 void sas_dprint_porte(int phyid, enum port_event pe);
 void sas_dprint_phye(int phyid, enum phy_event pe);
 void sas_dprint_hae(struct sas_ha_struct *sas_ha, enum ha_event he);
 void sas_dump_port(struct asd_sas_port *port);
-
-#else /* SAS_DEBUG */
-
-static inline void sas_dprint_porte(int phyid, enum port_event pe) { }
-static inline void sas_dprint_phye(int phyid, enum phy_event pe) { }
-static inline void sas_dprint_hae(struct sas_ha_struct *sas_ha,
-				  enum ha_event he) { }
-static inline void sas_dump_port(struct asd_sas_port *port) { }
-
-#endif /* SAS_DEBUG */
diff --git a/drivers/scsi/libsas/sas_expander.c b/drivers/scsi/libsas/sas_expander.c
index 505ffe358293..f3f693b772ac 100644
--- a/drivers/scsi/libsas/sas_expander.c
+++ b/drivers/scsi/libsas/sas_expander.c
@@ -244,6 +244,11 @@ static int sas_ex_phy_discover_helper(struct domain_device *dev, u8 *disc_req,
 		 * dev to host FIS as described in section G.5 of
 		 * sas-2 r 04b */
 		dr = &((struct smp_resp *)disc_resp)->disc;
+		if (memcmp(dev->sas_addr, dr->attached_sas_addr,
+			  SAS_ADDR_SIZE) == 0) {
+			sas_printk("Found loopback topology, just ignore it!\n");
+			return 0;
+		}
 		if (!(dr->attached_dev_type == 0 &&
 		      dr->attached_sata_dev))
 			break;
diff --git a/drivers/scsi/libsas/sas_internal.h b/drivers/scsi/libsas/sas_internal.h
index 0001374bd6b2..8b538bd1ff2b 100644
--- a/drivers/scsi/libsas/sas_internal.h
+++ b/drivers/scsi/libsas/sas_internal.h
@@ -33,11 +33,7 @@
 
 #define sas_printk(fmt, ...) printk(KERN_NOTICE "sas: " fmt, ## __VA_ARGS__)
 
-#ifdef SAS_DEBUG
-#define SAS_DPRINTK(fmt, ...) printk(KERN_NOTICE "sas: " fmt, ## __VA_ARGS__)
-#else
-#define SAS_DPRINTK(fmt, ...)
-#endif
+#define SAS_DPRINTK(fmt, ...) printk(KERN_DEBUG "sas: " fmt, ## __VA_ARGS__)
 
 #define TO_SAS_TASK(_scsi_cmd)  ((void *)(_scsi_cmd)->host_scribble)
 #define ASSIGN_SAS_TASK(_sc, _t) do { (_sc)->host_scribble = (void *) _t; } while (0)
diff --git a/drivers/scsi/libsas/sas_scsi_host.c b/drivers/scsi/libsas/sas_scsi_host.c
index 9a7aaf5f1311..bece759c51ec 100644
--- a/drivers/scsi/libsas/sas_scsi_host.c
+++ b/drivers/scsi/libsas/sas_scsi_host.c
@@ -663,11 +663,16 @@ void sas_scsi_recover_host(struct Scsi_Host *shost)
 	 * scsi_unjam_host does, but we skip scsi_eh_abort_cmds because any
 	 * command we see here has no sas_task and is thus unknown to the HA.
 	 */
-	if (!scsi_eh_get_sense(&eh_work_q, &ha->eh_done_q))
-		scsi_eh_ready_devs(shost, &eh_work_q, &ha->eh_done_q);
+	if (!sas_ata_eh(shost, &eh_work_q, &ha->eh_done_q))
+		if (!scsi_eh_get_sense(&eh_work_q, &ha->eh_done_q))
+			scsi_eh_ready_devs(shost, &eh_work_q, &ha->eh_done_q);
 
 out:
+	/* now link into libata eh --- if we have any ata devices */
+	sas_ata_strategy_handler(shost);
+
 	scsi_eh_flush_done_q(&ha->eh_done_q);
+
 	SAS_DPRINTK("--- Exit %s\n", __func__);
 	return;
 }
@@ -676,6 +681,11 @@ enum blk_eh_timer_return sas_scsi_timed_out(struct scsi_cmnd *cmd)
 {
 	struct sas_task *task = TO_SAS_TASK(cmd);
 	unsigned long flags;
+	enum blk_eh_timer_return rtn;
+
+	if (sas_ata_timed_out(cmd, task, &rtn))
+		return rtn;
+
 
 	if (!task) {
 		cmd->request->timeout /= 2;
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index 746dd3d7a092..b64c6da870d3 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2004-2010 Emulex.  All rights reserved.           *
+ * Copyright (C) 2004-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  * Portions Copyright (C) 2004-2005 Christoph Hellwig              *
@@ -325,6 +325,7 @@ struct lpfc_vport {
 #define FC_VPORT_CVL_RCVD	0x400000 /* VLink failed due to CVL	 */
 #define FC_VFI_REGISTERED	0x800000 /* VFI is registered */
 #define FC_FDISC_COMPLETED	0x1000000/* FDISC completed */
+#define FC_DISC_DELAYED		0x2000000/* Delay NPort discovery */
 
 	uint32_t ct_flags;
 #define FC_CT_RFF_ID		0x1	 /* RFF_ID accepted by switch */
@@ -348,6 +349,8 @@ struct lpfc_vport {
 
 	uint32_t fc_myDID;	/* fibre channel S_ID */
 	uint32_t fc_prevDID;	/* previous fibre channel S_ID */
+	struct lpfc_name fabric_portname;
+	struct lpfc_name fabric_nodename;
 
 	int32_t stopped;   /* HBA has not been restarted since last ERATT */
 	uint8_t fc_linkspeed;	/* Link speed after last READ_LA */
@@ -372,6 +375,7 @@ struct lpfc_vport {
 #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 */
 #define WORKER_HB_TMO                  0x200	/* hba: Heart beat timeout */
@@ -382,6 +386,7 @@ struct lpfc_vport {
 
 	struct timer_list fc_fdmitmo;
 	struct timer_list els_tmofunc;
+	struct timer_list delayed_disc_tmo;
 
 	int unreg_vpi_cmpl;
 
@@ -548,6 +553,8 @@ struct lpfc_hba {
 #define LPFC_SLI3_CRP_ENABLED		0x08
 #define LPFC_SLI3_BG_ENABLED		0x20
 #define LPFC_SLI3_DSS_ENABLED		0x40
+#define LPFC_SLI4_PERFH_ENABLED		0x80
+#define LPFC_SLI4_PHWQ_ENABLED		0x100
 	uint32_t iocb_cmd_size;
 	uint32_t iocb_rsp_size;
 
@@ -655,7 +662,7 @@ struct lpfc_hba {
 #define LPFC_INITIALIZE_LINK              0	/* do normal init_link mbox */
 #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;
 	lpfc_vpd_t vpd;		/* vital product data */
 
 	struct pci_dev *pcidev;
@@ -792,6 +799,10 @@ struct lpfc_hba {
 	struct dentry *debug_slow_ring_trc;
 	struct lpfc_debugfs_trc *slow_ring_trc;
 	atomic_t slow_ring_trc_cnt;
+	/* iDiag debugfs sub-directory */
+	struct dentry *idiag_root;
+	struct dentry *idiag_pci_cfg;
+	struct dentry *idiag_que_info;
 #endif
 
 	/* Used for deferred freeing of ELS data buffers */
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index 3512abb8a587..e7c020df12fa 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2004-2009 Emulex.  All rights reserved.           *
+ * Copyright (C) 2004-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  * Portions Copyright (C) 2004-2005 Christoph Hellwig              *
@@ -623,10 +623,14 @@ lpfc_do_offline(struct lpfc_hba *phba, uint32_t type)
 	int status = 0;
 	int cnt = 0;
 	int i;
+	int rc;
 
 	init_completion(&online_compl);
-	lpfc_workq_post_event(phba, &status, &online_compl,
+	rc = lpfc_workq_post_event(phba, &status, &online_compl,
 			      LPFC_EVT_OFFLINE_PREP);
+	if (rc == 0)
+		return -ENOMEM;
+
 	wait_for_completion(&online_compl);
 
 	if (status != 0)
@@ -652,7 +656,10 @@ lpfc_do_offline(struct lpfc_hba *phba, uint32_t type)
 	}
 
 	init_completion(&online_compl);
-	lpfc_workq_post_event(phba, &status, &online_compl, type);
+	rc = lpfc_workq_post_event(phba, &status, &online_compl, type);
+	if (rc == 0)
+		return -ENOMEM;
+
 	wait_for_completion(&online_compl);
 
 	if (status != 0)
@@ -671,6 +678,7 @@ lpfc_do_offline(struct lpfc_hba *phba, uint32_t type)
  *
  * Notes:
  * Assumes any error from lpfc_do_offline() will be negative.
+ * Do not make this function static.
  *
  * Returns:
  * lpfc_do_offline() return code if not zero
@@ -682,6 +690,7 @@ lpfc_selective_reset(struct lpfc_hba *phba)
 {
 	struct completion online_compl;
 	int status = 0;
+	int rc;
 
 	if (!phba->cfg_enable_hba_reset)
 		return -EIO;
@@ -692,8 +701,11 @@ lpfc_selective_reset(struct lpfc_hba *phba)
 		return status;
 
 	init_completion(&online_compl);
-	lpfc_workq_post_event(phba, &status, &online_compl,
+	rc = lpfc_workq_post_event(phba, &status, &online_compl,
 			      LPFC_EVT_ONLINE);
+	if (rc == 0)
+		return -ENOMEM;
+
 	wait_for_completion(&online_compl);
 
 	if (status != 0)
@@ -812,14 +824,17 @@ lpfc_board_mode_store(struct device *dev, struct device_attribute *attr,
 	struct lpfc_hba   *phba = vport->phba;
 	struct completion online_compl;
 	int status=0;
+	int rc;
 
 	if (!phba->cfg_enable_hba_reset)
 		return -EACCES;
 	init_completion(&online_compl);
 
 	if(strncmp(buf, "online", sizeof("online") - 1) == 0) {
-		lpfc_workq_post_event(phba, &status, &online_compl,
+		rc = lpfc_workq_post_event(phba, &status, &online_compl,
 				      LPFC_EVT_ONLINE);
+		if (rc == 0)
+			return -ENOMEM;
 		wait_for_completion(&online_compl);
 	} else if (strncmp(buf, "offline", sizeof("offline") - 1) == 0)
 		status = lpfc_do_offline(phba, LPFC_EVT_OFFLINE);
@@ -1279,6 +1294,28 @@ lpfc_fips_rev_show(struct device *dev,  struct device_attribute *attr,
 }
 
 /**
+ * lpfc_dss_show - Return the current state of dss and the configured state
+ * @dev: class converted to a Scsi_host structure.
+ * @attr: device attribute, not used.
+ * @buf: on return contains the formatted text.
+ *
+ * Returns: size of formatted string.
+ **/
+static ssize_t
+lpfc_dss_show(struct device *dev, struct device_attribute *attr,
+	      char *buf)
+{
+	struct Scsi_Host *shost = class_to_shost(dev);
+	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
+	struct lpfc_hba   *phba = vport->phba;
+
+	return snprintf(buf, PAGE_SIZE, "%s - %sOperational\n",
+			(phba->cfg_enable_dss) ? "Enabled" : "Disabled",
+			(phba->sli3_options & LPFC_SLI3_DSS_ENABLED) ?
+				"" : "Not ");
+}
+
+/**
  * lpfc_param_show - Return a cfg attribute value in decimal
  *
  * Description:
@@ -1597,13 +1634,13 @@ lpfc_##attr##_store(struct device *dev, struct device_attribute *attr, \
 
 #define LPFC_ATTR(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_param_init(name, defval, minval, maxval)
 
 #define LPFC_ATTR_R(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_param_show(name)\
 lpfc_param_init(name, defval, minval, maxval)\
@@ -1611,7 +1648,7 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO , lpfc_##name##_show, NULL)
 
 #define LPFC_ATTR_RW(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_param_show(name)\
 lpfc_param_init(name, defval, minval, maxval)\
@@ -1622,7 +1659,7 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO | S_IWUSR,\
 
 #define LPFC_ATTR_HEX_R(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_param_hex_show(name)\
 lpfc_param_init(name, defval, minval, maxval)\
@@ -1630,7 +1667,7 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO , lpfc_##name##_show, NULL)
 
 #define LPFC_ATTR_HEX_RW(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_param_hex_show(name)\
 lpfc_param_init(name, defval, minval, maxval)\
@@ -1641,13 +1678,13 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO | S_IWUSR,\
 
 #define LPFC_VPORT_ATTR(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_vport_param_init(name, defval, minval, maxval)
 
 #define LPFC_VPORT_ATTR_R(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_vport_param_show(name)\
 lpfc_vport_param_init(name, defval, minval, maxval)\
@@ -1655,7 +1692,7 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO , lpfc_##name##_show, NULL)
 
 #define LPFC_VPORT_ATTR_RW(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_vport_param_show(name)\
 lpfc_vport_param_init(name, defval, minval, maxval)\
@@ -1666,7 +1703,7 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO | S_IWUSR,\
 
 #define LPFC_VPORT_ATTR_HEX_R(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_vport_param_hex_show(name)\
 lpfc_vport_param_init(name, defval, minval, maxval)\
@@ -1674,7 +1711,7 @@ static DEVICE_ATTR(lpfc_##name, S_IRUGO , lpfc_##name##_show, NULL)
 
 #define LPFC_VPORT_ATTR_HEX_RW(name, defval, minval, maxval, desc) \
 static uint lpfc_##name = defval;\
-module_param(lpfc_##name, uint, 0);\
+module_param(lpfc_##name, uint, S_IRUGO);\
 MODULE_PARM_DESC(lpfc_##name, desc);\
 lpfc_vport_param_hex_show(name)\
 lpfc_vport_param_init(name, defval, minval, maxval)\
@@ -1718,7 +1755,7 @@ static DEVICE_ATTR(npiv_info, S_IRUGO, lpfc_npiv_info_show, NULL);
 static DEVICE_ATTR(lpfc_temp_sensor, S_IRUGO, lpfc_temp_sensor_show, NULL);
 static DEVICE_ATTR(lpfc_fips_level, S_IRUGO, lpfc_fips_level_show, NULL);
 static DEVICE_ATTR(lpfc_fips_rev, S_IRUGO, lpfc_fips_rev_show, NULL);
-
+static DEVICE_ATTR(lpfc_dss, S_IRUGO, lpfc_dss_show, NULL);
 
 static char *lpfc_soft_wwn_key = "C99G71SL8032A";
 
@@ -1813,6 +1850,7 @@ lpfc_soft_wwpn_store(struct device *dev, struct device_attribute *attr,
 	int stat1=0, stat2=0;
 	unsigned int i, j, cnt=count;
 	u8 wwpn[8];
+	int rc;
 
 	if (!phba->cfg_enable_hba_reset)
 		return -EACCES;
@@ -1863,7 +1901,11 @@ lpfc_soft_wwpn_store(struct device *dev, struct device_attribute *attr,
 				"0463 lpfc_soft_wwpn attribute set failed to "
 				"reinit adapter - %d\n", stat1);
 	init_completion(&online_compl);
-	lpfc_workq_post_event(phba, &stat2, &online_compl, LPFC_EVT_ONLINE);
+	rc = lpfc_workq_post_event(phba, &stat2, &online_compl,
+				   LPFC_EVT_ONLINE);
+	if (rc == 0)
+		return -ENOMEM;
+
 	wait_for_completion(&online_compl);
 	if (stat2)
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
@@ -1954,7 +1996,7 @@ static DEVICE_ATTR(lpfc_soft_wwnn, S_IRUGO | S_IWUSR,\
 
 
 static int lpfc_poll = 0;
-module_param(lpfc_poll, int, 0);
+module_param(lpfc_poll, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_poll, "FCP ring polling mode control:"
 		 " 0 - none,"
 		 " 1 - poll with interrupts enabled"
@@ -1964,21 +2006,21 @@ static DEVICE_ATTR(lpfc_poll, S_IRUGO | S_IWUSR,
 		   lpfc_poll_show, lpfc_poll_store);
 
 int  lpfc_sli_mode = 0;
-module_param(lpfc_sli_mode, int, 0);
+module_param(lpfc_sli_mode, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_sli_mode, "SLI mode selector:"
 		 " 0 - auto (SLI-3 if supported),"
 		 " 2 - select SLI-2 even on SLI-3 capable HBAs,"
 		 " 3 - select SLI-3");
 
 int lpfc_enable_npiv = 1;
-module_param(lpfc_enable_npiv, int, 0);
+module_param(lpfc_enable_npiv, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_enable_npiv, "Enable NPIV functionality");
 lpfc_param_show(enable_npiv);
 lpfc_param_init(enable_npiv, 1, 0, 1);
 static DEVICE_ATTR(lpfc_enable_npiv, S_IRUGO, lpfc_enable_npiv_show, NULL);
 
 int lpfc_enable_rrq;
-module_param(lpfc_enable_rrq, int, 0);
+module_param(lpfc_enable_rrq, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_enable_rrq, "Enable RRQ functionality");
 lpfc_param_show(enable_rrq);
 lpfc_param_init(enable_rrq, 0, 0, 1);
@@ -2040,7 +2082,7 @@ static DEVICE_ATTR(txcmplq_hw, S_IRUGO,
 			 lpfc_txcmplq_hw_show, NULL);
 
 int lpfc_iocb_cnt = 2;
-module_param(lpfc_iocb_cnt, int, 1);
+module_param(lpfc_iocb_cnt, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_iocb_cnt,
 	"Number of IOCBs alloc for ELS, CT, and ABTS: 1k to 5k IOCBs");
 lpfc_param_show(iocb_cnt);
@@ -2192,7 +2234,7 @@ static DEVICE_ATTR(lpfc_nodev_tmo, S_IRUGO | S_IWUSR,
 # disappear until the timer expires. Value range is [0,255]. Default
 # value is 30.
 */
-module_param(lpfc_devloss_tmo, int, 0);
+module_param(lpfc_devloss_tmo, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_devloss_tmo,
 		 "Seconds driver will hold I/O waiting "
 		 "for a device to come back");
@@ -2302,7 +2344,7 @@ LPFC_VPORT_ATTR_R(peer_port_login, 0, 0, 1,
 # Default value of this parameter is 1.
 */
 static int lpfc_restrict_login = 1;
-module_param(lpfc_restrict_login, int, 0);
+module_param(lpfc_restrict_login, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_restrict_login,
 		 "Restrict virtual ports login to remote initiators.");
 lpfc_vport_param_show(restrict_login);
@@ -2473,7 +2515,7 @@ lpfc_topology_store(struct device *dev, struct device_attribute *attr,
 	return -EINVAL;
 }
 static int lpfc_topology = 0;
-module_param(lpfc_topology, int, 0);
+module_param(lpfc_topology, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_topology, "Select Fibre Channel topology");
 lpfc_param_show(topology)
 lpfc_param_init(topology, 0, 0, 6)
@@ -2915,7 +2957,7 @@ lpfc_link_speed_store(struct device *dev, struct device_attribute *attr,
 }
 
 static int lpfc_link_speed = 0;
-module_param(lpfc_link_speed, int, 0);
+module_param(lpfc_link_speed, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_link_speed, "Select link speed");
 lpfc_param_show(link_speed)
 
@@ -3043,7 +3085,7 @@ lpfc_aer_support_store(struct device *dev, struct device_attribute *attr,
 }
 
 static int lpfc_aer_support = 1;
-module_param(lpfc_aer_support, int, 1);
+module_param(lpfc_aer_support, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_aer_support, "Enable PCIe device AER support");
 lpfc_param_show(aer_support)
 
@@ -3155,7 +3197,7 @@ LPFC_VPORT_ATTR_RW(use_adisc, 0, 0, 1,
 # The value is set in milliseconds.
 */
 static int lpfc_max_scsicmpl_time;
-module_param(lpfc_max_scsicmpl_time, int, 0);
+module_param(lpfc_max_scsicmpl_time, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_max_scsicmpl_time,
 	"Use command completion time to control queue depth");
 lpfc_vport_param_show(max_scsicmpl_time);
@@ -3331,7 +3373,7 @@ LPFC_ATTR_R(enable_bg, 0, 0, 1, "Enable BlockGuard Support");
 */
 unsigned int lpfc_prot_mask = SHOST_DIF_TYPE1_PROTECTION;
 
-module_param(lpfc_prot_mask, uint, 0);
+module_param(lpfc_prot_mask, uint, S_IRUGO);
 MODULE_PARM_DESC(lpfc_prot_mask, "host protection mask");
 
 /*
@@ -3343,9 +3385,28 @@ MODULE_PARM_DESC(lpfc_prot_mask, "host protection mask");
 #
 */
 unsigned char lpfc_prot_guard = SHOST_DIX_GUARD_IP;
-module_param(lpfc_prot_guard, byte, 0);
+module_param(lpfc_prot_guard, byte, S_IRUGO);
 MODULE_PARM_DESC(lpfc_prot_guard, "host protection guard type");
 
+/*
+ * Delay initial NPort discovery when Clean Address bit is cleared in
+ * FLOGI/FDISC accept and FCID/Fabric name/Fabric portname is changed.
+ * This parameter can have value 0 or 1.
+ * When this parameter is set to 0, no delay is added to the initial
+ * discovery.
+ * When this parameter is set to non-zero value, initial Nport discovery is
+ * delayed by ra_tov seconds when Clean Address bit is cleared in FLOGI/FDISC
+ * accept and FCID/Fabric name/Fabric portname is changed.
+ * Driver always delay Nport discovery for subsequent FLOGI/FDISC completion
+ * when Clean Address bit is cleared in FLOGI/FDISC
+ * accept and FCID/Fabric name/Fabric portname is changed.
+ * Default value is 0.
+ */
+int lpfc_delay_discovery;
+module_param(lpfc_delay_discovery, int, S_IRUGO);
+MODULE_PARM_DESC(lpfc_delay_discovery,
+	"Delay NPort discovery when Clean Address bit is cleared. "
+	"Allowed values: 0,1.");
 
 /*
  * lpfc_sg_seg_cnt - Initial Maximum DMA Segment Count
@@ -3437,6 +3498,7 @@ struct device_attribute *lpfc_hba_attrs[] = {
 	&dev_attr_txcmplq_hw,
 	&dev_attr_lpfc_fips_level,
 	&dev_attr_lpfc_fips_rev,
+	&dev_attr_lpfc_dss,
 	NULL,
 };
 
@@ -4639,6 +4701,7 @@ lpfc_get_cfgparam(struct lpfc_hba *phba)
 	lpfc_aer_support_init(phba, lpfc_aer_support);
 	lpfc_suppress_link_up_init(phba, lpfc_suppress_link_up);
 	lpfc_iocb_cnt_init(phba, lpfc_iocb_cnt);
+	phba->cfg_enable_dss = 1;
 	return;
 }
 
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 17fde522c84a..3d40023f4804 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -53,9 +53,9 @@ void lpfc_unreg_vpi(struct lpfc_hba *, uint16_t, LPFC_MBOXQ_t *);
 void lpfc_init_link(struct lpfc_hba *, LPFC_MBOXQ_t *, uint32_t, uint32_t);
 void lpfc_request_features(struct lpfc_hba *, struct lpfcMboxq *);
 void lpfc_supported_pages(struct lpfcMboxq *);
-void lpfc_sli4_params(struct lpfcMboxq *);
+void lpfc_pc_sli4_params(struct lpfcMboxq *);
 int lpfc_pc_sli4_params_get(struct lpfc_hba *, LPFC_MBOXQ_t *);
-
+int lpfc_get_sli4_parameters(struct lpfc_hba *, LPFC_MBOXQ_t *);
 struct lpfc_vport *lpfc_find_vport_by_did(struct lpfc_hba *, uint32_t);
 void lpfc_cleanup_rcv_buffers(struct lpfc_vport *);
 void lpfc_rcv_seq_check_edtov(struct lpfc_vport *);
@@ -167,6 +167,8 @@ 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 *);
+void lpfc_delayed_disc_tmo(unsigned long);
+void lpfc_delayed_disc_timeout_handler(struct lpfc_vport *);
 
 int lpfc_config_port_prep(struct lpfc_hba *);
 int lpfc_config_port_post(struct lpfc_hba *);
@@ -341,6 +343,7 @@ extern struct fc_function_template lpfc_transport_functions;
 extern struct fc_function_template lpfc_vport_transport_functions;
 extern int lpfc_sli_mode;
 extern int lpfc_enable_npiv;
+extern int lpfc_delay_discovery;
 
 int  lpfc_vport_symbolic_node_name(struct lpfc_vport *, char *, size_t);
 int  lpfc_vport_symbolic_port_name(struct lpfc_vport *, char *,	size_t);
@@ -423,6 +426,6 @@ int lpfc_send_rrq(struct lpfc_hba *, struct lpfc_node_rrq *);
 int lpfc_set_rrq_active(struct lpfc_hba *, struct lpfc_nodelist *,
 	uint16_t, uint16_t, uint16_t);
 void lpfc_cleanup_wt_rrqs(struct lpfc_hba *);
-void lpfc_cleanup_vports_rrqs(struct lpfc_vport *);
+void lpfc_cleanup_vports_rrqs(struct lpfc_vport *, struct lpfc_nodelist *);
 struct lpfc_node_rrq *lpfc_get_active_rrq(struct lpfc_vport *, uint16_t,
 	uint32_t);
diff --git a/drivers/scsi/lpfc/lpfc_ct.c b/drivers/scsi/lpfc/lpfc_ct.c
index c004fa9a681e..d9edfd90d7ff 100644
--- a/drivers/scsi/lpfc/lpfc_ct.c
+++ b/drivers/scsi/lpfc/lpfc_ct.c
@@ -1738,6 +1738,55 @@ fdmi_cmd_exit:
 	return 1;
 }
 
+/**
+ * lpfc_delayed_disc_tmo - Timeout handler for delayed discovery timer.
+ * @ptr - Context object of the timer.
+ *
+ * This function set the WORKER_DELAYED_DISC_TMO flag and wake up
+ * the worker thread.
+ **/
+void
+lpfc_delayed_disc_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_DELAYED_DISC_TMO;
+	if (!tmo_posted)
+		vport->work_port_events |= WORKER_DELAYED_DISC_TMO;
+	spin_unlock_irqrestore(&vport->work_port_lock, iflag);
+
+	if (!tmo_posted)
+		lpfc_worker_wake_up(phba);
+	return;
+}
+
+/**
+ * lpfc_delayed_disc_timeout_handler - Function called by worker thread to
+ *      handle delayed discovery.
+ * @vport: pointer to a host virtual N_Port data structure.
+ *
+ * This function start nport discovery of the vport.
+ **/
+void
+lpfc_delayed_disc_timeout_handler(struct lpfc_vport *vport)
+{
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
+
+	spin_lock_irq(shost->host_lock);
+	if (!(vport->fc_flag & FC_DISC_DELAYED)) {
+		spin_unlock_irq(shost->host_lock);
+		return;
+	}
+	vport->fc_flag &= ~FC_DISC_DELAYED;
+	spin_unlock_irq(shost->host_lock);
+
+	lpfc_do_scr_ns_plogi(vport->phba, vport);
+}
+
 void
 lpfc_fdmi_tmo(unsigned long ptr)
 {
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index a80d938fafc9..a753581509d6 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2007-2009 Emulex.  All rights reserved.           *
+ * Copyright (C) 2007-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  *                                                                 *
@@ -57,8 +57,8 @@
  * # mount -t debugfs none /sys/kernel/debug
  *
  * The lpfc debugfs directory hierarchy is:
- * lpfc/lpfcX/vportY
- * where X is the lpfc hba unique_id
+ * /sys/kernel/debug/lpfc/fnX/vportY
+ * where X is the lpfc hba function unique_id
  * where Y is the vport VPI on that hba
  *
  * Debugging services available per vport:
@@ -82,52 +82,34 @@
  *                               the HBA. X MUST also be a power of 2.
  */
 static int lpfc_debugfs_enable = 1;
-module_param(lpfc_debugfs_enable, int, 0);
+module_param(lpfc_debugfs_enable, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_debugfs_enable, "Enable debugfs services");
 
 /* This MUST be a power of 2 */
 static int lpfc_debugfs_max_disc_trc;
-module_param(lpfc_debugfs_max_disc_trc, int, 0);
+module_param(lpfc_debugfs_max_disc_trc, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_debugfs_max_disc_trc,
 	"Set debugfs discovery trace depth");
 
 /* This MUST be a power of 2 */
 static int lpfc_debugfs_max_slow_ring_trc;
-module_param(lpfc_debugfs_max_slow_ring_trc, int, 0);
+module_param(lpfc_debugfs_max_slow_ring_trc, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_debugfs_max_slow_ring_trc,
 	"Set debugfs slow ring trace depth");
 
 static int lpfc_debugfs_mask_disc_trc;
-module_param(lpfc_debugfs_mask_disc_trc, int, 0);
+module_param(lpfc_debugfs_mask_disc_trc, int, S_IRUGO);
 MODULE_PARM_DESC(lpfc_debugfs_mask_disc_trc,
 	"Set debugfs discovery trace mask");
 
 #include <linux/debugfs.h>
 
-/* size of output line, for discovery_trace and slow_ring_trace */
-#define LPFC_DEBUG_TRC_ENTRY_SIZE 100
-
-/* nodelist output buffer size */
-#define LPFC_NODELIST_SIZE 8192
-#define LPFC_NODELIST_ENTRY_SIZE 120
-
-/* dumpHBASlim output buffer size */
-#define LPFC_DUMPHBASLIM_SIZE 4096
-
-/* dumpHostSlim output buffer size */
-#define LPFC_DUMPHOSTSLIM_SIZE 4096
-
-/* hbqinfo output buffer size */
-#define LPFC_HBQINFO_SIZE 8192
-
-struct lpfc_debug {
-	char *buffer;
-	int  len;
-};
-
 static atomic_t lpfc_debugfs_seq_trc_cnt = ATOMIC_INIT(0);
 static unsigned long lpfc_debugfs_start_time = 0L;
 
+/* iDiag */
+static struct lpfc_idiag idiag;
+
 /**
  * lpfc_debugfs_disc_trc_data - Dump discovery logging to a buffer
  * @vport: The vport to gather the log info from.
@@ -996,8 +978,6 @@ lpfc_debugfs_dumpDataDif_write(struct file *file, const char __user *buf,
 	return nbytes;
 }
 
-
-
 /**
  * lpfc_debugfs_nodelist_open - Open the nodelist debugfs file
  * @inode: The inode pointer that contains a vport pointer.
@@ -1099,6 +1079,7 @@ lpfc_debugfs_read(struct file *file, char __user *buf,
 		  size_t nbytes, loff_t *ppos)
 {
 	struct lpfc_debug *debug = file->private_data;
+
 	return simple_read_from_buffer(buf, nbytes, ppos, debug->buffer,
 				       debug->len);
 }
@@ -1137,6 +1118,776 @@ lpfc_debugfs_dumpDataDif_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
+/*
+ * iDiag debugfs file access methods
+ */
+
+/*
+ * iDiag PCI config space register access methods:
+ *
+ * The PCI config space register accessees of read, write, read-modify-write
+ * for set bits, and read-modify-write for clear bits to SLI4 PCI functions
+ * are provided. In the proper SLI4 PCI function's debugfs iDiag directory,
+ *
+ *      /sys/kernel/debug/lpfc/fn<#>/iDiag
+ *
+ * the access is through the debugfs entry pciCfg:
+ *
+ * 1. For PCI config space register read access, there are two read methods:
+ *    A) read a single PCI config space register in the size of a byte
+ *    (8 bits), a word (16 bits), or a dword (32 bits); or B) browse through
+ *    the 4K extended PCI config space.
+ *
+ *    A) Read a single PCI config space register consists of two steps:
+ *
+ *    Step-1: Set up PCI config space register read command, the command
+ *    syntax is,
+ *
+ *        echo 1 <where> <count> > pciCfg
+ *
+ *    where, 1 is the iDiag command for PCI config space read, <where> is the
+ *    offset from the beginning of the device's PCI config space to read from,
+ *    and <count> is the size of PCI config space register data to read back,
+ *    it will be 1 for reading a byte (8 bits), 2 for reading a word (16 bits
+ *    or 2 bytes), or 4 for reading a dword (32 bits or 4 bytes).
+ *
+ *    Setp-2: Perform the debugfs read operation to execute the idiag command
+ *    set up in Step-1,
+ *
+ *        cat pciCfg
+ *
+ *    Examples:
+ *    To read PCI device's vendor-id and device-id from PCI config space,
+ *
+ *        echo 1 0 4 > pciCfg
+ *        cat pciCfg
+ *
+ *    To read PCI device's currnt command from config space,
+ *
+ *        echo 1 4 2 > pciCfg
+ *        cat pciCfg
+ *
+ *    B) Browse through the entire 4K extended PCI config space also consists
+ *    of two steps:
+ *
+ *    Step-1: Set up PCI config space register browsing command, the command
+ *    syntax is,
+ *
+ *        echo 1 0 4096 > pciCfg
+ *
+ *    where, 1 is the iDiag command for PCI config space read, 0 must be used
+ *    as the offset for PCI config space register browse, and 4096 must be
+ *    used as the count for PCI config space register browse.
+ *
+ *    Step-2: Repeately issue the debugfs read operation to browse through
+ *    the entire PCI config space registers:
+ *
+ *        cat pciCfg
+ *        cat pciCfg
+ *        cat pciCfg
+ *        ...
+ *
+ *    When browsing to the end of the 4K PCI config space, the browse method
+ *    shall wrap around to start reading from beginning again, and again...
+ *
+ * 2. For PCI config space register write access, it supports a single PCI
+ *    config space register write in the size of a byte (8 bits), a word
+ *    (16 bits), or a dword (32 bits). The command syntax is,
+ *
+ *        echo 2 <where> <count> <value> > pciCfg
+ *
+ *    where, 2 is the iDiag command for PCI config space write, <where> is
+ *    the offset from the beginning of the device's PCI config space to write
+ *    into, <count> is the size of data to write into the PCI config space,
+ *    it will be 1 for writing a byte (8 bits), 2 for writing a word (16 bits
+ *    or 2 bytes), or 4 for writing a dword (32 bits or 4 bytes), and <value>
+ *    is the data to be written into the PCI config space register at the
+ *    offset.
+ *
+ *    Examples:
+ *    To disable PCI device's interrupt assertion,
+ *
+ *    1) Read in device's PCI config space register command field <cmd>:
+ *
+ *           echo 1 4 2 > pciCfg
+ *           cat pciCfg
+ *
+ *    2) Set bit 10 (Interrupt Disable bit) in the <cmd>:
+ *
+ *           <cmd> = <cmd> | (1 < 10)
+ *
+ *    3) Write the modified command back:
+ *
+ *           echo 2 4 2 <cmd> > pciCfg
+ *
+ * 3. For PCI config space register set bits access, it supports a single PCI
+ *    config space register set bits in the size of a byte (8 bits), a word
+ *    (16 bits), or a dword (32 bits). The command syntax is,
+ *
+ *        echo 3 <where> <count> <bitmask> > pciCfg
+ *
+ *    where, 3 is the iDiag command for PCI config space set bits, <where> is
+ *    the offset from the beginning of the device's PCI config space to set
+ *    bits into, <count> is the size of the bitmask to set into the PCI config
+ *    space, it will be 1 for setting a byte (8 bits), 2 for setting a word
+ *    (16 bits or 2 bytes), or 4 for setting a dword (32 bits or 4 bytes), and
+ *    <bitmask> is the bitmask, indicating the bits to be set into the PCI
+ *    config space register at the offset. The logic performed to the content
+ *    of the PCI config space register, regval, is,
+ *
+ *        regval |= <bitmask>
+ *
+ * 4. For PCI config space register clear bits access, it supports a single
+ *    PCI config space register clear bits in the size of a byte (8 bits),
+ *    a word (16 bits), or a dword (32 bits). The command syntax is,
+ *
+ *        echo 4 <where> <count> <bitmask> > pciCfg
+ *
+ *    where, 4 is the iDiag command for PCI config space clear bits, <where>
+ *    is the offset from the beginning of the device's PCI config space to
+ *    clear bits from, <count> is the size of the bitmask to set into the PCI
+ *    config space, it will be 1 for setting a byte (8 bits), 2 for setting
+ *    a word(16 bits or 2 bytes), or 4 for setting a dword (32 bits or 4
+ *    bytes), and <bitmask> is the bitmask, indicating the bits to be cleared
+ *    from the PCI config space register at the offset. the logic performed
+ *    to the content of the PCI config space register, regval, is,
+ *
+ *        regval &= ~<bitmask>
+ *
+ * Note, for all single register read, write, set bits, or clear bits access,
+ * the offset (<where>) must be aligned with the size of the data:
+ *
+ * For data size of byte (8 bits), the offset must be aligned to the byte
+ * boundary; for data size of word (16 bits), the offset must be aligned
+ * to the word boundary; while for data size of dword (32 bits), the offset
+ * must be aligned to the dword boundary. Otherwise, the interface will
+ * return the error:
+ *
+ *     "-bash: echo: write error: Invalid argument".
+ *
+ * For example:
+ *
+ *     echo 1 2 4 > pciCfg
+ *     -bash: echo: write error: Invalid argument
+ *
+ * Note also, all of the numbers in the command fields for all read, write,
+ * set bits, and clear bits PCI config space register command fields can be
+ * either decimal or hex.
+ *
+ * For example,
+ *     echo 1 0 4096 > pciCfg
+ *
+ * will be the same as
+ *     echo 1 0 0x1000 > pciCfg
+ *
+ * And,
+ *     echo 2 155 1 10 > pciCfg
+ *
+ * will be
+ *     echo 2 0x9b 1 0xa > pciCfg
+ */
+
+/**
+ * lpfc_idiag_cmd_get - Get and parse idiag debugfs comands from user space
+ * @buf: The pointer to the user space buffer.
+ * @nbytes: The number of bytes in the user space buffer.
+ * @idiag_cmd: pointer to the idiag command struct.
+ *
+ * This routine reads data from debugfs user space buffer and parses the
+ * buffer for getting the idiag command and arguments. The while space in
+ * between the set of data is used as the parsing separator.
+ *
+ * This routine returns 0 when successful, it returns proper error code
+ * back to the user space in error conditions.
+ */
+static int lpfc_idiag_cmd_get(const char __user *buf, size_t nbytes,
+			      struct lpfc_idiag_cmd *idiag_cmd)
+{
+	char mybuf[64];
+	char *pbuf, *step_str;
+	int bsize, i;
+
+	/* Protect copy from user */
+	if (!access_ok(VERIFY_READ, buf, nbytes))
+		return -EFAULT;
+
+	memset(mybuf, 0, sizeof(mybuf));
+	memset(idiag_cmd, 0, sizeof(*idiag_cmd));
+	bsize = min(nbytes, (sizeof(mybuf)-1));
+
+	if (copy_from_user(mybuf, buf, bsize))
+		return -EFAULT;
+	pbuf = &mybuf[0];
+	step_str = strsep(&pbuf, "\t ");
+
+	/* The opcode must present */
+	if (!step_str)
+		return -EINVAL;
+
+	idiag_cmd->opcode = simple_strtol(step_str, NULL, 0);
+	if (idiag_cmd->opcode == 0)
+		return -EINVAL;
+
+	for (i = 0; i < LPFC_IDIAG_CMD_DATA_SIZE; i++) {
+		step_str = strsep(&pbuf, "\t ");
+		if (!step_str)
+			return 0;
+		idiag_cmd->data[i] = simple_strtol(step_str, NULL, 0);
+	}
+	return 0;
+}
+
+/**
+ * lpfc_idiag_open - idiag open debugfs
+ * @inode: The inode pointer that contains a pointer to phba.
+ * @file: The file pointer to attach the file operation.
+ *
+ * Description:
+ * This routine is the entry point for the debugfs open file operation. It
+ * gets the reference to phba from the i_private field in @inode, it then
+ * allocates buffer for the file operation, performs the necessary PCI config
+ * space read into the allocated buffer according to the idiag user command
+ * setup, and then returns a pointer to buffer in the private_data field in
+ * @file.
+ *
+ * Returns:
+ * This function returns zero if successful. On error it will return an
+ * negative error value.
+ **/
+static int
+lpfc_idiag_open(struct inode *inode, struct file *file)
+{
+	struct lpfc_debug *debug;
+
+	debug = kmalloc(sizeof(*debug), GFP_KERNEL);
+	if (!debug)
+		return -ENOMEM;
+
+	debug->i_private = inode->i_private;
+	debug->buffer = NULL;
+	file->private_data = debug;
+
+	return 0;
+}
+
+/**
+ * lpfc_idiag_release - Release idiag access file operation
+ * @inode: The inode pointer that contains a vport pointer. (unused)
+ * @file: The file pointer that contains the buffer to release.
+ *
+ * Description:
+ * This routine is the generic release routine for the idiag access file
+ * operation, it frees the buffer that was allocated when the debugfs file
+ * was opened.
+ *
+ * Returns:
+ * This function returns zero.
+ **/
+static int
+lpfc_idiag_release(struct inode *inode, struct file *file)
+{
+	struct lpfc_debug *debug = file->private_data;
+
+	/* Free the buffers to the file operation */
+	kfree(debug->buffer);
+	kfree(debug);
+
+	return 0;
+}
+
+/**
+ * lpfc_idiag_cmd_release - Release idiag cmd access file operation
+ * @inode: The inode pointer that contains a vport pointer. (unused)
+ * @file: The file pointer that contains the buffer to release.
+ *
+ * Description:
+ * This routine frees the buffer that was allocated when the debugfs file
+ * was opened. It also reset the fields in the idiag command struct in the
+ * case the command is not continuous browsing of the data structure.
+ *
+ * Returns:
+ * This function returns zero.
+ **/
+static int
+lpfc_idiag_cmd_release(struct inode *inode, struct file *file)
+{
+	struct lpfc_debug *debug = file->private_data;
+
+	/* Read PCI config register, if not read all, clear command fields */
+	if ((debug->op == LPFC_IDIAG_OP_RD) &&
+	    (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_RD))
+		if ((idiag.cmd.data[1] == sizeof(uint8_t)) ||
+		    (idiag.cmd.data[1] == sizeof(uint16_t)) ||
+		    (idiag.cmd.data[1] == sizeof(uint32_t)))
+			memset(&idiag, 0, sizeof(idiag));
+
+	/* Write PCI config register, clear command fields */
+	if ((debug->op == LPFC_IDIAG_OP_WR) &&
+	    (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_WR))
+		memset(&idiag, 0, sizeof(idiag));
+
+	/* Free the buffers to the file operation */
+	kfree(debug->buffer);
+	kfree(debug);
+
+	return 0;
+}
+
+/**
+ * lpfc_idiag_pcicfg_read - idiag debugfs read pcicfg
+ * @file: The file pointer to read from.
+ * @buf: The buffer to copy the data to.
+ * @nbytes: The number of bytes to read.
+ * @ppos: The position in the file to start reading from.
+ *
+ * Description:
+ * This routine reads data from the @phba pci config space according to the
+ * idiag command, and copies to user @buf. Depending on the PCI config space
+ * read command setup, it does either a single register read of a byte
+ * (8 bits), a word (16 bits), or a dword (32 bits) or browsing through all
+ * registers from the 4K extended PCI config space.
+ *
+ * Returns:
+ * This function returns the amount of data that was read (this could be less
+ * than @nbytes if the end of the file was reached) or a negative error value.
+ **/
+static ssize_t
+lpfc_idiag_pcicfg_read(struct file *file, char __user *buf, size_t nbytes,
+		       loff_t *ppos)
+{
+	struct lpfc_debug *debug = file->private_data;
+	struct lpfc_hba *phba = (struct lpfc_hba *)debug->i_private;
+	int offset_label, offset, len = 0, index = LPFC_PCI_CFG_RD_SIZE;
+	int where, count;
+	char *pbuffer;
+	struct pci_dev *pdev;
+	uint32_t u32val;
+	uint16_t u16val;
+	uint8_t u8val;
+
+	pdev = phba->pcidev;
+	if (!pdev)
+		return 0;
+
+	/* This is a user read operation */
+	debug->op = LPFC_IDIAG_OP_RD;
+
+	if (!debug->buffer)
+		debug->buffer = kmalloc(LPFC_PCI_CFG_SIZE, GFP_KERNEL);
+	if (!debug->buffer)
+		return 0;
+	pbuffer = debug->buffer;
+
+	if (*ppos)
+		return 0;
+
+	if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_RD) {
+		where = idiag.cmd.data[0];
+		count = idiag.cmd.data[1];
+	} else
+		return 0;
+
+	/* Read single PCI config space register */
+	switch (count) {
+	case SIZE_U8: /* byte (8 bits) */
+		pci_read_config_byte(pdev, where, &u8val);
+		len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+				"%03x: %02x\n", where, u8val);
+		break;
+	case SIZE_U16: /* word (16 bits) */
+		pci_read_config_word(pdev, where, &u16val);
+		len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+				"%03x: %04x\n", where, u16val);
+		break;
+	case SIZE_U32: /* double word (32 bits) */
+		pci_read_config_dword(pdev, where, &u32val);
+		len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+				"%03x: %08x\n", where, u32val);
+		break;
+	case LPFC_PCI_CFG_SIZE: /* browse all */
+		goto pcicfg_browse;
+		break;
+	default:
+		/* illegal count */
+		len = 0;
+		break;
+	}
+	return simple_read_from_buffer(buf, nbytes, ppos, pbuffer, len);
+
+pcicfg_browse:
+
+	/* Browse all PCI config space registers */
+	offset_label = idiag.offset.last_rd;
+	offset = offset_label;
+
+	/* Read PCI config space */
+	len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+			"%03x: ", offset_label);
+	while (index > 0) {
+		pci_read_config_dword(pdev, offset, &u32val);
+		len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+				"%08x ", u32val);
+		offset += sizeof(uint32_t);
+		index -= sizeof(uint32_t);
+		if (!index)
+			len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+					"\n");
+		else if (!(index % (8 * sizeof(uint32_t)))) {
+			offset_label += (8 * sizeof(uint32_t));
+			len += snprintf(pbuffer+len, LPFC_PCI_CFG_SIZE-len,
+					"\n%03x: ", offset_label);
+		}
+	}
+
+	/* Set up the offset for next portion of pci cfg read */
+	idiag.offset.last_rd += LPFC_PCI_CFG_RD_SIZE;
+	if (idiag.offset.last_rd >= LPFC_PCI_CFG_SIZE)
+		idiag.offset.last_rd = 0;
+
+	return simple_read_from_buffer(buf, nbytes, ppos, pbuffer, len);
+}
+
+/**
+ * lpfc_idiag_pcicfg_write - Syntax check and set up idiag pcicfg commands
+ * @file: The file pointer to read from.
+ * @buf: The buffer to copy the user data from.
+ * @nbytes: The number of bytes to get.
+ * @ppos: The position in the file to start reading from.
+ *
+ * This routine get the debugfs idiag command struct from user space and
+ * then perform the syntax check for PCI config space read or write command
+ * accordingly. In the case of PCI config space read command, it sets up
+ * the command in the idiag command struct for the debugfs read operation.
+ * In the case of PCI config space write operation, it executes the write
+ * operation into the PCI config space accordingly.
+ *
+ * It returns the @nbytges passing in from debugfs user space when successful.
+ * In case of error conditions, it returns proper error code back to the user
+ * space.
+ */
+static ssize_t
+lpfc_idiag_pcicfg_write(struct file *file, const char __user *buf,
+			size_t nbytes, loff_t *ppos)
+{
+	struct lpfc_debug *debug = file->private_data;
+	struct lpfc_hba *phba = (struct lpfc_hba *)debug->i_private;
+	uint32_t where, value, count;
+	uint32_t u32val;
+	uint16_t u16val;
+	uint8_t u8val;
+	struct pci_dev *pdev;
+	int rc;
+
+	pdev = phba->pcidev;
+	if (!pdev)
+		return -EFAULT;
+
+	/* This is a user write operation */
+	debug->op = LPFC_IDIAG_OP_WR;
+
+	rc = lpfc_idiag_cmd_get(buf, nbytes, &idiag.cmd);
+	if (rc)
+		return rc;
+
+	if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_RD) {
+		/* Read command from PCI config space, set up command fields */
+		where = idiag.cmd.data[0];
+		count = idiag.cmd.data[1];
+		if (count == LPFC_PCI_CFG_SIZE) {
+			if (where != 0)
+				goto error_out;
+		} else if ((count != sizeof(uint8_t)) &&
+			   (count != sizeof(uint16_t)) &&
+			   (count != sizeof(uint32_t)))
+			goto error_out;
+		if (count == sizeof(uint8_t)) {
+			if (where > LPFC_PCI_CFG_SIZE - sizeof(uint8_t))
+				goto error_out;
+			if (where % sizeof(uint8_t))
+				goto error_out;
+		}
+		if (count == sizeof(uint16_t)) {
+			if (where > LPFC_PCI_CFG_SIZE - sizeof(uint16_t))
+				goto error_out;
+			if (where % sizeof(uint16_t))
+				goto error_out;
+		}
+		if (count == sizeof(uint32_t)) {
+			if (where > LPFC_PCI_CFG_SIZE - sizeof(uint32_t))
+				goto error_out;
+			if (where % sizeof(uint32_t))
+				goto error_out;
+		}
+	} else if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_WR ||
+		   idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_ST ||
+		   idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_CL) {
+		/* Write command to PCI config space, read-modify-write */
+		where = idiag.cmd.data[0];
+		count = idiag.cmd.data[1];
+		value = idiag.cmd.data[2];
+		/* Sanity checks */
+		if ((count != sizeof(uint8_t)) &&
+		    (count != sizeof(uint16_t)) &&
+		    (count != sizeof(uint32_t)))
+			goto error_out;
+		if (count == sizeof(uint8_t)) {
+			if (where > LPFC_PCI_CFG_SIZE - sizeof(uint8_t))
+				goto error_out;
+			if (where % sizeof(uint8_t))
+				goto error_out;
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_WR)
+				pci_write_config_byte(pdev, where,
+						      (uint8_t)value);
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_ST) {
+				rc = pci_read_config_byte(pdev, where, &u8val);
+				if (!rc) {
+					u8val |= (uint8_t)value;
+					pci_write_config_byte(pdev, where,
+							      u8val);
+				}
+			}
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_CL) {
+				rc = pci_read_config_byte(pdev, where, &u8val);
+				if (!rc) {
+					u8val &= (uint8_t)(~value);
+					pci_write_config_byte(pdev, where,
+							      u8val);
+				}
+			}
+		}
+		if (count == sizeof(uint16_t)) {
+			if (where > LPFC_PCI_CFG_SIZE - sizeof(uint16_t))
+				goto error_out;
+			if (where % sizeof(uint16_t))
+				goto error_out;
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_WR)
+				pci_write_config_word(pdev, where,
+						      (uint16_t)value);
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_ST) {
+				rc = pci_read_config_word(pdev, where, &u16val);
+				if (!rc) {
+					u16val |= (uint16_t)value;
+					pci_write_config_word(pdev, where,
+							      u16val);
+				}
+			}
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_CL) {
+				rc = pci_read_config_word(pdev, where, &u16val);
+				if (!rc) {
+					u16val &= (uint16_t)(~value);
+					pci_write_config_word(pdev, where,
+							      u16val);
+				}
+			}
+		}
+		if (count == sizeof(uint32_t)) {
+			if (where > LPFC_PCI_CFG_SIZE - sizeof(uint32_t))
+				goto error_out;
+			if (where % sizeof(uint32_t))
+				goto error_out;
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_WR)
+				pci_write_config_dword(pdev, where, value);
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_ST) {
+				rc = pci_read_config_dword(pdev, where,
+							   &u32val);
+				if (!rc) {
+					u32val |= value;
+					pci_write_config_dword(pdev, where,
+							       u32val);
+				}
+			}
+			if (idiag.cmd.opcode == LPFC_IDIAG_CMD_PCICFG_CL) {
+				rc = pci_read_config_dword(pdev, where,
+							   &u32val);
+				if (!rc) {
+					u32val &= ~value;
+					pci_write_config_dword(pdev, where,
+							       u32val);
+				}
+			}
+		}
+	} else
+		/* All other opecodes are illegal for now */
+		goto error_out;
+
+	return nbytes;
+error_out:
+	memset(&idiag, 0, sizeof(idiag));
+	return -EINVAL;
+}
+
+/**
+ * lpfc_idiag_queinfo_read - idiag debugfs read queue information
+ * @file: The file pointer to read from.
+ * @buf: The buffer to copy the data to.
+ * @nbytes: The number of bytes to read.
+ * @ppos: The position in the file to start reading from.
+ *
+ * Description:
+ * This routine reads data from the @phba SLI4 PCI function queue information,
+ * and copies to user @buf.
+ *
+ * Returns:
+ * This function returns the amount of data that was read (this could be less
+ * than @nbytes if the end of the file was reached) or a negative error value.
+ **/
+static ssize_t
+lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
+			loff_t *ppos)
+{
+	struct lpfc_debug *debug = file->private_data;
+	struct lpfc_hba *phba = (struct lpfc_hba *)debug->i_private;
+	int len = 0, fcp_qidx;
+	char *pbuffer;
+
+	if (!debug->buffer)
+		debug->buffer = kmalloc(LPFC_QUE_INFO_GET_BUF_SIZE, GFP_KERNEL);
+	if (!debug->buffer)
+		return 0;
+	pbuffer = debug->buffer;
+
+	if (*ppos)
+		return 0;
+
+	/* Get slow-path event queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Slow-path EQ information:\n");
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], EQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n\n",
+			phba->sli4_hba.sp_eq->queue_id,
+			phba->sli4_hba.sp_eq->entry_count,
+			phba->sli4_hba.sp_eq->host_index,
+			phba->sli4_hba.sp_eq->hba_index);
+
+	/* Get fast-path event queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Fast-path EQ information:\n");
+	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count; fcp_qidx++) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+				"\tID [%02d], EQE-COUNT [%04d], "
+				"HOST-INDEX [%04x], PORT-INDEX [%04x]\n",
+				phba->sli4_hba.fp_eq[fcp_qidx]->queue_id,
+				phba->sli4_hba.fp_eq[fcp_qidx]->entry_count,
+				phba->sli4_hba.fp_eq[fcp_qidx]->host_index,
+				phba->sli4_hba.fp_eq[fcp_qidx]->hba_index);
+	}
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
+
+	/* Get mailbox complete queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Mailbox CQ information:\n");
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\t\tAssociated EQ-ID [%02d]:\n",
+			phba->sli4_hba.mbx_cq->assoc_qid);
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], CQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n\n",
+			phba->sli4_hba.mbx_cq->queue_id,
+			phba->sli4_hba.mbx_cq->entry_count,
+			phba->sli4_hba.mbx_cq->host_index,
+			phba->sli4_hba.mbx_cq->hba_index);
+
+	/* Get slow-path complete queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Slow-path CQ information:\n");
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\t\tAssociated EQ-ID [%02d]:\n",
+			phba->sli4_hba.els_cq->assoc_qid);
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], CQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n\n",
+			phba->sli4_hba.els_cq->queue_id,
+			phba->sli4_hba.els_cq->entry_count,
+			phba->sli4_hba.els_cq->host_index,
+			phba->sli4_hba.els_cq->hba_index);
+
+	/* Get fast-path complete queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Fast-path CQ information:\n");
+	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count; fcp_qidx++) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+				"\t\tAssociated EQ-ID [%02d]:\n",
+				phba->sli4_hba.fcp_cq[fcp_qidx]->assoc_qid);
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		"\tID [%02d], EQE-COUNT [%04d], "
+		"HOST-INDEX [%04x], PORT-INDEX [%04x]\n",
+		phba->sli4_hba.fcp_cq[fcp_qidx]->queue_id,
+		phba->sli4_hba.fcp_cq[fcp_qidx]->entry_count,
+		phba->sli4_hba.fcp_cq[fcp_qidx]->host_index,
+		phba->sli4_hba.fcp_cq[fcp_qidx]->hba_index);
+	}
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
+
+	/* Get mailbox queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Mailbox MQ information:\n");
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\t\tAssociated CQ-ID [%02d]:\n",
+			phba->sli4_hba.mbx_wq->assoc_qid);
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], MQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n\n",
+			phba->sli4_hba.mbx_wq->queue_id,
+			phba->sli4_hba.mbx_wq->entry_count,
+			phba->sli4_hba.mbx_wq->host_index,
+			phba->sli4_hba.mbx_wq->hba_index);
+
+	/* Get slow-path work queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Slow-path WQ information:\n");
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\t\tAssociated CQ-ID [%02d]:\n",
+			phba->sli4_hba.els_wq->assoc_qid);
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], WQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n\n",
+			phba->sli4_hba.els_wq->queue_id,
+			phba->sli4_hba.els_wq->entry_count,
+			phba->sli4_hba.els_wq->host_index,
+			phba->sli4_hba.els_wq->hba_index);
+
+	/* Get fast-path work queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Fast-path WQ information:\n");
+	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_wq_count; fcp_qidx++) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+				"\t\tAssociated CQ-ID [%02d]:\n",
+				phba->sli4_hba.fcp_wq[fcp_qidx]->assoc_qid);
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+				"\tID [%02d], WQE-COUNT [%04d], "
+				"HOST-INDEX [%04x], PORT-INDEX [%04x]\n",
+				phba->sli4_hba.fcp_wq[fcp_qidx]->queue_id,
+				phba->sli4_hba.fcp_wq[fcp_qidx]->entry_count,
+				phba->sli4_hba.fcp_wq[fcp_qidx]->host_index,
+				phba->sli4_hba.fcp_wq[fcp_qidx]->hba_index);
+	}
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
+
+	/* Get receive queue information */
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"Slow-path RQ information:\n");
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\t\tAssociated CQ-ID [%02d]:\n",
+			phba->sli4_hba.hdr_rq->assoc_qid);
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], RHQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n",
+			phba->sli4_hba.hdr_rq->queue_id,
+			phba->sli4_hba.hdr_rq->entry_count,
+			phba->sli4_hba.hdr_rq->host_index,
+			phba->sli4_hba.hdr_rq->hba_index);
+	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			"\tID [%02d], RDQE-COUNT [%04d], "
+			"HOST-INDEX [%04x], PORT-INDEX [%04x]\n",
+			phba->sli4_hba.dat_rq->queue_id,
+			phba->sli4_hba.dat_rq->entry_count,
+			phba->sli4_hba.dat_rq->host_index,
+			phba->sli4_hba.dat_rq->hba_index);
+
+	return simple_read_from_buffer(buf, nbytes, ppos, pbuffer, len);
+}
+
 #undef lpfc_debugfs_op_disc_trc
 static const struct file_operations lpfc_debugfs_op_disc_trc = {
 	.owner =        THIS_MODULE,
@@ -1213,6 +1964,28 @@ static const struct file_operations lpfc_debugfs_op_slow_ring_trc = {
 
 static struct dentry *lpfc_debugfs_root = NULL;
 static atomic_t lpfc_debugfs_hba_count;
+
+/*
+ * File operations for the iDiag debugfs
+ */
+#undef lpfc_idiag_op_pciCfg
+static const struct file_operations lpfc_idiag_op_pciCfg = {
+	.owner =        THIS_MODULE,
+	.open =         lpfc_idiag_open,
+	.llseek =       lpfc_debugfs_lseek,
+	.read =         lpfc_idiag_pcicfg_read,
+	.write =        lpfc_idiag_pcicfg_write,
+	.release =      lpfc_idiag_cmd_release,
+};
+
+#undef lpfc_idiag_op_queInfo
+static const struct file_operations lpfc_idiag_op_queInfo = {
+	.owner =        THIS_MODULE,
+	.open =         lpfc_idiag_open,
+	.read =         lpfc_idiag_queinfo_read,
+	.release =      lpfc_idiag_release,
+};
+
 #endif
 
 /**
@@ -1249,8 +2022,8 @@ lpfc_debugfs_initialize(struct lpfc_vport *vport)
 	if (!lpfc_debugfs_start_time)
 		lpfc_debugfs_start_time = jiffies;
 
-	/* Setup lpfcX directory for specific HBA */
-	snprintf(name, sizeof(name), "lpfc%d", phba->brd_no);
+	/* Setup funcX directory for specific HBA PCI function */
+	snprintf(name, sizeof(name), "fn%d", phba->brd_no);
 	if (!phba->hba_debugfs_root) {
 		phba->hba_debugfs_root =
 			debugfs_create_dir(name, lpfc_debugfs_root);
@@ -1275,28 +2048,38 @@ lpfc_debugfs_initialize(struct lpfc_vport *vport)
 		}
 
 		/* Setup dumpHBASlim */
-		snprintf(name, sizeof(name), "dumpHBASlim");
-		phba->debug_dumpHBASlim =
-			debugfs_create_file(name, S_IFREG|S_IRUGO|S_IWUSR,
-				 phba->hba_debugfs_root,
-				 phba, &lpfc_debugfs_op_dumpHBASlim);
-		if (!phba->debug_dumpHBASlim) {
-			lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
-				"0413 Cannot create debugfs dumpHBASlim\n");
-			goto debug_failed;
-		}
+		if (phba->sli_rev < LPFC_SLI_REV4) {
+			snprintf(name, sizeof(name), "dumpHBASlim");
+			phba->debug_dumpHBASlim =
+				debugfs_create_file(name,
+					S_IFREG|S_IRUGO|S_IWUSR,
+					phba->hba_debugfs_root,
+					phba, &lpfc_debugfs_op_dumpHBASlim);
+			if (!phba->debug_dumpHBASlim) {
+				lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+						 "0413 Cannot create debugfs "
+						"dumpHBASlim\n");
+				goto debug_failed;
+			}
+		} else
+			phba->debug_dumpHBASlim = NULL;
 
 		/* Setup dumpHostSlim */
-		snprintf(name, sizeof(name), "dumpHostSlim");
-		phba->debug_dumpHostSlim =
-			debugfs_create_file(name, S_IFREG|S_IRUGO|S_IWUSR,
-				 phba->hba_debugfs_root,
-				 phba, &lpfc_debugfs_op_dumpHostSlim);
-		if (!phba->debug_dumpHostSlim) {
-			lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
-				"0414 Cannot create debugfs dumpHostSlim\n");
-			goto debug_failed;
-		}
+		if (phba->sli_rev < LPFC_SLI_REV4) {
+			snprintf(name, sizeof(name), "dumpHostSlim");
+			phba->debug_dumpHostSlim =
+				debugfs_create_file(name,
+					S_IFREG|S_IRUGO|S_IWUSR,
+					phba->hba_debugfs_root,
+					phba, &lpfc_debugfs_op_dumpHostSlim);
+			if (!phba->debug_dumpHostSlim) {
+				lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+						 "0414 Cannot create debugfs "
+						 "dumpHostSlim\n");
+				goto debug_failed;
+			}
+		} else
+			phba->debug_dumpHBASlim = NULL;
 
 		/* Setup dumpData */
 		snprintf(name, sizeof(name), "dumpData");
@@ -1322,8 +2105,6 @@ lpfc_debugfs_initialize(struct lpfc_vport *vport)
 			goto debug_failed;
 		}
 
-
-
 		/* Setup slow ring trace */
 		if (lpfc_debugfs_max_slow_ring_trc) {
 			num = lpfc_debugfs_max_slow_ring_trc - 1;
@@ -1342,7 +2123,6 @@ lpfc_debugfs_initialize(struct lpfc_vport *vport)
 			}
 		}
 
-
 		snprintf(name, sizeof(name), "slow_ring_trace");
 		phba->debug_slow_ring_trc =
 			debugfs_create_file(name, S_IFREG|S_IRUGO|S_IWUSR,
@@ -1434,6 +2214,53 @@ lpfc_debugfs_initialize(struct lpfc_vport *vport)
 				 "0409 Cant create debugfs nodelist\n");
 		goto debug_failed;
 	}
+
+	/*
+	 * iDiag debugfs root entry points for SLI4 device only
+	 */
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		goto debug_failed;
+
+	snprintf(name, sizeof(name), "iDiag");
+	if (!phba->idiag_root) {
+		phba->idiag_root =
+			debugfs_create_dir(name, phba->hba_debugfs_root);
+		if (!phba->idiag_root) {
+			lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+					 "2922 Can't create idiag debugfs\n");
+			goto debug_failed;
+		}
+		/* Initialize iDiag data structure */
+		memset(&idiag, 0, sizeof(idiag));
+	}
+
+	/* iDiag read PCI config space */
+	snprintf(name, sizeof(name), "pciCfg");
+	if (!phba->idiag_pci_cfg) {
+		phba->idiag_pci_cfg =
+			debugfs_create_file(name, S_IFREG|S_IRUGO|S_IWUSR,
+				phba->idiag_root, phba, &lpfc_idiag_op_pciCfg);
+		if (!phba->idiag_pci_cfg) {
+			lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+					 "2923 Can't create idiag debugfs\n");
+			goto debug_failed;
+		}
+		idiag.offset.last_rd = 0;
+	}
+
+	/* iDiag get PCI function queue information */
+	snprintf(name, sizeof(name), "queInfo");
+	if (!phba->idiag_que_info) {
+		phba->idiag_que_info =
+			debugfs_create_file(name, S_IFREG|S_IRUGO,
+			phba->idiag_root, phba, &lpfc_idiag_op_queInfo);
+		if (!phba->idiag_que_info) {
+			lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+					 "2924 Can't create idiag debugfs\n");
+			goto debug_failed;
+		}
+	}
+
 debug_failed:
 	return;
 #endif
@@ -1508,8 +2335,31 @@ lpfc_debugfs_terminate(struct lpfc_vport *vport)
 			phba->debug_slow_ring_trc = NULL;
 		}
 
+		/*
+		 * iDiag release
+		 */
+		if (phba->sli_rev == LPFC_SLI_REV4) {
+			if (phba->idiag_que_info) {
+				/* iDiag queInfo */
+				debugfs_remove(phba->idiag_que_info);
+				phba->idiag_que_info = NULL;
+			}
+			if (phba->idiag_pci_cfg) {
+				/* iDiag pciCfg */
+				debugfs_remove(phba->idiag_pci_cfg);
+				phba->idiag_pci_cfg = NULL;
+			}
+
+			/* Finally remove the iDiag debugfs root */
+			if (phba->idiag_root) {
+				/* iDiag root */
+				debugfs_remove(phba->idiag_root);
+				phba->idiag_root = NULL;
+			}
+		}
+
 		if (phba->hba_debugfs_root) {
-			debugfs_remove(phba->hba_debugfs_root); /* lpfcX */
+			debugfs_remove(phba->hba_debugfs_root); /* fnX */
 			phba->hba_debugfs_root = NULL;
 			atomic_dec(&lpfc_debugfs_hba_count);
 		}
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.h b/drivers/scsi/lpfc/lpfc_debugfs.h
index 03c7313a1012..91b9a9427cda 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.h
+++ b/drivers/scsi/lpfc/lpfc_debugfs.h
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2007 Emulex.  All rights reserved.                *
+ * Copyright (C) 2007-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  *                                                                 *
@@ -22,6 +22,44 @@
 #define _H_LPFC_DEBUG_FS
 
 #ifdef CONFIG_SCSI_LPFC_DEBUG_FS
+
+/* size of output line, for discovery_trace and slow_ring_trace */
+#define LPFC_DEBUG_TRC_ENTRY_SIZE 100
+
+/* nodelist output buffer size */
+#define LPFC_NODELIST_SIZE 8192
+#define LPFC_NODELIST_ENTRY_SIZE 120
+
+/* dumpHBASlim output buffer size */
+#define LPFC_DUMPHBASLIM_SIZE 4096
+
+/* dumpHostSlim output buffer size */
+#define LPFC_DUMPHOSTSLIM_SIZE 4096
+
+/* hbqinfo output buffer size */
+#define LPFC_HBQINFO_SIZE 8192
+
+/* rdPciConf output buffer size */
+#define LPFC_PCI_CFG_SIZE 4096
+#define LPFC_PCI_CFG_RD_BUF_SIZE (LPFC_PCI_CFG_SIZE/2)
+#define LPFC_PCI_CFG_RD_SIZE (LPFC_PCI_CFG_SIZE/4)
+
+/* queue info output buffer size */
+#define LPFC_QUE_INFO_GET_BUF_SIZE 2048
+
+#define SIZE_U8  sizeof(uint8_t)
+#define SIZE_U16 sizeof(uint16_t)
+#define SIZE_U32 sizeof(uint32_t)
+
+struct lpfc_debug {
+	char *i_private;
+	char op;
+#define LPFC_IDIAG_OP_RD 1
+#define LPFC_IDIAG_OP_WR 2
+	char *buffer;
+	int  len;
+};
+
 struct lpfc_debugfs_trc {
 	char *fmt;
 	uint32_t data1;
@@ -30,6 +68,26 @@ struct lpfc_debugfs_trc {
 	uint32_t seq_cnt;
 	unsigned long jif;
 };
+
+struct lpfc_idiag_offset {
+	uint32_t last_rd;
+};
+
+#define LPFC_IDIAG_CMD_DATA_SIZE 4
+struct lpfc_idiag_cmd {
+	uint32_t opcode;
+#define LPFC_IDIAG_CMD_PCICFG_RD 0x00000001
+#define LPFC_IDIAG_CMD_PCICFG_WR 0x00000002
+#define LPFC_IDIAG_CMD_PCICFG_ST 0x00000003
+#define LPFC_IDIAG_CMD_PCICFG_CL 0x00000004
+	uint32_t data[LPFC_IDIAG_CMD_DATA_SIZE];
+};
+
+struct lpfc_idiag {
+	uint32_t active;
+	struct lpfc_idiag_cmd cmd;
+	struct lpfc_idiag_offset offset;
+};
 #endif
 
 /* Mask for discovery_trace */
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index c62d567cc845..8e28edf9801e 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2004-2009 Emulex.  All rights reserved.           *
+ * Copyright (C) 2004-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  * Portions Copyright (C) 2004-2005 Christoph Hellwig              *
@@ -485,6 +485,59 @@ fail:
 }
 
 /**
+ * lpfc_check_clean_addr_bit - Check whether assigned FCID is clean.
+ * @vport: pointer to a host virtual N_Port data structure.
+ * @sp: pointer to service parameter data structure.
+ *
+ * This routine is called from FLOGI/FDISC completion handler functions.
+ * lpfc_check_clean_addr_bit return 1 when FCID/Fabric portname/ Fabric
+ * node nodename is changed in the completion service parameter else return
+ * 0. This function also set flag in the vport data structure to delay
+ * NP_Port discovery after the FLOGI/FDISC completion if Clean address bit
+ * in FLOGI/FDISC response is cleared and FCID/Fabric portname/ Fabric
+ * node nodename is changed in the completion service parameter.
+ *
+ * Return code
+ *   0 - FCID and Fabric Nodename and Fabric portname is not changed.
+ *   1 - FCID or Fabric Nodename or Fabric portname is changed.
+ *
+ **/
+static uint8_t
+lpfc_check_clean_addr_bit(struct lpfc_vport *vport,
+		struct serv_parm *sp)
+{
+	uint8_t fabric_param_changed = 0;
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
+
+	if ((vport->fc_prevDID != vport->fc_myDID) ||
+		memcmp(&vport->fabric_portname, &sp->portName,
+			sizeof(struct lpfc_name)) ||
+		memcmp(&vport->fabric_nodename, &sp->nodeName,
+			sizeof(struct lpfc_name)))
+		fabric_param_changed = 1;
+
+	/*
+	 * Word 1 Bit 31 in common service parameter is overloaded.
+	 * Word 1 Bit 31 in FLOGI request is multiple NPort request
+	 * Word 1 Bit 31 in FLOGI response is clean address bit
+	 *
+	 * If fabric parameter is changed and clean address bit is
+	 * cleared delay nport discovery if
+	 * - vport->fc_prevDID != 0 (not initial discovery) OR
+	 * - lpfc_delay_discovery module parameter is set.
+	 */
+	if (fabric_param_changed && !sp->cmn.clean_address_bit &&
+	    (vport->fc_prevDID || lpfc_delay_discovery)) {
+		spin_lock_irq(shost->host_lock);
+		vport->fc_flag |= FC_DISC_DELAYED;
+		spin_unlock_irq(shost->host_lock);
+	}
+
+	return fabric_param_changed;
+}
+
+
+/**
  * lpfc_cmpl_els_flogi_fabric - Completion function for flogi to a fabric port
  * @vport: pointer to a host virtual N_Port data structure.
  * @ndlp: pointer to a node-list data structure.
@@ -512,6 +565,7 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	struct lpfc_hba  *phba = vport->phba;
 	struct lpfc_nodelist *np;
 	struct lpfc_nodelist *next_np;
+	uint8_t fabric_param_changed;
 
 	spin_lock_irq(shost->host_lock);
 	vport->fc_flag |= FC_FABRIC;
@@ -544,6 +598,12 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 		ndlp->nlp_class_sup |= FC_COS_CLASS4;
 	ndlp->nlp_maxframe = ((sp->cmn.bbRcvSizeMsb & 0x0F) << 8) |
 				sp->cmn.bbRcvSizeLsb;
+
+	fabric_param_changed = lpfc_check_clean_addr_bit(vport, sp);
+	memcpy(&vport->fabric_portname, &sp->portName,
+			sizeof(struct lpfc_name));
+	memcpy(&vport->fabric_nodename, &sp->nodeName,
+			sizeof(struct lpfc_name));
 	memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
 
 	if (phba->sli3_options & LPFC_SLI3_NPIV_ENABLED) {
@@ -565,7 +625,7 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 		}
 	}
 
-	if ((vport->fc_prevDID != vport->fc_myDID) &&
+	if (fabric_param_changed &&
 		!(vport->fc_flag & FC_VPORT_NEEDS_REG_VPI)) {
 
 		/* If our NportID changed, we need to ensure all
@@ -2203,6 +2263,7 @@ lpfc_cmpl_els_logo(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 	struct Scsi_Host  *shost = lpfc_shost_from_vport(vport);
 	IOCB_t *irsp;
 	struct lpfc_sli *psli;
+	struct lpfcMboxq *mbox;
 
 	psli = &phba->sli;
 	/* we pass cmdiocb to state machine which needs rspiocb as well */
@@ -2260,6 +2321,21 @@ lpfc_cmpl_els_logo(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 					NLP_EVT_CMPL_LOGO);
 out:
 	lpfc_els_free_iocb(phba, cmdiocb);
+	/* If we are in pt2pt mode, we could rcv new S_ID on PLOGI */
+	if ((vport->fc_flag & FC_PT2PT) &&
+		!(vport->fc_flag & FC_PT2PT_PLOGI)) {
+		phba->pport->fc_myDID = 0;
+		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+		if (mbox) {
+			lpfc_config_link(phba, mbox);
+			mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+			mbox->vport = vport;
+			if (lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT) ==
+				MBX_NOT_FINISHED) {
+				mempool_free(mbox, phba->mbox_mem_pool);
+			}
+		}
+	}
 	return;
 }
 
@@ -2745,7 +2821,8 @@ lpfc_els_retry_delay_handler(struct lpfc_nodelist *ndlp)
 		}
 		break;
 	case ELS_CMD_FDISC:
-		lpfc_issue_els_fdisc(vport, ndlp, retry);
+		if (!(vport->fc_flag & FC_VPORT_NEEDS_INIT_VPI))
+			lpfc_issue_els_fdisc(vport, ndlp, retry);
 		break;
 	}
 	return;
@@ -2815,9 +2892,17 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 
 	switch (irsp->ulpStatus) {
 	case IOSTAT_FCP_RSP_ERROR:
+		break;
 	case IOSTAT_REMOTE_STOP:
+		if (phba->sli_rev == LPFC_SLI_REV4) {
+			/* This IO was aborted by the target, we don't
+			 * know the rxid and because we did not send the
+			 * ABTS we cannot generate and RRQ.
+			 */
+			lpfc_set_rrq_active(phba, ndlp,
+					 cmdiocb->sli4_xritag, 0, 0);
+		}
 		break;
-
 	case IOSTAT_LOCAL_REJECT:
 		switch ((irsp->un.ulpWord[4] & 0xff)) {
 		case IOERR_LOOP_OPEN_FAILURE:
@@ -4013,28 +4098,34 @@ lpfc_els_clear_rrq(struct lpfc_vport *vport,
 	uint8_t *pcmd;
 	struct RRQ *rrq;
 	uint16_t rxid;
+	uint16_t xri;
 	struct lpfc_node_rrq *prrq;
 
 
 	pcmd = (uint8_t *) (((struct lpfc_dmabuf *) iocb->context2)->virt);
 	pcmd += sizeof(uint32_t);
 	rrq = (struct RRQ *)pcmd;
-	rxid = bf_get(rrq_oxid, rrq);
+	rrq->rrq_exchg = be32_to_cpu(rrq->rrq_exchg);
+	rxid = be16_to_cpu(bf_get(rrq_rxid, rrq));
 
 	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
 			"2883 Clear RRQ for SID:x%x OXID:x%x RXID:x%x"
 			" x%x x%x\n",
-			bf_get(rrq_did, rrq),
-			bf_get(rrq_oxid, rrq),
+			be32_to_cpu(bf_get(rrq_did, rrq)),
+			be16_to_cpu(bf_get(rrq_oxid, rrq)),
 			rxid,
 			iocb->iotag, iocb->iocb.ulpContext);
 
 	lpfc_debugfs_disc_trc(vport, LPFC_DISC_TRC_ELS_RSP,
 		"Clear RRQ:  did:x%x flg:x%x exchg:x%.08x",
 		ndlp->nlp_DID, ndlp->nlp_flag, rrq->rrq_exchg);
-	prrq = lpfc_get_active_rrq(vport, rxid, ndlp->nlp_DID);
+	if (vport->fc_myDID == be32_to_cpu(bf_get(rrq_did, rrq)))
+		xri = be16_to_cpu(bf_get(rrq_oxid, rrq));
+	else
+		xri = rxid;
+	prrq = lpfc_get_active_rrq(vport, xri, ndlp->nlp_DID);
 	if (prrq)
-		lpfc_clr_rrq_active(phba, rxid, prrq);
+		lpfc_clr_rrq_active(phba, xri, prrq);
 	return;
 }
 
@@ -6166,6 +6257,11 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	if (vport->load_flag & FC_UNLOADING)
 		goto dropit;
 
+	/* If NPort discovery is delayed drop incoming ELS */
+	if ((vport->fc_flag & FC_DISC_DELAYED) &&
+			(cmd != ELS_CMD_PLOGI))
+		goto dropit;
+
 	ndlp = lpfc_findnode_did(vport, did);
 	if (!ndlp) {
 		/* Cannot find existing Fabric ndlp, so allocate a new one */
@@ -6218,6 +6314,12 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 		ndlp = lpfc_plogi_confirm_nport(phba, payload, ndlp);
 
 		lpfc_send_els_event(vport, ndlp, payload);
+
+		/* If Nport discovery is delayed, reject PLOGIs */
+		if (vport->fc_flag & FC_DISC_DELAYED) {
+			rjt_err = LSRJT_UNABLE_TPC;
+			break;
+		}
 		if (vport->port_state < LPFC_DISC_AUTH) {
 			if (!(phba->pport->fc_flag & FC_PT2PT) ||
 				(phba->pport->fc_flag & FC_PT2PT_PLOGI)) {
@@ -6596,6 +6698,21 @@ void
 lpfc_do_scr_ns_plogi(struct lpfc_hba *phba, struct lpfc_vport *vport)
 {
 	struct lpfc_nodelist *ndlp, *ndlp_fdmi;
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
+
+	/*
+	 * If lpfc_delay_discovery parameter is set and the clean address
+	 * bit is cleared and fc fabric parameters chenged, delay FC NPort
+	 * discovery.
+	 */
+	spin_lock_irq(shost->host_lock);
+	if (vport->fc_flag & FC_DISC_DELAYED) {
+		spin_unlock_irq(shost->host_lock);
+		mod_timer(&vport->delayed_disc_tmo,
+			jiffies + HZ * phba->fc_ratov);
+		return;
+	}
+	spin_unlock_irq(shost->host_lock);
 
 	ndlp = lpfc_findnode_did(vport, NameServer_DID);
 	if (!ndlp) {
@@ -6938,6 +7055,9 @@ lpfc_cmpl_els_fdisc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 	struct lpfc_nodelist *next_np;
 	IOCB_t *irsp = &rspiocb->iocb;
 	struct lpfc_iocbq *piocb;
+	struct lpfc_dmabuf *pcmd = cmdiocb->context2, *prsp;
+	struct serv_parm *sp;
+	uint8_t fabric_param_changed;
 
 	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
 			 "0123 FDISC completes. x%x/x%x prevDID: x%x\n",
@@ -6981,7 +7101,14 @@ lpfc_cmpl_els_fdisc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 
 	vport->fc_myDID = irsp->un.ulpWord[4] & Mask_DID;
 	lpfc_vport_set_state(vport, FC_VPORT_ACTIVE);
-	if ((vport->fc_prevDID != vport->fc_myDID) &&
+	prsp = list_get_first(&pcmd->list, struct lpfc_dmabuf, list);
+	sp = prsp->virt + sizeof(uint32_t);
+	fabric_param_changed = lpfc_check_clean_addr_bit(vport, sp);
+	memcpy(&vport->fabric_portname, &sp->portName,
+		sizeof(struct lpfc_name));
+	memcpy(&vport->fabric_nodename, &sp->nodeName,
+		sizeof(struct lpfc_name));
+	if (fabric_param_changed &&
 		!(vport->fc_flag & FC_VPORT_NEEDS_REG_VPI)) {
 		/* If our NportID changed, we need to ensure all
 		 * remaining NPORTs get unreg_login'ed so we can
@@ -7582,6 +7709,32 @@ void lpfc_fabric_abort_hba(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_sli4_vport_delete_els_xri_aborted -Remove all ndlp references for vport
+ * @vport: pointer to lpfc vport data structure.
+ *
+ * This routine is invoked by the vport cleanup for deletions and the cleanup
+ * for an ndlp on removal.
+ **/
+void
+lpfc_sli4_vport_delete_els_xri_aborted(struct lpfc_vport *vport)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_sglq *sglq_entry = NULL, *sglq_next = NULL;
+	unsigned long iflag = 0;
+
+	spin_lock_irqsave(&phba->hbalock, iflag);
+	spin_lock(&phba->sli4_hba.abts_sgl_list_lock);
+	list_for_each_entry_safe(sglq_entry, sglq_next,
+			&phba->sli4_hba.lpfc_abts_els_sgl_list, list) {
+		if (sglq_entry->ndlp && sglq_entry->ndlp->vport == vport)
+			sglq_entry->ndlp = NULL;
+	}
+	spin_unlock(&phba->sli4_hba.abts_sgl_list_lock);
+	spin_unlock_irqrestore(&phba->hbalock, iflag);
+	return;
+}
+
+/**
  * lpfc_sli4_els_xri_aborted - Slow-path process of els xri abort
  * @phba: pointer to lpfc hba data structure.
  * @axri: pointer to the els xri abort wcqe structure.
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index bb015960dbc9..154c715fb3af 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -658,6 +658,8 @@ lpfc_work_done(struct lpfc_hba *phba)
 				lpfc_ramp_down_queue_handler(phba);
 			if (work_port_events & WORKER_RAMP_UP_QUEUE)
 				lpfc_ramp_up_queue_handler(phba);
+			if (work_port_events & WORKER_DELAYED_DISC_TMO)
+				lpfc_delayed_disc_timeout_handler(vport);
 		}
 	lpfc_destroy_vport_work_array(phba, vports);
 
@@ -838,6 +840,11 @@ lpfc_linkdown_port(struct lpfc_vport *vport)
 
 	lpfc_port_link_failure(vport);
 
+	/* Stop delayed Nport discovery */
+	spin_lock_irq(shost->host_lock);
+	vport->fc_flag &= ~FC_DISC_DELAYED;
+	spin_unlock_irq(shost->host_lock);
+	del_timer_sync(&vport->delayed_disc_tmo);
 }
 
 int
@@ -3160,7 +3167,7 @@ lpfc_mbx_cmpl_unreg_vpi(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	spin_unlock_irq(shost->host_lock);
 	vport->unreg_vpi_cmpl = VPORT_OK;
 	mempool_free(pmb, phba->mbox_mem_pool);
-	lpfc_cleanup_vports_rrqs(vport);
+	lpfc_cleanup_vports_rrqs(vport, NULL);
 	/*
 	 * This shost reference might have been taken at the beginning of
 	 * lpfc_vport_delete()
@@ -3900,6 +3907,8 @@ lpfc_drop_node(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 	if (ndlp->nlp_state == NLP_STE_UNUSED_NODE)
 		return;
 	lpfc_nlp_set_state(vport, ndlp, NLP_STE_UNUSED_NODE);
+	if (vport->phba->sli_rev == LPFC_SLI_REV4)
+		lpfc_cleanup_vports_rrqs(vport, ndlp);
 	lpfc_nlp_put(ndlp);
 	return;
 }
@@ -4289,7 +4298,7 @@ lpfc_cleanup_node(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 
 	list_del_init(&ndlp->els_retry_evt.evt_listp);
 	list_del_init(&ndlp->dev_loss_evt.evt_listp);
-
+	lpfc_cleanup_vports_rrqs(vport, ndlp);
 	lpfc_unreg_rpi(vport, ndlp);
 
 	return 0;
@@ -4426,10 +4435,11 @@ lpfc_findnode_did(struct lpfc_vport *vport, uint32_t did)
 {
 	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
 	struct lpfc_nodelist *ndlp;
+	unsigned long iflags;
 
-	spin_lock_irq(shost->host_lock);
+	spin_lock_irqsave(shost->host_lock, iflags);
 	ndlp = __lpfc_findnode_did(vport, did);
-	spin_unlock_irq(shost->host_lock);
+	spin_unlock_irqrestore(shost->host_lock, iflags);
 	return ndlp;
 }
 
diff --git a/drivers/scsi/lpfc/lpfc_hw.h b/drivers/scsi/lpfc/lpfc_hw.h
index 96ed3ba6ba95..94ae37c5111a 100644
--- a/drivers/scsi/lpfc/lpfc_hw.h
+++ b/drivers/scsi/lpfc/lpfc_hw.h
@@ -341,6 +341,12 @@ struct csp {
 	uint8_t bbCreditMsb;
 	uint8_t bbCreditlsb;	/* FC Word 0, byte 3 */
 
+/*
+ * Word 1 Bit 31 in common service parameter is overloaded.
+ * Word 1 Bit 31 in FLOGI request is multiple NPort request
+ * Word 1 Bit 31 in FLOGI response is clean address bit
+ */
+#define clean_address_bit request_multiple_Nport /* Word 1, bit 31 */
 #ifdef __BIG_ENDIAN_BITFIELD
 	uint16_t request_multiple_Nport:1;	/* FC Word 1, bit 31 */
 	uint16_t randomOffset:1;	/* FC Word 1, bit 30 */
@@ -3198,7 +3204,10 @@ typedef struct {
 #define IOERR_SLER_RRQ_RJT_ERR        0x4C
 #define IOERR_SLER_RRQ_RETRY_ERR      0x4D
 #define IOERR_SLER_ABTS_ERR           0x4E
-
+#define IOERR_ELXSEC_KEY_UNWRAP_ERROR		0xF0
+#define IOERR_ELXSEC_KEY_UNWRAP_COMPARE_ERROR	0xF1
+#define IOERR_ELXSEC_CRYPTO_ERROR		0xF2
+#define IOERR_ELXSEC_CRYPTO_COMPARE_ERROR	0xF3
 #define IOERR_DRVR_MASK               0x100
 #define IOERR_SLI_DOWN                0x101  /* ulpStatus  - Driver defined */
 #define IOERR_SLI_BRESET              0x102
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 94c1aa1136de..c7178d60c7bf 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -778,6 +778,7 @@ struct mbox_header {
 #define LPFC_MBOX_OPCODE_QUERY_FW_CFG		0x3A
 #define LPFC_MBOX_OPCODE_FUNCTION_RESET		0x3D
 #define LPFC_MBOX_OPCODE_MQ_CREATE_EXT		0x5A
+#define LPFC_MBOX_OPCODE_GET_SLI4_PARAMETERS	0xB5
 
 /* FCoE Opcodes */
 #define LPFC_MBOX_OPCODE_FCOE_WQ_CREATE			0x01
@@ -1852,6 +1853,9 @@ struct lpfc_mbx_request_features {
 #define lpfc_mbx_rq_ftr_rq_ifip_SHIFT		7
 #define lpfc_mbx_rq_ftr_rq_ifip_MASK		0x00000001
 #define lpfc_mbx_rq_ftr_rq_ifip_WORD		word2
+#define lpfc_mbx_rq_ftr_rq_perfh_SHIFT		11
+#define lpfc_mbx_rq_ftr_rq_perfh_MASK		0x00000001
+#define lpfc_mbx_rq_ftr_rq_perfh_WORD		word2
 	uint32_t word3;
 #define lpfc_mbx_rq_ftr_rsp_iaab_SHIFT		0
 #define lpfc_mbx_rq_ftr_rsp_iaab_MASK		0x00000001
@@ -1877,6 +1881,9 @@ struct lpfc_mbx_request_features {
 #define lpfc_mbx_rq_ftr_rsp_ifip_SHIFT		7
 #define lpfc_mbx_rq_ftr_rsp_ifip_MASK		0x00000001
 #define lpfc_mbx_rq_ftr_rsp_ifip_WORD		word3
+#define lpfc_mbx_rq_ftr_rsp_perfh_SHIFT		11
+#define lpfc_mbx_rq_ftr_rsp_perfh_MASK		0x00000001
+#define lpfc_mbx_rq_ftr_rsp_perfh_WORD		word3
 };
 
 struct lpfc_mbx_supp_pages {
@@ -1935,7 +1942,7 @@ struct lpfc_mbx_supp_pages {
 #define LPFC_SLI4_PARAMETERS		2
 };
 
-struct lpfc_mbx_sli4_params {
+struct lpfc_mbx_pc_sli4_params {
 	uint32_t word1;
 #define qs_SHIFT				0
 #define qs_MASK					0x00000001
@@ -2051,6 +2058,88 @@ struct lpfc_mbx_sli4_params {
 	uint32_t rsvd_13_63[51];
 };
 
+struct lpfc_sli4_parameters {
+	uint32_t word0;
+#define cfg_prot_type_SHIFT			0
+#define cfg_prot_type_MASK			0x000000FF
+#define cfg_prot_type_WORD			word0
+	uint32_t word1;
+#define cfg_ft_SHIFT				0
+#define cfg_ft_MASK				0x00000001
+#define cfg_ft_WORD				word1
+#define cfg_sli_rev_SHIFT			4
+#define cfg_sli_rev_MASK			0x0000000f
+#define cfg_sli_rev_WORD			word1
+#define cfg_sli_family_SHIFT			8
+#define cfg_sli_family_MASK			0x0000000f
+#define cfg_sli_family_WORD			word1
+#define cfg_if_type_SHIFT			12
+#define cfg_if_type_MASK			0x0000000f
+#define cfg_if_type_WORD			word1
+#define cfg_sli_hint_1_SHIFT			16
+#define cfg_sli_hint_1_MASK			0x000000ff
+#define cfg_sli_hint_1_WORD			word1
+#define cfg_sli_hint_2_SHIFT			24
+#define cfg_sli_hint_2_MASK			0x0000001f
+#define cfg_sli_hint_2_WORD			word1
+	uint32_t word2;
+	uint32_t word3;
+	uint32_t word4;
+#define cfg_cqv_SHIFT				14
+#define cfg_cqv_MASK				0x00000003
+#define cfg_cqv_WORD				word4
+	uint32_t word5;
+	uint32_t word6;
+#define cfg_mqv_SHIFT				14
+#define cfg_mqv_MASK				0x00000003
+#define cfg_mqv_WORD				word6
+	uint32_t word7;
+	uint32_t word8;
+#define cfg_wqv_SHIFT				14
+#define cfg_wqv_MASK				0x00000003
+#define cfg_wqv_WORD				word8
+	uint32_t word9;
+	uint32_t word10;
+#define cfg_rqv_SHIFT				14
+#define cfg_rqv_MASK				0x00000003
+#define cfg_rqv_WORD				word10
+	uint32_t word11;
+#define cfg_rq_db_window_SHIFT			28
+#define cfg_rq_db_window_MASK			0x0000000f
+#define cfg_rq_db_window_WORD			word11
+	uint32_t word12;
+#define cfg_fcoe_SHIFT				0
+#define cfg_fcoe_MASK				0x00000001
+#define cfg_fcoe_WORD				word12
+#define cfg_phwq_SHIFT				15
+#define cfg_phwq_MASK				0x00000001
+#define cfg_phwq_WORD				word12
+#define cfg_loopbk_scope_SHIFT			28
+#define cfg_loopbk_scope_MASK			0x0000000f
+#define cfg_loopbk_scope_WORD			word12
+	uint32_t sge_supp_len;
+	uint32_t word14;
+#define cfg_sgl_page_cnt_SHIFT			0
+#define cfg_sgl_page_cnt_MASK			0x0000000f
+#define cfg_sgl_page_cnt_WORD			word14
+#define cfg_sgl_page_size_SHIFT			8
+#define cfg_sgl_page_size_MASK			0x000000ff
+#define cfg_sgl_page_size_WORD			word14
+#define cfg_sgl_pp_align_SHIFT			16
+#define cfg_sgl_pp_align_MASK			0x000000ff
+#define cfg_sgl_pp_align_WORD			word14
+	uint32_t word15;
+	uint32_t word16;
+	uint32_t word17;
+	uint32_t word18;
+	uint32_t word19;
+};
+
+struct lpfc_mbx_get_sli4_parameters {
+	struct mbox_header header;
+	struct lpfc_sli4_parameters sli4_parameters;
+};
+
 /* Mailbox Completion Queue Error Messages */
 #define MB_CQE_STATUS_SUCCESS 			0x0
 #define MB_CQE_STATUS_INSUFFICIENT_PRIVILEGES	0x1
@@ -2103,7 +2192,8 @@ struct lpfc_mqe {
 		struct lpfc_mbx_post_hdr_tmpl hdr_tmpl;
 		struct lpfc_mbx_query_fw_cfg query_fw_cfg;
 		struct lpfc_mbx_supp_pages supp_pages;
-		struct lpfc_mbx_sli4_params sli4_params;
+		struct lpfc_mbx_pc_sli4_params sli4_params;
+		struct lpfc_mbx_get_sli4_parameters get_sli4_parameters;
 		struct lpfc_mbx_nop nop;
 	} un;
 };
@@ -2381,6 +2471,10 @@ struct wqe_common {
 #define wqe_wqes_SHIFT        15
 #define wqe_wqes_MASK         0x00000001
 #define wqe_wqes_WORD         word10
+/* Note that this field overlaps above fields */
+#define wqe_wqid_SHIFT        1
+#define wqe_wqid_MASK         0x0000007f
+#define wqe_wqid_WORD         word10
 #define wqe_pri_SHIFT         16
 #define wqe_pri_MASK          0x00000007
 #define wqe_pri_WORD          word10
@@ -2599,7 +2693,8 @@ struct fcp_iwrite64_wqe {
 	uint32_t total_xfer_len;
 	uint32_t initial_xfer_len;
 	struct wqe_common wqe_com;     /* words 6-11 */
-	uint32_t rsvd_12_15[4];         /* word 12-15 */
+	uint32_t rsrvd12;
+	struct ulp_bde64 ph_bde;       /* words 13-15 */
 };
 
 struct fcp_iread64_wqe {
@@ -2608,7 +2703,8 @@ struct fcp_iread64_wqe {
 	uint32_t total_xfer_len;       /* word 4 */
 	uint32_t rsrvd5;               /* word 5 */
 	struct wqe_common wqe_com;     /* words 6-11 */
-	uint32_t rsvd_12_15[4];         /* word 12-15 */
+	uint32_t rsrvd12;
+	struct ulp_bde64 ph_bde;       /* words 13-15 */
 };
 
 struct fcp_icmnd64_wqe {
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 6d0b36aa3389..35665cfb5689 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -460,7 +460,7 @@ lpfc_config_port_post(struct lpfc_hba *phba)
 	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_16G)
 		&& !(phba->lmt & LMT_16Gb))) {
 		/* Reset link speed to auto */
-		lpfc_printf_log(phba, KERN_WARNING, LOG_LINK_EVENT,
+		lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
 			"1302 Invalid speed for this board: "
 			"Reset link speed to auto: x%x\n",
 			phba->cfg_link_speed);
@@ -945,17 +945,13 @@ static void
 lpfc_rrq_timeout(unsigned long ptr)
 {
 	struct lpfc_hba *phba;
-	uint32_t tmo_posted;
 	unsigned long iflag;
 
 	phba = (struct lpfc_hba *)ptr;
 	spin_lock_irqsave(&phba->pport->work_port_lock, iflag);
-	tmo_posted = phba->hba_flag & HBA_RRQ_ACTIVE;
-	if (!tmo_posted)
-		phba->hba_flag |= HBA_RRQ_ACTIVE;
+	phba->hba_flag |= HBA_RRQ_ACTIVE;
 	spin_unlock_irqrestore(&phba->pport->work_port_lock, iflag);
-	if (!tmo_posted)
-		lpfc_worker_wake_up(phba);
+	lpfc_worker_wake_up(phba);
 }
 
 /**
@@ -2280,6 +2276,7 @@ lpfc_cleanup(struct lpfc_vport *vport)
 		/* Wait for any activity on ndlps to settle */
 		msleep(10);
 	}
+	lpfc_cleanup_vports_rrqs(vport, NULL);
 }
 
 /**
@@ -2295,6 +2292,7 @@ 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;
 }
@@ -2355,6 +2353,10 @@ lpfc_stop_hba_timers(struct lpfc_hba *phba)
 	del_timer_sync(&phba->fabric_block_timer);
 	del_timer_sync(&phba->eratt_poll);
 	del_timer_sync(&phba->hb_tmofunc);
+	if (phba->sli_rev == LPFC_SLI_REV4) {
+		del_timer_sync(&phba->rrq_tmr);
+		phba->hba_flag &= ~HBA_RRQ_ACTIVE;
+	}
 	phba->hb_outstanding = 0;
 
 	switch (phba->pci_dev_grp) {
@@ -2732,6 +2734,11 @@ lpfc_create_port(struct lpfc_hba *phba, int instance, struct device *dev)
 	init_timer(&vport->els_tmofunc);
 	vport->els_tmofunc.function = lpfc_els_timeout;
 	vport->els_tmofunc.data = (unsigned long)vport;
+
+	init_timer(&vport->delayed_disc_tmo);
+	vport->delayed_disc_tmo.function = lpfc_delayed_disc_tmo;
+	vport->delayed_disc_tmo.data = (unsigned long)vport;
+
 	error = scsi_add_host_with_dma(shost, dev, &phba->pcidev->dev);
 	if (error)
 		goto out_put_shost;
@@ -4283,36 +4290,37 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 		goto out_free_bsmbx;
 	}
 
-	/* Get the Supported Pages. It is always available. */
+	/* Get the Supported Pages if PORT_CAPABILITIES is supported by port. */
 	lpfc_supported_pages(mboxq);
 	rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
-	if (unlikely(rc)) {
-		rc = -EIO;
-		mempool_free(mboxq, phba->mbox_mem_pool);
-		goto out_free_bsmbx;
-	}
-
-	mqe = &mboxq->u.mqe;
-	memcpy(&pn_page[0], ((uint8_t *)&mqe->un.supp_pages.word3),
-	       LPFC_MAX_SUPPORTED_PAGES);
-	for (i = 0; i < LPFC_MAX_SUPPORTED_PAGES; i++) {
-		switch (pn_page[i]) {
-		case LPFC_SLI4_PARAMETERS:
-			phba->sli4_hba.pc_sli4_params.supported = 1;
-			break;
-		default:
-			break;
+	if (!rc) {
+		mqe = &mboxq->u.mqe;
+		memcpy(&pn_page[0], ((uint8_t *)&mqe->un.supp_pages.word3),
+		       LPFC_MAX_SUPPORTED_PAGES);
+		for (i = 0; i < LPFC_MAX_SUPPORTED_PAGES; i++) {
+			switch (pn_page[i]) {
+			case LPFC_SLI4_PARAMETERS:
+				phba->sli4_hba.pc_sli4_params.supported = 1;
+				break;
+			default:
+				break;
+			}
+		}
+		/* Read the port's SLI4 Parameters capabilities if supported. */
+		if (phba->sli4_hba.pc_sli4_params.supported)
+			rc = lpfc_pc_sli4_params_get(phba, mboxq);
+		if (rc) {
+			mempool_free(mboxq, phba->mbox_mem_pool);
+			rc = -EIO;
+			goto out_free_bsmbx;
 		}
 	}
-
-	/* Read the port's SLI4 Parameters capabilities if supported. */
-	if (phba->sli4_hba.pc_sli4_params.supported)
-		rc = lpfc_pc_sli4_params_get(phba, mboxq);
+	/*
+	 * Get sli4 parameters that override parameters from Port capabilities.
+	 * If this call fails it is not a critical error so continue loading.
+	 */
+	lpfc_get_sli4_parameters(phba, mboxq);
 	mempool_free(mboxq, phba->mbox_mem_pool);
-	if (rc) {
-		rc = -EIO;
-		goto out_free_bsmbx;
-	}
 	/* Create all the SLI4 queues */
 	rc = lpfc_sli4_queue_create(phba);
 	if (rc)
@@ -7810,7 +7818,7 @@ lpfc_pc_sli4_params_get(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 	mqe = &mboxq->u.mqe;
 
 	/* Read the port's SLI4 Parameters port capabilities */
-	lpfc_sli4_params(mboxq);
+	lpfc_pc_sli4_params(mboxq);
 	if (!phba->sli4_hba.intr_enable)
 		rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
 	else {
@@ -7854,6 +7862,66 @@ lpfc_pc_sli4_params_get(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 }
 
 /**
+ * lpfc_get_sli4_parameters - Get the SLI4 Config PARAMETERS.
+ * @phba: Pointer to HBA context object.
+ * @mboxq: Pointer to the mailboxq memory for the mailbox command response.
+ *
+ * This function is called in the SLI4 code path to read the port's
+ * sli4 capabilities.
+ *
+ * This function may be be called from any context that can block-wait
+ * for the completion.  The expectation is that this routine is called
+ * typically from probe_one or from the online routine.
+ **/
+int
+lpfc_get_sli4_parameters(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
+{
+	int rc;
+	struct lpfc_mqe *mqe = &mboxq->u.mqe;
+	struct lpfc_pc_sli4_params *sli4_params;
+	int length;
+	struct lpfc_sli4_parameters *mbx_sli4_parameters;
+
+	/* Read the port's SLI4 Config Parameters */
+	length = (sizeof(struct lpfc_mbx_get_sli4_parameters) -
+		  sizeof(struct lpfc_sli4_cfg_mhdr));
+	lpfc_sli4_config(phba, mboxq, LPFC_MBOX_SUBSYSTEM_COMMON,
+			 LPFC_MBOX_OPCODE_GET_SLI4_PARAMETERS,
+			 length, LPFC_SLI4_MBX_EMBED);
+	if (!phba->sli4_hba.intr_enable)
+		rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
+	else
+		rc = lpfc_sli_issue_mbox_wait(phba, mboxq,
+			lpfc_mbox_tmo_val(phba, MBX_SLI4_CONFIG));
+	if (unlikely(rc))
+		return rc;
+	sli4_params = &phba->sli4_hba.pc_sli4_params;
+	mbx_sli4_parameters = &mqe->un.get_sli4_parameters.sli4_parameters;
+	sli4_params->if_type = bf_get(cfg_if_type, mbx_sli4_parameters);
+	sli4_params->sli_rev = bf_get(cfg_sli_rev, mbx_sli4_parameters);
+	sli4_params->sli_family = bf_get(cfg_sli_family, mbx_sli4_parameters);
+	sli4_params->featurelevel_1 = bf_get(cfg_sli_hint_1,
+					     mbx_sli4_parameters);
+	sli4_params->featurelevel_2 = bf_get(cfg_sli_hint_2,
+					     mbx_sli4_parameters);
+	if (bf_get(cfg_phwq, mbx_sli4_parameters))
+		phba->sli3_options |= LPFC_SLI4_PHWQ_ENABLED;
+	else
+		phba->sli3_options &= ~LPFC_SLI4_PHWQ_ENABLED;
+	sli4_params->sge_supp_len = mbx_sli4_parameters->sge_supp_len;
+	sli4_params->loopbk_scope = bf_get(loopbk_scope, mbx_sli4_parameters);
+	sli4_params->cqv = bf_get(cfg_cqv, mbx_sli4_parameters);
+	sli4_params->mqv = bf_get(cfg_mqv, mbx_sli4_parameters);
+	sli4_params->wqv = bf_get(cfg_wqv, mbx_sli4_parameters);
+	sli4_params->rqv = bf_get(cfg_rqv, mbx_sli4_parameters);
+	sli4_params->sgl_pages_max = bf_get(cfg_sgl_page_cnt,
+					    mbx_sli4_parameters);
+	sli4_params->sgl_pp_align = bf_get(cfg_sgl_pp_align,
+					   mbx_sli4_parameters);
+	return 0;
+}
+
+/**
  * lpfc_pci_probe_one_s3 - PCI probe func to reg SLI-3 device to PCI subsystem.
  * @pdev: pointer to PCI device
  * @pid: pointer to PCI device identifier
diff --git a/drivers/scsi/lpfc/lpfc_mbox.c b/drivers/scsi/lpfc/lpfc_mbox.c
index 23403c650207..dba32dfdb59b 100644
--- a/drivers/scsi/lpfc/lpfc_mbox.c
+++ b/drivers/scsi/lpfc/lpfc_mbox.c
@@ -1263,7 +1263,8 @@ lpfc_config_port(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	if (phba->sli_rev == LPFC_SLI_REV3 && phba->vpd.sli3Feat.cerbm) {
 		if (phba->cfg_enable_bg)
 			mb->un.varCfgPort.cbg = 1; /* configure BlockGuard */
-		mb->un.varCfgPort.cdss = 1; /* Configure Security */
+		if (phba->cfg_enable_dss)
+			mb->un.varCfgPort.cdss = 1; /* Configure Security */
 		mb->un.varCfgPort.cerbm = 1; /* Request HBQs */
 		mb->un.varCfgPort.ccrp = 1; /* Command Ring Polling */
 		mb->un.varCfgPort.max_hbq = lpfc_sli_hbq_count();
@@ -1692,7 +1693,7 @@ lpfc_sli4_mbox_cmd_free(struct lpfc_hba *phba, struct lpfcMboxq *mbox)
  * @mbox: pointer to lpfc mbox command.
  * @subsystem: The sli4 config sub mailbox subsystem.
  * @opcode: The sli4 config sub mailbox command opcode.
- * @length: Length of the sli4 config mailbox command.
+ * @length: Length of the sli4 config mailbox command (including sub-header).
  *
  * This routine sets up the header fields of SLI4 specific mailbox command
  * for sending IOCTL command.
@@ -1723,14 +1724,14 @@ lpfc_sli4_config(struct lpfc_hba *phba, struct lpfcMboxq *mbox,
 	if (emb) {
 		/* Set up main header fields */
 		bf_set(lpfc_mbox_hdr_emb, &sli4_config->header.cfg_mhdr, 1);
-		sli4_config->header.cfg_mhdr.payload_length =
-					LPFC_MBX_CMD_HDR_LENGTH + length;
+		sli4_config->header.cfg_mhdr.payload_length = length;
 		/* Set up sub-header fields following main header */
 		bf_set(lpfc_mbox_hdr_opcode,
 			&sli4_config->header.cfg_shdr.request, opcode);
 		bf_set(lpfc_mbox_hdr_subsystem,
 			&sli4_config->header.cfg_shdr.request, subsystem);
-		sli4_config->header.cfg_shdr.request.request_length = length;
+		sli4_config->header.cfg_shdr.request.request_length =
+			length - LPFC_MBX_CMD_HDR_LENGTH;
 		return length;
 	}
 
@@ -1902,6 +1903,7 @@ lpfc_request_features(struct lpfc_hba *phba, struct lpfcMboxq *mboxq)
 
 	/* Set up host requested features. */
 	bf_set(lpfc_mbx_rq_ftr_rq_fcpi, &mboxq->u.mqe.un.req_ftrs, 1);
+	bf_set(lpfc_mbx_rq_ftr_rq_perfh, &mboxq->u.mqe.un.req_ftrs, 1);
 
 	/* Enable DIF (block guard) only if configured to do so. */
 	if (phba->cfg_enable_bg)
@@ -2159,17 +2161,16 @@ lpfc_supported_pages(struct lpfcMboxq *mbox)
 }
 
 /**
- * lpfc_sli4_params - Initialize the PORT_CAPABILITIES SLI4 Params
- *                    mailbox command.
+ * lpfc_pc_sli4_params - Initialize the PORT_CAPABILITIES SLI4 Params mbox cmd.
  * @mbox: pointer to lpfc mbox command to initialize.
  *
  * The PORT_CAPABILITIES SLI4 parameters mailbox command is issued to
  * retrieve the particular SLI4 features supported by the port.
  **/
 void
-lpfc_sli4_params(struct lpfcMboxq *mbox)
+lpfc_pc_sli4_params(struct lpfcMboxq *mbox)
 {
-	struct lpfc_mbx_sli4_params *sli4_params;
+	struct lpfc_mbx_pc_sli4_params *sli4_params;
 
 	memset(mbox, 0, sizeof(*mbox));
 	sli4_params = &mbox->u.mqe.un.sli4_params;
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index d85a7423a694..52b35159fc35 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -350,7 +350,11 @@ lpfc_rcv_plogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	ndlp->nlp_maxframe =
 		((sp->cmn.bbRcvSizeMsb & 0x0F) << 8) | sp->cmn.bbRcvSizeLsb;
 
-	/* no need to reg_login if we are already in one of these states */
+	/*
+	 * Need to unreg_login if we are already in one of these states and
+	 * change to NPR state. This will block the port until after the ACC
+	 * completes and the reg_login is issued and completed.
+	 */
 	switch (ndlp->nlp_state) {
 	case  NLP_STE_NPR_NODE:
 		if (!(ndlp->nlp_flag & NLP_NPR_ADISC))
@@ -359,8 +363,9 @@ lpfc_rcv_plogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	case  NLP_STE_PRLI_ISSUE:
 	case  NLP_STE_UNMAPPED_NODE:
 	case  NLP_STE_MAPPED_NODE:
-		lpfc_els_rsp_acc(vport, ELS_CMD_PLOGI, cmdiocb, ndlp, NULL);
-		return 1;
+		lpfc_unreg_rpi(vport, ndlp);
+		ndlp->nlp_prev_state = ndlp->nlp_state;
+		lpfc_nlp_set_state(vport, ndlp, NLP_STE_NPR_NODE);
 	}
 
 	if ((vport->fc_flag & FC_PT2PT) &&
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index c97751c95d77..bf34178b80bf 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -609,6 +609,32 @@ lpfc_new_scsi_buf_s3(struct lpfc_vport *vport, int num_to_alloc)
 }
 
 /**
+ * lpfc_sli4_vport_delete_fcp_xri_aborted -Remove all ndlp references for vport
+ * @vport: pointer to lpfc vport data structure.
+ *
+ * This routine is invoked by the vport cleanup for deletions and the cleanup
+ * for an ndlp on removal.
+ **/
+void
+lpfc_sli4_vport_delete_fcp_xri_aborted(struct lpfc_vport *vport)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_scsi_buf *psb, *next_psb;
+	unsigned long iflag = 0;
+
+	spin_lock_irqsave(&phba->hbalock, iflag);
+	spin_lock(&phba->sli4_hba.abts_scsi_buf_list_lock);
+	list_for_each_entry_safe(psb, next_psb,
+				&phba->sli4_hba.lpfc_abts_scsi_buf_list, list) {
+		if (psb->rdata && psb->rdata->pnode
+			&& psb->rdata->pnode->vport == vport)
+			psb->rdata = NULL;
+	}
+	spin_unlock(&phba->sli4_hba.abts_scsi_buf_list_lock);
+	spin_unlock_irqrestore(&phba->hbalock, iflag);
+}
+
+/**
  * lpfc_sli4_fcp_xri_aborted - Fast-path process of fcp xri abort
  * @phba: pointer to lpfc hba data structure.
  * @axri: pointer to the fcp xri abort wcqe structure.
@@ -640,7 +666,11 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
 			psb->status = IOSTAT_SUCCESS;
 			spin_unlock(
 				&phba->sli4_hba.abts_scsi_buf_list_lock);
-			ndlp = psb->rdata->pnode;
+			if (psb->rdata && psb->rdata->pnode)
+				ndlp = psb->rdata->pnode;
+			else
+				ndlp = NULL;
+
 			rrq_empty = list_empty(&phba->active_rrq_list);
 			spin_unlock_irqrestore(&phba->hbalock, iflag);
 			if (ndlp)
@@ -964,36 +994,29 @@ lpfc_get_scsi_buf_s3(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 static struct lpfc_scsi_buf*
 lpfc_get_scsi_buf_s4(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
 {
-	struct  lpfc_scsi_buf *lpfc_cmd = NULL;
-	struct  lpfc_scsi_buf *start_lpfc_cmd = NULL;
-	struct list_head *scsi_buf_list = &phba->lpfc_scsi_buf_list;
+	struct lpfc_scsi_buf *lpfc_cmd ;
 	unsigned long iflag = 0;
 	int found = 0;
 
 	spin_lock_irqsave(&phba->scsi_buf_list_lock, iflag);
-	list_remove_head(scsi_buf_list, lpfc_cmd, struct lpfc_scsi_buf, list);
-	spin_unlock_irqrestore(&phba->scsi_buf_list_lock, iflag);
-	while (!found && lpfc_cmd) {
+	list_for_each_entry(lpfc_cmd, &phba->lpfc_scsi_buf_list,
+							list) {
 		if (lpfc_test_rrq_active(phba, ndlp,
-					 lpfc_cmd->cur_iocbq.sli4_xritag)) {
-			lpfc_release_scsi_buf_s4(phba, lpfc_cmd);
-			spin_lock_irqsave(&phba->scsi_buf_list_lock, iflag);
-			list_remove_head(scsi_buf_list, lpfc_cmd,
-					 struct lpfc_scsi_buf, list);
-			spin_unlock_irqrestore(&phba->scsi_buf_list_lock,
-						 iflag);
-			if (lpfc_cmd == start_lpfc_cmd) {
-				lpfc_cmd = NULL;
-				break;
-			} else
-				continue;
-		}
+					 lpfc_cmd->cur_iocbq.sli4_xritag))
+			continue;
+		list_del(&lpfc_cmd->list);
 		found = 1;
 		lpfc_cmd->seg_cnt = 0;
 		lpfc_cmd->nonsg_phys = 0;
 		lpfc_cmd->prot_seg_cnt = 0;
+		break;
 	}
-	return  lpfc_cmd;
+	spin_unlock_irqrestore(&phba->scsi_buf_list_lock,
+						 iflag);
+	if (!found)
+		return NULL;
+	else
+		return  lpfc_cmd;
 }
 /**
  * lpfc_get_scsi_buf - Get a scsi buffer from lpfc_scsi_buf_list of the HBA
@@ -1981,12 +2004,14 @@ lpfc_scsi_prep_dma_buf_s4(struct lpfc_hba *phba, struct lpfc_scsi_buf *lpfc_cmd)
 	struct scatterlist *sgel = NULL;
 	struct fcp_cmnd *fcp_cmnd = lpfc_cmd->fcp_cmnd;
 	struct sli4_sge *sgl = (struct sli4_sge *)lpfc_cmd->fcp_bpl;
+	struct sli4_sge *first_data_sgl;
 	IOCB_t *iocb_cmd = &lpfc_cmd->cur_iocbq.iocb;
 	dma_addr_t physaddr;
 	uint32_t num_bde = 0;
 	uint32_t dma_len;
 	uint32_t dma_offset = 0;
 	int nseg;
+	struct ulp_bde64 *bde;
 
 	/*
 	 * There are three possibilities here - use scatter-gather segment, use
@@ -2011,7 +2036,7 @@ lpfc_scsi_prep_dma_buf_s4(struct lpfc_hba *phba, struct lpfc_scsi_buf *lpfc_cmd)
 		bf_set(lpfc_sli4_sge_last, sgl, 0);
 		sgl->word2 = cpu_to_le32(sgl->word2);
 		sgl += 1;
-
+		first_data_sgl = sgl;
 		lpfc_cmd->seg_cnt = nseg;
 		if (lpfc_cmd->seg_cnt > phba->cfg_sg_seg_cnt) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_BG, "9074 BLKGRD:"
@@ -2047,6 +2072,17 @@ lpfc_scsi_prep_dma_buf_s4(struct lpfc_hba *phba, struct lpfc_scsi_buf *lpfc_cmd)
 			dma_offset += dma_len;
 			sgl++;
 		}
+		/* setup the performance hint (first data BDE) if enabled */
+		if (phba->sli3_options & LPFC_SLI4_PERFH_ENABLED) {
+			bde = (struct ulp_bde64 *)
+					&(iocb_cmd->unsli3.sli3Words[5]);
+			bde->addrLow = first_data_sgl->addr_lo;
+			bde->addrHigh = first_data_sgl->addr_hi;
+			bde->tus.f.bdeSize =
+					le32_to_cpu(first_data_sgl->sge_len);
+			bde->tus.f.bdeFlags = BUFF_TYPE_BDE_64;
+			bde->tus.w = cpu_to_le32(bde->tus.w);
+		}
 	} else {
 		sgl += 1;
 		/* clear the last flag in the fcp_rsp map entry */
@@ -2471,6 +2507,16 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 			lpfc_worker_wake_up(phba);
 			break;
 		case IOSTAT_LOCAL_REJECT:
+		case IOSTAT_REMOTE_STOP:
+			if (lpfc_cmd->result == IOERR_ELXSEC_KEY_UNWRAP_ERROR ||
+			    lpfc_cmd->result ==
+					IOERR_ELXSEC_KEY_UNWRAP_COMPARE_ERROR ||
+			    lpfc_cmd->result == IOERR_ELXSEC_CRYPTO_ERROR ||
+			    lpfc_cmd->result ==
+					IOERR_ELXSEC_CRYPTO_COMPARE_ERROR) {
+				cmd->result = ScsiResult(DID_NO_CONNECT, 0);
+				break;
+			}
 			if (lpfc_cmd->result == IOERR_INVALID_RPI ||
 			    lpfc_cmd->result == IOERR_NO_RESOURCES ||
 			    lpfc_cmd->result == IOERR_ABORT_REQUESTED ||
@@ -2478,7 +2524,6 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 				cmd->result = ScsiResult(DID_REQUEUE, 0);
 				break;
 			}
-
 			if ((lpfc_cmd->result == IOERR_RX_DMA_FAILED ||
 			     lpfc_cmd->result == IOERR_TX_DMA_FAILED) &&
 			     pIocbOut->iocb.unsli3.sli3_bg.bgstat) {
@@ -2497,7 +2542,17 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 							"on unprotected cmd\n");
 				}
 			}
-
+			if ((lpfc_cmd->status == IOSTAT_REMOTE_STOP)
+				&& (phba->sli_rev == LPFC_SLI_REV4)
+				&& (pnode && NLP_CHK_NODE_ACT(pnode))) {
+				/* This IO was aborted by the target, we don't
+				 * know the rxid and because we did not send the
+				 * ABTS we cannot generate and RRQ.
+				 */
+				lpfc_set_rrq_active(phba, pnode,
+						lpfc_cmd->cur_iocbq.sli4_xritag,
+						0, 0);
+			}
 		/* else: fall through */
 		default:
 			cmd->result = ScsiResult(DID_ERROR, 0);
@@ -2508,9 +2563,8 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 		    || (pnode->nlp_state != NLP_STE_MAPPED_NODE))
 			cmd->result = ScsiResult(DID_TRANSPORT_DISRUPTED,
 						 SAM_STAT_BUSY);
-	} else {
+	} else
 		cmd->result = ScsiResult(DID_OK, 0);
-	}
 
 	if (cmd->result || lpfc_cmd->fcp_rsp->rspSnsLen) {
 		uint32_t *lp = (uint32_t *)cmd->sense_buffer;
@@ -3004,11 +3058,11 @@ lpfc_queuecommand_lck(struct scsi_cmnd *cmnd, void (*done) (struct scsi_cmnd *))
 	 * transport is still transitioning.
 	 */
 	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
-		cmnd->result = ScsiResult(DID_TRANSPORT_DISRUPTED, 0);
+		cmnd->result = ScsiResult(DID_IMM_RETRY, 0);
 		goto out_fail_command;
 	}
 	if (atomic_read(&ndlp->cmd_pending) >= ndlp->cmd_qdepth)
-		goto out_host_busy;
+		goto out_tgt_busy;
 
 	lpfc_cmd = lpfc_get_scsi_buf(phba, ndlp);
 	if (lpfc_cmd == NULL) {
@@ -3125,6 +3179,9 @@ lpfc_queuecommand_lck(struct scsi_cmnd *cmnd, void (*done) (struct scsi_cmnd *))
  out_host_busy:
 	return SCSI_MLQUEUE_HOST_BUSY;
 
+ out_tgt_busy:
+	return SCSI_MLQUEUE_TARGET_BUSY;
+
  out_fail_command:
 	done(cmnd);
 	return 0;
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index a359d2b873ce..2ee0374a9908 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -96,7 +96,8 @@ lpfc_sli4_wq_put(struct lpfc_queue *q, union lpfc_wqe *wqe)
 	/* set consumption flag every once in a while */
 	if (!((q->host_index + 1) % LPFC_RELEASE_NOTIFICATION_INTERVAL))
 		bf_set(wqe_wqec, &wqe->generic.wqe_com, 1);
-
+	if (q->phba->sli3_options & LPFC_SLI4_PHWQ_ENABLED)
+		bf_set(wqe_wqid, &wqe->generic.wqe_com, q->queue_id);
 	lpfc_sli_pcimem_bcopy(wqe, temp_wqe, q->entry_size);
 
 	/* Update the host index before invoking device */
@@ -534,15 +535,35 @@ __lpfc_set_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 	uint16_t adj_xri;
 	struct lpfc_node_rrq *rrq;
 	int empty;
+	uint32_t did = 0;
+
+
+	if (!ndlp)
+		return -EINVAL;
+
+	if (!phba->cfg_enable_rrq)
+		return -EINVAL;
+
+	if (phba->pport->load_flag & FC_UNLOADING) {
+		phba->hba_flag &= ~HBA_RRQ_ACTIVE;
+		goto out;
+	}
+	did = ndlp->nlp_DID;
 
 	/*
 	 * set the active bit even if there is no mem available.
 	 */
 	adj_xri = xritag - phba->sli4_hba.max_cfg_param.xri_base;
-	if (!ndlp)
-		return -EINVAL;
+
+	if (NLP_CHK_FREE_REQ(ndlp))
+		goto out;
+
+	if (ndlp->vport && (ndlp->vport->load_flag & FC_UNLOADING))
+		goto out;
+
 	if (test_and_set_bit(adj_xri, ndlp->active_rrqs.xri_bitmap))
-		return -EINVAL;
+		goto out;
+
 	rrq = mempool_alloc(phba->rrq_pool, GFP_KERNEL);
 	if (rrq) {
 		rrq->send_rrq = send_rrq;
@@ -553,14 +574,7 @@ __lpfc_set_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 		rrq->vport = ndlp->vport;
 		rrq->rxid = rxid;
 		empty = list_empty(&phba->active_rrq_list);
-		if (phba->cfg_enable_rrq && send_rrq)
-			/*
-			 * We need the xri before we can add this to the
-			 * phba active rrq list.
-			 */
-			rrq->send_rrq = send_rrq;
-		else
-			rrq->send_rrq = 0;
+		rrq->send_rrq = send_rrq;
 		list_add_tail(&rrq->list, &phba->active_rrq_list);
 		if (!(phba->hba_flag & HBA_RRQ_ACTIVE)) {
 			phba->hba_flag |= HBA_RRQ_ACTIVE;
@@ -569,40 +583,49 @@ __lpfc_set_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 		}
 		return 0;
 	}
-	return -ENOMEM;
+out:
+	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+			"2921 Can't set rrq active xri:0x%x rxid:0x%x"
+			" DID:0x%x Send:%d\n",
+			xritag, rxid, did, send_rrq);
+	return -EINVAL;
 }
 
 /**
- * __lpfc_clr_rrq_active - Clears RRQ active bit in xri_bitmap.
+ * lpfc_clr_rrq_active - Clears RRQ active bit in xri_bitmap.
  * @phba: Pointer to HBA context object.
  * @xritag: xri used in this exchange.
  * @rrq: The RRQ to be cleared.
  *
- * This function is called with hbalock held. This function
  **/
-static void
-__lpfc_clr_rrq_active(struct lpfc_hba *phba,
-			uint16_t xritag,
-			struct lpfc_node_rrq *rrq)
+void
+lpfc_clr_rrq_active(struct lpfc_hba *phba,
+		    uint16_t xritag,
+		    struct lpfc_node_rrq *rrq)
 {
 	uint16_t adj_xri;
-	struct lpfc_nodelist *ndlp;
+	struct lpfc_nodelist *ndlp = NULL;
 
-	ndlp = lpfc_findnode_did(rrq->vport, rrq->nlp_DID);
+	if ((rrq->vport) && NLP_CHK_NODE_ACT(rrq->ndlp))
+		ndlp = lpfc_findnode_did(rrq->vport, rrq->nlp_DID);
 
 	/* The target DID could have been swapped (cable swap)
 	 * we should use the ndlp from the findnode if it is
 	 * available.
 	 */
-	if (!ndlp)
+	if ((!ndlp) && rrq->ndlp)
 		ndlp = rrq->ndlp;
 
+	if (!ndlp)
+		goto out;
+
 	adj_xri = xritag - phba->sli4_hba.max_cfg_param.xri_base;
 	if (test_and_clear_bit(adj_xri, ndlp->active_rrqs.xri_bitmap)) {
 		rrq->send_rrq = 0;
 		rrq->xritag = 0;
 		rrq->rrq_stop_time = 0;
 	}
+out:
 	mempool_free(rrq, phba->rrq_pool);
 }
 
@@ -627,34 +650,34 @@ lpfc_handle_rrq_active(struct lpfc_hba *phba)
 	struct lpfc_node_rrq *nextrrq;
 	unsigned long next_time;
 	unsigned long iflags;
+	LIST_HEAD(send_rrq);
 
 	spin_lock_irqsave(&phba->hbalock, iflags);
 	phba->hba_flag &= ~HBA_RRQ_ACTIVE;
 	next_time = jiffies + HZ * (phba->fc_ratov + 1);
 	list_for_each_entry_safe(rrq, nextrrq,
-			&phba->active_rrq_list, list) {
-		if (time_after(jiffies, rrq->rrq_stop_time)) {
-			list_del(&rrq->list);
-			if (!rrq->send_rrq)
-				/* this call will free the rrq */
-				__lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
-			else {
-			/* if we send the rrq then the completion handler
-			 *  will clear the bit in the xribitmap.
-			 */
-				spin_unlock_irqrestore(&phba->hbalock, iflags);
-				if (lpfc_send_rrq(phba, rrq)) {
-					lpfc_clr_rrq_active(phba, rrq->xritag,
-								 rrq);
-				}
-				spin_lock_irqsave(&phba->hbalock, iflags);
-			}
-		} else if  (time_before(rrq->rrq_stop_time, next_time))
+				 &phba->active_rrq_list, list) {
+		if (time_after(jiffies, rrq->rrq_stop_time))
+			list_move(&rrq->list, &send_rrq);
+		else if (time_before(rrq->rrq_stop_time, next_time))
 			next_time = rrq->rrq_stop_time;
 	}
 	spin_unlock_irqrestore(&phba->hbalock, iflags);
 	if (!list_empty(&phba->active_rrq_list))
 		mod_timer(&phba->rrq_tmr, next_time);
+	list_for_each_entry_safe(rrq, nextrrq, &send_rrq, list) {
+		list_del(&rrq->list);
+		if (!rrq->send_rrq)
+			/* this call will free the rrq */
+		lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
+		else if (lpfc_send_rrq(phba, rrq)) {
+			/* if we send the rrq then the completion handler
+			*  will clear the bit in the xribitmap.
+			*/
+			lpfc_clr_rrq_active(phba, rrq->xritag,
+					    rrq);
+		}
+	}
 }
 
 /**
@@ -692,29 +715,37 @@ lpfc_get_active_rrq(struct lpfc_vport *vport, uint16_t xri, uint32_t did)
 /**
  * lpfc_cleanup_vports_rrqs - Remove and clear the active RRQ for this vport.
  * @vport: Pointer to vport context object.
- *
- * Remove all active RRQs for this vport from the phba->active_rrq_list and
- * clear the rrq.
+ * @ndlp: Pointer to the lpfc_node_list structure.
+ * If ndlp is NULL Remove all active RRQs for this vport from the
+ * phba->active_rrq_list and clear the rrq.
+ * If ndlp is not NULL then only remove rrqs for this vport & this ndlp.
  **/
 void
-lpfc_cleanup_vports_rrqs(struct lpfc_vport *vport)
+lpfc_cleanup_vports_rrqs(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 
 {
 	struct lpfc_hba *phba = vport->phba;
 	struct lpfc_node_rrq *rrq;
 	struct lpfc_node_rrq *nextrrq;
 	unsigned long iflags;
+	LIST_HEAD(rrq_list);
 
 	if (phba->sli_rev != LPFC_SLI_REV4)
 		return;
-	spin_lock_irqsave(&phba->hbalock, iflags);
-	list_for_each_entry_safe(rrq, nextrrq, &phba->active_rrq_list, list) {
-		if (rrq->vport == vport) {
-			list_del(&rrq->list);
-			__lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
-		}
+	if (!ndlp) {
+		lpfc_sli4_vport_delete_els_xri_aborted(vport);
+		lpfc_sli4_vport_delete_fcp_xri_aborted(vport);
 	}
+	spin_lock_irqsave(&phba->hbalock, iflags);
+	list_for_each_entry_safe(rrq, nextrrq, &phba->active_rrq_list, list)
+		if ((rrq->vport == vport) && (!ndlp  || rrq->ndlp == ndlp))
+			list_move(&rrq->list, &rrq_list);
 	spin_unlock_irqrestore(&phba->hbalock, iflags);
+
+	list_for_each_entry_safe(rrq, nextrrq, &rrq_list, list) {
+		list_del(&rrq->list);
+		lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
+	}
 }
 
 /**
@@ -732,24 +763,27 @@ lpfc_cleanup_wt_rrqs(struct lpfc_hba *phba)
 	struct lpfc_node_rrq *nextrrq;
 	unsigned long next_time;
 	unsigned long iflags;
+	LIST_HEAD(rrq_list);
 
 	if (phba->sli_rev != LPFC_SLI_REV4)
 		return;
 	spin_lock_irqsave(&phba->hbalock, iflags);
 	phba->hba_flag &= ~HBA_RRQ_ACTIVE;
 	next_time = jiffies + HZ * (phba->fc_ratov * 2);
-	list_for_each_entry_safe(rrq, nextrrq, &phba->active_rrq_list, list) {
+	list_splice_init(&phba->active_rrq_list, &rrq_list);
+	spin_unlock_irqrestore(&phba->hbalock, iflags);
+
+	list_for_each_entry_safe(rrq, nextrrq, &rrq_list, list) {
 		list_del(&rrq->list);
-		__lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
+		lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
 	}
-	spin_unlock_irqrestore(&phba->hbalock, iflags);
 	if (!list_empty(&phba->active_rrq_list))
 		mod_timer(&phba->rrq_tmr, next_time);
 }
 
 
 /**
- * __lpfc_test_rrq_active - Test RRQ bit in xri_bitmap.
+ * lpfc_test_rrq_active - Test RRQ bit in xri_bitmap.
  * @phba: Pointer to HBA context object.
  * @ndlp: Targets nodelist pointer for this exchange.
  * @xritag the xri in the bitmap to test.
@@ -758,8 +792,8 @@ lpfc_cleanup_wt_rrqs(struct lpfc_hba *phba)
  * returns 0 = rrq not active for this xri
  *         1 = rrq is valid for this xri.
  **/
-static int
-__lpfc_test_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
+int
+lpfc_test_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 			uint16_t  xritag)
 {
 	uint16_t adj_xri;
@@ -802,52 +836,6 @@ lpfc_set_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
 }
 
 /**
- * lpfc_clr_rrq_active - Clears RRQ active bit in xri_bitmap.
- * @phba: Pointer to HBA context object.
- * @xritag: xri used in this exchange.
- * @rrq: The RRQ to be cleared.
- *
- * This function is takes the hbalock.
- **/
-void
-lpfc_clr_rrq_active(struct lpfc_hba *phba,
-			uint16_t xritag,
-			struct lpfc_node_rrq *rrq)
-{
-	unsigned long iflags;
-
-	spin_lock_irqsave(&phba->hbalock, iflags);
-	__lpfc_clr_rrq_active(phba, xritag, rrq);
-	spin_unlock_irqrestore(&phba->hbalock, iflags);
-	return;
-}
-
-
-
-/**
- * lpfc_test_rrq_active - Test RRQ bit in xri_bitmap.
- * @phba: Pointer to HBA context object.
- * @ndlp: Targets nodelist pointer for this exchange.
- * @xritag the xri in the bitmap to test.
- *
- * This function takes the hbalock.
- * returns 0 = rrq not active for this xri
- *         1 = rrq is valid for this xri.
- **/
-int
-lpfc_test_rrq_active(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp,
-			uint16_t  xritag)
-{
-	int ret;
-	unsigned long iflags;
-
-	spin_lock_irqsave(&phba->hbalock, iflags);
-	ret = __lpfc_test_rrq_active(phba, ndlp, xritag);
-	spin_unlock_irqrestore(&phba->hbalock, iflags);
-	return ret;
-}
-
-/**
  * __lpfc_sli_get_sglq - Allocates an iocb object from sgl pool
  * @phba: Pointer to HBA context object.
  * @piocb: Pointer to the iocbq.
@@ -884,7 +872,7 @@ __lpfc_sli_get_sglq(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq)
 			return NULL;
 		adj_xri = sglq->sli4_xritag -
 				phba->sli4_hba.max_cfg_param.xri_base;
-		if (__lpfc_test_rrq_active(phba, ndlp, sglq->sli4_xritag)) {
+		if (lpfc_test_rrq_active(phba, ndlp, sglq->sli4_xritag)) {
 			/* This xri has an rrq outstanding for this DID.
 			 * put it back in the list and get another xri.
 			 */
@@ -969,7 +957,8 @@ __lpfc_sli_release_iocbq_s4(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 		} else {
 			sglq->state = SGL_FREED;
 			sglq->ndlp = NULL;
-			list_add(&sglq->list, &phba->sli4_hba.lpfc_sgl_list);
+			list_add_tail(&sglq->list,
+				&phba->sli4_hba.lpfc_sgl_list);
 
 			/* Check if TXQ queue needs to be serviced */
 			if (pring->txq_cnt)
@@ -4817,7 +4806,10 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 				"0378 No support for fcpi mode.\n");
 		ftr_rsp++;
 	}
-
+	if (bf_get(lpfc_mbx_rq_ftr_rsp_perfh, &mqe->un.req_ftrs))
+		phba->sli3_options |= LPFC_SLI4_PERFH_ENABLED;
+	else
+		phba->sli3_options &= ~LPFC_SLI4_PERFH_ENABLED;
 	/*
 	 * If the port cannot support the host's requested features
 	 * then turn off the global config parameters to disable the
@@ -5004,7 +4996,8 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	phba->link_state = LPFC_LINK_DOWN;
 	spin_unlock_irq(&phba->hbalock);
-	rc = phba->lpfc_hba_init_link(phba, MBX_NOWAIT);
+	if (phba->cfg_suppress_link_up == LPFC_INITIALIZE_LINK)
+		rc = phba->lpfc_hba_init_link(phba, MBX_NOWAIT);
 out_unset_queue:
 	/* Unset all the queues set up in this routine when error out */
 	if (rc)
@@ -10478,6 +10471,7 @@ lpfc_cq_create(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	cq->type = type;
 	cq->subtype = subtype;
 	cq->queue_id = bf_get(lpfc_mbx_cq_create_q_id, &cq_create->u.response);
+	cq->assoc_qid = eq->queue_id;
 	cq->host_index = 0;
 	cq->hba_index = 0;
 
@@ -10672,6 +10666,7 @@ lpfc_mq_create(struct lpfc_hba *phba, struct lpfc_queue *mq,
 		goto out;
 	}
 	mq->type = LPFC_MQ;
+	mq->assoc_qid = cq->queue_id;
 	mq->subtype = subtype;
 	mq->host_index = 0;
 	mq->hba_index = 0;
@@ -10759,6 +10754,7 @@ lpfc_wq_create(struct lpfc_hba *phba, struct lpfc_queue *wq,
 		goto out;
 	}
 	wq->type = LPFC_WQ;
+	wq->assoc_qid = cq->queue_id;
 	wq->subtype = subtype;
 	wq->host_index = 0;
 	wq->hba_index = 0;
@@ -10876,6 +10872,7 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 		goto out;
 	}
 	hrq->type = LPFC_HRQ;
+	hrq->assoc_qid = cq->queue_id;
 	hrq->subtype = subtype;
 	hrq->host_index = 0;
 	hrq->hba_index = 0;
@@ -10936,6 +10933,7 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 		goto out;
 	}
 	drq->type = LPFC_DRQ;
+	drq->assoc_qid = cq->queue_id;
 	drq->subtype = subtype;
 	drq->host_index = 0;
 	drq->hba_index = 0;
@@ -11189,7 +11187,7 @@ lpfc_rq_destroy(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	if (!mbox)
 		return -ENOMEM;
 	length = (sizeof(struct lpfc_mbx_rq_destroy) -
-		  sizeof(struct mbox_header));
+		  sizeof(struct lpfc_sli4_cfg_mhdr));
 	lpfc_sli4_config(phba, mbox, LPFC_MBOX_SUBSYSTEM_FCOE,
 			 LPFC_MBOX_OPCODE_FCOE_RQ_DESTROY,
 			 length, LPFC_SLI4_MBX_EMBED);
@@ -11279,7 +11277,7 @@ lpfc_sli4_post_sgl(struct lpfc_hba *phba,
 	lpfc_sli4_config(phba, mbox, LPFC_MBOX_SUBSYSTEM_FCOE,
 			LPFC_MBOX_OPCODE_FCOE_POST_SGL_PAGES,
 			sizeof(struct lpfc_mbx_post_sgl_pages) -
-			sizeof(struct mbox_header), LPFC_SLI4_MBX_EMBED);
+			sizeof(struct lpfc_sli4_cfg_mhdr), LPFC_SLI4_MBX_EMBED);
 
 	post_sgl_pages = (struct lpfc_mbx_post_sgl_pages *)
 				&mbox->u.mqe.un.post_sgl_pages;
@@ -12402,7 +12400,8 @@ lpfc_sli4_post_rpi_hdr(struct lpfc_hba *phba, struct lpfc_rpi_hdr *rpi_page)
 	lpfc_sli4_config(phba, mboxq, LPFC_MBOX_SUBSYSTEM_FCOE,
 			 LPFC_MBOX_OPCODE_FCOE_POST_HDR_TEMPLATE,
 			 sizeof(struct lpfc_mbx_post_hdr_tmpl) -
-			 sizeof(struct mbox_header), LPFC_SLI4_MBX_EMBED);
+			 sizeof(struct lpfc_sli4_cfg_mhdr),
+			 LPFC_SLI4_MBX_EMBED);
 	bf_set(lpfc_mbx_post_hdr_tmpl_page_cnt,
 	       hdr_tmpl, rpi_page->page_count);
 	bf_set(lpfc_mbx_post_hdr_tmpl_rpi_offset, hdr_tmpl,
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index c7217d579e0f..595056b89608 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -125,9 +125,9 @@ struct lpfc_queue {
 	uint32_t entry_count;	/* Number of entries to support on the queue */
 	uint32_t entry_size;	/* Size of each queue entry. */
 	uint32_t queue_id;	/* Queue ID assigned by the hardware */
+	uint32_t assoc_qid;     /* Queue ID associated with, for CQ/WQ/MQ */
 	struct list_head page_list;
 	uint32_t page_count;	/* Number of pages allocated for this queue */
-
 	uint32_t host_index;	/* The host's index for putting or getting */
 	uint32_t hba_index;	/* The last known hba index for get or put */
 	union sli4_qe qe[1];	/* array to index entries (must be last) */
@@ -359,6 +359,10 @@ struct lpfc_pc_sli4_params {
 	uint32_t hdr_pp_align;
 	uint32_t sgl_pages_max;
 	uint32_t sgl_pp_align;
+	uint8_t cqv;
+	uint8_t mqv;
+	uint8_t wqv;
+	uint8_t rqv;
 };
 
 /* SLI4 HBA data structure entries */
@@ -562,6 +566,8 @@ void lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *,
 			       struct sli4_wcqe_xri_aborted *);
 void lpfc_sli4_els_xri_aborted(struct lpfc_hba *,
 			       struct sli4_wcqe_xri_aborted *);
+void lpfc_sli4_vport_delete_els_xri_aborted(struct lpfc_vport *);
+void lpfc_sli4_vport_delete_fcp_xri_aborted(struct lpfc_vport *);
 int lpfc_sli4_brdreset(struct lpfc_hba *);
 int lpfc_sli4_add_fcf_record(struct lpfc_hba *, struct fcf_record *);
 void lpfc_sli_remove_dflt_fcf(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 386cf92de492..0a4d376dbca5 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2004-2010 Emulex.  All rights reserved.           *
+ * Copyright (C) 2004-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  *                                                                 *
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "8.3.20"
+#define LPFC_DRIVER_VERSION "8.3.21"
 #define LPFC_DRIVER_NAME		"lpfc"
 #define LPFC_SP_DRIVER_HANDLER_NAME	"lpfc:sp"
 #define LPFC_FP_DRIVER_HANDLER_NAME	"lpfc:fp"
diff --git a/drivers/scsi/lpfc/lpfc_vport.c b/drivers/scsi/lpfc/lpfc_vport.c
index 6b8d2952e32f..30ba5440c67a 100644
--- a/drivers/scsi/lpfc/lpfc_vport.c
+++ b/drivers/scsi/lpfc/lpfc_vport.c
@@ -464,6 +464,7 @@ disable_vport(struct fc_vport *fc_vport)
 	struct lpfc_hba   *phba = vport->phba;
 	struct lpfc_nodelist *ndlp = NULL, *next_ndlp = NULL;
 	long timeout;
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
 
 	ndlp = lpfc_findnode_did(vport, Fabric_DID);
 	if (ndlp && NLP_CHK_NODE_ACT(ndlp)
@@ -498,6 +499,9 @@ disable_vport(struct fc_vport *fc_vport)
 	 * scsi_host_put() to release the vport.
 	 */
 	lpfc_mbx_unreg_vpi(vport);
+	spin_lock_irq(shost->host_lock);
+	vport->fc_flag |= FC_VPORT_NEEDS_INIT_VPI;
+	spin_unlock_irq(shost->host_lock);
 
 	lpfc_vport_set_state(vport, FC_VPORT_DISABLED);
 	lpfc_printf_vlog(vport, KERN_ERR, LOG_VPORT,
diff --git a/drivers/scsi/megaraid.c b/drivers/scsi/megaraid.c
index 9aa048525eb2..c212694a9714 100644
--- a/drivers/scsi/megaraid.c
+++ b/drivers/scsi/megaraid.c
@@ -1412,7 +1412,7 @@ megaraid_isr_memmapped(int irq, void *devp)
  * @nstatus - number of completed commands
  * @status - status of the last command completed
  *
- * Complete the comamnds and call the scsi mid-layer callback hooks.
+ * Complete the commands and call the scsi mid-layer callback hooks.
  */
 static void
 mega_cmd_done(adapter_t *adapter, u8 completed[], int nstatus, int status)
@@ -4296,7 +4296,7 @@ mega_support_cluster(adapter_t *adapter)
  * @adapter - pointer to our soft state
  * @dma_handle - DMA address of the buffer
  *
- * Issue internal comamnds while interrupts are available.
+ * Issue internal commands while interrupts are available.
  * We only issue direct mailbox commands from within the driver. ioctl()
  * interface using these routines can issue passthru commands.
  */
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index 1b5e375732c0..635b228c3ead 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -33,9 +33,9 @@
 /*
  * MegaRAID SAS Driver meta data
  */
-#define MEGASAS_VERSION				"00.00.05.29-rc1"
-#define MEGASAS_RELDATE				"Dec. 7, 2010"
-#define MEGASAS_EXT_VERSION			"Tue. Dec. 7 17:00:00 PDT 2010"
+#define MEGASAS_VERSION				"00.00.05.34-rc1"
+#define MEGASAS_RELDATE				"Feb. 24, 2011"
+#define MEGASAS_EXT_VERSION			"Thu. Feb. 24 17:00:00 PDT 2011"
 
 /*
  * Device IDs
@@ -723,6 +723,7 @@ struct megasas_ctrl_info {
 						MEGASAS_MAX_DEV_PER_CHANNEL)
 
 #define MEGASAS_MAX_SECTORS                    (2*1024)
+#define MEGASAS_MAX_SECTORS_IEEE		(2*128)
 #define MEGASAS_DBG_LVL				1
 
 #define MEGASAS_FW_BUSY				1
@@ -1477,4 +1478,7 @@ struct megasas_mgmt_info {
 	int max_index;
 };
 
+#define msi_control_reg(base) (base + PCI_MSI_FLAGS)
+#define PCI_MSIX_FLAGS_ENABLE (1 << 15)
+
 #endif				/*LSI_MEGARAID_SAS_H */
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 5d6d07bd1cd0..bbd10c81fd9c 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -18,12 +18,13 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  *
  *  FILE: megaraid_sas_base.c
- *  Version : v00.00.05.29-rc1
+ *  Version : v00.00.05.34-rc1
  *
  *  Authors: LSI Corporation
  *           Sreenivas Bagalkote
  *           Sumant Patro
  *           Bo Yang
+ *           Adam Radford <linuxraid@lsi.com>
  *
  *  Send feedback to: <megaraidlinux@lsi.com>
  *
@@ -134,7 +135,11 @@ spinlock_t poll_aen_lock;
 void
 megasas_complete_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd,
 		     u8 alt_status);
-
+static u32
+megasas_read_fw_status_reg_gen2(struct megasas_register_set __iomem *regs);
+static int
+megasas_adp_reset_gen2(struct megasas_instance *instance,
+		       struct megasas_register_set __iomem *reg_set);
 static irqreturn_t megasas_isr(int irq, void *devp);
 static u32
 megasas_init_adapter_mfi(struct megasas_instance *instance);
@@ -554,6 +559,8 @@ static int
 megasas_clear_intr_skinny(struct megasas_register_set __iomem *regs)
 {
 	u32 status;
+	u32 mfiStatus = 0;
+
 	/*
 	 * Check if it is our interrupt
 	 */
@@ -564,6 +571,15 @@ megasas_clear_intr_skinny(struct megasas_register_set __iomem *regs)
 	}
 
 	/*
+	 * Check if it is our interrupt
+	 */
+	if ((megasas_read_fw_status_reg_gen2(regs) & MFI_STATE_MASK) ==
+	    MFI_STATE_FAULT) {
+		mfiStatus = MFI_INTR_FLAG_FIRMWARE_STATE_CHANGE;
+	} else
+		mfiStatus = MFI_INTR_FLAG_REPLY_MESSAGE;
+
+	/*
 	 * Clear the interrupt by writing back the same value
 	 */
 	writel(status, &regs->outbound_intr_status);
@@ -573,7 +589,7 @@ megasas_clear_intr_skinny(struct megasas_register_set __iomem *regs)
 	*/
 	readl(&regs->outbound_intr_status);
 
-	return 1;
+	return mfiStatus;
 }
 
 /**
@@ -597,17 +613,6 @@ megasas_fire_cmd_skinny(struct megasas_instance *instance,
 }
 
 /**
- * megasas_adp_reset_skinny -	For controller reset
- * @regs:				MFI register set
- */
-static int
-megasas_adp_reset_skinny(struct megasas_instance *instance,
-			struct megasas_register_set __iomem *regs)
-{
-	return 0;
-}
-
-/**
  * megasas_check_reset_skinny -	For controller reset check
  * @regs:				MFI register set
  */
@@ -625,7 +630,7 @@ static struct megasas_instance_template megasas_instance_template_skinny = {
 	.disable_intr = megasas_disable_intr_skinny,
 	.clear_intr = megasas_clear_intr_skinny,
 	.read_fw_status_reg = megasas_read_fw_status_reg_skinny,
-	.adp_reset = megasas_adp_reset_skinny,
+	.adp_reset = megasas_adp_reset_gen2,
 	.check_reset = megasas_check_reset_skinny,
 	.service_isr = megasas_isr,
 	.tasklet = megasas_complete_cmd_dpc,
@@ -740,20 +745,28 @@ megasas_adp_reset_gen2(struct megasas_instance *instance,
 {
 	u32			retry = 0 ;
 	u32			HostDiag;
+	u32			*seq_offset = &reg_set->seq_offset;
+	u32			*hostdiag_offset = &reg_set->host_diag;
+
+	if (instance->instancet == &megasas_instance_template_skinny) {
+		seq_offset = &reg_set->fusion_seq_offset;
+		hostdiag_offset = &reg_set->fusion_host_diag;
+	}
+
+	writel(0, seq_offset);
+	writel(4, seq_offset);
+	writel(0xb, seq_offset);
+	writel(2, seq_offset);
+	writel(7, seq_offset);
+	writel(0xd, seq_offset);
 
-	writel(0, &reg_set->seq_offset);
-	writel(4, &reg_set->seq_offset);
-	writel(0xb, &reg_set->seq_offset);
-	writel(2, &reg_set->seq_offset);
-	writel(7, &reg_set->seq_offset);
-	writel(0xd, &reg_set->seq_offset);
 	msleep(1000);
 
-	HostDiag = (u32)readl(&reg_set->host_diag);
+	HostDiag = (u32)readl(hostdiag_offset);
 
 	while ( !( HostDiag & DIAG_WRITE_ENABLE) ) {
 		msleep(100);
-		HostDiag = (u32)readl(&reg_set->host_diag);
+		HostDiag = (u32)readl(hostdiag_offset);
 		printk(KERN_NOTICE "RESETGEN2: retry=%x, hostdiag=%x\n",
 					retry, HostDiag);
 
@@ -764,14 +777,14 @@ megasas_adp_reset_gen2(struct megasas_instance *instance,
 
 	printk(KERN_NOTICE "ADP_RESET_GEN2: HostDiag=%x\n", HostDiag);
 
-	writel((HostDiag | DIAG_RESET_ADAPTER), &reg_set->host_diag);
+	writel((HostDiag | DIAG_RESET_ADAPTER), hostdiag_offset);
 
 	ssleep(10);
 
-	HostDiag = (u32)readl(&reg_set->host_diag);
+	HostDiag = (u32)readl(hostdiag_offset);
 	while ( ( HostDiag & DIAG_RESET_ADAPTER) ) {
 		msleep(100);
-		HostDiag = (u32)readl(&reg_set->host_diag);
+		HostDiag = (u32)readl(hostdiag_offset);
 		printk(KERN_NOTICE "RESET_GEN2: retry=%x, hostdiag=%x\n",
 				retry, HostDiag);
 
@@ -877,7 +890,7 @@ megasas_issue_blocked_cmd(struct megasas_instance *instance,
  * @instance:				Adapter soft state
  * @cmd_to_abort:			Previously issued cmd to be aborted
  *
- * MFI firmware can abort previously issued AEN comamnd (automatic event
+ * MFI firmware can abort previously issued AEN command (automatic event
  * notification). The megasas_issue_blocked_abort_cmd() issues such abort
  * cmd and waits for return status.
  * Max wait time is MEGASAS_INTERNAL_CMD_WAIT_TIME secs
@@ -2503,7 +2516,9 @@ megasas_deplete_reply_queue(struct megasas_instance *instance,
 	if ((mfiStatus = instance->instancet->clear_intr(
 						instance->reg_set)
 						) == 0) {
-		return IRQ_NONE;
+		/* Hardware may not set outbound_intr_status in MSI-X mode */
+		if (!instance->msi_flag)
+			return IRQ_NONE;
 	}
 
 	instance->mfiStatus = mfiStatus;
@@ -2611,7 +2626,9 @@ megasas_transition_to_ready(struct megasas_instance* instance)
 		case MFI_STATE_FAULT:
 
 			printk(KERN_DEBUG "megasas: FW in FAULT state!!\n");
-			return -ENODEV;
+			max_wait = MEGASAS_RESET_WAIT_TIME;
+			cur_state = MFI_STATE_FAULT;
+			break;
 
 		case MFI_STATE_WAIT_HANDSHAKE:
 			/*
@@ -3424,7 +3441,6 @@ fail_reply_queue:
 	megasas_free_cmds(instance);
 
 fail_alloc_cmds:
-	iounmap(instance->reg_set);
 	return 1;
 }
 
@@ -3494,7 +3510,7 @@ static int megasas_init_fw(struct megasas_instance *instance)
 
 	/* Get operational params, sge flags, send init cmd to controller */
 	if (instance->instancet->init_adapter(instance))
-		return -ENODEV;
+		goto fail_init_adapter;
 
 	printk(KERN_ERR "megasas: INIT adapter done\n");
 
@@ -3543,7 +3559,7 @@ static int megasas_init_fw(struct megasas_instance *instance)
 	* Setup tasklet for cmd completion
 	*/
 
-	tasklet_init(&instance->isr_tasklet, megasas_complete_cmd_dpc,
+	tasklet_init(&instance->isr_tasklet, instance->instancet->tasklet,
 		(unsigned long)instance);
 
 	/* Initialize the cmd completion timer */
@@ -3553,6 +3569,7 @@ static int megasas_init_fw(struct megasas_instance *instance)
 				MEGASAS_COMPLETION_TIMER_INTERVAL);
 	return 0;
 
+fail_init_adapter:
 fail_ready_state:
 	iounmap(instance->reg_set);
 
@@ -3820,6 +3837,10 @@ static int megasas_io_attach(struct megasas_instance *instance)
 			instance->max_fw_cmds - MEGASAS_INT_CMDS;
 	host->this_id = instance->init_id;
 	host->sg_tablesize = instance->max_num_sge;
+
+	if (instance->fw_support_ieee)
+		instance->max_sectors_per_req = MEGASAS_MAX_SECTORS_IEEE;
+
 	/*
 	 * Check if the module parameter value for max_sectors can be used
 	 */
@@ -3899,9 +3920,26 @@ fail_set_dma_mask:
 static int __devinit
 megasas_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 {
-	int rval;
+	int rval, pos;
 	struct Scsi_Host *host;
 	struct megasas_instance *instance;
+	u16 control = 0;
+
+	/* Reset MSI-X in the kdump kernel */
+	if (reset_devices) {
+		pos = pci_find_capability(pdev, PCI_CAP_ID_MSIX);
+		if (pos) {
+			pci_read_config_word(pdev, msi_control_reg(pos),
+					     &control);
+			if (control & PCI_MSIX_FLAGS_ENABLE) {
+				dev_info(&pdev->dev, "resetting MSI-X\n");
+				pci_write_config_word(pdev,
+						      msi_control_reg(pos),
+						      control &
+						      ~PCI_MSIX_FLAGS_ENABLE);
+			}
+		}
+	}
 
 	/*
 	 * Announce PCI information
@@ -4039,12 +4077,6 @@ megasas_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	else
 		INIT_WORK(&instance->work_init, process_fw_state_change_wq);
 
-	/*
-	 * Initialize MFI Firmware
-	 */
-	if (megasas_init_fw(instance))
-		goto fail_init_mfi;
-
 	/* Try to enable MSI-X */
 	if ((instance->pdev->device != PCI_DEVICE_ID_LSI_SAS1078R) &&
 	    (instance->pdev->device != PCI_DEVICE_ID_LSI_SAS1078DE) &&
@@ -4054,6 +4086,12 @@ megasas_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		instance->msi_flag = 1;
 
 	/*
+	 * Initialize MFI Firmware
+	 */
+	if (megasas_init_fw(instance))
+		goto fail_init_mfi;
+
+	/*
 	 * Register IRQ
 	 */
 	if (request_irq(instance->msi_flag ? instance->msixentry.vector :
@@ -4105,24 +4143,23 @@ megasas_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	instance->instancet->disable_intr(instance->reg_set);
 	free_irq(instance->msi_flag ? instance->msixentry.vector :
 		 instance->pdev->irq, instance);
+fail_irq:
+	if (instance->pdev->device == PCI_DEVICE_ID_LSI_FUSION)
+		megasas_release_fusion(instance);
+	else
+		megasas_release_mfi(instance);
+      fail_init_mfi:
 	if (instance->msi_flag)
 		pci_disable_msix(instance->pdev);
-
-      fail_irq:
-      fail_init_mfi:
       fail_alloc_dma_buf:
 	if (instance->evt_detail)
 		pci_free_consistent(pdev, sizeof(struct megasas_evt_detail),
 				    instance->evt_detail,
 				    instance->evt_detail_h);
 
-	if (instance->producer) {
+	if (instance->producer)
 		pci_free_consistent(pdev, sizeof(u32), instance->producer,
 				    instance->producer_h);
-		megasas_release_mfi(instance);
-	} else {
-		megasas_release_fusion(instance);
-	}
 	if (instance->consumer)
 		pci_free_consistent(pdev, sizeof(u32), instance->consumer,
 				    instance->consumer_h);
@@ -4242,9 +4279,8 @@ megasas_suspend(struct pci_dev *pdev, pm_message_t state)
 	/* cancel the delayed work if this work still in queue */
 	if (instance->ev != NULL) {
 		struct megasas_aen_event *ev = instance->ev;
-		cancel_delayed_work(
+		cancel_delayed_work_sync(
 			(struct delayed_work *)&ev->hotplug_work);
-		flush_scheduled_work();
 		instance->ev = NULL;
 	}
 
@@ -4297,6 +4333,10 @@ megasas_resume(struct pci_dev *pdev)
 	if (megasas_set_dma_mask(pdev))
 		goto fail_set_dma_mask;
 
+	/* Now re-enable MSI-X */
+	if (instance->msi_flag)
+		pci_enable_msix(instance->pdev, &instance->msixentry, 1);
+
 	/*
 	 * Initialize MFI Firmware
 	 */
@@ -4333,10 +4373,6 @@ megasas_resume(struct pci_dev *pdev)
 	tasklet_init(&instance->isr_tasklet, instance->instancet->tasklet,
 		     (unsigned long)instance);
 
-	/* Now re-enable MSI-X */
-	if (instance->msi_flag)
-		pci_enable_msix(instance->pdev, &instance->msixentry, 1);
-
 	/*
 	 * Register IRQ
 	 */
@@ -4417,9 +4453,8 @@ static void __devexit megasas_detach_one(struct pci_dev *pdev)
 	/* cancel the delayed work if this work still in queue*/
 	if (instance->ev != NULL) {
 		struct megasas_aen_event *ev = instance->ev;
-		cancel_delayed_work(
+		cancel_delayed_work_sync(
 			(struct delayed_work *)&ev->hotplug_work);
-		flush_scheduled_work();
 		instance->ev = NULL;
 	}
 
@@ -4611,6 +4646,9 @@ megasas_mgmt_fw_ioctl(struct megasas_instance *instance,
 	 * For each user buffer, create a mirror buffer and copy in
 	 */
 	for (i = 0; i < ioc->sge_count; i++) {
+		if (!ioc->sgl[i].iov_len)
+			continue;
+
 		kbuff_arr[i] = dma_alloc_coherent(&instance->pdev->dev,
 						    ioc->sgl[i].iov_len,
 						    &buf_handle, GFP_KERNEL);
@@ -5177,6 +5215,7 @@ megasas_aen_polling(struct work_struct *work)
 			break;
 
 		case MR_EVT_LD_OFFLINE:
+		case MR_EVT_CFG_CLEARED:
 		case MR_EVT_LD_DELETED:
 			megasas_get_ld_list(instance);
 			for (i = 0; i < MEGASAS_MAX_LD_CHANNELS; i++) {
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.c b/drivers/scsi/megaraid/megaraid_sas_fusion.c
index c1e09d5a6196..905c1e9317b6 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.c
@@ -82,6 +82,10 @@ u16 MR_TargetIdToLdGet(u32 ldTgtId, struct MR_FW_RAID_MAP_ALL *map);
 struct MR_LD_RAID *MR_LdRaidGet(u32 ld, struct MR_FW_RAID_MAP_ALL *map);
 
 u16 MR_GetLDTgtId(u32 ld, struct MR_FW_RAID_MAP_ALL *map);
+
+void
+megasas_check_and_restore_queue_depth(struct megasas_instance *instance);
+
 u8 MR_ValidateMapInfo(struct MR_FW_RAID_MAP_ALL *map,
 		      struct LD_LOAD_BALANCE_INFO *lbInfo);
 u16 get_updated_dev_handle(struct LD_LOAD_BALANCE_INFO *lbInfo,
@@ -984,13 +988,15 @@ megasas_init_adapter_fusion(struct megasas_instance *instance)
 
 	return 0;
 
-fail_alloc_cmds:
-fail_alloc_mfi_cmds:
 fail_map_info:
 	if (i == 1)
 		dma_free_coherent(&instance->pdev->dev, fusion->map_sz,
 				  fusion->ld_map[0], fusion->ld_map_phys[0]);
 fail_ioc_init:
+	megasas_free_cmds_fusion(instance);
+fail_alloc_cmds:
+	megasas_free_cmds(instance);
+fail_alloc_mfi_cmds:
 	return 1;
 }
 
@@ -1432,8 +1438,7 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 	local_map_ptr = fusion->ld_map[(instance->map_id & 1)];
 
 	/* Check if this is a system PD I/O */
-	if ((instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) &&
-	    (instance->pd_list[pd_index].driveType == TYPE_DISK)) {
+	if (instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) {
 		io_request->Function = 0;
 		io_request->DevHandle =
 			local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
@@ -1456,7 +1461,7 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 			 MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
 	}
 	io_request->RaidContext.VirtualDiskTgtId = device_id;
-	io_request->LUN[0] = scmd->device->lun;
+	io_request->LUN[1] = scmd->device->lun;
 	io_request->DataLength = scsi_bufflen(scmd);
 }
 
@@ -1480,7 +1485,7 @@ megasas_build_io_fusion(struct megasas_instance *instance,
 	device_id = MEGASAS_DEV_INDEX(instance, scp);
 
 	/* Zero out some fields so they don't get reused */
-	io_request->LUN[0] = 0;
+	io_request->LUN[1] = 0;
 	io_request->CDB.EEDP32.PrimaryReferenceTag = 0;
 	io_request->CDB.EEDP32.PrimaryApplicationTagMask = 0;
 	io_request->EEDPFlags = 0;
@@ -1744,7 +1749,7 @@ complete_cmd_fusion(struct megasas_instance *instance)
 	wmb();
 	writel(fusion->last_reply_idx,
 	       &instance->reg_set->reply_post_host_index);
-
+	megasas_check_and_restore_queue_depth(instance);
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2.h b/drivers/scsi/mpt2sas/mpi/mpi2.h
index 8be75e65f763..a3e60385787f 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2.h
@@ -8,7 +8,7 @@
  *                  scatter/gather formats.
  *  Creation Date:  June 21, 2006
  *
- *  mpi2.h Version:  02.00.16
+ *  mpi2.h Version:  02.00.17
  *
  *  Version History
  *  ---------------
@@ -63,6 +63,7 @@
  *                      function codes, 0xF0 to 0xFF.
  *  05-12-10  02.00.16  Bumped MPI2_HEADER_VERSION_UNIT.
  *                      Added alternative defines for the SGE Direction bit.
+ *  08-11-10  02.00.17  Bumped MPI2_HEADER_VERSION_UNIT.
  *  --------------------------------------------------------------------------
  */
 
@@ -88,7 +89,7 @@
 #define MPI2_VERSION_02_00                  (0x0200)
 
 /* versioning for this MPI header set */
-#define MPI2_HEADER_VERSION_UNIT            (0x10)
+#define MPI2_HEADER_VERSION_UNIT            (0x11)
 #define MPI2_HEADER_VERSION_DEV             (0x00)
 #define MPI2_HEADER_VERSION_UNIT_MASK       (0xFF00)
 #define MPI2_HEADER_VERSION_UNIT_SHIFT      (8)
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
index d76a65847603..f5b9c766e28f 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
@@ -6,7 +6,7 @@
  *          Title:  MPI Configuration messages and pages
  *  Creation Date:  November 10, 2006
  *
- *    mpi2_cnfg.h Version:  02.00.15
+ *    mpi2_cnfg.h Version:  02.00.16
  *
  *  Version History
  *  ---------------
@@ -125,6 +125,8 @@
  *                      define.
  *                      Added MPI2_PHYSDISK0_INCOMPATIBLE_MEDIA_TYPE define.
  *                      Added MPI2_SAS_NEG_LINK_RATE_UNSUPPORTED_PHY define.
+ *  08-11-10  02.00.16  Removed IO Unit Page 1 device path (multi-pathing)
+ *                      defines.
  *  --------------------------------------------------------------------------
  */
 
@@ -745,8 +747,6 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_1
 #define MPI2_IOUNITPAGE1_DISABLE_IR                     (0x00000040)
 #define MPI2_IOUNITPAGE1_DISABLE_TASK_SET_FULL_HANDLING (0x00000020)
 #define MPI2_IOUNITPAGE1_IR_USE_STATIC_VOLUME_ID        (0x00000004)
-#define MPI2_IOUNITPAGE1_MULTI_PATHING                  (0x00000002)
-#define MPI2_IOUNITPAGE1_SINGLE_PATHING                 (0x00000000)
 
 
 /* IO Unit Page 3 */
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_history.txt b/drivers/scsi/mpt2sas/mpi/mpi2_history.txt
deleted file mode 100644
index b1e88f26b748..000000000000
--- a/drivers/scsi/mpt2sas/mpi/mpi2_history.txt
+++ /dev/null
@@ -1,384 +0,0 @@
- ==============================
- Fusion-MPT MPI 2.0 Header File Change History
- ==============================
-
- Copyright (c) 2000-2010 LSI Corporation.
-
- ---------------------------------------
- Header Set Release Version:    02.00.14
- Header Set Release Date:       10-28-09
- ---------------------------------------
-
- Filename               Current version     Prior version
- ----------             ---------------     -------------
- mpi2.h                 02.00.14            02.00.13
- mpi2_cnfg.h            02.00.13            02.00.12
- mpi2_init.h            02.00.08            02.00.07
- mpi2_ioc.h             02.00.13            02.00.12
- mpi2_raid.h            02.00.04            02.00.04
- mpi2_sas.h             02.00.03            02.00.02
- mpi2_targ.h            02.00.03            02.00.03
- mpi2_tool.h            02.00.04            02.00.04
- mpi2_type.h            02.00.00            02.00.00
- mpi2_ra.h              02.00.00            02.00.00
- mpi2_hbd.h             02.00.00
- mpi2_history.txt       02.00.14            02.00.13
-
-
- *  Date      Version   Description
- *  --------  --------  ------------------------------------------------------
-
-mpi2.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  06-04-07  02.00.01  Bumped MPI2_HEADER_VERSION_UNIT.
- *  06-26-07  02.00.02  Bumped MPI2_HEADER_VERSION_UNIT.
- *  08-31-07  02.00.03  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      Moved ReplyPostHostIndex register to offset 0x6C of the
- *                      MPI2_SYSTEM_INTERFACE_REGS and modified the define for
- *                      MPI2_REPLY_POST_HOST_INDEX_OFFSET.
- *                      Added union of request descriptors.
- *                      Added union of reply descriptors.
- *  10-31-07  02.00.04  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      Added define for MPI2_VERSION_02_00.
- *                      Fixed the size of the FunctionDependent5 field in the
- *                      MPI2_DEFAULT_REPLY structure.
- *  12-18-07  02.00.05  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      Removed the MPI-defined Fault Codes and extended the
- *                      product specific codes up to 0xEFFF.
- *                      Added a sixth key value for the WriteSequence register
- *                      and changed the flush value to 0x0.
- *                      Added message function codes for Diagnostic Buffer Post
- *                      and Diagnsotic Release.
- *                      New IOCStatus define: MPI2_IOCSTATUS_DIAGNOSTIC_RELEASED
- *                      Moved MPI2_VERSION_UNION from mpi2_ioc.h.
- *  02-29-08  02.00.06  Bumped MPI2_HEADER_VERSION_UNIT.
- *  03-03-08  02.00.07  Bumped MPI2_HEADER_VERSION_UNIT.
- *  05-21-08  02.00.08  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      Added #defines for marking a reply descriptor as unused.
- *  06-27-08  02.00.09  Bumped MPI2_HEADER_VERSION_UNIT.
- *  10-02-08  02.00.10  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      Moved LUN field defines from mpi2_init.h.
- *  01-19-09  02.00.11  Bumped MPI2_HEADER_VERSION_UNIT.
- *  05-06-09  02.00.12  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      In all request and reply descriptors, replaced VF_ID
- *                      field with MSIxIndex field.
- *                      Removed DevHandle field from
- *                      MPI2_SCSI_IO_SUCCESS_REPLY_DESCRIPTOR and made those
- *                      bytes reserved.
- *                      Added RAID Accelerator functionality.
- *  07-30-09  02.00.13  Bumped MPI2_HEADER_VERSION_UNIT.
- *  10-28-09  02.00.14  Bumped MPI2_HEADER_VERSION_UNIT.
- *                      Added MSI-x index mask and shift for Reply Post Host
- *                      Index register.
- *                      Added function code for Host Based Discovery Action.
- *  --------------------------------------------------------------------------
-
-mpi2_cnfg.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  06-04-07  02.00.01  Added defines for SAS IO Unit Page 2 PhyFlags.
- *                      Added Manufacturing Page 11.
- *                      Added MPI2_SAS_EXPANDER0_FLAGS_CONNECTOR_END_DEVICE
- *                      define.
- *  06-26-07  02.00.02  Adding generic structure for product-specific
- *                      Manufacturing pages: MPI2_CONFIG_PAGE_MANUFACTURING_PS.
- *                      Rework of BIOS Page 2 configuration page.
- *                      Fixed MPI2_BIOSPAGE2_BOOT_DEVICE to be a union of the
- *                      forms.
- *                      Added configuration pages IOC Page 8 and Driver
- *                      Persistent Mapping Page 0.
- *  08-31-07  02.00.03  Modified configuration pages dealing with Integrated
- *                      RAID (Manufacturing Page 4, RAID Volume Pages 0 and 1,
- *                      RAID Physical Disk Pages 0 and 1, RAID Configuration
- *                      Page 0).
- *                      Added new value for AccessStatus field of SAS Device
- *                      Page 0 (_SATA_NEEDS_INITIALIZATION).
- *  10-31-07  02.00.04  Added missing SEPDevHandle field to
- *                      MPI2_CONFIG_PAGE_SAS_ENCLOSURE_0.
- *  12-18-07  02.00.05  Modified IO Unit Page 0 to use 32-bit version fields for
- *                      NVDATA.
- *                      Modified IOC Page 7 to use masks and added field for
- *                      SASBroadcastPrimitiveMasks.
- *                      Added MPI2_CONFIG_PAGE_BIOS_4.
- *                      Added MPI2_CONFIG_PAGE_LOG_0.
- *  02-29-08  02.00.06  Modified various names to make them 32-character unique.
- *                      Added SAS Device IDs.
- *                      Updated Integrated RAID configuration pages including
- *                      Manufacturing Page 4, IOC Page 6, and RAID Configuration
- *                      Page 0.
- *  05-21-08  02.00.07  Added define MPI2_MANPAGE4_MIX_SSD_SAS_SATA.
- *                      Added define MPI2_MANPAGE4_PHYSDISK_128MB_COERCION.
- *                      Fixed define MPI2_IOCPAGE8_FLAGS_ENCLOSURE_SLOT_MAPPING.
- *                      Added missing MaxNumRoutedSasAddresses field to
- *                      MPI2_CONFIG_PAGE_EXPANDER_0.
- *                      Added SAS Port Page 0.
- *                      Modified structure layout for
- *                      MPI2_CONFIG_PAGE_DRIVER_MAPPING_0.
- *  06-27-08  02.00.08  Changed MPI2_CONFIG_PAGE_RD_PDISK_1 to use
- *                      MPI2_RAID_PHYS_DISK1_PATH_MAX to size the array.
- *  10-02-08  02.00.09  Changed MPI2_RAID_PGAD_CONFIGNUM_MASK from 0x0000FFFF
- *                      to 0x000000FF.
- *                      Added two new values for the Physical Disk Coercion Size
- *                      bits in the Flags field of Manufacturing Page 4.
- *                      Added product-specific Manufacturing pages 16 to 31.
- *                      Modified Flags bits for controlling write cache on SATA
- *                      drives in IO Unit Page 1.
- *                      Added new bit to AdditionalControlFlags of SAS IO Unit
- *                      Page 1 to control Invalid Topology Correction.
- *                      Added SupportedPhysDisks field to RAID Volume Page 1 and
- *                      added related defines.
- *                      Added additional defines for RAID Volume Page 0
- *                      VolumeStatusFlags field.
- *                      Modified meaning of RAID Volume Page 0 VolumeSettings
- *                      define for auto-configure of hot-swap drives.
- *                      Added PhysDiskAttributes field (and related defines) to
- *                      RAID Physical Disk Page 0.
- *                      Added MPI2_SAS_PHYINFO_PHY_VACANT define.
- *                      Added three new DiscoveryStatus bits for SAS IO Unit
- *                      Page 0 and SAS Expander Page 0.
- *                      Removed multiplexing information from SAS IO Unit pages.
- *                      Added BootDeviceWaitTime field to SAS IO Unit Page 4.
- *                      Removed Zone Address Resolved bit from PhyInfo and from
- *                      Expander Page 0 Flags field.
- *                      Added two new AccessStatus values to SAS Device Page 0
- *                      for indicating routing problems. Added 3 reserved words
- *                      to this page.
- *  01-19-09  02.00.10  Fixed defines for GPIOVal field of IO Unit Page 3.
- *                      Inserted missing reserved field into structure for IOC
- *                      Page 6.
- *                      Added more pending task bits to RAID Volume Page 0
- *                      VolumeStatusFlags defines.
- *                      Added MPI2_PHYSDISK0_STATUS_FLAG_NOT_CERTIFIED define.
- *                      Added a new DiscoveryStatus bit for SAS IO Unit Page 0
- *                      and SAS Expander Page 0 to flag a downstream initiator
- *                      when in simplified routing mode.
- *                      Removed SATA Init Failure defines for DiscoveryStatus
- *                      fields of SAS IO Unit Page 0 and SAS Expander Page 0.
- *                      Added MPI2_SAS_DEVICE0_ASTATUS_DEVICE_BLOCKED define.
- *                      Added PortGroups, DmaGroup, and ControlGroup fields to
- *                      SAS Device Page 0.
- *  05-06-09  02.00.11  Added structures and defines for IO Unit Page 5 and IO
- *                      Unit Page 6.
- *                      Added expander reduced functionality data to SAS
- *                      Expander Page 0.
- *                      Added SAS PHY Page 2 and SAS PHY Page 3.
- *  07-30-09  02.00.12  Added IO Unit Page 7.
- *                      Added new device ids.
- *                      Added SAS IO Unit Page 5.
- *                      Added partial and slumber power management capable flags
- *                      to SAS Device Page 0 Flags field.
- *                      Added PhyInfo defines for power condition.
- *                      Added Ethernet configuration pages.
- *  10-28-09  02.00.13  Added MPI2_IOUNITPAGE1_ENABLE_HOST_BASED_DISCOVERY.
- *                      Added SAS PHY Page 4 structure and defines.
- *  --------------------------------------------------------------------------
-
-mpi2_init.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  10-31-07  02.00.01  Fixed name for pMpi2SCSITaskManagementRequest_t.
- *  12-18-07  02.00.02  Modified Task Management Target Reset Method defines.
- *  02-29-08  02.00.03  Added Query Task Set and Query Unit Attention.
- *  03-03-08  02.00.04  Fixed name of struct _MPI2_SCSI_TASK_MANAGE_REPLY.
- *  05-21-08  02.00.05  Fixed typo in name of Mpi2SepRequest_t.
- *  10-02-08  02.00.06  Removed Untagged and No Disconnect values from SCSI IO
- *                      Control field Task Attribute flags.
- *                      Moved LUN field defines to mpi2.h becasue they are
- *                      common to many structures.
- *  05-06-09  02.00.07  Changed task management type of Query Unit Attention to
- *                      Query Asynchronous Event.
- *                      Defined two new bits in the SlotStatus field of the SCSI
- *                      Enclosure Processor Request and Reply.
- *  10-28-09  02.00.08  Added defines for decoding the ResponseInfo bytes for
- *                      both SCSI IO Error Reply and SCSI Task Management Reply.
- *                      Added ResponseInfo field to MPI2_SCSI_TASK_MANAGE_REPLY.
- *                      Added MPI2_SCSITASKMGMT_RSP_TM_OVERLAPPED_TAG define.
- *  --------------------------------------------------------------------------
-
-mpi2_ioc.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  06-04-07  02.00.01  In IOCFacts Reply structure, renamed MaxDevices to
- *                      MaxTargets.
- *                      Added TotalImageSize field to FWDownload Request.
- *                      Added reserved words to FWUpload Request.
- *  06-26-07  02.00.02  Added IR Configuration Change List Event.
- *  08-31-07  02.00.03  Removed SystemReplyQueueDepth field from the IOCInit
- *                      request and replaced it with
- *                      ReplyDescriptorPostQueueDepth and ReplyFreeQueueDepth.
- *                      Replaced the MinReplyQueueDepth field of the IOCFacts
- *                      reply with MaxReplyDescriptorPostQueueDepth.
- *                      Added MPI2_RDPQ_DEPTH_MIN define to specify the minimum
- *                      depth for the Reply Descriptor Post Queue.
- *                      Added SASAddress field to Initiator Device Table
- *                      Overflow Event data.
- *  10-31-07  02.00.04  Added ReasonCode MPI2_EVENT_SAS_INIT_RC_NOT_RESPONDING
- *                      for SAS Initiator Device Status Change Event data.
- *                      Modified Reason Code defines for SAS Topology Change
- *                      List Event data, including adding a bit for PHY Vacant
- *                      status, and adding a mask for the Reason Code.
- *                      Added define for
- *                      MPI2_EVENT_SAS_TOPO_ES_DELAY_NOT_RESPONDING.
- *                      Added define for MPI2_EXT_IMAGE_TYPE_MEGARAID.
- *  12-18-07  02.00.05  Added Boot Status defines for the IOCExceptions field of
- *                      the IOCFacts Reply.
- *                      Removed MPI2_IOCFACTS_CAPABILITY_EXTENDED_BUFFER define.
- *                      Moved MPI2_VERSION_UNION to mpi2.h.
- *                      Changed MPI2_EVENT_NOTIFICATION_REQUEST to use masks
- *                      instead of enables, and added SASBroadcastPrimitiveMasks
- *                      field.
- *                      Added Log Entry Added Event and related structure.
- *  02-29-08  02.00.06  Added define MPI2_IOCFACTS_CAPABILITY_INTEGRATED_RAID.
- *                      Removed define MPI2_IOCFACTS_PROTOCOL_SMP_TARGET.
- *                      Added MaxVolumes and MaxPersistentEntries fields to
- *                      IOCFacts reply.
- *                      Added ProtocalFlags and IOCCapabilities fields to
- *                      MPI2_FW_IMAGE_HEADER.
- *                      Removed MPI2_PORTENABLE_FLAGS_ENABLE_SINGLE_PORT.
- *  03-03-08  02.00.07  Fixed MPI2_FW_IMAGE_HEADER by changing Reserved26 to
- *                      a U16 (from a U32).
- *                      Removed extra 's' from EventMasks name.
- *  06-27-08  02.00.08  Fixed an offset in a comment.
- *  10-02-08  02.00.09  Removed SystemReplyFrameSize from MPI2_IOC_INIT_REQUEST.
- *                      Removed CurReplyFrameSize from MPI2_IOC_FACTS_REPLY and
- *                      renamed MinReplyFrameSize to ReplyFrameSize.
- *                      Added MPI2_IOCFACTS_EXCEPT_IR_FOREIGN_CONFIG_MAX.
- *                      Added two new RAIDOperation values for Integrated RAID
- *                      Operations Status Event data.
- *                      Added four new IR Configuration Change List Event data
- *                      ReasonCode values.
- *                      Added two new ReasonCode defines for SAS Device Status
- *                      Change Event data.
- *                      Added three new DiscoveryStatus bits for the SAS
- *                      Discovery event data.
- *                      Added Multiplexing Status Change bit to the PhyStatus
- *                      field of the SAS Topology Change List event data.
- *                      Removed define for MPI2_INIT_IMAGE_BOOTFLAGS_XMEMCOPY.
- *                      BootFlags are now product-specific.
- *                      Added defines for the indivdual signature bytes
- *                      for MPI2_INIT_IMAGE_FOOTER.
- *  01-19-09  02.00.10  Added MPI2_IOCFACTS_CAPABILITY_EVENT_REPLAY define.
- *                      Added MPI2_EVENT_SAS_DISC_DS_DOWNSTREAM_INITIATOR
- *                      define.
- *                      Added MPI2_EVENT_SAS_DEV_STAT_RC_SATA_INIT_FAILURE
- *                      define.
- *                      Removed MPI2_EVENT_SAS_DISC_DS_SATA_INIT_FAILURE define.
- *  05-06-09  02.00.11  Added MPI2_IOCFACTS_CAPABILITY_RAID_ACCELERATOR define.
- *                      Added MPI2_IOCFACTS_CAPABILITY_MSI_X_INDEX define.
- *                      Added two new reason codes for SAS Device Status Change
- *                      Event.
- *                      Added new event: SAS PHY Counter.
- *  07-30-09  02.00.12  Added GPIO Interrupt event define and structure.
- *                      Added MPI2_IOCFACTS_CAPABILITY_EXTENDED_BUFFER define.
- *                      Added new product id family for 2208.
- *  10-28-09  02.00.13  Added HostMSIxVectors field to MPI2_IOC_INIT_REQUEST.
- *                      Added MaxMSIxVectors field to MPI2_IOC_FACTS_REPLY.
- *                      Added MinDevHandle field to MPI2_IOC_FACTS_REPLY.
- *                      Added MPI2_IOCFACTS_CAPABILITY_HOST_BASED_DISCOVERY.
- *                      Added MPI2_EVENT_HOST_BASED_DISCOVERY_PHY define.
- *                      Added MPI2_EVENT_SAS_TOPO_ES_NO_EXPANDER define.
- *                      Added Host Based Discovery Phy Event data.
- *                      Added defines for ProductID Product field
- *                      (MPI2_FW_HEADER_PID_).
- *                      Modified values for SAS ProductID Family
- *                      (MPI2_FW_HEADER_PID_FAMILY_).
- *  --------------------------------------------------------------------------
-
-mpi2_raid.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  08-31-07  02.00.01  Modifications to RAID Action request and reply,
- *                      including the Actions and ActionData.
- *  02-29-08  02.00.02  Added MPI2_RAID_ACTION_ADATA_DISABL_FULL_REBUILD.
- *  05-21-08  02.00.03  Added MPI2_RAID_VOL_CREATION_NUM_PHYSDISKS so that
- *                      the PhysDisk array in MPI2_RAID_VOLUME_CREATION_STRUCT
- *                      can be sized by the build environment.
- *  07-30-09  02.00.04  Added proper define for the Use Default Settings bit of
- *                      VolumeCreationFlags and marked the old one as obsolete.
- *  05-12-10  02.00.05  Added MPI2_RAID_VOL_FLAGS_OP_MDC define.
- *  --------------------------------------------------------------------------
-
-mpi2_sas.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  06-26-07  02.00.01  Added Clear All Persistent Operation to SAS IO Unit
- *                      Control Request.
- *  10-02-08  02.00.02  Added Set IOC Parameter Operation to SAS IO Unit Control
- *                      Request.
- *  10-28-09  02.00.03  Changed the type of SGL in MPI2_SATA_PASSTHROUGH_REQUEST
- *                      to MPI2_SGE_IO_UNION since it supports chained SGLs.
- *  05-12-10  02.00.04  Modified some comments.
- *  --------------------------------------------------------------------------
-
-mpi2_targ.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  08-31-07  02.00.01  Added Command Buffer Data Location Address Space bits to
- *                      BufferPostFlags field of CommandBufferPostBase Request.
- *  02-29-08  02.00.02  Modified various names to make them 32-character unique.
- *  10-02-08  02.00.03  Removed NextCmdBufferOffset from
- *                      MPI2_TARGET_CMD_BUF_POST_BASE_REQUEST.
- *                      Target Status Send Request only takes a single SGE for
- *                      response data.
- *  --------------------------------------------------------------------------
-
-mpi2_tool.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  12-18-07  02.00.01  Added Diagnostic Buffer Post and Diagnostic Release
- *                      structures and defines.
- *  02-29-08  02.00.02  Modified various names to make them 32-character unique.
- *  05-06-09  02.00.03  Added ISTWI Read Write Tool and Diagnostic CLI Tool.
- *  07-30-09  02.00.04  Added ExtendedType field to DiagnosticBufferPost request
- *                      and reply messages.
- *                      Added MPI2_DIAG_BUF_TYPE_EXTENDED.
- *                      Incremented MPI2_DIAG_BUF_TYPE_COUNT.
- *  05-12-10  02.00.05  Added Diagnostic Data Upload tool.
- *  --------------------------------------------------------------------------
-
-mpi2_type.h
- *  04-30-07  02.00.00  Corresponds to Fusion-MPT MPI Specification Rev A.
- *  --------------------------------------------------------------------------
-
-mpi2_ra.h
- *  05-06-09  02.00.00  Initial version.
- *  --------------------------------------------------------------------------
-
-mpi2_hbd.h
- *  10-28-09  02.00.00  Initial version.
- *  --------------------------------------------------------------------------
-
-
-mpi2_history.txt         Parts list history
-
-Filename     02.00.14  02.00.13  02.00.12
-----------   --------  --------  --------
-mpi2.h       02.00.14  02.00.13  02.00.12
-mpi2_cnfg.h  02.00.13  02.00.12  02.00.11
-mpi2_init.h  02.00.08  02.00.07  02.00.07
-mpi2_ioc.h   02.00.13  02.00.12  02.00.11
-mpi2_raid.h  02.00.04  02.00.04  02.00.03
-mpi2_sas.h   02.00.03  02.00.02  02.00.02
-mpi2_targ.h  02.00.03  02.00.03  02.00.03
-mpi2_tool.h  02.00.04  02.00.04  02.00.03
-mpi2_type.h  02.00.00  02.00.00  02.00.00
-mpi2_ra.h    02.00.00  02.00.00  02.00.00
-mpi2_hbd.h   02.00.00
-
-Filename     02.00.11  02.00.10  02.00.09  02.00.08  02.00.07  02.00.06
-----------   --------  --------  --------  --------  --------  --------
-mpi2.h       02.00.11  02.00.10  02.00.09  02.00.08  02.00.07  02.00.06
-mpi2_cnfg.h  02.00.10  02.00.09  02.00.08  02.00.07  02.00.06  02.00.06
-mpi2_init.h  02.00.06  02.00.06  02.00.05  02.00.05  02.00.04  02.00.03
-mpi2_ioc.h   02.00.10  02.00.09  02.00.08  02.00.07  02.00.07  02.00.06
-mpi2_raid.h  02.00.03  02.00.03  02.00.03  02.00.03  02.00.02  02.00.02
-mpi2_sas.h   02.00.02  02.00.02  02.00.01  02.00.01  02.00.01  02.00.01
-mpi2_targ.h  02.00.03  02.00.03  02.00.02  02.00.02  02.00.02  02.00.02
-mpi2_tool.h  02.00.02  02.00.02  02.00.02  02.00.02  02.00.02  02.00.02
-mpi2_type.h  02.00.00  02.00.00  02.00.00  02.00.00  02.00.00  02.00.00
-
-Filename     02.00.05  02.00.04  02.00.03  02.00.02  02.00.01  02.00.00
-----------   --------  --------  --------  --------  --------  --------
-mpi2.h       02.00.05  02.00.04  02.00.03  02.00.02  02.00.01  02.00.00
-mpi2_cnfg.h  02.00.05  02.00.04  02.00.03  02.00.02  02.00.01  02.00.00
-mpi2_init.h  02.00.02  02.00.01  02.00.00  02.00.00  02.00.00  02.00.00
-mpi2_ioc.h   02.00.05  02.00.04  02.00.03  02.00.02  02.00.01  02.00.00
-mpi2_raid.h  02.00.01  02.00.01  02.00.01  02.00.00  02.00.00  02.00.00
-mpi2_sas.h   02.00.01  02.00.01  02.00.01  02.00.01  02.00.00  02.00.00
-mpi2_targ.h  02.00.01  02.00.01  02.00.01  02.00.00  02.00.00  02.00.00
-mpi2_tool.h  02.00.01  02.00.00  02.00.00  02.00.00  02.00.00  02.00.00
-mpi2_type.h  02.00.00  02.00.00  02.00.00  02.00.00  02.00.00  02.00.00
-
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_sas.h b/drivers/scsi/mpt2sas/mpi/mpi2_sas.h
index 608f6d6e6fca..fdffde1ebc0f 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_sas.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_sas.h
@@ -6,7 +6,7 @@
  *          Title:  MPI Serial Attached SCSI structures and definitions
  *  Creation Date:  February 9, 2007
  *
- *  mpi2_sas.h Version:  02.00.04
+ *  mpi2_sas.h Version:  02.00.05
  *
  *  Version History
  *  ---------------
@@ -21,6 +21,7 @@
  *  10-28-09  02.00.03  Changed the type of SGL in MPI2_SATA_PASSTHROUGH_REQUEST
  *                      to MPI2_SGE_IO_UNION since it supports chained SGLs.
  *  05-12-10  02.00.04  Modified some comments.
+ *  08-11-10  02.00.05  Added NCQ operations to SAS IO Unit Control.
  *  --------------------------------------------------------------------------
  */
 
@@ -163,7 +164,7 @@ typedef struct _MPI2_SATA_PASSTHROUGH_REQUEST
     U32                     Reserved4;          /* 0x14 */
     U32                     DataLength;         /* 0x18 */
     U8                      CommandFIS[20];     /* 0x1C */
-    MPI2_SGE_IO_UNION       SGL;                /* 0x20 */
+    MPI2_SGE_IO_UNION       SGL;                /* 0x30 */
 } MPI2_SATA_PASSTHROUGH_REQUEST, MPI2_POINTER PTR_MPI2_SATA_PASSTHROUGH_REQUEST,
   Mpi2SataPassthroughRequest_t, MPI2_POINTER pMpi2SataPassthroughRequest_t;
 
@@ -246,6 +247,8 @@ typedef struct _MPI2_SAS_IOUNIT_CONTROL_REQUEST
 #define MPI2_SAS_OP_REMOVE_DEVICE               (0x0D)
 #define MPI2_SAS_OP_LOOKUP_MAPPING              (0x0E)
 #define MPI2_SAS_OP_SET_IOC_PARAMETER           (0x0F)
+#define MPI2_SAS_OP_DEV_ENABLE_NCQ              (0x14)
+#define MPI2_SAS_OP_DEV_DISABLE_NCQ             (0x15)
 #define MPI2_SAS_OP_PRODUCT_SPECIFIC_MIN        (0x80)
 
 /* values for the PrimFlags field */
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_tool.h b/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
index 5c6e3a67bb94..2a4bceda364b 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
@@ -6,7 +6,7 @@
  *          Title:  MPI diagnostic tool structures and definitions
  *  Creation Date:  March 26, 2007
  *
- *    mpi2_tool.h Version:  02.00.05
+ *    mpi2_tool.h Version:  02.00.06
  *
  *  Version History
  *  ---------------
@@ -23,6 +23,8 @@
  *                      Added MPI2_DIAG_BUF_TYPE_EXTENDED.
  *                      Incremented MPI2_DIAG_BUF_TYPE_COUNT.
  *  05-12-10  02.00.05  Added Diagnostic Data Upload tool.
+ *  08-11-10  02.00.06  Added defines that were missing for Diagnostic Buffer
+ *                      Post Request.
  *  --------------------------------------------------------------------------
  */
 
@@ -354,6 +356,10 @@ typedef struct _MPI2_DIAG_BUFFER_POST_REQUEST
 /* count of the number of buffer types */
 #define MPI2_DIAG_BUF_TYPE_COUNT                    (0x03)
 
+/* values for the Flags field */
+#define MPI2_DIAG_BUF_FLAG_RELEASE_ON_FULL          (0x00000002)
+#define MPI2_DIAG_BUF_FLAG_IMMEDIATE_RELEASE        (0x00000001)
+
 
 /****************************************************************************
 *  Diagnostic Buffer Post reply
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index 9ead0399808a..e8a6f1cf1e4b 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -752,20 +752,19 @@ static u8
 _base_get_cb_idx(struct MPT2SAS_ADAPTER *ioc, u16 smid)
 {
 	int i;
-	u8 cb_idx = 0xFF;
-
-	if (smid >= ioc->hi_priority_smid) {
-		if (smid < ioc->internal_smid) {
-			i = smid - ioc->hi_priority_smid;
-			cb_idx = ioc->hpr_lookup[i].cb_idx;
-		} else if (smid <= ioc->hba_queue_depth)  {
-			i = smid - ioc->internal_smid;
-			cb_idx = ioc->internal_lookup[i].cb_idx;
-		}
-	} else {
+	u8 cb_idx;
+
+	if (smid < ioc->hi_priority_smid) {
 		i = smid - 1;
 		cb_idx = ioc->scsi_lookup[i].cb_idx;
-	}
+	} else if (smid < ioc->internal_smid) {
+		i = smid - ioc->hi_priority_smid;
+		cb_idx = ioc->hpr_lookup[i].cb_idx;
+	} else if (smid <= ioc->hba_queue_depth) {
+		i = smid - ioc->internal_smid;
+		cb_idx = ioc->internal_lookup[i].cb_idx;
+	} else
+		cb_idx = 0xFF;
 	return cb_idx;
 }
 
@@ -1430,7 +1429,7 @@ mpt2sas_base_get_smid_scsiio(struct MPT2SAS_ADAPTER *ioc, u8 cb_idx,
     struct scsi_cmnd *scmd)
 {
 	unsigned long flags;
-	struct request_tracker *request;
+	struct scsiio_tracker *request;
 	u16 smid;
 
 	spin_lock_irqsave(&ioc->scsi_lookup_lock, flags);
@@ -1442,7 +1441,7 @@ mpt2sas_base_get_smid_scsiio(struct MPT2SAS_ADAPTER *ioc, u8 cb_idx,
 	}
 
 	request = list_entry(ioc->free_list.next,
-	    struct request_tracker, tracker_list);
+	    struct scsiio_tracker, tracker_list);
 	request->scmd = scmd;
 	request->cb_idx = cb_idx;
 	smid = request->smid;
@@ -1496,48 +1495,47 @@ mpt2sas_base_free_smid(struct MPT2SAS_ADAPTER *ioc, u16 smid)
 	struct chain_tracker *chain_req, *next;
 
 	spin_lock_irqsave(&ioc->scsi_lookup_lock, flags);
-	if (smid >= ioc->hi_priority_smid) {
-		if (smid < ioc->internal_smid) {
-			/* hi-priority */
-			i = smid - ioc->hi_priority_smid;
-			ioc->hpr_lookup[i].cb_idx = 0xFF;
-			list_add_tail(&ioc->hpr_lookup[i].tracker_list,
-			    &ioc->hpr_free_list);
-		} else {
-			/* internal queue */
-			i = smid - ioc->internal_smid;
-			ioc->internal_lookup[i].cb_idx = 0xFF;
-			list_add_tail(&ioc->internal_lookup[i].tracker_list,
-			    &ioc->internal_free_list);
+	if (smid < ioc->hi_priority_smid) {
+		/* scsiio queue */
+		i = smid - 1;
+		if (!list_empty(&ioc->scsi_lookup[i].chain_list)) {
+			list_for_each_entry_safe(chain_req, next,
+			    &ioc->scsi_lookup[i].chain_list, tracker_list) {
+				list_del_init(&chain_req->tracker_list);
+				list_add_tail(&chain_req->tracker_list,
+				    &ioc->free_chain_list);
+			}
 		}
+		ioc->scsi_lookup[i].cb_idx = 0xFF;
+		ioc->scsi_lookup[i].scmd = NULL;
+		list_add_tail(&ioc->scsi_lookup[i].tracker_list,
+		    &ioc->free_list);
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
-		return;
-	}
 
-	/* scsiio queue */
-	i = smid - 1;
-	if (!list_empty(&ioc->scsi_lookup[i].chain_list)) {
-		list_for_each_entry_safe(chain_req, next,
-		    &ioc->scsi_lookup[i].chain_list, tracker_list) {
-			list_del_init(&chain_req->tracker_list);
-			list_add_tail(&chain_req->tracker_list,
-			    &ioc->free_chain_list);
+		/*
+		 * See _wait_for_commands_to_complete() call with regards
+		 * to this code.
+		 */
+		if (ioc->shost_recovery && ioc->pending_io_count) {
+			if (ioc->pending_io_count == 1)
+				wake_up(&ioc->reset_wq);
+			ioc->pending_io_count--;
 		}
+		return;
+	} else if (smid < ioc->internal_smid) {
+		/* hi-priority */
+		i = smid - ioc->hi_priority_smid;
+		ioc->hpr_lookup[i].cb_idx = 0xFF;
+		list_add_tail(&ioc->hpr_lookup[i].tracker_list,
+		    &ioc->hpr_free_list);
+	} else if (smid <= ioc->hba_queue_depth) {
+		/* internal queue */
+		i = smid - ioc->internal_smid;
+		ioc->internal_lookup[i].cb_idx = 0xFF;
+		list_add_tail(&ioc->internal_lookup[i].tracker_list,
+		    &ioc->internal_free_list);
 	}
-	ioc->scsi_lookup[i].cb_idx = 0xFF;
-	ioc->scsi_lookup[i].scmd = NULL;
-	list_add_tail(&ioc->scsi_lookup[i].tracker_list,
-	    &ioc->free_list);
 	spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
-
-	/*
-	 * See _wait_for_commands_to_complete() call with regards to this code.
-	 */
-	if (ioc->shost_recovery && ioc->pending_io_count) {
-		if (ioc->pending_io_count == 1)
-			wake_up(&ioc->reset_wq);
-		ioc->pending_io_count--;
-	}
 }
 
 /**
@@ -1725,6 +1723,31 @@ _base_display_dell_branding(struct MPT2SAS_ADAPTER *ioc)
 }
 
 /**
+ * _base_display_intel_branding - Display branding string
+ * @ioc: per adapter object
+ *
+ * Return nothing.
+ */
+static void
+_base_display_intel_branding(struct MPT2SAS_ADAPTER *ioc)
+{
+	if (ioc->pdev->subsystem_vendor == PCI_VENDOR_ID_INTEL &&
+	    ioc->pdev->device == MPI2_MFGPAGE_DEVID_SAS2008) {
+
+		switch (ioc->pdev->subsystem_device) {
+		case MPT2SAS_INTEL_RMS2LL080_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RMS2LL080_BRANDING);
+			break;
+		case MPT2SAS_INTEL_RMS2LL040_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RMS2LL040_BRANDING);
+			break;
+		}
+	}
+}
+
+/**
  * _base_display_ioc_capabilities - Disply IOC's capabilities.
  * @ioc: per adapter object
  *
@@ -1754,6 +1777,7 @@ _base_display_ioc_capabilities(struct MPT2SAS_ADAPTER *ioc)
 	    ioc->bios_pg3.BiosVersion & 0x000000FF);
 
 	_base_display_dell_branding(ioc);
+	_base_display_intel_branding(ioc);
 
 	printk(MPT2SAS_INFO_FMT "Protocol=(", ioc->name);
 
@@ -2252,9 +2276,9 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 	    ioc->name, (unsigned long long) ioc->request_dma));
 	total_sz += sz;
 
-	sz = ioc->scsiio_depth * sizeof(struct request_tracker);
+	sz = ioc->scsiio_depth * sizeof(struct scsiio_tracker);
 	ioc->scsi_lookup_pages = get_order(sz);
-	ioc->scsi_lookup = (struct request_tracker *)__get_free_pages(
+	ioc->scsi_lookup = (struct scsiio_tracker *)__get_free_pages(
 	    GFP_KERNEL, ioc->scsi_lookup_pages);
 	if (!ioc->scsi_lookup) {
 		printk(MPT2SAS_ERR_FMT "scsi_lookup: get_free_pages failed, "
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index 283568c6fb04..a3f8aa9baea4 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -69,8 +69,8 @@
 #define MPT2SAS_DRIVER_NAME		"mpt2sas"
 #define MPT2SAS_AUTHOR	"LSI Corporation <DL-MPTFusionLinux@lsi.com>"
 #define MPT2SAS_DESCRIPTION	"LSI MPT Fusion SAS 2.0 Device Driver"
-#define MPT2SAS_DRIVER_VERSION		"07.100.00.00"
-#define MPT2SAS_MAJOR_VERSION		07
+#define MPT2SAS_DRIVER_VERSION		"08.100.00.00"
+#define MPT2SAS_MAJOR_VERSION		08
 #define MPT2SAS_MINOR_VERSION		100
 #define MPT2SAS_BUILD_VERSION		00
 #define MPT2SAS_RELEASE_VERSION		00
@@ -101,7 +101,8 @@
 #define MPT_NAME_LENGTH			32	/* generic length of strings */
 #define MPT_STRING_LENGTH		64
 
-#define	MPT_MAX_CALLBACKS		16
+#define MPT_MAX_CALLBACKS		16
+
 
 #define	 CAN_SLEEP			1
 #define  NO_SLEEP			0
@@ -154,6 +155,20 @@
 #define MPT2SAS_DELL_6GBPS_SAS_SSDID               0x1F22
 
 /*
+ * Intel HBA branding
+ */
+#define MPT2SAS_INTEL_RMS2LL080_BRANDING	\
+				"Intel Integrated RAID Module RMS2LL080"
+#define MPT2SAS_INTEL_RMS2LL040_BRANDING	\
+				"Intel Integrated RAID Module RMS2LL040"
+
+/*
+ * Intel HBA SSDIDs
+ */
+#define MPT2SAS_INTEL_RMS2LL080_SSDID          0x350E
+#define MPT2SAS_INTEL_RMS2LL040_SSDID          0x350F
+
+/*
  * per target private data
  */
 #define MPT_TARGET_FLAGS_RAID_COMPONENT	0x01
@@ -431,14 +446,14 @@ struct chain_tracker {
 };
 
 /**
- * struct request_tracker - firmware request tracker
+ * struct scsiio_tracker - scsi mf request tracker
  * @smid: system message id
  * @scmd: scsi request pointer
  * @cb_idx: callback index
  * @chain_list: list of chains associated to this IO
  * @tracker_list: list of free request (ioc->free_list)
  */
-struct request_tracker {
+struct scsiio_tracker {
 	u16	smid;
 	struct scsi_cmnd *scmd;
 	u8	cb_idx;
@@ -447,6 +462,19 @@ struct request_tracker {
 };
 
 /**
+ * struct request_tracker - misc mf request tracker
+ * @smid: system message id
+ * @scmd: scsi request pointer
+ * @cb_idx: callback index
+ * @tracker_list: list of free request (ioc->free_list)
+ */
+struct request_tracker {
+	u16	smid;
+	u8	cb_idx;
+	struct list_head tracker_list;
+};
+
+/**
  * struct _tr_list - target reset list
  * @handle: device handle
  * @state: state machine
@@ -709,7 +737,7 @@ struct MPT2SAS_ADAPTER {
 	u8		*request;
 	dma_addr_t	request_dma;
 	u32		request_dma_sz;
-	struct request_tracker *scsi_lookup;
+	struct scsiio_tracker *scsi_lookup;
 	ulong		scsi_lookup_pages;
 	spinlock_t 	scsi_lookup_lock;
 	struct list_head free_list;
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index 5ded3db6e316..6ceb7759bfe5 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -6975,7 +6975,6 @@ _scsih_suspend(struct pci_dev *pdev, pm_message_t state)
 	u32 device_state;
 
 	mpt2sas_base_stop_watchdog(ioc);
-	flush_scheduled_work();
 	scsi_block_requests(shost);
 	device_state = pci_choose_state(pdev, state);
 	printk(MPT2SAS_INFO_FMT "pdev=0x%p, slot=%s, entering "
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index b37c8a3c1bb0..86afb13f1e79 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -1005,11 +1005,23 @@ int osd_req_read_sg(struct osd_request *or,
 	const struct osd_sg_entry *sglist, unsigned numentries)
 {
 	u64 len;
-	int ret = _add_sg_continuation_descriptor(or, sglist, numentries, &len);
+	u64 off;
+	int ret;
 
-	if (ret)
-		return ret;
-	osd_req_read(or, obj, 0, bio, len);
+	if (numentries > 1) {
+		off = 0;
+		ret = _add_sg_continuation_descriptor(or, sglist, numentries,
+						      &len);
+		if (ret)
+			return ret;
+	} else {
+		/* Optimize the case of single segment, read_sg is a
+		 * bidi operation.
+		 */
+		len = sglist->len;
+		off = sglist->offset;
+	}
+	osd_req_read(or, obj, off, bio, len);
 
 	return 0;
 }
diff --git a/drivers/scsi/osst.c b/drivers/scsi/osst.c
index 54de1d1af1a7..521e2182d45b 100644
--- a/drivers/scsi/osst.c
+++ b/drivers/scsi/osst.c
@@ -1484,7 +1484,7 @@ static int osst_read_back_buffer_and_rewrite(struct osst_tape * STp, struct osst
 	int			dbg              = debugging;
 #endif
 
-	if ((buffer = (unsigned char *)vmalloc((nframes + 1) * OS_DATA_SIZE)) == NULL)
+	if ((buffer = vmalloc((nframes + 1) * OS_DATA_SIZE)) == NULL)
 		return (-EIO);
 
 	printk(KERN_INFO "%s:I: Reading back %d frames from drive buffer%s\n",
@@ -2296,7 +2296,7 @@ static int osst_write_header(struct osst_tape * STp, struct osst_request ** aSRp
 	if (STp->raw) return 0;
 
 	if (STp->header_cache == NULL) {
-		if ((STp->header_cache = (os_header_t *)vmalloc(sizeof(os_header_t))) == NULL) {
+		if ((STp->header_cache = vmalloc(sizeof(os_header_t))) == NULL) {
 			printk(KERN_ERR "%s:E: Failed to allocate header cache\n", name);
 			return (-ENOMEM);
 		}
@@ -2484,7 +2484,7 @@ static int __osst_analyze_headers(struct osst_tape * STp, struct osst_request **
 				   name, ppos, update_frame_cntr);
 #endif
 		if (STp->header_cache == NULL) {
-			if ((STp->header_cache = (os_header_t *)vmalloc(sizeof(os_header_t))) == NULL) {
+			if ((STp->header_cache = vmalloc(sizeof(os_header_t))) == NULL) {
 				printk(KERN_ERR "%s:E: Failed to allocate header cache\n", name);
 				return 0;
 			}
@@ -5851,9 +5851,7 @@ static int osst_probe(struct device *dev)
 	/* if this is the first attach, build the infrastructure */
 	write_lock(&os_scsi_tapes_lock);
 	if (os_scsi_tapes == NULL) {
-		os_scsi_tapes =
-			(struct osst_tape **)kmalloc(osst_max_dev * sizeof(struct osst_tape *),
-				   GFP_ATOMIC);
+		os_scsi_tapes = kmalloc(osst_max_dev * sizeof(struct osst_tape *), GFP_ATOMIC);
 		if (os_scsi_tapes == NULL) {
 			write_unlock(&os_scsi_tapes_lock);
 			printk(KERN_ERR "osst :E: Unable to allocate array for OnStream SCSI tapes.\n");
diff --git a/drivers/scsi/pm8001/pm8001_hwi.c b/drivers/scsi/pm8001/pm8001_hwi.c
index d8db0137c0c7..18b6c55cd08c 100644
--- a/drivers/scsi/pm8001/pm8001_hwi.c
+++ b/drivers/scsi/pm8001/pm8001_hwi.c
@@ -1382,53 +1382,50 @@ static u32 mpi_msg_consume(struct pm8001_hba_info *pm8001_ha,
 	return MPI_IO_STATUS_BUSY;
 }
 
-static void pm8001_work_queue(struct work_struct *work)
+static void pm8001_work_fn(struct work_struct *work)
 {
-	struct delayed_work *dw = container_of(work, struct delayed_work, work);
-	struct pm8001_wq *wq = container_of(dw, struct pm8001_wq, work_q);
+	struct pm8001_work *pw = container_of(work, struct pm8001_work, work);
 	struct pm8001_device *pm8001_dev;
-	struct domain_device	*dev;
+	struct domain_device *dev;
 
-	switch (wq->handler) {
+	switch (pw->handler) {
 	case IO_OPEN_CNX_ERROR_IT_NEXUS_LOSS:
-		pm8001_dev = wq->data;
+		pm8001_dev = pw->data;
 		dev = pm8001_dev->sas_device;
 		pm8001_I_T_nexus_reset(dev);
 		break;
 	case IO_OPEN_CNX_ERROR_STP_RESOURCES_BUSY:
-		pm8001_dev = wq->data;
+		pm8001_dev = pw->data;
 		dev = pm8001_dev->sas_device;
 		pm8001_I_T_nexus_reset(dev);
 		break;
 	case IO_DS_IN_ERROR:
-		pm8001_dev = wq->data;
+		pm8001_dev = pw->data;
 		dev = pm8001_dev->sas_device;
 		pm8001_I_T_nexus_reset(dev);
 		break;
 	case IO_DS_NON_OPERATIONAL:
-		pm8001_dev = wq->data;
+		pm8001_dev = pw->data;
 		dev = pm8001_dev->sas_device;
 		pm8001_I_T_nexus_reset(dev);
 		break;
 	}
-	list_del(&wq->entry);
-	kfree(wq);
+	kfree(pw);
 }
 
 static int pm8001_handle_event(struct pm8001_hba_info *pm8001_ha, void *data,
 			       int handler)
 {
-	struct pm8001_wq *wq;
+	struct pm8001_work *pw;
 	int ret = 0;
 
-	wq = kmalloc(sizeof(struct pm8001_wq), GFP_ATOMIC);
-	if (wq) {
-		wq->pm8001_ha = pm8001_ha;
-		wq->data = data;
-		wq->handler = handler;
-		INIT_DELAYED_WORK(&wq->work_q, pm8001_work_queue);
-		list_add_tail(&wq->entry, &pm8001_ha->wq_list);
-		schedule_delayed_work(&wq->work_q, 0);
+	pw = kmalloc(sizeof(struct pm8001_work), GFP_ATOMIC);
+	if (pw) {
+		pw->pm8001_ha = pm8001_ha;
+		pw->data = data;
+		pw->handler = handler;
+		INIT_WORK(&pw->work, pm8001_work_fn);
+		queue_work(pm8001_wq, &pw->work);
 	} else
 		ret = -ENOMEM;
 
diff --git a/drivers/scsi/pm8001/pm8001_init.c b/drivers/scsi/pm8001/pm8001_init.c
index b95285f3383f..002360da01e3 100644
--- a/drivers/scsi/pm8001/pm8001_init.c
+++ b/drivers/scsi/pm8001/pm8001_init.c
@@ -51,6 +51,8 @@ static int pm8001_id;
 
 LIST_HEAD(hba_list);
 
+struct workqueue_struct *pm8001_wq;
+
 /**
  * The main structure which LLDD must register for scsi core.
  */
@@ -134,7 +136,6 @@ static void __devinit pm8001_phy_init(struct pm8001_hba_info *pm8001_ha,
 static void pm8001_free(struct pm8001_hba_info *pm8001_ha)
 {
 	int i;
-	struct pm8001_wq *wq;
 
 	if (!pm8001_ha)
 		return;
@@ -150,8 +151,7 @@ static void pm8001_free(struct pm8001_hba_info *pm8001_ha)
 	PM8001_CHIP_DISP->chip_iounmap(pm8001_ha);
 	if (pm8001_ha->shost)
 		scsi_host_put(pm8001_ha->shost);
-	list_for_each_entry(wq, &pm8001_ha->wq_list, entry)
-		cancel_delayed_work(&wq->work_q);
+	flush_workqueue(pm8001_wq);
 	kfree(pm8001_ha->tags);
 	kfree(pm8001_ha);
 }
@@ -381,7 +381,6 @@ pm8001_pci_alloc(struct pci_dev *pdev, u32 chip_id, struct Scsi_Host *shost)
 	pm8001_ha->sas = sha;
 	pm8001_ha->shost = shost;
 	pm8001_ha->id = pm8001_id++;
-	INIT_LIST_HEAD(&pm8001_ha->wq_list);
 	pm8001_ha->logging_level = 0x01;
 	sprintf(pm8001_ha->name, "%s%d", DRV_NAME, pm8001_ha->id);
 #ifdef PM8001_USE_TASKLET
@@ -758,7 +757,7 @@ static int pm8001_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 	int i , pos;
 	u32 device_state;
 	pm8001_ha = sha->lldd_ha;
-	flush_scheduled_work();
+	flush_workqueue(pm8001_wq);
 	scsi_block_requests(pm8001_ha->shost);
 	pos = pci_find_capability(pdev, PCI_CAP_ID_PM);
 	if (pos == 0) {
@@ -870,17 +869,26 @@ static struct pci_driver pm8001_pci_driver = {
  */
 static int __init pm8001_init(void)
 {
-	int rc;
+	int rc = -ENOMEM;
+
+	pm8001_wq = alloc_workqueue("pm8001", 0, 0);
+	if (!pm8001_wq)
+		goto err;
+
 	pm8001_id = 0;
 	pm8001_stt = sas_domain_attach_transport(&pm8001_transport_ops);
 	if (!pm8001_stt)
-		return -ENOMEM;
+		goto err_wq;
 	rc = pci_register_driver(&pm8001_pci_driver);
 	if (rc)
-		goto err_out;
+		goto err_tp;
 	return 0;
-err_out:
+
+err_tp:
 	sas_release_transport(pm8001_stt);
+err_wq:
+	destroy_workqueue(pm8001_wq);
+err:
 	return rc;
 }
 
@@ -888,6 +896,7 @@ static void __exit pm8001_exit(void)
 {
 	pci_unregister_driver(&pm8001_pci_driver);
 	sas_release_transport(pm8001_stt);
+	destroy_workqueue(pm8001_wq);
 }
 
 module_init(pm8001_init);
diff --git a/drivers/scsi/pm8001/pm8001_sas.h b/drivers/scsi/pm8001/pm8001_sas.h
index 7f064f9ca828..bdb6b27dedd6 100644
--- a/drivers/scsi/pm8001/pm8001_sas.h
+++ b/drivers/scsi/pm8001/pm8001_sas.h
@@ -50,6 +50,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/pci.h>
 #include <linux/interrupt.h>
+#include <linux/workqueue.h>
 #include <scsi/libsas.h>
 #include <scsi/scsi_tcq.h>
 #include <scsi/sas_ata.h>
@@ -379,18 +380,16 @@ struct pm8001_hba_info {
 #ifdef PM8001_USE_TASKLET
 	struct tasklet_struct	tasklet;
 #endif
-	struct list_head 	wq_list;
 	u32			logging_level;
 	u32			fw_status;
 	const struct firmware 	*fw_image;
 };
 
-struct pm8001_wq {
-	struct delayed_work work_q;
+struct pm8001_work {
+	struct work_struct work;
 	struct pm8001_hba_info *pm8001_ha;
 	void *data;
 	int handler;
-	struct list_head entry;
 };
 
 struct pm8001_fw_image_header {
@@ -460,6 +459,9 @@ struct fw_control_ex {
 	void			*param3;
 };
 
+/* pm8001 workqueue */
+extern struct workqueue_struct *pm8001_wq;
+
 /******************** function prototype *********************/
 int pm8001_tag_alloc(struct pm8001_hba_info *pm8001_ha, u32 *tag_out);
 void pm8001_tag_init(struct pm8001_hba_info *pm8001_ha);
diff --git a/drivers/scsi/pmcraid.c b/drivers/scsi/pmcraid.c
index 321cf3ae8630..bcf858e88c64 100644
--- a/drivers/scsi/pmcraid.c
+++ b/drivers/scsi/pmcraid.c
@@ -5454,7 +5454,7 @@ static void __devexit pmcraid_remove(struct pci_dev *pdev)
 	pmcraid_shutdown(pdev);
 
 	pmcraid_disable_interrupts(pinstance, ~0);
-	flush_scheduled_work();
+	flush_work_sync(&pinstance->worker_q);
 
 	pmcraid_kill_tasklets(pinstance);
 	pmcraid_unregister_interrupt_handler(pinstance);
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index ccfc8e78be21..6c51c0a35b9e 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -2402,13 +2402,13 @@ struct qla_hw_data {
 	volatile struct {
 		uint32_t	mbox_int		:1;
 		uint32_t	mbox_busy		:1;
-
 		uint32_t	disable_risc_code_load	:1;
 		uint32_t	enable_64bit_addressing	:1;
 		uint32_t	enable_lip_reset	:1;
 		uint32_t	enable_target_reset	:1;
 		uint32_t	enable_lip_full_login	:1;
 		uint32_t	enable_led_scheme	:1;
+
 		uint32_t	msi_enabled		:1;
 		uint32_t	msix_enabled		:1;
 		uint32_t	disable_serdes		:1;
@@ -2417,6 +2417,7 @@ struct qla_hw_data {
 		uint32_t	pci_channel_io_perm_failure	:1;
 		uint32_t	fce_enabled		:1;
 		uint32_t	fac_supported		:1;
+
 		uint32_t	chip_reset_done		:1;
 		uint32_t	port0			:1;
 		uint32_t	running_gold_fw		:1;
@@ -2424,9 +2425,11 @@ struct qla_hw_data {
 		uint32_t	cpu_affinity_enabled	:1;
 		uint32_t	disable_msix_handshake	:1;
 		uint32_t	fcp_prio_enabled	:1;
-		uint32_t	fw_hung	:1;
-		uint32_t        quiesce_owner:1;
+		uint32_t	isp82xx_fw_hung:1;
+
+		uint32_t	quiesce_owner:1;
 		uint32_t	thermal_supported:1;
+		uint32_t	isp82xx_reset_hdlr_active:1;
 		/* 26 bits */
 	} flags;
 
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index 89e900adb679..d48326ee3f61 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -565,6 +565,7 @@ extern int qla82xx_mbx_intr_enable(scsi_qla_host_t *);
 extern int qla82xx_mbx_intr_disable(scsi_qla_host_t *);
 extern void qla82xx_start_iocbs(srb_t *);
 extern int qla82xx_fcoe_ctx_reset(scsi_qla_host_t *);
+extern void qla82xx_chip_reset_cleanup(scsi_qla_host_t *);
 
 /* BSG related functions */
 extern int qla24xx_bsg_request(struct fc_bsg_job *);
diff --git a/drivers/scsi/qla2xxx/qla_gs.c b/drivers/scsi/qla2xxx/qla_gs.c
index 4c083928c2fb..74a91b6dfc68 100644
--- a/drivers/scsi/qla2xxx/qla_gs.c
+++ b/drivers/scsi/qla2xxx/qla_gs.c
@@ -121,8 +121,11 @@ qla2x00_chk_ms_status(scsi_qla_host_t *vha, ms_iocb_entry_t *ms_pkt,
 
 	rval = QLA_FUNCTION_FAILED;
 	if (ms_pkt->entry_status != 0) {
-		DEBUG2_3(printk("scsi(%ld): %s failed, error status (%x).\n",
-		    vha->host_no, routine, ms_pkt->entry_status));
+		DEBUG2_3(printk(KERN_WARNING "scsi(%ld): %s failed, error status "
+		    "(%x) on port_id: %02x%02x%02x.\n",
+		    vha->host_no, routine, ms_pkt->entry_status,
+		    vha->d_id.b.domain, vha->d_id.b.area,
+		    vha->d_id.b.al_pa));
 	} else {
 		if (IS_FWI2_CAPABLE(ha))
 			comp_status = le16_to_cpu(
@@ -136,8 +139,10 @@ qla2x00_chk_ms_status(scsi_qla_host_t *vha, ms_iocb_entry_t *ms_pkt,
 			if (ct_rsp->header.response !=
 			    __constant_cpu_to_be16(CT_ACCEPT_RESPONSE)) {
 				DEBUG2_3(printk("scsi(%ld): %s failed, "
-				    "rejected request:\n", vha->host_no,
-				    routine));
+				    "rejected request on port_id: %02x%02x%02x\n",
+				    vha->host_no, routine,
+				    vha->d_id.b.domain, vha->d_id.b.area,
+				    vha->d_id.b.al_pa));
 				DEBUG2_3(qla2x00_dump_buffer(
 				    (uint8_t *)&ct_rsp->header,
 				    sizeof(struct ct_rsp_hdr)));
@@ -147,8 +152,10 @@ qla2x00_chk_ms_status(scsi_qla_host_t *vha, ms_iocb_entry_t *ms_pkt,
 			break;
 		default:
 			DEBUG2_3(printk("scsi(%ld): %s failed, completion "
-			    "status (%x).\n", vha->host_no, routine,
-			    comp_status));
+			    "status (%x) on port_id: %02x%02x%02x.\n",
+			    vha->host_no, routine, comp_status,
+			    vha->d_id.b.domain, vha->d_id.b.area,
+			    vha->d_id.b.al_pa));
 			break;
 		}
 	}
@@ -1965,7 +1972,7 @@ qla2x00_gff_id(scsi_qla_host_t *vha, sw_info_t *list)
 			    "scsi(%ld): GFF_ID issue IOCB failed "
 			    "(%d).\n", vha->host_no, rval));
 		} else if (qla2x00_chk_ms_status(vha, ms_pkt, ct_rsp,
-			       "GPN_ID") != QLA_SUCCESS) {
+			       "GFF_ID") != QLA_SUCCESS) {
 			DEBUG2_3(printk(KERN_INFO
 			    "scsi(%ld): GFF_ID IOCB status had a "
 			    "failure status code\n", vha->host_no));
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index d9479c3fe5f8..8575808dbae0 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -1967,7 +1967,7 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 		} else {
 			/* Mailbox cmd failed. Timeout on min_wait. */
 			if (time_after_eq(jiffies, mtime) ||
-			    (IS_QLA82XX(ha) && ha->flags.fw_hung))
+				ha->flags.isp82xx_fw_hung)
 				break;
 		}
 
@@ -3945,8 +3945,13 @@ qla2x00_abort_isp_cleanup(scsi_qla_host_t *vha)
 	struct qla_hw_data *ha = vha->hw;
 	struct scsi_qla_host *vp;
 	unsigned long flags;
+	fc_port_t *fcport;
 
-	vha->flags.online = 0;
+	/* For ISP82XX, driver waits for completion of the commands.
+	 * online flag should be set.
+	 */
+	if (!IS_QLA82XX(ha))
+		vha->flags.online = 0;
 	ha->flags.chip_reset_done = 0;
 	clear_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
 	ha->qla_stats.total_isp_aborts++;
@@ -3954,7 +3959,10 @@ qla2x00_abort_isp_cleanup(scsi_qla_host_t *vha)
 	qla_printk(KERN_INFO, ha,
 	    "Performing ISP error recovery - ha= %p.\n", ha);
 
-	/* Chip reset does not apply to 82XX */
+	/* For ISP82XX, reset_chip is just disabling interrupts.
+	 * Driver waits for the completion of the commands.
+	 * the interrupts need to be enabled.
+	 */
 	if (!IS_QLA82XX(ha))
 		ha->isp_ops->reset_chip(vha);
 
@@ -3980,14 +3988,31 @@ qla2x00_abort_isp_cleanup(scsi_qla_host_t *vha)
 			    LOOP_DOWN_TIME);
 	}
 
+	/* Clear all async request states across all VPs. */
+	list_for_each_entry(fcport, &vha->vp_fcports, list)
+		fcport->flags &= ~(FCF_LOGIN_NEEDED | FCF_ASYNC_SENT);
+	spin_lock_irqsave(&ha->vport_slock, flags);
+	list_for_each_entry(vp, &ha->vp_list, list) {
+		atomic_inc(&vp->vref_count);
+		spin_unlock_irqrestore(&ha->vport_slock, flags);
+
+		list_for_each_entry(fcport, &vp->vp_fcports, list)
+			fcport->flags &= ~(FCF_LOGIN_NEEDED | FCF_ASYNC_SENT);
+
+		spin_lock_irqsave(&ha->vport_slock, flags);
+		atomic_dec(&vp->vref_count);
+	}
+	spin_unlock_irqrestore(&ha->vport_slock, flags);
+
 	if (!ha->flags.eeh_busy) {
 		/* Make sure for ISP 82XX IO DMA is complete */
 		if (IS_QLA82XX(ha)) {
-			if (qla2x00_eh_wait_for_pending_commands(vha, 0, 0,
-				WAIT_HOST) == QLA_SUCCESS) {
-				DEBUG2(qla_printk(KERN_INFO, ha,
-				"Done wait for pending commands\n"));
-			}
+			qla82xx_chip_reset_cleanup(vha);
+
+			/* Done waiting for pending commands.
+			 * Reset the online flag.
+			 */
+			vha->flags.online = 0;
 		}
 
 		/* Requeue all commands in outstanding command list. */
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index 4c1ba6263eb3..d78d5896fc33 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -328,6 +328,7 @@ qla2x00_start_scsi(srb_t *sp)
 	struct qla_hw_data *ha;
 	struct req_que *req;
 	struct rsp_que *rsp;
+	char		tag[2];
 
 	/* Setup device pointers. */
 	ret = 0;
@@ -406,7 +407,22 @@ qla2x00_start_scsi(srb_t *sp)
 	cmd_pkt->lun = cpu_to_le16(sp->cmd->device->lun);
 
 	/* Update tagged queuing modifier */
-	cmd_pkt->control_flags = __constant_cpu_to_le16(CF_SIMPLE_TAG);
+	if (scsi_populate_tag_msg(cmd, tag)) {
+		switch (tag[0]) {
+		case HEAD_OF_QUEUE_TAG:
+			cmd_pkt->control_flags =
+			    __constant_cpu_to_le16(CF_HEAD_TAG);
+			break;
+		case ORDERED_QUEUE_TAG:
+			cmd_pkt->control_flags =
+			    __constant_cpu_to_le16(CF_ORDERED_TAG);
+			break;
+		default:
+			cmd_pkt->control_flags =
+			    __constant_cpu_to_le16(CF_SIMPLE_TAG);
+			break;
+		}
+	}
 
 	/* Load SCSI command packet. */
 	memcpy(cmd_pkt->scsi_cdb, cmd->cmnd, cmd->cmd_len);
@@ -971,6 +987,7 @@ qla24xx_build_scsi_crc_2_iocbs(srb_t *sp, struct cmd_type_crc_2 *cmd_pkt,
 	uint16_t		fcp_cmnd_len;
 	struct fcp_cmnd		*fcp_cmnd;
 	dma_addr_t		crc_ctx_dma;
+	char			tag[2];
 
 	cmd = sp->cmd;
 
@@ -1068,9 +1085,27 @@ qla24xx_build_scsi_crc_2_iocbs(srb_t *sp, struct cmd_type_crc_2 *cmd_pkt,
 	    LSD(crc_ctx_dma + CRC_CONTEXT_FCPCMND_OFF));
 	cmd_pkt->fcp_cmnd_dseg_address[1] = cpu_to_le32(
 	    MSD(crc_ctx_dma + CRC_CONTEXT_FCPCMND_OFF));
-	fcp_cmnd->task_attribute = 0;
 	fcp_cmnd->task_management = 0;
 
+	/*
+	 * Update tagged queuing modifier if using command tag queuing
+	 */
+	if (scsi_populate_tag_msg(cmd, tag)) {
+		switch (tag[0]) {
+		case HEAD_OF_QUEUE_TAG:
+		    fcp_cmnd->task_attribute = TSK_HEAD_OF_QUEUE;
+		    break;
+		case ORDERED_QUEUE_TAG:
+		    fcp_cmnd->task_attribute = TSK_ORDERED;
+		    break;
+		default:
+		    fcp_cmnd->task_attribute = 0;
+		    break;
+		}
+	} else {
+		fcp_cmnd->task_attribute = 0;
+	}
+
 	cmd_pkt->fcp_rsp_dseg_len = 0; /* Let response come in status iocb */
 
 	DEBUG18(printk(KERN_INFO "%s(%ld): Total SG(s) Entries %d, Data"
@@ -1177,6 +1212,7 @@ qla24xx_start_scsi(srb_t *sp)
 	struct scsi_cmnd *cmd = sp->cmd;
 	struct scsi_qla_host *vha = sp->fcport->vha;
 	struct qla_hw_data *ha = vha->hw;
+	char		tag[2];
 
 	/* Setup device pointers. */
 	ret = 0;
@@ -1260,6 +1296,18 @@ qla24xx_start_scsi(srb_t *sp)
 	int_to_scsilun(sp->cmd->device->lun, &cmd_pkt->lun);
 	host_to_fcp_swap((uint8_t *)&cmd_pkt->lun, sizeof(cmd_pkt->lun));
 
+	/* Update tagged queuing modifier -- default is TSK_SIMPLE (0). */
+	if (scsi_populate_tag_msg(cmd, tag)) {
+		switch (tag[0]) {
+		case HEAD_OF_QUEUE_TAG:
+			cmd_pkt->task = TSK_HEAD_OF_QUEUE;
+			break;
+		case ORDERED_QUEUE_TAG:
+			cmd_pkt->task = TSK_ORDERED;
+			break;
+		}
+	}
+
 	/* Load SCSI command packet. */
 	memcpy(cmd_pkt->fcp_cdb, cmd->cmnd, cmd->cmd_len);
 	host_to_fcp_swap(cmd_pkt->fcp_cdb, sizeof(cmd_pkt->fcp_cdb));
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index e473e9fb363c..7a7c0ecfe7dd 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -71,6 +71,13 @@ qla2x00_mailbox_command(scsi_qla_host_t *vha, mbx_cmd_t *mcp)
 		return QLA_FUNCTION_TIMEOUT;
 	}
 
+	if (ha->flags.isp82xx_fw_hung) {
+		/* Setting Link-Down error */
+		mcp->mb[0] = MBS_LINK_DOWN_ERROR;
+		rval = QLA_FUNCTION_FAILED;
+		goto premature_exit;
+	}
+
 	/*
 	 * Wait for active mailbox commands to finish by waiting at most tov
 	 * seconds. This is to serialize actual issuing of mailbox cmds during
@@ -83,13 +90,6 @@ qla2x00_mailbox_command(scsi_qla_host_t *vha, mbx_cmd_t *mcp)
 		return QLA_FUNCTION_TIMEOUT;
 	}
 
-	if (IS_QLA82XX(ha) && ha->flags.fw_hung) {
-		/* Setting Link-Down error */
-		mcp->mb[0] = MBS_LINK_DOWN_ERROR;
-		rval = QLA_FUNCTION_FAILED;
-		goto premature_exit;
-	}
-
 	ha->flags.mbox_busy = 1;
 	/* Save mailbox command for debug */
 	ha->mcp = mcp;
@@ -223,7 +223,7 @@ qla2x00_mailbox_command(scsi_qla_host_t *vha, mbx_cmd_t *mcp)
 		ha->flags.mbox_int = 0;
 		clear_bit(MBX_INTERRUPT, &ha->mbx_cmd_flags);
 
-		if (IS_QLA82XX(ha) && ha->flags.fw_hung) {
+		if (ha->flags.isp82xx_fw_hung) {
 			ha->flags.mbox_busy = 0;
 			/* Setting Link-Down error */
 			mcp->mb[0] = MBS_LINK_DOWN_ERROR;
@@ -2462,22 +2462,19 @@ __qla24xx_issue_tmf(char *name, uint32_t type, struct fc_port *fcport,
 		    "-- completion status (%x).\n", __func__,
 		    vha->host_no, le16_to_cpu(sts->comp_status)));
 		rval = QLA_FUNCTION_FAILED;
-	} else if (!(le16_to_cpu(sts->scsi_status) &
-	    SS_RESPONSE_INFO_LEN_VALID)) {
-		DEBUG2_3_11(printk("%s(%ld): failed to complete IOCB "
-		    "-- no response info (%x).\n", __func__, vha->host_no,
-		    le16_to_cpu(sts->scsi_status)));
-		rval = QLA_FUNCTION_FAILED;
-	} else if (le32_to_cpu(sts->rsp_data_len) < 4) {
-		DEBUG2_3_11(printk("%s(%ld): failed to complete IOCB "
-		    "-- not enough response info (%d).\n", __func__,
-		    vha->host_no, le32_to_cpu(sts->rsp_data_len)));
-		rval = QLA_FUNCTION_FAILED;
-	} else if (sts->data[3]) {
-		DEBUG2_3_11(printk("%s(%ld): failed to complete IOCB "
-		    "-- response (%x).\n", __func__,
-		    vha->host_no, sts->data[3]));
-		rval = QLA_FUNCTION_FAILED;
+	} else if (le16_to_cpu(sts->scsi_status) &
+	    SS_RESPONSE_INFO_LEN_VALID) {
+		if (le32_to_cpu(sts->rsp_data_len) < 4) {
+			DEBUG2_3_11(printk("%s(%ld): ignoring inconsistent "
+			    "data length -- not enough response info (%d).\n",
+			    __func__, vha->host_no,
+			    le32_to_cpu(sts->rsp_data_len)));
+		} else if (sts->data[3]) {
+			DEBUG2_3_11(printk("%s(%ld): failed to complete IOCB "
+			    "-- response (%x).\n", __func__,
+			    vha->host_no, sts->data[3]));
+			rval = QLA_FUNCTION_FAILED;
+		}
 	}
 
 	/* Issue marker IOCB. */
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index fdb96a3584a5..76ec876e6b21 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -7,6 +7,7 @@
 #include "qla_def.h"
 #include <linux/delay.h>
 #include <linux/pci.h>
+#include <scsi/scsi_tcq.h>
 
 #define MASK(n)			((1ULL<<(n))-1)
 #define MN_WIN(addr) (((addr & 0x1fc0000) >> 1) | \
@@ -2547,7 +2548,7 @@ qla2xx_build_scsi_type_6_iocbs(srb_t *sp, struct cmd_type_6 *cmd_pkt,
 			dsd_seg = (uint32_t *)&cmd_pkt->fcp_data_dseg_address;
 			*dsd_seg++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
 			*dsd_seg++ = cpu_to_le32(MSD(dsd_ptr->dsd_list_dma));
-			*dsd_seg++ = dsd_list_len;
+			cmd_pkt->fcp_data_dseg_len = dsd_list_len;
 		} else {
 			*cur_dsd++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
 			*cur_dsd++ = cpu_to_le32(MSD(dsd_ptr->dsd_list_dma));
@@ -2620,6 +2621,7 @@ qla82xx_start_scsi(srb_t *sp)
 	struct qla_hw_data *ha = vha->hw;
 	struct req_que *req = NULL;
 	struct rsp_que *rsp = NULL;
+	char		tag[2];
 
 	/* Setup device pointers. */
 	ret = 0;
@@ -2770,6 +2772,22 @@ sufficient_dsds:
 		int_to_scsilun(sp->cmd->device->lun, &cmd_pkt->lun);
 		host_to_fcp_swap((uint8_t *)&cmd_pkt->lun, sizeof(cmd_pkt->lun));
 
+		/*
+		 * Update tagged queuing modifier -- default is TSK_SIMPLE (0).
+		 */
+		if (scsi_populate_tag_msg(cmd, tag)) {
+			switch (tag[0]) {
+			case HEAD_OF_QUEUE_TAG:
+				ctx->fcp_cmnd->task_attribute =
+				    TSK_HEAD_OF_QUEUE;
+				break;
+			case ORDERED_QUEUE_TAG:
+				ctx->fcp_cmnd->task_attribute =
+				    TSK_ORDERED;
+				break;
+			}
+		}
+
 		/* build FCP_CMND IU */
 		memset(ctx->fcp_cmnd, 0, sizeof(struct fcp_cmnd));
 		int_to_scsilun(sp->cmd->device->lun, &ctx->fcp_cmnd->lun);
@@ -2835,6 +2853,20 @@ sufficient_dsds:
 		host_to_fcp_swap((uint8_t *)&cmd_pkt->lun,
 			sizeof(cmd_pkt->lun));
 
+		/*
+		 * Update tagged queuing modifier -- default is TSK_SIMPLE (0).
+		 */
+		if (scsi_populate_tag_msg(cmd, tag)) {
+			switch (tag[0]) {
+			case HEAD_OF_QUEUE_TAG:
+				cmd_pkt->task = TSK_HEAD_OF_QUEUE;
+				break;
+			case ORDERED_QUEUE_TAG:
+				cmd_pkt->task = TSK_ORDERED;
+				break;
+			}
+		}
+
 		/* Load SCSI command packet. */
 		memcpy(cmd_pkt->fcp_cdb, cmd->cmnd, cmd->cmd_len);
 		host_to_fcp_swap(cmd_pkt->fcp_cdb, sizeof(cmd_pkt->fcp_cdb));
@@ -3457,46 +3489,28 @@ qla82xx_need_reset_handler(scsi_qla_host_t *vha)
 	}
 }
 
-static void
+int
 qla82xx_check_fw_alive(scsi_qla_host_t *vha)
 {
-	uint32_t fw_heartbeat_counter, halt_status;
-	struct qla_hw_data *ha = vha->hw;
+	uint32_t fw_heartbeat_counter;
+	int status = 0;
 
-	fw_heartbeat_counter = qla82xx_rd_32(ha, QLA82XX_PEG_ALIVE_COUNTER);
+	fw_heartbeat_counter = qla82xx_rd_32(vha->hw,
+		QLA82XX_PEG_ALIVE_COUNTER);
 	/* all 0xff, assume AER/EEH in progress, ignore */
 	if (fw_heartbeat_counter == 0xffffffff)
-		return;
+		return status;
 	if (vha->fw_heartbeat_counter == fw_heartbeat_counter) {
 		vha->seconds_since_last_heartbeat++;
 		/* FW not alive after 2 seconds */
 		if (vha->seconds_since_last_heartbeat == 2) {
 			vha->seconds_since_last_heartbeat = 0;
-			halt_status = qla82xx_rd_32(ha,
-				QLA82XX_PEG_HALT_STATUS1);
-			if (halt_status & HALT_STATUS_UNRECOVERABLE) {
-				set_bit(ISP_UNRECOVERABLE, &vha->dpc_flags);
-			} else {
-				qla_printk(KERN_INFO, ha,
-					"scsi(%ld): %s - detect abort needed\n",
-					vha->host_no, __func__);
-				set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
-			}
-			qla2xxx_wake_dpc(vha);
-			ha->flags.fw_hung = 1;
-			if (ha->flags.mbox_busy) {
-				ha->flags.mbox_int = 1;
-				DEBUG2(qla_printk(KERN_ERR, ha,
-					"Due to fw hung, doing premature "
-					"completion of mbx command\n"));
-				if (test_bit(MBX_INTR_WAIT,
-					&ha->mbx_cmd_flags))
-					complete(&ha->mbx_intr_comp);
-			}
+			status = 1;
 		}
 	} else
 		vha->seconds_since_last_heartbeat = 0;
 	vha->fw_heartbeat_counter = fw_heartbeat_counter;
+	return status;
 }
 
 /*
@@ -3557,6 +3571,8 @@ qla82xx_device_state_handler(scsi_qla_host_t *vha)
 			break;
 		case QLA82XX_DEV_NEED_RESET:
 			qla82xx_need_reset_handler(vha);
+			dev_init_timeout = jiffies +
+				(ha->nx_dev_init_timeout * HZ);
 			break;
 		case QLA82XX_DEV_NEED_QUIESCENT:
 			qla82xx_need_qsnt_handler(vha);
@@ -3596,30 +3612,18 @@ exit:
 
 void qla82xx_watchdog(scsi_qla_host_t *vha)
 {
-	uint32_t dev_state;
+	uint32_t dev_state, halt_status;
 	struct qla_hw_data *ha = vha->hw;
 
-	dev_state = qla82xx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
-
 	/* don't poll if reset is going on */
-	if (!(test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags) ||
-		test_bit(ABORT_ISP_ACTIVE, &vha->dpc_flags) ||
-		test_bit(ISP_ABORT_RETRY, &vha->dpc_flags))) {
-		if (dev_state == QLA82XX_DEV_NEED_RESET) {
+	if (!ha->flags.isp82xx_reset_hdlr_active) {
+		dev_state = qla82xx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
+		if (dev_state == QLA82XX_DEV_NEED_RESET &&
+		    !test_bit(ISP_ABORT_NEEDED, &vha->dpc_flags)) {
 			qla_printk(KERN_WARNING, ha,
-				"%s(): Adapter reset needed!\n", __func__);
+			    "%s(): Adapter reset needed!\n", __func__);
 			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
 			qla2xxx_wake_dpc(vha);
-			ha->flags.fw_hung = 1;
-			if (ha->flags.mbox_busy) {
-				ha->flags.mbox_int = 1;
-				DEBUG2(qla_printk(KERN_ERR, ha,
-					"Need reset, doing premature "
-					"completion of mbx command\n"));
-				if (test_bit(MBX_INTR_WAIT,
-					&ha->mbx_cmd_flags))
-					complete(&ha->mbx_intr_comp);
-			}
 		} else if (dev_state == QLA82XX_DEV_NEED_QUIESCENT &&
 			!test_bit(ISP_QUIESCE_NEEDED, &vha->dpc_flags)) {
 			DEBUG(qla_printk(KERN_INFO, ha,
@@ -3629,6 +3633,31 @@ void qla82xx_watchdog(scsi_qla_host_t *vha)
 			qla2xxx_wake_dpc(vha);
 		} else {
 			qla82xx_check_fw_alive(vha);
+			if (qla82xx_check_fw_alive(vha)) {
+				halt_status = qla82xx_rd_32(ha,
+				    QLA82XX_PEG_HALT_STATUS1);
+				if (halt_status & HALT_STATUS_UNRECOVERABLE) {
+					set_bit(ISP_UNRECOVERABLE,
+					    &vha->dpc_flags);
+				} else {
+					qla_printk(KERN_INFO, ha,
+					    "scsi(%ld): %s - detect abort needed\n",
+					    vha->host_no, __func__);
+					set_bit(ISP_ABORT_NEEDED,
+					    &vha->dpc_flags);
+				}
+				qla2xxx_wake_dpc(vha);
+				ha->flags.isp82xx_fw_hung = 1;
+				if (ha->flags.mbox_busy) {
+					ha->flags.mbox_int = 1;
+					DEBUG2(qla_printk(KERN_ERR, ha,
+					    "Due to fw hung, doing premature "
+					    "completion of mbx command\n"));
+					if (test_bit(MBX_INTR_WAIT,
+					    &ha->mbx_cmd_flags))
+						complete(&ha->mbx_intr_comp);
+				}
+			}
 		}
 	}
 }
@@ -3663,6 +3692,7 @@ qla82xx_abort_isp(scsi_qla_host_t *vha)
 			"Exiting.\n", __func__, vha->host_no);
 		return QLA_SUCCESS;
 	}
+	ha->flags.isp82xx_reset_hdlr_active = 1;
 
 	qla82xx_idc_lock(ha);
 	dev_state = qla82xx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
@@ -3683,7 +3713,8 @@ qla82xx_abort_isp(scsi_qla_host_t *vha)
 	qla82xx_idc_unlock(ha);
 
 	if (rval == QLA_SUCCESS) {
-		ha->flags.fw_hung = 0;
+		ha->flags.isp82xx_fw_hung = 0;
+		ha->flags.isp82xx_reset_hdlr_active = 0;
 		qla82xx_restart_isp(vha);
 	}
 
@@ -3791,3 +3822,71 @@ int qla2x00_wait_for_fcoe_ctx_reset(scsi_qla_host_t *vha)
 
 	return status;
 }
+
+void
+qla82xx_chip_reset_cleanup(scsi_qla_host_t *vha)
+{
+	int i;
+	unsigned long flags;
+	struct qla_hw_data *ha = vha->hw;
+
+	/* Check if 82XX firmware is alive or not
+	 * We may have arrived here from NEED_RESET
+	 * detection only
+	 */
+	if (!ha->flags.isp82xx_fw_hung) {
+		for (i = 0; i < 2; i++) {
+			msleep(1000);
+			if (qla82xx_check_fw_alive(vha)) {
+				ha->flags.isp82xx_fw_hung = 1;
+				if (ha->flags.mbox_busy) {
+					ha->flags.mbox_int = 1;
+					complete(&ha->mbx_intr_comp);
+				}
+				break;
+			}
+		}
+	}
+
+	/* Abort all commands gracefully if fw NOT hung */
+	if (!ha->flags.isp82xx_fw_hung) {
+		int cnt, que;
+		srb_t *sp;
+		struct req_que *req;
+
+		spin_lock_irqsave(&ha->hardware_lock, flags);
+		for (que = 0; que < ha->max_req_queues; que++) {
+			req = ha->req_q_map[que];
+			if (!req)
+				continue;
+			for (cnt = 1; cnt < MAX_OUTSTANDING_COMMANDS; cnt++) {
+				sp = req->outstanding_cmds[cnt];
+				if (sp) {
+					if (!sp->ctx ||
+					    (sp->flags & SRB_FCP_CMND_DMA_VALID)) {
+						spin_unlock_irqrestore(
+						    &ha->hardware_lock, flags);
+						if (ha->isp_ops->abort_command(sp)) {
+							qla_printk(KERN_INFO, ha,
+							    "scsi(%ld): mbx abort command failed in %s\n",
+							    vha->host_no, __func__);
+						} else {
+							qla_printk(KERN_INFO, ha,
+							    "scsi(%ld): mbx abort command success in %s\n",
+							    vha->host_no, __func__);
+						}
+						spin_lock_irqsave(&ha->hardware_lock, flags);
+					}
+				}
+			}
+		}
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+		/* Wait for pending cmds (physical and virtual) to complete */
+		if (!qla2x00_eh_wait_for_pending_commands(vha, 0, 0,
+		    WAIT_HOST) == QLA_SUCCESS) {
+			DEBUG2(qla_printk(KERN_INFO, ha,
+			    "Done wait for pending commands\n"));
+		}
+	}
+}
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index f27724d76cf6..75a966c94860 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -349,7 +349,7 @@ static int qla25xx_setup_mode(struct scsi_qla_host *vha)
 				"Can't create request queue\n");
 			goto fail;
 		}
-		ha->wq = create_workqueue("qla2xxx_wq");
+		ha->wq = alloc_workqueue("qla2xxx_wq", WQ_MEM_RECLAIM, 1);
 		vha->req = ha->req_q_map[req];
 		options |= BIT_1;
 		for (ques = 1; ques < ha->max_rsp_queues; ques++) {
@@ -506,7 +506,7 @@ qla24xx_fw_version_str(struct scsi_qla_host *vha, char *str)
 
 static inline srb_t *
 qla2x00_get_new_sp(scsi_qla_host_t *vha, fc_port_t *fcport,
-    struct scsi_cmnd *cmd, void (*done)(struct scsi_cmnd *))
+	struct scsi_cmnd *cmd)
 {
 	srb_t *sp;
 	struct qla_hw_data *ha = vha->hw;
@@ -520,14 +520,13 @@ qla2x00_get_new_sp(scsi_qla_host_t *vha, fc_port_t *fcport,
 	sp->cmd = cmd;
 	sp->flags = 0;
 	CMD_SP(cmd) = (void *)sp;
-	cmd->scsi_done = done;
 	sp->ctx = NULL;
 
 	return sp;
 }
 
 static int
-qla2xxx_queuecommand_lck(struct scsi_cmnd *cmd, void (*done)(struct scsi_cmnd *))
+qla2xxx_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 {
 	scsi_qla_host_t *vha = shost_priv(cmd->device->host);
 	fc_port_t *fcport = (struct fc_port *) cmd->device->hostdata;
@@ -537,7 +536,6 @@ qla2xxx_queuecommand_lck(struct scsi_cmnd *cmd, void (*done)(struct scsi_cmnd *)
 	srb_t *sp;
 	int rval;
 
-	spin_unlock_irq(vha->host->host_lock);
 	if (ha->flags.eeh_busy) {
 		if (ha->flags.pci_channel_io_perm_failure)
 			cmd->result = DID_NO_CONNECT << 16;
@@ -569,40 +567,32 @@ qla2xxx_queuecommand_lck(struct scsi_cmnd *cmd, void (*done)(struct scsi_cmnd *)
 		goto qc24_target_busy;
 	}
 
-	sp = qla2x00_get_new_sp(base_vha, fcport, cmd, done);
+	sp = qla2x00_get_new_sp(base_vha, fcport, cmd);
 	if (!sp)
-		goto qc24_host_busy_lock;
+		goto qc24_host_busy;
 
 	rval = ha->isp_ops->start_scsi(sp);
 	if (rval != QLA_SUCCESS)
 		goto qc24_host_busy_free_sp;
 
-	spin_lock_irq(vha->host->host_lock);
-
 	return 0;
 
 qc24_host_busy_free_sp:
 	qla2x00_sp_free_dma(sp);
 	mempool_free(sp, ha->srb_mempool);
 
-qc24_host_busy_lock:
-	spin_lock_irq(vha->host->host_lock);
+qc24_host_busy:
 	return SCSI_MLQUEUE_HOST_BUSY;
 
 qc24_target_busy:
-	spin_lock_irq(vha->host->host_lock);
 	return SCSI_MLQUEUE_TARGET_BUSY;
 
 qc24_fail_command:
-	spin_lock_irq(vha->host->host_lock);
-	done(cmd);
+	cmd->scsi_done(cmd);
 
 	return 0;
 }
 
-static DEF_SCSI_QCMD(qla2xxx_queuecommand)
-
-
 /*
  * qla2x00_eh_wait_on_command
  *    Waits for the command to be returned by the Firmware for some
@@ -821,17 +811,20 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 {
 	scsi_qla_host_t *vha = shost_priv(cmd->device->host);
 	srb_t *sp;
-	int ret = SUCCESS;
+	int ret;
 	unsigned int id, lun;
 	unsigned long flags;
 	int wait = 0;
 	struct qla_hw_data *ha = vha->hw;
 
-	fc_block_scsi_eh(cmd);
-
 	if (!CMD_SP(cmd))
 		return SUCCESS;
 
+	ret = fc_block_scsi_eh(cmd);
+	if (ret != 0)
+		return ret;
+	ret = SUCCESS;
+
 	id = cmd->device->id;
 	lun = cmd->device->lun;
 
@@ -940,11 +933,13 @@ __qla2xxx_eh_generic_reset(char *name, enum nexus_wait_type type,
 	fc_port_t *fcport = (struct fc_port *) cmd->device->hostdata;
 	int err;
 
-	fc_block_scsi_eh(cmd);
-
 	if (!fcport)
 		return FAILED;
 
+	err = fc_block_scsi_eh(cmd);
+	if (err != 0)
+		return err;
+
 	qla_printk(KERN_INFO, vha->hw, "scsi(%ld:%d:%d): %s RESET ISSUED.\n",
 	    vha->host_no, cmd->device->id, cmd->device->lun, name);
 
@@ -1018,14 +1013,17 @@ qla2xxx_eh_bus_reset(struct scsi_cmnd *cmd)
 	int ret = FAILED;
 	unsigned int id, lun;
 
-	fc_block_scsi_eh(cmd);
-
 	id = cmd->device->id;
 	lun = cmd->device->lun;
 
 	if (!fcport)
 		return ret;
 
+	ret = fc_block_scsi_eh(cmd);
+	if (ret != 0)
+		return ret;
+	ret = FAILED;
+
 	qla_printk(KERN_INFO, vha->hw,
 	    "scsi(%ld:%d:%d): BUS RESET ISSUED.\n", vha->host_no, id, lun);
 
@@ -1078,14 +1076,17 @@ qla2xxx_eh_host_reset(struct scsi_cmnd *cmd)
 	unsigned int id, lun;
 	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
 
-	fc_block_scsi_eh(cmd);
-
 	id = cmd->device->id;
 	lun = cmd->device->lun;
 
 	if (!fcport)
 		return ret;
 
+	ret = fc_block_scsi_eh(cmd);
+	if (ret != 0)
+		return ret;
+	ret = FAILED;
+
 	qla_printk(KERN_INFO, ha,
 	    "scsi(%ld:%d:%d): ADAPTER RESET ISSUED.\n", vha->host_no, id, lun);
 
@@ -3805,7 +3806,7 @@ qla2xxx_pci_error_detected(struct pci_dev *pdev, pci_channel_state_t state)
 		ha->flags.eeh_busy = 1;
 		/* For ISP82XX complete any pending mailbox cmd */
 		if (IS_QLA82XX(ha)) {
-			ha->flags.fw_hung = 1;
+			ha->flags.isp82xx_fw_hung = 1;
 			if (ha->flags.mbox_busy) {
 				ha->flags.mbox_int = 1;
 				DEBUG2(qla_printk(KERN_ERR, ha,
@@ -3945,7 +3946,7 @@ uint32_t qla82xx_error_recovery(scsi_qla_host_t *base_vha)
 			qla82xx_wr_32(ha, QLA82XX_CRB_DEV_STATE,
 			    QLA82XX_DEV_READY);
 			qla82xx_idc_unlock(ha);
-			ha->flags.fw_hung = 0;
+			ha->flags.isp82xx_fw_hung = 0;
 			rval = qla82xx_restart_isp(base_vha);
 			qla82xx_idc_lock(ha);
 			/* Clear driver state register */
@@ -3958,7 +3959,7 @@ uint32_t qla82xx_error_recovery(scsi_qla_host_t *base_vha)
 		    "This devfn is not reset owner = 0x%x\n", ha->pdev->devfn));
 		if ((qla82xx_rd_32(ha, QLA82XX_CRB_DEV_STATE) ==
 		    QLA82XX_DEV_READY)) {
-			ha->flags.fw_hung = 0;
+			ha->flags.isp82xx_fw_hung = 0;
 			rval = qla82xx_restart_isp(base_vha);
 			qla82xx_idc_lock(ha);
 			qla82xx_set_drv_active(base_vha);
diff --git a/drivers/scsi/qla2xxx/qla_version.h b/drivers/scsi/qla2xxx/qla_version.h
index cf0075a2d0c2..3a260c3f055a 100644
--- a/drivers/scsi/qla2xxx/qla_version.h
+++ b/drivers/scsi/qla2xxx/qla_version.h
@@ -7,9 +7,9 @@
 /*
  * Driver version
  */
-#define QLA2XXX_VERSION      "8.03.05-k0"
+#define QLA2XXX_VERSION      "8.03.07.00"
 
 #define QLA_DRIVER_MAJOR_VER	8
 #define QLA_DRIVER_MINOR_VER	3
-#define QLA_DRIVER_PATCH_VER	5
+#define QLA_DRIVER_PATCH_VER	7
 #define QLA_DRIVER_BETA_VER	0
diff --git a/drivers/scsi/qla4xxx/ql4_isr.c b/drivers/scsi/qla4xxx/ql4_isr.c
index 6ffbe9727dff..03e028e6e809 100644
--- a/drivers/scsi/qla4xxx/ql4_isr.c
+++ b/drivers/scsi/qla4xxx/ql4_isr.c
@@ -1027,7 +1027,7 @@ void qla4xxx_process_aen(struct scsi_qla_host * ha, uint8_t process_aen)
 					((ddb_entry->default_time2wait +
 					  4) * HZ);
 
-				DEBUG2(printk("scsi%ld: ddb [%d] initate"
+				DEBUG2(printk("scsi%ld: ddb [%d] initiate"
 					      " RELOGIN after %d seconds\n",
 					      ha->host_no,
 					      ddb_entry->fw_ddb_index,
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index 3fc1d256636f..967836ef5ab2 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -812,7 +812,7 @@ static void qla4xxx_timer(struct scsi_qla_host *ha)
 					);
 				start_dpc++;
 				DEBUG(printk("scsi%ld:%d:%d: ddb [%d] "
-					     "initate relogin after"
+					     "initiate relogin after"
 					     " %d seconds\n",
 					     ha->host_no, ddb_entry->bus,
 					     ddb_entry->target,
diff --git a/drivers/scsi/qlogicpti.c b/drivers/scsi/qlogicpti.c
index 664c9572d0c9..e2d45c91b8e8 100644
--- a/drivers/scsi/qlogicpti.c
+++ b/drivers/scsi/qlogicpti.c
@@ -1292,15 +1292,19 @@ static struct scsi_host_template qpti_template = {
 	.use_clustering		= ENABLE_CLUSTERING,
 };
 
-static int __devinit qpti_sbus_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit qpti_sbus_probe(struct platform_device *op)
 {
-	struct scsi_host_template *tpnt = match->data;
+	struct scsi_host_template *tpnt;
 	struct device_node *dp = op->dev.of_node;
 	struct Scsi_Host *host;
 	struct qlogicpti *qpti;
 	static int nqptis;
 	const char *fcode;
 
+	if (!op->dev.of_match)
+		return -EINVAL;
+	tpnt = op->dev.of_match->data;
+
 	/* Sometimes Antares cards come up not completely
 	 * setup, and we get a report of a zero IRQ.
 	 */
@@ -1457,7 +1461,7 @@ static const struct of_device_id qpti_match[] = {
 };
 MODULE_DEVICE_TABLE(of, qpti_match);
 
-static struct of_platform_driver qpti_sbus_driver = {
+static struct platform_driver qpti_sbus_driver = {
 	.driver = {
 		.name = "qpti",
 		.owner = THIS_MODULE,
@@ -1469,12 +1473,12 @@ static struct of_platform_driver qpti_sbus_driver = {
 
 static int __init qpti_init(void)
 {
-	return of_register_platform_driver(&qpti_sbus_driver);
+	return platform_driver_register(&qpti_sbus_driver);
 }
 
 static void __exit qpti_exit(void)
 {
-	of_unregister_platform_driver(&qpti_sbus_driver);
+	platform_driver_unregister(&qpti_sbus_driver);
 }
 
 MODULE_DESCRIPTION("QlogicISP SBUS driver");
diff --git a/drivers/scsi/scsi_devinfo.c b/drivers/scsi/scsi_devinfo.c
index 43fad4c09beb..82e9e5c0476e 100644
--- a/drivers/scsi/scsi_devinfo.c
+++ b/drivers/scsi/scsi_devinfo.c
@@ -382,6 +382,91 @@ int scsi_dev_info_list_add_keyed(int compatible, char *vendor, char *model,
 EXPORT_SYMBOL(scsi_dev_info_list_add_keyed);
 
 /**
+ * scsi_dev_info_list_del_keyed - remove one dev_info list entry.
+ * @vendor:	vendor string
+ * @model:	model (product) string
+ * @key:	specify list to use
+ *
+ * Description:
+ * 	Remove and destroy one dev_info entry for @vendor, @model
+ * 	in list specified by @key.
+ *
+ * Returns: 0 OK, -error on failure.
+ **/
+int scsi_dev_info_list_del_keyed(char *vendor, char *model, int key)
+{
+	struct scsi_dev_info_list *devinfo, *found = NULL;
+	struct scsi_dev_info_list_table *devinfo_table =
+		scsi_devinfo_lookup_by_key(key);
+
+	if (IS_ERR(devinfo_table))
+		return PTR_ERR(devinfo_table);
+
+	list_for_each_entry(devinfo, &devinfo_table->scsi_dev_info_list,
+			    dev_info_list) {
+		if (devinfo->compatible) {
+			/*
+			 * Behave like the older version of get_device_flags.
+			 */
+			size_t max;
+			/*
+			 * XXX why skip leading spaces? If an odd INQUIRY
+			 * value, that should have been part of the
+			 * scsi_static_device_list[] entry, such as "  FOO"
+			 * rather than "FOO". Since this code is already
+			 * here, and we don't know what device it is
+			 * trying to work with, leave it as-is.
+			 */
+			max = 8;	/* max length of vendor */
+			while ((max > 0) && *vendor == ' ') {
+				max--;
+				vendor++;
+			}
+			/*
+			 * XXX removing the following strlen() would be
+			 * good, using it means that for a an entry not in
+			 * the list, we scan every byte of every vendor
+			 * listed in scsi_static_device_list[], and never match
+			 * a single one (and still have to compare at
+			 * least the first byte of each vendor).
+			 */
+			if (memcmp(devinfo->vendor, vendor,
+				    min(max, strlen(devinfo->vendor))))
+				continue;
+			/*
+			 * Skip spaces again.
+			 */
+			max = 16;	/* max length of model */
+			while ((max > 0) && *model == ' ') {
+				max--;
+				model++;
+			}
+			if (memcmp(devinfo->model, model,
+				   min(max, strlen(devinfo->model))))
+				continue;
+			found = devinfo;
+		} else {
+			if (!memcmp(devinfo->vendor, vendor,
+				     sizeof(devinfo->vendor)) &&
+			     !memcmp(devinfo->model, model,
+				      sizeof(devinfo->model)))
+				found = devinfo;
+		}
+		if (found)
+			break;
+	}
+
+	if (found) {
+		list_del(&found->dev_info_list);
+		kfree(found);
+		return 0;
+	}
+
+	return -ENOENT;
+}
+EXPORT_SYMBOL(scsi_dev_info_list_del_keyed);
+
+/**
  * scsi_dev_info_list_add_str - parse dev_list and add to the scsi_dev_info_list.
  * @dev_list:	string of device flags to add
  *
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index 45c75649b9e0..991de3c15cfc 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -223,7 +223,7 @@ static inline void scsi_eh_prt_fail_stats(struct Scsi_Host *shost,
  * @scmd:	Cmd to have sense checked.
  *
  * Return value:
- * 	SUCCESS or FAILED or NEEDS_RETRY
+ *	SUCCESS or FAILED or NEEDS_RETRY or TARGET_ERROR
  *
  * Notes:
  *	When a deferred error is detected the current command has
@@ -326,17 +326,19 @@ static int scsi_check_sense(struct scsi_cmnd *scmd)
 		 */
 		return SUCCESS;
 
-		/* these three are not supported */
+		/* these are not supported */
 	case COPY_ABORTED:
 	case VOLUME_OVERFLOW:
 	case MISCOMPARE:
-		return SUCCESS;
+	case BLANK_CHECK:
+	case DATA_PROTECT:
+		return TARGET_ERROR;
 
 	case MEDIUM_ERROR:
 		if (sshdr.asc == 0x11 || /* UNRECOVERED READ ERR */
 		    sshdr.asc == 0x13 || /* AMNF DATA FIELD */
 		    sshdr.asc == 0x14) { /* RECORD NOT FOUND */
-			return SUCCESS;
+			return TARGET_ERROR;
 		}
 		return NEEDS_RETRY;
 
@@ -344,11 +346,9 @@ static int scsi_check_sense(struct scsi_cmnd *scmd)
 		if (scmd->device->retry_hwerror)
 			return ADD_TO_MLQUEUE;
 		else
-			return SUCCESS;
+			return TARGET_ERROR;
 
 	case ILLEGAL_REQUEST:
-	case BLANK_CHECK:
-	case DATA_PROTECT:
 	default:
 		return SUCCESS;
 	}
@@ -787,6 +787,7 @@ static int scsi_send_eh_cmnd(struct scsi_cmnd *scmd, unsigned char *cmnd,
 		case SUCCESS:
 		case NEEDS_RETRY:
 		case FAILED:
+		case TARGET_ERROR:
 			break;
 		case ADD_TO_MLQUEUE:
 			rtn = NEEDS_RETRY;
@@ -1469,6 +1470,14 @@ int scsi_decide_disposition(struct scsi_cmnd *scmd)
 		rtn = scsi_check_sense(scmd);
 		if (rtn == NEEDS_RETRY)
 			goto maybe_retry;
+		else if (rtn == TARGET_ERROR) {
+			/*
+			 * Need to modify host byte to signal a
+			 * permanent target failure
+			 */
+			scmd->result |= (DID_TARGET_FAILURE << 16);
+			rtn = SUCCESS;
+		}
 		/* if rtn == FAILED, we have no sense information;
 		 * returning FAILED will wake the error handler thread
 		 * to collect the sense and redo the decide
@@ -1486,6 +1495,7 @@ int scsi_decide_disposition(struct scsi_cmnd *scmd)
 	case RESERVATION_CONFLICT:
 		sdev_printk(KERN_INFO, scmd->device,
 			    "reservation conflict\n");
+		scmd->result |= (DID_NEXUS_FAILURE << 16);
 		return SUCCESS; /* causes immediate i/o error */
 	default:
 		return FAILED;
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 9045c52abd25..87137f39086b 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -443,7 +443,7 @@ static void scsi_run_queue(struct request_queue *q)
 					&sdev->request_queue->queue_flags);
 		if (flagset)
 			queue_flag_set(QUEUE_FLAG_REENTER, sdev->request_queue);
-		__blk_run_queue(sdev->request_queue);
+		__blk_run_queue(sdev->request_queue, false);
 		if (flagset)
 			queue_flag_clear(QUEUE_FLAG_REENTER, sdev->request_queue);
 		spin_unlock(sdev->request_queue->queue_lock);
@@ -667,6 +667,30 @@ void scsi_release_buffers(struct scsi_cmnd *cmd)
 }
 EXPORT_SYMBOL(scsi_release_buffers);
 
+static int __scsi_error_from_host_byte(struct scsi_cmnd *cmd, int result)
+{
+	int error = 0;
+
+	switch(host_byte(result)) {
+	case DID_TRANSPORT_FAILFAST:
+		error = -ENOLINK;
+		break;
+	case DID_TARGET_FAILURE:
+		cmd->result |= (DID_OK << 16);
+		error = -EREMOTEIO;
+		break;
+	case DID_NEXUS_FAILURE:
+		cmd->result |= (DID_OK << 16);
+		error = -EBADE;
+		break;
+	default:
+		error = -EIO;
+		break;
+	}
+
+	return error;
+}
+
 /*
  * Function:    scsi_io_completion()
  *
@@ -737,7 +761,7 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 				req->sense_len = len;
 			}
 			if (!sense_deferred)
-				error = -EIO;
+				error = __scsi_error_from_host_byte(cmd, result);
 		}
 
 		req->resid_len = scsi_get_resid(cmd);
@@ -796,7 +820,7 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 	if (scsi_end_request(cmd, error, good_bytes, result == 0) == NULL)
 		return;
 
-	error = -EIO;
+	error = __scsi_error_from_host_byte(cmd, result);
 
 	if (host_byte(result) == DID_RESET) {
 		/* Third party bus reset or reset for error recovery
diff --git a/drivers/scsi/scsi_priv.h b/drivers/scsi/scsi_priv.h
index b4056d14f812..2a588955423a 100644
--- a/drivers/scsi/scsi_priv.h
+++ b/drivers/scsi/scsi_priv.h
@@ -45,6 +45,7 @@ static inline void scsi_log_completion(struct scsi_cmnd *cmd, int disposition)
 enum {
 	SCSI_DEVINFO_GLOBAL = 0,
 	SCSI_DEVINFO_SPI,
+	SCSI_DEVINFO_DH,
 };
 
 extern int scsi_get_device_flags(struct scsi_device *sdev,
@@ -56,6 +57,7 @@ extern int scsi_get_device_flags_keyed(struct scsi_device *sdev,
 extern int scsi_dev_info_list_add_keyed(int compatible, char *vendor,
 					char *model, char *strflags,
 					int flags, int key);
+extern int scsi_dev_info_list_del_keyed(char *vendor, char *model, int key);
 extern int scsi_dev_info_add_list(int key, const char *name);
 extern int scsi_dev_info_remove_list(int key);
 
@@ -146,7 +148,7 @@ static inline void scsi_netlink_exit(void) {}
 #endif
 
 /* scsi_pm.c */
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 extern const struct dev_pm_ops scsi_bus_pm_ops;
 #endif
 #ifdef CONFIG_PM_RUNTIME
diff --git a/drivers/scsi/scsi_sysfs.c b/drivers/scsi/scsi_sysfs.c
index 490ce213204e..e44ff64233fd 100644
--- a/drivers/scsi/scsi_sysfs.c
+++ b/drivers/scsi/scsi_sysfs.c
@@ -383,7 +383,7 @@ struct bus_type scsi_bus_type = {
         .name		= "scsi",
         .match		= scsi_bus_match,
 	.uevent		= scsi_bus_uevent,
-#ifdef CONFIG_PM_OPS
+#ifdef CONFIG_PM
 	.pm		= &scsi_bus_pm_ops,
 #endif
 };
diff --git a/drivers/scsi/scsi_tgt_lib.c b/drivers/scsi/scsi_tgt_lib.c
index c399be979921..f67282058ba1 100644
--- a/drivers/scsi/scsi_tgt_lib.c
+++ b/drivers/scsi/scsi_tgt_lib.c
@@ -629,7 +629,7 @@ static int __init scsi_tgt_init(void)
 	if (!scsi_tgt_cmd_cache)
 		return -ENOMEM;
 
-	scsi_tgtd = create_workqueue("scsi_tgtd");
+	scsi_tgtd = alloc_workqueue("scsi_tgtd", 0, 1);
 	if (!scsi_tgtd) {
 		err = -ENOMEM;
 		goto free_kmemcache;
diff --git a/drivers/scsi/scsi_transport_fc.c b/drivers/scsi/scsi_transport_fc.c
index 998c01be3234..5c3ccfc6b622 100644
--- a/drivers/scsi/scsi_transport_fc.c
+++ b/drivers/scsi/scsi_transport_fc.c
@@ -3829,7 +3829,7 @@ fc_bsg_goose_queue(struct fc_rport *rport)
 		  !test_bit(QUEUE_FLAG_REENTER, &rport->rqst_q->queue_flags);
 	if (flagset)
 		queue_flag_set(QUEUE_FLAG_REENTER, rport->rqst_q);
-	__blk_run_queue(rport->rqst_q);
+	__blk_run_queue(rport->rqst_q, false);
 	if (flagset)
 		queue_flag_clear(QUEUE_FLAG_REENTER, rport->rqst_q);
 	spin_unlock_irqrestore(rport->rqst_q->queue_lock, flags);
diff --git a/drivers/scsi/scsi_transport_iscsi.c b/drivers/scsi/scsi_transport_iscsi.c
index f905ecb5704d..b4218390941e 100644
--- a/drivers/scsi/scsi_transport_iscsi.c
+++ b/drivers/scsi/scsi_transport_iscsi.c
@@ -954,6 +954,7 @@ iscsi_create_conn(struct iscsi_cls_session *session, int dd_size, uint32_t cid)
 	if (dd_size)
 		conn->dd_data = &conn[1];
 
+	mutex_init(&conn->ep_mutex);
 	INIT_LIST_HEAD(&conn->conn_list);
 	conn->transport = transport;
 	conn->cid = cid;
@@ -975,7 +976,6 @@ iscsi_create_conn(struct iscsi_cls_session *session, int dd_size, uint32_t cid)
 
 	spin_lock_irqsave(&connlock, flags);
 	list_add(&conn->conn_list, &connlist);
-	conn->active = 1;
 	spin_unlock_irqrestore(&connlock, flags);
 
 	ISCSI_DBG_TRANS_CONN(conn, "Completed conn creation\n");
@@ -1001,7 +1001,6 @@ int iscsi_destroy_conn(struct iscsi_cls_conn *conn)
 	unsigned long flags;
 
 	spin_lock_irqsave(&connlock, flags);
-	conn->active = 0;
 	list_del(&conn->conn_list);
 	spin_unlock_irqrestore(&connlock, flags);
 
@@ -1430,6 +1429,29 @@ release_host:
 	return err;
 }
 
+static int iscsi_if_ep_disconnect(struct iscsi_transport *transport,
+				  u64 ep_handle)
+{
+	struct iscsi_cls_conn *conn;
+	struct iscsi_endpoint *ep;
+
+	if (!transport->ep_disconnect)
+		return -EINVAL;
+
+	ep = iscsi_lookup_endpoint(ep_handle);
+	if (!ep)
+		return -EINVAL;
+	conn = ep->conn;
+	if (conn) {
+		mutex_lock(&conn->ep_mutex);
+		conn->ep = NULL;
+		mutex_unlock(&conn->ep_mutex);
+	}
+
+	transport->ep_disconnect(ep);
+	return 0;
+}
+
 static int
 iscsi_if_transport_ep(struct iscsi_transport *transport,
 		      struct iscsi_uevent *ev, int msg_type)
@@ -1454,14 +1476,8 @@ iscsi_if_transport_ep(struct iscsi_transport *transport,
 						   ev->u.ep_poll.timeout_ms);
 		break;
 	case ISCSI_UEVENT_TRANSPORT_EP_DISCONNECT:
-		if (!transport->ep_disconnect)
-			return -EINVAL;
-
-		ep = iscsi_lookup_endpoint(ev->u.ep_disconnect.ep_handle);
-		if (!ep)
-			return -EINVAL;
-
-		transport->ep_disconnect(ep);
+		rc = iscsi_if_ep_disconnect(transport,
+					    ev->u.ep_disconnect.ep_handle);
 		break;
 	}
 	return rc;
@@ -1609,12 +1625,31 @@ iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 		session = iscsi_session_lookup(ev->u.b_conn.sid);
 		conn = iscsi_conn_lookup(ev->u.b_conn.sid, ev->u.b_conn.cid);
 
-		if (session && conn)
-			ev->r.retcode =	transport->bind_conn(session, conn,
-					ev->u.b_conn.transport_eph,
-					ev->u.b_conn.is_leading);
-		else
+		if (conn && conn->ep)
+			iscsi_if_ep_disconnect(transport, conn->ep->id);
+
+		if (!session || !conn) {
 			err = -EINVAL;
+			break;
+		}
+
+		ev->r.retcode =	transport->bind_conn(session, conn,
+						ev->u.b_conn.transport_eph,
+						ev->u.b_conn.is_leading);
+		if (ev->r.retcode || !transport->ep_connect)
+			break;
+
+		ep = iscsi_lookup_endpoint(ev->u.b_conn.transport_eph);
+		if (ep) {
+			ep->conn = conn;
+
+			mutex_lock(&conn->ep_mutex);
+			conn->ep = ep;
+			mutex_unlock(&conn->ep_mutex);
+		} else
+			iscsi_cls_conn_printk(KERN_ERR, conn,
+					      "Could not set ep conn "
+					      "binding\n");
 		break;
 	case ISCSI_UEVENT_SET_PARAM:
 		err = iscsi_set_param(transport, ev);
@@ -1747,13 +1782,48 @@ iscsi_conn_attr(data_digest, ISCSI_PARAM_DATADGST_EN);
 iscsi_conn_attr(ifmarker, ISCSI_PARAM_IFMARKER_EN);
 iscsi_conn_attr(ofmarker, ISCSI_PARAM_OFMARKER_EN);
 iscsi_conn_attr(persistent_port, ISCSI_PARAM_PERSISTENT_PORT);
-iscsi_conn_attr(port, ISCSI_PARAM_CONN_PORT);
 iscsi_conn_attr(exp_statsn, ISCSI_PARAM_EXP_STATSN);
 iscsi_conn_attr(persistent_address, ISCSI_PARAM_PERSISTENT_ADDRESS);
-iscsi_conn_attr(address, ISCSI_PARAM_CONN_ADDRESS);
 iscsi_conn_attr(ping_tmo, ISCSI_PARAM_PING_TMO);
 iscsi_conn_attr(recv_tmo, ISCSI_PARAM_RECV_TMO);
 
+#define iscsi_conn_ep_attr_show(param)					\
+static ssize_t show_conn_ep_param_##param(struct device *dev,		\
+					  struct device_attribute *attr,\
+					  char *buf)			\
+{									\
+	struct iscsi_cls_conn *conn = iscsi_dev_to_conn(dev->parent);	\
+	struct iscsi_transport *t = conn->transport;			\
+	struct iscsi_endpoint *ep;					\
+	ssize_t rc;							\
+									\
+	/*								\
+	 * Need to make sure ep_disconnect does not free the LLD's	\
+	 * interconnect resources while we are trying to read them.	\
+	 */								\
+	mutex_lock(&conn->ep_mutex);					\
+	ep = conn->ep;							\
+	if (!ep && t->ep_connect) {					\
+		mutex_unlock(&conn->ep_mutex);				\
+		return -ENOTCONN;					\
+	}								\
+									\
+	if (ep)								\
+		rc = t->get_ep_param(ep, param, buf);			\
+	else								\
+		rc = t->get_conn_param(conn, param, buf);		\
+	mutex_unlock(&conn->ep_mutex);					\
+	return rc;							\
+}
+
+#define iscsi_conn_ep_attr(field, param)				\
+	iscsi_conn_ep_attr_show(param)					\
+static ISCSI_CLASS_ATTR(conn, field, S_IRUGO,				\
+			show_conn_ep_param_##param, NULL);
+
+iscsi_conn_ep_attr(address, ISCSI_PARAM_CONN_ADDRESS);
+iscsi_conn_ep_attr(port, ISCSI_PARAM_CONN_PORT);
+
 /*
  * iSCSI session attrs
  */
diff --git a/drivers/scsi/sun_esp.c b/drivers/scsi/sun_esp.c
index 193b37ba1834..676fe9ac7f61 100644
--- a/drivers/scsi/sun_esp.c
+++ b/drivers/scsi/sun_esp.c
@@ -562,7 +562,7 @@ fail:
 	return err;
 }
 
-static int __devinit esp_sbus_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit esp_sbus_probe(struct platform_device *op)
 {
 	struct device_node *dma_node = NULL;
 	struct device_node *dp = op->dev.of_node;
@@ -632,7 +632,7 @@ static const struct of_device_id esp_match[] = {
 };
 MODULE_DEVICE_TABLE(of, esp_match);
 
-static struct of_platform_driver esp_sbus_driver = {
+static struct platform_driver esp_sbus_driver = {
 	.driver = {
 		.name = "esp",
 		.owner = THIS_MODULE,
@@ -644,12 +644,12 @@ static struct of_platform_driver esp_sbus_driver = {
 
 static int __init sunesp_init(void)
 {
-	return of_register_platform_driver(&esp_sbus_driver);
+	return platform_driver_register(&esp_sbus_driver);
 }
 
 static void __exit sunesp_exit(void)
 {
-	of_unregister_platform_driver(&esp_sbus_driver);
+	platform_driver_unregister(&esp_sbus_driver);
 }
 
 MODULE_DESCRIPTION("Sun ESP SCSI driver");
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index bb233a9cbad2..0f48066445f9 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -53,6 +53,12 @@ if SPI_MASTER
 
 comment "SPI Master Controller Drivers"
 
+config SPI_ALTERA
+	tristate "Altera SPI Controller"
+	select SPI_BITBANG
+	help
+	  This is the driver for the Altera SPI Controller.
+
 config SPI_ATH79
 	tristate "Atheros AR71XX/AR724X/AR913X SPI controller driver"
 	depends on ATH79 && GENERIC_GPIO
@@ -161,13 +167,13 @@ config SPI_IMX_VER_0_0
 	def_bool y if SOC_IMX21 || SOC_IMX27
 
 config SPI_IMX_VER_0_4
-	def_bool y if ARCH_MX31
+	def_bool y if SOC_IMX31
 
 config SPI_IMX_VER_0_7
-	def_bool y if ARCH_MX25 || ARCH_MX35 || ARCH_MX51 || ARCH_MX53
+	def_bool y if ARCH_MX25 || SOC_IMX35 || SOC_IMX51 || SOC_IMX53
 
 config SPI_IMX_VER_2_3
-	def_bool y if ARCH_MX51 || ARCH_MX53
+	def_bool y if SOC_IMX51 || SOC_IMX53
 
 config SPI_IMX
 	tristate "Freescale i.MX SPI controllers"
@@ -231,6 +237,13 @@ config SPI_FSL_ESPI
 	  From MPC8536, 85xx platform uses the controller, and all P10xx,
 	  P20xx, P30xx,P40xx, P50xx uses this controller.
 
+config SPI_OC_TINY
+	tristate "OpenCores tiny SPI"
+	depends on GENERIC_GPIO
+	select SPI_BITBANG
+	help
+	  This is the driver for OpenCores tiny SPI master controller.
+
 config SPI_OMAP_UWIRE
 	tristate "OMAP1 MicroWire"
 	depends on ARCH_OMAP1
@@ -330,6 +343,12 @@ config SPI_SH_MSIOF
 	help
 	  SPI driver for SuperH MSIOF blocks.
 
+config SPI_SH
+	tristate "SuperH SPI controller"
+	depends on SUPERH
+	help
+	  SPI driver for SuperH SPI blocks.
+
 config SPI_SH_SCI
 	tristate "SuperH SCI SPI controller"
 	depends on SUPERH
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 86d1b5f9bbd9..8b5a315045a6 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -9,6 +9,7 @@ ccflags-$(CONFIG_SPI_DEBUG) := -DDEBUG
 obj-$(CONFIG_SPI_MASTER)		+= spi.o
 
 # SPI master controller drivers (bus)
+obj-$(CONFIG_SPI_ALTERA)		+= spi_altera.o
 obj-$(CONFIG_SPI_ATMEL)			+= atmel_spi.o
 obj-$(CONFIG_SPI_ATH79)			+= ath79_spi.o
 obj-$(CONFIG_SPI_BFIN)			+= spi_bfin5xx.o
@@ -27,6 +28,7 @@ obj-$(CONFIG_SPI_IMX)			+= spi_imx.o
 obj-$(CONFIG_SPI_LM70_LLP)		+= spi_lm70llp.o
 obj-$(CONFIG_SPI_PXA2XX)		+= pxa2xx_spi.o
 obj-$(CONFIG_SPI_PXA2XX_PCI)		+= pxa2xx_spi_pci.o
+obj-$(CONFIG_SPI_OC_TINY)		+= spi_oc_tiny.o
 obj-$(CONFIG_SPI_OMAP_UWIRE)		+= omap_uwire.o
 obj-$(CONFIG_SPI_OMAP24XX)		+= omap2_mcspi.o
 obj-$(CONFIG_SPI_OMAP_100K)		+= omap_spi_100k.o
@@ -46,6 +48,7 @@ obj-$(CONFIG_SPI_TEGRA)			+= spi_tegra.o
 obj-$(CONFIG_SPI_TOPCLIFF_PCH)		+= spi_topcliff_pch.o
 obj-$(CONFIG_SPI_TXX9)			+= spi_txx9.o
 obj-$(CONFIG_SPI_XILINX)		+= xilinx_spi.o
+obj-$(CONFIG_SPI_SH)			+= spi_sh.o
 obj-$(CONFIG_SPI_SH_SCI)		+= spi_sh_sci.o
 obj-$(CONFIG_SPI_SH_MSIOF)		+= spi_sh_msiof.o
 obj-$(CONFIG_SPI_STMP3XXX)		+= spi_stmp.o
diff --git a/drivers/spi/amba-pl022.c b/drivers/spi/amba-pl022.c
index 71a1219a995d..5c2b092a915e 100644
--- a/drivers/spi/amba-pl022.c
+++ b/drivers/spi/amba-pl022.c
@@ -329,15 +329,16 @@ struct vendor_data {
 /**
  * struct pl022 - This is the private SSP driver data structure
  * @adev: AMBA device model hookup
- * @vendor: Vendor data for the IP block
- * @phybase: The physical memory where the SSP device resides
- * @virtbase: The virtual memory where the SSP is mapped
+ * @vendor: vendor data for the IP block
+ * @phybase: the physical memory where the SSP device resides
+ * @virtbase: the virtual memory where the SSP is mapped
+ * @clk: outgoing clock "SPICLK" for the SPI bus
  * @master: SPI framework hookup
  * @master_info: controller-specific data from machine setup
- * @regs: SSP controller register's virtual address
- * @pump_messages: Work struct for scheduling work to the workqueue
- * @lock: spinlock to syncronise access to driver data
  * @workqueue: a workqueue on which any spi_message request is queued
+ * @pump_messages: work struct for scheduling work to the workqueue
+ * @queue_lock: spinlock to syncronise access to message queue
+ * @queue: message queue
  * @busy: workqueue is busy
  * @running: workqueue is running
  * @pump_transfers: Tasklet used in Interrupt Transfer mode
@@ -348,8 +349,14 @@ struct vendor_data {
  * @tx_end: end position in TX buffer to be read
  * @rx: current position in RX buffer to be written
  * @rx_end: end position in RX buffer to be written
- * @readingtype: the type of read currently going on
- * @writingtype: the type or write currently going on
+ * @read: the type of read currently going on
+ * @write: the type of write currently going on
+ * @exp_fifo_level: expected FIFO level
+ * @dma_rx_channel: optional channel for RX DMA
+ * @dma_tx_channel: optional channel for TX DMA
+ * @sgt_rx: scattertable for the RX transfer
+ * @sgt_tx: scattertable for the TX transfer
+ * @dummypage: a dummy page used for driving data on the bus with DMA
  */
 struct pl022 {
 	struct amba_device		*adev;
@@ -397,8 +404,8 @@ struct pl022 {
  * @cpsr: Value of Clock prescale register
  * @n_bytes: how many bytes(power of 2) reqd for a given data width of client
  * @enable_dma: Whether to enable DMA or not
- * @write: function ptr to be used to write when doing xfer for this chip
  * @read: function ptr to be used to read when doing xfer for this chip
+ * @write: function ptr to be used to write when doing xfer for this chip
  * @cs_control: chip select callback provided by chip
  * @xfer_type: polling/interrupt/DMA
  *
@@ -508,9 +515,10 @@ static void giveback(struct pl022 *pl022)
 	msg->state = NULL;
 	if (msg->complete)
 		msg->complete(msg->context);
-	/* This message is completed, so let's turn off the clocks! */
+	/* This message is completed, so let's turn off the clocks & power */
 	clk_disable(pl022->clk);
 	amba_pclk_disable(pl022->adev);
+	amba_vcore_disable(pl022->adev);
 }
 
 /**
@@ -917,7 +925,6 @@ static int configure_dma(struct pl022 *pl022)
 	struct dma_chan *txchan = pl022->dma_tx_channel;
 	struct dma_async_tx_descriptor *rxdesc;
 	struct dma_async_tx_descriptor *txdesc;
-	dma_cookie_t cookie;
 
 	/* Check that the channels are available */
 	if (!rxchan || !txchan)
@@ -962,10 +969,8 @@ static int configure_dma(struct pl022 *pl022)
 		tx_conf.dst_addr_width = rx_conf.src_addr_width;
 	BUG_ON(rx_conf.src_addr_width != tx_conf.dst_addr_width);
 
-	rxchan->device->device_control(rxchan, DMA_SLAVE_CONFIG,
-				       (unsigned long) &rx_conf);
-	txchan->device->device_control(txchan, DMA_SLAVE_CONFIG,
-				       (unsigned long) &tx_conf);
+	dmaengine_slave_config(rxchan, &rx_conf);
+	dmaengine_slave_config(txchan, &tx_conf);
 
 	/* Create sglists for the transfers */
 	pages = (pl022->cur_transfer->len >> PAGE_SHIFT) + 1;
@@ -1018,23 +1023,17 @@ static int configure_dma(struct pl022 *pl022)
 	rxdesc->callback_param = pl022;
 
 	/* Submit and fire RX and TX with TX last so we're ready to read! */
-	cookie = rxdesc->tx_submit(rxdesc);
-	if (dma_submit_error(cookie))
-		goto err_submit_rx;
-	cookie = txdesc->tx_submit(txdesc);
-	if (dma_submit_error(cookie))
-		goto err_submit_tx;
-	rxchan->device->device_issue_pending(rxchan);
-	txchan->device->device_issue_pending(txchan);
+	dmaengine_submit(rxdesc);
+	dmaengine_submit(txdesc);
+	dma_async_issue_pending(rxchan);
+	dma_async_issue_pending(txchan);
 
 	return 0;
 
-err_submit_tx:
-err_submit_rx:
 err_txdesc:
-	txchan->device->device_control(txchan, DMA_TERMINATE_ALL, 0);
+	dmaengine_terminate_all(txchan);
 err_rxdesc:
-	rxchan->device->device_control(rxchan, DMA_TERMINATE_ALL, 0);
+	dmaengine_terminate_all(rxchan);
 	dma_unmap_sg(txchan->device->dev, pl022->sgt_tx.sgl,
 		     pl022->sgt_tx.nents, DMA_TO_DEVICE);
 err_tx_sgmap:
@@ -1101,8 +1100,8 @@ static void terminate_dma(struct pl022 *pl022)
 	struct dma_chan *rxchan = pl022->dma_rx_channel;
 	struct dma_chan *txchan = pl022->dma_tx_channel;
 
-	rxchan->device->device_control(rxchan, DMA_TERMINATE_ALL, 0);
-	txchan->device->device_control(txchan, DMA_TERMINATE_ALL, 0);
+	dmaengine_terminate_all(rxchan);
+	dmaengine_terminate_all(txchan);
 	unmap_free_dma_scatter(pl022);
 }
 
@@ -1482,9 +1481,11 @@ static void pump_messages(struct work_struct *work)
 	/* Setup the SPI using the per chip configuration */
 	pl022->cur_chip = spi_get_ctldata(pl022->cur_msg->spi);
 	/*
-	 * We enable the clocks here, then the clocks will be disabled when
-	 * giveback() is called in each method (poll/interrupt/DMA)
+	 * We enable the core voltage and clocks here, then the clocks
+	 * and core will be disabled when giveback() is called in each method
+	 * (poll/interrupt/DMA)
 	 */
+	amba_vcore_enable(pl022->adev);
 	amba_pclk_enable(pl022->adev);
 	clk_enable(pl022->clk);
 	restore_state(pl022);
@@ -1910,8 +1911,6 @@ static int pl022_setup(struct spi_device *spi)
 	    && ((pl022->master_info)->enable_dma)) {
 		chip->enable_dma = true;
 		dev_dbg(&spi->dev, "DMA mode set in controller state\n");
-		if (status < 0)
-			goto err_config_params;
 		SSP_WRITE_BITS(chip->dmacr, SSP_DMA_ENABLED,
 			       SSP_DMACR_MASK_RXDMAE, 0);
 		SSP_WRITE_BITS(chip->dmacr, SSP_DMA_ENABLED,
@@ -2021,7 +2020,7 @@ static void pl022_cleanup(struct spi_device *spi)
 
 
 static int __devinit
-pl022_probe(struct amba_device *adev, struct amba_id *id)
+pl022_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	struct device *dev = &adev->dev;
 	struct pl022_ssp_controller *platform_info = adev->dev.platform_data;
@@ -2130,8 +2129,12 @@ pl022_probe(struct amba_device *adev, struct amba_id *id)
 		goto err_spi_register;
 	}
 	dev_dbg(dev, "probe succeded\n");
-	/* Disable the silicon block pclk and clock it when needed */
+	/*
+	 * Disable the silicon block pclk and any voltage domain and just
+	 * power it up and clock it when it's needed
+	 */
 	amba_pclk_disable(adev);
+	amba_vcore_disable(adev);
 	return 0;
 
  err_spi_register:
@@ -2196,9 +2199,11 @@ static int pl022_suspend(struct amba_device *adev, pm_message_t state)
 		return status;
 	}
 
+	amba_vcore_enable(adev);
 	amba_pclk_enable(adev);
 	load_ssp_default_config(pl022);
 	amba_pclk_disable(adev);
+	amba_vcore_disable(adev);
 	dev_dbg(&adev->dev, "suspended\n");
 	return 0;
 }
diff --git a/drivers/spi/davinci_spi.c b/drivers/spi/davinci_spi.c
index 6beab99bf95b..166a879fd9e8 100644
--- a/drivers/spi/davinci_spi.c
+++ b/drivers/spi/davinci_spi.c
@@ -790,7 +790,6 @@ static int davinci_spi_probe(struct platform_device *pdev)
 	struct resource *r, *mem;
 	resource_size_t dma_rx_chan = SPI_NO_RESOURCE;
 	resource_size_t	dma_tx_chan = SPI_NO_RESOURCE;
-	resource_size_t	dma_eventq = SPI_NO_RESOURCE;
 	int i = 0, ret = 0;
 	u32 spipc0;
 
@@ -878,17 +877,13 @@ static int davinci_spi_probe(struct platform_device *pdev)
 	r = platform_get_resource(pdev, IORESOURCE_DMA, 1);
 	if (r)
 		dma_tx_chan = r->start;
-	r = platform_get_resource(pdev, IORESOURCE_DMA, 2);
-	if (r)
-		dma_eventq = r->start;
 
 	dspi->bitbang.txrx_bufs = davinci_spi_bufs;
 	if (dma_rx_chan != SPI_NO_RESOURCE &&
-	    dma_tx_chan != SPI_NO_RESOURCE &&
-	    dma_eventq != SPI_NO_RESOURCE) {
+	    dma_tx_chan != SPI_NO_RESOURCE) {
 		dspi->dma.rx_channel = dma_rx_chan;
 		dspi->dma.tx_channel = dma_tx_chan;
-		dspi->dma.eventq = dma_eventq;
+		dspi->dma.eventq = pdata->dma_event_q;
 
 		ret = davinci_spi_request_dma(dspi);
 		if (ret)
@@ -897,7 +892,7 @@ static int davinci_spi_probe(struct platform_device *pdev)
 		dev_info(&pdev->dev, "DMA: supported\n");
 		dev_info(&pdev->dev, "DMA: RX channel: %d, TX channel: %d, "
 				"event queue: %d\n", dma_rx_chan, dma_tx_chan,
-				dma_eventq);
+				pdata->dma_event_q);
 	}
 
 	dspi->get_rx = davinci_spi_rx_buf_u8;
diff --git a/drivers/spi/mpc512x_psc_spi.c b/drivers/spi/mpc512x_psc_spi.c
index 77d9e7ee8b27..6a5b4238fb6b 100644
--- a/drivers/spi/mpc512x_psc_spi.c
+++ b/drivers/spi/mpc512x_psc_spi.c
@@ -507,8 +507,7 @@ static int __devexit mpc512x_psc_spi_do_remove(struct device *dev)
 	return 0;
 }
 
-static int __devinit mpc512x_psc_spi_of_probe(struct platform_device *op,
-					      const struct of_device_id *match)
+static int __devinit mpc512x_psc_spi_of_probe(struct platform_device *op)
 {
 	const u32 *regaddr_p;
 	u64 regaddr64, size64;
@@ -551,7 +550,7 @@ static struct of_device_id mpc512x_psc_spi_of_match[] = {
 
 MODULE_DEVICE_TABLE(of, mpc512x_psc_spi_of_match);
 
-static struct of_platform_driver mpc512x_psc_spi_of_driver = {
+static struct platform_driver mpc512x_psc_spi_of_driver = {
 	.probe = mpc512x_psc_spi_of_probe,
 	.remove = __devexit_p(mpc512x_psc_spi_of_remove),
 	.driver = {
@@ -563,13 +562,13 @@ static struct of_platform_driver mpc512x_psc_spi_of_driver = {
 
 static int __init mpc512x_psc_spi_init(void)
 {
-	return of_register_platform_driver(&mpc512x_psc_spi_of_driver);
+	return platform_driver_register(&mpc512x_psc_spi_of_driver);
 }
 module_init(mpc512x_psc_spi_init);
 
 static void __exit mpc512x_psc_spi_exit(void)
 {
-	of_unregister_platform_driver(&mpc512x_psc_spi_of_driver);
+	platform_driver_unregister(&mpc512x_psc_spi_of_driver);
 }
 module_exit(mpc512x_psc_spi_exit);
 
diff --git a/drivers/spi/mpc52xx_psc_spi.c b/drivers/spi/mpc52xx_psc_spi.c
index 8a904c1c8485..e30baf0852ac 100644
--- a/drivers/spi/mpc52xx_psc_spi.c
+++ b/drivers/spi/mpc52xx_psc_spi.c
@@ -450,8 +450,7 @@ free_master:
 	return ret;
 }
 
-static int __devinit mpc52xx_psc_spi_of_probe(struct platform_device *op,
-	const struct of_device_id *match)
+static int __devinit mpc52xx_psc_spi_of_probe(struct platform_device *op)
 {
 	const u32 *regaddr_p;
 	u64 regaddr64, size64;
@@ -503,7 +502,7 @@ static const struct of_device_id mpc52xx_psc_spi_of_match[] = {
 
 MODULE_DEVICE_TABLE(of, mpc52xx_psc_spi_of_match);
 
-static struct of_platform_driver mpc52xx_psc_spi_of_driver = {
+static struct platform_driver mpc52xx_psc_spi_of_driver = {
 	.probe = mpc52xx_psc_spi_of_probe,
 	.remove = __devexit_p(mpc52xx_psc_spi_of_remove),
 	.driver = {
@@ -515,13 +514,13 @@ static struct of_platform_driver mpc52xx_psc_spi_of_driver = {
 
 static int __init mpc52xx_psc_spi_init(void)
 {
-	return of_register_platform_driver(&mpc52xx_psc_spi_of_driver);
+	return platform_driver_register(&mpc52xx_psc_spi_of_driver);
 }
 module_init(mpc52xx_psc_spi_init);
 
 static void __exit mpc52xx_psc_spi_exit(void)
 {
-	of_unregister_platform_driver(&mpc52xx_psc_spi_of_driver);
+	platform_driver_unregister(&mpc52xx_psc_spi_of_driver);
 }
 module_exit(mpc52xx_psc_spi_exit);
 
diff --git a/drivers/spi/mpc52xx_spi.c b/drivers/spi/mpc52xx_spi.c
index 84439f655601..015a974bed72 100644
--- a/drivers/spi/mpc52xx_spi.c
+++ b/drivers/spi/mpc52xx_spi.c
@@ -390,8 +390,7 @@ static int mpc52xx_spi_transfer(struct spi_device *spi, struct spi_message *m)
 /*
  * OF Platform Bus Binding
  */
-static int __devinit mpc52xx_spi_probe(struct platform_device *op,
-				       const struct of_device_id *match)
+static int __devinit mpc52xx_spi_probe(struct platform_device *op)
 {
 	struct spi_master *master;
 	struct mpc52xx_spi *ms;
@@ -556,7 +555,7 @@ static const struct of_device_id mpc52xx_spi_match[] __devinitconst = {
 };
 MODULE_DEVICE_TABLE(of, mpc52xx_spi_match);
 
-static struct of_platform_driver mpc52xx_spi_of_driver = {
+static struct platform_driver mpc52xx_spi_of_driver = {
 	.driver = {
 		.name = "mpc52xx-spi",
 		.owner = THIS_MODULE,
@@ -568,13 +567,13 @@ static struct of_platform_driver mpc52xx_spi_of_driver = {
 
 static int __init mpc52xx_spi_init(void)
 {
-	return of_register_platform_driver(&mpc52xx_spi_of_driver);
+	return platform_driver_register(&mpc52xx_spi_of_driver);
 }
 module_init(mpc52xx_spi_init);
 
 static void __exit mpc52xx_spi_exit(void)
 {
-	of_unregister_platform_driver(&mpc52xx_spi_of_driver);
+	platform_driver_unregister(&mpc52xx_spi_of_driver);
 }
 module_exit(mpc52xx_spi_exit);
 
diff --git a/drivers/spi/omap2_mcspi.c b/drivers/spi/omap2_mcspi.c
index abb1ffbf3d20..3a5ed06d3d2f 100644
--- a/drivers/spi/omap2_mcspi.c
+++ b/drivers/spi/omap2_mcspi.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2005, 2006 Nokia Corporation
  * Author:	Samuel Ortiz <samuel.ortiz@nokia.com> and
- *		Juha Yrjölä <juha.yrjola@nokia.com>
+ *		Juha Yrj�l� <juha.yrjola@nokia.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
@@ -33,6 +33,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/spi/spi.h>
 
@@ -46,7 +47,6 @@
 #define OMAP2_MCSPI_MAX_CTRL 		4
 
 #define OMAP2_MCSPI_REVISION		0x00
-#define OMAP2_MCSPI_SYSCONFIG		0x10
 #define OMAP2_MCSPI_SYSSTATUS		0x14
 #define OMAP2_MCSPI_IRQSTATUS		0x18
 #define OMAP2_MCSPI_IRQENABLE		0x1c
@@ -63,13 +63,6 @@
 
 /* per-register bitmasks: */
 
-#define OMAP2_MCSPI_SYSCONFIG_SMARTIDLE	BIT(4)
-#define OMAP2_MCSPI_SYSCONFIG_ENAWAKEUP	BIT(2)
-#define OMAP2_MCSPI_SYSCONFIG_AUTOIDLE	BIT(0)
-#define OMAP2_MCSPI_SYSCONFIG_SOFTRESET	BIT(1)
-
-#define OMAP2_MCSPI_SYSSTATUS_RESETDONE	BIT(0)
-
 #define OMAP2_MCSPI_MODULCTRL_SINGLE	BIT(0)
 #define OMAP2_MCSPI_MODULCTRL_MS	BIT(2)
 #define OMAP2_MCSPI_MODULCTRL_STEST	BIT(3)
@@ -122,13 +115,12 @@ struct omap2_mcspi {
 	spinlock_t		lock;
 	struct list_head	msg_queue;
 	struct spi_master	*master;
-	struct clk		*ick;
-	struct clk		*fck;
 	/* Virtual base address of the controller */
 	void __iomem		*base;
 	unsigned long		phys;
 	/* SPI1 has 4 channels, while SPI2 has 2 */
 	struct omap2_mcspi_dma	*dma_channels;
+	struct  device		*dev;
 };
 
 struct omap2_mcspi_cs {
@@ -144,7 +136,6 @@ struct omap2_mcspi_cs {
  * corresponding registers are modified.
  */
 struct omap2_mcspi_regs {
-	u32 sysconfig;
 	u32 modulctrl;
 	u32 wakeupenable;
 	struct list_head cs;
@@ -268,9 +259,6 @@ static void omap2_mcspi_restore_ctx(struct omap2_mcspi *mcspi)
 	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_MODULCTRL,
 			omap2_mcspi_ctx[spi_cntrl->bus_num - 1].modulctrl);
 
-	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_SYSCONFIG,
-			omap2_mcspi_ctx[spi_cntrl->bus_num - 1].sysconfig);
-
 	mcspi_write_reg(spi_cntrl, OMAP2_MCSPI_WAKEUPENABLE,
 			omap2_mcspi_ctx[spi_cntrl->bus_num - 1].wakeupenable);
 
@@ -280,20 +268,12 @@ static void omap2_mcspi_restore_ctx(struct omap2_mcspi *mcspi)
 }
 static void omap2_mcspi_disable_clocks(struct omap2_mcspi *mcspi)
 {
-	clk_disable(mcspi->ick);
-	clk_disable(mcspi->fck);
+	pm_runtime_put_sync(mcspi->dev);
 }
 
 static int omap2_mcspi_enable_clocks(struct omap2_mcspi *mcspi)
 {
-	if (clk_enable(mcspi->ick))
-		return -ENODEV;
-	if (clk_enable(mcspi->fck))
-		return -ENODEV;
-
-	omap2_mcspi_restore_ctx(mcspi);
-
-	return 0;
+	return pm_runtime_get_sync(mcspi->dev);
 }
 
 static int mcspi_wait_for_reg_bit(void __iomem *reg, unsigned long bit)
@@ -487,6 +467,9 @@ omap2_mcspi_txrx_pio(struct spi_device *spi, struct spi_transfer *xfer)
 	rx_reg		= base + OMAP2_MCSPI_RX0;
 	chstat_reg	= base + OMAP2_MCSPI_CHSTAT0;
 
+	if (c < (word_len>>3))
+		return 0;
+
 	if (word_len <= 8) {
 		u8		*rx;
 		const u8	*tx;
@@ -534,7 +517,7 @@ omap2_mcspi_txrx_pio(struct spi_device *spi, struct spi_transfer *xfer)
 				dev_vdbg(&spi->dev, "read-%d %02x\n",
 						word_len, *(rx - 1));
 			}
-		} while (c);
+		} while (c > (word_len>>3));
 	} else if (word_len <= 16) {
 		u16		*rx;
 		const u16	*tx;
@@ -581,7 +564,7 @@ omap2_mcspi_txrx_pio(struct spi_device *spi, struct spi_transfer *xfer)
 				dev_vdbg(&spi->dev, "read-%d %04x\n",
 						word_len, *(rx - 1));
 			}
-		} while (c);
+		} while (c > (word_len>>3));
 	} else if (word_len <= 32) {
 		u32		*rx;
 		const u32	*tx;
@@ -628,7 +611,7 @@ omap2_mcspi_txrx_pio(struct spi_device *spi, struct spi_transfer *xfer)
 				dev_vdbg(&spi->dev, "read-%d %08x\n",
 						word_len, *(rx - 1));
 			}
-		} while (c);
+		} while (c > (word_len>>3));
 	}
 
 	/* for TX_ONLY mode, be sure all words have shifted out */
@@ -651,6 +634,17 @@ out:
 	return count - c;
 }
 
+static u32 omap2_mcspi_calc_divisor(u32 speed_hz)
+{
+	u32 div;
+
+	for (div = 0; div < 15; div++)
+		if (speed_hz >= (OMAP2_MCSPI_MAX_FREQ >> div))
+			return div;
+
+	return 15;
+}
+
 /* called only when no transfer is active to this device */
 static int omap2_mcspi_setup_transfer(struct spi_device *spi,
 		struct spi_transfer *t)
@@ -673,12 +667,8 @@ static int omap2_mcspi_setup_transfer(struct spi_device *spi,
 	if (t && t->speed_hz)
 		speed_hz = t->speed_hz;
 
-	if (speed_hz) {
-		while (div <= 15 && (OMAP2_MCSPI_MAX_FREQ / (1 << div))
-					> speed_hz)
-			div++;
-	} else
-		div = 15;
+	speed_hz = min_t(u32, speed_hz, OMAP2_MCSPI_MAX_FREQ);
+	div = omap2_mcspi_calc_divisor(speed_hz);
 
 	l = mcspi_cached_chconf0(spi);
 
@@ -715,7 +705,7 @@ static int omap2_mcspi_setup_transfer(struct spi_device *spi,
 	mcspi_write_chconf0(spi, l);
 
 	dev_dbg(&spi->dev, "setup: speed %d, sample %s edge, clk %s\n",
-			OMAP2_MCSPI_MAX_FREQ / (1 << div),
+			OMAP2_MCSPI_MAX_FREQ >> div,
 			(spi->mode & SPI_CPHA) ? "trailing" : "leading",
 			(spi->mode & SPI_CPOL) ? "inverted" : "normal");
 
@@ -819,8 +809,9 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 			return ret;
 	}
 
-	if (omap2_mcspi_enable_clocks(mcspi))
-		return -ENODEV;
+	ret = omap2_mcspi_enable_clocks(mcspi);
+	if (ret < 0)
+		return ret;
 
 	ret = omap2_mcspi_setup_transfer(spi, NULL);
 	omap2_mcspi_disable_clocks(mcspi);
@@ -863,10 +854,11 @@ static void omap2_mcspi_work(struct work_struct *work)
 	struct omap2_mcspi	*mcspi;
 
 	mcspi = container_of(work, struct omap2_mcspi, work);
-	spin_lock_irq(&mcspi->lock);
 
-	if (omap2_mcspi_enable_clocks(mcspi))
-		goto out;
+	if (omap2_mcspi_enable_clocks(mcspi) < 0)
+		return;
+
+	spin_lock_irq(&mcspi->lock);
 
 	/* We only enable one channel at a time -- the one whose message is
 	 * at the head of the queue -- although this controller would gladly
@@ -979,10 +971,9 @@ static void omap2_mcspi_work(struct work_struct *work)
 		spin_lock_irq(&mcspi->lock);
 	}
 
-	omap2_mcspi_disable_clocks(mcspi);
-
-out:
 	spin_unlock_irq(&mcspi->lock);
+
+	omap2_mcspi_disable_clocks(mcspi);
 }
 
 static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
@@ -1015,10 +1006,10 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 					t->bits_per_word);
 			return -EINVAL;
 		}
-		if (t->speed_hz && t->speed_hz < OMAP2_MCSPI_MAX_FREQ/(1<<16)) {
-			dev_dbg(&spi->dev, "%d Hz max exceeds %d\n",
-					t->speed_hz,
-					OMAP2_MCSPI_MAX_FREQ/(1<<16));
+		if (t->speed_hz && t->speed_hz < (OMAP2_MCSPI_MAX_FREQ >> 15)) {
+			dev_dbg(&spi->dev, "speed_hz %d below minimum %d Hz\n",
+				t->speed_hz,
+				OMAP2_MCSPI_MAX_FREQ >> 15);
 			return -EINVAL;
 		}
 
@@ -1058,25 +1049,15 @@ static int omap2_mcspi_transfer(struct spi_device *spi, struct spi_message *m)
 	return 0;
 }
 
-static int __init omap2_mcspi_reset(struct omap2_mcspi *mcspi)
+static int __init omap2_mcspi_master_setup(struct omap2_mcspi *mcspi)
 {
 	struct spi_master	*master = mcspi->master;
 	u32			tmp;
+	int ret = 0;
 
-	if (omap2_mcspi_enable_clocks(mcspi))
-		return -1;
-
-	mcspi_write_reg(master, OMAP2_MCSPI_SYSCONFIG,
-			OMAP2_MCSPI_SYSCONFIG_SOFTRESET);
-	do {
-		tmp = mcspi_read_reg(master, OMAP2_MCSPI_SYSSTATUS);
-	} while (!(tmp & OMAP2_MCSPI_SYSSTATUS_RESETDONE));
-
-	tmp = OMAP2_MCSPI_SYSCONFIG_AUTOIDLE |
-		OMAP2_MCSPI_SYSCONFIG_ENAWAKEUP |
-		OMAP2_MCSPI_SYSCONFIG_SMARTIDLE;
-	mcspi_write_reg(master, OMAP2_MCSPI_SYSCONFIG, tmp);
-	omap2_mcspi_ctx[master->bus_num - 1].sysconfig = tmp;
+	ret = omap2_mcspi_enable_clocks(mcspi);
+	if (ret < 0)
+		return ret;
 
 	tmp = OMAP2_MCSPI_WAKEUPENABLE_WKEN;
 	mcspi_write_reg(master, OMAP2_MCSPI_WAKEUPENABLE, tmp);
@@ -1087,91 +1068,26 @@ static int __init omap2_mcspi_reset(struct omap2_mcspi *mcspi)
 	return 0;
 }
 
-static u8 __initdata spi1_rxdma_id [] = {
-	OMAP24XX_DMA_SPI1_RX0,
-	OMAP24XX_DMA_SPI1_RX1,
-	OMAP24XX_DMA_SPI1_RX2,
-	OMAP24XX_DMA_SPI1_RX3,
-};
-
-static u8 __initdata spi1_txdma_id [] = {
-	OMAP24XX_DMA_SPI1_TX0,
-	OMAP24XX_DMA_SPI1_TX1,
-	OMAP24XX_DMA_SPI1_TX2,
-	OMAP24XX_DMA_SPI1_TX3,
-};
-
-static u8 __initdata spi2_rxdma_id[] = {
-	OMAP24XX_DMA_SPI2_RX0,
-	OMAP24XX_DMA_SPI2_RX1,
-};
-
-static u8 __initdata spi2_txdma_id[] = {
-	OMAP24XX_DMA_SPI2_TX0,
-	OMAP24XX_DMA_SPI2_TX1,
-};
-
-#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3) \
-	|| defined(CONFIG_ARCH_OMAP4)
-static u8 __initdata spi3_rxdma_id[] = {
-	OMAP24XX_DMA_SPI3_RX0,
-	OMAP24XX_DMA_SPI3_RX1,
-};
+static int omap_mcspi_runtime_resume(struct device *dev)
+{
+	struct omap2_mcspi	*mcspi;
+	struct spi_master	*master;
 
-static u8 __initdata spi3_txdma_id[] = {
-	OMAP24XX_DMA_SPI3_TX0,
-	OMAP24XX_DMA_SPI3_TX1,
-};
-#endif
+	master = dev_get_drvdata(dev);
+	mcspi = spi_master_get_devdata(master);
+	omap2_mcspi_restore_ctx(mcspi);
 
-#if defined(CONFIG_ARCH_OMAP3) || defined(CONFIG_ARCH_OMAP4)
-static u8 __initdata spi4_rxdma_id[] = {
-	OMAP34XX_DMA_SPI4_RX0,
-};
+	return 0;
+}
 
-static u8 __initdata spi4_txdma_id[] = {
-	OMAP34XX_DMA_SPI4_TX0,
-};
-#endif
 
 static int __init omap2_mcspi_probe(struct platform_device *pdev)
 {
 	struct spi_master	*master;
+	struct omap2_mcspi_platform_config *pdata = pdev->dev.platform_data;
 	struct omap2_mcspi	*mcspi;
 	struct resource		*r;
 	int			status = 0, i;
-	const u8		*rxdma_id, *txdma_id;
-	unsigned		num_chipselect;
-
-	switch (pdev->id) {
-	case 1:
-		rxdma_id = spi1_rxdma_id;
-		txdma_id = spi1_txdma_id;
-		num_chipselect = 4;
-		break;
-	case 2:
-		rxdma_id = spi2_rxdma_id;
-		txdma_id = spi2_txdma_id;
-		num_chipselect = 2;
-		break;
-#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3) \
-	|| defined(CONFIG_ARCH_OMAP4)
-	case 3:
-		rxdma_id = spi3_rxdma_id;
-		txdma_id = spi3_txdma_id;
-		num_chipselect = 2;
-		break;
-#endif
-#if defined(CONFIG_ARCH_OMAP3) || defined(CONFIG_ARCH_OMAP4)
-	case 4:
-		rxdma_id = spi4_rxdma_id;
-		txdma_id = spi4_txdma_id;
-		num_chipselect = 1;
-		break;
-#endif
-	default:
-		return -EINVAL;
-	}
 
 	master = spi_alloc_master(&pdev->dev, sizeof *mcspi);
 	if (master == NULL) {
@@ -1188,7 +1104,7 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 	master->setup = omap2_mcspi_setup;
 	master->transfer = omap2_mcspi_transfer;
 	master->cleanup = omap2_mcspi_cleanup;
-	master->num_chipselect = num_chipselect;
+	master->num_chipselect = pdata->num_cs;
 
 	dev_set_drvdata(&pdev->dev, master);
 
@@ -1206,49 +1122,62 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
+	r->start += pdata->regs_offset;
+	r->end += pdata->regs_offset;
 	mcspi->phys = r->start;
 	mcspi->base = ioremap(r->start, r->end - r->start + 1);
 	if (!mcspi->base) {
 		dev_dbg(&pdev->dev, "can't ioremap MCSPI\n");
 		status = -ENOMEM;
-		goto err1aa;
+		goto err2;
 	}
 
+	mcspi->dev = &pdev->dev;
 	INIT_WORK(&mcspi->work, omap2_mcspi_work);
 
 	spin_lock_init(&mcspi->lock);
 	INIT_LIST_HEAD(&mcspi->msg_queue);
 	INIT_LIST_HEAD(&omap2_mcspi_ctx[master->bus_num - 1].cs);
 
-	mcspi->ick = clk_get(&pdev->dev, "ick");
-	if (IS_ERR(mcspi->ick)) {
-		dev_dbg(&pdev->dev, "can't get mcspi_ick\n");
-		status = PTR_ERR(mcspi->ick);
-		goto err1a;
-	}
-	mcspi->fck = clk_get(&pdev->dev, "fck");
-	if (IS_ERR(mcspi->fck)) {
-		dev_dbg(&pdev->dev, "can't get mcspi_fck\n");
-		status = PTR_ERR(mcspi->fck);
-		goto err2;
-	}
-
 	mcspi->dma_channels = kcalloc(master->num_chipselect,
 			sizeof(struct omap2_mcspi_dma),
 			GFP_KERNEL);
 
 	if (mcspi->dma_channels == NULL)
-		goto err3;
+		goto err2;
+
+	for (i = 0; i < master->num_chipselect; i++) {
+		char dma_ch_name[14];
+		struct resource *dma_res;
+
+		sprintf(dma_ch_name, "rx%d", i);
+		dma_res = platform_get_resource_byname(pdev, IORESOURCE_DMA,
+							dma_ch_name);
+		if (!dma_res) {
+			dev_dbg(&pdev->dev, "cannot get DMA RX channel\n");
+			status = -ENODEV;
+			break;
+		}
 
-	for (i = 0; i < num_chipselect; i++) {
 		mcspi->dma_channels[i].dma_rx_channel = -1;
-		mcspi->dma_channels[i].dma_rx_sync_dev = rxdma_id[i];
+		mcspi->dma_channels[i].dma_rx_sync_dev = dma_res->start;
+		sprintf(dma_ch_name, "tx%d", i);
+		dma_res = platform_get_resource_byname(pdev, IORESOURCE_DMA,
+							dma_ch_name);
+		if (!dma_res) {
+			dev_dbg(&pdev->dev, "cannot get DMA TX channel\n");
+			status = -ENODEV;
+			break;
+		}
+
 		mcspi->dma_channels[i].dma_tx_channel = -1;
-		mcspi->dma_channels[i].dma_tx_sync_dev = txdma_id[i];
+		mcspi->dma_channels[i].dma_tx_sync_dev = dma_res->start;
 	}
 
-	if (omap2_mcspi_reset(mcspi) < 0)
-		goto err4;
+	pm_runtime_enable(&pdev->dev);
+
+	if (status || omap2_mcspi_master_setup(mcspi) < 0)
+		goto err3;
 
 	status = spi_register_master(master);
 	if (status < 0)
@@ -1257,17 +1186,13 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 	return status;
 
 err4:
-	kfree(mcspi->dma_channels);
+	spi_master_put(master);
 err3:
-	clk_put(mcspi->fck);
+	kfree(mcspi->dma_channels);
 err2:
-	clk_put(mcspi->ick);
-err1a:
-	iounmap(mcspi->base);
-err1aa:
 	release_mem_region(r->start, (r->end - r->start) + 1);
+	iounmap(mcspi->base);
 err1:
-	spi_master_put(master);
 	return status;
 }
 
@@ -1283,9 +1208,7 @@ static int __exit omap2_mcspi_remove(struct platform_device *pdev)
 	mcspi = spi_master_get_devdata(master);
 	dma_channels = mcspi->dma_channels;
 
-	clk_put(mcspi->fck);
-	clk_put(mcspi->ick);
-
+	omap2_mcspi_disable_clocks(mcspi);
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(r->start, (r->end - r->start) + 1);
 
@@ -1336,6 +1259,7 @@ static int omap2_mcspi_resume(struct device *dev)
 
 static const struct dev_pm_ops omap2_mcspi_pm_ops = {
 	.resume = omap2_mcspi_resume,
+	.runtime_resume	= omap_mcspi_runtime_resume,
 };
 
 static struct platform_driver omap2_mcspi_driver = {
diff --git a/drivers/spi/pxa2xx_spi.c b/drivers/spi/pxa2xx_spi.c
index 95928833855b..a429b01d0285 100644
--- a/drivers/spi/pxa2xx_spi.c
+++ b/drivers/spi/pxa2xx_spi.c
@@ -1557,9 +1557,7 @@ static int __devinit pxa2xx_spi_probe(struct platform_device *pdev)
 	drv_data->ssp = ssp;
 
 	master->dev.parent = &pdev->dev;
-#ifdef CONFIG_OF
 	master->dev.of_node = pdev->dev.of_node;
-#endif
 	/* the spi->mode bits understood by this driver: */
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
 
diff --git a/drivers/spi/pxa2xx_spi_pci.c b/drivers/spi/pxa2xx_spi_pci.c
index 19752b09e155..378e504f89eb 100644
--- a/drivers/spi/pxa2xx_spi_pci.c
+++ b/drivers/spi/pxa2xx_spi_pci.c
@@ -89,9 +89,7 @@ static int __devinit ce4100_spi_probe(struct pci_dev *dev,
 		goto err_nomem;
 
 	pdev->dev.parent = &dev->dev;
-#ifdef CONFIG_OF
 	pdev->dev.of_node = dev->dev.of_node;
-#endif
 	ssp = &spi_info->ssp;
 	ssp->phys_base = pci_resource_start(dev, 0);
 	ssp->mmio_base = ioremap(phys_beg, phys_len);
diff --git a/drivers/spi/spi_altera.c b/drivers/spi/spi_altera.c
new file mode 100644
index 000000000000..4813a63ce6fb
--- /dev/null
+++ b/drivers/spi/spi_altera.c
@@ -0,0 +1,339 @@
+/*
+ * Altera SPI driver
+ *
+ * Copyright (C) 2008 Thomas Chou <thomas@wytron.com.tw>
+ *
+ * Based on spi_s3c24xx.c, which is:
+ * Copyright (c) 2006 Ben Dooks
+ * Copyright (c) 2006 Simtec Electronics
+ *	Ben Dooks <ben@simtec.co.uk>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+#define DRV_NAME "spi_altera"
+
+#define ALTERA_SPI_RXDATA	0
+#define ALTERA_SPI_TXDATA	4
+#define ALTERA_SPI_STATUS	8
+#define ALTERA_SPI_CONTROL	12
+#define ALTERA_SPI_SLAVE_SEL	20
+
+#define ALTERA_SPI_STATUS_ROE_MSK	0x8
+#define ALTERA_SPI_STATUS_TOE_MSK	0x10
+#define ALTERA_SPI_STATUS_TMT_MSK	0x20
+#define ALTERA_SPI_STATUS_TRDY_MSK	0x40
+#define ALTERA_SPI_STATUS_RRDY_MSK	0x80
+#define ALTERA_SPI_STATUS_E_MSK		0x100
+
+#define ALTERA_SPI_CONTROL_IROE_MSK	0x8
+#define ALTERA_SPI_CONTROL_ITOE_MSK	0x10
+#define ALTERA_SPI_CONTROL_ITRDY_MSK	0x40
+#define ALTERA_SPI_CONTROL_IRRDY_MSK	0x80
+#define ALTERA_SPI_CONTROL_IE_MSK	0x100
+#define ALTERA_SPI_CONTROL_SSO_MSK	0x400
+
+struct altera_spi {
+	/* bitbang has to be first */
+	struct spi_bitbang bitbang;
+	struct completion done;
+
+	void __iomem *base;
+	int irq;
+	int len;
+	int count;
+	int bytes_per_word;
+	unsigned long imr;
+
+	/* data buffers */
+	const unsigned char *tx;
+	unsigned char *rx;
+};
+
+static inline struct altera_spi *altera_spi_to_hw(struct spi_device *sdev)
+{
+	return spi_master_get_devdata(sdev->master);
+}
+
+static void altera_spi_chipsel(struct spi_device *spi, int value)
+{
+	struct altera_spi *hw = altera_spi_to_hw(spi);
+
+	if (spi->mode & SPI_CS_HIGH) {
+		switch (value) {
+		case BITBANG_CS_INACTIVE:
+			writel(1 << spi->chip_select,
+			       hw->base + ALTERA_SPI_SLAVE_SEL);
+			hw->imr |= ALTERA_SPI_CONTROL_SSO_MSK;
+			writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+			break;
+
+		case BITBANG_CS_ACTIVE:
+			hw->imr &= ~ALTERA_SPI_CONTROL_SSO_MSK;
+			writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+			writel(0, hw->base + ALTERA_SPI_SLAVE_SEL);
+			break;
+		}
+	} else {
+		switch (value) {
+		case BITBANG_CS_INACTIVE:
+			hw->imr &= ~ALTERA_SPI_CONTROL_SSO_MSK;
+			writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+			break;
+
+		case BITBANG_CS_ACTIVE:
+			writel(1 << spi->chip_select,
+			       hw->base + ALTERA_SPI_SLAVE_SEL);
+			hw->imr |= ALTERA_SPI_CONTROL_SSO_MSK;
+			writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+			break;
+		}
+	}
+}
+
+static int altera_spi_setupxfer(struct spi_device *spi, struct spi_transfer *t)
+{
+	return 0;
+}
+
+static int altera_spi_setup(struct spi_device *spi)
+{
+	return 0;
+}
+
+static inline unsigned int hw_txbyte(struct altera_spi *hw, int count)
+{
+	if (hw->tx) {
+		switch (hw->bytes_per_word) {
+		case 1:
+			return hw->tx[count];
+		case 2:
+			return (hw->tx[count * 2]
+				| (hw->tx[count * 2 + 1] << 8));
+		}
+	}
+	return 0;
+}
+
+static int altera_spi_txrx(struct spi_device *spi, struct spi_transfer *t)
+{
+	struct altera_spi *hw = altera_spi_to_hw(spi);
+
+	hw->tx = t->tx_buf;
+	hw->rx = t->rx_buf;
+	hw->count = 0;
+	hw->bytes_per_word = (t->bits_per_word ? : spi->bits_per_word) / 8;
+	hw->len = t->len / hw->bytes_per_word;
+
+	if (hw->irq >= 0) {
+		/* enable receive interrupt */
+		hw->imr |= ALTERA_SPI_CONTROL_IRRDY_MSK;
+		writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+
+		/* send the first byte */
+		writel(hw_txbyte(hw, 0), hw->base + ALTERA_SPI_TXDATA);
+
+		wait_for_completion(&hw->done);
+		/* disable receive interrupt */
+		hw->imr &= ~ALTERA_SPI_CONTROL_IRRDY_MSK;
+		writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+	} else {
+		/* send the first byte */
+		writel(hw_txbyte(hw, 0), hw->base + ALTERA_SPI_TXDATA);
+
+		while (1) {
+			unsigned int rxd;
+
+			while (!(readl(hw->base + ALTERA_SPI_STATUS) &
+				 ALTERA_SPI_STATUS_RRDY_MSK))
+				cpu_relax();
+
+			rxd = readl(hw->base + ALTERA_SPI_RXDATA);
+			if (hw->rx) {
+				switch (hw->bytes_per_word) {
+				case 1:
+					hw->rx[hw->count] = rxd;
+					break;
+				case 2:
+					hw->rx[hw->count * 2] = rxd;
+					hw->rx[hw->count * 2 + 1] = rxd >> 8;
+					break;
+				}
+			}
+
+			hw->count++;
+
+			if (hw->count < hw->len)
+				writel(hw_txbyte(hw, hw->count),
+				       hw->base + ALTERA_SPI_TXDATA);
+			else
+				break;
+		}
+
+	}
+
+	return hw->count * hw->bytes_per_word;
+}
+
+static irqreturn_t altera_spi_irq(int irq, void *dev)
+{
+	struct altera_spi *hw = dev;
+	unsigned int rxd;
+
+	rxd = readl(hw->base + ALTERA_SPI_RXDATA);
+	if (hw->rx) {
+		switch (hw->bytes_per_word) {
+		case 1:
+			hw->rx[hw->count] = rxd;
+			break;
+		case 2:
+			hw->rx[hw->count * 2] = rxd;
+			hw->rx[hw->count * 2 + 1] = rxd >> 8;
+			break;
+		}
+	}
+
+	hw->count++;
+
+	if (hw->count < hw->len)
+		writel(hw_txbyte(hw, hw->count), hw->base + ALTERA_SPI_TXDATA);
+	else
+		complete(&hw->done);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit altera_spi_probe(struct platform_device *pdev)
+{
+	struct altera_spi_platform_data *platp = pdev->dev.platform_data;
+	struct altera_spi *hw;
+	struct spi_master *master;
+	struct resource *res;
+	int err = -ENODEV;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(struct altera_spi));
+	if (!master)
+		return err;
+
+	/* setup the master state. */
+	master->bus_num = pdev->id;
+	master->num_chipselect = 16;
+	master->mode_bits = SPI_CS_HIGH;
+	master->setup = altera_spi_setup;
+
+	hw = spi_master_get_devdata(master);
+	platform_set_drvdata(pdev, hw);
+
+	/* setup the state for the bitbang driver */
+	hw->bitbang.master = spi_master_get(master);
+	if (!hw->bitbang.master)
+		return err;
+	hw->bitbang.setup_transfer = altera_spi_setupxfer;
+	hw->bitbang.chipselect = altera_spi_chipsel;
+	hw->bitbang.txrx_bufs = altera_spi_txrx;
+
+	/* find and map our resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		goto exit_busy;
+	if (!devm_request_mem_region(&pdev->dev, res->start, resource_size(res),
+				     pdev->name))
+		goto exit_busy;
+	hw->base = devm_ioremap_nocache(&pdev->dev, res->start,
+					resource_size(res));
+	if (!hw->base)
+		goto exit_busy;
+	/* program defaults into the registers */
+	hw->imr = 0;		/* disable spi interrupts */
+	writel(hw->imr, hw->base + ALTERA_SPI_CONTROL);
+	writel(0, hw->base + ALTERA_SPI_STATUS);	/* clear status reg */
+	if (readl(hw->base + ALTERA_SPI_STATUS) & ALTERA_SPI_STATUS_RRDY_MSK)
+		readl(hw->base + ALTERA_SPI_RXDATA);	/* flush rxdata */
+	/* irq is optional */
+	hw->irq = platform_get_irq(pdev, 0);
+	if (hw->irq >= 0) {
+		init_completion(&hw->done);
+		err = devm_request_irq(&pdev->dev, hw->irq, altera_spi_irq, 0,
+				       pdev->name, hw);
+		if (err)
+			goto exit;
+	}
+	/* find platform data */
+	if (!platp)
+		hw->bitbang.master->dev.of_node = pdev->dev.of_node;
+
+	/* register our spi controller */
+	err = spi_bitbang_start(&hw->bitbang);
+	if (err)
+		goto exit;
+	dev_info(&pdev->dev, "base %p, irq %d\n", hw->base, hw->irq);
+
+	return 0;
+
+exit_busy:
+	err = -EBUSY;
+exit:
+	platform_set_drvdata(pdev, NULL);
+	spi_master_put(master);
+	return err;
+}
+
+static int __devexit altera_spi_remove(struct platform_device *dev)
+{
+	struct altera_spi *hw = platform_get_drvdata(dev);
+	struct spi_master *master = hw->bitbang.master;
+
+	spi_bitbang_stop(&hw->bitbang);
+	platform_set_drvdata(dev, NULL);
+	spi_master_put(master);
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id altera_spi_match[] = {
+	{ .compatible = "ALTR,spi-1.0", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, altera_spi_match);
+#else /* CONFIG_OF */
+#define altera_spi_match NULL
+#endif /* CONFIG_OF */
+
+static struct platform_driver altera_spi_driver = {
+	.probe = altera_spi_probe,
+	.remove = __devexit_p(altera_spi_remove),
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.pm = NULL,
+		.of_match_table = altera_spi_match,
+	},
+};
+
+static int __init altera_spi_init(void)
+{
+	return platform_driver_register(&altera_spi_driver);
+}
+module_init(altera_spi_init);
+
+static void __exit altera_spi_exit(void)
+{
+	platform_driver_unregister(&altera_spi_driver);
+}
+module_exit(altera_spi_exit);
+
+MODULE_DESCRIPTION("Altera SPI driver");
+MODULE_AUTHOR("Thomas Chou <thomas@wytron.com.tw>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/spi/spi_bfin5xx.c b/drivers/spi/spi_bfin5xx.c
index 3f223511127b..a28462486df8 100644
--- a/drivers/spi/spi_bfin5xx.c
+++ b/drivers/spi/spi_bfin5xx.c
@@ -425,6 +425,7 @@ static irqreturn_t bfin_spi_pio_irq_handler(int irq, void *dev_id)
 	struct bfin_spi_slave_data *chip = drv_data->cur_chip;
 	struct spi_message *msg = drv_data->cur_msg;
 	int n_bytes = drv_data->n_bytes;
+	int loop = 0;
 
 	/* wait until transfer finished. */
 	while (!(read_STAT(drv_data) & BIT_STAT_RXS))
@@ -435,10 +436,15 @@ static irqreturn_t bfin_spi_pio_irq_handler(int irq, void *dev_id)
 		/* last read */
 		if (drv_data->rx) {
 			dev_dbg(&drv_data->pdev->dev, "last read\n");
-			if (n_bytes == 2)
-				*(u16 *) (drv_data->rx) = read_RDBR(drv_data);
-			else if (n_bytes == 1)
-				*(u8 *) (drv_data->rx) = read_RDBR(drv_data);
+			if (n_bytes % 2) {
+				u16 *buf = (u16 *)drv_data->rx;
+				for (loop = 0; loop < n_bytes / 2; loop++)
+					*buf++ = read_RDBR(drv_data);
+			} else {
+				u8 *buf = (u8 *)drv_data->rx;
+				for (loop = 0; loop < n_bytes; loop++)
+					*buf++ = read_RDBR(drv_data);
+			}
 			drv_data->rx += n_bytes;
 		}
 
@@ -458,29 +464,53 @@ static irqreturn_t bfin_spi_pio_irq_handler(int irq, void *dev_id)
 	if (drv_data->rx && drv_data->tx) {
 		/* duplex */
 		dev_dbg(&drv_data->pdev->dev, "duplex: write_TDBR\n");
-		if (drv_data->n_bytes == 2) {
-			*(u16 *) (drv_data->rx) = read_RDBR(drv_data);
-			write_TDBR(drv_data, (*(u16 *) (drv_data->tx)));
-		} else if (drv_data->n_bytes == 1) {
-			*(u8 *) (drv_data->rx) = read_RDBR(drv_data);
-			write_TDBR(drv_data, (*(u8 *) (drv_data->tx)));
+		if (n_bytes % 2) {
+			u16 *buf = (u16 *)drv_data->rx;
+			u16 *buf2 = (u16 *)drv_data->tx;
+			for (loop = 0; loop < n_bytes / 2; loop++) {
+				*buf++ = read_RDBR(drv_data);
+				write_TDBR(drv_data, *buf2++);
+			}
+		} else {
+			u8 *buf = (u8 *)drv_data->rx;
+			u8 *buf2 = (u8 *)drv_data->tx;
+			for (loop = 0; loop < n_bytes; loop++) {
+				*buf++ = read_RDBR(drv_data);
+				write_TDBR(drv_data, *buf2++);
+			}
 		}
 	} else if (drv_data->rx) {
 		/* read */
 		dev_dbg(&drv_data->pdev->dev, "read: write_TDBR\n");
-		if (drv_data->n_bytes == 2)
-			*(u16 *) (drv_data->rx) = read_RDBR(drv_data);
-		else if (drv_data->n_bytes == 1)
-			*(u8 *) (drv_data->rx) = read_RDBR(drv_data);
-		write_TDBR(drv_data, chip->idle_tx_val);
+		if (n_bytes % 2) {
+			u16 *buf = (u16 *)drv_data->rx;
+			for (loop = 0; loop < n_bytes / 2; loop++) {
+				*buf++ = read_RDBR(drv_data);
+				write_TDBR(drv_data, chip->idle_tx_val);
+			}
+		} else {
+			u8 *buf = (u8 *)drv_data->rx;
+			for (loop = 0; loop < n_bytes; loop++) {
+				*buf++ = read_RDBR(drv_data);
+				write_TDBR(drv_data, chip->idle_tx_val);
+			}
+		}
 	} else if (drv_data->tx) {
 		/* write */
 		dev_dbg(&drv_data->pdev->dev, "write: write_TDBR\n");
-		bfin_spi_dummy_read(drv_data);
-		if (drv_data->n_bytes == 2)
-			write_TDBR(drv_data, (*(u16 *) (drv_data->tx)));
-		else if (drv_data->n_bytes == 1)
-			write_TDBR(drv_data, (*(u8 *) (drv_data->tx)));
+		if (n_bytes % 2) {
+			u16 *buf = (u16 *)drv_data->tx;
+			for (loop = 0; loop < n_bytes / 2; loop++) {
+				read_RDBR(drv_data);
+				write_TDBR(drv_data, *buf++);
+			}
+		} else {
+			u8 *buf = (u8 *)drv_data->tx;
+			for (loop = 0; loop < n_bytes; loop++) {
+				read_RDBR(drv_data);
+				write_TDBR(drv_data, *buf++);
+			}
+		}
 	}
 
 	if (drv_data->tx)
@@ -623,6 +653,7 @@ static void bfin_spi_pump_transfers(unsigned long data)
 		message->state = bfin_spi_next_transfer(drv_data);
 		/* Schedule next transfer tasklet */
 		tasklet_schedule(&drv_data->pump_transfers);
+		return;
 	}
 
 	if (transfer->tx_buf != NULL) {
@@ -651,16 +682,16 @@ static void bfin_spi_pump_transfers(unsigned long data)
 
 	/* Bits per word setup */
 	bits_per_word = transfer->bits_per_word ? : message->spi->bits_per_word;
-	if (bits_per_word == 8) {
-		drv_data->n_bytes = 1;
-		drv_data->len = transfer->len;
-		cr_width = 0;
-		drv_data->ops = &bfin_bfin_spi_transfer_ops_u8;
-	} else if (bits_per_word == 16) {
-		drv_data->n_bytes = 2;
+	if ((bits_per_word > 0) && (bits_per_word % 16 == 0)) {
+		drv_data->n_bytes = bits_per_word/8;
 		drv_data->len = (transfer->len) >> 1;
 		cr_width = BIT_CTL_WORDSIZE;
 		drv_data->ops = &bfin_bfin_spi_transfer_ops_u16;
+	} else if ((bits_per_word > 0) && (bits_per_word % 8 == 0)) {
+		drv_data->n_bytes = bits_per_word/8;
+		drv_data->len = transfer->len;
+		cr_width = 0;
+		drv_data->ops = &bfin_bfin_spi_transfer_ops_u8;
 	} else {
 		dev_err(&drv_data->pdev->dev, "transfer: unsupported bits_per_word\n");
 		message->status = -EINVAL;
@@ -815,10 +846,19 @@ static void bfin_spi_pump_transfers(unsigned long data)
 		if (drv_data->tx == NULL)
 			write_TDBR(drv_data, chip->idle_tx_val);
 		else {
-			if (bits_per_word == 8)
-				write_TDBR(drv_data, (*(u8 *) (drv_data->tx)));
-			else
-				write_TDBR(drv_data, (*(u16 *) (drv_data->tx)));
+			int loop;
+			if (bits_per_word % 16 == 0) {
+				u16 *buf = (u16 *)drv_data->tx;
+				for (loop = 0; loop < bits_per_word / 16;
+						loop++) {
+					write_TDBR(drv_data, *buf++);
+				}
+			} else if (bits_per_word % 8 == 0) {
+				u8 *buf = (u8 *)drv_data->tx;
+				for (loop = 0; loop < bits_per_word / 8; loop++)
+					write_TDBR(drv_data, *buf++);
+			}
+
 			drv_data->tx += drv_data->n_bytes;
 		}
 
@@ -1031,7 +1071,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 		chip->ctl_reg &= bfin_ctl_reg;
 	}
 
-	if (spi->bits_per_word != 8 && spi->bits_per_word != 16) {
+	if (spi->bits_per_word % 8) {
 		dev_err(&spi->dev, "%d bits_per_word is not supported\n",
 				spi->bits_per_word);
 		goto error;
diff --git a/drivers/spi/spi_bitbang.c b/drivers/spi/spi_bitbang.c
index 8b55724d5f39..14a63f6010d1 100644
--- a/drivers/spi/spi_bitbang.c
+++ b/drivers/spi/spi_bitbang.c
@@ -259,10 +259,6 @@ static void bitbang_work(struct work_struct *work)
 	struct spi_bitbang	*bitbang =
 		container_of(work, struct spi_bitbang, work);
 	unsigned long		flags;
-	int			(*setup_transfer)(struct spi_device *,
-					struct spi_transfer *);
-
-	setup_transfer = bitbang->setup_transfer;
 
 	spin_lock_irqsave(&bitbang->lock, flags);
 	bitbang->busy = 1;
@@ -300,11 +296,7 @@ static void bitbang_work(struct work_struct *work)
 
 			/* init (-1) or override (1) transfer params */
 			if (do_setup != 0) {
-				if (!setup_transfer) {
-					status = -ENOPROTOOPT;
-					break;
-				}
-				status = setup_transfer(spi, t);
+				status = bitbang->setup_transfer(spi, t);
 				if (status < 0)
 					break;
 				if (do_setup == -1)
@@ -465,6 +457,9 @@ int spi_bitbang_start(struct spi_bitbang *bitbang)
 		}
 	} else if (!bitbang->master->setup)
 		return -EINVAL;
+	if (bitbang->master->transfer == spi_bitbang_transfer &&
+			!bitbang->setup_transfer)
+		return -EINVAL;
 
 	/* this task is the only thing to touch the SPI bits */
 	bitbang->busy = 0;
diff --git a/drivers/spi/spi_fsl_espi.c b/drivers/spi/spi_fsl_espi.c
index a99e2333b949..900e921ab80e 100644
--- a/drivers/spi/spi_fsl_espi.c
+++ b/drivers/spi/spi_fsl_espi.c
@@ -685,8 +685,7 @@ static int of_fsl_espi_get_chipselects(struct device *dev)
 	return 0;
 }
 
-static int __devinit of_fsl_espi_probe(struct platform_device *ofdev,
-					const struct of_device_id *ofid)
+static int __devinit of_fsl_espi_probe(struct platform_device *ofdev)
 {
 	struct device *dev = &ofdev->dev;
 	struct device_node *np = ofdev->dev.of_node;
@@ -695,7 +694,7 @@ static int __devinit of_fsl_espi_probe(struct platform_device *ofdev,
 	struct resource irq;
 	int ret = -ENOMEM;
 
-	ret = of_mpc8xxx_spi_probe(ofdev, ofid);
+	ret = of_mpc8xxx_spi_probe(ofdev);
 	if (ret)
 		return ret;
 
@@ -736,7 +735,7 @@ static const struct of_device_id of_fsl_espi_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_fsl_espi_match);
 
-static struct of_platform_driver fsl_espi_driver = {
+static struct platform_driver fsl_espi_driver = {
 	.driver = {
 		.name = "fsl_espi",
 		.owner = THIS_MODULE,
@@ -748,13 +747,13 @@ static struct of_platform_driver fsl_espi_driver = {
 
 static int __init fsl_espi_init(void)
 {
-	return of_register_platform_driver(&fsl_espi_driver);
+	return platform_driver_register(&fsl_espi_driver);
 }
 module_init(fsl_espi_init);
 
 static void __exit fsl_espi_exit(void)
 {
-	of_unregister_platform_driver(&fsl_espi_driver);
+	platform_driver_unregister(&fsl_espi_driver);
 }
 module_exit(fsl_espi_exit);
 
diff --git a/drivers/spi/spi_fsl_lib.c b/drivers/spi/spi_fsl_lib.c
index 5cd741fdb5c3..ff59f42ae990 100644
--- a/drivers/spi/spi_fsl_lib.c
+++ b/drivers/spi/spi_fsl_lib.c
@@ -189,8 +189,7 @@ int __devexit mpc8xxx_spi_remove(struct device *dev)
 	return 0;
 }
 
-int __devinit of_mpc8xxx_spi_probe(struct platform_device *ofdev,
-					const struct of_device_id *ofid)
+int __devinit of_mpc8xxx_spi_probe(struct platform_device *ofdev)
 {
 	struct device *dev = &ofdev->dev;
 	struct device_node *np = ofdev->dev.of_node;
diff --git a/drivers/spi/spi_fsl_lib.h b/drivers/spi/spi_fsl_lib.h
index 281e060977cd..cbe881b9ea76 100644
--- a/drivers/spi/spi_fsl_lib.h
+++ b/drivers/spi/spi_fsl_lib.h
@@ -118,7 +118,6 @@ extern const char *mpc8xxx_spi_strmode(unsigned int flags);
 extern int mpc8xxx_spi_probe(struct device *dev, struct resource *mem,
 		unsigned int irq);
 extern int mpc8xxx_spi_remove(struct device *dev);
-extern int of_mpc8xxx_spi_probe(struct platform_device *ofdev,
-				const struct of_device_id *ofid);
+extern int of_mpc8xxx_spi_probe(struct platform_device *ofdev);
 
 #endif /* __SPI_FSL_LIB_H__ */
diff --git a/drivers/spi/spi_fsl_spi.c b/drivers/spi/spi_fsl_spi.c
index 7ca52d3ae8f8..7963c9b49566 100644
--- a/drivers/spi/spi_fsl_spi.c
+++ b/drivers/spi/spi_fsl_spi.c
@@ -1042,8 +1042,7 @@ static int of_fsl_spi_free_chipselects(struct device *dev)
 	return 0;
 }
 
-static int __devinit of_fsl_spi_probe(struct platform_device *ofdev,
-					const struct of_device_id *ofid)
+static int __devinit of_fsl_spi_probe(struct platform_device *ofdev)
 {
 	struct device *dev = &ofdev->dev;
 	struct device_node *np = ofdev->dev.of_node;
@@ -1052,7 +1051,7 @@ static int __devinit of_fsl_spi_probe(struct platform_device *ofdev,
 	struct resource irq;
 	int ret = -ENOMEM;
 
-	ret = of_mpc8xxx_spi_probe(ofdev, ofid);
+	ret = of_mpc8xxx_spi_probe(ofdev);
 	if (ret)
 		return ret;
 
@@ -1100,7 +1099,7 @@ static const struct of_device_id of_fsl_spi_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_fsl_spi_match);
 
-static struct of_platform_driver of_fsl_spi_driver = {
+static struct platform_driver of_fsl_spi_driver = {
 	.driver = {
 		.name = "fsl_spi",
 		.owner = THIS_MODULE,
@@ -1177,13 +1176,13 @@ static void __exit legacy_driver_unregister(void) {}
 static int __init fsl_spi_init(void)
 {
 	legacy_driver_register();
-	return of_register_platform_driver(&of_fsl_spi_driver);
+	return platform_driver_register(&of_fsl_spi_driver);
 }
 module_init(fsl_spi_init);
 
 static void __exit fsl_spi_exit(void)
 {
-	of_unregister_platform_driver(&of_fsl_spi_driver);
+	platform_driver_unregister(&of_fsl_spi_driver);
 	legacy_driver_unregister();
 }
 module_exit(fsl_spi_exit);
diff --git a/drivers/spi/spi_imx.c b/drivers/spi/spi_imx.c
index 1cf9d5faabf4..69d6dba67c19 100644
--- a/drivers/spi/spi_imx.c
+++ b/drivers/spi/spi_imx.c
@@ -174,7 +174,7 @@ static unsigned int spi_imx_clkdiv_2(unsigned int fin,
 #define SPI_IMX2_3_CTRL		0x08
 #define SPI_IMX2_3_CTRL_ENABLE		(1 <<  0)
 #define SPI_IMX2_3_CTRL_XCH		(1 <<  2)
-#define SPI_IMX2_3_CTRL_MODE(cs)	(1 << ((cs) +  4))
+#define SPI_IMX2_3_CTRL_MODE_MASK	(0xf << 4)
 #define SPI_IMX2_3_CTRL_POSTDIV_OFFSET	8
 #define SPI_IMX2_3_CTRL_PREDIV_OFFSET	12
 #define SPI_IMX2_3_CTRL_CS(cs)		((cs) << 18)
@@ -253,8 +253,14 @@ static int __maybe_unused spi_imx2_3_config(struct spi_imx_data *spi_imx,
 {
 	u32 ctrl = SPI_IMX2_3_CTRL_ENABLE, cfg = 0;
 
-	/* set master mode */
-	ctrl |= SPI_IMX2_3_CTRL_MODE(config->cs);
+	/*
+	 * The hardware seems to have a race condition when changing modes. The
+	 * current assumption is that the selection of the channel arrives
+	 * earlier in the hardware than the mode bits when they are written at
+	 * the same time.
+	 * So set master mode for all channels as we do not support slave mode.
+	 */
+	ctrl |= SPI_IMX2_3_CTRL_MODE_MASK;
 
 	/* set clock speed */
 	ctrl |= spi_imx2_3_clkdiv(spi_imx->spi_clk, config->speed_hz);
diff --git a/drivers/spi/spi_oc_tiny.c b/drivers/spi/spi_oc_tiny.c
new file mode 100644
index 000000000000..f1bde66cea19
--- /dev/null
+++ b/drivers/spi/spi_oc_tiny.c
@@ -0,0 +1,425 @@
+/*
+ * OpenCores tiny SPI master driver
+ *
+ * http://opencores.org/project,tiny_spi
+ *
+ * Copyright (C) 2011 Thomas Chou <thomas@wytron.com.tw>
+ *
+ * Based on spi_s3c24xx.c, which is:
+ * Copyright (c) 2006 Ben Dooks
+ * Copyright (c) 2006 Simtec Electronics
+ *	Ben Dooks <ben@simtec.co.uk>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+#include <linux/spi/spi_oc_tiny.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+
+#define DRV_NAME "spi_oc_tiny"
+
+#define TINY_SPI_RXDATA 0
+#define TINY_SPI_TXDATA 4
+#define TINY_SPI_STATUS 8
+#define TINY_SPI_CONTROL 12
+#define TINY_SPI_BAUD 16
+
+#define TINY_SPI_STATUS_TXE 0x1
+#define TINY_SPI_STATUS_TXR 0x2
+
+struct tiny_spi {
+	/* bitbang has to be first */
+	struct spi_bitbang bitbang;
+	struct completion done;
+
+	void __iomem *base;
+	int irq;
+	unsigned int freq;
+	unsigned int baudwidth;
+	unsigned int baud;
+	unsigned int speed_hz;
+	unsigned int mode;
+	unsigned int len;
+	unsigned int txc, rxc;
+	const u8 *txp;
+	u8 *rxp;
+	unsigned int gpio_cs_count;
+	int *gpio_cs;
+};
+
+static inline struct tiny_spi *tiny_spi_to_hw(struct spi_device *sdev)
+{
+	return spi_master_get_devdata(sdev->master);
+}
+
+static unsigned int tiny_spi_baud(struct spi_device *spi, unsigned int hz)
+{
+	struct tiny_spi *hw = tiny_spi_to_hw(spi);
+
+	return min(DIV_ROUND_UP(hw->freq, hz * 2), (1U << hw->baudwidth)) - 1;
+}
+
+static void tiny_spi_chipselect(struct spi_device *spi, int is_active)
+{
+	struct tiny_spi *hw = tiny_spi_to_hw(spi);
+
+	if (hw->gpio_cs_count) {
+		gpio_set_value(hw->gpio_cs[spi->chip_select],
+			(spi->mode & SPI_CS_HIGH) ? is_active : !is_active);
+	}
+}
+
+static int tiny_spi_setup_transfer(struct spi_device *spi,
+				   struct spi_transfer *t)
+{
+	struct tiny_spi *hw = tiny_spi_to_hw(spi);
+	unsigned int baud = hw->baud;
+
+	if (t) {
+		if (t->speed_hz && t->speed_hz != hw->speed_hz)
+			baud = tiny_spi_baud(spi, t->speed_hz);
+	}
+	writel(baud, hw->base + TINY_SPI_BAUD);
+	writel(hw->mode, hw->base + TINY_SPI_CONTROL);
+	return 0;
+}
+
+static int tiny_spi_setup(struct spi_device *spi)
+{
+	struct tiny_spi *hw = tiny_spi_to_hw(spi);
+
+	if (spi->max_speed_hz != hw->speed_hz) {
+		hw->speed_hz = spi->max_speed_hz;
+		hw->baud = tiny_spi_baud(spi, hw->speed_hz);
+	}
+	hw->mode = spi->mode & (SPI_CPOL | SPI_CPHA);
+	return 0;
+}
+
+static inline void tiny_spi_wait_txr(struct tiny_spi *hw)
+{
+	while (!(readb(hw->base + TINY_SPI_STATUS) &
+		 TINY_SPI_STATUS_TXR))
+		cpu_relax();
+}
+
+static inline void tiny_spi_wait_txe(struct tiny_spi *hw)
+{
+	while (!(readb(hw->base + TINY_SPI_STATUS) &
+		 TINY_SPI_STATUS_TXE))
+		cpu_relax();
+}
+
+static int tiny_spi_txrx_bufs(struct spi_device *spi, struct spi_transfer *t)
+{
+	struct tiny_spi *hw = tiny_spi_to_hw(spi);
+	const u8 *txp = t->tx_buf;
+	u8 *rxp = t->rx_buf;
+	unsigned int i;
+
+	if (hw->irq >= 0) {
+		/* use intrrupt driven data transfer */
+		hw->len = t->len;
+		hw->txp = t->tx_buf;
+		hw->rxp = t->rx_buf;
+		hw->txc = 0;
+		hw->rxc = 0;
+
+		/* send the first byte */
+		if (t->len > 1) {
+			writeb(hw->txp ? *hw->txp++ : 0,
+			       hw->base + TINY_SPI_TXDATA);
+			hw->txc++;
+			writeb(hw->txp ? *hw->txp++ : 0,
+			       hw->base + TINY_SPI_TXDATA);
+			hw->txc++;
+			writeb(TINY_SPI_STATUS_TXR, hw->base + TINY_SPI_STATUS);
+		} else {
+			writeb(hw->txp ? *hw->txp++ : 0,
+			       hw->base + TINY_SPI_TXDATA);
+			hw->txc++;
+			writeb(TINY_SPI_STATUS_TXE, hw->base + TINY_SPI_STATUS);
+		}
+
+		wait_for_completion(&hw->done);
+	} else if (txp && rxp) {
+		/* we need to tighten the transfer loop */
+		writeb(*txp++, hw->base + TINY_SPI_TXDATA);
+		if (t->len > 1) {
+			writeb(*txp++, hw->base + TINY_SPI_TXDATA);
+			for (i = 2; i < t->len; i++) {
+				u8 rx, tx = *txp++;
+				tiny_spi_wait_txr(hw);
+				rx = readb(hw->base + TINY_SPI_TXDATA);
+				writeb(tx, hw->base + TINY_SPI_TXDATA);
+				*rxp++ = rx;
+			}
+			tiny_spi_wait_txr(hw);
+			*rxp++ = readb(hw->base + TINY_SPI_TXDATA);
+		}
+		tiny_spi_wait_txe(hw);
+		*rxp++ = readb(hw->base + TINY_SPI_RXDATA);
+	} else if (rxp) {
+		writeb(0, hw->base + TINY_SPI_TXDATA);
+		if (t->len > 1) {
+			writeb(0,
+			       hw->base + TINY_SPI_TXDATA);
+			for (i = 2; i < t->len; i++) {
+				u8 rx;
+				tiny_spi_wait_txr(hw);
+				rx = readb(hw->base + TINY_SPI_TXDATA);
+				writeb(0, hw->base + TINY_SPI_TXDATA);
+				*rxp++ = rx;
+			}
+			tiny_spi_wait_txr(hw);
+			*rxp++ = readb(hw->base + TINY_SPI_TXDATA);
+		}
+		tiny_spi_wait_txe(hw);
+		*rxp++ = readb(hw->base + TINY_SPI_RXDATA);
+	} else if (txp) {
+		writeb(*txp++, hw->base + TINY_SPI_TXDATA);
+		if (t->len > 1) {
+			writeb(*txp++, hw->base + TINY_SPI_TXDATA);
+			for (i = 2; i < t->len; i++) {
+				u8 tx = *txp++;
+				tiny_spi_wait_txr(hw);
+				writeb(tx, hw->base + TINY_SPI_TXDATA);
+			}
+		}
+		tiny_spi_wait_txe(hw);
+	} else {
+		writeb(0, hw->base + TINY_SPI_TXDATA);
+		if (t->len > 1) {
+			writeb(0, hw->base + TINY_SPI_TXDATA);
+			for (i = 2; i < t->len; i++) {
+				tiny_spi_wait_txr(hw);
+				writeb(0, hw->base + TINY_SPI_TXDATA);
+			}
+		}
+		tiny_spi_wait_txe(hw);
+	}
+	return t->len;
+}
+
+static irqreturn_t tiny_spi_irq(int irq, void *dev)
+{
+	struct tiny_spi *hw = dev;
+
+	writeb(0, hw->base + TINY_SPI_STATUS);
+	if (hw->rxc + 1 == hw->len) {
+		if (hw->rxp)
+			*hw->rxp++ = readb(hw->base + TINY_SPI_RXDATA);
+		hw->rxc++;
+		complete(&hw->done);
+	} else {
+		if (hw->rxp)
+			*hw->rxp++ = readb(hw->base + TINY_SPI_TXDATA);
+		hw->rxc++;
+		if (hw->txc < hw->len) {
+			writeb(hw->txp ? *hw->txp++ : 0,
+			       hw->base + TINY_SPI_TXDATA);
+			hw->txc++;
+			writeb(TINY_SPI_STATUS_TXR,
+			       hw->base + TINY_SPI_STATUS);
+		} else {
+			writeb(TINY_SPI_STATUS_TXE,
+			       hw->base + TINY_SPI_STATUS);
+		}
+	}
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_OF
+#include <linux/of_gpio.h>
+
+static int __devinit tiny_spi_of_probe(struct platform_device *pdev)
+{
+	struct tiny_spi *hw = platform_get_drvdata(pdev);
+	struct device_node *np = pdev->dev.of_node;
+	unsigned int i;
+	const __be32 *val;
+	int len;
+
+	if (!np)
+		return 0;
+	hw->gpio_cs_count = of_gpio_count(np);
+	if (hw->gpio_cs_count) {
+		hw->gpio_cs = devm_kzalloc(&pdev->dev,
+				hw->gpio_cs_count * sizeof(unsigned int),
+				GFP_KERNEL);
+		if (!hw->gpio_cs)
+			return -ENOMEM;
+	}
+	for (i = 0; i < hw->gpio_cs_count; i++) {
+		hw->gpio_cs[i] = of_get_gpio_flags(np, i, NULL);
+		if (hw->gpio_cs[i] < 0)
+			return -ENODEV;
+	}
+	hw->bitbang.master->dev.of_node = pdev->dev.of_node;
+	val = of_get_property(pdev->dev.of_node,
+			      "clock-frequency", &len);
+	if (val && len >= sizeof(__be32))
+		hw->freq = be32_to_cpup(val);
+	val = of_get_property(pdev->dev.of_node, "baud-width", &len);
+	if (val && len >= sizeof(__be32))
+		hw->baudwidth = be32_to_cpup(val);
+	return 0;
+}
+#else /* !CONFIG_OF */
+static int __devinit tiny_spi_of_probe(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif /* CONFIG_OF */
+
+static int __devinit tiny_spi_probe(struct platform_device *pdev)
+{
+	struct tiny_spi_platform_data *platp = pdev->dev.platform_data;
+	struct tiny_spi *hw;
+	struct spi_master *master;
+	struct resource *res;
+	unsigned int i;
+	int err = -ENODEV;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(struct tiny_spi));
+	if (!master)
+		return err;
+
+	/* setup the master state. */
+	master->bus_num = pdev->id;
+	master->num_chipselect = 255;
+	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
+	master->setup = tiny_spi_setup;
+
+	hw = spi_master_get_devdata(master);
+	platform_set_drvdata(pdev, hw);
+
+	/* setup the state for the bitbang driver */
+	hw->bitbang.master = spi_master_get(master);
+	if (!hw->bitbang.master)
+		return err;
+	hw->bitbang.setup_transfer = tiny_spi_setup_transfer;
+	hw->bitbang.chipselect = tiny_spi_chipselect;
+	hw->bitbang.txrx_bufs = tiny_spi_txrx_bufs;
+
+	/* find and map our resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		goto exit_busy;
+	if (!devm_request_mem_region(&pdev->dev, res->start, resource_size(res),
+				     pdev->name))
+		goto exit_busy;
+	hw->base = devm_ioremap_nocache(&pdev->dev, res->start,
+					resource_size(res));
+	if (!hw->base)
+		goto exit_busy;
+	/* irq is optional */
+	hw->irq = platform_get_irq(pdev, 0);
+	if (hw->irq >= 0) {
+		init_completion(&hw->done);
+		err = devm_request_irq(&pdev->dev, hw->irq, tiny_spi_irq, 0,
+				       pdev->name, hw);
+		if (err)
+			goto exit;
+	}
+	/* find platform data */
+	if (platp) {
+		hw->gpio_cs_count = platp->gpio_cs_count;
+		hw->gpio_cs = platp->gpio_cs;
+		if (platp->gpio_cs_count && !platp->gpio_cs)
+			goto exit_busy;
+		hw->freq = platp->freq;
+		hw->baudwidth = platp->baudwidth;
+	} else {
+		err = tiny_spi_of_probe(pdev);
+		if (err)
+			goto exit;
+	}
+	for (i = 0; i < hw->gpio_cs_count; i++) {
+		err = gpio_request(hw->gpio_cs[i], dev_name(&pdev->dev));
+		if (err)
+			goto exit_gpio;
+		gpio_direction_output(hw->gpio_cs[i], 1);
+	}
+	hw->bitbang.master->num_chipselect = max(1U, hw->gpio_cs_count);
+
+	/* register our spi controller */
+	err = spi_bitbang_start(&hw->bitbang);
+	if (err)
+		goto exit;
+	dev_info(&pdev->dev, "base %p, irq %d\n", hw->base, hw->irq);
+
+	return 0;
+
+exit_gpio:
+	while (i-- > 0)
+		gpio_free(hw->gpio_cs[i]);
+exit_busy:
+	err = -EBUSY;
+exit:
+	platform_set_drvdata(pdev, NULL);
+	spi_master_put(master);
+	return err;
+}
+
+static int __devexit tiny_spi_remove(struct platform_device *pdev)
+{
+	struct tiny_spi *hw = platform_get_drvdata(pdev);
+	struct spi_master *master = hw->bitbang.master;
+	unsigned int i;
+
+	spi_bitbang_stop(&hw->bitbang);
+	for (i = 0; i < hw->gpio_cs_count; i++)
+		gpio_free(hw->gpio_cs[i]);
+	platform_set_drvdata(pdev, NULL);
+	spi_master_put(master);
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id tiny_spi_match[] = {
+	{ .compatible = "opencores,tiny-spi-rtlsvn2", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, tiny_spi_match);
+#else /* CONFIG_OF */
+#define tiny_spi_match NULL
+#endif /* CONFIG_OF */
+
+static struct platform_driver tiny_spi_driver = {
+	.probe = tiny_spi_probe,
+	.remove = __devexit_p(tiny_spi_remove),
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.pm = NULL,
+		.of_match_table = tiny_spi_match,
+	},
+};
+
+static int __init tiny_spi_init(void)
+{
+	return platform_driver_register(&tiny_spi_driver);
+}
+module_init(tiny_spi_init);
+
+static void __exit tiny_spi_exit(void)
+{
+	platform_driver_unregister(&tiny_spi_driver);
+}
+module_exit(tiny_spi_exit);
+
+MODULE_DESCRIPTION("OpenCores tiny SPI driver");
+MODULE_AUTHOR("Thomas Chou <thomas@wytron.com.tw>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/spi/spi_ppc4xx.c b/drivers/spi/spi_ppc4xx.c
index 80e172d3e72a..2a298c029194 100644
--- a/drivers/spi/spi_ppc4xx.c
+++ b/drivers/spi/spi_ppc4xx.c
@@ -390,8 +390,7 @@ static void free_gpios(struct ppc4xx_spi *hw)
 /*
  * platform_device layer stuff...
  */
-static int __init spi_ppc4xx_of_probe(struct platform_device *op,
-				      const struct of_device_id *match)
+static int __init spi_ppc4xx_of_probe(struct platform_device *op)
 {
 	struct ppc4xx_spi *hw;
 	struct spi_master *master;
@@ -586,7 +585,7 @@ static const struct of_device_id spi_ppc4xx_of_match[] = {
 
 MODULE_DEVICE_TABLE(of, spi_ppc4xx_of_match);
 
-static struct of_platform_driver spi_ppc4xx_of_driver = {
+static struct platform_driver spi_ppc4xx_of_driver = {
 	.probe = spi_ppc4xx_of_probe,
 	.remove = __exit_p(spi_ppc4xx_of_remove),
 	.driver = {
@@ -598,13 +597,13 @@ static struct of_platform_driver spi_ppc4xx_of_driver = {
 
 static int __init spi_ppc4xx_init(void)
 {
-	return of_register_platform_driver(&spi_ppc4xx_of_driver);
+	return platform_driver_register(&spi_ppc4xx_of_driver);
 }
 module_init(spi_ppc4xx_init);
 
 static void __exit spi_ppc4xx_exit(void)
 {
-	of_unregister_platform_driver(&spi_ppc4xx_of_driver);
+	platform_driver_unregister(&spi_ppc4xx_of_driver);
 }
 module_exit(spi_ppc4xx_exit);
 
diff --git a/drivers/spi/spi_sh.c b/drivers/spi/spi_sh.c
new file mode 100644
index 000000000000..869a07d375d6
--- /dev/null
+++ b/drivers/spi/spi_sh.c
@@ -0,0 +1,543 @@
+/*
+ * SH SPI bus driver
+ *
+ * Copyright (C) 2011  Renesas Solutions Corp.
+ *
+ * Based on pxa2xx_spi.c:
+ * Copyright (C) 2005 Stephen Street / StreetFire Sound Labs
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/errno.h>
+#include <linux/timer.h>
+#include <linux/delay.h>
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/spi/spi.h>
+
+#define SPI_SH_TBR		0x00
+#define SPI_SH_RBR		0x00
+#define SPI_SH_CR1		0x08
+#define SPI_SH_CR2		0x10
+#define SPI_SH_CR3		0x18
+#define SPI_SH_CR4		0x20
+#define SPI_SH_CR5		0x28
+
+/* CR1 */
+#define SPI_SH_TBE		0x80
+#define SPI_SH_TBF		0x40
+#define SPI_SH_RBE		0x20
+#define SPI_SH_RBF		0x10
+#define SPI_SH_PFONRD		0x08
+#define SPI_SH_SSDB		0x04
+#define SPI_SH_SSD		0x02
+#define SPI_SH_SSA		0x01
+
+/* CR2 */
+#define SPI_SH_RSTF		0x80
+#define SPI_SH_LOOPBK		0x40
+#define SPI_SH_CPOL		0x20
+#define SPI_SH_CPHA		0x10
+#define SPI_SH_L1M0		0x08
+
+/* CR3 */
+#define SPI_SH_MAX_BYTE		0xFF
+
+/* CR4 */
+#define SPI_SH_TBEI		0x80
+#define SPI_SH_TBFI		0x40
+#define SPI_SH_RBEI		0x20
+#define SPI_SH_RBFI		0x10
+#define SPI_SH_WPABRT		0x04
+#define SPI_SH_SSS		0x01
+
+/* CR8 */
+#define SPI_SH_P1L0		0x80
+#define SPI_SH_PP1L0		0x40
+#define SPI_SH_MUXI		0x20
+#define SPI_SH_MUXIRQ		0x10
+
+#define SPI_SH_FIFO_SIZE	32
+#define SPI_SH_SEND_TIMEOUT	(3 * HZ)
+#define SPI_SH_RECEIVE_TIMEOUT	(HZ >> 3)
+
+#undef DEBUG
+
+struct spi_sh_data {
+	void __iomem *addr;
+	int irq;
+	struct spi_master *master;
+	struct list_head queue;
+	struct workqueue_struct *workqueue;
+	struct work_struct ws;
+	unsigned long cr1;
+	wait_queue_head_t wait;
+	spinlock_t lock;
+};
+
+static void spi_sh_write(struct spi_sh_data *ss, unsigned long data,
+			     unsigned long offset)
+{
+	writel(data, ss->addr + offset);
+}
+
+static unsigned long spi_sh_read(struct spi_sh_data *ss, unsigned long offset)
+{
+	return readl(ss->addr + offset);
+}
+
+static void spi_sh_set_bit(struct spi_sh_data *ss, unsigned long val,
+				unsigned long offset)
+{
+	unsigned long tmp;
+
+	tmp = spi_sh_read(ss, offset);
+	tmp |= val;
+	spi_sh_write(ss, tmp, offset);
+}
+
+static void spi_sh_clear_bit(struct spi_sh_data *ss, unsigned long val,
+				unsigned long offset)
+{
+	unsigned long tmp;
+
+	tmp = spi_sh_read(ss, offset);
+	tmp &= ~val;
+	spi_sh_write(ss, tmp, offset);
+}
+
+static void clear_fifo(struct spi_sh_data *ss)
+{
+	spi_sh_set_bit(ss, SPI_SH_RSTF, SPI_SH_CR2);
+	spi_sh_clear_bit(ss, SPI_SH_RSTF, SPI_SH_CR2);
+}
+
+static int spi_sh_wait_receive_buffer(struct spi_sh_data *ss)
+{
+	int timeout = 100000;
+
+	while (spi_sh_read(ss, SPI_SH_CR1) & SPI_SH_RBE) {
+		udelay(10);
+		if (timeout-- < 0)
+			return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+static int spi_sh_wait_write_buffer_empty(struct spi_sh_data *ss)
+{
+	int timeout = 100000;
+
+	while (!(spi_sh_read(ss, SPI_SH_CR1) & SPI_SH_TBE)) {
+		udelay(10);
+		if (timeout-- < 0)
+			return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+static int spi_sh_send(struct spi_sh_data *ss, struct spi_message *mesg,
+			struct spi_transfer *t)
+{
+	int i, retval = 0;
+	int remain = t->len;
+	int cur_len;
+	unsigned char *data;
+	unsigned long tmp;
+	long ret;
+
+	if (t->len)
+		spi_sh_set_bit(ss, SPI_SH_SSA, SPI_SH_CR1);
+
+	data = (unsigned char *)t->tx_buf;
+	while (remain > 0) {
+		cur_len = min(SPI_SH_FIFO_SIZE, remain);
+		for (i = 0; i < cur_len &&
+				!(spi_sh_read(ss, SPI_SH_CR4) &
+							SPI_SH_WPABRT) &&
+				!(spi_sh_read(ss, SPI_SH_CR1) & SPI_SH_TBF);
+				i++)
+			spi_sh_write(ss, (unsigned long)data[i], SPI_SH_TBR);
+
+		if (spi_sh_read(ss, SPI_SH_CR4) & SPI_SH_WPABRT) {
+			/* Abort SPI operation */
+			spi_sh_set_bit(ss, SPI_SH_WPABRT, SPI_SH_CR4);
+			retval = -EIO;
+			break;
+		}
+
+		cur_len = i;
+
+		remain -= cur_len;
+		data += cur_len;
+
+		if (remain > 0) {
+			ss->cr1 &= ~SPI_SH_TBE;
+			spi_sh_set_bit(ss, SPI_SH_TBE, SPI_SH_CR4);
+			ret = wait_event_interruptible_timeout(ss->wait,
+						 ss->cr1 & SPI_SH_TBE,
+						 SPI_SH_SEND_TIMEOUT);
+			if (ret == 0 && !(ss->cr1 & SPI_SH_TBE)) {
+				printk(KERN_ERR "%s: timeout\n", __func__);
+				return -ETIMEDOUT;
+			}
+		}
+	}
+
+	if (list_is_last(&t->transfer_list, &mesg->transfers)) {
+		tmp = spi_sh_read(ss, SPI_SH_CR1);
+		tmp = tmp & ~(SPI_SH_SSD | SPI_SH_SSDB);
+		spi_sh_write(ss, tmp, SPI_SH_CR1);
+		spi_sh_set_bit(ss, SPI_SH_SSA, SPI_SH_CR1);
+
+		ss->cr1 &= ~SPI_SH_TBE;
+		spi_sh_set_bit(ss, SPI_SH_TBE, SPI_SH_CR4);
+		ret = wait_event_interruptible_timeout(ss->wait,
+					 ss->cr1 & SPI_SH_TBE,
+					 SPI_SH_SEND_TIMEOUT);
+		if (ret == 0 && (ss->cr1 & SPI_SH_TBE)) {
+			printk(KERN_ERR "%s: timeout\n", __func__);
+			return -ETIMEDOUT;
+		}
+	}
+
+	return retval;
+}
+
+static int spi_sh_receive(struct spi_sh_data *ss, struct spi_message *mesg,
+			  struct spi_transfer *t)
+{
+	int i;
+	int remain = t->len;
+	int cur_len;
+	unsigned char *data;
+	unsigned long tmp;
+	long ret;
+
+	if (t->len > SPI_SH_MAX_BYTE)
+		spi_sh_write(ss, SPI_SH_MAX_BYTE, SPI_SH_CR3);
+	else
+		spi_sh_write(ss, t->len, SPI_SH_CR3);
+
+	tmp = spi_sh_read(ss, SPI_SH_CR1);
+	tmp = tmp & ~(SPI_SH_SSD | SPI_SH_SSDB);
+	spi_sh_write(ss, tmp, SPI_SH_CR1);
+	spi_sh_set_bit(ss, SPI_SH_SSA, SPI_SH_CR1);
+
+	spi_sh_wait_write_buffer_empty(ss);
+
+	data = (unsigned char *)t->rx_buf;
+	while (remain > 0) {
+		if (remain >= SPI_SH_FIFO_SIZE) {
+			ss->cr1 &= ~SPI_SH_RBF;
+			spi_sh_set_bit(ss, SPI_SH_RBF, SPI_SH_CR4);
+			ret = wait_event_interruptible_timeout(ss->wait,
+						 ss->cr1 & SPI_SH_RBF,
+						 SPI_SH_RECEIVE_TIMEOUT);
+			if (ret == 0 &&
+			    spi_sh_read(ss, SPI_SH_CR1) & SPI_SH_RBE) {
+				printk(KERN_ERR "%s: timeout\n", __func__);
+				return -ETIMEDOUT;
+			}
+		}
+
+		cur_len = min(SPI_SH_FIFO_SIZE, remain);
+		for (i = 0; i < cur_len; i++) {
+			if (spi_sh_wait_receive_buffer(ss))
+				break;
+			data[i] = (unsigned char)spi_sh_read(ss, SPI_SH_RBR);
+		}
+
+		remain -= cur_len;
+		data += cur_len;
+	}
+
+	/* deassert CS when SPI is receiving. */
+	if (t->len > SPI_SH_MAX_BYTE) {
+		clear_fifo(ss);
+		spi_sh_write(ss, 1, SPI_SH_CR3);
+	} else {
+		spi_sh_write(ss, 0, SPI_SH_CR3);
+	}
+
+	return 0;
+}
+
+static void spi_sh_work(struct work_struct *work)
+{
+	struct spi_sh_data *ss = container_of(work, struct spi_sh_data, ws);
+	struct spi_message *mesg;
+	struct spi_transfer *t;
+	unsigned long flags;
+	int ret;
+
+	pr_debug("%s: enter\n", __func__);
+
+	spin_lock_irqsave(&ss->lock, flags);
+	while (!list_empty(&ss->queue)) {
+		mesg = list_entry(ss->queue.next, struct spi_message, queue);
+		list_del_init(&mesg->queue);
+
+		spin_unlock_irqrestore(&ss->lock, flags);
+		list_for_each_entry(t, &mesg->transfers, transfer_list) {
+			pr_debug("tx_buf = %p, rx_buf = %p\n",
+					t->tx_buf, t->rx_buf);
+			pr_debug("len = %d, delay_usecs = %d\n",
+					t->len, t->delay_usecs);
+
+			if (t->tx_buf) {
+				ret = spi_sh_send(ss, mesg, t);
+				if (ret < 0)
+					goto error;
+			}
+			if (t->rx_buf) {
+				ret = spi_sh_receive(ss, mesg, t);
+				if (ret < 0)
+					goto error;
+			}
+			mesg->actual_length += t->len;
+		}
+		spin_lock_irqsave(&ss->lock, flags);
+
+		mesg->status = 0;
+		mesg->complete(mesg->context);
+	}
+
+	clear_fifo(ss);
+	spi_sh_set_bit(ss, SPI_SH_SSD, SPI_SH_CR1);
+	udelay(100);
+
+	spi_sh_clear_bit(ss, SPI_SH_SSA | SPI_SH_SSDB | SPI_SH_SSD,
+			 SPI_SH_CR1);
+
+	clear_fifo(ss);
+
+	spin_unlock_irqrestore(&ss->lock, flags);
+
+	return;
+
+ error:
+	mesg->status = ret;
+	mesg->complete(mesg->context);
+
+	spi_sh_clear_bit(ss, SPI_SH_SSA | SPI_SH_SSDB | SPI_SH_SSD,
+			 SPI_SH_CR1);
+	clear_fifo(ss);
+
+}
+
+static int spi_sh_setup(struct spi_device *spi)
+{
+	struct spi_sh_data *ss = spi_master_get_devdata(spi->master);
+
+	if (!spi->bits_per_word)
+		spi->bits_per_word = 8;
+
+	pr_debug("%s: enter\n", __func__);
+
+	spi_sh_write(ss, 0xfe, SPI_SH_CR1);	/* SPI sycle stop */
+	spi_sh_write(ss, 0x00, SPI_SH_CR1);	/* CR1 init */
+	spi_sh_write(ss, 0x00, SPI_SH_CR3);	/* CR3 init */
+
+	clear_fifo(ss);
+
+	/* 1/8 clock */
+	spi_sh_write(ss, spi_sh_read(ss, SPI_SH_CR2) | 0x07, SPI_SH_CR2);
+	udelay(10);
+
+	return 0;
+}
+
+static int spi_sh_transfer(struct spi_device *spi, struct spi_message *mesg)
+{
+	struct spi_sh_data *ss = spi_master_get_devdata(spi->master);
+	unsigned long flags;
+
+	pr_debug("%s: enter\n", __func__);
+	pr_debug("\tmode = %02x\n", spi->mode);
+
+	spin_lock_irqsave(&ss->lock, flags);
+
+	mesg->actual_length = 0;
+	mesg->status = -EINPROGRESS;
+
+	spi_sh_clear_bit(ss, SPI_SH_SSA, SPI_SH_CR1);
+
+	list_add_tail(&mesg->queue, &ss->queue);
+	queue_work(ss->workqueue, &ss->ws);
+
+	spin_unlock_irqrestore(&ss->lock, flags);
+
+	return 0;
+}
+
+static void spi_sh_cleanup(struct spi_device *spi)
+{
+	struct spi_sh_data *ss = spi_master_get_devdata(spi->master);
+
+	pr_debug("%s: enter\n", __func__);
+
+	spi_sh_clear_bit(ss, SPI_SH_SSA | SPI_SH_SSDB | SPI_SH_SSD,
+			 SPI_SH_CR1);
+}
+
+static irqreturn_t spi_sh_irq(int irq, void *_ss)
+{
+	struct spi_sh_data *ss = (struct spi_sh_data *)_ss;
+	unsigned long cr1;
+
+	cr1 = spi_sh_read(ss, SPI_SH_CR1);
+	if (cr1 & SPI_SH_TBE)
+		ss->cr1 |= SPI_SH_TBE;
+	if (cr1 & SPI_SH_TBF)
+		ss->cr1 |= SPI_SH_TBF;
+	if (cr1 & SPI_SH_RBE)
+		ss->cr1 |= SPI_SH_RBE;
+	if (cr1 & SPI_SH_RBF)
+		ss->cr1 |= SPI_SH_RBF;
+
+	if (ss->cr1) {
+		spi_sh_clear_bit(ss, ss->cr1, SPI_SH_CR4);
+		wake_up(&ss->wait);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int __devexit spi_sh_remove(struct platform_device *pdev)
+{
+	struct spi_sh_data *ss = dev_get_drvdata(&pdev->dev);
+
+	destroy_workqueue(ss->workqueue);
+	free_irq(ss->irq, ss);
+	iounmap(ss->addr);
+	spi_master_put(ss->master);
+
+	return 0;
+}
+
+static int __devinit spi_sh_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct spi_master *master;
+	struct spi_sh_data *ss;
+	int ret, irq;
+
+	/* get base addr */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (unlikely(res == NULL)) {
+		dev_err(&pdev->dev, "invalid resource\n");
+		return -EINVAL;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "platform_get_irq error\n");
+		return -ENODEV;
+	}
+
+	master = spi_alloc_master(&pdev->dev, sizeof(struct spi_sh_data));
+	if (master == NULL) {
+		dev_err(&pdev->dev, "spi_alloc_master error.\n");
+		return -ENOMEM;
+	}
+
+	ss = spi_master_get_devdata(master);
+	dev_set_drvdata(&pdev->dev, ss);
+
+	ss->irq = irq;
+	ss->master = master;
+	ss->addr = ioremap(res->start, resource_size(res));
+	if (ss->addr == NULL) {
+		dev_err(&pdev->dev, "ioremap error.\n");
+		ret = -ENOMEM;
+		goto error1;
+	}
+	INIT_LIST_HEAD(&ss->queue);
+	spin_lock_init(&ss->lock);
+	INIT_WORK(&ss->ws, spi_sh_work);
+	init_waitqueue_head(&ss->wait);
+	ss->workqueue = create_singlethread_workqueue(
+					dev_name(master->dev.parent));
+	if (ss->workqueue == NULL) {
+		dev_err(&pdev->dev, "create workqueue error\n");
+		ret = -EBUSY;
+		goto error2;
+	}
+
+	ret = request_irq(irq, spi_sh_irq, IRQF_DISABLED, "spi_sh", ss);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "request_irq error\n");
+		goto error3;
+	}
+
+	master->num_chipselect = 2;
+	master->bus_num = pdev->id;
+	master->setup = spi_sh_setup;
+	master->transfer = spi_sh_transfer;
+	master->cleanup = spi_sh_cleanup;
+
+	ret = spi_register_master(master);
+	if (ret < 0) {
+		printk(KERN_ERR "spi_register_master error.\n");
+		goto error4;
+	}
+
+	return 0;
+
+ error4:
+	free_irq(irq, ss);
+ error3:
+	destroy_workqueue(ss->workqueue);
+ error2:
+	iounmap(ss->addr);
+ error1:
+	spi_master_put(master);
+
+	return ret;
+}
+
+static struct platform_driver spi_sh_driver = {
+	.probe = spi_sh_probe,
+	.remove = __devexit_p(spi_sh_remove),
+	.driver = {
+		.name = "sh_spi",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init spi_sh_init(void)
+{
+	return platform_driver_register(&spi_sh_driver);
+}
+module_init(spi_sh_init);
+
+static void __exit spi_sh_exit(void)
+{
+	platform_driver_unregister(&spi_sh_driver);
+}
+module_exit(spi_sh_exit);
+
+MODULE_DESCRIPTION("SH SPI bus driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Yoshihiro Shimoda");
+MODULE_ALIAS("platform:sh_spi");
diff --git a/drivers/spi/spi_sh_msiof.c b/drivers/spi/spi_sh_msiof.c
index 2c665fceaac7..e00d94b22250 100644
--- a/drivers/spi/spi_sh_msiof.c
+++ b/drivers/spi/spi_sh_msiof.c
@@ -9,22 +9,22 @@
  *
  */
 
-#include <linux/kernel.h>
-#include <linux/init.h>
+#include <linux/bitmap.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
 #include <linux/platform_device.h>
-#include <linux/completion.h>
 #include <linux/pm_runtime.h>
-#include <linux/gpio.h>
-#include <linux/bitmap.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-#include <linux/err.h>
 
+#include <linux/spi/sh_msiof.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
-#include <linux/spi/sh_msiof.h>
 
 #include <asm/unaligned.h>
 
@@ -67,7 +67,7 @@ struct sh_msiof_spi_priv {
 #define STR_TEOF  (1 << 23)
 #define STR_REOF  (1 << 7)
 
-static unsigned long sh_msiof_read(struct sh_msiof_spi_priv *p, int reg_offs)
+static u32 sh_msiof_read(struct sh_msiof_spi_priv *p, int reg_offs)
 {
 	switch (reg_offs) {
 	case TSCR:
@@ -79,7 +79,7 @@ static unsigned long sh_msiof_read(struct sh_msiof_spi_priv *p, int reg_offs)
 }
 
 static void sh_msiof_write(struct sh_msiof_spi_priv *p, int reg_offs,
-			   unsigned long value)
+			   u32 value)
 {
 	switch (reg_offs) {
 	case TSCR:
@@ -93,10 +93,10 @@ static void sh_msiof_write(struct sh_msiof_spi_priv *p, int reg_offs,
 }
 
 static int sh_msiof_modify_ctr_wait(struct sh_msiof_spi_priv *p,
-				    unsigned long clr, unsigned long set)
+				    u32 clr, u32 set)
 {
-	unsigned long mask = clr | set;
-	unsigned long data;
+	u32 mask = clr | set;
+	u32 data;
 	int k;
 
 	data = sh_msiof_read(p, CTR);
@@ -166,10 +166,10 @@ static void sh_msiof_spi_set_clk_regs(struct sh_msiof_spi_priv *p,
 }
 
 static void sh_msiof_spi_set_pin_regs(struct sh_msiof_spi_priv *p,
-				      int cpol, int cpha,
-				      int tx_hi_z, int lsb_first)
+				      u32 cpol, u32 cpha,
+				      u32 tx_hi_z, u32 lsb_first)
 {
-	unsigned long tmp;
+	u32 tmp;
 	int edge;
 
 	/*
@@ -187,7 +187,7 @@ static void sh_msiof_spi_set_pin_regs(struct sh_msiof_spi_priv *p,
 	tmp |= cpol << 30; /* TSCKIZ */
 	tmp |= cpol << 28; /* RSCKIZ */
 
-	edge = cpol ? cpha : !cpha;
+	edge = cpol ^ !cpha;
 
 	tmp |= edge << 27; /* TEDG */
 	tmp |= edge << 26; /* REDG */
@@ -197,11 +197,9 @@ static void sh_msiof_spi_set_pin_regs(struct sh_msiof_spi_priv *p,
 
 static void sh_msiof_spi_set_mode_regs(struct sh_msiof_spi_priv *p,
 				       const void *tx_buf, void *rx_buf,
-				       int bits, int words)
+				       u32 bits, u32 words)
 {
-	unsigned long dr2;
-
-	dr2 = ((bits - 1) << 24) | ((words - 1) << 16);
+	u32 dr2 = ((bits - 1) << 24) | ((words - 1) << 16);
 
 	if (tx_buf)
 		sh_msiof_write(p, TMDR2, dr2);
@@ -222,7 +220,7 @@ static void sh_msiof_reset_str(struct sh_msiof_spi_priv *p)
 static void sh_msiof_spi_write_fifo_8(struct sh_msiof_spi_priv *p,
 				      const void *tx_buf, int words, int fs)
 {
-	const unsigned char *buf_8 = tx_buf;
+	const u8 *buf_8 = tx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -232,7 +230,7 @@ static void sh_msiof_spi_write_fifo_8(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_write_fifo_16(struct sh_msiof_spi_priv *p,
 				       const void *tx_buf, int words, int fs)
 {
-	const unsigned short *buf_16 = tx_buf;
+	const u16 *buf_16 = tx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -242,7 +240,7 @@ static void sh_msiof_spi_write_fifo_16(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_write_fifo_16u(struct sh_msiof_spi_priv *p,
 					const void *tx_buf, int words, int fs)
 {
-	const unsigned short *buf_16 = tx_buf;
+	const u16 *buf_16 = tx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -252,7 +250,7 @@ static void sh_msiof_spi_write_fifo_16u(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_write_fifo_32(struct sh_msiof_spi_priv *p,
 				       const void *tx_buf, int words, int fs)
 {
-	const unsigned int *buf_32 = tx_buf;
+	const u32 *buf_32 = tx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -262,17 +260,37 @@ static void sh_msiof_spi_write_fifo_32(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_write_fifo_32u(struct sh_msiof_spi_priv *p,
 					const void *tx_buf, int words, int fs)
 {
-	const unsigned int *buf_32 = tx_buf;
+	const u32 *buf_32 = tx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
 		sh_msiof_write(p, TFDR, get_unaligned(&buf_32[k]) << fs);
 }
 
+static void sh_msiof_spi_write_fifo_s32(struct sh_msiof_spi_priv *p,
+					const void *tx_buf, int words, int fs)
+{
+	const u32 *buf_32 = tx_buf;
+	int k;
+
+	for (k = 0; k < words; k++)
+		sh_msiof_write(p, TFDR, swab32(buf_32[k] << fs));
+}
+
+static void sh_msiof_spi_write_fifo_s32u(struct sh_msiof_spi_priv *p,
+					 const void *tx_buf, int words, int fs)
+{
+	const u32 *buf_32 = tx_buf;
+	int k;
+
+	for (k = 0; k < words; k++)
+		sh_msiof_write(p, TFDR, swab32(get_unaligned(&buf_32[k]) << fs));
+}
+
 static void sh_msiof_spi_read_fifo_8(struct sh_msiof_spi_priv *p,
 				     void *rx_buf, int words, int fs)
 {
-	unsigned char *buf_8 = rx_buf;
+	u8 *buf_8 = rx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -282,7 +300,7 @@ static void sh_msiof_spi_read_fifo_8(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_read_fifo_16(struct sh_msiof_spi_priv *p,
 				      void *rx_buf, int words, int fs)
 {
-	unsigned short *buf_16 = rx_buf;
+	u16 *buf_16 = rx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -292,7 +310,7 @@ static void sh_msiof_spi_read_fifo_16(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_read_fifo_16u(struct sh_msiof_spi_priv *p,
 				       void *rx_buf, int words, int fs)
 {
-	unsigned short *buf_16 = rx_buf;
+	u16 *buf_16 = rx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -302,7 +320,7 @@ static void sh_msiof_spi_read_fifo_16u(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_read_fifo_32(struct sh_msiof_spi_priv *p,
 				      void *rx_buf, int words, int fs)
 {
-	unsigned int *buf_32 = rx_buf;
+	u32 *buf_32 = rx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
@@ -312,19 +330,40 @@ static void sh_msiof_spi_read_fifo_32(struct sh_msiof_spi_priv *p,
 static void sh_msiof_spi_read_fifo_32u(struct sh_msiof_spi_priv *p,
 				       void *rx_buf, int words, int fs)
 {
-	unsigned int *buf_32 = rx_buf;
+	u32 *buf_32 = rx_buf;
 	int k;
 
 	for (k = 0; k < words; k++)
 		put_unaligned(sh_msiof_read(p, RFDR) >> fs, &buf_32[k]);
 }
 
+static void sh_msiof_spi_read_fifo_s32(struct sh_msiof_spi_priv *p,
+				       void *rx_buf, int words, int fs)
+{
+	u32 *buf_32 = rx_buf;
+	int k;
+
+	for (k = 0; k < words; k++)
+		buf_32[k] = swab32(sh_msiof_read(p, RFDR) >> fs);
+}
+
+static void sh_msiof_spi_read_fifo_s32u(struct sh_msiof_spi_priv *p,
+				       void *rx_buf, int words, int fs)
+{
+	u32 *buf_32 = rx_buf;
+	int k;
+
+	for (k = 0; k < words; k++)
+		put_unaligned(swab32(sh_msiof_read(p, RFDR) >> fs), &buf_32[k]);
+}
+
 static int sh_msiof_spi_bits(struct spi_device *spi, struct spi_transfer *t)
 {
 	int bits;
 
 	bits = t ? t->bits_per_word : 0;
-	bits = bits ? bits : spi->bits_per_word;
+	if (!bits)
+		bits = spi->bits_per_word;
 	return bits;
 }
 
@@ -334,7 +373,8 @@ static unsigned long sh_msiof_spi_hz(struct spi_device *spi,
 	unsigned long hz;
 
 	hz = t ? t->speed_hz : 0;
-	hz = hz ? hz : spi->max_speed_hz;
+	if (!hz)
+		hz = spi->max_speed_hz;
 	return hz;
 }
 
@@ -468,9 +508,17 @@ static int sh_msiof_spi_txrx(struct spi_device *spi, struct spi_transfer *t)
 	int bytes_done;
 	int words;
 	int n;
+	bool swab;
 
 	bits = sh_msiof_spi_bits(spi, t);
 
+	if (bits <= 8 && t->len > 15 && !(t->len & 3)) {
+		bits = 32;
+		swab = true;
+	} else {
+		swab = false;
+	}
+
 	/* setup bytes per word and fifo read/write functions */
 	if (bits <= 8) {
 		bytes_per_word = 1;
@@ -487,6 +535,17 @@ static int sh_msiof_spi_txrx(struct spi_device *spi, struct spi_transfer *t)
 			rx_fifo = sh_msiof_spi_read_fifo_16u;
 		else
 			rx_fifo = sh_msiof_spi_read_fifo_16;
+	} else if (swab) {
+		bytes_per_word = 4;
+		if ((unsigned long)t->tx_buf & 0x03)
+			tx_fifo = sh_msiof_spi_write_fifo_s32u;
+		else
+			tx_fifo = sh_msiof_spi_write_fifo_s32;
+
+		if ((unsigned long)t->rx_buf & 0x03)
+			rx_fifo = sh_msiof_spi_read_fifo_s32u;
+		else
+			rx_fifo = sh_msiof_spi_read_fifo_s32;
 	} else {
 		bytes_per_word = 4;
 		if ((unsigned long)t->tx_buf & 0x03)
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index 603428213d21..d9fd86211365 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -30,6 +30,7 @@
 #include <linux/errno.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/compat.h>
 
 #include <linux/spi/spi.h>
 #include <linux/spi/spidev.h>
@@ -471,6 +472,16 @@ spidev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	return retval;
 }
 
+#ifdef CONFIG_COMPAT
+static long
+spidev_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	return spidev_ioctl(filp, cmd, (unsigned long)compat_ptr(arg));
+}
+#else
+#define spidev_compat_ioctl NULL
+#endif /* CONFIG_COMPAT */
+
 static int spidev_open(struct inode *inode, struct file *filp)
 {
 	struct spidev_data	*spidev;
@@ -543,6 +554,7 @@ static const struct file_operations spidev_fops = {
 	.write =	spidev_write,
 	.read =		spidev_read,
 	.unlocked_ioctl = spidev_ioctl,
+	.compat_ioctl = spidev_compat_ioctl,
 	.open =		spidev_open,
 	.release =	spidev_release,
 	.llseek =	no_llseek,
diff --git a/drivers/spi/xilinx_spi.c b/drivers/spi/xilinx_spi.c
index 7adaef62a991..c69c6f2c2c5c 100644
--- a/drivers/spi/xilinx_spi.c
+++ b/drivers/spi/xilinx_spi.c
@@ -18,6 +18,7 @@
 #include <linux/interrupt.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
 #include <linux/spi/xilinx_spi.h>
@@ -351,14 +352,12 @@ static irqreturn_t xilinx_spi_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-#ifdef CONFIG_OF
 static const struct of_device_id xilinx_spi_of_match[] = {
 	{ .compatible = "xlnx,xps-spi-2.00.a", },
 	{ .compatible = "xlnx,xps-spi-2.00.b", },
 	{}
 };
 MODULE_DEVICE_TABLE(of, xilinx_spi_of_match);
-#endif
 
 struct spi_master *xilinx_spi_init(struct device *dev, struct resource *mem,
 	u32 irq, s16 bus_num, int num_cs, int little_endian, int bits_per_word)
@@ -394,9 +393,7 @@ struct spi_master *xilinx_spi_init(struct device *dev, struct resource *mem,
 
 	master->bus_num = bus_num;
 	master->num_chipselect = num_cs;
-#ifdef CONFIG_OF
 	master->dev.of_node = dev->of_node;
-#endif
 
 	xspi->mem = *mem;
 	xspi->irq = irq;
@@ -474,7 +471,7 @@ static int __devinit xilinx_spi_probe(struct platform_device *dev)
 	struct spi_master *master;
 	u8 i;
 
-	pdata = dev->dev.platform_data;
+	pdata = mfd_get_data(dev);
 	if (pdata) {
 		num_cs = pdata->num_chipselect;
 		little_endian = pdata->little_endian;
@@ -539,9 +536,7 @@ static struct platform_driver xilinx_spi_driver = {
 	.driver = {
 		.name = XILINX_SPI_NAME,
 		.owner = THIS_MODULE,
-#ifdef CONFIG_OF
 		.of_match_table = xilinx_spi_of_match,
-#endif
 	},
 };
 
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index 3918d2cc5856..e05ba6eefc7e 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -1192,10 +1192,10 @@ void ssb_device_enable(struct ssb_device *dev, u32 core_specific_flags)
 }
 EXPORT_SYMBOL(ssb_device_enable);
 
-/* Wait for a bit in a register to get set or unset.
+/* Wait for bitmask in a register to get set or cleared.
  * timeout is in units of ten-microseconds */
-static int ssb_wait_bit(struct ssb_device *dev, u16 reg, u32 bitmask,
-			int timeout, int set)
+static int ssb_wait_bits(struct ssb_device *dev, u16 reg, u32 bitmask,
+			 int timeout, int set)
 {
 	int i;
 	u32 val;
@@ -1203,7 +1203,7 @@ static int ssb_wait_bit(struct ssb_device *dev, u16 reg, u32 bitmask,
 	for (i = 0; i < timeout; i++) {
 		val = ssb_read32(dev, reg);
 		if (set) {
-			if (val & bitmask)
+			if ((val & bitmask) == bitmask)
 				return 0;
 		} else {
 			if (!(val & bitmask))
@@ -1220,20 +1220,38 @@ static int ssb_wait_bit(struct ssb_device *dev, u16 reg, u32 bitmask,
 
 void ssb_device_disable(struct ssb_device *dev, u32 core_specific_flags)
 {
-	u32 reject;
+	u32 reject, val;
 
 	if (ssb_read32(dev, SSB_TMSLOW) & SSB_TMSLOW_RESET)
 		return;
 
 	reject = ssb_tmslow_reject_bitmask(dev);
-	ssb_write32(dev, SSB_TMSLOW, reject | SSB_TMSLOW_CLOCK);
-	ssb_wait_bit(dev, SSB_TMSLOW, reject, 1000, 1);
-	ssb_wait_bit(dev, SSB_TMSHIGH, SSB_TMSHIGH_BUSY, 1000, 0);
-	ssb_write32(dev, SSB_TMSLOW,
-		    SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
-		    reject | SSB_TMSLOW_RESET |
-		    core_specific_flags);
-	ssb_flush_tmslow(dev);
+
+	if (ssb_read32(dev, SSB_TMSLOW) & SSB_TMSLOW_CLOCK) {
+		ssb_write32(dev, SSB_TMSLOW, reject | SSB_TMSLOW_CLOCK);
+		ssb_wait_bits(dev, SSB_TMSLOW, reject, 1000, 1);
+		ssb_wait_bits(dev, SSB_TMSHIGH, SSB_TMSHIGH_BUSY, 1000, 0);
+
+		if (ssb_read32(dev, SSB_IDLOW) & SSB_IDLOW_INITIATOR) {
+			val = ssb_read32(dev, SSB_IMSTATE);
+			val |= SSB_IMSTATE_REJECT;
+			ssb_write32(dev, SSB_IMSTATE, val);
+			ssb_wait_bits(dev, SSB_IMSTATE, SSB_IMSTATE_BUSY, 1000,
+				      0);
+		}
+
+		ssb_write32(dev, SSB_TMSLOW,
+			SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
+			reject | SSB_TMSLOW_RESET |
+			core_specific_flags);
+		ssb_flush_tmslow(dev);
+
+		if (ssb_read32(dev, SSB_IDLOW) & SSB_IDLOW_INITIATOR) {
+			val = ssb_read32(dev, SSB_IMSTATE);
+			val &= ~SSB_IMSTATE_REJECT;
+			ssb_write32(dev, SSB_IMSTATE, val);
+		}
+	}
 
 	ssb_write32(dev, SSB_TMSLOW,
 		    reject | SSB_TMSLOW_RESET |
diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c
index 158449e55044..a467b20baac8 100644
--- a/drivers/ssb/pci.c
+++ b/drivers/ssb/pci.c
@@ -468,10 +468,14 @@ static void sprom_extract_r45(struct ssb_sprom *out, const u16 *in)
 		SPEX(country_code, SSB_SPROM4_CCODE, 0xFFFF, 0);
 		SPEX(boardflags_lo, SSB_SPROM4_BFLLO, 0xFFFF, 0);
 		SPEX(boardflags_hi, SSB_SPROM4_BFLHI, 0xFFFF, 0);
+		SPEX(boardflags2_lo, SSB_SPROM4_BFL2LO, 0xFFFF, 0);
+		SPEX(boardflags2_hi, SSB_SPROM4_BFL2HI, 0xFFFF, 0);
 	} else {
 		SPEX(country_code, SSB_SPROM5_CCODE, 0xFFFF, 0);
 		SPEX(boardflags_lo, SSB_SPROM5_BFLLO, 0xFFFF, 0);
 		SPEX(boardflags_hi, SSB_SPROM5_BFLHI, 0xFFFF, 0);
+		SPEX(boardflags2_lo, SSB_SPROM5_BFL2LO, 0xFFFF, 0);
+		SPEX(boardflags2_hi, SSB_SPROM5_BFL2HI, 0xFFFF, 0);
 	}
 	SPEX(ant_available_a, SSB_SPROM4_ANTAVAIL, SSB_SPROM4_ANTAVAIL_A,
 	     SSB_SPROM4_ANTAVAIL_A_SHIFT);
@@ -641,7 +645,7 @@ static int sprom_extract(struct ssb_bus *bus, struct ssb_sprom *out,
 		break;
 	default:
 		ssb_printk(KERN_WARNING PFX "Unsupported SPROM"
-			   "  revision %d detected. Will extract"
+			   " revision %d detected. Will extract"
 			   " v1\n", out->revision);
 		out->revision = 1;
 		sprom_extract_r123(out, in);
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 58e4a8e15a0e..1ae0c1befc77 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -55,11 +55,7 @@ source "drivers/staging/cx25821/Kconfig"
 
 source "drivers/staging/tm6000/Kconfig"
 
-source "drivers/staging/dabusb/Kconfig"
-
-source "drivers/staging/se401/Kconfig"
-
-source "drivers/staging/usbvideo/Kconfig"
+source "drivers/staging/cxd2099/Kconfig"
 
 source "drivers/staging/usbip/Kconfig"
 
@@ -183,5 +179,7 @@ source "drivers/staging/cptm1217/Kconfig"
 
 source "drivers/staging/ste_rmi4/Kconfig"
 
+source "drivers/staging/altera-stapl/Kconfig"
+
 endif # !STAGING_EXCLUDE_BUILD
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index ff7372d25c91..02691426703d 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -10,9 +10,7 @@ obj-$(CONFIG_SLICOSS)		+= slicoss/
 obj-$(CONFIG_VIDEO_GO7007)	+= go7007/
 obj-$(CONFIG_VIDEO_CX25821)	+= cx25821/
 obj-$(CONFIG_VIDEO_TM6000)	+= tm6000/
-obj-$(CONFIG_USB_DABUSB)        += dabusb/
-obj-$(CONFIG_USB_VICAM)         += usbvideo/
-obj-$(CONFIG_USB_SE401)         += se401/
+obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
 obj-$(CONFIG_USB_IP_COMMON)	+= usbip/
 obj-$(CONFIG_W35UND)		+= winbond/
@@ -70,5 +68,6 @@ obj-$(CONFIG_BCM_WIMAX)	+= bcm/
 obj-$(CONFIG_FT1000)		+= ft1000/
 obj-$(CONFIG_SND_INTEL_SST)		+= intel_sst/
 obj-$(CONFIG_SPEAKUP)	+= speakup/
+obj-$(CONFIG_ALTERA_STAPL)	+=altera-stapl/
 obj-$(CONFIG_TOUCHSCREEN_CLEARPAD_TM1217)	+= cptm1217/
 obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4)	+= ste_rmi4/
diff --git a/drivers/staging/altera-stapl/Kconfig b/drivers/staging/altera-stapl/Kconfig
new file mode 100644
index 000000000000..7f01d8e93992
--- /dev/null
+++ b/drivers/staging/altera-stapl/Kconfig
@@ -0,0 +1,8 @@
+comment "Altera FPGA firmware download module"
+
+config ALTERA_STAPL
+	tristate "Altera FPGA firmware download module"
+	depends on I2C
+	default n
+	help
+	  An Altera FPGA module. Say Y when you want to support this tool.
diff --git a/drivers/staging/altera-stapl/Makefile b/drivers/staging/altera-stapl/Makefile
new file mode 100644
index 000000000000..055f61ee781a
--- /dev/null
+++ b/drivers/staging/altera-stapl/Makefile
@@ -0,0 +1,3 @@
+altera-stapl-objs = altera-lpt.o altera-jtag.o altera-comp.o altera.o
+
+obj-$(CONFIG_ALTERA_STAPL) += altera-stapl.o
diff --git a/drivers/staging/altera-stapl/altera-comp.c b/drivers/staging/altera-stapl/altera-comp.c
new file mode 100644
index 000000000000..49b103bedaaf
--- /dev/null
+++ b/drivers/staging/altera-stapl/altera-comp.c
@@ -0,0 +1,142 @@
+/*
+ * altera-comp.c
+ *
+ * altera FPGA driver
+ *
+ * Copyright (C) Altera Corporation 1998-2001
+ * Copyright (C) 2010 NetUP Inc.
+ * Copyright (C) 2010 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include "altera-exprt.h"
+
+#define	SHORT_BITS		16
+#define	CHAR_BITS		8
+#define	DATA_BLOB_LENGTH	3
+#define	MATCH_DATA_LENGTH	8192
+#define ALTERA_REQUEST_SIZE	1024
+#define ALTERA_BUFFER_SIZE	(MATCH_DATA_LENGTH + ALTERA_REQUEST_SIZE)
+
+static u32 altera_bits_req(u32 n)
+{
+	u32 result = SHORT_BITS;
+
+	if (n == 0)
+		result = 1;
+	else {
+		/* Look for the highest non-zero bit position */
+		while ((n & (1 << (SHORT_BITS - 1))) == 0) {
+			n <<= 1;
+			--result;
+		}
+	}
+
+	return result;
+}
+
+static u32 altera_read_packed(u8 *buffer, u32 bits, u32 *bits_avail,
+							u32 *in_index)
+{
+	u32 result = 0;
+	u32 shift = 0;
+	u32 databyte = 0;
+
+	while (bits > 0) {
+		databyte = buffer[*in_index];
+		result |= (((databyte >> (CHAR_BITS - *bits_avail))
+			& (0xff >> (CHAR_BITS - *bits_avail))) << shift);
+
+		if (bits <= *bits_avail) {
+			result &= (0xffff >> (SHORT_BITS - (bits + shift)));
+			*bits_avail -= bits;
+			bits = 0;
+		} else {
+			++(*in_index);
+			shift += *bits_avail;
+			bits -= *bits_avail;
+			*bits_avail = CHAR_BITS;
+		}
+	}
+
+	return result;
+}
+
+u32 altera_shrink(u8 *in, u32 in_length, u8 *out, u32 out_length, s32 version)
+{
+	u32 i, j, data_length = 0L;
+	u32 offset, length;
+	u32 match_data_length = MATCH_DATA_LENGTH;
+	u32 bits_avail = CHAR_BITS;
+	u32 in_index = 0L;
+
+	if (version > 0)
+		--match_data_length;
+
+	for (i = 0; i < out_length; ++i)
+		out[i] = 0;
+
+	/* Read number of bytes in data. */
+	for (i = 0; i < sizeof(in_length); ++i) {
+		data_length = data_length | (
+			altera_read_packed(in,
+					CHAR_BITS,
+					&bits_avail,
+					&in_index) << (i * CHAR_BITS));
+	}
+
+	if (data_length > out_length) {
+		data_length = 0L;
+		return data_length;
+	}
+
+	i = 0;
+	while (i < data_length) {
+		/* A 0 bit indicates literal data. */
+		if (altera_read_packed(in, 1, &bits_avail,
+						&in_index) == 0) {
+			for (j = 0; j < DATA_BLOB_LENGTH; ++j) {
+				if (i < data_length) {
+					out[i] = (u8)altera_read_packed(in,
+							CHAR_BITS,
+							&bits_avail,
+							&in_index);
+					i++;
+				}
+			}
+		} else {
+			/* A 1 bit indicates offset/length to follow. */
+			offset = altera_read_packed(in, altera_bits_req((s16)
+					(i > match_data_length ?
+						match_data_length : i)),
+					&bits_avail,
+					&in_index);
+			length = altera_read_packed(in, CHAR_BITS,
+					&bits_avail,
+					&in_index);
+			for (j = 0; j < length; ++j) {
+				if (i < data_length) {
+					out[i] = out[i - offset];
+					i++;
+				}
+			}
+		}
+	}
+
+	return data_length;
+}
diff --git a/drivers/staging/altera-stapl/altera-exprt.h b/drivers/staging/altera-stapl/altera-exprt.h
new file mode 100644
index 000000000000..39c38d84a670
--- /dev/null
+++ b/drivers/staging/altera-stapl/altera-exprt.h
@@ -0,0 +1,33 @@
+/*
+ * altera-exprt.h
+ *
+ * altera FPGA driver
+ *
+ * Copyright (C) Altera Corporation 1998-2001
+ * Copyright (C) 2010 NetUP Inc.
+ * Copyright (C) 2010 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef ALTERA_EXPRT_H
+#define ALTERA_EXPRT_H
+
+
+u32 altera_shrink(u8 *in, u32 in_length, u8 *out, u32 out_length, s32 version);
+int netup_jtag_io_lpt(void *device, int tms, int tdi, int read_tdo);
+
+#endif /* ALTERA_EXPRT_H */
diff --git a/drivers/staging/altera-stapl/altera-jtag.c b/drivers/staging/altera-stapl/altera-jtag.c
new file mode 100644
index 000000000000..6b633b179a7e
--- /dev/null
+++ b/drivers/staging/altera-stapl/altera-jtag.c
@@ -0,0 +1,1020 @@
+/*
+ * altera-jtag.c
+ *
+ * altera FPGA driver
+ *
+ * Copyright (C) Altera Corporation 1998-2001
+ * Copyright (C) 2010 NetUP Inc.
+ * Copyright (C) 2010 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/firmware.h>
+#include <linux/slab.h>
+#include <staging/altera.h>
+#include "altera-exprt.h"
+#include "altera-jtag.h"
+
+#define	alt_jtag_io(a, b, c)\
+		astate->config->jtag_io(astate->config->dev, a, b, c);
+
+#define	alt_malloc(a)	kzalloc(a, GFP_KERNEL);
+
+/*
+ * This structure shows, for each JTAG state, which state is reached after
+ * a single TCK clock cycle with TMS high or TMS low, respectively.  This
+ * describes all possible state transitions in the JTAG state machine.
+ */
+struct altera_jtag_machine {
+	enum altera_jtag_state tms_high;
+	enum altera_jtag_state tms_low;
+};
+
+static const struct altera_jtag_machine altera_transitions[] = {
+	/* RESET     */	{ RESET,	IDLE },
+	/* IDLE      */	{ DRSELECT,	IDLE },
+	/* DRSELECT  */	{ IRSELECT,	DRCAPTURE },
+	/* DRCAPTURE */	{ DREXIT1,	DRSHIFT },
+	/* DRSHIFT   */	{ DREXIT1,	DRSHIFT },
+	/* DREXIT1   */	{ DRUPDATE,	DRPAUSE },
+	/* DRPAUSE   */	{ DREXIT2,	DRPAUSE },
+	/* DREXIT2   */	{ DRUPDATE,	DRSHIFT },
+	/* DRUPDATE  */	{ DRSELECT,	IDLE },
+	/* IRSELECT  */	{ RESET,	IRCAPTURE },
+	/* IRCAPTURE */	{ IREXIT1,	IRSHIFT },
+	/* IRSHIFT   */	{ IREXIT1,	IRSHIFT },
+	/* IREXIT1   */	{ IRUPDATE,	IRPAUSE },
+	/* IRPAUSE   */	{ IREXIT2,	IRPAUSE },
+	/* IREXIT2   */	{ IRUPDATE,	IRSHIFT },
+	/* IRUPDATE  */	{ DRSELECT,	IDLE }
+};
+
+/*
+ * This table contains the TMS value to be used to take the NEXT STEP on
+ * the path to the desired state.  The array index is the current state,
+ * and the bit position is the desired endstate.  To find out which state
+ * is used as the intermediate state, look up the TMS value in the
+ * altera_transitions[] table.
+ */
+static const u16 altera_jtag_path_map[16] = {
+	/* RST	RTI	SDRS	CDR	SDR	E1DR	PDR	E2DR */
+	0x0001,	0xFFFD,	0xFE01,	0xFFE7,	0xFFEF,	0xFF0F,	0xFFBF,	0xFFFF,
+	/* UDR	SIRS	CIR	SIR	E1IR	PIR	E2IR	UIR */
+	0xFEFD,	0x0001,	0xF3FF,	0xF7FF,	0x87FF,	0xDFFF,	0xFFFF,	0x7FFD
+};
+
+/* Flag bits for alt_jtag_io() function */
+#define TMS_HIGH   1
+#define TMS_LOW    0
+#define TDI_HIGH   1
+#define TDI_LOW    0
+#define READ_TDO   1
+#define IGNORE_TDO 0
+
+int altera_jinit(struct altera_state *astate)
+{
+	struct altera_jtag *js = &astate->js;
+
+	/* initial JTAG state is unknown */
+	js->jtag_state = ILLEGAL_JTAG_STATE;
+
+	/* initialize to default state */
+	js->drstop_state = IDLE;
+	js->irstop_state = IDLE;
+	js->dr_pre  = 0;
+	js->dr_post = 0;
+	js->ir_pre  = 0;
+	js->ir_post = 0;
+	js->dr_length    = 0;
+	js->ir_length    = 0;
+
+	js->dr_pre_data  = NULL;
+	js->dr_post_data = NULL;
+	js->ir_pre_data  = NULL;
+	js->ir_post_data = NULL;
+	js->dr_buffer	 = NULL;
+	js->ir_buffer	 = NULL;
+
+	return 0;
+}
+
+int altera_set_drstop(struct altera_jtag *js, enum altera_jtag_state state)
+{
+	js->drstop_state = state;
+
+	return 0;
+}
+
+int altera_set_irstop(struct altera_jtag *js, enum altera_jtag_state state)
+{
+	js->irstop_state = state;
+
+	return 0;
+}
+
+int altera_set_dr_pre(struct altera_jtag *js,
+				u32 count, u32 start_index,
+				u8 *preamble_data)
+{
+	int status = 0;
+	u32 i;
+	u32 j;
+
+	if (count > js->dr_pre) {
+		kfree(js->dr_pre_data);
+		js->dr_pre_data = (u8 *)alt_malloc((count + 7) >> 3);
+		if (js->dr_pre_data == NULL)
+			status = -ENOMEM;
+		else
+			js->dr_pre = count;
+	} else
+		js->dr_pre = count;
+
+	if (status == 0) {
+		for (i = 0; i < count; ++i) {
+			j = i + start_index;
+
+			if (preamble_data == NULL)
+				js->dr_pre_data[i >> 3] |= (1 << (i & 7));
+			else {
+				if (preamble_data[j >> 3] & (1 << (j & 7)))
+					js->dr_pre_data[i >> 3] |=
+							(1 << (i & 7));
+				else
+					js->dr_pre_data[i >> 3] &=
+							~(u32)(1 << (i & 7));
+
+			}
+		}
+	}
+
+	return status;
+}
+
+int altera_set_ir_pre(struct altera_jtag *js, u32 count, u32 start_index,
+							u8 *preamble_data)
+{
+	int status = 0;
+	u32 i;
+	u32 j;
+
+	if (count > js->ir_pre) {
+		kfree(js->ir_pre_data);
+		js->ir_pre_data = (u8 *)alt_malloc((count + 7) >> 3);
+		if (js->ir_pre_data == NULL)
+			status = -ENOMEM;
+		else
+			js->ir_pre = count;
+
+	} else
+		js->ir_pre = count;
+
+	if (status == 0) {
+		for (i = 0; i < count; ++i) {
+			j = i + start_index;
+			if (preamble_data == NULL)
+				js->ir_pre_data[i >> 3] |= (1 << (i & 7));
+			else {
+				if (preamble_data[j >> 3] & (1 << (j & 7)))
+					js->ir_pre_data[i >> 3] |=
+							(1 << (i & 7));
+				else
+					js->ir_pre_data[i >> 3] &=
+							~(u32)(1 << (i & 7));
+
+			}
+		}
+	}
+
+	return status;
+}
+
+int altera_set_dr_post(struct altera_jtag *js, u32 count, u32 start_index,
+						u8 *postamble_data)
+{
+	int status = 0;
+	u32 i;
+	u32 j;
+
+	if (count > js->dr_post) {
+		kfree(js->dr_post_data);
+		js->dr_post_data = (u8 *)alt_malloc((count + 7) >> 3);
+
+		if (js->dr_post_data == NULL)
+			status = -ENOMEM;
+		else
+			js->dr_post = count;
+
+	} else
+		js->dr_post = count;
+
+	if (status == 0) {
+		for (i = 0; i < count; ++i) {
+			j = i + start_index;
+
+			if (postamble_data == NULL)
+				js->dr_post_data[i >> 3] |= (1 << (i & 7));
+			else {
+				if (postamble_data[j >> 3] & (1 << (j & 7)))
+					js->dr_post_data[i >> 3] |=
+								(1 << (i & 7));
+				else
+					js->dr_post_data[i >> 3] &=
+					    ~(u32)(1 << (i & 7));
+
+			}
+		}
+	}
+
+	return status;
+}
+
+int altera_set_ir_post(struct altera_jtag *js, u32 count, u32 start_index,
+						u8 *postamble_data)
+{
+	int status = 0;
+	u32 i;
+	u32 j;
+
+	if (count > js->ir_post) {
+		kfree(js->ir_post_data);
+		js->ir_post_data = (u8 *)alt_malloc((count + 7) >> 3);
+		if (js->ir_post_data == NULL)
+			status = -ENOMEM;
+		else
+			js->ir_post = count;
+
+	} else
+		js->ir_post = count;
+
+	if (status != 0)
+		return status;
+
+	for (i = 0; i < count; ++i) {
+		j = i + start_index;
+
+		if (postamble_data == NULL)
+			js->ir_post_data[i >> 3] |= (1 << (i & 7));
+		else {
+			if (postamble_data[j >> 3] & (1 << (j & 7)))
+				js->ir_post_data[i >> 3] |= (1 << (i & 7));
+			else
+				js->ir_post_data[i >> 3] &=
+				    ~(u32)(1 << (i & 7));
+
+		}
+	}
+
+	return status;
+}
+
+static void altera_jreset_idle(struct altera_state *astate)
+{
+	struct altera_jtag *js = &astate->js;
+	int i;
+	/* Go to Test Logic Reset (no matter what the starting state may be) */
+	for (i = 0; i < 5; ++i)
+		alt_jtag_io(TMS_HIGH, TDI_LOW, IGNORE_TDO);
+
+	/* Now step to Run Test / Idle */
+	alt_jtag_io(TMS_LOW, TDI_LOW, IGNORE_TDO);
+	js->jtag_state = IDLE;
+}
+
+int altera_goto_jstate(struct altera_state *astate,
+					enum altera_jtag_state state)
+{
+	struct altera_jtag *js = &astate->js;
+	int tms;
+	int count = 0;
+	int status = 0;
+
+	if (js->jtag_state == ILLEGAL_JTAG_STATE)
+		/* initialize JTAG chain to known state */
+		altera_jreset_idle(astate);
+
+	if (js->jtag_state == state) {
+		/*
+		 * We are already in the desired state.
+		 * If it is a stable state, loop here.
+		 * Otherwise do nothing (no clock cycles).
+		 */
+		if ((state == IDLE) || (state == DRSHIFT) ||
+			(state == DRPAUSE) || (state == IRSHIFT) ||
+				(state == IRPAUSE)) {
+			alt_jtag_io(TMS_LOW, TDI_LOW, IGNORE_TDO);
+		} else if (state == RESET)
+			alt_jtag_io(TMS_HIGH, TDI_LOW, IGNORE_TDO);
+
+	} else {
+		while ((js->jtag_state != state) && (count < 9)) {
+			/* Get TMS value to take a step toward desired state */
+			tms = (altera_jtag_path_map[js->jtag_state] &
+							(1 << state))
+							? TMS_HIGH : TMS_LOW;
+
+			/* Take a step */
+			alt_jtag_io(tms, TDI_LOW, IGNORE_TDO);
+
+			if (tms)
+				js->jtag_state =
+					altera_transitions[js->jtag_state].tms_high;
+			else
+				js->jtag_state =
+					altera_transitions[js->jtag_state].tms_low;
+
+			++count;
+		}
+	}
+
+	if (js->jtag_state != state)
+		status = -EREMOTEIO;
+
+	return status;
+}
+
+int altera_wait_cycles(struct altera_state *astate,
+					s32 cycles,
+					enum altera_jtag_state wait_state)
+{
+	struct altera_jtag *js = &astate->js;
+	int tms;
+	s32 count;
+	int status = 0;
+
+	if (js->jtag_state != wait_state)
+		status = altera_goto_jstate(astate, wait_state);
+
+	if (status == 0) {
+		/*
+		 * Set TMS high to loop in RESET state
+		 * Set TMS low to loop in any other stable state
+		 */
+		tms = (wait_state == RESET) ? TMS_HIGH : TMS_LOW;
+
+		for (count = 0L; count < cycles; count++)
+			alt_jtag_io(tms, TDI_LOW, IGNORE_TDO);
+
+	}
+
+	return status;
+}
+
+int altera_wait_msecs(struct altera_state *astate,
+			s32 microseconds, enum altera_jtag_state wait_state)
+/*
+ * Causes JTAG hardware to sit in the specified stable
+ * state for the specified duration of real time.  If
+ * no JTAG operations have been performed yet, then only
+ * a delay is performed.  This permits the WAIT USECS
+ * statement to be used in VECTOR programs without causing
+ * any JTAG operations.
+ * Returns 0 for success, else appropriate error code.
+ */
+{
+	struct altera_jtag *js = &astate->js;
+	int status = 0;
+
+	if ((js->jtag_state != ILLEGAL_JTAG_STATE) &&
+	    (js->jtag_state != wait_state))
+		status = altera_goto_jstate(astate, wait_state);
+
+	if (status == 0)
+		/* Wait for specified time interval */
+		udelay(microseconds);
+
+	return status;
+}
+
+static void altera_concatenate_data(u8 *buffer,
+				u8 *preamble_data,
+				u32 preamble_count,
+				u8 *target_data,
+				u32 start_index,
+				u32 target_count,
+				u8 *postamble_data,
+				u32 postamble_count)
+/*
+ * Copies preamble data, target data, and postamble data
+ * into one buffer for IR or DR scans.
+ */
+{
+	u32 i, j, k;
+
+	for (i = 0L; i < preamble_count; ++i) {
+		if (preamble_data[i >> 3L] & (1L << (i & 7L)))
+			buffer[i >> 3L] |= (1L << (i & 7L));
+		else
+			buffer[i >> 3L] &= ~(u32)(1L << (i & 7L));
+
+	}
+
+	j = start_index;
+	k = preamble_count + target_count;
+	for (; i < k; ++i, ++j) {
+		if (target_data[j >> 3L] & (1L << (j & 7L)))
+			buffer[i >> 3L] |= (1L << (i & 7L));
+		else
+			buffer[i >> 3L] &= ~(u32)(1L << (i & 7L));
+
+	}
+
+	j = 0L;
+	k = preamble_count + target_count + postamble_count;
+	for (; i < k; ++i, ++j) {
+		if (postamble_data[j >> 3L] & (1L << (j & 7L)))
+			buffer[i >> 3L] |= (1L << (i & 7L));
+		else
+			buffer[i >> 3L] &= ~(u32)(1L << (i & 7L));
+
+	}
+}
+
+static int alt_jtag_drscan(struct altera_state *astate,
+			int start_state,
+			int count,
+			u8 *tdi,
+			u8 *tdo)
+{
+	int i = 0;
+	int tdo_bit = 0;
+	int status = 1;
+
+	/* First go to DRSHIFT state */
+	switch (start_state) {
+	case 0:						/* IDLE */
+		alt_jtag_io(1, 0, 0);	/* DRSELECT */
+		alt_jtag_io(0, 0, 0);	/* DRCAPTURE */
+		alt_jtag_io(0, 0, 0);	/* DRSHIFT */
+		break;
+
+	case 1:						/* DRPAUSE */
+		alt_jtag_io(1, 0, 0);	/* DREXIT2 */
+		alt_jtag_io(1, 0, 0);	/* DRUPDATE */
+		alt_jtag_io(1, 0, 0);	/* DRSELECT */
+		alt_jtag_io(0, 0, 0);	/* DRCAPTURE */
+		alt_jtag_io(0, 0, 0);	/* DRSHIFT */
+		break;
+
+	case 2:						/* IRPAUSE */
+		alt_jtag_io(1, 0, 0);	/* IREXIT2 */
+		alt_jtag_io(1, 0, 0);	/* IRUPDATE */
+		alt_jtag_io(1, 0, 0);	/* DRSELECT */
+		alt_jtag_io(0, 0, 0);	/* DRCAPTURE */
+		alt_jtag_io(0, 0, 0);	/* DRSHIFT */
+		break;
+
+	default:
+		status = 0;
+	}
+
+	if (status) {
+		/* loop in the SHIFT-DR state */
+		for (i = 0; i < count; i++) {
+			tdo_bit = alt_jtag_io(
+					(i == count - 1),
+					tdi[i >> 3] & (1 << (i & 7)),
+					(tdo != NULL));
+
+			if (tdo != NULL) {
+				if (tdo_bit)
+					tdo[i >> 3] |= (1 << (i & 7));
+				else
+					tdo[i >> 3] &= ~(u32)(1 << (i & 7));
+
+			}
+		}
+
+		alt_jtag_io(0, 0, 0);	/* DRPAUSE */
+	}
+
+	return status;
+}
+
+static int alt_jtag_irscan(struct altera_state *astate,
+		    int start_state,
+		    int count,
+		    u8 *tdi,
+		    u8 *tdo)
+{
+	int i = 0;
+	int tdo_bit = 0;
+	int status = 1;
+
+	/* First go to IRSHIFT state */
+	switch (start_state) {
+	case 0:						/* IDLE */
+		alt_jtag_io(1, 0, 0);	/* DRSELECT */
+		alt_jtag_io(1, 0, 0);	/* IRSELECT */
+		alt_jtag_io(0, 0, 0);	/* IRCAPTURE */
+		alt_jtag_io(0, 0, 0);	/* IRSHIFT */
+		break;
+
+	case 1:						/* DRPAUSE */
+		alt_jtag_io(1, 0, 0);	/* DREXIT2 */
+		alt_jtag_io(1, 0, 0);	/* DRUPDATE */
+		alt_jtag_io(1, 0, 0);	/* DRSELECT */
+		alt_jtag_io(1, 0, 0);	/* IRSELECT */
+		alt_jtag_io(0, 0, 0);	/* IRCAPTURE */
+		alt_jtag_io(0, 0, 0);	/* IRSHIFT */
+		break;
+
+	case 2:						/* IRPAUSE */
+		alt_jtag_io(1, 0, 0);	/* IREXIT2 */
+		alt_jtag_io(1, 0, 0);	/* IRUPDATE */
+		alt_jtag_io(1, 0, 0);	/* DRSELECT */
+		alt_jtag_io(1, 0, 0);	/* IRSELECT */
+		alt_jtag_io(0, 0, 0);	/* IRCAPTURE */
+		alt_jtag_io(0, 0, 0);	/* IRSHIFT */
+		break;
+
+	default:
+		status = 0;
+	}
+
+	if (status) {
+		/* loop in the SHIFT-IR state */
+		for (i = 0; i < count; i++) {
+			tdo_bit = alt_jtag_io(
+				      (i == count - 1),
+				      tdi[i >> 3] & (1 << (i & 7)),
+				      (tdo != NULL));
+			if (tdo != NULL) {
+				if (tdo_bit)
+					tdo[i >> 3] |= (1 << (i & 7));
+				else
+					tdo[i >> 3] &= ~(u32)(1 << (i & 7));
+
+			}
+		}
+
+		alt_jtag_io(0, 0, 0);	/* IRPAUSE */
+	}
+
+	return status;
+}
+
+static void altera_extract_target_data(u8 *buffer,
+				u8 *target_data,
+				u32 start_index,
+				u32 preamble_count,
+				u32 target_count)
+/*
+ * Copies target data from scan buffer, filtering out
+ * preamble and postamble data.
+ */
+{
+	u32 i;
+	u32 j;
+	u32 k;
+
+	j = preamble_count;
+	k = start_index + target_count;
+	for (i = start_index; i < k; ++i, ++j) {
+		if (buffer[j >> 3] & (1 << (j & 7)))
+			target_data[i >> 3] |= (1 << (i & 7));
+		else
+			target_data[i >> 3] &= ~(u32)(1 << (i & 7));
+
+	}
+}
+
+int altera_irscan(struct altera_state *astate,
+				u32 count,
+				u8 *tdi_data,
+				u32 start_index)
+/* Shifts data into instruction register */
+{
+	struct altera_jtag *js = &astate->js;
+	int start_code = 0;
+	u32 alloc_chars = 0;
+	u32 shift_count = js->ir_pre + count + js->ir_post;
+	int status = 0;
+	enum altera_jtag_state start_state = ILLEGAL_JTAG_STATE;
+
+	switch (js->jtag_state) {
+	case ILLEGAL_JTAG_STATE:
+	case RESET:
+	case IDLE:
+		start_code = 0;
+		start_state = IDLE;
+		break;
+
+	case DRSELECT:
+	case DRCAPTURE:
+	case DRSHIFT:
+	case DREXIT1:
+	case DRPAUSE:
+	case DREXIT2:
+	case DRUPDATE:
+		start_code = 1;
+		start_state = DRPAUSE;
+		break;
+
+	case IRSELECT:
+	case IRCAPTURE:
+	case IRSHIFT:
+	case IREXIT1:
+	case IRPAUSE:
+	case IREXIT2:
+	case IRUPDATE:
+		start_code = 2;
+		start_state = IRPAUSE;
+		break;
+
+	default:
+		status = -EREMOTEIO;
+		break;
+	}
+
+	if (status == 0)
+		if (js->jtag_state != start_state)
+			status = altera_goto_jstate(astate, start_state);
+
+	if (status == 0) {
+		if (shift_count > js->ir_length) {
+			alloc_chars = (shift_count + 7) >> 3;
+			kfree(js->ir_buffer);
+			js->ir_buffer = (u8 *)alt_malloc(alloc_chars);
+			if (js->ir_buffer == NULL)
+				status = -ENOMEM;
+			else
+				js->ir_length = alloc_chars * 8;
+
+		}
+	}
+
+	if (status == 0) {
+		/*
+		 * Copy preamble data, IR data,
+		 * and postamble data into a buffer
+		 */
+		altera_concatenate_data(js->ir_buffer,
+					js->ir_pre_data,
+					js->ir_pre,
+					tdi_data,
+					start_index,
+					count,
+					js->ir_post_data,
+					js->ir_post);
+		/* Do the IRSCAN */
+		alt_jtag_irscan(astate,
+				start_code,
+				shift_count,
+				js->ir_buffer,
+				NULL);
+
+		/* alt_jtag_irscan() always ends in IRPAUSE state */
+		js->jtag_state = IRPAUSE;
+	}
+
+	if (status == 0)
+		if (js->irstop_state != IRPAUSE)
+			status = altera_goto_jstate(astate, js->irstop_state);
+
+
+	return status;
+}
+
+int altera_swap_ir(struct altera_state *astate,
+			    u32 count,
+			    u8 *in_data,
+			    u32 in_index,
+			    u8 *out_data,
+			    u32 out_index)
+/* Shifts data into instruction register, capturing output data */
+{
+	struct altera_jtag *js = &astate->js;
+	int start_code = 0;
+	u32 alloc_chars = 0;
+	u32 shift_count = js->ir_pre + count + js->ir_post;
+	int status = 0;
+	enum altera_jtag_state start_state = ILLEGAL_JTAG_STATE;
+
+	switch (js->jtag_state) {
+	case ILLEGAL_JTAG_STATE:
+	case RESET:
+	case IDLE:
+		start_code = 0;
+		start_state = IDLE;
+		break;
+
+	case DRSELECT:
+	case DRCAPTURE:
+	case DRSHIFT:
+	case DREXIT1:
+	case DRPAUSE:
+	case DREXIT2:
+	case DRUPDATE:
+		start_code = 1;
+		start_state = DRPAUSE;
+		break;
+
+	case IRSELECT:
+	case IRCAPTURE:
+	case IRSHIFT:
+	case IREXIT1:
+	case IRPAUSE:
+	case IREXIT2:
+	case IRUPDATE:
+		start_code = 2;
+		start_state = IRPAUSE;
+		break;
+
+	default:
+		status = -EREMOTEIO;
+		break;
+	}
+
+	if (status == 0)
+		if (js->jtag_state != start_state)
+			status = altera_goto_jstate(astate, start_state);
+
+	if (status == 0) {
+		if (shift_count > js->ir_length) {
+			alloc_chars = (shift_count + 7) >> 3;
+			kfree(js->ir_buffer);
+			js->ir_buffer = (u8 *)alt_malloc(alloc_chars);
+			if (js->ir_buffer == NULL)
+				status = -ENOMEM;
+			else
+				js->ir_length = alloc_chars * 8;
+
+		}
+	}
+
+	if (status == 0) {
+		/*
+		 * Copy preamble data, IR data,
+		 * and postamble data into a buffer
+		 */
+		altera_concatenate_data(js->ir_buffer,
+					js->ir_pre_data,
+					js->ir_pre,
+					in_data,
+					in_index,
+					count,
+					js->ir_post_data,
+					js->ir_post);
+
+		/* Do the IRSCAN */
+		alt_jtag_irscan(astate,
+				start_code,
+				shift_count,
+				js->ir_buffer,
+				js->ir_buffer);
+
+		/* alt_jtag_irscan() always ends in IRPAUSE state */
+		js->jtag_state = IRPAUSE;
+	}
+
+	if (status == 0)
+		if (js->irstop_state != IRPAUSE)
+			status = altera_goto_jstate(astate, js->irstop_state);
+
+
+	if (status == 0)
+		/* Now extract the returned data from the buffer */
+		altera_extract_target_data(js->ir_buffer,
+					out_data, out_index,
+					js->ir_pre, count);
+
+	return status;
+}
+
+int altera_drscan(struct altera_state *astate,
+				u32 count,
+				u8 *tdi_data,
+				u32 start_index)
+/* Shifts data into data register (ignoring output data) */
+{
+	struct altera_jtag *js = &astate->js;
+	int start_code = 0;
+	u32 alloc_chars = 0;
+	u32 shift_count = js->dr_pre + count + js->dr_post;
+	int status = 0;
+	enum altera_jtag_state start_state = ILLEGAL_JTAG_STATE;
+
+	switch (js->jtag_state) {
+	case ILLEGAL_JTAG_STATE:
+	case RESET:
+	case IDLE:
+		start_code = 0;
+		start_state = IDLE;
+		break;
+
+	case DRSELECT:
+	case DRCAPTURE:
+	case DRSHIFT:
+	case DREXIT1:
+	case DRPAUSE:
+	case DREXIT2:
+	case DRUPDATE:
+		start_code = 1;
+		start_state = DRPAUSE;
+		break;
+
+	case IRSELECT:
+	case IRCAPTURE:
+	case IRSHIFT:
+	case IREXIT1:
+	case IRPAUSE:
+	case IREXIT2:
+	case IRUPDATE:
+		start_code = 2;
+		start_state = IRPAUSE;
+		break;
+
+	default:
+		status = -EREMOTEIO;
+		break;
+	}
+
+	if (status == 0)
+		if (js->jtag_state != start_state)
+			status = altera_goto_jstate(astate, start_state);
+
+	if (status == 0) {
+		if (shift_count > js->dr_length) {
+			alloc_chars = (shift_count + 7) >> 3;
+			kfree(js->dr_buffer);
+			js->dr_buffer = (u8 *)alt_malloc(alloc_chars);
+			if (js->dr_buffer == NULL)
+				status = -ENOMEM;
+			else
+				js->dr_length = alloc_chars * 8;
+
+		}
+	}
+
+	if (status == 0) {
+		/*
+		 * Copy preamble data, DR data,
+		 * and postamble data into a buffer
+		 */
+		altera_concatenate_data(js->dr_buffer,
+					js->dr_pre_data,
+					js->dr_pre,
+					tdi_data,
+					start_index,
+					count,
+					js->dr_post_data,
+					js->dr_post);
+		/* Do the DRSCAN */
+		alt_jtag_drscan(astate, start_code, shift_count,
+				js->dr_buffer, NULL);
+		/* alt_jtag_drscan() always ends in DRPAUSE state */
+		js->jtag_state = DRPAUSE;
+	}
+
+	if (status == 0)
+		if (js->drstop_state != DRPAUSE)
+			status = altera_goto_jstate(astate, js->drstop_state);
+
+	return status;
+}
+
+int altera_swap_dr(struct altera_state *astate, u32 count,
+				u8 *in_data, u32 in_index,
+				u8 *out_data, u32 out_index)
+/* Shifts data into data register, capturing output data */
+{
+	struct altera_jtag *js = &astate->js;
+	int start_code = 0;
+	u32 alloc_chars = 0;
+	u32 shift_count = js->dr_pre + count + js->dr_post;
+	int status = 0;
+	enum altera_jtag_state start_state = ILLEGAL_JTAG_STATE;
+
+	switch (js->jtag_state) {
+	case ILLEGAL_JTAG_STATE:
+	case RESET:
+	case IDLE:
+		start_code = 0;
+		start_state = IDLE;
+		break;
+
+	case DRSELECT:
+	case DRCAPTURE:
+	case DRSHIFT:
+	case DREXIT1:
+	case DRPAUSE:
+	case DREXIT2:
+	case DRUPDATE:
+		start_code = 1;
+		start_state = DRPAUSE;
+		break;
+
+	case IRSELECT:
+	case IRCAPTURE:
+	case IRSHIFT:
+	case IREXIT1:
+	case IRPAUSE:
+	case IREXIT2:
+	case IRUPDATE:
+		start_code = 2;
+		start_state = IRPAUSE;
+		break;
+
+	default:
+		status = -EREMOTEIO;
+		break;
+	}
+
+	if (status == 0)
+		if (js->jtag_state != start_state)
+			status = altera_goto_jstate(astate, start_state);
+
+	if (status == 0) {
+		if (shift_count > js->dr_length) {
+			alloc_chars = (shift_count + 7) >> 3;
+			kfree(js->dr_buffer);
+			js->dr_buffer = (u8 *)alt_malloc(alloc_chars);
+
+			if (js->dr_buffer == NULL)
+				status = -ENOMEM;
+			else
+				js->dr_length = alloc_chars * 8;
+
+		}
+	}
+
+	if (status == 0) {
+		/*
+		 * Copy preamble data, DR data,
+		 * and postamble data into a buffer
+		 */
+		altera_concatenate_data(js->dr_buffer,
+				js->dr_pre_data,
+				js->dr_pre,
+				in_data,
+				in_index,
+				count,
+				js->dr_post_data,
+				js->dr_post);
+
+		/* Do the DRSCAN */
+		alt_jtag_drscan(astate,
+				start_code,
+				shift_count,
+				js->dr_buffer,
+				js->dr_buffer);
+
+		/* alt_jtag_drscan() always ends in DRPAUSE state */
+		js->jtag_state = DRPAUSE;
+	}
+
+	if (status == 0)
+		if (js->drstop_state != DRPAUSE)
+			status = altera_goto_jstate(astate, js->drstop_state);
+
+	if (status == 0)
+		/* Now extract the returned data from the buffer */
+		altera_extract_target_data(js->dr_buffer,
+					out_data,
+					out_index,
+					js->dr_pre,
+					count);
+
+	return status;
+}
+
+void altera_free_buffers(struct altera_state *astate)
+{
+	struct altera_jtag *js = &astate->js;
+	/* If the JTAG interface was used, reset it to TLR */
+	if (js->jtag_state != ILLEGAL_JTAG_STATE)
+		altera_jreset_idle(astate);
+
+	kfree(js->dr_pre_data);
+	js->dr_pre_data = NULL;
+
+	kfree(js->dr_post_data);
+	js->dr_post_data = NULL;
+
+	kfree(js->dr_buffer);
+	js->dr_buffer = NULL;
+
+	kfree(js->ir_pre_data);
+	js->ir_pre_data = NULL;
+
+	kfree(js->ir_post_data);
+	js->ir_post_data = NULL;
+
+	kfree(js->ir_buffer);
+	js->ir_buffer = NULL;
+}
diff --git a/drivers/staging/altera-stapl/altera-jtag.h b/drivers/staging/altera-stapl/altera-jtag.h
new file mode 100644
index 000000000000..2f97e36a2fbc
--- /dev/null
+++ b/drivers/staging/altera-stapl/altera-jtag.h
@@ -0,0 +1,113 @@
+/*
+ * altera-jtag.h
+ *
+ * altera FPGA driver
+ *
+ * Copyright (C) Altera Corporation 1998-2001
+ * Copyright (C) 2010 NetUP Inc.
+ * Copyright (C) 2010 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef ALTERA_JTAG_H
+#define ALTERA_JTAG_H
+
+/* Function Prototypes */
+enum altera_jtag_state {
+	ILLEGAL_JTAG_STATE = -1,
+	RESET = 0,
+	IDLE = 1,
+	DRSELECT = 2,
+	DRCAPTURE = 3,
+	DRSHIFT = 4,
+	DREXIT1 = 5,
+	DRPAUSE = 6,
+	DREXIT2 = 7,
+	DRUPDATE = 8,
+	IRSELECT = 9,
+	IRCAPTURE = 10,
+	IRSHIFT = 11,
+	IREXIT1 = 12,
+	IRPAUSE = 13,
+	IREXIT2 = 14,
+	IRUPDATE = 15
+
+};
+
+struct altera_jtag {
+	/* Global variable to store the current JTAG state */
+	enum altera_jtag_state jtag_state;
+
+	/* Store current stop-state for DR and IR scan commands */
+	enum altera_jtag_state drstop_state;
+	enum altera_jtag_state irstop_state;
+
+	/* Store current padding values */
+	u32 dr_pre;
+	u32 dr_post;
+	u32 ir_pre;
+	u32 ir_post;
+	u32 dr_length;
+	u32 ir_length;
+	u8 *dr_pre_data;
+	u8 *dr_post_data;
+	u8 *ir_pre_data;
+	u8 *ir_post_data;
+	u8 *dr_buffer;
+	u8 *ir_buffer;
+};
+
+#define ALTERA_STACK_SIZE 128
+#define ALTERA_MESSAGE_LENGTH 1024
+
+struct altera_state {
+	struct altera_config	*config;
+	struct altera_jtag	js;
+	char			msg_buff[ALTERA_MESSAGE_LENGTH + 1];
+	long			stack[ALTERA_STACK_SIZE];
+};
+
+int altera_jinit(struct altera_state *astate);
+int altera_set_drstop(struct altera_jtag *js, enum altera_jtag_state state);
+int altera_set_irstop(struct altera_jtag *js, enum altera_jtag_state state);
+int altera_set_dr_pre(struct altera_jtag *js, u32 count, u32 start_index,
+				u8 *preamble_data);
+int altera_set_ir_pre(struct altera_jtag *js, u32 count, u32 start_index,
+				u8 *preamble_data);
+int altera_set_dr_post(struct altera_jtag *js, u32 count, u32 start_index,
+				u8 *postamble_data);
+int altera_set_ir_post(struct altera_jtag *js, u32 count, u32 start_index,
+				u8 *postamble_data);
+int altera_goto_jstate(struct altera_state *astate,
+				enum altera_jtag_state state);
+int altera_wait_cycles(struct altera_state *astate, s32 cycles,
+				enum altera_jtag_state wait_state);
+int altera_wait_msecs(struct altera_state *astate, s32 microseconds,
+				enum altera_jtag_state wait_state);
+int altera_irscan(struct altera_state *astate, u32 count,
+				u8 *tdi_data, u32 start_index);
+int altera_swap_ir(struct altera_state *astate,
+				u32 count, u8 *in_data,
+				u32 in_index, u8 *out_data,
+				u32 out_index);
+int altera_drscan(struct altera_state *astate, u32 count,
+				u8 *tdi_data, u32 start_index);
+int altera_swap_dr(struct altera_state *astate, u32 count,
+				u8 *in_data, u32 in_index,
+				u8 *out_data, u32 out_index);
+void altera_free_buffers(struct altera_state *astate);
+#endif /* ALTERA_JTAG_H */
diff --git a/drivers/staging/altera-stapl/altera-lpt.c b/drivers/staging/altera-stapl/altera-lpt.c
new file mode 100644
index 000000000000..91456a03612d
--- /dev/null
+++ b/drivers/staging/altera-stapl/altera-lpt.c
@@ -0,0 +1,70 @@
+/*
+ * altera-lpt.c
+ *
+ * altera FPGA driver
+ *
+ * Copyright (C) Altera Corporation 1998-2001
+ * Copyright (C) 2010 NetUP Inc.
+ * Copyright (C) 2010 Abylay Ospan <aospan@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include "altera-exprt.h"
+
+static int lpt_hardware_initialized;
+
+static void byteblaster_write(int port, int data)
+{
+	outb((u8)data, (u16)(port + 0x378));
+};
+
+static int byteblaster_read(int port)
+{
+	int data = 0;
+	data = inb((u16)(port + 0x378));
+	return data & 0xff;
+};
+
+int netup_jtag_io_lpt(void *device, int tms, int tdi, int read_tdo)
+{
+	int data = 0;
+	int tdo = 0;
+	int initial_lpt_ctrl = 0;
+
+	if (!lpt_hardware_initialized) {
+		initial_lpt_ctrl = byteblaster_read(2);
+		byteblaster_write(2, (initial_lpt_ctrl | 0x02) & 0xdf);
+		lpt_hardware_initialized = 1;
+	}
+
+	data = ((tdi ? 0x40 : 0) | (tms ? 0x02 : 0));
+
+	byteblaster_write(0, data);
+
+	if (read_tdo) {
+		tdo = byteblaster_read(1);
+		tdo = ((tdo & 0x80) ? 0 : 1);
+	}
+
+	byteblaster_write(0, data | 0x01);
+
+	byteblaster_write(0, data);
+
+	return tdo;
+}
diff --git a/drivers/staging/altera-stapl/altera.c b/drivers/staging/altera-stapl/altera.c
new file mode 100644
index 000000000000..05aad351b120
--- /dev/null
+++ b/drivers/staging/altera-stapl/altera.c
@@ -0,0 +1,2527 @@
+/*
+ * altera.c
+ *
+ * altera FPGA driver
+ *
+ * Copyright (C) Altera Corporation 1998-2001
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <asm/unaligned.h>
+#include <linux/ctype.h>
+#include <linux/string.h>
+#include <linux/firmware.h>
+#include <linux/slab.h>
+#include <staging/altera.h>
+#include "altera-exprt.h"
+#include "altera-jtag.h"
+
+static int debug = 1;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "enable debugging information");
+
+MODULE_DESCRIPTION("altera FPGA kernel module");
+MODULE_AUTHOR("Igor M. Liplianin  <liplianin@netup.ru>");
+MODULE_LICENSE("GPL");
+
+#define dprintk(args...) \
+	if (debug) { \
+		printk(KERN_DEBUG args); \
+	}
+
+enum altera_fpga_opcode {
+	OP_NOP = 0,
+	OP_DUP,
+	OP_SWP,
+	OP_ADD,
+	OP_SUB,
+	OP_MULT,
+	OP_DIV,
+	OP_MOD,
+	OP_SHL,
+	OP_SHR,
+	OP_NOT,
+	OP_AND,
+	OP_OR,
+	OP_XOR,
+	OP_INV,
+	OP_GT,
+	OP_LT,
+	OP_RET,
+	OP_CMPS,
+	OP_PINT,
+	OP_PRNT,
+	OP_DSS,
+	OP_DSSC,
+	OP_ISS,
+	OP_ISSC,
+	OP_DPR = 0x1c,
+	OP_DPRL,
+	OP_DPO,
+	OP_DPOL,
+	OP_IPR,
+	OP_IPRL,
+	OP_IPO,
+	OP_IPOL,
+	OP_PCHR,
+	OP_EXIT,
+	OP_EQU,
+	OP_POPT,
+	OP_ABS = 0x2c,
+	OP_BCH0,
+	OP_PSH0 = 0x2f,
+	OP_PSHL = 0x40,
+	OP_PSHV,
+	OP_JMP,
+	OP_CALL,
+	OP_NEXT,
+	OP_PSTR,
+	OP_SINT = 0x47,
+	OP_ST,
+	OP_ISTP,
+	OP_DSTP,
+	OP_SWPN,
+	OP_DUPN,
+	OP_POPV,
+	OP_POPE,
+	OP_POPA,
+	OP_JMPZ,
+	OP_DS,
+	OP_IS,
+	OP_DPRA,
+	OP_DPOA,
+	OP_IPRA,
+	OP_IPOA,
+	OP_EXPT,
+	OP_PSHE,
+	OP_PSHA,
+	OP_DYNA,
+	OP_EXPV = 0x5c,
+	OP_COPY = 0x80,
+	OP_REVA,
+	OP_DSC,
+	OP_ISC,
+	OP_WAIT,
+	OP_VS,
+	OP_CMPA = 0xc0,
+	OP_VSC,
+};
+
+struct altera_procinfo {
+	char			*name;
+	u8			attrs;
+	struct altera_procinfo	*next;
+};
+
+/* This function checks if enough parameters are available on the stack. */
+static int altera_check_stack(int stack_ptr, int count, int *status)
+{
+	if (stack_ptr < count) {
+		*status = -EOVERFLOW;
+		return 0;
+	}
+
+	return 1;
+}
+
+static void altera_export_int(char *key, s32 value)
+{
+	dprintk("Export: key = \"%s\", value = %d\n", key, value);
+}
+
+#define HEX_LINE_CHARS 72
+#define HEX_LINE_BITS (HEX_LINE_CHARS * 4)
+
+static void altera_export_bool_array(char *key, u8 *data, s32 count)
+{
+	char string[HEX_LINE_CHARS + 1];
+	s32 i, offset;
+	u32 size, line, lines, linebits, value, j, k;
+
+	if (count > HEX_LINE_BITS) {
+		dprintk("Export: key = \"%s\", %d bits, value = HEX\n",
+							key, count);
+		lines = (count + (HEX_LINE_BITS - 1)) / HEX_LINE_BITS;
+
+		for (line = 0; line < lines; ++line) {
+			if (line < (lines - 1)) {
+				linebits = HEX_LINE_BITS;
+				size = HEX_LINE_CHARS;
+				offset = count - ((line + 1) * HEX_LINE_BITS);
+			} else {
+				linebits =
+					count - ((lines - 1) * HEX_LINE_BITS);
+				size = (linebits + 3) / 4;
+				offset = 0L;
+			}
+
+			string[size] = '\0';
+			j = size - 1;
+			value = 0;
+
+			for (k = 0; k < linebits; ++k) {
+				i = k + offset;
+				if (data[i >> 3] & (1 << (i & 7)))
+					value |= (1 << (i & 3));
+				if ((i & 3) == 3) {
+					sprintf(&string[j], "%1x", value);
+					value = 0;
+					--j;
+				}
+			}
+			if ((k & 3) > 0)
+				sprintf(&string[j], "%1x", value);
+
+			dprintk("%s\n", string);
+		}
+
+	} else {
+		size = (count + 3) / 4;
+		string[size] = '\0';
+		j = size - 1;
+		value = 0;
+
+		for (i = 0; i < count; ++i) {
+			if (data[i >> 3] & (1 << (i & 7)))
+				value |= (1 << (i & 3));
+			if ((i & 3) == 3) {
+				sprintf(&string[j], "%1x", value);
+				value = 0;
+				--j;
+			}
+		}
+		if ((i & 3) > 0)
+			sprintf(&string[j], "%1x", value);
+
+		dprintk("Export: key = \"%s\", %d bits, value = HEX %s\n",
+			key, count, string);
+	}
+}
+
+static int altera_execute(struct altera_state *astate,
+				u8 *p,
+				s32 program_size,
+				s32 *error_address,
+				int *exit_code,
+				int *format_version)
+{
+	struct altera_config *aconf = astate->config;
+	char *msg_buff = astate->msg_buff;
+	long *stack = astate->stack;
+	int status = 0;
+	u32 first_word = 0L;
+	u32 action_table = 0L;
+	u32 proc_table = 0L;
+	u32 str_table = 0L;
+	u32 sym_table = 0L;
+	u32 data_sect = 0L;
+	u32 code_sect = 0L;
+	u32 debug_sect = 0L;
+	u32 action_count = 0L;
+	u32 proc_count = 0L;
+	u32 sym_count = 0L;
+	long *vars = NULL;
+	s32 *var_size = NULL;
+	char *attrs = NULL;
+	u8 *proc_attributes = NULL;
+	u32 pc;
+	u32 opcode_address;
+	u32 args[3];
+	u32 opcode;
+	u32 name_id;
+	u8 charbuf[4];
+	long long_tmp;
+	u32 variable_id;
+	u8 *charptr_tmp;
+	u8 *charptr_tmp2;
+	long *longptr_tmp;
+	int version = 0;
+	int delta = 0;
+	int stack_ptr = 0;
+	u32 arg_count;
+	int done = 0;
+	int bad_opcode = 0;
+	u32 count;
+	u32 index;
+	u32 index2;
+	s32 long_count;
+	s32 long_idx;
+	s32 long_idx2;
+	u32 i;
+	u32 j;
+	u32 uncomp_size;
+	u32 offset;
+	u32 value;
+	int current_proc = 0;
+	int reverse;
+
+	char *name;
+
+	dprintk("%s\n", __func__);
+
+	/* Read header information */
+	if (program_size > 52L) {
+		first_word    = get_unaligned_be32(&p[0]);
+		version = (first_word & 1L);
+		*format_version = version + 1;
+		delta = version * 8;
+
+		action_table  = get_unaligned_be32(&p[4]);
+		proc_table    = get_unaligned_be32(&p[8]);
+		str_table  = get_unaligned_be32(&p[4 + delta]);
+		sym_table  = get_unaligned_be32(&p[16 + delta]);
+		data_sect  = get_unaligned_be32(&p[20 + delta]);
+		code_sect  = get_unaligned_be32(&p[24 + delta]);
+		debug_sect = get_unaligned_be32(&p[28 + delta]);
+		action_count  = get_unaligned_be32(&p[40 + delta]);
+		proc_count    = get_unaligned_be32(&p[44 + delta]);
+		sym_count  = get_unaligned_be32(&p[48 + (2 * delta)]);
+	}
+
+	if ((first_word != 0x4A414D00L) && (first_word != 0x4A414D01L)) {
+		done = 1;
+		status = -EIO;
+		goto exit_done;
+	}
+
+	if (sym_count <= 0)
+		goto exit_done;
+
+	vars = kzalloc(sym_count * sizeof(long), GFP_KERNEL);
+
+	if (vars == NULL)
+		status = -ENOMEM;
+
+	if (status == 0) {
+		var_size = kzalloc(sym_count * sizeof(s32), GFP_KERNEL);
+
+		if (var_size == NULL)
+			status = -ENOMEM;
+	}
+
+	if (status == 0) {
+		attrs = kzalloc(sym_count, GFP_KERNEL);
+
+		if (attrs == NULL)
+			status = -ENOMEM;
+	}
+
+	if ((status == 0) && (version > 0)) {
+		proc_attributes = kzalloc(proc_count, GFP_KERNEL);
+
+		if (proc_attributes == NULL)
+			status = -ENOMEM;
+	}
+
+	if (status != 0)
+		goto exit_done;
+
+	delta = version * 2;
+
+	for (i = 0; i < sym_count; ++i) {
+		offset = (sym_table + ((11 + delta) * i));
+
+		value = get_unaligned_be32(&p[offset + 3 + delta]);
+
+		attrs[i] = p[offset];
+
+		/*
+		 * use bit 7 of attribute byte to indicate that
+		 * this buffer was dynamically allocated
+		 * and should be freed later
+		 */
+		attrs[i] &= 0x7f;
+
+		var_size[i] = get_unaligned_be32(&p[offset + 7 + delta]);
+
+		/*
+		 * Attribute bits:
+		 * bit 0: 0 = read-only, 1 = read-write
+		 * bit 1: 0 = not compressed, 1 = compressed
+		 * bit 2: 0 = not initialized, 1 = initialized
+		 * bit 3: 0 = scalar, 1 = array
+		 * bit 4: 0 = Boolean, 1 = integer
+		 * bit 5: 0 = declared variable,
+		 *	1 = compiler created temporary variable
+		 */
+
+		if ((attrs[i] & 0x0c) == 0x04)
+			/* initialized scalar variable */
+			vars[i] = value;
+		else if ((attrs[i] & 0x1e) == 0x0e) {
+			/* initialized compressed Boolean array */
+			uncomp_size = get_unaligned_le32(&p[data_sect + value]);
+
+			/* allocate a buffer for the uncompressed data */
+			vars[i] = (long)kzalloc(uncomp_size, GFP_KERNEL);
+			if (vars[i] == 0L)
+				status = -ENOMEM;
+			else {
+				/* set flag so buffer will be freed later */
+				attrs[i] |= 0x80;
+
+				/* uncompress the data */
+				if (altera_shrink(&p[data_sect + value],
+						var_size[i],
+						(u8 *)vars[i],
+						uncomp_size,
+						version) != uncomp_size)
+					/* decompression failed */
+					status = -EIO;
+				else
+					var_size[i] = uncomp_size * 8L;
+
+			}
+		} else if ((attrs[i] & 0x1e) == 0x0c) {
+			/* initialized Boolean array */
+			vars[i] = value + data_sect + (long)p;
+		} else if ((attrs[i] & 0x1c) == 0x1c) {
+			/* initialized integer array */
+			vars[i] = value + data_sect;
+		} else if ((attrs[i] & 0x0c) == 0x08) {
+			/* uninitialized array */
+
+			/* flag attrs so that memory is freed */
+			attrs[i] |= 0x80;
+
+			if (var_size[i] > 0) {
+				u32 size;
+
+				if (attrs[i] & 0x10)
+					/* integer array */
+					size = (var_size[i] * sizeof(s32));
+				else
+					/* Boolean array */
+					size = ((var_size[i] + 7L) / 8L);
+
+				vars[i] = (long)kzalloc(size, GFP_KERNEL);
+
+				if (vars[i] == 0) {
+					status = -ENOMEM;
+				} else {
+					/* zero out memory */
+					for (j = 0; j < size; ++j)
+						((u8 *)(vars[i]))[j] = 0;
+
+				}
+			} else
+				vars[i] = 0;
+
+		} else
+			vars[i] = 0;
+
+	}
+
+exit_done:
+	if (status != 0)
+		done = 1;
+
+	altera_jinit(astate);
+
+	pc = code_sect;
+	msg_buff[0] = '\0';
+
+	/*
+	 * For JBC version 2, we will execute the procedures corresponding to
+	 * the selected ACTION
+	 */
+	if (version > 0) {
+		if (aconf->action == NULL) {
+			status = -EINVAL;
+			done = 1;
+		} else {
+			int action_found = 0;
+			for (i = 0; (i < action_count) && !action_found; ++i) {
+				name_id = get_unaligned_be32(&p[action_table +
+								(12 * i)]);
+
+				name = &p[str_table + name_id];
+
+				if (strnicmp(aconf->action, name, strlen(name)) == 0) {
+					action_found = 1;
+					current_proc =
+						get_unaligned_be32(&p[action_table +
+								(12 * i) + 8]);
+				}
+			}
+
+			if (!action_found) {
+				status = -EINVAL;
+				done = 1;
+			}
+		}
+
+		if (status == 0) {
+			int first_time = 1;
+			i = current_proc;
+			while ((i != 0) || first_time) {
+				first_time = 0;
+				/* check procedure attribute byte */
+				proc_attributes[i] =
+						(p[proc_table +
+								(13 * i) + 8] &
+									0x03);
+
+				/*
+				 * BIT0 - OPTIONAL
+				 * BIT1 - RECOMMENDED
+				 * BIT6 - FORCED OFF
+				 * BIT7 - FORCED ON
+				 */
+
+				i = get_unaligned_be32(&p[proc_table +
+							(13 * i) + 4]);
+			}
+
+			/*
+			 * Set current_proc to the first procedure
+			 * to be executed
+			 */
+			i = current_proc;
+			while ((i != 0) &&
+				((proc_attributes[i] == 1) ||
+				((proc_attributes[i] & 0xc0) == 0x40))) {
+				i = get_unaligned_be32(&p[proc_table +
+							(13 * i) + 4]);
+			}
+
+			if ((i != 0) || ((i == 0) && (current_proc == 0) &&
+				((proc_attributes[0] != 1) &&
+				((proc_attributes[0] & 0xc0) != 0x40)))) {
+				current_proc = i;
+				pc = code_sect +
+					get_unaligned_be32(&p[proc_table +
+								(13 * i) + 9]);
+				if ((pc < code_sect) || (pc >= debug_sect))
+					status = -ERANGE;
+			} else
+				/* there are no procedures to execute! */
+				done = 1;
+
+		}
+	}
+
+	msg_buff[0] = '\0';
+
+	while (!done) {
+		opcode = (p[pc] & 0xff);
+		opcode_address = pc;
+		++pc;
+
+		if (debug > 1)
+			printk("opcode: %02x\n", opcode);
+
+		arg_count = (opcode >> 6) & 3;
+		for (i = 0; i < arg_count; ++i) {
+			args[i] = get_unaligned_be32(&p[pc]);
+			pc += 4;
+		}
+
+		switch (opcode) {
+		case OP_NOP:
+			break;
+		case OP_DUP:
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				stack[stack_ptr] = stack[stack_ptr - 1];
+				++stack_ptr;
+			}
+			break;
+		case OP_SWP:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				long_tmp = stack[stack_ptr - 2];
+				stack[stack_ptr - 2] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+			break;
+		case OP_ADD:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] += stack[stack_ptr];
+			}
+			break;
+		case OP_SUB:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] -= stack[stack_ptr];
+			}
+			break;
+		case OP_MULT:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] *= stack[stack_ptr];
+			}
+			break;
+		case OP_DIV:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] /= stack[stack_ptr];
+			}
+			break;
+		case OP_MOD:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] %= stack[stack_ptr];
+			}
+			break;
+		case OP_SHL:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] <<= stack[stack_ptr];
+			}
+			break;
+		case OP_SHR:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] >>= stack[stack_ptr];
+			}
+			break;
+		case OP_NOT:
+			if (altera_check_stack(stack_ptr, 1, &status))
+				stack[stack_ptr - 1] ^= (-1L);
+
+			break;
+		case OP_AND:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] &= stack[stack_ptr];
+			}
+			break;
+		case OP_OR:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] |= stack[stack_ptr];
+			}
+			break;
+		case OP_XOR:
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				--stack_ptr;
+				stack[stack_ptr - 1] ^= stack[stack_ptr];
+			}
+			break;
+		case OP_INV:
+			if (!altera_check_stack(stack_ptr, 1, &status))
+				break;
+			stack[stack_ptr - 1] = stack[stack_ptr - 1] ? 0L : 1L;
+			break;
+		case OP_GT:
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			--stack_ptr;
+			stack[stack_ptr - 1] =
+				(stack[stack_ptr - 1] > stack[stack_ptr]) ?
+									1L : 0L;
+
+			break;
+		case OP_LT:
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			--stack_ptr;
+			stack[stack_ptr - 1] =
+				(stack[stack_ptr - 1] < stack[stack_ptr]) ?
+									1L : 0L;
+
+			break;
+		case OP_RET:
+			if ((version > 0) && (stack_ptr == 0)) {
+				/*
+				 * We completed one of the main procedures
+				 * of an ACTION.
+				 * Find the next procedure
+				 * to be executed and jump to it.
+				 * If there are no more procedures, then EXIT.
+				 */
+				i = get_unaligned_be32(&p[proc_table +
+						(13 * current_proc) + 4]);
+				while ((i != 0) &&
+					((proc_attributes[i] == 1) ||
+					((proc_attributes[i] & 0xc0) == 0x40)))
+					i = get_unaligned_be32(&p[proc_table +
+								(13 * i) + 4]);
+
+				if (i == 0) {
+					/* no procedures to execute! */
+					done = 1;
+					*exit_code = 0;	/* success */
+				} else {
+					current_proc = i;
+					pc = code_sect + get_unaligned_be32(
+								&p[proc_table +
+								(13 * i) + 9]);
+					if ((pc < code_sect) ||
+					    (pc >= debug_sect))
+						status = -ERANGE;
+				}
+
+			} else
+				if (altera_check_stack(stack_ptr, 1, &status)) {
+					pc = stack[--stack_ptr] + code_sect;
+					if ((pc <= code_sect) ||
+					    (pc >= debug_sect))
+						status = -ERANGE;
+
+				}
+
+			break;
+		case OP_CMPS:
+			/*
+			 * Array short compare
+			 * ...stack 0 is source 1 value
+			 * ...stack 1 is source 2 value
+			 * ...stack 2 is mask value
+			 * ...stack 3 is count
+			 */
+			if (altera_check_stack(stack_ptr, 4, &status)) {
+				s32 a = stack[--stack_ptr];
+				s32 b = stack[--stack_ptr];
+				long_tmp = stack[--stack_ptr];
+				count = stack[stack_ptr - 1];
+
+				if ((count < 1) || (count > 32))
+					status = -ERANGE;
+				else {
+					long_tmp &= ((-1L) >> (32 - count));
+
+					stack[stack_ptr - 1] =
+					((a & long_tmp) == (b & long_tmp))
+								? 1L : 0L;
+				}
+			}
+			break;
+		case OP_PINT:
+			/*
+			 * PRINT add integer
+			 * ...stack 0 is integer value
+			 */
+			if (!altera_check_stack(stack_ptr, 1, &status))
+				break;
+			sprintf(&msg_buff[strlen(msg_buff)],
+					"%ld", stack[--stack_ptr]);
+			break;
+		case OP_PRNT:
+			/* PRINT finish */
+			if (debug)
+				printk(msg_buff, "\n");
+
+			msg_buff[0] = '\0';
+			break;
+		case OP_DSS:
+			/*
+			 * DRSCAN short
+			 * ...stack 0 is scan data
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			long_tmp = stack[--stack_ptr];
+			count = stack[--stack_ptr];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_drscan(astate, count, charbuf, 0);
+			break;
+		case OP_DSSC:
+			/*
+			 * DRSCAN short with capture
+			 * ...stack 0 is scan data
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			long_tmp = stack[--stack_ptr];
+			count = stack[stack_ptr - 1];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_swap_dr(astate, count, charbuf,
+							0, charbuf, 0);
+			stack[stack_ptr - 1] = get_unaligned_le32(&charbuf[0]);
+			break;
+		case OP_ISS:
+			/*
+			 * IRSCAN short
+			 * ...stack 0 is scan data
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			long_tmp = stack[--stack_ptr];
+			count = stack[--stack_ptr];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_irscan(astate, count, charbuf, 0);
+			break;
+		case OP_ISSC:
+			/*
+			 * IRSCAN short with capture
+			 * ...stack 0 is scan data
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			long_tmp = stack[--stack_ptr];
+			count = stack[stack_ptr - 1];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_swap_ir(astate, count, charbuf,
+							0, charbuf, 0);
+			stack[stack_ptr - 1] = get_unaligned_le32(&charbuf[0]);
+			break;
+		case OP_DPR:
+			if (!altera_check_stack(stack_ptr, 1, &status))
+				break;
+			count = stack[--stack_ptr];
+			status = altera_set_dr_pre(&astate->js, count, 0, NULL);
+			break;
+		case OP_DPRL:
+			/*
+			 * DRPRE with literal data
+			 * ...stack 0 is count
+			 * ...stack 1 is literal data
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			count = stack[--stack_ptr];
+			long_tmp = stack[--stack_ptr];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_set_dr_pre(&astate->js, count, 0,
+						charbuf);
+			break;
+		case OP_DPO:
+			/*
+			 * DRPOST
+			 * ...stack 0 is count
+			 */
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				count = stack[--stack_ptr];
+				status = altera_set_dr_post(&astate->js, count,
+								0, NULL);
+			}
+			break;
+		case OP_DPOL:
+			/*
+			 * DRPOST with literal data
+			 * ...stack 0 is count
+			 * ...stack 1 is literal data
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			count = stack[--stack_ptr];
+			long_tmp = stack[--stack_ptr];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_set_dr_post(&astate->js, count, 0,
+							charbuf);
+			break;
+		case OP_IPR:
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				count = stack[--stack_ptr];
+				status = altera_set_ir_pre(&astate->js, count,
+								0, NULL);
+			}
+			break;
+		case OP_IPRL:
+			/*
+			 * IRPRE with literal data
+			 * ...stack 0 is count
+			 * ...stack 1 is literal data
+			 */
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				count = stack[--stack_ptr];
+				long_tmp = stack[--stack_ptr];
+				put_unaligned_le32(long_tmp, &charbuf[0]);
+				status = altera_set_ir_pre(&astate->js, count,
+							0, charbuf);
+			}
+			break;
+		case OP_IPO:
+			/*
+			 * IRPOST
+			 * ...stack 0 is count
+			 */
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				count = stack[--stack_ptr];
+				status = altera_set_ir_post(&astate->js, count,
+							0, NULL);
+			}
+			break;
+		case OP_IPOL:
+			/*
+			 * IRPOST with literal data
+			 * ...stack 0 is count
+			 * ...stack 1 is literal data
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			count = stack[--stack_ptr];
+			long_tmp = stack[--stack_ptr];
+			put_unaligned_le32(long_tmp, &charbuf[0]);
+			status = altera_set_ir_post(&astate->js, count, 0,
+							charbuf);
+			break;
+		case OP_PCHR:
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				u8 ch;
+				count = strlen(msg_buff);
+				ch = (char) stack[--stack_ptr];
+				if ((ch < 1) || (ch > 127)) {
+					/*
+					 * character code out of range
+					 * instead of flagging an error,
+					 * force the value to 127
+					 */
+					ch = 127;
+				}
+				msg_buff[count] = ch;
+				msg_buff[count + 1] = '\0';
+			}
+			break;
+		case OP_EXIT:
+			if (altera_check_stack(stack_ptr, 1, &status))
+				*exit_code = stack[--stack_ptr];
+
+			done = 1;
+			break;
+		case OP_EQU:
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			--stack_ptr;
+			stack[stack_ptr - 1] =
+				(stack[stack_ptr - 1] == stack[stack_ptr]) ?
+									1L : 0L;
+			break;
+		case OP_POPT:
+			if (altera_check_stack(stack_ptr, 1, &status))
+				--stack_ptr;
+
+			break;
+		case OP_ABS:
+			if (!altera_check_stack(stack_ptr, 1, &status))
+				break;
+			if (stack[stack_ptr - 1] < 0)
+				stack[stack_ptr - 1] = 0 - stack[stack_ptr - 1];
+
+			break;
+		case OP_BCH0:
+			/*
+			 * Batch operation 0
+			 * SWP
+			 * SWPN 7
+			 * SWP
+			 * SWPN 6
+			 * DUPN 8
+			 * SWPN 2
+			 * SWP
+			 * DUPN 6
+			 * DUPN 6
+			 */
+
+			/* SWP  */
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				long_tmp = stack[stack_ptr - 2];
+				stack[stack_ptr - 2] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+
+			/* SWPN 7 */
+			index = 7 + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				long_tmp = stack[stack_ptr - index];
+				stack[stack_ptr - index] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+
+			/* SWP  */
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				long_tmp = stack[stack_ptr - 2];
+				stack[stack_ptr - 2] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+
+			/* SWPN 6 */
+			index = 6 + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				long_tmp = stack[stack_ptr - index];
+				stack[stack_ptr - index] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+
+			/* DUPN 8 */
+			index = 8 + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				stack[stack_ptr] = stack[stack_ptr - index];
+				++stack_ptr;
+			}
+
+			/* SWPN 2 */
+			index = 2 + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				long_tmp = stack[stack_ptr - index];
+				stack[stack_ptr - index] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+
+			/* SWP  */
+			if (altera_check_stack(stack_ptr, 2, &status)) {
+				long_tmp = stack[stack_ptr - 2];
+				stack[stack_ptr - 2] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+
+			/* DUPN 6 */
+			index = 6 + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				stack[stack_ptr] = stack[stack_ptr - index];
+				++stack_ptr;
+			}
+
+			/* DUPN 6 */
+			index = 6 + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				stack[stack_ptr] = stack[stack_ptr - index];
+				++stack_ptr;
+			}
+			break;
+		case OP_PSH0:
+			stack[stack_ptr++] = 0;
+			break;
+		case OP_PSHL:
+			stack[stack_ptr++] = (s32) args[0];
+			break;
+		case OP_PSHV:
+			stack[stack_ptr++] = vars[args[0]];
+			break;
+		case OP_JMP:
+			pc = args[0] + code_sect;
+			if ((pc < code_sect) || (pc >= debug_sect))
+				status = -ERANGE;
+			break;
+		case OP_CALL:
+			stack[stack_ptr++] = pc;
+			pc = args[0] + code_sect;
+			if ((pc < code_sect) || (pc >= debug_sect))
+				status = -ERANGE;
+			break;
+		case OP_NEXT:
+			/*
+			 * Process FOR / NEXT loop
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is step value
+			 * ...stack 1 is end value
+			 * ...stack 2 is top address
+			 */
+			if (altera_check_stack(stack_ptr, 3, &status)) {
+				s32 step = stack[stack_ptr - 1];
+				s32 end = stack[stack_ptr - 2];
+				s32 top = stack[stack_ptr - 3];
+				s32 iterator = vars[args[0]];
+				int break_out = 0;
+
+				if (step < 0) {
+					if (iterator <= end)
+						break_out = 1;
+				} else if (iterator >= end)
+					break_out = 1;
+
+				if (break_out) {
+					stack_ptr -= 3;
+				} else {
+					vars[args[0]] = iterator + step;
+					pc = top + code_sect;
+					if ((pc < code_sect) ||
+					    (pc >= debug_sect))
+						status = -ERANGE;
+				}
+			}
+			break;
+		case OP_PSTR:
+			/*
+			 * PRINT add string
+			 * ...argument 0 is string ID
+			 */
+			count = strlen(msg_buff);
+			strlcpy(&msg_buff[count],
+				&p[str_table + args[0]],
+				ALTERA_MESSAGE_LENGTH - count);
+			break;
+		case OP_SINT:
+			/*
+			 * STATE intermediate state
+			 * ...argument 0 is state code
+			 */
+			status = altera_goto_jstate(astate, args[0]);
+			break;
+		case OP_ST:
+			/*
+			 * STATE final state
+			 * ...argument 0 is state code
+			 */
+			status = altera_goto_jstate(astate, args[0]);
+			break;
+		case OP_ISTP:
+			/*
+			 * IRSTOP state
+			 * ...argument 0 is state code
+			 */
+			status = altera_set_irstop(&astate->js, args[0]);
+			break;
+		case OP_DSTP:
+			/*
+			 * DRSTOP state
+			 * ...argument 0 is state code
+			 */
+			status = altera_set_drstop(&astate->js, args[0]);
+			break;
+
+		case OP_SWPN:
+			/*
+			 * Exchange top with Nth stack value
+			 * ...argument 0 is 0-based stack entry
+			 * to swap with top element
+			 */
+			index = (args[0]) + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				long_tmp = stack[stack_ptr - index];
+				stack[stack_ptr - index] = stack[stack_ptr - 1];
+				stack[stack_ptr - 1] = long_tmp;
+			}
+			break;
+		case OP_DUPN:
+			/*
+			 * Duplicate Nth stack value
+			 * ...argument 0 is 0-based stack entry to duplicate
+			 */
+			index = (args[0]) + 1;
+			if (altera_check_stack(stack_ptr, index, &status)) {
+				stack[stack_ptr] = stack[stack_ptr - index];
+				++stack_ptr;
+			}
+			break;
+		case OP_POPV:
+			/*
+			 * Pop stack into scalar variable
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is value
+			 */
+			if (altera_check_stack(stack_ptr, 1, &status))
+				vars[args[0]] = stack[--stack_ptr];
+
+			break;
+		case OP_POPE:
+			/*
+			 * Pop stack into integer array element
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is array index
+			 * ...stack 1 is value
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			variable_id = args[0];
+
+			/*
+			 * If variable is read-only,
+			 * convert to writable array
+			 */
+			if ((version > 0) &&
+				((attrs[variable_id] & 0x9c) == 0x1c)) {
+				/* Allocate a writable buffer for this array */
+				count = var_size[variable_id];
+				long_tmp = vars[variable_id];
+				longptr_tmp = kzalloc(count * sizeof(long),
+								GFP_KERNEL);
+				vars[variable_id] = (long)longptr_tmp;
+
+				if (vars[variable_id] == 0) {
+					status = -ENOMEM;
+					break;
+				}
+
+				/* copy previous contents into buffer */
+				for (i = 0; i < count; ++i) {
+					longptr_tmp[i] =
+						get_unaligned_be32(&p[long_tmp]);
+					long_tmp += sizeof(long);
+				}
+
+				/*
+				 * set bit 7 - buffer was
+				 * dynamically allocated
+				 */
+				attrs[variable_id] |= 0x80;
+
+				/* clear bit 2 - variable is writable */
+				attrs[variable_id] &= ~0x04;
+				attrs[variable_id] |= 0x01;
+
+			}
+
+			/* check that variable is a writable integer array */
+			if ((attrs[variable_id] & 0x1c) != 0x18)
+				status = -ERANGE;
+			else {
+				longptr_tmp = (long *)vars[variable_id];
+
+				/* pop the array index */
+				index = stack[--stack_ptr];
+
+				/* pop the value and store it into the array */
+				longptr_tmp[index] = stack[--stack_ptr];
+			}
+
+			break;
+		case OP_POPA:
+			/*
+			 * Pop stack into Boolean array
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is count
+			 * ...stack 1 is array index
+			 * ...stack 2 is value
+			 */
+			if (!altera_check_stack(stack_ptr, 3, &status))
+				break;
+			variable_id = args[0];
+
+			/*
+			 * If variable is read-only,
+			 * convert to writable array
+			 */
+			if ((version > 0) &&
+				((attrs[variable_id] & 0x9c) == 0x0c)) {
+				/* Allocate a writable buffer for this array */
+				long_tmp =
+					(var_size[variable_id] + 7L) >> 3L;
+				charptr_tmp2 = (u8 *)vars[variable_id];
+				charptr_tmp =
+					kzalloc(long_tmp, GFP_KERNEL);
+				vars[variable_id] = (long)charptr_tmp;
+
+				if (vars[variable_id] == 0) {
+					status = -ENOMEM;
+					break;
+				}
+
+				/* zero the buffer */
+				for (long_idx = 0L;
+					long_idx < long_tmp;
+					++long_idx) {
+					charptr_tmp[long_idx] = 0;
+				}
+
+				/* copy previous contents into buffer */
+				for (long_idx = 0L;
+					long_idx < var_size[variable_id];
+					++long_idx) {
+					long_idx2 = long_idx;
+
+					if (charptr_tmp2[long_idx2 >> 3] &
+						(1 << (long_idx2 & 7))) {
+						charptr_tmp[long_idx >> 3] |=
+							(1 << (long_idx & 7));
+					}
+				}
+
+				/*
+				 * set bit 7 - buffer was
+				 * dynamically allocated
+				 */
+				attrs[variable_id] |= 0x80;
+
+				/* clear bit 2 - variable is writable */
+				attrs[variable_id] &= ~0x04;
+				attrs[variable_id] |= 0x01;
+
+			}
+
+			/*
+			 * check that variable is
+			 * a writable Boolean array
+			 */
+			if ((attrs[variable_id] & 0x1c) != 0x08) {
+				status = -ERANGE;
+				break;
+			}
+
+			charptr_tmp = (u8 *)vars[variable_id];
+
+			/* pop the count (number of bits to copy) */
+			long_count = stack[--stack_ptr];
+
+			/* pop the array index */
+			long_idx = stack[--stack_ptr];
+
+			reverse = 0;
+
+			if (version > 0) {
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 */
+
+				if (long_idx > long_count) {
+					reverse = 1;
+					long_tmp = long_count;
+					long_count = 1 + long_idx -
+								long_count;
+					long_idx = long_tmp;
+
+					/* reverse POPA is not supported */
+					status = -ERANGE;
+					break;
+				} else
+					long_count = 1 + long_count -
+								long_idx;
+
+			}
+
+			/* pop the data */
+			long_tmp = stack[--stack_ptr];
+
+			if (long_count < 1) {
+				status = -ERANGE;
+				break;
+			}
+
+			for (i = 0; i < long_count; ++i) {
+				if (long_tmp & (1L << (s32) i))
+					charptr_tmp[long_idx >> 3L] |=
+						(1L << (long_idx & 7L));
+				else
+					charptr_tmp[long_idx >> 3L] &=
+						~(1L << (long_idx & 7L));
+
+				++long_idx;
+			}
+
+			break;
+		case OP_JMPZ:
+			/*
+			 * Pop stack and branch if zero
+			 * ...argument 0 is address
+			 * ...stack 0 is condition value
+			 */
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				if (stack[--stack_ptr] == 0) {
+					pc = args[0] + code_sect;
+					if ((pc < code_sect) ||
+					    (pc >= debug_sect))
+						status = -ERANGE;
+				}
+			}
+			break;
+		case OP_DS:
+		case OP_IS:
+			/*
+			 * DRSCAN
+			 * IRSCAN
+			 * ...argument 0 is scan data variable ID
+			 * ...stack 0 is array index
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			long_idx = stack[--stack_ptr];
+			long_count = stack[--stack_ptr];
+			reverse = 0;
+			if (version > 0) {
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 * stack 2 = count
+				 */
+				long_tmp = long_count;
+				long_count = stack[--stack_ptr];
+
+				if (long_idx > long_tmp) {
+					reverse = 1;
+					long_idx = long_tmp;
+				}
+			}
+
+			charptr_tmp = (u8 *)vars[args[0]];
+
+			if (reverse) {
+				/*
+				 * allocate a buffer
+				 * and reverse the data order
+				 */
+				charptr_tmp2 = charptr_tmp;
+				charptr_tmp = kzalloc((long_count >> 3) + 1,
+								GFP_KERNEL);
+				if (charptr_tmp == NULL) {
+					status = -ENOMEM;
+					break;
+				}
+
+				long_tmp = long_idx + long_count - 1;
+				long_idx2 = 0;
+				while (long_idx2 < long_count) {
+					if (charptr_tmp2[long_tmp >> 3] &
+							(1 << (long_tmp & 7)))
+						charptr_tmp[long_idx2 >> 3] |=
+							(1 << (long_idx2 & 7));
+					else
+						charptr_tmp[long_idx2 >> 3] &=
+							~(1 << (long_idx2 & 7));
+
+					--long_tmp;
+					++long_idx2;
+				}
+			}
+
+			if (opcode == 0x51) /* DS */
+				status = altera_drscan(astate, long_count,
+						charptr_tmp, long_idx);
+			else /* IS */
+				status = altera_irscan(astate, long_count,
+						charptr_tmp, long_idx);
+
+			if (reverse)
+				kfree(charptr_tmp);
+
+			break;
+		case OP_DPRA:
+			/*
+			 * DRPRE with array data
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is array index
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			index = stack[--stack_ptr];
+			count = stack[--stack_ptr];
+
+			if (version > 0)
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 */
+				count = 1 + count - index;
+
+			charptr_tmp = (u8 *)vars[args[0]];
+			status = altera_set_dr_pre(&astate->js, count, index,
+							charptr_tmp);
+			break;
+		case OP_DPOA:
+			/*
+			 * DRPOST with array data
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is array index
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			index = stack[--stack_ptr];
+			count = stack[--stack_ptr];
+
+			if (version > 0)
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 */
+				count = 1 + count - index;
+
+			charptr_tmp = (u8 *)vars[args[0]];
+			status = altera_set_dr_post(&astate->js, count, index,
+							charptr_tmp);
+			break;
+		case OP_IPRA:
+			/*
+			 * IRPRE with array data
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is array index
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			index = stack[--stack_ptr];
+			count = stack[--stack_ptr];
+
+			if (version > 0)
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 */
+				count = 1 + count - index;
+
+			charptr_tmp = (u8 *)vars[args[0]];
+			status = altera_set_ir_pre(&astate->js, count, index,
+							charptr_tmp);
+
+			break;
+		case OP_IPOA:
+			/*
+			 * IRPOST with array data
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is array index
+			 * ...stack 1 is count
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			index = stack[--stack_ptr];
+			count = stack[--stack_ptr];
+
+			if (version > 0)
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 */
+				count = 1 + count - index;
+
+			charptr_tmp = (u8 *)vars[args[0]];
+			status = altera_set_ir_post(&astate->js, count, index,
+							charptr_tmp);
+
+			break;
+		case OP_EXPT:
+			/*
+			 * EXPORT
+			 * ...argument 0 is string ID
+			 * ...stack 0 is integer expression
+			 */
+			if (altera_check_stack(stack_ptr, 1, &status)) {
+				name = &p[str_table + args[0]];
+				long_tmp = stack[--stack_ptr];
+				altera_export_int(name, long_tmp);
+			}
+			break;
+		case OP_PSHE:
+			/*
+			 * Push integer array element
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is array index
+			 */
+			if (!altera_check_stack(stack_ptr, 1, &status))
+				break;
+			variable_id = args[0];
+			index = stack[stack_ptr - 1];
+
+			/* check variable type */
+			if ((attrs[variable_id] & 0x1f) == 0x19) {
+				/* writable integer array */
+				longptr_tmp = (long *)vars[variable_id];
+				stack[stack_ptr - 1] = longptr_tmp[index];
+			} else if ((attrs[variable_id] & 0x1f) == 0x1c) {
+				/* read-only integer array */
+				long_tmp = vars[variable_id] +
+						(index * sizeof(long));
+				stack[stack_ptr - 1] =
+					get_unaligned_be32(&p[long_tmp]);
+			} else
+				status = -ERANGE;
+
+			break;
+		case OP_PSHA:
+			/*
+			 * Push Boolean array
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is count
+			 * ...stack 1 is array index
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			variable_id = args[0];
+
+			/* check that variable is a Boolean array */
+			if ((attrs[variable_id] & 0x18) != 0x08) {
+				status = -ERANGE;
+				break;
+			}
+
+			charptr_tmp = (u8 *)vars[variable_id];
+
+			/* pop the count (number of bits to copy) */
+			count = stack[--stack_ptr];
+
+			/* pop the array index */
+			index = stack[stack_ptr - 1];
+
+			if (version > 0)
+				/*
+				 * stack 0 = array right index
+				 * stack 1 = array left index
+				 */
+				count = 1 + count - index;
+
+			if ((count < 1) || (count > 32)) {
+				status = -ERANGE;
+				break;
+			}
+
+			long_tmp = 0L;
+
+			for (i = 0; i < count; ++i)
+				if (charptr_tmp[(i + index) >> 3] &
+						(1 << ((i + index) & 7)))
+					long_tmp |= (1L << i);
+
+			stack[stack_ptr - 1] = long_tmp;
+
+			break;
+		case OP_DYNA:
+			/*
+			 * Dynamically change size of array
+			 * ...argument 0 is variable ID
+			 * ...stack 0 is new size
+			 */
+			if (!altera_check_stack(stack_ptr, 1, &status))
+				break;
+			variable_id = args[0];
+			long_tmp = stack[--stack_ptr];
+
+			if (long_tmp > var_size[variable_id]) {
+				var_size[variable_id] = long_tmp;
+
+				if (attrs[variable_id] & 0x10)
+					/* allocate integer array */
+					long_tmp *= sizeof(long);
+				else
+					/* allocate Boolean array */
+					long_tmp = (long_tmp + 7) >> 3;
+
+				/*
+				 * If the buffer was previously allocated,
+				 * free it
+				 */
+				if (attrs[variable_id] & 0x80) {
+					kfree((void *)vars[variable_id]);
+					vars[variable_id] = 0;
+				}
+
+				/*
+				 * Allocate a new buffer
+				 * of the requested size
+				 */
+				vars[variable_id] = (long)
+					kzalloc(long_tmp, GFP_KERNEL);
+
+				if (vars[variable_id] == 0) {
+					status = -ENOMEM;
+					break;
+				}
+
+				/*
+				 * Set the attribute bit to indicate that
+				 * this buffer was dynamically allocated and
+				 * should be freed later
+				 */
+				attrs[variable_id] |= 0x80;
+
+				/* zero out memory */
+				count = ((var_size[variable_id] + 7L) /
+									8L);
+				charptr_tmp = (u8 *)(vars[variable_id]);
+				for (index = 0; index < count; ++index)
+					charptr_tmp[index] = 0;
+
+			}
+
+			break;
+		case OP_EXPV:
+			/*
+			 * Export Boolean array
+			 * ...argument 0 is string ID
+			 * ...stack 0 is variable ID
+			 * ...stack 1 is array right index
+			 * ...stack 2 is array left index
+			 */
+			if (!altera_check_stack(stack_ptr, 3, &status))
+				break;
+			if (version == 0) {
+				/* EXPV is not supported in JBC 1.0 */
+				bad_opcode = 1;
+				break;
+			}
+			name = &p[str_table + args[0]];
+			variable_id = stack[--stack_ptr];
+			long_idx = stack[--stack_ptr];/* right indx */
+			long_idx2 = stack[--stack_ptr];/* left indx */
+
+			if (long_idx > long_idx2) {
+				/* reverse indices not supported */
+				status = -ERANGE;
+				break;
+			}
+
+			long_count = 1 + long_idx2 - long_idx;
+
+			charptr_tmp = (u8 *)vars[variable_id];
+			charptr_tmp2 = NULL;
+
+			if ((long_idx & 7L) != 0) {
+				s32 k = long_idx;
+				charptr_tmp2 =
+					kzalloc(((long_count + 7L) / 8L),
+							GFP_KERNEL);
+				if (charptr_tmp2 == NULL) {
+					status = -ENOMEM;
+					break;
+				}
+
+				for (i = 0; i < long_count; ++i) {
+					if (charptr_tmp[k >> 3] &
+							(1 << (k & 7)))
+						charptr_tmp2[i >> 3] |=
+								(1 << (i & 7));
+					else
+						charptr_tmp2[i >> 3] &=
+								~(1 << (i & 7));
+
+					++k;
+				}
+				charptr_tmp = charptr_tmp2;
+
+			} else if (long_idx != 0)
+				charptr_tmp = &charptr_tmp[long_idx >> 3];
+
+			altera_export_bool_array(name, charptr_tmp,
+							long_count);
+
+			/* free allocated buffer */
+			if ((long_idx & 7L) != 0)
+				kfree(charptr_tmp2);
+
+			break;
+		case OP_COPY: {
+			/*
+			 * Array copy
+			 * ...argument 0 is dest ID
+			 * ...argument 1 is source ID
+			 * ...stack 0 is count
+			 * ...stack 1 is dest index
+			 * ...stack 2 is source index
+			 */
+			s32 copy_count;
+			s32 copy_index;
+			s32 copy_index2;
+			s32 destleft;
+			s32 src_count;
+			s32 dest_count;
+			int src_reverse = 0;
+			int dest_reverse = 0;
+
+			if (!altera_check_stack(stack_ptr, 3, &status))
+				break;
+
+			copy_count = stack[--stack_ptr];
+			copy_index = stack[--stack_ptr];
+			copy_index2 = stack[--stack_ptr];
+			reverse = 0;
+
+			if (version > 0) {
+				/*
+				 * stack 0 = source right index
+				 * stack 1 = source left index
+				 * stack 2 = destination right index
+				 * stack 3 = destination left index
+				 */
+				destleft = stack[--stack_ptr];
+
+				if (copy_count > copy_index) {
+					src_reverse = 1;
+					reverse = 1;
+					src_count = 1 + copy_count - copy_index;
+					/* copy_index = source start index */
+				} else {
+					src_count = 1 + copy_index - copy_count;
+					/* source start index */
+					copy_index = copy_count;
+				}
+
+				if (copy_index2 > destleft) {
+					dest_reverse = 1;
+					reverse = !reverse;
+					dest_count = 1 + copy_index2 - destleft;
+					/* destination start index */
+					copy_index2 = destleft;
+				} else
+					dest_count = 1 + destleft - copy_index2;
+
+				copy_count = (src_count < dest_count) ?
+							src_count : dest_count;
+
+				if ((src_reverse || dest_reverse) &&
+					(src_count != dest_count))
+					/*
+					 * If either the source or destination
+					 * is reversed, we can't tolerate
+					 * a length mismatch, because we
+					 * "left justify" arrays when copying.
+					 * This won't work correctly
+					 * with reversed arrays.
+					 */
+					status = -ERANGE;
+
+			}
+
+			count = copy_count;
+			index = copy_index;
+			index2 = copy_index2;
+
+			/*
+			 * If destination is a read-only array,
+			 * allocate a buffer and convert it to a writable array
+			 */
+			variable_id = args[1];
+			if ((version > 0) &&
+				((attrs[variable_id] & 0x9c) == 0x0c)) {
+				/* Allocate a writable buffer for this array */
+				long_tmp =
+					(var_size[variable_id] + 7L) >> 3L;
+				charptr_tmp2 = (u8 *)vars[variable_id];
+				charptr_tmp =
+					kzalloc(long_tmp, GFP_KERNEL);
+				vars[variable_id] = (long)charptr_tmp;
+
+				if (vars[variable_id] == 0) {
+					status = -ENOMEM;
+					break;
+				}
+
+				/* zero the buffer */
+				for (long_idx = 0L; long_idx < long_tmp;
+								++long_idx)
+					charptr_tmp[long_idx] = 0;
+
+				/* copy previous contents into buffer */
+				for (long_idx = 0L;
+					long_idx < var_size[variable_id];
+								++long_idx) {
+					long_idx2 = long_idx;
+
+					if (charptr_tmp2[long_idx2 >> 3] &
+						(1 << (long_idx2 & 7)))
+						charptr_tmp[long_idx >> 3] |=
+							(1 << (long_idx & 7));
+
+				}
+
+				/*
+				set bit 7 - buffer was dynamically allocated */
+				attrs[variable_id] |= 0x80;
+
+				/* clear bit 2 - variable is writable */
+				attrs[variable_id] &= ~0x04;
+				attrs[variable_id] |= 0x01;
+			}
+
+			charptr_tmp = (u8 *)vars[args[1]];
+			charptr_tmp2 = (u8 *)vars[args[0]];
+
+			/* check if destination is a writable Boolean array */
+			if ((attrs[args[1]] & 0x1c) != 0x08) {
+				status = -ERANGE;
+				break;
+			}
+
+			if (count < 1) {
+				status = -ERANGE;
+				break;
+			}
+
+			if (reverse)
+				index2 += (count - 1);
+
+			for (i = 0; i < count; ++i) {
+				if (charptr_tmp2[index >> 3] &
+							(1 << (index & 7)))
+					charptr_tmp[index2 >> 3] |=
+							(1 << (index2 & 7));
+				else
+					charptr_tmp[index2 >> 3] &=
+						~(1 << (index2 & 7));
+
+				++index;
+				if (reverse)
+					--index2;
+				else
+					++index2;
+			}
+
+			break;
+		}
+		case OP_DSC:
+		case OP_ISC: {
+			/*
+			 * DRSCAN with capture
+			 * IRSCAN with capture
+			 * ...argument 0 is scan data variable ID
+			 * ...argument 1 is capture variable ID
+			 * ...stack 0 is capture index
+			 * ...stack 1 is scan data index
+			 * ...stack 2 is count
+			 */
+			s32 scan_right, scan_left;
+			s32 capture_count = 0;
+			s32 scan_count = 0;
+			s32 capture_index;
+			s32 scan_index;
+
+			if (!altera_check_stack(stack_ptr, 3, &status))
+				break;
+
+			capture_index = stack[--stack_ptr];
+			scan_index = stack[--stack_ptr];
+
+			if (version > 0) {
+				/*
+				 * stack 0 = capture right index
+				 * stack 1 = capture left index
+				 * stack 2 = scan right index
+				 * stack 3 = scan left index
+				 * stack 4 = count
+				 */
+				scan_right = stack[--stack_ptr];
+				scan_left = stack[--stack_ptr];
+				capture_count = 1 + scan_index - capture_index;
+				scan_count = 1 + scan_left - scan_right;
+				scan_index = scan_right;
+			}
+
+			long_count = stack[--stack_ptr];
+			/*
+			 * If capture array is read-only, allocate a buffer
+			 * and convert it to a writable array
+			 */
+			variable_id = args[1];
+			if ((version > 0) &&
+				((attrs[variable_id] & 0x9c) == 0x0c)) {
+				/* Allocate a writable buffer for this array */
+				long_tmp =
+					(var_size[variable_id] + 7L) >> 3L;
+				charptr_tmp2 = (u8 *)vars[variable_id];
+				charptr_tmp =
+					kzalloc(long_tmp, GFP_KERNEL);
+				vars[variable_id] = (long)charptr_tmp;
+
+				if (vars[variable_id] == 0) {
+					status = -ENOMEM;
+					break;
+				}
+
+				/* zero the buffer */
+				for (long_idx = 0L; long_idx < long_tmp;
+								++long_idx)
+					charptr_tmp[long_idx] = 0;
+
+				/* copy previous contents into buffer */
+				for (long_idx = 0L;
+					long_idx < var_size[variable_id];
+								++long_idx) {
+					long_idx2 = long_idx;
+
+					if (charptr_tmp2[long_idx2 >> 3] &
+						(1 << (long_idx2 & 7)))
+						charptr_tmp[long_idx >> 3] |=
+							(1 << (long_idx & 7));
+
+				}
+
+				/*
+				 * set bit 7 - buffer was
+				 * dynamically allocated
+				 */
+				attrs[variable_id] |= 0x80;
+
+				/* clear bit 2 - variable is writable */
+				attrs[variable_id] &= ~0x04;
+				attrs[variable_id] |= 0x01;
+
+			}
+
+			charptr_tmp = (u8 *)vars[args[0]];
+			charptr_tmp2 = (u8 *)vars[args[1]];
+
+			if ((version > 0) &&
+					((long_count > capture_count) ||
+					(long_count > scan_count))) {
+				status = -ERANGE;
+				break;
+			}
+
+			/*
+			 * check that capture array
+			 * is a writable Boolean array
+			 */
+			if ((attrs[args[1]] & 0x1c) != 0x08) {
+				status = -ERANGE;
+				break;
+			}
+
+			if (status == 0) {
+				if (opcode == 0x82) /* DSC */
+					status = altera_swap_dr(astate,
+							long_count,
+							charptr_tmp,
+							scan_index,
+							charptr_tmp2,
+							capture_index);
+				else /* ISC */
+					status = altera_swap_ir(astate,
+							long_count,
+							charptr_tmp,
+							scan_index,
+							charptr_tmp2,
+							capture_index);
+
+			}
+
+			break;
+		}
+		case OP_WAIT:
+			/*
+			 * WAIT
+			 * ...argument 0 is wait state
+			 * ...argument 1 is end state
+			 * ...stack 0 is cycles
+			 * ...stack 1 is microseconds
+			 */
+			if (!altera_check_stack(stack_ptr, 2, &status))
+				break;
+			long_tmp = stack[--stack_ptr];
+
+			if (long_tmp != 0L)
+				status = altera_wait_cycles(astate, long_tmp,
+								args[0]);
+
+			long_tmp = stack[--stack_ptr];
+
+			if ((status == 0) && (long_tmp != 0L))
+				status = altera_wait_msecs(astate,
+								long_tmp,
+								args[0]);
+
+			if ((status == 0) && (args[1] != args[0]))
+				status = altera_goto_jstate(astate,
+								args[1]);
+
+			if (version > 0) {
+				--stack_ptr; /* throw away MAX cycles */
+				--stack_ptr; /* throw away MAX microseconds */
+			}
+			break;
+		case OP_CMPA: {
+			/*
+			 * Array compare
+			 * ...argument 0 is source 1 ID
+			 * ...argument 1 is source 2 ID
+			 * ...argument 2 is mask ID
+			 * ...stack 0 is source 1 index
+			 * ...stack 1 is source 2 index
+			 * ...stack 2 is mask index
+			 * ...stack 3 is count
+			 */
+			s32 a, b;
+			u8 *source1 = (u8 *)vars[args[0]];
+			u8 *source2 = (u8 *)vars[args[1]];
+			u8 *mask = (u8 *)vars[args[2]];
+			u32 index1;
+			u32 index2;
+			u32 mask_index;
+
+			if (!altera_check_stack(stack_ptr, 4, &status))
+				break;
+
+			index1 = stack[--stack_ptr];
+			index2 = stack[--stack_ptr];
+			mask_index = stack[--stack_ptr];
+			long_count = stack[--stack_ptr];
+
+			if (version > 0) {
+				/*
+				 * stack 0 = source 1 right index
+				 * stack 1 = source 1 left index
+				 * stack 2 = source 2 right index
+				 * stack 3 = source 2 left index
+				 * stack 4 = mask right index
+				 * stack 5 = mask left index
+				 */
+				s32 mask_right = stack[--stack_ptr];
+				s32 mask_left = stack[--stack_ptr];
+				/* source 1 count */
+				a = 1 + index2 - index1;
+				/* source 2 count */
+				b = 1 + long_count - mask_index;
+				a = (a < b) ? a : b;
+				/* mask count */
+				b = 1 + mask_left - mask_right;
+				a = (a < b) ? a : b;
+				/* source 2 start index */
+				index2 = mask_index;
+				/* mask start index */
+				mask_index = mask_right;
+				long_count = a;
+			}
+
+			long_tmp = 1L;
+
+			if (long_count < 1)
+				status = -ERANGE;
+			else {
+				count = long_count;
+
+				for (i = 0; i < count; ++i) {
+					if (mask[mask_index >> 3] &
+						(1 << (mask_index & 7))) {
+						a = source1[index1 >> 3] &
+							(1 << (index1 & 7))
+								? 1 : 0;
+						b = source2[index2 >> 3] &
+							(1 << (index2 & 7))
+								? 1 : 0;
+
+						if (a != b) /* failure */
+							long_tmp = 0L;
+					}
+					++index1;
+					++index2;
+					++mask_index;
+				}
+			}
+
+			stack[stack_ptr++] = long_tmp;
+
+			break;
+		}
+		default:
+			/* Unrecognized opcode -- ERROR! */
+			bad_opcode = 1;
+			break;
+		}
+
+		if (bad_opcode)
+			status = -ENOSYS;
+
+		if ((stack_ptr < 0) || (stack_ptr >= ALTERA_STACK_SIZE))
+			status = -EOVERFLOW;
+
+		if (status != 0) {
+			done = 1;
+			*error_address = (s32)(opcode_address - code_sect);
+		}
+	}
+
+	altera_free_buffers(astate);
+
+	/* Free all dynamically allocated arrays */
+	if ((attrs != NULL) && (vars != NULL))
+		for (i = 0; i < sym_count; ++i)
+			if (attrs[i] & 0x80)
+				kfree((void *)vars[i]);
+
+	kfree(vars);
+	kfree(var_size);
+	kfree(attrs);
+	kfree(proc_attributes);
+
+	return status;
+}
+
+static int altera_get_note(u8 *p, s32 program_size,
+			s32 *offset, char *key, char *value, int length)
+/*
+ * Gets key and value of NOTE fields in the JBC file.
+ * Can be called in two modes:  if offset pointer is NULL,
+ * then the function searches for note fields which match
+ * the key string provided.  If offset is not NULL, then
+ * the function finds the next note field of any key,
+ * starting at the offset specified by the offset pointer.
+ * Returns 0 for success, else appropriate error code
+ */
+{
+	int status = -ENODATA;
+	u32 note_strings = 0L;
+	u32 note_table = 0L;
+	u32 note_count = 0L;
+	u32 first_word = 0L;
+	int version = 0;
+	int delta = 0;
+	char *key_ptr;
+	char *value_ptr;
+	int i;
+
+	/* Read header information */
+	if (program_size > 52L) {
+		first_word    = get_unaligned_be32(&p[0]);
+		version = (first_word & 1L);
+		delta = version * 8;
+
+		note_strings  = get_unaligned_be32(&p[8 + delta]);
+		note_table    = get_unaligned_be32(&p[12 + delta]);
+		note_count    = get_unaligned_be32(&p[44 + (2 * delta)]);
+	}
+
+	if ((first_word != 0x4A414D00L) && (first_word != 0x4A414D01L))
+		return -EIO;
+
+	if (note_count <= 0L)
+		return status;
+
+	if (offset == NULL) {
+		/*
+		 * We will search for the first note with a specific key,
+		 * and return only the value
+		 */
+		for (i = 0; (i < note_count) &&
+						(status != 0); ++i) {
+			key_ptr = &p[note_strings +
+					get_unaligned_be32(
+					&p[note_table + (8 * i)])];
+			if ((strnicmp(key, key_ptr, strlen(key_ptr)) == 0) &&
+						(key != NULL)) {
+				status = 0;
+
+				value_ptr = &p[note_strings +
+						get_unaligned_be32(
+						&p[note_table + (8 * i) + 4])];
+
+				if (value != NULL)
+					strlcpy(value, value_ptr, length);
+
+			}
+		}
+	} else {
+		/*
+		 * We will search for the next note, regardless of the key,
+		 * and return both the value and the key
+		 */
+
+		i = *offset;
+
+		if ((i >= 0) && (i < note_count)) {
+			status = 0;
+
+			if (key != NULL)
+				strlcpy(key, &p[note_strings +
+						get_unaligned_be32(
+						&p[note_table + (8 * i)])],
+					length);
+
+			if (value != NULL)
+				strlcpy(value, &p[note_strings +
+						get_unaligned_be32(
+						&p[note_table + (8 * i) + 4])],
+					length);
+
+			*offset = i + 1;
+		}
+	}
+
+	return status;
+}
+
+static int altera_check_crc(u8 *p, s32 program_size)
+{
+	int status = 0;
+	u16 local_expected = 0,
+	    local_actual = 0,
+	    shift_reg = 0xffff;
+	int bit, feedback;
+	u8 databyte;
+	u32 i;
+	u32 crc_section = 0L;
+	u32 first_word = 0L;
+	int version = 0;
+	int delta = 0;
+
+	if (program_size > 52L) {
+		first_word  = get_unaligned_be32(&p[0]);
+		version = (first_word & 1L);
+		delta = version * 8;
+
+		crc_section = get_unaligned_be32(&p[32 + delta]);
+	}
+
+	if ((first_word != 0x4A414D00L) && (first_word != 0x4A414D01L))
+		status = -EIO;
+
+	if (crc_section >= program_size)
+		status = -EIO;
+
+	if (status == 0) {
+		local_expected = (u16)get_unaligned_be16(&p[crc_section]);
+
+		for (i = 0; i < crc_section; ++i) {
+			databyte = p[i];
+			for (bit = 0; bit < 8; bit++) {
+				feedback = (databyte ^ shift_reg) & 0x01;
+				shift_reg >>= 1;
+				if (feedback)
+					shift_reg ^= 0x8408;
+
+				databyte >>= 1;
+			}
+		}
+
+		local_actual = (u16)~shift_reg;
+
+		if (local_expected != local_actual)
+			status = -EILSEQ;
+
+	}
+
+	if (debug || status) {
+		switch (status) {
+		case 0:
+			printk(KERN_INFO "%s: CRC matched: %04x\n", __func__,
+				local_actual);
+			break;
+		case -EILSEQ:
+			printk(KERN_ERR "%s: CRC mismatch: expected %04x, "
+				"actual %04x\n", __func__, local_expected,
+				local_actual);
+			break;
+		case -ENODATA:
+			printk(KERN_ERR "%s: expected CRC not found, "
+				"actual CRC = %04x\n", __func__,
+				local_actual);
+			break;
+		case -EIO:
+			printk(KERN_ERR "%s: error: format isn't "
+				"recognized.\n", __func__);
+			break;
+		default:
+			printk(KERN_ERR "%s: CRC function returned error "
+				"code %d\n", __func__, status);
+			break;
+		}
+	}
+
+	return status;
+}
+
+static int altera_get_file_info(u8 *p,
+					s32 program_size,
+					int *format_version,
+					int *action_count,
+					int *procedure_count)
+{
+	int status = -EIO;
+	u32 first_word = 0;
+	int version = 0;
+
+	if (program_size <= 52L)
+		return status;
+
+	first_word = get_unaligned_be32(&p[0]);
+
+	if ((first_word == 0x4A414D00L) || (first_word == 0x4A414D01L)) {
+		status = 0;
+
+		version = (first_word & 1L);
+		*format_version = version + 1;
+
+		if (version > 0) {
+			*action_count = get_unaligned_be32(&p[48]);
+			*procedure_count = get_unaligned_be32(&p[52]);
+		}
+	}
+
+	return status;
+}
+
+static int altera_get_act_info(u8 *p,
+					s32 program_size,
+					int index,
+					char **name,
+					char **description,
+					struct altera_procinfo **proc_list)
+{
+	int status = -EIO;
+	struct altera_procinfo *procptr = NULL;
+	struct altera_procinfo *tmpptr = NULL;
+	u32 first_word = 0L;
+	u32 action_table = 0L;
+	u32 proc_table = 0L;
+	u32 str_table = 0L;
+	u32 note_strings = 0L;
+	u32 action_count = 0L;
+	u32 proc_count = 0L;
+	u32 act_name_id = 0L;
+	u32 act_desc_id = 0L;
+	u32 act_proc_id = 0L;
+	u32 act_proc_name = 0L;
+	u8 act_proc_attribute = 0;
+
+	if (program_size <= 52L)
+		return status;
+	/* Read header information */
+	first_word = get_unaligned_be32(&p[0]);
+
+	if (first_word != 0x4A414D01L)
+		return status;
+
+	action_table = get_unaligned_be32(&p[4]);
+	proc_table   = get_unaligned_be32(&p[8]);
+	str_table = get_unaligned_be32(&p[12]);
+	note_strings = get_unaligned_be32(&p[16]);
+	action_count = get_unaligned_be32(&p[48]);
+	proc_count   = get_unaligned_be32(&p[52]);
+
+	if (index >= action_count)
+		return status;
+
+	act_name_id = get_unaligned_be32(&p[action_table + (12 * index)]);
+	act_desc_id = get_unaligned_be32(&p[action_table + (12 * index) + 4]);
+	act_proc_id = get_unaligned_be32(&p[action_table + (12 * index) + 8]);
+
+	*name = &p[str_table + act_name_id];
+
+	if (act_desc_id < (note_strings - str_table))
+		*description = &p[str_table + act_desc_id];
+
+	do {
+		act_proc_name = get_unaligned_be32(
+					&p[proc_table + (13 * act_proc_id)]);
+		act_proc_attribute =
+			(p[proc_table + (13 * act_proc_id) + 8] & 0x03);
+
+		procptr = (struct altera_procinfo *)
+				kzalloc(sizeof(struct altera_procinfo),
+								GFP_KERNEL);
+
+		if (procptr == NULL)
+			status = -ENOMEM;
+		else {
+			procptr->name = &p[str_table + act_proc_name];
+			procptr->attrs = act_proc_attribute;
+			procptr->next = NULL;
+
+			/* add record to end of linked list */
+			if (*proc_list == NULL)
+				*proc_list = procptr;
+			else {
+				tmpptr = *proc_list;
+				while (tmpptr->next != NULL)
+					tmpptr = tmpptr->next;
+				tmpptr->next = procptr;
+			}
+		}
+
+		act_proc_id = get_unaligned_be32(
+				&p[proc_table + (13 * act_proc_id) + 4]);
+	} while ((act_proc_id != 0) && (act_proc_id < proc_count));
+
+	return status;
+}
+
+int altera_init(struct altera_config *config, const struct firmware *fw)
+{
+	struct altera_state *astate = NULL;
+	struct altera_procinfo *proc_list = NULL;
+	struct altera_procinfo *procptr = NULL;
+	char *key = NULL;
+	char *value = NULL;
+	char *action_name = NULL;
+	char *description = NULL;
+	int exec_result = 0;
+	int exit_code = 0;
+	int format_version = 0;
+	int action_count = 0;
+	int procedure_count = 0;
+	int index = 0;
+	s32 offset = 0L;
+	s32 error_address = 0L;
+
+	key = kzalloc(33 * sizeof(char), GFP_KERNEL);
+	if (!key)
+		return -ENOMEM;
+	value = kzalloc(257 * sizeof(char), GFP_KERNEL);
+	if (!value)
+		return -ENOMEM;
+	astate = kzalloc(sizeof(struct altera_state), GFP_KERNEL);
+	if (!astate)
+		return -ENOMEM;
+
+	astate->config = config;
+	if (!astate->config->jtag_io) {
+		dprintk(KERN_INFO "%s: using byteblaster!\n", __func__);
+		astate->config->jtag_io = netup_jtag_io_lpt;
+	}
+
+	altera_check_crc((u8 *)fw->data, fw->size);
+
+	if (debug) {
+		altera_get_file_info((u8 *)fw->data, fw->size, &format_version,
+					&action_count, &procedure_count);
+		printk(KERN_INFO "%s: File format is %s ByteCode format\n",
+			__func__, (format_version == 2) ? "Jam STAPL" :
+						"pre-standardized Jam 1.1");
+		while (altera_get_note((u8 *)fw->data, fw->size,
+					&offset, key, value, 256) == 0)
+			printk(KERN_INFO "%s: NOTE \"%s\" = \"%s\"\n",
+					__func__, key, value);
+	}
+
+	if (debug && (format_version == 2) && (action_count > 0)) {
+		printk(KERN_INFO "%s: Actions available:\n", __func__);
+		for (index = 0; index < action_count; ++index) {
+			altera_get_act_info((u8 *)fw->data, fw->size,
+						index, &action_name,
+						&description,
+						&proc_list);
+
+			if (description == NULL)
+				printk(KERN_INFO "%s: %s\n",
+						__func__,
+						action_name);
+			else
+				printk(KERN_INFO "%s: %s \"%s\"\n",
+						__func__,
+						action_name,
+						description);
+
+			procptr = proc_list;
+			while (procptr != NULL) {
+				if (procptr->attrs != 0)
+					printk(KERN_INFO "%s:    %s (%s)\n",
+						__func__,
+						procptr->name,
+						(procptr->attrs == 1) ?
+						"optional" : "recommended");
+
+				proc_list = procptr->next;
+				kfree(procptr);
+				procptr = proc_list;
+			}
+		}
+
+		printk(KERN_INFO "\n");
+	}
+
+	exec_result = altera_execute(astate, (u8 *)fw->data, fw->size,
+				&error_address, &exit_code, &format_version);
+
+	if (exit_code)
+		exec_result = -EREMOTEIO;
+
+	if ((format_version == 2) && (exec_result == -EINVAL)) {
+		if (astate->config->action == NULL)
+			printk(KERN_ERR "%s: error: no action specified for "
+				"Jam STAPL file.\nprogram terminated.\n",
+				__func__);
+		else
+			printk(KERN_ERR "%s: error: action \"%s\""
+				" is not supported "
+				"for this Jam STAPL file.\n"
+				"Program terminated.\n", __func__,
+				astate->config->action);
+
+	} else if (exec_result)
+		printk(KERN_ERR "%s: error %d\n", __func__, exec_result);
+
+	kfree(key);
+	kfree(value);
+	kfree(astate);
+
+	return 0;
+}
+EXPORT_SYMBOL(altera_init);
diff --git a/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c
index 991463f4a7f4..9b7b71c294b8 100644
--- a/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c
@@ -2313,7 +2313,9 @@ static s32 wl_inform_single_bss(struct wl_priv *wl, struct wl_bss_info *bi)
 	notif_bss_info->frame_len =
 	    offsetof(struct ieee80211_mgmt,
 		     u.beacon.variable) + wl_get_ielen(wl);
-	freq = ieee80211_channel_to_frequency(notif_bss_info->channel);
+	freq = ieee80211_channel_to_frequency(notif_bss_info->channel,
+					      band->band);
+
 	channel = ieee80211_get_channel(wiphy, freq);
 
 	WL_DBG("SSID : \"%s\", rssi %d, channel %d, capability : 0x04%x, bssid %pM\n",
diff --git a/drivers/staging/brcm80211/sys/wl_mac80211.c b/drivers/staging/brcm80211/sys/wl_mac80211.c
index cd8392badff0..6363077468f1 100644
--- a/drivers/staging/brcm80211/sys/wl_mac80211.c
+++ b/drivers/staging/brcm80211/sys/wl_mac80211.c
@@ -104,9 +104,6 @@ static int wl_request_fw(struct wl_info *wl, struct pci_dev *pdev);
 static void wl_release_fw(struct wl_info *wl);
 
 /* local prototypes */
-static int wl_start(struct sk_buff *skb, struct wl_info *wl);
-static int wl_start_int(struct wl_info *wl, struct ieee80211_hw *hw,
-			struct sk_buff *skb);
 static void wl_dpc(unsigned long data);
 
 MODULE_AUTHOR("Broadcom Corporation");
@@ -135,7 +132,6 @@ module_param(phymsglevel, int, 0);
 
 #define HW_TO_WL(hw)	 (hw->priv)
 #define WL_TO_HW(wl)	  (wl->pub->ieee_hw)
-static int wl_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 static int wl_ops_start(struct ieee80211_hw *hw);
 static void wl_ops_stop(struct ieee80211_hw *hw);
 static int wl_ops_add_interface(struct ieee80211_hw *hw,
@@ -173,20 +169,18 @@ static int wl_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			   enum ieee80211_ampdu_mlme_action action,
 			   struct ieee80211_sta *sta, u16 tid, u16 *ssn);
 
-static int wl_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void wl_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
-	int status;
 	struct wl_info *wl = hw->priv;
 	WL_LOCK(wl);
 	if (!wl->pub->up) {
 		WL_ERROR("ops->tx called while down\n");
-		status = -ENETDOWN;
+		kfree_skb(skb);
 		goto done;
 	}
-	status = wl_start(skb, wl);
+	wlc_sendpkt_mac80211(wl->wlc, skb, hw);
  done:
 	WL_UNLOCK(wl);
-	return status;
 }
 
 static int wl_ops_start(struct ieee80211_hw *hw)
@@ -1325,22 +1319,6 @@ void wl_free(struct wl_info *wl)
 	osl_detach(osh);
 }
 
-/* transmit a packet */
-static int BCMFASTPATH wl_start(struct sk_buff *skb, struct wl_info *wl)
-{
-	if (!wl)
-		return -ENETDOWN;
-
-	return wl_start_int(wl, WL_TO_HW(wl), skb);
-}
-
-static int BCMFASTPATH
-wl_start_int(struct wl_info *wl, struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	wlc_sendpkt_mac80211(wl->wlc, skb, hw);
-	return NETDEV_TX_OK;
-}
-
 void wl_txflowcontrol(struct wl_info *wl, struct wl_if *wlif, bool state,
 		      int prio)
 {
diff --git a/drivers/staging/brcm80211/sys/wlc_mac80211.c b/drivers/staging/brcm80211/sys/wlc_mac80211.c
index e37e8058e2b8..aa12d1a65184 100644
--- a/drivers/staging/brcm80211/sys/wlc_mac80211.c
+++ b/drivers/staging/brcm80211/sys/wlc_mac80211.c
@@ -6818,11 +6818,14 @@ prep_mac80211_status(struct wlc_info *wlc, d11rxhdr_t *rxh, struct sk_buff *p,
 	ratespec_t rspec;
 	unsigned char *plcp;
 
+#if 0
+	/* Clearly, this is bogus -- reading the TSF now is wrong */
 	wlc_read_tsf(wlc, &tsf_l, &tsf_h);	/* mactime */
 	rx_status->mactime = tsf_h;
 	rx_status->mactime <<= 32;
 	rx_status->mactime |= tsf_l;
-	rx_status->flag |= RX_FLAG_TSFT;
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU; /* clearly wrong */
+#endif
 
 	channel = WLC_CHAN_CHANNEL(rxh->RxChan);
 
diff --git a/drivers/staging/cx25821/Kconfig b/drivers/staging/cx25821/Kconfig
index b2656957aa8f..5f6b54213713 100644
--- a/drivers/staging/cx25821/Kconfig
+++ b/drivers/staging/cx25821/Kconfig
@@ -1,7 +1,6 @@
 config VIDEO_CX25821
 	tristate "Conexant cx25821 support"
 	depends on DVB_CORE && VIDEO_DEV && PCI && I2C
-	depends on BKL # please fix
 	select I2C_ALGOBIT
 	select VIDEO_BTCX
 	select VIDEO_TVEEPROM
diff --git a/drivers/staging/cx25821/cx25821-alsa.c b/drivers/staging/cx25821/cx25821-alsa.c
index 160f6693aa33..ebdba7c65bc5 100644
--- a/drivers/staging/cx25821/cx25821-alsa.c
+++ b/drivers/staging/cx25821/cx25821-alsa.c
@@ -770,10 +770,12 @@ static int cx25821_alsa_init(void)
 	struct cx25821_dev *dev = NULL;
 	struct list_head *list;
 
+	mutex_lock(&cx25821_devlist_mutex);
 	list_for_each(list, &cx25821_devlist) {
 		dev = list_entry(list, struct cx25821_dev, devlist);
 		cx25821_audio_initdev(dev);
 	}
+	mutex_unlock(&cx25821_devlist_mutex);
 
 	if (dev == NULL)
 		pr_info("ERROR ALSA: no cx25821 cards found\n");
diff --git a/drivers/staging/cx25821/cx25821-core.c b/drivers/staging/cx25821/cx25821-core.c
index a216b620b718..523ac5e16c1b 100644
--- a/drivers/staging/cx25821/cx25821-core.c
+++ b/drivers/staging/cx25821/cx25821-core.c
@@ -33,9 +33,6 @@ MODULE_DESCRIPTION("Driver for Athena cards");
 MODULE_AUTHOR("Shu Lin - Hiep Huynh");
 MODULE_LICENSE("GPL");
 
-struct list_head cx25821_devlist;
-EXPORT_SYMBOL(cx25821_devlist);
-
 static unsigned int debug;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "enable debug messages");
@@ -46,8 +43,10 @@ MODULE_PARM_DESC(card, "card type");
 
 static unsigned int cx25821_devcount;
 
-static DEFINE_MUTEX(devlist);
+DEFINE_MUTEX(cx25821_devlist_mutex);
+EXPORT_SYMBOL(cx25821_devlist_mutex);
 LIST_HEAD(cx25821_devlist);
+EXPORT_SYMBOL(cx25821_devlist);
 
 struct sram_channel cx25821_sram_channels[] = {
 	[SRAM_CH00] = {
@@ -911,9 +910,9 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 	dev->nr = ++cx25821_devcount;
 	sprintf(dev->name, "cx25821[%d]", dev->nr);
 
-	mutex_lock(&devlist);
+	mutex_lock(&cx25821_devlist_mutex);
 	list_add_tail(&dev->devlist, &cx25821_devlist);
-	mutex_unlock(&devlist);
+	mutex_unlock(&cx25821_devlist_mutex);
 
 	strcpy(cx25821_boards[UNKNOWN_BOARD].name, "unknown");
 	strcpy(cx25821_boards[CX25821_BOARD].name, "cx25821");
@@ -1465,9 +1464,9 @@ static void __devexit cx25821_finidev(struct pci_dev *pci_dev)
 	if (pci_dev->irq)
 		free_irq(pci_dev->irq, dev);
 
-	mutex_lock(&devlist);
+	mutex_lock(&cx25821_devlist_mutex);
 	list_del(&dev->devlist);
-	mutex_unlock(&devlist);
+	mutex_unlock(&cx25821_devlist_mutex);
 
 	cx25821_dev_unregister(dev);
 	v4l2_device_unregister(v4l2_dev);
@@ -1501,7 +1500,6 @@ static struct pci_driver cx25821_pci_driver = {
 
 static int __init cx25821_init(void)
 {
-	INIT_LIST_HEAD(&cx25821_devlist);
 	pr_info("driver version %d.%d.%d loaded\n",
 		(CX25821_VERSION_CODE >> 16) & 0xff,
 		(CX25821_VERSION_CODE >> 8) & 0xff,
diff --git a/drivers/staging/cx25821/cx25821-video.c b/drivers/staging/cx25821/cx25821-video.c
index 0d8d75670516..ab05392386e8 100644
--- a/drivers/staging/cx25821/cx25821-video.c
+++ b/drivers/staging/cx25821/cx25821-video.c
@@ -27,7 +27,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include "cx25821-video.h"
-#include <linux/smp_lock.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx25821 based TV cards");
 MODULE_AUTHOR("Hiep Huynh <hiep.huynh@conexant.com>");
@@ -815,7 +814,7 @@ static int video_open(struct file *file)
        if (NULL == fh)
 	       return -ENOMEM;
 
-       lock_kernel();
+	mutex_lock(&cx25821_devlist_mutex);
 
        list_for_each(list, &cx25821_devlist)
        {
@@ -832,8 +831,8 @@ static int video_open(struct file *file)
        }
 
        if (NULL == dev) {
-	       unlock_kernel();
-	       return -ENODEV;
+		mutex_unlock(&cx25821_devlist_mutex);
+		return -ENODEV;
        }
 
        file->private_data = fh;
@@ -862,7 +861,7 @@ static int video_open(struct file *file)
 			      sizeof(struct cx25821_buffer), fh, NULL);
 
        dprintk(1, "post videobuf_queue_init()\n");
-       unlock_kernel();
+	mutex_unlock(&cx25821_devlist_mutex);
 
        return 0;
 }
diff --git a/drivers/staging/cx25821/cx25821.h b/drivers/staging/cx25821/cx25821.h
index 55115235f7f6..6230243e2ccb 100644
--- a/drivers/staging/cx25821/cx25821.h
+++ b/drivers/staging/cx25821/cx25821.h
@@ -31,7 +31,6 @@
 #include <linux/delay.h>
 #include <linux/sched.h>
 #include <linux/kdev_t.h>
-#include <linux/smp_lock.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
@@ -445,6 +444,8 @@ static inline struct cx25821_dev *get_cx25821(struct v4l2_device *v4l2_dev)
 	v4l2_device_call_all(&dev->v4l2_dev, 0, o, f, ##args)
 
 extern struct list_head cx25821_devlist;
+extern struct mutex cx25821_devlist_mutex;
+
 extern struct cx25821_board cx25821_boards[];
 extern struct cx25821_subid cx25821_subids[];
 
diff --git a/drivers/staging/cxd2099/Kconfig b/drivers/staging/cxd2099/Kconfig
new file mode 100644
index 000000000000..9d638c30735d
--- /dev/null
+++ b/drivers/staging/cxd2099/Kconfig
@@ -0,0 +1,11 @@
+config DVB_CXD2099
+        tristate "CXD2099AR Common Interface driver"
+        depends on DVB_CORE && PCI && I2C && DVB_NGENE
+        ---help---
+          Support for the CI module found on cineS2 DVB-S2, supported by
+	  the Micronas PCIe device driver (ngene).
+
+	  For now, data is passed through '/dev/dvb/adapterX/sec0':
+	    - Encrypted data must be written to 'sec0'.
+	    - Decrypted data can be read from 'sec0'.
+	    - Setup the CAM using device 'ca0'.
diff --git a/drivers/staging/cxd2099/Makefile b/drivers/staging/cxd2099/Makefile
new file mode 100644
index 000000000000..72b14558c119
--- /dev/null
+++ b/drivers/staging/cxd2099/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_DVB_CXD2099) += cxd2099.o
+
+EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core/
+EXTRA_CFLAGS += -Idrivers/media/dvb/frontends/
+EXTRA_CFLAGS += -Idrivers/media/common/tuners/
diff --git a/drivers/staging/cxd2099/TODO b/drivers/staging/cxd2099/TODO
new file mode 100644
index 000000000000..375bb6f8ee2c
--- /dev/null
+++ b/drivers/staging/cxd2099/TODO
@@ -0,0 +1,12 @@
+For now, data is passed through '/dev/dvb/adapterX/sec0':
+ - Encrypted data must be written to 'sec0'.
+ - Decrypted data can be read from 'sec0'.
+ - Setup the CAM using device 'ca0'.
+
+But this is wrong. There are some discussions about the proper way for
+doing it, as seen at:
+	http://www.mail-archive.com/linux-media@vger.kernel.org/msg22196.html
+
+While there's no proper fix for it, the driver should be kept in staging.
+
+Patches should be submitted to: linux-media@vger.kernel.org.
diff --git a/drivers/staging/cxd2099/cxd2099.c b/drivers/staging/cxd2099/cxd2099.c
new file mode 100644
index 000000000000..b49186c74eb3
--- /dev/null
+++ b/drivers/staging/cxd2099/cxd2099.c
@@ -0,0 +1,574 @@
+/*
+ * cxd2099.c: Driver for the CXD2099AR Common Interface Controller
+ *
+ * Copyright (C) 2010 DigitalDevices UG
+ *
+ *
+ * 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 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
+ * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/version.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+
+#include "cxd2099.h"
+
+#define MAX_BUFFER_SIZE 248
+
+struct cxd {
+	struct dvb_ca_en50221 en;
+
+	struct i2c_adapter *i2c;
+	u8     adr;
+	u8     regs[0x23];
+	u8     lastaddress;
+	u8     clk_reg_f;
+	u8     clk_reg_b;
+	int    mode;
+	u32    bitrate;
+	int    ready;
+	int    dr;
+	int    slot_stat;
+
+	u8     amem[1024];
+	int    amem_read;
+
+	int    cammode;
+	struct mutex lock;
+};
+
+static int i2c_write_reg(struct i2c_adapter *adapter, u8 adr,
+			 u8 reg, u8 data)
+{
+	u8 m[2] = {reg, data};
+	struct i2c_msg msg = {.addr = adr, .flags = 0, .buf = m, .len = 2};
+
+	if (i2c_transfer(adapter, &msg, 1) != 1) {
+		printk(KERN_ERR "Failed to write to I2C register %02x@%02x!\n",
+		       reg, adr);
+		return -1;
+	}
+	return 0;
+}
+
+static int i2c_write(struct i2c_adapter *adapter, u8 adr,
+		     u8 *data, u8 len)
+{
+	struct i2c_msg msg = {.addr = adr, .flags = 0, .buf = data, .len = len};
+
+	if (i2c_transfer(adapter, &msg, 1) != 1) {
+		printk(KERN_ERR "Failed to write to I2C!\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int i2c_read_reg(struct i2c_adapter *adapter, u8 adr,
+			u8 reg, u8 *val)
+{
+	struct i2c_msg msgs[2] = {{.addr = adr, .flags = 0,
+				   .buf = &reg, .len = 1 },
+				  {.addr = adr, .flags = I2C_M_RD,
+				   .buf = val, .len = 1 } };
+
+	if (i2c_transfer(adapter, msgs, 2) != 2) {
+		printk(KERN_ERR "error in i2c_read_reg\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int i2c_read(struct i2c_adapter *adapter, u8 adr,
+		    u8 reg, u8 *data, u8 n)
+{
+	struct i2c_msg msgs[2] = {{.addr = adr, .flags = 0,
+				   .buf = &reg, .len = 1 },
+				  {.addr = adr, .flags = I2C_M_RD,
+				   .buf = data, .len = n } };
+
+	if (i2c_transfer(adapter, msgs, 2) != 2) {
+		printk(KERN_ERR "error in i2c_read\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int read_block(struct cxd *ci, u8 adr, u8 *data, u8 n)
+{
+	int status;
+
+	status = i2c_write_reg(ci->i2c, ci->adr, 0, adr);
+	if (!status) {
+		ci->lastaddress = adr;
+		status = i2c_read(ci->i2c, ci->adr, 1, data, n);
+	}
+	return status;
+}
+
+static int read_reg(struct cxd *ci, u8 reg, u8 *val)
+{
+	return read_block(ci, reg, val, 1);
+}
+
+
+static int read_pccard(struct cxd *ci, u16 address, u8 *data, u8 n)
+{
+	int status;
+	u8 addr[3] = { 2, address&0xff, address>>8 };
+
+	status = i2c_write(ci->i2c, ci->adr, addr, 3);
+	if (!status)
+		status = i2c_read(ci->i2c, ci->adr, 3, data, n);
+	return status;
+}
+
+static int write_pccard(struct cxd *ci, u16 address, u8 *data, u8 n)
+{
+	int status;
+	u8 addr[3] = { 2, address&0xff, address>>8 };
+
+	status = i2c_write(ci->i2c, ci->adr, addr, 3);
+	if (!status) {
+		u8 buf[256] = {3};
+		memcpy(buf+1, data, n);
+		status = i2c_write(ci->i2c, ci->adr, buf, n+1);
+	}
+	return status;
+}
+
+static int read_io(struct cxd *ci, u16 address, u8 *val)
+{
+	int status;
+	u8 addr[3] = { 2, address&0xff, address>>8 };
+
+	status = i2c_write(ci->i2c, ci->adr, addr, 3);
+	if (!status)
+		status = i2c_read(ci->i2c, ci->adr, 3, val, 1);
+	return status;
+}
+
+static int write_io(struct cxd *ci, u16 address, u8 val)
+{
+	int status;
+	u8 addr[3] = { 2, address&0xff, address>>8 };
+	u8 buf[2] = { 3, val };
+
+	status = i2c_write(ci->i2c, ci->adr, addr, 3);
+	if (!status)
+		status = i2c_write(ci->i2c, ci->adr, buf, 2);
+
+	return status;
+}
+
+
+static int write_regm(struct cxd *ci, u8 reg, u8 val, u8 mask)
+{
+	int status;
+
+	status = i2c_write_reg(ci->i2c, ci->adr, 0, reg);
+	if (!status && reg >= 6 && reg <= 8 && mask != 0xff)
+		status = i2c_read_reg(ci->i2c, ci->adr, 1, &ci->regs[reg]);
+	ci->regs[reg] = (ci->regs[reg]&(~mask))|val;
+	if (!status) {
+		ci->lastaddress = reg;
+		status = i2c_write_reg(ci->i2c, ci->adr, 1, ci->regs[reg]);
+	}
+	if (reg == 0x20)
+		ci->regs[reg] &= 0x7f;
+	return status;
+}
+
+static int write_reg(struct cxd *ci, u8 reg, u8 val)
+{
+	return write_regm(ci, reg, val, 0xff);
+}
+
+#ifdef BUFFER_MODE
+static int write_block(struct cxd *ci, u8 adr, u8 *data, int n)
+{
+	int status;
+	u8 buf[256] = {1};
+
+	status = i2c_write_reg(ci->i2c, ci->adr, 0, adr);
+	if (!status) {
+		ci->lastaddress = adr;
+		memcpy(buf+1, data, n);
+		status = i2c_write(ci->i2c, ci->adr, buf, n+1);
+	}
+	return status;
+}
+#endif
+
+static void set_mode(struct cxd *ci, int mode)
+{
+	if (mode == ci->mode)
+		return;
+
+	switch (mode) {
+	case 0x00: /* IO mem */
+		write_regm(ci, 0x06, 0x00, 0x07);
+		break;
+	case 0x01: /* ATT mem */
+		write_regm(ci, 0x06, 0x02, 0x07);
+		break;
+	default:
+		break;
+	}
+	ci->mode = mode;
+}
+
+static void cam_mode(struct cxd *ci, int mode)
+{
+	if (mode == ci->cammode)
+		return;
+
+	switch (mode) {
+	case 0x00:
+		write_regm(ci, 0x20, 0x80, 0x80);
+		break;
+	case 0x01:
+		printk(KERN_INFO "enable cam buffer mode\n");
+		/* write_reg(ci, 0x0d, 0x00); */
+		/* write_reg(ci, 0x0e, 0x01); */
+		write_regm(ci, 0x08, 0x40, 0x40);
+		/* read_reg(ci, 0x12, &dummy); */
+		write_regm(ci, 0x08, 0x80, 0x80);
+		break;
+	default:
+		break;
+	}
+	ci->cammode = mode;
+}
+
+
+
+#define CHK_ERROR(s) if ((status = s)) break
+
+static int init(struct cxd *ci)
+{
+	int status;
+
+	mutex_lock(&ci->lock);
+	ci->mode = -1;
+	do {
+		CHK_ERROR(write_reg(ci, 0x00, 0x00));
+		CHK_ERROR(write_reg(ci, 0x01, 0x00));
+		CHK_ERROR(write_reg(ci, 0x02, 0x10));
+		CHK_ERROR(write_reg(ci, 0x03, 0x00));
+		CHK_ERROR(write_reg(ci, 0x05, 0xFF));
+		CHK_ERROR(write_reg(ci, 0x06, 0x1F));
+		CHK_ERROR(write_reg(ci, 0x07, 0x1F));
+		CHK_ERROR(write_reg(ci, 0x08, 0x28));
+		CHK_ERROR(write_reg(ci, 0x14, 0x20));
+
+		CHK_ERROR(write_reg(ci, 0x09, 0x4D)); /* Input Mode C, BYPass Serial, TIVAL = low, MSB */
+		CHK_ERROR(write_reg(ci, 0x0A, 0xA7)); /* TOSTRT = 8, Mode B (gated clock), falling Edge, Serial, POL=HIGH, MSB */
+
+		/* Sync detector */
+		CHK_ERROR(write_reg(ci, 0x0B, 0x33));
+		CHK_ERROR(write_reg(ci, 0x0C, 0x33));
+
+		CHK_ERROR(write_regm(ci, 0x14, 0x00, 0x0F));
+		CHK_ERROR(write_reg(ci, 0x15, ci->clk_reg_b));
+		CHK_ERROR(write_regm(ci, 0x16, 0x00, 0x0F));
+		CHK_ERROR(write_reg(ci, 0x17, ci->clk_reg_f));
+
+		CHK_ERROR(write_reg(ci, 0x20, 0x28)); /* Integer Divider, Falling Edge, Internal Sync, */
+		CHK_ERROR(write_reg(ci, 0x21, 0x00)); /* MCLKI = TICLK/8 */
+		CHK_ERROR(write_reg(ci, 0x22, 0x07)); /* MCLKI = TICLK/8 */
+
+
+		CHK_ERROR(write_regm(ci, 0x20, 0x80, 0x80)); /* Reset CAM state machine */
+
+		CHK_ERROR(write_regm(ci, 0x03, 0x02, 02));  /* Enable IREQA Interrupt */
+		CHK_ERROR(write_reg(ci, 0x01, 0x04));  /* Enable CD Interrupt */
+		CHK_ERROR(write_reg(ci, 0x00, 0x31));  /* Enable TS1,Hot Swap,Slot A */
+		CHK_ERROR(write_regm(ci, 0x09, 0x08, 0x08));  /* Put TS in bypass */
+		ci->cammode = -1;
+#ifdef BUFFER_MODE
+		cam_mode(ci, 0);
+#endif
+	} while (0);
+	mutex_unlock(&ci->lock);
+
+	return 0;
+}
+
+
+static int read_attribute_mem(struct dvb_ca_en50221 *ca,
+			      int slot, int address)
+{
+	struct cxd *ci = ca->data;
+	u8 val;
+	mutex_lock(&ci->lock);
+	set_mode(ci, 1);
+	read_pccard(ci, address, &val, 1);
+	mutex_unlock(&ci->lock);
+	return val;
+}
+
+
+static int write_attribute_mem(struct dvb_ca_en50221 *ca, int slot,
+			       int address, u8 value)
+{
+	struct cxd *ci = ca->data;
+
+	mutex_lock(&ci->lock);
+	set_mode(ci, 1);
+	write_pccard(ci, address, &value, 1);
+	mutex_unlock(&ci->lock);
+	return 0;
+}
+
+static int read_cam_control(struct dvb_ca_en50221 *ca,
+			    int slot, u8 address)
+{
+	struct cxd *ci = ca->data;
+	u8 val;
+
+	mutex_lock(&ci->lock);
+	set_mode(ci, 0);
+	read_io(ci, address, &val);
+	mutex_unlock(&ci->lock);
+	return val;
+}
+
+static int write_cam_control(struct dvb_ca_en50221 *ca, int slot,
+			     u8 address, u8 value)
+{
+	struct cxd *ci = ca->data;
+
+	mutex_lock(&ci->lock);
+	set_mode(ci, 0);
+	write_io(ci, address, value);
+	mutex_unlock(&ci->lock);
+	return 0;
+}
+
+static int slot_reset(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct cxd *ci = ca->data;
+
+	mutex_lock(&ci->lock);
+	cam_mode(ci, 0);
+	write_reg(ci, 0x00, 0x21);
+	write_reg(ci, 0x06, 0x1F);
+	write_reg(ci, 0x00, 0x31);
+	write_regm(ci, 0x20, 0x80, 0x80);
+	write_reg(ci, 0x03, 0x02);
+	ci->ready = 0;
+	ci->mode = -1;
+	{
+		int i;
+		for (i = 0; i < 100; i++) {
+			msleep(10);
+			if (ci->ready)
+				break;
+		}
+	}
+	mutex_unlock(&ci->lock);
+	/* msleep(500); */
+	return 0;
+}
+
+static int slot_shutdown(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct cxd *ci = ca->data;
+
+	printk(KERN_INFO "slot_shutdown\n");
+	mutex_lock(&ci->lock);
+	/* write_regm(ci, 0x09, 0x08, 0x08); */
+	write_regm(ci, 0x20, 0x80, 0x80);
+	write_regm(ci, 0x06, 0x07, 0x07);
+	ci->mode = -1;
+	mutex_unlock(&ci->lock);
+	return 0; /* shutdown(ci); */
+}
+
+static int slot_ts_enable(struct dvb_ca_en50221 *ca, int slot)
+{
+	struct cxd *ci = ca->data;
+
+	mutex_lock(&ci->lock);
+	write_regm(ci, 0x09, 0x00, 0x08);
+	set_mode(ci, 0);
+#ifdef BUFFER_MODE
+	cam_mode(ci, 1);
+#endif
+	mutex_unlock(&ci->lock);
+	return 0;
+}
+
+
+static int campoll(struct cxd *ci)
+{
+	u8 istat;
+
+	read_reg(ci, 0x04, &istat);
+	if (!istat)
+		return 0;
+	write_reg(ci, 0x05, istat);
+
+	if (istat&0x40) {
+		ci->dr = 1;
+		printk(KERN_INFO "DR\n");
+	}
+	if (istat&0x20)
+		printk(KERN_INFO "WC\n");
+
+	if (istat&2) {
+		u8 slotstat;
+
+		read_reg(ci, 0x01, &slotstat);
+		if (!(2&slotstat)) {
+			if (!ci->slot_stat) {
+				ci->slot_stat |= DVB_CA_EN50221_POLL_CAM_PRESENT;
+				write_regm(ci, 0x03, 0x08, 0x08);
+			}
+
+		} else {
+			if (ci->slot_stat) {
+				ci->slot_stat = 0;
+				write_regm(ci, 0x03, 0x00, 0x08);
+				printk(KERN_INFO "NO CAM\n");
+				ci->ready = 0;
+			}
+		}
+		if (istat&8 && ci->slot_stat == DVB_CA_EN50221_POLL_CAM_PRESENT) {
+			ci->ready = 1;
+			ci->slot_stat |= DVB_CA_EN50221_POLL_CAM_READY;
+			printk(KERN_INFO "READY\n");
+		}
+	}
+	return 0;
+}
+
+
+static int poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open)
+{
+	struct cxd *ci = ca->data;
+	u8 slotstat;
+
+	mutex_lock(&ci->lock);
+	campoll(ci);
+	read_reg(ci, 0x01, &slotstat);
+	mutex_unlock(&ci->lock);
+
+	return ci->slot_stat;
+}
+
+#ifdef BUFFER_MODE
+static int read_data(struct dvb_ca_en50221 *ca, int slot, u8 *ebuf, int ecount)
+{
+	struct cxd *ci = ca->data;
+	u8 msb, lsb;
+	u16 len;
+
+	mutex_lock(&ci->lock);
+	campoll(ci);
+	mutex_unlock(&ci->lock);
+
+	printk(KERN_INFO "read_data\n");
+	if (!ci->dr)
+		return 0;
+
+	mutex_lock(&ci->lock);
+	read_reg(ci, 0x0f, &msb);
+	read_reg(ci, 0x10, &lsb);
+	len = (msb<<8)|lsb;
+	read_block(ci, 0x12, ebuf, len);
+	ci->dr = 0;
+	mutex_unlock(&ci->lock);
+
+	return len;
+}
+
+static int write_data(struct dvb_ca_en50221 *ca, int slot, u8 *ebuf, int ecount)
+{
+	struct cxd *ci = ca->data;
+
+	mutex_lock(&ci->lock);
+	printk(KERN_INFO "write_data %d\n", ecount);
+	write_reg(ci, 0x0d, ecount>>8);
+	write_reg(ci, 0x0e, ecount&0xff);
+	write_block(ci, 0x11, ebuf, ecount);
+	mutex_unlock(&ci->lock);
+	return ecount;
+}
+#endif
+
+static struct dvb_ca_en50221 en_templ = {
+	.read_attribute_mem  = read_attribute_mem,
+	.write_attribute_mem = write_attribute_mem,
+	.read_cam_control    = read_cam_control,
+	.write_cam_control   = write_cam_control,
+	.slot_reset          = slot_reset,
+	.slot_shutdown       = slot_shutdown,
+	.slot_ts_enable      = slot_ts_enable,
+	.poll_slot_status    = poll_slot_status,
+#ifdef BUFFER_MODE
+	.read_data           = read_data,
+	.write_data          = write_data,
+#endif
+
+};
+
+struct dvb_ca_en50221 *cxd2099_attach(u8 adr, void *priv,
+				      struct i2c_adapter *i2c)
+{
+	struct cxd *ci = 0;
+	u32 bitrate = 62000000;
+	u8 val;
+
+	if (i2c_read_reg(i2c, adr, 0, &val) < 0) {
+		printk(KERN_ERR "No CXD2099 detected at %02x\n", adr);
+		return 0;
+	}
+
+	ci = kmalloc(sizeof(struct cxd), GFP_KERNEL);
+	if (!ci)
+		return 0;
+	memset(ci, 0, sizeof(*ci));
+
+	mutex_init(&ci->lock);
+	ci->i2c = i2c;
+	ci->adr = adr;
+	ci->lastaddress = 0xff;
+	ci->clk_reg_b = 0x4a;
+	ci->clk_reg_f = 0x1b;
+	ci->bitrate = bitrate;
+
+	memcpy(&ci->en, &en_templ, sizeof(en_templ));
+	ci->en.data = ci;
+	init(ci);
+	printk(KERN_INFO "Attached CXD2099AR at %02x\n", ci->adr);
+	return &ci->en;
+}
+EXPORT_SYMBOL(cxd2099_attach);
+
+MODULE_DESCRIPTION("cxd2099");
+MODULE_AUTHOR("Ralph Metzler <rjkm@metzlerbros.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/cxd2099/cxd2099.h b/drivers/staging/cxd2099/cxd2099.h
new file mode 100644
index 000000000000..bed54ff3e30b
--- /dev/null
+++ b/drivers/staging/cxd2099/cxd2099.h
@@ -0,0 +1,41 @@
+/*
+ * cxd2099.h: Driver for the CXD2099AR Common Interface Controller
+ *
+ * Copyright (C) 2010 DigitalDevices UG
+ *
+ *
+ * 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 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
+ * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
+ */
+
+#ifndef _CXD2099_H_
+#define _CXD2099_H_
+
+#include <dvb_ca_en50221.h>
+
+#if defined(CONFIG_DVB_CXD2099) || \
+        (defined(CONFIG_DVB_CXD2099_MODULE) && defined(MODULE))
+struct dvb_ca_en50221 *cxd2099_attach(u8 adr, void *priv, struct i2c_adapter *i2c);
+#else
+static inline struct dvb_ca_en50221 *cxd2099_attach(u8 adr, void *priv, struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/staging/dabusb/Kconfig b/drivers/staging/dabusb/Kconfig
deleted file mode 100644
index 87bdc425d3c5..000000000000
--- a/drivers/staging/dabusb/Kconfig
+++ /dev/null
@@ -1,14 +0,0 @@
-config USB_DABUSB
-	tristate "DABUSB driver"
-	depends on USB
-	---help---
-	  A Digital Audio Broadcasting (DAB) Receiver for USB and Linux
-	  brought to you by the DAB-Team
-	  <http://wwwbode.cs.tum.edu/Par/arch/dab/>.  This driver can be taken
-	  as an example for URB-based bulk, control, and isochronous
-	  transactions. URB's are explained in
-	  <Documentation/usb/URB.txt>.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called dabusb.
-
diff --git a/drivers/staging/dabusb/Makefile b/drivers/staging/dabusb/Makefile
deleted file mode 100644
index 2ff2f228e5f9..000000000000
--- a/drivers/staging/dabusb/Makefile
+++ /dev/null
@@ -1,2 +0,0 @@
-obj-$(CONFIG_USB_DABUSB)        += dabusb.o
-
diff --git a/drivers/staging/dabusb/TODO b/drivers/staging/dabusb/TODO
deleted file mode 100644
index f9c0314ea0c1..000000000000
--- a/drivers/staging/dabusb/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-This is a driver for an experimental sample developed in 2003. The driver
-never supported any commercial product, nor had any known user.
-If nobody takes care on it, the driver will be removed for 2.6.39.
-
-Please send patches to linux-media@vger.kernel.org
diff --git a/drivers/staging/dabusb/dabusb.c b/drivers/staging/dabusb/dabusb.c
deleted file mode 100644
index f3e25e91366d..000000000000
--- a/drivers/staging/dabusb/dabusb.c
+++ /dev/null
@@ -1,914 +0,0 @@
-/*****************************************************************************/
-
-/*
- *      dabusb.c  --  dab usb driver.
- *
- *      Copyright (C) 1999  Deti Fliegl (deti@fliegl.de)
- *
- *      This program is free software; you can redistribute it and/or modify
- *      it under the terms of the GNU General Public License as published by
- *      the Free Software Foundation; either version 2 of the License, or
- *      (at your option) any later version.
- *
- *      This program is distributed in the hope that it will be useful,
- *      but WITHOUT ANY WARRANTY; without even the implied warranty of
- *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *      GNU General Public License for more details.
- *
- *      You should have received a copy of the GNU General Public License
- *      along with this program; if not, write to the Free Software
- *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *
- *  $Id: dabusb.c,v 1.54 2000/07/24 21:39:39 deti Exp $
- *
- */
-
-/*****************************************************************************/
-
-#include <linux/module.h>
-#include <linux/socket.h>
-#include <linux/list.h>
-#include <linux/vmalloc.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <asm/uaccess.h>
-#include <asm/atomic.h>
-#include <linux/delay.h>
-#include <linux/usb.h>
-#include <linux/mutex.h>
-#include <linux/firmware.h>
-#include <linux/ihex.h>
-
-#include "dabusb.h"
-
-/*
- * Version Information
- */
-#define DRIVER_VERSION "v1.54"
-#define DRIVER_AUTHOR "Deti Fliegl, deti@fliegl.de"
-#define DRIVER_DESC "DAB-USB Interface Driver for Linux (c)1999"
-
-/* --------------------------------------------------------------------- */
-
-#ifdef CONFIG_USB_DYNAMIC_MINORS
-#define NRDABUSB 256
-#else
-#define NRDABUSB 4
-#endif
-
-/*-------------------------------------------------------------------*/
-
-static dabusb_t dabusb[NRDABUSB];
-static int buffers = 256;
-static struct usb_driver dabusb_driver;
-
-/*-------------------------------------------------------------------*/
-
-static int dabusb_add_buf_tail (pdabusb_t s, struct list_head *dst, struct list_head *src)
-{
-	unsigned long flags;
-	struct list_head *tmp;
-	int ret = 0;
-
-	spin_lock_irqsave (&s->lock, flags);
-
-	if (list_empty (src)) {
-		// no elements in source buffer
-		ret = -1;
-		goto err;
-	}
-	tmp = src->next;
-	list_move_tail (tmp, dst);
-
-  err:	spin_unlock_irqrestore (&s->lock, flags);
-	return ret;
-}
-/*-------------------------------------------------------------------*/
-#ifdef DEBUG
-static void dump_urb (struct urb *urb)
-{
-	dbg("urb                   :%p", urb);
-	dbg("dev                   :%p", urb->dev);
-	dbg("pipe                  :%08X", urb->pipe);
-	dbg("status                :%d", urb->status);
-	dbg("transfer_flags        :%08X", urb->transfer_flags);
-	dbg("transfer_buffer       :%p", urb->transfer_buffer);
-	dbg("transfer_buffer_length:%d", urb->transfer_buffer_length);
-	dbg("actual_length         :%d", urb->actual_length);
-	dbg("setup_packet          :%p", urb->setup_packet);
-	dbg("start_frame           :%d", urb->start_frame);
-	dbg("number_of_packets     :%d", urb->number_of_packets);
-	dbg("interval              :%d", urb->interval);
-	dbg("error_count           :%d", urb->error_count);
-	dbg("context               :%p", urb->context);
-	dbg("complete              :%p", urb->complete);
-}
-#endif
-/*-------------------------------------------------------------------*/
-static int dabusb_cancel_queue (pdabusb_t s, struct list_head *q)
-{
-	unsigned long flags;
-	pbuff_t b;
-
-	dbg("dabusb_cancel_queue");
-
-	spin_lock_irqsave (&s->lock, flags);
-
-	list_for_each_entry(b, q, buff_list) {
-#ifdef DEBUG
-		dump_urb(b->purb);
-#endif
-		usb_unlink_urb (b->purb);
-	}
-	spin_unlock_irqrestore (&s->lock, flags);
-	return 0;
-}
-/*-------------------------------------------------------------------*/
-static int dabusb_free_queue (struct list_head *q)
-{
-	struct list_head *tmp;
-	struct list_head *p;
-	pbuff_t b;
-
-	dbg("dabusb_free_queue");
-	for (p = q->next; p != q;) {
-		b = list_entry (p, buff_t, buff_list);
-
-#ifdef DEBUG
-		dump_urb(b->purb);
-#endif
-		kfree(b->purb->transfer_buffer);
-		usb_free_urb(b->purb);
-		tmp = p->next;
-		list_del (p);
-		kfree (b);
-		p = tmp;
-	}
-
-	return 0;
-}
-/*-------------------------------------------------------------------*/
-static int dabusb_free_buffers (pdabusb_t s)
-{
-	unsigned long flags;
-	dbg("dabusb_free_buffers");
-
-	spin_lock_irqsave(&s->lock, flags);
-
-	dabusb_free_queue (&s->free_buff_list);
-	dabusb_free_queue (&s->rec_buff_list);
-
-	spin_unlock_irqrestore(&s->lock, flags);
-
-	s->got_mem = 0;
-	return 0;
-}
-/*-------------------------------------------------------------------*/
-static void dabusb_iso_complete (struct urb *purb)
-{
-	pbuff_t b = purb->context;
-	pdabusb_t s = b->s;
-	int i;
-	int len;
-	int dst = 0;
-	void *buf = purb->transfer_buffer;
-
-	dbg("dabusb_iso_complete");
-
-	// process if URB was not killed
-	if (purb->status != -ENOENT) {
-		unsigned int pipe = usb_rcvisocpipe (purb->dev, _DABUSB_ISOPIPE);
-		int pipesize = usb_maxpacket (purb->dev, pipe, usb_pipeout (pipe));
-		for (i = 0; i < purb->number_of_packets; i++)
-			if (!purb->iso_frame_desc[i].status) {
-				len = purb->iso_frame_desc[i].actual_length;
-				if (len <= pipesize) {
-					memcpy (buf + dst, buf + purb->iso_frame_desc[i].offset, len);
-					dst += len;
-				}
-				else
-					dev_err(&purb->dev->dev,
-						"dabusb_iso_complete: invalid len %d\n", len);
-			}
-			else
-				dev_warn(&purb->dev->dev, "dabusb_iso_complete: corrupted packet status: %d\n", purb->iso_frame_desc[i].status);
-		if (dst != purb->actual_length)
-			dev_err(&purb->dev->dev,
-				"dst!=purb->actual_length:%d!=%d\n",
-					dst, purb->actual_length);
-	}
-
-	if (atomic_dec_and_test (&s->pending_io) && !s->remove_pending && s->state != _stopped) {
-		s->overruns++;
-		dev_err(&purb->dev->dev, "overrun (%d)\n", s->overruns);
-	}
-	wake_up (&s->wait);
-}
-/*-------------------------------------------------------------------*/
-static int dabusb_alloc_buffers (pdabusb_t s)
-{
-	int transfer_len = 0;
-	pbuff_t b;
-	unsigned int pipe = usb_rcvisocpipe (s->usbdev, _DABUSB_ISOPIPE);
-	int pipesize = usb_maxpacket (s->usbdev, pipe, usb_pipeout (pipe));
-	int packets = _ISOPIPESIZE / pipesize;
-	int transfer_buffer_length = packets * pipesize;
-	int i;
-
-	dbg("dabusb_alloc_buffers pipesize:%d packets:%d transfer_buffer_len:%d",
-		 pipesize, packets, transfer_buffer_length);
-
-	while (transfer_len < (s->total_buffer_size << 10)) {
-		b = kzalloc(sizeof (buff_t), GFP_KERNEL);
-		if (!b) {
-			dev_err(&s->usbdev->dev,
-				"kzalloc(sizeof(buff_t))==NULL\n");
-			goto err;
-		}
-		b->s = s;
-		b->purb = usb_alloc_urb(packets, GFP_KERNEL);
-		if (!b->purb) {
-			dev_err(&s->usbdev->dev, "usb_alloc_urb == NULL\n");
-			kfree (b);
-			goto err;
-		}
-
-		b->purb->transfer_buffer = kmalloc (transfer_buffer_length, GFP_KERNEL);
-		if (!b->purb->transfer_buffer) {
-			kfree (b->purb);
-			kfree (b);
-			dev_err(&s->usbdev->dev,
-				"kmalloc(%d)==NULL\n", transfer_buffer_length);
-			goto err;
-		}
-
-		b->purb->transfer_buffer_length = transfer_buffer_length;
-		b->purb->number_of_packets = packets;
-		b->purb->complete = dabusb_iso_complete;
-		b->purb->context = b;
-		b->purb->dev = s->usbdev;
-		b->purb->pipe = pipe;
-		b->purb->transfer_flags = URB_ISO_ASAP;
-
-		for (i = 0; i < packets; i++) {
-			b->purb->iso_frame_desc[i].offset = i * pipesize;
-			b->purb->iso_frame_desc[i].length = pipesize;
-		}
-
-		transfer_len += transfer_buffer_length;
-		list_add_tail (&b->buff_list, &s->free_buff_list);
-	}
-	s->got_mem = transfer_len;
-
-	return 0;
-
-	err:
-	dabusb_free_buffers (s);
-	return -ENOMEM;
-}
-/*-------------------------------------------------------------------*/
-static int dabusb_bulk (pdabusb_t s, pbulk_transfer_t pb)
-{
-	int ret;
-	unsigned int pipe;
-	int actual_length;
-
-	dbg("dabusb_bulk");
-
-	if (!pb->pipe)
-		pipe = usb_rcvbulkpipe (s->usbdev, 2);
-	else
-		pipe = usb_sndbulkpipe (s->usbdev, 2);
-
-	ret=usb_bulk_msg(s->usbdev, pipe, pb->data, pb->size, &actual_length, 100);
-	if(ret<0) {
-		dev_err(&s->usbdev->dev,
-			"usb_bulk_msg failed(%d)\n", ret);
-
-		if (usb_set_interface (s->usbdev, _DABUSB_IF, 1) < 0) {
-			dev_err(&s->usbdev->dev, "set_interface failed\n");
-			return -EINVAL;
-		}
-
-	}
-
-	if( ret == -EPIPE ) {
-		dev_warn(&s->usbdev->dev, "CLEAR_FEATURE request to remove STALL condition.\n");
-		if(usb_clear_halt(s->usbdev, usb_pipeendpoint(pipe)))
-			dev_err(&s->usbdev->dev, "request failed\n");
-	}
-
-	pb->size = actual_length;
-	return ret;
-}
-/* --------------------------------------------------------------------- */
-static int dabusb_writemem (pdabusb_t s, int pos, const unsigned char *data,
-			    int len)
-{
-	int ret;
-	unsigned char *transfer_buffer =  kmalloc (len, GFP_KERNEL);
-
-	if (!transfer_buffer) {
-		dev_err(&s->usbdev->dev,
-			"dabusb_writemem: kmalloc(%d) failed.\n", len);
-		return -ENOMEM;
-	}
-
-	memcpy (transfer_buffer, data, len);
-
-	ret=usb_control_msg(s->usbdev, usb_sndctrlpipe( s->usbdev, 0 ), 0xa0, 0x40, pos, 0, transfer_buffer, len, 300);
-
-	kfree (transfer_buffer);
-	return ret;
-}
-/* --------------------------------------------------------------------- */
-static int dabusb_8051_reset (pdabusb_t s, unsigned char reset_bit)
-{
-	dbg("dabusb_8051_reset: %d",reset_bit);
-	return dabusb_writemem (s, CPUCS_REG, &reset_bit, 1);
-}
-/* --------------------------------------------------------------------- */
-static int dabusb_loadmem (pdabusb_t s, const char *fname)
-{
-	int ret;
-	const struct ihex_binrec *rec;
-	const struct firmware *uninitialized_var(fw);
-
-	dbg("Enter dabusb_loadmem (internal)");
-
-	ret = request_ihex_firmware(&fw, "dabusb/firmware.fw", &s->usbdev->dev);
-	if (ret) {
-		dev_err(&s->usbdev->dev,
-			"Failed to load \"dabusb/firmware.fw\": %d\n", ret);
-		goto out;
-	}
-	ret = dabusb_8051_reset (s, 1);
-
-	for (rec = (const struct ihex_binrec *)fw->data; rec;
-	     rec = ihex_next_binrec(rec)) {
-		dbg("dabusb_writemem: %04X %p %d)", be32_to_cpu(rec->addr),
-		    rec->data, be16_to_cpu(rec->len));
-
-		ret = dabusb_writemem(s, be32_to_cpu(rec->addr), rec->data,
-				       be16_to_cpu(rec->len));
-		if (ret < 0) {
-			dev_err(&s->usbdev->dev,
-				"dabusb_writemem failed (%d %04X %p %d)\n",
-				ret, be32_to_cpu(rec->addr),
-				rec->data, be16_to_cpu(rec->len));
-			break;
-		}
-	}
-	ret = dabusb_8051_reset (s, 0);
-	release_firmware(fw);
- out:
-	dbg("dabusb_loadmem: exit");
-
-	return ret;
-}
-/* --------------------------------------------------------------------- */
-static int dabusb_fpga_clear (pdabusb_t s, pbulk_transfer_t b)
-{
-	b->size = 4;
-	b->data[0] = 0x2a;
-	b->data[1] = 0;
-	b->data[2] = 0;
-	b->data[3] = 0;
-
-	dbg("dabusb_fpga_clear");
-
-	return dabusb_bulk (s, b);
-}
-/* --------------------------------------------------------------------- */
-static int dabusb_fpga_init (pdabusb_t s, pbulk_transfer_t b)
-{
-	b->size = 4;
-	b->data[0] = 0x2c;
-	b->data[1] = 0;
-	b->data[2] = 0;
-	b->data[3] = 0;
-
-	dbg("dabusb_fpga_init");
-
-	return dabusb_bulk (s, b);
-}
-/* --------------------------------------------------------------------- */
-static int dabusb_fpga_download (pdabusb_t s, const char *fname)
-{
-	pbulk_transfer_t b = kmalloc (sizeof (bulk_transfer_t), GFP_KERNEL);
-	const struct firmware *fw;
-	unsigned int blen, n;
-	int ret;
-
-	dbg("Enter dabusb_fpga_download (internal)");
-
-	if (!b) {
-		dev_err(&s->usbdev->dev,
-			"kmalloc(sizeof(bulk_transfer_t))==NULL\n");
-		return -ENOMEM;
-	}
-
-	ret = request_firmware(&fw, "dabusb/bitstream.bin", &s->usbdev->dev);
-	if (ret) {
-		dev_err(&s->usbdev->dev,
-			"Failed to load \"dabusb/bitstream.bin\": %d\n", ret);
-		kfree(b);
-		return ret;
-	}
-
-	b->pipe = 1;
-	ret = dabusb_fpga_clear (s, b);
-	mdelay (10);
-	blen = fw->data[73] + (fw->data[72] << 8);
-
-	dbg("Bitstream len: %i", blen);
-
-	b->data[0] = 0x2b;
-	b->data[1] = 0;
-	b->data[2] = 0;
-	b->data[3] = 60;
-
-	for (n = 0; n <= blen + 60; n += 60) {
-		// some cclks for startup
-		b->size = 64;
-		memcpy (b->data + 4, fw->data + 74 + n, 60);
-		ret = dabusb_bulk (s, b);
-		if (ret < 0) {
-			dev_err(&s->usbdev->dev, "dabusb_bulk failed.\n");
-			break;
-		}
-		mdelay (1);
-	}
-
-	ret = dabusb_fpga_init (s, b);
-	kfree (b);
-	release_firmware(fw);
-
-	dbg("exit dabusb_fpga_download");
-
-	return ret;
-}
-
-static int dabusb_stop (pdabusb_t s)
-{
-	dbg("dabusb_stop");
-
-	s->state = _stopped;
-	dabusb_cancel_queue (s, &s->rec_buff_list);
-
-	dbg("pending_io: %d", s->pending_io.counter);
-
-	s->pending_io.counter = 0;
-	return 0;
-}
-
-static int dabusb_startrek (pdabusb_t s)
-{
-	if (!s->got_mem && s->state != _started) {
-
-		dbg("dabusb_startrek");
-
-		if (dabusb_alloc_buffers (s) < 0)
-			return -ENOMEM;
-		dabusb_stop (s);
-		s->state = _started;
-		s->readptr = 0;
-	}
-
-	if (!list_empty (&s->free_buff_list)) {
-		pbuff_t end;
-		int ret;
-
-	while (!dabusb_add_buf_tail (s, &s->rec_buff_list, &s->free_buff_list)) {
-
-			dbg("submitting: end:%p s->rec_buff_list:%p", s->rec_buff_list.prev, &s->rec_buff_list);
-
-			end = list_entry (s->rec_buff_list.prev, buff_t, buff_list);
-
-			ret = usb_submit_urb (end->purb, GFP_KERNEL);
-			if (ret) {
-				dev_err(&s->usbdev->dev,
-					"usb_submit_urb returned:%d\n", ret);
-				if (dabusb_add_buf_tail (s, &s->free_buff_list, &s->rec_buff_list))
-					dev_err(&s->usbdev->dev,
-						"startrek: dabusb_add_buf_tail failed\n");
-				break;
-			}
-			else
-				atomic_inc (&s->pending_io);
-		}
-		dbg("pending_io: %d",s->pending_io.counter);
-	}
-
-	return 0;
-}
-
-static ssize_t dabusb_read (struct file *file, char __user *buf, size_t count, loff_t * ppos)
-{
-	pdabusb_t s = (pdabusb_t) file->private_data;
-	unsigned long flags;
-	unsigned ret = 0;
-	int rem;
-	int cnt;
-	pbuff_t b;
-	struct urb *purb = NULL;
-
-	dbg("dabusb_read");
-
-	if (*ppos)
-		return -ESPIPE;
-
-	if (s->remove_pending)
-		return -EIO;
-
-
-	if (!s->usbdev)
-		return -EIO;
-
-	while (count > 0) {
-		dabusb_startrek (s);
-
-		spin_lock_irqsave (&s->lock, flags);
-
-		if (list_empty (&s->rec_buff_list)) {
-
-			spin_unlock_irqrestore(&s->lock, flags);
-
-			dev_err(&s->usbdev->dev,
-				"error: rec_buf_list is empty\n");
-			goto err;
-		}
-
-		b = list_entry (s->rec_buff_list.next, buff_t, buff_list);
-		purb = b->purb;
-
-		spin_unlock_irqrestore(&s->lock, flags);
-
-		if (purb->status == -EINPROGRESS) {
-			if (file->f_flags & O_NONBLOCK)		// return nonblocking
-			 {
-				if (!ret)
-					ret = -EAGAIN;
-				goto err;
-			}
-
-			interruptible_sleep_on (&s->wait);
-
-			if (signal_pending (current)) {
-				if (!ret)
-					ret = -ERESTARTSYS;
-				goto err;
-			}
-
-			spin_lock_irqsave (&s->lock, flags);
-
-			if (list_empty (&s->rec_buff_list)) {
-				spin_unlock_irqrestore(&s->lock, flags);
-				dev_err(&s->usbdev->dev,
-					"error: still no buffer available.\n");
-				goto err;
-			}
-			spin_unlock_irqrestore(&s->lock, flags);
-			s->readptr = 0;
-		}
-		if (s->remove_pending) {
-			ret = -EIO;
-			goto err;
-		}
-
-		rem = purb->actual_length - s->readptr;		// set remaining bytes to copy
-
-		if (count >= rem)
-			cnt = rem;
-		else
-			cnt = count;
-
-		dbg("copy_to_user:%p %p %d",buf, purb->transfer_buffer + s->readptr, cnt);
-
-		if (copy_to_user (buf, purb->transfer_buffer + s->readptr, cnt)) {
-			dev_err(&s->usbdev->dev, "read: copy_to_user failed\n");
-			if (!ret)
-				ret = -EFAULT;
-			goto err;
-		}
-
-		s->readptr += cnt;
-		count -= cnt;
-		buf += cnt;
-		ret += cnt;
-
-		if (s->readptr == purb->actual_length) {
-			// finished, take next buffer
-			if (dabusb_add_buf_tail (s, &s->free_buff_list, &s->rec_buff_list))
-				dev_err(&s->usbdev->dev,
-					"read: dabusb_add_buf_tail failed\n");
-			s->readptr = 0;
-		}
-	}
-      err:			//mutex_unlock(&s->mutex);
-	return ret;
-}
-
-static int dabusb_open (struct inode *inode, struct file *file)
-{
-	int devnum = iminor(inode);
-	pdabusb_t s;
-	int r;
-
-	if (devnum < DABUSB_MINOR || devnum >= (DABUSB_MINOR + NRDABUSB))
-		return -EIO;
-
-	s = &dabusb[devnum - DABUSB_MINOR];
-
-	dbg("dabusb_open");
-	mutex_lock(&s->mutex);
-
-	while (!s->usbdev || s->opened) {
-		mutex_unlock(&s->mutex);
-
-		if (file->f_flags & O_NONBLOCK)
-			return -EBUSY;
-		msleep_interruptible(500);
-
-		if (signal_pending (current))
-			return -EAGAIN;
-		mutex_lock(&s->mutex);
-	}
-	if (usb_set_interface (s->usbdev, _DABUSB_IF, 1) < 0) {
-		mutex_unlock(&s->mutex);
-		dev_err(&s->usbdev->dev, "set_interface failed\n");
-		return -EINVAL;
-	}
-	s->opened = 1;
-	mutex_unlock(&s->mutex);
-
-	file->f_pos = 0;
-	file->private_data = s;
-
-	r = nonseekable_open(inode, file);
-	return r;
-}
-
-static int dabusb_release (struct inode *inode, struct file *file)
-{
-	pdabusb_t s = (pdabusb_t) file->private_data;
-
-	dbg("dabusb_release");
-
-	mutex_lock(&s->mutex);
-	dabusb_stop (s);
-	dabusb_free_buffers (s);
-	mutex_unlock(&s->mutex);
-
-	if (!s->remove_pending) {
-		if (usb_set_interface (s->usbdev, _DABUSB_IF, 0) < 0)
-			dev_err(&s->usbdev->dev, "set_interface failed\n");
-	}
-	else
-		wake_up (&s->remove_ok);
-
-	s->opened = 0;
-	return 0;
-}
-
-static long dabusb_ioctl (struct file *file, unsigned int cmd, unsigned long arg)
-{
-	pdabusb_t s = (pdabusb_t) file->private_data;
-	pbulk_transfer_t pbulk;
-	int ret = 0;
-	int version = DABUSB_VERSION;
-
-	dbg("dabusb_ioctl");
-
-	if (s->remove_pending)
-		return -EIO;
-
-	mutex_lock(&s->mutex);
-
-	if (!s->usbdev) {
-		mutex_unlock(&s->mutex);
-		return -EIO;
-	}
-
-	switch (cmd) {
-
-	case IOCTL_DAB_BULK:
-		pbulk = memdup_user((void __user *)arg,
-				    sizeof(bulk_transfer_t));
-
-		if (IS_ERR(pbulk)) {
-			ret = PTR_ERR(pbulk);
-			break;
-		}
-
-		ret=dabusb_bulk (s, pbulk);
-		if(ret==0)
-			if (copy_to_user((void __user *)arg, pbulk,
-					 sizeof(bulk_transfer_t)))
-				ret = -EFAULT;
-		kfree (pbulk);
-		break;
-
-	case IOCTL_DAB_OVERRUNS:
-		ret = put_user (s->overruns, (unsigned int __user *) arg);
-		break;
-
-	case IOCTL_DAB_VERSION:
-		ret = put_user (version, (unsigned int __user *) arg);
-		break;
-
-	default:
-		ret = -ENOIOCTLCMD;
-		break;
-	}
-	mutex_unlock(&s->mutex);
-	return ret;
-}
-
-static const struct file_operations dabusb_fops =
-{
-	.owner =	THIS_MODULE,
-	.llseek =	no_llseek,
-	.read =		dabusb_read,
-	.unlocked_ioctl =	dabusb_ioctl,
-	.open =		dabusb_open,
-	.release =	dabusb_release,
-};
-
-static char *dabusb_devnode(struct device *dev, mode_t *mode)
-{
-	return kasprintf(GFP_KERNEL, "usb/%s", dev_name(dev));
-}
-
-static struct usb_class_driver dabusb_class = {
-	.name =		"dabusb%d",
-	.devnode =	dabusb_devnode,
-	.fops =		&dabusb_fops,
-	.minor_base =	DABUSB_MINOR,
-};
-
-
-/* --------------------------------------------------------------------- */
-static int dabusb_probe (struct usb_interface *intf,
-			 const struct usb_device_id *id)
-{
-	struct usb_device *usbdev = interface_to_usbdev(intf);
-	int retval;
-	pdabusb_t s;
-
-	dbg("dabusb: probe: vendor id 0x%x, device id 0x%x ifnum:%d",
-	    le16_to_cpu(usbdev->descriptor.idVendor),
-	    le16_to_cpu(usbdev->descriptor.idProduct),
-	    intf->altsetting->desc.bInterfaceNumber);
-
-	/* We don't handle multiple configurations */
-	if (usbdev->descriptor.bNumConfigurations != 1)
-		return -ENODEV;
-
-	if (intf->altsetting->desc.bInterfaceNumber != _DABUSB_IF &&
-	    le16_to_cpu(usbdev->descriptor.idProduct) == 0x9999)
-		return -ENODEV;
-
-
-
-	s = &dabusb[intf->minor];
-
-	mutex_lock(&s->mutex);
-	s->remove_pending = 0;
-	s->usbdev = usbdev;
-	s->devnum = intf->minor;
-
-	if (usb_reset_configuration (usbdev) < 0) {
-		dev_err(&intf->dev, "reset_configuration failed\n");
-		goto reject;
-	}
-	if (le16_to_cpu(usbdev->descriptor.idProduct) == 0x2131) {
-		dabusb_loadmem (s, NULL);
-		goto reject;
-	}
-	else {
-		dabusb_fpga_download (s, NULL);
-
-		if (usb_set_interface (s->usbdev, _DABUSB_IF, 0) < 0) {
-			dev_err(&intf->dev, "set_interface failed\n");
-			goto reject;
-		}
-	}
-	dbg("bound to interface: %d", intf->altsetting->desc.bInterfaceNumber);
-	usb_set_intfdata (intf, s);
-	mutex_unlock(&s->mutex);
-
-	retval = usb_register_dev(intf, &dabusb_class);
-	if (retval) {
-		usb_set_intfdata (intf, NULL);
-		return -ENOMEM;
-	}
-
-	return 0;
-
-      reject:
-	mutex_unlock(&s->mutex);
-	s->usbdev = NULL;
-	return -ENODEV;
-}
-
-static void dabusb_disconnect (struct usb_interface *intf)
-{
-	wait_queue_t __wait;
-	pdabusb_t s = usb_get_intfdata (intf);
-
-	dbg("dabusb_disconnect");
-
-	init_waitqueue_entry(&__wait, current);
-
-	usb_set_intfdata (intf, NULL);
-	if (s) {
-		usb_deregister_dev (intf, &dabusb_class);
-		s->remove_pending = 1;
-		wake_up (&s->wait);
-		add_wait_queue(&s->remove_ok, &__wait);
-		set_current_state(TASK_UNINTERRUPTIBLE);
-		if (s->state == _started)
-			schedule();
-		current->state = TASK_RUNNING;
-		remove_wait_queue(&s->remove_ok, &__wait);
-
-		s->usbdev = NULL;
-		s->overruns = 0;
-	}
-}
-
-static struct usb_device_id dabusb_ids [] = {
-	// { USB_DEVICE(0x0547, 0x2131) },	/* An2131 chip, no boot ROM */
-	{ USB_DEVICE(0x0547, 0x9999) },
-	{ }						/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE (usb, dabusb_ids);
-
-static struct usb_driver dabusb_driver = {
-	.name =		"dabusb",
-	.probe =	dabusb_probe,
-	.disconnect =	dabusb_disconnect,
-	.id_table =	dabusb_ids,
-};
-
-/* --------------------------------------------------------------------- */
-
-static int __init dabusb_init (void)
-{
-	int retval;
-	unsigned u;
-
-	/* initialize struct */
-	for (u = 0; u < NRDABUSB; u++) {
-		pdabusb_t s = &dabusb[u];
-		memset (s, 0, sizeof (dabusb_t));
-		mutex_init (&s->mutex);
-		s->usbdev = NULL;
-		s->total_buffer_size = buffers;
-		init_waitqueue_head (&s->wait);
-		init_waitqueue_head (&s->remove_ok);
-		spin_lock_init (&s->lock);
-		INIT_LIST_HEAD (&s->free_buff_list);
-		INIT_LIST_HEAD (&s->rec_buff_list);
-	}
-
-	/* register misc device */
-	retval = usb_register(&dabusb_driver);
-	if (retval)
-		goto out;
-
-	dbg("dabusb_init: driver registered");
-
-	printk(KERN_INFO KBUILD_MODNAME ": " DRIVER_VERSION ":"
-	       DRIVER_DESC "\n");
-
-out:
-	return retval;
-}
-
-static void __exit dabusb_cleanup (void)
-{
-	dbg("dabusb_cleanup");
-
-	usb_deregister (&dabusb_driver);
-}
-
-/* --------------------------------------------------------------------- */
-
-MODULE_AUTHOR( DRIVER_AUTHOR );
-MODULE_DESCRIPTION( DRIVER_DESC );
-MODULE_LICENSE("GPL");
-MODULE_FIRMWARE("dabusb/firmware.fw");
-MODULE_FIRMWARE("dabusb/bitstream.bin");
-
-module_param(buffers, int, 0);
-MODULE_PARM_DESC (buffers, "Number of buffers (default=256)");
-
-module_init (dabusb_init);
-module_exit (dabusb_cleanup);
-
-/* --------------------------------------------------------------------- */
diff --git a/drivers/staging/dabusb/dabusb.h b/drivers/staging/dabusb/dabusb.h
deleted file mode 100644
index 00eb34c863eb..000000000000
--- a/drivers/staging/dabusb/dabusb.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#define _BULK_DATA_LEN 64
-typedef struct
-{
-	unsigned char data[_BULK_DATA_LEN];
-	unsigned int size;
-	unsigned int pipe;
-}bulk_transfer_t,*pbulk_transfer_t;
-
-#define DABUSB_MINOR 240		/* some unassigned USB minor */
-#define DABUSB_VERSION 0x1000
-#define IOCTL_DAB_BULK              _IOWR('d', 0x30, bulk_transfer_t)
-#define IOCTL_DAB_OVERRUNS	    _IOR('d',  0x15, int)
-#define IOCTL_DAB_VERSION           _IOR('d', 0x3f, int)
-
-#ifdef __KERNEL__
-
-typedef enum { _stopped=0, _started } driver_state_t;
-
-typedef struct
-{
-	struct mutex mutex;
-	struct usb_device *usbdev;
-	wait_queue_head_t wait;
-	wait_queue_head_t remove_ok;
-	spinlock_t lock;
-	atomic_t pending_io;
-	driver_state_t state;
-	int remove_pending;
-	int got_mem;
-	int total_buffer_size;
-	unsigned int overruns;
-	int readptr;
-	int opened;
-	int devnum;
-	struct list_head free_buff_list;
-	struct list_head rec_buff_list;
-} dabusb_t,*pdabusb_t;
-
-typedef struct
-{
-	pdabusb_t s;
-	struct urb *purb;
-	struct list_head buff_list;
-} buff_t,*pbuff_t;
-
-typedef struct
-{
-	wait_queue_head_t wait;
-} bulk_completion_context_t, *pbulk_completion_context_t;
-
-
-#define _DABUSB_IF 2
-#define _DABUSB_ISOPIPE 0x09
-#define _ISOPIPESIZE	16384
-
-#define _BULK_DATA_LEN 64
-// Vendor specific request code for Anchor Upload/Download
-// This one is implemented in the core
-#define ANCHOR_LOAD_INTERNAL  0xA0
-
-// EZ-USB Control and Status Register.  Bit 0 controls 8051 reset
-#define CPUCS_REG    0x7F92
-#define _TOTAL_BUFFERS 384
-
-#define MAX_INTEL_HEX_RECORD_LENGTH 16
-
-#ifndef _BYTE_DEFINED
-#define _BYTE_DEFINED
-typedef unsigned char BYTE;
-#endif // !_BYTE_DEFINED
-
-#ifndef _WORD_DEFINED
-#define _WORD_DEFINED
-typedef unsigned short WORD;
-#endif // !_WORD_DEFINED
-
-typedef struct _INTEL_HEX_RECORD
-{
-   BYTE  Length;
-   WORD  Address;
-   BYTE  Type;
-   BYTE  Data[MAX_INTEL_HEX_RECORD_LENGTH];
-} INTEL_HEX_RECORD, *PINTEL_HEX_RECORD;
-
-#endif
diff --git a/drivers/staging/easycap/easycap_ioctl.c b/drivers/staging/easycap/easycap_ioctl.c
index 447953a4e80c..7ac43da4e252 100644
--- a/drivers/staging/easycap/easycap_ioctl.c
+++ b/drivers/staging/easycap/easycap_ioctl.c
@@ -1399,11 +1399,6 @@ case VIDIOC_G_CTRL: {
 	break;
 }
 /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
-#if defined(VIDIOC_S_CTRL_OLD)
-case VIDIOC_S_CTRL_OLD: {
-	JOM(8, "VIDIOC_S_CTRL_OLD required at least for xawtv\n");
-}
-#endif /*VIDIOC_S_CTRL_OLD*/
 case VIDIOC_S_CTRL:
 	{
 	struct v4l2_control v4l2_control;
diff --git a/drivers/staging/pohmelfs/config.c b/drivers/staging/pohmelfs/config.c
index 89279ba1b737..39413b7d387d 100644
--- a/drivers/staging/pohmelfs/config.c
+++ b/drivers/staging/pohmelfs/config.c
@@ -525,7 +525,7 @@ static void pohmelfs_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *n
 {
 	int err;
 
-	if (!cap_raised(nsp->eff_cap, CAP_SYS_ADMIN))
+	if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
 		return;
 
 	switch (msg->flags) {
diff --git a/drivers/staging/se401/Kconfig b/drivers/staging/se401/Kconfig
deleted file mode 100644
index b7f8222ad21b..000000000000
--- a/drivers/staging/se401/Kconfig
+++ /dev/null
@@ -1,13 +0,0 @@
-config USB_SE401
-	tristate "USB SE401 Camera support (DEPRECATED)"
-	depends on VIDEO_DEV && VIDEO_V4L2_COMMON && USB
-	---help---
-	  Say Y here if you want to connect this type of camera to your
-	  computer's USB port. See <file:Documentation/video4linux/se401.txt>
-	  for more information and for a list of supported cameras.
-
-	  This driver uses the deprecated V4L1 API and will be removed in
-	  2.6.39, unless someone converts it to the V4L2 API.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called se401.
diff --git a/drivers/staging/se401/Makefile b/drivers/staging/se401/Makefile
deleted file mode 100644
index b465d49783af..000000000000
--- a/drivers/staging/se401/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_USB_SE401)         += se401.o
diff --git a/drivers/staging/se401/TODO b/drivers/staging/se401/TODO
deleted file mode 100644
index 3b2c03836286..000000000000
--- a/drivers/staging/se401/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-This is an obsolete driver for some old webcams that still use V4L1 API. 
-As V4L1 support is being removed from kernel, if nobody take care on it, 
-the driver will be removed for 2.6.39.
-
-Please send patches to linux-media@vger.kernel.org
diff --git a/drivers/staging/se401/se401.c b/drivers/staging/se401/se401.c
deleted file mode 100644
index 41360d7c3e96..000000000000
--- a/drivers/staging/se401/se401.c
+++ /dev/null
@@ -1,1492 +0,0 @@
-/*
- * Endpoints (formerly known as AOX) se401 USB Camera Driver
- *
- * Copyright (c) 2000 Jeroen B. Vreeken (pe1rxq@amsat.org)
- *
- * Still somewhat based on the Linux ov511 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.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- * Thanks to Endpoints Inc. (www.endpoints.com) for making documentation on
- * their chipset available and supporting me while writing this driver.
- * 	- Jeroen Vreeken
- */
-
-static const char version[] = "0.24";
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/vmalloc.h>
-#include <linux/slab.h>
-#include <linux/pagemap.h>
-#include <linux/usb.h>
-#include "se401.h"
-
-static int flickerless;
-static int video_nr = -1;
-
-static struct usb_device_id device_table[] = {
-	{ USB_DEVICE(0x03e8, 0x0004) },/* Endpoints/Aox SE401 */
-	{ USB_DEVICE(0x0471, 0x030b) },/* Philips PCVC665K */
-	{ USB_DEVICE(0x047d, 0x5001) },/* Kensington 67014 */
-	{ USB_DEVICE(0x047d, 0x5002) },/* Kensington 6701(5/7) */
-	{ USB_DEVICE(0x047d, 0x5003) },/* Kensington 67016 */
-	{ }
-};
-
-MODULE_DEVICE_TABLE(usb, device_table);
-
-MODULE_AUTHOR("Jeroen Vreeken <pe1rxq@amsat.org>");
-MODULE_DESCRIPTION("SE401 USB Camera Driver");
-MODULE_LICENSE("GPL");
-module_param(flickerless, int, 0);
-MODULE_PARM_DESC(flickerless,
-		"Net frequency to adjust exposure time to (0/50/60)");
-module_param(video_nr, int, 0);
-
-static struct usb_driver se401_driver;
-
-
-/**********************************************************************
- *
- * Memory management
- *
- **********************************************************************/
-static void *rvmalloc(unsigned long size)
-{
-	void *mem;
-	unsigned long adr;
-
-	size = PAGE_ALIGN(size);
-	mem = vmalloc_32(size);
-	if (!mem)
-		return NULL;
-
-	memset(mem, 0, size); /* Clear the ram out, no junk to the user */
-	adr = (unsigned long) mem;
-	while (size > 0) {
-		SetPageReserved(vmalloc_to_page((void *)adr));
-		adr +=  PAGE_SIZE;
-		size -=  PAGE_SIZE;
-	}
-
-	return mem;
-}
-
-static void rvfree(void *mem, unsigned long size)
-{
-	unsigned long adr;
-
-	if (!mem)
-		return;
-
-	adr = (unsigned long) mem;
-	while ((long) size > 0) {
-		ClearPageReserved(vmalloc_to_page((void *)adr));
-		adr +=  PAGE_SIZE;
-		size -=  PAGE_SIZE;
-	}
-	vfree(mem);
-}
-
-
-
-/****************************************************************************
- *
- * se401 register read/write functions
- *
- ***************************************************************************/
-
-static int se401_sndctrl(int set, struct usb_se401 *se401, unsigned short req,
-			 unsigned short value, unsigned char *cp, int size)
-{
-	return usb_control_msg(
-		se401->dev,
-		set ? usb_sndctrlpipe(se401->dev, 0) : usb_rcvctrlpipe(se401->dev, 0),
-		req,
-		(set ? USB_DIR_OUT : USB_DIR_IN) | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		value,
-		0,
-		cp,
-		size,
-		1000
-	);
-}
-
-static int se401_set_feature(struct usb_se401 *se401, unsigned short selector,
-			     unsigned short param)
-{
-	/* specs say that the selector (address) should go in the value field
-	   and the param in index, but in the logs of the windows driver they do
-	   this the other way around...
-	 */
-	return usb_control_msg(
-		se401->dev,
-		usb_sndctrlpipe(se401->dev, 0),
-		SE401_REQ_SET_EXT_FEATURE,
-		USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		param,
-		selector,
-		NULL,
-		0,
-		1000
-	);
-}
-
-static unsigned short se401_get_feature(struct usb_se401 *se401,
-					unsigned short selector)
-{
-	/* For 'set' the selecetor should be in index, not sure if the spec is
-	   wrong here to....
-	 */
-	unsigned char cp[2];
-	usb_control_msg(
-		se401->dev,
-		usb_rcvctrlpipe(se401->dev, 0),
-		SE401_REQ_GET_EXT_FEATURE,
-		USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		0,
-		selector,
-		cp,
-		2,
-		1000
-	);
-	return cp[0]+cp[1]*256;
-}
-
-/****************************************************************************
- *
- * Camera control
- *
- ***************************************************************************/
-
-
-static int se401_send_pict(struct usb_se401 *se401)
-{
-	/* integration time low */
-	se401_set_feature(se401, HV7131_REG_TITL, se401->expose_l);
-	/* integration time mid */
-	se401_set_feature(se401, HV7131_REG_TITM, se401->expose_m);
-	/* integration time mid */
-	se401_set_feature(se401, HV7131_REG_TITU, se401->expose_h);
-	/* reset level value */
-	se401_set_feature(se401, HV7131_REG_ARLV, se401->resetlevel);
-	/* red color gain */
-	se401_set_feature(se401, HV7131_REG_ARCG, se401->rgain);
-	/* green color gain */
-	se401_set_feature(se401, HV7131_REG_AGCG, se401->ggain);
-	/* blue color gain */
-	se401_set_feature(se401, HV7131_REG_ABCG, se401->bgain);
-
-	return 0;
-}
-
-static void se401_set_exposure(struct usb_se401 *se401, int brightness)
-{
-	int integration = brightness << 5;
-
-	if (flickerless == 50)
-		integration = integration-integration % 106667;
-	if (flickerless == 60)
-		integration = integration-integration % 88889;
-	se401->brightness = integration >> 5;
-	se401->expose_h = (integration >> 16) & 0xff;
-	se401->expose_m = (integration >> 8) & 0xff;
-	se401->expose_l = integration & 0xff;
-}
-
-static int se401_get_pict(struct usb_se401 *se401, struct video_picture *p)
-{
-	p->brightness = se401->brightness;
-	if (se401->enhance)
-		p->whiteness = 32768;
-	else
-		p->whiteness = 0;
-
-	p->colour = 65535;
-	p->contrast = 65535;
-	p->hue = se401->rgain << 10;
-	p->palette = se401->palette;
-	p->depth = 3; /* rgb24 */
-	return 0;
-}
-
-
-static int se401_set_pict(struct usb_se401 *se401, struct video_picture *p)
-{
-	if (p->palette != VIDEO_PALETTE_RGB24)
-		return 1;
-	se401->palette = p->palette;
-	if (p->hue != se401->hue) {
-		se401->rgain =  p->hue >> 10;
-		se401->bgain =  0x40-(p->hue >> 10);
-		se401->hue = p->hue;
-	}
-	if (p->brightness != se401->brightness)
-		se401_set_exposure(se401, p->brightness);
-
-	if (p->whiteness >= 32768)
-		se401->enhance = 1;
-	else
-		se401->enhance = 0;
-	se401_send_pict(se401);
-	se401_send_pict(se401);
-	return 0;
-}
-
-/*
-	Hyundai have some really nice docs about this and other sensor related
-	stuff on their homepage: www.hei.co.kr
-*/
-static void se401_auto_resetlevel(struct usb_se401 *se401)
-{
-	unsigned int ahrc, alrc;
-	int oldreset = se401->resetlevel;
-
-	/* For some reason this normally read-only register doesn't get reset
-	   to zero after reading them just once...
-	 */
-	se401_get_feature(se401, HV7131_REG_HIREFNOH);
-	se401_get_feature(se401, HV7131_REG_HIREFNOL);
-	se401_get_feature(se401, HV7131_REG_LOREFNOH);
-	se401_get_feature(se401, HV7131_REG_LOREFNOL);
-	ahrc = 256*se401_get_feature(se401, HV7131_REG_HIREFNOH) +
-	    se401_get_feature(se401, HV7131_REG_HIREFNOL);
-	alrc = 256*se401_get_feature(se401, HV7131_REG_LOREFNOH) +
-	    se401_get_feature(se401, HV7131_REG_LOREFNOL);
-
-	/* Not an exact science, but it seems to work pretty well... */
-	if (alrc > 10) {
-		while (alrc >= 10 && se401->resetlevel < 63) {
-			se401->resetlevel++;
-			alrc /= 2;
-		}
-	} else if (ahrc > 20) {
-		while (ahrc >= 20 && se401->resetlevel > 0) {
-			se401->resetlevel--;
-			ahrc /= 2;
-		}
-	}
-	if (se401->resetlevel != oldreset)
-		se401_set_feature(se401, HV7131_REG_ARLV, se401->resetlevel);
-
-	return;
-}
-
-/* irq handler for snapshot button */
-static void se401_button_irq(struct urb *urb)
-{
-	struct usb_se401 *se401 = urb->context;
-	int status;
-
-	if (!se401->dev) {
-		dev_info(&urb->dev->dev, "device vapourished\n");
-		return;
-	}
-
-	switch (urb->status) {
-	case 0:
-		/* success */
-		break;
-	case -ECONNRESET:
-	case -ENOENT:
-	case -ESHUTDOWN:
-		/* this urb is terminated, clean up */
-		dbg("%s - urb shutting down with status: %d",
-							__func__, urb->status);
-		return;
-	default:
-		dbg("%s - nonzero urb status received: %d",
-							__func__, urb->status);
-		goto exit;
-	}
-
-	if (urb->actual_length  >= 2)
-		if (se401->button)
-			se401->buttonpressed = 1;
-exit:
-	status = usb_submit_urb(urb, GFP_ATOMIC);
-	if (status)
-		err("%s - usb_submit_urb failed with result %d",
-		     __func__, status);
-}
-
-static void se401_video_irq(struct urb *urb)
-{
-	struct usb_se401 *se401 = urb->context;
-	int length = urb->actual_length;
-
-	/* ohoh... */
-	if (!se401->streaming)
-		return;
-
-	if (!se401->dev) {
-		dev_info(&urb->dev->dev, "device vapourished\n");
-		return;
-	}
-
-	/* 0 sized packets happen if we are to fast, but sometimes the camera
-	   keeps sending them forever...
-	 */
-	if (length && !urb->status) {
-		se401->nullpackets = 0;
-		switch (se401->scratch[se401->scratch_next].state) {
-		case BUFFER_READY:
-		case BUFFER_BUSY:
-			se401->dropped++;
-			break;
-		case BUFFER_UNUSED:
-			memcpy(se401->scratch[se401->scratch_next].data,
-				(unsigned char *)urb->transfer_buffer, length);
-			se401->scratch[se401->scratch_next].state
-							= BUFFER_READY;
-			se401->scratch[se401->scratch_next].offset
-							= se401->bayeroffset;
-			se401->scratch[se401->scratch_next].length = length;
-			if (waitqueue_active(&se401->wq))
-				wake_up_interruptible(&se401->wq);
-			se401->scratch_overflow = 0;
-			se401->scratch_next++;
-			if (se401->scratch_next >= SE401_NUMSCRATCH)
-				se401->scratch_next = 0;
-			break;
-		}
-		se401->bayeroffset += length;
-		if (se401->bayeroffset >= se401->cheight * se401->cwidth)
-			se401->bayeroffset = 0;
-	} else {
-		se401->nullpackets++;
-		if (se401->nullpackets > SE401_MAX_NULLPACKETS)
-			if (waitqueue_active(&se401->wq))
-				wake_up_interruptible(&se401->wq);
-	}
-
-	/* Resubmit urb for new data */
-	urb->status = 0;
-	urb->dev = se401->dev;
-	if (usb_submit_urb(urb, GFP_KERNEL))
-		dev_info(&urb->dev->dev, "urb burned down\n");
-	return;
-}
-
-static void se401_send_size(struct usb_se401 *se401, int width, int height)
-{
-	int i = 0;
-	int mode = 0x03; /* No compression */
-	int sendheight = height;
-	int sendwidth = width;
-
-	/* JangGu compression can only be used with the camera supported sizes,
-	   but bayer seems to work with any size that fits on the sensor.
-	   We check if we can use compression with the current size with either
-	   4 or 16 times subcapturing, if not we use uncompressed bayer data
-	   but this will result in cutouts of the maximum size....
-	 */
-	while (i < se401->sizes && !(se401->width[i] == width &&
-						se401->height[i] == height))
-		i++;
-	while (i < se401->sizes) {
-		if (se401->width[i] == width * 2 &&
-				se401->height[i] == height * 2) {
-			sendheight = se401->height[i];
-			sendwidth = se401->width[i];
-			mode = 0x40;
-		}
-		if (se401->width[i] == width * 4 &&
-				se401->height[i] == height * 4) {
-			sendheight = se401->height[i];
-			sendwidth = se401->width[i];
-			mode = 0x42;
-		}
-		i++;
-	}
-
-	se401_sndctrl(1, se401, SE401_REQ_SET_WIDTH, sendwidth, NULL, 0);
-	se401_sndctrl(1, se401, SE401_REQ_SET_HEIGHT, sendheight, NULL, 0);
-	se401_set_feature(se401, SE401_OPERATINGMODE, mode);
-
-	if (mode == 0x03)
-		se401->format = FMT_BAYER;
-	else
-		se401->format = FMT_JANGGU;
-}
-
-/*
-	In this function se401_send_pict is called several times,
-	for some reason (depending on the state of the sensor and the phase of
-	the moon :) doing this only in either place doesn't always work...
-*/
-static int se401_start_stream(struct usb_se401 *se401)
-{
-	struct urb *urb;
-	int err = 0, i;
-	se401->streaming = 1;
-
-	se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 1, NULL, 0);
-	se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);
-
-	/* Set picture settings */
-	/* windowed + pix intg */
-	se401_set_feature(se401, HV7131_REG_MODE_B, 0x05);
-	se401_send_pict(se401);
-
-	se401_send_size(se401, se401->cwidth, se401->cheight);
-
-	se401_sndctrl(1, se401, SE401_REQ_START_CONTINUOUS_CAPTURE,
-								0, NULL, 0);
-
-	/* Do some memory allocation */
-	for (i = 0; i < SE401_NUMFRAMES; i++) {
-		se401->frame[i].data = se401->fbuf + i * se401->maxframesize;
-		se401->frame[i].curpix = 0;
-	}
-	for (i = 0; i < SE401_NUMSBUF; i++) {
-		se401->sbuf[i].data = kmalloc(SE401_PACKETSIZE, GFP_KERNEL);
-		if (!se401->sbuf[i].data) {
-			for (i = i - 1; i >= 0; i--) {
-				kfree(se401->sbuf[i].data);
-				se401->sbuf[i].data = NULL;
-			}
-			return -ENOMEM;
-		}
-	}
-
-	se401->bayeroffset = 0;
-	se401->scratch_next = 0;
-	se401->scratch_use = 0;
-	se401->scratch_overflow = 0;
-	for (i = 0; i < SE401_NUMSCRATCH; i++) {
-		se401->scratch[i].data = kmalloc(SE401_PACKETSIZE, GFP_KERNEL);
-		if (!se401->scratch[i].data) {
-			for (i = i - 1; i >= 0; i--) {
-				kfree(se401->scratch[i].data);
-				se401->scratch[i].data = NULL;
-			}
-			goto nomem_sbuf;
-		}
-		se401->scratch[i].state = BUFFER_UNUSED;
-	}
-
-	for (i = 0; i < SE401_NUMSBUF; i++) {
-		urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!urb) {
-			for (i = i - 1; i >= 0; i--) {
-				usb_kill_urb(se401->urb[i]);
-				usb_free_urb(se401->urb[i]);
-				se401->urb[i] = NULL;
-			}
-			goto nomem_scratch;
-		}
-
-		usb_fill_bulk_urb(urb, se401->dev,
-			usb_rcvbulkpipe(se401->dev, SE401_VIDEO_ENDPOINT),
-			se401->sbuf[i].data, SE401_PACKETSIZE,
-			se401_video_irq,
-			se401);
-
-		se401->urb[i] = urb;
-
-		err = usb_submit_urb(se401->urb[i], GFP_KERNEL);
-		if (err)
-			err("urb burned down");
-	}
-
-	se401->framecount = 0;
-
-	return 0;
-
- nomem_scratch:
-	for (i = 0; i < SE401_NUMSCRATCH; i++) {
-		kfree(se401->scratch[i].data);
-		se401->scratch[i].data = NULL;
-	}
- nomem_sbuf:
-	for (i = 0; i < SE401_NUMSBUF; i++) {
-		kfree(se401->sbuf[i].data);
-		se401->sbuf[i].data = NULL;
-	}
-	return -ENOMEM;
-}
-
-static int se401_stop_stream(struct usb_se401 *se401)
-{
-	int i;
-
-	if (!se401->streaming || !se401->dev)
-		return 1;
-
-	se401->streaming = 0;
-
-	se401_sndctrl(1, se401, SE401_REQ_STOP_CONTINUOUS_CAPTURE, 0, NULL, 0);
-
-	se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 0, NULL, 0);
-	se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 0, NULL, 0);
-
-	for (i = 0; i < SE401_NUMSBUF; i++)
-		if (se401->urb[i]) {
-			usb_kill_urb(se401->urb[i]);
-			usb_free_urb(se401->urb[i]);
-			se401->urb[i] = NULL;
-			kfree(se401->sbuf[i].data);
-		}
-	for (i = 0; i < SE401_NUMSCRATCH; i++) {
-		kfree(se401->scratch[i].data);
-		se401->scratch[i].data = NULL;
-	}
-
-	return 0;
-}
-
-static int se401_set_size(struct usb_se401 *se401, int width, int height)
-{
-	int wasstreaming = se401->streaming;
-	/* Check to see if we need to change */
-	if (se401->cwidth == width && se401->cheight == height)
-		return 0;
-
-	/* Check for a valid mode */
-	if (!width || !height)
-		return 1;
-	if ((width & 1) || (height & 1))
-		return 1;
-	if (width > se401->width[se401->sizes-1])
-		return 1;
-	if (height > se401->height[se401->sizes-1])
-		return 1;
-
-	/* Stop a current stream and start it again at the new size */
-	if (wasstreaming)
-		se401_stop_stream(se401);
-	se401->cwidth = width;
-	se401->cheight = height;
-	if (wasstreaming)
-		se401_start_stream(se401);
-	return 0;
-}
-
-
-/****************************************************************************
- *
- * Video Decoding
- *
- ***************************************************************************/
-
-/*
-	This shouldn't really be done in a v4l driver....
-	But it does make the image look a lot more usable.
-	Basically it lifts the dark pixels more than the light pixels.
-*/
-static inline void enhance_picture(unsigned char *frame, int len)
-{
-	while (len--) {
-		*frame = (((*frame^255)*(*frame^255))/255)^255;
-		frame++;
-	}
-}
-
-static inline void decode_JangGu_integrate(struct usb_se401 *se401, int data)
-{
-	struct se401_frame *frame = &se401->frame[se401->curframe];
-	int linelength = se401->cwidth * 3;
-
-	if (frame->curlinepix >= linelength) {
-		frame->curlinepix = 0;
-		frame->curline += linelength;
-	}
-
-	/* First three are absolute, all others relative.
-	 * Format is rgb from right to left (mirrorred image),
-	 * we flip it to get bgr from left to right. */
-	if (frame->curlinepix < 3)
-		*(frame->curline-frame->curlinepix) = 1 + data * 4;
-	else
-		*(frame->curline-frame->curlinepix) =
-		    *(frame->curline-frame->curlinepix + 3) + data * 4;
-	frame->curlinepix++;
-}
-
-static inline void decode_JangGu_vlc(struct usb_se401 *se401,
-			unsigned char *data, int bit_exp, int packetlength)
-{
-	int pos = 0;
-	int vlc_cod = 0;
-	int vlc_size = 0;
-	int vlc_data = 0;
-	int bit_cur;
-	int bit;
-	data += 4;
-	while (pos < packetlength) {
-		bit_cur = 8;
-		while (bit_cur && bit_exp) {
-			bit = ((*data) >> (bit_cur-1))&1;
-			if (!vlc_cod) {
-				if (bit) {
-					vlc_size++;
-				} else {
-					if (!vlc_size)
-						decode_JangGu_integrate(se401, 0);
-					else {
-						vlc_cod = 2;
-						vlc_data = 0;
-					}
-				}
-			} else {
-				if (vlc_cod == 2) {
-					if (!bit)
-						vlc_data =  -(1 << vlc_size) + 1;
-					vlc_cod--;
-				}
-				vlc_size--;
-				vlc_data += bit << vlc_size;
-				if (!vlc_size) {
-					decode_JangGu_integrate(se401, vlc_data);
-					vlc_cod = 0;
-				}
-			}
-			bit_cur--;
-			bit_exp--;
-		}
-		pos++;
-		data++;
-	}
-}
-
-static inline void decode_JangGu(struct usb_se401 *se401,
-						struct se401_scratch *buffer)
-{
-	unsigned char *data = buffer->data;
-	int len = buffer->length;
-	int bit_exp = 0, pix_exp = 0, frameinfo = 0, packetlength = 0, size;
-	int datapos = 0;
-
-	/* New image? */
-	if (!se401->frame[se401->curframe].curpix) {
-		se401->frame[se401->curframe].curlinepix = 0;
-		se401->frame[se401->curframe].curline =
-		    se401->frame[se401->curframe].data+
-		    se401->cwidth * 3 - 1;
-		if (se401->frame[se401->curframe].grabstate == FRAME_READY)
-			se401->frame[se401->curframe].grabstate = FRAME_GRABBING;
-		se401->vlcdatapos = 0;
-	}
-	while (datapos < len) {
-		size = 1024 - se401->vlcdatapos;
-		if (size+datapos > len)
-			size = len-datapos;
-		memcpy(se401->vlcdata+se401->vlcdatapos, data+datapos, size);
-		se401->vlcdatapos += size;
-		packetlength = 0;
-		if (se401->vlcdatapos >= 4) {
-			bit_exp = se401->vlcdata[3] + (se401->vlcdata[2] << 8);
-			pix_exp = se401->vlcdata[1] +
-					((se401->vlcdata[0] & 0x3f) << 8);
-			frameinfo = se401->vlcdata[0] & 0xc0;
-			packetlength = ((bit_exp + 47) >> 4) << 1;
-			if (packetlength > 1024) {
-				se401->vlcdatapos = 0;
-				datapos = len;
-				packetlength = 0;
-				se401->error++;
-				se401->frame[se401->curframe].curpix = 0;
-			}
-		}
-		if (packetlength && se401->vlcdatapos >= packetlength) {
-			decode_JangGu_vlc(se401, se401->vlcdata, bit_exp,
-								packetlength);
-			se401->frame[se401->curframe].curpix += pix_exp * 3;
-			datapos += size-(se401->vlcdatapos-packetlength);
-			se401->vlcdatapos = 0;
-			if (se401->frame[se401->curframe].curpix >= se401->cwidth * se401->cheight * 3) {
-				if (se401->frame[se401->curframe].curpix == se401->cwidth * se401->cheight * 3) {
-					if (se401->frame[se401->curframe].grabstate == FRAME_GRABBING) {
-						se401->frame[se401->curframe].grabstate = FRAME_DONE;
-						se401->framecount++;
-						se401->readcount++;
-					}
-					if (se401->frame[(se401->curframe + 1) & (SE401_NUMFRAMES - 1)].grabstate == FRAME_READY)
-						se401->curframe = (se401->curframe + 1) & (SE401_NUMFRAMES - 1);
-				} else
-					se401->error++;
-				se401->frame[se401->curframe].curpix = 0;
-				datapos = len;
-			}
-		} else
-			datapos += size;
-	}
-}
-
-static inline void decode_bayer(struct usb_se401 *se401,
-						struct se401_scratch *buffer)
-{
-	unsigned char *data = buffer->data;
-	int len = buffer->length;
-	int offset = buffer->offset;
-	int datasize = se401->cwidth * se401->cheight;
-	struct se401_frame *frame = &se401->frame[se401->curframe];
-	unsigned char *framedata = frame->data, *curline, *nextline;
-	int width = se401->cwidth;
-	int blineoffset = 0, bline;
-	int linelength = width * 3, i;
-
-
-	if (frame->curpix == 0) {
-		if (frame->grabstate == FRAME_READY)
-			frame->grabstate = FRAME_GRABBING;
-
-		frame->curline = framedata + linelength;
-		frame->curlinepix = 0;
-	}
-
-	if (offset != frame->curpix) {
-		/* Regard frame as lost :( */
-		frame->curpix = 0;
-		se401->error++;
-		return;
-	}
-
-	/* Check if we have to much data */
-	if (frame->curpix + len > datasize)
-		len = datasize-frame->curpix;
-
-	if (se401->cheight % 4)
-		blineoffset = 1;
-	bline = frame->curpix / se401->cwidth+blineoffset;
-
-	curline = frame->curline;
-	nextline = curline + linelength;
-	if (nextline >= framedata+datasize * 3)
-		nextline = curline;
-	while (len) {
-		if (frame->curlinepix >= width) {
-			frame->curlinepix -= width;
-			bline = frame->curpix / width + blineoffset;
-			curline += linelength*2;
-			nextline += linelength*2;
-			if (curline >= framedata+datasize * 3) {
-				frame->curlinepix++;
-				curline -= 3;
-				nextline -= 3;
-				len--;
-				data++;
-				frame->curpix++;
-			}
-			if (nextline >= framedata+datasize*3)
-				nextline = curline;
-		}
-		if (bline & 1) {
-			if (frame->curlinepix & 1) {
-				*(curline + 2) = *data;
-				*(curline - 1) = *data;
-				*(nextline + 2) = *data;
-				*(nextline - 1) = *data;
-			} else {
-				*(curline + 1) =
-					(*(curline + 1) + *data) / 2;
-				*(curline-2) =
-					(*(curline - 2) + *data) / 2;
-				*(nextline + 1) = *data;
-				*(nextline - 2) = *data;
-			}
-		} else {
-			if (frame->curlinepix & 1) {
-				*(curline + 1) =
-					(*(curline + 1) + *data) / 2;
-				*(curline - 2) =
-					(*(curline - 2) + *data) / 2;
-				*(nextline + 1) = *data;
-				*(nextline - 2) = *data;
-			} else {
-				*curline = *data;
-				*(curline - 3) = *data;
-				*nextline = *data;
-				*(nextline - 3) = *data;
-			}
-		}
-		frame->curlinepix++;
-		curline -= 3;
-		nextline -= 3;
-		len--;
-		data++;
-		frame->curpix++;
-	}
-	frame->curline = curline;
-
-	if (frame->curpix >= datasize) {
-		/* Fix the top line */
-		framedata += linelength;
-		for (i = 0; i < linelength; i++) {
-			framedata--;
-			*framedata = *(framedata + linelength);
-		}
-		/* Fix the left side (green is already present) */
-		for (i = 0; i < se401->cheight; i++) {
-			*framedata = *(framedata + 3);
-			*(framedata + 1) = *(framedata + 4);
-			*(framedata + 2) = *(framedata + 5);
-			framedata += linelength;
-		}
-		frame->curpix = 0;
-		frame->grabstate = FRAME_DONE;
-		se401->framecount++;
-		se401->readcount++;
-		if (se401->frame[(se401->curframe + 1) &
-		    (SE401_NUMFRAMES - 1)].grabstate == FRAME_READY) {
-			se401->curframe = (se401->curframe+1) &
-							(SE401_NUMFRAMES-1);
-		}
-	}
-}
-
-static int se401_newframe(struct usb_se401 *se401, int framenr)
-{
-	DECLARE_WAITQUEUE(wait, current);
-	int errors = 0;
-
-	while (se401->streaming &&
-	    (se401->frame[framenr].grabstate == FRAME_READY ||
-	     se401->frame[framenr].grabstate == FRAME_GRABBING)) {
-		if (!se401->frame[framenr].curpix)
-			errors++;
-
-		wait_interruptible(
-		    se401->scratch[se401->scratch_use].state != BUFFER_READY,
-						    &se401->wq, &wait);
-		if (se401->nullpackets > SE401_MAX_NULLPACKETS) {
-			se401->nullpackets = 0;
-			dev_info(&se401->dev->dev,
-			 "too many null length packets, restarting capture\n");
-			se401_stop_stream(se401);
-			se401_start_stream(se401);
-		} else {
-			if (se401->scratch[se401->scratch_use].state !=
-								BUFFER_READY) {
-				se401->frame[framenr].grabstate = FRAME_ERROR;
-				return -EIO;
-			}
-			se401->scratch[se401->scratch_use].state = BUFFER_BUSY;
-			if (se401->format == FMT_JANGGU)
-				decode_JangGu(se401,
-					&se401->scratch[se401->scratch_use]);
-			else
-				decode_bayer(se401,
-					&se401->scratch[se401->scratch_use]);
-
-			se401->scratch[se401->scratch_use].state =
-							BUFFER_UNUSED;
-			se401->scratch_use++;
-			if (se401->scratch_use >= SE401_NUMSCRATCH)
-				se401->scratch_use = 0;
-			if (errors > SE401_MAX_ERRORS) {
-				errors = 0;
-				dev_info(&se401->dev->dev,
-				      "too many errors, restarting capture\n");
-				se401_stop_stream(se401);
-				se401_start_stream(se401);
-			}
-		}
-	}
-
-	if (se401->frame[framenr].grabstate == FRAME_DONE)
-		if (se401->enhance)
-			enhance_picture(se401->frame[framenr].data,
-					se401->cheight * se401->cwidth * 3);
-	return 0;
-}
-
-static void usb_se401_remove_disconnected(struct usb_se401 *se401)
-{
-	int i;
-
-	se401->dev = NULL;
-
-	for (i = 0; i < SE401_NUMSBUF; i++)
-		if (se401->urb[i]) {
-			usb_kill_urb(se401->urb[i]);
-			usb_free_urb(se401->urb[i]);
-			se401->urb[i] = NULL;
-			kfree(se401->sbuf[i].data);
-		}
-
-	for (i = 0; i < SE401_NUMSCRATCH; i++)
-		kfree(se401->scratch[i].data);
-
-	if (se401->inturb) {
-		usb_kill_urb(se401->inturb);
-		usb_free_urb(se401->inturb);
-	}
-	dev_info(&se401->dev->dev, "%s disconnected", se401->camera_name);
-
-	/* Free the memory */
-	kfree(se401->width);
-	kfree(se401->height);
-	kfree(se401);
-}
-
-
-
-/****************************************************************************
- *
- * Video4Linux
- *
- ***************************************************************************/
-
-
-static int se401_open(struct file *file)
-{
-	struct video_device *dev = video_devdata(file);
-	struct usb_se401 *se401 = (struct usb_se401 *)dev;
-	int err = 0;
-
-	mutex_lock(&se401->lock);
-	if (se401->user) {
-		mutex_unlock(&se401->lock);
-		return -EBUSY;
-	}
-	se401->fbuf = rvmalloc(se401->maxframesize * SE401_NUMFRAMES);
-	if (se401->fbuf)
-		file->private_data = dev;
-	else
-		err = -ENOMEM;
-	se401->user = !err;
-	mutex_unlock(&se401->lock);
-
-	return err;
-}
-
-static int se401_close(struct file *file)
-{
-	struct video_device *dev = file->private_data;
-	struct usb_se401 *se401 = (struct usb_se401 *)dev;
-	int i;
-
-	rvfree(se401->fbuf, se401->maxframesize * SE401_NUMFRAMES);
-	if (se401->removed) {
-		dev_info(&se401->dev->dev, "device unregistered\n");
-		usb_se401_remove_disconnected(se401);
-	} else {
-		for (i = 0; i < SE401_NUMFRAMES; i++)
-			se401->frame[i].grabstate = FRAME_UNUSED;
-		if (se401->streaming)
-			se401_stop_stream(se401);
-		se401->user = 0;
-	}
-	file->private_data = NULL;
-	return 0;
-}
-
-static long se401_do_ioctl(struct file *file, unsigned int cmd, void *arg)
-{
-	struct video_device *vdev = file->private_data;
-	struct usb_se401 *se401 = (struct usb_se401 *)vdev;
-
-	if (!se401->dev)
-		return -EIO;
-
-	switch (cmd) {
-	case VIDIOCGCAP:
-	{
-		struct video_capability *b = arg;
-		strcpy(b->name, se401->camera_name);
-		b->type = VID_TYPE_CAPTURE;
-		b->channels = 1;
-		b->audios = 0;
-		b->maxwidth = se401->width[se401->sizes-1];
-		b->maxheight = se401->height[se401->sizes-1];
-		b->minwidth = se401->width[0];
-		b->minheight = se401->height[0];
-		return 0;
-	}
-	case VIDIOCGCHAN:
-	{
-		struct video_channel *v = arg;
-
-		if (v->channel != 0)
-			return -EINVAL;
-		v->flags = 0;
-		v->tuners = 0;
-		v->type = VIDEO_TYPE_CAMERA;
-		strcpy(v->name, "Camera");
-		return 0;
-	}
-	case VIDIOCSCHAN:
-	{
-		struct video_channel *v = arg;
-
-		if (v->channel != 0)
-			return -EINVAL;
-		return 0;
-	}
-	case VIDIOCGPICT:
-	{
-		struct video_picture *p = arg;
-
-		se401_get_pict(se401, p);
-		return 0;
-	}
-	case VIDIOCSPICT:
-	{
-		struct video_picture *p = arg;
-
-		if (se401_set_pict(se401, p))
-			return -EINVAL;
-		return 0;
-	}
-	case VIDIOCSWIN:
-	{
-		struct video_window *vw = arg;
-
-		if (vw->flags)
-			return -EINVAL;
-		if (vw->clipcount)
-			return -EINVAL;
-		if (se401_set_size(se401, vw->width, vw->height))
-			return -EINVAL;
-		return 0;
-	}
-	case VIDIOCGWIN:
-	{
-		struct video_window *vw = arg;
-
-		vw->x = 0;               /* FIXME */
-		vw->y = 0;
-		vw->chromakey = 0;
-		vw->flags = 0;
-		vw->clipcount = 0;
-		vw->width = se401->cwidth;
-		vw->height = se401->cheight;
-		return 0;
-	}
-	case VIDIOCGMBUF:
-	{
-		struct video_mbuf *vm = arg;
-		int i;
-
-		memset(vm, 0, sizeof(*vm));
-		vm->size = SE401_NUMFRAMES * se401->maxframesize;
-		vm->frames = SE401_NUMFRAMES;
-		for (i = 0; i < SE401_NUMFRAMES; i++)
-			vm->offsets[i] = se401->maxframesize * i;
-		return 0;
-	}
-	case VIDIOCMCAPTURE:
-	{
-		struct video_mmap *vm = arg;
-
-		if (vm->format != VIDEO_PALETTE_RGB24)
-			return -EINVAL;
-		if (vm->frame >= SE401_NUMFRAMES)
-			return -EINVAL;
-		if (se401->frame[vm->frame].grabstate != FRAME_UNUSED)
-			return -EBUSY;
-
-		/* Is this according to the v4l spec??? */
-		if (se401_set_size(se401, vm->width, vm->height))
-			return -EINVAL;
-		se401->frame[vm->frame].grabstate = FRAME_READY;
-
-		if (!se401->streaming)
-			se401_start_stream(se401);
-
-		/* Set the picture properties */
-		if (se401->framecount == 0)
-			se401_send_pict(se401);
-		/* Calibrate the reset level after a few frames. */
-		if (se401->framecount % 20 == 1)
-			se401_auto_resetlevel(se401);
-
-		return 0;
-	}
-	case VIDIOCSYNC:
-	{
-		int *frame = arg;
-		int ret = 0;
-
-		if (*frame < 0 || *frame >= SE401_NUMFRAMES)
-			return -EINVAL;
-
-		ret = se401_newframe(se401, *frame);
-		se401->frame[*frame].grabstate = FRAME_UNUSED;
-		return ret;
-	}
-	case VIDIOCGFBUF:
-	{
-		struct video_buffer *vb = arg;
-
-		memset(vb, 0, sizeof(*vb));
-		return 0;
-	}
-	case VIDIOCKEY:
-		return 0;
-	case VIDIOCCAPTURE:
-		return -EINVAL;
-	case VIDIOCSFBUF:
-		return -EINVAL;
-	case VIDIOCGTUNER:
-	case VIDIOCSTUNER:
-		return -EINVAL;
-	case VIDIOCGFREQ:
-	case VIDIOCSFREQ:
-		return -EINVAL;
-	case VIDIOCGAUDIO:
-	case VIDIOCSAUDIO:
-		return -EINVAL;
-	default:
-		return -ENOIOCTLCMD;
-	} /* end switch */
-
-	return 0;
-}
-
-static long se401_ioctl(struct file *file,
-		       unsigned int cmd, unsigned long arg)
-{
-	return video_usercopy(file, cmd, arg, se401_do_ioctl);
-}
-
-static ssize_t se401_read(struct file *file, char __user *buf,
-		     size_t count, loff_t *ppos)
-{
-	int realcount = count, ret = 0;
-	struct video_device *dev = file->private_data;
-	struct usb_se401 *se401 = (struct usb_se401 *)dev;
-
-
-	if (se401->dev ==  NULL)
-		return -EIO;
-	if (realcount > se401->cwidth*se401->cheight*3)
-		realcount = se401->cwidth*se401->cheight*3;
-
-	/* Shouldn't happen: */
-	if (se401->frame[0].grabstate == FRAME_GRABBING)
-		return -EBUSY;
-	se401->frame[0].grabstate = FRAME_READY;
-	se401->frame[1].grabstate = FRAME_UNUSED;
-	se401->curframe = 0;
-
-	if (!se401->streaming)
-		se401_start_stream(se401);
-
-	/* Set the picture properties */
-	if (se401->framecount == 0)
-		se401_send_pict(se401);
-	/* Calibrate the reset level after a few frames. */
-	if (se401->framecount%20 == 1)
-		se401_auto_resetlevel(se401);
-
-	ret = se401_newframe(se401, 0);
-
-	se401->frame[0].grabstate = FRAME_UNUSED;
-	if (ret)
-		return ret;
-	if (copy_to_user(buf, se401->frame[0].data, realcount))
-		return -EFAULT;
-
-	return realcount;
-}
-
-static int se401_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct video_device *dev = file->private_data;
-	struct usb_se401 *se401 = (struct usb_se401 *)dev;
-	unsigned long start = vma->vm_start;
-	unsigned long size  = vma->vm_end-vma->vm_start;
-	unsigned long page, pos;
-
-	mutex_lock(&se401->lock);
-
-	if (se401->dev ==  NULL) {
-		mutex_unlock(&se401->lock);
-		return -EIO;
-	}
-	if (size > (((SE401_NUMFRAMES * se401->maxframesize) + PAGE_SIZE - 1)
-							& ~(PAGE_SIZE - 1))) {
-		mutex_unlock(&se401->lock);
-		return -EINVAL;
-	}
-	pos = (unsigned long)se401->fbuf;
-	while (size > 0) {
-		page = vmalloc_to_pfn((void *)pos);
-		if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED)) {
-			mutex_unlock(&se401->lock);
-			return -EAGAIN;
-		}
-		start +=  PAGE_SIZE;
-		pos +=  PAGE_SIZE;
-		if (size > PAGE_SIZE)
-			size -=  PAGE_SIZE;
-		else
-			size = 0;
-	}
-	mutex_unlock(&se401->lock);
-
-	return 0;
-}
-
-static const struct v4l2_file_operations se401_fops = {
-	.owner  = 	THIS_MODULE,
-	.open =         se401_open,
-	.release =      se401_close,
-	.read =         se401_read,
-	.mmap =         se401_mmap,
-	.ioctl =        se401_ioctl,
-};
-static struct video_device se401_template = {
-	.name =         "se401 USB camera",
-	.fops =         &se401_fops,
-	.release = video_device_release_empty,
-};
-
-
-
-/***************************/
-static int se401_init(struct usb_se401 *se401, int button)
-{
-	int i = 0, rc;
-	unsigned char cp[0x40];
-	char temp[200];
-	int slen;
-
-	/* led on */
-	se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);
-
-	/* get camera descriptor */
-	rc = se401_sndctrl(0, se401, SE401_REQ_GET_CAMERA_DESCRIPTOR, 0,
-							cp, sizeof(cp));
-	if (cp[1] != 0x41) {
-		err("Wrong descriptor type");
-		return 1;
-	}
-	slen = snprintf(temp, 200, "ExtraFeatures: %d", cp[3]);
-
-	se401->sizes = cp[4] + cp[5] * 256;
-	se401->width = kmalloc(se401->sizes*sizeof(int), GFP_KERNEL);
-	if (!se401->width)
-		return 1;
-	se401->height = kmalloc(se401->sizes*sizeof(int), GFP_KERNEL);
-	if (!se401->height) {
-		kfree(se401->width);
-		return 1;
-	}
-	for (i = 0; i < se401->sizes; i++) {
-		se401->width[i] = cp[6 + i * 4 + 0] + cp[6 + i*4 + 1] * 256;
-		se401->height[i] = cp[6 + i * 4 + 2] + cp[6 + i * 4 + 3] * 256;
-	}
-	slen += snprintf(temp + slen, 200 - slen, " Sizes:");
-	for (i = 0; i < se401->sizes; i++) {
-		slen +=  snprintf(temp + slen, 200 - slen,
-			" %dx%d", se401->width[i], se401->height[i]);
-	}
-	dev_info(&se401->dev->dev, "%s\n", temp);
-	se401->maxframesize = se401->width[se401->sizes-1] *
-					se401->height[se401->sizes - 1] * 3;
-
-	rc = se401_sndctrl(0, se401, SE401_REQ_GET_WIDTH, 0, cp, sizeof(cp));
-	se401->cwidth = cp[0]+cp[1]*256;
-	rc = se401_sndctrl(0, se401, SE401_REQ_GET_HEIGHT, 0, cp, sizeof(cp));
-	se401->cheight = cp[0]+cp[1]*256;
-
-	if (!(cp[2] & SE401_FORMAT_BAYER)) {
-		err("Bayer format not supported!");
-		return 1;
-	}
-	/* set output mode (BAYER) */
-	se401_sndctrl(1, se401, SE401_REQ_SET_OUTPUT_MODE,
-						SE401_FORMAT_BAYER, NULL, 0);
-
-	rc = se401_sndctrl(0, se401, SE401_REQ_GET_BRT, 0, cp, sizeof(cp));
-	se401->brightness = cp[0]+cp[1]*256;
-	/* some default values */
-	se401->resetlevel = 0x2d;
-	se401->rgain = 0x20;
-	se401->ggain = 0x20;
-	se401->bgain = 0x20;
-	se401_set_exposure(se401, 20000);
-	se401->palette = VIDEO_PALETTE_RGB24;
-	se401->enhance = 1;
-	se401->dropped = 0;
-	se401->error = 0;
-	se401->framecount = 0;
-	se401->readcount = 0;
-
-	/* Start interrupt transfers for snapshot button */
-	if (button) {
-		se401->inturb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!se401->inturb) {
-			dev_info(&se401->dev->dev,
-				 "Allocation of inturb failed\n");
-			return 1;
-		}
-		usb_fill_int_urb(se401->inturb, se401->dev,
-		    usb_rcvintpipe(se401->dev, SE401_BUTTON_ENDPOINT),
-		    &se401->button, sizeof(se401->button),
-		    se401_button_irq,
-		    se401,
-		    8
-		);
-		if (usb_submit_urb(se401->inturb, GFP_KERNEL)) {
-			dev_info(&se401->dev->dev, "int urb burned down\n");
-			return 1;
-		}
-	} else
-		se401->inturb = NULL;
-
-	/* Flash the led */
-	se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 1, NULL, 0);
-	se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);
-	se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 0, NULL, 0);
-	se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 0, NULL, 0);
-
-	return 0;
-}
-
-static int se401_probe(struct usb_interface *intf,
-	const struct usb_device_id *id)
-{
-	struct usb_device *dev = interface_to_usbdev(intf);
-	struct usb_interface_descriptor *interface;
-	struct usb_se401 *se401;
-	char *camera_name = NULL;
-	int button = 1;
-
-	/* We don't handle multi-config cameras */
-	if (dev->descriptor.bNumConfigurations != 1)
-		return -ENODEV;
-
-	interface = &intf->cur_altsetting->desc;
-
-	/* Is it an se401? */
-	if (le16_to_cpu(dev->descriptor.idVendor) ==  0x03e8 &&
-	    le16_to_cpu(dev->descriptor.idProduct) ==  0x0004) {
-		camera_name = "Endpoints/Aox SE401";
-	} else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x0471 &&
-	    le16_to_cpu(dev->descriptor.idProduct) ==  0x030b) {
-		camera_name = "Philips PCVC665K";
-	} else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x047d &&
-	    le16_to_cpu(dev->descriptor.idProduct) ==  0x5001) {
-		camera_name = "Kensington VideoCAM 67014";
-	} else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x047d &&
-	    le16_to_cpu(dev->descriptor.idProduct) ==  0x5002) {
-		camera_name = "Kensington VideoCAM 6701(5/7)";
-	} else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x047d &&
-	    le16_to_cpu(dev->descriptor.idProduct) ==  0x5003) {
-		camera_name = "Kensington VideoCAM 67016";
-		button = 0;
-	} else
-		return -ENODEV;
-
-	/* Checking vendor/product should be enough, but what the hell */
-	if (interface->bInterfaceClass != 0x00)
-		return -ENODEV;
-	if (interface->bInterfaceSubClass != 0x00)
-		return -ENODEV;
-
-	/* We found one */
-	dev_info(&intf->dev, "SE401 camera found: %s\n", camera_name);
-
-	se401 = kzalloc(sizeof(*se401), GFP_KERNEL);
-	if (se401 ==  NULL) {
-		err("couldn't kmalloc se401 struct");
-		return -ENOMEM;
-	}
-
-	se401->dev = dev;
-	se401->iface = interface->bInterfaceNumber;
-	se401->camera_name = camera_name;
-
-	dev_info(&intf->dev, "firmware version: %02x\n",
-		 le16_to_cpu(dev->descriptor.bcdDevice) & 255);
-
-	if (se401_init(se401, button)) {
-		kfree(se401);
-		return -EIO;
-	}
-
-	memcpy(&se401->vdev, &se401_template, sizeof(se401_template));
-	memcpy(se401->vdev.name, se401->camera_name,
-					strlen(se401->camera_name));
-	init_waitqueue_head(&se401->wq);
-	mutex_init(&se401->lock);
-	wmb();
-
-	if (video_register_device(&se401->vdev,
-					VFL_TYPE_GRABBER, video_nr) < 0) {
-		kfree(se401);
-		err("video_register_device failed");
-		return -EIO;
-	}
-	dev_info(&intf->dev, "registered new video device: %s\n",
-		 video_device_node_name(&se401->vdev));
-
-	usb_set_intfdata(intf, se401);
-	return 0;
-}
-
-static void se401_disconnect(struct usb_interface *intf)
-{
-	struct usb_se401 *se401 = usb_get_intfdata(intf);
-
-	usb_set_intfdata(intf, NULL);
-	if (se401) {
-		video_unregister_device(&se401->vdev);
-		if (!se401->user)
-			usb_se401_remove_disconnected(se401);
-		else {
-			se401->frame[0].grabstate = FRAME_ERROR;
-			se401->frame[0].grabstate = FRAME_ERROR;
-
-			se401->streaming = 0;
-
-			wake_up_interruptible(&se401->wq);
-			se401->removed = 1;
-		}
-	}
-}
-
-static struct usb_driver se401_driver = {
-	.name		 =  "se401",
-	.id_table	 =  device_table,
-	.probe		 =  se401_probe,
-	.disconnect	 =  se401_disconnect,
-};
-
-
-
-/****************************************************************************
- *
- *  Module routines
- *
- ***************************************************************************/
-
-static int __init usb_se401_init(void)
-{
-	printk(KERN_INFO "SE401 usb camera driver version %s registering\n",
-								version);
-	if (flickerless)
-		if (flickerless != 50 && flickerless != 60) {
-			printk(KERN_ERR "Invallid flickerless value, use 0, 50 or 60.\n");
-			return -1;
-	}
-	return usb_register(&se401_driver);
-}
-
-static void __exit usb_se401_exit(void)
-{
-	usb_deregister(&se401_driver);
-	printk(KERN_INFO "SE401 driver deregistered\frame");
-}
-
-module_init(usb_se401_init);
-module_exit(usb_se401_exit);
diff --git a/drivers/staging/se401/se401.h b/drivers/staging/se401/se401.h
deleted file mode 100644
index 2758f4716c3d..000000000000
--- a/drivers/staging/se401/se401.h
+++ /dev/null
@@ -1,236 +0,0 @@
-
-#ifndef __LINUX_se401_H
-#define __LINUX_se401_H
-
-#include <linux/uaccess.h>
-#include "videodev.h"
-#include <media/v4l2-common.h>
-#include <media/v4l2-ioctl.h>
-#include <linux/mutex.h>
-
-#define se401_DEBUG	/* Turn on debug messages */
-
-#ifdef se401_DEBUG
-#  define PDEBUG(level, fmt, args...) \
-if (debug >= level) \
-	info("[" __PRETTY_FUNCTION__ ":%d] " fmt, __LINE__ , ## args)
-#else
-#  define PDEBUG(level, fmt, args...) do {} while (0)
-#endif
-
-/* An almost drop-in replacement for sleep_on_interruptible */
-#define wait_interruptible(test, queue, wait) \
-{ \
-	add_wait_queue(queue, wait); \
-	set_current_state(TASK_INTERRUPTIBLE); \
-	if (test) \
-		schedule(); \
-	remove_wait_queue(queue, wait); \
-	set_current_state(TASK_RUNNING); \
-	if (signal_pending(current)) \
-		break; \
-}
-
-#define SE401_REQ_GET_CAMERA_DESCRIPTOR		0x06
-#define SE401_REQ_START_CONTINUOUS_CAPTURE	0x41
-#define SE401_REQ_STOP_CONTINUOUS_CAPTURE	0x42
-#define SE401_REQ_CAPTURE_FRAME			0x43
-#define SE401_REQ_GET_BRT			0x44
-#define SE401_REQ_SET_BRT			0x45
-#define SE401_REQ_GET_WIDTH			0x4c
-#define SE401_REQ_SET_WIDTH			0x4d
-#define SE401_REQ_GET_HEIGHT			0x4e
-#define SE401_REQ_SET_HEIGHT			0x4f
-#define SE401_REQ_GET_OUTPUT_MODE		0x50
-#define SE401_REQ_SET_OUTPUT_MODE		0x51
-#define SE401_REQ_GET_EXT_FEATURE		0x52
-#define SE401_REQ_SET_EXT_FEATURE		0x53
-#define SE401_REQ_CAMERA_POWER			0x56
-#define SE401_REQ_LED_CONTROL			0x57
-#define SE401_REQ_BIOS				0xff
-
-#define SE401_BIOS_READ				0x07
-
-#define SE401_FORMAT_BAYER	0x40
-
-/* Hyundai hv7131b registers
-   7121 and 7141 should be the same (haven't really checked...) */
-/* Mode registers: */
-#define HV7131_REG_MODE_A		0x00
-#define HV7131_REG_MODE_B		0x01
-#define HV7131_REG_MODE_C		0x02
-/* Frame registers: */
-#define HV7131_REG_FRSU		0x10
-#define HV7131_REG_FRSL		0x11
-#define HV7131_REG_FCSU		0x12
-#define HV7131_REG_FCSL		0x13
-#define HV7131_REG_FWHU		0x14
-#define HV7131_REG_FWHL		0x15
-#define HV7131_REG_FWWU		0x16
-#define HV7131_REG_FWWL		0x17
-/* Timing registers: */
-#define HV7131_REG_THBU		0x20
-#define HV7131_REG_THBL		0x21
-#define HV7131_REG_TVBU		0x22
-#define HV7131_REG_TVBL		0x23
-#define HV7131_REG_TITU		0x25
-#define HV7131_REG_TITM		0x26
-#define HV7131_REG_TITL		0x27
-#define HV7131_REG_TMCD		0x28
-/* Adjust Registers: */
-#define HV7131_REG_ARLV		0x30
-#define HV7131_REG_ARCG		0x31
-#define HV7131_REG_AGCG		0x32
-#define HV7131_REG_ABCG		0x33
-#define HV7131_REG_APBV		0x34
-#define HV7131_REG_ASLP		0x54
-/* Offset Registers: */
-#define HV7131_REG_OFSR		0x50
-#define HV7131_REG_OFSG		0x51
-#define HV7131_REG_OFSB		0x52
-/* REset level statistics registers: */
-#define HV7131_REG_LOREFNOH	0x57
-#define HV7131_REG_LOREFNOL	0x58
-#define HV7131_REG_HIREFNOH	0x59
-#define HV7131_REG_HIREFNOL	0x5a
-
-/* se401 registers */
-#define SE401_OPERATINGMODE	0x2000
-
-
-/* size of usb transfers */
-#define SE401_PACKETSIZE	4096
-/* number of queued bulk transfers to use, should be about 8 */
-#define SE401_NUMSBUF		1
-/* read the usb specs for this one :) */
-#define SE401_VIDEO_ENDPOINT	1
-#define SE401_BUTTON_ENDPOINT	2
-/* number of frames supported by the v4l part */
-#define SE401_NUMFRAMES		2
-/* scratch buffers for passing data to the decoders */
-#define SE401_NUMSCRATCH	32
-/* maximum amount of data in a JangGu packet */
-#define SE401_VLCDATALEN	1024
-/* number of nul sized packets to receive before kicking the camera */
-#define SE401_MAX_NULLPACKETS	4000
-/* number of decoding errors before kicking the camera */
-#define SE401_MAX_ERRORS	200
-
-struct usb_device;
-
-struct se401_sbuf {
-	unsigned char *data;
-};
-
-enum {
-	FRAME_UNUSED,		/* Unused (no MCAPTURE) */
-	FRAME_READY,		/* Ready to start grabbing */
-	FRAME_GRABBING,		/* In the process of being grabbed into */
-	FRAME_DONE,		/* Finished grabbing, but not been synced yet */
-	FRAME_ERROR,		/* Something bad happened while processing */
-};
-
-enum {
-	FMT_BAYER,
-	FMT_JANGGU,
-};
-
-enum {
-	BUFFER_UNUSED,
-	BUFFER_READY,
-	BUFFER_BUSY,
-	BUFFER_DONE,
-};
-
-struct se401_scratch {
-	unsigned char *data;
-	volatile int state;
-	int offset;
-	int length;
-};
-
-struct se401_frame {
-	unsigned char *data;		/* Frame buffer */
-
-	volatile int grabstate;	/* State of grabbing */
-
-	unsigned char *curline;
-	int curlinepix;
-	int curpix;
-};
-
-struct usb_se401 {
-	struct video_device vdev;
-
-	/* Device structure */
-	struct usb_device *dev;
-
-	unsigned char iface;
-
-	char *camera_name;
-
-	int change;
-	int brightness;
-	int hue;
-	int rgain;
-	int ggain;
-	int bgain;
-	int expose_h;
-	int expose_m;
-	int expose_l;
-	int resetlevel;
-
-	int enhance;
-
-	int format;
-	int sizes;
-	int *width;
-	int *height;
-	int cwidth;		/* current width */
-	int cheight;		/* current height */
-	int palette;
-	int maxframesize;
-	int cframesize;		/* current framesize */
-
-	struct mutex lock;
-	int user;		/* user count for exclusive use */
-	int removed;		/* device disconnected */
-
-	int streaming;		/* Are we streaming video? */
-
-	char *fbuf;		/* Videodev buffer area */
-
-	struct urb *urb[SE401_NUMSBUF];
-	struct urb *inturb;
-
-	int button;
-	int buttonpressed;
-
-	int curframe;		/* Current receiving frame */
-	struct se401_frame frame[SE401_NUMFRAMES];
-	int readcount;
-	int framecount;
-	int error;
-	int dropped;
-
-	int scratch_next;
-	int scratch_use;
-	int scratch_overflow;
-	struct se401_scratch scratch[SE401_NUMSCRATCH];
-
-	/* Decoder specific data: */
-	unsigned char vlcdata[SE401_VLCDATALEN];
-	int vlcdatapos;
-	int bayeroffset;
-
-	struct se401_sbuf sbuf[SE401_NUMSBUF];
-
-	wait_queue_head_t wq;	/* Processes waiting */
-
-	int nullpackets;
-};
-
-
-
-#endif
-
diff --git a/drivers/staging/se401/videodev.h b/drivers/staging/se401/videodev.h
deleted file mode 100644
index f11efbef1c05..000000000000
--- a/drivers/staging/se401/videodev.h
+++ /dev/null
@@ -1,318 +0,0 @@
-/*
- *	Video for Linux version 1 - OBSOLETE
- *
- *	Header file for v4l1 drivers and applications, for
- *	Linux kernels 2.2.x or 2.4.x.
- *
- *	Provides header for legacy drivers and applications
- *
- *	See http://linuxtv.org for more info
- *
- */
-#ifndef __LINUX_VIDEODEV_H
-#define __LINUX_VIDEODEV_H
-
-#include <linux/types.h>
-#include <linux/ioctl.h>
-#include <linux/videodev2.h>
-
-#define VID_TYPE_CAPTURE	1	/* Can capture */
-#define VID_TYPE_TUNER		2	/* Can tune */
-#define VID_TYPE_TELETEXT	4	/* Does teletext */
-#define VID_TYPE_OVERLAY	8	/* Overlay onto frame buffer */
-#define VID_TYPE_CHROMAKEY	16	/* Overlay by chromakey */
-#define VID_TYPE_CLIPPING	32	/* Can clip */
-#define VID_TYPE_FRAMERAM	64	/* Uses the frame buffer memory */
-#define VID_TYPE_SCALES		128	/* Scalable */
-#define VID_TYPE_MONOCHROME	256	/* Monochrome only */
-#define VID_TYPE_SUBCAPTURE	512	/* Can capture subareas of the image */
-#define VID_TYPE_MPEG_DECODER	1024	/* Can decode MPEG streams */
-#define VID_TYPE_MPEG_ENCODER	2048	/* Can encode MPEG streams */
-#define VID_TYPE_MJPEG_DECODER	4096	/* Can decode MJPEG streams */
-#define VID_TYPE_MJPEG_ENCODER	8192	/* Can encode MJPEG streams */
-
-struct video_capability
-{
-	char name[32];
-	int type;
-	int channels;	/* Num channels */
-	int audios;	/* Num audio devices */
-	int maxwidth;	/* Supported width */
-	int maxheight;	/* And height */
-	int minwidth;	/* Supported width */
-	int minheight;	/* And height */
-};
-
-
-struct video_channel
-{
-	int channel;
-	char name[32];
-	int tuners;
-	__u32  flags;
-#define VIDEO_VC_TUNER		1	/* Channel has a tuner */
-#define VIDEO_VC_AUDIO		2	/* Channel has audio */
-	__u16  type;
-#define VIDEO_TYPE_TV		1
-#define VIDEO_TYPE_CAMERA	2
-	__u16 norm;			/* Norm set by channel */
-};
-
-struct video_tuner
-{
-	int tuner;
-	char name[32];
-	unsigned long rangelow, rangehigh;	/* Tuner range */
-	__u32 flags;
-#define VIDEO_TUNER_PAL		1
-#define VIDEO_TUNER_NTSC	2
-#define VIDEO_TUNER_SECAM	4
-#define VIDEO_TUNER_LOW		8	/* Uses KHz not MHz */
-#define VIDEO_TUNER_NORM	16	/* Tuner can set norm */
-#define VIDEO_TUNER_STEREO_ON	128	/* Tuner is seeing stereo */
-#define VIDEO_TUNER_RDS_ON      256     /* Tuner is seeing an RDS datastream */
-#define VIDEO_TUNER_MBS_ON      512     /* Tuner is seeing an MBS datastream */
-	__u16 mode;			/* PAL/NTSC/SECAM/OTHER */
-#define VIDEO_MODE_PAL		0
-#define VIDEO_MODE_NTSC		1
-#define VIDEO_MODE_SECAM	2
-#define VIDEO_MODE_AUTO		3
-	__u16 signal;			/* Signal strength 16bit scale */
-};
-
-struct video_picture
-{
-	__u16	brightness;
-	__u16	hue;
-	__u16	colour;
-	__u16	contrast;
-	__u16	whiteness;	/* Black and white only */
-	__u16	depth;		/* Capture depth */
-	__u16   palette;	/* Palette in use */
-#define VIDEO_PALETTE_GREY	1	/* Linear greyscale */
-#define VIDEO_PALETTE_HI240	2	/* High 240 cube (BT848) */
-#define VIDEO_PALETTE_RGB565	3	/* 565 16 bit RGB */
-#define VIDEO_PALETTE_RGB24	4	/* 24bit RGB */
-#define VIDEO_PALETTE_RGB32	5	/* 32bit RGB */
-#define VIDEO_PALETTE_RGB555	6	/* 555 15bit RGB */
-#define VIDEO_PALETTE_YUV422	7	/* YUV422 capture */
-#define VIDEO_PALETTE_YUYV	8
-#define VIDEO_PALETTE_UYVY	9	/* The great thing about standards is ... */
-#define VIDEO_PALETTE_YUV420	10
-#define VIDEO_PALETTE_YUV411	11	/* YUV411 capture */
-#define VIDEO_PALETTE_RAW	12	/* RAW capture (BT848) */
-#define VIDEO_PALETTE_YUV422P	13	/* YUV 4:2:2 Planar */
-#define VIDEO_PALETTE_YUV411P	14	/* YUV 4:1:1 Planar */
-#define VIDEO_PALETTE_YUV420P	15	/* YUV 4:2:0 Planar */
-#define VIDEO_PALETTE_YUV410P	16	/* YUV 4:1:0 Planar */
-#define VIDEO_PALETTE_PLANAR	13	/* start of planar entries */
-#define VIDEO_PALETTE_COMPONENT 7	/* start of component entries */
-};
-
-struct video_audio
-{
-	int	audio;		/* Audio channel */
-	__u16	volume;		/* If settable */
-	__u16	bass, treble;
-	__u32	flags;
-#define VIDEO_AUDIO_MUTE	1
-#define VIDEO_AUDIO_MUTABLE	2
-#define VIDEO_AUDIO_VOLUME	4
-#define VIDEO_AUDIO_BASS	8
-#define VIDEO_AUDIO_TREBLE	16
-#define VIDEO_AUDIO_BALANCE	32
-	char    name[16];
-#define VIDEO_SOUND_MONO	1
-#define VIDEO_SOUND_STEREO	2
-#define VIDEO_SOUND_LANG1	4
-#define VIDEO_SOUND_LANG2	8
-	__u16   mode;
-	__u16	balance;	/* Stereo balance */
-	__u16	step;		/* Step actual volume uses */
-};
-
-struct video_clip
-{
-	__s32	x,y;
-	__s32	width, height;
-	struct	video_clip *next;	/* For user use/driver use only */
-};
-
-struct video_window
-{
-	__u32	x,y;			/* Position of window */
-	__u32	width,height;		/* Its size */
-	__u32	chromakey;
-	__u32	flags;
-	struct	video_clip __user *clips;	/* Set only */
-	int	clipcount;
-#define VIDEO_WINDOW_INTERLACE	1
-#define VIDEO_WINDOW_CHROMAKEY	16	/* Overlay by chromakey */
-#define VIDEO_CLIP_BITMAP	-1
-/* bitmap is 1024x625, a '1' bit represents a clipped pixel */
-#define VIDEO_CLIPMAP_SIZE	(128 * 625)
-};
-
-struct video_capture
-{
-	__u32 	x,y;			/* Offsets into image */
-	__u32	width, height;		/* Area to capture */
-	__u16	decimation;		/* Decimation divider */
-	__u16	flags;			/* Flags for capture */
-#define VIDEO_CAPTURE_ODD		0	/* Temporal */
-#define VIDEO_CAPTURE_EVEN		1
-};
-
-struct video_buffer
-{
-	void	*base;
-	int	height,width;
-	int	depth;
-	int	bytesperline;
-};
-
-struct video_mmap
-{
-	unsigned	int frame;		/* Frame (0 - n) for double buffer */
-	int		height,width;
-	unsigned	int format;		/* should be VIDEO_PALETTE_* */
-};
-
-struct video_key
-{
-	__u8	key[8];
-	__u32	flags;
-};
-
-struct video_mbuf
-{
-	int	size;		/* Total memory to map */
-	int	frames;		/* Frames */
-	int	offsets[VIDEO_MAX_FRAME];
-};
-
-#define 	VIDEO_NO_UNIT	(-1)
-
-struct video_unit
-{
-	int 	video;		/* Video minor */
-	int	vbi;		/* VBI minor */
-	int	radio;		/* Radio minor */
-	int	audio;		/* Audio minor */
-	int	teletext;	/* Teletext minor */
-};
-
-struct vbi_format {
-	__u32	sampling_rate;	/* in Hz */
-	__u32	samples_per_line;
-	__u32	sample_format;	/* VIDEO_PALETTE_RAW only (1 byte) */
-	__s32	start[2];	/* starting line for each frame */
-	__u32	count[2];	/* count of lines for each frame */
-	__u32	flags;
-#define	VBI_UNSYNC	1	/* can distingues between top/bottom field */
-#define	VBI_INTERLACED	2	/* lines are interlaced */
-};
-
-/* video_info is biased towards hardware mpeg encode/decode */
-/* but it could apply generically to any hardware compressor/decompressor */
-struct video_info
-{
-	__u32	frame_count;	/* frames output since decode/encode began */
-	__u32	h_size;		/* current unscaled horizontal size */
-	__u32	v_size;		/* current unscaled veritcal size */
-	__u32	smpte_timecode;	/* current SMPTE timecode (for current GOP) */
-	__u32	picture_type;	/* current picture type */
-	__u32	temporal_reference;	/* current temporal reference */
-	__u8	user_data[256];	/* user data last found in compressed stream */
-	/* user_data[0] contains user data flags, user_data[1] has count */
-};
-
-/* generic structure for setting playback modes */
-struct video_play_mode
-{
-	int	mode;
-	int	p1;
-	int	p2;
-};
-
-/* for loading microcode / fpga programming */
-struct video_code
-{
-	char	loadwhat[16];	/* name or tag of file being passed */
-	int	datasize;
-	__u8	*data;
-};
-
-#define VIDIOCGCAP		_IOR('v',1,struct video_capability)	/* Get capabilities */
-#define VIDIOCGCHAN		_IOWR('v',2,struct video_channel)	/* Get channel info (sources) */
-#define VIDIOCSCHAN		_IOW('v',3,struct video_channel)	/* Set channel 	*/
-#define VIDIOCGTUNER		_IOWR('v',4,struct video_tuner)		/* Get tuner abilities */
-#define VIDIOCSTUNER		_IOW('v',5,struct video_tuner)		/* Tune the tuner for the current channel */
-#define VIDIOCGPICT		_IOR('v',6,struct video_picture)	/* Get picture properties */
-#define VIDIOCSPICT		_IOW('v',7,struct video_picture)	/* Set picture properties */
-#define VIDIOCCAPTURE		_IOW('v',8,int)				/* Start, end capture */
-#define VIDIOCGWIN		_IOR('v',9, struct video_window)	/* Get the video overlay window */
-#define VIDIOCSWIN		_IOW('v',10, struct video_window)	/* Set the video overlay window - passes clip list for hardware smarts , chromakey etc */
-#define VIDIOCGFBUF		_IOR('v',11, struct video_buffer)	/* Get frame buffer */
-#define VIDIOCSFBUF		_IOW('v',12, struct video_buffer)	/* Set frame buffer - root only */
-#define VIDIOCKEY		_IOR('v',13, struct video_key)		/* Video key event - to dev 255 is to all - cuts capture on all DMA windows with this key (0xFFFFFFFF == all) */
-#define VIDIOCGFREQ		_IOR('v',14, unsigned long)		/* Set tuner */
-#define VIDIOCSFREQ		_IOW('v',15, unsigned long)		/* Set tuner */
-#define VIDIOCGAUDIO		_IOR('v',16, struct video_audio)	/* Get audio info */
-#define VIDIOCSAUDIO		_IOW('v',17, struct video_audio)	/* Audio source, mute etc */
-#define VIDIOCSYNC		_IOW('v',18, int)			/* Sync with mmap grabbing */
-#define VIDIOCMCAPTURE		_IOW('v',19, struct video_mmap)		/* Grab frames */
-#define VIDIOCGMBUF		_IOR('v',20, struct video_mbuf)		/* Memory map buffer info */
-#define VIDIOCGUNIT		_IOR('v',21, struct video_unit)		/* Get attached units */
-#define VIDIOCGCAPTURE		_IOR('v',22, struct video_capture)	/* Get subcapture */
-#define VIDIOCSCAPTURE		_IOW('v',23, struct video_capture)	/* Set subcapture */
-#define VIDIOCSPLAYMODE		_IOW('v',24, struct video_play_mode)	/* Set output video mode/feature */
-#define VIDIOCSWRITEMODE	_IOW('v',25, int)			/* Set write mode */
-#define VIDIOCGPLAYINFO		_IOR('v',26, struct video_info)		/* Get current playback info from hardware */
-#define VIDIOCSMICROCODE	_IOW('v',27, struct video_code)		/* Load microcode into hardware */
-#define	VIDIOCGVBIFMT		_IOR('v',28, struct vbi_format)		/* Get VBI information */
-#define	VIDIOCSVBIFMT		_IOW('v',29, struct vbi_format)		/* Set VBI information */
-
-
-#define BASE_VIDIOCPRIVATE	192		/* 192-255 are private */
-
-/* VIDIOCSWRITEMODE */
-#define VID_WRITE_MPEG_AUD		0
-#define VID_WRITE_MPEG_VID		1
-#define VID_WRITE_OSD			2
-#define VID_WRITE_TTX			3
-#define VID_WRITE_CC			4
-#define VID_WRITE_MJPEG			5
-
-/* VIDIOCSPLAYMODE */
-#define VID_PLAY_VID_OUT_MODE		0
-	/* p1: = VIDEO_MODE_PAL, VIDEO_MODE_NTSC, etc ... */
-#define VID_PLAY_GENLOCK		1
-	/* p1: 0 = OFF, 1 = ON */
-	/* p2: GENLOCK FINE DELAY value */
-#define VID_PLAY_NORMAL			2
-#define VID_PLAY_PAUSE			3
-#define VID_PLAY_SINGLE_FRAME		4
-#define VID_PLAY_FAST_FORWARD		5
-#define VID_PLAY_SLOW_MOTION		6
-#define VID_PLAY_IMMEDIATE_NORMAL	7
-#define VID_PLAY_SWITCH_CHANNELS	8
-#define VID_PLAY_FREEZE_FRAME		9
-#define VID_PLAY_STILL_MODE		10
-#define VID_PLAY_MASTER_MODE		11
-	/* p1: see below */
-#define		VID_PLAY_MASTER_NONE	1
-#define		VID_PLAY_MASTER_VIDEO	2
-#define		VID_PLAY_MASTER_AUDIO	3
-#define VID_PLAY_ACTIVE_SCANLINES	12
-	/* p1 = first active; p2 = last active */
-#define VID_PLAY_RESET			13
-#define VID_PLAY_END_MARK		14
-
-#endif /* __LINUX_VIDEODEV_H */
-
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/staging/tm6000/tm6000-alsa.c b/drivers/staging/tm6000/tm6000-alsa.c
index 184cc505ed86..acb03172a887 100644
--- a/drivers/staging/tm6000/tm6000-alsa.c
+++ b/drivers/staging/tm6000/tm6000-alsa.c
@@ -76,14 +76,11 @@ MODULE_PARM_DESC(debug, "enable debug messages");
 static int _tm6000_start_audio_dma(struct snd_tm6000_card *chip)
 {
 	struct tm6000_core *core = chip->core;
-	int val;
 
 	dprintk(1, "Starting audio DMA\n");
 
 	/* Enables audio */
-	val = tm6000_get_reg(core, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, 0x0);
-	val |= 0x20;
-	tm6000_set_reg(core, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, val);
+	tm6000_set_reg_mask(core, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, 0x40, 0x40);
 
 	tm6000_set_audio_bitrate(core, 48000);
 
@@ -98,13 +95,11 @@ static int _tm6000_start_audio_dma(struct snd_tm6000_card *chip)
 static int _tm6000_stop_audio_dma(struct snd_tm6000_card *chip)
 {
 	struct tm6000_core *core = chip->core;
-	int val;
+
 	dprintk(1, "Stopping audio DMA\n");
 
-	/* Enables audio */
-	val = tm6000_get_reg(core, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, 0x0);
-	val &= ~0x20;
-	tm6000_set_reg(core, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, val);
+	/* Disables audio */
+	tm6000_set_reg_mask(core, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, 0x00, 0x40);
 
 	tm6000_set_reg(core, TM6010_REQ08_R01_A_INIT, 0);
 
diff --git a/drivers/staging/tm6000/tm6000-cards.c b/drivers/staging/tm6000/tm6000-cards.c
index 455038bdfc9f..88144a12745d 100644
--- a/drivers/staging/tm6000/tm6000-cards.c
+++ b/drivers/staging/tm6000/tm6000-cards.c
@@ -50,6 +50,9 @@
 #define TM6010_BOARD_BEHOLD_VOYAGER		11
 #define TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE	12
 #define TM6010_BOARD_TWINHAN_TU501		13
+#define TM6010_BOARD_BEHOLD_WANDER_LITE		14
+#define TM6010_BOARD_BEHOLD_VOYAGER_LITE	15
+#define TM5600_BOARD_TERRATEC_GRABSTER		16
 
 #define TM6000_MAXBOARDS        16
 static unsigned int card[]     = {[0 ... (TM6000_MAXBOARDS - 1)] = UNSET };
@@ -63,6 +66,8 @@ struct tm6000_board {
 	char            *name;
 
 	struct tm6000_capabilities caps;
+	enum            tm6000_inaudio aradio;
+	enum            tm6000_inaudio avideo;
 
 	enum		tm6000_devtype type;	/* variant of the chipset */
 	int             tuner_type;     /* type of the tuner */
@@ -227,6 +232,8 @@ struct tm6000_board tm6000_boards[] = {
 		.tuner_addr   = 0xc2 >> 1,
 		.demod_addr   = 0x1e >> 1,
 		.type         = TM6010,
+		.avideo       = TM6000_AIP_SIF1,
+		.aradio       = TM6000_AIP_LINE1,
 		.caps = {
 			.has_tuner    = 1,
 			.has_dvb      = 1,
@@ -245,6 +252,8 @@ struct tm6000_board tm6000_boards[] = {
 		.tuner_type   = TUNER_XC5000,
 		.tuner_addr   = 0xc2 >> 1,
 		.type         = TM6010,
+		.avideo       = TM6000_AIP_SIF1,
+		.aradio       = TM6000_AIP_LINE1,
 		.caps = {
 			.has_tuner    = 1,
 			.has_dvb      = 0,
@@ -281,6 +290,11 @@ struct tm6000_board tm6000_boards[] = {
 		},
 		.ir_codes = RC_MAP_NEC_TERRATEC_CINERGY_XS,
 	},
+	[TM5600_BOARD_TERRATEC_GRABSTER] = {
+		.name         = "Terratec Grabster AV 150/250 MX",
+		.type         = TM5600,
+		.tuner_type   = TUNER_ABSENT,
+	},
 	[TM6010_BOARD_TWINHAN_TU501] = {
 		.name         = "Twinhan TU501(704D1)",
 		.tuner_type   = TUNER_XC2028, /* has a XC3028 */
@@ -303,7 +317,45 @@ struct tm6000_board tm6000_boards[] = {
 			.dvb_led	= TM6010_GPIO_5,
 			.ir		= TM6010_GPIO_0,
 		},
-	}
+	},
+	[TM6010_BOARD_BEHOLD_WANDER_LITE] = {
+		.name         = "Beholder Wander Lite DVB-T/TV/FM USB2.0",
+		.tuner_type   = TUNER_XC5000,
+		.tuner_addr   = 0xc2 >> 1,
+		.demod_addr   = 0x1e >> 1,
+		.type         = TM6010,
+		.avideo       = TM6000_AIP_SIF1,
+		.aradio       = TM6000_AIP_LINE1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 1,
+			.has_zl10353  = 1,
+			.has_eeprom   = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_0,
+			.demod_reset	= TM6010_GPIO_1,
+			.power_led	= TM6010_GPIO_6,
+		},
+	},
+	[TM6010_BOARD_BEHOLD_VOYAGER_LITE] = {
+		.name         = "Beholder Voyager Lite TV/FM USB2.0",
+		.tuner_type   = TUNER_XC5000,
+		.tuner_addr   = 0xc2 >> 1,
+		.type         = TM6010,
+		.avideo       = TM6000_AIP_SIF1,
+		.aradio       = TM6000_AIP_LINE1,
+		.caps = {
+			.has_tuner    = 1,
+			.has_dvb      = 0,
+			.has_zl10353  = 0,
+			.has_eeprom   = 1,
+		},
+		.gpio = {
+			.tuner_reset	= TM6010_GPIO_0,
+			.power_led	= TM6010_GPIO_6,
+		},
+	},
 };
 
 /* table of devices that work with this driver */
@@ -321,10 +373,13 @@ struct usb_device_id tm6000_id_table[] = {
 	{ USB_DEVICE(0x6000, 0xdec1), .driver_info = TM6010_BOARD_BEHOLD_VOYAGER },
 	{ USB_DEVICE(0x0ccd, 0x0086), .driver_info = TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE },
 	{ USB_DEVICE(0x0ccd, 0x00A5), .driver_info = TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE },
+	{ USB_DEVICE(0x0ccd, 0x0079), .driver_info = TM5600_BOARD_TERRATEC_GRABSTER },
 	{ USB_DEVICE(0x13d3, 0x3240), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
 	{ USB_DEVICE(0x13d3, 0x3241), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
 	{ USB_DEVICE(0x13d3, 0x3243), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
 	{ USB_DEVICE(0x13d3, 0x3264), .driver_info = TM6010_BOARD_TWINHAN_TU501 },
+	{ USB_DEVICE(0x6000, 0xdec2), .driver_info = TM6010_BOARD_BEHOLD_WANDER_LITE },
+	{ USB_DEVICE(0x6000, 0xdec3), .driver_info = TM6010_BOARD_BEHOLD_VOYAGER_LITE },
 	{ },
 };
 
@@ -346,6 +401,8 @@ void tm6000_flash_led(struct tm6000_core *dev, u8 state)
 			break;
 		case TM6010_BOARD_BEHOLD_WANDER:
 		case TM6010_BOARD_BEHOLD_VOYAGER:
+		case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
 			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
 				dev->gpio.power_led, 0x01);
 			break;
@@ -362,6 +419,8 @@ void tm6000_flash_led(struct tm6000_core *dev, u8 state)
 			break;
 		case TM6010_BOARD_BEHOLD_WANDER:
 		case TM6010_BOARD_BEHOLD_VOYAGER:
+		case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
 			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
 				dev->gpio.power_led, 0x00);
 			break;
@@ -520,6 +579,7 @@ int tm6000_cards_setup(struct tm6000_core *dev)
 		msleep(15);
 		break;
 	case TM6010_BOARD_BEHOLD_WANDER:
+	case TM6010_BOARD_BEHOLD_WANDER_LITE:
 		/* Power led on (blue) */
 		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.power_led, 0x01);
 		msleep(15);
@@ -530,6 +590,7 @@ int tm6000_cards_setup(struct tm6000_core *dev)
 		msleep(15);
 		break;
 	case TM6010_BOARD_BEHOLD_VOYAGER:
+	case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
 		/* Power led on (blue) */
 		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, dev->gpio.power_led, 0x01);
 		msleep(15);
@@ -588,8 +649,6 @@ static void tm6000_config_tuner(struct tm6000_core *dev)
 	tun_setup.mode_mask = 0;
 	if (dev->caps.has_tuner)
 		tun_setup.mode_mask |= (T_ANALOG_TV | T_RADIO);
-	if (dev->caps.has_dvb)
-		tun_setup.mode_mask |= T_DIGITAL_TV;
 
 	switch (dev->tuner_type) {
 	case TUNER_XC2028:
@@ -644,13 +703,12 @@ static void tm6000_config_tuner(struct tm6000_core *dev)
 		struct xc5000_config ctl = {
 			.i2c_address = dev->tuner_addr,
 			.if_khz      = 4570,
-			.radio_input = XC5000_RADIO_FM1,
+			.radio_input = XC5000_RADIO_FM1_MONO,
 			};
 
 		xc5000_cfg.tuner = TUNER_XC5000;
 		xc5000_cfg.priv  = &ctl;
 
-
 		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config,
 				     &xc5000_cfg);
 		}
@@ -683,6 +741,8 @@ static int tm6000_init_dev(struct tm6000_core *dev)
 
 	dev->caps = tm6000_boards[dev->model].caps;
 
+	dev->avideo = tm6000_boards[dev->model].avideo;
+	dev->aradio = tm6000_boards[dev->model].aradio;
 	/* initialize hardware */
 	rc = tm6000_init(dev);
 	if (rc < 0)
@@ -957,6 +1017,8 @@ static void tm6000_usb_disconnect(struct usb_interface *interface)
 			break;
 		case TM6010_BOARD_BEHOLD_WANDER:
 		case TM6010_BOARD_BEHOLD_VOYAGER:
+		case TM6010_BOARD_BEHOLD_WANDER_LITE:
+		case TM6010_BOARD_BEHOLD_VOYAGER_LITE:
 			/* Power led off */
 			tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
 				dev->gpio.power_led, 0x00);
diff --git a/drivers/staging/tm6000/tm6000-core.c b/drivers/staging/tm6000/tm6000-core.c
index 96aed4ace467..778e53413afb 100644
--- a/drivers/staging/tm6000/tm6000-core.c
+++ b/drivers/staging/tm6000/tm6000-core.c
@@ -116,6 +116,29 @@ int tm6000_get_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index)
 }
 EXPORT_SYMBOL_GPL(tm6000_get_reg);
 
+int tm6000_set_reg_mask(struct tm6000_core *dev, u8 req, u16 value,
+						u16 index, u16 mask)
+{
+	int rc;
+	u8 buf[1];
+	u8 new_index;
+
+	rc = tm6000_read_write_usb(dev, USB_DIR_IN | USB_TYPE_VENDOR, req,
+					value, index, buf, 1);
+
+	if (rc < 0)
+		return rc;
+
+	new_index = (buf[0] & ~mask) | (index & mask);
+
+	if (new_index == index)
+		return 0;
+
+	return tm6000_read_write_usb(dev, USB_DIR_OUT | USB_TYPE_VENDOR,
+				      req, value, new_index, NULL, 0);
+}
+EXPORT_SYMBOL_GPL(tm6000_set_reg_mask);
+
 int tm6000_get_reg16(struct tm6000_core *dev, u8 req, u16 value, u16 index)
 {
 	int rc;
@@ -245,17 +268,12 @@ int tm6000_init_analog_mode(struct tm6000_core *dev)
 	struct v4l2_frequency f;
 
 	if (dev->dev_type == TM6010) {
-		int val;
-
 		/* Enable video */
-		val = tm6000_get_reg(dev, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, 0);
-		val |= 0x60;
-		tm6000_set_reg(dev, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF, val);
-		val = tm6000_get_reg(dev,
-			TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0);
-		val &= ~0x40;
-		tm6000_set_reg(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, val);
 
+		tm6000_set_reg_mask(dev, TM6010_REQ07_RCC_ACTIVE_VIDEO_IF,
+							0x60, 0x60);
+		tm6000_set_reg_mask(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE,
+							0x00, 0x40);
 		tm6000_set_reg(dev, TM6010_REQ08_RF1_AADC_POWER_DOWN, 0xfc);
 
 	} else {
@@ -268,11 +286,11 @@ int tm6000_init_analog_mode(struct tm6000_core *dev)
 			tm6000_set_reg(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0x80);
 
 		tm6000_set_reg(dev, TM6010_REQ07_RC3_HSTART1, 0x88);
-		tm6000_set_reg(dev, TM6010_REQ07_RD8_IR_WAKEUP_SEL, 0x23);
+		tm6000_set_reg(dev, TM6000_REQ07_RDA_CLK_SEL, 0x23);
 		tm6000_set_reg(dev, TM6010_REQ07_RD1_ADDR_FOR_REQ1, 0xc0);
 		tm6000_set_reg(dev, TM6010_REQ07_RD2_ADDR_FOR_REQ2, 0xd8);
 		tm6000_set_reg(dev, TM6010_REQ07_RD6_ENDP_REQ1_REQ2, 0x06);
-		tm6000_set_reg(dev, TM6010_REQ07_RD8_IR_PULSE_CNT0, 0x1f);
+		tm6000_set_reg(dev, TM6000_REQ07_RDF_PWDOWN_ACLK, 0x1f);
 
 		/* AP Software reset */
 		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x08);
@@ -284,8 +302,8 @@ int tm6000_init_analog_mode(struct tm6000_core *dev)
 		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x00);
 
 		/* E3: Select input 0 - TV tuner */
-		tm6000_set_reg(dev, TM6010_REQ07_RE3_OUT_SEL1, 0x00);
-		tm6000_set_reg(dev, REQ_07_SET_GET_AVREG, 0xeb, 0x60);
+		tm6000_set_reg(dev, TM6000_REQ07_RE3_VADC_INP_LPF_SEL1, 0x00);
+		tm6000_set_reg(dev, TM6000_REQ07_REB_VADC_AADC_MODE, 0x60);
 
 		/* This controls input */
 		tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, TM6000_GPIO_2, 0x0);
@@ -344,21 +362,21 @@ int tm6000_init_digital_mode(struct tm6000_core *dev)
 		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x08);
 		tm6000_set_reg(dev, TM6010_REQ07_RFF_SOFT_RESET, 0x00);
 		tm6000_set_reg(dev, TM6010_REQ07_R3F_RESET, 0x01);
-		tm6000_set_reg(dev, TM6010_REQ07_RD8_IR_PULSE_CNT0, 0x08);
-		tm6000_set_reg(dev, TM6010_REQ07_RE2_OUT_SEL2, 0x0c);
-		tm6000_set_reg(dev, TM6010_REQ07_RE8_TYPESEL_MOS_I2S, 0xff);
-		tm6000_set_reg(dev, REQ_07_SET_GET_AVREG, 0x00eb, 0xd8);
+		tm6000_set_reg(dev, TM6000_REQ07_RDF_PWDOWN_ACLK, 0x08);
+		tm6000_set_reg(dev, TM6000_REQ07_RE2_VADC_STATUS_CTL, 0x0c);
+		tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0xff);
+		tm6000_set_reg(dev, TM6000_REQ07_REB_VADC_AADC_MODE, 0xd8);
 		tm6000_set_reg(dev, TM6010_REQ07_RC0_ACTIVE_VIDEO_SOURCE, 0x40);
 		tm6000_set_reg(dev, TM6010_REQ07_RC1_TRESHOLD, 0xd0);
 		tm6000_set_reg(dev, TM6010_REQ07_RC3_HSTART1, 0x09);
-		tm6000_set_reg(dev, TM6010_REQ07_RD8_IR_WAKEUP_SEL, 0x37);
+		tm6000_set_reg(dev, TM6000_REQ07_RDA_CLK_SEL, 0x37);
 		tm6000_set_reg(dev, TM6010_REQ07_RD1_ADDR_FOR_REQ1, 0xd8);
 		tm6000_set_reg(dev, TM6010_REQ07_RD2_ADDR_FOR_REQ2, 0xc0);
 		tm6000_set_reg(dev, TM6010_REQ07_RD6_ENDP_REQ1_REQ2, 0x60);
 
-		tm6000_set_reg(dev, TM6010_REQ07_RE2_OUT_SEL2, 0x0c);
-		tm6000_set_reg(dev, TM6010_REQ07_RE8_TYPESEL_MOS_I2S, 0xff);
-		tm6000_set_reg(dev, REQ_07_SET_GET_AVREG, 0x00eb, 0x08);
+		tm6000_set_reg(dev, TM6000_REQ07_RE2_VADC_STATUS_CTL, 0x0c);
+		tm6000_set_reg(dev, TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0xff);
+		tm6000_set_reg(dev, TM6000_REQ07_REB_VADC_AADC_MODE, 0x08);
 		msleep(50);
 
 		tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT, 0x0020, 0x00);
@@ -388,18 +406,19 @@ struct reg_init {
 /* The meaning of those initializations are unknown */
 struct reg_init tm6000_init_tab[] = {
 	/* REG  VALUE */
-	{ TM6010_REQ07_RD8_IR_PULSE_CNT0, 0x1f },
+	{ TM6000_REQ07_RDF_PWDOWN_ACLK, 0x1f },
 	{ TM6010_REQ07_RFF_SOFT_RESET, 0x08 },
 	{ TM6010_REQ07_RFF_SOFT_RESET, 0x00 },
 	{ TM6010_REQ07_RD5_POWERSAVE, 0x4f },
-	{ TM6010_REQ07_RD8_IR_WAKEUP_SEL, 0x23 },
-	{ TM6010_REQ07_RD8_IR_WAKEUP_ADD, 0x08 },
-	{ TM6010_REQ07_RE2_OUT_SEL2, 0x00 },
-	{ TM6010_REQ07_RE3_OUT_SEL1, 0x10 },
-	{ TM6010_REQ07_RE5_REMOTE_WAKEUP, 0x00 },
-	{ TM6010_REQ07_RE8_TYPESEL_MOS_I2S, 0x00 },
-	{ REQ_07_SET_GET_AVREG,  0xeb, 0x64 },		/* 48000 bits/sample, external input */
-	{ REQ_07_SET_GET_AVREG,  0xee, 0xc2 },
+	{ TM6000_REQ07_RDA_CLK_SEL, 0x23 },
+	{ TM6000_REQ07_RDB_OUT_SEL, 0x08 },
+	{ TM6000_REQ07_RE2_VADC_STATUS_CTL, 0x00 },
+	{ TM6000_REQ07_RE3_VADC_INP_LPF_SEL1, 0x10 },
+	{ TM6000_REQ07_RE5_VADC_INP_LPF_SEL2, 0x00 },
+	{ TM6000_REQ07_RE8_VADC_PWDOWN_CTL, 0x00 },
+	{ TM6000_REQ07_REB_VADC_AADC_MODE, 0x64 },	/* 48000 bits/sample, external input */
+	{ TM6000_REQ07_REE_VADC_CTRL_SEL_CONTROL, 0xc2 },
+
 	{ TM6010_REQ07_R3F_RESET, 0x01 },		/* Start of soft reset */
 	{ TM6010_REQ07_R00_VIDEO_CONTROL0, 0x00 },
 	{ TM6010_REQ07_R01_VIDEO_CONTROL1, 0x07 },
@@ -470,6 +489,14 @@ struct reg_init tm6010_init_tab[] = {
 	{ TM6010_REQ08_REB_SIF_GAIN_CTRL, 0xf0 },
 	{ TM6010_REQ08_REC_REVERSE_YC_CTRL, 0xc2 },
 	{ TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG, 0x60 },
+	{ TM6010_REQ08_R03_A_AUTO_GAIN_CTRL, 0x00},
+	{ TM6010_REQ08_R04_A_SIF_AMP_CTRL, 0x80},
+	{ TM6010_REQ08_R0C_A_ASD_THRES2, 0x0a},
+	{ TM6010_REQ08_R0D_A_AMD_THRES, 0x40},
+	{ TM6010_REQ08_R1A_A_NICAM_SER_MAX, 0x64},
+	{ TM6010_REQ08_R1B_A_NICAM_SER_MIN, 0x20},
+	{ TM6010_REQ08_R16_A_AGC_GAIN_MAX, 0xfe},
+	{ TM6010_REQ08_R17_A_AGC_GAIN_MIN, 0x01},
 	{ TM6010_REQ08_RF1_AADC_POWER_DOWN, 0xfc },
 
 	{ TM6010_REQ07_R3F_RESET, 0x01 },
@@ -590,38 +617,213 @@ int tm6000_init(struct tm6000_core *dev)
 
 int tm6000_set_audio_bitrate(struct tm6000_core *dev, int bitrate)
 {
-	int val;
+	int val = 0;
+	u8 areg_f0 = 0x60; /* ADC MCLK = 250 Fs */
+	u8 areg_0a = 0x91; /* SIF 48KHz */
 
+	switch (bitrate) {
+	case 48000:
+		areg_f0 = 0x60; /* ADC MCLK = 250 Fs */
+		areg_0a = 0x91; /* SIF 48KHz */
+		dev->audio_bitrate = bitrate;
+		break;
+	case 32000:
+		areg_f0 = 0x00; /* ADC MCLK = 375 Fs */
+		areg_0a = 0x90; /* SIF 32KHz */
+		dev->audio_bitrate = bitrate;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+
+	/* enable I2S, if we use sif or external I2S device */
 	if (dev->dev_type == TM6010) {
-		val = tm6000_get_reg(dev, TM6010_REQ08_R0A_A_I2S_MOD, 0);
+		val = tm6000_set_reg(dev, TM6010_REQ08_R0A_A_I2S_MOD, areg_0a);
 		if (val < 0)
 			return val;
-		val = (val & 0xf0) | 0x1; /* 48 kHz, not muted */
-		val = tm6000_set_reg(dev, TM6010_REQ08_R0A_A_I2S_MOD, val);
+
+		val = tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+							areg_f0, 0xf0);
+		if (val < 0)
+			return val;
+	} else {
+		val = tm6000_set_reg_mask(dev, TM6000_REQ07_REB_VADC_AADC_MODE,
+							areg_f0, 0xf0);
 		if (val < 0)
 			return val;
 	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(tm6000_set_audio_bitrate);
 
-	val = tm6000_get_reg(dev, REQ_07_SET_GET_AVREG, 0xeb, 0x0);
-	if (val < 0)
-		return val;
+int tm6000_set_audio_input(struct tm6000_core *dev, enum tm6000_inaudio ainp)
+{
+	if (dev->dev_type == TM6010) {
+		/* Audio crossbar setting, default SIF1 */
+		u8 areg_f0 = 0x03;
 
-	val &= 0x0f;		/* Preserve the audio input control bits */
-	switch (bitrate) {
-	case 44100:
-		val |= 0xd0;
-		dev->audio_bitrate = bitrate;
+		switch (ainp) {
+		case TM6000_AIP_SIF1:
+		case TM6000_AIP_SIF2:
+			areg_f0 = 0x03;
+			break;
+		case TM6000_AIP_LINE1:
+			areg_f0 = 0x00;
+			break;
+		case TM6000_AIP_LINE2:
+			areg_f0 = 0x08;
+			break;
+		default:
+			return 0;
+			break;
+		}
+		/* Set audio input crossbar */
+		tm6000_set_reg_mask(dev, TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG,
+							areg_f0, 0x0f);
+	} else {
+		/* Audio setting, default LINE1 */
+		u8 areg_eb = 0x00;
+
+		switch (ainp) {
+		case TM6000_AIP_LINE1:
+			areg_eb = 0x00;
+			break;
+		case TM6000_AIP_LINE2:
+			areg_eb = 0x04;
+			break;
+		default:
+			return 0;
+			break;
+		}
+		/* Set audio input */
+		tm6000_set_reg_mask(dev, TM6000_REQ07_REB_VADC_AADC_MODE,
+							areg_eb, 0x0f);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(tm6000_set_audio_input);
+
+void tm6010_set_mute_sif(struct tm6000_core *dev, u8 mute)
+{
+	u8 mute_reg = 0;
+
+	if (mute)
+		mute_reg = 0x08;
+
+	tm6000_set_reg_mask(dev, TM6010_REQ08_R0A_A_I2S_MOD, mute_reg, 0x08);
+}
+
+void tm6010_set_mute_adc(struct tm6000_core *dev, u8 mute)
+{
+	u8 mute_reg = 0;
+
+	if (mute)
+		mute_reg = 0x20;
+
+	if (dev->dev_type == TM6010) {
+		tm6000_set_reg_mask(dev, TM6010_REQ08_RF2_LEFT_CHANNEL_VOL,
+							mute_reg, 0x20);
+		tm6000_set_reg_mask(dev, TM6010_REQ08_RF3_RIGHT_CHANNEL_VOL,
+							mute_reg, 0x20);
+	} else {
+		tm6000_set_reg_mask(dev, TM6000_REQ07_REC_VADC_AADC_LVOL,
+							mute_reg, 0x20);
+		tm6000_set_reg_mask(dev, TM6000_REQ07_RED_VADC_AADC_RVOL,
+							mute_reg, 0x20);
+	}
+}
+
+int tm6000_tvaudio_set_mute(struct tm6000_core *dev, u8 mute)
+{
+	enum tm6000_inaudio ainp;
+
+	if (dev->radio)
+		ainp = dev->aradio;
+	else
+		ainp = dev->avideo;
+
+	switch (ainp) {
+	case TM6000_AIP_SIF1:
+	case TM6000_AIP_SIF2:
+		if (dev->dev_type == TM6010)
+			tm6010_set_mute_sif(dev, mute);
+		else {
+			printk(KERN_INFO "ERROR: TM5600 and TM6000 don't has"
+					" SIF audio inputs. Please check the %s"
+					" configuration.\n", dev->name);
+			return -EINVAL;
+		}
 		break;
-	case 48000:
-		val |= 0x60;
-		dev->audio_bitrate = bitrate;
+	case TM6000_AIP_LINE1:
+	case TM6000_AIP_LINE2:
+		tm6010_set_mute_adc(dev, mute);
+		break;
+	default:
+		return -EINVAL;
 		break;
 	}
-	val = tm6000_set_reg(dev, REQ_07_SET_GET_AVREG, 0xeb, val);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(tm6000_tvaudio_set_mute);
+
+void tm6010_set_volume_sif(struct tm6000_core *dev, int vol)
+{
+	u8 vol_reg;
+
+	vol_reg = vol & 0x0F;
+
+	if (vol < 0)
+		vol_reg |= 0x40;
+
+	tm6000_set_reg(dev, TM6010_REQ08_R07_A_LEFT_VOL, vol_reg);
+	tm6000_set_reg(dev, TM6010_REQ08_R08_A_RIGHT_VOL, vol_reg);
+}
+
+void tm6010_set_volume_adc(struct tm6000_core *dev, int vol)
+{
+	u8 vol_reg;
+
+	vol_reg = (vol + 0x10) & 0x1f;
+
+	if (dev->dev_type == TM6010) {
+		tm6000_set_reg(dev, TM6010_REQ08_RF2_LEFT_CHANNEL_VOL, vol_reg);
+		tm6000_set_reg(dev, TM6010_REQ08_RF3_RIGHT_CHANNEL_VOL, vol_reg);
+	} else {
+		tm6000_set_reg(dev, TM6000_REQ07_REC_VADC_AADC_LVOL, vol_reg);
+		tm6000_set_reg(dev, TM6000_REQ07_RED_VADC_AADC_RVOL, vol_reg);
+	}
+}
+
+void tm6000_set_volume(struct tm6000_core *dev, int vol)
+{
+	enum tm6000_inaudio ainp;
+
+	if (dev->radio) {
+		ainp = dev->aradio;
+		vol += 8; /* Offset to 0 dB */
+	} else
+		ainp = dev->avideo;
 
-	return val;
+	switch (ainp) {
+	case TM6000_AIP_SIF1:
+	case TM6000_AIP_SIF2:
+		if (dev->dev_type == TM6010)
+			tm6010_set_volume_sif(dev, vol);
+		else
+			printk(KERN_INFO "ERROR: TM5600 and TM6000 don't has"
+					" SIF audio inputs. Please check the %s"
+					" configuration.\n", dev->name);
+		break;
+	case TM6000_AIP_LINE1:
+	case TM6000_AIP_LINE2:
+		tm6010_set_volume_adc(dev, vol);
+		break;
+	default:
+		break;
+	}
 }
-EXPORT_SYMBOL_GPL(tm6000_set_audio_bitrate);
+EXPORT_SYMBOL_GPL(tm6000_set_volume);
 
 static LIST_HEAD(tm6000_devlist);
 static DEFINE_MUTEX(tm6000_devlist_mutex);
diff --git a/drivers/staging/tm6000/tm6000-regs.h b/drivers/staging/tm6000/tm6000-regs.h
index 1f0ced8fa20f..5375a8347374 100644
--- a/drivers/staging/tm6000/tm6000-regs.h
+++ b/drivers/staging/tm6000/tm6000-regs.h
@@ -97,6 +97,34 @@ enum {
 	TM6000_URB_MSG_ERR,
 };
 
+/* Define specific TM6000 Video decoder registers */
+#define TM6000_REQ07_RD8_TEST_SEL			0x07, 0xd8
+#define TM6000_REQ07_RD9_A_SIM_SEL			0x07, 0xd9
+#define TM6000_REQ07_RDA_CLK_SEL			0x07, 0xda
+#define TM6000_REQ07_RDB_OUT_SEL			0x07, 0xdb
+#define TM6000_REQ07_RDC_NSEL_I2S			0x07, 0xdc
+#define TM6000_REQ07_RDD_GPIO2_MDRV			0x07, 0xdd
+#define TM6000_REQ07_RDE_GPIO1_MDRV			0x07, 0xde
+#define TM6000_REQ07_RDF_PWDOWN_ACLK			0x07, 0xdf
+#define TM6000_REQ07_RE0_VADC_REF_CTL			0x07, 0xe0
+#define TM6000_REQ07_RE1_VADC_DACLIMP			0x07, 0xe1
+#define TM6000_REQ07_RE2_VADC_STATUS_CTL		0x07, 0xe2
+#define TM6000_REQ07_RE3_VADC_INP_LPF_SEL1		0x07, 0xe3
+#define TM6000_REQ07_RE4_VADC_TARGET1			0x07, 0xe4
+#define TM6000_REQ07_RE5_VADC_INP_LPF_SEL2		0x07, 0xe5
+#define TM6000_REQ07_RE6_VADC_TARGET2			0x07, 0xe6
+#define TM6000_REQ07_RE7_VADC_AGAIN_CTL			0x07, 0xe7
+#define TM6000_REQ07_RE8_VADC_PWDOWN_CTL		0x07, 0xe8
+#define TM6000_REQ07_RE9_VADC_INPUT_CTL1		0x07, 0xe9
+#define TM6000_REQ07_REA_VADC_INPUT_CTL2		0x07, 0xea
+#define TM6000_REQ07_REB_VADC_AADC_MODE			0x07, 0xeb
+#define TM6000_REQ07_REC_VADC_AADC_LVOL			0x07, 0xec
+#define TM6000_REQ07_RED_VADC_AADC_RVOL			0x07, 0xed
+#define TM6000_REQ07_REE_VADC_CTRL_SEL_CONTROL		0x07, 0xee
+#define TM6000_REQ07_REF_VADC_GAIN_MAP_CTL		0x07, 0xef
+#define TM6000_REQ07_RFD_BIST_ERR_VST_LOW		0x07, 0xfd
+#define TM6000_REQ07_RFE_BIST_ERR_VST_HIGH		0x07, 0xfe
+
 /* Define TM6000/TM6010 Video decoder registers */
 #define TM6010_REQ07_R00_VIDEO_CONTROL0			0x07, 0x00
 #define TM6010_REQ07_R01_VIDEO_CONTROL1			0x07, 0x01
@@ -241,6 +269,7 @@ enum {
 #define TM6010_REQ07_RC9_VEND1				0x07, 0xc9
 #define TM6010_REQ07_RCA_VEND0				0x07, 0xca
 #define TM6010_REQ07_RCB_DELAY				0x07, 0xcb
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RCC_ACTIVE_VIDEO_IF		0x07, 0xcc
 #define TM6010_REQ07_RD0_USB_PERIPHERY_CONTROL		0x07, 0xd0
 #define TM6010_REQ07_RD1_ADDR_FOR_REQ1			0x07, 0xd1
@@ -250,32 +279,59 @@ enum {
 #define TM6010_REQ07_RD5_POWERSAVE			0x07, 0xd5
 #define TM6010_REQ07_RD6_ENDP_REQ1_REQ2			0x07, 0xd6
 #define TM6010_REQ07_RD7_ENDP_REQ3_REQ4			0x07, 0xd7
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR				0x07, 0xd8
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_BSIZE			0x07, 0xd9
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_WAKEUP_SEL			0x07, 0xda
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_WAKEUP_ADD			0x07, 0xdb
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_LEADER1			0x07, 0xdc
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_LEADER0			0x07, 0xdd
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_PULSE_CNT1			0x07, 0xde
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RD8_IR_PULSE_CNT0			0x07, 0xdf
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE0_DVIDEO_SOURCE			0x07, 0xe0
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE0_DVIDEO_SOURCE_IF		0x07, 0xe1
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE2_OUT_SEL2			0x07, 0xe2
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE3_OUT_SEL1			0x07, 0xe3
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE4_OUT_SEL0			0x07, 0xe4
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE5_REMOTE_WAKEUP			0x07, 0xe5
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE7_PUB_GPIO			0x07, 0xe7
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE8_TYPESEL_MOS_I2S		0x07, 0xe8
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RE9_TYPESEL_MOS_TS			0x07, 0xe9
+/* ONLY for TM6010 */
 #define TM6010_REQ07_REA_TYPESEL_MOS_CCIR		0x07, 0xea
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF0_BIST_CRC_RESULT0		0x07, 0xf0
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF1_BIST_CRC_RESULT1		0x07, 0xf1
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF2_BIST_CRC_RESULT2		0x07, 0xf2
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF3_BIST_CRC_RESULT3		0x07, 0xf3
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF4_BIST_ERR_VST2			0x07, 0xf4
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF5_BIST_ERR_VST1			0x07, 0xf5
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF6_BIST_ERR_VST0			0x07, 0xf6
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RF7_BIST				0x07, 0xf7
+/* ONLY for TM6010 */
 #define TM6010_REQ07_RFE_POWER_DOWN			0x07, 0xfe
 #define TM6010_REQ07_RFF_SOFT_RESET			0x07, 0xff
 
@@ -477,7 +533,8 @@ enum {
 #define TM6010_REQ05_RC4_DATA_FIFO14		0x05, 0xf8
 #define TM6010_REQ05_RC4_DATA_FIFO15		0x05, 0xfc
 
-/* Define TM6000/TM6010 Audio decoder registers */
+/* Define TM6010 Audio decoder registers */
+/* This core available only in TM6010 */
 #define TM6010_REQ08_R00_A_VERSION		0x08, 0x00
 #define TM6010_REQ08_R01_A_INIT			0x08, 0x01
 #define TM6010_REQ08_R02_A_FIX_GAIN_CTRL	0x08, 0x02
@@ -518,7 +575,7 @@ enum {
 #define TM6010_REQ08_R27_A_NOISE_AMP		0x08, 0x27
 #define TM6010_REQ08_R28_A_AUDIO_MODE_RES	0x08, 0x28
 
-/* Define TM6000/TM6010 Video ADC registers */
+/* Define TM6010 Video ADC registers */
 #define TM6010_REQ08_RE0_ADC_REF		0x08, 0xe0
 #define TM6010_REQ08_RE1_DAC_CLMP		0x08, 0xe1
 #define TM6010_REQ08_RE2_POWER_DOWN_CTRL1	0x08, 0xe2
@@ -534,7 +591,7 @@ enum {
 #define TM6010_REQ08_REC_REVERSE_YC_CTRL	0x08, 0xec
 #define TM6010_REQ08_RED_GAIN_SEL		0x08, 0xed
 
-/* Define TM6000/TM6010 Audio ADC registers */
+/* Define TM6010 Audio ADC registers */
 #define TM6010_REQ08_RF0_DAUDIO_INPUT_CONFIG	0x08, 0xf0
 #define TM6010_REQ08_RF1_AADC_POWER_DOWN	0x08, 0xf1
 #define TM6010_REQ08_RF2_LEFT_CHANNEL_VOL	0x08, 0xf2
diff --git a/drivers/staging/tm6000/tm6000-stds.c b/drivers/staging/tm6000/tm6000-stds.c
index cc7b8664fc20..a4c07e5a6f1d 100644
--- a/drivers/staging/tm6000/tm6000-stds.c
+++ b/drivers/staging/tm6000/tm6000-stds.c
@@ -952,6 +952,22 @@ static int tm6000_set_audio_std(struct tm6000_core *dev,
 	uint8_t mono_flag = 0;  /* No mono */
 	uint8_t nicam_flag = 0; /* No NICAM */
 
+	if (dev->radio) {
+		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x00);
+		tm6000_set_reg(dev, TM6010_REQ08_R02_A_FIX_GAIN_CTRL, 0x04);
+		tm6000_set_reg(dev, TM6010_REQ08_R03_A_AUTO_GAIN_CTRL, 0x00);
+		tm6000_set_reg(dev, TM6010_REQ08_R04_A_SIF_AMP_CTRL, 0x80);
+		tm6000_set_reg(dev, TM6010_REQ08_R05_A_STANDARD_MOD, 0x0c);
+		tm6000_set_reg(dev, TM6010_REQ08_R06_A_SOUND_MOD, 0x00);
+		tm6000_set_reg(dev, TM6010_REQ08_R09_A_MAIN_VOL, 0x18);
+		tm6000_set_reg(dev, TM6010_REQ08_R0C_A_ASD_THRES2, 0x0a);
+		tm6000_set_reg(dev, TM6010_REQ08_R0D_A_AMD_THRES, 0x40);
+		tm6000_set_reg(dev, TM6010_REQ08_RF1_AADC_POWER_DOWN, 0xfc);
+		tm6000_set_reg(dev, TM6010_REQ08_R1E_A_GAIN_DEEMPH_OUT, 0x13);
+		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x80);
+		return 0;
+	}
+
 	switch (std) {
 #if 0
 	case DK_MONO:
@@ -984,20 +1000,6 @@ static int tm6000_set_audio_std(struct tm6000_core *dev,
 	case EIAJ:
 		areg_05 = 0x02;
 		break;
-	case FM_RADIO:
-		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x00);
-		tm6000_set_reg(dev, TM6010_REQ08_R02_A_FIX_GAIN_CTRL, 0x04);
-		tm6000_set_reg(dev, TM6010_REQ08_R03_A_AUTO_GAIN_CTRL, 0x00);
-		tm6000_set_reg(dev, TM6010_REQ08_R05_A_STANDARD_MOD, 0x0c);
-		tm6000_set_reg(dev, TM6010_REQ08_R06_A_SOUND_MOD, 0x00);
-		tm6000_set_reg(dev, TM6010_REQ08_R09_A_MAIN_VOL, 0x18);
-		tm6000_set_reg(dev, TM6010_REQ08_R0A_A_I2S_MOD, 0x91);
-		tm6000_set_reg(dev, TM6010_REQ08_R16_A_AGC_GAIN_MAX, 0xfe);
-		tm6000_set_reg(dev, TM6010_REQ08_R17_A_AGC_GAIN_MIN, 0x01);
-		tm6000_set_reg(dev, TM6010_REQ08_R1E_A_GAIN_DEEMPH_OUT, 0x13);
-		tm6000_set_reg(dev, TM6010_REQ08_R01_A_INIT, 0x80);
-		return 0;
-		break;
 	case I_NICAM:
 		areg_05 = 0x08;
 		nicam_flag = 1;
@@ -1010,6 +1012,9 @@ static int tm6000_set_audio_std(struct tm6000_core *dev,
 		areg_05 = 0x0a;
 		nicam_flag = 1;
 		break;
+	default:
+		/* do nothink */
+		break;
 	}
 
 #if 0
diff --git a/drivers/staging/tm6000/tm6000-video.c b/drivers/staging/tm6000/tm6000-video.c
index eb9b9f1bc138..b5503409ca55 100644
--- a/drivers/staging/tm6000/tm6000-video.c
+++ b/drivers/staging/tm6000/tm6000-video.c
@@ -53,11 +53,17 @@
 /* Declare static vars that will be used as parameters */
 static unsigned int vid_limit = 16;	/* Video memory limit, in Mb */
 static int video_nr = -1;		/* /dev/videoN, -1 for autodetect */
+static int radio_nr = -1;		/* /dev/radioN, -1 for autodetect */
 
 /* Debug level */
 int tm6000_debug;
 EXPORT_SYMBOL_GPL(tm6000_debug);
 
+static const struct v4l2_queryctrl no_ctrl = {
+	.name  = "42",
+	.flags = V4L2_CTRL_FLAG_DISABLED,
+};
+
 /* supported controls */
 static struct v4l2_queryctrl tm6000_qctrl[] = {
 	{
@@ -96,9 +102,26 @@ static struct v4l2_queryctrl tm6000_qctrl[] = {
 		.step          = 0x1,
 		.default_value = 0,
 		.flags         = 0,
+	},
+		/* --- audio --- */
+	{
+		.id            = V4L2_CID_AUDIO_MUTE,
+		.name          = "Mute",
+		.minimum       = 0,
+		.maximum       = 1,
+		.type          = V4L2_CTRL_TYPE_BOOLEAN,
+	}, {
+		.id            = V4L2_CID_AUDIO_VOLUME,
+		.name          = "Volume",
+		.minimum       = -15,
+		.maximum       = 15,
+		.step          = 1,
+		.default_value = 0,
+		.type          = V4L2_CTRL_TYPE_INTEGER,
 	}
 };
 
+static const unsigned int CTRLS = ARRAY_SIZE(tm6000_qctrl);
 static int qctl_regs[ARRAY_SIZE(tm6000_qctrl)];
 
 static struct tm6000_fmt format[] = {
@@ -117,6 +140,16 @@ static struct tm6000_fmt format[] = {
 	}
 };
 
+static const struct v4l2_queryctrl *ctrl_by_id(unsigned int id)
+{
+	unsigned int i;
+
+	for (i = 0; i < CTRLS; i++)
+		if (tm6000_qctrl[i].id == id)
+			return tm6000_qctrl+i;
+	return NULL;
+}
+
 /* ------------------------------------------------------------------
  *	DMA and thread functions
  * ------------------------------------------------------------------
@@ -199,13 +232,17 @@ static int copy_streams(u8 *data, unsigned long len,
 	char *voutp = NULL;
 	unsigned int linewidth;
 
-	/* get video buffer */
-	get_next_buf(dma_q, &vbuf);
-	if (!vbuf)
-		return rc;
-	voutp = videobuf_to_vmalloc(&vbuf->vb);
-	if (!voutp)
-		return 0;
+	if (!dev->radio) {
+		/* get video buffer */
+		get_next_buf(dma_q, &vbuf);
+
+		if (!vbuf)
+			return rc;
+		voutp = videobuf_to_vmalloc(&vbuf->vb);
+
+		if (!voutp)
+			return 0;
+	}
 
 	for (ptr = data; ptr < endp;) {
 		if (!dev->isoc_ctl.cmd) {
@@ -257,29 +294,31 @@ static int copy_streams(u8 *data, unsigned long len,
 			 */
 			switch (cmd) {
 			case TM6000_URB_MSG_VIDEO:
-				if ((dev->isoc_ctl.vfield != field) &&
-					(field == 1)) {
+				if (!dev->radio) {
+					if ((dev->isoc_ctl.vfield != field) &&
+						(field == 1)) {
 					/* Announces that a new buffer
 					 * were filled
 					 */
-					buffer_filled(dev, dma_q, vbuf);
-					dprintk(dev, V4L2_DEBUG_ISOC,
+						buffer_filled(dev, dma_q, vbuf);
+						dprintk(dev, V4L2_DEBUG_ISOC,
 							"new buffer filled\n");
-					get_next_buf(dma_q, &vbuf);
-					if (!vbuf)
-						return rc;
-					voutp = videobuf_to_vmalloc(&vbuf->vb);
-					if (!voutp)
-						return rc;
-					memset(voutp, 0, vbuf->vb.size);
-				}
-				linewidth = vbuf->vb.width << 1;
-				pos = ((line << 1) - field - 1) * linewidth +
-					block * TM6000_URB_MSG_LEN;
-				/* Don't allow to write out of the buffer */
-				if (pos + size > vbuf->vb.size)
-					cmd = TM6000_URB_MSG_ERR;
-				dev->isoc_ctl.vfield = field;
+						get_next_buf(dma_q, &vbuf);
+						if (!vbuf)
+							return rc;
+						voutp = videobuf_to_vmalloc(&vbuf->vb);
+						if (!voutp)
+							return rc;
+						memset(voutp, 0, vbuf->vb.size);
+					}
+					linewidth = vbuf->vb.width << 1;
+					pos = ((line << 1) - field - 1) *
+					linewidth + block * TM6000_URB_MSG_LEN;
+					/* Don't allow to write out of the buffer */
+					if (pos + size > vbuf->vb.size)
+						cmd = TM6000_URB_MSG_ERR;
+					dev->isoc_ctl.vfield = field;
+			}
 				break;
 			case TM6000_URB_MSG_VBI:
 				break;
@@ -537,7 +576,7 @@ static void tm6000_uninit_isoc(struct tm6000_core *dev)
 /*
  * Allocate URBs and start IRQ
  */
-static int tm6000_prepare_isoc(struct tm6000_core *dev, unsigned int framesize)
+static int tm6000_prepare_isoc(struct tm6000_core *dev)
 {
 	struct tm6000_dmaqueue *dma_q = &dev->vidq;
 	int i, j, sb_size, pipe, size, max_packets, num_bufs = 8;
@@ -566,11 +605,7 @@ static int tm6000_prepare_isoc(struct tm6000_core *dev, unsigned int framesize)
 
 	dev->isoc_ctl.max_pkt_size = size;
 
-	max_packets = (framesize + size - 1) / size;
-
-	if (max_packets > TM6000_MAX_ISO_PACKETS)
-		max_packets = TM6000_MAX_ISO_PACKETS;
-
+	max_packets = TM6000_MAX_ISO_PACKETS;
 	sb_size = max_packets * size;
 
 	dev->isoc_ctl.num_bufs = num_bufs;
@@ -746,7 +781,7 @@ buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 		urb_init = 1;
 
 	if (urb_init) {
-		rc = tm6000_prepare_isoc(dev, buf->vb.size);
+		rc = tm6000_prepare_isoc(dev);
 		if (rc < 0)
 			goto fail;
 
@@ -1143,6 +1178,12 @@ static int vidioc_g_ctrl(struct file *file, void *priv,
 	case V4L2_CID_HUE:
 		val = tm6000_get_reg(dev, TM6010_REQ07_R0B_CHROMA_HUE_PHASE_ADJ, 0);
 		return 0;
+	case V4L2_CID_AUDIO_MUTE:
+		val = dev->ctl_mute;
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		val = dev->ctl_volume;
+		return 0;
 	default:
 		return -EINVAL;
 	}
@@ -1174,6 +1215,14 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 	case V4L2_CID_HUE:
 		tm6000_set_reg(dev, TM6010_REQ07_R0B_CHROMA_HUE_PHASE_ADJ, val);
 		return 0;
+	case V4L2_CID_AUDIO_MUTE:
+		dev->ctl_mute = val;
+		tm6000_tvaudio_set_mute(dev, val);
+		return 0;
+	case V4L2_CID_AUDIO_VOLUME:
+		dev->ctl_volume = val;
+		tm6000_set_volume(dev, val);
+		return 0;
 	}
 	return -EINVAL;
 }
@@ -1221,7 +1270,7 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 	if (unlikely(UNSET == dev->tuner_type))
 		return -EINVAL;
 
-	f->type = V4L2_TUNER_ANALOG_TV;
+	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 	f->frequency = dev->freq;
 
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_frequency, f);
@@ -1235,13 +1284,14 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	struct tm6000_fh   *fh  = priv;
 	struct tm6000_core *dev = fh->dev;
 
-	if (unlikely(f->type != V4L2_TUNER_ANALOG_TV))
-		return -EINVAL;
-
 	if (unlikely(UNSET == dev->tuner_type))
 		return -EINVAL;
 	if (unlikely(f->tuner != 0))
 		return -EINVAL;
+	if (0 == fh->radio && V4L2_TUNER_ANALOG_TV != f->type)
+		return -EINVAL;
+	if (1 == fh->radio && V4L2_TUNER_RADIO != f->type)
+		return -EINVAL;
 
 	dev->freq = f->frequency;
 	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_frequency, f);
@@ -1249,6 +1299,122 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	return 0;
 }
 
+static int radio_querycap(struct file *file, void *priv,
+					struct v4l2_capability *cap)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+
+	strcpy(cap->driver, "tm6000");
+	strlcpy(cap->card, dev->name, sizeof(dev->name));
+	sprintf(cap->bus_info, "USB%04x:%04x",
+		le16_to_cpu(dev->udev->descriptor.idVendor),
+		le16_to_cpu(dev->udev->descriptor.idProduct));
+	cap->version = dev->dev_type;
+	cap->capabilities = V4L2_CAP_TUNER;
+
+	return 0;
+}
+
+static int radio_g_tuner(struct file *file, void *priv,
+					struct v4l2_tuner *t)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+
+	if (0 != t->index)
+		return -EINVAL;
+
+	memset(t, 0, sizeof(*t));
+	strcpy(t->name, "Radio");
+	t->type = V4L2_TUNER_RADIO;
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, g_tuner, t);
+
+	if ((dev->aradio == TM6000_AIP_LINE1) ||
+				(dev->aradio == TM6000_AIP_LINE2)) {
+		t->rxsubchans = V4L2_TUNER_SUB_MONO;
+	}
+	else {
+		t->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	}
+
+	return 0;
+}
+
+static int radio_s_tuner(struct file *file, void *priv,
+					struct v4l2_tuner *t)
+{
+	struct tm6000_fh *fh = file->private_data;
+	struct tm6000_core *dev = fh->dev;
+
+	if (0 != t->index)
+		return -EINVAL;
+
+	v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_tuner, t);
+
+	return 0;
+}
+
+static int radio_enum_input(struct file *file, void *priv,
+					struct v4l2_input *i)
+{
+	if (i->index != 0)
+		return -EINVAL;
+
+	strcpy(i->name, "Radio");
+	i->type = V4L2_INPUT_TYPE_TUNER;
+
+	return 0;
+}
+
+static int radio_g_input(struct file *filp, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int radio_g_audio(struct file *file, void *priv,
+					struct v4l2_audio *a)
+{
+	memset(a, 0, sizeof(*a));
+	strcpy(a->name, "Radio");
+	return 0;
+}
+
+static int radio_s_audio(struct file *file, void *priv,
+					struct v4l2_audio *a)
+{
+	return 0;
+}
+
+static int radio_s_input(struct file *filp, void *priv, unsigned int i)
+{
+	return 0;
+}
+
+static int radio_s_std(struct file *file, void *fh, v4l2_std_id *norm)
+{
+	return 0;
+}
+
+static int radio_queryctrl(struct file *file, void *priv,
+					struct v4l2_queryctrl *c)
+{
+	const struct v4l2_queryctrl *ctrl;
+
+	if (c->id <  V4L2_CID_BASE ||
+	    c->id >= V4L2_CID_LASTP1)
+		return -EINVAL;
+	if (c->id == V4L2_CID_AUDIO_MUTE) {
+		ctrl = ctrl_by_id(c->id);
+		*c = *ctrl;
+	} else
+		*c = no_ctrl;
+
+	return 0;
+}
+
 /* ------------------------------------------------------------------
 	File operations for the device
    ------------------------------------------------------------------*/
@@ -1260,6 +1426,7 @@ static int tm6000_open(struct file *file)
 	struct tm6000_fh *fh;
 	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	int i, rc;
+	int radio = 0;
 
 	printk(KERN_INFO "tm6000: open called (dev=%s)\n",
 		video_device_node_name(vdev));
@@ -1267,6 +1434,17 @@ static int tm6000_open(struct file *file)
 	dprintk(dev, V4L2_DEBUG_OPEN, "tm6000: open called (dev=%s)\n",
 		video_device_node_name(vdev));
 
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		break;
+	case VFL_TYPE_VBI:
+		type = V4L2_BUF_TYPE_VBI_CAPTURE;
+		break;
+	case VFL_TYPE_RADIO:
+		radio = 1;
+		break;
+	}
 
 	/* If more than one user, mutex should be added */
 	dev->users++;
@@ -1284,8 +1462,9 @@ static int tm6000_open(struct file *file)
 
 	file->private_data = fh;
 	fh->dev      = dev;
-
-	fh->type     = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fh->radio    = radio;
+	dev->radio   = radio;
+	fh->type     = type;
 	dev->fourcc  = format[0].fourcc;
 
 	fh->fmt      = format_by_fourcc(dev->fourcc);
@@ -1322,6 +1501,19 @@ static int tm6000_open(struct file *file)
 			V4L2_FIELD_INTERLACED,
 			sizeof(struct tm6000_buffer), fh, &dev->lock);
 
+	if (fh->radio) {
+		dprintk(dev, V4L2_DEBUG_OPEN, "video_open: setting radio device\n");
+		tm6000_set_audio_input(dev, dev->aradio);
+		tm6000_set_volume(dev, dev->ctl_volume);
+		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_radio);
+		tm6000_prepare_isoc(dev);
+		tm6000_start_thread(dev);
+	}
+	else {
+		tm6000_set_audio_input(dev, dev->avideo);
+		tm6000_set_volume(dev, dev->ctl_volume);
+	}
+
 	return 0;
 }
 
@@ -1445,6 +1637,36 @@ static struct video_device tm6000_template = {
 	.current_norm   = V4L2_STD_NTSC_M,
 };
 
+static const struct v4l2_file_operations radio_fops = {
+	.owner	  = THIS_MODULE,
+	.open	  = tm6000_open,
+	.release  = tm6000_release,
+	.ioctl	  = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops radio_ioctl_ops = {
+	.vidioc_querycap	= radio_querycap,
+	.vidioc_g_tuner		= radio_g_tuner,
+	.vidioc_enum_input	= radio_enum_input,
+	.vidioc_g_audio		= radio_g_audio,
+	.vidioc_s_tuner		= radio_s_tuner,
+	.vidioc_s_audio		= radio_s_audio,
+	.vidioc_s_input		= radio_s_input,
+	.vidioc_s_std		= radio_s_std,
+	.vidioc_queryctrl	= radio_queryctrl,
+	.vidioc_g_input		= radio_g_input,
+	.vidioc_g_ctrl		= vidioc_g_ctrl,
+	.vidioc_s_ctrl		= vidioc_s_ctrl,
+	.vidioc_g_frequency	= vidioc_g_frequency,
+	.vidioc_s_frequency	= vidioc_s_frequency,
+};
+
+struct video_device tm6000_radio_template = {
+	.name			= "tm6000",
+	.fops			= &radio_fops,
+	.ioctl_ops 		= &radio_ioctl_ops,
+};
+
 /* -----------------------------------------------------------------
  *	Initialization and module stuff
  * ------------------------------------------------------------------
@@ -1499,6 +1721,25 @@ int tm6000_v4l2_register(struct tm6000_core *dev)
 	printk(KERN_INFO "%s: registered device %s\n",
 	       dev->name, video_device_node_name(dev->vfd));
 
+	dev->radio_dev = vdev_init(dev, &tm6000_radio_template,
+						   "radio");
+	if (!dev->radio_dev) {
+		printk(KERN_INFO "%s: can't register radio device\n",
+		       dev->name);
+		return ret; /* FIXME release resource */
+	}
+
+	ret = video_register_device(dev->radio_dev, VFL_TYPE_RADIO,
+				    radio_nr);
+	if (ret < 0) {
+		printk(KERN_INFO "%s: can't register radio device\n",
+		       dev->name);
+		return ret; /* FIXME release resource */
+	}
+
+	printk(KERN_INFO "%s: registered device %s\n",
+	       dev->name, video_device_node_name(dev->radio_dev));
+
 	printk(KERN_INFO "Trident TVMaster TM5600/TM6000/TM6010 USB2 board (Load status: %d)\n", ret);
 	return ret;
 }
@@ -1507,6 +1748,14 @@ int tm6000_v4l2_unregister(struct tm6000_core *dev)
 {
 	video_unregister_device(dev->vfd);
 
+	if (dev->radio_dev) {
+		if (video_is_registered(dev->radio_dev))
+			video_unregister_device(dev->radio_dev);
+		else
+			video_device_release(dev->radio_dev);
+		dev->radio_dev = NULL;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/staging/tm6000/tm6000.h b/drivers/staging/tm6000/tm6000.h
index bf11eeec92c7..ccd120fe1340 100644
--- a/drivers/staging/tm6000/tm6000.h
+++ b/drivers/staging/tm6000/tm6000.h
@@ -53,6 +53,14 @@ enum tm6000_devtype {
 	TM6010,
 };
 
+enum tm6000_inaudio {
+	TM6000_AIP_UNK = 0,
+	TM6000_AIP_SIF1,
+	TM6000_AIP_SIF2,
+	TM6000_AIP_LINE1,
+	TM6000_AIP_LINE2,
+};
+
 /* ------------------------------------------------------------------
  *	Basic structures
  * ------------------------------------------------------------------
@@ -174,6 +182,8 @@ struct tm6000_core {
 
 	char				*ir_codes;
 
+	__u8				radio;
+
 	/* Demodulator configuration */
 	int				demod_addr;	/* demodulator address */
 
@@ -194,6 +204,7 @@ struct tm6000_core {
 	bool				is_res_read;
 
 	struct video_device		*vfd;
+	struct video_device		*radio_dev;
 	struct tm6000_dmaqueue		vidq;
 	struct v4l2_device		v4l2_dev;
 
@@ -203,6 +214,9 @@ struct tm6000_core {
 
 	enum tm6000_mode		mode;
 
+	int				ctl_mute;             /* audio */
+	int				ctl_volume;
+
 	/* DVB-T support */
 	struct tm6000_dvb		*dvb;
 
@@ -210,7 +224,8 @@ struct tm6000_core {
 	struct snd_tm6000_card		*adev;
 	struct work_struct		wq_trigger;   /* Trigger to start/stop audio for alsa module */
 	atomic_t			stream_started;  /* stream should be running if true */
-
+	enum tm6000_inaudio		avideo;
+	enum tm6000_inaudio		aradio;
 
 	struct tm6000_IR		*ir;
 
@@ -248,6 +263,7 @@ struct tm6000_ops {
 
 struct tm6000_fh {
 	struct tm6000_core           *dev;
+	unsigned int                 radio;
 
 	/* video capture */
 	struct tm6000_fmt            *fmt;
@@ -276,12 +292,17 @@ int tm6000_get_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index);
 int tm6000_get_reg16(struct tm6000_core *dev, u8 req, u16 value, u16 index);
 int tm6000_get_reg32(struct tm6000_core *dev, u8 req, u16 value, u16 index);
 int tm6000_set_reg(struct tm6000_core *dev, u8 req, u16 value, u16 index);
+int tm6000_set_reg_mask(struct tm6000_core *dev, u8 req, u16 value,
+						u16 index, u16 mask);
 int tm6000_i2c_reset(struct tm6000_core *dev, u16 tsleep);
 int tm6000_init(struct tm6000_core *dev);
 
 int tm6000_init_analog_mode(struct tm6000_core *dev);
 int tm6000_init_digital_mode(struct tm6000_core *dev);
 int tm6000_set_audio_bitrate(struct tm6000_core *dev, int bitrate);
+int tm6000_set_audio_input(struct tm6000_core *dev, enum tm6000_inaudio ainp);
+int tm6000_tvaudio_set_mute(struct tm6000_core *dev, u8 mute);
+void tm6000_set_volume(struct tm6000_core *dev, int vol);
 
 int tm6000_v4l2_register(struct tm6000_core *dev);
 int tm6000_v4l2_unregister(struct tm6000_core *dev);
diff --git a/drivers/staging/usbvideo/Kconfig b/drivers/staging/usbvideo/Kconfig
deleted file mode 100644
index 566d659e6ff3..000000000000
--- a/drivers/staging/usbvideo/Kconfig
+++ /dev/null
@@ -1,15 +0,0 @@
-config VIDEO_USBVIDEO
-	tristate
-
-config USB_VICAM
-	tristate "USB 3com HomeConnect (aka vicam) support (DEPRECATED)"
-	depends on VIDEO_DEV && VIDEO_V4L2_COMMON && USB
-	select VIDEO_USBVIDEO
-	---help---
-	  Say Y here if you have 3com homeconnect camera (vicam).
-
-	  This driver uses the deprecated V4L1 API and will be removed in
-	  2.6.39, unless someone converts it to the V4L2 API.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called vicam.
diff --git a/drivers/staging/usbvideo/Makefile b/drivers/staging/usbvideo/Makefile
deleted file mode 100644
index 3c99a9a2d8d3..000000000000
--- a/drivers/staging/usbvideo/Makefile
+++ /dev/null
@@ -1,2 +0,0 @@
-obj-$(CONFIG_VIDEO_USBVIDEO)    += usbvideo.o
-obj-$(CONFIG_USB_VICAM)         += vicam.o
diff --git a/drivers/staging/usbvideo/TODO b/drivers/staging/usbvideo/TODO
deleted file mode 100644
index 3b2c03836286..000000000000
--- a/drivers/staging/usbvideo/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-This is an obsolete driver for some old webcams that still use V4L1 API. 
-As V4L1 support is being removed from kernel, if nobody take care on it, 
-the driver will be removed for 2.6.39.
-
-Please send patches to linux-media@vger.kernel.org
diff --git a/drivers/staging/usbvideo/usbvideo.c b/drivers/staging/usbvideo/usbvideo.c
deleted file mode 100644
index f1fcf9744961..000000000000
--- a/drivers/staging/usbvideo/usbvideo.c
+++ /dev/null
@@ -1,2230 +0,0 @@
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/list.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/mm.h>
-#include <linux/vmalloc.h>
-#include <linux/init.h>
-#include <linux/spinlock.h>
-
-#include <asm/io.h>
-
-#include "usbvideo.h"
-
-#if defined(MAP_NR)
-#define	virt_to_page(v)	MAP_NR(v)	/* Kernels 2.2.x */
-#endif
-
-static int video_nr = -1;
-module_param(video_nr, int, 0);
-
-/*
- * Local prototypes.
- */
-static void usbvideo_Disconnect(struct usb_interface *intf);
-static void usbvideo_CameraRelease(struct uvd *uvd);
-
-static long usbvideo_v4l_ioctl(struct file *file,
-			      unsigned int cmd, unsigned long arg);
-static int usbvideo_v4l_mmap(struct file *file, struct vm_area_struct *vma);
-static int usbvideo_v4l_open(struct file *file);
-static ssize_t usbvideo_v4l_read(struct file *file, char __user *buf,
-			     size_t count, loff_t *ppos);
-static int usbvideo_v4l_close(struct file *file);
-
-static int usbvideo_StartDataPump(struct uvd *uvd);
-static void usbvideo_StopDataPump(struct uvd *uvd);
-static int usbvideo_GetFrame(struct uvd *uvd, int frameNum);
-static int usbvideo_NewFrame(struct uvd *uvd, int framenum);
-static void usbvideo_SoftwareContrastAdjustment(struct uvd *uvd,
-						struct usbvideo_frame *frame);
-
-/*******************************/
-/* Memory management functions */
-/*******************************/
-static void *usbvideo_rvmalloc(unsigned long size)
-{
-	void *mem;
-	unsigned long adr;
-
-	size = PAGE_ALIGN(size);
-	mem = vmalloc_32(size);
-	if (!mem)
-		return NULL;
-
-	memset(mem, 0, size); /* Clear the ram out, no junk to the user */
-	adr = (unsigned long) mem;
-	while (size > 0) {
-		SetPageReserved(vmalloc_to_page((void *)adr));
-		adr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-
-	return mem;
-}
-
-static void usbvideo_rvfree(void *mem, unsigned long size)
-{
-	unsigned long adr;
-
-	if (!mem)
-		return;
-
-	adr = (unsigned long) mem;
-	while ((long) size > 0) {
-		ClearPageReserved(vmalloc_to_page((void *)adr));
-		adr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-	vfree(mem);
-}
-
-static void RingQueue_Initialize(struct RingQueue *rq)
-{
-	assert(rq != NULL);
-	init_waitqueue_head(&rq->wqh);
-}
-
-static void RingQueue_Allocate(struct RingQueue *rq, int rqLen)
-{
-	/* Make sure the requested size is a power of 2 and
-	   round up if necessary. This allows index wrapping
-	   using masks rather than modulo */
-
-	int i = 1;
-	assert(rq != NULL);
-	assert(rqLen > 0);
-
-	while(rqLen >> i)
-		i++;
-	if(rqLen != 1 << (i-1))
-		rqLen = 1 << i;
-
-	rq->length = rqLen;
-	rq->ri = rq->wi = 0;
-	rq->queue = usbvideo_rvmalloc(rq->length);
-	assert(rq->queue != NULL);
-}
-
-static int RingQueue_IsAllocated(const struct RingQueue *rq)
-{
-	if (rq == NULL)
-		return 0;
-	return (rq->queue != NULL) && (rq->length > 0);
-}
-
-static void RingQueue_Free(struct RingQueue *rq)
-{
-	assert(rq != NULL);
-	if (RingQueue_IsAllocated(rq)) {
-		usbvideo_rvfree(rq->queue, rq->length);
-		rq->queue = NULL;
-		rq->length = 0;
-	}
-}
-
-int RingQueue_Dequeue(struct RingQueue *rq, unsigned char *dst, int len)
-{
-	int rql, toread;
-
-	assert(rq != NULL);
-	assert(dst != NULL);
-
-	rql = RingQueue_GetLength(rq);
-	if(!rql)
-		return 0;
-
-	/* Clip requested length to available data */
-	if(len > rql)
-		len = rql;
-
-	toread = len;
-	if(rq->ri > rq->wi) {
-		/* Read data from tail */
-		int read = (toread < (rq->length - rq->ri)) ? toread : rq->length - rq->ri;
-		memcpy(dst, rq->queue + rq->ri, read);
-		toread -= read;
-		dst += read;
-		rq->ri = (rq->ri + read) & (rq->length-1);
-	}
-	if(toread) {
-		/* Read data from head */
-		memcpy(dst, rq->queue + rq->ri, toread);
-		rq->ri = (rq->ri + toread) & (rq->length-1);
-	}
-	return len;
-}
-
-EXPORT_SYMBOL(RingQueue_Dequeue);
-
-int RingQueue_Enqueue(struct RingQueue *rq, const unsigned char *cdata, int n)
-{
-	int enqueued = 0;
-
-	assert(rq != NULL);
-	assert(cdata != NULL);
-	assert(rq->length > 0);
-	while (n > 0) {
-		int m, q_avail;
-
-		/* Calculate the largest chunk that fits the tail of the ring */
-		q_avail = rq->length - rq->wi;
-		if (q_avail <= 0) {
-			rq->wi = 0;
-			q_avail = rq->length;
-		}
-		m = n;
-		assert(q_avail > 0);
-		if (m > q_avail)
-			m = q_avail;
-
-		memcpy(rq->queue + rq->wi, cdata, m);
-		RING_QUEUE_ADVANCE_INDEX(rq, wi, m);
-		cdata += m;
-		enqueued += m;
-		n -= m;
-	}
-	return enqueued;
-}
-
-EXPORT_SYMBOL(RingQueue_Enqueue);
-
-static void RingQueue_InterruptibleSleepOn(struct RingQueue *rq)
-{
-	assert(rq != NULL);
-	interruptible_sleep_on(&rq->wqh);
-}
-
-void RingQueue_WakeUpInterruptible(struct RingQueue *rq)
-{
-	assert(rq != NULL);
-	if (waitqueue_active(&rq->wqh))
-		wake_up_interruptible(&rq->wqh);
-}
-
-EXPORT_SYMBOL(RingQueue_WakeUpInterruptible);
-
-void RingQueue_Flush(struct RingQueue *rq)
-{
-	assert(rq != NULL);
-	rq->ri = 0;
-	rq->wi = 0;
-}
-
-EXPORT_SYMBOL(RingQueue_Flush);
-
-
-/*
- * usbvideo_VideosizeToString()
- *
- * This procedure converts given videosize value to readable string.
- *
- * History:
- * 07-Aug-2000 Created.
- * 19-Oct-2000 Reworked for usbvideo module.
- */
-static void usbvideo_VideosizeToString(char *buf, int bufLen, videosize_t vs)
-{
-	char tmp[40];
-	int n;
-
-	n = 1 + sprintf(tmp, "%ldx%ld", VIDEOSIZE_X(vs), VIDEOSIZE_Y(vs));
-	assert(n < sizeof(tmp));
-	if ((buf == NULL) || (bufLen < n))
-		err("usbvideo_VideosizeToString: buffer is too small.");
-	else
-		memmove(buf, tmp, n);
-}
-
-/*
- * usbvideo_OverlayChar()
- *
- * History:
- * 01-Feb-2000 Created.
- */
-static void usbvideo_OverlayChar(struct uvd *uvd, struct usbvideo_frame *frame,
-				 int x, int y, int ch)
-{
-	static const unsigned short digits[16] = {
-		0xF6DE, /* 0 */
-		0x2492, /* 1 */
-		0xE7CE, /* 2 */
-		0xE79E, /* 3 */
-		0xB792, /* 4 */
-		0xF39E, /* 5 */
-		0xF3DE, /* 6 */
-		0xF492, /* 7 */
-		0xF7DE, /* 8 */
-		0xF79E, /* 9 */
-		0x77DA, /* a */
-		0xD75C, /* b */
-		0xF24E, /* c */
-		0xD6DC, /* d */
-		0xF34E, /* e */
-		0xF348  /* f */
-	};
-	unsigned short digit;
-	int ix, iy;
-	int value;
-
-	if ((uvd == NULL) || (frame == NULL))
-		return;
-
-	value = hex_to_bin(ch);
-	if (value < 0)
-		return;
-	digit = digits[value];
-
-	for (iy=0; iy < 5; iy++) {
-		for (ix=0; ix < 3; ix++) {
-			if (digit & 0x8000) {
-				if (uvd->paletteBits & (1L << VIDEO_PALETTE_RGB24)) {
-/* TODO */				RGB24_PUTPIXEL(frame, x+ix, y+iy, 0xFF, 0xFF, 0xFF);
-				}
-			}
-			digit = digit << 1;
-		}
-	}
-}
-
-/*
- * usbvideo_OverlayString()
- *
- * History:
- * 01-Feb-2000 Created.
- */
-static void usbvideo_OverlayString(struct uvd *uvd, struct usbvideo_frame *frame,
-				   int x, int y, const char *str)
-{
-	while (*str) {
-		usbvideo_OverlayChar(uvd, frame, x, y, *str);
-		str++;
-		x += 4; /* 3 pixels character + 1 space */
-	}
-}
-
-/*
- * usbvideo_OverlayStats()
- *
- * Overlays important debugging information.
- *
- * History:
- * 01-Feb-2000 Created.
- */
-static void usbvideo_OverlayStats(struct uvd *uvd, struct usbvideo_frame *frame)
-{
-	const int y_diff = 8;
-	char tmp[16];
-	int x = 10, y=10;
-	long i, j, barLength;
-	const int qi_x1 = 60, qi_y1 = 10;
-	const int qi_x2 = VIDEOSIZE_X(frame->request) - 10, qi_h = 10;
-
-	/* Call the user callback, see if we may proceed after that */
-	if (VALID_CALLBACK(uvd, overlayHook)) {
-		if (GET_CALLBACK(uvd, overlayHook)(uvd, frame) < 0)
-			return;
-	}
-
-	/*
-	 * We draw a (mostly) hollow rectangle with qi_xxx coordinates.
-	 * Left edge symbolizes the queue index 0; right edge symbolizes
-	 * the full capacity of the queue.
-	 */
-	barLength = qi_x2 - qi_x1 - 2;
-	if ((barLength > 10) && (uvd->paletteBits & (1L << VIDEO_PALETTE_RGB24))) {
-/* TODO */	long u_lo, u_hi, q_used;
-		long m_ri, m_wi, m_lo, m_hi;
-
-		/*
-		 * Determine fill zones (used areas of the queue):
-		 * 0 xxxxxxx u_lo ...... uvd->dp.ri xxxxxxxx u_hi ..... uvd->dp.length
-		 *
-		 * if u_lo < 0 then there is no first filler.
-		 */
-
-		q_used = RingQueue_GetLength(&uvd->dp);
-		if ((uvd->dp.ri + q_used) >= uvd->dp.length) {
-			u_hi = uvd->dp.length;
-			u_lo = (q_used + uvd->dp.ri) & (uvd->dp.length-1);
-		} else {
-			u_hi = (q_used + uvd->dp.ri);
-			u_lo = -1;
-		}
-
-		/* Convert byte indices into screen units */
-		m_ri = qi_x1 + ((barLength * uvd->dp.ri) / uvd->dp.length);
-		m_wi = qi_x1 + ((barLength * uvd->dp.wi) / uvd->dp.length);
-		m_lo = (u_lo > 0) ? (qi_x1 + ((barLength * u_lo) / uvd->dp.length)) : -1;
-		m_hi = qi_x1 + ((barLength * u_hi) / uvd->dp.length);
-
-		for (j=qi_y1; j < (qi_y1 + qi_h); j++) {
-			for (i=qi_x1; i < qi_x2; i++) {
-				/* Draw border lines */
-				if ((j == qi_y1) || (j == (qi_y1 + qi_h - 1)) ||
-				    (i == qi_x1) || (i == (qi_x2 - 1))) {
-					RGB24_PUTPIXEL(frame, i, j, 0xFF, 0xFF, 0xFF);
-					continue;
-				}
-				/* For all other points the Y coordinate does not matter */
-				if ((i >= m_ri) && (i <= (m_ri + 3))) {
-					RGB24_PUTPIXEL(frame, i, j, 0x00, 0xFF, 0x00);
-				} else if ((i >= m_wi) && (i <= (m_wi + 3))) {
-					RGB24_PUTPIXEL(frame, i, j, 0xFF, 0x00, 0x00);
-				} else if ((i < m_lo) || ((i > m_ri) && (i < m_hi)))
-					RGB24_PUTPIXEL(frame, i, j, 0x00, 0x00, 0xFF);
-			}
-		}
-	}
-
-	sprintf(tmp, "%8lx", uvd->stats.frame_num);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8lx", uvd->stats.urb_count);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8lx", uvd->stats.urb_length);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8lx", uvd->stats.data_count);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8lx", uvd->stats.header_count);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8lx", uvd->stats.iso_skip_count);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8lx", uvd->stats.iso_err_count);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8x", uvd->vpic.colour);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8x", uvd->vpic.hue);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8x", uvd->vpic.brightness >> 8);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8x", uvd->vpic.contrast >> 12);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-
-	sprintf(tmp, "%8d", uvd->vpic.whiteness >> 8);
-	usbvideo_OverlayString(uvd, frame, x, y, tmp);
-	y += y_diff;
-}
-
-/*
- * usbvideo_ReportStatistics()
- *
- * This procedure prints packet and transfer statistics.
- *
- * History:
- * 14-Jan-2000 Corrected default multiplier.
- */
-static void usbvideo_ReportStatistics(const struct uvd *uvd)
-{
-	if ((uvd != NULL) && (uvd->stats.urb_count > 0)) {
-		unsigned long allPackets, badPackets, goodPackets, percent;
-		allPackets = uvd->stats.urb_count * CAMERA_URB_FRAMES;
-		badPackets = uvd->stats.iso_skip_count + uvd->stats.iso_err_count;
-		goodPackets = allPackets - badPackets;
-		/* Calculate percentage wisely, remember integer limits */
-		assert(allPackets != 0);
-		if (goodPackets < (((unsigned long)-1)/100))
-			percent = (100 * goodPackets) / allPackets;
-		else
-			percent = goodPackets / (allPackets / 100);
-		dev_info(&uvd->dev->dev,
-			 "Packet Statistics: Total=%lu. Empty=%lu. Usage=%lu%%\n",
-			 allPackets, badPackets, percent);
-		if (uvd->iso_packet_len > 0) {
-			unsigned long allBytes, xferBytes;
-			char multiplier = ' ';
-			allBytes = allPackets * uvd->iso_packet_len;
-			xferBytes = uvd->stats.data_count;
-			assert(allBytes != 0);
-			if (xferBytes < (((unsigned long)-1)/100))
-				percent = (100 * xferBytes) / allBytes;
-			else
-				percent = xferBytes / (allBytes / 100);
-			/* Scale xferBytes for easy reading */
-			if (xferBytes > 10*1024) {
-				xferBytes /= 1024;
-				multiplier = 'K';
-				if (xferBytes > 10*1024) {
-					xferBytes /= 1024;
-					multiplier = 'M';
-					if (xferBytes > 10*1024) {
-						xferBytes /= 1024;
-						multiplier = 'G';
-						if (xferBytes > 10*1024) {
-							xferBytes /= 1024;
-							multiplier = 'T';
-						}
-					}
-				}
-			}
-			dev_info(&uvd->dev->dev,
-				 "Transfer Statistics: Transferred=%lu%cB Usage=%lu%%\n",
-				 xferBytes, multiplier, percent);
-		}
-	}
-}
-
-/*
- * usbvideo_TestPattern()
- *
- * Procedure forms a test pattern (yellow grid on blue background).
- *
- * Parameters:
- * fullframe: if TRUE then entire frame is filled, otherwise the procedure
- *	      continues from the current scanline.
- * pmode      0: fill the frame with solid blue color (like on VCR or TV)
- *	      1: Draw a colored grid
- *
- * History:
- * 01-Feb-2000 Created.
- */
-void usbvideo_TestPattern(struct uvd *uvd, int fullframe, int pmode)
-{
-	struct usbvideo_frame *frame;
-	int num_cell = 0;
-	int scan_length = 0;
-	static int num_pass;
-
-	if (uvd == NULL) {
-		err("%s: uvd == NULL", __func__);
-		return;
-	}
-	if ((uvd->curframe < 0) || (uvd->curframe >= USBVIDEO_NUMFRAMES)) {
-		err("%s: uvd->curframe=%d.", __func__, uvd->curframe);
-		return;
-	}
-
-	/* Grab the current frame */
-	frame = &uvd->frame[uvd->curframe];
-
-	/* Optionally start at the beginning */
-	if (fullframe) {
-		frame->curline = 0;
-		frame->seqRead_Length = 0;
-	}
-#if 0
-	{	/* For debugging purposes only */
-		char tmp[20];
-		usbvideo_VideosizeToString(tmp, sizeof(tmp), frame->request);
-		dev_info(&uvd->dev->dev, "testpattern: frame=%s\n", tmp);
-	}
-#endif
-	/* Form every scan line */
-	for (; frame->curline < VIDEOSIZE_Y(frame->request); frame->curline++) {
-		int i;
-		unsigned char *f = frame->data +
-			(VIDEOSIZE_X(frame->request) * V4L_BYTES_PER_PIXEL * frame->curline);
-		for (i=0; i < VIDEOSIZE_X(frame->request); i++) {
-			unsigned char cb=0x80;
-			unsigned char cg = 0;
-			unsigned char cr = 0;
-
-			if (pmode == 1) {
-				if (frame->curline % 32 == 0)
-					cb = 0, cg = cr = 0xFF;
-				else if (i % 32 == 0) {
-					if (frame->curline % 32 == 1)
-						num_cell++;
-					cb = 0, cg = cr = 0xFF;
-				} else {
-					cb = ((num_cell*7) + num_pass) & 0xFF;
-					cg = ((num_cell*5) + num_pass*2) & 0xFF;
-					cr = ((num_cell*3) + num_pass*3) & 0xFF;
-				}
-			} else {
-				/* Just the blue screen */
-			}
-
-			*f++ = cb;
-			*f++ = cg;
-			*f++ = cr;
-			scan_length += 3;
-		}
-	}
-
-	frame->frameState = FrameState_Done;
-	frame->seqRead_Length += scan_length;
-	++num_pass;
-
-	/* We do this unconditionally, regardless of FLAGS_OVERLAY_STATS */
-	usbvideo_OverlayStats(uvd, frame);
-}
-
-EXPORT_SYMBOL(usbvideo_TestPattern);
-
-
-#ifdef DEBUG
-/*
- * usbvideo_HexDump()
- *
- * A debugging tool. Prints hex dumps.
- *
- * History:
- * 29-Jul-2000 Added printing of offsets.
- */
-void usbvideo_HexDump(const unsigned char *data, int len)
-{
-	const int bytes_per_line = 32;
-	char tmp[128]; /* 32*3 + 5 */
-	int i, k;
-
-	for (i=k=0; len > 0; i++, len--) {
-		if (i > 0 && ((i % bytes_per_line) == 0)) {
-			printk("%s\n", tmp);
-			k=0;
-		}
-		if ((i % bytes_per_line) == 0)
-			k += sprintf(&tmp[k], "%04x: ", i);
-		k += sprintf(&tmp[k], "%02x ", data[i]);
-	}
-	if (k > 0)
-		printk("%s\n", tmp);
-}
-
-EXPORT_SYMBOL(usbvideo_HexDump);
-
-#endif
-
-/* ******************************************************************** */
-
-/* XXX: this piece of crap really wants some error handling.. */
-static int usbvideo_ClientIncModCount(struct uvd *uvd)
-{
-	if (uvd == NULL) {
-		err("%s: uvd == NULL", __func__);
-		return -EINVAL;
-	}
-	if (uvd->handle == NULL) {
-		err("%s: uvd->handle == NULL", __func__);
-		return -EINVAL;
-	}
-	if (!try_module_get(uvd->handle->md_module)) {
-		err("%s: try_module_get() == 0", __func__);
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void usbvideo_ClientDecModCount(struct uvd *uvd)
-{
-	if (uvd == NULL) {
-		err("%s: uvd == NULL", __func__);
-		return;
-	}
-	if (uvd->handle == NULL) {
-		err("%s: uvd->handle == NULL", __func__);
-		return;
-	}
-	if (uvd->handle->md_module == NULL) {
-		err("%s: uvd->handle->md_module == NULL", __func__);
-		return;
-	}
-	module_put(uvd->handle->md_module);
-}
-
-int usbvideo_register(
-	struct usbvideo **pCams,
-	const int num_cams,
-	const int num_extra,
-	const char *driverName,
-	const struct usbvideo_cb *cbTbl,
-	struct module *md,
-	const struct usb_device_id *id_table)
-{
-	struct usbvideo *cams;
-	int i, base_size, result;
-
-	/* Check parameters for sanity */
-	if ((num_cams <= 0) || (pCams == NULL) || (cbTbl == NULL)) {
-		err("%s: Illegal call", __func__);
-		return -EINVAL;
-	}
-
-	/* Check registration callback - must be set! */
-	if (cbTbl->probe == NULL) {
-		err("%s: probe() is required!", __func__);
-		return -EINVAL;
-	}
-
-	base_size = num_cams * sizeof(struct uvd) + sizeof(struct usbvideo);
-	cams = kzalloc(base_size, GFP_KERNEL);
-	if (cams == NULL) {
-		err("Failed to allocate %d. bytes for usbvideo struct", base_size);
-		return -ENOMEM;
-	}
-	dbg("%s: Allocated $%p (%d. bytes) for %d. cameras",
-	    __func__, cams, base_size, num_cams);
-
-	/* Copy callbacks, apply defaults for those that are not set */
-	memmove(&cams->cb, cbTbl, sizeof(cams->cb));
-	if (cams->cb.getFrame == NULL)
-		cams->cb.getFrame = usbvideo_GetFrame;
-	if (cams->cb.disconnect == NULL)
-		cams->cb.disconnect = usbvideo_Disconnect;
-	if (cams->cb.startDataPump == NULL)
-		cams->cb.startDataPump = usbvideo_StartDataPump;
-	if (cams->cb.stopDataPump == NULL)
-		cams->cb.stopDataPump = usbvideo_StopDataPump;
-
-	cams->num_cameras = num_cams;
-	cams->cam = (struct uvd *) &cams[1];
-	cams->md_module = md;
-	mutex_init(&cams->lock);	/* to 1 == available */
-
-	for (i = 0; i < num_cams; i++) {
-		struct uvd *up = &cams->cam[i];
-
-		up->handle = cams;
-
-		/* Allocate user_data separately because of kmalloc's limits */
-		if (num_extra > 0) {
-			up->user_size = num_cams * num_extra;
-			up->user_data = kmalloc(up->user_size, GFP_KERNEL);
-			if (up->user_data == NULL) {
-				err("%s: Failed to allocate user_data (%d. bytes)",
-				    __func__, up->user_size);
-				while (i) {
-					up = &cams->cam[--i];
-					kfree(up->user_data);
-				}
-				kfree(cams);
-				return -ENOMEM;
-			}
-			dbg("%s: Allocated cams[%d].user_data=$%p (%d. bytes)",
-			     __func__, i, up->user_data, up->user_size);
-		}
-	}
-
-	/*
-	 * Register ourselves with USB stack.
-	 */
-	strcpy(cams->drvName, (driverName != NULL) ? driverName : "Unknown");
-	cams->usbdrv.name = cams->drvName;
-	cams->usbdrv.probe = cams->cb.probe;
-	cams->usbdrv.disconnect = cams->cb.disconnect;
-	cams->usbdrv.id_table = id_table;
-
-	/*
-	 * Update global handle to usbvideo. This is very important
-	 * because probe() can be called before usb_register() returns.
-	 * If the handle is not yet updated then the probe() will fail.
-	 */
-	*pCams = cams;
-	result = usb_register(&cams->usbdrv);
-	if (result) {
-		for (i = 0; i < num_cams; i++) {
-			struct uvd *up = &cams->cam[i];
-			kfree(up->user_data);
-		}
-		kfree(cams);
-	}
-
-	return result;
-}
-
-EXPORT_SYMBOL(usbvideo_register);
-
-/*
- * usbvideo_Deregister()
- *
- * Procedure frees all usbvideo and user data structures. Be warned that
- * if you had some dynamically allocated components in ->user field then
- * you should free them before calling here.
- */
-void usbvideo_Deregister(struct usbvideo **pCams)
-{
-	struct usbvideo *cams;
-	int i;
-
-	if (pCams == NULL) {
-		err("%s: pCams == NULL", __func__);
-		return;
-	}
-	cams = *pCams;
-	if (cams == NULL) {
-		err("%s: cams == NULL", __func__);
-		return;
-	}
-
-	dbg("%s: Deregistering %s driver.", __func__, cams->drvName);
-	usb_deregister(&cams->usbdrv);
-
-	dbg("%s: Deallocating cams=$%p (%d. cameras)", __func__, cams, cams->num_cameras);
-	for (i=0; i < cams->num_cameras; i++) {
-		struct uvd *up = &cams->cam[i];
-		int warning = 0;
-
-		if (up->user_data != NULL) {
-			if (up->user_size <= 0)
-				++warning;
-		} else {
-			if (up->user_size > 0)
-				++warning;
-		}
-		if (warning) {
-			err("%s: Warning: user_data=$%p user_size=%d.",
-			    __func__, up->user_data, up->user_size);
-		} else {
-			dbg("%s: Freeing %d. $%p->user_data=$%p",
-			    __func__, i, up, up->user_data);
-			kfree(up->user_data);
-		}
-	}
-	/* Whole array was allocated in one chunk */
-	dbg("%s: Freed %d uvd structures",
-	    __func__, cams->num_cameras);
-	kfree(cams);
-	*pCams = NULL;
-}
-
-EXPORT_SYMBOL(usbvideo_Deregister);
-
-/*
- * usbvideo_Disconnect()
- *
- * This procedure stops all driver activity. Deallocation of
- * the interface-private structure (pointed by 'ptr') is done now
- * (if we don't have any open files) or later, when those files
- * are closed. After that driver should be removable.
- *
- * This code handles surprise removal. The uvd->user is a counter which
- * increments on open() and decrements on close(). If we see here that
- * this counter is not 0 then we have a client who still has us opened.
- * We set uvd->remove_pending flag as early as possible, and after that
- * all access to the camera will gracefully fail. These failures should
- * prompt client to (eventually) close the video device, and then - in
- * usbvideo_v4l_close() - we decrement uvd->uvd_used and usage counter.
- *
- * History:
- * 22-Jan-2000 Added polling of MOD_IN_USE to delay removal until all users gone.
- * 27-Jan-2000 Reworked to allow pending disconnects; see xxx_close()
- * 24-May-2000 Corrected to prevent race condition (MOD_xxx_USE_COUNT).
- * 19-Oct-2000 Moved to usbvideo module.
- */
-static void usbvideo_Disconnect(struct usb_interface *intf)
-{
-	struct uvd *uvd = usb_get_intfdata (intf);
-	int i;
-
-	if (uvd == NULL) {
-		err("%s($%p): Illegal call.", __func__, intf);
-		return;
-	}
-
-	usb_set_intfdata (intf, NULL);
-
-	usbvideo_ClientIncModCount(uvd);
-	if (uvd->debug > 0)
-		dev_info(&intf->dev, "%s(%p.)\n", __func__, intf);
-
-	mutex_lock(&uvd->lock);
-	uvd->remove_pending = 1; /* Now all ISO data will be ignored */
-
-	/* At this time we ask to cancel outstanding URBs */
-	GET_CALLBACK(uvd, stopDataPump)(uvd);
-
-	for (i=0; i < USBVIDEO_NUMSBUF; i++)
-		usb_free_urb(uvd->sbuf[i].urb);
-
-	usb_put_dev(uvd->dev);
-	uvd->dev = NULL;    	    /* USB device is no more */
-
-	video_unregister_device(&uvd->vdev);
-	if (uvd->debug > 0)
-		dev_info(&intf->dev, "%s: Video unregistered.\n", __func__);
-
-	if (uvd->user)
-		dev_info(&intf->dev, "%s: In use, disconnect pending.\n",
-			 __func__);
-	else
-		usbvideo_CameraRelease(uvd);
-	mutex_unlock(&uvd->lock);
-	dev_info(&intf->dev, "USB camera disconnected.\n");
-
-	usbvideo_ClientDecModCount(uvd);
-}
-
-/*
- * usbvideo_CameraRelease()
- *
- * This code does final release of uvd. This happens
- * after the device is disconnected -and- all clients
- * closed their files.
- *
- * History:
- * 27-Jan-2000 Created.
- */
-static void usbvideo_CameraRelease(struct uvd *uvd)
-{
-	if (uvd == NULL) {
-		err("%s: Illegal call", __func__);
-		return;
-	}
-
-	RingQueue_Free(&uvd->dp);
-	if (VALID_CALLBACK(uvd, userFree))
-		GET_CALLBACK(uvd, userFree)(uvd);
-	uvd->uvd_used = 0;	/* This is atomic, no need to take mutex */
-}
-
-/*
- * usbvideo_find_struct()
- *
- * This code searches the array of preallocated (static) structures
- * and returns index of the first one that isn't in use. Returns -1
- * if there are no free structures.
- *
- * History:
- * 27-Jan-2000 Created.
- */
-static int usbvideo_find_struct(struct usbvideo *cams)
-{
-	int u, rv = -1;
-
-	if (cams == NULL) {
-		err("No usbvideo handle?");
-		return -1;
-	}
-	mutex_lock(&cams->lock);
-	for (u = 0; u < cams->num_cameras; u++) {
-		struct uvd *uvd = &cams->cam[u];
-		if (!uvd->uvd_used) /* This one is free */
-		{
-			uvd->uvd_used = 1;	/* In use now */
-			mutex_init(&uvd->lock);	/* to 1 == available */
-			uvd->dev = NULL;
-			rv = u;
-			break;
-		}
-	}
-	mutex_unlock(&cams->lock);
-	return rv;
-}
-
-static const struct v4l2_file_operations usbvideo_fops = {
-	.owner =  THIS_MODULE,
-	.open =   usbvideo_v4l_open,
-	.release =usbvideo_v4l_close,
-	.read =   usbvideo_v4l_read,
-	.mmap =   usbvideo_v4l_mmap,
-	.ioctl =  usbvideo_v4l_ioctl,
-};
-static const struct video_device usbvideo_template = {
-	.fops =       &usbvideo_fops,
-};
-
-struct uvd *usbvideo_AllocateDevice(struct usbvideo *cams)
-{
-	int i, devnum;
-	struct uvd *uvd = NULL;
-
-	if (cams == NULL) {
-		err("No usbvideo handle?");
-		return NULL;
-	}
-
-	devnum = usbvideo_find_struct(cams);
-	if (devnum == -1) {
-		err("IBM USB camera driver: Too many devices!");
-		return NULL;
-	}
-	uvd = &cams->cam[devnum];
-	dbg("Device entry #%d. at $%p", devnum, uvd);
-
-	/* Not relying upon caller we increase module counter ourselves */
-	usbvideo_ClientIncModCount(uvd);
-
-	mutex_lock(&uvd->lock);
-	for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-		uvd->sbuf[i].urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
-		if (uvd->sbuf[i].urb == NULL) {
-			err("usb_alloc_urb(%d.) failed.", FRAMES_PER_DESC);
-			uvd->uvd_used = 0;
-			uvd = NULL;
-			goto allocate_done;
-		}
-	}
-	uvd->user=0;
-	uvd->remove_pending = 0;
-	uvd->last_error = 0;
-	RingQueue_Initialize(&uvd->dp);
-
-	/* Initialize video device structure */
-	uvd->vdev = usbvideo_template;
-	sprintf(uvd->vdev.name, "%.20s USB Camera", cams->drvName);
-	/*
-	 * The client is free to overwrite those because we
-	 * return control to the client's probe function right now.
-	 */
-allocate_done:
-	mutex_unlock(&uvd->lock);
-	usbvideo_ClientDecModCount(uvd);
-	return uvd;
-}
-
-EXPORT_SYMBOL(usbvideo_AllocateDevice);
-
-int usbvideo_RegisterVideoDevice(struct uvd *uvd)
-{
-	char tmp1[20], tmp2[20];	/* Buffers for printing */
-
-	if (uvd == NULL) {
-		err("%s: Illegal call.", __func__);
-		return -EINVAL;
-	}
-	if (uvd->video_endp == 0) {
-		dev_info(&uvd->dev->dev,
-			 "%s: No video endpoint specified; data pump disabled.\n",
-			 __func__);
-	}
-	if (uvd->paletteBits == 0) {
-		err("%s: No palettes specified!", __func__);
-		return -EINVAL;
-	}
-	if (uvd->defaultPalette == 0) {
-		dev_info(&uvd->dev->dev, "%s: No default palette!\n",
-			 __func__);
-	}
-
-	uvd->max_frame_size = VIDEOSIZE_X(uvd->canvas) *
-		VIDEOSIZE_Y(uvd->canvas) * V4L_BYTES_PER_PIXEL;
-	usbvideo_VideosizeToString(tmp1, sizeof(tmp1), uvd->videosize);
-	usbvideo_VideosizeToString(tmp2, sizeof(tmp2), uvd->canvas);
-
-	if (uvd->debug > 0) {
-		dev_info(&uvd->dev->dev,
-			 "%s: iface=%d. endpoint=$%02x paletteBits=$%08lx\n",
-			 __func__, uvd->iface, uvd->video_endp,
-			 uvd->paletteBits);
-	}
-	if (uvd->dev == NULL) {
-		err("%s: uvd->dev == NULL", __func__);
-		return -EINVAL;
-	}
-	uvd->vdev.parent = &uvd->dev->dev;
-	uvd->vdev.release = video_device_release_empty;
-	if (video_register_device(&uvd->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
-		err("%s: video_register_device failed", __func__);
-		return -EPIPE;
-	}
-	if (uvd->debug > 1) {
-		dev_info(&uvd->dev->dev,
-			 "%s: video_register_device() successful\n", __func__);
-	}
-
-	dev_info(&uvd->dev->dev, "%s on %s: canvas=%s videosize=%s\n",
-		 (uvd->handle != NULL) ? uvd->handle->drvName : "???",
-		 video_device_node_name(&uvd->vdev), tmp2, tmp1);
-
-	usb_get_dev(uvd->dev);
-	return 0;
-}
-
-EXPORT_SYMBOL(usbvideo_RegisterVideoDevice);
-
-/* ******************************************************************** */
-
-static int usbvideo_v4l_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct uvd *uvd = file->private_data;
-	unsigned long start = vma->vm_start;
-	unsigned long size  = vma->vm_end-vma->vm_start;
-	unsigned long page, pos;
-
-	if (!CAMERA_IS_OPERATIONAL(uvd))
-		return -EFAULT;
-
-	if (size > (((USBVIDEO_NUMFRAMES * uvd->max_frame_size) + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1)))
-		return -EINVAL;
-
-	pos = (unsigned long) uvd->fbuf;
-	while (size > 0) {
-		page = vmalloc_to_pfn((void *)pos);
-		if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED))
-			return -EAGAIN;
-
-		start += PAGE_SIZE;
-		pos += PAGE_SIZE;
-		if (size > PAGE_SIZE)
-			size -= PAGE_SIZE;
-		else
-			size = 0;
-	}
-
-	return 0;
-}
-
-/*
- * usbvideo_v4l_open()
- *
- * This is part of Video 4 Linux API. The driver can be opened by one
- * client only (checks internal counter 'uvdser'). The procedure
- * then allocates buffers needed for video processing.
- *
- * History:
- * 22-Jan-2000 Rewrote, moved scratch buffer allocation here. Now the
- *             camera is also initialized here (once per connect), at
- *             expense of V4L client (it waits on open() call).
- * 27-Jan-2000 Used USBVIDEO_NUMSBUF as number of URB buffers.
- * 24-May-2000 Corrected to prevent race condition (MOD_xxx_USE_COUNT).
- */
-static int usbvideo_v4l_open(struct file *file)
-{
-	struct video_device *dev = video_devdata(file);
-	struct uvd *uvd = (struct uvd *) dev;
-	const int sb_size = FRAMES_PER_DESC * uvd->iso_packet_len;
-	int i, errCode = 0;
-
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "%s($%p)\n", __func__, dev);
-
-	if (usbvideo_ClientIncModCount(uvd) < 0)
-		return -ENODEV;
-	mutex_lock(&uvd->lock);
-
-	if (uvd->user) {
-		err("%s: Someone tried to open an already opened device!", __func__);
-		errCode = -EBUSY;
-	} else {
-		/* Clear statistics */
-		memset(&uvd->stats, 0, sizeof(uvd->stats));
-
-		/* Clean pointers so we know if we allocated something */
-		for (i=0; i < USBVIDEO_NUMSBUF; i++)
-			uvd->sbuf[i].data = NULL;
-
-		/* Allocate memory for the frame buffers */
-		uvd->fbuf_size = USBVIDEO_NUMFRAMES * uvd->max_frame_size;
-		uvd->fbuf = usbvideo_rvmalloc(uvd->fbuf_size);
-		RingQueue_Allocate(&uvd->dp, RING_QUEUE_SIZE);
-		if ((uvd->fbuf == NULL) ||
-		    (!RingQueue_IsAllocated(&uvd->dp))) {
-			err("%s: Failed to allocate fbuf or dp", __func__);
-			errCode = -ENOMEM;
-		} else {
-			/* Allocate all buffers */
-			for (i=0; i < USBVIDEO_NUMFRAMES; i++) {
-				uvd->frame[i].frameState = FrameState_Unused;
-				uvd->frame[i].data = uvd->fbuf + i*(uvd->max_frame_size);
-				/*
-				 * Set default sizes in case IOCTL (VIDIOCMCAPTURE)
-				 * is not used (using read() instead).
-				 */
-				uvd->frame[i].canvas = uvd->canvas;
-				uvd->frame[i].seqRead_Index = 0;
-			}
-			for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-				uvd->sbuf[i].data = kmalloc(sb_size, GFP_KERNEL);
-				if (uvd->sbuf[i].data == NULL) {
-					errCode = -ENOMEM;
-					break;
-				}
-			}
-		}
-		if (errCode != 0) {
-			/* Have to free all that memory */
-			if (uvd->fbuf != NULL) {
-				usbvideo_rvfree(uvd->fbuf, uvd->fbuf_size);
-				uvd->fbuf = NULL;
-			}
-			RingQueue_Free(&uvd->dp);
-			for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-				kfree(uvd->sbuf[i].data);
-				uvd->sbuf[i].data = NULL;
-			}
-		}
-	}
-
-	/* If so far no errors then we shall start the camera */
-	if (errCode == 0) {
-		/* Start data pump if we have valid endpoint */
-		if (uvd->video_endp != 0)
-			errCode = GET_CALLBACK(uvd, startDataPump)(uvd);
-		if (errCode == 0) {
-			if (VALID_CALLBACK(uvd, setupOnOpen)) {
-				if (uvd->debug > 1)
-					dev_info(&uvd->dev->dev,
-						 "%s: setupOnOpen callback\n",
-						 __func__);
-				errCode = GET_CALLBACK(uvd, setupOnOpen)(uvd);
-				if (errCode < 0) {
-					err("%s: setupOnOpen callback failed (%d.).",
-					    __func__, errCode);
-				} else if (uvd->debug > 1) {
-					dev_info(&uvd->dev->dev,
-						 "%s: setupOnOpen callback successful\n",
-						 __func__);
-				}
-			}
-			if (errCode == 0) {
-				uvd->settingsAdjusted = 0;
-				if (uvd->debug > 1)
-					dev_info(&uvd->dev->dev,
-						 "%s: Open succeeded.\n",
-						 __func__);
-				uvd->user++;
-				file->private_data = uvd;
-			}
-		}
-	}
-	mutex_unlock(&uvd->lock);
-	if (errCode != 0)
-		usbvideo_ClientDecModCount(uvd);
-	if (uvd->debug > 0)
-		dev_info(&uvd->dev->dev, "%s: Returning %d.\n", __func__,
-			 errCode);
-	return errCode;
-}
-
-/*
- * usbvideo_v4l_close()
- *
- * This is part of Video 4 Linux API. The procedure
- * stops streaming and deallocates all buffers that were earlier
- * allocated in usbvideo_v4l_open().
- *
- * History:
- * 22-Jan-2000 Moved scratch buffer deallocation here.
- * 27-Jan-2000 Used USBVIDEO_NUMSBUF as number of URB buffers.
- * 24-May-2000 Moved MOD_DEC_USE_COUNT outside of code that can sleep.
- */
-static int usbvideo_v4l_close(struct file *file)
-{
-	struct video_device *dev = file->private_data;
-	struct uvd *uvd = (struct uvd *) dev;
-	int i;
-
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "%s($%p)\n", __func__, dev);
-
-	mutex_lock(&uvd->lock);
-	GET_CALLBACK(uvd, stopDataPump)(uvd);
-	usbvideo_rvfree(uvd->fbuf, uvd->fbuf_size);
-	uvd->fbuf = NULL;
-	RingQueue_Free(&uvd->dp);
-
-	for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-		kfree(uvd->sbuf[i].data);
-		uvd->sbuf[i].data = NULL;
-	}
-
-#if USBVIDEO_REPORT_STATS
-	usbvideo_ReportStatistics(uvd);
-#endif
-
-	uvd->user--;
-	if (uvd->remove_pending) {
-		if (uvd->debug > 0)
-			dev_info(&uvd->dev->dev, "%s: Final disconnect.\n",
-				 __func__);
-		usbvideo_CameraRelease(uvd);
-	}
-	mutex_unlock(&uvd->lock);
-	usbvideo_ClientDecModCount(uvd);
-
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "%s: Completed.\n", __func__);
-	file->private_data = NULL;
-	return 0;
-}
-
-/*
- * usbvideo_v4l_ioctl()
- *
- * This is part of Video 4 Linux API. The procedure handles ioctl() calls.
- *
- * History:
- * 22-Jan-2000 Corrected VIDIOCSPICT to reject unsupported settings.
- */
-static long usbvideo_v4l_do_ioctl(struct file *file, unsigned int cmd, void *arg)
-{
-	struct uvd *uvd = file->private_data;
-
-	if (!CAMERA_IS_OPERATIONAL(uvd))
-		return -EIO;
-
-	switch (cmd) {
-		case VIDIOCGCAP:
-		{
-			struct video_capability *b = arg;
-			*b = uvd->vcap;
-			return 0;
-		}
-		case VIDIOCGCHAN:
-		{
-			struct video_channel *v = arg;
-			*v = uvd->vchan;
-			return 0;
-		}
-		case VIDIOCSCHAN:
-		{
-			struct video_channel *v = arg;
-			if (v->channel != 0)
-				return -EINVAL;
-			return 0;
-		}
-		case VIDIOCGPICT:
-		{
-			struct video_picture *pic = arg;
-			*pic = uvd->vpic;
-			return 0;
-		}
-		case VIDIOCSPICT:
-		{
-			struct video_picture *pic = arg;
-			/*
-			 * Use temporary 'video_picture' structure to preserve our
-			 * own settings (such as color depth, palette) that we
-			 * aren't allowing everyone (V4L client) to change.
-			 */
-			uvd->vpic.brightness = pic->brightness;
-			uvd->vpic.hue = pic->hue;
-			uvd->vpic.colour = pic->colour;
-			uvd->vpic.contrast = pic->contrast;
-			uvd->settingsAdjusted = 0;	/* Will force new settings */
-			return 0;
-		}
-		case VIDIOCSWIN:
-		{
-			struct video_window *vw = arg;
-
-			if(VALID_CALLBACK(uvd, setVideoMode)) {
-				return GET_CALLBACK(uvd, setVideoMode)(uvd, vw);
-			}
-
-			if (vw->flags)
-				return -EINVAL;
-			if (vw->clipcount)
-				return -EINVAL;
-			if (vw->width != VIDEOSIZE_X(uvd->canvas))
-				return -EINVAL;
-			if (vw->height != VIDEOSIZE_Y(uvd->canvas))
-				return -EINVAL;
-
-			return 0;
-		}
-		case VIDIOCGWIN:
-		{
-			struct video_window *vw = arg;
-
-			vw->x = 0;
-			vw->y = 0;
-			vw->width = VIDEOSIZE_X(uvd->videosize);
-			vw->height = VIDEOSIZE_Y(uvd->videosize);
-			vw->chromakey = 0;
-			if (VALID_CALLBACK(uvd, getFPS))
-				vw->flags = GET_CALLBACK(uvd, getFPS)(uvd);
-			else
-				vw->flags = 10; /* FIXME: do better! */
-			return 0;
-		}
-		case VIDIOCGMBUF:
-		{
-			struct video_mbuf *vm = arg;
-			int i;
-
-			memset(vm, 0, sizeof(*vm));
-			vm->size = uvd->max_frame_size * USBVIDEO_NUMFRAMES;
-			vm->frames = USBVIDEO_NUMFRAMES;
-			for(i = 0; i < USBVIDEO_NUMFRAMES; i++)
-			  vm->offsets[i] = i * uvd->max_frame_size;
-
-			return 0;
-		}
-		case VIDIOCMCAPTURE:
-		{
-			struct video_mmap *vm = arg;
-
-			if (uvd->debug >= 1) {
-				dev_info(&uvd->dev->dev,
-					 "VIDIOCMCAPTURE: frame=%d. size=%dx%d, format=%d.\n",
-					 vm->frame, vm->width, vm->height, vm->format);
-			}
-			/*
-			 * Check if the requested size is supported. If the requestor
-			 * requests too big a frame then we may be tricked into accessing
-			 * outside of own preallocated frame buffer (in uvd->frame).
-			 * This will cause oops or a security hole. Theoretically, we
-			 * could only clamp the size down to acceptable bounds, but then
-			 * we'd need to figure out how to insert our smaller buffer into
-			 * larger caller's buffer... this is not an easy question. So we
-			 * here just flatly reject too large requests, assuming that the
-			 * caller will resubmit with smaller size. Callers should know
-			 * what size we support (returned by VIDIOCGCAP). However vidcat,
-			 * for one, does not care and allows to ask for any size.
-			 */
-			if ((vm->width > VIDEOSIZE_X(uvd->canvas)) ||
-			    (vm->height > VIDEOSIZE_Y(uvd->canvas))) {
-				if (uvd->debug > 0) {
-					dev_info(&uvd->dev->dev,
-						 "VIDIOCMCAPTURE: Size=%dx%d "
-						 "too large; allowed only up "
-						 "to %ldx%ld\n", vm->width,
-						 vm->height,
-						 VIDEOSIZE_X(uvd->canvas),
-						 VIDEOSIZE_Y(uvd->canvas));
-				}
-				return -EINVAL;
-			}
-			/* Check if the palette is supported */
-			if (((1L << vm->format) & uvd->paletteBits) == 0) {
-				if (uvd->debug > 0) {
-					dev_info(&uvd->dev->dev,
-						 "VIDIOCMCAPTURE: format=%d. "
-						 "not supported "
-						 "(paletteBits=$%08lx)\n",
-						 vm->format, uvd->paletteBits);
-				}
-				return -EINVAL;
-			}
-			if ((vm->frame < 0) || (vm->frame >= USBVIDEO_NUMFRAMES)) {
-				err("VIDIOCMCAPTURE: vm.frame=%d. !E [0-%d]", vm->frame, USBVIDEO_NUMFRAMES-1);
-				return -EINVAL;
-			}
-			if (uvd->frame[vm->frame].frameState == FrameState_Grabbing) {
-				/* Not an error - can happen */
-			}
-			uvd->frame[vm->frame].request = VIDEOSIZE(vm->width, vm->height);
-			uvd->frame[vm->frame].palette = vm->format;
-
-			/* Mark it as ready */
-			uvd->frame[vm->frame].frameState = FrameState_Ready;
-
-			return usbvideo_NewFrame(uvd, vm->frame);
-		}
-		case VIDIOCSYNC:
-		{
-			int *frameNum = arg;
-			int ret;
-
-			if (*frameNum < 0 || *frameNum >= USBVIDEO_NUMFRAMES)
-				return -EINVAL;
-
-			if (uvd->debug >= 1)
-				dev_info(&uvd->dev->dev,
-					 "VIDIOCSYNC: syncing to frame %d.\n",
-					 *frameNum);
-			if (uvd->flags & FLAGS_NO_DECODING)
-				ret = usbvideo_GetFrame(uvd, *frameNum);
-			else if (VALID_CALLBACK(uvd, getFrame)) {
-				ret = GET_CALLBACK(uvd, getFrame)(uvd, *frameNum);
-				if ((ret < 0) && (uvd->debug >= 1)) {
-					err("VIDIOCSYNC: getFrame() returned %d.", ret);
-				}
-			} else {
-				err("VIDIOCSYNC: getFrame is not set");
-				ret = -EFAULT;
-			}
-
-			/*
-			 * The frame is in FrameState_Done_Hold state. Release it
-			 * right now because its data is already mapped into
-			 * the user space and it's up to the application to
-			 * make use of it until it asks for another frame.
-			 */
-			uvd->frame[*frameNum].frameState = FrameState_Unused;
-			return ret;
-		}
-		case VIDIOCGFBUF:
-		{
-			struct video_buffer *vb = arg;
-
-			memset(vb, 0, sizeof(*vb));
-			return 0;
-		}
-		case VIDIOCKEY:
-			return 0;
-
-		case VIDIOCCAPTURE:
-			return -EINVAL;
-
-		case VIDIOCSFBUF:
-
-		case VIDIOCGTUNER:
-		case VIDIOCSTUNER:
-
-		case VIDIOCGFREQ:
-		case VIDIOCSFREQ:
-
-		case VIDIOCGAUDIO:
-		case VIDIOCSAUDIO:
-			return -EINVAL;
-
-		default:
-			return -ENOIOCTLCMD;
-	}
-	return 0;
-}
-
-static long usbvideo_v4l_ioctl(struct file *file,
-		       unsigned int cmd, unsigned long arg)
-{
-	return video_usercopy(file, cmd, arg, usbvideo_v4l_do_ioctl);
-}
-
-/*
- * usbvideo_v4l_read()
- *
- * This is mostly boring stuff. We simply ask for a frame and when it
- * arrives copy all the video data from it into user space. There is
- * no obvious need to override this method.
- *
- * History:
- * 20-Oct-2000 Created.
- * 01-Nov-2000 Added mutex (uvd->lock).
- */
-static ssize_t usbvideo_v4l_read(struct file *file, char __user *buf,
-		      size_t count, loff_t *ppos)
-{
-	struct uvd *uvd = file->private_data;
-	int noblock = file->f_flags & O_NONBLOCK;
-	int frmx = -1, i;
-	struct usbvideo_frame *frame;
-
-	if (!CAMERA_IS_OPERATIONAL(uvd) || (buf == NULL))
-		return -EFAULT;
-
-	if (uvd->debug >= 1)
-		dev_info(&uvd->dev->dev,
-			 "%s: %Zd. bytes, noblock=%d.\n",
-			 __func__, count, noblock);
-
-	mutex_lock(&uvd->lock);
-
-	/* See if a frame is completed, then use it. */
-	for(i = 0; i < USBVIDEO_NUMFRAMES; i++) {
-		if ((uvd->frame[i].frameState == FrameState_Done) ||
-		    (uvd->frame[i].frameState == FrameState_Done_Hold) ||
-		    (uvd->frame[i].frameState == FrameState_Error)) {
-			frmx = i;
-			break;
-		}
-	}
-
-	/* FIXME: If we don't start a frame here then who ever does? */
-	if (noblock && (frmx == -1)) {
-		count = -EAGAIN;
-		goto read_done;
-	}
-
-	/*
-	 * If no FrameState_Done, look for a FrameState_Grabbing state.
-	 * See if a frame is in process (grabbing), then use it.
-	 * We will need to wait until it becomes cooked, of course.
-	 */
-	if (frmx == -1) {
-		for(i = 0; i < USBVIDEO_NUMFRAMES; i++) {
-			if (uvd->frame[i].frameState == FrameState_Grabbing) {
-				frmx = i;
-				break;
-			}
-		}
-	}
-
-	/*
-	 * If no frame is active, start one. We don't care which one
-	 * it will be, so #0 is as good as any.
-	 * In read access mode we don't have convenience of VIDIOCMCAPTURE
-	 * to specify the requested palette (video format) on per-frame
-	 * basis. This means that we have to return data in -some- format
-	 * and just hope that the client knows what to do with it.
-	 * The default format is configured in uvd->defaultPalette field
-	 * as one of VIDEO_PALETTE_xxx values. We stuff it into the new
-	 * frame and initiate the frame filling process.
-	 */
-	if (frmx == -1) {
-		if (uvd->defaultPalette == 0) {
-			err("%s: No default palette; don't know what to do!", __func__);
-			count = -EFAULT;
-			goto read_done;
-		}
-		frmx = 0;
-		/*
-		 * We have no per-frame control over video size.
-		 * Therefore we only can use whatever size was
-		 * specified as default.
-		 */
-		uvd->frame[frmx].request = uvd->videosize;
-		uvd->frame[frmx].palette = uvd->defaultPalette;
-		uvd->frame[frmx].frameState = FrameState_Ready;
-		usbvideo_NewFrame(uvd, frmx);
-		/* Now frame 0 is supposed to start filling... */
-	}
-
-	/*
-	 * Get a pointer to the active frame. It is either previously
-	 * completed frame or frame in progress but not completed yet.
-	 */
-	frame = &uvd->frame[frmx];
-
-	/*
-	 * Sit back & wait until the frame gets filled and postprocessed.
-	 * If we fail to get the picture [in time] then return the error.
-	 * In this call we specify that we want the frame to be waited for,
-	 * postprocessed and switched into FrameState_Done_Hold state. This
-	 * state is used to hold the frame as "fully completed" between
-	 * subsequent partial reads of the same frame.
-	 */
-	if (frame->frameState != FrameState_Done_Hold) {
-		long rv = -EFAULT;
-		if (uvd->flags & FLAGS_NO_DECODING)
-			rv = usbvideo_GetFrame(uvd, frmx);
-		else if (VALID_CALLBACK(uvd, getFrame))
-			rv = GET_CALLBACK(uvd, getFrame)(uvd, frmx);
-		else
-			err("getFrame is not set");
-		if ((rv != 0) || (frame->frameState != FrameState_Done_Hold)) {
-			count = rv;
-			goto read_done;
-		}
-	}
-
-	/*
-	 * Copy bytes to user space. We allow for partial reads, which
-	 * means that the user application can request read less than
-	 * the full frame size. It is up to the application to issue
-	 * subsequent calls until entire frame is read.
-	 *
-	 * First things first, make sure we don't copy more than we
-	 * have - even if the application wants more. That would be
-	 * a big security embarassment!
-	 */
-	if ((count + frame->seqRead_Index) > frame->seqRead_Length)
-		count = frame->seqRead_Length - frame->seqRead_Index;
-
-	/*
-	 * Copy requested amount of data to user space. We start
-	 * copying from the position where we last left it, which
-	 * will be zero for a new frame (not read before).
-	 */
-	if (copy_to_user(buf, frame->data + frame->seqRead_Index, count)) {
-		count = -EFAULT;
-		goto read_done;
-	}
-
-	/* Update last read position */
-	frame->seqRead_Index += count;
-	if (uvd->debug >= 1) {
-		err("%s: {copy} count used=%Zd, new seqRead_Index=%ld",
-			__func__, count, frame->seqRead_Index);
-	}
-
-	/* Finally check if the frame is done with and "release" it */
-	if (frame->seqRead_Index >= frame->seqRead_Length) {
-		/* All data has been read */
-		frame->seqRead_Index = 0;
-
-		/* Mark it as available to be used again. */
-		uvd->frame[frmx].frameState = FrameState_Unused;
-		if (usbvideo_NewFrame(uvd, (frmx + 1) % USBVIDEO_NUMFRAMES)) {
-			err("%s: usbvideo_NewFrame failed.", __func__);
-		}
-	}
-read_done:
-	mutex_unlock(&uvd->lock);
-	return count;
-}
-
-/*
- * Make all of the blocks of data contiguous
- */
-static int usbvideo_CompressIsochronous(struct uvd *uvd, struct urb *urb)
-{
-	char *cdata;
-	int i, totlen = 0;
-
-	for (i = 0; i < urb->number_of_packets; i++) {
-		int n = urb->iso_frame_desc[i].actual_length;
-		int st = urb->iso_frame_desc[i].status;
-
-		cdata = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
-
-		/* Detect and ignore errored packets */
-		if (st < 0) {
-			if (uvd->debug >= 1)
-				err("Data error: packet=%d. len=%d. status=%d.", i, n, st);
-			uvd->stats.iso_err_count++;
-			continue;
-		}
-
-		/* Detect and ignore empty packets */
-		if (n <= 0) {
-			uvd->stats.iso_skip_count++;
-			continue;
-		}
-		totlen += n;	/* Little local accounting */
-		RingQueue_Enqueue(&uvd->dp, cdata, n);
-	}
-	return totlen;
-}
-
-static void usbvideo_IsocIrq(struct urb *urb)
-{
-	int i, ret, len;
-	struct uvd *uvd = urb->context;
-
-	/* We don't want to do anything if we are about to be removed! */
-	if (!CAMERA_IS_OPERATIONAL(uvd))
-		return;
-#if 0
-	if (urb->actual_length > 0) {
-		dev_info(&uvd->dev->dev,
-			 "urb=$%p status=%d. errcount=%d. length=%d.\n",
-			 urb, urb->status, urb->error_count,
-			 urb->actual_length);
-	} else {
-		static int c = 0;
-		if (c++ % 100 == 0)
-			dev_info(&uvd->dev->dev, "No Isoc data\n");
-	}
-#endif
-
-	if (!uvd->streaming) {
-		if (uvd->debug >= 1)
-			dev_info(&uvd->dev->dev,
-				 "Not streaming, but interrupt!\n");
-		return;
-	}
-
-	uvd->stats.urb_count++;
-	if (urb->actual_length <= 0)
-		goto urb_done_with;
-
-	/* Copy the data received into ring queue */
-	len = usbvideo_CompressIsochronous(uvd, urb);
-	uvd->stats.urb_length = len;
-	if (len <= 0)
-		goto urb_done_with;
-
-	/* Here we got some data */
-	uvd->stats.data_count += len;
-	RingQueue_WakeUpInterruptible(&uvd->dp);
-
-urb_done_with:
-	for (i = 0; i < FRAMES_PER_DESC; i++) {
-		urb->iso_frame_desc[i].status = 0;
-		urb->iso_frame_desc[i].actual_length = 0;
-	}
-	urb->status = 0;
-	urb->dev = uvd->dev;
-	ret = usb_submit_urb (urb, GFP_KERNEL);
-	if(ret)
-		err("usb_submit_urb error (%d)", ret);
-	return;
-}
-
-/*
- * usbvideo_StartDataPump()
- *
- * History:
- * 27-Jan-2000 Used ibmcam->iface, ibmcam->ifaceAltActive instead
- *             of hardcoded values. Simplified by using for loop,
- *             allowed any number of URBs.
- */
-static int usbvideo_StartDataPump(struct uvd *uvd)
-{
-	struct usb_device *dev = uvd->dev;
-	int i, errFlag;
-
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "%s($%p)\n", __func__, uvd);
-
-	if (!CAMERA_IS_OPERATIONAL(uvd)) {
-		err("%s: Camera is not operational", __func__);
-		return -EFAULT;
-	}
-	uvd->curframe = -1;
-
-	/* Alternate interface 1 is is the biggest frame size */
-	i = usb_set_interface(dev, uvd->iface, uvd->ifaceAltActive);
-	if (i < 0) {
-		err("%s: usb_set_interface error", __func__);
-		uvd->last_error = i;
-		return -EBUSY;
-	}
-	if (VALID_CALLBACK(uvd, videoStart))
-		GET_CALLBACK(uvd, videoStart)(uvd);
-	else
-		err("%s: videoStart not set", __func__);
-
-	/* We double buffer the Iso lists */
-	for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-		int j, k;
-		struct urb *urb = uvd->sbuf[i].urb;
-		urb->dev = dev;
-		urb->context = uvd;
-		urb->pipe = usb_rcvisocpipe(dev, uvd->video_endp);
-		urb->interval = 1;
-		urb->transfer_flags = URB_ISO_ASAP;
-		urb->transfer_buffer = uvd->sbuf[i].data;
-		urb->complete = usbvideo_IsocIrq;
-		urb->number_of_packets = FRAMES_PER_DESC;
-		urb->transfer_buffer_length = uvd->iso_packet_len * FRAMES_PER_DESC;
-		for (j=k=0; j < FRAMES_PER_DESC; j++, k += uvd->iso_packet_len) {
-			urb->iso_frame_desc[j].offset = k;
-			urb->iso_frame_desc[j].length = uvd->iso_packet_len;
-		}
-	}
-
-	/* Submit all URBs */
-	for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-		errFlag = usb_submit_urb(uvd->sbuf[i].urb, GFP_KERNEL);
-		if (errFlag)
-			err("%s: usb_submit_isoc(%d) ret %d", __func__, i, errFlag);
-	}
-
-	uvd->streaming = 1;
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev,
-			 "%s: streaming=1 video_endp=$%02x\n", __func__,
-			 uvd->video_endp);
-	return 0;
-}
-
-/*
- * usbvideo_StopDataPump()
- *
- * This procedure stops streaming and deallocates URBs. Then it
- * activates zero-bandwidth alt. setting of the video interface.
- *
- * History:
- * 22-Jan-2000 Corrected order of actions to work after surprise removal.
- * 27-Jan-2000 Used uvd->iface, uvd->ifaceAltInactive instead of hardcoded values.
- */
-static void usbvideo_StopDataPump(struct uvd *uvd)
-{
-	int i, j;
-
-	if ((uvd == NULL) || (!uvd->streaming) || (uvd->dev == NULL))
-		return;
-
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "%s($%p)\n", __func__, uvd);
-
-	/* Unschedule all of the iso td's */
-	for (i=0; i < USBVIDEO_NUMSBUF; i++) {
-		usb_kill_urb(uvd->sbuf[i].urb);
-	}
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "%s: streaming=0\n", __func__);
-	uvd->streaming = 0;
-
-	if (!uvd->remove_pending) {
-		/* Invoke minidriver's magic to stop the camera */
-		if (VALID_CALLBACK(uvd, videoStop))
-			GET_CALLBACK(uvd, videoStop)(uvd);
-		else
-			err("%s: videoStop not set", __func__);
-
-		/* Set packet size to 0 */
-		j = usb_set_interface(uvd->dev, uvd->iface, uvd->ifaceAltInactive);
-		if (j < 0) {
-			err("%s: usb_set_interface() error %d.", __func__, j);
-			uvd->last_error = j;
-		}
-	}
-}
-
-/*
- * usbvideo_NewFrame()
- *
- * History:
- * 29-Mar-00 Added copying of previous frame into the current one.
- * 6-Aug-00  Added model 3 video sizes, removed redundant width, height.
- */
-static int usbvideo_NewFrame(struct uvd *uvd, int framenum)
-{
-	struct usbvideo_frame *frame;
-	int n;
-
-	if (uvd->debug > 1)
-		dev_info(&uvd->dev->dev, "usbvideo_NewFrame($%p,%d.)\n", uvd,
-			 framenum);
-
-	/* If we're not grabbing a frame right now and the other frame is */
-	/*  ready to be grabbed into, then use it instead */
-	if (uvd->curframe != -1)
-		return 0;
-
-	/* If necessary we adjust picture settings between frames */
-	if (!uvd->settingsAdjusted) {
-		if (VALID_CALLBACK(uvd, adjustPicture))
-			GET_CALLBACK(uvd, adjustPicture)(uvd);
-		uvd->settingsAdjusted = 1;
-	}
-
-	n = (framenum + 1) % USBVIDEO_NUMFRAMES;
-	if (uvd->frame[n].frameState == FrameState_Ready)
-		framenum = n;
-
-	frame = &uvd->frame[framenum];
-
-	frame->frameState = FrameState_Grabbing;
-	frame->scanstate = ScanState_Scanning;
-	frame->seqRead_Length = 0;	/* Accumulated in xxx_parse_data() */
-	frame->deinterlace = Deinterlace_None;
-	frame->flags = 0; /* No flags yet, up to minidriver (or us) to set them */
-	uvd->curframe = framenum;
-
-	/*
-	 * Normally we would want to copy previous frame into the current one
-	 * before we even start filling it with data; this allows us to stop
-	 * filling at any moment; top portion of the frame will be new and
-	 * bottom portion will stay as it was in previous frame. If we don't
-	 * do that then missing chunks of video stream will result in flickering
-	 * portions of old data whatever it was before.
-	 *
-	 * If we choose not to copy previous frame (to, for example, save few
-	 * bus cycles - the frame can be pretty large!) then we have an option
-	 * to clear the frame before using. If we experience losses in this
-	 * mode then missing picture will be black (no flickering).
-	 *
-	 * Finally, if user chooses not to clean the current frame before
-	 * filling it with data then the old data will be visible if we fail
-	 * to refill entire frame with new data.
-	 */
-	if (!(uvd->flags & FLAGS_SEPARATE_FRAMES)) {
-		/* This copies previous frame into this one to mask losses */
-		int prev = (framenum - 1 + USBVIDEO_NUMFRAMES) % USBVIDEO_NUMFRAMES;
-		memmove(frame->data, uvd->frame[prev].data, uvd->max_frame_size);
-	} else {
-		if (uvd->flags & FLAGS_CLEAN_FRAMES) {
-			/* This provides a "clean" frame but slows things down */
-			memset(frame->data, 0, uvd->max_frame_size);
-		}
-	}
-	return 0;
-}
-
-/*
- * usbvideo_CollectRawData()
- *
- * This procedure can be used instead of 'processData' callback if you
- * only want to dump the raw data from the camera into the output
- * device (frame buffer). You can look at it with V4L client, but the
- * image will be unwatchable. The main purpose of this code and of the
- * mode FLAGS_NO_DECODING is debugging and capturing of datastreams from
- * new, unknown cameras. This procedure will be automatically invoked
- * instead of the specified callback handler when uvd->flags has bit
- * FLAGS_NO_DECODING set. Therefore, any regular build of any driver
- * based on usbvideo can use this feature at any time.
- */
-static void usbvideo_CollectRawData(struct uvd *uvd, struct usbvideo_frame *frame)
-{
-	int n;
-
-	assert(uvd != NULL);
-	assert(frame != NULL);
-
-	/* Try to move data from queue into frame buffer */
-	n = RingQueue_GetLength(&uvd->dp);
-	if (n > 0) {
-		int m;
-		/* See how much space we have left */
-		m = uvd->max_frame_size - frame->seqRead_Length;
-		if (n > m)
-			n = m;
-		/* Now move that much data into frame buffer */
-		RingQueue_Dequeue(
-			&uvd->dp,
-			frame->data + frame->seqRead_Length,
-			m);
-		frame->seqRead_Length += m;
-	}
-	/* See if we filled the frame */
-	if (frame->seqRead_Length >= uvd->max_frame_size) {
-		frame->frameState = FrameState_Done;
-		uvd->curframe = -1;
-		uvd->stats.frame_num++;
-	}
-}
-
-static int usbvideo_GetFrame(struct uvd *uvd, int frameNum)
-{
-	struct usbvideo_frame *frame = &uvd->frame[frameNum];
-
-	if (uvd->debug >= 2)
-		dev_info(&uvd->dev->dev, "%s($%p,%d.)\n", __func__, uvd,
-			 frameNum);
-
-	switch (frame->frameState) {
-	case FrameState_Unused:
-		if (uvd->debug >= 2)
-			dev_info(&uvd->dev->dev, "%s: FrameState_Unused\n",
-				 __func__);
-		return -EINVAL;
-	case FrameState_Ready:
-	case FrameState_Grabbing:
-	case FrameState_Error:
-	{
-		int ntries, signalPending;
-	redo:
-		if (!CAMERA_IS_OPERATIONAL(uvd)) {
-			if (uvd->debug >= 2)
-				dev_info(&uvd->dev->dev,
-					 "%s: Camera is not operational (1)\n",
-					 __func__);
-			return -EIO;
-		}
-		ntries = 0;
-		do {
-			RingQueue_InterruptibleSleepOn(&uvd->dp);
-			signalPending = signal_pending(current);
-			if (!CAMERA_IS_OPERATIONAL(uvd)) {
-				if (uvd->debug >= 2)
-					dev_info(&uvd->dev->dev,
-						 "%s: Camera is not "
-						 "operational (2)\n", __func__);
-				return -EIO;
-			}
-			assert(uvd->fbuf != NULL);
-			if (signalPending) {
-				if (uvd->debug >= 2)
-					dev_info(&uvd->dev->dev,
-					"%s: Signal=$%08x\n", __func__,
-					signalPending);
-				if (uvd->flags & FLAGS_RETRY_VIDIOCSYNC) {
-					usbvideo_TestPattern(uvd, 1, 0);
-					uvd->curframe = -1;
-					uvd->stats.frame_num++;
-					if (uvd->debug >= 2)
-						dev_info(&uvd->dev->dev,
-							 "%s: Forced test "
-							 "pattern screen\n",
-							 __func__);
-					return 0;
-				} else {
-					/* Standard answer: Interrupted! */
-					if (uvd->debug >= 2)
-						dev_info(&uvd->dev->dev,
-							 "%s: Interrupted!\n",
-							 __func__);
-					return -EINTR;
-				}
-			} else {
-				/* No signals - we just got new data in dp queue */
-				if (uvd->flags & FLAGS_NO_DECODING)
-					usbvideo_CollectRawData(uvd, frame);
-				else if (VALID_CALLBACK(uvd, processData))
-					GET_CALLBACK(uvd, processData)(uvd, frame);
-				else
-					err("%s: processData not set", __func__);
-			}
-		} while (frame->frameState == FrameState_Grabbing);
-		if (uvd->debug >= 2) {
-			dev_info(&uvd->dev->dev,
-				 "%s: Grabbing done; state=%d. (%lu. bytes)\n",
-				 __func__, frame->frameState,
-				 frame->seqRead_Length);
-		}
-		if (frame->frameState == FrameState_Error) {
-			int ret = usbvideo_NewFrame(uvd, frameNum);
-			if (ret < 0) {
-				err("%s: usbvideo_NewFrame() failed (%d.)", __func__, ret);
-				return ret;
-			}
-			goto redo;
-		}
-		/* Note that we fall through to meet our destiny below */
-	}
-	case FrameState_Done:
-		/*
-		 * Do all necessary postprocessing of data prepared in
-		 * "interrupt" code and the collecting code above. The
-		 * frame gets marked as FrameState_Done by queue parsing code.
-		 * This status means that we collected enough data and
-		 * most likely processed it as we went through. However
-		 * the data may need postprocessing, such as deinterlacing
-		 * or picture adjustments implemented in software (horror!)
-		 *
-		 * As soon as the frame becomes "final" it gets promoted to
-		 * FrameState_Done_Hold status where it will remain until the
-		 * caller consumed all the video data from the frame. Then
-		 * the empty shell of ex-frame is thrown out for dogs to eat.
-		 * But we, worried about pets, will recycle the frame!
-		 */
-		uvd->stats.frame_num++;
-		if ((uvd->flags & FLAGS_NO_DECODING) == 0) {
-			if (VALID_CALLBACK(uvd, postProcess))
-				GET_CALLBACK(uvd, postProcess)(uvd, frame);
-			if (frame->flags & USBVIDEO_FRAME_FLAG_SOFTWARE_CONTRAST)
-				usbvideo_SoftwareContrastAdjustment(uvd, frame);
-		}
-		frame->frameState = FrameState_Done_Hold;
-		if (uvd->debug >= 2)
-			dev_info(&uvd->dev->dev,
-				 "%s: Entered FrameState_Done_Hold state.\n",
-				 __func__);
-		return 0;
-
-	case FrameState_Done_Hold:
-		/*
-		 * We stay in this state indefinitely until someone external,
-		 * like ioctl() or read() call finishes digesting the frame
-		 * data. Then it will mark the frame as FrameState_Unused and
-		 * it will be released back into the wild to roam freely.
-		 */
-		if (uvd->debug >= 2)
-			dev_info(&uvd->dev->dev,
-				 "%s: FrameState_Done_Hold state.\n",
-				 __func__);
-		return 0;
-	}
-
-	/* Catch-all for other cases. We shall not be here. */
-	err("%s: Invalid state %d.", __func__, frame->frameState);
-	frame->frameState = FrameState_Unused;
-	return 0;
-}
-
-/*
- * usbvideo_DeinterlaceFrame()
- *
- * This procedure deinterlaces the given frame. Some cameras produce
- * only half of scanlines - sometimes only even lines, sometimes only
- * odd lines. The deinterlacing method is stored in frame->deinterlace
- * variable.
- *
- * Here we scan the frame vertically and replace missing scanlines with
- * average between surrounding ones - before and after. If we have no
- * line above then we just copy next line. Similarly, if we need to
- * create a last line then preceding line is used.
- */
-void usbvideo_DeinterlaceFrame(struct uvd *uvd, struct usbvideo_frame *frame)
-{
-	if ((uvd == NULL) || (frame == NULL))
-		return;
-
-	if ((frame->deinterlace == Deinterlace_FillEvenLines) ||
-	    (frame->deinterlace == Deinterlace_FillOddLines))
-	{
-		const int v4l_linesize = VIDEOSIZE_X(frame->request) * V4L_BYTES_PER_PIXEL;
-		int i = (frame->deinterlace == Deinterlace_FillEvenLines) ? 0 : 1;
-
-		for (; i < VIDEOSIZE_Y(frame->request); i += 2) {
-			const unsigned char *fs1, *fs2;
-			unsigned char *fd;
-			int ip, in, j;	/* Previous and next lines */
-
-			/*
-			 * Need to average lines before and after 'i'.
-			 * If we go out of bounds seeking those lines then
-			 * we point back to existing line.
-			 */
-			ip = i - 1;	/* First, get rough numbers */
-			in = i + 1;
-
-			/* Now validate */
-			if (ip < 0)
-				ip = in;
-			if (in >= VIDEOSIZE_Y(frame->request))
-				in = ip;
-
-			/* Sanity check */
-			if ((ip < 0) || (in < 0) ||
-			    (ip >= VIDEOSIZE_Y(frame->request)) ||
-			    (in >= VIDEOSIZE_Y(frame->request)))
-			{
-				err("Error: ip=%d. in=%d. req.height=%ld.",
-				    ip, in, VIDEOSIZE_Y(frame->request));
-				break;
-			}
-
-			/* Now we need to average lines 'ip' and 'in' to produce line 'i' */
-			fs1 = frame->data + (v4l_linesize * ip);
-			fs2 = frame->data + (v4l_linesize * in);
-			fd = frame->data + (v4l_linesize * i);
-
-			/* Average lines around destination */
-			for (j=0; j < v4l_linesize; j++) {
-				fd[j] = (unsigned char)((((unsigned) fs1[j]) +
-							 ((unsigned)fs2[j])) >> 1);
-			}
-		}
-	}
-
-	/* Optionally display statistics on the screen */
-	if (uvd->flags & FLAGS_OVERLAY_STATS)
-		usbvideo_OverlayStats(uvd, frame);
-}
-
-EXPORT_SYMBOL(usbvideo_DeinterlaceFrame);
-
-/*
- * usbvideo_SoftwareContrastAdjustment()
- *
- * This code adjusts the contrast of the frame, assuming RGB24 format.
- * As most software image processing, this job is CPU-intensive.
- * Get a camera that supports hardware adjustment!
- *
- * History:
- * 09-Feb-2001  Created.
- */
-static void usbvideo_SoftwareContrastAdjustment(struct uvd *uvd,
-						struct usbvideo_frame *frame)
-{
-	int i, j, v4l_linesize;
-	signed long adj;
-	const int ccm = 128; /* Color correction median - see below */
-
-	if ((uvd == NULL) || (frame == NULL)) {
-		err("%s: Illegal call.", __func__);
-		return;
-	}
-	adj = (uvd->vpic.contrast - 0x8000) >> 8; /* -128..+127 = -ccm..+(ccm-1)*/
-	RESTRICT_TO_RANGE(adj, -ccm, ccm+1);
-	if (adj == 0) {
-		/* In rare case of no adjustment */
-		return;
-	}
-	v4l_linesize = VIDEOSIZE_X(frame->request) * V4L_BYTES_PER_PIXEL;
-	for (i=0; i < VIDEOSIZE_Y(frame->request); i++) {
-		unsigned char *fd = frame->data + (v4l_linesize * i);
-		for (j=0; j < v4l_linesize; j++) {
-			signed long v = (signed long) fd[j];
-			/* Magnify up to 2 times, reduce down to zero */
-			v = 128 + ((ccm + adj) * (v - 128)) / ccm;
-			RESTRICT_TO_RANGE(v, 0, 0xFF); /* Must flatten tails */
-			fd[j] = (unsigned char) v;
-		}
-	}
-}
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/usbvideo/usbvideo.h b/drivers/staging/usbvideo/usbvideo.h
deleted file mode 100644
index 95638a072b19..000000000000
--- a/drivers/staging/usbvideo/usbvideo.h
+++ /dev/null
@@ -1,395 +0,0 @@
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-#ifndef usbvideo_h
-#define	usbvideo_h
-
-#include "videodev.h"
-#include <media/v4l2-common.h>
-#include <media/v4l2-ioctl.h>
-#include <linux/usb.h>
-#include <linux/mutex.h>
-
-/* Most helpful debugging aid */
-#define assert(expr) ((void) ((expr) ? 0 : (err("assert failed at line %d",__LINE__))))
-
-#define USBVIDEO_REPORT_STATS	1	/* Set to 0 to block statistics on close */
-
-/* Bit flags (options) */
-#define FLAGS_RETRY_VIDIOCSYNC		(1 << 0)
-#define	FLAGS_MONOCHROME		(1 << 1)
-#define FLAGS_DISPLAY_HINTS		(1 << 2)
-#define FLAGS_OVERLAY_STATS		(1 << 3)
-#define FLAGS_FORCE_TESTPATTERN		(1 << 4)
-#define FLAGS_SEPARATE_FRAMES		(1 << 5)
-#define FLAGS_CLEAN_FRAMES		(1 << 6)
-#define	FLAGS_NO_DECODING		(1 << 7)
-
-/* Bit flags for frames (apply to the frame where they are specified) */
-#define USBVIDEO_FRAME_FLAG_SOFTWARE_CONTRAST	(1 << 0)
-
-/* Camera capabilities (maximum) */
-#define CAMERA_URB_FRAMES       32
-#define CAMERA_MAX_ISO_PACKET   1023 /* 1022 actually sent by camera */
-#define FRAMES_PER_DESC		(CAMERA_URB_FRAMES)
-#define FRAME_SIZE_PER_DESC	(CAMERA_MAX_ISO_PACKET)
-
-/* This macro restricts an int variable to an inclusive range */
-#define RESTRICT_TO_RANGE(v,mi,ma) { if ((v) < (mi)) (v) = (mi); else if ((v) > (ma)) (v) = (ma); }
-
-#define V4L_BYTES_PER_PIXEL     3	/* Because we produce RGB24 */
-
-/*
- * Use this macro to construct constants for different video sizes.
- * We have to deal with different video sizes that have to be
- * configured in the device or compared against when we receive
- * a data. Normally one would define a bunch of VIDEOSIZE_x_by_y
- * #defines and that's the end of story. However this solution
- * does not allow to convert between real pixel sizes and the
- * constant (integer) value that may be used to tag a frame or
- * whatever. The set of macros below constructs videosize constants
- * from the pixel size and allows to reconstruct the pixel size
- * from the combined value later.
- */
-#define	VIDEOSIZE(x,y)	(((x) & 0xFFFFL) | (((y) & 0xFFFFL) << 16))
-#define	VIDEOSIZE_X(vs)	((vs) & 0xFFFFL)
-#define	VIDEOSIZE_Y(vs)	(((vs) >> 16) & 0xFFFFL)
-typedef unsigned long videosize_t;
-
-/*
- * This macro checks if the camera is still operational. The 'uvd'
- * pointer must be valid, uvd->dev must be valid, we are not
- * removing the device and the device has not erred on us.
- */
-#define CAMERA_IS_OPERATIONAL(uvd) (\
-	(uvd != NULL) && \
-	((uvd)->dev != NULL) && \
-	((uvd)->last_error == 0) && \
-	(!(uvd)->remove_pending))
-
-/*
- * We use macros to do YUV -> RGB conversion because this is
- * very important for speed and totally unimportant for size.
- *
- * YUV -> RGB Conversion
- * ---------------------
- *
- * B = 1.164*(Y-16)		    + 2.018*(V-128)
- * G = 1.164*(Y-16) - 0.813*(U-128) - 0.391*(V-128)
- * R = 1.164*(Y-16) + 1.596*(U-128)
- *
- * If you fancy integer arithmetics (as you should), hear this:
- *
- * 65536*B = 76284*(Y-16)		  + 132252*(V-128)
- * 65536*G = 76284*(Y-16) -  53281*(U-128) -  25625*(V-128)
- * 65536*R = 76284*(Y-16) + 104595*(U-128)
- *
- * Make sure the output values are within [0..255] range.
- */
-#define LIMIT_RGB(x) (((x) < 0) ? 0 : (((x) > 255) ? 255 : (x)))
-#define YUV_TO_RGB_BY_THE_BOOK(my,mu,mv,mr,mg,mb) { \
-    int mm_y, mm_yc, mm_u, mm_v, mm_r, mm_g, mm_b; \
-    mm_y = (my) - 16;  \
-    mm_u = (mu) - 128; \
-    mm_v = (mv) - 128; \
-    mm_yc= mm_y * 76284; \
-    mm_b = (mm_yc		+ 132252*mm_v	) >> 16; \
-    mm_g = (mm_yc -  53281*mm_u -  25625*mm_v	) >> 16; \
-    mm_r = (mm_yc + 104595*mm_u			) >> 16; \
-    mb = LIMIT_RGB(mm_b); \
-    mg = LIMIT_RGB(mm_g); \
-    mr = LIMIT_RGB(mm_r); \
-}
-
-#define	RING_QUEUE_SIZE		(128*1024)	/* Must be a power of 2 */
-#define	RING_QUEUE_ADVANCE_INDEX(rq,ind,n) (rq)->ind = ((rq)->ind + (n)) & ((rq)->length-1)
-#define	RING_QUEUE_DEQUEUE_BYTES(rq,n) RING_QUEUE_ADVANCE_INDEX(rq,ri,n)
-#define	RING_QUEUE_PEEK(rq,ofs) ((rq)->queue[((ofs) + (rq)->ri) & ((rq)->length-1)])
-
-struct RingQueue {
-	unsigned char *queue;	/* Data from the Isoc data pump */
-	int length;		/* How many bytes allocated for the queue */
-	int wi;			/* That's where we write */
-	int ri;			/* Read from here until you hit write index */
-	wait_queue_head_t wqh;	/* Processes waiting */
-};
-
-enum ScanState {
-	ScanState_Scanning,	/* Scanning for header */
-	ScanState_Lines		/* Parsing lines */
-};
-
-/* Completion states of the data parser */
-enum ParseState {
-	scan_Continue,		/* Just parse next item */
-	scan_NextFrame,		/* Frame done, send it to V4L */
-	scan_Out,		/* Not enough data for frame */
-	scan_EndParse		/* End parsing */
-};
-
-enum FrameState {
-	FrameState_Unused,	/* Unused (no MCAPTURE) */
-	FrameState_Ready,	/* Ready to start grabbing */
-	FrameState_Grabbing,	/* In the process of being grabbed into */
-	FrameState_Done,	/* Finished grabbing, but not been synced yet */
-	FrameState_Done_Hold,	/* Are syncing or reading */
-	FrameState_Error,	/* Something bad happened while processing */
-};
-
-/*
- * Some frames may contain only even or odd lines. This type
- * specifies what type of deinterlacing is required.
- */
-enum Deinterlace {
-	Deinterlace_None=0,
-	Deinterlace_FillOddLines,
-	Deinterlace_FillEvenLines
-};
-
-#define USBVIDEO_NUMFRAMES	2	/* How many frames we work with */
-#define USBVIDEO_NUMSBUF	2	/* How many URBs linked in a ring */
-
-/* This structure represents one Isoc request - URB and buffer */
-struct usbvideo_sbuf {
-	char *data;
-	struct urb *urb;
-};
-
-struct usbvideo_frame {
-	char *data;		/* Frame buffer */
-	unsigned long header;	/* Significant bits from the header */
-
-	videosize_t canvas;	/* The canvas (max. image) allocated */
-	videosize_t request;	/* That's what the application asked for */
-	unsigned short palette;	/* The desired format */
-
-	enum FrameState frameState;/* State of grabbing */
-	enum ScanState scanstate;	/* State of scanning */
-	enum Deinterlace deinterlace;
-	int flags;		/* USBVIDEO_FRAME_FLAG_xxx bit flags */
-
-	int curline;		/* Line of frame we're working on */
-
-	long seqRead_Length;	/* Raw data length of frame */
-	long seqRead_Index;	/* Amount of data that has been already read */
-
-	void *user;		/* Additional data that user may need */
-};
-
-/* Statistics that can be overlaid on screen */
-struct usbvideo_statistics {
-	unsigned long frame_num;	/* Sequential number of the frame */
-	unsigned long urb_count;        /* How many URBs we received so far */
-	unsigned long urb_length;       /* Length of last URB */
-	unsigned long data_count;       /* How many bytes we received */
-	unsigned long header_count;     /* How many frame headers we found */
-	unsigned long iso_skip_count;	/* How many empty ISO packets received */
-	unsigned long iso_err_count;	/* How many bad ISO packets received */
-};
-
-struct usbvideo;
-
-struct uvd {
-	struct video_device vdev;	/* Must be the first field! */
-	struct usb_device *dev;
-	struct usbvideo *handle;	/* Points back to the struct usbvideo */
-	void *user_data;		/* Camera-dependent data */
-	int user_size;			/* Size of that camera-dependent data */
-	int debug;			/* Debug level for usbvideo */
-	unsigned char iface;		/* Video interface number */
-	unsigned char video_endp;
-	unsigned char ifaceAltActive;
-	unsigned char ifaceAltInactive; /* Alt settings */
-	unsigned long flags;		/* FLAGS_USBVIDEO_xxx */
-	unsigned long paletteBits;	/* Which palettes we accept? */
-	unsigned short defaultPalette;	/* What palette to use for read() */
-	struct mutex lock;
-	int user;		/* user count for exclusive use */
-
-	videosize_t videosize;	/* Current setting */
-	videosize_t canvas;	/* This is the width,height of the V4L canvas */
-	int max_frame_size;	/* Bytes in one video frame */
-
-	int uvd_used;        	/* Is this structure in use? */
-	int streaming;		/* Are we streaming Isochronous? */
-	int grabbing;		/* Are we grabbing? */
-	int settingsAdjusted;	/* Have we adjusted contrast etc.? */
-	int last_error;		/* What calamity struck us? */
-
-	char *fbuf;		/* Videodev buffer area */
-	int fbuf_size;		/* Videodev buffer size */
-
-	int curframe;
-	int iso_packet_len;	/* Videomode-dependent, saves bus bandwidth */
-
-	struct RingQueue dp;	/* Isoc data pump */
-	struct usbvideo_frame frame[USBVIDEO_NUMFRAMES];
-	struct usbvideo_sbuf sbuf[USBVIDEO_NUMSBUF];
-
-	volatile int remove_pending;	/* If set then about to exit */
-
-	struct video_picture vpic, vpic_old;	/* Picture settings */
-	struct video_capability vcap;		/* Video capabilities */
-	struct video_channel vchan;	/* May be used for tuner support */
-	struct usbvideo_statistics stats;
-	char videoName[32];		/* Holds name like "video7" */
-};
-
-/*
- * usbvideo callbacks (virtual methods). They are set when usbvideo
- * services are registered. All of these default to NULL, except those
- * that default to usbvideo-provided methods.
- */
-struct usbvideo_cb {
-	int (*probe)(struct usb_interface *, const struct usb_device_id *);
-	void (*userFree)(struct uvd *);
-	void (*disconnect)(struct usb_interface *);
-	int (*setupOnOpen)(struct uvd *);
-	void (*videoStart)(struct uvd *);
-	void (*videoStop)(struct uvd *);
-	void (*processData)(struct uvd *, struct usbvideo_frame *);
-	void (*postProcess)(struct uvd *, struct usbvideo_frame *);
-	void (*adjustPicture)(struct uvd *);
-	int (*getFPS)(struct uvd *);
-	int (*overlayHook)(struct uvd *, struct usbvideo_frame *);
-	int (*getFrame)(struct uvd *, int);
-	int (*startDataPump)(struct uvd *uvd);
-	void (*stopDataPump)(struct uvd *uvd);
-	int (*setVideoMode)(struct uvd *uvd, struct video_window *vw);
-};
-
-struct usbvideo {
-	int num_cameras;		/* As allocated */
-	struct usb_driver usbdrv;	/* Interface to the USB stack */
-	char drvName[80];		/* Driver name */
-	struct mutex lock;		/* Mutex protecting camera structures */
-	struct usbvideo_cb cb;		/* Table of callbacks (virtual methods) */
-	struct video_device vdt;	/* Video device template */
-	struct uvd *cam;			/* Array of camera structures */
-	struct module *md_module;	/* Minidriver module */
-};
-
-
-/*
- * This macro retrieves callback address from the struct uvd object.
- * No validity checks are done here, so be sure to check the
- * callback beforehand with VALID_CALLBACK.
- */
-#define	GET_CALLBACK(uvd,cbName) ((uvd)->handle->cb.cbName)
-
-/*
- * This macro returns either callback pointer or NULL. This is safe
- * macro, meaning that most of components of data structures involved
- * may be NULL - this only results in NULL being returned. You may
- * wish to use this macro to make sure that the callback is callable.
- * However keep in mind that those checks take time.
- */
-#define	VALID_CALLBACK(uvd,cbName) ((((uvd) != NULL) && \
-		((uvd)->handle != NULL)) ? GET_CALLBACK(uvd,cbName) : NULL)
-
-int  RingQueue_Dequeue(struct RingQueue *rq, unsigned char *dst, int len);
-int  RingQueue_Enqueue(struct RingQueue *rq, const unsigned char *cdata, int n);
-void RingQueue_WakeUpInterruptible(struct RingQueue *rq);
-void RingQueue_Flush(struct RingQueue *rq);
-
-static inline int RingQueue_GetLength(const struct RingQueue *rq)
-{
-	return (rq->wi - rq->ri + rq->length) & (rq->length-1);
-}
-
-static inline int RingQueue_GetFreeSpace(const struct RingQueue *rq)
-{
-	return rq->length - RingQueue_GetLength(rq);
-}
-
-void usbvideo_DrawLine(
-	struct usbvideo_frame *frame,
-	int x1, int y1,
-	int x2, int y2,
-	unsigned char cr, unsigned char cg, unsigned char cb);
-void usbvideo_HexDump(const unsigned char *data, int len);
-void usbvideo_SayAndWait(const char *what);
-void usbvideo_TestPattern(struct uvd *uvd, int fullframe, int pmode);
-
-/* Memory allocation routines */
-unsigned long usbvideo_kvirt_to_pa(unsigned long adr);
-
-int usbvideo_register(
-	struct usbvideo **pCams,
-	const int num_cams,
-	const int num_extra,
-	const char *driverName,
-	const struct usbvideo_cb *cbTable,
-	struct module *md,
-	const struct usb_device_id *id_table);
-struct uvd *usbvideo_AllocateDevice(struct usbvideo *cams);
-int usbvideo_RegisterVideoDevice(struct uvd *uvd);
-void usbvideo_Deregister(struct usbvideo **uvt);
-
-int usbvideo_v4l_initialize(struct video_device *dev);
-
-void usbvideo_DeinterlaceFrame(struct uvd *uvd, struct usbvideo_frame *frame);
-
-/*
- * This code performs bounds checking - use it when working with
- * new formats, or else you may get oopses all over the place.
- * If pixel falls out of bounds then it gets shoved back (as close
- * to place of offence as possible) and is painted bright red.
- *
- * There are two important concepts: frame width, height and
- * V4L canvas width, height. The former is the area requested by
- * the application -for this very frame-. The latter is the largest
- * possible frame that we can serve (we advertise that via V4L ioctl).
- * The frame data is expected to be formatted as lines of length
- * VIDEOSIZE_X(fr->request), total VIDEOSIZE_Y(frame->request) lines.
- */
-static inline void RGB24_PUTPIXEL(
-	struct usbvideo_frame *fr,
-	int ix, int iy,
-	unsigned char vr,
-	unsigned char vg,
-	unsigned char vb)
-{
-	register unsigned char *pf;
-	int limiter = 0, mx, my;
-	mx = ix;
-	my = iy;
-	if (mx < 0) {
-		mx=0;
-		limiter++;
-	} else if (mx >= VIDEOSIZE_X((fr)->request)) {
-		mx= VIDEOSIZE_X((fr)->request) - 1;
-		limiter++;
-	}
-	if (my < 0) {
-		my = 0;
-		limiter++;
-	} else if (my >= VIDEOSIZE_Y((fr)->request)) {
-		my = VIDEOSIZE_Y((fr)->request) - 1;
-		limiter++;
-	}
-	pf = (fr)->data + V4L_BYTES_PER_PIXEL*((iy)*VIDEOSIZE_X((fr)->request) + (ix));
-	if (limiter) {
-		*pf++ = 0;
-		*pf++ = 0;
-		*pf++ = 0xFF;
-	} else {
-		*pf++ = (vb);
-		*pf++ = (vg);
-		*pf++ = (vr);
-	}
-}
-
-#endif /* usbvideo_h */
diff --git a/drivers/staging/usbvideo/vicam.c b/drivers/staging/usbvideo/vicam.c
deleted file mode 100644
index ecdb121297c9..000000000000
--- a/drivers/staging/usbvideo/vicam.c
+++ /dev/null
@@ -1,952 +0,0 @@
-/*
- * USB ViCam WebCam driver
- * Copyright (c) 2002 Joe Burks (jburks@wavicle.org),
- *                    Christopher L Cheney (ccheney@cheney.cx),
- *                    Pavel Machek (pavel@ucw.cz),
- *                    John Tyner (jtyner@cs.ucr.edu),
- *                    Monroe Williams (monroe@pobox.com)
- *
- * Supports 3COM HomeConnect PC Digital WebCam
- * Supports Compro PS39U WebCam
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- * This source code is based heavily on the CPiA webcam driver which was
- * written by Peter Pregler, Scott J. Bertin and Johannes Erdfelt
- *
- * Portions of this code were also copied from usbvideo.c
- *
- * Special thanks to the whole team at Sourceforge for help making
- * this driver become a reality.  Notably:
- * Andy Armstrong who reverse engineered the color encoding and
- * Pavel Machek and Chris Cheney who worked on reverse engineering the
- *    camera controls and wrote the first generation driver.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include "videodev.h"
-#include <linux/usb.h>
-#include <linux/vmalloc.h>
-#include <linux/mm.h>
-#include <linux/slab.h>
-#include <linux/mutex.h>
-#include <linux/firmware.h>
-#include <linux/ihex.h>
-#include "usbvideo.h"
-
-// #define VICAM_DEBUG
-
-#ifdef VICAM_DEBUG
-#define ADBG(lineno,fmt,args...) printk(fmt, jiffies, __func__, lineno, ##args)
-#define DBG(fmt,args...) ADBG((__LINE__),KERN_DEBUG __FILE__"(%ld):%s (%d):"fmt,##args)
-#else
-#define DBG(fmn,args...) do {} while(0)
-#endif
-
-#define DRIVER_AUTHOR           "Joe Burks, jburks@wavicle.org"
-#define DRIVER_DESC             "ViCam WebCam Driver"
-
-/* Define these values to match your device */
-#define USB_VICAM_VENDOR_ID	0x04c1
-#define USB_VICAM_PRODUCT_ID	0x009d
-#define USB_COMPRO_VENDOR_ID	0x0602
-#define USB_COMPRO_PRODUCT_ID	0x1001
-
-#define VICAM_BYTES_PER_PIXEL   3
-#define VICAM_MAX_READ_SIZE     (512*242+128)
-#define VICAM_MAX_FRAME_SIZE    (VICAM_BYTES_PER_PIXEL*320*240)
-#define VICAM_FRAMES            2
-
-#define VICAM_HEADER_SIZE       64
-
-/* rvmalloc / rvfree copied from usbvideo.c
- *
- * Not sure why these are not yet non-statics which I can reference through
- * usbvideo.h the same as it is in 2.4.20.  I bet this will get fixed sometime
- * in the future.
- *
-*/
-static void *rvmalloc(unsigned long size)
-{
-	void *mem;
-	unsigned long adr;
-
-	size = PAGE_ALIGN(size);
-	mem = vmalloc_32(size);
-	if (!mem)
-		return NULL;
-
-	memset(mem, 0, size); /* Clear the ram out, no junk to the user */
-	adr = (unsigned long) mem;
-	while (size > 0) {
-		SetPageReserved(vmalloc_to_page((void *)adr));
-		adr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-
-	return mem;
-}
-
-static void rvfree(void *mem, unsigned long size)
-{
-	unsigned long adr;
-
-	if (!mem)
-		return;
-
-	adr = (unsigned long) mem;
-	while ((long) size > 0) {
-		ClearPageReserved(vmalloc_to_page((void *)adr));
-		adr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-	vfree(mem);
-}
-
-struct vicam_camera {
-	u16 shutter_speed;	// capture shutter speed
-	u16 gain;		// capture gain
-
-	u8 *raw_image;		// raw data captured from the camera
-	u8 *framebuf;		// processed data in RGB24 format
-	u8 *cntrlbuf;		// area used to send control msgs
-
-	struct video_device vdev;	// v4l video device
-	struct usb_device *udev;	// usb device
-
-	/* guard against simultaneous accesses to the camera */
-	struct mutex cam_lock;
-
-	int is_initialized;
-	u8 open_count;
-	u8 bulkEndpoint;
-	int needsDummyRead;
-};
-
-static int vicam_probe( struct usb_interface *intf, const struct usb_device_id *id);
-static void vicam_disconnect(struct usb_interface *intf);
-static void read_frame(struct vicam_camera *cam, int framenum);
-static void vicam_decode_color(const u8 *, u8 *);
-
-static int __send_control_msg(struct vicam_camera *cam,
-			      u8 request,
-			      u16 value,
-			      u16 index,
-			      unsigned char *cp,
-			      u16 size)
-{
-	int status;
-
-	/* cp must be memory that has been allocated by kmalloc */
-
-	status = usb_control_msg(cam->udev,
-				 usb_sndctrlpipe(cam->udev, 0),
-				 request,
-				 USB_DIR_OUT | USB_TYPE_VENDOR |
-				 USB_RECIP_DEVICE, value, index,
-				 cp, size, 1000);
-
-	status = min(status, 0);
-
-	if (status < 0) {
-		printk(KERN_INFO "Failed sending control message, error %d.\n",
-		       status);
-	}
-
-	return status;
-}
-
-static int send_control_msg(struct vicam_camera *cam,
-			    u8 request,
-			    u16 value,
-			    u16 index,
-			    unsigned char *cp,
-			    u16 size)
-{
-	int status = -ENODEV;
-	mutex_lock(&cam->cam_lock);
-	if (cam->udev) {
-		status = __send_control_msg(cam, request, value,
-					    index, cp, size);
-	}
-	mutex_unlock(&cam->cam_lock);
-	return status;
-}
-static int
-initialize_camera(struct vicam_camera *cam)
-{
-	int err;
-	const struct ihex_binrec *rec;
-	const struct firmware *uninitialized_var(fw);
-
-	err = request_ihex_firmware(&fw, "vicam/firmware.fw", &cam->udev->dev);
-	if (err) {
-		printk(KERN_ERR "Failed to load \"vicam/firmware.fw\": %d\n",
-		       err);
-		return err;
-	}
-
-	for (rec = (void *)fw->data; rec; rec = ihex_next_binrec(rec)) {
-		memcpy(cam->cntrlbuf, rec->data, be16_to_cpu(rec->len));
-
-		err = send_control_msg(cam, 0xff, 0, 0,
-				       cam->cntrlbuf, be16_to_cpu(rec->len));
-		if (err)
-			break;
-	}
-
-	release_firmware(fw);
-
-	return err;
-}
-
-static int
-set_camera_power(struct vicam_camera *cam, int state)
-{
-	int status;
-
-	if ((status = send_control_msg(cam, 0x50, state, 0, NULL, 0)) < 0)
-		return status;
-
-	if (state) {
-		send_control_msg(cam, 0x55, 1, 0, NULL, 0);
-	}
-
-	return 0;
-}
-
-static long
-vicam_ioctl(struct file *file, unsigned int ioctlnr, unsigned long arg)
-{
-	void __user *user_arg = (void __user *)arg;
-	struct vicam_camera *cam = file->private_data;
-	long retval = 0;
-
-	if (!cam)
-		return -ENODEV;
-
-	switch (ioctlnr) {
-		/* query capabilities */
-	case VIDIOCGCAP:
-		{
-			struct video_capability b;
-
-			DBG("VIDIOCGCAP\n");
-			memset(&b, 0, sizeof(b));
-			strcpy(b.name, "ViCam-based Camera");
-			b.type = VID_TYPE_CAPTURE;
-			b.channels = 1;
-			b.audios = 0;
-			b.maxwidth = 320;	/* VIDEOSIZE_CIF */
-			b.maxheight = 240;
-			b.minwidth = 320;	/* VIDEOSIZE_48_48 */
-			b.minheight = 240;
-
-			if (copy_to_user(user_arg, &b, sizeof(b)))
-				retval = -EFAULT;
-
-			break;
-		}
-		/* get/set video source - we are a camera and nothing else */
-	case VIDIOCGCHAN:
-		{
-			struct video_channel v;
-
-			DBG("VIDIOCGCHAN\n");
-			if (copy_from_user(&v, user_arg, sizeof(v))) {
-				retval = -EFAULT;
-				break;
-			}
-			if (v.channel != 0) {
-				retval = -EINVAL;
-				break;
-			}
-
-			v.channel = 0;
-			strcpy(v.name, "Camera");
-			v.tuners = 0;
-			v.flags = 0;
-			v.type = VIDEO_TYPE_CAMERA;
-			v.norm = 0;
-
-			if (copy_to_user(user_arg, &v, sizeof(v)))
-				retval = -EFAULT;
-			break;
-		}
-
-	case VIDIOCSCHAN:
-		{
-			int v;
-
-			if (copy_from_user(&v, user_arg, sizeof(v)))
-				retval = -EFAULT;
-			DBG("VIDIOCSCHAN %d\n", v);
-
-			if (retval == 0 && v != 0)
-				retval = -EINVAL;
-
-			break;
-		}
-
-		/* image properties */
-	case VIDIOCGPICT:
-		{
-			struct video_picture vp;
-			DBG("VIDIOCGPICT\n");
-			memset(&vp, 0, sizeof (struct video_picture));
-			vp.brightness = cam->gain << 8;
-			vp.depth = 24;
-			vp.palette = VIDEO_PALETTE_RGB24;
-			if (copy_to_user(user_arg, &vp, sizeof (struct video_picture)))
-				retval = -EFAULT;
-			break;
-		}
-
-	case VIDIOCSPICT:
-		{
-			struct video_picture vp;
-
-			if (copy_from_user(&vp, user_arg, sizeof(vp))) {
-				retval = -EFAULT;
-				break;
-			}
-
-			DBG("VIDIOCSPICT depth = %d, pal = %d\n", vp.depth,
-			    vp.palette);
-
-			cam->gain = vp.brightness >> 8;
-
-			if (vp.depth != 24
-			    || vp.palette != VIDEO_PALETTE_RGB24)
-				retval = -EINVAL;
-
-			break;
-		}
-
-		/* get/set capture window */
-	case VIDIOCGWIN:
-		{
-			struct video_window vw;
-			vw.x = 0;
-			vw.y = 0;
-			vw.width = 320;
-			vw.height = 240;
-			vw.chromakey = 0;
-			vw.flags = 0;
-			vw.clips = NULL;
-			vw.clipcount = 0;
-
-			DBG("VIDIOCGWIN\n");
-
-			if (copy_to_user(user_arg, (void *)&vw, sizeof(vw)))
-				retval = -EFAULT;
-
-			// I'm not sure what the deal with a capture window is, it is very poorly described
-			// in the doc.  So I won't support it now.
-			break;
-		}
-
-	case VIDIOCSWIN:
-		{
-
-			struct video_window vw;
-
-			if (copy_from_user(&vw, user_arg, sizeof(vw))) {
-				retval = -EFAULT;
-				break;
-			}
-
-			DBG("VIDIOCSWIN %d x %d\n", vw.width, vw.height);
-
-			if ( vw.width != 320 || vw.height != 240 )
-				retval = -EFAULT;
-
-			break;
-		}
-
-		/* mmap interface */
-	case VIDIOCGMBUF:
-		{
-			struct video_mbuf vm;
-			int i;
-
-			DBG("VIDIOCGMBUF\n");
-			memset(&vm, 0, sizeof (vm));
-			vm.size =
-			    VICAM_MAX_FRAME_SIZE * VICAM_FRAMES;
-			vm.frames = VICAM_FRAMES;
-			for (i = 0; i < VICAM_FRAMES; i++)
-				vm.offsets[i] = VICAM_MAX_FRAME_SIZE * i;
-
-			if (copy_to_user(user_arg, (void *)&vm, sizeof(vm)))
-				retval = -EFAULT;
-
-			break;
-		}
-
-	case VIDIOCMCAPTURE:
-		{
-			struct video_mmap vm;
-			// int video_size;
-
-			if (copy_from_user((void *)&vm, user_arg, sizeof(vm))) {
-				retval = -EFAULT;
-				break;
-			}
-
-			DBG("VIDIOCMCAPTURE frame=%d, height=%d, width=%d, format=%d.\n",vm.frame,vm.width,vm.height,vm.format);
-
-			if ( vm.frame >= VICAM_FRAMES || vm.format != VIDEO_PALETTE_RGB24 )
-				retval = -EINVAL;
-
-			// in theory right here we'd start the image capturing
-			// (fill in a bulk urb and submit it asynchronously)
-			//
-			// Instead we're going to do a total hack job for now and
-			// retrieve the frame in VIDIOCSYNC
-
-			break;
-		}
-
-	case VIDIOCSYNC:
-		{
-			int frame;
-
-			if (copy_from_user((void *)&frame, user_arg, sizeof(int))) {
-				retval = -EFAULT;
-				break;
-			}
-			DBG("VIDIOCSYNC: %d\n", frame);
-
-			read_frame(cam, frame);
-			vicam_decode_color(cam->raw_image,
-					   cam->framebuf +
-					   frame * VICAM_MAX_FRAME_SIZE );
-
-			break;
-		}
-
-		/* pointless to implement overlay with this camera */
-	case VIDIOCCAPTURE:
-	case VIDIOCGFBUF:
-	case VIDIOCSFBUF:
-	case VIDIOCKEY:
-		retval = -EINVAL;
-		break;
-
-		/* tuner interface - we have none */
-	case VIDIOCGTUNER:
-	case VIDIOCSTUNER:
-	case VIDIOCGFREQ:
-	case VIDIOCSFREQ:
-		retval = -EINVAL;
-		break;
-
-		/* audio interface - we have none */
-	case VIDIOCGAUDIO:
-	case VIDIOCSAUDIO:
-		retval = -EINVAL;
-		break;
-	default:
-		retval = -ENOIOCTLCMD;
-		break;
-	}
-
-	return retval;
-}
-
-static int
-vicam_open(struct file *file)
-{
-	struct vicam_camera *cam = video_drvdata(file);
-
-	DBG("open\n");
-
-	if (!cam) {
-		printk(KERN_ERR
-		       "vicam video_device improperly initialized");
-		return -EINVAL;
-	}
-
-	/* cam_lock/open_count protects us from simultaneous opens
-	 * ... for now. we probably shouldn't rely on this fact forever.
-	 */
-
-	mutex_lock(&cam->cam_lock);
-	if (cam->open_count > 0) {
-		printk(KERN_INFO
-		       "vicam_open called on already opened camera");
-		mutex_unlock(&cam->cam_lock);
-		return -EBUSY;
-	}
-
-	cam->raw_image = kmalloc(VICAM_MAX_READ_SIZE, GFP_KERNEL);
-	if (!cam->raw_image) {
-		mutex_unlock(&cam->cam_lock);
-		return -ENOMEM;
-	}
-
-	cam->framebuf = rvmalloc(VICAM_MAX_FRAME_SIZE * VICAM_FRAMES);
-	if (!cam->framebuf) {
-		kfree(cam->raw_image);
-		mutex_unlock(&cam->cam_lock);
-		return -ENOMEM;
-	}
-
-	cam->cntrlbuf = kmalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!cam->cntrlbuf) {
-		kfree(cam->raw_image);
-		rvfree(cam->framebuf, VICAM_MAX_FRAME_SIZE * VICAM_FRAMES);
-		mutex_unlock(&cam->cam_lock);
-		return -ENOMEM;
-	}
-
-	cam->needsDummyRead = 1;
-	cam->open_count++;
-
-	file->private_data = cam;
-	mutex_unlock(&cam->cam_lock);
-
-
-	// First upload firmware, then turn the camera on
-
-	if (!cam->is_initialized) {
-		initialize_camera(cam);
-
-		cam->is_initialized = 1;
-	}
-
-	set_camera_power(cam, 1);
-
-	return 0;
-}
-
-static int
-vicam_close(struct file *file)
-{
-	struct vicam_camera *cam = file->private_data;
-	int open_count;
-	struct usb_device *udev;
-
-	DBG("close\n");
-
-	/* it's not the end of the world if
-	 * we fail to turn the camera off.
-	 */
-
-	set_camera_power(cam, 0);
-
-	kfree(cam->raw_image);
-	rvfree(cam->framebuf, VICAM_MAX_FRAME_SIZE * VICAM_FRAMES);
-	kfree(cam->cntrlbuf);
-
-	mutex_lock(&cam->cam_lock);
-
-	cam->open_count--;
-	open_count = cam->open_count;
-	udev = cam->udev;
-
-	mutex_unlock(&cam->cam_lock);
-
-	if (!open_count && !udev) {
-		kfree(cam);
-	}
-
-	return 0;
-}
-
-static void vicam_decode_color(const u8 *data, u8 *rgb)
-{
-	/* vicam_decode_color - Convert from Vicam Y-Cr-Cb to RGB
-	 * Copyright (C) 2002 Monroe Williams (monroe@pobox.com)
-	 */
-
-	int i, prevY, nextY;
-
-	prevY = 512;
-	nextY = 512;
-
-	data += VICAM_HEADER_SIZE;
-
-	for( i = 0; i < 240; i++, data += 512 ) {
-		const int y = ( i * 242 ) / 240;
-
-		int j, prevX, nextX;
-		int Y, Cr, Cb;
-
-		if ( y == 242 - 1 ) {
-			nextY = -512;
-		}
-
-		prevX = 1;
-		nextX = 1;
-
-		for ( j = 0; j < 320; j++, rgb += 3 ) {
-			const int x = ( j * 512 ) / 320;
-			const u8 * const src = &data[x];
-
-			if ( x == 512 - 1 ) {
-				nextX = -1;
-			}
-
-			Cr = ( src[prevX] - src[0] ) +
-				( src[nextX] - src[0] );
-			Cr /= 2;
-
-			Cb = ( src[prevY] - src[prevX + prevY] ) +
-				( src[prevY] - src[nextX + prevY] ) +
-				( src[nextY] - src[prevX + nextY] ) +
-				( src[nextY] - src[nextX + nextY] );
-			Cb /= 4;
-
-			Y = 1160 * ( src[0] + ( Cr / 2 ) - 16 );
-
-			if ( i & 1 ) {
-				int Ct = Cr;
-				Cr = Cb;
-				Cb = Ct;
-			}
-
-			if ( ( x ^ i ) & 1 ) {
-				Cr = -Cr;
-				Cb = -Cb;
-			}
-
-			rgb[0] = clamp( ( ( Y + ( 2017 * Cb ) ) +
-					500 ) / 900, 0, 255 );
-			rgb[1] = clamp( ( ( Y - ( 392 * Cb ) -
-					  ( 813 * Cr ) ) +
-					  500 ) / 1000, 0, 255 );
-			rgb[2] = clamp( ( ( Y + ( 1594 * Cr ) ) +
-					500 ) / 1300, 0, 255 );
-
-			prevX = -1;
-		}
-
-		prevY = -512;
-	}
-}
-
-static void
-read_frame(struct vicam_camera *cam, int framenum)
-{
-	unsigned char *request = cam->cntrlbuf;
-	int realShutter;
-	int n;
-	int actual_length;
-
-	if (cam->needsDummyRead) {
-		cam->needsDummyRead = 0;
-		read_frame(cam, framenum);
-	}
-
-	memset(request, 0, 16);
-	request[0] = cam->gain;	// 0 = 0% gain, FF = 100% gain
-
-	request[1] = 0;	// 512x242 capture
-
-	request[2] = 0x90;	// the function of these two bytes
-	request[3] = 0x07;	// is not yet understood
-
-	if (cam->shutter_speed > 60) {
-		// Short exposure
-		realShutter =
-		    ((-15631900 / cam->shutter_speed) + 260533) / 1000;
-		request[4] = realShutter & 0xFF;
-		request[5] = (realShutter >> 8) & 0xFF;
-		request[6] = 0x03;
-		request[7] = 0x01;
-	} else {
-		// Long exposure
-		realShutter = 15600 / cam->shutter_speed - 1;
-		request[4] = 0;
-		request[5] = 0;
-		request[6] = realShutter & 0xFF;
-		request[7] = realShutter >> 8;
-	}
-
-	// Per John Markus Bjørndalen, byte at index 8 causes problems if it isn't 0
-	request[8] = 0;
-	// bytes 9-15 do not seem to affect exposure or image quality
-
-	mutex_lock(&cam->cam_lock);
-
-	if (!cam->udev) {
-		goto done;
-	}
-
-	n = __send_control_msg(cam, 0x51, 0x80, 0, request, 16);
-
-	if (n < 0) {
-		printk(KERN_ERR
-		       " Problem sending frame capture control message");
-		goto done;
-	}
-
-	n = usb_bulk_msg(cam->udev,
-			 usb_rcvbulkpipe(cam->udev, cam->bulkEndpoint),
-			 cam->raw_image,
-			 512 * 242 + 128, &actual_length, 10000);
-
-	if (n < 0) {
-		printk(KERN_ERR "Problem during bulk read of frame data: %d\n",
-		       n);
-	}
-
- done:
-	mutex_unlock(&cam->cam_lock);
-}
-
-static ssize_t
-vicam_read( struct file *file, char __user *buf, size_t count, loff_t *ppos )
-{
-	struct vicam_camera *cam = file->private_data;
-
-	DBG("read %d bytes.\n", (int) count);
-
-	if (*ppos >= VICAM_MAX_FRAME_SIZE) {
-		*ppos = 0;
-		return 0;
-	}
-
-	if (*ppos == 0) {
-		read_frame(cam, 0);
-		vicam_decode_color(cam->raw_image,
-				   cam->framebuf +
-				   0 * VICAM_MAX_FRAME_SIZE);
-	}
-
-	count = min_t(size_t, count, VICAM_MAX_FRAME_SIZE - *ppos);
-
-	if (copy_to_user(buf, &cam->framebuf[*ppos], count)) {
-		count = -EFAULT;
-	} else {
-		*ppos += count;
-	}
-
-	if (count == VICAM_MAX_FRAME_SIZE) {
-		*ppos = 0;
-	}
-
-	return count;
-}
-
-
-static int
-vicam_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	// TODO: allocate the raw frame buffer if necessary
-	unsigned long page, pos;
-	unsigned long start = vma->vm_start;
-	unsigned long size  = vma->vm_end-vma->vm_start;
-	struct vicam_camera *cam = file->private_data;
-
-	if (!cam)
-		return -ENODEV;
-
-	DBG("vicam_mmap: %ld\n", size);
-
-	/* We let mmap allocate as much as it wants because Linux was adding 2048 bytes
-	 * to the size the application requested for mmap and it was screwing apps up.
-	 if (size > VICAM_FRAMES*VICAM_MAX_FRAME_SIZE)
-	 return -EINVAL;
-	 */
-
-	pos = (unsigned long)cam->framebuf;
-	while (size > 0) {
-		page = vmalloc_to_pfn((void *)pos);
-		if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED))
-			return -EAGAIN;
-
-		start += PAGE_SIZE;
-		pos += PAGE_SIZE;
-		if (size > PAGE_SIZE)
-			size -= PAGE_SIZE;
-		else
-			size = 0;
-	}
-
-	return 0;
-}
-
-static const struct v4l2_file_operations vicam_fops = {
-	.owner		= THIS_MODULE,
-	.open		= vicam_open,
-	.release	= vicam_close,
-	.read		= vicam_read,
-	.mmap		= vicam_mmap,
-	.ioctl		= vicam_ioctl,
-};
-
-static struct video_device vicam_template = {
-	.name 		= "ViCam-based USB Camera",
-	.fops 		= &vicam_fops,
-	.release 	= video_device_release_empty,
-};
-
-/* table of devices that work with this driver */
-static struct usb_device_id vicam_table[] = {
-	{USB_DEVICE(USB_VICAM_VENDOR_ID, USB_VICAM_PRODUCT_ID)},
-	{USB_DEVICE(USB_COMPRO_VENDOR_ID, USB_COMPRO_PRODUCT_ID)},
-	{}			/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, vicam_table);
-
-static struct usb_driver vicam_driver = {
-	.name		= "vicam",
-	.probe		= vicam_probe,
-	.disconnect	= vicam_disconnect,
-	.id_table	= vicam_table
-};
-
-/**
- *	vicam_probe
- *	@intf: the interface
- *	@id: the device id
- *
- *	Called by the usb core when a new device is connected that it thinks
- *	this driver might be interested in.
- */
-static int
-vicam_probe( struct usb_interface *intf, const struct usb_device_id *id)
-{
-	struct usb_device *dev = interface_to_usbdev(intf);
-	int bulkEndpoint = 0;
-	const struct usb_host_interface *interface;
-	const struct usb_endpoint_descriptor *endpoint;
-	struct vicam_camera *cam;
-
-	printk(KERN_INFO "ViCam based webcam connected\n");
-
-	interface = intf->cur_altsetting;
-
-	DBG(KERN_DEBUG "Interface %d. has %u. endpoints!\n",
-	       interface->desc.bInterfaceNumber, (unsigned) (interface->desc.bNumEndpoints));
-	endpoint = &interface->endpoint[0].desc;
-
-	if (usb_endpoint_is_bulk_in(endpoint)) {
-		/* we found a bulk in endpoint */
-		bulkEndpoint = endpoint->bEndpointAddress;
-	} else {
-		printk(KERN_ERR
-		       "No bulk in endpoint was found ?! (this is bad)\n");
-	}
-
-	if ((cam =
-	     kzalloc(sizeof (struct vicam_camera), GFP_KERNEL)) == NULL) {
-		printk(KERN_WARNING
-		       "could not allocate kernel memory for vicam_camera struct\n");
-		return -ENOMEM;
-	}
-
-
-	cam->shutter_speed = 15;
-
-	mutex_init(&cam->cam_lock);
-
-	memcpy(&cam->vdev, &vicam_template, sizeof(vicam_template));
-	video_set_drvdata(&cam->vdev, cam);
-
-	cam->udev = dev;
-	cam->bulkEndpoint = bulkEndpoint;
-
-	if (video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1) < 0) {
-		kfree(cam);
-		printk(KERN_WARNING "video_register_device failed\n");
-		return -EIO;
-	}
-
-	printk(KERN_INFO "ViCam webcam driver now controlling device %s\n",
-		video_device_node_name(&cam->vdev));
-
-	usb_set_intfdata (intf, cam);
-
-	return 0;
-}
-
-static void
-vicam_disconnect(struct usb_interface *intf)
-{
-	int open_count;
-	struct vicam_camera *cam = usb_get_intfdata (intf);
-	usb_set_intfdata (intf, NULL);
-
-	/* we must unregister the device before taking its
-	 * cam_lock. This is because the video open call
-	 * holds the same lock as video unregister. if we
-	 * unregister inside of the cam_lock and open also
-	 * uses the cam_lock, we get deadlock.
-	 */
-
-	video_unregister_device(&cam->vdev);
-
-	/* stop the camera from being used */
-
-	mutex_lock(&cam->cam_lock);
-
-	/* mark the camera as gone */
-
-	cam->udev = NULL;
-
-	/* the only thing left to do is synchronize with
-	 * our close/release function on who should release
-	 * the camera memory. if there are any users using the
-	 * camera, it's their job. if there are no users,
-	 * it's ours.
-	 */
-
-	open_count = cam->open_count;
-
-	mutex_unlock(&cam->cam_lock);
-
-	if (!open_count) {
-		kfree(cam);
-	}
-
-	printk(KERN_DEBUG "ViCam-based WebCam disconnected\n");
-}
-
-/*
- */
-static int __init
-usb_vicam_init(void)
-{
-	int retval;
-	DBG(KERN_INFO "ViCam-based WebCam driver startup\n");
-	retval = usb_register(&vicam_driver);
-	if (retval)
-		printk(KERN_WARNING "usb_register failed!\n");
-	return retval;
-}
-
-static void __exit
-usb_vicam_exit(void)
-{
-	DBG(KERN_INFO
-	       "ViCam-based WebCam driver shutdown\n");
-
-	usb_deregister(&vicam_driver);
-}
-
-module_init(usb_vicam_init);
-module_exit(usb_vicam_exit);
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_LICENSE("GPL");
-MODULE_FIRMWARE("vicam/firmware.fw");
diff --git a/drivers/staging/usbvideo/videodev.h b/drivers/staging/usbvideo/videodev.h
deleted file mode 100644
index f11efbef1c05..000000000000
--- a/drivers/staging/usbvideo/videodev.h
+++ /dev/null
@@ -1,318 +0,0 @@
-/*
- *	Video for Linux version 1 - OBSOLETE
- *
- *	Header file for v4l1 drivers and applications, for
- *	Linux kernels 2.2.x or 2.4.x.
- *
- *	Provides header for legacy drivers and applications
- *
- *	See http://linuxtv.org for more info
- *
- */
-#ifndef __LINUX_VIDEODEV_H
-#define __LINUX_VIDEODEV_H
-
-#include <linux/types.h>
-#include <linux/ioctl.h>
-#include <linux/videodev2.h>
-
-#define VID_TYPE_CAPTURE	1	/* Can capture */
-#define VID_TYPE_TUNER		2	/* Can tune */
-#define VID_TYPE_TELETEXT	4	/* Does teletext */
-#define VID_TYPE_OVERLAY	8	/* Overlay onto frame buffer */
-#define VID_TYPE_CHROMAKEY	16	/* Overlay by chromakey */
-#define VID_TYPE_CLIPPING	32	/* Can clip */
-#define VID_TYPE_FRAMERAM	64	/* Uses the frame buffer memory */
-#define VID_TYPE_SCALES		128	/* Scalable */
-#define VID_TYPE_MONOCHROME	256	/* Monochrome only */
-#define VID_TYPE_SUBCAPTURE	512	/* Can capture subareas of the image */
-#define VID_TYPE_MPEG_DECODER	1024	/* Can decode MPEG streams */
-#define VID_TYPE_MPEG_ENCODER	2048	/* Can encode MPEG streams */
-#define VID_TYPE_MJPEG_DECODER	4096	/* Can decode MJPEG streams */
-#define VID_TYPE_MJPEG_ENCODER	8192	/* Can encode MJPEG streams */
-
-struct video_capability
-{
-	char name[32];
-	int type;
-	int channels;	/* Num channels */
-	int audios;	/* Num audio devices */
-	int maxwidth;	/* Supported width */
-	int maxheight;	/* And height */
-	int minwidth;	/* Supported width */
-	int minheight;	/* And height */
-};
-
-
-struct video_channel
-{
-	int channel;
-	char name[32];
-	int tuners;
-	__u32  flags;
-#define VIDEO_VC_TUNER		1	/* Channel has a tuner */
-#define VIDEO_VC_AUDIO		2	/* Channel has audio */
-	__u16  type;
-#define VIDEO_TYPE_TV		1
-#define VIDEO_TYPE_CAMERA	2
-	__u16 norm;			/* Norm set by channel */
-};
-
-struct video_tuner
-{
-	int tuner;
-	char name[32];
-	unsigned long rangelow, rangehigh;	/* Tuner range */
-	__u32 flags;
-#define VIDEO_TUNER_PAL		1
-#define VIDEO_TUNER_NTSC	2
-#define VIDEO_TUNER_SECAM	4
-#define VIDEO_TUNER_LOW		8	/* Uses KHz not MHz */
-#define VIDEO_TUNER_NORM	16	/* Tuner can set norm */
-#define VIDEO_TUNER_STEREO_ON	128	/* Tuner is seeing stereo */
-#define VIDEO_TUNER_RDS_ON      256     /* Tuner is seeing an RDS datastream */
-#define VIDEO_TUNER_MBS_ON      512     /* Tuner is seeing an MBS datastream */
-	__u16 mode;			/* PAL/NTSC/SECAM/OTHER */
-#define VIDEO_MODE_PAL		0
-#define VIDEO_MODE_NTSC		1
-#define VIDEO_MODE_SECAM	2
-#define VIDEO_MODE_AUTO		3
-	__u16 signal;			/* Signal strength 16bit scale */
-};
-
-struct video_picture
-{
-	__u16	brightness;
-	__u16	hue;
-	__u16	colour;
-	__u16	contrast;
-	__u16	whiteness;	/* Black and white only */
-	__u16	depth;		/* Capture depth */
-	__u16   palette;	/* Palette in use */
-#define VIDEO_PALETTE_GREY	1	/* Linear greyscale */
-#define VIDEO_PALETTE_HI240	2	/* High 240 cube (BT848) */
-#define VIDEO_PALETTE_RGB565	3	/* 565 16 bit RGB */
-#define VIDEO_PALETTE_RGB24	4	/* 24bit RGB */
-#define VIDEO_PALETTE_RGB32	5	/* 32bit RGB */
-#define VIDEO_PALETTE_RGB555	6	/* 555 15bit RGB */
-#define VIDEO_PALETTE_YUV422	7	/* YUV422 capture */
-#define VIDEO_PALETTE_YUYV	8
-#define VIDEO_PALETTE_UYVY	9	/* The great thing about standards is ... */
-#define VIDEO_PALETTE_YUV420	10
-#define VIDEO_PALETTE_YUV411	11	/* YUV411 capture */
-#define VIDEO_PALETTE_RAW	12	/* RAW capture (BT848) */
-#define VIDEO_PALETTE_YUV422P	13	/* YUV 4:2:2 Planar */
-#define VIDEO_PALETTE_YUV411P	14	/* YUV 4:1:1 Planar */
-#define VIDEO_PALETTE_YUV420P	15	/* YUV 4:2:0 Planar */
-#define VIDEO_PALETTE_YUV410P	16	/* YUV 4:1:0 Planar */
-#define VIDEO_PALETTE_PLANAR	13	/* start of planar entries */
-#define VIDEO_PALETTE_COMPONENT 7	/* start of component entries */
-};
-
-struct video_audio
-{
-	int	audio;		/* Audio channel */
-	__u16	volume;		/* If settable */
-	__u16	bass, treble;
-	__u32	flags;
-#define VIDEO_AUDIO_MUTE	1
-#define VIDEO_AUDIO_MUTABLE	2
-#define VIDEO_AUDIO_VOLUME	4
-#define VIDEO_AUDIO_BASS	8
-#define VIDEO_AUDIO_TREBLE	16
-#define VIDEO_AUDIO_BALANCE	32
-	char    name[16];
-#define VIDEO_SOUND_MONO	1
-#define VIDEO_SOUND_STEREO	2
-#define VIDEO_SOUND_LANG1	4
-#define VIDEO_SOUND_LANG2	8
-	__u16   mode;
-	__u16	balance;	/* Stereo balance */
-	__u16	step;		/* Step actual volume uses */
-};
-
-struct video_clip
-{
-	__s32	x,y;
-	__s32	width, height;
-	struct	video_clip *next;	/* For user use/driver use only */
-};
-
-struct video_window
-{
-	__u32	x,y;			/* Position of window */
-	__u32	width,height;		/* Its size */
-	__u32	chromakey;
-	__u32	flags;
-	struct	video_clip __user *clips;	/* Set only */
-	int	clipcount;
-#define VIDEO_WINDOW_INTERLACE	1
-#define VIDEO_WINDOW_CHROMAKEY	16	/* Overlay by chromakey */
-#define VIDEO_CLIP_BITMAP	-1
-/* bitmap is 1024x625, a '1' bit represents a clipped pixel */
-#define VIDEO_CLIPMAP_SIZE	(128 * 625)
-};
-
-struct video_capture
-{
-	__u32 	x,y;			/* Offsets into image */
-	__u32	width, height;		/* Area to capture */
-	__u16	decimation;		/* Decimation divider */
-	__u16	flags;			/* Flags for capture */
-#define VIDEO_CAPTURE_ODD		0	/* Temporal */
-#define VIDEO_CAPTURE_EVEN		1
-};
-
-struct video_buffer
-{
-	void	*base;
-	int	height,width;
-	int	depth;
-	int	bytesperline;
-};
-
-struct video_mmap
-{
-	unsigned	int frame;		/* Frame (0 - n) for double buffer */
-	int		height,width;
-	unsigned	int format;		/* should be VIDEO_PALETTE_* */
-};
-
-struct video_key
-{
-	__u8	key[8];
-	__u32	flags;
-};
-
-struct video_mbuf
-{
-	int	size;		/* Total memory to map */
-	int	frames;		/* Frames */
-	int	offsets[VIDEO_MAX_FRAME];
-};
-
-#define 	VIDEO_NO_UNIT	(-1)
-
-struct video_unit
-{
-	int 	video;		/* Video minor */
-	int	vbi;		/* VBI minor */
-	int	radio;		/* Radio minor */
-	int	audio;		/* Audio minor */
-	int	teletext;	/* Teletext minor */
-};
-
-struct vbi_format {
-	__u32	sampling_rate;	/* in Hz */
-	__u32	samples_per_line;
-	__u32	sample_format;	/* VIDEO_PALETTE_RAW only (1 byte) */
-	__s32	start[2];	/* starting line for each frame */
-	__u32	count[2];	/* count of lines for each frame */
-	__u32	flags;
-#define	VBI_UNSYNC	1	/* can distingues between top/bottom field */
-#define	VBI_INTERLACED	2	/* lines are interlaced */
-};
-
-/* video_info is biased towards hardware mpeg encode/decode */
-/* but it could apply generically to any hardware compressor/decompressor */
-struct video_info
-{
-	__u32	frame_count;	/* frames output since decode/encode began */
-	__u32	h_size;		/* current unscaled horizontal size */
-	__u32	v_size;		/* current unscaled veritcal size */
-	__u32	smpte_timecode;	/* current SMPTE timecode (for current GOP) */
-	__u32	picture_type;	/* current picture type */
-	__u32	temporal_reference;	/* current temporal reference */
-	__u8	user_data[256];	/* user data last found in compressed stream */
-	/* user_data[0] contains user data flags, user_data[1] has count */
-};
-
-/* generic structure for setting playback modes */
-struct video_play_mode
-{
-	int	mode;
-	int	p1;
-	int	p2;
-};
-
-/* for loading microcode / fpga programming */
-struct video_code
-{
-	char	loadwhat[16];	/* name or tag of file being passed */
-	int	datasize;
-	__u8	*data;
-};
-
-#define VIDIOCGCAP		_IOR('v',1,struct video_capability)	/* Get capabilities */
-#define VIDIOCGCHAN		_IOWR('v',2,struct video_channel)	/* Get channel info (sources) */
-#define VIDIOCSCHAN		_IOW('v',3,struct video_channel)	/* Set channel 	*/
-#define VIDIOCGTUNER		_IOWR('v',4,struct video_tuner)		/* Get tuner abilities */
-#define VIDIOCSTUNER		_IOW('v',5,struct video_tuner)		/* Tune the tuner for the current channel */
-#define VIDIOCGPICT		_IOR('v',6,struct video_picture)	/* Get picture properties */
-#define VIDIOCSPICT		_IOW('v',7,struct video_picture)	/* Set picture properties */
-#define VIDIOCCAPTURE		_IOW('v',8,int)				/* Start, end capture */
-#define VIDIOCGWIN		_IOR('v',9, struct video_window)	/* Get the video overlay window */
-#define VIDIOCSWIN		_IOW('v',10, struct video_window)	/* Set the video overlay window - passes clip list for hardware smarts , chromakey etc */
-#define VIDIOCGFBUF		_IOR('v',11, struct video_buffer)	/* Get frame buffer */
-#define VIDIOCSFBUF		_IOW('v',12, struct video_buffer)	/* Set frame buffer - root only */
-#define VIDIOCKEY		_IOR('v',13, struct video_key)		/* Video key event - to dev 255 is to all - cuts capture on all DMA windows with this key (0xFFFFFFFF == all) */
-#define VIDIOCGFREQ		_IOR('v',14, unsigned long)		/* Set tuner */
-#define VIDIOCSFREQ		_IOW('v',15, unsigned long)		/* Set tuner */
-#define VIDIOCGAUDIO		_IOR('v',16, struct video_audio)	/* Get audio info */
-#define VIDIOCSAUDIO		_IOW('v',17, struct video_audio)	/* Audio source, mute etc */
-#define VIDIOCSYNC		_IOW('v',18, int)			/* Sync with mmap grabbing */
-#define VIDIOCMCAPTURE		_IOW('v',19, struct video_mmap)		/* Grab frames */
-#define VIDIOCGMBUF		_IOR('v',20, struct video_mbuf)		/* Memory map buffer info */
-#define VIDIOCGUNIT		_IOR('v',21, struct video_unit)		/* Get attached units */
-#define VIDIOCGCAPTURE		_IOR('v',22, struct video_capture)	/* Get subcapture */
-#define VIDIOCSCAPTURE		_IOW('v',23, struct video_capture)	/* Set subcapture */
-#define VIDIOCSPLAYMODE		_IOW('v',24, struct video_play_mode)	/* Set output video mode/feature */
-#define VIDIOCSWRITEMODE	_IOW('v',25, int)			/* Set write mode */
-#define VIDIOCGPLAYINFO		_IOR('v',26, struct video_info)		/* Get current playback info from hardware */
-#define VIDIOCSMICROCODE	_IOW('v',27, struct video_code)		/* Load microcode into hardware */
-#define	VIDIOCGVBIFMT		_IOR('v',28, struct vbi_format)		/* Get VBI information */
-#define	VIDIOCSVBIFMT		_IOW('v',29, struct vbi_format)		/* Set VBI information */
-
-
-#define BASE_VIDIOCPRIVATE	192		/* 192-255 are private */
-
-/* VIDIOCSWRITEMODE */
-#define VID_WRITE_MPEG_AUD		0
-#define VID_WRITE_MPEG_VID		1
-#define VID_WRITE_OSD			2
-#define VID_WRITE_TTX			3
-#define VID_WRITE_CC			4
-#define VID_WRITE_MJPEG			5
-
-/* VIDIOCSPLAYMODE */
-#define VID_PLAY_VID_OUT_MODE		0
-	/* p1: = VIDEO_MODE_PAL, VIDEO_MODE_NTSC, etc ... */
-#define VID_PLAY_GENLOCK		1
-	/* p1: 0 = OFF, 1 = ON */
-	/* p2: GENLOCK FINE DELAY value */
-#define VID_PLAY_NORMAL			2
-#define VID_PLAY_PAUSE			3
-#define VID_PLAY_SINGLE_FRAME		4
-#define VID_PLAY_FAST_FORWARD		5
-#define VID_PLAY_SLOW_MOTION		6
-#define VID_PLAY_IMMEDIATE_NORMAL	7
-#define VID_PLAY_SWITCH_CHANNELS	8
-#define VID_PLAY_FREEZE_FRAME		9
-#define VID_PLAY_STILL_MODE		10
-#define VID_PLAY_MASTER_MODE		11
-	/* p1: see below */
-#define		VID_PLAY_MASTER_NONE	1
-#define		VID_PLAY_MASTER_VIDEO	2
-#define		VID_PLAY_MASTER_AUDIO	3
-#define VID_PLAY_ACTIVE_SCANLINES	12
-	/* p1 = first active; p2 = last active */
-#define VID_PLAY_RESET			13
-#define VID_PLAY_END_MARK		14
-
-#endif /* __LINUX_VIDEODEV_H */
-
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/staging/winbond/wbusb.c b/drivers/staging/winbond/wbusb.c
index 2163d60c2eaf..3724e1e67ec2 100644
--- a/drivers/staging/winbond/wbusb.c
+++ b/drivers/staging/winbond/wbusb.c
@@ -118,13 +118,14 @@ static void wbsoft_configure_filter(struct ieee80211_hw *dev,
 	*total_flags = new_flags;
 }
 
-static int wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+static void wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct wbsoft_priv *priv = dev->priv;
 
 	if (priv->sMlmeFrame.IsInUsed != PACKET_FREE_TO_USE) {
 		priv->sMlmeFrame.wNumTxMMPDUDiscarded++;
-		return NETDEV_TX_BUSY;
+		kfree_skb(skb);
+		return;
 	}
 
 	priv->sMlmeFrame.IsInUsed = PACKET_COME_FROM_MLME;
@@ -140,8 +141,6 @@ static int wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	 */
 
 	Mds_Tx(priv);
-
-	return NETDEV_TX_OK;
 }
 
 static int wbsoft_start(struct ieee80211_hw *dev)
diff --git a/drivers/target/target_core_hba.c b/drivers/target/target_core_hba.c
index 4bbe8208b241..74d84f7fbb3d 100644
--- a/drivers/target/target_core_hba.c
+++ b/drivers/target/target_core_hba.c
@@ -38,7 +38,6 @@
 
 #include <target/target_core_base.h>
 #include <target/target_core_device.h>
-#include <target/target_core_device.h>
 #include <target/target_core_tpg.h>
 #include <target/target_core_transport.h>
 
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index 158cecbec718..4a109835e420 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -282,6 +282,9 @@ int core_tmr_lun_reset(
 
 			atomic_set(&task->task_active, 0);
 			atomic_set(&task->task_stop, 0);
+		} else {
+			if (atomic_read(&task->task_execute_queue) != 0)
+				transport_remove_task_from_execute_queue(task, dev);
 		}
 		__transport_stop_task_timer(task, &flags);
 
@@ -301,6 +304,7 @@ int core_tmr_lun_reset(
 			DEBUG_LR("LUN_RESET: got t_transport_active = 1 for"
 				" task: %p, t_fe_count: %d dev: %p\n", task,
 				fe_count, dev);
+			atomic_set(&T_TASK(cmd)->t_transport_aborted, 1);
 			spin_unlock_irqrestore(&T_TASK(cmd)->t_state_lock,
 						flags);
 			core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
@@ -310,6 +314,7 @@ int core_tmr_lun_reset(
 		}
 		DEBUG_LR("LUN_RESET: Got t_transport_active = 0 for task: %p,"
 			" t_fe_count: %d dev: %p\n", task, fe_count, dev);
+		atomic_set(&T_TASK(cmd)->t_transport_aborted, 1);
 		spin_unlock_irqrestore(&T_TASK(cmd)->t_state_lock, flags);
 		core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
 
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 236e22d8cfae..4bbf6c147f89 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -1207,7 +1207,7 @@ transport_get_task_from_execute_queue(struct se_device *dev)
  *
  *
  */
-static void transport_remove_task_from_execute_queue(
+void transport_remove_task_from_execute_queue(
 	struct se_task *task,
 	struct se_device *dev)
 {
@@ -5549,7 +5549,8 @@ static void transport_generic_wait_for_tasks(
 
 		atomic_set(&T_TASK(cmd)->transport_lun_stop, 0);
 	}
-	if (!atomic_read(&T_TASK(cmd)->t_transport_active))
+	if (!atomic_read(&T_TASK(cmd)->t_transport_active) ||
+	     atomic_read(&T_TASK(cmd)->t_transport_aborted))
 		goto remove;
 
 	atomic_set(&T_TASK(cmd)->t_transport_stop, 1);
@@ -5956,6 +5957,9 @@ static void transport_processing_shutdown(struct se_device *dev)
 
 			atomic_set(&task->task_active, 0);
 			atomic_set(&task->task_stop, 0);
+		} else {
+			if (atomic_read(&task->task_execute_queue) != 0)
+				transport_remove_task_from_execute_queue(task, dev);
 		}
 		__transport_stop_task_timer(task, &flags);
 
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index f7a5dba3ca23..bf7c687519ef 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -4,7 +4,6 @@
 
 menuconfig THERMAL
 	tristate "Generic Thermal sysfs driver"
-	depends on NET
 	help
 	  Generic Thermal Sysfs driver offers a generic mechanism for
 	  thermal management. Usually it's made up of one or more thermal
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_sys.c
index 7d0e63c79280..713b7ea4a607 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_sys.c
@@ -62,20 +62,6 @@ static DEFINE_MUTEX(thermal_list_lock);
 
 static unsigned int thermal_event_seqnum;
 
-static struct genl_family thermal_event_genl_family = {
-	.id = GENL_ID_GENERATE,
-	.name = THERMAL_GENL_FAMILY_NAME,
-	.version = THERMAL_GENL_VERSION,
-	.maxattr = THERMAL_GENL_ATTR_MAX,
-};
-
-static struct genl_multicast_group thermal_event_mcgrp = {
-	.name = THERMAL_GENL_MCAST_GROUP_NAME,
-};
-
-static int genetlink_init(void);
-static void genetlink_exit(void);
-
 static int get_idr(struct idr *idr, struct mutex *lock, int *id)
 {
 	int err;
@@ -1225,6 +1211,18 @@ void thermal_zone_device_unregister(struct thermal_zone_device *tz)
 
 EXPORT_SYMBOL(thermal_zone_device_unregister);
 
+#ifdef CONFIG_NET
+static struct genl_family thermal_event_genl_family = {
+	.id = GENL_ID_GENERATE,
+	.name = THERMAL_GENL_FAMILY_NAME,
+	.version = THERMAL_GENL_VERSION,
+	.maxattr = THERMAL_GENL_ATTR_MAX,
+};
+
+static struct genl_multicast_group thermal_event_mcgrp = {
+	.name = THERMAL_GENL_MCAST_GROUP_NAME,
+};
+
 int generate_netlink_event(u32 orig, enum events event)
 {
 	struct sk_buff *skb;
@@ -1301,6 +1299,15 @@ static int genetlink_init(void)
 	return result;
 }
 
+static void genetlink_exit(void)
+{
+	genl_unregister_family(&thermal_event_genl_family);
+}
+#else /* !CONFIG_NET */
+static inline int genetlink_init(void) { return 0; }
+static inline void genetlink_exit(void) {}
+#endif /* !CONFIG_NET */
+
 static int __init thermal_init(void)
 {
 	int result = 0;
@@ -1316,11 +1323,6 @@ static int __init thermal_init(void)
 	return result;
 }
 
-static void genetlink_exit(void)
-{
-	genl_unregister_family(&thermal_event_genl_family);
-}
-
 static void __exit thermal_exit(void)
 {
 	class_unregister(&thermal_class);
diff --git a/drivers/tty/hvc/hvcs.c b/drivers/tty/hvc/hvcs.c
index bedc6c1b6fa5..bef238f9ffd7 100644
--- a/drivers/tty/hvc/hvcs.c
+++ b/drivers/tty/hvc/hvcs.c
@@ -292,7 +292,7 @@ struct hvcs_struct {
 	/*
 	 * Any variable below the kref is valid before a tty is connected and
 	 * stays valid after the tty is disconnected.  These shouldn't be
-	 * whacked until the koject refcount reaches zero though some entries
+	 * whacked until the kobject refcount reaches zero though some entries
 	 * may be changed via sysfs initiatives.
 	 */
 	struct kref kref; /* ref count & hvcs_struct lifetime */
@@ -309,6 +309,7 @@ struct hvcs_struct {
 
 static LIST_HEAD(hvcs_structs);
 static DEFINE_SPINLOCK(hvcs_structs_lock);
+static DEFINE_MUTEX(hvcs_init_mutex);
 
 static void hvcs_unthrottle(struct tty_struct *tty);
 static void hvcs_throttle(struct tty_struct *tty);
@@ -340,6 +341,7 @@ static int __devinit hvcs_probe(struct vio_dev *dev,
 static int __devexit hvcs_remove(struct vio_dev *dev);
 static int __init hvcs_module_init(void);
 static void __exit hvcs_module_exit(void);
+static int __devinit hvcs_initialize(void);
 
 #define HVCS_SCHED_READ	0x00000001
 #define HVCS_QUICK_READ	0x00000002
@@ -762,7 +764,7 @@ static int __devinit hvcs_probe(
 	const struct vio_device_id *id)
 {
 	struct hvcs_struct *hvcsd;
-	int index;
+	int index, rc;
 	int retval;
 
 	if (!dev || !id) {
@@ -770,6 +772,13 @@ static int __devinit hvcs_probe(
 		return -EPERM;
 	}
 
+	/* Make sure we are properly initialized */
+	rc = hvcs_initialize();
+	if (rc) {
+		pr_err("HVCS: Failed to initialize core driver.\n");
+		return rc;
+	}
+
 	/* early to avoid cleanup on failure */
 	index = hvcs_get_index();
 	if (index < 0) {
@@ -1464,12 +1473,15 @@ static void hvcs_free_index_list(void)
 	hvcs_index_count = 0;
 }
 
-static int __init hvcs_module_init(void)
+static int __devinit hvcs_initialize(void)
 {
-	int rc;
-	int num_ttys_to_alloc;
+	int rc, num_ttys_to_alloc;
 
-	printk(KERN_INFO "Initializing %s\n", hvcs_driver_string);
+	mutex_lock(&hvcs_init_mutex);
+	if (hvcs_task) {
+		mutex_unlock(&hvcs_init_mutex);
+		return 0;
+	}
 
 	/* Has the user specified an overload with an insmod param? */
 	if (hvcs_parm_num_devs <= 0 ||
@@ -1528,35 +1540,13 @@ static int __init hvcs_module_init(void)
 
 	hvcs_task = kthread_run(khvcsd, NULL, "khvcsd");
 	if (IS_ERR(hvcs_task)) {
-		printk(KERN_ERR "HVCS: khvcsd creation failed.  Driver not loaded.\n");
+		printk(KERN_ERR "HVCS: khvcsd creation failed.\n");
 		rc = -EIO;
 		goto kthread_fail;
 	}
-
-	rc = vio_register_driver(&hvcs_vio_driver);
-	if (rc) {
-		printk(KERN_ERR "HVCS: can't register vio driver\n");
-		goto vio_fail;
-	}
-
-	/*
-	 * This needs to be done AFTER the vio_register_driver() call or else
-	 * the kobjects won't be initialized properly.
-	 */
-	rc = driver_create_file(&(hvcs_vio_driver.driver), &driver_attr_rescan);
-	if (rc) {
-		printk(KERN_ERR "HVCS: sysfs attr create failed\n");
-		goto attr_fail;
-	}
-
-	printk(KERN_INFO "HVCS: driver module inserted.\n");
-
+	mutex_unlock(&hvcs_init_mutex);
 	return 0;
 
-attr_fail:
-	vio_unregister_driver(&hvcs_vio_driver);
-vio_fail:
-	kthread_stop(hvcs_task);
 kthread_fail:
 	kfree(hvcs_pi_buff);
 buff_alloc_fail:
@@ -1566,15 +1556,39 @@ register_fail:
 index_fail:
 	put_tty_driver(hvcs_tty_driver);
 	hvcs_tty_driver = NULL;
+	mutex_unlock(&hvcs_init_mutex);
 	return rc;
 }
 
+static int __init hvcs_module_init(void)
+{
+	int rc = vio_register_driver(&hvcs_vio_driver);
+	if (rc) {
+		printk(KERN_ERR "HVCS: can't register vio driver\n");
+		return rc;
+	}
+
+	pr_info("HVCS: Driver registered.\n");
+
+	/* This needs to be done AFTER the vio_register_driver() call or else
+	 * the kobjects won't be initialized properly.
+	 */
+	rc = driver_create_file(&(hvcs_vio_driver.driver), &driver_attr_rescan);
+	if (rc)
+		pr_warning(KERN_ERR "HVCS: Failed to create rescan file (err %d)\n", rc);
+
+	return 0;
+}
+
 static void __exit hvcs_module_exit(void)
 {
 	/*
 	 * This driver receives hvcs_remove callbacks for each device upon
 	 * module removal.
 	 */
+	vio_unregister_driver(&hvcs_vio_driver);
+	if (!hvcs_task)
+		return;
 
 	/*
 	 * This synchronous operation  will wake the khvcsd kthread if it is
@@ -1589,8 +1603,6 @@ static void __exit hvcs_module_exit(void)
 
 	driver_remove_file(&hvcs_vio_driver.driver, &driver_attr_rescan);
 
-	vio_unregister_driver(&hvcs_vio_driver);
-
 	tty_unregister_driver(hvcs_tty_driver);
 
 	hvcs_free_index_list();
diff --git a/drivers/tty/ipwireless/main.c b/drivers/tty/ipwireless/main.c
index 94b8eb4d691d..444155a305ae 100644
--- a/drivers/tty/ipwireless/main.c
+++ b/drivers/tty/ipwireless/main.c
@@ -78,7 +78,6 @@ static void signalled_reboot_callback(void *callback_data)
 static int ipwireless_probe(struct pcmcia_device *p_dev, void *priv_data)
 {
 	struct ipw_dev *ipw = priv_data;
-	struct resource *io_resource;
 	int ret;
 
 	p_dev->resource[0]->flags &= ~IO_DATA_PATH_WIDTH;
@@ -92,9 +91,12 @@ static int ipwireless_probe(struct pcmcia_device *p_dev, void *priv_data)
 	if (ret)
 		return ret;
 
-	io_resource = request_region(p_dev->resource[0]->start,
-				resource_size(p_dev->resource[0]),
-				IPWIRELESS_PCCARD_NAME);
+	if (!request_region(p_dev->resource[0]->start,
+			    resource_size(p_dev->resource[0]),
+			    IPWIRELESS_PCCARD_NAME)) {
+		ret = -EBUSY;
+		goto exit;
+	}
 
 	p_dev->resource[2]->flags |=
 		WIN_DATA_WIDTH_16 | WIN_MEMORY_TYPE_CM | WIN_ENABLE;
@@ -105,22 +107,25 @@ static int ipwireless_probe(struct pcmcia_device *p_dev, void *priv_data)
 
 	ret = pcmcia_map_mem_page(p_dev, p_dev->resource[2], p_dev->card_addr);
 	if (ret != 0)
-		goto exit2;
+		goto exit1;
 
 	ipw->is_v2_card = resource_size(p_dev->resource[2]) == 0x100;
 
-	ipw->attr_memory = ioremap(p_dev->resource[2]->start,
+	ipw->common_memory = ioremap(p_dev->resource[2]->start,
 				resource_size(p_dev->resource[2]));
-	request_mem_region(p_dev->resource[2]->start,
-			resource_size(p_dev->resource[2]),
-			IPWIRELESS_PCCARD_NAME);
+	if (!request_mem_region(p_dev->resource[2]->start,
+				resource_size(p_dev->resource[2]),
+				IPWIRELESS_PCCARD_NAME)) {
+		ret = -EBUSY;
+		goto exit2;
+	}
 
 	p_dev->resource[3]->flags |= WIN_DATA_WIDTH_16 | WIN_MEMORY_TYPE_AM |
 					WIN_ENABLE;
 	p_dev->resource[3]->end = 0; /* this used to be 0x1000 */
 	ret = pcmcia_request_window(p_dev, p_dev->resource[3], 0);
 	if (ret != 0)
-		goto exit2;
+		goto exit3;
 
 	ret = pcmcia_map_mem_page(p_dev, p_dev->resource[3], 0);
 	if (ret != 0)
@@ -128,23 +133,28 @@ static int ipwireless_probe(struct pcmcia_device *p_dev, void *priv_data)
 
 	ipw->attr_memory = ioremap(p_dev->resource[3]->start,
 				resource_size(p_dev->resource[3]));
-	request_mem_region(p_dev->resource[3]->start,
-			resource_size(p_dev->resource[3]),
-			IPWIRELESS_PCCARD_NAME);
+	if (!request_mem_region(p_dev->resource[3]->start,
+				resource_size(p_dev->resource[3]),
+				IPWIRELESS_PCCARD_NAME)) {
+		ret = -EBUSY;
+		goto exit4;
+	}
 
 	return 0;
 
+exit4:
+	iounmap(ipw->attr_memory);
 exit3:
+	release_mem_region(p_dev->resource[2]->start,
+			resource_size(p_dev->resource[2]));
 exit2:
-	if (ipw->common_memory) {
-		release_mem_region(p_dev->resource[2]->start,
-				resource_size(p_dev->resource[2]));
-		iounmap(ipw->common_memory);
-	}
+	iounmap(ipw->common_memory);
 exit1:
-	release_resource(io_resource);
+	release_region(p_dev->resource[0]->start,
+		       resource_size(p_dev->resource[0]));
+exit:
 	pcmcia_disable_device(p_dev);
-	return -1;
+	return ret;
 }
 
 static int config_ipwireless(struct ipw_dev *ipw)
@@ -219,6 +229,8 @@ exit:
 
 static void release_ipwireless(struct ipw_dev *ipw)
 {
+	release_region(ipw->link->resource[0]->start,
+		       resource_size(ipw->link->resource[0]));
 	if (ipw->common_memory) {
 		release_mem_region(ipw->link->resource[2]->start,
 				resource_size(ipw->link->resource[2]));
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index d9ccbf825095..e1aee37270f5 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -537,8 +537,8 @@ config SERIAL_S3C6400
 
 config SERIAL_S5PV210
 	tristate "Samsung S5PV210 Serial port support"
-	depends on SERIAL_SAMSUNG && (CPU_S5PV210 || CPU_S5P6442 || CPU_S5PV310)
-	select SERIAL_SAMSUNG_UARTS_4 if (CPU_S5PV210 || CPU_S5PV310)
+	depends on SERIAL_SAMSUNG && (CPU_S5PV210 || CPU_S5P6442 || CPU_EXYNOS4210)
+	select SERIAL_SAMSUNG_UARTS_4 if (CPU_S5PV210 || CPU_EXYNOS4210)
 	default y
 	help
 	  Serial port support for Samsung's S5P Family of SoC's
@@ -1110,29 +1110,6 @@ config SERIAL_PMACZILOG_CONSOLE
 	  on your (Power)Mac as the console, you can do so by answering
 	  Y to this option.
 
-config SERIAL_LH7A40X
-	tristate "Sharp LH7A40X embedded UART support"
-	depends on ARM && ARCH_LH7A40X
-	select SERIAL_CORE
-	help
-	  This enables support for the three on-board UARTs of the
-	  Sharp LH7A40X series CPUs.  Choose Y or M.
-
-config SERIAL_LH7A40X_CONSOLE
-	bool "Support for console on Sharp LH7A40X serial port"
-	depends on SERIAL_LH7A40X=y
-	select SERIAL_CORE_CONSOLE
-	help
-	  Say Y here if you wish to use one of the serial ports as the
-	  system console--the system console is the device which
-	  receives all kernel messages and warnings and which allows
-	  logins in single user mode.
-
-	  Even if you say Y here, the currently visible framebuffer console
-	  (/dev/tty0) will still be used as the default system console, but
-	  you can alter that using a kernel command line, for example
-	  "console=ttyAM1".
-
 config SERIAL_CPM
 	tristate "CPM SCC/SMC serial port support"
 	depends on CPM2 || 8xx
@@ -1621,4 +1598,18 @@ config SERIAL_MSM_SMD
 	  Enables userspace clients to read and write to some streaming SMD
 	  ports via tty device interface for MSM chipset.
 
+config SERIAL_MXS_AUART
+	depends on ARCH_MXS
+	tristate "MXS AUART support"
+	select SERIAL_CORE
+	help
+	  This driver supports the MXS Application UART (AUART) port.
+
+config SERIAL_MXS_AUART_CONSOLE
+	bool "MXS AUART console support"
+	depends on SERIAL_MXS_AUART=y
+	select SERIAL_CORE_CONSOLE
+	help
+	  Enable a MXS AUART port to be the system console.
+
 endmenu
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index d94dc005c8a6..fee0690ef8e3 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -54,7 +54,6 @@ obj-$(CONFIG_SERIAL_68328) += 68328serial.o
 obj-$(CONFIG_SERIAL_68360) += 68360serial.o
 obj-$(CONFIG_SERIAL_MCF) += mcf.o
 obj-$(CONFIG_SERIAL_PMACZILOG) += pmac_zilog.o
-obj-$(CONFIG_SERIAL_LH7A40X) += serial_lh7a40x.o
 obj-$(CONFIG_SERIAL_DZ) += dz.o
 obj-$(CONFIG_SERIAL_ZS) += zs.o
 obj-$(CONFIG_SERIAL_SH_SCI) += sh-sci.o
@@ -94,3 +93,4 @@ obj-$(CONFIG_SERIAL_MFD_HSU)	+= mfd.o
 obj-$(CONFIG_SERIAL_IFX6X60)  	+= ifx6x60.o
 obj-$(CONFIG_SERIAL_PCH_UART)	+= pch_uart.o
 obj-$(CONFIG_SERIAL_MSM_SMD)	+= msm_smd_tty.o
+obj-$(CONFIG_SERIAL_MXS_AUART) += mxs-auart.o
diff --git a/drivers/tty/serial/altera_jtaguart.c b/drivers/tty/serial/altera_jtaguart.c
index 8f014bb916b7..60e049b041a7 100644
--- a/drivers/tty/serial/altera_jtaguart.c
+++ b/drivers/tty/serial/altera_jtaguart.c
@@ -466,12 +466,23 @@ static int __devexit altera_jtaguart_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static struct of_device_id altera_jtaguart_match[] = {
+	{ .compatible = "ALTR,juart-1.0", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, altera_jtaguart_match);
+#else
+#define altera_jtaguart_match NULL
+#endif /* CONFIG_OF */
+
 static struct platform_driver altera_jtaguart_platform_driver = {
 	.probe	= altera_jtaguart_probe,
 	.remove	= __devexit_p(altera_jtaguart_remove),
 	.driver	= {
-		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
+		.name		= DRV_NAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= altera_jtaguart_match,
 	},
 };
 
diff --git a/drivers/tty/serial/altera_uart.c b/drivers/tty/serial/altera_uart.c
index 3a573528555e..6d5b036ac783 100644
--- a/drivers/tty/serial/altera_uart.c
+++ b/drivers/tty/serial/altera_uart.c
@@ -24,6 +24,7 @@
 #include <linux/serial.h>
 #include <linux/serial_core.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/io.h>
 #include <linux/altera_uart.h>
 
@@ -507,6 +508,29 @@ static struct uart_driver altera_uart_driver = {
 	.cons		= ALTERA_UART_CONSOLE,
 };
 
+#ifdef CONFIG_OF
+static int altera_uart_get_of_uartclk(struct platform_device *pdev,
+				      struct uart_port *port)
+{
+	int len;
+	const __be32 *clk;
+
+	clk = of_get_property(pdev->dev.of_node, "clock-frequency", &len);
+	if (!clk || len < sizeof(__be32))
+		return -ENODEV;
+
+	port->uartclk = be32_to_cpup(clk);
+
+	return 0;
+}
+#else
+static int altera_uart_get_of_uartclk(struct platform_device *pdev,
+				      struct uart_port *port)
+{
+	return -ENODEV;
+}
+#endif /* CONFIG_OF */
+
 static int __devinit altera_uart_probe(struct platform_device *pdev)
 {
 	struct altera_uart_platform_uart *platp = pdev->dev.platform_data;
@@ -514,6 +538,7 @@ static int __devinit altera_uart_probe(struct platform_device *pdev)
 	struct resource *res_mem;
 	struct resource *res_irq;
 	int i = pdev->id;
+	int ret;
 
 	/* -1 emphasizes that the platform must have one port, no .N suffix */
 	if (i == -1)
@@ -538,6 +563,15 @@ static int __devinit altera_uart_probe(struct platform_device *pdev)
 	else if (platp->irq)
 		port->irq = platp->irq;
 
+	/* Check platform data first so we can override device node data */
+	if (platp)
+		port->uartclk = platp->uartclk;
+	else {
+		ret = altera_uart_get_of_uartclk(pdev, port);
+		if (ret)
+			return ret;
+	}
+
 	port->membase = ioremap(port->mapbase, ALTERA_UART_SIZE);
 	if (!port->membase)
 		return -ENOMEM;
@@ -550,7 +584,6 @@ static int __devinit altera_uart_probe(struct platform_device *pdev)
 	port->line = i;
 	port->type = PORT_ALTERA_UART;
 	port->iotype = SERIAL_IO_MEM;
-	port->uartclk = platp->uartclk;
 	port->ops = &altera_uart_ops;
 	port->flags = UPF_BOOT_AUTOCONF;
 
@@ -573,13 +606,23 @@ static int __devexit altera_uart_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static struct of_device_id altera_uart_match[] = {
+	{ .compatible = "ALTR,uart-1.0", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, altera_uart_match);
+#else
+#define altera_uart_match NULL
+#endif /* CONFIG_OF */
+
 static struct platform_driver altera_uart_platform_driver = {
 	.probe	= altera_uart_probe,
 	.remove	= __devexit_p(altera_uart_remove),
 	.driver	= {
-		.name	= DRV_NAME,
-		.owner	= THIS_MODULE,
-		.pm	= NULL,
+		.name		= DRV_NAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= altera_uart_match,
 	},
 };
 
diff --git a/drivers/tty/serial/amba-pl010.c b/drivers/tty/serial/amba-pl010.c
index 2904aa044126..d742dd2c525c 100644
--- a/drivers/tty/serial/amba-pl010.c
+++ b/drivers/tty/serial/amba-pl010.c
@@ -676,7 +676,7 @@ static struct uart_driver amba_reg = {
 	.cons			= AMBA_CONSOLE,
 };
 
-static int pl010_probe(struct amba_device *dev, struct amba_id *id)
+static int pl010_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct uart_amba_port *uap;
 	void __iomem *base;
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index e76d7d000128..0dae46f91021 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -1349,7 +1349,7 @@ static struct uart_driver amba_reg = {
 	.cons			= AMBA_CONSOLE,
 };
 
-static int pl011_probe(struct amba_device *dev, struct amba_id *id)
+static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct uart_amba_port *uap;
 	struct vendor_data *vendor = id->data;
diff --git a/drivers/tty/serial/apbuart.c b/drivers/tty/serial/apbuart.c
index 095a5d562618..1ab999b04ef3 100644
--- a/drivers/tty/serial/apbuart.c
+++ b/drivers/tty/serial/apbuart.c
@@ -553,8 +553,7 @@ static struct uart_driver grlib_apbuart_driver = {
 /* OF Platform Driver                                                       */
 /* ======================================================================== */
 
-static int __devinit apbuart_probe(struct platform_device *op,
-				   const struct of_device_id *match)
+static int __devinit apbuart_probe(struct platform_device *op)
 {
 	int i = -1;
 	struct uart_port *port = NULL;
@@ -587,7 +586,7 @@ static struct of_device_id __initdata apbuart_match[] = {
 	{},
 };
 
-static struct of_platform_driver grlib_apbuart_of_driver = {
+static struct platform_driver grlib_apbuart_of_driver = {
 	.probe = apbuart_probe,
 	.driver = {
 		.owner = THIS_MODULE,
@@ -676,10 +675,10 @@ static int __init grlib_apbuart_init(void)
 		return ret;
 	}
 
-	ret = of_register_platform_driver(&grlib_apbuart_of_driver);
+	ret = platform_driver_register(&grlib_apbuart_of_driver);
 	if (ret) {
 		printk(KERN_ERR
-		       "%s: of_register_platform_driver failed (%i)\n",
+		       "%s: platform_driver_register failed (%i)\n",
 		       __FILE__, ret);
 		uart_unregister_driver(&grlib_apbuart_driver);
 		return ret;
@@ -697,7 +696,7 @@ static void __exit grlib_apbuart_exit(void)
 				     &grlib_apbuart_ports[i]);
 
 	uart_unregister_driver(&grlib_apbuart_driver);
-	of_unregister_platform_driver(&grlib_apbuart_of_driver);
+	platform_driver_unregister(&grlib_apbuart_of_driver);
 }
 
 module_init(grlib_apbuart_init);
diff --git a/drivers/tty/serial/cpm_uart/cpm_uart_core.c b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
index 8692ff98fc07..a9a6a5fd169e 100644
--- a/drivers/tty/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
@@ -1359,8 +1359,7 @@ static struct uart_driver cpm_reg = {
 
 static int probe_index;
 
-static int __devinit cpm_uart_probe(struct platform_device *ofdev,
-                                    const struct of_device_id *match)
+static int __devinit cpm_uart_probe(struct platform_device *ofdev)
 {
 	int index = probe_index++;
 	struct uart_cpm_port *pinfo = &cpm_uart_ports[index];
@@ -1405,7 +1404,7 @@ static struct of_device_id cpm_uart_match[] = {
 	{}
 };
 
-static struct of_platform_driver cpm_uart_driver = {
+static struct platform_driver cpm_uart_driver = {
 	.driver = {
 		.name = "cpm_uart",
 		.owner = THIS_MODULE,
@@ -1421,7 +1420,7 @@ static int __init cpm_uart_init(void)
 	if (ret)
 		return ret;
 
-	ret = of_register_platform_driver(&cpm_uart_driver);
+	ret = platform_driver_register(&cpm_uart_driver);
 	if (ret)
 		uart_unregister_driver(&cpm_reg);
 
@@ -1430,7 +1429,7 @@ static int __init cpm_uart_init(void)
 
 static void __exit cpm_uart_exit(void)
 {
-	of_unregister_platform_driver(&cpm_uart_driver);
+	platform_driver_unregister(&cpm_uart_driver);
 	uart_unregister_driver(&cpm_reg);
 }
 
diff --git a/drivers/tty/serial/kgdboc.c b/drivers/tty/serial/kgdboc.c
index 25a8bc565f40..87e7e6c876d4 100644
--- a/drivers/tty/serial/kgdboc.c
+++ b/drivers/tty/serial/kgdboc.c
@@ -131,7 +131,7 @@ static void kgdboc_unregister_kbd(void)
 
 static int kgdboc_option_setup(char *opt)
 {
-	if (strlen(opt) > MAX_CONFIG_LEN) {
+	if (strlen(opt) >= MAX_CONFIG_LEN) {
 		printk(KERN_ERR "kgdboc: config string too long\n");
 		return -ENOSPC;
 	}
diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index 126ec7f568ec..a0bcd8a3758d 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -1302,8 +1302,7 @@ static struct of_device_id mpc52xx_uart_of_match[] = {
 	{},
 };
 
-static int __devinit
-mpc52xx_uart_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit mpc52xx_uart_of_probe(struct platform_device *op)
 {
 	int idx = -1;
 	unsigned int uartclk;
@@ -1311,8 +1310,6 @@ mpc52xx_uart_of_probe(struct platform_device *op, const struct of_device_id *mat
 	struct resource res;
 	int ret;
 
-	dev_dbg(&op->dev, "mpc52xx_uart_probe(op=%p, match=%p)\n", op, match);
-
 	/* Check validity & presence */
 	for (idx = 0; idx < MPC52xx_PSC_MAXNUM; idx++)
 		if (mpc52xx_uart_nodes[idx] == op->dev.of_node)
@@ -1453,7 +1450,7 @@ mpc52xx_uart_of_enumerate(void)
 
 MODULE_DEVICE_TABLE(of, mpc52xx_uart_of_match);
 
-static struct of_platform_driver mpc52xx_uart_of_driver = {
+static struct platform_driver mpc52xx_uart_of_driver = {
 	.probe		= mpc52xx_uart_of_probe,
 	.remove		= mpc52xx_uart_of_remove,
 #ifdef CONFIG_PM
@@ -1497,9 +1494,9 @@ mpc52xx_uart_init(void)
 			return ret;
 	}
 
-	ret = of_register_platform_driver(&mpc52xx_uart_of_driver);
+	ret = platform_driver_register(&mpc52xx_uart_of_driver);
 	if (ret) {
-		printk(KERN_ERR "%s: of_register_platform_driver failed (%i)\n",
+		printk(KERN_ERR "%s: platform_driver_register failed (%i)\n",
 		       __FILE__, ret);
 		uart_unregister_driver(&mpc52xx_uart_driver);
 		return ret;
@@ -1514,7 +1511,7 @@ mpc52xx_uart_exit(void)
 	if (psc_ops->fifoc_uninit)
 		psc_ops->fifoc_uninit();
 
-	of_unregister_platform_driver(&mpc52xx_uart_of_driver);
+	platform_driver_unregister(&mpc52xx_uart_of_driver);
 	uart_unregister_driver(&mpc52xx_uart_driver);
 }
 
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 8e43a7b69e64..bfee9b4c6661 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2007 Google, Inc.
  * Author: Robert Love <rlove@google.com>
+ * Copyright (c) 2011, Code Aurora Forum. 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
@@ -31,6 +32,7 @@
 #include <linux/serial.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
+#include <linux/delay.h>
 
 #include "msm_serial.h"
 
@@ -38,9 +40,20 @@ struct msm_port {
 	struct uart_port	uart;
 	char			name[16];
 	struct clk		*clk;
+	struct clk		*pclk;
 	unsigned int		imr;
+	unsigned int            *gsbi_base;
+	int			is_uartdm;
+	unsigned int		old_snap_state;
 };
 
+static inline void wait_for_xmitr(struct uart_port *port, int bits)
+{
+	if (!(msm_read(port, UART_SR) & UART_SR_TX_EMPTY))
+		while ((msm_read(port, UART_ISR) & bits) != bits)
+			cpu_relax();
+}
+
 static void msm_stop_tx(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
@@ -73,6 +86,61 @@ static void msm_enable_ms(struct uart_port *port)
 	msm_write(port, msm_port->imr, UART_IMR);
 }
 
+static void handle_rx_dm(struct uart_port *port, unsigned int misr)
+{
+	struct tty_struct *tty = port->state->port.tty;
+	unsigned int sr;
+	int count = 0;
+	struct msm_port *msm_port = UART_TO_MSM(port);
+
+	if ((msm_read(port, UART_SR) & UART_SR_OVERRUN)) {
+		port->icount.overrun++;
+		tty_insert_flip_char(tty, 0, TTY_OVERRUN);
+		msm_write(port, UART_CR_CMD_RESET_ERR, UART_CR);
+	}
+
+	if (misr & UART_IMR_RXSTALE) {
+		count = msm_read(port, UARTDM_RX_TOTAL_SNAP) -
+			msm_port->old_snap_state;
+		msm_port->old_snap_state = 0;
+	} else {
+		count = 4 * (msm_read(port, UART_RFWR));
+		msm_port->old_snap_state += count;
+	}
+
+	/* TODO: Precise error reporting */
+
+	port->icount.rx += count;
+
+	while (count > 0) {
+		unsigned int c;
+
+		sr = msm_read(port, UART_SR);
+		if ((sr & UART_SR_RX_READY) == 0) {
+			msm_port->old_snap_state -= count;
+			break;
+		}
+		c = msm_read(port, UARTDM_RF);
+		if (sr & UART_SR_RX_BREAK) {
+			port->icount.brk++;
+			if (uart_handle_break(port))
+				continue;
+		} else if (sr & UART_SR_PAR_FRAME_ERR)
+			port->icount.frame++;
+
+		/* TODO: handle sysrq */
+		tty_insert_flip_string(tty, (char *) &c,
+				       (count > 4) ? 4 : count);
+		count -= 4;
+	}
+
+	tty_flip_buffer_push(tty);
+	if (misr & (UART_IMR_RXSTALE))
+		msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
+	msm_write(port, 0xFFFFFF, UARTDM_DMRX);
+	msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE, UART_CR);
+}
+
 static void handle_rx(struct uart_port *port)
 {
 	struct tty_struct *tty = port->state->port.tty;
@@ -121,6 +189,12 @@ static void handle_rx(struct uart_port *port)
 	tty_flip_buffer_push(tty);
 }
 
+static void reset_dm_count(struct uart_port *port)
+{
+	wait_for_xmitr(port, UART_ISR_TX_READY);
+	msm_write(port, 1, UARTDM_NCF_TX);
+}
+
 static void handle_tx(struct uart_port *port)
 {
 	struct circ_buf *xmit = &port->state->xmit;
@@ -128,11 +202,18 @@ static void handle_tx(struct uart_port *port)
 	int sent_tx;
 
 	if (port->x_char) {
-		msm_write(port, port->x_char, UART_TF);
+		if (msm_port->is_uartdm)
+			reset_dm_count(port);
+
+		msm_write(port, port->x_char,
+			  msm_port->is_uartdm ? UARTDM_TF : UART_TF);
 		port->icount.tx++;
 		port->x_char = 0;
 	}
 
+	if (msm_port->is_uartdm)
+		reset_dm_count(port);
+
 	while (msm_read(port, UART_SR) & UART_SR_TX_READY) {
 		if (uart_circ_empty(xmit)) {
 			/* disable tx interrupts */
@@ -140,8 +221,11 @@ static void handle_tx(struct uart_port *port)
 			msm_write(port, msm_port->imr, UART_IMR);
 			break;
 		}
+		msm_write(port, xmit->buf[xmit->tail],
+			  msm_port->is_uartdm ? UARTDM_TF : UART_TF);
 
-		msm_write(port, xmit->buf[xmit->tail], UART_TF);
+		if (msm_port->is_uartdm)
+			reset_dm_count(port);
 
 		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
 		port->icount.tx++;
@@ -169,8 +253,12 @@ static irqreturn_t msm_irq(int irq, void *dev_id)
 	misr = msm_read(port, UART_MISR);
 	msm_write(port, 0, UART_IMR); /* disable interrupt */
 
-	if (misr & (UART_IMR_RXLEV | UART_IMR_RXSTALE))
-		handle_rx(port);
+	if (misr & (UART_IMR_RXLEV | UART_IMR_RXSTALE)) {
+		if (msm_port->is_uartdm)
+			handle_rx_dm(port, misr);
+		else
+			handle_rx(port);
+	}
 	if (misr & UART_IMR_TXLEV)
 		handle_tx(port);
 	if (misr & UART_IMR_DELTA_CTS)
@@ -192,10 +280,21 @@ static unsigned int msm_get_mctrl(struct uart_port *port)
 	return TIOCM_CAR | TIOCM_CTS | TIOCM_DSR | TIOCM_RTS;
 }
 
-static void msm_set_mctrl(struct uart_port *port, unsigned int mctrl)
+
+static void msm_reset(struct uart_port *port)
 {
-	unsigned int mr;
+	/* reset everything */
+	msm_write(port, UART_CR_CMD_RESET_RX, UART_CR);
+	msm_write(port, UART_CR_CMD_RESET_TX, UART_CR);
+	msm_write(port, UART_CR_CMD_RESET_ERR, UART_CR);
+	msm_write(port, UART_CR_CMD_RESET_BREAK_INT, UART_CR);
+	msm_write(port, UART_CR_CMD_RESET_CTS, UART_CR);
+	msm_write(port, UART_CR_CMD_SET_RFR, UART_CR);
+}
 
+void msm_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	unsigned int mr;
 	mr = msm_read(port, UART_MR1);
 
 	if (!(mctrl & TIOCM_RTS)) {
@@ -219,6 +318,7 @@ static void msm_break_ctl(struct uart_port *port, int break_ctl)
 static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
 {
 	unsigned int baud_code, rxstale, watermark;
+	struct msm_port *msm_port = UART_TO_MSM(port);
 
 	switch (baud) {
 	case 300:
@@ -273,6 +373,9 @@ static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
 		break;
 	}
 
+	if (msm_port->is_uartdm)
+		msm_write(port, UART_CR_CMD_RESET_RX, UART_CR);
+
 	msm_write(port, baud_code, UART_CSR);
 
 	/* RX stale watermark */
@@ -288,25 +391,23 @@ static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
 	/* set TX watermark */
 	msm_write(port, 10, UART_TFWR);
 
+	if (msm_port->is_uartdm) {
+		msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
+		msm_write(port, 0xFFFFFF, UARTDM_DMRX);
+		msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE, UART_CR);
+	}
+
 	return baud;
 }
 
-static void msm_reset(struct uart_port *port)
-{
-	/* reset everything */
-	msm_write(port, UART_CR_CMD_RESET_RX, UART_CR);
-	msm_write(port, UART_CR_CMD_RESET_TX, UART_CR);
-	msm_write(port, UART_CR_CMD_RESET_ERR, UART_CR);
-	msm_write(port, UART_CR_CMD_RESET_BREAK_INT, UART_CR);
-	msm_write(port, UART_CR_CMD_RESET_CTS, UART_CR);
-	msm_write(port, UART_CR_CMD_SET_RFR, UART_CR);
-}
 
 static void msm_init_clock(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
 
 	clk_enable(msm_port->clk);
+	if (!IS_ERR(msm_port->pclk))
+		clk_enable(msm_port->pclk);
 	msm_serial_set_mnd_regs(port);
 }
 
@@ -347,15 +448,31 @@ static int msm_startup(struct uart_port *port)
 		msm_write(port, data, UART_IPR);
 	}
 
-	msm_reset(port);
+	data = 0;
+	if (!port->cons || (port->cons && !(port->cons->flags & CON_ENABLED))) {
+		msm_write(port, UART_CR_CMD_PROTECTION_EN, UART_CR);
+		msm_reset(port);
+		data = UART_CR_TX_ENABLE;
+	}
+
+	data |= UART_CR_RX_ENABLE;
+	msm_write(port, data, UART_CR);	/* enable TX & RX */
 
-	msm_write(port, 0x05, UART_CR);	/* enable TX & RX */
+	/* Make sure IPR is not 0 to start with*/
+	if (msm_port->is_uartdm)
+		msm_write(port, UART_IPR_STALE_LSB, UART_IPR);
 
 	/* turn on RX and CTS interrupts */
 	msm_port->imr = UART_IMR_RXLEV | UART_IMR_RXSTALE |
 			UART_IMR_CURRENT_CTS;
-	msm_write(port, msm_port->imr, UART_IMR);
 
+	if (msm_port->is_uartdm) {
+		msm_write(port, 0xFFFFFF, UARTDM_DMRX);
+		msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
+		msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE, UART_CR);
+	}
+
+	msm_write(port, msm_port->imr, UART_IMR);
 	return 0;
 }
 
@@ -384,7 +501,7 @@ static void msm_set_termios(struct uart_port *port, struct ktermios *termios,
 	baud = msm_set_baud_rate(port, baud);
 	if (tty_termios_baud_rate(termios))
 		tty_termios_encode_baud_rate(termios, baud, baud);
-	
+
 	/* calculate parity */
 	mr = msm_read(port, UART_MR2);
 	mr &= ~UART_MR2_PARITY_MODE;
@@ -454,48 +571,105 @@ static const char *msm_type(struct uart_port *port)
 static void msm_release_port(struct uart_port *port)
 {
 	struct platform_device *pdev = to_platform_device(port->dev);
-	struct resource *resource;
+	struct msm_port *msm_port = UART_TO_MSM(port);
+	struct resource *uart_resource;
+	struct resource *gsbi_resource;
 	resource_size_t size;
 
-	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (unlikely(!resource))
+	uart_resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (unlikely(!uart_resource))
 		return;
-	size = resource->end - resource->start + 1;
+	size = resource_size(uart_resource);
 
 	release_mem_region(port->mapbase, size);
 	iounmap(port->membase);
 	port->membase = NULL;
+
+	if (msm_port->gsbi_base) {
+		iowrite32(GSBI_PROTOCOL_IDLE, msm_port->gsbi_base +
+			  GSBI_CONTROL);
+
+		gsbi_resource = platform_get_resource_byname(pdev,
+							     IORESOURCE_MEM,
+							     "gsbi_resource");
+
+		if (unlikely(!gsbi_resource))
+			return;
+
+		size = resource_size(gsbi_resource);
+		release_mem_region(gsbi_resource->start, size);
+		iounmap(msm_port->gsbi_base);
+		msm_port->gsbi_base = NULL;
+	}
 }
 
 static int msm_request_port(struct uart_port *port)
 {
+	struct msm_port *msm_port = UART_TO_MSM(port);
 	struct platform_device *pdev = to_platform_device(port->dev);
-	struct resource *resource;
+	struct resource *uart_resource;
+	struct resource *gsbi_resource;
 	resource_size_t size;
+	int ret;
 
-	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (unlikely(!resource))
+	uart_resource = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						     "uart_resource");
+	if (unlikely(!uart_resource))
 		return -ENXIO;
-	size = resource->end - resource->start + 1;
 
-	if (unlikely(!request_mem_region(port->mapbase, size, "msm_serial")))
+	size = resource_size(uart_resource);
+
+	if (!request_mem_region(port->mapbase, size, "msm_serial"))
 		return -EBUSY;
 
 	port->membase = ioremap(port->mapbase, size);
 	if (!port->membase) {
-		release_mem_region(port->mapbase, size);
-		return -EBUSY;
+		ret = -EBUSY;
+		goto fail_release_port;
+	}
+
+	gsbi_resource = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						     "gsbi_resource");
+	/* Is this a GSBI-based port? */
+	if (gsbi_resource) {
+		size = resource_size(gsbi_resource);
+
+		if (!request_mem_region(gsbi_resource->start, size,
+						 "msm_serial")) {
+			ret = -EBUSY;
+			goto fail_release_port;
+		}
+
+		msm_port->gsbi_base = ioremap(gsbi_resource->start, size);
+		if (!msm_port->gsbi_base) {
+			ret = -EBUSY;
+			goto fail_release_gsbi;
+		}
 	}
 
 	return 0;
+
+fail_release_gsbi:
+	release_mem_region(gsbi_resource->start, size);
+fail_release_port:
+	release_mem_region(port->mapbase, size);
+	return ret;
 }
 
 static void msm_config_port(struct uart_port *port, int flags)
 {
+	struct msm_port *msm_port = UART_TO_MSM(port);
+	int ret;
 	if (flags & UART_CONFIG_TYPE) {
 		port->type = PORT_MSM;
-		msm_request_port(port);
+		ret = msm_request_port(port);
+		if (ret)
+			return;
 	}
+
+	if (msm_port->is_uartdm)
+		iowrite32(GSBI_PROTOCOL_UART, msm_port->gsbi_base +
+			  GSBI_CONTROL);
 }
 
 static int msm_verify_port(struct uart_port *port, struct serial_struct *ser)
@@ -515,9 +689,13 @@ static void msm_power(struct uart_port *port, unsigned int state,
 	switch (state) {
 	case 0:
 		clk_enable(msm_port->clk);
+		if (!IS_ERR(msm_port->pclk))
+			clk_enable(msm_port->pclk);
 		break;
 	case 3:
 		clk_disable(msm_port->clk);
+		if (!IS_ERR(msm_port->pclk))
+			clk_disable(msm_port->pclk);
 		break;
 	default:
 		printk(KERN_ERR "msm_serial: Unknown PM state %d\n", state);
@@ -550,7 +728,7 @@ static struct msm_port msm_uart_ports[] = {
 			.iotype = UPIO_MEM,
 			.ops = &msm_uart_pops,
 			.flags = UPF_BOOT_AUTOCONF,
-			.fifosize = 512,
+			.fifosize = 64,
 			.line = 0,
 		},
 	},
@@ -559,7 +737,7 @@ static struct msm_port msm_uart_ports[] = {
 			.iotype = UPIO_MEM,
 			.ops = &msm_uart_pops,
 			.flags = UPF_BOOT_AUTOCONF,
-			.fifosize = 512,
+			.fifosize = 64,
 			.line = 1,
 		},
 	},
@@ -585,9 +763,14 @@ static inline struct uart_port *get_port_from_line(unsigned int line)
 
 static void msm_console_putchar(struct uart_port *port, int c)
 {
+	struct msm_port *msm_port = UART_TO_MSM(port);
+
+	if (msm_port->is_uartdm)
+		reset_dm_count(port);
+
 	while (!(msm_read(port, UART_SR) & UART_SR_TX_READY))
 		;
-	msm_write(port, c, UART_TF);
+	msm_write(port, c, msm_port->is_uartdm ? UARTDM_TF : UART_TF);
 }
 
 static void msm_console_write(struct console *co, const char *s,
@@ -609,12 +792,14 @@ static void msm_console_write(struct console *co, const char *s,
 static int __init msm_console_setup(struct console *co, char *options)
 {
 	struct uart_port *port;
+	struct msm_port *msm_port;
 	int baud, flow, bits, parity;
 
 	if (unlikely(co->index >= UART_NR || co->index < 0))
 		return -ENXIO;
 
 	port = get_port_from_line(co->index);
+	msm_port = UART_TO_MSM(port);
 
 	if (unlikely(!port->membase))
 		return -ENXIO;
@@ -638,6 +823,11 @@ static int __init msm_console_setup(struct console *co, char *options)
 
 	msm_reset(port);
 
+	if (msm_port->is_uartdm) {
+		msm_write(port, UART_CR_CMD_PROTECTION_EN, UART_CR);
+		msm_write(port, UART_CR_TX_ENABLE, UART_CR);
+	}
+
 	printk(KERN_INFO "msm_serial: console setup on port #%d\n", port->line);
 
 	return uart_set_options(port, co, baud, parity, bits, flow);
@@ -685,14 +875,32 @@ static int __init msm_serial_probe(struct platform_device *pdev)
 	port->dev = &pdev->dev;
 	msm_port = UART_TO_MSM(port);
 
-	msm_port->clk = clk_get(&pdev->dev, "uart_clk");
-	if (IS_ERR(msm_port->clk))
-		return PTR_ERR(msm_port->clk);
+	if (platform_get_resource_byname(pdev, IORESOURCE_MEM, "gsbi_resource"))
+		msm_port->is_uartdm = 1;
+	else
+		msm_port->is_uartdm = 0;
+
+	if (msm_port->is_uartdm) {
+		msm_port->clk = clk_get(&pdev->dev, "gsbi_uart_clk");
+		msm_port->pclk = clk_get(&pdev->dev, "gsbi_pclk");
+	} else {
+		msm_port->clk = clk_get(&pdev->dev, "uart_clk");
+		msm_port->pclk = ERR_PTR(-ENOENT);
+	}
+
+	if (unlikely(IS_ERR(msm_port->clk) || (IS_ERR(msm_port->pclk) &&
+					       msm_port->is_uartdm)))
+			return PTR_ERR(msm_port->clk);
+
+	if (msm_port->is_uartdm)
+		clk_set_rate(msm_port->clk, 7372800);
+
 	port->uartclk = clk_get_rate(msm_port->clk);
 	printk(KERN_INFO "uartclk = %d\n", port->uartclk);
 
 
-	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	resource = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						     "uart_resource");
 	if (unlikely(!resource))
 		return -ENXIO;
 	port->mapbase = resource->start;
diff --git a/drivers/tty/serial/msm_serial.h b/drivers/tty/serial/msm_serial.h
index f6ca9ca79e98..9b8dc5d0d855 100644
--- a/drivers/tty/serial/msm_serial.h
+++ b/drivers/tty/serial/msm_serial.h
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2007 Google, Inc.
  * Author: Robert Love <rlove@google.com>
+ * Copyright (c) 2011, Code Aurora Forum. 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
@@ -54,6 +55,7 @@
 #define UART_CSR_300	0x22
 
 #define UART_TF		0x000C
+#define UARTDM_TF	0x0070
 
 #define UART_CR				0x0010
 #define UART_CR_CMD_NULL		(0 << 4)
@@ -64,14 +66,17 @@
 #define UART_CR_CMD_START_BREAK		(5 << 4)
 #define UART_CR_CMD_STOP_BREAK		(6 << 4)
 #define UART_CR_CMD_RESET_CTS		(7 << 4)
+#define UART_CR_CMD_RESET_STALE_INT	(8 << 4)
 #define UART_CR_CMD_PACKET_MODE		(9 << 4)
 #define UART_CR_CMD_MODE_RESET		(12 << 4)
 #define UART_CR_CMD_SET_RFR		(13 << 4)
 #define UART_CR_CMD_RESET_RFR		(14 << 4)
+#define UART_CR_CMD_PROTECTION_EN	(16 << 4)
+#define UART_CR_CMD_STALE_EVENT_ENABLE	(80 << 4)
 #define UART_CR_TX_DISABLE		(1 << 3)
-#define UART_CR_TX_ENABLE		(1 << 3)
-#define UART_CR_RX_DISABLE		(1 << 3)
-#define UART_CR_RX_ENABLE		(1 << 3)
+#define UART_CR_TX_ENABLE		(1 << 2)
+#define UART_CR_RX_DISABLE		(1 << 1)
+#define UART_CR_RX_ENABLE		(1 << 0)
 
 #define UART_IMR		0x0014
 #define UART_IMR_TXLEV		(1 << 0)
@@ -110,9 +115,20 @@
 #define UART_SR_RX_FULL		(1 << 1)
 #define UART_SR_RX_READY	(1 << 0)
 
-#define UART_RF		0x000C
-#define UART_MISR	0x0010
-#define UART_ISR	0x0014
+#define UART_RF			0x000C
+#define UARTDM_RF		0x0070
+#define UART_MISR		0x0010
+#define UART_ISR		0x0014
+#define UART_ISR_TX_READY	(1 << 7)
+
+#define GSBI_CONTROL		0x0
+#define GSBI_PROTOCOL_CODE	0x30
+#define GSBI_PROTOCOL_UART	0x40
+#define GSBI_PROTOCOL_IDLE	0x0
+
+#define UARTDM_DMRX		0x34
+#define UARTDM_NCF_TX		0x40
+#define UARTDM_RX_TOTAL_SNAP	0x38
 
 #define UART_TO_MSM(uart_port)	((struct msm_port *) uart_port)
 
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
new file mode 100644
index 000000000000..7e02c9c344fe
--- /dev/null
+++ b/drivers/tty/serial/mxs-auart.c
@@ -0,0 +1,798 @@
+/*
+ * Freescale STMP37XX/STMP378X Application UART driver
+ *
+ * Author: dmitry pervushin <dimka@embeddedalley.com>
+ *
+ * Copyright 2008-2010 Freescale Semiconductor, Inc.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ *
+ * 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/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include <asm/cacheflush.h>
+
+#define MXS_AUART_PORTS 5
+
+#define AUART_CTRL0			0x00000000
+#define AUART_CTRL0_SET			0x00000004
+#define AUART_CTRL0_CLR			0x00000008
+#define AUART_CTRL0_TOG			0x0000000c
+#define AUART_CTRL1			0x00000010
+#define AUART_CTRL1_SET			0x00000014
+#define AUART_CTRL1_CLR			0x00000018
+#define AUART_CTRL1_TOG			0x0000001c
+#define AUART_CTRL2			0x00000020
+#define AUART_CTRL2_SET			0x00000024
+#define AUART_CTRL2_CLR			0x00000028
+#define AUART_CTRL2_TOG			0x0000002c
+#define AUART_LINECTRL			0x00000030
+#define AUART_LINECTRL_SET		0x00000034
+#define AUART_LINECTRL_CLR		0x00000038
+#define AUART_LINECTRL_TOG		0x0000003c
+#define AUART_LINECTRL2			0x00000040
+#define AUART_LINECTRL2_SET		0x00000044
+#define AUART_LINECTRL2_CLR		0x00000048
+#define AUART_LINECTRL2_TOG		0x0000004c
+#define AUART_INTR			0x00000050
+#define AUART_INTR_SET			0x00000054
+#define AUART_INTR_CLR			0x00000058
+#define AUART_INTR_TOG			0x0000005c
+#define AUART_DATA			0x00000060
+#define AUART_STAT			0x00000070
+#define AUART_DEBUG			0x00000080
+#define AUART_VERSION			0x00000090
+#define AUART_AUTOBAUD			0x000000a0
+
+#define AUART_CTRL0_SFTRST			(1 << 31)
+#define AUART_CTRL0_CLKGATE			(1 << 30)
+
+#define AUART_CTRL2_CTSEN			(1 << 15)
+#define AUART_CTRL2_RTS				(1 << 11)
+#define AUART_CTRL2_RXE				(1 << 9)
+#define AUART_CTRL2_TXE				(1 << 8)
+#define AUART_CTRL2_UARTEN			(1 << 0)
+
+#define AUART_LINECTRL_BAUD_DIVINT_SHIFT	16
+#define AUART_LINECTRL_BAUD_DIVINT_MASK		0xffff0000
+#define AUART_LINECTRL_BAUD_DIVINT(v)		(((v) & 0xffff) << 16)
+#define AUART_LINECTRL_BAUD_DIVFRAC_SHIFT	8
+#define AUART_LINECTRL_BAUD_DIVFRAC_MASK	0x00003f00
+#define AUART_LINECTRL_BAUD_DIVFRAC(v)		(((v) & 0x3f) << 8)
+#define AUART_LINECTRL_WLEN_MASK		0x00000060
+#define AUART_LINECTRL_WLEN(v)			(((v) & 0x3) << 5)
+#define AUART_LINECTRL_FEN			(1 << 4)
+#define AUART_LINECTRL_STP2			(1 << 3)
+#define AUART_LINECTRL_EPS			(1 << 2)
+#define AUART_LINECTRL_PEN			(1 << 1)
+#define AUART_LINECTRL_BRK			(1 << 0)
+
+#define AUART_INTR_RTIEN			(1 << 22)
+#define AUART_INTR_TXIEN			(1 << 21)
+#define AUART_INTR_RXIEN			(1 << 20)
+#define AUART_INTR_CTSMIEN			(1 << 17)
+#define AUART_INTR_RTIS				(1 << 6)
+#define AUART_INTR_TXIS				(1 << 5)
+#define AUART_INTR_RXIS				(1 << 4)
+#define AUART_INTR_CTSMIS			(1 << 1)
+
+#define AUART_STAT_BUSY				(1 << 29)
+#define AUART_STAT_CTS				(1 << 28)
+#define AUART_STAT_TXFE				(1 << 27)
+#define AUART_STAT_TXFF				(1 << 25)
+#define AUART_STAT_RXFE				(1 << 24)
+#define AUART_STAT_OERR				(1 << 19)
+#define AUART_STAT_BERR				(1 << 18)
+#define AUART_STAT_PERR				(1 << 17)
+#define AUART_STAT_FERR				(1 << 16)
+
+static struct uart_driver auart_driver;
+
+struct mxs_auart_port {
+	struct uart_port port;
+
+	unsigned int flags;
+	unsigned int ctrl;
+
+	unsigned int irq;
+
+	struct clk *clk;
+	struct device *dev;
+};
+
+static void mxs_auart_stop_tx(struct uart_port *u);
+
+#define to_auart_port(u) container_of(u, struct mxs_auart_port, port)
+
+static inline void mxs_auart_tx_chars(struct mxs_auart_port *s)
+{
+	struct circ_buf *xmit = &s->port.state->xmit;
+
+	while (!(readl(s->port.membase + AUART_STAT) &
+		 AUART_STAT_TXFF)) {
+		if (s->port.x_char) {
+			s->port.icount.tx++;
+			writel(s->port.x_char,
+				     s->port.membase + AUART_DATA);
+			s->port.x_char = 0;
+			continue;
+		}
+		if (!uart_circ_empty(xmit) && !uart_tx_stopped(&s->port)) {
+			s->port.icount.tx++;
+			writel(xmit->buf[xmit->tail],
+				     s->port.membase + AUART_DATA);
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+			if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+				uart_write_wakeup(&s->port);
+		} else
+			break;
+	}
+	if (uart_circ_empty(&(s->port.state->xmit)))
+		writel(AUART_INTR_TXIEN,
+			     s->port.membase + AUART_INTR_CLR);
+	else
+		writel(AUART_INTR_TXIEN,
+			     s->port.membase + AUART_INTR_SET);
+
+	if (uart_tx_stopped(&s->port))
+		mxs_auart_stop_tx(&s->port);
+}
+
+static void mxs_auart_rx_char(struct mxs_auart_port *s)
+{
+	int flag;
+	u32 stat;
+	u8 c;
+
+	c = readl(s->port.membase + AUART_DATA);
+	stat = readl(s->port.membase + AUART_STAT);
+
+	flag = TTY_NORMAL;
+	s->port.icount.rx++;
+
+	if (stat & AUART_STAT_BERR) {
+		s->port.icount.brk++;
+		if (uart_handle_break(&s->port))
+			goto out;
+	} else if (stat & AUART_STAT_PERR) {
+		s->port.icount.parity++;
+	} else if (stat & AUART_STAT_FERR) {
+		s->port.icount.frame++;
+	}
+
+	/*
+	 * Mask off conditions which should be ingored.
+	 */
+	stat &= s->port.read_status_mask;
+
+	if (stat & AUART_STAT_BERR) {
+		flag = TTY_BREAK;
+	} else if (stat & AUART_STAT_PERR)
+		flag = TTY_PARITY;
+	else if (stat & AUART_STAT_FERR)
+		flag = TTY_FRAME;
+
+	if (stat & AUART_STAT_OERR)
+		s->port.icount.overrun++;
+
+	if (uart_handle_sysrq_char(&s->port, c))
+		goto out;
+
+	uart_insert_char(&s->port, stat, AUART_STAT_OERR, c, flag);
+out:
+	writel(stat, s->port.membase + AUART_STAT);
+}
+
+static void mxs_auart_rx_chars(struct mxs_auart_port *s)
+{
+	struct tty_struct *tty = s->port.state->port.tty;
+	u32 stat = 0;
+
+	for (;;) {
+		stat = readl(s->port.membase + AUART_STAT);
+		if (stat & AUART_STAT_RXFE)
+			break;
+		mxs_auart_rx_char(s);
+	}
+
+	writel(stat, s->port.membase + AUART_STAT);
+	tty_flip_buffer_push(tty);
+}
+
+static int mxs_auart_request_port(struct uart_port *u)
+{
+	return 0;
+}
+
+static int mxs_auart_verify_port(struct uart_port *u,
+				    struct serial_struct *ser)
+{
+	if (u->type != PORT_UNKNOWN && u->type != PORT_IMX)
+		return -EINVAL;
+	return 0;
+}
+
+static void mxs_auart_config_port(struct uart_port *u, int flags)
+{
+}
+
+static const char *mxs_auart_type(struct uart_port *u)
+{
+	struct mxs_auart_port *s = to_auart_port(u);
+
+	return dev_name(s->dev);
+}
+
+static void mxs_auart_release_port(struct uart_port *u)
+{
+}
+
+static void mxs_auart_set_mctrl(struct uart_port *u, unsigned mctrl)
+{
+	struct mxs_auart_port *s = to_auart_port(u);
+
+	u32 ctrl = readl(u->membase + AUART_CTRL2);
+
+	ctrl &= ~AUART_CTRL2_RTS;
+	if (mctrl & TIOCM_RTS)
+		ctrl |= AUART_CTRL2_RTS;
+	s->ctrl = mctrl;
+	writel(ctrl, u->membase + AUART_CTRL2);
+}
+
+static u32 mxs_auart_get_mctrl(struct uart_port *u)
+{
+	struct mxs_auart_port *s = to_auart_port(u);
+	u32 stat = readl(u->membase + AUART_STAT);
+	int ctrl2 = readl(u->membase + AUART_CTRL2);
+	u32 mctrl = s->ctrl;
+
+	mctrl &= ~TIOCM_CTS;
+	if (stat & AUART_STAT_CTS)
+		mctrl |= TIOCM_CTS;
+
+	if (ctrl2 & AUART_CTRL2_RTS)
+		mctrl |= TIOCM_RTS;
+
+	return mctrl;
+}
+
+static void mxs_auart_settermios(struct uart_port *u,
+				 struct ktermios *termios,
+				 struct ktermios *old)
+{
+	u32 bm, ctrl, ctrl2, div;
+	unsigned int cflag, baud;
+
+	cflag = termios->c_cflag;
+
+	ctrl = AUART_LINECTRL_FEN;
+	ctrl2 = readl(u->membase + AUART_CTRL2);
+
+	/* byte size */
+	switch (cflag & CSIZE) {
+	case CS5:
+		bm = 0;
+		break;
+	case CS6:
+		bm = 1;
+		break;
+	case CS7:
+		bm = 2;
+		break;
+	case CS8:
+		bm = 3;
+		break;
+	default:
+		return;
+	}
+
+	ctrl |= AUART_LINECTRL_WLEN(bm);
+
+	/* parity */
+	if (cflag & PARENB) {
+		ctrl |= AUART_LINECTRL_PEN;
+		if ((cflag & PARODD) == 0)
+			ctrl |= AUART_LINECTRL_EPS;
+	}
+
+	u->read_status_mask = 0;
+
+	if (termios->c_iflag & INPCK)
+		u->read_status_mask |= AUART_STAT_PERR;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		u->read_status_mask |= AUART_STAT_BERR;
+
+	/*
+	 * Characters to ignore
+	 */
+	u->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR)
+		u->ignore_status_mask |= AUART_STAT_PERR;
+	if (termios->c_iflag & IGNBRK) {
+		u->ignore_status_mask |= AUART_STAT_BERR;
+		/*
+		 * If we're ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support).
+		 */
+		if (termios->c_iflag & IGNPAR)
+			u->ignore_status_mask |= AUART_STAT_OERR;
+	}
+
+	/*
+	 * ignore all characters if CREAD is not set
+	 */
+	if (cflag & CREAD)
+		ctrl2 |= AUART_CTRL2_RXE;
+	else
+		ctrl2 &= ~AUART_CTRL2_RXE;
+
+	/* figure out the stop bits requested */
+	if (cflag & CSTOPB)
+		ctrl |= AUART_LINECTRL_STP2;
+
+	/* figure out the hardware flow control settings */
+	if (cflag & CRTSCTS)
+		ctrl2 |= AUART_CTRL2_CTSEN;
+	else
+		ctrl2 &= ~AUART_CTRL2_CTSEN;
+
+	/* set baud rate */
+	baud = uart_get_baud_rate(u, termios, old, 0, u->uartclk);
+	div = u->uartclk * 32 / baud;
+	ctrl |= AUART_LINECTRL_BAUD_DIVFRAC(div & 0x3F);
+	ctrl |= AUART_LINECTRL_BAUD_DIVINT(div >> 6);
+
+	writel(ctrl, u->membase + AUART_LINECTRL);
+	writel(ctrl2, u->membase + AUART_CTRL2);
+}
+
+static irqreturn_t mxs_auart_irq_handle(int irq, void *context)
+{
+	u32 istatus, istat;
+	struct mxs_auart_port *s = context;
+	u32 stat = readl(s->port.membase + AUART_STAT);
+
+	istatus = istat = readl(s->port.membase + AUART_INTR);
+
+	if (istat & AUART_INTR_CTSMIS) {
+		uart_handle_cts_change(&s->port, stat & AUART_STAT_CTS);
+		writel(AUART_INTR_CTSMIS,
+				s->port.membase + AUART_INTR_CLR);
+		istat &= ~AUART_INTR_CTSMIS;
+	}
+
+	if (istat & (AUART_INTR_RTIS | AUART_INTR_RXIS)) {
+		mxs_auart_rx_chars(s);
+		istat &= ~(AUART_INTR_RTIS | AUART_INTR_RXIS);
+	}
+
+	if (istat & AUART_INTR_TXIS) {
+		mxs_auart_tx_chars(s);
+		istat &= ~AUART_INTR_TXIS;
+	}
+
+	writel(istatus & (AUART_INTR_RTIS
+		| AUART_INTR_TXIS
+		| AUART_INTR_RXIS
+		| AUART_INTR_CTSMIS),
+			s->port.membase + AUART_INTR_CLR);
+
+	return IRQ_HANDLED;
+}
+
+static void mxs_auart_reset(struct uart_port *u)
+{
+	int i;
+	unsigned int reg;
+
+	writel(AUART_CTRL0_SFTRST, u->membase + AUART_CTRL0_CLR);
+
+	for (i = 0; i < 10000; i++) {
+		reg = readl(u->membase + AUART_CTRL0);
+		if (!(reg & AUART_CTRL0_SFTRST))
+			break;
+		udelay(3);
+	}
+	writel(AUART_CTRL0_CLKGATE, u->membase + AUART_CTRL0_CLR);
+}
+
+static int mxs_auart_startup(struct uart_port *u)
+{
+	struct mxs_auart_port *s = to_auart_port(u);
+
+	clk_enable(s->clk);
+
+	writel(AUART_CTRL0_CLKGATE, u->membase + AUART_CTRL0_CLR);
+
+	writel(AUART_CTRL2_UARTEN, u->membase + AUART_CTRL2_SET);
+
+	writel(AUART_INTR_RXIEN | AUART_INTR_RTIEN | AUART_INTR_CTSMIEN,
+			u->membase + AUART_INTR);
+
+	/*
+	 * Enable fifo so all four bytes of a DMA word are written to
+	 * output (otherwise, only the LSB is written, ie. 1 in 4 bytes)
+	 */
+	writel(AUART_LINECTRL_FEN, u->membase + AUART_LINECTRL_SET);
+
+	return 0;
+}
+
+static void mxs_auart_shutdown(struct uart_port *u)
+{
+	struct mxs_auart_port *s = to_auart_port(u);
+
+	writel(AUART_CTRL2_UARTEN, u->membase + AUART_CTRL2_CLR);
+
+	writel(AUART_CTRL0_CLKGATE, u->membase + AUART_CTRL0_SET);
+
+	writel(AUART_INTR_RXIEN | AUART_INTR_RTIEN | AUART_INTR_CTSMIEN,
+			u->membase + AUART_INTR_CLR);
+
+	clk_disable(s->clk);
+}
+
+static unsigned int mxs_auart_tx_empty(struct uart_port *u)
+{
+	if (readl(u->membase + AUART_STAT) & AUART_STAT_TXFE)
+		return TIOCSER_TEMT;
+	else
+		return 0;
+}
+
+static void mxs_auart_start_tx(struct uart_port *u)
+{
+	struct mxs_auart_port *s = to_auart_port(u);
+
+	/* enable transmitter */
+	writel(AUART_CTRL2_TXE, u->membase + AUART_CTRL2_SET);
+
+	mxs_auart_tx_chars(s);
+}
+
+static void mxs_auart_stop_tx(struct uart_port *u)
+{
+	writel(AUART_CTRL2_TXE, u->membase + AUART_CTRL2_CLR);
+}
+
+static void mxs_auart_stop_rx(struct uart_port *u)
+{
+	writel(AUART_CTRL2_RXE, u->membase + AUART_CTRL2_CLR);
+}
+
+static void mxs_auart_break_ctl(struct uart_port *u, int ctl)
+{
+	if (ctl)
+		writel(AUART_LINECTRL_BRK,
+			     u->membase + AUART_LINECTRL_SET);
+	else
+		writel(AUART_LINECTRL_BRK,
+			     u->membase + AUART_LINECTRL_CLR);
+}
+
+static void mxs_auart_enable_ms(struct uart_port *port)
+{
+	/* just empty */
+}
+
+static struct uart_ops mxs_auart_ops = {
+	.tx_empty       = mxs_auart_tx_empty,
+	.start_tx       = mxs_auart_start_tx,
+	.stop_tx	= mxs_auart_stop_tx,
+	.stop_rx	= mxs_auart_stop_rx,
+	.enable_ms      = mxs_auart_enable_ms,
+	.break_ctl      = mxs_auart_break_ctl,
+	.set_mctrl	= mxs_auart_set_mctrl,
+	.get_mctrl      = mxs_auart_get_mctrl,
+	.startup	= mxs_auart_startup,
+	.shutdown       = mxs_auart_shutdown,
+	.set_termios    = mxs_auart_settermios,
+	.type	   	= mxs_auart_type,
+	.release_port   = mxs_auart_release_port,
+	.request_port   = mxs_auart_request_port,
+	.config_port    = mxs_auart_config_port,
+	.verify_port    = mxs_auart_verify_port,
+};
+
+static struct mxs_auart_port *auart_port[MXS_AUART_PORTS];
+
+#ifdef CONFIG_SERIAL_MXS_AUART_CONSOLE
+static void mxs_auart_console_putchar(struct uart_port *port, int ch)
+{
+	unsigned int to = 1000;
+
+	while (readl(port->membase + AUART_STAT) & AUART_STAT_TXFF) {
+		if (!to--)
+			break;
+		udelay(1);
+	}
+
+	writel(ch, port->membase + AUART_DATA);
+}
+
+static void
+auart_console_write(struct console *co, const char *str, unsigned int count)
+{
+	struct mxs_auart_port *s;
+	struct uart_port *port;
+	unsigned int old_ctrl0, old_ctrl2;
+	unsigned int to = 1000;
+
+	if (co->index >	MXS_AUART_PORTS || co->index < 0)
+		return;
+
+	s = auart_port[co->index];
+	port = &s->port;
+
+	clk_enable(s->clk);
+
+	/* First save the CR then disable the interrupts */
+	old_ctrl2 = readl(port->membase + AUART_CTRL2);
+	old_ctrl0 = readl(port->membase + AUART_CTRL0);
+
+	writel(AUART_CTRL0_CLKGATE,
+		     port->membase + AUART_CTRL0_CLR);
+	writel(AUART_CTRL2_UARTEN | AUART_CTRL2_TXE,
+		     port->membase + AUART_CTRL2_SET);
+
+	uart_console_write(port, str, count, mxs_auart_console_putchar);
+
+	/*
+	 * Finally, wait for transmitter to become empty
+	 * and restore the TCR
+	 */
+	while (readl(port->membase + AUART_STAT) & AUART_STAT_BUSY) {
+		if (!to--)
+			break;
+		udelay(1);
+	}
+
+	writel(old_ctrl0, port->membase + AUART_CTRL0);
+	writel(old_ctrl2, port->membase + AUART_CTRL2);
+
+	clk_disable(s->clk);
+}
+
+static void __init
+auart_console_get_options(struct uart_port *port, int *baud,
+			  int *parity, int *bits)
+{
+	unsigned int lcr_h, quot;
+
+	if (!(readl(port->membase + AUART_CTRL2) & AUART_CTRL2_UARTEN))
+		return;
+
+	lcr_h = readl(port->membase + AUART_LINECTRL);
+
+	*parity = 'n';
+	if (lcr_h & AUART_LINECTRL_PEN) {
+		if (lcr_h & AUART_LINECTRL_EPS)
+			*parity = 'e';
+		else
+			*parity = 'o';
+	}
+
+	if ((lcr_h & AUART_LINECTRL_WLEN_MASK) == AUART_LINECTRL_WLEN(2))
+		*bits = 7;
+	else
+		*bits = 8;
+
+	quot = ((readl(port->membase + AUART_LINECTRL)
+			& AUART_LINECTRL_BAUD_DIVINT_MASK))
+			    >> (AUART_LINECTRL_BAUD_DIVINT_SHIFT - 6);
+	quot |= ((readl(port->membase + AUART_LINECTRL)
+			& AUART_LINECTRL_BAUD_DIVFRAC_MASK))
+				>> AUART_LINECTRL_BAUD_DIVFRAC_SHIFT;
+	if (quot == 0)
+		quot = 1;
+
+	*baud = (port->uartclk << 2) / quot;
+}
+
+static int __init
+auart_console_setup(struct console *co, char *options)
+{
+	struct mxs_auart_port *s;
+	int baud = 9600;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+	int ret;
+
+	/*
+	 * Check whether an invalid uart number has been specified, and
+	 * if so, search for the first available port that does have
+	 * console support.
+	 */
+	if (co->index == -1 || co->index >= ARRAY_SIZE(auart_port))
+		co->index = 0;
+	s = auart_port[co->index];
+	if (!s)
+		return -ENODEV;
+
+	clk_enable(s->clk);
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	else
+		auart_console_get_options(&s->port, &baud, &parity, &bits);
+
+	ret = uart_set_options(&s->port, co, baud, parity, bits, flow);
+
+	clk_disable(s->clk);
+
+	return ret;
+}
+
+static struct console auart_console = {
+	.name		= "ttyAPP",
+	.write		= auart_console_write,
+	.device		= uart_console_device,
+	.setup		= auart_console_setup,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+	.data		= &auart_driver,
+};
+#endif
+
+static struct uart_driver auart_driver = {
+	.owner		= THIS_MODULE,
+	.driver_name	= "ttyAPP",
+	.dev_name	= "ttyAPP",
+	.major		= 0,
+	.minor		= 0,
+	.nr		= MXS_AUART_PORTS,
+#ifdef CONFIG_SERIAL_MXS_AUART_CONSOLE
+	.cons =		&auart_console,
+#endif
+};
+
+static int __devinit mxs_auart_probe(struct platform_device *pdev)
+{
+	struct mxs_auart_port *s;
+	u32 version;
+	int ret = 0;
+	struct resource *r;
+
+	s = kzalloc(sizeof(struct mxs_auart_port), GFP_KERNEL);
+	if (!s) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	s->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(s->clk)) {
+		ret = PTR_ERR(s->clk);
+		goto out_free;
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		ret = -ENXIO;
+		goto out_free_clk;
+	}
+
+	s->port.mapbase = r->start;
+	s->port.membase = ioremap(r->start, resource_size(r));
+	s->port.ops = &mxs_auart_ops;
+	s->port.iotype = UPIO_MEM;
+	s->port.line = pdev->id < 0 ? 0 : pdev->id;
+	s->port.fifosize = 16;
+	s->port.uartclk = clk_get_rate(s->clk);
+	s->port.type = PORT_IMX;
+	s->port.dev = s->dev = get_device(&pdev->dev);
+
+	s->flags = 0;
+	s->ctrl = 0;
+
+	s->irq = platform_get_irq(pdev, 0);
+	s->port.irq = s->irq;
+	ret = request_irq(s->irq, mxs_auart_irq_handle, 0, dev_name(&pdev->dev), s);
+	if (ret)
+		goto out_free_clk;
+
+	platform_set_drvdata(pdev, s);
+
+	auart_port[pdev->id] = s;
+
+	mxs_auart_reset(&s->port);
+
+	ret = uart_add_one_port(&auart_driver, &s->port);
+	if (ret)
+		goto out_free_irq;
+
+	version = readl(s->port.membase + AUART_VERSION);
+	dev_info(&pdev->dev, "Found APPUART %d.%d.%d\n",
+	       (version >> 24) & 0xff,
+	       (version >> 16) & 0xff, version & 0xffff);
+
+	return 0;
+
+out_free_irq:
+	auart_port[pdev->id] = NULL;
+	free_irq(s->irq, s);
+out_free_clk:
+	clk_put(s->clk);
+out_free:
+	kfree(s);
+out:
+	return ret;
+}
+
+static int __devexit mxs_auart_remove(struct platform_device *pdev)
+{
+	struct mxs_auart_port *s = platform_get_drvdata(pdev);
+
+	uart_remove_one_port(&auart_driver, &s->port);
+
+	auart_port[pdev->id] = NULL;
+
+	clk_put(s->clk);
+	free_irq(s->irq, s);
+	kfree(s);
+
+	return 0;
+}
+
+static struct platform_driver mxs_auart_driver = {
+	.probe = mxs_auart_probe,
+	.remove = __devexit_p(mxs_auart_remove),
+	.driver = {
+		.name = "mxs-auart",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init mxs_auart_init(void)
+{
+	int r;
+
+	r = uart_register_driver(&auart_driver);
+	if (r)
+		goto out;
+
+	r = platform_driver_register(&mxs_auart_driver);
+	if (r)
+		goto out_err;
+
+	return 0;
+out_err:
+	uart_unregister_driver(&auart_driver);
+out:
+	return r;
+}
+
+static void __exit mxs_auart_exit(void)
+{
+	platform_driver_unregister(&mxs_auart_driver);
+	uart_unregister_driver(&auart_driver);
+}
+
+module_init(mxs_auart_init);
+module_exit(mxs_auart_exit);
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Freescale MXS application uart driver");
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 6a18ca6ddaa9..0e8eec516df4 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -80,14 +80,16 @@ static int __devinit of_platform_serial_setup(struct platform_device *ofdev,
 /*
  * Try to register a serial port
  */
-static int __devinit of_platform_serial_probe(struct platform_device *ofdev,
-						const struct of_device_id *id)
+static int __devinit of_platform_serial_probe(struct platform_device *ofdev)
 {
 	struct of_serial_info *info;
 	struct uart_port port;
 	int port_type;
 	int ret;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+
 	if (of_find_property(ofdev->dev.of_node, "used-by-rtas", NULL))
 		return -EBUSY;
 
@@ -95,7 +97,7 @@ static int __devinit of_platform_serial_probe(struct platform_device *ofdev,
 	if (info == NULL)
 		return -ENOMEM;
 
-	port_type = (unsigned long)id->data;
+	port_type = (unsigned long)ofdev->dev.of_match->data;
 	ret = of_platform_serial_setup(ofdev, port_type, &port);
 	if (ret)
 		goto out;
@@ -174,7 +176,7 @@ static struct of_device_id __devinitdata of_platform_serial_table[] = {
 	{ /* end of list */ },
 };
 
-static struct of_platform_driver of_platform_serial_driver = {
+static struct platform_driver of_platform_serial_driver = {
 	.driver = {
 		.name = "of_serial",
 		.owner = THIS_MODULE,
@@ -186,13 +188,13 @@ static struct of_platform_driver of_platform_serial_driver = {
 
 static int __init of_platform_serial_init(void)
 {
-	return of_register_platform_driver(&of_platform_serial_driver);
+	return platform_driver_register(&of_platform_serial_driver);
 }
 module_init(of_platform_serial_init);
 
 static void __exit of_platform_serial_exit(void)
 {
-	return of_unregister_platform_driver(&of_platform_serial_driver);
+	return platform_driver_unregister(&of_platform_serial_driver);
 };
 module_exit(of_platform_serial_exit);
 
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index a5ce9a5c018d..7aba41f1f23c 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -15,7 +15,6 @@
  *Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
  */
 #include <linux/serial_reg.h>
-#include <linux/pci.h>
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/serial_core.h>
diff --git a/drivers/tty/serial/serial_cs.c b/drivers/tty/serial/serial_cs.c
index 93760b2ea172..1ef4df9bf7e4 100644
--- a/drivers/tty/serial/serial_cs.c
+++ b/drivers/tty/serial/serial_cs.c
@@ -712,6 +712,7 @@ static struct pcmcia_device_id serial_ids[] = {
 	PCMCIA_PFC_DEVICE_PROD_ID12(1, "Xircom", "CreditCard Ethernet+Modem II", 0x2e3ee845, 0xeca401bf),
 	PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0032, 0x0e01),
 	PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0032, 0x0a05),
+	PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0032, 0x0b05),
 	PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0032, 0x1101),
 	PCMCIA_MFC_DEVICE_MANF_CARD(0, 0x0104, 0x0070),
 	PCMCIA_MFC_DEVICE_MANF_CARD(1, 0x0101, 0x0562),
diff --git a/drivers/tty/serial/serial_lh7a40x.c b/drivers/tty/serial/serial_lh7a40x.c
deleted file mode 100644
index ea744707c4d6..000000000000
--- a/drivers/tty/serial/serial_lh7a40x.c
+++ /dev/null
@@ -1,682 +0,0 @@
-/* drivers/serial/serial_lh7a40x.c
- *
- *  Copyright (C) 2004 Coastal Environmental Systems
- *
- *  This program is free software; you can redistribute it and/or
- *  modify it under the terms of the GNU General Public License
- *  version 2 as published by the Free Software Foundation.
- *
- */
-
-/* Driver for Sharp LH7A40X embedded serial ports
- *
- *  Based on drivers/char/serial.c, by Linus Torvalds, Theodore Ts'o.
- *  Based on drivers/serial/amba.c, by Deep Blue Solutions Ltd.
- *
- *  ---
- *
- * This driver supports the embedded UARTs of the Sharp LH7A40X series
- * CPUs.  While similar to the 16550 and other UART chips, there is
- * nothing close to register compatibility.  Moreover, some of the
- * modem control lines are not available, either in the chip or they
- * are lacking in the board-level implementation.
- *
- * - Use of SIRDIS
- *   For simplicity, we disable the IR functions of any UART whenever
- *   we enable it.
- *
- */
-
-
-#if defined(CONFIG_SERIAL_LH7A40X_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
-#define SUPPORT_SYSRQ
-#endif
-
-#include <linux/module.h>
-#include <linux/ioport.h>
-#include <linux/init.h>
-#include <linux/console.h>
-#include <linux/sysrq.h>
-#include <linux/tty.h>
-#include <linux/tty_flip.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-#include <linux/io.h>
-
-#include <asm/irq.h>
-#include <mach/hardware.h>
-
-#define DEV_MAJOR	204
-#define DEV_MINOR	16
-#define DEV_NR		3
-
-#define ISR_LOOP_LIMIT	256
-
-#define UR(p,o)	_UR ((p)->membase, o)
-#define _UR(b,o) (*((volatile unsigned int*)(((unsigned char*) b) + (o))))
-#define BIT_CLR(p,o,m)	UR(p,o) = UR(p,o) & (~(unsigned int)m)
-#define BIT_SET(p,o,m)	UR(p,o) = UR(p,o) | ( (unsigned int)m)
-
-#define UART_REG_SIZE	32
-
-#define UART_R_DATA	(0x00)
-#define UART_R_FCON	(0x04)
-#define UART_R_BRCON	(0x08)
-#define UART_R_CON	(0x0c)
-#define UART_R_STATUS	(0x10)
-#define UART_R_RAWISR	(0x14)
-#define UART_R_INTEN	(0x18)
-#define UART_R_ISR	(0x1c)
-
-#define UARTEN		(0x01)		/* UART enable */
-#define SIRDIS		(0x02)		/* Serial IR disable (UART1 only) */
-
-#define RxEmpty		(0x10)
-#define TxEmpty		(0x80)
-#define TxFull		(0x20)
-#define nRxRdy		RxEmpty
-#define nTxRdy		TxFull
-#define TxBusy		(0x08)
-
-#define RxBreak		(0x0800)
-#define RxOverrunError	(0x0400)
-#define RxParityError	(0x0200)
-#define RxFramingError	(0x0100)
-#define RxError     (RxBreak | RxOverrunError | RxParityError | RxFramingError)
-
-#define DCD		(0x04)
-#define DSR		(0x02)
-#define CTS		(0x01)
-
-#define RxInt		(0x01)
-#define TxInt		(0x02)
-#define ModemInt	(0x04)
-#define RxTimeoutInt	(0x08)
-
-#define MSEOI		(0x10)
-
-#define WLEN_8		(0x60)
-#define WLEN_7		(0x40)
-#define WLEN_6		(0x20)
-#define WLEN_5		(0x00)
-#define WLEN		(0x60)	/* Mask for all word-length bits */
-#define STP2		(0x08)
-#define PEN		(0x02)	/* Parity Enable */
-#define EPS		(0x04)	/* Even Parity Set */
-#define FEN		(0x10)	/* FIFO Enable */
-#define BRK		(0x01)	/* Send Break */
-
-
-struct uart_port_lh7a40x {
-	struct uart_port port;
-	unsigned int statusPrev; /* Most recently read modem status */
-};
-
-static void lh7a40xuart_stop_tx (struct uart_port* port)
-{
-	BIT_CLR (port, UART_R_INTEN, TxInt);
-}
-
-static void lh7a40xuart_start_tx (struct uart_port* port)
-{
-	BIT_SET (port, UART_R_INTEN, TxInt);
-
-	/* *** FIXME: do I need to check for startup of the
-		      transmitter?  The old driver did, but AMBA
-		      doesn't . */
-}
-
-static void lh7a40xuart_stop_rx (struct uart_port* port)
-{
-	BIT_SET (port, UART_R_INTEN, RxTimeoutInt | RxInt);
-}
-
-static void lh7a40xuart_enable_ms (struct uart_port* port)
-{
-	BIT_SET (port, UART_R_INTEN, ModemInt);
-}
-
-static void lh7a40xuart_rx_chars (struct uart_port* port)
-{
-	struct tty_struct* tty = port->state->port.tty;
-	int cbRxMax = 256;	/* (Gross) limit on receive */
-	unsigned int data;	/* Received data and status */
-	unsigned int flag;
-
-	while (!(UR (port, UART_R_STATUS) & nRxRdy) && --cbRxMax) {
-		data = UR (port, UART_R_DATA);
-		flag = TTY_NORMAL;
-		++port->icount.rx;
-
-		if (unlikely(data & RxError)) {
-			if (data & RxBreak) {
-				data &= ~(RxFramingError | RxParityError);
-				++port->icount.brk;
-				if (uart_handle_break (port))
-					continue;
-			}
-			else if (data & RxParityError)
-				++port->icount.parity;
-			else if (data & RxFramingError)
-				++port->icount.frame;
-			if (data & RxOverrunError)
-				++port->icount.overrun;
-
-				/* Mask by termios, leave Rx'd byte */
-			data &= port->read_status_mask | 0xff;
-
-			if (data & RxBreak)
-				flag = TTY_BREAK;
-			else if (data & RxParityError)
-				flag = TTY_PARITY;
-			else if (data & RxFramingError)
-				flag = TTY_FRAME;
-		}
-
-		if (uart_handle_sysrq_char (port, (unsigned char) data))
-			continue;
-
-		uart_insert_char(port, data, RxOverrunError, data, flag);
-	}
-	tty_flip_buffer_push (tty);
-	return;
-}
-
-static void lh7a40xuart_tx_chars (struct uart_port* port)
-{
-	struct circ_buf* xmit = &port->state->xmit;
-	int cbTxMax = port->fifosize;
-
-	if (port->x_char) {
-		UR (port, UART_R_DATA) = port->x_char;
-		++port->icount.tx;
-		port->x_char = 0;
-		return;
-	}
-	if (uart_circ_empty (xmit) || uart_tx_stopped (port)) {
-		lh7a40xuart_stop_tx (port);
-		return;
-	}
-
-	/* Unlike the AMBA UART, the lh7a40x UART does not guarantee
-	   that at least half of the FIFO is empty.  Instead, we check
-	   status for every character.  Using the AMBA method causes
-	   the transmitter to drop characters. */
-
-	do {
-		UR (port, UART_R_DATA) = xmit->buf[xmit->tail];
-		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
-		++port->icount.tx;
-		if (uart_circ_empty(xmit))
-			break;
-	} while (!(UR (port, UART_R_STATUS) & nTxRdy)
-		 && cbTxMax--);
-
-	if (uart_circ_chars_pending (xmit) < WAKEUP_CHARS)
-		uart_write_wakeup (port);
-
-	if (uart_circ_empty (xmit))
-		lh7a40xuart_stop_tx (port);
-}
-
-static void lh7a40xuart_modem_status (struct uart_port* port)
-{
-	unsigned int status = UR (port, UART_R_STATUS);
-	unsigned int delta
-		= status ^ ((struct uart_port_lh7a40x*) port)->statusPrev;
-
-	BIT_SET (port, UART_R_RAWISR, MSEOI); /* Clear modem status intr */
-
-	if (!delta)		/* Only happens if we missed 2 transitions */
-		return;
-
-	((struct uart_port_lh7a40x*) port)->statusPrev = status;
-
-	if (delta & DCD)
-		uart_handle_dcd_change (port, status & DCD);
-
-	if (delta & DSR)
-		++port->icount.dsr;
-
-	if (delta & CTS)
-		uart_handle_cts_change (port, status & CTS);
-
-	wake_up_interruptible (&port->state->port.delta_msr_wait);
-}
-
-static irqreturn_t lh7a40xuart_int (int irq, void* dev_id)
-{
-	struct uart_port* port = dev_id;
-	unsigned int cLoopLimit = ISR_LOOP_LIMIT;
-	unsigned int isr = UR (port, UART_R_ISR);
-
-
-	do {
-		if (isr & (RxInt | RxTimeoutInt))
-			lh7a40xuart_rx_chars(port);
-		if (isr & ModemInt)
-			lh7a40xuart_modem_status (port);
-		if (isr & TxInt)
-			lh7a40xuart_tx_chars (port);
-
-		if (--cLoopLimit == 0)
-			break;
-
-		isr = UR (port, UART_R_ISR);
-	} while (isr & (RxInt | TxInt | RxTimeoutInt));
-
-	return IRQ_HANDLED;
-}
-
-static unsigned int lh7a40xuart_tx_empty (struct uart_port* port)
-{
-	return (UR (port, UART_R_STATUS) & TxEmpty) ? TIOCSER_TEMT : 0;
-}
-
-static unsigned int lh7a40xuart_get_mctrl (struct uart_port* port)
-{
-	unsigned int result = 0;
-	unsigned int status = UR (port, UART_R_STATUS);
-
-	if (status & DCD)
-		result |= TIOCM_CAR;
-	if (status & DSR)
-		result |= TIOCM_DSR;
-	if (status & CTS)
-		result |= TIOCM_CTS;
-
-	return result;
-}
-
-static void lh7a40xuart_set_mctrl (struct uart_port* port, unsigned int mctrl)
-{
-	/* None of the ports supports DTR. UART1 supports RTS through GPIO. */
-	/* Note, kernel appears to be setting DTR and RTS on console. */
-
-	/* *** FIXME: this deserves more work.  There's some work in
-	       tracing all of the IO pins. */
-#if 0
-	if( port->mapbase == UART1_PHYS) {
-		gpioRegs_t *gpio = (gpioRegs_t *)IO_ADDRESS(GPIO_PHYS);
-
-		if (mctrl & TIOCM_RTS)
-			gpio->pbdr &= ~GPIOB_UART1_RTS;
-		else
-			gpio->pbdr |= GPIOB_UART1_RTS;
-	}
-#endif
-}
-
-static void lh7a40xuart_break_ctl (struct uart_port* port, int break_state)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&port->lock, flags);
-	if (break_state == -1)
-		BIT_SET (port, UART_R_FCON, BRK); /* Assert break */
-	else
-		BIT_CLR (port, UART_R_FCON, BRK); /* Deassert break */
-	spin_unlock_irqrestore(&port->lock, flags);
-}
-
-static int lh7a40xuart_startup (struct uart_port* port)
-{
-	int retval;
-
-	retval = request_irq (port->irq, lh7a40xuart_int, 0,
-			      "serial_lh7a40x", port);
-	if (retval)
-		return retval;
-
-				/* Initial modem control-line settings */
-	((struct uart_port_lh7a40x*) port)->statusPrev
-		= UR (port, UART_R_STATUS);
-
-	/* There is presently no configuration option to enable IR.
-	   Thus, we always disable it. */
-
-	BIT_SET (port, UART_R_CON, UARTEN | SIRDIS);
-	BIT_SET (port, UART_R_INTEN, RxTimeoutInt | RxInt);
-
-	return 0;
-}
-
-static void lh7a40xuart_shutdown (struct uart_port* port)
-{
-	free_irq (port->irq, port);
-	BIT_CLR (port, UART_R_FCON, BRK | FEN);
-	BIT_CLR (port, UART_R_CON, UARTEN);
-}
-
-static void lh7a40xuart_set_termios (struct uart_port* port,
-				     struct ktermios* termios,
-				     struct ktermios* old)
-{
-	unsigned int con;
-	unsigned int inten;
-	unsigned int fcon;
-	unsigned long flags;
-	unsigned int baud;
-	unsigned int quot;
-
-	baud = uart_get_baud_rate (port, termios, old, 8, port->uartclk/16);
-	quot = uart_get_divisor (port, baud); /* -1 performed elsewhere */
-
-	switch (termios->c_cflag & CSIZE) {
-	case CS5:
-		fcon = WLEN_5;
-		break;
-	case CS6:
-		fcon = WLEN_6;
-		break;
-	case CS7:
-		fcon = WLEN_7;
-		break;
-	case CS8:
-	default:
-		fcon = WLEN_8;
-		break;
-	}
-	if (termios->c_cflag & CSTOPB)
-		fcon |= STP2;
-	if (termios->c_cflag & PARENB) {
-		fcon |= PEN;
-		if (!(termios->c_cflag & PARODD))
-			fcon |= EPS;
-	}
-	if (port->fifosize > 1)
-		fcon |= FEN;
-
-	spin_lock_irqsave (&port->lock, flags);
-
-	uart_update_timeout (port, termios->c_cflag, baud);
-
-	port->read_status_mask = RxOverrunError;
-	if (termios->c_iflag & INPCK)
-		port->read_status_mask |= RxFramingError | RxParityError;
-	if (termios->c_iflag & (BRKINT | PARMRK))
-		port->read_status_mask |= RxBreak;
-
-		/* Figure mask for status we ignore */
-	port->ignore_status_mask = 0;
-	if (termios->c_iflag & IGNPAR)
-		port->ignore_status_mask |= RxFramingError | RxParityError;
-	if (termios->c_iflag & IGNBRK) {
-		port->ignore_status_mask |= RxBreak;
-		/* Ignore overrun when ignorning parity */
-		/* *** FIXME: is this in the right place? */
-		if (termios->c_iflag & IGNPAR)
-			port->ignore_status_mask |= RxOverrunError;
-	}
-
-		/* Ignore all receive errors when receive disabled */
-	if ((termios->c_cflag & CREAD) == 0)
-		port->ignore_status_mask |= RxError;
-
-	con   = UR (port, UART_R_CON);
-	inten = (UR (port, UART_R_INTEN) & ~ModemInt);
-
-	if (UART_ENABLE_MS (port, termios->c_cflag))
-		inten |= ModemInt;
-
-	BIT_CLR (port, UART_R_CON, UARTEN);	/* Disable UART */
-	UR (port, UART_R_INTEN) = 0;		/* Disable interrupts */
-	UR (port, UART_R_BRCON) = quot - 1;	/* Set baud rate divisor */
-	UR (port, UART_R_FCON)  = fcon;		/* Set FIFO and frame ctrl */
-	UR (port, UART_R_INTEN) = inten;	/* Enable interrupts */
-	UR (port, UART_R_CON)   = con;		/* Restore UART mode */
-
-	spin_unlock_irqrestore(&port->lock, flags);
-}
-
-static const char* lh7a40xuart_type (struct uart_port* port)
-{
-	return port->type == PORT_LH7A40X ? "LH7A40X" : NULL;
-}
-
-static void lh7a40xuart_release_port (struct uart_port* port)
-{
-	release_mem_region (port->mapbase, UART_REG_SIZE);
-}
-
-static int lh7a40xuart_request_port (struct uart_port* port)
-{
-	return request_mem_region (port->mapbase, UART_REG_SIZE,
-				   "serial_lh7a40x") != NULL
-		? 0 : -EBUSY;
-}
-
-static void lh7a40xuart_config_port (struct uart_port* port, int flags)
-{
-	if (flags & UART_CONFIG_TYPE) {
-		port->type = PORT_LH7A40X;
-		lh7a40xuart_request_port (port);
-	}
-}
-
-static int lh7a40xuart_verify_port (struct uart_port* port,
-				    struct serial_struct* ser)
-{
-	int ret = 0;
-
-	if (ser->type != PORT_UNKNOWN && ser->type != PORT_LH7A40X)
-		ret = -EINVAL;
-	if (ser->irq < 0 || ser->irq >= nr_irqs)
-		ret = -EINVAL;
-	if (ser->baud_base < 9600) /* *** FIXME: is this true? */
-		ret = -EINVAL;
-	return ret;
-}
-
-static struct uart_ops lh7a40x_uart_ops = {
-	.tx_empty	= lh7a40xuart_tx_empty,
-	.set_mctrl	= lh7a40xuart_set_mctrl,
-	.get_mctrl	= lh7a40xuart_get_mctrl,
-	.stop_tx	= lh7a40xuart_stop_tx,
-	.start_tx	= lh7a40xuart_start_tx,
-	.stop_rx	= lh7a40xuart_stop_rx,
-	.enable_ms	= lh7a40xuart_enable_ms,
-	.break_ctl	= lh7a40xuart_break_ctl,
-	.startup	= lh7a40xuart_startup,
-	.shutdown	= lh7a40xuart_shutdown,
-	.set_termios	= lh7a40xuart_set_termios,
-	.type		= lh7a40xuart_type,
-	.release_port	= lh7a40xuart_release_port,
-	.request_port	= lh7a40xuart_request_port,
-	.config_port	= lh7a40xuart_config_port,
-	.verify_port	= lh7a40xuart_verify_port,
-};
-
-static struct uart_port_lh7a40x lh7a40x_ports[DEV_NR] = {
-	{
-		.port = {
-			.membase	= (void*) io_p2v (UART1_PHYS),
-			.mapbase	= UART1_PHYS,
-			.iotype		= UPIO_MEM,
-			.irq		= IRQ_UART1INTR,
-			.uartclk	= 14745600/2,
-			.fifosize	= 16,
-			.ops		= &lh7a40x_uart_ops,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 0,
-		},
-	},
-	{
-		.port = {
-			.membase	= (void*) io_p2v (UART2_PHYS),
-			.mapbase	= UART2_PHYS,
-			.iotype		= UPIO_MEM,
-			.irq		= IRQ_UART2INTR,
-			.uartclk	= 14745600/2,
-			.fifosize	= 16,
-			.ops		= &lh7a40x_uart_ops,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 1,
-		},
-	},
-	{
-		.port = {
-			.membase	= (void*) io_p2v (UART3_PHYS),
-			.mapbase	= UART3_PHYS,
-			.iotype		= UPIO_MEM,
-			.irq		= IRQ_UART3INTR,
-			.uartclk	= 14745600/2,
-			.fifosize	= 16,
-			.ops		= &lh7a40x_uart_ops,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 2,
-		},
-	},
-};
-
-#ifndef CONFIG_SERIAL_LH7A40X_CONSOLE
-# define LH7A40X_CONSOLE NULL
-#else
-# define LH7A40X_CONSOLE &lh7a40x_console
-
-static void lh7a40xuart_console_putchar(struct uart_port *port, int ch)
-{
-	while (UR(port, UART_R_STATUS) & nTxRdy)
-		;
-	UR(port, UART_R_DATA) = ch;
-}
-
-static void lh7a40xuart_console_write (struct console* co,
-				       const char* s,
-				       unsigned int count)
-{
-	struct uart_port* port = &lh7a40x_ports[co->index].port;
-	unsigned int con = UR (port, UART_R_CON);
-	unsigned int inten = UR (port, UART_R_INTEN);
-
-
-	UR (port, UART_R_INTEN) = 0;		/* Disable all interrupts */
-	BIT_SET (port, UART_R_CON, UARTEN | SIRDIS); /* Enable UART */
-
-	uart_console_write(port, s, count, lh7a40xuart_console_putchar);
-
-				/* Wait until all characters are sent */
-	while (UR (port, UART_R_STATUS) & TxBusy)
-		;
-
-				/* Restore control and interrupt mask */
-	UR (port, UART_R_CON) = con;
-	UR (port, UART_R_INTEN) = inten;
-}
-
-static void __init lh7a40xuart_console_get_options (struct uart_port* port,
-						    int* baud,
-						    int* parity,
-						    int* bits)
-{
-	if (UR (port, UART_R_CON) & UARTEN) {
-		unsigned int fcon = UR (port, UART_R_FCON);
-		unsigned int quot = UR (port, UART_R_BRCON) + 1;
-
-		switch (fcon & (PEN | EPS)) {
-		default:        *parity = 'n'; break;
-		case PEN:       *parity = 'o'; break;
-		case PEN | EPS: *parity = 'e'; break;
-		}
-
-		switch (fcon & WLEN) {
-		default:
-		case WLEN_8: *bits = 8; break;
-		case WLEN_7: *bits = 7; break;
-		case WLEN_6: *bits = 6; break;
-		case WLEN_5: *bits = 5; break;
-		}
-
-		*baud = port->uartclk/(16*quot);
-	}
-}
-
-static int __init lh7a40xuart_console_setup (struct console* co, char* options)
-{
-	struct uart_port* port;
-	int baud = 38400;
-	int bits = 8;
-	int parity = 'n';
-	int flow = 'n';
-
-	if (co->index >= DEV_NR) /* Bounds check on device number */
-		co->index = 0;
-	port = &lh7a40x_ports[co->index].port;
-
-	if (options)
-		uart_parse_options (options, &baud, &parity, &bits, &flow);
-	else
-		lh7a40xuart_console_get_options (port, &baud, &parity, &bits);
-
-	return uart_set_options (port, co, baud, parity, bits, flow);
-}
-
-static struct uart_driver lh7a40x_reg;
-static struct console lh7a40x_console = {
-	.name		= "ttyAM",
-	.write		= lh7a40xuart_console_write,
-	.device		= uart_console_device,
-	.setup		= lh7a40xuart_console_setup,
-	.flags		= CON_PRINTBUFFER,
-	.index		= -1,
-	.data		= &lh7a40x_reg,
-};
-
-static int __init lh7a40xuart_console_init(void)
-{
-	register_console (&lh7a40x_console);
-	return 0;
-}
-
-console_initcall (lh7a40xuart_console_init);
-
-#endif
-
-static struct uart_driver lh7a40x_reg = {
-	.owner			= THIS_MODULE,
-	.driver_name		= "ttyAM",
-	.dev_name		= "ttyAM",
-	.major			= DEV_MAJOR,
-	.minor			= DEV_MINOR,
-	.nr			= DEV_NR,
-	.cons			= LH7A40X_CONSOLE,
-};
-
-static int __init lh7a40xuart_init(void)
-{
-	int ret;
-
-	printk (KERN_INFO "serial: LH7A40X serial driver\n");
-
-	ret = uart_register_driver (&lh7a40x_reg);
-
-	if (ret == 0) {
-		int i;
-
-		for (i = 0; i < DEV_NR; i++) {
-			/* UART3, when used, requires GPIO pin reallocation */
-			if (lh7a40x_ports[i].port.mapbase == UART3_PHYS)
-				GPIO_PINMUX |= 1<<3;
-			uart_add_one_port (&lh7a40x_reg,
-					   &lh7a40x_ports[i].port);
-		}
-	}
-	return ret;
-}
-
-static void __exit lh7a40xuart_exit(void)
-{
-	int i;
-
-	for (i = 0; i < DEV_NR; i++)
-		uart_remove_one_port (&lh7a40x_reg, &lh7a40x_ports[i].port);
-
-	uart_unregister_driver (&lh7a40x_reg);
-}
-
-module_init (lh7a40xuart_init);
-module_exit (lh7a40xuart_exit);
-
-MODULE_AUTHOR ("Marc Singer");
-MODULE_DESCRIPTION ("Sharp LH7A40X serial port driver");
-MODULE_LICENSE ("GPL");
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 92c91c83edde..eb7958c675a8 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -47,7 +47,6 @@
 #include <linux/clk.h>
 #include <linux/ctype.h>
 #include <linux/err.h>
-#include <linux/list.h>
 #include <linux/dmaengine.h>
 #include <linux/scatterlist.h>
 #include <linux/slab.h>
@@ -65,11 +64,8 @@
 struct sci_port {
 	struct uart_port	port;
 
-	/* Port type */
-	unsigned int		type;
-
-	/* Port IRQs: ERI, RXI, TXI, BRI (optional) */
-	unsigned int		irqs[SCIx_NR_IRQS];
+	/* Platform configuration */
+	struct plat_sci_port	*cfg;
 
 	/* Port enable callback */
 	void			(*enable)(struct uart_port *port);
@@ -81,26 +77,15 @@ struct sci_port {
 	struct timer_list	break_timer;
 	int			break_flag;
 
-	/* SCSCR initialization */
-	unsigned int		scscr;
-
-	/* SCBRR calculation algo */
-	unsigned int		scbrr_algo_id;
-
 	/* Interface clock */
 	struct clk		*iclk;
 	/* Function clock */
 	struct clk		*fclk;
 
-	struct list_head	node;
-
 	struct dma_chan			*chan_tx;
 	struct dma_chan			*chan_rx;
 
 #ifdef CONFIG_SERIAL_SH_SCI_DMA
-	struct device			*dma_dev;
-	unsigned int			slave_tx;
-	unsigned int			slave_rx;
 	struct dma_async_tx_descriptor	*desc_tx;
 	struct dma_async_tx_descriptor	*desc_rx[2];
 	dma_cookie_t			cookie_tx;
@@ -117,16 +102,14 @@ struct sci_port {
 	struct timer_list		rx_timer;
 	unsigned int			rx_timeout;
 #endif
-};
 
-struct sh_sci_priv {
-	spinlock_t lock;
-	struct list_head ports;
-	struct notifier_block clk_nb;
+	struct notifier_block		freq_transition;
 };
 
 /* Function prototypes */
+static void sci_start_tx(struct uart_port *port);
 static void sci_stop_tx(struct uart_port *port);
+static void sci_start_rx(struct uart_port *port);
 
 #define SCI_NPORTS CONFIG_SERIAL_SH_SCI_NR_UARTS
 
@@ -142,12 +125,6 @@ to_sci_port(struct uart_port *uart)
 #if defined(CONFIG_CONSOLE_POLL) || defined(CONFIG_SERIAL_SH_SCI_CONSOLE)
 
 #ifdef CONFIG_CONSOLE_POLL
-static inline void handle_error(struct uart_port *port)
-{
-	/* Clear error flags */
-	sci_out(port, SCxSR, SCxSR_ERROR_CLEAR(port));
-}
-
 static int sci_poll_get_char(struct uart_port *port)
 {
 	unsigned short status;
@@ -156,7 +133,7 @@ static int sci_poll_get_char(struct uart_port *port)
 	do {
 		status = sci_in(port, SCxSR);
 		if (status & SCxSR_ERRORS(port)) {
-			handle_error(port);
+			sci_out(port, SCxSR, SCxSR_ERROR_CLEAR(port));
 			continue;
 		}
 		break;
@@ -475,7 +452,7 @@ static void sci_transmit_chars(struct uart_port *port)
 /* On SH3, SCIF may read end-of-break as a space->mark char */
 #define STEPFN(c)  ({int __c = (c); (((__c-1)|(__c)) == -1); })
 
-static inline void sci_receive_chars(struct uart_port *port)
+static void sci_receive_chars(struct uart_port *port)
 {
 	struct sci_port *sci_port = to_sci_port(port);
 	struct tty_struct *tty = port->state->port.tty;
@@ -566,18 +543,20 @@ static inline void sci_receive_chars(struct uart_port *port)
 }
 
 #define SCI_BREAK_JIFFIES (HZ/20)
-/* The sci generates interrupts during the break,
+
+/*
+ * The sci generates interrupts during the break,
  * 1 per millisecond or so during the break period, for 9600 baud.
  * So dont bother disabling interrupts.
  * But dont want more than 1 break event.
  * Use a kernel timer to periodically poll the rx line until
  * the break is finished.
  */
-static void sci_schedule_break_timer(struct sci_port *port)
+static inline void sci_schedule_break_timer(struct sci_port *port)
 {
-	port->break_timer.expires = jiffies + SCI_BREAK_JIFFIES;
-	add_timer(&port->break_timer);
+	mod_timer(&port->break_timer, jiffies + SCI_BREAK_JIFFIES);
 }
+
 /* Ensure that two consecutive samples find the break over. */
 static void sci_break_timer(unsigned long data)
 {
@@ -594,7 +573,7 @@ static void sci_break_timer(unsigned long data)
 		port->break_flag = 0;
 }
 
-static inline int sci_handle_errors(struct uart_port *port)
+static int sci_handle_errors(struct uart_port *port)
 {
 	int copied = 0;
 	unsigned short status = sci_in(port, SCxSR);
@@ -650,7 +629,7 @@ static inline int sci_handle_errors(struct uart_port *port)
 	return copied;
 }
 
-static inline int sci_handle_fifo_overrun(struct uart_port *port)
+static int sci_handle_fifo_overrun(struct uart_port *port)
 {
 	struct tty_struct *tty = port->state->port.tty;
 	int copied = 0;
@@ -671,7 +650,7 @@ static inline int sci_handle_fifo_overrun(struct uart_port *port)
 	return copied;
 }
 
-static inline int sci_handle_breaks(struct uart_port *port)
+static int sci_handle_breaks(struct uart_port *port)
 {
 	int copied = 0;
 	unsigned short status = sci_in(port, SCxSR);
@@ -794,7 +773,7 @@ static inline unsigned long port_rx_irq_mask(struct uart_port *port)
 	 * it's unset, it's logically inferred that there's no point in
 	 * testing for it.
 	 */
-	return SCSCR_RIE | (to_sci_port(port)->scscr & SCSCR_REIE);
+	return SCSCR_RIE | (to_sci_port(port)->cfg->scscr & SCSCR_REIE);
 }
 
 static irqreturn_t sci_mpxed_interrupt(int irq, void *ptr)
@@ -839,17 +818,18 @@ static irqreturn_t sci_mpxed_interrupt(int irq, void *ptr)
 static int sci_notifier(struct notifier_block *self,
 			unsigned long phase, void *p)
 {
-	struct sh_sci_priv *priv = container_of(self,
-						struct sh_sci_priv, clk_nb);
 	struct sci_port *sci_port;
 	unsigned long flags;
 
+	sci_port = container_of(self, struct sci_port, freq_transition);
+
 	if ((phase == CPUFREQ_POSTCHANGE) ||
 	    (phase == CPUFREQ_RESUMECHANGE)) {
-		spin_lock_irqsave(&priv->lock, flags);
-		list_for_each_entry(sci_port, &priv->ports, node)
-			sci_port->port.uartclk = clk_get_rate(sci_port->iclk);
-		spin_unlock_irqrestore(&priv->lock, flags);
+		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;
@@ -882,21 +862,21 @@ static int sci_request_irq(struct sci_port *port)
 	const char *desc[] = { "SCI Receive Error", "SCI Receive Data Full",
 			       "SCI Transmit Data Empty", "SCI Break" };
 
-	if (port->irqs[0] == port->irqs[1]) {
-		if (unlikely(!port->irqs[0]))
+	if (port->cfg->irqs[0] == port->cfg->irqs[1]) {
+		if (unlikely(!port->cfg->irqs[0]))
 			return -ENODEV;
 
-		if (request_irq(port->irqs[0], sci_mpxed_interrupt,
+		if (request_irq(port->cfg->irqs[0], sci_mpxed_interrupt,
 				IRQF_DISABLED, "sci", port)) {
 			dev_err(port->port.dev, "Can't allocate IRQ\n");
 			return -ENODEV;
 		}
 	} else {
 		for (i = 0; i < ARRAY_SIZE(handlers); i++) {
-			if (unlikely(!port->irqs[i]))
+			if (unlikely(!port->cfg->irqs[i]))
 				continue;
 
-			if (request_irq(port->irqs[i], handlers[i],
+			if (request_irq(port->cfg->irqs[i], handlers[i],
 					IRQF_DISABLED, desc[i], port)) {
 				dev_err(port->port.dev, "Can't allocate IRQ\n");
 				return -ENODEV;
@@ -911,14 +891,14 @@ static void sci_free_irq(struct sci_port *port)
 {
 	int i;
 
-	if (port->irqs[0] == port->irqs[1])
-		free_irq(port->irqs[0], port);
+	if (port->cfg->irqs[0] == port->cfg->irqs[1])
+		free_irq(port->cfg->irqs[0], port);
 	else {
-		for (i = 0; i < ARRAY_SIZE(port->irqs); i++) {
-			if (!port->irqs[i])
+		for (i = 0; i < ARRAY_SIZE(port->cfg->irqs); i++) {
+			if (!port->cfg->irqs[i])
 				continue;
 
-			free_irq(port->irqs[i], port);
+			free_irq(port->cfg->irqs[i], port);
 		}
 	}
 }
@@ -1037,9 +1017,6 @@ static void sci_dma_rx_complete(void *arg)
 	schedule_work(&s->work_rx);
 }
 
-static void sci_start_rx(struct uart_port *port);
-static void sci_start_tx(struct uart_port *port);
-
 static void sci_rx_dma_release(struct sci_port *s, bool enable_pio)
 {
 	struct dma_chan *chan = s->chan_rx;
@@ -1325,7 +1302,7 @@ static void rx_timer_fn(unsigned long arg)
 
 	if (port->type == PORT_SCIFA || port->type == PORT_SCIFB) {
 		scr &= ~0x4000;
-		enable_irq(s->irqs[1]);
+		enable_irq(s->cfg->irqs[1]);
 	}
 	sci_out(port, SCSCR, scr | SCSCR_RIE);
 	dev_dbg(port->dev, "DMA Rx timed out\n");
@@ -1341,9 +1318,9 @@ static void sci_request_dma(struct uart_port *port)
 	int nent;
 
 	dev_dbg(port->dev, "%s: port %d DMA %p\n", __func__,
-		port->line, s->dma_dev);
+		port->line, s->cfg->dma_dev);
 
-	if (!s->dma_dev)
+	if (!s->cfg->dma_dev)
 		return;
 
 	dma_cap_zero(mask);
@@ -1352,8 +1329,8 @@ static void sci_request_dma(struct uart_port *port)
 	param = &s->param_tx;
 
 	/* Slave ID, e.g., SHDMA_SLAVE_SCIF0_TX */
-	param->slave_id = s->slave_tx;
-	param->dma_dev = s->dma_dev;
+	param->slave_id = s->cfg->dma_slave_tx;
+	param->dma_dev = s->cfg->dma_dev;
 
 	s->cookie_tx = -EINVAL;
 	chan = dma_request_channel(mask, filter, param);
@@ -1381,8 +1358,8 @@ static void sci_request_dma(struct uart_port *port)
 	param = &s->param_rx;
 
 	/* Slave ID, e.g., SHDMA_SLAVE_SCIF0_RX */
-	param->slave_id = s->slave_rx;
-	param->dma_dev = s->dma_dev;
+	param->slave_id = s->cfg->dma_slave_rx;
+	param->dma_dev = s->cfg->dma_dev;
 
 	chan = dma_request_channel(mask, filter, param);
 	dev_dbg(port->dev, "%s: RX: got channel %p\n", __func__, chan);
@@ -1427,7 +1404,7 @@ static void sci_free_dma(struct uart_port *port)
 {
 	struct sci_port *s = to_sci_port(port);
 
-	if (!s->dma_dev)
+	if (!s->cfg->dma_dev)
 		return;
 
 	if (s->chan_tx)
@@ -1435,21 +1412,32 @@ static void sci_free_dma(struct uart_port *port)
 	if (s->chan_rx)
 		sci_rx_dma_release(s, false);
 }
+#else
+static inline void sci_request_dma(struct uart_port *port)
+{
+}
+
+static inline void sci_free_dma(struct uart_port *port)
+{
+}
 #endif
 
 static int sci_startup(struct uart_port *port)
 {
 	struct sci_port *s = to_sci_port(port);
+	int ret;
 
 	dev_dbg(port->dev, "%s(%d)\n", __func__, port->line);
 
 	if (s->enable)
 		s->enable(port);
 
-	sci_request_irq(s);
-#ifdef CONFIG_SERIAL_SH_SCI_DMA
+	ret = sci_request_irq(s);
+	if (unlikely(ret < 0))
+		return ret;
+
 	sci_request_dma(port);
-#endif
+
 	sci_start_tx(port);
 	sci_start_rx(port);
 
@@ -1464,9 +1452,8 @@ static void sci_shutdown(struct uart_port *port)
 
 	sci_stop_rx(port);
 	sci_stop_tx(port);
-#ifdef CONFIG_SERIAL_SH_SCI_DMA
+
 	sci_free_dma(port);
-#endif
 	sci_free_irq(s);
 
 	if (s->disable)
@@ -1491,6 +1478,7 @@ static unsigned int sci_scbrr_calc(unsigned int algo_id, unsigned int bps,
 
 	/* Warn, but use a safe default */
 	WARN_ON(1);
+
 	return ((freq + 16 * bps) / (32 * bps) - 1);
 }
 
@@ -1514,7 +1502,10 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	baud = uart_get_baud_rate(port, termios, old, 0, max_baud);
 	if (likely(baud && port->uartclk))
-		t = sci_scbrr_calc(s->scbrr_algo_id, baud, port->uartclk);
+		t = sci_scbrr_calc(s->cfg->scbrr_algo_id, baud, port->uartclk);
+
+	if (s->enable)
+		s->enable(port);
 
 	do {
 		status = sci_in(port, SCxSR);
@@ -1526,6 +1517,7 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 		sci_out(port, SCFCR, scfcr | SCFCR_RFRST | SCFCR_TFRST);
 
 	smr_val = sci_in(port, SCSMR) & 3;
+
 	if ((termios->c_cflag & CSIZE) == CS7)
 		smr_val |= 0x40;
 	if (termios->c_cflag & PARENB)
@@ -1540,7 +1532,7 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 	sci_out(port, SCSMR, smr_val);
 
 	dev_dbg(port->dev, "%s: SMR %x, t %x, SCSCR %x\n", __func__, smr_val, t,
-		s->scscr);
+		s->cfg->scscr);
 
 	if (t > 0) {
 		if (t >= 256) {
@@ -1556,7 +1548,7 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 	sci_init_pins(port, termios->c_cflag);
 	sci_out(port, SCFCR, scfcr | ((termios->c_cflag & CRTSCTS) ? SCFCR_MCE : 0));
 
-	sci_out(port, SCSCR, s->scscr);
+	sci_out(port, SCSCR, s->cfg->scscr);
 
 #ifdef CONFIG_SERIAL_SH_SCI_DMA
 	/*
@@ -1582,6 +1574,9 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	if ((termios->c_cflag & CREAD) != 0)
 		sci_start_rx(port);
+
+	if (s->disable)
+		s->disable(port);
 }
 
 static const char *sci_type(struct uart_port *port)
@@ -1602,31 +1597,33 @@ static const char *sci_type(struct uart_port *port)
 	return NULL;
 }
 
-static void sci_release_port(struct uart_port *port)
+static inline unsigned long sci_port_size(struct uart_port *port)
 {
-	/* Nothing here yet .. */
-}
-
-static int sci_request_port(struct uart_port *port)
-{
-	/* Nothing here yet .. */
-	return 0;
+	/*
+	 * Pick an arbitrary size that encapsulates all of the base
+	 * registers by default. This can be optimized later, or derived
+	 * from platform resource data at such a time that ports begin to
+	 * behave more erratically.
+	 */
+	return 64;
 }
 
-static void sci_config_port(struct uart_port *port, int flags)
+static int sci_remap_port(struct uart_port *port)
 {
-	struct sci_port *s = to_sci_port(port);
-
-	port->type = s->type;
+	unsigned long size = sci_port_size(port);
 
+	/*
+	 * Nothing to do if there's already an established membase.
+	 */
 	if (port->membase)
-		return;
+		return 0;
 
 	if (port->flags & UPF_IOREMAP) {
-		port->membase = ioremap_nocache(port->mapbase, 0x40);
-
-		if (IS_ERR(port->membase))
+		port->membase = ioremap_nocache(port->mapbase, size);
+		if (unlikely(!port->membase)) {
 			dev_err(port->dev, "can't remap port#%d\n", port->line);
+			return -ENXIO;
+		}
 	} else {
 		/*
 		 * For the simple (and majority of) cases where we don't
@@ -1635,13 +1632,54 @@ static void sci_config_port(struct uart_port *port, int flags)
 		 */
 		port->membase = (void __iomem *)port->mapbase;
 	}
+
+	return 0;
+}
+
+static void sci_release_port(struct uart_port *port)
+{
+	if (port->flags & UPF_IOREMAP) {
+		iounmap(port->membase);
+		port->membase = NULL;
+	}
+
+	release_mem_region(port->mapbase, sci_port_size(port));
+}
+
+static int sci_request_port(struct uart_port *port)
+{
+	unsigned long size = sci_port_size(port);
+	struct resource *res;
+	int ret;
+
+	res = request_mem_region(port->mapbase, size, dev_name(port->dev));
+	if (unlikely(res == NULL))
+		return -EBUSY;
+
+	ret = sci_remap_port(port);
+	if (unlikely(ret != 0)) {
+		release_resource(res);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void sci_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE) {
+		struct sci_port *sport = to_sci_port(port);
+
+		port->type = sport->cfg->type;
+		sci_request_port(port);
+	}
 }
 
 static int sci_verify_port(struct uart_port *port, struct serial_struct *ser)
 {
 	struct sci_port *s = to_sci_port(port);
 
-	if (ser->irq != s->irqs[SCIx_TXI_IRQ] || ser->irq > nr_irqs)
+	if (ser->irq != s->cfg->irqs[SCIx_TXI_IRQ] || ser->irq > nr_irqs)
 		return -EINVAL;
 	if (ser->baud_base < 2400)
 		/* No paper tape reader for Mitch.. */
@@ -1726,36 +1764,29 @@ static int __devinit sci_init_single(struct platform_device *dev,
 	sci_port->break_timer.function = sci_break_timer;
 	init_timer(&sci_port->break_timer);
 
-	port->mapbase	= p->mapbase;
-	port->membase	= p->membase;
+	sci_port->cfg		= p;
 
-	port->irq		= p->irqs[SCIx_TXI_IRQ];
+	port->mapbase		= p->mapbase;
+	port->type		= p->type;
 	port->flags		= p->flags;
-	sci_port->type		= port->type = p->type;
-	sci_port->scscr		= p->scscr;
-	sci_port->scbrr_algo_id	= p->scbrr_algo_id;
 
-#ifdef CONFIG_SERIAL_SH_SCI_DMA
-	sci_port->dma_dev	= p->dma_dev;
-	sci_port->slave_tx	= p->dma_slave_tx;
-	sci_port->slave_rx	= p->dma_slave_rx;
+	/*
+	 * The UART port needs an IRQ value, so we peg this to the TX IRQ
+	 * for the multi-IRQ ports, which is where we are primarily
+	 * concerned with the shutdown path synchronization.
+	 *
+	 * For the muxed case there's nothing more to do.
+	 */
+	port->irq		= p->irqs[SCIx_TXI_IRQ];
 
-	dev_dbg(port->dev, "%s: DMA device %p, tx %d, rx %d\n", __func__,
-		p->dma_dev, p->dma_slave_tx, p->dma_slave_rx);
-#endif
+	if (p->dma_dev)
+		dev_dbg(port->dev, "DMA device %p, tx %d, rx %d\n",
+			p->dma_dev, p->dma_slave_tx, p->dma_slave_rx);
 
-	memcpy(&sci_port->irqs, &p->irqs, sizeof(p->irqs));
 	return 0;
 }
 
 #ifdef CONFIG_SERIAL_SH_SCI_CONSOLE
-static struct tty_driver *serial_console_device(struct console *co, int *index)
-{
-	struct uart_driver *p = &sci_uart_driver;
-	*index = co->index;
-	return p->tty_driver;
-}
-
 static void serial_console_putchar(struct uart_port *port, int ch)
 {
 	sci_poll_put_char(port, ch);
@@ -1768,8 +1799,8 @@ static void serial_console_putchar(struct uart_port *port, int ch)
 static void serial_console_write(struct console *co, const char *s,
 				 unsigned count)
 {
-	struct uart_port *port = co->data;
-	struct sci_port *sci_port = to_sci_port(port);
+	struct sci_port *sci_port = &sci_ports[co->index];
+	struct uart_port *port = &sci_port->port;
 	unsigned short bits;
 
 	if (sci_port->enable)
@@ -1797,32 +1828,17 @@ static int __devinit serial_console_setup(struct console *co, char *options)
 	int ret;
 
 	/*
-	 * Check whether an invalid uart number has been specified, and
-	 * if so, search for the first available port that does have
-	 * console support.
-	 */
-	if (co->index >= SCI_NPORTS)
-		co->index = 0;
-
-	if (co->data) {
-		port = co->data;
-		sci_port = to_sci_port(port);
-	} else {
-		sci_port = &sci_ports[co->index];
-		port = &sci_port->port;
-		co->data = port;
-	}
-
-	/*
-	 * Also need to check port->type, we don't actually have any
-	 * UPIO_PORT ports, but uart_report_port() handily misreports
-	 * it anyways if we don't have a port available by the time this is
-	 * called.
+	 * Refuse to handle any bogus ports.
 	 */
-	if (!port->type)
+	if (co->index < 0 || co->index >= SCI_NPORTS)
 		return -ENODEV;
 
-	sci_config_port(port, 0);
+	sci_port = &sci_ports[co->index];
+	port = &sci_port->port;
+
+	ret = sci_remap_port(port);
+	if (unlikely(ret != 0))
+		return ret;
 
 	if (sci_port->enable)
 		sci_port->enable(port);
@@ -1842,11 +1858,12 @@ static int __devinit serial_console_setup(struct console *co, char *options)
 
 static struct console serial_console = {
 	.name		= "ttySC",
-	.device		= serial_console_device,
+	.device		= uart_console_device,
 	.write		= serial_console_write,
 	.setup		= serial_console_setup,
 	.flags		= CON_PRINTBUFFER,
 	.index		= -1,
+	.data		= &sci_uart_driver,
 };
 
 static int __init sci_console_init(void)
@@ -1856,14 +1873,39 @@ static int __init sci_console_init(void)
 }
 console_initcall(sci_console_init);
 
-static struct sci_port early_serial_port;
 static struct console early_serial_console = {
 	.name           = "early_ttySC",
 	.write          = serial_console_write,
 	.flags          = CON_PRINTBUFFER,
+	.index		= -1,
 };
+
 static char early_serial_buf[32];
 
+static int __devinit sci_probe_earlyprintk(struct platform_device *pdev)
+{
+	struct plat_sci_port *cfg = pdev->dev.platform_data;
+
+	if (early_serial_console.data)
+		return -EEXIST;
+
+	early_serial_console.index = pdev->id;
+
+	sci_init_single(NULL, &sci_ports[pdev->id], pdev->id, cfg);
+
+	serial_console_setup(&early_serial_console, early_serial_buf);
+
+	if (!strstr(early_serial_buf, "keep"))
+		early_serial_console.flags |= CON_BOOT;
+
+	register_console(&early_serial_console);
+	return 0;
+}
+#else
+static inline int __devinit sci_probe_earlyprintk(struct platform_device *pdev)
+{
+	return -EINVAL;
+}
 #endif /* CONFIG_SERIAL_SH_SCI_CONSOLE */
 
 #if defined(CONFIG_SERIAL_SH_SCI_CONSOLE)
@@ -1885,24 +1927,18 @@ static struct uart_driver sci_uart_driver = {
 	.cons		= SCI_CONSOLE,
 };
 
-
 static int sci_remove(struct platform_device *dev)
 {
-	struct sh_sci_priv *priv = platform_get_drvdata(dev);
-	struct sci_port *p;
-	unsigned long flags;
+	struct sci_port *port = platform_get_drvdata(dev);
 
-	cpufreq_unregister_notifier(&priv->clk_nb, CPUFREQ_TRANSITION_NOTIFIER);
+	cpufreq_unregister_notifier(&port->freq_transition,
+				    CPUFREQ_TRANSITION_NOTIFIER);
 
-	spin_lock_irqsave(&priv->lock, flags);
-	list_for_each_entry(p, &priv->ports, node) {
-		uart_remove_one_port(&sci_uart_driver, &p->port);
-		clk_put(p->iclk);
-		clk_put(p->fclk);
-	}
-	spin_unlock_irqrestore(&priv->lock, flags);
+	uart_remove_one_port(&sci_uart_driver, &port->port);
+
+	clk_put(port->iclk);
+	clk_put(port->fclk);
 
-	kfree(priv);
 	return 0;
 }
 
@@ -1911,8 +1947,6 @@ static int __devinit sci_probe_single(struct platform_device *dev,
 				      struct plat_sci_port *p,
 				      struct sci_port *sciport)
 {
-	struct sh_sci_priv *priv = platform_get_drvdata(dev);
-	unsigned long flags;
 	int ret;
 
 	/* Sanity check */
@@ -1929,68 +1963,35 @@ static int __devinit sci_probe_single(struct platform_device *dev,
 	if (ret)
 		return ret;
 
-	ret = uart_add_one_port(&sci_uart_driver, &sciport->port);
-	if (ret)
-		return ret;
-
-	INIT_LIST_HEAD(&sciport->node);
-
-	spin_lock_irqsave(&priv->lock, flags);
-	list_add(&sciport->node, &priv->ports);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
+	return uart_add_one_port(&sci_uart_driver, &sciport->port);
 }
 
-/*
- * Register a set of serial devices attached to a platform device.  The
- * list is terminated with a zero flags entry, which means we expect
- * all entries to have at least UPF_BOOT_AUTOCONF set. Platforms that need
- * remapping (such as sh64) should also set UPF_IOREMAP.
- */
 static int __devinit sci_probe(struct platform_device *dev)
 {
 	struct plat_sci_port *p = dev->dev.platform_data;
-	struct sh_sci_priv *priv;
-	int i, ret = -EINVAL;
+	struct sci_port *sp = &sci_ports[dev->id];
+	int ret;
 
-#ifdef CONFIG_SERIAL_SH_SCI_CONSOLE
-	if (is_early_platform_device(dev)) {
-		if (dev->id == -1)
-			return -ENOTSUPP;
-		early_serial_console.index = dev->id;
-		early_serial_console.data = &early_serial_port.port;
-		sci_init_single(NULL, &early_serial_port, dev->id, p);
-		serial_console_setup(&early_serial_console, early_serial_buf);
-		if (!strstr(early_serial_buf, "keep"))
-			early_serial_console.flags |= CON_BOOT;
-		register_console(&early_serial_console);
-		return 0;
-	}
-#endif
+	/*
+	 * If we've come here via earlyprintk initialization, head off to
+	 * the special early probe. We don't have sufficient device state
+	 * to make it beyond this yet.
+	 */
+	if (is_early_platform_device(dev))
+		return sci_probe_earlyprintk(dev);
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
+	platform_set_drvdata(dev, sp);
 
-	INIT_LIST_HEAD(&priv->ports);
-	spin_lock_init(&priv->lock);
-	platform_set_drvdata(dev, priv);
+	ret = sci_probe_single(dev, dev->id, p, sp);
+	if (ret)
+		goto err_unreg;
 
-	priv->clk_nb.notifier_call = sci_notifier;
-	cpufreq_register_notifier(&priv->clk_nb, CPUFREQ_TRANSITION_NOTIFIER);
+	sp->freq_transition.notifier_call = sci_notifier;
 
-	if (dev->id != -1) {
-		ret = sci_probe_single(dev, dev->id, p, &sci_ports[dev->id]);
-		if (ret)
-			goto err_unreg;
-	} else {
-		for (i = 0; p && p->flags != 0; p++, i++) {
-			ret = sci_probe_single(dev, i, p, &sci_ports[i]);
-			if (ret)
-				goto err_unreg;
-		}
-	}
+	ret = cpufreq_register_notifier(&sp->freq_transition,
+					CPUFREQ_TRANSITION_NOTIFIER);
+	if (unlikely(ret < 0))
+		goto err_unreg;
 
 #ifdef CONFIG_SH_STANDARD_BIOS
 	sh_bios_gdb_detach();
@@ -2005,28 +2006,20 @@ err_unreg:
 
 static int sci_suspend(struct device *dev)
 {
-	struct sh_sci_priv *priv = dev_get_drvdata(dev);
-	struct sci_port *p;
-	unsigned long flags;
+	struct sci_port *sport = dev_get_drvdata(dev);
 
-	spin_lock_irqsave(&priv->lock, flags);
-	list_for_each_entry(p, &priv->ports, node)
-		uart_suspend_port(&sci_uart_driver, &p->port);
-	spin_unlock_irqrestore(&priv->lock, flags);
+	if (sport)
+		uart_suspend_port(&sci_uart_driver, &sport->port);
 
 	return 0;
 }
 
 static int sci_resume(struct device *dev)
 {
-	struct sh_sci_priv *priv = dev_get_drvdata(dev);
-	struct sci_port *p;
-	unsigned long flags;
+	struct sci_port *sport = dev_get_drvdata(dev);
 
-	spin_lock_irqsave(&priv->lock, flags);
-	list_for_each_entry(p, &priv->ports, node)
-		uart_resume_port(&sci_uart_driver, &p->port);
-	spin_unlock_irqrestore(&priv->lock, flags);
+	if (sport)
+		uart_resume_port(&sci_uart_driver, &sport->port);
 
 	return 0;
 }
diff --git a/drivers/tty/serial/sh-sci.h b/drivers/tty/serial/sh-sci.h
index b223d6cbf33a..5fefed53fa42 100644
--- a/drivers/tty/serial/sh-sci.h
+++ b/drivers/tty/serial/sh-sci.h
@@ -54,9 +54,6 @@
 # define PBCR 0xa4050102
 #elif defined(CONFIG_CPU_SUBTYPE_SH7343)
 # define SCSPTR0 0xffe00010	/* 16 bit SCIF */
-# define SCSPTR1 0xffe10010	/* 16 bit SCIF */
-# define SCSPTR2 0xffe20010	/* 16 bit SCIF */
-# define SCSPTR3 0xffe30010	/* 16 bit SCIF */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7722)
 # define PADR			0xA4050120
 # define PSDR			0xA405013e
@@ -69,77 +66,42 @@
 # define SCIF_ORER		0x0001  /* overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7723)
 # define SCSPTR0                0xa4050160
-# define SCSPTR1                0xa405013e
-# define SCSPTR2                0xa4050160
-# define SCSPTR3                0xa405013e
-# define SCSPTR4                0xa4050128
-# define SCSPTR5                0xa4050128
 # define SCIF_ORER              0x0001  /* overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7724)
 # define SCIF_ORER              0x0001  /* overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH4_202)
 # define SCSPTR2 0xffe80020 /* 16 bit SCIF */
 # define SCIF_ORER 0x0001   /* overrun error bit */
-#elif defined(CONFIG_CPU_SUBTYPE_SH5_101) || defined(CONFIG_CPU_SUBTYPE_SH5_103)
-# define SCIF_PTR2_OFFS    0x0000020
-# define SCSPTR2           ((port->mapbase)+SCIF_PTR2_OFFS) /* 16 bit SCIF */
 #elif defined(CONFIG_H83007) || defined(CONFIG_H83068)
 # define H8300_SCI_DR(ch) *(volatile char *)(P1DR + h8300_sci_pins[ch].port)
 #elif defined(CONFIG_H8S2678)
 # define H8300_SCI_DR(ch) *(volatile char *)(P1DR + h8300_sci_pins[ch].port)
 #elif defined(CONFIG_CPU_SUBTYPE_SH7757)
 # define SCSPTR0 0xfe4b0020
-# define SCSPTR1 0xfe4b0020
-# define SCSPTR2 0xfe4b0020
 # define SCIF_ORER 0x0001
-# define SCIF_ONLY
 #elif defined(CONFIG_CPU_SUBTYPE_SH7763)
 # define SCSPTR0 0xffe00024 /* 16 bit SCIF */
-# define SCSPTR1 0xffe08024 /* 16 bit SCIF */
-# define SCSPTR2 0xffe10020 /* 16 bit SCIF/IRDA */
 # define SCIF_ORER 0x0001  /* overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7770)
 # define SCSPTR0 0xff923020 /* 16 bit SCIF */
-# define SCSPTR1 0xff924020 /* 16 bit SCIF */
-# define SCSPTR2 0xff925020 /* 16 bit SCIF */
 # define SCIF_ORER 0x0001  /* overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7780)
 # define SCSPTR0	0xffe00024	/* 16 bit SCIF */
-# define SCSPTR1	0xffe10024	/* 16 bit SCIF */
 # define SCIF_ORER	0x0001		/* Overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7785) || \
       defined(CONFIG_CPU_SUBTYPE_SH7786)
 # define SCSPTR0	0xffea0024	/* 16 bit SCIF */
-# define SCSPTR1	0xffeb0024	/* 16 bit SCIF */
-# define SCSPTR2	0xffec0024	/* 16 bit SCIF */
-# define SCSPTR3	0xffed0024	/* 16 bit SCIF */
-# define SCSPTR4	0xffee0024	/* 16 bit SCIF */
-# define SCSPTR5	0xffef0024	/* 16 bit SCIF */
 # define SCIF_ORER	0x0001		/* Overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SH7201) || \
       defined(CONFIG_CPU_SUBTYPE_SH7203) || \
       defined(CONFIG_CPU_SUBTYPE_SH7206) || \
       defined(CONFIG_CPU_SUBTYPE_SH7263)
 # define SCSPTR0 0xfffe8020 /* 16 bit SCIF */
-# define SCSPTR1 0xfffe8820 /* 16 bit SCIF */
-# define SCSPTR2 0xfffe9020 /* 16 bit SCIF */
-# define SCSPTR3 0xfffe9820 /* 16 bit SCIF */
-# if defined(CONFIG_CPU_SUBTYPE_SH7201)
-#  define SCSPTR4 0xfffeA020 /* 16 bit SCIF */
-#  define SCSPTR5 0xfffeA820 /* 16 bit SCIF */
-#  define SCSPTR6 0xfffeB020 /* 16 bit SCIF */
-#  define SCSPTR7 0xfffeB820 /* 16 bit SCIF */
-# endif
 #elif defined(CONFIG_CPU_SUBTYPE_SH7619)
 # define SCSPTR0 0xf8400020 /* 16 bit SCIF */
-# define SCSPTR1 0xf8410020 /* 16 bit SCIF */
-# define SCSPTR2 0xf8420020 /* 16 bit SCIF */
 # define SCIF_ORER 0x0001  /* overrun error bit */
 #elif defined(CONFIG_CPU_SUBTYPE_SHX3)
 # define SCSPTR0 0xffc30020		/* 16 bit SCIF */
-# define SCSPTR1 0xffc40020		/* 16 bit SCIF */
-# define SCSPTR2 0xffc50020		/* 16 bit SCIF */
-# define SCSPTR3 0xffc60020		/* 16 bit SCIF */
 # define SCIF_ORER 0x0001		/* Overrun error bit */
 #else
 # error CPU subtype not defined
@@ -411,7 +373,6 @@ SCIF_FNS(SCSPTR,			0,  0, 0x24, 16)
 SCIF_FNS(SCLSR,				0,  0, 0x28, 16)
 #elif defined(CONFIG_CPU_SUBTYPE_SH7763)
 SCIF_FNS(SCFDR,				0,  0, 0x1C, 16)
-SCIF_FNS(SCSPTR2,			0,  0, 0x20, 16)
 SCIF_FNS(SCTFDR,		     0x0e, 16, 0x1C, 16)
 SCIF_FNS(SCRFDR,		     0x0e, 16, 0x20, 16)
 SCIF_FNS(SCSPTR,			0,  0, 0x24, 16)
diff --git a/drivers/tty/serial/sunhv.c b/drivers/tty/serial/sunhv.c
index c9014868297d..c0b7246d7339 100644
--- a/drivers/tty/serial/sunhv.c
+++ b/drivers/tty/serial/sunhv.c
@@ -519,7 +519,7 @@ static struct console sunhv_console = {
 	.data	=	&sunhv_reg,
 };
 
-static int __devinit hv_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit hv_probe(struct platform_device *op)
 {
 	struct uart_port *port;
 	unsigned long minor;
@@ -629,7 +629,7 @@ static const struct of_device_id hv_match[] = {
 };
 MODULE_DEVICE_TABLE(of, hv_match);
 
-static struct of_platform_driver hv_driver = {
+static struct platform_driver hv_driver = {
 	.driver = {
 		.name = "hv",
 		.owner = THIS_MODULE,
@@ -644,12 +644,12 @@ static int __init sunhv_init(void)
 	if (tlb_type != hypervisor)
 		return -ENODEV;
 
-	return of_register_platform_driver(&hv_driver);
+	return platform_driver_register(&hv_driver);
 }
 
 static void __exit sunhv_exit(void)
 {
-	of_unregister_platform_driver(&hv_driver);
+	platform_driver_unregister(&hv_driver);
 }
 
 module_init(sunhv_init);
diff --git a/drivers/tty/serial/sunsab.c b/drivers/tty/serial/sunsab.c
index 5b246b18f42f..b5fa2a57b9da 100644
--- a/drivers/tty/serial/sunsab.c
+++ b/drivers/tty/serial/sunsab.c
@@ -1006,7 +1006,7 @@ static int __devinit sunsab_init_one(struct uart_sunsab_port *up,
 	return 0;
 }
 
-static int __devinit sab_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit sab_probe(struct platform_device *op)
 {
 	static int inst;
 	struct uart_sunsab_port *up;
@@ -1092,7 +1092,7 @@ static const struct of_device_id sab_match[] = {
 };
 MODULE_DEVICE_TABLE(of, sab_match);
 
-static struct of_platform_driver sab_driver = {
+static struct platform_driver sab_driver = {
 	.driver = {
 		.name = "sab",
 		.owner = THIS_MODULE,
@@ -1130,12 +1130,12 @@ static int __init sunsab_init(void)
 		}
 	}
 
-	return of_register_platform_driver(&sab_driver);
+	return platform_driver_register(&sab_driver);
 }
 
 static void __exit sunsab_exit(void)
 {
-	of_unregister_platform_driver(&sab_driver);
+	platform_driver_unregister(&sab_driver);
 	if (sunsab_reg.nr) {
 		sunserial_unregister_minors(&sunsab_reg, sunsab_reg.nr);
 	}
diff --git a/drivers/tty/serial/sunsu.c b/drivers/tty/serial/sunsu.c
index 551ebfe3ccbb..92aa54550e84 100644
--- a/drivers/tty/serial/sunsu.c
+++ b/drivers/tty/serial/sunsu.c
@@ -1406,7 +1406,7 @@ static enum su_type __devinit su_get_type(struct device_node *dp)
 	return SU_PORT_PORT;
 }
 
-static int __devinit su_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit su_probe(struct platform_device *op)
 {
 	static int inst;
 	struct device_node *dp = op->dev.of_node;
@@ -1543,7 +1543,7 @@ static const struct of_device_id su_match[] = {
 };
 MODULE_DEVICE_TABLE(of, su_match);
 
-static struct of_platform_driver su_driver = {
+static struct platform_driver su_driver = {
 	.driver = {
 		.name = "su",
 		.owner = THIS_MODULE,
@@ -1586,7 +1586,7 @@ static int __init sunsu_init(void)
 			return err;
 	}
 
-	err = of_register_platform_driver(&su_driver);
+	err = platform_driver_register(&su_driver);
 	if (err && num_uart)
 		sunserial_unregister_minors(&sunsu_reg, num_uart);
 
diff --git a/drivers/tty/serial/sunzilog.c b/drivers/tty/serial/sunzilog.c
index c1967ac1c07f..99ff9abf57ce 100644
--- a/drivers/tty/serial/sunzilog.c
+++ b/drivers/tty/serial/sunzilog.c
@@ -1399,7 +1399,7 @@ static void __devinit sunzilog_init_hw(struct uart_sunzilog_port *up)
 
 static int zilog_irq = -1;
 
-static int __devinit zs_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit zs_probe(struct platform_device *op)
 {
 	static int kbm_inst, uart_inst;
 	int inst;
@@ -1540,7 +1540,7 @@ static const struct of_device_id zs_match[] = {
 };
 MODULE_DEVICE_TABLE(of, zs_match);
 
-static struct of_platform_driver zs_driver = {
+static struct platform_driver zs_driver = {
 	.driver = {
 		.name = "zs",
 		.owner = THIS_MODULE,
@@ -1576,7 +1576,7 @@ static int __init sunzilog_init(void)
 			goto out_free_tables;
 	}
 
-	err = of_register_platform_driver(&zs_driver);
+	err = platform_driver_register(&zs_driver);
 	if (err)
 		goto out_unregister_uart;
 
@@ -1604,7 +1604,7 @@ out:
 	return err;
 
 out_unregister_driver:
-	of_unregister_platform_driver(&zs_driver);
+	platform_driver_unregister(&zs_driver);
 
 out_unregister_uart:
 	if (num_sunzilog) {
@@ -1619,7 +1619,7 @@ out_free_tables:
 
 static void __exit sunzilog_exit(void)
 {
-	of_unregister_platform_driver(&zs_driver);
+	platform_driver_unregister(&zs_driver);
 
 	if (zilog_irq != -1) {
 		struct uart_sunzilog_port *up = sunzilog_irq_chain;
diff --git a/drivers/tty/serial/uartlite.c b/drivers/tty/serial/uartlite.c
index d2fce865b731..8af1ed83a4c0 100644
--- a/drivers/tty/serial/uartlite.c
+++ b/drivers/tty/serial/uartlite.c
@@ -19,22 +19,11 @@
 #include <linux/interrupt.h>
 #include <linux/init.h>
 #include <asm/io.h>
-#if defined(CONFIG_OF) && (defined(CONFIG_PPC32) || defined(CONFIG_MICROBLAZE))
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
 
-/* Match table for of_platform binding */
-static struct of_device_id ulite_of_match[] __devinitdata = {
-	{ .compatible = "xlnx,opb-uartlite-1.00.b", },
-	{ .compatible = "xlnx,xps-uartlite-1.00.a", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, ulite_of_match);
-
-#endif
-
 #define ULITE_NAME		"ttyUL"
 #define ULITE_MAJOR		204
 #define ULITE_MINOR		187
@@ -571,9 +560,29 @@ static int __devexit ulite_release(struct device *dev)
  * Platform bus binding
  */
 
+#if defined(CONFIG_OF)
+/* Match table for of_platform binding */
+static struct of_device_id ulite_of_match[] __devinitdata = {
+	{ .compatible = "xlnx,opb-uartlite-1.00.b", },
+	{ .compatible = "xlnx,xps-uartlite-1.00.a", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ulite_of_match);
+#else /* CONFIG_OF */
+#define ulite_of_match NULL
+#endif /* CONFIG_OF */
+
 static int __devinit ulite_probe(struct platform_device *pdev)
 {
 	struct resource *res, *res2;
+	int id = pdev->id;
+#ifdef CONFIG_OF
+	const __be32 *prop;
+
+	prop = of_get_property(pdev->dev.of_node, "port-number", NULL);
+	if (prop)
+		id = be32_to_cpup(prop);
+#endif
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
@@ -583,7 +592,7 @@ static int __devinit ulite_probe(struct platform_device *pdev)
 	if (!res2)
 		return -ENODEV;
 
-	return ulite_assign(&pdev->dev, pdev->id, res->start, res2->start);
+	return ulite_assign(&pdev->dev, id, res->start, res2->start);
 }
 
 static int __devexit ulite_remove(struct platform_device *pdev)
@@ -595,72 +604,15 @@ static int __devexit ulite_remove(struct platform_device *pdev)
 MODULE_ALIAS("platform:uartlite");
 
 static struct platform_driver ulite_platform_driver = {
-	.probe	= ulite_probe,
-	.remove	= __devexit_p(ulite_remove),
-	.driver	= {
-		   .owner = THIS_MODULE,
-		   .name  = "uartlite",
-		   },
-};
-
-/* ---------------------------------------------------------------------
- * OF bus bindings
- */
-#if defined(CONFIG_OF) && (defined(CONFIG_PPC32) || defined(CONFIG_MICROBLAZE))
-static int __devinit
-ulite_of_probe(struct platform_device *op, const struct of_device_id *match)
-{
-	struct resource res;
-	const unsigned int *id;
-	int irq, rc;
-
-	dev_dbg(&op->dev, "%s(%p, %p)\n", __func__, op, match);
-
-	rc = of_address_to_resource(op->dev.of_node, 0, &res);
-	if (rc) {
-		dev_err(&op->dev, "invalid address\n");
-		return rc;
-	}
-
-	irq = irq_of_parse_and_map(op->dev.of_node, 0);
-
-	id = of_get_property(op->dev.of_node, "port-number", NULL);
-
-	return ulite_assign(&op->dev, id ? *id : -1, res.start, irq);
-}
-
-static int __devexit ulite_of_remove(struct platform_device *op)
-{
-	return ulite_release(&op->dev);
-}
-
-static struct of_platform_driver ulite_of_driver = {
-	.probe = ulite_of_probe,
-	.remove = __devexit_p(ulite_of_remove),
+	.probe = ulite_probe,
+	.remove = __devexit_p(ulite_remove),
 	.driver = {
-		.name = "uartlite",
 		.owner = THIS_MODULE,
+		.name  = "uartlite",
 		.of_match_table = ulite_of_match,
 	},
 };
 
-/* Registration helpers to keep the number of #ifdefs to a minimum */
-static inline int __init ulite_of_register(void)
-{
-	pr_debug("uartlite: calling of_register_platform_driver()\n");
-	return of_register_platform_driver(&ulite_of_driver);
-}
-
-static inline void __exit ulite_of_unregister(void)
-{
-	of_unregister_platform_driver(&ulite_of_driver);
-}
-#else /* CONFIG_OF && (CONFIG_PPC32 || CONFIG_MICROBLAZE) */
-/* Appropriate config not enabled; do nothing helpers */
-static inline int __init ulite_of_register(void) { return 0; }
-static inline void __exit ulite_of_unregister(void) { }
-#endif /* CONFIG_OF && (CONFIG_PPC32 || CONFIG_MICROBLAZE) */
-
 /* ---------------------------------------------------------------------
  * Module setup/teardown
  */
@@ -674,10 +626,6 @@ int __init ulite_init(void)
 	if (ret)
 		goto err_uart;
 
-	ret = ulite_of_register();
-	if (ret)
-		goto err_of;
-
 	pr_debug("uartlite: calling platform_driver_register()\n");
 	ret = platform_driver_register(&ulite_platform_driver);
 	if (ret)
@@ -686,8 +634,6 @@ int __init ulite_init(void)
 	return 0;
 
 err_plat:
-	ulite_of_unregister();
-err_of:
 	uart_unregister_driver(&ulite_uart_driver);
 err_uart:
 	printk(KERN_ERR "registering uartlite driver failed: err=%i", ret);
@@ -697,7 +643,6 @@ err_uart:
 void __exit ulite_exit(void)
 {
 	platform_driver_unregister(&ulite_platform_driver);
-	ulite_of_unregister();
 	uart_unregister_driver(&ulite_uart_driver);
 }
 
diff --git a/drivers/tty/serial/ucc_uart.c b/drivers/tty/serial/ucc_uart.c
index 3f4848e2174a..ff51dae1df0c 100644
--- a/drivers/tty/serial/ucc_uart.c
+++ b/drivers/tty/serial/ucc_uart.c
@@ -1194,8 +1194,7 @@ static void uart_firmware_cont(const struct firmware *fw, void *context)
 	release_firmware(fw);
 }
 
-static int ucc_uart_probe(struct platform_device *ofdev,
-	const struct of_device_id *match)
+static int ucc_uart_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	const unsigned int *iprop;      /* Integer OF properties */
@@ -1485,7 +1484,7 @@ static struct of_device_id ucc_uart_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ucc_uart_match);
 
-static struct of_platform_driver ucc_uart_of_driver = {
+static struct platform_driver ucc_uart_of_driver = {
 	.driver = {
 		.name = "ucc_uart",
 		.owner = THIS_MODULE,
@@ -1510,7 +1509,7 @@ static int __init ucc_uart_init(void)
 		return ret;
 	}
 
-	ret = of_register_platform_driver(&ucc_uart_of_driver);
+	ret = platform_driver_register(&ucc_uart_of_driver);
 	if (ret)
 		printk(KERN_ERR
 		       "ucc-uart: could not register platform driver\n");
@@ -1523,7 +1522,7 @@ static void __exit ucc_uart_exit(void)
 	printk(KERN_INFO
 	       "Freescale QUICC Engine UART device driver unloading\n");
 
-	of_unregister_platform_driver(&ucc_uart_of_driver);
+	platform_driver_unregister(&ucc_uart_of_driver);
 	uart_unregister_driver(&ucc_uart_driver);
 }
 
diff --git a/drivers/uio/Kconfig b/drivers/uio/Kconfig
index bb440792a1b7..6f3ea9bbc818 100644
--- a/drivers/uio/Kconfig
+++ b/drivers/uio/Kconfig
@@ -94,4 +94,21 @@ config UIO_NETX
 	  To compile this driver as a module, choose M here; the module
 	  will be called uio_netx.
 
+config UIO_PRUSS
+	tristate "Texas Instruments PRUSS driver"
+	depends on ARCH_DAVINCI_DA850
+	help
+	  PRUSS driver for OMAPL138/DA850/AM18XX devices
+	  PRUSS driver requires user space components, examples and user space
+	  driver is available from below SVN repo - you may use anonymous login
+
+	  https://gforge.ti.com/gf/project/pru_sw/
+
+	  More info on API is available at below wiki
+
+	  http://processors.wiki.ti.com/index.php/PRU_Linux_Application_Loader
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called uio_pruss.
+
 endif
diff --git a/drivers/uio/Makefile b/drivers/uio/Makefile
index 18fd818c5b97..d4dd9a5552f8 100644
--- a/drivers/uio/Makefile
+++ b/drivers/uio/Makefile
@@ -6,3 +6,4 @@ obj-$(CONFIG_UIO_AEC)	+= uio_aec.o
 obj-$(CONFIG_UIO_SERCOS3)	+= uio_sercos3.o
 obj-$(CONFIG_UIO_PCI_GENERIC)	+= uio_pci_generic.o
 obj-$(CONFIG_UIO_NETX)	+= uio_netx.o
+obj-$(CONFIG_UIO_PRUSS)         += uio_pruss.o
diff --git a/drivers/uio/uio_pruss.c b/drivers/uio/uio_pruss.c
new file mode 100644
index 000000000000..daf6e77de2b1
--- /dev/null
+++ b/drivers/uio/uio_pruss.c
@@ -0,0 +1,247 @@
+/*
+ * Programmable Real-Time Unit Sub System (PRUSS) UIO driver (uio_pruss)
+ *
+ * This driver exports PRUSS host event out interrupts and PRUSS, L3 RAM,
+ * and DDR RAM to user space for applications interacting with PRUSS firmware
+ *
+ * Copyright (C) 2010-11 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/uio_driver.h>
+#include <linux/platform_data/uio_pruss.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <mach/sram.h>
+
+#define DRV_NAME "pruss_uio"
+#define DRV_VERSION "1.0"
+
+static int sram_pool_sz = SZ_16K;
+module_param(sram_pool_sz, int, 0);
+MODULE_PARM_DESC(sram_pool_sz, "sram pool size to allocate ");
+
+static int extram_pool_sz = SZ_256K;
+module_param(extram_pool_sz, int, 0);
+MODULE_PARM_DESC(extram_pool_sz, "external ram pool size to allocate");
+
+/*
+ * Host event IRQ numbers from PRUSS - PRUSS can generate upto 8 interrupt
+ * events to AINTC of ARM host processor - which can be used for IPC b/w PRUSS
+ * firmware and user space application, async notification from PRU firmware
+ * to user space application
+ * 3	PRU_EVTOUT0
+ * 4	PRU_EVTOUT1
+ * 5	PRU_EVTOUT2
+ * 6	PRU_EVTOUT3
+ * 7	PRU_EVTOUT4
+ * 8	PRU_EVTOUT5
+ * 9	PRU_EVTOUT6
+ * 10	PRU_EVTOUT7
+*/
+#define MAX_PRUSS_EVT	8
+
+#define PINTC_HIDISR	0x0038
+#define PINTC_HIPIR	0x0900
+#define HIPIR_NOPEND	0x80000000
+#define PINTC_HIER	0x1500
+
+struct uio_pruss_dev {
+	struct uio_info *info;
+	struct clk *pruss_clk;
+	dma_addr_t sram_paddr;
+	dma_addr_t ddr_paddr;
+	void __iomem *prussio_vaddr;
+	void *sram_vaddr;
+	void *ddr_vaddr;
+	unsigned int hostirq_start;
+	unsigned int pintc_base;
+};
+
+static irqreturn_t pruss_handler(int irq, struct uio_info *info)
+{
+	struct uio_pruss_dev *gdev = info->priv;
+	int intr_bit = (irq - gdev->hostirq_start + 2);
+	int val, intr_mask = (1 << intr_bit);
+	void __iomem *base = gdev->prussio_vaddr + gdev->pintc_base;
+	void __iomem *intren_reg = base + PINTC_HIER;
+	void __iomem *intrdis_reg = base + PINTC_HIDISR;
+	void __iomem *intrstat_reg = base + PINTC_HIPIR + (intr_bit << 2);
+
+	val = ioread32(intren_reg);
+	/* Is interrupt enabled and active ? */
+	if (!(val & intr_mask) && (ioread32(intrstat_reg) & HIPIR_NOPEND))
+		return IRQ_NONE;
+	/* Disable interrupt */
+	iowrite32(intr_bit, intrdis_reg);
+	return IRQ_HANDLED;
+}
+
+static void pruss_cleanup(struct platform_device *dev,
+			struct uio_pruss_dev *gdev)
+{
+	int cnt;
+	struct uio_info *p = gdev->info;
+
+	for (cnt = 0; cnt < MAX_PRUSS_EVT; cnt++, p++) {
+		uio_unregister_device(p);
+		kfree(p->name);
+	}
+	iounmap(gdev->prussio_vaddr);
+	if (gdev->ddr_vaddr) {
+		dma_free_coherent(&dev->dev, extram_pool_sz, gdev->ddr_vaddr,
+			gdev->ddr_paddr);
+	}
+	if (gdev->sram_vaddr)
+		sram_free(gdev->sram_vaddr, sram_pool_sz);
+	kfree(gdev->info);
+	clk_put(gdev->pruss_clk);
+	kfree(gdev);
+}
+
+static int __devinit pruss_probe(struct platform_device *dev)
+{
+	struct uio_info *p;
+	struct uio_pruss_dev *gdev;
+	struct resource *regs_prussio;
+	int ret = -ENODEV, cnt = 0, len;
+	struct uio_pruss_pdata *pdata = dev->dev.platform_data;
+
+	gdev = kzalloc(sizeof(struct uio_pruss_dev), GFP_KERNEL);
+	if (!gdev)
+		return -ENOMEM;
+
+	gdev->info = kzalloc(sizeof(*p) * MAX_PRUSS_EVT, GFP_KERNEL);
+	if (!gdev->info) {
+		kfree(gdev);
+		return -ENOMEM;
+	}
+	/* Power on PRU in case its not done as part of boot-loader */
+	gdev->pruss_clk = clk_get(&dev->dev, "pruss");
+	if (IS_ERR(gdev->pruss_clk)) {
+		dev_err(&dev->dev, "Failed to get clock\n");
+		kfree(gdev->info);
+		kfree(gdev);
+		ret = PTR_ERR(gdev->pruss_clk);
+		return ret;
+	} else {
+		clk_enable(gdev->pruss_clk);
+	}
+
+	regs_prussio = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!regs_prussio) {
+		dev_err(&dev->dev, "No PRUSS I/O resource specified\n");
+		goto out_free;
+	}
+
+	if (!regs_prussio->start) {
+		dev_err(&dev->dev, "Invalid memory resource\n");
+		goto out_free;
+	}
+
+	gdev->sram_vaddr = sram_alloc(sram_pool_sz, &(gdev->sram_paddr));
+	if (!gdev->sram_vaddr) {
+		dev_err(&dev->dev, "Could not allocate SRAM pool\n");
+		goto out_free;
+	}
+
+	gdev->ddr_vaddr = dma_alloc_coherent(&dev->dev, extram_pool_sz,
+				&(gdev->ddr_paddr), GFP_KERNEL | GFP_DMA);
+	if (!gdev->ddr_vaddr) {
+		dev_err(&dev->dev, "Could not allocate external memory\n");
+		goto out_free;
+	}
+
+	len = resource_size(regs_prussio);
+	gdev->prussio_vaddr = ioremap(regs_prussio->start, len);
+	if (!gdev->prussio_vaddr) {
+		dev_err(&dev->dev, "Can't remap PRUSS I/O  address range\n");
+		goto out_free;
+	}
+
+	gdev->pintc_base = pdata->pintc_base;
+	gdev->hostirq_start = platform_get_irq(dev, 0);
+
+	for (cnt = 0, p = gdev->info; cnt < MAX_PRUSS_EVT; cnt++, p++) {
+		p->mem[0].addr = regs_prussio->start;
+		p->mem[0].size = resource_size(regs_prussio);
+		p->mem[0].memtype = UIO_MEM_PHYS;
+
+		p->mem[1].addr = gdev->sram_paddr;
+		p->mem[1].size = sram_pool_sz;
+		p->mem[1].memtype = UIO_MEM_PHYS;
+
+		p->mem[2].addr = gdev->ddr_paddr;
+		p->mem[2].size = extram_pool_sz;
+		p->mem[2].memtype = UIO_MEM_PHYS;
+
+		p->name = kasprintf(GFP_KERNEL, "pruss_evt%d", cnt);
+		p->version = DRV_VERSION;
+
+		/* Register PRUSS IRQ lines */
+		p->irq = gdev->hostirq_start + cnt;
+		p->handler = pruss_handler;
+		p->priv = gdev;
+
+		ret = uio_register_device(&dev->dev, p);
+		if (ret < 0)
+			goto out_free;
+	}
+
+	platform_set_drvdata(dev, gdev);
+	return 0;
+
+out_free:
+	pruss_cleanup(dev, gdev);
+	return ret;
+}
+
+static int __devexit pruss_remove(struct platform_device *dev)
+{
+	struct uio_pruss_dev *gdev = platform_get_drvdata(dev);
+
+	pruss_cleanup(dev, gdev);
+	platform_set_drvdata(dev, NULL);
+	return 0;
+}
+
+static struct platform_driver pruss_driver = {
+	.probe = pruss_probe,
+	.remove = __devexit_p(pruss_remove),
+	.driver = {
+		   .name = DRV_NAME,
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init pruss_init_module(void)
+{
+	return platform_driver_register(&pruss_driver);
+}
+
+module_init(pruss_init_module);
+
+static void __exit pruss_exit_module(void)
+{
+	platform_driver_unregister(&pruss_driver);
+}
+
+module_exit(pruss_exit_module);
+
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR("Amit Chatterjee <amit.chatterjee@ti.com>");
+MODULE_AUTHOR("Pratheesh Gangadhar <pratheesh@ti.com>");
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index fceea5e4e02f..41b6e51188e4 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -31,7 +31,6 @@ config USB_ARCH_HAS_OHCI
 	# ARM:
 	default y if SA1111
 	default y if ARCH_OMAP
-	default y if ARCH_LH7A404
 	default y if ARCH_S3C2410
 	default y if PXA27x
 	default y if PXA3xx
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index f71e8e307e0f..64a035ba2eab 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -335,7 +335,7 @@ void usb_hcd_pci_shutdown(struct pci_dev *dev)
 }
 EXPORT_SYMBOL_GPL(usb_hcd_pci_shutdown);
 
-#ifdef	CONFIG_PM_OPS
+#ifdef	CONFIG_PM
 
 #ifdef	CONFIG_PPC_PMAC
 static void powermac_set_asic(struct pci_dev *pci_dev, int enable)
@@ -580,4 +580,4 @@ const struct dev_pm_ops usb_hcd_pci_pm_ops = {
 };
 EXPORT_SYMBOL_GPL(usb_hcd_pci_pm_ops);
 
-#endif	/* CONFIG_PM_OPS */
+#endif	/* CONFIG_PM */
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index d041c6826e43..c03c65b63b9d 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -616,7 +616,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 }
 
 /*
- * Disable a port and mark a logical connnect-change event, so that some
+ * Disable a port and mark a logical connect-change event, so that some
  * time later khubd will disconnect() any existing usb_device on the port
  * and will re-enumerate if there actually is a device attached.
  */
@@ -1465,6 +1465,7 @@ void usb_set_device_state(struct usb_device *udev,
 		enum usb_device_state new_state)
 {
 	unsigned long flags;
+	int wakeup = -1;
 
 	spin_lock_irqsave(&device_state_lock, flags);
 	if (udev->state == USB_STATE_NOTATTACHED)
@@ -1479,11 +1480,10 @@ void usb_set_device_state(struct usb_device *udev,
 					|| new_state == USB_STATE_SUSPENDED)
 				;	/* No change to wakeup settings */
 			else if (new_state == USB_STATE_CONFIGURED)
-				device_set_wakeup_capable(&udev->dev,
-					(udev->actconfig->desc.bmAttributes
-					 & USB_CONFIG_ATT_WAKEUP));
+				wakeup = udev->actconfig->desc.bmAttributes
+					 & USB_CONFIG_ATT_WAKEUP;
 			else
-				device_set_wakeup_capable(&udev->dev, 0);
+				wakeup = 0;
 		}
 		if (udev->state == USB_STATE_SUSPENDED &&
 			new_state != USB_STATE_SUSPENDED)
@@ -1495,6 +1495,8 @@ void usb_set_device_state(struct usb_device *udev,
 	} else
 		recursively_mark_NOTATTACHED(udev);
 	spin_unlock_irqrestore(&device_state_lock, flags);
+	if (wakeup >= 0)
+		device_set_wakeup_capable(&udev->dev, wakeup);
 }
 EXPORT_SYMBOL_GPL(usb_set_device_state);
 
@@ -2681,17 +2683,13 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 
 	mutex_lock(&usb_address0_mutex);
 
-	if (!udev->config && oldspeed == USB_SPEED_SUPER) {
-		/* Don't reset USB 3.0 devices during an initial setup */
-		usb_set_device_state(udev, USB_STATE_DEFAULT);
-	} else {
-		/* Reset the device; full speed may morph to high speed */
-		/* FIXME a USB 2.0 device may morph into SuperSpeed on reset. */
-		retval = hub_port_reset(hub, port1, udev, delay);
-		if (retval < 0)		/* error or disconnect */
-			goto fail;
-		/* success, speed is known */
-	}
+	/* Reset the device; full speed may morph to high speed */
+	/* FIXME a USB 2.0 device may morph into SuperSpeed on reset. */
+	retval = hub_port_reset(hub, port1, udev, delay);
+	if (retval < 0)		/* error or disconnect */
+		goto fail;
+	/* success, speed is known */
+
 	retval = -ENODEV;
 
 	if (oldspeed != USB_SPEED_UNKNOWN && oldspeed != udev->speed) {
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 44c595432d6f..81ce6a8e1d94 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -48,6 +48,10 @@ static const struct usb_device_id usb_quirk_list[] = {
 	{ USB_DEVICE(0x04b4, 0x0526), .driver_info =
 			USB_QUIRK_CONFIG_INTF_STRINGS },
 
+	/* Samsung Android phone modem - ID conflict with SPH-I500 */
+	{ USB_DEVICE(0x04e8, 0x6601), .driver_info =
+			USB_QUIRK_CONFIG_INTF_STRINGS },
+
 	/* Roland SC-8820 */
 	{ USB_DEVICE(0x0582, 0x0007), .driver_info = USB_QUIRK_RESET_RESUME },
 
@@ -68,6 +72,10 @@ static const struct usb_device_id usb_quirk_list[] = {
 	/* M-Systems Flash Disk Pioneers */
 	{ USB_DEVICE(0x08ec, 0x1000), .driver_info = USB_QUIRK_RESET_RESUME },
 
+	/* Keytouch QWERTY Panel keyboard */
+	{ USB_DEVICE(0x0926, 0x3333), .driver_info =
+			USB_QUIRK_CONFIG_INTF_STRINGS },
+
 	/* X-Rite/Gretag-Macbeth Eye-One Pro display colorimeter */
 	{ USB_DEVICE(0x0971, 0x2000), .driver_info = USB_QUIRK_NO_SET_INTF },
 
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index d50099675f28..e8cbbdac7a15 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -176,18 +176,6 @@ config USB_FSL_USB2
 	default USB_GADGET
 	select USB_GADGET_SELECTED
 
-config USB_GADGET_LH7A40X
-	boolean "LH7A40X"
-	depends on ARCH_LH7A40X
-	help
-	   This driver provides USB Device Controller driver for LH7A40x
-
-config USB_LH7A40X
-	tristate
-	depends on USB_GADGET_LH7A40X
-	default USB_GADGET
-	select USB_GADGET_SELECTED
-
 config USB_GADGET_OMAP
 	boolean "OMAP USB Device Controller"
 	depends on ARCH_OMAP
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 55f5e8ae5924..a2f7f9a4f4a8 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -11,7 +11,6 @@ obj-$(CONFIG_USB_PXA27X)	+= pxa27x_udc.o
 obj-$(CONFIG_USB_IMX)		+= imx_udc.o
 obj-$(CONFIG_USB_GOKU)		+= goku_udc.o
 obj-$(CONFIG_USB_OMAP)		+= omap_udc.o
-obj-$(CONFIG_USB_LH7A40X)	+= lh7a40x_udc.o
 obj-$(CONFIG_USB_S3C2410)	+= s3c2410_udc.o
 obj-$(CONFIG_USB_AT91)		+= at91_udc.o
 obj-$(CONFIG_USB_ATMEL_USBA)	+= atmel_usba_udc.o
diff --git a/drivers/usb/gadget/f_phonet.c b/drivers/usb/gadget/f_phonet.c
index 3c6e1a058745..5e1495097ec3 100644
--- a/drivers/usb/gadget/f_phonet.c
+++ b/drivers/usb/gadget/f_phonet.c
@@ -346,14 +346,19 @@ static void pn_rx_complete(struct usb_ep *ep, struct usb_request *req)
 
 		if (unlikely(!skb))
 			break;
-		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page, 0,
-				req->actual);
-		page = NULL;
 
-		if (req->actual < req->length) { /* Last fragment */
+		if (skb->len == 0) { /* First fragment */
 			skb->protocol = htons(ETH_P_PHONET);
 			skb_reset_mac_header(skb);
-			pskb_pull(skb, 1);
+			/* Can't use pskb_pull() on page in IRQ */
+			memcpy(skb_put(skb, 1), page_address(page), 1);
+		}
+
+		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
+				skb->len == 0, req->actual);
+		page = NULL;
+
+		if (req->actual < req->length) { /* Last fragment */
 			skb->dev = dev;
 			dev->stats.rx_packets++;
 			dev->stats.rx_bytes += skb->len;
diff --git a/drivers/usb/gadget/fsl_mxc_udc.c b/drivers/usb/gadget/fsl_mxc_udc.c
index 77b1eb577029..834fe945f9dd 100644
--- a/drivers/usb/gadget/fsl_mxc_udc.c
+++ b/drivers/usb/gadget/fsl_mxc_udc.c
@@ -88,7 +88,7 @@ eenahb:
 void fsl_udc_clk_finalize(struct platform_device *pdev)
 {
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
-#if defined(CONFIG_ARCH_MX35)
+#if defined(CONFIG_SOC_IMX35)
 	unsigned int v;
 
 	/* workaround ENGcm09152 for i.MX35 */
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index 792d5ef40137..aee7e3c53c38 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -2523,8 +2523,7 @@ static void qe_udc_release(struct device *dev)
 }
 
 /* Driver probe functions */
-static int __devinit qe_udc_probe(struct platform_device *ofdev,
-			const struct of_device_id *match)
+static int __devinit qe_udc_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct qe_ep *ep;
@@ -2532,6 +2531,9 @@ static int __devinit qe_udc_probe(struct platform_device *ofdev,
 	unsigned int i;
 	const void *prop;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+
 	prop = of_get_property(np, "mode", NULL);
 	if (!prop || strcmp(prop, "peripheral"))
 		return -ENODEV;
@@ -2543,7 +2545,7 @@ static int __devinit qe_udc_probe(struct platform_device *ofdev,
 		return -ENOMEM;
 	}
 
-	udc_controller->soc_type = (unsigned long)match->data;
+	udc_controller->soc_type = (unsigned long)ofdev->dev.of_match->data;
 	udc_controller->usb_regs = of_iomap(np, 0);
 	if (!udc_controller->usb_regs) {
 		ret = -ENOMEM;
@@ -2768,7 +2770,7 @@ static const struct of_device_id qe_udc_match[] __devinitconst = {
 
 MODULE_DEVICE_TABLE(of, qe_udc_match);
 
-static struct of_platform_driver udc_driver = {
+static struct platform_driver udc_driver = {
 	.driver = {
 		.name = (char *)driver_name,
 		.owner = THIS_MODULE,
@@ -2786,12 +2788,12 @@ static int __init qe_udc_init(void)
 {
 	printk(KERN_INFO "%s: %s, %s\n", driver_name, driver_desc,
 			DRIVER_VERSION);
-	return of_register_platform_driver(&udc_driver);
+	return platform_driver_register(&udc_driver);
 }
 
 static void __exit qe_udc_exit(void)
 {
-	of_unregister_platform_driver(&udc_driver);
+	platform_driver_unregister(&udc_driver);
 }
 
 module_init(qe_udc_init);
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index 5c2720d64ffa..e896f6359dfe 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -45,12 +45,6 @@
 #define	gadget_is_goku(g)	0
 #endif
 
-#ifdef CONFIG_USB_GADGET_LH7A40X
-#define	gadget_is_lh7a40x(g)	!strcmp("lh7a40x_udc", (g)->name)
-#else
-#define	gadget_is_lh7a40x(g)	0
-#endif
-
 #ifdef CONFIG_USB_GADGET_OMAP
 #define	gadget_is_omap(g)	!strcmp("omap_udc", (g)->name)
 #else
@@ -181,8 +175,6 @@ static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x06;
 	else if (gadget_is_omap(gadget))
 		return 0x08;
-	else if (gadget_is_lh7a40x(gadget))
-		return 0x09;
 	else if (gadget_is_pxa27x(gadget))
 		return 0x11;
 	else if (gadget_is_s3c2410(gadget))
diff --git a/drivers/usb/gadget/lh7a40x_udc.c b/drivers/usb/gadget/lh7a40x_udc.c
deleted file mode 100644
index 6b58bd8ce623..000000000000
--- a/drivers/usb/gadget/lh7a40x_udc.c
+++ /dev/null
@@ -1,2152 +0,0 @@
-/*
- * linux/drivers/usb/gadget/lh7a40x_udc.c
- * Sharp LH7A40x on-chip full speed USB device controllers
- *
- * Copyright (C) 2004 Mikko Lahteenmaki, Nordic ID
- * Copyright (C) 2004 Bo Henriksen, Nordic ID
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU 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/platform_device.h>
-#include <linux/slab.h>
-
-#include "lh7a40x_udc.h"
-
-//#define DEBUG printk
-//#define DEBUG_EP0 printk
-//#define DEBUG_SETUP printk
-
-#ifndef DEBUG_EP0
-# define DEBUG_EP0(fmt,args...)
-#endif
-#ifndef DEBUG_SETUP
-# define DEBUG_SETUP(fmt,args...)
-#endif
-#ifndef DEBUG
-# define NO_STATES
-# define DEBUG(fmt,args...)
-#endif
-
-#define	DRIVER_DESC			"LH7A40x USB Device Controller"
-#define	DRIVER_VERSION		__DATE__
-
-#ifndef _BIT			/* FIXME - what happended to _BIT in 2.6.7bk18? */
-#define _BIT(x) (1<<(x))
-#endif
-
-struct lh7a40x_udc *the_controller;
-
-static const char driver_name[] = "lh7a40x_udc";
-static const char driver_desc[] = DRIVER_DESC;
-static const char ep0name[] = "ep0-control";
-
-/*
-  Local definintions.
-*/
-
-#ifndef NO_STATES
-static char *state_names[] = {
-	"WAIT_FOR_SETUP",
-	"DATA_STATE_XMIT",
-	"DATA_STATE_NEED_ZLP",
-	"WAIT_FOR_OUT_STATUS",
-	"DATA_STATE_RECV"
-};
-#endif
-
-/*
-  Local declarations.
-*/
-static int lh7a40x_ep_enable(struct usb_ep *ep,
-			     const struct usb_endpoint_descriptor *);
-static int lh7a40x_ep_disable(struct usb_ep *ep);
-static struct usb_request *lh7a40x_alloc_request(struct usb_ep *ep, gfp_t);
-static void lh7a40x_free_request(struct usb_ep *ep, struct usb_request *);
-static int lh7a40x_queue(struct usb_ep *ep, struct usb_request *, gfp_t);
-static int lh7a40x_dequeue(struct usb_ep *ep, struct usb_request *);
-static int lh7a40x_set_halt(struct usb_ep *ep, int);
-static int lh7a40x_fifo_status(struct usb_ep *ep);
-static void lh7a40x_fifo_flush(struct usb_ep *ep);
-static void lh7a40x_ep0_kick(struct lh7a40x_udc *dev, struct lh7a40x_ep *ep);
-static void lh7a40x_handle_ep0(struct lh7a40x_udc *dev, u32 intr);
-
-static void done(struct lh7a40x_ep *ep, struct lh7a40x_request *req,
-		 int status);
-static void pio_irq_enable(int bEndpointAddress);
-static void pio_irq_disable(int bEndpointAddress);
-static void stop_activity(struct lh7a40x_udc *dev,
-			  struct usb_gadget_driver *driver);
-static void flush(struct lh7a40x_ep *ep);
-static void udc_enable(struct lh7a40x_udc *dev);
-static void udc_set_address(struct lh7a40x_udc *dev, unsigned char address);
-
-static struct usb_ep_ops lh7a40x_ep_ops = {
-	.enable = lh7a40x_ep_enable,
-	.disable = lh7a40x_ep_disable,
-
-	.alloc_request = lh7a40x_alloc_request,
-	.free_request = lh7a40x_free_request,
-
-	.queue = lh7a40x_queue,
-	.dequeue = lh7a40x_dequeue,
-
-	.set_halt = lh7a40x_set_halt,
-	.fifo_status = lh7a40x_fifo_status,
-	.fifo_flush = lh7a40x_fifo_flush,
-};
-
-/* Inline code */
-
-static __inline__ int write_packet(struct lh7a40x_ep *ep,
-				   struct lh7a40x_request *req, int max)
-{
-	u8 *buf;
-	int length, count;
-	volatile u32 *fifo = (volatile u32 *)ep->fifo;
-
-	buf = req->req.buf + req->req.actual;
-	prefetch(buf);
-
-	length = req->req.length - req->req.actual;
-	length = min(length, max);
-	req->req.actual += length;
-
-	DEBUG("Write %d (max %d), fifo %p\n", length, max, fifo);
-
-	count = length;
-	while (count--) {
-		*fifo = *buf++;
-	}
-
-	return length;
-}
-
-static __inline__ void usb_set_index(u32 ep)
-{
-	*(volatile u32 *)io_p2v(USB_INDEX) = ep;
-}
-
-static __inline__ u32 usb_read(u32 port)
-{
-	return *(volatile u32 *)io_p2v(port);
-}
-
-static __inline__ void usb_write(u32 val, u32 port)
-{
-	*(volatile u32 *)io_p2v(port) = val;
-}
-
-static __inline__ void usb_set(u32 val, u32 port)
-{
-	volatile u32 *ioport = (volatile u32 *)io_p2v(port);
-	u32 after = (*ioport) | val;
-	*ioport = after;
-}
-
-static __inline__ void usb_clear(u32 val, u32 port)
-{
-	volatile u32 *ioport = (volatile u32 *)io_p2v(port);
-	u32 after = (*ioport) & ~val;
-	*ioport = after;
-}
-
-/*-------------------------------------------------------------------------*/
-
-#define GPIO_PORTC_DR 	(0x80000E08)
-#define GPIO_PORTC_DDR 	(0x80000E18)
-#define GPIO_PORTC_PDR 	(0x80000E70)
-
-/* get port C pin data register */
-#define get_portc_pdr(bit) 		((usb_read(GPIO_PORTC_PDR) & _BIT(bit)) != 0)
-/* get port C data direction register */
-#define get_portc_ddr(bit) 		((usb_read(GPIO_PORTC_DDR) & _BIT(bit)) != 0)
-/* set port C data register */
-#define set_portc_dr(bit, val) 	(val ? usb_set(_BIT(bit), GPIO_PORTC_DR) : usb_clear(_BIT(bit), GPIO_PORTC_DR))
-/* set port C data direction register */
-#define set_portc_ddr(bit, val) (val ? usb_set(_BIT(bit), GPIO_PORTC_DDR) : usb_clear(_BIT(bit), GPIO_PORTC_DDR))
-
-/*
- * LPD7A404 GPIO's:
- * Port C bit 1 = USB Port 1 Power Enable
- * Port C bit 2 = USB Port 1 Data Carrier Detect
- */
-#define is_usb_connected() 		get_portc_pdr(2)
-
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-
-static const char proc_node_name[] = "driver/udc";
-
-static int
-udc_proc_read(char *page, char **start, off_t off, int count,
-	      int *eof, void *_dev)
-{
-	char *buf = page;
-	struct lh7a40x_udc *dev = _dev;
-	char *next = buf;
-	unsigned size = count;
-	unsigned long flags;
-	int t;
-
-	if (off != 0)
-		return 0;
-
-	local_irq_save(flags);
-
-	/* basic device status */
-	t = scnprintf(next, size,
-		      DRIVER_DESC "\n"
-		      "%s version: %s\n"
-		      "Gadget driver: %s\n"
-		      "Host: %s\n\n",
-		      driver_name, DRIVER_VERSION,
-		      dev->driver ? dev->driver->driver.name : "(none)",
-		      is_usb_connected()? "full speed" : "disconnected");
-	size -= t;
-	next += t;
-
-	t = scnprintf(next, size,
-		      "GPIO:\n"
-		      " Port C bit 1: %d, dir %d\n"
-		      " Port C bit 2: %d, dir %d\n\n",
-		      get_portc_pdr(1), get_portc_ddr(1),
-		      get_portc_pdr(2), get_portc_ddr(2)
-	    );
-	size -= t;
-	next += t;
-
-	t = scnprintf(next, size,
-		      "DCP pullup: %d\n\n",
-		      (usb_read(USB_PM) & PM_USB_DCP) != 0);
-	size -= t;
-	next += t;
-
-	local_irq_restore(flags);
-	*eof = 1;
-	return count - size;
-}
-
-#define create_proc_files() 	create_proc_read_entry(proc_node_name, 0, NULL, udc_proc_read, dev)
-#define remove_proc_files() 	remove_proc_entry(proc_node_name, NULL)
-
-#else	/* !CONFIG_USB_GADGET_DEBUG_FILES */
-
-#define create_proc_files() do {} while (0)
-#define remove_proc_files() do {} while (0)
-
-#endif	/* CONFIG_USB_GADGET_DEBUG_FILES */
-
-/*
- * 	udc_disable - disable USB device controller
- */
-static void udc_disable(struct lh7a40x_udc *dev)
-{
-	DEBUG("%s, %p\n", __func__, dev);
-
-	udc_set_address(dev, 0);
-
-	/* Disable interrupts */
-	usb_write(0, USB_IN_INT_EN);
-	usb_write(0, USB_OUT_INT_EN);
-	usb_write(0, USB_INT_EN);
-
-	/* Disable the USB */
-	usb_write(0, USB_PM);
-
-#ifdef CONFIG_ARCH_LH7A404
-	/* Disable USB power */
-	set_portc_dr(1, 0);
-#endif
-
-	/* if hardware supports it, disconnect from usb */
-	/* make_usb_disappear(); */
-
-	dev->ep0state = WAIT_FOR_SETUP;
-	dev->gadget.speed = USB_SPEED_UNKNOWN;
-	dev->usb_address = 0;
-}
-
-/*
- * 	udc_reinit - initialize software state
- */
-static void udc_reinit(struct lh7a40x_udc *dev)
-{
-	u32 i;
-
-	DEBUG("%s, %p\n", __func__, dev);
-
-	/* device/ep0 records init */
-	INIT_LIST_HEAD(&dev->gadget.ep_list);
-	INIT_LIST_HEAD(&dev->gadget.ep0->ep_list);
-	dev->ep0state = WAIT_FOR_SETUP;
-
-	/* basic endpoint records init */
-	for (i = 0; i < UDC_MAX_ENDPOINTS; i++) {
-		struct lh7a40x_ep *ep = &dev->ep[i];
-
-		if (i != 0)
-			list_add_tail(&ep->ep.ep_list, &dev->gadget.ep_list);
-
-		ep->desc = 0;
-		ep->stopped = 0;
-		INIT_LIST_HEAD(&ep->queue);
-		ep->pio_irqs = 0;
-	}
-
-	/* the rest was statically initialized, and is read-only */
-}
-
-#define BYTES2MAXP(x)	(x / 8)
-#define MAXP2BYTES(x)	(x * 8)
-
-/* until it's enabled, this UDC should be completely invisible
- * to any USB host.
- */
-static void udc_enable(struct lh7a40x_udc *dev)
-{
-	int ep;
-
-	DEBUG("%s, %p\n", __func__, dev);
-
-	dev->gadget.speed = USB_SPEED_UNKNOWN;
-
-#ifdef CONFIG_ARCH_LH7A404
-	/* Set Port C bit 1 & 2 as output */
-	set_portc_ddr(1, 1);
-	set_portc_ddr(2, 1);
-
-	/* Enable USB power */
-	set_portc_dr(1, 0);
-#endif
-
-	/*
-	 * C.f Chapter 18.1.3.1 Initializing the USB
-	 */
-
-	/* Disable the USB */
-	usb_clear(PM_USB_ENABLE, USB_PM);
-
-	/* Reset APB & I/O sides of the USB */
-	usb_set(USB_RESET_APB | USB_RESET_IO, USB_RESET);
-	mdelay(5);
-	usb_clear(USB_RESET_APB | USB_RESET_IO, USB_RESET);
-
-	/* Set MAXP values for each */
-	for (ep = 0; ep < UDC_MAX_ENDPOINTS; ep++) {
-		struct lh7a40x_ep *ep_reg = &dev->ep[ep];
-		u32 csr;
-
-		usb_set_index(ep);
-
-		switch (ep_reg->ep_type) {
-		case ep_bulk_in:
-		case ep_interrupt:
-			usb_clear(USB_IN_CSR2_USB_DMA_EN | USB_IN_CSR2_AUTO_SET,
-				  ep_reg->csr2);
-			/* Fall through */
-		case ep_control:
-			usb_write(BYTES2MAXP(ep_maxpacket(ep_reg)),
-				  USB_IN_MAXP);
-			break;
-		case ep_bulk_out:
-			usb_clear(USB_OUT_CSR2_USB_DMA_EN |
-				  USB_OUT_CSR2_AUTO_CLR, ep_reg->csr2);
-			usb_write(BYTES2MAXP(ep_maxpacket(ep_reg)),
-				  USB_OUT_MAXP);
-			break;
-		}
-
-		/* Read & Write CSR1, just in case */
-		csr = usb_read(ep_reg->csr1);
-		usb_write(csr, ep_reg->csr1);
-
-		flush(ep_reg);
-	}
-
-	/* Disable interrupts */
-	usb_write(0, USB_IN_INT_EN);
-	usb_write(0, USB_OUT_INT_EN);
-	usb_write(0, USB_INT_EN);
-
-	/* Enable interrupts */
-	usb_set(USB_IN_INT_EP0, USB_IN_INT_EN);
-	usb_set(USB_INT_RESET_INT | USB_INT_RESUME_INT, USB_INT_EN);
-	/* Dont enable rest of the interrupts */
-	/* usb_set(USB_IN_INT_EP3 | USB_IN_INT_EP1 | USB_IN_INT_EP0, USB_IN_INT_EN);
-	   usb_set(USB_OUT_INT_EP2, USB_OUT_INT_EN); */
-
-	/* Enable SUSPEND */
-	usb_set(PM_ENABLE_SUSPEND, USB_PM);
-
-	/* Enable the USB */
-	usb_set(PM_USB_ENABLE, USB_PM);
-
-#ifdef CONFIG_ARCH_LH7A404
-	/* NOTE: DOES NOT WORK! */
-	/* Let host detect UDC:
-	 * Software must write a 0 to the PMR:DCP_CTRL bit to turn this
-	 * transistor on and pull the USBDP pin HIGH.
-	 */
-	/* usb_clear(PM_USB_DCP, USB_PM);
-	   usb_set(PM_USB_DCP, USB_PM); */
-#endif
-}
-
-/*
-  Register entry point for the peripheral controller driver.
-*/
-int usb_gadget_probe_driver(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
-{
-	struct lh7a40x_udc *dev = the_controller;
-	int retval;
-
-	DEBUG("%s: %s\n", __func__, driver->driver.name);
-
-	if (!driver
-			|| driver->speed != USB_SPEED_FULL
-			|| !bind
-			|| !driver->disconnect
-			|| !driver->setup)
-		return -EINVAL;
-	if (!dev)
-		return -ENODEV;
-	if (dev->driver)
-		return -EBUSY;
-
-	/* first hook up the driver ... */
-	dev->driver = driver;
-	dev->gadget.dev.driver = &driver->driver;
-
-	device_add(&dev->gadget.dev);
-	retval = bind(&dev->gadget);
-	if (retval) {
-		printk(KERN_WARNING "%s: bind to driver %s --> error %d\n",
-		       dev->gadget.name, driver->driver.name, retval);
-		device_del(&dev->gadget.dev);
-
-		dev->driver = 0;
-		dev->gadget.dev.driver = 0;
-		return retval;
-	}
-
-	/* ... then enable host detection and ep0; and we're ready
-	 * for set_configuration as well as eventual disconnect.
-	 * NOTE:  this shouldn't power up until later.
-	 */
-	printk(KERN_WARNING "%s: registered gadget driver '%s'\n",
-	       dev->gadget.name, driver->driver.name);
-
-	udc_enable(dev);
-
-	return 0;
-}
-EXPORT_SYMBOL(usb_gadget_probe_driver);
-
-/*
-  Unregister entry point for the peripheral controller driver.
-*/
-int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
-{
-	struct lh7a40x_udc *dev = the_controller;
-	unsigned long flags;
-
-	if (!dev)
-		return -ENODEV;
-	if (!driver || driver != dev->driver || !driver->unbind)
-		return -EINVAL;
-
-	spin_lock_irqsave(&dev->lock, flags);
-	dev->driver = 0;
-	stop_activity(dev, driver);
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	driver->unbind(&dev->gadget);
-	dev->gadget.dev.driver = NULL;
-	device_del(&dev->gadget.dev);
-
-	udc_disable(dev);
-
-	DEBUG("unregistered gadget driver '%s'\n", driver->driver.name);
-	return 0;
-}
-
-EXPORT_SYMBOL(usb_gadget_unregister_driver);
-
-/*-------------------------------------------------------------------------*/
-
-/** Write request to FIFO (max write == maxp size)
- *  Return:  0 = still running, 1 = completed, negative = errno
- *  NOTE: INDEX register must be set for EP
- */
-static int write_fifo(struct lh7a40x_ep *ep, struct lh7a40x_request *req)
-{
-	u32 max;
-	u32 csr;
-
-	max = le16_to_cpu(ep->desc->wMaxPacketSize);
-
-	csr = usb_read(ep->csr1);
-	DEBUG("CSR: %x %d\n", csr, csr & USB_IN_CSR1_FIFO_NOT_EMPTY);
-
-	if (!(csr & USB_IN_CSR1_FIFO_NOT_EMPTY)) {
-		unsigned count;
-		int is_last, is_short;
-
-		count = write_packet(ep, req, max);
-		usb_set(USB_IN_CSR1_IN_PKT_RDY, ep->csr1);
-
-		/* last packet is usually short (or a zlp) */
-		if (unlikely(count != max))
-			is_last = is_short = 1;
-		else {
-			if (likely(req->req.length != req->req.actual)
-			    || req->req.zero)
-				is_last = 0;
-			else
-				is_last = 1;
-			/* interrupt/iso maxpacket may not fill the fifo */
-			is_short = unlikely(max < ep_maxpacket(ep));
-		}
-
-		DEBUG("%s: wrote %s %d bytes%s%s %d left %p\n", __func__,
-		      ep->ep.name, count,
-		      is_last ? "/L" : "", is_short ? "/S" : "",
-		      req->req.length - req->req.actual, req);
-
-		/* requests complete when all IN data is in the FIFO */
-		if (is_last) {
-			done(ep, req, 0);
-			if (list_empty(&ep->queue)) {
-				pio_irq_disable(ep_index(ep));
-			}
-			return 1;
-		}
-	} else {
-		DEBUG("Hmm.. %d ep FIFO is not empty!\n", ep_index(ep));
-	}
-
-	return 0;
-}
-
-/** Read to request from FIFO (max read == bytes in fifo)
- *  Return:  0 = still running, 1 = completed, negative = errno
- *  NOTE: INDEX register must be set for EP
- */
-static int read_fifo(struct lh7a40x_ep *ep, struct lh7a40x_request *req)
-{
-	u32 csr;
-	u8 *buf;
-	unsigned bufferspace, count, is_short;
-	volatile u32 *fifo = (volatile u32 *)ep->fifo;
-
-	/* make sure there's a packet in the FIFO. */
-	csr = usb_read(ep->csr1);
-	if (!(csr & USB_OUT_CSR1_OUT_PKT_RDY)) {
-		DEBUG("%s: Packet NOT ready!\n", __func__);
-		return -EINVAL;
-	}
-
-	buf = req->req.buf + req->req.actual;
-	prefetchw(buf);
-	bufferspace = req->req.length - req->req.actual;
-
-	/* read all bytes from this packet */
-	count = usb_read(USB_OUT_FIFO_WC1);
-	req->req.actual += min(count, bufferspace);
-
-	is_short = (count < ep->ep.maxpacket);
-	DEBUG("read %s %02x, %d bytes%s req %p %d/%d\n",
-	      ep->ep.name, csr, count,
-	      is_short ? "/S" : "", req, req->req.actual, req->req.length);
-
-	while (likely(count-- != 0)) {
-		u8 byte = (u8) (*fifo & 0xff);
-
-		if (unlikely(bufferspace == 0)) {
-			/* this happens when the driver's buffer
-			 * is smaller than what the host sent.
-			 * discard the extra data.
-			 */
-			if (req->req.status != -EOVERFLOW)
-				printk(KERN_WARNING "%s overflow %d\n",
-				       ep->ep.name, count);
-			req->req.status = -EOVERFLOW;
-		} else {
-			*buf++ = byte;
-			bufferspace--;
-		}
-	}
-
-	usb_clear(USB_OUT_CSR1_OUT_PKT_RDY, ep->csr1);
-
-	/* completion */
-	if (is_short || req->req.actual == req->req.length) {
-		done(ep, req, 0);
-		usb_set(USB_OUT_CSR1_FIFO_FLUSH, ep->csr1);
-
-		if (list_empty(&ep->queue))
-			pio_irq_disable(ep_index(ep));
-		return 1;
-	}
-
-	/* finished that packet.  the next one may be waiting... */
-	return 0;
-}
-
-/*
- *	done - retire a request; caller blocked irqs
- *  INDEX register is preserved to keep same
- */
-static void done(struct lh7a40x_ep *ep, struct lh7a40x_request *req, int status)
-{
-	unsigned int stopped = ep->stopped;
-	u32 index;
-
-	DEBUG("%s, %p\n", __func__, ep);
-	list_del_init(&req->queue);
-
-	if (likely(req->req.status == -EINPROGRESS))
-		req->req.status = status;
-	else
-		status = req->req.status;
-
-	if (status && status != -ESHUTDOWN)
-		DEBUG("complete %s req %p stat %d len %u/%u\n",
-		      ep->ep.name, &req->req, status,
-		      req->req.actual, req->req.length);
-
-	/* don't modify queue heads during completion callback */
-	ep->stopped = 1;
-	/* Read current index (completion may modify it) */
-	index = usb_read(USB_INDEX);
-
-	spin_unlock(&ep->dev->lock);
-	req->req.complete(&ep->ep, &req->req);
-	spin_lock(&ep->dev->lock);
-
-	/* Restore index */
-	usb_set_index(index);
-	ep->stopped = stopped;
-}
-
-/** Enable EP interrupt */
-static void pio_irq_enable(int ep)
-{
-	DEBUG("%s: %d\n", __func__, ep);
-
-	switch (ep) {
-	case 1:
-		usb_set(USB_IN_INT_EP1, USB_IN_INT_EN);
-		break;
-	case 2:
-		usb_set(USB_OUT_INT_EP2, USB_OUT_INT_EN);
-		break;
-	case 3:
-		usb_set(USB_IN_INT_EP3, USB_IN_INT_EN);
-		break;
-	default:
-		DEBUG("Unknown endpoint: %d\n", ep);
-		break;
-	}
-}
-
-/** Disable EP interrupt */
-static void pio_irq_disable(int ep)
-{
-	DEBUG("%s: %d\n", __func__, ep);
-
-	switch (ep) {
-	case 1:
-		usb_clear(USB_IN_INT_EP1, USB_IN_INT_EN);
-		break;
-	case 2:
-		usb_clear(USB_OUT_INT_EP2, USB_OUT_INT_EN);
-		break;
-	case 3:
-		usb_clear(USB_IN_INT_EP3, USB_IN_INT_EN);
-		break;
-	default:
-		DEBUG("Unknown endpoint: %d\n", ep);
-		break;
-	}
-}
-
-/*
- * 	nuke - dequeue ALL requests
- */
-void nuke(struct lh7a40x_ep *ep, int status)
-{
-	struct lh7a40x_request *req;
-
-	DEBUG("%s, %p\n", __func__, ep);
-
-	/* Flush FIFO */
-	flush(ep);
-
-	/* called with irqs blocked */
-	while (!list_empty(&ep->queue)) {
-		req = list_entry(ep->queue.next, struct lh7a40x_request, queue);
-		done(ep, req, status);
-	}
-
-	/* Disable IRQ if EP is enabled (has descriptor) */
-	if (ep->desc)
-		pio_irq_disable(ep_index(ep));
-}
-
-/*
-void nuke_all(struct lh7a40x_udc *dev)
-{
-	int n;
-	for(n=0; n<UDC_MAX_ENDPOINTS; n++) {
-		struct lh7a40x_ep *ep = &dev->ep[n];
-		usb_set_index(n);
-		nuke(ep, 0);
-	}
-}*/
-
-/*
-static void flush_all(struct lh7a40x_udc *dev)
-{
-	int n;
-    for (n = 0; n < UDC_MAX_ENDPOINTS; n++)
-    {
-		struct lh7a40x_ep *ep = &dev->ep[n];
-		flush(ep);
-    }
-}
-*/
-
-/** Flush EP
- * NOTE: INDEX register must be set before this call
- */
-static void flush(struct lh7a40x_ep *ep)
-{
-	DEBUG("%s, %p\n", __func__, ep);
-
-	switch (ep->ep_type) {
-	case ep_control:
-		/* check, by implication c.f. 15.1.2.11 */
-		break;
-
-	case ep_bulk_in:
-	case ep_interrupt:
-		/* if(csr & USB_IN_CSR1_IN_PKT_RDY) */
-		usb_set(USB_IN_CSR1_FIFO_FLUSH, ep->csr1);
-		break;
-
-	case ep_bulk_out:
-		/* if(csr & USB_OUT_CSR1_OUT_PKT_RDY) */
-		usb_set(USB_OUT_CSR1_FIFO_FLUSH, ep->csr1);
-		break;
-	}
-}
-
-/**
- * lh7a40x_in_epn - handle IN interrupt
- */
-static void lh7a40x_in_epn(struct lh7a40x_udc *dev, u32 ep_idx, u32 intr)
-{
-	u32 csr;
-	struct lh7a40x_ep *ep = &dev->ep[ep_idx];
-	struct lh7a40x_request *req;
-
-	usb_set_index(ep_idx);
-
-	csr = usb_read(ep->csr1);
-	DEBUG("%s: %d, csr %x\n", __func__, ep_idx, csr);
-
-	if (csr & USB_IN_CSR1_SENT_STALL) {
-		DEBUG("USB_IN_CSR1_SENT_STALL\n");
-		usb_set(USB_IN_CSR1_SENT_STALL /*|USB_IN_CSR1_SEND_STALL */ ,
-			ep->csr1);
-		return;
-	}
-
-	if (!ep->desc) {
-		DEBUG("%s: NO EP DESC\n", __func__);
-		return;
-	}
-
-	if (list_empty(&ep->queue))
-		req = 0;
-	else
-		req = list_entry(ep->queue.next, struct lh7a40x_request, queue);
-
-	DEBUG("req: %p\n", req);
-
-	if (!req)
-		return;
-
-	write_fifo(ep, req);
-}
-
-/* ********************************************************************************************* */
-/* Bulk OUT (recv)
- */
-
-static void lh7a40x_out_epn(struct lh7a40x_udc *dev, u32 ep_idx, u32 intr)
-{
-	struct lh7a40x_ep *ep = &dev->ep[ep_idx];
-	struct lh7a40x_request *req;
-
-	DEBUG("%s: %d\n", __func__, ep_idx);
-
-	usb_set_index(ep_idx);
-
-	if (ep->desc) {
-		u32 csr;
-		csr = usb_read(ep->csr1);
-
-		while ((csr =
-			usb_read(ep->
-				 csr1)) & (USB_OUT_CSR1_OUT_PKT_RDY |
-					   USB_OUT_CSR1_SENT_STALL)) {
-			DEBUG("%s: %x\n", __func__, csr);
-
-			if (csr & USB_OUT_CSR1_SENT_STALL) {
-				DEBUG("%s: stall sent, flush fifo\n",
-				      __func__);
-				/* usb_set(USB_OUT_CSR1_FIFO_FLUSH, ep->csr1); */
-				flush(ep);
-			} else if (csr & USB_OUT_CSR1_OUT_PKT_RDY) {
-				if (list_empty(&ep->queue))
-					req = 0;
-				else
-					req =
-					    list_entry(ep->queue.next,
-						       struct lh7a40x_request,
-						       queue);
-
-				if (!req) {
-					printk(KERN_WARNING
-					       "%s: NULL REQ %d\n",
-					       __func__, ep_idx);
-					flush(ep);
-					break;
-				} else {
-					read_fifo(ep, req);
-				}
-			}
-
-		}
-
-	} else {
-		/* Throw packet away.. */
-		printk(KERN_WARNING "%s: No descriptor?!?\n", __func__);
-		flush(ep);
-	}
-}
-
-static void stop_activity(struct lh7a40x_udc *dev,
-			  struct usb_gadget_driver *driver)
-{
-	int i;
-
-	/* don't disconnect drivers more than once */
-	if (dev->gadget.speed == USB_SPEED_UNKNOWN)
-		driver = 0;
-	dev->gadget.speed = USB_SPEED_UNKNOWN;
-
-	/* prevent new request submissions, kill any outstanding requests  */
-	for (i = 0; i < UDC_MAX_ENDPOINTS; i++) {
-		struct lh7a40x_ep *ep = &dev->ep[i];
-		ep->stopped = 1;
-
-		usb_set_index(i);
-		nuke(ep, -ESHUTDOWN);
-	}
-
-	/* report disconnect; the driver is already quiesced */
-	if (driver) {
-		spin_unlock(&dev->lock);
-		driver->disconnect(&dev->gadget);
-		spin_lock(&dev->lock);
-	}
-
-	/* re-init driver-visible data structures */
-	udc_reinit(dev);
-}
-
-/** Handle USB RESET interrupt
- */
-static void lh7a40x_reset_intr(struct lh7a40x_udc *dev)
-{
-#if 0				/* def CONFIG_ARCH_LH7A404 */
-	/* Does not work always... */
-
-	DEBUG("%s: %d\n", __func__, dev->usb_address);
-
-	if (!dev->usb_address) {
-		/*usb_set(USB_RESET_IO, USB_RESET);
-		   mdelay(5);
-		   usb_clear(USB_RESET_IO, USB_RESET); */
-		return;
-	}
-	/* Put the USB controller into reset. */
-	usb_set(USB_RESET_IO, USB_RESET);
-
-	/* Set Device ID to 0 */
-	udc_set_address(dev, 0);
-
-	/* Let PLL2 settle down */
-	mdelay(5);
-
-	/* Release the USB controller from reset */
-	usb_clear(USB_RESET_IO, USB_RESET);
-
-	/* Re-enable UDC */
-	udc_enable(dev);
-
-#endif
-	dev->gadget.speed = USB_SPEED_FULL;
-}
-
-/*
- *	lh7a40x usb client interrupt handler.
- */
-static irqreturn_t lh7a40x_udc_irq(int irq, void *_dev)
-{
-	struct lh7a40x_udc *dev = _dev;
-
-	DEBUG("\n\n");
-
-	spin_lock(&dev->lock);
-
-	for (;;) {
-		u32 intr_in = usb_read(USB_IN_INT);
-		u32 intr_out = usb_read(USB_OUT_INT);
-		u32 intr_int = usb_read(USB_INT);
-
-		/* Test also against enable bits.. (lh7a40x errata).. Sigh.. */
-		u32 in_en = usb_read(USB_IN_INT_EN);
-		u32 out_en = usb_read(USB_OUT_INT_EN);
-
-		if (!intr_out && !intr_in && !intr_int)
-			break;
-
-		DEBUG("%s (on state %s)\n", __func__,
-		      state_names[dev->ep0state]);
-		DEBUG("intr_out = %x\n", intr_out);
-		DEBUG("intr_in  = %x\n", intr_in);
-		DEBUG("intr_int = %x\n", intr_int);
-
-		if (intr_in) {
-			usb_write(intr_in, USB_IN_INT);
-
-			if ((intr_in & USB_IN_INT_EP1)
-			    && (in_en & USB_IN_INT_EP1)) {
-				DEBUG("USB_IN_INT_EP1\n");
-				lh7a40x_in_epn(dev, 1, intr_in);
-			}
-			if ((intr_in & USB_IN_INT_EP3)
-			    && (in_en & USB_IN_INT_EP3)) {
-				DEBUG("USB_IN_INT_EP3\n");
-				lh7a40x_in_epn(dev, 3, intr_in);
-			}
-			if (intr_in & USB_IN_INT_EP0) {
-				DEBUG("USB_IN_INT_EP0 (control)\n");
-				lh7a40x_handle_ep0(dev, intr_in);
-			}
-		}
-
-		if (intr_out) {
-			usb_write(intr_out, USB_OUT_INT);
-
-			if ((intr_out & USB_OUT_INT_EP2)
-			    && (out_en & USB_OUT_INT_EP2)) {
-				DEBUG("USB_OUT_INT_EP2\n");
-				lh7a40x_out_epn(dev, 2, intr_out);
-			}
-		}
-
-		if (intr_int) {
-			usb_write(intr_int, USB_INT);
-
-			if (intr_int & USB_INT_RESET_INT) {
-				lh7a40x_reset_intr(dev);
-			}
-
-			if (intr_int & USB_INT_RESUME_INT) {
-				DEBUG("USB resume\n");
-
-				if (dev->gadget.speed != USB_SPEED_UNKNOWN
-				    && dev->driver
-				    && dev->driver->resume
-				    && is_usb_connected()) {
-					dev->driver->resume(&dev->gadget);
-				}
-			}
-
-			if (intr_int & USB_INT_SUSPEND_INT) {
-				DEBUG("USB suspend%s\n",
-				      is_usb_connected()? "" : "+disconnect");
-				if (!is_usb_connected()) {
-					stop_activity(dev, dev->driver);
-				} else if (dev->gadget.speed !=
-					   USB_SPEED_UNKNOWN && dev->driver
-					   && dev->driver->suspend) {
-					dev->driver->suspend(&dev->gadget);
-				}
-			}
-
-		}
-	}
-
-	spin_unlock(&dev->lock);
-
-	return IRQ_HANDLED;
-}
-
-static int lh7a40x_ep_enable(struct usb_ep *_ep,
-			     const struct usb_endpoint_descriptor *desc)
-{
-	struct lh7a40x_ep *ep;
-	struct lh7a40x_udc *dev;
-	unsigned long flags;
-
-	DEBUG("%s, %p\n", __func__, _ep);
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (!_ep || !desc || ep->desc || _ep->name == ep0name
-	    || desc->bDescriptorType != USB_DT_ENDPOINT
-	    || ep->bEndpointAddress != desc->bEndpointAddress
-	    || ep_maxpacket(ep) < le16_to_cpu(desc->wMaxPacketSize)) {
-		DEBUG("%s, bad ep or descriptor\n", __func__);
-		return -EINVAL;
-	}
-
-	/* xfer types must match, except that interrupt ~= bulk */
-	if (ep->bmAttributes != desc->bmAttributes
-	    && ep->bmAttributes != USB_ENDPOINT_XFER_BULK
-	    && desc->bmAttributes != USB_ENDPOINT_XFER_INT) {
-		DEBUG("%s, %s type mismatch\n", __func__, _ep->name);
-		return -EINVAL;
-	}
-
-	/* hardware _could_ do smaller, but driver doesn't */
-	if ((desc->bmAttributes == USB_ENDPOINT_XFER_BULK
-	     && le16_to_cpu(desc->wMaxPacketSize) != ep_maxpacket(ep))
-	    || !desc->wMaxPacketSize) {
-		DEBUG("%s, bad %s maxpacket\n", __func__, _ep->name);
-		return -ERANGE;
-	}
-
-	dev = ep->dev;
-	if (!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN) {
-		DEBUG("%s, bogus device state\n", __func__);
-		return -ESHUTDOWN;
-	}
-
-	spin_lock_irqsave(&ep->dev->lock, flags);
-
-	ep->stopped = 0;
-	ep->desc = desc;
-	ep->pio_irqs = 0;
-	ep->ep.maxpacket = le16_to_cpu(desc->wMaxPacketSize);
-
-	spin_unlock_irqrestore(&ep->dev->lock, flags);
-
-	/* Reset halt state (does flush) */
-	lh7a40x_set_halt(_ep, 0);
-
-	DEBUG("%s: enabled %s\n", __func__, _ep->name);
-	return 0;
-}
-
-/** Disable EP
- *  NOTE: Sets INDEX register
- */
-static int lh7a40x_ep_disable(struct usb_ep *_ep)
-{
-	struct lh7a40x_ep *ep;
-	unsigned long flags;
-
-	DEBUG("%s, %p\n", __func__, _ep);
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (!_ep || !ep->desc) {
-		DEBUG("%s, %s not enabled\n", __func__,
-		      _ep ? ep->ep.name : NULL);
-		return -EINVAL;
-	}
-
-	spin_lock_irqsave(&ep->dev->lock, flags);
-
-	usb_set_index(ep_index(ep));
-
-	/* Nuke all pending requests (does flush) */
-	nuke(ep, -ESHUTDOWN);
-
-	/* Disable ep IRQ */
-	pio_irq_disable(ep_index(ep));
-
-	ep->desc = 0;
-	ep->stopped = 1;
-
-	spin_unlock_irqrestore(&ep->dev->lock, flags);
-
-	DEBUG("%s: disabled %s\n", __func__, _ep->name);
-	return 0;
-}
-
-static struct usb_request *lh7a40x_alloc_request(struct usb_ep *ep,
-						 gfp_t gfp_flags)
-{
-	struct lh7a40x_request *req;
-
-	DEBUG("%s, %p\n", __func__, ep);
-
-	req = kzalloc(sizeof(*req), gfp_flags);
-	if (!req)
-		return 0;
-
-	INIT_LIST_HEAD(&req->queue);
-
-	return &req->req;
-}
-
-static void lh7a40x_free_request(struct usb_ep *ep, struct usb_request *_req)
-{
-	struct lh7a40x_request *req;
-
-	DEBUG("%s, %p\n", __func__, ep);
-
-	req = container_of(_req, struct lh7a40x_request, req);
-	WARN_ON(!list_empty(&req->queue));
-	kfree(req);
-}
-
-/** Queue one request
- *  Kickstart transfer if needed
- *  NOTE: Sets INDEX register
- */
-static int lh7a40x_queue(struct usb_ep *_ep, struct usb_request *_req,
-			 gfp_t gfp_flags)
-{
-	struct lh7a40x_request *req;
-	struct lh7a40x_ep *ep;
-	struct lh7a40x_udc *dev;
-	unsigned long flags;
-
-	DEBUG("\n\n\n%s, %p\n", __func__, _ep);
-
-	req = container_of(_req, struct lh7a40x_request, req);
-	if (unlikely
-	    (!_req || !_req->complete || !_req->buf
-	     || !list_empty(&req->queue))) {
-		DEBUG("%s, bad params\n", __func__);
-		return -EINVAL;
-	}
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (unlikely(!_ep || (!ep->desc && ep->ep.name != ep0name))) {
-		DEBUG("%s, bad ep\n", __func__);
-		return -EINVAL;
-	}
-
-	dev = ep->dev;
-	if (unlikely(!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN)) {
-		DEBUG("%s, bogus device state %p\n", __func__, dev->driver);
-		return -ESHUTDOWN;
-	}
-
-	DEBUG("%s queue req %p, len %d buf %p\n", _ep->name, _req, _req->length,
-	      _req->buf);
-
-	spin_lock_irqsave(&dev->lock, flags);
-
-	_req->status = -EINPROGRESS;
-	_req->actual = 0;
-
-	/* kickstart this i/o queue? */
-	DEBUG("Add to %d Q %d %d\n", ep_index(ep), list_empty(&ep->queue),
-	      ep->stopped);
-	if (list_empty(&ep->queue) && likely(!ep->stopped)) {
-		u32 csr;
-
-		if (unlikely(ep_index(ep) == 0)) {
-			/* EP0 */
-			list_add_tail(&req->queue, &ep->queue);
-			lh7a40x_ep0_kick(dev, ep);
-			req = 0;
-		} else if (ep_is_in(ep)) {
-			/* EP1 & EP3 */
-			usb_set_index(ep_index(ep));
-			csr = usb_read(ep->csr1);
-			pio_irq_enable(ep_index(ep));
-			if ((csr & USB_IN_CSR1_FIFO_NOT_EMPTY) == 0) {
-				if (write_fifo(ep, req) == 1)
-					req = 0;
-			}
-		} else {
-			/* EP2 */
-			usb_set_index(ep_index(ep));
-			csr = usb_read(ep->csr1);
-			pio_irq_enable(ep_index(ep));
-			if (!(csr & USB_OUT_CSR1_FIFO_FULL)) {
-				if (read_fifo(ep, req) == 1)
-					req = 0;
-			}
-		}
-	}
-
-	/* pio or dma irq handler advances the queue. */
-	if (likely(req != 0))
-		list_add_tail(&req->queue, &ep->queue);
-
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	return 0;
-}
-
-/* dequeue JUST ONE request */
-static int lh7a40x_dequeue(struct usb_ep *_ep, struct usb_request *_req)
-{
-	struct lh7a40x_ep *ep;
-	struct lh7a40x_request *req;
-	unsigned long flags;
-
-	DEBUG("%s, %p\n", __func__, _ep);
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (!_ep || ep->ep.name == ep0name)
-		return -EINVAL;
-
-	spin_lock_irqsave(&ep->dev->lock, flags);
-
-	/* make sure it's actually queued on this endpoint */
-	list_for_each_entry(req, &ep->queue, queue) {
-		if (&req->req == _req)
-			break;
-	}
-	if (&req->req != _req) {
-		spin_unlock_irqrestore(&ep->dev->lock, flags);
-		return -EINVAL;
-	}
-
-	done(ep, req, -ECONNRESET);
-
-	spin_unlock_irqrestore(&ep->dev->lock, flags);
-	return 0;
-}
-
-/** Halt specific EP
- *  Return 0 if success
- *  NOTE: Sets INDEX register to EP !
- */
-static int lh7a40x_set_halt(struct usb_ep *_ep, int value)
-{
-	struct lh7a40x_ep *ep;
-	unsigned long flags;
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (unlikely(!_ep || (!ep->desc && ep->ep.name != ep0name))) {
-		DEBUG("%s, bad ep\n", __func__);
-		return -EINVAL;
-	}
-
-	usb_set_index(ep_index(ep));
-
-	DEBUG("%s, ep %d, val %d\n", __func__, ep_index(ep), value);
-
-	spin_lock_irqsave(&ep->dev->lock, flags);
-
-	if (ep_index(ep) == 0) {
-		/* EP0 */
-		usb_set(EP0_SEND_STALL, ep->csr1);
-	} else if (ep_is_in(ep)) {
-		u32 csr = usb_read(ep->csr1);
-		if (value && ((csr & USB_IN_CSR1_FIFO_NOT_EMPTY)
-			      || !list_empty(&ep->queue))) {
-			/*
-			 * Attempts to halt IN endpoints will fail (returning -EAGAIN)
-			 * if any transfer requests are still queued, or if the controller
-			 * FIFO still holds bytes that the host hasn't collected.
-			 */
-			spin_unlock_irqrestore(&ep->dev->lock, flags);
-			DEBUG
-			    ("Attempt to halt IN endpoint failed (returning -EAGAIN) %d %d\n",
-			     (csr & USB_IN_CSR1_FIFO_NOT_EMPTY),
-			     !list_empty(&ep->queue));
-			return -EAGAIN;
-		}
-		flush(ep);
-		if (value)
-			usb_set(USB_IN_CSR1_SEND_STALL, ep->csr1);
-		else {
-			usb_clear(USB_IN_CSR1_SEND_STALL, ep->csr1);
-			usb_set(USB_IN_CSR1_CLR_DATA_TOGGLE, ep->csr1);
-		}
-
-	} else {
-
-		flush(ep);
-		if (value)
-			usb_set(USB_OUT_CSR1_SEND_STALL, ep->csr1);
-		else {
-			usb_clear(USB_OUT_CSR1_SEND_STALL, ep->csr1);
-			usb_set(USB_OUT_CSR1_CLR_DATA_REG, ep->csr1);
-		}
-	}
-
-	if (value) {
-		ep->stopped = 1;
-	} else {
-		ep->stopped = 0;
-	}
-
-	spin_unlock_irqrestore(&ep->dev->lock, flags);
-
-	DEBUG("%s %s halted\n", _ep->name, value == 0 ? "NOT" : "IS");
-
-	return 0;
-}
-
-/** Return bytes in EP FIFO
- *  NOTE: Sets INDEX register to EP
- */
-static int lh7a40x_fifo_status(struct usb_ep *_ep)
-{
-	u32 csr;
-	int count = 0;
-	struct lh7a40x_ep *ep;
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (!_ep) {
-		DEBUG("%s, bad ep\n", __func__);
-		return -ENODEV;
-	}
-
-	DEBUG("%s, %d\n", __func__, ep_index(ep));
-
-	/* LPD can't report unclaimed bytes from IN fifos */
-	if (ep_is_in(ep))
-		return -EOPNOTSUPP;
-
-	usb_set_index(ep_index(ep));
-
-	csr = usb_read(ep->csr1);
-	if (ep->dev->gadget.speed != USB_SPEED_UNKNOWN ||
-	    csr & USB_OUT_CSR1_OUT_PKT_RDY) {
-		count = usb_read(USB_OUT_FIFO_WC1);
-	}
-
-	return count;
-}
-
-/** Flush EP FIFO
- *  NOTE: Sets INDEX register to EP
- */
-static void lh7a40x_fifo_flush(struct usb_ep *_ep)
-{
-	struct lh7a40x_ep *ep;
-
-	ep = container_of(_ep, struct lh7a40x_ep, ep);
-	if (unlikely(!_ep || (!ep->desc && ep->ep.name != ep0name))) {
-		DEBUG("%s, bad ep\n", __func__);
-		return;
-	}
-
-	usb_set_index(ep_index(ep));
-	flush(ep);
-}
-
-/****************************************************************/
-/* End Point 0 related functions                                */
-/****************************************************************/
-
-/* return:  0 = still running, 1 = completed, negative = errno */
-static int write_fifo_ep0(struct lh7a40x_ep *ep, struct lh7a40x_request *req)
-{
-	u32 max;
-	unsigned count;
-	int is_last;
-
-	max = ep_maxpacket(ep);
-
-	DEBUG_EP0("%s\n", __func__);
-
-	count = write_packet(ep, req, max);
-
-	/* last packet is usually short (or a zlp) */
-	if (unlikely(count != max))
-		is_last = 1;
-	else {
-		if (likely(req->req.length != req->req.actual) || req->req.zero)
-			is_last = 0;
-		else
-			is_last = 1;
-	}
-
-	DEBUG_EP0("%s: wrote %s %d bytes%s %d left %p\n", __func__,
-		  ep->ep.name, count,
-		  is_last ? "/L" : "", req->req.length - req->req.actual, req);
-
-	/* requests complete when all IN data is in the FIFO */
-	if (is_last) {
-		done(ep, req, 0);
-		return 1;
-	}
-
-	return 0;
-}
-
-static __inline__ int lh7a40x_fifo_read(struct lh7a40x_ep *ep,
-					unsigned char *cp, int max)
-{
-	int bytes;
-	int count = usb_read(USB_OUT_FIFO_WC1);
-	volatile u32 *fifo = (volatile u32 *)ep->fifo;
-
-	if (count > max)
-		count = max;
-	bytes = count;
-	while (count--)
-		*cp++ = *fifo & 0xFF;
-	return bytes;
-}
-
-static __inline__ void lh7a40x_fifo_write(struct lh7a40x_ep *ep,
-					  unsigned char *cp, int count)
-{
-	volatile u32 *fifo = (volatile u32 *)ep->fifo;
-	DEBUG_EP0("fifo_write: %d %d\n", ep_index(ep), count);
-	while (count--)
-		*fifo = *cp++;
-}
-
-static int read_fifo_ep0(struct lh7a40x_ep *ep, struct lh7a40x_request *req)
-{
-	u32 csr;
-	u8 *buf;
-	unsigned bufferspace, count, is_short;
-	volatile u32 *fifo = (volatile u32 *)ep->fifo;
-
-	DEBUG_EP0("%s\n", __func__);
-
-	csr = usb_read(USB_EP0_CSR);
-	if (!(csr & USB_OUT_CSR1_OUT_PKT_RDY))
-		return 0;
-
-	buf = req->req.buf + req->req.actual;
-	prefetchw(buf);
-	bufferspace = req->req.length - req->req.actual;
-
-	/* read all bytes from this packet */
-	if (likely(csr & EP0_OUT_PKT_RDY)) {
-		count = usb_read(USB_OUT_FIFO_WC1);
-		req->req.actual += min(count, bufferspace);
-	} else			/* zlp */
-		count = 0;
-
-	is_short = (count < ep->ep.maxpacket);
-	DEBUG_EP0("read %s %02x, %d bytes%s req %p %d/%d\n",
-		  ep->ep.name, csr, count,
-		  is_short ? "/S" : "", req, req->req.actual, req->req.length);
-
-	while (likely(count-- != 0)) {
-		u8 byte = (u8) (*fifo & 0xff);
-
-		if (unlikely(bufferspace == 0)) {
-			/* this happens when the driver's buffer
-			 * is smaller than what the host sent.
-			 * discard the extra data.
-			 */
-			if (req->req.status != -EOVERFLOW)
-				DEBUG_EP0("%s overflow %d\n", ep->ep.name,
-					  count);
-			req->req.status = -EOVERFLOW;
-		} else {
-			*buf++ = byte;
-			bufferspace--;
-		}
-	}
-
-	/* completion */
-	if (is_short || req->req.actual == req->req.length) {
-		done(ep, req, 0);
-		return 1;
-	}
-
-	/* finished that packet.  the next one may be waiting... */
-	return 0;
-}
-
-/**
- * udc_set_address - set the USB address for this device
- * @address:
- *
- * Called from control endpoint function after it decodes a set address setup packet.
- */
-static void udc_set_address(struct lh7a40x_udc *dev, unsigned char address)
-{
-	DEBUG_EP0("%s: %d\n", __func__, address);
-	/* c.f. 15.1.2.2 Table 15-4 address will be used after DATA_END is set */
-	dev->usb_address = address;
-	usb_set((address & USB_FA_FUNCTION_ADDR), USB_FA);
-	usb_set(USB_FA_ADDR_UPDATE | (address & USB_FA_FUNCTION_ADDR), USB_FA);
-	/* usb_read(USB_FA); */
-}
-
-/*
- * DATA_STATE_RECV (OUT_PKT_RDY)
- *      - if error
- *              set EP0_CLR_OUT | EP0_DATA_END | EP0_SEND_STALL bits
- *      - else
- *              set EP0_CLR_OUT bit
- 				if last set EP0_DATA_END bit
- */
-static void lh7a40x_ep0_out(struct lh7a40x_udc *dev, u32 csr)
-{
-	struct lh7a40x_request *req;
-	struct lh7a40x_ep *ep = &dev->ep[0];
-	int ret;
-
-	DEBUG_EP0("%s: %x\n", __func__, csr);
-
-	if (list_empty(&ep->queue))
-		req = 0;
-	else
-		req = list_entry(ep->queue.next, struct lh7a40x_request, queue);
-
-	if (req) {
-
-		if (req->req.length == 0) {
-			DEBUG_EP0("ZERO LENGTH OUT!\n");
-			usb_set((EP0_CLR_OUT | EP0_DATA_END), USB_EP0_CSR);
-			dev->ep0state = WAIT_FOR_SETUP;
-			return;
-		}
-		ret = read_fifo_ep0(ep, req);
-		if (ret) {
-			/* Done! */
-			DEBUG_EP0("%s: finished, waiting for status\n",
-				  __func__);
-
-			usb_set((EP0_CLR_OUT | EP0_DATA_END), USB_EP0_CSR);
-			dev->ep0state = WAIT_FOR_SETUP;
-		} else {
-			/* Not done yet.. */
-			DEBUG_EP0("%s: not finished\n", __func__);
-			usb_set(EP0_CLR_OUT, USB_EP0_CSR);
-		}
-	} else {
-		DEBUG_EP0("NO REQ??!\n");
-	}
-}
-
-/*
- * DATA_STATE_XMIT
- */
-static int lh7a40x_ep0_in(struct lh7a40x_udc *dev, u32 csr)
-{
-	struct lh7a40x_request *req;
-	struct lh7a40x_ep *ep = &dev->ep[0];
-	int ret, need_zlp = 0;
-
-	DEBUG_EP0("%s: %x\n", __func__, csr);
-
-	if (list_empty(&ep->queue))
-		req = 0;
-	else
-		req = list_entry(ep->queue.next, struct lh7a40x_request, queue);
-
-	if (!req) {
-		DEBUG_EP0("%s: NULL REQ\n", __func__);
-		return 0;
-	}
-
-	if (req->req.length == 0) {
-
-		usb_set((EP0_IN_PKT_RDY | EP0_DATA_END), USB_EP0_CSR);
-		dev->ep0state = WAIT_FOR_SETUP;
-		return 1;
-	}
-
-	if (req->req.length - req->req.actual == EP0_PACKETSIZE) {
-		/* Next write will end with the packet size, */
-		/* so we need Zero-length-packet */
-		need_zlp = 1;
-	}
-
-	ret = write_fifo_ep0(ep, req);
-
-	if (ret == 1 && !need_zlp) {
-		/* Last packet */
-		DEBUG_EP0("%s: finished, waiting for status\n", __func__);
-
-		usb_set((EP0_IN_PKT_RDY | EP0_DATA_END), USB_EP0_CSR);
-		dev->ep0state = WAIT_FOR_SETUP;
-	} else {
-		DEBUG_EP0("%s: not finished\n", __func__);
-		usb_set(EP0_IN_PKT_RDY, USB_EP0_CSR);
-	}
-
-	if (need_zlp) {
-		DEBUG_EP0("%s: Need ZLP!\n", __func__);
-		usb_set(EP0_IN_PKT_RDY, USB_EP0_CSR);
-		dev->ep0state = DATA_STATE_NEED_ZLP;
-	}
-
-	return 1;
-}
-
-static int lh7a40x_handle_get_status(struct lh7a40x_udc *dev,
-				     struct usb_ctrlrequest *ctrl)
-{
-	struct lh7a40x_ep *ep0 = &dev->ep[0];
-	struct lh7a40x_ep *qep;
-	int reqtype = (ctrl->bRequestType & USB_RECIP_MASK);
-	u16 val = 0;
-
-	if (reqtype == USB_RECIP_INTERFACE) {
-		/* This is not supported.
-		 * And according to the USB spec, this one does nothing..
-		 * Just return 0
-		 */
-		DEBUG_SETUP("GET_STATUS: USB_RECIP_INTERFACE\n");
-	} else if (reqtype == USB_RECIP_DEVICE) {
-		DEBUG_SETUP("GET_STATUS: USB_RECIP_DEVICE\n");
-		val |= (1 << 0);	/* Self powered */
-		/*val |= (1<<1); *//* Remote wakeup */
-	} else if (reqtype == USB_RECIP_ENDPOINT) {
-		int ep_num = (ctrl->wIndex & ~USB_DIR_IN);
-
-		DEBUG_SETUP
-		    ("GET_STATUS: USB_RECIP_ENDPOINT (%d), ctrl->wLength = %d\n",
-		     ep_num, ctrl->wLength);
-
-		if (ctrl->wLength > 2 || ep_num > 3)
-			return -EOPNOTSUPP;
-
-		qep = &dev->ep[ep_num];
-		if (ep_is_in(qep) != ((ctrl->wIndex & USB_DIR_IN) ? 1 : 0)
-		    && ep_index(qep) != 0) {
-			return -EOPNOTSUPP;
-		}
-
-		usb_set_index(ep_index(qep));
-
-		/* Return status on next IN token */
-		switch (qep->ep_type) {
-		case ep_control:
-			val =
-			    (usb_read(qep->csr1) & EP0_SEND_STALL) ==
-			    EP0_SEND_STALL;
-			break;
-		case ep_bulk_in:
-		case ep_interrupt:
-			val =
-			    (usb_read(qep->csr1) & USB_IN_CSR1_SEND_STALL) ==
-			    USB_IN_CSR1_SEND_STALL;
-			break;
-		case ep_bulk_out:
-			val =
-			    (usb_read(qep->csr1) & USB_OUT_CSR1_SEND_STALL) ==
-			    USB_OUT_CSR1_SEND_STALL;
-			break;
-		}
-
-		/* Back to EP0 index */
-		usb_set_index(0);
-
-		DEBUG_SETUP("GET_STATUS, ep: %d (%x), val = %d\n", ep_num,
-			    ctrl->wIndex, val);
-	} else {
-		DEBUG_SETUP("Unknown REQ TYPE: %d\n", reqtype);
-		return -EOPNOTSUPP;
-	}
-
-	/* Clear "out packet ready" */
-	usb_set((EP0_CLR_OUT), USB_EP0_CSR);
-	/* Put status to FIFO */
-	lh7a40x_fifo_write(ep0, (u8 *) & val, sizeof(val));
-	/* Issue "In packet ready" */
-	usb_set((EP0_IN_PKT_RDY | EP0_DATA_END), USB_EP0_CSR);
-
-	return 0;
-}
-
-/*
- * WAIT_FOR_SETUP (OUT_PKT_RDY)
- *      - read data packet from EP0 FIFO
- *      - decode command
- *      - if error
- *              set EP0_CLR_OUT | EP0_DATA_END | EP0_SEND_STALL bits
- *      - else
- *              set EP0_CLR_OUT | EP0_DATA_END bits
- */
-static void lh7a40x_ep0_setup(struct lh7a40x_udc *dev, u32 csr)
-{
-	struct lh7a40x_ep *ep = &dev->ep[0];
-	struct usb_ctrlrequest ctrl;
-	int i, bytes, is_in;
-
-	DEBUG_SETUP("%s: %x\n", __func__, csr);
-
-	/* Nuke all previous transfers */
-	nuke(ep, -EPROTO);
-
-	/* read control req from fifo (8 bytes) */
-	bytes = lh7a40x_fifo_read(ep, (unsigned char *)&ctrl, 8);
-
-	DEBUG_SETUP("Read CTRL REQ %d bytes\n", bytes);
-	DEBUG_SETUP("CTRL.bRequestType = %d (is_in %d)\n", ctrl.bRequestType,
-		    ctrl.bRequestType == USB_DIR_IN);
-	DEBUG_SETUP("CTRL.bRequest = %d\n", ctrl.bRequest);
-	DEBUG_SETUP("CTRL.wLength = %d\n", ctrl.wLength);
-	DEBUG_SETUP("CTRL.wValue = %d (%d)\n", ctrl.wValue, ctrl.wValue >> 8);
-	DEBUG_SETUP("CTRL.wIndex = %d\n", ctrl.wIndex);
-
-	/* Set direction of EP0 */
-	if (likely(ctrl.bRequestType & USB_DIR_IN)) {
-		ep->bEndpointAddress |= USB_DIR_IN;
-		is_in = 1;
-	} else {
-		ep->bEndpointAddress &= ~USB_DIR_IN;
-		is_in = 0;
-	}
-
-	dev->req_pending = 1;
-
-	/* Handle some SETUP packets ourselves */
-	switch (ctrl.bRequest) {
-	case USB_REQ_SET_ADDRESS:
-		if (ctrl.bRequestType != (USB_TYPE_STANDARD | USB_RECIP_DEVICE))
-			break;
-
-		DEBUG_SETUP("USB_REQ_SET_ADDRESS (%d)\n", ctrl.wValue);
-		udc_set_address(dev, ctrl.wValue);
-		usb_set((EP0_CLR_OUT | EP0_DATA_END), USB_EP0_CSR);
-		return;
-
-	case USB_REQ_GET_STATUS:{
-			if (lh7a40x_handle_get_status(dev, &ctrl) == 0)
-				return;
-
-	case USB_REQ_CLEAR_FEATURE:
-	case USB_REQ_SET_FEATURE:
-			if (ctrl.bRequestType == USB_RECIP_ENDPOINT) {
-				struct lh7a40x_ep *qep;
-				int ep_num = (ctrl.wIndex & 0x0f);
-
-				/* Support only HALT feature */
-				if (ctrl.wValue != 0 || ctrl.wLength != 0
-				    || ep_num > 3 || ep_num < 1)
-					break;
-
-				qep = &dev->ep[ep_num];
-				spin_unlock(&dev->lock);
-				if (ctrl.bRequest == USB_REQ_SET_FEATURE) {
-					DEBUG_SETUP("SET_FEATURE (%d)\n",
-						    ep_num);
-					lh7a40x_set_halt(&qep->ep, 1);
-				} else {
-					DEBUG_SETUP("CLR_FEATURE (%d)\n",
-						    ep_num);
-					lh7a40x_set_halt(&qep->ep, 0);
-				}
-				spin_lock(&dev->lock);
-				usb_set_index(0);
-
-				/* Reply with a ZLP on next IN token */
-				usb_set((EP0_CLR_OUT | EP0_DATA_END),
-					USB_EP0_CSR);
-				return;
-			}
-			break;
-		}
-
-	default:
-		break;
-	}
-
-	if (likely(dev->driver)) {
-		/* device-2-host (IN) or no data setup command, process immediately */
-		spin_unlock(&dev->lock);
-		i = dev->driver->setup(&dev->gadget, &ctrl);
-		spin_lock(&dev->lock);
-
-		if (i < 0) {
-			/* setup processing failed, force stall */
-			DEBUG_SETUP
-			    ("  --> ERROR: gadget setup FAILED (stalling), setup returned %d\n",
-			     i);
-			usb_set_index(0);
-			usb_set((EP0_CLR_OUT | EP0_DATA_END | EP0_SEND_STALL),
-				USB_EP0_CSR);
-
-			/* ep->stopped = 1; */
-			dev->ep0state = WAIT_FOR_SETUP;
-		}
-	}
-}
-
-/*
- * DATA_STATE_NEED_ZLP
- */
-static void lh7a40x_ep0_in_zlp(struct lh7a40x_udc *dev, u32 csr)
-{
-	DEBUG_EP0("%s: %x\n", __func__, csr);
-
-	/* c.f. Table 15-14 */
-	usb_set((EP0_IN_PKT_RDY | EP0_DATA_END), USB_EP0_CSR);
-	dev->ep0state = WAIT_FOR_SETUP;
-}
-
-/*
- * handle ep0 interrupt
- */
-static void lh7a40x_handle_ep0(struct lh7a40x_udc *dev, u32 intr)
-{
-	struct lh7a40x_ep *ep = &dev->ep[0];
-	u32 csr;
-
-	/* Set index 0 */
-	usb_set_index(0);
- 	csr = usb_read(USB_EP0_CSR);
-
-	DEBUG_EP0("%s: csr = %x\n", __func__, csr);
-
-	/*
-	 * For overview of what we should be doing see c.f. Chapter 18.1.2.4
-	 * We will follow that outline here modified by our own global state
-	 * indication which provides hints as to what we think should be
-	 * happening..
-	 */
-
-	/*
-	 * if SENT_STALL is set
-	 *      - clear the SENT_STALL bit
-	 */
-	if (csr & EP0_SENT_STALL) {
-		DEBUG_EP0("%s: EP0_SENT_STALL is set: %x\n", __func__, csr);
-		usb_clear((EP0_SENT_STALL | EP0_SEND_STALL), USB_EP0_CSR);
-		nuke(ep, -ECONNABORTED);
-		dev->ep0state = WAIT_FOR_SETUP;
-		return;
-	}
-
-	/*
-	 * if a transfer is in progress && IN_PKT_RDY and OUT_PKT_RDY are clear
-	 *      - fill EP0 FIFO
-	 *      - if last packet
-	 *      -       set IN_PKT_RDY | DATA_END
-	 *      - else
-	 *              set IN_PKT_RDY
-	 */
-	if (!(csr & (EP0_IN_PKT_RDY | EP0_OUT_PKT_RDY))) {
-		DEBUG_EP0("%s: IN_PKT_RDY and OUT_PKT_RDY are clear\n",
-			  __func__);
-
-		switch (dev->ep0state) {
-		case DATA_STATE_XMIT:
-			DEBUG_EP0("continue with DATA_STATE_XMIT\n");
-			lh7a40x_ep0_in(dev, csr);
-			return;
-		case DATA_STATE_NEED_ZLP:
-			DEBUG_EP0("continue with DATA_STATE_NEED_ZLP\n");
-			lh7a40x_ep0_in_zlp(dev, csr);
-			return;
-		default:
-			/* Stall? */
-			DEBUG_EP0("Odd state!! state = %s\n",
-				  state_names[dev->ep0state]);
-			dev->ep0state = WAIT_FOR_SETUP;
-			/* nuke(ep, 0); */
-			/* usb_set(EP0_SEND_STALL, ep->csr1); */
-			break;
-		}
-	}
-
-	/*
-	 * if SETUP_END is set
-	 *      - abort the last transfer
-	 *      - set SERVICED_SETUP_END_BIT
-	 */
-	if (csr & EP0_SETUP_END) {
-		DEBUG_EP0("%s: EP0_SETUP_END is set: %x\n", __func__, csr);
-
-		usb_set(EP0_CLR_SETUP_END, USB_EP0_CSR);
-
-		nuke(ep, 0);
-		dev->ep0state = WAIT_FOR_SETUP;
-	}
-
-	/*
-	 * if EP0_OUT_PKT_RDY is set
-	 *      - read data packet from EP0 FIFO
-	 *      - decode command
-	 *      - if error
-	 *              set SERVICED_OUT_PKT_RDY | DATA_END bits | SEND_STALL
-	 *      - else
-	 *              set SERVICED_OUT_PKT_RDY | DATA_END bits
-	 */
-	if (csr & EP0_OUT_PKT_RDY) {
-
-		DEBUG_EP0("%s: EP0_OUT_PKT_RDY is set: %x\n", __func__,
-			  csr);
-
-		switch (dev->ep0state) {
-		case WAIT_FOR_SETUP:
-			DEBUG_EP0("WAIT_FOR_SETUP\n");
-			lh7a40x_ep0_setup(dev, csr);
-			break;
-
-		case DATA_STATE_RECV:
-			DEBUG_EP0("DATA_STATE_RECV\n");
-			lh7a40x_ep0_out(dev, csr);
-			break;
-
-		default:
-			/* send stall? */
-			DEBUG_EP0("strange state!! 2. send stall? state = %d\n",
-				  dev->ep0state);
-			break;
-		}
-	}
-}
-
-static void lh7a40x_ep0_kick(struct lh7a40x_udc *dev, struct lh7a40x_ep *ep)
-{
-	u32 csr;
-
-	usb_set_index(0);
-	csr = usb_read(USB_EP0_CSR);
-
-	DEBUG_EP0("%s: %x\n", __func__, csr);
-
-	/* Clear "out packet ready" */
-	usb_set(EP0_CLR_OUT, USB_EP0_CSR);
-
-	if (ep_is_in(ep)) {
-		dev->ep0state = DATA_STATE_XMIT;
-		lh7a40x_ep0_in(dev, csr);
-	} else {
-		dev->ep0state = DATA_STATE_RECV;
-		lh7a40x_ep0_out(dev, csr);
-	}
-}
-
-/* ---------------------------------------------------------------------------
- * 	device-scoped parts of the api to the usb controller hardware
- * ---------------------------------------------------------------------------
- */
-
-static int lh7a40x_udc_get_frame(struct usb_gadget *_gadget)
-{
-	u32 frame1 = usb_read(USB_FRM_NUM1);	/* Least significant 8 bits */
-	u32 frame2 = usb_read(USB_FRM_NUM2);	/* Most significant 3 bits */
-	DEBUG("%s, %p\n", __func__, _gadget);
-	return ((frame2 & 0x07) << 8) | (frame1 & 0xff);
-}
-
-static int lh7a40x_udc_wakeup(struct usb_gadget *_gadget)
-{
-	/* host may not have enabled remote wakeup */
-	/*if ((UDCCS0 & UDCCS0_DRWF) == 0)
-	   return -EHOSTUNREACH;
-	   udc_set_mask_UDCCR(UDCCR_RSM); */
-	return -ENOTSUPP;
-}
-
-static const struct usb_gadget_ops lh7a40x_udc_ops = {
-	.get_frame = lh7a40x_udc_get_frame,
-	.wakeup = lh7a40x_udc_wakeup,
-	/* current versions must always be self-powered */
-};
-
-static void nop_release(struct device *dev)
-{
-	DEBUG("%s %s\n", __func__, dev_name(dev));
-}
-
-static struct lh7a40x_udc memory = {
-	.usb_address = 0,
-
-	.gadget = {
-		   .ops = &lh7a40x_udc_ops,
-		   .ep0 = &memory.ep[0].ep,
-		   .name = driver_name,
-		   .dev = {
-			   .init_name = "gadget",
-			   .release = nop_release,
-			   },
-		   },
-
-	/* control endpoint */
-	.ep[0] = {
-		  .ep = {
-			 .name = ep0name,
-			 .ops = &lh7a40x_ep_ops,
-			 .maxpacket = EP0_PACKETSIZE,
-			 },
-		  .dev = &memory,
-
-		  .bEndpointAddress = 0,
-		  .bmAttributes = 0,
-
-		  .ep_type = ep_control,
-		  .fifo = io_p2v(USB_EP0_FIFO),
-		  .csr1 = USB_EP0_CSR,
-		  .csr2 = USB_EP0_CSR,
-		  },
-
-	/* first group of endpoints */
-	.ep[1] = {
-		  .ep = {
-			 .name = "ep1in-bulk",
-			 .ops = &lh7a40x_ep_ops,
-			 .maxpacket = 64,
-			 },
-		  .dev = &memory,
-
-		  .bEndpointAddress = USB_DIR_IN | 1,
-		  .bmAttributes = USB_ENDPOINT_XFER_BULK,
-
-		  .ep_type = ep_bulk_in,
-		  .fifo = io_p2v(USB_EP1_FIFO),
-		  .csr1 = USB_IN_CSR1,
-		  .csr2 = USB_IN_CSR2,
-		  },
-
-	.ep[2] = {
-		  .ep = {
-			 .name = "ep2out-bulk",
-			 .ops = &lh7a40x_ep_ops,
-			 .maxpacket = 64,
-			 },
-		  .dev = &memory,
-
-		  .bEndpointAddress = 2,
-		  .bmAttributes = USB_ENDPOINT_XFER_BULK,
-
-		  .ep_type = ep_bulk_out,
-		  .fifo = io_p2v(USB_EP2_FIFO),
-		  .csr1 = USB_OUT_CSR1,
-		  .csr2 = USB_OUT_CSR2,
-		  },
-
-	.ep[3] = {
-		  .ep = {
-			 .name = "ep3in-int",
-			 .ops = &lh7a40x_ep_ops,
-			 .maxpacket = 64,
-			 },
-		  .dev = &memory,
-
-		  .bEndpointAddress = USB_DIR_IN | 3,
-		  .bmAttributes = USB_ENDPOINT_XFER_INT,
-
-		  .ep_type = ep_interrupt,
-		  .fifo = io_p2v(USB_EP3_FIFO),
-		  .csr1 = USB_IN_CSR1,
-		  .csr2 = USB_IN_CSR2,
-		  },
-};
-
-/*
- * 	probe - binds to the platform device
- */
-static int lh7a40x_udc_probe(struct platform_device *pdev)
-{
-	struct lh7a40x_udc *dev = &memory;
-	int retval;
-
-	DEBUG("%s: %p\n", __func__, pdev);
-
-	spin_lock_init(&dev->lock);
-	dev->dev = &pdev->dev;
-
-	device_initialize(&dev->gadget.dev);
-	dev->gadget.dev.parent = &pdev->dev;
-
-	the_controller = dev;
-	platform_set_drvdata(pdev, dev);
-
-	udc_disable(dev);
-	udc_reinit(dev);
-
-	/* irq setup after old hardware state is cleaned up */
-	retval =
-	    request_irq(IRQ_USBINTR, lh7a40x_udc_irq, IRQF_DISABLED, driver_name,
-			dev);
-	if (retval != 0) {
-		DEBUG(KERN_ERR "%s: can't get irq %i, err %d\n", driver_name,
-		      IRQ_USBINTR, retval);
-		return -EBUSY;
-	}
-
-	create_proc_files();
-
-	return retval;
-}
-
-static int lh7a40x_udc_remove(struct platform_device *pdev)
-{
-	struct lh7a40x_udc *dev = platform_get_drvdata(pdev);
-
-	DEBUG("%s: %p\n", __func__, pdev);
-
-	if (dev->driver)
-		return -EBUSY;
-
-	udc_disable(dev);
-	remove_proc_files();
-
-	free_irq(IRQ_USBINTR, dev);
-
-	platform_set_drvdata(pdev, 0);
-
-	the_controller = 0;
-
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-static struct platform_driver udc_driver = {
-	.probe = lh7a40x_udc_probe,
-	.remove = lh7a40x_udc_remove,
-	    /* FIXME power management support */
-	    /* .suspend = ... disable UDC */
-	    /* .resume = ... re-enable UDC */
-	.driver	= {
-		.name = (char *)driver_name,
-		.owner = THIS_MODULE,
-	},
-};
-
-static int __init udc_init(void)
-{
-	DEBUG("%s: %s version %s\n", __func__, driver_name, DRIVER_VERSION);
-	return platform_driver_register(&udc_driver);
-}
-
-static void __exit udc_exit(void)
-{
-	platform_driver_unregister(&udc_driver);
-}
-
-module_init(udc_init);
-module_exit(udc_exit);
-
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_AUTHOR("Mikko Lahteenmaki, Bo Henriksen");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:lh7a40x_udc");
diff --git a/drivers/usb/gadget/lh7a40x_udc.h b/drivers/usb/gadget/lh7a40x_udc.h
deleted file mode 100644
index ca861203a301..000000000000
--- a/drivers/usb/gadget/lh7a40x_udc.h
+++ /dev/null
@@ -1,259 +0,0 @@
-/*
- * linux/drivers/usb/gadget/lh7a40x_udc.h
- * Sharp LH7A40x on-chip full speed USB device controllers
- *
- * Copyright (C) 2004 Mikko Lahteenmaki, Nordic ID
- * Copyright (C) 2004 Bo Henriksen, Nordic ID
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- */
-
-#ifndef __LH7A40X_H_
-#define __LH7A40X_H_
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/ioport.h>
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/delay.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/timer.h>
-#include <linux/list.h>
-#include <linux/interrupt.h>
-#include <linux/proc_fs.h>
-#include <linux/mm.h>
-#include <linux/device.h>
-#include <linux/dma-mapping.h>
-
-#include <asm/byteorder.h>
-#include <asm/dma.h>
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/system.h>
-#include <asm/unaligned.h>
-#include <mach/hardware.h>
-
-#include <linux/usb/ch9.h>
-#include <linux/usb/gadget.h>
-
-/*
- * Memory map
- */
-
-#define USB_FA					0x80000200	// function address register
-#define USB_PM					0x80000204	// power management register
-
-#define USB_IN_INT				0x80000208	// IN interrupt register bank (EP0-EP3)
-#define USB_OUT_INT				0x80000210	// OUT interrupt register bank (EP2)
-#define USB_INT					0x80000218	// interrupt register bank
-
-#define USB_IN_INT_EN			0x8000021C	// IN interrupt enable register bank
-#define USB_OUT_INT_EN			0x80000224	// OUT interrupt enable register bank
-#define USB_INT_EN				0x8000022C	// USB interrupt enable register bank
-
-#define USB_FRM_NUM1			0x80000230	// Frame number1 register
-#define USB_FRM_NUM2			0x80000234	// Frame number2 register
-#define USB_INDEX				0x80000238	// index register
-
-#define USB_IN_MAXP				0x80000240	// IN MAXP register
-#define USB_IN_CSR1				0x80000244	// IN CSR1 register/EP0 CSR register
-#define USB_EP0_CSR				0x80000244	// IN CSR1 register/EP0 CSR register
-#define USB_IN_CSR2				0x80000248	// IN CSR2 register
-#define USB_OUT_MAXP			0x8000024C	// OUT MAXP register
-
-#define USB_OUT_CSR1			0x80000250	// OUT CSR1 register
-#define USB_OUT_CSR2			0x80000254	// OUT CSR2 register
-#define USB_OUT_FIFO_WC1		0x80000258	// OUT FIFO write count1 register
-#define USB_OUT_FIFO_WC2		0x8000025C	// OUT FIFO write count2 register
-
-#define USB_RESET				0x8000044C	// USB reset register
-
-#define	USB_EP0_FIFO			0x80000280
-#define	USB_EP1_FIFO			0x80000284
-#define	USB_EP2_FIFO			0x80000288
-#define	USB_EP3_FIFO			0x8000028c
-
-/*
- * USB reset register
- */
-#define USB_RESET_APB			(1<<1)	//resets USB APB control side WRITE
-#define USB_RESET_IO			(1<<0)	//resets USB IO side WRITE
-
-/*
- * USB function address register
- */
-#define USB_FA_ADDR_UPDATE		(1<<7)
-#define USB_FA_FUNCTION_ADDR	(0x7F)
-
-/*
- * Power Management register
- */
-#define PM_USB_DCP				(1<<5)
-#define PM_USB_ENABLE			(1<<4)
-#define PM_USB_RESET			(1<<3)
-#define PM_UC_RESUME			(1<<2)
-#define PM_SUSPEND_MODE			(1<<1)
-#define PM_ENABLE_SUSPEND		(1<<0)
-
-/*
- * IN interrupt register
- */
-#define USB_IN_INT_EP3				(1<<3)
-#define USB_IN_INT_EP1				(1<<1)
-#define USB_IN_INT_EP0				(1<<0)
-
-/*
- * OUT interrupt register
- */
-#define USB_OUT_INT_EP2				(1<<2)
-
-/*
- * USB interrupt register
- */
-#define USB_INT_RESET_INT			(1<<2)
-#define USB_INT_RESUME_INT			(1<<1)
-#define USB_INT_SUSPEND_INT			(1<<0)
-
-/*
- * USB interrupt enable register
- */
-#define USB_INT_EN_USB_RESET_INTER		(1<<2)
-#define USB_INT_EN_RESUME_INTER			(1<<1)
-#define USB_INT_EN_SUSPEND_INTER		(1<<0)
-
-/*
- * INCSR1 register
- */
-#define USB_IN_CSR1_CLR_DATA_TOGGLE		(1<<6)
-#define USB_IN_CSR1_SENT_STALL			(1<<5)
-#define USB_IN_CSR1_SEND_STALL			(1<<4)
-#define USB_IN_CSR1_FIFO_FLUSH			(1<<3)
-#define USB_IN_CSR1_FIFO_NOT_EMPTY		(1<<1)
-#define USB_IN_CSR1_IN_PKT_RDY			(1<<0)
-
-/*
- * INCSR2 register
- */
-#define USB_IN_CSR2_AUTO_SET			(1<<7)
-#define USB_IN_CSR2_USB_DMA_EN			(1<<4)
-
-/*
- * OUT CSR1 register
- */
-#define USB_OUT_CSR1_CLR_DATA_REG		(1<<7)
-#define USB_OUT_CSR1_SENT_STALL			(1<<6)
-#define USB_OUT_CSR1_SEND_STALL			(1<<5)
-#define USB_OUT_CSR1_FIFO_FLUSH			(1<<4)
-#define USB_OUT_CSR1_FIFO_FULL			(1<<1)
-#define USB_OUT_CSR1_OUT_PKT_RDY		(1<<0)
-
-/*
- * OUT CSR2 register
- */
-#define USB_OUT_CSR2_AUTO_CLR			(1<<7)
-#define USB_OUT_CSR2_USB_DMA_EN			(1<<4)
-
-/*
- * EP0 CSR
- */
-#define EP0_CLR_SETUP_END		(1<<7)	/* Clear "Setup Ends" Bit (w) */
-#define EP0_CLR_OUT				(1<<6)	/* Clear "Out packet ready" Bit (w) */
-#define EP0_SEND_STALL			(1<<5)	/* Send STALL Handshake (rw) */
-#define EP0_SETUP_END			(1<<4)	/* Setup Ends (r) */
-
-#define EP0_DATA_END			(1<<3)	/* Data end (rw) */
-#define EP0_SENT_STALL			(1<<2)	/* Sent Stall Handshake (r) */
-#define EP0_IN_PKT_RDY			(1<<1)	/* In packet ready (rw) */
-#define EP0_OUT_PKT_RDY			(1<<0)	/* Out packet ready (r) */
-
-/* general CSR */
-#define OUT_PKT_RDY		(1<<0)
-#define IN_PKT_RDY		(1<<0)
-
-/*
- * IN/OUT MAXP register
- */
-#define USB_OUT_MAXP_MAXP			(0xF)
-#define USB_IN_MAXP_MAXP			(0xF)
-
-// Max packet size
-//#define EP0_PACKETSIZE        0x10
-#define EP0_PACKETSIZE  	0x8
-#define EP0_MAXPACKETSIZE  	0x10
-
-#define UDC_MAX_ENDPOINTS       4
-
-#define WAIT_FOR_SETUP          0
-#define DATA_STATE_XMIT         1
-#define DATA_STATE_NEED_ZLP     2
-#define WAIT_FOR_OUT_STATUS     3
-#define DATA_STATE_RECV         4
-
-/* ********************************************************************************************* */
-/* IO
- */
-
-typedef enum ep_type {
-	ep_control, ep_bulk_in, ep_bulk_out, ep_interrupt
-} ep_type_t;
-
-struct lh7a40x_ep {
-	struct usb_ep ep;
-	struct lh7a40x_udc *dev;
-
-	const struct usb_endpoint_descriptor *desc;
-	struct list_head queue;
-	unsigned long pio_irqs;
-
-	u8 stopped;
-	u8 bEndpointAddress;
-	u8 bmAttributes;
-
-	ep_type_t ep_type;
-	u32 fifo;
-	u32 csr1;
-	u32 csr2;
-};
-
-struct lh7a40x_request {
-	struct usb_request req;
-	struct list_head queue;
-};
-
-struct lh7a40x_udc {
-	struct usb_gadget gadget;
-	struct usb_gadget_driver *driver;
-	struct device *dev;
-	spinlock_t lock;
-
-	int ep0state;
-	struct lh7a40x_ep ep[UDC_MAX_ENDPOINTS];
-
-	unsigned char usb_address;
-
-	unsigned req_pending:1, req_std:1, req_config:1;
-};
-
-extern struct lh7a40x_udc *the_controller;
-
-#define ep_is_in(EP) 		(((EP)->bEndpointAddress&USB_DIR_IN)==USB_DIR_IN)
-#define ep_index(EP) 		((EP)->bEndpointAddress&0xF)
-#define ep_maxpacket(EP) 	((EP)->ep.maxpacket)
-
-#endif
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 74dcf49bd015..c4de2d75a3d7 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -1306,24 +1306,24 @@ static int __init ehci_hcd_init(void)
 #endif
 
 #ifdef OF_PLATFORM_DRIVER
-	retval = of_register_platform_driver(&OF_PLATFORM_DRIVER);
+	retval = platform_driver_register(&OF_PLATFORM_DRIVER);
 	if (retval < 0)
 		goto clean3;
 #endif
 
 #ifdef XILINX_OF_PLATFORM_DRIVER
-	retval = of_register_platform_driver(&XILINX_OF_PLATFORM_DRIVER);
+	retval = platform_driver_register(&XILINX_OF_PLATFORM_DRIVER);
 	if (retval < 0)
 		goto clean4;
 #endif
 	return retval;
 
 #ifdef XILINX_OF_PLATFORM_DRIVER
-	/* of_unregister_platform_driver(&XILINX_OF_PLATFORM_DRIVER); */
+	/* platform_driver_unregister(&XILINX_OF_PLATFORM_DRIVER); */
 clean4:
 #endif
 #ifdef OF_PLATFORM_DRIVER
-	of_unregister_platform_driver(&OF_PLATFORM_DRIVER);
+	platform_driver_unregister(&OF_PLATFORM_DRIVER);
 clean3:
 #endif
 #ifdef PS3_SYSTEM_BUS_DRIVER
@@ -1351,10 +1351,10 @@ module_init(ehci_hcd_init);
 static void __exit ehci_hcd_cleanup(void)
 {
 #ifdef XILINX_OF_PLATFORM_DRIVER
-	of_unregister_platform_driver(&XILINX_OF_PLATFORM_DRIVER);
+	platform_driver_unregister(&XILINX_OF_PLATFORM_DRIVER);
 #endif
 #ifdef OF_PLATFORM_DRIVER
-	of_unregister_platform_driver(&OF_PLATFORM_DRIVER);
+	platform_driver_unregister(&OF_PLATFORM_DRIVER);
 #endif
 #ifdef PLATFORM_DRIVER
 	platform_driver_unregister(&PLATFORM_DRIVER);
diff --git a/drivers/usb/host/ehci-mxc.c b/drivers/usb/host/ehci-mxc.c
index c8e360d7d975..25c8c10bb689 100644
--- a/drivers/usb/host/ehci-mxc.c
+++ b/drivers/usb/host/ehci-mxc.c
@@ -203,11 +203,6 @@ static int ehci_mxc_drv_probe(struct platform_device *pdev)
 		mdelay(10);
 	}
 
-	/* setup specific usb hw */
-	ret = mxc_initialize_usb_hw(pdev->id, pdata->flags);
-	if (ret < 0)
-		goto err_init;
-
 	ehci = hcd_to_ehci(hcd);
 
 	/* EHCI registers start at offset 0x100 */
diff --git a/drivers/usb/host/ehci-ppc-of.c b/drivers/usb/host/ehci-ppc-of.c
index ba52be473027..1f09f253697e 100644
--- a/drivers/usb/host/ehci-ppc-of.c
+++ b/drivers/usb/host/ehci-ppc-of.c
@@ -105,8 +105,7 @@ ppc44x_enable_bmt(struct device_node *dn)
 }
 
 
-static int __devinit
-ehci_hcd_ppc_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit ehci_hcd_ppc_of_probe(struct platform_device *op)
 {
 	struct device_node *dn = op->dev.of_node;
 	struct usb_hcd *hcd;
@@ -255,14 +254,12 @@ static int ehci_hcd_ppc_of_remove(struct platform_device *op)
 }
 
 
-static int ehci_hcd_ppc_of_shutdown(struct platform_device *op)
+static void ehci_hcd_ppc_of_shutdown(struct platform_device *op)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(&op->dev);
 
 	if (hcd->driver->shutdown)
 		hcd->driver->shutdown(hcd);
-
-	return 0;
 }
 
 
@@ -275,7 +272,7 @@ static const struct of_device_id ehci_hcd_ppc_of_match[] = {
 MODULE_DEVICE_TABLE(of, ehci_hcd_ppc_of_match);
 
 
-static struct of_platform_driver ehci_hcd_ppc_of_driver = {
+static struct platform_driver ehci_hcd_ppc_of_driver = {
 	.probe		= ehci_hcd_ppc_of_probe,
 	.remove		= ehci_hcd_ppc_of_remove,
 	.shutdown	= ehci_hcd_ppc_of_shutdown,
diff --git a/drivers/usb/host/ehci-xilinx-of.c b/drivers/usb/host/ehci-xilinx-of.c
index e8f4f36fdf0b..effc58d7af8b 100644
--- a/drivers/usb/host/ehci-xilinx-of.c
+++ b/drivers/usb/host/ehci-xilinx-of.c
@@ -29,6 +29,7 @@
 
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/of_address.h>
 
 /**
  * ehci_xilinx_of_setup - Initialize the device for ehci_reset()
@@ -142,15 +143,13 @@ static const struct hc_driver ehci_xilinx_of_hc_driver = {
 /**
  * ehci_hcd_xilinx_of_probe - Probe method for the USB host controller
  * @op:		pointer to the platform_device bound to the host controller
- * @match:	pointer to of_device_id structure, not used
  *
  * This function requests resources and sets up appropriate properties for the
  * host controller. Because the Xilinx USB host controller can be configured
  * as HS only or HS/FS only, it checks the configuration in the device tree
  * entry, and sets an appropriate value for hcd->has_tt.
  */
-static int __devinit
-ehci_hcd_xilinx_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit ehci_hcd_xilinx_of_probe(struct platform_device *op)
 {
 	struct device_node *dn = op->dev.of_node;
 	struct usb_hcd *hcd;
@@ -288,7 +287,7 @@ static const struct of_device_id ehci_hcd_xilinx_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ehci_hcd_xilinx_of_match);
 
-static struct of_platform_driver ehci_hcd_xilinx_of_driver = {
+static struct platform_driver ehci_hcd_xilinx_of_driver = {
 	.probe		= ehci_hcd_xilinx_of_probe,
 	.remove		= ehci_hcd_xilinx_of_remove,
 	.shutdown	= ehci_hcd_xilinx_of_shutdown,
diff --git a/drivers/usb/host/fhci-hcd.c b/drivers/usb/host/fhci-hcd.c
index 12fd184226f2..b84ff7e51896 100644
--- a/drivers/usb/host/fhci-hcd.c
+++ b/drivers/usb/host/fhci-hcd.c
@@ -561,8 +561,7 @@ static const struct hc_driver fhci_driver = {
 	.hub_control = fhci_hub_control,
 };
 
-static int __devinit of_fhci_probe(struct platform_device *ofdev,
-				   const struct of_device_id *ofid)
+static int __devinit of_fhci_probe(struct platform_device *ofdev)
 {
 	struct device *dev = &ofdev->dev;
 	struct device_node *node = dev->of_node;
@@ -812,7 +811,7 @@ static const struct of_device_id of_fhci_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_fhci_match);
 
-static struct of_platform_driver of_fhci_driver = {
+static struct platform_driver of_fhci_driver = {
 	.driver = {
 		.name = "fsl,usb-fhci",
 		.owner = THIS_MODULE,
@@ -824,13 +823,13 @@ static struct of_platform_driver of_fhci_driver = {
 
 static int __init fhci_module_init(void)
 {
-	return of_register_platform_driver(&of_fhci_driver);
+	return platform_driver_register(&of_fhci_driver);
 }
 module_init(fhci_module_init);
 
 static void __exit fhci_module_exit(void)
 {
-	of_unregister_platform_driver(&of_fhci_driver);
+	platform_driver_unregister(&of_fhci_driver);
 }
 module_exit(fhci_module_exit);
 
diff --git a/drivers/usb/host/isp1760-if.c b/drivers/usb/host/isp1760-if.c
index 3b28dbfca058..7ee30056f373 100644
--- a/drivers/usb/host/isp1760-if.c
+++ b/drivers/usb/host/isp1760-if.c
@@ -27,8 +27,7 @@
 #endif
 
 #ifdef CONFIG_PPC_OF
-static int of_isp1760_probe(struct platform_device *dev,
-		const struct of_device_id *match)
+static int of_isp1760_probe(struct platform_device *dev)
 {
 	struct usb_hcd *hcd;
 	struct device_node *dp = dev->dev.of_node;
@@ -119,7 +118,7 @@ static const struct of_device_id of_isp1760_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_isp1760_match);
 
-static struct of_platform_driver isp1760_of_driver = {
+static struct platform_driver isp1760_of_driver = {
 	.driver = {
 		.name = "nxp-isp1760",
 		.owner = THIS_MODULE,
@@ -398,7 +397,7 @@ static int __init isp1760_init(void)
 	if (!ret)
 		any_ret = 0;
 #ifdef CONFIG_PPC_OF
-	ret = of_register_platform_driver(&isp1760_of_driver);
+	ret = platform_driver_register(&isp1760_of_driver);
 	if (!ret)
 		any_ret = 0;
 #endif
@@ -418,7 +417,7 @@ static void __exit isp1760_exit(void)
 {
 	platform_driver_unregister(&isp1760_plat_driver);
 #ifdef CONFIG_PPC_OF
-	of_unregister_platform_driver(&isp1760_of_driver);
+	platform_driver_unregister(&isp1760_of_driver);
 #endif
 #ifdef CONFIG_PCI
 	pci_unregister_driver(&isp1761_pci_driver);
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 759a12ff8048..2be257efb01a 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1023,11 +1023,6 @@ MODULE_LICENSE ("GPL");
 #define OMAP3_PLATFORM_DRIVER	ohci_hcd_omap3_driver
 #endif
 
-#ifdef CONFIG_ARCH_LH7A404
-#include "ohci-lh7a404.c"
-#define PLATFORM_DRIVER		ohci_hcd_lh7a404_driver
-#endif
-
 #if defined(CONFIG_PXA27x) || defined(CONFIG_PXA3xx)
 #include "ohci-pxa27x.c"
 #define PLATFORM_DRIVER		ohci_hcd_pxa27x_driver
@@ -1180,7 +1175,7 @@ static int __init ohci_hcd_mod_init(void)
 #endif
 
 #ifdef OF_PLATFORM_DRIVER
-	retval = of_register_platform_driver(&OF_PLATFORM_DRIVER);
+	retval = platform_driver_register(&OF_PLATFORM_DRIVER);
 	if (retval < 0)
 		goto error_of_platform;
 #endif
@@ -1239,7 +1234,7 @@ static int __init ohci_hcd_mod_init(void)
  error_sa1111:
 #endif
 #ifdef OF_PLATFORM_DRIVER
-	of_unregister_platform_driver(&OF_PLATFORM_DRIVER);
+	platform_driver_unregister(&OF_PLATFORM_DRIVER);
  error_of_platform:
 #endif
 #ifdef PLATFORM_DRIVER
@@ -1287,7 +1282,7 @@ static void __exit ohci_hcd_mod_exit(void)
 	sa1111_driver_unregister(&SA1111_DRIVER);
 #endif
 #ifdef OF_PLATFORM_DRIVER
-	of_unregister_platform_driver(&OF_PLATFORM_DRIVER);
+	platform_driver_unregister(&OF_PLATFORM_DRIVER);
 #endif
 #ifdef PLATFORM_DRIVER
 	platform_driver_unregister(&PLATFORM_DRIVER);
diff --git a/drivers/usb/host/ohci-lh7a404.c b/drivers/usb/host/ohci-lh7a404.c
deleted file mode 100644
index 18d39f0463ee..000000000000
--- a/drivers/usb/host/ohci-lh7a404.c
+++ /dev/null
@@ -1,252 +0,0 @@
-/*
- * OHCI HCD (Host Controller Driver) for USB.
- *
- * (C) Copyright 1999 Roman Weissgaerber <weissg@vienna.at>
- * (C) Copyright 2000-2002 David Brownell <dbrownell@users.sourceforge.net>
- * (C) Copyright 2002 Hewlett-Packard Company
- *
- * Bus Glue for Sharp LH7A404
- *
- * Written by Christopher Hoover <ch@hpl.hp.com>
- * Based on fragments of previous driver by Russell King et al.
- *
- * Modified for LH7A404 from ohci-sa1111.c
- *  by Durgesh Pattamatta <pattamattad@sharpsec.com>
- *
- * This file is licenced under the GPL.
- */
-
-#include <linux/platform_device.h>
-#include <linux/signal.h>
-
-#include <mach/hardware.h>
-
-
-extern int usb_disabled(void);
-
-/*-------------------------------------------------------------------------*/
-
-static void lh7a404_start_hc(struct platform_device *dev)
-{
-	printk(KERN_DEBUG "%s: starting LH7A404 OHCI USB Controller\n",
-	       __FILE__);
-
-	/*
-	 * Now, carefully enable the USB clock, and take
-	 * the USB host controller out of reset.
-	 */
-	CSC_PWRCNT |= CSC_PWRCNT_USBH_EN; /* Enable clock */
-	udelay(1000);
-	USBH_CMDSTATUS = OHCI_HCR;
-
-	printk(KERN_DEBUG "%s: Clock to USB host has been enabled \n", __FILE__);
-}
-
-static void lh7a404_stop_hc(struct platform_device *dev)
-{
-	printk(KERN_DEBUG "%s: stopping LH7A404 OHCI USB Controller\n",
-	       __FILE__);
-
-	CSC_PWRCNT &= ~CSC_PWRCNT_USBH_EN; /* Disable clock */
-}
-
-
-/*-------------------------------------------------------------------------*/
-
-/* configure so an HC device and id are always provided */
-/* always called with process context; sleeping is OK */
-
-
-/**
- * usb_hcd_lh7a404_probe - initialize LH7A404-based HCDs
- * Context: !in_interrupt()
- *
- * Allocates basic resources for this USB host controller, and
- * then invokes the start() method for the HCD associated with it
- * through the hotplug entry's driver_data.
- *
- */
-int usb_hcd_lh7a404_probe (const struct hc_driver *driver,
-			  struct platform_device *dev)
-{
-	int retval;
-	struct usb_hcd *hcd;
-
-	if (dev->resource[1].flags != IORESOURCE_IRQ) {
-		pr_debug("resource[1] is not IORESOURCE_IRQ");
-		return -ENOMEM;
-	}
-
-	hcd = usb_create_hcd(driver, &dev->dev, "lh7a404");
-	if (!hcd)
-		return -ENOMEM;
-	hcd->rsrc_start = dev->resource[0].start;
-	hcd->rsrc_len = dev->resource[0].end - dev->resource[0].start + 1;
-
-	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len, hcd_name)) {
-		pr_debug("request_mem_region failed");
-		retval = -EBUSY;
-		goto err1;
-	}
-
-	hcd->regs = ioremap(hcd->rsrc_start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		pr_debug("ioremap failed");
-		retval = -ENOMEM;
-		goto err2;
-	}
-
-	lh7a404_start_hc(dev);
-	ohci_hcd_init(hcd_to_ohci(hcd));
-
-	retval = usb_add_hcd(hcd, dev->resource[1].start, IRQF_DISABLED);
-	if (retval == 0)
-		return retval;
-
-	lh7a404_stop_hc(dev);
-	iounmap(hcd->regs);
- err2:
-	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
- err1:
-	usb_put_hcd(hcd);
-	return retval;
-}
-
-
-/* may be called without controller electrically present */
-/* may be called with controller, bus, and devices active */
-
-/**
- * usb_hcd_lh7a404_remove - shutdown processing for LH7A404-based HCDs
- * @dev: USB Host Controller being removed
- * Context: !in_interrupt()
- *
- * Reverses the effect of usb_hcd_lh7a404_probe(), first invoking
- * the HCD's stop() method.  It is always called from a thread
- * context, normally "rmmod", "apmd", or something similar.
- *
- */
-void usb_hcd_lh7a404_remove (struct usb_hcd *hcd, struct platform_device *dev)
-{
-	usb_remove_hcd(hcd);
-	lh7a404_stop_hc(dev);
-	iounmap(hcd->regs);
-	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
-	usb_put_hcd(hcd);
-}
-
-/*-------------------------------------------------------------------------*/
-
-static int __devinit
-ohci_lh7a404_start (struct usb_hcd *hcd)
-{
-	struct ohci_hcd	*ohci = hcd_to_ohci (hcd);
-	int		ret;
-
-	ohci_dbg (ohci, "ohci_lh7a404_start, ohci:%p", ohci);
-	if ((ret = ohci_init(ohci)) < 0)
-		return ret;
-
-	if ((ret = ohci_run (ohci)) < 0) {
-		err ("can't start %s", hcd->self.bus_name);
-		ohci_stop (hcd);
-		return ret;
-	}
-	return 0;
-}
-
-/*-------------------------------------------------------------------------*/
-
-static const struct hc_driver ohci_lh7a404_hc_driver = {
-	.description =		hcd_name,
-	.product_desc =		"LH7A404 OHCI",
-	.hcd_priv_size =	sizeof(struct ohci_hcd),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq =			ohci_irq,
-	.flags =		HCD_USB11 | HCD_MEMORY,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.start =		ohci_lh7a404_start,
-	.stop =			ohci_stop,
-	.shutdown =		ohci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue =		ohci_urb_enqueue,
-	.urb_dequeue =		ohci_urb_dequeue,
-	.endpoint_disable =	ohci_endpoint_disable,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number =	ohci_get_frame,
-
-	/*
-	 * root hub support
-	 */
-	.hub_status_data =	ohci_hub_status_data,
-	.hub_control =		ohci_hub_control,
-#ifdef	CONFIG_PM
-	.bus_suspend =		ohci_bus_suspend,
-	.bus_resume =		ohci_bus_resume,
-#endif
-	.start_port_reset =	ohci_start_port_reset,
-};
-
-/*-------------------------------------------------------------------------*/
-
-static int ohci_hcd_lh7a404_drv_probe(struct platform_device *pdev)
-{
-	int ret;
-
-	pr_debug ("In ohci_hcd_lh7a404_drv_probe");
-
-	if (usb_disabled())
-		return -ENODEV;
-
-	ret = usb_hcd_lh7a404_probe(&ohci_lh7a404_hc_driver, pdev);
-	return ret;
-}
-
-static int ohci_hcd_lh7a404_drv_remove(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-
-	usb_hcd_lh7a404_remove(hcd, pdev);
-	return 0;
-}
-	/*TBD*/
-/*static int ohci_hcd_lh7a404_drv_suspend(struct platform_device *dev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(dev);
-
-	return 0;
-}
-static int ohci_hcd_lh7a404_drv_resume(struct platform_device *dev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(dev);
-
-
-	return 0;
-}
-*/
-
-static struct platform_driver ohci_hcd_lh7a404_driver = {
-	.probe		= ohci_hcd_lh7a404_drv_probe,
-	.remove		= ohci_hcd_lh7a404_drv_remove,
-	.shutdown	= usb_hcd_platform_shutdown,
-	/*.suspend	= ohci_hcd_lh7a404_drv_suspend, */
-	/*.resume	= ohci_hcd_lh7a404_drv_resume, */
-	.driver		= {
-		.name	= "lh7a404-ohci",
-		.owner	= THIS_MODULE,
-	},
-};
-
-MODULE_ALIAS("platform:lh7a404-ohci");
diff --git a/drivers/usb/host/ohci-ppc-of.c b/drivers/usb/host/ohci-ppc-of.c
index b2c2dbf08766..1ca1821320f4 100644
--- a/drivers/usb/host/ohci-ppc-of.c
+++ b/drivers/usb/host/ohci-ppc-of.c
@@ -80,8 +80,7 @@ static const struct hc_driver ohci_ppc_of_hc_driver = {
 };
 
 
-static int __devinit
-ohci_hcd_ppc_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit ohci_hcd_ppc_of_probe(struct platform_device *op)
 {
 	struct device_node *dn = op->dev.of_node;
 	struct usb_hcd *hcd;
@@ -201,14 +200,12 @@ static int ohci_hcd_ppc_of_remove(struct platform_device *op)
 	return 0;
 }
 
-static int ohci_hcd_ppc_of_shutdown(struct platform_device *op)
+static void ohci_hcd_ppc_of_shutdown(struct platform_device *op)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(&op->dev);
 
         if (hcd->driver->shutdown)
                 hcd->driver->shutdown(hcd);
-
-	return 0;
 }
 
 
@@ -243,7 +240,7 @@ MODULE_DEVICE_TABLE(of, ohci_hcd_ppc_of_match);
 #endif
 
 
-static struct of_platform_driver ohci_hcd_ppc_of_driver = {
+static struct platform_driver ohci_hcd_ppc_of_driver = {
 	.probe		= ohci_hcd_ppc_of_probe,
 	.remove		= ohci_hcd_ppc_of_remove,
 	.shutdown 	= ohci_hcd_ppc_of_shutdown,
diff --git a/drivers/usb/host/ohci-tmio.c b/drivers/usb/host/ohci-tmio.c
index 8dabe8e31d8c..3558491dd87d 100644
--- a/drivers/usb/host/ohci-tmio.c
+++ b/drivers/usb/host/ohci-tmio.c
@@ -185,7 +185,7 @@ static struct platform_driver ohci_hcd_tmio_driver;
 
 static int __devinit ohci_hcd_tmio_drv_probe(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct resource *regs = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	struct resource *config = platform_get_resource(dev, IORESOURCE_MEM, 1);
 	struct resource *sram = platform_get_resource(dev, IORESOURCE_MEM, 2);
@@ -274,7 +274,7 @@ static int __devexit ohci_hcd_tmio_drv_remove(struct platform_device *dev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(dev);
 	struct tmio_hcd *tmio = hcd_to_tmio(hcd);
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 
 	usb_remove_hcd(hcd);
 	tmio_stop_hc(dev);
@@ -293,7 +293,7 @@ static int __devexit ohci_hcd_tmio_drv_remove(struct platform_device *dev)
 #ifdef CONFIG_PM
 static int ohci_hcd_tmio_drv_suspend(struct platform_device *dev, pm_message_t state)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct usb_hcd *hcd = platform_get_drvdata(dev);
 	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
 	struct tmio_hcd *tmio = hcd_to_tmio(hcd);
@@ -326,7 +326,7 @@ static int ohci_hcd_tmio_drv_suspend(struct platform_device *dev, pm_message_t s
 
 static int ohci_hcd_tmio_drv_resume(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct usb_hcd *hcd = platform_get_drvdata(dev);
 	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
 	struct tmio_hcd *tmio = hcd_to_tmio(hcd);
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 51facb985c84..04812b42fe6f 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -575,18 +575,8 @@ static inline void _ohci_writel (const struct ohci_hcd *ohci,
 #endif
 }
 
-#ifdef CONFIG_ARCH_LH7A404
-/* Marc Singer: at the time this code was written, the LH7A404
- * had a problem reading the USB host registers.  This
- * implementation of the ohci_readl function performs the read
- * twice as a work-around.
- */
-#define ohci_readl(o,r)		(_ohci_readl(o,r),_ohci_readl(o,r))
-#define ohci_writel(o,v,r)	_ohci_writel(o,v,r)
-#else
 #define ohci_readl(o,r)		_ohci_readl(o,r)
 #define ohci_writel(o,v,r)	_ohci_writel(o,v,r)
-#endif
 
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/host/uhci-hcd.c b/drivers/usb/host/uhci-hcd.c
index cee867829ec9..4f65b14e5e08 100644
--- a/drivers/usb/host/uhci-hcd.c
+++ b/drivers/usb/host/uhci-hcd.c
@@ -471,7 +471,7 @@ static irqreturn_t uhci_irq(struct usb_hcd *hcd)
 
 /*
  * Store the current frame number in uhci->frame_number if the controller
- * is runnning.  Expand from 11 bits (of which we use only 10) to a
+ * is running.  Expand from 11 bits (of which we use only 10) to a
  * full-sized integer.
  *
  * Like many other parts of the driver, this code relies on being polled
diff --git a/drivers/usb/host/xhci-dbg.c b/drivers/usb/host/xhci-dbg.c
index fcbf4abbf381..0231814a97a5 100644
--- a/drivers/usb/host/xhci-dbg.c
+++ b/drivers/usb/host/xhci-dbg.c
@@ -169,9 +169,10 @@ static void xhci_print_ports(struct xhci_hcd *xhci)
 	}
 }
 
-void xhci_print_ir_set(struct xhci_hcd *xhci, struct xhci_intr_reg *ir_set, int set_num)
+void xhci_print_ir_set(struct xhci_hcd *xhci, int set_num)
 {
-	void *addr;
+	struct xhci_intr_reg __iomem *ir_set = &xhci->run_regs->ir_set[set_num];
+	void __iomem *addr;
 	u32 temp;
 	u64 temp_64;
 
@@ -449,7 +450,7 @@ char *xhci_get_slot_state(struct xhci_hcd *xhci,
 	}
 }
 
-void xhci_dbg_slot_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx)
+static void xhci_dbg_slot_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx)
 {
 	/* Fields are 32 bits wide, DMA addresses are in bytes */
 	int field_size = 32 / 8;
@@ -488,7 +489,7 @@ void xhci_dbg_slot_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx)
 		dbg_rsvd64(xhci, (u64 *)slot_ctx, dma);
 }
 
-void xhci_dbg_ep_ctx(struct xhci_hcd *xhci,
+static void xhci_dbg_ep_ctx(struct xhci_hcd *xhci,
 		     struct xhci_container_ctx *ctx,
 		     unsigned int last_ep)
 {
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 1d0f45f0e7a6..a9534396e85b 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -307,7 +307,7 @@ struct xhci_ep_ctx *xhci_get_ep_ctx(struct xhci_hcd *xhci,
 
 /***************** Streams structures manipulation *************************/
 
-void xhci_free_stream_ctx(struct xhci_hcd *xhci,
+static void xhci_free_stream_ctx(struct xhci_hcd *xhci,
 		unsigned int num_stream_ctxs,
 		struct xhci_stream_ctx *stream_ctx, dma_addr_t dma)
 {
@@ -335,7 +335,7 @@ void xhci_free_stream_ctx(struct xhci_hcd *xhci,
  * The stream context array must be a power of 2, and can be as small as
  * 64 bytes or as large as 1MB.
  */
-struct xhci_stream_ctx *xhci_alloc_stream_ctx(struct xhci_hcd *xhci,
+static struct xhci_stream_ctx *xhci_alloc_stream_ctx(struct xhci_hcd *xhci,
 		unsigned int num_stream_ctxs, dma_addr_t *dma,
 		gfp_t mem_flags)
 {
@@ -1900,11 +1900,11 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	val &= DBOFF_MASK;
 	xhci_dbg(xhci, "// Doorbell array is located at offset 0x%x"
 			" from cap regs base addr\n", val);
-	xhci->dba = (void *) xhci->cap_regs + val;
+	xhci->dba = (void __iomem *) xhci->cap_regs + val;
 	xhci_dbg_regs(xhci);
 	xhci_print_run_regs(xhci);
 	/* Set ir_set to interrupt register set 0 */
-	xhci->ir_set = (void *) xhci->run_regs->ir_set;
+	xhci->ir_set = &xhci->run_regs->ir_set[0];
 
 	/*
 	 * Event ring setup: Allocate a normal ring, but also setup
@@ -1961,7 +1961,7 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	/* Set the event ring dequeue address */
 	xhci_set_hc_event_deq(xhci);
 	xhci_dbg(xhci, "Wrote ERST address to ir_set 0.\n");
-	xhci_print_ir_set(xhci, xhci->ir_set, 0);
+	xhci_print_ir_set(xhci, 0);
 
 	/*
 	 * XXX: Might need to set the Interrupter Moderation Register to
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 3e8211c1ce5a..3289bf4832c9 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -474,8 +474,11 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 	state->new_deq_seg = find_trb_seg(cur_td->start_seg,
 			dev->eps[ep_index].stopped_trb,
 			&state->new_cycle_state);
-	if (!state->new_deq_seg)
-		BUG();
+	if (!state->new_deq_seg) {
+		WARN_ON(1);
+		return;
+	}
+
 	/* Dig out the cycle state saved by the xHC during the stop ep cmd */
 	xhci_dbg(xhci, "Finding endpoint context\n");
 	ep_ctx = xhci_get_ep_ctx(xhci, dev->out_ctx, ep_index);
@@ -486,8 +489,10 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 	state->new_deq_seg = find_trb_seg(state->new_deq_seg,
 			state->new_deq_ptr,
 			&state->new_cycle_state);
-	if (!state->new_deq_seg)
-		BUG();
+	if (!state->new_deq_seg) {
+		WARN_ON(1);
+		return;
+	}
 
 	trb = &state->new_deq_ptr->generic;
 	if ((trb->field[3] & TRB_TYPE_BITMASK) == TRB_TYPE(TRB_LINK) &&
@@ -2363,12 +2368,13 @@ static unsigned int count_sg_trbs_needed(struct xhci_hcd *xhci, struct urb *urb)
 
 		/* Scatter gather list entries may cross 64KB boundaries */
 		running_total = TRB_MAX_BUFF_SIZE -
-			(sg_dma_address(sg) & ((1 << TRB_MAX_BUFF_SHIFT) - 1));
+			(sg_dma_address(sg) & (TRB_MAX_BUFF_SIZE - 1));
+		running_total &= TRB_MAX_BUFF_SIZE - 1;
 		if (running_total != 0)
 			num_trbs++;
 
 		/* How many more 64KB chunks to transfer, how many more TRBs? */
-		while (running_total < sg_dma_len(sg)) {
+		while (running_total < sg_dma_len(sg) && running_total < temp) {
 			num_trbs++;
 			running_total += TRB_MAX_BUFF_SIZE;
 		}
@@ -2394,11 +2400,11 @@ static unsigned int count_sg_trbs_needed(struct xhci_hcd *xhci, struct urb *urb)
 static void check_trb_math(struct urb *urb, int num_trbs, int running_total)
 {
 	if (num_trbs != 0)
-		dev_dbg(&urb->dev->dev, "%s - ep %#x - Miscalculated number of "
+		dev_err(&urb->dev->dev, "%s - ep %#x - Miscalculated number of "
 				"TRBs, %d left\n", __func__,
 				urb->ep->desc.bEndpointAddress, num_trbs);
 	if (running_total != urb->transfer_buffer_length)
-		dev_dbg(&urb->dev->dev, "%s - ep %#x - Miscalculated tx length, "
+		dev_err(&urb->dev->dev, "%s - ep %#x - Miscalculated tx length, "
 				"queued %#x (%d), asked for %#x (%d)\n",
 				__func__,
 				urb->ep->desc.bEndpointAddress,
@@ -2533,8 +2539,7 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	sg = urb->sg;
 	addr = (u64) sg_dma_address(sg);
 	this_sg_len = sg_dma_len(sg);
-	trb_buff_len = TRB_MAX_BUFF_SIZE -
-		(addr & ((1 << TRB_MAX_BUFF_SHIFT) - 1));
+	trb_buff_len = TRB_MAX_BUFF_SIZE - (addr & (TRB_MAX_BUFF_SIZE - 1));
 	trb_buff_len = min_t(int, trb_buff_len, this_sg_len);
 	if (trb_buff_len > urb->transfer_buffer_length)
 		trb_buff_len = urb->transfer_buffer_length;
@@ -2572,7 +2577,7 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 				(unsigned int) (addr + TRB_MAX_BUFF_SIZE) & ~(TRB_MAX_BUFF_SIZE - 1),
 				(unsigned int) addr + trb_buff_len);
 		if (TRB_MAX_BUFF_SIZE -
-				(addr & ((1 << TRB_MAX_BUFF_SHIFT) - 1)) < trb_buff_len) {
+				(addr & (TRB_MAX_BUFF_SIZE - 1)) < trb_buff_len) {
 			xhci_warn(xhci, "WARN: sg dma xfer crosses 64KB boundaries!\n");
 			xhci_dbg(xhci, "Next boundary at %#x, end dma = %#x\n",
 					(unsigned int) (addr + TRB_MAX_BUFF_SIZE) & ~(TRB_MAX_BUFF_SIZE - 1),
@@ -2616,7 +2621,7 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		}
 
 		trb_buff_len = TRB_MAX_BUFF_SIZE -
-			(addr & ((1 << TRB_MAX_BUFF_SHIFT) - 1));
+			(addr & (TRB_MAX_BUFF_SIZE - 1));
 		trb_buff_len = min_t(int, trb_buff_len, this_sg_len);
 		if (running_total + trb_buff_len > urb->transfer_buffer_length)
 			trb_buff_len =
@@ -2656,7 +2661,8 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	num_trbs = 0;
 	/* How much data is (potentially) left before the 64KB boundary? */
 	running_total = TRB_MAX_BUFF_SIZE -
-		(urb->transfer_dma & ((1 << TRB_MAX_BUFF_SHIFT) - 1));
+		(urb->transfer_dma & (TRB_MAX_BUFF_SIZE - 1));
+	running_total &= TRB_MAX_BUFF_SIZE - 1;
 
 	/* If there's some data on this 64KB chunk, or we have to send a
 	 * zero-length transfer, we need at least one TRB
@@ -2700,8 +2706,8 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	/* How much data is in the first TRB? */
 	addr = (u64) urb->transfer_dma;
 	trb_buff_len = TRB_MAX_BUFF_SIZE -
-		(urb->transfer_dma & ((1 << TRB_MAX_BUFF_SHIFT) - 1));
-	if (urb->transfer_buffer_length < trb_buff_len)
+		(urb->transfer_dma & (TRB_MAX_BUFF_SIZE - 1));
+	if (trb_buff_len > urb->transfer_buffer_length)
 		trb_buff_len = urb->transfer_buffer_length;
 
 	first_trb = true;
@@ -2879,8 +2885,8 @@ static int count_isoc_trbs_needed(struct xhci_hcd *xhci,
 	addr = (u64) (urb->transfer_dma + urb->iso_frame_desc[i].offset);
 	td_len = urb->iso_frame_desc[i].length;
 
-	running_total = TRB_MAX_BUFF_SIZE -
-			(addr & ((1 << TRB_MAX_BUFF_SHIFT) - 1));
+	running_total = TRB_MAX_BUFF_SIZE - (addr & (TRB_MAX_BUFF_SIZE - 1));
+	running_total &= TRB_MAX_BUFF_SIZE - 1;
 	if (running_total != 0)
 		num_trbs++;
 
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 34cf4e165877..2083fc2179b2 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -109,7 +109,7 @@ int xhci_halt(struct xhci_hcd *xhci)
 /*
  * Set the run bit and wait for the host to be running.
  */
-int xhci_start(struct xhci_hcd *xhci)
+static int xhci_start(struct xhci_hcd *xhci)
 {
 	u32 temp;
 	int ret;
@@ -329,7 +329,7 @@ int xhci_init(struct usb_hcd *hcd)
 
 
 #ifdef CONFIG_USB_XHCI_HCD_DEBUGGING
-void xhci_event_ring_work(unsigned long arg)
+static void xhci_event_ring_work(unsigned long arg)
 {
 	unsigned long flags;
 	int temp;
@@ -473,7 +473,7 @@ int xhci_run(struct usb_hcd *hcd)
 			xhci->ir_set, (unsigned int) ER_IRQ_ENABLE(temp));
 	xhci_writel(xhci, ER_IRQ_ENABLE(temp),
 			&xhci->ir_set->irq_pending);
-	xhci_print_ir_set(xhci, xhci->ir_set, 0);
+	xhci_print_ir_set(xhci, 0);
 
 	if (NUM_TEST_NOOPS > 0)
 		doorbell = xhci_setup_one_noop(xhci);
@@ -528,7 +528,7 @@ void xhci_stop(struct usb_hcd *hcd)
 	temp = xhci_readl(xhci, &xhci->ir_set->irq_pending);
 	xhci_writel(xhci, ER_IRQ_DISABLE(temp),
 			&xhci->ir_set->irq_pending);
-	xhci_print_ir_set(xhci, xhci->ir_set, 0);
+	xhci_print_ir_set(xhci, 0);
 
 	xhci_dbg(xhci, "cleaning up memory\n");
 	xhci_mem_cleanup(xhci);
@@ -755,7 +755,7 @@ int xhci_resume(struct xhci_hcd *xhci, bool hibernated)
 		temp = xhci_readl(xhci, &xhci->ir_set->irq_pending);
 		xhci_writel(xhci, ER_IRQ_DISABLE(temp),
 				&xhci->ir_set->irq_pending);
-		xhci_print_ir_set(xhci, xhci->ir_set, 0);
+		xhci_print_ir_set(xhci, 0);
 
 		xhci_dbg(xhci, "cleaning up memory\n");
 		xhci_mem_cleanup(xhci);
@@ -857,7 +857,7 @@ unsigned int xhci_last_valid_endpoint(u32 added_ctxs)
 /* Returns 1 if the arguments are OK;
  * returns 0 this is a root hub; returns -EINVAL for NULL pointers.
  */
-int xhci_check_args(struct usb_hcd *hcd, struct usb_device *udev,
+static int xhci_check_args(struct usb_hcd *hcd, struct usb_device *udev,
 		struct usb_host_endpoint *ep, int check_ep, bool check_virt_dev,
 		const char *func) {
 	struct xhci_hcd	*xhci;
@@ -1693,7 +1693,7 @@ static void xhci_setup_input_ctx_for_config_ep(struct xhci_hcd *xhci,
 	xhci_dbg_ctx(xhci, in_ctx, xhci_last_valid_endpoint(add_flags));
 }
 
-void xhci_setup_input_ctx_for_quirk(struct xhci_hcd *xhci,
+static void xhci_setup_input_ctx_for_quirk(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		struct xhci_dequeue_state *deq_state)
 {
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 7f236fd22015..7f127df6dd55 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1348,7 +1348,7 @@ static inline int xhci_link_trb_quirk(struct xhci_hcd *xhci)
 }
 
 /* xHCI debugging */
-void xhci_print_ir_set(struct xhci_hcd *xhci, struct xhci_intr_reg *ir_set, int set_num);
+void xhci_print_ir_set(struct xhci_hcd *xhci, int set_num);
 void xhci_print_registers(struct xhci_hcd *xhci);
 void xhci_dbg_regs(struct xhci_hcd *xhci);
 void xhci_print_run_regs(struct xhci_hcd *xhci);
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 54a8bd1047d6..e550f35eff0f 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1530,7 +1530,7 @@ static int __init musb_core_init(u16 musb_type, struct musb *musb)
 
 /*-------------------------------------------------------------------------*/
 
-#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3430) || \
+#if defined(CONFIG_SOC_OMAP2430) || defined(CONFIG_SOC_OMAP3430) || \
 	defined(CONFIG_ARCH_OMAP4) || defined(CONFIG_ARCH_U8500) || \
 	defined(CONFIG_ARCH_U5500)
 
@@ -1864,6 +1864,7 @@ allocate_instance(struct device *dev,
 	INIT_LIST_HEAD(&musb->out_bulk);
 
 	hcd->uses_new_polling = 1;
+	hcd->has_tt = 1;
 
 	musb->vbuserr_retry = VBUSERR_RETRY_COUNT;
 	musb->a_wait_bcon = OTG_TIME_A_WAIT_BCON;
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index d74a8113ae74..3fb617ef9e72 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -212,8 +212,8 @@ enum musb_g_ep0_state {
  * directly with the "flat" model, or after setting up an index register.
  */
 
-#if defined(CONFIG_ARCH_DAVINCI) || defined(CONFIG_ARCH_OMAP2430) \
-		|| defined(CONFIG_ARCH_OMAP3430) || defined(CONFIG_BLACKFIN) \
+#if defined(CONFIG_ARCH_DAVINCI) || defined(CONFIG_SOC_OMAP2430) \
+		|| defined(CONFIG_SOC_OMAP3430) || defined(CONFIG_BLACKFIN) \
 		|| defined(CONFIG_ARCH_OMAP4)
 /* REVISIT indexed access seemed to
  * misbehave (on DaVinci) for at least peripheral IN ...
@@ -358,7 +358,7 @@ struct musb_csr_regs {
 
 struct musb_context_registers {
 
-#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3) || \
+#if defined(CONFIG_SOC_OMAP2430) || defined(CONFIG_ARCH_OMAP3) || \
     defined(CONFIG_ARCH_OMAP4)
 	u32 otg_sysconfig, otg_forcestandby;
 #endif
@@ -488,6 +488,15 @@ struct musb {
 	unsigned		set_address:1;
 	unsigned		test_mode:1;
 	unsigned		softconnect:1;
+
+	u8			address;
+	u8			test_mode_nr;
+	u16			ackpend;		/* ep0 */
+	enum musb_g_ep0_state	ep0_state;
+	struct usb_gadget	g;			/* the gadget */
+	struct usb_gadget_driver *gadget_driver;	/* its driver */
+#endif
+
 	/*
 	 * FIXME: Remove this flag.
 	 *
@@ -501,14 +510,6 @@ struct musb {
 	 */
 	unsigned                double_buffer_not_ok:1 __deprecated;
 
-	u8			address;
-	u8			test_mode_nr;
-	u16			ackpend;		/* ep0 */
-	enum musb_g_ep0_state	ep0_state;
-	struct usb_gadget	g;			/* the gadget */
-	struct usb_gadget_driver *gadget_driver;	/* its driver */
-#endif
-
 	struct musb_hdrc_config	*config;
 
 #ifdef MUSB_CONFIG_PROC_FS
diff --git a/drivers/usb/musb/musbhsdma.h b/drivers/usb/musb/musbhsdma.h
index 21056c924c74..320fd4afb93f 100644
--- a/drivers/usb/musb/musbhsdma.h
+++ b/drivers/usb/musb/musbhsdma.h
@@ -31,7 +31,7 @@
  *
  */
 
-#if defined(CONFIG_ARCH_OMAP2430) || defined(CONFIG_ARCH_OMAP3430)
+#if defined(CONFIG_SOC_OMAP2430) || defined(CONFIG_SOC_OMAP3430)
 #include "omap2430.h"
 #endif
 
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index a3f12333fc41..bc8badd16897 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -362,6 +362,7 @@ static int omap2430_musb_init(struct musb *musb)
 
 static int omap2430_musb_exit(struct musb *musb)
 {
+	del_timer_sync(&musb_idle_timer);
 
 	omap2430_low_level_exit(musb);
 	otg_put_transceiver(musb->xceiv);
diff --git a/drivers/usb/otg/isp1301_omap.c b/drivers/usb/otg/isp1301_omap.c
index e00fa1b22ecd..8c6fdef61d1c 100644
--- a/drivers/usb/otg/isp1301_omap.c
+++ b/drivers/usb/otg/isp1301_omap.c
@@ -1510,7 +1510,7 @@ isp1301_start_hnp(struct otg_transceiver *dev)
 
 /*-------------------------------------------------------------------------*/
 
-static int __init
+static int __devinit
 isp1301_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 {
 	int			status;
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index 79ee6c79ad54..2c1e812b189d 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -301,6 +301,9 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x1199, 0x68A3), 	/* Sierra Wireless Direct IP modems */
 	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
 	},
+	{ USB_DEVICE(0x0f3d, 0x68A3), 	/* Airprime/Sierra Wireless Direct IP modems */
+	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
+	},
        { USB_DEVICE(0x413C, 0x08133) }, /* Dell Computer Corp. Wireless 5720 VZW Mobile Broadband (EVDO Rev-A) Minicard GPS Port */
 
 	{ }
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index b38d77b389d4..aac2ff030332 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -295,12 +295,15 @@ static void usb_wwan_indat_callback(struct urb *urb)
 		    __func__, status, endpoint);
 	} else {
 		tty = tty_port_tty_get(&port->port);
-		if (urb->actual_length) {
-			tty_insert_flip_string(tty, data, urb->actual_length);
-			tty_flip_buffer_push(tty);
-		} else
-			dbg("%s: empty read urb received", __func__);
-		tty_kref_put(tty);
+		if (tty) {
+			if (urb->actual_length) {
+				tty_insert_flip_string(tty, data,
+						urb->actual_length);
+				tty_flip_buffer_push(tty);
+			} else
+				dbg("%s: empty read urb received", __func__);
+			tty_kref_put(tty);
+		}
 
 		/* Resubmit urb so we continue receiving */
 		if (status != -ESHUTDOWN) {
diff --git a/drivers/usb/serial/visor.c b/drivers/usb/serial/visor.c
index 15a5d89b7f39..1c11959a7d58 100644
--- a/drivers/usb/serial/visor.c
+++ b/drivers/usb/serial/visor.c
@@ -27,6 +27,7 @@
 #include <linux/uaccess.h>
 #include <linux/usb.h>
 #include <linux/usb/serial.h>
+#include <linux/usb/cdc.h>
 #include "visor.h"
 
 /*
@@ -479,6 +480,17 @@ static int visor_probe(struct usb_serial *serial,
 
 	dbg("%s", __func__);
 
+	/*
+	 * some Samsung Android phones in modem mode have the same ID
+	 * as SPH-I500, but they are ACM devices, so dont bind to them
+	 */
+	if (id->idVendor == SAMSUNG_VENDOR_ID &&
+		id->idProduct == SAMSUNG_SPH_I500_ID &&
+		serial->dev->descriptor.bDeviceClass == USB_CLASS_COMM &&
+		serial->dev->descriptor.bDeviceSubClass ==
+			USB_CDC_SUBCLASS_ACM)
+		return -ENODEV;
+
 	if (serial->dev->actconfig->desc.bConfigurationValue != 1) {
 		dev_err(&serial->dev->dev, "active config #%d != 1 ??\n",
 			serial->dev->actconfig->desc.bConfigurationValue);
diff --git a/drivers/usb/wusbcore/rh.c b/drivers/usb/wusbcore/rh.c
index a68ad7aa0b59..785772e66ed0 100644
--- a/drivers/usb/wusbcore/rh.c
+++ b/drivers/usb/wusbcore/rh.c
@@ -156,7 +156,7 @@ int wusbhc_rh_status_data(struct usb_hcd *usb_hcd, char *_buf)
 EXPORT_SYMBOL_GPL(wusbhc_rh_status_data);
 
 /*
- * Return the hub's desciptor
+ * Return the hub's descriptor
  *
  * NOTE: almost cut and paste from ehci-hub.c
  *
diff --git a/drivers/vbus/Kconfig b/drivers/vbus/Kconfig
new file mode 100644
index 000000000000..f51cba10913e
--- /dev/null
+++ b/drivers/vbus/Kconfig
@@ -0,0 +1,25 @@
+#
+# Virtual-Bus (VBus) driver configuration
+#
+
+config VBUS_PROXY
+       bool "Virtual-Bus support"
+       select SHM_SIGNAL
+       select IOQ
+       default n
+       help
+       Adds support for a virtual-bus model drivers in a guest to connect
+	to host side virtual-bus resources.  If you are using this kernel
+	in a virtualization solution which implements virtual-bus devices
+	on the backend, say Y.  If unsure, say N.
+
+config VBUS_PCIBRIDGE
+       bool "PCI to Virtual-Bus bridge"
+       depends on PCI
+       depends on VBUS_PROXY
+       select IOQ
+       default n
+       help
+        Provides a way to bridge host side vbus devices via a PCI-BRIDGE
+        object.  If you are running virtualization with vbus devices on the
+	host, and the vbus is exposed via PCI, say Y.  Otherwise, say N.
diff --git a/drivers/vbus/Makefile b/drivers/vbus/Makefile
new file mode 100644
index 000000000000..944b7f1fec90
--- /dev/null
+++ b/drivers/vbus/Makefile
@@ -0,0 +1,6 @@
+
+vbus-proxy-objs += bus-proxy.o
+obj-$(CONFIG_VBUS_PROXY) += vbus-proxy.o
+
+vbus-pcibridge-objs += pci-bridge.o
+obj-$(CONFIG_VBUS_PCIBRIDGE) += vbus-pcibridge.o
diff --git a/drivers/vbus/bus-proxy.c b/drivers/vbus/bus-proxy.c
new file mode 100644
index 000000000000..ae11f679d34e
--- /dev/null
+++ b/drivers/vbus/bus-proxy.c
@@ -0,0 +1,248 @@
+/*
+ * Copyright 2009 Novell.  All Rights Reserved.
+ *
+ * Author:
+ *      Gregory Haskins <ghaskins@novell.com>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/vbus_driver.h>
+
+MODULE_AUTHOR("Gregory Haskins");
+MODULE_LICENSE("GPL");
+
+#define VBUS_PROXY_NAME "vbus-proxy"
+
+static struct vbus_device_proxy *to_dev(struct device *_dev)
+{
+	return _dev ? container_of(_dev, struct vbus_device_proxy, dev) : NULL;
+}
+
+static struct vbus_driver *to_drv(struct device_driver *_drv)
+{
+	return container_of(_drv, struct vbus_driver, drv);
+}
+
+/*
+ * This function is invoked whenever a new driver and/or device is added
+ * to check if there is a match
+ */
+static int vbus_dev_proxy_match(struct device *_dev, struct device_driver *_drv)
+{
+	struct vbus_device_proxy *dev = to_dev(_dev);
+	struct vbus_driver *drv = to_drv(_drv);
+
+	return !strcmp(dev->type, drv->type);
+}
+
+static int vbus_dev_proxy_uevent(struct device *_dev, struct kobj_uevent_env *env)
+{
+	struct vbus_device_proxy *dev = to_dev(_dev);
+
+	if (add_uevent_var(env, "MODALIAS=vbus-proxy:%s", dev->type))
+		return -ENOMEM;
+
+	return 0;
+}
+
+/*
+ * This function is invoked after the bus infrastructure has already made a
+ * match.  The device will contain a reference to the paired driver which
+ * we will extract.
+ */
+static int vbus_dev_proxy_probe(struct device *_dev)
+{
+	int ret = 0;
+	struct vbus_device_proxy *dev = to_dev(_dev);
+	struct vbus_driver *drv = to_drv(_dev->driver);
+
+	if (drv->ops->probe)
+		ret = drv->ops->probe(dev);
+
+	return ret;
+}
+
+static struct bus_type vbus_proxy = {
+	.name   = VBUS_PROXY_NAME,
+	.match  = vbus_dev_proxy_match,
+	.uevent = vbus_dev_proxy_uevent,
+};
+
+static struct device vbus_proxy_rootdev = {
+	.parent    = NULL,
+	.init_name = VBUS_PROXY_NAME,
+};
+
+static int __init vbus_init(void)
+{
+	int ret;
+
+	ret = bus_register(&vbus_proxy);
+	BUG_ON(ret < 0);
+
+	ret = device_register(&vbus_proxy_rootdev);
+	BUG_ON(ret < 0);
+
+	return 0;
+}
+
+postcore_initcall(vbus_init);
+
+static void device_release(struct device *dev)
+{
+	struct vbus_device_proxy *_dev;
+
+	_dev = container_of(dev, struct vbus_device_proxy, dev);
+
+	_dev->ops->release(_dev);
+}
+
+static ssize_t _show_modalias(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "vbus-proxy:%s\n", to_dev(dev)->type);
+}
+static DEVICE_ATTR(modalias, S_IRUSR | S_IRGRP | S_IROTH, _show_modalias, NULL);
+
+int vbus_device_proxy_register(struct vbus_device_proxy *new)
+{
+	int ret;
+
+	new->dev.parent  = &vbus_proxy_rootdev;
+	new->dev.bus     = &vbus_proxy;
+	new->dev.release = &device_release;
+
+	ret = device_register(&new->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = device_create_file(&new->dev, &dev_attr_modalias);
+	if (ret < 0) {
+		device_unregister(&new->dev);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vbus_device_proxy_register);
+
+void vbus_device_proxy_unregister(struct vbus_device_proxy *dev)
+{
+	device_remove_file(&dev->dev, &dev_attr_modalias);
+	device_unregister(&dev->dev);
+}
+EXPORT_SYMBOL_GPL(vbus_device_proxy_unregister);
+
+static int match_device_id(struct device *_dev, void *data)
+{
+	struct vbus_device_proxy *dev = to_dev(_dev);
+	u64 id = *(u64 *)data;
+
+	return dev->id == id;
+}
+
+struct vbus_device_proxy *vbus_device_proxy_find(u64 id)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&vbus_proxy, NULL, &id, &match_device_id);
+
+	return to_dev(dev);
+}
+EXPORT_SYMBOL_GPL(vbus_device_proxy_find);
+
+int vbus_driver_register(struct vbus_driver *new)
+{
+	new->drv.bus   = &vbus_proxy;
+	new->drv.name  = new->type;
+	new->drv.owner = new->owner;
+	new->drv.probe = vbus_dev_proxy_probe;
+
+	return driver_register(&new->drv);
+}
+EXPORT_SYMBOL_GPL(vbus_driver_register);
+
+void vbus_driver_unregister(struct vbus_driver *drv)
+{
+	driver_unregister(&drv->drv);
+}
+EXPORT_SYMBOL_GPL(vbus_driver_unregister);
+
+/*
+ *---------------------------------
+ * driver-side IOQ helper
+ *---------------------------------
+ */
+static void
+vbus_driver_ioq_release(struct ioq *ioq)
+{
+	kfree(ioq->head_desc);
+	kfree(ioq);
+}
+
+static struct ioq_ops vbus_driver_ioq_ops = {
+	.release = vbus_driver_ioq_release,
+};
+
+
+int vbus_driver_ioq_alloc(struct vbus_device_proxy *dev, const char *name,
+			  int id, int prio, size_t count, struct ioq **ioq)
+{
+	struct ioq           *_ioq;
+	struct ioq_ring_head *head = NULL;
+	struct shm_signal    *signal = NULL;
+	size_t                len = IOQ_HEAD_DESC_SIZE(count);
+	int                   ret = -ENOMEM;
+
+	_ioq = kzalloc(sizeof(*_ioq), GFP_KERNEL);
+	if (!_ioq)
+		goto error;
+
+	head = kzalloc(len, GFP_KERNEL | GFP_DMA);
+	if (!head)
+		goto error;
+
+	head->magic     = IOQ_RING_MAGIC;
+	head->ver	= IOQ_RING_VER;
+	head->count     = cpu_to_le32(count);
+
+	ret = dev->ops->shm(dev, name, id, prio, head, len,
+			    &head->signal, &signal, 0);
+	if (ret < 0)
+		goto error;
+
+	ioq_init(_ioq,
+		 &vbus_driver_ioq_ops,
+		 ioq_locality_north,
+		 head,
+		 signal,
+		 count);
+
+	*ioq = _ioq;
+
+	return 0;
+
+ error:
+	kfree(_ioq);
+	kfree(head);
+
+	if (signal)
+		shm_signal_put(signal);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vbus_driver_ioq_alloc);
diff --git a/drivers/vbus/pci-bridge.c b/drivers/vbus/pci-bridge.c
new file mode 100644
index 000000000000..36de7c48891c
--- /dev/null
+++ b/drivers/vbus/pci-bridge.c
@@ -0,0 +1,1016 @@
+/*
+ * Copyright (C) 2009 Novell.  All Rights Reserved.
+ *
+ * Author:
+ *	Gregory Haskins <ghaskins@novell.com>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/mm.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/ioq.h>
+#include <linux/interrupt.h>
+#include <linux/vbus_driver.h>
+#include <linux/vbus_pci.h>
+
+MODULE_AUTHOR("Gregory Haskins");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1");
+
+#define VBUS_PCI_NAME "pci-to-vbus-bridge"
+
+struct vbus_pci {
+	spinlock_t                lock;
+	struct pci_dev           *dev;
+	struct ioq                eventq;
+	struct vbus_pci_event    *ring;
+	struct vbus_pci_regs     *regs;
+	struct vbus_pci_signals  *signals;
+	int                       irq;
+	bool                      enabled;
+	struct {
+		struct dentry    *fs;
+		int               events;
+		int               qnotify;
+		int               qinject;
+		int               notify;
+		int               inject;
+		int               bridgecalls;
+		int               buscalls;
+	} stats;
+};
+
+static struct vbus_pci vbus_pci;
+
+struct vbus_pci_device {
+	char                     type[VBUS_MAX_DEVTYPE_LEN];
+	u64                      handle;
+	struct list_head         shms;
+	struct vbus_device_proxy vdev;
+	struct work_struct       drop;
+};
+
+static DEFINE_PER_CPU(struct vbus_pci_fastcall_desc, vbus_pci_percpu_fastcall)
+____cacheline_aligned;
+
+/*
+ * -------------------
+ * common routines
+ * -------------------
+ */
+
+static int
+vbus_pci_bridgecall(unsigned long nr, void *data, unsigned long len)
+{
+	struct vbus_pci_call_desc params = {
+		.vector = nr,
+		.len    = len,
+		.datap  = __pa(data),
+	};
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&vbus_pci.lock, flags);
+
+	memcpy_toio(&vbus_pci.regs->bridgecall, &params, sizeof(params));
+	ret = ioread32(&vbus_pci.regs->bridgecall);
+
+	spin_unlock_irqrestore(&vbus_pci.lock, flags);
+
+	vbus_pci.stats.bridgecalls++;
+
+	return ret;
+}
+
+static int
+vbus_pci_buscall(unsigned long nr, void *data, unsigned long len)
+{
+	struct vbus_pci_fastcall_desc *params;
+	int ret;
+
+	preempt_disable();
+
+	params = &get_cpu_var(vbus_pci_percpu_fastcall);
+
+	params->call.vector = nr;
+	params->call.len    = len;
+	params->call.datap  = __pa(data);
+
+	iowrite32(smp_processor_id(), &vbus_pci.signals->fastcall);
+
+	ret = params->result;
+
+	preempt_enable();
+
+	vbus_pci.stats.buscalls++;
+
+	return ret;
+}
+
+static struct vbus_pci_device *
+to_dev(struct vbus_device_proxy *vdev)
+{
+	return container_of(vdev, struct vbus_pci_device, vdev);
+}
+
+static void
+_signal_init(struct shm_signal *signal, struct shm_signal_desc *desc,
+	     struct shm_signal_ops *ops)
+{
+	desc->magic = SHM_SIGNAL_MAGIC;
+	desc->ver   = SHM_SIGNAL_VER;
+
+	shm_signal_init(signal, shm_locality_north, ops, desc);
+}
+
+/*
+ * -------------------
+ * _signal
+ * -------------------
+ */
+
+struct _signal {
+	char               name[64];
+	struct vbus_pci   *pcivbus;
+	struct shm_signal  signal;
+	u32                handle;
+	struct rb_node     node;
+	struct list_head   list;
+	int                irq;
+	struct irq_desc   *desc;
+};
+
+static struct _signal *
+to_signal(struct shm_signal *signal)
+{
+       return container_of(signal, struct _signal, signal);
+}
+
+static int
+_signal_inject(struct shm_signal *signal)
+{
+	struct _signal *_signal = to_signal(signal);
+
+	vbus_pci.stats.inject++;
+	iowrite32(_signal->handle, &vbus_pci.signals->shmsignal);
+
+	return 0;
+}
+
+static void
+_signal_release(struct shm_signal *signal)
+{
+	struct _signal *_signal = to_signal(signal);
+
+	kfree(_signal);
+}
+
+static struct shm_signal_ops _signal_ops = {
+	.inject  = _signal_inject,
+	.release = _signal_release,
+};
+
+static void shmsignal_disconnect(struct _signal *_signal);
+
+/*
+ * -------------------
+ * vbus_device_proxy routines
+ * -------------------
+ */
+
+static int
+vbus_pci_device_open(struct vbus_device_proxy *vdev, int version, int flags)
+{
+	struct vbus_pci_device *dev = to_dev(vdev);
+	struct vbus_pci_deviceopen params;
+	int ret;
+
+	if (dev->handle)
+		return -EINVAL;
+
+	params.devid   = vdev->id;
+	params.version = version;
+
+	ret = vbus_pci_buscall(VBUS_PCI_HC_DEVOPEN,
+				 &params, sizeof(params));
+	if (ret < 0)
+		return ret;
+
+	dev->handle = params.handle;
+
+	return 0;
+}
+
+static int
+vbus_pci_device_close(struct vbus_device_proxy *vdev, int flags)
+{
+	struct vbus_pci_device *dev = to_dev(vdev);
+	unsigned long iflags;
+	int ret;
+
+	if (!dev->handle)
+		return -EINVAL;
+
+	spin_lock_irqsave(&vbus_pci.lock, iflags);
+
+	while (!list_empty(&dev->shms)) {
+		struct _signal *_signal;
+
+		_signal = list_first_entry(&dev->shms, struct _signal, list);
+
+		list_del(&_signal->list);
+		shmsignal_disconnect(_signal);
+
+		spin_unlock_irqrestore(&vbus_pci.lock, iflags);
+		shm_signal_put(&_signal->signal);
+		spin_lock_irqsave(&vbus_pci.lock, iflags);
+	}
+
+	spin_unlock_irqrestore(&vbus_pci.lock, iflags);
+
+	/*
+	 * The DEVICECLOSE will implicitly close all of the shm on the
+	 * host-side, so there is no need to do an explicit per-shm
+	 * hypercall
+	 */
+	ret = vbus_pci_buscall(VBUS_PCI_HC_DEVCLOSE,
+				 &dev->handle, sizeof(dev->handle));
+
+	if (ret < 0)
+		printk(KERN_ERR "VBUS-PCI: Error closing device %s/%lld: %d\n",
+		       vdev->type, vdev->id, ret);
+
+	dev->handle = 0;
+
+	return 0;
+}
+
+/*
+ * -------------------
+ * shmsignal interrupt routines
+ * -------------------
+ */
+
+/* We abstract these routines so that we can drop in irqchip later */
+
+static void
+shmsignal_wakeup(struct _signal *_signal)
+{
+	_shm_signal_wakeup(&_signal->signal);
+}
+
+static int
+shmsignal_connect(struct _signal *_signal)
+{
+	return 0;
+}
+
+static void
+shmsignal_disconnect(struct _signal *_signal)
+{
+
+}
+
+static int
+vbus_pci_device_shm(struct vbus_device_proxy *vdev, const char *name,
+		    int id, int prio,
+		    void *ptr, size_t len,
+		    struct shm_signal_desc *sdesc, struct shm_signal **signal,
+		    int flags)
+{
+	struct vbus_pci_device *dev = to_dev(vdev);
+	struct _signal *_signal = NULL;
+	struct vbus_pci_deviceshm params;
+	unsigned long iflags;
+	int ret;
+
+	if (!dev->handle)
+		return -EINVAL;
+
+	params.devh   = dev->handle;
+	params.id     = id;
+	params.flags  = flags;
+	params.datap  = (u64)__pa(ptr);
+	params.len    = len;
+
+	if (signal) {
+		/*
+		 * The signal descriptor must be embedded within the
+		 * provided ptr
+		 */
+		if (!sdesc
+		    || (len < sizeof(*sdesc))
+		    || ((void *)sdesc < ptr)
+		    || ((void *)sdesc > (ptr + len - sizeof(*sdesc))))
+			return -EINVAL;
+
+		_signal = kzalloc(sizeof(*_signal), GFP_KERNEL);
+		if (!_signal)
+			return -ENOMEM;
+
+		_signal_init(&_signal->signal, sdesc, &_signal_ops);
+
+		/*
+		 * take another reference for the host.  This is dropped
+		 * by a SHMCLOSE event
+		 */
+		shm_signal_get(&_signal->signal);
+
+		params.signal.offset = (u64)(unsigned long)sdesc -
+					(u64)(unsigned long)ptr;
+		params.signal.prio   = prio;
+		params.signal.cookie = (u64)(unsigned long)_signal;
+
+	} else
+		params.signal.offset = -1; /* yes, this is a u32, but its ok */
+
+	ret = vbus_pci_buscall(VBUS_PCI_HC_DEVSHM,
+				 &params, sizeof(params));
+	if (ret < 0)
+		goto fail;
+
+	if (signal) {
+
+		BUG_ON(ret < 0);
+
+		_signal->handle = ret;
+
+		if (!name)
+			snprintf(_signal->name, sizeof(_signal->name),
+				 "dev%lld-id%d", vdev->id, id);
+		else
+			snprintf(_signal->name, sizeof(_signal->name),
+				 "%s", name);
+
+		shmsignal_connect(_signal);
+
+		spin_lock_irqsave(&vbus_pci.lock, iflags);
+		list_add_tail(&_signal->list, &dev->shms);
+		spin_unlock_irqrestore(&vbus_pci.lock, iflags);
+
+		shm_signal_get(&_signal->signal);
+		*signal = &_signal->signal;
+	}
+
+	return 0;
+
+fail:
+	if (_signal) {
+		/*
+		 * We held two references above, so we need to drop
+		 * both of them
+		 */
+		shm_signal_put(&_signal->signal);
+		shm_signal_put(&_signal->signal);
+	}
+
+	return ret;
+}
+
+static int
+vbus_pci_device_call(struct vbus_device_proxy *vdev, u32 func, void *data,
+		     size_t len, int flags)
+{
+	struct vbus_pci_device *dev = to_dev(vdev);
+	struct vbus_pci_devicecall params = {
+		.devh  = dev->handle,
+		.func  = func,
+		.datap = (u64)__pa(data),
+		.len   = len,
+		.flags = flags,
+	};
+
+	if (!dev->handle)
+		return -EINVAL;
+
+	return vbus_pci_buscall(VBUS_PCI_HC_DEVCALL, &params, sizeof(params));
+}
+
+static void
+vbus_pci_device_release(struct vbus_device_proxy *vdev)
+{
+	struct vbus_pci_device *_dev = to_dev(vdev);
+
+	vbus_pci_device_close(vdev, 0);
+
+	kfree(_dev);
+}
+
+static struct vbus_device_proxy_ops vbus_pci_device_ops = {
+	.open    = vbus_pci_device_open,
+	.close   = vbus_pci_device_close,
+	.shm     = vbus_pci_device_shm,
+	.call    = vbus_pci_device_call,
+	.release = vbus_pci_device_release,
+};
+
+/*
+ * -------------------
+ * vbus events
+ * -------------------
+ */
+
+struct deferred_devadd_event {
+	struct work_struct        work;
+	struct vbus_pci_add_event event;
+};
+
+static void deferred_devdrop(struct work_struct *work);
+
+static void
+deferred_devadd(struct work_struct *work)
+{
+	struct deferred_devadd_event *_event;
+	struct vbus_pci_device *new;
+	int ret;
+
+	_event = container_of(work, struct deferred_devadd_event, work);
+
+	new = kzalloc(sizeof(*new), GFP_KERNEL);
+	if (!new) {
+		printk(KERN_ERR "VBUS_PCI: Out of memory on add_event\n");
+		return;
+	}
+
+	INIT_LIST_HEAD(&new->shms);
+
+	memcpy(new->type, _event->event.type, VBUS_MAX_DEVTYPE_LEN);
+	new->vdev.type        = new->type;
+	new->vdev.id          = _event->event.id;
+	new->vdev.ops         = &vbus_pci_device_ops;
+
+	dev_set_name(&new->vdev.dev, "%lld", _event->event.id);
+
+	INIT_WORK(&new->drop, deferred_devdrop);
+
+	ret = vbus_device_proxy_register(&new->vdev);
+	if (ret < 0)
+		panic("failed to register device %lld(%s): %d\n",
+		      new->vdev.id, new->type, ret);
+
+	kfree(_event);
+}
+
+static void
+deferred_devdrop(struct work_struct *work)
+{
+	struct vbus_pci_device *dev;
+
+	dev = container_of(work, struct vbus_pci_device, drop);
+	vbus_device_proxy_unregister(&dev->vdev);
+}
+
+static void
+event_devadd(struct vbus_pci_add_event *event)
+{
+	struct deferred_devadd_event *_event;
+
+	_event = kzalloc(sizeof(*_event), GFP_ATOMIC);
+	if (!_event) {
+		printk(KERN_ERR \
+		       "VBUS_PCI: Out of ATOMIC memory on add_event\n");
+		return;
+	}
+
+	INIT_WORK(&_event->work, deferred_devadd);
+	memcpy(&_event->event, event, sizeof(*event));
+
+	schedule_work(&_event->work);
+}
+
+static void
+event_devdrop(struct vbus_pci_handle_event *event)
+{
+	struct vbus_device_proxy *dev = vbus_device_proxy_find(event->handle);
+
+	if (!dev) {
+		printk(KERN_WARNING "VBUS-PCI: devdrop failed: %lld\n",
+		       event->handle);
+		return;
+	}
+
+	schedule_work(&to_dev(dev)->drop);
+}
+
+static void
+event_shmsignal(struct vbus_pci_handle_event *event)
+{
+	struct _signal *_signal = (struct _signal *)(unsigned long)event->handle;
+
+	vbus_pci.stats.notify++;
+
+	shmsignal_wakeup(_signal);
+}
+
+static void
+event_shmclose(struct vbus_pci_handle_event *event)
+{
+	struct _signal *_signal = (struct _signal *)(unsigned long)event->handle;
+
+	/*
+	 * This reference was taken during the DEVICESHM call
+	 */
+	shm_signal_put(&_signal->signal);
+}
+
+/*
+ * -------------------
+ * eventq routines
+ * -------------------
+ */
+
+static struct ioq_notifier eventq_notifier;
+
+static int __devinit
+eventq_init(int qlen)
+{
+	struct ioq_iterator iter;
+	int ret;
+	int i;
+
+	vbus_pci.ring = kzalloc(sizeof(struct vbus_pci_event) * qlen,
+				GFP_KERNEL);
+	if (!vbus_pci.ring)
+		return -ENOMEM;
+
+	/*
+	 * We want to iterate on the "valid" index.  By default the iterator
+	 * will not "autoupdate" which means it will not hypercall the host
+	 * with our changes.  This is good, because we are really just
+	 * initializing stuff here anyway.  Note that you can always manually
+	 * signal the host with ioq_signal() if the autoupdate feature is not
+	 * used.
+	 */
+	ret = ioq_iter_init(&vbus_pci.eventq, &iter, ioq_idxtype_valid, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * Seek to the tail of the valid index (which should be our first
+	 * item since the queue is brand-new)
+	 */
+	ret = ioq_iter_seek(&iter, ioq_seek_tail, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * Now populate each descriptor with an empty vbus_event and mark it
+	 * valid
+	 */
+	for (i = 0; i < qlen; i++) {
+		struct vbus_pci_event *event = &vbus_pci.ring[i];
+		size_t                 len   = sizeof(*event);
+		struct ioq_ring_desc  *desc  = iter.desc;
+
+		BUG_ON(iter.desc->valid);
+
+		desc->cookie = (u64)(unsigned long)event;
+		desc->ptr    = cpu_to_le64(__pa(event));
+		desc->len    = cpu_to_le64(len); /* total length */
+		desc->valid  = 1;
+
+		/*
+		 * This push operation will simultaneously advance the
+		 * valid-tail index and increment our position in the queue
+		 * by one.
+		 */
+		ret = ioq_iter_push(&iter, 0);
+		BUG_ON(ret < 0);
+	}
+
+	vbus_pci.eventq.notifier = &eventq_notifier;
+
+	/*
+	 * And finally, ensure that we can receive notification
+	 */
+	ioq_notify_enable(&vbus_pci.eventq, 0);
+
+	return 0;
+}
+
+/* Invoked whenever the hypervisor ioq_signal()s our eventq */
+static void
+eventq_wakeup(struct ioq_notifier *notifier)
+{
+	struct ioq_iterator iter;
+	int ret;
+
+	/* We want to iterate on the head of the in-use index */
+	ret = ioq_iter_init(&vbus_pci.eventq, &iter, ioq_idxtype_inuse, 0);
+	BUG_ON(ret < 0);
+
+	ret = ioq_iter_seek(&iter, ioq_seek_head, 0, 0);
+	BUG_ON(ret < 0);
+
+	/*
+	 * The EOM is indicated by finding a packet that is still owned by
+	 * the south side.
+	 *
+	 * FIXME: This in theory could run indefinitely if the host keeps
+	 * feeding us events since there is nothing like a NAPI budget.  We
+	 * might need to address that
+	 */
+	while (!iter.desc->sown) {
+		struct ioq_ring_desc *desc  = iter.desc;
+		struct vbus_pci_event *event;
+
+		event = (struct vbus_pci_event *)(unsigned long)desc->cookie;
+
+		switch (event->eventid) {
+		case VBUS_PCI_EVENT_DEVADD:
+			event_devadd(&event->data.add);
+			break;
+		case VBUS_PCI_EVENT_DEVDROP:
+			event_devdrop(&event->data.handle);
+			break;
+		case VBUS_PCI_EVENT_SHMSIGNAL:
+			event_shmsignal(&event->data.handle);
+			break;
+		case VBUS_PCI_EVENT_SHMCLOSE:
+			event_shmclose(&event->data.handle);
+			break;
+		default:
+			printk(KERN_WARNING "VBUS_PCI: Unexpected event %d\n",
+			       event->eventid);
+			break;
+		};
+
+		memset(event, 0, sizeof(*event));
+
+		/* Advance the in-use head */
+		ret = ioq_iter_pop(&iter, 0);
+		BUG_ON(ret < 0);
+
+		vbus_pci.stats.events++;
+	}
+
+	/* And let the south side know that we changed the queue */
+	ioq_signal(&vbus_pci.eventq, 0);
+}
+
+static struct ioq_notifier eventq_notifier = {
+	.signal = &eventq_wakeup,
+};
+
+/* Injected whenever the host issues an ioq_signal() on the eventq */
+static irqreturn_t
+eventq_intr(int irq, void *dev)
+{
+	vbus_pci.stats.qnotify++;
+	_shm_signal_wakeup(vbus_pci.eventq.signal);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * -------------------
+ */
+
+static int
+eventq_signal_inject(struct shm_signal *signal)
+{
+	vbus_pci.stats.qinject++;
+
+	/* The eventq uses the special-case handle=0 */
+	iowrite32(0, &vbus_pci.signals->eventq);
+
+	return 0;
+}
+
+static void
+eventq_signal_release(struct shm_signal *signal)
+{
+	kfree(signal);
+}
+
+static struct shm_signal_ops eventq_signal_ops = {
+	.inject  = eventq_signal_inject,
+	.release = eventq_signal_release,
+};
+
+/*
+ * -------------------
+ */
+
+static void
+eventq_ioq_release(struct ioq *ioq)
+{
+	/* released as part of the vbus_pci object */
+}
+
+static struct ioq_ops eventq_ioq_ops = {
+	.release = eventq_ioq_release,
+};
+
+/*
+ * -------------------
+ */
+
+static void
+vbus_pci_release(void)
+{
+#ifdef CONFIG_DEBUG_FS
+	if (vbus_pci.stats.fs)
+		debugfs_remove(vbus_pci.stats.fs);
+#endif
+
+	if (vbus_pci.irq > 0)
+		free_irq(vbus_pci.irq, NULL);
+
+	if (vbus_pci.signals)
+		pci_iounmap(vbus_pci.dev, (void *)vbus_pci.signals);
+
+	if (vbus_pci.regs)
+		pci_iounmap(vbus_pci.dev, (void *)vbus_pci.regs);
+
+	pci_release_regions(vbus_pci.dev);
+	pci_disable_device(vbus_pci.dev);
+
+	kfree(vbus_pci.eventq.head_desc);
+	kfree(vbus_pci.ring);
+
+	vbus_pci.enabled = false;
+}
+
+static int __devinit
+vbus_pci_open(void)
+{
+	struct vbus_pci_bridge_negotiate params = {
+		.magic        = VBUS_PCI_ABI_MAGIC,
+		.version      = VBUS_PCI_HC_VERSION,
+		.capabilities = 0,
+	};
+
+	return vbus_pci_bridgecall(VBUS_PCI_BRIDGE_NEGOTIATE,
+				  &params, sizeof(params));
+}
+
+#define QLEN 1024
+
+static int __devinit
+vbus_pci_eventq_register(void)
+{
+	struct vbus_pci_busreg params = {
+		.count = 1,
+		.eventq = {
+			{
+				.count = QLEN,
+				.ring  = (u64)__pa(vbus_pci.eventq.head_desc),
+				.data  = (u64)__pa(vbus_pci.ring),
+			},
+		},
+	};
+
+	return vbus_pci_bridgecall(VBUS_PCI_BRIDGE_QREG,
+				   &params, sizeof(params));
+}
+
+static int __devinit
+_ioq_init(size_t ringsize, struct ioq *ioq, struct ioq_ops *ops)
+{
+	struct shm_signal    *signal = NULL;
+	struct ioq_ring_head *head = NULL;
+	size_t                len  = IOQ_HEAD_DESC_SIZE(ringsize);
+
+	head = kzalloc(len, GFP_KERNEL | GFP_DMA);
+	if (!head)
+		return -ENOMEM;
+
+	signal = kzalloc(sizeof(*signal), GFP_KERNEL);
+	if (!signal) {
+		kfree(head);
+		return -ENOMEM;
+	}
+
+	head->magic     = IOQ_RING_MAGIC;
+	head->ver	= IOQ_RING_VER;
+	head->count     = cpu_to_le32(ringsize);
+
+	_signal_init(signal, &head->signal, &eventq_signal_ops);
+
+	ioq_init(ioq, ops, ioq_locality_north, head, signal, ringsize);
+
+	return 0;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int _debugfs_seq_show(struct seq_file *m, void *p)
+{
+#define P(F) \
+	seq_printf(m, "  .%-30s: %d\n", #F, (int)vbus_pci.stats.F)
+
+	P(events);
+	P(qnotify);
+	P(qinject);
+	P(notify);
+	P(inject);
+	P(bridgecalls);
+	P(buscalls);
+
+#undef P
+
+	return 0;
+}
+
+static int _debugfs_fops_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, _debugfs_seq_show, inode->i_private);
+}
+
+static const struct file_operations stat_fops = {
+	.open		= _debugfs_fops_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+	.owner		= THIS_MODULE,
+};
+#endif
+
+static int __devinit
+vbus_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int ret;
+	int cpu;
+
+	if (vbus_pci.enabled)
+		return -EEXIST; /* we only support one bridge per kernel */
+
+	if (pdev->revision != VBUS_PCI_ABI_VERSION) {
+		printk(KERN_DEBUG "VBUS_PCI: expected ABI version %d, got %d\n",
+		       VBUS_PCI_ABI_VERSION,
+		       pdev->revision);
+		return -ENODEV;
+	}
+
+	vbus_pci.dev = pdev;
+
+	ret = pci_enable_device(pdev);
+	if (ret < 0)
+		return ret;
+
+	pci_set_master(pdev);
+
+	ret = pci_request_regions(pdev, VBUS_PCI_NAME);
+	if (ret < 0) {
+		printk(KERN_ERR "VBUS_PCI: Could not init BARs: %d\n", ret);
+		goto out_fail;
+	}
+
+	vbus_pci.regs = pci_iomap(pdev, 0, sizeof(struct vbus_pci_regs));
+	if (!vbus_pci.regs) {
+		printk(KERN_ERR "VBUS_PCI: Could not map BARs\n");
+		goto out_fail;
+	}
+
+	vbus_pci.signals = pci_iomap(pdev, 1, sizeof(struct vbus_pci_signals));
+	if (!vbus_pci.signals) {
+		printk(KERN_ERR "VBUS_PCI: Could not map BARs\n");
+		goto out_fail;
+	}
+
+	ret = vbus_pci_open();
+	if (ret < 0) {
+		printk(KERN_DEBUG "VBUS_PCI: Could not register with host: %d\n",
+		       ret);
+		goto out_fail;
+	}
+
+	/*
+	 * Allocate an IOQ to use for host-2-guest event notification
+	 */
+	ret = _ioq_init(QLEN, &vbus_pci.eventq, &eventq_ioq_ops);
+	if (ret < 0) {
+		printk(KERN_ERR "VBUS_PCI: Cound not init eventq: %d\n", ret);
+		goto out_fail;
+	}
+
+	ret = eventq_init(QLEN);
+	if (ret < 0) {
+		printk(KERN_ERR "VBUS_PCI: Cound not setup ring: %d\n", ret);
+		goto out_fail;
+	}
+
+	ret = pci_enable_msi(pdev);
+	if (ret < 0) {
+		printk(KERN_ERR "VBUS_PCI: Cound not enable MSI: %d\n", ret);
+		goto out_fail;
+	}
+
+	vbus_pci.irq = pdev->irq;
+
+	ret = request_irq(pdev->irq, eventq_intr, 0, "vbus", NULL);
+	if (ret < 0) {
+		printk(KERN_ERR "VBUS_PCI: Failed to register IRQ %d\n: %d",
+		       pdev->irq, ret);
+		goto out_fail;
+	}
+
+	/*
+	 * Add one fastcall vector per cpu so that we can do lockless
+	 * hypercalls
+	 */
+	for_each_possible_cpu(cpu) {
+		struct vbus_pci_fastcall_desc *desc =
+			&per_cpu(vbus_pci_percpu_fastcall, cpu);
+		struct vbus_pci_call_desc params = {
+			.vector = cpu,
+			.len    = sizeof(*desc),
+			.datap  = __pa(desc),
+		};
+
+		ret = vbus_pci_bridgecall(VBUS_PCI_BRIDGE_FASTCALL_ADD,
+					  &params, sizeof(params));
+		if (ret < 0) {
+			printk(KERN_ERR \
+			       "VBUS_PCI: Failed to register cpu:%d\n: %d",
+			       cpu, ret);
+			goto out_fail;
+		}
+	}
+
+	/*
+	 * Finally register our queue on the host to start receiving events
+	 */
+	ret = vbus_pci_eventq_register();
+	if (ret < 0) {
+		printk(KERN_ERR "VBUS_PCI: Could not register with host: %d\n",
+		       ret);
+		goto out_fail;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	vbus_pci.stats.fs = debugfs_create_file(VBUS_PCI_NAME, S_IRUGO,
+						NULL, NULL, &stat_fops);
+	if (IS_ERR(vbus_pci.stats.fs)) {
+		ret = PTR_ERR(vbus_pci.stats.fs);
+		printk(KERN_ERR "VBUS_PCI: error creating stats-fs: %d\n", ret);
+		goto out_fail;
+	}
+#endif
+
+	vbus_pci.enabled = true;
+
+	printk(KERN_INFO "Virtual-Bus: Copyright (c) 2009, " \
+	       "Gregory Haskins <ghaskins@novell.com>\n");
+
+	return 0;
+
+ out_fail:
+	vbus_pci_release();
+
+	return ret;
+}
+
+static void __devexit
+vbus_pci_remove(struct pci_dev *pdev)
+{
+	vbus_pci_release();
+}
+
+static DEFINE_PCI_DEVICE_TABLE(vbus_pci_tbl) = {
+	{ PCI_DEVICE(0x11da, 0x2000) },
+	{ 0 },
+};
+
+MODULE_DEVICE_TABLE(pci, vbus_pci_tbl);
+
+static struct pci_driver vbus_pci_driver = {
+	.name     = VBUS_PCI_NAME,
+	.id_table = vbus_pci_tbl,
+	.probe    = vbus_pci_probe,
+	.remove   = vbus_pci_remove,
+};
+
+static int __init
+vbus_pci_init(void)
+{
+	memset(&vbus_pci, 0, sizeof(vbus_pci));
+	spin_lock_init(&vbus_pci.lock);
+
+	return pci_register_driver(&vbus_pci_driver);
+}
+
+static void __exit
+vbus_pci_exit(void)
+{
+	pci_unregister_driver(&vbus_pci_driver);
+}
+
+module_init(vbus_pci_init);
+module_exit(vbus_pci_exit);
+
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 6bafb51bb437..eaa5ed7de8d9 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -322,69 +322,6 @@ config FB_ARMCLCD
 	  here and read <file:Documentation/kbuild/modules.txt>.  The module
 	  will be called amba-clcd.
 
-choice
-
-	depends on FB_ARMCLCD && (ARCH_LH7A40X || ARCH_LH7952X)
-	prompt "LCD Panel"
-	default FB_ARMCLCD_SHARP_LQ035Q7DB02
-
-config FB_ARMCLCD_SHARP_LQ035Q7DB02_HRTFT
-	bool "LogicPD LCD 3.5\" QVGA w/HRTFT IC"
-	help
-	  This is an implementation of the Sharp LQ035Q7DB02, a 3.5"
-	  color QVGA, HRTFT panel.  The LogicPD device includes
-	  an integrated HRTFT controller IC.
-	  The native resolution is 240x320.
-
-config FB_ARMCLCD_SHARP_LQ057Q3DC02
-	bool "LogicPD LCD 5.7\" QVGA"
-	help
-	  This is an implementation of the Sharp LQ057Q3DC02, a 5.7"
-	  color QVGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 320x240.
-
-config FB_ARMCLCD_SHARP_LQ64D343
-	bool "LogicPD LCD 6.4\" VGA"
-	help
-	  This is an implementation of the Sharp LQ64D343, a 6.4"
-	  color VGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 640x480.
-
-config FB_ARMCLCD_SHARP_LQ10D368
-	bool "LogicPD LCD 10.4\" VGA"
-	help
-	  This is an implementation of the Sharp LQ10D368, a 10.4"
-	  color VGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 640x480.
-
-
-config FB_ARMCLCD_SHARP_LQ121S1DG41
-	bool "LogicPD LCD 12.1\" SVGA"
-	help
-	  This is an implementation of the Sharp LQ121S1DG41, a 12.1"
-	  color SVGA, TFT panel.  The LogicPD device includes an
-	  The native resolution is 800x600.
-
-	  This panel requires a clock rate may be an integer fraction
-	  of the base LCDCLK frequency.  The driver will select the
-	  highest frequency available that is lower than the maximum
-	  allowed.  The panel may flicker if the clock rate is
-	  slower than the recommended minimum.
-
-config FB_ARMCLCD_AUO_A070VW01_WIDE
-	bool "AU Optronics A070VW01 LCD 7.0\" WIDE"
-	help
-	  This is an implementation of the AU Optronics, a 7.0"
-	  WIDE Color.  The native resolution is 234x480.
-
-config FB_ARMCLCD_HITACHI
-	bool "Hitachi Wide Screen 800x480"
-	help
-	  This is an implementation of the Hitachi 800x480.
-
-endchoice
-
-
 config FB_ACORN
 	bool "Acorn VIDC support"
 	depends on (FB = y) && ARM && ARCH_ACORN
@@ -439,6 +376,24 @@ config FB_CYBER2000
 	  Say Y if you have a NetWinder or a graphics card containing this
 	  device, otherwise say N.
 
+config FB_CYBER2000_DDC
+	bool "DDC for CyberPro support"
+	depends on FB_CYBER2000
+	select FB_DDC
+	default y
+	help
+	  Say Y here if you want DDC support for your CyberPro graphics
+	  card. This is only I2C bus support, driver does not use EDID.
+
+config FB_CYBER2000_I2C
+	bool "CyberPro 2000/2010/5000 I2C support"
+	depends on FB_CYBER2000 && I2C && ARCH_NETWINDER
+	select I2C_ALGOBIT
+	help
+	  Enable support for the I2C video decoder interface on the
+	  Integraphics CyberPro 20x0 and 5000 VGA chips.  This is used
+	  on the Netwinder machines for the SAA7111 video capture.
+
 config FB_APOLLO
 	bool
 	depends on (FB = y) && APOLLO
@@ -2365,6 +2320,26 @@ config FB_JZ4740
 	help
 	  Framebuffer support for the JZ4740 SoC.
 
+config FB_MXS
+	tristate "MXS LCD framebuffer support"
+	depends on FB && ARCH_MXS
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  Framebuffer support for the MXS SoC.
+
+config FB_PUV3_UNIGFX
+	tristate "PKUnity v3 Unigfx framebuffer support"
+	depends on FB && UNICORE32 && ARCH_PUV3
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	help
+	  Choose this option if you want to use the Unigfx device as a
+	  framebuffer device. Without the support of PCI & AGP.
+
 source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
 
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 8c8fabdff9d0..2ea44b6625fe 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -139,6 +139,7 @@ obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
 obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
 obj-$(CONFIG_FB_JZ4740)		  += jz4740_fb.o
+obj-$(CONFIG_FB_PUV3_UNIGFX)      += fb-puv3.o
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
@@ -153,6 +154,7 @@ obj-$(CONFIG_FB_BFIN_T350MCQB)	  += bfin-t350mcqb-fb.o
 obj-$(CONFIG_FB_BFIN_7393)        += bfin_adv7393fb.o
 obj-$(CONFIG_FB_MX3)		  += mx3fb.o
 obj-$(CONFIG_FB_DA8XX)		  += da8xx-fb.o
+obj-$(CONFIG_FB_MXS)		  += mxsfb.o
 
 # the test framebuffer is last
 obj-$(CONFIG_FB_VIRTUAL)          += vfb.o
diff --git a/drivers/video/amba-clcd.c b/drivers/video/amba-clcd.c
index 1c2c68356ea7..5fc983c5b92c 100644
--- a/drivers/video/amba-clcd.c
+++ b/drivers/video/amba-clcd.c
@@ -120,8 +120,23 @@ static void clcdfb_enable(struct clcd_fb *fb, u32 cntl)
 static int
 clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 {
+	u32 caps;
 	int ret = 0;
 
+	if (fb->panel->caps && fb->board->caps)
+		caps = fb->panel->caps & fb->board->caps;
+	else {
+		/* Old way of specifying what can be used */
+		caps = fb->panel->cntl & CNTL_BGR ?
+			CLCD_CAP_BGR : CLCD_CAP_RGB;
+		/* But mask out 444 modes as they weren't supported */
+		caps &= ~CLCD_CAP_444;
+	}
+
+	/* Only TFT panels can do RGB888/BGR888 */
+	if (!(fb->panel->cntl & CNTL_LCDTFT))
+		caps &= ~CLCD_CAP_888;
+
 	memset(&var->transp, 0, sizeof(var->transp));
 
 	var->red.msb_right = 0;
@@ -133,6 +148,13 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 	case 2:
 	case 4:
 	case 8:
+		/* If we can't do 5551, reject */
+		caps &= CLCD_CAP_5551;
+		if (!caps) {
+			ret = -EINVAL;
+			break;
+		}
+
 		var->red.length		= var->bits_per_pixel;
 		var->red.offset		= 0;
 		var->green.length	= var->bits_per_pixel;
@@ -140,23 +162,61 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 		var->blue.length	= var->bits_per_pixel;
 		var->blue.offset	= 0;
 		break;
+
 	case 16:
-		var->red.length = 5;
-		var->blue.length = 5;
+		/* If we can't do 444, 5551 or 565, reject */
+		if (!(caps & (CLCD_CAP_444 | CLCD_CAP_5551 | CLCD_CAP_565))) {
+			ret = -EINVAL;
+			break;
+		}
+
 		/*
-		 * Green length can be 5 or 6 depending whether
-		 * we're operating in RGB555 or RGB565 mode.
+		 * Green length can be 4, 5 or 6 depending whether
+		 * we're operating in 444, 5551 or 565 mode.
 		 */
-		if (var->green.length != 5 && var->green.length != 6)
-			var->green.length = 6;
+		if (var->green.length == 4 && caps & CLCD_CAP_444)
+			caps &= CLCD_CAP_444;
+		if (var->green.length == 5 && caps & CLCD_CAP_5551)
+			caps &= CLCD_CAP_5551;
+		else if (var->green.length == 6 && caps & CLCD_CAP_565)
+			caps &= CLCD_CAP_565;
+		else {
+			/*
+			 * PL110 officially only supports RGB555,
+			 * but may be wired up to allow RGB565.
+			 */
+			if (caps & CLCD_CAP_565) {
+				var->green.length = 6;
+				caps &= CLCD_CAP_565;
+			} else if (caps & CLCD_CAP_5551) {
+				var->green.length = 5;
+				caps &= CLCD_CAP_5551;
+			} else {
+				var->green.length = 4;
+				caps &= CLCD_CAP_444;
+			}
+		}
+
+		if (var->green.length >= 5) {
+			var->red.length = 5;
+			var->blue.length = 5;
+		} else {
+			var->red.length = 4;
+			var->blue.length = 4;
+		}
 		break;
 	case 32:
-		if (fb->panel->cntl & CNTL_LCDTFT) {
-			var->red.length		= 8;
-			var->green.length	= 8;
-			var->blue.length	= 8;
+		/* If we can't do 888, reject */
+		caps &= CLCD_CAP_888;
+		if (!caps) {
+			ret = -EINVAL;
 			break;
 		}
+
+		var->red.length = 8;
+		var->green.length = 8;
+		var->blue.length = 8;
+		break;
 	default:
 		ret = -EINVAL;
 		break;
@@ -168,7 +228,20 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 	 * the bitfield length defined above.
 	 */
 	if (ret == 0 && var->bits_per_pixel >= 16) {
-		if (fb->panel->cntl & CNTL_BGR) {
+		bool bgr, rgb;
+
+		bgr = caps & CLCD_CAP_BGR && var->blue.offset == 0;
+		rgb = caps & CLCD_CAP_RGB && var->red.offset == 0;
+
+		if (!bgr && !rgb)
+			/*
+			 * The requested format was not possible, try just
+			 * our capabilities.  One of BGR or RGB must be
+			 * supported.
+			 */
+			bgr = caps & CLCD_CAP_BGR;
+
+		if (bgr) {
 			var->blue.offset = 0;
 			var->green.offset = var->blue.offset + var->blue.length;
 			var->red.offset = var->green.offset + var->green.length;
@@ -443,8 +516,8 @@ static int clcdfb_register(struct clcd_fb *fb)
 
 	fb_set_var(&fb->fb, &fb->fb.var);
 
-        printk(KERN_INFO "CLCD: %s hardware, %s display\n",
-               fb->board->name, fb->panel->mode.name);
+	dev_info(&fb->dev->dev, "%s hardware, %s display\n",
+	         fb->board->name, fb->panel->mode.name);
 
 	ret = register_framebuffer(&fb->fb);
 	if (ret == 0)
@@ -461,7 +534,7 @@ static int clcdfb_register(struct clcd_fb *fb)
 	return ret;
 }
 
-static int clcdfb_probe(struct amba_device *dev, struct amba_id *id)
+static int clcdfb_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct clcd_board *board = dev->dev.platform_data;
 	struct clcd_fb *fb;
@@ -486,6 +559,10 @@ static int clcdfb_probe(struct amba_device *dev, struct amba_id *id)
 	fb->dev = dev;
 	fb->board = board;
 
+	dev_info(&fb->dev->dev, "PL%03x rev%u at 0x%08llx\n",
+		amba_part(dev), amba_rev(dev),
+		(unsigned long long)dev->res.start);
+
 	ret = fb->board->setup(fb);
 	if (ret)
 		goto free_fb;
diff --git a/drivers/video/backlight/ltv350qv.c b/drivers/video/backlight/ltv350qv.c
index 8010aaeb5adb..dd0e84a9bd2f 100644
--- a/drivers/video/backlight/ltv350qv.c
+++ b/drivers/video/backlight/ltv350qv.c
@@ -239,11 +239,15 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 	lcd->spi = spi;
 	lcd->power = FB_BLANK_POWERDOWN;
 	lcd->buffer = kzalloc(8, GFP_KERNEL);
+	if (!lcd->buffer) {
+		ret = -ENOMEM;
+		goto out_free_lcd;
+	}
 
 	ld = lcd_device_register("ltv350qv", &spi->dev, lcd, &ltv_ops);
 	if (IS_ERR(ld)) {
 		ret = PTR_ERR(ld);
-		goto out_free_lcd;
+		goto out_free_buffer;
 	}
 	lcd->ld = ld;
 
@@ -257,6 +261,8 @@ static int __devinit ltv350qv_probe(struct spi_device *spi)
 
 out_unregister:
 	lcd_device_unregister(ld);
+out_free_buffer:
+	kfree(lcd->buffer);
 out_free_lcd:
 	kfree(lcd);
 	return ret;
@@ -268,6 +274,7 @@ static int __devexit ltv350qv_remove(struct spi_device *spi)
 
 	ltv350qv_power(lcd, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(lcd->ld);
+	kfree(lcd->buffer);
 	kfree(lcd);
 
 	return 0;
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 4dc13467281d..7ba74cd4be61 100644
--- a/drivers/video/bw2.c
+++ b/drivers/video/bw2.c
@@ -273,7 +273,7 @@ static int __devinit bw2_do_default_mode(struct bw2_par *par,
 	return 0;
 }
 
-static int __devinit bw2_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit bw2_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -375,7 +375,7 @@ static const struct of_device_id bw2_match[] = {
 };
 MODULE_DEVICE_TABLE(of, bw2_match);
 
-static struct of_platform_driver bw2_driver = {
+static struct platform_driver bw2_driver = {
 	.driver = {
 		.name = "bw2",
 		.owner = THIS_MODULE,
@@ -390,12 +390,12 @@ static int __init bw2_init(void)
 	if (fb_get_options("bw2fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&bw2_driver);
+	return platform_driver_register(&bw2_driver);
 }
 
 static void __exit bw2_exit(void)
 {
-	of_unregister_platform_driver(&bw2_driver);
+	platform_driver_unregister(&bw2_driver);
 }
 
 module_init(bw2_init);
diff --git a/drivers/video/cg14.c b/drivers/video/cg14.c
index 24249535ac86..e2c85b0db632 100644
--- a/drivers/video/cg14.c
+++ b/drivers/video/cg14.c
@@ -463,7 +463,7 @@ static void cg14_unmap_regs(struct platform_device *op, struct fb_info *info,
 			   info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit cg14_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit cg14_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -595,7 +595,7 @@ static const struct of_device_id cg14_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg14_match);
 
-static struct of_platform_driver cg14_driver = {
+static struct platform_driver cg14_driver = {
 	.driver = {
 		.name = "cg14",
 		.owner = THIS_MODULE,
@@ -610,12 +610,12 @@ static int __init cg14_init(void)
 	if (fb_get_options("cg14fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&cg14_driver);
+	return platform_driver_register(&cg14_driver);
 }
 
 static void __exit cg14_exit(void)
 {
-	of_unregister_platform_driver(&cg14_driver);
+	platform_driver_unregister(&cg14_driver);
 }
 
 module_init(cg14_init);
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index 09c0c3c42482..f927a7b1a8d4 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -346,8 +346,7 @@ static int __devinit cg3_do_default_mode(struct cg3_par *par)
 	return 0;
 }
 
-static int __devinit cg3_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit cg3_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -462,7 +461,7 @@ static const struct of_device_id cg3_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg3_match);
 
-static struct of_platform_driver cg3_driver = {
+static struct platform_driver cg3_driver = {
 	.driver = {
 		.name = "cg3",
 		.owner = THIS_MODULE,
@@ -477,12 +476,12 @@ static int __init cg3_init(void)
 	if (fb_get_options("cg3fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&cg3_driver);
+	return platform_driver_register(&cg3_driver);
 }
 
 static void __exit cg3_exit(void)
 {
-	of_unregister_platform_driver(&cg3_driver);
+	platform_driver_unregister(&cg3_driver);
 }
 
 module_init(cg3_init);
diff --git a/drivers/video/cg6.c b/drivers/video/cg6.c
index 2b5a97058b08..4ffad90bde42 100644
--- a/drivers/video/cg6.c
+++ b/drivers/video/cg6.c
@@ -737,8 +737,7 @@ static void cg6_unmap_regs(struct platform_device *op, struct fb_info *info,
 			   info->fix.smem_len);
 }
 
-static int __devinit cg6_probe(struct platform_device *op,
-				const struct of_device_id *match)
+static int __devinit cg6_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -855,7 +854,7 @@ static const struct of_device_id cg6_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cg6_match);
 
-static struct of_platform_driver cg6_driver = {
+static struct platform_driver cg6_driver = {
 	.driver = {
 		.name = "cg6",
 		.owner = THIS_MODULE,
@@ -870,12 +869,12 @@ static int __init cg6_init(void)
 	if (fb_get_options("cg6fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&cg6_driver);
+	return platform_driver_register(&cg6_driver);
 }
 
 static void __exit cg6_exit(void)
 {
-	of_unregister_platform_driver(&cg6_driver);
+	platform_driver_unregister(&cg6_driver);
 }
 
 module_init(cg6_init);
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index 0c1afd13ddd3..850380795b05 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -47,6 +47,8 @@
 #include <linux/pci.h>
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
 
 #include <asm/pgtable.h>
 #include <asm/system.h>
@@ -61,10 +63,10 @@ struct cfb_info {
 	struct fb_info		fb;
 	struct display_switch	*dispsw;
 	struct display		*display;
-	struct pci_dev		*dev;
 	unsigned char		__iomem *region;
 	unsigned char		__iomem *regs;
 	u_int			id;
+	u_int			irq;
 	int			func_use_count;
 	u_long			ref_ps;
 
@@ -88,6 +90,19 @@ struct cfb_info {
 	u_char			ramdac_powerdown;
 
 	u32			pseudo_palette[16];
+
+	spinlock_t		reg_b0_lock;
+
+#ifdef CONFIG_FB_CYBER2000_DDC
+	bool			ddc_registered;
+	struct i2c_adapter	ddc_adapter;
+	struct i2c_algo_bit_data	ddc_algo;
+#endif
+
+#ifdef CONFIG_FB_CYBER2000_I2C
+	struct i2c_adapter	i2c_adapter;
+	struct i2c_algo_bit_data i2c_algo;
+#endif
 };
 
 static char *default_font = "Acorn8x8";
@@ -494,6 +509,7 @@ static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
 	cyber2000_attrw(0x14, 0x00, cfb);
 
 	/* PLL registers */
+	spin_lock(&cfb->reg_b0_lock);
 	cyber2000_grphw(EXT_DCLK_MULT, hw->clock_mult, cfb);
 	cyber2000_grphw(EXT_DCLK_DIV, hw->clock_div, cfb);
 	cyber2000_grphw(EXT_MCLK_MULT, cfb->mclk_mult, cfb);
@@ -501,6 +517,7 @@ static void cyber2000fb_set_timing(struct cfb_info *cfb, struct par_info *hw)
 	cyber2000_grphw(0x90, 0x01, cfb);
 	cyber2000_grphw(0xb9, 0x80, cfb);
 	cyber2000_grphw(0xb9, 0x00, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
 
 	cfb->ramdac_ctrl = hw->ramdac;
 	cyber2000fb_write_ramdac_ctrl(cfb);
@@ -681,9 +698,9 @@ cyber2000fb_decode_clock(struct par_info *hw, struct cfb_info *cfb,
 	 *    pll_ps_calc = best_div1 / (ref_ps * best_mult)
 	 */
 	best_diff = 0x7fffffff;
-	best_mult = 32;
-	best_div1 = 255;
-	for (t_div1 = 32; t_div1 > 1; t_div1 -= 1) {
+	best_mult = 2;
+	best_div1 = 32;
+	for (t_div1 = 2; t_div1 < 32; t_div1 += 1) {
 		u_int rr, t_mult, t_pll_ps;
 		int diff;
 
@@ -1105,24 +1122,22 @@ void cyber2000fb_disable_extregs(struct cfb_info *cfb)
 }
 EXPORT_SYMBOL(cyber2000fb_disable_extregs);
 
-void cyber2000fb_get_fb_var(struct cfb_info *cfb, struct fb_var_screeninfo *var)
-{
-	memcpy(var, &cfb->fb.var, sizeof(struct fb_var_screeninfo));
-}
-EXPORT_SYMBOL(cyber2000fb_get_fb_var);
-
 /*
  * Attach a capture/tv driver to the core CyberX0X0 driver.
  */
 int cyber2000fb_attach(struct cyberpro_info *info, int idx)
 {
 	if (int_cfb_info != NULL) {
-		info->dev	      = int_cfb_info->dev;
+		info->dev	      = int_cfb_info->fb.device;
+#ifdef CONFIG_FB_CYBER2000_I2C
+		info->i2c	      = &int_cfb_info->i2c_adapter;
+#else
+		info->i2c	      = NULL;
+#endif
 		info->regs	      = int_cfb_info->regs;
+		info->irq             = int_cfb_info->irq;
 		info->fb	      = int_cfb_info->fb.screen_base;
 		info->fb_size	      = int_cfb_info->fb.fix.smem_len;
-		info->enable_extregs  = cyber2000fb_enable_extregs;
-		info->disable_extregs = cyber2000fb_disable_extregs;
 		info->info	      = int_cfb_info;
 
 		strlcpy(info->dev_name, int_cfb_info->fb.fix.id,
@@ -1141,6 +1156,183 @@ void cyber2000fb_detach(int idx)
 }
 EXPORT_SYMBOL(cyber2000fb_detach);
 
+#ifdef CONFIG_FB_CYBER2000_DDC
+
+#define DDC_REG		0xb0
+#define DDC_SCL_OUT	(1 << 0)
+#define DDC_SDA_OUT	(1 << 4)
+#define DDC_SCL_IN	(1 << 2)
+#define DDC_SDA_IN	(1 << 6)
+
+static void cyber2000fb_enable_ddc(struct cfb_info *cfb)
+{
+	spin_lock(&cfb->reg_b0_lock);
+	cyber2000fb_writew(0x1bf, 0x3ce, cfb);
+}
+
+static void cyber2000fb_disable_ddc(struct cfb_info *cfb)
+{
+	cyber2000fb_writew(0x0bf, 0x3ce, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
+}
+
+
+static void cyber2000fb_ddc_setscl(void *data, int val)
+{
+	struct cfb_info *cfb = data;
+	unsigned char reg;
+
+	cyber2000fb_enable_ddc(cfb);
+	reg = cyber2000_grphr(DDC_REG, cfb);
+	if (!val)	/* bit is inverted */
+		reg |= DDC_SCL_OUT;
+	else
+		reg &= ~DDC_SCL_OUT;
+	cyber2000_grphw(DDC_REG, reg, cfb);
+	cyber2000fb_disable_ddc(cfb);
+}
+
+static void cyber2000fb_ddc_setsda(void *data, int val)
+{
+	struct cfb_info *cfb = data;
+	unsigned char reg;
+
+	cyber2000fb_enable_ddc(cfb);
+	reg = cyber2000_grphr(DDC_REG, cfb);
+	if (!val)	/* bit is inverted */
+		reg |= DDC_SDA_OUT;
+	else
+		reg &= ~DDC_SDA_OUT;
+	cyber2000_grphw(DDC_REG, reg, cfb);
+	cyber2000fb_disable_ddc(cfb);
+}
+
+static int cyber2000fb_ddc_getscl(void *data)
+{
+	struct cfb_info *cfb = data;
+	int retval;
+
+	cyber2000fb_enable_ddc(cfb);
+	retval = !!(cyber2000_grphr(DDC_REG, cfb) & DDC_SCL_IN);
+	cyber2000fb_disable_ddc(cfb);
+
+	return retval;
+}
+
+static int cyber2000fb_ddc_getsda(void *data)
+{
+	struct cfb_info *cfb = data;
+	int retval;
+
+	cyber2000fb_enable_ddc(cfb);
+	retval = !!(cyber2000_grphr(DDC_REG, cfb) & DDC_SDA_IN);
+	cyber2000fb_disable_ddc(cfb);
+
+	return retval;
+}
+
+static int __devinit cyber2000fb_setup_ddc_bus(struct cfb_info *cfb)
+{
+	strlcpy(cfb->ddc_adapter.name, cfb->fb.fix.id,
+		sizeof(cfb->ddc_adapter.name));
+	cfb->ddc_adapter.owner		= THIS_MODULE;
+	cfb->ddc_adapter.class		= I2C_CLASS_DDC;
+	cfb->ddc_adapter.algo_data	= &cfb->ddc_algo;
+	cfb->ddc_adapter.dev.parent	= cfb->fb.device;
+	cfb->ddc_algo.setsda		= cyber2000fb_ddc_setsda;
+	cfb->ddc_algo.setscl		= cyber2000fb_ddc_setscl;
+	cfb->ddc_algo.getsda		= cyber2000fb_ddc_getsda;
+	cfb->ddc_algo.getscl		= cyber2000fb_ddc_getscl;
+	cfb->ddc_algo.udelay		= 10;
+	cfb->ddc_algo.timeout		= 20;
+	cfb->ddc_algo.data		= cfb;
+
+	i2c_set_adapdata(&cfb->ddc_adapter, cfb);
+
+	return i2c_bit_add_bus(&cfb->ddc_adapter);
+}
+#endif /* CONFIG_FB_CYBER2000_DDC */
+
+#ifdef CONFIG_FB_CYBER2000_I2C
+static void cyber2000fb_i2c_setsda(void *data, int state)
+{
+	struct cfb_info *cfb = data;
+	unsigned int latch2;
+
+	spin_lock(&cfb->reg_b0_lock);
+	latch2 = cyber2000_grphr(EXT_LATCH2, cfb);
+	latch2 &= EXT_LATCH2_I2C_CLKEN;
+	if (state)
+		latch2 |= EXT_LATCH2_I2C_DATEN;
+	cyber2000_grphw(EXT_LATCH2, latch2, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
+}
+
+static void cyber2000fb_i2c_setscl(void *data, int state)
+{
+	struct cfb_info *cfb = data;
+	unsigned int latch2;
+
+	spin_lock(&cfb->reg_b0_lock);
+	latch2 = cyber2000_grphr(EXT_LATCH2, cfb);
+	latch2 &= EXT_LATCH2_I2C_DATEN;
+	if (state)
+		latch2 |= EXT_LATCH2_I2C_CLKEN;
+	cyber2000_grphw(EXT_LATCH2, latch2, cfb);
+	spin_unlock(&cfb->reg_b0_lock);
+}
+
+static int cyber2000fb_i2c_getsda(void *data)
+{
+	struct cfb_info *cfb = data;
+	int ret;
+
+	spin_lock(&cfb->reg_b0_lock);
+	ret = !!(cyber2000_grphr(EXT_LATCH2, cfb) & EXT_LATCH2_I2C_DAT);
+	spin_unlock(&cfb->reg_b0_lock);
+
+	return ret;
+}
+
+static int cyber2000fb_i2c_getscl(void *data)
+{
+	struct cfb_info *cfb = data;
+	int ret;
+
+	spin_lock(&cfb->reg_b0_lock);
+	ret = !!(cyber2000_grphr(EXT_LATCH2, cfb) & EXT_LATCH2_I2C_CLK);
+	spin_unlock(&cfb->reg_b0_lock);
+
+	return ret;
+}
+
+static int __devinit cyber2000fb_i2c_register(struct cfb_info *cfb)
+{
+	strlcpy(cfb->i2c_adapter.name, cfb->fb.fix.id,
+		sizeof(cfb->i2c_adapter.name));
+	cfb->i2c_adapter.owner = THIS_MODULE;
+	cfb->i2c_adapter.algo_data = &cfb->i2c_algo;
+	cfb->i2c_adapter.dev.parent = cfb->fb.device;
+	cfb->i2c_algo.setsda = cyber2000fb_i2c_setsda;
+	cfb->i2c_algo.setscl = cyber2000fb_i2c_setscl;
+	cfb->i2c_algo.getsda = cyber2000fb_i2c_getsda;
+	cfb->i2c_algo.getscl = cyber2000fb_i2c_getscl;
+	cfb->i2c_algo.udelay = 5;
+	cfb->i2c_algo.timeout = msecs_to_jiffies(100);
+	cfb->i2c_algo.data = cfb;
+
+	return i2c_bit_add_bus(&cfb->i2c_adapter);
+}
+
+static void cyber2000fb_i2c_unregister(struct cfb_info *cfb)
+{
+	i2c_del_adapter(&cfb->i2c_adapter);
+}
+#else
+#define cyber2000fb_i2c_register(cfb)	(0)
+#define cyber2000fb_i2c_unregister(cfb)	do { } while (0)
+#endif
+
 /*
  * These parameters give
  * 640x480, hsync 31.5kHz, vsync 60Hz
@@ -1275,6 +1467,8 @@ static struct cfb_info __devinit *cyberpro_alloc_fb_info(unsigned int id,
 	cfb->fb.flags		= FBINFO_DEFAULT | FBINFO_HWACCEL_YPAN;
 	cfb->fb.pseudo_palette	= cfb->pseudo_palette;
 
+	spin_lock_init(&cfb->reg_b0_lock);
+
 	fb_alloc_cmap(&cfb->fb.cmap, NR_PALETTE, 0);
 
 	return cfb;
@@ -1369,6 +1563,11 @@ static int __devinit cyberpro_common_probe(struct cfb_info *cfb)
 	cfb->fb.fix.mmio_len   = MMIO_SIZE;
 	cfb->fb.screen_base    = cfb->region;
 
+#ifdef CONFIG_FB_CYBER2000_DDC
+	if (cyber2000fb_setup_ddc_bus(cfb) == 0)
+		cfb->ddc_registered = true;
+#endif
+
 	err = -EINVAL;
 	if (!fb_find_mode(&cfb->fb.var, &cfb->fb, NULL, NULL, 0,
 			  &cyber2000fb_default_mode, 8)) {
@@ -1401,14 +1600,32 @@ static int __devinit cyberpro_common_probe(struct cfb_info *cfb)
 		cfb->fb.var.xres, cfb->fb.var.yres,
 		h_sync / 1000, h_sync % 1000, v_sync);
 
-	if (cfb->dev)
-		cfb->fb.device = &cfb->dev->dev;
+	err = cyber2000fb_i2c_register(cfb);
+	if (err)
+		goto failed;
+
 	err = register_framebuffer(&cfb->fb);
+	if (err)
+		cyber2000fb_i2c_unregister(cfb);
 
 failed:
+#ifdef CONFIG_FB_CYBER2000_DDC
+	if (err && cfb->ddc_registered)
+		i2c_del_adapter(&cfb->ddc_adapter);
+#endif
 	return err;
 }
 
+static void __devexit cyberpro_common_remove(struct cfb_info *cfb)
+{
+	unregister_framebuffer(&cfb->fb);
+#ifdef CONFIG_FB_CYBER2000_DDC
+	if (cfb->ddc_registered)
+		i2c_del_adapter(&cfb->ddc_adapter);
+#endif
+	cyber2000fb_i2c_unregister(cfb);
+}
+
 static void cyberpro_common_resume(struct cfb_info *cfb)
 {
 	cyberpro_init_hw(cfb);
@@ -1442,12 +1659,13 @@ static int __devinit cyberpro_vl_probe(void)
 	if (!cfb)
 		goto failed_release;
 
-	cfb->dev = NULL;
+	cfb->irq = -1;
 	cfb->region = ioremap(FB_START, FB_SIZE);
 	if (!cfb->region)
 		goto failed_ioremap;
 
 	cfb->regs = cfb->region + MMIO_OFFSET;
+	cfb->fb.device = NULL;
 	cfb->fb.fix.mmio_start = FB_START + MMIO_OFFSET;
 	cfb->fb.fix.smem_start = FB_START;
 
@@ -1585,12 +1803,13 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (err)
 		goto failed_regions;
 
-	cfb->dev = dev;
+	cfb->irq = dev->irq;
 	cfb->region = pci_ioremap_bar(dev, 0);
 	if (!cfb->region)
 		goto failed_ioremap;
 
 	cfb->regs = cfb->region + MMIO_OFFSET;
+	cfb->fb.device = &dev->dev;
 	cfb->fb.fix.mmio_start = pci_resource_start(dev, 0) + MMIO_OFFSET;
 	cfb->fb.fix.smem_start = pci_resource_start(dev, 0);
 
@@ -1648,15 +1867,7 @@ static void __devexit cyberpro_pci_remove(struct pci_dev *dev)
 	struct cfb_info *cfb = pci_get_drvdata(dev);
 
 	if (cfb) {
-		/*
-		 * If unregister_framebuffer fails, then
-		 * we will be leaving hooks that could cause
-		 * oopsen laying around.
-		 */
-		if (unregister_framebuffer(&cfb->fb))
-			printk(KERN_WARNING "%s: danger Will Robinson, "
-				"danger danger!  Oopsen imminent!\n",
-				cfb->fb.fix.id);
+		cyberpro_common_remove(cfb);
 		iounmap(cfb->region);
 		cyberpro_free_fb_info(cfb);
 
diff --git a/drivers/video/cyber2000fb.h b/drivers/video/cyber2000fb.h
index de4fc43e51c1..bad69102e774 100644
--- a/drivers/video/cyber2000fb.h
+++ b/drivers/video/cyber2000fb.h
@@ -464,12 +464,14 @@ static void debug_printf(char *fmt, ...)
 struct cfb_info;
 
 struct cyberpro_info {
-	struct pci_dev	*dev;
+	struct device	*dev;
+	struct i2c_adapter *i2c;
 	unsigned char	__iomem *regs;
 	char		__iomem *fb;
 	char		dev_name[32];
 	unsigned int	fb_size;
 	unsigned int	chip_id;
+	unsigned int	irq;
 
 	/*
 	 * The following is a pointer to be passed into the
@@ -478,15 +480,6 @@ struct cyberpro_info {
 	 * is within this structure.
 	 */
 	struct cfb_info *info;
-
-	/*
-	 * Use these to enable the BM or TV registers.  In an SMP
-	 * environment, these two function pointers should only be
-	 * called from the module_init() or module_exit()
-	 * functions.
-	 */
-	void (*enable_extregs)(struct cfb_info *);
-	void (*disable_extregs)(struct cfb_info *);
 };
 
 #define ID_IGA_1682		0
@@ -494,8 +487,6 @@ struct cyberpro_info {
 #define ID_CYBERPRO_2010	2
 #define ID_CYBERPRO_5000	3
 
-struct fb_var_screeninfo;
-
 /*
  * Note! Writing to the Cyber20x0 registers from an interrupt
  * routine is definitely a bad idea atm.
@@ -504,4 +495,3 @@ int cyber2000fb_attach(struct cyberpro_info *info, int idx);
 void cyber2000fb_detach(int idx);
 void cyber2000fb_enable_extregs(struct cfb_info *cfb);
 void cyber2000fb_disable_extregs(struct cfb_info *cfb);
-void cyber2000fb_get_fb_var(struct cfb_info *cfb, struct fb_var_screeninfo *var);
diff --git a/drivers/video/fb-puv3.c b/drivers/video/fb-puv3.c
new file mode 100644
index 000000000000..dbd2dc4745d1
--- /dev/null
+++ b/drivers/video/fb-puv3.c
@@ -0,0 +1,846 @@
+/*
+ * Frame Buffer Driver for PKUnity-v3 Unigfx
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *	Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *	Copyright (C) 2001-2010 Guan Xuetao
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/vmalloc.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/console.h>
+
+#include <asm/sizes.h>
+#include <mach/hardware.h>
+
+/* Platform_data reserved for unifb registers. */
+#define UNIFB_REGS_NUM		10
+/* RAM reserved for the frame buffer. */
+#define UNIFB_MEMSIZE		(SZ_4M)		/* 4 MB for 1024*768*32b */
+
+/*
+ * cause UNIGFX don not have EDID
+ * all the modes are organized as follow
+ */
+static const struct fb_videomode unifb_modes[] = {
+	/* 0 640x480-60 VESA */
+	{ "640x480@60",  60,  640, 480,  25175000,  48, 16, 34, 10,  96, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 1 640x480-75 VESA */
+	{ "640x480@75",  75,  640, 480,  31500000, 120, 16, 18,  1,  64, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 2 800x600-60 VESA */
+	{ "800x600@60",  60,  800, 600,  40000000,  88, 40, 26,  1, 128, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 3 800x600-75 VESA */
+	{ "800x600@75",  75,  800, 600,  49500000, 160, 16, 23,  1,  80, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 4 1024x768-60 VESA */
+	{ "1024x768@60", 60, 1024, 768,  65000000, 160, 24, 34,  3, 136, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 5 1024x768-75 VESA */
+	{ "1024x768@75", 75, 1024, 768,  78750000, 176, 16, 30,  1,  96, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 6 1280x960-60 VESA */
+	{ "1280x960@60", 60, 1280, 960, 108000000, 312, 96, 38,  1, 112, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 7 1440x900-60 VESA */
+	{ "1440x900@60", 60, 1440, 900, 106500000, 232, 80, 30,  3, 152, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 8 FIXME 9 1024x600-60 VESA UNTESTED */
+	{ "1024x600@60", 60, 1024, 600,  50650000, 160, 24, 26,  1, 136, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 9 FIXME 10 1024x600-75 VESA UNTESTED */
+	{ "1024x600@75", 75, 1024, 600,  61500000, 176, 16, 23,  1,  96, 1,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+	/* 10 FIXME 11 1366x768-60 VESA UNTESTED */
+	{ "1366x768@60", 60, 1366, 768,  85500000, 256, 58, 18,  1,  112, 3,
+	  0, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
+};
+
+static struct fb_var_screeninfo unifb_default = {
+	.xres =		640,
+	.yres =		480,
+	.xres_virtual =	640,
+	.yres_virtual =	480,
+	.bits_per_pixel = 16,
+	.red =		{ 11, 5, 0 },
+	.green =	{ 5,  6, 0 },
+	.blue =		{ 0,  5, 0 },
+	.activate =	FB_ACTIVATE_NOW,
+	.height =	-1,
+	.width =	-1,
+	.pixclock =	25175000,
+	.left_margin =	48,
+	.right_margin =	16,
+	.upper_margin =	33,
+	.lower_margin =	10,
+	.hsync_len =	96,
+	.vsync_len =	2,
+	.vmode =	FB_VMODE_NONINTERLACED,
+};
+
+static struct fb_fix_screeninfo unifb_fix = {
+	.id =		"UNIGFX FB",
+	.type =		FB_TYPE_PACKED_PIXELS,
+	.visual =	FB_VISUAL_TRUECOLOR,
+	.xpanstep =	1,
+	.ypanstep =	1,
+	.ywrapstep =	1,
+	.accel =	FB_ACCEL_NONE,
+};
+
+static void unifb_sync(struct fb_info *info)
+{
+	/* TODO: may, this can be replaced by interrupt */
+	int cnt;
+
+	for (cnt = 0; cnt < 0x10000000; cnt++) {
+		if (readl(UGE_COMMAND) & 0x1000000)
+			return;
+	}
+
+	if (cnt > 0x8000000)
+		dev_warn(info->device, "Warning: UniGFX GE time out ...\n");
+}
+
+static void unifb_prim_fillrect(struct fb_info *info,
+				const struct fb_fillrect *region)
+{
+	int awidth = region->width;
+	int aheight = region->height;
+	int m_iBpp = info->var.bits_per_pixel;
+	int screen_width = info->var.xres;
+	int src_sel = 1;	/* from fg_color */
+	int pat_sel = 1;
+	int src_x0 = 0;
+	int dst_x0 = region->dx;
+	int src_y0 = 0;
+	int dst_y0 = region->dy;
+	int rop_alpha_sel = 0;
+	int rop_alpha_code = 0xCC;
+	int x_dir = 1;
+	int y_dir = 1;
+	int alpha_r = 0;
+	int alpha_sel = 0;
+	int dst_pitch = screen_width * (m_iBpp / 8);
+	int dst_offset = dst_y0 * dst_pitch + dst_x0 * (m_iBpp / 8);
+	int src_pitch = screen_width * (m_iBpp / 8);
+	int src_offset = src_y0 * src_pitch + src_x0 * (m_iBpp / 8);
+	unsigned int command = 0;
+	int clip_region = 0;
+	int clip_en = 0;
+	int tp_en = 0;
+	int fg_color = 0;
+	int bottom = info->var.yres - 1;
+	int right = info->var.xres - 1;
+	int top = 0;
+
+	bottom = (bottom << 16) | right;
+	command = (rop_alpha_sel << 26) | (pat_sel << 18) | (src_sel << 16)
+		| (x_dir << 20) | (y_dir << 21) | (command << 24)
+		| (clip_region << 23) | (clip_en << 22) | (tp_en << 27);
+	src_pitch = (dst_pitch << 16) | src_pitch;
+	awidth = awidth | (aheight << 16);
+	alpha_r = ((rop_alpha_code & 0xff) << 8) | (alpha_r & 0xff)
+		| (alpha_sel << 16);
+	src_x0 = (src_x0 & 0x1fff) | ((src_y0 & 0x1fff) << 16);
+	dst_x0 = (dst_x0 & 0x1fff) | ((dst_y0 & 0x1fff) << 16);
+	fg_color = region->color;
+
+	unifb_sync(info);
+
+	writel(((u32 *)(info->pseudo_palette))[fg_color], UGE_FCOLOR);
+	writel(0, UGE_BCOLOR);
+	writel(src_pitch, UGE_PITCH);
+	writel(src_offset, UGE_SRCSTART);
+	writel(dst_offset, UGE_DSTSTART);
+	writel(awidth, UGE_WIDHEIGHT);
+	writel(top, UGE_CLIP0);
+	writel(bottom, UGE_CLIP1);
+	writel(alpha_r, UGE_ROPALPHA);
+	writel(src_x0, UGE_SRCXY);
+	writel(dst_x0, UGE_DSTXY);
+	writel(command, UGE_COMMAND);
+}
+
+static void unifb_fillrect(struct fb_info *info,
+		const struct fb_fillrect *region)
+{
+	struct fb_fillrect modded;
+	int vxres, vyres;
+
+	if (info->flags & FBINFO_HWACCEL_DISABLED) {
+		sys_fillrect(info, region);
+		return;
+	}
+
+	vxres = info->var.xres_virtual;
+	vyres = info->var.yres_virtual;
+
+	memcpy(&modded, region, sizeof(struct fb_fillrect));
+
+	if (!modded.width || !modded.height ||
+	    modded.dx >= vxres || modded.dy >= vyres)
+		return;
+
+	if (modded.dx + modded.width > vxres)
+		modded.width = vxres - modded.dx;
+	if (modded.dy + modded.height > vyres)
+		modded.height = vyres - modded.dy;
+
+	unifb_prim_fillrect(info, &modded);
+}
+
+static void unifb_prim_copyarea(struct fb_info *info,
+				const struct fb_copyarea *area)
+{
+	int awidth = area->width;
+	int aheight = area->height;
+	int m_iBpp = info->var.bits_per_pixel;
+	int screen_width = info->var.xres;
+	int src_sel = 2;	/* from mem */
+	int pat_sel = 0;
+	int src_x0 = area->sx;
+	int dst_x0 = area->dx;
+	int src_y0 = area->sy;
+	int dst_y0 = area->dy;
+
+	int rop_alpha_sel = 0;
+	int rop_alpha_code = 0xCC;
+	int x_dir = 1;
+	int y_dir = 1;
+
+	int alpha_r = 0;
+	int alpha_sel = 0;
+	int dst_pitch = screen_width * (m_iBpp / 8);
+	int dst_offset = dst_y0 * dst_pitch + dst_x0 * (m_iBpp / 8);
+	int src_pitch = screen_width * (m_iBpp / 8);
+	int src_offset = src_y0 * src_pitch + src_x0 * (m_iBpp / 8);
+	unsigned int command = 0;
+	int clip_region = 0;
+	int clip_en = 1;
+	int tp_en = 0;
+	int top = 0;
+	int bottom = info->var.yres;
+	int right = info->var.xres;
+	int fg_color = 0;
+	int bg_color = 0;
+
+	if (src_x0 < 0)
+		src_x0 = 0;
+	if (src_y0 < 0)
+		src_y0 = 0;
+
+	if (src_y0 - dst_y0 > 0) {
+		y_dir = 1;
+	} else {
+		y_dir = 0;
+		src_offset = (src_y0 + aheight) * src_pitch +
+				src_x0 * (m_iBpp / 8);
+		dst_offset = (dst_y0 + aheight) * dst_pitch +
+				dst_x0 * (m_iBpp / 8);
+		src_y0 += aheight;
+		dst_y0 += aheight;
+	}
+
+	command = (rop_alpha_sel << 26) | (pat_sel << 18) | (src_sel << 16) |
+		(x_dir << 20) | (y_dir << 21) | (command << 24) |
+		(clip_region << 23) | (clip_en << 22) | (tp_en << 27);
+	src_pitch = (dst_pitch << 16) | src_pitch;
+	awidth = awidth | (aheight << 16);
+	alpha_r = ((rop_alpha_code & 0xff) << 8) | (alpha_r & 0xff) |
+		(alpha_sel << 16);
+	src_x0 = (src_x0 & 0x1fff) | ((src_y0 & 0x1fff) << 16);
+	dst_x0 = (dst_x0 & 0x1fff) | ((dst_y0 & 0x1fff) << 16);
+	bottom = (bottom << 16) | right;
+
+	unifb_sync(info);
+
+	writel(src_pitch, UGE_PITCH);
+	writel(src_offset, UGE_SRCSTART);
+	writel(dst_offset, UGE_DSTSTART);
+	writel(awidth, UGE_WIDHEIGHT);
+	writel(top, UGE_CLIP0);
+	writel(bottom, UGE_CLIP1);
+	writel(bg_color, UGE_BCOLOR);
+	writel(fg_color, UGE_FCOLOR);
+	writel(alpha_r, UGE_ROPALPHA);
+	writel(src_x0, UGE_SRCXY);
+	writel(dst_x0, UGE_DSTXY);
+	writel(command, UGE_COMMAND);
+}
+
+static void unifb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
+{
+	struct fb_copyarea modded;
+	u32 vxres, vyres;
+	modded.sx = area->sx;
+	modded.sy = area->sy;
+	modded.dx = area->dx;
+	modded.dy = area->dy;
+	modded.width = area->width;
+	modded.height = area->height;
+
+	if (info->flags & FBINFO_HWACCEL_DISABLED) {
+		sys_copyarea(info, area);
+		return;
+	}
+
+	vxres = info->var.xres_virtual;
+	vyres = info->var.yres_virtual;
+
+	if (!modded.width || !modded.height ||
+	    modded.sx >= vxres || modded.sy >= vyres ||
+	    modded.dx >= vxres || modded.dy >= vyres)
+		return;
+
+	if (modded.sx + modded.width > vxres)
+		modded.width = vxres - modded.sx;
+	if (modded.dx + modded.width > vxres)
+		modded.width = vxres - modded.dx;
+	if (modded.sy + modded.height > vyres)
+		modded.height = vyres - modded.sy;
+	if (modded.dy + modded.height > vyres)
+		modded.height = vyres - modded.dy;
+
+	unifb_prim_copyarea(info, &modded);
+}
+
+static void unifb_imageblit(struct fb_info *info, const struct fb_image *image)
+{
+	sys_imageblit(info, image);
+}
+
+static u_long get_line_length(int xres_virtual, int bpp)
+{
+	u_long length;
+
+	length = xres_virtual * bpp;
+	length = (length + 31) & ~31;
+	length >>= 3;
+	return length;
+}
+
+/*
+ *  Setting the video mode has been split into two parts.
+ *  First part, xxxfb_check_var, must not write anything
+ *  to hardware, it should only verify and adjust var.
+ *  This means it doesn't alter par but it does use hardware
+ *  data from it to check this var.
+ */
+static int unifb_check_var(struct fb_var_screeninfo *var,
+			 struct fb_info *info)
+{
+	u_long line_length;
+
+	/*
+	 *  FB_VMODE_CONUPDATE and FB_VMODE_SMOOTH_XPAN are equal!
+	 *  as FB_VMODE_SMOOTH_XPAN is only used internally
+	 */
+
+	if (var->vmode & FB_VMODE_CONUPDATE) {
+		var->vmode |= FB_VMODE_YWRAP;
+		var->xoffset = info->var.xoffset;
+		var->yoffset = info->var.yoffset;
+	}
+
+	/*
+	 *  Some very basic checks
+	 */
+	if (!var->xres)
+		var->xres = 1;
+	if (!var->yres)
+		var->yres = 1;
+	if (var->xres > var->xres_virtual)
+		var->xres_virtual = var->xres;
+	if (var->yres > var->yres_virtual)
+		var->yres_virtual = var->yres;
+	if (var->bits_per_pixel <= 1)
+		var->bits_per_pixel = 1;
+	else if (var->bits_per_pixel <= 8)
+		var->bits_per_pixel = 8;
+	else if (var->bits_per_pixel <= 16)
+		var->bits_per_pixel = 16;
+	else if (var->bits_per_pixel <= 24)
+		var->bits_per_pixel = 24;
+	else if (var->bits_per_pixel <= 32)
+		var->bits_per_pixel = 32;
+	else
+		return -EINVAL;
+
+	if (var->xres_virtual < var->xoffset + var->xres)
+		var->xres_virtual = var->xoffset + var->xres;
+	if (var->yres_virtual < var->yoffset + var->yres)
+		var->yres_virtual = var->yoffset + var->yres;
+
+	/*
+	 *  Memory limit
+	 */
+	line_length =
+	    get_line_length(var->xres_virtual, var->bits_per_pixel);
+	if (line_length * var->yres_virtual > UNIFB_MEMSIZE)
+		return -ENOMEM;
+
+	/*
+	 * Now that we checked it we alter var. The reason being is that the
+	 * video mode passed in might not work but slight changes to it might
+	 * make it work. This way we let the user know what is acceptable.
+	 */
+	switch (var->bits_per_pixel) {
+	case 1:
+	case 8:
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 0;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+	case 16:		/* RGBA 5551 */
+		if (var->transp.length) {
+			var->red.offset = 0;
+			var->red.length = 5;
+			var->green.offset = 5;
+			var->green.length = 5;
+			var->blue.offset = 10;
+			var->blue.length = 5;
+			var->transp.offset = 15;
+			var->transp.length = 1;
+		} else {	/* RGB 565 */
+			var->red.offset = 11;
+			var->red.length = 5;
+			var->green.offset = 5;
+			var->green.length = 6;
+			var->blue.offset = 0;
+			var->blue.length = 5;
+			var->transp.offset = 0;
+			var->transp.length = 0;
+		}
+		break;
+	case 24:		/* RGB 888 */
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 16;
+		var->blue.length = 8;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+	case 32:		/* RGBA 8888 */
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->transp.offset = 24;
+		var->transp.length = 8;
+		break;
+	}
+	var->red.msb_right = 0;
+	var->green.msb_right = 0;
+	var->blue.msb_right = 0;
+	var->transp.msb_right = 0;
+
+	return 0;
+}
+
+/*
+ * This routine actually sets the video mode. It's in here where we
+ * the hardware state info->par and fix which can be affected by the
+ * change in par. For this driver it doesn't do much.
+ */
+static int unifb_set_par(struct fb_info *info)
+{
+	int hTotal, vTotal, hSyncStart, hSyncEnd, vSyncStart, vSyncEnd;
+	int format;
+
+#ifdef CONFIG_PUV3_PM
+	struct clk *clk_vga;
+	u32 pixclk = 0;
+	int i;
+
+	for (i = 0; i <= 10; i++) {
+		if    (info->var.xres         == unifb_modes[i].xres
+		    && info->var.yres         == unifb_modes[i].yres
+		    && info->var.upper_margin == unifb_modes[i].upper_margin
+		    && info->var.lower_margin == unifb_modes[i].lower_margin
+		    && info->var.left_margin  == unifb_modes[i].left_margin
+		    && info->var.right_margin == unifb_modes[i].right_margin
+		    && info->var.hsync_len    == unifb_modes[i].hsync_len
+		    && info->var.vsync_len    == unifb_modes[i].vsync_len) {
+			pixclk = unifb_modes[i].pixclock;
+			break;
+		}
+	}
+
+	/* set clock rate */
+	clk_vga = clk_get(info->device, "VGA_CLK");
+	if (clk_vga == ERR_PTR(-ENOENT))
+		return -ENOENT;
+
+	if (pixclk != 0) {
+		if (clk_set_rate(clk_vga, pixclk)) { /* set clock failed */
+			info->fix = unifb_fix;
+			info->var = unifb_default;
+			if (clk_set_rate(clk_vga, unifb_default.pixclock))
+				return -EINVAL;
+		}
+	}
+#endif
+
+	info->fix.line_length = get_line_length(info->var.xres_virtual,
+						info->var.bits_per_pixel);
+
+	hSyncStart = info->var.xres + info->var.right_margin;
+	hSyncEnd = hSyncStart + info->var.hsync_len;
+	hTotal = hSyncEnd + info->var.left_margin;
+
+	vSyncStart = info->var.yres + info->var.lower_margin;
+	vSyncEnd = vSyncStart + info->var.vsync_len;
+	vTotal = vSyncEnd + info->var.upper_margin;
+
+	switch (info->var.bits_per_pixel) {
+	case 8:
+		format = UDE_CFG_DST8;
+		break;
+	case 16:
+		format = UDE_CFG_DST16;
+		break;
+	case 24:
+		format = UDE_CFG_DST24;
+		break;
+	case 32:
+		format = UDE_CFG_DST32;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	writel(PKUNITY_UNIGFX_MMAP_BASE, UDE_FSA);
+	writel(info->var.yres, UDE_LS);
+	writel(get_line_length(info->var.xres,
+			info->var.bits_per_pixel) >> 3, UDE_PS);
+			/* >> 3 for hardware required. */
+	writel((hTotal << 16) | (info->var.xres), UDE_HAT);
+	writel(((hTotal - 1) << 16) | (info->var.xres - 1), UDE_HBT);
+	writel(((hSyncEnd - 1) << 16) | (hSyncStart - 1), UDE_HST);
+	writel((vTotal << 16) | (info->var.yres), UDE_VAT);
+	writel(((vTotal - 1) << 16) | (info->var.yres - 1), UDE_VBT);
+	writel(((vSyncEnd - 1) << 16) | (vSyncStart - 1), UDE_VST);
+	writel(UDE_CFG_GDEN_ENABLE | UDE_CFG_TIMEUP_ENABLE
+			| format | 0xC0000001, UDE_CFG);
+
+	return 0;
+}
+
+/*
+ *  Set a single color register. The values supplied are already
+ *  rounded down to the hardware's capabilities (according to the
+ *  entries in the var structure). Return != 0 for invalid regno.
+ */
+static int unifb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+			 u_int transp, struct fb_info *info)
+{
+	if (regno >= 256)	/* no. of hw registers */
+		return 1;
+
+	/* grayscale works only partially under directcolor */
+	if (info->var.grayscale) {
+		/* grayscale = 0.30*R + 0.59*G + 0.11*B */
+		red = green = blue =
+		    (red * 77 + green * 151 + blue * 28) >> 8;
+	}
+
+#define CNVT_TOHW(val, width) ((((val)<<(width))+0x7FFF-(val))>>16)
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		red = CNVT_TOHW(red, info->var.red.length);
+		green = CNVT_TOHW(green, info->var.green.length);
+		blue = CNVT_TOHW(blue, info->var.blue.length);
+		transp = CNVT_TOHW(transp, info->var.transp.length);
+		break;
+	case FB_VISUAL_DIRECTCOLOR:
+		red = CNVT_TOHW(red, 8);	/* expect 8 bit DAC */
+		green = CNVT_TOHW(green, 8);
+		blue = CNVT_TOHW(blue, 8);
+		/* hey, there is bug in transp handling... */
+		transp = CNVT_TOHW(transp, 8);
+		break;
+	}
+#undef CNVT_TOHW
+	/* Truecolor has hardware independent palette */
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+		u32 v;
+
+		if (regno >= 16)
+			return 1;
+
+		v = (red << info->var.red.offset) |
+		    (green << info->var.green.offset) |
+		    (blue << info->var.blue.offset) |
+		    (transp << info->var.transp.offset);
+		switch (info->var.bits_per_pixel) {
+		case 8:
+			break;
+		case 16:
+		case 24:
+		case 32:
+			((u32 *) (info->pseudo_palette))[regno] = v;
+			break;
+		default:
+			return 1;
+		}
+		return 0;
+	}
+	return 0;
+}
+
+/*
+ *  Pan or Wrap the Display
+ *
+ *  This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
+ */
+static int unifb_pan_display(struct fb_var_screeninfo *var,
+			   struct fb_info *info)
+{
+	if (var->vmode & FB_VMODE_YWRAP) {
+		if (var->yoffset < 0
+		    || var->yoffset >= info->var.yres_virtual
+		    || var->xoffset)
+			return -EINVAL;
+	} else {
+		if (var->xoffset + var->xres > info->var.xres_virtual ||
+		    var->yoffset + var->yres > info->var.yres_virtual)
+			return -EINVAL;
+	}
+	info->var.xoffset = var->xoffset;
+	info->var.yoffset = var->yoffset;
+	if (var->vmode & FB_VMODE_YWRAP)
+		info->var.vmode |= FB_VMODE_YWRAP;
+	else
+		info->var.vmode &= ~FB_VMODE_YWRAP;
+	return 0;
+}
+
+int unifb_mmap(struct fb_info *info,
+		    struct vm_area_struct *vma)
+{
+	unsigned long size = vma->vm_end - vma->vm_start;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long pos = info->fix.smem_start + offset;
+
+	if (offset + size > info->fix.smem_len)
+		return -EINVAL;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (io_remap_pfn_range(vma, vma->vm_start, pos >> PAGE_SHIFT, size,
+				vma->vm_page_prot))
+		return -EAGAIN;
+
+	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
+	return 0;
+
+}
+
+static struct fb_ops unifb_ops = {
+	.fb_read        = fb_sys_read,
+	.fb_write       = fb_sys_write,
+	.fb_check_var	= unifb_check_var,
+	.fb_set_par	= unifb_set_par,
+	.fb_setcolreg	= unifb_setcolreg,
+	.fb_pan_display	= unifb_pan_display,
+	.fb_fillrect	= unifb_fillrect,
+	.fb_copyarea	= unifb_copyarea,
+	.fb_imageblit   = unifb_imageblit,
+	.fb_mmap	= unifb_mmap,
+};
+
+/*
+ *  Initialisation
+ */
+static int unifb_probe(struct platform_device *dev)
+{
+	struct fb_info *info;
+	u32 unifb_regs[UNIFB_REGS_NUM];
+	int retval = -ENOMEM;
+	struct resource *iomem, *mapmem;
+
+	info = framebuffer_alloc(sizeof(u32)*256, &dev->dev);
+	if (!info)
+		goto err;
+
+	info->screen_base = (char __iomem *)KUSER_UNIGFX_BASE;
+	info->fbops = &unifb_ops;
+
+	retval = fb_find_mode(&info->var, info, NULL,
+			      unifb_modes, 10, &unifb_modes[0], 16);
+
+	if (!retval || (retval == 4))
+		info->var = unifb_default;
+
+	iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	unifb_fix.mmio_start = iomem->start;
+
+	mapmem = platform_get_resource(dev, IORESOURCE_MEM, 1);
+	unifb_fix.smem_start = mapmem->start;
+	unifb_fix.smem_len = UNIFB_MEMSIZE;
+
+	info->fix = unifb_fix;
+	info->pseudo_palette = info->par;
+	info->par = NULL;
+	info->flags = FBINFO_FLAG_DEFAULT;
+#ifdef FB_ACCEL_PUV3_UNIGFX
+	info->fix.accel = FB_ACCEL_PUV3_UNIGFX;
+#endif
+
+	retval = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (retval < 0)
+		goto err1;
+
+	retval = register_framebuffer(info);
+	if (retval < 0)
+		goto err2;
+	platform_set_drvdata(dev, info);
+	platform_device_add_data(dev, unifb_regs, sizeof(u32) * UNIFB_REGS_NUM);
+
+	printk(KERN_INFO
+	       "fb%d: Virtual frame buffer device, using %dM of video memory\n",
+	       info->node, UNIFB_MEMSIZE >> 20);
+	return 0;
+err2:
+	fb_dealloc_cmap(&info->cmap);
+err1:
+	framebuffer_release(info);
+err:
+	return retval;
+}
+
+static int unifb_remove(struct platform_device *dev)
+{
+	struct fb_info *info = platform_get_drvdata(dev);
+
+	if (info) {
+		unregister_framebuffer(info);
+		fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int unifb_resume(struct platform_device *dev)
+{
+	int rc = 0;
+	u32 *unifb_regs = dev->dev.platform_data;
+
+	if (dev->dev.power.power_state.event == PM_EVENT_ON)
+		return 0;
+
+	console_lock();
+
+	if (dev->dev.power.power_state.event == PM_EVENT_SUSPEND) {
+		writel(unifb_regs[0], UDE_FSA);
+		writel(unifb_regs[1], UDE_LS);
+		writel(unifb_regs[2], UDE_PS);
+		writel(unifb_regs[3], UDE_HAT);
+		writel(unifb_regs[4], UDE_HBT);
+		writel(unifb_regs[5], UDE_HST);
+		writel(unifb_regs[6], UDE_VAT);
+		writel(unifb_regs[7], UDE_VBT);
+		writel(unifb_regs[8], UDE_VST);
+		writel(unifb_regs[9], UDE_CFG);
+	}
+	dev->dev.power.power_state = PMSG_ON;
+
+	console_unlock();
+
+	return rc;
+}
+
+static int unifb_suspend(struct platform_device *dev, pm_message_t mesg)
+{
+	u32 *unifb_regs = dev->dev.platform_data;
+
+	unifb_regs[0] = readl(UDE_FSA);
+	unifb_regs[1] = readl(UDE_LS);
+	unifb_regs[2] = readl(UDE_PS);
+	unifb_regs[3] = readl(UDE_HAT);
+	unifb_regs[4] = readl(UDE_HBT);
+	unifb_regs[5] = readl(UDE_HST);
+	unifb_regs[6] = readl(UDE_VAT);
+	unifb_regs[7] = readl(UDE_VBT);
+	unifb_regs[8] = readl(UDE_VST);
+	unifb_regs[9] = readl(UDE_CFG);
+
+	if (mesg.event == dev->dev.power.power_state.event)
+		return 0;
+
+	switch (mesg.event) {
+	case PM_EVENT_FREEZE:		/* about to take snapshot */
+	case PM_EVENT_PRETHAW:		/* before restoring snapshot */
+		goto done;
+	}
+
+	console_lock();
+
+	/* do nothing... */
+
+	console_unlock();
+
+done:
+	dev->dev.power.power_state = mesg;
+
+	return 0;
+}
+#else
+#define	unifb_resume	NULL
+#define unifb_suspend	NULL
+#endif
+
+static struct platform_driver unifb_driver = {
+	.probe	 = unifb_probe,
+	.remove  = unifb_remove,
+	.resume  = unifb_resume,
+	.suspend = unifb_suspend,
+	.driver  = {
+		.name	= "PKUnity-v3-UNIGFX",
+	},
+};
+
+static int __init unifb_init(void)
+{
+#ifndef MODULE
+	if (fb_get_options("unifb", NULL))
+		return -ENODEV;
+#endif
+
+	return platform_driver_register(&unifb_driver);
+}
+
+module_init(unifb_init);
+
+static void __exit unifb_exit(void)
+{
+	platform_driver_unregister(&unifb_driver);
+}
+
+module_exit(unifb_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/ffb.c b/drivers/video/ffb.c
index 6739b2af3bc0..910c5e6f6702 100644
--- a/drivers/video/ffb.c
+++ b/drivers/video/ffb.c
@@ -893,8 +893,7 @@ static void ffb_init_fix(struct fb_info *info)
 	info->fix.accel = FB_ACCEL_SUN_CREATOR;
 }
 
-static int __devinit ffb_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit ffb_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct ffb_fbc __iomem *fbc;
@@ -1052,7 +1051,7 @@ static const struct of_device_id ffb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ffb_match);
 
-static struct of_platform_driver ffb_driver = {
+static struct platform_driver ffb_driver = {
 	.driver = {
 		.name = "ffb",
 		.owner = THIS_MODULE,
@@ -1067,12 +1066,12 @@ static int __init ffb_init(void)
 	if (fb_get_options("ffb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&ffb_driver);
+	return platform_driver_register(&ffb_driver);
 }
 
 static void __exit ffb_exit(void)
 {
-	of_unregister_platform_driver(&ffb_driver);
+	platform_driver_unregister(&ffb_driver);
 }
 
 module_init(ffb_init);
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 8bbbf08fa3ce..9048f87fa8c1 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -1487,8 +1487,7 @@ static ssize_t show_monitor(struct device *device,
 	return diu_ops.show_monitor_port(machine_data->monitor_port, buf);
 }
 
-static int __devinit fsl_diu_probe(struct platform_device *ofdev,
-	const struct of_device_id *match)
+static int __devinit fsl_diu_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct mfb_info *mfbi;
@@ -1735,7 +1734,7 @@ static struct of_device_id fsl_diu_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fsl_diu_match);
 
-static struct of_platform_driver fsl_diu_driver = {
+static struct platform_driver fsl_diu_driver = {
 	.driver = {
 		.name = "fsl_diu",
 		.owner = THIS_MODULE,
@@ -1797,7 +1796,7 @@ static int __init fsl_diu_init(void)
 	if (!coherence_data)
 		return -ENOMEM;
 #endif
-	ret = of_register_platform_driver(&fsl_diu_driver);
+	ret = platform_driver_register(&fsl_diu_driver);
 	if (ret) {
 		printk(KERN_ERR
 			"fsl-diu: failed to register platform driver\n");
@@ -1811,7 +1810,7 @@ static int __init fsl_diu_init(void)
 
 static void __exit fsl_diu_exit(void)
 {
-	of_unregister_platform_driver(&fsl_diu_driver);
+	platform_driver_unregister(&fsl_diu_driver);
 #if defined(CONFIG_NOT_COHERENT_CACHE)
 	vfree(coherence_data);
 #endif
diff --git a/drivers/video/leo.c b/drivers/video/leo.c
index b599e5e36ced..9e946e2c1da9 100644
--- a/drivers/video/leo.c
+++ b/drivers/video/leo.c
@@ -547,8 +547,7 @@ static void leo_unmap_regs(struct platform_device *op, struct fb_info *info,
 		of_iounmap(&op->resource[0], info->screen_base, 0x800000);
 }
 
-static int __devinit leo_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit leo_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -662,7 +661,7 @@ static const struct of_device_id leo_match[] = {
 };
 MODULE_DEVICE_TABLE(of, leo_match);
 
-static struct of_platform_driver leo_driver = {
+static struct platform_driver leo_driver = {
 	.driver = {
 		.name = "leo",
 		.owner = THIS_MODULE,
@@ -677,12 +676,12 @@ static int __init leo_init(void)
 	if (fb_get_options("leofb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&leo_driver);
+	return platform_driver_register(&leo_driver);
 }
 
 static void __exit leo_exit(void)
 {
-	of_unregister_platform_driver(&leo_driver);
+	platform_driver_unregister(&leo_driver);
 }
 
 module_init(leo_init);
diff --git a/drivers/video/matrox/matroxfb_base.c b/drivers/video/matrox/matroxfb_base.c
index a082debe824b..8c9dbac9dc1b 100644
--- a/drivers/video/matrox/matroxfb_base.c
+++ b/drivers/video/matrox/matroxfb_base.c
@@ -101,8 +101,6 @@
 
 #include <linux/version.h>
 
-#define __OLD_VIDIOC_
-
 #include "matroxfb_base.h"
 #include "matroxfb_misc.h"
 #include "matroxfb_accel.h"
@@ -1152,7 +1150,6 @@ static int matroxfb_ioctl(struct fb_info *info,
 					return -EFAULT;
 				return err;
 			}
-		case VIDIOC_S_CTRL_OLD:
 		case VIDIOC_S_CTRL:
 			{
 				struct v4l2_control ctrl;
diff --git a/drivers/video/mb862xx/mb862xxfb.c b/drivers/video/mb862xx/mb862xxfb.c
index b1c4374cf940..c76e663a6cd4 100644
--- a/drivers/video/mb862xx/mb862xxfb.c
+++ b/drivers/video/mb862xx/mb862xxfb.c
@@ -550,8 +550,7 @@ static int mb862xx_gdc_init(struct mb862xxfb_par *par)
 	return 0;
 }
 
-static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev,
-					       const struct of_device_id *id)
+static int __devinit of_platform_mb862xx_probe(struct platform_device *ofdev)
 {
 	struct device_node *np = ofdev->dev.of_node;
 	struct device *dev = &ofdev->dev;
@@ -717,7 +716,7 @@ static struct of_device_id __devinitdata of_platform_mb862xx_tbl[] = {
 	{ /* end */ }
 };
 
-static struct of_platform_driver of_platform_mb862xxfb_driver = {
+static struct platform_driver of_platform_mb862xxfb_driver = {
 	.driver = {
 		.name = DRV_NAME,
 		.owner = THIS_MODULE,
@@ -1038,7 +1037,7 @@ static int __devinit mb862xxfb_init(void)
 	int ret = -ENODEV;
 
 #if defined(CONFIG_FB_MB862XX_LIME)
-	ret = of_register_platform_driver(&of_platform_mb862xxfb_driver);
+	ret = platform_driver_register(&of_platform_mb862xxfb_driver);
 #endif
 #if defined(CONFIG_FB_MB862XX_PCI_GDC)
 	ret = pci_register_driver(&mb862xxfb_pci_driver);
@@ -1049,7 +1048,7 @@ static int __devinit mb862xxfb_init(void)
 static void __exit mb862xxfb_exit(void)
 {
 #if defined(CONFIG_FB_MB862XX_LIME)
-	of_unregister_platform_driver(&of_platform_mb862xxfb_driver);
+	platform_driver_unregister(&of_platform_mb862xxfb_driver);
 #endif
 #if defined(CONFIG_FB_MB862XX_PCI_GDC)
 	pci_unregister_driver(&mb862xxfb_pci_driver);
diff --git a/drivers/video/msm/mdp_hw.h b/drivers/video/msm/mdp_hw.h
index 4e3deb4e592b..d80477415caa 100644
--- a/drivers/video/msm/mdp_hw.h
+++ b/drivers/video/msm/mdp_hw.h
@@ -449,6 +449,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 #define PPP_CFG_MDP_XRGB_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 #define PPP_CFG_MDP_RGBA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 #define PPP_CFG_MDP_BGRA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
+#define PPP_CFG_MDP_RGBX_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
 
 #define PPP_CFG_MDP_Y_CBCR_H2V2(dir)   (PPP_##dir##_C2R_8BIT | \
 					PPP_##dir##_C0G_8BIT | \
@@ -488,12 +489,14 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 	MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8)
 #define PPP_PACK_PATTERN_MDP_RGB_888 PPP_PACK_PATTERN_MDP_RGB_565
 #define PPP_PACK_PATTERN_MDP_XRGB_8888 \
-	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
+	MDP_GET_PACK_PATTERN(CLR_B, CLR_G, CLR_R, CLR_ALPHA, 8)
 #define PPP_PACK_PATTERN_MDP_ARGB_8888 PPP_PACK_PATTERN_MDP_XRGB_8888
 #define PPP_PACK_PATTERN_MDP_RGBA_8888 \
 	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
 #define PPP_PACK_PATTERN_MDP_BGRA_8888 \
 	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
+#define PPP_PACK_PATTERN_MDP_RGBX_8888 \
+	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
 #define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1 \
 	MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8)
 #define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V2 PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1
@@ -509,6 +512,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 #define PPP_CHROMA_SAMP_MDP_ARGB_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_RGBA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_BGRA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
+#define PPP_CHROMA_SAMP_MDP_RGBX_8888(dir) PPP_OP_##dir##_CHROMA_RGB
 #define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
 #define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V2(dir) PPP_OP_##dir##_CHROMA_420
 #define PPP_CHROMA_SAMP_MDP_Y_CRCB_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
@@ -523,6 +527,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 	[MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888,\
 	[MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888,\
 	[MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888,\
+	[MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888,\
 	[MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1,\
 	[MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2,\
 	[MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1,\
@@ -536,6 +541,7 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 	[MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888(dir),\
 	[MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888(dir),\
 	[MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888(dir),\
+	[MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888(dir),\
 	[MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1(dir),\
 	[MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2(dir),\
 	[MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1(dir),\
@@ -547,7 +553,8 @@ int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
 		       (img == MDP_YCRYCB_H2V1))
 #define IS_RGB(img) ((img == MDP_RGB_565) | (img == MDP_RGB_888) | \
 		     (img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
-		     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888))
+		     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888) | \
+		     (img == MDP_RGBX_8888))
 #define HAS_ALPHA(img) ((img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
 			(img == MDP_BGRA_8888))
 
diff --git a/drivers/video/msm/mdp_ppp.c b/drivers/video/msm/mdp_ppp.c
index 4ff001f4cbbd..2b6564e8bfea 100644
--- a/drivers/video/msm/mdp_ppp.c
+++ b/drivers/video/msm/mdp_ppp.c
@@ -69,6 +69,7 @@ static uint32_t bytes_per_pixel[] = {
 	[MDP_ARGB_8888] = 4,
 	[MDP_RGBA_8888] = 4,
 	[MDP_BGRA_8888] = 4,
+	[MDP_RGBX_8888] = 4,
 	[MDP_Y_CBCR_H2V1] = 1,
 	[MDP_Y_CBCR_H2V2] = 1,
 	[MDP_Y_CRCB_H2V1] = 1,
diff --git a/drivers/video/msm/msm_fb.c b/drivers/video/msm/msm_fb.c
index debe5933fd2e..ec351309e607 100644
--- a/drivers/video/msm/msm_fb.c
+++ b/drivers/video/msm/msm_fb.c
@@ -81,7 +81,6 @@ struct msmfb_info {
 	spinlock_t update_lock;
 	struct mutex panel_init_lock;
 	wait_queue_head_t frame_wq;
-	struct workqueue_struct *resume_workqueue;
 	struct work_struct resume_work;
 	struct msmfb_callback dma_callback;
 	struct msmfb_callback vsync_callback;
@@ -111,7 +110,7 @@ static void msmfb_handle_dma_interrupt(struct msmfb_callback *callback)
 	if (msmfb->sleeping == UPDATING &&
 	    msmfb->frame_done == msmfb->update_frame) {
 		DLOG(SUSPEND_RESUME, "full update completed\n");
-		queue_work(msmfb->resume_workqueue, &msmfb->resume_work);
+		schedule_work(&msmfb->resume_work);
 	}
 	spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
 	wake_up(&msmfb->frame_wq);
@@ -220,8 +219,8 @@ restart:
 
 	sleeping = msmfb->sleeping;
 	/* on a full update, if the last frame has not completed, wait for it */
-	if (pan_display && (msmfb->frame_requested != msmfb->frame_done ||
-			    sleeping == UPDATING)) {
+	if ((pan_display && msmfb->frame_requested != msmfb->frame_done) ||
+			    sleeping == UPDATING) {
 		int ret;
 		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
 		ret = wait_event_interruptible_timeout(msmfb->frame_wq,
@@ -470,6 +469,18 @@ static void setup_fb_info(struct msmfb_info *msmfb)
 	fb_info->var.yoffset = 0;
 
 	if (msmfb->panel->caps & MSMFB_CAP_PARTIAL_UPDATES) {
+		/*
+		 * Set the param in the fixed screen, so userspace can't
+		 * change it. This will be used to check for the
+		 * capability.
+		 */
+		fb_info->fix.reserved[0] = 0x5444;
+		fb_info->fix.reserved[1] = 0x5055;
+
+		/*
+		 * This preloads the value so that if userspace doesn't
+		 * change it, it will be a full update
+		 */
 		fb_info->var.reserved[0] = 0x54445055;
 		fb_info->var.reserved[1] = 0;
 		fb_info->var.reserved[2] = (uint16_t)msmfb->xres |
@@ -559,12 +570,6 @@ static int msmfb_probe(struct platform_device *pdev)
 	spin_lock_init(&msmfb->update_lock);
 	mutex_init(&msmfb->panel_init_lock);
 	init_waitqueue_head(&msmfb->frame_wq);
-	msmfb->resume_workqueue = create_workqueue("panel_on");
-	if (msmfb->resume_workqueue == NULL) {
-		printk(KERN_ERR "failed to create panel_on workqueue\n");
-		ret = -ENOMEM;
-		goto error_create_workqueue;
-	}
 	INIT_WORK(&msmfb->resume_work, power_on_panel);
 	msmfb->black = kzalloc(msmfb->fb->var.bits_per_pixel*msmfb->xres,
 			       GFP_KERNEL);
@@ -589,8 +594,6 @@ static int msmfb_probe(struct platform_device *pdev)
 	return 0;
 
 error_register_framebuffer:
-	destroy_workqueue(msmfb->resume_workqueue);
-error_create_workqueue:
 	iounmap(fb->screen_base);
 error_setup_fbmem:
 	framebuffer_release(msmfb->fb);
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
new file mode 100644
index 000000000000..7d0284882984
--- /dev/null
+++ b/drivers/video/mxsfb.c
@@ -0,0 +1,919 @@
+/*
+ * Copyright (C) 2010 Juergen Beisert, Pengutronix
+ *
+ * This code is based on:
+ * Author: Vitaly Wool <vital@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Embedded Alley Solutions, Inc All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#define DRIVER_NAME "mxsfb"
+
+/**
+ * @file
+ * @brief LCDIF driver for i.MX23 and i.MX28
+ *
+ * The LCDIF support four modes of operation
+ * - MPU interface (to drive smart displays) -> not supported yet
+ * - VSYNC interface (like MPU interface plus Vsync) -> not supported yet
+ * - Dotclock interface (to drive LC displays with RGB data and sync signals)
+ * - DVI (to drive ITU-R BT656)  -> not supported yet
+ *
+ * This driver depends on a correct setup of the pins used for this purpose
+ * (platform specific).
+ *
+ * For the developer: Don't forget to set the data bus width to the display
+ * in the imx_fb_videomode structure. You will else end up with ugly colours.
+ * If you fight against jitter you can vary the clock delay. This is a feature
+ * of the i.MX28 and you can vary it between 2 ns ... 8 ns in 2 ns steps. Give
+ * the required value in the imx_fb_videomode structure.
+ */
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <mach/mxsfb.h>
+
+#define REG_SET	4
+#define REG_CLR	8
+
+#define LCDC_CTRL			0x00
+#define LCDC_CTRL1			0x10
+#define LCDC_V4_CTRL2			0x20
+#define LCDC_V3_TRANSFER_COUNT		0x20
+#define LCDC_V4_TRANSFER_COUNT		0x30
+#define LCDC_V4_CUR_BUF			0x40
+#define LCDC_V4_NEXT_BUF		0x50
+#define LCDC_V3_CUR_BUF			0x30
+#define LCDC_V3_NEXT_BUF		0x40
+#define LCDC_TIMING			0x60
+#define LCDC_VDCTRL0			0x70
+#define LCDC_VDCTRL1			0x80
+#define LCDC_VDCTRL2			0x90
+#define LCDC_VDCTRL3			0xa0
+#define LCDC_VDCTRL4			0xb0
+#define LCDC_DVICTRL0			0xc0
+#define LCDC_DVICTRL1			0xd0
+#define LCDC_DVICTRL2			0xe0
+#define LCDC_DVICTRL3			0xf0
+#define LCDC_DVICTRL4			0x100
+#define LCDC_V4_DATA			0x180
+#define LCDC_V3_DATA			0x1b0
+#define LCDC_V4_DEBUG0			0x1d0
+#define LCDC_V3_DEBUG0			0x1f0
+
+#define CTRL_SFTRST			(1 << 31)
+#define CTRL_CLKGATE			(1 << 30)
+#define CTRL_BYPASS_COUNT		(1 << 19)
+#define CTRL_VSYNC_MODE			(1 << 18)
+#define CTRL_DOTCLK_MODE		(1 << 17)
+#define CTRL_DATA_SELECT		(1 << 16)
+#define CTRL_SET_BUS_WIDTH(x)		(((x) & 0x3) << 10)
+#define CTRL_GET_BUS_WIDTH(x)		(((x) >> 10) & 0x3)
+#define CTRL_SET_WORD_LENGTH(x)		(((x) & 0x3) << 8)
+#define CTRL_GET_WORD_LENGTH(x)		(((x) >> 8) & 0x3)
+#define CTRL_MASTER			(1 << 5)
+#define CTRL_DF16			(1 << 3)
+#define CTRL_DF18			(1 << 2)
+#define CTRL_DF24			(1 << 1)
+#define CTRL_RUN			(1 << 0)
+
+#define CTRL1_FIFO_CLEAR		(1 << 21)
+#define CTRL1_SET_BYTE_PACKAGING(x)	(((x) & 0xf) << 16)
+#define CTRL1_GET_BYTE_PACKAGING(x)	(((x) >> 16) & 0xf)
+
+#define TRANSFER_COUNT_SET_VCOUNT(x)	(((x) & 0xffff) << 16)
+#define TRANSFER_COUNT_GET_VCOUNT(x)	(((x) >> 16) & 0xffff)
+#define TRANSFER_COUNT_SET_HCOUNT(x)	((x) & 0xffff)
+#define TRANSFER_COUNT_GET_HCOUNT(x)	((x) & 0xffff)
+
+
+#define VDCTRL0_ENABLE_PRESENT		(1 << 28)
+#define VDCTRL0_VSYNC_ACT_HIGH		(1 << 27)
+#define VDCTRL0_HSYNC_ACT_HIGH		(1 << 26)
+#define VDCTRL0_DOTCLK_ACT_FAILING	(1 << 25)
+#define VDCTRL0_ENABLE_ACT_HIGH		(1 << 24)
+#define VDCTRL0_VSYNC_PERIOD_UNIT	(1 << 21)
+#define VDCTRL0_VSYNC_PULSE_WIDTH_UNIT	(1 << 20)
+#define VDCTRL0_HALF_LINE		(1 << 19)
+#define VDCTRL0_HALF_LINE_MODE		(1 << 18)
+#define VDCTRL0_SET_VSYNC_PULSE_WIDTH(x) ((x) & 0x3ffff)
+#define VDCTRL0_GET_VSYNC_PULSE_WIDTH(x) ((x) & 0x3ffff)
+
+#define VDCTRL2_SET_HSYNC_PERIOD(x)	((x) & 0x3ffff)
+#define VDCTRL2_GET_HSYNC_PERIOD(x)	((x) & 0x3ffff)
+
+#define VDCTRL3_MUX_SYNC_SIGNALS	(1 << 29)
+#define VDCTRL3_VSYNC_ONLY		(1 << 28)
+#define SET_HOR_WAIT_CNT(x)		(((x) & 0xfff) << 16)
+#define GET_HOR_WAIT_CNT(x)		(((x) >> 16) & 0xfff)
+#define SET_VERT_WAIT_CNT(x)		((x) & 0xffff)
+#define GET_VERT_WAIT_CNT(x)		((x) & 0xffff)
+
+#define VDCTRL4_SET_DOTCLK_DLY(x)	(((x) & 0x7) << 29) /* v4 only */
+#define VDCTRL4_GET_DOTCLK_DLY(x)	(((x) >> 29) & 0x7) /* v4 only */
+#define VDCTRL4_SYNC_SIGNALS_ON		(1 << 18)
+#define SET_DOTCLK_H_VALID_DATA_CNT(x)	((x) & 0x3ffff)
+
+#define DEBUG0_HSYNC			(1 < 26)
+#define DEBUG0_VSYNC			(1 < 25)
+
+#define MIN_XRES			120
+#define MIN_YRES			120
+
+#define RED 0
+#define GREEN 1
+#define BLUE 2
+#define TRANSP 3
+
+enum mxsfb_devtype {
+	MXSFB_V3,
+	MXSFB_V4,
+};
+
+/* CPU dependent register offsets */
+struct mxsfb_devdata {
+	unsigned transfer_count;
+	unsigned cur_buf;
+	unsigned next_buf;
+	unsigned debug0;
+	unsigned hs_wdth_mask;
+	unsigned hs_wdth_shift;
+	unsigned ipversion;
+};
+
+struct mxsfb_info {
+	struct fb_info fb_info;
+	struct platform_device *pdev;
+	struct clk *clk;
+	void __iomem *base;	/* registers */
+	unsigned allocated_size;
+	int enabled;
+	unsigned ld_intf_width;
+	unsigned dotclk_delay;
+	const struct mxsfb_devdata *devdata;
+	int mapped;
+};
+
+#define mxsfb_is_v3(host) (host->devdata->ipversion == 3)
+#define mxsfb_is_v4(host) (host->devdata->ipversion == 4)
+
+static const struct mxsfb_devdata mxsfb_devdata[] = {
+	[MXSFB_V3] = {
+		.transfer_count = LCDC_V3_TRANSFER_COUNT,
+		.cur_buf = LCDC_V3_CUR_BUF,
+		.next_buf = LCDC_V3_NEXT_BUF,
+		.debug0 = LCDC_V3_DEBUG0,
+		.hs_wdth_mask = 0xff,
+		.hs_wdth_shift = 24,
+		.ipversion = 3,
+	},
+	[MXSFB_V4] = {
+		.transfer_count = LCDC_V4_TRANSFER_COUNT,
+		.cur_buf = LCDC_V4_CUR_BUF,
+		.next_buf = LCDC_V4_NEXT_BUF,
+		.debug0 = LCDC_V4_DEBUG0,
+		.hs_wdth_mask = 0x3fff,
+		.hs_wdth_shift = 18,
+		.ipversion = 4,
+	},
+};
+
+#define to_imxfb_host(x) (container_of(x, struct mxsfb_info, fb_info))
+
+/* mask and shift depends on architecture */
+static inline u32 set_hsync_pulse_width(struct mxsfb_info *host, unsigned val)
+{
+	return (val & host->devdata->hs_wdth_mask) <<
+		host->devdata->hs_wdth_shift;
+}
+
+static inline u32 get_hsync_pulse_width(struct mxsfb_info *host, unsigned val)
+{
+	return (val >> host->devdata->hs_wdth_shift) &
+		host->devdata->hs_wdth_mask;
+}
+
+static const struct fb_bitfield def_rgb565[] = {
+	[RED] = {
+		.offset = 11,
+		.length = 5,
+	},
+	[GREEN] = {
+		.offset = 5,
+		.length = 6,
+	},
+	[BLUE] = {
+		.offset = 0,
+		.length = 5,
+	},
+	[TRANSP] = {	/* no support for transparency */
+		.length = 0,
+	}
+};
+
+static const struct fb_bitfield def_rgb666[] = {
+	[RED] = {
+		.offset = 16,
+		.length = 6,
+	},
+	[GREEN] = {
+		.offset = 8,
+		.length = 6,
+	},
+	[BLUE] = {
+		.offset = 0,
+		.length = 6,
+	},
+	[TRANSP] = {	/* no support for transparency */
+		.length = 0,
+	}
+};
+
+static const struct fb_bitfield def_rgb888[] = {
+	[RED] = {
+		.offset = 16,
+		.length = 8,
+	},
+	[GREEN] = {
+		.offset = 8,
+		.length = 8,
+	},
+	[BLUE] = {
+		.offset = 0,
+		.length = 8,
+	},
+	[TRANSP] = {	/* no support for transparency */
+		.length = 0,
+	}
+};
+
+static inline unsigned chan_to_field(unsigned chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int mxsfb_check_var(struct fb_var_screeninfo *var,
+		struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	const struct fb_bitfield *rgb = NULL;
+
+	if (var->xres < MIN_XRES)
+		var->xres = MIN_XRES;
+	if (var->yres < MIN_YRES)
+		var->yres = MIN_YRES;
+
+	var->xres_virtual = var->xres;
+
+	var->yres_virtual = var->yres;
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		/* always expect RGB 565 */
+		rgb = def_rgb565;
+		break;
+	case 32:
+		switch (host->ld_intf_width) {
+		case STMLCDIF_8BIT:
+			pr_debug("Unsupported LCD bus width mapping\n");
+			break;
+		case STMLCDIF_16BIT:
+		case STMLCDIF_18BIT:
+			/* 24 bit to 18 bit mapping */
+			rgb = def_rgb666;
+			break;
+		case STMLCDIF_24BIT:
+			/* real 24 bit */
+			rgb = def_rgb888;
+			break;
+		}
+		break;
+	default:
+		pr_debug("Unsupported colour depth: %u\n", var->bits_per_pixel);
+		return -EINVAL;
+	}
+
+	/*
+	 * Copy the RGB parameters for this display
+	 * from the machine specific parameters.
+	 */
+	var->red    = rgb[RED];
+	var->green  = rgb[GREEN];
+	var->blue   = rgb[BLUE];
+	var->transp = rgb[TRANSP];
+
+	return 0;
+}
+
+static void mxsfb_enable_controller(struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	u32 reg;
+
+	dev_dbg(&host->pdev->dev, "%s\n", __func__);
+
+	clk_enable(host->clk);
+	clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
+
+	/* if it was disabled, re-enable the mode again */
+	writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_SET);
+
+	/* enable the SYNC signals first, then the DMA engine */
+	reg = readl(host->base + LCDC_VDCTRL4);
+	reg |= VDCTRL4_SYNC_SIGNALS_ON;
+	writel(reg, host->base + LCDC_VDCTRL4);
+
+	writel(CTRL_RUN, host->base + LCDC_CTRL + REG_SET);
+
+	host->enabled = 1;
+}
+
+static void mxsfb_disable_controller(struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	unsigned loop;
+	u32 reg;
+
+	dev_dbg(&host->pdev->dev, "%s\n", __func__);
+
+	/*
+	 * Even if we disable the controller here, it will still continue
+	 * until its FIFOs are running out of data
+	 */
+	writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_CLR);
+
+	loop = 1000;
+	while (loop) {
+		reg = readl(host->base + LCDC_CTRL);
+		if (!(reg & CTRL_RUN))
+			break;
+		loop--;
+	}
+
+	writel(VDCTRL4_SYNC_SIGNALS_ON, host->base + LCDC_VDCTRL4 + REG_CLR);
+
+	clk_disable(host->clk);
+
+	host->enabled = 0;
+}
+
+static int mxsfb_set_par(struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	u32 ctrl, vdctrl0, vdctrl4;
+	int line_size, fb_size;
+	int reenable = 0;
+
+	line_size =  fb_info->var.xres * (fb_info->var.bits_per_pixel >> 3);
+	fb_size = fb_info->var.yres_virtual * line_size;
+
+	if (fb_size > fb_info->fix.smem_len)
+		return -ENOMEM;
+
+	fb_info->fix.line_length = line_size;
+
+	/*
+	 * It seems, you can't re-program the controller if it is still running.
+	 * This may lead into shifted pictures (FIFO issue?).
+	 * So, first stop the controller and drain its FIFOs
+	 */
+	if (host->enabled) {
+		reenable = 1;
+		mxsfb_disable_controller(fb_info);
+	}
+
+	/* clear the FIFOs */
+	writel(CTRL1_FIFO_CLEAR, host->base + LCDC_CTRL1 + REG_SET);
+
+	ctrl = CTRL_BYPASS_COUNT | CTRL_MASTER |
+		CTRL_SET_BUS_WIDTH(host->ld_intf_width);;
+
+	switch (fb_info->var.bits_per_pixel) {
+	case 16:
+		dev_dbg(&host->pdev->dev, "Setting up RGB565 mode\n");
+		ctrl |= CTRL_SET_WORD_LENGTH(0);
+		writel(CTRL1_SET_BYTE_PACKAGING(0xf), host->base + LCDC_CTRL1);
+		break;
+	case 32:
+		dev_dbg(&host->pdev->dev, "Setting up RGB888/666 mode\n");
+		ctrl |= CTRL_SET_WORD_LENGTH(3);
+		switch (host->ld_intf_width) {
+		case STMLCDIF_8BIT:
+			dev_dbg(&host->pdev->dev,
+					"Unsupported LCD bus width mapping\n");
+			return -EINVAL;
+		case STMLCDIF_16BIT:
+		case STMLCDIF_18BIT:
+			/* 24 bit to 18 bit mapping */
+			ctrl |= CTRL_DF24; /* ignore the upper 2 bits in
+					    *  each colour component
+					    */
+			break;
+		case STMLCDIF_24BIT:
+			/* real 24 bit */
+			break;
+		}
+		/* do not use packed pixels = one pixel per word instead */
+		writel(CTRL1_SET_BYTE_PACKAGING(0x7), host->base + LCDC_CTRL1);
+		break;
+	default:
+		dev_dbg(&host->pdev->dev, "Unhandled color depth of %u\n",
+				fb_info->var.bits_per_pixel);
+		return -EINVAL;
+	}
+
+	writel(ctrl, host->base + LCDC_CTRL);
+
+	writel(TRANSFER_COUNT_SET_VCOUNT(fb_info->var.yres) |
+			TRANSFER_COUNT_SET_HCOUNT(fb_info->var.xres),
+			host->base + host->devdata->transfer_count);
+
+	vdctrl0 = VDCTRL0_ENABLE_PRESENT |	/* always in DOTCLOCK mode */
+		VDCTRL0_VSYNC_PERIOD_UNIT |
+		VDCTRL0_VSYNC_PULSE_WIDTH_UNIT |
+		VDCTRL0_SET_VSYNC_PULSE_WIDTH(fb_info->var.vsync_len);
+	if (fb_info->var.sync & FB_SYNC_HOR_HIGH_ACT)
+		vdctrl0 |= VDCTRL0_HSYNC_ACT_HIGH;
+	if (fb_info->var.sync & FB_SYNC_VERT_HIGH_ACT)
+		vdctrl0 |= VDCTRL0_VSYNC_ACT_HIGH;
+	if (fb_info->var.sync & FB_SYNC_DATA_ENABLE_HIGH_ACT)
+		vdctrl0 |= VDCTRL0_ENABLE_ACT_HIGH;
+	if (fb_info->var.sync & FB_SYNC_DOTCLK_FAILING_ACT)
+		vdctrl0 |= VDCTRL0_DOTCLK_ACT_FAILING;
+
+	writel(vdctrl0, host->base + LCDC_VDCTRL0);
+
+	/* frame length in lines */
+	writel(fb_info->var.upper_margin + fb_info->var.vsync_len +
+		fb_info->var.lower_margin + fb_info->var.yres,
+		host->base + LCDC_VDCTRL1);
+
+	/* line length in units of clocks or pixels */
+	writel(set_hsync_pulse_width(host, fb_info->var.hsync_len) |
+		VDCTRL2_SET_HSYNC_PERIOD(fb_info->var.left_margin +
+		fb_info->var.hsync_len + fb_info->var.right_margin +
+		fb_info->var.xres),
+		host->base + LCDC_VDCTRL2);
+
+	writel(SET_HOR_WAIT_CNT(fb_info->var.left_margin +
+		fb_info->var.hsync_len) |
+		SET_VERT_WAIT_CNT(fb_info->var.upper_margin +
+			fb_info->var.vsync_len),
+		host->base + LCDC_VDCTRL3);
+
+	vdctrl4 = SET_DOTCLK_H_VALID_DATA_CNT(fb_info->var.xres);
+	if (mxsfb_is_v4(host))
+		vdctrl4 |= VDCTRL4_SET_DOTCLK_DLY(host->dotclk_delay);
+	writel(vdctrl4, host->base + LCDC_VDCTRL4);
+
+	writel(fb_info->fix.smem_start +
+			fb_info->fix.line_length * fb_info->var.yoffset,
+			host->base + host->devdata->next_buf);
+
+	if (reenable)
+		mxsfb_enable_controller(fb_info);
+
+	return 0;
+}
+
+static int mxsfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
+		u_int transp, struct fb_info *fb_info)
+{
+	unsigned int val;
+	int ret = -EINVAL;
+
+	/*
+	 * If greyscale is true, then we convert the RGB value
+	 * to greyscale no matter what visual we are using.
+	 */
+	if (fb_info->var.grayscale)
+		red = green = blue = (19595 * red + 38470 * green +
+					7471 * blue) >> 16;
+
+	switch (fb_info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		/*
+		 * 12 or 16-bit True Colour.  We encode the RGB value
+		 * according to the RGB bitfield information.
+		 */
+		if (regno < 16) {
+			u32 *pal = fb_info->pseudo_palette;
+
+			val  = chan_to_field(red, &fb_info->var.red);
+			val |= chan_to_field(green, &fb_info->var.green);
+			val |= chan_to_field(blue, &fb_info->var.blue);
+
+			pal[regno] = val;
+			ret = 0;
+		}
+		break;
+
+	case FB_VISUAL_STATIC_PSEUDOCOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		break;
+	}
+
+	return ret;
+}
+
+static int mxsfb_blank(int blank, struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		if (host->enabled)
+			mxsfb_disable_controller(fb_info);
+		break;
+
+	case FB_BLANK_UNBLANK:
+		if (!host->enabled)
+			mxsfb_enable_controller(fb_info);
+		break;
+	}
+	return 0;
+}
+
+static int mxsfb_pan_display(struct fb_var_screeninfo *var,
+		struct fb_info *fb_info)
+{
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	unsigned offset;
+
+	if (var->xoffset != 0)
+		return -EINVAL;
+
+	offset = fb_info->fix.line_length * var->yoffset;
+
+	/* update on next VSYNC */
+	writel(fb_info->fix.smem_start + offset,
+			host->base + host->devdata->next_buf);
+
+	return 0;
+}
+
+static struct fb_ops mxsfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = mxsfb_check_var,
+	.fb_set_par = mxsfb_set_par,
+	.fb_setcolreg = mxsfb_setcolreg,
+	.fb_blank = mxsfb_blank,
+	.fb_pan_display = mxsfb_pan_display,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+static int __devinit mxsfb_restore_mode(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+	unsigned line_count;
+	unsigned period;
+	unsigned long pa, fbsize;
+	int bits_per_pixel, ofs;
+	u32 transfer_count, vdctrl0, vdctrl2, vdctrl3, vdctrl4, ctrl;
+	struct fb_videomode vmode;
+
+	/* Only restore the mode when the controller is running */
+	ctrl = readl(host->base + LCDC_CTRL);
+	if (!(ctrl & CTRL_RUN))
+		return -EINVAL;
+
+	vdctrl0 = readl(host->base + LCDC_VDCTRL0);
+	vdctrl2 = readl(host->base + LCDC_VDCTRL2);
+	vdctrl3 = readl(host->base + LCDC_VDCTRL3);
+	vdctrl4 = readl(host->base + LCDC_VDCTRL4);
+
+	transfer_count = readl(host->base + host->devdata->transfer_count);
+
+	vmode.xres = TRANSFER_COUNT_GET_HCOUNT(transfer_count);
+	vmode.yres = TRANSFER_COUNT_GET_VCOUNT(transfer_count);
+
+	switch (CTRL_GET_WORD_LENGTH(ctrl)) {
+	case 0:
+		bits_per_pixel = 16;
+		break;
+	case 3:
+		bits_per_pixel = 32;
+	case 1:
+	default:
+		return -EINVAL;
+	}
+
+	fb_info->var.bits_per_pixel = bits_per_pixel;
+
+	vmode.pixclock = KHZ2PICOS(clk_get_rate(host->clk) / 1000U);
+	vmode.hsync_len = get_hsync_pulse_width(host, vdctrl2);
+	vmode.left_margin = GET_HOR_WAIT_CNT(vdctrl3) - vmode.hsync_len;
+	vmode.right_margin = VDCTRL2_GET_HSYNC_PERIOD(vdctrl2) - vmode.hsync_len -
+		vmode.left_margin - vmode.xres;
+	vmode.vsync_len = VDCTRL0_GET_VSYNC_PULSE_WIDTH(vdctrl0);
+	period = readl(host->base + LCDC_VDCTRL1);
+	vmode.upper_margin = GET_VERT_WAIT_CNT(vdctrl3) - vmode.vsync_len;
+	vmode.lower_margin = period - vmode.vsync_len - vmode.upper_margin - vmode.yres;
+
+	vmode.vmode = FB_VMODE_NONINTERLACED;
+
+	vmode.sync = 0;
+	if (vdctrl0 & VDCTRL0_HSYNC_ACT_HIGH)
+		vmode.sync |= FB_SYNC_HOR_HIGH_ACT;
+	if (vdctrl0 & VDCTRL0_VSYNC_ACT_HIGH)
+		vmode.sync |= FB_SYNC_VERT_HIGH_ACT;
+
+	pr_debug("Reconstructed video mode:\n");
+	pr_debug("%dx%d, hsync: %u left: %u, right: %u, vsync: %u, upper: %u, lower: %u\n",
+			vmode.xres, vmode.yres,
+			vmode.hsync_len, vmode.left_margin, vmode.right_margin,
+			vmode.vsync_len, vmode.upper_margin, vmode.lower_margin);
+	pr_debug("pixclk: %ldkHz\n", PICOS2KHZ(vmode.pixclock));
+
+	fb_add_videomode(&vmode, &fb_info->modelist);
+
+	host->ld_intf_width = CTRL_GET_BUS_WIDTH(ctrl);
+	host->dotclk_delay = VDCTRL4_GET_DOTCLK_DLY(vdctrl4);
+
+	fb_info->fix.line_length = vmode.xres * (bits_per_pixel >> 3);
+
+	pa = readl(host->base + host->devdata->cur_buf);
+	fbsize = fb_info->fix.line_length * vmode.yres;
+	if (pa < fb_info->fix.smem_start)
+		return -EINVAL;
+	if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len)
+		return -EINVAL;
+	ofs = pa - fb_info->fix.smem_start;
+	if (ofs) {
+		memmove(fb_info->screen_base, fb_info->screen_base + ofs, fbsize);
+		writel(fb_info->fix.smem_start, host->base + host->devdata->next_buf);
+	}
+
+	line_count = fb_info->fix.smem_len / fb_info->fix.line_length;
+	fb_info->fix.ypanstep = 1;
+
+	clk_enable(host->clk);
+	host->enabled = 1;
+
+	return 0;
+}
+
+static int __devinit mxsfb_init_fbinfo(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+	struct fb_var_screeninfo *var = &fb_info->var;
+	struct mxsfb_platform_data *pdata = host->pdev->dev.platform_data;
+	dma_addr_t fb_phys;
+	void *fb_virt;
+	unsigned fb_size = pdata->fb_size;
+
+	fb_info->fbops = &mxsfb_ops;
+	fb_info->flags = FBINFO_FLAG_DEFAULT | FBINFO_READS_FAST;
+	strlcpy(fb_info->fix.id, "mxs", sizeof(fb_info->fix.id));
+	fb_info->fix.type = FB_TYPE_PACKED_PIXELS;
+	fb_info->fix.ypanstep = 1;
+	fb_info->fix.visual = FB_VISUAL_TRUECOLOR,
+	fb_info->fix.accel = FB_ACCEL_NONE;
+
+	var->bits_per_pixel = pdata->default_bpp ? pdata->default_bpp : 16;
+	var->nonstd = 0;
+	var->activate = FB_ACTIVATE_NOW;
+	var->accel_flags = 0;
+	var->vmode = FB_VMODE_NONINTERLACED;
+
+	host->dotclk_delay = pdata->dotclk_delay;
+	host->ld_intf_width = pdata->ld_intf_width;
+
+	/* Memory allocation for framebuffer */
+	if (pdata->fb_phys) {
+		if (!fb_size)
+			return -EINVAL;
+
+		fb_phys = pdata->fb_phys;
+
+		if (!request_mem_region(fb_phys, fb_size, host->pdev->name))
+			return -ENOMEM;
+
+		fb_virt = ioremap(fb_phys, fb_size);
+		if (!fb_virt) {
+			release_mem_region(fb_phys, fb_size);
+			return -ENOMEM;
+		}
+		host->mapped = 1;
+	} else {
+		if (!fb_size)
+			fb_size = SZ_2M; /* default */
+		fb_virt = alloc_pages_exact(fb_size, GFP_DMA);
+		if (!fb_virt)
+			return -ENOMEM;
+
+		fb_phys = virt_to_phys(fb_virt);
+	}
+
+	fb_info->fix.smem_start = fb_phys;
+	fb_info->screen_base = fb_virt;
+	fb_info->screen_size = fb_info->fix.smem_len = fb_size;
+
+	if (mxsfb_restore_mode(host))
+		memset(fb_virt, 0, fb_size);
+
+	return 0;
+}
+
+static void __devexit mxsfb_free_videomem(struct mxsfb_info *host)
+{
+	struct fb_info *fb_info = &host->fb_info;
+
+	if (host->mapped) {
+		iounmap(fb_info->screen_base);
+		release_mem_region(fb_info->fix.smem_start,
+				fb_info->screen_size);
+	} else {
+		free_pages_exact(fb_info->screen_base, fb_info->fix.smem_len);
+	}
+}
+
+static int __devinit mxsfb_probe(struct platform_device *pdev)
+{
+	struct mxsfb_platform_data *pdata = pdev->dev.platform_data;
+	struct resource *res;
+	struct mxsfb_info *host;
+	struct fb_info *fb_info;
+	struct fb_modelist *modelist;
+	int i, ret;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platformdata. Giving up\n");
+		return -ENODEV;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Cannot get memory IO resource\n");
+		return -ENODEV;
+	}
+
+	if (!request_mem_region(res->start, resource_size(res), pdev->name))
+		return -EBUSY;
+
+	fb_info = framebuffer_alloc(sizeof(struct mxsfb_info), &pdev->dev);
+	if (!fb_info) {
+		dev_err(&pdev->dev, "Failed to allocate fbdev\n");
+		ret = -ENOMEM;
+		goto error_alloc_info;
+	}
+
+	host = to_imxfb_host(fb_info);
+
+	host->base = ioremap(res->start, resource_size(res));
+	if (!host->base) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		ret = -ENOMEM;
+		goto error_ioremap;
+	}
+
+	host->pdev = pdev;
+	platform_set_drvdata(pdev, host);
+
+	host->devdata = &mxsfb_devdata[pdev->id_entry->driver_data];
+
+	host->clk = clk_get(&host->pdev->dev, NULL);
+	if (IS_ERR(host->clk)) {
+		ret = PTR_ERR(host->clk);
+		goto error_getclock;
+	}
+
+	fb_info->pseudo_palette = kmalloc(sizeof(u32) * 16, GFP_KERNEL);
+	if (!fb_info->pseudo_palette) {
+		ret = -ENOMEM;
+		goto error_pseudo_pallette;
+	}
+
+	INIT_LIST_HEAD(&fb_info->modelist);
+
+	ret = mxsfb_init_fbinfo(host);
+	if (ret != 0)
+		goto error_init_fb;
+
+	for (i = 0; i < pdata->mode_count; i++)
+		fb_add_videomode(&pdata->mode_list[i], &fb_info->modelist);
+
+	modelist = list_first_entry(&fb_info->modelist,
+			struct fb_modelist, list);
+	fb_videomode_to_var(&fb_info->var, &modelist->mode);
+
+	/* init the color fields */
+	mxsfb_check_var(&fb_info->var, fb_info);
+
+	platform_set_drvdata(pdev, fb_info);
+
+	ret = register_framebuffer(fb_info);
+	if (ret != 0) {
+		dev_err(&pdev->dev,"Failed to register framebuffer\n");
+		goto error_register;
+	}
+
+	if (!host->enabled) {
+		writel(0, host->base + LCDC_CTRL);
+		mxsfb_set_par(fb_info);
+		mxsfb_enable_controller(fb_info);
+	}
+
+	dev_info(&pdev->dev, "initialized\n");
+
+	return 0;
+
+error_register:
+	if (host->enabled)
+		clk_disable(host->clk);
+	fb_destroy_modelist(&fb_info->modelist);
+error_init_fb:
+	kfree(fb_info->pseudo_palette);
+error_pseudo_pallette:
+	clk_put(host->clk);
+error_getclock:
+	iounmap(host->base);
+error_ioremap:
+	framebuffer_release(fb_info);
+error_alloc_info:
+	release_mem_region(res->start, resource_size(res));
+
+	return ret;
+}
+
+static int __devexit mxsfb_remove(struct platform_device *pdev)
+{
+	struct fb_info *fb_info = platform_get_drvdata(pdev);
+	struct mxsfb_info *host = to_imxfb_host(fb_info);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (host->enabled)
+		mxsfb_disable_controller(fb_info);
+
+	unregister_framebuffer(fb_info);
+	kfree(fb_info->pseudo_palette);
+	mxsfb_free_videomem(host);
+	iounmap(host->base);
+	clk_put(host->clk);
+
+	framebuffer_release(fb_info);
+	release_mem_region(res->start, resource_size(res));
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_device_id mxsfb_devtype[] = {
+	{
+		.name = "imx23-fb",
+		.driver_data = MXSFB_V3,
+	}, {
+		.name = "imx28-fb",
+		.driver_data = MXSFB_V4,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, mxsfb_devtype);
+
+static struct platform_driver mxsfb_driver = {
+	.probe = mxsfb_probe,
+	.remove = __devexit_p(mxsfb_remove),
+	.id_table = mxsfb_devtype,
+	.driver = {
+		   .name = DRIVER_NAME,
+	},
+};
+
+static int __init mxsfb_init(void)
+{
+	return platform_driver_register(&mxsfb_driver);
+}
+
+static void __exit mxsfb_exit(void)
+{
+	platform_driver_unregister(&mxsfb_driver);
+}
+
+module_init(mxsfb_init);
+module_exit(mxsfb_exit);
+
+MODULE_DESCRIPTION("Freescale mxs framebuffer driver");
+MODULE_AUTHOR("Sascha Hauer, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap/Kconfig b/drivers/video/omap/Kconfig
index 083c8fe53e24..15e7f1912af9 100644
--- a/drivers/video/omap/Kconfig
+++ b/drivers/video/omap/Kconfig
@@ -5,13 +5,18 @@ config FB_OMAP
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
+	select TWL4030_CORE if MACH_OMAP_2430SDP
 	help
           Frame buffer driver for OMAP based boards.
 
 config FB_OMAP_LCD_VGA
         bool "Use LCD in VGA mode"
 	        depends on MACH_OMAP_3430SDP || MACH_OMAP_LDP
-
+		help
+		  Set LCD resolution as VGA (640 X 480).
+		  Default resolution without this option is QVGA(320 X 240).
+		  Please take a look at drivers/video/omap/lcd_ldp.c file
+		  for lcd driver code.
 choice
 	depends on FB_OMAP && MACH_OVERO
 	prompt "Screen resolution"
diff --git a/drivers/video/omap2/displays/Kconfig b/drivers/video/omap2/displays/Kconfig
index 940cab394c2e..d18ad6b2372a 100644
--- a/drivers/video/omap2/displays/Kconfig
+++ b/drivers/video/omap2/displays/Kconfig
@@ -9,6 +9,12 @@ config PANEL_GENERIC_DPI
 	  Supports LCD Panel used in TI SDP3430 and EVM boards,
 	  OMAP3517 EVM boards and CM-T35.
 
+config PANEL_LGPHILIPS_LB035Q02
+	tristate "LG.Philips LB035Q02 LCD Panel"
+	depends on OMAP2_DSS && SPI
+	help
+	  LCD Panel used on the Gumstix Overo Palo35
+
 config PANEL_SHARP_LS037V7DW01
         tristate "Sharp LS037V7DW01 LCD Panel"
         depends on OMAP2_DSS
diff --git a/drivers/video/omap2/displays/Makefile b/drivers/video/omap2/displays/Makefile
index 861f0255ec6b..0f601ab3abf4 100644
--- a/drivers/video/omap2/displays/Makefile
+++ b/drivers/video/omap2/displays/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_PANEL_GENERIC_DPI) += panel-generic-dpi.o
+obj-$(CONFIG_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
 obj-$(CONFIG_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
 obj-$(CONFIG_PANEL_NEC_NL8048HL11_01B) += panel-nec-nl8048hl11-01b.o
 
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index 07eb30ee59c8..4a9b9ff59467 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -156,6 +156,31 @@ static struct panel_config generic_dpi_panels[] = {
 		.power_off_delay	= 0,
 		.name			= "toppoly_tdo35s",
 	},
+
+	/* Samsung LTE430WQ-F0C */
+	{
+		{
+			.x_res		= 480,
+			.y_res		= 272,
+
+			.pixel_clock	= 9200,
+
+			.hfp		= 8,
+			.hsw		= 41,
+			.hbp		= 45 - 41,
+
+			.vfp		= 4,
+			.vsw		= 10,
+			.vbp		= 12 - 10,
+		},
+		.acbi			= 0x0,
+		.acb			= 0x0,
+		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+						OMAP_DSS_LCD_IHS,
+		.power_on_delay		= 0,
+		.power_off_delay	= 0,
+		.name			= "samsung_lte430wq_f0c",
+	},
 };
 
 struct panel_drv_data {
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
new file mode 100644
index 000000000000..271324db2436
--- /dev/null
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -0,0 +1,279 @@
+/*
+ * LCD panel driver for LG.Philips LB035Q02
+ *
+ * Author: 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.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 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/mutex.h>
+
+#include <plat/display.h>
+
+struct lb035q02_data {
+	struct mutex lock;
+};
+
+static struct omap_video_timings lb035q02_timings = {
+	.x_res = 320,
+	.y_res = 240,
+
+	.pixel_clock	= 6500,
+
+	.hsw		= 2,
+	.hfp		= 20,
+	.hbp		= 68,
+
+	.vsw		= 2,
+	.vfp		= 4,
+	.vbp		= 18,
+};
+
+static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+		return 0;
+
+	r = omapdss_dpi_display_enable(dssdev);
+	if (r)
+		goto err0;
+
+	if (dssdev->platform_enable) {
+		r = dssdev->platform_enable(dssdev);
+		if (r)
+			goto err1;
+	}
+
+	return 0;
+err1:
+	omapdss_dpi_display_disable(dssdev);
+err0:
+	return r;
+}
+
+static void lb035q02_panel_power_off(struct omap_dss_device *dssdev)
+{
+	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+		return;
+
+	if (dssdev->platform_disable)
+		dssdev->platform_disable(dssdev);
+
+	omapdss_dpi_display_disable(dssdev);
+}
+
+static int lb035q02_panel_probe(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld;
+	int r;
+
+	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+		OMAP_DSS_LCD_IHS;
+	dssdev->panel.timings = lb035q02_timings;
+
+	ld = kzalloc(sizeof(*ld), GFP_KERNEL);
+	if (!ld) {
+		r = -ENOMEM;
+		goto err;
+	}
+	mutex_init(&ld->lock);
+	dev_set_drvdata(&dssdev->dev, ld);
+	return 0;
+err:
+	return r;
+}
+
+static void lb035q02_panel_remove(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+	kfree(ld);
+}
+
+static int lb035q02_panel_enable(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&ld->lock);
+
+	r = lb035q02_panel_power_on(dssdev);
+	if (r)
+		goto err;
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	mutex_unlock(&ld->lock);
+	return 0;
+err:
+	mutex_unlock(&ld->lock);
+	return r;
+}
+
+static void lb035q02_panel_disable(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&ld->lock);
+
+	lb035q02_panel_power_off(dssdev);
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+	mutex_unlock(&ld->lock);
+}
+
+static int lb035q02_panel_suspend(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&ld->lock);
+
+	lb035q02_panel_power_off(dssdev);
+	dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+	mutex_unlock(&ld->lock);
+	return 0;
+}
+
+static int lb035q02_panel_resume(struct omap_dss_device *dssdev)
+{
+	struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&ld->lock);
+
+	r = lb035q02_panel_power_on(dssdev);
+	if (r)
+		goto err;
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	mutex_unlock(&ld->lock);
+	return 0;
+err:
+	mutex_unlock(&ld->lock);
+	return r;
+}
+
+static struct omap_dss_driver lb035q02_driver = {
+	.probe		= lb035q02_panel_probe,
+	.remove		= lb035q02_panel_remove,
+
+	.enable		= lb035q02_panel_enable,
+	.disable	= lb035q02_panel_disable,
+	.suspend	= lb035q02_panel_suspend,
+	.resume		= lb035q02_panel_resume,
+
+	.driver         = {
+		.name   = "lgphilips_lb035q02_panel",
+		.owner  = THIS_MODULE,
+	},
+};
+
+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 __devinit lb035q02_panel_spi_probe(struct spi_device *spi)
+{
+	init_lb035q02_panel(spi);
+	return omap_dss_register_driver(&lb035q02_driver);
+}
+
+static int __devexit lb035q02_panel_spi_remove(struct spi_device *spi)
+{
+	omap_dss_unregister_driver(&lb035q02_driver);
+	return 0;
+}
+
+static struct spi_driver lb035q02_spi_driver = {
+	.driver		= {
+		.name	= "lgphilips_lb035q02_panel-spi",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= lb035q02_panel_spi_probe,
+	.remove		= __devexit_p(lb035q02_panel_spi_remove),
+};
+
+static int __init lb035q02_panel_drv_init(void)
+{
+	return spi_register_driver(&lb035q02_spi_driver);
+}
+
+static void __exit lb035q02_panel_drv_exit(void)
+{
+	spi_unregister_driver(&lb035q02_spi_driver);
+}
+
+module_init(lb035q02_panel_drv_init);
+module_exit(lb035q02_panel_drv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index 61026f96ad20..abdfdd81001f 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -218,6 +218,8 @@ struct taal_data {
 		u16 w;
 		u16 h;
 	} update_region;
+	int channel;
+
 	struct delayed_work te_timeout_work;
 
 	bool use_dsi_bl;
@@ -257,12 +259,12 @@ static void hw_guard_wait(struct taal_data *td)
 	}
 }
 
-static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
+static int taal_dcs_read_1(struct taal_data *td, u8 dcs_cmd, u8 *data)
 {
 	int r;
 	u8 buf[1];
 
-	r = dsi_vc_dcs_read(TCH, dcs_cmd, buf, 1);
+	r = dsi_vc_dcs_read(td->channel, dcs_cmd, buf, 1);
 
 	if (r < 0)
 		return r;
@@ -272,17 +274,17 @@ static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
 	return 0;
 }
 
-static int taal_dcs_write_0(u8 dcs_cmd)
+static int taal_dcs_write_0(struct taal_data *td, u8 dcs_cmd)
 {
-	return dsi_vc_dcs_write(TCH, &dcs_cmd, 1);
+	return dsi_vc_dcs_write(td->channel, &dcs_cmd, 1);
 }
 
-static int taal_dcs_write_1(u8 dcs_cmd, u8 param)
+static int taal_dcs_write_1(struct taal_data *td, u8 dcs_cmd, u8 param)
 {
 	u8 buf[2];
 	buf[0] = dcs_cmd;
 	buf[1] = param;
-	return dsi_vc_dcs_write(TCH, buf, 2);
+	return dsi_vc_dcs_write(td->channel, buf, 2);
 }
 
 static int taal_sleep_in(struct taal_data *td)
@@ -294,7 +296,7 @@ static int taal_sleep_in(struct taal_data *td)
 	hw_guard_wait(td);
 
 	cmd = DCS_SLEEP_IN;
-	r = dsi_vc_dcs_write_nosync(TCH, &cmd, 1);
+	r = dsi_vc_dcs_write_nosync(td->channel, &cmd, 1);
 	if (r)
 		return r;
 
@@ -312,7 +314,7 @@ static int taal_sleep_out(struct taal_data *td)
 
 	hw_guard_wait(td);
 
-	r = taal_dcs_write_0(DCS_SLEEP_OUT);
+	r = taal_dcs_write_0(td, DCS_SLEEP_OUT);
 	if (r)
 		return r;
 
@@ -324,30 +326,30 @@ static int taal_sleep_out(struct taal_data *td)
 	return 0;
 }
 
-static int taal_get_id(u8 *id1, u8 *id2, u8 *id3)
+static int taal_get_id(struct taal_data *td, u8 *id1, u8 *id2, u8 *id3)
 {
 	int r;
 
-	r = taal_dcs_read_1(DCS_GET_ID1, id1);
+	r = taal_dcs_read_1(td, DCS_GET_ID1, id1);
 	if (r)
 		return r;
-	r = taal_dcs_read_1(DCS_GET_ID2, id2);
+	r = taal_dcs_read_1(td, DCS_GET_ID2, id2);
 	if (r)
 		return r;
-	r = taal_dcs_read_1(DCS_GET_ID3, id3);
+	r = taal_dcs_read_1(td, DCS_GET_ID3, id3);
 	if (r)
 		return r;
 
 	return 0;
 }
 
-static int taal_set_addr_mode(u8 rotate, bool mirror)
+static int taal_set_addr_mode(struct taal_data *td, u8 rotate, bool mirror)
 {
 	int r;
 	u8 mode;
 	int b5, b6, b7;
 
-	r = taal_dcs_read_1(DCS_READ_MADCTL, &mode);
+	r = taal_dcs_read_1(td, DCS_READ_MADCTL, &mode);
 	if (r)
 		return r;
 
@@ -381,10 +383,11 @@ static int taal_set_addr_mode(u8 rotate, bool mirror)
 	mode &= ~((1<<7) | (1<<6) | (1<<5));
 	mode |= (b7 << 7) | (b6 << 6) | (b5 << 5);
 
-	return taal_dcs_write_1(DCS_MEM_ACC_CTRL, mode);
+	return taal_dcs_write_1(td, DCS_MEM_ACC_CTRL, mode);
 }
 
-static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
+static int taal_set_update_window(struct taal_data *td,
+		u16 x, u16 y, u16 w, u16 h)
 {
 	int r;
 	u16 x1 = x;
@@ -399,7 +402,7 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
 	buf[3] = (x2 >> 8) & 0xff;
 	buf[4] = (x2 >> 0) & 0xff;
 
-	r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+	r = dsi_vc_dcs_write_nosync(td->channel, buf, sizeof(buf));
 	if (r)
 		return r;
 
@@ -409,11 +412,11 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
 	buf[3] = (y2 >> 8) & 0xff;
 	buf[4] = (y2 >> 0) & 0xff;
 
-	r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+	r = dsi_vc_dcs_write_nosync(td->channel, buf, sizeof(buf));
 	if (r)
 		return r;
 
-	dsi_vc_send_bta_sync(TCH);
+	dsi_vc_send_bta_sync(td->channel);
 
 	return r;
 }
@@ -439,7 +442,7 @@ static int taal_bl_update_status(struct backlight_device *dev)
 	if (td->use_dsi_bl) {
 		if (td->enabled) {
 			dsi_bus_lock();
-			r = taal_dcs_write_1(DCS_BRIGHTNESS, level);
+			r = taal_dcs_write_1(td, DCS_BRIGHTNESS, level);
 			dsi_bus_unlock();
 		} else {
 			r = 0;
@@ -502,7 +505,7 @@ static ssize_t taal_num_errors_show(struct device *dev,
 
 	if (td->enabled) {
 		dsi_bus_lock();
-		r = taal_dcs_read_1(DCS_READ_NUM_ERRORS, &errors);
+		r = taal_dcs_read_1(td, DCS_READ_NUM_ERRORS, &errors);
 		dsi_bus_unlock();
 	} else {
 		r = -ENODEV;
@@ -528,7 +531,7 @@ static ssize_t taal_hw_revision_show(struct device *dev,
 
 	if (td->enabled) {
 		dsi_bus_lock();
-		r = taal_get_id(&id1, &id2, &id3);
+		r = taal_get_id(td, &id1, &id2, &id3);
 		dsi_bus_unlock();
 	} else {
 		r = -ENODEV;
@@ -590,7 +593,7 @@ static ssize_t store_cabc_mode(struct device *dev,
 	if (td->enabled) {
 		dsi_bus_lock();
 		if (!td->cabc_broken)
-			taal_dcs_write_1(DCS_WRITE_CABC, i);
+			taal_dcs_write_1(td, DCS_WRITE_CABC, i);
 		dsi_bus_unlock();
 	}
 
@@ -774,14 +777,29 @@ static int taal_probe(struct omap_dss_device *dssdev)
 		dev_dbg(&dssdev->dev, "Using GPIO TE\n");
 	}
 
+	r = omap_dsi_request_vc(dssdev, &td->channel);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to get virtual channel\n");
+		goto err_req_vc;
+	}
+
+	r = omap_dsi_set_vc_id(dssdev, td->channel, TCH);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to set VC_ID\n");
+		goto err_vc_id;
+	}
+
 	r = sysfs_create_group(&dssdev->dev.kobj, &taal_attr_group);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to create sysfs files\n");
-		goto err_sysfs;
+		goto err_vc_id;
 	}
 
 	return 0;
-err_sysfs:
+
+err_vc_id:
+	omap_dsi_release_vc(dssdev, td->channel);
+err_req_vc:
 	if (panel_data->use_ext_te)
 		free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
 err_irq:
@@ -808,6 +826,7 @@ static void taal_remove(struct omap_dss_device *dssdev)
 	dev_dbg(&dssdev->dev, "remove\n");
 
 	sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
+	omap_dsi_release_vc(dssdev, td->channel);
 
 	if (panel_data->use_ext_te) {
 		int gpio = panel_data->ext_te_gpio;
@@ -846,13 +865,13 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 
 	taal_hw_reset(dssdev);
 
-	omapdss_dsi_vc_enable_hs(TCH, false);
+	omapdss_dsi_vc_enable_hs(td->channel, false);
 
 	r = taal_sleep_out(td);
 	if (r)
 		goto err;
 
-	r = taal_get_id(&id1, &id2, &id3);
+	r = taal_get_id(td, &id1, &id2, &id3);
 	if (r)
 		goto err;
 
@@ -861,30 +880,30 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 		(id2 == 0x00 || id2 == 0xff || id2 == 0x81))
 		td->cabc_broken = true;
 
-	r = taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
+	r = taal_dcs_write_1(td, DCS_BRIGHTNESS, 0xff);
 	if (r)
 		goto err;
 
-	r = taal_dcs_write_1(DCS_CTRL_DISPLAY,
+	r = taal_dcs_write_1(td, DCS_CTRL_DISPLAY,
 			(1<<2) | (1<<5));	/* BL | BCTRL */
 	if (r)
 		goto err;
 
-	r = taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+	r = taal_dcs_write_1(td, DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
 	if (r)
 		goto err;
 
-	r = taal_set_addr_mode(td->rotate, td->mirror);
+	r = taal_set_addr_mode(td, td->rotate, td->mirror);
 	if (r)
 		goto err;
 
 	if (!td->cabc_broken) {
-		r = taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+		r = taal_dcs_write_1(td, DCS_WRITE_CABC, td->cabc_mode);
 		if (r)
 			goto err;
 	}
 
-	r = taal_dcs_write_0(DCS_DISPLAY_ON);
+	r = taal_dcs_write_0(td, DCS_DISPLAY_ON);
 	if (r)
 		goto err;
 
@@ -903,7 +922,7 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 		td->intro_printed = true;
 	}
 
-	omapdss_dsi_vc_enable_hs(TCH, true);
+	omapdss_dsi_vc_enable_hs(td->channel, true);
 
 	return 0;
 err:
@@ -921,7 +940,7 @@ static void taal_power_off(struct omap_dss_device *dssdev)
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
 	int r;
 
-	r = taal_dcs_write_0(DCS_DISPLAY_OFF);
+	r = taal_dcs_write_0(td, DCS_DISPLAY_OFF);
 	if (!r) {
 		r = taal_sleep_in(td);
 		/* HACK: wait a bit so that the message goes through */
@@ -1089,7 +1108,7 @@ static irqreturn_t taal_te_isr(int irq, void *data)
 	if (old) {
 		cancel_delayed_work(&td->te_timeout_work);
 
-		r = omap_dsi_update(dssdev, TCH,
+		r = omap_dsi_update(dssdev, td->channel,
 				td->update_region.x,
 				td->update_region.y,
 				td->update_region.w,
@@ -1139,7 +1158,7 @@ static int taal_update(struct omap_dss_device *dssdev,
 	if (r)
 		goto err;
 
-	r = taal_set_update_window(x, y, w, h);
+	r = taal_set_update_window(td, x, y, w, h);
 	if (r)
 		goto err;
 
@@ -1153,7 +1172,7 @@ static int taal_update(struct omap_dss_device *dssdev,
 				msecs_to_jiffies(250));
 		atomic_set(&td->do_update, 1);
 	} else {
-		r = omap_dsi_update(dssdev, TCH, x, y, w, h,
+		r = omap_dsi_update(dssdev, td->channel, x, y, w, h,
 				taal_framedone_cb, dssdev);
 		if (r)
 			goto err;
@@ -1191,9 +1210,9 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 	int r;
 
 	if (enable)
-		r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+		r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
 	else
-		r = taal_dcs_write_0(DCS_TEAR_OFF);
+		r = taal_dcs_write_0(td, DCS_TEAR_OFF);
 
 	if (!panel_data->use_ext_te)
 		omapdss_dsi_enable_te(dssdev, enable);
@@ -1263,7 +1282,7 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
 	dsi_bus_lock();
 
 	if (td->enabled) {
-		r = taal_set_addr_mode(rotate, td->mirror);
+		r = taal_set_addr_mode(td, rotate, td->mirror);
 		if (r)
 			goto err;
 	}
@@ -1306,7 +1325,7 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
 
 	dsi_bus_lock();
 	if (td->enabled) {
-		r = taal_set_addr_mode(td->rotate, enable);
+		r = taal_set_addr_mode(td, td->rotate, enable);
 		if (r)
 			goto err;
 	}
@@ -1350,13 +1369,13 @@ static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
 
 	dsi_bus_lock();
 
-	r = taal_dcs_read_1(DCS_GET_ID1, &id1);
+	r = taal_dcs_read_1(td, DCS_GET_ID1, &id1);
 	if (r)
 		goto err2;
-	r = taal_dcs_read_1(DCS_GET_ID2, &id2);
+	r = taal_dcs_read_1(td, DCS_GET_ID2, &id2);
 	if (r)
 		goto err2;
-	r = taal_dcs_read_1(DCS_GET_ID3, &id3);
+	r = taal_dcs_read_1(td, DCS_GET_ID3, &id3);
 	if (r)
 		goto err2;
 
@@ -1404,9 +1423,9 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 	else
 		plen = 2;
 
-	taal_set_update_window(x, y, w, h);
+	taal_set_update_window(td, x, y, w, h);
 
-	r = dsi_vc_set_max_rx_packet_size(TCH, plen);
+	r = dsi_vc_set_max_rx_packet_size(td->channel, plen);
 	if (r)
 		goto err2;
 
@@ -1414,7 +1433,7 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 		u8 dcs_cmd = first ? 0x2e : 0x3e;
 		first = 0;
 
-		r = dsi_vc_dcs_read(TCH, dcs_cmd,
+		r = dsi_vc_dcs_read(td->channel, dcs_cmd,
 				buf + buf_used, size - buf_used);
 
 		if (r < 0) {
@@ -1440,7 +1459,7 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
 	r = buf_used;
 
 err3:
-	dsi_vc_set_max_rx_packet_size(TCH, 1);
+	dsi_vc_set_max_rx_packet_size(td->channel, 1);
 err2:
 	dsi_bus_unlock();
 err1:
@@ -1466,7 +1485,7 @@ static void taal_esd_work(struct work_struct *work)
 
 	dsi_bus_lock();
 
-	r = taal_dcs_read_1(DCS_RDDSDR, &state1);
+	r = taal_dcs_read_1(td, DCS_RDDSDR, &state1);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to read Taal status\n");
 		goto err;
@@ -1479,7 +1498,7 @@ static void taal_esd_work(struct work_struct *work)
 		goto err;
 	}
 
-	r = taal_dcs_read_1(DCS_RDDSDR, &state2);
+	r = taal_dcs_read_1(td, DCS_RDDSDR, &state2);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to read Taal status\n");
 		goto err;
@@ -1495,7 +1514,7 @@ static void taal_esd_work(struct work_struct *work)
 	/* Self-diagnostics result is also shown on TE GPIO line. We need
 	 * to re-enable TE after self diagnostics */
 	if (td->te_enabled && panel_data->use_ext_te) {
-		r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+		r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
 		if (r)
 			goto err;
 	}
diff --git a/drivers/video/omap2/dss/Kconfig b/drivers/video/omap2/dss/Kconfig
index 43b64403eaa4..db01473c3307 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -1,8 +1,8 @@
 menuconfig OMAP2_DSS
-        tristate "OMAP2/3 Display Subsystem support (EXPERIMENTAL)"
-        depends on ARCH_OMAP2 || ARCH_OMAP3
+        tristate "OMAP2+ Display Subsystem support (EXPERIMENTAL)"
+        depends on ARCH_OMAP2PLUS
         help
-          OMAP2/3 Display Subsystem support.
+	  OMAP2+ Display Subsystem support.
 
 if OMAP2_DSS
 
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index 8e89f6049280..c2f930bf2844 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -34,332 +34,26 @@
 #include <linux/regulator/consumer.h>
 
 #include <plat/display.h>
-#include <plat/clock.h>
 
 #include "dss.h"
 #include "dss_features.h"
 
 static struct {
 	struct platform_device *pdev;
-	int		ctx_id;
-
-	struct clk      *dss_ick;
-	struct clk	*dss1_fck;
-	struct clk	*dss2_fck;
-	struct clk      *dss_54m_fck;
-	struct clk	*dss_96m_fck;
-	unsigned	num_clks_enabled;
 
 	struct regulator *vdds_dsi_reg;
 	struct regulator *vdds_sdi_reg;
-	struct regulator *vdda_dac_reg;
 } core;
 
-static void dss_clk_enable_all_no_ctx(void);
-static void dss_clk_disable_all_no_ctx(void);
-static void dss_clk_enable_no_ctx(enum dss_clock clks);
-static void dss_clk_disable_no_ctx(enum dss_clock clks);
-
 static char *def_disp_name;
 module_param_named(def_disp, def_disp_name, charp, 0);
-MODULE_PARM_DESC(def_disp_name, "default display name");
+MODULE_PARM_DESC(def_disp, "default display name");
 
 #ifdef DEBUG
 unsigned int dss_debug;
 module_param_named(debug, dss_debug, bool, 0644);
 #endif
 
-/* CONTEXT */
-static int dss_get_ctx_id(void)
-{
-	struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
-	int r;
-
-	if (!pdata->get_last_off_on_transaction_id)
-		return 0;
-	r = pdata->get_last_off_on_transaction_id(&core.pdev->dev);
-	if (r < 0) {
-		dev_err(&core.pdev->dev, "getting transaction ID failed, "
-				"will force context restore\n");
-		r = -1;
-	}
-	return r;
-}
-
-int dss_need_ctx_restore(void)
-{
-	int id = dss_get_ctx_id();
-
-	if (id < 0 || id != core.ctx_id) {
-		DSSDBG("ctx id %d -> id %d\n",
-				core.ctx_id, id);
-		core.ctx_id = id;
-		return 1;
-	} else {
-		return 0;
-	}
-}
-
-static void save_all_ctx(void)
-{
-	DSSDBG("save context\n");
-
-	dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-	dss_save_context();
-	dispc_save_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_save_context();
-#endif
-
-	dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-}
-
-static void restore_all_ctx(void)
-{
-	DSSDBG("restore context\n");
-
-	dss_clk_enable_all_no_ctx();
-
-	dss_restore_context();
-	dispc_restore_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_restore_context();
-#endif
-
-	dss_clk_disable_all_no_ctx();
-}
-
-#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-/* CLOCKS */
-static void core_dump_clocks(struct seq_file *s)
-{
-	int i;
-	struct clk *clocks[5] = {
-		core.dss_ick,
-		core.dss1_fck,
-		core.dss2_fck,
-		core.dss_54m_fck,
-		core.dss_96m_fck
-	};
-
-	seq_printf(s, "- CORE -\n");
-
-	seq_printf(s, "internal clk count\t\t%u\n", core.num_clks_enabled);
-
-	for (i = 0; i < 5; i++) {
-		if (!clocks[i])
-			continue;
-		seq_printf(s, "%-15s\t%lu\t%d\n",
-				clocks[i]->name,
-				clk_get_rate(clocks[i]),
-				clocks[i]->usecount);
-	}
-}
-#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
-
-static int dss_get_clock(struct clk **clock, const char *clk_name)
-{
-	struct clk *clk;
-
-	clk = clk_get(&core.pdev->dev, clk_name);
-
-	if (IS_ERR(clk)) {
-		DSSERR("can't get clock %s", clk_name);
-		return PTR_ERR(clk);
-	}
-
-	*clock = clk;
-
-	DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
-
-	return 0;
-}
-
-static int dss_get_clocks(void)
-{
-	int r;
-
-	core.dss_ick = NULL;
-	core.dss1_fck = NULL;
-	core.dss2_fck = NULL;
-	core.dss_54m_fck = NULL;
-	core.dss_96m_fck = NULL;
-
-	r = dss_get_clock(&core.dss_ick, "ick");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss1_fck, "dss1_fck");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss2_fck, "dss2_fck");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss_54m_fck, "tv_fck");
-	if (r)
-		goto err;
-
-	r = dss_get_clock(&core.dss_96m_fck, "video_fck");
-	if (r)
-		goto err;
-
-	return 0;
-
-err:
-	if (core.dss_ick)
-		clk_put(core.dss_ick);
-	if (core.dss1_fck)
-		clk_put(core.dss1_fck);
-	if (core.dss2_fck)
-		clk_put(core.dss2_fck);
-	if (core.dss_54m_fck)
-		clk_put(core.dss_54m_fck);
-	if (core.dss_96m_fck)
-		clk_put(core.dss_96m_fck);
-
-	return r;
-}
-
-static void dss_put_clocks(void)
-{
-	if (core.dss_96m_fck)
-		clk_put(core.dss_96m_fck);
-	clk_put(core.dss_54m_fck);
-	clk_put(core.dss1_fck);
-	clk_put(core.dss2_fck);
-	clk_put(core.dss_ick);
-}
-
-unsigned long dss_clk_get_rate(enum dss_clock clk)
-{
-	switch (clk) {
-	case DSS_CLK_ICK:
-		return clk_get_rate(core.dss_ick);
-	case DSS_CLK_FCK1:
-		return clk_get_rate(core.dss1_fck);
-	case DSS_CLK_FCK2:
-		return clk_get_rate(core.dss2_fck);
-	case DSS_CLK_54M:
-		return clk_get_rate(core.dss_54m_fck);
-	case DSS_CLK_96M:
-		return clk_get_rate(core.dss_96m_fck);
-	}
-
-	BUG();
-	return 0;
-}
-
-static unsigned count_clk_bits(enum dss_clock clks)
-{
-	unsigned num_clks = 0;
-
-	if (clks & DSS_CLK_ICK)
-		++num_clks;
-	if (clks & DSS_CLK_FCK1)
-		++num_clks;
-	if (clks & DSS_CLK_FCK2)
-		++num_clks;
-	if (clks & DSS_CLK_54M)
-		++num_clks;
-	if (clks & DSS_CLK_96M)
-		++num_clks;
-
-	return num_clks;
-}
-
-static void dss_clk_enable_no_ctx(enum dss_clock clks)
-{
-	unsigned num_clks = count_clk_bits(clks);
-
-	if (clks & DSS_CLK_ICK)
-		clk_enable(core.dss_ick);
-	if (clks & DSS_CLK_FCK1)
-		clk_enable(core.dss1_fck);
-	if (clks & DSS_CLK_FCK2)
-		clk_enable(core.dss2_fck);
-	if (clks & DSS_CLK_54M)
-		clk_enable(core.dss_54m_fck);
-	if (clks & DSS_CLK_96M)
-		clk_enable(core.dss_96m_fck);
-
-	core.num_clks_enabled += num_clks;
-}
-
-void dss_clk_enable(enum dss_clock clks)
-{
-	bool check_ctx = core.num_clks_enabled == 0;
-
-	dss_clk_enable_no_ctx(clks);
-
-	if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
-		restore_all_ctx();
-}
-
-static void dss_clk_disable_no_ctx(enum dss_clock clks)
-{
-	unsigned num_clks = count_clk_bits(clks);
-
-	if (clks & DSS_CLK_ICK)
-		clk_disable(core.dss_ick);
-	if (clks & DSS_CLK_FCK1)
-		clk_disable(core.dss1_fck);
-	if (clks & DSS_CLK_FCK2)
-		clk_disable(core.dss2_fck);
-	if (clks & DSS_CLK_54M)
-		clk_disable(core.dss_54m_fck);
-	if (clks & DSS_CLK_96M)
-		clk_disable(core.dss_96m_fck);
-
-	core.num_clks_enabled -= num_clks;
-}
-
-void dss_clk_disable(enum dss_clock clks)
-{
-	if (cpu_is_omap34xx()) {
-		unsigned num_clks = count_clk_bits(clks);
-
-		BUG_ON(core.num_clks_enabled < num_clks);
-
-		if (core.num_clks_enabled == num_clks)
-			save_all_ctx();
-	}
-
-	dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_enable_all_no_ctx(void)
-{
-	enum dss_clock clks;
-
-	clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-	if (cpu_is_omap34xx())
-		clks |= DSS_CLK_96M;
-	dss_clk_enable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all_no_ctx(void)
-{
-	enum dss_clock clks;
-
-	clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-	if (cpu_is_omap34xx())
-		clks |= DSS_CLK_96M;
-	dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all(void)
-{
-	enum dss_clock clks;
-
-	clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-	if (cpu_is_omap34xx())
-		clks |= DSS_CLK_96M;
-	dss_clk_disable(clks);
-}
-
 /* REGULATORS */
 
 struct regulator *dss_get_vdds_dsi(void)
@@ -390,32 +84,7 @@ struct regulator *dss_get_vdds_sdi(void)
 	return reg;
 }
 
-struct regulator *dss_get_vdda_dac(void)
-{
-	struct regulator *reg;
-
-	if (core.vdda_dac_reg != NULL)
-		return core.vdda_dac_reg;
-
-	reg = regulator_get(&core.pdev->dev, "vdda_dac");
-	if (!IS_ERR(reg))
-		core.vdda_dac_reg = reg;
-
-	return reg;
-}
-
-/* DEBUGFS */
 #if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-static void dss_debug_dump_clocks(struct seq_file *s)
-{
-	core_dump_clocks(s);
-	dss_dump_clocks(s);
-	dispc_dump_clocks(s);
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_dump_clocks(s);
-#endif
-}
-
 static int dss_debug_show(struct seq_file *s, void *unused)
 {
 	void (*func)(struct seq_file *) = s->private;
@@ -497,7 +166,6 @@ static inline void dss_uninitialize_debugfs(void)
 static int omap_dss_probe(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int skip_init = 0;
 	int r;
 	int i;
 
@@ -508,63 +176,37 @@ static int omap_dss_probe(struct platform_device *pdev)
 	dss_init_overlay_managers(pdev);
 	dss_init_overlays(pdev);
 
-	r = dss_get_clocks();
-	if (r)
-		goto err_clocks;
-
-	dss_clk_enable_all_no_ctx();
-
-	core.ctx_id = dss_get_ctx_id();
-	DSSDBG("initial ctx id %u\n", core.ctx_id);
-
-#ifdef CONFIG_FB_OMAP_BOOTLOADER_INIT
-	/* DISPC_CONTROL */
-	if (omap_readl(0x48050440) & 1)	/* LCD enabled? */
-		skip_init = 1;
-#endif
-
-	r = dss_init(skip_init);
+	r = dss_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize DSS\n");
+		DSSERR("Failed to initialize DSS platform driver\n");
 		goto err_dss;
 	}
 
-	r = rfbi_init();
-	if (r) {
-		DSSERR("Failed to initialize rfbi\n");
-		goto err_rfbi;
-	}
+	/* keep clocks enabled to prevent context saves/restores during init */
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
-	r = dpi_init(pdev);
+	r = rfbi_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize dpi\n");
-		goto err_dpi;
+		DSSERR("Failed to initialize rfbi platform driver\n");
+		goto err_rfbi;
 	}
 
-	r = dispc_init();
+	r = dispc_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize dispc\n");
+		DSSERR("Failed to initialize dispc platform driver\n");
 		goto err_dispc;
 	}
 
-	r = venc_init(pdev);
+	r = venc_init_platform_driver();
 	if (r) {
-		DSSERR("Failed to initialize venc\n");
+		DSSERR("Failed to initialize venc platform driver\n");
 		goto err_venc;
 	}
 
-	if (cpu_is_omap34xx()) {
-		r = sdi_init(skip_init);
-		if (r) {
-			DSSERR("Failed to initialize SDI\n");
-			goto err_sdi;
-		}
-
-		r = dsi_init(pdev);
-		if (r) {
-			DSSERR("Failed to initialize DSI\n");
-			goto err_dsi;
-		}
+	r = dsi_init_platform_driver();
+	if (r) {
+		DSSERR("Failed to initialize DSI platform driver\n");
+		goto err_dsi;
 	}
 
 	r = dss_initialize_debugfs();
@@ -589,32 +231,23 @@ static int omap_dss_probe(struct platform_device *pdev)
 			pdata->default_device = dssdev;
 	}
 
-	dss_clk_disable_all();
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	return 0;
 
 err_register:
 	dss_uninitialize_debugfs();
 err_debugfs:
-	if (cpu_is_omap34xx())
-		dsi_exit();
+	dsi_uninit_platform_driver();
 err_dsi:
-	if (cpu_is_omap34xx())
-		sdi_exit();
-err_sdi:
-	venc_exit();
+	venc_uninit_platform_driver();
 err_venc:
-	dispc_exit();
+	dispc_uninit_platform_driver();
 err_dispc:
-	dpi_exit();
-err_dpi:
-	rfbi_exit();
+	rfbi_uninit_platform_driver();
 err_rfbi:
-	dss_exit();
+	dss_uninit_platform_driver();
 err_dss:
-	dss_clk_disable_all_no_ctx();
-	dss_put_clocks();
-err_clocks:
 
 	return r;
 }
@@ -623,61 +256,14 @@ static int omap_dss_remove(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
 	int i;
-	int c;
 
 	dss_uninitialize_debugfs();
 
-	venc_exit();
-	dispc_exit();
-	dpi_exit();
-	rfbi_exit();
-	if (cpu_is_omap34xx()) {
-		dsi_exit();
-		sdi_exit();
-	}
-
-	dss_exit();
-
-	/* these should be removed at some point */
-	c = core.dss_ick->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss_ick usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss_ick);
-	}
-
-	c = core.dss1_fck->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss1_fck usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss1_fck);
-	}
-
-	c = core.dss2_fck->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss2_fck usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss2_fck);
-	}
-
-	c = core.dss_54m_fck->usecount;
-	if (c > 0) {
-		DSSERR("warning: dss_54m_fck usecount %d, disabling\n", c);
-		while (c-- > 0)
-			clk_disable(core.dss_54m_fck);
-	}
-
-	if (core.dss_96m_fck) {
-		c = core.dss_96m_fck->usecount;
-		if (c > 0) {
-			DSSERR("warning: dss_96m_fck usecount %d, disabling\n",
-					c);
-			while (c-- > 0)
-				clk_disable(core.dss_96m_fck);
-		}
-	}
-
-	dss_put_clocks();
+	venc_uninit_platform_driver();
+	dispc_uninit_platform_driver();
+	rfbi_uninit_platform_driver();
+	dsi_uninit_platform_driver();
+	dss_uninit_platform_driver();
 
 	dss_uninit_overlays(pdev);
 	dss_uninit_overlay_managers(pdev);
@@ -965,11 +551,6 @@ static void __exit omap_dss_exit(void)
 		core.vdds_sdi_reg = NULL;
 	}
 
-	if (core.vdda_dac_reg != NULL) {
-		regulator_put(core.vdda_dac_reg);
-		core.vdda_dac_reg = NULL;
-	}
-
 	platform_driver_unregister(&omap_dss_driver);
 
 	omap_dss_bus_unregister();
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index 9f8c69f16e61..4a368c47700e 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -32,6 +32,7 @@
 #include <linux/delay.h>
 #include <linux/workqueue.h>
 #include <linux/hardirq.h>
+#include <linux/interrupt.h>
 
 #include <plat/sram.h>
 #include <plat/clock.h>
@@ -42,8 +43,6 @@
 #include "dss_features.h"
 
 /* DISPC */
-#define DISPC_BASE			0x48050400
-
 #define DISPC_SZ_REGS			SZ_4K
 
 struct dispc_reg { u16 idx; };
@@ -74,7 +73,7 @@ struct dispc_reg { u16 idx; };
 #define DISPC_TIMING_H(ch)		DISPC_REG(ch != 2 ? 0x0064 : 0x0400)
 #define DISPC_TIMING_V(ch)		DISPC_REG(ch != 2 ? 0x0068 : 0x0404)
 #define DISPC_POL_FREQ(ch)		DISPC_REG(ch != 2 ? 0x006C : 0x0408)
-#define DISPC_DIVISOR(ch)		DISPC_REG(ch != 2 ? 0x0070 : 0x040C)
+#define DISPC_DIVISORo(ch)		DISPC_REG(ch != 2 ? 0x0070 : 0x040C)
 #define DISPC_GLOBAL_ALPHA		DISPC_REG(0x0074)
 #define DISPC_SIZE_DIG			DISPC_REG(0x0078)
 #define DISPC_SIZE_LCD(ch)		DISPC_REG(ch != 2 ? 0x007C : 0x03CC)
@@ -129,6 +128,7 @@ struct dispc_reg { u16 idx; };
 
 #define DISPC_VID_PRELOAD(n)		DISPC_REG(0x230 + (n)*0x04)
 
+#define DISPC_DIVISOR			DISPC_REG(0x0804)
 
 #define DISPC_IRQ_MASK_ERROR            (DISPC_IRQ_GFX_FIFO_UNDERFLOW | \
 					 DISPC_IRQ_OCP_ERR | \
@@ -178,7 +178,9 @@ struct dispc_irq_stats {
 };
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem    *base;
+	int irq;
 
 	u32	fifo_size[3];
 
@@ -230,7 +232,7 @@ void dispc_save_context(void)
 	SR(TIMING_H(0));
 	SR(TIMING_V(0));
 	SR(POL_FREQ(0));
-	SR(DIVISOR(0));
+	SR(DIVISORo(0));
 	SR(GLOBAL_ALPHA);
 	SR(SIZE_DIG);
 	SR(SIZE_LCD(0));
@@ -242,7 +244,7 @@ void dispc_save_context(void)
 		SR(TIMING_H(2));
 		SR(TIMING_V(2));
 		SR(POL_FREQ(2));
-		SR(DIVISOR(2));
+		SR(DIVISORo(2));
 		SR(CONFIG2);
 	}
 
@@ -373,6 +375,9 @@ void dispc_save_context(void)
 	SR(VID_FIR_COEF_V(1, 7));
 
 	SR(VID_PRELOAD(1));
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV))
+		SR(DIVISOR);
 }
 
 void dispc_restore_context(void)
@@ -389,7 +394,7 @@ void dispc_restore_context(void)
 	RR(TIMING_H(0));
 	RR(TIMING_V(0));
 	RR(POL_FREQ(0));
-	RR(DIVISOR(0));
+	RR(DIVISORo(0));
 	RR(GLOBAL_ALPHA);
 	RR(SIZE_DIG);
 	RR(SIZE_LCD(0));
@@ -400,7 +405,7 @@ void dispc_restore_context(void)
 		RR(TIMING_H(2));
 		RR(TIMING_V(2));
 		RR(POL_FREQ(2));
-		RR(DIVISOR(2));
+		RR(DIVISORo(2));
 		RR(CONFIG2);
 	}
 
@@ -532,6 +537,9 @@ void dispc_restore_context(void)
 
 	RR(VID_PRELOAD(1));
 
+	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))
@@ -552,9 +560,9 @@ void dispc_restore_context(void)
 static inline void enable_clocks(bool enable)
 {
 	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 bool dispc_go_busy(enum omap_channel channel)
@@ -1129,10 +1137,16 @@ static void _dispc_set_vid_accu0(enum omap_plane plane, int haccu, int vaccu)
 	u32 val;
 	const struct dispc_reg ac0_reg[] = { DISPC_VID_ACCU0(0),
 				      DISPC_VID_ACCU0(1) };
+	u8 hor_start, hor_end, vert_start, vert_end;
 
 	BUG_ON(plane == OMAP_DSS_GFX);
 
-	val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
+	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(ac0_reg[plane-1], val);
 }
 
@@ -1141,10 +1155,16 @@ static void _dispc_set_vid_accu1(enum omap_plane plane, int haccu, int vaccu)
 	u32 val;
 	const struct dispc_reg ac1_reg[] = { DISPC_VID_ACCU1(0),
 				      DISPC_VID_ACCU1(1) };
+	u8 hor_start, hor_end, vert_start, vert_end;
 
 	BUG_ON(plane == OMAP_DSS_GFX);
 
-	val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
+	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(ac1_reg[plane-1], val);
 }
 
@@ -1182,16 +1202,25 @@ static void _dispc_set_scaling(enum omap_plane plane,
 	_dispc_set_fir(plane, fir_hinc, fir_vinc);
 
 	l = dispc_read_reg(dispc_reg_att[plane]);
-	l &= ~((0x0f << 5) | (0x3 << 21));
 
+	/* RESIZEENABLE and VERTICALTAPS */
+	l &= ~((0x3 << 5) | (0x1 << 21));
 	l |= fir_hinc ? (1 << 5) : 0;
 	l |= fir_vinc ? (1 << 6) : 0;
+	l |= five_taps ? (1 << 21) : 0;
 
-	l |= hscaleup ? 0 : (1 << 7);
-	l |= vscaleup ? 0 : (1 << 8);
+	/* VRESIZECONF and HRESIZECONF */
+	if (dss_has_feature(FEAT_RESIZECONF)) {
+		l &= ~(0x3 << 7);
+		l |= hscaleup ? 0 : (1 << 7);
+		l |= vscaleup ? 0 : (1 << 8);
+	}
 
-	l |= five_taps ? (1 << 21) : 0;
-	l |= five_taps ? (1 << 22) : 0;
+	/* LINEBUFFERSPLIT */
+	if (dss_has_feature(FEAT_LINEBUFFERSPLIT)) {
+		l &= ~(0x1 << 22);
+		l |= five_taps ? (1 << 22) : 0;
+	}
 
 	dispc_write_reg(dispc_reg_att[plane], l);
 
@@ -1215,9 +1244,11 @@ static void _dispc_set_scaling(enum omap_plane plane,
 static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
 		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) {
-		int vidrot = 0;
 
 		if (mirroring) {
 			switch (rotation) {
@@ -1251,16 +1282,15 @@ static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
 			}
 		}
 
-		REG_FLD_MOD(dispc_reg_att[plane], vidrot, 13, 12);
-
 		if (rotation == OMAP_DSS_ROT_90 || rotation == OMAP_DSS_ROT_270)
-			REG_FLD_MOD(dispc_reg_att[plane], 0x1, 18, 18);
+			row_repeat = true;
 		else
-			REG_FLD_MOD(dispc_reg_att[plane], 0x0, 18, 18);
-	} else {
-		REG_FLD_MOD(dispc_reg_att[plane], 0, 13, 12);
-		REG_FLD_MOD(dispc_reg_att[plane], 0, 18, 18);
+			row_repeat = false;
 	}
+
+	REG_FLD_MOD(dispc_reg_att[plane], vidrot, 13, 12);
+	if (dss_has_feature(FEAT_ROWREPEATENABLE))
+		REG_FLD_MOD(dispc_reg_att[plane], row_repeat ? 1 : 0, 18, 18);
 }
 
 static int color_mode_to_bpp(enum omap_color_mode color_mode)
@@ -2293,7 +2323,7 @@ static void dispc_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
 	BUG_ON(pck_div < 2);
 
 	enable_clocks(1);
-	dispc_write_reg(DISPC_DIVISOR(channel),
+	dispc_write_reg(DISPC_DIVISORo(channel),
 			FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
 	enable_clocks(0);
 }
@@ -2302,7 +2332,7 @@ static void dispc_get_lcd_divisor(enum omap_channel channel, int *lck_div,
 		int *pck_div)
 {
 	u32 l;
-	l = dispc_read_reg(DISPC_DIVISOR(channel));
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
 	*lck_div = FLD_GET(l, 23, 16);
 	*pck_div = FLD_GET(l, 7, 0);
 }
@@ -2311,11 +2341,11 @@ unsigned long dispc_fclk_rate(void)
 {
 	unsigned long r = 0;
 
-	if (dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK)
-		r = dss_clk_get_rate(DSS_CLK_FCK1);
+	if (dss_get_dispc_clk_source() == DSS_CLK_SRC_FCK)
+		r = dss_clk_get_rate(DSS_CLK_FCK);
 	else
 #ifdef CONFIG_OMAP2_DSS_DSI
-		r = dsi_get_dsi1_pll_rate();
+		r = dsi_get_pll_hsdiv_dispc_rate();
 #else
 	BUG();
 #endif
@@ -2328,7 +2358,7 @@ unsigned long dispc_lclk_rate(enum omap_channel channel)
 	unsigned long r;
 	u32 l;
 
-	l = dispc_read_reg(DISPC_DIVISOR(channel));
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
 
 	lcd = FLD_GET(l, 23, 16);
 
@@ -2343,7 +2373,7 @@ unsigned long dispc_pclk_rate(enum omap_channel channel)
 	unsigned long r;
 	u32 l;
 
-	l = dispc_read_reg(DISPC_DIVISOR(channel));
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
 
 	lcd = FLD_GET(l, 23, 16);
 	pcd = FLD_GET(l, 7, 0);
@@ -2356,17 +2386,27 @@ unsigned long dispc_pclk_rate(enum omap_channel channel)
 void dispc_dump_clocks(struct seq_file *s)
 {
 	int lcd, pcd;
+	u32 l;
+	enum dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
 
 	enable_clocks(1);
 
 	seq_printf(s, "- DISPC -\n");
 
-	seq_printf(s, "dispc fclk source = %s\n",
-			dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-			"dss1_alwon_fclk" : "dsi1_pll_fclk");
+	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);
+	}
 	seq_printf(s, "- LCD1 -\n");
 
 	dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD, &lcd, &pcd);
@@ -2440,7 +2480,7 @@ void dispc_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dispc_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	DUMPREG(DISPC_REVISION);
 	DUMPREG(DISPC_SYSCONFIG);
@@ -2459,7 +2499,7 @@ void dispc_dump_regs(struct seq_file *s)
 	DUMPREG(DISPC_TIMING_H(0));
 	DUMPREG(DISPC_TIMING_V(0));
 	DUMPREG(DISPC_POL_FREQ(0));
-	DUMPREG(DISPC_DIVISOR(0));
+	DUMPREG(DISPC_DIVISORo(0));
 	DUMPREG(DISPC_GLOBAL_ALPHA);
 	DUMPREG(DISPC_SIZE_DIG);
 	DUMPREG(DISPC_SIZE_LCD(0));
@@ -2471,7 +2511,7 @@ void dispc_dump_regs(struct seq_file *s)
 		DUMPREG(DISPC_TIMING_H(2));
 		DUMPREG(DISPC_TIMING_V(2));
 		DUMPREG(DISPC_POL_FREQ(2));
-		DUMPREG(DISPC_DIVISOR(2));
+		DUMPREG(DISPC_DIVISORo(2));
 		DUMPREG(DISPC_SIZE_LCD(2));
 	}
 
@@ -2597,7 +2637,7 @@ void dispc_dump_regs(struct seq_file *s)
 	DUMPREG(DISPC_VID_PRELOAD(0));
 	DUMPREG(DISPC_VID_PRELOAD(1));
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
@@ -2713,8 +2753,8 @@ int dispc_get_clock_div(enum omap_channel channel,
 
 	fck = dispc_fclk_rate();
 
-	cinfo->lck_div = REG_GET(DISPC_DIVISOR(channel), 23, 16);
-	cinfo->pck_div = REG_GET(DISPC_DIVISOR(channel), 7, 0);
+	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;
@@ -2791,6 +2831,9 @@ int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
 		break;
 	}
 
+	if (ret)
+		goto err;
+
 	_omap_dispc_set_irqs();
 
 	spin_unlock_irqrestore(&dispc.irq_lock, flags);
@@ -2866,10 +2909,10 @@ static void print_irq_status(u32 status)
  * 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. */
-void dispc_irq_handler(void)
+static irqreturn_t omap_dispc_irq_handler(int irq, void *arg)
 {
 	int i;
-	u32 irqstatus;
+	u32 irqstatus, irqenable;
 	u32 handledirqs = 0;
 	u32 unhandled_errors;
 	struct omap_dispc_isr_data *isr_data;
@@ -2878,6 +2921,13 @@ void dispc_irq_handler(void)
 	spin_lock(&dispc.irq_lock);
 
 	irqstatus = dispc_read_reg(DISPC_IRQSTATUS);
+	irqenable = dispc_read_reg(DISPC_IRQENABLE);
+
+	/* IRQ is not for us */
+	if (!(irqstatus & irqenable)) {
+		spin_unlock(&dispc.irq_lock);
+		return IRQ_NONE;
+	}
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
 	spin_lock(&dispc.irq_stats_lock);
@@ -2929,6 +2979,8 @@ void dispc_irq_handler(void)
 	}
 
 	spin_unlock(&dispc.irq_lock);
+
+	return IRQ_HANDLED;
 }
 
 static void dispc_error_worker(struct work_struct *work)
@@ -3253,6 +3305,15 @@ static void _omap_dispc_initial_config(void)
 	l = FLD_MOD(l, 1, 0, 0);	/* AUTOIDLE */
 	dispc_write_reg(DISPC_SYSCONFIG, 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);
+	}
+
 	/* FUNCGATED */
 	if (dss_has_feature(FEAT_FUNCGATED))
 		REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
@@ -3269,47 +3330,6 @@ static void _omap_dispc_initial_config(void)
 	dispc_read_plane_fifo_sizes();
 }
 
-int dispc_init(void)
-{
-	u32 rev;
-
-	spin_lock_init(&dispc.irq_lock);
-
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-	spin_lock_init(&dispc.irq_stats_lock);
-	dispc.irq_stats.last_reset = jiffies;
-#endif
-
-	INIT_WORK(&dispc.error_work, dispc_error_worker);
-
-	dispc.base = ioremap(DISPC_BASE, DISPC_SZ_REGS);
-	if (!dispc.base) {
-		DSSERR("can't ioremap DISPC\n");
-		return -ENOMEM;
-	}
-
-	enable_clocks(1);
-
-	_omap_dispc_initial_config();
-
-	_omap_dispc_initialize_irq();
-
-	dispc_save_context();
-
-	rev = dispc_read_reg(DISPC_REVISION);
-	printk(KERN_INFO "OMAP DISPC rev %d.%d\n",
-	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-	enable_clocks(0);
-
-	return 0;
-}
-
-void dispc_exit(void)
-{
-	iounmap(dispc.base);
-}
-
 int dispc_enable_plane(enum omap_plane plane, bool enable)
 {
 	DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
@@ -3359,3 +3379,94 @@ int dispc_setup_plane(enum omap_plane plane,
 
 	return r;
 }
+
+/* DISPC HW IP initialisation */
+static int omap_dispchw_probe(struct platform_device *pdev)
+{
+	u32 rev;
+	int r = 0;
+	struct resource *dispc_mem;
+
+	dispc.pdev = pdev;
+
+	spin_lock_init(&dispc.irq_lock);
+
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+	spin_lock_init(&dispc.irq_stats_lock);
+	dispc.irq_stats.last_reset = jiffies;
+#endif
+
+	INIT_WORK(&dispc.error_work, dispc_error_worker);
+
+	dispc_mem = platform_get_resource(dispc.pdev, IORESOURCE_MEM, 0);
+	if (!dispc_mem) {
+		DSSERR("can't get IORESOURCE_MEM DISPC\n");
+		r = -EINVAL;
+		goto fail0;
+	}
+	dispc.base = ioremap(dispc_mem->start, resource_size(dispc_mem));
+	if (!dispc.base) {
+		DSSERR("can't ioremap DISPC\n");
+		r = -ENOMEM;
+		goto fail0;
+	}
+	dispc.irq = platform_get_irq(dispc.pdev, 0);
+	if (dispc.irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		r = -ENODEV;
+		goto fail1;
+	}
+
+	r = request_irq(dispc.irq, omap_dispc_irq_handler, IRQF_SHARED,
+		"OMAP DISPC", dispc.pdev);
+	if (r < 0) {
+		DSSERR("request_irq failed\n");
+		goto fail1;
+	}
+
+	enable_clocks(1);
+
+	_omap_dispc_initial_config();
+
+	_omap_dispc_initialize_irq();
+
+	dispc_save_context();
+
+	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));
+
+	enable_clocks(0);
+
+	return 0;
+fail1:
+	iounmap(dispc.base);
+fail0:
+	return r;
+}
+
+static int omap_dispchw_remove(struct platform_device *pdev)
+{
+	free_irq(dispc.irq, dispc.pdev);
+	iounmap(dispc.base);
+	return 0;
+}
+
+static struct platform_driver omap_dispchw_driver = {
+	.probe          = omap_dispchw_probe,
+	.remove         = omap_dispchw_remove,
+	.driver         = {
+		.name   = "omapdss_dispc",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int dispc_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dispchw_driver);
+}
+
+void dispc_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_dispchw_driver);
+}
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 22dd7a474f79..7d96b724c992 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -25,14 +25,11 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/jiffies.h>
-#include <linux/list.h>
 #include <linux/platform_device.h>
 
 #include <plat/display.h>
 #include "dss.h"
 
-static LIST_HEAD(display_list);
-
 static ssize_t display_enabled_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -396,29 +393,6 @@ void dss_init_device(struct platform_device *pdev,
 	switch (dssdev->type) {
 #ifdef CONFIG_OMAP2_DSS_DPI
 	case OMAP_DISPLAY_TYPE_DPI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_RFBI
-	case OMAP_DISPLAY_TYPE_DBI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_SDI
-	case OMAP_DISPLAY_TYPE_SDI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_DSI
-	case OMAP_DISPLAY_TYPE_DSI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_VENC
-	case OMAP_DISPLAY_TYPE_VENC:
-#endif
-		break;
-	default:
-		DSSERR("Support for display '%s' not compiled in.\n",
-				dssdev->name);
-		return;
-	}
-
-	switch (dssdev->type) {
-#ifdef CONFIG_OMAP2_DSS_DPI
-	case OMAP_DISPLAY_TYPE_DPI:
 		r = dpi_init_display(dssdev);
 		break;
 #endif
@@ -443,7 +417,9 @@ void dss_init_device(struct platform_device *pdev,
 		break;
 #endif
 	default:
-		BUG();
+		DSSERR("Support for display '%s' not compiled in.\n",
+				dssdev->name);
+		return;
 	}
 
 	if (r) {
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 75fb0a515430..2d3ca4ca4a05 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -57,13 +57,13 @@ static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
 	if (r)
 		return r;
 
-	dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
+	dss_select_dispc_clk_source(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC);
 
 	r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
 	if (r)
 		return r;
 
-	*fck = dsi_cinfo.dsi1_pll_fclk;
+	*fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
 	*lck_div = dispc_cinfo.lck_div;
 	*pck_div = dispc_cinfo.pck_div;
 
@@ -107,7 +107,7 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 	bool is_tft;
 	int r = 0;
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
 			dssdev->panel.acbi, dssdev->panel.acb);
@@ -137,7 +137,7 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 	dispc_set_lcd_timings(dssdev->manager->id, t);
 
 err0:
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 	return r;
 }
 
@@ -173,14 +173,14 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 			goto err1;
 	}
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	r = dpi_basic_init(dssdev);
 	if (r)
 		goto err2;
 
 #ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	dss_clk_enable(DSS_CLK_FCK2);
+	dss_clk_enable(DSS_CLK_SYSCK);
 	r = dsi_pll_init(dssdev, 0, 1);
 	if (r)
 		goto err3;
@@ -199,10 +199,10 @@ err4:
 #ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
 	dsi_pll_uninit();
 err3:
-	dss_clk_disable(DSS_CLK_FCK2);
+	dss_clk_disable(DSS_CLK_SYSCK);
 #endif
 err2:
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 	if (cpu_is_omap34xx())
 		regulator_disable(dpi.vdds_dsi_reg);
 err1:
@@ -217,12 +217,12 @@ void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 	dssdev->manager->disable(dssdev->manager);
 
 #ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-	dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+	dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
 	dsi_pll_uninit();
-	dss_clk_disable(DSS_CLK_FCK2);
+	dss_clk_disable(DSS_CLK_SYSCK);
 #endif
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	if (cpu_is_omap34xx())
 		regulator_disable(dpi.vdds_dsi_reg);
@@ -271,7 +271,7 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 		if (r)
 			return r;
 
-		fck = dsi_cinfo.dsi1_pll_fclk;
+		fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
 		lck_div = dispc_cinfo.lck_div;
 		pck_div = dispc_cinfo.pck_div;
 	}
@@ -303,22 +303,27 @@ int dpi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("init_display\n");
 
-	return 0;
-}
+	if (cpu_is_omap34xx() && dpi.vdds_dsi_reg == NULL) {
+		struct regulator *vdds_dsi;
 
-int dpi_init(struct platform_device *pdev)
-{
-	if (cpu_is_omap34xx()) {
-		dpi.vdds_dsi_reg = dss_get_vdds_dsi();
-		if (IS_ERR(dpi.vdds_dsi_reg)) {
+		vdds_dsi = dss_get_vdds_dsi();
+
+		if (IS_ERR(vdds_dsi)) {
 			DSSERR("can't get VDDS_DSI regulator\n");
-			return PTR_ERR(dpi.vdds_dsi_reg);
+			return PTR_ERR(vdds_dsi);
 		}
+
+		dpi.vdds_dsi_reg = vdds_dsi;
 	}
 
 	return 0;
 }
 
+int dpi_init(void)
+{
+	return 0;
+}
+
 void dpi_exit(void)
 {
 }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index ddf3a0560822..be7694ff7ca7 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -38,12 +38,11 @@
 #include <plat/clock.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 /*#define VERBOSE_IRQ*/
 #define DSI_CATCH_MISSING_TE
 
-#define DSI_BASE		0x4804FC00
-
 struct dsi_reg { u16 idx; };
 
 #define DSI_REG(idx)		((const struct dsi_reg) { idx })
@@ -190,8 +189,8 @@ struct dsi_reg { u16 idx; };
 #define FINT_MIN 750000
 #define REGN_MAX (1 << 7)
 #define REGM_MAX ((1 << 11) - 1)
-#define REGM3_MAX (1 << 4)
-#define REGM4_MAX (1 << 4)
+#define REGM_DISPC_MAX (1 << 4)
+#define REGM_DSI_MAX (1 << 4)
 #define LP_DIV_MAX ((1 << 13) - 1)
 
 enum fifo_size {
@@ -222,7 +221,9 @@ struct dsi_irq_stats {
 
 static struct
 {
+	struct platform_device *pdev;
 	void __iomem	*base;
+	int irq;
 
 	struct dsi_clock_info current_cinfo;
 
@@ -232,6 +233,7 @@ static struct
 		enum dsi_vc_mode mode;
 		struct omap_dss_device *dssdev;
 		enum fifo_size fifo_size;
+		int vc_id;
 	} vc[4];
 
 	struct mutex lock;
@@ -481,13 +483,17 @@ static void print_irq_status_cio(u32 status)
 static int debug_irq;
 
 /* called from dss */
-void dsi_irq_handler(void)
+static irqreturn_t omap_dsi_irq_handler(int irq, void *arg)
 {
 	u32 irqstatus, vcstatus, ciostatus;
 	int i;
 
 	irqstatus = dsi_read_reg(DSI_IRQSTATUS);
 
+	/* IRQ is not for us */
+	if (!irqstatus)
+		return IRQ_NONE;
+
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
 	spin_lock(&dsi.irq_stats_lock);
 	dsi.irq_stats.irq_count++;
@@ -565,9 +571,9 @@ void dsi_irq_handler(void)
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
 	spin_unlock(&dsi.irq_stats_lock);
 #endif
+	return IRQ_HANDLED;
 }
 
-
 static void _dsi_initialize_irq(void)
 {
 	u32 l;
@@ -637,22 +643,22 @@ static void dsi_vc_disable_bta_irq(int channel)
 	dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
 }
 
-/* DSI func clock. this could also be DSI2_PLL_FCLK */
+/* DSI func clock. this could also be dsi_pll_hsdiv_dsi_clk */
 static inline void enable_clocks(bool enable)
 {
 	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 /* source clock for DSI PLL. this could also be PCLKFREE */
 static inline void dsi_enable_pll_clock(bool enable)
 {
 	if (enable)
-		dss_clk_enable(DSS_CLK_FCK2);
+		dss_clk_enable(DSS_CLK_SYSCK);
 	else
-		dss_clk_disable(DSS_CLK_FCK2);
+		dss_clk_disable(DSS_CLK_SYSCK);
 
 	if (enable && dsi.pll_locked) {
 		if (wait_for_bit_change(DSI_PLL_STATUS, 1, 1) != 1)
@@ -707,14 +713,14 @@ static inline int dsi_if_enable(bool enable)
 	return 0;
 }
 
-unsigned long dsi_get_dsi1_pll_rate(void)
+unsigned long dsi_get_pll_hsdiv_dispc_rate(void)
 {
-	return dsi.current_cinfo.dsi1_pll_fclk;
+	return dsi.current_cinfo.dsi_pll_hsdiv_dispc_clk;
 }
 
-static unsigned long dsi_get_dsi2_pll_rate(void)
+static unsigned long dsi_get_pll_hsdiv_dsi_rate(void)
 {
-	return dsi.current_cinfo.dsi2_pll_fclk;
+	return dsi.current_cinfo.dsi_pll_hsdiv_dsi_clk;
 }
 
 static unsigned long dsi_get_txbyteclkhs(void)
@@ -726,12 +732,12 @@ static unsigned long dsi_fclk_rate(void)
 {
 	unsigned long r;
 
-	if (dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK) {
-		/* DSI FCLK source is DSS1_ALWON_FCK, which is dss1_fck */
-		r = dss_clk_get_rate(DSS_CLK_FCK1);
+	if (dss_get_dsi_clk_source() == DSS_CLK_SRC_FCK) {
+		/* DSI FCLK source is DSS_CLK_FCK */
+		r = dss_clk_get_rate(DSS_CLK_FCK);
 	} else {
-		/* DSI FCLK source is DSI2_PLL_FCLK */
-		r = dsi_get_dsi2_pll_rate();
+		/* DSI FCLK source is dsi_pll_hsdiv_dsi_clk */
+		r = dsi_get_pll_hsdiv_dsi_rate();
 	}
 
 	return r;
@@ -801,16 +807,16 @@ static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
 	if (cinfo->regm == 0 || cinfo->regm > REGM_MAX)
 		return -EINVAL;
 
-	if (cinfo->regm3 > REGM3_MAX)
+	if (cinfo->regm_dispc > REGM_DISPC_MAX)
 		return -EINVAL;
 
-	if (cinfo->regm4 > REGM4_MAX)
+	if (cinfo->regm_dsi > REGM_DSI_MAX)
 		return -EINVAL;
 
-	if (cinfo->use_dss2_fck) {
-		cinfo->clkin = dss_clk_get_rate(DSS_CLK_FCK2);
+	if (cinfo->use_sys_clk) {
+		cinfo->clkin = dss_clk_get_rate(DSS_CLK_SYSCK);
 		/* XXX it is unclear if highfreq should be used
-		 * with DSS2_FCK source also */
+		 * with DSS_SYS_CLK source also */
 		cinfo->highfreq = 0;
 	} else {
 		cinfo->clkin = dispc_pclk_rate(dssdev->manager->id);
@@ -831,15 +837,17 @@ static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
 	if (cinfo->clkin4ddr > 1800 * 1000 * 1000)
 		return -EINVAL;
 
-	if (cinfo->regm3 > 0)
-		cinfo->dsi1_pll_fclk = cinfo->clkin4ddr / cinfo->regm3;
+	if (cinfo->regm_dispc > 0)
+		cinfo->dsi_pll_hsdiv_dispc_clk =
+			cinfo->clkin4ddr / cinfo->regm_dispc;
 	else
-		cinfo->dsi1_pll_fclk = 0;
+		cinfo->dsi_pll_hsdiv_dispc_clk = 0;
 
-	if (cinfo->regm4 > 0)
-		cinfo->dsi2_pll_fclk = cinfo->clkin4ddr / cinfo->regm4;
+	if (cinfo->regm_dsi > 0)
+		cinfo->dsi_pll_hsdiv_dsi_clk =
+			cinfo->clkin4ddr / cinfo->regm_dsi;
 	else
-		cinfo->dsi2_pll_fclk = 0;
+		cinfo->dsi_pll_hsdiv_dsi_clk = 0;
 
 	return 0;
 }
@@ -852,23 +860,25 @@ int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
 	struct dispc_clock_info best_dispc;
 	int min_fck_per_pck;
 	int match = 0;
-	unsigned long dss_clk_fck2;
+	unsigned long dss_sys_clk, max_dss_fck;
 
-	dss_clk_fck2 = dss_clk_get_rate(DSS_CLK_FCK2);
+	dss_sys_clk = dss_clk_get_rate(DSS_CLK_SYSCK);
+
+	max_dss_fck = dss_feat_get_max_dss_fck();
 
 	if (req_pck == dsi.cache_req_pck &&
-			dsi.cache_cinfo.clkin == dss_clk_fck2) {
+			dsi.cache_cinfo.clkin == dss_sys_clk) {
 		DSSDBG("DSI clock info found from cache\n");
 		*dsi_cinfo = dsi.cache_cinfo;
-		dispc_find_clk_divs(is_tft, req_pck, dsi_cinfo->dsi1_pll_fclk,
-				dispc_cinfo);
+		dispc_find_clk_divs(is_tft, req_pck,
+			dsi_cinfo->dsi_pll_hsdiv_dispc_clk, dispc_cinfo);
 		return 0;
 	}
 
 	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
 	if (min_fck_per_pck &&
-		req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+		req_pck * min_fck_per_pck > max_dss_fck) {
 		DSSERR("Requested pixel clock not possible with the current "
 				"OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
 				"the constraint off.\n");
@@ -882,8 +892,8 @@ retry:
 	memset(&best_dispc, 0, sizeof(best_dispc));
 
 	memset(&cur, 0, sizeof(cur));
-	cur.clkin = dss_clk_fck2;
-	cur.use_dss2_fck = 1;
+	cur.clkin = dss_sys_clk;
+	cur.use_sys_clk = 1;
 	cur.highfreq = 0;
 
 	/* no highfreq: 0.75MHz < Fint = clkin / regn < 2.1MHz */
@@ -909,30 +919,32 @@ retry:
 			if (cur.clkin4ddr > 1800 * 1000 * 1000)
 				break;
 
-			/* DSI1_PLL_FCLK(MHz) = DSIPHY(MHz) / regm3  < 173MHz */
-			for (cur.regm3 = 1; cur.regm3 < REGM3_MAX;
-					++cur.regm3) {
+			/* dsi_pll_hsdiv_dispc_clk(MHz) =
+			 * DSIPHY(MHz) / regm_dispc  < 173MHz/186Mhz */
+			for (cur.regm_dispc = 1; cur.regm_dispc < REGM_DISPC_MAX;
+					++cur.regm_dispc) {
 				struct dispc_clock_info cur_dispc;
-				cur.dsi1_pll_fclk = cur.clkin4ddr / cur.regm3;
+				cur.dsi_pll_hsdiv_dispc_clk =
+					cur.clkin4ddr / cur.regm_dispc;
 
 				/* this will narrow down the search a bit,
 				 * but still give pixclocks below what was
 				 * requested */
-				if (cur.dsi1_pll_fclk  < req_pck)
+				if (cur.dsi_pll_hsdiv_dispc_clk  < req_pck)
 					break;
 
-				if (cur.dsi1_pll_fclk > DISPC_MAX_FCK)
+				if (cur.dsi_pll_hsdiv_dispc_clk > max_dss_fck)
 					continue;
 
 				if (min_fck_per_pck &&
-					cur.dsi1_pll_fclk <
+					cur.dsi_pll_hsdiv_dispc_clk <
 						req_pck * min_fck_per_pck)
 					continue;
 
 				match = 1;
 
 				dispc_find_clk_divs(is_tft, req_pck,
-						cur.dsi1_pll_fclk,
+						cur.dsi_pll_hsdiv_dispc_clk,
 						&cur_dispc);
 
 				if (abs(cur_dispc.pck - req_pck) <
@@ -961,9 +973,9 @@ found:
 		return -EINVAL;
 	}
 
-	/* DSI2_PLL_FCLK (regm4) is not used */
-	best.regm4 = 0;
-	best.dsi2_pll_fclk = 0;
+	/* dsi_pll_hsdiv_dsi_clk (regm_dsi) is not used */
+	best.regm_dsi = 0;
+	best.dsi_pll_hsdiv_dsi_clk = 0;
 
 	if (dsi_cinfo)
 		*dsi_cinfo = best;
@@ -987,18 +999,20 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
 	dsi.current_cinfo.fint = cinfo->fint;
 	dsi.current_cinfo.clkin4ddr = cinfo->clkin4ddr;
-	dsi.current_cinfo.dsi1_pll_fclk = cinfo->dsi1_pll_fclk;
-	dsi.current_cinfo.dsi2_pll_fclk = cinfo->dsi2_pll_fclk;
+	dsi.current_cinfo.dsi_pll_hsdiv_dispc_clk =
+			cinfo->dsi_pll_hsdiv_dispc_clk;
+	dsi.current_cinfo.dsi_pll_hsdiv_dsi_clk =
+			cinfo->dsi_pll_hsdiv_dsi_clk;
 
 	dsi.current_cinfo.regn = cinfo->regn;
 	dsi.current_cinfo.regm = cinfo->regm;
-	dsi.current_cinfo.regm3 = cinfo->regm3;
-	dsi.current_cinfo.regm4 = cinfo->regm4;
+	dsi.current_cinfo.regm_dispc = cinfo->regm_dispc;
+	dsi.current_cinfo.regm_dsi = cinfo->regm_dsi;
 
 	DSSDBG("DSI Fint %ld\n", cinfo->fint);
 
 	DSSDBG("clkin (%s) rate %ld, highfreq %d\n",
-			cinfo->use_dss2_fck ? "dss2_fck" : "pclkfree",
+			cinfo->use_sys_clk ? "dss_sys_clk" : "pclkfree",
 			cinfo->clkin,
 			cinfo->highfreq);
 
@@ -1015,10 +1029,14 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
 	DSSDBG("Clock lane freq %ld Hz\n", cinfo->clkin4ddr / 4);
 
-	DSSDBG("regm3 = %d, dsi1_pll_fclk = %lu\n",
-			cinfo->regm3, cinfo->dsi1_pll_fclk);
-	DSSDBG("regm4 = %d, dsi2_pll_fclk = %lu\n",
-			cinfo->regm4, cinfo->dsi2_pll_fclk);
+	DSSDBG("regm_dispc = %d, %s (%s) = %lu\n", cinfo->regm_dispc,
+		dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+		dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+		cinfo->dsi_pll_hsdiv_dispc_clk);
+	DSSDBG("regm_dsi = %d, %s (%s) = %lu\n", cinfo->regm_dsi,
+		dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+		dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+		cinfo->dsi_pll_hsdiv_dsi_clk);
 
 	REG_FLD_MOD(DSI_PLL_CONTROL, 0, 0, 0); /* DSI_PLL_AUTOMODE = manual */
 
@@ -1026,9 +1044,9 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 	l = FLD_MOD(l, 1, 0, 0);		/* DSI_PLL_STOPMODE */
 	l = FLD_MOD(l, cinfo->regn - 1, 7, 1);	/* DSI_PLL_REGN */
 	l = FLD_MOD(l, cinfo->regm, 18, 8);	/* DSI_PLL_REGM */
-	l = FLD_MOD(l, cinfo->regm3 > 0 ? cinfo->regm3 - 1 : 0,
+	l = FLD_MOD(l, cinfo->regm_dispc > 0 ? cinfo->regm_dispc - 1 : 0,
 			22, 19);		/* DSI_CLOCK_DIV */
-	l = FLD_MOD(l, cinfo->regm4 > 0 ? cinfo->regm4 - 1 : 0,
+	l = FLD_MOD(l, cinfo->regm_dsi > 0 ? cinfo->regm_dsi - 1 : 0,
 			26, 23);		/* DSIPROTO_CLOCK_DIV */
 	dsi_write_reg(DSI_PLL_CONFIGURATION1, l);
 
@@ -1046,7 +1064,7 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
 	l = dsi_read_reg(DSI_PLL_CONFIGURATION2);
 	l = FLD_MOD(l, f, 4, 1);		/* DSI_PLL_FREQSEL */
-	l = FLD_MOD(l, cinfo->use_dss2_fck ? 0 : 1,
+	l = FLD_MOD(l, cinfo->use_sys_clk ? 0 : 1,
 			11, 11);		/* DSI_PLL_CLKSEL */
 	l = FLD_MOD(l, cinfo->highfreq,
 			12, 12);		/* DSI_PLL_HIGHFREQ */
@@ -1101,6 +1119,26 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
 
 	DSSDBG("PLL init\n");
 
+#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
+	/*
+	 * HACK: this is just a quick hack to get the USE_DSI_PLL
+	 * option working. USE_DSI_PLL is itself a big hack, and
+	 * should be removed.
+	 */
+	if (dsi.vdds_dsi_reg == NULL) {
+		struct regulator *vdds_dsi;
+
+		vdds_dsi = regulator_get(&dsi.pdev->dev, "vdds_dsi");
+
+		if (IS_ERR(vdds_dsi)) {
+			DSSERR("can't get VDDS_DSI regulator\n");
+			return PTR_ERR(vdds_dsi);
+		}
+
+		dsi.vdds_dsi_reg = vdds_dsi;
+	}
+#endif
+
 	enable_clocks(1);
 	dsi_enable_pll_clock(1);
 
@@ -1162,6 +1200,10 @@ void dsi_dump_clocks(struct seq_file *s)
 {
 	int clksel;
 	struct dsi_clock_info *cinfo = &dsi.current_cinfo;
+	enum dss_clk_source dispc_clk_src, dsi_clk_src;
+
+	dispc_clk_src = dss_get_dispc_clk_source();
+	dsi_clk_src = dss_get_dsi_clk_source();
 
 	enable_clocks(1);
 
@@ -1171,30 +1213,34 @@ void dsi_dump_clocks(struct seq_file *s)
 
 	seq_printf(s,	"dsi pll source = %s\n",
 			clksel == 0 ?
-			"dss2_alwon_fclk" : "pclkfree");
+			"dss_sys_clk" : "pclkfree");
 
 	seq_printf(s,	"Fint\t\t%-16luregn %u\n", cinfo->fint, cinfo->regn);
 
 	seq_printf(s,	"CLKIN4DDR\t%-16luregm %u\n",
 			cinfo->clkin4ddr, cinfo->regm);
 
-	seq_printf(s,	"dsi1_pll_fck\t%-16luregm3 %u\t(%s)\n",
-			cinfo->dsi1_pll_fclk,
-			cinfo->regm3,
-			dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+	seq_printf(s,	"%s (%s)\t%-16luregm_dispc %u\t(%s)\n",
+			dss_get_generic_clk_source_name(dispc_clk_src),
+			dss_feat_get_clk_source_name(dispc_clk_src),
+			cinfo->dsi_pll_hsdiv_dispc_clk,
+			cinfo->regm_dispc,
+			dispc_clk_src == DSS_CLK_SRC_FCK ?
 			"off" : "on");
 
-	seq_printf(s,	"dsi2_pll_fck\t%-16luregm4 %u\t(%s)\n",
-			cinfo->dsi2_pll_fclk,
-			cinfo->regm4,
-			dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+	seq_printf(s,	"%s (%s)\t%-16luregm_dsi %u\t(%s)\n",
+			dss_get_generic_clk_source_name(dsi_clk_src),
+			dss_feat_get_clk_source_name(dsi_clk_src),
+			cinfo->dsi_pll_hsdiv_dsi_clk,
+			cinfo->regm_dsi,
+			dsi_clk_src == DSS_CLK_SRC_FCK ?
 			"off" : "on");
 
 	seq_printf(s,	"- DSI -\n");
 
-	seq_printf(s,	"dsi fclk source = %s\n",
-			dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-			"dss1_alwon_fclk" : "dsi2_pll_fclk");
+	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());
 
@@ -1306,7 +1352,7 @@ void dsi_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	DUMPREG(DSI_REVISION);
 	DUMPREG(DSI_SYSCONFIG);
@@ -1378,7 +1424,7 @@ void dsi_dump_regs(struct seq_file *s)
 	DUMPREG(DSI_PLL_CONFIGURATION1);
 	DUMPREG(DSI_PLL_CONFIGURATION2);
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
@@ -1622,20 +1668,6 @@ static int _dsi_reset(void)
 	return _dsi_wait_reset();
 }
 
-static void dsi_reset_tx_fifo(int channel)
-{
-	u32 mask;
-	u32 l;
-
-	/* set fifosize of the channel to 0, then return the old size */
-	l = dsi_read_reg(DSI_TX_FIFO_VC_SIZE);
-
-	mask = FLD_MASK((8 * channel) + 7, (8 * channel) + 4);
-	dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l & ~mask);
-
-	dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l);
-}
-
 static void dsi_config_tx_fifo(enum fifo_size size1, enum fifo_size size2,
 		enum fifo_size size3, enum fifo_size size4)
 {
@@ -1753,8 +1785,6 @@ static void dsi_vc_initial_config(int channel)
 	r = FLD_MOD(r, 4, 23, 21); /* DMA_TX_REQ_NB = no dma */
 
 	dsi_write_reg(DSI_VC_CTRL(channel), r);
-
-	dsi.vc[channel].mode = DSI_VC_MODE_L4;
 }
 
 static int dsi_vc_config_l4(int channel)
@@ -1961,7 +1991,7 @@ static inline void dsi_vc_write_long_header(int channel, u8 data_type,
 
 	WARN_ON(!dsi_bus_is_locked());
 
-	data_id = data_type | channel << 6;
+	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);
@@ -2064,7 +2094,7 @@ static int dsi_vc_send_short(int channel, u8 data_type, u16 data, u8 ecc)
 		return -EINVAL;
 	}
 
-	data_id = data_type | channel << 6;
+	data_id = data_type | dsi.vc[channel].vc_id << 6;
 
 	r = (data_id << 0) | (data << 8) | (ecc << 24);
 
@@ -2984,12 +3014,12 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 	struct dsi_clock_info cinfo;
 	int r;
 
-	/* we always use DSS2_FCK as input clock */
-	cinfo.use_dss2_fck = true;
+	/* we always use DSS_CLK_SYSCK as input clock */
+	cinfo.use_sys_clk = true;
 	cinfo.regn  = dssdev->phy.dsi.div.regn;
 	cinfo.regm  = dssdev->phy.dsi.div.regm;
-	cinfo.regm3 = dssdev->phy.dsi.div.regm3;
-	cinfo.regm4 = dssdev->phy.dsi.div.regm4;
+	cinfo.regm_dispc = dssdev->phy.dsi.div.regm_dispc;
+	cinfo.regm_dsi = dssdev->phy.dsi.div.regm_dsi;
 	r = dsi_calc_clock_rates(dssdev, &cinfo);
 	if (r) {
 		DSSERR("Failed to calc dsi clocks\n");
@@ -3011,7 +3041,7 @@ static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
 	int r;
 	unsigned long long fck;
 
-	fck = dsi_get_dsi1_pll_rate();
+	fck = dsi_get_pll_hsdiv_dispc_rate();
 
 	dispc_cinfo.lck_div = dssdev->phy.dsi.div.lck_div;
 	dispc_cinfo.pck_div = dssdev->phy.dsi.div.pck_div;
@@ -3045,8 +3075,8 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 	if (r)
 		goto err1;
 
-	dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
-	dss_select_dsi_clk_source(DSS_SRC_DSI2_PLL_FCLK);
+	dss_select_dispc_clk_source(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC);
+	dss_select_dsi_clk_source(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI);
 
 	DSSDBG("PLL OK\n");
 
@@ -3082,8 +3112,8 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 err3:
 	dsi_complexio_uninit();
 err2:
-	dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-	dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+	dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
+	dss_select_dsi_clk_source(DSS_CLK_SRC_FCK);
 err1:
 	dsi_pll_uninit();
 err0:
@@ -3099,8 +3129,8 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev)
 	dsi_vc_enable(2, 0);
 	dsi_vc_enable(3, 0);
 
-	dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-	dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+	dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
+	dss_select_dsi_clk_source(DSS_CLK_SRC_FCK);
 	dsi_complexio_uninit();
 	dsi_pll_uninit();
 }
@@ -3220,28 +3250,94 @@ int dsi_init_display(struct omap_dss_device *dssdev)
 	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
 		OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
 
-	dsi.vc[0].dssdev = dssdev;
-	dsi.vc[1].dssdev = dssdev;
+	if (dsi.vdds_dsi_reg == NULL) {
+		struct regulator *vdds_dsi;
+
+		vdds_dsi = regulator_get(&dsi.pdev->dev, "vdds_dsi");
+
+		if (IS_ERR(vdds_dsi)) {
+			DSSERR("can't get VDDS_DSI regulator\n");
+			return PTR_ERR(vdds_dsi);
+		}
+
+		dsi.vdds_dsi_reg = vdds_dsi;
+	}
+
+	return 0;
+}
+
+int omap_dsi_request_vc(struct omap_dss_device *dssdev, int *channel)
+{
+	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;
+}
+EXPORT_SYMBOL(omap_dsi_request_vc);
+
+int omap_dsi_set_vc_id(struct omap_dss_device *dssdev, int channel, int vc_id)
+{
+	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;
 }
+EXPORT_SYMBOL(omap_dsi_set_vc_id);
 
-void dsi_wait_dsi1_pll_active(void)
+void omap_dsi_release_vc(struct omap_dss_device *dssdev, int channel)
+{
+	if ((channel >= 0 && channel <= 3) &&
+		dsi.vc[channel].dssdev == dssdev) {
+		dsi.vc[channel].dssdev = NULL;
+		dsi.vc[channel].vc_id = 0;
+	}
+}
+EXPORT_SYMBOL(omap_dsi_release_vc);
+
+void dsi_wait_pll_hsdiv_dispc_active(void)
 {
 	if (wait_for_bit_change(DSI_PLL_STATUS, 7, 1) != 1)
-		DSSERR("DSI1 PLL clock not active\n");
+		DSSERR("%s (%s) not active\n",
+			dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+			dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC));
 }
 
-void dsi_wait_dsi2_pll_active(void)
+void dsi_wait_pll_hsdiv_dsi_active(void)
 {
 	if (wait_for_bit_change(DSI_PLL_STATUS, 8, 1) != 1)
-		DSSERR("DSI2 PLL clock not active\n");
+		DSSERR("%s (%s) not active\n",
+			dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+			dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI));
 }
 
-int dsi_init(struct platform_device *pdev)
+static int dsi_init(struct platform_device *pdev)
 {
 	u32 rev;
-	int r;
+	int r, i;
+	struct resource *dsi_mem;
 
 	spin_lock_init(&dsi.errors_lock);
 	dsi.errors = 0;
@@ -3268,24 +3364,43 @@ int dsi_init(struct platform_device *pdev)
 	dsi.te_timer.function = dsi_te_timeout;
 	dsi.te_timer.data = 0;
 #endif
-	dsi.base = ioremap(DSI_BASE, DSI_SZ_REGS);
+	dsi_mem = platform_get_resource(dsi.pdev, IORESOURCE_MEM, 0);
+	if (!dsi_mem) {
+		DSSERR("can't get IORESOURCE_MEM DSI\n");
+		r = -EINVAL;
+		goto err1;
+	}
+	dsi.base = ioremap(dsi_mem->start, resource_size(dsi_mem));
 	if (!dsi.base) {
 		DSSERR("can't ioremap DSI\n");
 		r = -ENOMEM;
 		goto err1;
 	}
+	dsi.irq	= platform_get_irq(dsi.pdev, 0);
+	if (dsi.irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		r = -ENODEV;
+		goto err2;
+	}
 
-	dsi.vdds_dsi_reg = dss_get_vdds_dsi();
-	if (IS_ERR(dsi.vdds_dsi_reg)) {
-		DSSERR("can't get VDDS_DSI regulator\n");
-		r = PTR_ERR(dsi.vdds_dsi_reg);
+	r = request_irq(dsi.irq, omap_dsi_irq_handler, IRQF_SHARED,
+		"OMAP DSI1", dsi.pdev);
+	if (r < 0) {
+		DSSERR("request_irq failed\n");
 		goto err2;
 	}
 
+	/* DSI VCs initialization */
+	for (i = 0; i < ARRAY_SIZE(dsi.vc); i++) {
+		dsi.vc[i].mode = DSI_VC_MODE_L4;
+		dsi.vc[i].dssdev = NULL;
+		dsi.vc[i].vc_id = 0;
+	}
+
 	enable_clocks(1);
 
 	rev = dsi_read_reg(DSI_REVISION);
-	printk(KERN_INFO "OMAP DSI rev %d.%d\n",
+	dev_dbg(&pdev->dev, "OMAP DSI rev %d.%d\n",
 	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
 	enable_clocks(0);
@@ -3298,8 +3413,14 @@ err1:
 	return r;
 }
 
-void dsi_exit(void)
+static void dsi_exit(void)
 {
+	if (dsi.vdds_dsi_reg != NULL) {
+		regulator_put(dsi.vdds_dsi_reg);
+		dsi.vdds_dsi_reg = NULL;
+	}
+
+	free_irq(dsi.irq, dsi.pdev);
 	iounmap(dsi.base);
 
 	destroy_workqueue(dsi.workqueue);
@@ -3307,3 +3428,41 @@ void dsi_exit(void)
 	DSSDBG("omap_dsi_exit\n");
 }
 
+/* DSI1 HW IP initialisation */
+static int omap_dsi1hw_probe(struct platform_device *pdev)
+{
+	int r;
+	dsi.pdev = pdev;
+	r = dsi_init(pdev);
+	if (r) {
+		DSSERR("Failed to initialize DSI\n");
+		goto err_dsi;
+	}
+err_dsi:
+	return r;
+}
+
+static int omap_dsi1hw_remove(struct platform_device *pdev)
+{
+	dsi_exit();
+	return 0;
+}
+
+static struct platform_driver omap_dsi1hw_driver = {
+	.probe          = omap_dsi1hw_probe,
+	.remove         = omap_dsi1hw_remove,
+	.driver         = {
+		.name   = "omapdss_dsi1",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int dsi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dsi1hw_driver);
+}
+
+void dsi_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_dsi1hw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 77c3621c9171..2be4d03ece43 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -26,14 +26,13 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/delay.h>
-#include <linux/interrupt.h>
 #include <linux/seq_file.h>
 #include <linux/clk.h>
 
 #include <plat/display.h>
+#include <plat/clock.h>
 #include "dss.h"
-
-#define DSS_BASE			0x48050000
+#include "dss_features.h"
 
 #define DSS_SZ_REGS			SZ_512
 
@@ -59,9 +58,17 @@ struct dss_reg {
 	dss_write_reg(idx, FLD_MOD(dss_read_reg(idx), val, start, end))
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem    *base;
+	int             ctx_id;
 
 	struct clk	*dpll4_m4_ck;
+	struct clk	*dss_ick;
+	struct clk	*dss_fck;
+	struct clk	*dss_sys_clk;
+	struct clk	*dss_tv_fck;
+	struct clk	*dss_video_fck;
+	unsigned	num_clks_enabled;
 
 	unsigned long	cache_req_pck;
 	unsigned long	cache_prate;
@@ -74,6 +81,17 @@ static struct {
 	u32		ctx[DSS_SZ_REGS / sizeof(u32)];
 } dss;
 
+static const struct dss_clk_source_name dss_generic_clk_source_names[] = {
+	{ DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC, "DSI_PLL_HSDIV_DISPC" },
+	{ DSS_CLK_SRC_DSI_PLL_HSDIV_DSI, "DSI_PLL_HSDIV_DSI" },
+	{ DSS_CLK_SRC_FCK, "DSS_FCK" },
+};
+
+static void dss_clk_enable_all_no_ctx(void);
+static void dss_clk_disable_all_no_ctx(void);
+static void dss_clk_enable_no_ctx(enum dss_clock clks);
+static void dss_clk_disable_no_ctx(enum dss_clock clks);
+
 static int _omap_dss_wait_reset(void);
 
 static inline void dss_write_reg(const struct dss_reg idx, u32 val)
@@ -99,10 +117,11 @@ void dss_save_context(void)
 	SR(SYSCONFIG);
 	SR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-	SR(SDI_CONTROL);
-	SR(PLL_CONTROL);
-#endif
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		SR(SDI_CONTROL);
+		SR(PLL_CONTROL);
+	}
 }
 
 void dss_restore_context(void)
@@ -113,10 +132,11 @@ void dss_restore_context(void)
 	RR(SYSCONFIG);
 	RR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-	RR(SDI_CONTROL);
-	RR(PLL_CONTROL);
-#endif
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		RR(SDI_CONTROL);
+		RR(PLL_CONTROL);
+	}
 }
 
 #undef SR
@@ -209,12 +229,17 @@ void dss_sdi_disable(void)
 	REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
 }
 
+const char *dss_get_generic_clk_source_name(enum dss_clk_source clk_src)
+{
+	return dss_generic_clk_source_names[clk_src].clksrc_name;
+}
+
 void dss_dump_clocks(struct seq_file *s)
 {
 	unsigned long dpll4_ck_rate;
 	unsigned long dpll4_m4_ck_rate;
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
 	dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
@@ -224,35 +249,43 @@ void dss_dump_clocks(struct seq_file *s)
 	seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
 
 	if (cpu_is_omap3630())
-		seq_printf(s, "dss1_alwon_fclk = %lu / %lu  = %lu\n",
+		seq_printf(s, "%s (%s) = %lu / %lu  = %lu\n",
+			dss_get_generic_clk_source_name(DSS_CLK_SRC_FCK),
+			dss_feat_get_clk_source_name(DSS_CLK_SRC_FCK),
 			dpll4_ck_rate,
 			dpll4_ck_rate / dpll4_m4_ck_rate,
-			dss_clk_get_rate(DSS_CLK_FCK1));
+			dss_clk_get_rate(DSS_CLK_FCK));
 	else
-		seq_printf(s, "dss1_alwon_fclk = %lu / %lu * 2 = %lu\n",
+		seq_printf(s, "%s (%s) = %lu / %lu * 2 = %lu\n",
+			dss_get_generic_clk_source_name(DSS_CLK_SRC_FCK),
+			dss_feat_get_clk_source_name(DSS_CLK_SRC_FCK),
 			dpll4_ck_rate,
 			dpll4_ck_rate / dpll4_m4_ck_rate,
-			dss_clk_get_rate(DSS_CLK_FCK1));
+			dss_clk_get_rate(DSS_CLK_FCK));
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 void dss_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dss_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	DUMPREG(DSS_REVISION);
 	DUMPREG(DSS_SYSCONFIG);
 	DUMPREG(DSS_SYSSTATUS);
 	DUMPREG(DSS_IRQSTATUS);
 	DUMPREG(DSS_CONTROL);
-	DUMPREG(DSS_SDI_CONTROL);
-	DUMPREG(DSS_PLL_CONTROL);
-	DUMPREG(DSS_SDI_STATUS);
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	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_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
@@ -260,13 +293,13 @@ void dss_select_dispc_clk_source(enum dss_clk_source clk_src)
 {
 	int b;
 
-	BUG_ON(clk_src != DSS_SRC_DSI1_PLL_FCLK &&
-			clk_src != DSS_SRC_DSS1_ALWON_FCLK);
+	BUG_ON(clk_src != DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC &&
+			clk_src != DSS_CLK_SRC_FCK);
 
-	b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
+	b = clk_src == DSS_CLK_SRC_FCK ? 0 : 1;
 
-	if (clk_src == DSS_SRC_DSI1_PLL_FCLK)
-		dsi_wait_dsi1_pll_active();
+	if (clk_src == DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC)
+		dsi_wait_pll_hsdiv_dispc_active();
 
 	REG_FLD_MOD(DSS_CONTROL, b, 0, 0);	/* DISPC_CLK_SWITCH */
 
@@ -277,13 +310,13 @@ void dss_select_dsi_clk_source(enum dss_clk_source clk_src)
 {
 	int b;
 
-	BUG_ON(clk_src != DSS_SRC_DSI2_PLL_FCLK &&
-			clk_src != DSS_SRC_DSS1_ALWON_FCLK);
+	BUG_ON(clk_src != DSS_CLK_SRC_DSI_PLL_HSDIV_DSI &&
+			clk_src != DSS_CLK_SRC_FCK);
 
-	b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
+	b = clk_src == DSS_CLK_SRC_FCK ? 0 : 1;
 
-	if (clk_src == DSS_SRC_DSI2_PLL_FCLK)
-		dsi_wait_dsi2_pll_active();
+	if (clk_src == DSS_CLK_SRC_DSI_PLL_HSDIV_DSI)
+		dsi_wait_pll_hsdiv_dsi_active();
 
 	REG_FLD_MOD(DSS_CONTROL, b, 1, 1);	/* DSI_CLK_SWITCH */
 
@@ -337,7 +370,7 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 
 int dss_get_clock_div(struct dss_clock_info *cinfo)
 {
-	cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK1);
+	cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK);
 
 	if (cpu_is_omap34xx()) {
 		unsigned long prate;
@@ -369,7 +402,7 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 	struct dss_clock_info best_dss;
 	struct dispc_clock_info best_dispc;
 
-	unsigned long fck;
+	unsigned long fck, max_dss_fck;
 
 	u16 fck_div;
 
@@ -378,7 +411,9 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 
 	prate = dss_get_dpll4_rate();
 
-	fck = dss_clk_get_rate(DSS_CLK_FCK1);
+	max_dss_fck = dss_feat_get_max_dss_fck();
+
+	fck = dss_clk_get_rate(DSS_CLK_FCK);
 	if (req_pck == dss.cache_req_pck &&
 			((cpu_is_omap34xx() && prate == dss.cache_prate) ||
 			 dss.cache_dss_cinfo.fck == fck)) {
@@ -391,7 +426,7 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
 	if (min_fck_per_pck &&
-		req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+		req_pck * min_fck_per_pck > max_dss_fck) {
 		DSSERR("Requested pixel clock not possible with the current "
 				"OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
 				"the constraint off.\n");
@@ -405,7 +440,7 @@ retry:
 	if (cpu_is_omap24xx()) {
 		struct dispc_clock_info cur_dispc;
 		/* XXX can we change the clock on omap2? */
-		fck = dss_clk_get_rate(DSS_CLK_FCK1);
+		fck = dss_clk_get_rate(DSS_CLK_FCK);
 		fck_div = 1;
 
 		dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
@@ -427,7 +462,7 @@ retry:
 			else
 				fck = prate / fck_div * 2;
 
-			if (fck > DISPC_MAX_FCK)
+			if (fck > max_dss_fck)
 				continue;
 
 			if (min_fck_per_pck &&
@@ -482,31 +517,6 @@ found:
 	return 0;
 }
 
-
-
-static irqreturn_t dss_irq_handler_omap2(int irq, void *arg)
-{
-	dispc_irq_handler();
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t dss_irq_handler_omap3(int irq, void *arg)
-{
-	u32 irqstatus;
-
-	irqstatus = dss_read_reg(DSS_IRQSTATUS);
-
-	if (irqstatus & (1<<0))	/* DISPC_IRQ */
-		dispc_irq_handler();
-#ifdef CONFIG_OMAP2_DSS_DSI
-	if (irqstatus & (1<<1))	/* DSI_IRQ */
-		dsi_irq_handler();
-#endif
-
-	return IRQ_HANDLED;
-}
-
 static int _omap_dss_wait_reset(void)
 {
 	int t = 0;
@@ -549,34 +559,39 @@ void dss_set_dac_pwrdn_bgz(bool enable)
 	REG_FLD_MOD(DSS_CONTROL, enable, 5, 5);	/* DAC Power-Down Control */
 }
 
-int dss_init(bool skip_init)
+static int dss_init(void)
 {
 	int r;
 	u32 rev;
+	struct resource *dss_mem;
 
-	dss.base = ioremap(DSS_BASE, DSS_SZ_REGS);
+	dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
+	if (!dss_mem) {
+		DSSERR("can't get IORESOURCE_MEM DSS\n");
+		r = -EINVAL;
+		goto fail0;
+	}
+	dss.base = ioremap(dss_mem->start, resource_size(dss_mem));
 	if (!dss.base) {
 		DSSERR("can't ioremap DSS\n");
 		r = -ENOMEM;
 		goto fail0;
 	}
 
-	if (!skip_init) {
-		/* disable LCD and DIGIT output. This seems to fix the synclost
-		 * problem that we get, if the bootloader starts the DSS and
-		 * the kernel resets it */
-		omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
-
-		/* We need to wait here a bit, otherwise we sometimes start to
-		 * get synclost errors, and after that only power cycle will
-		 * restore DSS functionality. I have no idea why this happens.
-		 * And we have to wait _before_ resetting the DSS, but after
-		 * enabling clocks.
-		 */
-		msleep(50);
-
-		_omap_dss_reset();
-	}
+	/* disable LCD and DIGIT output. This seems to fix the synclost
+	 * problem that we get, if the bootloader starts the DSS and
+	 * the kernel resets it */
+	omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
+
+	/* We need to wait here a bit, otherwise we sometimes start to
+	 * get synclost errors, and after that only power cycle will
+	 * restore DSS functionality. I have no idea why this happens.
+	 * And we have to wait _before_ resetting the DSS, but after
+	 * enabling clocks.
+	 */
+	msleep(50);
+
+	_omap_dss_reset();
 
 	/* autoidle */
 	REG_FLD_MOD(DSS_SYSCONFIG, 1, 0, 0);
@@ -590,28 +605,17 @@ int dss_init(bool skip_init)
 	REG_FLD_MOD(DSS_CONTROL, 0, 2, 2);	/* venc clock mode = normal */
 #endif
 
-	r = request_irq(INT_24XX_DSS_IRQ,
-			cpu_is_omap24xx()
-			? dss_irq_handler_omap2
-			: dss_irq_handler_omap3,
-			0, "OMAP DSS", NULL);
-
-	if (r < 0) {
-		DSSERR("omap2 dss: request_irq failed\n");
-		goto fail1;
-	}
-
 	if (cpu_is_omap34xx()) {
 		dss.dpll4_m4_ck = clk_get(NULL, "dpll4_m4_ck");
 		if (IS_ERR(dss.dpll4_m4_ck)) {
 			DSSERR("Failed to get dpll4_m4_ck\n");
 			r = PTR_ERR(dss.dpll4_m4_ck);
-			goto fail2;
+			goto fail1;
 		}
 	}
 
-	dss.dsi_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
-	dss.dispc_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
+	dss.dsi_clk_source = DSS_CLK_SRC_FCK;
+	dss.dispc_clk_source = DSS_CLK_SRC_FCK;
 
 	dss_save_context();
 
@@ -621,21 +625,416 @@ int dss_init(bool skip_init)
 
 	return 0;
 
-fail2:
-	free_irq(INT_24XX_DSS_IRQ, NULL);
 fail1:
 	iounmap(dss.base);
 fail0:
 	return r;
 }
 
-void dss_exit(void)
+static void dss_exit(void)
 {
 	if (cpu_is_omap34xx())
 		clk_put(dss.dpll4_m4_ck);
 
-	free_irq(INT_24XX_DSS_IRQ, NULL);
-
 	iounmap(dss.base);
 }
 
+/* CONTEXT */
+static int dss_get_ctx_id(void)
+{
+	struct omap_display_platform_data *pdata = dss.pdev->dev.platform_data;
+	int r;
+
+	if (!pdata->board_data->get_last_off_on_transaction_id)
+		return 0;
+	r = pdata->board_data->get_last_off_on_transaction_id(&dss.pdev->dev);
+	if (r < 0) {
+		dev_err(&dss.pdev->dev, "getting transaction ID failed, "
+				"will force context restore\n");
+		r = -1;
+	}
+	return r;
+}
+
+int dss_need_ctx_restore(void)
+{
+	int id = dss_get_ctx_id();
+
+	if (id < 0 || id != dss.ctx_id) {
+		DSSDBG("ctx id %d -> id %d\n",
+				dss.ctx_id, id);
+		dss.ctx_id = id;
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static void save_all_ctx(void)
+{
+	DSSDBG("save context\n");
+
+	dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK);
+
+	dss_save_context();
+	dispc_save_context();
+#ifdef CONFIG_OMAP2_DSS_DSI
+	dsi_save_context();
+#endif
+
+	dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK);
+}
+
+static void restore_all_ctx(void)
+{
+	DSSDBG("restore context\n");
+
+	dss_clk_enable_all_no_ctx();
+
+	dss_restore_context();
+	dispc_restore_context();
+#ifdef CONFIG_OMAP2_DSS_DSI
+	dsi_restore_context();
+#endif
+
+	dss_clk_disable_all_no_ctx();
+}
+
+static int dss_get_clock(struct clk **clock, const char *clk_name)
+{
+	struct clk *clk;
+
+	clk = clk_get(&dss.pdev->dev, clk_name);
+
+	if (IS_ERR(clk)) {
+		DSSERR("can't get clock %s", clk_name);
+		return PTR_ERR(clk);
+	}
+
+	*clock = clk;
+
+	DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
+
+	return 0;
+}
+
+static int dss_get_clocks(void)
+{
+	int r;
+	struct omap_display_platform_data *pdata = dss.pdev->dev.platform_data;
+
+	dss.dss_ick = NULL;
+	dss.dss_fck = NULL;
+	dss.dss_sys_clk = NULL;
+	dss.dss_tv_fck = NULL;
+	dss.dss_video_fck = NULL;
+
+	r = dss_get_clock(&dss.dss_ick, "ick");
+	if (r)
+		goto err;
+
+	r = dss_get_clock(&dss.dss_fck, "fck");
+	if (r)
+		goto err;
+
+	if (!pdata->opt_clock_available) {
+		r = -ENODEV;
+		goto err;
+	}
+
+	if (pdata->opt_clock_available("sys_clk")) {
+		r = dss_get_clock(&dss.dss_sys_clk, "sys_clk");
+		if (r)
+			goto err;
+	}
+
+	if (pdata->opt_clock_available("tv_clk")) {
+		r = dss_get_clock(&dss.dss_tv_fck, "tv_clk");
+		if (r)
+			goto err;
+	}
+
+	if (pdata->opt_clock_available("video_clk")) {
+		r = dss_get_clock(&dss.dss_video_fck, "video_clk");
+		if (r)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	if (dss.dss_ick)
+		clk_put(dss.dss_ick);
+	if (dss.dss_fck)
+		clk_put(dss.dss_fck);
+	if (dss.dss_sys_clk)
+		clk_put(dss.dss_sys_clk);
+	if (dss.dss_tv_fck)
+		clk_put(dss.dss_tv_fck);
+	if (dss.dss_video_fck)
+		clk_put(dss.dss_video_fck);
+
+	return r;
+}
+
+static void dss_put_clocks(void)
+{
+	if (dss.dss_video_fck)
+		clk_put(dss.dss_video_fck);
+	if (dss.dss_tv_fck)
+		clk_put(dss.dss_tv_fck);
+	if (dss.dss_sys_clk)
+		clk_put(dss.dss_sys_clk);
+	clk_put(dss.dss_fck);
+	clk_put(dss.dss_ick);
+}
+
+unsigned long dss_clk_get_rate(enum dss_clock clk)
+{
+	switch (clk) {
+	case DSS_CLK_ICK:
+		return clk_get_rate(dss.dss_ick);
+	case DSS_CLK_FCK:
+		return clk_get_rate(dss.dss_fck);
+	case DSS_CLK_SYSCK:
+		return clk_get_rate(dss.dss_sys_clk);
+	case DSS_CLK_TVFCK:
+		return clk_get_rate(dss.dss_tv_fck);
+	case DSS_CLK_VIDFCK:
+		return clk_get_rate(dss.dss_video_fck);
+	}
+
+	BUG();
+	return 0;
+}
+
+static unsigned count_clk_bits(enum dss_clock clks)
+{
+	unsigned num_clks = 0;
+
+	if (clks & DSS_CLK_ICK)
+		++num_clks;
+	if (clks & DSS_CLK_FCK)
+		++num_clks;
+	if (clks & DSS_CLK_SYSCK)
+		++num_clks;
+	if (clks & DSS_CLK_TVFCK)
+		++num_clks;
+	if (clks & DSS_CLK_VIDFCK)
+		++num_clks;
+
+	return num_clks;
+}
+
+static void dss_clk_enable_no_ctx(enum dss_clock clks)
+{
+	unsigned num_clks = count_clk_bits(clks);
+
+	if (clks & DSS_CLK_ICK)
+		clk_enable(dss.dss_ick);
+	if (clks & DSS_CLK_FCK)
+		clk_enable(dss.dss_fck);
+	if ((clks & DSS_CLK_SYSCK) && dss.dss_sys_clk)
+		clk_enable(dss.dss_sys_clk);
+	if ((clks & DSS_CLK_TVFCK) && dss.dss_tv_fck)
+		clk_enable(dss.dss_tv_fck);
+	if ((clks & DSS_CLK_VIDFCK) && dss.dss_video_fck)
+		clk_enable(dss.dss_video_fck);
+
+	dss.num_clks_enabled += num_clks;
+}
+
+void dss_clk_enable(enum dss_clock clks)
+{
+	bool check_ctx = dss.num_clks_enabled == 0;
+
+	dss_clk_enable_no_ctx(clks);
+
+	/*
+	 * HACK: On omap4 the registers may not be accessible right after
+	 * enabling the clocks. At some point this will be handled by
+	 * pm_runtime, but for the time begin this should make things work.
+	 */
+	if (cpu_is_omap44xx() && check_ctx)
+		udelay(10);
+
+	if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
+		restore_all_ctx();
+}
+
+static void dss_clk_disable_no_ctx(enum dss_clock clks)
+{
+	unsigned num_clks = count_clk_bits(clks);
+
+	if (clks & DSS_CLK_ICK)
+		clk_disable(dss.dss_ick);
+	if (clks & DSS_CLK_FCK)
+		clk_disable(dss.dss_fck);
+	if ((clks & DSS_CLK_SYSCK) && dss.dss_sys_clk)
+		clk_disable(dss.dss_sys_clk);
+	if ((clks & DSS_CLK_TVFCK) && dss.dss_tv_fck)
+		clk_disable(dss.dss_tv_fck);
+	if ((clks & DSS_CLK_VIDFCK) && dss.dss_video_fck)
+		clk_disable(dss.dss_video_fck);
+
+	dss.num_clks_enabled -= num_clks;
+}
+
+void dss_clk_disable(enum dss_clock clks)
+{
+	if (cpu_is_omap34xx()) {
+		unsigned num_clks = count_clk_bits(clks);
+
+		BUG_ON(dss.num_clks_enabled < num_clks);
+
+		if (dss.num_clks_enabled == num_clks)
+			save_all_ctx();
+	}
+
+	dss_clk_disable_no_ctx(clks);
+}
+
+static void dss_clk_enable_all_no_ctx(void)
+{
+	enum dss_clock clks;
+
+	clks = DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_SYSCK | DSS_CLK_TVFCK;
+	if (cpu_is_omap34xx())
+		clks |= DSS_CLK_VIDFCK;
+	dss_clk_enable_no_ctx(clks);
+}
+
+static void dss_clk_disable_all_no_ctx(void)
+{
+	enum dss_clock clks;
+
+	clks = DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_SYSCK | DSS_CLK_TVFCK;
+	if (cpu_is_omap34xx())
+		clks |= DSS_CLK_VIDFCK;
+	dss_clk_disable_no_ctx(clks);
+}
+
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+/* CLOCKS */
+static void core_dump_clocks(struct seq_file *s)
+{
+	int i;
+	struct clk *clocks[5] = {
+		dss.dss_ick,
+		dss.dss_fck,
+		dss.dss_sys_clk,
+		dss.dss_tv_fck,
+		dss.dss_video_fck
+	};
+
+	seq_printf(s, "- CORE -\n");
+
+	seq_printf(s, "internal clk count\t\t%u\n", dss.num_clks_enabled);
+
+	for (i = 0; i < 5; i++) {
+		if (!clocks[i])
+			continue;
+		seq_printf(s, "%-15s\t%lu\t%d\n",
+				clocks[i]->name,
+				clk_get_rate(clocks[i]),
+				clocks[i]->usecount);
+	}
+}
+#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
+
+/* DEBUGFS */
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s)
+{
+	core_dump_clocks(s);
+	dss_dump_clocks(s);
+	dispc_dump_clocks(s);
+#ifdef CONFIG_OMAP2_DSS_DSI
+	dsi_dump_clocks(s);
+#endif
+}
+#endif
+
+
+/* DSS HW IP initialisation */
+static int omap_dsshw_probe(struct platform_device *pdev)
+{
+	int r;
+
+	dss.pdev = pdev;
+
+	r = dss_get_clocks();
+	if (r)
+		goto err_clocks;
+
+	dss_clk_enable_all_no_ctx();
+
+	dss.ctx_id = dss_get_ctx_id();
+	DSSDBG("initial ctx id %u\n", dss.ctx_id);
+
+	r = dss_init();
+	if (r) {
+		DSSERR("Failed to initialize DSS\n");
+		goto err_dss;
+	}
+
+	r = dpi_init();
+	if (r) {
+		DSSERR("Failed to initialize DPI\n");
+		goto err_dpi;
+	}
+
+	r = sdi_init();
+	if (r) {
+		DSSERR("Failed to initialize SDI\n");
+		goto err_sdi;
+	}
+
+	dss_clk_disable_all_no_ctx();
+	return 0;
+err_sdi:
+	dpi_exit();
+err_dpi:
+	dss_exit();
+err_dss:
+	dss_clk_disable_all_no_ctx();
+	dss_put_clocks();
+err_clocks:
+	return r;
+}
+
+static int omap_dsshw_remove(struct platform_device *pdev)
+{
+
+	dss_exit();
+
+	/*
+	 * As part of hwmod changes, DSS is not the only controller of dss
+	 * clocks; hwmod framework itself will also enable clocks during hwmod
+	 * init for dss, and autoidle is set in h/w for DSS. Hence, there's no
+	 * need to disable clocks if their usecounts > 1.
+	 */
+	WARN_ON(dss.num_clks_enabled > 0);
+
+	dss_put_clocks();
+	return 0;
+}
+
+static struct platform_driver omap_dsshw_driver = {
+	.probe          = omap_dsshw_probe,
+	.remove         = omap_dsshw_remove,
+	.driver         = {
+		.name   = "omapdss_dss",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int dss_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dsshw_driver);
+}
+
+void dss_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_dsshw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index b394951120ac..85d4141689c1 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -97,8 +97,6 @@ extern unsigned int dss_debug;
 #define FLD_MOD(orig, val, start, end) \
 	(((orig) & ~FLD_MASK(start, end)) | FLD_VAL(val, start, end))
 
-#define DISPC_MAX_FCK 173000000
-
 enum omap_burst_size {
 	OMAP_DSS_BURST_4x32 = 0,
 	OMAP_DSS_BURST_8x32 = 1,
@@ -112,17 +110,23 @@ enum omap_parallel_interface_mode {
 };
 
 enum dss_clock {
-	DSS_CLK_ICK	= 1 << 0,
-	DSS_CLK_FCK1	= 1 << 1,
-	DSS_CLK_FCK2	= 1 << 2,
-	DSS_CLK_54M	= 1 << 3,
-	DSS_CLK_96M	= 1 << 4,
+	DSS_CLK_ICK	= 1 << 0,	/* DSS_L3_ICLK and DSS_L4_ICLK */
+	DSS_CLK_FCK	= 1 << 1,	/* DSS1_ALWON_FCLK */
+	DSS_CLK_SYSCK	= 1 << 2,	/* DSS2_ALWON_FCLK */
+	DSS_CLK_TVFCK	= 1 << 3,	/* DSS_TV_FCLK */
+	DSS_CLK_VIDFCK	= 1 << 4,	/* DSS_96M_FCLK*/
 };
 
 enum dss_clk_source {
-	DSS_SRC_DSI1_PLL_FCLK,
-	DSS_SRC_DSI2_PLL_FCLK,
-	DSS_SRC_DSS1_ALWON_FCLK,
+	DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC,	/* DSI1_PLL_FCLK */
+	DSS_CLK_SRC_DSI_PLL_HSDIV_DSI,		/* DSI2_PLL_FCLK */
+	DSS_CLK_SRC_FCK,			/* DSS1_ALWON_FCLK */
+};
+
+/* Correlates clock source name and dss_clk_source member */
+struct dss_clk_source_name {
+	enum dss_clk_source clksrc;
+	const char *clksrc_name;
 };
 
 struct dss_clock_info {
@@ -148,36 +152,30 @@ struct dsi_clock_info {
 	unsigned long fint;
 	unsigned long clkin4ddr;
 	unsigned long clkin;
-	unsigned long dsi1_pll_fclk;
-	unsigned long dsi2_pll_fclk;
+	unsigned long dsi_pll_hsdiv_dispc_clk;	/* DSI1_PLL_CLK */
+	unsigned long dsi_pll_hsdiv_dsi_clk;	/* DSI2_PLL_CLK */
 
 	unsigned long lp_clk;
 
 	/* dividers */
 	u16 regn;
 	u16 regm;
-	u16 regm3;
-	u16 regm4;
+	u16 regm_dispc;	/* REGM3 */
+	u16 regm_dsi;	/* REGM4 */
 
 	u16 lp_clk_div;
 
 	u8 highfreq;
-	bool use_dss2_fck;
+	bool use_sys_clk;
 };
 
 struct seq_file;
 struct platform_device;
 
 /* core */
-void dss_clk_enable(enum dss_clock clks);
-void dss_clk_disable(enum dss_clock clks);
-unsigned long dss_clk_get_rate(enum dss_clock clk);
-int dss_need_ctx_restore(void);
-void dss_dump_clocks(struct seq_file *s);
 struct bus_type *dss_get_bus(void);
 struct regulator *dss_get_vdds_dsi(void);
 struct regulator *dss_get_vdds_sdi(void);
-struct regulator *dss_get_vdda_dac(void);
 
 /* display */
 int dss_suspend_all_devices(void);
@@ -214,13 +212,22 @@ void dss_overlay_setup_l4_manager(struct omap_overlay_manager *mgr);
 void dss_recheck_connections(struct omap_dss_device *dssdev, bool force);
 
 /* DSS */
-int dss_init(bool skip_init);
-void dss_exit(void);
+int dss_init_platform_driver(void);
+void dss_uninit_platform_driver(void);
 
 void dss_save_context(void);
 void dss_restore_context(void);
+void dss_clk_enable(enum dss_clock clks);
+void dss_clk_disable(enum dss_clock clks);
+unsigned long dss_clk_get_rate(enum dss_clock clk);
+int dss_need_ctx_restore(void);
+const char *dss_get_generic_clk_source_name(enum dss_clk_source clk_src);
+void dss_dump_clocks(struct seq_file *s);
 
 void dss_dump_regs(struct seq_file *s);
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s);
+#endif
 
 void dss_sdi_init(u8 datapairs);
 int dss_sdi_enable(void);
@@ -244,11 +251,11 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 
 /* SDI */
 #ifdef CONFIG_OMAP2_DSS_SDI
-int sdi_init(bool skip_init);
+int sdi_init(void);
 void sdi_exit(void);
 int sdi_init_display(struct omap_dss_device *display);
 #else
-static inline int sdi_init(bool skip_init)
+static inline int sdi_init(void)
 {
 	return 0;
 }
@@ -259,8 +266,8 @@ static inline void sdi_exit(void)
 
 /* DSI */
 #ifdef CONFIG_OMAP2_DSS_DSI
-int dsi_init(struct platform_device *pdev);
-void dsi_exit(void);
+int dsi_init_platform_driver(void);
+void dsi_uninit_platform_driver(void);
 
 void dsi_dump_clocks(struct seq_file *s);
 void dsi_dump_irqs(struct seq_file *s);
@@ -271,7 +278,7 @@ void dsi_restore_context(void);
 
 int dsi_init_display(struct omap_dss_device *display);
 void dsi_irq_handler(void);
-unsigned long dsi_get_dsi1_pll_rate(void);
+unsigned long dsi_get_pll_hsdiv_dispc_rate(void);
 int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo);
 int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
 		struct dsi_clock_info *cinfo,
@@ -282,31 +289,31 @@ void dsi_pll_uninit(void);
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
 		u32 fifo_size, enum omap_burst_size *burst_size,
 		u32 *fifo_low, u32 *fifo_high);
-void dsi_wait_dsi1_pll_active(void);
-void dsi_wait_dsi2_pll_active(void);
+void dsi_wait_pll_hsdiv_dispc_active(void);
+void dsi_wait_pll_hsdiv_dsi_active(void);
 #else
-static inline int dsi_init(struct platform_device *pdev)
+static inline int dsi_init_platform_driver(void)
 {
 	return 0;
 }
-static inline void dsi_exit(void)
+static inline void dsi_uninit_platform_driver(void)
 {
 }
-static inline void dsi_wait_dsi1_pll_active(void)
+static inline void dsi_wait_pll_hsdiv_dispc_active(void)
 {
 }
-static inline void dsi_wait_dsi2_pll_active(void)
+static inline void dsi_wait_pll_hsdiv_dsi_active(void)
 {
 }
 #endif
 
 /* DPI */
 #ifdef CONFIG_OMAP2_DSS_DPI
-int dpi_init(struct platform_device *pdev);
+int dpi_init(void);
 void dpi_exit(void);
 int dpi_init_display(struct omap_dss_device *dssdev);
 #else
-static inline int dpi_init(struct platform_device *pdev)
+static inline int dpi_init(void)
 {
 	return 0;
 }
@@ -316,8 +323,8 @@ static inline void dpi_exit(void)
 #endif
 
 /* DISPC */
-int dispc_init(void);
-void dispc_exit(void);
+int dispc_init_platform_driver(void);
+void dispc_uninit_platform_driver(void);
 void dispc_dump_clocks(struct seq_file *s);
 void dispc_dump_irqs(struct seq_file *s);
 void dispc_dump_regs(struct seq_file *s);
@@ -409,24 +416,24 @@ int dispc_get_clock_div(enum omap_channel channel,
 
 /* VENC */
 #ifdef CONFIG_OMAP2_DSS_VENC
-int venc_init(struct platform_device *pdev);
-void venc_exit(void);
+int venc_init_platform_driver(void);
+void venc_uninit_platform_driver(void);
 void venc_dump_regs(struct seq_file *s);
 int venc_init_display(struct omap_dss_device *display);
 #else
-static inline int venc_init(struct platform_device *pdev)
+static inline int venc_init_platform_driver(void)
 {
 	return 0;
 }
-static inline void venc_exit(void)
+static inline void venc_uninit_platform_driver(void)
 {
 }
 #endif
 
 /* RFBI */
 #ifdef CONFIG_OMAP2_DSS_RFBI
-int rfbi_init(void);
-void rfbi_exit(void);
+int rfbi_init_platform_driver(void);
+void rfbi_uninit_platform_driver(void);
 void rfbi_dump_regs(struct seq_file *s);
 
 int rfbi_configure(int rfbi_module, int bpp, int lines);
@@ -437,11 +444,11 @@ void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t);
 unsigned long rfbi_get_max_tx_rate(void);
 int rfbi_init_display(struct omap_dss_device *display);
 #else
-static inline int rfbi_init(void)
+static inline int rfbi_init_platform_driver(void)
 {
 	return 0;
 }
-static inline void rfbi_exit(void)
+static inline void rfbi_uninit_platform_driver(void)
 {
 }
 #endif
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index cf3ef696e141..dc170ad079f2 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -25,6 +25,7 @@
 #include <plat/display.h>
 #include <plat/cpu.h>
 
+#include "dss.h"
 #include "dss_features.h"
 
 /* Defines a generic omap register field */
@@ -41,8 +42,10 @@ struct omap_dss_features {
 
 	const int num_mgrs;
 	const int num_ovls;
+	const unsigned long max_dss_fck;
 	const enum omap_display_type *supported_displays;
 	const enum omap_color_mode *supported_color_modes;
+	const struct dss_clk_source_name *clksrc_names;
 };
 
 /* This struct is assigned to one of the below during initialization */
@@ -54,6 +57,8 @@ static const struct dss_reg_field omap2_dss_reg_fields[] = {
 	{ 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 },
 };
 
 static const struct dss_reg_field omap3_dss_reg_fields[] = {
@@ -62,10 +67,30 @@ static const struct dss_reg_field omap3_dss_reg_fields[] = {
 	{ 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 },
+};
+
+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 },
 };
 
 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,
 
@@ -73,10 +98,10 @@ static const enum omap_display_type omap2_dss_supported_displays[] = {
 	OMAP_DISPLAY_TYPE_VENC,
 };
 
-static const enum omap_display_type omap3_dss_supported_displays[] = {
+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_SDI | OMAP_DISPLAY_TYPE_DSI,
+	OMAP_DISPLAY_TYPE_DSI,
 
 	/* OMAP_DSS_CHANNEL_DIGIT */
 	OMAP_DISPLAY_TYPE_VENC,
@@ -134,6 +159,18 @@ static const enum omap_color_mode omap3_dss_supported_color_modes[] = {
 	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32,
 };
 
+static const struct dss_clk_source_name omap2_dss_clk_source_names[] = {
+	{ DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC, "N/A" },
+	{ DSS_CLK_SRC_DSI_PLL_HSDIV_DSI, "N/A" },
+	{ DSS_CLK_SRC_FCK, "DSS_FCLK1" },
+};
+
+static const struct dss_clk_source_name omap3_dss_clk_source_names[] = {
+	{ DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC, "DSI1_PLL_FCLK" },
+	{ DSS_CLK_SRC_DSI_PLL_HSDIV_DSI, "DSI2_PLL_FCLK" },
+	{ DSS_CLK_SRC_FCK, "DSS1_ALWON_FCLK" },
+};
+
 /* OMAP2 DSS Features */
 static struct omap_dss_features omap2_dss_features = {
 	.reg_fields = omap2_dss_reg_fields,
@@ -141,12 +178,15 @@ static struct omap_dss_features omap2_dss_features = {
 
 	.has_feature	=
 		FEAT_LCDENABLEPOL | FEAT_LCDENABLESIGNAL |
-		FEAT_PCKFREEENABLE | FEAT_FUNCGATED,
+		FEAT_PCKFREEENABLE | FEAT_FUNCGATED |
+		FEAT_ROWREPEATENABLE | FEAT_RESIZECONF,
 
 	.num_mgrs = 2,
 	.num_ovls = 3,
+	.max_dss_fck = 173000000,
 	.supported_displays = omap2_dss_supported_displays,
 	.supported_color_modes = omap2_dss_supported_color_modes,
+	.clksrc_names = omap2_dss_clk_source_names,
 };
 
 /* OMAP3 DSS Features */
@@ -157,12 +197,15 @@ static struct omap_dss_features omap3430_dss_features = {
 	.has_feature	=
 		FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
 		FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
-		FEAT_FUNCGATED,
+		FEAT_FUNCGATED | FEAT_ROWREPEATENABLE |
+		FEAT_LINEBUFFERSPLIT | FEAT_RESIZECONF,
 
 	.num_mgrs = 2,
 	.num_ovls = 3,
-	.supported_displays = omap3_dss_supported_displays,
+	.max_dss_fck = 173000000,
+	.supported_displays = omap3430_dss_supported_displays,
 	.supported_color_modes = omap3_dss_supported_color_modes,
+	.clksrc_names = omap3_dss_clk_source_names,
 };
 
 static struct omap_dss_features omap3630_dss_features = {
@@ -172,27 +215,34 @@ static struct omap_dss_features omap3630_dss_features = {
 	.has_feature    =
 		FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
 		FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
-		FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED,
+		FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED |
+		FEAT_ROWREPEATENABLE | FEAT_LINEBUFFERSPLIT |
+		FEAT_RESIZECONF,
 
 	.num_mgrs = 2,
 	.num_ovls = 3,
-	.supported_displays = omap3_dss_supported_displays,
+	.max_dss_fck = 173000000,
+	.supported_displays = omap3630_dss_supported_displays,
 	.supported_color_modes = omap3_dss_supported_color_modes,
+	.clksrc_names = omap3_dss_clk_source_names,
 };
 
 /* OMAP4 DSS Features */
 static struct omap_dss_features omap4_dss_features = {
-	.reg_fields = omap3_dss_reg_fields,
-	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+	.reg_fields = omap4_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
 
 	.has_feature	=
 		FEAT_GLOBAL_ALPHA | FEAT_PRE_MULT_ALPHA |
-		FEAT_MGR_LCD2,
+		FEAT_MGR_LCD2 | FEAT_GLOBAL_ALPHA_VID1 |
+		FEAT_CORE_CLK_DIV,
 
 	.num_mgrs = 3,
 	.num_ovls = 3,
+	.max_dss_fck = 186000000,
 	.supported_displays = omap4_dss_supported_displays,
 	.supported_color_modes = omap3_dss_supported_color_modes,
+	.clksrc_names = omap3_dss_clk_source_names,
 };
 
 /* Functions returning values related to a DSS feature */
@@ -206,6 +256,12 @@ int dss_feat_get_num_ovls(void)
 	return omap_current_dss_features->num_ovls;
 }
 
+/* Max supported DSS FCK in Hz */
+unsigned long dss_feat_get_max_dss_fck(void)
+{
+	return omap_current_dss_features->max_dss_fck;
+}
+
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel)
 {
 	return omap_current_dss_features->supported_displays[channel];
@@ -223,6 +279,11 @@ bool dss_feat_color_mode_supported(enum omap_plane plane,
 			color_mode;
 }
 
+const char *dss_feat_get_clk_source_name(enum dss_clk_source id)
+{
+	return omap_current_dss_features->clksrc_names[id].clksrc_name;
+}
+
 /* DSS has_feature check */
 bool dss_has_feature(enum dss_feat_id id)
 {
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
index b9c70be92588..569d1b295edd 100644
--- a/drivers/video/omap2/dss/dss_features.h
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -33,6 +33,11 @@ enum dss_feat_id {
 	FEAT_PCKFREEENABLE	= 1 << 5,
 	FEAT_FUNCGATED		= 1 << 6,
 	FEAT_MGR_LCD2		= 1 << 7,
+	FEAT_LINEBUFFERSPLIT	= 1 << 8,
+	FEAT_ROWREPEATENABLE	= 1 << 9,
+	FEAT_RESIZECONF		= 1 << 10,
+	/* Independent core clk divider */
+	FEAT_CORE_CLK_DIV	= 1 << 11,
 };
 
 /* DSS register field id */
@@ -42,15 +47,19 @@ enum dss_feat_reg_field {
 	FEAT_REG_FIFOHIGHTHRESHOLD,
 	FEAT_REG_FIFOLOWTHRESHOLD,
 	FEAT_REG_FIFOSIZE,
+	FEAT_REG_HORIZONTALACCU,
+	FEAT_REG_VERTICALACCU,
 };
 
 /* DSS Feature Functions */
 int dss_feat_get_num_mgrs(void);
 int dss_feat_get_num_ovls(void);
+unsigned long dss_feat_get_max_dss_fck(void);
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
 enum omap_color_mode dss_feat_get_supported_color_modes(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 dss_clk_source id);
 
 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);
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 172d4e697309..1f53bf20b6cb 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -1394,7 +1394,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 	}
 
 	r = 0;
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 	if (!dss_cache.irq_enabled) {
 		u32 mask;
 
@@ -1407,7 +1407,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
 		dss_cache.irq_enabled = true;
 	}
 	configure_dispc();
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	spin_unlock_irqrestore(&dss_cache.lock, flags);
 
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 456efef03c20..996e9a4f6771 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -490,7 +490,7 @@ static int omap_dss_set_manager(struct omap_overlay *ovl,
 
 	ovl->manager = mgr;
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 	/* XXX: on manual update display, in auto update mode, a bug happens
 	 * here. When an overlay is first enabled on LCD, then it's disabled,
 	 * and the manager is changed to TV, we sometimes get SYNC_LOST_DIGIT
@@ -499,7 +499,7 @@ static int omap_dss_set_manager(struct omap_overlay *ovl,
 	 * but I don't understand how or why. */
 	msleep(40);
 	dispc_set_channel_out(ovl->id, mgr->id);
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	return 0;
 }
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index 10a2ffe02882..5ea17f49c611 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -36,8 +36,6 @@
 #include <plat/display.h>
 #include "dss.h"
 
-#define RFBI_BASE               0x48050800
-
 struct rfbi_reg { u16 idx; };
 
 #define RFBI_REG(idx)		((const struct rfbi_reg) { idx })
@@ -100,6 +98,7 @@ 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;
@@ -142,9 +141,9 @@ static inline u32 rfbi_read_reg(const struct rfbi_reg idx)
 static void rfbi_enable_clocks(bool enable)
 {
 	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 void omap_rfbi_write_command(const void *buf, u32 len)
@@ -497,7 +496,7 @@ unsigned long rfbi_get_max_tx_rate(void)
 	};
 
 	l4_rate = rfbi.l4_khz / 1000;
-	dss1_rate = dss_clk_get_rate(DSS_CLK_FCK1) / 1000000;
+	dss1_rate = dss_clk_get_rate(DSS_CLK_FCK) / 1000000;
 
 	for (i = 0; i < ARRAY_SIZE(ftab); i++) {
 		/* Use a window instead of an exact match, to account
@@ -922,7 +921,7 @@ void rfbi_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, rfbi_read_reg(r))
 
-	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	DUMPREG(RFBI_REVISION);
 	DUMPREG(RFBI_SYSCONFIG);
@@ -953,54 +952,10 @@ void rfbi_dump_regs(struct seq_file *s)
 	DUMPREG(RFBI_VSYNC_WIDTH);
 	DUMPREG(RFBI_HSYNC_WIDTH);
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
-int rfbi_init(void)
-{
-	u32 rev;
-	u32 l;
-
-	spin_lock_init(&rfbi.cmd_lock);
-
-	init_completion(&rfbi.cmd_done);
-	atomic_set(&rfbi.cmd_fifo_full, 0);
-	atomic_set(&rfbi.cmd_pending, 0);
-
-	rfbi.base = ioremap(RFBI_BASE, SZ_256);
-	if (!rfbi.base) {
-		DSSERR("can't ioremap RFBI\n");
-		return -ENOMEM;
-	}
-
-	rfbi_enable_clocks(1);
-
-	msleep(10);
-
-	rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
-
-	/* Enable autoidle and smart-idle */
-	l = rfbi_read_reg(RFBI_SYSCONFIG);
-	l |= (1 << 0) | (2 << 3);
-	rfbi_write_reg(RFBI_SYSCONFIG, l);
-
-	rev = rfbi_read_reg(RFBI_REVISION);
-	printk(KERN_INFO "OMAP RFBI rev %d.%d\n",
-	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-	rfbi_enable_clocks(0);
-
-	return 0;
-}
-
-void rfbi_exit(void)
-{
-	DSSDBG("rfbi_exit\n");
-
-	iounmap(rfbi.base);
-}
-
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
 	int r;
@@ -1056,3 +1011,74 @@ int rfbi_init_display(struct omap_dss_device *dssdev)
 	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 	return 0;
 }
+
+/* RFBI HW IP initialisation */
+static int omap_rfbihw_probe(struct platform_device *pdev)
+{
+	u32 rev;
+	u32 l;
+	struct resource *rfbi_mem;
+
+	rfbi.pdev = pdev;
+
+	spin_lock_init(&rfbi.cmd_lock);
+
+	init_completion(&rfbi.cmd_done);
+	atomic_set(&rfbi.cmd_fifo_full, 0);
+	atomic_set(&rfbi.cmd_pending, 0);
+
+	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 = ioremap(rfbi_mem->start, resource_size(rfbi_mem));
+	if (!rfbi.base) {
+		DSSERR("can't ioremap RFBI\n");
+		return -ENOMEM;
+	}
+
+	rfbi_enable_clocks(1);
+
+	msleep(10);
+
+	rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
+
+	/* Enable autoidle and smart-idle */
+	l = rfbi_read_reg(RFBI_SYSCONFIG);
+	l |= (1 << 0) | (2 << 3);
+	rfbi_write_reg(RFBI_SYSCONFIG, l);
+
+	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_enable_clocks(0);
+
+	return 0;
+}
+
+static int omap_rfbihw_remove(struct platform_device *pdev)
+{
+	iounmap(rfbi.base);
+	return 0;
+}
+
+static struct platform_driver omap_rfbihw_driver = {
+	.probe          = omap_rfbihw_probe,
+	.remove         = omap_rfbihw_remove,
+	.driver         = {
+		.name   = "omapdss_rfbi",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int rfbi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_rfbihw_driver);
+}
+
+void rfbi_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_rfbihw_driver);
+}
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index b64adf7dfc88..54a53e648180 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -30,7 +30,6 @@
 #include "dss.h"
 
 static struct {
-	bool skip_init;
 	bool update_enabled;
 	struct regulator *vdds_sdi_reg;
 } sdi;
@@ -68,9 +67,7 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err1;
 
-	/* In case of skip_init sdi_init has already enabled the clocks */
-	if (!sdi.skip_init)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	sdi_basic_init(dssdev);
 
@@ -80,14 +77,8 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
 			dssdev->panel.acbi, dssdev->panel.acb);
 
-	if (!sdi.skip_init) {
-		r = dss_calc_clock_div(1, t->pixel_clock * 1000,
-				&dss_cinfo, &dispc_cinfo);
-	} else {
-		r = dss_get_clock_div(&dss_cinfo);
-		r = dispc_get_clock_div(dssdev->manager->id, &dispc_cinfo);
-	}
-
+	r = dss_calc_clock_div(1, t->pixel_clock * 1000,
+			&dss_cinfo, &dispc_cinfo);
 	if (r)
 		goto err2;
 
@@ -116,21 +107,17 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err2;
 
-	if (!sdi.skip_init) {
-		dss_sdi_init(dssdev->phy.sdi.datapairs);
-		r = dss_sdi_enable();
-		if (r)
-			goto err1;
-		mdelay(2);
-	}
+	dss_sdi_init(dssdev->phy.sdi.datapairs);
+	r = dss_sdi_enable();
+	if (r)
+		goto err1;
+	mdelay(2);
 
 	dssdev->manager->enable(dssdev->manager);
 
-	sdi.skip_init = 0;
-
 	return 0;
 err2:
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 	regulator_disable(sdi.vdds_sdi_reg);
 err1:
 	omap_dss_stop_device(dssdev);
@@ -145,7 +132,7 @@ void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 
 	dss_sdi_disable();
 
-	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+	dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
 	regulator_disable(sdi.vdds_sdi_reg);
 
@@ -157,25 +144,24 @@ int sdi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("SDI init\n");
 
+	if (sdi.vdds_sdi_reg == NULL) {
+		struct regulator *vdds_sdi;
+
+		vdds_sdi = dss_get_vdds_sdi();
+
+		if (IS_ERR(vdds_sdi)) {
+			DSSERR("can't get VDDS_SDI regulator\n");
+			return PTR_ERR(vdds_sdi);
+		}
+
+		sdi.vdds_sdi_reg = vdds_sdi;
+	}
+
 	return 0;
 }
 
-int sdi_init(bool skip_init)
+int sdi_init(void)
 {
-	/* we store this for first display enable, then clear it */
-	sdi.skip_init = skip_init;
-
-	sdi.vdds_sdi_reg = dss_get_vdds_sdi();
-	if (IS_ERR(sdi.vdds_sdi_reg)) {
-		DSSERR("can't get VDDS_SDI regulator\n");
-		return PTR_ERR(sdi.vdds_sdi_reg);
-	}
-	/*
-	 * Enable clocks already here, otherwise there would be a toggle
-	 * of them until sdi_display_enable is called.
-	 */
-	if (skip_init)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
 	return 0;
 }
 
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index eff35050e28a..43009e57cd3e 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -39,8 +39,6 @@
 
 #include "dss.h"
 
-#define VENC_BASE	0x48050C00
-
 /* Venc registers */
 #define VENC_REV_ID				0x00
 #define VENC_STATUS				0x04
@@ -289,6 +287,7 @@ const struct omap_video_timings omap_dss_ntsc_timings = {
 EXPORT_SYMBOL(omap_dss_ntsc_timings);
 
 static struct {
+	struct platform_device *pdev;
 	void __iomem *base;
 	struct mutex venc_lock;
 	u32 wss_data;
@@ -381,11 +380,11 @@ static void venc_reset(void)
 static void venc_enable_clocks(int enable)
 {
 	if (enable)
-		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-				DSS_CLK_96M);
+		dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_TVFCK |
+				DSS_CLK_VIDFCK);
 	else
-		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-				DSS_CLK_96M);
+		dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_TVFCK |
+				DSS_CLK_VIDFCK);
 }
 
 static const struct venc_config *venc_timings_to_config(
@@ -641,50 +640,23 @@ static struct omap_dss_driver venc_driver = {
 };
 /* driver end */
 
-
-
-int venc_init(struct platform_device *pdev)
+int venc_init_display(struct omap_dss_device *dssdev)
 {
-	u8 rev_id;
+	DSSDBG("init_display\n");
 
-	mutex_init(&venc.venc_lock);
+	if (venc.vdda_dac_reg == NULL) {
+		struct regulator *vdda_dac;
 
-	venc.wss_data = 0;
+		vdda_dac = regulator_get(&venc.pdev->dev, "vdda_dac");
 
-	venc.base = ioremap(VENC_BASE, SZ_1K);
-	if (!venc.base) {
-		DSSERR("can't ioremap VENC\n");
-		return -ENOMEM;
-	}
+		if (IS_ERR(vdda_dac)) {
+			DSSERR("can't get VDDA_DAC regulator\n");
+			return PTR_ERR(vdda_dac);
+		}
 
-	venc.vdda_dac_reg = dss_get_vdda_dac();
-	if (IS_ERR(venc.vdda_dac_reg)) {
-		iounmap(venc.base);
-		DSSERR("can't get VDDA_DAC regulator\n");
-		return PTR_ERR(venc.vdda_dac_reg);
+		venc.vdda_dac_reg = vdda_dac;
 	}
 
-	venc_enable_clocks(1);
-
-	rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
-	printk(KERN_INFO "OMAP VENC rev %d\n", rev_id);
-
-	venc_enable_clocks(0);
-
-	return omap_dss_register_driver(&venc_driver);
-}
-
-void venc_exit(void)
-{
-	omap_dss_unregister_driver(&venc_driver);
-
-	iounmap(venc.base);
-}
-
-int venc_init_display(struct omap_dss_device *dssdev)
-{
-	DSSDBG("init_display\n");
-
 	return 0;
 }
 
@@ -740,3 +712,67 @@ void venc_dump_regs(struct seq_file *s)
 
 #undef DUMPREG
 }
+
+/* VENC HW IP initialisation */
+static int omap_venchw_probe(struct platform_device *pdev)
+{
+	u8 rev_id;
+	struct resource *venc_mem;
+
+	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 = ioremap(venc_mem->start, resource_size(venc_mem));
+	if (!venc.base) {
+		DSSERR("can't ioremap VENC\n");
+		return -ENOMEM;
+	}
+
+	venc_enable_clocks(1);
+
+	rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
+	dev_dbg(&pdev->dev, "OMAP VENC rev %d\n", rev_id);
+
+	venc_enable_clocks(0);
+
+	return omap_dss_register_driver(&venc_driver);
+}
+
+static int omap_venchw_remove(struct platform_device *pdev)
+{
+	if (venc.vdda_dac_reg != NULL) {
+		regulator_put(venc.vdda_dac_reg);
+		venc.vdda_dac_reg = NULL;
+	}
+	omap_dss_unregister_driver(&venc_driver);
+
+	iounmap(venc.base);
+	return 0;
+}
+
+static struct platform_driver omap_venchw_driver = {
+	.probe          = omap_venchw_probe,
+	.remove         = omap_venchw_remove,
+	.driver         = {
+		.name   = "omapdss_venc",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int venc_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_venchw_driver);
+}
+
+void venc_uninit_platform_driver(void)
+{
+	return platform_driver_unregister(&omap_venchw_driver);
+}
diff --git a/drivers/video/omap2/omapfb/Kconfig b/drivers/video/omap2/omapfb/Kconfig
index 65149b22cf37..aa33386c81ff 100644
--- a/drivers/video/omap2/omapfb/Kconfig
+++ b/drivers/video/omap2/omapfb/Kconfig
@@ -1,5 +1,5 @@
 menuconfig FB_OMAP2
-        tristate "OMAP2/3 frame buffer support (EXPERIMENTAL)"
+        tristate "OMAP2+ frame buffer support (EXPERIMENTAL)"
         depends on FB && OMAP2_DSS
 
 	select OMAP2_VRAM
@@ -8,10 +8,10 @@ menuconfig FB_OMAP2
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
         help
-          Frame buffer driver for OMAP2/3 based boards.
+	  Frame buffer driver for OMAP2+ based boards.
 
 config FB_OMAP2_DEBUG_SUPPORT
-	bool "Debug support for OMAP2/3 FB"
+        bool "Debug support for OMAP2+ FB"
 	default y
 	depends on FB_OMAP2
 	help
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 4fdab8e9c496..505ec6672049 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -2090,7 +2090,7 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 {
 	int r;
 	u8 bpp;
-	struct omap_video_timings timings;
+	struct omap_video_timings timings, temp_timings;
 
 	r = omapfb_mode_to_timings(mode_str, &timings, &bpp);
 	if (r)
@@ -2100,14 +2100,23 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 	fbdev->bpp_overrides[fbdev->num_bpp_overrides].bpp = bpp;
 	++fbdev->num_bpp_overrides;
 
-	if (!display->driver->check_timings || !display->driver->set_timings)
-		return -EINVAL;
+	if (display->driver->check_timings) {
+		r = display->driver->check_timings(display, &timings);
+		if (r)
+			return r;
+	} else {
+		/* If check_timings is not present compare xres and yres */
+		if (display->driver->get_timings) {
+			display->driver->get_timings(display, &temp_timings);
 
-	r = display->driver->check_timings(display, &timings);
-	if (r)
-		return r;
+			if (temp_timings.x_res != timings.x_res ||
+				temp_timings.y_res != timings.y_res)
+				return -EINVAL;
+		}
+	}
 
-	display->driver->set_timings(display, &timings);
+	if (display->driver->set_timings)
+			display->driver->set_timings(display, &timings);
 
 	return 0;
 }
diff --git a/drivers/video/p9100.c b/drivers/video/p9100.c
index b6c3fc2db632..d57cc58c5168 100644
--- a/drivers/video/p9100.c
+++ b/drivers/video/p9100.c
@@ -249,7 +249,7 @@ static void p9100_init_fix(struct fb_info *info, int linebytes, struct device_no
 	info->fix.accel = FB_ACCEL_SUN_CGTHREE;
 }
 
-static int __devinit p9100_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit p9100_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -352,7 +352,7 @@ static const struct of_device_id p9100_match[] = {
 };
 MODULE_DEVICE_TABLE(of, p9100_match);
 
-static struct of_platform_driver p9100_driver = {
+static struct platform_driver p9100_driver = {
 	.driver = {
 		.name = "p9100",
 		.owner = THIS_MODULE,
@@ -367,12 +367,12 @@ static int __init p9100_init(void)
 	if (fb_get_options("p9100fb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&p9100_driver);
+	return platform_driver_register(&p9100_driver);
 }
 
 static void __exit p9100_exit(void)
 {
-	of_unregister_platform_driver(&p9100_driver);
+	platform_driver_unregister(&p9100_driver);
 }
 
 module_init(p9100_init);
diff --git a/drivers/video/platinumfb.c b/drivers/video/platinumfb.c
index a50e1977b316..ef532d9d3c99 100644
--- a/drivers/video/platinumfb.c
+++ b/drivers/video/platinumfb.c
@@ -533,8 +533,7 @@ static int __init platinumfb_setup(char *options)
 #define invalidate_cache(addr)
 #endif
 
-static int __devinit platinumfb_probe(struct platform_device* odev,
-				      const struct of_device_id *match)
+static int __devinit platinumfb_probe(struct platform_device* odev)
 {
 	struct device_node	*dp = odev->dev.of_node;
 	struct fb_info		*info;
@@ -677,7 +676,7 @@ static struct of_device_id platinumfb_match[] =
 	{},
 };
 
-static struct of_platform_driver platinum_driver = 
+static struct platform_driver platinum_driver = 
 {
 	.driver = {
 		.name = "platinumfb",
@@ -697,14 +696,14 @@ static int __init platinumfb_init(void)
 		return -ENODEV;
 	platinumfb_setup(option);
 #endif
-	of_register_platform_driver(&platinum_driver);
+	platform_driver_register(&platinum_driver);
 
 	return 0;
 }
 
 static void __exit platinumfb_exit(void)
 {
-	of_unregister_platform_driver(&platinum_driver);
+	platform_driver_unregister(&platinum_driver);
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/sunxvr1000.c b/drivers/video/sunxvr1000.c
index 5dbe06af2226..b7f27acaf817 100644
--- a/drivers/video/sunxvr1000.c
+++ b/drivers/video/sunxvr1000.c
@@ -111,8 +111,7 @@ static int __devinit gfb_set_fbinfo(struct gfb_info *gp)
         return 0;
 }
 
-static int __devinit gfb_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit gfb_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -198,7 +197,7 @@ static const struct of_device_id gfb_match[] = {
 };
 MODULE_DEVICE_TABLE(of, ffb_match);
 
-static struct of_platform_driver gfb_driver = {
+static struct platform_driver gfb_driver = {
 	.probe		= gfb_probe,
 	.remove		= __devexit_p(gfb_remove),
 	.driver = {
@@ -213,12 +212,12 @@ static int __init gfb_init(void)
 	if (fb_get_options("gfb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&gfb_driver);
+	return platform_driver_register(&gfb_driver);
 }
 
 static void __exit gfb_exit(void)
 {
-	of_unregister_platform_driver(&gfb_driver);
+	platform_driver_unregister(&gfb_driver);
 }
 
 module_init(gfb_init);
diff --git a/drivers/video/tcx.c b/drivers/video/tcx.c
index 77ad27955cf0..855b71993f61 100644
--- a/drivers/video/tcx.c
+++ b/drivers/video/tcx.c
@@ -362,8 +362,7 @@ static void tcx_unmap_regs(struct platform_device *op, struct fb_info *info,
 			   info->screen_base, info->fix.smem_len);
 }
 
-static int __devinit tcx_probe(struct platform_device *op,
-			       const struct of_device_id *match)
+static int __devinit tcx_probe(struct platform_device *op)
 {
 	struct device_node *dp = op->dev.of_node;
 	struct fb_info *info;
@@ -511,7 +510,7 @@ static const struct of_device_id tcx_match[] = {
 };
 MODULE_DEVICE_TABLE(of, tcx_match);
 
-static struct of_platform_driver tcx_driver = {
+static struct platform_driver tcx_driver = {
 	.driver = {
 		.name = "tcx",
 		.owner = THIS_MODULE,
@@ -526,12 +525,12 @@ static int __init tcx_init(void)
 	if (fb_get_options("tcxfb", NULL))
 		return -ENODEV;
 
-	return of_register_platform_driver(&tcx_driver);
+	return platform_driver_register(&tcx_driver);
 }
 
 static void __exit tcx_exit(void)
 {
-	of_unregister_platform_driver(&tcx_driver);
+	platform_driver_unregister(&tcx_driver);
 }
 
 module_init(tcx_init);
diff --git a/drivers/video/tmiofb.c b/drivers/video/tmiofb.c
index dfef88c803d4..9710bf8caeae 100644
--- a/drivers/video/tmiofb.c
+++ b/drivers/video/tmiofb.c
@@ -250,8 +250,7 @@ static irqreturn_t tmiofb_irq(int irq, void *__info)
  */
 static int tmiofb_hw_stop(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data = mfd_get_data(dev);
 	struct fb_info *info = platform_get_drvdata(dev);
 	struct tmiofb_par *par = info->par;
 
@@ -268,7 +267,7 @@ static int tmiofb_hw_stop(struct platform_device *dev)
  */
 static int tmiofb_hw_init(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct fb_info *info = platform_get_drvdata(dev);
 	struct tmiofb_par *par = info->par;
 	const struct resource *nlcr = &cell->resources[0];
@@ -312,8 +311,7 @@ static int tmiofb_hw_init(struct platform_device *dev)
  */
 static void tmiofb_hw_mode(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data = mfd_get_data(dev);
 	struct fb_info *info = platform_get_drvdata(dev);
 	struct fb_videomode *mode = info->mode;
 	struct tmiofb_par *par = info->par;
@@ -559,9 +557,8 @@ static int tmiofb_ioctl(struct fb_info *fbi,
 static struct fb_videomode *
 tmiofb_find_mode(struct fb_info *info, struct fb_var_screeninfo *var)
 {
-	struct mfd_cell *cell =
-		info->device->platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data =
+			mfd_get_data(to_platform_device(info->device));
 	struct fb_videomode *best = NULL;
 	int i;
 
@@ -581,9 +578,8 @@ static int tmiofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 
 	struct fb_videomode *mode;
-	struct mfd_cell *cell =
-		info->device->platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	struct tmio_fb_data *data =
+			mfd_get_data(to_platform_device(info->device));
 
 	mode = tmiofb_find_mode(info, var);
 	if (!mode || var->bits_per_pixel > 16)
@@ -683,8 +679,8 @@ static struct fb_ops tmiofb_ops = {
 
 static int __devinit tmiofb_probe(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
-	struct tmio_fb_data *data = cell->driver_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
+	struct tmio_fb_data *data = mfd_get_data(dev);
 	struct resource *ccr = platform_get_resource(dev, IORESOURCE_MEM, 1);
 	struct resource *lcr = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	struct resource *vram = platform_get_resource(dev, IORESOURCE_MEM, 2);
@@ -811,7 +807,7 @@ err_ioremap_ccr:
 
 static int __devexit tmiofb_remove(struct platform_device *dev)
 {
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	struct fb_info *info = platform_get_drvdata(dev);
 	int irq = platform_get_irq(dev, 0);
 	struct tmiofb_par *par;
@@ -941,7 +937,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 #ifdef CONFIG_FB_TMIO_ACCELL
 	struct tmiofb_par *par = info->par;
 #endif
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	int retval = 0;
 
 	console_lock();
@@ -973,7 +969,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 static int tmiofb_resume(struct platform_device *dev)
 {
 	struct fb_info *info = platform_get_drvdata(dev);
-	struct mfd_cell *cell = dev->dev.platform_data;
+	const struct mfd_cell *cell = mfd_get_cell(dev);
 	int retval = 0;
 
 	console_lock();
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index 52ec0959d462..5180a215d781 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -73,7 +73,7 @@ static void uvesafb_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *ns
 	struct uvesafb_task *utask;
 	struct uvesafb_ktask *task;
 
-	if (!cap_raised(nsp->eff_cap, CAP_SYS_ADMIN))
+	if (!cap_raised(current_cap(), CAP_SYS_ADMIN))
 		return;
 
 	if (msg->seq >= UVESAFB_TASKS_MAX)
diff --git a/drivers/video/xilinxfb.c b/drivers/video/xilinxfb.c
index 68bd23476c64..77dea015ff69 100644
--- a/drivers/video/xilinxfb.c
+++ b/drivers/video/xilinxfb.c
@@ -404,8 +404,7 @@ static int xilinxfb_release(struct device *dev)
  * OF bus binding
  */
 
-static int __devinit
-xilinxfb_of_probe(struct platform_device *op, const struct of_device_id *match)
+static int __devinit xilinxfb_of_probe(struct platform_device *op)
 {
 	const u32 *prop;
 	u32 *p;
@@ -418,8 +417,6 @@ xilinxfb_of_probe(struct platform_device *op, const struct of_device_id *match)
 	/* Copy with the default pdata (not a ptr reference!) */
 	pdata = xilinx_fb_default_pdata;
 
-	dev_dbg(&op->dev, "xilinxfb_of_probe(%p, %p)\n", op, match);
-
 	/* Allocate the driver data region */
 	drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
 	if (!drvdata) {
@@ -505,7 +502,7 @@ static struct of_device_id xilinxfb_of_match[] __devinitdata = {
 };
 MODULE_DEVICE_TABLE(of, xilinxfb_of_match);
 
-static struct of_platform_driver xilinxfb_of_driver = {
+static struct platform_driver xilinxfb_of_driver = {
 	.probe = xilinxfb_of_probe,
 	.remove = __devexit_p(xilinxfb_of_remove),
 	.driver = {
@@ -523,13 +520,13 @@ static struct of_platform_driver xilinxfb_of_driver = {
 static int __init
 xilinxfb_init(void)
 {
-	return of_register_platform_driver(&xilinxfb_of_driver);
+	return platform_driver_register(&xilinxfb_of_driver);
 }
 
 static void __exit
 xilinxfb_cleanup(void)
 {
-	of_unregister_platform_driver(&xilinxfb_of_driver);
+	platform_driver_unregister(&xilinxfb_of_driver);
 }
 
 module_init(xilinxfb_init);
diff --git a/drivers/w1/masters/Kconfig b/drivers/w1/masters/Kconfig
index 80b3b123dd7f..7c608c5ccf84 100644
--- a/drivers/w1/masters/Kconfig
+++ b/drivers/w1/masters/Kconfig
@@ -60,7 +60,7 @@ config W1_MASTER_GPIO
 
 config HDQ_MASTER_OMAP
 	tristate "OMAP HDQ driver"
-	depends on ARCH_OMAP2430 || ARCH_OMAP3
+	depends on SOC_OMAP2430 || ARCH_OMAP3
 	help
 	  Say Y here if you want support for the 1-wire or HDQ Interface
 	  on an OMAP processor.
diff --git a/drivers/w1/masters/ds1wm.c b/drivers/w1/masters/ds1wm.c
index 6b85e7fefa43..22fc726feb9b 100644
--- a/drivers/w1/masters/ds1wm.c
+++ b/drivers/w1/masters/ds1wm.c
@@ -216,7 +216,7 @@ static int ds1wm_find_divisor(int gclk)
 static void ds1wm_up(struct ds1wm_data *ds1wm_data)
 {
 	int divisor;
-	struct ds1wm_driver_data *plat = ds1wm_data->cell->driver_data;
+	struct ds1wm_driver_data *plat = mfd_get_data(ds1wm_data->pdev);
 
 	if (ds1wm_data->cell->enable)
 		ds1wm_data->cell->enable(ds1wm_data->pdev);
@@ -336,7 +336,7 @@ static int ds1wm_probe(struct platform_device *pdev)
 	if (!pdev)
 		return -ENODEV;
 
-	cell = pdev->dev.platform_data;
+	cell = mfd_get_cell(pdev);
 	if (!cell)
 		return -ENODEV;
 
@@ -356,7 +356,7 @@ static int ds1wm_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto err0;
 	}
-	plat = cell->driver_data;
+	plat = mfd_get_data(pdev);
 
 	/* calculate bus shift from mem resource */
 	ds1wm_data->bus_shift = resource_size(res) >> 3;
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 31649b7b672f..b69d71482554 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -533,6 +533,16 @@ config I6300ESB_WDT
 	  To compile this driver as a module, choose M here: the
 	  module will be called i6300esb.
 
+config INTEL_SCU_WATCHDOG
+	bool "Intel SCU Watchdog for Mobile Platforms"
+	depends on WATCHDOG
+	depends on INTEL_SCU_IPC
+	---help---
+	  Hardware driver for the watchdog time built into the Intel SCU
+	  for Intel Mobile Platforms.
+
+	  To compile this driver as a module, choose M here.
+
 config ITCO_WDT
 	tristate "Intel TCO Timer/Watchdog"
 	depends on (X86 || IA64) && PCI
@@ -580,7 +590,7 @@ config IT87_WDT
 	depends on X86 && EXPERIMENTAL
 	---help---
 	  This is the driver for the hardware watchdog on the ITE IT8702,
-	  IT8712, IT8716, IT8718, IT8720, IT8726, IT8712 Super I/O chips.
+	  IT8712, IT8716, IT8718, IT8720, IT8721, IT8726 Super I/O chips.
 	  This watchdog simply watches your kernel to make sure it doesn't
 	  freeze, and if it does, it reboots your computer after a certain
 	  amount of time.
@@ -589,18 +599,20 @@ config IT87_WDT
 	  be called it87_wdt.
 
 config HP_WATCHDOG
-	tristate "HP Proliant iLO2+ Hardware Watchdog Timer"
+	tristate "HP ProLiant iLO2+ Hardware Watchdog Timer"
 	depends on X86
+	default m
 	help
 	  A software monitoring watchdog and NMI sourcing driver. This driver
 	  will detect lockups and provide a stack trace. This is a driver that
-	  will only load on a HP ProLiant system with a minimum of iLO2 support.
+	  will only load on an HP ProLiant system with a minimum of iLO2 support.
 	  To compile this driver as a module, choose M here: the module will be
 	  called hpwdt.
 
 config HPWDT_NMI_DECODING
 	bool "NMI decoding support for the HP ProLiant iLO2+ Hardware Watchdog Timer"
 	depends on HP_WATCHDOG
+	default y
 	help
 	  When an NMI occurs this feature will make the necessary BIOS calls to
 	  log the cause of the NMI.
@@ -903,6 +915,12 @@ config INDYDOG
 	  timer expired and no process has written to /dev/watchdog during
 	  that time.
 
+config JZ4740_WDT
+	tristate "Ingenic jz4740 SoC hardware watchdog"
+	depends on MACH_JZ4740
+	help
+	  Hardware driver for the built-in watchdog timer on Ingenic jz4740 SoCs.
+
 config WDT_MTX1
 	tristate "MTX-1 Hardware Watchdog"
 	depends on MIPS_MTX1
@@ -1083,14 +1101,6 @@ config SH_WDT
 	  To compile this driver as a module, choose M here: the
 	  module will be called shwdt.
 
-config SH_WDT_MMAP
-	bool "Allow mmap of SH WDT"
-	default n
-	depends on SH_WDT
-	help
-	  If you say Y here, user applications will be able to mmap the
-	  WDT/CPG registers.
-
 # SPARC Architecture
 
 # SPARC64 Architecture
@@ -1119,6 +1129,16 @@ config WATCHDOG_RIO
 
 # XTENSA Architecture
 
+# Xen Architecture
+
+config XEN_WDT
+	tristate "Xen Watchdog support"
+	depends on XEN
+	help
+	  Say Y here to support the hypervisor watchdog capability provided
+	  by Xen 4.0 and newer.  The watchdog timeout period is normally one
+	  minute but can be changed with a boot-time parameter.
+
 #
 # ISA-based Watchdog Cards
 #
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 20e44c4782b3..d520bf9c3355 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -102,6 +102,7 @@ obj-$(CONFIG_W83877F_WDT) += w83877f_wdt.o
 obj-$(CONFIG_W83977F_WDT) += w83977f_wdt.o
 obj-$(CONFIG_MACHZ_WDT) += machzwd.o
 obj-$(CONFIG_SBC_EPX_C3_WATCHDOG) += sbc_epx_c3.o
+obj-$(CONFIG_INTEL_SCU_WATCHDOG) += intel_scu_watchdog.o
 
 # M32R Architecture
 
@@ -114,6 +115,7 @@ obj-$(CONFIG_BCM47XX_WDT) += bcm47xx_wdt.o
 obj-$(CONFIG_BCM63XX_WDT) += bcm63xx_wdt.o
 obj-$(CONFIG_RC32434_WDT) += rc32434_wdt.o
 obj-$(CONFIG_INDYDOG) += indydog.o
+obj-$(CONFIG_JZ4740_WDT) += jz4740_wdt.o
 obj-$(CONFIG_WDT_MTX1) += mtx-1_wdt.o
 obj-$(CONFIG_PNX833X_WDT) += pnx833x_wdt.o
 obj-$(CONFIG_SIBYTE_WDOG) += sb_wdog.o
@@ -148,6 +150,9 @@ obj-$(CONFIG_WATCHDOG_CP1XXX)		+= cpwd.o
 
 # XTENSA Architecture
 
+# Xen
+obj-$(CONFIG_XEN_WDT) += xen_wdt.o
+
 # Architecture Independant
 obj-$(CONFIG_WM831X_WATCHDOG) += wm831x_wdt.o
 obj-$(CONFIG_WM8350_WATCHDOG) += wm8350_wdt.o
diff --git a/drivers/watchdog/alim1535_wdt.c b/drivers/watchdog/alim1535_wdt.c
index fa4d36033552..f16dcbd475fb 100644
--- a/drivers/watchdog/alim1535_wdt.c
+++ b/drivers/watchdog/alim1535_wdt.c
@@ -301,7 +301,7 @@ static int ali_notify_sys(struct notifier_block *this,
  *	want to register another driver on the same PCI id.
  */
 
-static struct pci_device_id ali_pci_tbl[] __used = {
+static DEFINE_PCI_DEVICE_TABLE(ali_pci_tbl) __used = {
 	{ PCI_VENDOR_ID_AL, 0x1533, PCI_ANY_ID, PCI_ANY_ID,},
 	{ PCI_VENDOR_ID_AL, 0x1535, PCI_ANY_ID, PCI_ANY_ID,},
 	{ 0, },
@@ -362,12 +362,12 @@ static int __init ali_find_watchdog(void)
  */
 
 static const struct file_operations ali_fops = {
-	.owner 		=	THIS_MODULE,
-	.llseek 	=	no_llseek,
+	.owner		=	THIS_MODULE,
+	.llseek		=	no_llseek,
 	.write		=	ali_write,
 	.unlocked_ioctl =	ali_ioctl,
-	.open 		=	ali_open,
-	.release 	=	ali_release,
+	.open		=	ali_open,
+	.release	=	ali_release,
 };
 
 static struct miscdevice ali_miscdev = {
diff --git a/drivers/watchdog/alim7101_wdt.c b/drivers/watchdog/alim7101_wdt.c
index 4b7a2b4138ed..46f4b85b46de 100644
--- a/drivers/watchdog/alim7101_wdt.c
+++ b/drivers/watchdog/alim7101_wdt.c
@@ -430,7 +430,7 @@ err_out:
 module_init(alim7101_wdt_init);
 module_exit(alim7101_wdt_unload);
 
-static struct pci_device_id alim7101_pci_tbl[] __devinitdata __used = {
+static DEFINE_PCI_DEVICE_TABLE(alim7101_pci_tbl) __used = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_AL, PCI_DEVICE_ID_AL_M1533) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AL, PCI_DEVICE_ID_AL_M7101) },
 	{ }
diff --git a/drivers/watchdog/bcm47xx_wdt.c b/drivers/watchdog/bcm47xx_wdt.c
index 5f245522397b..bd44417c84d4 100644
--- a/drivers/watchdog/bcm47xx_wdt.c
+++ b/drivers/watchdog/bcm47xx_wdt.c
@@ -150,8 +150,8 @@ static ssize_t bcm47xx_wdt_write(struct file *file, const char __user *data,
 }
 
 static const struct watchdog_info bcm47xx_wdt_info = {
-	.identity 	= DRV_NAME,
-	.options 	= WDIOF_SETTIMEOUT |
+	.identity	= DRV_NAME,
+	.options	= WDIOF_SETTIMEOUT |
 				WDIOF_KEEPALIVEPING |
 				WDIOF_MAGICCLOSE,
 };
diff --git a/drivers/watchdog/bfin_wdt.c b/drivers/watchdog/bfin_wdt.c
index 9042a95fc98c..b9fa9b71583a 100644
--- a/drivers/watchdog/bfin_wdt.c
+++ b/drivers/watchdog/bfin_wdt.c
@@ -63,7 +63,7 @@ static DEFINE_SPINLOCK(bfin_wdt_spinlock);
 /**
  *	bfin_wdt_keepalive - Keep the Userspace Watchdog Alive
  *
- * 	The Userspace watchdog got a KeepAlive: schedule the next timeout.
+ *	The Userspace watchdog got a KeepAlive: schedule the next timeout.
  */
 static int bfin_wdt_keepalive(void)
 {
@@ -337,7 +337,7 @@ static int bfin_wdt_resume(struct platform_device *pdev)
 static const struct file_operations bfin_wdt_fops = {
 	.owner		= THIS_MODULE,
 	.llseek		= no_llseek,
-	.write    	= bfin_wdt_write,
+	.write		= bfin_wdt_write,
 	.unlocked_ioctl	= bfin_wdt_ioctl,
 	.open		= bfin_wdt_open,
 	.release	= bfin_wdt_release,
diff --git a/drivers/watchdog/booke_wdt.c b/drivers/watchdog/booke_wdt.c
index 7e7ec9c35b6a..337265b47305 100644
--- a/drivers/watchdog/booke_wdt.c
+++ b/drivers/watchdog/booke_wdt.c
@@ -4,7 +4,7 @@
  * Author: Matthew McClintock
  * Maintainer: Kumar Gala <galak@kernel.crashing.org>
  *
- * Copyright 2005, 2008, 2010 Freescale Semiconductor Inc.
+ * Copyright 2005, 2008, 2010-2011 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
@@ -221,9 +221,8 @@ static int booke_wdt_open(struct inode *inode, struct file *file)
 	if (booke_wdt_enabled == 0) {
 		booke_wdt_enabled = 1;
 		on_each_cpu(__booke_wdt_enable, NULL, 0);
-		printk(KERN_INFO
-		      "PowerPC Book-E Watchdog Timer Enabled (wdt_period=%d)\n",
-				booke_wdt_period);
+		pr_debug("booke_wdt: watchdog enabled (timeout = %llu sec)\n",
+			period_to_sec(booke_wdt_period));
 	}
 	spin_unlock(&booke_wdt_lock);
 
@@ -240,6 +239,7 @@ static int booke_wdt_release(struct inode *inode, struct file *file)
 	 */
 	on_each_cpu(__booke_wdt_disable, NULL, 0);
 	booke_wdt_enabled = 0;
+	pr_debug("booke_wdt: watchdog disabled\n");
 #endif
 
 	clear_bit(0, &wdt_is_active);
@@ -271,21 +271,20 @@ static int __init booke_wdt_init(void)
 {
 	int ret = 0;
 
-	printk(KERN_INFO "PowerPC Book-E Watchdog Timer Loaded\n");
+	pr_info("booke_wdt: powerpc book-e watchdog driver loaded\n");
 	ident.firmware_version = cur_cpu_spec->pvr_value;
 
 	ret = misc_register(&booke_wdt_miscdev);
 	if (ret) {
-		printk(KERN_CRIT "Cannot register miscdev on minor=%d: %d\n",
-				WATCHDOG_MINOR, ret);
+		pr_err("booke_wdt: cannot register device (minor=%u, ret=%i)\n",
+		       WATCHDOG_MINOR, ret);
 		return ret;
 	}
 
 	spin_lock(&booke_wdt_lock);
 	if (booke_wdt_enabled == 1) {
-		printk(KERN_INFO
-		      "PowerPC Book-E Watchdog Timer Enabled (wdt_period=%d)\n",
-				booke_wdt_period);
+		pr_info("booke_wdt: watchdog enabled (timeout = %llu sec)\n",
+			period_to_sec(booke_wdt_period));
 		on_each_cpu(__booke_wdt_enable, NULL, 0);
 	}
 	spin_unlock(&booke_wdt_lock);
diff --git a/drivers/watchdog/cpwd.c b/drivers/watchdog/cpwd.c
index eca855a55c0d..1e013e8457b7 100644
--- a/drivers/watchdog/cpwd.c
+++ b/drivers/watchdog/cpwd.c
@@ -5,10 +5,10 @@
  * interface and Solaris-compatible ioctls as best it is
  * able.
  *
- * NOTE: 	CP1400 systems appear to have a defective intr_mask
- * 			register on the PLD, preventing the disabling of
- * 			timer interrupts.  We use a timer to periodically
- * 			reset 'stopped' watchdogs on affected platforms.
+ * NOTE:	CP1400 systems appear to have a defective intr_mask
+ *			register on the PLD, preventing the disabling of
+ *			timer interrupts.  We use a timer to periodically
+ *			reset 'stopped' watchdogs on affected platforms.
  *
  * Copyright (c) 2000 Eric Brower (ebrower@usa.net)
  * Copyright (C) 2008 David S. Miller <davem@davemloft.net>
@@ -107,13 +107,13 @@ static struct cpwd *cpwd_device;
  * -------------------
  * |-  counter val  -|
  * -------------------
- * dcntr - 	Current 16-bit downcounter value.
- * 			When downcounter reaches '0' watchdog expires.
- * 			Reading this register resets downcounter with
- * 			'limit' value.
- * limit - 	16-bit countdown value in 1/10th second increments.
- * 			Writing this register begins countdown with input value.
- * 			Reading from this register does not affect counter.
+ * dcntr -	Current 16-bit downcounter value.
+ *			When downcounter reaches '0' watchdog expires.
+ *			Reading this register resets downcounter with
+ *			'limit' value.
+ * limit -	16-bit countdown value in 1/10th second increments.
+ *			Writing this register begins countdown with input value.
+ *			Reading from this register does not affect counter.
  * NOTES:	After watchdog reset, dcntr and limit contain '1'
  *
  * status register (byte access):
@@ -123,7 +123,7 @@ static struct cpwd *cpwd_device;
  * |-   UNUSED  -| EXP | RUN |
  * ---------------------------
  * status-	Bit 0 - Watchdog is running
- * 			Bit 1 - Watchdog has expired
+ *			Bit 1 - Watchdog has expired
  *
  *** PLD register block definition (struct wd_pld_regblk)
  *
@@ -197,7 +197,7 @@ static u8 cpwd_readb(void __iomem *addr)
  * Because of the CP1400 defect this should only be
  * called during initialzation or by wd_[start|stop]timer()
  *
- * index 	- sub-device index, or -1 for 'all'
+ * index	- sub-device index, or -1 for 'all'
  * enable	- non-zero to enable interrupts, zero to disable
  */
 static void cpwd_toggleintr(struct cpwd *p, int index, int enable)
@@ -317,13 +317,13 @@ static int cpwd_getstatus(struct cpwd *p, int index)
 		} else {
 			/* Fudge WD_EXPIRED status for defective CP1400--
 			 * IF timer is running
-			 * 	AND brokenstop is set
-			 * 	AND an interrupt has been serviced
+			 *	AND brokenstop is set
+			 *	AND an interrupt has been serviced
 			 * we are WD_EXPIRED.
 			 *
 			 * IF timer is running
-			 * 	AND brokenstop is set
-			 * 	AND no interrupt has been serviced
+			 *	AND brokenstop is set
+			 *	AND no interrupt has been serviced
 			 * we are WD_FREERUN.
 			 */
 			if (p->broken &&
@@ -528,8 +528,7 @@ static const struct file_operations cpwd_fops = {
 	.llseek =		no_llseek,
 };
 
-static int __devinit cpwd_probe(struct platform_device *op,
-				const struct of_device_id *match)
+static int __devinit cpwd_probe(struct platform_device *op)
 {
 	struct device_node *options;
 	const char *str_prop;
@@ -614,7 +613,7 @@ static int __devinit cpwd_probe(struct platform_device *op,
 
 	if (p->broken) {
 		init_timer(&cpwd_timer);
-		cpwd_timer.function 	= cpwd_brokentimer;
+		cpwd_timer.function	= cpwd_brokentimer;
 		cpwd_timer.data		= (unsigned long) p;
 		cpwd_timer.expires	= WD_BTIMEOUT;
 
@@ -646,7 +645,7 @@ static int __devexit cpwd_remove(struct platform_device *op)
 	struct cpwd *p = dev_get_drvdata(&op->dev);
 	int i;
 
-	for (i = 0; i < 4; i++) {
+	for (i = 0; i < WD_NUMDEVS; i++) {
 		misc_deregister(&p->devs[i].misc);
 
 		if (!p->enabled) {
@@ -678,7 +677,7 @@ static const struct of_device_id cpwd_match[] = {
 };
 MODULE_DEVICE_TABLE(of, cpwd_match);
 
-static struct of_platform_driver cpwd_driver = {
+static struct platform_driver cpwd_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -690,12 +689,12 @@ static struct of_platform_driver cpwd_driver = {
 
 static int __init cpwd_init(void)
 {
-	return of_register_platform_driver(&cpwd_driver);
+	return platform_driver_register(&cpwd_driver);
 }
 
 static void __exit cpwd_exit(void)
 {
-	of_unregister_platform_driver(&cpwd_driver);
+	platform_driver_unregister(&cpwd_driver);
 }
 
 module_init(cpwd_init);
diff --git a/drivers/watchdog/eurotechwdt.c b/drivers/watchdog/eurotechwdt.c
index 3f3dc093ad68..f1d1da662fbe 100644
--- a/drivers/watchdog/eurotechwdt.c
+++ b/drivers/watchdog/eurotechwdt.c
@@ -201,7 +201,7 @@ static void eurwdt_ping(void)
 static ssize_t eurwdt_write(struct file *file, const char __user *buf,
 size_t count, loff_t *ppos)
 {
-	if (count) 	{
+	if (count) {
 		if (!nowayout) {
 			size_t i;
 
diff --git a/drivers/watchdog/gef_wdt.c b/drivers/watchdog/gef_wdt.c
index f6bd6f10fcec..29a7cd4b90c8 100644
--- a/drivers/watchdog/gef_wdt.c
+++ b/drivers/watchdog/gef_wdt.c
@@ -261,8 +261,7 @@ static struct miscdevice gef_wdt_miscdev = {
 };
 
 
-static int __devinit gef_wdt_probe(struct platform_device *dev,
-	const struct of_device_id *match)
+static int __devinit gef_wdt_probe(struct platform_device *dev)
 {
 	int timeout = 10;
 	u32 freq;
@@ -303,7 +302,7 @@ static const struct of_device_id gef_wdt_ids[] = {
 	{},
 };
 
-static struct of_platform_driver gef_wdt_driver = {
+static struct platform_driver gef_wdt_driver = {
 	.driver = {
 		.name = "gef_wdt",
 		.owner = THIS_MODULE,
@@ -315,12 +314,12 @@ static struct of_platform_driver gef_wdt_driver = {
 static int __init gef_wdt_init(void)
 {
 	printk(KERN_INFO "GE watchdog driver\n");
-	return of_register_platform_driver(&gef_wdt_driver);
+	return platform_driver_register(&gef_wdt_driver);
 }
 
 static void __exit gef_wdt_exit(void)
 {
-	of_unregister_platform_driver(&gef_wdt_driver);
+	platform_driver_unregister(&gef_wdt_driver);
 }
 
 module_init(gef_wdt_init);
diff --git a/drivers/watchdog/hpwdt.c b/drivers/watchdog/hpwdt.c
index 24b966d5061a..8cb26855bfed 100644
--- a/drivers/watchdog/hpwdt.c
+++ b/drivers/watchdog/hpwdt.c
@@ -52,7 +52,7 @@ static void __iomem *pci_mem_addr;		/* the PCI-memory address */
 static unsigned long __iomem *hpwdt_timer_reg;
 static unsigned long __iomem *hpwdt_timer_con;
 
-static struct pci_device_id hpwdt_devices[] = {
+static DEFINE_PCI_DEVICE_TABLE(hpwdt_devices) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_COMPAQ, 0xB203) },	/* iLO2 */
 	{ PCI_DEVICE(PCI_VENDOR_ID_HP, 0x3306) },	/* iLO3 */
 	{0},			/* terminate list */
@@ -710,7 +710,7 @@ static int __devinit hpwdt_init_nmi_decoding(struct pci_dev *dev)
 	return 0;
 }
 
-static void __devexit hpwdt_exit_nmi_decoding(void)
+static void hpwdt_exit_nmi_decoding(void)
 {
 	unregister_die_notifier(&die_notifier);
 	if (cru_rom_addr)
@@ -726,7 +726,7 @@ static int __devinit hpwdt_init_nmi_decoding(struct pci_dev *dev)
 	return 0;
 }
 
-static void __devexit hpwdt_exit_nmi_decoding(void)
+static void hpwdt_exit_nmi_decoding(void)
 {
 }
 #endif /* CONFIG_HPWDT_NMI_DECODING */
diff --git a/drivers/watchdog/i6300esb.c b/drivers/watchdog/i6300esb.c
index bb9750a03942..db45091ef434 100644
--- a/drivers/watchdog/i6300esb.c
+++ b/drivers/watchdog/i6300esb.c
@@ -334,7 +334,7 @@ static struct miscdevice esb_miscdev = {
 /*
  * Data for PCI driver interface
  */
-static struct pci_device_id esb_pci_tbl[] = {
+static DEFINE_PCI_DEVICE_TABLE(esb_pci_tbl) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_ESB_9), },
 	{ 0, },                 /* End of list */
 };
diff --git a/drivers/watchdog/iTCO_wdt.c b/drivers/watchdog/iTCO_wdt.c
index 2c6c2b4ad8bf..35a0d12dad73 100644
--- a/drivers/watchdog/iTCO_wdt.c
+++ b/drivers/watchdog/iTCO_wdt.c
@@ -247,7 +247,7 @@ static struct {
 	{NULL, 0}
 };
 
-#define ITCO_PCI_DEVICE(dev, data) 	\
+#define ITCO_PCI_DEVICE(dev, data) \
 	.vendor = PCI_VENDOR_ID_INTEL,	\
 	.device = dev,			\
 	.subvendor = PCI_ANY_ID,	\
@@ -262,7 +262,7 @@ static struct {
  * pci_driver, because the I/O Controller Hub has also other
  * functions that probably will be registered by other drivers.
  */
-static struct pci_device_id iTCO_wdt_pci_tbl[] = {
+static DEFINE_PCI_DEVICE_TABLE(iTCO_wdt_pci_tbl) = {
 	{ ITCO_PCI_DEVICE(PCI_DEVICE_ID_INTEL_82801AA_0,	TCO_ICH)},
 	{ ITCO_PCI_DEVICE(PCI_DEVICE_ID_INTEL_82801AB_0,	TCO_ICH0)},
 	{ ITCO_PCI_DEVICE(PCI_DEVICE_ID_INTEL_82801BA_0,	TCO_ICH2)},
diff --git a/drivers/watchdog/intel_scu_watchdog.c b/drivers/watchdog/intel_scu_watchdog.c
new file mode 100644
index 000000000000..919bdd16136f
--- /dev/null
+++ b/drivers/watchdog/intel_scu_watchdog.c
@@ -0,0 +1,572 @@
+/*
+ *      Intel_SCU 0.2:  An Intel SCU IOH Based Watchdog Device
+ *			for Intel part #(s):
+ *				- AF82MP20 PCH
+ *
+ *      Copyright (C) 2009-2010 Intel Corporation. All rights reserved.
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of version 2 of the GNU General
+ *      Public License as published by the Free Software Foundation.
+ *
+ *      This program is distributed in the hope that it will be
+ *      useful, but WITHOUT ANY WARRANTY; without even the implied
+ *      warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ *      PURPOSE.  See the GNU General Public License for more details.
+ *      You should have received a copy of the GNU General Public
+ *      License along with this program; if not, write to the Free
+ *      Software Foundation, Inc., 59 Temple Place - Suite 330,
+ *      Boston, MA  02111-1307, USA.
+ *      The full GNU General Public License is included in this
+ *      distribution in the file called COPYING.
+ *
+ */
+
+#include <linux/compiler.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/miscdevice.h>
+#include <linux/watchdog.h>
+#include <linux/fs.h>
+#include <linux/notifier.h>
+#include <linux/reboot.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/signal.h>
+#include <linux/sfi.h>
+#include <linux/types.h>
+#include <asm/irq.h>
+#include <asm/atomic.h>
+#include <asm/intel_scu_ipc.h>
+#include <asm/apb_timer.h>
+#include <asm/mrst.h>
+
+#include "intel_scu_watchdog.h"
+
+/* Bounds number of times we will retry loading time count */
+/* This retry is a work around for a silicon bug.	   */
+#define MAX_RETRY 16
+
+#define IPC_SET_WATCHDOG_TIMER	0xF8
+
+static int timer_margin = DEFAULT_SOFT_TO_HARD_MARGIN;
+module_param(timer_margin, int, 0);
+MODULE_PARM_DESC(timer_margin,
+		"Watchdog timer margin"
+		"Time between interrupt and resetting the system"
+		"The range is from 1 to 160"
+		"This is the time for all keep alives to arrive");
+
+static int timer_set = DEFAULT_TIME;
+module_param(timer_set, int, 0);
+MODULE_PARM_DESC(timer_set,
+		"Default Watchdog timer setting"
+		"Complete cycle time"
+		"The range is from 1 to 170"
+		"This is the time for all keep alives to arrive");
+
+/* After watchdog device is closed, check force_boot. If:
+ * force_boot == 0, then force boot on next watchdog interrupt after close,
+ * force_boot == 1, then force boot immediately when device is closed.
+ */
+static int force_boot;
+module_param(force_boot, int, 0);
+MODULE_PARM_DESC(force_boot,
+		"A value of 1 means that the driver will reboot"
+		"the system immediately if the /dev/watchdog device is closed"
+		"A value of 0 means that when /dev/watchdog device is closed"
+		"the watchdog timer will be refreshed for one more interval"
+		"of length: timer_set. At the end of this interval, the"
+		"watchdog timer will reset the system."
+		);
+
+/* there is only one device in the system now; this can be made into
+ * an array in the future if we have more than one device */
+
+static struct intel_scu_watchdog_dev watchdog_device;
+
+/* Forces restart, if force_reboot is set */
+static void watchdog_fire(void)
+{
+	if (force_boot) {
+		printk(KERN_CRIT PFX "Initiating system reboot.\n");
+		emergency_restart();
+		printk(KERN_CRIT PFX "Reboot didn't ?????\n");
+	}
+
+	else {
+		printk(KERN_CRIT PFX "Immediate Reboot Disabled\n");
+		printk(KERN_CRIT PFX
+			"System will reset when watchdog timer times out!\n");
+	}
+}
+
+static int check_timer_margin(int new_margin)
+{
+	if ((new_margin < MIN_TIME_CYCLE) ||
+	    (new_margin > MAX_TIME - timer_set)) {
+		pr_debug("Watchdog timer: value of new_margin %d is out of the range %d to %d\n",
+			  new_margin, MIN_TIME_CYCLE, MAX_TIME - timer_set);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * IPC operations
+ */
+static int watchdog_set_ipc(int soft_threshold, int threshold)
+{
+	u32	*ipc_wbuf;
+	u8	 cbuf[16] = { '\0' };
+	int	 ipc_ret = 0;
+
+	ipc_wbuf = (u32 *)&cbuf;
+	ipc_wbuf[0] = soft_threshold;
+	ipc_wbuf[1] = threshold;
+
+	ipc_ret = intel_scu_ipc_command(
+			IPC_SET_WATCHDOG_TIMER,
+			0,
+			ipc_wbuf,
+			2,
+			NULL,
+			0);
+
+	if (ipc_ret != 0)
+		pr_err("Error setting SCU watchdog timer: %x\n", ipc_ret);
+
+	return ipc_ret;
+};
+
+/*
+ *      Intel_SCU operations
+ */
+
+/* timer interrupt handler */
+static irqreturn_t watchdog_timer_interrupt(int irq, void *dev_id)
+{
+	int int_status;
+	int_status = ioread32(watchdog_device.timer_interrupt_status_addr);
+
+	pr_debug("Watchdog timer: irq, int_status: %x\n", int_status);
+
+	if (int_status != 0)
+		return IRQ_NONE;
+
+	/* has the timer been started? If not, then this is spurious */
+	if (watchdog_device.timer_started == 0) {
+		pr_debug("Watchdog timer: spurious interrupt received\n");
+		return IRQ_HANDLED;
+	}
+
+	/* temporarily disable the timer */
+	iowrite32(0x00000002, watchdog_device.timer_control_addr);
+
+	/* set the timer to the threshold */
+	iowrite32(watchdog_device.threshold,
+		  watchdog_device.timer_load_count_addr);
+
+	/* allow the timer to run */
+	iowrite32(0x00000003, watchdog_device.timer_control_addr);
+
+	return IRQ_HANDLED;
+}
+
+static int intel_scu_keepalive(void)
+{
+
+	/* read eoi register - clears interrupt */
+	ioread32(watchdog_device.timer_clear_interrupt_addr);
+
+	/* temporarily disable the timer */
+	iowrite32(0x00000002, watchdog_device.timer_control_addr);
+
+	/* set the timer to the soft_threshold */
+	iowrite32(watchdog_device.soft_threshold,
+		  watchdog_device.timer_load_count_addr);
+
+	/* allow the timer to run */
+	iowrite32(0x00000003, watchdog_device.timer_control_addr);
+
+	return 0;
+}
+
+static int intel_scu_stop(void)
+{
+	iowrite32(0, watchdog_device.timer_control_addr);
+	return 0;
+}
+
+static int intel_scu_set_heartbeat(u32 t)
+{
+	int			 ipc_ret;
+	int			 retry_count;
+	u32			 soft_value;
+	u32			 hw_pre_value;
+	u32			 hw_value;
+
+	watchdog_device.timer_set = t;
+	watchdog_device.threshold =
+		timer_margin * watchdog_device.timer_tbl_ptr->freq_hz;
+	watchdog_device.soft_threshold =
+		(watchdog_device.timer_set - timer_margin)
+		* watchdog_device.timer_tbl_ptr->freq_hz;
+
+	pr_debug("Watchdog timer: set_heartbeat: timer freq is %d\n",
+		watchdog_device.timer_tbl_ptr->freq_hz);
+	pr_debug("Watchdog timer: set_heartbeat: timer_set is %x (hex)\n",
+		watchdog_device.timer_set);
+	pr_debug("Watchdog timer: set_hearbeat: timer_margin is %x (hex)\n",
+		timer_margin);
+	pr_debug("Watchdog timer: set_heartbeat: threshold is %x (hex)\n",
+		watchdog_device.threshold);
+	pr_debug("Watchdog timer: set_heartbeat: soft_threshold is %x (hex)\n",
+		watchdog_device.soft_threshold);
+
+	/* Adjust thresholds by FREQ_ADJUSTMENT factor, to make the */
+	/* watchdog timing come out right. */
+	watchdog_device.threshold =
+		watchdog_device.threshold / FREQ_ADJUSTMENT;
+	watchdog_device.soft_threshold =
+		watchdog_device.soft_threshold / FREQ_ADJUSTMENT;
+
+	/* temporarily disable the timer */
+	iowrite32(0x00000002, watchdog_device.timer_control_addr);
+
+	/* send the threshold and soft_threshold via IPC to the processor */
+	ipc_ret = watchdog_set_ipc(watchdog_device.soft_threshold,
+				   watchdog_device.threshold);
+
+	if (ipc_ret != 0) {
+		/* Make sure the watchdog timer is stopped */
+		intel_scu_stop();
+		return ipc_ret;
+	}
+
+	/* Soft Threshold set loop. Early versions of silicon did */
+	/* not always set this count correctly.  This loop checks */
+	/* the value and retries if it was not set correctly.     */
+
+	retry_count = 0;
+	soft_value = watchdog_device.soft_threshold & 0xFFFF0000;
+	do {
+
+		/* Make sure timer is stopped */
+		intel_scu_stop();
+
+		if (MAX_RETRY < retry_count++) {
+			/* Unable to set timer value */
+			pr_err("Watchdog timer: Unable to set timer\n");
+			return -ENODEV;
+		}
+
+		/* set the timer to the soft threshold */
+		iowrite32(watchdog_device.soft_threshold,
+			watchdog_device.timer_load_count_addr);
+
+		/* read count value before starting timer */
+		hw_pre_value = ioread32(watchdog_device.timer_load_count_addr);
+		hw_pre_value = hw_pre_value & 0xFFFF0000;
+
+		/* Start the timer */
+		iowrite32(0x00000003, watchdog_device.timer_control_addr);
+
+		/* read the value the time loaded into its count reg */
+		hw_value = ioread32(watchdog_device.timer_load_count_addr);
+		hw_value = hw_value & 0xFFFF0000;
+
+
+	} while (soft_value != hw_value);
+
+	watchdog_device.timer_started = 1;
+
+	return 0;
+}
+
+/*
+ * /dev/watchdog handling
+ */
+
+static int intel_scu_open(struct inode *inode, struct file *file)
+{
+
+	/* Set flag to indicate that watchdog device is open */
+	if (test_and_set_bit(0, &watchdog_device.driver_open))
+		return -EBUSY;
+
+	/* Check for reopen of driver. Reopens are not allowed */
+	if (watchdog_device.driver_closed)
+		return -EPERM;
+
+	return nonseekable_open(inode, file);
+}
+
+static int intel_scu_release(struct inode *inode, struct file *file)
+{
+	/*
+	 * This watchdog should not be closed, after the timer
+	 * is started with the WDIPC_SETTIMEOUT ioctl
+	 * If force_boot is set watchdog_fire() will cause an
+	 * immediate reset. If force_boot is not set, the watchdog
+	 * timer is refreshed for one more interval. At the end
+	 * of that interval, the watchdog timer will reset the system.
+	 */
+
+	if (!test_and_clear_bit(0, &watchdog_device.driver_open)) {
+		pr_debug("Watchdog timer: intel_scu_release, without open\n");
+		return -ENOTTY;
+	}
+
+	if (!watchdog_device.timer_started) {
+		/* Just close, since timer has not been started */
+		pr_debug("Watchdog timer: closed, without starting timer\n");
+		return 0;
+	}
+
+	printk(KERN_CRIT PFX
+	       "Unexpected close of /dev/watchdog!\n");
+
+	/* Since the timer was started, prevent future reopens */
+	watchdog_device.driver_closed = 1;
+
+	/* Refresh the timer for one more interval */
+	intel_scu_keepalive();
+
+	/* Reboot system (if force_boot is set) */
+	watchdog_fire();
+
+	/* We should only reach this point if force_boot is not set */
+	return 0;
+}
+
+static ssize_t intel_scu_write(struct file *file,
+			      char const *data,
+			      size_t len,
+			      loff_t *ppos)
+{
+
+	if (watchdog_device.timer_started)
+		/* Watchdog already started, keep it alive */
+		intel_scu_keepalive();
+	else
+		/* Start watchdog with timer value set by init */
+		intel_scu_set_heartbeat(watchdog_device.timer_set);
+
+	return len;
+}
+
+static long intel_scu_ioctl(struct file *file,
+			   unsigned int cmd,
+			   unsigned long arg)
+{
+	void __user *argp = (void __user *)arg;
+	u32 __user *p = argp;
+	u32 new_margin;
+
+
+	static const struct watchdog_info ident = {
+		.options =          WDIOF_SETTIMEOUT
+				    | WDIOF_KEEPALIVEPING,
+		.firmware_version = 0,  /* @todo Get from SCU via
+						 ipc_get_scu_fw_version()? */
+		.identity =         "Intel_SCU IOH Watchdog"  /* len < 32 */
+	};
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		return copy_to_user(argp,
+				    &ident,
+				    sizeof(ident)) ? -EFAULT : 0;
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		return put_user(0, p);
+	case WDIOC_KEEPALIVE:
+		intel_scu_keepalive();
+
+		return 0;
+	case WDIOC_SETTIMEOUT:
+		if (get_user(new_margin, p))
+			return -EFAULT;
+
+		if (check_timer_margin(new_margin))
+			return -EINVAL;
+
+		if (intel_scu_set_heartbeat(new_margin))
+			return -EINVAL;
+		return 0;
+	case WDIOC_GETTIMEOUT:
+		return put_user(watchdog_device.soft_threshold, p);
+
+	default:
+		return -ENOTTY;
+	}
+}
+
+/*
+ *      Notifier for system down
+ */
+static int intel_scu_notify_sys(struct notifier_block *this,
+			       unsigned long code,
+			       void *another_unused)
+{
+	if (code == SYS_DOWN || code == SYS_HALT)
+		/* Turn off the watchdog timer. */
+		intel_scu_stop();
+	return NOTIFY_DONE;
+}
+
+/*
+ *      Kernel Interfaces
+ */
+static const struct file_operations intel_scu_fops = {
+	.owner          = THIS_MODULE,
+	.llseek         = no_llseek,
+	.write          = intel_scu_write,
+	.unlocked_ioctl = intel_scu_ioctl,
+	.open           = intel_scu_open,
+	.release        = intel_scu_release,
+};
+
+static int __init intel_scu_watchdog_init(void)
+{
+	int ret;
+	u32 __iomem *tmp_addr;
+
+	/*
+	 * We don't really need to check this as the SFI timer get will fail
+	 * but if we do so we can exit with a clearer reason and no noise.
+	 *
+	 * If it isn't an intel MID device then it doesn't have this watchdog
+	 */
+	if (!mrst_identify_cpu())
+		return -ENODEV;
+
+	/* Check boot parameters to verify that their initial values */
+	/* are in range. */
+	/* Check value of timer_set boot parameter */
+	if ((timer_set < MIN_TIME_CYCLE) ||
+	    (timer_set > MAX_TIME - MIN_TIME_CYCLE)) {
+		pr_err("Watchdog timer: value of timer_set %x (hex) "
+		  "is out of range from %x to %x (hex)\n",
+		  timer_set, MIN_TIME_CYCLE, MAX_TIME - MIN_TIME_CYCLE);
+		return -EINVAL;
+	}
+
+	/* Check value of timer_margin boot parameter */
+	if (check_timer_margin(timer_margin))
+		return -EINVAL;
+
+	watchdog_device.timer_tbl_ptr = sfi_get_mtmr(sfi_mtimer_num-1);
+
+	if (watchdog_device.timer_tbl_ptr == NULL) {
+		pr_debug("Watchdog timer - Intel SCU watchdog: timer is not available\n");
+		return -ENODEV;
+	}
+	/* make sure the timer exists */
+	if (watchdog_device.timer_tbl_ptr->phys_addr == 0) {
+		pr_debug("Watchdog timer - Intel SCU watchdog - timer %d does not have valid physical memory\n",
+								sfi_mtimer_num);
+		return -ENODEV;
+	}
+
+	if (watchdog_device.timer_tbl_ptr->irq == 0) {
+		pr_debug("Watchdog timer: timer %d invalid irq\n",
+							sfi_mtimer_num);
+		return -ENODEV;
+	}
+
+	tmp_addr = ioremap_nocache(watchdog_device.timer_tbl_ptr->phys_addr,
+			20);
+
+	if (tmp_addr == NULL) {
+		pr_debug("Watchdog timer: timer unable to ioremap\n");
+		return -ENOMEM;
+	}
+
+	watchdog_device.timer_load_count_addr = tmp_addr++;
+	watchdog_device.timer_current_value_addr = tmp_addr++;
+	watchdog_device.timer_control_addr = tmp_addr++;
+	watchdog_device.timer_clear_interrupt_addr = tmp_addr++;
+	watchdog_device.timer_interrupt_status_addr = tmp_addr++;
+
+	/* Set the default time values in device structure */
+
+	watchdog_device.timer_set = timer_set;
+	watchdog_device.threshold =
+		timer_margin * watchdog_device.timer_tbl_ptr->freq_hz;
+	watchdog_device.soft_threshold =
+		(watchdog_device.timer_set - timer_margin)
+		* watchdog_device.timer_tbl_ptr->freq_hz;
+
+
+	watchdog_device.intel_scu_notifier.notifier_call =
+		intel_scu_notify_sys;
+
+	ret = register_reboot_notifier(&watchdog_device.intel_scu_notifier);
+	if (ret) {
+		pr_err("Watchdog timer: cannot register notifier %d)\n", ret);
+		goto register_reboot_error;
+	}
+
+	watchdog_device.miscdev.minor = WATCHDOG_MINOR;
+	watchdog_device.miscdev.name = "watchdog";
+	watchdog_device.miscdev.fops = &intel_scu_fops;
+
+	ret = misc_register(&watchdog_device.miscdev);
+	if (ret) {
+		pr_err("Watchdog timer: cannot register miscdev %d err =%d\n",
+							WATCHDOG_MINOR, ret);
+		goto misc_register_error;
+	}
+
+	ret = request_irq((unsigned int)watchdog_device.timer_tbl_ptr->irq,
+		watchdog_timer_interrupt,
+		IRQF_SHARED, "watchdog",
+		&watchdog_device.timer_load_count_addr);
+	if (ret) {
+		pr_err("Watchdog timer: error requesting irq %d\n", ret);
+		goto request_irq_error;
+	}
+	/* Make sure timer is disabled before returning */
+	intel_scu_stop();
+	return 0;
+
+/* error cleanup */
+
+request_irq_error:
+	misc_deregister(&watchdog_device.miscdev);
+misc_register_error:
+	unregister_reboot_notifier(&watchdog_device.intel_scu_notifier);
+register_reboot_error:
+	intel_scu_stop();
+	iounmap(watchdog_device.timer_load_count_addr);
+	return ret;
+}
+
+static void __exit intel_scu_watchdog_exit(void)
+{
+
+	misc_deregister(&watchdog_device.miscdev);
+	unregister_reboot_notifier(&watchdog_device.intel_scu_notifier);
+	/* disable the timer */
+	iowrite32(0x00000002, watchdog_device.timer_control_addr);
+	iounmap(watchdog_device.timer_load_count_addr);
+}
+
+late_initcall(intel_scu_watchdog_init);
+module_exit(intel_scu_watchdog_exit);
+
+MODULE_AUTHOR("Intel Corporation");
+MODULE_DESCRIPTION("Intel SCU Watchdog Device Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
+MODULE_VERSION(WDT_VER);
diff --git a/drivers/watchdog/intel_scu_watchdog.h b/drivers/watchdog/intel_scu_watchdog.h
new file mode 100644
index 000000000000..d2b074a82db6
--- /dev/null
+++ b/drivers/watchdog/intel_scu_watchdog.h
@@ -0,0 +1,66 @@
+/*
+ *      Intel_SCU 0.2:  An Intel SCU IOH Based Watchdog Device
+ *			for Intel part #(s):
+ *				- AF82MP20 PCH
+ *
+ *      Copyright (C) 2009-2010 Intel Corporation. All rights reserved.
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of version 2 of the GNU General
+ *      Public License as published by the Free Software Foundation.
+ *
+ *      This program is distributed in the hope that it will be
+ *      useful, but WITHOUT ANY WARRANTY; without even the implied
+ *      warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ *      PURPOSE.  See the GNU General Public License for more details.
+ *      You should have received a copy of the GNU General Public
+ *      License along with this program; if not, write to the Free
+ *      Software Foundation, Inc., 59 Temple Place - Suite 330,
+ *      Boston, MA  02111-1307, USA.
+ *      The full GNU General Public License is included in this
+ *      distribution in the file called COPYING.
+ *
+ */
+
+#ifndef __INTEL_SCU_WATCHDOG_H
+#define __INTEL_SCU_WATCHDOG_H
+
+#define PFX "Intel_SCU: "
+#define WDT_VER "0.3"
+
+/* minimum time between interrupts */
+#define MIN_TIME_CYCLE 1
+
+/* Time from warning to reboot is 2 seconds */
+#define DEFAULT_SOFT_TO_HARD_MARGIN 2
+
+#define MAX_TIME 170
+
+#define DEFAULT_TIME 5
+
+#define MAX_SOFT_TO_HARD_MARGIN (MAX_TIME-MIN_TIME_CYCLE)
+
+/* Ajustment to clock tick frequency to make timing come out right */
+#define FREQ_ADJUSTMENT 8
+
+struct intel_scu_watchdog_dev {
+	ulong driver_open;
+	ulong driver_closed;
+	u32 timer_started;
+	u32 timer_set;
+	u32 threshold;
+	u32 soft_threshold;
+	u32 __iomem *timer_load_count_addr;
+	u32 __iomem *timer_current_value_addr;
+	u32 __iomem *timer_control_addr;
+	u32 __iomem *timer_clear_interrupt_addr;
+	u32 __iomem *timer_interrupt_status_addr;
+	struct sfi_timer_table_entry *timer_tbl_ptr;
+	struct notifier_block intel_scu_notifier;
+	struct miscdevice miscdev;
+};
+
+extern int sfi_mtimer_num;
+
+/* extern struct sfi_timer_table_entry *sfi_get_mtmr(int hint); */
+#endif /* __INTEL_SCU_WATCHDOG_H */
diff --git a/drivers/watchdog/it8712f_wdt.c b/drivers/watchdog/it8712f_wdt.c
index b32c6c045b1a..6143f52ba6b8 100644
--- a/drivers/watchdog/it8712f_wdt.c
+++ b/drivers/watchdog/it8712f_wdt.c
@@ -69,7 +69,7 @@ static unsigned short address;
 #define IT8712F_DEVID	0x8712
 
 #define LDN_GPIO	0x07	/* GPIO and Watch Dog Timer */
-#define LDN_GAME 	0x09	/* Game Port */
+#define LDN_GAME	0x09	/* Game Port */
 
 #define WDT_CONTROL	0x71	/* WDT Register: Control */
 #define WDT_CONFIG	0x72	/* WDT Register: Configuration */
diff --git a/drivers/watchdog/it87_wdt.c b/drivers/watchdog/it87_wdt.c
index dad29245a6a7..b1bc72f9a209 100644
--- a/drivers/watchdog/it87_wdt.c
+++ b/drivers/watchdog/it87_wdt.c
@@ -12,7 +12,7 @@
  *		    http://www.ite.com.tw/
  *
  *	Support of the watchdog timers, which are available on
- *	IT8702, IT8712, IT8716, IT8718, IT8720 and IT8726.
+ *	IT8702, IT8712, IT8716, IT8718, IT8720, IT8721 and IT8726.
  *
  *	This program is free software; you can redistribute it and/or
  *	modify it under the terms of the GNU General Public License
@@ -45,7 +45,7 @@
 
 #include <asm/system.h>
 
-#define WATCHDOG_VERSION	"1.13"
+#define WATCHDOG_VERSION	"1.14"
 #define WATCHDOG_NAME		"IT87 WDT"
 #define PFX			WATCHDOG_NAME ": "
 #define DRIVER_VERSION		WATCHDOG_NAME " driver, v" WATCHDOG_VERSION "\n"
@@ -54,7 +54,7 @@
 /* Defaults for Module Parameter */
 #define DEFAULT_NOGAMEPORT	0
 #define DEFAULT_EXCLUSIVE	1
-#define DEFAULT_TIMEOUT 	60
+#define DEFAULT_TIMEOUT		60
 #define DEFAULT_TESTMODE	0
 #define DEFAULT_NOWAYOUT	WATCHDOG_NOWAYOUT
 
@@ -70,9 +70,9 @@
 /* Configuration Registers and Functions */
 #define LDNREG		0x07
 #define CHIPID		0x20
-#define CHIPREV 	0x22
+#define CHIPREV		0x22
 #define ACTREG		0x30
-#define BASEREG 	0x60
+#define BASEREG		0x60
 
 /* Chip Id numbers */
 #define NO_DEV_ID	0xffff
@@ -82,10 +82,11 @@
 #define IT8716_ID	0x8716
 #define IT8718_ID	0x8718
 #define IT8720_ID	0x8720
+#define IT8721_ID	0x8721
 #define IT8726_ID	0x8726	/* the data sheet suggest wrongly 0x8716 */
 
 /* GPIO Configuration Registers LDN=0x07 */
-#define WDTCTRL 	0x71
+#define WDTCTRL		0x71
 #define WDTCFG		0x72
 #define WDTVALLSB	0x73
 #define WDTVALMSB	0x74
@@ -94,7 +95,7 @@
 #define WDT_CIRINT	0x80
 #define WDT_MOUSEINT	0x40
 #define WDT_KYBINT	0x20
-#define WDT_GAMEPORT	0x10 /* not in it8718, it8720 */
+#define WDT_GAMEPORT	0x10 /* not in it8718, it8720, it8721 */
 #define WDT_FORCE	0x02
 #define WDT_ZERO	0x01
 
@@ -102,11 +103,11 @@
 #define WDT_TOV1	0x80
 #define WDT_KRST	0x40
 #define WDT_TOVE	0x20
-#define WDT_PWROK	0x10
+#define WDT_PWROK	0x10 /* not in it8721 */
 #define WDT_INT_MASK	0x0f
 
 /* CIR Configuration Register LDN=0x0a */
-#define CIR_ILS 	0x70
+#define CIR_ILS		0x70
 
 /* The default Base address is not always available, we use this */
 #define CIR_BASE	0x0208
@@ -134,7 +135,7 @@
 #define WDTS_USE_GP	4
 #define WDTS_EXPECTED	5
 
-static	unsigned int base, gpact, ciract, max_units;
+static	unsigned int base, gpact, ciract, max_units, chip_type;
 static	unsigned long wdt_status;
 static	DEFINE_SPINLOCK(spinlock);
 
@@ -215,7 +216,7 @@ static inline void superio_outw(int val, int reg)
 /* Internal function, should be called after superio_select(GPIO) */
 static void wdt_update_timeout(void)
 {
-	unsigned char cfg = WDT_KRST | WDT_PWROK;
+	unsigned char cfg = WDT_KRST;
 	int tm = timeout;
 
 	if (testmode)
@@ -226,6 +227,9 @@ static void wdt_update_timeout(void)
 	else
 		tm /= 60;
 
+	if (chip_type != IT8721_ID)
+		cfg |= WDT_PWROK;
+
 	superio_outb(cfg, WDTCFG);
 	superio_outb(tm, WDTVALLSB);
 	if (max_units > 255)
@@ -555,7 +559,6 @@ static int __init it87_wdt_init(void)
 {
 	int rc = 0;
 	int try_gameport = !nogameport;
-	u16 chip_type;
 	u8  chip_rev;
 	unsigned long flags;
 
@@ -581,6 +584,7 @@ static int __init it87_wdt_init(void)
 		break;
 	case IT8718_ID:
 	case IT8720_ID:
+	case IT8721_ID:
 		max_units = 65535;
 		try_gameport = 0;
 		break;
diff --git a/drivers/watchdog/jz4740_wdt.c b/drivers/watchdog/jz4740_wdt.c
new file mode 100644
index 000000000000..684ba01fb540
--- /dev/null
+++ b/drivers/watchdog/jz4740_wdt.c
@@ -0,0 +1,322 @@
+/*
+ *  Copyright (C) 2010, Paul Cercueil <paul@crapouillou.net>
+ *  JZ4740 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.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/watchdog.h>
+#include <linux/init.h>
+#include <linux/bitops.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+
+#include <asm/mach-jz4740/timer.h>
+
+#define JZ_REG_WDT_TIMER_DATA     0x0
+#define JZ_REG_WDT_COUNTER_ENABLE 0x4
+#define JZ_REG_WDT_TIMER_COUNTER  0x8
+#define JZ_REG_WDT_TIMER_CONTROL  0xC
+
+#define JZ_WDT_CLOCK_PCLK 0x1
+#define JZ_WDT_CLOCK_RTC  0x2
+#define JZ_WDT_CLOCK_EXT  0x4
+
+#define WDT_IN_USE        0
+#define WDT_OK_TO_CLOSE   1
+
+#define JZ_WDT_CLOCK_DIV_SHIFT   3
+
+#define JZ_WDT_CLOCK_DIV_1    (0 << JZ_WDT_CLOCK_DIV_SHIFT)
+#define JZ_WDT_CLOCK_DIV_4    (1 << JZ_WDT_CLOCK_DIV_SHIFT)
+#define JZ_WDT_CLOCK_DIV_16   (2 << JZ_WDT_CLOCK_DIV_SHIFT)
+#define JZ_WDT_CLOCK_DIV_64   (3 << JZ_WDT_CLOCK_DIV_SHIFT)
+#define JZ_WDT_CLOCK_DIV_256  (4 << JZ_WDT_CLOCK_DIV_SHIFT)
+#define JZ_WDT_CLOCK_DIV_1024 (5 << JZ_WDT_CLOCK_DIV_SHIFT)
+
+#define DEFAULT_HEARTBEAT 5
+#define MAX_HEARTBEAT     2048
+
+static struct {
+	void __iomem *base;
+	struct resource	*mem;
+	struct clk *rtc_clk;
+	unsigned long status;
+} jz4740_wdt;
+
+static int heartbeat = DEFAULT_HEARTBEAT;
+
+
+static void jz4740_wdt_service(void)
+{
+	writew(0x0, jz4740_wdt.base + JZ_REG_WDT_TIMER_COUNTER);
+}
+
+static void jz4740_wdt_set_heartbeat(int new_heartbeat)
+{
+	unsigned int rtc_clk_rate;
+	unsigned int timeout_value;
+	unsigned short clock_div = JZ_WDT_CLOCK_DIV_1;
+
+	heartbeat = new_heartbeat;
+
+	rtc_clk_rate = clk_get_rate(jz4740_wdt.rtc_clk);
+
+	timeout_value = rtc_clk_rate * heartbeat;
+	while (timeout_value > 0xffff) {
+		if (clock_div == JZ_WDT_CLOCK_DIV_1024) {
+			/* Requested timeout too high;
+			* use highest possible value. */
+			timeout_value = 0xffff;
+			break;
+		}
+		timeout_value >>= 2;
+		clock_div += (1 << JZ_WDT_CLOCK_DIV_SHIFT);
+	}
+
+	writeb(0x0, jz4740_wdt.base + JZ_REG_WDT_COUNTER_ENABLE);
+	writew(clock_div, jz4740_wdt.base + JZ_REG_WDT_TIMER_CONTROL);
+
+	writew((u16)timeout_value, jz4740_wdt.base + JZ_REG_WDT_TIMER_DATA);
+	writew(0x0, jz4740_wdt.base + JZ_REG_WDT_TIMER_COUNTER);
+	writew(clock_div | JZ_WDT_CLOCK_RTC,
+		jz4740_wdt.base + JZ_REG_WDT_TIMER_CONTROL);
+
+	writeb(0x1, jz4740_wdt.base + JZ_REG_WDT_COUNTER_ENABLE);
+}
+
+static void jz4740_wdt_enable(void)
+{
+	jz4740_timer_enable_watchdog();
+	jz4740_wdt_set_heartbeat(heartbeat);
+}
+
+static void jz4740_wdt_disable(void)
+{
+	jz4740_timer_disable_watchdog();
+	writeb(0x0, jz4740_wdt.base + JZ_REG_WDT_COUNTER_ENABLE);
+}
+
+static int jz4740_wdt_open(struct inode *inode, struct file *file)
+{
+	if (test_and_set_bit(WDT_IN_USE, &jz4740_wdt.status))
+		return -EBUSY;
+
+	jz4740_wdt_enable();
+
+	return nonseekable_open(inode, file);
+}
+
+static ssize_t jz4740_wdt_write(struct file *file, const char *data,
+		size_t len, loff_t *ppos)
+{
+	if (len) {
+		size_t i;
+
+		clear_bit(WDT_OK_TO_CLOSE, &jz4740_wdt.status);
+		for (i = 0; i != len; i++) {
+			char c;
+
+			if (get_user(c, data + i))
+				return -EFAULT;
+
+			if (c == 'V')
+				set_bit(WDT_OK_TO_CLOSE, &jz4740_wdt.status);
+		}
+		jz4740_wdt_service();
+	}
+
+	return len;
+}
+
+static const struct watchdog_info ident = {
+	.options = WDIOF_KEEPALIVEPING,
+	.identity = "jz4740 Watchdog",
+};
+
+static long jz4740_wdt_ioctl(struct file *file,
+					unsigned int cmd, unsigned long arg)
+{
+	int ret = -ENOTTY;
+	int heartbeat_seconds;
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		ret = copy_to_user((struct watchdog_info *)arg, &ident,
+				sizeof(ident)) ? -EFAULT : 0;
+		break;
+
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		ret = put_user(0, (int *)arg);
+		break;
+
+	case WDIOC_KEEPALIVE:
+		jz4740_wdt_service();
+		return 0;
+
+	case WDIOC_SETTIMEOUT:
+		if (get_user(heartbeat_seconds, (int __user *)arg))
+			return -EFAULT;
+
+		jz4740_wdt_set_heartbeat(heartbeat_seconds);
+		return 0;
+
+	case WDIOC_GETTIMEOUT:
+		return put_user(heartbeat, (int *)arg);
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static int jz4740_wdt_release(struct inode *inode, struct file *file)
+{
+	jz4740_wdt_service();
+
+	if (test_and_clear_bit(WDT_OK_TO_CLOSE, &jz4740_wdt.status))
+		jz4740_wdt_disable();
+
+	clear_bit(WDT_IN_USE, &jz4740_wdt.status);
+	return 0;
+}
+
+static const struct file_operations jz4740_wdt_fops = {
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.write = jz4740_wdt_write,
+	.unlocked_ioctl = jz4740_wdt_ioctl,
+	.open = jz4740_wdt_open,
+	.release = jz4740_wdt_release,
+};
+
+static struct miscdevice jz4740_wdt_miscdev = {
+	.minor = WATCHDOG_MINOR,
+	.name = "watchdog",
+	.fops = &jz4740_wdt_fops,
+};
+
+static int __devinit jz4740_wdt_probe(struct platform_device *pdev)
+{
+	int ret = 0, size;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(dev, "failed to get memory region resource\n");
+		return -ENXIO;
+	}
+
+	size = resource_size(res);
+	jz4740_wdt.mem = request_mem_region(res->start, size, pdev->name);
+	if (jz4740_wdt.mem == NULL) {
+		dev_err(dev, "failed to get memory region\n");
+		return -EBUSY;
+	}
+
+	jz4740_wdt.base = ioremap_nocache(res->start, size);
+	if (jz4740_wdt.base == NULL) {
+		dev_err(dev, "failed to map memory region\n");
+		ret = -EBUSY;
+		goto err_release_region;
+	}
+
+	jz4740_wdt.rtc_clk = clk_get(NULL, "rtc");
+	if (IS_ERR(jz4740_wdt.rtc_clk)) {
+		dev_err(dev, "cannot find RTC clock\n");
+		ret = PTR_ERR(jz4740_wdt.rtc_clk);
+		goto err_iounmap;
+	}
+
+	ret = misc_register(&jz4740_wdt_miscdev);
+	if (ret < 0) {
+		dev_err(dev, "cannot register misc device\n");
+		goto err_disable_clk;
+	}
+
+	return 0;
+
+err_disable_clk:
+	clk_put(jz4740_wdt.rtc_clk);
+err_iounmap:
+	iounmap(jz4740_wdt.base);
+err_release_region:
+	release_mem_region(jz4740_wdt.mem->start,
+			resource_size(jz4740_wdt.mem));
+	return ret;
+}
+
+
+static int __devexit jz4740_wdt_remove(struct platform_device *pdev)
+{
+	jz4740_wdt_disable();
+	misc_deregister(&jz4740_wdt_miscdev);
+	clk_put(jz4740_wdt.rtc_clk);
+
+	iounmap(jz4740_wdt.base);
+	jz4740_wdt.base = NULL;
+
+	release_mem_region(jz4740_wdt.mem->start,
+				resource_size(jz4740_wdt.mem));
+	jz4740_wdt.mem = NULL;
+
+	return 0;
+}
+
+
+static struct platform_driver jz4740_wdt_driver = {
+	.probe = jz4740_wdt_probe,
+	.remove = __devexit_p(jz4740_wdt_remove),
+	.driver = {
+		.name = "jz4740-wdt",
+		.owner	= THIS_MODULE,
+	},
+};
+
+
+static int __init jz4740_wdt_init(void)
+{
+	return platform_driver_register(&jz4740_wdt_driver);
+}
+module_init(jz4740_wdt_init);
+
+static void __exit jz4740_wdt_exit(void)
+{
+	platform_driver_unregister(&jz4740_wdt_driver);
+}
+module_exit(jz4740_wdt_exit);
+
+MODULE_AUTHOR("Paul Cercueil <paul@crapouillou.net>");
+MODULE_DESCRIPTION("jz4740 Watchdog Driver");
+
+module_param(heartbeat, int, 0);
+MODULE_PARM_DESC(heartbeat,
+		"Watchdog heartbeat period in seconds from 1 to "
+		__MODULE_STRING(MAX_HEARTBEAT) ", default "
+		__MODULE_STRING(DEFAULT_HEARTBEAT));
+
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
+MODULE_ALIAS("platform:jz4740-wdt");
diff --git a/drivers/watchdog/machzwd.c b/drivers/watchdog/machzwd.c
index 928035069396..1332b838cc58 100644
--- a/drivers/watchdog/machzwd.c
+++ b/drivers/watchdog/machzwd.c
@@ -54,7 +54,7 @@
 
 /* indexes */			/* size */
 #define ZFL_VERSION	0x02	/* 16   */
-#define CONTROL 	0x10	/* 16   */
+#define CONTROL		0x10	/* 16   */
 #define STATUS		0x12	/* 8    */
 #define COUNTER_1	0x0C	/* 16   */
 #define COUNTER_2	0x0E	/* 8    */
diff --git a/drivers/watchdog/max63xx_wdt.c b/drivers/watchdog/max63xx_wdt.c
index 3053ff05ca41..7a82ce5a6337 100644
--- a/drivers/watchdog/max63xx_wdt.c
+++ b/drivers/watchdog/max63xx_wdt.c
@@ -41,7 +41,7 @@ static int nowayout  = WATCHDOG_NOWAYOUT;
  * to ping the watchdog.
  */
 #define MAX6369_WDSET	(7 << 0)
-#define MAX6369_WDI   	(1 << 3)
+#define MAX6369_WDI	(1 << 3)
 
 static DEFINE_SPINLOCK(io_lock);
 
diff --git a/drivers/watchdog/mpc8xxx_wdt.c b/drivers/watchdog/mpc8xxx_wdt.c
index 8fa213cdb499..6709d723e017 100644
--- a/drivers/watchdog/mpc8xxx_wdt.c
+++ b/drivers/watchdog/mpc8xxx_wdt.c
@@ -2,9 +2,9 @@
  * mpc8xxx_wdt.c - MPC8xx/MPC83xx/MPC86xx watchdog userspace interface
  *
  * Authors: Dave Updegraff <dave@cray.org>
- * 	    Kumar Gala <galak@kernel.crashing.org>
- * 		Attribution: from 83xx_wst: Florian Schirmer <jolt@tuxbox.org>
- * 				..and from sc520_wdt
+ *	    Kumar Gala <galak@kernel.crashing.org>
+ *		Attribution: from 83xx_wst: Florian Schirmer <jolt@tuxbox.org>
+ *				..and from sc520_wdt
  * Copyright (c) 2008  MontaVista Software, Inc.
  *                     Anton Vorontsov <avorontsov@ru.mvista.com>
  *
@@ -185,15 +185,18 @@ static struct miscdevice mpc8xxx_wdt_miscdev = {
 	.fops	= &mpc8xxx_wdt_fops,
 };
 
-static int __devinit mpc8xxx_wdt_probe(struct platform_device *ofdev,
-				       const struct of_device_id *match)
+static int __devinit mpc8xxx_wdt_probe(struct platform_device *ofdev)
 {
 	int ret;
 	struct device_node *np = ofdev->dev.of_node;
-	struct mpc8xxx_wdt_type *wdt_type = match->data;
+	struct mpc8xxx_wdt_type *wdt_type;
 	u32 freq = fsl_get_sys_freq();
 	bool enabled;
 
+	if (!ofdev->dev.of_match)
+		return -EINVAL;
+	wdt_type = match->data;
+
 	if (!freq || freq == -1)
 		return -EINVAL;
 
@@ -272,7 +275,7 @@ static const struct of_device_id mpc8xxx_wdt_match[] = {
 };
 MODULE_DEVICE_TABLE(of, mpc8xxx_wdt_match);
 
-static struct of_platform_driver mpc8xxx_wdt_driver = {
+static struct platform_driver mpc8xxx_wdt_driver = {
 	.probe		= mpc8xxx_wdt_probe,
 	.remove		= __devexit_p(mpc8xxx_wdt_remove),
 	.driver = {
@@ -308,13 +311,13 @@ module_init(mpc8xxx_wdt_init_late);
 
 static int __init mpc8xxx_wdt_init(void)
 {
-	return of_register_platform_driver(&mpc8xxx_wdt_driver);
+	return platform_driver_register(&mpc8xxx_wdt_driver);
 }
 arch_initcall(mpc8xxx_wdt_init);
 
 static void __exit mpc8xxx_wdt_exit(void)
 {
-	of_unregister_platform_driver(&mpc8xxx_wdt_driver);
+	platform_driver_unregister(&mpc8xxx_wdt_driver);
 }
 module_exit(mpc8xxx_wdt_exit);
 
diff --git a/drivers/watchdog/mpcore_wdt.c b/drivers/watchdog/mpcore_wdt.c
index b8ec7aca3c8e..2b4af222b5f2 100644
--- a/drivers/watchdog/mpcore_wdt.c
+++ b/drivers/watchdog/mpcore_wdt.c
@@ -172,7 +172,7 @@ static int mpcore_wdt_release(struct inode *inode, struct file *file)
 
 	/*
 	 *	Shut off the timer.
-	 * 	Lock it in if it's a module and we set nowayout
+	 *	Lock it in if it's a module and we set nowayout
 	 */
 	if (wdt->expect_close == 42)
 		mpcore_wdt_stop(wdt);
diff --git a/drivers/watchdog/mtx-1_wdt.c b/drivers/watchdog/mtx-1_wdt.c
index 08e8a6ab74e1..5ec5ac1f7878 100644
--- a/drivers/watchdog/mtx-1_wdt.c
+++ b/drivers/watchdog/mtx-1_wdt.c
@@ -190,19 +190,19 @@ static ssize_t mtx1_wdt_write(struct file *file, const char *buf,
 }
 
 static const struct file_operations mtx1_wdt_fops = {
-	.owner 		= THIS_MODULE,
+	.owner		= THIS_MODULE,
 	.llseek		= no_llseek,
 	.unlocked_ioctl	= mtx1_wdt_ioctl,
-	.open 		= mtx1_wdt_open,
-	.write 		= mtx1_wdt_write,
-	.release 	= mtx1_wdt_release,
+	.open		= mtx1_wdt_open,
+	.write		= mtx1_wdt_write,
+	.release	= mtx1_wdt_release,
 };
 
 
 static struct miscdevice mtx1_wdt_misc = {
-	.minor 	= WATCHDOG_MINOR,
-	.name 	= "watchdog",
-	.fops 	= &mtx1_wdt_fops,
+	.minor	= WATCHDOG_MINOR,
+	.name	= "watchdog",
+	.fops	= &mtx1_wdt_fops,
 };
 
 
diff --git a/drivers/watchdog/nv_tco.c b/drivers/watchdog/nv_tco.c
index 1a50aa7079bf..267377a5a83e 100644
--- a/drivers/watchdog/nv_tco.c
+++ b/drivers/watchdog/nv_tco.c
@@ -289,7 +289,7 @@ static struct miscdevice nv_tco_miscdev = {
  * register a pci_driver, because someone else might one day
  * want to register another driver on the same PCI id.
  */
-static struct pci_device_id tco_pci_tbl[] = {
+static DEFINE_PCI_DEVICE_TABLE(tco_pci_tbl) = {
 	{ PCI_VENDOR_ID_NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SMBUS,
 	  PCI_ANY_ID, PCI_ANY_ID, },
 	{ PCI_VENDOR_ID_NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SMBUS,
diff --git a/drivers/watchdog/omap_wdt.h b/drivers/watchdog/omap_wdt.h
index fc02ec6a0386..09b774cf75b9 100644
--- a/drivers/watchdog/omap_wdt.h
+++ b/drivers/watchdog/omap_wdt.h
@@ -44,7 +44,7 @@
  * months before firing.  These limits work without scaling,
  * with the 60 second default assumed by most tools and docs.
  */
-#define TIMER_MARGIN_MAX    	(24 * 60 * 60)	/* 1 day */
+#define TIMER_MARGIN_MAX	(24 * 60 * 60)	/* 1 day */
 #define TIMER_MARGIN_DEFAULT	60	/* 60 secs */
 #define TIMER_MARGIN_MIN	1
 
diff --git a/drivers/watchdog/pc87413_wdt.c b/drivers/watchdog/pc87413_wdt.c
index 3a56bc360924..139d773300c6 100644
--- a/drivers/watchdog/pc87413_wdt.c
+++ b/drivers/watchdog/pc87413_wdt.c
@@ -514,7 +514,7 @@ static struct miscdevice pc87413_miscdev = {
 /* -- Module init functions -------------------------------------*/
 
 /**
- * 	pc87413_init: module's "constructor"
+ *	pc87413_init: module's "constructor"
  *
  *	Set up the WDT watchdog board. All we have to do is grab the
  *	resources we require and bitch if anyone beat us to them.
diff --git a/drivers/watchdog/pcwd_pci.c b/drivers/watchdog/pcwd_pci.c
index 64374d636f09..b8d14f88f0b5 100644
--- a/drivers/watchdog/pcwd_pci.c
+++ b/drivers/watchdog/pcwd_pci.c
@@ -817,7 +817,7 @@ static void __devexit pcipcwd_card_exit(struct pci_dev *pdev)
 	cards_found--;
 }
 
-static struct pci_device_id pcipcwd_pci_tbl[] = {
+static DEFINE_PCI_DEVICE_TABLE(pcipcwd_pci_tbl) = {
 	{ PCI_VENDOR_ID_QUICKLOGIC, PCI_DEVICE_ID_WATCHDOG_PCIPCWD,
 		PCI_ANY_ID, PCI_ANY_ID, },
 	{ 0 },			/* End of list */
diff --git a/drivers/watchdog/pnx4008_wdt.c b/drivers/watchdog/pnx4008_wdt.c
index bf5b97c546eb..c7cf4cbf8ab3 100644
--- a/drivers/watchdog/pnx4008_wdt.c
+++ b/drivers/watchdog/pnx4008_wdt.c
@@ -4,7 +4,7 @@
  * Watchdog driver for PNX4008 board
  *
  * Authors: Dmitry Chigirev <source@mvista.com>,
- * 	    Vitaly Wool <vitalywool@gmail.com>
+ *	    Vitaly Wool <vitalywool@gmail.com>
  * Based on sa1100 driver,
  * Copyright (C) 2000 Oleg Drokin <green@crimea.edu>
  *
diff --git a/drivers/watchdog/rdc321x_wdt.c b/drivers/watchdog/rdc321x_wdt.c
index 3939e53f5f98..d8e725082fdc 100644
--- a/drivers/watchdog/rdc321x_wdt.c
+++ b/drivers/watchdog/rdc321x_wdt.c
@@ -37,6 +37,7 @@
 #include <linux/io.h>
 #include <linux/uaccess.h>
 #include <linux/mfd/rdc321x.h>
+#include <linux/mfd/core.h>
 
 #define RDC_WDT_MASK	0x80000000 /* Mask */
 #define RDC_WDT_EN	0x00800000 /* Enable bit */
@@ -231,7 +232,7 @@ static int __devinit rdc321x_wdt_probe(struct platform_device *pdev)
 	struct resource *r;
 	struct rdc321x_wdt_pdata *pdata;
 
-	pdata = platform_get_drvdata(pdev);
+	pdata = mfd_get_data(pdev);
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform data supplied\n");
 		return -ENODEV;
diff --git a/drivers/watchdog/riowd.c b/drivers/watchdog/riowd.c
index 3faee1ae64bd..109b533896b7 100644
--- a/drivers/watchdog/riowd.c
+++ b/drivers/watchdog/riowd.c
@@ -172,8 +172,7 @@ static struct miscdevice riowd_miscdev = {
 	.fops	= &riowd_fops
 };
 
-static int __devinit riowd_probe(struct platform_device *op,
-				 const struct of_device_id *match)
+static int __devinit riowd_probe(struct platform_device *op)
 {
 	struct riowd *p;
 	int err = -EINVAL;
@@ -238,7 +237,7 @@ static const struct of_device_id riowd_match[] = {
 };
 MODULE_DEVICE_TABLE(of, riowd_match);
 
-static struct of_platform_driver riowd_driver = {
+static struct platform_driver riowd_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
@@ -250,12 +249,12 @@ static struct of_platform_driver riowd_driver = {
 
 static int __init riowd_init(void)
 {
-	return of_register_platform_driver(&riowd_driver);
+	return platform_driver_register(&riowd_driver);
 }
 
 static void __exit riowd_exit(void)
 {
-	of_unregister_platform_driver(&riowd_driver);
+	platform_driver_unregister(&riowd_driver);
 }
 
 module_init(riowd_init);
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index ae53662c29bc..25b39bf35925 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -224,7 +224,7 @@ static int s3c2410wdt_release(struct inode *inode, struct file *file)
 {
 	/*
 	 *	Shut off the timer.
-	 * 	Lock it in if it's a module and we set nowayout
+	 *	Lock it in if it's a module and we set nowayout
 	 */
 
 	if (expect_close == 42)
diff --git a/drivers/watchdog/sbc8360.c b/drivers/watchdog/sbc8360.c
index 68e2e2d6f73d..514ec23050f7 100644
--- a/drivers/watchdog/sbc8360.c
+++ b/drivers/watchdog/sbc8360.c
@@ -114,7 +114,7 @@ static char expect_close;
  *	C |	6.5s	65s	650s	1300s
  *	D |	7s	70s	700s	1400s
  *	E |	7.5s	75s	750s	1500s
- *	F |	8s	80s 	800s 	1600s
+ *	F |	8s	80s	800s	1600s
  *
  * Another way to say the same things is:
  *  For N=1, Timeout = (M+1) * 0.5s
diff --git a/drivers/watchdog/sbc_epx_c3.c b/drivers/watchdog/sbc_epx_c3.c
index 28f1214457bd..3066a5127ca8 100644
--- a/drivers/watchdog/sbc_epx_c3.c
+++ b/drivers/watchdog/sbc_epx_c3.c
@@ -220,7 +220,7 @@ module_exit(watchdog_exit);
 MODULE_AUTHOR("Calin A. Culianu <calin@ajvar.org>");
 MODULE_DESCRIPTION("Hardware Watchdog Device for Winsystems EPX-C3 SBC.  "
 	"Note that there is no way to probe for this device -- "
-	"so only use it if you are *sure* you are runnning on this specific "
+	"so only use it if you are *sure* you are running on this specific "
 	"SBC system from Winsystems!  It writes to IO ports 0x1ee and 0x1ef!");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
diff --git a/drivers/watchdog/sbc_fitpc2_wdt.c b/drivers/watchdog/sbc_fitpc2_wdt.c
index c7d67e9a7465..d5d399464599 100644
--- a/drivers/watchdog/sbc_fitpc2_wdt.c
+++ b/drivers/watchdog/sbc_fitpc2_wdt.c
@@ -41,7 +41,7 @@ static DEFINE_MUTEX(wdt_lock);
 #define IFACE_ON_COMMAND	1
 #define REBOOT_COMMAND		2
 
-#define WATCHDOG_NAME 		"SBC-FITPC2 Watchdog"
+#define WATCHDOG_NAME		"SBC-FITPC2 Watchdog"
 
 static void wdt_send_data(unsigned char command, unsigned char data)
 {
@@ -201,11 +201,14 @@ static struct miscdevice fitpc2_wdt_miscdev = {
 static int __init fitpc2_wdt_init(void)
 {
 	int err;
+	const char *brd_name;
 
-	if (!strstr(dmi_get_system_info(DMI_BOARD_NAME), "SBC-FITPC2"))
+	brd_name = dmi_get_system_info(DMI_BOARD_NAME);
+
+	if (!brd_name || !strstr(brd_name, "SBC-FITPC2"))
 		return -ENODEV;
 
-	pr_info("%s found\n", dmi_get_system_info(DMI_BOARD_NAME));
+	pr_info("%s found\n", brd_name);
 
 	if (!request_region(COMMAND_PORT, 1, WATCHDOG_NAME)) {
 		pr_err("I/O address 0x%04x already in use\n", COMMAND_PORT);
diff --git a/drivers/watchdog/sch311x_wdt.c b/drivers/watchdog/sch311x_wdt.c
index 0461858e07d0..b61ab1c54293 100644
--- a/drivers/watchdog/sch311x_wdt.c
+++ b/drivers/watchdog/sch311x_wdt.c
@@ -508,7 +508,7 @@ static int __init sch311x_detect(int sio_config_port, unsigned short *addr)
 	sch311x_sio_outb(sio_config_port, 0x07, 0x0a);
 
 	/* Check if Logical Device Register is currently active */
-	if (sch311x_sio_inb(sio_config_port, 0x30) && 0x01 == 0)
+	if ((sch311x_sio_inb(sio_config_port, 0x30) & 0x01) == 0)
 		printk(KERN_INFO PFX "Seems that LDN 0x0a is not active...\n");
 
 	/* Get the base address of the runtime registers */
diff --git a/drivers/watchdog/shwdt.c b/drivers/watchdog/shwdt.c
index 6fc74065abee..4e3e7eb5919c 100644
--- a/drivers/watchdog/shwdt.c
+++ b/drivers/watchdog/shwdt.c
@@ -1,9 +1,9 @@
 /*
- * drivers/char/watchdog/shwdt.c
+ * drivers/watchdog/shwdt.c
  *
  * Watchdog driver for integrated watchdog in the SuperH processors.
  *
- * Copyright (C) 2001, 2002, 2003 Paul Mundt <lethal@linux-sh.org>
+ * Copyright (C) 2001 - 2010  Paul Mundt <lethal@linux-sh.org>
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the
@@ -19,6 +19,7 @@
  */
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/platform_device.h>
 #include <linux/init.h>
 #include <linux/types.h>
 #include <linux/miscdevice.h>
@@ -28,11 +29,12 @@
 #include <linux/ioport.h>
 #include <linux/fs.h>
 #include <linux/mm.h>
+#include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
 #include <asm/watchdog.h>
 
-#define PFX "shwdt: "
+#define DRV_NAME "sh-wdt"
 
 /*
  * Default clock division ratio is 5.25 msecs. For an additional table of
@@ -62,37 +64,36 @@
  * misses its deadline, the kernel timer will allow the WDT to overflow.
  */
 static int clock_division_ratio = WTCSR_CKS_4096;
-
 #define next_ping_period(cks)	msecs_to_jiffies(cks - 4)
 
-static void sh_wdt_ping(unsigned long data);
-
-static unsigned long shwdt_is_open;
 static const struct watchdog_info sh_wdt_info;
-static char shwdt_expect_close;
-static DEFINE_TIMER(timer, sh_wdt_ping, 0, 0);
-static unsigned long next_heartbeat;
+static struct platform_device *sh_wdt_dev;
 static DEFINE_SPINLOCK(shwdt_lock);
 
 #define WATCHDOG_HEARTBEAT 30			/* 30 sec default heartbeat */
 static int heartbeat = WATCHDOG_HEARTBEAT;	/* in seconds */
-
 static int nowayout = WATCHDOG_NOWAYOUT;
+static unsigned long next_heartbeat;
 
-/**
- * 	sh_wdt_start - Start the Watchdog
- *
- * 	Starts the watchdog.
- */
-static void sh_wdt_start(void)
+struct sh_wdt {
+	void __iomem		*base;
+	struct device		*dev;
+
+	struct timer_list	timer;
+
+	unsigned long		enabled;
+	char			expect_close;
+};
+
+static void sh_wdt_start(struct sh_wdt *wdt)
 {
-	__u8 csr;
 	unsigned long flags;
+	u8 csr;
 
 	spin_lock_irqsave(&shwdt_lock, flags);
 
 	next_heartbeat = jiffies + (heartbeat * HZ);
-	mod_timer(&timer, next_ping_period(clock_division_ratio));
+	mod_timer(&wdt->timer, next_ping_period(clock_division_ratio));
 
 	csr = sh_wdt_read_csr();
 	csr |= WTCSR_WT | clock_division_ratio;
@@ -114,15 +115,6 @@ static void sh_wdt_start(void)
 	sh_wdt_write_csr(csr);
 
 #ifdef CONFIG_CPU_SH2
-	/*
-	 * Whoever came up with the RSTCSR semantics must've been smoking
-	 * some of the good stuff, since in addition to the WTCSR/WTCNT write
-	 * brain-damage, it's managed to fuck things up one step further..
-	 *
-	 * If we need to clear the WOVF bit, the upper byte has to be 0xa5..
-	 * but if we want to touch RSTE or RSTS, the upper byte has to be
-	 * 0x5a..
-	 */
 	csr = sh_wdt_read_rstcsr();
 	csr &= ~RSTCSR_RSTS;
 	sh_wdt_write_rstcsr(csr);
@@ -130,30 +122,23 @@ static void sh_wdt_start(void)
 	spin_unlock_irqrestore(&shwdt_lock, flags);
 }
 
-/**
- * 	sh_wdt_stop - Stop the Watchdog
- * 	Stops the watchdog.
- */
-static void sh_wdt_stop(void)
+static void sh_wdt_stop(struct sh_wdt *wdt)
 {
-	__u8 csr;
 	unsigned long flags;
+	u8 csr;
 
 	spin_lock_irqsave(&shwdt_lock, flags);
 
-	del_timer(&timer);
+	del_timer(&wdt->timer);
 
 	csr = sh_wdt_read_csr();
 	csr &= ~WTCSR_TME;
 	sh_wdt_write_csr(csr);
+
 	spin_unlock_irqrestore(&shwdt_lock, flags);
 }
 
-/**
- * 	sh_wdt_keepalive - Keep the Userspace Watchdog Alive
- * 	The Userspace watchdog got a KeepAlive: schedule the next heartbeat.
- */
-static inline void sh_wdt_keepalive(void)
+static inline void sh_wdt_keepalive(struct sh_wdt *wdt)
 {
 	unsigned long flags;
 
@@ -162,10 +147,6 @@ static inline void sh_wdt_keepalive(void)
 	spin_unlock_irqrestore(&shwdt_lock, flags);
 }
 
-/**
- * 	sh_wdt_set_heartbeat - Set the Userspace Watchdog heartbeat
- * 	Set the Userspace Watchdog heartbeat
- */
 static int sh_wdt_set_heartbeat(int t)
 {
 	unsigned long flags;
@@ -179,19 +160,14 @@ static int sh_wdt_set_heartbeat(int t)
 	return 0;
 }
 
-/**
- * 	sh_wdt_ping - Ping the Watchdog
- *	@data: Unused
- *
- * 	Clears overflow bit, resets timer counter.
- */
 static void sh_wdt_ping(unsigned long data)
 {
+	struct sh_wdt *wdt = (struct sh_wdt *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&shwdt_lock, flags);
 	if (time_before(jiffies, next_heartbeat)) {
-		__u8 csr;
+		u8 csr;
 
 		csr = sh_wdt_read_csr();
 		csr &= ~WTCSR_IOVF;
@@ -199,148 +175,76 @@ static void sh_wdt_ping(unsigned long data)
 
 		sh_wdt_write_cnt(0);
 
-		mod_timer(&timer, next_ping_period(clock_division_ratio));
+		mod_timer(&wdt->timer, next_ping_period(clock_division_ratio));
 	} else
-		printk(KERN_WARNING PFX "Heartbeat lost! Will not ping "
-		       "the watchdog\n");
+		dev_warn(wdt->dev, "Heartbeat lost! Will not ping "
+		         "the watchdog\n");
 	spin_unlock_irqrestore(&shwdt_lock, flags);
 }
 
-/**
- * 	sh_wdt_open - Open the Device
- * 	@inode: inode of device
- * 	@file: file handle of device
- *
- * 	Watchdog device is opened and started.
- */
 static int sh_wdt_open(struct inode *inode, struct file *file)
 {
-	if (test_and_set_bit(0, &shwdt_is_open))
+	struct sh_wdt *wdt = platform_get_drvdata(sh_wdt_dev);
+
+	if (test_and_set_bit(0, &wdt->enabled))
 		return -EBUSY;
 	if (nowayout)
 		__module_get(THIS_MODULE);
 
-	sh_wdt_start();
+	file->private_data = wdt;
+
+	sh_wdt_start(wdt);
 
 	return nonseekable_open(inode, file);
 }
 
-/**
- * 	sh_wdt_close - Close the Device
- * 	@inode: inode of device
- * 	@file: file handle of device
- *
- * 	Watchdog device is closed and stopped.
- */
 static int sh_wdt_close(struct inode *inode, struct file *file)
 {
-	if (shwdt_expect_close == 42) {
-		sh_wdt_stop();
+	struct sh_wdt *wdt = file->private_data;
+
+	if (wdt->expect_close == 42) {
+		sh_wdt_stop(wdt);
 	} else {
-		printk(KERN_CRIT PFX "Unexpected close, not "
-		       "stopping watchdog!\n");
-		sh_wdt_keepalive();
+		dev_crit(wdt->dev, "Unexpected close, not "
+		         "stopping watchdog!\n");
+		sh_wdt_keepalive(wdt);
 	}
 
-	clear_bit(0, &shwdt_is_open);
-	shwdt_expect_close = 0;
+	clear_bit(0, &wdt->enabled);
+	wdt->expect_close = 0;
 
 	return 0;
 }
 
-/**
- * 	sh_wdt_write - Write to Device
- * 	@file: file handle of device
- * 	@buf: buffer to write
- * 	@count: length of buffer
- * 	@ppos: offset
- *
- * 	Pings the watchdog on write.
- */
 static ssize_t sh_wdt_write(struct file *file, const char *buf,
 			    size_t count, loff_t *ppos)
 {
+	struct sh_wdt *wdt = file->private_data;
+
 	if (count) {
 		if (!nowayout) {
 			size_t i;
 
-			shwdt_expect_close = 0;
+			wdt->expect_close = 0;
 
 			for (i = 0; i != count; i++) {
 				char c;
 				if (get_user(c, buf + i))
 					return -EFAULT;
 				if (c == 'V')
-					shwdt_expect_close = 42;
+					wdt->expect_close = 42;
 			}
 		}
-		sh_wdt_keepalive();
+		sh_wdt_keepalive(wdt);
 	}
 
 	return count;
 }
 
-/**
- * 	sh_wdt_mmap - map WDT/CPG registers into userspace
- * 	@file: file structure for the device
- * 	@vma: VMA to map the registers into
- *
- * 	A simple mmap() implementation for the corner cases where the counter
- * 	needs to be mapped in userspace directly. Due to the relatively small
- * 	size of the area, neighbouring registers not necessarily tied to the
- * 	CPG will also be accessible through the register page, so this remains
- * 	configurable for users that really know what they're doing.
- *
- *	Additionaly, the register page maps in the CPG register base relative
- *	to the nearest page-aligned boundary, which requires that userspace do
- *	the appropriate CPU subtype math for calculating the page offset for
- *	the counter value.
- */
-static int sh_wdt_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	int ret = -ENOSYS;
-
-#ifdef CONFIG_SH_WDT_MMAP
-	unsigned long addr;
-
-	/* Only support the simple cases where we map in a register page. */
-	if (((vma->vm_end - vma->vm_start) != PAGE_SIZE) || vma->vm_pgoff)
-		return -EINVAL;
-
-	/*
-	 * Pick WTCNT as the start, it's usually the first register after the
-	 * FRQCR, and neither one are generally page-aligned out of the box.
-	 */
-	addr = WTCNT & ~(PAGE_SIZE - 1);
-
-	vma->vm_flags |= VM_IO;
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-
-	if (io_remap_pfn_range(vma, vma->vm_start, addr >> PAGE_SHIFT,
-			       PAGE_SIZE, vma->vm_page_prot)) {
-		printk(KERN_ERR PFX "%s: io_remap_pfn_range failed\n",
-		       __func__);
-		return -EAGAIN;
-	}
-
-	ret = 0;
-#endif
-
-	return ret;
-}
-
-/**
- * 	sh_wdt_ioctl - Query Device
- * 	@file: file handle of device
- * 	@cmd: watchdog command
- * 	@arg: argument
- *
- * 	Query basic information from the device or ping it, as outlined by the
- * 	watchdog API.
- */
 static long sh_wdt_ioctl(struct file *file, unsigned int cmd,
 							unsigned long arg)
 {
+	struct sh_wdt *wdt = file->private_data;
 	int new_heartbeat;
 	int options, retval = -EINVAL;
 
@@ -356,18 +260,18 @@ static long sh_wdt_ioctl(struct file *file, unsigned int cmd,
 			return -EFAULT;
 
 		if (options & WDIOS_DISABLECARD) {
-			sh_wdt_stop();
+			sh_wdt_stop(wdt);
 			retval = 0;
 		}
 
 		if (options & WDIOS_ENABLECARD) {
-			sh_wdt_start();
+			sh_wdt_start(wdt);
 			retval = 0;
 		}
 
 		return retval;
 	case WDIOC_KEEPALIVE:
-		sh_wdt_keepalive();
+		sh_wdt_keepalive(wdt);
 		return 0;
 	case WDIOC_SETTIMEOUT:
 		if (get_user(new_heartbeat, (int *)arg))
@@ -376,7 +280,7 @@ static long sh_wdt_ioctl(struct file *file, unsigned int cmd,
 		if (sh_wdt_set_heartbeat(new_heartbeat))
 			return -EINVAL;
 
-		sh_wdt_keepalive();
+		sh_wdt_keepalive(wdt);
 		/* Fall */
 	case WDIOC_GETTIMEOUT:
 		return put_user(heartbeat, (int *)arg);
@@ -386,20 +290,13 @@ static long sh_wdt_ioctl(struct file *file, unsigned int cmd,
 	return 0;
 }
 
-/**
- * 	sh_wdt_notify_sys - Notifier Handler
- * 	@this: notifier block
- * 	@code: notifier event
- * 	@unused: unused
- *
- * 	Handles specific events, such as turning off the watchdog during a
- * 	shutdown event.
- */
 static int sh_wdt_notify_sys(struct notifier_block *this,
 			     unsigned long code, void *unused)
 {
+	struct sh_wdt *wdt = platform_get_drvdata(sh_wdt_dev);
+
 	if (code == SYS_DOWN || code == SYS_HALT)
-		sh_wdt_stop();
+		sh_wdt_stop(wdt);
 
 	return NOTIFY_DONE;
 }
@@ -411,7 +308,6 @@ static const struct file_operations sh_wdt_fops = {
 	.unlocked_ioctl	= sh_wdt_ioctl,
 	.open		= sh_wdt_open,
 	.release	= sh_wdt_close,
-	.mmap		= sh_wdt_mmap,
 };
 
 static const struct watchdog_info sh_wdt_info = {
@@ -431,66 +327,148 @@ static struct miscdevice sh_wdt_miscdev = {
 	.fops		= &sh_wdt_fops,
 };
 
-/**
- * 	sh_wdt_init - Initialize module
- * 	Registers the device and notifier handler. Actual device
- * 	initialization is handled by sh_wdt_open().
- */
-static int __init sh_wdt_init(void)
+static int __devinit sh_wdt_probe(struct platform_device *pdev)
 {
+	struct sh_wdt *wdt;
+	struct resource *res;
 	int rc;
 
-	if (clock_division_ratio < 0x5 || clock_division_ratio > 0x7) {
-		clock_division_ratio = WTCSR_CKS_4096;
-		printk(KERN_INFO PFX
-		  "clock_division_ratio value must be 0x5<=x<=0x7, using %d\n",
-				clock_division_ratio);
+	/*
+	 * As this driver only covers the global watchdog case, reject
+	 * any attempts to register per-CPU watchdogs.
+	 */
+	if (pdev->id != -1)
+		return -EINVAL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (unlikely(!res))
+		return -EINVAL;
+
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+				     resource_size(res), DRV_NAME))
+		return -EBUSY;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(struct sh_wdt), GFP_KERNEL);
+	if (unlikely(!wdt)) {
+		rc = -ENOMEM;
+		goto out_release;
 	}
 
-	rc = sh_wdt_set_heartbeat(heartbeat);
-	if (unlikely(rc)) {
-		heartbeat = WATCHDOG_HEARTBEAT;
-		printk(KERN_INFO PFX
-			"heartbeat value must be 1<=x<=3600, using %d\n",
-								heartbeat);
+	wdt->dev = &pdev->dev;
+
+	wdt->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (unlikely(!wdt->base)) {
+		rc = -ENXIO;
+		goto out_err;
 	}
 
 	rc = register_reboot_notifier(&sh_wdt_notifier);
 	if (unlikely(rc)) {
-		printk(KERN_ERR PFX
+		dev_err(&pdev->dev,
 			"Can't register reboot notifier (err=%d)\n", rc);
-		return rc;
+		goto out_unmap;
 	}
 
+	sh_wdt_miscdev.parent = wdt->dev;
+
 	rc = misc_register(&sh_wdt_miscdev);
 	if (unlikely(rc)) {
-		printk(KERN_ERR PFX
+		dev_err(&pdev->dev,
 			"Can't register miscdev on minor=%d (err=%d)\n",
 						sh_wdt_miscdev.minor, rc);
-		unregister_reboot_notifier(&sh_wdt_notifier);
-		return rc;
+		goto out_unreg;
 	}
 
-	printk(KERN_INFO PFX "initialized. heartbeat=%d sec (nowayout=%d)\n",
-		heartbeat, nowayout);
+	init_timer(&wdt->timer);
+	wdt->timer.function	= sh_wdt_ping;
+	wdt->timer.data		= (unsigned long)wdt;
+	wdt->timer.expires	= next_ping_period(clock_division_ratio);
+
+	platform_set_drvdata(pdev, wdt);
+	sh_wdt_dev = pdev;
+
+	dev_info(&pdev->dev, "initialized.\n");
 
 	return 0;
+
+out_unreg:
+	unregister_reboot_notifier(&sh_wdt_notifier);
+out_unmap:
+	devm_iounmap(&pdev->dev, wdt->base);
+out_err:
+	devm_kfree(&pdev->dev, wdt);
+out_release:
+	devm_release_mem_region(&pdev->dev, res->start, resource_size(res));
+
+	return rc;
 }
 
-/**
- * 	sh_wdt_exit - Deinitialize module
- * 	Unregisters the device and notifier handler. Actual device
- * 	deinitialization is handled by sh_wdt_close().
- */
-static void __exit sh_wdt_exit(void)
+static int __devexit sh_wdt_remove(struct platform_device *pdev)
 {
+	struct sh_wdt *wdt = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	platform_set_drvdata(pdev, NULL);
+
 	misc_deregister(&sh_wdt_miscdev);
+
+	sh_wdt_dev = NULL;
+
 	unregister_reboot_notifier(&sh_wdt_notifier);
+	devm_release_mem_region(&pdev->dev, res->start, resource_size(res));
+	devm_iounmap(&pdev->dev, wdt->base);
+	devm_kfree(&pdev->dev, wdt);
+
+	return 0;
 }
 
+static struct platform_driver sh_wdt_driver = {
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+
+	.probe	= sh_wdt_probe,
+	.remove	= __devexit_p(sh_wdt_remove),
+};
+
+static int __init sh_wdt_init(void)
+{
+	int rc;
+
+	if (unlikely(clock_division_ratio < 0x5 ||
+		     clock_division_ratio > 0x7)) {
+		clock_division_ratio = WTCSR_CKS_4096;
+
+		pr_info("%s: divisor must be 0x5<=x<=0x7, using %d\n",
+			 DRV_NAME, clock_division_ratio);
+	}
+
+	rc = sh_wdt_set_heartbeat(heartbeat);
+	if (unlikely(rc)) {
+		heartbeat = WATCHDOG_HEARTBEAT;
+
+		pr_info("%s: heartbeat value must be 1<=x<=3600, using %d\n",
+			DRV_NAME, heartbeat);
+	}
+
+	pr_info("%s: configured with heartbeat=%d sec (nowayout=%d)\n",
+		DRV_NAME, heartbeat, nowayout);
+
+	return platform_driver_register(&sh_wdt_driver);
+}
+
+static void __exit sh_wdt_exit(void)
+{
+	platform_driver_unregister(&sh_wdt_driver);
+}
+module_init(sh_wdt_init);
+module_exit(sh_wdt_exit);
+
 MODULE_AUTHOR("Paul Mundt <lethal@linux-sh.org>");
 MODULE_DESCRIPTION("SuperH watchdog driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
 MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
 
 module_param(clock_division_ratio, int, 0);
@@ -507,6 +485,3 @@ module_param(nowayout, int, 0);
 MODULE_PARM_DESC(nowayout,
 	"Watchdog cannot be stopped once started (default="
 				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
-
-module_init(sh_wdt_init);
-module_exit(sh_wdt_exit);
diff --git a/drivers/watchdog/smsc37b787_wdt.c b/drivers/watchdog/smsc37b787_wdt.c
index 8a1f0bc3e271..df88cfa05f35 100644
--- a/drivers/watchdog/smsc37b787_wdt.c
+++ b/drivers/watchdog/smsc37b787_wdt.c
@@ -434,11 +434,11 @@ static long wb_smsc_wdt_ioctl(struct file *file,
 	} uarg;
 
 	static const struct watchdog_info ident = {
-		.options = 		WDIOF_KEEPALIVEPING |
+		.options =		WDIOF_KEEPALIVEPING |
 					WDIOF_SETTIMEOUT |
 					WDIOF_MAGICCLOSE,
 		.firmware_version =	0,
-		.identity = 		"SMsC 37B787 Watchdog",
+		.identity =		"SMsC 37B787 Watchdog",
 	};
 
 	uarg.i = (int __user *)arg;
diff --git a/drivers/watchdog/softdog.c b/drivers/watchdog/softdog.c
index 833f49f43d43..100b114e3c3c 100644
--- a/drivers/watchdog/softdog.c
+++ b/drivers/watchdog/softdog.c
@@ -151,7 +151,7 @@ static int softdog_release(struct inode *inode, struct file *file)
 {
 	/*
 	 *	Shut off the timer.
-	 * 	Lock it in if it's a module and we set nowayout
+	 *	Lock it in if it's a module and we set nowayout
 	 */
 	if (expect_close == 42) {
 		softdog_stop();
diff --git a/drivers/watchdog/sp5100_tco.c b/drivers/watchdog/sp5100_tco.c
index 808372883e88..1bc493848ed4 100644
--- a/drivers/watchdog/sp5100_tco.c
+++ b/drivers/watchdog/sp5100_tco.c
@@ -259,7 +259,7 @@ static struct miscdevice sp5100_tco_miscdev = {
  * register a pci_driver, because someone else might
  * want to register another driver on the same PCI id.
  */
-static struct pci_device_id sp5100_tco_pci_tbl[] = {
+static DEFINE_PCI_DEVICE_TABLE(sp5100_tco_pci_tbl) = {
 	{ PCI_VENDOR_ID_ATI, PCI_DEVICE_ID_ATI_SBX00_SMBUS, PCI_ANY_ID,
 	  PCI_ANY_ID, },
 	{ 0, },			/* End of list */
diff --git a/drivers/watchdog/sp805_wdt.c b/drivers/watchdog/sp805_wdt.c
index 9127eda2145b..0a0efe713bc8 100644
--- a/drivers/watchdog/sp805_wdt.c
+++ b/drivers/watchdog/sp805_wdt.c
@@ -278,7 +278,7 @@ static struct miscdevice sp805_wdt_miscdev = {
 };
 
 static int __devinit
-sp805_wdt_probe(struct amba_device *adev, struct amba_id *id)
+sp805_wdt_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	int ret = 0;
 
diff --git a/drivers/watchdog/ts72xx_wdt.c b/drivers/watchdog/ts72xx_wdt.c
index 18cdeb4c4258..5a90a4a871dd 100644
--- a/drivers/watchdog/ts72xx_wdt.c
+++ b/drivers/watchdog/ts72xx_wdt.c
@@ -68,7 +68,7 @@ struct platform_device *ts72xx_wdt_pdev;
  * to control register):
  *	value	description
  *	-------------------------
- * 	0x00	watchdog disabled
+ *	0x00	watchdog disabled
  *	0x01	250ms
  *	0x02	500ms
  *	0x03	1s
diff --git a/drivers/watchdog/w83697ug_wdt.c b/drivers/watchdog/w83697ug_wdt.c
index a6c12dec91a1..be9c4d839e15 100644
--- a/drivers/watchdog/w83697ug_wdt.c
+++ b/drivers/watchdog/w83697ug_wdt.c
@@ -87,10 +87,10 @@ static int w83697ug_select_wd_register(void)
 	outb_p(0x87, WDT_EFER); /* Enter extended function mode */
 	outb_p(0x87, WDT_EFER); /* Again according to manual */
 
-	outb(0x20, WDT_EFER); 	/* check chip version	*/
+	outb(0x20, WDT_EFER);	/* check chip version	*/
 	version = inb(WDT_EFDR);
 
-	if (version == 0x68) {	/* W83697UG 		*/
+	if (version == 0x68) {	/* W83697UG		*/
 		printk(KERN_INFO PFX "Watchdog chip version 0x%02x = "
 			"W83697UG/UF found at 0x%04x\n", version, wdt_io);
 
@@ -109,7 +109,7 @@ static int w83697ug_select_wd_register(void)
 	outb_p(0x08, WDT_EFDR); /* select logical device 8 (GPIO2) */
 	outb_p(0x30, WDT_EFER); /* select CR30 */
 	c = inb_p(WDT_EFDR);
-	outb_p(c || 0x01, WDT_EFDR); /* set bit 0 to activate GPIO2 */
+	outb_p(c | 0x01, WDT_EFDR); /* set bit 0 to activate GPIO2 */
 
 	return 0;
 }
diff --git a/drivers/watchdog/wdt.c b/drivers/watchdog/wdt.c
index 552a4381e78f..bb03e151a1d0 100644
--- a/drivers/watchdog/wdt.c
+++ b/drivers/watchdog/wdt.c
@@ -581,7 +581,7 @@ static void __exit wdt_exit(void)
 }
 
 /**
- * 	wdt_init:
+ *	wdt_init:
  *
  *	Set up the WDT watchdog board. All we have to do is grab the
  *	resources we require and bitch if anyone beat us to them.
diff --git a/drivers/watchdog/wdt977.c b/drivers/watchdog/wdt977.c
index 5c2521fc836c..a2f01c9f5c34 100644
--- a/drivers/watchdog/wdt977.c
+++ b/drivers/watchdog/wdt977.c
@@ -281,7 +281,7 @@ static int wdt977_release(struct inode *inode, struct file *file)
 {
 	/*
 	 *	Shut off the timer.
-	 * 	Lock it in if it's a module and we set nowayout
+	 *	Lock it in if it's a module and we set nowayout
 	 */
 	if (expect_close == 42) {
 		wdt977_stop();
diff --git a/drivers/watchdog/wdt_pci.c b/drivers/watchdog/wdt_pci.c
index 6130c88fa5ac..172dad6c7693 100644
--- a/drivers/watchdog/wdt_pci.c
+++ b/drivers/watchdog/wdt_pci.c
@@ -31,7 +31,7 @@
  *		Jeff Garzik	:	PCI cleanups
  *		Tigran Aivazian	:	Restructured wdtpci_init_one() to handle
  *					failures
- *		Joel Becker 	:	Added WDIOC_GET/SETTIMEOUT
+ *		Joel Becker	:	Added WDIOC_GET/SETTIMEOUT
  *		Zwane Mwaikambo	:	Magic char closing, locking changes,
  *					cleanups
  *		Matt Domsch	:	nowayout module option
@@ -727,7 +727,7 @@ static void __devexit wdtpci_remove_one(struct pci_dev *pdev)
 }
 
 
-static struct pci_device_id wdtpci_pci_tbl[] = {
+static DEFINE_PCI_DEVICE_TABLE(wdtpci_pci_tbl) = {
 	{
 		.vendor	   = PCI_VENDOR_ID_ACCESSIO,
 		.device	   = PCI_DEVICE_ID_ACCESSIO_WDG_CSM,
@@ -764,7 +764,7 @@ static void __exit wdtpci_cleanup(void)
 
 
 /**
- * 	wdtpci_init:
+ *	wdtpci_init:
  *
  *	Set up the WDT watchdog board. All we have to do is grab the
  *	resources we require and bitch if anyone beat us to them.
diff --git a/drivers/watchdog/xen_wdt.c b/drivers/watchdog/xen_wdt.c
new file mode 100644
index 000000000000..49bd9d395562
--- /dev/null
+++ b/drivers/watchdog/xen_wdt.c
@@ -0,0 +1,359 @@
+/*
+ *	Xen Watchdog Driver
+ *
+ *	(c) Copyright 2010 Novell, 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.
+ */
+
+#define DRV_NAME	"wdt"
+#define DRV_VERSION	"0.01"
+#define PFX		DRV_NAME ": "
+
+#include <linux/bug.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/hrtimer.h>
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/init.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/watchdog.h>
+#include <xen/xen.h>
+#include <asm/xen/hypercall.h>
+#include <xen/interface/sched.h>
+
+static struct platform_device *platform_device;
+static DEFINE_SPINLOCK(wdt_lock);
+static struct sched_watchdog wdt;
+static __kernel_time_t wdt_expires;
+static bool is_active, expect_release;
+
+#define WATCHDOG_TIMEOUT 60 /* in seconds */
+static unsigned int timeout = WATCHDOG_TIMEOUT;
+module_param(timeout, uint, S_IRUGO);
+MODULE_PARM_DESC(timeout, "Watchdog timeout in seconds "
+	"(default=" __MODULE_STRING(WATCHDOG_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 inline __kernel_time_t set_timeout(void)
+{
+	wdt.timeout = timeout;
+	return ktime_to_timespec(ktime_get()).tv_sec + timeout;
+}
+
+static int xen_wdt_start(void)
+{
+	__kernel_time_t expires;
+	int err;
+
+	spin_lock(&wdt_lock);
+
+	expires = set_timeout();
+	if (!wdt.id)
+		err = HYPERVISOR_sched_op(SCHEDOP_watchdog, &wdt);
+	else
+		err = -EBUSY;
+	if (err > 0) {
+		wdt.id = err;
+		wdt_expires = expires;
+		err = 0;
+	} else
+		BUG_ON(!err);
+
+	spin_unlock(&wdt_lock);
+
+	return err;
+}
+
+static int xen_wdt_stop(void)
+{
+	int err = 0;
+
+	spin_lock(&wdt_lock);
+
+	wdt.timeout = 0;
+	if (wdt.id)
+		err = HYPERVISOR_sched_op(SCHEDOP_watchdog, &wdt);
+	if (!err)
+		wdt.id = 0;
+
+	spin_unlock(&wdt_lock);
+
+	return err;
+}
+
+static int xen_wdt_kick(void)
+{
+	__kernel_time_t expires;
+	int err;
+
+	spin_lock(&wdt_lock);
+
+	expires = set_timeout();
+	if (wdt.id)
+		err = HYPERVISOR_sched_op(SCHEDOP_watchdog, &wdt);
+	else
+		err = -ENXIO;
+	if (!err)
+		wdt_expires = expires;
+
+	spin_unlock(&wdt_lock);
+
+	return err;
+}
+
+static int xen_wdt_open(struct inode *inode, struct file *file)
+{
+	int err;
+
+	/* /dev/watchdog can only be opened once */
+	if (xchg(&is_active, true))
+		return -EBUSY;
+
+	err = xen_wdt_start();
+	if (err == -EBUSY)
+		err = xen_wdt_kick();
+	return err ?: nonseekable_open(inode, file);
+}
+
+static int xen_wdt_release(struct inode *inode, struct file *file)
+{
+	if (expect_release)
+		xen_wdt_stop();
+	else {
+		printk(KERN_CRIT PFX
+		       "unexpected close, not stopping watchdog!\n");
+		xen_wdt_kick();
+	}
+	is_active = false;
+	expect_release = false;
+	return 0;
+}
+
+static ssize_t xen_wdt_write(struct file *file, const char __user *data,
+			     size_t len, loff_t *ppos)
+{
+	/* See if we got the magic character 'V' and reload the timer */
+	if (len) {
+		if (!nowayout) {
+			size_t i;
+
+			/* in case it was set long ago */
+			expect_release = false;
+
+			/* scan to see whether or not we got the magic
+			   character */
+			for (i = 0; i != len; i++) {
+				char c;
+				if (get_user(c, data + i))
+					return -EFAULT;
+				if (c == 'V')
+					expect_release = true;
+			}
+		}
+
+		/* someone wrote to us, we should reload the timer */
+		xen_wdt_kick();
+	}
+	return len;
+}
+
+static long xen_wdt_ioctl(struct file *file, unsigned int cmd,
+			  unsigned long arg)
+{
+	int new_options, retval = -EINVAL;
+	int new_timeout;
+	int __user *argp = (void __user *)arg;
+	static const struct watchdog_info ident = {
+		.options =		WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE,
+		.firmware_version =	0,
+		.identity =		DRV_NAME,
+	};
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		return copy_to_user(argp, &ident, sizeof(ident)) ? -EFAULT : 0;
+
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		return put_user(0, argp);
+
+	case WDIOC_SETOPTIONS:
+		if (get_user(new_options, argp))
+			return -EFAULT;
+
+		if (new_options & WDIOS_DISABLECARD)
+			retval = xen_wdt_stop();
+		if (new_options & WDIOS_ENABLECARD) {
+			retval = xen_wdt_start();
+			if (retval == -EBUSY)
+				retval = xen_wdt_kick();
+		}
+		return retval;
+
+	case WDIOC_KEEPALIVE:
+		xen_wdt_kick();
+		return 0;
+
+	case WDIOC_SETTIMEOUT:
+		if (get_user(new_timeout, argp))
+			return -EFAULT;
+		if (!new_timeout)
+			return -EINVAL;
+		timeout = new_timeout;
+		xen_wdt_kick();
+		/* fall through */
+	case WDIOC_GETTIMEOUT:
+		return put_user(timeout, argp);
+
+	case WDIOC_GETTIMELEFT:
+		retval = wdt_expires - ktime_to_timespec(ktime_get()).tv_sec;
+		return put_user(retval, argp);
+	}
+
+	return -ENOTTY;
+}
+
+static const struct file_operations xen_wdt_fops = {
+	.owner =		THIS_MODULE,
+	.llseek =		no_llseek,
+	.write =		xen_wdt_write,
+	.unlocked_ioctl =	xen_wdt_ioctl,
+	.open =			xen_wdt_open,
+	.release =		xen_wdt_release,
+};
+
+static struct miscdevice xen_wdt_miscdev = {
+	.minor =	WATCHDOG_MINOR,
+	.name =		"watchdog",
+	.fops =		&xen_wdt_fops,
+};
+
+static int __devinit xen_wdt_probe(struct platform_device *dev)
+{
+	struct sched_watchdog wd = { .id = ~0 };
+	int ret = HYPERVISOR_sched_op(SCHEDOP_watchdog, &wd);
+
+	switch (ret) {
+	case -EINVAL:
+		if (!timeout) {
+			timeout = WATCHDOG_TIMEOUT;
+			printk(KERN_INFO PFX
+			       "timeout value invalid, using %d\n", timeout);
+		}
+
+		ret = misc_register(&xen_wdt_miscdev);
+		if (ret) {
+			printk(KERN_ERR PFX
+			       "cannot register miscdev on minor=%d (%d)\n",
+			       WATCHDOG_MINOR, ret);
+			break;
+		}
+
+		printk(KERN_INFO PFX
+		       "initialized (timeout=%ds, nowayout=%d)\n",
+		       timeout, nowayout);
+		break;
+
+	case -ENOSYS:
+		printk(KERN_INFO PFX "not supported\n");
+		ret = -ENODEV;
+		break;
+
+	default:
+		printk(KERN_INFO PFX "bogus return value %d\n", ret);
+		break;
+	}
+
+	return ret;
+}
+
+static int __devexit xen_wdt_remove(struct platform_device *dev)
+{
+	/* Stop the timer before we leave */
+	if (!nowayout)
+		xen_wdt_stop();
+
+	misc_deregister(&xen_wdt_miscdev);
+
+	return 0;
+}
+
+static void xen_wdt_shutdown(struct platform_device *dev)
+{
+	xen_wdt_stop();
+}
+
+static int xen_wdt_suspend(struct platform_device *dev, pm_message_t state)
+{
+	return xen_wdt_stop();
+}
+
+static int xen_wdt_resume(struct platform_device *dev)
+{
+	return xen_wdt_start();
+}
+
+static struct platform_driver xen_wdt_driver = {
+	.probe          = xen_wdt_probe,
+	.remove         = __devexit_p(xen_wdt_remove),
+	.shutdown       = xen_wdt_shutdown,
+	.suspend        = xen_wdt_suspend,
+	.resume         = xen_wdt_resume,
+	.driver         = {
+		.owner  = THIS_MODULE,
+		.name   = DRV_NAME,
+	},
+};
+
+static int __init xen_wdt_init_module(void)
+{
+	int err;
+
+	if (!xen_domain())
+		return -ENODEV;
+
+	printk(KERN_INFO PFX "Xen WatchDog Timer Driver v%s\n", DRV_VERSION);
+
+	err = platform_driver_register(&xen_wdt_driver);
+	if (err)
+		return err;
+
+	platform_device = platform_device_register_simple(DRV_NAME,
+								  -1, NULL, 0);
+	if (IS_ERR(platform_device)) {
+		err = PTR_ERR(platform_device);
+		platform_driver_unregister(&xen_wdt_driver);
+	}
+
+	return err;
+}
+
+static void __exit xen_wdt_cleanup_module(void)
+{
+	platform_device_unregister(platform_device);
+	platform_driver_unregister(&xen_wdt_driver);
+	printk(KERN_INFO PFX "module unloaded\n");
+}
+
+module_init(xen_wdt_init_module);
+module_exit(xen_wdt_cleanup_module);
+
+MODULE_AUTHOR("Jan Beulich <jbeulich@novell.com>");
+MODULE_DESCRIPTION("Xen WatchDog Timer Driver");
+MODULE_VERSION(DRV_VERSION);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 07bec09d1dad..a59638b37c1a 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -76,10 +76,20 @@ config XEN_XENBUS_FRONTEND
 config XEN_GNTDEV
 	tristate "userspace grant access device driver"
 	depends on XEN
+	default m
 	select MMU_NOTIFIER
 	help
 	  Allows userspace processes to use grants.
 
+config XEN_GRANT_DEV_ALLOC
+	tristate "User-space grant reference allocator driver"
+	depends on XEN
+	default m
+	help
+	  Allows userspace processes to create pages with access granted
+	  to other domains. This can be used to implement frontend drivers
+	  or as part of an inter-domain shared memory channel.
+
 config XEN_PLATFORM_PCI
 	tristate "xen platform pci device driver"
 	depends on XEN_PVHVM && PCI
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index 5088cc2e6fe2..9585a1da52c6 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_XEN_XENCOMM)	+= xencomm.o
 obj-$(CONFIG_XEN_BALLOON)	+= balloon.o
 obj-$(CONFIG_XEN_DEV_EVTCHN)	+= xen-evtchn.o
 obj-$(CONFIG_XEN_GNTDEV)	+= xen-gntdev.o
+obj-$(CONFIG_XEN_GRANT_DEV_ALLOC)	+= xen-gntalloc.o
 obj-$(CONFIG_XENFS)		+= xenfs/
 obj-$(CONFIG_XEN_SYS_HYPERVISOR)	+= sys-hypervisor.o
 obj-$(CONFIG_XEN_PLATFORM_PCI)	+= xen-platform-pci.o
@@ -18,5 +19,6 @@ obj-$(CONFIG_XEN_DOM0)		+= pci.o
 
 xen-evtchn-y			:= evtchn.o
 xen-gntdev-y				:= gntdev.o
+xen-gntalloc-y				:= gntalloc.o
 
 xen-platform-pci-y		:= platform-pci.o
diff --git a/drivers/xen/balloon.c b/drivers/xen/balloon.c
index 43f9f02c7db0..718050ace08f 100644
--- a/drivers/xen/balloon.c
+++ b/drivers/xen/balloon.c
@@ -232,7 +232,7 @@ static int increase_reservation(unsigned long nr_pages)
 		set_phys_to_machine(pfn, frame_list[i]);
 
 		/* Link back into the page tables if not highmem. */
-		if (pfn < max_low_pfn) {
+		if (!xen_hvm_domain() && pfn < max_low_pfn) {
 			int ret;
 			ret = HYPERVISOR_update_va_mapping(
 				(unsigned long)__va(pfn << PAGE_SHIFT),
@@ -280,7 +280,7 @@ static int decrease_reservation(unsigned long nr_pages)
 
 		scrub_page(page);
 
-		if (!PageHighMem(page)) {
+		if (!xen_hvm_domain() && !PageHighMem(page)) {
 			ret = HYPERVISOR_update_va_mapping(
 				(unsigned long)__va(pfn << PAGE_SHIFT),
 				__pte_ma(0), 0);
@@ -296,7 +296,7 @@ static int decrease_reservation(unsigned long nr_pages)
 	/* No more mappings: invalidate P2M and add to balloon. */
 	for (i = 0; i < nr_pages; i++) {
 		pfn = mfn_to_pfn(frame_list[i]);
-		set_phys_to_machine(pfn, INVALID_P2M_ENTRY);
+		__set_phys_to_machine(pfn, INVALID_P2M_ENTRY);
 		balloon_append(pfn_to_page(pfn));
 	}
 
@@ -392,15 +392,19 @@ static struct notifier_block xenstore_notifier;
 
 static int __init balloon_init(void)
 {
-	unsigned long pfn, extra_pfn_end;
+ 	unsigned long pfn, nr_pages, extra_pfn_end;
 	struct page *page;
 
-	if (!xen_pv_domain())
+	if (!xen_domain())
 		return -ENODEV;
 
 	pr_info("xen_balloon: Initialising balloon driver.\n");
 
-	balloon_stats.current_pages = min(xen_start_info->nr_pages, max_pfn);
+ 	if (xen_pv_domain())
+ 		nr_pages = xen_start_info->nr_pages;
+ 	else
+ 		nr_pages = max_pfn;
+ 	balloon_stats.current_pages = min(nr_pages, max_pfn);
 	balloon_stats.target_pages  = balloon_stats.current_pages;
 	balloon_stats.balloon_low   = 0;
 	balloon_stats.balloon_high  = 0;
diff --git a/drivers/xen/events.c b/drivers/xen/events.c
index 74681478100a..0dd0af30d6a0 100644
--- a/drivers/xen/events.c
+++ b/drivers/xen/events.c
@@ -114,7 +114,7 @@ struct cpu_evtchn_s {
 static __initdata struct cpu_evtchn_s init_evtchn_mask = {
 	.bits[0 ... (NR_EVENT_CHANNELS/BITS_PER_LONG)-1] = ~0ul,
 };
-static struct cpu_evtchn_s *cpu_evtchn_mask_p = &init_evtchn_mask;
+static struct cpu_evtchn_s __refdata *cpu_evtchn_mask_p = &init_evtchn_mask;
 
 static inline unsigned long *cpu_evtchn_mask(int cpu)
 {
@@ -277,7 +277,7 @@ static void bind_evtchn_to_cpu(unsigned int chn, unsigned int cpu)
 
 	BUG_ON(irq == -1);
 #ifdef CONFIG_SMP
-	cpumask_copy(irq_to_desc(irq)->affinity, cpumask_of(cpu));
+	cpumask_copy(irq_to_desc(irq)->irq_data.affinity, cpumask_of(cpu));
 #endif
 
 	clear_bit(chn, cpu_evtchn_mask(cpu_from_irq(irq)));
@@ -294,7 +294,7 @@ static void init_evtchn_cpu_bindings(void)
 
 	/* By default all event channels notify CPU#0. */
 	for_each_irq_desc(i, desc) {
-		cpumask_copy(desc->affinity, cpumask_of(0));
+		cpumask_copy(desc->irq_data.affinity, cpumask_of(0));
 	}
 #endif
 
@@ -376,81 +376,69 @@ static void unmask_evtchn(int port)
 	put_cpu();
 }
 
-static int get_nr_hw_irqs(void)
+static int xen_allocate_irq_dynamic(void)
 {
-	int ret = 1;
+	int first = 0;
+	int irq;
 
 #ifdef CONFIG_X86_IO_APIC
-	ret = get_nr_irqs_gsi();
+	/*
+	 * For an HVM guest or domain 0 which see "real" (emulated or
+	 * actual repectively) GSIs we allocate dynamic IRQs
+	 * e.g. those corresponding to event channels or MSIs
+	 * etc. from the range above those "real" GSIs to avoid
+	 * collisions.
+	 */
+	if (xen_initial_domain() || xen_hvm_domain())
+		first = get_nr_irqs_gsi();
 #endif
 
-	return ret;
-}
+retry:
+	irq = irq_alloc_desc_from(first, -1);
 
-static int find_unbound_pirq(int type)
-{
-	int rc, i;
-	struct physdev_get_free_pirq op_get_free_pirq;
-	op_get_free_pirq.type = type;
+	if (irq == -ENOMEM && first > NR_IRQS_LEGACY) {
+		printk(KERN_ERR "Out of dynamic IRQ space and eating into GSI space. You should increase nr_irqs\n");
+		first = max(NR_IRQS_LEGACY, first - NR_IRQS_LEGACY);
+		goto retry;
+	}
 
-	rc = HYPERVISOR_physdev_op(PHYSDEVOP_get_free_pirq, &op_get_free_pirq);
-	if (!rc)
-		return op_get_free_pirq.pirq;
+	if (irq < 0)
+		panic("No available IRQ to bind to: increase nr_irqs!\n");
 
-	for (i = 0; i < nr_irqs; i++) {
-		if (pirq_to_irq[i] < 0)
-			return i;
-	}
-	return -1;
+	return irq;
 }
 
-static int find_unbound_irq(void)
+static int xen_allocate_irq_gsi(unsigned gsi)
 {
-	struct irq_data *data;
-	int irq, res;
-	int bottom = get_nr_hw_irqs();
-	int top = nr_irqs-1;
-
-	if (bottom == nr_irqs)
-		goto no_irqs;
+	int irq;
 
-	/* This loop starts from the top of IRQ space and goes down.
-	 * We need this b/c if we have a PCI device in a Xen PV guest
-	 * we do not have an IO-APIC (though the backend might have them)
-	 * mapped in. To not have a collision of physical IRQs with the Xen
-	 * event channels start at the top of the IRQ space for virtual IRQs.
+	/*
+	 * A PV guest has no concept of a GSI (since it has no ACPI
+	 * nor access to/knowledge of the physical APICs). Therefore
+	 * all IRQs are dynamically allocated from the entire IRQ
+	 * space.
 	 */
-	for (irq = top; irq > bottom; irq--) {
-		data = irq_get_irq_data(irq);
-		/* only 15->0 have init'd desc; handle irq > 16 */
-		if (!data)
-			break;
-		if (data->chip == &no_irq_chip)
-			break;
-		if (data->chip != &xen_dynamic_chip)
-			continue;
-		if (irq_info[irq].type == IRQT_UNBOUND)
-			return irq;
-	}
+	if (xen_pv_domain() && !xen_initial_domain())
+		return xen_allocate_irq_dynamic();
 
-	if (irq == bottom)
-		goto no_irqs;
+	/* Legacy IRQ descriptors are already allocated by the arch. */
+	if (gsi < NR_IRQS_LEGACY)
+		return gsi;
 
-	res = irq_alloc_desc_at(irq, -1);
-
-	if (WARN_ON(res != irq))
-		return -1;
+	irq = irq_alloc_desc_at(gsi, -1);
+	if (irq < 0)
+		panic("Unable to allocate to IRQ%d (%d)\n", gsi, irq);
 
 	return irq;
-
-no_irqs:
-	panic("No available IRQ to bind to: increase nr_irqs!\n");
 }
 
-static bool identity_mapped_irq(unsigned irq)
+static void xen_free_irq(unsigned irq)
 {
-	/* identity map all the hardware irqs */
-	return irq < get_nr_hw_irqs();
+	/* Legacy IRQ descriptors are managed by the arch. */
+	if (irq < NR_IRQS_LEGACY)
+		return;
+
+	irq_free_desc(irq);
 }
 
 static void pirq_unmask_notify(int irq)
@@ -486,7 +474,7 @@ static bool probing_irq(int irq)
 	return desc && desc->action == NULL;
 }
 
-static unsigned int startup_pirq(unsigned int irq)
+static unsigned int __startup_pirq(unsigned int irq)
 {
 	struct evtchn_bind_pirq bind_pirq;
 	struct irq_info *info = info_for_irq(irq);
@@ -524,9 +512,15 @@ out:
 	return 0;
 }
 
-static void shutdown_pirq(unsigned int irq)
+static unsigned int startup_pirq(struct irq_data *data)
+{
+	return __startup_pirq(data->irq);
+}
+
+static void shutdown_pirq(struct irq_data *data)
 {
 	struct evtchn_close close;
+	unsigned int irq = data->irq;
 	struct irq_info *info = info_for_irq(irq);
 	int evtchn = evtchn_from_irq(irq);
 
@@ -546,20 +540,20 @@ static void shutdown_pirq(unsigned int irq)
 	info->evtchn = 0;
 }
 
-static void enable_pirq(unsigned int irq)
+static void enable_pirq(struct irq_data *data)
 {
-	startup_pirq(irq);
+	startup_pirq(data);
 }
 
-static void disable_pirq(unsigned int irq)
+static void disable_pirq(struct irq_data *data)
 {
 }
 
-static void ack_pirq(unsigned int irq)
+static void ack_pirq(struct irq_data *data)
 {
-	int evtchn = evtchn_from_irq(irq);
+	int evtchn = evtchn_from_irq(data->irq);
 
-	move_native_irq(irq);
+	move_native_irq(data->irq);
 
 	if (VALID_EVTCHN(evtchn)) {
 		mask_evtchn(evtchn);
@@ -567,23 +561,6 @@ static void ack_pirq(unsigned int irq)
 	}
 }
 
-static void end_pirq(unsigned int irq)
-{
-	int evtchn = evtchn_from_irq(irq);
-	struct irq_desc *desc = irq_to_desc(irq);
-
-	if (WARN_ON(!desc))
-		return;
-
-	if ((desc->status & (IRQ_DISABLED|IRQ_PENDING)) ==
-	    (IRQ_DISABLED|IRQ_PENDING)) {
-		shutdown_pirq(irq);
-	} else if (VALID_EVTCHN(evtchn)) {
-		unmask_evtchn(evtchn);
-		pirq_unmask_notify(irq);
-	}
-}
-
 static int find_irq_by_gsi(unsigned gsi)
 {
 	int irq;
@@ -638,14 +615,7 @@ int xen_map_pirq_gsi(unsigned pirq, unsigned gsi, int shareable, char *name)
 		goto out;	/* XXX need refcount? */
 	}
 
-	/* If we are a PV guest, we don't have GSIs (no ACPI passed). Therefore
-	 * we are using the !xen_initial_domain() to drop in the function.*/
-	if (identity_mapped_irq(gsi) || (!xen_initial_domain() &&
-				xen_pv_domain())) {
-		irq = gsi;
-		irq_alloc_desc_at(irq, -1);
-	} else
-		irq = find_unbound_irq();
+	irq = xen_allocate_irq_gsi(gsi);
 
 	set_irq_chip_and_handler_name(irq, &xen_pirq_chip,
 				      handle_level_irq, name);
@@ -658,7 +628,7 @@ int xen_map_pirq_gsi(unsigned pirq, unsigned gsi, int shareable, char *name)
 	 * this in the priv domain. */
 	if (xen_initial_domain() &&
 	    HYPERVISOR_physdev_op(PHYSDEVOP_alloc_irq_vector, &irq_op)) {
-		irq_free_desc(irq);
+		xen_free_irq(irq);
 		irq = -ENOSPC;
 		goto out;
 	}
@@ -674,87 +644,46 @@ out:
 }
 
 #ifdef CONFIG_PCI_MSI
-#include <linux/msi.h>
-#include "../pci/msi.h"
-
-void xen_allocate_pirq_msi(char *name, int *irq, int *pirq, int alloc)
+int xen_allocate_pirq_msi(struct pci_dev *dev, struct msi_desc *msidesc)
 {
-	spin_lock(&irq_mapping_update_lock);
-
-	if (alloc & XEN_ALLOC_IRQ) {
-		*irq = find_unbound_irq();
-		if (*irq == -1)
-			goto out;
-	}
-
-	if (alloc & XEN_ALLOC_PIRQ) {
-		*pirq = find_unbound_pirq(MAP_PIRQ_TYPE_MSI);
-		if (*pirq == -1)
-			goto out;
-	}
+	int rc;
+	struct physdev_get_free_pirq op_get_free_pirq;
 
-	set_irq_chip_and_handler_name(*irq, &xen_pirq_chip,
-				      handle_level_irq, name);
+	op_get_free_pirq.type = MAP_PIRQ_TYPE_MSI;
+	rc = HYPERVISOR_physdev_op(PHYSDEVOP_get_free_pirq, &op_get_free_pirq);
 
-	irq_info[*irq] = mk_pirq_info(0, *pirq, 0, 0);
-	pirq_to_irq[*pirq] = *irq;
+	WARN_ONCE(rc == -ENOSYS,
+		  "hypervisor does not support the PHYSDEVOP_get_free_pirq interface\n");
 
-out:
-	spin_unlock(&irq_mapping_update_lock);
+	return rc ? -1 : op_get_free_pirq.pirq;
 }
 
-int xen_create_msi_irq(struct pci_dev *dev, struct msi_desc *msidesc, int type)
+int xen_bind_pirq_msi_to_irq(struct pci_dev *dev, struct msi_desc *msidesc,
+			     int pirq, int vector, const char *name)
 {
-	int irq = -1;
-	struct physdev_map_pirq map_irq;
-	int rc;
-	int pos;
-	u32 table_offset, bir;
-
-	memset(&map_irq, 0, sizeof(map_irq));
-	map_irq.domid = DOMID_SELF;
-	map_irq.type = MAP_PIRQ_TYPE_MSI;
-	map_irq.index = -1;
-	map_irq.pirq = -1;
-	map_irq.bus = dev->bus->number;
-	map_irq.devfn = dev->devfn;
-
-	if (type == PCI_CAP_ID_MSIX) {
-		pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-
-		pci_read_config_dword(dev, msix_table_offset_reg(pos),
-					&table_offset);
-		bir = (u8)(table_offset & PCI_MSIX_FLAGS_BIRMASK);
-
-		map_irq.table_base = pci_resource_start(dev, bir);
-		map_irq.entry_nr = msidesc->msi_attrib.entry_nr;
-	}
+	int irq, ret;
 
 	spin_lock(&irq_mapping_update_lock);
 
-	irq = find_unbound_irq();
-
+	irq = xen_allocate_irq_dynamic();
 	if (irq == -1)
 		goto out;
 
-	rc = HYPERVISOR_physdev_op(PHYSDEVOP_map_pirq, &map_irq);
-	if (rc) {
-		printk(KERN_WARNING "xen map irq failed %d\n", rc);
-
-		irq_free_desc(irq);
-
-		irq = -1;
-		goto out;
-	}
-	irq_info[irq] = mk_pirq_info(0, map_irq.pirq, 0, map_irq.index);
-
 	set_irq_chip_and_handler_name(irq, &xen_pirq_chip,
-			handle_level_irq,
-			(type == PCI_CAP_ID_MSIX) ? "msi-x":"msi");
+				      handle_level_irq, name);
 
+	irq_info[irq] = mk_pirq_info(0, pirq, 0, vector);
+	pirq_to_irq[pirq] = irq;
+	ret = set_irq_msi(irq, msidesc);
+	if (ret < 0)
+		goto error_irq;
 out:
 	spin_unlock(&irq_mapping_update_lock);
 	return irq;
+error_irq:
+	spin_unlock(&irq_mapping_update_lock);
+	xen_free_irq(irq);
+	return -1;
 }
 #endif
 
@@ -779,11 +708,12 @@ int xen_destroy_irq(int irq)
 			printk(KERN_WARNING "unmap irq failed %d\n", rc);
 			goto out;
 		}
-		pirq_to_irq[info->u.pirq.pirq] = -1;
 	}
+	pirq_to_irq[info->u.pirq.pirq] = -1;
+
 	irq_info[irq] = mk_unbound_info();
 
-	irq_free_desc(irq);
+	xen_free_irq(irq);
 
 out:
 	spin_unlock(&irq_mapping_update_lock);
@@ -814,7 +744,7 @@ int bind_evtchn_to_irq(unsigned int evtchn)
 	irq = evtchn_to_irq[evtchn];
 
 	if (irq == -1) {
-		irq = find_unbound_irq();
+		irq = xen_allocate_irq_dynamic();
 
 		set_irq_chip_and_handler_name(irq, &xen_dynamic_chip,
 					      handle_fasteoi_irq, "event");
@@ -839,7 +769,7 @@ static int bind_ipi_to_irq(unsigned int ipi, unsigned int cpu)
 	irq = per_cpu(ipi_to_irq, cpu)[ipi];
 
 	if (irq == -1) {
-		irq = find_unbound_irq();
+		irq = xen_allocate_irq_dynamic();
 		if (irq < 0)
 			goto out;
 
@@ -864,6 +794,21 @@ static int bind_ipi_to_irq(unsigned int ipi, unsigned int cpu)
 	return irq;
 }
 
+static int bind_interdomain_evtchn_to_irq(unsigned int remote_domain,
+					  unsigned int remote_port)
+{
+	struct evtchn_bind_interdomain bind_interdomain;
+	int err;
+
+	bind_interdomain.remote_dom  = remote_domain;
+	bind_interdomain.remote_port = remote_port;
+
+	err = HYPERVISOR_event_channel_op(EVTCHNOP_bind_interdomain,
+					  &bind_interdomain);
+
+	return err ? : bind_evtchn_to_irq(bind_interdomain.local_port);
+}
+
 
 int bind_virq_to_irq(unsigned int virq, unsigned int cpu)
 {
@@ -875,7 +820,7 @@ int bind_virq_to_irq(unsigned int virq, unsigned int cpu)
 	irq = per_cpu(virq_to_irq, cpu)[virq];
 
 	if (irq == -1) {
-		irq = find_unbound_irq();
+		irq = xen_allocate_irq_dynamic();
 
 		set_irq_chip_and_handler_name(irq, &xen_percpu_chip,
 					      handle_percpu_irq, "virq");
@@ -934,7 +879,7 @@ static void unbind_from_irq(unsigned int irq)
 	if (irq_info[irq].type != IRQT_UNBOUND) {
 		irq_info[irq] = mk_unbound_info();
 
-		irq_free_desc(irq);
+		xen_free_irq(irq);
 	}
 
 	spin_unlock(&irq_mapping_update_lock);
@@ -959,6 +904,29 @@ int bind_evtchn_to_irqhandler(unsigned int evtchn,
 }
 EXPORT_SYMBOL_GPL(bind_evtchn_to_irqhandler);
 
+int bind_interdomain_evtchn_to_irqhandler(unsigned int remote_domain,
+					  unsigned int remote_port,
+					  irq_handler_t handler,
+					  unsigned long irqflags,
+					  const char *devname,
+					  void *dev_id)
+{
+	int irq, retval;
+
+	irq = bind_interdomain_evtchn_to_irq(remote_domain, remote_port);
+	if (irq < 0)
+		return irq;
+
+	retval = request_irq(irq, handler, irqflags, devname, dev_id);
+	if (retval != 0) {
+		unbind_from_irq(irq);
+		return retval;
+	}
+
+	return irq;
+}
+EXPORT_SYMBOL_GPL(bind_interdomain_evtchn_to_irqhandler);
+
 int bind_virq_to_irqhandler(unsigned int virq, unsigned int cpu,
 			    irq_handler_t handler,
 			    unsigned long irqflags, const char *devname, void *dev_id)
@@ -990,7 +958,7 @@ int bind_ipi_to_irqhandler(enum ipi_vector ipi,
 	if (irq < 0)
 		return irq;
 
-	irqflags |= IRQF_NO_SUSPEND;
+	irqflags |= IRQF_NO_SUSPEND | IRQF_FORCE_RESUME;
 	retval = request_irq(irq, handler, irqflags, devname, dev_id);
 	if (retval != 0) {
 		unbind_from_irq(irq);
@@ -1234,11 +1202,12 @@ static int rebind_irq_to_cpu(unsigned irq, unsigned tcpu)
 	return 0;
 }
 
-static int set_affinity_irq(unsigned irq, const struct cpumask *dest)
+static int set_affinity_irq(struct irq_data *data, const struct cpumask *dest,
+			    bool force)
 {
 	unsigned tcpu = cpumask_first(dest);
 
-	return rebind_irq_to_cpu(irq, tcpu);
+	return rebind_irq_to_cpu(data->irq, tcpu);
 }
 
 int resend_irq_on_evtchn(unsigned int irq)
@@ -1257,35 +1226,35 @@ int resend_irq_on_evtchn(unsigned int irq)
 	return 1;
 }
 
-static void enable_dynirq(unsigned int irq)
+static void enable_dynirq(struct irq_data *data)
 {
-	int evtchn = evtchn_from_irq(irq);
+	int evtchn = evtchn_from_irq(data->irq);
 
 	if (VALID_EVTCHN(evtchn))
 		unmask_evtchn(evtchn);
 }
 
-static void disable_dynirq(unsigned int irq)
+static void disable_dynirq(struct irq_data *data)
 {
-	int evtchn = evtchn_from_irq(irq);
+	int evtchn = evtchn_from_irq(data->irq);
 
 	if (VALID_EVTCHN(evtchn))
 		mask_evtchn(evtchn);
 }
 
-static void ack_dynirq(unsigned int irq)
+static void ack_dynirq(struct irq_data *data)
 {
-	int evtchn = evtchn_from_irq(irq);
+	int evtchn = evtchn_from_irq(data->irq);
 
-	move_masked_irq(irq);
+	move_masked_irq(data->irq);
 
 	if (VALID_EVTCHN(evtchn))
 		unmask_evtchn(evtchn);
 }
 
-static int retrigger_dynirq(unsigned int irq)
+static int retrigger_dynirq(struct irq_data *data)
 {
-	int evtchn = evtchn_from_irq(irq);
+	int evtchn = evtchn_from_irq(data->irq);
 	struct shared_info *sh = HYPERVISOR_shared_info;
 	int ret = 0;
 
@@ -1334,7 +1303,7 @@ static void restore_cpu_pirqs(void)
 
 		printk(KERN_DEBUG "xen: --> irq=%d, pirq=%d\n", irq, map_irq.pirq);
 
-		startup_pirq(irq);
+		__startup_pirq(irq);
 	}
 }
 
@@ -1445,7 +1414,6 @@ void xen_poll_irq(int irq)
 void xen_irq_resume(void)
 {
 	unsigned int cpu, irq, evtchn;
-	struct irq_desc *desc;
 
 	init_evtchn_cpu_bindings();
 
@@ -1465,66 +1433,48 @@ void xen_irq_resume(void)
 		restore_cpu_ipis(cpu);
 	}
 
-	/*
-	 * Unmask any IRQF_NO_SUSPEND IRQs which are enabled. These
-	 * are not handled by the IRQ core.
-	 */
-	for_each_irq_desc(irq, desc) {
-		if (!desc->action || !(desc->action->flags & IRQF_NO_SUSPEND))
-			continue;
-		if (desc->status & IRQ_DISABLED)
-			continue;
-
-		evtchn = evtchn_from_irq(irq);
-		if (evtchn == -1)
-			continue;
-
-		unmask_evtchn(evtchn);
-	}
-
 	restore_cpu_pirqs();
 }
 
 static struct irq_chip xen_dynamic_chip __read_mostly = {
-	.name		= "xen-dyn",
+	.name			= "xen-dyn",
 
-	.disable	= disable_dynirq,
-	.mask		= disable_dynirq,
-	.unmask		= enable_dynirq,
+	.irq_disable		= disable_dynirq,
+	.irq_mask		= disable_dynirq,
+	.irq_unmask		= enable_dynirq,
 
-	.eoi		= ack_dynirq,
-	.set_affinity	= set_affinity_irq,
-	.retrigger	= retrigger_dynirq,
+	.irq_eoi		= ack_dynirq,
+	.irq_set_affinity	= set_affinity_irq,
+	.irq_retrigger		= retrigger_dynirq,
 };
 
 static struct irq_chip xen_pirq_chip __read_mostly = {
-	.name		= "xen-pirq",
+	.name			= "xen-pirq",
 
-	.startup	= startup_pirq,
-	.shutdown	= shutdown_pirq,
+	.irq_startup		= startup_pirq,
+	.irq_shutdown		= shutdown_pirq,
 
-	.enable		= enable_pirq,
-	.unmask		= enable_pirq,
+	.irq_enable		= enable_pirq,
+	.irq_unmask		= enable_pirq,
 
-	.disable	= disable_pirq,
-	.mask		= disable_pirq,
+	.irq_disable		= disable_pirq,
+	.irq_mask		= disable_pirq,
 
-	.ack		= ack_pirq,
-	.end		= end_pirq,
+	.irq_ack		= ack_pirq,
 
-	.set_affinity	= set_affinity_irq,
+	.irq_set_affinity	= set_affinity_irq,
 
-	.retrigger	= retrigger_dynirq,
+	.irq_retrigger		= retrigger_dynirq,
 };
 
 static struct irq_chip xen_percpu_chip __read_mostly = {
-	.name		= "xen-percpu",
+	.name			= "xen-percpu",
 
-	.disable	= disable_dynirq,
-	.mask		= disable_dynirq,
-	.unmask		= enable_dynirq,
+	.irq_disable		= disable_dynirq,
+	.irq_mask		= disable_dynirq,
+	.irq_unmask		= enable_dynirq,
 
-	.ack		= ack_dynirq,
+	.irq_ack		= ack_dynirq,
 };
 
 int xen_set_callback_via(uint64_t via)
diff --git a/drivers/xen/gntalloc.c b/drivers/xen/gntalloc.c
new file mode 100644
index 000000000000..a7ffdfe19fc9
--- /dev/null
+++ b/drivers/xen/gntalloc.c
@@ -0,0 +1,545 @@
+/******************************************************************************
+ * gntalloc.c
+ *
+ * Device for creating grant references (in user-space) that may be shared
+ * with other domains.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+/*
+ * This driver exists to allow userspace programs in Linux to allocate kernel
+ * memory that will later be shared with another domain.  Without this device,
+ * Linux userspace programs cannot create grant references.
+ *
+ * How this stuff works:
+ *   X -> granting a page to Y
+ *   Y -> mapping the grant from X
+ *
+ *   1. X uses the gntalloc device to allocate a page of kernel memory, P.
+ *   2. X creates an entry in the grant table that says domid(Y) can access P.
+ *      This is done without a hypercall unless the grant table needs expansion.
+ *   3. X gives the grant reference identifier, GREF, to Y.
+ *   4. Y maps the page, either directly into kernel memory for use in a backend
+ *      driver, or via a the gntdev device to map into the address space of an
+ *      application running in Y. This is the first point at which Xen does any
+ *      tracking of the page.
+ *   5. A program in X mmap()s a segment of the gntalloc device that corresponds
+ *      to the shared page, and can now communicate with Y over the shared page.
+ *
+ *
+ * NOTE TO USERSPACE LIBRARIES:
+ *   The grant allocation and mmap()ing are, naturally, two separate operations.
+ *   You set up the sharing by calling the create ioctl() and then the mmap().
+ *   Teardown requires munmap() and either close() or ioctl().
+ *
+ * WARNING: Since Xen does not allow a guest to forcibly end the use of a grant
+ * reference, this device can be used to consume kernel memory by leaving grant
+ * references mapped by another domain when an application exits. Therefore,
+ * there is a global limit on the number of pages that can be allocated. When
+ * all references to the page are unmapped, it will be freed during the next
+ * grant operation.
+ */
+
+#include <linux/atomic.h>
+#include <linux/module.h>
+#include <linux/miscdevice.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/highmem.h>
+
+#include <xen/xen.h>
+#include <xen/page.h>
+#include <xen/grant_table.h>
+#include <xen/gntalloc.h>
+#include <xen/events.h>
+
+static int limit = 1024;
+module_param(limit, int, 0644);
+MODULE_PARM_DESC(limit, "Maximum number of grants that may be allocated by "
+		"the gntalloc device");
+
+static LIST_HEAD(gref_list);
+static DEFINE_SPINLOCK(gref_lock);
+static int gref_size;
+
+struct notify_info {
+	uint16_t pgoff:12;    /* Bits 0-11: Offset of the byte to clear */
+	uint16_t flags:2;     /* Bits 12-13: Unmap notification flags */
+	int event;            /* Port (event channel) to notify */
+};
+
+/* Metadata on a grant reference. */
+struct gntalloc_gref {
+	struct list_head next_gref;  /* list entry gref_list */
+	struct list_head next_file;  /* list entry file->list, if open */
+	struct page *page;	     /* The shared page */
+	uint64_t file_index;         /* File offset for mmap() */
+	unsigned int users;          /* Use count - when zero, waiting on Xen */
+	grant_ref_t gref_id;         /* The grant reference number */
+	struct notify_info notify;   /* Unmap notification */
+};
+
+struct gntalloc_file_private_data {
+	struct list_head list;
+	uint64_t index;
+};
+
+static void __del_gref(struct gntalloc_gref *gref);
+
+static void do_cleanup(void)
+{
+	struct gntalloc_gref *gref, *n;
+	list_for_each_entry_safe(gref, n, &gref_list, next_gref) {
+		if (!gref->users)
+			__del_gref(gref);
+	}
+}
+
+static int add_grefs(struct ioctl_gntalloc_alloc_gref *op,
+	uint32_t *gref_ids, struct gntalloc_file_private_data *priv)
+{
+	int i, rc, readonly;
+	LIST_HEAD(queue_gref);
+	LIST_HEAD(queue_file);
+	struct gntalloc_gref *gref;
+
+	readonly = !(op->flags & GNTALLOC_FLAG_WRITABLE);
+	rc = -ENOMEM;
+	for (i = 0; i < op->count; i++) {
+		gref = kzalloc(sizeof(*gref), GFP_KERNEL);
+		if (!gref)
+			goto undo;
+		list_add_tail(&gref->next_gref, &queue_gref);
+		list_add_tail(&gref->next_file, &queue_file);
+		gref->users = 1;
+		gref->file_index = op->index + i * PAGE_SIZE;
+		gref->page = alloc_page(GFP_KERNEL|__GFP_ZERO);
+		if (!gref->page)
+			goto undo;
+
+		/* Grant foreign access to the page. */
+		gref->gref_id = gnttab_grant_foreign_access(op->domid,
+			pfn_to_mfn(page_to_pfn(gref->page)), readonly);
+		if (gref->gref_id < 0) {
+			rc = gref->gref_id;
+			goto undo;
+		}
+		gref_ids[i] = gref->gref_id;
+	}
+
+	/* Add to gref lists. */
+	spin_lock(&gref_lock);
+	list_splice_tail(&queue_gref, &gref_list);
+	list_splice_tail(&queue_file, &priv->list);
+	spin_unlock(&gref_lock);
+
+	return 0;
+
+undo:
+	spin_lock(&gref_lock);
+	gref_size -= (op->count - i);
+
+	list_for_each_entry(gref, &queue_file, next_file) {
+		/* __del_gref does not remove from queue_file */
+		__del_gref(gref);
+	}
+
+	/* It's possible for the target domain to map the just-allocated grant
+	 * references by blindly guessing their IDs; if this is done, then
+	 * __del_gref will leave them in the queue_gref list. They need to be
+	 * added to the global list so that we can free them when they are no
+	 * longer referenced.
+	 */
+	if (unlikely(!list_empty(&queue_gref)))
+		list_splice_tail(&queue_gref, &gref_list);
+	spin_unlock(&gref_lock);
+	return rc;
+}
+
+static void __del_gref(struct gntalloc_gref *gref)
+{
+	if (gref->notify.flags & UNMAP_NOTIFY_CLEAR_BYTE) {
+		uint8_t *tmp = kmap(gref->page);
+		tmp[gref->notify.pgoff] = 0;
+		kunmap(gref->page);
+	}
+	if (gref->notify.flags & UNMAP_NOTIFY_SEND_EVENT)
+		notify_remote_via_evtchn(gref->notify.event);
+
+	gref->notify.flags = 0;
+
+	if (gref->gref_id > 0) {
+		if (gnttab_query_foreign_access(gref->gref_id))
+			return;
+
+		if (!gnttab_end_foreign_access_ref(gref->gref_id, 0))
+			return;
+	}
+
+	gref_size--;
+	list_del(&gref->next_gref);
+
+	if (gref->page)
+		__free_page(gref->page);
+
+	kfree(gref);
+}
+
+/* finds contiguous grant references in a file, returns the first */
+static struct gntalloc_gref *find_grefs(struct gntalloc_file_private_data *priv,
+		uint64_t index, uint32_t count)
+{
+	struct gntalloc_gref *rv = NULL, *gref;
+	list_for_each_entry(gref, &priv->list, next_file) {
+		if (gref->file_index == index && !rv)
+			rv = gref;
+		if (rv) {
+			if (gref->file_index != index)
+				return NULL;
+			index += PAGE_SIZE;
+			count--;
+			if (count == 0)
+				return rv;
+		}
+	}
+	return NULL;
+}
+
+/*
+ * -------------------------------------
+ *  File operations.
+ * -------------------------------------
+ */
+static int gntalloc_open(struct inode *inode, struct file *filp)
+{
+	struct gntalloc_file_private_data *priv;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		goto out_nomem;
+	INIT_LIST_HEAD(&priv->list);
+
+	filp->private_data = priv;
+
+	pr_debug("%s: priv %p\n", __func__, priv);
+
+	return 0;
+
+out_nomem:
+	return -ENOMEM;
+}
+
+static int gntalloc_release(struct inode *inode, struct file *filp)
+{
+	struct gntalloc_file_private_data *priv = filp->private_data;
+	struct gntalloc_gref *gref;
+
+	pr_debug("%s: priv %p\n", __func__, priv);
+
+	spin_lock(&gref_lock);
+	while (!list_empty(&priv->list)) {
+		gref = list_entry(priv->list.next,
+			struct gntalloc_gref, next_file);
+		list_del(&gref->next_file);
+		gref->users--;
+		if (gref->users == 0)
+			__del_gref(gref);
+	}
+	kfree(priv);
+	spin_unlock(&gref_lock);
+
+	return 0;
+}
+
+static long gntalloc_ioctl_alloc(struct gntalloc_file_private_data *priv,
+		struct ioctl_gntalloc_alloc_gref __user *arg)
+{
+	int rc = 0;
+	struct ioctl_gntalloc_alloc_gref op;
+	uint32_t *gref_ids;
+
+	pr_debug("%s: priv %p\n", __func__, priv);
+
+	if (copy_from_user(&op, arg, sizeof(op))) {
+		rc = -EFAULT;
+		goto out;
+	}
+
+	gref_ids = kzalloc(sizeof(gref_ids[0]) * op.count, GFP_TEMPORARY);
+	if (!gref_ids) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	spin_lock(&gref_lock);
+	/* Clean up pages that were at zero (local) users but were still mapped
+	 * by remote domains. Since those pages count towards the limit that we
+	 * are about to enforce, removing them here is a good idea.
+	 */
+	do_cleanup();
+	if (gref_size + op.count > limit) {
+		spin_unlock(&gref_lock);
+		rc = -ENOSPC;
+		goto out_free;
+	}
+	gref_size += op.count;
+	op.index = priv->index;
+	priv->index += op.count * PAGE_SIZE;
+	spin_unlock(&gref_lock);
+
+	rc = add_grefs(&op, gref_ids, priv);
+	if (rc < 0)
+		goto out_free;
+
+	/* Once we finish add_grefs, it is unsafe to touch the new reference,
+	 * since it is possible for a concurrent ioctl to remove it (by guessing
+	 * its index). If the userspace application doesn't provide valid memory
+	 * to write the IDs to, then it will need to close the file in order to
+	 * release - which it will do by segfaulting when it tries to access the
+	 * IDs to close them.
+	 */
+	if (copy_to_user(arg, &op, sizeof(op))) {
+		rc = -EFAULT;
+		goto out_free;
+	}
+	if (copy_to_user(arg->gref_ids, gref_ids,
+			sizeof(gref_ids[0]) * op.count)) {
+		rc = -EFAULT;
+		goto out_free;
+	}
+
+out_free:
+	kfree(gref_ids);
+out:
+	return rc;
+}
+
+static long gntalloc_ioctl_dealloc(struct gntalloc_file_private_data *priv,
+		void __user *arg)
+{
+	int i, rc = 0;
+	struct ioctl_gntalloc_dealloc_gref op;
+	struct gntalloc_gref *gref, *n;
+
+	pr_debug("%s: priv %p\n", __func__, priv);
+
+	if (copy_from_user(&op, arg, sizeof(op))) {
+		rc = -EFAULT;
+		goto dealloc_grant_out;
+	}
+
+	spin_lock(&gref_lock);
+	gref = find_grefs(priv, op.index, op.count);
+	if (gref) {
+		/* Remove from the file list only, and decrease reference count.
+		 * The later call to do_cleanup() will remove from gref_list and
+		 * free the memory if the pages aren't mapped anywhere.
+		 */
+		for (i = 0; i < op.count; i++) {
+			n = list_entry(gref->next_file.next,
+				struct gntalloc_gref, next_file);
+			list_del(&gref->next_file);
+			gref->users--;
+			gref = n;
+		}
+	} else {
+		rc = -EINVAL;
+	}
+
+	do_cleanup();
+
+	spin_unlock(&gref_lock);
+dealloc_grant_out:
+	return rc;
+}
+
+static long gntalloc_ioctl_unmap_notify(struct gntalloc_file_private_data *priv,
+		void __user *arg)
+{
+	struct ioctl_gntalloc_unmap_notify op;
+	struct gntalloc_gref *gref;
+	uint64_t index;
+	int pgoff;
+	int rc;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	index = op.index & ~(PAGE_SIZE - 1);
+	pgoff = op.index & (PAGE_SIZE - 1);
+
+	spin_lock(&gref_lock);
+
+	gref = find_grefs(priv, index, 1);
+	if (!gref) {
+		rc = -ENOENT;
+		goto unlock_out;
+	}
+
+	if (op.action & ~(UNMAP_NOTIFY_CLEAR_BYTE|UNMAP_NOTIFY_SEND_EVENT)) {
+		rc = -EINVAL;
+		goto unlock_out;
+	}
+
+	gref->notify.flags = op.action;
+	gref->notify.pgoff = pgoff;
+	gref->notify.event = op.event_channel_port;
+	rc = 0;
+ unlock_out:
+	spin_unlock(&gref_lock);
+	return rc;
+}
+
+static long gntalloc_ioctl(struct file *filp, unsigned int cmd,
+		unsigned long arg)
+{
+	struct gntalloc_file_private_data *priv = filp->private_data;
+
+	switch (cmd) {
+	case IOCTL_GNTALLOC_ALLOC_GREF:
+		return gntalloc_ioctl_alloc(priv, (void __user *)arg);
+
+	case IOCTL_GNTALLOC_DEALLOC_GREF:
+		return gntalloc_ioctl_dealloc(priv, (void __user *)arg);
+
+	case IOCTL_GNTALLOC_SET_UNMAP_NOTIFY:
+		return gntalloc_ioctl_unmap_notify(priv, (void __user *)arg);
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+
+	return 0;
+}
+
+static void gntalloc_vma_close(struct vm_area_struct *vma)
+{
+	struct gntalloc_gref *gref = vma->vm_private_data;
+	if (!gref)
+		return;
+
+	spin_lock(&gref_lock);
+	gref->users--;
+	if (gref->users == 0)
+		__del_gref(gref);
+	spin_unlock(&gref_lock);
+}
+
+static struct vm_operations_struct gntalloc_vmops = {
+	.close = gntalloc_vma_close,
+};
+
+static int gntalloc_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct gntalloc_file_private_data *priv = filp->private_data;
+	struct gntalloc_gref *gref;
+	int count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	int rv, i;
+
+	pr_debug("%s: priv %p, page %lu+%d\n", __func__,
+		       priv, vma->vm_pgoff, count);
+
+	if (!(vma->vm_flags & VM_SHARED)) {
+		printk(KERN_ERR "%s: Mapping must be shared.\n", __func__);
+		return -EINVAL;
+	}
+
+	spin_lock(&gref_lock);
+	gref = find_grefs(priv, vma->vm_pgoff << PAGE_SHIFT, count);
+	if (gref == NULL) {
+		rv = -ENOENT;
+		pr_debug("%s: Could not find grant reference",
+				__func__);
+		goto out_unlock;
+	}
+
+	vma->vm_private_data = gref;
+
+	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTCOPY;
+	vma->vm_flags |= VM_PFNMAP | VM_PFN_AT_MMAP;
+
+	vma->vm_ops = &gntalloc_vmops;
+
+	for (i = 0; i < count; i++) {
+		gref->users++;
+		rv = vm_insert_page(vma, vma->vm_start + i * PAGE_SIZE,
+				gref->page);
+		if (rv)
+			goto out_unlock;
+
+		gref = list_entry(gref->next_file.next,
+				struct gntalloc_gref, next_file);
+	}
+	rv = 0;
+
+out_unlock:
+	spin_unlock(&gref_lock);
+	return rv;
+}
+
+static const struct file_operations gntalloc_fops = {
+	.owner = THIS_MODULE,
+	.open = gntalloc_open,
+	.release = gntalloc_release,
+	.unlocked_ioctl = gntalloc_ioctl,
+	.mmap = gntalloc_mmap
+};
+
+/*
+ * -------------------------------------
+ * Module creation/destruction.
+ * -------------------------------------
+ */
+static struct miscdevice gntalloc_miscdev = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "xen/gntalloc",
+	.fops	= &gntalloc_fops,
+};
+
+static int __init gntalloc_init(void)
+{
+	int err;
+
+	if (!xen_domain())
+		return -ENODEV;
+
+	err = misc_register(&gntalloc_miscdev);
+	if (err != 0) {
+		printk(KERN_ERR "Could not register misc gntalloc device\n");
+		return err;
+	}
+
+	pr_debug("Created grant allocation device at %d,%d\n",
+			MISC_MAJOR, gntalloc_miscdev.minor);
+
+	return 0;
+}
+
+static void __exit gntalloc_exit(void)
+{
+	misc_deregister(&gntalloc_miscdev);
+}
+
+module_init(gntalloc_init);
+module_exit(gntalloc_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Carter Weatherly <carter.weatherly@jhuapl.edu>, "
+		"Daniel De Graaf <dgdegra@tycho.nsa.gov>");
+MODULE_DESCRIPTION("User-space grant reference allocator driver");
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index 1e31cdcdae1e..d43ff3072c99 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -32,10 +32,12 @@
 #include <linux/sched.h>
 #include <linux/spinlock.h>
 #include <linux/slab.h>
+#include <linux/highmem.h>
 
 #include <xen/xen.h>
 #include <xen/grant_table.h>
 #include <xen/gntdev.h>
+#include <xen/events.h>
 #include <asm/xen/hypervisor.h>
 #include <asm/xen/hypercall.h>
 #include <asm/xen/page.h>
@@ -45,35 +47,46 @@ MODULE_AUTHOR("Derek G. Murray <Derek.Murray@cl.cam.ac.uk>, "
 	      "Gerd Hoffmann <kraxel@redhat.com>");
 MODULE_DESCRIPTION("User-space granted page access driver");
 
-static int limit = 1024;
+static int limit = 1024*1024;
 module_param(limit, int, 0644);
-MODULE_PARM_DESC(limit, "Maximum number of grants that may be mapped at "
-		"once by a gntdev instance");
+MODULE_PARM_DESC(limit, "Maximum number of grants that may be mapped by "
+		"the gntdev device");
+
+static atomic_t pages_mapped = ATOMIC_INIT(0);
+
+static int use_ptemod;
 
 struct gntdev_priv {
 	struct list_head maps;
-	uint32_t used;
-	uint32_t limit;
 	/* lock protects maps from concurrent changes */
 	spinlock_t lock;
 	struct mm_struct *mm;
 	struct mmu_notifier mn;
 };
 
+struct unmap_notify {
+	int flags;
+	/* Address relative to the start of the grant_map */
+	int addr;
+	int event;
+};
+
 struct grant_map {
 	struct list_head next;
-	struct gntdev_priv *priv;
 	struct vm_area_struct *vma;
 	int index;
 	int count;
 	int flags;
-	int is_mapped;
+	atomic_t users;
+	struct unmap_notify notify;
 	struct ioctl_gntdev_grant_ref *grants;
 	struct gnttab_map_grant_ref   *map_ops;
 	struct gnttab_unmap_grant_ref *unmap_ops;
 	struct page **pages;
 };
 
+static int unmap_grant_pages(struct grant_map *map, int offset, int pages);
+
 /* ------------------------------------------------------------------ */
 
 static void gntdev_print_maps(struct gntdev_priv *priv,
@@ -82,9 +95,7 @@ static void gntdev_print_maps(struct gntdev_priv *priv,
 #ifdef DEBUG
 	struct grant_map *map;
 
-	pr_debug("maps list (priv %p, usage %d/%d)\n",
-	       priv, priv->used, priv->limit);
-
+	pr_debug("%s: maps list (priv %p)\n", __func__, priv);
 	list_for_each_entry(map, &priv->maps, next)
 		pr_debug("  index %2d, count %2d %s\n",
 		       map->index, map->count,
@@ -115,14 +126,13 @@ static struct grant_map *gntdev_alloc_map(struct gntdev_priv *priv, int count)
 		add->pages[i] = alloc_page(GFP_KERNEL | __GFP_HIGHMEM);
 		if (add->pages[i] == NULL)
 			goto err;
+		add->map_ops[i].handle = -1;
+		add->unmap_ops[i].handle = -1;
 	}
 
 	add->index = 0;
 	add->count = count;
-	add->priv  = priv;
-
-	if (add->count + priv->used > priv->limit)
-		goto err;
+	atomic_set(&add->users, 1);
 
 	return add;
 
@@ -154,7 +164,6 @@ static void gntdev_add_map(struct gntdev_priv *priv, struct grant_map *add)
 	list_add_tail(&add->next, &priv->maps);
 
 done:
-	priv->used += add->count;
 	gntdev_print_maps(priv, "[new]", add->index);
 }
 
@@ -166,57 +175,57 @@ static struct grant_map *gntdev_find_map_index(struct gntdev_priv *priv,
 	list_for_each_entry(map, &priv->maps, next) {
 		if (map->index != index)
 			continue;
-		if (map->count != count)
-			continue;
-		return map;
-	}
-	return NULL;
-}
-
-static struct grant_map *gntdev_find_map_vaddr(struct gntdev_priv *priv,
-					       unsigned long vaddr)
-{
-	struct grant_map *map;
-
-	list_for_each_entry(map, &priv->maps, next) {
-		if (!map->vma)
-			continue;
-		if (vaddr < map->vma->vm_start)
-			continue;
-		if (vaddr >= map->vma->vm_end)
+		if (count && map->count != count)
 			continue;
 		return map;
 	}
 	return NULL;
 }
 
-static int gntdev_del_map(struct grant_map *map)
+static void gntdev_put_map(struct grant_map *map)
 {
 	int i;
 
-	if (map->vma)
-		return -EBUSY;
-	for (i = 0; i < map->count; i++)
-		if (map->unmap_ops[i].handle)
-			return -EBUSY;
+	if (!map)
+		return;
 
-	map->priv->used -= map->count;
-	list_del(&map->next);
-	return 0;
-}
+	if (!atomic_dec_and_test(&map->users))
+		return;
 
-static void gntdev_free_map(struct grant_map *map)
-{
-	int i;
+	atomic_sub(map->count, &pages_mapped);
 
-	if (!map)
-		return;
+	if (map->notify.flags & UNMAP_NOTIFY_SEND_EVENT) {
+		notify_remote_via_evtchn(map->notify.event);
+	}
+
+	if (map->pages) {
+		if (!use_ptemod)
+			unmap_grant_pages(map, 0, map->count);
 
-	if (map->pages)
 		for (i = 0; i < map->count; i++) {
-			if (map->pages[i])
+			uint32_t check, *tmp;
+			if (!map->pages[i])
+				continue;
+			/* XXX When unmapping in an HVM domain, Xen will
+			 * sometimes end up mapping the GFN to an invalid MFN.
+			 * In this case, writes will be discarded and reads will
+			 * return all 0xFF bytes.  Leak these unusable GFNs
+			 * until Xen supports fixing their p2m mapping.
+			 *
+			 * Confirmed present in Xen 4.1-RC3 with HVM source
+			 */
+			tmp = kmap(map->pages[i]);
+			*tmp = 0xdeaddead;
+			mb();
+			check = *tmp;
+			kunmap(map->pages[i]);
+			if (check == 0xdeaddead)
 				__free_page(map->pages[i]);
+			else
+				pr_debug("Discard page %d=%ld\n", i,
+					page_to_pfn(map->pages[i]));
 		}
+	}
 	kfree(map->pages);
 	kfree(map->grants);
 	kfree(map->map_ops);
@@ -231,24 +240,39 @@ static int find_grant_ptes(pte_t *pte, pgtable_t token,
 {
 	struct grant_map *map = data;
 	unsigned int pgnr = (addr - map->vma->vm_start) >> PAGE_SHIFT;
+	int flags = map->flags | GNTMAP_application_map | GNTMAP_contains_pte;
 	u64 pte_maddr;
 
 	BUG_ON(pgnr >= map->count);
 	pte_maddr = arbitrary_virt_to_machine(pte).maddr;
 
-	gnttab_set_map_op(&map->map_ops[pgnr], pte_maddr,
-			  GNTMAP_contains_pte | map->flags,
+	gnttab_set_map_op(&map->map_ops[pgnr], pte_maddr, flags,
 			  map->grants[pgnr].ref,
 			  map->grants[pgnr].domid);
-	gnttab_set_unmap_op(&map->unmap_ops[pgnr], pte_maddr,
-			    GNTMAP_contains_pte | map->flags,
-			    0 /* handle */);
+	gnttab_set_unmap_op(&map->unmap_ops[pgnr], pte_maddr, flags,
+			    -1 /* handle */);
 	return 0;
 }
 
 static int map_grant_pages(struct grant_map *map)
 {
 	int i, err = 0;
+	phys_addr_t addr;
+
+	if (!use_ptemod) {
+		/* Note: it could already be mapped */
+		if (map->map_ops[0].handle != -1)
+			return 0;
+		for (i = 0; i < map->count; i++) {
+			addr = (phys_addr_t)
+				pfn_to_kaddr(page_to_pfn(map->pages[i]));
+			gnttab_set_map_op(&map->map_ops[i], addr, map->flags,
+				map->grants[i].ref,
+				map->grants[i].domid);
+			gnttab_set_unmap_op(&map->unmap_ops[i], addr,
+				map->flags, -1 /* handle */);
+		}
+	}
 
 	pr_debug("map %d+%d\n", map->index, map->count);
 	err = gnttab_map_refs(map->map_ops, map->pages, map->count);
@@ -258,28 +282,81 @@ static int map_grant_pages(struct grant_map *map)
 	for (i = 0; i < map->count; i++) {
 		if (map->map_ops[i].status)
 			err = -EINVAL;
-		map->unmap_ops[i].handle = map->map_ops[i].handle;
+		else {
+			BUG_ON(map->map_ops[i].handle == -1);
+			map->unmap_ops[i].handle = map->map_ops[i].handle;
+			pr_debug("map handle=%d\n", map->map_ops[i].handle);
+		}
 	}
 	return err;
 }
 
-static int unmap_grant_pages(struct grant_map *map, int offset, int pages)
+static int __unmap_grant_pages(struct grant_map *map, int offset, int pages)
 {
 	int i, err = 0;
 
-	pr_debug("map %d+%d [%d+%d]\n", map->index, map->count, offset, pages);
-	err = gnttab_unmap_refs(map->unmap_ops + offset, map->pages, pages);
+	if (map->notify.flags & UNMAP_NOTIFY_CLEAR_BYTE) {
+		int pgno = (map->notify.addr >> PAGE_SHIFT);
+		if (pgno >= offset && pgno < offset + pages && use_ptemod) {
+			void __user *tmp = (void __user *)
+				map->vma->vm_start + map->notify.addr;
+			err = copy_to_user(tmp, &err, 1);
+			if (err)
+				return err;
+			map->notify.flags &= ~UNMAP_NOTIFY_CLEAR_BYTE;
+		} else if (pgno >= offset && pgno < offset + pages) {
+			uint8_t *tmp = kmap(map->pages[pgno]);
+			tmp[map->notify.addr & (PAGE_SIZE-1)] = 0;
+			kunmap(map->pages[pgno]);
+			map->notify.flags &= ~UNMAP_NOTIFY_CLEAR_BYTE;
+		}
+	}
+
+	err = gnttab_unmap_refs(map->unmap_ops + offset, map->pages + offset, pages);
 	if (err)
 		return err;
 
 	for (i = 0; i < pages; i++) {
 		if (map->unmap_ops[offset+i].status)
 			err = -EINVAL;
-		map->unmap_ops[offset+i].handle = 0;
+		pr_debug("unmap handle=%d st=%d\n",
+			map->unmap_ops[offset+i].handle,
+			map->unmap_ops[offset+i].status);
+		map->unmap_ops[offset+i].handle = -1;
 	}
 	return err;
 }
 
+static int unmap_grant_pages(struct grant_map *map, int offset, int pages)
+{
+	int range, err = 0;
+
+	pr_debug("unmap %d+%d [%d+%d]\n", map->index, map->count, offset, pages);
+
+	/* It is possible the requested range will have a "hole" where we
+	 * already unmapped some of the grants. Only unmap valid ranges.
+	 */
+	while (pages && !err) {
+		while (pages && map->unmap_ops[offset].handle == -1) {
+			offset++;
+			pages--;
+		}
+		range = 0;
+		while (range < pages) {
+			if (map->unmap_ops[offset+range].handle == -1) {
+				range--;
+				break;
+			}
+			range++;
+		}
+		err = __unmap_grant_pages(map, offset, range);
+		offset += range;
+		pages -= range;
+	}
+
+	return err;
+}
+
 /* ------------------------------------------------------------------ */
 
 static void gntdev_vma_close(struct vm_area_struct *vma)
@@ -287,22 +364,13 @@ static void gntdev_vma_close(struct vm_area_struct *vma)
 	struct grant_map *map = vma->vm_private_data;
 
 	pr_debug("close %p\n", vma);
-	map->is_mapped = 0;
 	map->vma = NULL;
 	vma->vm_private_data = NULL;
-}
-
-static int gntdev_vma_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
-{
-	pr_debug("vaddr %p, pgoff %ld (shouldn't happen)\n",
-			vmf->virtual_address, vmf->pgoff);
-	vmf->flags = VM_FAULT_ERROR;
-	return 0;
+	gntdev_put_map(map);
 }
 
 static struct vm_operations_struct gntdev_vmops = {
 	.close = gntdev_vma_close,
-	.fault = gntdev_vma_fault,
 };
 
 /* ------------------------------------------------------------------ */
@@ -320,8 +388,6 @@ static void mn_invl_range_start(struct mmu_notifier *mn,
 	list_for_each_entry(map, &priv->maps, next) {
 		if (!map->vma)
 			continue;
-		if (!map->is_mapped)
-			continue;
 		if (map->vma->vm_start >= end)
 			continue;
 		if (map->vma->vm_end <= start)
@@ -386,16 +452,17 @@ static int gntdev_open(struct inode *inode, struct file *flip)
 
 	INIT_LIST_HEAD(&priv->maps);
 	spin_lock_init(&priv->lock);
-	priv->limit = limit;
 
-	priv->mm = get_task_mm(current);
-	if (!priv->mm) {
-		kfree(priv);
-		return -ENOMEM;
+	if (use_ptemod) {
+		priv->mm = get_task_mm(current);
+		if (!priv->mm) {
+			kfree(priv);
+			return -ENOMEM;
+		}
+		priv->mn.ops = &gntdev_mmu_ops;
+		ret = mmu_notifier_register(&priv->mn, priv->mm);
+		mmput(priv->mm);
 	}
-	priv->mn.ops = &gntdev_mmu_ops;
-	ret = mmu_notifier_register(&priv->mn, priv->mm);
-	mmput(priv->mm);
 
 	if (ret) {
 		kfree(priv);
@@ -412,21 +479,19 @@ static int gntdev_release(struct inode *inode, struct file *flip)
 {
 	struct gntdev_priv *priv = flip->private_data;
 	struct grant_map *map;
-	int err;
 
 	pr_debug("priv %p\n", priv);
 
 	spin_lock(&priv->lock);
 	while (!list_empty(&priv->maps)) {
 		map = list_entry(priv->maps.next, struct grant_map, next);
-		err = gntdev_del_map(map);
-		if (WARN_ON(err))
-			gntdev_free_map(map);
-
+		list_del(&map->next);
+		gntdev_put_map(map);
 	}
 	spin_unlock(&priv->lock);
 
-	mmu_notifier_unregister(&priv->mn, priv->mm);
+	if (use_ptemod)
+		mmu_notifier_unregister(&priv->mn, priv->mm);
 	kfree(priv);
 	return 0;
 }
@@ -443,16 +508,21 @@ static long gntdev_ioctl_map_grant_ref(struct gntdev_priv *priv,
 	pr_debug("priv %p, add %d\n", priv, op.count);
 	if (unlikely(op.count <= 0))
 		return -EINVAL;
-	if (unlikely(op.count > priv->limit))
-		return -EINVAL;
 
 	err = -ENOMEM;
 	map = gntdev_alloc_map(priv, op.count);
 	if (!map)
 		return err;
+
+	if (unlikely(atomic_add_return(op.count, &pages_mapped) > limit)) {
+		pr_debug("can't map: over limit\n");
+		gntdev_put_map(map);
+		return err;
+	}
+
 	if (copy_from_user(map->grants, &u->refs,
 			   sizeof(map->grants[0]) * op.count) != 0) {
-		gntdev_free_map(map);
+		gntdev_put_map(map);
 		return err;
 	}
 
@@ -461,13 +531,9 @@ static long gntdev_ioctl_map_grant_ref(struct gntdev_priv *priv,
 	op.index = map->index << PAGE_SHIFT;
 	spin_unlock(&priv->lock);
 
-	if (copy_to_user(u, &op, sizeof(op)) != 0) {
-		spin_lock(&priv->lock);
-		gntdev_del_map(map);
-		spin_unlock(&priv->lock);
-		gntdev_free_map(map);
-		return err;
-	}
+	if (copy_to_user(u, &op, sizeof(op)) != 0)
+		return -EFAULT;
+
 	return 0;
 }
 
@@ -484,11 +550,12 @@ static long gntdev_ioctl_unmap_grant_ref(struct gntdev_priv *priv,
 
 	spin_lock(&priv->lock);
 	map = gntdev_find_map_index(priv, op.index >> PAGE_SHIFT, op.count);
-	if (map)
-		err = gntdev_del_map(map);
+	if (map) {
+		list_del(&map->next);
+		gntdev_put_map(map);
+		err = 0;
+	}
 	spin_unlock(&priv->lock);
-	if (!err)
-		gntdev_free_map(map);
 	return err;
 }
 
@@ -496,43 +563,66 @@ static long gntdev_ioctl_get_offset_for_vaddr(struct gntdev_priv *priv,
 					      struct ioctl_gntdev_get_offset_for_vaddr __user *u)
 {
 	struct ioctl_gntdev_get_offset_for_vaddr op;
+	struct vm_area_struct *vma;
 	struct grant_map *map;
 
 	if (copy_from_user(&op, u, sizeof(op)) != 0)
 		return -EFAULT;
 	pr_debug("priv %p, offset for vaddr %lx\n", priv, (unsigned long)op.vaddr);
 
-	spin_lock(&priv->lock);
-	map = gntdev_find_map_vaddr(priv, op.vaddr);
-	if (map == NULL ||
-	    map->vma->vm_start != op.vaddr) {
-		spin_unlock(&priv->lock);
+	vma = find_vma(current->mm, op.vaddr);
+	if (!vma || vma->vm_ops != &gntdev_vmops)
 		return -EINVAL;
-	}
+
+	map = vma->vm_private_data;
+	if (!map)
+		return -EINVAL;
+
 	op.offset = map->index << PAGE_SHIFT;
 	op.count = map->count;
-	spin_unlock(&priv->lock);
 
 	if (copy_to_user(u, &op, sizeof(op)) != 0)
 		return -EFAULT;
 	return 0;
 }
 
-static long gntdev_ioctl_set_max_grants(struct gntdev_priv *priv,
-					struct ioctl_gntdev_set_max_grants __user *u)
+static long gntdev_ioctl_notify(struct gntdev_priv *priv, void __user *u)
 {
-	struct ioctl_gntdev_set_max_grants op;
+	struct ioctl_gntdev_unmap_notify op;
+	struct grant_map *map;
+	int rc;
 
-	if (copy_from_user(&op, u, sizeof(op)) != 0)
+	if (copy_from_user(&op, u, sizeof(op)))
 		return -EFAULT;
-	pr_debug("priv %p, limit %d\n", priv, op.count);
-	if (op.count > limit)
-		return -E2BIG;
+
+	if (op.action & ~(UNMAP_NOTIFY_CLEAR_BYTE|UNMAP_NOTIFY_SEND_EVENT))
+		return -EINVAL;
 
 	spin_lock(&priv->lock);
-	priv->limit = op.count;
+
+	list_for_each_entry(map, &priv->maps, next) {
+		uint64_t begin = map->index << PAGE_SHIFT;
+		uint64_t end = (map->index + map->count) << PAGE_SHIFT;
+		if (op.index >= begin && op.index < end)
+			goto found;
+	}
+	rc = -ENOENT;
+	goto unlock_out;
+
+ found:
+	if ((op.action & UNMAP_NOTIFY_CLEAR_BYTE) &&
+			(map->flags & GNTMAP_readonly)) {
+		rc = -EINVAL;
+		goto unlock_out;
+	}
+
+	map->notify.flags = op.action;
+	map->notify.addr = op.index - (map->index << PAGE_SHIFT);
+	map->notify.event = op.event_channel_port;
+	rc = 0;
+ unlock_out:
 	spin_unlock(&priv->lock);
-	return 0;
+	return rc;
 }
 
 static long gntdev_ioctl(struct file *flip,
@@ -551,8 +641,8 @@ static long gntdev_ioctl(struct file *flip,
 	case IOCTL_GNTDEV_GET_OFFSET_FOR_VADDR:
 		return gntdev_ioctl_get_offset_for_vaddr(priv, ptr);
 
-	case IOCTL_GNTDEV_SET_MAX_GRANTS:
-		return gntdev_ioctl_set_max_grants(priv, ptr);
+	case IOCTL_GNTDEV_SET_UNMAP_NOTIFY:
+		return gntdev_ioctl_notify(priv, ptr);
 
 	default:
 		pr_debug("priv %p, unknown cmd %x\n", priv, cmd);
@@ -568,7 +658,7 @@ static int gntdev_mmap(struct file *flip, struct vm_area_struct *vma)
 	int index = vma->vm_pgoff;
 	int count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
 	struct grant_map *map;
-	int err = -EINVAL;
+	int i, err = -EINVAL;
 
 	if ((vma->vm_flags & VM_WRITE) && !(vma->vm_flags & VM_SHARED))
 		return -EINVAL;
@@ -580,47 +670,70 @@ static int gntdev_mmap(struct file *flip, struct vm_area_struct *vma)
 	map = gntdev_find_map_index(priv, index, count);
 	if (!map)
 		goto unlock_out;
-	if (map->vma)
+	if (use_ptemod && map->vma)
 		goto unlock_out;
-	if (priv->mm != vma->vm_mm) {
+	if (use_ptemod && priv->mm != vma->vm_mm) {
 		printk(KERN_WARNING "Huh? Other mm?\n");
 		goto unlock_out;
 	}
 
+	atomic_inc(&map->users);
+
 	vma->vm_ops = &gntdev_vmops;
 
 	vma->vm_flags |= VM_RESERVED|VM_DONTCOPY|VM_DONTEXPAND|VM_PFNMAP;
 
 	vma->vm_private_data = map;
-	map->vma = vma;
 
-	map->flags = GNTMAP_host_map | GNTMAP_application_map;
-	if (!(vma->vm_flags & VM_WRITE))
-		map->flags |= GNTMAP_readonly;
+	if (use_ptemod)
+		map->vma = vma;
+
+	if (map->flags) {
+		if ((vma->vm_flags & VM_WRITE) &&
+				(map->flags & GNTMAP_readonly))
+			return -EINVAL;
+	} else {
+		map->flags = GNTMAP_host_map;
+		if (!(vma->vm_flags & VM_WRITE))
+			map->flags |= GNTMAP_readonly;
+	}
 
 	spin_unlock(&priv->lock);
 
-	err = apply_to_page_range(vma->vm_mm, vma->vm_start,
-				  vma->vm_end - vma->vm_start,
-				  find_grant_ptes, map);
-	if (err) {
-		printk(KERN_WARNING "find_grant_ptes() failure.\n");
-		return err;
+	if (use_ptemod) {
+		err = apply_to_page_range(vma->vm_mm, vma->vm_start,
+					  vma->vm_end - vma->vm_start,
+					  find_grant_ptes, map);
+		if (err) {
+			printk(KERN_WARNING "find_grant_ptes() failure.\n");
+			goto out_put_map;
+		}
 	}
 
 	err = map_grant_pages(map);
-	if (err) {
-		printk(KERN_WARNING "map_grant_pages() failure.\n");
-		return err;
-	}
+	if (err)
+		goto out_put_map;
 
-	map->is_mapped = 1;
+	if (!use_ptemod) {
+		for (i = 0; i < count; i++) {
+			err = vm_insert_page(vma, vma->vm_start + i*PAGE_SIZE,
+				map->pages[i]);
+			if (err)
+				goto out_put_map;
+		}
+	}
 
 	return 0;
 
 unlock_out:
 	spin_unlock(&priv->lock);
 	return err;
+
+out_put_map:
+	if (use_ptemod)
+		map->vma = NULL;
+	gntdev_put_map(map);
+	return err;
 }
 
 static const struct file_operations gntdev_fops = {
@@ -646,6 +759,8 @@ static int __init gntdev_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
+	use_ptemod = xen_pv_domain();
+
 	err = misc_register(&gntdev_miscdev);
 	if (err != 0) {
 		printk(KERN_ERR "Could not register gntdev device\n");
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index 9ef54ebc1194..9428ced04807 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -458,6 +458,9 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 	if (ret)
 		return ret;
 
+	if (xen_feature(XENFEAT_auto_translated_physmap))
+		return ret;
+
 	for (i = 0; i < count; i++) {
 		/* m2p override only supported for GNTMAP_contains_pte mappings */
 		if (!(map_ops[i].flags & GNTMAP_contains_pte))
@@ -483,6 +486,9 @@ int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
 	if (ret)
 		return ret;
 
+	if (xen_feature(XENFEAT_auto_translated_physmap))
+		return ret;
+
 	for (i = 0; i < count; i++) {
 		ret = m2p_remove_override(pages[i]);
 		if (ret)
diff --git a/drivers/xen/manage.c b/drivers/xen/manage.c
index 24177272bcb8..ebb292859b59 100644
--- a/drivers/xen/manage.c
+++ b/drivers/xen/manage.c
@@ -34,42 +34,38 @@ enum shutdown_state {
 /* Ignore multiple shutdown requests. */
 static enum shutdown_state shutting_down = SHUTDOWN_INVALID;
 
-#ifdef CONFIG_PM_SLEEP
-static int xen_hvm_suspend(void *data)
-{
-	int err;
-	struct sched_shutdown r = { .reason = SHUTDOWN_suspend };
-	int *cancelled = data;
-
-	BUG_ON(!irqs_disabled());
-
-	err = sysdev_suspend(PMSG_SUSPEND);
-	if (err) {
-		printk(KERN_ERR "xen_hvm_suspend: sysdev_suspend failed: %d\n",
-		       err);
-		return err;
-	}
-
-	*cancelled = HYPERVISOR_sched_op(SCHEDOP_shutdown, &r);
+struct suspend_info {
+	int cancelled;
+	unsigned long arg; /* extra hypercall argument */
+	void (*pre)(void);
+	void (*post)(int cancelled);
+};
 
-	xen_hvm_post_suspend(*cancelled);
+static void xen_hvm_post_suspend(int cancelled)
+{
+	xen_arch_hvm_post_suspend(cancelled);
 	gnttab_resume();
+}
 
-	if (!*cancelled) {
-		xen_irq_resume();
-		xen_console_resume();
-		xen_timer_resume();
-	}
-
-	sysdev_resume();
+static void xen_pre_suspend(void)
+{
+	xen_mm_pin_all();
+	gnttab_suspend();
+	xen_arch_pre_suspend();
+}
 
-	return 0;
+static void xen_post_suspend(int cancelled)
+{
+	xen_arch_post_suspend(cancelled);
+	gnttab_resume();
+	xen_mm_unpin_all();
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int xen_suspend(void *data)
 {
+	struct suspend_info *si = data;
 	int err;
-	int *cancelled = data;
 
 	BUG_ON(!irqs_disabled());
 
@@ -80,22 +76,20 @@ static int xen_suspend(void *data)
 		return err;
 	}
 
-	xen_mm_pin_all();
-	gnttab_suspend();
-	xen_pre_suspend();
+	if (si->pre)
+		si->pre();
 
 	/*
 	 * This hypercall returns 1 if suspend was cancelled
 	 * or the domain was merely checkpointed, and 0 if it
 	 * is resuming in a new domain.
 	 */
-	*cancelled = HYPERVISOR_suspend(virt_to_mfn(xen_start_info));
+	si->cancelled = HYPERVISOR_suspend(si->arg);
 
-	xen_post_suspend(*cancelled);
-	gnttab_resume();
-	xen_mm_unpin_all();
+	if (si->post)
+		si->post(si->cancelled);
 
-	if (!*cancelled) {
+	if (!si->cancelled) {
 		xen_irq_resume();
 		xen_console_resume();
 		xen_timer_resume();
@@ -109,7 +103,7 @@ static int xen_suspend(void *data)
 static void do_suspend(void)
 {
 	int err;
-	int cancelled = 1;
+	struct suspend_info si;
 
 	shutting_down = SHUTDOWN_SUSPEND;
 
@@ -139,20 +133,29 @@ static void do_suspend(void)
 		goto out_resume;
 	}
 
-	if (xen_hvm_domain())
-		err = stop_machine(xen_hvm_suspend, &cancelled, cpumask_of(0));
-	else
-		err = stop_machine(xen_suspend, &cancelled, cpumask_of(0));
+	si.cancelled = 1;
+
+	if (xen_hvm_domain()) {
+		si.arg = 0UL;
+		si.pre = NULL;
+		si.post = &xen_hvm_post_suspend;
+	} else {
+		si.arg = virt_to_mfn(xen_start_info);
+		si.pre = &xen_pre_suspend;
+		si.post = &xen_post_suspend;
+	}
+
+	err = stop_machine(xen_suspend, &si, cpumask_of(0));
 
 	dpm_resume_noirq(PMSG_RESUME);
 
 	if (err) {
 		printk(KERN_ERR "failed to start xen_suspend: %d\n", err);
-		cancelled = 1;
+		si.cancelled = 1;
 	}
 
 out_resume:
-	if (!cancelled) {
+	if (!si.cancelled) {
 		xen_arch_resume();
 		xs_resume();
 	} else
@@ -172,12 +175,39 @@ out:
 }
 #endif	/* CONFIG_PM_SLEEP */
 
+struct shutdown_handler {
+	const char *command;
+	void (*cb)(void);
+};
+
+static void do_poweroff(void)
+{
+	shutting_down = SHUTDOWN_POWEROFF;
+	orderly_poweroff(false);
+}
+
+static void do_reboot(void)
+{
+	shutting_down = SHUTDOWN_POWEROFF; /* ? */
+	ctrl_alt_del();
+}
+
 static void shutdown_handler(struct xenbus_watch *watch,
 			     const char **vec, unsigned int len)
 {
 	char *str;
 	struct xenbus_transaction xbt;
 	int err;
+	static struct shutdown_handler handlers[] = {
+		{ "poweroff",	do_poweroff },
+		{ "halt",	do_poweroff },
+		{ "reboot",	do_reboot   },
+#ifdef CONFIG_PM_SLEEP
+		{ "suspend",	do_suspend  },
+#endif
+		{NULL, NULL},
+	};
+	static struct shutdown_handler *handler;
 
 	if (shutting_down != SHUTDOWN_INVALID)
 		return;
@@ -194,7 +224,14 @@ static void shutdown_handler(struct xenbus_watch *watch,
 		return;
 	}
 
-	xenbus_write(xbt, "control", "shutdown", "");
+	for (handler = &handlers[0]; handler->command; handler++) {
+		if (strcmp(str, handler->command) == 0)
+			break;
+	}
+
+	/* Only acknowledge commands which we are prepared to handle. */
+	if (handler->cb)
+		xenbus_write(xbt, "control", "shutdown", "");
 
 	err = xenbus_transaction_end(xbt, 0);
 	if (err == -EAGAIN) {
@@ -202,17 +239,8 @@ static void shutdown_handler(struct xenbus_watch *watch,
 		goto again;
 	}
 
-	if (strcmp(str, "poweroff") == 0 ||
-	    strcmp(str, "halt") == 0) {
-		shutting_down = SHUTDOWN_POWEROFF;
-		orderly_poweroff(false);
-	} else if (strcmp(str, "reboot") == 0) {
-		shutting_down = SHUTDOWN_POWEROFF; /* ? */
-		ctrl_alt_del();
-#ifdef CONFIG_PM_SLEEP
-	} else if (strcmp(str, "suspend") == 0) {
-		do_suspend();
-#endif
+	if (handler->cb) {
+		handler->cb();
 	} else {
 		printk(KERN_INFO "Ignoring shutdown request: %s\n", str);
 		shutting_down = SHUTDOWN_INVALID;
@@ -291,27 +319,18 @@ static int shutdown_event(struct notifier_block *notifier,
 	return NOTIFY_DONE;
 }
 
-static int __init __setup_shutdown_event(void)
-{
-	/* Delay initialization in the PV on HVM case */
-	if (xen_hvm_domain())
-		return 0;
-
-	if (!xen_pv_domain())
-		return -ENODEV;
-
-	return xen_setup_shutdown_event();
-}
-
 int xen_setup_shutdown_event(void)
 {
 	static struct notifier_block xenstore_notifier = {
 		.notifier_call = shutdown_event
 	};
+
+	if (!xen_domain())
+		return -ENODEV;
 	register_xenstore_notifier(&xenstore_notifier);
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(xen_setup_shutdown_event);
 
-subsys_initcall(__setup_shutdown_event);
+subsys_initcall(xen_setup_shutdown_event);
diff --git a/drivers/xen/platform-pci.c b/drivers/xen/platform-pci.c
index afbe041f42c5..319dd0a94d51 100644
--- a/drivers/xen/platform-pci.c
+++ b/drivers/xen/platform-pci.c
@@ -156,9 +156,6 @@ static int __devinit platform_pci_init(struct pci_dev *pdev,
 	if (ret)
 		goto out;
 	xenbus_probe(NULL);
-	ret = xen_setup_shutdown_event();
-	if (ret)
-		goto out;
 	return 0;
 
 out: